From 7cfcdc373cebe617ccc9062ceb7378c2e472cdd6 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Sun, 10 Mar 2024 18:58:52 -0700 Subject: [PATCH 01/75] refactor: cleaned up climb commands with things for testing left in --- .../org/team1540/robot2024/Constants.java | 5 ++++ .../commands/climb/ClimbAlignment.java | 8 ++----- .../commands/climb/ClimbSequence.java | 20 +--------------- .../robot2024/commands/climb/ScoreInTrap.java | 24 ------------------- .../commands/climb/TrapAndClimbSequence.java | 2 -- 5 files changed, 8 insertions(+), 51 deletions(-) delete mode 100644 src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index cd4d685d..14cd95a4 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -79,6 +79,11 @@ public static class Auto { Drivetrain.MAX_LINEAR_SPEED, Drivetrain.MAX_LINEAR_SPEED * LINEAR_ACCEL_TIME_SECS, Constants.Drivetrain.MAX_ANGULAR_SPEED, Constants.Drivetrain.MAX_ANGULAR_SPEED * ANGULAR_ACCEL_TIME_SECS); + + public static final PathConstraints STAGE_PATH_CONSTRAINTS = new PathConstraints( + 3.0, 1, + 1, + 0.3); } public static class Indexer { diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java index c8f7b7ba..f56f5937 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java @@ -1,7 +1,6 @@ package org.team1540.robot2024.commands.climb; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.*; import org.team1540.robot2024.Constants; @@ -20,15 +19,12 @@ import java.util.function.Supplier; +import static org.team1540.robot2024.Constants.Auto.STAGE_PATH_CONSTRAINTS; + public class ClimbAlignment extends ParallelRaceGroup { private final Drivetrain drivetrain; - public static final PathConstraints STAGE_PATH_CONSTRAINTS = new PathConstraints( - 3.0, 1, - 1, - 0.3); - public ClimbAlignment(Drivetrain drivetrain, Elevator elevator, Hooks hooks, Tramp tramp, Indexer indexer, Shooter shooter){ this.drivetrain = drivetrain; addCommands( diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java index 30e046ac..f22b6acc 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java @@ -1,31 +1,19 @@ package org.team1540.robot2024.commands.climb; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; -import edu.wpi.first.math.geometry.Pose2d; + import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import org.team1540.robot2024.Constants; import org.team1540.robot2024.Constants.Elevator.ElevatorState; import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; -import org.team1540.robot2024.commands.indexer.StageTrampCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.fakesubsystems.Hooks; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.subsystems.tramp.Tramp; -import org.team1540.robot2024.util.auto.PathHelper; -import org.team1540.robot2024.util.vision.AprilTagsCrescendo; - -import java.util.function.Supplier; public class ClimbSequence extends ParallelRaceGroup { - public static final PathConstraints STAGE_PATH_CONSTRAINTS = new PathConstraints( - 1.0, 0.5, - 1, - 0.3); public ClimbSequence(Drivetrain drivetrain, Elevator elevator, Hooks hooks, Tramp tramp, Indexer indexer, Shooter shooter, CommandXboxController controller) { addCommands( @@ -38,14 +26,8 @@ public ClimbSequence(Drivetrain drivetrain, Elevator elevator, Hooks hooks, Tram Commands.waitUntil(controller.a()), new ElevatorSetpointCommand(elevator, ElevatorState.TOP), Commands.waitSeconds(5), //Confirm that nothing will break. Also might need to be tuned if chain does weird things -// new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM) Commands.startEnd(()->elevator.setVoltage(-10), elevator::holdPosition, elevator).until(elevator::getLowerLimit) ) -// new ElevatorSetpointCommand(elevator, ElevatorState.CLIMB), -// Commands.runOnce(() -> drivetrain.setBrakeMode(true)), - //TODO: Put whatever drive/alignment command we plan on using here -// new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM) -// hooks.deployHooksCommand() //TODO: Deploy hooks ); } } diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java b/src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java deleted file mode 100644 index 9e92ad93..00000000 --- a/src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.team1540.robot2024.commands.climb; - -import edu.wpi.first.wpilibj2.command.Command; -import org.team1540.robot2024.subsystems.tramp.Tramp; - -//TODO: Write this command Tramp people :D -public class ScoreInTrap extends Command { - private final Tramp tramp; - - public ScoreInTrap(Tramp tramp) { - this.tramp = tramp; - addRequirements(tramp); - } - - @Override - public void initialize() { - //TODO: Score in trap :D - } - - @Override - public void end(boolean interrupted) { - - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java index 5bc3cd96..dd949605 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import org.team1540.robot2024.Constants; import org.team1540.robot2024.Constants.Elevator.ElevatorState; import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; @@ -12,7 +11,6 @@ import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.subsystems.tramp.Tramp; -import javax.imageio.metadata.IIOMetadataNode; public class TrapAndClimbSequence extends SequentialCommandGroup { From e7d21028995e1526886726a47dfad22f38024b45 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Sun, 10 Mar 2024 19:19:23 -0700 Subject: [PATCH 02/75] refactor: clean up shooting commands --- .../team1540/robot2024/RobotContainer.java | 6 ++-- .../commands/autos/AmpLanePABCSprint.java | 7 ++--- .../commands/autos/AmpLanePADESprint.java | 4 +-- .../commands/autos/CenterLanePBDA.java | 10 ++----- .../commands/autos/CenterLanePCBA.java | 6 ++-- .../commands/autos/CenterLanePCBADSprint.java | 6 ++-- .../commands/autos/CenterLanePCBAFSprint.java | 6 ++-- .../commands/autos/CenterLanePCBFSprint.java | 8 ++--- .../autos/CenterLanePSubCSubBSubFSub.java | 4 --- .../commands/autos/SourceLanePGHSprint.java | 5 ++-- .../commands/autos/SourceLanePHG.java | 4 +-- .../commands/shooter/AutoShootPrepare.java | 27 ++++++++++++----- .../AutoShootPrepareWithTargeting.java | 16 ++++++++++ .../commands/shooter/AutoShooterPrepare.java | 29 ------------------ .../commands/shooter/HoldPivotCommand.java | 29 ------------------ .../shooter/OverStageShootPrepare.java | 30 ++++++++++++++----- .../OverStageShootPrepareWithTargeting.java | 16 ++++++++++ .../shooter/OverStageShooterPrepare.java | 29 ------------------ .../shooter/PrepareShooterCommand.java | 5 ---- .../commands/shooter/ShootAndIntake.java | 25 ---------------- .../commands/shooter/ShootSequence.java | 4 +-- 21 files changed, 101 insertions(+), 175 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWithTargeting.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/shooter/AutoShooterPrepare.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/shooter/HoldPivotCommand.java create mode 100644 src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/shooter/OverStageShooterPrepare.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/shooter/ShootAndIntake.java diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index e4c560d2..a7a67ac7 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -147,9 +147,9 @@ private void configureButtonBindings() { LedPattern lockedDrivePattern = new LedPatternWave(100); LedPattern lockedOverstageDrivePattern = new LedPatternWave(280); - Command targetDrive = new AutoShootPrepare(driver.getHID(), drivetrain, shooter).alongWith(leds.commandShowPattern(lockedDrivePattern, Leds.PatternCriticality.DRIVER_LOCK)); - Command overstageTargetDrive = new OverStageShootPrepare(driver.getHID(), drivetrain, shooter).alongWith(leds.commandShowPattern(lockedOverstageDrivePattern, Leds.PatternCriticality.DRIVER_LOCK)); - Command autoShooterCommand = new AutoShooterPrepare(drivetrain, shooter).alongWith().alongWith(leds.commandShowPattern(new LedPatternWave(200), Leds.PatternCriticality.DRIVER_LOCK)); + Command targetDrive = new AutoShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter).alongWith(leds.commandShowPattern(lockedDrivePattern, Leds.PatternCriticality.DRIVER_LOCK)); + Command overstageTargetDrive = new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter).alongWith(leds.commandShowPattern(lockedOverstageDrivePattern, Leds.PatternCriticality.DRIVER_LOCK)); + Command autoShooterCommand = new AutoShootPrepare(drivetrain, shooter).alongWith().alongWith(leds.commandShowPattern(new LedPatternWave(200), Leds.PatternCriticality.DRIVER_LOCK)); driver.rightBumper().toggleOnTrue(targetDrive); driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java index 8af00e7a..608ec383 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java @@ -1,11 +1,8 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.PrintCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; -import org.team1540.robot2024.commands.shooter.AutoShooterPrepare; -import org.team1540.robot2024.commands.shooter.ShootSequence; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; @@ -26,7 +23,7 @@ public AmpLanePABCSprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer addCommands( Commands.parallel( - new AutoShooterPrepare(drivetrain, shooter), + new AutoShootPrepare(drivetrain, shooter), Commands.sequence( createSegmentSequence(drivetrain, indexer, 0), IntakeAndFeed.withDefaults(indexer).withTimeout(2), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java index a96b33d0..6316fdaa 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java @@ -1,7 +1,7 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.commands.shooter.AutoShooterPrepare; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; @@ -23,7 +23,7 @@ public AmpLanePADESprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer addCommands( Commands.parallel( - new AutoShooterPrepare(drivetrain, shooter), + new AutoShootPrepare(drivetrain, shooter), Commands.sequence( createSegmentSequence(drivetrain, indexer, 0), createSegmentSequence(drivetrain, indexer, 1), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBDA.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBDA.java index f92e9cd5..4e9c2cbf 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBDA.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBDA.java @@ -1,11 +1,7 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import org.team1540.robot2024.commands.indexer.IntakeAndFeed; -import org.team1540.robot2024.commands.indexer.IntakeCommand; -import org.team1540.robot2024.commands.shooter.AutoShooterPrepare; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.commands.shooter.ShootSequence; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -24,9 +20,9 @@ public CenterLanePBDA(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { ); addCommands( - ShootSequence.forAuto(shooter, indexer), + ShootSequence.forAutoSubwoofer(shooter, indexer), Commands.parallel( - new AutoShooterPrepare(drivetrain, shooter), + new AutoShootPrepare(drivetrain, shooter), Commands.sequence( createSegmentSequence(drivetrain, indexer, 0), createSegmentSequence(drivetrain, indexer, 1), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java index 23320f4a..d40fe1d2 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java @@ -1,7 +1,7 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.commands.shooter.AutoShooterPrepare; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.commands.shooter.ShootSequence; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -20,9 +20,9 @@ public CenterLanePCBA(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { ); addCommands( - ShootSequence.forAuto(shooter, indexer), + ShootSequence.forAutoSubwoofer(shooter, indexer), Commands.parallel( - new AutoShooterPrepare(drivetrain, shooter), + new AutoShootPrepare(drivetrain, shooter), Commands.sequence( createSegmentSequence(drivetrain, indexer, 0), createSegmentSequence(drivetrain, indexer, 1), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBADSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBADSprint.java index e7948173..5657109f 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBADSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBADSprint.java @@ -1,7 +1,7 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.commands.shooter.AutoShooterPrepare; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.commands.shooter.ShootSequence; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -22,9 +22,9 @@ public CenterLanePCBADSprint(Drivetrain drivetrain, Shooter shooter, Indexer ind ); addCommands( - ShootSequence.forAuto(shooter, indexer), + ShootSequence.forAutoSubwoofer(shooter, indexer), Commands.parallel( - new AutoShooterPrepare(drivetrain, shooter), + new AutoShootPrepare(drivetrain, shooter), Commands.sequence( createSegmentSequence(drivetrain, indexer, 0), createSegmentSequence(drivetrain, indexer, 1), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java index f0daa775..670c6ba9 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java @@ -1,7 +1,7 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.commands.shooter.AutoShooterPrepare; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.commands.shooter.ShootSequence; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -23,9 +23,9 @@ public CenterLanePCBAFSprint(Drivetrain drivetrain, Shooter shooter, Indexer ind ); addCommands( - ShootSequence.forAuto(shooter, indexer), + ShootSequence.forAutoSubwoofer(shooter, indexer), Commands.parallel( - new AutoShooterPrepare(drivetrain, shooter), + new AutoShootPrepare(drivetrain, shooter), Commands.sequence( createSegmentSequence(drivetrain, indexer, 0), drivetrain.commandCopyVisionPose(), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java index 9bdb3a47..bb44450f 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java @@ -1,9 +1,7 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import org.team1540.robot2024.commands.indexer.IntakeCommand; -import org.team1540.robot2024.commands.shooter.AutoShooterPrepare; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.commands.shooter.ShootSequence; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -24,9 +22,9 @@ public CenterLanePCBFSprint(Drivetrain drivetrain, Shooter shooter, Indexer inde ); addCommands( - ShootSequence.forAuto(shooter, indexer), + ShootSequence.forAutoSubwoofer(shooter, indexer), Commands.parallel( - new AutoShooterPrepare(drivetrain, shooter), + new AutoShootPrepare(drivetrain, shooter), Commands.sequence( createSegmentSequence(drivetrain, indexer, 0), createSegmentSequence(drivetrain, indexer, 1), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePSubCSubBSubFSub.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePSubCSubBSubFSub.java index de3db7f0..0aba480d 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePSubCSubBSubFSub.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePSubCSubBSubFSub.java @@ -2,8 +2,6 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import org.team1540.robot2024.commands.indexer.IntakeCommand; -import org.team1540.robot2024.commands.shooter.AutoShooterPrepare; -import org.team1540.robot2024.commands.shooter.PrepareShooterCommand; import org.team1540.robot2024.commands.shooter.ShootSequence; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -11,8 +9,6 @@ import org.team1540.robot2024.util.auto.AutoCommand; import org.team1540.robot2024.util.auto.PathHelper; -import static org.team1540.robot2024.Constants.Shooter.Pivot.HUB_SHOOT; - public class CenterLanePSubCSubBSubFSub extends AutoCommand { public CenterLanePSubCSubBSubFSub(Drivetrain drivetrain, Shooter shooter, Indexer indexer ) { diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java index 17d0f82e..bd5ca822 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java @@ -1,8 +1,7 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.commands.shooter.AutoShooterPrepare; -import org.team1540.robot2024.commands.shooter.ShootSequence; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; @@ -21,7 +20,7 @@ public SourceLanePGHSprint(Drivetrain drivetrain, Shooter shooter, Indexer index addCommands( Commands.parallel( - new AutoShooterPrepare(drivetrain, shooter), + new AutoShootPrepare(drivetrain, shooter), Commands.sequence( createSegmentSequence(drivetrain, indexer, 0), drivetrain.commandCopyVisionPose(), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java index 08a06c86..f1bab0ae 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java @@ -1,7 +1,7 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.commands.shooter.AutoShooterPrepare; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; @@ -19,7 +19,7 @@ public SourceLanePHG(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ addCommands( Commands.parallel( - new AutoShooterPrepare(drivetrain, shooter), + new AutoShootPrepare(drivetrain, shooter), Commands.sequence( createSegmentSequence(drivetrain, indexer, 0), createSegmentSequence(drivetrain, indexer, 1) diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java index b0310e79..e0a901c6 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java @@ -1,21 +1,32 @@ package org.team1540.robot2024.commands.shooter; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import org.team1540.robot2024.Constants; -import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerTargetingCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.util.shooter.ShooterSetpoint; import org.team1540.robot2024.util.vision.AprilTagsCrescendo; -public class AutoShootPrepare extends ParallelCommandGroup { - public AutoShootPrepare(XboxController controller, Drivetrain drivetrain, Shooter shooter) { +import java.util.function.Supplier; + +public class AutoShootPrepare extends SequentialCommandGroup { + public AutoShootPrepare(Drivetrain drivetrain, Shooter shooter) { + this(drivetrain::getPose, shooter); + } + public AutoShootPrepare(Supplier positionSupplier, Shooter shooter) { addCommands( - new DriveWithSpeakerTargetingCommand(drivetrain,controller), - new AutoShooterPrepare(drivetrain, shooter) + new PrepareShooterCommand(shooter, () -> new ShooterSetpoint( + Rotation2d.fromRadians( + Math.atan2(Constants.Targeting.SPEAKER_CENTER_HEIGHT - Constants.Shooter.Pivot.PIVOT_HEIGHT, + positionSupplier.get().getTranslation().getDistance( + AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation() + ) + ) + ).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), + 8000, 6000) + ) ); } } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWithTargeting.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWithTargeting.java new file mode 100644 index 00000000..41eb13e2 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWithTargeting.java @@ -0,0 +1,16 @@ +package org.team1540.robot2024.commands.shooter; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerTargetingCommand; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.shooter.Shooter; + +public class AutoShootPrepareWithTargeting extends ParallelCommandGroup { + public AutoShootPrepareWithTargeting(XboxController controller, Drivetrain drivetrain, Shooter shooter) { + addCommands( + new DriveWithSpeakerTargetingCommand(drivetrain,controller), + new AutoShootPrepare(drivetrain, shooter) + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShooterPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShooterPrepare.java deleted file mode 100644 index 5f6c055d..00000000 --- a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShooterPrepare.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.team1540.robot2024.commands.shooter; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import org.team1540.robot2024.Constants; -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.subsystems.shooter.Shooter; -import org.team1540.robot2024.util.shooter.ShooterSetpoint; -import org.team1540.robot2024.util.vision.AprilTagsCrescendo; - -import java.util.function.Supplier; - -public class AutoShooterPrepare extends SequentialCommandGroup { - public AutoShooterPrepare(Drivetrain drivetrain, Shooter shooter) { - this(drivetrain::getPose, shooter); - } - public AutoShooterPrepare(Supplier positionSupplier, Shooter shooter) { - addCommands( - new PrepareShooterCommand(shooter, () -> new ShooterSetpoint( - Rotation2d.fromRadians( - Math.atan2(Constants.Targeting.SPEAKER_CENTER_HEIGHT - Constants.Shooter.Pivot.PIVOT_HEIGHT, positionSupplier.get().getTranslation().getDistance( - AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation() - ))).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), - 8000, 6000) - ) - ); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/HoldPivotCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/HoldPivotCommand.java deleted file mode 100644 index 02e1dee2..00000000 --- a/src/main/java/org/team1540/robot2024/commands/shooter/HoldPivotCommand.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.team1540.robot2024.commands.shooter; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import org.team1540.robot2024.subsystems.shooter.Shooter; - -public class HoldPivotCommand extends Command { - private final Shooter shooter; - - public HoldPivotCommand(Shooter shooter) { - this.shooter = shooter; - addRequirements(shooter); - } - - @Override - public void initialize() { - shooter.setPivotVolts(4); - } - @Override - public void execute() { - - } - - @Override - public void end(boolean interrupted) { - shooter.stopFlywheels(); - shooter.stopPivot(); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java index ffcaa743..3cc006c7 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java @@ -1,16 +1,32 @@ package org.team1540.robot2024.commands.shooter; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerTargetingCommand; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.team1540.robot2024.Constants; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.shooter.ShooterSetpoint; +import org.team1540.robot2024.util.vision.AprilTagsCrescendo; -public class OverStageShootPrepare extends ParallelCommandGroup { - public OverStageShootPrepare(XboxController controller, Drivetrain drivetrain, Shooter shooter) { +import java.util.function.Supplier; + +public class OverStageShootPrepare extends SequentialCommandGroup { + public OverStageShootPrepare(Drivetrain drivetrain, Shooter shooter) { + this(drivetrain::getPose, shooter); + } + public OverStageShootPrepare(Supplier positionSupplier, Shooter shooter) { addCommands( - new DriveWithSpeakerTargetingCommand(drivetrain,controller), - new OverStageShooterPrepare(drivetrain, shooter) + new PrepareShooterCommand(shooter, () -> new ShooterSetpoint( + Rotation2d.fromRadians( + Math.atan2(Constants.Targeting.STAGE_MAX_HEIGHT + 1.5 - Constants.Shooter.Pivot.PIVOT_HEIGHT, + positionSupplier.get().getTranslation().getDistance( + AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.CLIMB_FAR).toPose2d().getTranslation() + ) + ) + ).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), + 3000, 3000) + ) ); } } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java new file mode 100644 index 00000000..73f56b89 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java @@ -0,0 +1,16 @@ +package org.team1540.robot2024.commands.shooter; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerTargetingCommand; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.shooter.Shooter; + +public class OverStageShootPrepareWithTargeting extends ParallelCommandGroup { + public OverStageShootPrepareWithTargeting(XboxController controller, Drivetrain drivetrain, Shooter shooter) { + addCommands( + new DriveWithSpeakerTargetingCommand(drivetrain,controller), + new OverStageShootPrepare(drivetrain, shooter) + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShooterPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShooterPrepare.java deleted file mode 100644 index 2515ee28..00000000 --- a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShooterPrepare.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.team1540.robot2024.commands.shooter; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import org.team1540.robot2024.Constants; -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.subsystems.shooter.Shooter; -import org.team1540.robot2024.util.shooter.ShooterSetpoint; -import org.team1540.robot2024.util.vision.AprilTagsCrescendo; - -import java.util.function.Supplier; - -public class OverStageShooterPrepare extends SequentialCommandGroup { - public OverStageShooterPrepare(Drivetrain drivetrain, Shooter shooter) { - this(drivetrain::getPose, shooter); - } - public OverStageShooterPrepare(Supplier positionSupplier, Shooter shooter) { - addCommands( - new PrepareShooterCommand(shooter, () -> new ShooterSetpoint( - Rotation2d.fromRadians( - Math.atan2(Constants.Targeting.STAGE_MAX_HEIGHT + 1.5 - Constants.Shooter.Pivot.PIVOT_HEIGHT, positionSupplier.get().getTranslation().getDistance( - AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.CLIMB_FAR).toPose2d().getTranslation() - ))).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), - 3000, 3000) - ) - ); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java index 29e56730..1337626b 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java @@ -11,8 +11,6 @@ public class PrepareShooterCommand extends Command { private final Shooter shooter; private final Supplier setpoint; - private final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 3200); - private final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 2500); public PrepareShooterCommand(Shooter shooter, Supplier setpoint) { this.shooter = shooter; @@ -22,7 +20,6 @@ public PrepareShooterCommand(Shooter shooter, Supplier setpoint @Override public void execute() { ShooterSetpoint setpoint = this.setpoint.get(); - // TODO: Make this dynamically update based on estimated pose shooter.setFlywheelSpeeds(setpoint.leftSetpoint, setpoint.rightSetpoint); shooter.setPivotPosition(setpoint.pivot); } @@ -30,8 +27,6 @@ public void execute() { @Override public boolean isFinished() { return false; -// return shooter.areFlywheelsSpunUp() && shooter.isPivotAtSetpoint(); TODO make this terminate properly so it works -// return shooter.areFlywheelsSpunUp(); } @Override diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootAndIntake.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootAndIntake.java deleted file mode 100644 index a2862528..00000000 --- a/src/main/java/org/team1540/robot2024/commands/shooter/ShootAndIntake.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.team1540.robot2024.commands.shooter; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.*; -import org.team1540.robot2024.subsystems.indexer.Indexer; -import org.team1540.robot2024.subsystems.shooter.Shooter; - -public class ShootAndIntake extends ParallelDeadlineGroup{ - public ShootAndIntake(Indexer indexer, Shooter shooter, double intakePercent, double feederSpeed, Rotation2d pivotAngle, double leftSetpoint, double rightSetpoint){ - super( - Commands.sequence( - Commands.waitUntil(indexer::isNoteStaged), - Commands.waitUntil(() -> !indexer.isNoteStaged()), - Commands.waitSeconds(0.3) // TODO: Adjust - - ), - Commands.startEnd( - () -> {shooter.setPivotPosition(pivotAngle); shooter.setFlywheelSpeeds(leftSetpoint, rightSetpoint);}, - () -> {shooter.setFlywheelSpeeds(0,0);} - ), - indexer.setIntakeAndFeeder(intakePercent, feederSpeed) - ); - addRequirements(shooter, indexer); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java index 3557dd56..fcb2b04b 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java @@ -3,9 +3,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.PrintCommand; import org.team1540.robot2024.Constants; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -47,7 +45,7 @@ public ShootSequence(Shooter shooter, Indexer indexer, Supplier ); } - public static ShootSequence forAuto(Shooter shooter, Indexer indexer) { + public static ShootSequence forAutoSubwoofer(Shooter shooter, Indexer indexer) { return new ShootSequence(shooter, indexer, () -> HUB_SHOOT, 1.5); } From 42e3f69038954b0a8de825384b54e21ea20f7196 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Sun, 10 Mar 2024 19:27:46 -0700 Subject: [PATCH 03/75] refactor: rename amp commands --- src/main/java/org/team1540/robot2024/RobotContainer.java | 8 ++++---- .../{TrampScoreSequence.java => AmpScoreSequence.java} | 8 ++++---- ...TrampStageSequence.java => AmpScoreStageSequence.java} | 4 ++-- .../commands/tramp/{TrampShoot.java => TrampOuttake.java} | 5 ++--- 4 files changed, 12 insertions(+), 13 deletions(-) rename src/main/java/org/team1540/robot2024/commands/tramp/{TrampScoreSequence.java => AmpScoreSequence.java} (65%) rename src/main/java/org/team1540/robot2024/commands/tramp/{TrampStageSequence.java => AmpScoreStageSequence.java} (87%) rename src/main/java/org/team1540/robot2024/commands/tramp/{TrampShoot.java => TrampOuttake.java} (71%) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index a7a67ac7..cd8557ad 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -16,8 +16,8 @@ import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.commands.indexer.StageTrampCommand; import org.team1540.robot2024.commands.shooter.*; -import org.team1540.robot2024.commands.tramp.TrampScoreSequence; -import org.team1540.robot2024.commands.tramp.TrampStageSequence; +import org.team1540.robot2024.commands.tramp.AmpScoreSequence; +import org.team1540.robot2024.commands.tramp.AmpScoreStageSequence; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -159,7 +159,7 @@ private void configureButtonBindings() { overstageTargetDrive.cancel(); })); - copilot.leftBumper().whileTrue(new TrampScoreSequence(tramp, indexer, elevator)); + copilot.leftBumper().whileTrue(new AmpScoreSequence(tramp, indexer, elevator)); Command intakeCommand = new IntakeCommand(indexer, tramp::isNoteStaged, 1, driver.getHID(), copilot.getHID()); copilot.rightBumper().whileTrue(intakeCommand); @@ -183,7 +183,7 @@ private void configureButtonBindings() { copilot.x().whileTrue(new ShootSequence(shooter, indexer)); - copilot.a().whileTrue(new TrampStageSequence(indexer, tramp, elevator)); + copilot.a().whileTrue(new AmpScoreStageSequence(indexer, tramp, elevator)); // copilot.b().whileTrue(new ShootSequence(shooter, indexer, PODIUM_SHOOT)); // copilot.b().whileTrue(new TuneShooterCommand(shooter, indexer)); copilot.b() diff --git a/src/main/java/org/team1540/robot2024/commands/tramp/TrampScoreSequence.java b/src/main/java/org/team1540/robot2024/commands/tramp/AmpScoreSequence.java similarity index 65% rename from src/main/java/org/team1540/robot2024/commands/tramp/TrampScoreSequence.java rename to src/main/java/org/team1540/robot2024/commands/tramp/AmpScoreSequence.java index 34488978..5dacf5a9 100644 --- a/src/main/java/org/team1540/robot2024/commands/tramp/TrampScoreSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/tramp/AmpScoreSequence.java @@ -7,11 +7,11 @@ import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.tramp.Tramp; -public class TrampScoreSequence extends SequentialCommandGroup { - public TrampScoreSequence(Tramp tramp, Indexer indexer, Elevator elevator) { +public class AmpScoreSequence extends SequentialCommandGroup { + public AmpScoreSequence(Tramp tramp, Indexer indexer, Elevator elevator) { addCommands( - new TrampStageSequence(indexer, tramp, elevator).unless(tramp::isNoteStaged), - new TrampShoot(tramp), + new AmpScoreStageSequence(indexer, tramp, elevator).unless(tramp::isNoteStaged), + new TrampOuttake(tramp), new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.BOTTOM) ); } diff --git a/src/main/java/org/team1540/robot2024/commands/tramp/TrampStageSequence.java b/src/main/java/org/team1540/robot2024/commands/tramp/AmpScoreStageSequence.java similarity index 87% rename from src/main/java/org/team1540/robot2024/commands/tramp/TrampStageSequence.java rename to src/main/java/org/team1540/robot2024/commands/tramp/AmpScoreStageSequence.java index fbdf69c9..b156161c 100644 --- a/src/main/java/org/team1540/robot2024/commands/tramp/TrampStageSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/tramp/AmpScoreStageSequence.java @@ -10,8 +10,8 @@ import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.tramp.Tramp; -public class TrampStageSequence extends SequentialCommandGroup { - public TrampStageSequence(Indexer indexer, Tramp tramp, Elevator elevator){ +public class AmpScoreStageSequence extends SequentialCommandGroup { + public AmpScoreStageSequence(Indexer indexer, Tramp tramp, Elevator elevator){ addCommands( new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.BOTTOM).withTimeout(3), new StageTrampCommand(tramp, indexer).withTimeout(3), diff --git a/src/main/java/org/team1540/robot2024/commands/tramp/TrampShoot.java b/src/main/java/org/team1540/robot2024/commands/tramp/TrampOuttake.java similarity index 71% rename from src/main/java/org/team1540/robot2024/commands/tramp/TrampShoot.java rename to src/main/java/org/team1540/robot2024/commands/tramp/TrampOuttake.java index a2502fc7..4d3ee9d0 100644 --- a/src/main/java/org/team1540/robot2024/commands/tramp/TrampShoot.java +++ b/src/main/java/org/team1540/robot2024/commands/tramp/TrampOuttake.java @@ -1,12 +1,11 @@ package org.team1540.robot2024.commands.tramp; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import org.team1540.robot2024.subsystems.tramp.Tramp; -public class TrampShoot extends SequentialCommandGroup { - public TrampShoot(Tramp tramp) { +public class TrampOuttake extends SequentialCommandGroup { + public TrampOuttake(Tramp tramp) { addCommands( tramp.commandRun(1).onlyWhile(tramp::isNoteStaged), Commands.runOnce(()->tramp.setDistanceToGo(1)) From 5ddc0580449790281a333e77c8ba3a7f572fb268 Mon Sep 17 00:00:00 2001 From: Zach R Date: Sun, 10 Mar 2024 22:05:28 -0700 Subject: [PATCH 04/75] fix: allow leds to change in commands on disable --- .../org/team1540/robot2024/Constants.java | 2 +- .../team1540/robot2024/RobotContainer.java | 33 ++++++-------- .../robot2024/subsystems/led/LedTriager.java | 7 ++- .../robot2024/subsystems/led/Leds.java | 44 ++++++++++--------- .../team1540/robot2024/util/CommandUtils.java | 8 +++- 5 files changed, 49 insertions(+), 45 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 14cd95a4..4af35230 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -26,7 +26,7 @@ public final class Constants { public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; public static final class Leds { public static final int LED_STRIP_PORT_PWM = 9; - public static final int LED_STRIP_LENGTH= 80; + public static final int LED_STRIP_LENGTH= 40; } public enum Mode { /** diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index cd8557ad..78341633 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -1,6 +1,5 @@ package org.team1540.robot2024; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; @@ -32,8 +31,6 @@ import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher; import org.team1540.robot2024.util.auto.AutoCommand; import org.team1540.robot2024.util.auto.AutoManager; -import org.team1540.robot2024.util.shooter.ShooterSetpoint; -import org.team1540.robot2024.util.vision.AprilTagsCrescendo; import static org.team1540.robot2024.Constants.SwerveConfig; import static org.team1540.robot2024.Constants.isTuningMode; @@ -147,9 +144,13 @@ private void configureButtonBindings() { LedPattern lockedDrivePattern = new LedPatternWave(100); LedPattern lockedOverstageDrivePattern = new LedPatternWave(280); - Command targetDrive = new AutoShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter).alongWith(leds.commandShowPattern(lockedDrivePattern, Leds.PatternCriticality.DRIVER_LOCK)); - Command overstageTargetDrive = new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter).alongWith(leds.commandShowPattern(lockedOverstageDrivePattern, Leds.PatternCriticality.DRIVER_LOCK)); - Command autoShooterCommand = new AutoShootPrepare(drivetrain, shooter).alongWith().alongWith(leds.commandShowPattern(new LedPatternWave(200), Leds.PatternCriticality.DRIVER_LOCK)); + + Command targetDrive = new AutoShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter) + .alongWith(leds.commandShowPattern(lockedDrivePattern, Leds.PatternLevel.DRIVER_LOCK)); + Command overstageTargetDrive = new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter) + .alongWith(leds.commandShowPattern(lockedOverstageDrivePattern, Leds.PatternLevel.DRIVER_LOCK)); + Command autoShooterCommand = new AutoShootPrepare(drivetrain, shooter) + .alongWith(leds.commandShowPattern(new LedPatternWave(200), Leds.PatternLevel.DRIVER_LOCK)); driver.rightBumper().toggleOnTrue(targetDrive); driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); @@ -171,12 +172,6 @@ private void configureButtonBindings() { // () -> leds.setPatternAll(() -> new LedPatternWave(DriverStation.getAlliance().orElse(DriverStation.Alliance.Red) == DriverStation.Alliance.Red ? 0: 216), Leds.PatternCriticality.HIGH), // 5 // )); - copilot.povLeft().whileTrue(new ShootSequence(shooter, indexer, () -> new ShooterSetpoint( - Rotation2d.fromRadians( - Math.atan2(Constants.Targeting.SPEAKER_CENTER_HEIGHT - Constants.Shooter.Pivot.PIVOT_HEIGHT, drivetrain.getPose().getTranslation().getDistance( - AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation() - ))).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), - 8000, 6000))); copilot.rightTrigger(0.95).whileTrue(Commands.startEnd(() -> tramp.setPercent(1), tramp::stop, tramp)); copilot.leftTrigger(0.95).whileTrue(new ClimbAlignment(drivetrain, elevator, null, tramp, indexer, shooter)); @@ -198,25 +193,25 @@ private void configureButtonBindings() { new Trigger(indexer::isNoteStaged).debounce(0.1) - .onTrue(CommandUtils.rumbleCommand(driver.getHID(), 1, 1)) - .whileTrue(Commands.startEnd(() -> leds.setPattern(Leds.Zone.ELEVATOR_BACK, new LedPatternWave(0), Leds.PatternCriticality.HAS_INTAKE), () -> leds.clearPattern(Leds.Zone.ELEVATOR_BACK, Leds.PatternCriticality.HAS_INTAKE))); + .onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 1, 1)) + .whileTrue(leds.commandShowPattern(new LedPatternWave(0), Leds.PatternLevel.INTAKE_STATE)); - new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommand(driver.getHID(), 0.3, 0.4)); + new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.3, 0.4)); new Trigger(RobotController::getUserButton).toggleOnTrue(Commands.startEnd( () -> { elevator.setBrakeMode(false); - leds.setPatternAll(() -> new LedPatternRSLState(Color.kMagenta), Leds.PatternCriticality.EXTREME); + leds.setPatternAll(() -> new LedPatternRSLState(Color.kMagenta), Leds.PatternLevel.ELEVATOR_STATE); Commands.sequence( Commands.waitSeconds(5), - Commands.runOnce(() -> leds.clearPatternAll(Leds.PatternCriticality.EXTREME)) + leds.commandClear(Leds.PatternLevel.ELEVATOR_STATE) ).schedule(); }, () -> { elevator.setBrakeMode(true); - leds.clearPatternAll(Leds.PatternCriticality.EXTREME); + leds.clearPatternAll(Leds.PatternLevel.ELEVATOR_STATE); } - )); + ).ignoringDisable(true)); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java index 4cacde1c..380f84a5 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java @@ -1,12 +1,11 @@ package org.team1540.robot2024.subsystems.led; import org.team1540.robot2024.subsystems.led.patterns.LedPattern; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame; import org.team1540.robot2024.subsystems.led.patterns.LedPatternRainbow; public class LedTriager { - private final LedPattern[] patterns = new LedPattern[Leds.CRITICALITY_COUNT]; + private final LedPattern[] patterns = new LedPattern[Leds.LEVEL_COUNT]; private final LedPattern defaultPattern = new LedPatternRainbow(1); private boolean isNew = true; public LedPattern getPattern() { @@ -24,12 +23,12 @@ public boolean shouldRefresh() { return val; } - public void clearPattern(Leds.PatternCriticality criticality) { + public void clearPattern(Leds.PatternLevel criticality) { patterns[criticality.ordinal()] = null; isNew = true; } - public boolean addPattern(LedPattern pattern, Leds.PatternCriticality criticality) { + public boolean addPattern(LedPattern pattern, Leds.PatternLevel criticality) { patterns[criticality.ordinal()] = pattern; isNew = true; return getPattern() == pattern; diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index 705707a8..f2b5a6e4 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -38,30 +38,30 @@ public void periodic() { strip.setData(ledBuffer); } - public void setPattern(Zone zone, LedPattern pattern, PatternCriticality criticality) { - patterns[zone.ordinal()].addPattern(pattern, criticality); + public void setPattern(Zone zone, LedPattern pattern, PatternLevel priority) { + patterns[zone.ordinal()].addPattern(pattern, priority); pattern.setLength(buffers[zone.ordinal()].getLength()); } public void setPattern(Zone zone, LedPattern pattern) { - setPattern(zone, pattern, PatternCriticality.NORMAL); + setPattern(zone, pattern, PatternLevel.DEFAULT); } - public void clearPattern(Zone zone, PatternCriticality criticality) { - patterns[zone.ordinal()].clearPattern(criticality); + public void clearPattern(Zone zone, PatternLevel priority) { + patterns[zone.ordinal()].clearPattern(priority); } - public void setPatternAll(Supplier patternSupplier, PatternCriticality criticality) { + public void setPatternAll(Supplier patternSupplier, PatternLevel priority) { for (int i = 0; i this.setPattern(Leds.Zone.ELEVATOR_BACK, pattern, criticality), - () -> this.clearPattern(Leds.Zone.ELEVATOR_BACK, criticality) - ); + () -> this.setPattern(Leds.Zone.ELEVATOR_BACK, pattern, priority), + () -> this.clearPattern(Leds.Zone.ELEVATOR_BACK, priority) + ).ignoringDisable(true); + } + public Command commandSet(LedPattern pattern, PatternLevel priority) { + return Commands.runOnce(() -> this.setPattern(Leds.Zone.ELEVATOR_BACK, pattern, priority)).ignoringDisable(true); + } + public Command commandClear(PatternLevel priority) { + return Commands.runOnce(() -> this.clearPattern(Leds.Zone.ELEVATOR_BACK, priority)).ignoringDisable(true); } } diff --git a/src/main/java/org/team1540/robot2024/util/CommandUtils.java b/src/main/java/org/team1540/robot2024/util/CommandUtils.java index eeac3fd1..6f02fb83 100644 --- a/src/main/java/org/team1540/robot2024/util/CommandUtils.java +++ b/src/main/java/org/team1540/robot2024/util/CommandUtils.java @@ -14,11 +14,17 @@ public static Command startStopTimed(Runnable start, Runnable end, double durati ); } - public static Command rumbleCommand(XboxController controller, double amount, double duration) { + public static Command rumbleCommandTimed(XboxController controller, double amount, double duration) { return startStopTimed( () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, amount), () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 0), duration ); } + public static Command rumbleCommand(XboxController controller, double amount) { + return Commands.startEnd( + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, amount), + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 0) + ); + } } From 5ac87cac30125e2a426adf442f072a752265d541 Mon Sep 17 00:00:00 2001 From: Zach R Date: Sun, 10 Mar 2024 22:16:51 -0700 Subject: [PATCH 05/75] refactor: move driver feedback to RobotContainer --- .../team1540/robot2024/RobotContainer.java | 15 +++-------- .../commands/indexer/IntakeCommand.java | 25 ++----------------- .../team1540/robot2024/util/CommandUtils.java | 4 +++ 3 files changed, 10 insertions(+), 34 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 78341633..1970951f 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -161,17 +161,14 @@ private void configureButtonBindings() { })); copilot.leftBumper().whileTrue(new AmpScoreSequence(tramp, indexer, elevator)); - Command intakeCommand = new IntakeCommand(indexer, tramp::isNoteStaged, 1, driver.getHID(), copilot.getHID()); + Command intakeCommand = new IntakeCommand(indexer, tramp::isNoteStaged, 1) + .deadlineWith(CommandUtils.rumbleCommand(driver, 0.5), CommandUtils.rumbleCommand(copilot, 0.5)); copilot.rightBumper().whileTrue(intakeCommand); copilot.povDown().whileTrue(indexer.commandRunIntake(-1)); copilot.povUp().whileTrue(indexer.commandRunIntake(1)); copilot.povRight().whileTrue(Commands.startEnd(() -> tramp.setPercent(1), tramp::stop, tramp)); -// copilot.povLeft().onTrue(CommandUtils.startStopTimed( -// () -> leds.setPatternAll(() -> new LedPatternWave(DriverStation.getAlliance().orElse(DriverStation.Alliance.Red) == DriverStation.Alliance.Red ? 0: 216), Leds.PatternCriticality.HIGH), -// () -> leds.setPatternAll(() -> new LedPatternWave(DriverStation.getAlliance().orElse(DriverStation.Alliance.Red) == DriverStation.Alliance.Red ? 0: 216), Leds.PatternCriticality.HIGH), -// 5 -// )); + copilot.rightTrigger(0.95).whileTrue(Commands.startEnd(() -> tramp.setPercent(1), tramp::stop, tramp)); copilot.leftTrigger(0.95).whileTrue(new ClimbAlignment(drivetrain, elevator, null, tramp, indexer, shooter)); @@ -196,16 +193,12 @@ private void configureButtonBindings() { .onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 1, 1)) .whileTrue(leds.commandShowPattern(new LedPatternWave(0), Leds.PatternLevel.INTAKE_STATE)); - new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.3, 0.4)); + new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4)); new Trigger(RobotController::getUserButton).toggleOnTrue(Commands.startEnd( () -> { elevator.setBrakeMode(false); leds.setPatternAll(() -> new LedPatternRSLState(Color.kMagenta), Leds.PatternLevel.ELEVATOR_STATE); - Commands.sequence( - Commands.waitSeconds(5), - leds.commandClear(Leds.PatternLevel.ELEVATOR_STATE) - ).schedule(); }, () -> { elevator.setBrakeMode(true); diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java index 58b3a6b4..c6942759 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java @@ -12,42 +12,23 @@ public class IntakeCommand extends Command { private final double percent; private final boolean shouldUseBeambreak; private final BooleanSupplier noteInTramp; - private final XboxController driver; - private final XboxController copilot; - public IntakeCommand(Indexer indexer, BooleanSupplier noteInTramp, double percent, XboxController driver, XboxController copilot) { - this.indexer = indexer; - this.noteInTramp = noteInTramp; - this.percent = percent; - this.shouldUseBeambreak = true; - this.driver = driver; - this.copilot = copilot; - addRequirements(indexer); - } public IntakeCommand(Indexer indexer, BooleanSupplier noteInTramp, double percent) { - this(indexer, noteInTramp, percent, null, null); + this(indexer, noteInTramp, percent, true); } - public IntakeCommand(Indexer indexer, BooleanSupplier noteInTramp, double percent, boolean shouldUseBeambreak, XboxController driver, XboxController copilot) { + public IntakeCommand(Indexer indexer, BooleanSupplier noteInTramp, double percent, boolean shouldUseBeambreak) { this.indexer = indexer; this.noteInTramp = noteInTramp; this.percent = percent; this.shouldUseBeambreak = shouldUseBeambreak; - this.driver = driver; - this.copilot = copilot; addRequirements(indexer); } - public IntakeCommand(Indexer indexer, BooleanSupplier noteInTramp, double percent, boolean shouldUseBeambreak) { - this(indexer, noteInTramp, percent, shouldUseBeambreak, null, null); - } - @Override public void initialize() { indexer.setIntakePercent(percent); - if (copilot != null) copilot.setRumble(GenericHID.RumbleType.kLeftRumble, 0.5); - if (driver != null) driver.setRumble(GenericHID.RumbleType.kLeftRumble, 0.5); } @Override @@ -58,7 +39,5 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { indexer.stopIntake(); - if (copilot != null) copilot.setRumble(GenericHID.RumbleType.kLeftRumble, 0); - if (driver != null) driver.setRumble(GenericHID.RumbleType.kLeftRumble, 0); } } diff --git a/src/main/java/org/team1540/robot2024/util/CommandUtils.java b/src/main/java/org/team1540/robot2024/util/CommandUtils.java index 6f02fb83..aff67d44 100644 --- a/src/main/java/org/team1540/robot2024/util/CommandUtils.java +++ b/src/main/java/org/team1540/robot2024/util/CommandUtils.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class CommandUtils { public static Command startStopTimed(Runnable start, Runnable end, double duration, Subsystem... requirements) { @@ -27,4 +28,7 @@ public static Command rumbleCommand(XboxController controller, double amount) { () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 0) ); } + public static Command rumbleCommand(CommandXboxController controller, double amount) { + return rumbleCommand(controller.getHID(), amount); + } } From 82e3b17cfb1a0f7cfec60f4605c6ed5ae08ca555 Mon Sep 17 00:00:00 2001 From: Zach R Date: Sun, 10 Mar 2024 22:54:53 -0700 Subject: [PATCH 06/75] fix: don't crash, clean up unused imports --- src/main/java/org/team1540/robot2024/Constants.java | 2 +- src/main/java/org/team1540/robot2024/Robot.java | 10 ++++++---- .../robot2024/commands/indexer/IntakeCommand.java | 2 -- .../org/team1540/robot2024/subsystems/led/Leds.java | 1 - 4 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 4af35230..c3a6d177 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -26,7 +26,7 @@ public final class Constants { public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; public static final class Leds { public static final int LED_STRIP_PORT_PWM = 9; - public static final int LED_STRIP_LENGTH= 40; + public static final int LED_STRIP_LENGTH= 41; } public enum Mode { /** diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index b3b213bd..5461bf01 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.littletonrobotics.junction.LogFileUtil; @@ -11,9 +10,11 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import org.team1540.robot2024.subsystems.led.*; -import org.team1540.robot2024.subsystems.led.patterns.*; -import org.team1540.robot2024.util.LoggedTunableNumber; +import org.team1540.robot2024.subsystems.led.Leds; +import org.team1540.robot2024.subsystems.led.patterns.LedPattern; +import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame; +import org.team1540.robot2024.subsystems.led.patterns.LedPatternRainbow; +import org.team1540.robot2024.subsystems.led.patterns.LedPatternTuneColor; import org.team1540.robot2024.util.MechanismVisualiser; import org.team1540.robot2024.util.auto.AutoManager; import org.team1540.robot2024.util.vision.AprilTagsCrescendo; @@ -37,6 +38,7 @@ public class Robot extends LoggedRobot { public void robotInit() { // Record metadata Logger.recordMetadata("IsCompBot", String.valueOf(Constants.IS_COMPETITION_ROBOT)); + Logger.recordMetadata("IsTuningMode", String.valueOf(Constants.isTuningMode())); Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java index c6942759..65c73b56 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java @@ -1,7 +1,5 @@ package org.team1540.robot2024.commands.indexer; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import org.team1540.robot2024.subsystems.indexer.Indexer; diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index f2b5a6e4..8558665f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -72,7 +72,6 @@ public enum Zone { } static final int LEVEL_COUNT = PatternLevel.values().length; public enum PatternLevel { - LOWEST, DEFAULT, INTAKE_STATE, DRIVER_LOCK, From ce13bdb37ccebf73cfdcbfd1bea2ba772f221030 Mon Sep 17 00:00:00 2001 From: Zach R Date: Sun, 10 Mar 2024 23:01:48 -0700 Subject: [PATCH 07/75] fix: gracefully handle error from leds --- .../robot2024/subsystems/led/Leds.java | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index 8558665f..25c72967 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -2,9 +2,11 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.team1540.robot2024.Robot; import org.team1540.robot2024.subsystems.led.patterns.LedPattern; import java.util.function.Supplier; @@ -30,12 +32,20 @@ public Leds() { @Override public void periodic() { - for (int i = 0; i < ZONE_COUNT;i++) { - if (patterns[i].shouldRefresh()) { - patterns[i].getPattern().apply(buffers[i]); + try { + for (int i = 0; i < ZONE_COUNT; i++) { + if (patterns[i].shouldRefresh()) { + patterns[i].getPattern().apply(buffers[i]); + } + } + strip.setData(ledBuffer); + } catch (Exception e) { + if (Robot.isReal()) { + DriverStation.reportWarning("Error in LEDs periodic: "+e+": "+e.getMessage(), e.getStackTrace()); + } else { + throw e; } } - strip.setData(ledBuffer); } public void setPattern(Zone zone, LedPattern pattern, PatternLevel priority) { From d0b1f79025789622b238aac935a28378974af35d Mon Sep 17 00:00:00 2001 From: Zach R Date: Mon, 11 Mar 2024 10:57:33 -0700 Subject: [PATCH 08/75] feat: put shooter down on elevator raise --- src/main/java/org/team1540/robot2024/RobotContainer.java | 3 +++ .../robot2024/commands/shooter/PrepareShooterCommand.java | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 1970951f..5caacaca 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -31,6 +31,7 @@ import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher; import org.team1540.robot2024.util.auto.AutoCommand; import org.team1540.robot2024.util.auto.AutoManager; +import org.team1540.robot2024.util.shooter.ShooterSetpoint; import static org.team1540.robot2024.Constants.SwerveConfig; import static org.team1540.robot2024.Constants.isTuningMode; @@ -189,6 +190,8 @@ private void configureButtonBindings() { // copilot.leftTrigger(0.5).whileTrue(new ElevatorSetpointCommand(elevator, ElevatorState.CLIMB)); + new Trigger(() -> elevator.getPosition() > 0.1).debounce(0.1) + .whileTrue(PrepareShooterCommand.lowerPivot(shooter)); new Trigger(indexer::isNoteStaged).debounce(0.1) .onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 1, 1)) .whileTrue(leds.commandShowPattern(new LedPatternWave(0), Leds.PatternLevel.INTAKE_STATE)); diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java index 1337626b..34460b22 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java @@ -17,6 +17,10 @@ public PrepareShooterCommand(Shooter shooter, Supplier setpoint this.setpoint = setpoint; addRequirements(shooter); } + + public static PrepareShooterCommand lowerPivot(Shooter shooter) { + return new PrepareShooterCommand(shooter, ()->new ShooterSetpoint(0,0,0)); + } @Override public void execute() { ShooterSetpoint setpoint = this.setpoint.get(); From ed4b7c6aad3fcdafb67e6ed1377be2d6cf857bf1 Mon Sep 17 00:00:00 2001 From: Zach R Date: Mon, 11 Mar 2024 11:00:26 -0700 Subject: [PATCH 09/75] bump: upgrade vendordeps --- vendordeps/AdvantageKit.json | 10 +++++----- vendordeps/PathplannerLib.json | 6 +++--- vendordeps/REVLib-2024.json | 10 +++++----- vendordeps/photonlib-json-1.0.json | 10 +++++----- 4 files changed, 18 insertions(+), 18 deletions(-) diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index b47c2203..2cce1d24 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.1.0", + "version": "3.1.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.1.0" + "version": "3.1.1" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.1.0" + "version": "3.1.1" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.1.0" + "version": "3.1.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.1.0", + "version": "3.1.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 787450f4..3ec4c12f 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.4", + "version": "2024.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.4" + "version": "2024.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.4", + "version": "2024.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib-2024.json b/vendordeps/REVLib-2024.json index 74e5a37f..6c04fab6 100644 --- a/vendordeps/REVLib-2024.json +++ b/vendordeps/REVLib-2024.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.2", + "version": "2024.2.3", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.2" + "version": "2024.2.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.2", + "version": "2024.2.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.2", + "version": "2024.2.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.2", + "version": "2024.2.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib-json-1.0.json b/vendordeps/photonlib-json-1.0.json index d2fb2a57..5850529a 100644 --- a/vendordeps/photonlib-json-1.0.json +++ b/vendordeps/photonlib-json-1.0.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.8", + "version": "v2024.2.9", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.8", + "version": "v2024.2.9", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.8", + "version": "v2024.2.9", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.8" + "version": "v2024.2.9" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.8" + "version": "v2024.2.9" } ] } From be2bd7f422bf86e87602b4d7ccaf28101158d1bc Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Tue, 12 Mar 2024 09:19:10 -0700 Subject: [PATCH 10/75] feat: vision stddev scaling based on distance and stop updating heading based on single tag poses --- .../org/team1540/robot2024/Constants.java | 12 +++---- .../subsystems/drive/Drivetrain.java | 11 +++--- .../subsystems/vision/AprilTagVision.java | 25 ++++++------- .../subsystems/vision/AprilTagVisionIO.java | 5 ++- .../vision/AprilTagVisionIOLimelight.java | 12 +++---- .../vision/AprilTagVisionIOPhoton.java | 17 ++++----- .../vision/AprilTagVisionIOSim.java | 27 ++++++++------ .../util/vision/EstimatedVisionPose.java | 36 +++++++++++++++++++ .../util/vision/TimestampedVisionPose.java | 25 ------------- .../util/vision/VisionPoseAcceptor.java | 8 ++--- 10 files changed, 96 insertions(+), 82 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java delete mode 100644 src/main/java/org/team1540/robot2024/util/vision/TimestampedVisionPose.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index c3a6d177..91ad7a98 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -109,13 +109,13 @@ public static class Vision { public static final String FRONT_CAMERA_NAME = "limelight-front"; public static final String REAR_CAMERA_NAME = "limelight-rear"; - //0.341306 - //0.609832 - // TODO: measure these offsets public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.0975290, 0, 0.665479, new Rotation3d(0, Math.toRadians(-25), 0)); public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.03639, 0, 0.715274, new Rotation3d(Math.PI, 0, Math.PI)); - // TODO: find these values + // TODO i literally stole these from 6328 check if these actually work + public static final double XY_STD_DEV_COEFF = 0.005; + public static final double ROT_STD_DEV_COEFF = 0.01; + public static final double MAX_AMBIGUITY_RATIO = 0.3; public static final double MAX_VISION_DELAY_SECS = 0.08; public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0; @@ -126,9 +126,9 @@ public static class Vision { public static final int SIM_RES_WIDTH = 1280; public static final int SIM_RES_HEIGHT = 960; - public static final Rotation2d SIM_DIAGONAL_FOV = Rotation2d.fromDegrees(100); + public static final Rotation2d SIM_DIAGONAL_FOV = Rotation2d.fromDegrees(70); public static final double SIM_FPS = 14.5; - public static final double SIM_AVG_LATENCY_MS = 67.0; + public static final double SIM_AVG_LATENCY_MS = 100; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 6b415d1d..70ec170e 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -17,7 +17,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -27,7 +26,7 @@ import org.team1540.robot2024.Constants; import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher; import org.team1540.robot2024.util.swerve.SwerveFactory; -import org.team1540.robot2024.util.vision.TimestampedVisionPose; +import org.team1540.robot2024.util.vision.EstimatedVisionPose; import org.team1540.robot2024.util.vision.VisionPoseAcceptor; import java.util.function.Supplier; @@ -326,12 +325,14 @@ public void setPose(Pose2d pose) { visionPoseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } - public void addVisionMeasurement(TimestampedVisionPose visionPose) { + public void addVisionMeasurement(EstimatedVisionPose visionPose) { boolean shouldAccept = poseAcceptor.shouldAcceptVision(visionPose); if (shouldAccept) { - visionPoseEstimator.addVisionMeasurement(visionPose.poseMeters, visionPose.timestampSecs); + visionPoseEstimator.setVisionMeasurementStdDevs(visionPose.getStdDevs()); + visionPoseEstimator.addVisionMeasurement(visionPose.poseMeters.toPose2d(), visionPose.timestampSecs); if (!blockTags) { - poseEstimator.addVisionMeasurement(visionPose.poseMeters, visionPose.timestampSecs); + poseEstimator.setVisionMeasurementStdDevs(visionPose.getStdDevs()); + poseEstimator.addVisionMeasurement(visionPose.poseMeters.toPose2d(), visionPose.timestampSecs); } } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java index e73456c7..bea6779b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java @@ -2,13 +2,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; import org.team1540.robot2024.Constants; -import org.team1540.robot2024.util.vision.TimestampedVisionPose; -import org.team1540.robot2024.util.vision.VisionPoseAcceptor; +import org.team1540.robot2024.util.vision.EstimatedVisionPose; import java.util.function.Consumer; import java.util.function.Supplier; @@ -22,18 +20,18 @@ public class AprilTagVision extends SubsystemBase { private final AprilTagVisionIO rearCameraIO; private final AprilTagVisionIOInputsAutoLogged rearCameraInputs = new AprilTagVisionIOInputsAutoLogged(); - private final Consumer visionPoseConsumer; + private final Consumer visionPoseConsumer; private final Supplier elevatorHeightSupplierMeters; - private final TimestampedVisionPose frontPose = new TimestampedVisionPose(); - private final TimestampedVisionPose rearPose = new TimestampedVisionPose(); + private final EstimatedVisionPose frontPose = new EstimatedVisionPose(); + private final EstimatedVisionPose rearPose = new EstimatedVisionPose(); private static boolean hasInstance = false; private AprilTagVision( AprilTagVisionIO frontCameraIO, AprilTagVisionIO rearCameraIO, - Consumer visionPoseConsumer, + Consumer visionPoseConsumer, Supplier elevatorHeightSupplierMeters) { if (hasInstance) throw new IllegalStateException("Instance of vision already exists"); hasInstance = true; @@ -44,7 +42,7 @@ private AprilTagVision( this.elevatorHeightSupplierMeters = elevatorHeightSupplierMeters; } - public static AprilTagVision createReal(Consumer visionPoseConsumer, + public static AprilTagVision createReal(Consumer visionPoseConsumer, Supplier elevatorHeightSupplierMeters) { if (Constants.currentMode != Constants.Mode.REAL) { DriverStation.reportWarning("Using real vision on simulated robot", false); @@ -56,7 +54,7 @@ public static AprilTagVision createReal(Consumer visionPo elevatorHeightSupplierMeters); } - public static AprilTagVision createSim(Consumer visionPoseConsumer, + public static AprilTagVision createSim(Consumer visionPoseConsumer, Supplier drivetrainPoseSupplier, Supplier elevatorHeightSupplierMeters) { if (Constants.currentMode == Constants.Mode.REAL) { @@ -106,13 +104,12 @@ public void periodic() { updateAndAcceptPose(rearCameraInputs, rearPose); } - private void updateAndAcceptPose(AprilTagVisionIOInputsAutoLogged cameraInputs, TimestampedVisionPose pose) { + private void updateAndAcceptPose(AprilTagVisionIOInputsAutoLogged cameraInputs, EstimatedVisionPose pose) { if (cameraInputs.lastMeasurementTimestampSecs > pose.timestampSecs) { pose.timestampSecs = cameraInputs.lastMeasurementTimestampSecs; - pose.poseMeters = cameraInputs.estimatedPoseMeters.toPose2d(); - pose.hasTargets = cameraInputs.hasTargets; - pose.primaryTagID = cameraInputs.primaryTagID; - pose.primaryTagPose = cameraInputs.primaryTagPoseMeters.toPose2d(); + pose.poseMeters = cameraInputs.estimatedPoseMeters; + pose.tagIDs = cameraInputs.seenTagIDs; + pose.tagPoses = cameraInputs.tagPosesMeters; visionPoseConsumer.accept(pose); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java index 9f926d6e..75464950 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java @@ -7,9 +7,8 @@ public interface AprilTagVisionIO { @AutoLog class AprilTagVisionIOInputs { public Pose3d estimatedPoseMeters = new Pose3d(); - public boolean hasTargets = false; - public int primaryTagID = 0; - public Pose3d primaryTagPoseMeters = new Pose3d(); + public int[] seenTagIDs = new int[0]; + public Pose3d[] tagPosesMeters = new Pose3d[0]; public double lastMeasurementTimestampSecs = 0.0; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java index b2d28f3f..c0fae798 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -30,19 +30,19 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { Math.toRadians(botPose[4]), Math.toRadians(botPose[5]))); - inputs.hasTargets = table.getEntry("tv").getNumber(0).intValue() == 1; - inputs.primaryTagID = inputs.hasTargets ? table.getEntry("tid").getNumber(0).intValue() : -1; + boolean hasTargets = table.getEntry("tv").getNumber(0).intValue() == 1; + inputs.seenTagIDs = hasTargets ? new int[]{table.getEntry("tid").getNumber(0).intValue()} : new int[0]; double[] tagPose = table.getEntry("targetpose_robotspace").getDoubleArray(new double[6]); - inputs.primaryTagPoseMeters = inputs.hasTargets - ? new Pose3d( + inputs.tagPosesMeters = hasTargets + ? new Pose3d[] {new Pose3d( tagPose[0], tagPose[1], tagPose[2], new Rotation3d( Math.toRadians(tagPose[3]), Math.toRadians(tagPose[4]), - Math.toRadians(tagPose[5]))) - : new Pose3d(); + Math.toRadians(tagPose[5])))} + : new Pose3d[0]; inputs.lastMeasurementTimestampSecs = Timer.getFPGATimestamp() - (table.getEntry("cl").getDouble(0) + table.getEntry("tl").getDouble(0)) / 1000.0; diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java index 0a7b0530..1ce1f901 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java @@ -10,6 +10,7 @@ import org.team1540.robot2024.Constants; import org.team1540.robot2024.util.vision.AprilTagsCrescendo; +import java.util.List; import java.util.Optional; public class AprilTagVisionIOPhoton implements AprilTagVisionIO { @@ -34,10 +35,11 @@ public AprilTagVisionIOPhoton(String name, Pose3d cameraOffsetMeters) { @Override public void updateInputs(AprilTagVisionIOInputs inputs) { PhotonPipelineResult latestResult = camera.getLatestResult(); + List targets = latestResult.getTargets(); Optional estimatedPose = photonEstimator.update(latestResult); double maxAmbiguityRatio = 0; - for (PhotonTrackedTarget target : latestResult.getTargets()) + for (PhotonTrackedTarget target : targets) if (target.getPoseAmbiguity() > maxAmbiguityRatio) maxAmbiguityRatio = target.getPoseAmbiguity(); if (estimatedPose.isPresent() && maxAmbiguityRatio < Constants.Vision.MAX_AMBIGUITY_RATIO) { @@ -46,13 +48,12 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds; } - inputs.hasTargets = latestResult.hasTargets(); - inputs.primaryTagID = inputs.hasTargets ? latestResult.getBestTarget().getFiducialId() : -1; - inputs.primaryTagPoseMeters = - inputs.hasTargets - ? new Pose3d().plus( - latestResult.getBestTarget().getBestCameraToTarget().plus(cameraTransform.inverse())) - : new Pose3d(); + inputs.seenTagIDs = new int[targets.size()]; + inputs.tagPosesMeters = new Pose3d[targets.size()]; + for (int i = 0; i < targets.size(); i++) { + inputs.seenTagIDs[i] = targets.get(i).getFiducialId(); + inputs.tagPosesMeters[i] = new Pose3d().plus(targets.get(i).getBestCameraToTarget()); + } } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java index d75a5b89..76be899a 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java @@ -12,8 +12,11 @@ import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.team1540.robot2024.Constants; import java.io.IOException; +import java.util.List; import java.util.Optional; import java.util.function.Supplier; @@ -57,7 +60,7 @@ public AprilTagVisionIOSim(String name, Pose3d cameraOffsetMeters, Supplier estimatedPose = photonEstimator.update(); PhotonPipelineResult latestResult = camera.getLatestResult(); + List targets = latestResult.getTargets(); + Optional estimatedPose = photonEstimator.update(latestResult); - if (estimatedPose.isPresent()) { + double maxAmbiguityRatio = 0; + for (PhotonTrackedTarget target : targets) + if (target.getPoseAmbiguity() > maxAmbiguityRatio) maxAmbiguityRatio = target.getPoseAmbiguity(); + + if (estimatedPose.isPresent() && maxAmbiguityRatio < Constants.Vision.MAX_AMBIGUITY_RATIO) { lastEstimatedPose = estimatedPose.get().estimatedPose; inputs.estimatedPoseMeters = lastEstimatedPose; inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds; } - inputs.hasTargets = latestResult.hasTargets(); - inputs.primaryTagID = inputs.hasTargets ? latestResult.getBestTarget().getFiducialId() : -1; - inputs.primaryTagPoseMeters = - inputs.hasTargets - ? new Pose3d().plus( - latestResult.getBestTarget().getBestCameraToTarget().plus(cameraTransform.inverse())) - : new Pose3d(); + inputs.seenTagIDs = new int[targets.size()]; + inputs.tagPosesMeters = new Pose3d[targets.size()]; + for (int i = 0; i < targets.size(); i++) { + inputs.seenTagIDs[i] = targets.get(i).getFiducialId(); + inputs.tagPosesMeters[i] = new Pose3d().plus(targets.get(i).getBestCameraToTarget()); + } } @Override diff --git a/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java b/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java new file mode 100644 index 00000000..09ecb176 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java @@ -0,0 +1,36 @@ +package org.team1540.robot2024.util.vision; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import org.team1540.robot2024.Constants; + +public class EstimatedVisionPose { + public double timestampSecs = -1; + public Pose3d poseMeters = new Pose3d(); + public int[] tagIDs = new int[0]; + public Pose3d[] tagPoses = new Pose3d[0]; + + public double getAvgTagDistance() { + double avgDistance = 0; + for (Pose3d pose : tagPoses) avgDistance += pose.getTranslation().getNorm(); + avgDistance /= tagPoses.length; + return avgDistance; + } + + public Matrix getStdDevs() { + double avgDistance = getAvgTagDistance(); + + double xyStdDev = + Constants.Vision.XY_STD_DEV_COEFF + * Math.pow(avgDistance, 2.0) + / tagPoses.length; + double rotStdDev = + Constants.Vision.ROT_STD_DEV_COEFF + * Math.pow(avgDistance, 2.0) + / tagPoses.length; + return VecBuilder.fill(xyStdDev, xyStdDev, tagIDs.length > 1 ? rotStdDev : Double.POSITIVE_INFINITY); + } +} diff --git a/src/main/java/org/team1540/robot2024/util/vision/TimestampedVisionPose.java b/src/main/java/org/team1540/robot2024/util/vision/TimestampedVisionPose.java deleted file mode 100644 index b64f87fd..00000000 --- a/src/main/java/org/team1540/robot2024/util/vision/TimestampedVisionPose.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.team1540.robot2024.util.vision; - -import edu.wpi.first.math.geometry.Pose2d; - -import java.util.Objects; - -public class TimestampedVisionPose { - public double timestampSecs = -1; - public Pose2d poseMeters = new Pose2d(); - public boolean hasTargets = false; - public int primaryTagID = -1; - public Pose2d primaryTagPose = new Pose2d(); - - @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - TimestampedVisionPose that = (TimestampedVisionPose) o; - return Double.compare(timestampSecs, that.timestampSecs) == 0 - && hasTargets == that.hasTargets - && primaryTagID == that.primaryTagID - && Objects.equals(poseMeters, that.poseMeters) - && Objects.equals(primaryTagPose, that.primaryTagPose); - } -} diff --git a/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java b/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java index 34141705..8c70c605 100644 --- a/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java +++ b/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java @@ -1,8 +1,6 @@ package org.team1540.robot2024.util.vision; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.RobotState; -import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; @@ -19,17 +17,17 @@ public VisionPoseAcceptor( this.elevatorVelocitySupplier = elevatorVelocitySupplier; } - public boolean shouldAcceptVision(TimestampedVisionPose visionPose) { + public boolean shouldAcceptVision(EstimatedVisionPose visionPose) { ChassisSpeeds robotVelocity = robotVelocitySupplier.get(); double elevatorVelocity = elevatorVelocitySupplier.get(); // Do not accept poses that have too much delay // if (Timer.getFPGATimestamp() - visionPose.timestampSecs >= MAX_VISION_DELAY_SECS) return false; // Do not accept poses that see too little tags - if (!visionPose.hasTargets) return false; + if (visionPose.tagIDs.length < MIN_ACCEPTED_NUM_TAGS) return false; // Do not accept poses that have an average tag distance that is too far away - if (visionPose.primaryTagPose.getTranslation().getNorm() > MAX_ACCEPTED_AVG_TAG_DIST_METERS) return false; + if (visionPose.getAvgTagDistance() > MAX_ACCEPTED_AVG_TAG_DIST_METERS) return false; // Do not accept poses taken when the robot has too much rotational or translational velocity boolean rotatingTooFast = Math.abs(robotVelocity.omegaRadiansPerSecond) > MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC; From 27b2b18e3c9f56acf9c54a987230b6cc88b36af1 Mon Sep 17 00:00:00 2001 From: Zach R Date: Tue, 12 Mar 2024 22:51:12 -0700 Subject: [PATCH 11/75] fix: use command run for tramp --- src/main/java/org/team1540/robot2024/RobotContainer.java | 4 ++-- .../team1540/robot2024/commands/indexer/IntakeAndFeed.java | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 5caacaca..4a9d016f 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -168,10 +168,10 @@ private void configureButtonBindings() { copilot.povDown().whileTrue(indexer.commandRunIntake(-1)); copilot.povUp().whileTrue(indexer.commandRunIntake(1)); - copilot.povRight().whileTrue(Commands.startEnd(() -> tramp.setPercent(1), tramp::stop, tramp)); + copilot.povRight().whileTrue(tramp.commandRun(1)); - copilot.rightTrigger(0.95).whileTrue(Commands.startEnd(() -> tramp.setPercent(1), tramp::stop, tramp)); + copilot.rightTrigger(0.95).whileTrue(tramp.commandRun(1)); copilot.leftTrigger(0.95).whileTrue(new ClimbAlignment(drivetrain, elevator, null, tramp, indexer, shooter)); diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeAndFeed.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeAndFeed.java index 81e0eee1..0123bf2a 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeAndFeed.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeAndFeed.java @@ -13,6 +13,7 @@ public class IntakeAndFeed extends Command { public static Command withDefaults(Indexer indexer) { return new IntakeAndFeed(indexer, () -> 1, () -> 1); } + public IntakeAndFeed(Indexer indexer, DoubleSupplier intakePercent, DoubleSupplier feederPercent) { this.indexer = indexer; this.intakePercent = intakePercent; From 529b057c18790d4484a5e05af4b228cca52376ac Mon Sep 17 00:00:00 2001 From: Zach R Date: Tue, 12 Mar 2024 22:52:05 -0700 Subject: [PATCH 12/75] feat: speed up shooting feeder speed --- src/main/java/org/team1540/robot2024/RobotContainer.java | 2 +- .../org/team1540/robot2024/commands/shooter/ShootSequence.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 4a9d016f..0e366da0 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -180,7 +180,7 @@ private void configureButtonBindings() { // copilot.b().whileTrue(new ShootSequence(shooter, indexer, PODIUM_SHOOT)); // copilot.b().whileTrue(new TuneShooterCommand(shooter, indexer)); copilot.b() - .whileTrue(new IntakeAndFeed(indexer, () -> 1, () -> 0.5)) + .whileTrue(IntakeAndFeed.withDefaults(indexer)) .onFalse(Commands.runOnce(() -> { targetDrive.cancel(); overstageTargetDrive.cancel(); diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java index fcb2b04b..3bef39de 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java @@ -38,7 +38,7 @@ public ShootSequence(Shooter shooter, Indexer indexer, Supplier new PrepareShooterCommand(shooter, setpoint), Commands.sequence( Commands.waitSeconds(waitTime), - new IntakeAndFeed(indexer, () -> 1, () -> 0.5).withTimeout(0.5) + IntakeAndFeed.withDefaults(indexer).withTimeout(0.5) ) // TODO: Add a wait for having completed the shot (steady then current spike/velocity dip and then back down?) From 542773dea907a9076aec40392f5501412f4a511a Mon Sep 17 00:00:00 2001 From: Zach R Date: Tue, 12 Mar 2024 23:08:09 -0700 Subject: [PATCH 13/75] fix: allow passing colors to led patterns --- .../subsystems/led/patterns/LedPattern.java | 35 +++++-------------- .../led/patterns/LedPatternBreathing.java | 2 +- .../led/patterns/LedPatternWave.java | 10 +++++- 3 files changed, 19 insertions(+), 28 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPattern.java index 1328f009..24340364 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPattern.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPattern.java @@ -3,6 +3,8 @@ import edu.wpi.first.wpilibj.util.Color; import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer; +import java.util.Arrays; + public abstract class LedPattern { private final boolean isDynamic; @@ -15,33 +17,14 @@ public final boolean isDynamic() { } public abstract void apply(ZonedAddressableLEDBuffer buffer); - public void setLength(int length) {} - - protected static int getHue(Color color) { - final int red = (int) color.red * 255; - final int green = (int) color.green * 255; - final int blue = (int) color.blue * 255; - float min = Math.min(Math.min(red, green), blue); - float max = Math.max(Math.max(red, green), blue); - - if (min == max) { - return 0; - } - float hue = 0f; - if (max == red) { - hue = (green - blue) / (max - min); - - } else if (max == green) { - hue = 2f + (blue - red) / (max - min); - - } else { - hue = 4f + (red - green) / (max - min); - } - - hue = hue * 60; - if (hue < 0) hue = hue + 360; + public void setLength(int length) {} - return Math.round(hue); + protected static int[] getHSV(Color color) { + float[] val = java.awt.Color.RGBtoHSB((int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255), null); + return new int[]{ + (int) (val[0] * 180), + (int)(val[1] * 255), + (int)(val[2] * 255)}; } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java index 57a0de3c..c2a0c134 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java @@ -12,7 +12,7 @@ public class LedPatternBreathing extends LedPattern { public LedPatternBreathing(int speed, Color color) { super(true); this.speed = speed; - this.hue = getHue(color); + this.hue = getHSV(color)[0]; } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java index 0b20ad97..97567c0a 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java @@ -7,12 +7,20 @@ public class LedPatternWave extends LedPattern { private static final double degradation = 0.7; private static final int delay = 5; private final int hue; + private final int saturation; private int location = 0; private int ticker = 0; public LedPatternWave(int hue) { super(true); this.hue = hue; + this.saturation = 255; + } + public LedPatternWave(Color color) { + super(true); + int[] hsv = getHSV(color); + this.hue = hsv[0]; + this.saturation = hsv[1]; } @Override @@ -26,7 +34,7 @@ public void apply(ZonedAddressableLEDBuffer buffer) { distance = buffer.getLength() - i + location; } } - buffer.setHSV(i, hue, 255, (int) Math.max(255.0 - distance * distance * degradation, 0)); + buffer.setHSV(i, hue, saturation, (int) Math.max(255.0 - distance * distance * degradation, 0)); } ticker++; if (ticker % delay == 0) { From 3b3f49c21282cd7a435686930ec6def39cde9030 Mon Sep 17 00:00:00 2001 From: Zach R Date: Tue, 12 Mar 2024 23:18:15 -0700 Subject: [PATCH 14/75] fix: actually use different colors for different modes --- src/main/java/org/team1540/robot2024/Robot.java | 6 +----- .../java/org/team1540/robot2024/RobotContainer.java | 13 ++++--------- .../subsystems/led/patterns/LedPatternWave.java | 9 ++++----- 3 files changed, 9 insertions(+), 19 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 5461bf01..8759fb73 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -11,10 +11,7 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.team1540.robot2024.subsystems.led.Leds; -import org.team1540.robot2024.subsystems.led.patterns.LedPattern; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternRainbow; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternTuneColor; +import org.team1540.robot2024.subsystems.led.patterns.*; import org.team1540.robot2024.util.MechanismVisualiser; import org.team1540.robot2024.util.auto.AutoManager; import org.team1540.robot2024.util.vision.AprilTagsCrescendo; @@ -28,7 +25,6 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; - private LedPattern flamePattern = new LedPatternFlame(40); /** * This function is run when the robot is first started up and should be used for any diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 0e366da0..f15e9688 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -21,7 +21,6 @@ import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.led.Leds; -import org.team1540.robot2024.subsystems.led.patterns.LedPattern; import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState; import org.team1540.robot2024.subsystems.led.patterns.LedPatternWave; import org.team1540.robot2024.subsystems.shooter.Shooter; @@ -31,7 +30,6 @@ import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher; import org.team1540.robot2024.util.auto.AutoCommand; import org.team1540.robot2024.util.auto.AutoManager; -import org.team1540.robot2024.util.shooter.ShooterSetpoint; import static org.team1540.robot2024.Constants.SwerveConfig; import static org.team1540.robot2024.Constants.isTuningMode; @@ -143,15 +141,12 @@ private void configureButtonBindings() { drivetrain.setBrakeMode(true); }).ignoringDisable(true)); - LedPattern lockedDrivePattern = new LedPatternWave(100); - LedPattern lockedOverstageDrivePattern = new LedPatternWave(280); - Command targetDrive = new AutoShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter) - .alongWith(leds.commandShowPattern(lockedDrivePattern, Leds.PatternLevel.DRIVER_LOCK)); + .alongWith(leds.commandShowPattern(new LedPatternWave("#00a9ff"), Leds.PatternLevel.DRIVER_LOCK)); Command overstageTargetDrive = new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter) - .alongWith(leds.commandShowPattern(lockedOverstageDrivePattern, Leds.PatternLevel.DRIVER_LOCK)); + .alongWith(leds.commandShowPattern(new LedPatternWave("#f700ff"), Leds.PatternLevel.DRIVER_LOCK)); Command autoShooterCommand = new AutoShootPrepare(drivetrain, shooter) - .alongWith(leds.commandShowPattern(new LedPatternWave(200), Leds.PatternLevel.DRIVER_LOCK)); + .alongWith(leds.commandShowPattern(new LedPatternWave("#00ffbc"), Leds.PatternLevel.DRIVER_LOCK)); driver.rightBumper().toggleOnTrue(targetDrive); driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); @@ -194,7 +189,7 @@ private void configureButtonBindings() { .whileTrue(PrepareShooterCommand.lowerPivot(shooter)); new Trigger(indexer::isNoteStaged).debounce(0.1) .onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 1, 1)) - .whileTrue(leds.commandShowPattern(new LedPatternWave(0), Leds.PatternLevel.INTAKE_STATE)); + .whileTrue(leds.commandShowPattern(new LedPatternWave("#ff0000"), Leds.PatternLevel.INTAKE_STATE)); new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4)); diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java index 97567c0a..38185364 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java @@ -11,11 +11,6 @@ public class LedPatternWave extends LedPattern { private int location = 0; private int ticker = 0; - public LedPatternWave(int hue) { - super(true); - this.hue = hue; - this.saturation = 255; - } public LedPatternWave(Color color) { super(true); int[] hsv = getHSV(color); @@ -23,6 +18,10 @@ public LedPatternWave(Color color) { this.saturation = hsv[1]; } + public LedPatternWave(String hex) { + this(new Color(hex)); + } + @Override public void apply(ZonedAddressableLEDBuffer buffer) { for (int i = 0; i < buffer.getLength(); i++) { From 8e18ec494d55462e7d2d51b92308c81103710be3 Mon Sep 17 00:00:00 2001 From: Zach R Date: Tue, 12 Mar 2024 23:29:25 -0700 Subject: [PATCH 15/75] fix: make breathing pattern work and look normal --- .../team1540/robot2024/RobotContainer.java | 3 +- .../led/patterns/LedPatternBreathing.java | 30 +++++++++++++------ .../led/patterns/LedPatternRSLState.java | 2 +- .../led/patterns/SimpleLedPattern.java | 3 +- 4 files changed, 25 insertions(+), 13 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index f15e9688..9fb7fbeb 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -21,6 +21,7 @@ import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.led.Leds; +import org.team1540.robot2024.subsystems.led.patterns.LedPatternBreathing; import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState; import org.team1540.robot2024.subsystems.led.patterns.LedPatternWave; import org.team1540.robot2024.subsystems.shooter.Shooter; @@ -189,7 +190,7 @@ private void configureButtonBindings() { .whileTrue(PrepareShooterCommand.lowerPivot(shooter)); new Trigger(indexer::isNoteStaged).debounce(0.1) .onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 1, 1)) - .whileTrue(leds.commandShowPattern(new LedPatternWave("#ff0000"), Leds.PatternLevel.INTAKE_STATE)); + .whileTrue(leds.commandShowPattern(new LedPatternBreathing("#ff0000"), Leds.PatternLevel.INTAKE_STATE)); new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4)); diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java index c2a0c134..1173feb0 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java @@ -6,34 +6,46 @@ public class LedPatternBreathing extends LedPattern { private final int speed; private final int hue; - private int saturation; + private final int saturation; + private int value = min; private boolean isReversed = false; + private static final int max = 255; + private static final int min = 60; public LedPatternBreathing(int speed, Color color) { super(true); this.speed = speed; - this.hue = getHSV(color)[0]; + int[] hsv = getHSV(color); + this.hue = hsv[0]; + this.saturation = hsv[1]; } + public LedPatternBreathing(int speed, String color) { + this(speed, new Color(color)); + } + public LedPatternBreathing(String color) { + this(3, new Color(color)); + } + @Override public void apply(ZonedAddressableLEDBuffer buffer) { - if (saturation > 255) { - saturation = 255; + if (value > max) { + value = max; isReversed = !isReversed; } - if (saturation < 0) { - saturation = 0; + if (value < min) { + value = min; isReversed = !isReversed; } for (int i = 0; i < buffer.getLength(); i++) { - buffer.setHSV(i, hue, saturation, 255); + buffer.setHSV(i, hue, saturation, value); } if (!isReversed) { - saturation = saturation + speed; + value = value + speed; } else { - saturation = saturation - speed; + value = value - speed; } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java index 6dd4a3c4..be76a94c 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java @@ -30,6 +30,6 @@ public void apply(ZonedAddressableLEDBuffer buffer) { } public static LedPattern matchingColors() { - return new LedPatternRSLState(new Color(1, 0.1, 0)); + return new LedPatternRSLState(new Color("#FF1A00")); } } \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java index 7ff0a952..2c3e316d 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java @@ -22,8 +22,7 @@ public static LedPattern solid(Color color) { return new SimpleLedPattern((buffer, i) -> buffer.setLED(i, color)); } public static LedPattern alternating(Color... colors) { - final int colorCount = colors.length; - return new SimpleLedPattern((buffer, i) -> buffer.setLED(i, colors[i%colorCount])); + return alternating(1, colors); } public static LedPattern alternating(int groupSize, Color... colors) { final int colorCount = colors.length; From 125412946529123d570aa1efc68daa788ec54483 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Wed, 13 Mar 2024 10:05:35 -0700 Subject: [PATCH 16/75] feat: use pose closest to current estimated pose --- .../team1540/robot2024/RobotContainer.java | 3 ++- .../subsystems/drive/Drivetrain.java | 2 +- .../subsystems/vision/AprilTagVision.java | 7 ++++--- .../vision/AprilTagVisionIOPhoton.java | 21 ++++++++++++------- .../vision/AprilTagVisionIOSim.java | 15 ++++++------- 5 files changed, 28 insertions(+), 20 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 5caacaca..055fa21c 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -66,7 +66,8 @@ public RobotContainer() { indexer = Indexer.createReal(); aprilTagVision = AprilTagVision.createReal( drivetrain::addVisionMeasurement, - elevator::getPosition); + elevator::getPosition, + drivetrain::getVisionPose); break; case SIM: // Sim robot, instantiate physics sim IO implementations diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 70ec170e..c5debdfc 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -296,7 +296,7 @@ public Pose2d getPose() { } @AutoLogOutput(key = "Odometry/RobotVision") - private Pose2d getPoseVision() { + public Pose2d getVisionPose() { return visionPoseEstimator.getEstimatedPosition(); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java index bea6779b..3bb7b5d6 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java @@ -43,13 +43,14 @@ private AprilTagVision( } public static AprilTagVision createReal(Consumer visionPoseConsumer, - Supplier elevatorHeightSupplierMeters) { + Supplier elevatorHeightSupplierMeters, + Supplier drivetrainPoseSupplier) { if (Constants.currentMode != Constants.Mode.REAL) { DriverStation.reportWarning("Using real vision on simulated robot", false); } return new AprilTagVision( - new AprilTagVisionIOPhoton(FRONT_CAMERA_NAME, FRONT_CAMERA_POSE), - new AprilTagVisionIOPhoton(REAR_CAMERA_NAME, REAR_CAMERA_POSE), + new AprilTagVisionIOPhoton(FRONT_CAMERA_NAME, FRONT_CAMERA_POSE, drivetrainPoseSupplier), + new AprilTagVisionIOPhoton(REAR_CAMERA_NAME, REAR_CAMERA_POSE, drivetrainPoseSupplier), visionPoseConsumer, elevatorHeightSupplierMeters); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java index 1ce1f901..e18261e3 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java @@ -1,5 +1,6 @@ package org.team1540.robot2024.subsystems.vision; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import org.photonvision.EstimatedRobotPose; @@ -12,15 +13,17 @@ import java.util.List; import java.util.Optional; +import java.util.function.Supplier; public class AprilTagVisionIOPhoton implements AprilTagVisionIO { private final PhotonCamera camera; private final PhotonPoseEstimator photonEstimator; + private final Supplier drivetrainPoseSupplier; private Transform3d cameraTransform; private Pose3d lastEstimatedPose; - public AprilTagVisionIOPhoton(String name, Pose3d cameraOffsetMeters) { + public AprilTagVisionIOPhoton(String name, Pose3d cameraOffsetMeters, Supplier drivetrainPoseSupplier) { camera = new PhotonCamera(name); cameraTransform = new Transform3d(cameraOffsetMeters.getTranslation(), cameraOffsetMeters.getRotation()); photonEstimator = new PhotonPoseEstimator( @@ -28,12 +31,14 @@ public AprilTagVisionIOPhoton(String name, Pose3d cameraOffsetMeters) { PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, cameraTransform); - photonEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY); + photonEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE); lastEstimatedPose = new Pose3d(); + this.drivetrainPoseSupplier = drivetrainPoseSupplier; } @Override public void updateInputs(AprilTagVisionIOInputs inputs) { + photonEstimator.setReferencePose(drivetrainPoseSupplier.get()); PhotonPipelineResult latestResult = camera.getLatestResult(); List targets = latestResult.getTargets(); Optional estimatedPose = photonEstimator.update(latestResult); @@ -46,13 +51,13 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { lastEstimatedPose = estimatedPose.get().estimatedPose; inputs.estimatedPoseMeters = lastEstimatedPose; inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds; - } - inputs.seenTagIDs = new int[targets.size()]; - inputs.tagPosesMeters = new Pose3d[targets.size()]; - for (int i = 0; i < targets.size(); i++) { - inputs.seenTagIDs[i] = targets.get(i).getFiducialId(); - inputs.tagPosesMeters[i] = new Pose3d().plus(targets.get(i).getBestCameraToTarget()); + inputs.seenTagIDs = new int[targets.size()]; + inputs.tagPosesMeters = new Pose3d[targets.size()]; + for (int i = 0; i < targets.size(); i++) { + inputs.seenTagIDs[i] = targets.get(i).getFiducialId(); + inputs.tagPosesMeters[i] = new Pose3d().plus(targets.get(i).getBestCameraToTarget()); + } } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java index 76be899a..d556831b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java @@ -60,7 +60,7 @@ public AprilTagVisionIOSim(String name, Pose3d cameraOffsetMeters, Supplier targets = latestResult.getTargets(); Optional estimatedPose = photonEstimator.update(latestResult); @@ -80,13 +81,13 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { lastEstimatedPose = estimatedPose.get().estimatedPose; inputs.estimatedPoseMeters = lastEstimatedPose; inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds; - } - inputs.seenTagIDs = new int[targets.size()]; - inputs.tagPosesMeters = new Pose3d[targets.size()]; - for (int i = 0; i < targets.size(); i++) { - inputs.seenTagIDs[i] = targets.get(i).getFiducialId(); - inputs.tagPosesMeters[i] = new Pose3d().plus(targets.get(i).getBestCameraToTarget()); + inputs.seenTagIDs = new int[targets.size()]; + inputs.tagPosesMeters = new Pose3d[targets.size()]; + for (int i = 0; i < targets.size(); i++) { + inputs.seenTagIDs[i] = targets.get(i).getFiducialId(); + inputs.tagPosesMeters[i] = new Pose3d().plus(targets.get(i).getBestCameraToTarget()); + } } } From 4e305ee8092d7b354d241da05561d13834784da5 Mon Sep 17 00:00:00 2001 From: Zach R Date: Thu, 14 Mar 2024 08:32:22 -0700 Subject: [PATCH 17/75] fix: module relocation --- src/main/java/org/team1540/robot2024/Constants.java | 6 +++--- .../org/team1540/robot2024/util/swerve/SwerveFactory.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index c3a6d177..7329a115 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -20,7 +20,7 @@ public final class Constants { public static final boolean IS_COMPETITION_ROBOT = true; // Objects.equals(RobotController.getComments(), "comp"); // Whether to pull PID constants from SmartDashboard - private static final boolean tuningMode = false; // TODO: DO NOT SET TO TRUE FOR COMP + private static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; @@ -51,8 +51,8 @@ public static class SwerveConfig { public static final String CAN_BUS = IS_COMPETITION_ROBOT ? "swerve" : "swerve"; public static final double CAN_UPDATE_FREQUENCY_HZ = 200.0; - public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 9 : 1; - public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 2 : 7; + public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 2 : 1; + public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 9 : 7; public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 5 : 4; public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 6 : 3; diff --git a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java index f1869c02..43426e50 100644 --- a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java +++ b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java @@ -16,14 +16,14 @@ public class SwerveFactory { private static final double[] moduleOffsetsRots = new double[]{ -0.9245605469, // Module 1 - 0.16552734375, // Module 2 + 0.6655273438, // Module 2 -0.7197265, // Module 3 -0.7722, // Module 4 -0.41162109375, // Module 5 -0.594970703125, // Module 6 -0.826660, // Module 7 0.0, // Module 8 - 0.75634765625 // Module 9 + 0.2563476562 // Module 9 }; public static SwerveModuleHW getModuleMotors(int id, SwerveCorner corner) { From d8ecd4de64a2eb7cf315cd94008a4d97c29d70e6 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Thu, 14 Mar 2024 17:50:50 -0700 Subject: [PATCH 18/75] feat: shooting on the move and 6328 stolen setpoints --- .../team1540/robot2024/RobotContainer.java | 2 +- .../DriveWithSpeakerLookAheadCommand.java | 75 +++++++++++++++++++ .../commands/indexer/StageTrampCommand.java | 1 - .../commands/shooter/AutoShootPrepare.java | 20 ++--- .../shooter/AutoShootPrepareWhileMoving.java | 39 ++++++++++ .../subsystems/drive/Drivetrain.java | 2 +- .../robot2024/subsystems/shooter/Shooter.java | 20 +++++ .../robot2024/util/shooter/ShooterLerp.java | 4 +- .../util/shooter/ShooterSetpoint.java | 8 +- 9 files changed, 155 insertions(+), 16 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerLookAheadCommand.java create mode 100644 src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 9fb7fbeb..7472142f 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -9,6 +9,7 @@ import org.team1540.robot2024.commands.FeedForwardCharacterization; import org.team1540.robot2024.commands.autos.*; import org.team1540.robot2024.commands.climb.ClimbAlignment; +import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerLookAheadCommand; import org.team1540.robot2024.commands.drivetrain.SwerveDriveCommand; import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; @@ -134,7 +135,6 @@ private void configureButtonBindings() { drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); shooter.setDefaultCommand(manualPivotCommand); - driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); // driver.y().toggleOnTrue(new DriveWithSpeakerTargetingCommand(drivetrain, driver)); driver.y().onTrue(Commands.runOnce(() -> { diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerLookAheadCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerLookAheadCommand.java new file mode 100644 index 00000000..3cc96f86 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerLookAheadCommand.java @@ -0,0 +1,75 @@ +package org.team1540.robot2024.commands.drivetrain; + +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.littletonrobotics.junction.Logger; +import org.team1540.robot2024.Constants; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.util.LoggedTunableNumber; + +import static org.team1540.robot2024.Constants.Targeting.*; + +public class DriveWithSpeakerLookAheadCommand extends Command { + private final Drivetrain drivetrain; + private final XboxController controller; + + private final PIDController rotController = new PIDController(ROT_KP, ROT_KI, ROT_KD); + + private final LoggedTunableNumber kP = new LoggedTunableNumber("Targeting/ROT_KP", ROT_KP); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Targeting/ROT_KI", ROT_KI); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/ROT_KD", ROT_KD); + + private boolean isFlipped; + private Pose2d speakerPose; + private Pose3d speakerPose3d; + + public DriveWithSpeakerLookAheadCommand(Drivetrain drivetrain, XboxController controller) { + this.drivetrain = drivetrain; + this.controller = controller; + rotController.enableContinuousInput(-Math.PI, Math.PI); + addRequirements(drivetrain); + } + + @Override + public void initialize() { + rotController.reset(); + isFlipped = DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red; + speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; + speakerPose3d = new Pose3d(speakerPose.getX(), speakerPose.getY(), SPEAKER_CENTER_HEIGHT, new Rotation3d()); + } + + @Override + public void execute() { +// if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { +// rotController.setPID(kP.get(), kI.get(), kD.get()); +// } + double flightDistance = speakerPose3d.getTranslation().getDistance(new Translation3d(drivetrain.getPose().getX(), drivetrain.getPose().getY(), Constants.Shooter.Pivot.PIVOT_HEIGHT)); + double NOTE_VELOCITY = 32; + double time = flightDistance/NOTE_VELOCITY; + Translation2d modifier = new Translation2d(drivetrain.getChassisSpeeds().vxMetersPerSecond * time,drivetrain.getChassisSpeeds().vyMetersPerSecond*time); + Pose2d drivetrainPose = drivetrain.getPose().plus(new Transform2d(modifier.getX(), modifier.getY(), new Rotation2d())); + Logger.recordOutput("Odometry/PredictedPosition", drivetrainPose); + Rotation2d targetRot = + drivetrainPose + .minus(speakerPose).getTranslation().getAngle() + .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0)); + Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); + Logger.recordOutput("Targeting/rotErrorDegrees", Math.abs(targetRot.minus(drivetrain.getRotation()).getDegrees())); + + double xPercent = MathUtil.applyDeadband((-controller.getLeftY()), 0.1); + double yPercent = MathUtil.applyDeadband((-controller.getLeftX()), 0.1); + double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians()); + drivetrain.drivePercent(xPercent, yPercent, rotPercent, true); + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java index 32b8cb21..d899a375 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java @@ -17,7 +17,6 @@ public StageTrampCommand(Tramp tramp, Indexer indexer) { @Override public void initialize() { tramp.setPercent(1); //TODO: Tune this -// indexer.setFeederVelocity(-600); TODO Make this work again indexer.setFeederPercent(-1); indexer.setIntakePercent(1); } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java index e0a901c6..00b71d25 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java @@ -15,17 +15,19 @@ public class AutoShootPrepare extends SequentialCommandGroup { public AutoShootPrepare(Drivetrain drivetrain, Shooter shooter) { this(drivetrain::getPose, shooter); } + public AutoShootPrepare(Supplier positionSupplier, Shooter shooter) { addCommands( - new PrepareShooterCommand(shooter, () -> new ShooterSetpoint( - Rotation2d.fromRadians( - Math.atan2(Constants.Targeting.SPEAKER_CENTER_HEIGHT - Constants.Shooter.Pivot.PIVOT_HEIGHT, - positionSupplier.get().getTranslation().getDistance( - AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation() - ) - ) - ).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), - 8000, 6000) + new PrepareShooterCommand(shooter, () -> //new ShooterSetpoint( +// Rotation2d.fromRadians( +// Math.atan2(Constants.Targeting.SPEAKER_CENTER_HEIGHT - Constants.Shooter.Pivot.PIVOT_HEIGHT, +// positionSupplier.get().getTranslation().getDistance( +// AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation() +// ) +// ) +// ).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), +// 8000, 6000) + shooter.lerp.get(positionSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation())) ) ); } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java new file mode 100644 index 00000000..9e19663d --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java @@ -0,0 +1,39 @@ +package org.team1540.robot2024.commands.shooter; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import org.team1540.robot2024.Constants; +import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerLookAheadCommand; +import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerTargetingCommand; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.vision.AprilTagsCrescendo; + +import java.util.function.Supplier; + +import static org.team1540.robot2024.Constants.Targeting.SPEAKER_CENTER_HEIGHT; + +public class AutoShootPrepareWhileMoving extends ParallelCommandGroup { + public AutoShootPrepareWhileMoving(XboxController controller, Drivetrain drivetrain, Shooter shooter) { + Pose3d speakerPose3d = new Pose3d( + AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getX(), + AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getY(), + SPEAKER_CENTER_HEIGHT, + new Rotation3d()); + Supplier modifiedPosition = ()->{ + double flightDistance = speakerPose3d.getTranslation().getDistance(new Translation3d(drivetrain.getPose().getX(), drivetrain.getPose().getY(), Constants.Shooter.Pivot.PIVOT_HEIGHT)); + double NOTE_VELOCITY = 32; + double time = flightDistance/NOTE_VELOCITY; + Translation2d modifier = new Translation2d(drivetrain.getChassisSpeeds().vxMetersPerSecond * time,drivetrain.getChassisSpeeds().vyMetersPerSecond*time); + Pose2d drivetrainPose = drivetrain.getPose().plus(new Transform2d(modifier.getX(), modifier.getY(), new Rotation2d())); + return drivetrainPose; + }; + + addCommands( + new DriveWithSpeakerLookAheadCommand(drivetrain,controller), + new AutoShootPrepare(modifiedPosition, shooter) + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 6b415d1d..024c1f5f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -271,7 +271,7 @@ public double getCharacterizationVelocity() { } return driveVelocityAverage / 4.0; } - + @AutoLogOutput(key = "Odometry/ChassisSpeeds") public ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index b330a857..3239e449 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -1,6 +1,7 @@ package org.team1540.robot2024.subsystems.shooter; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotState; @@ -14,6 +15,8 @@ import org.team1540.robot2024.util.LoggedTunableNumber; import org.team1540.robot2024.util.MechanismVisualiser; import org.team1540.robot2024.util.math.AverageFilter; +import org.team1540.robot2024.util.shooter.ShooterLerp; +import org.team1540.robot2024.util.shooter.ShooterSetpoint; import java.util.function.Supplier; @@ -44,6 +47,23 @@ public class Shooter extends SubsystemBase { private final LoggedTunableNumber pivotKI = new LoggedTunableNumber("Shooter/Pivot/kI", Pivot.KI); private final LoggedTunableNumber pivotKD = new LoggedTunableNumber("Shooter/Pivot/kD", Pivot.KD); + public final ShooterLerp lerp = new ShooterLerp().put( + new Pair<>(1.22, new ShooterSetpoint(Rotation2d.fromDegrees(52.5).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(1.39, new ShooterSetpoint(Rotation2d.fromDegrees(47.5).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(1.66, new ShooterSetpoint(Rotation2d.fromDegrees(43.5).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(2.00, new ShooterSetpoint(Rotation2d.fromDegrees(40.5).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(2.13, new ShooterSetpoint(Rotation2d.fromDegrees(38.5).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(2.51, new ShooterSetpoint(Rotation2d.fromDegrees(33.5).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(2.79, new ShooterSetpoint(Rotation2d.fromDegrees(31.5).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(2.94, new ShooterSetpoint(Rotation2d.fromDegrees(29.5).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(3.38, new ShooterSetpoint(Rotation2d.fromDegrees(26.5).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(3.75, new ShooterSetpoint(Rotation2d.fromDegrees(24.5).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(4.06, new ShooterSetpoint(Rotation2d.fromDegrees(23.7).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(4.42, new ShooterSetpoint(Rotation2d.fromDegrees(22.8).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(4.89, new ShooterSetpoint(Rotation2d.fromDegrees(22.0).minus(Pivot.REAL_ZEROED_ANGLE))), + new Pair<>(5.20, new ShooterSetpoint(Rotation2d.fromDegrees(21.5).minus(Pivot.REAL_ZEROED_ANGLE))) + ); + private static boolean hasInstance = false; private Shooter(ShooterPivotIO pivotIO, FlywheelsIO flywheelsIO) { diff --git a/src/main/java/org/team1540/robot2024/util/shooter/ShooterLerp.java b/src/main/java/org/team1540/robot2024/util/shooter/ShooterLerp.java index da9cd74b..c7dcaae2 100644 --- a/src/main/java/org/team1540/robot2024/util/shooter/ShooterLerp.java +++ b/src/main/java/org/team1540/robot2024/util/shooter/ShooterLerp.java @@ -19,12 +19,14 @@ public void put(Double key, ShooterSetpoint value){ right.put(key, value.rightSetpoint); } - public void put(Pair... dataPoints){ + public ShooterLerp put(Pair... dataPoints){ for (Pair point : dataPoints) { put(point.getFirst(), point.getSecond()); } + return this; } + public ShooterSetpoint get(Double key){ return new ShooterSetpoint(Rotation2d.fromDegrees(angle.get(key)), left.get(key), right.get(key)); } diff --git a/src/main/java/org/team1540/robot2024/util/shooter/ShooterSetpoint.java b/src/main/java/org/team1540/robot2024/util/shooter/ShooterSetpoint.java index d05019a8..0bd1fedc 100644 --- a/src/main/java/org/team1540/robot2024/util/shooter/ShooterSetpoint.java +++ b/src/main/java/org/team1540/robot2024/util/shooter/ShooterSetpoint.java @@ -13,8 +13,10 @@ public ShooterSetpoint(Rotation2d pivot, double leftSetpoint, double rightSetpoi this.rightSetpoint = rightSetpoint; } public ShooterSetpoint(double pivotRots, double leftSetpoint, double rightSetpoint) { - this.pivot = Rotation2d.fromRotations(pivotRots); - this.leftSetpoint = leftSetpoint; - this.rightSetpoint = rightSetpoint; + this(Rotation2d.fromRotations(pivotRots), leftSetpoint, rightSetpoint); + } + + public ShooterSetpoint(Rotation2d pivot){ + this(pivot, 8000, 6000); } } From 48a56ca50a893ee19637ca04abca48ae0aa7c562 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Thu, 14 Mar 2024 18:05:44 -0700 Subject: [PATCH 19/75] feat: limelight 3g pose estimation --- .../subsystems/vision/AprilTagVision.java | 6 +- .../subsystems/vision/AprilTagVisionIO.java | 4 +- .../vision/AprilTagVisionIOLimelight.java | 42 +- .../vision/AprilTagVisionIOPhoton.java | 11 +- .../vision/AprilTagVisionIOSim.java | 11 +- .../util/vision/EstimatedVisionPose.java | 18 +- .../util/vision/LimelightHelpers.java | 866 ++++++++++++++++++ .../util/vision/VisionPoseAcceptor.java | 4 +- vendordeps/photonlib-json-1.0.json | 10 +- 9 files changed, 906 insertions(+), 66 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java index 3bb7b5d6..90336062 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java @@ -50,7 +50,7 @@ public static AprilTagVision createReal(Consumer visionPose } return new AprilTagVision( new AprilTagVisionIOPhoton(FRONT_CAMERA_NAME, FRONT_CAMERA_POSE, drivetrainPoseSupplier), - new AprilTagVisionIOPhoton(REAR_CAMERA_NAME, REAR_CAMERA_POSE, drivetrainPoseSupplier), + new AprilTagVisionIOLimelight(REAR_CAMERA_NAME, REAR_CAMERA_POSE), visionPoseConsumer, elevatorHeightSupplierMeters); } @@ -109,8 +109,8 @@ private void updateAndAcceptPose(AprilTagVisionIOInputsAutoLogged cameraInputs, if (cameraInputs.lastMeasurementTimestampSecs > pose.timestampSecs) { pose.timestampSecs = cameraInputs.lastMeasurementTimestampSecs; pose.poseMeters = cameraInputs.estimatedPoseMeters; - pose.tagIDs = cameraInputs.seenTagIDs; - pose.tagPoses = cameraInputs.tagPosesMeters; + pose.numTags = cameraInputs.numTagsSeen; + pose.avgDistance = cameraInputs.avgTagDistance; visionPoseConsumer.accept(pose); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java index 75464950..6b7ab900 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java @@ -7,8 +7,8 @@ public interface AprilTagVisionIO { @AutoLog class AprilTagVisionIOInputs { public Pose3d estimatedPoseMeters = new Pose3d(); - public int[] seenTagIDs = new int[0]; - public Pose3d[] tagPosesMeters = new Pose3d[0]; + public int numTagsSeen = 0; + public double avgTagDistance = Double.POSITIVE_INFINITY; public double lastMeasurementTimestampSecs = 0.0; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java index c0fae798..fdedbfb1 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -5,59 +5,39 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Timer; +import org.team1540.robot2024.util.vision.LimelightHelpers; public class AprilTagVisionIOLimelight implements AprilTagVisionIO { private final String name; - private final NetworkTable table; public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) { this.name = name; - this.table = NetworkTableInstance.getDefault().getTable(name); - table.getEntry("camMode").setNumber(0); - table.getEntry("ledMode").setNumber(0); + LimelightHelpers.setCameraMode_Processor(name); + LimelightHelpers.setLEDMode_PipelineControl(name); setPoseOffset(cameraOffsetMeters); } @Override public void updateInputs(AprilTagVisionIOInputs inputs) { - double[] botPose = table.getEntry("botpose_wpiblue").getDoubleArray(new double[6]); - inputs.estimatedPoseMeters = new Pose3d( - botPose[0], - botPose[1], - botPose[2], - new Rotation3d( - Math.toRadians(botPose[3]), - Math.toRadians(botPose[4]), - Math.toRadians(botPose[5]))); + LimelightHelpers.PoseEstimate measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); - boolean hasTargets = table.getEntry("tv").getNumber(0).intValue() == 1; - inputs.seenTagIDs = hasTargets ? new int[]{table.getEntry("tid").getNumber(0).intValue()} : new int[0]; - double[] tagPose = table.getEntry("targetpose_robotspace").getDoubleArray(new double[6]); - inputs.tagPosesMeters = hasTargets - ? new Pose3d[] {new Pose3d( - tagPose[0], - tagPose[1], - tagPose[2], - new Rotation3d( - Math.toRadians(tagPose[3]), - Math.toRadians(tagPose[4]), - Math.toRadians(tagPose[5])))} - : new Pose3d[0]; + inputs.estimatedPoseMeters = new Pose3d(measurement.pose); + inputs.numTagsSeen = measurement.tagCount; + inputs.avgTagDistance = measurement.avgTagDist; - inputs.lastMeasurementTimestampSecs = Timer.getFPGATimestamp() - - (table.getEntry("cl").getDouble(0) + table.getEntry("tl").getDouble(0)) / 1000.0; + inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds; } @Override public void setPoseOffset(Pose3d poseOffsetMeters) { - table.getEntry("camerapose_robotspace_set").setDoubleArray(new double[]{ + LimelightHelpers.setCameraPose_RobotSpace( + name, poseOffsetMeters.getX(), poseOffsetMeters.getY(), poseOffsetMeters.getZ(), Math.toDegrees(poseOffsetMeters.getRotation().getX()), Math.toDegrees(poseOffsetMeters.getRotation().getY()), - Math.toDegrees(poseOffsetMeters.getRotation().getZ()) - }); + Math.toDegrees(poseOffsetMeters.getRotation().getZ())); } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java index e18261e3..a3a3bf73 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java @@ -52,11 +52,12 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { inputs.estimatedPoseMeters = lastEstimatedPose; inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds; - inputs.seenTagIDs = new int[targets.size()]; - inputs.tagPosesMeters = new Pose3d[targets.size()]; - for (int i = 0; i < targets.size(); i++) { - inputs.seenTagIDs[i] = targets.get(i).getFiducialId(); - inputs.tagPosesMeters[i] = new Pose3d().plus(targets.get(i).getBestCameraToTarget()); + inputs.numTagsSeen = targets.size(); + if (inputs.numTagsSeen > 0) { + inputs.avgTagDistance = 0; + for (PhotonTrackedTarget target : targets) + inputs.avgTagDistance += new Pose3d().plus(target.getBestCameraToTarget()).getTranslation().getNorm(); + inputs.avgTagDistance /= inputs.numTagsSeen; } } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java index d556831b..f144bc99 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java @@ -82,11 +82,12 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { inputs.estimatedPoseMeters = lastEstimatedPose; inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds; - inputs.seenTagIDs = new int[targets.size()]; - inputs.tagPosesMeters = new Pose3d[targets.size()]; - for (int i = 0; i < targets.size(); i++) { - inputs.seenTagIDs[i] = targets.get(i).getFiducialId(); - inputs.tagPosesMeters[i] = new Pose3d().plus(targets.get(i).getBestCameraToTarget()); + inputs.numTagsSeen = targets.size(); + if (inputs.numTagsSeen > 0) { + inputs.avgTagDistance = 0; + for (PhotonTrackedTarget target : targets) + inputs.avgTagDistance += new Pose3d().plus(target.getBestCameraToTarget()).getTranslation().getNorm(); + inputs.avgTagDistance /= inputs.numTagsSeen; } } } diff --git a/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java b/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java index 09ecb176..628c79f2 100644 --- a/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java +++ b/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java @@ -10,27 +10,19 @@ public class EstimatedVisionPose { public double timestampSecs = -1; public Pose3d poseMeters = new Pose3d(); - public int[] tagIDs = new int[0]; - public Pose3d[] tagPoses = new Pose3d[0]; - - public double getAvgTagDistance() { - double avgDistance = 0; - for (Pose3d pose : tagPoses) avgDistance += pose.getTranslation().getNorm(); - avgDistance /= tagPoses.length; - return avgDistance; - } + public int numTags = 0; + public double avgDistance = Double.POSITIVE_INFINITY; public Matrix getStdDevs() { - double avgDistance = getAvgTagDistance(); double xyStdDev = Constants.Vision.XY_STD_DEV_COEFF * Math.pow(avgDistance, 2.0) - / tagPoses.length; + / numTags; double rotStdDev = Constants.Vision.ROT_STD_DEV_COEFF * Math.pow(avgDistance, 2.0) - / tagPoses.length; - return VecBuilder.fill(xyStdDev, xyStdDev, tagIDs.length > 1 ? rotStdDev : Double.POSITIVE_INFINITY); + / numTags; + return VecBuilder.fill(xyStdDev, xyStdDev, numTags > 1 ? rotStdDev : Double.POSITIVE_INFINITY); } } diff --git a/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java b/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java new file mode 100644 index 00000000..a0664278 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java @@ -0,0 +1,866 @@ +//LimelightHelpers v1.3.1 (March 4, 2024) + +package org.team1540.robot2024.util.vision; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; + +public class LimelightHelpers { + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class Results { + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public String error; + + public LimelightResults() { + targetingResults = new Results(); + error = ""; + } + + + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int tagCount, double tagSpan, double avgTagDist, double avgTagArea) { + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + } + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + private static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + private static double extractBotPoseEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray,6); + int tagCount = (int)extractBotPoseEntry(poseArray,7); + double tagSpan = extractBotPoseEntry(poseArray,8); + double tagDist = extractBotPoseEntry(poseArray,9); + double tagArea = extractBotPoseEntry(poseArray,10); + //getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea); + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java b/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java index 8c70c605..9d7d2f25 100644 --- a/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java +++ b/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java @@ -24,10 +24,10 @@ public boolean shouldAcceptVision(EstimatedVisionPose visionPose) { // if (Timer.getFPGATimestamp() - visionPose.timestampSecs >= MAX_VISION_DELAY_SECS) return false; // Do not accept poses that see too little tags - if (visionPose.tagIDs.length < MIN_ACCEPTED_NUM_TAGS) return false; + if (visionPose.numTags < MIN_ACCEPTED_NUM_TAGS) return false; // Do not accept poses that have an average tag distance that is too far away - if (visionPose.getAvgTagDistance() > MAX_ACCEPTED_AVG_TAG_DIST_METERS) return false; + if (visionPose.avgDistance > MAX_ACCEPTED_AVG_TAG_DIST_METERS) return false; // Do not accept poses taken when the robot has too much rotational or translational velocity boolean rotatingTooFast = Math.abs(robotVelocity.omegaRadiansPerSecond) > MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC; diff --git a/vendordeps/photonlib-json-1.0.json b/vendordeps/photonlib-json-1.0.json index 5850529a..d2fb2a57 100644 --- a/vendordeps/photonlib-json-1.0.json +++ b/vendordeps/photonlib-json-1.0.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.9", + "version": "v2024.2.8", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.9", + "version": "v2024.2.8", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.9", + "version": "v2024.2.8", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.9" + "version": "v2024.2.8" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.9" + "version": "v2024.2.8" } ] } From e288e06d207cf06f5b71f5ba63db427cfc6cb80f Mon Sep 17 00:00:00 2001 From: Zach R Date: Thu, 14 Mar 2024 18:21:04 -0700 Subject: [PATCH 20/75] feat: fix pivot pid --- .../java/org/team1540/robot2024/Constants.java | 10 +++++----- .../org/team1540/robot2024/RobotContainer.java | 16 ++++++++++------ .../commands/shooter/TuneShooterCommand.java | 2 +- .../robot2024/subsystems/shooter/Shooter.java | 7 ++++--- .../subsystems/shooter/ShooterPivotIO.java | 2 +- .../subsystems/shooter/ShooterPivotIOSim.java | 2 +- .../shooter/ShooterPivotIOTalonFX.java | 6 ++++-- 7 files changed, 26 insertions(+), 19 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 7329a115..aaec69bf 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -174,11 +174,11 @@ public static class Pivot { public static final double PIVOT_HEIGHT = Units.inchesToMeters(10.5); // TODO: tune pid - public static final double KP = 80.0; - public static final double KI = 40.0; - public static final double KD = 0.0; + public static final double KP = 200.0; + public static final double KI = 0.0; + public static final double KD = 20.0; public static final double KS = 0.0; - public static final double KG = 0.0; + public static final double KG = 1.0; public static final double KV = 0.0; public static final double SIM_KP = 254; @@ -189,7 +189,7 @@ public static class Pivot { public static final double SIM_KV = 0.187; public static final double CRUISE_VELOCITY_RPS = 1.0; - public static final double MAX_ACCEL_RPS2 = 0.8; + public static final double MAX_ACCEL_RPS2 = 2; public static final double JERK_RPS3 = 2000; diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 9fb7fbeb..c225b358 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -1,5 +1,6 @@ package org.team1540.robot2024; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; @@ -63,9 +64,7 @@ public RobotContainer() { tramp = Tramp.createReal(); shooter = Shooter.createReal(); indexer = Indexer.createReal(); - aprilTagVision = AprilTagVision.createReal( - drivetrain::addVisionMeasurement, - elevator::getPosition); + aprilTagVision = AprilTagVision.createDummy(); break; case SIM: // Sim robot, instantiate physics sim IO implementations @@ -130,7 +129,7 @@ private void configureLedBindings() { } private void configureButtonBindings() { - ManualPivotCommand manualPivotCommand = new ManualPivotCommand(shooter, copilot); + Command manualPivotCommand = new ManualPivotCommand(shooter, copilot); drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); shooter.setDefaultCommand(manualPivotCommand); @@ -149,9 +148,14 @@ private void configureButtonBindings() { Command autoShooterCommand = new AutoShootPrepare(drivetrain, shooter) .alongWith(leds.commandShowPattern(new LedPatternWave("#00ffbc"), Leds.PatternLevel.DRIVER_LOCK)); - driver.rightBumper().toggleOnTrue(targetDrive); +// driver.rightBumper().toggleOnTrue(targetDrive); +// driver.leftBumper().toggleOnTrue(overstageTargetDrive); + //TODO: remove + driver.rightBumper().onTrue(shooter.setPivotPositionCommand(() -> Constants.Shooter.Pivot.MAX_ANGLE)); + driver.leftBumper().onTrue(shooter.setPivotPositionCommand(() -> Rotation2d.fromRotations(0.05))); + driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); - driver.leftBumper().toggleOnTrue(overstageTargetDrive); + driver.rightStick().onTrue(Commands.runOnce(() -> { targetDrive.cancel(); overstageTargetDrive.cancel(); diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java index 78b87068..ee8b9bd5 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java @@ -23,7 +23,7 @@ public TuneShooterCommand(Shooter shooter, Indexer indexer) { public void execute() { shooter.setFlywheelSpeeds(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()); shooter.setPivotPosition(Rotation2d.fromDegrees(angleSetpoint.get())); - indexer.setFeederPercent(0.5); + indexer.setFeederPercent(1); indexer.setIntakePercent(1); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index b330a857..adef342f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -43,6 +43,7 @@ public class Shooter extends SubsystemBase { private final LoggedTunableNumber pivotKP = new LoggedTunableNumber("Shooter/Pivot/kP", Pivot.KP); private final LoggedTunableNumber pivotKI = new LoggedTunableNumber("Shooter/Pivot/kI", Pivot.KI); private final LoggedTunableNumber pivotKD = new LoggedTunableNumber("Shooter/Pivot/kD", Pivot.KD); + private final LoggedTunableNumber pivotKG = new LoggedTunableNumber("Shooter/Pivot/kG", Pivot.KG); private static boolean hasInstance = false; @@ -90,11 +91,11 @@ public void periodic() { } // Update tunable numbers - if (flywheelsKP.hasChanged(hashCode()) || flywheelsKI.hasChanged(hashCode()) || flywheelsKD.hasChanged(hashCode())) { + if (Constants.isTuningMode() && (flywheelsKP.hasChanged(hashCode()) || flywheelsKI.hasChanged(hashCode()) || flywheelsKD.hasChanged(hashCode()))) { flywheelsIO.configPID(flywheelsKP.get(), flywheelsKI.get(), flywheelsKD.get()); } - if (pivotKP.hasChanged(hashCode()) || pivotKI.hasChanged(hashCode()) || pivotKD.hasChanged(hashCode())) { - pivotIO.configPID(pivotKP.get(), pivotKI.get(), pivotKD.get()); + if (Constants.isTuningMode() && (pivotKP.hasChanged(hashCode()) || pivotKI.hasChanged(hashCode()) || pivotKD.hasChanged(hashCode()) || pivotKG.hasChanged(hashCode()))) { + pivotIO.configPID(pivotKP.get(), pivotKI.get(), pivotKD.get(), pivotKG.get()); } // Add values to filters diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java index afe2d60b..65c13394 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java @@ -38,7 +38,7 @@ default void setBrakeMode(boolean isBrakeMode) {} /** * Configures the PID controller */ - default void configPID(double kP, double kI, double kD) {} + default void configPID(double kP, double kI, double kD, double kG) {} default void setEncoderPosition(double rots) {} } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java index 5f22feb9..9131b854 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java @@ -70,7 +70,7 @@ public void setVoltage(double volts) { } @Override - public void configPID(double kP, double kI, double kD) { + public void configPID(double kP, double kI, double kD, double kG) { controller.setPID(kP, kI, kD); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java index a25e026a..1cd8c33e 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java @@ -74,7 +74,7 @@ public ShooterPivotIOTalonFX() { cancoder.getConfigurator().apply(cancoderConfig); motor.getConfigurator().apply(motorConfig); - motor.setPosition(absolutePosition.getValue()*MOTOR_TO_CANCODER); +// motor.setPosition(absolutePosition.getValue()*MOTOR_TO_CANCODER); BaseStatusSignal.setUpdateFrequencyForAll( 50, position, @@ -119,13 +119,15 @@ public void setBrakeMode(boolean isBrakeMode) { } @Override - public void configPID(double kP, double kI, double kD) { + public void configPID(double kP, double kI, double kD, double kG) { Slot0Configs pidConfigs = new Slot0Configs(); motor.getConfigurator().refresh(pidConfigs); pidConfigs.kP = kP; pidConfigs.kI = kI; pidConfigs.kD = kD; + pidConfigs.kG = kG; motor.getConfigurator().apply(pidConfigs); + System.out.println(pidConfigs); } @Override public void setEncoderPosition(double rots) { From 677c90f87b2f55e3c37c6516b1ca6d8069f74994 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Thu, 14 Mar 2024 19:03:43 -0700 Subject: [PATCH 21/75] fix: better stddevs --- .../java/org/team1540/robot2024/Constants.java | 4 ++-- .../org/team1540/robot2024/RobotContainer.java | 4 ++-- .../vision/AprilTagVisionIOPhoton.java | 18 +++++++++--------- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index b5ae9929..f1cc3b86 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -113,8 +113,8 @@ public static class Vision { public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.03639, 0, 0.715274, new Rotation3d(Math.PI, 0, Math.PI)); // TODO i literally stole these from 6328 check if these actually work - public static final double XY_STD_DEV_COEFF = 0.005; - public static final double ROT_STD_DEV_COEFF = 0.01; + public static final double XY_STD_DEV_COEFF = 0.1; + public static final double ROT_STD_DEV_COEFF = 0.1; public static final double MAX_AMBIGUITY_RATIO = 0.3; public static final double MAX_VISION_DELAY_SECS = 0.08; diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index d23c5791..8fd92ec7 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -150,8 +150,8 @@ private void configureButtonBindings() { Command autoShooterCommand = new AutoShootPrepare(drivetrain, shooter) .alongWith(leds.commandShowPattern(new LedPatternWave("#00ffbc"), Leds.PatternLevel.DRIVER_LOCK)); -// driver.rightBumper().toggleOnTrue(targetDrive); -// driver.leftBumper().toggleOnTrue(overstageTargetDrive); + driver.rightBumper().toggleOnTrue(targetDrive); + driver.leftBumper().toggleOnTrue(overstageTargetDrive); driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java index a3a3bf73..15bb931a 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java @@ -31,14 +31,14 @@ public AprilTagVisionIOPhoton(String name, Pose3d cameraOffsetMeters, Supplier

targets = latestResult.getTargets(); Optional estimatedPose = photonEstimator.update(latestResult); @@ -51,14 +51,14 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { lastEstimatedPose = estimatedPose.get().estimatedPose; inputs.estimatedPoseMeters = lastEstimatedPose; inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds; + } - inputs.numTagsSeen = targets.size(); - if (inputs.numTagsSeen > 0) { - inputs.avgTagDistance = 0; - for (PhotonTrackedTarget target : targets) - inputs.avgTagDistance += new Pose3d().plus(target.getBestCameraToTarget()).getTranslation().getNorm(); - inputs.avgTagDistance /= inputs.numTagsSeen; - } + inputs.numTagsSeen = targets.size(); + if (inputs.numTagsSeen > 0) { + inputs.avgTagDistance = 0; + for (PhotonTrackedTarget target : targets) + inputs.avgTagDistance += new Pose3d().plus(target.getBestCameraToTarget()).getTranslation().getNorm(); + inputs.avgTagDistance /= inputs.numTagsSeen; } } From 83fea185e62439ec82a21935cfef4e61f0f4a46e Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Fri, 15 Mar 2024 10:39:39 -0700 Subject: [PATCH 22/75] feat: good shooter tuning command --- .../commands/shooter/TuneShooterCommand.java | 44 ++++++++++--------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java index ee8b9bd5..de420055 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java @@ -2,35 +2,37 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import org.team1540.robot2024.commands.indexer.IntakeAndFeed; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.shooter.ShooterSetpoint; -public class TuneShooterCommand extends Command { - private final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000); +public class TuneShooterCommand extends ParallelCommandGroup { + private final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 8000); private final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000); private final LoggedDashboardNumber angleSetpoint = new LoggedDashboardNumber("Shooter/Pivot/angleSetpoint", 15); - private final Shooter shooter; - private final Indexer indexer; + private int shotNum = 0; public TuneShooterCommand(Shooter shooter, Indexer indexer) { - this.shooter = shooter; - this.indexer = indexer; - addRequirements(shooter, indexer); - } - @Override - public void execute() { - shooter.setFlywheelSpeeds(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()); - shooter.setPivotPosition(Rotation2d.fromDegrees(angleSetpoint.get())); - indexer.setFeederPercent(1); - indexer.setIntakePercent(1); - } - - @Override - public void end(boolean interrupted) { - shooter.stopFlywheels(); - shooter.stopPivot(); - indexer.stopAll(); + addCommands( + new IntakeAndFeed(indexer, ()->1, ()->1), + new PrepareShooterCommand(shooter, ()-> + new ShooterSetpoint( + Rotation2d.fromDegrees(angleSetpoint.get()), leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get() + )), + Commands.sequence( + Commands.waitUntil(indexer::isNoteStaged), + Commands.waitUntil(()->!indexer.isNoteStaged()), + Commands.waitSeconds(1), + Commands.runOnce(()->shotNum += 1), + Commands.print("Shot Number: " + shotNum + " Angle Degrees Setpoint: " + angleSetpoint.get() + " Left RPM Setpoint: " + leftFlywheelSetpoint.get() + " Right RPM Setpoint: " + rightFlywheelSetpoint.get()) + ).repeatedly() + ); + addRequirements(shooter, indexer); } } From e44225a4785b062588f3662f14f52103215ac3a8 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 15 Mar 2024 12:02:55 -0700 Subject: [PATCH 23/75] fix: stop repeatedly getting encoders for indexer --- .../robot2024/subsystems/indexer/Indexer.java | 6 ----- .../subsystems/indexer/IndexerIOSparkMax.java | 25 +++++++------------ 2 files changed, 9 insertions(+), 22 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index e5ea8020..c5477449 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -58,12 +58,6 @@ public void periodic() { if (RobotState.isDisabled()){ stopAll(); } - -// if (Constants.isTuningMode()) { -// if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { -// io.configureFeederPID(kP.get(), kI.get(), kD.get()); -// } -// } } public void setIntakePercent(double percent) { diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java index b4b06712..4154c663 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -1,13 +1,7 @@ package org.team1540.robot2024.subsystems.indexer; -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkLowLevel; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkPIDController; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.wpilibj.AnalogPotentiometer; +import com.revrobotics.*; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import static org.team1540.robot2024.Constants.Indexer.*; @@ -16,9 +10,11 @@ public class IndexerIOSparkMax implements IndexerIO { private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless); private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless); private final DigitalInput indexerBeamBreak = new DigitalInput(7); -// private final Debouncer beamBreakDebouncer = new Debouncer(0.2, Debouncer.DebounceType.kRising); -// private final AnalogPotentiometer ultrasonic = new AnalogPotentiometer(0,1,0); - private final SparkPIDController feederPID; + + private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder(); + private final RelativeEncoder feederEncoder = feederMotor.getEncoder(); + + private final SparkPIDController feederPID = feederMotor.getPIDController(); private double setpointRPM; @@ -32,7 +28,6 @@ public IndexerIOSparkMax() { feederMotor.enableVoltageCompensation(12.0); feederMotor.setSmartCurrentLimit(60); - feederPID = feederMotor.getPIDController(); feederPID.setP(FEEDER_KP, 0); feederPID.setI(FEEDER_KI, 0); feederPID.setD(FEEDER_KD, 0); @@ -43,15 +38,13 @@ public IndexerIOSparkMax() { public void updateInputs(IndexerIOInputs inputs) { inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent(); inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); - inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity(); + inputs.intakeVelocityRPM = intakeEncoder.getVelocity(); inputs.feederCurrentAmps = feederMotor.getOutputCurrent(); inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput(); - inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity(); + inputs.feederVelocityRPM = feederEncoder.getVelocity(); inputs.noteInIntake = !indexerBeamBreak.get(); -// SmartDashboard.putNumber("ultrasonic", ultrasonic.get()); -// inputs.noteInIntake = ultrasonic.get() < 0.02; inputs.setpointRPM = setpointRPM; - inputs.feederVelocityError = setpointRPM - feederMotor.getEncoder().getVelocity(); + inputs.feederVelocityError = setpointRPM - feederEncoder.getVelocity(); } From dbcfe057807dfeeb02c98c20b0ac7e1b2dc2222b Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Fri, 15 Mar 2024 13:41:30 -0700 Subject: [PATCH 24/75] feat: updated logging for tuning --- .../robot2024/commands/shooter/TuneShooterCommand.java | 3 ++- .../org/team1540/robot2024/subsystems/shooter/Shooter.java | 6 +++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java index de420055..7417711a 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java @@ -30,7 +30,8 @@ public TuneShooterCommand(Shooter shooter, Indexer indexer) { Commands.waitUntil(()->!indexer.isNoteStaged()), Commands.waitSeconds(1), Commands.runOnce(()->shotNum += 1), - Commands.print("Shot Number: " + shotNum + " Angle Degrees Setpoint: " + angleSetpoint.get() + " Left RPM Setpoint: " + leftFlywheelSetpoint.get() + " Right RPM Setpoint: " + rightFlywheelSetpoint.get()) + Commands.print("Shot Number: " + shotNum + " Angle Degrees Setpoint: " + shooter.getPivotSetpoint() + " Left RPM Setpoint: " + shooter.getLeftFlywheelSetpointRPM() + " Right RPM Setpoint: " + shooter.getRightFlywheelSetpointRPM() + + " Angle Degrees: " + shooter.getPivotPosition().getDegrees() + " Left RPM: " + shooter.getLeftFlywheelSpeed() + " Right RPM: " + shooter.getRightFlywheelSpeed()) ).repeatedly() ); addRequirements(shooter, indexer); diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index adef342f..bd12cb8d 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -102,7 +102,6 @@ public void periodic() { leftSpeedFilter.add(getLeftFlywheelSpeed()); rightSpeedFilter.add(getRightFlywheelSpeed()); pivotPositionFilter.add(getPivotPosition().getRotations()); - Logger.recordOutput("Shooter/Pivot/Setpoint", pivotSetpoint); Logger.recordOutput("Shooter/Pivot/Error", pivotSetpoint.getDegrees() - pivotInputs.position.getDegrees()); } @@ -253,6 +252,11 @@ public double getRightFlywheelSetpointRPM() { return rightFlywheelSetpointRPM; } + @AutoLogOutput(key = "Pivot/PivotSetpoint") + public Rotation2d getPivotSetpoint(){ + return pivotSetpoint; + } + public void zeroPivot() { pivotIO.setEncoderPosition(0); } From 09fdfbc04e773c0367354a66c08ee32db7943d91 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 15 Mar 2024 14:45:16 -0700 Subject: [PATCH 25/75] feat: indexer feeder is a falcon --- .../org/team1540/robot2024/Constants.java | 3 +- .../robot2024/subsystems/indexer/Indexer.java | 2 +- .../indexer/IndexerIOTalonFXSparkMax.java | 90 +++++++++++++++++++ 3 files changed, 92 insertions(+), 3 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFXSparkMax.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index f1cc3b86..312553a7 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -112,9 +112,8 @@ public static class Vision { public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.0975290, 0, 0.665479, new Rotation3d(0, Math.toRadians(-25), 0)); public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.03639, 0, 0.715274, new Rotation3d(Math.PI, 0, Math.PI)); - // TODO i literally stole these from 6328 check if these actually work public static final double XY_STD_DEV_COEFF = 0.1; - public static final double ROT_STD_DEV_COEFF = 0.1; + public static final double ROT_STD_DEV_COEFF = 0.5; public static final double MAX_AMBIGUITY_RATIO = 0.3; public static final double MAX_VISION_DELAY_SECS = 0.08; diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index c5477449..59fc2579 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -33,7 +33,7 @@ public static Indexer createReal() { if (Constants.currentMode != Constants.Mode.REAL) { DriverStation.reportWarning("Using real indexer on simulated robot", false); } - return new Indexer(new IndexerIOSparkMax()); + return new Indexer(new IndexerIOTalonFXSparkMax()); } public static Indexer createSim() { diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFXSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFXSparkMax.java new file mode 100644 index 00000000..d7437bd2 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFXSparkMax.java @@ -0,0 +1,90 @@ +package org.team1540.robot2024.subsystems.indexer; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.wpilibj.DigitalInput; + +import static org.team1540.robot2024.Constants.Indexer.*; + +public class IndexerIOTalonFXSparkMax implements IndexerIO { + private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless); + private final TalonFX feederMotor = new TalonFX(FEEDER_ID); + private final DigitalInput indexerBeamBreak = new DigitalInput(7); + + private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder(); + + private final VoltageOut voltageCtrlReq = new VoltageOut(0).withEnableFOC(true); + private final VelocityVoltage velocityCtrlReq = new VelocityVoltage(0).withEnableFOC(true); + private final StatusSignal feederVoltage = feederMotor.getMotorVoltage(); + private final StatusSignal feederCurrent = feederMotor.getSupplyCurrent(); + private final StatusSignal feederVelocity = feederMotor.getVelocity(); + + public IndexerIOTalonFXSparkMax() { + intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); + intakeMotor.enableVoltageCompensation(12.0); + intakeMotor.setSmartCurrentLimit(55); + + TalonFXConfiguration feederConfig = new TalonFXConfiguration(); + feederConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + feederConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + feederConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + feederConfig.CurrentLimits.SupplyCurrentLimit = 60; + feederConfig.CurrentLimits.SupplyCurrentThreshold = 80; + feederConfig.CurrentLimits.SupplyTimeThreshold = 0.1; + feederConfig.Slot0.kP = FEEDER_KP; + feederConfig.Slot0.kI = FEEDER_KI; + feederConfig.Slot0.kD = FEEDER_KD; + feederConfig.Slot0.kV = FEEDER_KV; + + feederMotor.getConfigurator().apply(feederConfig); + } + + @Override + public void updateInputs(IndexerIOInputs inputs) { + BaseStatusSignal.refreshAll(feederVoltage, feederCurrent, feederVelocity); + inputs.intakeVoltage = intakeMotor.getAppliedOutput() * intakeMotor.getBusVoltage(); + inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent(); + inputs.intakeVelocityRPM = intakeEncoder.getVelocity(); + inputs.feederVoltage = feederVoltage.getValueAsDouble(); + inputs.feederCurrentAmps = feederCurrent.getValueAsDouble(); + inputs.feederVelocityRPM = feederVelocity.getValueAsDouble(); + inputs.feederVelocityError = inputs.feederVelocityRPM - velocityCtrlReq.Velocity; + inputs.noteInIntake = !indexerBeamBreak.get(); + } + + @Override + public void setIntakeVoltage(double volts) { + intakeMotor.setVoltage(volts); + } + + @Override + public void setFeederVoltage(double voltage) { + feederMotor.setControl(voltageCtrlReq.withOutput(voltage)); + } + + @Override + public void setFeederVelocity(double velocity) { + feederMotor.setControl(velocityCtrlReq.withVelocity(velocity)); + } + + @Override + public void configureFeederPID(double p, double i, double d) { + Slot0Configs pidConfigs = new Slot0Configs(); + feederMotor.getConfigurator().refresh(pidConfigs); + pidConfigs.kP = p; + pidConfigs.kI = i; + pidConfigs.kD = d; + feederMotor.getConfigurator().apply(pidConfigs); + } +} From f28f42431d6bb910e5a093640f43e05f7e0a6eea Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Fri, 15 Mar 2024 18:21:24 -0700 Subject: [PATCH 26/75] feat: l2 l3 handling --- src/main/java/org/team1540/robot2024/Constants.java | 7 ++++--- .../org/team1540/robot2024/subsystems/shooter/Shooter.java | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 312553a7..ef309a74 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -60,12 +60,13 @@ public static class SwerveConfig { } public static class Drivetrain { - public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0); + public static final boolean IS_L3 = false; + public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (IS_L3 ? 16.0 / 28.0 : 17.0 / 27.0) * (45.0 / 15.0); public static final double TURN_GEAR_RATIO = 150.0 / 7.0; public static final boolean IS_TURN_MOTOR_INVERTED = true; public static final double WHEEL_RADIUS = Units.inchesToMeters(1.967); - public static final double MAX_LINEAR_SPEED = Units.feetToMeters(16); + public static final double MAX_LINEAR_SPEED = Units.feetToMeters(IS_L3 ? 16.0 : 15.7); public static final double TRACK_WIDTH_X = Units.inchesToMeters(18.75); public static final double TRACK_WIDTH_Y = Units.inchesToMeters(19.75); public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); @@ -168,7 +169,7 @@ public static class Pivot { public static final Rotation2d MAX_ANGLE = Rotation2d.fromRotations(0.14); public static final Rotation2d MIN_ANGLE = Rotation2d.fromRotations(0.01); - public static final Rotation2d REAL_ZEROED_ANGLE = Rotation2d.fromDegrees(7.5); //TODO Need this number + public static final Rotation2d REAL_ZEROED_ANGLE = Rotation2d.fromDegrees(8.5); //TODO Need this number public static final double PIVOT_HEIGHT = Units.inchesToMeters(10.5); diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index bd12cb8d..5eaf21a5 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -252,7 +252,7 @@ public double getRightFlywheelSetpointRPM() { return rightFlywheelSetpointRPM; } - @AutoLogOutput(key = "Pivot/PivotSetpoint") + @AutoLogOutput(key = "Shooter/Pivot/PivotSetpoint") public Rotation2d getPivotSetpoint(){ return pivotSetpoint; } From 14f48139947df16b34fd8e62b1e8d2675caee460 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 15 Mar 2024 18:25:31 -0700 Subject: [PATCH 27/75] fix: correct module nums --- src/main/java/org/team1540/robot2024/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 312553a7..13ad4e8d 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -51,10 +51,10 @@ public static class SwerveConfig { public static final String CAN_BUS = IS_COMPETITION_ROBOT ? "swerve" : "swerve"; public static final double CAN_UPDATE_FREQUENCY_HZ = 200.0; - public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 2 : 1; - public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 9 : 7; - public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 5 : 4; - public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 6 : 3; + public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 1 : 1; + public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 7 : 9; + public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 4 : 5; + public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 3 : 6; public static final int PIGEON_ID = 9; } From 311ec62730c350f7813142ebdc33b24a4e08ad03 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 15 Mar 2024 21:33:58 -0700 Subject: [PATCH 28/75] feat: module change, shooter stuff --- .../org/team1540/robot2024/Constants.java | 6 +- .../java/org/team1540/robot2024/Robot.java | 1 + .../team1540/robot2024/RobotContainer.java | 2 + .../WheelRadiusCharacterization.java | 95 +++++++++++++++++++ .../commands/shooter/TuneShooterCommand.java | 27 ++++-- .../subsystems/drive/Drivetrain.java | 21 +++- .../robot2024/subsystems/drive/Module.java | 4 + .../subsystems/shooter/FlywheelsIO.java | 2 +- .../subsystems/shooter/FlywheelsIOSim.java | 6 +- .../shooter/FlywheelsIOTalonFX.java | 3 +- .../robot2024/subsystems/shooter/Shooter.java | 6 +- .../robot2024/util/swerve/SwerveFactory.java | 29 +++--- 12 files changed, 170 insertions(+), 32 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 6270dae2..65fdd421 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -142,7 +142,7 @@ public static class Flywheels { public static final double SIM_MOI = 4.08232288e-4; // TODO: if it's tuned in simulation, it's tuned in real life - public static final double KP = 0.53; + public static final double KP = 0.3; public static final double KI = 0.2; public static final double KD = 0.0; public static final double KS = 0.26925; @@ -174,11 +174,11 @@ public static class Pivot { public static final double PIVOT_HEIGHT = Units.inchesToMeters(10.5); // TODO: tune pid - public static final double KP = 200.0; + public static final double KP = 400.0; public static final double KI = 0.0; public static final double KD = 20.0; public static final double KS = 0.0; - public static final double KG = 1.0; + public static final double KG = 0.5; public static final double KV = 0.0; public static final double SIM_KP = 254; diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 8759fb73..97cc2206 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -88,6 +88,7 @@ public void robotInit() { robotContainer.shooter.setPivotBrakeMode(false); AprilTagsCrescendo.getInstance().getTag(1); + robotContainer.drivetrain.setBrakeMode(false); } /** diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 8fd92ec7..06fd07eb 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -153,6 +153,8 @@ private void configureButtonBindings() { driver.rightBumper().toggleOnTrue(targetDrive); driver.leftBumper().toggleOnTrue(overstageTargetDrive); + driver.a().whileTrue(new TuneShooterCommand(shooter, indexer, drivetrain::getPose)); + driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); driver.rightStick().onTrue(Commands.runOnce(() -> { diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java new file mode 100644 index 00000000..f59c9f14 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java @@ -0,0 +1,95 @@ +package org.team1540.robot2024.commands.drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; +import org.team1540.robot2024.Constants; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.util.LoggedTunableNumber; + +public class WheelRadiusCharacterization extends Command { + private static final LoggedTunableNumber characterizationSpeed = + new LoggedTunableNumber("WheelRadiusCharacterization/SpeedRadsPerSec", 0.1); + private static final double driveRadius = Constants.Drivetrain.DRIVE_BASE_RADIUS; + + public enum Direction { + CLOCKWISE(-1), + COUNTER_CLOCKWISE(1); + + Direction(int value) { + this.value = value; + } + + private final int value; + } + + private final Drivetrain drive; + private final Direction omegaDirection; + private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1.0); + + private double lastGyroYawRads = 0.0; + private double accumGyroYawRads = 0.0; + + private double[] startWheelPositions; + + private double currentEffectiveWheelRadius = 0.0; + + private final DoubleSupplier gyroYawRadsSupplier; + + public WheelRadiusCharacterization(Drivetrain drive, Direction omegaDirection) { + this.drive = drive; + this.omegaDirection = omegaDirection; + gyroYawRadsSupplier = () -> drive.getRawGyroRotation().getRadians(); + addRequirements(drive); + } + + @Override + public void initialize() { + // Reset + lastGyroYawRads = gyroYawRadsSupplier.getAsDouble(); + accumGyroYawRads = 0.0; + + startWheelPositions = drive.getWheelRadiusCharacterizationPosition(); + + omegaLimiter.reset(0); + } + + @Override + public void execute() { + // Run drive at velocity + drive.runWheelRadiusCharacterization( + omegaLimiter.calculate(omegaDirection.value * characterizationSpeed.get())); + + // Get yaw and wheel positions + accumGyroYawRads += MathUtil.angleModulus(gyroYawRadsSupplier.getAsDouble() - lastGyroYawRads); + lastGyroYawRads = gyroYawRadsSupplier.getAsDouble(); + double averageWheelPosition = 0.0; + double[] wheelPositions = drive.getWheelRadiusCharacterizationPosition(); + for (int i = 0; i < 4; i++) { + averageWheelPosition += Math.abs(wheelPositions[i] - startWheelPositions[i]); + } + averageWheelPosition /= 4.0; + + currentEffectiveWheelRadius = (accumGyroYawRads * driveRadius) / averageWheelPosition; + Logger.recordOutput("Drive/RadiusCharacterization/DrivePosition", averageWheelPosition); + Logger.recordOutput("Drive/RadiusCharacterization/AccumGyroYawRads", accumGyroYawRads); + Logger.recordOutput( + "Drive/RadiusCharacterization/CurrentWheelRadiusInches", + Units.metersToInches(currentEffectiveWheelRadius)); + } + + @Override + public void end(boolean interrupted) { + if (accumGyroYawRads <= Math.PI * 2.0) { + System.out.println("Not enough data for characterization"); + } else { + System.out.println( + "Effective Wheel Radius: " + + Units.metersToInches(currentEffectiveWheelRadius) + + " inches"); + } + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java index 7417711a..2f2fd4af 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java @@ -1,23 +1,34 @@ package org.team1540.robot2024.commands.shooter; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.util.shooter.ShooterSetpoint; +import org.team1540.robot2024.util.vision.AprilTagsCrescendo; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; public class TuneShooterCommand extends ParallelCommandGroup { private final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 8000); private final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000); private final LoggedDashboardNumber angleSetpoint = new LoggedDashboardNumber("Shooter/Pivot/angleSetpoint", 15); + private final DoubleSupplier shooterLeftSupplier; + private final DoubleSupplier shooterRightSupplier; + private final Supplier pivotRotationSupplier; + private int shotNum = 0; - public TuneShooterCommand(Shooter shooter, Indexer indexer) { + public TuneShooterCommand(Shooter shooter, Indexer indexer, Supplier poseSupplier) { + shooterLeftSupplier = shooter::getLeftFlywheelSetpointRPM; + shooterRightSupplier = shooter::getRightFlywheelSetpointRPM; + pivotRotationSupplier = shooter::getPivotPosition; addCommands( new IntakeAndFeed(indexer, ()->1, ()->1), @@ -30,9 +41,13 @@ public TuneShooterCommand(Shooter shooter, Indexer indexer) { Commands.waitUntil(()->!indexer.isNoteStaged()), Commands.waitSeconds(1), Commands.runOnce(()->shotNum += 1), - Commands.print("Shot Number: " + shotNum + " Angle Degrees Setpoint: " + shooter.getPivotSetpoint() + " Left RPM Setpoint: " + shooter.getLeftFlywheelSetpointRPM() + " Right RPM Setpoint: " + shooter.getRightFlywheelSetpointRPM() + - " Angle Degrees: " + shooter.getPivotPosition().getDegrees() + " Left RPM: " + shooter.getLeftFlywheelSpeed() + " Right RPM: " + shooter.getRightFlywheelSpeed()) - ).repeatedly() + Commands.print("Shot Number: " + shotNum + " Angle Degrees Setpoint: " + pivotRotationSupplier.get() + " Left RPM Setpoint: " + shooterLeftSupplier.getAsDouble() + " Right RPM Setpoint: " + shooterRightSupplier.getAsDouble() + + " Angle Degrees: " + shooter.getPivotPosition().getDegrees() + " Left RPM: " + shooter.getLeftFlywheelSpeed() + " Right RPM: " + shooter.getRightFlywheelSpeed() + + " Distance: " + poseSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation())) + ).repeatedly(), + Commands.runOnce( + () -> Logger.recordOutput("shooterTuning/Distance", + poseSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation()))).repeatedly() ); addRequirements(shooter, indexer); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index b1f8e8e7..fb027499 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -32,6 +32,7 @@ import org.team1540.robot2024.util.vision.EstimatedVisionPose; import org.team1540.robot2024.util.vision.VisionPoseAcceptor; +import java.util.Arrays; import java.util.function.Supplier; import static org.team1540.robot2024.Constants.Drivetrain.*; @@ -52,6 +53,8 @@ public class Drivetrain extends SubsystemBase { private static boolean hasInstance = false; private boolean blockTags = false; + private boolean isCharacterizingWheels = false; + private double characterizationInput = 0.0; private Drivetrain( GyroIO gyroIO, @@ -163,6 +166,10 @@ public void periodic() { Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{}); } + if (isCharacterizingWheels) { + runVelocity(new ChassisSpeeds(0, 0, characterizationInput)); + } + // Calculate module deltas SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; @@ -309,6 +316,10 @@ public Pose2d getVisionPose() { public Rotation2d getRotation() { return getPose().getRotation(); } + + public Rotation2d getRawGyroRotation() { + return rawGyroRotation; + } public void zeroFieldOrientationManual() { fieldOrientationOffset = rawGyroRotation; @@ -332,7 +343,6 @@ public void addVisionMeasurement(EstimatedVisionPose visionPose) { boolean shouldAccept = poseAcceptor.shouldAcceptVision(visionPose); if (shouldAccept) { Matrix stdDevs = visionPose.getStdDevs(); - System.out.println(stdDevs.toString()); visionPoseEstimator.setVisionMeasurementStdDevs(stdDevs); visionPoseEstimator.addVisionMeasurement(visionPose.poseMeters.toPose2d(), visionPose.timestampSecs); if (!blockTags) { @@ -396,4 +406,13 @@ public static Translation2d[] getModuleTranslations() { public Command commandStop() { return Commands.runOnce(this::stop); } + + public double[] getWheelRadiusCharacterizationPosition() { + return Arrays.stream(modules).mapToDouble(Module::getPositionRads).toArray(); + } + + public void runWheelRadiusCharacterization(double omegaSpeed) { + isCharacterizingWheels = true; + characterizationInput = omegaSpeed; + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java index 8cdd8ce7..57db7c03 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java @@ -145,6 +145,10 @@ public double getPositionMeters() { return inputs.drivePositionRad * WHEEL_RADIUS; } + public double getPositionRads() { + return inputs.drivePositionRad; + } + /** * Returns the current drive velocity of the module in meters per second. */ diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java index edbf485d..bfdd707f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java @@ -34,5 +34,5 @@ default void setSpeeds(double leftRPM, double rightRPM) {} /** * Configures the PID controller */ - default void configPID(double kP, double kI, double kD) {} + default void configPID(double kP, double kI, double kD, double kV) {} } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java index 24f27a87..1d5e15be 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java @@ -16,7 +16,7 @@ public class FlywheelsIOSim implements FlywheelsIO { private final PIDController rightController = new PIDController(KP, KI, KD); private final PIDController leftController = new PIDController(KP, KI, KD); - private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV); + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV); private boolean isClosedLoop; private double leftSetpointRPS; @@ -67,8 +67,10 @@ public void setSpeeds(double leftRPM, double rightRPM) { } @Override - public void configPID(double kP, double kI, double kD) { + public void configPID(double kP, double kI, double kD, double kV) { leftController.setPID(kP, kI, kD); rightController.setPID(kP, kI, kD); + + feedforward = new SimpleMotorFeedforward(KS, kV); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java index 5e26995d..82f950db 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java @@ -102,12 +102,13 @@ public void setVoltage(double leftVolts, double rightVolts) { } @Override - public void configPID(double kP, double kI, double kD) { + public void configPID(double kP, double kI, double kD, double kV) { Slot0Configs pidConfigs = new Slot0Configs(); leftMotor.getConfigurator().refresh(pidConfigs); pidConfigs.kP = kP; pidConfigs.kI = kI; pidConfigs.kD = kD; + pidConfigs.kV = kV; leftMotor.getConfigurator().apply(pidConfigs); rightMotor.getConfigurator().apply(pidConfigs); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index 5eaf21a5..1229b723 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.team1540.robot2024.Constants; @@ -39,6 +38,7 @@ public class Shooter extends SubsystemBase { private final LoggedTunableNumber flywheelsKP = new LoggedTunableNumber("Shooter/Flywheels/kP", Flywheels.KP); private final LoggedTunableNumber flywheelsKI = new LoggedTunableNumber("Shooter/Flywheels/kI", Flywheels.KI); private final LoggedTunableNumber flywheelsKD = new LoggedTunableNumber("Shooter/Flywheels/kD", Flywheels.KD); + private final LoggedTunableNumber flywheelsKV = new LoggedTunableNumber("Shooter/Flywheels/kV", Flywheels.KV); private final LoggedTunableNumber pivotKP = new LoggedTunableNumber("Shooter/Pivot/kP", Pivot.KP); private final LoggedTunableNumber pivotKI = new LoggedTunableNumber("Shooter/Pivot/kI", Pivot.KI); @@ -91,8 +91,8 @@ public void periodic() { } // Update tunable numbers - if (Constants.isTuningMode() && (flywheelsKP.hasChanged(hashCode()) || flywheelsKI.hasChanged(hashCode()) || flywheelsKD.hasChanged(hashCode()))) { - flywheelsIO.configPID(flywheelsKP.get(), flywheelsKI.get(), flywheelsKD.get()); + if (Constants.isTuningMode() && (flywheelsKP.hasChanged(hashCode()) || flywheelsKI.hasChanged(hashCode()) || flywheelsKD.hasChanged(hashCode()) || flywheelsKV.hasChanged(hashCode()))) { + flywheelsIO.configPID(flywheelsKP.get(), flywheelsKI.get(), flywheelsKD.get(), flywheelsKV.get()); } if (Constants.isTuningMode() && (pivotKP.hasChanged(hashCode()) || pivotKI.hasChanged(hashCode()) || pivotKD.hasChanged(hashCode()) || pivotKG.hasChanged(hashCode()))) { pivotIO.configPID(pivotKP.get(), pivotKI.get(), pivotKD.get(), pivotKG.get()); diff --git a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java index 43426e50..2df557a4 100644 --- a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java +++ b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java @@ -15,15 +15,15 @@ public class SwerveFactory { private static final double[] moduleOffsetsRots = new double[]{ - -0.9245605469, // Module 1 - 0.6655273438, // Module 2 - -0.7197265, // Module 3 - -0.7722, // Module 4 - -0.41162109375, // Module 5 - -0.594970703125, // Module 6 - -0.826660, // Module 7 - 0.0, // Module 8 - 0.2563476562 // Module 9 +// -0.7834472656 + 0.5, // Module 1 +// 0.6655273438, // Module 2 +// -0.7197265, // Module 3 +// -0.66479 - 0.25-0.5, // Module 4 +// -0.41162109375, // Module 5 +// -0.594970703125, // Module 6 +// -0.826660, // Module 7 +// 0.0, // Module 8 +// 0.2563476562 // Module 9 }; public static SwerveModuleHW getModuleMotors(int id, SwerveCorner corner) { @@ -59,10 +59,14 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { int driveID = 30 + id; int turnID = 20 + id; int canCoderID = 10 + id; + this.driveMotor = new TalonFX(driveID, canbus); + this.cancoder = new CANcoder(canCoderID, canbus); + this.turnMotor = new TalonFX(turnID, canbus); TalonFXConfiguration driveConfig = new TalonFXConfiguration(); TalonFXConfiguration turnConfig = new TalonFXConfiguration(); CANcoderConfiguration canCoderConfig = new CANcoderConfiguration(); + cancoder.getConfigurator().refresh(canCoderConfig); driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; driveConfig.CurrentLimits.SupplyCurrentThreshold = 60.0; @@ -78,17 +82,12 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { turnConfig.Feedback.SensorToMechanismRatio = 1.0; turnConfig.Feedback.RotorToSensorRatio = TURN_GEAR_RATIO; - canCoderConfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id - 1] + corner.offsetRots; +// canCoderConfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id - 1] + corner.offsetRots; canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; canCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; - this.driveMotor = new TalonFX(driveID, canbus); this.driveMotor.getConfigurator().apply(driveConfig); - - this.cancoder = new CANcoder(canCoderID, canbus); this.cancoder.getConfigurator().apply(canCoderConfig); - - this.turnMotor = new TalonFX(turnID, canbus); this.turnMotor.getConfigurator().apply(turnConfig); } } From 44231a764cec86c97cadb2b52b9f709bd9c4fd2e Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sat, 16 Mar 2024 00:38:39 -0700 Subject: [PATCH 29/75] feat: vision snapshots --- .../org/team1540/robot2024/Constants.java | 3 +++ .../team1540/robot2024/RobotContainer.java | 1 + .../subsystems/drive/Drivetrain.java | 3 --- .../subsystems/vision/AprilTagVision.java | 19 +++++++++++++++++++ .../subsystems/vision/AprilTagVisionIO.java | 2 ++ .../vision/AprilTagVisionIOLimelight.java | 5 +++++ .../vision/AprilTagVisionIOPhoton.java | 5 +++++ 7 files changed, 35 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 65fdd421..ab8aabc3 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -113,6 +113,9 @@ public static class Vision { public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.0975290, 0, 0.665479, new Rotation3d(0, Math.toRadians(-25), 0)); public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.03639, 0, 0.715274, new Rotation3d(Math.PI, 0, Math.PI)); + public static final boolean TAKE_SNAPSHOTS = false; + public static final double SNAPSHOT_PERIOD_SECS = 10; + public static final double XY_STD_DEV_COEFF = 0.1; public static final double ROT_STD_DEV_COEFF = 0.5; diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index f73b7e08..f0e2a976 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -153,6 +153,7 @@ private void configureButtonBindings() { driver.rightBumper().toggleOnTrue(targetDrive); driver.leftBumper().toggleOnTrue(overstageTargetDrive); + // TODO remove this driver.a().whileTrue(new TuneShooterCommand(shooter, indexer, drivetrain::getPose)); driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 82c2b1e8..2781778a 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -77,7 +77,6 @@ private Drivetrain( rawGyroRotation, getModulePositions(), new Pose2d(), - // TODO: tune std devs (scale vision by distance?) VecBuilder.fill(0.1, 0.1, 0.1), VecBuilder.fill(0.5, 0.5, 5.0)); // Trust the gyro more than the AprilTags @@ -86,7 +85,6 @@ private Drivetrain( rawGyroRotation, getModulePositions(), new Pose2d(), - // TODO: tune std devs (scale vision by distance?) VecBuilder.fill(0.1, 0.1, 0.1), VecBuilder.fill(0.5, 0.5, 5.0)); // Trust the gyro more than the AprilTags @@ -245,7 +243,6 @@ public void setBrakeMode(boolean enabled) { } } - /** * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will * return to their normal orientations the next time a nonzero velocity is requested. diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java index 90336062..1f76e4d9 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java @@ -3,6 +3,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; import org.team1540.robot2024.Constants; @@ -103,6 +105,23 @@ public void periodic() { updateAndAcceptPose(frontCameraInputs, frontPose); updateAndAcceptPose(rearCameraInputs, rearPose); + + if (TAKE_SNAPSHOTS && DriverStation.isFMSAttached() && RobotState.isEnabled()) { + Logger.runEveryN((int) (SNAPSHOT_PERIOD_SECS / Constants.LOOP_PERIOD_SECS), + () -> { + takeSnapshot( + String.format("%s_%s%d_%d", + DriverStation.getEventName(), + DriverStation.getMatchType().toString(), + DriverStation.getMatchNumber(), + (int) Timer.getFPGATimestamp())); + }); + } + } + + public void takeSnapshot(String snapshotName) { + frontCameraIO.takeSnapshot(snapshotName); + rearCameraIO.takeSnapshot(snapshotName); } private void updateAndAcceptPose(AprilTagVisionIOInputsAutoLogged cameraInputs, EstimatedVisionPose pose) { diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java index 6b7ab900..b4fa235e 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java @@ -17,6 +17,8 @@ default void updateInputs(AprilTagVisionIOInputs inputs) {} /** Sets the robot-space pose of the camera */ default void setPoseOffset(Pose3d newPose) {} + default void takeSnapshot(String snapshotName) {} + default String getName() { return ""; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java index fdedbfb1..c935f83b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -44,4 +44,9 @@ public void setPoseOffset(Pose3d poseOffsetMeters) { public String getName() { return name; } + + @Override + public void takeSnapshot(String snapshotName) { + LimelightHelpers.takeSnapshot(name, snapshotName); + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java index 15bb931a..0a82f912 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java @@ -72,4 +72,9 @@ public void setPoseOffset(Pose3d newPose) { public String getName() { return camera.getName(); } + + @Override + public void takeSnapshot(String snapshotName) { + camera.takeOutputSnapshot(); + } } From b2395552a860b164a6b342082e780bdd117f29eb Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Sat, 16 Mar 2024 00:59:48 -0700 Subject: [PATCH 30/75] feat: lerp setpoints and generalized yaw targeting --- .../team1540/robot2024/RobotContainer.java | 2 +- ...nd.java => DriveWithTargetingCommand.java} | 31 +++++++++---------- .../shooter/AutoShootPrepareWhileMoving.java | 29 ++++++++++------- .../robot2024/subsystems/shooter/Shooter.java | 22 +++++-------- 4 files changed, 40 insertions(+), 44 deletions(-) rename src/main/java/org/team1540/robot2024/commands/drivetrain/{DriveWithSpeakerLookAheadCommand.java => DriveWithTargetingCommand.java} (64%) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index f73b7e08..285cd922 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -9,7 +9,6 @@ import org.team1540.robot2024.commands.FeedForwardCharacterization; import org.team1540.robot2024.commands.autos.*; import org.team1540.robot2024.commands.climb.ClimbAlignment; -import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerLookAheadCommand; import org.team1540.robot2024.commands.drivetrain.SwerveDriveCommand; import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; @@ -134,6 +133,7 @@ private void configureLedBindings() { private void configureButtonBindings() { Command manualPivotCommand = new ManualPivotCommand(shooter, copilot); drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); +// drivetrain.setDefaultCommand(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter)); elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); shooter.setDefaultCommand(manualPivotCommand); driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerLookAheadCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java similarity index 64% rename from src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerLookAheadCommand.java rename to src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java index 3cc96f86..b3f1b7a5 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerLookAheadCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java @@ -12,10 +12,13 @@ import org.team1540.robot2024.Constants; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.util.LoggedTunableNumber; +import org.team1540.robot2024.util.vision.AprilTagsCrescendo; + +import java.util.function.Supplier; import static org.team1540.robot2024.Constants.Targeting.*; -public class DriveWithSpeakerLookAheadCommand extends Command { +public class DriveWithTargetingCommand extends Command { private final Drivetrain drivetrain; private final XboxController controller; @@ -26,12 +29,16 @@ public class DriveWithSpeakerLookAheadCommand extends Command { private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/ROT_KD", ROT_KD); private boolean isFlipped; - private Pose2d speakerPose; - private Pose3d speakerPose3d; - public DriveWithSpeakerLookAheadCommand(Drivetrain drivetrain, XboxController controller) { + private Supplier target; + + public DriveWithTargetingCommand(Drivetrain drivetrain, XboxController controller){ + this(drivetrain, controller, ()-> AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d()); + } + public DriveWithTargetingCommand(Drivetrain drivetrain, XboxController controller, Supplier target) { this.drivetrain = drivetrain; this.controller = controller; + this.target = target; rotController.enableContinuousInput(-Math.PI, Math.PI); addRequirements(drivetrain); } @@ -40,27 +47,17 @@ public DriveWithSpeakerLookAheadCommand(Drivetrain drivetrain, XboxController co public void initialize() { rotController.reset(); isFlipped = DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red; - speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; - speakerPose3d = new Pose3d(speakerPose.getX(), speakerPose.getY(), SPEAKER_CENTER_HEIGHT, new Rotation3d()); } @Override public void execute() { -// if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { -// rotController.setPID(kP.get(), kI.get(), kD.get()); -// } - double flightDistance = speakerPose3d.getTranslation().getDistance(new Translation3d(drivetrain.getPose().getX(), drivetrain.getPose().getY(), Constants.Shooter.Pivot.PIVOT_HEIGHT)); - double NOTE_VELOCITY = 32; - double time = flightDistance/NOTE_VELOCITY; - Translation2d modifier = new Translation2d(drivetrain.getChassisSpeeds().vxMetersPerSecond * time,drivetrain.getChassisSpeeds().vyMetersPerSecond*time); - Pose2d drivetrainPose = drivetrain.getPose().plus(new Transform2d(modifier.getX(), modifier.getY(), new Rotation2d())); - Logger.recordOutput("Odometry/PredictedPosition", drivetrainPose); Rotation2d targetRot = - drivetrainPose - .minus(speakerPose).getTranslation().getAngle() + drivetrain.getPose() + .minus(target.get()).getTranslation().getAngle() .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0)); Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); Logger.recordOutput("Targeting/rotErrorDegrees", Math.abs(targetRot.minus(drivetrain.getRotation()).getDegrees())); + Logger.recordOutput("Targeting/target", target.get()); double xPercent = MathUtil.applyDeadband((-controller.getLeftY()), 0.1); double yPercent = MathUtil.applyDeadband((-controller.getLeftX()), 0.1); diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java index 9e19663d..9a8ea9ec 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java @@ -5,8 +5,8 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import org.team1540.robot2024.Constants; -import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerLookAheadCommand; import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerTargetingCommand; +import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.util.vision.AprilTagsCrescendo; @@ -17,23 +17,28 @@ public class AutoShootPrepareWhileMoving extends ParallelCommandGroup { public AutoShootPrepareWhileMoving(XboxController controller, Drivetrain drivetrain, Shooter shooter) { - Pose3d speakerPose3d = new Pose3d( - AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getX(), - AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getY(), - SPEAKER_CENTER_HEIGHT, - new Rotation3d()); - Supplier modifiedPosition = ()->{ + Supplier modifiedPosition = ()->{ + Pose3d speakerPose3d = new Pose3d( + AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getX(), + AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getY(), + SPEAKER_CENTER_HEIGHT, + new Rotation3d()); double flightDistance = speakerPose3d.getTranslation().getDistance(new Translation3d(drivetrain.getPose().getX(), drivetrain.getPose().getY(), Constants.Shooter.Pivot.PIVOT_HEIGHT)); - double NOTE_VELOCITY = 32; +// double NOTE_VELOCITY = 6000.0*1.5*2.0*Math.PI*2.54/100.0/60.0; + double NOTE_VELOCITY = 24.0; double time = flightDistance/NOTE_VELOCITY; Translation2d modifier = new Translation2d(drivetrain.getChassisSpeeds().vxMetersPerSecond * time,drivetrain.getChassisSpeeds().vyMetersPerSecond*time); - Pose2d drivetrainPose = drivetrain.getPose().plus(new Transform2d(modifier.getX(), modifier.getY(), new Rotation2d())); - return drivetrainPose; + return modifier; + }; addCommands( - new DriveWithSpeakerLookAheadCommand(drivetrain,controller), - new AutoShootPrepare(modifiedPosition, shooter) + new DriveWithTargetingCommand(drivetrain,controller, ()-> + AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().plus(new Transform2d(-modifiedPosition.get().getX(), -modifiedPosition.get().getY(), new Rotation2d())) + ), + new AutoShootPrepare(()-> + drivetrain.getPose().plus(new Transform2d(modifiedPosition.get().getX(), modifiedPosition.get().getY(), new Rotation2d())), + shooter) ); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index d94f28a8..39e26b7b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -49,20 +49,14 @@ public class Shooter extends SubsystemBase { private final LoggedTunableNumber pivotKG = new LoggedTunableNumber("Shooter/Pivot/kG", Pivot.KG); public final ShooterLerp lerp = new ShooterLerp().put( - new Pair<>(1.22, new ShooterSetpoint(Rotation2d.fromDegrees(52.5).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(1.39, new ShooterSetpoint(Rotation2d.fromDegrees(47.5).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(1.66, new ShooterSetpoint(Rotation2d.fromDegrees(43.5).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(2.00, new ShooterSetpoint(Rotation2d.fromDegrees(40.5).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(2.13, new ShooterSetpoint(Rotation2d.fromDegrees(38.5).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(2.51, new ShooterSetpoint(Rotation2d.fromDegrees(33.5).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(2.79, new ShooterSetpoint(Rotation2d.fromDegrees(31.5).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(2.94, new ShooterSetpoint(Rotation2d.fromDegrees(29.5).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(3.38, new ShooterSetpoint(Rotation2d.fromDegrees(26.5).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(3.75, new ShooterSetpoint(Rotation2d.fromDegrees(24.5).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(4.06, new ShooterSetpoint(Rotation2d.fromDegrees(23.7).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(4.42, new ShooterSetpoint(Rotation2d.fromDegrees(22.8).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(4.89, new ShooterSetpoint(Rotation2d.fromDegrees(22.0).minus(Pivot.REAL_ZEROED_ANGLE))), - new Pair<>(5.20, new ShooterSetpoint(Rotation2d.fromDegrees(21.5).minus(Pivot.REAL_ZEROED_ANGLE))) + new Pair<>(1.2954, new ShooterSetpoint(Rotation2d.fromDegrees(40))), + new Pair<>(1.842, new ShooterSetpoint(Rotation2d.fromDegrees(37))), + new Pair<>(2.238, new ShooterSetpoint(Rotation2d.fromDegrees(31))), + new Pair<>(2.707, new ShooterSetpoint(Rotation2d.fromDegrees(27))), + new Pair<>(3.208, new ShooterSetpoint(Rotation2d.fromDegrees(23))), + new Pair<>(3.775, new ShooterSetpoint(Rotation2d.fromDegrees(20.625))), + new Pair<>(4.355, new ShooterSetpoint(Rotation2d.fromDegrees(15.5))), + new Pair<>(3.831, new ShooterSetpoint(Rotation2d.fromDegrees(22))) ); private static boolean hasInstance = false; From 771238d0b8e5e80cc3a919d616cbee3f7c05b047 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Sat, 16 Mar 2024 18:25:49 -0700 Subject: [PATCH 31/75] feat: software day --- paths.chor | 7918 ++++++++--------- src/main/deploy/choreo/AmpLanePABCSprint.traj | 850 -- .../deploy/choreo/AmpLanePADESprint.1.traj | 281 +- .../deploy/choreo/AmpLanePADESprint.2.traj | 78 +- .../deploy/choreo/AmpLanePADESprint.3.traj | 952 +- .../deploy/choreo/AmpLanePADESprint.4.traj | 1252 +-- .../deploy/choreo/AmpLanePADESprint.5.traj | 353 +- src/main/deploy/choreo/AmpLanePADESprint.traj | 2906 +++--- .../deploy/choreo/SourceLanePGHSprint.1.traj | 333 +- .../deploy/choreo/SourceLanePGHSprint.2.traj | 1487 ++-- .../deploy/choreo/SourceLanePGHSprint.3.traj | 1451 +-- .../deploy/choreo/SourceLanePGHSprint.4.traj | 680 +- .../deploy/choreo/SourceLanePGHSprint.traj | 3945 ++++---- .../org/team1540/robot2024/Constants.java | 6 +- .../java/org/team1540/robot2024/Robot.java | 1 + .../team1540/robot2024/RobotContainer.java | 49 +- .../commands/autos/AmpLanePADESprint.java | 12 +- .../commands/autos/CenterLanePCBAFSprint.java | 2 +- .../commands/autos/SourceLanePGHSprint.java | 5 +- .../drivetrain/DriveWithTargetingCommand.java | 11 +- .../WheelRadiusCharacterization.java | 8 +- .../shooter/AutoShootPrepareWhileMoving.java | 3 +- .../AutoShootPrepareWithTargeting.java | 4 +- .../shooter/OverStageShootPrepare.java | 2 +- .../OverStageShootPrepareWithTargeting.java | 7 +- .../commands/shooter/TuneShooterCommand.java | 9 +- .../subsystems/drive/Drivetrain.java | 15 + .../robot2024/subsystems/drive/Module.java | 3 +- .../shooter/FlywheelsIOTalonFX.java | 2 + .../robot2024/subsystems/shooter/Shooter.java | 20 +- .../vision/AprilTagVisionIOLimelight.java | 11 +- .../robot2024/util/auto/AutoCommand.java | 29 +- .../util/shooter/ShooterSetpoint.java | 2 +- 33 files changed, 10754 insertions(+), 11933 deletions(-) delete mode 100644 src/main/deploy/choreo/AmpLanePABCSprint.traj diff --git a/paths.chor b/paths.chor index d327cbf9..778dd1b6 100644 --- a/paths.chor +++ b/paths.chor @@ -5,12 +5,12 @@ "rotationalInertia": 3.6816072891155396, "motorMaxTorque": 0.5, "motorMaxVelocity": 4864, - "gearing": 6.1224489796, + "gearing": 6.746, "wheelbase": 0.4762497428251389, "trackWidth": 0.5016497291091463, "bumperLength": 0.8635995336562519, "bumperWidth": 0.8000995679462333, - "wheelRadius": 0.050799972568014815 + "wheelRadius": 0.0500888 }, "paths": { "SourceLanePHGFSprint": { @@ -1459,957 +1459,6 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "AmpLanePABCSprint": { - "waypoints": [ - { - "x": 1.468971848487854, - "y": 6.933434963226318, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 2.59665584564209, - "y": 6.861835956573486, - "heading": 0.49934697619601603, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 2.489257335662842, - "y": 5.573054313659668, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 2.560856342315674, - "y": 4.302172660827637, - "heading": -0.6696388746005083, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 2.776085138320923, - "y": 3.1934335231781006, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 5.041757106781006, - "y": 3.6918814182281494, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 - }, - { - "x": 7.896503925323486, - "y": 3.1254632472991943, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.468971848487854, - "y": 6.933434963226318, - "heading": 0, - "angularVelocity": 2.1788179528279398e-36, - "velocityX": -1.368271424490806e-35, - "velocityY": -2.8235582012814922e-36, - "timestamp": 0 - }, - { - "x": 1.4919852747959301, - "y": 6.931977994889289, - "heading": 0.010182405612263554, - "angularVelocity": 0.13209480038984944, - "velocityX": 0.29854968169707696, - "velocityY": -0.018901028792491784, - "timestamp": 0.07708407584713683 - }, - { - "x": 1.538012185703421, - "y": 6.9290637945517, - "heading": 0.030545200045879524, - "angularVelocity": 0.26416343725773955, - "velocityX": 0.5971001195988331, - "velocityY": -0.037805478051881794, - "timestamp": 0.15416815169427367 - }, - { - "x": 1.6070526656722113, - "y": 6.924691955570066, - "heading": 0.06108581336299875, - "angularVelocity": 0.39619873471251593, - "velocityX": 0.895651653211777, - "velocityY": -0.056715202635427786, - "timestamp": 0.2312522275414105 - }, - { - "x": 1.699106838032385, - "y": 6.918861907235003, - "heading": 0.10180106951516922, - "angularVelocity": 0.5281928297734501, - "velocityX": 1.1942047867671626, - "velocityY": -0.07563233094502542, - "timestamp": 0.30833630338854734 - }, - { - "x": 1.8141748726519586, - "y": 6.911572878632461, - "heading": 0.15268749584080604, - "angularVelocity": 0.6601418745234515, - "velocityX": 1.492760123994504, - "velocityY": -0.09455946020546946, - "timestamp": 0.3854203792356842 - }, - { - "x": 1.952256977521906, - "y": 6.902823843817773, - "heading": 0.2137424539083774, - "angularVelocity": 0.7920566913022042, - "velocityX": 1.7913181594571344, - "velocityY": -0.11349989889010634, - "timestamp": 0.462504455082821 - }, - { - "x": 2.1133533536109033, - "y": 6.892613440825621, - "heading": 0.2849668134211211, - "angularVelocity": 0.9239827906088756, - "velocityX": 2.089878802055858, - "velocityY": -0.132458006143837, - "timestamp": 0.5395885309299578 - }, - { - "x": 2.251439762891142, - "y": 6.8838222842371355, - "heading": 0.3461905263541886, - "angularVelocity": 0.7942459225233288, - "velocityX": 1.7913739999176204, - "velocityY": -0.11404633825952669, - "timestamp": 0.6166726067770947 - }, - { - "x": 2.3665117189435922, - "y": 6.876494714440304, - "heading": 0.39723045192442524, - "angularVelocity": 0.6621331968933817, - "velocityX": 1.492810996147195, - "velocityY": -0.09505944926110971, - "timestamp": 0.6937566826242315 - }, - { - "x": 2.4585692711434595, - "y": 6.8706316961774005, - "heading": 0.43807452893142784, - "angularVelocity": 0.5298640031437787, - "velocityX": 1.1942486329138104, - "velocityY": -0.07606004480784023, - "timestamp": 0.7708407584713683 - }, - { - "x": 2.527612473955509, - "y": 6.866233949603197, - "heading": 0.46871228827353184, - "angularVelocity": 0.3974589953294743, - "velocityX": 0.8956869762435377, - "velocityY": -0.057051297896133134, - "timestamp": 0.8479248343185052 - }, - { - "x": 2.5736413469679076, - "y": 6.863301955753749, - "heading": 0.48913694367966715, - "angularVelocity": 0.26496595025201874, - "velocityX": 0.5971255736875779, - "velocityY": -0.038036310576809694, - "timestamp": 0.925008910165642 - }, - { - "x": 2.59665584564209, - "y": 6.861835956573486, - "heading": 0.499346976196016, - "angularVelocity": 0.1324532000175497, - "velocityX": 0.298563593339587, - "velocityY": -0.019018184549158077, - "timestamp": 1.0020929860127787 - }, - { - "x": 2.59665584564209, - "y": 6.861835956573486, - "heading": 0.499346976196016, - "angularVelocity": 4.187102555037149e-35, - "velocityX": 0, - "velocityY": 1.1382387123849342e-35, - "timestamp": 1.0791770618599155 - }, - { - "x": 2.5947418201277674, - "y": 6.838819593279985, - "heading": 0.49061685118958936, - "angularVelocity": -0.11327050952346711, - "velocityX": -0.024833853477310732, - "velocityY": -0.2986297671239509, - "timestamp": 1.156250300456215 - }, - { - "x": 2.590913679072716, - "y": 6.792786870102318, - "heading": 0.47314863568765103, - "angularVelocity": -0.2266443686560877, - "velocityX": -0.0496688750177347, - "velocityY": -0.5972594900128827, - "timestamp": 1.2333235390525146 - }, - { - "x": 2.5851712843326613, - "y": 6.7237379431029165, - "heading": 0.4469236634346606, - "angularVelocity": -0.34026041633404946, - "velocityX": -0.07450568893481502, - "velocityY": -0.8958871880429407, - "timestamp": 1.3103967776488141 - }, - { - "x": 2.5775144084565245, - "y": 6.631673124734894, - "heading": 0.41191275130054616, - "angularVelocity": -0.45425510555612447, - "velocityX": -0.09934545395507093, - "velocityY": -1.1945108320963094, - "timestamp": 1.3874700162451137 - }, - { - "x": 2.567942678897023, - "y": 6.516592884025286, - "heading": 0.36807680590612885, - "angularVelocity": -0.568757018555102, - "velocityX": -0.12419005265416512, - "velocityY": -1.4931283906776436, - "timestamp": 1.4645432548414132 - }, - { - "x": 2.55645550871041, - "y": 6.378497838506786, - "heading": 0.31536820111597746, - "angularVelocity": -0.6838768650456329, - "velocityX": -0.14904226675592608, - "velocityY": -1.791737936974802, - "timestamp": 1.5416164934377128 - }, - { - "x": 2.543052018141666, - "y": 6.217388734848081, - "heading": 0.2537333559903418, - "angularVelocity": -0.7996919066612944, - "velocityX": -0.17390589539062137, - "velocityY": -2.0903377954905458, - "timestamp": 1.6186897320340123 - }, - { - "x": 2.529607207091661, - "y": 6.056304138753961, - "heading": 0.19035809858653865, - "angularVelocity": -0.8222731853238346, - "velocityX": -0.174442015086806, - "velocityY": -2.0900198178755933, - "timestamp": 1.695762970630312 - }, - { - "x": 2.518080937879921, - "y": 5.918232305257069, - "heading": 0.13599588472897933, - "angularVelocity": -0.7053319005096194, - "velocityX": -0.14954956378715376, - "velocityY": -1.7914367686051997, - "timestamp": 1.7728362092266114 - }, - { - "x": 2.5084742268750184, - "y": 5.803172814891468, - "heading": 0.09067284716302221, - "angularVelocity": -0.5880515518927888, - "velocityX": -0.12464392543852573, - "velocityY": -1.4928591617677907, - "timestamp": 1.849909447822911 - }, - { - "x": 2.5007879415658545, - "y": 5.711125391009445, - "heading": 0.054405158598584594, - "angularVelocity": -0.4705613676674917, - "velocityX": -0.09972703170582883, - "velocityY": -1.194285144343763, - "timestamp": 1.9269826864192106 - }, - { - "x": 2.495022759802516, - "y": 5.642089871721298, - "heading": 0.027201747413913846, - "angularVelocity": -0.35295534066187323, - "velocityX": -0.07480134309050894, - "velocityY": -0.8957132273855447, - "timestamp": 2.00405592501551 - }, - { - "x": 2.491179145461549, - "y": 5.596066182088173, - "heading": 0.009066533897654676, - "angularVelocity": -0.2352984491964748, - "velocityX": -0.049869635829104156, - "velocityY": -0.5971422827343685, - "timestamp": 2.0811291636118097 - }, - { - "x": 2.489257335662842, - "y": 5.573054313659668, - "heading": -1.290637644918987e-42, - "angularVelocity": -0.11763530458534513, - "velocityX": -0.024934852014888275, - "velocityY": -0.2985714477243978, - "timestamp": 2.158202402208109 - }, - { - "x": 2.489257335662842, - "y": 5.573054313659668, - "heading": 3.5400616344493244e-42, - "angularVelocity": 6.267668380261972e-41, - "velocityX": 0, - "velocityY": -1.030203639730239e-41, - "timestamp": 2.2352756408044088 - }, - { - "x": 2.490545501701153, - "y": 5.550368032283891, - "heading": -0.012391572665595225, - "angularVelocity": -0.16176043708854124, - "velocityX": 0.016815807567216828, - "velocityY": -0.29614826868975896, - "timestamp": 2.311880111518175 - }, - { - "x": 2.4931203515767737, - "y": 5.504995392935276, - "heading": -0.03717086071864737, - "angularVelocity": -0.32347052100444956, - "velocityX": 0.033612266381182004, - "velocityY": -0.592297537282776, - "timestamp": 2.3884845822319414 - }, - { - "x": 2.4969797328869174, - "y": 5.436936063650543, - "heading": -0.07432246767470774, - "angularVelocity": -0.48497961815933416, - "velocityX": 0.05038062758196422, - "velocityY": -0.8884511393471786, - "timestamp": 2.4650890529457077 - }, - { - "x": 2.5021209417107193, - "y": 5.346189326696005, - "heading": -0.12381236756993079, - "angularVelocity": -0.6460445380550001, - "velocityX": 0.06711369161484333, - "velocityY": -1.1846141107561945, - "timestamp": 2.541693523659474 - }, - { - "x": 2.5085409420107596, - "y": 5.23275393263894, - "heading": -0.18557968154848511, - "angularVelocity": -0.8063147412028813, - "velocityX": 0.08380712300759265, - "velocityY": -1.4807933923454437, - "timestamp": 2.6182979943732403 - }, - { - "x": 2.516236715901849, - "y": 5.096627982067514, - "heading": -0.2595292463430887, - "angularVelocity": -0.9653426765510463, - "velocityX": 0.10046115872068286, - "velocityY": -1.7769974689866566, - "timestamp": 2.6949024650870066 - }, - { - "x": 2.525205747324836, - "y": 4.937808900810427, - "heading": -0.3455278832614149, - "angularVelocity": -1.1226320881409273, - "velocityX": 0.11708234962551842, - "velocityY": -2.073235149036095, - "timestamp": 2.771506935800773 - }, - { - "x": 2.534115785608997, - "y": 4.778903807663307, - "heading": -0.42682973592121154, - "angularVelocity": -1.0613199451972188, - "velocityX": 0.11631224915649217, - "velocityY": -2.074357954131309, - "timestamp": 2.848111406514539 - }, - { - "x": 2.541754175790119, - "y": 4.6426966190718195, - "heading": -0.4963415581149959, - "angularVelocity": -0.9074120811240399, - "velocityX": 0.09971206784605312, - "velocityY": -1.7780579556567497, - "timestamp": 2.9247158772283055 - }, - { - "x": 2.5481206072451936, - "y": 4.529189096235521, - "heading": -0.5541628291197376, - "angularVelocity": -0.7548028263362259, - "velocityX": 0.08310783164161595, - "velocityY": -1.481734966362742, - "timestamp": 3.001320347942072 - }, - { - "x": 2.553214526426421, - "y": 4.438382432251887, - "heading": -0.6003661240439829, - "angularVelocity": -0.6031409719790967, - "velocityX": 0.06649636938633684, - "velocityY": -1.1853964022926788, - "timestamp": 3.077924818655838 - }, - { - "x": 2.5570353684728135, - "y": 4.370277332669397, - "heading": -0.6349992929465087, - "angularVelocity": -0.4521037555619042, - "velocityX": 0.04987753339709207, - "velocityY": -0.8890486279445305, - "timestamp": 3.1545292893696044 - }, - { - "x": 2.55958270709048, - "y": 4.3248740935696794, - "heading": -0.6580881830664621, - "angularVelocity": -0.3014039507723422, - "velocityX": 0.03325313253824122, - "velocityY": -0.5926969885265119, - "timestamp": 3.2311337600833707 - }, - { - "x": 2.560856342315674, - "y": 4.302172660827637, - "heading": -0.6696388746005083, - "angularVelocity": -0.1507835172858963, - "velocityX": 0.016626121338957477, - "velocityY": -0.2963460556612492, - "timestamp": 3.307738230797137 - }, - { - "x": 2.560856342315674, - "y": 4.302172660827637, - "heading": -0.6696388746005083, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -7.298514595392752e-42, - "timestamp": 3.3843427015109033 - }, - { - "x": 2.555942923489597, - "y": 4.287142223743098, - "heading": -0.6619594332481032, - "angularVelocity": 0.12026451741757689, - "velocityX": -0.07694699612537333, - "velocityY": -0.23538538542014806, - "timestamp": 3.4481972906890306 - }, - { - "x": 2.546450590215251, - "y": 4.256979387192444, - "heading": -0.646488848875746, - "angularVelocity": 0.2422783479070068, - "velocityX": -0.1486554591693719, - "velocityY": -0.47236756103015765, - "timestamp": 3.512051879867158 - }, - { - "x": 2.5328417294288803, - "y": 4.211556395130359, - "heading": -0.6230747892348256, - "angularVelocity": 0.3666777899957196, - "velocityX": -0.21312267389909825, - "velocityY": -0.7113504706039822, - "timestamp": 3.575906469045285 - }, - { - "x": 2.5157945553970475, - "y": 4.150712589378183, - "heading": -0.5915084780030003, - "angularVelocity": 0.49434679070236714, - "velocityX": -0.26696865881132753, - "velocityY": -0.9528493806834795, - "timestamp": 3.6397610582234123 - }, - { - "x": 2.4963909891627085, - "y": 4.074255034081668, - "heading": -0.5515032223856839, - "angularVelocity": 0.6265055672931927, - "velocityX": -0.30387113101943486, - "velocityY": -1.19736977843881, - "timestamp": 3.7036156474015396 - }, - { - "x": 2.4765840952198315, - "y": 3.9820247553374495, - "heading": -0.502689860958291, - "angularVelocity": 0.7644456264721134, - "velocityX": -0.3101874774830074, - "velocityY": -1.4443797999691932, - "timestamp": 3.767470236579667 - }, - { - "x": 2.460561114471436, - "y": 3.8744889188548193, - "heading": -0.4448360830574491, - "angularVelocity": 0.9060238057354711, - "velocityX": -0.2509291963917905, - "velocityY": -1.6840737348203896, - "timestamp": 3.831324825757794 - }, - { - "x": 2.4570633682910814, - "y": 3.757300143536982, - "heading": -0.37961365420873555, - "angularVelocity": 1.021421164683569, - "velocityX": -0.0547767392347868, - "velocityY": -1.8352443704700634, - "timestamp": 3.8951794149359213 - }, - { - "x": 2.4691364189829197, - "y": 3.643410857725524, - "heading": -0.313317924824764, - "angularVelocity": 1.038229675224039, - "velocityX": 0.18907099469640304, - "velocityY": -1.78357244604229, - "timestamp": 3.9590340041140486 - }, - { - "x": 2.4943273688840915, - "y": 3.5385198822209882, - "heading": -0.2494838956973126, - "angularVelocity": 0.999678017650094, - "velocityX": 0.39450492478934795, - "velocityY": -1.6426536738328135, - "timestamp": 4.022888593292176 - }, - { - "x": 2.530951098426381, - "y": 3.444665017333843, - "heading": -0.18957977093606787, - "angularVelocity": 0.9381334299111623, - "velocityX": 0.5735489024935178, - "velocityY": -1.469821763715844, - "timestamp": 4.086743182470304 - }, - { - "x": 2.577997037977533, - "y": 3.3627901789759114, - "heading": -0.13433219775233787, - "angularVelocity": 0.8652091242747275, - "velocityX": 0.7367667720782568, - "velocityY": -1.282207581502646, - "timestamp": 4.150597771648432 - }, - { - "x": 2.6348140958033937, - "y": 3.293422653958234, - "heading": -0.08415783264044516, - "angularVelocity": 0.7857597356382248, - "velocityX": 0.8897881664756364, - "velocityY": -1.0863357811945384, - "timestamp": 4.214452360826559 - }, - { - "x": 2.7009526723986657, - "y": 3.2368947239219907, - "heading": -0.03932053859858279, - "angularVelocity": 0.7021780990053088, - "velocityX": 1.035768571163667, - "velocityY": -0.8852602571532359, - "timestamp": 4.278306950004687 - }, - { - "x": 2.776085138320923, - "y": 3.1934335231781006, - "heading": 0, - "angularVelocity": 0.6157825005951433, - "velocityX": 1.1766181082563942, - "velocityY": -0.6806276777171288, - "timestamp": 4.342161539182815 - }, - { - "x": 2.850874204910404, - "y": 3.1650170711048373, - "heading": 0.030840918072749232, - "angularVelocity": 0.536085986843282, - "velocityX": 1.3000057415001567, - "velocityY": -0.4939431996267531, - "timestamp": 4.399691336322924 - }, - { - "x": 2.9329362221959228, - "y": 3.147223404249923, - "heading": 0.05711727907812475, - "angularVelocity": 0.4567435018305658, - "velocityX": 1.426426328006394, - "velocityY": -0.3092947957313142, - "timestamp": 4.457221133463033 - }, - { - "x": 3.0224739967279888, - "y": 3.13991100956747, - "heading": 0.07884780123756048, - "angularVelocity": 0.37772638249554086, - "velocityX": 1.5563721581357917, - "velocityY": -0.12710621357910618, - "timestamp": 4.514750930603142 - }, - { - "x": 3.119725791831946, - "y": 3.1429060034855505, - "heading": 0.09605037284161863, - "angularVelocity": 0.2990202027335969, - "velocityX": 1.6904595520667056, - "velocityY": 0.0520598727436279, - "timestamp": 4.572280727743252 - }, - { - "x": 3.2249748898290944, - "y": 3.1559904101117278, - "heading": 0.10874346264141094, - "angularVelocity": 0.22063505228219965, - "velocityX": 1.8294710433416355, - "velocityY": 0.22743703744185526, - "timestamp": 4.629810524883361 - }, - { - "x": 3.3385625417753837, - "y": 3.1788843955905044, - "heading": 0.11694851256012442, - "angularVelocity": 0.14262261170034615, - "velocityX": 1.9744142617026068, - "velocityY": 0.3979500470516161, - "timestamp": 4.68734032202347 - }, - { - "x": 3.4609055664038406, - "y": 3.2112183215831944, - "heading": 0.12069410858340832, - "angularVelocity": 0.06510706120102956, - "velocityX": 2.1266027469295685, - "velocityY": 0.5620378934058015, - "timestamp": 4.744870119163579 - }, - { - "x": 3.592519911370183, - "y": 3.252486932254075, - "heading": 0.12002359804350324, - "angularVelocity": -0.011655013110373149, - "velocityX": 2.2877595873631624, - "velocityY": 0.7173432329402094, - "timestamp": 4.802399916303688 - }, - { - "x": 3.7340501302900706, - "y": 3.301970950084958, - "heading": 0.11500982718595847, - "angularVelocity": -0.08715085237193064, - "velocityX": 2.4601202499498154, - "velocityY": 0.8601458772811008, - "timestamp": 4.8599297134437975 - }, - { - "x": 3.8862975149517003, - "y": 3.358597851211711, - "heading": 0.10578551382477244, - "angularVelocity": -0.16033975122006716, - "velocityX": 2.646409204100688, - "velocityY": 0.9843055936533752, - "timestamp": 4.917459510583907 - }, - { - "x": 4.050210856024716, - "y": 3.420693253911601, - "heading": 0.09260907243382549, - "angularVelocity": -0.22903681302502776, - "velocityX": 2.849190319128336, - "velocityY": 1.0793607102187723, - "timestamp": 4.974989307724016 - }, - { - "x": 4.226710679977497, - "y": 3.485583407764258, - "heading": 0.07600213210042343, - "angularVelocity": -0.2886667632941102, - "velocityX": 3.0679722983018776, - "velocityY": 1.127939903814055, - "timestamp": 5.032519104864125 - }, - { - "x": 4.41606032843016, - "y": 3.549260198670433, - "heading": 0.05695039663507196, - "angularVelocity": -0.3311629175217238, - "velocityX": 3.291331759636103, - "velocityY": 1.1068488691363714, - "timestamp": 5.090048902004234 - }, - { - "x": 4.616894975800281, - "y": 3.607020793660326, - "heading": 0.03693511887030335, - "angularVelocity": -0.3479114956032789, - "velocityX": 3.490967417823566, - "velocityY": 1.0040117966907118, - "timestamp": 5.1475786991443435 - }, - { - "x": 4.826412483110604, - "y": 3.6552133079506532, - "heading": 0.017545300992940664, - "angularVelocity": -0.337039566298841, - "velocityX": 3.6418954650589184, - "velocityY": 0.8376965796169683, - "timestamp": 5.205108496284453 - }, - { - "x": 5.041757106781006, - "y": 3.6918814182281494, - "heading": 0, - "angularVelocity": -0.3049776266412084, - "velocityX": 3.7431841302333626, - "velocityY": 0.6373759703722572, - "timestamp": 5.262638293424562 - }, - { - "x": 5.318468189453411, - "y": 3.7184056106005574, - "heading": -0.017910640554298394, - "angularVelocity": -0.247405457661866, - "velocityX": 3.822299478409622, - "velocityY": 0.3663872284808979, - "timestamp": 5.335032171569227 - }, - { - "x": 5.597577139842402, - "y": 3.724637925984956, - "heading": -0.03131839137135645, - "angularVelocity": -0.1852055886585485, - "velocityX": 3.8554219989602294, - "velocityY": 0.08608898354560332, - "timestamp": 5.407426049713893 - }, - { - "x": 5.874223781041164, - "y": 3.7105889006362736, - "heading": -0.03999959733056603, - "angularVelocity": -0.11991629930174207, - "velocityX": 3.8214093275392043, - "velocityY": -0.194063720700357, - "timestamp": 5.479819927858558 - }, - { - "x": 6.142135014447851, - "y": 3.6780687785591137, - "heading": -0.04416827770374048, - "angularVelocity": -0.05758332720957581, - "velocityX": 3.7007443208293824, - "velocityY": -0.4492109403529163, - "timestamp": 5.552213806003223 - }, - { - "x": 6.395319274879328, - "y": 3.631372287860468, - "heading": -0.04465854916399648, - "angularVelocity": -0.0067722778889713385, - "velocityX": 3.49731589079308, - "velocityY": -0.6450336947736285, - "timestamp": 5.624607684147889 - }, - { - "x": 6.630037177214525, - "y": 3.5758872476628394, - "heading": -0.042558245366794624, - "angularVelocity": 0.029012174109595102, - "velocityX": 3.2422341273961908, - "velocityY": -0.7664327650295508, - "timestamp": 5.697001562292554 - }, - { - "x": 6.844724227675687, - "y": 3.5162488607697617, - "heading": -0.03880262566507215, - "angularVelocity": 0.05187758686193831, - "velocityX": 2.965541506591899, - "velocityY": -0.8238042832005938, - "timestamp": 5.769395440437219 - }, - { - "x": 7.038965694034982, - "y": 3.4558518067523507, - "heading": -0.03408005700737659, - "angularVelocity": 0.06523436482099192, - "velocityX": 2.6831200556922665, - "velocityY": -0.8342839970075755, - "timestamp": 5.841789318581885 - }, - { - "x": 7.212812310142381, - "y": 3.3970955320884197, - "heading": -0.02888390227680435, - "angularVelocity": 0.07177616206979107, - "velocityX": 2.4013994078338827, - "velocityY": -0.8116193823256383, - "timestamp": 5.91418319672655 - }, - { - "x": 7.366475721480626, - "y": 3.3416998550485086, - "heading": -0.023574199348340368, - "angularVelocity": 0.07334463996877644, - "velocityX": 2.122602287325693, - "velocityY": -0.7651983628949022, - "timestamp": 5.9865770748712155 - }, - { - "x": 7.500208431967021, - "y": 3.2909325516518573, - "heading": -0.018421222254793455, - "angularVelocity": 0.07117973543632572, - "velocityX": 1.847293084909134, - "velocityY": -0.7012651442046296, - "timestamp": 6.058970953015881 - }, - { - "x": 7.614258897319107, - "y": 3.245756058228632, - "heading": -0.013633605820409669, - "angularVelocity": 0.0661328907510193, - "velocityX": 1.5754158814945374, - "velocityY": -0.6240374819117769, - "timestamp": 6.131364831160546 - }, - { - "x": 7.70885616888735, - "y": 3.206920861206511, - "heading": -0.009376453647745345, - "angularVelocity": 0.05880541672539207, - "velocityX": 1.3067026382977804, - "velocityY": -0.5364431084147118, - "timestamp": 6.203758709305212 - }, - { - "x": 7.784206086933648, - "y": 3.1750260064914886, - "heading": -0.005783242083132249, - "angularVelocity": 0.04963419085565119, - "velocityX": 1.0408327330623905, - "velocityY": -0.4405739205086726, - "timestamp": 6.276152587449877 - }, - { - "x": 7.840491875309718, - "y": 3.150559356787349, - "heading": -0.0029638574374597423, - "angularVelocity": 0.03894506991376383, - "velocityX": 0.7774937580162987, - "velocityY": -0.3379657276440889, - "timestamp": 6.348546465594542 - }, - { - "x": 7.877876256619461, - "y": 3.1339251307436706, - "heading": -0.0010101702732461582, - "angularVelocity": 0.026986911245582144, - "velocityX": 0.5164025228077691, - "velocityY": -0.22977393213329078, - "timestamp": 6.420940343739208 - }, - { - "x": 7.896503925323486, - "y": 3.1254632472991943, - "heading": 0, - "angularVelocity": 0.013953807961876554, - "velocityX": 0.25730999887588185, - "velocityY": -0.11688672663131537, - "timestamp": 6.493334221883873 - }, - { - "x": 7.896503925323486, - "y": 3.1254632472991943, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.3140323971906594e-42, - "velocityY": 0, - "timestamp": 6.565728100028538 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, "CenterLanePCBADEF": { "waypoints": [ { @@ -27132,7 +26181,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 20 + "controlIntervalCount": 19 }, { "x": 2.3639590740203857, @@ -27141,7 +26190,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 23 + "controlIntervalCount": 22 }, { "x": 5.5030035972595215, @@ -27153,13 +26202,22 @@ "controlIntervalCount": 17 }, { - "x": 8.084636688232422, - "y": 2.3980138301849365, - "heading": 0, + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 9 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { "x": 5.5030035972595215, @@ -27168,7 +26226,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 23 + "controlIntervalCount": 22 }, { "x": 2.3639590740203857, @@ -27177,25 +26235,34 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 23 + "controlIntervalCount": 21 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": 0, + "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 17 + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 8.063037872314453, - "y": 0.7318625450134277, - "heading": 0, + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 9 + }, + { + "x": 8.288774490356445, + "y": 0.7401233315467834, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { "x": 5.5030035972595215, @@ -27204,7 +26271,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 23 + "controlIntervalCount": 22 }, { "x": 2.37211537361145, @@ -27213,7 +26280,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 23 + "controlIntervalCount": 22 }, { "x": 5.5030035972595215, @@ -27222,7 +26289,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { "x": 8.063037872314453, @@ -27238,1991 +26305,2090 @@ { "x": 0.433241993188858, "y": 4.121134281158447, - "heading": 5.938255486131716e-34, + "heading": -6.349582694240188e-38, "angularVelocity": 0, - "velocityX": 2.0926991678208567e-34, - "velocityY": 4.74101269436352e-34, + "velocityX": -2.609211774957871e-36, + "velocityY": -5.016830702674688e-35, "timestamp": 0 }, { - "x": 0.45254673947106955, - "y": 4.110409875807789, - "heading": -0.00804589337615742, - "angularVelocity": -0.10676392342489109, - "velocityX": 0.2561617905500149, - "velocityY": -0.1423060856148132, - "timestamp": 0.0753615371002897 - }, - { - "x": 0.49115624056874074, - "y": 4.0889613071373185, - "heading": -0.024138137832086688, - "angularVelocity": -0.21353392028766638, - "velocityX": 0.5123236943308417, - "velocityY": -0.28460895963318283, - "timestamp": 0.1507230742005794 - }, - { - "x": 0.5490705891188512, - "y": 4.056788900712756, - "heading": -0.048273761664479664, - "angularVelocity": -0.32026448452442974, - "velocityX": 0.7684868273458795, - "velocityY": -0.4269075136000509, - "timestamp": 0.2260846113008691 - }, - { - "x": 0.6262899536624102, - "y": 4.013893070922723, - "heading": -0.08044715876351015, - "angularVelocity": -0.4269206592245423, - "velocityX": 1.024652196793663, - "velocityY": -0.5692005688916374, - "timestamp": 0.3014461484011588 - }, - { - "x": 0.722814567995576, - "y": 3.960274325008801, - "heading": -0.12065106662148206, - "angularVelocity": -0.5334804650344289, - "velocityX": 1.2808206685688042, - "velocityY": -0.7114868934078041, - "timestamp": 0.3768076855014485 - }, - { - "x": 0.8386447198033338, - "y": 3.895933264485532, - "heading": -0.16887761745268762, - "angularVelocity": -0.6399358702971596, - "velocityX": 1.5369929577420018, - "velocityY": -0.8537652362059028, - "timestamp": 0.4521692226017382 - }, - { - "x": 0.973780741976376, - "y": 3.8208705846111153, - "heading": -0.22511931827381823, - "angularVelocity": -0.7462918484038512, - "velocityX": 1.7931696641644363, - "velocityY": -0.99603435336682, - "timestamp": 0.5275307597020279 - }, - { - "x": 1.1282230140958218, - "y": 3.7350870801858624, - "heading": -0.289369793237253, - "angularVelocity": -0.8525632230395118, - "velocityX": 2.049351407388589, - "velocityY": -1.1382929240295867, - "timestamp": 0.6028922968023176 - }, - { - "x": 1.3019720073188241, - "y": 3.638583716526852, - "heading": -0.36162402966579443, - "angularVelocity": -0.9587680826147016, - "velocityX": 2.305539402570577, - "velocityY": -1.2805386855443919, - "timestamp": 0.6782538339026073 - }, - { - "x": 1.4950570361880926, - "y": 3.5314160759250663, - "heading": -0.44172046980509583, - "angularVelocity": -1.0628291728273882, - "velocityX": 2.56211638321966, - "velocityY": -1.4220469051629, - "timestamp": 0.753615371002897 - }, - { - "x": 1.668834943253154, - "y": 3.4349673750959555, - "heading": -0.5138143173751882, - "angularVelocity": -0.9566398237625026, - "velocityX": 2.30592307099311, - "velocityY": -1.279813344315926, - "timestamp": 0.8289769081031867 - }, - { - "x": 1.8233052433392802, - "y": 3.34923681430091, - "heading": -0.5779052248066122, - "angularVelocity": -0.8504458626704097, - "velocityX": 2.0497233208043544, - "velocityY": -1.1375903954952034, - "timestamp": 0.9043384452034764 - }, - { - "x": 1.958467560801758, - "y": 3.2742237758858077, - "heading": -0.6339913690664654, - "angularVelocity": -0.7442277110831049, - "velocityX": 1.7935185860474994, - "velocityY": -0.9953756425545938, - "timestamp": 0.9796999823037661 - }, - { - "x": 2.074321574453109, - "y": 3.2099277413643645, - "heading": -0.6820701443598719, - "angularVelocity": -0.6379749822435855, - "velocityX": 1.53730958933567, - "velocityY": -0.8531677695994906, - "timestamp": 1.0550615194040558 - }, - { - "x": 2.1708670101410656, - "y": 3.1563482775049274, - "heading": -0.7221386740225205, - "angularVelocity": -0.5316840818855123, - "velocityX": 1.281096954796391, - "velocityY": -0.7109656453547993, - "timestamp": 1.1304230565043456 - }, - { - "x": 2.2481036395899285, - "y": 3.1134850360999096, - "heading": -0.7541943378892257, - "angularVelocity": -0.4253584135902941, - "velocityX": 1.0248812911827636, - "velocityY": -0.5687681416048598, - "timestamp": 1.2057845936046352 - }, - { - "x": 2.306031279527139, - "y": 3.0813377577306427, - "heading": -0.7782353006154736, - "angularVelocity": -0.31900839143254983, - "velocityX": 0.7686631956580385, - "velocityY": -0.4265740801768146, - "timestamp": 1.2811461307049248 - }, - { - "x": 2.3446497902447807, - "y": 3.0599062760826214, - "heading": -0.7942609825639004, - "angularVelocity": -0.21265067785308137, - "velocityX": 0.5124432462974974, - "velocityY": -0.284382225637208, - "timestamp": 1.3565076678052144 + "x": 0.45469188032137553, + "y": 4.109218526997661, + "heading": -0.008937300145995164, + "angularVelocity": -0.11893237262991438, + "velocityX": 0.2854425752342375, + "velocityY": -0.1585678997982562, + "timestamp": 0.07514606787342622 + }, + { + "x": 0.4975916635116251, + "y": 4.0853873108981436, + "heading": -0.026812413296636577, + "angularVelocity": -0.2378715700833438, + "velocityX": 0.570885269240018, + "velocityY": -0.31713191087600345, + "timestamp": 0.15029213574685243 + }, + { + "x": 0.5619414537480725, + "y": 4.049641027140498, + "heading": -0.05362171709802473, + "angularVelocity": -0.35676256336585627, + "velocityX": 0.856329440215504, + "velocityY": -0.4756906750976657, + "timestamp": 0.22543820362027867 + }, + { + "x": 0.6477414523773106, + "y": 4.001980177860296, + "heading": -0.08935853104911506, + "angularVelocity": -0.4755646564406316, + "velocityX": 1.141776290593902, + "velocityY": -0.6342427571923069, + "timestamp": 0.30058427149370487 + }, + { + "x": 0.7549919358777414, + "y": 3.942405375853288, + "heading": -0.13401442200653152, + "angularVelocity": -0.5942545261667384, + "velocityX": 1.4272268201854585, + "velocityY": -0.7927866845588475, + "timestamp": 0.37573033936713107 + }, + { + "x": 0.8836932393269182, + "y": 3.8709173416225418, + "heading": -0.18758057158075755, + "angularVelocity": -0.7128270459134503, + "velocityX": 1.7126818087934743, + "velocityY": -0.9513210238912078, + "timestamp": 0.4508764072405573 + }, + { + "x": 1.0338457421402325, + "y": 3.7875168932975933, + "heading": -0.2500489995023069, + "angularVelocity": -0.8312933688928248, + "velocityX": 1.998141846440012, + "velocityY": -1.109844476033343, + "timestamp": 0.5260224751139835 + }, + { + "x": 1.2054498603190866, + "y": 3.692204931169301, + "heading": -0.32141344961465557, + "angularVelocity": -0.9496764385936038, + "velocityX": 2.283607419990343, + "velocityY": -1.2683559476303248, + "timestamp": 0.6011685429874097 + }, + { + "x": 1.3985060453231253, + "y": 3.5849824249792595, + "heading": -0.40166985798593763, + "angularVelocity": -1.0680054278616893, + "velocityX": 2.5690790013020615, + "velocityY": -1.4268545144723213, + "timestamp": 0.676314610860836 + }, + { + "x": 1.5915937569821006, + "y": 3.4778198154599647, + "heading": -0.48176859267038563, + "angularVelocity": -1.065907198489266, + "velocityX": 2.5694985396202825, + "velocityY": -1.4260574445464824, + "timestamp": 0.7514606787342623 + }, + { + "x": 1.7632284858900336, + "y": 3.382566036520044, + "heading": -0.5529765476804537, + "angularVelocity": -0.9475938931363465, + "velocityX": 2.2840147697019786, + "velocityY": -1.2675816797275796, + "timestamp": 0.8266067466076885 + }, + { + "x": 1.9134098098562766, + "y": 3.2992204007390877, + "heading": -0.6152918418017524, + "angularVelocity": -0.8292555536806094, + "velocityX": 1.9985253815170194, + "velocityY": -1.1091150626981792, + "timestamp": 0.9017528144811148 + }, + { + "x": 2.0421373577646693, + "y": 3.2277823059512354, + "heading": -0.6687115003742364, + "angularVelocity": -0.7108776291856339, + "velocityX": 1.7130310547348626, + "velocityY": -0.9506564589404816, + "timestamp": 0.976898882354541 + }, + { + "x": 2.149410808485792, + "y": 3.1682512394298143, + "heading": -0.713232041811307, + "angularVelocity": -0.5924533737687984, + "velocityX": 1.4275324545498613, + "velocityY": -0.792204678250007, + "timestamp": 1.0520449502279672 + }, + { + "x": 2.2352298936713018, + "y": 3.120626784362015, + "heading": -0.7488501306819736, + "angularVelocity": -0.4739847323836121, + "velocityX": 1.1420302833417786, + "velocityY": -0.6337584442610682, + "timestamp": 1.1271910181013933 + }, + { + "x": 2.2995943987852443, + "y": 3.0849086278583613, + "heading": -0.7755632624607879, + "angularVelocity": -0.3554827622359307, + "velocityX": 0.8565252572144717, + "velocityY": -0.4753163740226096, + "timestamp": 1.2023370859748195 + }, + { + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080857, + "angularVelocity": -0.23696688264902363, + "velocityX": 0.5710180813549187, + "velocityY": -0.3168769856057537, + "timestamp": 1.2774831538482456 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.1063066084363967, - "velocityX": 0.25622200022152375, - "velocityY": -0.14219129883360584, - "timestamp": 1.431869204905504 + "angularVelocity": -0.11846288599550474, + "velocityX": 0.2855094397331478, + "velocityY": -0.15843872456938407, + "timestamp": 1.3526292217216718 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": 1.280873526188021e-35, - "velocityY": 2.1400066509893793e-37, - "timestamp": 1.5072307420057935 - }, - { - "x": 2.374445038905671, - "y": 3.0395740293561597, - "heading": -0.8007582593302504, - "angularVelocity": 0.02507755763306408, - "velocityX": 0.17366966855318383, - "velocityY": -0.15926936399484595, - "timestamp": 1.5676095345297967 - }, - { - "x": 2.3955442849732433, - "y": 3.020481915034863, - "heading": -0.7977129883648791, - "angularVelocity": 0.050436102447073194, - "velocityX": 0.349447963193112, - "velocityY": -0.3162056331899431, - "timestamp": 1.6279883270537998 - }, - { - "x": 2.4273985359193944, - "y": 2.992075542224587, - "heading": -0.7931177380351067, - "angularVelocity": 0.07610702595527519, - "velocityX": 0.527573500803075, - "velocityY": -0.4704693754679464, - "timestamp": 1.688367119577803 - }, - { - "x": 2.470166324344177, - "y": 2.9545414012529414, - "heading": -0.7869514399590605, - "angularVelocity": 0.10212688625057974, - "velocityX": 0.7083246722395268, - "velocityY": -0.6216444450545164, - "timestamp": 1.748745912101806 - }, - { - "x": 2.524025883283138, - "y": 2.908097185821786, - "heading": -0.7791904398300458, - "angularVelocity": 0.12853851169563282, - "velocityX": 0.8920277582157603, - "velocityY": -0.7692140483381156, - "timestamp": 1.8091247046258092 - }, - { - "x": 2.589178616513917, - "y": 2.8529998964635936, - "heading": -0.7698080390828491, - "angularVelocity": 0.15539232162463004, - "velocityX": 1.0790665150330332, - "velocityY": -0.9125271813988101, - "timestamp": 1.8695034971498123 - }, - { - "x": 2.6658532194281834, - "y": 2.7895568383610723, - "heading": -0.758773944440501, - "angularVelocity": 0.18274785203698177, - "velocityX": 1.269892949313043, - "velocityY": -1.050750693255423, - "timestamp": 1.9298822896738155 - }, - { - "x": 2.7543104503881763, - "y": 2.7181408322396914, - "heading": -0.7460536194222877, - "angularVelocity": 0.21067537932555425, - "velocityX": 1.4650380913932193, - "velocityY": -1.1827995085027596, - "timestamp": 1.9902610821978186 - }, - { - "x": 2.8548483465105847, - "y": 2.6392116728155934, - "heading": -0.7316075530041294, - "angularVelocity": 0.23925729240801266, - "velocityX": 1.6651193559798465, - "velocityY": -1.3072331546332332, - "timestamp": 2.0506398747218215 - }, - { - "x": 2.9678071516801605, - "y": 2.5533470022723117, - "heading": -0.7153905179087263, - "angularVelocity": 0.2685882644797192, - "velocityX": 1.870835776066071, - "velocityY": -1.4220998293257756, - "timestamp": 2.1110186672458244 - }, - { - "x": 3.093571941086637, - "y": 2.4612875031660972, - "heading": -0.6973510365861332, - "angularVelocity": 0.2987718132226927, - "velocityX": 2.0829298525054067, - "velocityY": -1.5246992405424025, - "timestamp": 2.1713974597698273 - }, - { - "x": 3.2325678737819445, - "y": 2.364003698472316, - "heading": -0.6774316325200306, - "angularVelocity": 0.3299072941576924, - "velocityX": 2.302065458497713, - "velocityY": -1.611224746753705, - "timestamp": 2.2317762522938303 - }, - { - "x": 3.3852359851566844, - "y": 2.2627937385026136, - "heading": -0.6555713107834819, - "angularVelocity": 0.36205297957653465, - "velocityX": 2.528505539656955, - "velocityY": -1.6762501490811843, - "timestamp": 2.292155044817833 - }, - { - "x": 3.551962630271022, - "y": 2.159418364676261, - "heading": -0.6317136848324263, - "angularVelocity": 0.39513254495063227, - "velocityX": 2.761344474519871, - "velocityY": -1.7121139642740746, - "timestamp": 2.352533837341836 - }, - { - "x": 3.7329118909154024, - "y": 2.0562532945853267, - "heading": -0.6058279711502043, - "angularVelocity": 0.4287219502101063, - "velocityX": 2.9969009494922476, - "velocityY": -1.7086308913832926, - "timestamp": 2.412912629865839 - }, - { - "x": 3.927705999512227, - "y": 1.9563533988353388, - "heading": -0.5779537605481534, - "angularVelocity": 0.46165564823062205, - "velocityX": 3.2262007975629157, - "velocityY": -1.6545527257815482, - "timestamp": 2.473291422389842 - }, - { - "x": 4.135029249104847, - "y": 1.8631919103127867, - "heading": -0.5482681931936219, - "angularVelocity": 0.491655531910981, - "velocityX": 3.433709766723148, - "velocityY": -1.5429505067614013, - "timestamp": 2.533670214913845 - }, - { - "x": 4.35253951782844, - "y": 1.7799641253131642, - "heading": -0.5171193508091871, - "angularVelocity": 0.5158904489859046, - "velocityX": 3.602428263816689, - "velocityY": -1.3784274497794216, - "timestamp": 2.5940490074378477 - }, - { - "x": 4.577376652915107, - "y": 1.7089361045956586, - "heading": -0.4849529126600114, - "angularVelocity": 0.5327439785482329, - "velocityX": 3.7237766057889528, - "velocityY": -1.1763736528727222, - "timestamp": 2.6544277999618506 - }, - { - "x": 4.806831379756707, - "y": 1.6513703939365607, - "heading": -0.452194444563849, - "angularVelocity": 0.5425492416586429, - "velocityX": 3.8002536528099977, - "velocityY": -0.9534094381932716, - "timestamp": 2.7148065924858535 - }, - { - "x": 5.0386739792276956, - "y": 1.6078357362088507, - "heading": -0.4191901676167688, - "angularVelocity": 0.5466203540582497, - "velocityX": 3.8398018539178493, - "velocityY": -0.7210256434062197, - "timestamp": 2.7751853850098565 - }, - { - "x": 5.271173472263796, - "y": 1.578518825120004, - "heading": -0.38620445457775493, - "angularVelocity": 0.5463128966333781, - "velocityX": 3.8506813951880883, - "velocityY": -0.4855498075287272, - "timestamp": 2.8355641775338594 + "angularVelocity": 3.467177167133168e-32, + "velocityX": -1.439882477346126e-32, + "velocityY": -1.3815367235364851e-32, + "timestamp": 1.427775289595098 + }, + { + "x": 2.3773546785315864, + "y": 3.0407954088560687, + "heading": -0.7860770412156382, + "angularVelocity": 0.2650004205849919, + "velocityX": 0.21918861143707746, + "velocityY": -0.13736692695017716, + "timestamp": 1.4888897992558612 + }, + { + "x": 2.4041715283150715, + "y": 3.0239975454455843, + "heading": -0.75411979281041, + "angularVelocity": 0.5229077118120985, + "velocityX": 0.438796775632191, + "velocityY": -0.2748588429118795, + "timestamp": 1.5500043089166244 + }, + { + "x": 2.4444388170293236, + "y": 2.998788446329585, + "heading": -0.7069088657797715, + "angularVelocity": 0.772499481591177, + "velocityX": 0.6588826276733534, + "velocityY": -0.41248959135778174, + "timestamp": 1.6111188185773877 + }, + { + "x": 2.498190798561039, + "y": 2.965157152573787, + "heading": -0.6450804787000075, + "angularVelocity": 1.0116809808826641, + "velocityX": 0.8795289666903056, + "velocityY": -0.5502996578468837, + "timestamp": 1.672233328238151 + }, + { + "x": 2.5654677170302604, + "y": 2.923088008476279, + "heading": -0.5694581806203519, + "angularVelocity": 1.237386972413308, + "velocityX": 1.1008338092322851, + "velocityY": -0.6883658943027919, + "timestamp": 1.7333478378989142 + }, + { + "x": 2.6463157759141644, + "y": 2.8725583813996622, + "heading": -0.4811095285927534, + "angularVelocity": 1.445624819997864, + "velocityX": 1.3228946666295571, + "velocityY": -0.8268024624119354, + "timestamp": 1.7944623475596775 + }, + { + "x": 2.7407862870930626, + "y": 2.813537598490058, + "heading": -0.38139476987658943, + "angularVelocity": 1.6316053138553275, + "velocityX": 1.5457951262848801, + "velocityY": -0.9657409220366661, + "timestamp": 1.8555768572204407 + }, + { + "x": 2.84893500974016, + "y": 2.7459886359527177, + "heading": -0.27202745092011676, + "angularVelocity": 1.7895475160244771, + "velocityX": 1.7696079580350594, + "velocityY": -1.105285191884772, + "timestamp": 1.916691366881204 + }, + { + "x": 2.970822636317052, + "y": 2.669873093439706, + "heading": -0.15523119808259794, + "angularVelocity": 1.9111051284848073, + "velocityX": 1.9944138839282306, + "velocityY": -1.245457796119401, + "timestamp": 1.9778058765419673 + }, + { + "x": 3.106512263881207, + "y": 2.5851587340080315, + "heading": -0.03419676462946541, + "angularVelocity": 1.980453318286855, + "velocityX": 2.220252249708728, + "velocityY": -1.3861578846318265, + "timestamp": 2.0389203862027303 + }, + { + "x": 3.256035657144846, + "y": 2.4918371927335334, + "heading": 0.08572826850512172, + "angularVelocity": 1.962300504418209, + "velocityX": 2.4466103727841246, + "velocityY": -1.5269948461095477, + "timestamp": 2.1000348958634936 + }, + { + "x": 3.419180860242083, + "y": 2.390029027608911, + "heading": 0.19415018142326176, + "angularVelocity": 1.7740780956923756, + "velocityX": 2.6695003200194134, + "velocityY": -1.6658591501387068, + "timestamp": 2.161149405524257 + }, + { + "x": 3.5929644641848295, + "y": 2.279455345020229, + "heading": 0.26705766200125053, + "angularVelocity": 1.1929651564364412, + "velocityX": 2.8435735622750062, + "velocityY": -1.8092869140644157, + "timestamp": 2.22226391518502 + }, + { + "x": 3.7768655464749616, + "y": 2.1600020280366277, + "heading": 0.30152237044830693, + "angularVelocity": 0.5639365944088304, + "velocityX": 3.0091230922237155, + "velocityY": -1.9545819421061743, + "timestamp": 2.2833784248457833 + }, + { + "x": 3.9728967066647076, + "y": 2.0375522576737963, + "heading": 0.30152241765502197, + "angularVelocity": 7.724305624896446e-7, + "velocityX": 3.2076042379769354, + "velocityY": -2.003612088889046, + "timestamp": 2.3444929345065466 + }, + { + "x": 4.174386727718979, + "y": 1.9243094504352285, + "heading": 0.30152246485493484, + "angularVelocity": 7.723192600121745e-7, + "velocityX": 3.2969260847008353, + "velocityY": -1.85296107040964, + "timestamp": 2.40560744416731 + }, + { + "x": 4.383354452565466, + "y": 1.8255438101811146, + "heading": 0.30152251288342946, + "angularVelocity": 7.858771165145409e-7, + "velocityX": 3.419281705873656, + "velocityY": -1.6160751481496927, + "timestamp": 2.466721953828073 + }, + { + "x": 4.598761540028333, + "y": 1.7417467464566943, + "heading": 0.30152256294147634, + "angularVelocity": 8.190861247953373e-7, + "velocityX": 3.5246472344874826, + "velocityY": -1.3711484259558735, + "timestamp": 2.5278364634888364 + }, + { + "x": 4.819537390717514, + "y": 1.6733348748555603, + "heading": 0.30152261646908723, + "angularVelocity": 8.758576510857166e-7, + "velocityX": 3.612494838208991, + "velocityY": -1.1194047367945392, + "timestamp": 2.5889509731495997 + }, + { + "x": 5.04458467733647, + "y": 1.620648272623701, + "heading": 0.30152267534909943, + "angularVelocity": 9.634375304093619e-7, + "velocityX": 3.682387175617667, + "velocityY": -0.8620964567058491, + "timestamp": 2.650065482810363 + }, + { + "x": 5.27278482830379, + "y": 1.5839488347993986, + "heading": 0.301522742248914, + "angularVelocity": 0.000001094663361600596, + "velocityX": 3.7339766322927477, + "velocityY": -0.6005028597630109, + "timestamp": 2.711179992471126 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.35343685387939117, - "angularVelocity": 0.5427004967901151, - "velocityX": 3.839595250328378, - "velocityY": -0.2500851718896869, - "timestamp": 2.8959429700578623 - }, - { - "x": 5.773415110681756, - "y": 1.565214819314658, - "heading": -0.31539772557234425, - "angularVelocity": 0.5354796131957132, - "velocityX": 3.8066028075669824, - "velocityY": 0.025280100286796007, - "timestamp": 2.9669804612520303 - }, - { - "x": 6.038838987490814, - "y": 1.5860691706810086, - "heading": -0.2782614638760959, - "angularVelocity": 0.5227698933615667, - "velocityX": 3.7363914793045105, - "velocityY": 0.2935682414423857, - "timestamp": 3.0380179524461983 - }, - { - "x": 6.296044587353018, - "y": 1.6248260837737782, - "heading": -0.24248714943667204, - "angularVelocity": 0.5035976614326285, - "velocityX": 3.620702188921338, - "velocityY": 0.5455839225351394, - "timestamp": 3.1090554436403663 - }, - { - "x": 6.541409154187439, - "y": 1.6793226331219655, - "heading": -0.2085611266677552, - "angularVelocity": 0.4775791233418755, - "velocityX": 3.4540150941389784, - "velocityY": 0.7671519423346602, - "timestamp": 3.1800929348345344 - }, - { - "x": 6.77147442693619, - "y": 1.7462193726308344, - "heading": -0.17691445924041233, - "angularVelocity": 0.44549247017807186, - "velocityX": 3.238645803522406, - "velocityY": 0.9417103332945568, - "timestamp": 3.2511304260287024 - }, - { - "x": 6.983655507583721, - "y": 1.8213558866056687, - "heading": -0.14784470725521442, - "angularVelocity": 0.40921704154417726, - "velocityX": 2.986888712997661, - "velocityY": 1.0577022458389254, - "timestamp": 3.3221679172228704 - }, - { - "x": 7.176562959829929, - "y": 1.900519098632156, - "heading": -0.12149718500073287, - "angularVelocity": 0.37089601295836144, - "velocityX": 2.7155724252554383, - "velocityY": 1.114386371136183, - "timestamp": 3.3932054084170384 - }, - { - "x": 7.34978445026805, - "y": 1.9800560342439697, - "heading": -0.09790193374425028, - "angularVelocity": 0.33215209123854533, - "velocityX": 2.4384516897514468, - "velocityY": 1.119647305595492, - "timestamp": 3.4642428996112065 - }, - { - "x": 7.503478866038821, - "y": 2.0570663546440358, - "heading": -0.07702399456491762, - "angularVelocity": 0.2939002888244837, - "velocityX": 2.163567620239804, - "velocityY": 1.0840799570127342, - "timestamp": 3.5352803908053745 - }, - { - "x": 7.6380640040828975, - "y": 2.1293280640018195, - "heading": -0.058799951231258585, - "angularVelocity": 0.2565412013755806, - "velocityX": 1.8945649090592562, - "velocityY": 1.0172334093312752, - "timestamp": 3.6063178819995425 - }, - { - "x": 7.754041712551309, - "y": 2.1951475174693176, - "heading": -0.04315811189523446, - "angularVelocity": 0.220191325356213, - "velocityX": 1.6326267512940102, - "velocityY": 0.9265452982790828, - "timestamp": 3.6773553731937105 - }, - { - "x": 7.851915110788357, - "y": 2.2532212040315036, - "heading": -0.030028067927435956, - "angularVelocity": 0.18483259680314387, - "velocityX": 1.377771041625462, - "velocityY": 0.8175075665813204, - "timestamp": 3.7483928643878786 - }, - { - "x": 7.932154394774834, - "y": 2.302530059896823, - "heading": -0.01934465130254991, - "angularVelocity": 0.15039124334620582, - "velocityX": 1.1295343154385586, - "velocityY": 0.6941243987705448, - "timestamp": 3.8194303555820466 - }, - { - "x": 7.995185674411745, - "y": 2.3422635628439994, - "heading": -0.011049205592550179, - "angularVelocity": 0.1167756007503934, - "velocityX": 0.887295969738388, - "velocityY": 0.5593314499039962, - "timestamp": 3.8904678467762146 - }, - { - "x": 8.04138998965963, - "y": 2.3717661278123887, - "heading": -0.005089649909509545, - "angularVelocity": 0.08389310465302457, - "velocityX": 0.6504215516506965, - "velocityY": 0.41530978181295497, - "timestamp": 3.9615053379703826 - }, - { - "x": 8.071106442714182, - "y": 2.39049915084481, - "heading": -0.0014200461836977572, - "angularVelocity": 0.051657282149528694, - "velocityX": 0.418320700168416, - "velocityY": 0.26370614611400395, - "timestamp": 4.032542829164551 - }, - { - "x": 8.084636688232422, - "y": 2.3980138301849365, - "heading": -1.2697794666383574e-33, - "angularVelocity": 0.01999009480523916, - "velocityX": 0.19046626352917562, - "velocityY": 0.10578469500825122, - "timestamp": 4.103580320358718 - }, - { - "x": 8.081561182876898, - "y": 2.39334444963861, - "heading": -0.0009066648087102295, - "angularVelocity": -0.012314301117252947, - "velocityX": -0.041771444829232424, - "velocityY": -0.06341942195849094, - "timestamp": 4.177207300897626 - }, - { - "x": 8.061093542733351, - "y": 2.376630216741082, - "heading": -0.004227711790797553, - "angularVelocity": -0.045106385699632864, - "velocityX": -0.2779910298335672, - "velocityY": -0.22701233671663687, - "timestamp": 4.250834281436534 - }, - { - "x": 8.02290518913695, - "y": 2.348360537804544, - "heading": -0.01000387000903876, - "angularVelocity": -0.07845165150007544, - "velocityX": -0.5186733629015444, - "velocityY": -0.38395814590820065, - "timestamp": 4.324461261975442 - }, - { - "x": 7.966627275516003, - "y": 2.3091224913101702, - "heading": -0.01828119946614907, - "angularVelocity": -0.11242250322537992, - "velocityX": -0.7643653618418706, - "velocityY": -0.5329302683224771, - "timestamp": 4.3980882425143495 - }, - { - "x": 7.891846094053721, - "y": 2.2596302081809454, - "heading": -0.029111517424954568, - "angularVelocity": -0.1470971358533753, - "velocityX": -1.0156763310803423, - "velocityY": -0.6722030805415261, - "timestamp": 4.471715223053257 - }, - { - "x": 7.798100115514375, - "y": 2.2007657686480786, - "heading": -0.04255255372711989, - "angularVelocity": -0.18255585389737042, - "velocityX": -1.27325578005752, - "velocityY": -0.7994954988241099, - "timestamp": 4.545342203592165 - }, - { - "x": 7.684881725302932, - "y": 2.1336367022021943, - "heading": -0.058667438591618416, - "angularVelocity": -0.21887200516097166, - "velocityX": -1.5377296390908446, - "velocityY": -0.9117454763802326, - "timestamp": 4.618969184131073 - }, - { - "x": 7.5516502833969605, - "y": 2.0596568999010922, - "heading": -0.07752270936377831, - "angularVelocity": -0.25609186515799065, - "velocityX": -1.8095464587953205, - "velocityY": -1.0047920172688334, - "timestamp": 4.692596164669981 - }, - { - "x": 7.397870326337049, - "y": 1.98065825109379, - "heading": -0.09918321696192219, - "angularVelocity": -0.2941925288746233, - "velocityX": -2.0886359312079446, - "velocityY": -1.0729578780642273, - "timestamp": 4.766223145208889 - }, - { - "x": 7.2231018276798915, - "y": 1.8990346142555161, - "heading": -0.12370090160049274, - "angularVelocity": -0.33299864342004915, - "velocityX": -2.3737018329144046, - "velocityY": -1.1086104066856408, - "timestamp": 4.839850125747796 - }, - { - "x": 7.027186991114378, - "y": 1.8178941660655976, - "heading": -0.1510925803805667, - "angularVelocity": -0.37203316745549647, - "velocityX": -2.660910920582776, - "velocityY": -1.102047749289958, - "timestamp": 4.913477106286704 - }, - { - "x": 6.8105769237451845, - "y": 1.741127234048135, - "heading": -0.18130204311682424, - "angularVelocity": -0.4103042460133704, - "velocityX": -2.941993081662873, - "velocityY": -1.0426467506282693, - "timestamp": 4.987104086825612 - }, - { - "x": 6.574746676835865, - "y": 1.6731958946569394, - "heading": -0.21415137217886637, - "angularVelocity": -0.4461588513015718, - "velocityX": -3.2030411295312824, - "velocityY": -0.9226419295477921, - "timestamp": 5.06073106736452 - }, - { - "x": 6.322411514317352, - "y": 1.6185000003009695, - "heading": -0.24931002488043913, - "angularVelocity": -0.47752403323118087, - "velocityX": -3.427210523527703, - "velocityY": -0.7428784116315408, - "timestamp": 5.134358047903428 - }, - { - "x": 6.057197180213997, - "y": 1.5805921161213812, - "heading": -0.28632018217818705, - "angularVelocity": -0.5026711271717876, - "velocityX": -3.602135143423469, - "velocityY": -0.5148640335665581, - "timestamp": 5.2079850284423355 - }, - { - "x": 5.782918806315175, - "y": 1.5618062942255864, - "heading": -0.3246756429318106, - "angularVelocity": -0.5209430085667437, - "velocityX": -3.725242728837456, - "velocityY": -0.25514861207526496, - "timestamp": 5.281612008981243 + "heading": 0.3015228161300743, + "angularVelocity": 0.0000012088972110609183, + "velocityX": 3.7670067261218194, + "velocityY": -0.33592432468507777, + "timestamp": 2.7722945021318894 + }, + { + "x": 5.642161233224445, + "y": 1.5569512204704454, + "heading": 0.30152289610635313, + "angularVelocity": 0.0000021712102282875206, + "velocityX": 3.7778762267369546, + "velocityY": -0.17558800471026415, + "timestamp": 2.809129387028348 + }, + { + "x": 5.781468008961295, + "y": 1.556401100893367, + "heading": 0.30152296986307525, + "angularVelocity": 0.0000020023605969229892, + "velocityX": 3.781925099764402, + "velocityY": -0.014934744023907952, + "timestamp": 2.8459642719248066 + }, + { + "x": 5.920672391336217, + "y": 1.5617696165699877, + "heading": 0.3015230386365787, + "angularVelocity": 0.0000018670752912406278, + "velocityX": 3.779145306581512, + "velocityY": 0.14574541746802047, + "timestamp": 2.882799156821265 + }, + { + "x": 6.0595230325638285, + "y": 1.5730470711005764, + "heading": 0.3015231034083751, + "angularVelocity": 0.0000017584362380693717, + "velocityX": 3.769541879061509, + "velocityY": 0.30616233937716997, + "timestamp": 2.919634041717724 + }, + { + "x": 6.197769224123459, + "y": 1.5902130982406153, + "heading": 0.3015231649742209, + "angularVelocity": 0.000001671400517937078, + "velocityX": 3.753132172076425, + "velocityY": 0.46602635486148125, + "timestamp": 2.9564689266141824 + }, + { + "x": 6.335161349542959, + "y": 1.6132366986444322, + "heading": 0.3015232239953199, + "angularVelocity": 0.0000016023152822250254, + "velocityX": 3.72994583275345, + "velocityY": 0.6250487946015117, + "timestamp": 2.993303811510641 + }, + { + "x": 6.471451335210211, + "y": 1.6420762959087598, + "heading": 0.30152328103508963, + "angularVelocity": 0.000001548525804202316, + "velocityX": 3.7000247469309935, + "velocityY": 0.7829425107583317, + "timestamp": 3.0301386964070995 + }, + { + "x": 6.606393098338155, + "y": 1.6766798119201127, + "heading": 0.301523336612691, + "angularVelocity": 0.000001508830595721816, + "velocityX": 3.663422961881362, + "velocityY": 0.9394224010369989, + "timestamp": 3.066973581303558 + }, + { + "x": 6.739742463422568, + "y": 1.716984614087568, + "heading": 0.3015255656708468, + "angularVelocity": 0.00006051486687697733, + "velocityX": 3.6201922568579414, + "velocityY": 1.094201930608725, + "timestamp": 3.1038084662000167 + }, + { + "x": 6.8694207904091344, + "y": 1.7614703171513673, + "heading": 0.3101205256410531, + "angularVelocity": 0.23333750042565204, + "velocityX": 3.520530262306672, + "velocityY": 1.207705771006116, + "timestamp": 3.1406433510964753 + }, + { + "x": 6.994764102779472, + "y": 1.808693107878132, + "heading": 0.3306180633638746, + "angularVelocity": 0.5564707961064413, + "velocityX": 3.4028425152588873, + "velocityY": 1.282012713206686, + "timestamp": 3.177478235992934 + }, + { + "x": 7.114597005425528, + "y": 1.856512920318317, + "heading": 0.35962771339750194, + "angularVelocity": 0.7875591335542964, + "velocityX": 3.253244932973218, + "velocityY": 1.2982207647615676, + "timestamp": 3.2143131208893925 + }, + { + "x": 7.228563598795877, + "y": 1.9039637614742513, + "heading": 0.39146295222083616, + "angularVelocity": 0.8642687200685354, + "velocityX": 3.093985326429113, + "velocityY": 1.2882038667778264, + "timestamp": 3.251148005785851 + }, + { + "x": 7.336676614220082, + "y": 1.9505339221399116, + "heading": 0.42341393247502157, + "angularVelocity": 0.8674108889982484, + "velocityX": 2.935071352282109, + "velocityY": 1.2642949963483496, + "timestamp": 3.2879828906823096 + }, + { + "x": 7.438996464546078, + "y": 1.995939510716552, + "heading": 0.45395278545537515, + "angularVelocity": 0.8290742068611583, + "velocityX": 2.77779747686502, + "velocityY": 1.2326789863542051, + "timestamp": 3.3248177755787682 + }, + { + "x": 7.535582073954796, + "y": 2.0400031624519186, + "heading": 0.48208291467910164, + "angularVelocity": 0.7636817463336474, + "velocityX": 2.6221232855814667, + "velocityY": 1.196247846551653, + "timestamp": 3.361652660475227 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6791276743107281, + "velocityX": 2.4678193928677437, + "velocityY": 1.1565397180800703, + "timestamp": 3.3984875453716854 + }, + { + "x": 7.766333855095899, + "y": 2.1513911601167894, + "heading": 0.5393931714770351, + "angularVelocity": 0.5089507306836832, + "velocityX": 2.2039777611896034, + "velocityY": 1.0840548028640598, + "timestamp": 3.461940970502476 + }, + { + "x": 7.8897168213730975, + "y": 2.214535022901093, + "heading": 0.5621690879226864, + "angularVelocity": 0.3589391179231937, + "velocityX": 1.9444650312080196, + "velocityY": 0.9951214241650669, + "timestamp": 3.5253943956332665 + }, + { + "x": 7.996856912991746, + "y": 2.271342778200504, + "heading": 0.5763089063837279, + "angularVelocity": 0.2228377496076287, + "velocityX": 1.6884839769927567, + "velocityY": 0.8952669644281369, + "timestamp": 3.588847820764057 + }, + { + "x": 8.08793407853681, + "y": 2.3213235528188796, + "heading": 0.5824533246990756, + "angularVelocity": 0.09683351690917834, + "velocityX": 1.4353388387992245, + "velocityY": 0.7876765441007412, + "timestamp": 3.6523012458948476 + }, + { + "x": 8.16309393881241, + "y": 2.364112462024999, + "heading": 0.5810872026138336, + "angularVelocity": -0.021529524725041504, + "velocityX": 1.1844886248564517, + "velocityY": 0.6743356898059124, + "timestamp": 3.715754671025638 + }, + { + "x": 8.222455939995415, + "y": 2.399428067130867, + "heading": 0.5725898810376202, + "angularVelocity": -0.13391430893917436, + "velocityX": 0.9355208337555891, + "velocityY": 0.5565594770191685, + "timestamp": 3.7792080961564287 + }, + { + "x": 8.266119462815807, + "y": 2.4270468912932266, + "heading": 0.5572661381126698, + "angularVelocity": -0.24149591441856846, + "velocityX": 0.6881192422693254, + "velocityY": 0.43526136068197446, + "timestamp": 3.8426615212872193 + }, + { + "x": 8.294168308263885, + "y": 2.4467872814650136, + "heading": 0.5353660980042331, + "angularVelocity": -0.3451356654632318, + "velocityX": 0.4420383200790668, + "velocityY": 0.3111004666981787, + "timestamp": 3.90611494641801 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.44548570157766576, + "velocityX": 0.19708463824313602, + "velocityY": 0.18456741878341876, + "timestamp": 3.9695683715488004 + }, + { + "x": 8.303024530407173, + "y": 2.461881127582199, + "heading": 0.47119103232326837, + "angularVelocity": -0.5465136547157349, + "velocityX": -0.05554517814981655, + "velocityY": 0.05148048067312068, + "timestamp": 4.035271161880262 + }, + { + "x": 8.282750419132642, + "y": 2.456558519757899, + "heading": 0.4288756816904664, + "angularVelocity": -0.6440419108431604, + "velocityX": -0.3085730632177365, + "velocityY": -0.08101037714605593, + "timestamp": 4.1009739522117235 + }, + { + "x": 8.245821395629305, + "y": 2.442576926285189, + "heading": 0.3804195474569323, + "angularVelocity": -0.7375049672788585, + "velocityX": -0.5620617224479442, + "velocityY": -0.21280060408660287, + "timestamp": 4.166676742543185 + }, + { + "x": 8.19220201387014, + "y": 2.4199912479232255, + "heading": 0.32613641295591117, + "angularVelocity": -0.8261922245184762, + "velocityX": -0.8160898721144456, + "velocityY": -0.34375523852216117, + "timestamp": 4.2323795328746465 + }, + { + "x": 8.121850214183777, + "y": 2.388868090056124, + "heading": 0.2664003971864745, + "angularVelocity": -0.909185370485436, + "velocityX": -1.070758172239708, + "velocityY": -0.4736961354318384, + "timestamp": 4.298082323206108 + }, + { + "x": 8.034715295558529, + "y": 2.349289919990928, + "heading": 0.20166644793914118, + "angularVelocity": -0.9852541866298153, + "velocityX": -1.3261981444876794, + "velocityY": -0.6023818754961501, + "timestamp": 4.36378511353757 + }, + { + "x": 7.9307349853362235, + "y": 2.301361396168203, + "heading": 0.1325025874209066, + "angularVelocity": -1.052677674255729, + "velocityX": -1.5825859099398853, + "velocityY": -0.7294747084702512, + "timestamp": 4.429487903869031 + }, + { + "x": 7.809831081747403, + "y": 2.2452194829459002, + "heading": 0.0596429970770519, + "angularVelocity": -1.1089268808263426, + "velocityX": -1.8401639105261376, + "velocityY": -0.8544829365552749, + "timestamp": 4.495190694200493 + }, + { + "x": 7.671902780233127, + "y": 2.1810506699963903, + "heading": -0.015920256793080897, + "angularVelocity": -1.1500767850029918, + "velocityX": -2.099276161917122, + "velocityY": -0.9766527818040505, + "timestamp": 4.560893484531954 + }, + { + "x": 7.516816185598606, + "y": 2.109122861945235, + "heading": -0.09276870443286889, + "angularVelocity": -1.1696375032490751, + "velocityX": -2.3604263053689314, + "velocityY": -1.0947451042534069, + "timestamp": 4.626596274863416 + }, + { + "x": 7.344387868807497, + "y": 2.02985165560778, + "heading": -0.16871929267657412, + "angularVelocity": -1.1559720349858067, + "velocityX": -2.6243682486121434, + "velocityY": -1.206512020837212, + "timestamp": 4.692299065194877 + }, + { + "x": 7.154363408340053, + "y": 1.9439629823466762, + "heading": -0.2400485240661188, + "angularVelocity": -1.0856347352935642, + "velocityX": -2.8921825010596516, + "velocityY": -1.3072302230667405, + "timestamp": 4.758001855526339 + }, + { + "x": 6.9464441838510815, + "y": 1.8530162365161489, + "heading": -0.2992772419214471, + "angularVelocity": -0.9014642689682996, + "velocityX": -3.1645417712101533, + "velocityY": -1.3842143594162994, + "timestamp": 4.8237046458578 + }, + { + "x": 6.72212326982476, + "y": 1.7626751908782685, + "heading": -0.31855321839260475, + "angularVelocity": -0.2933813978662556, + "velocityX": -3.4141763674670904, + "velocityY": -1.3749955699312393, + "timestamp": 4.889407436189262 + }, + { + "x": 6.485759745047901, + "y": 1.6860130581467425, + "heading": -0.3185532921212082, + "angularVelocity": -0.0000011221533066716862, + "velocityX": -3.597465550312819, + "velocityY": -1.1668017803319484, + "timestamp": 4.955110226520723 + }, + { + "x": 6.24426876944993, + "y": 1.6274732096370486, + "heading": -0.3185533177664736, + "angularVelocity": -3.9032231775904613e-7, + "velocityX": -3.675505627381763, + "velocityY": -0.8909796405048926, + "timestamp": 5.020813016852185 + }, + { + "x": 5.999037658914315, + "y": 1.5873919634602989, + "heading": -0.3185533462426243, + "angularVelocity": -4.334085446884588e-7, + "velocityX": -3.732430682143934, + "velocityY": -0.6100387209515062, + "timestamp": 5.0865158071836465 + }, + { + "x": 5.751475205523387, + "y": 1.5659995878977724, + "heading": -0.3185533789943251, + "angularVelocity": -4.984826463431063e-7, + "velocityX": -3.7679138456983283, + "velocityY": -0.325593105781428, + "timestamp": 5.152218597515108 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.3639012008110169, - "angularVelocity": -0.5327606482310914, - "velocityX": -3.8018020976391327, - "velocityY": 0.021903521993656888, - "timestamp": 5.355238989520151 - }, - { - "x": 5.261378018303958, - "y": 1.580163952485418, - "heading": -0.3978140479254385, - "angularVelocity": -0.5381806191653729, - "velocityX": -3.83448205483159, - "velocityY": 0.265734612450363, - "timestamp": 5.418252866101241 - }, - { - "x": 5.01887444525289, - "y": 1.6123863305278272, - "heading": -0.4318947982287382, - "angularVelocity": -0.5408451622468005, - "velocityX": -3.848415400042849, - "velocityY": 0.5113536857384613, - "timestamp": 5.481266742682331 - }, - { - "x": 4.777050990194419, - "y": 1.6600961195625756, - "heading": -0.4659121119973733, - "angularVelocity": -0.5398384548657204, - "velocityX": -3.8376222536837075, - "velocityY": 0.7571314704524225, - "timestamp": 5.5442806192634215 - }, - { - "x": 4.5379789584596235, - "y": 1.7230618675308726, - "heading": -0.49955411376200026, - "angularVelocity": -0.5338824333610737, - "velocityX": -3.793958485114657, - "velocityY": 0.9992362219973634, - "timestamp": 5.607294495844512 - }, - { - "x": 4.3043600249129454, - "y": 1.8005379950644207, - "heading": -0.5324078746922887, - "angularVelocity": -0.5213734293590093, - "velocityX": -3.7074204321653204, - "velocityY": 1.2295089865459496, - "timestamp": 5.670308372425602 - }, - { - "x": 4.07944895912211, - "y": 1.8908360479698116, - "heading": -0.5639729512662652, - "angularVelocity": -0.5009226266750401, - "velocityX": -3.5692307471578024, - "velocityY": 1.4329867928247444, - "timestamp": 5.733322249006692 - }, - { - "x": 3.866497717212655, - "y": 1.9909883194289872, - "heading": -0.5937624584033184, - "angularVelocity": -0.47274519127097403, - "velocityX": -3.37943407807034, - "velocityY": 1.5893685152078068, - "timestamp": 5.796336125587782 - }, - { - "x": 3.667854490580529, - "y": 2.0970860288082087, - "heading": -0.6214571487340739, - "angularVelocity": -0.4395014532254029, - "velocityX": -3.1523727377175264, - "velocityY": 1.6837197635775993, - "timestamp": 5.859350002168872 - }, - { - "x": 3.4845656078038156, - "y": 2.20525192218672, - "heading": -0.6469466767255073, - "angularVelocity": -0.40450658449225635, - "velocityX": -2.9087066646478257, - "velocityY": 1.7165408517490015, - "timestamp": 5.922363878749962 - }, - { - "x": 3.316731609967858, - "y": 2.3123318355961353, - "heading": -0.6702493227736513, - "angularVelocity": -0.3698018168769004, - "velocityX": -2.6634450527730666, - "velocityY": 1.699306870473487, - "timestamp": 5.9853777553310525 - }, - { - "x": 3.1640084855137136, - "y": 2.4159899224511383, - "heading": -0.6914332444345033, - "angularVelocity": -0.3361786769869806, - "velocityX": -2.423642739351382, - "velocityY": 1.6450041241568225, - "timestamp": 6.048391631912143 - }, - { - "x": 3.0259122517259884, - "y": 2.5145372691122856, - "heading": -0.7105770269066637, - "angularVelocity": -0.30380264650953404, - "velocityX": -2.19152099950578, - "velocityY": 1.5638991283821457, - "timestamp": 6.111405508493233 - }, - { - "x": 2.9019526205870343, - "y": 2.6067411641295175, - "heading": -0.7277548924092291, - "angularVelocity": -0.2726044870523041, - "velocityX": -1.9671798953589927, - "velocityY": 1.4632315931012068, - "timestamp": 6.174419385074323 - }, - { - "x": 2.79168051154695, - "y": 2.6916821542694485, - "heading": -0.743032506079513, - "angularVelocity": -0.24244840183135916, - "velocityX": -1.7499654841609167, - "velocityY": 1.3479727759745657, - "timestamp": 6.237433261655413 - }, - { - "x": 2.694699726347074, - "y": 2.76865749223089, - "heading": -0.7564666750477512, - "angularVelocity": -0.21319381852265873, - "velocityX": -1.5390385493118295, - "velocityY": 1.2215616962143954, - "timestamp": 6.300447138236503 - }, - { - "x": 2.6106653021864603, - "y": 2.8371173452263947, - "heading": -0.7681062788003478, - "angularVelocity": -0.18471492922067745, - "velocityX": -1.3335860086702125, - "velocityY": 1.0864250338162604, - "timestamp": 6.363461014817593 - }, - { - "x": 2.5392776642565695, - "y": 2.8966223272202614, - "heading": -0.7779934551732625, - "angularVelocity": -0.15690474716614497, - "velocityX": -1.1328875765645765, - "velocityY": 0.9443155257602959, - "timestamp": 6.4264748913986836 - }, - { - "x": 2.480276064918479, - "y": 2.946814696080483, - "heading": -0.7861647133126454, - "angularVelocity": -0.1296739477512957, - "velocityX": -0.9363270844345449, - "velocityY": 0.7965288216418583, - "timestamp": 6.489488767979774 - }, - { - "x": 2.4334325341041674, - "y": 2.9873983829565494, - "heading": -0.7926518839069914, - "angularVelocity": -0.10294828609691896, - "velocityX": -0.7433843679499733, - "velocityY": 0.6440436468599187, - "timestamp": 6.552502644560864 - }, - { - "x": 2.398546680913597, - "y": 3.0181248328808925, - "heading": -0.7974829010951674, - "angularVelocity": -0.07666592582919579, - "velocityX": -0.5536217589418905, - "velocityY": 0.4876140239491929, - "timestamp": 6.615516521141954 - }, - { - "x": 2.3754413559178094, - "y": 3.038782749760636, - "heading": -0.8006824366316742, - "angularVelocity": -0.0507750944728719, - "velocityX": -0.36667042641082753, - "velocityY": 0.32783123338174003, - "timestamp": 6.678530397723044 + "heading": -0.3185534180019772, + "angularVelocity": -5.936985611348973e-7, + "velocityX": -3.781751231726404, + "velocityY": -0.039276923728911305, + "timestamp": 5.21792138784657 + }, + { + "x": 5.26265149067387, + "y": 1.5786031053003822, + "heading": -0.31855345112044986, + "angularVelocity": -5.20084412877405e-7, + "velocityX": -3.7744308273915435, + "velocityY": 0.23844772894968475, + "timestamp": 5.28160042121006 + }, + { + "x": 5.0240625547753925, + "y": 1.61139052009726, + "heading": -0.3185534786660252, + "angularVelocity": -4.3256899255575315e-7, + "velocityX": -3.7467424251962935, + "velocityY": 0.5148855606793222, + "timestamp": 5.34527945457355 + }, + { + "x": 4.788524290519247, + "y": 1.661604289022676, + "heading": -0.31855350247111613, + "angularVelocity": -3.7382933947955595e-7, + "velocityX": -3.698835422197071, + "velocityY": 0.7885447732660761, + "timestamp": 5.40895848793704 + }, + { + "x": 4.55730773239592, + "y": 1.7289734284079028, + "heading": -0.3185535237219762, + "angularVelocity": -3.337183198598428e-7, + "velocityX": -3.6309684037367047, + "velocityY": 1.0579485244487485, + "timestamp": 5.4726375213005305 + }, + { + "x": 4.33166058516164, + "y": 1.8131343663413284, + "heading": -0.3185535432404584, + "angularVelocity": -3.0651348259884106e-7, + "velocityX": -3.543507734268667, + "velocityY": 1.321642831055895, + "timestamp": 5.536316554664021 + }, + { + "x": 4.112800476187319, + "y": 1.9136328827222209, + "heading": -0.318553561635836, + "angularVelocity": -2.888765193967595e-7, + "velocityX": -3.436925741712075, + "velocityY": 1.5782041760469432, + "timestamp": 5.599995588027511 + }, + { + "x": 3.9019083093617053, + "y": 2.0299264483320134, + "heading": -0.31855357939397577, + "angularVelocity": -2.7886949297236075e-7, + "velocityX": -3.311799122668965, + "velocityY": 1.826245774585971, + "timestamp": 5.663674621391001 + }, + { + "x": 3.7001203074329077, + "y": 2.161384813738153, + "heading": -0.3185535969362317, + "angularVelocity": -2.75479306959021e-7, + "velocityX": -3.1688295388681516, + "velocityY": 2.0643900898393674, + "timestamp": 5.727353654754491 + }, + { + "x": 3.509285404964888, + "y": 2.2879634464070207, + "heading": -0.3579155843505842, + "angularVelocity": -0.6181310446354924, + "velocityX": -2.996824737880426, + "velocityY": 1.9877599577609337, + "timestamp": 5.791032688117982 + }, + { + "x": 3.332909755538667, + "y": 2.4050971261724468, + "heading": -0.4129449232759771, + "angularVelocity": -0.8641673093760197, + "velocityX": -2.769760156682042, + "velocityY": 1.8394387222684185, + "timestamp": 5.854711721481472 + }, + { + "x": 3.171287726021972, + "y": 2.512479984884094, + "heading": -0.4699204658498491, + "angularVelocity": -0.8947300165290918, + "velocityX": -2.5380729100288124, + "velocityY": 1.6863142079856834, + "timestamp": 5.918390754844962 + }, + { + "x": 3.024408806786741, + "y": 2.6100923699912792, + "heading": -0.5252799185719804, + "angularVelocity": -0.8693513358805374, + "velocityX": -2.306550704009953, + "velocityY": 1.5328810748429236, + "timestamp": 5.982069788208452 + }, + { + "x": 2.892255079193046, + "y": 2.6979336450739244, + "heading": -0.5773440254447586, + "angularVelocity": -0.8176020288434336, + "velocityX": -2.0753098879397367, + "velocityY": 1.3794379475145813, + "timestamp": 6.045748821571943 + }, + { + "x": 2.7748123815012584, + "y": 2.7760062309054065, + "heading": -0.625128837055519, + "angularVelocity": -0.7504010203483636, + "velocityX": -1.8442914643726742, + "velocityY": 1.2260328354205916, + "timestamp": 6.109427854935433 + }, + { + "x": 2.6720699818921556, + "y": 2.844312916696855, + "heading": -0.6679860381773631, + "angularVelocity": -0.6730190277419643, + "velocityX": -1.6134415706757543, + "velocityY": 1.0726715244175122, + "timestamp": 6.173106888298923 + }, + { + "x": 2.5840196243973645, + "y": 2.9028562717726505, + "heading": -0.7054558016839698, + "angularVelocity": -0.5884160221579265, + "velocityX": -1.382721326691407, + "velocityY": 0.9193505614575078, + "timestamp": 6.236785921662413 + }, + { + "x": 2.510654811151598, + "y": 2.95163853061359, + "heading": -0.7371951288142529, + "angularVelocity": -0.4984266477336408, + "velocityX": -1.1521031235978156, + "velocityY": 0.7660646882386306, + "timestamp": 6.3004649550259035 + }, + { + "x": 2.4519703126340358, + "y": 2.9906615990271557, + "heading": -0.7629386573341134, + "angularVelocity": -0.4042700895428552, + "velocityX": -0.9215670436230066, + "velocityY": 0.6128087433553882, + "timestamp": 6.364143988389394 + }, + { + "x": 2.4079618327799786, + "y": 3.0199270886206313, + "heading": -0.7824753669479414, + "angularVelocity": -0.3067997201262361, + "velocityX": -0.6910984279998401, + "velocityY": 0.4595781067596215, + "timestamp": 6.427823021752884 + }, + { + "x": 2.378625774904473, + "y": 3.039436353320747, + "heading": -0.795633833561549, + "angularVelocity": -0.20663734856804464, + "velocityX": -0.4606862938394533, + "velocityY": 0.3063687318988399, + "timestamp": 6.491502055116374 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.025232146222040765, - "velocityX": -0.18221830683035964, - "velocityY": 0.16516634183273946, - "timestamp": 6.741544274304134 + "angularVelocity": -0.10425061542851642, + "velocityX": -0.2303222914890624, + "velocityY": 0.15317707264507022, + "timestamp": 6.5551810884798645 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 5.951839521214632e-29, - "velocityX": -4.256198532236221e-31, - "velocityY": 3.167098439878756e-31, - "timestamp": 6.804558150885224 - }, - { - "x": 2.375813180458781, - "y": 3.0435390743709614, - "heading": -0.7990461525696796, - "angularVelocity": 0.05557590004806609, - "velocityX": 0.20420014353356836, - "velocityY": -0.09735244641805807, - "timestamp": 6.862609561786642 - }, - { - "x": 2.3995218612588998, - "y": 3.03223611715748, - "heading": -0.7926240802250826, - "angularVelocity": 0.110627325759628, - "velocityX": 0.4084083475659286, - "velocityY": -0.19470598626234253, - "timestamp": 6.92066097268806 - }, - { - "x": 2.435085642006342, - "y": 3.015281569795486, - "heading": -0.7830411709963286, - "angularVelocity": 0.1650762501712104, - "velocityX": 0.6126256053937512, - "velocityY": -0.2920609008243783, - "timestamp": 6.978712383589477 - }, - { - "x": 2.4825051165422036, - "y": 2.9926753368780465, - "heading": -0.7703376648494888, - "angularVelocity": 0.21883199649381424, - "velocityX": 0.8168530927937191, - "velocityY": -0.3894174588767385, - "timestamp": 7.036763794490895 - }, - { - "x": 2.541780960274715, - "y": 2.964417308476329, - "heading": -0.7545600110272952, - "angularVelocity": 0.2717876030435742, - "velocityX": 1.0210922148502688, - "velocityY": -0.4867759105752865, - "timestamp": 7.094815205392313 - }, - { - "x": 2.612913947196807, - "y": 2.930507361776858, - "heading": -0.7357621134740673, - "angularVelocity": 0.3238146543096182, - "velocityX": 1.225344669794324, - "velocityY": -0.5841364778722867, - "timestamp": 7.15286661629373 - }, - { - "x": 2.6959049720092736, - "y": 2.8909453636117948, - "heading": -0.7140070097413544, - "angularVelocity": 0.37475581376751455, - "velocityX": 1.4296125369528379, - "velocityY": -0.6814993391331685, - "timestamp": 7.210918027195148 - }, - { - "x": 2.7907550793730915, - "y": 2.84573117446325, - "heading": -0.6893691896527133, - "angularVelocity": 0.4244138033179133, - "velocityX": 1.6338983995564174, - "velocityY": -0.778864603744556, - "timestamp": 7.2689694380965655 - }, - { - "x": 2.8974655032774796, - "y": 2.7948646549900524, - "heading": -0.6619378845471044, - "angularVelocity": 0.47253468399231374, - "velocityX": 1.8382055189942428, - "velocityY": -0.876232268662314, - "timestamp": 7.327020848997983 - }, - { - "x": 3.0160377210166036, - "y": 2.7383456770648533, - "heading": -0.6318218809717984, - "angularVelocity": 0.5187815956178783, - "velocityX": 2.042538086464129, - "velocityY": -0.9736021407159139, - "timestamp": 7.385072259899401 - }, - { - "x": 3.1464735286190364, - "y": 2.676174143314885, - "heading": -0.5991568348591108, - "angularVelocity": 0.5626916832074861, - "velocityX": 2.2469015925200937, - "velocityY": -1.0709736901235276, - "timestamp": 7.443123670800818 - }, - { - "x": 3.288775148081632, - "y": 2.6083500237720547, - "heading": -0.5641169164360986, - "angularVelocity": 0.6036014952766117, - "velocityX": 2.4513033749386723, - "velocityY": -1.1683457557648163, - "timestamp": 7.501175081702236 - }, - { - "x": 3.4429453809853263, - "y": 2.534873429900399, - "heading": -0.5269344943074844, - "angularVelocity": 0.6405085001595859, - "velocityX": 2.6557534176991067, - "velocityY": -1.2657159013143156, - "timestamp": 7.559226492603654 - }, - { - "x": 3.608987822216531, - "y": 2.4557447796300744, - "heading": -0.48793616320693267, - "angularVelocity": 0.6717895481778894, - "velocityX": 2.860265386368935, - "velocityY": -1.3630788475529159, - "timestamp": 7.617277903505071 - }, - { - "x": 3.786907101523248, - "y": 2.370965219866687, - "heading": -0.44761641247502054, - "angularVelocity": 0.6945524683350642, - "velocityX": 3.064857107587936, - "velocityY": -1.4604220370691692, - "timestamp": 7.675329314406489 - }, - { - "x": 3.9767087353801904, - "y": 2.2805379602303306, - "heading": -0.4068154658739133, - "angularVelocity": 0.7028416013935566, - "velocityX": 3.2695438562081205, - "velocityY": -1.5577099373160743, - "timestamp": 7.7333807253079065 - }, - { - "x": 4.178394426915559, - "y": 2.1844743331965764, - "heading": -0.36728880481186277, - "angularVelocity": 0.6808906183034006, - "velocityX": 3.4742599431023264, - "velocityY": -1.6548026230902309, - "timestamp": 7.791432136209324 - }, - { - "x": 4.39186585424446, - "y": 2.082861864950712, - "heading": -0.33521197370242245, - "angularVelocity": 0.5525590267549734, - "velocityX": 3.6772823263747743, - "velocityY": -1.7503875731534284, - "timestamp": 7.849483547110742 - }, - { - "x": 4.613290428731911, - "y": 1.9772010264517206, - "heading": -0.3352119609115161, - "angularVelocity": 2.203375614401519e-7, - "velocityX": 3.81428411556565, - "velocityY": -1.8201252451628447, - "timestamp": 7.907534958012159 - }, - { - "x": 4.83471490814338, - "y": 1.8715399887082398, - "heading": -0.33521194812268507, - "angularVelocity": 2.2030181304603152e-7, - "velocityX": 3.81428247777632, - "velocityY": -1.8201286773703715, - "timestamp": 7.965586368913577 - }, - { - "x": 5.056139387672164, - "y": 1.7658789512106026, - "heading": -0.33521193533385407, - "angularVelocity": 2.2030181241344307e-7, - "velocityX": 3.8142824797971753, - "velocityY": -1.8201286731354431, - "timestamp": 8.023637779814994 - }, - { - "x": 5.277564966924424, - "y": 1.6602202183369479, - "heading": -0.33521192254501997, - "angularVelocity": 2.203018646355495e-7, - "velocityX": 3.814301423754948, - "velocityY": -1.8200889734288124, - "timestamp": 8.08168919071641 + "angularVelocity": -1.4092752497411054e-30, + "velocityX": 2.9428006004895336e-31, + "velocityY": 5.314710200495422e-31, + "timestamp": 6.618860121843355 + }, + { + "x": 2.3794842314620843, + "y": 3.041727640405621, + "heading": -0.7962432666233688, + "angularVelocity": 0.0957711014436812, + "velocityX": 0.24661230413774035, + "velocityY": -0.11854554422657079, + "timestamp": 6.681813823589832 + }, + { + "x": 2.410536447869941, + "y": 3.0268006675090624, + "heading": -0.7842917338243374, + "angularVelocity": 0.18984638658997072, + "velocityX": 0.4932548134009415, + "velocityY": -0.23711032842312177, + "timestamp": 6.744767525336309 + }, + { + "x": 2.4571179241280796, + "y": 3.0044081651489933, + "heading": -0.7665449064601504, + "angularVelocity": 0.28190284084732253, + "velocityX": 0.7399322830248891, + "velocityY": -0.3556979452971155, + "timestamp": 6.807721227082786 + }, + { + "x": 2.5192312369840457, + "y": 2.9745484345249116, + "heading": -0.743155503226628, + "angularVelocity": 0.37153340605314533, + "velocityX": 0.9866506834833091, + "velocityY": -0.4743125470894598, + "timestamp": 6.8706749288292635 + }, + { + "x": 2.596879439737626, + "y": 2.937219467270397, + "heading": -0.714309410311632, + "angularVelocity": 0.4582112268975565, + "velocityX": 1.233417584660547, + "velocityY": -0.5929590511586358, + "timestamp": 6.933628630575741 + }, + { + "x": 2.690066196626848, + "y": 2.892418878795792, + "heading": -0.6802368283616581, + "angularVelocity": 0.5412323819683986, + "velocityX": 1.4802426911208155, + "velocityY": -0.7116434336939077, + "timestamp": 6.996582332322218 + }, + { + "x": 2.798795963694134, + "y": 2.8401438158547943, + "heading": -0.6412294413317386, + "angularVelocity": 0.6196202280051365, + "velocityX": 1.7271385804309867, + "velocityY": -0.8303731391605256, + "timestamp": 7.059536034068695 + }, + { + "x": 2.9230742308720847, + "y": 2.7803908299690017, + "heading": -0.5976682545562845, + "angularVelocity": 0.6919559226378926, + "velocityX": 1.9741216756154527, + "velocityY": -0.9491576226355342, + "timestamp": 7.122489735815172 + }, + { + "x": 3.062907829622796, + "y": 2.713155712528932, + "heading": -0.5500718966234338, + "angularVelocity": 0.7560533632244142, + "velocityX": 2.22121328645359, + "velocityY": -1.0680089585650514, + "timestamp": 7.185443437561649 + }, + { + "x": 3.218305230273769, + "y": 2.638433320090989, + "heading": -0.4991885737635272, + "angularVelocity": 0.8082657802208455, + "velocityX": 2.4684394458133507, + "velocityY": -1.1869419964986359, + "timestamp": 7.248397139308127 + }, + { + "x": 3.389276272924326, + "y": 2.556217612467452, + "heading": -0.4461960441931904, + "angularVelocity": 0.8417698737358584, + "velocityX": 2.715821912094713, + "velocityY": -1.3059709809381974, + "timestamp": 7.311350841054604 + }, + { + "x": 3.575827436112715, + "y": 2.4665034787202997, + "heading": -0.3932374103043789, + "angularVelocity": 0.841231451362194, + "velocityX": 2.9633072879440143, + "velocityY": -1.4250811510408536, + "timestamp": 7.374304542801081 + }, + { + "x": 3.777911234478158, + "y": 2.369308059755886, + "heading": -0.34555593776982274, + "angularVelocity": 0.7574053822375035, + "velocityX": 3.210038373585424, + "velocityY": -1.5439190431697394, + "timestamp": 7.437258244547558 + }, + { + "x": 3.9926293770053456, + "y": 2.266439040708753, + "heading": -0.3455557491739834, + "angularVelocity": 0.000002995786333953476, + "velocityX": 3.4107310066036556, + "velocityY": -1.6340424183696203, + "timestamp": 7.500211946294035 + }, + { + "x": 4.207200483864901, + "y": 2.1632635581376745, + "heading": -0.34555573252846533, + "angularVelocity": 2.644088848166298e-7, + "velocityX": 3.4083953906898548, + "velocityY": -1.6389104962656516, + "timestamp": 7.5631656480405125 + }, + { + "x": 4.421771581956281, + "y": 2.0600880573316713, + "heading": -0.3455557158829472, + "angularVelocity": 2.6440888572481287e-7, + "velocityX": 3.408395251410094, + "velocityY": -1.638910785921765, + "timestamp": 7.62611934978699 + }, + { + "x": 4.636342680047139, + "y": 1.9569125565245837, + "heading": -0.3455556992374292, + "angularVelocity": 2.6440888404451857e-7, + "velocityX": 3.4083952514018088, + "velocityY": -1.6389107859389946, + "timestamp": 7.689073051533467 + }, + { + "x": 4.850913778140846, + "y": 1.853737055723424, + "heading": -0.3455556825919111, + "angularVelocity": 2.64408884846605e-7, + "velocityX": 3.408395251447087, + "velocityY": -1.63891078584483, + "timestamp": 7.752026753279944 + }, + { + "x": 5.065484924140348, + "y": 1.7505616545506137, + "heading": -0.34555566594639303, + "angularVelocity": 2.644088848456774e-7, + "velocityX": 3.4083960124157384, + "velocityY": -1.6389092032794423, + "timestamp": 7.814980455026421 + }, + { + "x": 5.280845302209934, + "y": 1.6490439336616942, + "heading": -0.34555564929194366, + "angularVelocity": 2.6455075541297124e-7, + "velocityX": 3.4209327187283005, + "velocityY": -1.6125774668143436, + "timestamp": 7.877934156772898 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.3352119097112988, - "angularVelocity": 2.2107509356850104e-7, - "velocityX": 3.8834306838456705, - "velocityY": -1.6675087206439847, - "timestamp": 8.139740601617827 - }, - { - "x": 5.771247738907838, - "y": 1.4675302215572488, - "heading": -0.3352118975519742, - "angularVelocity": 1.8039593836487576e-7, - "velocityX": 3.9796744633245575, - "velocityY": -1.4226072506653686, - "timestamp": 8.207144140304313 - }, - { - "x": 6.040462534260166, - "y": 1.3811079630666692, - "heading": -0.3269077113367565, - "angularVelocity": 0.1232010422159426, - "velocityX": 3.994075097518657, - "velocityY": -1.2821620374051248, - "timestamp": 8.2745476789908 - }, - { - "x": 6.293431004629783, - "y": 1.2999254087750478, - "heading": -0.29863774275478366, - "angularVelocity": 0.4194137152570709, - "velocityX": 3.7530443549299446, - "velocityY": -1.2044257003957306, - "timestamp": 8.341951217677286 - }, - { - "x": 6.529552502522878, - "y": 1.2241576590494234, - "heading": -0.2655052058197796, - "angularVelocity": 0.4915548587013119, - "velocityX": 3.5031023963202834, - "velocityY": -1.1240915714832609, - "timestamp": 8.409354756363772 - }, - { - "x": 6.748788082038609, - "y": 1.153810716509259, - "heading": -0.23113245426293594, - "angularVelocity": 0.5099547030716253, - "velocityX": 3.252582635690123, - "velocityY": -1.0436683876104664, - "timestamp": 8.476758295050258 - }, - { - "x": 6.951134609386156, - "y": 1.0888826430758505, - "heading": -0.19719131465702286, - "angularVelocity": 0.5035513011235752, - "velocityX": 3.0020163820882195, - "velocityY": -0.9632739571049619, - "timestamp": 8.544161833736744 - }, - { - "x": 7.136594604940876, - "y": 1.0293710541923635, - "heading": -0.1646509365883203, - "angularVelocity": 0.48276958009664506, - "velocityX": 2.7514875208162133, - "velocityY": -0.8829149039235648, - "timestamp": 8.61156537242323 - }, - { - "x": 7.305171448299849, - "y": 0.9752739365513875, - "heading": -0.134145865535612, - "angularVelocity": 0.45257373199049533, - "velocityX": 2.501008799301714, - "velocityY": -0.8025857202037736, - "timestamp": 8.678968911109717 - }, - { - "x": 7.4568683544045555, - "y": 0.9265896769068921, - "heading": -0.10612469685480012, - "angularVelocity": 0.4157225158629608, - "velocityX": 2.2505777747114233, - "velocityY": -0.7222804706284149, - "timestamp": 8.746372449796203 - }, - { - "x": 7.591688163065539, - "y": 0.8833169839764341, - "heading": -0.08092177349469894, - "angularVelocity": 0.3739109822902337, - "velocityX": 2.0001888815967246, - "velocityY": -0.6419943785404568, - "timestamp": 8.81377598848269 - }, - { - "x": 7.709633332729291, - "y": 0.8454548114841277, - "heading": -0.05879603455931429, - "angularVelocity": 0.3282578239445074, - "velocityX": 1.7498364620342992, - "velocityY": -0.5617238090186147, - "timestamp": 8.881179527169175 - }, - { - "x": 7.810705984005889, - "y": 0.8130022977681454, - "heading": -0.03995388423570108, - "angularVelocity": 0.27954244971105247, - "velocityX": 1.499515503877575, - "velocityY": -0.48146602312571224, - "timestamp": 8.948583065855662 - }, - { - "x": 7.894907949862419, - "y": 0.7859587206023052, - "heading": -0.024563521252436132, - "angularVelocity": 0.22833167639537438, - "velocityX": 1.2492217396504763, - "velocityY": -0.4012189521922299, - "timestamp": 9.015986604542148 - }, - { - "x": 7.962240820715571, - "y": 0.7643234635620257, - "heading": -0.012764369724625984, - "angularVelocity": 0.17505240463252983, - "velocityX": 0.9989515708713329, - "velocityY": -0.3209810265438958, - "timestamp": 9.083390143228634 - }, - { - "x": 8.012705982087093, - "y": 0.7480959907369016, - "heading": -0.004673541051565745, - "angularVelocity": 0.12003566623844747, - "velocityX": 0.7487019577154632, - "velocityY": -0.24075105167108718, - "timestamp": 9.15079368191512 - }, - { - "x": 8.046304645291002, - "y": 0.7372758274108119, - "heading": -0.00039041667718953013, - "angularVelocity": 0.06354450312022167, - "velocityX": 0.49847031563414146, - "velocityY": -0.1605281196943911, - "timestamp": 9.218197220601606 - }, - { - "x": 8.063037872314453, - "y": 0.7318625450134277, - "heading": -3.5207342228472116e-29, - "angularVelocity": 0.005792228194527766, - "velocityX": 0.24825442921152802, - "velocityY": -0.08031154599409078, - "timestamp": 9.285600759288092 - }, - { - "x": 8.061895055441012, - "y": 0.7321772752029715, - "heading": -0.004013012422633091, - "angularVelocity": -0.05636038425014907, - "velocityX": -0.016050186575884747, - "velocityY": 0.004420199229323397, - "timestamp": 9.35680347509688 - }, - { - "x": 8.041931973498654, - "y": 0.7385255618667966, - "heading": -0.012367513802926035, - "angularVelocity": -0.11733402701567407, - "velocityX": -0.2803696701115773, - "velocityY": 0.0891579287631834, - "timestamp": 9.428006190905668 - }, - { - "x": 8.003147425566949, - "y": 0.7509078775903022, - "heading": -0.024966290125142357, - "angularVelocity": -0.17694235646930861, - "velocityX": -0.5447060198638873, - "velocityY": 0.17390229547925942, - "timestamp": 9.499208906714456 - }, - { - "x": 7.945540047254358, - "y": 0.7693247566912358, - "heading": -0.04169615417860038, - "angularVelocity": -0.23496103854227787, - "velocityX": -0.8090615316878346, - "velocityY": 0.25865416637184857, - "timestamp": 9.570411622523244 - }, - { - "x": 7.869108284660135, - "y": 0.7937768147652166, - "heading": -0.06242431663850855, - "angularVelocity": -0.2911147731440275, - "velocityX": -1.0734388671280197, - "velocityY": 0.34341468294054744, - "timestamp": 9.641614338332031 - }, - { - "x": 7.773850362284446, - "y": 0.8242647742315067, - "heading": -0.08699338329336499, - "angularVelocity": -0.34505799920387553, - "velocityX": -1.3378411384124769, - "velocityY": 0.4281853454601993, - "timestamp": 9.71281705414082 - }, - { - "x": 7.65976424379608, - "y": 0.8607894982947099, - "heading": -0.11521425502187198, - "angularVelocity": -0.39634544002927347, - "velocityX": -1.6022720087635367, - "velocityY": 0.5129681311775954, - "timestamp": 9.784019769949607 - }, - { - "x": 7.526847585445219, - "y": 0.9033520367557786, - "heading": -0.14685571507130035, - "angularVelocity": -0.44438557841539145, - "velocityX": -1.8667357956935873, - "velocityY": 0.59776566072801, - "timestamp": 9.855222485758395 - }, - { - "x": 7.375097685187462, - "y": 0.9519536884245254, - "heading": -0.18162855174599865, - "angularVelocity": -0.48836390971499116, - "velocityX": -2.1312375312379404, - "velocityY": 0.6825814312935032, - "timestamp": 9.926425201567183 - }, - { - "x": 7.204511441301595, - "y": 1.0065960860089886, - "heading": -0.21916015731822874, - "angularVelocity": -0.5271091860179415, - "velocityX": -2.395782828620887, - "velocityY": 0.7674201322770242, - "timestamp": 9.99762791737597 - }, - { - "x": 7.01508536939227, - "y": 1.0672813074513483, - "heading": -0.25895131555418466, - "angularVelocity": -0.5588432657936879, - "velocityX": -2.6603770622741663, - "velocityY": 0.8522880167285697, - "timestamp": 10.068830633184758 - }, - { - "x": 6.806815850395879, - "y": 1.13401199877149, - "heading": -0.3002964260461437, - "angularVelocity": -0.5806676054744402, - "velocityX": -2.925022123533756, - "velocityY": 0.9371930629632811, - "timestamp": 10.140033348993546 - }, - { - "x": 6.579700283529664, - "y": 1.2067913837362447, - "heading": -0.34211833205863673, - "angularVelocity": -0.5873639163541298, - "velocityX": -3.1897037112478492, - "velocityY": 1.0221433850950452, - "timestamp": 10.211236064802334 - }, - { - "x": 6.3337423865313776, - "y": 1.285622359058628, - "heading": -0.38256120014941036, - "angularVelocity": -0.5679961449699128, - "velocityX": -3.454333085535639, - "velocityY": 1.107134389846602, - "timestamp": 10.282438780611121 - }, - { - "x": 6.0689850231424955, - "y": 1.3704991616653164, - "heading": -0.41762738854860754, - "angularVelocity": -0.4924838610562412, - "velocityX": -3.718360463944625, - "velocityY": 1.1920444556443872, - "timestamp": 10.35364149641991 - }, - { - "x": 5.786063768357388, - "y": 1.4612805523811225, - "heading": -0.43124692378177526, - "angularVelocity": -0.19127831120575167, - "velocityX": -3.9734615677424734, - "velocityY": 1.2749709008234504, - "timestamp": 10.424844212228697 + "heading": -0.34555563246426124, + "angularVelocity": 2.6730250915052296e-7, + "velocityX": 3.52891551864971, + "velocityY": -1.360125725304771, + "timestamp": 7.940887858519376 + }, + { + "x": 5.655413318775525, + "y": 1.5130951799644636, + "heading": -0.34555563491945995, + "angularVelocity": -5.785220393859716e-8, + "velocityX": 3.5912523855702734, + "velocityY": -1.18578710714545, + "timestamp": 7.983327014878419 + }, + { + "x": 5.810103118724588, + "y": 1.4702901058122384, + "heading": -0.3455556373457815, + "angularVelocity": -5.7171766263130695e-8, + "velocityX": 3.6449782045702985, + "velocityY": -1.0086221740622332, + "timestamp": 8.025766171237462 + }, + { + "x": 5.966313456902953, + "y": 1.4334181808859694, + "heading": -0.34555563976573417, + "angularVelocity": -5.702169541040829e-8, + "velocityX": 3.680806867525754, + "velocityY": -0.8688185178405917, + "timestamp": 8.068205327596505 + }, + { + "x": 6.1225244823663525, + "y": 1.3965491678093302, + "heading": -0.3455556421856862, + "angularVelocity": -5.7021679533312126e-8, + "velocityX": 3.680823062122731, + "velocityY": -0.868749905505203, + "timestamp": 8.110644483955548 + }, + { + "x": 6.278735507957822, + "y": 1.3596801552753135, + "heading": -0.3455556446056381, + "angularVelocity": -5.702167859922657e-8, + "velocityX": 3.680823065140465, + "velocityY": -0.8687498927193055, + "timestamp": 8.153083640314591 + }, + { + "x": 6.434946533537413, + "y": 1.3228111426909752, + "heading": -0.34555564702559005, + "angularVelocity": -5.7021678932866206e-8, + "velocityX": 3.680823064860606, + "velocityY": -0.868749893905046, + "timestamp": 8.195522796673634 + }, + { + "x": 6.591157495254782, + "y": 1.285941859528607, + "heading": -0.3455556494455419, + "angularVelocity": -5.7021677677252914e-8, + "velocityX": 3.6808215600657594, + "velocityY": -0.8687562695744264, + "timestamp": 8.237961953032677 + }, + { + "x": 6.747031505398479, + "y": 1.2476730535838276, + "heading": -0.34555565194208476, + "angularVelocity": -5.8826401859372655e-8, + "velocityX": 3.6728819212374164, + "velocityY": -0.9017334279932018, + "timestamp": 8.28040110939172 + }, + { + "x": 6.897830261276489, + "y": 1.2034176695315386, + "heading": -0.3468060133183351, + "angularVelocity": -0.029462446559305244, + "velocityX": 3.5532929684610997, + "velocityY": -1.0427960367044076, + "timestamp": 8.322840265750763 + }, + { + "x": 7.040966542913821, + "y": 1.160898981796106, + "heading": -0.34756195184747823, + "angularVelocity": -0.017812289262957588, + "velocityX": 3.3727409759603195, + "velocityY": -1.001874009363348, + "timestamp": 8.365279422109806 + }, + { + "x": 7.176448137174031, + "y": 1.12015095875213, + "heading": -0.34781665101790943, + "angularVelocity": -0.006001513514462224, + "velocityX": 3.1923724664555144, + "velocityY": -0.9601515802821338, + "timestamp": 8.407718578468849 + }, + { + "x": 7.30427768133993, + "y": 1.0811849503221362, + "heading": -0.3475678387477372, + "angularVelocity": 0.005862799629361019, + "velocityX": 3.0120660996283153, + "velocityY": -0.918161711329369, + "timestamp": 8.450157734827892 + }, + { + "x": 7.424456501916377, + "y": 1.044006636667021, + "heading": -0.3468143875407301, + "angularVelocity": 0.017753680130510166, + "velocityX": 2.8317909894275823, + "velocityY": -0.8760379999211017, + "timestamp": 8.492596891186935 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.02966022853563592, + "velocityX": 2.65153469287831, + "velocityY": -0.8338339406813241, + "timestamp": 8.535036047545978 + }, + { + "x": 7.689789531297121, + "y": 0.9591642629851495, + "heading": -0.34249163023542684, + "angularVelocity": 0.04769743470803589, + "velocityX": 2.378707539440784, + "velocityY": -0.769870356769085, + "timestamp": 8.599274349827635 + }, + { + "x": 7.82516177952376, + "y": 0.9142093996063804, + "heading": -0.3392101704632614, + "angularVelocity": 0.05108260423473965, + "velocityX": 2.1073447369933547, + "velocityY": -0.6998140016475143, + "timestamp": 8.663512652109292 + }, + { + "x": 7.943169958712402, + "y": 0.8740090692537413, + "heading": -0.33632582750820933, + "angularVelocity": 0.044900672225200826, + "velocityX": 1.837037639494695, + "velocityY": -0.6258000122166694, + "timestamp": 8.727750954390949 + }, + { + "x": 8.043864833075729, + "y": 0.8387415563520763, + "heading": -0.3342715201633078, + "angularVelocity": 0.03197947753809357, + "velocityX": 1.567520790350633, + "velocityY": -0.5490106626266708, + "timestamp": 8.791989256672606 + }, + { + "x": 8.12728568078066, + "y": 0.8085387560486261, + "heading": -0.33336873570137193, + "angularVelocity": 0.014053678722353937, + "velocityX": 1.2986153858669556, + "velocityY": -0.4701680964578417, + "timestamp": 8.856227558954263 + }, + { + "x": 8.193463737140812, + "y": 0.7835021726798679, + "heading": -0.33386568134146205, + "angularVelocity": -0.007735970946293819, + "velocityX": 1.030196222652151, + "velocityY": -0.38974540857234485, + "timestamp": 8.92046586123592 + }, + { + "x": 8.242424408463135, + "y": 0.7637123264900073, + "heading": -0.33595980135953535, + "angularVelocity": -0.03259924287680334, + "velocityX": 0.7621725603464993, + "velocityY": -0.3080692591016908, + "timestamp": 8.984704163517577 + }, + { + "x": 8.274188750584008, + "y": 0.7492346436854774, + "heading": -0.3398119288829893, + "angularVelocity": -0.05996620998114242, + "velocityX": 0.4944766750154696, + "velocityY": -0.2253746174836856, + "timestamp": 9.048942465799234 + }, + { + "x": 8.288774490356445, + "y": 0.7401233315467834, + "heading": -0.34555563246426124, + "angularVelocity": -0.08941244362418393, + "velocityX": 0.2270567442533848, + "velocityY": -0.14183612914838375, + "timestamp": 9.113180768080891 + }, + { + "x": 8.286058974983396, + "y": 0.7364361738247144, + "heading": -0.3533713967772531, + "angularVelocity": -0.1208300500196775, + "velocityX": -0.04198128874091145, + "velocityY": -0.05700267231030528, + "timestamp": 9.177864713107141 + }, + { + "x": 8.26594084231072, + "y": 0.7382366486349878, + "heading": -0.3631834148956511, + "angularVelocity": -0.15169170826572306, + "velocityX": -0.3110220420927156, + "velocityY": 0.027834956719829915, + "timestamp": 9.24254865813339 + }, + { + "x": 8.22841989282902, + "y": 0.7455250700569162, + "heading": -0.3749497746847549, + "angularVelocity": -0.18190541384463682, + "velocityX": -0.5800658798172126, + "velocityY": 0.11267744134917045, + "timestamp": 9.30723260315964 + }, + { + "x": 8.173495898954863, + "y": 0.7583018091084519, + "heading": -0.38862097650887273, + "angularVelocity": -0.21135386560868902, + "velocityX": -0.8491132359330754, + "velocityY": 0.1975256618369646, + "timestamp": 9.37191654818589 + }, + { + "x": 8.101168599327213, + "y": 0.7765673113174031, + "heading": -0.4041376276128682, + "angularVelocity": -0.23988411804039605, + "velocityX": -1.11816463263485, + "velocityY": 0.2823807700896826, + "timestamp": 9.43660049321214 + }, + { + "x": 8.011437691690773, + "y": 0.8003221216608263, + "heading": -0.42142708913788907, + "angularVelocity": -0.26729138920027884, + "velocityX": -1.3872207021390834, + "velocityY": 0.36724430357151194, + "timestamp": 9.50128443823839 + }, + { + "x": 7.904302824322884, + "y": 0.8295669211666158, + "heading": -0.44039841224325665, + "angularVelocity": -0.29329261067284323, + "velocityX": -1.6562822092006166, + "velocityY": 0.45211836559939883, + "timestamp": 9.56596838326464 + }, + { + "x": 7.779763586783285, + "y": 0.8643025828479174, + "heading": -0.46093432615418145, + "angularVelocity": -0.317480850968364, + "velocityX": -1.9253500615811048, + "velocityY": 0.5370059242243979, + "timestamp": 9.63065232829089 + }, + { + "x": 7.63781950374517, + "y": 0.9045302615405436, + "heading": -0.48287779045070706, + "angularVelocity": -0.33924127985113656, + "velocityX": -2.194425262412658, + "velocityY": 0.6219113363648586, + "timestamp": 9.695336273317139 + }, + { + "x": 7.478470046612931, + "y": 0.9502515474489494, + "heading": -0.5060075980261466, + "angularVelocity": -0.3575818940241346, + "velocityX": -2.4635086352196294, + "velocityY": 0.7068413327271726, + "timestamp": 9.760020218343389 + }, + { + "x": 7.301714722310279, + "y": 1.0014687500113455, + "heading": -0.5299890975728885, + "angularVelocity": -0.37074887032647375, + "velocityX": -2.732599630880914, + "velocityY": 0.7918070325118697, + "timestamp": 9.824704163369638 + }, + { + "x": 7.107553521342319, + "y": 1.0581854755780744, + "heading": -0.5542575516052335, + "angularVelocity": -0.37518512549746846, + "velocityX": -3.0016907733312475, + "velocityY": 0.8768284857040194, + "timestamp": 9.889388108395888 + }, + { + "x": 6.895989597473325, + "y": 1.1204078946209601, + "heading": -0.5776591781660143, + "angularVelocity": -0.3617841575878507, + "velocityX": -3.2707331592582385, + "velocityY": 0.961945333075069, + "timestamp": 9.954072053422138 + }, + { + "x": 6.667061067423103, + "y": 1.188145012147935, + "heading": -0.5965312726664429, + "angularVelocity": -0.2917585575952417, + "velocityX": -3.5391862688232427, + "velocityY": 1.0472013959489617, + "timestamp": 10.018755998448388 + }, + { + "x": 6.433742027972299, + "y": 1.2616769248691375, + "heading": -0.5965312781733252, + "angularVelocity": -8.513522502240808e-8, + "velocityX": -3.607062608134345, + "velocityY": 1.136787694247182, + "timestamp": 10.083439943474637 + }, + { + "x": 6.200423078644191, + "y": 1.3352091235520722, + "heading": -0.5965312836800714, + "angularVelocity": -8.513312137357674e-8, + "velocityX": -3.6070612148566714, + "velocityY": 1.1367921151546025, + "timestamp": 10.148123888500887 + }, + { + "x": 5.967104129330135, + "y": 1.4087413222795935, + "heading": -0.5965312891868175, + "angularVelocity": -8.513312220445073e-8, + "velocityX": -3.607061214639434, + "velocityY": 1.136792115843899, + "timestamp": 10.212807833527137 + }, + { + "x": 5.733785345709214, + "y": 1.4822740467522528, + "heading": -0.5965312946935636, + "angularVelocity": -8.513311865132804e-8, + "velocityX": -3.6070586530588855, + "velocityY": 1.1368002437516491, + "timestamp": 10.277491778553387 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.431246936810795, - "angularVelocity": -1.829848702149455e-7, - "velocityX": -3.97541256513325, - "velocityY": 1.4344738240927952, - "timestamp": 10.496046928037485 - }, - { - "x": 5.267799654684855, - "y": 1.6634628697072642, - "heading": -0.4312469486344858, - "angularVelocity": -1.9550516647777393e-7, - "velocityX": -3.889105914989318, - "velocityY": 1.6542293543074849, - "timestamp": 10.556524566784258 - }, - { - "x": 5.036859662555262, - "y": 1.772990919381671, - "heading": -0.43124696041084615, - "angularVelocity": -1.9472255536128903e-7, - "velocityX": -3.8186013362155586, - "velocityY": 1.8110503641356064, - "timestamp": 10.617002205531032 - }, - { - "x": 4.805920651459889, - "y": 1.8825210375437151, - "heading": -0.4312469721872024, - "angularVelocity": -1.9472248822096454e-7, - "velocityX": -3.8185851147783296, - "velocityY": 1.8110845666554114, - "timestamp": 10.677479844277805 - }, - { - "x": 4.574981640487933, - "y": 1.9920511559659781, - "heading": -0.4312469839635587, - "angularVelocity": -1.947224875225229e-7, - "velocityX": -3.8185851127376225, - "velocityY": 1.8110845709581442, - "timestamp": 10.737957483024578 - }, - { - "x": 4.344043475908902, - "y": 2.101581482580224, - "heading": -0.4312495820185635, - "angularVelocity": -0.00004295893592741073, - "velocityX": -3.8185711175992116, - "velocityY": 1.811088013420298, - "timestamp": 10.798435121771352 - }, - { - "x": 4.124944651592417, - "y": 2.2054837302340013, - "heading": -0.4555627986647105, - "angularVelocity": -0.402019939104465, - "velocityX": -3.6228071871964698, - "velocityY": 1.7180275190442889, - "timestamp": 10.858912760518125 - }, - { - "x": 3.9186815541610502, - "y": 2.3032986019181108, - "heading": -0.48568611973278303, - "angularVelocity": -0.49809023123757634, - "velocityX": -3.4105679670301026, - "velocityY": 1.6173725315842862, - "timestamp": 10.919390399264898 - }, - { - "x": 3.725312007029145, - "y": 2.394998463425361, - "heading": -0.5178804694719192, - "angularVelocity": -0.5323347671351059, - "velocityX": -3.197372634562718, - "velocityY": 1.5162606114832984, - "timestamp": 10.979868038011672 - }, - { - "x": 3.5448441178456083, - "y": 2.4805793192106775, - "heading": -0.5504848182217125, - "angularVelocity": -0.5391141159845556, - "velocityX": -2.9840432418199043, - "velocityY": 1.4150826248963195, - "timestamp": 11.040345676758445 - }, - { - "x": 3.3772776543095704, - "y": 2.5600412902138987, - "heading": -0.5825517458866412, - "angularVelocity": -0.530227838411517, - "velocityX": -2.7707176901805863, - "velocityY": 1.3139066380540503, - "timestamp": 11.100823315505219 - }, - { - "x": 3.222610601114179, - "y": 2.6333854258294824, - "heading": -0.6134664857875873, - "angularVelocity": -0.5111763709954703, - "velocityX": -2.5574254617148258, - "velocityY": 1.2127480029880529, - "timestamp": 11.161300954251992 - }, - { - "x": 3.0808406790679506, - "y": 2.7006129397280647, - "heading": -0.6427968546636391, - "angularVelocity": -0.48497873732909724, - "velocityX": -2.344170919764792, - "velocityY": 1.111609436011059, - "timestamp": 11.221778592998765 - }, - { - "x": 2.951965745045311, - "y": 2.761724997445756, - "heading": -0.6702218815330417, - "angularVelocity": -0.45347383657350077, - "velocityX": -2.130951814475617, - "velocityY": 1.0104901412168834, - "timestamp": 11.282256231745539 - }, - { - "x": 2.8359838859812436, - "y": 2.8167226593903436, - "heading": -0.6954934767072644, - "angularVelocity": -0.4178667636155553, - "velocityX": -1.917764341787469, - "velocityY": 0.9093883803048015, - "timestamp": 11.342733870492312 - }, - { - "x": 2.732893421596196, - "y": 2.865606872368782, - "heading": -0.7184140024714569, - "angularVelocity": -0.3789917437115094, - "velocityX": -1.7046046525840333, - "velocityY": 0.8083022748808345, - "timestamp": 11.403211509239085 - }, - { - "x": 2.6426928809449626, - "y": 2.9083784763416602, - "heading": -0.7388222811864813, - "angularVelocity": -0.33745164556568824, - "velocityX": -1.4914692855141438, - "velocityY": 0.7072300582363449, - "timestamp": 11.463689147985859 - }, - { - "x": 2.565380973908943, - "y": 2.945038215167831, - "heading": -0.756584421622472, - "angularVelocity": -0.29369765096755573, - "velocityX": -1.2783552506031493, - "velocityY": 0.6061701413256031, - "timestamp": 11.524166786732632 - }, - { - "x": 2.500956564421106, - "y": 2.975586747465841, - "heading": -0.7715875591190221, - "angularVelocity": -0.24807743502306231, - "velocityX": -1.0652600006026816, - "velocityY": 0.5051211146969536, - "timestamp": 11.584644425479405 - }, - { - "x": 2.4494186472691353, - "y": 3.0000246563455213, - "heading": -0.7837354404326269, - "angularVelocity": -0.2008656681268564, - "velocityX": -0.8521813718251273, - "velocityY": 0.40408172981098234, - "timestamp": 11.645122064226179 - }, - { - "x": 2.410766328604012, - "y": 3.018352457725689, - "heading": -0.7929452205270133, - "angularVelocity": -0.152284055482875, - "velocityX": -0.6391175228742725, - "velocityY": 0.3030508756618002, - "timestamp": 11.705599702972952 - }, - { - "x": 2.3849988097281045, - "y": 3.030570607297698, - "heading": -0.7991450800016932, - "angularVelocity": -0.10251490638778926, - "velocityX": -0.42606688041837987, - "velocityY": 0.20202755638603895, - "timestamp": 11.766077341719726 + "heading": -0.5965313002103377, + "angularVelocity": -8.528815258110653e-8, + "velocityX": -3.567836630187545, + "velocityY": 1.254483436778072, + "timestamp": 10.342175723579636 + }, + { + "x": 5.285431093647359, + "y": 1.6581112689784743, + "heading": -0.5965313049795081, + "angularVelocity": -7.601302145092289e-8, + "velocityX": -3.4677610362467894, + "velocityY": 1.509245007513348, + "timestamp": 10.404917216242477 + }, + { + "x": 5.070948144949037, + "y": 1.7596075143791599, + "heading": -0.5965313097283005, + "angularVelocity": -7.568822719486902e-8, + "velocityX": -3.4185184252932688, + "velocityY": 1.6176893646140194, + "timestamp": 10.467658708905319 + }, + { + "x": 4.856465419222307, + "y": 1.861104230964457, + "heading": -0.5965313144770923, + "angularVelocity": -7.568821760685447e-8, + "velocityX": -3.4185148714793154, + "velocityY": 1.6176968745502942, + "timestamp": 10.53040020156816 + }, + { + "x": 4.641982693508075, + "y": 1.9626009475761665, + "heading": -0.5965313192258841, + "angularVelocity": -7.568821790279648e-8, + "velocityX": -3.418514871280104, + "velocityY": 1.6176968749712675, + "timestamp": 10.593141694231 + }, + { + "x": 4.427499967793844, + "y": 2.064097664187878, + "heading": -0.5965313239746759, + "angularVelocity": -7.568821822441872e-8, + "velocityX": -3.4185148712800926, + "velocityY": 1.617696874971291, + "timestamp": 10.655883186893842 + }, + { + "x": 4.213017242079613, + "y": 2.165594380799589, + "heading": -0.5965313287234677, + "angularVelocity": -7.568821811615467e-8, + "velocityX": -3.4185148712800917, + "velocityY": 1.617696874971293, + "timestamp": 10.718624679556683 + }, + { + "x": 3.998534516366738, + "y": 2.267091097414165, + "heading": -0.5965313334722594, + "angularVelocity": -7.56882168459371e-8, + "velocityX": -3.418514871258486, + "velocityY": 1.6176968750169503, + "timestamp": 10.781366172219524 + }, + { + "x": 3.7840518148370346, + "y": 2.3685878651306878, + "heading": -0.5965313382224369, + "angularVelocity": -7.571030356184493e-8, + "velocityX": -3.418514485817036, + "velocityY": 1.6176976895010247, + "timestamp": 10.844107664882365 + }, + { + "x": 3.5823467745214304, + "y": 2.46403001529431, + "heading": -0.6258886394381601, + "angularVelocity": -0.46790887449049295, + "velocityX": -3.214858808022383, + "velocityY": 1.5211966772373215, + "timestamp": 10.906849157545206 + }, + { + "x": 3.3961574491843427, + "y": 2.552130310684733, + "heading": -0.6529986278149837, + "angularVelocity": -0.43209026795882993, + "velocityX": -2.967562890759225, + "velocityY": 1.40417914288166, + "timestamp": 10.969590650208048 + }, + { + "x": 3.225483861981861, + "y": 2.6328887858865286, + "heading": -0.6778585687680168, + "angularVelocity": -0.3962280764760425, + "velocityX": -2.7202666044246855, + "velocityY": 1.2871621597492864, + "timestamp": 11.032332142870889 + }, + { + "x": 3.0703260256030673, + "y": 2.7063054766361296, + "heading": -0.7004659993691003, + "angularVelocity": -0.36032662982009556, + "velocityX": -2.4729701158463, + "velocityY": 1.17014574619904, + "timestamp": 11.09507363553373 + }, + { + "x": 2.9306839491479293, + "y": 2.7723804156875507, + "heading": -0.7208186227896802, + "angularVelocity": -0.32438857535555815, + "velocityX": -2.2256734822288196, + "velocityY": 1.0531298546958971, + "timestamp": 11.157815128196571 + }, + { + "x": 2.8065576397395007, + "y": 2.8311136320269066, + "heading": -0.7389143343573537, + "angularVelocity": -0.28841697574707026, + "velocityX": -1.9783767350811556, + "velocityY": 0.9361144251855119, + "timestamp": 11.220556620859412 + }, + { + "x": 2.697947103240289, + "y": 2.8825051506567756, + "heading": -0.7547512635952348, + "angularVelocity": -0.25241556370017226, + "velocityX": -1.7310798944944172, + "velocityY": 0.8190993941766099, + "timestamp": 11.283298113522253 + }, + { + "x": 2.6048523446905505, + "y": 2.9265549925489487, + "heading": -0.7683278153992709, + "angularVelocity": -0.21638872822158642, + "velocityX": -1.4837829735739574, + "velocityY": 0.702084697424841, + "timestamp": 11.346039606185094 + }, + { + "x": 2.527273368620795, + "y": 2.9632631746604594, + "heading": -0.779642706302846, + "angularVelocity": -0.18034143631837077, + "velocityX": -1.2364859804443729, + "velocityY": 0.5850702709413225, + "timestamp": 11.408781098847935 + }, + { + "x": 2.4652101792880994, + "y": 2.9926297099741825, + "heading": -0.7886949946743065, + "angularVelocity": -0.1442791362982944, + "velocityX": -0.9891889194637066, + "velocityY": 0.46805605138424683, + "timestamp": 11.471522591510777 + }, + { + "x": 2.4186627808554446, + "y": 3.014654607545873, + "heading": -0.7954841045559687, + "angularVelocity": -0.10820765642515946, + "velocityX": -0.7418917921317392, + "velocityY": 0.3510419761615724, + "timestamp": 11.534264084173618 + }, + { + "x": 2.3876311775224326, + "y": 3.0293378725476474, + "heading": -0.8000098431420335, + "angularVelocity": -0.07213310353301693, + "velocityX": -0.49459459786475296, + "velocityY": 0.23402798337424993, + "timestamp": 11.597005576836459 }, { "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": -0.05171055025787837, - "velocityX": -0.21302809407950682, - "velocityY": 0.10101087163407141, - "timestamp": 11.826554980466499 + "angularVelocity": -0.036061762982130895, + "velocityX": -0.24729733470577522, + "velocityY": 0.11701401166345779, + "timestamp": 11.6597470694993 }, { "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": -1.423507620090277e-32, - "velocityY": 0, - "timestamp": 11.887032619213272 - }, - { - "x": 2.3839325851596245, - "y": 3.031084448154912, - "heading": -0.7992419373744997, - "angularVelocity": 0.052323339006150715, - "velocityX": 0.20403271649422824, - "velocityY": -0.09660273136476924, - "timestamp": 11.944950838169635 - }, - { - "x": 2.407567371385834, - "y": 3.019894305720653, - "heading": -0.7932053890740913, - "angularVelocity": 0.10422537863183293, - "velocityX": 0.4080717026885203, - "velocityY": -0.19320591406117554, - "timestamp": 12.002869057125999 - }, - { - "x": 2.4430201410442214, - "y": 3.003109041895344, - "heading": -0.7841908751963366, - "angularVelocity": 0.15564211124217459, - "velocityX": 0.6121177463191404, - "velocityY": -0.2898097373808346, - "timestamp": 12.060787276082362 - }, - { - "x": 2.490291356941472, - "y": 2.980728609584666, - "heading": -0.7722307955425777, - "angularVelocity": 0.2064994378154119, - "velocityX": 0.8161717806423892, - "velocityY": -0.38641437381801885, - "timestamp": 12.118705495038725 - }, - { - "x": 2.5493815465754834, - "y": 2.952752953058649, - "heading": -0.7573625934321272, - "angularVelocity": 0.2567102783608131, - "velocityX": 1.0202349225989056, - "velocityY": -0.4830199724735087, - "timestamp": 12.176623713995088 - }, - { - "x": 2.6202913158109356, - "y": 2.9191820098836145, - "heading": -0.7396297457639593, - "angularVelocity": 0.30617045875544285, - "velocityX": 1.2243085252479435, - "velocityY": -0.5796266490916698, - "timestamp": 12.234541932951451 - }, - { - "x": 2.7030213667788137, - "y": 2.8800157137424582, - "heading": -0.7190830975629909, - "angularVelocity": 0.35475276296822733, - "velocityX": 1.428394250696986, - "velocityY": -0.6762344707226781, - "timestamp": 12.292460151907814 - }, - { - "x": 2.797572521744028, - "y": 2.8352539986604253, - "heading": -0.6957827053311118, - "angularVelocity": 0.40229814817741966, - "velocityX": 1.632494173145279, - "velocityY": -0.7728434314556114, - "timestamp": 12.350378370864178 - }, - { - "x": 2.9039457555692096, - "y": 2.7848968055361296, - "heading": -0.6698004526247588, - "angularVelocity": 0.448602411720027, - "velocityX": 1.8366109273029618, - "velocityY": -0.8694534126167829, - "timestamp": 12.40829658982054 - }, - { - "x": 3.0221422408423324, - "y": 2.7289440926274007, - "heading": -0.6412238784283038, - "angularVelocity": 0.4933952512936468, - "velocityX": 2.040747927041322, - "velocityY": -0.9660641144867576, - "timestamp": 12.466214808776904 - }, - { - "x": 3.1521634121347564, - "y": 2.6673958532051016, - "heading": -0.6101619934003036, - "angularVelocity": 0.5363059428917012, - "velocityX": 2.2449096956932593, - "velocityY": -1.0626749325401574, - "timestamp": 12.524133027733267 - }, - { - "x": 3.2940110599022505, - "y": 2.600252147103853, - "heading": -0.5767545340853645, - "angularVelocity": 0.576803981837031, - "velocityX": 2.4491023778608563, - "velocityY": -1.1592847175054986, - "timestamp": 12.58205124668963 - }, - { - "x": 3.4476874712025682, - "y": 2.527513161589764, - "heading": -0.54118758447353, - "angularVelocity": 0.6140891459150629, - "velocityX": 2.6533345477370784, - "velocityY": -1.2558912691858224, - "timestamp": 12.639969465645994 - }, - { - "x": 3.613195643589333, - "y": 2.4491793412538243, - "heading": -0.5037220986038603, - "angularVelocity": 0.6468687495017285, - "velocityX": 2.8576184725476987, - "velocityY": -1.3524901446807027, - "timestamp": 12.697887684602357 - }, - { - "x": 3.7905395971194613, - "y": 2.3652517067540755, - "heading": -0.46475198370690785, - "angularVelocity": 0.6728472594489399, - "velocityX": 3.0619718065526835, - "velocityY": -1.4490713977752308, - "timestamp": 12.75580590355872 - }, - { - "x": 3.979724665259552, - "y": 2.275732818118546, - "heading": -0.4249432999516913, - "angularVelocity": 0.6873257581558144, - "velocityX": 3.266417226721487, - "velocityY": -1.545608450131651, - "timestamp": 12.813724122515083 - }, - { - "x": 4.180755824131631, - "y": 2.1806309244718047, - "heading": -0.3856731863911582, - "angularVelocity": 0.6780269536623745, - "velocityX": 3.47094856324124, - "velocityY": -1.6420030753776067, - "timestamp": 12.871642341471446 - }, - { - "x": 4.393589257468926, - "y": 2.0800009314187498, - "heading": -0.3515823344877535, - "angularVelocity": 0.5886032498528599, - "velocityX": 3.6747233801793717, - "velocityY": -1.7374497155872817, - "timestamp": 12.92956056042781 - }, - { - "x": 4.614791128841965, - "y": 1.9751825525280107, - "heading": -0.35158232270208695, - "angularVelocity": 2.034880699916496e-7, - "velocityX": 3.8192105240614955, - "velocityY": -1.8097652306904055, - "timestamp": 12.987478779384173 - }, - { - "x": 4.835992849247887, - "y": 1.870363855043589, - "heading": -0.3515823109197321, - "angularVelocity": 2.034308907507492e-7, - "velocityX": 3.8192079175048774, - "velocityY": -1.8097707314410818, - "timestamp": 13.045396998340536 - }, - { - "x": 5.0571945697179865, - "y": 1.7655451576946017, - "heading": -0.35158229913737715, - "angularVelocity": 2.0343089162960125e-7, - "velocityX": 3.8192079186129373, - "velocityY": -1.8097707291027114, - "timestamp": 13.103315217296899 - }, - { - "x": 5.278396987351414, - "y": 1.6607279316008212, - "heading": -0.35158228735502095, - "angularVelocity": 2.03430914726069e-7, - "velocityX": 3.8192199556427555, - "velocityY": -1.8097453268159511, - "timestamp": 13.161233436253262 + "angularVelocity": 2.8180807610025885e-36, + "velocityX": -1.7033240054088048e-37, + "velocityY": 8.364918339692578e-38, + "timestamp": 11.722488562162141 + }, + { + "x": 2.3862784269967885, + "y": 3.0299921206795677, + "heading": -0.7979445022798209, + "angularVelocity": 0.07214760590413402, + "velocityX": 0.2361025217555023, + "velocityY": -0.11148080618080956, + "timestamp": 11.782475441082498 + }, + { + "x": 2.414605712310544, + "y": 3.016617241283919, + "heading": -0.7893555783917804, + "angularVelocity": 0.1431800427464117, + "velocityX": 0.4722246901920792, + "velocityY": -0.22296341527296182, + "timestamp": 11.842462320002856 + }, + { + "x": 2.457098595113841, + "y": 2.996554734272641, + "heading": -0.7765849369819304, + "angularVelocity": 0.2128905794016223, + "velocityX": 0.7083696229589314, + "velocityY": -0.33444825555792773, + "timestamp": 11.902449198923213 + }, + { + "x": 2.5137586742709717, + "y": 2.9698044458506936, + "heading": -0.759727315329062, + "angularVelocity": 0.281021816041631, + "velocityX": 0.944541209292721, + "velocityY": -0.44593565965421617, + "timestamp": 11.96243607784357 + }, + { + "x": 2.5845878442638655, + "y": 2.9363662114763, + "heading": -0.7388971783301493, + "angularVelocity": 0.34724488711220036, + "velocityX": 1.180744377231747, + "velocityY": -0.5574258067133014, + "timestamp": 12.022422956763927 + }, + { + "x": 2.669588381015972, + "y": 2.8962398712988433, + "heading": -0.7142349782126004, + "angularVelocity": 0.41112657570152883, + "velocityX": 1.4169854855252446, + "velocityY": -0.6689186185320778, + "timestamp": 12.082409835684285 + }, + { + "x": 2.768763062860025, + "y": 2.849425297465716, + "heading": -0.6859166617272396, + "angularVelocity": 0.4720751770225952, + "velocityX": 1.653272909492815, + "velocityY": -0.7804135616937369, + "timestamp": 12.142396714604642 + }, + { + "x": 2.8821153449579997, + "y": 2.795922445610523, + "heading": -0.6541688123580982, + "angularVelocity": 0.5292465609236296, + "velocityX": 1.8896179320892514, + "velocityY": -0.8919092444570627, + "timestamp": 12.202383593525 + }, + { + "x": 3.0096496153353156, + "y": 2.73573145985777, + "heading": -0.6192942663212868, + "angularVelocity": 0.5813695705541383, + "velocityX": 2.1260361044394105, + "velocityY": -1.0034025246198655, + "timestamp": 12.262370472445356 + }, + { + "x": 3.151371569227902, + "y": 2.6688529097388005, + "heading": -0.5817190571900679, + "angularVelocity": 0.6263904675071726, + "velocityX": 2.3625492181506123, + "velocityY": -1.1148863105173612, + "timestamp": 12.322357351365714 + }, + { + "x": 3.30728870131474, + "y": 2.5952884046594, + "heading": -0.5420886244414685, + "angularVelocity": 0.6606516868666429, + "velocityX": 2.5991872705003334, + "velocityY": -1.2263432671179575, + "timestamp": 12.382344230286071 + }, + { + "x": 3.477410464899007, + "y": 2.5150425607670805, + "heading": -0.501501110267189, + "angularVelocity": 0.6766065330414338, + "velocityX": 2.8359829123654157, + "velocityY": -1.3377232710983202, + "timestamp": 12.442331109206428 + }, + { + "x": 3.661742704544347, + "y": 2.428132084038069, + "heading": -0.46226256646132613, + "angularVelocity": 0.6541187758402722, + "velocityX": 3.0728759849311573, + "velocityY": -1.4488247812392314, + "timestamp": 12.502317988126785 + }, + { + "x": 3.860146403975295, + "y": 2.3346891355318427, + "heading": -0.4327535611710109, + "angularVelocity": 0.4919243311440368, + "velocityX": 3.307451612782871, + "velocityY": -1.5577231252568988, + "timestamp": 12.562304867047143 + }, + { + "x": 4.065242173401202, + "y": 2.237711412138144, + "heading": -0.4327535477418668, + "angularVelocity": 2.238680243649846e-7, + "velocityX": 3.41901050891823, + "velocityY": -1.6166489262168846, + "timestamp": 12.6222917459675 + }, + { + "x": 4.270337924993822, + "y": 2.140733651028875, + "heading": -0.4327535343131544, + "angularVelocity": 2.2386082909736944e-7, + "velocityX": 3.419010211631728, + "velocityY": -1.6166495549472233, + "timestamp": 12.682278624887857 + }, + { + "x": 4.475433676585232, + "y": 2.0437558899170503, + "heading": -0.43275352088444197, + "angularVelocity": 2.2386082765717578e-7, + "velocityX": 3.419010211611586, + "velocityY": -1.6166495549898217, + "timestamp": 12.742265503808214 + }, + { + "x": 4.6805294281766425, + "y": 1.9467781288052255, + "heading": -0.43275350745572955, + "angularVelocity": 2.2386082872977313e-7, + "velocityX": 3.4190102116115844, + "velocityY": -1.616649554989824, + "timestamp": 12.802252382728572 + }, + { + "x": 4.885625179768054, + "y": 1.8498003676934025, + "heading": -0.4327534940270171, + "angularVelocity": 2.2386082836686737e-7, + "velocityX": 3.419010211611597, + "velocityY": -1.6166495549897981, + "timestamp": 12.862239261648929 + }, + { + "x": 5.090720931370458, + "y": 1.7528226066048298, + "heading": -0.4327534805983047, + "angularVelocity": 2.2386082909228912e-7, + "velocityX": 3.4190102117948658, + "velocityY": -1.616649554602208, + "timestamp": 12.922226140569286 + }, + { + "x": 5.295816845235169, + "y": 1.6558451886813434, + "heading": -0.4327534671695918, + "angularVelocity": 2.2386083672590555e-7, + "velocityX": 3.4190129167581613, + "velocityY": -1.6166438339330798, + "timestamp": 12.982213019489643 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.35158227553759686, - "angularVelocity": 2.0403638579093497e-7, - "velocityX": 3.8779958008952624, - "velocityY": -1.6801094533136367, - "timestamp": 13.219151655209625 - }, - { - "x": 5.78771748375251, - "y": 1.4618238256689142, - "heading": -0.3515822643983809, - "angularVelocity": 1.5573311965555955e-7, - "velocityX": 3.980476011114963, - "velocityY": -1.420363078204806, - "timestamp": 13.29067925283814 - }, - { - "x": 6.071768529925899, - "y": 1.3687010115486713, - "heading": -0.3384707068713864, - "angularVelocity": 0.18330767370505657, - "velocityX": 3.9712090939868023, - "velocityY": -1.3019144666913607, - "timestamp": 13.362206850466656 - }, - { - "x": 6.337299554037092, - "y": 1.2816927202415032, - "heading": -0.3077191306864491, - "angularVelocity": 0.4299260314130538, - "velocityX": 3.712287745077815, - "velocityY": -1.2164296606053124, - "timestamp": 13.433734448095171 - }, - { - "x": 6.583882404182654, - "y": 1.200909898706293, - "heading": -0.27261549465283386, - "angularVelocity": 0.4907705165204766, - "velocityX": 3.4473805680740197, - "velocityY": -1.1293937474981692, - "timestamp": 13.505262045723686 - }, - { - "x": 6.811487459476513, - "y": 1.1263530850980985, - "heading": -0.23658461598105995, - "angularVelocity": 0.5037339413928503, - "velocityX": 3.182059272785126, - "velocityY": -1.042350310650886, - "timestamp": 13.576789643352202 - }, - { - "x": 7.020112866315064, - "y": 1.0580186639861602, - "heading": -0.20123048803983856, - "angularVelocity": 0.49427254812661375, - "velocityX": 2.916712062972736, - "velocityY": -0.9553574197590016, - "timestamp": 13.648317240980717 - }, - { - "x": 7.209761115265014, - "y": 0.9959034826022224, - "heading": -0.16748852563901473, - "angularVelocity": 0.47173347797958504, - "velocityX": 2.6513996728214475, - "velocityY": -0.8684086065149057, - "timestamp": 13.719844838609232 - }, - { - "x": 7.380435279840303, - "y": 0.9400050984767416, - "heading": -0.13597326201279447, - "angularVelocity": 0.4406028535992147, - "velocityX": 2.3861302522936825, - "velocityY": -0.78149394050383, - "timestamp": 13.791372436237747 - }, - { - "x": 7.5321382283527605, - "y": 0.8903216280368556, - "heading": -0.10712014952027268, - "angularVelocity": 0.403384336244213, - "velocityX": 2.1209009325371073, - "velocityY": -0.6946056080049196, - "timestamp": 13.862900033866262 - }, - { - "x": 7.664872474877682, - "y": 0.8468515953524562, - "heading": -0.08125430314522812, - "angularVelocity": 0.36162051058083977, - "velocityX": 1.8557067611062361, - "velocityY": -0.6077379099206526, - "timestamp": 13.934427631494778 - }, - { - "x": 7.77864018727303, - "y": 0.8095938204087205, - "heading": -0.05862785653621709, - "angularVelocity": 0.3163317007586838, - "velocityX": 1.590542897668813, - "velocityY": -0.520886709172551, - "timestamp": 14.005955229123293 - }, - { - "x": 7.873443230968776, - "y": 0.7785473403012081, - "heading": -0.039441988553792, - "angularVelocity": 0.2682302861906329, - "velocityX": 1.3254051140947378, - "velocityY": -0.4340489704233464, - "timestamp": 14.077482826751808 - }, - { - "x": 7.9492832154345106, - "y": 0.7537113535170058, - "heading": -0.023860737345292347, - "angularVelocity": 0.21783551699054987, - "velocityX": 1.0602898318998024, - "velocityY": -0.3472224373197895, - "timestamp": 14.149010424380323 - }, - { - "x": 8.006161534793161, - "y": 0.7350851799018175, - "heading": -0.01202009660498375, - "angularVelocity": 0.1655394719364682, - "velocityX": 0.7951940404045607, - "velocityY": -0.26040541319344906, - "timestamp": 14.220538022008839 - }, - { - "x": 8.044079401282437, - "y": 0.7226682313019233, - "heading": -0.004034250807333449, - "angularVelocity": 0.11164705739350395, - "velocityX": 0.5301151967413267, - "velocityY": -0.17359661181943797, - "timestamp": 14.292065619637354 + "heading": -0.4327534537085892, + "angularVelocity": 2.2439911588435075e-7, + "velocityX": 3.4538678416562716, + "velocityY": -1.5407736813730113, + "timestamp": 13.04219989841 + }, + { + "x": 5.767637732978587, + "y": 1.4715486447521433, + "heading": -0.43275344189521775, + "angularVelocity": 1.5949042736003556e-7, + "velocityX": 3.5727828839624562, + "velocityY": -1.2403266728714895, + "timestamp": 13.116269368711977 + }, + { + "x": 6.033774192023134, + "y": 1.384125705645109, + "heading": -0.43275343009903516, + "angularVelocity": 1.5925836231633141e-7, + "velocityX": 3.5930655094403474, + "velocityY": -1.1802830336252825, + "timestamp": 13.190338839013954 + }, + { + "x": 6.2999107025526015, + "y": 1.2967029232707166, + "heading": -0.43275341830285263, + "angularVelocity": 1.5925836222768275e-7, + "velocityX": 3.593066204529984, + "velocityY": -1.1802809176031077, + "timestamp": 13.26440830931593 + }, + { + "x": 6.566047160708243, + "y": 1.2092799814598572, + "heading": -0.4327534065048956, + "angularVelocity": 1.5928231960994732e-7, + "velocityX": 3.5930654974393894, + "velocityY": -1.1802830701291929, + "timestamp": 13.338477779617907 + }, + { + "x": 6.815604168443372, + "y": 1.1270295237235333, + "heading": -0.36800659105834677, + "angularVelocity": 0.874136336908857, + "velocityX": 3.369229005117782, + "velocityY": -1.1104501949452927, + "timestamp": 13.412547249919884 + }, + { + "x": 7.0424486777806585, + "y": 1.0523256982782683, + "heading": -0.30458898646479193, + "angularVelocity": 0.8561908750664163, + "velocityX": 3.0625912189253857, + "velocityY": -1.0085643267152118, + "timestamp": 13.48661672022186 + }, + { + "x": 7.2465905745815595, + "y": 0.9851213052520007, + "heading": -0.2456798424413219, + "angularVelocity": 0.7953228743678266, + "velocityX": 2.756086900157761, + "velocityY": -0.9073156963628867, + "timestamp": 13.560686190523837 + }, + { + "x": 7.428038548794458, + "y": 0.9253999088853845, + "heading": -0.1923064776337199, + "angularVelocity": 0.7205852099387569, + "velocityX": 2.449699902984963, + "velocityY": -0.8062889625528059, + "timestamp": 13.634755660825814 + }, + { + "x": 7.586797949690042, + "y": 0.8731533407207944, + "heading": -0.14497999296920738, + "angularVelocity": 0.6389472541327177, + "velocityX": 2.1433851254549703, + "velocityY": -0.7053725097747339, + "timestamp": 13.70882513112779 + }, + { + "x": 7.722872327084394, + "y": 0.8283767866857153, + "heading": -0.10400779573923787, + "angularVelocity": 0.5531590419497899, + "velocityX": 1.837118273420693, + "velocityY": -0.6045210510150557, + "timestamp": 13.782894601429767 + }, + { + "x": 7.836264194581955, + "y": 0.7910670998170554, + "heading": -0.06959570799723021, + "angularVelocity": 0.46459205934255987, + "velocityX": 1.5308853571555237, + "velocityY": -0.5037120788977025, + "timestamp": 13.856964071731744 + }, + { + "x": 7.926975414267803, + "y": 0.7612220655404804, + "heading": -0.04189104317473832, + "angularVelocity": 0.3740362218001813, + "velocityX": 1.2246775806013535, + "velocityY": -0.4029330053920843, + "timestamp": 13.93103354203372 + }, + { + "x": 7.99500740651227, + "y": 0.7388400306235591, + "heading": -0.021003806792755153, + "angularVelocity": 0.28199521741990785, + "velocityX": 0.9184889802384881, + "velocityY": -0.30217625191150127, + "timestamp": 14.005103012335697 + }, + { + "x": 8.040361272813582, + "y": 0.7239196956147146, + "heading": -0.007018297206773477, + "angularVelocity": 0.18881611450660732, + "velocityX": 0.6123152510259342, + "velocityY": -0.20143704211755917, + "timestamp": 14.079172482637674 }, { "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": -6.370509592451438e-38, - "angularVelocity": 0.05640131838742408, - "velocityX": 0.2650511363527046, - "velocityY": -0.08679505477643516, - "timestamp": 14.36359321726587 + "heading": 2.0779165225884802e-41, + "angularVelocity": 0.09475290127174345, + "velocityX": 0.3061531209608937, + "velocityY": -0.10071229126619018, + "timestamp": 14.15324195293965 }, { "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": -1.0565058286352088e-38, - "angularVelocity": 7.429305530105007e-37, - "velocityX": -1.702944321634373e-36, - "velocityY": 1.0409042171025374e-38, - "timestamp": 14.435120814894384 + "heading": 1.169210048317657e-41, + "angularVelocity": -1.2268302369298258e-40, + "velocityX": -9.944733020002764e-42, + "velocityY": 5.460591898189625e-42, + "timestamp": 14.227311423241627 } ], "constraints": [ @@ -29240,7 +28406,7 @@ }, { "scope": [ - 5 + 6 ], "type": "StopPoint" }, @@ -29252,7 +28418,7 @@ }, { "scope": [ - 9 + 11 ], "type": "StopPoint" } @@ -29271,7 +28437,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { "x": 2.286245346069336, @@ -29289,16 +28455,25 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 27 + "controlIntervalCount": 24 }, { - "x": 7.972879886627197, - "y": 7.4593186378479, + "x": 7.125290870666504, + "y": 7.458, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 24 + "controlIntervalCount": 10 + }, + { + "x": 8.017494201660156, + "y": 7.457698345184326, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 }, { "x": 3.408520460128784, @@ -29307,7 +28482,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 17 }, { "x": 5.851044178009033, @@ -29316,12 +28491,21 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 17 + "controlIntervalCount": 14 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { "x": 7.985864639282227, "y": 5.863699913024902, - "heading": -0.23779450135898958, + "heading": -0.344987478573796, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -29334,7 +28518,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 18 + "controlIntervalCount": 17 }, { "x": 3.408520460128784, @@ -29343,7 +28527,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 20 }, { "x": 6.829305171966553, @@ -29359,1505 +28543,1577 @@ { "x": 0.43297290802001953, "y": 6.9807281494140625, - "heading": -2.898893570682157e-41, - "angularVelocity": 0, - "velocityX": -2.361892666986074e-43, - "velocityY": -3.008824272021237e-41, + "heading": 0, + "angularVelocity": 2.735053895521923e-38, + "velocityX": -3.8673469053295564e-39, + "velocityY": 1.858643532249755e-38, "timestamp": 0 }, { - "x": 0.4587135721409647, - "y": 6.976639655231649, - "heading": 0.007035495853073787, - "angularVelocity": 0.08601037544230704, - "velocityX": 0.3146848824037811, - "velocityY": -0.04998267740708988, - "timestamp": 0.08179822279456238 - }, - { - "x": 0.5101949084110605, - "y": 6.968462527202184, - "heading": 0.02110609468280026, - "angularVelocity": 0.17201594789002958, - "velocityX": 0.6293698629540154, - "velocityY": -0.09996706224293399, - "timestamp": 0.16359644558912476 - }, - { - "x": 0.5874169050244689, - "y": 6.956196559867369, - "heading": 0.04221287349505025, - "angularVelocity": 0.25803468695475235, - "velocityX": 0.944054699175467, - "velocityY": -0.14995395884848578, - "timestamp": 0.24539466838368712 - }, - { - "x": 0.6903795277325874, - "y": 6.939841483301487, - "heading": 0.07035879886747527, - "angularVelocity": 0.34408969303787923, - "velocityX": 1.2587391167007476, - "velocityY": -0.19994415535112514, - "timestamp": 0.3271928911782495 - }, - { - "x": 0.8190827157007662, - "y": 6.919396965353516, - "heading": 0.10554932385802422, - "angularVelocity": 0.4302113638694884, - "velocityX": 1.5734227905099982, - "velocityY": -0.24993841246792703, - "timestamp": 0.4089911139728119 - }, - { - "x": 0.97352637537035, - "y": 6.894862615393891, - "heading": 0.14779318141850775, - "angularVelocity": 0.5164397968227219, - "velocityX": 1.8881053205455545, - "velocityY": -0.29993744511103754, - "timestamp": 0.4907893367673743 - }, - { - "x": 1.1537103713281627, - "y": 6.866237990592905, - "heading": 0.197103396735309, - "angularVelocity": 0.6028274653429168, - "velocityX": 2.2027861951273398, - "velocityY": -0.3499418914378741, - "timestamp": 0.5725875595619366 - }, - { - "x": 1.3596345093826188, - "y": 6.8335226068750465, - "heading": 0.2534985369685149, - "angularVelocity": 0.6894421211918415, - "velocityX": 2.51746469567729, - "velocityY": -0.39995225568682646, - "timestamp": 0.654385782356499 - }, - { - "x": 1.565548746560234, - "y": 6.800784734541898, - "heading": 0.31096699128340616, - "angularVelocity": 0.7025636053148037, - "velocityX": 2.517343655433349, - "velocityY": -0.40022718360727877, - "timestamp": 0.7361840051510614 - }, - { - "x": 1.7457232772224913, - "y": 6.772138465169413, - "heading": 0.3613042914743826, - "angularVelocity": 0.6153837879509854, - "velocityX": 2.202670479953688, - "velocityY": -0.35020650075038995, - "timestamp": 0.8179822279456238 - }, - { - "x": 1.9001583012776542, - "y": 6.747584097506383, - "heading": 0.40448766007892617, - "angularVelocity": 0.5279255114502831, - "velocityX": 1.887999748393421, - "velocityY": -0.3001821656284431, - "timestamp": 0.8997804507401862 - }, - { - "x": 2.028853992721499, - "y": 6.727121858473899, - "heading": 0.44049724757544967, - "angularVelocity": 0.4402245704892907, - "velocityX": 1.5733311439671065, - "velocityY": -0.2501550563497558, - "timestamp": 0.9815786735347486 - }, - { - "x": 2.1318104848303743, - "y": 6.710751914339462, - "heading": 0.4693170803360535, - "angularVelocity": 0.3523283486608914, - "velocityX": 1.258664168871395, - "velocityY": -0.20012591441688285, - "timestamp": 1.0633768963293109 - }, - { - "x": 2.2090278625669106, - "y": 6.698474380778823, - "heading": 0.49093579476282356, - "angularVelocity": 0.2642932045243309, - "velocityX": 0.9439982324612256, - "velocityY": -0.1500953583242809, - "timestamp": 1.1451751191238733 - }, - { - "x": 2.2605061577286842, - "y": 6.690289331413325, - "heading": 0.5053471622954702, - "angularVelocity": 0.17618191496459498, - "velocityX": 0.6293326847829228, - "velocityY": -0.10006390219570446, - "timestamp": 1.2269733419184357 + "x": 0.46193105617219093, + "y": 6.976128411049919, + "heading": 0.007924686694784022, + "angularVelocity": 0.0965568963773332, + "velocityX": 0.35283526252830383, + "velocityY": -0.05604467125266167, + "timestamp": 0.08207271559159549 + }, + { + "x": 0.5198473628641517, + "y": 6.966928755942856, + "heading": 0.023773564700110772, + "angularVelocity": 0.19310775683593598, + "velocityX": 0.7056706516226394, + "velocityY": -0.11209151592888836, + "timestamp": 0.16414543118319097 + }, + { + "x": 0.6067218129336875, + "y": 6.9531289220308485, + "heading": 0.04754806846022335, + "angularVelocity": 0.2896760950181983, + "velocityX": 1.0585058559757474, + "velocityY": -0.16814155365198188, + "timestamp": 0.24621814677478646 + }, + { + "x": 0.7225543617111448, + "y": 6.934728565336742, + "heading": 0.07925217561750111, + "angularVelocity": 0.38629289805690764, + "velocityX": 1.4113405160596262, + "velocityY": -0.22419578250180797, + "timestamp": 0.32829086236638194 + }, + { + "x": 0.8673449286095066, + "y": 6.9117272629835975, + "heading": 0.11889328943914418, + "angularVelocity": 0.48299990485147826, + "velocityX": 1.764174194245729, + "velocityY": -0.2802551638184133, + "timestamp": 0.4103635779579774 + }, + { + "x": 1.0410933872613453, + "y": 6.884124518124938, + "heading": 0.16648343863275697, + "angularVelocity": 0.5798534732349732, + "velocityX": 2.117006332731008, + "velocityY": -0.33632059887008814, + "timestamp": 0.49243629354957286 + }, + { + "x": 1.243799549589456, + "y": 6.851919767180746, + "heading": 0.22204089753091144, + "angularVelocity": 0.6769297018830912, + "velocityX": 2.4698361796239694, + "velocityY": -0.39239290075946437, + "timestamp": 0.5745090091411683 + }, + { + "x": 1.4754631289031934, + "y": 6.815112383244328, + "heading": 0.28559273385120976, + "angularVelocity": 0.7743357321882771, + "velocityX": 2.8226625333871667, + "velocityY": -0.44847284107882956, + "timestamp": 0.6565817247327638 + }, + { + "x": 1.6781593090979035, + "y": 6.782884452250807, + "heading": 0.3422497740441209, + "angularVelocity": 0.6903273491624664, + "velocityX": 2.4697145541443093, + "velocityY": -0.3926753338331338, + "timestamp": 0.7386544403243592 + }, + { + "x": 1.8518984587746443, + "y": 6.755259929387126, + "heading": 0.39086385083963315, + "angularVelocity": 0.5923293319234401, + "velocityX": 2.1168929092256374, + "velocityY": -0.33658594899118865, + "timestamp": 0.8207271559159547 + }, + { + "x": 1.9966808291576696, + "y": 6.732239139219163, + "heading": 0.43140805286474204, + "angularVelocity": 0.4940034179795163, + "velocityX": 1.7640743253029585, + "velocityY": -0.28049260953564537, + "timestamp": 0.9027998715075501 + }, + { + "x": 2.1125066103123933, + "y": 6.7138223061958735, + "heading": 0.463861007608982, + "angularVelocity": 0.39541709458878044, + "velocityX": 1.4112580571975715, + "velocityY": -0.22439653532283038, + "timestamp": 0.9848725870991456 + }, + { + "x": 2.199375929341993, + "y": 6.700009583150749, + "heading": 0.4882072246931022, + "angularVelocity": 0.29664203150374724, + "velocityX": 1.0584433377576143, + "velocityY": -0.16829859893826818, + "timestamp": 1.0669453026907412 + }, + { + "x": 2.257288845375976, + "y": 6.690801066111525, + "heading": 0.5044377142261048, + "angularVelocity": 0.19775743273523133, + "velocityX": 0.7056293387703308, + "velocityY": -0.11219949252132488, + "timestamp": 1.1490180182823366 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 0.0880612934907036, - "velocityX": 0.31466684068792017, - "velocityY": -0.05003198390964021, - "timestamp": 1.308771564712998 + "angularVelocity": 0.0988477756026137, + "velocityX": 0.3528151893675758, + "velocityY": -0.0560997894575031, + "timestamp": 1.231090733873932 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 1.0872291890838523e-40, - "velocityX": -1.1027466263693948e-42, - "velocityY": -6.469494292232001e-38, - "timestamp": 1.3905697875075604 + "angularVelocity": 0, + "velocityX": 3.087311543739499e-36, + "velocityY": 0, + "timestamp": 1.3131634494655275 }, { - "x": 2.3177158255221877, - "y": 6.704097470712436, + "x": 2.3177158256290964, + "y": 6.704097470773247, "heading": 0.5125504196, - "angularVelocity": 2.0395665276637554e-17, - "velocityX": 0.32679827958625257, - "velocityY": 0.18588554008515779, - "timestamp": 1.4868691912030012 + "angularVelocity": -9.1973743139294e-17, + "velocityX": 0.34546318060283326, + "velocityY": 0.1965022887733569, + "timestamp": 1.4042599289943862 }, { - "x": 2.3806567825560636, - "y": 6.739898802979336, + "x": 2.380656782769881, + "y": 6.7398988031009575, "heading": 0.5125504196, - "angularVelocity": 1.2188419350872931e-17, - "velocityX": 0.653596539734919, - "velocityY": 0.37177106911405733, - "timestamp": 1.583168594898442 + "angularVelocity": -1.3336684994607708e-16, + "velocityX": 0.6909263394843395, + "velocityY": 0.3930045651914449, + "timestamp": 1.495356408523245 }, { "x": 2.475068211555481, "y": 6.793600797653198, "heading": 0.5125504196, - "angularVelocity": 9.65238482155011e-17, - "velocityX": 0.9803947415708354, - "velocityY": 0.5576565649741866, - "timestamp": 1.6794679985938827 + "angularVelocity": 6.653839443641591e-19, + "velocityX": 1.0363894332018742, + "velocityY": 0.5895068045437317, + "timestamp": 1.5864528880521036 }, { - "x": 2.569479640554898, - "y": 6.84730279232706, + "x": 2.569479640341081, + "y": 6.847302792205439, "heading": 0.5125504196, - "angularVelocity": 2.650620165495025e-16, - "velocityX": 0.9803947415708352, - "velocityY": 0.5576565649741866, - "timestamp": 1.7757674022893235 + "angularVelocity": 5.174999729369457e-17, + "velocityX": 1.0363894332018742, + "velocityY": 0.5895068045437317, + "timestamp": 1.6775493675809623 }, { - "x": 2.6324205975887742, - "y": 6.88310412459396, + "x": 2.6324205974818655, + "y": 6.8831041245331495, "heading": 0.5125504196, - "angularVelocity": 1.8072666853064398e-16, - "velocityX": 0.653596539734919, - "velocityY": 0.3717710691140574, - "timestamp": 1.8720668059847643 + "angularVelocity": -1.3901806543706914e-17, + "velocityX": 0.6909263394843393, + "velocityY": 0.39300456519144494, + "timestamp": 1.768645847109821 }, { "x": 2.663891077041626, "y": 6.901004791259766, "heading": 0.5125504196, - "angularVelocity": 1.455758590389613e-16, - "velocityX": 0.3267982795862525, - "velocityY": 0.18588554008515784, - "timestamp": 1.968366209680205 + "angularVelocity": -1.8725484396545262e-17, + "velocityX": 0.3454631806028332, + "velocityY": 0.19650228877335693, + "timestamp": 1.8597423266386797 }, { "x": 2.663891077041626, "y": 6.901004791259766, "heading": 0.5125504196, - "angularVelocity": 5.011309571958317e-32, - "velocityX": 1.411034806804274e-32, - "velocityY": -7.886289900823042e-34, - "timestamp": 2.0646656133756456 - }, - { - "x": 2.6921026735754903, - "y": 6.9044448198890755, - "heading": 0.509005120328227, - "angularVelocity": -0.041542724029198924, - "velocityX": 0.3305747919676489, - "velocityY": 0.04030919509045999, - "timestamp": 2.15000665334407 - }, - { - "x": 2.7485258570604016, - "y": 6.911324890060512, - "heading": 0.5019144397700286, - "angularVelocity": -0.08308640908081097, - "velocityX": 0.6611494716467927, - "velocityY": 0.0806185414893288, - "timestamp": 2.235347693312494 - }, - { - "x": 2.8331606213159497, - "y": 6.921645023106144, - "heading": 0.4912793044898821, - "angularVelocity": -0.12461923693549357, - "velocityX": 0.9917240789057943, - "velocityY": 0.12092813785079831, - "timestamp": 2.3206887332809183 - }, - { - "x": 2.946006963275403, - "y": 6.935405249135601, - "heading": 0.4771015882559512, - "angularVelocity": -0.16613010855242188, - "velocityX": 1.3222986502297882, - "velocityY": 0.16123808702762443, - "timestamp": 2.4060297732493425 - }, - { - "x": 3.0870648825703277, - "y": 6.952605607471804, - "heading": 0.45938402488204233, - "angularVelocity": -0.20760894618186487, - "velocityX": 1.6528732172365783, - "velocityY": 0.20154849697831853, - "timestamp": 2.4913708132177668 - }, - { - "x": 3.2563343809791183, - "y": 6.973246147139034, - "heading": 0.438130093703468, - "angularVelocity": -0.24904701403261814, - "velocityX": 1.9834478050820517, - "velocityY": 0.24185948138044716, - "timestamp": 2.576711853186191 - }, - { - "x": 3.453815461723479, - "y": 6.997326927370896, - "heading": 0.41334387603725764, - "angularVelocity": -0.29043725826848604, - "velocityX": 2.314022430678465, - "velocityY": 0.2821711598636699, - "timestamp": 2.6620528931546152 - }, - { - "x": 3.679508128581092, - "y": 7.024848018097057, - "heading": 0.3850298811864858, - "angularVelocity": -0.3317746638809172, - "velocityX": 2.644597100540597, - "velocityY": 0.32248365776117877, - "timestamp": 2.7473939331230395 - }, - { - "x": 3.9334123847380087, - "y": 7.055809500355339, - "heading": 0.35319284126218836, - "angularVelocity": -0.3730566200749024, - "velocityX": 2.97517180773588, - "velocityY": 0.3627971052349304, - "timestamp": 2.8327349730914637 - }, - { - "x": 4.215528231151698, - "y": 7.090211466549705, - "heading": 0.31783747613011065, - "angularVelocity": -0.4142832703369811, - "velocityX": 3.3057465261505037, - "velocityY": 0.4031116354698126, - "timestamp": 2.918076013059888 - }, - { - "x": 4.525855663499604, - "y": 7.128054020379336, - "heading": 0.2789682373559137, - "angularVelocity": -0.45545775852483755, - "velocityX": 3.636321193914748, - "velocityY": 0.4434273808197387, - "timestamp": 3.003417053028312 - }, - { - "x": 4.864394661167, - "y": 7.169337275538201, - "heading": 0.23658909177211548, - "angularVelocity": -0.49658576459202225, - "velocityX": 3.966895620121981, - "velocityY": 0.48374445840054986, - "timestamp": 3.0887580929967364 - }, - { - "x": 5.222418230866736, - "y": 7.213006191908579, - "heading": 0.23658908655312502, - "angularVelocity": -6.115452145733656e-8, - "velocityX": 4.195209829083447, - "velocityY": 0.511698901097697, - "timestamp": 3.1740991329651607 - }, - { - "x": 5.580441834844563, - "y": 7.2566748272476005, - "heading": 0.2365890813344374, - "angularVelocity": -6.115097279558182e-8, - "velocityX": 4.195210230743547, - "velocityY": 0.5116956080589027, - "timestamp": 3.259440172933585 - }, - { - "x": 5.9337628383316385, - "y": 7.293044212723314, - "heading": 0.21681067321560563, - "angularVelocity": -0.23175728964809564, - "velocityX": 4.1401066077681214, - "velocityY": 0.4261652481522448, - "timestamp": 3.344781212902009 - }, - { - "x": 6.258898803951296, - "y": 7.325913470340443, - "heading": 0.1915866655524086, - "angularVelocity": -0.2955671465045406, - "velocityX": 3.80984302206719, - "velocityY": 0.3851518288186972, - "timestamp": 3.4301222528704334 - }, - { - "x": 6.555814253467, - "y": 7.355310299665644, - "heading": 0.1654932429147573, - "angularVelocity": -0.30575468317829, - "velocityX": 3.47916371332669, - "velocityY": 0.344462984468874, - "timestamp": 3.5154632928388576 - }, - { - "x": 6.82450746796162, - "y": 7.381244823587674, - "heading": 0.13997843257634243, - "angularVelocity": -0.29897468261290555, - "velocityX": 3.1484642628451307, - "velocityY": 0.30389275700912277, - "timestamp": 3.600804332807282 - }, - { - "x": 7.064979286092556, - "y": 7.4037222220368, - "heading": 0.11575388397248855, - "angularVelocity": -0.2838557933301128, - "velocityX": 2.8177746394924394, - "velocityY": 0.26338322637552325, - "timestamp": 3.686145372775706 - }, - { - "x": 7.277230735069167, - "y": 7.422745623676577, - "heading": 0.0932430635255194, - "angularVelocity": -0.26377485504392817, - "velocityX": 2.4870970526623895, - "velocityY": 0.22291035645705817, - "timestamp": 3.7714864127441303 - }, - { - "x": 7.461262714918097, - "y": 7.43831711432498, - "heading": 0.07272706483914178, - "angularVelocity": -0.24040014855652678, - "velocityX": 2.15643001206713, - "velocityY": 0.18246192751066057, - "timestamp": 3.8568274527125546 - }, - { - "x": 7.61707597350389, - "y": 7.450438179449409, - "heading": 0.054406235810438235, - "angularVelocity": -0.21467782716829087, - "velocityX": 1.8257717347180626, - "velocityY": 0.14203090481336236, - "timestamp": 3.942168492680979 - }, - { - "x": 7.744671129130212, - "y": 7.459109928713578, - "heading": 0.038430689484197006, - "angularVelocity": -0.1871965273935276, - "velocityX": 1.4951207024607605, - "velocityY": 0.10161288481341456, - "timestamp": 4.027509532649403 - }, - { - "x": 7.84404869662698, - "y": 7.464333222010732, - "heading": 0.02491711833560148, - "angularVelocity": -0.15834786116498592, - "velocityX": 1.1644757028217392, - "velocityY": 0.061204940777464564, - "timestamp": 4.112850572617827 - }, - { - "x": 7.915209108962923, - "y": 7.466108745654989, - "heading": 0.013958811300242875, - "angularVelocity": -0.12840606394547271, - "velocityX": 0.8338357765768063, - "velocityY": 0.02080503875876404, - "timestamp": 4.198191612586251 - }, - { - "x": 7.958152733899896, - "y": 7.464437061148586, - "heading": 0.005631988057869677, - "angularVelocity": -0.09757114801336027, - "velocityX": 0.5032001596519237, - "velocityY": -0.0195882837497823, - "timestamp": 4.283532652554674 - }, - { - "x": 7.972879886627197, - "y": 7.4593186378479, + "angularVelocity": 1.431796971368011e-33, + "velocityX": 3.743360269099248e-35, + "velocityY": -8.057233109044706e-35, + "timestamp": 1.9508388061675384 + }, + { + "x": 2.6835271230542426, + "y": 6.903699548014665, + "heading": 0.5052880772246282, + "angularVelocity": -0.10752621184517612, + "velocityX": 0.2907312178663514, + "velocityY": 0.039898557617057115, + "timestamp": 2.018379011023157 + }, + { + "x": 2.7228033814945674, + "y": 6.909089957964811, + "heading": 0.49095749727137794, + "angularVelocity": -0.21217850884350908, + "velocityX": 0.5815241236576919, + "velocityY": 0.0798103879262595, + "timestamp": 2.0859192158787754 + }, + { + "x": 2.7817246923505206, + "y": 6.91717710303389, + "heading": 0.46979167660622734, + "angularVelocity": -0.31338105518627096, + "velocityX": 0.8723886902906202, + "velocityY": 0.11973823719319023, + "timestamp": 2.153459420734394 + }, + { + "x": 2.860296723174767, + "y": 6.927962260865171, + "heading": 0.4420737054643263, + "angularVelocity": -0.41039216865205086, + "velocityX": 1.1633371706853932, + "velocityY": 0.15968500324119947, + "timestamp": 2.220999625590012 + }, + { + "x": 2.9585261643123166, + "y": 6.941446920815512, + "heading": 0.40815310467896726, + "angularVelocity": -0.502228278073356, + "velocityX": 1.454384708301312, + "velocityY": 0.19965382070083557, + "timestamp": 2.2885398304456306 + }, + { + "x": 3.0764209718394073, + "y": 6.9576328058659405, + "heading": 0.3684706356327604, + "angularVelocity": -0.5875384762459123, + "velocityX": 1.7455500435498434, + "velocityY": 0.23964814861057657, + "timestamp": 2.356080035301249 + }, + { + "x": 3.2139906503876703, + "y": 6.9765218958360915, + "heading": 0.323597871318007, + "angularVelocity": -0.6643859670055544, + "velocityX": 2.0368561043358877, + "velocityY": 0.27967178972185397, + "timestamp": 2.4236202401568674 + }, + { + "x": 3.3712465090237984, + "y": 6.998116434011635, + "heading": 0.27430452270447037, + "angularVelocity": -0.7298371202591344, + "velocityX": 2.3283296071176522, + "velocityY": 0.3197286449116718, + "timestamp": 2.491160445012486 + }, + { + "x": 3.5482015820439243, + "y": 7.022418860702876, + "heading": 0.22168358710657873, + "angularVelocity": -0.7791053596947181, + "velocityX": 2.619996095635254, + "velocityY": 0.3598216313260006, + "timestamp": 2.558700649868104 + }, + { + "x": 3.7448688058303015, + "y": 7.04943148246024, + "heading": 0.16741520227188003, + "angularVelocity": -0.8034974864335817, + "velocityX": 2.911854120176202, + "velocityY": 0.3999487685166161, + "timestamp": 2.6262408547237226 + }, + { + "x": 3.961249724160237, + "y": 7.079155012062387, + "heading": 0.1144423105156959, + "angularVelocity": -0.7843164211512963, + "velocityX": 3.2037350018777047, + "velocityY": 0.4400864591051642, + "timestamp": 2.693781059579341 + }, + { + "x": 4.197248092224393, + "y": 7.111577952654102, + "heading": 0.06945280731873603, + "angularVelocity": -0.6661144024234814, + "velocityX": 3.494190883321295, + "velocityY": 0.4800539273019009, + "timestamp": 2.7613212644349594 + }, + { + "x": 4.450279816565845, + "y": 7.14615999986413, + "heading": 0.06926458523360347, + "angularVelocity": -0.0027868154314146247, + "velocityX": 3.746386687490236, + "velocityY": 0.5120216511625231, + "timestamp": 2.828861469290578 + }, + { + "x": 4.703335353284152, + "y": 7.180937109425349, + "heading": 0.06926456657564138, + "angularVelocity": -2.762497114544702e-7, + "velocityX": 3.746739253445653, + "velocityY": 0.5149097435455242, + "timestamp": 2.896401674146196 + }, + { + "x": 4.956390888818978, + "y": 7.215714227598158, + "heading": 0.06926454791768001, + "angularVelocity": -2.7624970048363726e-7, + "velocityX": 3.746739235923052, + "velocityY": 0.5149098710487069, + "timestamp": 2.9639418790018146 + }, + { + "x": 5.209446424353711, + "y": 7.250491345771652, + "heading": 0.0692645292597187, + "angularVelocity": -2.7624969990656975e-7, + "velocityX": 3.7467392359216607, + "velocityY": 0.5149098710588286, + "timestamp": 3.031482083857433 + }, + { + "x": 5.462501959888314, + "y": 7.285268463946087, + "heading": 0.0692645106017574, + "angularVelocity": -2.7624969939846597e-7, + "velocityX": 3.7467392359197462, + "velocityY": 0.5149098710727624, + "timestamp": 3.0990222887130514 + }, + { + "x": 5.715557495424461, + "y": 7.320045582109288, + "heading": 0.06926449194379608, + "angularVelocity": -2.762496999067462e-7, + "velocityX": 3.746739235942603, + "velocityY": 0.5149098709064452, + "timestamp": 3.16656249356867 + }, + { + "x": 5.968613025759916, + "y": 7.354822738115272, + "heading": 0.06926447328583468, + "angularVelocity": -2.762497009102298e-7, + "velocityX": 3.7467391589411925, + "velocityY": 0.5149104312065347, + "timestamp": 3.234102698424288 + }, + { + "x": 6.221690653746151, + "y": 7.389438720345797, + "heading": 0.06926445462738323, + "angularVelocity": -2.7625695670270355e-7, + "velocityX": 3.74706633666926, + "velocityY": 0.5125240929387752, + "timestamp": 3.3016429032799066 + }, + { + "x": 6.476102895472122, + "y": 7.412263085598601, + "heading": 0.06926437909315673, + "angularVelocity": -0.0000011183594520291744, + "velocityX": 3.766826622303432, + "velocityY": 0.33793745964490024, + "timestamp": 3.369183108135525 + }, + { + "x": 6.712136074986816, + "y": 7.430641724558031, + "heading": 0.04341074490415746, + "angularVelocity": -0.38278880326565307, + "velocityX": 3.4947063015172035, + "velocityY": 0.27211405412107026, + "timestamp": 3.4367233129911434 + }, + { + "x": 6.9285315703702866, + "y": 7.445814297629393, + "heading": 0.019185726579151432, + "angularVelocity": -0.3586755233685199, + "velocityX": 3.203950829673405, + "velocityY": 0.2246450555457597, + "timestamp": 3.504263517846762 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, + "angularVelocity": -0.2840637901552857, + "velocityX": 2.913217404608591, + "velocityY": 0.18042146002750595, + "timestamp": 3.5718037227023802 + }, + { + "x": 7.284551146493795, + "y": 7.4665356658331685, + "heading": -0.011983560395956015, + "angularVelocity": -0.19978893791899283, + "velocityX": 2.6551742811725556, + "velocityY": 0.1423059220209307, + "timestamp": 3.6317848233617918 + }, + { + "x": 7.428286998104642, + "y": 7.47296871812108, + "heading": -0.020071347748117502, + "angularVelocity": -0.13483892864997732, + "velocityX": 2.396352351501819, + "velocityY": 0.10725132111930623, + "timestamp": 3.6917659240212033 + }, + { + "x": 7.556472866857842, + "y": 7.4774155432867, + "heading": -0.02498803078182015, + "angularVelocity": -0.08197053704667534, + "velocityX": 2.137104310257201, + "velocityY": 0.0741371051336659, + "timestamp": 3.751747024680615 + }, + { + "x": 7.669092898448606, + "y": 7.47995645101358, + "heading": -0.027230015787281982, + "angularVelocity": -0.037378190476904, + "velocityX": 1.877591947341067, + "velocityY": 0.04236180561785803, + "timestamp": 3.8117281253400264 + }, + { + "x": 7.766136403587462, + "y": 7.480650177988498, + "heading": -0.027158837231048453, + "angularVelocity": 0.001186683062681687, + "velocityX": 1.6179013734658474, + "velocityY": 0.011565759335732523, + "timestamp": 3.871709225999438 + }, + { + "x": 7.847595735447885, + "y": 7.479541543526011, + "heading": -0.025049657080079103, + "angularVelocity": 0.03516407881452247, + "velocityX": 1.3580833123248393, + "velocityY": -0.018483063003166142, + "timestamp": 3.9316903266588494 + }, + { + "x": 7.913465176456017, + "y": 7.4766658686921605, + "heading": -0.021118970688138776, + "angularVelocity": 0.06553208175121362, + "velocityX": 1.0981699282605077, + "velocityY": -0.04794301542047442, + "timestamp": 3.991671427318261 + }, + { + "x": 7.963740302847386, + "y": 7.472051704673601, + "heading": -0.015541591171907823, + "angularVelocity": 0.09298561471722216, + "velocityX": 0.8381827915570343, + "velocityY": -0.07692696479112789, + "timestamp": 4.051652527977672 + }, + { + "x": 7.998417597522527, + "y": 7.465722607991469, + "heading": -0.008461644038298986, + "angularVelocity": 0.11803629903043378, + "velocityX": 0.5781370180592035, + "velocityY": -0.10551818176978627, + "timestamp": 4.111633628637083 + }, + { + "x": 8.017494201660156, + "y": 7.457698345184326, "heading": 0, - "angularVelocity": -0.06599390000348598, - "velocityX": 0.1725682360181047, - "velocityY": -0.05997610648498418, - "timestamp": 4.368873692523098 - }, - { - "x": 7.955787513452433, - "y": 7.4497474833995785, - "heading": -0.0028764502828999656, - "angularVelocity": -0.0311548498072446, - "velocityX": -0.18512759364374298, - "velocityY": -0.10366522970762994, - "timestamp": 4.461201221004108 - }, - { - "x": 7.9056697739729325, - "y": 7.4361426444977665, - "heading": -0.002567951866504596, - "angularVelocity": 0.0033413481490390143, - "velocityX": -0.5428255288974695, - "velocityY": -0.147354089572646, - "timestamp": 4.553528749485118 - }, - { - "x": 7.822526434902679, - "y": 7.418504156260187, - "heading": 0.0008870875255376844, - "angularVelocity": 0.03742155182625641, - "velocityX": -0.9005259908733981, - "velocityY": -0.19104256907740527, - "timestamp": 4.6458562779661285 - }, - { - "x": 7.70635721281995, - "y": 7.396832065786244, - "heading": 0.007441383928023924, - "angularVelocity": 0.07098962260030964, - "velocityX": -1.2582295225916607, - "velocityY": -0.2347305384482473, - "timestamp": 4.738183806447139 - }, - { - "x": 7.557161757823986, - "y": 7.371126434037665, - "heading": 0.017035597131834973, - "angularVelocity": 0.10391497922295617, - "velocityX": -1.6159368440870827, - "velocityY": -0.27841784754226306, - "timestamp": 4.830511334928149 - }, - { - "x": 7.3749396288270415, - "y": 7.34138733896449, - "heading": 0.02959331840082248, - "angularVelocity": 0.13601275237829405, - "velocityX": -1.9736489430064588, - "velocityY": -0.3221043123589225, - "timestamp": 4.922838863409159 - }, - { - "x": 7.1596902544281695, - "y": 7.307614881050875, - "heading": 0.04501267183179304, - "angularVelocity": 0.1670071070313771, - "velocityX": -2.3313672307728326, - "velocityY": -0.3657896888311208, - "timestamp": 5.015166391890169 - }, - { - "x": 6.911412867489138, - "y": 7.269809194103027, - "heading": 0.06315110065106552, - "angularVelocity": 0.19645742843645173, - "velocityX": -2.689093827417871, - "velocityY": -0.4094736160475027, - "timestamp": 5.107493920371179 - }, - { - "x": 6.630106388703286, - "y": 7.227970469120692, - "heading": 0.0837947312429067, - "angularVelocity": 0.22359128346089294, - "velocityX": -3.046832114039641, - "velocityY": -0.45315547454453764, - "timestamp": 5.199821448852189 - }, - { - "x": 6.315769207511686, - "y": 7.182099018189511, - "heading": 0.10658632788484147, - "angularVelocity": 0.24685591628960976, - "velocityX": -3.404587844634587, - "velocityY": -0.4968339528401363, - "timestamp": 5.292148977333199 - }, - { - "x": 5.9683988091079465, - "y": 7.13219550920707, - "heading": 0.13080671297913374, - "angularVelocity": 0.2623311323585783, - "velocityX": -3.7623708131122173, - "velocityY": -0.5405051971331102, - "timestamp": 5.3844765058142094 - }, - { - "x": 5.587993514060399, - "y": 7.078262670189621, - "heading": 0.1542532135313182, - "angularVelocity": 0.2539491843649532, - "velocityX": -4.1201719715240674, - "velocityY": -0.584146894266117, - "timestamp": 5.47680403429522 - }, - { - "x": 5.224748052314718, - "y": 7.033899534150213, - "heading": 0.1946636792264689, - "angularVelocity": 0.43768598986663365, - "velocityX": -3.9343137168498292, - "velocityY": -0.4804973854415783, - "timestamp": 5.56913156277623 - }, - { - "x": 4.89452487756816, - "y": 6.993569704537409, - "heading": 0.23141354143983248, - "angularVelocity": 0.39803797218423637, - "velocityX": -3.5766491335731794, - "velocityY": -0.43681261998796816, - "timestamp": 5.66145909125724 - }, - { - "x": 4.597324012424621, - "y": 6.957273083756657, - "heading": 0.264499660893691, - "angularVelocity": 0.3583559529665485, - "velocityX": -3.2189843054736125, - "velocityY": -0.393128911581516, - "timestamp": 5.75378661973825 - }, - { - "x": 4.333145462586867, - "y": 6.925009592651694, - "heading": 0.2939186790634633, - "angularVelocity": 0.318637557549515, - "velocityX": -2.8613194156073467, - "velocityY": -0.34944606051704, - "timestamp": 5.84611414821926 - }, - { - "x": 4.101989229015984, - "y": 6.89677916444039, - "heading": 0.31966724628437904, - "angularVelocity": 0.2788828818937978, - "velocityX": -2.503654515331547, - "velocityY": -0.30576393277018515, - "timestamp": 5.93844167670027 - }, - { - "x": 3.903855311009448, - "y": 6.872581743315058, - "heading": 0.3417422393667011, - "angularVelocity": 0.23909437895180408, - "velocityX": -2.1459896226647937, - "velocityY": -0.2620824094766995, - "timestamp": 6.03076920518128 - }, - { - "x": 3.7387437076676333, - "y": 6.85241728377867, - "heading": 0.36014094824966475, - "angularVelocity": 0.19927652332584478, - "velocityX": -1.7883247397418944, - "velocityY": -0.21840137896180364, - "timestamp": 6.12309673366229 - }, - { - "x": 3.606654418811003, - "y": 6.836285750188712, - "heading": 0.3748612292329891, - "angularVelocity": 0.15943544926962971, - "velocityX": -1.4306598587635568, - "velocityY": -0.17472073449118222, - "timestamp": 6.2154242621433005 - }, - { - "x": 3.507587445613312, - "y": 6.824187116399864, - "heading": 0.3859016251505043, - "angularVelocity": 0.11957859263812058, - "velocityX": -1.0729949650722783, - "velocityY": -0.13104037320068423, - "timestamp": 6.307751790624311 - }, - { - "x": 3.441542791040051, - "y": 6.8161213654846, - "heading": 0.39326145350117914, - "angularVelocity": 0.07971434383395841, - "velocityX": -0.7153300392617483, - "velocityY": -0.08736019525231867, - "timestamp": 6.400079319105321 + "angularVelocity": 0.14107183671647575, + "velocityX": 0.3180435825269393, + "velocityY": -0.1337798526356988, + "timestamp": 4.171614729296494 + }, + { + "x": 8.008975668857596, + "y": 7.440959395735479, + "heading": 0.016440344425850486, + "angularVelocity": 0.17454816573497248, + "velocityX": -0.09044179592138195, + "velocityY": -0.17771847395317164, + "timestamp": 4.265802739560738 + }, + { + "x": 7.961981832760565, + "y": 7.4200887947149115, + "heading": 0.0360050890089686, + "angularVelocity": 0.20772011775415367, + "velocityX": -0.4989364990850843, + "velocityY": -0.22158447728129313, + "timestamp": 4.3599907498249815 + }, + { + "x": 7.876511570645417, + "y": 7.39509531765153, + "heading": 0.058657775076795544, + "angularVelocity": 0.24050498576490828, + "velocityX": -0.9074431222759898, + "velocityY": -0.2653573102697685, + "timestamp": 4.454178760089225 + }, + { + "x": 7.752563396875145, + "y": 7.365990633137338, + "heading": 0.08435039467917142, + "angularVelocity": 0.2727801503641011, + "velocityX": -1.315965518567977, + "velocityY": -0.3090062570866462, + "timestamp": 4.548366770353469 + }, + { + "x": 7.590135252234061, + "y": 7.332791018782474, + "heading": 0.11301654484596554, + "angularVelocity": 0.3043503104733978, + "velocityX": -1.7245097776818146, + "velocityY": -0.35248238349789773, + "timestamp": 4.642554780617712 + }, + { + "x": 7.3892240901384, + "y": 7.295520773303632, + "heading": 0.14455765680612448, + "angularVelocity": 0.33487395977121315, + "velocityX": -2.13308638256617, + "velocityY": -0.39570052891318314, + "timestamp": 4.736742790881956 + }, + { + "x": 7.149824938411661, + "y": 7.254220108341429, + "heading": 0.1788109128374065, + "angularVelocity": 0.36366896312157765, + "velocityX": -2.54171577735964, + "velocityY": -0.4384917448232911, + "timestamp": 4.8309308011461995 + }, + { + "x": 6.871928226350113, + "y": 7.208968463075644, + "heading": 0.21545413209734332, + "angularVelocity": 0.3890433523028524, + "velocityX": -2.9504467849136042, + "velocityY": -0.4804395499897761, + "timestamp": 4.925118811410443 + }, + { + "x": 6.555508023323144, + "y": 7.15999689291417, + "heading": 0.25354713749374036, + "angularVelocity": 0.40443582245264, + "velocityX": -3.3594530995957568, + "velocityY": -0.5199342254293745, + "timestamp": 5.019306821674687 + }, + { + "x": 6.201946981022424, + "y": 7.116596127430346, + "heading": 0.25354714311943394, + "angularVelocity": 5.972834101491316e-8, + "velocityX": -3.753779714730222, + "velocityY": -0.46078864350212084, + "timestamp": 5.11349483193893 + }, + { + "x": 5.847829680383322, + "y": 7.077995922009697, + "heading": 0.25354714640605686, + "angularVelocity": 3.4894280958051e-8, + "velocityX": -3.7596855443238373, + "velocityY": -0.4098207968546854, + "timestamp": 5.207682842203174 + }, + { + "x": 5.493712379149646, + "y": 7.039395722043647, + "heading": 0.25354714969267966, + "angularVelocity": 3.4894280048226024e-8, + "velocityX": -3.759685550636459, + "velocityY": -0.40982073894285953, + "timestamp": 5.301870852467418 + }, + { + "x": 5.13959507794107, + "y": 7.0007955218474756, + "heading": 0.25354715297933, + "angularVelocity": 3.489457219723442e-8, + "velocityX": -3.7596855503699804, + "velocityY": -0.4098207413860709, + "timestamp": 5.396058862731661 + }, + { + "x": 4.793375065328026, + "y": 6.963049240971564, + "heading": 0.2809926873119857, + "angularVelocity": 0.2913909557666365, + "velocityX": -3.675839543076952, + "velocityY": -0.4007546265178986, + "timestamp": 5.490246872995905 + }, + { + "x": 4.4856271372174605, + "y": 6.929500155423233, + "heading": 0.3062072463057812, + "angularVelocity": 0.2677045509620205, + "velocityX": -3.2673790140292933, + "velocityY": -0.3561927410315718, + "timestamp": 5.5844348832601485 + }, + { + "x": 4.216349125910067, + "y": 6.900146018877801, + "heading": 0.32859640421091696, + "angularVelocity": 0.23770709076795637, + "velocityX": -2.858941499580855, + "velocityY": -0.31165470491497754, + "timestamp": 5.678622893524392 + }, + { + "x": 3.9855401854111148, + "y": 6.874986045056756, + "heading": 0.34796083888612495, + "angularVelocity": 0.2055934149249067, + "velocityX": -2.450512967111424, + "velocityY": -0.26712501676655553, + "timestamp": 5.772810903788636 + }, + { + "x": 3.7931998687162816, + "y": 6.854019825581219, + "heading": 0.36419988308525797, + "angularVelocity": 0.17241094862896578, + "velocityX": -2.042089180514842, + "velocityY": -0.22259966440226955, + "timestamp": 5.866998914052879 + }, + { + "x": 3.639327899901621, + "y": 6.837247108195556, + "heading": 0.37725261237367164, + "angularVelocity": 0.13858164379727655, + "velocityX": -1.6336683234200817, + "velocityY": -0.17807699025180046, + "timestamp": 5.961186924317123 + }, + { + "x": 3.5239240923326007, + "y": 6.824667722043819, + "heading": 0.38707830352299477, + "angularVelocity": 0.10431997790119188, + "velocityX": -1.2252494478358333, + "velocityY": -0.13355613008965372, + "timestamp": 6.0553749345813666 + }, + { + "x": 3.4469883122501845, + "y": 6.8162815454148475, + "heading": 0.39364810050149374, + "angularVelocity": 0.06975194571015403, + "velocityX": -0.8168319923796448, + "velocityY": -0.08903656214250527, + "timestamp": 6.14956294484561 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 0.03985171226128589, - "velocityX": -0.35766505888933303, - "velocityY": -0.04368010298204797, - "timestamp": 6.492406847586331 + "angularVelocity": 0.03495947190378589, + "velocityX": -0.4084155935928461, + "velocityY": -0.04451793673752315, + "timestamp": 6.243750955109854 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 5.690856102528897e-33, - "velocityX": 4.038395222816766e-35, - "velocityY": 1.1020881462714252e-31, - "timestamp": 6.584734376067341 - }, - { - "x": 3.4229402433753, - "y": 6.8112531460784576, - "heading": 0.3945108838826677, - "angularVelocity": -0.039933151023800836, - "velocityX": 0.23696797878861645, - "velocityY": -0.013727643925556041, - "timestamp": 6.645585564992654 - }, - { - "x": 3.451777703834916, - "y": 6.809543306220452, - "heading": 0.3896720623982013, - "angularVelocity": -0.07951893085286188, - "velocityX": 0.4739013480083423, - "velocityY": -0.02809870913293678, - "timestamp": 6.706436753917967 - }, - { - "x": 3.4950303346590754, - "y": 6.80691471214731, - "heading": 0.3824488904717316, - "angularVelocity": -0.11870223169074358, - "velocityX": 0.7107935208504587, - "velocityY": -0.04319708652477366, - "timestamp": 6.76728794284328 - }, - { - "x": 3.5526951141421756, - "y": 6.803316933089257, - "heading": 0.37286978619030314, - "angularVelocity": -0.15741852296732456, - "velocityX": 0.9476360363948891, - "velocityY": -0.05912421961826942, - "timestamp": 6.8281391317685936 - }, - { - "x": 3.624768348044375, - "y": 6.798691977814201, - "heading": 0.3609678412629577, - "angularVelocity": -0.1955910005629235, - "velocityX": 1.1844178425282745, - "velocityY": -0.07600435351777092, - "timestamp": 6.888990320693907 - }, - { - "x": 3.7112454465000653, - "y": 6.7929724589112, - "heading": 0.3467818152826883, - "angularVelocity": -0.23312652112156756, - "velocityX": 1.4211242209553094, - "velocityY": -0.09399190063518563, - "timestamp": 6.94984150961922 - }, - { - "x": 3.8121205998573195, - "y": 6.78607911243415, - "heading": 0.3303574905541313, - "angularVelocity": -0.2699096766821729, - "velocityX": 1.6577351262777436, - "velocityY": -0.1132820343988894, - "timestamp": 7.010692698544533 - }, - { - "x": 3.927386292802013, - "y": 6.777917363603635, - "heading": 0.31174956650876195, - "angularVelocity": -0.30579392734968835, - "velocityX": 1.8942225284401175, - "velocityY": -0.1341263658879746, - "timestamp": 7.071543887469846 - }, - { - "x": 4.057032548891421, - "y": 6.7683724330022, - "heading": 0.29102438798021885, - "angularVelocity": -0.34058789802744316, - "velocityX": 2.13054598240523, - "velocityY": -0.1568569286813843, - "timestamp": 7.132395076395159 - }, - { - "x": 4.201045703157843, - "y": 6.75730211970409, - "heading": 0.2682640135119274, - "angularVelocity": -0.37403335695259643, - "velocityX": 2.3666448726775124, - "velocityY": -0.1819243550310427, - "timestamp": 7.1932462653204725 - }, - { - "x": 4.359406298184771, - "y": 6.744525706474344, - "heading": 0.2435725432613573, - "angularVelocity": -0.4057680825410948, - "velocityX": 2.6024240088603046, - "velocityY": -0.20996160396188043, - "timestamp": 7.254097454245786 - }, - { - "x": 4.532085228362737, - "y": 6.729806001912393, - "heading": 0.2170864931038356, - "angularVelocity": -0.4352593700351523, - "velocityX": 2.837724837059597, - "velocityY": -0.24189674551828952, - "timestamp": 7.314948643171099 - }, - { - "x": 4.71903604094914, - "y": 6.712818312739342, - "heading": 0.18899299215778056, - "angularVelocity": -0.46167546505190277, - "velocityX": 3.072262282596678, - "velocityY": -0.27916774467464406, - "timestamp": 7.375799832096412 - }, - { - "x": 4.920177705816601, - "y": 6.693092062890064, - "heading": 0.1595647143501649, - "angularVelocity": -0.4836105641869241, - "velocityX": 3.3054681168898354, - "velocityY": -0.32417197096165007, - "timestamp": 7.436651021021725 - }, - { - "x": 5.13534940168168, - "y": 6.66988755215724, - "heading": 0.12923590547960542, - "angularVelocity": -0.4984094708122179, - "velocityX": 3.536031089370736, - "velocityY": -0.3813320847568572, - "timestamp": 7.497502209947038 - }, - { - "x": 5.364160197364154, - "y": 6.6418909322873, - "heading": 0.09880136738513437, - "angularVelocity": -0.5001469754654356, - "velocityX": 3.760169681537533, - "velocityY": -0.46008336672439587, - "timestamp": 7.5583533988723515 - }, - { - "x": 5.605261544574765, - "y": 6.606294945250104, - "heading": 0.07010036543409427, - "angularVelocity": -0.4716588526522781, - "velocityX": 3.9621468613626054, - "velocityY": -0.584967815187393, - "timestamp": 7.619204587797665 + "angularVelocity": 0, + "velocityX": -1.9952146739162206e-34, + "velocityY": -3.951795765453016e-33, + "timestamp": 6.337938965374097 + }, + { + "x": 3.425127749537185, + "y": 6.810478633677757, + "heading": 0.3886892234850989, + "angularVelocity": -0.13293807639169805, + "velocityX": 0.2675517930120232, + "velocityY": -0.025935588272518423, + "timestamp": 6.400010272097332 + }, + { + "x": 3.458347138585669, + "y": 6.807258412874914, + "heading": 0.3723514064813247, + "angularVelocity": -0.26321045691257755, + "velocityX": 0.535181081278085, + "velocityY": -0.05187937829625554, + "timestamp": 6.462081578820567 + }, + { + "x": 3.508184213080087, + "y": 6.802427294600358, + "heading": 0.34812438720722594, + "angularVelocity": -0.3903094771652888, + "velocityX": 0.8029003596884762, + "velocityY": -0.07783174754300737, + "timestamp": 6.524152885543802 + }, + { + "x": 3.5746455076736363, + "y": 6.795984642746885, + "heading": 0.31624502791908643, + "angularVelocity": -0.5135925272251962, + "velocityX": 1.070724914651617, + "velocityY": -0.10379436479708294, + "timestamp": 6.586224192267037 + }, + { + "x": 3.6577387353786484, + "y": 6.787929625128416, + "heading": 0.2770021560106679, + "angularVelocity": -0.6322224225666108, + "velocityX": 1.3386737301264124, + "velocityY": -0.12977038898779852, + "timestamp": 6.648295498990271 + }, + { + "x": 3.7574730901724314, + "y": 6.7782611086258875, + "heading": 0.23075444743671972, + "angularVelocity": -0.7450738677080124, + "velocityX": 1.6067706652041787, + "velocityY": -0.1557646682971112, + "timestamp": 6.710366805713506 + }, + { + "x": 3.873859634378807, + "y": 6.766977536857145, + "heading": 0.17795757602156642, + "angularVelocity": -0.8505841781386996, + "velocityX": 1.875045819887845, + "velocityY": -0.18178402170643457, + "timestamp": 6.772438112436741 + }, + { + "x": 4.006911762109537, + "y": 6.754076769706622, + "heading": 0.11920696294593584, + "angularVelocity": -0.9465019535937051, + "velocityX": 2.1435367604550115, + "velocityY": -0.2078378534549514, + "timestamp": 6.834509419159976 + }, + { + "x": 4.156645657189765, + "y": 6.73955582918857, + "heading": 0.055309270826440785, + "angularVelocity": -1.029423988195197, + "velocityX": 2.4122884305927053, + "velocityY": -0.23393966205310818, + "timestamp": 6.896580725883211 + }, + { + "x": 4.323080375842652, + "y": 6.723410435666096, + "heading": -0.012586184407094544, + "angularVelocity": -1.0938299645646508, + "velocityX": 2.681347106079046, + "velocityY": -0.2601104177560936, + "timestamp": 6.958652032606445 + }, + { + "x": 4.506235840968647, + "y": 6.705634179985004, + "heading": -0.08270962096165607, + "angularVelocity": -1.1297238652836274, + "velocityX": 2.9507267495214404, + "velocityY": -0.2863844281597597, + "timestamp": 7.02072333932968 + }, + { + "x": 4.706118949857515, + "y": 6.686217722826257, + "heading": -0.15194437984101536, + "angularVelocity": -1.1154068205470853, + "velocityX": 3.2202175117742615, + "velocityY": -0.3128088996947802, + "timestamp": 7.082794646052915 + }, + { + "x": 4.922607059053276, + "y": 6.66515425622058, + "heading": -0.21301282950448638, + "angularVelocity": -0.9838434678965439, + "velocityX": 3.4877324262085807, + "velocityY": -0.3393430510421988, + "timestamp": 7.14486595277615 + }, + { + "x": 5.152321807662402, + "y": 6.642265811014449, + "heading": -0.22692060137612477, + "angularVelocity": -0.22406120647088962, + "velocityX": 3.7008202458728654, + "velocityY": -0.3687443750482909, + "timestamp": 7.206937259499385 + }, + { + "x": 5.385997622338085, + "y": 6.619824669792801, + "heading": -0.22692062699124488, + "angularVelocity": -4.12672480414244e-7, + "velocityX": 3.7646350143327725, + "velocityY": -0.36153808267225507, + "timestamp": 7.269008566222619 + }, + { + "x": 5.6195399174914655, + "y": 6.5960341766076915, + "heading": -0.2269206525768778, + "angularVelocity": -4.121974272362167e-7, + "velocityX": 3.7624839476103156, + "velocityY": -0.3832768221124699, + "timestamp": 7.331079872945854 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": 0.04853422903733051, - "angularVelocity": -0.3544078066121842, - "velocityX": 4.0390769313634065, - "velocityY": -0.808003051659102, - "timestamp": 7.680055776722978 - }, - { - "x": 6.0904224796154285, - "y": 6.495587769280379, - "heading": 0.021863961487268806, - "angularVelocity": -0.4346445536401508, - "velocityX": 3.9011410312083004, - "velocityY": -1.0029029869972568, - "timestamp": 7.741416875871002 - }, - { - "x": 6.315513452841665, - "y": 6.430603187238758, - "heading": -0.005975333075399724, - "angularVelocity": -0.4536961519465426, - "velocityX": 3.6683008673498723, - "velocityY": -1.0590517924859284, - "timestamp": 7.802777975019025 - }, - { - "x": 6.525908252987294, - "y": 6.365933697646677, - "heading": -0.03348393584839473, - "angularVelocity": -0.4483068777277778, - "velocityX": 3.428797773620156, - "velocityY": -1.053916740247379, - "timestamp": 7.864139074167049 - }, - { - "x": 6.721741896707244, - "y": 6.303262822606784, - "heading": -0.05999730126977997, - "angularVelocity": -0.43208752433567293, - "velocityX": 3.1914950422829667, - "velocityY": -1.0213453785876696, - "timestamp": 7.925500173315073 - }, - { - "x": 6.903171972351107, - "y": 6.243508401888397, - "heading": -0.08515279346758128, - "angularVelocity": -0.4099583049696985, - "velocityX": 2.9567605235719823, - "velocityY": -0.9738160096226287, - "timestamp": 7.986861272463097 - }, - { - "x": 7.07032746244464, - "y": 6.187241571909668, - "heading": -0.10872472462882488, - "angularVelocity": -0.3841510580568351, - "velocityX": 2.7241280292306773, - "velocityY": -0.9169788475104336, - "timestamp": 8.04822237161112 - }, - { - "x": 7.223309884324563, - "y": 6.13485024328056, - "heading": -0.1305599608000083, - "angularVelocity": -0.3558481916777524, - "velocityX": 2.4931499598936324, - "velocityY": -0.8538199177743311, - "timestamp": 8.109583470759143 - }, - { - "x": 7.362199686401887, - "y": 6.086614483409434, - "heading": -0.15054810335595573, - "angularVelocity": -0.3257461622017125, - "velocityX": 2.2634829559078664, - "velocityY": -0.7860967378495876, - "timestamp": 8.170944569907165 - }, - { - "x": 7.487061657008568, - "y": 6.0427457582775, - "heading": -0.16860591224690066, - "angularVelocity": -0.29428757212095324, - "velocityX": 2.0348718054328225, - "velocityY": -0.7149273031453939, - "timestamp": 8.232305669055188 - }, - { - "x": 7.597948869371604, - "y": 6.003409277701244, - "heading": -0.18466841772723513, - "angularVelocity": -0.26177017203662434, - "velocityX": 1.8071255877528956, - "velocityY": -0.6410654489966826, - "timestamp": 8.293666768203211 - }, - { - "x": 7.694905491467594, - "y": 5.968737621588528, - "heading": -0.1986834963337821, - "angularVelocity": -0.2284033174297924, - "velocityX": 1.580099174268352, - "velocityY": -0.5650429440495477, - "timestamp": 8.355027867351234 - }, - { - "x": 7.7779688005582, - "y": 5.938839509139965, - "heading": -0.21060838151890787, - "angularVelocity": -0.19433949767358227, - "velocityX": 1.3536802672036419, - "velocityY": -0.487248645537448, - "timestamp": 8.416388966499257 - }, - { - "x": 7.847170649947212, - "y": 5.913805692553094, - "heading": -0.220407320835025, - "angularVelocity": -0.15969302134694188, - "velocityX": 1.1277804724793856, - "velocityY": -0.40797536117273564, - "timestamp": 8.47775006564728 - }, - { - "x": 7.902538555840789, - "y": 5.8937130619859, - "heading": -0.22804994580967364, - "angularVelocity": -0.12455163093170954, - "velocityX": 0.9023291085450026, - "velocityY": -0.3274490002000119, - "timestamp": 8.539111164795303 - }, - { - "x": 7.944096516826036, - "y": 5.878627591236197, - "heading": -0.23351010234644243, - "angularVelocity": -0.08898400798846613, - "velocityX": 0.6772688488678362, - "velocityY": -0.24584746621490028, - "timestamp": 8.600472263943326 - }, - { - "x": 7.97186564193086, - "y": 5.86860650570412, - "heading": -0.2367649883044089, - "angularVelocity": -0.05304477923569416, - "velocityX": 0.45255260238795525, - "velocityY": -0.16331333159306605, - "timestamp": 8.661833363091349 + "heading": -0.22692067835854435, + "angularVelocity": -4.1535562716932914e-7, + "velocityX": 3.729650183615213, + "velocityY": -0.6268142199732243, + "timestamp": 7.393151179669089 + }, + { + "x": 5.959383441341322, + "y": 6.535142573313356, + "heading": -0.22692070161190955, + "angularVelocity": -7.95525127627382e-7, + "velocityX": 3.706414340926548, + "velocityY": -0.7521132022680976, + "timestamp": 7.422381387996315 + }, + { + "x": 6.066940887967902, + "y": 6.509606194124557, + "heading": -0.22692072465630755, + "angularVelocity": -7.883761123678933e-7, + "velocityX": 3.6796674667011553, + "velocityY": -0.8736297361581465, + "timestamp": 7.4516115963235405 + }, + { + "x": 6.17368571603366, + "y": 6.480861582677863, + "heading": -0.2269207475995078, + "angularVelocity": -7.849140177187675e-7, + "velocityX": 3.651866824580282, + "velocityY": -0.9833871563580519, + "timestamp": 7.480841804650766 + }, + { + "x": 6.2799139038286755, + "y": 6.450262838777624, + "heading": -0.2269207705194487, + "angularVelocity": -7.841182881547448e-7, + "velocityX": 3.63419194984251, + "velocityY": -1.0468192206387272, + "timestamp": 7.510072012977992 + }, + { + "x": 6.38600768248991, + "y": 6.419201268635774, + "heading": -0.22692079344039778, + "angularVelocity": -7.841527793264923e-7, + "velocityX": 3.629593654398156, + "velocityY": -1.0626530537902528, + "timestamp": 7.539302221305218 + }, + { + "x": 6.4915378820851375, + "y": 6.386275784732387, + "heading": -0.22692082347226308, + "angularVelocity": -0.0000010274256324387564, + "velocityX": 3.6103129479556895, + "velocityY": -1.1264197481865303, + "timestamp": 7.568532429632444 + }, + { + "x": 6.594254502115419, + "y": 6.3529873750963075, + "heading": -0.23642872835410836, + "angularVelocity": -0.32527667183951403, + "velocityX": 3.514057063171563, + "velocityY": -1.1388358667671954, + "timestamp": 7.597762637959669 + }, + { + "x": 6.69366972769076, + "y": 6.320452523543764, + "heading": -0.25267195567048195, + "angularVelocity": -0.5557000187824542, + "velocityX": 3.4011124540202067, + "velocityY": -1.1130557534298289, + "timestamp": 7.626992846286895 + }, + { + "x": 6.789488054498416, + "y": 6.288864279277486, + "heading": -0.26990654049679197, + "angularVelocity": -0.5896155317621415, + "velocityX": 3.278058292810801, + "velocityY": -1.0806711985311153, + "timestamp": 7.656223054614121 + }, + { + "x": 6.881710669103943, + "y": 6.258257632428043, + "heading": -0.28694313831911705, + "angularVelocity": -0.5828421621787924, + "velocityX": 3.1550447254125045, + "velocityY": -1.047089589879878, + "timestamp": 7.685453262941347 + }, + { + "x": 6.970345859458022, + "y": 6.228646491338886, + "heading": -0.3032367291527766, + "angularVelocity": -0.5574230142767849, + "velocityX": 3.0323146986098455, + "velocityY": -1.0130321603485573, + "timestamp": 7.714683471268573 + }, + { + "x": 7.055400617247705, + "y": 6.200038168598195, + "heading": -0.3184704659597927, + "angularVelocity": -0.5211641544429054, + "velocityX": 2.909823865690951, + "velocityY": -0.978724558526454, + "timestamp": 7.743913679595798 + }, + { + "x": 7.136880378911089, + "y": 6.172437128670844, + "heading": -0.33243641899697507, + "angularVelocity": -0.477791772157844, + "velocityX": 2.7875190197494866, + "velocityY": -0.9442642220804569, + "timestamp": 7.773143887923024 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.42938659336036833, + "velocityX": 2.665359438451487, + "velocityY": -0.9097014120066562, + "timestamp": 7.80237409625025 + }, + { + "x": 7.370577859130934, + "y": 6.0916577592619054, + "heading": -0.3653758531922844, + "angularVelocity": -0.3132570133567013, + "velocityX": 2.393610638018096, + "velocityY": -0.8325804140238011, + "timestamp": 7.867459230019526 + }, + { + "x": 7.5087533836626745, + "y": 6.042843250581257, + "heading": -0.37947223160146565, + "angularVelocity": -0.21658368958955537, + "velocityX": 2.1229967049244562, + "velocityY": -0.7500101152698444, + "timestamp": 7.932544363788803 + }, + { + "x": 7.629367562165718, + "y": 5.999619792415774, + "heading": -0.38804895009153, + "angularVelocity": -0.1317769203712161, + "velocityX": 1.85317554897581, + "velocityY": -0.6641064658284028, + "timestamp": 7.997629497558079 + }, + { + "x": 7.7324579600950285, + "y": 5.962133690972344, + "heading": -0.39162657356882036, + "angularVelocity": -0.05496836635495089, + "velocityX": 1.5839315671495955, + "velocityY": -0.5759548958801737, + "timestamp": 8.062714631327355 + }, + { + "x": 7.81805300550911, + "y": 5.930490256491027, + "heading": -0.39057997729782273, + "angularVelocity": 0.01608041975771331, + "velocityX": 1.3151243679933469, + "velocityY": -0.4861852876186942, + "timestamp": 8.127799765096631 + }, + { + "x": 7.886174910867723, + "y": 5.904768911877698, + "heading": -0.38519213768662347, + "angularVelocity": 0.08278141718657105, + "velocityX": 1.0466584519915365, + "velocityY": -0.39519538677620675, + "timestamp": 8.192884898865907 + }, + { + "x": 7.936841490598973, + "y": 5.885031694896893, + "heading": -0.3756842946663964, + "angularVelocity": 0.1460831755210139, + "velocityX": 0.7784662456231665, + "velocityY": -0.3032523072130836, + "timestamp": 8.257970032635184 + }, + { + "x": 7.970067341476633, + "y": 5.8713284070268905, + "heading": -0.36223415085714655, + "angularVelocity": 0.206654623418748, + "velocityX": 0.510498311264806, + "velocityY": -0.21054405324847428, + "timestamp": 8.32305516640446 }, { "x": 7.985864639282227, "y": 5.863699913024902, - "heading": -0.2377945013589896, - "angularVelocity": -0.01677794349962979, - "velocityX": 0.22814124169445224, - "velocityY": -0.07996259433654461, - "timestamp": 8.723194462239372 - }, - { - "x": 7.985275174373748, - "y": 5.8642853483331905, - "heading": -0.23637136146087387, - "angularVelocity": 0.021914153931364394, - "velocityX": -0.009076848143066944, - "velocityY": 0.00901479853081729, - "timestamp": 8.78813604759891 - }, - { - "x": 7.9692615507734805, - "y": 5.87059931497324, - "heading": -0.23246035931952694, - "angularVelocity": 0.060223385673360406, - "velocityX": -0.24658504272125015, - "velocityY": 0.09722532342093965, - "timestamp": 8.853077632958447 - }, - { - "x": 7.937802661499015, - "y": 5.8825854207762855, - "heading": -0.22608944217873808, - "angularVelocity": 0.09810227307383063, - "velocityX": -0.4844182521923094, - "velocityY": 0.18456749610726042, - "timestamp": 8.918019218317985 - }, - { - "x": 7.890874700520602, - "y": 5.9001792828849275, - "heading": -0.21729032731874598, - "angularVelocity": 0.13549276340079022, - "velocityX": -0.722618037711973, - "velocityY": 0.27091827234023624, - "timestamp": 8.982960803677523 - }, - { - "x": 7.828450615730271, - "y": 5.923306702319573, - "heading": -0.20609939347439793, - "angularVelocity": 0.17232307746094241, - "velocityX": -0.9612343838656007, - "velocityY": 0.3561264990160738, - "timestamp": 9.047902389037061 - }, - { - "x": 7.750499406641646, - "y": 5.951881239016375, - "heading": -0.1925588645118683, - "angularVelocity": 0.20850320926975888, - "velocityX": -1.2003280895756, - "velocityY": 0.44000368236475507, - "timestamp": 9.112843974396599 - }, - { - "x": 7.6569852092442545, - "y": 5.985800920098193, - "heading": -0.1767184189861056, - "angularVelocity": 0.24391836814677034, - "velocityX": -1.4399740455929166, - "velocityY": 0.522310641078874, - "timestamp": 9.177785559756137 - }, - { - "x": 7.5478660841549114, - "y": 6.024943657824972, - "heading": -0.15863743627693694, - "angularVelocity": 0.27841917638854113, - "velocityX": -1.6802658032634041, - "velocityY": 0.6027376373716655, - "timestamp": 9.242727145115675 - }, - { - "x": 7.423092384274454, - "y": 6.069160679743017, - "heading": -0.13838822433863668, - "angularVelocity": 0.3118065539391131, - "velocityX": -1.9213220494952408, - "velocityY": 0.6808737679138067, - "timestamp": 9.307668730475212 - }, - { - "x": 7.282604518065142, - "y": 6.118266769407389, - "heading": -0.11606081984557375, - "angularVelocity": 0.34380750592168463, - "velocityX": -2.163295913266127, - "velocityY": 0.7561578515908439, - "timestamp": 9.37261031583475 - }, - { - "x": 7.126329839060539, - "y": 6.172025132426245, - "heading": -0.09177042771332564, - "angularVelocity": 0.37403448033749925, - "velocityX": -2.406388420292097, - "velocityY": 0.8277956677717754, - "timestamp": 9.437551901194288 - }, - { - "x": 6.954178295975344, - "y": 6.230122642965384, - "heading": -0.06566955623422352, - "angularVelocity": 0.40191306286409745, - "velocityX": -2.6508675778718325, - "velocityY": 0.8946118302701128, - "timestamp": 9.502493486553826 - }, - { - "x": 6.76603650192892, - "y": 6.292126520899546, - "heading": -0.037969143199572904, - "angularVelocity": 0.42654352956263775, - "velocityX": -2.897092718091327, - "velocityY": 0.9547638480164637, - "timestamp": 9.567435071913364 - }, - { - "x": 6.561760786884065, - "y": 6.357401498891707, - "heading": -0.008978614795991234, - "angularVelocity": 0.4464093114308898, - "velocityX": -3.145530154737028, - "velocityY": 1.0051337310411634, - "timestamp": 9.632376657272902 - }, - { - "x": 6.3411760884298385, - "y": 6.424931400851951, - "heading": 0.020808813203341207, - "angularVelocity": 0.4586803330165021, - "velocityX": -3.396663281824698, - "velocityY": 1.0398560735217077, - "timestamp": 9.69731824263244 - }, - { - "x": 6.104131062172235, - "y": 6.4928665412990725, - "heading": 0.05050238274553203, - "angularVelocity": 0.4572350579031523, - "velocityX": -3.6501268785670193, - "velocityY": 1.0460961196282834, - "timestamp": 9.762259827991977 + "heading": -0.344987478573796, + "angularVelocity": 0.2649863537883773, + "velocityX": 0.24271745160106833, + "velocityY": -0.1172079330593501, + "timestamp": 8.388140300173736 + }, + { + "x": 7.985045656249217, + "y": 5.861990715165632, + "heading": -0.3251381624172612, + "angularVelocity": 0.31897227576671205, + "velocityX": -0.01316080008970204, + "velocityY": -0.02746627272232015, + "timestamp": 8.450369270101682 + }, + { + "x": 7.968302412952749, + "y": 5.865866788787815, + "heading": -0.3020436028872691, + "angularVelocity": 0.37112231741475454, + "velocityX": -0.26905866055398503, + "velocityY": 0.062287285594966935, + "timestamp": 8.512598240029627 + }, + { + "x": 7.935633552055529, + "y": 5.875328955639361, + "heading": -0.2758355383990043, + "angularVelocity": 0.4211553641111372, + "velocityX": -0.5249783330022537, + "velocityY": 0.1520540491430483, + "timestamp": 8.574827209957572 + }, + { + "x": 7.887037558008595, + "y": 5.8903781286161365, + "heading": -0.24666763452179902, + "angularVelocity": 0.468719053376244, + "velocityX": -0.7809223598462517, + "velocityY": 0.24183548264097016, + "timestamp": 8.637056179885517 + }, + { + "x": 7.822512734192608, + "y": 5.911015324029987, + "heading": -0.21472164060862464, + "angularVelocity": 0.5133620876283883, + "velocityX": -1.0368936508301385, + "velocityY": 0.33163324795100674, + "timestamp": 8.699285149813463 + }, + { + "x": 7.742057181096897, + "y": 5.937241677003945, + "heading": -0.1802161756247138, + "angularVelocity": 0.5544919837796514, + "velocityX": -1.29289546635384, + "velocityY": 0.4214492543316279, + "timestamp": 8.761514119741408 + }, + { + "x": 7.645668783226514, + "y": 5.9690584605532635, + "heading": -0.14341967249459378, + "angularVelocity": 0.5913082471512265, + "velocityX": -1.5489312772168387, + "velocityY": 0.5112857176674774, + "timestamp": 8.823743089669353 + }, + { + "x": 7.533345224831275, + "y": 6.006467108033043, + "heading": -0.10467019490543185, + "angularVelocity": 0.6226919332592115, + "velocityX": -1.8050043014579413, + "velocityY": 0.6011452145695896, + "timestamp": 8.885972059597298 + }, + { + "x": 7.405084087505747, + "y": 6.04946923502926, + "heading": -0.06440725119965417, + "angularVelocity": 0.6470128583583754, + "velocityX": -2.0611161887146876, + "velocityY": 0.6910306734308482, + "timestamp": 8.948201029525244 + }, + { + "x": 7.260883177292953, + "y": 6.098066643466027, + "heading": -0.023226072117979224, + "angularVelocity": 0.6617686124221249, + "velocityX": -2.3172633321708815, + "velocityY": 0.7809450886466088, + "timestamp": 9.010429999453189 + }, + { + "x": 7.100741532540205, + "y": 6.152261239581916, + "heading": 0.018021968863737303, + "angularVelocity": 0.6628430621538008, + "velocityX": -2.5734259290837285, + "velocityY": 0.8708901365174432, + "timestamp": 9.072658969381134 + }, + { + "x": 6.924662708731195, + "y": 6.212054571383467, + "heading": 0.05802124542072086, + "angularVelocity": 0.6427758100977419, + "velocityX": -2.829531390490462, + "velocityY": 0.9608600603671181, + "timestamp": 9.13488793930908 + }, + { + "x": 6.732667433541327, + "y": 6.277445378277029, + "heading": 0.09448040335619008, + "angularVelocity": 0.5858872158366861, + "velocityX": -3.0853037646642187, + "velocityY": 1.050809726872821, + "timestamp": 9.197116909237025 + }, + { + "x": 6.524862060286313, + "y": 6.348410321942265, + "heading": 0.12245319566137218, + "angularVelocity": 0.44951398581032764, + "velocityX": -3.33936707446111, + "velocityY": 1.1403843539014786, + "timestamp": 9.25934587916497 + }, + { + "x": 6.30228026524581, + "y": 6.424202518084616, + "heading": 0.1231057628389814, + "angularVelocity": 0.010486549566300301, + "velocityX": -3.5768195311320152, + "velocityY": 1.2179567849204198, + "timestamp": 9.321574849092915 + }, + { + "x": 6.078984969036473, + "y": 6.498549743391548, + "heading": 0.1231057748040032, + "angularVelocity": 1.9227414178241924e-7, + "velocityX": -3.5882852707972814, + "velocityY": 1.194736557475109, + "timestamp": 9.38380381902086 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": 0.07814732109541168, - "angularVelocity": 0.42568930519370946, - "velocityX": -3.897146685933666, - "velocityY": 0.9895116857176834, - "timestamp": 9.827201413351515 - }, - { - "x": 5.590600161596063, - "y": 6.608035856956418, - "heading": 0.09892900965828218, - "angularVelocity": 0.32313056199342094, - "velocityX": -4.049594966104257, - "velocityY": 0.7915722470772912, - "timestamp": 9.891515009795834 - }, - { - "x": 5.336533047176871, - "y": 6.64421005176547, - "heading": 0.1269873566210566, - "angularVelocity": 0.4362739531611655, - "velocityX": -3.9504417178591757, - "velocityY": 0.5624657429999524, - "timestamp": 9.955828606240152 - }, - { - "x": 5.09664977853333, - "y": 6.672673453532738, - "heading": 0.15653495714348034, - "angularVelocity": 0.45943007631373584, - "velocityX": -3.7298997709019446, - "velocityY": 0.4425720740389818, - "timestamp": 10.02014220268447 - }, - { - "x": 4.872168601658864, - "y": 6.69628481617622, - "heading": 0.18579353230897896, - "angularVelocity": 0.45493607546625786, - "velocityX": -3.4904155464050137, - "velocityY": 0.3671286314072656, - "timestamp": 10.084455799128788 - }, - { - "x": 4.663439660624991, - "y": 6.716350544333371, - "heading": 0.214004287479728, - "angularVelocity": 0.43864371968645294, - "velocityX": -3.2454869976769207, - "velocityY": 0.3119982284698228, - "timestamp": 10.148769395573106 - }, - { - "x": 4.470607956167166, - "y": 6.7336023293083125, - "heading": 0.24075299474172163, - "angularVelocity": 0.415910612076444, - "velocityX": -2.9983038598187646, - "velocityY": 0.2682447558329072, - "timestamp": 10.213082992017425 - }, - { - "x": 4.293747206330888, - "y": 6.748505081783013, - "heading": 0.2657792876291017, - "angularVelocity": 0.3891291153193025, - "velocityX": -2.7499744939532724, - "velocityY": 0.23172009184096956, - "timestamp": 10.277396588461743 - }, - { - "x": 4.1329000630749535, - "y": 6.761379555283891, - "heading": 0.28890428844093546, - "angularVelocity": 0.35956628287542935, - "velocityX": -2.5009819408123937, - "velocityY": 0.20018276402913684, - "timestamp": 10.341710184906061 - }, - { - "x": 3.9880934920167372, - "y": 6.772460125748701, - "heading": 0.3099974247908716, - "angularVelocity": 0.32797320498470917, - "velocityX": -2.2515701043649203, - "velocityY": 0.17228970353732437, - "timestamp": 10.40602378135038 - }, - { - "x": 3.859345677355417, - "y": 6.781925432988514, - "heading": 0.3289591507198067, - "angularVelocity": 0.29483230572172986, - "velocityX": -2.0018755252287743, - "velocityY": 0.1471742798275413, - "timestamp": 10.470337377794698 - }, - { - "x": 3.746669497606821, - "y": 6.789916099525479, - "heading": 0.34571109262456645, - "angularVelocity": 0.2604727900617948, - "velocityX": -1.7519807004752146, - "velocityY": 0.12424536923360925, - "timestamp": 10.534650974239016 - }, - { - "x": 3.6500744318767597, - "y": 6.79654567470217, - "heading": 0.36019003249514225, - "angularVelocity": 0.2251303094690333, - "velocityX": -1.5019384868904437, - "velocityY": 0.10308201598445296, - "timestamp": 10.598964570683334 - }, - { - "x": 3.5695676775339344, - "y": 6.801907753643934, - "heading": 0.3723440332146637, - "angularVelocity": 0.18898026842651122, - "velocityX": -1.2517843627750997, - "velocityY": 0.08337395571411381, - "timestamp": 10.663278167127652 - }, - { - "x": 3.5051548414990994, - "y": 6.806080805460923, - "heading": 0.38212983379787385, - "angularVelocity": 0.1521575704708508, - "velocityX": -1.0015430577048066, - "velocityY": 0.06488599686074766, - "timestamp": 10.72759176357197 - }, - { - "x": 3.456840386946828, - "y": 6.809131562318706, - "heading": 0.3895110346173986, - "angularVelocity": 0.11476890156368999, - "velocityX": -0.7512323555735491, - "velocityY": 0.047435643883240675, - "timestamp": 10.791905360016289 - }, - { - "x": 3.4246279326905182, - "y": 6.8111174678092326, - "heading": 0.3944567935981517, - "angularVelocity": 0.0769006750389882, - "velocityX": -0.5008653851942321, - "velocityY": 0.03087847049956041, - "timestamp": 10.856218956460607 + "heading": 0.12310578689683861, + "angularVelocity": 1.9432806685756468e-7, + "velocityX": -3.662936913327793, + "velocityY": 0.941318096340742, + "timestamp": 9.446032788948806 + }, + { + "x": 5.606427257807288, + "y": 6.600564902613298, + "heading": 0.1231057972666647, + "angularVelocity": 1.578555424299444e-7, + "velocityX": -3.7237014740499577, + "velocityY": 0.6612371129043467, + "timestamp": 9.511724660841988 + }, + { + "x": 5.35931577124569, + "y": 6.62625930787543, + "heading": 0.12310580746861746, + "angularVelocity": 1.5530007712924032e-7, + "velocityX": -3.7616752185629396, + "velocityY": 0.39113522756534835, + "timestamp": 9.577416532735171 + }, + { + "x": 5.111994168115865, + "y": 6.649845615003531, + "heading": 0.1231058176602883, + "angularVelocity": 1.551435596748186e-7, + "velocityX": -3.764873735550995, + "velocityY": 0.35904452785959023, + "timestamp": 9.643108404628354 + }, + { + "x": 4.868859374012009, + "y": 6.67294055859981, + "heading": 0.13716866748145323, + "angularVelocity": 0.21407290454489739, + "velocityX": -3.701139685883833, + "velocityY": 0.35156470550620644, + "timestamp": 9.708800276521536 + }, + { + "x": 4.644141674488456, + "y": 6.6943246178055915, + "heading": 0.16550217221046287, + "angularVelocity": 0.43130913935716303, + "velocityX": -3.4207839272559015, + "velocityY": 0.3255206251475534, + "timestamp": 9.77449214841472 + }, + { + "x": 4.438149971299598, + "y": 6.713938607045374, + "heading": 0.1971398735951766, + "angularVelocity": 0.48160754858923427, + "velocityX": -3.1357258859027066, + "velocityY": 0.2985755874284469, + "timestamp": 9.840184020307902 + }, + { + "x": 4.2509016025753406, + "y": 6.731775563046276, + "heading": 0.22901708759854217, + "angularVelocity": 0.48525354940713283, + "velocityX": -2.8504039134206605, + "velocityY": 0.27152455070706877, + "timestamp": 9.905875892201085 + }, + { + "x": 4.082393919350406, + "y": 6.747832566507683, + "heading": 0.25967107343501483, + "angularVelocity": 0.46663285659323517, + "velocityX": -2.565122265033534, + "velocityY": 0.24442907468850525, + "timestamp": 9.971567764094267 + }, + { + "x": 3.9326220842038664, + "y": 6.762107971713798, + "heading": 0.28824128245235414, + "angularVelocity": 0.43491239013245725, + "velocityX": -2.279914254690053, + "velocityY": 0.21730854662395976, + "timestamp": 10.03725963598745 + }, + { + "x": 3.801581586235111, + "y": 6.77460070477556, + "heading": 0.314159509734349, + "angularVelocity": 0.3945423769342238, + "velocityX": -1.9947749119073828, + "velocityY": 0.19017167119358822, + "timestamp": 10.102951507880633 + }, + { + "x": 3.6892685916749244, + "y": 6.785310000165767, + "heading": 0.3370219943875935, + "angularVelocity": 0.34802607985383205, + "velocityX": -1.7096939289964497, + "velocityY": 0.16302314246152652, + "timestamp": 10.168643379773815 + }, + { + "x": 3.595679898494151, + "y": 6.794235280663889, + "heading": 0.35652698961990326, + "angularVelocity": 0.2969164170572818, + "velocityX": -1.4246616892414212, + "velocityY": 0.135865826941808, + "timestamp": 10.234335251666998 + }, + { + "x": 3.520812828526041, + "y": 6.801376095406927, + "heading": 0.37244068726433127, + "angularVelocity": 0.2422475899347831, + "velocityX": -1.1396702181031793, + "velocityY": 0.10870164812244498, + "timestamp": 10.30002712356018 + }, + { + "x": 3.464665127058438, + "y": 6.806732084957018, + "heading": 0.38457706026543664, + "angularVelocity": 0.18474695044220302, + "velocityX": -0.8547130695088268, + "velocityY": 0.08153199773025213, + "timestamp": 10.365718995453364 + }, + { + "x": 3.4272348818352074, + "y": 6.810302960282004, + "heading": 0.39278518447772715, + "angularVelocity": 0.12494885555456782, + "velocityX": -0.5697850303930024, + "velocityY": 0.05435794752192765, + "timestamp": 10.431410867346546 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 0.038624336675044973, - "velocityX": -0.2504520576093102, - "velocityY": 0.015098233918219028, - "timestamp": 10.920532552904925 + "angularVelocity": 0.0632601721112495, + "velocityX": -0.28488184560874424, + "velocityY": 0.027180367966521624, + "timestamp": 10.497102739239729 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, "angularVelocity": 0, - "velocityX": -2.188238548497709e-39, - "velocityY": -5.246059499257382e-38, - "timestamp": 10.984846149349243 - }, - { - "x": 3.439618429683473, - "y": 6.811489460292554, - "heading": 0.3933053508875659, - "angularVelocity": -0.04072214124873058, - "velocityX": 0.34833488669246676, - "velocityY": -0.006709852295815464, - "timestamp": 11.07412221885228 - }, - { - "x": 3.5018143627173566, - "y": 6.810291415046735, - "heading": 0.38603451078645395, - "angularVelocity": -0.08144220664715371, - "velocityX": 0.6966697053320448, - "velocityY": -0.013419556354671087, - "timestamp": 11.163398288355316 - }, - { - "x": 3.5951082578030915, - "y": 6.808494374460801, - "heading": 0.37512964589240666, - "angularVelocity": -0.12214768139715662, - "velocityX": 1.0450045079836527, - "velocityY": -0.0201290289316947, - "timestamp": 11.252674357858352 - }, - { - "x": 3.719500117899943, - "y": 6.8060983670587145, - "heading": 0.3605931109058113, - "angularVelocity": -0.1628267806536994, - "velocityX": 1.3933393437826105, - "velocityY": -0.026838182005813428, - "timestamp": 11.341950427361388 - }, - { - "x": 3.874989949959029, - "y": 6.8031034297547235, - "heading": 0.3424282200177148, - "angularVelocity": -0.20346875696043784, - "velocityX": 1.7416742574424975, - "velocityY": -0.03354692159569182, - "timestamp": 11.431226496864424 - }, - { - "x": 4.06157776435933, - "y": 6.799509608467501, - "heading": 0.32063912477995166, - "angularVelocity": -0.2440642308633667, - "velocityX": 2.0900092873595257, - "velocityY": -0.04025514684089667, - "timestamp": 11.52050256636746 - }, - { - "x": 4.279263574094258, - "y": 6.795316958786605, - "heading": 0.2952306601922425, - "angularVelocity": -0.2846055469192104, - "velocityX": 2.438344462818484, - "velocityY": -0.046962749415771964, - "timestamp": 11.609778635870496 - }, - { - "x": 4.52804739348343, - "y": 6.790525546657334, - "heading": 0.26620815675428894, - "angularVelocity": -0.3250871549286381, - "velocityX": 2.786679798674495, - "velocityY": -0.05366961332351963, - "timestamp": 11.699054705373532 - }, - { - "x": 4.807929235531671, - "y": 6.785135449055675, - "heading": 0.23357721651331317, - "angularVelocity": -0.3655060132308565, - "velocityX": 3.135015280200263, - "velocityY": -0.06037561500706378, - "timestamp": 11.788330774876568 - }, - { - "x": 5.118909101830157, - "y": 6.779146754720857, - "heading": 0.19734345180613538, - "angularVelocity": -0.40586200656991683, - "velocityX": 3.483350779549144, - "velocityY": -0.06708062270387599, - "timestamp": 11.877606844379605 - }, - { - "x": 5.4298901294687205, - "y": 6.773164805475954, - "heading": 0.1614535287560628, - "angularVelocity": -0.40201056397147766, - "velocityX": 3.483363787963201, - "velocityY": -0.06700506953546043, - "timestamp": 11.96688291388264 - }, - { - "x": 5.709773101250217, - "y": 6.767781279592902, - "heading": 0.12915793357975602, - "angularVelocity": -0.36174974274834387, - "velocityX": 3.1350279345796865, - "velocityY": -0.06030200380705819, - "timestamp": 12.056158983385677 - }, - { - "x": 5.958557987999821, - "y": 6.762996102236048, - "heading": 0.10045393097970265, - "angularVelocity": -0.32151956016698346, - "velocityX": 2.786691754402813, - "velocityY": -0.05359977632853685, - "timestamp": 12.145435052888713 - }, - { - "x": 6.176244776203503, - "y": 6.758809206943986, - "heading": 0.07533966785700749, - "angularVelocity": -0.2813101345354471, - "velocityX": 2.43835542285245, - "velocityY": -0.04689829329818694, - "timestamp": 12.234711122391749 - }, - { - "x": 6.36283345808173, - "y": 6.755220536230104, - "heading": 0.05381396772349776, - "angularVelocity": -0.24111388699496414, - "velocityX": 2.090019004161915, - "velocityY": -0.04019745418742752, - "timestamp": 12.323987191894785 - }, - { - "x": 6.5183240290861875, - "y": 6.752230041904144, - "heading": 0.03587615193014948, - "angularVelocity": -0.20092524114469607, - "velocityX": 1.7416825345247642, - "velocityY": -0.033497154865871154, - "timestamp": 12.413263261397821 - }, - { - "x": 6.642716486724279, - "y": 6.749837685266528, - "heading": 0.021525889401728494, - "angularVelocity": -0.16074030373764248, - "velocityX": 1.393346036967506, - "velocityY": -0.026797289026421714, - "timestamp": 12.502539330900857 - }, - { - "x": 6.736010829843697, - "y": 6.748043437222755, - "heading": 0.010763075905262787, - "angularVelocity": -0.12055653386599446, - "velocityX": 1.0450095265030122, - "velocityY": -0.020097749080595733, - "timestamp": 12.591815400403894 - }, - { - "x": 6.7982070581615215, - "y": 6.746847278346797, - "heading": 0.0035877434328324036, - "angularVelocity": -0.08037240564434038, - "velocityX": 0.6966730128694666, - "velocityY": -0.013398426729768601, - "timestamp": 12.68109146990693 + "velocityX": 1.9034366224257262e-34, + "velocityY": -2.547324229323533e-33, + "timestamp": 10.562794611132912 + }, + { + "x": 3.4428034429775143, + "y": 6.811427919070607, + "heading": 0.39249813327278055, + "angularVelocity": -0.050100574262489476, + "velocityX": 0.38660845958378076, + "velocityY": -0.007449238878501296, + "timestamp": 10.651470846737375 + }, + { + "x": 3.5113694006298597, + "y": 6.810106800279428, + "heading": 0.3836131655700934, + "angularVelocity": -0.10019558951868686, + "velocityX": 0.7732168284429818, + "velocityY": -0.014898228168725063, + "timestamp": 10.740147082341839 + }, + { + "x": 3.614218332551768, + "y": 6.808125167746075, + "heading": 0.37028807906085554, + "angularVelocity": -0.1502667137188131, + "velocityX": 1.1598251912796755, + "velocityY": -0.022346827420505266, + "timestamp": 10.828823317946302 + }, + { + "x": 3.7513502451988723, + "y": 6.8054830693306245, + "heading": 0.3525264976102127, + "angularVelocity": -0.20029697166969943, + "velocityX": 1.5464336269164098, + "velocityY": -0.02979488695523812, + "timestamp": 10.917499553550766 + }, + { + "x": 3.9227651512282486, + "y": 6.802180567172425, + "heading": 0.3303333796886753, + "angularVelocity": -0.2502713130553812, + "velocityX": 1.9330422052867193, + "velocityY": -0.03724224574585722, + "timestamp": 11.006175789155229 + }, + { + "x": 4.128463068361194, + "y": 6.79821773882544, + "heading": 0.3037147939289921, + "angularVelocity": -0.3001772186001935, + "velocityX": 2.319650983499712, + "velocityY": -0.04468872996211646, + "timestamp": 11.094852024759692 + }, + { + "x": 4.368444017678072, + "y": 6.793594678448866, + "heading": 0.2726776385751228, + "angularVelocity": -0.3500053327963678, + "velocityX": 2.706259999435507, + "velocityY": -0.05213415234714548, + "timestamp": 11.183528260364156 + }, + { + "x": 4.642708020512335, + "y": 6.788311497970363, + "heading": 0.23722930806330966, + "angularVelocity": -0.3997500600942169, + "velocityX": 3.092869255948188, + "velocityY": -0.05957831252634942, + "timestamp": 11.27220449596862 + }, + { + "x": 4.951255088173746, + "y": 6.782368328182758, + "heading": 0.19737735072702745, + "angularVelocity": -0.44940966499796203, + "velocityX": 3.479478640000833, + "velocityY": -0.0670209977576627, + "timestamp": 11.360880731573083 + }, + { + "x": 5.286562692788269, + "y": 6.775920770205836, + "heading": 0.19737734601955828, + "angularVelocity": -5.3086028753514235e-8, + "velocityX": 3.781256639153567, + "velocityY": -0.07270897251075666, + "timestamp": 11.449556967177546 + }, + { + "x": 5.595111114675473, + "y": 6.769985794431735, + "heading": 0.15789301075905496, + "angularVelocity": -0.4452639987631135, + "velocityX": 3.4794939115759345, + "velocityY": -0.06692859404378032, + "timestamp": 11.53823320278201 + }, + { + "x": 5.8693764275565705, + "y": 6.76471055259859, + "heading": 0.12280143262174693, + "angularVelocity": -0.3957269712466434, + "velocityX": 3.092884029318143, + "velocityY": -0.059488788593539996, + "timestamp": 11.626909438386473 + }, + { + "x": 6.109358599544783, + "y": 6.760094936608861, + "heading": 0.09209943623091729, + "angularVelocity": -0.3462257523850543, + "velocityX": 2.7062737874738274, + "velocityY": -0.05205020215694004, + "timestamp": 11.715585673990937 + }, + { + "x": 6.315057615509001, + "y": 6.756138852616793, + "heading": 0.06578495581515914, + "angularVelocity": -0.29674782918315123, + "velocityX": 2.3196633749963063, + "velocityY": -0.04461267401690494, + "timestamp": 11.8042619095954 + }, + { + "x": 6.4864734670806214, + "y": 6.752842221795132, + "heading": 0.043856783005957245, + "angularVelocity": -0.24728353272698234, + "velocityX": 1.9330528681462538, + "velocityY": -0.037176034810112814, + "timestamp": 11.892938145199864 + }, + { + "x": 6.623606149810205, + "y": 6.750204980767773, + "heading": 0.026314311963007715, + "angularVelocity": -0.19782606831887856, + "velocityX": 1.5464423111199512, + "velocityY": -0.029740110294281013, + "timestamp": 11.981614380804327 + }, + { + "x": 6.726455661694478, + "y": 6.74822708185799, + "heading": 0.013157327563715553, + "angularVelocity": -0.14837102984364775, + "velocityX": 1.1598317315027789, + "velocityY": -0.022304723427886322, + "timestamp": 12.07029061640879 + }, + { + "x": 6.795022002255089, + "y": 6.7469084932218335, + "heading": 0.004385844550104781, + "angularVelocity": -0.09891582512292933, + "velocityX": 0.7732211464912397, + "velocityY": -0.014869695664985829, + "timestamp": 12.158966852013254 }, { "x": 6.829305171966553, "y": 6.746249198913574, - "heading": -2.600985796322667e-43, - "angularVelocity": -0.04018706751768873, - "velocityX": 0.34833650247083475, - "velocityY": -0.006699213311607584, - "timestamp": 12.770367539409966 + "heading": -4.086871260037018e-38, + "angularVelocity": -0.04945907457853377, + "velocityX": 0.3866105668308079, + "velocityY": -0.007434847721776456, + "timestamp": 12.247643087617718 }, { "x": 6.829305171966553, "y": 6.746249198913574, - "heading": 1.4984085590895388e-43, - "angularVelocity": 4.5922760731607736e-42, - "velocityX": 5.97613511124824e-44, - "velocityY": 5.676824977871388e-42, - "timestamp": 12.859643608913002 + "heading": -5.480350490542979e-38, + "angularVelocity": -1.571392014077605e-37, + "velocityX": 4.425248303324229e-37, + "velocityY": 1.8999126670467387e-37, + "timestamp": 12.336319323222181 } ], "constraints": [ @@ -30881,13 +30137,13 @@ }, { "scope": [ - 4 + 5 ], "type": "StopPoint" }, { "scope": [ - 8 + 10 ], "type": "StopPoint" }, @@ -33706,29 +32962,25 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 4 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 1 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/AmpLanePABCSprint.traj b/src/main/deploy/choreo/AmpLanePABCSprint.traj deleted file mode 100644 index 4b15f1f1..00000000 --- a/src/main/deploy/choreo/AmpLanePABCSprint.traj +++ /dev/null @@ -1,850 +0,0 @@ -{ - "samples": [ - { - "x": 1.468971848487854, - "y": 6.933434963226318, - "heading": 0, - "angularVelocity": 2.1788179528279398e-36, - "velocityX": -1.368271424490806e-35, - "velocityY": -2.8235582012814922e-36, - "timestamp": 0 - }, - { - "x": 1.4919852747959301, - "y": 6.931977994889289, - "heading": 0.010182405612263554, - "angularVelocity": 0.13209480038984944, - "velocityX": 0.29854968169707696, - "velocityY": -0.018901028792491784, - "timestamp": 0.07708407584713683 - }, - { - "x": 1.538012185703421, - "y": 6.9290637945517, - "heading": 0.030545200045879524, - "angularVelocity": 0.26416343725773955, - "velocityX": 0.5971001195988331, - "velocityY": -0.037805478051881794, - "timestamp": 0.15416815169427367 - }, - { - "x": 1.6070526656722113, - "y": 6.924691955570066, - "heading": 0.06108581336299875, - "angularVelocity": 0.39619873471251593, - "velocityX": 0.895651653211777, - "velocityY": -0.056715202635427786, - "timestamp": 0.2312522275414105 - }, - { - "x": 1.699106838032385, - "y": 6.918861907235003, - "heading": 0.10180106951516922, - "angularVelocity": 0.5281928297734501, - "velocityX": 1.1942047867671626, - "velocityY": -0.07563233094502542, - "timestamp": 0.30833630338854734 - }, - { - "x": 1.8141748726519586, - "y": 6.911572878632461, - "heading": 0.15268749584080604, - "angularVelocity": 0.6601418745234515, - "velocityX": 1.492760123994504, - "velocityY": -0.09455946020546946, - "timestamp": 0.3854203792356842 - }, - { - "x": 1.952256977521906, - "y": 6.902823843817773, - "heading": 0.2137424539083774, - "angularVelocity": 0.7920566913022042, - "velocityX": 1.7913181594571344, - "velocityY": -0.11349989889010634, - "timestamp": 0.462504455082821 - }, - { - "x": 2.1133533536109033, - "y": 6.892613440825621, - "heading": 0.2849668134211211, - "angularVelocity": 0.9239827906088756, - "velocityX": 2.089878802055858, - "velocityY": -0.132458006143837, - "timestamp": 0.5395885309299578 - }, - { - "x": 2.251439762891142, - "y": 6.8838222842371355, - "heading": 0.3461905263541886, - "angularVelocity": 0.7942459225233288, - "velocityX": 1.7913739999176204, - "velocityY": -0.11404633825952669, - "timestamp": 0.6166726067770947 - }, - { - "x": 2.3665117189435922, - "y": 6.876494714440304, - "heading": 0.39723045192442524, - "angularVelocity": 0.6621331968933817, - "velocityX": 1.492810996147195, - "velocityY": -0.09505944926110971, - "timestamp": 0.6937566826242315 - }, - { - "x": 2.4585692711434595, - "y": 6.8706316961774005, - "heading": 0.43807452893142784, - "angularVelocity": 0.5298640031437787, - "velocityX": 1.1942486329138104, - "velocityY": -0.07606004480784023, - "timestamp": 0.7708407584713683 - }, - { - "x": 2.527612473955509, - "y": 6.866233949603197, - "heading": 0.46871228827353184, - "angularVelocity": 0.3974589953294743, - "velocityX": 0.8956869762435377, - "velocityY": -0.057051297896133134, - "timestamp": 0.8479248343185052 - }, - { - "x": 2.5736413469679076, - "y": 6.863301955753749, - "heading": 0.48913694367966715, - "angularVelocity": 0.26496595025201874, - "velocityX": 0.5971255736875779, - "velocityY": -0.038036310576809694, - "timestamp": 0.925008910165642 - }, - { - "x": 2.59665584564209, - "y": 6.861835956573486, - "heading": 0.499346976196016, - "angularVelocity": 0.1324532000175497, - "velocityX": 0.298563593339587, - "velocityY": -0.019018184549158077, - "timestamp": 1.0020929860127787 - }, - { - "x": 2.59665584564209, - "y": 6.861835956573486, - "heading": 0.499346976196016, - "angularVelocity": 4.187102555037149e-35, - "velocityX": 0, - "velocityY": 1.1382387123849342e-35, - "timestamp": 1.0791770618599155 - }, - { - "x": 2.5947418201277674, - "y": 6.838819593279985, - "heading": 0.49061685118958936, - "angularVelocity": -0.11327050952346711, - "velocityX": -0.024833853477310732, - "velocityY": -0.2986297671239509, - "timestamp": 1.156250300456215 - }, - { - "x": 2.590913679072716, - "y": 6.792786870102318, - "heading": 0.47314863568765103, - "angularVelocity": -0.2266443686560877, - "velocityX": -0.0496688750177347, - "velocityY": -0.5972594900128827, - "timestamp": 1.2333235390525146 - }, - { - "x": 2.5851712843326613, - "y": 6.7237379431029165, - "heading": 0.4469236634346606, - "angularVelocity": -0.34026041633404946, - "velocityX": -0.07450568893481502, - "velocityY": -0.8958871880429407, - "timestamp": 1.3103967776488141 - }, - { - "x": 2.5775144084565245, - "y": 6.631673124734894, - "heading": 0.41191275130054616, - "angularVelocity": -0.45425510555612447, - "velocityX": -0.09934545395507093, - "velocityY": -1.1945108320963094, - "timestamp": 1.3874700162451137 - }, - { - "x": 2.567942678897023, - "y": 6.516592884025286, - "heading": 0.36807680590612885, - "angularVelocity": -0.568757018555102, - "velocityX": -0.12419005265416512, - "velocityY": -1.4931283906776436, - "timestamp": 1.4645432548414132 - }, - { - "x": 2.55645550871041, - "y": 6.378497838506786, - "heading": 0.31536820111597746, - "angularVelocity": -0.6838768650456329, - "velocityX": -0.14904226675592608, - "velocityY": -1.791737936974802, - "timestamp": 1.5416164934377128 - }, - { - "x": 2.543052018141666, - "y": 6.217388734848081, - "heading": 0.2537333559903418, - "angularVelocity": -0.7996919066612944, - "velocityX": -0.17390589539062137, - "velocityY": -2.0903377954905458, - "timestamp": 1.6186897320340123 - }, - { - "x": 2.529607207091661, - "y": 6.056304138753961, - "heading": 0.19035809858653865, - "angularVelocity": -0.8222731853238346, - "velocityX": -0.174442015086806, - "velocityY": -2.0900198178755933, - "timestamp": 1.695762970630312 - }, - { - "x": 2.518080937879921, - "y": 5.918232305257069, - "heading": 0.13599588472897933, - "angularVelocity": -0.7053319005096194, - "velocityX": -0.14954956378715376, - "velocityY": -1.7914367686051997, - "timestamp": 1.7728362092266114 - }, - { - "x": 2.5084742268750184, - "y": 5.803172814891468, - "heading": 0.09067284716302221, - "angularVelocity": -0.5880515518927888, - "velocityX": -0.12464392543852573, - "velocityY": -1.4928591617677907, - "timestamp": 1.849909447822911 - }, - { - "x": 2.5007879415658545, - "y": 5.711125391009445, - "heading": 0.054405158598584594, - "angularVelocity": -0.4705613676674917, - "velocityX": -0.09972703170582883, - "velocityY": -1.194285144343763, - "timestamp": 1.9269826864192106 - }, - { - "x": 2.495022759802516, - "y": 5.642089871721298, - "heading": 0.027201747413913846, - "angularVelocity": -0.35295534066187323, - "velocityX": -0.07480134309050894, - "velocityY": -0.8957132273855447, - "timestamp": 2.00405592501551 - }, - { - "x": 2.491179145461549, - "y": 5.596066182088173, - "heading": 0.009066533897654676, - "angularVelocity": -0.2352984491964748, - "velocityX": -0.049869635829104156, - "velocityY": -0.5971422827343685, - "timestamp": 2.0811291636118097 - }, - { - "x": 2.489257335662842, - "y": 5.573054313659668, - "heading": -1.290637644918987e-42, - "angularVelocity": -0.11763530458534513, - "velocityX": -0.024934852014888275, - "velocityY": -0.2985714477243978, - "timestamp": 2.158202402208109 - }, - { - "x": 2.489257335662842, - "y": 5.573054313659668, - "heading": 3.5400616344493244e-42, - "angularVelocity": 6.267668380261972e-41, - "velocityX": 0, - "velocityY": -1.030203639730239e-41, - "timestamp": 2.2352756408044088 - }, - { - "x": 2.490545501701153, - "y": 5.550368032283891, - "heading": -0.012391572665595225, - "angularVelocity": -0.16176043708854124, - "velocityX": 0.016815807567216828, - "velocityY": -0.29614826868975896, - "timestamp": 2.311880111518175 - }, - { - "x": 2.4931203515767737, - "y": 5.504995392935276, - "heading": -0.03717086071864737, - "angularVelocity": -0.32347052100444956, - "velocityX": 0.033612266381182004, - "velocityY": -0.592297537282776, - "timestamp": 2.3884845822319414 - }, - { - "x": 2.4969797328869174, - "y": 5.436936063650543, - "heading": -0.07432246767470774, - "angularVelocity": -0.48497961815933416, - "velocityX": 0.05038062758196422, - "velocityY": -0.8884511393471786, - "timestamp": 2.4650890529457077 - }, - { - "x": 2.5021209417107193, - "y": 5.346189326696005, - "heading": -0.12381236756993079, - "angularVelocity": -0.6460445380550001, - "velocityX": 0.06711369161484333, - "velocityY": -1.1846141107561945, - "timestamp": 2.541693523659474 - }, - { - "x": 2.5085409420107596, - "y": 5.23275393263894, - "heading": -0.18557968154848511, - "angularVelocity": -0.8063147412028813, - "velocityX": 0.08380712300759265, - "velocityY": -1.4807933923454437, - "timestamp": 2.6182979943732403 - }, - { - "x": 2.516236715901849, - "y": 5.096627982067514, - "heading": -0.2595292463430887, - "angularVelocity": -0.9653426765510463, - "velocityX": 0.10046115872068286, - "velocityY": -1.7769974689866566, - "timestamp": 2.6949024650870066 - }, - { - "x": 2.525205747324836, - "y": 4.937808900810427, - "heading": -0.3455278832614149, - "angularVelocity": -1.1226320881409273, - "velocityX": 0.11708234962551842, - "velocityY": -2.073235149036095, - "timestamp": 2.771506935800773 - }, - { - "x": 2.534115785608997, - "y": 4.778903807663307, - "heading": -0.42682973592121154, - "angularVelocity": -1.0613199451972188, - "velocityX": 0.11631224915649217, - "velocityY": -2.074357954131309, - "timestamp": 2.848111406514539 - }, - { - "x": 2.541754175790119, - "y": 4.6426966190718195, - "heading": -0.4963415581149959, - "angularVelocity": -0.9074120811240399, - "velocityX": 0.09971206784605312, - "velocityY": -1.7780579556567497, - "timestamp": 2.9247158772283055 - }, - { - "x": 2.5481206072451936, - "y": 4.529189096235521, - "heading": -0.5541628291197376, - "angularVelocity": -0.7548028263362259, - "velocityX": 0.08310783164161595, - "velocityY": -1.481734966362742, - "timestamp": 3.001320347942072 - }, - { - "x": 2.553214526426421, - "y": 4.438382432251887, - "heading": -0.6003661240439829, - "angularVelocity": -0.6031409719790967, - "velocityX": 0.06649636938633684, - "velocityY": -1.1853964022926788, - "timestamp": 3.077924818655838 - }, - { - "x": 2.5570353684728135, - "y": 4.370277332669397, - "heading": -0.6349992929465087, - "angularVelocity": -0.4521037555619042, - "velocityX": 0.04987753339709207, - "velocityY": -0.8890486279445305, - "timestamp": 3.1545292893696044 - }, - { - "x": 2.55958270709048, - "y": 4.3248740935696794, - "heading": -0.6580881830664621, - "angularVelocity": -0.3014039507723422, - "velocityX": 0.03325313253824122, - "velocityY": -0.5926969885265119, - "timestamp": 3.2311337600833707 - }, - { - "x": 2.560856342315674, - "y": 4.302172660827637, - "heading": -0.6696388746005083, - "angularVelocity": -0.1507835172858963, - "velocityX": 0.016626121338957477, - "velocityY": -0.2963460556612492, - "timestamp": 3.307738230797137 - }, - { - "x": 2.560856342315674, - "y": 4.302172660827637, - "heading": -0.6696388746005083, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -7.298514595392752e-42, - "timestamp": 3.3843427015109033 - }, - { - "x": 2.555942923489597, - "y": 4.287142223743098, - "heading": -0.6619594332481032, - "angularVelocity": 0.12026451741757689, - "velocityX": -0.07694699612537333, - "velocityY": -0.23538538542014806, - "timestamp": 3.4481972906890306 - }, - { - "x": 2.546450590215251, - "y": 4.256979387192444, - "heading": -0.646488848875746, - "angularVelocity": 0.2422783479070068, - "velocityX": -0.1486554591693719, - "velocityY": -0.47236756103015765, - "timestamp": 3.512051879867158 - }, - { - "x": 2.5328417294288803, - "y": 4.211556395130359, - "heading": -0.6230747892348256, - "angularVelocity": 0.3666777899957196, - "velocityX": -0.21312267389909825, - "velocityY": -0.7113504706039822, - "timestamp": 3.575906469045285 - }, - { - "x": 2.5157945553970475, - "y": 4.150712589378183, - "heading": -0.5915084780030003, - "angularVelocity": 0.49434679070236714, - "velocityX": -0.26696865881132753, - "velocityY": -0.9528493806834795, - "timestamp": 3.6397610582234123 - }, - { - "x": 2.4963909891627085, - "y": 4.074255034081668, - "heading": -0.5515032223856839, - "angularVelocity": 0.6265055672931927, - "velocityX": -0.30387113101943486, - "velocityY": -1.19736977843881, - "timestamp": 3.7036156474015396 - }, - { - "x": 2.4765840952198315, - "y": 3.9820247553374495, - "heading": -0.502689860958291, - "angularVelocity": 0.7644456264721134, - "velocityX": -0.3101874774830074, - "velocityY": -1.4443797999691932, - "timestamp": 3.767470236579667 - }, - { - "x": 2.460561114471436, - "y": 3.8744889188548193, - "heading": -0.4448360830574491, - "angularVelocity": 0.9060238057354711, - "velocityX": -0.2509291963917905, - "velocityY": -1.6840737348203896, - "timestamp": 3.831324825757794 - }, - { - "x": 2.4570633682910814, - "y": 3.757300143536982, - "heading": -0.37961365420873555, - "angularVelocity": 1.021421164683569, - "velocityX": -0.0547767392347868, - "velocityY": -1.8352443704700634, - "timestamp": 3.8951794149359213 - }, - { - "x": 2.4691364189829197, - "y": 3.643410857725524, - "heading": -0.313317924824764, - "angularVelocity": 1.038229675224039, - "velocityX": 0.18907099469640304, - "velocityY": -1.78357244604229, - "timestamp": 3.9590340041140486 - }, - { - "x": 2.4943273688840915, - "y": 3.5385198822209882, - "heading": -0.2494838956973126, - "angularVelocity": 0.999678017650094, - "velocityX": 0.39450492478934795, - "velocityY": -1.6426536738328135, - "timestamp": 4.022888593292176 - }, - { - "x": 2.530951098426381, - "y": 3.444665017333843, - "heading": -0.18957977093606787, - "angularVelocity": 0.9381334299111623, - "velocityX": 0.5735489024935178, - "velocityY": -1.469821763715844, - "timestamp": 4.086743182470304 - }, - { - "x": 2.577997037977533, - "y": 3.3627901789759114, - "heading": -0.13433219775233787, - "angularVelocity": 0.8652091242747275, - "velocityX": 0.7367667720782568, - "velocityY": -1.282207581502646, - "timestamp": 4.150597771648432 - }, - { - "x": 2.6348140958033937, - "y": 3.293422653958234, - "heading": -0.08415783264044516, - "angularVelocity": 0.7857597356382248, - "velocityX": 0.8897881664756364, - "velocityY": -1.0863357811945384, - "timestamp": 4.214452360826559 - }, - { - "x": 2.7009526723986657, - "y": 3.2368947239219907, - "heading": -0.03932053859858279, - "angularVelocity": 0.7021780990053088, - "velocityX": 1.035768571163667, - "velocityY": -0.8852602571532359, - "timestamp": 4.278306950004687 - }, - { - "x": 2.776085138320923, - "y": 3.1934335231781006, - "heading": 0, - "angularVelocity": 0.6157825005951433, - "velocityX": 1.1766181082563942, - "velocityY": -0.6806276777171288, - "timestamp": 4.342161539182815 - }, - { - "x": 2.850874204910404, - "y": 3.1650170711048373, - "heading": 0.030840918072749232, - "angularVelocity": 0.536085986843282, - "velocityX": 1.3000057415001567, - "velocityY": -0.4939431996267531, - "timestamp": 4.399691336322924 - }, - { - "x": 2.9329362221959228, - "y": 3.147223404249923, - "heading": 0.05711727907812475, - "angularVelocity": 0.4567435018305658, - "velocityX": 1.426426328006394, - "velocityY": -0.3092947957313142, - "timestamp": 4.457221133463033 - }, - { - "x": 3.0224739967279888, - "y": 3.13991100956747, - "heading": 0.07884780123756048, - "angularVelocity": 0.37772638249554086, - "velocityX": 1.5563721581357917, - "velocityY": -0.12710621357910618, - "timestamp": 4.514750930603142 - }, - { - "x": 3.119725791831946, - "y": 3.1429060034855505, - "heading": 0.09605037284161863, - "angularVelocity": 0.2990202027335969, - "velocityX": 1.6904595520667056, - "velocityY": 0.0520598727436279, - "timestamp": 4.572280727743252 - }, - { - "x": 3.2249748898290944, - "y": 3.1559904101117278, - "heading": 0.10874346264141094, - "angularVelocity": 0.22063505228219965, - "velocityX": 1.8294710433416355, - "velocityY": 0.22743703744185526, - "timestamp": 4.629810524883361 - }, - { - "x": 3.3385625417753837, - "y": 3.1788843955905044, - "heading": 0.11694851256012442, - "angularVelocity": 0.14262261170034615, - "velocityX": 1.9744142617026068, - "velocityY": 0.3979500470516161, - "timestamp": 4.68734032202347 - }, - { - "x": 3.4609055664038406, - "y": 3.2112183215831944, - "heading": 0.12069410858340832, - "angularVelocity": 0.06510706120102956, - "velocityX": 2.1266027469295685, - "velocityY": 0.5620378934058015, - "timestamp": 4.744870119163579 - }, - { - "x": 3.592519911370183, - "y": 3.252486932254075, - "heading": 0.12002359804350324, - "angularVelocity": -0.011655013110373149, - "velocityX": 2.2877595873631624, - "velocityY": 0.7173432329402094, - "timestamp": 4.802399916303688 - }, - { - "x": 3.7340501302900706, - "y": 3.301970950084958, - "heading": 0.11500982718595847, - "angularVelocity": -0.08715085237193064, - "velocityX": 2.4601202499498154, - "velocityY": 0.8601458772811008, - "timestamp": 4.8599297134437975 - }, - { - "x": 3.8862975149517003, - "y": 3.358597851211711, - "heading": 0.10578551382477244, - "angularVelocity": -0.16033975122006716, - "velocityX": 2.646409204100688, - "velocityY": 0.9843055936533752, - "timestamp": 4.917459510583907 - }, - { - "x": 4.050210856024716, - "y": 3.420693253911601, - "heading": 0.09260907243382549, - "angularVelocity": -0.22903681302502776, - "velocityX": 2.849190319128336, - "velocityY": 1.0793607102187723, - "timestamp": 4.974989307724016 - }, - { - "x": 4.226710679977497, - "y": 3.485583407764258, - "heading": 0.07600213210042343, - "angularVelocity": -0.2886667632941102, - "velocityX": 3.0679722983018776, - "velocityY": 1.127939903814055, - "timestamp": 5.032519104864125 - }, - { - "x": 4.41606032843016, - "y": 3.549260198670433, - "heading": 0.05695039663507196, - "angularVelocity": -0.3311629175217238, - "velocityX": 3.291331759636103, - "velocityY": 1.1068488691363714, - "timestamp": 5.090048902004234 - }, - { - "x": 4.616894975800281, - "y": 3.607020793660326, - "heading": 0.03693511887030335, - "angularVelocity": -0.3479114956032789, - "velocityX": 3.490967417823566, - "velocityY": 1.0040117966907118, - "timestamp": 5.1475786991443435 - }, - { - "x": 4.826412483110604, - "y": 3.6552133079506532, - "heading": 0.017545300992940664, - "angularVelocity": -0.337039566298841, - "velocityX": 3.6418954650589184, - "velocityY": 0.8376965796169683, - "timestamp": 5.205108496284453 - }, - { - "x": 5.041757106781006, - "y": 3.6918814182281494, - "heading": 0, - "angularVelocity": -0.3049776266412084, - "velocityX": 3.7431841302333626, - "velocityY": 0.6373759703722572, - "timestamp": 5.262638293424562 - }, - { - "x": 5.318468189453411, - "y": 3.7184056106005574, - "heading": -0.017910640554298394, - "angularVelocity": -0.247405457661866, - "velocityX": 3.822299478409622, - "velocityY": 0.3663872284808979, - "timestamp": 5.335032171569227 - }, - { - "x": 5.597577139842402, - "y": 3.724637925984956, - "heading": -0.03131839137135645, - "angularVelocity": -0.1852055886585485, - "velocityX": 3.8554219989602294, - "velocityY": 0.08608898354560332, - "timestamp": 5.407426049713893 - }, - { - "x": 5.874223781041164, - "y": 3.7105889006362736, - "heading": -0.03999959733056603, - "angularVelocity": -0.11991629930174207, - "velocityX": 3.8214093275392043, - "velocityY": -0.194063720700357, - "timestamp": 5.479819927858558 - }, - { - "x": 6.142135014447851, - "y": 3.6780687785591137, - "heading": -0.04416827770374048, - "angularVelocity": -0.05758332720957581, - "velocityX": 3.7007443208293824, - "velocityY": -0.4492109403529163, - "timestamp": 5.552213806003223 - }, - { - "x": 6.395319274879328, - "y": 3.631372287860468, - "heading": -0.04465854916399648, - "angularVelocity": -0.0067722778889713385, - "velocityX": 3.49731589079308, - "velocityY": -0.6450336947736285, - "timestamp": 5.624607684147889 - }, - { - "x": 6.630037177214525, - "y": 3.5758872476628394, - "heading": -0.042558245366794624, - "angularVelocity": 0.029012174109595102, - "velocityX": 3.2422341273961908, - "velocityY": -0.7664327650295508, - "timestamp": 5.697001562292554 - }, - { - "x": 6.844724227675687, - "y": 3.5162488607697617, - "heading": -0.03880262566507215, - "angularVelocity": 0.05187758686193831, - "velocityX": 2.965541506591899, - "velocityY": -0.8238042832005938, - "timestamp": 5.769395440437219 - }, - { - "x": 7.038965694034982, - "y": 3.4558518067523507, - "heading": -0.03408005700737659, - "angularVelocity": 0.06523436482099192, - "velocityX": 2.6831200556922665, - "velocityY": -0.8342839970075755, - "timestamp": 5.841789318581885 - }, - { - "x": 7.212812310142381, - "y": 3.3970955320884197, - "heading": -0.02888390227680435, - "angularVelocity": 0.07177616206979107, - "velocityX": 2.4013994078338827, - "velocityY": -0.8116193823256383, - "timestamp": 5.91418319672655 - }, - { - "x": 7.366475721480626, - "y": 3.3416998550485086, - "heading": -0.023574199348340368, - "angularVelocity": 0.07334463996877644, - "velocityX": 2.122602287325693, - "velocityY": -0.7651983628949022, - "timestamp": 5.9865770748712155 - }, - { - "x": 7.500208431967021, - "y": 3.2909325516518573, - "heading": -0.018421222254793455, - "angularVelocity": 0.07117973543632572, - "velocityX": 1.847293084909134, - "velocityY": -0.7012651442046296, - "timestamp": 6.058970953015881 - }, - { - "x": 7.614258897319107, - "y": 3.245756058228632, - "heading": -0.013633605820409669, - "angularVelocity": 0.0661328907510193, - "velocityX": 1.5754158814945374, - "velocityY": -0.6240374819117769, - "timestamp": 6.131364831160546 - }, - { - "x": 7.70885616888735, - "y": 3.206920861206511, - "heading": -0.009376453647745345, - "angularVelocity": 0.05880541672539207, - "velocityX": 1.3067026382977804, - "velocityY": -0.5364431084147118, - "timestamp": 6.203758709305212 - }, - { - "x": 7.784206086933648, - "y": 3.1750260064914886, - "heading": -0.005783242083132249, - "angularVelocity": 0.04963419085565119, - "velocityX": 1.0408327330623905, - "velocityY": -0.4405739205086726, - "timestamp": 6.276152587449877 - }, - { - "x": 7.840491875309718, - "y": 3.150559356787349, - "heading": -0.0029638574374597423, - "angularVelocity": 0.03894506991376383, - "velocityX": 0.7774937580162987, - "velocityY": -0.3379657276440889, - "timestamp": 6.348546465594542 - }, - { - "x": 7.877876256619461, - "y": 3.1339251307436706, - "heading": -0.0010101702732461582, - "angularVelocity": 0.026986911245582144, - "velocityX": 0.5164025228077691, - "velocityY": -0.22977393213329078, - "timestamp": 6.420940343739208 - }, - { - "x": 7.896503925323486, - "y": 3.1254632472991943, - "heading": 0, - "angularVelocity": 0.013953807961876554, - "velocityX": 0.25730999887588185, - "velocityY": -0.11688672663131537, - "timestamp": 6.493334221883873 - }, - { - "x": 7.896503925323486, - "y": 3.1254632472991943, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.3140323971906594e-42, - "velocityY": 0, - "timestamp": 6.565728100028538 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.1.traj b/src/main/deploy/choreo/AmpLanePADESprint.1.traj index bd8ab2a1..814d4db7 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.1.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.1.traj @@ -3,164 +3,155 @@ { "x": 0.43297290802001953, "y": 6.9807281494140625, - "heading": -2.898893570682157e-41, - "angularVelocity": 0, - "velocityX": -2.361892666986074e-43, - "velocityY": -3.008824272021237e-41, + "heading": 0, + "angularVelocity": 2.735053895521923e-38, + "velocityX": -3.8673469053295564e-39, + "velocityY": 1.858643532249755e-38, "timestamp": 0 }, { - "x": 0.4587135721409647, - "y": 6.976639655231649, - "heading": 0.007035495853073787, - "angularVelocity": 0.08601037544230704, - "velocityX": 0.3146848824037811, - "velocityY": -0.04998267740708988, - "timestamp": 0.08179822279456238 - }, - { - "x": 0.5101949084110605, - "y": 6.968462527202184, - "heading": 0.02110609468280026, - "angularVelocity": 0.17201594789002958, - "velocityX": 0.6293698629540154, - "velocityY": -0.09996706224293399, - "timestamp": 0.16359644558912476 - }, - { - "x": 0.5874169050244689, - "y": 6.956196559867369, - "heading": 0.04221287349505025, - "angularVelocity": 0.25803468695475235, - "velocityX": 0.944054699175467, - "velocityY": -0.14995395884848578, - "timestamp": 0.24539466838368712 - }, - { - "x": 0.6903795277325874, - "y": 6.939841483301487, - "heading": 0.07035879886747527, - "angularVelocity": 0.34408969303787923, - "velocityX": 1.2587391167007476, - "velocityY": -0.19994415535112514, - "timestamp": 0.3271928911782495 - }, - { - "x": 0.8190827157007662, - "y": 6.919396965353516, - "heading": 0.10554932385802422, - "angularVelocity": 0.4302113638694884, - "velocityX": 1.5734227905099982, - "velocityY": -0.24993841246792703, - "timestamp": 0.4089911139728119 - }, - { - "x": 0.97352637537035, - "y": 6.894862615393891, - "heading": 0.14779318141850775, - "angularVelocity": 0.5164397968227219, - "velocityX": 1.8881053205455545, - "velocityY": -0.29993744511103754, - "timestamp": 0.4907893367673743 - }, - { - "x": 1.1537103713281627, - "y": 6.866237990592905, - "heading": 0.197103396735309, - "angularVelocity": 0.6028274653429168, - "velocityX": 2.2027861951273398, - "velocityY": -0.3499418914378741, - "timestamp": 0.5725875595619366 - }, - { - "x": 1.3596345093826188, - "y": 6.8335226068750465, - "heading": 0.2534985369685149, - "angularVelocity": 0.6894421211918415, - "velocityX": 2.51746469567729, - "velocityY": -0.39995225568682646, - "timestamp": 0.654385782356499 - }, - { - "x": 1.565548746560234, - "y": 6.800784734541898, - "heading": 0.31096699128340616, - "angularVelocity": 0.7025636053148037, - "velocityX": 2.517343655433349, - "velocityY": -0.40022718360727877, - "timestamp": 0.7361840051510614 - }, - { - "x": 1.7457232772224913, - "y": 6.772138465169413, - "heading": 0.3613042914743826, - "angularVelocity": 0.6153837879509854, - "velocityX": 2.202670479953688, - "velocityY": -0.35020650075038995, - "timestamp": 0.8179822279456238 - }, - { - "x": 1.9001583012776542, - "y": 6.747584097506383, - "heading": 0.40448766007892617, - "angularVelocity": 0.5279255114502831, - "velocityX": 1.887999748393421, - "velocityY": -0.3001821656284431, - "timestamp": 0.8997804507401862 - }, - { - "x": 2.028853992721499, - "y": 6.727121858473899, - "heading": 0.44049724757544967, - "angularVelocity": 0.4402245704892907, - "velocityX": 1.5733311439671065, - "velocityY": -0.2501550563497558, - "timestamp": 0.9815786735347486 - }, - { - "x": 2.1318104848303743, - "y": 6.710751914339462, - "heading": 0.4693170803360535, - "angularVelocity": 0.3523283486608914, - "velocityX": 1.258664168871395, - "velocityY": -0.20012591441688285, - "timestamp": 1.0633768963293109 - }, - { - "x": 2.2090278625669106, - "y": 6.698474380778823, - "heading": 0.49093579476282356, - "angularVelocity": 0.2642932045243309, - "velocityX": 0.9439982324612256, - "velocityY": -0.1500953583242809, - "timestamp": 1.1451751191238733 - }, - { - "x": 2.2605061577286842, - "y": 6.690289331413325, - "heading": 0.5053471622954702, - "angularVelocity": 0.17618191496459498, - "velocityX": 0.6293326847829228, - "velocityY": -0.10006390219570446, - "timestamp": 1.2269733419184357 + "x": 0.46193105617219093, + "y": 6.976128411049919, + "heading": 0.007924686694784022, + "angularVelocity": 0.0965568963773332, + "velocityX": 0.35283526252830383, + "velocityY": -0.05604467125266167, + "timestamp": 0.08207271559159549 + }, + { + "x": 0.5198473628641517, + "y": 6.966928755942856, + "heading": 0.023773564700110772, + "angularVelocity": 0.19310775683593598, + "velocityX": 0.7056706516226394, + "velocityY": -0.11209151592888836, + "timestamp": 0.16414543118319097 + }, + { + "x": 0.6067218129336875, + "y": 6.9531289220308485, + "heading": 0.04754806846022335, + "angularVelocity": 0.2896760950181983, + "velocityX": 1.0585058559757474, + "velocityY": -0.16814155365198188, + "timestamp": 0.24621814677478646 + }, + { + "x": 0.7225543617111448, + "y": 6.934728565336742, + "heading": 0.07925217561750111, + "angularVelocity": 0.38629289805690764, + "velocityX": 1.4113405160596262, + "velocityY": -0.22419578250180797, + "timestamp": 0.32829086236638194 + }, + { + "x": 0.8673449286095066, + "y": 6.9117272629835975, + "heading": 0.11889328943914418, + "angularVelocity": 0.48299990485147826, + "velocityX": 1.764174194245729, + "velocityY": -0.2802551638184133, + "timestamp": 0.4103635779579774 + }, + { + "x": 1.0410933872613453, + "y": 6.884124518124938, + "heading": 0.16648343863275697, + "angularVelocity": 0.5798534732349732, + "velocityX": 2.117006332731008, + "velocityY": -0.33632059887008814, + "timestamp": 0.49243629354957286 + }, + { + "x": 1.243799549589456, + "y": 6.851919767180746, + "heading": 0.22204089753091144, + "angularVelocity": 0.6769297018830912, + "velocityX": 2.4698361796239694, + "velocityY": -0.39239290075946437, + "timestamp": 0.5745090091411683 + }, + { + "x": 1.4754631289031934, + "y": 6.815112383244328, + "heading": 0.28559273385120976, + "angularVelocity": 0.7743357321882771, + "velocityX": 2.8226625333871667, + "velocityY": -0.44847284107882956, + "timestamp": 0.6565817247327638 + }, + { + "x": 1.6781593090979035, + "y": 6.782884452250807, + "heading": 0.3422497740441209, + "angularVelocity": 0.6903273491624664, + "velocityX": 2.4697145541443093, + "velocityY": -0.3926753338331338, + "timestamp": 0.7386544403243592 + }, + { + "x": 1.8518984587746443, + "y": 6.755259929387126, + "heading": 0.39086385083963315, + "angularVelocity": 0.5923293319234401, + "velocityX": 2.1168929092256374, + "velocityY": -0.33658594899118865, + "timestamp": 0.8207271559159547 + }, + { + "x": 1.9966808291576696, + "y": 6.732239139219163, + "heading": 0.43140805286474204, + "angularVelocity": 0.4940034179795163, + "velocityX": 1.7640743253029585, + "velocityY": -0.28049260953564537, + "timestamp": 0.9027998715075501 + }, + { + "x": 2.1125066103123933, + "y": 6.7138223061958735, + "heading": 0.463861007608982, + "angularVelocity": 0.39541709458878044, + "velocityX": 1.4112580571975715, + "velocityY": -0.22439653532283038, + "timestamp": 0.9848725870991456 + }, + { + "x": 2.199375929341993, + "y": 6.700009583150749, + "heading": 0.4882072246931022, + "angularVelocity": 0.29664203150374724, + "velocityX": 1.0584433377576143, + "velocityY": -0.16829859893826818, + "timestamp": 1.0669453026907412 + }, + { + "x": 2.257288845375976, + "y": 6.690801066111525, + "heading": 0.5044377142261048, + "angularVelocity": 0.19775743273523133, + "velocityX": 0.7056293387703308, + "velocityY": -0.11219949252132488, + "timestamp": 1.1490180182823366 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 0.0880612934907036, - "velocityX": 0.31466684068792017, - "velocityY": -0.05003198390964021, - "timestamp": 1.308771564712998 + "angularVelocity": 0.0988477756026137, + "velocityX": 0.3528151893675758, + "velocityY": -0.0560997894575031, + "timestamp": 1.231090733873932 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 1.0872291890838523e-40, - "velocityX": -1.1027466263693948e-42, - "velocityY": -6.469494292232001e-38, - "timestamp": 1.3905697875075604 + "angularVelocity": 0, + "velocityX": 3.087311543739499e-36, + "velocityY": 0, + "timestamp": 1.3131634494655275 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.2.traj b/src/main/deploy/choreo/AmpLanePADESprint.2.traj index 65aeed69..4fd97589 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.2.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.2.traj @@ -4,73 +4,73 @@ "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 1.0872291890838523e-40, - "velocityX": -1.1027466263693948e-42, - "velocityY": -6.469494292232001e-38, + "angularVelocity": 0, + "velocityX": 3.087311543739499e-36, + "velocityY": 0, "timestamp": 0 }, { - "x": 2.3177158255221877, - "y": 6.704097470712436, + "x": 2.3177158256290964, + "y": 6.704097470773247, "heading": 0.5125504196, - "angularVelocity": 2.0395665276637554e-17, - "velocityX": 0.32679827958625257, - "velocityY": 0.18588554008515779, - "timestamp": 0.09629940369544077 + "angularVelocity": -9.1973743139294e-17, + "velocityX": 0.34546318060283326, + "velocityY": 0.1965022887733569, + "timestamp": 0.09109647952885869 }, { - "x": 2.3806567825560636, - "y": 6.739898802979336, + "x": 2.380656782769881, + "y": 6.7398988031009575, "heading": 0.5125504196, - "angularVelocity": 1.2188419350872931e-17, - "velocityX": 0.653596539734919, - "velocityY": 0.37177106911405733, - "timestamp": 0.19259880739088153 + "angularVelocity": -1.3336684994607708e-16, + "velocityX": 0.6909263394843395, + "velocityY": 0.3930045651914449, + "timestamp": 0.18219295905771737 }, { "x": 2.475068211555481, "y": 6.793600797653198, "heading": 0.5125504196, - "angularVelocity": 9.65238482155011e-17, - "velocityX": 0.9803947415708354, - "velocityY": 0.5576565649741866, - "timestamp": 0.2888982110863223 + "angularVelocity": 6.653839443641591e-19, + "velocityX": 1.0363894332018742, + "velocityY": 0.5895068045437317, + "timestamp": 0.27328943858657606 }, { - "x": 2.569479640554898, - "y": 6.84730279232706, + "x": 2.569479640341081, + "y": 6.847302792205439, "heading": 0.5125504196, - "angularVelocity": 2.650620165495025e-16, - "velocityX": 0.9803947415708352, - "velocityY": 0.5576565649741866, - "timestamp": 0.38519761478176306 + "angularVelocity": 5.174999729369457e-17, + "velocityX": 1.0363894332018742, + "velocityY": 0.5895068045437317, + "timestamp": 0.36438591811543475 }, { - "x": 2.6324205975887742, - "y": 6.88310412459396, + "x": 2.6324205974818655, + "y": 6.8831041245331495, "heading": 0.5125504196, - "angularVelocity": 1.8072666853064398e-16, - "velocityX": 0.653596539734919, - "velocityY": 0.3717710691140574, - "timestamp": 0.48149701847720383 + "angularVelocity": -1.3901806543706914e-17, + "velocityX": 0.6909263394843393, + "velocityY": 0.39300456519144494, + "timestamp": 0.45548239764429344 }, { "x": 2.663891077041626, "y": 6.901004791259766, "heading": 0.5125504196, - "angularVelocity": 1.455758590389613e-16, - "velocityX": 0.3267982795862525, - "velocityY": 0.18588554008515784, - "timestamp": 0.5777964221726446 + "angularVelocity": -1.8725484396545262e-17, + "velocityX": 0.3454631806028332, + "velocityY": 0.19650228877335693, + "timestamp": 0.5465788771731521 }, { "x": 2.663891077041626, "y": 6.901004791259766, "heading": 0.5125504196, - "angularVelocity": 5.011309571958317e-32, - "velocityX": 1.411034806804274e-32, - "velocityY": -7.886289900823042e-34, - "timestamp": 0.6740958258680851 + "angularVelocity": 1.431796971368011e-33, + "velocityX": 3.743360269099248e-35, + "velocityY": -8.057233109044706e-35, + "timestamp": 0.6376753567020108 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.3.traj b/src/main/deploy/choreo/AmpLanePADESprint.3.traj index 917518f2..963ea88e 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.3.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.3.traj @@ -4,469 +4,523 @@ "x": 2.663891077041626, "y": 6.901004791259766, "heading": 0.5125504196, - "angularVelocity": 5.011309571958317e-32, - "velocityX": 1.411034806804274e-32, - "velocityY": -7.886289900823042e-34, + "angularVelocity": 1.431796971368011e-33, + "velocityX": 3.743360269099248e-35, + "velocityY": -8.057233109044706e-35, "timestamp": 0 }, { - "x": 2.6921026735754903, - "y": 6.9044448198890755, - "heading": 0.509005120328227, - "angularVelocity": -0.041542724029198924, - "velocityX": 0.3305747919676489, - "velocityY": 0.04030919509045999, - "timestamp": 0.08534103996842424 - }, - { - "x": 2.7485258570604016, - "y": 6.911324890060512, - "heading": 0.5019144397700286, - "angularVelocity": -0.08308640908081097, - "velocityX": 0.6611494716467927, - "velocityY": 0.0806185414893288, - "timestamp": 0.17068207993684847 - }, - { - "x": 2.8331606213159497, - "y": 6.921645023106144, - "heading": 0.4912793044898821, - "angularVelocity": -0.12461923693549357, - "velocityX": 0.9917240789057943, - "velocityY": 0.12092813785079831, - "timestamp": 0.2560231199052727 - }, - { - "x": 2.946006963275403, - "y": 6.935405249135601, - "heading": 0.4771015882559512, - "angularVelocity": -0.16613010855242188, - "velocityX": 1.3222986502297882, - "velocityY": 0.16123808702762443, - "timestamp": 0.34136415987369695 - }, - { - "x": 3.0870648825703277, - "y": 6.952605607471804, - "heading": 0.45938402488204233, - "angularVelocity": -0.20760894618186487, - "velocityX": 1.6528732172365783, - "velocityY": 0.20154849697831853, - "timestamp": 0.4267051998421212 - }, - { - "x": 3.2563343809791183, - "y": 6.973246147139034, - "heading": 0.438130093703468, - "angularVelocity": -0.24904701403261814, - "velocityX": 1.9834478050820517, - "velocityY": 0.24185948138044716, - "timestamp": 0.5120462398105454 - }, - { - "x": 3.453815461723479, - "y": 6.997326927370896, - "heading": 0.41334387603725764, - "angularVelocity": -0.29043725826848604, - "velocityX": 2.314022430678465, - "velocityY": 0.2821711598636699, - "timestamp": 0.5973872797789697 - }, - { - "x": 3.679508128581092, - "y": 7.024848018097057, - "heading": 0.3850298811864858, - "angularVelocity": -0.3317746638809172, - "velocityX": 2.644597100540597, - "velocityY": 0.32248365776117877, - "timestamp": 0.6827283197473939 - }, - { - "x": 3.9334123847380087, - "y": 7.055809500355339, - "heading": 0.35319284126218836, - "angularVelocity": -0.3730566200749024, - "velocityX": 2.97517180773588, - "velocityY": 0.3627971052349304, - "timestamp": 0.7680693597158181 - }, - { - "x": 4.215528231151698, - "y": 7.090211466549705, - "heading": 0.31783747613011065, - "angularVelocity": -0.4142832703369811, - "velocityX": 3.3057465261505037, - "velocityY": 0.4031116354698126, - "timestamp": 0.8534103996842424 - }, - { - "x": 4.525855663499604, - "y": 7.128054020379336, - "heading": 0.2789682373559137, - "angularVelocity": -0.45545775852483755, - "velocityX": 3.636321193914748, - "velocityY": 0.4434273808197387, - "timestamp": 0.9387514396526666 - }, - { - "x": 4.864394661167, - "y": 7.169337275538201, - "heading": 0.23658909177211548, - "angularVelocity": -0.49658576459202225, - "velocityX": 3.966895620121981, - "velocityY": 0.48374445840054986, - "timestamp": 1.0240924796210908 - }, - { - "x": 5.222418230866736, - "y": 7.213006191908579, - "heading": 0.23658908655312502, - "angularVelocity": -6.115452145733656e-8, - "velocityX": 4.195209829083447, - "velocityY": 0.511698901097697, - "timestamp": 1.109433519589515 - }, - { - "x": 5.580441834844563, - "y": 7.2566748272476005, - "heading": 0.2365890813344374, - "angularVelocity": -6.115097279558182e-8, - "velocityX": 4.195210230743547, - "velocityY": 0.5116956080589027, - "timestamp": 1.1947745595579393 - }, - { - "x": 5.9337628383316385, - "y": 7.293044212723314, - "heading": 0.21681067321560563, - "angularVelocity": -0.23175728964809564, - "velocityX": 4.1401066077681214, - "velocityY": 0.4261652481522448, - "timestamp": 1.2801155995263636 - }, - { - "x": 6.258898803951296, - "y": 7.325913470340443, - "heading": 0.1915866655524086, - "angularVelocity": -0.2955671465045406, - "velocityX": 3.80984302206719, - "velocityY": 0.3851518288186972, - "timestamp": 1.3654566394947878 - }, - { - "x": 6.555814253467, - "y": 7.355310299665644, - "heading": 0.1654932429147573, - "angularVelocity": -0.30575468317829, - "velocityX": 3.47916371332669, - "velocityY": 0.344462984468874, - "timestamp": 1.450797679463212 - }, - { - "x": 6.82450746796162, - "y": 7.381244823587674, - "heading": 0.13997843257634243, - "angularVelocity": -0.29897468261290555, - "velocityX": 3.1484642628451307, - "velocityY": 0.30389275700912277, - "timestamp": 1.5361387194316363 - }, - { - "x": 7.064979286092556, - "y": 7.4037222220368, - "heading": 0.11575388397248855, - "angularVelocity": -0.2838557933301128, - "velocityX": 2.8177746394924394, - "velocityY": 0.26338322637552325, - "timestamp": 1.6214797594000605 - }, - { - "x": 7.277230735069167, - "y": 7.422745623676577, - "heading": 0.0932430635255194, - "angularVelocity": -0.26377485504392817, - "velocityX": 2.4870970526623895, - "velocityY": 0.22291035645705817, - "timestamp": 1.7068207993684847 - }, - { - "x": 7.461262714918097, - "y": 7.43831711432498, - "heading": 0.07272706483914178, - "angularVelocity": -0.24040014855652678, - "velocityX": 2.15643001206713, - "velocityY": 0.18246192751066057, - "timestamp": 1.792161839336909 - }, - { - "x": 7.61707597350389, - "y": 7.450438179449409, - "heading": 0.054406235810438235, - "angularVelocity": -0.21467782716829087, - "velocityX": 1.8257717347180626, - "velocityY": 0.14203090481336236, - "timestamp": 1.8775028793053332 - }, - { - "x": 7.744671129130212, - "y": 7.459109928713578, - "heading": 0.038430689484197006, - "angularVelocity": -0.1871965273935276, - "velocityX": 1.4951207024607605, - "velocityY": 0.10161288481341456, - "timestamp": 1.9628439192737575 - }, - { - "x": 7.84404869662698, - "y": 7.464333222010732, - "heading": 0.02491711833560148, - "angularVelocity": -0.15834786116498592, - "velocityX": 1.1644757028217392, - "velocityY": 0.061204940777464564, - "timestamp": 2.0481849592421812 - }, - { - "x": 7.915209108962923, - "y": 7.466108745654989, - "heading": 0.013958811300242875, - "angularVelocity": -0.12840606394547271, - "velocityX": 0.8338357765768063, - "velocityY": 0.02080503875876404, - "timestamp": 2.133525999210605 - }, - { - "x": 7.958152733899896, - "y": 7.464437061148586, - "heading": 0.005631988057869677, - "angularVelocity": -0.09757114801336027, - "velocityX": 0.5032001596519237, - "velocityY": -0.0195882837497823, - "timestamp": 2.218867039179029 - }, - { - "x": 7.972879886627197, - "y": 7.4593186378479, + "x": 2.6835271230542426, + "y": 6.903699548014665, + "heading": 0.5052880772246282, + "angularVelocity": -0.10752621184517612, + "velocityX": 0.2907312178663514, + "velocityY": 0.039898557617057115, + "timestamp": 0.06754020485561862 + }, + { + "x": 2.7228033814945674, + "y": 6.909089957964811, + "heading": 0.49095749727137794, + "angularVelocity": -0.21217850884350908, + "velocityX": 0.5815241236576919, + "velocityY": 0.0798103879262595, + "timestamp": 0.13508040971123703 + }, + { + "x": 2.7817246923505206, + "y": 6.91717710303389, + "heading": 0.46979167660622734, + "angularVelocity": -0.31338105518627096, + "velocityX": 0.8723886902906202, + "velocityY": 0.11973823719319023, + "timestamp": 0.20262061456685543 + }, + { + "x": 2.860296723174767, + "y": 6.927962260865171, + "heading": 0.4420737054643263, + "angularVelocity": -0.41039216865205086, + "velocityX": 1.1633371706853932, + "velocityY": 0.15968500324119947, + "timestamp": 0.27016081942247383 + }, + { + "x": 2.9585261643123166, + "y": 6.941446920815512, + "heading": 0.40815310467896726, + "angularVelocity": -0.502228278073356, + "velocityX": 1.454384708301312, + "velocityY": 0.19965382070083557, + "timestamp": 0.33770102427809223 + }, + { + "x": 3.0764209718394073, + "y": 6.9576328058659405, + "heading": 0.3684706356327604, + "angularVelocity": -0.5875384762459123, + "velocityX": 1.7455500435498434, + "velocityY": 0.23964814861057657, + "timestamp": 0.40524122913371063 + }, + { + "x": 3.2139906503876703, + "y": 6.9765218958360915, + "heading": 0.323597871318007, + "angularVelocity": -0.6643859670055544, + "velocityX": 2.0368561043358877, + "velocityY": 0.27967178972185397, + "timestamp": 0.47278143398932904 + }, + { + "x": 3.3712465090237984, + "y": 6.998116434011635, + "heading": 0.27430452270447037, + "angularVelocity": -0.7298371202591344, + "velocityX": 2.3283296071176522, + "velocityY": 0.3197286449116718, + "timestamp": 0.5403216388449474 + }, + { + "x": 3.5482015820439243, + "y": 7.022418860702876, + "heading": 0.22168358710657873, + "angularVelocity": -0.7791053596947181, + "velocityX": 2.619996095635254, + "velocityY": 0.3598216313260006, + "timestamp": 0.6078618437005658 + }, + { + "x": 3.7448688058303015, + "y": 7.04943148246024, + "heading": 0.16741520227188003, + "angularVelocity": -0.8034974864335817, + "velocityX": 2.911854120176202, + "velocityY": 0.3999487685166161, + "timestamp": 0.6754020485561842 + }, + { + "x": 3.961249724160237, + "y": 7.079155012062387, + "heading": 0.1144423105156959, + "angularVelocity": -0.7843164211512963, + "velocityX": 3.2037350018777047, + "velocityY": 0.4400864591051642, + "timestamp": 0.7429422534118026 + }, + { + "x": 4.197248092224393, + "y": 7.111577952654102, + "heading": 0.06945280731873603, + "angularVelocity": -0.6661144024234814, + "velocityX": 3.494190883321295, + "velocityY": 0.4800539273019009, + "timestamp": 0.810482458267421 + }, + { + "x": 4.450279816565845, + "y": 7.14615999986413, + "heading": 0.06926458523360347, + "angularVelocity": -0.0027868154314146247, + "velocityX": 3.746386687490236, + "velocityY": 0.5120216511625231, + "timestamp": 0.8780226631230394 + }, + { + "x": 4.703335353284152, + "y": 7.180937109425349, + "heading": 0.06926456657564138, + "angularVelocity": -2.762497114544702e-7, + "velocityX": 3.746739253445653, + "velocityY": 0.5149097435455242, + "timestamp": 0.9455628679786579 + }, + { + "x": 4.956390888818978, + "y": 7.215714227598158, + "heading": 0.06926454791768001, + "angularVelocity": -2.7624970048363726e-7, + "velocityX": 3.746739235923052, + "velocityY": 0.5149098710487069, + "timestamp": 1.0131030728342763 + }, + { + "x": 5.209446424353711, + "y": 7.250491345771652, + "heading": 0.0692645292597187, + "angularVelocity": -2.7624969990656975e-7, + "velocityX": 3.7467392359216607, + "velocityY": 0.5149098710588286, + "timestamp": 1.0806432776898947 + }, + { + "x": 5.462501959888314, + "y": 7.285268463946087, + "heading": 0.0692645106017574, + "angularVelocity": -2.7624969939846597e-7, + "velocityX": 3.7467392359197462, + "velocityY": 0.5149098710727624, + "timestamp": 1.148183482545513 + }, + { + "x": 5.715557495424461, + "y": 7.320045582109288, + "heading": 0.06926449194379608, + "angularVelocity": -2.762496999067462e-7, + "velocityX": 3.746739235942603, + "velocityY": 0.5149098709064452, + "timestamp": 1.2157236874011315 + }, + { + "x": 5.968613025759916, + "y": 7.354822738115272, + "heading": 0.06926447328583468, + "angularVelocity": -2.762497009102298e-7, + "velocityX": 3.7467391589411925, + "velocityY": 0.5149104312065347, + "timestamp": 1.2832638922567499 + }, + { + "x": 6.221690653746151, + "y": 7.389438720345797, + "heading": 0.06926445462738323, + "angularVelocity": -2.7625695670270355e-7, + "velocityX": 3.74706633666926, + "velocityY": 0.5125240929387752, + "timestamp": 1.3508040971123683 + }, + { + "x": 6.476102895472122, + "y": 7.412263085598601, + "heading": 0.06926437909315673, + "angularVelocity": -0.0000011183594520291744, + "velocityX": 3.766826622303432, + "velocityY": 0.33793745964490024, + "timestamp": 1.4183443019679867 + }, + { + "x": 6.712136074986816, + "y": 7.430641724558031, + "heading": 0.04341074490415746, + "angularVelocity": -0.38278880326565307, + "velocityX": 3.4947063015172035, + "velocityY": 0.27211405412107026, + "timestamp": 1.485884506823605 + }, + { + "x": 6.9285315703702866, + "y": 7.445814297629393, + "heading": 0.019185726579151432, + "angularVelocity": -0.3586755233685199, + "velocityX": 3.203950829673405, + "velocityY": 0.2246450555457597, + "timestamp": 1.5534247116792235 + }, + { + "x": 7.125290870666504, + "y": 7.458, "heading": 0, - "angularVelocity": -0.06599390000348598, - "velocityX": 0.1725682360181047, - "velocityY": -0.05997610648498418, - "timestamp": 2.3042080791474526 - }, - { - "x": 7.955787513452433, - "y": 7.4497474833995785, - "heading": -0.0028764502828999656, - "angularVelocity": -0.0311548498072446, - "velocityX": -0.18512759364374298, - "velocityY": -0.10366522970762994, - "timestamp": 2.3965356076284627 - }, - { - "x": 7.9056697739729325, - "y": 7.4361426444977665, - "heading": -0.002567951866504596, - "angularVelocity": 0.0033413481490390143, - "velocityX": -0.5428255288974695, - "velocityY": -0.147354089572646, - "timestamp": 2.488863136109473 - }, - { - "x": 7.822526434902679, - "y": 7.418504156260187, - "heading": 0.0008870875255376844, - "angularVelocity": 0.03742155182625641, - "velocityX": -0.9005259908733981, - "velocityY": -0.19104256907740527, - "timestamp": 2.581190664590483 - }, - { - "x": 7.70635721281995, - "y": 7.396832065786244, - "heading": 0.007441383928023924, - "angularVelocity": 0.07098962260030964, - "velocityX": -1.2582295225916607, - "velocityY": -0.2347305384482473, - "timestamp": 2.673518193071493 - }, - { - "x": 7.557161757823986, - "y": 7.371126434037665, - "heading": 0.017035597131834973, - "angularVelocity": 0.10391497922295617, - "velocityX": -1.6159368440870827, - "velocityY": -0.27841784754226306, - "timestamp": 2.765845721552503 - }, - { - "x": 7.3749396288270415, - "y": 7.34138733896449, - "heading": 0.02959331840082248, - "angularVelocity": 0.13601275237829405, - "velocityX": -1.9736489430064588, - "velocityY": -0.3221043123589225, - "timestamp": 2.8581732500335133 - }, - { - "x": 7.1596902544281695, - "y": 7.307614881050875, - "heading": 0.04501267183179304, - "angularVelocity": 0.1670071070313771, - "velocityX": -2.3313672307728326, - "velocityY": -0.3657896888311208, - "timestamp": 2.9505007785145234 - }, - { - "x": 6.911412867489138, - "y": 7.269809194103027, - "heading": 0.06315110065106552, - "angularVelocity": 0.19645742843645173, - "velocityX": -2.689093827417871, - "velocityY": -0.4094736160475027, - "timestamp": 3.0428283069955335 - }, - { - "x": 6.630106388703286, - "y": 7.227970469120692, - "heading": 0.0837947312429067, - "angularVelocity": 0.22359128346089294, - "velocityX": -3.046832114039641, - "velocityY": -0.45315547454453764, - "timestamp": 3.1351558354765436 - }, - { - "x": 6.315769207511686, - "y": 7.182099018189511, - "heading": 0.10658632788484147, - "angularVelocity": 0.24685591628960976, - "velocityX": -3.404587844634587, - "velocityY": -0.4968339528401363, - "timestamp": 3.2274833639575538 - }, - { - "x": 5.9683988091079465, - "y": 7.13219550920707, - "heading": 0.13080671297913374, - "angularVelocity": 0.2623311323585783, - "velocityX": -3.7623708131122173, - "velocityY": -0.5405051971331102, - "timestamp": 3.319810892438564 - }, - { - "x": 5.587993514060399, - "y": 7.078262670189621, - "heading": 0.1542532135313182, - "angularVelocity": 0.2539491843649532, - "velocityX": -4.1201719715240674, - "velocityY": -0.584146894266117, - "timestamp": 3.412138420919574 - }, - { - "x": 5.224748052314718, - "y": 7.033899534150213, - "heading": 0.1946636792264689, - "angularVelocity": 0.43768598986663365, - "velocityX": -3.9343137168498292, - "velocityY": -0.4804973854415783, - "timestamp": 3.504465949400584 - }, - { - "x": 4.89452487756816, - "y": 6.993569704537409, - "heading": 0.23141354143983248, - "angularVelocity": 0.39803797218423637, - "velocityX": -3.5766491335731794, - "velocityY": -0.43681261998796816, - "timestamp": 3.596793477881594 - }, - { - "x": 4.597324012424621, - "y": 6.957273083756657, - "heading": 0.264499660893691, - "angularVelocity": 0.3583559529665485, - "velocityX": -3.2189843054736125, - "velocityY": -0.393128911581516, - "timestamp": 3.6891210063626043 - }, - { - "x": 4.333145462586867, - "y": 6.925009592651694, - "heading": 0.2939186790634633, - "angularVelocity": 0.318637557549515, - "velocityX": -2.8613194156073467, - "velocityY": -0.34944606051704, - "timestamp": 3.7814485348436144 - }, - { - "x": 4.101989229015984, - "y": 6.89677916444039, - "heading": 0.31966724628437904, - "angularVelocity": 0.2788828818937978, - "velocityX": -2.503654515331547, - "velocityY": -0.30576393277018515, - "timestamp": 3.8737760633246245 - }, - { - "x": 3.903855311009448, - "y": 6.872581743315058, - "heading": 0.3417422393667011, - "angularVelocity": 0.23909437895180408, - "velocityX": -2.1459896226647937, - "velocityY": -0.2620824094766995, - "timestamp": 3.9661035918056347 - }, - { - "x": 3.7387437076676333, - "y": 6.85241728377867, - "heading": 0.36014094824966475, - "angularVelocity": 0.19927652332584478, - "velocityX": -1.7883247397418944, - "velocityY": -0.21840137896180364, - "timestamp": 4.058431120286645 - }, - { - "x": 3.606654418811003, - "y": 6.836285750188712, - "heading": 0.3748612292329891, - "angularVelocity": 0.15943544926962971, - "velocityX": -1.4306598587635568, - "velocityY": -0.17472073449118222, - "timestamp": 4.150758648767654 - }, - { - "x": 3.507587445613312, - "y": 6.824187116399864, - "heading": 0.3859016251505043, - "angularVelocity": 0.11957859263812058, - "velocityX": -1.0729949650722783, - "velocityY": -0.13104037320068423, - "timestamp": 4.243086177248665 - }, - { - "x": 3.441542791040051, - "y": 6.8161213654846, - "heading": 0.39326145350117914, - "angularVelocity": 0.07971434383395841, - "velocityX": -0.7153300392617483, - "velocityY": -0.08736019525231867, - "timestamp": 4.335413705729675 + "angularVelocity": -0.2840637901552857, + "velocityX": 2.913217404608591, + "velocityY": 0.18042146002750595, + "timestamp": 1.6209649165348419 + }, + { + "x": 7.284551146493795, + "y": 7.4665356658331685, + "heading": -0.011983560395956015, + "angularVelocity": -0.19978893791899283, + "velocityX": 2.6551742811725556, + "velocityY": 0.1423059220209307, + "timestamp": 1.6809460171942534 + }, + { + "x": 7.428286998104642, + "y": 7.47296871812108, + "heading": -0.020071347748117502, + "angularVelocity": -0.13483892864997732, + "velocityX": 2.396352351501819, + "velocityY": 0.10725132111930623, + "timestamp": 1.740927117853665 + }, + { + "x": 7.556472866857842, + "y": 7.4774155432867, + "heading": -0.02498803078182015, + "angularVelocity": -0.08197053704667534, + "velocityX": 2.137104310257201, + "velocityY": 0.0741371051336659, + "timestamp": 1.8009082185130765 + }, + { + "x": 7.669092898448606, + "y": 7.47995645101358, + "heading": -0.027230015787281982, + "angularVelocity": -0.037378190476904, + "velocityX": 1.877591947341067, + "velocityY": 0.04236180561785803, + "timestamp": 1.860889319172488 + }, + { + "x": 7.766136403587462, + "y": 7.480650177988498, + "heading": -0.027158837231048453, + "angularVelocity": 0.001186683062681687, + "velocityX": 1.6179013734658474, + "velocityY": 0.011565759335732523, + "timestamp": 1.9208704198318995 + }, + { + "x": 7.847595735447885, + "y": 7.479541543526011, + "heading": -0.025049657080079103, + "angularVelocity": 0.03516407881452247, + "velocityX": 1.3580833123248393, + "velocityY": -0.018483063003166142, + "timestamp": 1.980851520491311 + }, + { + "x": 7.913465176456017, + "y": 7.4766658686921605, + "heading": -0.021118970688138776, + "angularVelocity": 0.06553208175121362, + "velocityX": 1.0981699282605077, + "velocityY": -0.04794301542047442, + "timestamp": 2.040832621150723 + }, + { + "x": 7.963740302847386, + "y": 7.472051704673601, + "heading": -0.015541591171907823, + "angularVelocity": 0.09298561471722216, + "velocityX": 0.8381827915570343, + "velocityY": -0.07692696479112789, + "timestamp": 2.100813721810134 + }, + { + "x": 7.998417597522527, + "y": 7.465722607991469, + "heading": -0.008461644038298986, + "angularVelocity": 0.11803629903043378, + "velocityX": 0.5781370180592035, + "velocityY": -0.10551818176978627, + "timestamp": 2.160794822469545 + }, + { + "x": 8.017494201660156, + "y": 7.457698345184326, + "heading": 0, + "angularVelocity": 0.14107183671647575, + "velocityX": 0.3180435825269393, + "velocityY": -0.1337798526356988, + "timestamp": 2.220775923128956 + }, + { + "x": 8.008975668857596, + "y": 7.440959395735479, + "heading": 0.016440344425850486, + "angularVelocity": 0.17454816573497248, + "velocityX": -0.09044179592138195, + "velocityY": -0.17771847395317164, + "timestamp": 2.3149639333931997 + }, + { + "x": 7.961981832760565, + "y": 7.4200887947149115, + "heading": 0.0360050890089686, + "angularVelocity": 0.20772011775415367, + "velocityX": -0.4989364990850843, + "velocityY": -0.22158447728129313, + "timestamp": 2.4091519436574433 + }, + { + "x": 7.876511570645417, + "y": 7.39509531765153, + "heading": 0.058657775076795544, + "angularVelocity": 0.24050498576490828, + "velocityX": -0.9074431222759898, + "velocityY": -0.2653573102697685, + "timestamp": 2.503339953921687 + }, + { + "x": 7.752563396875145, + "y": 7.365990633137338, + "heading": 0.08435039467917142, + "angularVelocity": 0.2727801503641011, + "velocityX": -1.315965518567977, + "velocityY": -0.3090062570866462, + "timestamp": 2.5975279641859306 + }, + { + "x": 7.590135252234061, + "y": 7.332791018782474, + "heading": 0.11301654484596554, + "angularVelocity": 0.3043503104733978, + "velocityX": -1.7245097776818146, + "velocityY": -0.35248238349789773, + "timestamp": 2.691715974450174 + }, + { + "x": 7.3892240901384, + "y": 7.295520773303632, + "heading": 0.14455765680612448, + "angularVelocity": 0.33487395977121315, + "velocityX": -2.13308638256617, + "velocityY": -0.39570052891318314, + "timestamp": 2.785903984714418 + }, + { + "x": 7.149824938411661, + "y": 7.254220108341429, + "heading": 0.1788109128374065, + "angularVelocity": 0.36366896312157765, + "velocityX": -2.54171577735964, + "velocityY": -0.4384917448232911, + "timestamp": 2.8800919949786614 + }, + { + "x": 6.871928226350113, + "y": 7.208968463075644, + "heading": 0.21545413209734332, + "angularVelocity": 0.3890433523028524, + "velocityX": -2.9504467849136042, + "velocityY": -0.4804395499897761, + "timestamp": 2.974280005242905 + }, + { + "x": 6.555508023323144, + "y": 7.15999689291417, + "heading": 0.25354713749374036, + "angularVelocity": 0.40443582245264, + "velocityX": -3.3594530995957568, + "velocityY": -0.5199342254293745, + "timestamp": 3.0684680155071486 + }, + { + "x": 6.201946981022424, + "y": 7.116596127430346, + "heading": 0.25354714311943394, + "angularVelocity": 5.972834101491316e-8, + "velocityX": -3.753779714730222, + "velocityY": -0.46078864350212084, + "timestamp": 3.1626560257713923 + }, + { + "x": 5.847829680383322, + "y": 7.077995922009697, + "heading": 0.25354714640605686, + "angularVelocity": 3.4894280958051e-8, + "velocityX": -3.7596855443238373, + "velocityY": -0.4098207968546854, + "timestamp": 3.256844036035636 + }, + { + "x": 5.493712379149646, + "y": 7.039395722043647, + "heading": 0.25354714969267966, + "angularVelocity": 3.4894280048226024e-8, + "velocityX": -3.759685550636459, + "velocityY": -0.40982073894285953, + "timestamp": 3.3510320462998795 + }, + { + "x": 5.13959507794107, + "y": 7.0007955218474756, + "heading": 0.25354715297933, + "angularVelocity": 3.489457219723442e-8, + "velocityX": -3.7596855503699804, + "velocityY": -0.4098207413860709, + "timestamp": 3.445220056564123 + }, + { + "x": 4.793375065328026, + "y": 6.963049240971564, + "heading": 0.2809926873119857, + "angularVelocity": 0.2913909557666365, + "velocityX": -3.675839543076952, + "velocityY": -0.4007546265178986, + "timestamp": 3.5394080668283667 + }, + { + "x": 4.4856271372174605, + "y": 6.929500155423233, + "heading": 0.3062072463057812, + "angularVelocity": 0.2677045509620205, + "velocityX": -3.2673790140292933, + "velocityY": -0.3561927410315718, + "timestamp": 3.6335960770926103 + }, + { + "x": 4.216349125910067, + "y": 6.900146018877801, + "heading": 0.32859640421091696, + "angularVelocity": 0.23770709076795637, + "velocityX": -2.858941499580855, + "velocityY": -0.31165470491497754, + "timestamp": 3.727784087356854 + }, + { + "x": 3.9855401854111148, + "y": 6.874986045056756, + "heading": 0.34796083888612495, + "angularVelocity": 0.2055934149249067, + "velocityX": -2.450512967111424, + "velocityY": -0.26712501676655553, + "timestamp": 3.8219720976210976 + }, + { + "x": 3.7931998687162816, + "y": 6.854019825581219, + "heading": 0.36419988308525797, + "angularVelocity": 0.17241094862896578, + "velocityX": -2.042089180514842, + "velocityY": -0.22259966440226955, + "timestamp": 3.916160107885341 + }, + { + "x": 3.639327899901621, + "y": 6.837247108195556, + "heading": 0.37725261237367164, + "angularVelocity": 0.13858164379727655, + "velocityX": -1.6336683234200817, + "velocityY": -0.17807699025180046, + "timestamp": 4.010348118149585 + }, + { + "x": 3.5239240923326007, + "y": 6.824667722043819, + "heading": 0.38707830352299477, + "angularVelocity": 0.10431997790119188, + "velocityX": -1.2252494478358333, + "velocityY": -0.13355613008965372, + "timestamp": 4.104536128413828 + }, + { + "x": 3.4469883122501845, + "y": 6.8162815454148475, + "heading": 0.39364810050149374, + "angularVelocity": 0.06975194571015403, + "velocityX": -0.8168319923796448, + "velocityY": -0.08903656214250527, + "timestamp": 4.198724138678072 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 0.03985171226128589, - "velocityX": -0.35766505888933303, - "velocityY": -0.04368010298204797, - "timestamp": 4.427741234210686 + "angularVelocity": 0.03495947190378589, + "velocityX": -0.4084155935928461, + "velocityY": -0.04451793673752315, + "timestamp": 4.292912148942316 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 5.690856102528897e-33, - "velocityX": 4.038395222816766e-35, - "velocityY": 1.1020881462714252e-31, - "timestamp": 4.520068762691695 + "angularVelocity": 0, + "velocityX": -1.9952146739162206e-34, + "velocityY": -3.951795765453016e-33, + "timestamp": 4.387100159206559 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.4.traj b/src/main/deploy/choreo/AmpLanePADESprint.4.traj index ba49e562..ab69c40b 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.4.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.4.traj @@ -4,640 +4,676 @@ "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 5.690856102528897e-33, - "velocityX": 4.038395222816766e-35, - "velocityY": 1.1020881462714252e-31, + "angularVelocity": 0, + "velocityX": -1.9952146739162206e-34, + "velocityY": -3.951795765453016e-33, "timestamp": 0 }, { - "x": 3.4229402433753, - "y": 6.8112531460784576, - "heading": 0.3945108838826677, - "angularVelocity": -0.039933151023800836, - "velocityX": 0.23696797878861645, - "velocityY": -0.013727643925556041, - "timestamp": 0.06085118892531316 - }, - { - "x": 3.451777703834916, - "y": 6.809543306220452, - "heading": 0.3896720623982013, - "angularVelocity": -0.07951893085286188, - "velocityX": 0.4739013480083423, - "velocityY": -0.02809870913293678, - "timestamp": 0.12170237785062632 - }, - { - "x": 3.4950303346590754, - "y": 6.80691471214731, - "heading": 0.3824488904717316, - "angularVelocity": -0.11870223169074358, - "velocityX": 0.7107935208504587, - "velocityY": -0.04319708652477366, - "timestamp": 0.18255356677593948 - }, - { - "x": 3.5526951141421756, - "y": 6.803316933089257, - "heading": 0.37286978619030314, - "angularVelocity": -0.15741852296732456, - "velocityX": 0.9476360363948891, - "velocityY": -0.05912421961826942, - "timestamp": 0.24340475570125264 - }, - { - "x": 3.624768348044375, - "y": 6.798691977814201, - "heading": 0.3609678412629577, - "angularVelocity": -0.1955910005629235, - "velocityX": 1.1844178425282745, - "velocityY": -0.07600435351777092, - "timestamp": 0.3042559446265658 - }, - { - "x": 3.7112454465000653, - "y": 6.7929724589112, - "heading": 0.3467818152826883, - "angularVelocity": -0.23312652112156756, - "velocityX": 1.4211242209553094, - "velocityY": -0.09399190063518563, - "timestamp": 0.36510713355187896 - }, - { - "x": 3.8121205998573195, - "y": 6.78607911243415, - "heading": 0.3303574905541313, - "angularVelocity": -0.2699096766821729, - "velocityX": 1.6577351262777436, - "velocityY": -0.1132820343988894, - "timestamp": 0.4259583224771921 - }, - { - "x": 3.927386292802013, - "y": 6.777917363603635, - "heading": 0.31174956650876195, - "angularVelocity": -0.30579392734968835, - "velocityX": 1.8942225284401175, - "velocityY": -0.1341263658879746, - "timestamp": 0.4868095114025053 - }, - { - "x": 4.057032548891421, - "y": 6.7683724330022, - "heading": 0.29102438798021885, - "angularVelocity": -0.34058789802744316, - "velocityX": 2.13054598240523, - "velocityY": -0.1568569286813843, - "timestamp": 0.5476607003278184 - }, - { - "x": 4.201045703157843, - "y": 6.75730211970409, - "heading": 0.2682640135119274, - "angularVelocity": -0.37403335695259643, - "velocityX": 2.3666448726775124, - "velocityY": -0.1819243550310427, - "timestamp": 0.6085118892531316 - }, - { - "x": 4.359406298184771, - "y": 6.744525706474344, - "heading": 0.2435725432613573, - "angularVelocity": -0.4057680825410948, - "velocityX": 2.6024240088603046, - "velocityY": -0.20996160396188043, - "timestamp": 0.6693630781784448 - }, - { - "x": 4.532085228362737, - "y": 6.729806001912393, - "heading": 0.2170864931038356, - "angularVelocity": -0.4352593700351523, - "velocityX": 2.837724837059597, - "velocityY": -0.24189674551828952, - "timestamp": 0.7302142671037579 - }, - { - "x": 4.71903604094914, - "y": 6.712818312739342, - "heading": 0.18899299215778056, - "angularVelocity": -0.46167546505190277, - "velocityX": 3.072262282596678, - "velocityY": -0.27916774467464406, - "timestamp": 0.7910654560290711 - }, - { - "x": 4.920177705816601, - "y": 6.693092062890064, - "heading": 0.1595647143501649, - "angularVelocity": -0.4836105641869241, - "velocityX": 3.3054681168898354, - "velocityY": -0.32417197096165007, - "timestamp": 0.8519166449543842 - }, - { - "x": 5.13534940168168, - "y": 6.66988755215724, - "heading": 0.12923590547960542, - "angularVelocity": -0.4984094708122179, - "velocityX": 3.536031089370736, - "velocityY": -0.3813320847568572, - "timestamp": 0.9127678338796974 - }, - { - "x": 5.364160197364154, - "y": 6.6418909322873, - "heading": 0.09880136738513437, - "angularVelocity": -0.5001469754654356, - "velocityX": 3.760169681537533, - "velocityY": -0.46008336672439587, - "timestamp": 0.9736190228050106 - }, - { - "x": 5.605261544574765, - "y": 6.606294945250104, - "heading": 0.07010036543409427, - "angularVelocity": -0.4716588526522781, - "velocityX": 3.9621468613626054, - "velocityY": -0.584967815187393, - "timestamp": 1.0344702117303237 + "x": 3.425127749537185, + "y": 6.810478633677757, + "heading": 0.3886892234850989, + "angularVelocity": -0.13293807639169805, + "velocityX": 0.2675517930120232, + "velocityY": -0.025935588272518423, + "timestamp": 0.0620713067232348 + }, + { + "x": 3.458347138585669, + "y": 6.807258412874914, + "heading": 0.3723514064813247, + "angularVelocity": -0.26321045691257755, + "velocityX": 0.535181081278085, + "velocityY": -0.05187937829625554, + "timestamp": 0.1241426134464696 + }, + { + "x": 3.508184213080087, + "y": 6.802427294600358, + "heading": 0.34812438720722594, + "angularVelocity": -0.3903094771652888, + "velocityX": 0.8029003596884762, + "velocityY": -0.07783174754300737, + "timestamp": 0.1862139201697044 + }, + { + "x": 3.5746455076736363, + "y": 6.795984642746885, + "heading": 0.31624502791908643, + "angularVelocity": -0.5135925272251962, + "velocityX": 1.070724914651617, + "velocityY": -0.10379436479708294, + "timestamp": 0.2482852268929392 + }, + { + "x": 3.6577387353786484, + "y": 6.787929625128416, + "heading": 0.2770021560106679, + "angularVelocity": -0.6322224225666108, + "velocityX": 1.3386737301264124, + "velocityY": -0.12977038898779852, + "timestamp": 0.310356533616174 + }, + { + "x": 3.7574730901724314, + "y": 6.7782611086258875, + "heading": 0.23075444743671972, + "angularVelocity": -0.7450738677080124, + "velocityX": 1.6067706652041787, + "velocityY": -0.1557646682971112, + "timestamp": 0.3724278403394088 + }, + { + "x": 3.873859634378807, + "y": 6.766977536857145, + "heading": 0.17795757602156642, + "angularVelocity": -0.8505841781386996, + "velocityX": 1.875045819887845, + "velocityY": -0.18178402170643457, + "timestamp": 0.4344991470626436 + }, + { + "x": 4.006911762109537, + "y": 6.754076769706622, + "heading": 0.11920696294593584, + "angularVelocity": -0.9465019535937051, + "velocityX": 2.1435367604550115, + "velocityY": -0.2078378534549514, + "timestamp": 0.4965704537858784 + }, + { + "x": 4.156645657189765, + "y": 6.73955582918857, + "heading": 0.055309270826440785, + "angularVelocity": -1.029423988195197, + "velocityX": 2.4122884305927053, + "velocityY": -0.23393966205310818, + "timestamp": 0.5586417605091132 + }, + { + "x": 4.323080375842652, + "y": 6.723410435666096, + "heading": -0.012586184407094544, + "angularVelocity": -1.0938299645646508, + "velocityX": 2.681347106079046, + "velocityY": -0.2601104177560936, + "timestamp": 0.620713067232348 + }, + { + "x": 4.506235840968647, + "y": 6.705634179985004, + "heading": -0.08270962096165607, + "angularVelocity": -1.1297238652836274, + "velocityX": 2.9507267495214404, + "velocityY": -0.2863844281597597, + "timestamp": 0.6827843739555828 + }, + { + "x": 4.706118949857515, + "y": 6.686217722826257, + "heading": -0.15194437984101536, + "angularVelocity": -1.1154068205470853, + "velocityX": 3.2202175117742615, + "velocityY": -0.3128088996947802, + "timestamp": 0.7448556806788176 + }, + { + "x": 4.922607059053276, + "y": 6.66515425622058, + "heading": -0.21301282950448638, + "angularVelocity": -0.9838434678965439, + "velocityX": 3.4877324262085807, + "velocityY": -0.3393430510421988, + "timestamp": 0.8069269874020524 + }, + { + "x": 5.152321807662402, + "y": 6.642265811014449, + "heading": -0.22692060137612477, + "angularVelocity": -0.22406120647088962, + "velocityX": 3.7008202458728654, + "velocityY": -0.3687443750482909, + "timestamp": 0.8689982941252872 + }, + { + "x": 5.385997622338085, + "y": 6.619824669792801, + "heading": -0.22692062699124488, + "angularVelocity": -4.12672480414244e-7, + "velocityX": 3.7646350143327725, + "velocityY": -0.36153808267225507, + "timestamp": 0.931069600848522 + }, + { + "x": 5.6195399174914655, + "y": 6.5960341766076915, + "heading": -0.2269206525768778, + "angularVelocity": -4.121974272362167e-7, + "velocityX": 3.7624839476103156, + "velocityY": -0.3832768221124699, + "timestamp": 0.9931409075717568 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": 0.04853422903733051, - "angularVelocity": -0.3544078066121842, - "velocityX": 4.0390769313634065, - "velocityY": -0.808003051659102, - "timestamp": 1.0953214006556369 - }, - { - "x": 6.0904224796154285, - "y": 6.495587769280379, - "heading": 0.021863961487268806, - "angularVelocity": -0.4346445536401508, - "velocityX": 3.9011410312083004, - "velocityY": -1.0029029869972568, - "timestamp": 1.1566824998036607 - }, - { - "x": 6.315513452841665, - "y": 6.430603187238758, - "heading": -0.005975333075399724, - "angularVelocity": -0.4536961519465426, - "velocityX": 3.6683008673498723, - "velocityY": -1.0590517924859284, - "timestamp": 1.2180435989516845 - }, - { - "x": 6.525908252987294, - "y": 6.365933697646677, - "heading": -0.03348393584839473, - "angularVelocity": -0.4483068777277778, - "velocityX": 3.428797773620156, - "velocityY": -1.053916740247379, - "timestamp": 1.2794046980997082 - }, - { - "x": 6.721741896707244, - "y": 6.303262822606784, - "heading": -0.05999730126977997, - "angularVelocity": -0.43208752433567293, - "velocityX": 3.1914950422829667, - "velocityY": -1.0213453785876696, - "timestamp": 1.340765797247732 - }, - { - "x": 6.903171972351107, - "y": 6.243508401888397, - "heading": -0.08515279346758128, - "angularVelocity": -0.4099583049696985, - "velocityX": 2.9567605235719823, - "velocityY": -0.9738160096226287, - "timestamp": 1.4021268963957558 - }, - { - "x": 7.07032746244464, - "y": 6.187241571909668, - "heading": -0.10872472462882488, - "angularVelocity": -0.3841510580568351, - "velocityX": 2.7241280292306773, - "velocityY": -0.9169788475104336, - "timestamp": 1.4634879955437787 - }, - { - "x": 7.223309884324563, - "y": 6.13485024328056, - "heading": -0.1305599608000083, - "angularVelocity": -0.3558481916777524, - "velocityX": 2.4931499598936324, - "velocityY": -0.8538199177743311, - "timestamp": 1.5248490946918016 - }, - { - "x": 7.362199686401887, - "y": 6.086614483409434, - "heading": -0.15054810335595573, - "angularVelocity": -0.3257461622017125, - "velocityX": 2.2634829559078664, - "velocityY": -0.7860967378495876, - "timestamp": 1.5862101938398245 - }, - { - "x": 7.487061657008568, - "y": 6.0427457582775, - "heading": -0.16860591224690066, - "angularVelocity": -0.29428757212095324, - "velocityX": 2.0348718054328225, - "velocityY": -0.7149273031453939, - "timestamp": 1.6475712929878474 - }, - { - "x": 7.597948869371604, - "y": 6.003409277701244, - "heading": -0.18466841772723513, - "angularVelocity": -0.26177017203662434, - "velocityX": 1.8071255877528956, - "velocityY": -0.6410654489966826, - "timestamp": 1.7089323921358703 - }, - { - "x": 7.694905491467594, - "y": 5.968737621588528, - "heading": -0.1986834963337821, - "angularVelocity": -0.2284033174297924, - "velocityX": 1.580099174268352, - "velocityY": -0.5650429440495477, - "timestamp": 1.7702934912838932 - }, - { - "x": 7.7779688005582, - "y": 5.938839509139965, - "heading": -0.21060838151890787, - "angularVelocity": -0.19433949767358227, - "velocityX": 1.3536802672036419, - "velocityY": -0.487248645537448, - "timestamp": 1.8316545904319161 - }, - { - "x": 7.847170649947212, - "y": 5.913805692553094, - "heading": -0.220407320835025, - "angularVelocity": -0.15969302134694188, - "velocityX": 1.1277804724793856, - "velocityY": -0.40797536117273564, - "timestamp": 1.893015689579939 - }, - { - "x": 7.902538555840789, - "y": 5.8937130619859, - "heading": -0.22804994580967364, - "angularVelocity": -0.12455163093170954, - "velocityX": 0.9023291085450026, - "velocityY": -0.3274490002000119, - "timestamp": 1.954376788727962 - }, - { - "x": 7.944096516826036, - "y": 5.878627591236197, - "heading": -0.23351010234644243, - "angularVelocity": -0.08898400798846613, - "velocityX": 0.6772688488678362, - "velocityY": -0.24584746621490028, - "timestamp": 2.015737887875985 - }, - { - "x": 7.97186564193086, - "y": 5.86860650570412, - "heading": -0.2367649883044089, - "angularVelocity": -0.05304477923569416, - "velocityX": 0.45255260238795525, - "velocityY": -0.16331333159306605, - "timestamp": 2.0770989870240077 + "heading": -0.22692067835854435, + "angularVelocity": -4.1535562716932914e-7, + "velocityX": 3.729650183615213, + "velocityY": -0.6268142199732243, + "timestamp": 1.0552122142949916 + }, + { + "x": 5.959383441341322, + "y": 6.535142573313356, + "heading": -0.22692070161190955, + "angularVelocity": -7.95525127627382e-7, + "velocityX": 3.706414340926548, + "velocityY": -0.7521132022680976, + "timestamp": 1.0844424226222174 + }, + { + "x": 6.066940887967902, + "y": 6.509606194124557, + "heading": -0.22692072465630755, + "angularVelocity": -7.883761123678933e-7, + "velocityX": 3.6796674667011553, + "velocityY": -0.8736297361581465, + "timestamp": 1.1136726309494431 + }, + { + "x": 6.17368571603366, + "y": 6.480861582677863, + "heading": -0.2269207475995078, + "angularVelocity": -7.849140177187675e-7, + "velocityX": 3.651866824580282, + "velocityY": -0.9833871563580519, + "timestamp": 1.142902839276669 + }, + { + "x": 6.2799139038286755, + "y": 6.450262838777624, + "heading": -0.2269207705194487, + "angularVelocity": -7.841182881547448e-7, + "velocityX": 3.63419194984251, + "velocityY": -1.0468192206387272, + "timestamp": 1.1721330476038947 + }, + { + "x": 6.38600768248991, + "y": 6.419201268635774, + "heading": -0.22692079344039778, + "angularVelocity": -7.841527793264923e-7, + "velocityX": 3.629593654398156, + "velocityY": -1.0626530537902528, + "timestamp": 1.2013632559311205 + }, + { + "x": 6.4915378820851375, + "y": 6.386275784732387, + "heading": -0.22692082347226308, + "angularVelocity": -0.0000010274256324387564, + "velocityX": 3.6103129479556895, + "velocityY": -1.1264197481865303, + "timestamp": 1.2305934642583463 + }, + { + "x": 6.594254502115419, + "y": 6.3529873750963075, + "heading": -0.23642872835410836, + "angularVelocity": -0.32527667183951403, + "velocityX": 3.514057063171563, + "velocityY": -1.1388358667671954, + "timestamp": 1.259823672585572 + }, + { + "x": 6.69366972769076, + "y": 6.320452523543764, + "heading": -0.25267195567048195, + "angularVelocity": -0.5557000187824542, + "velocityX": 3.4011124540202067, + "velocityY": -1.1130557534298289, + "timestamp": 1.2890538809127978 + }, + { + "x": 6.789488054498416, + "y": 6.288864279277486, + "heading": -0.26990654049679197, + "angularVelocity": -0.5896155317621415, + "velocityX": 3.278058292810801, + "velocityY": -1.0806711985311153, + "timestamp": 1.3182840892400236 + }, + { + "x": 6.881710669103943, + "y": 6.258257632428043, + "heading": -0.28694313831911705, + "angularVelocity": -0.5828421621787924, + "velocityX": 3.1550447254125045, + "velocityY": -1.047089589879878, + "timestamp": 1.3475142975672494 + }, + { + "x": 6.970345859458022, + "y": 6.228646491338886, + "heading": -0.3032367291527766, + "angularVelocity": -0.5574230142767849, + "velocityX": 3.0323146986098455, + "velocityY": -1.0130321603485573, + "timestamp": 1.3767445058944752 + }, + { + "x": 7.055400617247705, + "y": 6.200038168598195, + "heading": -0.3184704659597927, + "angularVelocity": -0.5211641544429054, + "velocityX": 2.909823865690951, + "velocityY": -0.978724558526454, + "timestamp": 1.405974714221701 + }, + { + "x": 7.136880378911089, + "y": 6.172437128670844, + "heading": -0.33243641899697507, + "angularVelocity": -0.477791772157844, + "velocityX": 2.7875190197494866, + "velocityY": -0.9442642220804569, + "timestamp": 1.4352049225489267 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.42938659336036833, + "velocityX": 2.665359438451487, + "velocityY": -0.9097014120066562, + "timestamp": 1.4644351308761525 + }, + { + "x": 7.370577859130934, + "y": 6.0916577592619054, + "heading": -0.3653758531922844, + "angularVelocity": -0.3132570133567013, + "velocityX": 2.393610638018096, + "velocityY": -0.8325804140238011, + "timestamp": 1.529520264645429 + }, + { + "x": 7.5087533836626745, + "y": 6.042843250581257, + "heading": -0.37947223160146565, + "angularVelocity": -0.21658368958955537, + "velocityX": 2.1229967049244562, + "velocityY": -0.7500101152698444, + "timestamp": 1.5946053984147053 + }, + { + "x": 7.629367562165718, + "y": 5.999619792415774, + "heading": -0.38804895009153, + "angularVelocity": -0.1317769203712161, + "velocityX": 1.85317554897581, + "velocityY": -0.6641064658284028, + "timestamp": 1.6596905321839817 + }, + { + "x": 7.7324579600950285, + "y": 5.962133690972344, + "heading": -0.39162657356882036, + "angularVelocity": -0.05496836635495089, + "velocityX": 1.5839315671495955, + "velocityY": -0.5759548958801737, + "timestamp": 1.7247756659532572 + }, + { + "x": 7.81805300550911, + "y": 5.930490256491027, + "heading": -0.39057997729782273, + "angularVelocity": 0.01608041975771331, + "velocityX": 1.3151243679933469, + "velocityY": -0.4861852876186942, + "timestamp": 1.7898607997225335 + }, + { + "x": 7.886174910867723, + "y": 5.904768911877698, + "heading": -0.38519213768662347, + "angularVelocity": 0.08278141718657105, + "velocityX": 1.0466584519915365, + "velocityY": -0.39519538677620675, + "timestamp": 1.85494593349181 + }, + { + "x": 7.936841490598973, + "y": 5.885031694896893, + "heading": -0.3756842946663964, + "angularVelocity": 0.1460831755210139, + "velocityX": 0.7784662456231665, + "velocityY": -0.3032523072130836, + "timestamp": 1.9200310672610863 + }, + { + "x": 7.970067341476633, + "y": 5.8713284070268905, + "heading": -0.36223415085714655, + "angularVelocity": 0.206654623418748, + "velocityX": 0.510498311264806, + "velocityY": -0.21054405324847428, + "timestamp": 1.9851162010303627 }, { "x": 7.985864639282227, "y": 5.863699913024902, - "heading": -0.2377945013589896, - "angularVelocity": -0.01677794349962979, - "velocityX": 0.22814124169445224, - "velocityY": -0.07996259433654461, - "timestamp": 2.1384600861720306 - }, - { - "x": 7.985275174373748, - "y": 5.8642853483331905, - "heading": -0.23637136146087387, - "angularVelocity": 0.021914153931364394, - "velocityX": -0.009076848143066944, - "velocityY": 0.00901479853081729, - "timestamp": 2.2034016715315685 - }, - { - "x": 7.9692615507734805, - "y": 5.87059931497324, - "heading": -0.23246035931952694, - "angularVelocity": 0.060223385673360406, - "velocityX": -0.24658504272125015, - "velocityY": 0.09722532342093965, - "timestamp": 2.2683432568911064 - }, - { - "x": 7.937802661499015, - "y": 5.8825854207762855, - "heading": -0.22608944217873808, - "angularVelocity": 0.09810227307383063, - "velocityX": -0.4844182521923094, - "velocityY": 0.18456749610726042, - "timestamp": 2.3332848422506443 - }, - { - "x": 7.890874700520602, - "y": 5.9001792828849275, - "heading": -0.21729032731874598, - "angularVelocity": 0.13549276340079022, - "velocityX": -0.722618037711973, - "velocityY": 0.27091827234023624, - "timestamp": 2.398226427610182 - }, - { - "x": 7.828450615730271, - "y": 5.923306702319573, - "heading": -0.20609939347439793, - "angularVelocity": 0.17232307746094241, - "velocityX": -0.9612343838656007, - "velocityY": 0.3561264990160738, - "timestamp": 2.46316801296972 - }, - { - "x": 7.750499406641646, - "y": 5.951881239016375, - "heading": -0.1925588645118683, - "angularVelocity": 0.20850320926975888, - "velocityX": -1.2003280895756, - "velocityY": 0.44000368236475507, - "timestamp": 2.528109598329258 - }, - { - "x": 7.6569852092442545, - "y": 5.985800920098193, - "heading": -0.1767184189861056, - "angularVelocity": 0.24391836814677034, - "velocityX": -1.4399740455929166, - "velocityY": 0.522310641078874, - "timestamp": 2.5930511836887957 - }, - { - "x": 7.5478660841549114, - "y": 6.024943657824972, - "heading": -0.15863743627693694, - "angularVelocity": 0.27841917638854113, - "velocityX": -1.6802658032634041, - "velocityY": 0.6027376373716655, - "timestamp": 2.6579927690483336 - }, - { - "x": 7.423092384274454, - "y": 6.069160679743017, - "heading": -0.13838822433863668, - "angularVelocity": 0.3118065539391131, - "velocityX": -1.9213220494952408, - "velocityY": 0.6808737679138067, - "timestamp": 2.7229343544078715 - }, - { - "x": 7.282604518065142, - "y": 6.118266769407389, - "heading": -0.11606081984557375, - "angularVelocity": 0.34380750592168463, - "velocityX": -2.163295913266127, - "velocityY": 0.7561578515908439, - "timestamp": 2.7878759397674093 - }, - { - "x": 7.126329839060539, - "y": 6.172025132426245, - "heading": -0.09177042771332564, - "angularVelocity": 0.37403448033749925, - "velocityX": -2.406388420292097, - "velocityY": 0.8277956677717754, - "timestamp": 2.852817525126947 - }, - { - "x": 6.954178295975344, - "y": 6.230122642965384, - "heading": -0.06566955623422352, - "angularVelocity": 0.40191306286409745, - "velocityX": -2.6508675778718325, - "velocityY": 0.8946118302701128, - "timestamp": 2.917759110486485 - }, - { - "x": 6.76603650192892, - "y": 6.292126520899546, - "heading": -0.037969143199572904, - "angularVelocity": 0.42654352956263775, - "velocityX": -2.897092718091327, - "velocityY": 0.9547638480164637, - "timestamp": 2.982700695846023 - }, - { - "x": 6.561760786884065, - "y": 6.357401498891707, - "heading": -0.008978614795991234, - "angularVelocity": 0.4464093114308898, - "velocityX": -3.145530154737028, - "velocityY": 1.0051337310411634, - "timestamp": 3.047642281205561 - }, - { - "x": 6.3411760884298385, - "y": 6.424931400851951, - "heading": 0.020808813203341207, - "angularVelocity": 0.4586803330165021, - "velocityX": -3.396663281824698, - "velocityY": 1.0398560735217077, - "timestamp": 3.1125838665650987 - }, - { - "x": 6.104131062172235, - "y": 6.4928665412990725, - "heading": 0.05050238274553203, - "angularVelocity": 0.4572350579031523, - "velocityX": -3.6501268785670193, - "velocityY": 1.0460961196282834, - "timestamp": 3.1775254519246365 + "heading": -0.344987478573796, + "angularVelocity": 0.2649863537883773, + "velocityX": 0.24271745160106833, + "velocityY": -0.1172079330593501, + "timestamp": 2.050201334799639 + }, + { + "x": 7.985045656249217, + "y": 5.861990715165632, + "heading": -0.3251381624172612, + "angularVelocity": 0.31897227576671205, + "velocityX": -0.01316080008970204, + "velocityY": -0.02746627272232015, + "timestamp": 2.1124303047275843 + }, + { + "x": 7.968302412952749, + "y": 5.865866788787815, + "heading": -0.3020436028872691, + "angularVelocity": 0.37112231741475454, + "velocityX": -0.26905866055398503, + "velocityY": 0.062287285594966935, + "timestamp": 2.1746592746555296 + }, + { + "x": 7.935633552055529, + "y": 5.875328955639361, + "heading": -0.2758355383990043, + "angularVelocity": 0.4211553641111372, + "velocityX": -0.5249783330022537, + "velocityY": 0.1520540491430483, + "timestamp": 2.236888244583475 + }, + { + "x": 7.887037558008595, + "y": 5.8903781286161365, + "heading": -0.24666763452179902, + "angularVelocity": 0.468719053376244, + "velocityX": -0.7809223598462517, + "velocityY": 0.24183548264097016, + "timestamp": 2.29911721451142 + }, + { + "x": 7.822512734192608, + "y": 5.911015324029987, + "heading": -0.21472164060862464, + "angularVelocity": 0.5133620876283883, + "velocityX": -1.0368936508301385, + "velocityY": 0.33163324795100674, + "timestamp": 2.3613461844393653 + }, + { + "x": 7.742057181096897, + "y": 5.937241677003945, + "heading": -0.1802161756247138, + "angularVelocity": 0.5544919837796514, + "velocityX": -1.29289546635384, + "velocityY": 0.4214492543316279, + "timestamp": 2.4235751543673105 + }, + { + "x": 7.645668783226514, + "y": 5.9690584605532635, + "heading": -0.14341967249459378, + "angularVelocity": 0.5913082471512265, + "velocityX": -1.5489312772168387, + "velocityY": 0.5112857176674774, + "timestamp": 2.4858041242952558 + }, + { + "x": 7.533345224831275, + "y": 6.006467108033043, + "heading": -0.10467019490543185, + "angularVelocity": 0.6226919332592115, + "velocityX": -1.8050043014579413, + "velocityY": 0.6011452145695896, + "timestamp": 2.548033094223201 + }, + { + "x": 7.405084087505747, + "y": 6.04946923502926, + "heading": -0.06440725119965417, + "angularVelocity": 0.6470128583583754, + "velocityX": -2.0611161887146876, + "velocityY": 0.6910306734308482, + "timestamp": 2.6102620641511463 + }, + { + "x": 7.260883177292953, + "y": 6.098066643466027, + "heading": -0.023226072117979224, + "angularVelocity": 0.6617686124221249, + "velocityX": -2.3172633321708815, + "velocityY": 0.7809450886466088, + "timestamp": 2.6724910340790915 + }, + { + "x": 7.100741532540205, + "y": 6.152261239581916, + "heading": 0.018021968863737303, + "angularVelocity": 0.6628430621538008, + "velocityX": -2.5734259290837285, + "velocityY": 0.8708901365174432, + "timestamp": 2.7347200040070367 + }, + { + "x": 6.924662708731195, + "y": 6.212054571383467, + "heading": 0.05802124542072086, + "angularVelocity": 0.6427758100977419, + "velocityX": -2.829531390490462, + "velocityY": 0.9608600603671181, + "timestamp": 2.796948973934982 + }, + { + "x": 6.732667433541327, + "y": 6.277445378277029, + "heading": 0.09448040335619008, + "angularVelocity": 0.5858872158366861, + "velocityX": -3.0853037646642187, + "velocityY": 1.050809726872821, + "timestamp": 2.8591779438629272 + }, + { + "x": 6.524862060286313, + "y": 6.348410321942265, + "heading": 0.12245319566137218, + "angularVelocity": 0.44951398581032764, + "velocityX": -3.33936707446111, + "velocityY": 1.1403843539014786, + "timestamp": 2.9214069137908725 + }, + { + "x": 6.30228026524581, + "y": 6.424202518084616, + "heading": 0.1231057628389814, + "angularVelocity": 0.010486549566300301, + "velocityX": -3.5768195311320152, + "velocityY": 1.2179567849204198, + "timestamp": 2.9836358837188177 + }, + { + "x": 6.078984969036473, + "y": 6.498549743391548, + "heading": 0.1231057748040032, + "angularVelocity": 1.9227414178241924e-7, + "velocityX": -3.5882852707972814, + "velocityY": 1.194736557475109, + "timestamp": 3.045864853646763 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": 0.07814732109541168, - "angularVelocity": 0.42568930519370946, - "velocityX": -3.897146685933666, - "velocityY": 0.9895116857176834, - "timestamp": 3.2424670372841744 - }, - { - "x": 5.590600161596063, - "y": 6.608035856956418, - "heading": 0.09892900965828218, - "angularVelocity": 0.32313056199342094, - "velocityX": -4.049594966104257, - "velocityY": 0.7915722470772912, - "timestamp": 3.3067806337284926 - }, - { - "x": 5.336533047176871, - "y": 6.64421005176547, - "heading": 0.1269873566210566, - "angularVelocity": 0.4362739531611655, - "velocityX": -3.9504417178591757, - "velocityY": 0.5624657429999524, - "timestamp": 3.371094230172811 - }, - { - "x": 5.09664977853333, - "y": 6.672673453532738, - "heading": 0.15653495714348034, - "angularVelocity": 0.45943007631373584, - "velocityX": -3.7298997709019446, - "velocityY": 0.4425720740389818, - "timestamp": 3.435407826617129 - }, - { - "x": 4.872168601658864, - "y": 6.69628481617622, - "heading": 0.18579353230897896, - "angularVelocity": 0.45493607546625786, - "velocityX": -3.4904155464050137, - "velocityY": 0.3671286314072656, - "timestamp": 3.4997214230614473 - }, - { - "x": 4.663439660624991, - "y": 6.716350544333371, - "heading": 0.214004287479728, - "angularVelocity": 0.43864371968645294, - "velocityX": -3.2454869976769207, - "velocityY": 0.3119982284698228, - "timestamp": 3.5640350195057655 - }, - { - "x": 4.470607956167166, - "y": 6.7336023293083125, - "heading": 0.24075299474172163, - "angularVelocity": 0.415910612076444, - "velocityX": -2.9983038598187646, - "velocityY": 0.2682447558329072, - "timestamp": 3.6283486159500837 - }, - { - "x": 4.293747206330888, - "y": 6.748505081783013, - "heading": 0.2657792876291017, - "angularVelocity": 0.3891291153193025, - "velocityX": -2.7499744939532724, - "velocityY": 0.23172009184096956, - "timestamp": 3.692662212394402 - }, - { - "x": 4.1329000630749535, - "y": 6.761379555283891, - "heading": 0.28890428844093546, - "angularVelocity": 0.35956628287542935, - "velocityX": -2.5009819408123937, - "velocityY": 0.20018276402913684, - "timestamp": 3.75697580883872 - }, - { - "x": 3.9880934920167372, - "y": 6.772460125748701, - "heading": 0.3099974247908716, - "angularVelocity": 0.32797320498470917, - "velocityX": -2.2515701043649203, - "velocityY": 0.17228970353732437, - "timestamp": 3.8212894052830384 - }, - { - "x": 3.859345677355417, - "y": 6.781925432988514, - "heading": 0.3289591507198067, - "angularVelocity": 0.29483230572172986, - "velocityX": -2.0018755252287743, - "velocityY": 0.1471742798275413, - "timestamp": 3.8856030017273566 - }, - { - "x": 3.746669497606821, - "y": 6.789916099525479, - "heading": 0.34571109262456645, - "angularVelocity": 0.2604727900617948, - "velocityX": -1.7519807004752146, - "velocityY": 0.12424536923360925, - "timestamp": 3.949916598171675 - }, - { - "x": 3.6500744318767597, - "y": 6.79654567470217, - "heading": 0.36019003249514225, - "angularVelocity": 0.2251303094690333, - "velocityX": -1.5019384868904437, - "velocityY": 0.10308201598445296, - "timestamp": 4.014230194615993 - }, - { - "x": 3.5695676775339344, - "y": 6.801907753643934, - "heading": 0.3723440332146637, - "angularVelocity": 0.18898026842651122, - "velocityX": -1.2517843627750997, - "velocityY": 0.08337395571411381, - "timestamp": 4.078543791060311 - }, - { - "x": 3.5051548414990994, - "y": 6.806080805460923, - "heading": 0.38212983379787385, - "angularVelocity": 0.1521575704708508, - "velocityX": -1.0015430577048066, - "velocityY": 0.06488599686074766, - "timestamp": 4.1428573875046295 - }, - { - "x": 3.456840386946828, - "y": 6.809131562318706, - "heading": 0.3895110346173986, - "angularVelocity": 0.11476890156368999, - "velocityX": -0.7512323555735491, - "velocityY": 0.047435643883240675, - "timestamp": 4.207170983948948 - }, - { - "x": 3.4246279326905182, - "y": 6.8111174678092326, - "heading": 0.3944567935981517, - "angularVelocity": 0.0769006750389882, - "velocityX": -0.5008653851942321, - "velocityY": 0.03087847049956041, - "timestamp": 4.271484580393266 + "heading": 0.12310578689683861, + "angularVelocity": 1.9432806685756468e-7, + "velocityX": -3.662936913327793, + "velocityY": 0.941318096340742, + "timestamp": 3.108093823574708 + }, + { + "x": 5.606427257807288, + "y": 6.600564902613298, + "heading": 0.1231057972666647, + "angularVelocity": 1.578555424299444e-7, + "velocityX": -3.7237014740499577, + "velocityY": 0.6612371129043467, + "timestamp": 3.173785695467891 + }, + { + "x": 5.35931577124569, + "y": 6.62625930787543, + "heading": 0.12310580746861746, + "angularVelocity": 1.5530007712924032e-7, + "velocityX": -3.7616752185629396, + "velocityY": 0.39113522756534835, + "timestamp": 3.2394775673610736 + }, + { + "x": 5.111994168115865, + "y": 6.649845615003531, + "heading": 0.1231058176602883, + "angularVelocity": 1.551435596748186e-7, + "velocityX": -3.764873735550995, + "velocityY": 0.35904452785959023, + "timestamp": 3.3051694392542563 + }, + { + "x": 4.868859374012009, + "y": 6.67294055859981, + "heading": 0.13716866748145323, + "angularVelocity": 0.21407290454489739, + "velocityX": -3.701139685883833, + "velocityY": 0.35156470550620644, + "timestamp": 3.370861311147439 + }, + { + "x": 4.644141674488456, + "y": 6.6943246178055915, + "heading": 0.16550217221046287, + "angularVelocity": 0.43130913935716303, + "velocityX": -3.4207839272559015, + "velocityY": 0.3255206251475534, + "timestamp": 3.4365531830406217 + }, + { + "x": 4.438149971299598, + "y": 6.713938607045374, + "heading": 0.1971398735951766, + "angularVelocity": 0.48160754858923427, + "velocityX": -3.1357258859027066, + "velocityY": 0.2985755874284469, + "timestamp": 3.5022450549338044 + }, + { + "x": 4.2509016025753406, + "y": 6.731775563046276, + "heading": 0.22901708759854217, + "angularVelocity": 0.48525354940713283, + "velocityX": -2.8504039134206605, + "velocityY": 0.27152455070706877, + "timestamp": 3.567936926826987 + }, + { + "x": 4.082393919350406, + "y": 6.747832566507683, + "heading": 0.25967107343501483, + "angularVelocity": 0.46663285659323517, + "velocityX": -2.565122265033534, + "velocityY": 0.24442907468850525, + "timestamp": 3.63362879872017 + }, + { + "x": 3.9326220842038664, + "y": 6.762107971713798, + "heading": 0.28824128245235414, + "angularVelocity": 0.43491239013245725, + "velocityX": -2.279914254690053, + "velocityY": 0.21730854662395976, + "timestamp": 3.6993206706133526 + }, + { + "x": 3.801581586235111, + "y": 6.77460070477556, + "heading": 0.314159509734349, + "angularVelocity": 0.3945423769342238, + "velocityX": -1.9947749119073828, + "velocityY": 0.19017167119358822, + "timestamp": 3.7650125425065353 + }, + { + "x": 3.6892685916749244, + "y": 6.785310000165767, + "heading": 0.3370219943875935, + "angularVelocity": 0.34802607985383205, + "velocityX": -1.7096939289964497, + "velocityY": 0.16302314246152652, + "timestamp": 3.830704414399718 + }, + { + "x": 3.595679898494151, + "y": 6.794235280663889, + "heading": 0.35652698961990326, + "angularVelocity": 0.2969164170572818, + "velocityX": -1.4246616892414212, + "velocityY": 0.135865826941808, + "timestamp": 3.8963962862929007 + }, + { + "x": 3.520812828526041, + "y": 6.801376095406927, + "heading": 0.37244068726433127, + "angularVelocity": 0.2422475899347831, + "velocityX": -1.1396702181031793, + "velocityY": 0.10870164812244498, + "timestamp": 3.9620881581860834 + }, + { + "x": 3.464665127058438, + "y": 6.806732084957018, + "heading": 0.38457706026543664, + "angularVelocity": 0.18474695044220302, + "velocityX": -0.8547130695088268, + "velocityY": 0.08153199773025213, + "timestamp": 4.027780030079266 + }, + { + "x": 3.4272348818352074, + "y": 6.810302960282004, + "heading": 0.39278518447772715, + "angularVelocity": 0.12494885555456782, + "velocityX": -0.5697850303930024, + "velocityY": 0.05435794752192765, + "timestamp": 4.093471901972449 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 0.038624336675044973, - "velocityX": -0.2504520576093102, - "velocityY": 0.015098233918219028, - "timestamp": 4.335798176837584 + "angularVelocity": 0.0632601721112495, + "velocityX": -0.28488184560874424, + "velocityY": 0.027180367966521624, + "timestamp": 4.1591637738656315 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, "angularVelocity": 0, - "velocityX": -2.188238548497709e-39, - "velocityY": -5.246059499257382e-38, - "timestamp": 4.400111773281902 + "velocityX": 1.9034366224257262e-34, + "velocityY": -2.547324229323533e-33, + "timestamp": 4.224855645758814 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.5.traj b/src/main/deploy/choreo/AmpLanePADESprint.5.traj index 47a28d7c..4848618c 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.5.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.5.traj @@ -5,198 +5,189 @@ "y": 6.812088489532471, "heading": 0.3969408636, "angularVelocity": 0, - "velocityX": -2.188238548497709e-39, - "velocityY": -5.246059499257382e-38, + "velocityX": 1.9034366224257262e-34, + "velocityY": -2.547324229323533e-33, "timestamp": 0 }, { - "x": 3.439618429683473, - "y": 6.811489460292554, - "heading": 0.3933053508875659, - "angularVelocity": -0.04072214124873058, - "velocityX": 0.34833488669246676, - "velocityY": -0.006709852295815464, - "timestamp": 0.08927606950303613 - }, - { - "x": 3.5018143627173566, - "y": 6.810291415046735, - "heading": 0.38603451078645395, - "angularVelocity": -0.08144220664715371, - "velocityX": 0.6966697053320448, - "velocityY": -0.013419556354671087, - "timestamp": 0.17855213900607225 - }, - { - "x": 3.5951082578030915, - "y": 6.808494374460801, - "heading": 0.37512964589240666, - "angularVelocity": -0.12214768139715662, - "velocityX": 1.0450045079836527, - "velocityY": -0.0201290289316947, - "timestamp": 0.2678282085091084 - }, - { - "x": 3.719500117899943, - "y": 6.8060983670587145, - "heading": 0.3605931109058113, - "angularVelocity": -0.1628267806536994, - "velocityX": 1.3933393437826105, - "velocityY": -0.026838182005813428, - "timestamp": 0.3571042780121445 - }, - { - "x": 3.874989949959029, - "y": 6.8031034297547235, - "heading": 0.3424282200177148, - "angularVelocity": -0.20346875696043784, - "velocityX": 1.7416742574424975, - "velocityY": -0.03354692159569182, - "timestamp": 0.44638034751518063 - }, - { - "x": 4.06157776435933, - "y": 6.799509608467501, - "heading": 0.32063912477995166, - "angularVelocity": -0.2440642308633667, - "velocityX": 2.0900092873595257, - "velocityY": -0.04025514684089667, - "timestamp": 0.5356564170182168 - }, - { - "x": 4.279263574094258, - "y": 6.795316958786605, - "heading": 0.2952306601922425, - "angularVelocity": -0.2846055469192104, - "velocityX": 2.438344462818484, - "velocityY": -0.046962749415771964, - "timestamp": 0.6249324865212529 - }, - { - "x": 4.52804739348343, - "y": 6.790525546657334, - "heading": 0.26620815675428894, - "angularVelocity": -0.3250871549286381, - "velocityX": 2.786679798674495, - "velocityY": -0.05366961332351963, - "timestamp": 0.714208556024289 - }, - { - "x": 4.807929235531671, - "y": 6.785135449055675, - "heading": 0.23357721651331317, - "angularVelocity": -0.3655060132308565, - "velocityX": 3.135015280200263, - "velocityY": -0.06037561500706378, - "timestamp": 0.8034846255273251 - }, - { - "x": 5.118909101830157, - "y": 6.779146754720857, - "heading": 0.19734345180613538, - "angularVelocity": -0.40586200656991683, - "velocityX": 3.483350779549144, - "velocityY": -0.06708062270387599, - "timestamp": 0.8927606950303613 - }, - { - "x": 5.4298901294687205, - "y": 6.773164805475954, - "heading": 0.1614535287560628, - "angularVelocity": -0.40201056397147766, - "velocityX": 3.483363787963201, - "velocityY": -0.06700506953546043, - "timestamp": 0.9820367645333974 - }, - { - "x": 5.709773101250217, - "y": 6.767781279592902, - "heading": 0.12915793357975602, - "angularVelocity": -0.36174974274834387, - "velocityX": 3.1350279345796865, - "velocityY": -0.06030200380705819, - "timestamp": 1.0713128340364335 - }, - { - "x": 5.958557987999821, - "y": 6.762996102236048, - "heading": 0.10045393097970265, - "angularVelocity": -0.32151956016698346, - "velocityX": 2.786691754402813, - "velocityY": -0.05359977632853685, - "timestamp": 1.1605889035394696 - }, - { - "x": 6.176244776203503, - "y": 6.758809206943986, - "heading": 0.07533966785700749, - "angularVelocity": -0.2813101345354471, - "velocityX": 2.43835542285245, - "velocityY": -0.04689829329818694, - "timestamp": 1.2498649730425058 - }, - { - "x": 6.36283345808173, - "y": 6.755220536230104, - "heading": 0.05381396772349776, - "angularVelocity": -0.24111388699496414, - "velocityX": 2.090019004161915, - "velocityY": -0.04019745418742752, - "timestamp": 1.339141042545542 - }, - { - "x": 6.5183240290861875, - "y": 6.752230041904144, - "heading": 0.03587615193014948, - "angularVelocity": -0.20092524114469607, - "velocityX": 1.7416825345247642, - "velocityY": -0.033497154865871154, - "timestamp": 1.428417112048578 - }, - { - "x": 6.642716486724279, - "y": 6.749837685266528, - "heading": 0.021525889401728494, - "angularVelocity": -0.16074030373764248, - "velocityX": 1.393346036967506, - "velocityY": -0.026797289026421714, - "timestamp": 1.5176931815516141 - }, - { - "x": 6.736010829843697, - "y": 6.748043437222755, - "heading": 0.010763075905262787, - "angularVelocity": -0.12055653386599446, - "velocityX": 1.0450095265030122, - "velocityY": -0.020097749080595733, - "timestamp": 1.6069692510546503 - }, - { - "x": 6.7982070581615215, - "y": 6.746847278346797, - "heading": 0.0035877434328324036, - "angularVelocity": -0.08037240564434038, - "velocityX": 0.6966730128694666, - "velocityY": -0.013398426729768601, - "timestamp": 1.6962453205576864 + "x": 3.4428034429775143, + "y": 6.811427919070607, + "heading": 0.39249813327278055, + "angularVelocity": -0.050100574262489476, + "velocityX": 0.38660845958378076, + "velocityY": -0.007449238878501296, + "timestamp": 0.08867623560446347 + }, + { + "x": 3.5113694006298597, + "y": 6.810106800279428, + "heading": 0.3836131655700934, + "angularVelocity": -0.10019558951868686, + "velocityX": 0.7732168284429818, + "velocityY": -0.014898228168725063, + "timestamp": 0.17735247120892694 + }, + { + "x": 3.614218332551768, + "y": 6.808125167746075, + "heading": 0.37028807906085554, + "angularVelocity": -0.1502667137188131, + "velocityX": 1.1598251912796755, + "velocityY": -0.022346827420505266, + "timestamp": 0.2660287068133904 + }, + { + "x": 3.7513502451988723, + "y": 6.8054830693306245, + "heading": 0.3525264976102127, + "angularVelocity": -0.20029697166969943, + "velocityX": 1.5464336269164098, + "velocityY": -0.02979488695523812, + "timestamp": 0.3547049424178539 + }, + { + "x": 3.9227651512282486, + "y": 6.802180567172425, + "heading": 0.3303333796886753, + "angularVelocity": -0.2502713130553812, + "velocityX": 1.9330422052867193, + "velocityY": -0.03724224574585722, + "timestamp": 0.44338117802231736 + }, + { + "x": 4.128463068361194, + "y": 6.79821773882544, + "heading": 0.3037147939289921, + "angularVelocity": -0.3001772186001935, + "velocityX": 2.319650983499712, + "velocityY": -0.04468872996211646, + "timestamp": 0.5320574136267808 + }, + { + "x": 4.368444017678072, + "y": 6.793594678448866, + "heading": 0.2726776385751228, + "angularVelocity": -0.3500053327963678, + "velocityX": 2.706259999435507, + "velocityY": -0.05213415234714548, + "timestamp": 0.6207336492312443 + }, + { + "x": 4.642708020512335, + "y": 6.788311497970363, + "heading": 0.23722930806330966, + "angularVelocity": -0.3997500600942169, + "velocityX": 3.092869255948188, + "velocityY": -0.05957831252634942, + "timestamp": 0.7094098848357078 + }, + { + "x": 4.951255088173746, + "y": 6.782368328182758, + "heading": 0.19737735072702745, + "angularVelocity": -0.44940966499796203, + "velocityX": 3.479478640000833, + "velocityY": -0.0670209977576627, + "timestamp": 0.7980861204401712 + }, + { + "x": 5.286562692788269, + "y": 6.775920770205836, + "heading": 0.19737734601955828, + "angularVelocity": -5.3086028753514235e-8, + "velocityX": 3.781256639153567, + "velocityY": -0.07270897251075666, + "timestamp": 0.8867623560446347 + }, + { + "x": 5.595111114675473, + "y": 6.769985794431735, + "heading": 0.15789301075905496, + "angularVelocity": -0.4452639987631135, + "velocityX": 3.4794939115759345, + "velocityY": -0.06692859404378032, + "timestamp": 0.9754385916490982 + }, + { + "x": 5.8693764275565705, + "y": 6.76471055259859, + "heading": 0.12280143262174693, + "angularVelocity": -0.3957269712466434, + "velocityX": 3.092884029318143, + "velocityY": -0.059488788593539996, + "timestamp": 1.0641148272535617 + }, + { + "x": 6.109358599544783, + "y": 6.760094936608861, + "heading": 0.09209943623091729, + "angularVelocity": -0.3462257523850543, + "velocityX": 2.7062737874738274, + "velocityY": -0.05205020215694004, + "timestamp": 1.1527910628580251 + }, + { + "x": 6.315057615509001, + "y": 6.756138852616793, + "heading": 0.06578495581515914, + "angularVelocity": -0.29674782918315123, + "velocityX": 2.3196633749963063, + "velocityY": -0.04461267401690494, + "timestamp": 1.2414672984624886 + }, + { + "x": 6.4864734670806214, + "y": 6.752842221795132, + "heading": 0.043856783005957245, + "angularVelocity": -0.24728353272698234, + "velocityX": 1.9330528681462538, + "velocityY": -0.037176034810112814, + "timestamp": 1.330143534066952 + }, + { + "x": 6.623606149810205, + "y": 6.750204980767773, + "heading": 0.026314311963007715, + "angularVelocity": -0.19782606831887856, + "velocityX": 1.5464423111199512, + "velocityY": -0.029740110294281013, + "timestamp": 1.4188197696714155 + }, + { + "x": 6.726455661694478, + "y": 6.74822708185799, + "heading": 0.013157327563715553, + "angularVelocity": -0.14837102984364775, + "velocityX": 1.1598317315027789, + "velocityY": -0.022304723427886322, + "timestamp": 1.507496005275879 + }, + { + "x": 6.795022002255089, + "y": 6.7469084932218335, + "heading": 0.004385844550104781, + "angularVelocity": -0.09891582512292933, + "velocityX": 0.7732211464912397, + "velocityY": -0.014869695664985829, + "timestamp": 1.5961722408803425 }, { "x": 6.829305171966553, "y": 6.746249198913574, - "heading": -2.600985796322667e-43, - "angularVelocity": -0.04018706751768873, - "velocityX": 0.34833650247083475, - "velocityY": -0.006699213311607584, - "timestamp": 1.7855213900607225 + "heading": -4.086871260037018e-38, + "angularVelocity": -0.04945907457853377, + "velocityX": 0.3866105668308079, + "velocityY": -0.007434847721776456, + "timestamp": 1.684848476484806 }, { "x": 6.829305171966553, "y": 6.746249198913574, - "heading": 1.4984085590895388e-43, - "angularVelocity": 4.5922760731607736e-42, - "velocityX": 5.97613511124824e-44, - "velocityY": 5.676824977871388e-42, - "timestamp": 1.8747974595637587 + "heading": -5.480350490542979e-38, + "angularVelocity": -1.571392014077605e-37, + "velocityX": 4.425248303324229e-37, + "velocityY": 1.8999126670467387e-37, + "timestamp": 1.7735247120892694 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.traj b/src/main/deploy/choreo/AmpLanePADESprint.traj index a03ddd49..c05a0903 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.traj @@ -3,1505 +3,1577 @@ { "x": 0.43297290802001953, "y": 6.9807281494140625, - "heading": -2.898893570682157e-41, - "angularVelocity": 0, - "velocityX": -2.361892666986074e-43, - "velocityY": -3.008824272021237e-41, + "heading": 0, + "angularVelocity": 2.735053895521923e-38, + "velocityX": -3.8673469053295564e-39, + "velocityY": 1.858643532249755e-38, "timestamp": 0 }, { - "x": 0.4587135721409647, - "y": 6.976639655231649, - "heading": 0.007035495853073787, - "angularVelocity": 0.08601037544230704, - "velocityX": 0.3146848824037811, - "velocityY": -0.04998267740708988, - "timestamp": 0.08179822279456238 - }, - { - "x": 0.5101949084110605, - "y": 6.968462527202184, - "heading": 0.02110609468280026, - "angularVelocity": 0.17201594789002958, - "velocityX": 0.6293698629540154, - "velocityY": -0.09996706224293399, - "timestamp": 0.16359644558912476 - }, - { - "x": 0.5874169050244689, - "y": 6.956196559867369, - "heading": 0.04221287349505025, - "angularVelocity": 0.25803468695475235, - "velocityX": 0.944054699175467, - "velocityY": -0.14995395884848578, - "timestamp": 0.24539466838368712 - }, - { - "x": 0.6903795277325874, - "y": 6.939841483301487, - "heading": 0.07035879886747527, - "angularVelocity": 0.34408969303787923, - "velocityX": 1.2587391167007476, - "velocityY": -0.19994415535112514, - "timestamp": 0.3271928911782495 - }, - { - "x": 0.8190827157007662, - "y": 6.919396965353516, - "heading": 0.10554932385802422, - "angularVelocity": 0.4302113638694884, - "velocityX": 1.5734227905099982, - "velocityY": -0.24993841246792703, - "timestamp": 0.4089911139728119 - }, - { - "x": 0.97352637537035, - "y": 6.894862615393891, - "heading": 0.14779318141850775, - "angularVelocity": 0.5164397968227219, - "velocityX": 1.8881053205455545, - "velocityY": -0.29993744511103754, - "timestamp": 0.4907893367673743 - }, - { - "x": 1.1537103713281627, - "y": 6.866237990592905, - "heading": 0.197103396735309, - "angularVelocity": 0.6028274653429168, - "velocityX": 2.2027861951273398, - "velocityY": -0.3499418914378741, - "timestamp": 0.5725875595619366 - }, - { - "x": 1.3596345093826188, - "y": 6.8335226068750465, - "heading": 0.2534985369685149, - "angularVelocity": 0.6894421211918415, - "velocityX": 2.51746469567729, - "velocityY": -0.39995225568682646, - "timestamp": 0.654385782356499 - }, - { - "x": 1.565548746560234, - "y": 6.800784734541898, - "heading": 0.31096699128340616, - "angularVelocity": 0.7025636053148037, - "velocityX": 2.517343655433349, - "velocityY": -0.40022718360727877, - "timestamp": 0.7361840051510614 - }, - { - "x": 1.7457232772224913, - "y": 6.772138465169413, - "heading": 0.3613042914743826, - "angularVelocity": 0.6153837879509854, - "velocityX": 2.202670479953688, - "velocityY": -0.35020650075038995, - "timestamp": 0.8179822279456238 - }, - { - "x": 1.9001583012776542, - "y": 6.747584097506383, - "heading": 0.40448766007892617, - "angularVelocity": 0.5279255114502831, - "velocityX": 1.887999748393421, - "velocityY": -0.3001821656284431, - "timestamp": 0.8997804507401862 - }, - { - "x": 2.028853992721499, - "y": 6.727121858473899, - "heading": 0.44049724757544967, - "angularVelocity": 0.4402245704892907, - "velocityX": 1.5733311439671065, - "velocityY": -0.2501550563497558, - "timestamp": 0.9815786735347486 - }, - { - "x": 2.1318104848303743, - "y": 6.710751914339462, - "heading": 0.4693170803360535, - "angularVelocity": 0.3523283486608914, - "velocityX": 1.258664168871395, - "velocityY": -0.20012591441688285, - "timestamp": 1.0633768963293109 - }, - { - "x": 2.2090278625669106, - "y": 6.698474380778823, - "heading": 0.49093579476282356, - "angularVelocity": 0.2642932045243309, - "velocityX": 0.9439982324612256, - "velocityY": -0.1500953583242809, - "timestamp": 1.1451751191238733 - }, - { - "x": 2.2605061577286842, - "y": 6.690289331413325, - "heading": 0.5053471622954702, - "angularVelocity": 0.17618191496459498, - "velocityX": 0.6293326847829228, - "velocityY": -0.10006390219570446, - "timestamp": 1.2269733419184357 + "x": 0.46193105617219093, + "y": 6.976128411049919, + "heading": 0.007924686694784022, + "angularVelocity": 0.0965568963773332, + "velocityX": 0.35283526252830383, + "velocityY": -0.05604467125266167, + "timestamp": 0.08207271559159549 + }, + { + "x": 0.5198473628641517, + "y": 6.966928755942856, + "heading": 0.023773564700110772, + "angularVelocity": 0.19310775683593598, + "velocityX": 0.7056706516226394, + "velocityY": -0.11209151592888836, + "timestamp": 0.16414543118319097 + }, + { + "x": 0.6067218129336875, + "y": 6.9531289220308485, + "heading": 0.04754806846022335, + "angularVelocity": 0.2896760950181983, + "velocityX": 1.0585058559757474, + "velocityY": -0.16814155365198188, + "timestamp": 0.24621814677478646 + }, + { + "x": 0.7225543617111448, + "y": 6.934728565336742, + "heading": 0.07925217561750111, + "angularVelocity": 0.38629289805690764, + "velocityX": 1.4113405160596262, + "velocityY": -0.22419578250180797, + "timestamp": 0.32829086236638194 + }, + { + "x": 0.8673449286095066, + "y": 6.9117272629835975, + "heading": 0.11889328943914418, + "angularVelocity": 0.48299990485147826, + "velocityX": 1.764174194245729, + "velocityY": -0.2802551638184133, + "timestamp": 0.4103635779579774 + }, + { + "x": 1.0410933872613453, + "y": 6.884124518124938, + "heading": 0.16648343863275697, + "angularVelocity": 0.5798534732349732, + "velocityX": 2.117006332731008, + "velocityY": -0.33632059887008814, + "timestamp": 0.49243629354957286 + }, + { + "x": 1.243799549589456, + "y": 6.851919767180746, + "heading": 0.22204089753091144, + "angularVelocity": 0.6769297018830912, + "velocityX": 2.4698361796239694, + "velocityY": -0.39239290075946437, + "timestamp": 0.5745090091411683 + }, + { + "x": 1.4754631289031934, + "y": 6.815112383244328, + "heading": 0.28559273385120976, + "angularVelocity": 0.7743357321882771, + "velocityX": 2.8226625333871667, + "velocityY": -0.44847284107882956, + "timestamp": 0.6565817247327638 + }, + { + "x": 1.6781593090979035, + "y": 6.782884452250807, + "heading": 0.3422497740441209, + "angularVelocity": 0.6903273491624664, + "velocityX": 2.4697145541443093, + "velocityY": -0.3926753338331338, + "timestamp": 0.7386544403243592 + }, + { + "x": 1.8518984587746443, + "y": 6.755259929387126, + "heading": 0.39086385083963315, + "angularVelocity": 0.5923293319234401, + "velocityX": 2.1168929092256374, + "velocityY": -0.33658594899118865, + "timestamp": 0.8207271559159547 + }, + { + "x": 1.9966808291576696, + "y": 6.732239139219163, + "heading": 0.43140805286474204, + "angularVelocity": 0.4940034179795163, + "velocityX": 1.7640743253029585, + "velocityY": -0.28049260953564537, + "timestamp": 0.9027998715075501 + }, + { + "x": 2.1125066103123933, + "y": 6.7138223061958735, + "heading": 0.463861007608982, + "angularVelocity": 0.39541709458878044, + "velocityX": 1.4112580571975715, + "velocityY": -0.22439653532283038, + "timestamp": 0.9848725870991456 + }, + { + "x": 2.199375929341993, + "y": 6.700009583150749, + "heading": 0.4882072246931022, + "angularVelocity": 0.29664203150374724, + "velocityX": 1.0584433377576143, + "velocityY": -0.16829859893826818, + "timestamp": 1.0669453026907412 + }, + { + "x": 2.257288845375976, + "y": 6.690801066111525, + "heading": 0.5044377142261048, + "angularVelocity": 0.19775743273523133, + "velocityX": 0.7056293387703308, + "velocityY": -0.11219949252132488, + "timestamp": 1.1490180182823366 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 0.0880612934907036, - "velocityX": 0.31466684068792017, - "velocityY": -0.05003198390964021, - "timestamp": 1.308771564712998 + "angularVelocity": 0.0988477756026137, + "velocityX": 0.3528151893675758, + "velocityY": -0.0560997894575031, + "timestamp": 1.231090733873932 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 1.0872291890838523e-40, - "velocityX": -1.1027466263693948e-42, - "velocityY": -6.469494292232001e-38, - "timestamp": 1.3905697875075604 + "angularVelocity": 0, + "velocityX": 3.087311543739499e-36, + "velocityY": 0, + "timestamp": 1.3131634494655275 }, { - "x": 2.3177158255221877, - "y": 6.704097470712436, + "x": 2.3177158256290964, + "y": 6.704097470773247, "heading": 0.5125504196, - "angularVelocity": 2.0395665276637554e-17, - "velocityX": 0.32679827958625257, - "velocityY": 0.18588554008515779, - "timestamp": 1.4868691912030012 + "angularVelocity": -9.1973743139294e-17, + "velocityX": 0.34546318060283326, + "velocityY": 0.1965022887733569, + "timestamp": 1.4042599289943862 }, { - "x": 2.3806567825560636, - "y": 6.739898802979336, + "x": 2.380656782769881, + "y": 6.7398988031009575, "heading": 0.5125504196, - "angularVelocity": 1.2188419350872931e-17, - "velocityX": 0.653596539734919, - "velocityY": 0.37177106911405733, - "timestamp": 1.583168594898442 + "angularVelocity": -1.3336684994607708e-16, + "velocityX": 0.6909263394843395, + "velocityY": 0.3930045651914449, + "timestamp": 1.495356408523245 }, { "x": 2.475068211555481, "y": 6.793600797653198, "heading": 0.5125504196, - "angularVelocity": 9.65238482155011e-17, - "velocityX": 0.9803947415708354, - "velocityY": 0.5576565649741866, - "timestamp": 1.6794679985938827 + "angularVelocity": 6.653839443641591e-19, + "velocityX": 1.0363894332018742, + "velocityY": 0.5895068045437317, + "timestamp": 1.5864528880521036 }, { - "x": 2.569479640554898, - "y": 6.84730279232706, + "x": 2.569479640341081, + "y": 6.847302792205439, "heading": 0.5125504196, - "angularVelocity": 2.650620165495025e-16, - "velocityX": 0.9803947415708352, - "velocityY": 0.5576565649741866, - "timestamp": 1.7757674022893235 + "angularVelocity": 5.174999729369457e-17, + "velocityX": 1.0363894332018742, + "velocityY": 0.5895068045437317, + "timestamp": 1.6775493675809623 }, { - "x": 2.6324205975887742, - "y": 6.88310412459396, + "x": 2.6324205974818655, + "y": 6.8831041245331495, "heading": 0.5125504196, - "angularVelocity": 1.8072666853064398e-16, - "velocityX": 0.653596539734919, - "velocityY": 0.3717710691140574, - "timestamp": 1.8720668059847643 + "angularVelocity": -1.3901806543706914e-17, + "velocityX": 0.6909263394843393, + "velocityY": 0.39300456519144494, + "timestamp": 1.768645847109821 }, { "x": 2.663891077041626, "y": 6.901004791259766, "heading": 0.5125504196, - "angularVelocity": 1.455758590389613e-16, - "velocityX": 0.3267982795862525, - "velocityY": 0.18588554008515784, - "timestamp": 1.968366209680205 + "angularVelocity": -1.8725484396545262e-17, + "velocityX": 0.3454631806028332, + "velocityY": 0.19650228877335693, + "timestamp": 1.8597423266386797 }, { "x": 2.663891077041626, "y": 6.901004791259766, "heading": 0.5125504196, - "angularVelocity": 5.011309571958317e-32, - "velocityX": 1.411034806804274e-32, - "velocityY": -7.886289900823042e-34, - "timestamp": 2.0646656133756456 - }, - { - "x": 2.6921026735754903, - "y": 6.9044448198890755, - "heading": 0.509005120328227, - "angularVelocity": -0.041542724029198924, - "velocityX": 0.3305747919676489, - "velocityY": 0.04030919509045999, - "timestamp": 2.15000665334407 - }, - { - "x": 2.7485258570604016, - "y": 6.911324890060512, - "heading": 0.5019144397700286, - "angularVelocity": -0.08308640908081097, - "velocityX": 0.6611494716467927, - "velocityY": 0.0806185414893288, - "timestamp": 2.235347693312494 - }, - { - "x": 2.8331606213159497, - "y": 6.921645023106144, - "heading": 0.4912793044898821, - "angularVelocity": -0.12461923693549357, - "velocityX": 0.9917240789057943, - "velocityY": 0.12092813785079831, - "timestamp": 2.3206887332809183 - }, - { - "x": 2.946006963275403, - "y": 6.935405249135601, - "heading": 0.4771015882559512, - "angularVelocity": -0.16613010855242188, - "velocityX": 1.3222986502297882, - "velocityY": 0.16123808702762443, - "timestamp": 2.4060297732493425 - }, - { - "x": 3.0870648825703277, - "y": 6.952605607471804, - "heading": 0.45938402488204233, - "angularVelocity": -0.20760894618186487, - "velocityX": 1.6528732172365783, - "velocityY": 0.20154849697831853, - "timestamp": 2.4913708132177668 - }, - { - "x": 3.2563343809791183, - "y": 6.973246147139034, - "heading": 0.438130093703468, - "angularVelocity": -0.24904701403261814, - "velocityX": 1.9834478050820517, - "velocityY": 0.24185948138044716, - "timestamp": 2.576711853186191 - }, - { - "x": 3.453815461723479, - "y": 6.997326927370896, - "heading": 0.41334387603725764, - "angularVelocity": -0.29043725826848604, - "velocityX": 2.314022430678465, - "velocityY": 0.2821711598636699, - "timestamp": 2.6620528931546152 - }, - { - "x": 3.679508128581092, - "y": 7.024848018097057, - "heading": 0.3850298811864858, - "angularVelocity": -0.3317746638809172, - "velocityX": 2.644597100540597, - "velocityY": 0.32248365776117877, - "timestamp": 2.7473939331230395 - }, - { - "x": 3.9334123847380087, - "y": 7.055809500355339, - "heading": 0.35319284126218836, - "angularVelocity": -0.3730566200749024, - "velocityX": 2.97517180773588, - "velocityY": 0.3627971052349304, - "timestamp": 2.8327349730914637 - }, - { - "x": 4.215528231151698, - "y": 7.090211466549705, - "heading": 0.31783747613011065, - "angularVelocity": -0.4142832703369811, - "velocityX": 3.3057465261505037, - "velocityY": 0.4031116354698126, - "timestamp": 2.918076013059888 - }, - { - "x": 4.525855663499604, - "y": 7.128054020379336, - "heading": 0.2789682373559137, - "angularVelocity": -0.45545775852483755, - "velocityX": 3.636321193914748, - "velocityY": 0.4434273808197387, - "timestamp": 3.003417053028312 - }, - { - "x": 4.864394661167, - "y": 7.169337275538201, - "heading": 0.23658909177211548, - "angularVelocity": -0.49658576459202225, - "velocityX": 3.966895620121981, - "velocityY": 0.48374445840054986, - "timestamp": 3.0887580929967364 - }, - { - "x": 5.222418230866736, - "y": 7.213006191908579, - "heading": 0.23658908655312502, - "angularVelocity": -6.115452145733656e-8, - "velocityX": 4.195209829083447, - "velocityY": 0.511698901097697, - "timestamp": 3.1740991329651607 - }, - { - "x": 5.580441834844563, - "y": 7.2566748272476005, - "heading": 0.2365890813344374, - "angularVelocity": -6.115097279558182e-8, - "velocityX": 4.195210230743547, - "velocityY": 0.5116956080589027, - "timestamp": 3.259440172933585 - }, - { - "x": 5.9337628383316385, - "y": 7.293044212723314, - "heading": 0.21681067321560563, - "angularVelocity": -0.23175728964809564, - "velocityX": 4.1401066077681214, - "velocityY": 0.4261652481522448, - "timestamp": 3.344781212902009 - }, - { - "x": 6.258898803951296, - "y": 7.325913470340443, - "heading": 0.1915866655524086, - "angularVelocity": -0.2955671465045406, - "velocityX": 3.80984302206719, - "velocityY": 0.3851518288186972, - "timestamp": 3.4301222528704334 - }, - { - "x": 6.555814253467, - "y": 7.355310299665644, - "heading": 0.1654932429147573, - "angularVelocity": -0.30575468317829, - "velocityX": 3.47916371332669, - "velocityY": 0.344462984468874, - "timestamp": 3.5154632928388576 - }, - { - "x": 6.82450746796162, - "y": 7.381244823587674, - "heading": 0.13997843257634243, - "angularVelocity": -0.29897468261290555, - "velocityX": 3.1484642628451307, - "velocityY": 0.30389275700912277, - "timestamp": 3.600804332807282 - }, - { - "x": 7.064979286092556, - "y": 7.4037222220368, - "heading": 0.11575388397248855, - "angularVelocity": -0.2838557933301128, - "velocityX": 2.8177746394924394, - "velocityY": 0.26338322637552325, - "timestamp": 3.686145372775706 - }, - { - "x": 7.277230735069167, - "y": 7.422745623676577, - "heading": 0.0932430635255194, - "angularVelocity": -0.26377485504392817, - "velocityX": 2.4870970526623895, - "velocityY": 0.22291035645705817, - "timestamp": 3.7714864127441303 - }, - { - "x": 7.461262714918097, - "y": 7.43831711432498, - "heading": 0.07272706483914178, - "angularVelocity": -0.24040014855652678, - "velocityX": 2.15643001206713, - "velocityY": 0.18246192751066057, - "timestamp": 3.8568274527125546 - }, - { - "x": 7.61707597350389, - "y": 7.450438179449409, - "heading": 0.054406235810438235, - "angularVelocity": -0.21467782716829087, - "velocityX": 1.8257717347180626, - "velocityY": 0.14203090481336236, - "timestamp": 3.942168492680979 - }, - { - "x": 7.744671129130212, - "y": 7.459109928713578, - "heading": 0.038430689484197006, - "angularVelocity": -0.1871965273935276, - "velocityX": 1.4951207024607605, - "velocityY": 0.10161288481341456, - "timestamp": 4.027509532649403 - }, - { - "x": 7.84404869662698, - "y": 7.464333222010732, - "heading": 0.02491711833560148, - "angularVelocity": -0.15834786116498592, - "velocityX": 1.1644757028217392, - "velocityY": 0.061204940777464564, - "timestamp": 4.112850572617827 - }, - { - "x": 7.915209108962923, - "y": 7.466108745654989, - "heading": 0.013958811300242875, - "angularVelocity": -0.12840606394547271, - "velocityX": 0.8338357765768063, - "velocityY": 0.02080503875876404, - "timestamp": 4.198191612586251 - }, - { - "x": 7.958152733899896, - "y": 7.464437061148586, - "heading": 0.005631988057869677, - "angularVelocity": -0.09757114801336027, - "velocityX": 0.5032001596519237, - "velocityY": -0.0195882837497823, - "timestamp": 4.283532652554674 - }, - { - "x": 7.972879886627197, - "y": 7.4593186378479, + "angularVelocity": 1.431796971368011e-33, + "velocityX": 3.743360269099248e-35, + "velocityY": -8.057233109044706e-35, + "timestamp": 1.9508388061675384 + }, + { + "x": 2.6835271230542426, + "y": 6.903699548014665, + "heading": 0.5052880772246282, + "angularVelocity": -0.10752621184517612, + "velocityX": 0.2907312178663514, + "velocityY": 0.039898557617057115, + "timestamp": 2.018379011023157 + }, + { + "x": 2.7228033814945674, + "y": 6.909089957964811, + "heading": 0.49095749727137794, + "angularVelocity": -0.21217850884350908, + "velocityX": 0.5815241236576919, + "velocityY": 0.0798103879262595, + "timestamp": 2.0859192158787754 + }, + { + "x": 2.7817246923505206, + "y": 6.91717710303389, + "heading": 0.46979167660622734, + "angularVelocity": -0.31338105518627096, + "velocityX": 0.8723886902906202, + "velocityY": 0.11973823719319023, + "timestamp": 2.153459420734394 + }, + { + "x": 2.860296723174767, + "y": 6.927962260865171, + "heading": 0.4420737054643263, + "angularVelocity": -0.41039216865205086, + "velocityX": 1.1633371706853932, + "velocityY": 0.15968500324119947, + "timestamp": 2.220999625590012 + }, + { + "x": 2.9585261643123166, + "y": 6.941446920815512, + "heading": 0.40815310467896726, + "angularVelocity": -0.502228278073356, + "velocityX": 1.454384708301312, + "velocityY": 0.19965382070083557, + "timestamp": 2.2885398304456306 + }, + { + "x": 3.0764209718394073, + "y": 6.9576328058659405, + "heading": 0.3684706356327604, + "angularVelocity": -0.5875384762459123, + "velocityX": 1.7455500435498434, + "velocityY": 0.23964814861057657, + "timestamp": 2.356080035301249 + }, + { + "x": 3.2139906503876703, + "y": 6.9765218958360915, + "heading": 0.323597871318007, + "angularVelocity": -0.6643859670055544, + "velocityX": 2.0368561043358877, + "velocityY": 0.27967178972185397, + "timestamp": 2.4236202401568674 + }, + { + "x": 3.3712465090237984, + "y": 6.998116434011635, + "heading": 0.27430452270447037, + "angularVelocity": -0.7298371202591344, + "velocityX": 2.3283296071176522, + "velocityY": 0.3197286449116718, + "timestamp": 2.491160445012486 + }, + { + "x": 3.5482015820439243, + "y": 7.022418860702876, + "heading": 0.22168358710657873, + "angularVelocity": -0.7791053596947181, + "velocityX": 2.619996095635254, + "velocityY": 0.3598216313260006, + "timestamp": 2.558700649868104 + }, + { + "x": 3.7448688058303015, + "y": 7.04943148246024, + "heading": 0.16741520227188003, + "angularVelocity": -0.8034974864335817, + "velocityX": 2.911854120176202, + "velocityY": 0.3999487685166161, + "timestamp": 2.6262408547237226 + }, + { + "x": 3.961249724160237, + "y": 7.079155012062387, + "heading": 0.1144423105156959, + "angularVelocity": -0.7843164211512963, + "velocityX": 3.2037350018777047, + "velocityY": 0.4400864591051642, + "timestamp": 2.693781059579341 + }, + { + "x": 4.197248092224393, + "y": 7.111577952654102, + "heading": 0.06945280731873603, + "angularVelocity": -0.6661144024234814, + "velocityX": 3.494190883321295, + "velocityY": 0.4800539273019009, + "timestamp": 2.7613212644349594 + }, + { + "x": 4.450279816565845, + "y": 7.14615999986413, + "heading": 0.06926458523360347, + "angularVelocity": -0.0027868154314146247, + "velocityX": 3.746386687490236, + "velocityY": 0.5120216511625231, + "timestamp": 2.828861469290578 + }, + { + "x": 4.703335353284152, + "y": 7.180937109425349, + "heading": 0.06926456657564138, + "angularVelocity": -2.762497114544702e-7, + "velocityX": 3.746739253445653, + "velocityY": 0.5149097435455242, + "timestamp": 2.896401674146196 + }, + { + "x": 4.956390888818978, + "y": 7.215714227598158, + "heading": 0.06926454791768001, + "angularVelocity": -2.7624970048363726e-7, + "velocityX": 3.746739235923052, + "velocityY": 0.5149098710487069, + "timestamp": 2.9639418790018146 + }, + { + "x": 5.209446424353711, + "y": 7.250491345771652, + "heading": 0.0692645292597187, + "angularVelocity": -2.7624969990656975e-7, + "velocityX": 3.7467392359216607, + "velocityY": 0.5149098710588286, + "timestamp": 3.031482083857433 + }, + { + "x": 5.462501959888314, + "y": 7.285268463946087, + "heading": 0.0692645106017574, + "angularVelocity": -2.7624969939846597e-7, + "velocityX": 3.7467392359197462, + "velocityY": 0.5149098710727624, + "timestamp": 3.0990222887130514 + }, + { + "x": 5.715557495424461, + "y": 7.320045582109288, + "heading": 0.06926449194379608, + "angularVelocity": -2.762496999067462e-7, + "velocityX": 3.746739235942603, + "velocityY": 0.5149098709064452, + "timestamp": 3.16656249356867 + }, + { + "x": 5.968613025759916, + "y": 7.354822738115272, + "heading": 0.06926447328583468, + "angularVelocity": -2.762497009102298e-7, + "velocityX": 3.7467391589411925, + "velocityY": 0.5149104312065347, + "timestamp": 3.234102698424288 + }, + { + "x": 6.221690653746151, + "y": 7.389438720345797, + "heading": 0.06926445462738323, + "angularVelocity": -2.7625695670270355e-7, + "velocityX": 3.74706633666926, + "velocityY": 0.5125240929387752, + "timestamp": 3.3016429032799066 + }, + { + "x": 6.476102895472122, + "y": 7.412263085598601, + "heading": 0.06926437909315673, + "angularVelocity": -0.0000011183594520291744, + "velocityX": 3.766826622303432, + "velocityY": 0.33793745964490024, + "timestamp": 3.369183108135525 + }, + { + "x": 6.712136074986816, + "y": 7.430641724558031, + "heading": 0.04341074490415746, + "angularVelocity": -0.38278880326565307, + "velocityX": 3.4947063015172035, + "velocityY": 0.27211405412107026, + "timestamp": 3.4367233129911434 + }, + { + "x": 6.9285315703702866, + "y": 7.445814297629393, + "heading": 0.019185726579151432, + "angularVelocity": -0.3586755233685199, + "velocityX": 3.203950829673405, + "velocityY": 0.2246450555457597, + "timestamp": 3.504263517846762 + }, + { + "x": 7.125290870666504, + "y": 7.458, "heading": 0, - "angularVelocity": -0.06599390000348598, - "velocityX": 0.1725682360181047, - "velocityY": -0.05997610648498418, - "timestamp": 4.368873692523098 - }, - { - "x": 7.955787513452433, - "y": 7.4497474833995785, - "heading": -0.0028764502828999656, - "angularVelocity": -0.0311548498072446, - "velocityX": -0.18512759364374298, - "velocityY": -0.10366522970762994, - "timestamp": 4.461201221004108 - }, - { - "x": 7.9056697739729325, - "y": 7.4361426444977665, - "heading": -0.002567951866504596, - "angularVelocity": 0.0033413481490390143, - "velocityX": -0.5428255288974695, - "velocityY": -0.147354089572646, - "timestamp": 4.553528749485118 - }, - { - "x": 7.822526434902679, - "y": 7.418504156260187, - "heading": 0.0008870875255376844, - "angularVelocity": 0.03742155182625641, - "velocityX": -0.9005259908733981, - "velocityY": -0.19104256907740527, - "timestamp": 4.6458562779661285 - }, - { - "x": 7.70635721281995, - "y": 7.396832065786244, - "heading": 0.007441383928023924, - "angularVelocity": 0.07098962260030964, - "velocityX": -1.2582295225916607, - "velocityY": -0.2347305384482473, - "timestamp": 4.738183806447139 - }, - { - "x": 7.557161757823986, - "y": 7.371126434037665, - "heading": 0.017035597131834973, - "angularVelocity": 0.10391497922295617, - "velocityX": -1.6159368440870827, - "velocityY": -0.27841784754226306, - "timestamp": 4.830511334928149 - }, - { - "x": 7.3749396288270415, - "y": 7.34138733896449, - "heading": 0.02959331840082248, - "angularVelocity": 0.13601275237829405, - "velocityX": -1.9736489430064588, - "velocityY": -0.3221043123589225, - "timestamp": 4.922838863409159 - }, - { - "x": 7.1596902544281695, - "y": 7.307614881050875, - "heading": 0.04501267183179304, - "angularVelocity": 0.1670071070313771, - "velocityX": -2.3313672307728326, - "velocityY": -0.3657896888311208, - "timestamp": 5.015166391890169 - }, - { - "x": 6.911412867489138, - "y": 7.269809194103027, - "heading": 0.06315110065106552, - "angularVelocity": 0.19645742843645173, - "velocityX": -2.689093827417871, - "velocityY": -0.4094736160475027, - "timestamp": 5.107493920371179 - }, - { - "x": 6.630106388703286, - "y": 7.227970469120692, - "heading": 0.0837947312429067, - "angularVelocity": 0.22359128346089294, - "velocityX": -3.046832114039641, - "velocityY": -0.45315547454453764, - "timestamp": 5.199821448852189 - }, - { - "x": 6.315769207511686, - "y": 7.182099018189511, - "heading": 0.10658632788484147, - "angularVelocity": 0.24685591628960976, - "velocityX": -3.404587844634587, - "velocityY": -0.4968339528401363, - "timestamp": 5.292148977333199 - }, - { - "x": 5.9683988091079465, - "y": 7.13219550920707, - "heading": 0.13080671297913374, - "angularVelocity": 0.2623311323585783, - "velocityX": -3.7623708131122173, - "velocityY": -0.5405051971331102, - "timestamp": 5.3844765058142094 - }, - { - "x": 5.587993514060399, - "y": 7.078262670189621, - "heading": 0.1542532135313182, - "angularVelocity": 0.2539491843649532, - "velocityX": -4.1201719715240674, - "velocityY": -0.584146894266117, - "timestamp": 5.47680403429522 - }, - { - "x": 5.224748052314718, - "y": 7.033899534150213, - "heading": 0.1946636792264689, - "angularVelocity": 0.43768598986663365, - "velocityX": -3.9343137168498292, - "velocityY": -0.4804973854415783, - "timestamp": 5.56913156277623 - }, - { - "x": 4.89452487756816, - "y": 6.993569704537409, - "heading": 0.23141354143983248, - "angularVelocity": 0.39803797218423637, - "velocityX": -3.5766491335731794, - "velocityY": -0.43681261998796816, - "timestamp": 5.66145909125724 - }, - { - "x": 4.597324012424621, - "y": 6.957273083756657, - "heading": 0.264499660893691, - "angularVelocity": 0.3583559529665485, - "velocityX": -3.2189843054736125, - "velocityY": -0.393128911581516, - "timestamp": 5.75378661973825 - }, - { - "x": 4.333145462586867, - "y": 6.925009592651694, - "heading": 0.2939186790634633, - "angularVelocity": 0.318637557549515, - "velocityX": -2.8613194156073467, - "velocityY": -0.34944606051704, - "timestamp": 5.84611414821926 - }, - { - "x": 4.101989229015984, - "y": 6.89677916444039, - "heading": 0.31966724628437904, - "angularVelocity": 0.2788828818937978, - "velocityX": -2.503654515331547, - "velocityY": -0.30576393277018515, - "timestamp": 5.93844167670027 - }, - { - "x": 3.903855311009448, - "y": 6.872581743315058, - "heading": 0.3417422393667011, - "angularVelocity": 0.23909437895180408, - "velocityX": -2.1459896226647937, - "velocityY": -0.2620824094766995, - "timestamp": 6.03076920518128 - }, - { - "x": 3.7387437076676333, - "y": 6.85241728377867, - "heading": 0.36014094824966475, - "angularVelocity": 0.19927652332584478, - "velocityX": -1.7883247397418944, - "velocityY": -0.21840137896180364, - "timestamp": 6.12309673366229 - }, - { - "x": 3.606654418811003, - "y": 6.836285750188712, - "heading": 0.3748612292329891, - "angularVelocity": 0.15943544926962971, - "velocityX": -1.4306598587635568, - "velocityY": -0.17472073449118222, - "timestamp": 6.2154242621433005 - }, - { - "x": 3.507587445613312, - "y": 6.824187116399864, - "heading": 0.3859016251505043, - "angularVelocity": 0.11957859263812058, - "velocityX": -1.0729949650722783, - "velocityY": -0.13104037320068423, - "timestamp": 6.307751790624311 - }, - { - "x": 3.441542791040051, - "y": 6.8161213654846, - "heading": 0.39326145350117914, - "angularVelocity": 0.07971434383395841, - "velocityX": -0.7153300392617483, - "velocityY": -0.08736019525231867, - "timestamp": 6.400079319105321 + "angularVelocity": -0.2840637901552857, + "velocityX": 2.913217404608591, + "velocityY": 0.18042146002750595, + "timestamp": 3.5718037227023802 + }, + { + "x": 7.284551146493795, + "y": 7.4665356658331685, + "heading": -0.011983560395956015, + "angularVelocity": -0.19978893791899283, + "velocityX": 2.6551742811725556, + "velocityY": 0.1423059220209307, + "timestamp": 3.6317848233617918 + }, + { + "x": 7.428286998104642, + "y": 7.47296871812108, + "heading": -0.020071347748117502, + "angularVelocity": -0.13483892864997732, + "velocityX": 2.396352351501819, + "velocityY": 0.10725132111930623, + "timestamp": 3.6917659240212033 + }, + { + "x": 7.556472866857842, + "y": 7.4774155432867, + "heading": -0.02498803078182015, + "angularVelocity": -0.08197053704667534, + "velocityX": 2.137104310257201, + "velocityY": 0.0741371051336659, + "timestamp": 3.751747024680615 + }, + { + "x": 7.669092898448606, + "y": 7.47995645101358, + "heading": -0.027230015787281982, + "angularVelocity": -0.037378190476904, + "velocityX": 1.877591947341067, + "velocityY": 0.04236180561785803, + "timestamp": 3.8117281253400264 + }, + { + "x": 7.766136403587462, + "y": 7.480650177988498, + "heading": -0.027158837231048453, + "angularVelocity": 0.001186683062681687, + "velocityX": 1.6179013734658474, + "velocityY": 0.011565759335732523, + "timestamp": 3.871709225999438 + }, + { + "x": 7.847595735447885, + "y": 7.479541543526011, + "heading": -0.025049657080079103, + "angularVelocity": 0.03516407881452247, + "velocityX": 1.3580833123248393, + "velocityY": -0.018483063003166142, + "timestamp": 3.9316903266588494 + }, + { + "x": 7.913465176456017, + "y": 7.4766658686921605, + "heading": -0.021118970688138776, + "angularVelocity": 0.06553208175121362, + "velocityX": 1.0981699282605077, + "velocityY": -0.04794301542047442, + "timestamp": 3.991671427318261 + }, + { + "x": 7.963740302847386, + "y": 7.472051704673601, + "heading": -0.015541591171907823, + "angularVelocity": 0.09298561471722216, + "velocityX": 0.8381827915570343, + "velocityY": -0.07692696479112789, + "timestamp": 4.051652527977672 + }, + { + "x": 7.998417597522527, + "y": 7.465722607991469, + "heading": -0.008461644038298986, + "angularVelocity": 0.11803629903043378, + "velocityX": 0.5781370180592035, + "velocityY": -0.10551818176978627, + "timestamp": 4.111633628637083 + }, + { + "x": 8.017494201660156, + "y": 7.457698345184326, + "heading": 0, + "angularVelocity": 0.14107183671647575, + "velocityX": 0.3180435825269393, + "velocityY": -0.1337798526356988, + "timestamp": 4.171614729296494 + }, + { + "x": 8.008975668857596, + "y": 7.440959395735479, + "heading": 0.016440344425850486, + "angularVelocity": 0.17454816573497248, + "velocityX": -0.09044179592138195, + "velocityY": -0.17771847395317164, + "timestamp": 4.265802739560738 + }, + { + "x": 7.961981832760565, + "y": 7.4200887947149115, + "heading": 0.0360050890089686, + "angularVelocity": 0.20772011775415367, + "velocityX": -0.4989364990850843, + "velocityY": -0.22158447728129313, + "timestamp": 4.3599907498249815 + }, + { + "x": 7.876511570645417, + "y": 7.39509531765153, + "heading": 0.058657775076795544, + "angularVelocity": 0.24050498576490828, + "velocityX": -0.9074431222759898, + "velocityY": -0.2653573102697685, + "timestamp": 4.454178760089225 + }, + { + "x": 7.752563396875145, + "y": 7.365990633137338, + "heading": 0.08435039467917142, + "angularVelocity": 0.2727801503641011, + "velocityX": -1.315965518567977, + "velocityY": -0.3090062570866462, + "timestamp": 4.548366770353469 + }, + { + "x": 7.590135252234061, + "y": 7.332791018782474, + "heading": 0.11301654484596554, + "angularVelocity": 0.3043503104733978, + "velocityX": -1.7245097776818146, + "velocityY": -0.35248238349789773, + "timestamp": 4.642554780617712 + }, + { + "x": 7.3892240901384, + "y": 7.295520773303632, + "heading": 0.14455765680612448, + "angularVelocity": 0.33487395977121315, + "velocityX": -2.13308638256617, + "velocityY": -0.39570052891318314, + "timestamp": 4.736742790881956 + }, + { + "x": 7.149824938411661, + "y": 7.254220108341429, + "heading": 0.1788109128374065, + "angularVelocity": 0.36366896312157765, + "velocityX": -2.54171577735964, + "velocityY": -0.4384917448232911, + "timestamp": 4.8309308011461995 + }, + { + "x": 6.871928226350113, + "y": 7.208968463075644, + "heading": 0.21545413209734332, + "angularVelocity": 0.3890433523028524, + "velocityX": -2.9504467849136042, + "velocityY": -0.4804395499897761, + "timestamp": 4.925118811410443 + }, + { + "x": 6.555508023323144, + "y": 7.15999689291417, + "heading": 0.25354713749374036, + "angularVelocity": 0.40443582245264, + "velocityX": -3.3594530995957568, + "velocityY": -0.5199342254293745, + "timestamp": 5.019306821674687 + }, + { + "x": 6.201946981022424, + "y": 7.116596127430346, + "heading": 0.25354714311943394, + "angularVelocity": 5.972834101491316e-8, + "velocityX": -3.753779714730222, + "velocityY": -0.46078864350212084, + "timestamp": 5.11349483193893 + }, + { + "x": 5.847829680383322, + "y": 7.077995922009697, + "heading": 0.25354714640605686, + "angularVelocity": 3.4894280958051e-8, + "velocityX": -3.7596855443238373, + "velocityY": -0.4098207968546854, + "timestamp": 5.207682842203174 + }, + { + "x": 5.493712379149646, + "y": 7.039395722043647, + "heading": 0.25354714969267966, + "angularVelocity": 3.4894280048226024e-8, + "velocityX": -3.759685550636459, + "velocityY": -0.40982073894285953, + "timestamp": 5.301870852467418 + }, + { + "x": 5.13959507794107, + "y": 7.0007955218474756, + "heading": 0.25354715297933, + "angularVelocity": 3.489457219723442e-8, + "velocityX": -3.7596855503699804, + "velocityY": -0.4098207413860709, + "timestamp": 5.396058862731661 + }, + { + "x": 4.793375065328026, + "y": 6.963049240971564, + "heading": 0.2809926873119857, + "angularVelocity": 0.2913909557666365, + "velocityX": -3.675839543076952, + "velocityY": -0.4007546265178986, + "timestamp": 5.490246872995905 + }, + { + "x": 4.4856271372174605, + "y": 6.929500155423233, + "heading": 0.3062072463057812, + "angularVelocity": 0.2677045509620205, + "velocityX": -3.2673790140292933, + "velocityY": -0.3561927410315718, + "timestamp": 5.5844348832601485 + }, + { + "x": 4.216349125910067, + "y": 6.900146018877801, + "heading": 0.32859640421091696, + "angularVelocity": 0.23770709076795637, + "velocityX": -2.858941499580855, + "velocityY": -0.31165470491497754, + "timestamp": 5.678622893524392 + }, + { + "x": 3.9855401854111148, + "y": 6.874986045056756, + "heading": 0.34796083888612495, + "angularVelocity": 0.2055934149249067, + "velocityX": -2.450512967111424, + "velocityY": -0.26712501676655553, + "timestamp": 5.772810903788636 + }, + { + "x": 3.7931998687162816, + "y": 6.854019825581219, + "heading": 0.36419988308525797, + "angularVelocity": 0.17241094862896578, + "velocityX": -2.042089180514842, + "velocityY": -0.22259966440226955, + "timestamp": 5.866998914052879 + }, + { + "x": 3.639327899901621, + "y": 6.837247108195556, + "heading": 0.37725261237367164, + "angularVelocity": 0.13858164379727655, + "velocityX": -1.6336683234200817, + "velocityY": -0.17807699025180046, + "timestamp": 5.961186924317123 + }, + { + "x": 3.5239240923326007, + "y": 6.824667722043819, + "heading": 0.38707830352299477, + "angularVelocity": 0.10431997790119188, + "velocityX": -1.2252494478358333, + "velocityY": -0.13355613008965372, + "timestamp": 6.0553749345813666 + }, + { + "x": 3.4469883122501845, + "y": 6.8162815454148475, + "heading": 0.39364810050149374, + "angularVelocity": 0.06975194571015403, + "velocityX": -0.8168319923796448, + "velocityY": -0.08903656214250527, + "timestamp": 6.14956294484561 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 0.03985171226128589, - "velocityX": -0.35766505888933303, - "velocityY": -0.04368010298204797, - "timestamp": 6.492406847586331 + "angularVelocity": 0.03495947190378589, + "velocityX": -0.4084155935928461, + "velocityY": -0.04451793673752315, + "timestamp": 6.243750955109854 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 5.690856102528897e-33, - "velocityX": 4.038395222816766e-35, - "velocityY": 1.1020881462714252e-31, - "timestamp": 6.584734376067341 - }, - { - "x": 3.4229402433753, - "y": 6.8112531460784576, - "heading": 0.3945108838826677, - "angularVelocity": -0.039933151023800836, - "velocityX": 0.23696797878861645, - "velocityY": -0.013727643925556041, - "timestamp": 6.645585564992654 - }, - { - "x": 3.451777703834916, - "y": 6.809543306220452, - "heading": 0.3896720623982013, - "angularVelocity": -0.07951893085286188, - "velocityX": 0.4739013480083423, - "velocityY": -0.02809870913293678, - "timestamp": 6.706436753917967 - }, - { - "x": 3.4950303346590754, - "y": 6.80691471214731, - "heading": 0.3824488904717316, - "angularVelocity": -0.11870223169074358, - "velocityX": 0.7107935208504587, - "velocityY": -0.04319708652477366, - "timestamp": 6.76728794284328 - }, - { - "x": 3.5526951141421756, - "y": 6.803316933089257, - "heading": 0.37286978619030314, - "angularVelocity": -0.15741852296732456, - "velocityX": 0.9476360363948891, - "velocityY": -0.05912421961826942, - "timestamp": 6.8281391317685936 - }, - { - "x": 3.624768348044375, - "y": 6.798691977814201, - "heading": 0.3609678412629577, - "angularVelocity": -0.1955910005629235, - "velocityX": 1.1844178425282745, - "velocityY": -0.07600435351777092, - "timestamp": 6.888990320693907 - }, - { - "x": 3.7112454465000653, - "y": 6.7929724589112, - "heading": 0.3467818152826883, - "angularVelocity": -0.23312652112156756, - "velocityX": 1.4211242209553094, - "velocityY": -0.09399190063518563, - "timestamp": 6.94984150961922 - }, - { - "x": 3.8121205998573195, - "y": 6.78607911243415, - "heading": 0.3303574905541313, - "angularVelocity": -0.2699096766821729, - "velocityX": 1.6577351262777436, - "velocityY": -0.1132820343988894, - "timestamp": 7.010692698544533 - }, - { - "x": 3.927386292802013, - "y": 6.777917363603635, - "heading": 0.31174956650876195, - "angularVelocity": -0.30579392734968835, - "velocityX": 1.8942225284401175, - "velocityY": -0.1341263658879746, - "timestamp": 7.071543887469846 - }, - { - "x": 4.057032548891421, - "y": 6.7683724330022, - "heading": 0.29102438798021885, - "angularVelocity": -0.34058789802744316, - "velocityX": 2.13054598240523, - "velocityY": -0.1568569286813843, - "timestamp": 7.132395076395159 - }, - { - "x": 4.201045703157843, - "y": 6.75730211970409, - "heading": 0.2682640135119274, - "angularVelocity": -0.37403335695259643, - "velocityX": 2.3666448726775124, - "velocityY": -0.1819243550310427, - "timestamp": 7.1932462653204725 - }, - { - "x": 4.359406298184771, - "y": 6.744525706474344, - "heading": 0.2435725432613573, - "angularVelocity": -0.4057680825410948, - "velocityX": 2.6024240088603046, - "velocityY": -0.20996160396188043, - "timestamp": 7.254097454245786 - }, - { - "x": 4.532085228362737, - "y": 6.729806001912393, - "heading": 0.2170864931038356, - "angularVelocity": -0.4352593700351523, - "velocityX": 2.837724837059597, - "velocityY": -0.24189674551828952, - "timestamp": 7.314948643171099 - }, - { - "x": 4.71903604094914, - "y": 6.712818312739342, - "heading": 0.18899299215778056, - "angularVelocity": -0.46167546505190277, - "velocityX": 3.072262282596678, - "velocityY": -0.27916774467464406, - "timestamp": 7.375799832096412 - }, - { - "x": 4.920177705816601, - "y": 6.693092062890064, - "heading": 0.1595647143501649, - "angularVelocity": -0.4836105641869241, - "velocityX": 3.3054681168898354, - "velocityY": -0.32417197096165007, - "timestamp": 7.436651021021725 - }, - { - "x": 5.13534940168168, - "y": 6.66988755215724, - "heading": 0.12923590547960542, - "angularVelocity": -0.4984094708122179, - "velocityX": 3.536031089370736, - "velocityY": -0.3813320847568572, - "timestamp": 7.497502209947038 - }, - { - "x": 5.364160197364154, - "y": 6.6418909322873, - "heading": 0.09880136738513437, - "angularVelocity": -0.5001469754654356, - "velocityX": 3.760169681537533, - "velocityY": -0.46008336672439587, - "timestamp": 7.5583533988723515 - }, - { - "x": 5.605261544574765, - "y": 6.606294945250104, - "heading": 0.07010036543409427, - "angularVelocity": -0.4716588526522781, - "velocityX": 3.9621468613626054, - "velocityY": -0.584967815187393, - "timestamp": 7.619204587797665 + "angularVelocity": 0, + "velocityX": -1.9952146739162206e-34, + "velocityY": -3.951795765453016e-33, + "timestamp": 6.337938965374097 + }, + { + "x": 3.425127749537185, + "y": 6.810478633677757, + "heading": 0.3886892234850989, + "angularVelocity": -0.13293807639169805, + "velocityX": 0.2675517930120232, + "velocityY": -0.025935588272518423, + "timestamp": 6.400010272097332 + }, + { + "x": 3.458347138585669, + "y": 6.807258412874914, + "heading": 0.3723514064813247, + "angularVelocity": -0.26321045691257755, + "velocityX": 0.535181081278085, + "velocityY": -0.05187937829625554, + "timestamp": 6.462081578820567 + }, + { + "x": 3.508184213080087, + "y": 6.802427294600358, + "heading": 0.34812438720722594, + "angularVelocity": -0.3903094771652888, + "velocityX": 0.8029003596884762, + "velocityY": -0.07783174754300737, + "timestamp": 6.524152885543802 + }, + { + "x": 3.5746455076736363, + "y": 6.795984642746885, + "heading": 0.31624502791908643, + "angularVelocity": -0.5135925272251962, + "velocityX": 1.070724914651617, + "velocityY": -0.10379436479708294, + "timestamp": 6.586224192267037 + }, + { + "x": 3.6577387353786484, + "y": 6.787929625128416, + "heading": 0.2770021560106679, + "angularVelocity": -0.6322224225666108, + "velocityX": 1.3386737301264124, + "velocityY": -0.12977038898779852, + "timestamp": 6.648295498990271 + }, + { + "x": 3.7574730901724314, + "y": 6.7782611086258875, + "heading": 0.23075444743671972, + "angularVelocity": -0.7450738677080124, + "velocityX": 1.6067706652041787, + "velocityY": -0.1557646682971112, + "timestamp": 6.710366805713506 + }, + { + "x": 3.873859634378807, + "y": 6.766977536857145, + "heading": 0.17795757602156642, + "angularVelocity": -0.8505841781386996, + "velocityX": 1.875045819887845, + "velocityY": -0.18178402170643457, + "timestamp": 6.772438112436741 + }, + { + "x": 4.006911762109537, + "y": 6.754076769706622, + "heading": 0.11920696294593584, + "angularVelocity": -0.9465019535937051, + "velocityX": 2.1435367604550115, + "velocityY": -0.2078378534549514, + "timestamp": 6.834509419159976 + }, + { + "x": 4.156645657189765, + "y": 6.73955582918857, + "heading": 0.055309270826440785, + "angularVelocity": -1.029423988195197, + "velocityX": 2.4122884305927053, + "velocityY": -0.23393966205310818, + "timestamp": 6.896580725883211 + }, + { + "x": 4.323080375842652, + "y": 6.723410435666096, + "heading": -0.012586184407094544, + "angularVelocity": -1.0938299645646508, + "velocityX": 2.681347106079046, + "velocityY": -0.2601104177560936, + "timestamp": 6.958652032606445 + }, + { + "x": 4.506235840968647, + "y": 6.705634179985004, + "heading": -0.08270962096165607, + "angularVelocity": -1.1297238652836274, + "velocityX": 2.9507267495214404, + "velocityY": -0.2863844281597597, + "timestamp": 7.02072333932968 + }, + { + "x": 4.706118949857515, + "y": 6.686217722826257, + "heading": -0.15194437984101536, + "angularVelocity": -1.1154068205470853, + "velocityX": 3.2202175117742615, + "velocityY": -0.3128088996947802, + "timestamp": 7.082794646052915 + }, + { + "x": 4.922607059053276, + "y": 6.66515425622058, + "heading": -0.21301282950448638, + "angularVelocity": -0.9838434678965439, + "velocityX": 3.4877324262085807, + "velocityY": -0.3393430510421988, + "timestamp": 7.14486595277615 + }, + { + "x": 5.152321807662402, + "y": 6.642265811014449, + "heading": -0.22692060137612477, + "angularVelocity": -0.22406120647088962, + "velocityX": 3.7008202458728654, + "velocityY": -0.3687443750482909, + "timestamp": 7.206937259499385 + }, + { + "x": 5.385997622338085, + "y": 6.619824669792801, + "heading": -0.22692062699124488, + "angularVelocity": -4.12672480414244e-7, + "velocityX": 3.7646350143327725, + "velocityY": -0.36153808267225507, + "timestamp": 7.269008566222619 + }, + { + "x": 5.6195399174914655, + "y": 6.5960341766076915, + "heading": -0.2269206525768778, + "angularVelocity": -4.121974272362167e-7, + "velocityX": 3.7624839476103156, + "velocityY": -0.3832768221124699, + "timestamp": 7.331079872945854 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": 0.04853422903733051, - "angularVelocity": -0.3544078066121842, - "velocityX": 4.0390769313634065, - "velocityY": -0.808003051659102, - "timestamp": 7.680055776722978 - }, - { - "x": 6.0904224796154285, - "y": 6.495587769280379, - "heading": 0.021863961487268806, - "angularVelocity": -0.4346445536401508, - "velocityX": 3.9011410312083004, - "velocityY": -1.0029029869972568, - "timestamp": 7.741416875871002 - }, - { - "x": 6.315513452841665, - "y": 6.430603187238758, - "heading": -0.005975333075399724, - "angularVelocity": -0.4536961519465426, - "velocityX": 3.6683008673498723, - "velocityY": -1.0590517924859284, - "timestamp": 7.802777975019025 - }, - { - "x": 6.525908252987294, - "y": 6.365933697646677, - "heading": -0.03348393584839473, - "angularVelocity": -0.4483068777277778, - "velocityX": 3.428797773620156, - "velocityY": -1.053916740247379, - "timestamp": 7.864139074167049 - }, - { - "x": 6.721741896707244, - "y": 6.303262822606784, - "heading": -0.05999730126977997, - "angularVelocity": -0.43208752433567293, - "velocityX": 3.1914950422829667, - "velocityY": -1.0213453785876696, - "timestamp": 7.925500173315073 - }, - { - "x": 6.903171972351107, - "y": 6.243508401888397, - "heading": -0.08515279346758128, - "angularVelocity": -0.4099583049696985, - "velocityX": 2.9567605235719823, - "velocityY": -0.9738160096226287, - "timestamp": 7.986861272463097 - }, - { - "x": 7.07032746244464, - "y": 6.187241571909668, - "heading": -0.10872472462882488, - "angularVelocity": -0.3841510580568351, - "velocityX": 2.7241280292306773, - "velocityY": -0.9169788475104336, - "timestamp": 8.04822237161112 - }, - { - "x": 7.223309884324563, - "y": 6.13485024328056, - "heading": -0.1305599608000083, - "angularVelocity": -0.3558481916777524, - "velocityX": 2.4931499598936324, - "velocityY": -0.8538199177743311, - "timestamp": 8.109583470759143 - }, - { - "x": 7.362199686401887, - "y": 6.086614483409434, - "heading": -0.15054810335595573, - "angularVelocity": -0.3257461622017125, - "velocityX": 2.2634829559078664, - "velocityY": -0.7860967378495876, - "timestamp": 8.170944569907165 - }, - { - "x": 7.487061657008568, - "y": 6.0427457582775, - "heading": -0.16860591224690066, - "angularVelocity": -0.29428757212095324, - "velocityX": 2.0348718054328225, - "velocityY": -0.7149273031453939, - "timestamp": 8.232305669055188 - }, - { - "x": 7.597948869371604, - "y": 6.003409277701244, - "heading": -0.18466841772723513, - "angularVelocity": -0.26177017203662434, - "velocityX": 1.8071255877528956, - "velocityY": -0.6410654489966826, - "timestamp": 8.293666768203211 - }, - { - "x": 7.694905491467594, - "y": 5.968737621588528, - "heading": -0.1986834963337821, - "angularVelocity": -0.2284033174297924, - "velocityX": 1.580099174268352, - "velocityY": -0.5650429440495477, - "timestamp": 8.355027867351234 - }, - { - "x": 7.7779688005582, - "y": 5.938839509139965, - "heading": -0.21060838151890787, - "angularVelocity": -0.19433949767358227, - "velocityX": 1.3536802672036419, - "velocityY": -0.487248645537448, - "timestamp": 8.416388966499257 - }, - { - "x": 7.847170649947212, - "y": 5.913805692553094, - "heading": -0.220407320835025, - "angularVelocity": -0.15969302134694188, - "velocityX": 1.1277804724793856, - "velocityY": -0.40797536117273564, - "timestamp": 8.47775006564728 - }, - { - "x": 7.902538555840789, - "y": 5.8937130619859, - "heading": -0.22804994580967364, - "angularVelocity": -0.12455163093170954, - "velocityX": 0.9023291085450026, - "velocityY": -0.3274490002000119, - "timestamp": 8.539111164795303 - }, - { - "x": 7.944096516826036, - "y": 5.878627591236197, - "heading": -0.23351010234644243, - "angularVelocity": -0.08898400798846613, - "velocityX": 0.6772688488678362, - "velocityY": -0.24584746621490028, - "timestamp": 8.600472263943326 - }, - { - "x": 7.97186564193086, - "y": 5.86860650570412, - "heading": -0.2367649883044089, - "angularVelocity": -0.05304477923569416, - "velocityX": 0.45255260238795525, - "velocityY": -0.16331333159306605, - "timestamp": 8.661833363091349 + "heading": -0.22692067835854435, + "angularVelocity": -4.1535562716932914e-7, + "velocityX": 3.729650183615213, + "velocityY": -0.6268142199732243, + "timestamp": 7.393151179669089 + }, + { + "x": 5.959383441341322, + "y": 6.535142573313356, + "heading": -0.22692070161190955, + "angularVelocity": -7.95525127627382e-7, + "velocityX": 3.706414340926548, + "velocityY": -0.7521132022680976, + "timestamp": 7.422381387996315 + }, + { + "x": 6.066940887967902, + "y": 6.509606194124557, + "heading": -0.22692072465630755, + "angularVelocity": -7.883761123678933e-7, + "velocityX": 3.6796674667011553, + "velocityY": -0.8736297361581465, + "timestamp": 7.4516115963235405 + }, + { + "x": 6.17368571603366, + "y": 6.480861582677863, + "heading": -0.2269207475995078, + "angularVelocity": -7.849140177187675e-7, + "velocityX": 3.651866824580282, + "velocityY": -0.9833871563580519, + "timestamp": 7.480841804650766 + }, + { + "x": 6.2799139038286755, + "y": 6.450262838777624, + "heading": -0.2269207705194487, + "angularVelocity": -7.841182881547448e-7, + "velocityX": 3.63419194984251, + "velocityY": -1.0468192206387272, + "timestamp": 7.510072012977992 + }, + { + "x": 6.38600768248991, + "y": 6.419201268635774, + "heading": -0.22692079344039778, + "angularVelocity": -7.841527793264923e-7, + "velocityX": 3.629593654398156, + "velocityY": -1.0626530537902528, + "timestamp": 7.539302221305218 + }, + { + "x": 6.4915378820851375, + "y": 6.386275784732387, + "heading": -0.22692082347226308, + "angularVelocity": -0.0000010274256324387564, + "velocityX": 3.6103129479556895, + "velocityY": -1.1264197481865303, + "timestamp": 7.568532429632444 + }, + { + "x": 6.594254502115419, + "y": 6.3529873750963075, + "heading": -0.23642872835410836, + "angularVelocity": -0.32527667183951403, + "velocityX": 3.514057063171563, + "velocityY": -1.1388358667671954, + "timestamp": 7.597762637959669 + }, + { + "x": 6.69366972769076, + "y": 6.320452523543764, + "heading": -0.25267195567048195, + "angularVelocity": -0.5557000187824542, + "velocityX": 3.4011124540202067, + "velocityY": -1.1130557534298289, + "timestamp": 7.626992846286895 + }, + { + "x": 6.789488054498416, + "y": 6.288864279277486, + "heading": -0.26990654049679197, + "angularVelocity": -0.5896155317621415, + "velocityX": 3.278058292810801, + "velocityY": -1.0806711985311153, + "timestamp": 7.656223054614121 + }, + { + "x": 6.881710669103943, + "y": 6.258257632428043, + "heading": -0.28694313831911705, + "angularVelocity": -0.5828421621787924, + "velocityX": 3.1550447254125045, + "velocityY": -1.047089589879878, + "timestamp": 7.685453262941347 + }, + { + "x": 6.970345859458022, + "y": 6.228646491338886, + "heading": -0.3032367291527766, + "angularVelocity": -0.5574230142767849, + "velocityX": 3.0323146986098455, + "velocityY": -1.0130321603485573, + "timestamp": 7.714683471268573 + }, + { + "x": 7.055400617247705, + "y": 6.200038168598195, + "heading": -0.3184704659597927, + "angularVelocity": -0.5211641544429054, + "velocityX": 2.909823865690951, + "velocityY": -0.978724558526454, + "timestamp": 7.743913679595798 + }, + { + "x": 7.136880378911089, + "y": 6.172437128670844, + "heading": -0.33243641899697507, + "angularVelocity": -0.477791772157844, + "velocityX": 2.7875190197494866, + "velocityY": -0.9442642220804569, + "timestamp": 7.773143887923024 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.42938659336036833, + "velocityX": 2.665359438451487, + "velocityY": -0.9097014120066562, + "timestamp": 7.80237409625025 + }, + { + "x": 7.370577859130934, + "y": 6.0916577592619054, + "heading": -0.3653758531922844, + "angularVelocity": -0.3132570133567013, + "velocityX": 2.393610638018096, + "velocityY": -0.8325804140238011, + "timestamp": 7.867459230019526 + }, + { + "x": 7.5087533836626745, + "y": 6.042843250581257, + "heading": -0.37947223160146565, + "angularVelocity": -0.21658368958955537, + "velocityX": 2.1229967049244562, + "velocityY": -0.7500101152698444, + "timestamp": 7.932544363788803 + }, + { + "x": 7.629367562165718, + "y": 5.999619792415774, + "heading": -0.38804895009153, + "angularVelocity": -0.1317769203712161, + "velocityX": 1.85317554897581, + "velocityY": -0.6641064658284028, + "timestamp": 7.997629497558079 + }, + { + "x": 7.7324579600950285, + "y": 5.962133690972344, + "heading": -0.39162657356882036, + "angularVelocity": -0.05496836635495089, + "velocityX": 1.5839315671495955, + "velocityY": -0.5759548958801737, + "timestamp": 8.062714631327355 + }, + { + "x": 7.81805300550911, + "y": 5.930490256491027, + "heading": -0.39057997729782273, + "angularVelocity": 0.01608041975771331, + "velocityX": 1.3151243679933469, + "velocityY": -0.4861852876186942, + "timestamp": 8.127799765096631 + }, + { + "x": 7.886174910867723, + "y": 5.904768911877698, + "heading": -0.38519213768662347, + "angularVelocity": 0.08278141718657105, + "velocityX": 1.0466584519915365, + "velocityY": -0.39519538677620675, + "timestamp": 8.192884898865907 + }, + { + "x": 7.936841490598973, + "y": 5.885031694896893, + "heading": -0.3756842946663964, + "angularVelocity": 0.1460831755210139, + "velocityX": 0.7784662456231665, + "velocityY": -0.3032523072130836, + "timestamp": 8.257970032635184 + }, + { + "x": 7.970067341476633, + "y": 5.8713284070268905, + "heading": -0.36223415085714655, + "angularVelocity": 0.206654623418748, + "velocityX": 0.510498311264806, + "velocityY": -0.21054405324847428, + "timestamp": 8.32305516640446 }, { "x": 7.985864639282227, "y": 5.863699913024902, - "heading": -0.2377945013589896, - "angularVelocity": -0.01677794349962979, - "velocityX": 0.22814124169445224, - "velocityY": -0.07996259433654461, - "timestamp": 8.723194462239372 - }, - { - "x": 7.985275174373748, - "y": 5.8642853483331905, - "heading": -0.23637136146087387, - "angularVelocity": 0.021914153931364394, - "velocityX": -0.009076848143066944, - "velocityY": 0.00901479853081729, - "timestamp": 8.78813604759891 - }, - { - "x": 7.9692615507734805, - "y": 5.87059931497324, - "heading": -0.23246035931952694, - "angularVelocity": 0.060223385673360406, - "velocityX": -0.24658504272125015, - "velocityY": 0.09722532342093965, - "timestamp": 8.853077632958447 - }, - { - "x": 7.937802661499015, - "y": 5.8825854207762855, - "heading": -0.22608944217873808, - "angularVelocity": 0.09810227307383063, - "velocityX": -0.4844182521923094, - "velocityY": 0.18456749610726042, - "timestamp": 8.918019218317985 - }, - { - "x": 7.890874700520602, - "y": 5.9001792828849275, - "heading": -0.21729032731874598, - "angularVelocity": 0.13549276340079022, - "velocityX": -0.722618037711973, - "velocityY": 0.27091827234023624, - "timestamp": 8.982960803677523 - }, - { - "x": 7.828450615730271, - "y": 5.923306702319573, - "heading": -0.20609939347439793, - "angularVelocity": 0.17232307746094241, - "velocityX": -0.9612343838656007, - "velocityY": 0.3561264990160738, - "timestamp": 9.047902389037061 - }, - { - "x": 7.750499406641646, - "y": 5.951881239016375, - "heading": -0.1925588645118683, - "angularVelocity": 0.20850320926975888, - "velocityX": -1.2003280895756, - "velocityY": 0.44000368236475507, - "timestamp": 9.112843974396599 - }, - { - "x": 7.6569852092442545, - "y": 5.985800920098193, - "heading": -0.1767184189861056, - "angularVelocity": 0.24391836814677034, - "velocityX": -1.4399740455929166, - "velocityY": 0.522310641078874, - "timestamp": 9.177785559756137 - }, - { - "x": 7.5478660841549114, - "y": 6.024943657824972, - "heading": -0.15863743627693694, - "angularVelocity": 0.27841917638854113, - "velocityX": -1.6802658032634041, - "velocityY": 0.6027376373716655, - "timestamp": 9.242727145115675 - }, - { - "x": 7.423092384274454, - "y": 6.069160679743017, - "heading": -0.13838822433863668, - "angularVelocity": 0.3118065539391131, - "velocityX": -1.9213220494952408, - "velocityY": 0.6808737679138067, - "timestamp": 9.307668730475212 - }, - { - "x": 7.282604518065142, - "y": 6.118266769407389, - "heading": -0.11606081984557375, - "angularVelocity": 0.34380750592168463, - "velocityX": -2.163295913266127, - "velocityY": 0.7561578515908439, - "timestamp": 9.37261031583475 - }, - { - "x": 7.126329839060539, - "y": 6.172025132426245, - "heading": -0.09177042771332564, - "angularVelocity": 0.37403448033749925, - "velocityX": -2.406388420292097, - "velocityY": 0.8277956677717754, - "timestamp": 9.437551901194288 - }, - { - "x": 6.954178295975344, - "y": 6.230122642965384, - "heading": -0.06566955623422352, - "angularVelocity": 0.40191306286409745, - "velocityX": -2.6508675778718325, - "velocityY": 0.8946118302701128, - "timestamp": 9.502493486553826 - }, - { - "x": 6.76603650192892, - "y": 6.292126520899546, - "heading": -0.037969143199572904, - "angularVelocity": 0.42654352956263775, - "velocityX": -2.897092718091327, - "velocityY": 0.9547638480164637, - "timestamp": 9.567435071913364 - }, - { - "x": 6.561760786884065, - "y": 6.357401498891707, - "heading": -0.008978614795991234, - "angularVelocity": 0.4464093114308898, - "velocityX": -3.145530154737028, - "velocityY": 1.0051337310411634, - "timestamp": 9.632376657272902 - }, - { - "x": 6.3411760884298385, - "y": 6.424931400851951, - "heading": 0.020808813203341207, - "angularVelocity": 0.4586803330165021, - "velocityX": -3.396663281824698, - "velocityY": 1.0398560735217077, - "timestamp": 9.69731824263244 - }, - { - "x": 6.104131062172235, - "y": 6.4928665412990725, - "heading": 0.05050238274553203, - "angularVelocity": 0.4572350579031523, - "velocityX": -3.6501268785670193, - "velocityY": 1.0460961196282834, - "timestamp": 9.762259827991977 + "heading": -0.344987478573796, + "angularVelocity": 0.2649863537883773, + "velocityX": 0.24271745160106833, + "velocityY": -0.1172079330593501, + "timestamp": 8.388140300173736 + }, + { + "x": 7.985045656249217, + "y": 5.861990715165632, + "heading": -0.3251381624172612, + "angularVelocity": 0.31897227576671205, + "velocityX": -0.01316080008970204, + "velocityY": -0.02746627272232015, + "timestamp": 8.450369270101682 + }, + { + "x": 7.968302412952749, + "y": 5.865866788787815, + "heading": -0.3020436028872691, + "angularVelocity": 0.37112231741475454, + "velocityX": -0.26905866055398503, + "velocityY": 0.062287285594966935, + "timestamp": 8.512598240029627 + }, + { + "x": 7.935633552055529, + "y": 5.875328955639361, + "heading": -0.2758355383990043, + "angularVelocity": 0.4211553641111372, + "velocityX": -0.5249783330022537, + "velocityY": 0.1520540491430483, + "timestamp": 8.574827209957572 + }, + { + "x": 7.887037558008595, + "y": 5.8903781286161365, + "heading": -0.24666763452179902, + "angularVelocity": 0.468719053376244, + "velocityX": -0.7809223598462517, + "velocityY": 0.24183548264097016, + "timestamp": 8.637056179885517 + }, + { + "x": 7.822512734192608, + "y": 5.911015324029987, + "heading": -0.21472164060862464, + "angularVelocity": 0.5133620876283883, + "velocityX": -1.0368936508301385, + "velocityY": 0.33163324795100674, + "timestamp": 8.699285149813463 + }, + { + "x": 7.742057181096897, + "y": 5.937241677003945, + "heading": -0.1802161756247138, + "angularVelocity": 0.5544919837796514, + "velocityX": -1.29289546635384, + "velocityY": 0.4214492543316279, + "timestamp": 8.761514119741408 + }, + { + "x": 7.645668783226514, + "y": 5.9690584605532635, + "heading": -0.14341967249459378, + "angularVelocity": 0.5913082471512265, + "velocityX": -1.5489312772168387, + "velocityY": 0.5112857176674774, + "timestamp": 8.823743089669353 + }, + { + "x": 7.533345224831275, + "y": 6.006467108033043, + "heading": -0.10467019490543185, + "angularVelocity": 0.6226919332592115, + "velocityX": -1.8050043014579413, + "velocityY": 0.6011452145695896, + "timestamp": 8.885972059597298 + }, + { + "x": 7.405084087505747, + "y": 6.04946923502926, + "heading": -0.06440725119965417, + "angularVelocity": 0.6470128583583754, + "velocityX": -2.0611161887146876, + "velocityY": 0.6910306734308482, + "timestamp": 8.948201029525244 + }, + { + "x": 7.260883177292953, + "y": 6.098066643466027, + "heading": -0.023226072117979224, + "angularVelocity": 0.6617686124221249, + "velocityX": -2.3172633321708815, + "velocityY": 0.7809450886466088, + "timestamp": 9.010429999453189 + }, + { + "x": 7.100741532540205, + "y": 6.152261239581916, + "heading": 0.018021968863737303, + "angularVelocity": 0.6628430621538008, + "velocityX": -2.5734259290837285, + "velocityY": 0.8708901365174432, + "timestamp": 9.072658969381134 + }, + { + "x": 6.924662708731195, + "y": 6.212054571383467, + "heading": 0.05802124542072086, + "angularVelocity": 0.6427758100977419, + "velocityX": -2.829531390490462, + "velocityY": 0.9608600603671181, + "timestamp": 9.13488793930908 + }, + { + "x": 6.732667433541327, + "y": 6.277445378277029, + "heading": 0.09448040335619008, + "angularVelocity": 0.5858872158366861, + "velocityX": -3.0853037646642187, + "velocityY": 1.050809726872821, + "timestamp": 9.197116909237025 + }, + { + "x": 6.524862060286313, + "y": 6.348410321942265, + "heading": 0.12245319566137218, + "angularVelocity": 0.44951398581032764, + "velocityX": -3.33936707446111, + "velocityY": 1.1403843539014786, + "timestamp": 9.25934587916497 + }, + { + "x": 6.30228026524581, + "y": 6.424202518084616, + "heading": 0.1231057628389814, + "angularVelocity": 0.010486549566300301, + "velocityX": -3.5768195311320152, + "velocityY": 1.2179567849204198, + "timestamp": 9.321574849092915 + }, + { + "x": 6.078984969036473, + "y": 6.498549743391548, + "heading": 0.1231057748040032, + "angularVelocity": 1.9227414178241924e-7, + "velocityX": -3.5882852707972814, + "velocityY": 1.194736557475109, + "timestamp": 9.38380381902086 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": 0.07814732109541168, - "angularVelocity": 0.42568930519370946, - "velocityX": -3.897146685933666, - "velocityY": 0.9895116857176834, - "timestamp": 9.827201413351515 - }, - { - "x": 5.590600161596063, - "y": 6.608035856956418, - "heading": 0.09892900965828218, - "angularVelocity": 0.32313056199342094, - "velocityX": -4.049594966104257, - "velocityY": 0.7915722470772912, - "timestamp": 9.891515009795834 - }, - { - "x": 5.336533047176871, - "y": 6.64421005176547, - "heading": 0.1269873566210566, - "angularVelocity": 0.4362739531611655, - "velocityX": -3.9504417178591757, - "velocityY": 0.5624657429999524, - "timestamp": 9.955828606240152 - }, - { - "x": 5.09664977853333, - "y": 6.672673453532738, - "heading": 0.15653495714348034, - "angularVelocity": 0.45943007631373584, - "velocityX": -3.7298997709019446, - "velocityY": 0.4425720740389818, - "timestamp": 10.02014220268447 - }, - { - "x": 4.872168601658864, - "y": 6.69628481617622, - "heading": 0.18579353230897896, - "angularVelocity": 0.45493607546625786, - "velocityX": -3.4904155464050137, - "velocityY": 0.3671286314072656, - "timestamp": 10.084455799128788 - }, - { - "x": 4.663439660624991, - "y": 6.716350544333371, - "heading": 0.214004287479728, - "angularVelocity": 0.43864371968645294, - "velocityX": -3.2454869976769207, - "velocityY": 0.3119982284698228, - "timestamp": 10.148769395573106 - }, - { - "x": 4.470607956167166, - "y": 6.7336023293083125, - "heading": 0.24075299474172163, - "angularVelocity": 0.415910612076444, - "velocityX": -2.9983038598187646, - "velocityY": 0.2682447558329072, - "timestamp": 10.213082992017425 - }, - { - "x": 4.293747206330888, - "y": 6.748505081783013, - "heading": 0.2657792876291017, - "angularVelocity": 0.3891291153193025, - "velocityX": -2.7499744939532724, - "velocityY": 0.23172009184096956, - "timestamp": 10.277396588461743 - }, - { - "x": 4.1329000630749535, - "y": 6.761379555283891, - "heading": 0.28890428844093546, - "angularVelocity": 0.35956628287542935, - "velocityX": -2.5009819408123937, - "velocityY": 0.20018276402913684, - "timestamp": 10.341710184906061 - }, - { - "x": 3.9880934920167372, - "y": 6.772460125748701, - "heading": 0.3099974247908716, - "angularVelocity": 0.32797320498470917, - "velocityX": -2.2515701043649203, - "velocityY": 0.17228970353732437, - "timestamp": 10.40602378135038 - }, - { - "x": 3.859345677355417, - "y": 6.781925432988514, - "heading": 0.3289591507198067, - "angularVelocity": 0.29483230572172986, - "velocityX": -2.0018755252287743, - "velocityY": 0.1471742798275413, - "timestamp": 10.470337377794698 - }, - { - "x": 3.746669497606821, - "y": 6.789916099525479, - "heading": 0.34571109262456645, - "angularVelocity": 0.2604727900617948, - "velocityX": -1.7519807004752146, - "velocityY": 0.12424536923360925, - "timestamp": 10.534650974239016 - }, - { - "x": 3.6500744318767597, - "y": 6.79654567470217, - "heading": 0.36019003249514225, - "angularVelocity": 0.2251303094690333, - "velocityX": -1.5019384868904437, - "velocityY": 0.10308201598445296, - "timestamp": 10.598964570683334 - }, - { - "x": 3.5695676775339344, - "y": 6.801907753643934, - "heading": 0.3723440332146637, - "angularVelocity": 0.18898026842651122, - "velocityX": -1.2517843627750997, - "velocityY": 0.08337395571411381, - "timestamp": 10.663278167127652 - }, - { - "x": 3.5051548414990994, - "y": 6.806080805460923, - "heading": 0.38212983379787385, - "angularVelocity": 0.1521575704708508, - "velocityX": -1.0015430577048066, - "velocityY": 0.06488599686074766, - "timestamp": 10.72759176357197 - }, - { - "x": 3.456840386946828, - "y": 6.809131562318706, - "heading": 0.3895110346173986, - "angularVelocity": 0.11476890156368999, - "velocityX": -0.7512323555735491, - "velocityY": 0.047435643883240675, - "timestamp": 10.791905360016289 - }, - { - "x": 3.4246279326905182, - "y": 6.8111174678092326, - "heading": 0.3944567935981517, - "angularVelocity": 0.0769006750389882, - "velocityX": -0.5008653851942321, - "velocityY": 0.03087847049956041, - "timestamp": 10.856218956460607 + "heading": 0.12310578689683861, + "angularVelocity": 1.9432806685756468e-7, + "velocityX": -3.662936913327793, + "velocityY": 0.941318096340742, + "timestamp": 9.446032788948806 + }, + { + "x": 5.606427257807288, + "y": 6.600564902613298, + "heading": 0.1231057972666647, + "angularVelocity": 1.578555424299444e-7, + "velocityX": -3.7237014740499577, + "velocityY": 0.6612371129043467, + "timestamp": 9.511724660841988 + }, + { + "x": 5.35931577124569, + "y": 6.62625930787543, + "heading": 0.12310580746861746, + "angularVelocity": 1.5530007712924032e-7, + "velocityX": -3.7616752185629396, + "velocityY": 0.39113522756534835, + "timestamp": 9.577416532735171 + }, + { + "x": 5.111994168115865, + "y": 6.649845615003531, + "heading": 0.1231058176602883, + "angularVelocity": 1.551435596748186e-7, + "velocityX": -3.764873735550995, + "velocityY": 0.35904452785959023, + "timestamp": 9.643108404628354 + }, + { + "x": 4.868859374012009, + "y": 6.67294055859981, + "heading": 0.13716866748145323, + "angularVelocity": 0.21407290454489739, + "velocityX": -3.701139685883833, + "velocityY": 0.35156470550620644, + "timestamp": 9.708800276521536 + }, + { + "x": 4.644141674488456, + "y": 6.6943246178055915, + "heading": 0.16550217221046287, + "angularVelocity": 0.43130913935716303, + "velocityX": -3.4207839272559015, + "velocityY": 0.3255206251475534, + "timestamp": 9.77449214841472 + }, + { + "x": 4.438149971299598, + "y": 6.713938607045374, + "heading": 0.1971398735951766, + "angularVelocity": 0.48160754858923427, + "velocityX": -3.1357258859027066, + "velocityY": 0.2985755874284469, + "timestamp": 9.840184020307902 + }, + { + "x": 4.2509016025753406, + "y": 6.731775563046276, + "heading": 0.22901708759854217, + "angularVelocity": 0.48525354940713283, + "velocityX": -2.8504039134206605, + "velocityY": 0.27152455070706877, + "timestamp": 9.905875892201085 + }, + { + "x": 4.082393919350406, + "y": 6.747832566507683, + "heading": 0.25967107343501483, + "angularVelocity": 0.46663285659323517, + "velocityX": -2.565122265033534, + "velocityY": 0.24442907468850525, + "timestamp": 9.971567764094267 + }, + { + "x": 3.9326220842038664, + "y": 6.762107971713798, + "heading": 0.28824128245235414, + "angularVelocity": 0.43491239013245725, + "velocityX": -2.279914254690053, + "velocityY": 0.21730854662395976, + "timestamp": 10.03725963598745 + }, + { + "x": 3.801581586235111, + "y": 6.77460070477556, + "heading": 0.314159509734349, + "angularVelocity": 0.3945423769342238, + "velocityX": -1.9947749119073828, + "velocityY": 0.19017167119358822, + "timestamp": 10.102951507880633 + }, + { + "x": 3.6892685916749244, + "y": 6.785310000165767, + "heading": 0.3370219943875935, + "angularVelocity": 0.34802607985383205, + "velocityX": -1.7096939289964497, + "velocityY": 0.16302314246152652, + "timestamp": 10.168643379773815 + }, + { + "x": 3.595679898494151, + "y": 6.794235280663889, + "heading": 0.35652698961990326, + "angularVelocity": 0.2969164170572818, + "velocityX": -1.4246616892414212, + "velocityY": 0.135865826941808, + "timestamp": 10.234335251666998 + }, + { + "x": 3.520812828526041, + "y": 6.801376095406927, + "heading": 0.37244068726433127, + "angularVelocity": 0.2422475899347831, + "velocityX": -1.1396702181031793, + "velocityY": 0.10870164812244498, + "timestamp": 10.30002712356018 + }, + { + "x": 3.464665127058438, + "y": 6.806732084957018, + "heading": 0.38457706026543664, + "angularVelocity": 0.18474695044220302, + "velocityX": -0.8547130695088268, + "velocityY": 0.08153199773025213, + "timestamp": 10.365718995453364 + }, + { + "x": 3.4272348818352074, + "y": 6.810302960282004, + "heading": 0.39278518447772715, + "angularVelocity": 0.12494885555456782, + "velocityX": -0.5697850303930024, + "velocityY": 0.05435794752192765, + "timestamp": 10.431410867346546 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 0.038624336675044973, - "velocityX": -0.2504520576093102, - "velocityY": 0.015098233918219028, - "timestamp": 10.920532552904925 + "angularVelocity": 0.0632601721112495, + "velocityX": -0.28488184560874424, + "velocityY": 0.027180367966521624, + "timestamp": 10.497102739239729 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, "angularVelocity": 0, - "velocityX": -2.188238548497709e-39, - "velocityY": -5.246059499257382e-38, - "timestamp": 10.984846149349243 - }, - { - "x": 3.439618429683473, - "y": 6.811489460292554, - "heading": 0.3933053508875659, - "angularVelocity": -0.04072214124873058, - "velocityX": 0.34833488669246676, - "velocityY": -0.006709852295815464, - "timestamp": 11.07412221885228 - }, - { - "x": 3.5018143627173566, - "y": 6.810291415046735, - "heading": 0.38603451078645395, - "angularVelocity": -0.08144220664715371, - "velocityX": 0.6966697053320448, - "velocityY": -0.013419556354671087, - "timestamp": 11.163398288355316 - }, - { - "x": 3.5951082578030915, - "y": 6.808494374460801, - "heading": 0.37512964589240666, - "angularVelocity": -0.12214768139715662, - "velocityX": 1.0450045079836527, - "velocityY": -0.0201290289316947, - "timestamp": 11.252674357858352 - }, - { - "x": 3.719500117899943, - "y": 6.8060983670587145, - "heading": 0.3605931109058113, - "angularVelocity": -0.1628267806536994, - "velocityX": 1.3933393437826105, - "velocityY": -0.026838182005813428, - "timestamp": 11.341950427361388 - }, - { - "x": 3.874989949959029, - "y": 6.8031034297547235, - "heading": 0.3424282200177148, - "angularVelocity": -0.20346875696043784, - "velocityX": 1.7416742574424975, - "velocityY": -0.03354692159569182, - "timestamp": 11.431226496864424 - }, - { - "x": 4.06157776435933, - "y": 6.799509608467501, - "heading": 0.32063912477995166, - "angularVelocity": -0.2440642308633667, - "velocityX": 2.0900092873595257, - "velocityY": -0.04025514684089667, - "timestamp": 11.52050256636746 - }, - { - "x": 4.279263574094258, - "y": 6.795316958786605, - "heading": 0.2952306601922425, - "angularVelocity": -0.2846055469192104, - "velocityX": 2.438344462818484, - "velocityY": -0.046962749415771964, - "timestamp": 11.609778635870496 - }, - { - "x": 4.52804739348343, - "y": 6.790525546657334, - "heading": 0.26620815675428894, - "angularVelocity": -0.3250871549286381, - "velocityX": 2.786679798674495, - "velocityY": -0.05366961332351963, - "timestamp": 11.699054705373532 - }, - { - "x": 4.807929235531671, - "y": 6.785135449055675, - "heading": 0.23357721651331317, - "angularVelocity": -0.3655060132308565, - "velocityX": 3.135015280200263, - "velocityY": -0.06037561500706378, - "timestamp": 11.788330774876568 - }, - { - "x": 5.118909101830157, - "y": 6.779146754720857, - "heading": 0.19734345180613538, - "angularVelocity": -0.40586200656991683, - "velocityX": 3.483350779549144, - "velocityY": -0.06708062270387599, - "timestamp": 11.877606844379605 - }, - { - "x": 5.4298901294687205, - "y": 6.773164805475954, - "heading": 0.1614535287560628, - "angularVelocity": -0.40201056397147766, - "velocityX": 3.483363787963201, - "velocityY": -0.06700506953546043, - "timestamp": 11.96688291388264 - }, - { - "x": 5.709773101250217, - "y": 6.767781279592902, - "heading": 0.12915793357975602, - "angularVelocity": -0.36174974274834387, - "velocityX": 3.1350279345796865, - "velocityY": -0.06030200380705819, - "timestamp": 12.056158983385677 - }, - { - "x": 5.958557987999821, - "y": 6.762996102236048, - "heading": 0.10045393097970265, - "angularVelocity": -0.32151956016698346, - "velocityX": 2.786691754402813, - "velocityY": -0.05359977632853685, - "timestamp": 12.145435052888713 - }, - { - "x": 6.176244776203503, - "y": 6.758809206943986, - "heading": 0.07533966785700749, - "angularVelocity": -0.2813101345354471, - "velocityX": 2.43835542285245, - "velocityY": -0.04689829329818694, - "timestamp": 12.234711122391749 - }, - { - "x": 6.36283345808173, - "y": 6.755220536230104, - "heading": 0.05381396772349776, - "angularVelocity": -0.24111388699496414, - "velocityX": 2.090019004161915, - "velocityY": -0.04019745418742752, - "timestamp": 12.323987191894785 - }, - { - "x": 6.5183240290861875, - "y": 6.752230041904144, - "heading": 0.03587615193014948, - "angularVelocity": -0.20092524114469607, - "velocityX": 1.7416825345247642, - "velocityY": -0.033497154865871154, - "timestamp": 12.413263261397821 - }, - { - "x": 6.642716486724279, - "y": 6.749837685266528, - "heading": 0.021525889401728494, - "angularVelocity": -0.16074030373764248, - "velocityX": 1.393346036967506, - "velocityY": -0.026797289026421714, - "timestamp": 12.502539330900857 - }, - { - "x": 6.736010829843697, - "y": 6.748043437222755, - "heading": 0.010763075905262787, - "angularVelocity": -0.12055653386599446, - "velocityX": 1.0450095265030122, - "velocityY": -0.020097749080595733, - "timestamp": 12.591815400403894 - }, - { - "x": 6.7982070581615215, - "y": 6.746847278346797, - "heading": 0.0035877434328324036, - "angularVelocity": -0.08037240564434038, - "velocityX": 0.6966730128694666, - "velocityY": -0.013398426729768601, - "timestamp": 12.68109146990693 + "velocityX": 1.9034366224257262e-34, + "velocityY": -2.547324229323533e-33, + "timestamp": 10.562794611132912 + }, + { + "x": 3.4428034429775143, + "y": 6.811427919070607, + "heading": 0.39249813327278055, + "angularVelocity": -0.050100574262489476, + "velocityX": 0.38660845958378076, + "velocityY": -0.007449238878501296, + "timestamp": 10.651470846737375 + }, + { + "x": 3.5113694006298597, + "y": 6.810106800279428, + "heading": 0.3836131655700934, + "angularVelocity": -0.10019558951868686, + "velocityX": 0.7732168284429818, + "velocityY": -0.014898228168725063, + "timestamp": 10.740147082341839 + }, + { + "x": 3.614218332551768, + "y": 6.808125167746075, + "heading": 0.37028807906085554, + "angularVelocity": -0.1502667137188131, + "velocityX": 1.1598251912796755, + "velocityY": -0.022346827420505266, + "timestamp": 10.828823317946302 + }, + { + "x": 3.7513502451988723, + "y": 6.8054830693306245, + "heading": 0.3525264976102127, + "angularVelocity": -0.20029697166969943, + "velocityX": 1.5464336269164098, + "velocityY": -0.02979488695523812, + "timestamp": 10.917499553550766 + }, + { + "x": 3.9227651512282486, + "y": 6.802180567172425, + "heading": 0.3303333796886753, + "angularVelocity": -0.2502713130553812, + "velocityX": 1.9330422052867193, + "velocityY": -0.03724224574585722, + "timestamp": 11.006175789155229 + }, + { + "x": 4.128463068361194, + "y": 6.79821773882544, + "heading": 0.3037147939289921, + "angularVelocity": -0.3001772186001935, + "velocityX": 2.319650983499712, + "velocityY": -0.04468872996211646, + "timestamp": 11.094852024759692 + }, + { + "x": 4.368444017678072, + "y": 6.793594678448866, + "heading": 0.2726776385751228, + "angularVelocity": -0.3500053327963678, + "velocityX": 2.706259999435507, + "velocityY": -0.05213415234714548, + "timestamp": 11.183528260364156 + }, + { + "x": 4.642708020512335, + "y": 6.788311497970363, + "heading": 0.23722930806330966, + "angularVelocity": -0.3997500600942169, + "velocityX": 3.092869255948188, + "velocityY": -0.05957831252634942, + "timestamp": 11.27220449596862 + }, + { + "x": 4.951255088173746, + "y": 6.782368328182758, + "heading": 0.19737735072702745, + "angularVelocity": -0.44940966499796203, + "velocityX": 3.479478640000833, + "velocityY": -0.0670209977576627, + "timestamp": 11.360880731573083 + }, + { + "x": 5.286562692788269, + "y": 6.775920770205836, + "heading": 0.19737734601955828, + "angularVelocity": -5.3086028753514235e-8, + "velocityX": 3.781256639153567, + "velocityY": -0.07270897251075666, + "timestamp": 11.449556967177546 + }, + { + "x": 5.595111114675473, + "y": 6.769985794431735, + "heading": 0.15789301075905496, + "angularVelocity": -0.4452639987631135, + "velocityX": 3.4794939115759345, + "velocityY": -0.06692859404378032, + "timestamp": 11.53823320278201 + }, + { + "x": 5.8693764275565705, + "y": 6.76471055259859, + "heading": 0.12280143262174693, + "angularVelocity": -0.3957269712466434, + "velocityX": 3.092884029318143, + "velocityY": -0.059488788593539996, + "timestamp": 11.626909438386473 + }, + { + "x": 6.109358599544783, + "y": 6.760094936608861, + "heading": 0.09209943623091729, + "angularVelocity": -0.3462257523850543, + "velocityX": 2.7062737874738274, + "velocityY": -0.05205020215694004, + "timestamp": 11.715585673990937 + }, + { + "x": 6.315057615509001, + "y": 6.756138852616793, + "heading": 0.06578495581515914, + "angularVelocity": -0.29674782918315123, + "velocityX": 2.3196633749963063, + "velocityY": -0.04461267401690494, + "timestamp": 11.8042619095954 + }, + { + "x": 6.4864734670806214, + "y": 6.752842221795132, + "heading": 0.043856783005957245, + "angularVelocity": -0.24728353272698234, + "velocityX": 1.9330528681462538, + "velocityY": -0.037176034810112814, + "timestamp": 11.892938145199864 + }, + { + "x": 6.623606149810205, + "y": 6.750204980767773, + "heading": 0.026314311963007715, + "angularVelocity": -0.19782606831887856, + "velocityX": 1.5464423111199512, + "velocityY": -0.029740110294281013, + "timestamp": 11.981614380804327 + }, + { + "x": 6.726455661694478, + "y": 6.74822708185799, + "heading": 0.013157327563715553, + "angularVelocity": -0.14837102984364775, + "velocityX": 1.1598317315027789, + "velocityY": -0.022304723427886322, + "timestamp": 12.07029061640879 + }, + { + "x": 6.795022002255089, + "y": 6.7469084932218335, + "heading": 0.004385844550104781, + "angularVelocity": -0.09891582512292933, + "velocityX": 0.7732211464912397, + "velocityY": -0.014869695664985829, + "timestamp": 12.158966852013254 }, { "x": 6.829305171966553, "y": 6.746249198913574, - "heading": -2.600985796322667e-43, - "angularVelocity": -0.04018706751768873, - "velocityX": 0.34833650247083475, - "velocityY": -0.006699213311607584, - "timestamp": 12.770367539409966 + "heading": -4.086871260037018e-38, + "angularVelocity": -0.04945907457853377, + "velocityX": 0.3866105668308079, + "velocityY": -0.007434847721776456, + "timestamp": 12.247643087617718 }, { "x": 6.829305171966553, "y": 6.746249198913574, - "heading": 1.4984085590895388e-43, - "angularVelocity": 4.5922760731607736e-42, - "velocityX": 5.97613511124824e-44, - "velocityY": 5.676824977871388e-42, - "timestamp": 12.859643608913002 + "heading": -5.480350490542979e-38, + "angularVelocity": -1.571392014077605e-37, + "velocityX": 4.425248303324229e-37, + "velocityY": 1.8999126670467387e-37, + "timestamp": 12.336319323222181 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.1.traj b/src/main/deploy/choreo/SourceLanePGHSprint.1.traj index 0f4422b2..3b1771e0 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.1.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.1.traj @@ -3,191 +3,182 @@ { "x": 0.433241993188858, "y": 4.121134281158447, - "heading": 5.938255486131716e-34, + "heading": -6.349582694240188e-38, "angularVelocity": 0, - "velocityX": 2.0926991678208567e-34, - "velocityY": 4.74101269436352e-34, + "velocityX": -2.609211774957871e-36, + "velocityY": -5.016830702674688e-35, "timestamp": 0 }, { - "x": 0.45254673947106955, - "y": 4.110409875807789, - "heading": -0.00804589337615742, - "angularVelocity": -0.10676392342489109, - "velocityX": 0.2561617905500149, - "velocityY": -0.1423060856148132, - "timestamp": 0.0753615371002897 - }, - { - "x": 0.49115624056874074, - "y": 4.0889613071373185, - "heading": -0.024138137832086688, - "angularVelocity": -0.21353392028766638, - "velocityX": 0.5123236943308417, - "velocityY": -0.28460895963318283, - "timestamp": 0.1507230742005794 - }, - { - "x": 0.5490705891188512, - "y": 4.056788900712756, - "heading": -0.048273761664479664, - "angularVelocity": -0.32026448452442974, - "velocityX": 0.7684868273458795, - "velocityY": -0.4269075136000509, - "timestamp": 0.2260846113008691 - }, - { - "x": 0.6262899536624102, - "y": 4.013893070922723, - "heading": -0.08044715876351015, - "angularVelocity": -0.4269206592245423, - "velocityX": 1.024652196793663, - "velocityY": -0.5692005688916374, - "timestamp": 0.3014461484011588 - }, - { - "x": 0.722814567995576, - "y": 3.960274325008801, - "heading": -0.12065106662148206, - "angularVelocity": -0.5334804650344289, - "velocityX": 1.2808206685688042, - "velocityY": -0.7114868934078041, - "timestamp": 0.3768076855014485 - }, - { - "x": 0.8386447198033338, - "y": 3.895933264485532, - "heading": -0.16887761745268762, - "angularVelocity": -0.6399358702971596, - "velocityX": 1.5369929577420018, - "velocityY": -0.8537652362059028, - "timestamp": 0.4521692226017382 - }, - { - "x": 0.973780741976376, - "y": 3.8208705846111153, - "heading": -0.22511931827381823, - "angularVelocity": -0.7462918484038512, - "velocityX": 1.7931696641644363, - "velocityY": -0.99603435336682, - "timestamp": 0.5275307597020279 - }, - { - "x": 1.1282230140958218, - "y": 3.7350870801858624, - "heading": -0.289369793237253, - "angularVelocity": -0.8525632230395118, - "velocityX": 2.049351407388589, - "velocityY": -1.1382929240295867, - "timestamp": 0.6028922968023176 - }, - { - "x": 1.3019720073188241, - "y": 3.638583716526852, - "heading": -0.36162402966579443, - "angularVelocity": -0.9587680826147016, - "velocityX": 2.305539402570577, - "velocityY": -1.2805386855443919, - "timestamp": 0.6782538339026073 - }, - { - "x": 1.4950570361880926, - "y": 3.5314160759250663, - "heading": -0.44172046980509583, - "angularVelocity": -1.0628291728273882, - "velocityX": 2.56211638321966, - "velocityY": -1.4220469051629, - "timestamp": 0.753615371002897 - }, - { - "x": 1.668834943253154, - "y": 3.4349673750959555, - "heading": -0.5138143173751882, - "angularVelocity": -0.9566398237625026, - "velocityX": 2.30592307099311, - "velocityY": -1.279813344315926, - "timestamp": 0.8289769081031867 - }, - { - "x": 1.8233052433392802, - "y": 3.34923681430091, - "heading": -0.5779052248066122, - "angularVelocity": -0.8504458626704097, - "velocityX": 2.0497233208043544, - "velocityY": -1.1375903954952034, - "timestamp": 0.9043384452034764 - }, - { - "x": 1.958467560801758, - "y": 3.2742237758858077, - "heading": -0.6339913690664654, - "angularVelocity": -0.7442277110831049, - "velocityX": 1.7935185860474994, - "velocityY": -0.9953756425545938, - "timestamp": 0.9796999823037661 - }, - { - "x": 2.074321574453109, - "y": 3.2099277413643645, - "heading": -0.6820701443598719, - "angularVelocity": -0.6379749822435855, - "velocityX": 1.53730958933567, - "velocityY": -0.8531677695994906, - "timestamp": 1.0550615194040558 - }, - { - "x": 2.1708670101410656, - "y": 3.1563482775049274, - "heading": -0.7221386740225205, - "angularVelocity": -0.5316840818855123, - "velocityX": 1.281096954796391, - "velocityY": -0.7109656453547993, - "timestamp": 1.1304230565043456 - }, - { - "x": 2.2481036395899285, - "y": 3.1134850360999096, - "heading": -0.7541943378892257, - "angularVelocity": -0.4253584135902941, - "velocityX": 1.0248812911827636, - "velocityY": -0.5687681416048598, - "timestamp": 1.2057845936046352 - }, - { - "x": 2.306031279527139, - "y": 3.0813377577306427, - "heading": -0.7782353006154736, - "angularVelocity": -0.31900839143254983, - "velocityX": 0.7686631956580385, - "velocityY": -0.4265740801768146, - "timestamp": 1.2811461307049248 - }, - { - "x": 2.3446497902447807, - "y": 3.0599062760826214, - "heading": -0.7942609825639004, - "angularVelocity": -0.21265067785308137, - "velocityX": 0.5124432462974974, - "velocityY": -0.284382225637208, - "timestamp": 1.3565076678052144 + "x": 0.45469188032137553, + "y": 4.109218526997661, + "heading": -0.008937300145995164, + "angularVelocity": -0.11893237262991438, + "velocityX": 0.2854425752342375, + "velocityY": -0.1585678997982562, + "timestamp": 0.07514606787342622 + }, + { + "x": 0.4975916635116251, + "y": 4.0853873108981436, + "heading": -0.026812413296636577, + "angularVelocity": -0.2378715700833438, + "velocityX": 0.570885269240018, + "velocityY": -0.31713191087600345, + "timestamp": 0.15029213574685243 + }, + { + "x": 0.5619414537480725, + "y": 4.049641027140498, + "heading": -0.05362171709802473, + "angularVelocity": -0.35676256336585627, + "velocityX": 0.856329440215504, + "velocityY": -0.4756906750976657, + "timestamp": 0.22543820362027867 + }, + { + "x": 0.6477414523773106, + "y": 4.001980177860296, + "heading": -0.08935853104911506, + "angularVelocity": -0.4755646564406316, + "velocityX": 1.141776290593902, + "velocityY": -0.6342427571923069, + "timestamp": 0.30058427149370487 + }, + { + "x": 0.7549919358777414, + "y": 3.942405375853288, + "heading": -0.13401442200653152, + "angularVelocity": -0.5942545261667384, + "velocityX": 1.4272268201854585, + "velocityY": -0.7927866845588475, + "timestamp": 0.37573033936713107 + }, + { + "x": 0.8836932393269182, + "y": 3.8709173416225418, + "heading": -0.18758057158075755, + "angularVelocity": -0.7128270459134503, + "velocityX": 1.7126818087934743, + "velocityY": -0.9513210238912078, + "timestamp": 0.4508764072405573 + }, + { + "x": 1.0338457421402325, + "y": 3.7875168932975933, + "heading": -0.2500489995023069, + "angularVelocity": -0.8312933688928248, + "velocityX": 1.998141846440012, + "velocityY": -1.109844476033343, + "timestamp": 0.5260224751139835 + }, + { + "x": 1.2054498603190866, + "y": 3.692204931169301, + "heading": -0.32141344961465557, + "angularVelocity": -0.9496764385936038, + "velocityX": 2.283607419990343, + "velocityY": -1.2683559476303248, + "timestamp": 0.6011685429874097 + }, + { + "x": 1.3985060453231253, + "y": 3.5849824249792595, + "heading": -0.40166985798593763, + "angularVelocity": -1.0680054278616893, + "velocityX": 2.5690790013020615, + "velocityY": -1.4268545144723213, + "timestamp": 0.676314610860836 + }, + { + "x": 1.5915937569821006, + "y": 3.4778198154599647, + "heading": -0.48176859267038563, + "angularVelocity": -1.065907198489266, + "velocityX": 2.5694985396202825, + "velocityY": -1.4260574445464824, + "timestamp": 0.7514606787342623 + }, + { + "x": 1.7632284858900336, + "y": 3.382566036520044, + "heading": -0.5529765476804537, + "angularVelocity": -0.9475938931363465, + "velocityX": 2.2840147697019786, + "velocityY": -1.2675816797275796, + "timestamp": 0.8266067466076885 + }, + { + "x": 1.9134098098562766, + "y": 3.2992204007390877, + "heading": -0.6152918418017524, + "angularVelocity": -0.8292555536806094, + "velocityX": 1.9985253815170194, + "velocityY": -1.1091150626981792, + "timestamp": 0.9017528144811148 + }, + { + "x": 2.0421373577646693, + "y": 3.2277823059512354, + "heading": -0.6687115003742364, + "angularVelocity": -0.7108776291856339, + "velocityX": 1.7130310547348626, + "velocityY": -0.9506564589404816, + "timestamp": 0.976898882354541 + }, + { + "x": 2.149410808485792, + "y": 3.1682512394298143, + "heading": -0.713232041811307, + "angularVelocity": -0.5924533737687984, + "velocityX": 1.4275324545498613, + "velocityY": -0.792204678250007, + "timestamp": 1.0520449502279672 + }, + { + "x": 2.2352298936713018, + "y": 3.120626784362015, + "heading": -0.7488501306819736, + "angularVelocity": -0.4739847323836121, + "velocityX": 1.1420302833417786, + "velocityY": -0.6337584442610682, + "timestamp": 1.1271910181013933 + }, + { + "x": 2.2995943987852443, + "y": 3.0849086278583613, + "heading": -0.7755632624607879, + "angularVelocity": -0.3554827622359307, + "velocityX": 0.8565252572144717, + "velocityY": -0.4753163740226096, + "timestamp": 1.2023370859748195 + }, + { + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080857, + "angularVelocity": -0.23696688264902363, + "velocityX": 0.5710180813549187, + "velocityY": -0.3168769856057537, + "timestamp": 1.2774831538482456 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.1063066084363967, - "velocityX": 0.25622200022152375, - "velocityY": -0.14219129883360584, - "timestamp": 1.431869204905504 + "angularVelocity": -0.11846288599550474, + "velocityX": 0.2855094397331478, + "velocityY": -0.15843872456938407, + "timestamp": 1.3526292217216718 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": 1.280873526188021e-35, - "velocityY": 2.1400066509893793e-37, - "timestamp": 1.5072307420057935 + "angularVelocity": 3.467177167133168e-32, + "velocityX": -1.439882477346126e-32, + "velocityY": -1.3815367235364851e-32, + "timestamp": 1.427775289595098 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.2.traj b/src/main/deploy/choreo/SourceLanePGHSprint.2.traj index 5f35cb92..3cd03d5b 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.2.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.2.traj @@ -4,730 +4,811 @@ "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": 1.280873526188021e-35, - "velocityY": 2.1400066509893793e-37, + "angularVelocity": 3.467177167133168e-32, + "velocityX": -1.439882477346126e-32, + "velocityY": -1.3815367235364851e-32, "timestamp": 0 }, { - "x": 2.374445038905671, - "y": 3.0395740293561597, - "heading": -0.8007582593302504, - "angularVelocity": 0.02507755763306408, - "velocityX": 0.17366966855318383, - "velocityY": -0.15926936399484595, - "timestamp": 0.06037879252400313 - }, - { - "x": 2.3955442849732433, - "y": 3.020481915034863, - "heading": -0.7977129883648791, - "angularVelocity": 0.050436102447073194, - "velocityX": 0.349447963193112, - "velocityY": -0.3162056331899431, - "timestamp": 0.12075758504800627 - }, - { - "x": 2.4273985359193944, - "y": 2.992075542224587, - "heading": -0.7931177380351067, - "angularVelocity": 0.07610702595527519, - "velocityX": 0.527573500803075, - "velocityY": -0.4704693754679464, - "timestamp": 0.1811363775720094 - }, - { - "x": 2.470166324344177, - "y": 2.9545414012529414, - "heading": -0.7869514399590605, - "angularVelocity": 0.10212688625057974, - "velocityX": 0.7083246722395268, - "velocityY": -0.6216444450545164, - "timestamp": 0.24151517009601253 - }, - { - "x": 2.524025883283138, - "y": 2.908097185821786, - "heading": -0.7791904398300458, - "angularVelocity": 0.12853851169563282, - "velocityX": 0.8920277582157603, - "velocityY": -0.7692140483381156, - "timestamp": 0.30189396262001567 - }, - { - "x": 2.589178616513917, - "y": 2.8529998964635936, - "heading": -0.7698080390828491, - "angularVelocity": 0.15539232162463004, - "velocityX": 1.0790665150330332, - "velocityY": -0.9125271813988101, - "timestamp": 0.3622727551440188 - }, - { - "x": 2.6658532194281834, - "y": 2.7895568383610723, - "heading": -0.758773944440501, - "angularVelocity": 0.18274785203698177, - "velocityX": 1.269892949313043, - "velocityY": -1.050750693255423, - "timestamp": 0.42265154766802193 - }, - { - "x": 2.7543104503881763, - "y": 2.7181408322396914, - "heading": -0.7460536194222877, - "angularVelocity": 0.21067537932555425, - "velocityX": 1.4650380913932193, - "velocityY": -1.1827995085027596, - "timestamp": 0.48303034019202506 - }, - { - "x": 2.8548483465105847, - "y": 2.6392116728155934, - "heading": -0.7316075530041294, - "angularVelocity": 0.23925729240801266, - "velocityX": 1.6651193559798465, - "velocityY": -1.3072331546332332, - "timestamp": 0.543409132716028 - }, - { - "x": 2.9678071516801605, - "y": 2.5533470022723117, - "heading": -0.7153905179087263, - "angularVelocity": 0.2685882644797192, - "velocityX": 1.870835776066071, - "velocityY": -1.4220998293257756, - "timestamp": 0.6037879252400309 - }, - { - "x": 3.093571941086637, - "y": 2.4612875031660972, - "heading": -0.6973510365861332, - "angularVelocity": 0.2987718132226927, - "velocityX": 2.0829298525054067, - "velocityY": -1.5246992405424025, - "timestamp": 0.6641667177640338 - }, - { - "x": 3.2325678737819445, - "y": 2.364003698472316, - "heading": -0.6774316325200306, - "angularVelocity": 0.3299072941576924, - "velocityX": 2.302065458497713, - "velocityY": -1.611224746753705, - "timestamp": 0.7245455102880367 - }, - { - "x": 3.3852359851566844, - "y": 2.2627937385026136, - "heading": -0.6555713107834819, - "angularVelocity": 0.36205297957653465, - "velocityX": 2.528505539656955, - "velocityY": -1.6762501490811843, - "timestamp": 0.7849243028120396 - }, - { - "x": 3.551962630271022, - "y": 2.159418364676261, - "heading": -0.6317136848324263, - "angularVelocity": 0.39513254495063227, - "velocityX": 2.761344474519871, - "velocityY": -1.7121139642740746, - "timestamp": 0.8453030953360425 - }, - { - "x": 3.7329118909154024, - "y": 2.0562532945853267, - "heading": -0.6058279711502043, - "angularVelocity": 0.4287219502101063, - "velocityX": 2.9969009494922476, - "velocityY": -1.7086308913832926, - "timestamp": 0.9056818878600454 - }, - { - "x": 3.927705999512227, - "y": 1.9563533988353388, - "heading": -0.5779537605481534, - "angularVelocity": 0.46165564823062205, - "velocityX": 3.2262007975629157, - "velocityY": -1.6545527257815482, - "timestamp": 0.9660606803840484 - }, - { - "x": 4.135029249104847, - "y": 1.8631919103127867, - "heading": -0.5482681931936219, - "angularVelocity": 0.491655531910981, - "velocityX": 3.433709766723148, - "velocityY": -1.5429505067614013, - "timestamp": 1.0264394729080513 - }, - { - "x": 4.35253951782844, - "y": 1.7799641253131642, - "heading": -0.5171193508091871, - "angularVelocity": 0.5158904489859046, - "velocityX": 3.602428263816689, - "velocityY": -1.3784274497794216, - "timestamp": 1.0868182654320542 - }, - { - "x": 4.577376652915107, - "y": 1.7089361045956586, - "heading": -0.4849529126600114, - "angularVelocity": 0.5327439785482329, - "velocityX": 3.7237766057889528, - "velocityY": -1.1763736528727222, - "timestamp": 1.147197057956057 - }, - { - "x": 4.806831379756707, - "y": 1.6513703939365607, - "heading": -0.452194444563849, - "angularVelocity": 0.5425492416586429, - "velocityX": 3.8002536528099977, - "velocityY": -0.9534094381932716, - "timestamp": 1.20757585048006 - }, - { - "x": 5.0386739792276956, - "y": 1.6078357362088507, - "heading": -0.4191901676167688, - "angularVelocity": 0.5466203540582497, - "velocityX": 3.8398018539178493, - "velocityY": -0.7210256434062197, - "timestamp": 1.267954643004063 - }, - { - "x": 5.271173472263796, - "y": 1.578518825120004, - "heading": -0.38620445457775493, - "angularVelocity": 0.5463128966333781, - "velocityX": 3.8506813951880883, - "velocityY": -0.4855498075287272, - "timestamp": 1.3283334355280658 + "x": 2.3773546785315864, + "y": 3.0407954088560687, + "heading": -0.7860770412156382, + "angularVelocity": 0.2650004205849919, + "velocityX": 0.21918861143707746, + "velocityY": -0.13736692695017716, + "timestamp": 0.06111450966076326 + }, + { + "x": 2.4041715283150715, + "y": 3.0239975454455843, + "heading": -0.75411979281041, + "angularVelocity": 0.5229077118120985, + "velocityX": 0.438796775632191, + "velocityY": -0.2748588429118795, + "timestamp": 0.12222901932152652 + }, + { + "x": 2.4444388170293236, + "y": 2.998788446329585, + "heading": -0.7069088657797715, + "angularVelocity": 0.772499481591177, + "velocityX": 0.6588826276733534, + "velocityY": -0.41248959135778174, + "timestamp": 0.18334352898228978 + }, + { + "x": 2.498190798561039, + "y": 2.965157152573787, + "heading": -0.6450804787000075, + "angularVelocity": 1.0116809808826641, + "velocityX": 0.8795289666903056, + "velocityY": -0.5502996578468837, + "timestamp": 0.24445803864305304 + }, + { + "x": 2.5654677170302604, + "y": 2.923088008476279, + "heading": -0.5694581806203519, + "angularVelocity": 1.237386972413308, + "velocityX": 1.1008338092322851, + "velocityY": -0.6883658943027919, + "timestamp": 0.3055725483038163 + }, + { + "x": 2.6463157759141644, + "y": 2.8725583813996622, + "heading": -0.4811095285927534, + "angularVelocity": 1.445624819997864, + "velocityX": 1.3228946666295571, + "velocityY": -0.8268024624119354, + "timestamp": 0.36668705796457957 + }, + { + "x": 2.7407862870930626, + "y": 2.813537598490058, + "heading": -0.38139476987658943, + "angularVelocity": 1.6316053138553275, + "velocityX": 1.5457951262848801, + "velocityY": -0.9657409220366661, + "timestamp": 0.4278015676253428 + }, + { + "x": 2.84893500974016, + "y": 2.7459886359527177, + "heading": -0.27202745092011676, + "angularVelocity": 1.7895475160244771, + "velocityX": 1.7696079580350594, + "velocityY": -1.105285191884772, + "timestamp": 0.4889160772861061 + }, + { + "x": 2.970822636317052, + "y": 2.669873093439706, + "heading": -0.15523119808259794, + "angularVelocity": 1.9111051284848073, + "velocityX": 1.9944138839282306, + "velocityY": -1.245457796119401, + "timestamp": 0.5500305869468693 + }, + { + "x": 3.106512263881207, + "y": 2.5851587340080315, + "heading": -0.03419676462946541, + "angularVelocity": 1.980453318286855, + "velocityX": 2.220252249708728, + "velocityY": -1.3861578846318265, + "timestamp": 0.6111450966076324 + }, + { + "x": 3.256035657144846, + "y": 2.4918371927335334, + "heading": 0.08572826850512172, + "angularVelocity": 1.962300504418209, + "velocityX": 2.4466103727841246, + "velocityY": -1.5269948461095477, + "timestamp": 0.6722596062683956 + }, + { + "x": 3.419180860242083, + "y": 2.390029027608911, + "heading": 0.19415018142326176, + "angularVelocity": 1.7740780956923756, + "velocityX": 2.6695003200194134, + "velocityY": -1.6658591501387068, + "timestamp": 0.7333741159291589 + }, + { + "x": 3.5929644641848295, + "y": 2.279455345020229, + "heading": 0.26705766200125053, + "angularVelocity": 1.1929651564364412, + "velocityX": 2.8435735622750062, + "velocityY": -1.8092869140644157, + "timestamp": 0.7944886255899222 + }, + { + "x": 3.7768655464749616, + "y": 2.1600020280366277, + "heading": 0.30152237044830693, + "angularVelocity": 0.5639365944088304, + "velocityX": 3.0091230922237155, + "velocityY": -1.9545819421061743, + "timestamp": 0.8556031352506854 + }, + { + "x": 3.9728967066647076, + "y": 2.0375522576737963, + "heading": 0.30152241765502197, + "angularVelocity": 7.724305624896446e-7, + "velocityX": 3.2076042379769354, + "velocityY": -2.003612088889046, + "timestamp": 0.9167176449114487 + }, + { + "x": 4.174386727718979, + "y": 1.9243094504352285, + "heading": 0.30152246485493484, + "angularVelocity": 7.723192600121745e-7, + "velocityX": 3.2969260847008353, + "velocityY": -1.85296107040964, + "timestamp": 0.977832154572212 + }, + { + "x": 4.383354452565466, + "y": 1.8255438101811146, + "heading": 0.30152251288342946, + "angularVelocity": 7.858771165145409e-7, + "velocityX": 3.419281705873656, + "velocityY": -1.6160751481496927, + "timestamp": 1.0389466642329752 + }, + { + "x": 4.598761540028333, + "y": 1.7417467464566943, + "heading": 0.30152256294147634, + "angularVelocity": 8.190861247953373e-7, + "velocityX": 3.5246472344874826, + "velocityY": -1.3711484259558735, + "timestamp": 1.1000611738937385 + }, + { + "x": 4.819537390717514, + "y": 1.6733348748555603, + "heading": 0.30152261646908723, + "angularVelocity": 8.758576510857166e-7, + "velocityX": 3.612494838208991, + "velocityY": -1.1194047367945392, + "timestamp": 1.1611756835545017 + }, + { + "x": 5.04458467733647, + "y": 1.620648272623701, + "heading": 0.30152267534909943, + "angularVelocity": 9.634375304093619e-7, + "velocityX": 3.682387175617667, + "velocityY": -0.8620964567058491, + "timestamp": 1.222290193215265 + }, + { + "x": 5.27278482830379, + "y": 1.5839488347993986, + "heading": 0.301522742248914, + "angularVelocity": 0.000001094663361600596, + "velocityX": 3.7339766322927477, + "velocityY": -0.6005028597630109, + "timestamp": 1.2834047028760283 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.35343685387939117, - "angularVelocity": 0.5427004967901151, - "velocityX": 3.839595250328378, - "velocityY": -0.2500851718896869, - "timestamp": 1.3887122280520687 - }, - { - "x": 5.773415110681756, - "y": 1.565214819314658, - "heading": -0.31539772557234425, - "angularVelocity": 0.5354796131957132, - "velocityX": 3.8066028075669824, - "velocityY": 0.025280100286796007, - "timestamp": 1.4597497192462368 - }, - { - "x": 6.038838987490814, - "y": 1.5860691706810086, - "heading": -0.2782614638760959, - "angularVelocity": 0.5227698933615667, - "velocityX": 3.7363914793045105, - "velocityY": 0.2935682414423857, - "timestamp": 1.5307872104404048 - }, - { - "x": 6.296044587353018, - "y": 1.6248260837737782, - "heading": -0.24248714943667204, - "angularVelocity": 0.5035976614326285, - "velocityX": 3.620702188921338, - "velocityY": 0.5455839225351394, - "timestamp": 1.6018247016345728 - }, - { - "x": 6.541409154187439, - "y": 1.6793226331219655, - "heading": -0.2085611266677552, - "angularVelocity": 0.4775791233418755, - "velocityX": 3.4540150941389784, - "velocityY": 0.7671519423346602, - "timestamp": 1.6728621928287408 - }, - { - "x": 6.77147442693619, - "y": 1.7462193726308344, - "heading": -0.17691445924041233, - "angularVelocity": 0.44549247017807186, - "velocityX": 3.238645803522406, - "velocityY": 0.9417103332945568, - "timestamp": 1.7438996840229088 - }, - { - "x": 6.983655507583721, - "y": 1.8213558866056687, - "heading": -0.14784470725521442, - "angularVelocity": 0.40921704154417726, - "velocityX": 2.986888712997661, - "velocityY": 1.0577022458389254, - "timestamp": 1.8149371752170769 - }, - { - "x": 7.176562959829929, - "y": 1.900519098632156, - "heading": -0.12149718500073287, - "angularVelocity": 0.37089601295836144, - "velocityX": 2.7155724252554383, - "velocityY": 1.114386371136183, - "timestamp": 1.885974666411245 - }, - { - "x": 7.34978445026805, - "y": 1.9800560342439697, - "heading": -0.09790193374425028, - "angularVelocity": 0.33215209123854533, - "velocityX": 2.4384516897514468, - "velocityY": 1.119647305595492, - "timestamp": 1.957012157605413 - }, - { - "x": 7.503478866038821, - "y": 2.0570663546440358, - "heading": -0.07702399456491762, - "angularVelocity": 0.2939002888244837, - "velocityX": 2.163567620239804, - "velocityY": 1.0840799570127342, - "timestamp": 2.028049648799581 - }, - { - "x": 7.6380640040828975, - "y": 2.1293280640018195, - "heading": -0.058799951231258585, - "angularVelocity": 0.2565412013755806, - "velocityX": 1.8945649090592562, - "velocityY": 1.0172334093312752, - "timestamp": 2.099087139993749 - }, - { - "x": 7.754041712551309, - "y": 2.1951475174693176, - "heading": -0.04315811189523446, - "angularVelocity": 0.220191325356213, - "velocityX": 1.6326267512940102, - "velocityY": 0.9265452982790828, - "timestamp": 2.170124631187917 - }, - { - "x": 7.851915110788357, - "y": 2.2532212040315036, - "heading": -0.030028067927435956, - "angularVelocity": 0.18483259680314387, - "velocityX": 1.377771041625462, - "velocityY": 0.8175075665813204, - "timestamp": 2.241162122382085 - }, - { - "x": 7.932154394774834, - "y": 2.302530059896823, - "heading": -0.01934465130254991, - "angularVelocity": 0.15039124334620582, - "velocityX": 1.1295343154385586, - "velocityY": 0.6941243987705448, - "timestamp": 2.312199613576253 - }, - { - "x": 7.995185674411745, - "y": 2.3422635628439994, - "heading": -0.011049205592550179, - "angularVelocity": 0.1167756007503934, - "velocityX": 0.887295969738388, - "velocityY": 0.5593314499039962, - "timestamp": 2.383237104770421 - }, - { - "x": 8.04138998965963, - "y": 2.3717661278123887, - "heading": -0.005089649909509545, - "angularVelocity": 0.08389310465302457, - "velocityX": 0.6504215516506965, - "velocityY": 0.41530978181295497, - "timestamp": 2.454274595964589 - }, - { - "x": 8.071106442714182, - "y": 2.39049915084481, - "heading": -0.0014200461836977572, - "angularVelocity": 0.051657282149528694, - "velocityX": 0.418320700168416, - "velocityY": 0.26370614611400395, - "timestamp": 2.525312087158757 - }, - { - "x": 8.084636688232422, - "y": 2.3980138301849365, - "heading": -1.2697794666383574e-33, - "angularVelocity": 0.01999009480523916, - "velocityX": 0.19046626352917562, - "velocityY": 0.10578469500825122, - "timestamp": 2.5963495783529247 - }, - { - "x": 8.081561182876898, - "y": 2.39334444963861, - "heading": -0.0009066648087102295, - "angularVelocity": -0.012314301117252947, - "velocityX": -0.041771444829232424, - "velocityY": -0.06341942195849094, - "timestamp": 2.6699765588918325 - }, - { - "x": 8.061093542733351, - "y": 2.376630216741082, - "heading": -0.004227711790797553, - "angularVelocity": -0.045106385699632864, - "velocityX": -0.2779910298335672, - "velocityY": -0.22701233671663687, - "timestamp": 2.7436035394307403 - }, - { - "x": 8.02290518913695, - "y": 2.348360537804544, - "heading": -0.01000387000903876, - "angularVelocity": -0.07845165150007544, - "velocityX": -0.5186733629015444, - "velocityY": -0.38395814590820065, - "timestamp": 2.817230519969648 - }, - { - "x": 7.966627275516003, - "y": 2.3091224913101702, - "heading": -0.01828119946614907, - "angularVelocity": -0.11242250322537992, - "velocityX": -0.7643653618418706, - "velocityY": -0.5329302683224771, - "timestamp": 2.890857500508556 - }, - { - "x": 7.891846094053721, - "y": 2.2596302081809454, - "heading": -0.029111517424954568, - "angularVelocity": -0.1470971358533753, - "velocityX": -1.0156763310803423, - "velocityY": -0.6722030805415261, - "timestamp": 2.9644844810474638 - }, - { - "x": 7.798100115514375, - "y": 2.2007657686480786, - "heading": -0.04255255372711989, - "angularVelocity": -0.18255585389737042, - "velocityX": -1.27325578005752, - "velocityY": -0.7994954988241099, - "timestamp": 3.0381114615863716 - }, - { - "x": 7.684881725302932, - "y": 2.1336367022021943, - "heading": -0.058667438591618416, - "angularVelocity": -0.21887200516097166, - "velocityX": -1.5377296390908446, - "velocityY": -0.9117454763802326, - "timestamp": 3.1117384421252794 - }, - { - "x": 7.5516502833969605, - "y": 2.0596568999010922, - "heading": -0.07752270936377831, - "angularVelocity": -0.25609186515799065, - "velocityX": -1.8095464587953205, - "velocityY": -1.0047920172688334, - "timestamp": 3.185365422664187 - }, - { - "x": 7.397870326337049, - "y": 1.98065825109379, - "heading": -0.09918321696192219, - "angularVelocity": -0.2941925288746233, - "velocityX": -2.0886359312079446, - "velocityY": -1.0729578780642273, - "timestamp": 3.258992403203095 - }, - { - "x": 7.2231018276798915, - "y": 1.8990346142555161, - "heading": -0.12370090160049274, - "angularVelocity": -0.33299864342004915, - "velocityX": -2.3737018329144046, - "velocityY": -1.1086104066856408, - "timestamp": 3.332619383742003 - }, - { - "x": 7.027186991114378, - "y": 1.8178941660655976, - "heading": -0.1510925803805667, - "angularVelocity": -0.37203316745549647, - "velocityX": -2.660910920582776, - "velocityY": -1.102047749289958, - "timestamp": 3.4062463642809107 - }, - { - "x": 6.8105769237451845, - "y": 1.741127234048135, - "heading": -0.18130204311682424, - "angularVelocity": -0.4103042460133704, - "velocityX": -2.941993081662873, - "velocityY": -1.0426467506282693, - "timestamp": 3.4798733448198185 - }, - { - "x": 6.574746676835865, - "y": 1.6731958946569394, - "heading": -0.21415137217886637, - "angularVelocity": -0.4461588513015718, - "velocityX": -3.2030411295312824, - "velocityY": -0.9226419295477921, - "timestamp": 3.5535003253587263 - }, - { - "x": 6.322411514317352, - "y": 1.6185000003009695, - "heading": -0.24931002488043913, - "angularVelocity": -0.47752403323118087, - "velocityX": -3.427210523527703, - "velocityY": -0.7428784116315408, - "timestamp": 3.627127305897634 - }, - { - "x": 6.057197180213997, - "y": 1.5805921161213812, - "heading": -0.28632018217818705, - "angularVelocity": -0.5026711271717876, - "velocityX": -3.602135143423469, - "velocityY": -0.5148640335665581, - "timestamp": 3.700754286436542 - }, - { - "x": 5.782918806315175, - "y": 1.5618062942255864, - "heading": -0.3246756429318106, - "angularVelocity": -0.5209430085667437, - "velocityX": -3.725242728837456, - "velocityY": -0.25514861207526496, - "timestamp": 3.7743812669754497 + "heading": 0.3015228161300743, + "angularVelocity": 0.0000012088972110609183, + "velocityX": 3.7670067261218194, + "velocityY": -0.33592432468507777, + "timestamp": 1.3445192125367915 + }, + { + "x": 5.642161233224445, + "y": 1.5569512204704454, + "heading": 0.30152289610635313, + "angularVelocity": 0.0000021712102282875206, + "velocityX": 3.7778762267369546, + "velocityY": -0.17558800471026415, + "timestamp": 1.38135409743325 + }, + { + "x": 5.781468008961295, + "y": 1.556401100893367, + "heading": 0.30152296986307525, + "angularVelocity": 0.0000020023605969229892, + "velocityX": 3.781925099764402, + "velocityY": -0.014934744023907952, + "timestamp": 1.4181889823297087 + }, + { + "x": 5.920672391336217, + "y": 1.5617696165699877, + "heading": 0.3015230386365787, + "angularVelocity": 0.0000018670752912406278, + "velocityX": 3.779145306581512, + "velocityY": 0.14574541746802047, + "timestamp": 1.4550238672261673 + }, + { + "x": 6.0595230325638285, + "y": 1.5730470711005764, + "heading": 0.3015231034083751, + "angularVelocity": 0.0000017584362380693717, + "velocityX": 3.769541879061509, + "velocityY": 0.30616233937716997, + "timestamp": 1.4918587521226259 + }, + { + "x": 6.197769224123459, + "y": 1.5902130982406153, + "heading": 0.3015231649742209, + "angularVelocity": 0.000001671400517937078, + "velocityX": 3.753132172076425, + "velocityY": 0.46602635486148125, + "timestamp": 1.5286936370190845 + }, + { + "x": 6.335161349542959, + "y": 1.6132366986444322, + "heading": 0.3015232239953199, + "angularVelocity": 0.0000016023152822250254, + "velocityX": 3.72994583275345, + "velocityY": 0.6250487946015117, + "timestamp": 1.565528521915543 + }, + { + "x": 6.471451335210211, + "y": 1.6420762959087598, + "heading": 0.30152328103508963, + "angularVelocity": 0.000001548525804202316, + "velocityX": 3.7000247469309935, + "velocityY": 0.7829425107583317, + "timestamp": 1.6023634068120016 + }, + { + "x": 6.606393098338155, + "y": 1.6766798119201127, + "heading": 0.301523336612691, + "angularVelocity": 0.000001508830595721816, + "velocityX": 3.663422961881362, + "velocityY": 0.9394224010369989, + "timestamp": 1.6391982917084602 + }, + { + "x": 6.739742463422568, + "y": 1.716984614087568, + "heading": 0.3015255656708468, + "angularVelocity": 0.00006051486687697733, + "velocityX": 3.6201922568579414, + "velocityY": 1.094201930608725, + "timestamp": 1.6760331766049188 + }, + { + "x": 6.8694207904091344, + "y": 1.7614703171513673, + "heading": 0.3101205256410531, + "angularVelocity": 0.23333750042565204, + "velocityX": 3.520530262306672, + "velocityY": 1.207705771006116, + "timestamp": 1.7128680615013774 + }, + { + "x": 6.994764102779472, + "y": 1.808693107878132, + "heading": 0.3306180633638746, + "angularVelocity": 0.5564707961064413, + "velocityX": 3.4028425152588873, + "velocityY": 1.282012713206686, + "timestamp": 1.749702946397836 + }, + { + "x": 7.114597005425528, + "y": 1.856512920318317, + "heading": 0.35962771339750194, + "angularVelocity": 0.7875591335542964, + "velocityX": 3.253244932973218, + "velocityY": 1.2982207647615676, + "timestamp": 1.7865378312942946 + }, + { + "x": 7.228563598795877, + "y": 1.9039637614742513, + "heading": 0.39146295222083616, + "angularVelocity": 0.8642687200685354, + "velocityX": 3.093985326429113, + "velocityY": 1.2882038667778264, + "timestamp": 1.8233727161907531 + }, + { + "x": 7.336676614220082, + "y": 1.9505339221399116, + "heading": 0.42341393247502157, + "angularVelocity": 0.8674108889982484, + "velocityX": 2.935071352282109, + "velocityY": 1.2642949963483496, + "timestamp": 1.8602076010872117 + }, + { + "x": 7.438996464546078, + "y": 1.995939510716552, + "heading": 0.45395278545537515, + "angularVelocity": 0.8290742068611583, + "velocityX": 2.77779747686502, + "velocityY": 1.2326789863542051, + "timestamp": 1.8970424859836703 + }, + { + "x": 7.535582073954796, + "y": 2.0400031624519186, + "heading": 0.48208291467910164, + "angularVelocity": 0.7636817463336474, + "velocityX": 2.6221232855814667, + "velocityY": 1.196247846551653, + "timestamp": 1.933877370880129 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6791276743107281, + "velocityX": 2.4678193928677437, + "velocityY": 1.1565397180800703, + "timestamp": 1.9707122557765875 + }, + { + "x": 7.766333855095899, + "y": 2.1513911601167894, + "heading": 0.5393931714770351, + "angularVelocity": 0.5089507306836832, + "velocityX": 2.2039777611896034, + "velocityY": 1.0840548028640598, + "timestamp": 2.0341656809073783 + }, + { + "x": 7.8897168213730975, + "y": 2.214535022901093, + "heading": 0.5621690879226864, + "angularVelocity": 0.3589391179231937, + "velocityX": 1.9444650312080196, + "velocityY": 0.9951214241650669, + "timestamp": 2.0976191060381684 + }, + { + "x": 7.996856912991746, + "y": 2.271342778200504, + "heading": 0.5763089063837279, + "angularVelocity": 0.2228377496076287, + "velocityX": 1.6884839769927567, + "velocityY": 0.8952669644281369, + "timestamp": 2.1610725311689594 + }, + { + "x": 8.08793407853681, + "y": 2.3213235528188796, + "heading": 0.5824533246990756, + "angularVelocity": 0.09683351690917834, + "velocityX": 1.4353388387992245, + "velocityY": 0.7876765441007412, + "timestamp": 2.2245259562997495 + }, + { + "x": 8.16309393881241, + "y": 2.364112462024999, + "heading": 0.5810872026138336, + "angularVelocity": -0.021529524725041504, + "velocityX": 1.1844886248564517, + "velocityY": 0.6743356898059124, + "timestamp": 2.2879793814305405 + }, + { + "x": 8.222455939995415, + "y": 2.399428067130867, + "heading": 0.5725898810376202, + "angularVelocity": -0.13391430893917436, + "velocityX": 0.9355208337555891, + "velocityY": 0.5565594770191685, + "timestamp": 2.3514328065613306 + }, + { + "x": 8.266119462815807, + "y": 2.4270468912932266, + "heading": 0.5572661381126698, + "angularVelocity": -0.24149591441856846, + "velocityX": 0.6881192422693254, + "velocityY": 0.43526136068197446, + "timestamp": 2.4148862316921216 + }, + { + "x": 8.294168308263885, + "y": 2.4467872814650136, + "heading": 0.5353660980042331, + "angularVelocity": -0.3451356654632318, + "velocityX": 0.4420383200790668, + "velocityY": 0.3111004666981787, + "timestamp": 2.4783396568229117 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.44548570157766576, + "velocityX": 0.19708463824313602, + "velocityY": 0.18456741878341876, + "timestamp": 2.5417930819537027 + }, + { + "x": 8.303024530407173, + "y": 2.461881127582199, + "heading": 0.47119103232326837, + "angularVelocity": -0.5465136547157349, + "velocityX": -0.05554517814981655, + "velocityY": 0.05148048067312068, + "timestamp": 2.607495872285164 + }, + { + "x": 8.282750419132642, + "y": 2.456558519757899, + "heading": 0.4288756816904664, + "angularVelocity": -0.6440419108431604, + "velocityX": -0.3085730632177365, + "velocityY": -0.08101037714605593, + "timestamp": 2.6731986626166258 + }, + { + "x": 8.245821395629305, + "y": 2.442576926285189, + "heading": 0.3804195474569323, + "angularVelocity": -0.7375049672788585, + "velocityX": -0.5620617224479442, + "velocityY": -0.21280060408660287, + "timestamp": 2.7389014529480873 + }, + { + "x": 8.19220201387014, + "y": 2.4199912479232255, + "heading": 0.32613641295591117, + "angularVelocity": -0.8261922245184762, + "velocityX": -0.8160898721144456, + "velocityY": -0.34375523852216117, + "timestamp": 2.804604243279549 + }, + { + "x": 8.121850214183777, + "y": 2.388868090056124, + "heading": 0.2664003971864745, + "angularVelocity": -0.909185370485436, + "velocityX": -1.070758172239708, + "velocityY": -0.4736961354318384, + "timestamp": 2.8703070336110104 + }, + { + "x": 8.034715295558529, + "y": 2.349289919990928, + "heading": 0.20166644793914118, + "angularVelocity": -0.9852541866298153, + "velocityX": -1.3261981444876794, + "velocityY": -0.6023818754961501, + "timestamp": 2.936009823942472 + }, + { + "x": 7.9307349853362235, + "y": 2.301361396168203, + "heading": 0.1325025874209066, + "angularVelocity": -1.052677674255729, + "velocityX": -1.5825859099398853, + "velocityY": -0.7294747084702512, + "timestamp": 3.0017126142739334 + }, + { + "x": 7.809831081747403, + "y": 2.2452194829459002, + "heading": 0.0596429970770519, + "angularVelocity": -1.1089268808263426, + "velocityX": -1.8401639105261376, + "velocityY": -0.8544829365552749, + "timestamp": 3.067415404605395 + }, + { + "x": 7.671902780233127, + "y": 2.1810506699963903, + "heading": -0.015920256793080897, + "angularVelocity": -1.1500767850029918, + "velocityX": -2.099276161917122, + "velocityY": -0.9766527818040505, + "timestamp": 3.1331181949368565 + }, + { + "x": 7.516816185598606, + "y": 2.109122861945235, + "heading": -0.09276870443286889, + "angularVelocity": -1.1696375032490751, + "velocityX": -2.3604263053689314, + "velocityY": -1.0947451042534069, + "timestamp": 3.198820985268318 + }, + { + "x": 7.344387868807497, + "y": 2.02985165560778, + "heading": -0.16871929267657412, + "angularVelocity": -1.1559720349858067, + "velocityX": -2.6243682486121434, + "velocityY": -1.206512020837212, + "timestamp": 3.2645237755997796 + }, + { + "x": 7.154363408340053, + "y": 1.9439629823466762, + "heading": -0.2400485240661188, + "angularVelocity": -1.0856347352935642, + "velocityX": -2.8921825010596516, + "velocityY": -1.3072302230667405, + "timestamp": 3.330226565931241 + }, + { + "x": 6.9464441838510815, + "y": 1.8530162365161489, + "heading": -0.2992772419214471, + "angularVelocity": -0.9014642689682996, + "velocityX": -3.1645417712101533, + "velocityY": -1.3842143594162994, + "timestamp": 3.3959293562627026 + }, + { + "x": 6.72212326982476, + "y": 1.7626751908782685, + "heading": -0.31855321839260475, + "angularVelocity": -0.2933813978662556, + "velocityX": -3.4141763674670904, + "velocityY": -1.3749955699312393, + "timestamp": 3.461632146594164 + }, + { + "x": 6.485759745047901, + "y": 1.6860130581467425, + "heading": -0.3185532921212082, + "angularVelocity": -0.0000011221533066716862, + "velocityX": -3.597465550312819, + "velocityY": -1.1668017803319484, + "timestamp": 3.5273349369256257 + }, + { + "x": 6.24426876944993, + "y": 1.6274732096370486, + "heading": -0.3185533177664736, + "angularVelocity": -3.9032231775904613e-7, + "velocityX": -3.675505627381763, + "velocityY": -0.8909796405048926, + "timestamp": 3.5930377272570873 + }, + { + "x": 5.999037658914315, + "y": 1.5873919634602989, + "heading": -0.3185533462426243, + "angularVelocity": -4.334085446884588e-7, + "velocityX": -3.732430682143934, + "velocityY": -0.6100387209515062, + "timestamp": 3.658740517588549 + }, + { + "x": 5.751475205523387, + "y": 1.5659995878977724, + "heading": -0.3185533789943251, + "angularVelocity": -4.984826463431063e-7, + "velocityX": -3.7679138456983283, + "velocityY": -0.325593105781428, + "timestamp": 3.7244433079200103 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.3639012008110169, - "angularVelocity": -0.5327606482310914, - "velocityX": -3.8018020976391327, - "velocityY": 0.021903521993656888, - "timestamp": 3.8480082475143575 - }, - { - "x": 5.261378018303958, - "y": 1.580163952485418, - "heading": -0.3978140479254385, - "angularVelocity": -0.5381806191653729, - "velocityX": -3.83448205483159, - "velocityY": 0.265734612450363, - "timestamp": 3.9110221240954477 - }, - { - "x": 5.01887444525289, - "y": 1.6123863305278272, - "heading": -0.4318947982287382, - "angularVelocity": -0.5408451622468005, - "velocityX": -3.848415400042849, - "velocityY": 0.5113536857384613, - "timestamp": 3.974036000676538 - }, - { - "x": 4.777050990194419, - "y": 1.6600961195625756, - "heading": -0.4659121119973733, - "angularVelocity": -0.5398384548657204, - "velocityX": -3.8376222536837075, - "velocityY": 0.7571314704524225, - "timestamp": 4.037049877257628 - }, - { - "x": 4.5379789584596235, - "y": 1.7230618675308726, - "heading": -0.49955411376200026, - "angularVelocity": -0.5338824333610737, - "velocityX": -3.793958485114657, - "velocityY": 0.9992362219973634, - "timestamp": 4.100063753838718 - }, - { - "x": 4.3043600249129454, - "y": 1.8005379950644207, - "heading": -0.5324078746922887, - "angularVelocity": -0.5213734293590093, - "velocityX": -3.7074204321653204, - "velocityY": 1.2295089865459496, - "timestamp": 4.163077630419808 - }, - { - "x": 4.07944895912211, - "y": 1.8908360479698116, - "heading": -0.5639729512662652, - "angularVelocity": -0.5009226266750401, - "velocityX": -3.5692307471578024, - "velocityY": 1.4329867928247444, - "timestamp": 4.226091507000898 - }, - { - "x": 3.866497717212655, - "y": 1.9909883194289872, - "heading": -0.5937624584033184, - "angularVelocity": -0.47274519127097403, - "velocityX": -3.37943407807034, - "velocityY": 1.5893685152078068, - "timestamp": 4.289105383581989 - }, - { - "x": 3.667854490580529, - "y": 2.0970860288082087, - "heading": -0.6214571487340739, - "angularVelocity": -0.4395014532254029, - "velocityX": -3.1523727377175264, - "velocityY": 1.6837197635775993, - "timestamp": 4.352119260163079 - }, - { - "x": 3.4845656078038156, - "y": 2.20525192218672, - "heading": -0.6469466767255073, - "angularVelocity": -0.40450658449225635, - "velocityX": -2.9087066646478257, - "velocityY": 1.7165408517490015, - "timestamp": 4.415133136744169 - }, - { - "x": 3.316731609967858, - "y": 2.3123318355961353, - "heading": -0.6702493227736513, - "angularVelocity": -0.3698018168769004, - "velocityX": -2.6634450527730666, - "velocityY": 1.699306870473487, - "timestamp": 4.478147013325259 - }, - { - "x": 3.1640084855137136, - "y": 2.4159899224511383, - "heading": -0.6914332444345033, - "angularVelocity": -0.3361786769869806, - "velocityX": -2.423642739351382, - "velocityY": 1.6450041241568225, - "timestamp": 4.541160889906349 - }, - { - "x": 3.0259122517259884, - "y": 2.5145372691122856, - "heading": -0.7105770269066637, - "angularVelocity": -0.30380264650953404, - "velocityX": -2.19152099950578, - "velocityY": 1.5638991283821457, - "timestamp": 4.604174766487439 - }, - { - "x": 2.9019526205870343, - "y": 2.6067411641295175, - "heading": -0.7277548924092291, - "angularVelocity": -0.2726044870523041, - "velocityX": -1.9671798953589927, - "velocityY": 1.4632315931012068, - "timestamp": 4.667188643068529 - }, - { - "x": 2.79168051154695, - "y": 2.6916821542694485, - "heading": -0.743032506079513, - "angularVelocity": -0.24244840183135916, - "velocityX": -1.7499654841609167, - "velocityY": 1.3479727759745657, - "timestamp": 4.73020251964962 - }, - { - "x": 2.694699726347074, - "y": 2.76865749223089, - "heading": -0.7564666750477512, - "angularVelocity": -0.21319381852265873, - "velocityX": -1.5390385493118295, - "velocityY": 1.2215616962143954, - "timestamp": 4.79321639623071 - }, - { - "x": 2.6106653021864603, - "y": 2.8371173452263947, - "heading": -0.7681062788003478, - "angularVelocity": -0.18471492922067745, - "velocityX": -1.3335860086702125, - "velocityY": 1.0864250338162604, - "timestamp": 4.8562302728118 - }, - { - "x": 2.5392776642565695, - "y": 2.8966223272202614, - "heading": -0.7779934551732625, - "angularVelocity": -0.15690474716614497, - "velocityX": -1.1328875765645765, - "velocityY": 0.9443155257602959, - "timestamp": 4.91924414939289 - }, - { - "x": 2.480276064918479, - "y": 2.946814696080483, - "heading": -0.7861647133126454, - "angularVelocity": -0.1296739477512957, - "velocityX": -0.9363270844345449, - "velocityY": 0.7965288216418583, - "timestamp": 4.98225802597398 - }, - { - "x": 2.4334325341041674, - "y": 2.9873983829565494, - "heading": -0.7926518839069914, - "angularVelocity": -0.10294828609691896, - "velocityX": -0.7433843679499733, - "velocityY": 0.6440436468599187, - "timestamp": 5.04527190255507 - }, - { - "x": 2.398546680913597, - "y": 3.0181248328808925, - "heading": -0.7974829010951674, - "angularVelocity": -0.07666592582919579, - "velocityX": -0.5536217589418905, - "velocityY": 0.4876140239491929, - "timestamp": 5.10828577913616 - }, - { - "x": 2.3754413559178094, - "y": 3.038782749760636, - "heading": -0.8006824366316742, - "angularVelocity": -0.0507750944728719, - "velocityX": -0.36667042641082753, - "velocityY": 0.32783123338174003, - "timestamp": 5.171299655717251 + "heading": -0.3185534180019772, + "angularVelocity": -5.936985611348973e-7, + "velocityX": -3.781751231726404, + "velocityY": -0.039276923728911305, + "timestamp": 3.790146098251472 + }, + { + "x": 5.26265149067387, + "y": 1.5786031053003822, + "heading": -0.31855345112044986, + "angularVelocity": -5.20084412877405e-7, + "velocityX": -3.7744308273915435, + "velocityY": 0.23844772894968475, + "timestamp": 3.853825131614962 + }, + { + "x": 5.0240625547753925, + "y": 1.61139052009726, + "heading": -0.3185534786660252, + "angularVelocity": -4.3256899255575315e-7, + "velocityX": -3.7467424251962935, + "velocityY": 0.5148855606793222, + "timestamp": 3.9175041649784523 + }, + { + "x": 4.788524290519247, + "y": 1.661604289022676, + "heading": -0.31855350247111613, + "angularVelocity": -3.7382933947955595e-7, + "velocityX": -3.698835422197071, + "velocityY": 0.7885447732660761, + "timestamp": 3.9811831983419426 + }, + { + "x": 4.55730773239592, + "y": 1.7289734284079028, + "heading": -0.3185535237219762, + "angularVelocity": -3.337183198598428e-7, + "velocityX": -3.6309684037367047, + "velocityY": 1.0579485244487485, + "timestamp": 4.044862231705433 + }, + { + "x": 4.33166058516164, + "y": 1.8131343663413284, + "heading": -0.3185535432404584, + "angularVelocity": -3.0651348259884106e-7, + "velocityX": -3.543507734268667, + "velocityY": 1.321642831055895, + "timestamp": 4.108541265068923 + }, + { + "x": 4.112800476187319, + "y": 1.9136328827222209, + "heading": -0.318553561635836, + "angularVelocity": -2.888765193967595e-7, + "velocityX": -3.436925741712075, + "velocityY": 1.5782041760469432, + "timestamp": 4.172220298432413 + }, + { + "x": 3.9019083093617053, + "y": 2.0299264483320134, + "heading": -0.31855357939397577, + "angularVelocity": -2.7886949297236075e-7, + "velocityX": -3.311799122668965, + "velocityY": 1.826245774585971, + "timestamp": 4.2358993317959035 + }, + { + "x": 3.7001203074329077, + "y": 2.161384813738153, + "heading": -0.3185535969362317, + "angularVelocity": -2.75479306959021e-7, + "velocityX": -3.1688295388681516, + "velocityY": 2.0643900898393674, + "timestamp": 4.299578365159394 + }, + { + "x": 3.509285404964888, + "y": 2.2879634464070207, + "heading": -0.3579155843505842, + "angularVelocity": -0.6181310446354924, + "velocityX": -2.996824737880426, + "velocityY": 1.9877599577609337, + "timestamp": 4.363257398522884 + }, + { + "x": 3.332909755538667, + "y": 2.4050971261724468, + "heading": -0.4129449232759771, + "angularVelocity": -0.8641673093760197, + "velocityX": -2.769760156682042, + "velocityY": 1.8394387222684185, + "timestamp": 4.426936431886374 + }, + { + "x": 3.171287726021972, + "y": 2.512479984884094, + "heading": -0.4699204658498491, + "angularVelocity": -0.8947300165290918, + "velocityX": -2.5380729100288124, + "velocityY": 1.6863142079856834, + "timestamp": 4.490615465249864 + }, + { + "x": 3.024408806786741, + "y": 2.6100923699912792, + "heading": -0.5252799185719804, + "angularVelocity": -0.8693513358805374, + "velocityX": -2.306550704009953, + "velocityY": 1.5328810748429236, + "timestamp": 4.554294498613355 + }, + { + "x": 2.892255079193046, + "y": 2.6979336450739244, + "heading": -0.5773440254447586, + "angularVelocity": -0.8176020288434336, + "velocityX": -2.0753098879397367, + "velocityY": 1.3794379475145813, + "timestamp": 4.617973531976845 + }, + { + "x": 2.7748123815012584, + "y": 2.7760062309054065, + "heading": -0.625128837055519, + "angularVelocity": -0.7504010203483636, + "velocityX": -1.8442914643726742, + "velocityY": 1.2260328354205916, + "timestamp": 4.681652565340335 + }, + { + "x": 2.6720699818921556, + "y": 2.844312916696855, + "heading": -0.6679860381773631, + "angularVelocity": -0.6730190277419643, + "velocityX": -1.6134415706757543, + "velocityY": 1.0726715244175122, + "timestamp": 4.745331598703825 + }, + { + "x": 2.5840196243973645, + "y": 2.9028562717726505, + "heading": -0.7054558016839698, + "angularVelocity": -0.5884160221579265, + "velocityX": -1.382721326691407, + "velocityY": 0.9193505614575078, + "timestamp": 4.809010632067316 + }, + { + "x": 2.510654811151598, + "y": 2.95163853061359, + "heading": -0.7371951288142529, + "angularVelocity": -0.4984266477336408, + "velocityX": -1.1521031235978156, + "velocityY": 0.7660646882386306, + "timestamp": 4.872689665430806 + }, + { + "x": 2.4519703126340358, + "y": 2.9906615990271557, + "heading": -0.7629386573341134, + "angularVelocity": -0.4042700895428552, + "velocityX": -0.9215670436230066, + "velocityY": 0.6128087433553882, + "timestamp": 4.936368698794296 + }, + { + "x": 2.4079618327799786, + "y": 3.0199270886206313, + "heading": -0.7824753669479414, + "angularVelocity": -0.3067997201262361, + "velocityX": -0.6910984279998401, + "velocityY": 0.4595781067596215, + "timestamp": 5.000047732157786 + }, + { + "x": 2.378625774904473, + "y": 3.039436353320747, + "heading": -0.795633833561549, + "angularVelocity": -0.20663734856804464, + "velocityX": -0.4606862938394533, + "velocityY": 0.3063687318988399, + "timestamp": 5.0637267655212765 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.025232146222040765, - "velocityX": -0.18221830683035964, - "velocityY": 0.16516634183273946, - "timestamp": 5.234313532298341 + "angularVelocity": -0.10425061542851642, + "velocityX": -0.2303222914890624, + "velocityY": 0.15317707264507022, + "timestamp": 5.127405798884767 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 5.951839521214632e-29, - "velocityX": -4.256198532236221e-31, - "velocityY": 3.167098439878756e-31, - "timestamp": 5.297327408879431 + "angularVelocity": -1.4092752497411054e-30, + "velocityX": 2.9428006004895336e-31, + "velocityY": 5.314710200495422e-31, + "timestamp": 5.191084832248257 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.3.traj b/src/main/deploy/choreo/SourceLanePGHSprint.3.traj index a86233fe..2bed8424 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.3.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.3.traj @@ -4,730 +4,775 @@ "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 5.951839521214632e-29, - "velocityX": -4.256198532236221e-31, - "velocityY": 3.167098439878756e-31, + "angularVelocity": -1.4092752497411054e-30, + "velocityX": 2.9428006004895336e-31, + "velocityY": 5.314710200495422e-31, "timestamp": 0 }, { - "x": 2.375813180458781, - "y": 3.0435390743709614, - "heading": -0.7990461525696796, - "angularVelocity": 0.05557590004806609, - "velocityX": 0.20420014353356836, - "velocityY": -0.09735244641805807, - "timestamp": 0.05805141090141763 - }, - { - "x": 2.3995218612588998, - "y": 3.03223611715748, - "heading": -0.7926240802250826, - "angularVelocity": 0.110627325759628, - "velocityX": 0.4084083475659286, - "velocityY": -0.19470598626234253, - "timestamp": 0.11610282180283527 - }, - { - "x": 2.435085642006342, - "y": 3.015281569795486, - "heading": -0.7830411709963286, - "angularVelocity": 0.1650762501712104, - "velocityX": 0.6126256053937512, - "velocityY": -0.2920609008243783, - "timestamp": 0.1741542327042529 - }, - { - "x": 2.4825051165422036, - "y": 2.9926753368780465, - "heading": -0.7703376648494888, - "angularVelocity": 0.21883199649381424, - "velocityX": 0.8168530927937191, - "velocityY": -0.3894174588767385, - "timestamp": 0.23220564360567053 - }, - { - "x": 2.541780960274715, - "y": 2.964417308476329, - "heading": -0.7545600110272952, - "angularVelocity": 0.2717876030435742, - "velocityX": 1.0210922148502688, - "velocityY": -0.4867759105752865, - "timestamp": 0.29025705450708816 - }, - { - "x": 2.612913947196807, - "y": 2.930507361776858, - "heading": -0.7357621134740673, - "angularVelocity": 0.3238146543096182, - "velocityX": 1.225344669794324, - "velocityY": -0.5841364778722867, - "timestamp": 0.3483084654085058 - }, - { - "x": 2.6959049720092736, - "y": 2.8909453636117948, - "heading": -0.7140070097413544, - "angularVelocity": 0.37475581376751455, - "velocityX": 1.4296125369528379, - "velocityY": -0.6814993391331685, - "timestamp": 0.40635987630992343 - }, - { - "x": 2.7907550793730915, - "y": 2.84573117446325, - "heading": -0.6893691896527133, - "angularVelocity": 0.4244138033179133, - "velocityX": 1.6338983995564174, - "velocityY": -0.778864603744556, - "timestamp": 0.46441128721134106 - }, - { - "x": 2.8974655032774796, - "y": 2.7948646549900524, - "heading": -0.6619378845471044, - "angularVelocity": 0.47253468399231374, - "velocityX": 1.8382055189942428, - "velocityY": -0.876232268662314, - "timestamp": 0.5224626981127587 - }, - { - "x": 3.0160377210166036, - "y": 2.7383456770648533, - "heading": -0.6318218809717984, - "angularVelocity": 0.5187815956178783, - "velocityX": 2.042538086464129, - "velocityY": -0.9736021407159139, - "timestamp": 0.5805141090141763 - }, - { - "x": 3.1464735286190364, - "y": 2.676174143314885, - "heading": -0.5991568348591108, - "angularVelocity": 0.5626916832074861, - "velocityX": 2.2469015925200937, - "velocityY": -1.0709736901235276, - "timestamp": 0.638565519915594 - }, - { - "x": 3.288775148081632, - "y": 2.6083500237720547, - "heading": -0.5641169164360986, - "angularVelocity": 0.6036014952766117, - "velocityX": 2.4513033749386723, - "velocityY": -1.1683457557648163, - "timestamp": 0.6966169308170116 - }, - { - "x": 3.4429453809853263, - "y": 2.534873429900399, - "heading": -0.5269344943074844, - "angularVelocity": 0.6405085001595859, - "velocityX": 2.6557534176991067, - "velocityY": -1.2657159013143156, - "timestamp": 0.7546683417184292 - }, - { - "x": 3.608987822216531, - "y": 2.4557447796300744, - "heading": -0.48793616320693267, - "angularVelocity": 0.6717895481778894, - "velocityX": 2.860265386368935, - "velocityY": -1.3630788475529159, - "timestamp": 0.8127197526198469 - }, - { - "x": 3.786907101523248, - "y": 2.370965219866687, - "heading": -0.44761641247502054, - "angularVelocity": 0.6945524683350642, - "velocityX": 3.064857107587936, - "velocityY": -1.4604220370691692, - "timestamp": 0.8707711635212645 - }, - { - "x": 3.9767087353801904, - "y": 2.2805379602303306, - "heading": -0.4068154658739133, - "angularVelocity": 0.7028416013935566, - "velocityX": 3.2695438562081205, - "velocityY": -1.5577099373160743, - "timestamp": 0.9288225744226821 - }, - { - "x": 4.178394426915559, - "y": 2.1844743331965764, - "heading": -0.36728880481186277, - "angularVelocity": 0.6808906183034006, - "velocityX": 3.4742599431023264, - "velocityY": -1.6548026230902309, - "timestamp": 0.9868739853240998 - }, - { - "x": 4.39186585424446, - "y": 2.082861864950712, - "heading": -0.33521197370242245, - "angularVelocity": 0.5525590267549734, - "velocityX": 3.6772823263747743, - "velocityY": -1.7503875731534284, - "timestamp": 1.0449253962255174 - }, - { - "x": 4.613290428731911, - "y": 1.9772010264517206, - "heading": -0.3352119609115161, - "angularVelocity": 2.203375614401519e-7, - "velocityX": 3.81428411556565, - "velocityY": -1.8201252451628447, - "timestamp": 1.102976807126935 - }, - { - "x": 4.83471490814338, - "y": 1.8715399887082398, - "heading": -0.33521194812268507, - "angularVelocity": 2.2030181304603152e-7, - "velocityX": 3.81428247777632, - "velocityY": -1.8201286773703715, - "timestamp": 1.1610282180283527 - }, - { - "x": 5.056139387672164, - "y": 1.7658789512106026, - "heading": -0.33521193533385407, - "angularVelocity": 2.2030181241344307e-7, - "velocityX": 3.8142824797971753, - "velocityY": -1.8201286731354431, - "timestamp": 1.2190796289297694 - }, - { - "x": 5.277564966924424, - "y": 1.6602202183369479, - "heading": -0.33521192254501997, - "angularVelocity": 2.203018646355495e-7, - "velocityX": 3.814301423754948, - "velocityY": -1.8200889734288124, - "timestamp": 1.2771310398311861 + "x": 2.3794842314620843, + "y": 3.041727640405621, + "heading": -0.7962432666233688, + "angularVelocity": 0.0957711014436812, + "velocityX": 0.24661230413774035, + "velocityY": -0.11854554422657079, + "timestamp": 0.06295370174647719 + }, + { + "x": 2.410536447869941, + "y": 3.0268006675090624, + "heading": -0.7842917338243374, + "angularVelocity": 0.18984638658997072, + "velocityX": 0.4932548134009415, + "velocityY": -0.23711032842312177, + "timestamp": 0.12590740349295437 + }, + { + "x": 2.4571179241280796, + "y": 3.0044081651489933, + "heading": -0.7665449064601504, + "angularVelocity": 0.28190284084732253, + "velocityX": 0.7399322830248891, + "velocityY": -0.3556979452971155, + "timestamp": 0.18886110523943156 + }, + { + "x": 2.5192312369840457, + "y": 2.9745484345249116, + "heading": -0.743155503226628, + "angularVelocity": 0.37153340605314533, + "velocityX": 0.9866506834833091, + "velocityY": -0.4743125470894598, + "timestamp": 0.25181480698590875 + }, + { + "x": 2.596879439737626, + "y": 2.937219467270397, + "heading": -0.714309410311632, + "angularVelocity": 0.4582112268975565, + "velocityX": 1.233417584660547, + "velocityY": -0.5929590511586358, + "timestamp": 0.31476850873238593 + }, + { + "x": 2.690066196626848, + "y": 2.892418878795792, + "heading": -0.6802368283616581, + "angularVelocity": 0.5412323819683986, + "velocityX": 1.4802426911208155, + "velocityY": -0.7116434336939077, + "timestamp": 0.3777222104788631 + }, + { + "x": 2.798795963694134, + "y": 2.8401438158547943, + "heading": -0.6412294413317386, + "angularVelocity": 0.6196202280051365, + "velocityX": 1.7271385804309867, + "velocityY": -0.8303731391605256, + "timestamp": 0.4406759122253403 + }, + { + "x": 2.9230742308720847, + "y": 2.7803908299690017, + "heading": -0.5976682545562845, + "angularVelocity": 0.6919559226378926, + "velocityX": 1.9741216756154527, + "velocityY": -0.9491576226355342, + "timestamp": 0.5036296139718175 + }, + { + "x": 3.062907829622796, + "y": 2.713155712528932, + "heading": -0.5500718966234338, + "angularVelocity": 0.7560533632244142, + "velocityX": 2.22121328645359, + "velocityY": -1.0680089585650514, + "timestamp": 0.5665833157182947 + }, + { + "x": 3.218305230273769, + "y": 2.638433320090989, + "heading": -0.4991885737635272, + "angularVelocity": 0.8082657802208455, + "velocityX": 2.4684394458133507, + "velocityY": -1.1869419964986359, + "timestamp": 0.6295370174647719 + }, + { + "x": 3.389276272924326, + "y": 2.556217612467452, + "heading": -0.4461960441931904, + "angularVelocity": 0.8417698737358584, + "velocityX": 2.715821912094713, + "velocityY": -1.3059709809381974, + "timestamp": 0.692490719211249 + }, + { + "x": 3.575827436112715, + "y": 2.4665034787202997, + "heading": -0.3932374103043789, + "angularVelocity": 0.841231451362194, + "velocityX": 2.9633072879440143, + "velocityY": -1.4250811510408536, + "timestamp": 0.7554444209577262 + }, + { + "x": 3.777911234478158, + "y": 2.369308059755886, + "heading": -0.34555593776982274, + "angularVelocity": 0.7574053822375035, + "velocityX": 3.210038373585424, + "velocityY": -1.5439190431697394, + "timestamp": 0.8183981227042034 + }, + { + "x": 3.9926293770053456, + "y": 2.266439040708753, + "heading": -0.3455557491739834, + "angularVelocity": 0.000002995786333953476, + "velocityX": 3.4107310066036556, + "velocityY": -1.6340424183696203, + "timestamp": 0.8813518244506806 + }, + { + "x": 4.207200483864901, + "y": 2.1632635581376745, + "heading": -0.34555573252846533, + "angularVelocity": 2.644088848166298e-7, + "velocityX": 3.4083953906898548, + "velocityY": -1.6389104962656516, + "timestamp": 0.9443055261971578 + }, + { + "x": 4.421771581956281, + "y": 2.0600880573316713, + "heading": -0.3455557158829472, + "angularVelocity": 2.6440888572481287e-7, + "velocityX": 3.408395251410094, + "velocityY": -1.638910785921765, + "timestamp": 1.007259227943635 + }, + { + "x": 4.636342680047139, + "y": 1.9569125565245837, + "heading": -0.3455556992374292, + "angularVelocity": 2.6440888404451857e-7, + "velocityX": 3.4083952514018088, + "velocityY": -1.6389107859389946, + "timestamp": 1.0702129296901122 + }, + { + "x": 4.850913778140846, + "y": 1.853737055723424, + "heading": -0.3455556825919111, + "angularVelocity": 2.64408884846605e-7, + "velocityX": 3.408395251447087, + "velocityY": -1.63891078584483, + "timestamp": 1.1331666314365894 + }, + { + "x": 5.065484924140348, + "y": 1.7505616545506137, + "heading": -0.34555566594639303, + "angularVelocity": 2.644088848456774e-7, + "velocityX": 3.4083960124157384, + "velocityY": -1.6389092032794423, + "timestamp": 1.1961203331830665 + }, + { + "x": 5.280845302209934, + "y": 1.6490439336616942, + "heading": -0.34555564929194366, + "angularVelocity": 2.6455075541297124e-7, + "velocityX": 3.4209327187283005, + "velocityY": -1.6125774668143436, + "timestamp": 1.2590740349295437 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.3352119097112988, - "angularVelocity": 2.2107509356850104e-7, - "velocityX": 3.8834306838456705, - "velocityY": -1.6675087206439847, - "timestamp": 1.3351824507326029 - }, - { - "x": 5.771247738907838, - "y": 1.4675302215572488, - "heading": -0.3352118975519742, - "angularVelocity": 1.8039593836487576e-7, - "velocityX": 3.9796744633245575, - "velocityY": -1.4226072506653686, - "timestamp": 1.402585989419089 - }, - { - "x": 6.040462534260166, - "y": 1.3811079630666692, - "heading": -0.3269077113367565, - "angularVelocity": 0.1232010422159426, - "velocityX": 3.994075097518657, - "velocityY": -1.2821620374051248, - "timestamp": 1.4699895281055753 - }, - { - "x": 6.293431004629783, - "y": 1.2999254087750478, - "heading": -0.29863774275478366, - "angularVelocity": 0.4194137152570709, - "velocityX": 3.7530443549299446, - "velocityY": -1.2044257003957306, - "timestamp": 1.5373930667920614 - }, - { - "x": 6.529552502522878, - "y": 1.2241576590494234, - "heading": -0.2655052058197796, - "angularVelocity": 0.4915548587013119, - "velocityX": 3.5031023963202834, - "velocityY": -1.1240915714832609, - "timestamp": 1.6047966054785476 - }, - { - "x": 6.748788082038609, - "y": 1.153810716509259, - "heading": -0.23113245426293594, - "angularVelocity": 0.5099547030716253, - "velocityX": 3.252582635690123, - "velocityY": -1.0436683876104664, - "timestamp": 1.6722001441650338 - }, - { - "x": 6.951134609386156, - "y": 1.0888826430758505, - "heading": -0.19719131465702286, - "angularVelocity": 0.5035513011235752, - "velocityX": 3.0020163820882195, - "velocityY": -0.9632739571049619, - "timestamp": 1.73960368285152 - }, - { - "x": 7.136594604940876, - "y": 1.0293710541923635, - "heading": -0.1646509365883203, - "angularVelocity": 0.48276958009664506, - "velocityX": 2.7514875208162133, - "velocityY": -0.8829149039235648, - "timestamp": 1.8070072215380062 - }, - { - "x": 7.305171448299849, - "y": 0.9752739365513875, - "heading": -0.134145865535612, - "angularVelocity": 0.45257373199049533, - "velocityX": 2.501008799301714, - "velocityY": -0.8025857202037736, - "timestamp": 1.8744107602244924 - }, - { - "x": 7.4568683544045555, - "y": 0.9265896769068921, - "heading": -0.10612469685480012, - "angularVelocity": 0.4157225158629608, - "velocityX": 2.2505777747114233, - "velocityY": -0.7222804706284149, - "timestamp": 1.9418142989109786 - }, - { - "x": 7.591688163065539, - "y": 0.8833169839764341, - "heading": -0.08092177349469894, - "angularVelocity": 0.3739109822902337, - "velocityX": 2.0001888815967246, - "velocityY": -0.6419943785404568, - "timestamp": 2.0092178375974648 - }, - { - "x": 7.709633332729291, - "y": 0.8454548114841277, - "heading": -0.05879603455931429, - "angularVelocity": 0.3282578239445074, - "velocityX": 1.7498364620342992, - "velocityY": -0.5617238090186147, - "timestamp": 2.076621376283951 - }, - { - "x": 7.810705984005889, - "y": 0.8130022977681454, - "heading": -0.03995388423570108, - "angularVelocity": 0.27954244971105247, - "velocityX": 1.499515503877575, - "velocityY": -0.48146602312571224, - "timestamp": 2.144024914970437 - }, - { - "x": 7.894907949862419, - "y": 0.7859587206023052, - "heading": -0.024563521252436132, - "angularVelocity": 0.22833167639537438, - "velocityX": 1.2492217396504763, - "velocityY": -0.4012189521922299, - "timestamp": 2.2114284536569233 - }, - { - "x": 7.962240820715571, - "y": 0.7643234635620257, - "heading": -0.012764369724625984, - "angularVelocity": 0.17505240463252983, - "velocityX": 0.9989515708713329, - "velocityY": -0.3209810265438958, - "timestamp": 2.2788319923434095 - }, - { - "x": 8.012705982087093, - "y": 0.7480959907369016, - "heading": -0.004673541051565745, - "angularVelocity": 0.12003566623844747, - "velocityX": 0.7487019577154632, - "velocityY": -0.24075105167108718, - "timestamp": 2.3462355310298957 - }, - { - "x": 8.046304645291002, - "y": 0.7372758274108119, - "heading": -0.00039041667718953013, - "angularVelocity": 0.06354450312022167, - "velocityX": 0.49847031563414146, - "velocityY": -0.1605281196943911, - "timestamp": 2.413639069716382 - }, - { - "x": 8.063037872314453, - "y": 0.7318625450134277, - "heading": -3.5207342228472116e-29, - "angularVelocity": 0.005792228194527766, - "velocityX": 0.24825442921152802, - "velocityY": -0.08031154599409078, - "timestamp": 2.481042608402868 - }, - { - "x": 8.061895055441012, - "y": 0.7321772752029715, - "heading": -0.004013012422633091, - "angularVelocity": -0.05636038425014907, - "velocityX": -0.016050186575884747, - "velocityY": 0.004420199229323397, - "timestamp": 2.552245324211656 - }, - { - "x": 8.041931973498654, - "y": 0.7385255618667966, - "heading": -0.012367513802926035, - "angularVelocity": -0.11733402701567407, - "velocityX": -0.2803696701115773, - "velocityY": 0.0891579287631834, - "timestamp": 2.6234480400204436 - }, - { - "x": 8.003147425566949, - "y": 0.7509078775903022, - "heading": -0.024966290125142357, - "angularVelocity": -0.17694235646930861, - "velocityX": -0.5447060198638873, - "velocityY": 0.17390229547925942, - "timestamp": 2.6946507558292314 - }, - { - "x": 7.945540047254358, - "y": 0.7693247566912358, - "heading": -0.04169615417860038, - "angularVelocity": -0.23496103854227787, - "velocityX": -0.8090615316878346, - "velocityY": 0.25865416637184857, - "timestamp": 2.765853471638019 - }, - { - "x": 7.869108284660135, - "y": 0.7937768147652166, - "heading": -0.06242431663850855, - "angularVelocity": -0.2911147731440275, - "velocityX": -1.0734388671280197, - "velocityY": 0.34341468294054744, - "timestamp": 2.837056187446807 - }, - { - "x": 7.773850362284446, - "y": 0.8242647742315067, - "heading": -0.08699338329336499, - "angularVelocity": -0.34505799920387553, - "velocityX": -1.3378411384124769, - "velocityY": 0.4281853454601993, - "timestamp": 2.9082589032555948 - }, - { - "x": 7.65976424379608, - "y": 0.8607894982947099, - "heading": -0.11521425502187198, - "angularVelocity": -0.39634544002927347, - "velocityX": -1.6022720087635367, - "velocityY": 0.5129681311775954, - "timestamp": 2.9794616190643826 - }, - { - "x": 7.526847585445219, - "y": 0.9033520367557786, - "heading": -0.14685571507130035, - "angularVelocity": -0.44438557841539145, - "velocityX": -1.8667357956935873, - "velocityY": 0.59776566072801, - "timestamp": 3.0506643348731703 - }, - { - "x": 7.375097685187462, - "y": 0.9519536884245254, - "heading": -0.18162855174599865, - "angularVelocity": -0.48836390971499116, - "velocityX": -2.1312375312379404, - "velocityY": 0.6825814312935032, - "timestamp": 3.121867050681958 - }, - { - "x": 7.204511441301595, - "y": 1.0065960860089886, - "heading": -0.21916015731822874, - "angularVelocity": -0.5271091860179415, - "velocityX": -2.395782828620887, - "velocityY": 0.7674201322770242, - "timestamp": 3.193069766490746 - }, - { - "x": 7.01508536939227, - "y": 1.0672813074513483, - "heading": -0.25895131555418466, - "angularVelocity": -0.5588432657936879, - "velocityX": -2.6603770622741663, - "velocityY": 0.8522880167285697, - "timestamp": 3.2642724822995337 - }, - { - "x": 6.806815850395879, - "y": 1.13401199877149, - "heading": -0.3002964260461437, - "angularVelocity": -0.5806676054744402, - "velocityX": -2.925022123533756, - "velocityY": 0.9371930629632811, - "timestamp": 3.3354751981083215 - }, - { - "x": 6.579700283529664, - "y": 1.2067913837362447, - "heading": -0.34211833205863673, - "angularVelocity": -0.5873639163541298, - "velocityX": -3.1897037112478492, - "velocityY": 1.0221433850950452, - "timestamp": 3.4066779139171093 - }, - { - "x": 6.3337423865313776, - "y": 1.285622359058628, - "heading": -0.38256120014941036, - "angularVelocity": -0.5679961449699128, - "velocityX": -3.454333085535639, - "velocityY": 1.107134389846602, - "timestamp": 3.477880629725897 - }, - { - "x": 6.0689850231424955, - "y": 1.3704991616653164, - "heading": -0.41762738854860754, - "angularVelocity": -0.4924838610562412, - "velocityX": -3.718360463944625, - "velocityY": 1.1920444556443872, - "timestamp": 3.549083345534685 - }, - { - "x": 5.786063768357388, - "y": 1.4612805523811225, - "heading": -0.43124692378177526, - "angularVelocity": -0.19127831120575167, - "velocityX": -3.9734615677424734, - "velocityY": 1.2749709008234504, - "timestamp": 3.6202860613434726 + "heading": -0.34555563246426124, + "angularVelocity": 2.6730250915052296e-7, + "velocityX": 3.52891551864971, + "velocityY": -1.360125725304771, + "timestamp": 1.322027736676021 + }, + { + "x": 5.655413318775525, + "y": 1.5130951799644636, + "heading": -0.34555563491945995, + "angularVelocity": -5.785220393859716e-8, + "velocityX": 3.5912523855702734, + "velocityY": -1.18578710714545, + "timestamp": 1.3644668930350639 + }, + { + "x": 5.810103118724588, + "y": 1.4702901058122384, + "heading": -0.3455556373457815, + "angularVelocity": -5.7171766263130695e-8, + "velocityX": 3.6449782045702985, + "velocityY": -1.0086221740622332, + "timestamp": 1.4069060493941077 + }, + { + "x": 5.966313456902953, + "y": 1.4334181808859694, + "heading": -0.34555563976573417, + "angularVelocity": -5.702169541040829e-8, + "velocityX": 3.680806867525754, + "velocityY": -0.8688185178405917, + "timestamp": 1.4493452057531506 + }, + { + "x": 6.1225244823663525, + "y": 1.3965491678093302, + "heading": -0.3455556421856862, + "angularVelocity": -5.7021679533312126e-8, + "velocityX": 3.680823062122731, + "velocityY": -0.868749905505203, + "timestamp": 1.4917843621121936 + }, + { + "x": 6.278735507957822, + "y": 1.3596801552753135, + "heading": -0.3455556446056381, + "angularVelocity": -5.702167859922657e-8, + "velocityX": 3.680823065140465, + "velocityY": -0.8687498927193055, + "timestamp": 1.5342235184712365 + }, + { + "x": 6.434946533537413, + "y": 1.3228111426909752, + "heading": -0.34555564702559005, + "angularVelocity": -5.7021678932866206e-8, + "velocityX": 3.680823064860606, + "velocityY": -0.868749893905046, + "timestamp": 1.5766626748302794 + }, + { + "x": 6.591157495254782, + "y": 1.285941859528607, + "heading": -0.3455556494455419, + "angularVelocity": -5.7021677677252914e-8, + "velocityX": 3.6808215600657594, + "velocityY": -0.8687562695744264, + "timestamp": 1.6191018311893224 + }, + { + "x": 6.747031505398479, + "y": 1.2476730535838276, + "heading": -0.34555565194208476, + "angularVelocity": -5.8826401859372655e-8, + "velocityX": 3.6728819212374164, + "velocityY": -0.9017334279932018, + "timestamp": 1.6615409875483653 + }, + { + "x": 6.897830261276489, + "y": 1.2034176695315386, + "heading": -0.3468060133183351, + "angularVelocity": -0.029462446559305244, + "velocityX": 3.5532929684610997, + "velocityY": -1.0427960367044076, + "timestamp": 1.7039801439074083 + }, + { + "x": 7.040966542913821, + "y": 1.160898981796106, + "heading": -0.34756195184747823, + "angularVelocity": -0.017812289262957588, + "velocityX": 3.3727409759603195, + "velocityY": -1.001874009363348, + "timestamp": 1.7464193002664512 + }, + { + "x": 7.176448137174031, + "y": 1.12015095875213, + "heading": -0.34781665101790943, + "angularVelocity": -0.006001513514462224, + "velocityX": 3.1923724664555144, + "velocityY": -0.9601515802821338, + "timestamp": 1.7888584566254941 + }, + { + "x": 7.30427768133993, + "y": 1.0811849503221362, + "heading": -0.3475678387477372, + "angularVelocity": 0.005862799629361019, + "velocityX": 3.0120660996283153, + "velocityY": -0.918161711329369, + "timestamp": 1.831297612984537 + }, + { + "x": 7.424456501916377, + "y": 1.044006636667021, + "heading": -0.3468143875407301, + "angularVelocity": 0.017753680130510166, + "velocityX": 2.8317909894275823, + "velocityY": -0.8760379999211017, + "timestamp": 1.87373676934358 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.02966022853563592, + "velocityX": 2.65153469287831, + "velocityY": -0.8338339406813241, + "timestamp": 1.916175925702623 + }, + { + "x": 7.689789531297121, + "y": 0.9591642629851495, + "heading": -0.34249163023542684, + "angularVelocity": 0.04769743470803589, + "velocityX": 2.378707539440784, + "velocityY": -0.769870356769085, + "timestamp": 1.98041422798428 + }, + { + "x": 7.82516177952376, + "y": 0.9142093996063804, + "heading": -0.3392101704632614, + "angularVelocity": 0.05108260423473965, + "velocityX": 2.1073447369933547, + "velocityY": -0.6998140016475143, + "timestamp": 2.044652530265937 + }, + { + "x": 7.943169958712402, + "y": 0.8740090692537413, + "heading": -0.33632582750820933, + "angularVelocity": 0.044900672225200826, + "velocityX": 1.837037639494695, + "velocityY": -0.6258000122166694, + "timestamp": 2.108890832547594 + }, + { + "x": 8.043864833075729, + "y": 0.8387415563520763, + "heading": -0.3342715201633078, + "angularVelocity": 0.03197947753809357, + "velocityX": 1.567520790350633, + "velocityY": -0.5490106626266708, + "timestamp": 2.173129134829251 + }, + { + "x": 8.12728568078066, + "y": 0.8085387560486261, + "heading": -0.33336873570137193, + "angularVelocity": 0.014053678722353937, + "velocityX": 1.2986153858669556, + "velocityY": -0.4701680964578417, + "timestamp": 2.2373674371109082 + }, + { + "x": 8.193463737140812, + "y": 0.7835021726798679, + "heading": -0.33386568134146205, + "angularVelocity": -0.007735970946293819, + "velocityX": 1.030196222652151, + "velocityY": -0.38974540857234485, + "timestamp": 2.3016057393925653 + }, + { + "x": 8.242424408463135, + "y": 0.7637123264900073, + "heading": -0.33595980135953535, + "angularVelocity": -0.03259924287680334, + "velocityX": 0.7621725603464993, + "velocityY": -0.3080692591016908, + "timestamp": 2.3658440416742224 + }, + { + "x": 8.274188750584008, + "y": 0.7492346436854774, + "heading": -0.3398119288829893, + "angularVelocity": -0.05996620998114242, + "velocityX": 0.4944766750154696, + "velocityY": -0.2253746174836856, + "timestamp": 2.4300823439558794 + }, + { + "x": 8.288774490356445, + "y": 0.7401233315467834, + "heading": -0.34555563246426124, + "angularVelocity": -0.08941244362418393, + "velocityX": 0.2270567442533848, + "velocityY": -0.14183612914838375, + "timestamp": 2.4943206462375365 + }, + { + "x": 8.286058974983396, + "y": 0.7364361738247144, + "heading": -0.3533713967772531, + "angularVelocity": -0.1208300500196775, + "velocityX": -0.04198128874091145, + "velocityY": -0.05700267231030528, + "timestamp": 2.559004591263786 + }, + { + "x": 8.26594084231072, + "y": 0.7382366486349878, + "heading": -0.3631834148956511, + "angularVelocity": -0.15169170826572306, + "velocityX": -0.3110220420927156, + "velocityY": 0.027834956719829915, + "timestamp": 2.623688536290036 + }, + { + "x": 8.22841989282902, + "y": 0.7455250700569162, + "heading": -0.3749497746847549, + "angularVelocity": -0.18190541384463682, + "velocityX": -0.5800658798172126, + "velocityY": 0.11267744134917045, + "timestamp": 2.6883724813162857 + }, + { + "x": 8.173495898954863, + "y": 0.7583018091084519, + "heading": -0.38862097650887273, + "angularVelocity": -0.21135386560868902, + "velocityX": -0.8491132359330754, + "velocityY": 0.1975256618369646, + "timestamp": 2.7530564263425354 + }, + { + "x": 8.101168599327213, + "y": 0.7765673113174031, + "heading": -0.4041376276128682, + "angularVelocity": -0.23988411804039605, + "velocityX": -1.11816463263485, + "velocityY": 0.2823807700896826, + "timestamp": 2.817740371368785 + }, + { + "x": 8.011437691690773, + "y": 0.8003221216608263, + "heading": -0.42142708913788907, + "angularVelocity": -0.26729138920027884, + "velocityX": -1.3872207021390834, + "velocityY": 0.36724430357151194, + "timestamp": 2.882424316395035 + }, + { + "x": 7.904302824322884, + "y": 0.8295669211666158, + "heading": -0.44039841224325665, + "angularVelocity": -0.29329261067284323, + "velocityX": -1.6562822092006166, + "velocityY": 0.45211836559939883, + "timestamp": 2.9471082614212847 + }, + { + "x": 7.779763586783285, + "y": 0.8643025828479174, + "heading": -0.46093432615418145, + "angularVelocity": -0.317480850968364, + "velocityX": -1.9253500615811048, + "velocityY": 0.5370059242243979, + "timestamp": 3.0117922064475344 + }, + { + "x": 7.63781950374517, + "y": 0.9045302615405436, + "heading": -0.48287779045070706, + "angularVelocity": -0.33924127985113656, + "velocityX": -2.194425262412658, + "velocityY": 0.6219113363648586, + "timestamp": 3.076476151473784 + }, + { + "x": 7.478470046612931, + "y": 0.9502515474489494, + "heading": -0.5060075980261466, + "angularVelocity": -0.3575818940241346, + "velocityX": -2.4635086352196294, + "velocityY": 0.7068413327271726, + "timestamp": 3.141160096500034 + }, + { + "x": 7.301714722310279, + "y": 1.0014687500113455, + "heading": -0.5299890975728885, + "angularVelocity": -0.37074887032647375, + "velocityX": -2.732599630880914, + "velocityY": 0.7918070325118697, + "timestamp": 3.2058440415262837 + }, + { + "x": 7.107553521342319, + "y": 1.0581854755780744, + "heading": -0.5542575516052335, + "angularVelocity": -0.37518512549746846, + "velocityX": -3.0016907733312475, + "velocityY": 0.8768284857040194, + "timestamp": 3.2705279865525334 + }, + { + "x": 6.895989597473325, + "y": 1.1204078946209601, + "heading": -0.5776591781660143, + "angularVelocity": -0.3617841575878507, + "velocityX": -3.2707331592582385, + "velocityY": 0.961945333075069, + "timestamp": 3.335211931578783 + }, + { + "x": 6.667061067423103, + "y": 1.188145012147935, + "heading": -0.5965312726664429, + "angularVelocity": -0.2917585575952417, + "velocityX": -3.5391862688232427, + "velocityY": 1.0472013959489617, + "timestamp": 3.399895876605033 + }, + { + "x": 6.433742027972299, + "y": 1.2616769248691375, + "heading": -0.5965312781733252, + "angularVelocity": -8.513522502240808e-8, + "velocityX": -3.607062608134345, + "velocityY": 1.136787694247182, + "timestamp": 3.4645798216312826 + }, + { + "x": 6.200423078644191, + "y": 1.3352091235520722, + "heading": -0.5965312836800714, + "angularVelocity": -8.513312137357674e-8, + "velocityX": -3.6070612148566714, + "velocityY": 1.1367921151546025, + "timestamp": 3.5292637666575324 + }, + { + "x": 5.967104129330135, + "y": 1.4087413222795935, + "heading": -0.5965312891868175, + "angularVelocity": -8.513312220445073e-8, + "velocityX": -3.607061214639434, + "velocityY": 1.136792115843899, + "timestamp": 3.593947711683782 + }, + { + "x": 5.733785345709214, + "y": 1.4822740467522528, + "heading": -0.5965312946935636, + "angularVelocity": -8.513311865132804e-8, + "velocityX": -3.6070586530588855, + "velocityY": 1.1368002437516491, + "timestamp": 3.658631656710032 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.431246936810795, - "angularVelocity": -1.829848702149455e-7, - "velocityX": -3.97541256513325, - "velocityY": 1.4344738240927952, - "timestamp": 3.6914887771522604 - }, - { - "x": 5.267799654684855, - "y": 1.6634628697072642, - "heading": -0.4312469486344858, - "angularVelocity": -1.9550516647777393e-7, - "velocityX": -3.889105914989318, - "velocityY": 1.6542293543074849, - "timestamp": 3.751966415899034 - }, - { - "x": 5.036859662555262, - "y": 1.772990919381671, - "heading": -0.43124696041084615, - "angularVelocity": -1.9472255536128903e-7, - "velocityX": -3.8186013362155586, - "velocityY": 1.8110503641356064, - "timestamp": 3.812444054645807 - }, - { - "x": 4.805920651459889, - "y": 1.8825210375437151, - "heading": -0.4312469721872024, - "angularVelocity": -1.9472248822096454e-7, - "velocityX": -3.8185851147783296, - "velocityY": 1.8110845666554114, - "timestamp": 3.8729216933925805 - }, - { - "x": 4.574981640487933, - "y": 1.9920511559659781, - "heading": -0.4312469839635587, - "angularVelocity": -1.947224875225229e-7, - "velocityX": -3.8185851127376225, - "velocityY": 1.8110845709581442, - "timestamp": 3.933399332139354 - }, - { - "x": 4.344043475908902, - "y": 2.101581482580224, - "heading": -0.4312495820185635, - "angularVelocity": -0.00004295893592741073, - "velocityX": -3.8185711175992116, - "velocityY": 1.811088013420298, - "timestamp": 3.9938769708861273 - }, - { - "x": 4.124944651592417, - "y": 2.2054837302340013, - "heading": -0.4555627986647105, - "angularVelocity": -0.402019939104465, - "velocityX": -3.6228071871964698, - "velocityY": 1.7180275190442889, - "timestamp": 4.054354609632901 - }, - { - "x": 3.9186815541610502, - "y": 2.3032986019181108, - "heading": -0.48568611973278303, - "angularVelocity": -0.49809023123757634, - "velocityX": -3.4105679670301026, - "velocityY": 1.6173725315842862, - "timestamp": 4.114832248379674 - }, - { - "x": 3.725312007029145, - "y": 2.394998463425361, - "heading": -0.5178804694719192, - "angularVelocity": -0.5323347671351059, - "velocityX": -3.197372634562718, - "velocityY": 1.5162606114832984, - "timestamp": 4.175309887126447 - }, - { - "x": 3.5448441178456083, - "y": 2.4805793192106775, - "heading": -0.5504848182217125, - "angularVelocity": -0.5391141159845556, - "velocityX": -2.9840432418199043, - "velocityY": 1.4150826248963195, - "timestamp": 4.235787525873221 - }, - { - "x": 3.3772776543095704, - "y": 2.5600412902138987, - "heading": -0.5825517458866412, - "angularVelocity": -0.530227838411517, - "velocityX": -2.7707176901805863, - "velocityY": 1.3139066380540503, - "timestamp": 4.296265164619994 - }, - { - "x": 3.222610601114179, - "y": 2.6333854258294824, - "heading": -0.6134664857875873, - "angularVelocity": -0.5111763709954703, - "velocityX": -2.5574254617148258, - "velocityY": 1.2127480029880529, - "timestamp": 4.3567428033667674 - }, - { - "x": 3.0808406790679506, - "y": 2.7006129397280647, - "heading": -0.6427968546636391, - "angularVelocity": -0.48497873732909724, - "velocityX": -2.344170919764792, - "velocityY": 1.111609436011059, - "timestamp": 4.417220442113541 - }, - { - "x": 2.951965745045311, - "y": 2.761724997445756, - "heading": -0.6702218815330417, - "angularVelocity": -0.45347383657350077, - "velocityX": -2.130951814475617, - "velocityY": 1.0104901412168834, - "timestamp": 4.477698080860314 - }, - { - "x": 2.8359838859812436, - "y": 2.8167226593903436, - "heading": -0.6954934767072644, - "angularVelocity": -0.4178667636155553, - "velocityX": -1.917764341787469, - "velocityY": 0.9093883803048015, - "timestamp": 4.5381757196070875 - }, - { - "x": 2.732893421596196, - "y": 2.865606872368782, - "heading": -0.7184140024714569, - "angularVelocity": -0.3789917437115094, - "velocityX": -1.7046046525840333, - "velocityY": 0.8083022748808345, - "timestamp": 4.598653358353861 - }, - { - "x": 2.6426928809449626, - "y": 2.9083784763416602, - "heading": -0.7388222811864813, - "angularVelocity": -0.33745164556568824, - "velocityX": -1.4914692855141438, - "velocityY": 0.7072300582363449, - "timestamp": 4.659130997100634 - }, - { - "x": 2.565380973908943, - "y": 2.945038215167831, - "heading": -0.756584421622472, - "angularVelocity": -0.29369765096755573, - "velocityX": -1.2783552506031493, - "velocityY": 0.6061701413256031, - "timestamp": 4.719608635847408 - }, - { - "x": 2.500956564421106, - "y": 2.975586747465841, - "heading": -0.7715875591190221, - "angularVelocity": -0.24807743502306231, - "velocityX": -1.0652600006026816, - "velocityY": 0.5051211146969536, - "timestamp": 4.780086274594181 - }, - { - "x": 2.4494186472691353, - "y": 3.0000246563455213, - "heading": -0.7837354404326269, - "angularVelocity": -0.2008656681268564, - "velocityX": -0.8521813718251273, - "velocityY": 0.40408172981098234, - "timestamp": 4.840563913340954 - }, - { - "x": 2.410766328604012, - "y": 3.018352457725689, - "heading": -0.7929452205270133, - "angularVelocity": -0.152284055482875, - "velocityX": -0.6391175228742725, - "velocityY": 0.3030508756618002, - "timestamp": 4.901041552087728 - }, - { - "x": 2.3849988097281045, - "y": 3.030570607297698, - "heading": -0.7991450800016932, - "angularVelocity": -0.10251490638778926, - "velocityX": -0.42606688041837987, - "velocityY": 0.20202755638603895, - "timestamp": 4.961519190834501 + "heading": -0.5965313002103377, + "angularVelocity": -8.528815258110653e-8, + "velocityX": -3.567836630187545, + "velocityY": 1.254483436778072, + "timestamp": 3.7233156017362816 + }, + { + "x": 5.285431093647359, + "y": 1.6581112689784743, + "heading": -0.5965313049795081, + "angularVelocity": -7.601302145092289e-8, + "velocityX": -3.4677610362467894, + "velocityY": 1.509245007513348, + "timestamp": 3.7860570943991227 + }, + { + "x": 5.070948144949037, + "y": 1.7596075143791599, + "heading": -0.5965313097283005, + "angularVelocity": -7.568822719486902e-8, + "velocityX": -3.4185184252932688, + "velocityY": 1.6176893646140194, + "timestamp": 3.848798587061964 + }, + { + "x": 4.856465419222307, + "y": 1.861104230964457, + "heading": -0.5965313144770923, + "angularVelocity": -7.568821760685447e-8, + "velocityX": -3.4185148714793154, + "velocityY": 1.6176968745502942, + "timestamp": 3.911540079724805 + }, + { + "x": 4.641982693508075, + "y": 1.9626009475761665, + "heading": -0.5965313192258841, + "angularVelocity": -7.568821790279648e-8, + "velocityX": -3.418514871280104, + "velocityY": 1.6176968749712675, + "timestamp": 3.974281572387646 + }, + { + "x": 4.427499967793844, + "y": 2.064097664187878, + "heading": -0.5965313239746759, + "angularVelocity": -7.568821822441872e-8, + "velocityX": -3.4185148712800926, + "velocityY": 1.617696874971291, + "timestamp": 4.037023065050487 + }, + { + "x": 4.213017242079613, + "y": 2.165594380799589, + "heading": -0.5965313287234677, + "angularVelocity": -7.568821811615467e-8, + "velocityX": -3.4185148712800917, + "velocityY": 1.617696874971293, + "timestamp": 4.099764557713328 + }, + { + "x": 3.998534516366738, + "y": 2.267091097414165, + "heading": -0.5965313334722594, + "angularVelocity": -7.56882168459371e-8, + "velocityX": -3.418514871258486, + "velocityY": 1.6176968750169503, + "timestamp": 4.1625060503761695 + }, + { + "x": 3.7840518148370346, + "y": 2.3685878651306878, + "heading": -0.5965313382224369, + "angularVelocity": -7.571030356184493e-8, + "velocityX": -3.418514485817036, + "velocityY": 1.6176976895010247, + "timestamp": 4.225247543039011 + }, + { + "x": 3.5823467745214304, + "y": 2.46403001529431, + "heading": -0.6258886394381601, + "angularVelocity": -0.46790887449049295, + "velocityX": -3.214858808022383, + "velocityY": 1.5211966772373215, + "timestamp": 4.287989035701852 + }, + { + "x": 3.3961574491843427, + "y": 2.552130310684733, + "heading": -0.6529986278149837, + "angularVelocity": -0.43209026795882993, + "velocityX": -2.967562890759225, + "velocityY": 1.40417914288166, + "timestamp": 4.350730528364693 + }, + { + "x": 3.225483861981861, + "y": 2.6328887858865286, + "heading": -0.6778585687680168, + "angularVelocity": -0.3962280764760425, + "velocityX": -2.7202666044246855, + "velocityY": 1.2871621597492864, + "timestamp": 4.413472021027534 + }, + { + "x": 3.0703260256030673, + "y": 2.7063054766361296, + "heading": -0.7004659993691003, + "angularVelocity": -0.36032662982009556, + "velocityX": -2.4729701158463, + "velocityY": 1.17014574619904, + "timestamp": 4.476213513690375 + }, + { + "x": 2.9306839491479293, + "y": 2.7723804156875507, + "heading": -0.7208186227896802, + "angularVelocity": -0.32438857535555815, + "velocityX": -2.2256734822288196, + "velocityY": 1.0531298546958971, + "timestamp": 4.538955006353216 + }, + { + "x": 2.8065576397395007, + "y": 2.8311136320269066, + "heading": -0.7389143343573537, + "angularVelocity": -0.28841697574707026, + "velocityX": -1.9783767350811556, + "velocityY": 0.9361144251855119, + "timestamp": 4.601696499016057 + }, + { + "x": 2.697947103240289, + "y": 2.8825051506567756, + "heading": -0.7547512635952348, + "angularVelocity": -0.25241556370017226, + "velocityX": -1.7310798944944172, + "velocityY": 0.8190993941766099, + "timestamp": 4.6644379916788985 + }, + { + "x": 2.6048523446905505, + "y": 2.9265549925489487, + "heading": -0.7683278153992709, + "angularVelocity": -0.21638872822158642, + "velocityX": -1.4837829735739574, + "velocityY": 0.702084697424841, + "timestamp": 4.72717948434174 + }, + { + "x": 2.527273368620795, + "y": 2.9632631746604594, + "heading": -0.779642706302846, + "angularVelocity": -0.18034143631837077, + "velocityX": -1.2364859804443729, + "velocityY": 0.5850702709413225, + "timestamp": 4.789920977004581 + }, + { + "x": 2.4652101792880994, + "y": 2.9926297099741825, + "heading": -0.7886949946743065, + "angularVelocity": -0.1442791362982944, + "velocityX": -0.9891889194637066, + "velocityY": 0.46805605138424683, + "timestamp": 4.852662469667422 + }, + { + "x": 2.4186627808554446, + "y": 3.014654607545873, + "heading": -0.7954841045559687, + "angularVelocity": -0.10820765642515946, + "velocityX": -0.7418917921317392, + "velocityY": 0.3510419761615724, + "timestamp": 4.915403962330263 + }, + { + "x": 2.3876311775224326, + "y": 3.0293378725476474, + "heading": -0.8000098431420335, + "angularVelocity": -0.07213310353301693, + "velocityX": -0.49459459786475296, + "velocityY": 0.23402798337424993, + "timestamp": 4.978145454993104 }, { "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": -0.05171055025787837, - "velocityX": -0.21302809407950682, - "velocityY": 0.10101087163407141, - "timestamp": 5.0219968295812745 + "angularVelocity": -0.036061762982130895, + "velocityX": -0.24729733470577522, + "velocityY": 0.11701401166345779, + "timestamp": 5.040886947655945 }, { "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": -1.423507620090277e-32, - "velocityY": 0, - "timestamp": 5.082474468328048 + "angularVelocity": 2.8180807610025885e-36, + "velocityX": -1.7033240054088048e-37, + "velocityY": 8.364918339692578e-38, + "timestamp": 5.103628440318786 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.4.traj b/src/main/deploy/choreo/SourceLanePGHSprint.4.traj index efcf48ca..5cd380c3 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.4.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.4.traj @@ -4,370 +4,352 @@ "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": -1.423507620090277e-32, - "velocityY": 0, + "angularVelocity": 2.8180807610025885e-36, + "velocityX": -1.7033240054088048e-37, + "velocityY": 8.364918339692578e-38, "timestamp": 0 }, { - "x": 2.3839325851596245, - "y": 3.031084448154912, - "heading": -0.7992419373744997, - "angularVelocity": 0.052323339006150715, - "velocityX": 0.20403271649422824, - "velocityY": -0.09660273136476924, - "timestamp": 0.05791821895636318 - }, - { - "x": 2.407567371385834, - "y": 3.019894305720653, - "heading": -0.7932053890740913, - "angularVelocity": 0.10422537863183293, - "velocityX": 0.4080717026885203, - "velocityY": -0.19320591406117554, - "timestamp": 0.11583643791272635 - }, - { - "x": 2.4430201410442214, - "y": 3.003109041895344, - "heading": -0.7841908751963366, - "angularVelocity": 0.15564211124217459, - "velocityX": 0.6121177463191404, - "velocityY": -0.2898097373808346, - "timestamp": 0.17375465686908953 - }, - { - "x": 2.490291356941472, - "y": 2.980728609584666, - "heading": -0.7722307955425777, - "angularVelocity": 0.2064994378154119, - "velocityX": 0.8161717806423892, - "velocityY": -0.38641437381801885, - "timestamp": 0.2316728758254527 - }, - { - "x": 2.5493815465754834, - "y": 2.952752953058649, - "heading": -0.7573625934321272, - "angularVelocity": 0.2567102783608131, - "velocityX": 1.0202349225989056, - "velocityY": -0.4830199724735087, - "timestamp": 0.2895910947818159 - }, - { - "x": 2.6202913158109356, - "y": 2.9191820098836145, - "heading": -0.7396297457639593, - "angularVelocity": 0.30617045875544285, - "velocityX": 1.2243085252479435, - "velocityY": -0.5796266490916698, - "timestamp": 0.34750931373817906 - }, - { - "x": 2.7030213667788137, - "y": 2.8800157137424582, - "heading": -0.7190830975629909, - "angularVelocity": 0.35475276296822733, - "velocityX": 1.428394250696986, - "velocityY": -0.6762344707226781, - "timestamp": 0.40542753269454224 - }, - { - "x": 2.797572521744028, - "y": 2.8352539986604253, - "heading": -0.6957827053311118, - "angularVelocity": 0.40229814817741966, - "velocityX": 1.632494173145279, - "velocityY": -0.7728434314556114, - "timestamp": 0.4633457516509054 - }, - { - "x": 2.9039457555692096, - "y": 2.7848968055361296, - "heading": -0.6698004526247588, - "angularVelocity": 0.448602411720027, - "velocityX": 1.8366109273029618, - "velocityY": -0.8694534126167829, - "timestamp": 0.5212639706072686 - }, - { - "x": 3.0221422408423324, - "y": 2.7289440926274007, - "heading": -0.6412238784283038, - "angularVelocity": 0.4933952512936468, - "velocityX": 2.040747927041322, - "velocityY": -0.9660641144867576, - "timestamp": 0.5791821895636318 - }, - { - "x": 3.1521634121347564, - "y": 2.6673958532051016, - "heading": -0.6101619934003036, - "angularVelocity": 0.5363059428917012, - "velocityX": 2.2449096956932593, - "velocityY": -1.0626749325401574, - "timestamp": 0.6371004085199949 - }, - { - "x": 3.2940110599022505, - "y": 2.600252147103853, - "heading": -0.5767545340853645, - "angularVelocity": 0.576803981837031, - "velocityX": 2.4491023778608563, - "velocityY": -1.1592847175054986, - "timestamp": 0.6950186274763581 - }, - { - "x": 3.4476874712025682, - "y": 2.527513161589764, - "heading": -0.54118758447353, - "angularVelocity": 0.6140891459150629, - "velocityX": 2.6533345477370784, - "velocityY": -1.2558912691858224, - "timestamp": 0.7529368464327213 - }, - { - "x": 3.613195643589333, - "y": 2.4491793412538243, - "heading": -0.5037220986038603, - "angularVelocity": 0.6468687495017285, - "velocityX": 2.8576184725476987, - "velocityY": -1.3524901446807027, - "timestamp": 0.8108550653890845 - }, - { - "x": 3.7905395971194613, - "y": 2.3652517067540755, - "heading": -0.46475198370690785, - "angularVelocity": 0.6728472594489399, - "velocityX": 3.0619718065526835, - "velocityY": -1.4490713977752308, - "timestamp": 0.8687732843454476 - }, - { - "x": 3.979724665259552, - "y": 2.275732818118546, - "heading": -0.4249432999516913, - "angularVelocity": 0.6873257581558144, - "velocityX": 3.266417226721487, - "velocityY": -1.545608450131651, - "timestamp": 0.9266915033018108 - }, - { - "x": 4.180755824131631, - "y": 2.1806309244718047, - "heading": -0.3856731863911582, - "angularVelocity": 0.6780269536623745, - "velocityX": 3.47094856324124, - "velocityY": -1.6420030753776067, - "timestamp": 0.984609722258174 - }, - { - "x": 4.393589257468926, - "y": 2.0800009314187498, - "heading": -0.3515823344877535, - "angularVelocity": 0.5886032498528599, - "velocityX": 3.6747233801793717, - "velocityY": -1.7374497155872817, - "timestamp": 1.0425279412145372 - }, - { - "x": 4.614791128841965, - "y": 1.9751825525280107, - "heading": -0.35158232270208695, - "angularVelocity": 2.034880699916496e-7, - "velocityX": 3.8192105240614955, - "velocityY": -1.8097652306904055, - "timestamp": 1.1004461601709004 - }, - { - "x": 4.835992849247887, - "y": 1.870363855043589, - "heading": -0.3515823109197321, - "angularVelocity": 2.034308907507492e-7, - "velocityX": 3.8192079175048774, - "velocityY": -1.8097707314410818, - "timestamp": 1.1583643791272635 - }, - { - "x": 5.0571945697179865, - "y": 1.7655451576946017, - "heading": -0.35158229913737715, - "angularVelocity": 2.0343089162960125e-7, - "velocityX": 3.8192079186129373, - "velocityY": -1.8097707291027114, - "timestamp": 1.2162825980836267 - }, - { - "x": 5.278396987351414, - "y": 1.6607279316008212, - "heading": -0.35158228735502095, - "angularVelocity": 2.03430914726069e-7, - "velocityX": 3.8192199556427555, - "velocityY": -1.8097453268159511, - "timestamp": 1.2742008170399899 + "x": 2.3862784269967885, + "y": 3.0299921206795677, + "heading": -0.7979445022798209, + "angularVelocity": 0.07214760590413402, + "velocityX": 0.2361025217555023, + "velocityY": -0.11148080618080956, + "timestamp": 0.059986878920357256 + }, + { + "x": 2.414605712310544, + "y": 3.016617241283919, + "heading": -0.7893555783917804, + "angularVelocity": 0.1431800427464117, + "velocityX": 0.4722246901920792, + "velocityY": -0.22296341527296182, + "timestamp": 0.11997375784071451 + }, + { + "x": 2.457098595113841, + "y": 2.996554734272641, + "heading": -0.7765849369819304, + "angularVelocity": 0.2128905794016223, + "velocityX": 0.7083696229589314, + "velocityY": -0.33444825555792773, + "timestamp": 0.17996063676107177 + }, + { + "x": 2.5137586742709717, + "y": 2.9698044458506936, + "heading": -0.759727315329062, + "angularVelocity": 0.281021816041631, + "velocityX": 0.944541209292721, + "velocityY": -0.44593565965421617, + "timestamp": 0.23994751568142902 + }, + { + "x": 2.5845878442638655, + "y": 2.9363662114763, + "heading": -0.7388971783301493, + "angularVelocity": 0.34724488711220036, + "velocityX": 1.180744377231747, + "velocityY": -0.5574258067133014, + "timestamp": 0.2999343946017863 + }, + { + "x": 2.669588381015972, + "y": 2.8962398712988433, + "heading": -0.7142349782126004, + "angularVelocity": 0.41112657570152883, + "velocityX": 1.4169854855252446, + "velocityY": -0.6689186185320778, + "timestamp": 0.35992127352214354 + }, + { + "x": 2.768763062860025, + "y": 2.849425297465716, + "heading": -0.6859166617272396, + "angularVelocity": 0.4720751770225952, + "velocityX": 1.653272909492815, + "velocityY": -0.7804135616937369, + "timestamp": 0.4199081524425008 + }, + { + "x": 2.8821153449579997, + "y": 2.795922445610523, + "heading": -0.6541688123580982, + "angularVelocity": 0.5292465609236296, + "velocityX": 1.8896179320892514, + "velocityY": -0.8919092444570627, + "timestamp": 0.47989503136285805 + }, + { + "x": 3.0096496153353156, + "y": 2.73573145985777, + "heading": -0.6192942663212868, + "angularVelocity": 0.5813695705541383, + "velocityX": 2.1260361044394105, + "velocityY": -1.0034025246198655, + "timestamp": 0.5398819102832153 + }, + { + "x": 3.151371569227902, + "y": 2.6688529097388005, + "heading": -0.5817190571900679, + "angularVelocity": 0.6263904675071726, + "velocityX": 2.3625492181506123, + "velocityY": -1.1148863105173612, + "timestamp": 0.5998687892035726 + }, + { + "x": 3.30728870131474, + "y": 2.5952884046594, + "heading": -0.5420886244414685, + "angularVelocity": 0.6606516868666429, + "velocityX": 2.5991872705003334, + "velocityY": -1.2263432671179575, + "timestamp": 0.6598556681239298 + }, + { + "x": 3.477410464899007, + "y": 2.5150425607670805, + "heading": -0.501501110267189, + "angularVelocity": 0.6766065330414338, + "velocityX": 2.8359829123654157, + "velocityY": -1.3377232710983202, + "timestamp": 0.7198425470442871 + }, + { + "x": 3.661742704544347, + "y": 2.428132084038069, + "heading": -0.46226256646132613, + "angularVelocity": 0.6541187758402722, + "velocityX": 3.0728759849311573, + "velocityY": -1.4488247812392314, + "timestamp": 0.7798294259646443 + }, + { + "x": 3.860146403975295, + "y": 2.3346891355318427, + "heading": -0.4327535611710109, + "angularVelocity": 0.4919243311440368, + "velocityX": 3.307451612782871, + "velocityY": -1.5577231252568988, + "timestamp": 0.8398163048850016 + }, + { + "x": 4.065242173401202, + "y": 2.237711412138144, + "heading": -0.4327535477418668, + "angularVelocity": 2.238680243649846e-7, + "velocityX": 3.41901050891823, + "velocityY": -1.6166489262168846, + "timestamp": 0.8998031838053588 + }, + { + "x": 4.270337924993822, + "y": 2.140733651028875, + "heading": -0.4327535343131544, + "angularVelocity": 2.2386082909736944e-7, + "velocityX": 3.419010211631728, + "velocityY": -1.6166495549472233, + "timestamp": 0.9597900627257161 + }, + { + "x": 4.475433676585232, + "y": 2.0437558899170503, + "heading": -0.43275352088444197, + "angularVelocity": 2.2386082765717578e-7, + "velocityX": 3.419010211611586, + "velocityY": -1.6166495549898217, + "timestamp": 1.0197769416460734 + }, + { + "x": 4.6805294281766425, + "y": 1.9467781288052255, + "heading": -0.43275350745572955, + "angularVelocity": 2.2386082872977313e-7, + "velocityX": 3.4190102116115844, + "velocityY": -1.616649554989824, + "timestamp": 1.0797638205664306 + }, + { + "x": 4.885625179768054, + "y": 1.8498003676934025, + "heading": -0.4327534940270171, + "angularVelocity": 2.2386082836686737e-7, + "velocityX": 3.419010211611597, + "velocityY": -1.6166495549897981, + "timestamp": 1.1397506994867879 + }, + { + "x": 5.090720931370458, + "y": 1.7528226066048298, + "heading": -0.4327534805983047, + "angularVelocity": 2.2386082909228912e-7, + "velocityX": 3.4190102117948658, + "velocityY": -1.616649554602208, + "timestamp": 1.1997375784071451 + }, + { + "x": 5.295816845235169, + "y": 1.6558451886813434, + "heading": -0.4327534671695918, + "angularVelocity": 2.2386083672590555e-7, + "velocityX": 3.4190129167581613, + "velocityY": -1.6166438339330798, + "timestamp": 1.2597244573275024 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.35158227553759686, - "angularVelocity": 2.0403638579093497e-7, - "velocityX": 3.8779958008952624, - "velocityY": -1.6801094533136367, - "timestamp": 1.332119035996353 - }, - { - "x": 5.78771748375251, - "y": 1.4618238256689142, - "heading": -0.3515822643983809, - "angularVelocity": 1.5573311965555955e-7, - "velocityX": 3.980476011114963, - "velocityY": -1.420363078204806, - "timestamp": 1.4036466336248683 - }, - { - "x": 6.071768529925899, - "y": 1.3687010115486713, - "heading": -0.3384707068713864, - "angularVelocity": 0.18330767370505657, - "velocityX": 3.9712090939868023, - "velocityY": -1.3019144666913607, - "timestamp": 1.4751742312533835 - }, - { - "x": 6.337299554037092, - "y": 1.2816927202415032, - "heading": -0.3077191306864491, - "angularVelocity": 0.4299260314130538, - "velocityX": 3.712287745077815, - "velocityY": -1.2164296606053124, - "timestamp": 1.5467018288818988 - }, - { - "x": 6.583882404182654, - "y": 1.200909898706293, - "heading": -0.27261549465283386, - "angularVelocity": 0.4907705165204766, - "velocityX": 3.4473805680740197, - "velocityY": -1.1293937474981692, - "timestamp": 1.618229426510414 - }, - { - "x": 6.811487459476513, - "y": 1.1263530850980985, - "heading": -0.23658461598105995, - "angularVelocity": 0.5037339413928503, - "velocityX": 3.182059272785126, - "velocityY": -1.042350310650886, - "timestamp": 1.6897570241389293 - }, - { - "x": 7.020112866315064, - "y": 1.0580186639861602, - "heading": -0.20123048803983856, - "angularVelocity": 0.49427254812661375, - "velocityX": 2.916712062972736, - "velocityY": -0.9553574197590016, - "timestamp": 1.7612846217674445 - }, - { - "x": 7.209761115265014, - "y": 0.9959034826022224, - "heading": -0.16748852563901473, - "angularVelocity": 0.47173347797958504, - "velocityX": 2.6513996728214475, - "velocityY": -0.8684086065149057, - "timestamp": 1.8328122193959597 - }, - { - "x": 7.380435279840303, - "y": 0.9400050984767416, - "heading": -0.13597326201279447, - "angularVelocity": 0.4406028535992147, - "velocityX": 2.3861302522936825, - "velocityY": -0.78149394050383, - "timestamp": 1.904339817024475 - }, - { - "x": 7.5321382283527605, - "y": 0.8903216280368556, - "heading": -0.10712014952027268, - "angularVelocity": 0.403384336244213, - "velocityX": 2.1209009325371073, - "velocityY": -0.6946056080049196, - "timestamp": 1.9758674146529902 - }, - { - "x": 7.664872474877682, - "y": 0.8468515953524562, - "heading": -0.08125430314522812, - "angularVelocity": 0.36162051058083977, - "velocityX": 1.8557067611062361, - "velocityY": -0.6077379099206526, - "timestamp": 2.0473950122815054 - }, - { - "x": 7.77864018727303, - "y": 0.8095938204087205, - "heading": -0.05862785653621709, - "angularVelocity": 0.3163317007586838, - "velocityX": 1.590542897668813, - "velocityY": -0.520886709172551, - "timestamp": 2.1189226099100207 - }, - { - "x": 7.873443230968776, - "y": 0.7785473403012081, - "heading": -0.039441988553792, - "angularVelocity": 0.2682302861906329, - "velocityX": 1.3254051140947378, - "velocityY": -0.4340489704233464, - "timestamp": 2.190450207538536 - }, - { - "x": 7.9492832154345106, - "y": 0.7537113535170058, - "heading": -0.023860737345292347, - "angularVelocity": 0.21783551699054987, - "velocityX": 1.0602898318998024, - "velocityY": -0.3472224373197895, - "timestamp": 2.261977805167051 - }, - { - "x": 8.006161534793161, - "y": 0.7350851799018175, - "heading": -0.01202009660498375, - "angularVelocity": 0.1655394719364682, - "velocityX": 0.7951940404045607, - "velocityY": -0.26040541319344906, - "timestamp": 2.3335054027955664 - }, - { - "x": 8.044079401282437, - "y": 0.7226682313019233, - "heading": -0.004034250807333449, - "angularVelocity": 0.11164705739350395, - "velocityX": 0.5301151967413267, - "velocityY": -0.17359661181943797, - "timestamp": 2.4050330004240816 + "heading": -0.4327534537085892, + "angularVelocity": 2.2439911588435075e-7, + "velocityX": 3.4538678416562716, + "velocityY": -1.5407736813730113, + "timestamp": 1.3197113362478596 + }, + { + "x": 5.767637732978587, + "y": 1.4715486447521433, + "heading": -0.43275344189521775, + "angularVelocity": 1.5949042736003556e-7, + "velocityX": 3.5727828839624562, + "velocityY": -1.2403266728714895, + "timestamp": 1.3937808065498363 + }, + { + "x": 6.033774192023134, + "y": 1.384125705645109, + "heading": -0.43275343009903516, + "angularVelocity": 1.5925836231633141e-7, + "velocityX": 3.5930655094403474, + "velocityY": -1.1802830336252825, + "timestamp": 1.467850276851813 + }, + { + "x": 6.2999107025526015, + "y": 1.2967029232707166, + "heading": -0.43275341830285263, + "angularVelocity": 1.5925836222768275e-7, + "velocityX": 3.593066204529984, + "velocityY": -1.1802809176031077, + "timestamp": 1.5419197471537895 + }, + { + "x": 6.566047160708243, + "y": 1.2092799814598572, + "heading": -0.4327534065048956, + "angularVelocity": 1.5928231960994732e-7, + "velocityX": 3.5930654974393894, + "velocityY": -1.1802830701291929, + "timestamp": 1.6159892174557662 + }, + { + "x": 6.815604168443372, + "y": 1.1270295237235333, + "heading": -0.36800659105834677, + "angularVelocity": 0.874136336908857, + "velocityX": 3.369229005117782, + "velocityY": -1.1104501949452927, + "timestamp": 1.6900586877577428 + }, + { + "x": 7.0424486777806585, + "y": 1.0523256982782683, + "heading": -0.30458898646479193, + "angularVelocity": 0.8561908750664163, + "velocityX": 3.0625912189253857, + "velocityY": -1.0085643267152118, + "timestamp": 1.7641281580597195 + }, + { + "x": 7.2465905745815595, + "y": 0.9851213052520007, + "heading": -0.2456798424413219, + "angularVelocity": 0.7953228743678266, + "velocityX": 2.756086900157761, + "velocityY": -0.9073156963628867, + "timestamp": 1.838197628361696 + }, + { + "x": 7.428038548794458, + "y": 0.9253999088853845, + "heading": -0.1923064776337199, + "angularVelocity": 0.7205852099387569, + "velocityX": 2.449699902984963, + "velocityY": -0.8062889625528059, + "timestamp": 1.9122670986636727 + }, + { + "x": 7.586797949690042, + "y": 0.8731533407207944, + "heading": -0.14497999296920738, + "angularVelocity": 0.6389472541327177, + "velocityX": 2.1433851254549703, + "velocityY": -0.7053725097747339, + "timestamp": 1.9863365689656494 + }, + { + "x": 7.722872327084394, + "y": 0.8283767866857153, + "heading": -0.10400779573923787, + "angularVelocity": 0.5531590419497899, + "velocityX": 1.837118273420693, + "velocityY": -0.6045210510150557, + "timestamp": 2.060406039267626 + }, + { + "x": 7.836264194581955, + "y": 0.7910670998170554, + "heading": -0.06959570799723021, + "angularVelocity": 0.46459205934255987, + "velocityX": 1.5308853571555237, + "velocityY": -0.5037120788977025, + "timestamp": 2.1344755095696026 + }, + { + "x": 7.926975414267803, + "y": 0.7612220655404804, + "heading": -0.04189104317473832, + "angularVelocity": 0.3740362218001813, + "velocityX": 1.2246775806013535, + "velocityY": -0.4029330053920843, + "timestamp": 2.2085449798715793 + }, + { + "x": 7.99500740651227, + "y": 0.7388400306235591, + "heading": -0.021003806792755153, + "angularVelocity": 0.28199521741990785, + "velocityX": 0.9184889802384881, + "velocityY": -0.30217625191150127, + "timestamp": 2.282614450173556 + }, + { + "x": 8.040361272813582, + "y": 0.7239196956147146, + "heading": -0.007018297206773477, + "angularVelocity": 0.18881611450660732, + "velocityX": 0.6123152510259342, + "velocityY": -0.20143704211755917, + "timestamp": 2.3566839204755325 }, { "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": -6.370509592451438e-38, - "angularVelocity": 0.05640131838742408, - "velocityX": 0.2650511363527046, - "velocityY": -0.08679505477643516, - "timestamp": 2.476560598052597 + "heading": 2.0779165225884802e-41, + "angularVelocity": 0.09475290127174345, + "velocityX": 0.3061531209608937, + "velocityY": -0.10071229126619018, + "timestamp": 2.430753390777509 }, { "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": -1.0565058286352088e-38, - "angularVelocity": 7.429305530105007e-37, - "velocityX": -1.702944321634373e-36, - "velocityY": 1.0409042171025374e-38, - "timestamp": 2.548088195681112 + "heading": 1.169210048317657e-41, + "angularVelocity": -1.2268302369298258e-40, + "velocityX": -9.944733020002764e-42, + "velocityY": 5.460591898189625e-42, + "timestamp": 2.504822861079486 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.traj b/src/main/deploy/choreo/SourceLanePGHSprint.traj index 2e02badb..1c871ae2 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.traj @@ -3,1991 +3,2090 @@ { "x": 0.433241993188858, "y": 4.121134281158447, - "heading": 5.938255486131716e-34, + "heading": -6.349582694240188e-38, "angularVelocity": 0, - "velocityX": 2.0926991678208567e-34, - "velocityY": 4.74101269436352e-34, + "velocityX": -2.609211774957871e-36, + "velocityY": -5.016830702674688e-35, "timestamp": 0 }, { - "x": 0.45254673947106955, - "y": 4.110409875807789, - "heading": -0.00804589337615742, - "angularVelocity": -0.10676392342489109, - "velocityX": 0.2561617905500149, - "velocityY": -0.1423060856148132, - "timestamp": 0.0753615371002897 - }, - { - "x": 0.49115624056874074, - "y": 4.0889613071373185, - "heading": -0.024138137832086688, - "angularVelocity": -0.21353392028766638, - "velocityX": 0.5123236943308417, - "velocityY": -0.28460895963318283, - "timestamp": 0.1507230742005794 - }, - { - "x": 0.5490705891188512, - "y": 4.056788900712756, - "heading": -0.048273761664479664, - "angularVelocity": -0.32026448452442974, - "velocityX": 0.7684868273458795, - "velocityY": -0.4269075136000509, - "timestamp": 0.2260846113008691 - }, - { - "x": 0.6262899536624102, - "y": 4.013893070922723, - "heading": -0.08044715876351015, - "angularVelocity": -0.4269206592245423, - "velocityX": 1.024652196793663, - "velocityY": -0.5692005688916374, - "timestamp": 0.3014461484011588 - }, - { - "x": 0.722814567995576, - "y": 3.960274325008801, - "heading": -0.12065106662148206, - "angularVelocity": -0.5334804650344289, - "velocityX": 1.2808206685688042, - "velocityY": -0.7114868934078041, - "timestamp": 0.3768076855014485 - }, - { - "x": 0.8386447198033338, - "y": 3.895933264485532, - "heading": -0.16887761745268762, - "angularVelocity": -0.6399358702971596, - "velocityX": 1.5369929577420018, - "velocityY": -0.8537652362059028, - "timestamp": 0.4521692226017382 - }, - { - "x": 0.973780741976376, - "y": 3.8208705846111153, - "heading": -0.22511931827381823, - "angularVelocity": -0.7462918484038512, - "velocityX": 1.7931696641644363, - "velocityY": -0.99603435336682, - "timestamp": 0.5275307597020279 - }, - { - "x": 1.1282230140958218, - "y": 3.7350870801858624, - "heading": -0.289369793237253, - "angularVelocity": -0.8525632230395118, - "velocityX": 2.049351407388589, - "velocityY": -1.1382929240295867, - "timestamp": 0.6028922968023176 - }, - { - "x": 1.3019720073188241, - "y": 3.638583716526852, - "heading": -0.36162402966579443, - "angularVelocity": -0.9587680826147016, - "velocityX": 2.305539402570577, - "velocityY": -1.2805386855443919, - "timestamp": 0.6782538339026073 - }, - { - "x": 1.4950570361880926, - "y": 3.5314160759250663, - "heading": -0.44172046980509583, - "angularVelocity": -1.0628291728273882, - "velocityX": 2.56211638321966, - "velocityY": -1.4220469051629, - "timestamp": 0.753615371002897 - }, - { - "x": 1.668834943253154, - "y": 3.4349673750959555, - "heading": -0.5138143173751882, - "angularVelocity": -0.9566398237625026, - "velocityX": 2.30592307099311, - "velocityY": -1.279813344315926, - "timestamp": 0.8289769081031867 - }, - { - "x": 1.8233052433392802, - "y": 3.34923681430091, - "heading": -0.5779052248066122, - "angularVelocity": -0.8504458626704097, - "velocityX": 2.0497233208043544, - "velocityY": -1.1375903954952034, - "timestamp": 0.9043384452034764 - }, - { - "x": 1.958467560801758, - "y": 3.2742237758858077, - "heading": -0.6339913690664654, - "angularVelocity": -0.7442277110831049, - "velocityX": 1.7935185860474994, - "velocityY": -0.9953756425545938, - "timestamp": 0.9796999823037661 - }, - { - "x": 2.074321574453109, - "y": 3.2099277413643645, - "heading": -0.6820701443598719, - "angularVelocity": -0.6379749822435855, - "velocityX": 1.53730958933567, - "velocityY": -0.8531677695994906, - "timestamp": 1.0550615194040558 - }, - { - "x": 2.1708670101410656, - "y": 3.1563482775049274, - "heading": -0.7221386740225205, - "angularVelocity": -0.5316840818855123, - "velocityX": 1.281096954796391, - "velocityY": -0.7109656453547993, - "timestamp": 1.1304230565043456 - }, - { - "x": 2.2481036395899285, - "y": 3.1134850360999096, - "heading": -0.7541943378892257, - "angularVelocity": -0.4253584135902941, - "velocityX": 1.0248812911827636, - "velocityY": -0.5687681416048598, - "timestamp": 1.2057845936046352 - }, - { - "x": 2.306031279527139, - "y": 3.0813377577306427, - "heading": -0.7782353006154736, - "angularVelocity": -0.31900839143254983, - "velocityX": 0.7686631956580385, - "velocityY": -0.4265740801768146, - "timestamp": 1.2811461307049248 - }, - { - "x": 2.3446497902447807, - "y": 3.0599062760826214, - "heading": -0.7942609825639004, - "angularVelocity": -0.21265067785308137, - "velocityX": 0.5124432462974974, - "velocityY": -0.284382225637208, - "timestamp": 1.3565076678052144 + "x": 0.45469188032137553, + "y": 4.109218526997661, + "heading": -0.008937300145995164, + "angularVelocity": -0.11893237262991438, + "velocityX": 0.2854425752342375, + "velocityY": -0.1585678997982562, + "timestamp": 0.07514606787342622 + }, + { + "x": 0.4975916635116251, + "y": 4.0853873108981436, + "heading": -0.026812413296636577, + "angularVelocity": -0.2378715700833438, + "velocityX": 0.570885269240018, + "velocityY": -0.31713191087600345, + "timestamp": 0.15029213574685243 + }, + { + "x": 0.5619414537480725, + "y": 4.049641027140498, + "heading": -0.05362171709802473, + "angularVelocity": -0.35676256336585627, + "velocityX": 0.856329440215504, + "velocityY": -0.4756906750976657, + "timestamp": 0.22543820362027867 + }, + { + "x": 0.6477414523773106, + "y": 4.001980177860296, + "heading": -0.08935853104911506, + "angularVelocity": -0.4755646564406316, + "velocityX": 1.141776290593902, + "velocityY": -0.6342427571923069, + "timestamp": 0.30058427149370487 + }, + { + "x": 0.7549919358777414, + "y": 3.942405375853288, + "heading": -0.13401442200653152, + "angularVelocity": -0.5942545261667384, + "velocityX": 1.4272268201854585, + "velocityY": -0.7927866845588475, + "timestamp": 0.37573033936713107 + }, + { + "x": 0.8836932393269182, + "y": 3.8709173416225418, + "heading": -0.18758057158075755, + "angularVelocity": -0.7128270459134503, + "velocityX": 1.7126818087934743, + "velocityY": -0.9513210238912078, + "timestamp": 0.4508764072405573 + }, + { + "x": 1.0338457421402325, + "y": 3.7875168932975933, + "heading": -0.2500489995023069, + "angularVelocity": -0.8312933688928248, + "velocityX": 1.998141846440012, + "velocityY": -1.109844476033343, + "timestamp": 0.5260224751139835 + }, + { + "x": 1.2054498603190866, + "y": 3.692204931169301, + "heading": -0.32141344961465557, + "angularVelocity": -0.9496764385936038, + "velocityX": 2.283607419990343, + "velocityY": -1.2683559476303248, + "timestamp": 0.6011685429874097 + }, + { + "x": 1.3985060453231253, + "y": 3.5849824249792595, + "heading": -0.40166985798593763, + "angularVelocity": -1.0680054278616893, + "velocityX": 2.5690790013020615, + "velocityY": -1.4268545144723213, + "timestamp": 0.676314610860836 + }, + { + "x": 1.5915937569821006, + "y": 3.4778198154599647, + "heading": -0.48176859267038563, + "angularVelocity": -1.065907198489266, + "velocityX": 2.5694985396202825, + "velocityY": -1.4260574445464824, + "timestamp": 0.7514606787342623 + }, + { + "x": 1.7632284858900336, + "y": 3.382566036520044, + "heading": -0.5529765476804537, + "angularVelocity": -0.9475938931363465, + "velocityX": 2.2840147697019786, + "velocityY": -1.2675816797275796, + "timestamp": 0.8266067466076885 + }, + { + "x": 1.9134098098562766, + "y": 3.2992204007390877, + "heading": -0.6152918418017524, + "angularVelocity": -0.8292555536806094, + "velocityX": 1.9985253815170194, + "velocityY": -1.1091150626981792, + "timestamp": 0.9017528144811148 + }, + { + "x": 2.0421373577646693, + "y": 3.2277823059512354, + "heading": -0.6687115003742364, + "angularVelocity": -0.7108776291856339, + "velocityX": 1.7130310547348626, + "velocityY": -0.9506564589404816, + "timestamp": 0.976898882354541 + }, + { + "x": 2.149410808485792, + "y": 3.1682512394298143, + "heading": -0.713232041811307, + "angularVelocity": -0.5924533737687984, + "velocityX": 1.4275324545498613, + "velocityY": -0.792204678250007, + "timestamp": 1.0520449502279672 + }, + { + "x": 2.2352298936713018, + "y": 3.120626784362015, + "heading": -0.7488501306819736, + "angularVelocity": -0.4739847323836121, + "velocityX": 1.1420302833417786, + "velocityY": -0.6337584442610682, + "timestamp": 1.1271910181013933 + }, + { + "x": 2.2995943987852443, + "y": 3.0849086278583613, + "heading": -0.7755632624607879, + "angularVelocity": -0.3554827622359307, + "velocityX": 0.8565252572144717, + "velocityY": -0.4753163740226096, + "timestamp": 1.2023370859748195 + }, + { + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080857, + "angularVelocity": -0.23696688264902363, + "velocityX": 0.5710180813549187, + "velocityY": -0.3168769856057537, + "timestamp": 1.2774831538482456 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.1063066084363967, - "velocityX": 0.25622200022152375, - "velocityY": -0.14219129883360584, - "timestamp": 1.431869204905504 + "angularVelocity": -0.11846288599550474, + "velocityX": 0.2855094397331478, + "velocityY": -0.15843872456938407, + "timestamp": 1.3526292217216718 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": 1.280873526188021e-35, - "velocityY": 2.1400066509893793e-37, - "timestamp": 1.5072307420057935 - }, - { - "x": 2.374445038905671, - "y": 3.0395740293561597, - "heading": -0.8007582593302504, - "angularVelocity": 0.02507755763306408, - "velocityX": 0.17366966855318383, - "velocityY": -0.15926936399484595, - "timestamp": 1.5676095345297967 - }, - { - "x": 2.3955442849732433, - "y": 3.020481915034863, - "heading": -0.7977129883648791, - "angularVelocity": 0.050436102447073194, - "velocityX": 0.349447963193112, - "velocityY": -0.3162056331899431, - "timestamp": 1.6279883270537998 - }, - { - "x": 2.4273985359193944, - "y": 2.992075542224587, - "heading": -0.7931177380351067, - "angularVelocity": 0.07610702595527519, - "velocityX": 0.527573500803075, - "velocityY": -0.4704693754679464, - "timestamp": 1.688367119577803 - }, - { - "x": 2.470166324344177, - "y": 2.9545414012529414, - "heading": -0.7869514399590605, - "angularVelocity": 0.10212688625057974, - "velocityX": 0.7083246722395268, - "velocityY": -0.6216444450545164, - "timestamp": 1.748745912101806 - }, - { - "x": 2.524025883283138, - "y": 2.908097185821786, - "heading": -0.7791904398300458, - "angularVelocity": 0.12853851169563282, - "velocityX": 0.8920277582157603, - "velocityY": -0.7692140483381156, - "timestamp": 1.8091247046258092 - }, - { - "x": 2.589178616513917, - "y": 2.8529998964635936, - "heading": -0.7698080390828491, - "angularVelocity": 0.15539232162463004, - "velocityX": 1.0790665150330332, - "velocityY": -0.9125271813988101, - "timestamp": 1.8695034971498123 - }, - { - "x": 2.6658532194281834, - "y": 2.7895568383610723, - "heading": -0.758773944440501, - "angularVelocity": 0.18274785203698177, - "velocityX": 1.269892949313043, - "velocityY": -1.050750693255423, - "timestamp": 1.9298822896738155 - }, - { - "x": 2.7543104503881763, - "y": 2.7181408322396914, - "heading": -0.7460536194222877, - "angularVelocity": 0.21067537932555425, - "velocityX": 1.4650380913932193, - "velocityY": -1.1827995085027596, - "timestamp": 1.9902610821978186 - }, - { - "x": 2.8548483465105847, - "y": 2.6392116728155934, - "heading": -0.7316075530041294, - "angularVelocity": 0.23925729240801266, - "velocityX": 1.6651193559798465, - "velocityY": -1.3072331546332332, - "timestamp": 2.0506398747218215 - }, - { - "x": 2.9678071516801605, - "y": 2.5533470022723117, - "heading": -0.7153905179087263, - "angularVelocity": 0.2685882644797192, - "velocityX": 1.870835776066071, - "velocityY": -1.4220998293257756, - "timestamp": 2.1110186672458244 - }, - { - "x": 3.093571941086637, - "y": 2.4612875031660972, - "heading": -0.6973510365861332, - "angularVelocity": 0.2987718132226927, - "velocityX": 2.0829298525054067, - "velocityY": -1.5246992405424025, - "timestamp": 2.1713974597698273 - }, - { - "x": 3.2325678737819445, - "y": 2.364003698472316, - "heading": -0.6774316325200306, - "angularVelocity": 0.3299072941576924, - "velocityX": 2.302065458497713, - "velocityY": -1.611224746753705, - "timestamp": 2.2317762522938303 - }, - { - "x": 3.3852359851566844, - "y": 2.2627937385026136, - "heading": -0.6555713107834819, - "angularVelocity": 0.36205297957653465, - "velocityX": 2.528505539656955, - "velocityY": -1.6762501490811843, - "timestamp": 2.292155044817833 - }, - { - "x": 3.551962630271022, - "y": 2.159418364676261, - "heading": -0.6317136848324263, - "angularVelocity": 0.39513254495063227, - "velocityX": 2.761344474519871, - "velocityY": -1.7121139642740746, - "timestamp": 2.352533837341836 - }, - { - "x": 3.7329118909154024, - "y": 2.0562532945853267, - "heading": -0.6058279711502043, - "angularVelocity": 0.4287219502101063, - "velocityX": 2.9969009494922476, - "velocityY": -1.7086308913832926, - "timestamp": 2.412912629865839 - }, - { - "x": 3.927705999512227, - "y": 1.9563533988353388, - "heading": -0.5779537605481534, - "angularVelocity": 0.46165564823062205, - "velocityX": 3.2262007975629157, - "velocityY": -1.6545527257815482, - "timestamp": 2.473291422389842 - }, - { - "x": 4.135029249104847, - "y": 1.8631919103127867, - "heading": -0.5482681931936219, - "angularVelocity": 0.491655531910981, - "velocityX": 3.433709766723148, - "velocityY": -1.5429505067614013, - "timestamp": 2.533670214913845 - }, - { - "x": 4.35253951782844, - "y": 1.7799641253131642, - "heading": -0.5171193508091871, - "angularVelocity": 0.5158904489859046, - "velocityX": 3.602428263816689, - "velocityY": -1.3784274497794216, - "timestamp": 2.5940490074378477 - }, - { - "x": 4.577376652915107, - "y": 1.7089361045956586, - "heading": -0.4849529126600114, - "angularVelocity": 0.5327439785482329, - "velocityX": 3.7237766057889528, - "velocityY": -1.1763736528727222, - "timestamp": 2.6544277999618506 - }, - { - "x": 4.806831379756707, - "y": 1.6513703939365607, - "heading": -0.452194444563849, - "angularVelocity": 0.5425492416586429, - "velocityX": 3.8002536528099977, - "velocityY": -0.9534094381932716, - "timestamp": 2.7148065924858535 - }, - { - "x": 5.0386739792276956, - "y": 1.6078357362088507, - "heading": -0.4191901676167688, - "angularVelocity": 0.5466203540582497, - "velocityX": 3.8398018539178493, - "velocityY": -0.7210256434062197, - "timestamp": 2.7751853850098565 - }, - { - "x": 5.271173472263796, - "y": 1.578518825120004, - "heading": -0.38620445457775493, - "angularVelocity": 0.5463128966333781, - "velocityX": 3.8506813951880883, - "velocityY": -0.4855498075287272, - "timestamp": 2.8355641775338594 + "angularVelocity": 3.467177167133168e-32, + "velocityX": -1.439882477346126e-32, + "velocityY": -1.3815367235364851e-32, + "timestamp": 1.427775289595098 + }, + { + "x": 2.3773546785315864, + "y": 3.0407954088560687, + "heading": -0.7860770412156382, + "angularVelocity": 0.2650004205849919, + "velocityX": 0.21918861143707746, + "velocityY": -0.13736692695017716, + "timestamp": 1.4888897992558612 + }, + { + "x": 2.4041715283150715, + "y": 3.0239975454455843, + "heading": -0.75411979281041, + "angularVelocity": 0.5229077118120985, + "velocityX": 0.438796775632191, + "velocityY": -0.2748588429118795, + "timestamp": 1.5500043089166244 + }, + { + "x": 2.4444388170293236, + "y": 2.998788446329585, + "heading": -0.7069088657797715, + "angularVelocity": 0.772499481591177, + "velocityX": 0.6588826276733534, + "velocityY": -0.41248959135778174, + "timestamp": 1.6111188185773877 + }, + { + "x": 2.498190798561039, + "y": 2.965157152573787, + "heading": -0.6450804787000075, + "angularVelocity": 1.0116809808826641, + "velocityX": 0.8795289666903056, + "velocityY": -0.5502996578468837, + "timestamp": 1.672233328238151 + }, + { + "x": 2.5654677170302604, + "y": 2.923088008476279, + "heading": -0.5694581806203519, + "angularVelocity": 1.237386972413308, + "velocityX": 1.1008338092322851, + "velocityY": -0.6883658943027919, + "timestamp": 1.7333478378989142 + }, + { + "x": 2.6463157759141644, + "y": 2.8725583813996622, + "heading": -0.4811095285927534, + "angularVelocity": 1.445624819997864, + "velocityX": 1.3228946666295571, + "velocityY": -0.8268024624119354, + "timestamp": 1.7944623475596775 + }, + { + "x": 2.7407862870930626, + "y": 2.813537598490058, + "heading": -0.38139476987658943, + "angularVelocity": 1.6316053138553275, + "velocityX": 1.5457951262848801, + "velocityY": -0.9657409220366661, + "timestamp": 1.8555768572204407 + }, + { + "x": 2.84893500974016, + "y": 2.7459886359527177, + "heading": -0.27202745092011676, + "angularVelocity": 1.7895475160244771, + "velocityX": 1.7696079580350594, + "velocityY": -1.105285191884772, + "timestamp": 1.916691366881204 + }, + { + "x": 2.970822636317052, + "y": 2.669873093439706, + "heading": -0.15523119808259794, + "angularVelocity": 1.9111051284848073, + "velocityX": 1.9944138839282306, + "velocityY": -1.245457796119401, + "timestamp": 1.9778058765419673 + }, + { + "x": 3.106512263881207, + "y": 2.5851587340080315, + "heading": -0.03419676462946541, + "angularVelocity": 1.980453318286855, + "velocityX": 2.220252249708728, + "velocityY": -1.3861578846318265, + "timestamp": 2.0389203862027303 + }, + { + "x": 3.256035657144846, + "y": 2.4918371927335334, + "heading": 0.08572826850512172, + "angularVelocity": 1.962300504418209, + "velocityX": 2.4466103727841246, + "velocityY": -1.5269948461095477, + "timestamp": 2.1000348958634936 + }, + { + "x": 3.419180860242083, + "y": 2.390029027608911, + "heading": 0.19415018142326176, + "angularVelocity": 1.7740780956923756, + "velocityX": 2.6695003200194134, + "velocityY": -1.6658591501387068, + "timestamp": 2.161149405524257 + }, + { + "x": 3.5929644641848295, + "y": 2.279455345020229, + "heading": 0.26705766200125053, + "angularVelocity": 1.1929651564364412, + "velocityX": 2.8435735622750062, + "velocityY": -1.8092869140644157, + "timestamp": 2.22226391518502 + }, + { + "x": 3.7768655464749616, + "y": 2.1600020280366277, + "heading": 0.30152237044830693, + "angularVelocity": 0.5639365944088304, + "velocityX": 3.0091230922237155, + "velocityY": -1.9545819421061743, + "timestamp": 2.2833784248457833 + }, + { + "x": 3.9728967066647076, + "y": 2.0375522576737963, + "heading": 0.30152241765502197, + "angularVelocity": 7.724305624896446e-7, + "velocityX": 3.2076042379769354, + "velocityY": -2.003612088889046, + "timestamp": 2.3444929345065466 + }, + { + "x": 4.174386727718979, + "y": 1.9243094504352285, + "heading": 0.30152246485493484, + "angularVelocity": 7.723192600121745e-7, + "velocityX": 3.2969260847008353, + "velocityY": -1.85296107040964, + "timestamp": 2.40560744416731 + }, + { + "x": 4.383354452565466, + "y": 1.8255438101811146, + "heading": 0.30152251288342946, + "angularVelocity": 7.858771165145409e-7, + "velocityX": 3.419281705873656, + "velocityY": -1.6160751481496927, + "timestamp": 2.466721953828073 + }, + { + "x": 4.598761540028333, + "y": 1.7417467464566943, + "heading": 0.30152256294147634, + "angularVelocity": 8.190861247953373e-7, + "velocityX": 3.5246472344874826, + "velocityY": -1.3711484259558735, + "timestamp": 2.5278364634888364 + }, + { + "x": 4.819537390717514, + "y": 1.6733348748555603, + "heading": 0.30152261646908723, + "angularVelocity": 8.758576510857166e-7, + "velocityX": 3.612494838208991, + "velocityY": -1.1194047367945392, + "timestamp": 2.5889509731495997 + }, + { + "x": 5.04458467733647, + "y": 1.620648272623701, + "heading": 0.30152267534909943, + "angularVelocity": 9.634375304093619e-7, + "velocityX": 3.682387175617667, + "velocityY": -0.8620964567058491, + "timestamp": 2.650065482810363 + }, + { + "x": 5.27278482830379, + "y": 1.5839488347993986, + "heading": 0.301522742248914, + "angularVelocity": 0.000001094663361600596, + "velocityX": 3.7339766322927477, + "velocityY": -0.6005028597630109, + "timestamp": 2.711179992471126 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.35343685387939117, - "angularVelocity": 0.5427004967901151, - "velocityX": 3.839595250328378, - "velocityY": -0.2500851718896869, - "timestamp": 2.8959429700578623 - }, - { - "x": 5.773415110681756, - "y": 1.565214819314658, - "heading": -0.31539772557234425, - "angularVelocity": 0.5354796131957132, - "velocityX": 3.8066028075669824, - "velocityY": 0.025280100286796007, - "timestamp": 2.9669804612520303 - }, - { - "x": 6.038838987490814, - "y": 1.5860691706810086, - "heading": -0.2782614638760959, - "angularVelocity": 0.5227698933615667, - "velocityX": 3.7363914793045105, - "velocityY": 0.2935682414423857, - "timestamp": 3.0380179524461983 - }, - { - "x": 6.296044587353018, - "y": 1.6248260837737782, - "heading": -0.24248714943667204, - "angularVelocity": 0.5035976614326285, - "velocityX": 3.620702188921338, - "velocityY": 0.5455839225351394, - "timestamp": 3.1090554436403663 - }, - { - "x": 6.541409154187439, - "y": 1.6793226331219655, - "heading": -0.2085611266677552, - "angularVelocity": 0.4775791233418755, - "velocityX": 3.4540150941389784, - "velocityY": 0.7671519423346602, - "timestamp": 3.1800929348345344 - }, - { - "x": 6.77147442693619, - "y": 1.7462193726308344, - "heading": -0.17691445924041233, - "angularVelocity": 0.44549247017807186, - "velocityX": 3.238645803522406, - "velocityY": 0.9417103332945568, - "timestamp": 3.2511304260287024 - }, - { - "x": 6.983655507583721, - "y": 1.8213558866056687, - "heading": -0.14784470725521442, - "angularVelocity": 0.40921704154417726, - "velocityX": 2.986888712997661, - "velocityY": 1.0577022458389254, - "timestamp": 3.3221679172228704 - }, - { - "x": 7.176562959829929, - "y": 1.900519098632156, - "heading": -0.12149718500073287, - "angularVelocity": 0.37089601295836144, - "velocityX": 2.7155724252554383, - "velocityY": 1.114386371136183, - "timestamp": 3.3932054084170384 - }, - { - "x": 7.34978445026805, - "y": 1.9800560342439697, - "heading": -0.09790193374425028, - "angularVelocity": 0.33215209123854533, - "velocityX": 2.4384516897514468, - "velocityY": 1.119647305595492, - "timestamp": 3.4642428996112065 - }, - { - "x": 7.503478866038821, - "y": 2.0570663546440358, - "heading": -0.07702399456491762, - "angularVelocity": 0.2939002888244837, - "velocityX": 2.163567620239804, - "velocityY": 1.0840799570127342, - "timestamp": 3.5352803908053745 - }, - { - "x": 7.6380640040828975, - "y": 2.1293280640018195, - "heading": -0.058799951231258585, - "angularVelocity": 0.2565412013755806, - "velocityX": 1.8945649090592562, - "velocityY": 1.0172334093312752, - "timestamp": 3.6063178819995425 - }, - { - "x": 7.754041712551309, - "y": 2.1951475174693176, - "heading": -0.04315811189523446, - "angularVelocity": 0.220191325356213, - "velocityX": 1.6326267512940102, - "velocityY": 0.9265452982790828, - "timestamp": 3.6773553731937105 - }, - { - "x": 7.851915110788357, - "y": 2.2532212040315036, - "heading": -0.030028067927435956, - "angularVelocity": 0.18483259680314387, - "velocityX": 1.377771041625462, - "velocityY": 0.8175075665813204, - "timestamp": 3.7483928643878786 - }, - { - "x": 7.932154394774834, - "y": 2.302530059896823, - "heading": -0.01934465130254991, - "angularVelocity": 0.15039124334620582, - "velocityX": 1.1295343154385586, - "velocityY": 0.6941243987705448, - "timestamp": 3.8194303555820466 - }, - { - "x": 7.995185674411745, - "y": 2.3422635628439994, - "heading": -0.011049205592550179, - "angularVelocity": 0.1167756007503934, - "velocityX": 0.887295969738388, - "velocityY": 0.5593314499039962, - "timestamp": 3.8904678467762146 - }, - { - "x": 8.04138998965963, - "y": 2.3717661278123887, - "heading": -0.005089649909509545, - "angularVelocity": 0.08389310465302457, - "velocityX": 0.6504215516506965, - "velocityY": 0.41530978181295497, - "timestamp": 3.9615053379703826 - }, - { - "x": 8.071106442714182, - "y": 2.39049915084481, - "heading": -0.0014200461836977572, - "angularVelocity": 0.051657282149528694, - "velocityX": 0.418320700168416, - "velocityY": 0.26370614611400395, - "timestamp": 4.032542829164551 - }, - { - "x": 8.084636688232422, - "y": 2.3980138301849365, - "heading": -1.2697794666383574e-33, - "angularVelocity": 0.01999009480523916, - "velocityX": 0.19046626352917562, - "velocityY": 0.10578469500825122, - "timestamp": 4.103580320358718 - }, - { - "x": 8.081561182876898, - "y": 2.39334444963861, - "heading": -0.0009066648087102295, - "angularVelocity": -0.012314301117252947, - "velocityX": -0.041771444829232424, - "velocityY": -0.06341942195849094, - "timestamp": 4.177207300897626 - }, - { - "x": 8.061093542733351, - "y": 2.376630216741082, - "heading": -0.004227711790797553, - "angularVelocity": -0.045106385699632864, - "velocityX": -0.2779910298335672, - "velocityY": -0.22701233671663687, - "timestamp": 4.250834281436534 - }, - { - "x": 8.02290518913695, - "y": 2.348360537804544, - "heading": -0.01000387000903876, - "angularVelocity": -0.07845165150007544, - "velocityX": -0.5186733629015444, - "velocityY": -0.38395814590820065, - "timestamp": 4.324461261975442 - }, - { - "x": 7.966627275516003, - "y": 2.3091224913101702, - "heading": -0.01828119946614907, - "angularVelocity": -0.11242250322537992, - "velocityX": -0.7643653618418706, - "velocityY": -0.5329302683224771, - "timestamp": 4.3980882425143495 - }, - { - "x": 7.891846094053721, - "y": 2.2596302081809454, - "heading": -0.029111517424954568, - "angularVelocity": -0.1470971358533753, - "velocityX": -1.0156763310803423, - "velocityY": -0.6722030805415261, - "timestamp": 4.471715223053257 - }, - { - "x": 7.798100115514375, - "y": 2.2007657686480786, - "heading": -0.04255255372711989, - "angularVelocity": -0.18255585389737042, - "velocityX": -1.27325578005752, - "velocityY": -0.7994954988241099, - "timestamp": 4.545342203592165 - }, - { - "x": 7.684881725302932, - "y": 2.1336367022021943, - "heading": -0.058667438591618416, - "angularVelocity": -0.21887200516097166, - "velocityX": -1.5377296390908446, - "velocityY": -0.9117454763802326, - "timestamp": 4.618969184131073 - }, - { - "x": 7.5516502833969605, - "y": 2.0596568999010922, - "heading": -0.07752270936377831, - "angularVelocity": -0.25609186515799065, - "velocityX": -1.8095464587953205, - "velocityY": -1.0047920172688334, - "timestamp": 4.692596164669981 - }, - { - "x": 7.397870326337049, - "y": 1.98065825109379, - "heading": -0.09918321696192219, - "angularVelocity": -0.2941925288746233, - "velocityX": -2.0886359312079446, - "velocityY": -1.0729578780642273, - "timestamp": 4.766223145208889 - }, - { - "x": 7.2231018276798915, - "y": 1.8990346142555161, - "heading": -0.12370090160049274, - "angularVelocity": -0.33299864342004915, - "velocityX": -2.3737018329144046, - "velocityY": -1.1086104066856408, - "timestamp": 4.839850125747796 - }, - { - "x": 7.027186991114378, - "y": 1.8178941660655976, - "heading": -0.1510925803805667, - "angularVelocity": -0.37203316745549647, - "velocityX": -2.660910920582776, - "velocityY": -1.102047749289958, - "timestamp": 4.913477106286704 - }, - { - "x": 6.8105769237451845, - "y": 1.741127234048135, - "heading": -0.18130204311682424, - "angularVelocity": -0.4103042460133704, - "velocityX": -2.941993081662873, - "velocityY": -1.0426467506282693, - "timestamp": 4.987104086825612 - }, - { - "x": 6.574746676835865, - "y": 1.6731958946569394, - "heading": -0.21415137217886637, - "angularVelocity": -0.4461588513015718, - "velocityX": -3.2030411295312824, - "velocityY": -0.9226419295477921, - "timestamp": 5.06073106736452 - }, - { - "x": 6.322411514317352, - "y": 1.6185000003009695, - "heading": -0.24931002488043913, - "angularVelocity": -0.47752403323118087, - "velocityX": -3.427210523527703, - "velocityY": -0.7428784116315408, - "timestamp": 5.134358047903428 - }, - { - "x": 6.057197180213997, - "y": 1.5805921161213812, - "heading": -0.28632018217818705, - "angularVelocity": -0.5026711271717876, - "velocityX": -3.602135143423469, - "velocityY": -0.5148640335665581, - "timestamp": 5.2079850284423355 - }, - { - "x": 5.782918806315175, - "y": 1.5618062942255864, - "heading": -0.3246756429318106, - "angularVelocity": -0.5209430085667437, - "velocityX": -3.725242728837456, - "velocityY": -0.25514861207526496, - "timestamp": 5.281612008981243 + "heading": 0.3015228161300743, + "angularVelocity": 0.0000012088972110609183, + "velocityX": 3.7670067261218194, + "velocityY": -0.33592432468507777, + "timestamp": 2.7722945021318894 + }, + { + "x": 5.642161233224445, + "y": 1.5569512204704454, + "heading": 0.30152289610635313, + "angularVelocity": 0.0000021712102282875206, + "velocityX": 3.7778762267369546, + "velocityY": -0.17558800471026415, + "timestamp": 2.809129387028348 + }, + { + "x": 5.781468008961295, + "y": 1.556401100893367, + "heading": 0.30152296986307525, + "angularVelocity": 0.0000020023605969229892, + "velocityX": 3.781925099764402, + "velocityY": -0.014934744023907952, + "timestamp": 2.8459642719248066 + }, + { + "x": 5.920672391336217, + "y": 1.5617696165699877, + "heading": 0.3015230386365787, + "angularVelocity": 0.0000018670752912406278, + "velocityX": 3.779145306581512, + "velocityY": 0.14574541746802047, + "timestamp": 2.882799156821265 + }, + { + "x": 6.0595230325638285, + "y": 1.5730470711005764, + "heading": 0.3015231034083751, + "angularVelocity": 0.0000017584362380693717, + "velocityX": 3.769541879061509, + "velocityY": 0.30616233937716997, + "timestamp": 2.919634041717724 + }, + { + "x": 6.197769224123459, + "y": 1.5902130982406153, + "heading": 0.3015231649742209, + "angularVelocity": 0.000001671400517937078, + "velocityX": 3.753132172076425, + "velocityY": 0.46602635486148125, + "timestamp": 2.9564689266141824 + }, + { + "x": 6.335161349542959, + "y": 1.6132366986444322, + "heading": 0.3015232239953199, + "angularVelocity": 0.0000016023152822250254, + "velocityX": 3.72994583275345, + "velocityY": 0.6250487946015117, + "timestamp": 2.993303811510641 + }, + { + "x": 6.471451335210211, + "y": 1.6420762959087598, + "heading": 0.30152328103508963, + "angularVelocity": 0.000001548525804202316, + "velocityX": 3.7000247469309935, + "velocityY": 0.7829425107583317, + "timestamp": 3.0301386964070995 + }, + { + "x": 6.606393098338155, + "y": 1.6766798119201127, + "heading": 0.301523336612691, + "angularVelocity": 0.000001508830595721816, + "velocityX": 3.663422961881362, + "velocityY": 0.9394224010369989, + "timestamp": 3.066973581303558 + }, + { + "x": 6.739742463422568, + "y": 1.716984614087568, + "heading": 0.3015255656708468, + "angularVelocity": 0.00006051486687697733, + "velocityX": 3.6201922568579414, + "velocityY": 1.094201930608725, + "timestamp": 3.1038084662000167 + }, + { + "x": 6.8694207904091344, + "y": 1.7614703171513673, + "heading": 0.3101205256410531, + "angularVelocity": 0.23333750042565204, + "velocityX": 3.520530262306672, + "velocityY": 1.207705771006116, + "timestamp": 3.1406433510964753 + }, + { + "x": 6.994764102779472, + "y": 1.808693107878132, + "heading": 0.3306180633638746, + "angularVelocity": 0.5564707961064413, + "velocityX": 3.4028425152588873, + "velocityY": 1.282012713206686, + "timestamp": 3.177478235992934 + }, + { + "x": 7.114597005425528, + "y": 1.856512920318317, + "heading": 0.35962771339750194, + "angularVelocity": 0.7875591335542964, + "velocityX": 3.253244932973218, + "velocityY": 1.2982207647615676, + "timestamp": 3.2143131208893925 + }, + { + "x": 7.228563598795877, + "y": 1.9039637614742513, + "heading": 0.39146295222083616, + "angularVelocity": 0.8642687200685354, + "velocityX": 3.093985326429113, + "velocityY": 1.2882038667778264, + "timestamp": 3.251148005785851 + }, + { + "x": 7.336676614220082, + "y": 1.9505339221399116, + "heading": 0.42341393247502157, + "angularVelocity": 0.8674108889982484, + "velocityX": 2.935071352282109, + "velocityY": 1.2642949963483496, + "timestamp": 3.2879828906823096 + }, + { + "x": 7.438996464546078, + "y": 1.995939510716552, + "heading": 0.45395278545537515, + "angularVelocity": 0.8290742068611583, + "velocityX": 2.77779747686502, + "velocityY": 1.2326789863542051, + "timestamp": 3.3248177755787682 + }, + { + "x": 7.535582073954796, + "y": 2.0400031624519186, + "heading": 0.48208291467910164, + "angularVelocity": 0.7636817463336474, + "velocityX": 2.6221232855814667, + "velocityY": 1.196247846551653, + "timestamp": 3.361652660475227 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6791276743107281, + "velocityX": 2.4678193928677437, + "velocityY": 1.1565397180800703, + "timestamp": 3.3984875453716854 + }, + { + "x": 7.766333855095899, + "y": 2.1513911601167894, + "heading": 0.5393931714770351, + "angularVelocity": 0.5089507306836832, + "velocityX": 2.2039777611896034, + "velocityY": 1.0840548028640598, + "timestamp": 3.461940970502476 + }, + { + "x": 7.8897168213730975, + "y": 2.214535022901093, + "heading": 0.5621690879226864, + "angularVelocity": 0.3589391179231937, + "velocityX": 1.9444650312080196, + "velocityY": 0.9951214241650669, + "timestamp": 3.5253943956332665 + }, + { + "x": 7.996856912991746, + "y": 2.271342778200504, + "heading": 0.5763089063837279, + "angularVelocity": 0.2228377496076287, + "velocityX": 1.6884839769927567, + "velocityY": 0.8952669644281369, + "timestamp": 3.588847820764057 + }, + { + "x": 8.08793407853681, + "y": 2.3213235528188796, + "heading": 0.5824533246990756, + "angularVelocity": 0.09683351690917834, + "velocityX": 1.4353388387992245, + "velocityY": 0.7876765441007412, + "timestamp": 3.6523012458948476 + }, + { + "x": 8.16309393881241, + "y": 2.364112462024999, + "heading": 0.5810872026138336, + "angularVelocity": -0.021529524725041504, + "velocityX": 1.1844886248564517, + "velocityY": 0.6743356898059124, + "timestamp": 3.715754671025638 + }, + { + "x": 8.222455939995415, + "y": 2.399428067130867, + "heading": 0.5725898810376202, + "angularVelocity": -0.13391430893917436, + "velocityX": 0.9355208337555891, + "velocityY": 0.5565594770191685, + "timestamp": 3.7792080961564287 + }, + { + "x": 8.266119462815807, + "y": 2.4270468912932266, + "heading": 0.5572661381126698, + "angularVelocity": -0.24149591441856846, + "velocityX": 0.6881192422693254, + "velocityY": 0.43526136068197446, + "timestamp": 3.8426615212872193 + }, + { + "x": 8.294168308263885, + "y": 2.4467872814650136, + "heading": 0.5353660980042331, + "angularVelocity": -0.3451356654632318, + "velocityX": 0.4420383200790668, + "velocityY": 0.3111004666981787, + "timestamp": 3.90611494641801 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.44548570157766576, + "velocityX": 0.19708463824313602, + "velocityY": 0.18456741878341876, + "timestamp": 3.9695683715488004 + }, + { + "x": 8.303024530407173, + "y": 2.461881127582199, + "heading": 0.47119103232326837, + "angularVelocity": -0.5465136547157349, + "velocityX": -0.05554517814981655, + "velocityY": 0.05148048067312068, + "timestamp": 4.035271161880262 + }, + { + "x": 8.282750419132642, + "y": 2.456558519757899, + "heading": 0.4288756816904664, + "angularVelocity": -0.6440419108431604, + "velocityX": -0.3085730632177365, + "velocityY": -0.08101037714605593, + "timestamp": 4.1009739522117235 + }, + { + "x": 8.245821395629305, + "y": 2.442576926285189, + "heading": 0.3804195474569323, + "angularVelocity": -0.7375049672788585, + "velocityX": -0.5620617224479442, + "velocityY": -0.21280060408660287, + "timestamp": 4.166676742543185 + }, + { + "x": 8.19220201387014, + "y": 2.4199912479232255, + "heading": 0.32613641295591117, + "angularVelocity": -0.8261922245184762, + "velocityX": -0.8160898721144456, + "velocityY": -0.34375523852216117, + "timestamp": 4.2323795328746465 + }, + { + "x": 8.121850214183777, + "y": 2.388868090056124, + "heading": 0.2664003971864745, + "angularVelocity": -0.909185370485436, + "velocityX": -1.070758172239708, + "velocityY": -0.4736961354318384, + "timestamp": 4.298082323206108 + }, + { + "x": 8.034715295558529, + "y": 2.349289919990928, + "heading": 0.20166644793914118, + "angularVelocity": -0.9852541866298153, + "velocityX": -1.3261981444876794, + "velocityY": -0.6023818754961501, + "timestamp": 4.36378511353757 + }, + { + "x": 7.9307349853362235, + "y": 2.301361396168203, + "heading": 0.1325025874209066, + "angularVelocity": -1.052677674255729, + "velocityX": -1.5825859099398853, + "velocityY": -0.7294747084702512, + "timestamp": 4.429487903869031 + }, + { + "x": 7.809831081747403, + "y": 2.2452194829459002, + "heading": 0.0596429970770519, + "angularVelocity": -1.1089268808263426, + "velocityX": -1.8401639105261376, + "velocityY": -0.8544829365552749, + "timestamp": 4.495190694200493 + }, + { + "x": 7.671902780233127, + "y": 2.1810506699963903, + "heading": -0.015920256793080897, + "angularVelocity": -1.1500767850029918, + "velocityX": -2.099276161917122, + "velocityY": -0.9766527818040505, + "timestamp": 4.560893484531954 + }, + { + "x": 7.516816185598606, + "y": 2.109122861945235, + "heading": -0.09276870443286889, + "angularVelocity": -1.1696375032490751, + "velocityX": -2.3604263053689314, + "velocityY": -1.0947451042534069, + "timestamp": 4.626596274863416 + }, + { + "x": 7.344387868807497, + "y": 2.02985165560778, + "heading": -0.16871929267657412, + "angularVelocity": -1.1559720349858067, + "velocityX": -2.6243682486121434, + "velocityY": -1.206512020837212, + "timestamp": 4.692299065194877 + }, + { + "x": 7.154363408340053, + "y": 1.9439629823466762, + "heading": -0.2400485240661188, + "angularVelocity": -1.0856347352935642, + "velocityX": -2.8921825010596516, + "velocityY": -1.3072302230667405, + "timestamp": 4.758001855526339 + }, + { + "x": 6.9464441838510815, + "y": 1.8530162365161489, + "heading": -0.2992772419214471, + "angularVelocity": -0.9014642689682996, + "velocityX": -3.1645417712101533, + "velocityY": -1.3842143594162994, + "timestamp": 4.8237046458578 + }, + { + "x": 6.72212326982476, + "y": 1.7626751908782685, + "heading": -0.31855321839260475, + "angularVelocity": -0.2933813978662556, + "velocityX": -3.4141763674670904, + "velocityY": -1.3749955699312393, + "timestamp": 4.889407436189262 + }, + { + "x": 6.485759745047901, + "y": 1.6860130581467425, + "heading": -0.3185532921212082, + "angularVelocity": -0.0000011221533066716862, + "velocityX": -3.597465550312819, + "velocityY": -1.1668017803319484, + "timestamp": 4.955110226520723 + }, + { + "x": 6.24426876944993, + "y": 1.6274732096370486, + "heading": -0.3185533177664736, + "angularVelocity": -3.9032231775904613e-7, + "velocityX": -3.675505627381763, + "velocityY": -0.8909796405048926, + "timestamp": 5.020813016852185 + }, + { + "x": 5.999037658914315, + "y": 1.5873919634602989, + "heading": -0.3185533462426243, + "angularVelocity": -4.334085446884588e-7, + "velocityX": -3.732430682143934, + "velocityY": -0.6100387209515062, + "timestamp": 5.0865158071836465 + }, + { + "x": 5.751475205523387, + "y": 1.5659995878977724, + "heading": -0.3185533789943251, + "angularVelocity": -4.984826463431063e-7, + "velocityX": -3.7679138456983283, + "velocityY": -0.325593105781428, + "timestamp": 5.152218597515108 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.3639012008110169, - "angularVelocity": -0.5327606482310914, - "velocityX": -3.8018020976391327, - "velocityY": 0.021903521993656888, - "timestamp": 5.355238989520151 - }, - { - "x": 5.261378018303958, - "y": 1.580163952485418, - "heading": -0.3978140479254385, - "angularVelocity": -0.5381806191653729, - "velocityX": -3.83448205483159, - "velocityY": 0.265734612450363, - "timestamp": 5.418252866101241 - }, - { - "x": 5.01887444525289, - "y": 1.6123863305278272, - "heading": -0.4318947982287382, - "angularVelocity": -0.5408451622468005, - "velocityX": -3.848415400042849, - "velocityY": 0.5113536857384613, - "timestamp": 5.481266742682331 - }, - { - "x": 4.777050990194419, - "y": 1.6600961195625756, - "heading": -0.4659121119973733, - "angularVelocity": -0.5398384548657204, - "velocityX": -3.8376222536837075, - "velocityY": 0.7571314704524225, - "timestamp": 5.5442806192634215 - }, - { - "x": 4.5379789584596235, - "y": 1.7230618675308726, - "heading": -0.49955411376200026, - "angularVelocity": -0.5338824333610737, - "velocityX": -3.793958485114657, - "velocityY": 0.9992362219973634, - "timestamp": 5.607294495844512 - }, - { - "x": 4.3043600249129454, - "y": 1.8005379950644207, - "heading": -0.5324078746922887, - "angularVelocity": -0.5213734293590093, - "velocityX": -3.7074204321653204, - "velocityY": 1.2295089865459496, - "timestamp": 5.670308372425602 - }, - { - "x": 4.07944895912211, - "y": 1.8908360479698116, - "heading": -0.5639729512662652, - "angularVelocity": -0.5009226266750401, - "velocityX": -3.5692307471578024, - "velocityY": 1.4329867928247444, - "timestamp": 5.733322249006692 - }, - { - "x": 3.866497717212655, - "y": 1.9909883194289872, - "heading": -0.5937624584033184, - "angularVelocity": -0.47274519127097403, - "velocityX": -3.37943407807034, - "velocityY": 1.5893685152078068, - "timestamp": 5.796336125587782 - }, - { - "x": 3.667854490580529, - "y": 2.0970860288082087, - "heading": -0.6214571487340739, - "angularVelocity": -0.4395014532254029, - "velocityX": -3.1523727377175264, - "velocityY": 1.6837197635775993, - "timestamp": 5.859350002168872 - }, - { - "x": 3.4845656078038156, - "y": 2.20525192218672, - "heading": -0.6469466767255073, - "angularVelocity": -0.40450658449225635, - "velocityX": -2.9087066646478257, - "velocityY": 1.7165408517490015, - "timestamp": 5.922363878749962 - }, - { - "x": 3.316731609967858, - "y": 2.3123318355961353, - "heading": -0.6702493227736513, - "angularVelocity": -0.3698018168769004, - "velocityX": -2.6634450527730666, - "velocityY": 1.699306870473487, - "timestamp": 5.9853777553310525 - }, - { - "x": 3.1640084855137136, - "y": 2.4159899224511383, - "heading": -0.6914332444345033, - "angularVelocity": -0.3361786769869806, - "velocityX": -2.423642739351382, - "velocityY": 1.6450041241568225, - "timestamp": 6.048391631912143 - }, - { - "x": 3.0259122517259884, - "y": 2.5145372691122856, - "heading": -0.7105770269066637, - "angularVelocity": -0.30380264650953404, - "velocityX": -2.19152099950578, - "velocityY": 1.5638991283821457, - "timestamp": 6.111405508493233 - }, - { - "x": 2.9019526205870343, - "y": 2.6067411641295175, - "heading": -0.7277548924092291, - "angularVelocity": -0.2726044870523041, - "velocityX": -1.9671798953589927, - "velocityY": 1.4632315931012068, - "timestamp": 6.174419385074323 - }, - { - "x": 2.79168051154695, - "y": 2.6916821542694485, - "heading": -0.743032506079513, - "angularVelocity": -0.24244840183135916, - "velocityX": -1.7499654841609167, - "velocityY": 1.3479727759745657, - "timestamp": 6.237433261655413 - }, - { - "x": 2.694699726347074, - "y": 2.76865749223089, - "heading": -0.7564666750477512, - "angularVelocity": -0.21319381852265873, - "velocityX": -1.5390385493118295, - "velocityY": 1.2215616962143954, - "timestamp": 6.300447138236503 - }, - { - "x": 2.6106653021864603, - "y": 2.8371173452263947, - "heading": -0.7681062788003478, - "angularVelocity": -0.18471492922067745, - "velocityX": -1.3335860086702125, - "velocityY": 1.0864250338162604, - "timestamp": 6.363461014817593 - }, - { - "x": 2.5392776642565695, - "y": 2.8966223272202614, - "heading": -0.7779934551732625, - "angularVelocity": -0.15690474716614497, - "velocityX": -1.1328875765645765, - "velocityY": 0.9443155257602959, - "timestamp": 6.4264748913986836 - }, - { - "x": 2.480276064918479, - "y": 2.946814696080483, - "heading": -0.7861647133126454, - "angularVelocity": -0.1296739477512957, - "velocityX": -0.9363270844345449, - "velocityY": 0.7965288216418583, - "timestamp": 6.489488767979774 - }, - { - "x": 2.4334325341041674, - "y": 2.9873983829565494, - "heading": -0.7926518839069914, - "angularVelocity": -0.10294828609691896, - "velocityX": -0.7433843679499733, - "velocityY": 0.6440436468599187, - "timestamp": 6.552502644560864 - }, - { - "x": 2.398546680913597, - "y": 3.0181248328808925, - "heading": -0.7974829010951674, - "angularVelocity": -0.07666592582919579, - "velocityX": -0.5536217589418905, - "velocityY": 0.4876140239491929, - "timestamp": 6.615516521141954 - }, - { - "x": 2.3754413559178094, - "y": 3.038782749760636, - "heading": -0.8006824366316742, - "angularVelocity": -0.0507750944728719, - "velocityX": -0.36667042641082753, - "velocityY": 0.32783123338174003, - "timestamp": 6.678530397723044 + "heading": -0.3185534180019772, + "angularVelocity": -5.936985611348973e-7, + "velocityX": -3.781751231726404, + "velocityY": -0.039276923728911305, + "timestamp": 5.21792138784657 + }, + { + "x": 5.26265149067387, + "y": 1.5786031053003822, + "heading": -0.31855345112044986, + "angularVelocity": -5.20084412877405e-7, + "velocityX": -3.7744308273915435, + "velocityY": 0.23844772894968475, + "timestamp": 5.28160042121006 + }, + { + "x": 5.0240625547753925, + "y": 1.61139052009726, + "heading": -0.3185534786660252, + "angularVelocity": -4.3256899255575315e-7, + "velocityX": -3.7467424251962935, + "velocityY": 0.5148855606793222, + "timestamp": 5.34527945457355 + }, + { + "x": 4.788524290519247, + "y": 1.661604289022676, + "heading": -0.31855350247111613, + "angularVelocity": -3.7382933947955595e-7, + "velocityX": -3.698835422197071, + "velocityY": 0.7885447732660761, + "timestamp": 5.40895848793704 + }, + { + "x": 4.55730773239592, + "y": 1.7289734284079028, + "heading": -0.3185535237219762, + "angularVelocity": -3.337183198598428e-7, + "velocityX": -3.6309684037367047, + "velocityY": 1.0579485244487485, + "timestamp": 5.4726375213005305 + }, + { + "x": 4.33166058516164, + "y": 1.8131343663413284, + "heading": -0.3185535432404584, + "angularVelocity": -3.0651348259884106e-7, + "velocityX": -3.543507734268667, + "velocityY": 1.321642831055895, + "timestamp": 5.536316554664021 + }, + { + "x": 4.112800476187319, + "y": 1.9136328827222209, + "heading": -0.318553561635836, + "angularVelocity": -2.888765193967595e-7, + "velocityX": -3.436925741712075, + "velocityY": 1.5782041760469432, + "timestamp": 5.599995588027511 + }, + { + "x": 3.9019083093617053, + "y": 2.0299264483320134, + "heading": -0.31855357939397577, + "angularVelocity": -2.7886949297236075e-7, + "velocityX": -3.311799122668965, + "velocityY": 1.826245774585971, + "timestamp": 5.663674621391001 + }, + { + "x": 3.7001203074329077, + "y": 2.161384813738153, + "heading": -0.3185535969362317, + "angularVelocity": -2.75479306959021e-7, + "velocityX": -3.1688295388681516, + "velocityY": 2.0643900898393674, + "timestamp": 5.727353654754491 + }, + { + "x": 3.509285404964888, + "y": 2.2879634464070207, + "heading": -0.3579155843505842, + "angularVelocity": -0.6181310446354924, + "velocityX": -2.996824737880426, + "velocityY": 1.9877599577609337, + "timestamp": 5.791032688117982 + }, + { + "x": 3.332909755538667, + "y": 2.4050971261724468, + "heading": -0.4129449232759771, + "angularVelocity": -0.8641673093760197, + "velocityX": -2.769760156682042, + "velocityY": 1.8394387222684185, + "timestamp": 5.854711721481472 + }, + { + "x": 3.171287726021972, + "y": 2.512479984884094, + "heading": -0.4699204658498491, + "angularVelocity": -0.8947300165290918, + "velocityX": -2.5380729100288124, + "velocityY": 1.6863142079856834, + "timestamp": 5.918390754844962 + }, + { + "x": 3.024408806786741, + "y": 2.6100923699912792, + "heading": -0.5252799185719804, + "angularVelocity": -0.8693513358805374, + "velocityX": -2.306550704009953, + "velocityY": 1.5328810748429236, + "timestamp": 5.982069788208452 + }, + { + "x": 2.892255079193046, + "y": 2.6979336450739244, + "heading": -0.5773440254447586, + "angularVelocity": -0.8176020288434336, + "velocityX": -2.0753098879397367, + "velocityY": 1.3794379475145813, + "timestamp": 6.045748821571943 + }, + { + "x": 2.7748123815012584, + "y": 2.7760062309054065, + "heading": -0.625128837055519, + "angularVelocity": -0.7504010203483636, + "velocityX": -1.8442914643726742, + "velocityY": 1.2260328354205916, + "timestamp": 6.109427854935433 + }, + { + "x": 2.6720699818921556, + "y": 2.844312916696855, + "heading": -0.6679860381773631, + "angularVelocity": -0.6730190277419643, + "velocityX": -1.6134415706757543, + "velocityY": 1.0726715244175122, + "timestamp": 6.173106888298923 + }, + { + "x": 2.5840196243973645, + "y": 2.9028562717726505, + "heading": -0.7054558016839698, + "angularVelocity": -0.5884160221579265, + "velocityX": -1.382721326691407, + "velocityY": 0.9193505614575078, + "timestamp": 6.236785921662413 + }, + { + "x": 2.510654811151598, + "y": 2.95163853061359, + "heading": -0.7371951288142529, + "angularVelocity": -0.4984266477336408, + "velocityX": -1.1521031235978156, + "velocityY": 0.7660646882386306, + "timestamp": 6.3004649550259035 + }, + { + "x": 2.4519703126340358, + "y": 2.9906615990271557, + "heading": -0.7629386573341134, + "angularVelocity": -0.4042700895428552, + "velocityX": -0.9215670436230066, + "velocityY": 0.6128087433553882, + "timestamp": 6.364143988389394 + }, + { + "x": 2.4079618327799786, + "y": 3.0199270886206313, + "heading": -0.7824753669479414, + "angularVelocity": -0.3067997201262361, + "velocityX": -0.6910984279998401, + "velocityY": 0.4595781067596215, + "timestamp": 6.427823021752884 + }, + { + "x": 2.378625774904473, + "y": 3.039436353320747, + "heading": -0.795633833561549, + "angularVelocity": -0.20663734856804464, + "velocityX": -0.4606862938394533, + "velocityY": 0.3063687318988399, + "timestamp": 6.491502055116374 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.025232146222040765, - "velocityX": -0.18221830683035964, - "velocityY": 0.16516634183273946, - "timestamp": 6.741544274304134 + "angularVelocity": -0.10425061542851642, + "velocityX": -0.2303222914890624, + "velocityY": 0.15317707264507022, + "timestamp": 6.5551810884798645 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 5.951839521214632e-29, - "velocityX": -4.256198532236221e-31, - "velocityY": 3.167098439878756e-31, - "timestamp": 6.804558150885224 - }, - { - "x": 2.375813180458781, - "y": 3.0435390743709614, - "heading": -0.7990461525696796, - "angularVelocity": 0.05557590004806609, - "velocityX": 0.20420014353356836, - "velocityY": -0.09735244641805807, - "timestamp": 6.862609561786642 - }, - { - "x": 2.3995218612588998, - "y": 3.03223611715748, - "heading": -0.7926240802250826, - "angularVelocity": 0.110627325759628, - "velocityX": 0.4084083475659286, - "velocityY": -0.19470598626234253, - "timestamp": 6.92066097268806 - }, - { - "x": 2.435085642006342, - "y": 3.015281569795486, - "heading": -0.7830411709963286, - "angularVelocity": 0.1650762501712104, - "velocityX": 0.6126256053937512, - "velocityY": -0.2920609008243783, - "timestamp": 6.978712383589477 - }, - { - "x": 2.4825051165422036, - "y": 2.9926753368780465, - "heading": -0.7703376648494888, - "angularVelocity": 0.21883199649381424, - "velocityX": 0.8168530927937191, - "velocityY": -0.3894174588767385, - "timestamp": 7.036763794490895 - }, - { - "x": 2.541780960274715, - "y": 2.964417308476329, - "heading": -0.7545600110272952, - "angularVelocity": 0.2717876030435742, - "velocityX": 1.0210922148502688, - "velocityY": -0.4867759105752865, - "timestamp": 7.094815205392313 - }, - { - "x": 2.612913947196807, - "y": 2.930507361776858, - "heading": -0.7357621134740673, - "angularVelocity": 0.3238146543096182, - "velocityX": 1.225344669794324, - "velocityY": -0.5841364778722867, - "timestamp": 7.15286661629373 - }, - { - "x": 2.6959049720092736, - "y": 2.8909453636117948, - "heading": -0.7140070097413544, - "angularVelocity": 0.37475581376751455, - "velocityX": 1.4296125369528379, - "velocityY": -0.6814993391331685, - "timestamp": 7.210918027195148 - }, - { - "x": 2.7907550793730915, - "y": 2.84573117446325, - "heading": -0.6893691896527133, - "angularVelocity": 0.4244138033179133, - "velocityX": 1.6338983995564174, - "velocityY": -0.778864603744556, - "timestamp": 7.2689694380965655 - }, - { - "x": 2.8974655032774796, - "y": 2.7948646549900524, - "heading": -0.6619378845471044, - "angularVelocity": 0.47253468399231374, - "velocityX": 1.8382055189942428, - "velocityY": -0.876232268662314, - "timestamp": 7.327020848997983 - }, - { - "x": 3.0160377210166036, - "y": 2.7383456770648533, - "heading": -0.6318218809717984, - "angularVelocity": 0.5187815956178783, - "velocityX": 2.042538086464129, - "velocityY": -0.9736021407159139, - "timestamp": 7.385072259899401 - }, - { - "x": 3.1464735286190364, - "y": 2.676174143314885, - "heading": -0.5991568348591108, - "angularVelocity": 0.5626916832074861, - "velocityX": 2.2469015925200937, - "velocityY": -1.0709736901235276, - "timestamp": 7.443123670800818 - }, - { - "x": 3.288775148081632, - "y": 2.6083500237720547, - "heading": -0.5641169164360986, - "angularVelocity": 0.6036014952766117, - "velocityX": 2.4513033749386723, - "velocityY": -1.1683457557648163, - "timestamp": 7.501175081702236 - }, - { - "x": 3.4429453809853263, - "y": 2.534873429900399, - "heading": -0.5269344943074844, - "angularVelocity": 0.6405085001595859, - "velocityX": 2.6557534176991067, - "velocityY": -1.2657159013143156, - "timestamp": 7.559226492603654 - }, - { - "x": 3.608987822216531, - "y": 2.4557447796300744, - "heading": -0.48793616320693267, - "angularVelocity": 0.6717895481778894, - "velocityX": 2.860265386368935, - "velocityY": -1.3630788475529159, - "timestamp": 7.617277903505071 - }, - { - "x": 3.786907101523248, - "y": 2.370965219866687, - "heading": -0.44761641247502054, - "angularVelocity": 0.6945524683350642, - "velocityX": 3.064857107587936, - "velocityY": -1.4604220370691692, - "timestamp": 7.675329314406489 - }, - { - "x": 3.9767087353801904, - "y": 2.2805379602303306, - "heading": -0.4068154658739133, - "angularVelocity": 0.7028416013935566, - "velocityX": 3.2695438562081205, - "velocityY": -1.5577099373160743, - "timestamp": 7.7333807253079065 - }, - { - "x": 4.178394426915559, - "y": 2.1844743331965764, - "heading": -0.36728880481186277, - "angularVelocity": 0.6808906183034006, - "velocityX": 3.4742599431023264, - "velocityY": -1.6548026230902309, - "timestamp": 7.791432136209324 - }, - { - "x": 4.39186585424446, - "y": 2.082861864950712, - "heading": -0.33521197370242245, - "angularVelocity": 0.5525590267549734, - "velocityX": 3.6772823263747743, - "velocityY": -1.7503875731534284, - "timestamp": 7.849483547110742 - }, - { - "x": 4.613290428731911, - "y": 1.9772010264517206, - "heading": -0.3352119609115161, - "angularVelocity": 2.203375614401519e-7, - "velocityX": 3.81428411556565, - "velocityY": -1.8201252451628447, - "timestamp": 7.907534958012159 - }, - { - "x": 4.83471490814338, - "y": 1.8715399887082398, - "heading": -0.33521194812268507, - "angularVelocity": 2.2030181304603152e-7, - "velocityX": 3.81428247777632, - "velocityY": -1.8201286773703715, - "timestamp": 7.965586368913577 - }, - { - "x": 5.056139387672164, - "y": 1.7658789512106026, - "heading": -0.33521193533385407, - "angularVelocity": 2.2030181241344307e-7, - "velocityX": 3.8142824797971753, - "velocityY": -1.8201286731354431, - "timestamp": 8.023637779814994 - }, - { - "x": 5.277564966924424, - "y": 1.6602202183369479, - "heading": -0.33521192254501997, - "angularVelocity": 2.203018646355495e-7, - "velocityX": 3.814301423754948, - "velocityY": -1.8200889734288124, - "timestamp": 8.08168919071641 + "angularVelocity": -1.4092752497411054e-30, + "velocityX": 2.9428006004895336e-31, + "velocityY": 5.314710200495422e-31, + "timestamp": 6.618860121843355 + }, + { + "x": 2.3794842314620843, + "y": 3.041727640405621, + "heading": -0.7962432666233688, + "angularVelocity": 0.0957711014436812, + "velocityX": 0.24661230413774035, + "velocityY": -0.11854554422657079, + "timestamp": 6.681813823589832 + }, + { + "x": 2.410536447869941, + "y": 3.0268006675090624, + "heading": -0.7842917338243374, + "angularVelocity": 0.18984638658997072, + "velocityX": 0.4932548134009415, + "velocityY": -0.23711032842312177, + "timestamp": 6.744767525336309 + }, + { + "x": 2.4571179241280796, + "y": 3.0044081651489933, + "heading": -0.7665449064601504, + "angularVelocity": 0.28190284084732253, + "velocityX": 0.7399322830248891, + "velocityY": -0.3556979452971155, + "timestamp": 6.807721227082786 + }, + { + "x": 2.5192312369840457, + "y": 2.9745484345249116, + "heading": -0.743155503226628, + "angularVelocity": 0.37153340605314533, + "velocityX": 0.9866506834833091, + "velocityY": -0.4743125470894598, + "timestamp": 6.8706749288292635 + }, + { + "x": 2.596879439737626, + "y": 2.937219467270397, + "heading": -0.714309410311632, + "angularVelocity": 0.4582112268975565, + "velocityX": 1.233417584660547, + "velocityY": -0.5929590511586358, + "timestamp": 6.933628630575741 + }, + { + "x": 2.690066196626848, + "y": 2.892418878795792, + "heading": -0.6802368283616581, + "angularVelocity": 0.5412323819683986, + "velocityX": 1.4802426911208155, + "velocityY": -0.7116434336939077, + "timestamp": 6.996582332322218 + }, + { + "x": 2.798795963694134, + "y": 2.8401438158547943, + "heading": -0.6412294413317386, + "angularVelocity": 0.6196202280051365, + "velocityX": 1.7271385804309867, + "velocityY": -0.8303731391605256, + "timestamp": 7.059536034068695 + }, + { + "x": 2.9230742308720847, + "y": 2.7803908299690017, + "heading": -0.5976682545562845, + "angularVelocity": 0.6919559226378926, + "velocityX": 1.9741216756154527, + "velocityY": -0.9491576226355342, + "timestamp": 7.122489735815172 + }, + { + "x": 3.062907829622796, + "y": 2.713155712528932, + "heading": -0.5500718966234338, + "angularVelocity": 0.7560533632244142, + "velocityX": 2.22121328645359, + "velocityY": -1.0680089585650514, + "timestamp": 7.185443437561649 + }, + { + "x": 3.218305230273769, + "y": 2.638433320090989, + "heading": -0.4991885737635272, + "angularVelocity": 0.8082657802208455, + "velocityX": 2.4684394458133507, + "velocityY": -1.1869419964986359, + "timestamp": 7.248397139308127 + }, + { + "x": 3.389276272924326, + "y": 2.556217612467452, + "heading": -0.4461960441931904, + "angularVelocity": 0.8417698737358584, + "velocityX": 2.715821912094713, + "velocityY": -1.3059709809381974, + "timestamp": 7.311350841054604 + }, + { + "x": 3.575827436112715, + "y": 2.4665034787202997, + "heading": -0.3932374103043789, + "angularVelocity": 0.841231451362194, + "velocityX": 2.9633072879440143, + "velocityY": -1.4250811510408536, + "timestamp": 7.374304542801081 + }, + { + "x": 3.777911234478158, + "y": 2.369308059755886, + "heading": -0.34555593776982274, + "angularVelocity": 0.7574053822375035, + "velocityX": 3.210038373585424, + "velocityY": -1.5439190431697394, + "timestamp": 7.437258244547558 + }, + { + "x": 3.9926293770053456, + "y": 2.266439040708753, + "heading": -0.3455557491739834, + "angularVelocity": 0.000002995786333953476, + "velocityX": 3.4107310066036556, + "velocityY": -1.6340424183696203, + "timestamp": 7.500211946294035 + }, + { + "x": 4.207200483864901, + "y": 2.1632635581376745, + "heading": -0.34555573252846533, + "angularVelocity": 2.644088848166298e-7, + "velocityX": 3.4083953906898548, + "velocityY": -1.6389104962656516, + "timestamp": 7.5631656480405125 + }, + { + "x": 4.421771581956281, + "y": 2.0600880573316713, + "heading": -0.3455557158829472, + "angularVelocity": 2.6440888572481287e-7, + "velocityX": 3.408395251410094, + "velocityY": -1.638910785921765, + "timestamp": 7.62611934978699 + }, + { + "x": 4.636342680047139, + "y": 1.9569125565245837, + "heading": -0.3455556992374292, + "angularVelocity": 2.6440888404451857e-7, + "velocityX": 3.4083952514018088, + "velocityY": -1.6389107859389946, + "timestamp": 7.689073051533467 + }, + { + "x": 4.850913778140846, + "y": 1.853737055723424, + "heading": -0.3455556825919111, + "angularVelocity": 2.64408884846605e-7, + "velocityX": 3.408395251447087, + "velocityY": -1.63891078584483, + "timestamp": 7.752026753279944 + }, + { + "x": 5.065484924140348, + "y": 1.7505616545506137, + "heading": -0.34555566594639303, + "angularVelocity": 2.644088848456774e-7, + "velocityX": 3.4083960124157384, + "velocityY": -1.6389092032794423, + "timestamp": 7.814980455026421 + }, + { + "x": 5.280845302209934, + "y": 1.6490439336616942, + "heading": -0.34555564929194366, + "angularVelocity": 2.6455075541297124e-7, + "velocityX": 3.4209327187283005, + "velocityY": -1.6125774668143436, + "timestamp": 7.877934156772898 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.3352119097112988, - "angularVelocity": 2.2107509356850104e-7, - "velocityX": 3.8834306838456705, - "velocityY": -1.6675087206439847, - "timestamp": 8.139740601617827 - }, - { - "x": 5.771247738907838, - "y": 1.4675302215572488, - "heading": -0.3352118975519742, - "angularVelocity": 1.8039593836487576e-7, - "velocityX": 3.9796744633245575, - "velocityY": -1.4226072506653686, - "timestamp": 8.207144140304313 - }, - { - "x": 6.040462534260166, - "y": 1.3811079630666692, - "heading": -0.3269077113367565, - "angularVelocity": 0.1232010422159426, - "velocityX": 3.994075097518657, - "velocityY": -1.2821620374051248, - "timestamp": 8.2745476789908 - }, - { - "x": 6.293431004629783, - "y": 1.2999254087750478, - "heading": -0.29863774275478366, - "angularVelocity": 0.4194137152570709, - "velocityX": 3.7530443549299446, - "velocityY": -1.2044257003957306, - "timestamp": 8.341951217677286 - }, - { - "x": 6.529552502522878, - "y": 1.2241576590494234, - "heading": -0.2655052058197796, - "angularVelocity": 0.4915548587013119, - "velocityX": 3.5031023963202834, - "velocityY": -1.1240915714832609, - "timestamp": 8.409354756363772 - }, - { - "x": 6.748788082038609, - "y": 1.153810716509259, - "heading": -0.23113245426293594, - "angularVelocity": 0.5099547030716253, - "velocityX": 3.252582635690123, - "velocityY": -1.0436683876104664, - "timestamp": 8.476758295050258 - }, - { - "x": 6.951134609386156, - "y": 1.0888826430758505, - "heading": -0.19719131465702286, - "angularVelocity": 0.5035513011235752, - "velocityX": 3.0020163820882195, - "velocityY": -0.9632739571049619, - "timestamp": 8.544161833736744 - }, - { - "x": 7.136594604940876, - "y": 1.0293710541923635, - "heading": -0.1646509365883203, - "angularVelocity": 0.48276958009664506, - "velocityX": 2.7514875208162133, - "velocityY": -0.8829149039235648, - "timestamp": 8.61156537242323 - }, - { - "x": 7.305171448299849, - "y": 0.9752739365513875, - "heading": -0.134145865535612, - "angularVelocity": 0.45257373199049533, - "velocityX": 2.501008799301714, - "velocityY": -0.8025857202037736, - "timestamp": 8.678968911109717 - }, - { - "x": 7.4568683544045555, - "y": 0.9265896769068921, - "heading": -0.10612469685480012, - "angularVelocity": 0.4157225158629608, - "velocityX": 2.2505777747114233, - "velocityY": -0.7222804706284149, - "timestamp": 8.746372449796203 - }, - { - "x": 7.591688163065539, - "y": 0.8833169839764341, - "heading": -0.08092177349469894, - "angularVelocity": 0.3739109822902337, - "velocityX": 2.0001888815967246, - "velocityY": -0.6419943785404568, - "timestamp": 8.81377598848269 - }, - { - "x": 7.709633332729291, - "y": 0.8454548114841277, - "heading": -0.05879603455931429, - "angularVelocity": 0.3282578239445074, - "velocityX": 1.7498364620342992, - "velocityY": -0.5617238090186147, - "timestamp": 8.881179527169175 - }, - { - "x": 7.810705984005889, - "y": 0.8130022977681454, - "heading": -0.03995388423570108, - "angularVelocity": 0.27954244971105247, - "velocityX": 1.499515503877575, - "velocityY": -0.48146602312571224, - "timestamp": 8.948583065855662 - }, - { - "x": 7.894907949862419, - "y": 0.7859587206023052, - "heading": -0.024563521252436132, - "angularVelocity": 0.22833167639537438, - "velocityX": 1.2492217396504763, - "velocityY": -0.4012189521922299, - "timestamp": 9.015986604542148 - }, - { - "x": 7.962240820715571, - "y": 0.7643234635620257, - "heading": -0.012764369724625984, - "angularVelocity": 0.17505240463252983, - "velocityX": 0.9989515708713329, - "velocityY": -0.3209810265438958, - "timestamp": 9.083390143228634 - }, - { - "x": 8.012705982087093, - "y": 0.7480959907369016, - "heading": -0.004673541051565745, - "angularVelocity": 0.12003566623844747, - "velocityX": 0.7487019577154632, - "velocityY": -0.24075105167108718, - "timestamp": 9.15079368191512 - }, - { - "x": 8.046304645291002, - "y": 0.7372758274108119, - "heading": -0.00039041667718953013, - "angularVelocity": 0.06354450312022167, - "velocityX": 0.49847031563414146, - "velocityY": -0.1605281196943911, - "timestamp": 9.218197220601606 - }, - { - "x": 8.063037872314453, - "y": 0.7318625450134277, - "heading": -3.5207342228472116e-29, - "angularVelocity": 0.005792228194527766, - "velocityX": 0.24825442921152802, - "velocityY": -0.08031154599409078, - "timestamp": 9.285600759288092 - }, - { - "x": 8.061895055441012, - "y": 0.7321772752029715, - "heading": -0.004013012422633091, - "angularVelocity": -0.05636038425014907, - "velocityX": -0.016050186575884747, - "velocityY": 0.004420199229323397, - "timestamp": 9.35680347509688 - }, - { - "x": 8.041931973498654, - "y": 0.7385255618667966, - "heading": -0.012367513802926035, - "angularVelocity": -0.11733402701567407, - "velocityX": -0.2803696701115773, - "velocityY": 0.0891579287631834, - "timestamp": 9.428006190905668 - }, - { - "x": 8.003147425566949, - "y": 0.7509078775903022, - "heading": -0.024966290125142357, - "angularVelocity": -0.17694235646930861, - "velocityX": -0.5447060198638873, - "velocityY": 0.17390229547925942, - "timestamp": 9.499208906714456 - }, - { - "x": 7.945540047254358, - "y": 0.7693247566912358, - "heading": -0.04169615417860038, - "angularVelocity": -0.23496103854227787, - "velocityX": -0.8090615316878346, - "velocityY": 0.25865416637184857, - "timestamp": 9.570411622523244 - }, - { - "x": 7.869108284660135, - "y": 0.7937768147652166, - "heading": -0.06242431663850855, - "angularVelocity": -0.2911147731440275, - "velocityX": -1.0734388671280197, - "velocityY": 0.34341468294054744, - "timestamp": 9.641614338332031 - }, - { - "x": 7.773850362284446, - "y": 0.8242647742315067, - "heading": -0.08699338329336499, - "angularVelocity": -0.34505799920387553, - "velocityX": -1.3378411384124769, - "velocityY": 0.4281853454601993, - "timestamp": 9.71281705414082 - }, - { - "x": 7.65976424379608, - "y": 0.8607894982947099, - "heading": -0.11521425502187198, - "angularVelocity": -0.39634544002927347, - "velocityX": -1.6022720087635367, - "velocityY": 0.5129681311775954, - "timestamp": 9.784019769949607 - }, - { - "x": 7.526847585445219, - "y": 0.9033520367557786, - "heading": -0.14685571507130035, - "angularVelocity": -0.44438557841539145, - "velocityX": -1.8667357956935873, - "velocityY": 0.59776566072801, - "timestamp": 9.855222485758395 - }, - { - "x": 7.375097685187462, - "y": 0.9519536884245254, - "heading": -0.18162855174599865, - "angularVelocity": -0.48836390971499116, - "velocityX": -2.1312375312379404, - "velocityY": 0.6825814312935032, - "timestamp": 9.926425201567183 - }, - { - "x": 7.204511441301595, - "y": 1.0065960860089886, - "heading": -0.21916015731822874, - "angularVelocity": -0.5271091860179415, - "velocityX": -2.395782828620887, - "velocityY": 0.7674201322770242, - "timestamp": 9.99762791737597 - }, - { - "x": 7.01508536939227, - "y": 1.0672813074513483, - "heading": -0.25895131555418466, - "angularVelocity": -0.5588432657936879, - "velocityX": -2.6603770622741663, - "velocityY": 0.8522880167285697, - "timestamp": 10.068830633184758 - }, - { - "x": 6.806815850395879, - "y": 1.13401199877149, - "heading": -0.3002964260461437, - "angularVelocity": -0.5806676054744402, - "velocityX": -2.925022123533756, - "velocityY": 0.9371930629632811, - "timestamp": 10.140033348993546 - }, - { - "x": 6.579700283529664, - "y": 1.2067913837362447, - "heading": -0.34211833205863673, - "angularVelocity": -0.5873639163541298, - "velocityX": -3.1897037112478492, - "velocityY": 1.0221433850950452, - "timestamp": 10.211236064802334 - }, - { - "x": 6.3337423865313776, - "y": 1.285622359058628, - "heading": -0.38256120014941036, - "angularVelocity": -0.5679961449699128, - "velocityX": -3.454333085535639, - "velocityY": 1.107134389846602, - "timestamp": 10.282438780611121 - }, - { - "x": 6.0689850231424955, - "y": 1.3704991616653164, - "heading": -0.41762738854860754, - "angularVelocity": -0.4924838610562412, - "velocityX": -3.718360463944625, - "velocityY": 1.1920444556443872, - "timestamp": 10.35364149641991 - }, - { - "x": 5.786063768357388, - "y": 1.4612805523811225, - "heading": -0.43124692378177526, - "angularVelocity": -0.19127831120575167, - "velocityX": -3.9734615677424734, - "velocityY": 1.2749709008234504, - "timestamp": 10.424844212228697 + "heading": -0.34555563246426124, + "angularVelocity": 2.6730250915052296e-7, + "velocityX": 3.52891551864971, + "velocityY": -1.360125725304771, + "timestamp": 7.940887858519376 + }, + { + "x": 5.655413318775525, + "y": 1.5130951799644636, + "heading": -0.34555563491945995, + "angularVelocity": -5.785220393859716e-8, + "velocityX": 3.5912523855702734, + "velocityY": -1.18578710714545, + "timestamp": 7.983327014878419 + }, + { + "x": 5.810103118724588, + "y": 1.4702901058122384, + "heading": -0.3455556373457815, + "angularVelocity": -5.7171766263130695e-8, + "velocityX": 3.6449782045702985, + "velocityY": -1.0086221740622332, + "timestamp": 8.025766171237462 + }, + { + "x": 5.966313456902953, + "y": 1.4334181808859694, + "heading": -0.34555563976573417, + "angularVelocity": -5.702169541040829e-8, + "velocityX": 3.680806867525754, + "velocityY": -0.8688185178405917, + "timestamp": 8.068205327596505 + }, + { + "x": 6.1225244823663525, + "y": 1.3965491678093302, + "heading": -0.3455556421856862, + "angularVelocity": -5.7021679533312126e-8, + "velocityX": 3.680823062122731, + "velocityY": -0.868749905505203, + "timestamp": 8.110644483955548 + }, + { + "x": 6.278735507957822, + "y": 1.3596801552753135, + "heading": -0.3455556446056381, + "angularVelocity": -5.702167859922657e-8, + "velocityX": 3.680823065140465, + "velocityY": -0.8687498927193055, + "timestamp": 8.153083640314591 + }, + { + "x": 6.434946533537413, + "y": 1.3228111426909752, + "heading": -0.34555564702559005, + "angularVelocity": -5.7021678932866206e-8, + "velocityX": 3.680823064860606, + "velocityY": -0.868749893905046, + "timestamp": 8.195522796673634 + }, + { + "x": 6.591157495254782, + "y": 1.285941859528607, + "heading": -0.3455556494455419, + "angularVelocity": -5.7021677677252914e-8, + "velocityX": 3.6808215600657594, + "velocityY": -0.8687562695744264, + "timestamp": 8.237961953032677 + }, + { + "x": 6.747031505398479, + "y": 1.2476730535838276, + "heading": -0.34555565194208476, + "angularVelocity": -5.8826401859372655e-8, + "velocityX": 3.6728819212374164, + "velocityY": -0.9017334279932018, + "timestamp": 8.28040110939172 + }, + { + "x": 6.897830261276489, + "y": 1.2034176695315386, + "heading": -0.3468060133183351, + "angularVelocity": -0.029462446559305244, + "velocityX": 3.5532929684610997, + "velocityY": -1.0427960367044076, + "timestamp": 8.322840265750763 + }, + { + "x": 7.040966542913821, + "y": 1.160898981796106, + "heading": -0.34756195184747823, + "angularVelocity": -0.017812289262957588, + "velocityX": 3.3727409759603195, + "velocityY": -1.001874009363348, + "timestamp": 8.365279422109806 + }, + { + "x": 7.176448137174031, + "y": 1.12015095875213, + "heading": -0.34781665101790943, + "angularVelocity": -0.006001513514462224, + "velocityX": 3.1923724664555144, + "velocityY": -0.9601515802821338, + "timestamp": 8.407718578468849 + }, + { + "x": 7.30427768133993, + "y": 1.0811849503221362, + "heading": -0.3475678387477372, + "angularVelocity": 0.005862799629361019, + "velocityX": 3.0120660996283153, + "velocityY": -0.918161711329369, + "timestamp": 8.450157734827892 + }, + { + "x": 7.424456501916377, + "y": 1.044006636667021, + "heading": -0.3468143875407301, + "angularVelocity": 0.017753680130510166, + "velocityX": 2.8317909894275823, + "velocityY": -0.8760379999211017, + "timestamp": 8.492596891186935 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.02966022853563592, + "velocityX": 2.65153469287831, + "velocityY": -0.8338339406813241, + "timestamp": 8.535036047545978 + }, + { + "x": 7.689789531297121, + "y": 0.9591642629851495, + "heading": -0.34249163023542684, + "angularVelocity": 0.04769743470803589, + "velocityX": 2.378707539440784, + "velocityY": -0.769870356769085, + "timestamp": 8.599274349827635 + }, + { + "x": 7.82516177952376, + "y": 0.9142093996063804, + "heading": -0.3392101704632614, + "angularVelocity": 0.05108260423473965, + "velocityX": 2.1073447369933547, + "velocityY": -0.6998140016475143, + "timestamp": 8.663512652109292 + }, + { + "x": 7.943169958712402, + "y": 0.8740090692537413, + "heading": -0.33632582750820933, + "angularVelocity": 0.044900672225200826, + "velocityX": 1.837037639494695, + "velocityY": -0.6258000122166694, + "timestamp": 8.727750954390949 + }, + { + "x": 8.043864833075729, + "y": 0.8387415563520763, + "heading": -0.3342715201633078, + "angularVelocity": 0.03197947753809357, + "velocityX": 1.567520790350633, + "velocityY": -0.5490106626266708, + "timestamp": 8.791989256672606 + }, + { + "x": 8.12728568078066, + "y": 0.8085387560486261, + "heading": -0.33336873570137193, + "angularVelocity": 0.014053678722353937, + "velocityX": 1.2986153858669556, + "velocityY": -0.4701680964578417, + "timestamp": 8.856227558954263 + }, + { + "x": 8.193463737140812, + "y": 0.7835021726798679, + "heading": -0.33386568134146205, + "angularVelocity": -0.007735970946293819, + "velocityX": 1.030196222652151, + "velocityY": -0.38974540857234485, + "timestamp": 8.92046586123592 + }, + { + "x": 8.242424408463135, + "y": 0.7637123264900073, + "heading": -0.33595980135953535, + "angularVelocity": -0.03259924287680334, + "velocityX": 0.7621725603464993, + "velocityY": -0.3080692591016908, + "timestamp": 8.984704163517577 + }, + { + "x": 8.274188750584008, + "y": 0.7492346436854774, + "heading": -0.3398119288829893, + "angularVelocity": -0.05996620998114242, + "velocityX": 0.4944766750154696, + "velocityY": -0.2253746174836856, + "timestamp": 9.048942465799234 + }, + { + "x": 8.288774490356445, + "y": 0.7401233315467834, + "heading": -0.34555563246426124, + "angularVelocity": -0.08941244362418393, + "velocityX": 0.2270567442533848, + "velocityY": -0.14183612914838375, + "timestamp": 9.113180768080891 + }, + { + "x": 8.286058974983396, + "y": 0.7364361738247144, + "heading": -0.3533713967772531, + "angularVelocity": -0.1208300500196775, + "velocityX": -0.04198128874091145, + "velocityY": -0.05700267231030528, + "timestamp": 9.177864713107141 + }, + { + "x": 8.26594084231072, + "y": 0.7382366486349878, + "heading": -0.3631834148956511, + "angularVelocity": -0.15169170826572306, + "velocityX": -0.3110220420927156, + "velocityY": 0.027834956719829915, + "timestamp": 9.24254865813339 + }, + { + "x": 8.22841989282902, + "y": 0.7455250700569162, + "heading": -0.3749497746847549, + "angularVelocity": -0.18190541384463682, + "velocityX": -0.5800658798172126, + "velocityY": 0.11267744134917045, + "timestamp": 9.30723260315964 + }, + { + "x": 8.173495898954863, + "y": 0.7583018091084519, + "heading": -0.38862097650887273, + "angularVelocity": -0.21135386560868902, + "velocityX": -0.8491132359330754, + "velocityY": 0.1975256618369646, + "timestamp": 9.37191654818589 + }, + { + "x": 8.101168599327213, + "y": 0.7765673113174031, + "heading": -0.4041376276128682, + "angularVelocity": -0.23988411804039605, + "velocityX": -1.11816463263485, + "velocityY": 0.2823807700896826, + "timestamp": 9.43660049321214 + }, + { + "x": 8.011437691690773, + "y": 0.8003221216608263, + "heading": -0.42142708913788907, + "angularVelocity": -0.26729138920027884, + "velocityX": -1.3872207021390834, + "velocityY": 0.36724430357151194, + "timestamp": 9.50128443823839 + }, + { + "x": 7.904302824322884, + "y": 0.8295669211666158, + "heading": -0.44039841224325665, + "angularVelocity": -0.29329261067284323, + "velocityX": -1.6562822092006166, + "velocityY": 0.45211836559939883, + "timestamp": 9.56596838326464 + }, + { + "x": 7.779763586783285, + "y": 0.8643025828479174, + "heading": -0.46093432615418145, + "angularVelocity": -0.317480850968364, + "velocityX": -1.9253500615811048, + "velocityY": 0.5370059242243979, + "timestamp": 9.63065232829089 + }, + { + "x": 7.63781950374517, + "y": 0.9045302615405436, + "heading": -0.48287779045070706, + "angularVelocity": -0.33924127985113656, + "velocityX": -2.194425262412658, + "velocityY": 0.6219113363648586, + "timestamp": 9.695336273317139 + }, + { + "x": 7.478470046612931, + "y": 0.9502515474489494, + "heading": -0.5060075980261466, + "angularVelocity": -0.3575818940241346, + "velocityX": -2.4635086352196294, + "velocityY": 0.7068413327271726, + "timestamp": 9.760020218343389 + }, + { + "x": 7.301714722310279, + "y": 1.0014687500113455, + "heading": -0.5299890975728885, + "angularVelocity": -0.37074887032647375, + "velocityX": -2.732599630880914, + "velocityY": 0.7918070325118697, + "timestamp": 9.824704163369638 + }, + { + "x": 7.107553521342319, + "y": 1.0581854755780744, + "heading": -0.5542575516052335, + "angularVelocity": -0.37518512549746846, + "velocityX": -3.0016907733312475, + "velocityY": 0.8768284857040194, + "timestamp": 9.889388108395888 + }, + { + "x": 6.895989597473325, + "y": 1.1204078946209601, + "heading": -0.5776591781660143, + "angularVelocity": -0.3617841575878507, + "velocityX": -3.2707331592582385, + "velocityY": 0.961945333075069, + "timestamp": 9.954072053422138 + }, + { + "x": 6.667061067423103, + "y": 1.188145012147935, + "heading": -0.5965312726664429, + "angularVelocity": -0.2917585575952417, + "velocityX": -3.5391862688232427, + "velocityY": 1.0472013959489617, + "timestamp": 10.018755998448388 + }, + { + "x": 6.433742027972299, + "y": 1.2616769248691375, + "heading": -0.5965312781733252, + "angularVelocity": -8.513522502240808e-8, + "velocityX": -3.607062608134345, + "velocityY": 1.136787694247182, + "timestamp": 10.083439943474637 + }, + { + "x": 6.200423078644191, + "y": 1.3352091235520722, + "heading": -0.5965312836800714, + "angularVelocity": -8.513312137357674e-8, + "velocityX": -3.6070612148566714, + "velocityY": 1.1367921151546025, + "timestamp": 10.148123888500887 + }, + { + "x": 5.967104129330135, + "y": 1.4087413222795935, + "heading": -0.5965312891868175, + "angularVelocity": -8.513312220445073e-8, + "velocityX": -3.607061214639434, + "velocityY": 1.136792115843899, + "timestamp": 10.212807833527137 + }, + { + "x": 5.733785345709214, + "y": 1.4822740467522528, + "heading": -0.5965312946935636, + "angularVelocity": -8.513311865132804e-8, + "velocityX": -3.6070586530588855, + "velocityY": 1.1368002437516491, + "timestamp": 10.277491778553387 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.431246936810795, - "angularVelocity": -1.829848702149455e-7, - "velocityX": -3.97541256513325, - "velocityY": 1.4344738240927952, - "timestamp": 10.496046928037485 - }, - { - "x": 5.267799654684855, - "y": 1.6634628697072642, - "heading": -0.4312469486344858, - "angularVelocity": -1.9550516647777393e-7, - "velocityX": -3.889105914989318, - "velocityY": 1.6542293543074849, - "timestamp": 10.556524566784258 - }, - { - "x": 5.036859662555262, - "y": 1.772990919381671, - "heading": -0.43124696041084615, - "angularVelocity": -1.9472255536128903e-7, - "velocityX": -3.8186013362155586, - "velocityY": 1.8110503641356064, - "timestamp": 10.617002205531032 - }, - { - "x": 4.805920651459889, - "y": 1.8825210375437151, - "heading": -0.4312469721872024, - "angularVelocity": -1.9472248822096454e-7, - "velocityX": -3.8185851147783296, - "velocityY": 1.8110845666554114, - "timestamp": 10.677479844277805 - }, - { - "x": 4.574981640487933, - "y": 1.9920511559659781, - "heading": -0.4312469839635587, - "angularVelocity": -1.947224875225229e-7, - "velocityX": -3.8185851127376225, - "velocityY": 1.8110845709581442, - "timestamp": 10.737957483024578 - }, - { - "x": 4.344043475908902, - "y": 2.101581482580224, - "heading": -0.4312495820185635, - "angularVelocity": -0.00004295893592741073, - "velocityX": -3.8185711175992116, - "velocityY": 1.811088013420298, - "timestamp": 10.798435121771352 - }, - { - "x": 4.124944651592417, - "y": 2.2054837302340013, - "heading": -0.4555627986647105, - "angularVelocity": -0.402019939104465, - "velocityX": -3.6228071871964698, - "velocityY": 1.7180275190442889, - "timestamp": 10.858912760518125 - }, - { - "x": 3.9186815541610502, - "y": 2.3032986019181108, - "heading": -0.48568611973278303, - "angularVelocity": -0.49809023123757634, - "velocityX": -3.4105679670301026, - "velocityY": 1.6173725315842862, - "timestamp": 10.919390399264898 - }, - { - "x": 3.725312007029145, - "y": 2.394998463425361, - "heading": -0.5178804694719192, - "angularVelocity": -0.5323347671351059, - "velocityX": -3.197372634562718, - "velocityY": 1.5162606114832984, - "timestamp": 10.979868038011672 - }, - { - "x": 3.5448441178456083, - "y": 2.4805793192106775, - "heading": -0.5504848182217125, - "angularVelocity": -0.5391141159845556, - "velocityX": -2.9840432418199043, - "velocityY": 1.4150826248963195, - "timestamp": 11.040345676758445 - }, - { - "x": 3.3772776543095704, - "y": 2.5600412902138987, - "heading": -0.5825517458866412, - "angularVelocity": -0.530227838411517, - "velocityX": -2.7707176901805863, - "velocityY": 1.3139066380540503, - "timestamp": 11.100823315505219 - }, - { - "x": 3.222610601114179, - "y": 2.6333854258294824, - "heading": -0.6134664857875873, - "angularVelocity": -0.5111763709954703, - "velocityX": -2.5574254617148258, - "velocityY": 1.2127480029880529, - "timestamp": 11.161300954251992 - }, - { - "x": 3.0808406790679506, - "y": 2.7006129397280647, - "heading": -0.6427968546636391, - "angularVelocity": -0.48497873732909724, - "velocityX": -2.344170919764792, - "velocityY": 1.111609436011059, - "timestamp": 11.221778592998765 - }, - { - "x": 2.951965745045311, - "y": 2.761724997445756, - "heading": -0.6702218815330417, - "angularVelocity": -0.45347383657350077, - "velocityX": -2.130951814475617, - "velocityY": 1.0104901412168834, - "timestamp": 11.282256231745539 - }, - { - "x": 2.8359838859812436, - "y": 2.8167226593903436, - "heading": -0.6954934767072644, - "angularVelocity": -0.4178667636155553, - "velocityX": -1.917764341787469, - "velocityY": 0.9093883803048015, - "timestamp": 11.342733870492312 - }, - { - "x": 2.732893421596196, - "y": 2.865606872368782, - "heading": -0.7184140024714569, - "angularVelocity": -0.3789917437115094, - "velocityX": -1.7046046525840333, - "velocityY": 0.8083022748808345, - "timestamp": 11.403211509239085 - }, - { - "x": 2.6426928809449626, - "y": 2.9083784763416602, - "heading": -0.7388222811864813, - "angularVelocity": -0.33745164556568824, - "velocityX": -1.4914692855141438, - "velocityY": 0.7072300582363449, - "timestamp": 11.463689147985859 - }, - { - "x": 2.565380973908943, - "y": 2.945038215167831, - "heading": -0.756584421622472, - "angularVelocity": -0.29369765096755573, - "velocityX": -1.2783552506031493, - "velocityY": 0.6061701413256031, - "timestamp": 11.524166786732632 - }, - { - "x": 2.500956564421106, - "y": 2.975586747465841, - "heading": -0.7715875591190221, - "angularVelocity": -0.24807743502306231, - "velocityX": -1.0652600006026816, - "velocityY": 0.5051211146969536, - "timestamp": 11.584644425479405 - }, - { - "x": 2.4494186472691353, - "y": 3.0000246563455213, - "heading": -0.7837354404326269, - "angularVelocity": -0.2008656681268564, - "velocityX": -0.8521813718251273, - "velocityY": 0.40408172981098234, - "timestamp": 11.645122064226179 - }, - { - "x": 2.410766328604012, - "y": 3.018352457725689, - "heading": -0.7929452205270133, - "angularVelocity": -0.152284055482875, - "velocityX": -0.6391175228742725, - "velocityY": 0.3030508756618002, - "timestamp": 11.705599702972952 - }, - { - "x": 2.3849988097281045, - "y": 3.030570607297698, - "heading": -0.7991450800016932, - "angularVelocity": -0.10251490638778926, - "velocityX": -0.42606688041837987, - "velocityY": 0.20202755638603895, - "timestamp": 11.766077341719726 + "heading": -0.5965313002103377, + "angularVelocity": -8.528815258110653e-8, + "velocityX": -3.567836630187545, + "velocityY": 1.254483436778072, + "timestamp": 10.342175723579636 + }, + { + "x": 5.285431093647359, + "y": 1.6581112689784743, + "heading": -0.5965313049795081, + "angularVelocity": -7.601302145092289e-8, + "velocityX": -3.4677610362467894, + "velocityY": 1.509245007513348, + "timestamp": 10.404917216242477 + }, + { + "x": 5.070948144949037, + "y": 1.7596075143791599, + "heading": -0.5965313097283005, + "angularVelocity": -7.568822719486902e-8, + "velocityX": -3.4185184252932688, + "velocityY": 1.6176893646140194, + "timestamp": 10.467658708905319 + }, + { + "x": 4.856465419222307, + "y": 1.861104230964457, + "heading": -0.5965313144770923, + "angularVelocity": -7.568821760685447e-8, + "velocityX": -3.4185148714793154, + "velocityY": 1.6176968745502942, + "timestamp": 10.53040020156816 + }, + { + "x": 4.641982693508075, + "y": 1.9626009475761665, + "heading": -0.5965313192258841, + "angularVelocity": -7.568821790279648e-8, + "velocityX": -3.418514871280104, + "velocityY": 1.6176968749712675, + "timestamp": 10.593141694231 + }, + { + "x": 4.427499967793844, + "y": 2.064097664187878, + "heading": -0.5965313239746759, + "angularVelocity": -7.568821822441872e-8, + "velocityX": -3.4185148712800926, + "velocityY": 1.617696874971291, + "timestamp": 10.655883186893842 + }, + { + "x": 4.213017242079613, + "y": 2.165594380799589, + "heading": -0.5965313287234677, + "angularVelocity": -7.568821811615467e-8, + "velocityX": -3.4185148712800917, + "velocityY": 1.617696874971293, + "timestamp": 10.718624679556683 + }, + { + "x": 3.998534516366738, + "y": 2.267091097414165, + "heading": -0.5965313334722594, + "angularVelocity": -7.56882168459371e-8, + "velocityX": -3.418514871258486, + "velocityY": 1.6176968750169503, + "timestamp": 10.781366172219524 + }, + { + "x": 3.7840518148370346, + "y": 2.3685878651306878, + "heading": -0.5965313382224369, + "angularVelocity": -7.571030356184493e-8, + "velocityX": -3.418514485817036, + "velocityY": 1.6176976895010247, + "timestamp": 10.844107664882365 + }, + { + "x": 3.5823467745214304, + "y": 2.46403001529431, + "heading": -0.6258886394381601, + "angularVelocity": -0.46790887449049295, + "velocityX": -3.214858808022383, + "velocityY": 1.5211966772373215, + "timestamp": 10.906849157545206 + }, + { + "x": 3.3961574491843427, + "y": 2.552130310684733, + "heading": -0.6529986278149837, + "angularVelocity": -0.43209026795882993, + "velocityX": -2.967562890759225, + "velocityY": 1.40417914288166, + "timestamp": 10.969590650208048 + }, + { + "x": 3.225483861981861, + "y": 2.6328887858865286, + "heading": -0.6778585687680168, + "angularVelocity": -0.3962280764760425, + "velocityX": -2.7202666044246855, + "velocityY": 1.2871621597492864, + "timestamp": 11.032332142870889 + }, + { + "x": 3.0703260256030673, + "y": 2.7063054766361296, + "heading": -0.7004659993691003, + "angularVelocity": -0.36032662982009556, + "velocityX": -2.4729701158463, + "velocityY": 1.17014574619904, + "timestamp": 11.09507363553373 + }, + { + "x": 2.9306839491479293, + "y": 2.7723804156875507, + "heading": -0.7208186227896802, + "angularVelocity": -0.32438857535555815, + "velocityX": -2.2256734822288196, + "velocityY": 1.0531298546958971, + "timestamp": 11.157815128196571 + }, + { + "x": 2.8065576397395007, + "y": 2.8311136320269066, + "heading": -0.7389143343573537, + "angularVelocity": -0.28841697574707026, + "velocityX": -1.9783767350811556, + "velocityY": 0.9361144251855119, + "timestamp": 11.220556620859412 + }, + { + "x": 2.697947103240289, + "y": 2.8825051506567756, + "heading": -0.7547512635952348, + "angularVelocity": -0.25241556370017226, + "velocityX": -1.7310798944944172, + "velocityY": 0.8190993941766099, + "timestamp": 11.283298113522253 + }, + { + "x": 2.6048523446905505, + "y": 2.9265549925489487, + "heading": -0.7683278153992709, + "angularVelocity": -0.21638872822158642, + "velocityX": -1.4837829735739574, + "velocityY": 0.702084697424841, + "timestamp": 11.346039606185094 + }, + { + "x": 2.527273368620795, + "y": 2.9632631746604594, + "heading": -0.779642706302846, + "angularVelocity": -0.18034143631837077, + "velocityX": -1.2364859804443729, + "velocityY": 0.5850702709413225, + "timestamp": 11.408781098847935 + }, + { + "x": 2.4652101792880994, + "y": 2.9926297099741825, + "heading": -0.7886949946743065, + "angularVelocity": -0.1442791362982944, + "velocityX": -0.9891889194637066, + "velocityY": 0.46805605138424683, + "timestamp": 11.471522591510777 + }, + { + "x": 2.4186627808554446, + "y": 3.014654607545873, + "heading": -0.7954841045559687, + "angularVelocity": -0.10820765642515946, + "velocityX": -0.7418917921317392, + "velocityY": 0.3510419761615724, + "timestamp": 11.534264084173618 + }, + { + "x": 2.3876311775224326, + "y": 3.0293378725476474, + "heading": -0.8000098431420335, + "angularVelocity": -0.07213310353301693, + "velocityX": -0.49459459786475296, + "velocityY": 0.23402798337424993, + "timestamp": 11.597005576836459 }, { "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": -0.05171055025787837, - "velocityX": -0.21302809407950682, - "velocityY": 0.10101087163407141, - "timestamp": 11.826554980466499 + "angularVelocity": -0.036061762982130895, + "velocityX": -0.24729733470577522, + "velocityY": 0.11701401166345779, + "timestamp": 11.6597470694993 }, { "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": -1.423507620090277e-32, - "velocityY": 0, - "timestamp": 11.887032619213272 - }, - { - "x": 2.3839325851596245, - "y": 3.031084448154912, - "heading": -0.7992419373744997, - "angularVelocity": 0.052323339006150715, - "velocityX": 0.20403271649422824, - "velocityY": -0.09660273136476924, - "timestamp": 11.944950838169635 - }, - { - "x": 2.407567371385834, - "y": 3.019894305720653, - "heading": -0.7932053890740913, - "angularVelocity": 0.10422537863183293, - "velocityX": 0.4080717026885203, - "velocityY": -0.19320591406117554, - "timestamp": 12.002869057125999 - }, - { - "x": 2.4430201410442214, - "y": 3.003109041895344, - "heading": -0.7841908751963366, - "angularVelocity": 0.15564211124217459, - "velocityX": 0.6121177463191404, - "velocityY": -0.2898097373808346, - "timestamp": 12.060787276082362 - }, - { - "x": 2.490291356941472, - "y": 2.980728609584666, - "heading": -0.7722307955425777, - "angularVelocity": 0.2064994378154119, - "velocityX": 0.8161717806423892, - "velocityY": -0.38641437381801885, - "timestamp": 12.118705495038725 - }, - { - "x": 2.5493815465754834, - "y": 2.952752953058649, - "heading": -0.7573625934321272, - "angularVelocity": 0.2567102783608131, - "velocityX": 1.0202349225989056, - "velocityY": -0.4830199724735087, - "timestamp": 12.176623713995088 - }, - { - "x": 2.6202913158109356, - "y": 2.9191820098836145, - "heading": -0.7396297457639593, - "angularVelocity": 0.30617045875544285, - "velocityX": 1.2243085252479435, - "velocityY": -0.5796266490916698, - "timestamp": 12.234541932951451 - }, - { - "x": 2.7030213667788137, - "y": 2.8800157137424582, - "heading": -0.7190830975629909, - "angularVelocity": 0.35475276296822733, - "velocityX": 1.428394250696986, - "velocityY": -0.6762344707226781, - "timestamp": 12.292460151907814 - }, - { - "x": 2.797572521744028, - "y": 2.8352539986604253, - "heading": -0.6957827053311118, - "angularVelocity": 0.40229814817741966, - "velocityX": 1.632494173145279, - "velocityY": -0.7728434314556114, - "timestamp": 12.350378370864178 - }, - { - "x": 2.9039457555692096, - "y": 2.7848968055361296, - "heading": -0.6698004526247588, - "angularVelocity": 0.448602411720027, - "velocityX": 1.8366109273029618, - "velocityY": -0.8694534126167829, - "timestamp": 12.40829658982054 - }, - { - "x": 3.0221422408423324, - "y": 2.7289440926274007, - "heading": -0.6412238784283038, - "angularVelocity": 0.4933952512936468, - "velocityX": 2.040747927041322, - "velocityY": -0.9660641144867576, - "timestamp": 12.466214808776904 - }, - { - "x": 3.1521634121347564, - "y": 2.6673958532051016, - "heading": -0.6101619934003036, - "angularVelocity": 0.5363059428917012, - "velocityX": 2.2449096956932593, - "velocityY": -1.0626749325401574, - "timestamp": 12.524133027733267 - }, - { - "x": 3.2940110599022505, - "y": 2.600252147103853, - "heading": -0.5767545340853645, - "angularVelocity": 0.576803981837031, - "velocityX": 2.4491023778608563, - "velocityY": -1.1592847175054986, - "timestamp": 12.58205124668963 - }, - { - "x": 3.4476874712025682, - "y": 2.527513161589764, - "heading": -0.54118758447353, - "angularVelocity": 0.6140891459150629, - "velocityX": 2.6533345477370784, - "velocityY": -1.2558912691858224, - "timestamp": 12.639969465645994 - }, - { - "x": 3.613195643589333, - "y": 2.4491793412538243, - "heading": -0.5037220986038603, - "angularVelocity": 0.6468687495017285, - "velocityX": 2.8576184725476987, - "velocityY": -1.3524901446807027, - "timestamp": 12.697887684602357 - }, - { - "x": 3.7905395971194613, - "y": 2.3652517067540755, - "heading": -0.46475198370690785, - "angularVelocity": 0.6728472594489399, - "velocityX": 3.0619718065526835, - "velocityY": -1.4490713977752308, - "timestamp": 12.75580590355872 - }, - { - "x": 3.979724665259552, - "y": 2.275732818118546, - "heading": -0.4249432999516913, - "angularVelocity": 0.6873257581558144, - "velocityX": 3.266417226721487, - "velocityY": -1.545608450131651, - "timestamp": 12.813724122515083 - }, - { - "x": 4.180755824131631, - "y": 2.1806309244718047, - "heading": -0.3856731863911582, - "angularVelocity": 0.6780269536623745, - "velocityX": 3.47094856324124, - "velocityY": -1.6420030753776067, - "timestamp": 12.871642341471446 - }, - { - "x": 4.393589257468926, - "y": 2.0800009314187498, - "heading": -0.3515823344877535, - "angularVelocity": 0.5886032498528599, - "velocityX": 3.6747233801793717, - "velocityY": -1.7374497155872817, - "timestamp": 12.92956056042781 - }, - { - "x": 4.614791128841965, - "y": 1.9751825525280107, - "heading": -0.35158232270208695, - "angularVelocity": 2.034880699916496e-7, - "velocityX": 3.8192105240614955, - "velocityY": -1.8097652306904055, - "timestamp": 12.987478779384173 - }, - { - "x": 4.835992849247887, - "y": 1.870363855043589, - "heading": -0.3515823109197321, - "angularVelocity": 2.034308907507492e-7, - "velocityX": 3.8192079175048774, - "velocityY": -1.8097707314410818, - "timestamp": 13.045396998340536 - }, - { - "x": 5.0571945697179865, - "y": 1.7655451576946017, - "heading": -0.35158229913737715, - "angularVelocity": 2.0343089162960125e-7, - "velocityX": 3.8192079186129373, - "velocityY": -1.8097707291027114, - "timestamp": 13.103315217296899 - }, - { - "x": 5.278396987351414, - "y": 1.6607279316008212, - "heading": -0.35158228735502095, - "angularVelocity": 2.03430914726069e-7, - "velocityX": 3.8192199556427555, - "velocityY": -1.8097453268159511, - "timestamp": 13.161233436253262 + "angularVelocity": 2.8180807610025885e-36, + "velocityX": -1.7033240054088048e-37, + "velocityY": 8.364918339692578e-38, + "timestamp": 11.722488562162141 + }, + { + "x": 2.3862784269967885, + "y": 3.0299921206795677, + "heading": -0.7979445022798209, + "angularVelocity": 0.07214760590413402, + "velocityX": 0.2361025217555023, + "velocityY": -0.11148080618080956, + "timestamp": 11.782475441082498 + }, + { + "x": 2.414605712310544, + "y": 3.016617241283919, + "heading": -0.7893555783917804, + "angularVelocity": 0.1431800427464117, + "velocityX": 0.4722246901920792, + "velocityY": -0.22296341527296182, + "timestamp": 11.842462320002856 + }, + { + "x": 2.457098595113841, + "y": 2.996554734272641, + "heading": -0.7765849369819304, + "angularVelocity": 0.2128905794016223, + "velocityX": 0.7083696229589314, + "velocityY": -0.33444825555792773, + "timestamp": 11.902449198923213 + }, + { + "x": 2.5137586742709717, + "y": 2.9698044458506936, + "heading": -0.759727315329062, + "angularVelocity": 0.281021816041631, + "velocityX": 0.944541209292721, + "velocityY": -0.44593565965421617, + "timestamp": 11.96243607784357 + }, + { + "x": 2.5845878442638655, + "y": 2.9363662114763, + "heading": -0.7388971783301493, + "angularVelocity": 0.34724488711220036, + "velocityX": 1.180744377231747, + "velocityY": -0.5574258067133014, + "timestamp": 12.022422956763927 + }, + { + "x": 2.669588381015972, + "y": 2.8962398712988433, + "heading": -0.7142349782126004, + "angularVelocity": 0.41112657570152883, + "velocityX": 1.4169854855252446, + "velocityY": -0.6689186185320778, + "timestamp": 12.082409835684285 + }, + { + "x": 2.768763062860025, + "y": 2.849425297465716, + "heading": -0.6859166617272396, + "angularVelocity": 0.4720751770225952, + "velocityX": 1.653272909492815, + "velocityY": -0.7804135616937369, + "timestamp": 12.142396714604642 + }, + { + "x": 2.8821153449579997, + "y": 2.795922445610523, + "heading": -0.6541688123580982, + "angularVelocity": 0.5292465609236296, + "velocityX": 1.8896179320892514, + "velocityY": -0.8919092444570627, + "timestamp": 12.202383593525 + }, + { + "x": 3.0096496153353156, + "y": 2.73573145985777, + "heading": -0.6192942663212868, + "angularVelocity": 0.5813695705541383, + "velocityX": 2.1260361044394105, + "velocityY": -1.0034025246198655, + "timestamp": 12.262370472445356 + }, + { + "x": 3.151371569227902, + "y": 2.6688529097388005, + "heading": -0.5817190571900679, + "angularVelocity": 0.6263904675071726, + "velocityX": 2.3625492181506123, + "velocityY": -1.1148863105173612, + "timestamp": 12.322357351365714 + }, + { + "x": 3.30728870131474, + "y": 2.5952884046594, + "heading": -0.5420886244414685, + "angularVelocity": 0.6606516868666429, + "velocityX": 2.5991872705003334, + "velocityY": -1.2263432671179575, + "timestamp": 12.382344230286071 + }, + { + "x": 3.477410464899007, + "y": 2.5150425607670805, + "heading": -0.501501110267189, + "angularVelocity": 0.6766065330414338, + "velocityX": 2.8359829123654157, + "velocityY": -1.3377232710983202, + "timestamp": 12.442331109206428 + }, + { + "x": 3.661742704544347, + "y": 2.428132084038069, + "heading": -0.46226256646132613, + "angularVelocity": 0.6541187758402722, + "velocityX": 3.0728759849311573, + "velocityY": -1.4488247812392314, + "timestamp": 12.502317988126785 + }, + { + "x": 3.860146403975295, + "y": 2.3346891355318427, + "heading": -0.4327535611710109, + "angularVelocity": 0.4919243311440368, + "velocityX": 3.307451612782871, + "velocityY": -1.5577231252568988, + "timestamp": 12.562304867047143 + }, + { + "x": 4.065242173401202, + "y": 2.237711412138144, + "heading": -0.4327535477418668, + "angularVelocity": 2.238680243649846e-7, + "velocityX": 3.41901050891823, + "velocityY": -1.6166489262168846, + "timestamp": 12.6222917459675 + }, + { + "x": 4.270337924993822, + "y": 2.140733651028875, + "heading": -0.4327535343131544, + "angularVelocity": 2.2386082909736944e-7, + "velocityX": 3.419010211631728, + "velocityY": -1.6166495549472233, + "timestamp": 12.682278624887857 + }, + { + "x": 4.475433676585232, + "y": 2.0437558899170503, + "heading": -0.43275352088444197, + "angularVelocity": 2.2386082765717578e-7, + "velocityX": 3.419010211611586, + "velocityY": -1.6166495549898217, + "timestamp": 12.742265503808214 + }, + { + "x": 4.6805294281766425, + "y": 1.9467781288052255, + "heading": -0.43275350745572955, + "angularVelocity": 2.2386082872977313e-7, + "velocityX": 3.4190102116115844, + "velocityY": -1.616649554989824, + "timestamp": 12.802252382728572 + }, + { + "x": 4.885625179768054, + "y": 1.8498003676934025, + "heading": -0.4327534940270171, + "angularVelocity": 2.2386082836686737e-7, + "velocityX": 3.419010211611597, + "velocityY": -1.6166495549897981, + "timestamp": 12.862239261648929 + }, + { + "x": 5.090720931370458, + "y": 1.7528226066048298, + "heading": -0.4327534805983047, + "angularVelocity": 2.2386082909228912e-7, + "velocityX": 3.4190102117948658, + "velocityY": -1.616649554602208, + "timestamp": 12.922226140569286 + }, + { + "x": 5.295816845235169, + "y": 1.6558451886813434, + "heading": -0.4327534671695918, + "angularVelocity": 2.2386083672590555e-7, + "velocityX": 3.4190129167581613, + "velocityY": -1.6166438339330798, + "timestamp": 12.982213019489643 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.35158227553759686, - "angularVelocity": 2.0403638579093497e-7, - "velocityX": 3.8779958008952624, - "velocityY": -1.6801094533136367, - "timestamp": 13.219151655209625 - }, - { - "x": 5.78771748375251, - "y": 1.4618238256689142, - "heading": -0.3515822643983809, - "angularVelocity": 1.5573311965555955e-7, - "velocityX": 3.980476011114963, - "velocityY": -1.420363078204806, - "timestamp": 13.29067925283814 - }, - { - "x": 6.071768529925899, - "y": 1.3687010115486713, - "heading": -0.3384707068713864, - "angularVelocity": 0.18330767370505657, - "velocityX": 3.9712090939868023, - "velocityY": -1.3019144666913607, - "timestamp": 13.362206850466656 - }, - { - "x": 6.337299554037092, - "y": 1.2816927202415032, - "heading": -0.3077191306864491, - "angularVelocity": 0.4299260314130538, - "velocityX": 3.712287745077815, - "velocityY": -1.2164296606053124, - "timestamp": 13.433734448095171 - }, - { - "x": 6.583882404182654, - "y": 1.200909898706293, - "heading": -0.27261549465283386, - "angularVelocity": 0.4907705165204766, - "velocityX": 3.4473805680740197, - "velocityY": -1.1293937474981692, - "timestamp": 13.505262045723686 - }, - { - "x": 6.811487459476513, - "y": 1.1263530850980985, - "heading": -0.23658461598105995, - "angularVelocity": 0.5037339413928503, - "velocityX": 3.182059272785126, - "velocityY": -1.042350310650886, - "timestamp": 13.576789643352202 - }, - { - "x": 7.020112866315064, - "y": 1.0580186639861602, - "heading": -0.20123048803983856, - "angularVelocity": 0.49427254812661375, - "velocityX": 2.916712062972736, - "velocityY": -0.9553574197590016, - "timestamp": 13.648317240980717 - }, - { - "x": 7.209761115265014, - "y": 0.9959034826022224, - "heading": -0.16748852563901473, - "angularVelocity": 0.47173347797958504, - "velocityX": 2.6513996728214475, - "velocityY": -0.8684086065149057, - "timestamp": 13.719844838609232 - }, - { - "x": 7.380435279840303, - "y": 0.9400050984767416, - "heading": -0.13597326201279447, - "angularVelocity": 0.4406028535992147, - "velocityX": 2.3861302522936825, - "velocityY": -0.78149394050383, - "timestamp": 13.791372436237747 - }, - { - "x": 7.5321382283527605, - "y": 0.8903216280368556, - "heading": -0.10712014952027268, - "angularVelocity": 0.403384336244213, - "velocityX": 2.1209009325371073, - "velocityY": -0.6946056080049196, - "timestamp": 13.862900033866262 - }, - { - "x": 7.664872474877682, - "y": 0.8468515953524562, - "heading": -0.08125430314522812, - "angularVelocity": 0.36162051058083977, - "velocityX": 1.8557067611062361, - "velocityY": -0.6077379099206526, - "timestamp": 13.934427631494778 - }, - { - "x": 7.77864018727303, - "y": 0.8095938204087205, - "heading": -0.05862785653621709, - "angularVelocity": 0.3163317007586838, - "velocityX": 1.590542897668813, - "velocityY": -0.520886709172551, - "timestamp": 14.005955229123293 - }, - { - "x": 7.873443230968776, - "y": 0.7785473403012081, - "heading": -0.039441988553792, - "angularVelocity": 0.2682302861906329, - "velocityX": 1.3254051140947378, - "velocityY": -0.4340489704233464, - "timestamp": 14.077482826751808 - }, - { - "x": 7.9492832154345106, - "y": 0.7537113535170058, - "heading": -0.023860737345292347, - "angularVelocity": 0.21783551699054987, - "velocityX": 1.0602898318998024, - "velocityY": -0.3472224373197895, - "timestamp": 14.149010424380323 - }, - { - "x": 8.006161534793161, - "y": 0.7350851799018175, - "heading": -0.01202009660498375, - "angularVelocity": 0.1655394719364682, - "velocityX": 0.7951940404045607, - "velocityY": -0.26040541319344906, - "timestamp": 14.220538022008839 - }, - { - "x": 8.044079401282437, - "y": 0.7226682313019233, - "heading": -0.004034250807333449, - "angularVelocity": 0.11164705739350395, - "velocityX": 0.5301151967413267, - "velocityY": -0.17359661181943797, - "timestamp": 14.292065619637354 + "heading": -0.4327534537085892, + "angularVelocity": 2.2439911588435075e-7, + "velocityX": 3.4538678416562716, + "velocityY": -1.5407736813730113, + "timestamp": 13.04219989841 + }, + { + "x": 5.767637732978587, + "y": 1.4715486447521433, + "heading": -0.43275344189521775, + "angularVelocity": 1.5949042736003556e-7, + "velocityX": 3.5727828839624562, + "velocityY": -1.2403266728714895, + "timestamp": 13.116269368711977 + }, + { + "x": 6.033774192023134, + "y": 1.384125705645109, + "heading": -0.43275343009903516, + "angularVelocity": 1.5925836231633141e-7, + "velocityX": 3.5930655094403474, + "velocityY": -1.1802830336252825, + "timestamp": 13.190338839013954 + }, + { + "x": 6.2999107025526015, + "y": 1.2967029232707166, + "heading": -0.43275341830285263, + "angularVelocity": 1.5925836222768275e-7, + "velocityX": 3.593066204529984, + "velocityY": -1.1802809176031077, + "timestamp": 13.26440830931593 + }, + { + "x": 6.566047160708243, + "y": 1.2092799814598572, + "heading": -0.4327534065048956, + "angularVelocity": 1.5928231960994732e-7, + "velocityX": 3.5930654974393894, + "velocityY": -1.1802830701291929, + "timestamp": 13.338477779617907 + }, + { + "x": 6.815604168443372, + "y": 1.1270295237235333, + "heading": -0.36800659105834677, + "angularVelocity": 0.874136336908857, + "velocityX": 3.369229005117782, + "velocityY": -1.1104501949452927, + "timestamp": 13.412547249919884 + }, + { + "x": 7.0424486777806585, + "y": 1.0523256982782683, + "heading": -0.30458898646479193, + "angularVelocity": 0.8561908750664163, + "velocityX": 3.0625912189253857, + "velocityY": -1.0085643267152118, + "timestamp": 13.48661672022186 + }, + { + "x": 7.2465905745815595, + "y": 0.9851213052520007, + "heading": -0.2456798424413219, + "angularVelocity": 0.7953228743678266, + "velocityX": 2.756086900157761, + "velocityY": -0.9073156963628867, + "timestamp": 13.560686190523837 + }, + { + "x": 7.428038548794458, + "y": 0.9253999088853845, + "heading": -0.1923064776337199, + "angularVelocity": 0.7205852099387569, + "velocityX": 2.449699902984963, + "velocityY": -0.8062889625528059, + "timestamp": 13.634755660825814 + }, + { + "x": 7.586797949690042, + "y": 0.8731533407207944, + "heading": -0.14497999296920738, + "angularVelocity": 0.6389472541327177, + "velocityX": 2.1433851254549703, + "velocityY": -0.7053725097747339, + "timestamp": 13.70882513112779 + }, + { + "x": 7.722872327084394, + "y": 0.8283767866857153, + "heading": -0.10400779573923787, + "angularVelocity": 0.5531590419497899, + "velocityX": 1.837118273420693, + "velocityY": -0.6045210510150557, + "timestamp": 13.782894601429767 + }, + { + "x": 7.836264194581955, + "y": 0.7910670998170554, + "heading": -0.06959570799723021, + "angularVelocity": 0.46459205934255987, + "velocityX": 1.5308853571555237, + "velocityY": -0.5037120788977025, + "timestamp": 13.856964071731744 + }, + { + "x": 7.926975414267803, + "y": 0.7612220655404804, + "heading": -0.04189104317473832, + "angularVelocity": 0.3740362218001813, + "velocityX": 1.2246775806013535, + "velocityY": -0.4029330053920843, + "timestamp": 13.93103354203372 + }, + { + "x": 7.99500740651227, + "y": 0.7388400306235591, + "heading": -0.021003806792755153, + "angularVelocity": 0.28199521741990785, + "velocityX": 0.9184889802384881, + "velocityY": -0.30217625191150127, + "timestamp": 14.005103012335697 + }, + { + "x": 8.040361272813582, + "y": 0.7239196956147146, + "heading": -0.007018297206773477, + "angularVelocity": 0.18881611450660732, + "velocityX": 0.6123152510259342, + "velocityY": -0.20143704211755917, + "timestamp": 14.079172482637674 }, { "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": -6.370509592451438e-38, - "angularVelocity": 0.05640131838742408, - "velocityX": 0.2650511363527046, - "velocityY": -0.08679505477643516, - "timestamp": 14.36359321726587 + "heading": 2.0779165225884802e-41, + "angularVelocity": 0.09475290127174345, + "velocityX": 0.3061531209608937, + "velocityY": -0.10071229126619018, + "timestamp": 14.15324195293965 }, { "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": -1.0565058286352088e-38, - "angularVelocity": 7.429305530105007e-37, - "velocityX": -1.702944321634373e-36, - "velocityY": 1.0409042171025374e-38, - "timestamp": 14.435120814894384 + "heading": 1.169210048317657e-41, + "angularVelocity": -1.2268302369298258e-40, + "velocityX": -9.944733020002764e-42, + "velocityY": 5.460591898189625e-42, + "timestamp": 14.227311423241627 } ] } \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index ab8aabc3..b96f5413 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -64,7 +64,7 @@ public static class Drivetrain { public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (IS_L3 ? 16.0 / 28.0 : 17.0 / 27.0) * (45.0 / 15.0); public static final double TURN_GEAR_RATIO = 150.0 / 7.0; public static final boolean IS_TURN_MOTOR_INVERTED = true; - public static final double WHEEL_RADIUS = Units.inchesToMeters(1.967); + public static final double WHEEL_RADIUS = Units.inchesToMeters(1.972022165051841); public static final double MAX_LINEAR_SPEED = Units.feetToMeters(IS_L3 ? 16.0 : 15.7); public static final double TRACK_WIDTH_X = Units.inchesToMeters(18.75); @@ -265,9 +265,9 @@ public static class Tramp { public static class Targeting { // TODO: tune these - public static final double ROT_KP = 0.6; + public static final double ROT_KP = 0.4; public static final double ROT_KI = 0.0; - public static final double ROT_KD = 0.028; + public static final double ROT_KD = 0.0; public static final Pose2d SPEAKER_POSE = new Pose2d(Units.inchesToMeters(8.861), Units.inchesToMeters(218), new Rotation2d()); diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 97cc2206..c8b99f9c 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -133,6 +133,7 @@ public void enabledInit() { // robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK,flamePattern); robotContainer.elevator.setBrakeMode(true); robotContainer.shooter.setPivotBrakeMode(true); + robotContainer.drivetrain.setBrakeMode(true); } /** diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 6b4ed94f..9c666711 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -10,6 +10,7 @@ import org.team1540.robot2024.commands.autos.*; import org.team1540.robot2024.commands.climb.ClimbAlignment; import org.team1540.robot2024.commands.drivetrain.SwerveDriveCommand; +import org.team1540.robot2024.commands.drivetrain.WheelRadiusCharacterization; import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; import org.team1540.robot2024.commands.indexer.IntakeCommand; @@ -56,7 +57,7 @@ public class RobotContainer { */ public RobotContainer() { switch (Constants.currentMode) { - case REAL: + case REAL -> { // Real robot, instantiate hardware IO implementations elevator = Elevator.createReal(); drivetrain = Drivetrain.createReal(odometrySignalRefresher, elevator::getVelocity); @@ -67,30 +68,28 @@ public RobotContainer() { drivetrain::addVisionMeasurement, elevator::getPosition, drivetrain::getVisionPose); - break; - case SIM: + } + case SIM -> { // Sim robot, instantiate physics sim IO implementations elevator = Elevator.createSim(); drivetrain = Drivetrain.createSim(elevator::getVelocity); tramp = Tramp.createSim(); shooter = Shooter.createSim(); - indexer = Indexer.createSim(); aprilTagVision = AprilTagVision.createSim( drivetrain::addVisionMeasurement, drivetrain::getPose, elevator::getPosition); - break; - default: + } + default -> { // Replayed robot, disable IO implementations elevator = Elevator.createDummy(); drivetrain = Drivetrain.createDummy(); tramp = Tramp.createDummy(); shooter = Shooter.createDummy(); - indexer = Indexer.createDummy(); aprilTagVision = AprilTagVision.createDummy(); - break; + } } // Set up FF characterization routines @@ -136,32 +135,42 @@ private void configureButtonBindings() { // drivetrain.setDefaultCommand(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter)); elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); shooter.setDefaultCommand(manualPivotCommand); - driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); + driver.b().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); // driver.y().toggleOnTrue(new DriveWithSpeakerTargetingCommand(drivetrain, driver)); driver.y().onTrue(Commands.runOnce(() -> { drivetrain.zeroFieldOrientationManual(); drivetrain.setBrakeMode(true); }).ignoringDisable(true)); + Command targetDrive = new AutoShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter) .alongWith(leds.commandShowPattern(new LedPatternWave("#00a9ff"), Leds.PatternLevel.DRIVER_LOCK)); Command overstageTargetDrive = new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter) .alongWith(leds.commandShowPattern(new LedPatternWave("#f700ff"), Leds.PatternLevel.DRIVER_LOCK)); Command autoShooterCommand = new AutoShootPrepare(drivetrain, shooter) .alongWith(leds.commandShowPattern(new LedPatternWave("#00ffbc"), Leds.PatternLevel.DRIVER_LOCK)); + Command cancelAlignment = Commands.runOnce(() -> { + targetDrive.cancel(); + overstageTargetDrive.cancel(); + }); + driver.x() + .whileTrue(IntakeAndFeed.withDefaults(indexer)) + .onFalse(cancelAlignment); driver.rightBumper().toggleOnTrue(targetDrive); driver.leftBumper().toggleOnTrue(overstageTargetDrive); // TODO remove this - driver.a().whileTrue(new TuneShooterCommand(shooter, indexer, drivetrain::getPose)); + if (isTuningMode()) { + driver.leftTrigger().whileTrue(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter).alongWith(leds.commandShowPattern(new LedPatternWave("#00ff00"), Leds.PatternLevel.DRIVER_LOCK))); + driver.a().whileTrue(new TuneShooterCommand(shooter, indexer, drivetrain::getPose)); + } driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); - driver.rightStick().onTrue(Commands.runOnce(() -> { - targetDrive.cancel(); - overstageTargetDrive.cancel(); - })); + driver.rightStick().onTrue(cancelAlignment); + + copilot.back().onTrue(Commands.runOnce(shooter::zeroPivotToCancoder).andThen(Commands.print("BACK IS PRESSED"))); copilot.leftBumper().whileTrue(new AmpScoreSequence(tramp, indexer, elevator)); Command intakeCommand = new IntakeCommand(indexer, tramp::isNoteStaged, 1) @@ -179,14 +188,8 @@ private void configureButtonBindings() { copilot.x().whileTrue(new ShootSequence(shooter, indexer)); copilot.a().whileTrue(new AmpScoreStageSequence(indexer, tramp, elevator)); -// copilot.b().whileTrue(new ShootSequence(shooter, indexer, PODIUM_SHOOT)); -// copilot.b().whileTrue(new TuneShooterCommand(shooter, indexer)); - copilot.b() - .whileTrue(IntakeAndFeed.withDefaults(indexer)) - .onFalse(Commands.runOnce(() -> { - targetDrive.cancel(); - overstageTargetDrive.cancel(); - })); + copilot.b().whileTrue(IntakeAndFeed.withDefaults(indexer)) + .onFalse(cancelAlignment); copilot.y().whileTrue(new StageTrampCommand(tramp, indexer)); // copilot.leftTrigger(0.5).whileTrue(new ElevatorSetpointCommand(elevator, ElevatorState.CLIMB)); @@ -255,7 +258,7 @@ private void configureAutoRoutines() { autos.add(new DriveSinglePath("SourceLaneSprint", drivetrain)); autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer)); autos.add(new SourceLanePHG(drivetrain, shooter, indexer)); - + autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.CLOCKWISE))); } diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java index 6316fdaa..3d5770b3 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java @@ -1,6 +1,8 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -25,10 +27,16 @@ public AmpLanePADESprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, indexer, 0), + createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), createSegmentSequence(drivetrain, indexer, 1), +// new WaitCommand(5), +// drivetrain.commandCopyVisionPose(), createSegmentSequence(drivetrain, indexer, 2), - createSegmentSequence(drivetrain, indexer, 3) +// new WaitCommand(5), +// drivetrain.commandCopyVisionPose(), + createSegmentSequence(drivetrain, indexer, 3), + getPath(4).getCommand(drivetrain) +// drivetrain.commandCopyVisionPose() ) ), getPath(4).getCommand(drivetrain) diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java index 670c6ba9..d001e634 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java @@ -27,7 +27,7 @@ public CenterLanePCBAFSprint(Drivetrain drivetrain, Shooter shooter, Indexer ind Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, indexer, 0), + createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), drivetrain.commandCopyVisionPose(), createSegmentSequence(drivetrain, indexer, 1), drivetrain.commandCopyVisionPose(), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java index bd5ca822..0ae22a75 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java @@ -22,12 +22,13 @@ public SourceLanePGHSprint(Drivetrain drivetrain, Shooter shooter, Indexer index Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, indexer, 0), + createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), drivetrain.commandCopyVisionPose(), createSegmentSequence(drivetrain, indexer, 1), drivetrain.commandCopyVisionPose(), createSegmentSequence(drivetrain, indexer, 2), - drivetrain.commandCopyVisionPose() + drivetrain.commandCopyVisionPose(), + getPath(3).getCommand(drivetrain) ) ), getPath(3).getCommand(drivetrain) diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java index b3f1b7a5..87927900 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java @@ -54,13 +54,18 @@ public void execute() { Rotation2d targetRot = drivetrain.getPose() .minus(target.get()).getTranslation().getAngle() - .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0)); + .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0)) + .minus(Rotation2d.fromDegrees(3)); Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); Logger.recordOutput("Targeting/rotErrorDegrees", Math.abs(targetRot.minus(drivetrain.getRotation()).getDegrees())); Logger.recordOutput("Targeting/target", target.get()); - double xPercent = MathUtil.applyDeadband((-controller.getLeftY()), 0.1); - double yPercent = MathUtil.applyDeadband((-controller.getLeftX()), 0.1); + double xPercent = 0; + double yPercent = 0; + if(controller != null){ + xPercent = MathUtil.applyDeadband((-controller.getLeftY()), 0.1); + yPercent = MathUtil.applyDeadband((-controller.getLeftX()), 0.1); + } double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians()); drivetrain.drivePercent(xPercent, yPercent, rotPercent, true); } diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java index f59c9f14..f7e939f9 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.DoubleSupplier; @@ -60,8 +61,9 @@ public void initialize() { @Override public void execute() { // Run drive at velocity - drive.runWheelRadiusCharacterization( - omegaLimiter.calculate(omegaDirection.value * characterizationSpeed.get())); + + drive.runVelocity(new ChassisSpeeds(0, 0, omegaLimiter.calculate(omegaDirection.value * characterizationSpeed.get()))); + System.out.println("running characterization"); // Get yaw and wheel positions accumGyroYawRads += MathUtil.angleModulus(gyroYawRadsSupplier.getAsDouble() - lastGyroYawRads); @@ -91,5 +93,7 @@ public void end(boolean interrupted) { + Units.metersToInches(currentEffectiveWheelRadius) + " inches"); } + drive.stop(); + drive.stopCharacterization(); } } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java index 9a8ea9ec..4bfc6108 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import org.team1540.robot2024.Constants; -import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerTargetingCommand; import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.shooter.Shooter; @@ -25,7 +24,7 @@ public AutoShootPrepareWhileMoving(XboxController controller, Drivetrain drivetr new Rotation3d()); double flightDistance = speakerPose3d.getTranslation().getDistance(new Translation3d(drivetrain.getPose().getX(), drivetrain.getPose().getY(), Constants.Shooter.Pivot.PIVOT_HEIGHT)); // double NOTE_VELOCITY = 6000.0*1.5*2.0*Math.PI*2.54/100.0/60.0; - double NOTE_VELOCITY = 24.0; + double NOTE_VELOCITY = 12.18; double time = flightDistance/NOTE_VELOCITY; Translation2d modifier = new Translation2d(drivetrain.getChassisSpeeds().vxMetersPerSecond * time,drivetrain.getChassisSpeeds().vyMetersPerSecond*time); return modifier; diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWithTargeting.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWithTargeting.java index 41eb13e2..e14ea09b 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWithTargeting.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWithTargeting.java @@ -2,14 +2,14 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerTargetingCommand; +import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.shooter.Shooter; public class AutoShootPrepareWithTargeting extends ParallelCommandGroup { public AutoShootPrepareWithTargeting(XboxController controller, Drivetrain drivetrain, Shooter shooter) { addCommands( - new DriveWithSpeakerTargetingCommand(drivetrain,controller), + new DriveWithTargetingCommand(drivetrain,controller), new AutoShootPrepare(drivetrain, shooter) ); } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java index 3cc006c7..09291aa7 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java @@ -25,7 +25,7 @@ public OverStageShootPrepare(Supplier positionSupplier, Shooter shooter) ) ) ).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), - 3000, 3000) + 3500, 3500) ) ); } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java index 73f56b89..94901017 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java @@ -1,15 +1,18 @@ package org.team1540.robot2024.commands.shooter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import org.team1540.robot2024.commands.drivetrain.DriveWithSpeakerTargetingCommand; +import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.vision.AprilTagsCrescendo; public class OverStageShootPrepareWithTargeting extends ParallelCommandGroup { public OverStageShootPrepareWithTargeting(XboxController controller, Drivetrain drivetrain, Shooter shooter) { addCommands( - new DriveWithSpeakerTargetingCommand(drivetrain,controller), + new DriveWithTargetingCommand(drivetrain,controller, ()-> AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().plus(new Transform2d(0,-2,new Rotation2d()))), new OverStageShootPrepare(drivetrain, shooter) ); } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java index 2f2fd4af..f5f820bc 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java @@ -16,8 +16,8 @@ import java.util.function.Supplier; public class TuneShooterCommand extends ParallelCommandGroup { - private final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 8000); - private final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000); + private final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 7000); + private final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 3000); private final LoggedDashboardNumber angleSetpoint = new LoggedDashboardNumber("Shooter/Pivot/angleSetpoint", 15); private final DoubleSupplier shooterLeftSupplier; @@ -44,10 +44,7 @@ public TuneShooterCommand(Shooter shooter, Indexer indexer, Supplier pos Commands.print("Shot Number: " + shotNum + " Angle Degrees Setpoint: " + pivotRotationSupplier.get() + " Left RPM Setpoint: " + shooterLeftSupplier.getAsDouble() + " Right RPM Setpoint: " + shooterRightSupplier.getAsDouble() + " Angle Degrees: " + shooter.getPivotPosition().getDegrees() + " Left RPM: " + shooter.getLeftFlywheelSpeed() + " Right RPM: " + shooter.getRightFlywheelSpeed() + " Distance: " + poseSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation())) - ).repeatedly(), - Commands.runOnce( - () -> Logger.recordOutput("shooterTuning/Distance", - poseSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation()))).repeatedly() + ).repeatedly() ); addRequirements(shooter, indexer); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 2781778a..cc02029d 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -29,6 +29,7 @@ import org.team1540.robot2024.Constants; import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher; import org.team1540.robot2024.util.swerve.SwerveFactory; +import org.team1540.robot2024.util.vision.AprilTagsCrescendo; import org.team1540.robot2024.util.vision.EstimatedVisionPose; import org.team1540.robot2024.util.vision.VisionPoseAcceptor; @@ -282,6 +283,11 @@ public ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } + @AutoLogOutput(key = "Odometry/ChassisSpeedMagnitude") + public double ChassisSpeedMagnitude(){ + return Math.hypot(getChassisSpeeds().vxMetersPerSecond, getChassisSpeeds().vyMetersPerSecond); + } + /** * Returns the module states (turn angles and drive velocities) for all the modules. */ @@ -412,4 +418,13 @@ public void runWheelRadiusCharacterization(double omegaSpeed) { isCharacterizingWheels = true; characterizationInput = omegaSpeed; } + + public void stopCharacterization() { + isCharacterizingWheels = false; + } + + @AutoLogOutput(key = "Targeting/SpeakerDistance") + public double getSpeakerDistanceMeters(){ + return getPose().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation()); + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java index 57db7c03..f04a6a53 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java @@ -32,7 +32,8 @@ public Module(ModuleIO io, int index) { switch (Constants.currentMode) { case REAL: case REPLAY: - driveFeedforward = new SimpleMotorFeedforward(0.206, 0.117); + driveFeedforward = new SimpleMotorFeedforward(0.258, 0.128); +// driveFeedforward = new SimpleMotorFeedforward(0.206, 0.117); driveFeedback = new PIDController(0.05, 0.0, 0.0); turnFeedback = new PIDController(7.0, 0.0, 0.0); break; diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java index 82f950db..322f824f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java @@ -37,6 +37,8 @@ public class FlywheelsIOTalonFX implements FlywheelsIO { public FlywheelsIOTalonFX() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.Voltage.PeakForwardVoltage = 10; + config.Voltage.PeakReverseVoltage = 10; // Shooter current limits are banned config.CurrentLimits.SupplyCurrentLimitEnable = false; diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index 39e26b7b..c57ce266 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -48,15 +48,20 @@ public class Shooter extends SubsystemBase { private final LoggedTunableNumber pivotKD = new LoggedTunableNumber("Shooter/Pivot/kD", Pivot.KD); private final LoggedTunableNumber pivotKG = new LoggedTunableNumber("Shooter/Pivot/kG", Pivot.KG); + private boolean flipper = false; + public final ShooterLerp lerp = new ShooterLerp().put( new Pair<>(1.2954, new ShooterSetpoint(Rotation2d.fromDegrees(40))), new Pair<>(1.842, new ShooterSetpoint(Rotation2d.fromDegrees(37))), + new Pair<>(2.012, new ShooterSetpoint(Rotation2d.fromDegrees(34.422))), new Pair<>(2.238, new ShooterSetpoint(Rotation2d.fromDegrees(31))), - new Pair<>(2.707, new ShooterSetpoint(Rotation2d.fromDegrees(27))), - new Pair<>(3.208, new ShooterSetpoint(Rotation2d.fromDegrees(23))), - new Pair<>(3.775, new ShooterSetpoint(Rotation2d.fromDegrees(20.625))), - new Pair<>(4.355, new ShooterSetpoint(Rotation2d.fromDegrees(15.5))), - new Pair<>(3.831, new ShooterSetpoint(Rotation2d.fromDegrees(22))) + new Pair<>(2.747, new ShooterSetpoint(Rotation2d.fromDegrees(25.5))), + new Pair<>(3.175, new ShooterSetpoint(Rotation2d.fromDegrees(22))), + new Pair<>(3.197, new ShooterSetpoint(Rotation2d.fromDegrees(22))), +// new Pair<>(3.990, new ShooterSetpoint(Rotation2d.fromDegrees(17.2))), + new Pair<>(4.029, new ShooterSetpoint(Rotation2d.fromDegrees(16.5))), + new Pair<>(4.256, new ShooterSetpoint(Rotation2d.fromDegrees(16.2))), + new Pair<>(5.300, new ShooterSetpoint(Rotation2d.fromDegrees(13))) ); private static boolean hasInstance = false; @@ -117,6 +122,7 @@ public void periodic() { rightSpeedFilter.add(getRightFlywheelSpeed()); pivotPositionFilter.add(getPivotPosition().getRotations()); Logger.recordOutput("Shooter/Pivot/Error", pivotSetpoint.getDegrees() - pivotInputs.position.getDegrees()); + Logger.recordOutput("Shooter/Pivot/ResettingToCancoder", flipper); } /** @@ -274,4 +280,8 @@ public Rotation2d getPivotSetpoint(){ public void zeroPivot() { pivotIO.setEncoderPosition(0); } + public void zeroPivotToCancoder(){ + pivotIO.setEncoderPosition(Rotation2d.fromDegrees(pivotInputs.absolutePosition.getDegrees()*0.984 - 140).getRotations()); + flipper = !flipper; + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java index c935f83b..d01109b3 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -20,12 +20,13 @@ public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) { @Override public void updateInputs(AprilTagVisionIOInputs inputs) { LimelightHelpers.PoseEstimate measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); + if (measurement.tagCount > 1) { + inputs.estimatedPoseMeters = new Pose3d(measurement.pose); + inputs.numTagsSeen = measurement.tagCount; + inputs.avgTagDistance = measurement.avgTagDist; - inputs.estimatedPoseMeters = new Pose3d(measurement.pose); - inputs.numTagsSeen = measurement.tagCount; - inputs.avgTagDistance = measurement.avgTagDist; - - inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds; + inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds; + } } @Override diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index 9f9b1d98..aa6395e4 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -4,11 +4,14 @@ import edu.wpi.first.math.proto.Trajectory; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; import java.util.ArrayList; import java.util.List; @@ -92,7 +95,31 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Indexer indexer, new IntakeCommand(indexer, () -> false, 1) ), drivetrain.commandStop(), - IntakeAndFeed.withDefaults(indexer).withTimeout(0.5) + drivetrain.commandCopyVisionPose(), + Commands.parallel( + new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4), +// IntakeAndFeed.withDefaults(indexer).withTimeout(0.5), + IntakeAndFeed.withDefaults(indexer).until(()->!indexer.isNoteStaged()).andThen(Commands.waitSeconds(0.2)) + ) + ); + } + + protected Command createCancoderSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex) { + return Commands.sequence( + Commands.deadline( + getPath(pathIndex).getCommand(drivetrain), + new IntakeCommand(indexer, () -> false, 1) + ), + drivetrain.commandStop(), + Commands.waitSeconds(0.25), + new InstantCommand(shooter::zeroPivotToCancoder), + drivetrain.commandCopyVisionPose(), + Commands.parallel( + new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4), + IntakeAndFeed.withDefaults(indexer).until(()->!indexer.isNoteStaged()).andThen(Commands.waitSeconds(0.1)) + ) + +// IntakeAndFeed.withDefaults(indexer).withTimeout(0.5) ); } diff --git a/src/main/java/org/team1540/robot2024/util/shooter/ShooterSetpoint.java b/src/main/java/org/team1540/robot2024/util/shooter/ShooterSetpoint.java index 0bd1fedc..3d9344c2 100644 --- a/src/main/java/org/team1540/robot2024/util/shooter/ShooterSetpoint.java +++ b/src/main/java/org/team1540/robot2024/util/shooter/ShooterSetpoint.java @@ -17,6 +17,6 @@ public ShooterSetpoint(double pivotRots, double leftSetpoint, double rightSetpoi } public ShooterSetpoint(Rotation2d pivot){ - this(pivot, 8000, 6000); + this(pivot, 7000, 3000); } } From eba92beb7872584f3fa69f00d3c6be4c07b0282a Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sun, 17 Mar 2024 12:03:37 -0700 Subject: [PATCH 32/75] feat: skillet code --- .../org/team1540/robot2024/Constants.java | 10 +++---- .../team1540/robot2024/RobotContainer.java | 27 ++++++++++++------- .../drivetrain/SwerveDriveCommand.java | 7 ++--- 3 files changed, 27 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index b96f5413..ea5c05ac 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -51,16 +51,16 @@ public static class SwerveConfig { public static final String CAN_BUS = IS_COMPETITION_ROBOT ? "swerve" : "swerve"; public static final double CAN_UPDATE_FREQUENCY_HZ = 200.0; - public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 1 : 1; - public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 7 : 9; - public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 4 : 5; - public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 3 : 6; + public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 1 : 9; + public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 7 : 2; + public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 4 : 6; + public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 3 : 5; public static final int PIGEON_ID = 9; } public static class Drivetrain { - public static final boolean IS_L3 = false; + public static final boolean IS_L3 = !IS_COMPETITION_ROBOT; public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (IS_L3 ? 16.0 / 28.0 : 17.0 / 27.0) * (45.0 / 15.0); public static final double TURN_GEAR_RATIO = 150.0 / 7.0; public static final boolean IS_TURN_MOTOR_INVERTED = true; diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 9c666711..bf52d318 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -59,15 +59,24 @@ public RobotContainer() { switch (Constants.currentMode) { case REAL -> { // Real robot, instantiate hardware IO implementations - elevator = Elevator.createReal(); - drivetrain = Drivetrain.createReal(odometrySignalRefresher, elevator::getVelocity); - tramp = Tramp.createReal(); - shooter = Shooter.createReal(); - indexer = Indexer.createReal(); - aprilTagVision = AprilTagVision.createReal( - drivetrain::addVisionMeasurement, - elevator::getPosition, - drivetrain::getVisionPose); + if (Constants.IS_COMPETITION_ROBOT) { + elevator = Elevator.createReal(); + drivetrain = Drivetrain.createReal(odometrySignalRefresher, elevator::getVelocity); + tramp = Tramp.createReal(); + shooter = Shooter.createReal(); + indexer = Indexer.createReal(); + aprilTagVision = AprilTagVision.createReal( + drivetrain::addVisionMeasurement, + elevator::getPosition, + drivetrain::getVisionPose); + } else { + elevator = Elevator.createDummy(); + drivetrain = Drivetrain.createReal(odometrySignalRefresher, () -> 0.0); + tramp = Tramp.createDummy(); + shooter = Shooter.createDummy(); + indexer = Indexer.createDummy(); + aprilTagVision = AprilTagVision.createDummy(); + } } case SIM -> { // Sim robot, instantiate physics sim IO implementations diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/SwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/SwerveDriveCommand.java index 8a0117da..76038cac 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/SwerveDriveCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/SwerveDriveCommand.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.team1540.robot2024.Constants; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.util.math.JoystickUtils; @@ -23,9 +24,9 @@ public void initialize() { @Override public void execute() { - double xPercent = JoystickUtils.squaredSmartDeadzone(-controller.getLeftY(), deadzone); - double yPercent = JoystickUtils.squaredSmartDeadzone(-controller.getLeftX(), deadzone); - double rotPercent = JoystickUtils.squaredSmartDeadzone(-controller.getRightX(), deadzone); + double xPercent = JoystickUtils.squaredSmartDeadzone(-controller.getLeftY(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); + double yPercent = JoystickUtils.squaredSmartDeadzone(-controller.getLeftX(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); + double rotPercent = JoystickUtils.squaredSmartDeadzone(-controller.getRightX(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); drivetrain.drivePercent(xPercent, yPercent, rotPercent, true); } From c2ba924f02ddb571c2ef0ba3656b93032dcc97c0 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sun, 17 Mar 2024 12:04:21 -0700 Subject: [PATCH 33/75] feat: EVERYTHING IS A FALCON --- .../robot2024/subsystems/indexer/Indexer.java | 2 +- ...nFXSparkMax.java => IndexerIOTalonFX.java} | 53 +++++++++++-------- 2 files changed, 33 insertions(+), 22 deletions(-) rename src/main/java/org/team1540/robot2024/subsystems/indexer/{IndexerIOTalonFXSparkMax.java => IndexerIOTalonFX.java} (58%) diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index 59fc2579..08e1d431 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -33,7 +33,7 @@ public static Indexer createReal() { if (Constants.currentMode != Constants.Mode.REAL) { DriverStation.reportWarning("Using real indexer on simulated robot", false); } - return new Indexer(new IndexerIOTalonFXSparkMax()); + return new Indexer(new IndexerIOTalonFX()); } public static Indexer createSim() { diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFXSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java similarity index 58% rename from src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFXSparkMax.java rename to src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java index d7437bd2..de53e276 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFXSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java @@ -10,30 +10,34 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkLowLevel; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; import edu.wpi.first.wpilibj.DigitalInput; import static org.team1540.robot2024.Constants.Indexer.*; -public class IndexerIOTalonFXSparkMax implements IndexerIO { - private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless); +public class IndexerIOTalonFX implements IndexerIO { + private final TalonFX intakeMotor = new TalonFX(INTAKE_ID); private final TalonFX feederMotor = new TalonFX(FEEDER_ID); private final DigitalInput indexerBeamBreak = new DigitalInput(7); - private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder(); - - private final VoltageOut voltageCtrlReq = new VoltageOut(0).withEnableFOC(true); - private final VelocityVoltage velocityCtrlReq = new VelocityVoltage(0).withEnableFOC(true); + private final VoltageOut feederVoltageCtrlReq = new VoltageOut(0).withEnableFOC(true); + private final VelocityVoltage feederVelocityCtrlReq = new VelocityVoltage(0).withEnableFOC(true); private final StatusSignal feederVoltage = feederMotor.getMotorVoltage(); private final StatusSignal feederCurrent = feederMotor.getSupplyCurrent(); private final StatusSignal feederVelocity = feederMotor.getVelocity(); - public IndexerIOTalonFXSparkMax() { - intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - intakeMotor.enableVoltageCompensation(12.0); - intakeMotor.setSmartCurrentLimit(55); + private final VoltageOut intakeVoltageCtrlReq = new VoltageOut(0).withEnableFOC(true); + private final StatusSignal intakeVoltage = intakeMotor.getMotorVoltage(); + private final StatusSignal intakeCurrent = intakeMotor.getSupplyCurrent(); + private final StatusSignal intakeVelocity = intakeMotor.getVelocity(); + + public IndexerIOTalonFX() { + TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); + intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + intakeConfig.CurrentLimits.SupplyCurrentLimit = 55; + intakeConfig.CurrentLimits.SupplyCurrentThreshold = 80; + intakeConfig.CurrentLimits.SupplyTimeThreshold = 0.1; TalonFXConfiguration feederConfig = new TalonFXConfiguration(); feederConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; @@ -47,35 +51,42 @@ public IndexerIOTalonFXSparkMax() { feederConfig.Slot0.kD = FEEDER_KD; feederConfig.Slot0.kV = FEEDER_KV; + intakeMotor.getConfigurator().apply(intakeConfig); feederMotor.getConfigurator().apply(feederConfig); } @Override public void updateInputs(IndexerIOInputs inputs) { - BaseStatusSignal.refreshAll(feederVoltage, feederCurrent, feederVelocity); - inputs.intakeVoltage = intakeMotor.getAppliedOutput() * intakeMotor.getBusVoltage(); - inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent(); - inputs.intakeVelocityRPM = intakeEncoder.getVelocity(); + BaseStatusSignal.refreshAll( + intakeVoltage, + intakeCurrent, + intakeVelocity, + feederVoltage, + feederCurrent, + feederVelocity); + inputs.intakeVoltage = intakeVoltage.getValueAsDouble(); + inputs.intakeCurrentAmps = intakeCurrent.getValueAsDouble(); + inputs.intakeVelocityRPM = intakeVelocity.getValueAsDouble(); inputs.feederVoltage = feederVoltage.getValueAsDouble(); inputs.feederCurrentAmps = feederCurrent.getValueAsDouble(); inputs.feederVelocityRPM = feederVelocity.getValueAsDouble(); - inputs.feederVelocityError = inputs.feederVelocityRPM - velocityCtrlReq.Velocity; + inputs.feederVelocityError = inputs.feederVelocityRPM - feederVelocityCtrlReq.Velocity; inputs.noteInIntake = !indexerBeamBreak.get(); } @Override public void setIntakeVoltage(double volts) { - intakeMotor.setVoltage(volts); + intakeMotor.setControl(intakeVoltageCtrlReq.withOutput(volts)); } @Override public void setFeederVoltage(double voltage) { - feederMotor.setControl(voltageCtrlReq.withOutput(voltage)); + feederMotor.setControl(feederVoltageCtrlReq.withOutput(voltage)); } @Override public void setFeederVelocity(double velocity) { - feederMotor.setControl(velocityCtrlReq.withVelocity(velocity)); + feederMotor.setControl(feederVelocityCtrlReq.withVelocity(velocity)); } @Override From d9d64bdc765404012e1f5428770584448142673b Mon Sep 17 00:00:00 2001 From: Zach R Date: Sun, 17 Mar 2024 17:14:13 -0700 Subject: [PATCH 34/75] feat: add urcl --- .../java/org/team1540/robot2024/Robot.java | 3 + vendordeps/URCL.json | 65 +++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 vendordeps/URCL.json diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index c8b99f9c..6528acbc 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -1,5 +1,6 @@ package org.team1540.robot2024; +import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; @@ -10,6 +11,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import org.littletonrobotics.urcl.URCL; import org.team1540.robot2024.subsystems.led.Leds; import org.team1540.robot2024.subsystems.led.patterns.*; import org.team1540.robot2024.util.MechanismVisualiser; @@ -56,6 +58,7 @@ public void robotInit() { switch (Constants.currentMode) { case REAL: // Running on a real robot, log to a USB stick ("/U/logs") + Logger.registerURCL(URCL.startExternal()); Logger.addDataReceiver(new WPILOGWriter("/media/sda1")); Logger.addDataReceiver(new NT4Publisher()); break; diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json new file mode 100644 index 00000000..998c2616 --- /dev/null +++ b/vendordeps/URCL.json @@ -0,0 +1,65 @@ +{ + "fileName": "URCL.json", + "name": "URCL", + "version": "2024.1.0", + "frcYear": "2024", + "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", + "mavenUrls": [ + "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.1.0" + ], + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-cpp", + "version": "2024.1.0", + "libName": "URCL", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + }, + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "libName": "URCLDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 2aef3d5e753c83f12a80c3852ed3904445172071 Mon Sep 17 00:00:00 2001 From: Zach R Date: Sun, 17 Mar 2024 22:15:51 -0700 Subject: [PATCH 35/75] fix: make elevator pid actually pull to bottom --- src/main/java/org/team1540/robot2024/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index ea5c05ac..d318dc97 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -206,7 +206,7 @@ public static class Pivot { public static class Elevator { public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25); - public static final double MINIMUM_HEIGHT = Units.inchesToMeters(0.0); + public static final double MINIMUM_HEIGHT = Units.inchesToMeters(-0.03); public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0); public static final double MAX_HEIGHT = MINIMUM_HEIGHT + Units.inchesToMeters(21.0); //TODO: Fix these constants to be more accurate public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + MAX_HEIGHT - MINIMUM_HEIGHT; @@ -227,7 +227,7 @@ public static class Elevator { public static final double SPROCKET_RADIUS_M = Units.inchesToMeters(1.751/2); public static final double SPROCKET_CIRCUMFERENCE_M = 2 * SPROCKET_RADIUS_M * Math.PI; public static final double MOTOR_ROTS_PER_METER = GEAR_RATIO / SPROCKET_CIRCUMFERENCE_M; - public static final double POS_ERR_TOLERANCE_METERS = 0.03; + public static final double POS_ERR_TOLERANCE_METERS = 0.01; public static final double SIM_CARRIAGE_MASS_KG = 1.55; //TODO: check this number :) public enum ElevatorState { From 5a8fc05dc5a419a2eaeb72f197d7a0d82aaaeb41 Mon Sep 17 00:00:00 2001 From: Zach R Date: Sun, 17 Mar 2024 22:20:14 -0700 Subject: [PATCH 36/75] fix: make elevator work better; clean up constants todo --- .../org/team1540/robot2024/Constants.java | 24 +++++++------------ .../subsystems/indexer/IndexerIOTalonFX.java | 2 +- 2 files changed, 9 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index d318dc97..793ff8ab 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -18,7 +18,7 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final boolean IS_COMPETITION_ROBOT = true; // Objects.equals(RobotController.getComments(), "comp"); + public static final boolean IS_COMPETITION_ROBOT = true; // Whether to pull PID constants from SmartDashboard private static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY @@ -48,7 +48,7 @@ public enum Mode { public static final double LOOP_PERIOD_SECS = 0.02; public static class SwerveConfig { - public static final String CAN_BUS = IS_COMPETITION_ROBOT ? "swerve" : "swerve"; + public static final String CAN_BUS = "swerve"; public static final double CAN_UPDATE_FREQUENCY_HZ = 200.0; public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 1 : 9; @@ -92,7 +92,6 @@ public static class Indexer { public static final int INTAKE_ID = 13; public static final int FEEDER_ID = 15; - // TODO: fix these constants public static final double FEEDER_KP = 0.5; public static final double FEEDER_KI = 0.1; public static final double FEEDER_KD = 0.001; @@ -137,30 +136,26 @@ public static class Vision { public static class Shooter { public static class Flywheels { - // TODO: determine ids public static final int LEFT_ID = 12; public static final int RIGHT_ID = 11; public static final double GEAR_RATIO = 24.0 / 36.0; public static final double SIM_MOI = 4.08232288e-4; - // TODO: if it's tuned in simulation, it's tuned in real life public static final double KP = 0.3; public static final double KI = 0.2; public static final double KD = 0.0; public static final double KS = 0.26925; - public static final double KV = 0.07485; // TODO: this is what recalc says, may have to tune + public static final double KV = 0.07485; public static final double ERROR_TOLERANCE_RPM = 100; } public static class Pivot { - // TODO: determine ids public static final int MOTOR_ID = 9; public static final int CANCODER_ID = 10; - // TODO: figure this out - public static final double CANCODER_OFFSET_ROTS = -0.2502; + // TODO: determine ratios public static final double CANCODER_TO_PIVOT = 28.0 / 15.0; public static final double MOTOR_TO_CANCODER = 56.0; @@ -172,11 +167,9 @@ public static class Pivot { public static final Rotation2d MAX_ANGLE = Rotation2d.fromRotations(0.14); public static final Rotation2d MIN_ANGLE = Rotation2d.fromRotations(0.01); - public static final Rotation2d REAL_ZEROED_ANGLE = Rotation2d.fromDegrees(8.5); //TODO Need this number - + public static final Rotation2d REAL_ZEROED_ANGLE = Rotation2d.fromDegrees(8.5); public static final double PIVOT_HEIGHT = Units.inchesToMeters(10.5); - // TODO: tune pid public static final double KP = 400.0; public static final double KI = 0.0; public static final double KD = 20.0; @@ -206,12 +199,12 @@ public static class Pivot { public static class Elevator { public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25); - public static final double MINIMUM_HEIGHT = Units.inchesToMeters(-0.03); + public static final double MINIMUM_HEIGHT = Units.inchesToMeters(-0.01); //TODO: Does this make it angry? public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0); public static final double MAX_HEIGHT = MINIMUM_HEIGHT + Units.inchesToMeters(21.0); //TODO: Fix these constants to be more accurate public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + MAX_HEIGHT - MINIMUM_HEIGHT; - public static final double GEAR_RATIO = 11.571; //TODO: Get constants right sometime + public static final double GEAR_RATIO = 11.571; public static final int LEADER_ID = 7; public static final int FOLLOWER_ID = 8; public static final double KS = 0.03178; @@ -246,7 +239,7 @@ public enum ElevatorState { /** * At height for top of initial climb :D */ - AMP(0.3); //TODO: Find these values :D + AMP(0.3); public final double heightMeters; @@ -259,7 +252,6 @@ public enum ElevatorState { public static class Tramp { public static final int BEAM_BREAK_CHANNEL = 9; public static final double GEAR_RATIO = 9.0; - public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D public static final int MOTOR_ID = 17; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java index de53e276..ba7ea871 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java @@ -17,7 +17,7 @@ public class IndexerIOTalonFX implements IndexerIO { private final TalonFX intakeMotor = new TalonFX(INTAKE_ID); private final TalonFX feederMotor = new TalonFX(FEEDER_ID); - private final DigitalInput indexerBeamBreak = new DigitalInput(7); + private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID); private final VoltageOut feederVoltageCtrlReq = new VoltageOut(0).withEnableFOC(true); private final VelocityVoltage feederVelocityCtrlReq = new VelocityVoltage(0).withEnableFOC(true); From 1c7af0da9b9fe76f0792de9111143269126200d9 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Mon, 18 Mar 2024 12:05:18 -0700 Subject: [PATCH 37/75] feat: CTRE signal logging --- src/main/java/org/team1540/robot2024/Constants.java | 2 +- src/main/java/org/team1540/robot2024/Robot.java | 10 ++++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 793ff8ab..60a22752 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -21,7 +21,7 @@ public final class Constants { public static final boolean IS_COMPETITION_ROBOT = true; // Whether to pull PID constants from SmartDashboard private static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP - private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY + private static final Mode simMode = Mode.REPLAY; // Can also be Mode.REPLAY public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; public static final class Leds { diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 6528acbc..6152eb8b 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -57,10 +57,16 @@ public void robotInit() { // Set up data receivers & replay source switch (Constants.currentMode) { case REAL: - // Running on a real robot, log to a USB stick ("/U/logs") - Logger.registerURCL(URCL.startExternal()); + // Running on a real robot, log to a USB stick Logger.addDataReceiver(new WPILOGWriter("/media/sda1")); Logger.addDataReceiver(new NT4Publisher()); + + // Start REV signal logger + Logger.registerURCL(URCL.startExternal()); + + // Start CTRE signal logger + SignalLogger.setPath("media/sda1/ctre-logs"); + SignalLogger.start(); break; case SIM: From 4f5c7d0e3df4f8997b3fc2901495daba25be7dbd Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Mon, 18 Mar 2024 12:09:55 -0700 Subject: [PATCH 38/75] fix: simulation mode --- src/main/java/org/team1540/robot2024/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 60a22752..ac1424b8 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -21,7 +21,7 @@ public final class Constants { public static final boolean IS_COMPETITION_ROBOT = true; // Whether to pull PID constants from SmartDashboard private static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP - private static final Mode simMode = Mode.REPLAY; // Can also be Mode.REPLAY + private static final Mode simMode = Mode.REAL; // Can also be Mode.REPLAY public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; public static final class Leds { From 75fda991d657a39559b39a62695a92e8a697ccfd Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Mon, 18 Mar 2024 16:34:46 -0700 Subject: [PATCH 39/75] feat: temperature logging --- .../robot2024/subsystems/drive/ModuleIO.java | 2 ++ .../subsystems/drive/ModuleIOTalonFX.java | 14 +++++++++++-- .../subsystems/elevator/ElevatorIO.java | 1 + .../elevator/ElevatorIOTalonFX.java | 15 +++++++++---- .../subsystems/indexer/IndexerIO.java | 2 ++ .../subsystems/indexer/IndexerIOSparkMax.java | 2 ++ .../subsystems/indexer/IndexerIOTalonFX.java | 21 +++++++++++++++++-- .../subsystems/shooter/FlywheelsIO.java | 3 ++- .../shooter/FlywheelsIOTalonFX.java | 12 +++++++++-- .../robot2024/subsystems/shooter/Shooter.java | 1 - .../subsystems/shooter/ShooterPivotIO.java | 1 + .../shooter/ShooterPivotIOTalonFX.java | 6 ++++-- .../robot2024/subsystems/tramp/TrampIO.java | 1 + .../subsystems/tramp/TrampIOSparkMax.java | 2 +- .../subsystems/tramp/TrampIOTalonFX.java | 12 ++++------- 15 files changed, 72 insertions(+), 23 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java index e0ab6de8..fbc8fad1 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java @@ -10,12 +10,14 @@ class ModuleIOInputs { public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; public double driveCurrentAmps = 0.0; + public double driveTempCelsius = 0.0; public Rotation2d turnAbsolutePosition = new Rotation2d(); public Rotation2d turnPosition = new Rotation2d(); public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; + public double turnTempCelsius = 0.0; } /** diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java index b04e7e1a..9d055d52 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java @@ -37,12 +37,14 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; private final StatusSignal driveCurrent; + private final StatusSignal driveTempCelsius; private final StatusSignal turnAbsolutePosition; private final StatusSignal turnPosition; private final StatusSignal turnVelocity; private final StatusSignal turnAppliedVolts; private final StatusSignal turnCurrent; + private final StatusSignal turnTempCelsius; private final PhoenixTimeSyncSignalRefresher odometrySignalRefresher; @@ -58,12 +60,14 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw, PhoenixTimeSyncSignalRef driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getSupplyCurrent(); + driveTempCelsius = driveTalon.getDeviceTemp(); turnAbsolutePosition = cancoder.getAbsolutePosition(); turnPosition = turnTalon.getPosition(); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); turnCurrent = turnTalon.getSupplyCurrent(); + turnTempCelsius = turnTalon.getDeviceTemp(); BaseStatusSignal.setUpdateFrequencyForAll(CAN_UPDATE_FREQUENCY_HZ, drivePosition, turnPosition); // Required for odometry, use faster rate BaseStatusSignal.setUpdateFrequencyForAll( @@ -71,10 +75,12 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw, PhoenixTimeSyncSignalRef driveVelocity, driveAppliedVolts, driveCurrent, + driveTempCelsius, turnAbsolutePosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnCurrent, + turnTempCelsius); driveTalon.optimizeBusUtilization(); turnTalon.optimizeBusUtilization(); @@ -90,21 +96,25 @@ public void updateInputs(ModuleIOInputs inputs) { driveVelocity, driveAppliedVolts, driveCurrent, + driveTempCelsius, turnAbsolutePosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnCurrent, + turnTempCelsius); inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + inputs.driveTempCelsius = driveTempCelsius.getValueAsDouble(); inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + inputs.turnTempCelsius = turnTempCelsius.getValueAsDouble(); } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java index ddbcefd0..f772f972 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java @@ -9,6 +9,7 @@ class ElevatorIOInputs { public double velocityMPS = 0.0; public double voltage = 0.0; public double[] current = new double[]{}; + public double[] tempCelsius = new double[]{}; public boolean atUpperLimit = false; public boolean atLowerLimit = false; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java index d009ef1f..186ef54f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java @@ -22,6 +22,8 @@ public class ElevatorIOTalonFX implements ElevatorIO { private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); private final StatusSignal leaderCurrent = leader.getStatorCurrent(); private final StatusSignal followerCurrent = follower.getStatorCurrent(); + private final StatusSignal leaderTemp = leader.getDeviceTemp(); + private final StatusSignal followerTemp = follower.getDeviceTemp(); private final StatusSignal topLimitStatus = leader.getForwardLimit(); private final StatusSignal bottomLimitStatus = leader.getReverseLimit(); TalonFXConfiguration config; @@ -43,6 +45,8 @@ public ElevatorIOTalonFX() { leaderAppliedVolts, leaderCurrent, followerCurrent, + leaderTemp, + followerTemp, topLimitStatus, bottomLimitStatus); leader.optimizeBusUtilization(); @@ -83,13 +87,16 @@ public void updateInputs(ElevatorIOInputs inputs) { leaderAppliedVolts, leaderCurrent, followerCurrent, + leaderTemp, + followerTemp, topLimitStatus, bottomLimitStatus); - inputs.positionMeters = leaderPosition.getValue(); - inputs.velocityMPS = leaderVelocity.getValue(); - inputs.voltage = leaderAppliedVolts.getValue(); - inputs.current = new double[]{leaderCurrent.getValue(), followerCurrent.getValue()}; + inputs.positionMeters = leaderPosition.getValueAsDouble(); + inputs.velocityMPS = leaderVelocity.getValueAsDouble(); + inputs.voltage = leaderAppliedVolts.getValueAsDouble(); + inputs.current = new double[]{leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; + inputs.tempCelsius = new double[]{leaderTemp.getValueAsDouble(), followerCurrent.getValueAsDouble()}; inputs.atUpperLimit = topLimitStatus.getValue() == ForwardLimitValue.ClosedToGround; inputs.atLowerLimit = bottomLimitStatus.getValue() == ReverseLimitValue.ClosedToGround; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java index f08a1858..387f4d8e 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -9,9 +9,11 @@ class IndexerIOInputs { public double intakeVoltage; public double intakeCurrentAmps; public double intakeVelocityRPM; + public double intakeTempCelsius; public double feederVoltage; public double feederCurrentAmps; public double feederVelocityRPM; + public double feederTempCelsius; public boolean noteInIntake = false; public double setpointRPM; public double feederVelocityError; diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java index 4154c663..1bb7801f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -39,9 +39,11 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent(); inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); inputs.intakeVelocityRPM = intakeEncoder.getVelocity(); + inputs.intakeTempCelsius = intakeMotor.getMotorTemperature(); inputs.feederCurrentAmps = feederMotor.getOutputCurrent(); inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput(); inputs.feederVelocityRPM = feederEncoder.getVelocity(); + inputs.feederTempCelsius = feederMotor.getMotorTemperature(); inputs.noteInIntake = !indexerBeamBreak.get(); inputs.setpointRPM = setpointRPM; inputs.feederVelocityError = setpointRPM - feederEncoder.getVelocity(); diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java index ba7ea871..2c67d3fa 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java @@ -9,7 +9,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.CANSparkBase; import edu.wpi.first.wpilibj.DigitalInput; import static org.team1540.robot2024.Constants.Indexer.*; @@ -24,11 +23,13 @@ public class IndexerIOTalonFX implements IndexerIO { private final StatusSignal feederVoltage = feederMotor.getMotorVoltage(); private final StatusSignal feederCurrent = feederMotor.getSupplyCurrent(); private final StatusSignal feederVelocity = feederMotor.getVelocity(); + private final StatusSignal feederTemp = feederMotor.getDeviceTemp(); private final VoltageOut intakeVoltageCtrlReq = new VoltageOut(0).withEnableFOC(true); private final StatusSignal intakeVoltage = intakeMotor.getMotorVoltage(); private final StatusSignal intakeCurrent = intakeMotor.getSupplyCurrent(); private final StatusSignal intakeVelocity = intakeMotor.getVelocity(); + private final StatusSignal intakeTemp = intakeMotor.getDeviceTemp(); public IndexerIOTalonFX() { TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); @@ -53,6 +54,18 @@ public IndexerIOTalonFX() { intakeMotor.getConfigurator().apply(intakeConfig); feederMotor.getConfigurator().apply(feederConfig); + + BaseStatusSignal.setUpdateFrequencyForAll(50, + feederVoltage, + feederCurrent, + feederVelocity, + feederTemp, + intakeVoltage, + intakeCurrent, + intakeVelocity, + intakeTemp); + intakeMotor.optimizeBusUtilization(); + feederMotor.optimizeBusUtilization(); } @Override @@ -61,15 +74,19 @@ public void updateInputs(IndexerIOInputs inputs) { intakeVoltage, intakeCurrent, intakeVelocity, + intakeTemp, feederVoltage, feederCurrent, - feederVelocity); + feederVelocity, + intakeTemp); inputs.intakeVoltage = intakeVoltage.getValueAsDouble(); inputs.intakeCurrentAmps = intakeCurrent.getValueAsDouble(); inputs.intakeVelocityRPM = intakeVelocity.getValueAsDouble(); + inputs.intakeTempCelsius = intakeTemp.getValueAsDouble(); inputs.feederVoltage = feederVoltage.getValueAsDouble(); inputs.feederCurrentAmps = feederCurrent.getValueAsDouble(); inputs.feederVelocityRPM = feederVelocity.getValueAsDouble(); + inputs.feederTempCelsius = feederTemp.getValueAsDouble(); inputs.feederVelocityError = inputs.feederVelocityRPM - feederVelocityCtrlReq.Velocity; inputs.noteInIntake = !indexerBeamBreak.get(); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java index bfdd707f..97d14bc4 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java @@ -8,11 +8,12 @@ class FlywheelsIOInputs { public double leftAppliedVolts = 0.0; public double leftCurrentAmps = 0.0; public double leftVelocityRPM = 0.0; + public double leftTempCelsius = 0.0; public double rightAppliedVolts = 0.0; public double rightCurrentAmps = 0.0; public double rightVelocityRPM = 0.0; - + public double rightTempCelsius = 0.0; } /** diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java index 322f824f..70f82bfd 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java @@ -19,10 +19,12 @@ public class FlywheelsIOTalonFX implements FlywheelsIO { private final StatusSignal leftVelocity = leftMotor.getVelocity(); private final StatusSignal leftAppliedVolts = leftMotor.getMotorVoltage(); private final StatusSignal leftCurrent = leftMotor.getSupplyCurrent(); + private final StatusSignal leftTempCelsius = leftMotor.getDeviceTemp(); private final StatusSignal rightVelocity = rightMotor.getVelocity(); private final StatusSignal rightAppliedVolts = rightMotor.getMotorVoltage(); private final StatusSignal rightCurrent = rightMotor.getSupplyCurrent(); + private final StatusSignal rightTempCelsius = rightMotor.getDeviceTemp(); private final VelocityVoltage leftVelocityCtrlReq = new VelocityVoltage(0).withEnableFOC(true).withSlot(0); @@ -64,9 +66,11 @@ public FlywheelsIOTalonFX() { leftVelocity, leftAppliedVolts, leftCurrent, + leftTempCelsius, rightVelocity, rightAppliedVolts, - rightCurrent); + rightCurrent, + rightTempCelsius); leftMotor.optimizeBusUtilization(); rightMotor.optimizeBusUtilization(); @@ -78,17 +82,21 @@ public void updateInputs(FlywheelsIOInputs inputs) { leftVelocity, leftAppliedVolts, leftCurrent, + leftTempCelsius, rightVelocity, rightAppliedVolts, - rightCurrent); + rightCurrent, + rightTempCelsius); inputs.leftVelocityRPM = leftVelocity.getValueAsDouble() * 60; inputs.leftAppliedVolts = leftAppliedVolts.getValueAsDouble(); inputs.leftCurrentAmps = leftCurrent.getValueAsDouble(); + inputs.leftTempCelsius = leftTempCelsius.getValueAsDouble(); inputs.rightVelocityRPM = rightVelocity.getValueAsDouble() * 60; inputs.rightAppliedVolts = rightAppliedVolts.getValueAsDouble(); inputs.rightCurrentAmps = rightCurrent.getValueAsDouble(); + inputs.rightTempCelsius = rightTempCelsius.getValueAsDouble(); } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index c57ce266..58120eb7 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -33,7 +33,6 @@ public class Shooter extends SubsystemBase { private final AverageFilter rightSpeedFilter = new AverageFilter(20); // Units: RPM private final AverageFilter pivotPositionFilter = new AverageFilter(10); // Units: rotations - private double leftFlywheelSetpointRPM; private double rightFlywheelSetpointRPM; private Rotation2d pivotSetpoint = new Rotation2d(); diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java index 65c13394..124eee58 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java @@ -11,6 +11,7 @@ class ShooterPivotIOInputs { public double velocityRPS = 0.0; public double appliedVolts = 0.0; public double currentAmps = 0.0; + public double tempCelsius = 0.0; public boolean isAtForwardLimit = false; public boolean isAtReverseLimit = false; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java index 1cd8c33e..3679cba3 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -25,6 +24,7 @@ public class ShooterPivotIOTalonFX implements ShooterPivotIO { private final StatusSignal velocity = motor.getVelocity(); private final StatusSignal appliedVoltage = motor.getMotorVoltage(); private final StatusSignal current = motor.getSupplyCurrent(); + private final StatusSignal temp = motor.getDeviceTemp(); private final StatusSignal forwardLimit = motor.getForwardLimit(); private final StatusSignal reverseLimit = motor.getReverseLimit(); @@ -82,6 +82,7 @@ public ShooterPivotIOTalonFX() { velocity, appliedVoltage, current, + temp, forwardLimit, reverseLimit); @@ -92,7 +93,7 @@ public ShooterPivotIOTalonFX() { @Override public void updateInputs(ShooterPivotIOInputs inputs) { - BaseStatusSignal.refreshAll(position, absolutePosition, velocity, appliedVoltage, current, forwardLimit, reverseLimit); + BaseStatusSignal.refreshAll(position, absolutePosition, velocity, appliedVoltage, current, temp, forwardLimit, reverseLimit); inputs.isAtForwardLimit = forwardLimit.getValue() == ForwardLimitValue.ClosedToGround; inputs.isAtReverseLimit = reverseLimit.getValue() == ReverseLimitValue.ClosedToGround; inputs.position = Rotation2d.fromRotations(position.getValueAsDouble()); @@ -100,6 +101,7 @@ public void updateInputs(ShooterPivotIOInputs inputs) { inputs.velocityRPS = velocity.getValueAsDouble(); inputs.appliedVolts = appliedVoltage.getValueAsDouble(); inputs.currentAmps = current.getValueAsDouble(); + inputs.tempCelsius = temp.getValueAsDouble(); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIO.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIO.java index dfcaaddb..eac7b060 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIO.java @@ -11,6 +11,7 @@ class TrampIOInputs { public double appliedVolts = 0.0; public double currentAmps = 0.0; public double positionRots = 0.0; + public double tempCelsius = 0.0; } default void setVoltage(double volts) {} diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java index 60ec65ff..b2150d71 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java @@ -31,6 +31,6 @@ public void updateInputs(TrampIOInputs inputs) { inputs.positionRots = motorEncoder.getPosition(); inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); inputs.currentAmps = motor.getOutputCurrent(); - + inputs.tempCelsius = motor.getMotorTemperature(); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java index d126ac1d..bac93225 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java @@ -4,16 +4,10 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; import edu.wpi.first.wpilibj.DigitalInput; -import static org.team1540.robot2024.Constants.Shooter.Pivot.CANCODER_TO_PIVOT; -import static org.team1540.robot2024.Constants.Shooter.Pivot.MOTOR_TO_CANCODER; import static org.team1540.robot2024.Constants.Tramp.BEAM_BREAK_CHANNEL; import static org.team1540.robot2024.Constants.Tramp.MOTOR_ID; @@ -24,6 +18,7 @@ public class TrampIOTalonFX implements TrampIO { private final StatusSignal position = motor.getPosition(); private final StatusSignal appliedVoltage = motor.getMotorVoltage(); private final StatusSignal current = motor.getSupplyCurrent(); + private final StatusSignal temp = motor.getDeviceTemp(); public TrampIOTalonFX() { TalonFXConfiguration motorConfig = new TalonFXConfiguration(); @@ -38,7 +33,7 @@ public TrampIOTalonFX() { motor.getConfigurator().apply(motorConfig); - BaseStatusSignal.setUpdateFrequencyForAll(50, position, velocity, appliedVoltage, current); + BaseStatusSignal.setUpdateFrequencyForAll(50, position, velocity, appliedVoltage, current, temp); motor.optimizeBusUtilization(); } @@ -49,11 +44,12 @@ public void setVoltage(double volts) { @Override public void updateInputs(TrampIOInputs inputs) { - BaseStatusSignal.refreshAll(position, velocity, appliedVoltage, current); + BaseStatusSignal.refreshAll(position, velocity, appliedVoltage, current, temp); inputs.noteInTramp = !(beamBreak.get()); inputs.velocityRPM = velocity.getValueAsDouble(); inputs.positionRots = position.getValueAsDouble(); inputs.appliedVolts = appliedVoltage.getValueAsDouble(); inputs.currentAmps = current.getValueAsDouble(); + inputs.tempCelsius = temp.getValueAsDouble(); } } From b66023503c09e39d02d3048aac2bac6df382a5b7 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Mon, 18 Mar 2024 20:18:59 -0700 Subject: [PATCH 40/75] feat: updated shoot in auto logic --- .../drivetrain/DriveWithTargetingCommand.java | 3 +- .../subsystems/drive/Drivetrain.java | 11 +++++ .../robot2024/util/auto/AutoCommand.java | 43 +++++++++---------- 3 files changed, 33 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java index 87927900..b763ecd3 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java @@ -56,7 +56,8 @@ public void execute() { .minus(target.get()).getTranslation().getAngle() .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0)) .minus(Rotation2d.fromDegrees(3)); - Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); +// Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); + drivetrain.setTargetPose(new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); Logger.recordOutput("Targeting/rotErrorDegrees", Math.abs(targetRot.minus(drivetrain.getRotation()).getDegrees())); Logger.recordOutput("Targeting/target", target.get()); diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index cc02029d..2a580a2f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -57,6 +57,8 @@ public class Drivetrain extends SubsystemBase { private boolean isCharacterizingWheels = false; private double characterizationInput = 0.0; + private Pose2d targetPose = new Pose2d(); + private Drivetrain( GyroIO gyroIO, ModuleIO flModuleIO, @@ -427,4 +429,13 @@ public void stopCharacterization() { public double getSpeakerDistanceMeters(){ return getPose().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation()); } + + public void setTargetPose(Pose2d pose){ + this.targetPose = pose; + } + + @AutoLogOutput(key = "Targeting/TargetPose") + public Pose2d getTargetPose() { + return targetPose; + } } diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index aa6395e4..77ab5674 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -2,10 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.proto.Trajectory; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.*; import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; import org.team1540.robot2024.commands.indexer.IntakeCommand; @@ -88,39 +85,39 @@ public void setPathIndex(int index){ * @param pathIndex * @return the commands to run in the segment */ - protected Command createSegmentSequence(Drivetrain drivetrain, Indexer indexer, int pathIndex) { + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder) { return Commands.sequence( Commands.deadline( getPath(pathIndex).getCommand(drivetrain), new IntakeCommand(indexer, () -> false, 1) ), drivetrain.commandStop(), + Commands.sequence( + Commands.waitSeconds(0.25), + new InstantCommand(shooter::zeroPivotToCancoder) + ).onlyIf(()->shouldZeroCancoder), drivetrain.commandCopyVisionPose(), Commands.parallel( new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4), -// IntakeAndFeed.withDefaults(indexer).withTimeout(0.5), - IntakeAndFeed.withDefaults(indexer).until(()->!indexer.isNoteStaged()).andThen(Commands.waitSeconds(0.2)) + Commands.sequence( + Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10), + new ParallelDeadlineGroup( + Commands.sequence( + Commands.waitUntil(()->!indexer.isNoteStaged()), + Commands.waitSeconds(0.2) + ).withTimeout(1), + IntakeAndFeed.withDefaults(indexer) + ) + ) ) ); } + protected Command createSegmentSequence(Drivetrain drivetrain, Indexer indexer, int pathIndex){ + return createSegmentSequence(drivetrain, null, indexer, pathIndex, false); + } protected Command createCancoderSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex) { - return Commands.sequence( - Commands.deadline( - getPath(pathIndex).getCommand(drivetrain), - new IntakeCommand(indexer, () -> false, 1) - ), - drivetrain.commandStop(), - Commands.waitSeconds(0.25), - new InstantCommand(shooter::zeroPivotToCancoder), - drivetrain.commandCopyVisionPose(), - Commands.parallel( - new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4), - IntakeAndFeed.withDefaults(indexer).until(()->!indexer.isNoteStaged()).andThen(Commands.waitSeconds(0.1)) - ) - -// IntakeAndFeed.withDefaults(indexer).withTimeout(0.5) - ); + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, true); } public List toTrajectory() { From b368af10d43b91a2256d6722fbc0ca1a70b3fbe3 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Tue, 19 Mar 2024 10:24:23 -0700 Subject: [PATCH 41/75] fix: kills your null ptrs --- src/main/java/org/team1540/robot2024/Constants.java | 2 +- .../robot2024/commands/autos/AmpLanePABCSprint.java | 6 +++--- .../robot2024/commands/autos/AmpLanePADESprint.java | 8 +++----- .../team1540/robot2024/commands/autos/CenterLanePBDA.java | 6 +++--- .../team1540/robot2024/commands/autos/CenterLanePCBA.java | 6 +++--- .../robot2024/commands/autos/CenterLanePCBADSprint.java | 8 ++++---- .../robot2024/commands/autos/CenterLanePCBAFSprint.java | 6 +++--- .../robot2024/commands/autos/CenterLanePCBFSprint.java | 6 +++--- .../robot2024/commands/autos/SourceLanePGHSprint.java | 4 ++-- .../team1540/robot2024/commands/autos/SourceLanePHG.java | 4 ++-- .../org/team1540/robot2024/util/auto/AutoCommand.java | 5 ++--- 11 files changed, 29 insertions(+), 32 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index ac1424b8..793ff8ab 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -21,7 +21,7 @@ public final class Constants { public static final boolean IS_COMPETITION_ROBOT = true; // Whether to pull PID constants from SmartDashboard private static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP - private static final Mode simMode = Mode.REAL; // Can also be Mode.REPLAY + private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; public static final class Leds { diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java index 608ec383..ddcf9f85 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java @@ -25,10 +25,10 @@ public AmpLanePABCSprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, indexer, 0), + createSegmentSequence(drivetrain, shooter, indexer, 0), IntakeAndFeed.withDefaults(indexer).withTimeout(2), - createSegmentSequence(drivetrain, indexer, 1), - createSegmentSequence(drivetrain, indexer, 2) + createSegmentSequence(drivetrain, shooter, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 2) ) ), getPath(3).getCommand(drivetrain) diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java index 3d5770b3..5664c357 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java @@ -1,8 +1,6 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; import org.team1540.robot2024.commands.shooter.AutoShootPrepare; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -28,13 +26,13 @@ public AmpLanePADESprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer new AutoShootPrepare(drivetrain, shooter), Commands.sequence( createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), - createSegmentSequence(drivetrain, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 1), // new WaitCommand(5), // drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, indexer, 2), + createSegmentSequence(drivetrain, shooter, indexer, 2), // new WaitCommand(5), // drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, indexer, 3), + createSegmentSequence(drivetrain, shooter, indexer, 3), getPath(4).getCommand(drivetrain) // drivetrain.commandCopyVisionPose() ) diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBDA.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBDA.java index 4e9c2cbf..0d55ef1b 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBDA.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBDA.java @@ -24,9 +24,9 @@ public CenterLanePBDA(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, indexer, 0), - createSegmentSequence(drivetrain, indexer, 1), - createSegmentSequence(drivetrain, indexer, 2) + createSegmentSequence(drivetrain, shooter, indexer, 0), + createSegmentSequence(drivetrain, shooter, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 2) ) ) ); diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java index d40fe1d2..0f822eed 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java @@ -24,9 +24,9 @@ public CenterLanePCBA(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, indexer, 0), - createSegmentSequence(drivetrain, indexer, 1), - createSegmentSequence(drivetrain, indexer, 1) + createSegmentSequence(drivetrain, shooter, indexer, 0), + createSegmentSequence(drivetrain, shooter, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 1) ) ) ); diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBADSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBADSprint.java index 5657109f..81b5744e 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBADSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBADSprint.java @@ -26,10 +26,10 @@ public CenterLanePCBADSprint(Drivetrain drivetrain, Shooter shooter, Indexer ind Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, indexer, 0), - createSegmentSequence(drivetrain, indexer, 1), - createSegmentSequence(drivetrain, indexer, 2), - createSegmentSequence(drivetrain, indexer, 3) + createSegmentSequence(drivetrain, shooter, indexer, 0), + createSegmentSequence(drivetrain, shooter, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 2), + createSegmentSequence(drivetrain, shooter, indexer, 3) ) ), getPath(4).getCommand(drivetrain) diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java index d001e634..85773305 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java @@ -29,11 +29,11 @@ public CenterLanePCBAFSprint(Drivetrain drivetrain, Shooter shooter, Indexer ind Commands.sequence( createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 1), drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, indexer, 2), + createSegmentSequence(drivetrain, shooter, indexer, 2), drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, indexer, 3), + createSegmentSequence(drivetrain, shooter, indexer, 3), drivetrain.commandCopyVisionPose() ) ), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java index bb44450f..f71b6978 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java @@ -26,11 +26,11 @@ public CenterLanePCBFSprint(Drivetrain drivetrain, Shooter shooter, Indexer inde Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, indexer, 0), - createSegmentSequence(drivetrain, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 0), + createSegmentSequence(drivetrain, shooter, indexer, 1), getPath(2).getCommand(drivetrain), Commands.runOnce(drivetrain::copyVisionPose), - createSegmentSequence(drivetrain, indexer, 3) + createSegmentSequence(drivetrain, shooter, indexer, 3) ) ), getPath(4).getCommand(drivetrain) diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java index 0ae22a75..198ec0da 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java @@ -24,9 +24,9 @@ public SourceLanePGHSprint(Drivetrain drivetrain, Shooter shooter, Indexer index Commands.sequence( createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 1), drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, indexer, 2), + createSegmentSequence(drivetrain, shooter, indexer, 2), drivetrain.commandCopyVisionPose(), getPath(3).getCommand(drivetrain) ) diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java index f1bab0ae..ed971ee4 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java @@ -21,8 +21,8 @@ public SourceLanePHG(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, indexer, 0), - createSegmentSequence(drivetrain, indexer, 1) + createSegmentSequence(drivetrain, shooter, indexer, 0), + createSegmentSequence(drivetrain, shooter, indexer, 1) ) ) ); diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index 77ab5674..786d0422 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -1,7 +1,6 @@ package org.team1540.robot2024.util.auto; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.proto.Trajectory; import edu.wpi.first.wpilibj2.command.*; import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; @@ -112,8 +111,8 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, ) ); } - protected Command createSegmentSequence(Drivetrain drivetrain, Indexer indexer, int pathIndex){ - return createSegmentSequence(drivetrain, null, indexer, pathIndex, false); + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex){ + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, false); } protected Command createCancoderSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex) { From 9c1f219ffebc15a306c468d92741e3343235503d Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Tue, 19 Mar 2024 18:40:00 -0700 Subject: [PATCH 42/75] feat: new LL mount and happier elevator --- src/main/java/org/team1540/robot2024/Constants.java | 6 +++--- src/main/java/org/team1540/robot2024/RobotContainer.java | 2 +- .../commands/drivetrain/WheelRadiusCharacterization.java | 3 +-- .../team1540/robot2024/subsystems/elevator/Elevator.java | 2 +- .../robot2024/subsystems/elevator/ElevatorIOTalonFX.java | 2 +- .../subsystems/vision/AprilTagVisionIOLimelight.java | 5 ++--- 6 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 793ff8ab..37e9745c 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -109,8 +109,8 @@ public static class Vision { public static final String FRONT_CAMERA_NAME = "limelight-front"; public static final String REAR_CAMERA_NAME = "limelight-rear"; - public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.0975290, 0, 0.665479, new Rotation3d(0, Math.toRadians(-25), 0)); - public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.03639, 0, 0.715274, new Rotation3d(Math.PI, 0, Math.PI)); + public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.066606, 0, 0.626963, new Rotation3d(0, Math.toRadians(-16.393), 0)); + public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.005553, 0, 0.540533, new Rotation3d(0, Math.toRadians(10), Math.PI)); public static final boolean TAKE_SNAPSHOTS = false; public static final double SNAPSHOT_PERIOD_SECS = 10; @@ -199,7 +199,7 @@ public static class Pivot { public static class Elevator { public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25); - public static final double MINIMUM_HEIGHT = Units.inchesToMeters(-0.01); //TODO: Does this make it angry? + public static final double MINIMUM_HEIGHT = Units.inchesToMeters(-2); //TODO: Does this make it angry? public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0); public static final double MAX_HEIGHT = MINIMUM_HEIGHT + Units.inchesToMeters(21.0); //TODO: Fix these constants to be more accurate public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + MAX_HEIGHT - MINIMUM_HEIGHT; diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index bf52d318..3115e27d 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -267,7 +267,7 @@ private void configureAutoRoutines() { autos.add(new DriveSinglePath("SourceLaneSprint", drivetrain)); autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer)); autos.add(new SourceLanePHG(drivetrain, shooter, indexer)); - autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.CLOCKWISE))); + autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.COUNTER_CLOCKWISE))); } diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java index f7e939f9..64485ab6 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/WheelRadiusCharacterization.java @@ -13,7 +13,7 @@ public class WheelRadiusCharacterization extends Command { private static final LoggedTunableNumber characterizationSpeed = - new LoggedTunableNumber("WheelRadiusCharacterization/SpeedRadsPerSec", 0.1); + new LoggedTunableNumber("WheelRadiusCharacterization/SpeedRadsPerSec", 1); private static final double driveRadius = Constants.Drivetrain.DRIVE_BASE_RADIUS; public enum Direction { @@ -63,7 +63,6 @@ public void execute() { // Run drive at velocity drive.runVelocity(new ChassisSpeeds(0, 0, omegaLimiter.calculate(omegaDirection.value * characterizationSpeed.get()))); - System.out.println("running characterization"); // Get yaw and wheel positions accumGyroYawRads += MathUtil.angleModulus(gyroYawRadsSupplier.getAsDouble() - lastGyroYawRads); diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java index 6871b0ec..9b76cc03 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java @@ -77,7 +77,7 @@ public void setElevatorPosition(double positionMeters) { } public boolean isAtSetpoint() { - return MathUtil.isNear(setpointMeters, positionFilter.getAverage(), POS_ERR_TOLERANCE_METERS); + return MathUtil.isNear(setpointMeters, positionFilter.getAverage(), POS_ERR_TOLERANCE_METERS) || (inputs.atLowerLimit && setpointMeters <= 0); } public void setVoltage(double voltage) { diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java index 186ef54f..6a3740dc 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java @@ -72,7 +72,7 @@ public ElevatorIOTalonFX() { config.HardwareLimitSwitch.ForwardLimitEnable = true; config.HardwareLimitSwitch.ReverseLimitEnable = true; config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = true; - config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = Constants.Elevator.MINIMUM_HEIGHT; + config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = 0; leader.getConfigurator().apply(config); config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; follower.getConfigurator().apply(config); diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java index d01109b3..97fcf898 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -22,11 +22,10 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { LimelightHelpers.PoseEstimate measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); if (measurement.tagCount > 1) { inputs.estimatedPoseMeters = new Pose3d(measurement.pose); - inputs.numTagsSeen = measurement.tagCount; - inputs.avgTagDistance = measurement.avgTagDist; - inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds; } + inputs.numTagsSeen = measurement.tagCount; + inputs.avgTagDistance = measurement.avgTagDist; } @Override From daf1b4e87ba62ec4d77f891210f85c98d1d5dc8f Mon Sep 17 00:00:00 2001 From: Alvin Zhang <114535782+mimizh2418@users.noreply.github.com> Date: Tue, 19 Mar 2024 22:05:38 -0700 Subject: [PATCH 43/75] Cleanup, part 3 (#42) * chore: IO and subsystem cleanup - correct units in variable names - logging outputs to better-defined locations - log setpoints and errors in outputs, not inputs - other stuff * refactor: remove annoying warning (idk what this actually does but intellij is happy now) * refactor: kill fake subsystems * refactor: units in RPM --- .../team1540/robot2024/RobotContainer.java | 5 +- .../commands/climb/ClimbAlignment.java | 3 +- .../commands/climb/ClimbSequence.java | 7 ++- .../commands/climb/DeclimbSequence.java | 17 ------- .../commands/climb/TrapAndClimbSequence.java | 5 +- .../subsystems/elevator/Elevator.java | 2 +- .../subsystems/elevator/ElevatorIO.java | 2 +- .../subsystems/elevator/ElevatorIOSim.java | 2 +- .../elevator/ElevatorIOTalonFX.java | 2 +- .../subsystems/fakesubsystems/Hooks.java | 32 ------------- .../robot2024/subsystems/indexer/Indexer.java | 48 ++++++++++++------- .../subsystems/indexer/IndexerIO.java | 20 ++++---- .../subsystems/indexer/IndexerIOSim.java | 7 +-- .../subsystems/indexer/IndexerIOSparkMax.java | 9 +--- .../subsystems/indexer/IndexerIOTalonFX.java | 9 ++-- .../robot2024/subsystems/shooter/Shooter.java | 5 +- .../robot2024/subsystems/tramp/Tramp.java | 4 +- .../subsystems/tramp/TrampIOTalonFX.java | 2 +- .../subsystems/vision/AprilTagVision.java | 19 ++++---- .../vision/AprilTagVisionIOPhoton.java | 7 +-- .../robot2024/util/shooter/ShooterLerp.java | 7 +-- 21 files changed, 76 insertions(+), 138 deletions(-) delete mode 100644 src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java delete mode 100644 src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 3115e27d..b774ab0b 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -67,8 +67,7 @@ public RobotContainer() { indexer = Indexer.createReal(); aprilTagVision = AprilTagVision.createReal( drivetrain::addVisionMeasurement, - elevator::getPosition, - drivetrain::getVisionPose); + elevator::getPosition); } else { elevator = Elevator.createDummy(); drivetrain = Drivetrain.createReal(odometrySignalRefresher, () -> 0.0); @@ -192,7 +191,7 @@ private void configureButtonBindings() { copilot.rightTrigger(0.95).whileTrue(tramp.commandRun(1)); - copilot.leftTrigger(0.95).whileTrue(new ClimbAlignment(drivetrain, elevator, null, tramp, indexer, shooter)); + copilot.leftTrigger(0.95).whileTrue(new ClimbAlignment(drivetrain, elevator, tramp, indexer, shooter)); copilot.x().whileTrue(new ShootSequence(shooter, indexer)); diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java index f56f5937..ba5e67c1 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java @@ -9,7 +9,6 @@ import org.team1540.robot2024.commands.shooter.PrepareShooterCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.elevator.Elevator; -import org.team1540.robot2024.subsystems.fakesubsystems.Hooks; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.subsystems.tramp.Tramp; @@ -25,7 +24,7 @@ public class ClimbAlignment extends ParallelRaceGroup { private final Drivetrain drivetrain; - public ClimbAlignment(Drivetrain drivetrain, Elevator elevator, Hooks hooks, Tramp tramp, Indexer indexer, Shooter shooter){ + public ClimbAlignment(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Indexer indexer, Shooter shooter){ this.drivetrain = drivetrain; addCommands( new SequentialCommandGroup( diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java index f22b6acc..bdfcfe19 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java @@ -7,7 +7,6 @@ import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.elevator.Elevator; -import org.team1540.robot2024.subsystems.fakesubsystems.Hooks; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.subsystems.tramp.Tramp; @@ -15,17 +14,17 @@ public class ClimbSequence extends ParallelRaceGroup { - public ClimbSequence(Drivetrain drivetrain, Elevator elevator, Hooks hooks, Tramp tramp, Indexer indexer, Shooter shooter, CommandXboxController controller) { + public ClimbSequence(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Indexer indexer, Shooter shooter, CommandXboxController controller) { addCommands( Commands.startEnd(() -> { shooter.stopPivot(); shooter.setPivotBrakeMode(false); }, () -> shooter.setPivotBrakeMode(true), shooter), Commands.sequence( - new ClimbAlignment(drivetrain, elevator, hooks, tramp, indexer, shooter), + new ClimbAlignment(drivetrain, elevator, tramp, indexer, shooter), Commands.waitUntil(controller.a()), new ElevatorSetpointCommand(elevator, ElevatorState.TOP), - Commands.waitSeconds(5), //Confirm that nothing will break. Also might need to be tuned if chain does weird things + Commands.waitSeconds(5), // Confirm that nothing will break. Also might need to be tuned if chain does weird things Commands.startEnd(()->elevator.setVoltage(-10), elevator::holdPosition, elevator).until(elevator::getLowerLimit) ) ); diff --git a/src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java deleted file mode 100644 index f1a1d33b..00000000 --- a/src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java +++ /dev/null @@ -1,17 +0,0 @@ -package org.team1540.robot2024.commands.climb; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import org.team1540.robot2024.Constants.Elevator.ElevatorState; -import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; -import org.team1540.robot2024.subsystems.elevator.Elevator; -import org.team1540.robot2024.subsystems.fakesubsystems.Hooks; - -public class DeclimbSequence extends SequentialCommandGroup { - public DeclimbSequence(Elevator elevator, Hooks hooks) { - addCommands( - new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM), - hooks.undeployHooksCommand(), //Release hooks - new ElevatorSetpointCommand(elevator, ElevatorState.TOP) - ); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java index dd949605..c7dd478a 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java @@ -6,7 +6,6 @@ import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.elevator.Elevator; -import org.team1540.robot2024.subsystems.fakesubsystems.Hooks; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.subsystems.tramp.Tramp; @@ -14,9 +13,9 @@ public class TrapAndClimbSequence extends SequentialCommandGroup { - public TrapAndClimbSequence(Drivetrain drivetrain, Elevator elevator, Hooks hooks, Tramp tramp, Indexer indexer, Shooter shooter, CommandXboxController controller) { + public TrapAndClimbSequence(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Indexer indexer, Shooter shooter, CommandXboxController controller) { addCommands( - new ClimbSequence(drivetrain, elevator, hooks, tramp, indexer, shooter, controller),//Confirm that nothing will break + new ClimbSequence(drivetrain, elevator, tramp, indexer, shooter, controller),//Confirm that nothing will break Commands.waitUntil(controller.a()), new ElevatorSetpointCommand(elevator, ElevatorState.TOP), Commands.runOnce(()->tramp.setDistanceToGo(3)) diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java index 9b76cc03..cde6eb84 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java @@ -88,7 +88,7 @@ public void stop() { io.setVoltage(0.0); } - @AutoLogOutput + @AutoLogOutput(key = "Elevator/setpoint") public double getSetpoint() { return setpointMeters; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java index f772f972..76a9bf7f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java @@ -8,7 +8,7 @@ class ElevatorIOInputs { public double positionMeters = 0.0; public double velocityMPS = 0.0; public double voltage = 0.0; - public double[] current = new double[]{}; + public double[] currentAmps = new double[]{}; public double[] tempCelsius = new double[]{}; public boolean atUpperLimit = false; public boolean atLowerLimit = false; diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java index 92cbd062..93733e69 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java @@ -46,7 +46,7 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.positionMeters = elevatorSim.getPositionMeters(); inputs.velocityMPS = elevatorSim.getVelocityMetersPerSecond(); inputs.voltage = elevatorAppliedVolts; - inputs.current = new double[]{elevatorSim.getCurrentDrawAmps()}; + inputs.currentAmps = new double[]{elevatorSim.getCurrentDrawAmps()}; inputs.atUpperLimit = elevatorSim.hasHitUpperLimit(); inputs.atLowerLimit = elevatorSim.hasHitLowerLimit(); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java index 6a3740dc..53bcec6f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java @@ -95,7 +95,7 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.positionMeters = leaderPosition.getValueAsDouble(); inputs.velocityMPS = leaderVelocity.getValueAsDouble(); inputs.voltage = leaderAppliedVolts.getValueAsDouble(); - inputs.current = new double[]{leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; + inputs.currentAmps = new double[]{leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; inputs.tempCelsius = new double[]{leaderTemp.getValueAsDouble(), followerCurrent.getValueAsDouble()}; inputs.atUpperLimit = topLimitStatus.getValue() == ForwardLimitValue.ClosedToGround; inputs.atLowerLimit = bottomLimitStatus.getValue() == ReverseLimitValue.ClosedToGround; diff --git a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java deleted file mode 100644 index b9853233..00000000 --- a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.team1540.robot2024.subsystems.fakesubsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -//TODO: Get rid of this class and make an actual one its just a temporary thingie for David :D -public class Hooks extends SubsystemBase { - - //TODO: Whoever does this it should retract or deploy the hooks depending on if they are already deployed or retracted - public void deployHooks() { - - } - - public void undeployHooks() { - - } - - /** - * Factory method for deploying hooks - */ - public Command deployHooksCommand() { - return new InstantCommand(this::deployHooks, this); - } - - /** - * Factory method for undeploying hooks - */ - public Command undeployHooksCommand() { - return new InstantCommand(this::undeployHooks, this); - } -} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index 08e1d431..66a00f20 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.team1540.robot2024.Constants; import org.team1540.robot2024.util.LoggedTunableNumber; @@ -21,6 +22,8 @@ public class Indexer extends SubsystemBase { private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI); private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD); + private double feederSetpointRPM = 0.0; + private static boolean hasInstance = false; private Indexer(IndexerIO io) { @@ -69,6 +72,7 @@ public boolean isNoteStaged() { } public void setFeederVelocity(double setpointRPM) { + feederSetpointRPM = setpointRPM; io.setFeederVelocity(setpointRPM); } @@ -76,6 +80,33 @@ public void setFeederPercent(double percent) { io.setFeederVoltage(12.0 * percent); } + public boolean isFeederAtSetpoint() { + return Math.abs(getFeederVelocityError()) < VELOCITY_ERR_TOLERANCE_RPM; + } + + public void stopFeeder() { + io.setFeederVoltage(0); + } + + public void stopIntake() { + io.setIntakeVoltage(0); + } + + public void stopAll() { + io.setIntakeVoltage(0); + io.setFeederVoltage(0); + } + + @AutoLogOutput(key = "Intake/Feeder/setpointRPM") + public double getFeederVelocitySetpoint() { + return feederSetpointRPM; + } + + @AutoLogOutput(key = "Intake/Feeder/velocityErrorRPM") + public double getFeederVelocityError() { + return inputs.feederVelocityRPM - getFeederVelocitySetpoint(); + } + public Command feedToAmp() { return Commands.runOnce(() -> io.setFeederVelocity(-600), this); } @@ -93,23 +124,6 @@ public Command moveNoteOut() { this ); } - public boolean isFeederAtSetpoint() { - return Math.abs(inputs.feederVelocityError) < VELOCITY_ERR_TOLERANCE_RPM; -// return MathUtil.isNear(inputs.setpointRPM, inputs.feederVelocityRPM, VELOCITY_ERR_TOLERANCE_RPM); - } - - public void stopFeeder() { - io.setFeederVoltage(0); - } - - public void stopIntake() { - io.setIntakeVoltage(0); - } - - public void stopAll() { - io.setIntakeVoltage(0); - io.setFeederVoltage(0); - } public Command commandRunIntake(double percent) { return Commands.startEnd( diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java index 387f4d8e..c7fcdcaf 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -6,17 +6,15 @@ public interface IndexerIO { @AutoLog class IndexerIOInputs { - public double intakeVoltage; - public double intakeCurrentAmps; - public double intakeVelocityRPM; - public double intakeTempCelsius; - public double feederVoltage; - public double feederCurrentAmps; - public double feederVelocityRPM; - public double feederTempCelsius; + public double intakeVoltage = 0.0; + public double intakeCurrentAmps = 0.01; + public double intakeVelocityRPM = 0.0; + public double intakeTempCelsius = 0.0; + public double feederVoltage = 0.0; + public double feederCurrentAmps = 0.0; + public double feederVelocityRPM = 0.0; + public double feederTempCelsius = 0.0; public boolean noteInIntake = false; - public double setpointRPM; - public double feederVelocityError; } /** @@ -28,7 +26,7 @@ default void setIntakeVoltage(double volts) {} default void setFeederVoltage(double volts) {} - default void setFeederVelocity(double velocity) {} + default void setFeederVelocity(double velocityRPM) {} default void configureFeederPID(double p, double i, double d) {} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java index 98fb3d9a..730a41cf 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java @@ -37,9 +37,6 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps(); inputs.feederVoltage = feederVoltage; inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); -// inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get(); - inputs.setpointRPM = feederSimPID.getSetpoint() * 60; - inputs.feederVelocityError = feederSimPID.getPositionError(); } @Override @@ -59,10 +56,10 @@ public void setFeederVoltage(double volts) { } @Override - public void setFeederVelocity(double velocity) { + public void setFeederVelocity(double velocityRPM) { isClosedLoop = true; feederSimPID.reset(); - feederSetpointRPS = velocity / 60; + feederSetpointRPS = velocityRPM / 60; } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java index 1bb7801f..d69271ab 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -15,7 +15,6 @@ public class IndexerIOSparkMax implements IndexerIO { private final RelativeEncoder feederEncoder = feederMotor.getEncoder(); private final SparkPIDController feederPID = feederMotor.getPIDController(); - private double setpointRPM; public IndexerIOSparkMax() { @@ -45,9 +44,6 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.feederVelocityRPM = feederEncoder.getVelocity(); inputs.feederTempCelsius = feederMotor.getMotorTemperature(); inputs.noteInIntake = !indexerBeamBreak.get(); - inputs.setpointRPM = setpointRPM; - inputs.feederVelocityError = setpointRPM - feederEncoder.getVelocity(); - } @Override @@ -61,10 +57,9 @@ public void setFeederVoltage(double volts) { } @Override - public void setFeederVelocity(double velocity) { - setpointRPM = velocity; + public void setFeederVelocity(double velocityRPM) { feederPID.setReference( - velocity * FEEDER_GEAR_RATIO, + velocityRPM * FEEDER_GEAR_RATIO, CANSparkBase.ControlType.kVelocity, 0, FEEDER_KS, diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java index 2c67d3fa..f2668afb 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java @@ -81,13 +81,12 @@ public void updateInputs(IndexerIOInputs inputs) { intakeTemp); inputs.intakeVoltage = intakeVoltage.getValueAsDouble(); inputs.intakeCurrentAmps = intakeCurrent.getValueAsDouble(); - inputs.intakeVelocityRPM = intakeVelocity.getValueAsDouble(); + inputs.intakeVelocityRPM = intakeVelocity.getValueAsDouble() * 60; inputs.intakeTempCelsius = intakeTemp.getValueAsDouble(); inputs.feederVoltage = feederVoltage.getValueAsDouble(); inputs.feederCurrentAmps = feederCurrent.getValueAsDouble(); - inputs.feederVelocityRPM = feederVelocity.getValueAsDouble(); + inputs.feederVelocityRPM = feederVelocity.getValueAsDouble() * 60; inputs.feederTempCelsius = feederTemp.getValueAsDouble(); - inputs.feederVelocityError = inputs.feederVelocityRPM - feederVelocityCtrlReq.Velocity; inputs.noteInIntake = !indexerBeamBreak.get(); } @@ -102,8 +101,8 @@ public void setFeederVoltage(double voltage) { } @Override - public void setFeederVelocity(double velocity) { - feederMotor.setControl(feederVelocityCtrlReq.withVelocity(velocity)); + public void setFeederVelocity(double velocityRPM) { + feederMotor.setControl(feederVelocityCtrlReq.withVelocity(velocityRPM / 60)); } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index 58120eb7..e895d5af 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -77,7 +77,6 @@ public static Shooter createReal() { DriverStation.reportWarning("Using real shooter on simulated robot", false); } return new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX()); -// return new Shooter(new ShooterPivotIO() {}, new FlywheelsIOTalonFX()); } public static Shooter createSim() { @@ -261,12 +260,12 @@ public Command spinUpAndSetPivotPosition(Supplier leftSetpoint, Supplier ); } - @AutoLogOutput + @AutoLogOutput(key = "Shooter/Flywheels/leftSetpointRPM") public double getLeftFlywheelSetpointRPM() { return leftFlywheelSetpointRPM; } - @AutoLogOutput + @AutoLogOutput(key = "Shooter/Flywheels/rightSetpointRPM") public double getRightFlywheelSetpointRPM() { return rightFlywheelSetpointRPM; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java index 5286e905..7ceb06e9 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; import org.littletonrobotics.junction.Logger; import org.team1540.robot2024.Constants; @@ -47,9 +46,10 @@ public void setPercent(double percentage) { isClosedLoop = false; io.setVoltage(12.0 * percentage); } + public void setDistanceToGo(double distanceRots) { isClosedLoop = true; - positionalPID.setSetpoint(distanceRots*Constants.Tramp.GEAR_RATIO+inputs.positionRots); + positionalPID.setSetpoint(distanceRots * Constants.Tramp.GEAR_RATIO + inputs.positionRots); } public boolean isNoteStaged() { diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java index bac93225..ffbf5e32 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java @@ -46,7 +46,7 @@ public void setVoltage(double volts) { public void updateInputs(TrampIOInputs inputs) { BaseStatusSignal.refreshAll(position, velocity, appliedVoltage, current, temp); inputs.noteInTramp = !(beamBreak.get()); - inputs.velocityRPM = velocity.getValueAsDouble(); + inputs.velocityRPM = velocity.getValueAsDouble() * 60; inputs.positionRots = position.getValueAsDouble(); inputs.appliedVolts = appliedVoltage.getValueAsDouble(); inputs.currentAmps = current.getValueAsDouble(); diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java index 1f76e4d9..8fc308c4 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java @@ -45,13 +45,12 @@ private AprilTagVision( } public static AprilTagVision createReal(Consumer visionPoseConsumer, - Supplier elevatorHeightSupplierMeters, - Supplier drivetrainPoseSupplier) { + Supplier elevatorHeightSupplierMeters) { if (Constants.currentMode != Constants.Mode.REAL) { DriverStation.reportWarning("Using real vision on simulated robot", false); } return new AprilTagVision( - new AprilTagVisionIOPhoton(FRONT_CAMERA_NAME, FRONT_CAMERA_POSE, drivetrainPoseSupplier), + new AprilTagVisionIOPhoton(FRONT_CAMERA_NAME, FRONT_CAMERA_POSE), new AprilTagVisionIOLimelight(REAR_CAMERA_NAME, REAR_CAMERA_POSE), visionPoseConsumer, elevatorHeightSupplierMeters); @@ -108,14 +107,12 @@ public void periodic() { if (TAKE_SNAPSHOTS && DriverStation.isFMSAttached() && RobotState.isEnabled()) { Logger.runEveryN((int) (SNAPSHOT_PERIOD_SECS / Constants.LOOP_PERIOD_SECS), - () -> { - takeSnapshot( - String.format("%s_%s%d_%d", - DriverStation.getEventName(), - DriverStation.getMatchType().toString(), - DriverStation.getMatchNumber(), - (int) Timer.getFPGATimestamp())); - }); + () -> takeSnapshot( + String.format("%s_%s%d_%d", + DriverStation.getEventName(), + DriverStation.getMatchType().toString(), + DriverStation.getMatchNumber(), + (int) Timer.getFPGATimestamp()))); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java index 0a82f912..eb7b3a4a 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java @@ -1,6 +1,5 @@ package org.team1540.robot2024.subsystems.vision; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import org.photonvision.EstimatedRobotPose; @@ -13,17 +12,15 @@ import java.util.List; import java.util.Optional; -import java.util.function.Supplier; public class AprilTagVisionIOPhoton implements AprilTagVisionIO { private final PhotonCamera camera; private final PhotonPoseEstimator photonEstimator; - private final Supplier drivetrainPoseSupplier; private Transform3d cameraTransform; private Pose3d lastEstimatedPose; - public AprilTagVisionIOPhoton(String name, Pose3d cameraOffsetMeters, Supplier drivetrainPoseSupplier) { + public AprilTagVisionIOPhoton(String name, Pose3d cameraOffsetMeters) { camera = new PhotonCamera(name); cameraTransform = new Transform3d(cameraOffsetMeters.getTranslation(), cameraOffsetMeters.getRotation()); photonEstimator = new PhotonPoseEstimator( @@ -33,12 +30,10 @@ public AprilTagVisionIOPhoton(String name, Pose3d cameraOffsetMeters, Supplier

targets = latestResult.getTargets(); Optional estimatedPose = photonEstimator.update(latestResult); diff --git a/src/main/java/org/team1540/robot2024/util/shooter/ShooterLerp.java b/src/main/java/org/team1540/robot2024/util/shooter/ShooterLerp.java index c7dcaae2..ce64c5ab 100644 --- a/src/main/java/org/team1540/robot2024/util/shooter/ShooterLerp.java +++ b/src/main/java/org/team1540/robot2024/util/shooter/ShooterLerp.java @@ -1,12 +1,8 @@ package org.team1540.robot2024.util.shooter; -import edu.wpi.first.math.InterpolatingMatrixTreeMap; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.math.interpolation.InterpolatingTreeMap; -import edu.wpi.first.math.interpolation.Interpolator; -import edu.wpi.first.math.interpolation.InverseInterpolator; public class ShooterLerp { InterpolatingDoubleTreeMap angle = new InterpolatingDoubleTreeMap(); @@ -19,7 +15,8 @@ public void put(Double key, ShooterSetpoint value){ right.put(key, value.rightSetpoint); } - public ShooterLerp put(Pair... dataPoints){ + @SafeVarargs + public final ShooterLerp put(Pair... dataPoints){ for (Pair point : dataPoints) { put(point.getFirst(), point.getSecond()); } From a662d255a88344ab23d417b697aa5938a95034a1 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Tue, 19 Mar 2024 22:25:56 -0700 Subject: [PATCH 44/75] chore: bump dependencies --- build.gradle | 2 +- vendordeps/AdvantageKit.json | 10 +++++----- vendordeps/PathplannerLib.json | 6 +++--- vendordeps/REVLib-2024.json | 10 +++++----- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/build.gradle b/build.gradle index 5623c813..3fddc54d 100755 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" id "com.peterabeles.gversion" version "1.10" } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 2cce1d24..b35a53ac 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.1.1", + "version": "3.2.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.1.1" + "version": "3.2.0" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.1.1" + "version": "3.2.0" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.1.1" + "version": "3.2.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.1.1", + "version": "3.2.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 3ec4c12f..a0197060 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.6", + "version": "2024.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.6" + "version": "2024.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.6", + "version": "2024.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib-2024.json b/vendordeps/REVLib-2024.json index 6c04fab6..28ef9253 100644 --- a/vendordeps/REVLib-2024.json +++ b/vendordeps/REVLib-2024.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.3", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.3" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 182a936ddf00d232277c61b197b6327ab9ef4949 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Tue, 19 Mar 2024 22:36:34 -0700 Subject: [PATCH 45/75] fix: warm up path following and pathfinding before auto to prevent delays - https://pathplanner.dev/pplib-follow-a-single-path.html#java-warmup - https://pathplanner.dev/pplib-pathfinding.html#java-warmup --- src/main/java/org/team1540/robot2024/Robot.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 6152eb8b..2568db08 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -1,6 +1,8 @@ package org.team1540.robot2024; import com.ctre.phoenix6.SignalLogger; +import com.pathplanner.lib.commands.FollowPathCommand; +import com.pathplanner.lib.commands.PathfindingCommand; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; @@ -96,8 +98,13 @@ public void robotInit() { robotContainer = new RobotContainer(); robotContainer.shooter.setPivotBrakeMode(false); - AprilTagsCrescendo.getInstance().getTag(1); robotContainer.drivetrain.setBrakeMode(false); + + // Pathplanner warmup (helps prevents delays at the start of auto) + FollowPathCommand.warmupCommand().schedule(); + PathfindingCommand.warmupCommand().schedule(); + + AprilTagsCrescendo.getInstance().getTag(1); } /** From 872c45fb3b9aa7ca28815231796d8f0059609458 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Wed, 20 Mar 2024 19:12:22 -0700 Subject: [PATCH 46/75] feat: good cancoder for shooting and equation --- .../org/team1540/robot2024/Constants.java | 10 +++--- .../drivetrain/DriveWithTargetingCommand.java | 2 +- .../commands/shooter/AutoShootPrepare.java | 13 +++++++- .../robot2024/subsystems/shooter/Shooter.java | 33 ++++++++++++------- .../shooter/ShooterPivotIOTalonFX.java | 4 +-- .../subsystems/vision/AprilTagVision.java | 2 +- .../robot2024/util/auto/AutoCommand.java | 10 +++--- 7 files changed, 48 insertions(+), 26 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 37e9745c..2e6724d6 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -112,8 +112,8 @@ public static class Vision { public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.066606, 0, 0.626963, new Rotation3d(0, Math.toRadians(-16.393), 0)); public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.005553, 0, 0.540533, new Rotation3d(0, Math.toRadians(10), Math.PI)); - public static final boolean TAKE_SNAPSHOTS = false; - public static final double SNAPSHOT_PERIOD_SECS = 10; + public static final boolean TAKE_SNAPSHOTS = true; + public static final double SNAPSHOT_PERIOD_SECS = 1; public static final double XY_STD_DEV_COEFF = 0.1; public static final double ROT_STD_DEV_COEFF = 0.5; @@ -159,13 +159,15 @@ public static class Pivot { // TODO: determine ratios public static final double CANCODER_TO_PIVOT = 28.0 / 15.0; public static final double MOTOR_TO_CANCODER = 56.0; + + public static final double CHAIN_FACTOR = 1.04; public static final double TOTAL_GEAR_RATIO = MOTOR_TO_CANCODER * CANCODER_TO_PIVOT; public static final double SIM_LENGTH_METERS = Units.inchesToMeters(12.910); // TODO: find the moi public static final double SIM_MOI = 0.04064471269; - public static final Rotation2d MAX_ANGLE = Rotation2d.fromRotations(0.14); - public static final Rotation2d MIN_ANGLE = Rotation2d.fromRotations(0.01); + public static final Rotation2d MAX_ANGLE = Rotation2d.fromRotations(0.174); + public static final Rotation2d MIN_ANGLE = Rotation2d.fromRotations(0.044); public static final Rotation2d REAL_ZEROED_ANGLE = Rotation2d.fromDegrees(8.5); public static final double PIVOT_HEIGHT = Units.inchesToMeters(10.5); diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java index b763ecd3..bf3b2661 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java @@ -55,7 +55,7 @@ public void execute() { drivetrain.getPose() .minus(target.get()).getTranslation().getAngle() .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0)) - .minus(Rotation2d.fromDegrees(3)); + .plus(Rotation2d.fromDegrees(5)); // Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); drivetrain.setTargetPose(new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); Logger.recordOutput("Targeting/rotErrorDegrees", Math.abs(targetRot.minus(drivetrain.getRotation()).getDegrees())); diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java index 00b71d25..dced6d9a 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java @@ -15,6 +15,10 @@ public class AutoShootPrepare extends SequentialCommandGroup { public AutoShootPrepare(Drivetrain drivetrain, Shooter shooter) { this(drivetrain::getPose, shooter); } + double A = -0.6953; + double B = 0.8702; + double C = -0.4942; + double D = 1.491; public AutoShootPrepare(Supplier positionSupplier, Shooter shooter) { addCommands( @@ -27,7 +31,14 @@ public AutoShootPrepare(Supplier positionSupplier, Shooter shooter) { // ) // ).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), // 8000, 6000) - shooter.lerp.get(positionSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation())) +// shooter.lerp.get(positionSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation())) + new ShooterSetpoint( + Rotation2d.fromRadians( + A * Math.atan(B * (positionSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation())) + C) + D + ), + 7000, + 3000 + ) ) ); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index e895d5af..13cb201e 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -49,18 +49,22 @@ public class Shooter extends SubsystemBase { private boolean flipper = false; + private Rotation2d angleOffset = new Rotation2d(); + public final ShooterLerp lerp = new ShooterLerp().put( - new Pair<>(1.2954, new ShooterSetpoint(Rotation2d.fromDegrees(40))), - new Pair<>(1.842, new ShooterSetpoint(Rotation2d.fromDegrees(37))), - new Pair<>(2.012, new ShooterSetpoint(Rotation2d.fromDegrees(34.422))), - new Pair<>(2.238, new ShooterSetpoint(Rotation2d.fromDegrees(31))), - new Pair<>(2.747, new ShooterSetpoint(Rotation2d.fromDegrees(25.5))), - new Pair<>(3.175, new ShooterSetpoint(Rotation2d.fromDegrees(22))), - new Pair<>(3.197, new ShooterSetpoint(Rotation2d.fromDegrees(22))), -// new Pair<>(3.990, new ShooterSetpoint(Rotation2d.fromDegrees(17.2))), - new Pair<>(4.029, new ShooterSetpoint(Rotation2d.fromDegrees(16.5))), - new Pair<>(4.256, new ShooterSetpoint(Rotation2d.fromDegrees(16.2))), - new Pair<>(5.300, new ShooterSetpoint(Rotation2d.fromDegrees(13))) + new Pair<>(1.386, new ShooterSetpoint(Rotation2d.fromRadians(1.06184))), + new Pair<>(1.673, new ShooterSetpoint(Rotation2d.fromRadians(0.9563528))), + new Pair<>(2.0398, new ShooterSetpoint(Rotation2d.fromRadians(0.8606))), + new Pair<>(2.35254, new ShooterSetpoint(Rotation2d.fromRadians(0.79248))), + new Pair<>(2.36, new ShooterSetpoint(Rotation2d.fromRadians(0.79976))), + new Pair<>(2.632, new ShooterSetpoint(Rotation2d.fromRadians(0.758056))), + new Pair<>(2.9345, new ShooterSetpoint(Rotation2d.fromRadians(0.7332))), + new Pair<>(3.222, new ShooterSetpoint(Rotation2d.fromRadians(0.699088))), + new Pair<>(3.5768, new ShooterSetpoint(Rotation2d.fromRadians(0.67704))), + new Pair<>(3.883, new ShooterSetpoint(Rotation2d.fromRadians(0.64688))), + new Pair<>(4.22, new ShooterSetpoint(Rotation2d.fromRadians(0.603304))), + new Pair<>(4.54, new ShooterSetpoint(Rotation2d.fromRadians(0.59904))), + new Pair<>(4.794, new ShooterSetpoint(Rotation2d.fromRadians(0.59384))) ); private static boolean hasInstance = false; @@ -275,11 +279,16 @@ public Rotation2d getPivotSetpoint(){ return pivotSetpoint; } + @AutoLogOutput(key = "Shooter/Pivot/AbsolutePivotSetpoint") + public Rotation2d getAbsolutePivotSetpoint(){ + return pivotSetpoint.plus(angleOffset); + } + public void zeroPivot() { pivotIO.setEncoderPosition(0); } public void zeroPivotToCancoder(){ - pivotIO.setEncoderPosition(Rotation2d.fromDegrees(pivotInputs.absolutePosition.getDegrees()*0.984 - 140).getRotations()); + pivotIO.setEncoderPosition(pivotInputs.absolutePosition.getRotations()); flipper = !flipper; } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java index 3679cba3..5278db60 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java @@ -88,7 +88,7 @@ public ShooterPivotIOTalonFX() { motor.optimizeBusUtilization(); cancoder.optimizeBusUtilization(); - motor.setPosition(0); + motor.setPosition(absolutePosition.getValueAsDouble() / CANCODER_TO_PIVOT * CHAIN_FACTOR); } @Override @@ -97,7 +97,7 @@ public void updateInputs(ShooterPivotIOInputs inputs) { inputs.isAtForwardLimit = forwardLimit.getValue() == ForwardLimitValue.ClosedToGround; inputs.isAtReverseLimit = reverseLimit.getValue() == ReverseLimitValue.ClosedToGround; inputs.position = Rotation2d.fromRotations(position.getValueAsDouble()); - inputs.absolutePosition = Rotation2d.fromRotations(absolutePosition.getValueAsDouble() / CANCODER_TO_PIVOT); + inputs.absolutePosition = Rotation2d.fromRotations(absolutePosition.getValueAsDouble() / CANCODER_TO_PIVOT * CHAIN_FACTOR); inputs.velocityRPS = velocity.getValueAsDouble(); inputs.appliedVolts = appliedVoltage.getValueAsDouble(); inputs.currentAmps = current.getValueAsDouble(); diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java index 8fc308c4..08585d1f 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java @@ -105,7 +105,7 @@ public void periodic() { updateAndAcceptPose(frontCameraInputs, frontPose); updateAndAcceptPose(rearCameraInputs, rearPose); - if (TAKE_SNAPSHOTS && DriverStation.isFMSAttached() && RobotState.isEnabled()) { + if (TAKE_SNAPSHOTS && /*DriverStation.isFMSAttached() &&*/ RobotState.isEnabled()) { Logger.runEveryN((int) (SNAPSHOT_PERIOD_SECS / Constants.LOOP_PERIOD_SECS), () -> takeSnapshot( String.format("%s_%s%d_%d", diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index 786d0422..fe861722 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -84,7 +84,7 @@ public void setPathIndex(int index){ * @param pathIndex * @return the commands to run in the segment */ - protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder) { + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw) { return Commands.sequence( Commands.deadline( getPath(pathIndex).getCommand(drivetrain), @@ -97,9 +97,9 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, ).onlyIf(()->shouldZeroCancoder), drivetrain.commandCopyVisionPose(), Commands.parallel( - new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4), + new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4).onlyIf(()->shouldRealignYaw), Commands.sequence( - Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10), + Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10).onlyIf(()->shouldRealignYaw), new ParallelDeadlineGroup( Commands.sequence( Commands.waitUntil(()->!indexer.isNoteStaged()), @@ -112,11 +112,11 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, ); } protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex){ - return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, false); + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, false, false); } protected Command createCancoderSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex) { - return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, true); + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, true, false); } public List toTrajectory() { From d4e0a2dae1824b2000f88dd0117d511dae0ff659 Mon Sep 17 00:00:00 2001 From: Zach R Date: Wed, 20 Mar 2024 19:15:48 -0700 Subject: [PATCH 47/75] fix: override leds on top --- .../java/org/team1540/robot2024/Robot.java | 6 ++--- .../team1540/robot2024/RobotContainer.java | 3 ++- .../robot2024/subsystems/led/Leds.java | 27 ++++++++++++++----- .../led/patterns/SimpleLedPattern.java | 9 +++++++ 4 files changed, 35 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 2568db08..5e6e7f0a 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -134,7 +134,8 @@ public void disabledInit() { robotContainer.shooter.setPivotBrakeMode(false); robotContainer.drivetrain.unblockTags(); robotContainer.shooter.setPivotPosition(new Rotation2d()); - robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK, new LedPatternRainbow(1)); + robotContainer.leds.setPattern(Leds.Zone.MAIN, new LedPatternRainbow(1)); + robotContainer.leds.setPattern(Leds.Zone.TOP, SimpleLedPattern.blank()); } /** @@ -146,7 +147,6 @@ public void disabledPeriodic() { } public void enabledInit() { -// robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK,flamePattern); robotContainer.elevator.setBrakeMode(true); robotContainer.shooter.setPivotBrakeMode(true); robotContainer.drivetrain.setBrakeMode(true); @@ -200,7 +200,7 @@ public void teleopPeriodic() { */ @Override public void testInit() { - robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK,new LedPatternTuneColor()); + robotContainer.leds.setPattern(Leds.Zone.MAIN,new LedPatternTuneColor()); // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index b774ab0b..dc3d9626 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -25,6 +25,7 @@ import org.team1540.robot2024.subsystems.led.patterns.LedPatternBreathing; import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState; import org.team1540.robot2024.subsystems.led.patterns.LedPatternWave; +import org.team1540.robot2024.subsystems.led.patterns.SimpleLedPattern; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.subsystems.tramp.Tramp; import org.team1540.robot2024.subsystems.vision.AprilTagVision; @@ -207,7 +208,7 @@ private void configureButtonBindings() { .whileTrue(PrepareShooterCommand.lowerPivot(shooter)); new Trigger(indexer::isNoteStaged).debounce(0.1) .onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 1, 1)) - .whileTrue(leds.commandShowPattern(new LedPatternBreathing("#ff0000"), Leds.PatternLevel.INTAKE_STATE)); + .whileTrue(leds.commandShowIntakePattern(SimpleLedPattern.solid("#ff0000"))); new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4)); diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index 25c72967..b5087880 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -24,7 +24,8 @@ public Leds() { strip.setData(ledBuffer); strip.start(); - buffers[Zone.ELEVATOR_BACK.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 41, false); + buffers[Zone.MAIN.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 41, false); + buffers[Zone.TOP.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 35, 41, false); for (int i = 0; i < ZONE_COUNT;i++) { patterns[i] = new LedTriager(); } @@ -78,7 +79,8 @@ public void clearPatternAll(PatternLevel priority) { private static final int ZONE_COUNT=Zone.values().length; public enum Zone { - ELEVATOR_BACK, + MAIN, + TOP } static final int LEVEL_COUNT = PatternLevel.values().length; public enum PatternLevel { @@ -90,15 +92,28 @@ public enum PatternLevel { public Command commandShowPattern(LedPattern pattern, PatternLevel priority) { return Commands.startEnd( - () -> this.setPattern(Leds.Zone.ELEVATOR_BACK, pattern, priority), - () -> this.clearPattern(Leds.Zone.ELEVATOR_BACK, priority) + () -> this.setPattern(Leds.Zone.MAIN, pattern, priority), + () -> this.clearPattern(Leds.Zone.MAIN, priority) + ).ignoringDisable(true); + } + + public Command commandShowIntakePattern(LedPattern pattern) { + return Commands.startEnd( + () -> { + this.setPattern(Zone.MAIN, pattern, PatternLevel.INTAKE_STATE); + this.setPattern(Zone.TOP, pattern, PatternLevel.INTAKE_STATE); + }, + () -> { + this.clearPattern(Zone.MAIN, PatternLevel.INTAKE_STATE); + this.clearPattern(Zone.TOP, PatternLevel.INTAKE_STATE); + } ).ignoringDisable(true); } public Command commandSet(LedPattern pattern, PatternLevel priority) { - return Commands.runOnce(() -> this.setPattern(Leds.Zone.ELEVATOR_BACK, pattern, priority)).ignoringDisable(true); + return Commands.runOnce(() -> this.setPattern(Leds.Zone.MAIN, pattern, priority)).ignoringDisable(true); } public Command commandClear(PatternLevel priority) { - return Commands.runOnce(() -> this.clearPattern(Leds.Zone.ELEVATOR_BACK, priority)).ignoringDisable(true); + return Commands.runOnce(() -> this.clearPattern(Leds.Zone.MAIN, priority)).ignoringDisable(true); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java index 2c3e316d..cb41f32c 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java @@ -21,6 +21,9 @@ public void apply(ZonedAddressableLEDBuffer buffer) { public static LedPattern solid(Color color) { return new SimpleLedPattern((buffer, i) -> buffer.setLED(i, color)); } + public static LedPattern solid(String color) { + return solid(new Color(color)); + } public static LedPattern alternating(Color... colors) { return alternating(1, colors); } @@ -28,4 +31,10 @@ public static LedPattern alternating(int groupSize, Color... colors) { final int colorCount = colors.length; return new SimpleLedPattern((buffer, i) -> buffer.setLED(i, colors[(i/groupSize)%colorCount])); } + public static LedPattern blank() { + return new LedPattern(false) { + @Override + public void apply(ZonedAddressableLEDBuffer buffer) {} + }; + } } From 7550dd740c5a30f39ab2b426d5cdb71c985ee4b2 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Wed, 20 Mar 2024 21:11:25 -0700 Subject: [PATCH 48/75] fix: actually log gyro inputs :skull: --- .../java/org/team1540/robot2024/subsystems/drive/Drivetrain.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 2a580a2f..7d8ee2ed 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -153,6 +153,7 @@ public static Drivetrain createDummy() { @Override public void periodic() { gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drivetrain/Gyro", gyroInputs); for (Module module : modules) { module.periodic(); From 0adf32ada024247ecf840cba0e35663e720d7bfc Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Wed, 20 Mar 2024 21:18:57 -0700 Subject: [PATCH 49/75] fix: remove unnecessary config print --- .../robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java index 5278db60..b813a1f8 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java @@ -129,7 +129,6 @@ public void configPID(double kP, double kI, double kD, double kG) { pidConfigs.kD = kD; pidConfigs.kG = kG; motor.getConfigurator().apply(pidConfigs); - System.out.println(pidConfigs); } @Override public void setEncoderPosition(double rots) { From 3fd8a41cb7fe7044070fa8ff8f462aa7314f7b60 Mon Sep 17 00:00:00 2001 From: Zach R Date: Wed, 20 Mar 2024 23:10:40 -0700 Subject: [PATCH 50/75] feat: add beam breaks to sim --- .../team1540/robot2024/subsystems/indexer/IndexerIOSim.java | 3 +++ .../org/team1540/robot2024/subsystems/tramp/TrampIOSim.java | 6 +++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java index 730a41cf..0346dda0 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import org.team1540.robot2024.Constants; @@ -14,6 +15,7 @@ public class IndexerIOSim implements IndexerIO { private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO, INTAKE_MOI); private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO, FEEDER_MOI); // private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break"); + private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID); private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI, FEEDER_KD); private boolean isClosedLoop = true; private double intakeVoltage = 0.0; @@ -37,6 +39,7 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps(); inputs.feederVoltage = feederVoltage; inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); + inputs.noteInIntake = indexerBeamBreak.get(); } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java index fd718ccd..2726701b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java @@ -2,22 +2,26 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import org.team1540.robot2024.Constants; +import static org.team1540.robot2024.Constants.Tramp.BEAM_BREAK_CHANNEL; import static org.team1540.robot2024.Constants.Tramp.GEAR_RATIO; public class TrampIOSim implements TrampIO { private final DCMotorSim sim = new DCMotorSim(DCMotor.getNEO(1), GEAR_RATIO, 0.025); //TODO: Fix MOI its probably wrong :D - + private final DigitalInput beamBreak = new DigitalInput(BEAM_BREAK_CHANNEL); private double appliedVolts = 0.0; + @Override public void updateInputs(TrampIOInputs inputs) { sim.update(Constants.LOOP_PERIOD_SECS); inputs.velocityRPM = sim.getAngularVelocityRPM(); inputs.appliedVolts = appliedVolts; inputs.currentAmps = sim.getCurrentDrawAmps(); + inputs.noteInTramp =beamBreak.get(); } @Override From 6ba98c9bbcc2ffbc55db3aee8796d65b4a00e193 Mon Sep 17 00:00:00 2001 From: Zach R Date: Thu, 21 Mar 2024 08:08:36 -0700 Subject: [PATCH 51/75] fix: add leds to indicate tramp state, fix intake indicator --- .../org/team1540/robot2024/RobotContainer.java | 4 ++++ .../team1540/robot2024/subsystems/led/Leds.java | 14 ++++++++++++++ .../subsystems/led/patterns/SimpleLedPattern.java | 2 +- 3 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index dc3d9626..87549197 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -206,10 +206,14 @@ private void configureButtonBindings() { new Trigger(() -> elevator.getPosition() > 0.1).debounce(0.1) .whileTrue(PrepareShooterCommand.lowerPivot(shooter)); + new Trigger(indexer::isNoteStaged).debounce(0.1) .onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 1, 1)) .whileTrue(leds.commandShowIntakePattern(SimpleLedPattern.solid("#ff0000"))); + new Trigger(tramp::isNoteStaged).debounce(0.1) + .whileTrue(leds.commandShowPattern(SimpleLedPattern.solid("#ff9900"), Leds.PatternLevel.TRAMP_STATE)); + new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4)); new Trigger(RobotController::getUserButton).toggleOnTrue(Commands.startEnd( diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index b5087880..cf8b4195 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -86,6 +86,7 @@ public enum Zone { public enum PatternLevel { DEFAULT, INTAKE_STATE, + TRAMP_STATE, DRIVER_LOCK, ELEVATOR_STATE } @@ -109,6 +110,19 @@ public Command commandShowIntakePattern(LedPattern pattern) { } ).ignoringDisable(true); } + + public Command commandShowTrampPattern(LedPattern pattern) { + return Commands.startEnd( + () -> { + this.setPattern(Zone.MAIN, pattern, PatternLevel.TRAMP_STATE); + this.setPattern(Zone.TOP, pattern, PatternLevel.TRAMP_STATE); + }, + () -> { + this.clearPattern(Zone.MAIN, PatternLevel.TRAMP_STATE); + this.clearPattern(Zone.TOP, PatternLevel.TRAMP_STATE); + } + ).ignoringDisable(true); + } public Command commandSet(LedPattern pattern, PatternLevel priority) { return Commands.runOnce(() -> this.setPattern(Leds.Zone.MAIN, pattern, priority)).ignoringDisable(true); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java index cb41f32c..049c57ed 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java @@ -8,7 +8,7 @@ public class SimpleLedPattern extends LedPattern{ private final BiConsumer applier; private SimpleLedPattern(BiConsumer applier) { - super(false); + super(true); this.applier = applier; } From f9e54bef4576ed99b7093baa8e52ef235d930dd1 Mon Sep 17 00:00:00 2001 From: Zach R Date: Thu, 21 Mar 2024 08:13:20 -0700 Subject: [PATCH 52/75] feat: allow tramp indicator to appear on top 5 --- src/main/java/org/team1540/robot2024/RobotContainer.java | 3 +-- src/main/java/org/team1540/robot2024/subsystems/led/Leds.java | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 87549197..ad9f694b 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -22,7 +22,6 @@ import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.led.Leds; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternBreathing; import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState; import org.team1540.robot2024.subsystems.led.patterns.LedPatternWave; import org.team1540.robot2024.subsystems.led.patterns.SimpleLedPattern; @@ -212,7 +211,7 @@ private void configureButtonBindings() { .whileTrue(leds.commandShowIntakePattern(SimpleLedPattern.solid("#ff0000"))); new Trigger(tramp::isNoteStaged).debounce(0.1) - .whileTrue(leds.commandShowPattern(SimpleLedPattern.solid("#ff9900"), Leds.PatternLevel.TRAMP_STATE)); + .whileTrue(leds.commandShowTrampPattern(SimpleLedPattern.solid("#ff9900"))); new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4)); diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index cf8b4195..91706287 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -85,8 +85,8 @@ public enum Zone { static final int LEVEL_COUNT = PatternLevel.values().length; public enum PatternLevel { DEFAULT, - INTAKE_STATE, TRAMP_STATE, + INTAKE_STATE, DRIVER_LOCK, ELEVATOR_STATE } From ab295b7c7fa6d4f03d12204c68af4a186fd2cb36 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Thu, 21 Mar 2024 08:29:11 -0700 Subject: [PATCH 53/75] feat: updates to CenterLanePCBAFSprint --- paths.chor | 3503 ++++++++--------- .../choreo/CenterLanePCBAFSprint.1.traj | 600 ++- .../choreo/CenterLanePCBAFSprint.2.traj | 146 +- .../choreo/CenterLanePCBAFSprint.3.traj | 329 +- .../choreo/CenterLanePCBAFSprint.4.traj | 1666 ++++---- .../choreo/CenterLanePCBAFSprint.5.traj | 728 ++-- .../deploy/choreo/CenterLanePCBAFSprint.traj | 3483 ++++++++-------- .../commands/autos/CenterLanePCBAFSprint.java | 12 +- .../drivetrain/DriveWithTargetingCommand.java | 3 +- .../commands/shooter/ShootSequence.java | 14 +- .../robot2024/util/auto/AutoCommand.java | 8 +- 11 files changed, 5100 insertions(+), 5392 deletions(-) diff --git a/paths.chor b/paths.chor index 778dd1b6..b1758313 100644 --- a/paths.chor +++ b/paths.chor @@ -22651,7 +22651,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 17 }, { "x": 2.6651716232299805, @@ -22660,7 +22660,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { "x": 1.8129411935806274, @@ -22672,7 +22672,7 @@ "controlIntervalCount": 10 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, "isInitialGuess": false, @@ -22687,7 +22687,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { "x": 2.6483445167541504, @@ -22696,7 +22696,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 18 }, { "x": 4.16749906539917, @@ -22705,7 +22705,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 13 }, { "x": 5.665951728820801, @@ -22714,7 +22714,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 12 }, { "x": 7.217291355133057, @@ -22723,7 +22723,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 9 }, { "x": 8.0634765625, @@ -22732,7 +22732,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 15 }, { "x": 5.665951728820801, @@ -22741,7 +22741,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 13 }, { "x": 4.16749906539917, @@ -22768,7 +22768,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 13 }, { "x": 5.665951728820801, @@ -22777,7 +22777,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 16 + "controlIntervalCount": 15 }, { "x": 8.0634765625, @@ -22791,1885 +22791,1786 @@ ], "trajectory": [ { - "x": 1.2899744510650635, + "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -5.737773088876545e-18, - "angularVelocity": -1.3800854803157527e-17, - "velocityX": -2.0304788403041935e-16, - "velocityY": -3.8084104282489897e-16, + "heading": -1.2424307943085809e-21, + "angularVelocity": -1.3399063907175068e-21, + "velocityX": -6.613526246981409e-20, + "velocityY": -3.127454467901795e-20, "timestamp": 0 }, { - "x": 1.3043755674833586, - "y": 5.574433574042641, - "heading": -0.009496998526070185, - "angularVelocity": -0.1264048176402608, - "velocityX": 0.19167850659141716, - "velocityY": -0.21989050741887875, - "timestamp": 0.07513161842720571 - }, - { - "x": 1.333181418313963, - "y": 5.541395354098072, - "heading": -0.02848354997968903, - "angularVelocity": -0.2527105345468862, - "velocityX": 0.38340516859413276, - "velocityY": -0.43973789779720845, - "timestamp": 0.15026323685441142 - }, - { - "x": 1.3763968626631116, - "y": 5.491843734926518, - "heading": -0.05694076492631607, - "angularVelocity": -0.3787648335429662, - "velocityX": 0.5751965051812923, - "velocityY": -0.6595308368163946, - "timestamp": 0.22539485528161712 - }, - { - "x": 1.434028520834508, - "y": 5.425784149039423, - "heading": -0.09483975910440062, - "angularVelocity": -0.5044346837260156, - "velocityX": 0.76707595784789, - "velocityY": -0.879251469241908, - "timestamp": 0.30052647370882285 - }, - { - "x": 1.5060856921499353, - "y": 5.34322422233644, - "heading": -0.14214370318506256, - "angularVelocity": -0.6296143364447188, - "velocityX": 0.9590791842586537, - "velocityY": -1.0988706011306708, - "timestamp": 0.3756580921360286 - }, - { - "x": 1.5925821728366396, - "y": 5.244175419847767, - "heading": -0.19881016609697202, - "angularVelocity": -0.7542292326445892, - "velocityX": 1.15126603811467, - "velocityY": -1.318337133749589, - "timestamp": 0.4507897105632343 - }, - { - "x": 1.6935404999326085, - "y": 5.128656800108543, - "heading": -0.2647926425863519, - "angularVelocity": -0.8782251450575851, - "velocityX": 1.3437528593918235, - "velocityY": -1.5375499977346858, - "timestamp": 0.52592132899044 - }, - { - "x": 1.8090047533297515, - "y": 4.99670620893713, - "heading": -0.34003720148047023, - "angularVelocity": -1.001503234885639, - "velocityX": 1.5368263829457236, - "velocityY": -1.7562591348086285, - "timestamp": 0.6010529474176457 - }, - { - "x": 1.9391049081706304, - "y": 4.848436880668431, - "heading": -0.42444836738377417, - "angularVelocity": -1.1235105495461353, - "velocityX": 1.731629872448852, - "velocityY": -1.9734611256713033, - "timestamp": 0.6761845658448514 - }, - { - "x": 2.0768333691424767, - "y": 4.720578672204384, - "heading": -0.4949304334552377, - "angularVelocity": -0.938114572048371, - "velocityX": 1.8331624402684015, - "velocityY": -1.7017896211431234, - "timestamp": 0.7513161842720572 - }, - { - "x": 2.200438107278665, - "y": 4.609474439269374, - "heading": -0.5559710604478235, - "angularVelocity": -0.8124492493047433, - "velocityX": 1.6451760354351246, - "velocityY": -1.4787946174642006, - "timestamp": 0.8264478026992629 - }, - { - "x": 2.30979055136467, - "y": 4.515017163004073, - "heading": -0.6076423765200641, - "angularVelocity": -0.6877439505764344, - "velocityX": 1.4554783509002944, - "velocityY": -1.2572240321695016, - "timestamp": 0.9015794211264686 - }, - { - "x": 2.404847576631005, - "y": 4.43717046489753, - "heading": -0.6499678685895577, - "angularVelocity": -0.5633512621433749, - "velocityX": 1.2652066767447667, - "velocityY": -1.0361376439603944, - "timestamp": 0.9767110395536743 - }, - { - "x": 2.4855875280124384, - "y": 4.375915950789138, - "heading": -0.6829583682346397, - "angularVelocity": -0.43910274175131525, - "velocityX": 1.0746467742038588, - "velocityY": -0.8152960815764004, - "timestamp": 1.05184265798088 - }, - { - "x": 2.5519973917574865, - "y": 4.331242518343638, - "heading": -0.7066196503968551, - "angularVelocity": -0.3149310856889676, - "velocityX": 0.8839136589509067, - "velocityY": -0.5946022910015373, - "timestamp": 1.1269742764080857 - }, - { - "x": 2.6040685139576465, - "y": 4.3031427656124555, - "heading": -0.720955031446972, - "angularVelocity": -0.19080357043979132, - "velocityX": 0.6930653603887426, - "velocityY": -0.37400702018003557, - "timestamp": 1.2021058948352914 - }, - { - "x": 2.641794765155857, - "y": 4.291611444641949, - "heading": -0.7259664732810771, - "angularVelocity": -0.06670216797910607, - "velocityX": 0.5021354789039092, - "velocityY": -0.15348159949241175, - "timestamp": 1.2772375132624971 + "x": 1.3060573925677244, + "y": 5.572382489557548, + "heading": -0.010694903924415845, + "angularVelocity": -0.14212387666711845, + "velocityX": 0.21372515458978017, + "velocityY": -0.2467996203811156, + "timestamp": 0.07525057840538203 + }, + { + "x": 1.3382234703338376, + "y": 5.5352391647325, + "heading": -0.03207617924105129, + "angularVelocity": -0.2841343650736142, + "velocityX": 0.4274528973429488, + "velocityY": -0.49359520700229137, + "timestamp": 0.15050115681076406 + }, + { + "x": 1.3864731960687624, + "y": 5.479524641793448, + "heading": -0.06412173122661301, + "angularVelocity": -0.4258512381516761, + "velocityX": 0.6411874401150632, + "velocityY": -0.7403866404709017, + "timestamp": 0.2257517352161461 + }, + { + "x": 1.4508074313628532, + "y": 5.405239333646717, + "heading": -0.10679840962568042, + "angularVelocity": -0.5671275796601069, + "velocityX": 0.8549334325048802, + "velocityY": -0.9871725868543993, + "timestamp": 0.30100231362152813 + }, + { + "x": 1.5312274241075188, + "y": 5.312383875814348, + "heading": -0.16006570142322632, + "angularVelocity": -0.7078655463694897, + "velocityX": 1.0686960080417647, + "velocityY": -1.2339500878272134, + "timestamp": 0.37625289202691015 + }, + { + "x": 1.6277348707062913, + "y": 5.200959293585018, + "heading": -0.22388050123959674, + "angularVelocity": -0.8480306885163599, + "velocityX": 1.282481126973902, + "velocityY": -1.480713963806038, + "timestamp": 0.45150347043229216 + }, + { + "x": 1.740332134586227, + "y": 5.070967309798755, + "heading": -0.29820239377925056, + "angularVelocity": -0.9876587544520216, + "velocityX": 1.4962976533331545, + "velocityY": -1.7274549450767482, + "timestamp": 0.5267540488376742 + }, + { + "x": 1.869023591989777, + "y": 4.922411591689274, + "heading": -0.38299783161333695, + "angularVelocity": -1.126841010806393, + "velocityX": 1.7101723352912572, + "velocityY": -1.9741471927192127, + "timestamp": 0.6020046272430563 + }, + { + "x": 2.0218400763966784, + "y": 4.778612689185939, + "heading": -0.4624367890743981, + "angularVelocity": -1.0556590945137452, + "velocityX": 2.030768236539849, + "velocityY": -1.9109341821756607, + "timestamp": 0.6772552056484383 + }, + { + "x": 2.158571900472673, + "y": 4.653385130962418, + "heading": -0.5314200615234237, + "angularVelocity": -0.9167141822805146, + "velocityX": 1.817020240554264, + "velocityY": -1.664140806319243, + "timestamp": 0.7525057840538204 + }, + { + "x": 2.2792154502458053, + "y": 4.546725640045298, + "heading": -0.5899528620113615, + "angularVelocity": -0.777838545939356, + "velocityX": 1.6032242187324346, + "velocityY": -1.4173909779475127, + "timestamp": 0.8277563624592025 + }, + { + "x": 2.383769251108378, + "y": 4.458632746098652, + "heading": -0.6380359031204368, + "angularVelocity": -0.6389723790566404, + "velocityX": 1.3894086009456479, + "velocityY": -1.170660688773468, + "timestamp": 0.9030069408645846 + }, + { + "x": 2.4722324605852046, + "y": 4.389105547577967, + "heading": -0.6756681828251195, + "angularVelocity": -0.5000928963223903, + "velocityX": 1.1755817875613708, + "velocityY": -0.9239423801653125, + "timestamp": 0.9782575192699666 + }, + { + "x": 2.5446045503525854, + "y": 4.33814344893939, + "heading": -0.7028480663782789, + "angularVelocity": -0.36119168954076036, + "velocityX": 0.9617479533181219, + "velocityY": -0.6772319857003534, + "timestamp": 1.0535080976753486 + }, + { + "x": 2.600885196215428, + "y": 4.305746063716301, + "heading": -0.7195737337553716, + "angularVelocity": -0.22226629657236585, + "velocityX": 0.7479098108675392, + "velocityY": -0.4305267269649537, + "timestamp": 1.1287586760807307 + }, + { + "x": 2.6410742311964075, + "y": 4.29191316967755, + "heading": -0.7258433495686913, + "angularVelocity": -0.08331651325714302, + "velocityX": 0.5340694494662552, + "velocityY": -0.1838244214447216, + "timestamp": 1.2040092544861127 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.057384071798383285, - "velocityX": 0.3111454080847126, - "velocityY": 0.06699234114487357, - "timestamp": 1.3523691316897029 - }, - { - "x": 2.6741706927639637, - "y": 4.318416553359305, - "heading": -0.7079150843864638, - "angularVelocity": 0.1820368667672511, - "velocityX": 0.11922552840405579, - "velocityY": 0.2884478427083078, - "timestamp": 1.4278485164558985 - }, - { - "x": 2.6686789976139393, - "y": 4.35689985788212, - "heading": -0.6847718372969428, - "angularVelocity": 0.306616795565399, - "velocityX": -0.0727575505371169, - "velocityY": 0.5098518574324268, - "timestamp": 1.5033279012220941 - }, - { - "x": 2.648690219871855, - "y": 4.4120894355267595, - "heading": -0.6522316141278892, - "angularVelocity": 0.4311140488042709, - "velocityX": -0.26482433323531773, - "velocityY": 0.7311874336773603, - "timestamp": 1.5788072859882898 - }, - { - "x": 2.6141955291979047, - "y": 4.483978018466003, - "heading": -0.6103014314016028, - "angularVelocity": 0.5555183425901422, - "velocityX": -0.4570081060213073, - "velocityY": 0.9524267210180263, - "timestamp": 1.6542866707544854 - }, - { - "x": 2.565181654912471, - "y": 4.572554575453533, - "heading": -0.5589890545924265, - "angularVelocity": 0.6798197543188936, - "velocityX": -0.6493676974062975, - "velocityY": 1.173519859229229, - "timestamp": 1.729766055520681 - }, - { - "x": 2.501626381973055, - "y": 4.6778004085012075, - "heading": -0.49830336104516576, - "angularVelocity": 0.8040035532069214, - "velocityX": -0.842021608129967, - "velocityY": 1.394365274326458, - "timestamp": 1.8052454402868767 - }, - { - "x": 2.4234850468656797, - "y": 4.799677413625942, - "heading": -0.42825793399699796, - "angularVelocity": 0.9280073925294743, - "velocityX": -1.0352672501499576, - "velocityY": 1.614705863061015, - "timestamp": 1.8807248250530724 - }, - { - "x": 2.330623714307083, - "y": 4.938069448938432, - "heading": -0.3489003984671445, - "angularVelocity": 1.0513802646058852, - "velocityX": -1.2302873539560828, - "velocityY": 1.8335077284709782, - "timestamp": 1.956204209819268 - }, - { - "x": 2.2158437322138607, - "y": 5.0727178717793375, - "heading": -0.2714542834500897, - "angularVelocity": 1.0260565220540956, - "velocityX": -1.5206798842301303, - "velocityY": 1.7839099147754294, - "timestamp": 2.0316835945854637 - }, - { - "x": 2.115226855971239, - "y": 5.19038091333927, - "heading": -0.20366940407384615, - "angularVelocity": 0.8980581861967815, - "velocityX": -1.333037842725262, - "velocityY": 1.5588765320192262, - "timestamp": 2.107162979351659 - }, - { - "x": 2.028914050239155, - "y": 5.291175475979272, - "heading": -0.14552121166963816, - "angularVelocity": 0.7703850870803018, - "velocityX": -1.1435282096780108, - "velocityY": 1.3353919478119864, - "timestamp": 2.1826423641178545 - }, - { - "x": 1.956951239222395, - "y": 5.375140495354652, - "heading": -0.09702855704513491, - "angularVelocity": 0.6424622401999767, - "velocityX": -0.9534101428822267, - "velocityY": 1.1124232085021941, - "timestamp": 2.25812174888405 - }, - { - "x": 1.8993610593828132, - "y": 5.442295470240139, - "heading": -0.05821541772623594, - "angularVelocity": 0.5142217234562786, - "velocityX": -0.7629921735099331, - "velocityY": 0.8897128016480877, - "timestamp": 2.3336011336502454 - }, - { - "x": 1.856157020573883, - "y": 5.492652116983436, - "heading": -0.029101141017864884, - "angularVelocity": 0.3857248810290673, - "velocityX": -0.5723952168077603, - "velocityY": 0.6671576205923756, - "timestamp": 2.4090805184164408 - }, - { - "x": 1.8273481914604963, - "y": 5.526218220177199, - "heading": -0.00969618965979829, - "angularVelocity": 0.2570894214180142, - "velocityX": -0.38167811253199, - "velocityY": 0.44470557488567636, - "timestamp": 2.484559903182636 + "angularVelocity": 0.05565717315833812, + "velocityX": 0.3202286619480617, + "velocityY": 0.06287683198324669, + "timestamp": 1.2792598328914948 + }, + { + "x": 2.6731094248331972, + "y": 4.320288497186145, + "heading": -0.7067986870474628, + "angularVelocity": 0.19580509628770462, + "velocityX": 0.10461882238047351, + "velocityY": 0.31162123137301345, + "timestamp": 1.355133384348122 + }, + { + "x": 2.6646882165451484, + "y": 4.3628057123101, + "heading": -0.6813129128721624, + "angularVelocity": 0.33589799984345314, + "velocityX": -0.11099003706005371, + "velocityY": 0.5603693817898957, + "timestamp": 1.431006935804749 + }, + { + "x": 2.6399081500891253, + "y": 4.424196738035329, + "heading": -0.6452019314670412, + "angularVelocity": 0.47593635347047025, + "velocityX": -0.32659689681441373, + "velocityY": 0.8091228701785946, + "timestamp": 1.5068804872613761 + }, + { + "x": 2.5987694540063986, + "y": 4.504462102028234, + "heading": -0.5984691810768386, + "angularVelocity": 0.6159293916394221, + "velocityX": -0.5422007444352149, + "velocityY": 1.057883313117199, + "timestamp": 1.5827540387180032 + }, + { + "x": 2.5412724297468685, + "y": 4.603602434973848, + "heading": -0.5411158632570503, + "angularVelocity": 0.7559065935192721, + "velocityX": -0.7578006189995966, + "velocityY": 1.30665206837309, + "timestamp": 1.6586275901746304 + }, + { + "x": 2.4674174122162253, + "y": 4.7216183838553025, + "heading": -0.4731379534998271, + "angularVelocity": 0.8959368377013766, + "velocityX": -0.973396079566176, + "velocityY": 1.5554293507523191, + "timestamp": 1.7345011416312575 + }, + { + "x": 2.3772045458929667, + "y": 4.8585103095087545, + "heading": -0.3945211417087051, + "angularVelocity": 1.0361556864260284, + "velocityX": -1.1889896359316807, + "velocityY": 1.8042113888883347, + "timestamp": 1.8103746930878846 + }, + { + "x": 2.270632337424553, + "y": 5.014276859543665, + "heading": -0.3052336675309315, + "angularVelocity": 1.176793130987344, + "velocityX": -1.4046028744197523, + "velocityY": 2.0529756027560517, + "timestamp": 1.8862482445445117 + }, + { + "x": 2.1562113194234667, + "y": 5.146461372900032, + "heading": -0.22910323394931606, + "angularVelocity": 1.0033856610117322, + "velocityX": -1.5080487970369372, + "velocityY": 1.7421685266957554, + "timestamp": 1.9621217960011388 + }, + { + "x": 2.05813446327852, + "y": 5.259759383516201, + "heading": -0.1637433708814866, + "angularVelocity": 0.861431444990317, + "velocityX": -1.2926356320754544, + "velocityY": 1.49324775815898, + "timestamp": 2.037995347457766 + }, + { + "x": 1.9764036449833426, + "y": 5.354173277082403, + "heading": -0.10920518207306891, + "angularVelocity": 0.7188036906324902, + "velocityX": -1.077197741849454, + "velocityY": 1.2443584325978703, + "timestamp": 2.1138688989143932 + }, + { + "x": 1.9110189705233616, + "y": 5.429703862947866, + "heading": -0.06553401357536529, + "angularVelocity": 0.5755782833319475, + "velocityX": -0.8617584547542609, + "velocityY": 0.9954797741165524, + "timestamp": 2.1897424503710203 + }, + { + "x": 1.8619803278190585, + "y": 5.486351581956986, + "heading": -0.032764600292238745, + "angularVelocity": 0.43189507613676265, + "velocityX": -0.6463206448473131, + "velocityY": 0.7466069258864354, + "timestamp": 2.2656160018276474 + }, + { + "x": 1.8292876878125428, + "y": 5.524116686024045, + "heading": -0.010918035230093376, + "angularVelocity": 0.287933866844679, + "velocityX": -0.43088321791822976, + "velocityY": 0.4977373978420955, + "timestamp": 2.3414895532842745 }, { - "x": 1.8129411935806279, + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.264631493815946e-17, - "angularVelocity": 0.12846142943609742, - "velocityX": -0.19087328181241156, - "velocityY": 0.22232623455065786, - "timestamp": 2.5600392879488316 + "heading": 2.4600717676583652e-21, + "angularVelocity": 0.1438977749227231, + "velocityX": -0.21544390526203344, + "velocityY": 0.24886908799668503, + "timestamp": 2.4173631047409017 }, { - "x": 1.8129411935806274, + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 6.021257273210334e-18, - "angularVelocity": -5.990893004521945e-18, - "velocityX": -3.48414823730851e-16, - "velocityY": -1.72070697740397e-16, - "timestamp": 2.635518672715027 - }, - { - "x": 1.8467557558007979, - "y": 5.543052711491933, - "heading": -1.0880372931867694e-19, - "angularVelocity": -6.586811222708636e-17, - "velocityX": 0.3633408180161408, - "velocityY": 0.000574260143737945, - "timestamp": 2.7285843579986113 - }, - { - "x": 1.914384879084952, - "y": 5.543159599317721, - "heading": -3.505509945985968e-18, - "angularVelocity": -3.6497944505922797e-17, - "velocityX": 0.7266816236089445, - "velocityY": 0.0011485202678409793, - "timestamp": 2.8216500432821956 - }, - { - "x": 2.015828561351954, - "y": 5.543319931052201, - "heading": -4.327530361987157e-18, - "angularVelocity": -8.832690735719102e-18, - "velocityX": 1.090022406839742, - "velocityY": 0.0017227803566008666, - "timestamp": 2.91471572856578 - }, - { - "x": 2.1510867977458235, - "y": 5.543533706687698, - "heading": -2.821606902688151e-18, - "angularVelocity": 1.6181296623948357e-17, - "velocityX": 1.453363137892531, - "velocityY": 0.0022970403628934204, - "timestamp": 3.007781413849364 - }, - { - "x": 2.3201595639866595, - "y": 5.5438009261858365, - "heading": -4.181254623727939e-19, - "angularVelocity": 2.582564597014347e-17, - "velocityX": 1.8167036080554047, - "velocityY": 0.002871299956849509, - "timestamp": 3.1008470991329484 - }, - { - "x": 2.4554178003805287, - "y": 5.544014701821333, - "heading": 5.073319506573115e-18, - "angularVelocity": 5.900611973144966e-17, - "velocityX": 1.4533631378925314, - "velocityY": 0.0022970403628935166, - "timestamp": 3.1939127844165327 - }, - { - "x": 2.556861482647531, - "y": 5.544175033555812, - "heading": 6.802500422384327e-18, - "angularVelocity": 1.8580220092148724e-17, - "velocityX": 1.0900224068397424, - "velocityY": 0.001722780356600995, - "timestamp": 3.286978469700117 - }, - { - "x": 2.6244906059316855, - "y": 5.544281921381602, - "heading": 5.0686461896360996e-18, - "angularVelocity": -1.8630435349857743e-17, - "velocityX": 0.726681623608945, - "velocityY": 0.0011485202678411268, - "timestamp": 3.3800441549837013 - }, - { - "x": 2.6583051681518555, + "heading": 1.1389483058862038e-21, + "angularVelocity": -1.912999393800189e-21, + "velocityX": -4.5659699774939935e-20, + "velocityY": -6.062533120754166e-20, + "timestamp": 2.4932366561975288 + }, + { + "x": 1.8507557561824508, + "y": 5.5430527114919315, + "heading": -3.663099117513338e-19, + "angularVelocity": -3.946860070844576e-18, + "velocityX": 0.4061756707874381, + "velocityY": 0.0005740544395093343, + "timestamp": 2.586335690239902 + }, + { + "x": 1.9263848800936068, + "y": 5.543159599317717, + "heading": -1.3979636927804314e-18, + "angularVelocity": -1.108125118245985e-17, + "velocityX": 0.8123513276919049, + "velocityY": 0.0011481088593976478, + "timestamp": 2.6794347242822756 + }, + { + "x": 2.039828562987611, + "y": 5.543319931052194, + "heading": -3.639885752731889e-18, + "angularVelocity": -2.408104534113515e-17, + "velocityX": 1.2185269596070252, + "velocityY": 0.0017221632439681264, + "timestamp": 2.772533758324649 + }, + { + "x": 2.1910867994360017, + "y": 5.543533706687691, + "heading": -8.31799511662456e-18, + "angularVelocity": -5.0248742235094303e-17, + "velocityX": 1.6247025332136762, + "velocityY": 0.002296217546130332, + "timestamp": 2.8656327923670224 + }, + { + "x": 2.380159562296481, + "y": 5.5438009261858445, + "heading": -4.964025869598509e-18, + "angularVelocity": 3.602582219554166e-17, + "velocityX": 2.0308778152781284, + "velocityY": 0.0028702714362513803, + "timestamp": 2.958731826409396 + }, + { + "x": 2.5314177987448714, + "y": 5.544014701821341, + "heading": -1.164993974430083e-17, + "angularVelocity": -7.181507244960338e-17, + "velocityX": 1.6247025332136764, + "velocityY": 0.0022962175461303325, + "timestamp": 3.051830860451769 + }, + { + "x": 2.6448614816388756, + "y": 5.544175033555818, + "heading": -5.397697309971077e-18, + "angularVelocity": 6.71568990871082e-17, + "velocityX": 1.218526959607025, + "velocityY": 0.0017221632439681266, + "timestamp": 3.1449298944941426 + }, + { + "x": 2.7204906055500317, + "y": 5.544281921381604, + "heading": -1.7112683408390196e-18, + "angularVelocity": 3.959685518772811e-17, + "velocityX": 0.8123513276919049, + "velocityY": 0.001148108859397648, + "timestamp": 3.238028928536516 + }, + { + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -4.658926801043138e-23, - "angularVelocity": -5.446360561015685e-17, - "velocityX": 0.36334081801614126, - "velocityY": 0.0005742601437381063, - "timestamp": 3.4731098402672855 + "heading": 2.8361562565882696e-27, + "angularVelocity": 1.838116110697215e-17, + "velocityX": 0.4061756707874381, + "velocityY": 0.0005740544395093341, + "timestamp": 3.3311279625788894 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -4.8718739199241653e-23, - "angularVelocity": -2.2883735459903696e-23, - "velocityX": 1.6624416710559118e-21, - "velocityY": 9.919860077979884e-23, - "timestamp": 3.56617552555087 - }, - { - "x": 2.6433995634924674, - "y": 5.563703580385797, - "heading": 0.004845900450156495, - "angularVelocity": 0.061212191042318315, - "velocityX": -0.18828383484070044, - "velocityY": 0.24465440313021325, - "timestamp": 3.6453411356628544 - }, - { - "x": 2.613808012454941, - "y": 5.602604430475709, - "heading": 0.014722463610686642, - "angularVelocity": 0.12475825230878822, - "velocityX": -0.37379300172975427, - "velocityY": 0.4913857170415791, - "timestamp": 3.724506745774839 - }, - { - "x": 2.5698581719130473, - "y": 5.6612759426030985, - "heading": 0.02990473352989547, - "angularVelocity": 0.19177860055310075, - "velocityX": -0.5551632897128445, - "velocityY": 0.7411237284017034, - "timestamp": 3.8036723558868237 - }, - { - "x": 2.512090436903312, - "y": 5.74009257508762, - "heading": 0.05083960434186738, - "angularVelocity": 0.2644440026718447, - "velocityX": -0.7297074440280276, - "velocityY": 0.9955918027162334, - "timestamp": 3.8828379659988084 - }, - { - "x": 2.4415596148175696, - "y": 5.839724514645118, - "heading": 0.07837099812745095, - "angularVelocity": 0.34776961545093493, - "velocityX": -0.8909275376766785, - "velocityY": 1.2585255064233432, - "timestamp": 3.962003576110793 - }, - { - "x": 2.3611631890754614, - "y": 5.961650149114452, - "heading": 0.11464704720334487, - "angularVelocity": 0.4582298933157867, - "velocityX": -1.0155473522958047, - "velocityY": 1.5401338320624671, - "timestamp": 4.041169186222778 - }, - { - "x": 2.2902542502823713, - "y": 6.1057913956854275, - "heading": 0.1670437133636231, - "angularVelocity": 0.6618614583549578, - "velocityX": -0.8957038124608023, - "velocityY": 1.8207558353567832, - "timestamp": 4.120334796334762 - }, - { - "x": 2.241416295543653, - "y": 6.239392989227262, - "heading": 0.22223140963064505, - "angularVelocity": 0.6971170460122205, - "velocityX": -0.6169087141453696, - "velocityY": 1.6876215992379322, - "timestamp": 4.199500406446747 - }, - { - "x": 2.2118788438375976, - "y": 6.357956546172331, - "heading": 0.2767017364027319, - "angularVelocity": 0.688055415666413, - "velocityX": -0.3731096326330706, - "velocityY": 1.4976649175993666, - "timestamp": 4.278666016558732 - }, - { - "x": 2.2005734730100426, - "y": 6.460208388316336, - "heading": 0.32930652128307913, - "angularVelocity": 0.664490361483155, - "velocityX": -0.14280659002770119, - "velocityY": 1.2916194544495236, - "timestamp": 4.357831626670716 - }, - { - "x": 2.206951813042022, - "y": 6.545559671097223, - "heading": 0.37948004279494146, - "angularVelocity": 0.6337792564333017, - "velocityX": 0.08056958094502178, - "velocityY": 1.0781358554573413, - "timestamp": 4.436997236782701 - }, - { - "x": 2.2306819541123337, - "y": 6.613672661059128, - "heading": 0.4268844504712619, - "angularVelocity": 0.598800509580661, - "velocityX": 0.29975315085356163, - "velocityY": 0.8603860927182078, - "timestamp": 4.5161628468946855 - }, - { - "x": 2.271541794524346, - "y": 6.6643287076043505, - "heading": 0.4712947460181937, - "angularVelocity": 0.5609796410854498, - "velocityX": 0.516131188204235, - "velocityY": 0.6398743908316679, - "timestamp": 4.59532845700667 + "heading": 2.9956768552498905e-27, + "angularVelocity": 1.2964344677835282e-27, + "velocityX": -1.3759431197263344e-22, + "velocityY": -2.5705739302596115e-25, + "timestamp": 3.424226996621263 + }, + { + "x": 2.7425045239965504, + "y": 5.564231116573788, + "heading": 0.0035475080989150498, + "angularVelocity": 0.04647392993401548, + "velocityX": -0.20699544832908057, + "velocityY": 0.26064316841975155, + "timestamp": 3.5005602893716685 + }, + { + "x": 2.7110743119163434, + "y": 5.604155225406367, + "heading": 0.01083868300587181, + "angularVelocity": 0.09551762598264181, + "velocityX": -0.4117497221425694, + "velocityY": 0.523023537883034, + "timestamp": 3.576893582122074 + }, + { + "x": 2.6642693453223805, + "y": 5.6643007250236845, + "heading": 0.022164936239812177, + "angularVelocity": 0.14837894221299866, + "velocityX": -0.6131658272230635, + "velocityY": 0.7879327283048184, + "timestamp": 3.65322687487248 + }, + { + "x": 2.60250917282708, + "y": 5.744974159519079, + "heading": 0.038001263541227126, + "angularVelocity": 0.20746291337380784, + "velocityX": -0.8090856593497568, + "velocityY": 1.0568577823464311, + "timestamp": 3.7295601676228856 + }, + { + "x": 2.526611736042362, + "y": 5.846734963341242, + "heading": 0.05925383993510262, + "angularVelocity": 0.27841817938297336, + "velocityX": -0.994290093483679, + "velocityY": 1.333111675856818, + "timestamp": 3.8058934603732912 + }, + { + "x": 2.4388385302471143, + "y": 5.970894906525885, + "heading": 0.08830658815011594, + "angularVelocity": 0.3806038907558967, + "velocityX": -1.149867936160551, + "velocityY": 1.6265503387967306, + "timestamp": 3.882226753123697 + }, + { + "x": 2.357791815962569, + "y": 6.119054185868648, + "heading": 0.1383597950882533, + "angularVelocity": 0.655719216800998, + "velocityX": -1.0617479131884362, + "velocityY": 1.9409522896806517, + "timestamp": 3.9585600458741026 + }, + { + "x": 2.2985967230184703, + "y": 6.254273676601559, + "heading": 0.19308840139911443, + "angularVelocity": 0.7169690228065029, + "velocityX": -0.7754819792414106, + "velocityY": 1.7714353182044804, + "timestamp": 4.034893338624508 + }, + { + "x": 2.2587354140798723, + "y": 6.372988574603886, + "heading": 0.24867936598579424, + "angularVelocity": 0.7282662988016366, + "velocityX": -0.5222008314109583, + "velocityY": 1.5552178312351754, + "timestamp": 4.111226631374914 + }, + { + "x": 2.2373189281980324, + "y": 6.474209987526736, + "heading": 0.30392383910912424, + "angularVelocity": 0.7237271069121585, + "velocityX": -0.2805654663931197, + "velocityY": 1.3260454157771489, + "timestamp": 4.1875599241253205 + }, + { + "x": 2.233901000873839, + "y": 6.557480302062636, + "heading": 0.35823088728392755, + "angularVelocity": 0.7114464242015108, + "velocityX": -0.044776364297153176, + "velocityY": 1.090878062972822, + "timestamp": 4.263893216875727 + }, + { + "x": 2.248214127214941, + "y": 6.6225364653992, + "heading": 0.41124898234582946, + "angularVelocity": 0.6945605665834471, + "velocityX": 0.1875083050315006, + "velocityY": 0.8522646016238898, + "timestamp": 4.340226509626133 + }, + { + "x": 2.280080256208058, + "y": 6.669207835860357, + "heading": 0.4627444117016496, + "angularVelocity": 0.6746129703090314, + "velocityX": 0.41746042709454745, + "velocityY": 0.6114156586138921, + "timestamp": 4.416559802376539 }, { "x": 2.3293724060058594, "y": 6.6973748207092285, "heading": 0.5125504196, - "angularVelocity": 0.5211312528691123, - "velocityX": 0.7305016837451087, - "velocityY": 0.4174301575915679, - "timestamp": 4.674494067118655 - }, - { - "x": 2.3973241454559178, - "y": 6.73620444415628, - "heading": 0.5391342040901619, - "angularVelocity": 0.37821212966035134, - "velocityX": 0.9667612262295853, - "velocityY": 0.5524358122619254, - "timestamp": 4.744782094406503 + "angularVelocity": 0.652480799710893, + "velocityX": 0.6457490306225382, + "velocityY": 0.3689999976939454, + "timestamp": 4.492893095126945 }, { - "x": 2.48097335716082, - "y": 6.784004065158346, - "heading": 0.5372874666531788, - "angularVelocity": -0.02627385499695578, - "velocityX": 1.1900918966232588, - "velocityY": 0.6800535289789074, - "timestamp": 4.815070121694351 + "x": 2.4053669705551046, + "y": 6.740800351083451, + "heading": 0.5568713225103131, + "angularVelocity": 0.5531681306272371, + "velocityX": 0.948486345023332, + "velocityY": 0.5419930073900953, + "timestamp": 4.573015033569164 }, { - "x": 2.5479307987525304, - "y": 6.822265517551258, - "heading": 0.5291862310389086, - "angularVelocity": -0.11525768935146673, - "velocityX": 0.952615177510983, - "velocityY": 0.5443523437671909, - "timestamp": 4.885358148982199 + "x": 2.502567960679766, + "y": 6.796343857044693, + "heading": 0.5404821418694866, + "angularVelocity": -0.204552972125677, + "velocityX": 1.213163236118634, + "velocityY": 0.6932371712561282, + "timestamp": 4.653136972011383 }, { - "x": 2.5981417297757714, - "y": 6.850957521028096, - "heading": 0.5214396908605601, - "angularVelocity": -0.11021137563904544, - "velocityX": 0.7143596564122449, - "velocityY": 0.4082061281836414, - "timestamp": 4.955646176270047 + "x": 2.575459179910046, + "y": 6.837996044585651, + "heading": 0.5268532537172577, + "angularVelocity": -0.17010182750455535, + "velocityX": 0.9097535662201399, + "velocityY": 0.5198599578440831, + "timestamp": 4.7332589104536025 }, { - "x": 2.6316113826745062, - "y": 6.870083065561309, - "heading": 0.5156519889527618, - "angularVelocity": -0.08234264256835937, - "velocityX": 0.4761785782046117, - "velocityY": 0.27210245145860107, - "timestamp": 5.025934203557895 + "x": 2.6240500842159764, + "y": 6.865762317125851, + "heading": 0.5173922106041889, + "angularVelocity": -0.11808305311898519, + "velocityX": 0.6064619160577619, + "velocityY": 0.34655018438206425, + "timestamp": 4.813380848895822 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.044126567104521494, - "velocityX": 0.23806521146364915, - "velocityY": 0.13603746705807154, - "timestamp": 5.096222230845743 + "angularVelocity": -0.06043027787801988, + "velocityX": 0.30321823224102623, + "velocityY": 0.17326782030138713, + "timestamp": 4.893502787338041 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": 5.036255157215061e-18, - "velocityX": -7.194949871952118e-18, - "velocityY": -3.0975432357907215e-18, - "timestamp": 5.1665102581335915 - }, - { - "x": 2.654709594748088, - "y": 6.868188525457388, - "heading": 0.5112832103988345, - "angularVelocity": -0.02186863812315321, - "velocityX": 0.10984420500359952, - "velocityY": -0.19770584790906034, - "timestamp": 5.224456674093623 - }, - { - "x": 2.6675869589575, - "y": 6.845358903138629, - "heading": 0.5087434985964555, - "angularVelocity": -0.0438286261591562, - "velocityX": 0.2222288297915463, - "velocityY": -0.393978159658854, - "timestamp": 5.282403090053654 - }, - { - "x": 2.6871400637397453, - "y": 6.811251205912889, - "heading": 0.5049256931937168, - "angularVelocity": -0.06588509987168945, - "velocityX": 0.3374342391724767, - "velocityY": -0.5886075378547303, - "timestamp": 5.340349506013685 - }, - { - "x": 2.7135513143277277, - "y": 6.765975453621827, - "heading": 0.4998239367817842, - "angularVelocity": -0.08804265677871101, - "velocityX": 0.45578747452127555, - "velocityY": -0.7813382681388139, - "timestamp": 5.398295921973716 - }, - { - "x": 2.747025360653779, - "y": 6.709659972264173, - "heading": 0.49343216509494003, - "angularVelocity": -0.1103048666762139, - "velocityX": 0.5776724197945273, - "velocityY": -0.9718544352509818, - "timestamp": 5.456242337933747 - }, - { - "x": 2.7877931285642683, - "y": 6.642455993729292, - "heading": 0.4857442111344985, - "angularVelocity": -0.1326735024601774, - "velocityX": 0.703542526920187, - "velocityY": -1.159760744844629, - "timestamp": 5.514188753893778 - }, - { - "x": 2.836116770448945, - "y": 6.564543815779258, - "heading": 0.4767539799591755, - "angularVelocity": -0.15514732061277248, - "velocityX": 0.8339366824344734, - "velocityY": -1.3445555977745738, - "timestamp": 5.5721351698538095 - }, - { - "x": 2.8922957481362634, - "y": 6.4761411900512496, - "heading": 0.4664557366916879, - "angularVelocity": -0.17772010739333818, - "velocityX": 0.969498747361149, - "velocityY": -1.5255926404315645, - "timestamp": 5.630081585813841 - }, - { - "x": 2.9566742703700464, - "y": 6.377514946767211, - "heading": 0.4548445780033014, - "angularVelocity": -0.2003775125002176, - "velocityX": 1.111000933658922, - "velocityY": -1.7020249078401652, - "timestamp": 5.688028001773872 - }, - { - "x": 3.0296502478902108, - "y": 6.268997399320861, - "heading": 0.44191720438373994, - "angularVelocity": -0.2230918583207183, - "velocityX": 1.2593699940044558, - "velocityY": -1.8727223357731155, - "timestamp": 5.745974417733903 - }, - { - "x": 3.1116856852367225, - "y": 6.1510099051267035, - "heading": 0.4276731903359535, - "angularVelocity": -0.24581354708112682, - "velocityX": 1.4157120157888006, - "velocityY": -2.036148262828567, - "timestamp": 5.803920833693934 - }, - { - "x": 3.2033177251351583, - "y": 6.024097216910698, - "heading": 0.41211708268393216, - "angularVelocity": -0.26845676983265077, - "velocityX": 1.5813236829287303, - "velocityY": -2.1901732162959853, - "timestamp": 5.861867249653965 - }, - { - "x": 3.3051677702115847, - "y": 5.888977978331293, - "heading": 0.3952618666988256, - "angularVelocity": -0.2908759015695001, - "velocityX": 1.7576590957183351, - "velocityY": -2.3317963042374306, - "timestamp": 5.919813665613996 - }, - { - "x": 3.417941892615677, - "y": 5.746618387863477, - "heading": 0.37713462149882643, - "angularVelocity": -0.31282772022506217, - "velocityX": 1.9461794234501006, - "velocityY": -2.456745393295919, - "timestamp": 5.9777600815740275 - }, - { - "x": 3.542406775687406, - "y": 5.598335086950221, - "heading": 0.35778539162317, - "angularVelocity": -0.3339159041172737, - "velocityX": 2.147930652994672, - "velocityY": -2.5589727760822365, - "timestamp": 6.035706497534059 - }, - { - "x": 3.679310043544789, - "y": 5.445921240793454, - "heading": 0.337299824049247, - "angularVelocity": -0.3535260504816983, - "velocityX": 2.3625838732082087, - "velocityY": -2.6302549283098875, - "timestamp": 6.09365291349409 - }, - { - "x": 3.8292016626883134, - "y": 5.291748474573123, - "heading": 0.3158135059859917, - "angularVelocity": -0.3707963246264704, - "velocityX": 2.5867280427993067, - "velocityY": -2.660609179464564, - "timestamp": 6.151599329454121 - }, - { - "x": 3.9921508426109233, - "y": 5.138721146636673, - "heading": 0.29352014457634995, - "angularVelocity": -0.3847237320945793, - "velocityX": 2.8120665829446008, - "velocityY": -2.640841981357415, - "timestamp": 6.209545745414152 + "angularVelocity": 1.1533672139704627e-20, + "velocityX": 1.3759845110959627e-18, + "velocityY": -2.227992646788969e-18, + "timestamp": 4.97362472578026 + }, + { + "x": 2.6571120181256966, + "y": 6.867792271481583, + "heading": 0.507044677895467, + "angularVelocity": -0.09452799306194601, + "velocityX": 0.15052909368010856, + "velocityY": -0.20349709127294008, + "timestamp": 5.031869289387202 + }, + { + "x": 2.6746521578330125, + "y": 6.844087252656105, + "heading": 0.49617547030513726, + "angularVelocity": -0.18661325482116445, + "velocityX": 0.30114638381847736, + "velocityY": -0.40699109680773127, + "timestamp": 5.090113852994143 + }, + { + "x": 2.7009708210230947, + "y": 6.808530072252695, + "heading": 0.4801087501886783, + "angularVelocity": -0.27584926594838455, + "velocityX": 0.45186471595342564, + "velocityY": -0.6104806732412771, + "timestamp": 5.148358416601085 + }, + { + "x": 2.73607484037394, + "y": 6.7611211343580875, + "heading": 0.4590398225372461, + "angularVelocity": -0.361732088742461, + "velocityX": 0.602700358229852, + "velocityY": -0.8139633119159942, + "timestamp": 5.206602980208027 + }, + { + "x": 2.7799722580023465, + "y": 6.701861088549211, + "heading": 0.4332010241003249, + "angularVelocity": -0.4436259255248601, + "velocityX": 0.7536740754835817, + "velocityY": -1.0174347980145875, + "timestamp": 5.264847543814969 + }, + { + "x": 2.832672676333526, + "y": 6.630750980608444, + "heading": 0.40287238280523807, + "angularVelocity": -0.5207119672104833, + "velocityX": 0.9048126566253185, + "velocityY": -1.2208883290919084, + "timestamp": 5.32309210742191 + }, + { + "x": 2.8941877403139116, + "y": 6.547792490679094, + "heading": 0.3683969681281308, + "angularVelocity": -0.5919078544353333, + "velocityX": 1.0561511696699046, + "velocityY": -1.4243130138151228, + "timestamp": 5.381336671028852 + }, + { + "x": 2.9645318213846927, + "y": 6.452988329970027, + "heading": 0.33020407419243936, + "angularVelocity": -0.6557331975810253, + "velocityX": 1.2077364257631285, + "velocityY": -1.627691149835798, + "timestamp": 5.439581234635794 + }, + { + "x": 3.04372302387486, + "y": 6.346342948824998, + "heading": 0.28884632079593436, + "angularVelocity": -0.7100706200771626, + "velocityX": 1.3596325147971153, + "velocityY": -1.8309928779742386, + "timestamp": 5.497825798242736 + }, + { + "x": 3.1317847243622103, + "y": 6.227863919029491, + "heading": 0.24506332676078993, + "angularVelocity": -0.7517095386036364, + "velocityX": 1.5119299559290544, + "velocityY": -2.0341646062463727, + "timestamp": 5.556070361849677 + }, + { + "x": 3.2287480012560774, + "y": 6.097564965294096, + "heading": 0.199900944291634, + "angularVelocity": -0.7753922370151164, + "velocityX": 1.6647609817838902, + "velocityY": -2.237100695177408, + "timestamp": 5.614314925456619 + }, + { + "x": 3.334655409720646, + "y": 5.955473736284568, + "heading": 0.1549620363959118, + "angularVelocity": -0.7715554055652005, + "velocityX": 1.8183226365858802, + "velocityY": -2.4395620845993164, + "timestamp": 5.672559489063561 + }, + { + "x": 3.4495650121656865, + "y": 5.8016567423537415, + "heading": 0.11303057394210353, + "angularVelocity": -0.7199206218932102, + "velocityX": 1.972881163991502, + "velocityY": -2.6408815588154475, + "timestamp": 5.730804052670503 + }, + { + "x": 3.5735262627096924, + "y": 5.636333431818966, + "heading": 0.08010242298971296, + "angularVelocity": -0.565342907787981, + "velocityX": 2.1282887683826965, + "velocityY": -2.838433328309314, + "timestamp": 5.789048616277444 + }, + { + "x": 3.7050107789119795, + "y": 5.46057947422046, + "heading": 0.07776405618458973, + "angularVelocity": -0.04014738303996017, + "velocityX": 2.2574555985962665, + "velocityY": -3.017516944320591, + "timestamp": 5.847293179884386 + }, + { + "x": 3.8485069507845076, + "y": 5.2934525910716275, + "heading": 0.07776403563237232, + "angularVelocity": -3.5286069899218235e-7, + "velocityX": 2.463683526601701, + "velocityY": -2.8693988382619477, + "timestamp": 5.905537743491328 + }, + { + "x": 4.002902013863093, + "y": 5.136338903873299, + "heading": 0.07776401516857159, + "angularVelocity": -3.5134267388587325e-7, + "velocityX": 2.650806418956214, + "velocityY": -2.697482433873059, + "timestamp": 5.96378230709827 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.2706621692829818, - "angularVelocity": -0.3944674560916278, - "velocityX": 3.026040866948455, - "velocityY": -2.5674297245815976, - "timestamp": 6.267492161374183 - }, - { - "x": 4.262820189871526, - "y": 4.913621110649205, - "heading": 0.2585516106232336, - "angularVelocity": -0.39720693073137303, - "velocityX": 3.1263802397054365, - "velocityY": -2.5033930490319927, - "timestamp": 6.297981454794659 - }, - { - "x": 4.3609649027366135, - "y": 4.839574515326335, - "heading": 0.24639337135429903, - "angularVelocity": -0.3987707783588398, - "velocityX": 3.218989417418724, - "velocityY": -2.4286097516822993, - "timestamp": 6.3284707482151346 - }, - { - "x": 4.461671433105771, - "y": 4.768098656214637, - "heading": 0.234223352498736, - "angularVelocity": -0.3991571299366988, - "velocityX": 3.3030129291721897, - "velocityY": -2.344293720617927, - "timestamp": 6.35896004163561 - }, - { - "x": 4.564664131810475, - "y": 4.699441485698375, - "heading": 0.22207645249799987, - "angularVelocity": -0.3983988685196772, - "velocityX": 3.3779955896103364, - "velocityY": -2.251845248409488, - "timestamp": 6.389449335056086 - }, - { - "x": 4.669665105994921, - "y": 4.6338072685219265, - "heading": 0.20998565945136316, - "angularVelocity": -0.39655865027233667, - "velocityX": 3.4438638093832386, - "velocityY": -2.1526972196859626, - "timestamp": 6.419938628476562 - }, - { - "x": 4.7764036789009445, - "y": 4.571359258447394, - "heading": 0.19798140822304883, - "angularVelocity": -0.39372021721567657, - "velocityX": 3.500854264938127, - "velocityY": -2.0481947289927476, - "timestamp": 6.450427921897037 - }, - { - "x": 4.88462301845396, - "y": 4.512224581779958, - "heading": 0.1860912294102901, - "angularVelocity": -0.3899788246561105, - "velocityX": 3.5494210397259125, - "velocityY": -1.9395226990639944, - "timestamp": 6.480917215317513 - }, - { - "x": 4.994084052884085, - "y": 4.456499991660438, - "heading": 0.17433965070276516, - "angularVelocity": -0.3854329631526573, - "velocityX": 3.590146643300487, - "velocityY": -1.827677321052551, - "timestamp": 6.511406508737989 - }, - { - "x": 5.1045672063720575, - "y": 4.404257556871473, - "heading": 0.162748286386498, - "angularVelocity": -0.380178187679756, - "velocityX": 3.6236705116221697, - "velocityY": -1.71346820237825, - "timestamp": 6.541895802158464 - }, - { - "x": 5.215872594429427, - "y": 4.35554976560558, - "heading": 0.15133604569057543, - "angularVelocity": -0.3743032197731828, - "velocityX": 3.650638488808694, - "velocityY": -1.5975375550415924, - "timestamp": 6.57238509557894 - }, - { - "x": 5.327819247886724, - "y": 4.310413837972956, - "heading": 0.14011940090490202, - "angularVelocity": -0.36788798713571186, - "velocityX": 3.6716709670326204, - "velocityY": -1.4803861476931233, - "timestamp": 6.602874388999416 - }, - { - "x": 5.440243798322744, - "y": 4.268875232037166, - "heading": 0.1291126733064667, - "angularVelocity": -0.36100303954643975, - "velocityX": 3.6873452226518335, - "velocityY": -1.3623997566271735, - "timestamp": 6.633363682419891 - }, - { - "x": 5.552998921854952, - "y": 4.230950423566602, - "heading": 0.1183283109495842, - "angularVelocity": -0.3537098157093855, - "velocityX": 3.6981874908418817, - "velocityY": -1.243872986741426, - "timestamp": 6.663852975840367 + "heading": 0.07776399412255879, + "angularVelocity": -3.6133866407320583e-7, + "velocityX": 2.8259641989395843, + "velocityY": -2.513386639709188, + "timestamp": 6.022026870705211 + }, + { + "x": 4.268976588700125, + "y": 4.906724140265527, + "heading": 0.07776397443770015, + "angularVelocity": -5.672615420255969e-7, + "velocityX": 2.9242931040000055, + "velocityY": -2.398268643648094, + "timestamp": 6.056728427523216 + }, + { + "x": 4.37370366170879, + "y": 4.827628597038176, + "heading": 0.07776395550809555, + "angularVelocity": -5.454972726636976e-7, + "velocityX": 3.0179358683506776, + "velocityY": -2.2793082063198216, + "timestamp": 6.09142998434122 + }, + { + "x": 4.4815124411311835, + "y": 4.752787886123784, + "heading": 0.0777639371726549, + "angularVelocity": -5.283751607380448e-7, + "velocityX": 3.106741867167675, + "velocityY": -2.1566960614159445, + "timestamp": 6.126131541159224 + }, + { + "x": 4.592230128734662, + "y": 4.682321875197502, + "heading": 0.07776391928788347, + "angularVelocity": -5.153881575305611e-7, + "velocityX": 3.1905683132358433, + "velocityY": -2.030629671626745, + "timestamp": 6.160833097977228 + }, + { + "x": 4.705679213251417, + "y": 4.616343315466854, + "heading": 0.07776390172302422, + "angularVelocity": -5.061691995708879e-7, + "velocityX": 3.269279390309479, + "velocityY": -1.9013141132738893, + "timestamp": 6.195534654795233 + }, + { + "x": 4.821677528011508, + "y": 4.554957192131842, + "heading": 0.07776388435588834, + "angularVelocity": -5.004713755679868e-7, + "velocityX": 3.3427409429626933, + "velocityY": -1.7689731805681452, + "timestamp": 6.230236211613237 + }, + { + "x": 4.939562213059443, + "y": 4.4972764656411774, + "heading": 0.07776386706661859, + "angularVelocity": -4.982274959213974e-7, + "velocityX": 3.397100760239454, + "velocityY": -1.6621942004843482, + "timestamp": 6.264937768431241 + }, + { + "x": 5.0574486320523055, + "y": 4.439599283026335, + "heading": 0.07776384977740043, + "angularVelocity": -4.982260088149746e-7, + "velocityX": 3.397150727592071, + "velocityY": -1.6620920760798994, + "timestamp": 6.299639325249245 + }, + { + "x": 5.175337486723699, + "y": 4.381927078962276, + "heading": 0.07776383248820665, + "angularVelocity": -4.9822530674329e-7, + "velocityX": 3.3972209169079175, + "velocityY": -1.6619486084306254, + "timestamp": 6.3343408820672495 + }, + { + "x": 5.295110440251067, + "y": 4.328277244219036, + "heading": 0.07776381519347733, + "angularVelocity": -4.983848248451832e-7, + "velocityX": 3.451515277989629, + "velocityY": -1.5460353846547075, + "timestamp": 6.369042438885254 + }, + { + "x": 5.416934261342807, + "y": 4.279463089154562, + "heading": 0.07776379778914431, + "angularVelocity": -5.015432909319555e-7, + "velocityX": 3.5106154381100505, + "velocityY": -1.4066848735485782, + "timestamp": 6.403743995703258 + }, + { + "x": 5.540614064021598, + "y": 4.235563528216825, + "heading": 0.07776378015516597, + "angularVelocity": -5.081610154974924e-7, + "velocityX": 3.5640995396097326, + "velocityY": -1.26506027288553, + "timestamp": 6.438445552521262 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.10777714440346366, - "angularVelocity": -0.3460613665458462, - "velocityX": 3.704671190903868, - "velocityY": -1.1250293189517762, - "timestamp": 6.694342269260843 - }, - { - "x": 5.798356559097709, - "y": 4.161450057515513, - "heading": 0.09575175638596573, - "angularVelocity": -0.3367399103461797, - "velocityX": 3.7076550554500285, - "velocityY": -0.9856574960167961, - "timestamp": 6.730053474401752 - }, - { - "x": 5.9304803491986915, - "y": 4.131221114350942, - "heading": 0.0841228883191355, - "angularVelocity": -0.32563639398191835, - "velocityX": 3.699785251705895, - "velocityY": -0.8464834229282695, - "timestamp": 6.765764679542662 - }, - { - "x": 6.061872954800802, - "y": 4.105915842420486, - "heading": 0.07296106661592697, - "angularVelocity": -0.3125579677070111, - "velocityX": 3.679310319650609, - "velocityY": -0.7086087358465492, - "timestamp": 6.801475884683572 - }, - { - "x": 6.19201941031956, - "y": 4.085429275069175, - "heading": 0.062341869522057725, - "angularVelocity": -0.29736316800199314, - "velocityX": 3.6444151073934408, - "velocityY": -0.5736733686380603, - "timestamp": 6.837187089824481 - }, - { - "x": 6.320346294042347, - "y": 4.069574852509192, - "heading": 0.052342014729276326, - "angularVelocity": -0.28002008762580527, - "velocityX": 3.593462702152777, - "velocityY": -0.4439621261008996, - "timestamp": 6.872898294965391 - }, - { - "x": 6.446241767080791, - "y": 4.05806211105344, - "heading": 0.043033222480069355, - "angularVelocity": -0.2606686672285132, - "velocityX": 3.525377330215658, - "velocityY": -0.32238456838199153, - "timestamp": 6.908609500106301 - }, - { - "x": 6.569091568872717, - "y": 4.050483885589985, - "heading": 0.034475036219150636, - "angularVelocity": -0.2396498865593223, - "velocityX": 3.440091178866215, - "velocityY": -0.21220861725478038, - "timestamp": 6.94432070524721 - }, - { - "x": 6.688325744738062, - "y": 4.046323173396873, - "heading": 0.026709152612686058, - "angularVelocity": -0.21746349852429026, - "velocityX": 3.338844919819132, - "velocityY": -0.11650999109930767, - "timestamp": 6.98003191038812 - }, - { - "x": 6.803462305567311, - "y": 4.044983622354132, - "heading": 0.019757511591013074, - "angularVelocity": -0.19466273944659512, - "velocityX": 3.2241018015197396, - "velocityY": -0.03751066471868068, - "timestamp": 7.0157431155290295 - }, - { - "x": 6.914132827604236, - "y": 4.045835628751743, - "heading": 0.013624262542277448, - "angularVelocity": -0.17174578747848585, - "velocityX": 3.099041928163472, - "velocityY": 0.023858237050529835, - "timestamp": 7.051454320669939 - }, - { - "x": 7.020084299428127, - "y": 4.048262286463527, - "heading": 0.00829997269110531, - "angularVelocity": -0.1490929760045692, - "velocityX": 2.966897123909054, - "velocityY": 0.06795227722528199, - "timestamp": 7.087165525810849 - }, - { - "x": 7.1211630661166305, - "y": 4.051692576367301, - "heading": 0.003766298606880027, - "angularVelocity": -0.1269538248942407, - "velocityX": 2.8304496106940285, - "velocityY": 0.09605640275200633, - "timestamp": 7.122876730951758 + "heading": 0.07776376214569573, + "angularVelocity": -5.189816214982998e-7, + "velocityX": 3.6118744025389584, + "velocityY": -1.1214036841768227, + "timestamp": 6.4731471093392665 + }, + { + "x": 5.801257373802826, + "y": 4.160893829303833, + "heading": 0.07776374465549807, + "angularVelocity": -4.7264774236323325e-7, + "velocityX": 3.6564428162530316, + "velocityY": -0.9662347026048084, + "timestamp": 6.510151829987593 + }, + { + "x": 5.937965681779902, + "y": 4.1309456443700405, + "heading": 0.07776372758116286, + "angularVelocity": -4.614096500770092e-7, + "velocityX": 3.6943477908205034, + "velocityY": -0.8093071480907553, + "timestamp": 6.547156550635919 + }, + { + "x": 6.075827506190729, + "y": 4.1068588699437765, + "heading": 0.07776371078252527, + "angularVelocity": -4.53959313742904e-7, + "velocityX": 3.7255199335509808, + "velocityY": -0.6509108568923265, + "timestamp": 6.584161271284246 + }, + { + "x": 6.2145915217996235, + "y": 4.088676368882122, + "heading": 0.0777636941280861, + "angularVelocity": -4.5006255582880887e-7, + "velocityX": 3.7499003688646537, + "velocityY": -0.4913562578799624, + "timestamp": 6.621165991932572 + }, + { + "x": 6.354004262869319, + "y": 4.076423083568819, + "heading": 0.07776367749075697, + "angularVelocity": -4.496001822495745e-7, + "velocityX": 3.7674312527474547, + "velocityY": -0.33112762638453114, + "timestamp": 6.658170712580898 + }, + { + "x": 6.49381285233259, + "y": 4.070129387117659, + "heading": 0.07776362451928018, + "angularVelocity": -0.0000014314788993599032, + "velocityX": 3.778128493170848, + "velocityY": -0.17007820464237144, + "timestamp": 6.695175433229225 + }, + { + "x": 6.629142082966274, + "y": 4.065515785319137, + "heading": 0.06238171659626575, + "angularVelocity": -0.4156742073314349, + "velocityX": 3.657080184979125, + "velocityY": -0.12467603369760688, + "timestamp": 6.732180153877551 + }, + { + "x": 6.758586834582697, + "y": 4.061933354582955, + "heading": 0.046880370354106624, + "angularVelocity": -0.4189018582108937, + "velocityX": 3.4980605000804377, + "velocityY": -0.0968101008038334, + "timestamp": 6.769184874525878 + }, + { + "x": 6.882125546140559, + "y": 4.059204781585899, + "heading": 0.032519289448839375, + "angularVelocity": -0.3880878075461594, + "velocityX": 3.338458158674076, + "velocityY": -0.07373580854685112, + "timestamp": 6.806189595174204 + }, + { + "x": 6.999755813322696, + "y": 4.05726367776388, + "heading": 0.01976964076410941, + "angularVelocity": -0.34454114127480795, + "velocityX": 3.1787908440123913, + "velocityY": -0.0524555729109991, + "timestamp": 6.84319431582253 + }, + { + "x": 7.111477612575904, + "y": 4.056075301962013, + "heading": 0.008878948528675913, + "angularVelocity": -0.2943054843984055, + "velocityX": 3.0191228928587734, + "velocityY": -0.03211416762637815, + "timestamp": 6.880199036470857 }, { "x": 7.217291355133057, "y": 4.0556182861328125, - "heading": -1.0909016317355304e-17, - "angularVelocity": -0.10546545802684394, - "velocityX": 2.6918242786016986, - "velocityY": 0.10992935550682857, - "timestamp": 7.158587936092668 - }, - { - "x": 7.368999916586432, - "y": 4.062586724556833, - "heading": -0.004298989973913876, - "angularVelocity": -0.06943407043774014, - "velocityX": 2.450283207423163, - "velocityY": 0.11254900507105683, - "timestamp": 7.2205026408883315 - }, - { - "x": 7.505752960791366, - "y": 4.0691868405641705, - "heading": -0.006878875376785559, - "angularVelocity": -0.041668379287055086, - "velocityX": 2.208732879471196, - "velocityY": 0.1066001368999474, - "timestamp": 7.282417345683995 - }, - { - "x": 7.627563527820959, - "y": 4.075012053328196, - "heading": -0.008132420768696622, - "angularVelocity": -0.02024632752507115, - "velocityX": 1.9673931650260594, - "velocityY": 0.09408447933734543, - "timestamp": 7.344332050479658 - }, - { - "x": 7.734450186690855, - "y": 4.079741519123993, - "heading": -0.008370221734057366, - "angularVelocity": -0.0038407833186891597, - "velocityX": 1.726353363432043, - "velocityY": 0.07638679391923996, - "timestamp": 7.406246755275322 - }, - { - "x": 7.826433301006775, - "y": 4.083115946879114, - "heading": -0.007843874074281608, - "angularVelocity": 0.00850117369553605, - "velocityX": 1.485642459565828, - "velocityY": 0.05450123304726754, - "timestamp": 7.468161460070985 - }, - { - "x": 7.903533270640132, - "y": 4.084921624600937, - "heading": -0.00676122659439774, - "angularVelocity": 0.01748611228070863, - "velocityX": 1.2452610391636303, - "velocityY": 0.029163955925833308, - "timestamp": 7.5300761648666485 - }, - { - "x": 7.965769715964552, - "y": 4.0849794807879745, - "heading": -0.005296815188372533, - "angularVelocity": 0.023652077658421113, - "velocityX": 1.0051965123603401, - "velocityY": 0.000934449856933646, - "timestamp": 7.591990869662312 - }, - { - "x": 8.013161126878536, - "y": 4.083137358866124, - "heading": -0.0035992352155136144, - "angularVelocity": 0.027418041941110364, - "velocityX": 0.7654306205672696, - "velocityY": -0.029752575384646835, - "timestamp": 7.653905574457975 - }, - { - "x": 8.045724744989489, - "y": 4.079264414164358, - "heading": -0.001796495631167538, - "angularVelocity": 0.02911650132703844, - "velocityX": 0.5259432023203758, - "velocityY": -0.06255290588153085, - "timestamp": 7.715820279253639 + "heading": -2.4974876469169754e-20, + "angularVelocity": -0.23994096896600625, + "velocityX": 2.8594660546893573, + "velocityY": -0.012350203465742983, + "timestamp": 6.917203757119183 + }, + { + "x": 7.386543487491071, + "y": 4.0570668323942645, + "heading": -0.009095721053457144, + "angularVelocity": -0.13843657850499821, + "velocityX": 2.57601194788322, + "velocityY": 0.022046826970953105, + "timestamp": 6.982906919908118 + }, + { + "x": 7.537031363324535, + "y": 4.05969750834718, + "heading": -0.013766029362825559, + "angularVelocity": -0.07108194052044785, + "velocityX": 2.2904205740732744, + "velocityY": 0.04003880241453354, + "timestamp": 7.048610082697054 + }, + { + "x": 7.6687074557367385, + "y": 4.062834301345331, + "heading": -0.015395893103822788, + "angularVelocity": -0.02480647311048019, + "velocityX": 2.0041058424417098, + "velocityY": 0.04774188737652573, + "timestamp": 7.114313245485989 + }, + { + "x": 7.781556988474885, + "y": 4.066015749956383, + "heading": -0.01492426229398527, + "angularVelocity": 0.007178205581253095, + "velocityX": 1.7175662167232906, + "velocityY": 0.04842154435201139, + "timestamp": 7.180016408274924 + }, + { + "x": 7.8755781593330285, + "y": 4.068907471663171, + "heading": -0.013029646010250598, + "angularVelocity": 0.028835998197239424, + "velocityX": 1.4309991614890225, + "velocityY": 0.044011910295353764, + "timestamp": 7.2457195710638596 + }, + { + "x": 7.950774573438036, + "y": 4.07125628842811, + "heading": -0.010225311537090487, + "angularVelocity": 0.042681879442679815, + "velocityX": 1.144486976168374, + "velocityY": 0.035748914743771835, + "timestamp": 7.311422733852795 + }, + { + "x": 8.007152043467745, + "y": 4.072863968231118, + "heading": -0.006913103442788773, + "angularVelocity": 0.0504116994328239, + "velocityX": 0.8580632596152882, + "velocityY": 0.024468834296048715, + "timestamp": 7.37712589664173 + }, + { + "x": 8.044717148266278, + "y": 4.073571148367015, + "heading": -0.0034161675760999402, + "angularVelocity": 0.053223250118451255, + "velocityX": 0.5717396728557544, + "velocityY": 0.010763258660280774, + "timestamp": 7.442829059430665 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 2.0297812570188607e-18, - "angularVelocity": 0.029015653665741454, - "velocityX": 0.2867140781676597, - "velocityY": -0.09718948531913073, - "timestamp": 7.777734984049302 - }, - { - "x": 8.063706917248451, - "y": 4.062947102090978, - "heading": 0.001985154079980339, - "angularVelocity": 0.027021345733527664, - "velocityX": 0.003135522507827122, - "velocityY": -0.1401986439325952, - "timestamp": 7.851201128169473 - }, - { - "x": 8.043035079312682, - "y": 4.049980112254385, - "heading": 0.003821402596114743, - "angularVelocity": 0.024994486074167273, - "velocityX": -0.2813791057545805, - "velocityY": -0.17650293195437572, - "timestamp": 7.924667272289644 - }, - { - "x": 8.001396508446343, - "y": 4.034912807351801, - "heading": 0.0055060818689868455, - "angularVelocity": 0.022931369177568325, - "velocityX": -0.5667722372665807, - "velocityY": -0.2050918158701582, - "timestamp": 7.998133416409814 - }, - { - "x": 7.938735716407508, - "y": 4.018402810035911, - "heading": 0.007036218210445593, - "angularVelocity": 0.020827775294097833, - "velocityX": -0.8529206587504942, - "velocityY": -0.2247293295927428, - "timestamp": 8.071599560529984 - }, - { - "x": 7.855014011007738, - "y": 4.00121976392101, - "heading": 0.008408488071179075, - "angularVelocity": 0.018678942214372884, - "velocityX": -1.139595747162445, - "velocityY": -0.2338906760479261, - "timestamp": 8.145065704650154 - }, - { - "x": 7.750221962754556, - "y": 3.9842725768704743, - "heading": 0.00961918059388702, - "angularVelocity": 0.016479598013576947, - "velocityX": -1.4263991871108765, - "velocityY": -0.23068023037679872, - "timestamp": 8.218531848770324 - }, - { - "x": 7.62439956654467, - "y": 3.9686442308926377, - "heading": 0.010664172857875897, - "angularVelocity": 0.014224133803450148, - "velocityX": -1.7126582280413951, - "velocityY": -0.21272854544091283, - "timestamp": 8.291997992890494 - }, - { - "x": 7.477668976002092, - "y": 3.955635460155464, - "heading": 0.011538940075399833, - "angularVelocity": 0.01190707948538909, - "velocityX": -1.9972545490146683, - "velocityY": -0.17707164154273572, - "timestamp": 8.365464137010663 - }, - { - "x": 7.3102875993376655, - "y": 3.9468171160612413, - "heading": 0.012238647278142662, - "angularVelocity": 0.009524212970784032, - "velocityX": -2.2783471035397658, - "velocityY": -0.12003276066589603, - "timestamp": 8.438930281130833 - }, - { - "x": 7.122733099589339, - "y": 3.9440864335777195, - "heading": 0.012758416420278173, - "angularVelocity": 0.0070749479009718985, - "velocityX": -2.5529378463301264, - "velocityY": -0.037169263695877315, - "timestamp": 8.512396425251003 - }, - { - "x": 6.915834286392237, - "y": 3.9497111054839014, - "heading": 0.013093946664375935, - "angularVelocity": 0.004567141070435388, - "velocityX": -2.816247071013707, - "velocityY": 0.07656141442487652, - "timestamp": 8.585862569371173 - }, - { - "x": 6.690954998420113, - "y": 3.9663229006089162, - "heading": 0.01324277694660248, - "angularVelocity": 0.00202583494763381, - "velocityX": -3.060992116372438, - "velocityY": 0.22611497205899783, - "timestamp": 8.659328713491343 - }, - { - "x": 6.450203582137421, - "y": 3.9967943470851464, - "heading": 0.013206490081435995, - "angularVelocity": -0.0004939263602446482, - "velocityX": -3.277038956732079, - "velocityY": 0.4147685555184044, - "timestamp": 8.732794857611513 - }, - { - "x": 6.196564054073483, - "y": 4.043936461503449, - "heading": 0.012993660347291816, - "angularVelocity": -0.0028969770592021916, - "velocityX": -3.4524682233091277, - "velocityY": 0.6416848874114194, - "timestamp": 8.806261001731682 - }, - { - "x": 5.933782874794111, - "y": 4.110063969722605, - "heading": 0.012622011909817094, - "angularVelocity": -0.005058771518848182, - "velocityX": -3.576901747416247, - "velocityY": 0.9001086011944357, - "timestamp": 8.879727145851852 + "heading": 5.107015447929928e-20, + "angularVelocity": 0.05199395936347874, + "velocityX": 0.28551767430107416, + "velocityY": -0.004934199232898976, + "timestamp": 7.508532222219601 + }, + { + "x": 8.06095184830656, + "y": 4.071458641245482, + "heading": 0.0034408492569327057, + "angularVelocity": 0.04684199855212652, + "velocityX": -0.034370194612683154, + "velocityY": -0.02434521970346025, + "timestamp": 7.581988722012275 + }, + { + "x": 8.034920990250148, + "y": 4.068388525754522, + "heading": 0.006505362431754428, + "angularVelocity": 0.041718747605332625, + "velocityX": -0.3543710649143617, + "velocityY": -0.041795014731511364, + "timestamp": 7.655445221804949 + }, + { + "x": 7.985375296011398, + "y": 4.064207377475085, + "heading": 0.009196043333328236, + "angularVelocity": 0.036629582258453454, + "velocityX": -0.6744902680986578, + "velocityY": -0.05692005869104256, + "timestamp": 7.728901721597623 + }, + { + "x": 7.912305948559603, + "y": 4.059120814150924, + "heading": 0.011515917649017375, + "angularVelocity": 0.03158160710402538, + "velocityX": -0.9947295019232808, + "velocityY": -0.06924592566372595, + "timestamp": 7.802358221390297 + }, + { + "x": 7.815704588785296, + "y": 4.0533811177027905, + "heading": 0.013468719297185195, + "angularVelocity": 0.026584463644190544, + "velocityX": -1.3150825324778364, + "velocityY": -0.0781373529140697, + "timestamp": 7.875814721182971 + }, + { + "x": 7.695564554163692, + "y": 4.047305026722375, + "heading": 0.015059175300172275, + "angularVelocity": 0.021651671499132708, + "velocityX": -1.6355262633081036, + "velocityY": -0.08271685960487347, + "timestamp": 7.949271220975645 + }, + { + "x": 7.551883548681463, + "y": 4.04130179428642, + "heading": 0.016293463732843762, + "angularVelocity": 0.016802984571211377, + "velocityX": -1.9560012509139277, + "velocityY": -0.08172499986929728, + "timestamp": 8.02272772076832 + }, + { + "x": 7.384669657766205, + "y": 4.035920047959674, + "heading": 0.017179998132389497, + "angularVelocity": 0.012068835324959903, + "velocityX": -2.276366167557798, + "velocityY": -0.07326439922859854, + "timestamp": 8.096184220560993 + }, + { + "x": 7.19395595849013, + "y": 4.0319320028167684, + "heading": 0.017730886759136543, + "angularVelocity": 0.007499521870792628, + "velocityX": -2.5962807895060482, + "velocityY": -0.054291249299417726, + "timestamp": 8.169640720353666 + }, + { + "x": 6.979840325568109, + "y": 4.030500043249597, + "heading": 0.017964968086133067, + "angularVelocity": 0.00318666595409774, + "velocityX": -2.9148629941032773, + "velocityY": -0.01949398039945934, + "timestamp": 8.243097220146339 + }, + { + "x": 6.742614358141766, + "y": 4.033551118259164, + "heading": 0.017915208666011314, + "angularVelocity": -0.0006773998252331979, + "velocityX": -3.22947551402392, + "velocityY": 0.041535807153613755, + "timestamp": 8.316553719939012 + }, + { + "x": 6.483307205091183, + "y": 4.044763583343556, + "heading": 0.017651500017929662, + "angularVelocity": -0.003589997465520818, + "velocityX": -3.530077716505134, + "velocityY": 0.1526408842789682, + "timestamp": 8.390010219731685 + }, + { + "x": 6.206941447635851, + "y": 4.072199279636492, + "heading": 0.017375156748179146, + "angularVelocity": -0.0037619988773032, + "velocityX": -3.762305013652376, + "velocityY": 0.37349582910118817, + "timestamp": 8.463466719524359 + }, + { + "x": 5.9338077856619895, + "y": 4.122953167788701, + "heading": 0.017375155210986027, + "angularVelocity": -2.0926577278102936e-8, + "velocityX": -3.7183048844521998, + "velocityY": 0.690938014953861, + "timestamp": 8.536923219317032 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.012117456973340398, - "angularVelocity": -0.0068678565143064085, - "velocityX": -3.6456404399720617, - "velocityY": 1.1785715157475694, - "timestamp": 8.953193289972022 - }, - { - "x": 5.55330454345789, - "y": 4.2367101473374795, - "heading": 0.011887459641735761, - "angularVelocity": -0.007456725508590102, - "velocityX": -3.6521255907888213, - "velocityY": 1.2988169090424007, - "timestamp": 8.98403757025828 - }, - { - "x": 5.440517148405761, - "y": 4.28048284495712, - "heading": 0.011638752310585518, - "angularVelocity": -0.008063320941259012, - "velocityX": -3.6566713181625206, - "velocityY": 1.4191512077245991, - "timestamp": 9.014881850544539 - }, - { - "x": 5.327667581738716, - "y": 4.327969293579141, - "heading": 0.011370630767981954, - "angularVelocity": -0.008692747573138404, - "velocityX": -3.6586869792296546, - "velocityY": 1.5395544386611915, - "timestamp": 9.045726130830797 - }, - { - "x": 5.214861744178507, - "y": 4.379169755070302, - "heading": 0.011082156069971438, - "angularVelocity": -0.009352615633539172, - "velocityX": -3.6572692412752423, - "velocityY": 1.659966159559613, - "timestamp": 9.076570411117055 - }, - { - "x": 5.1022510887558585, - "y": 4.434079353427009, - "heading": 0.010772019648005773, - "angularVelocity": -0.010054908692547623, - "velocityX": -3.650941256451206, - "velocityY": 1.7802197959265815, - "timestamp": 9.107414691403314 - }, - { - "x": 4.990068460344294, - "y": 4.4926784730980485, - "heading": 0.010438291328946931, - "angularVelocity": -0.010819779743978382, - "velocityX": -3.637064226184596, - "velocityY": 1.8998374780411038, - "timestamp": 9.138258971689572 - }, - { - "x": 4.878710505114531, - "y": 4.5548991474962115, - "heading": 0.010077925553591442, - "angularVelocity": -0.011683390632267332, - "velocityX": -3.6103275614238326, - "velocityY": 2.0172516207448092, - "timestamp": 9.16910325197583 - }, - { - "x": 4.768951534707332, - "y": 4.620472273914983, - "heading": 0.009685974313613142, - "angularVelocity": -0.012707420511702754, - "velocityX": -3.558486999487417, - "velocityY": 2.1259412056368423, - "timestamp": 9.199947532262089 - }, - { - "x": 4.662298486421541, - "y": 4.688082271388773, - "heading": 0.009258221595846085, - "angularVelocity": -0.013868137424417113, - "velocityX": -3.4577901411855687, - "velocityY": 2.19197844288524, - "timestamp": 9.230791812548347 - }, - { - "x": 4.559169523977437, - "y": 4.754519038672898, - "heading": 0.008807731583591003, - "angularVelocity": -0.014605301471592353, - "velocityX": -3.3435360295972387, - "velocityY": 2.1539412386199186, - "timestamp": 9.261636092834605 - }, - { - "x": 4.45859769074999, - "y": 4.818261954197611, - "heading": 0.008349427916360263, - "angularVelocity": -0.014858627368748727, - "velocityX": -3.260631543160092, - "velocityY": 2.0666040813119846, - "timestamp": 9.292480373120863 - }, - { - "x": 4.359992719048881, - "y": 4.878854116841475, - "heading": 0.007890638381237635, - "angularVelocity": -0.014874379653690404, - "velocityX": -3.1968640793682743, - "velocityY": 1.964453768462782, - "timestamp": 9.323324653407122 - }, - { - "x": 4.263026916364364, - "y": 4.936113272994566, - "heading": 0.007435172084260176, - "angularVelocity": -0.014766637209570182, - "velocityX": -3.1437207088185697, - "velocityY": 1.856394625573387, - "timestamp": 9.35416893369338 + "heading": 0.017375154199310676, + "angularVelocity": -1.377244159549356e-8, + "velocityX": -3.646458211284153, + "velocityY": 1.003259166632557, + "timestamp": 8.610379719109705 + }, + { + "x": 5.541092339335072, + "y": 4.2364336029082, + "heading": 0.01737515315796098, + "angularVelocity": -3.005342133477009e-8, + "velocityX": -3.6034502673557895, + "velocityY": 1.148184128750311, + "timestamp": 8.645029674075602 + }, + { + "x": 5.417922638814364, + "y": 4.281176215218788, + "heading": 0.017375152214225534, + "angularVelocity": -2.7236267527590686e-8, + "velocityX": -3.5546857316822638, + "velocityY": 1.2912747608077488, + "timestamp": 8.679679629041498 + }, + { + "x": 5.296639420951804, + "y": 4.33080542124118, + "heading": 0.017375151348717865, + "angularVelocity": -2.4978608726563434e-8, + "velocityX": -3.500241716964063, + "velocityY": 1.4323021796489428, + "timestamp": 8.714329584007395 + }, + { + "x": 5.177436464453021, + "y": 4.385241922289634, + "heading": 0.017375150546348885, + "angularVelocity": -2.3156422113484916e-8, + "velocityX": -3.4402052359405113, + "velocityY": 1.571041033156597, + "timestamp": 8.748979538973291 + }, + { + "x": 5.0605042229676425, + "y": 4.444398737399711, + "heading": 0.017375149795171643, + "angularVelocity": -2.167902498222188e-8, + "velocityX": -3.374672249948487, + "velocityY": 1.7072696102577922, + "timestamp": 8.783629493939188 + }, + { + "x": 4.946029519855076, + "y": 4.5081813412028975, + "heading": 0.017375149085535665, + "angularVelocity": -2.0480141439242773e-8, + "velocityX": -3.3037475294046756, + "velocityY": 1.8407701789500717, + "timestamp": 8.818279448905084 + }, + { + "x": 4.834195247479412, + "y": 4.576487812332227, + "heading": 0.017375148409491465, + "angularVelocity": -1.9510680466596443e-8, + "velocityX": -3.2275445230947857, + "velocityY": 1.9713292902272803, + "timestamp": 8.85292940387098 + }, + { + "x": 4.725180068322848, + "y": 4.649208988191292, + "heading": 0.01737514776036044, + "angularVelocity": -1.8733964504925207e-8, + "velocityX": -3.146185305691103, + "velocityY": 2.0987379617271817, + "timestamp": 8.887579358836877 + }, + { + "x": 4.619158095512492, + "y": 4.726228597192443, + "heading": 0.017375147132049722, + "angularVelocity": -1.813308899206411e-8, + "velocityX": -3.059801171883295, + "velocityY": 2.222791027490615, + "timestamp": 8.922229313802774 + }, + { + "x": 4.51014571232548, + "y": 4.798953964824882, + "heading": 0.01737514649311973, + "angularVelocity": -1.8439562026762692e-8, + "velocityX": -3.146104613824239, + "velocityY": 2.0988589365849712, + "timestamp": 8.95687926876867 + }, + { + "x": 4.398314099643168, + "y": 4.867264790943429, + "heading": 0.017375145827920196, + "angularVelocity": -1.9197702730409006e-8, + "velocityX": -3.2274677641681775, + "velocityY": 1.9714549755050896, + "timestamp": 8.991529223734567 + }, + { + "x": 4.2838418935996625, + "y": 4.931051865994745, + "heading": 0.017375140544826046, + "angularVelocity": -1.5247044777404565e-7, + "velocityX": -3.3036754638258423, + "velocityY": 1.840899219467885, + "timestamp": 9.026179178700463 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.006985288099960263, - "angularVelocity": -0.014585653486632353, - "velocityX": -3.097100988534025, - "velocityY": 1.7453648577216743, - "timestamp": 9.385013213979638 - }, - { - "x": 3.9486678661427668, - "y": 5.097895325970227, - "heading": 0.00595988192546784, - "angularVelocity": -0.014043783913272705, - "velocityX": -2.9970738935334476, - "velocityY": 1.4784305228888592, - "timestamp": 9.458028163570491 - }, - { - "x": 3.7418449106776346, - "y": 5.188842546635376, - "heading": 0.004995539128350481, - "angularVelocity": -0.01320747055940148, - "velocityX": -2.8326110834026332, - "velocityY": 1.2455972533677366, - "timestamp": 9.531043113161344 - }, - { - "x": 3.5499887292854875, - "y": 5.265325878573737, - "heading": 0.004104715734621832, - "angularVelocity": -0.01220056164827184, - "velocityX": -2.6276287591409173, - "velocityY": 1.0475023589955905, - "timestamp": 9.604058062752197 - }, - { - "x": 3.374901152496024, - "y": 5.329479478594356, - "heading": 0.0032944686159963646, - "angularVelocity": -0.011097003054385247, - "velocityX": -2.397968878573332, - "velocityY": 0.8786365036216773, - "timestamp": 9.67707301234305 - }, - { - "x": 3.217704545762821, - "y": 5.382997215745527, - "heading": 0.002568947278727608, - "angularVelocity": -0.0099366135474215, - "velocityX": -2.1529372767367914, - "velocityY": 0.7329695829561359, - "timestamp": 9.750087961933902 - }, - { - "x": 3.079126437625259, - "y": 5.42721119539832, - "heading": 0.0019307216893178917, - "angularVelocity": -0.008741026227999762, - "velocityX": -1.8979415710631973, - "velocityY": 0.6055469448455648, - "timestamp": 9.823102911524755 - }, - { - "x": 2.959658381908625, - "y": 5.463178096937211, - "heading": 0.0013814652459932491, - "angularVelocity": -0.007522520338676778, - "velocityX": -1.6362136300317622, - "velocityY": 0.49259640307136626, - "timestamp": 9.896117861115608 - }, - { - "x": 2.8596453147458623, - "y": 5.491748253962881, - "heading": 0.0009223163485468854, - "angularVelocity": -0.006288423124569153, - "velocityX": -1.369761503954984, - "velocityY": 0.3912918818100397, - "timestamp": 9.96913281070646 - }, - { - "x": 2.779337430145008, - "y": 5.5136166054710944, - "heading": 0.0005540788250022006, - "angularVelocity": -0.0050433168222144735, - "velocityX": -1.0998827644320655, - "velocityY": 0.29950512368706583, - "timestamp": 10.042147760297313 - }, - { - "x": 2.718921448994663, - "y": 5.529359550737521, - "heading": 0.00027733842586429876, - "angularVelocity": -0.003790188183223442, - "velocityX": -0.8274467282233734, - "velocityY": 0.21561262939498888, - "timestamp": 10.115162709888166 - }, - { - "x": 2.678540177748154, - "y": 5.539461677753878, - "heading": 0.0000925335672086805, - "angularVelocity": -0.002531055074216922, - "velocityX": -0.5530548397662466, - "velocityY": 0.13835696762054583, - "timestamp": 10.188177659479019 + "heading": 0.01686674274932993, + "angularVelocity": -0.014672394119890892, + "velocityX": -3.3576617434279026, + "velocityY": 1.6997404450060698, + "timestamp": 9.06082913366636 + }, + { + "x": 3.941206209015057, + "y": 5.0867984564258695, + "heading": 0.014981559594595455, + "angularVelocity": -0.027398080889315734, + "velocityX": -3.288799800865747, + "velocityY": 1.4075673366199248, + "timestamp": 9.129636257871532 + }, + { + "x": 3.7303087168982803, + "y": 5.169878519937419, + "heading": 0.012882808496808885, + "angularVelocity": -0.03050194470456946, + "velocityX": -3.0650531402520937, + "velocityY": 1.2074340334849432, + "timestamp": 9.198443382076704 + }, + { + "x": 3.5370906486542313, + "y": 5.2422774011944036, + "heading": 0.010786769401609648, + "angularVelocity": -0.030462530143669606, + "velocityX": -2.808111376198552, + "velocityY": 1.0522003657804835, + "timestamp": 9.267250506281876 + }, + { + "x": 3.362377869511596, + "y": 5.305500032913324, + "heading": 0.008787829989227822, + "angularVelocity": -0.029051343672224058, + "velocityX": -2.5391669999413216, + "velocityY": 0.9188384553087892, + "timestamp": 9.336057630487048 + }, + { + "x": 3.2065764087500264, + "y": 5.360415401059238, + "heading": 0.0069378788339289135, + "angularVelocity": -0.026886040895746154, + "velocityX": -2.2643216463602664, + "velocityY": 0.7981058470364881, + "timestamp": 9.40486475469222 + }, + { + "x": 3.0699232813800306, + "y": 5.407585492361195, + "heading": 0.005269555505228705, + "angularVelocity": -0.024246374891727204, + "velocityX": -1.9860316638509532, + "velocityY": 0.6855408047763067, + "timestamp": 9.473671878897392 + }, + { + "x": 2.9525724844135883, + "y": 5.4474025208162065, + "heading": 0.00380523783536115, + "angularVelocity": -0.021281483375198414, + "velocityX": -1.70550358443293, + "velocityY": 0.5786759571041632, + "timestamp": 9.542479003102564 + }, + { + "x": 2.854631606493464, + "y": 5.480155404840544, + "heading": 0.0025612082007498414, + "angularVelocity": -0.018079953914391628, + "velocityX": -1.423411878515389, + "velocityY": 0.47601007021705255, + "timestamp": 9.611286127307736 + }, + { + "x": 2.776179840063629, + "y": 5.506065679558788, + "heading": 0.0015498402303536507, + "angularVelocity": -0.014698593816833106, + "velocityX": -1.1401692388117297, + "velocityY": 0.376563837212327, + "timestamp": 9.680093251512908 + }, + { + "x": 2.717277807675311, + "y": 5.525308541082813, + "heading": 0.000780854270914727, + "angularVelocity": -0.01117596423803359, + "velocityX": -0.8560455486074661, + "velocityY": 0.2796637956651817, + "timestamp": 9.74890037571808 + }, + { + "x": 2.677973353536199, + "y": 5.538025975345556, + "heading": 0.00026208876345230333, + "angularVelocity": -0.007539415626724292, + "velocityX": -0.5712265204096013, + "velocityY": 0.1848272894652728, + "timestamp": 9.817707499923252 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -1.9416138419813845e-19, - "angularVelocity": -0.0012673235786260742, - "velocityX": -0.27713515806951416, - "velocityY": 0.06674917354380465, - "timestamp": 10.261192609069871 + "heading": -9.768710288964573e-20, + "angularVelocity": -0.003809035277666901, + "velocityX": -0.2858451884385741, + "velocityY": 0.09169675411867828, + "timestamp": 9.886514624128424 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -9.371198129604789e-20, - "angularVelocity": 9.233837824624604e-20, - "velocityX": 1.3188409475375661e-20, - "velocityY": 7.071902639587051e-21, - "timestamp": 10.334207558660724 - }, - { - "x": 2.6757817592371826, - "y": 5.540525112473874, - "heading": 2.9656831471890883e-18, - "angularVelocity": 4.5198899479057065e-17, - "velocityX": 0.25819570553696475, - "velocityY": -0.05629192276270124, - "timestamp": 10.401894939630061 - }, - { - "x": 2.7106551649219846, - "y": 5.532555521074409, - "heading": 6.8667018003075775e-18, - "angularVelocity": 5.763287923477954e-17, - "velocityX": 0.5152128090256504, - "velocityY": -0.11774116955531974, - "timestamp": 10.469582320599399 - }, - { - "x": 2.762825274578906, - "y": 5.520028158351496, - "heading": 1.4433249035955584e-17, - "angularVelocity": 1.1178667467062074e-16, - "velocityX": 0.7707508978749655, - "velocityY": -0.18507678305041053, - "timestamp": 10.537269701568736 - }, - { - "x": 2.832164805136809, - "y": 5.502484798756892, - "heading": 1.639060638651272e-17, - "angularVelocity": 2.8917610972365257e-17, - "velocityX": 1.0244085317662608, - "velocityY": -0.259182130308041, - "timestamp": 10.604957082538073 - }, - { - "x": 2.9185095362845113, - "y": 5.47939415323771, - "heading": 1.573002181515029e-17, - "angularVelocity": -9.759346009756976e-18, - "velocityX": 1.2756400072092744, - "velocityY": -0.34113663711767017, - "timestamp": 10.67264446350741 - }, - { - "x": 3.021644093219496, - "y": 5.450134961207605, - "heading": 1.4178342885926476e-17, - "angularVelocity": -2.2924198084943018e-17, - "velocityX": 1.52368957785063, - "velocityY": -0.4322695251476669, - "timestamp": 10.740331844476747 - }, - { - "x": 3.141280830981249, - "y": 5.413974520765915, - "heading": 1.5889264278443728e-17, - "angularVelocity": 2.5276814794004328e-17, - "velocityX": 1.7674895386475231, - "velocityY": -0.5342272063690965, - "timestamp": 10.808019225446085 - }, - { - "x": 3.27702786794977, - "y": 5.370041857723201, - "heading": 1.8375553239227874e-17, - "angularVelocity": 3.6731942133725327e-17, - "velocityX": 2.0054999177766217, - "velocityY": -0.6490524882712743, - "timestamp": 10.875706606415422 - }, - { - "x": 3.4283399225131155, - "y": 5.317295603371642, - "heading": 2.6053093742707064e-17, - "angularVelocity": 1.1342646729407338e-16, - "velocityX": 2.235454414049344, - "velocityY": -0.7792627458204298, - "timestamp": 10.94339398738476 - }, - { - "x": 3.594442154384911, - "y": 5.254489538104451, - "heading": 3.5054878226049046e-17, - "angularVelocity": 1.3299058635211391e-16, - "velocityX": 2.453961572941345, - "velocityY": -0.9278844057453234, - "timestamp": 11.011081368354096 - }, - { - "x": 3.774213631656668, - "y": 5.18014653899489, - "heading": 3.926192172002768e-17, - "angularVelocity": 6.215402980533945e-17, - "velocityX": 2.6559083051712955, - "velocityY": -1.098328788097711, - "timestamp": 11.078768749323434 - }, - { - "x": 3.9660187105503817, - "y": 5.092569435745629, - "heading": 4.466170927343128e-17, - "angularVelocity": 7.977539500755919e-17, - "velocityX": 2.8336903592207485, - "velocityY": -1.2938468292772884, - "timestamp": 11.146456130292771 + "heading": -4.776521402733554e-20, + "angularVelocity": 3.1435024538818593e-20, + "velocityX": 5.229513890071112e-19, + "velocityY": 2.1429873638516558e-18, + "timestamp": 9.955321748333596 + }, + { + "x": 2.6751665057839835, + "y": 5.538944468629318, + "heading": -4.314568505907974e-18, + "angularVelocity": -6.69844600274702e-17, + "velocityX": 0.2647058041795454, + "velocityY": -0.08463157955674895, + "timestamp": 10.01902015375538 + }, + { + "x": 2.7088624712986666, + "y": 5.528079837461981, + "heading": -9.548636187746045e-18, + "angularVelocity": -8.216952445023882e-17, + "velocityX": 0.5289922925316966, + "velocityY": -0.1705636286402427, + "timestamp": 10.082718559177163 + }, + { + "x": 2.7593613029745305, + "y": 5.511644735540107, + "heading": -1.3305788264629273e-17, + "angularVelocity": -5.898345573823082e-17, + "velocityX": 0.7927801542516099, + "velocityY": -0.25801433824046893, + "timestamp": 10.146416964598947 + }, + { + "x": 2.8266246280660523, + "y": 5.489524723354373, + "heading": -1.7929345665528888e-17, + "angularVelocity": -7.258513569049044e-17, + "velocityX": 1.0559656030026554, + "velocityY": -0.34726163142175154, + "timestamp": 10.21011537002073 + }, + { + "x": 2.9106051995536752, + "y": 5.461582331644453, + "heading": -2.2921460737278194e-17, + "angularVelocity": -7.837111523596277e-17, + "velocityX": 1.31840932173323, + "velocityY": -0.43866705178720505, + "timestamp": 10.273813775442514 + }, + { + "x": 3.0112434875257597, + "y": 5.427649372766817, + "heading": -2.8632725416108676e-17, + "angularVelocity": -8.966103061640699e-17, + "velocityX": 1.5799184815647955, + "velocityY": -0.5327128466237959, + "timestamp": 10.337512180864298 + }, + { + "x": 3.1284623112048178, + "y": 5.387515427846554, + "heading": -3.496086628632705e-17, + "angularVelocity": -9.93453576777077e-17, + "velocityX": 1.8402159819054262, + "velocityY": -0.6300620031932188, + "timestamp": 10.401210586286082 + }, + { + "x": 3.2621579049769083, + "y": 5.340909820689256, + "heading": -4.1773207656590006e-17, + "angularVelocity": -1.0694681169901965e-16, + "velocityX": 2.0988844679362666, + "velocityY": -0.7316604999559271, + "timestamp": 10.464908991707865 + }, + { + "x": 3.4121839694430376, + "y": 5.287471786804592, + "heading": -4.8890897323491556e-17, + "angularVelocity": -1.1174046853574415e-16, + "velocityX": 2.3552562026116513, + "velocityY": -0.8389226312781303, + "timestamp": 10.52860739712965 + }, + { + "x": 3.5783204640757695, + "y": 5.226697519809261, + "heading": -5.788296277144039e-17, + "angularVelocity": -1.4116625664827146e-16, + "velocityX": 2.608173525422577, + "velocityY": -0.954094008992977, + "timestamp": 10.592305802551433 + }, + { + "x": 3.76020443223388, + "y": 5.157837260218167, + "heading": -6.653949543666233e-17, + "angularVelocity": -1.3589873416470021e-16, + "velocityX": 2.85539279914075, + "velocityY": -1.0810358459545435, + "timestamp": 10.656004207973217 + }, + { + "x": 3.9571467965572817, + "y": 5.079670509622146, + "heading": -7.420373602789392e-17, + "angularVelocity": -1.2032076062800333e-16, + "velocityX": 3.0917942610859868, + "velocityY": -1.2271382631705205, + "timestamp": 10.719702613395 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 4.913032791004192e-17, - "angularVelocity": 6.601848931159295e-17, - "velocityX": 2.976630975573702, - "velocityY": -1.516111842533804, - "timestamp": 11.214143511262108 - }, - { - "x": 4.261627503076193, - "y": 4.9391622522038245, - "heading": 5.1271717716207476e-17, - "angularVelocity": 6.871367197331748e-17, - "velocityX": 3.0204265353966933, - "velocityY": -1.6296244523245107, - "timestamp": 11.245307466608786 - }, - { - "x": 4.357244155674493, - "y": 4.884889313220403, - "heading": 5.244866899623222e-17, - "angularVelocity": 3.776642810800902e-17, - "velocityX": 3.0681809011286347, - "velocityY": -1.7415292243770975, - "timestamp": 11.276471421955463 - }, - { - "x": 4.454522517564495, - "y": 4.827208233471932, - "heading": 5.393508555206354e-17, - "angularVelocity": 4.76966591102462e-17, - "velocityX": 3.121502415461902, - "velocityY": -1.8508908483152524, - "timestamp": 11.307635377302141 - }, - { - "x": 4.553721568715114, - "y": 4.766257957931851, - "heading": 5.435767688071947e-17, - "angularVelocity": 1.3560259702022703e-17, - "velocityX": 3.183134170457424, - "velocityY": -1.9557939568983076, - "timestamp": 11.338799332648819 - }, - { - "x": 4.6552566098247095, - "y": 4.702321075934724, - "heading": 5.3497639174451405e-17, - "angularVelocity": -2.7597193512319988e-17, - "velocityX": 3.2580922408625277, - "velocityY": -2.0516292391602082, - "timestamp": 11.369963287995496 - }, - { - "x": 4.7598200255019725, - "y": 4.636102620452091, - "heading": 5.221139321258901e-17, - "angularVelocity": -4.1273514455672286e-17, - "velocityX": 3.355267793002091, - "velocityY": -2.1248411745556077, - "timestamp": 11.401127243342174 - }, - { - "x": 4.868161384056621, - "y": 4.569562096793982, - "heading": 4.9900416681055594e-17, - "angularVelocity": -7.415543069862901e-17, - "velocityX": 3.476495757660606, - "velocityY": -2.135175811859932, - "timestamp": 11.432291198688851 - }, - { - "x": 4.979472809142361, - "y": 4.50537854159068, - "heading": 4.580479975569673e-17, - "angularVelocity": -1.3142160161653158e-16, - "velocityX": 3.5718003009398434, - "velocityY": -2.059544576075285, - "timestamp": 11.463455154035529 - }, - { - "x": 5.092478993411361, - "y": 4.444586805435104, - "heading": 4.211690012859059e-17, - "angularVelocity": -1.1833862502052825e-16, - "velocityX": 3.6261823318601563, - "velocityY": -1.950706689164141, - "timestamp": 11.494619109382207 - }, - { - "x": 5.206458430184718, - "y": 4.387459691402226, - "heading": 3.8483549973003e-17, - "angularVelocity": -1.1658822232196594e-16, - "velocityX": 3.657412401776814, - "velocityY": -1.8331150008841193, - "timestamp": 11.525783064728884 - }, - { - "x": 5.321007172735708, - "y": 4.334081255049934, - "heading": 3.4624290514440134e-17, - "angularVelocity": -1.2383727976295539e-16, - "velocityX": 3.675680486533673, - "velocityY": -1.7128261081911138, - "timestamp": 11.556947020075562 - }, - { - "x": 5.435875810085393, - "y": 4.2844809626648175, - "heading": 3.000921461469742e-17, - "angularVelocity": -1.4809018452502327e-16, - "velocityX": 3.685945383756665, - "velocityY": -1.5915916908925372, - "timestamp": 11.58811097542224 - }, - { - "x": 5.550897094649703, - "y": 4.2386692598409, - "heading": 2.554076914069394e-17, - "angularVelocity": -1.4338505567167463e-16, - "velocityX": 3.690843581463868, - "velocityY": -1.4700220916855413, - "timestamp": 11.619274930768917 + "heading": -8.561193314262968e-17, + "angularVelocity": -1.7909705963758273e-16, + "velocityX": 3.3023160854502294, + "velocityY": -1.4085550989874924, + "timestamp": 10.783401018816784 + }, + { + "x": 4.2842075611264425, + "y": 4.936417577245955, + "heading": -9.453694204366925e-17, + "angularVelocity": -2.5755488045645486e-16, + "velocityX": 3.3679341946759767, + "velocityY": -1.5447568972758807, + "timestamp": 10.81805386206763 + }, + { + "x": 4.401356584826297, + "y": 4.8776669667131465, + "heading": -1.0560165699964911e-16, + "angularVelocity": -3.1930179223080245e-16, + "velocityX": 3.380646801528862, + "velocityY": -1.6954051968412056, + "timestamp": 10.852706705318477 + }, + { + "x": 4.516063976001339, + "y": 4.814281195629902, + "heading": -9.662380875053523e-17, + "angularVelocity": 2.590797004510181e-16, + "velocityX": 3.3101869980680285, + "velocityY": -1.8291650882556678, + "timestamp": 10.887359548569323 + }, + { + "x": 4.628146410125643, + "y": 4.746361612927903, + "heading": -9.053238794887956e-17, + "angularVelocity": 1.7578415593930746e-16, + "velocityX": 3.234436877602137, + "velocityY": -1.9600002865663488, + "timestamp": 10.92201239182017 + }, + { + "x": 4.737424855337869, + "y": 4.674016859733605, + "heading": -7.951670404623386e-17, + "angularVelocity": 3.1788687072504797e-16, + "velocityX": 3.1535203163901047, + "velocityY": -2.087700356089278, + "timestamp": 10.956665235071016 + }, + { + "x": 4.844483096398324, + "y": 4.598425359059355, + "heading": -7.540763427184281e-17, + "angularVelocity": 1.1857814219565296e-16, + "velocityX": 3.089450417833756, + "velocityY": -2.181393893916722, + "timestamp": 10.991318078321862 + }, + { + "x": 4.954476829355994, + "y": 4.5271728465414585, + "heading": -1.0264313607639127e-16, + "angularVelocity": -7.859528757078724e-16, + "velocityX": 3.174161847599147, + "velocityY": -2.0561808450207573, + "timestamp": 11.025970921572709 + }, + { + "x": 5.06723038345343, + "y": 4.460373312794279, + "heading": -9.620613179989556e-17, + "angularVelocity": 1.8575688667134074e-16, + "velocityX": 3.25380383021476, + "velocityY": -1.9276782936288397, + "timestamp": 11.060623764823555 + }, + { + "x": 5.18256360847748, + "y": 4.398133545147613, + "heading": -8.10849749390595e-17, + "angularVelocity": 4.36361217215796e-16, + "velocityX": 3.3282470990668904, + "velocityY": -1.796094109684511, + "timestamp": 11.095276608074402 + }, + { + "x": 5.300292214676822, + "y": 4.340553023175948, + "heading": -6.741688231750934e-17, + "angularVelocity": 3.9442918212224324e-16, + "velocityX": 3.397372196766743, + "velocityY": -1.6616391779124173, + "timestamp": 11.129929451325248 + }, + { + "x": 5.4202280776683445, + "y": 4.287723772710641, + "heading": -7.654370000647077e-17, + "angularVelocity": -2.6337861002637873e-16, + "velocityX": 3.461068464810413, + "velocityY": -1.5245285959043562, + "timestamp": 11.164582294576094 + }, + { + "x": 5.542179542504675, + "y": 4.239730223009142, + "heading": -8.402477364947206e-17, + "angularVelocity": -2.1588628640817297e-16, + "velocityX": 3.5192340193716007, + "velocityY": -1.3849815830141687, + "timestamp": 11.19923513782694 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 2.1323021995570948e-17, - "angularVelocity": -1.353405592009617e-16, - "velocityX": 3.6919137154187807, - "velocityY": -1.3483585385427075, - "timestamp": 11.650438886115595 - }, - { - "x": 5.935813024971261, - "y": 4.118882926647287, - "heading": 1.853226702760062e-17, - "angularVelocity": -3.8136239390164817e-17, - "velocityX": 3.6877099925560546, - "velocityY": -1.0626903702408637, - "timestamp": 11.723617443186296 - }, - { - "x": 6.200932724790644, - "y": 4.061479044678689, - "heading": 1.7565627376800837e-17, - "angularVelocity": -1.3209329241757196e-17, - "velocityX": 3.622915105626314, - "velocityY": -0.7844358274670208, - "timestamp": 11.796796000256997 - }, - { - "x": 6.456598704926571, - "y": 4.0227227304075965, - "heading": 1.267506754253446e-17, - "angularVelocity": -6.683050380301111e-17, - "velocityX": 3.493728086069183, - "velocityY": -0.5296129880457807, - "timestamp": 11.869974557327698 - }, - { - "x": 6.698616689948628, - "y": 3.99980424542816, - "heading": 8.945633316051437e-18, - "angularVelocity": -5.0963484094285186e-17, - "velocityX": 3.307225432010503, - "velocityY": -0.31318580055211104, - "timestamp": 11.943153114398399 - }, - { - "x": 6.923883976637485, - "y": 3.9893965055012433, - "heading": 2.783843782577358e-18, - "angularVelocity": -8.420211852775213e-17, - "velocityX": 3.078323701726083, - "velocityY": -0.1422239019670945, - "timestamp": 12.0163316714691 - }, - { - "x": 7.130423561054918, - "y": 3.988282733586895, - "heading": 2.8545928007524544e-18, - "angularVelocity": 9.667998274288902e-19, - "velocityX": 2.822405806907136, - "velocityY": -0.015219921776707517, - "timestamp": 12.089510228539801 - }, - { - "x": 7.317098326514511, - "y": 3.993687679254755, - "heading": 5.295405456553911e-18, - "angularVelocity": 3.335420582506506e-17, - "velocityX": 2.5509489792103515, - "velocityY": 0.07385969174874675, - "timestamp": 12.162688785610502 - }, - { - "x": 7.4833023364759494, - "y": 4.003341497002791, - "heading": 9.255539418426772e-18, - "angularVelocity": 5.4116043269433334e-17, - "velocityX": 2.2712119043405083, - "velocityY": 0.13192140067353622, - "timestamp": 12.235867342681203 - }, - { - "x": 7.628740531129743, - "y": 4.015421358409773, - "heading": 1.1054283152069948e-17, - "angularVelocity": 2.458020224219918e-17, - "velocityX": 1.9874427766221006, - "velocityY": 0.16507378514324764, - "timestamp": 12.309045899751904 - }, - { - "x": 7.753293848479071, - "y": 4.028463870964022, - "heading": 1.0645910526563799e-17, - "angularVelocity": -5.5804957540262804e-18, - "velocityX": 1.7020466422833305, - "velocityY": 0.1782286106249456, - "timestamp": 12.382224456822605 - }, - { - "x": 7.856941288758158, - "y": 4.041284750258868, - "heading": 1.2932065845592816e-17, - "angularVelocity": 3.1240781584055425e-17, - "velocityX": 1.4163635418357237, - "velocityY": 0.17519994665183228, - "timestamp": 12.455403013893307 - }, - { - "x": 7.939715627162556, - "y": 4.0529146415487, - "heading": 1.2915183661697564e-17, - "angularVelocity": -2.306985186705733e-19, - "velocityX": 1.1311283211614112, - "velocityY": 0.15892485114999141, - "timestamp": 12.528581570964008 - }, - { - "x": 8.001678238256412, - "y": 4.062550327487904, - "heading": 8.098550414220998e-18, - "angularVelocity": -6.582028180467396e-17, - "velocityX": 0.8467317965013967, - "velocityY": 0.13167362578486927, - "timestamp": 12.601760128034709 - }, - { - "x": 8.042904705550558, - "y": 4.0695182051166965, - "heading": 2.8097102514123304e-18, - "angularVelocity": -7.227308620193683e-17, - "velocityX": 0.5633681360281035, - "velocityY": 0.09521747773817571, - "timestamp": 12.67493868510541 + "heading": -9.639099903602133e-17, + "angularVelocity": -3.5686033890323503e-16, + "velocityX": 3.5717757824418066, + "velocityY": -1.2432211735943417, + "timestamp": 11.233887981077787 + }, + { + "x": 5.93683892832179, + "y": 4.127579524293423, + "heading": -7.782590018599437e-17, + "angularVelocity": 2.51158487397316e-16, + "velocityX": 3.664705468644969, + "velocityY": -0.9344094480089544, + "timestamp": 11.307805844823916 + }, + { + "x": 6.212625572925941, + "y": 4.081838891930783, + "heading": -5.4414649647687885e-17, + "angularVelocity": 3.167197934552236e-16, + "velocityX": 3.7309877562390543, + "velocityY": -0.6188034941017393, + "timestamp": 11.381723708570044 + }, + { + "x": 6.491306353239059, + "y": 4.059759739930169, + "heading": -8.220332638334233e-17, + "angularVelocity": -3.7593993288383394e-16, + "velocityX": 3.7701411565443874, + "velocityY": -0.2986984590956788, + "timestamp": 11.455641572316173 + }, + { + "x": 6.752875972710037, + "y": 4.054277278014819, + "heading": -7.798662141044023e-17, + "angularVelocity": 5.704581760375439e-17, + "velocityX": 3.5386523123739373, + "velocityY": -0.07416964773468374, + "timestamp": 11.529559436062302 + }, + { + "x": 6.9910920962877245, + "y": 4.053575874325361, + "heading": -6.860310501240552e-17, + "angularVelocity": 1.269451783709716e-16, + "velocityX": 3.2227138543376115, + "velocityY": -0.00948896050171146, + "timestamp": 11.60347729980843 + }, + { + "x": 7.205567187241113, + "y": 4.055021140911149, + "heading": -5.885294926916e-17, + "angularVelocity": 1.3190526956719489e-16, + "velocityX": 2.901532594204896, + "velocityY": 0.019552331635990624, + "timestamp": 11.677395163554559 + }, + { + "x": 7.396227777452995, + "y": 4.057522153925315, + "heading": -4.8982438983156224e-17, + "angularVelocity": 1.3353348954892513e-16, + "velocityX": 2.5793574184815227, + "velocityY": 0.033835028333003926, + "timestamp": 11.751313027300688 + }, + { + "x": 7.563054971618611, + "y": 4.060486200727262, + "heading": -3.912779424726568e-17, + "angularVelocity": 1.333188519862415e-16, + "velocityX": 2.256926617070343, + "velocityY": 0.04009919458884174, + "timestamp": 11.825230891046816 + }, + { + "x": 7.70604444565388, + "y": 4.063541585038157, + "heading": -2.9177079820924904e-17, + "angularVelocity": 1.3461853362720707e-16, + "velocityX": 1.9344373171601388, + "velocityY": 0.041334856772760636, + "timestamp": 11.899148754792945 + }, + { + "x": 7.825196583277639, + "y": 4.066433602906459, + "heading": -1.9253434148047247e-17, + "angularVelocity": 1.3425233319755054e-16, + "velocityX": 1.6119532084015245, + "velocityY": 0.03912474903541938, + "timestamp": 11.973066618539074 + }, + { + "x": 7.920513375065405, + "y": 4.0689768762979055, + "heading": -9.302606124041008e-18, + "angularVelocity": 1.346200704365749e-16, + "velocityX": 1.2894960292025357, + "velocityY": 0.034406749093584967, + "timestamp": 12.046984482285202 + }, + { + "x": 7.991997299424133, + "y": 4.071030463180752, + "heading": 8.179153045928767e-19, + "angularVelocity": 1.369157726664237e-16, + "velocityX": 0.9670723792051265, + "velocityY": 0.027782010717991884, + "timestamp": 12.12090234603133 + }, + { + "x": 8.03965088919926, + "y": 4.072483600942051, + "heading": 2.229877553000587e-19, + "angularVelocity": -8.048494898574051e-18, + "velocityX": 0.6446829948819026, + "velocityY": 0.019658817066051014, + "timestamp": 12.19482020977746 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 3.1031533755807955e-28, - "angularVelocity": -3.8395267193036997e-17, - "velocityX": 0.2811186469496163, - "velocityY": 0.05095414427593911, - "timestamp": 12.74811724217611 + "heading": 3.803306693773524e-29, + "angularVelocity": -3.0166964241453683e-18, + "velocityX": 0.32232632402054306, + "velocityY": 0.01032706968038683, + "timestamp": 12.268738073523588 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 1.493594231911397e-28, - "angularVelocity": -1.5846843190307173e-28, - "velocityX": -6.450761173739877e-21, - "velocityY": 2.2636697046974056e-20, - "timestamp": 12.821295799246812 + "heading": 1.8619056951323718e-29, + "angularVelocity": -1.0754545583434217e-29, + "velocityX": -2.9997098934675086e-25, + "velocityY": 4.422144616548637e-24, + "timestamp": 12.342655937269717 } ], "constraints": [ diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj index f2a4cb6f..e8fd9ccf 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj @@ -1,328 +1,310 @@ { "samples": [ { - "x": 1.2899744510650635, + "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -5.737773088876545e-18, - "angularVelocity": -1.3800854803157527e-17, - "velocityX": -2.0304788403041935e-16, - "velocityY": -3.8084104282489897e-16, + "heading": -1.2424307943085809e-21, + "angularVelocity": -1.3399063907175068e-21, + "velocityX": -6.613526246981409e-20, + "velocityY": -3.127454467901795e-20, "timestamp": 0 }, { - "x": 1.3043755674833586, - "y": 5.574433574042641, - "heading": -0.009496998526070185, - "angularVelocity": -0.1264048176402608, - "velocityX": 0.19167850659141716, - "velocityY": -0.21989050741887875, - "timestamp": 0.07513161842720571 - }, - { - "x": 1.333181418313963, - "y": 5.541395354098072, - "heading": -0.02848354997968903, - "angularVelocity": -0.2527105345468862, - "velocityX": 0.38340516859413276, - "velocityY": -0.43973789779720845, - "timestamp": 0.15026323685441142 - }, - { - "x": 1.3763968626631116, - "y": 5.491843734926518, - "heading": -0.05694076492631607, - "angularVelocity": -0.3787648335429662, - "velocityX": 0.5751965051812923, - "velocityY": -0.6595308368163946, - "timestamp": 0.22539485528161712 - }, - { - "x": 1.434028520834508, - "y": 5.425784149039423, - "heading": -0.09483975910440062, - "angularVelocity": -0.5044346837260156, - "velocityX": 0.76707595784789, - "velocityY": -0.879251469241908, - "timestamp": 0.30052647370882285 - }, - { - "x": 1.5060856921499353, - "y": 5.34322422233644, - "heading": -0.14214370318506256, - "angularVelocity": -0.6296143364447188, - "velocityX": 0.9590791842586537, - "velocityY": -1.0988706011306708, - "timestamp": 0.3756580921360286 - }, - { - "x": 1.5925821728366396, - "y": 5.244175419847767, - "heading": -0.19881016609697202, - "angularVelocity": -0.7542292326445892, - "velocityX": 1.15126603811467, - "velocityY": -1.318337133749589, - "timestamp": 0.4507897105632343 - }, - { - "x": 1.6935404999326085, - "y": 5.128656800108543, - "heading": -0.2647926425863519, - "angularVelocity": -0.8782251450575851, - "velocityX": 1.3437528593918235, - "velocityY": -1.5375499977346858, - "timestamp": 0.52592132899044 - }, - { - "x": 1.8090047533297515, - "y": 4.99670620893713, - "heading": -0.34003720148047023, - "angularVelocity": -1.001503234885639, - "velocityX": 1.5368263829457236, - "velocityY": -1.7562591348086285, - "timestamp": 0.6010529474176457 - }, - { - "x": 1.9391049081706304, - "y": 4.848436880668431, - "heading": -0.42444836738377417, - "angularVelocity": -1.1235105495461353, - "velocityX": 1.731629872448852, - "velocityY": -1.9734611256713033, - "timestamp": 0.6761845658448514 - }, - { - "x": 2.0768333691424767, - "y": 4.720578672204384, - "heading": -0.4949304334552377, - "angularVelocity": -0.938114572048371, - "velocityX": 1.8331624402684015, - "velocityY": -1.7017896211431234, - "timestamp": 0.7513161842720572 - }, - { - "x": 2.200438107278665, - "y": 4.609474439269374, - "heading": -0.5559710604478235, - "angularVelocity": -0.8124492493047433, - "velocityX": 1.6451760354351246, - "velocityY": -1.4787946174642006, - "timestamp": 0.8264478026992629 - }, - { - "x": 2.30979055136467, - "y": 4.515017163004073, - "heading": -0.6076423765200641, - "angularVelocity": -0.6877439505764344, - "velocityX": 1.4554783509002944, - "velocityY": -1.2572240321695016, - "timestamp": 0.9015794211264686 - }, - { - "x": 2.404847576631005, - "y": 4.43717046489753, - "heading": -0.6499678685895577, - "angularVelocity": -0.5633512621433749, - "velocityX": 1.2652066767447667, - "velocityY": -1.0361376439603944, - "timestamp": 0.9767110395536743 - }, - { - "x": 2.4855875280124384, - "y": 4.375915950789138, - "heading": -0.6829583682346397, - "angularVelocity": -0.43910274175131525, - "velocityX": 1.0746467742038588, - "velocityY": -0.8152960815764004, - "timestamp": 1.05184265798088 - }, - { - "x": 2.5519973917574865, - "y": 4.331242518343638, - "heading": -0.7066196503968551, - "angularVelocity": -0.3149310856889676, - "velocityX": 0.8839136589509067, - "velocityY": -0.5946022910015373, - "timestamp": 1.1269742764080857 - }, - { - "x": 2.6040685139576465, - "y": 4.3031427656124555, - "heading": -0.720955031446972, - "angularVelocity": -0.19080357043979132, - "velocityX": 0.6930653603887426, - "velocityY": -0.37400702018003557, - "timestamp": 1.2021058948352914 - }, - { - "x": 2.641794765155857, - "y": 4.291611444641949, - "heading": -0.7259664732810771, - "angularVelocity": -0.06670216797910607, - "velocityX": 0.5021354789039092, - "velocityY": -0.15348159949241175, - "timestamp": 1.2772375132624971 + "x": 1.3060573925677244, + "y": 5.572382489557548, + "heading": -0.010694903924415845, + "angularVelocity": -0.14212387666711845, + "velocityX": 0.21372515458978017, + "velocityY": -0.2467996203811156, + "timestamp": 0.07525057840538203 + }, + { + "x": 1.3382234703338376, + "y": 5.5352391647325, + "heading": -0.03207617924105129, + "angularVelocity": -0.2841343650736142, + "velocityX": 0.4274528973429488, + "velocityY": -0.49359520700229137, + "timestamp": 0.15050115681076406 + }, + { + "x": 1.3864731960687624, + "y": 5.479524641793448, + "heading": -0.06412173122661301, + "angularVelocity": -0.4258512381516761, + "velocityX": 0.6411874401150632, + "velocityY": -0.7403866404709017, + "timestamp": 0.2257517352161461 + }, + { + "x": 1.4508074313628532, + "y": 5.405239333646717, + "heading": -0.10679840962568042, + "angularVelocity": -0.5671275796601069, + "velocityX": 0.8549334325048802, + "velocityY": -0.9871725868543993, + "timestamp": 0.30100231362152813 + }, + { + "x": 1.5312274241075188, + "y": 5.312383875814348, + "heading": -0.16006570142322632, + "angularVelocity": -0.7078655463694897, + "velocityX": 1.0686960080417647, + "velocityY": -1.2339500878272134, + "timestamp": 0.37625289202691015 + }, + { + "x": 1.6277348707062913, + "y": 5.200959293585018, + "heading": -0.22388050123959674, + "angularVelocity": -0.8480306885163599, + "velocityX": 1.282481126973902, + "velocityY": -1.480713963806038, + "timestamp": 0.45150347043229216 + }, + { + "x": 1.740332134586227, + "y": 5.070967309798755, + "heading": -0.29820239377925056, + "angularVelocity": -0.9876587544520216, + "velocityX": 1.4962976533331545, + "velocityY": -1.7274549450767482, + "timestamp": 0.5267540488376742 + }, + { + "x": 1.869023591989777, + "y": 4.922411591689274, + "heading": -0.38299783161333695, + "angularVelocity": -1.126841010806393, + "velocityX": 1.7101723352912572, + "velocityY": -1.9741471927192127, + "timestamp": 0.6020046272430563 + }, + { + "x": 2.0218400763966784, + "y": 4.778612689185939, + "heading": -0.4624367890743981, + "angularVelocity": -1.0556590945137452, + "velocityX": 2.030768236539849, + "velocityY": -1.9109341821756607, + "timestamp": 0.6772552056484383 + }, + { + "x": 2.158571900472673, + "y": 4.653385130962418, + "heading": -0.5314200615234237, + "angularVelocity": -0.9167141822805146, + "velocityX": 1.817020240554264, + "velocityY": -1.664140806319243, + "timestamp": 0.7525057840538204 + }, + { + "x": 2.2792154502458053, + "y": 4.546725640045298, + "heading": -0.5899528620113615, + "angularVelocity": -0.777838545939356, + "velocityX": 1.6032242187324346, + "velocityY": -1.4173909779475127, + "timestamp": 0.8277563624592025 + }, + { + "x": 2.383769251108378, + "y": 4.458632746098652, + "heading": -0.6380359031204368, + "angularVelocity": -0.6389723790566404, + "velocityX": 1.3894086009456479, + "velocityY": -1.170660688773468, + "timestamp": 0.9030069408645846 + }, + { + "x": 2.4722324605852046, + "y": 4.389105547577967, + "heading": -0.6756681828251195, + "angularVelocity": -0.5000928963223903, + "velocityX": 1.1755817875613708, + "velocityY": -0.9239423801653125, + "timestamp": 0.9782575192699666 + }, + { + "x": 2.5446045503525854, + "y": 4.33814344893939, + "heading": -0.7028480663782789, + "angularVelocity": -0.36119168954076036, + "velocityX": 0.9617479533181219, + "velocityY": -0.6772319857003534, + "timestamp": 1.0535080976753486 + }, + { + "x": 2.600885196215428, + "y": 4.305746063716301, + "heading": -0.7195737337553716, + "angularVelocity": -0.22226629657236585, + "velocityX": 0.7479098108675392, + "velocityY": -0.4305267269649537, + "timestamp": 1.1287586760807307 + }, + { + "x": 2.6410742311964075, + "y": 4.29191316967755, + "heading": -0.7258433495686913, + "angularVelocity": -0.08331651325714302, + "velocityX": 0.5340694494662552, + "velocityY": -0.1838244214447216, + "timestamp": 1.2040092544861127 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.057384071798383285, - "velocityX": 0.3111454080847126, - "velocityY": 0.06699234114487357, - "timestamp": 1.3523691316897029 - }, - { - "x": 2.6741706927639637, - "y": 4.318416553359305, - "heading": -0.7079150843864638, - "angularVelocity": 0.1820368667672511, - "velocityX": 0.11922552840405579, - "velocityY": 0.2884478427083078, - "timestamp": 1.4278485164558985 - }, - { - "x": 2.6686789976139393, - "y": 4.35689985788212, - "heading": -0.6847718372969428, - "angularVelocity": 0.306616795565399, - "velocityX": -0.0727575505371169, - "velocityY": 0.5098518574324268, - "timestamp": 1.5033279012220941 - }, - { - "x": 2.648690219871855, - "y": 4.4120894355267595, - "heading": -0.6522316141278892, - "angularVelocity": 0.4311140488042709, - "velocityX": -0.26482433323531773, - "velocityY": 0.7311874336773603, - "timestamp": 1.5788072859882898 - }, - { - "x": 2.6141955291979047, - "y": 4.483978018466003, - "heading": -0.6103014314016028, - "angularVelocity": 0.5555183425901422, - "velocityX": -0.4570081060213073, - "velocityY": 0.9524267210180263, - "timestamp": 1.6542866707544854 - }, - { - "x": 2.565181654912471, - "y": 4.572554575453533, - "heading": -0.5589890545924265, - "angularVelocity": 0.6798197543188936, - "velocityX": -0.6493676974062975, - "velocityY": 1.173519859229229, - "timestamp": 1.729766055520681 - }, - { - "x": 2.501626381973055, - "y": 4.6778004085012075, - "heading": -0.49830336104516576, - "angularVelocity": 0.8040035532069214, - "velocityX": -0.842021608129967, - "velocityY": 1.394365274326458, - "timestamp": 1.8052454402868767 - }, - { - "x": 2.4234850468656797, - "y": 4.799677413625942, - "heading": -0.42825793399699796, - "angularVelocity": 0.9280073925294743, - "velocityX": -1.0352672501499576, - "velocityY": 1.614705863061015, - "timestamp": 1.8807248250530724 - }, - { - "x": 2.330623714307083, - "y": 4.938069448938432, - "heading": -0.3489003984671445, - "angularVelocity": 1.0513802646058852, - "velocityX": -1.2302873539560828, - "velocityY": 1.8335077284709782, - "timestamp": 1.956204209819268 - }, - { - "x": 2.2158437322138607, - "y": 5.0727178717793375, - "heading": -0.2714542834500897, - "angularVelocity": 1.0260565220540956, - "velocityX": -1.5206798842301303, - "velocityY": 1.7839099147754294, - "timestamp": 2.0316835945854637 - }, - { - "x": 2.115226855971239, - "y": 5.19038091333927, - "heading": -0.20366940407384615, - "angularVelocity": 0.8980581861967815, - "velocityX": -1.333037842725262, - "velocityY": 1.5588765320192262, - "timestamp": 2.107162979351659 - }, - { - "x": 2.028914050239155, - "y": 5.291175475979272, - "heading": -0.14552121166963816, - "angularVelocity": 0.7703850870803018, - "velocityX": -1.1435282096780108, - "velocityY": 1.3353919478119864, - "timestamp": 2.1826423641178545 - }, - { - "x": 1.956951239222395, - "y": 5.375140495354652, - "heading": -0.09702855704513491, - "angularVelocity": 0.6424622401999767, - "velocityX": -0.9534101428822267, - "velocityY": 1.1124232085021941, - "timestamp": 2.25812174888405 - }, - { - "x": 1.8993610593828132, - "y": 5.442295470240139, - "heading": -0.05821541772623594, - "angularVelocity": 0.5142217234562786, - "velocityX": -0.7629921735099331, - "velocityY": 0.8897128016480877, - "timestamp": 2.3336011336502454 - }, - { - "x": 1.856157020573883, - "y": 5.492652116983436, - "heading": -0.029101141017864884, - "angularVelocity": 0.3857248810290673, - "velocityX": -0.5723952168077603, - "velocityY": 0.6671576205923756, - "timestamp": 2.4090805184164408 - }, - { - "x": 1.8273481914604963, - "y": 5.526218220177199, - "heading": -0.00969618965979829, - "angularVelocity": 0.2570894214180142, - "velocityX": -0.38167811253199, - "velocityY": 0.44470557488567636, - "timestamp": 2.484559903182636 - }, - { - "x": 1.8129411935806279, + "angularVelocity": 0.05565717315833812, + "velocityX": 0.3202286619480617, + "velocityY": 0.06287683198324669, + "timestamp": 1.2792598328914948 + }, + { + "x": 2.6731094248331972, + "y": 4.320288497186145, + "heading": -0.7067986870474628, + "angularVelocity": 0.19580509628770462, + "velocityX": 0.10461882238047351, + "velocityY": 0.31162123137301345, + "timestamp": 1.355133384348122 + }, + { + "x": 2.6646882165451484, + "y": 4.3628057123101, + "heading": -0.6813129128721624, + "angularVelocity": 0.33589799984345314, + "velocityX": -0.11099003706005371, + "velocityY": 0.5603693817898957, + "timestamp": 1.431006935804749 + }, + { + "x": 2.6399081500891253, + "y": 4.424196738035329, + "heading": -0.6452019314670412, + "angularVelocity": 0.47593635347047025, + "velocityX": -0.32659689681441373, + "velocityY": 0.8091228701785946, + "timestamp": 1.5068804872613761 + }, + { + "x": 2.5987694540063986, + "y": 4.504462102028234, + "heading": -0.5984691810768386, + "angularVelocity": 0.6159293916394221, + "velocityX": -0.5422007444352149, + "velocityY": 1.057883313117199, + "timestamp": 1.5827540387180032 + }, + { + "x": 2.5412724297468685, + "y": 4.603602434973848, + "heading": -0.5411158632570503, + "angularVelocity": 0.7559065935192721, + "velocityX": -0.7578006189995966, + "velocityY": 1.30665206837309, + "timestamp": 1.6586275901746304 + }, + { + "x": 2.4674174122162253, + "y": 4.7216183838553025, + "heading": -0.4731379534998271, + "angularVelocity": 0.8959368377013766, + "velocityX": -0.973396079566176, + "velocityY": 1.5554293507523191, + "timestamp": 1.7345011416312575 + }, + { + "x": 2.3772045458929667, + "y": 4.8585103095087545, + "heading": -0.3945211417087051, + "angularVelocity": 1.0361556864260284, + "velocityX": -1.1889896359316807, + "velocityY": 1.8042113888883347, + "timestamp": 1.8103746930878846 + }, + { + "x": 2.270632337424553, + "y": 5.014276859543665, + "heading": -0.3052336675309315, + "angularVelocity": 1.176793130987344, + "velocityX": -1.4046028744197523, + "velocityY": 2.0529756027560517, + "timestamp": 1.8862482445445117 + }, + { + "x": 2.1562113194234667, + "y": 5.146461372900032, + "heading": -0.22910323394931606, + "angularVelocity": 1.0033856610117322, + "velocityX": -1.5080487970369372, + "velocityY": 1.7421685266957554, + "timestamp": 1.9621217960011388 + }, + { + "x": 2.05813446327852, + "y": 5.259759383516201, + "heading": -0.1637433708814866, + "angularVelocity": 0.861431444990317, + "velocityX": -1.2926356320754544, + "velocityY": 1.49324775815898, + "timestamp": 2.037995347457766 + }, + { + "x": 1.9764036449833426, + "y": 5.354173277082403, + "heading": -0.10920518207306891, + "angularVelocity": 0.7188036906324902, + "velocityX": -1.077197741849454, + "velocityY": 1.2443584325978703, + "timestamp": 2.1138688989143932 + }, + { + "x": 1.9110189705233616, + "y": 5.429703862947866, + "heading": -0.06553401357536529, + "angularVelocity": 0.5755782833319475, + "velocityX": -0.8617584547542609, + "velocityY": 0.9954797741165524, + "timestamp": 2.1897424503710203 + }, + { + "x": 1.8619803278190585, + "y": 5.486351581956986, + "heading": -0.032764600292238745, + "angularVelocity": 0.43189507613676265, + "velocityX": -0.6463206448473131, + "velocityY": 0.7466069258864354, + "timestamp": 2.2656160018276474 + }, + { + "x": 1.8292876878125428, + "y": 5.524116686024045, + "heading": -0.010918035230093376, + "angularVelocity": 0.287933866844679, + "velocityX": -0.43088321791822976, + "velocityY": 0.4977373978420955, + "timestamp": 2.3414895532842745 + }, + { + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.264631493815946e-17, - "angularVelocity": 0.12846142943609742, - "velocityX": -0.19087328181241156, - "velocityY": 0.22232623455065786, - "timestamp": 2.5600392879488316 + "heading": 2.4600717676583652e-21, + "angularVelocity": 0.1438977749227231, + "velocityX": -0.21544390526203344, + "velocityY": 0.24886908799668503, + "timestamp": 2.4173631047409017 }, { - "x": 1.8129411935806274, + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 6.021257273210334e-18, - "angularVelocity": -5.990893004521945e-18, - "velocityX": -3.48414823730851e-16, - "velocityY": -1.72070697740397e-16, - "timestamp": 2.635518672715027 + "heading": 1.1389483058862038e-21, + "angularVelocity": -1.912999393800189e-21, + "velocityX": -4.5659699774939935e-20, + "velocityY": -6.062533120754166e-20, + "timestamp": 2.4932366561975288 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj index b093daa2..0e534664 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj @@ -1,103 +1,103 @@ { "samples": [ { - "x": 1.8129411935806274, + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 6.021257273210334e-18, - "angularVelocity": -5.990893004521945e-18, - "velocityX": -3.48414823730851e-16, - "velocityY": -1.72070697740397e-16, + "heading": 1.1389483058862038e-21, + "angularVelocity": -1.912999393800189e-21, + "velocityX": -4.5659699774939935e-20, + "velocityY": -6.062533120754166e-20, "timestamp": 0 }, { - "x": 1.8467557558007979, - "y": 5.543052711491933, - "heading": -1.0880372931867694e-19, - "angularVelocity": -6.586811222708636e-17, - "velocityX": 0.3633408180161408, - "velocityY": 0.000574260143737945, - "timestamp": 0.09306568528358428 + "x": 1.8507557561824508, + "y": 5.5430527114919315, + "heading": -3.663099117513338e-19, + "angularVelocity": -3.946860070844576e-18, + "velocityX": 0.4061756707874381, + "velocityY": 0.0005740544395093343, + "timestamp": 0.09309903404237341 }, { - "x": 1.914384879084952, - "y": 5.543159599317721, - "heading": -3.505509945985968e-18, - "angularVelocity": -3.6497944505922797e-17, - "velocityX": 0.7266816236089445, - "velocityY": 0.0011485202678409793, - "timestamp": 0.18613137056716855 + "x": 1.9263848800936068, + "y": 5.543159599317717, + "heading": -1.3979636927804314e-18, + "angularVelocity": -1.108125118245985e-17, + "velocityX": 0.8123513276919049, + "velocityY": 0.0011481088593976478, + "timestamp": 0.18619806808474682 }, { - "x": 2.015828561351954, - "y": 5.543319931052201, - "heading": -4.327530361987157e-18, - "angularVelocity": -8.832690735719102e-18, - "velocityX": 1.090022406839742, - "velocityY": 0.0017227803566008666, - "timestamp": 0.27919705585075283 + "x": 2.039828562987611, + "y": 5.543319931052194, + "heading": -3.639885752731889e-18, + "angularVelocity": -2.408104534113515e-17, + "velocityX": 1.2185269596070252, + "velocityY": 0.0017221632439681264, + "timestamp": 0.2792971021271202 }, { - "x": 2.1510867977458235, - "y": 5.543533706687698, - "heading": -2.821606902688151e-18, - "angularVelocity": 1.6181296623948357e-17, - "velocityX": 1.453363137892531, - "velocityY": 0.0022970403628934204, - "timestamp": 0.3722627411343371 + "x": 2.1910867994360017, + "y": 5.543533706687691, + "heading": -8.31799511662456e-18, + "angularVelocity": -5.0248742235094303e-17, + "velocityX": 1.6247025332136762, + "velocityY": 0.002296217546130332, + "timestamp": 0.37239613616949363 }, { - "x": 2.3201595639866595, - "y": 5.5438009261858365, - "heading": -4.181254623727939e-19, - "angularVelocity": 2.582564597014347e-17, - "velocityX": 1.8167036080554047, - "velocityY": 0.002871299956849509, - "timestamp": 0.4653284264179214 + "x": 2.380159562296481, + "y": 5.5438009261858445, + "heading": -4.964025869598509e-18, + "angularVelocity": 3.602582219554166e-17, + "velocityX": 2.0308778152781284, + "velocityY": 0.0028702714362513803, + "timestamp": 0.46549517021186704 }, { - "x": 2.4554178003805287, - "y": 5.544014701821333, - "heading": 5.073319506573115e-18, - "angularVelocity": 5.900611973144966e-17, - "velocityX": 1.4533631378925314, - "velocityY": 0.0022970403628935166, - "timestamp": 0.5583941117015057 + "x": 2.5314177987448714, + "y": 5.544014701821341, + "heading": -1.164993974430083e-17, + "angularVelocity": -7.181507244960338e-17, + "velocityX": 1.6247025332136764, + "velocityY": 0.0022962175461303325, + "timestamp": 0.5585942042542404 }, { - "x": 2.556861482647531, - "y": 5.544175033555812, - "heading": 6.802500422384327e-18, - "angularVelocity": 1.8580220092148724e-17, - "velocityX": 1.0900224068397424, - "velocityY": 0.001722780356600995, - "timestamp": 0.6514597969850899 + "x": 2.6448614816388756, + "y": 5.544175033555818, + "heading": -5.397697309971077e-18, + "angularVelocity": 6.71568990871082e-17, + "velocityX": 1.218526959607025, + "velocityY": 0.0017221632439681266, + "timestamp": 0.6516932382966139 }, { - "x": 2.6244906059316855, - "y": 5.544281921381602, - "heading": 5.0686461896360996e-18, - "angularVelocity": -1.8630435349857743e-17, - "velocityX": 0.726681623608945, - "velocityY": 0.0011485202678411268, - "timestamp": 0.7445254822686742 + "x": 2.7204906055500317, + "y": 5.544281921381604, + "heading": -1.7112683408390196e-18, + "angularVelocity": 3.959685518772811e-17, + "velocityX": 0.8123513276919049, + "velocityY": 0.001148108859397648, + "timestamp": 0.7447922723389873 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -4.658926801043138e-23, - "angularVelocity": -5.446360561015685e-17, - "velocityX": 0.36334081801614126, - "velocityY": 0.0005742601437381063, - "timestamp": 0.8375911675522585 + "heading": 2.8361562565882696e-27, + "angularVelocity": 1.838116110697215e-17, + "velocityX": 0.4061756707874381, + "velocityY": 0.0005740544395093341, + "timestamp": 0.8378913063813607 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -4.8718739199241653e-23, - "angularVelocity": -2.2883735459903696e-23, - "velocityX": 1.6624416710559118e-21, - "velocityY": 9.919860077979884e-23, - "timestamp": 0.9306568528358428 + "heading": 2.9956768552498905e-27, + "angularVelocity": 1.2964344677835282e-27, + "velocityX": -1.3759431197263344e-22, + "velocityY": -2.5705739302596115e-25, + "timestamp": 0.9309903404237341 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj index 1abb5ec2..042ccb78 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj @@ -1,202 +1,193 @@ { "samples": [ { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -4.8718739199241653e-23, - "angularVelocity": -2.2883735459903696e-23, - "velocityX": 1.6624416710559118e-21, - "velocityY": 9.919860077979884e-23, + "heading": 2.9956768552498905e-27, + "angularVelocity": 1.2964344677835282e-27, + "velocityX": -1.3759431197263344e-22, + "velocityY": -2.5705739302596115e-25, "timestamp": 0 }, { - "x": 2.6433995634924674, - "y": 5.563703580385797, - "heading": 0.004845900450156495, - "angularVelocity": 0.061212191042318315, - "velocityX": -0.18828383484070044, - "velocityY": 0.24465440313021325, - "timestamp": 0.07916561011198464 - }, - { - "x": 2.613808012454941, - "y": 5.602604430475709, - "heading": 0.014722463610686642, - "angularVelocity": 0.12475825230878822, - "velocityX": -0.37379300172975427, - "velocityY": 0.4913857170415791, - "timestamp": 0.15833122022396928 - }, - { - "x": 2.5698581719130473, - "y": 5.6612759426030985, - "heading": 0.02990473352989547, - "angularVelocity": 0.19177860055310075, - "velocityX": -0.5551632897128445, - "velocityY": 0.7411237284017034, - "timestamp": 0.23749683033595392 - }, - { - "x": 2.512090436903312, - "y": 5.74009257508762, - "heading": 0.05083960434186738, - "angularVelocity": 0.2644440026718447, - "velocityX": -0.7297074440280276, - "velocityY": 0.9955918027162334, - "timestamp": 0.31666244044793856 - }, - { - "x": 2.4415596148175696, - "y": 5.839724514645118, - "heading": 0.07837099812745095, - "angularVelocity": 0.34776961545093493, - "velocityX": -0.8909275376766785, - "velocityY": 1.2585255064233432, - "timestamp": 0.3958280505599232 - }, - { - "x": 2.3611631890754614, - "y": 5.961650149114452, - "heading": 0.11464704720334487, - "angularVelocity": 0.4582298933157867, - "velocityX": -1.0155473522958047, - "velocityY": 1.5401338320624671, - "timestamp": 0.47499366067190785 - }, - { - "x": 2.2902542502823713, - "y": 6.1057913956854275, - "heading": 0.1670437133636231, - "angularVelocity": 0.6618614583549578, - "velocityX": -0.8957038124608023, - "velocityY": 1.8207558353567832, - "timestamp": 0.5541592707838925 - }, - { - "x": 2.241416295543653, - "y": 6.239392989227262, - "heading": 0.22223140963064505, - "angularVelocity": 0.6971170460122205, - "velocityX": -0.6169087141453696, - "velocityY": 1.6876215992379322, - "timestamp": 0.6333248808958771 - }, - { - "x": 2.2118788438375976, - "y": 6.357956546172331, - "heading": 0.2767017364027319, - "angularVelocity": 0.688055415666413, - "velocityX": -0.3731096326330706, - "velocityY": 1.4976649175993666, - "timestamp": 0.7124904910078618 - }, - { - "x": 2.2005734730100426, - "y": 6.460208388316336, - "heading": 0.32930652128307913, - "angularVelocity": 0.664490361483155, - "velocityX": -0.14280659002770119, - "velocityY": 1.2916194544495236, - "timestamp": 0.7916561011198464 - }, - { - "x": 2.206951813042022, - "y": 6.545559671097223, - "heading": 0.37948004279494146, - "angularVelocity": 0.6337792564333017, - "velocityX": 0.08056958094502178, - "velocityY": 1.0781358554573413, - "timestamp": 0.870821711231831 - }, - { - "x": 2.2306819541123337, - "y": 6.613672661059128, - "heading": 0.4268844504712619, - "angularVelocity": 0.598800509580661, - "velocityX": 0.29975315085356163, - "velocityY": 0.8603860927182078, - "timestamp": 0.9499873213438157 - }, - { - "x": 2.271541794524346, - "y": 6.6643287076043505, - "heading": 0.4712947460181937, - "angularVelocity": 0.5609796410854498, - "velocityX": 0.516131188204235, - "velocityY": 0.6398743908316679, - "timestamp": 1.0291529314558003 + "x": 2.7425045239965504, + "y": 5.564231116573788, + "heading": 0.0035475080989150498, + "angularVelocity": 0.04647392993401548, + "velocityX": -0.20699544832908057, + "velocityY": 0.26064316841975155, + "timestamp": 0.07633329275040568 + }, + { + "x": 2.7110743119163434, + "y": 5.604155225406367, + "heading": 0.01083868300587181, + "angularVelocity": 0.09551762598264181, + "velocityX": -0.4117497221425694, + "velocityY": 0.523023537883034, + "timestamp": 0.15266658550081136 + }, + { + "x": 2.6642693453223805, + "y": 5.6643007250236845, + "heading": 0.022164936239812177, + "angularVelocity": 0.14837894221299866, + "velocityX": -0.6131658272230635, + "velocityY": 0.7879327283048184, + "timestamp": 0.22899987825121704 + }, + { + "x": 2.60250917282708, + "y": 5.744974159519079, + "heading": 0.038001263541227126, + "angularVelocity": 0.20746291337380784, + "velocityX": -0.8090856593497568, + "velocityY": 1.0568577823464311, + "timestamp": 0.3053331710016227 + }, + { + "x": 2.526611736042362, + "y": 5.846734963341242, + "heading": 0.05925383993510262, + "angularVelocity": 0.27841817938297336, + "velocityX": -0.994290093483679, + "velocityY": 1.333111675856818, + "timestamp": 0.3816664637520284 + }, + { + "x": 2.4388385302471143, + "y": 5.970894906525885, + "heading": 0.08830658815011594, + "angularVelocity": 0.3806038907558967, + "velocityX": -1.149867936160551, + "velocityY": 1.6265503387967306, + "timestamp": 0.4579997565024341 + }, + { + "x": 2.357791815962569, + "y": 6.119054185868648, + "heading": 0.1383597950882533, + "angularVelocity": 0.655719216800998, + "velocityX": -1.0617479131884362, + "velocityY": 1.9409522896806517, + "timestamp": 0.5343330492528398 + }, + { + "x": 2.2985967230184703, + "y": 6.254273676601559, + "heading": 0.19308840139911443, + "angularVelocity": 0.7169690228065029, + "velocityX": -0.7754819792414106, + "velocityY": 1.7714353182044804, + "timestamp": 0.6106663420032454 + }, + { + "x": 2.2587354140798723, + "y": 6.372988574603886, + "heading": 0.24867936598579424, + "angularVelocity": 0.7282662988016366, + "velocityX": -0.5222008314109583, + "velocityY": 1.5552178312351754, + "timestamp": 0.6869996347536516 + }, + { + "x": 2.2373189281980324, + "y": 6.474209987526736, + "heading": 0.30392383910912424, + "angularVelocity": 0.7237271069121585, + "velocityX": -0.2805654663931197, + "velocityY": 1.3260454157771489, + "timestamp": 0.7633329275040577 + }, + { + "x": 2.233901000873839, + "y": 6.557480302062636, + "heading": 0.35823088728392755, + "angularVelocity": 0.7114464242015108, + "velocityX": -0.044776364297153176, + "velocityY": 1.090878062972822, + "timestamp": 0.8396662202544638 + }, + { + "x": 2.248214127214941, + "y": 6.6225364653992, + "heading": 0.41124898234582946, + "angularVelocity": 0.6945605665834471, + "velocityX": 0.1875083050315006, + "velocityY": 0.8522646016238898, + "timestamp": 0.9159995130048699 + }, + { + "x": 2.280080256208058, + "y": 6.669207835860357, + "heading": 0.4627444117016496, + "angularVelocity": 0.6746129703090314, + "velocityX": 0.41746042709454745, + "velocityY": 0.6114156586138921, + "timestamp": 0.992332805755276 }, { "x": 2.3293724060058594, "y": 6.6973748207092285, "heading": 0.5125504196, - "angularVelocity": 0.5211312528691123, - "velocityX": 0.7305016837451087, - "velocityY": 0.4174301575915679, - "timestamp": 1.108318541567785 + "angularVelocity": 0.652480799710893, + "velocityX": 0.6457490306225382, + "velocityY": 0.3689999976939454, + "timestamp": 1.0686660985056822 }, { - "x": 2.3973241454559178, - "y": 6.73620444415628, - "heading": 0.5391342040901619, - "angularVelocity": 0.37821212966035134, - "velocityX": 0.9667612262295853, - "velocityY": 0.5524358122619254, - "timestamp": 1.178606568855633 + "x": 2.4053669705551046, + "y": 6.740800351083451, + "heading": 0.5568713225103131, + "angularVelocity": 0.5531681306272371, + "velocityX": 0.948486345023332, + "velocityY": 0.5419930073900953, + "timestamp": 1.1487880369479013 }, { - "x": 2.48097335716082, - "y": 6.784004065158346, - "heading": 0.5372874666531788, - "angularVelocity": -0.02627385499695578, - "velocityX": 1.1900918966232588, - "velocityY": 0.6800535289789074, - "timestamp": 1.2488945961434812 + "x": 2.502567960679766, + "y": 6.796343857044693, + "heading": 0.5404821418694866, + "angularVelocity": -0.204552972125677, + "velocityX": 1.213163236118634, + "velocityY": 0.6932371712561282, + "timestamp": 1.2289099753901205 }, { - "x": 2.5479307987525304, - "y": 6.822265517551258, - "heading": 0.5291862310389086, - "angularVelocity": -0.11525768935146673, - "velocityX": 0.952615177510983, - "velocityY": 0.5443523437671909, - "timestamp": 1.3191826234313293 + "x": 2.575459179910046, + "y": 6.837996044585651, + "heading": 0.5268532537172577, + "angularVelocity": -0.17010182750455535, + "velocityX": 0.9097535662201399, + "velocityY": 0.5198599578440831, + "timestamp": 1.3090319138323396 }, { - "x": 2.5981417297757714, - "y": 6.850957521028096, - "heading": 0.5214396908605601, - "angularVelocity": -0.11021137563904544, - "velocityX": 0.7143596564122449, - "velocityY": 0.4082061281836414, - "timestamp": 1.3894706507191774 - }, - { - "x": 2.6316113826745062, - "y": 6.870083065561309, - "heading": 0.5156519889527618, - "angularVelocity": -0.08234264256835937, - "velocityX": 0.4761785782046117, - "velocityY": 0.27210245145860107, - "timestamp": 1.4597586780070255 + "x": 2.6240500842159764, + "y": 6.865762317125851, + "heading": 0.5173922106041889, + "angularVelocity": -0.11808305311898519, + "velocityX": 0.6064619160577619, + "velocityY": 0.34655018438206425, + "timestamp": 1.3891538522745588 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.044126567104521494, - "velocityX": 0.23806521146364915, - "velocityY": 0.13603746705807154, - "timestamp": 1.5300467052948736 + "angularVelocity": -0.06043027787801988, + "velocityX": 0.30321823224102623, + "velocityY": 0.17326782030138713, + "timestamp": 1.469275790716778 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": 5.036255157215061e-18, - "velocityX": -7.194949871952118e-18, - "velocityY": -3.0975432357907215e-18, - "timestamp": 1.6003347325827217 + "angularVelocity": 1.1533672139704627e-20, + "velocityX": 1.3759845110959627e-18, + "velocityY": -2.227992646788969e-18, + "timestamp": 1.549397729158997 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj index aed06347..0cbffe6a 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj @@ -4,901 +4,847 @@ "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": 5.036255157215061e-18, - "velocityX": -7.194949871952118e-18, - "velocityY": -3.0975432357907215e-18, + "angularVelocity": 1.1533672139704627e-20, + "velocityX": 1.3759845110959627e-18, + "velocityY": -2.227992646788969e-18, "timestamp": 0 }, { - "x": 2.654709594748088, - "y": 6.868188525457388, - "heading": 0.5112832103988345, - "angularVelocity": -0.02186863812315321, - "velocityX": 0.10984420500359952, - "velocityY": -0.19770584790906034, - "timestamp": 0.05794641596003114 - }, - { - "x": 2.6675869589575, - "y": 6.845358903138629, - "heading": 0.5087434985964555, - "angularVelocity": -0.0438286261591562, - "velocityX": 0.2222288297915463, - "velocityY": -0.393978159658854, - "timestamp": 0.11589283192006228 - }, - { - "x": 2.6871400637397453, - "y": 6.811251205912889, - "heading": 0.5049256931937168, - "angularVelocity": -0.06588509987168945, - "velocityX": 0.3374342391724767, - "velocityY": -0.5886075378547303, - "timestamp": 0.17383924788009342 - }, - { - "x": 2.7135513143277277, - "y": 6.765975453621827, - "heading": 0.4998239367817842, - "angularVelocity": -0.08804265677871101, - "velocityX": 0.45578747452127555, - "velocityY": -0.7813382681388139, - "timestamp": 0.23178566384012456 - }, - { - "x": 2.747025360653779, - "y": 6.709659972264173, - "heading": 0.49343216509494003, - "angularVelocity": -0.1103048666762139, - "velocityX": 0.5776724197945273, - "velocityY": -0.9718544352509818, - "timestamp": 0.2897320798001557 - }, - { - "x": 2.7877931285642683, - "y": 6.642455993729292, - "heading": 0.4857442111344985, - "angularVelocity": -0.1326735024601774, - "velocityX": 0.703542526920187, - "velocityY": -1.159760744844629, - "timestamp": 0.34767849576018683 - }, - { - "x": 2.836116770448945, - "y": 6.564543815779258, - "heading": 0.4767539799591755, - "angularVelocity": -0.15514732061277248, - "velocityX": 0.8339366824344734, - "velocityY": -1.3445555977745738, - "timestamp": 0.405624911720218 - }, - { - "x": 2.8922957481362634, - "y": 6.4761411900512496, - "heading": 0.4664557366916879, - "angularVelocity": -0.17772010739333818, - "velocityX": 0.969498747361149, - "velocityY": -1.5255926404315645, - "timestamp": 0.4635713276802491 - }, - { - "x": 2.9566742703700464, - "y": 6.377514946767211, - "heading": 0.4548445780033014, - "angularVelocity": -0.2003775125002176, - "velocityX": 1.111000933658922, - "velocityY": -1.7020249078401652, - "timestamp": 0.5215177436402803 - }, - { - "x": 3.0296502478902108, - "y": 6.268997399320861, - "heading": 0.44191720438373994, - "angularVelocity": -0.2230918583207183, - "velocityX": 1.2593699940044558, - "velocityY": -1.8727223357731155, - "timestamp": 0.5794641596003114 - }, - { - "x": 3.1116856852367225, - "y": 6.1510099051267035, - "heading": 0.4276731903359535, - "angularVelocity": -0.24581354708112682, - "velocityX": 1.4157120157888006, - "velocityY": -2.036148262828567, - "timestamp": 0.6374105755603425 - }, - { - "x": 3.2033177251351583, - "y": 6.024097216910698, - "heading": 0.41211708268393216, - "angularVelocity": -0.26845676983265077, - "velocityX": 1.5813236829287303, - "velocityY": -2.1901732162959853, - "timestamp": 0.6953569915203737 - }, - { - "x": 3.3051677702115847, - "y": 5.888977978331293, - "heading": 0.3952618666988256, - "angularVelocity": -0.2908759015695001, - "velocityX": 1.7576590957183351, - "velocityY": -2.3317963042374306, - "timestamp": 0.7533034074804048 - }, - { - "x": 3.417941892615677, - "y": 5.746618387863477, - "heading": 0.37713462149882643, - "angularVelocity": -0.31282772022506217, - "velocityX": 1.9461794234501006, - "velocityY": -2.456745393295919, - "timestamp": 0.811249823440436 - }, - { - "x": 3.542406775687406, - "y": 5.598335086950221, - "heading": 0.35778539162317, - "angularVelocity": -0.3339159041172737, - "velocityX": 2.147930652994672, - "velocityY": -2.5589727760822365, - "timestamp": 0.8691962394004671 - }, - { - "x": 3.679310043544789, - "y": 5.445921240793454, - "heading": 0.337299824049247, - "angularVelocity": -0.3535260504816983, - "velocityX": 2.3625838732082087, - "velocityY": -2.6302549283098875, - "timestamp": 0.9271426553604982 - }, - { - "x": 3.8292016626883134, - "y": 5.291748474573123, - "heading": 0.3158135059859917, - "angularVelocity": -0.3707963246264704, - "velocityX": 2.5867280427993067, - "velocityY": -2.660609179464564, - "timestamp": 0.9850890713205294 - }, - { - "x": 3.9921508426109233, - "y": 5.138721146636673, - "heading": 0.29352014457634995, - "angularVelocity": -0.3847237320945793, - "velocityX": 2.8120665829446008, - "velocityY": -2.640841981357415, - "timestamp": 1.0430354872805605 + "x": 2.6571120181256966, + "y": 6.867792271481583, + "heading": 0.507044677895467, + "angularVelocity": -0.09452799306194601, + "velocityX": 0.15052909368010856, + "velocityY": -0.20349709127294008, + "timestamp": 0.058244563606941746 + }, + { + "x": 2.6746521578330125, + "y": 6.844087252656105, + "heading": 0.49617547030513726, + "angularVelocity": -0.18661325482116445, + "velocityX": 0.30114638381847736, + "velocityY": -0.40699109680773127, + "timestamp": 0.11648912721388349 + }, + { + "x": 2.7009708210230947, + "y": 6.808530072252695, + "heading": 0.4801087501886783, + "angularVelocity": -0.27584926594838455, + "velocityX": 0.45186471595342564, + "velocityY": -0.6104806732412771, + "timestamp": 0.17473369082082524 + }, + { + "x": 2.73607484037394, + "y": 6.7611211343580875, + "heading": 0.4590398225372461, + "angularVelocity": -0.361732088742461, + "velocityX": 0.602700358229852, + "velocityY": -0.8139633119159942, + "timestamp": 0.23297825442776698 + }, + { + "x": 2.7799722580023465, + "y": 6.701861088549211, + "heading": 0.4332010241003249, + "angularVelocity": -0.4436259255248601, + "velocityX": 0.7536740754835817, + "velocityY": -1.0174347980145875, + "timestamp": 0.29122281803470873 + }, + { + "x": 2.832672676333526, + "y": 6.630750980608444, + "heading": 0.40287238280523807, + "angularVelocity": -0.5207119672104833, + "velocityX": 0.9048126566253185, + "velocityY": -1.2208883290919084, + "timestamp": 0.3494673816416505 + }, + { + "x": 2.8941877403139116, + "y": 6.547792490679094, + "heading": 0.3683969681281308, + "angularVelocity": -0.5919078544353333, + "velocityX": 1.0561511696699046, + "velocityY": -1.4243130138151228, + "timestamp": 0.4077119452485922 + }, + { + "x": 2.9645318213846927, + "y": 6.452988329970027, + "heading": 0.33020407419243936, + "angularVelocity": -0.6557331975810253, + "velocityX": 1.2077364257631285, + "velocityY": -1.627691149835798, + "timestamp": 0.46595650885553397 + }, + { + "x": 3.04372302387486, + "y": 6.346342948824998, + "heading": 0.28884632079593436, + "angularVelocity": -0.7100706200771626, + "velocityX": 1.3596325147971153, + "velocityY": -1.8309928779742386, + "timestamp": 0.5242010724624757 + }, + { + "x": 3.1317847243622103, + "y": 6.227863919029491, + "heading": 0.24506332676078993, + "angularVelocity": -0.7517095386036364, + "velocityX": 1.5119299559290544, + "velocityY": -2.0341646062463727, + "timestamp": 0.5824456360694175 + }, + { + "x": 3.2287480012560774, + "y": 6.097564965294096, + "heading": 0.199900944291634, + "angularVelocity": -0.7753922370151164, + "velocityX": 1.6647609817838902, + "velocityY": -2.237100695177408, + "timestamp": 0.6406901996763592 + }, + { + "x": 3.334655409720646, + "y": 5.955473736284568, + "heading": 0.1549620363959118, + "angularVelocity": -0.7715554055652005, + "velocityX": 1.8183226365858802, + "velocityY": -2.4395620845993164, + "timestamp": 0.698934763283301 + }, + { + "x": 3.4495650121656865, + "y": 5.8016567423537415, + "heading": 0.11303057394210353, + "angularVelocity": -0.7199206218932102, + "velocityX": 1.972881163991502, + "velocityY": -2.6408815588154475, + "timestamp": 0.7571793268902427 + }, + { + "x": 3.5735262627096924, + "y": 5.636333431818966, + "heading": 0.08010242298971296, + "angularVelocity": -0.565342907787981, + "velocityX": 2.1282887683826965, + "velocityY": -2.838433328309314, + "timestamp": 0.8154238904971844 + }, + { + "x": 3.7050107789119795, + "y": 5.46057947422046, + "heading": 0.07776405618458973, + "angularVelocity": -0.04014738303996017, + "velocityX": 2.2574555985962665, + "velocityY": -3.017516944320591, + "timestamp": 0.8736684541041262 + }, + { + "x": 3.8485069507845076, + "y": 5.2934525910716275, + "heading": 0.07776403563237232, + "angularVelocity": -3.5286069899218235e-7, + "velocityX": 2.463683526601701, + "velocityY": -2.8693988382619477, + "timestamp": 0.9319130177110679 + }, + { + "x": 4.002902013863093, + "y": 5.136338903873299, + "heading": 0.07776401516857159, + "angularVelocity": -3.5134267388587325e-7, + "velocityX": 2.650806418956214, + "velocityY": -2.697482433873059, + "timestamp": 0.9901575813180097 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.2706621692829818, - "angularVelocity": -0.3944674560916278, - "velocityX": 3.026040866948455, - "velocityY": -2.5674297245815976, - "timestamp": 1.1009819032405916 - }, - { - "x": 4.262820189871526, - "y": 4.913621110649205, - "heading": 0.2585516106232336, - "angularVelocity": -0.39720693073137303, - "velocityX": 3.1263802397054365, - "velocityY": -2.5033930490319927, - "timestamp": 1.1314711966610673 - }, - { - "x": 4.3609649027366135, - "y": 4.839574515326335, - "heading": 0.24639337135429903, - "angularVelocity": -0.3987707783588398, - "velocityX": 3.218989417418724, - "velocityY": -2.4286097516822993, - "timestamp": 1.161960490081543 - }, - { - "x": 4.461671433105771, - "y": 4.768098656214637, - "heading": 0.234223352498736, - "angularVelocity": -0.3991571299366988, - "velocityX": 3.3030129291721897, - "velocityY": -2.344293720617927, - "timestamp": 1.1924497835020187 - }, - { - "x": 4.564664131810475, - "y": 4.699441485698375, - "heading": 0.22207645249799987, - "angularVelocity": -0.3983988685196772, - "velocityX": 3.3779955896103364, - "velocityY": -2.251845248409488, - "timestamp": 1.2229390769224944 - }, - { - "x": 4.669665105994921, - "y": 4.6338072685219265, - "heading": 0.20998565945136316, - "angularVelocity": -0.39655865027233667, - "velocityX": 3.4438638093832386, - "velocityY": -2.1526972196859626, - "timestamp": 1.25342837034297 - }, - { - "x": 4.7764036789009445, - "y": 4.571359258447394, - "heading": 0.19798140822304883, - "angularVelocity": -0.39372021721567657, - "velocityX": 3.500854264938127, - "velocityY": -2.0481947289927476, - "timestamp": 1.2839176637634457 - }, - { - "x": 4.88462301845396, - "y": 4.512224581779958, - "heading": 0.1860912294102901, - "angularVelocity": -0.3899788246561105, - "velocityX": 3.5494210397259125, - "velocityY": -1.9395226990639944, - "timestamp": 1.3144069571839214 - }, - { - "x": 4.994084052884085, - "y": 4.456499991660438, - "heading": 0.17433965070276516, - "angularVelocity": -0.3854329631526573, - "velocityX": 3.590146643300487, - "velocityY": -1.827677321052551, - "timestamp": 1.344896250604397 - }, - { - "x": 5.1045672063720575, - "y": 4.404257556871473, - "heading": 0.162748286386498, - "angularVelocity": -0.380178187679756, - "velocityX": 3.6236705116221697, - "velocityY": -1.71346820237825, - "timestamp": 1.3753855440248728 - }, - { - "x": 5.215872594429427, - "y": 4.35554976560558, - "heading": 0.15133604569057543, - "angularVelocity": -0.3743032197731828, - "velocityX": 3.650638488808694, - "velocityY": -1.5975375550415924, - "timestamp": 1.4058748374453485 - }, - { - "x": 5.327819247886724, - "y": 4.310413837972956, - "heading": 0.14011940090490202, - "angularVelocity": -0.36788798713571186, - "velocityX": 3.6716709670326204, - "velocityY": -1.4803861476931233, - "timestamp": 1.4363641308658242 - }, - { - "x": 5.440243798322744, - "y": 4.268875232037166, - "heading": 0.1291126733064667, - "angularVelocity": -0.36100303954643975, - "velocityX": 3.6873452226518335, - "velocityY": -1.3623997566271735, - "timestamp": 1.4668534242862998 - }, - { - "x": 5.552998921854952, - "y": 4.230950423566602, - "heading": 0.1183283109495842, - "angularVelocity": -0.3537098157093855, - "velocityX": 3.6981874908418817, - "velocityY": -1.243872986741426, - "timestamp": 1.4973427177067755 + "heading": 0.07776399412255879, + "angularVelocity": -3.6133866407320583e-7, + "velocityX": 2.8259641989395843, + "velocityY": -2.513386639709188, + "timestamp": 1.0484021449249514 + }, + { + "x": 4.268976588700125, + "y": 4.906724140265527, + "heading": 0.07776397443770015, + "angularVelocity": -5.672615420255969e-7, + "velocityX": 2.9242931040000055, + "velocityY": -2.398268643648094, + "timestamp": 1.0831037017429557 + }, + { + "x": 4.37370366170879, + "y": 4.827628597038176, + "heading": 0.07776395550809555, + "angularVelocity": -5.454972726636976e-7, + "velocityX": 3.0179358683506776, + "velocityY": -2.2793082063198216, + "timestamp": 1.11780525856096 + }, + { + "x": 4.4815124411311835, + "y": 4.752787886123784, + "heading": 0.0777639371726549, + "angularVelocity": -5.283751607380448e-7, + "velocityX": 3.106741867167675, + "velocityY": -2.1566960614159445, + "timestamp": 1.1525068153789642 + }, + { + "x": 4.592230128734662, + "y": 4.682321875197502, + "heading": 0.07776391928788347, + "angularVelocity": -5.153881575305611e-7, + "velocityX": 3.1905683132358433, + "velocityY": -2.030629671626745, + "timestamp": 1.1872083721969684 + }, + { + "x": 4.705679213251417, + "y": 4.616343315466854, + "heading": 0.07776390172302422, + "angularVelocity": -5.061691995708879e-7, + "velocityX": 3.269279390309479, + "velocityY": -1.9013141132738893, + "timestamp": 1.2219099290149726 + }, + { + "x": 4.821677528011508, + "y": 4.554957192131842, + "heading": 0.07776388435588834, + "angularVelocity": -5.004713755679868e-7, + "velocityX": 3.3427409429626933, + "velocityY": -1.7689731805681452, + "timestamp": 1.2566114858329769 + }, + { + "x": 4.939562213059443, + "y": 4.4972764656411774, + "heading": 0.07776386706661859, + "angularVelocity": -4.982274959213974e-7, + "velocityX": 3.397100760239454, + "velocityY": -1.6621942004843482, + "timestamp": 1.2913130426509811 + }, + { + "x": 5.0574486320523055, + "y": 4.439599283026335, + "heading": 0.07776384977740043, + "angularVelocity": -4.982260088149746e-7, + "velocityX": 3.397150727592071, + "velocityY": -1.6620920760798994, + "timestamp": 1.3260145994689854 + }, + { + "x": 5.175337486723699, + "y": 4.381927078962276, + "heading": 0.07776383248820665, + "angularVelocity": -4.9822530674329e-7, + "velocityX": 3.3972209169079175, + "velocityY": -1.6619486084306254, + "timestamp": 1.3607161562869896 + }, + { + "x": 5.295110440251067, + "y": 4.328277244219036, + "heading": 0.07776381519347733, + "angularVelocity": -4.983848248451832e-7, + "velocityX": 3.451515277989629, + "velocityY": -1.5460353846547075, + "timestamp": 1.3954177131049939 + }, + { + "x": 5.416934261342807, + "y": 4.279463089154562, + "heading": 0.07776379778914431, + "angularVelocity": -5.015432909319555e-7, + "velocityX": 3.5106154381100505, + "velocityY": -1.4066848735485782, + "timestamp": 1.430119269922998 + }, + { + "x": 5.540614064021598, + "y": 4.235563528216825, + "heading": 0.07776378015516597, + "angularVelocity": -5.081610154974924e-7, + "velocityX": 3.5640995396097326, + "velocityY": -1.26506027288553, + "timestamp": 1.4648208267410023 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.10777714440346366, - "angularVelocity": -0.3460613665458462, - "velocityX": 3.704671190903868, - "velocityY": -1.1250293189517762, - "timestamp": 1.5278320111272512 - }, - { - "x": 5.798356559097709, - "y": 4.161450057515513, - "heading": 0.09575175638596573, - "angularVelocity": -0.3367399103461797, - "velocityX": 3.7076550554500285, - "velocityY": -0.9856574960167961, - "timestamp": 1.5635432162681608 - }, - { - "x": 5.9304803491986915, - "y": 4.131221114350942, - "heading": 0.0841228883191355, - "angularVelocity": -0.32563639398191835, - "velocityX": 3.699785251705895, - "velocityY": -0.8464834229282695, - "timestamp": 1.5992544214090705 - }, - { - "x": 6.061872954800802, - "y": 4.105915842420486, - "heading": 0.07296106661592697, - "angularVelocity": -0.3125579677070111, - "velocityX": 3.679310319650609, - "velocityY": -0.7086087358465492, - "timestamp": 1.6349656265499801 - }, - { - "x": 6.19201941031956, - "y": 4.085429275069175, - "heading": 0.062341869522057725, - "angularVelocity": -0.29736316800199314, - "velocityX": 3.6444151073934408, - "velocityY": -0.5736733686380603, - "timestamp": 1.6706768316908898 - }, - { - "x": 6.320346294042347, - "y": 4.069574852509192, - "heading": 0.052342014729276326, - "angularVelocity": -0.28002008762580527, - "velocityX": 3.593462702152777, - "velocityY": -0.4439621261008996, - "timestamp": 1.7063880368317994 - }, - { - "x": 6.446241767080791, - "y": 4.05806211105344, - "heading": 0.043033222480069355, - "angularVelocity": -0.2606686672285132, - "velocityX": 3.525377330215658, - "velocityY": -0.32238456838199153, - "timestamp": 1.742099241972709 - }, - { - "x": 6.569091568872717, - "y": 4.050483885589985, - "heading": 0.034475036219150636, - "angularVelocity": -0.2396498865593223, - "velocityX": 3.440091178866215, - "velocityY": -0.21220861725478038, - "timestamp": 1.7778104471136187 - }, - { - "x": 6.688325744738062, - "y": 4.046323173396873, - "heading": 0.026709152612686058, - "angularVelocity": -0.21746349852429026, - "velocityX": 3.338844919819132, - "velocityY": -0.11650999109930767, - "timestamp": 1.8135216522545283 - }, - { - "x": 6.803462305567311, - "y": 4.044983622354132, - "heading": 0.019757511591013074, - "angularVelocity": -0.19466273944659512, - "velocityX": 3.2241018015197396, - "velocityY": -0.03751066471868068, - "timestamp": 1.849232857395438 - }, - { - "x": 6.914132827604236, - "y": 4.045835628751743, - "heading": 0.013624262542277448, - "angularVelocity": -0.17174578747848585, - "velocityX": 3.099041928163472, - "velocityY": 0.023858237050529835, - "timestamp": 1.8849440625363476 - }, - { - "x": 7.020084299428127, - "y": 4.048262286463527, - "heading": 0.00829997269110531, - "angularVelocity": -0.1490929760045692, - "velocityX": 2.966897123909054, - "velocityY": 0.06795227722528199, - "timestamp": 1.9206552676772572 - }, - { - "x": 7.1211630661166305, - "y": 4.051692576367301, - "heading": 0.003766298606880027, - "angularVelocity": -0.1269538248942407, - "velocityX": 2.8304496106940285, - "velocityY": 0.09605640275200633, - "timestamp": 1.956366472818167 + "heading": 0.07776376214569573, + "angularVelocity": -5.189816214982998e-7, + "velocityX": 3.6118744025389584, + "velocityY": -1.1214036841768227, + "timestamp": 1.4995223835590066 + }, + { + "x": 5.801257373802826, + "y": 4.160893829303833, + "heading": 0.07776374465549807, + "angularVelocity": -4.7264774236323325e-7, + "velocityX": 3.6564428162530316, + "velocityY": -0.9662347026048084, + "timestamp": 1.536527104207333 + }, + { + "x": 5.937965681779902, + "y": 4.1309456443700405, + "heading": 0.07776372758116286, + "angularVelocity": -4.614096500770092e-7, + "velocityX": 3.6943477908205034, + "velocityY": -0.8093071480907553, + "timestamp": 1.5735318248556593 + }, + { + "x": 6.075827506190729, + "y": 4.1068588699437765, + "heading": 0.07776371078252527, + "angularVelocity": -4.53959313742904e-7, + "velocityX": 3.7255199335509808, + "velocityY": -0.6509108568923265, + "timestamp": 1.6105365455039857 + }, + { + "x": 6.2145915217996235, + "y": 4.088676368882122, + "heading": 0.0777636941280861, + "angularVelocity": -4.5006255582880887e-7, + "velocityX": 3.7499003688646537, + "velocityY": -0.4913562578799624, + "timestamp": 1.6475412661523121 + }, + { + "x": 6.354004262869319, + "y": 4.076423083568819, + "heading": 0.07776367749075697, + "angularVelocity": -4.496001822495745e-7, + "velocityX": 3.7674312527474547, + "velocityY": -0.33112762638453114, + "timestamp": 1.6845459868006385 + }, + { + "x": 6.49381285233259, + "y": 4.070129387117659, + "heading": 0.07776362451928018, + "angularVelocity": -0.0000014314788993599032, + "velocityX": 3.778128493170848, + "velocityY": -0.17007820464237144, + "timestamp": 1.7215507074489649 + }, + { + "x": 6.629142082966274, + "y": 4.065515785319137, + "heading": 0.06238171659626575, + "angularVelocity": -0.4156742073314349, + "velocityX": 3.657080184979125, + "velocityY": -0.12467603369760688, + "timestamp": 1.7585554280972913 + }, + { + "x": 6.758586834582697, + "y": 4.061933354582955, + "heading": 0.046880370354106624, + "angularVelocity": -0.4189018582108937, + "velocityX": 3.4980605000804377, + "velocityY": -0.0968101008038334, + "timestamp": 1.7955601487456176 + }, + { + "x": 6.882125546140559, + "y": 4.059204781585899, + "heading": 0.032519289448839375, + "angularVelocity": -0.3880878075461594, + "velocityX": 3.338458158674076, + "velocityY": -0.07373580854685112, + "timestamp": 1.832564869393944 + }, + { + "x": 6.999755813322696, + "y": 4.05726367776388, + "heading": 0.01976964076410941, + "angularVelocity": -0.34454114127480795, + "velocityX": 3.1787908440123913, + "velocityY": -0.0524555729109991, + "timestamp": 1.8695695900422704 + }, + { + "x": 7.111477612575904, + "y": 4.056075301962013, + "heading": 0.008878948528675913, + "angularVelocity": -0.2943054843984055, + "velocityX": 3.0191228928587734, + "velocityY": -0.03211416762637815, + "timestamp": 1.9065743106905968 }, { "x": 7.217291355133057, "y": 4.0556182861328125, - "heading": -1.0909016317355304e-17, - "angularVelocity": -0.10546545802684394, - "velocityX": 2.6918242786016986, - "velocityY": 0.10992935550682857, - "timestamp": 1.9920776779590765 - }, - { - "x": 7.368999916586432, - "y": 4.062586724556833, - "heading": -0.004298989973913876, - "angularVelocity": -0.06943407043774014, - "velocityX": 2.450283207423163, - "velocityY": 0.11254900507105683, - "timestamp": 2.05399238275474 - }, - { - "x": 7.505752960791366, - "y": 4.0691868405641705, - "heading": -0.006878875376785559, - "angularVelocity": -0.041668379287055086, - "velocityX": 2.208732879471196, - "velocityY": 0.1066001368999474, - "timestamp": 2.1159070875504034 - }, - { - "x": 7.627563527820959, - "y": 4.075012053328196, - "heading": -0.008132420768696622, - "angularVelocity": -0.02024632752507115, - "velocityX": 1.9673931650260594, - "velocityY": 0.09408447933734543, - "timestamp": 2.1778217923460668 - }, - { - "x": 7.734450186690855, - "y": 4.079741519123993, - "heading": -0.008370221734057366, - "angularVelocity": -0.0038407833186891597, - "velocityX": 1.726353363432043, - "velocityY": 0.07638679391923996, - "timestamp": 2.23973649714173 - }, - { - "x": 7.826433301006775, - "y": 4.083115946879114, - "heading": -0.007843874074281608, - "angularVelocity": 0.00850117369553605, - "velocityX": 1.485642459565828, - "velocityY": 0.05450123304726754, - "timestamp": 2.3016512019373936 - }, - { - "x": 7.903533270640132, - "y": 4.084921624600937, - "heading": -0.00676122659439774, - "angularVelocity": 0.01748611228070863, - "velocityX": 1.2452610391636303, - "velocityY": 0.029163955925833308, - "timestamp": 2.363565906733057 - }, - { - "x": 7.965769715964552, - "y": 4.0849794807879745, - "heading": -0.005296815188372533, - "angularVelocity": 0.023652077658421113, - "velocityX": 1.0051965123603401, - "velocityY": 0.000934449856933646, - "timestamp": 2.4254806115287204 - }, - { - "x": 8.013161126878536, - "y": 4.083137358866124, - "heading": -0.0035992352155136144, - "angularVelocity": 0.027418041941110364, - "velocityX": 0.7654306205672696, - "velocityY": -0.029752575384646835, - "timestamp": 2.487395316324384 - }, - { - "x": 8.045724744989489, - "y": 4.079264414164358, - "heading": -0.001796495631167538, - "angularVelocity": 0.02911650132703844, - "velocityX": 0.5259432023203758, - "velocityY": -0.06255290588153085, - "timestamp": 2.5493100211200472 + "heading": -2.4974876469169754e-20, + "angularVelocity": -0.23994096896600625, + "velocityX": 2.8594660546893573, + "velocityY": -0.012350203465742983, + "timestamp": 1.9435790313389232 + }, + { + "x": 7.386543487491071, + "y": 4.0570668323942645, + "heading": -0.009095721053457144, + "angularVelocity": -0.13843657850499821, + "velocityX": 2.57601194788322, + "velocityY": 0.022046826970953105, + "timestamp": 2.0092821941278585 + }, + { + "x": 7.537031363324535, + "y": 4.05969750834718, + "heading": -0.013766029362825559, + "angularVelocity": -0.07108194052044785, + "velocityX": 2.2904205740732744, + "velocityY": 0.04003880241453354, + "timestamp": 2.0749853569167938 + }, + { + "x": 7.6687074557367385, + "y": 4.062834301345331, + "heading": -0.015395893103822788, + "angularVelocity": -0.02480647311048019, + "velocityX": 2.0041058424417098, + "velocityY": 0.04774188737652573, + "timestamp": 2.140688519705729 + }, + { + "x": 7.781556988474885, + "y": 4.066015749956383, + "heading": -0.01492426229398527, + "angularVelocity": 0.007178205581253095, + "velocityX": 1.7175662167232906, + "velocityY": 0.04842154435201139, + "timestamp": 2.2063916824946643 + }, + { + "x": 7.8755781593330285, + "y": 4.068907471663171, + "heading": -0.013029646010250598, + "angularVelocity": 0.028835998197239424, + "velocityX": 1.4309991614890225, + "velocityY": 0.044011910295353764, + "timestamp": 2.2720948452835996 + }, + { + "x": 7.950774573438036, + "y": 4.07125628842811, + "heading": -0.010225311537090487, + "angularVelocity": 0.042681879442679815, + "velocityX": 1.144486976168374, + "velocityY": 0.035748914743771835, + "timestamp": 2.337798008072535 + }, + { + "x": 8.007152043467745, + "y": 4.072863968231118, + "heading": -0.006913103442788773, + "angularVelocity": 0.0504116994328239, + "velocityX": 0.8580632596152882, + "velocityY": 0.024468834296048715, + "timestamp": 2.40350117086147 + }, + { + "x": 8.044717148266278, + "y": 4.073571148367015, + "heading": -0.0034161675760999402, + "angularVelocity": 0.053223250118451255, + "velocityX": 0.5717396728557544, + "velocityY": 0.010763258660280774, + "timestamp": 2.4692043336504055 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 2.0297812570188607e-18, - "angularVelocity": 0.029015653665741454, - "velocityX": 0.2867140781676597, - "velocityY": -0.09718948531913073, - "timestamp": 2.6112247259157106 - }, - { - "x": 8.063706917248451, - "y": 4.062947102090978, - "heading": 0.001985154079980339, - "angularVelocity": 0.027021345733527664, - "velocityX": 0.003135522507827122, - "velocityY": -0.1401986439325952, - "timestamp": 2.6846908700358814 - }, - { - "x": 8.043035079312682, - "y": 4.049980112254385, - "heading": 0.003821402596114743, - "angularVelocity": 0.024994486074167273, - "velocityX": -0.2813791057545805, - "velocityY": -0.17650293195437572, - "timestamp": 2.758157014156052 - }, - { - "x": 8.001396508446343, - "y": 4.034912807351801, - "heading": 0.0055060818689868455, - "angularVelocity": 0.022931369177568325, - "velocityX": -0.5667722372665807, - "velocityY": -0.2050918158701582, - "timestamp": 2.831623158276223 - }, - { - "x": 7.938735716407508, - "y": 4.018402810035911, - "heading": 0.007036218210445593, - "angularVelocity": 0.020827775294097833, - "velocityX": -0.8529206587504942, - "velocityY": -0.2247293295927428, - "timestamp": 2.9050893023963926 - }, - { - "x": 7.855014011007738, - "y": 4.00121976392101, - "heading": 0.008408488071179075, - "angularVelocity": 0.018678942214372884, - "velocityX": -1.139595747162445, - "velocityY": -0.2338906760479261, - "timestamp": 2.9785554465165625 - }, - { - "x": 7.750221962754556, - "y": 3.9842725768704743, - "heading": 0.00961918059388702, - "angularVelocity": 0.016479598013576947, - "velocityX": -1.4263991871108765, - "velocityY": -0.23068023037679872, - "timestamp": 3.0520215906367323 - }, - { - "x": 7.62439956654467, - "y": 3.9686442308926377, - "heading": 0.010664172857875897, - "angularVelocity": 0.014224133803450148, - "velocityX": -1.7126582280413951, - "velocityY": -0.21272854544091283, - "timestamp": 3.125487734756902 - }, - { - "x": 7.477668976002092, - "y": 3.955635460155464, - "heading": 0.011538940075399833, - "angularVelocity": 0.01190707948538909, - "velocityX": -1.9972545490146683, - "velocityY": -0.17707164154273572, - "timestamp": 3.198953878877072 - }, - { - "x": 7.3102875993376655, - "y": 3.9468171160612413, - "heading": 0.012238647278142662, - "angularVelocity": 0.009524212970784032, - "velocityX": -2.2783471035397658, - "velocityY": -0.12003276066589603, - "timestamp": 3.2724200229972418 - }, - { - "x": 7.122733099589339, - "y": 3.9440864335777195, - "heading": 0.012758416420278173, - "angularVelocity": 0.0070749479009718985, - "velocityX": -2.5529378463301264, - "velocityY": -0.037169263695877315, - "timestamp": 3.3458861671174116 - }, - { - "x": 6.915834286392237, - "y": 3.9497111054839014, - "heading": 0.013093946664375935, - "angularVelocity": 0.004567141070435388, - "velocityX": -2.816247071013707, - "velocityY": 0.07656141442487652, - "timestamp": 3.4193523112375814 - }, - { - "x": 6.690954998420113, - "y": 3.9663229006089162, - "heading": 0.01324277694660248, - "angularVelocity": 0.00202583494763381, - "velocityX": -3.060992116372438, - "velocityY": 0.22611497205899783, - "timestamp": 3.4928184553577513 - }, - { - "x": 6.450203582137421, - "y": 3.9967943470851464, - "heading": 0.013206490081435995, - "angularVelocity": -0.0004939263602446482, - "velocityX": -3.277038956732079, - "velocityY": 0.4147685555184044, - "timestamp": 3.566284599477921 - }, - { - "x": 6.196564054073483, - "y": 4.043936461503449, - "heading": 0.012993660347291816, - "angularVelocity": -0.0028969770592021916, - "velocityX": -3.4524682233091277, - "velocityY": 0.6416848874114194, - "timestamp": 3.639750743598091 - }, - { - "x": 5.933782874794111, - "y": 4.110063969722605, - "heading": 0.012622011909817094, - "angularVelocity": -0.005058771518848182, - "velocityX": -3.576901747416247, - "velocityY": 0.9001086011944357, - "timestamp": 3.7132168877182608 + "heading": 5.107015447929928e-20, + "angularVelocity": 0.05199395936347874, + "velocityX": 0.28551767430107416, + "velocityY": -0.004934199232898976, + "timestamp": 2.5349074964393408 + }, + { + "x": 8.06095184830656, + "y": 4.071458641245482, + "heading": 0.0034408492569327057, + "angularVelocity": 0.04684199855212652, + "velocityX": -0.034370194612683154, + "velocityY": -0.02434521970346025, + "timestamp": 2.608363996232015 + }, + { + "x": 8.034920990250148, + "y": 4.068388525754522, + "heading": 0.006505362431754428, + "angularVelocity": 0.041718747605332625, + "velocityX": -0.3543710649143617, + "velocityY": -0.041795014731511364, + "timestamp": 2.681820496024689 + }, + { + "x": 7.985375296011398, + "y": 4.064207377475085, + "heading": 0.009196043333328236, + "angularVelocity": 0.036629582258453454, + "velocityX": -0.6744902680986578, + "velocityY": -0.05692005869104256, + "timestamp": 2.755276995817363 + }, + { + "x": 7.912305948559603, + "y": 4.059120814150924, + "heading": 0.011515917649017375, + "angularVelocity": 0.03158160710402538, + "velocityX": -0.9947295019232808, + "velocityY": -0.06924592566372595, + "timestamp": 2.828733495610037 + }, + { + "x": 7.815704588785296, + "y": 4.0533811177027905, + "heading": 0.013468719297185195, + "angularVelocity": 0.026584463644190544, + "velocityX": -1.3150825324778364, + "velocityY": -0.0781373529140697, + "timestamp": 2.9021899954027113 + }, + { + "x": 7.695564554163692, + "y": 4.047305026722375, + "heading": 0.015059175300172275, + "angularVelocity": 0.021651671499132708, + "velocityX": -1.6355262633081036, + "velocityY": -0.08271685960487347, + "timestamp": 2.9756464951953854 + }, + { + "x": 7.551883548681463, + "y": 4.04130179428642, + "heading": 0.016293463732843762, + "angularVelocity": 0.016802984571211377, + "velocityX": -1.9560012509139277, + "velocityY": -0.08172499986929728, + "timestamp": 3.0491029949880595 + }, + { + "x": 7.384669657766205, + "y": 4.035920047959674, + "heading": 0.017179998132389497, + "angularVelocity": 0.012068835324959903, + "velocityX": -2.276366167557798, + "velocityY": -0.07326439922859854, + "timestamp": 3.1225594947807327 + }, + { + "x": 7.19395595849013, + "y": 4.0319320028167684, + "heading": 0.017730886759136543, + "angularVelocity": 0.007499521870792628, + "velocityX": -2.5962807895060482, + "velocityY": -0.054291249299417726, + "timestamp": 3.196015994573406 + }, + { + "x": 6.979840325568109, + "y": 4.030500043249597, + "heading": 0.017964968086133067, + "angularVelocity": 0.00318666595409774, + "velocityX": -2.9148629941032773, + "velocityY": -0.01949398039945934, + "timestamp": 3.269472494366079 + }, + { + "x": 6.742614358141766, + "y": 4.033551118259164, + "heading": 0.017915208666011314, + "angularVelocity": -0.0006773998252331979, + "velocityX": -3.22947551402392, + "velocityY": 0.041535807153613755, + "timestamp": 3.3429289941587523 + }, + { + "x": 6.483307205091183, + "y": 4.044763583343556, + "heading": 0.017651500017929662, + "angularVelocity": -0.003589997465520818, + "velocityX": -3.530077716505134, + "velocityY": 0.1526408842789682, + "timestamp": 3.4163854939514255 + }, + { + "x": 6.206941447635851, + "y": 4.072199279636492, + "heading": 0.017375156748179146, + "angularVelocity": -0.0037619988773032, + "velocityX": -3.762305013652376, + "velocityY": 0.37349582910118817, + "timestamp": 3.4898419937440988 + }, + { + "x": 5.9338077856619895, + "y": 4.122953167788701, + "heading": 0.017375155210986027, + "angularVelocity": -2.0926577278102936e-8, + "velocityX": -3.7183048844521998, + "velocityY": 0.690938014953861, + "timestamp": 3.563298493536772 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.012117456973340398, - "angularVelocity": -0.0068678565143064085, - "velocityX": -3.6456404399720617, - "velocityY": 1.1785715157475694, - "timestamp": 3.7866830318384306 - }, - { - "x": 5.55330454345789, - "y": 4.2367101473374795, - "heading": 0.011887459641735761, - "angularVelocity": -0.007456725508590102, - "velocityX": -3.6521255907888213, - "velocityY": 1.2988169090424007, - "timestamp": 3.817527312124689 - }, - { - "x": 5.440517148405761, - "y": 4.28048284495712, - "heading": 0.011638752310585518, - "angularVelocity": -0.008063320941259012, - "velocityX": -3.6566713181625206, - "velocityY": 1.4191512077245991, - "timestamp": 3.848371592410947 - }, - { - "x": 5.327667581738716, - "y": 4.327969293579141, - "heading": 0.011370630767981954, - "angularVelocity": -0.008692747573138404, - "velocityX": -3.6586869792296546, - "velocityY": 1.5395544386611915, - "timestamp": 3.8792158726972055 - }, - { - "x": 5.214861744178507, - "y": 4.379169755070302, - "heading": 0.011082156069971438, - "angularVelocity": -0.009352615633539172, - "velocityX": -3.6572692412752423, - "velocityY": 1.659966159559613, - "timestamp": 3.9100601529834638 - }, - { - "x": 5.1022510887558585, - "y": 4.434079353427009, - "heading": 0.010772019648005773, - "angularVelocity": -0.010054908692547623, - "velocityX": -3.650941256451206, - "velocityY": 1.7802197959265815, - "timestamp": 3.940904433269722 - }, - { - "x": 4.990068460344294, - "y": 4.4926784730980485, - "heading": 0.010438291328946931, - "angularVelocity": -0.010819779743978382, - "velocityX": -3.637064226184596, - "velocityY": 1.8998374780411038, - "timestamp": 3.9717487135559804 - }, - { - "x": 4.878710505114531, - "y": 4.5548991474962115, - "heading": 0.010077925553591442, - "angularVelocity": -0.011683390632267332, - "velocityX": -3.6103275614238326, - "velocityY": 2.0172516207448092, - "timestamp": 4.002592993842239 - }, - { - "x": 4.768951534707332, - "y": 4.620472273914983, - "heading": 0.009685974313613142, - "angularVelocity": -0.012707420511702754, - "velocityX": -3.558486999487417, - "velocityY": 2.1259412056368423, - "timestamp": 4.033437274128497 - }, - { - "x": 4.662298486421541, - "y": 4.688082271388773, - "heading": 0.009258221595846085, - "angularVelocity": -0.013868137424417113, - "velocityX": -3.4577901411855687, - "velocityY": 2.19197844288524, - "timestamp": 4.064281554414755 - }, - { - "x": 4.559169523977437, - "y": 4.754519038672898, - "heading": 0.008807731583591003, - "angularVelocity": -0.014605301471592353, - "velocityX": -3.3435360295972387, - "velocityY": 2.1539412386199186, - "timestamp": 4.0951258347010135 - }, - { - "x": 4.45859769074999, - "y": 4.818261954197611, - "heading": 0.008349427916360263, - "angularVelocity": -0.014858627368748727, - "velocityX": -3.260631543160092, - "velocityY": 2.0666040813119846, - "timestamp": 4.125970114987272 - }, - { - "x": 4.359992719048881, - "y": 4.878854116841475, - "heading": 0.007890638381237635, - "angularVelocity": -0.014874379653690404, - "velocityX": -3.1968640793682743, - "velocityY": 1.964453768462782, - "timestamp": 4.15681439527353 - }, - { - "x": 4.263026916364364, - "y": 4.936113272994566, - "heading": 0.007435172084260176, - "angularVelocity": -0.014766637209570182, - "velocityX": -3.1437207088185697, - "velocityY": 1.856394625573387, - "timestamp": 4.187658675559788 + "heading": 0.017375154199310676, + "angularVelocity": -1.377244159549356e-8, + "velocityX": -3.646458211284153, + "velocityY": 1.003259166632557, + "timestamp": 3.636754993329445 + }, + { + "x": 5.541092339335072, + "y": 4.2364336029082, + "heading": 0.01737515315796098, + "angularVelocity": -3.005342133477009e-8, + "velocityX": -3.6034502673557895, + "velocityY": 1.148184128750311, + "timestamp": 3.6714049482953417 + }, + { + "x": 5.417922638814364, + "y": 4.281176215218788, + "heading": 0.017375152214225534, + "angularVelocity": -2.7236267527590686e-8, + "velocityX": -3.5546857316822638, + "velocityY": 1.2912747608077488, + "timestamp": 3.706054903261238 + }, + { + "x": 5.296639420951804, + "y": 4.33080542124118, + "heading": 0.017375151348717865, + "angularVelocity": -2.4978608726563434e-8, + "velocityX": -3.500241716964063, + "velocityY": 1.4323021796489428, + "timestamp": 3.7407048582271347 + }, + { + "x": 5.177436464453021, + "y": 4.385241922289634, + "heading": 0.017375150546348885, + "angularVelocity": -2.3156422113484916e-8, + "velocityX": -3.4402052359405113, + "velocityY": 1.571041033156597, + "timestamp": 3.7753548131930312 + }, + { + "x": 5.0605042229676425, + "y": 4.444398737399711, + "heading": 0.017375149795171643, + "angularVelocity": -2.167902498222188e-8, + "velocityX": -3.374672249948487, + "velocityY": 1.7072696102577922, + "timestamp": 3.8100047681589277 + }, + { + "x": 4.946029519855076, + "y": 4.5081813412028975, + "heading": 0.017375149085535665, + "angularVelocity": -2.0480141439242773e-8, + "velocityX": -3.3037475294046756, + "velocityY": 1.8407701789500717, + "timestamp": 3.8446547231248243 + }, + { + "x": 4.834195247479412, + "y": 4.576487812332227, + "heading": 0.017375148409491465, + "angularVelocity": -1.9510680466596443e-8, + "velocityX": -3.2275445230947857, + "velocityY": 1.9713292902272803, + "timestamp": 3.8793046780907208 + }, + { + "x": 4.725180068322848, + "y": 4.649208988191292, + "heading": 0.01737514776036044, + "angularVelocity": -1.8733964504925207e-8, + "velocityX": -3.146185305691103, + "velocityY": 2.0987379617271817, + "timestamp": 3.9139546330566173 + }, + { + "x": 4.619158095512492, + "y": 4.726228597192443, + "heading": 0.017375147132049722, + "angularVelocity": -1.813308899206411e-8, + "velocityX": -3.059801171883295, + "velocityY": 2.222791027490615, + "timestamp": 3.948604588022514 + }, + { + "x": 4.51014571232548, + "y": 4.798953964824882, + "heading": 0.01737514649311973, + "angularVelocity": -1.8439562026762692e-8, + "velocityX": -3.146104613824239, + "velocityY": 2.0988589365849712, + "timestamp": 3.9832545429884103 + }, + { + "x": 4.398314099643168, + "y": 4.867264790943429, + "heading": 0.017375145827920196, + "angularVelocity": -1.9197702730409006e-8, + "velocityX": -3.2274677641681775, + "velocityY": 1.9714549755050896, + "timestamp": 4.017904497954307 + }, + { + "x": 4.2838418935996625, + "y": 4.931051865994745, + "heading": 0.017375140544826046, + "angularVelocity": -1.5247044777404565e-7, + "velocityX": -3.3036754638258423, + "velocityY": 1.840899219467885, + "timestamp": 4.052554452920203 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.006985288099960263, - "angularVelocity": -0.014585653486632353, - "velocityX": -3.097100988534025, - "velocityY": 1.7453648577216743, - "timestamp": 4.218502955846047 - }, - { - "x": 3.9486678661427668, - "y": 5.097895325970227, - "heading": 0.00595988192546784, - "angularVelocity": -0.014043783913272705, - "velocityX": -2.9970738935334476, - "velocityY": 1.4784305228888592, - "timestamp": 4.2915179054368995 - }, - { - "x": 3.7418449106776346, - "y": 5.188842546635376, - "heading": 0.004995539128350481, - "angularVelocity": -0.01320747055940148, - "velocityX": -2.8326110834026332, - "velocityY": 1.2455972533677366, - "timestamp": 4.364532855027752 - }, - { - "x": 3.5499887292854875, - "y": 5.265325878573737, - "heading": 0.004104715734621832, - "angularVelocity": -0.01220056164827184, - "velocityX": -2.6276287591409173, - "velocityY": 1.0475023589955905, - "timestamp": 4.437547804618605 - }, - { - "x": 3.374901152496024, - "y": 5.329479478594356, - "heading": 0.0032944686159963646, - "angularVelocity": -0.011097003054385247, - "velocityX": -2.397968878573332, - "velocityY": 0.8786365036216773, - "timestamp": 4.510562754209458 - }, - { - "x": 3.217704545762821, - "y": 5.382997215745527, - "heading": 0.002568947278727608, - "angularVelocity": -0.0099366135474215, - "velocityX": -2.1529372767367914, - "velocityY": 0.7329695829561359, - "timestamp": 4.5835777038003105 - }, - { - "x": 3.079126437625259, - "y": 5.42721119539832, - "heading": 0.0019307216893178917, - "angularVelocity": -0.008741026227999762, - "velocityX": -1.8979415710631973, - "velocityY": 0.6055469448455648, - "timestamp": 4.656592653391163 - }, - { - "x": 2.959658381908625, - "y": 5.463178096937211, - "heading": 0.0013814652459932491, - "angularVelocity": -0.007522520338676778, - "velocityX": -1.6362136300317622, - "velocityY": 0.49259640307136626, - "timestamp": 4.729607602982016 - }, - { - "x": 2.8596453147458623, - "y": 5.491748253962881, - "heading": 0.0009223163485468854, - "angularVelocity": -0.006288423124569153, - "velocityX": -1.369761503954984, - "velocityY": 0.3912918818100397, - "timestamp": 4.802622552572869 - }, - { - "x": 2.779337430145008, - "y": 5.5136166054710944, - "heading": 0.0005540788250022006, - "angularVelocity": -0.0050433168222144735, - "velocityX": -1.0998827644320655, - "velocityY": 0.29950512368706583, - "timestamp": 4.8756375021637215 - }, - { - "x": 2.718921448994663, - "y": 5.529359550737521, - "heading": 0.00027733842586429876, - "angularVelocity": -0.003790188183223442, - "velocityX": -0.8274467282233734, - "velocityY": 0.21561262939498888, - "timestamp": 4.948652451754574 - }, - { - "x": 2.678540177748154, - "y": 5.539461677753878, - "heading": 0.0000925335672086805, - "angularVelocity": -0.002531055074216922, - "velocityX": -0.5530548397662466, - "velocityY": 0.13835696762054583, - "timestamp": 5.021667401345427 + "heading": 0.01686674274932993, + "angularVelocity": -0.014672394119890892, + "velocityX": -3.3576617434279026, + "velocityY": 1.6997404450060698, + "timestamp": 4.0872044078861 + }, + { + "x": 3.941206209015057, + "y": 5.0867984564258695, + "heading": 0.014981559594595455, + "angularVelocity": -0.027398080889315734, + "velocityX": -3.288799800865747, + "velocityY": 1.4075673366199248, + "timestamp": 4.156011532091272 + }, + { + "x": 3.7303087168982803, + "y": 5.169878519937419, + "heading": 0.012882808496808885, + "angularVelocity": -0.03050194470456946, + "velocityX": -3.0650531402520937, + "velocityY": 1.2074340334849432, + "timestamp": 4.224818656296444 + }, + { + "x": 3.5370906486542313, + "y": 5.2422774011944036, + "heading": 0.010786769401609648, + "angularVelocity": -0.030462530143669606, + "velocityX": -2.808111376198552, + "velocityY": 1.0522003657804835, + "timestamp": 4.293625780501616 + }, + { + "x": 3.362377869511596, + "y": 5.305500032913324, + "heading": 0.008787829989227822, + "angularVelocity": -0.029051343672224058, + "velocityX": -2.5391669999413216, + "velocityY": 0.9188384553087892, + "timestamp": 4.362432904706788 + }, + { + "x": 3.2065764087500264, + "y": 5.360415401059238, + "heading": 0.0069378788339289135, + "angularVelocity": -0.026886040895746154, + "velocityX": -2.2643216463602664, + "velocityY": 0.7981058470364881, + "timestamp": 4.43124002891196 + }, + { + "x": 3.0699232813800306, + "y": 5.407585492361195, + "heading": 0.005269555505228705, + "angularVelocity": -0.024246374891727204, + "velocityX": -1.9860316638509532, + "velocityY": 0.6855408047763067, + "timestamp": 4.500047153117132 + }, + { + "x": 2.9525724844135883, + "y": 5.4474025208162065, + "heading": 0.00380523783536115, + "angularVelocity": -0.021281483375198414, + "velocityX": -1.70550358443293, + "velocityY": 0.5786759571041632, + "timestamp": 4.568854277322304 + }, + { + "x": 2.854631606493464, + "y": 5.480155404840544, + "heading": 0.0025612082007498414, + "angularVelocity": -0.018079953914391628, + "velocityX": -1.423411878515389, + "velocityY": 0.47601007021705255, + "timestamp": 4.637661401527476 + }, + { + "x": 2.776179840063629, + "y": 5.506065679558788, + "heading": 0.0015498402303536507, + "angularVelocity": -0.014698593816833106, + "velocityX": -1.1401692388117297, + "velocityY": 0.376563837212327, + "timestamp": 4.706468525732648 + }, + { + "x": 2.717277807675311, + "y": 5.525308541082813, + "heading": 0.000780854270914727, + "angularVelocity": -0.01117596423803359, + "velocityX": -0.8560455486074661, + "velocityY": 0.2796637956651817, + "timestamp": 4.77527564993782 + }, + { + "x": 2.677973353536199, + "y": 5.538025975345556, + "heading": 0.00026208876345230333, + "angularVelocity": -0.007539415626724292, + "velocityX": -0.5712265204096013, + "velocityY": 0.1848272894652728, + "timestamp": 4.844082774142992 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -1.9416138419813845e-19, - "angularVelocity": -0.0012673235786260742, - "velocityX": -0.27713515806951416, - "velocityY": 0.06674917354380465, - "timestamp": 5.09468235093628 + "heading": -9.768710288964573e-20, + "angularVelocity": -0.003809035277666901, + "velocityX": -0.2858451884385741, + "velocityY": 0.09169675411867828, + "timestamp": 4.912889898348164 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -9.371198129604789e-20, - "angularVelocity": 9.233837824624604e-20, - "velocityX": 1.3188409475375661e-20, - "velocityY": 7.071902639587051e-21, - "timestamp": 5.1676973005271325 + "heading": -4.776521402733554e-20, + "angularVelocity": 3.1435024538818593e-20, + "velocityX": 5.229513890071112e-19, + "velocityY": 2.1429873638516558e-18, + "timestamp": 4.981697022553336 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj index 2ca0e78f..9d8dfdaa 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj @@ -3,398 +3,380 @@ { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -9.371198129604789e-20, - "angularVelocity": 9.233837824624604e-20, - "velocityX": 1.3188409475375661e-20, - "velocityY": 7.071902639587051e-21, + "heading": -4.776521402733554e-20, + "angularVelocity": 3.1435024538818593e-20, + "velocityX": 5.229513890071112e-19, + "velocityY": 2.1429873638516558e-18, "timestamp": 0 }, { - "x": 2.6757817592371826, - "y": 5.540525112473874, - "heading": 2.9656831471890883e-18, - "angularVelocity": 4.5198899479057065e-17, - "velocityX": 0.25819570553696475, - "velocityY": -0.05629192276270124, - "timestamp": 0.06768738096933724 - }, - { - "x": 2.7106551649219846, - "y": 5.532555521074409, - "heading": 6.8667018003075775e-18, - "angularVelocity": 5.763287923477954e-17, - "velocityX": 0.5152128090256504, - "velocityY": -0.11774116955531974, - "timestamp": 0.13537476193867448 - }, - { - "x": 2.762825274578906, - "y": 5.520028158351496, - "heading": 1.4433249035955584e-17, - "angularVelocity": 1.1178667467062074e-16, - "velocityX": 0.7707508978749655, - "velocityY": -0.18507678305041053, - "timestamp": 0.20306214290801172 - }, - { - "x": 2.832164805136809, - "y": 5.502484798756892, - "heading": 1.639060638651272e-17, - "angularVelocity": 2.8917610972365257e-17, - "velocityX": 1.0244085317662608, - "velocityY": -0.259182130308041, - "timestamp": 0.27074952387734896 - }, - { - "x": 2.9185095362845113, - "y": 5.47939415323771, - "heading": 1.573002181515029e-17, - "angularVelocity": -9.759346009756976e-18, - "velocityX": 1.2756400072092744, - "velocityY": -0.34113663711767017, - "timestamp": 0.3384369048466862 - }, - { - "x": 3.021644093219496, - "y": 5.450134961207605, - "heading": 1.4178342885926476e-17, - "angularVelocity": -2.2924198084943018e-17, - "velocityX": 1.52368957785063, - "velocityY": -0.4322695251476669, - "timestamp": 0.40612428581602344 - }, - { - "x": 3.141280830981249, - "y": 5.413974520765915, - "heading": 1.5889264278443728e-17, - "angularVelocity": 2.5276814794004328e-17, - "velocityX": 1.7674895386475231, - "velocityY": -0.5342272063690965, - "timestamp": 0.4738116667853607 - }, - { - "x": 3.27702786794977, - "y": 5.370041857723201, - "heading": 1.8375553239227874e-17, - "angularVelocity": 3.6731942133725327e-17, - "velocityX": 2.0054999177766217, - "velocityY": -0.6490524882712743, - "timestamp": 0.5414990477546979 - }, - { - "x": 3.4283399225131155, - "y": 5.317295603371642, - "heading": 2.6053093742707064e-17, - "angularVelocity": 1.1342646729407338e-16, - "velocityX": 2.235454414049344, - "velocityY": -0.7792627458204298, - "timestamp": 0.6091864287240352 - }, - { - "x": 3.594442154384911, - "y": 5.254489538104451, - "heading": 3.5054878226049046e-17, - "angularVelocity": 1.3299058635211391e-16, - "velocityX": 2.453961572941345, - "velocityY": -0.9278844057453234, - "timestamp": 0.6768738096933724 - }, - { - "x": 3.774213631656668, - "y": 5.18014653899489, - "heading": 3.926192172002768e-17, - "angularVelocity": 6.215402980533945e-17, - "velocityX": 2.6559083051712955, - "velocityY": -1.098328788097711, - "timestamp": 0.7445611906627096 - }, - { - "x": 3.9660187105503817, - "y": 5.092569435745629, - "heading": 4.466170927343128e-17, - "angularVelocity": 7.977539500755919e-17, - "velocityX": 2.8336903592207485, - "velocityY": -1.2938468292772884, - "timestamp": 0.8122485716320469 + "x": 2.6751665057839835, + "y": 5.538944468629318, + "heading": -4.314568505907974e-18, + "angularVelocity": -6.69844600274702e-17, + "velocityX": 0.2647058041795454, + "velocityY": -0.08463157955674895, + "timestamp": 0.06369840542178373 + }, + { + "x": 2.7088624712986666, + "y": 5.528079837461981, + "heading": -9.548636187746045e-18, + "angularVelocity": -8.216952445023882e-17, + "velocityX": 0.5289922925316966, + "velocityY": -0.1705636286402427, + "timestamp": 0.12739681084356747 + }, + { + "x": 2.7593613029745305, + "y": 5.511644735540107, + "heading": -1.3305788264629273e-17, + "angularVelocity": -5.898345573823082e-17, + "velocityX": 0.7927801542516099, + "velocityY": -0.25801433824046893, + "timestamp": 0.1910952162653512 + }, + { + "x": 2.8266246280660523, + "y": 5.489524723354373, + "heading": -1.7929345665528888e-17, + "angularVelocity": -7.258513569049044e-17, + "velocityX": 1.0559656030026554, + "velocityY": -0.34726163142175154, + "timestamp": 0.25479362168713493 + }, + { + "x": 2.9106051995536752, + "y": 5.461582331644453, + "heading": -2.2921460737278194e-17, + "angularVelocity": -7.837111523596277e-17, + "velocityX": 1.31840932173323, + "velocityY": -0.43866705178720505, + "timestamp": 0.31849202710891866 + }, + { + "x": 3.0112434875257597, + "y": 5.427649372766817, + "heading": -2.8632725416108676e-17, + "angularVelocity": -8.966103061640699e-17, + "velocityX": 1.5799184815647955, + "velocityY": -0.5327128466237959, + "timestamp": 0.3821904325307024 + }, + { + "x": 3.1284623112048178, + "y": 5.387515427846554, + "heading": -3.496086628632705e-17, + "angularVelocity": -9.93453576777077e-17, + "velocityX": 1.8402159819054262, + "velocityY": -0.6300620031932188, + "timestamp": 0.44588883795248613 + }, + { + "x": 3.2621579049769083, + "y": 5.340909820689256, + "heading": -4.1773207656590006e-17, + "angularVelocity": -1.0694681169901965e-16, + "velocityX": 2.0988844679362666, + "velocityY": -0.7316604999559271, + "timestamp": 0.5095872433742699 + }, + { + "x": 3.4121839694430376, + "y": 5.287471786804592, + "heading": -4.8890897323491556e-17, + "angularVelocity": -1.1174046853574415e-16, + "velocityX": 2.3552562026116513, + "velocityY": -0.8389226312781303, + "timestamp": 0.5732856487960536 + }, + { + "x": 3.5783204640757695, + "y": 5.226697519809261, + "heading": -5.788296277144039e-17, + "angularVelocity": -1.4116625664827146e-16, + "velocityX": 2.608173525422577, + "velocityY": -0.954094008992977, + "timestamp": 0.6369840542178373 + }, + { + "x": 3.76020443223388, + "y": 5.157837260218167, + "heading": -6.653949543666233e-17, + "angularVelocity": -1.3589873416470021e-16, + "velocityX": 2.85539279914075, + "velocityY": -1.0810358459545435, + "timestamp": 0.7006824596396211 + }, + { + "x": 3.9571467965572817, + "y": 5.079670509622146, + "heading": -7.420373602789392e-17, + "angularVelocity": -1.2032076062800333e-16, + "velocityX": 3.0917942610859868, + "velocityY": -1.2271382631705205, + "timestamp": 0.7643808650614048 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 4.913032791004192e-17, - "angularVelocity": 6.601848931159295e-17, - "velocityX": 2.976630975573702, - "velocityY": -1.516111842533804, - "timestamp": 0.8799359526013841 - }, - { - "x": 4.261627503076193, - "y": 4.9391622522038245, - "heading": 5.1271717716207476e-17, - "angularVelocity": 6.871367197331748e-17, - "velocityX": 3.0204265353966933, - "velocityY": -1.6296244523245107, - "timestamp": 0.9110999079480617 - }, - { - "x": 4.357244155674493, - "y": 4.884889313220403, - "heading": 5.244866899623222e-17, - "angularVelocity": 3.776642810800902e-17, - "velocityX": 3.0681809011286347, - "velocityY": -1.7415292243770975, - "timestamp": 0.9422638632947393 - }, - { - "x": 4.454522517564495, - "y": 4.827208233471932, - "heading": 5.393508555206354e-17, - "angularVelocity": 4.76966591102462e-17, - "velocityX": 3.121502415461902, - "velocityY": -1.8508908483152524, - "timestamp": 0.9734278186414169 - }, - { - "x": 4.553721568715114, - "y": 4.766257957931851, - "heading": 5.435767688071947e-17, - "angularVelocity": 1.3560259702022703e-17, - "velocityX": 3.183134170457424, - "velocityY": -1.9557939568983076, - "timestamp": 1.0045917739880945 - }, - { - "x": 4.6552566098247095, - "y": 4.702321075934724, - "heading": 5.3497639174451405e-17, - "angularVelocity": -2.7597193512319988e-17, - "velocityX": 3.2580922408625277, - "velocityY": -2.0516292391602082, - "timestamp": 1.0357557293347721 - }, - { - "x": 4.7598200255019725, - "y": 4.636102620452091, - "heading": 5.221139321258901e-17, - "angularVelocity": -4.1273514455672286e-17, - "velocityX": 3.355267793002091, - "velocityY": -2.1248411745556077, - "timestamp": 1.0669196846814497 - }, - { - "x": 4.868161384056621, - "y": 4.569562096793982, - "heading": 4.9900416681055594e-17, - "angularVelocity": -7.415543069862901e-17, - "velocityX": 3.476495757660606, - "velocityY": -2.135175811859932, - "timestamp": 1.0980836400281273 - }, - { - "x": 4.979472809142361, - "y": 4.50537854159068, - "heading": 4.580479975569673e-17, - "angularVelocity": -1.3142160161653158e-16, - "velocityX": 3.5718003009398434, - "velocityY": -2.059544576075285, - "timestamp": 1.129247595374805 - }, - { - "x": 5.092478993411361, - "y": 4.444586805435104, - "heading": 4.211690012859059e-17, - "angularVelocity": -1.1833862502052825e-16, - "velocityX": 3.6261823318601563, - "velocityY": -1.950706689164141, - "timestamp": 1.1604115507214825 - }, - { - "x": 5.206458430184718, - "y": 4.387459691402226, - "heading": 3.8483549973003e-17, - "angularVelocity": -1.1658822232196594e-16, - "velocityX": 3.657412401776814, - "velocityY": -1.8331150008841193, - "timestamp": 1.19157550606816 - }, - { - "x": 5.321007172735708, - "y": 4.334081255049934, - "heading": 3.4624290514440134e-17, - "angularVelocity": -1.2383727976295539e-16, - "velocityX": 3.675680486533673, - "velocityY": -1.7128261081911138, - "timestamp": 1.2227394614148377 - }, - { - "x": 5.435875810085393, - "y": 4.2844809626648175, - "heading": 3.000921461469742e-17, - "angularVelocity": -1.4809018452502327e-16, - "velocityX": 3.685945383756665, - "velocityY": -1.5915916908925372, - "timestamp": 1.2539034167615153 - }, - { - "x": 5.550897094649703, - "y": 4.2386692598409, - "heading": 2.554076914069394e-17, - "angularVelocity": -1.4338505567167463e-16, - "velocityX": 3.690843581463868, - "velocityY": -1.4700220916855413, - "timestamp": 1.2850673721081929 + "heading": -8.561193314262968e-17, + "angularVelocity": -1.7909705963758273e-16, + "velocityX": 3.3023160854502294, + "velocityY": -1.4085550989874924, + "timestamp": 0.8280792704831885 + }, + { + "x": 4.2842075611264425, + "y": 4.936417577245955, + "heading": -9.453694204366925e-17, + "angularVelocity": -2.5755488045645486e-16, + "velocityX": 3.3679341946759767, + "velocityY": -1.5447568972758807, + "timestamp": 0.8627321137340349 + }, + { + "x": 4.401356584826297, + "y": 4.8776669667131465, + "heading": -1.0560165699964911e-16, + "angularVelocity": -3.1930179223080245e-16, + "velocityX": 3.380646801528862, + "velocityY": -1.6954051968412056, + "timestamp": 0.8973849569848813 + }, + { + "x": 4.516063976001339, + "y": 4.814281195629902, + "heading": -9.662380875053523e-17, + "angularVelocity": 2.590797004510181e-16, + "velocityX": 3.3101869980680285, + "velocityY": -1.8291650882556678, + "timestamp": 0.9320378002357277 + }, + { + "x": 4.628146410125643, + "y": 4.746361612927903, + "heading": -9.053238794887956e-17, + "angularVelocity": 1.7578415593930746e-16, + "velocityX": 3.234436877602137, + "velocityY": -1.9600002865663488, + "timestamp": 0.9666906434865741 + }, + { + "x": 4.737424855337869, + "y": 4.674016859733605, + "heading": -7.951670404623386e-17, + "angularVelocity": 3.1788687072504797e-16, + "velocityX": 3.1535203163901047, + "velocityY": -2.087700356089278, + "timestamp": 1.0013434867374205 + }, + { + "x": 4.844483096398324, + "y": 4.598425359059355, + "heading": -7.540763427184281e-17, + "angularVelocity": 1.1857814219565296e-16, + "velocityX": 3.089450417833756, + "velocityY": -2.181393893916722, + "timestamp": 1.0359963299882669 + }, + { + "x": 4.954476829355994, + "y": 4.5271728465414585, + "heading": -1.0264313607639127e-16, + "angularVelocity": -7.859528757078724e-16, + "velocityX": 3.174161847599147, + "velocityY": -2.0561808450207573, + "timestamp": 1.0706491732391132 + }, + { + "x": 5.06723038345343, + "y": 4.460373312794279, + "heading": -9.620613179989556e-17, + "angularVelocity": 1.8575688667134074e-16, + "velocityX": 3.25380383021476, + "velocityY": -1.9276782936288397, + "timestamp": 1.1053020164899596 + }, + { + "x": 5.18256360847748, + "y": 4.398133545147613, + "heading": -8.10849749390595e-17, + "angularVelocity": 4.36361217215796e-16, + "velocityX": 3.3282470990668904, + "velocityY": -1.796094109684511, + "timestamp": 1.139954859740806 + }, + { + "x": 5.300292214676822, + "y": 4.340553023175948, + "heading": -6.741688231750934e-17, + "angularVelocity": 3.9442918212224324e-16, + "velocityX": 3.397372196766743, + "velocityY": -1.6616391779124173, + "timestamp": 1.1746077029916524 + }, + { + "x": 5.4202280776683445, + "y": 4.287723772710641, + "heading": -7.654370000647077e-17, + "angularVelocity": -2.6337861002637873e-16, + "velocityX": 3.461068464810413, + "velocityY": -1.5245285959043562, + "timestamp": 1.2092605462424988 + }, + { + "x": 5.542179542504675, + "y": 4.239730223009142, + "heading": -8.402477364947206e-17, + "angularVelocity": -2.1588628640817297e-16, + "velocityX": 3.5192340193716007, + "velocityY": -1.3849815830141687, + "timestamp": 1.2439133894933452 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 2.1323021995570948e-17, - "angularVelocity": -1.353405592009617e-16, - "velocityX": 3.6919137154187807, - "velocityY": -1.3483585385427075, - "timestamp": 1.3162313274548705 - }, - { - "x": 5.935813024971261, - "y": 4.118882926647287, - "heading": 1.853226702760062e-17, - "angularVelocity": -3.8136239390164817e-17, - "velocityX": 3.6877099925560546, - "velocityY": -1.0626903702408637, - "timestamp": 1.3894098845255716 - }, - { - "x": 6.200932724790644, - "y": 4.061479044678689, - "heading": 1.7565627376800837e-17, - "angularVelocity": -1.3209329241757196e-17, - "velocityX": 3.622915105626314, - "velocityY": -0.7844358274670208, - "timestamp": 1.4625884415962727 - }, - { - "x": 6.456598704926571, - "y": 4.0227227304075965, - "heading": 1.267506754253446e-17, - "angularVelocity": -6.683050380301111e-17, - "velocityX": 3.493728086069183, - "velocityY": -0.5296129880457807, - "timestamp": 1.5357669986669737 - }, - { - "x": 6.698616689948628, - "y": 3.99980424542816, - "heading": 8.945633316051437e-18, - "angularVelocity": -5.0963484094285186e-17, - "velocityX": 3.307225432010503, - "velocityY": -0.31318580055211104, - "timestamp": 1.6089455557376748 - }, - { - "x": 6.923883976637485, - "y": 3.9893965055012433, - "heading": 2.783843782577358e-18, - "angularVelocity": -8.420211852775213e-17, - "velocityX": 3.078323701726083, - "velocityY": -0.1422239019670945, - "timestamp": 1.682124112808376 - }, - { - "x": 7.130423561054918, - "y": 3.988282733586895, - "heading": 2.8545928007524544e-18, - "angularVelocity": 9.667998274288902e-19, - "velocityX": 2.822405806907136, - "velocityY": -0.015219921776707517, - "timestamp": 1.755302669879077 - }, - { - "x": 7.317098326514511, - "y": 3.993687679254755, - "heading": 5.295405456553911e-18, - "angularVelocity": 3.335420582506506e-17, - "velocityX": 2.5509489792103515, - "velocityY": 0.07385969174874675, - "timestamp": 1.828481226949778 - }, - { - "x": 7.4833023364759494, - "y": 4.003341497002791, - "heading": 9.255539418426772e-18, - "angularVelocity": 5.4116043269433334e-17, - "velocityX": 2.2712119043405083, - "velocityY": 0.13192140067353622, - "timestamp": 1.9016597840204792 - }, - { - "x": 7.628740531129743, - "y": 4.015421358409773, - "heading": 1.1054283152069948e-17, - "angularVelocity": 2.458020224219918e-17, - "velocityX": 1.9874427766221006, - "velocityY": 0.16507378514324764, - "timestamp": 1.9748383410911803 - }, - { - "x": 7.753293848479071, - "y": 4.028463870964022, - "heading": 1.0645910526563799e-17, - "angularVelocity": -5.5804957540262804e-18, - "velocityX": 1.7020466422833305, - "velocityY": 0.1782286106249456, - "timestamp": 2.0480168981618814 - }, - { - "x": 7.856941288758158, - "y": 4.041284750258868, - "heading": 1.2932065845592816e-17, - "angularVelocity": 3.1240781584055425e-17, - "velocityX": 1.4163635418357237, - "velocityY": 0.17519994665183228, - "timestamp": 2.1211954552325825 - }, - { - "x": 7.939715627162556, - "y": 4.0529146415487, - "heading": 1.2915183661697564e-17, - "angularVelocity": -2.306985186705733e-19, - "velocityX": 1.1311283211614112, - "velocityY": 0.15892485114999141, - "timestamp": 2.1943740123032835 - }, - { - "x": 8.001678238256412, - "y": 4.062550327487904, - "heading": 8.098550414220998e-18, - "angularVelocity": -6.582028180467396e-17, - "velocityX": 0.8467317965013967, - "velocityY": 0.13167362578486927, - "timestamp": 2.2675525693739846 - }, - { - "x": 8.042904705550558, - "y": 4.0695182051166965, - "heading": 2.8097102514123304e-18, - "angularVelocity": -7.227308620193683e-17, - "velocityX": 0.5633681360281035, - "velocityY": 0.09521747773817571, - "timestamp": 2.3407311264446857 + "heading": -9.639099903602133e-17, + "angularVelocity": -3.5686033890323503e-16, + "velocityX": 3.5717757824418066, + "velocityY": -1.2432211735943417, + "timestamp": 1.2785662327441916 + }, + { + "x": 5.93683892832179, + "y": 4.127579524293423, + "heading": -7.782590018599437e-17, + "angularVelocity": 2.51158487397316e-16, + "velocityX": 3.664705468644969, + "velocityY": -0.9344094480089544, + "timestamp": 1.3524840964903202 + }, + { + "x": 6.212625572925941, + "y": 4.081838891930783, + "heading": -5.4414649647687885e-17, + "angularVelocity": 3.167197934552236e-16, + "velocityX": 3.7309877562390543, + "velocityY": -0.6188034941017393, + "timestamp": 1.4264019602364488 + }, + { + "x": 6.491306353239059, + "y": 4.059759739930169, + "heading": -8.220332638334233e-17, + "angularVelocity": -3.7593993288383394e-16, + "velocityX": 3.7701411565443874, + "velocityY": -0.2986984590956788, + "timestamp": 1.5003198239825775 + }, + { + "x": 6.752875972710037, + "y": 4.054277278014819, + "heading": -7.798662141044023e-17, + "angularVelocity": 5.704581760375439e-17, + "velocityX": 3.5386523123739373, + "velocityY": -0.07416964773468374, + "timestamp": 1.574237687728706 + }, + { + "x": 6.9910920962877245, + "y": 4.053575874325361, + "heading": -6.860310501240552e-17, + "angularVelocity": 1.269451783709716e-16, + "velocityX": 3.2227138543376115, + "velocityY": -0.00948896050171146, + "timestamp": 1.6481555514748347 + }, + { + "x": 7.205567187241113, + "y": 4.055021140911149, + "heading": -5.885294926916e-17, + "angularVelocity": 1.3190526956719489e-16, + "velocityX": 2.901532594204896, + "velocityY": 0.019552331635990624, + "timestamp": 1.7220734152209634 + }, + { + "x": 7.396227777452995, + "y": 4.057522153925315, + "heading": -4.8982438983156224e-17, + "angularVelocity": 1.3353348954892513e-16, + "velocityX": 2.5793574184815227, + "velocityY": 0.033835028333003926, + "timestamp": 1.795991278967092 + }, + { + "x": 7.563054971618611, + "y": 4.060486200727262, + "heading": -3.912779424726568e-17, + "angularVelocity": 1.333188519862415e-16, + "velocityX": 2.256926617070343, + "velocityY": 0.04009919458884174, + "timestamp": 1.8699091427132206 + }, + { + "x": 7.70604444565388, + "y": 4.063541585038157, + "heading": -2.9177079820924904e-17, + "angularVelocity": 1.3461853362720707e-16, + "velocityX": 1.9344373171601388, + "velocityY": 0.041334856772760636, + "timestamp": 1.9438270064593492 + }, + { + "x": 7.825196583277639, + "y": 4.066433602906459, + "heading": -1.9253434148047247e-17, + "angularVelocity": 1.3425233319755054e-16, + "velocityX": 1.6119532084015245, + "velocityY": 0.03912474903541938, + "timestamp": 2.017744870205478 + }, + { + "x": 7.920513375065405, + "y": 4.0689768762979055, + "heading": -9.302606124041008e-18, + "angularVelocity": 1.346200704365749e-16, + "velocityX": 1.2894960292025357, + "velocityY": 0.034406749093584967, + "timestamp": 2.0916627339516065 + }, + { + "x": 7.991997299424133, + "y": 4.071030463180752, + "heading": 8.179153045928767e-19, + "angularVelocity": 1.369157726664237e-16, + "velocityX": 0.9670723792051265, + "velocityY": 0.027782010717991884, + "timestamp": 2.165580597697735 + }, + { + "x": 8.03965088919926, + "y": 4.072483600942051, + "heading": 2.229877553000587e-19, + "angularVelocity": -8.048494898574051e-18, + "velocityX": 0.6446829948819026, + "velocityY": 0.019658817066051014, + "timestamp": 2.2394984614438638 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 3.1031533755807955e-28, - "angularVelocity": -3.8395267193036997e-17, - "velocityX": 0.2811186469496163, - "velocityY": 0.05095414427593911, - "timestamp": 2.413909683515387 + "heading": 3.803306693773524e-29, + "angularVelocity": -3.0166964241453683e-18, + "velocityX": 0.32232632402054306, + "velocityY": 0.01032706968038683, + "timestamp": 2.3134163251899924 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 1.493594231911397e-28, - "angularVelocity": -1.5846843190307173e-28, - "velocityX": -6.450761173739877e-21, - "velocityY": 2.2636697046974056e-20, - "timestamp": 2.487088240586088 + "heading": 1.8619056951323718e-29, + "angularVelocity": -1.0754545583434217e-29, + "velocityX": -2.9997098934675086e-25, + "velocityY": 4.422144616548637e-24, + "timestamp": 2.387334188936121 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.traj index 4315bb24..412c2ecc 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.traj @@ -1,1885 +1,1786 @@ { "samples": [ { - "x": 1.2899744510650635, + "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -5.737773088876545e-18, - "angularVelocity": -1.3800854803157527e-17, - "velocityX": -2.0304788403041935e-16, - "velocityY": -3.8084104282489897e-16, + "heading": -1.2424307943085809e-21, + "angularVelocity": -1.3399063907175068e-21, + "velocityX": -6.613526246981409e-20, + "velocityY": -3.127454467901795e-20, "timestamp": 0 }, { - "x": 1.3043755674833586, - "y": 5.574433574042641, - "heading": -0.009496998526070185, - "angularVelocity": -0.1264048176402608, - "velocityX": 0.19167850659141716, - "velocityY": -0.21989050741887875, - "timestamp": 0.07513161842720571 - }, - { - "x": 1.333181418313963, - "y": 5.541395354098072, - "heading": -0.02848354997968903, - "angularVelocity": -0.2527105345468862, - "velocityX": 0.38340516859413276, - "velocityY": -0.43973789779720845, - "timestamp": 0.15026323685441142 - }, - { - "x": 1.3763968626631116, - "y": 5.491843734926518, - "heading": -0.05694076492631607, - "angularVelocity": -0.3787648335429662, - "velocityX": 0.5751965051812923, - "velocityY": -0.6595308368163946, - "timestamp": 0.22539485528161712 - }, - { - "x": 1.434028520834508, - "y": 5.425784149039423, - "heading": -0.09483975910440062, - "angularVelocity": -0.5044346837260156, - "velocityX": 0.76707595784789, - "velocityY": -0.879251469241908, - "timestamp": 0.30052647370882285 - }, - { - "x": 1.5060856921499353, - "y": 5.34322422233644, - "heading": -0.14214370318506256, - "angularVelocity": -0.6296143364447188, - "velocityX": 0.9590791842586537, - "velocityY": -1.0988706011306708, - "timestamp": 0.3756580921360286 - }, - { - "x": 1.5925821728366396, - "y": 5.244175419847767, - "heading": -0.19881016609697202, - "angularVelocity": -0.7542292326445892, - "velocityX": 1.15126603811467, - "velocityY": -1.318337133749589, - "timestamp": 0.4507897105632343 - }, - { - "x": 1.6935404999326085, - "y": 5.128656800108543, - "heading": -0.2647926425863519, - "angularVelocity": -0.8782251450575851, - "velocityX": 1.3437528593918235, - "velocityY": -1.5375499977346858, - "timestamp": 0.52592132899044 - }, - { - "x": 1.8090047533297515, - "y": 4.99670620893713, - "heading": -0.34003720148047023, - "angularVelocity": -1.001503234885639, - "velocityX": 1.5368263829457236, - "velocityY": -1.7562591348086285, - "timestamp": 0.6010529474176457 - }, - { - "x": 1.9391049081706304, - "y": 4.848436880668431, - "heading": -0.42444836738377417, - "angularVelocity": -1.1235105495461353, - "velocityX": 1.731629872448852, - "velocityY": -1.9734611256713033, - "timestamp": 0.6761845658448514 - }, - { - "x": 2.0768333691424767, - "y": 4.720578672204384, - "heading": -0.4949304334552377, - "angularVelocity": -0.938114572048371, - "velocityX": 1.8331624402684015, - "velocityY": -1.7017896211431234, - "timestamp": 0.7513161842720572 - }, - { - "x": 2.200438107278665, - "y": 4.609474439269374, - "heading": -0.5559710604478235, - "angularVelocity": -0.8124492493047433, - "velocityX": 1.6451760354351246, - "velocityY": -1.4787946174642006, - "timestamp": 0.8264478026992629 - }, - { - "x": 2.30979055136467, - "y": 4.515017163004073, - "heading": -0.6076423765200641, - "angularVelocity": -0.6877439505764344, - "velocityX": 1.4554783509002944, - "velocityY": -1.2572240321695016, - "timestamp": 0.9015794211264686 - }, - { - "x": 2.404847576631005, - "y": 4.43717046489753, - "heading": -0.6499678685895577, - "angularVelocity": -0.5633512621433749, - "velocityX": 1.2652066767447667, - "velocityY": -1.0361376439603944, - "timestamp": 0.9767110395536743 - }, - { - "x": 2.4855875280124384, - "y": 4.375915950789138, - "heading": -0.6829583682346397, - "angularVelocity": -0.43910274175131525, - "velocityX": 1.0746467742038588, - "velocityY": -0.8152960815764004, - "timestamp": 1.05184265798088 - }, - { - "x": 2.5519973917574865, - "y": 4.331242518343638, - "heading": -0.7066196503968551, - "angularVelocity": -0.3149310856889676, - "velocityX": 0.8839136589509067, - "velocityY": -0.5946022910015373, - "timestamp": 1.1269742764080857 - }, - { - "x": 2.6040685139576465, - "y": 4.3031427656124555, - "heading": -0.720955031446972, - "angularVelocity": -0.19080357043979132, - "velocityX": 0.6930653603887426, - "velocityY": -0.37400702018003557, - "timestamp": 1.2021058948352914 - }, - { - "x": 2.641794765155857, - "y": 4.291611444641949, - "heading": -0.7259664732810771, - "angularVelocity": -0.06670216797910607, - "velocityX": 0.5021354789039092, - "velocityY": -0.15348159949241175, - "timestamp": 1.2772375132624971 + "x": 1.3060573925677244, + "y": 5.572382489557548, + "heading": -0.010694903924415845, + "angularVelocity": -0.14212387666711845, + "velocityX": 0.21372515458978017, + "velocityY": -0.2467996203811156, + "timestamp": 0.07525057840538203 + }, + { + "x": 1.3382234703338376, + "y": 5.5352391647325, + "heading": -0.03207617924105129, + "angularVelocity": -0.2841343650736142, + "velocityX": 0.4274528973429488, + "velocityY": -0.49359520700229137, + "timestamp": 0.15050115681076406 + }, + { + "x": 1.3864731960687624, + "y": 5.479524641793448, + "heading": -0.06412173122661301, + "angularVelocity": -0.4258512381516761, + "velocityX": 0.6411874401150632, + "velocityY": -0.7403866404709017, + "timestamp": 0.2257517352161461 + }, + { + "x": 1.4508074313628532, + "y": 5.405239333646717, + "heading": -0.10679840962568042, + "angularVelocity": -0.5671275796601069, + "velocityX": 0.8549334325048802, + "velocityY": -0.9871725868543993, + "timestamp": 0.30100231362152813 + }, + { + "x": 1.5312274241075188, + "y": 5.312383875814348, + "heading": -0.16006570142322632, + "angularVelocity": -0.7078655463694897, + "velocityX": 1.0686960080417647, + "velocityY": -1.2339500878272134, + "timestamp": 0.37625289202691015 + }, + { + "x": 1.6277348707062913, + "y": 5.200959293585018, + "heading": -0.22388050123959674, + "angularVelocity": -0.8480306885163599, + "velocityX": 1.282481126973902, + "velocityY": -1.480713963806038, + "timestamp": 0.45150347043229216 + }, + { + "x": 1.740332134586227, + "y": 5.070967309798755, + "heading": -0.29820239377925056, + "angularVelocity": -0.9876587544520216, + "velocityX": 1.4962976533331545, + "velocityY": -1.7274549450767482, + "timestamp": 0.5267540488376742 + }, + { + "x": 1.869023591989777, + "y": 4.922411591689274, + "heading": -0.38299783161333695, + "angularVelocity": -1.126841010806393, + "velocityX": 1.7101723352912572, + "velocityY": -1.9741471927192127, + "timestamp": 0.6020046272430563 + }, + { + "x": 2.0218400763966784, + "y": 4.778612689185939, + "heading": -0.4624367890743981, + "angularVelocity": -1.0556590945137452, + "velocityX": 2.030768236539849, + "velocityY": -1.9109341821756607, + "timestamp": 0.6772552056484383 + }, + { + "x": 2.158571900472673, + "y": 4.653385130962418, + "heading": -0.5314200615234237, + "angularVelocity": -0.9167141822805146, + "velocityX": 1.817020240554264, + "velocityY": -1.664140806319243, + "timestamp": 0.7525057840538204 + }, + { + "x": 2.2792154502458053, + "y": 4.546725640045298, + "heading": -0.5899528620113615, + "angularVelocity": -0.777838545939356, + "velocityX": 1.6032242187324346, + "velocityY": -1.4173909779475127, + "timestamp": 0.8277563624592025 + }, + { + "x": 2.383769251108378, + "y": 4.458632746098652, + "heading": -0.6380359031204368, + "angularVelocity": -0.6389723790566404, + "velocityX": 1.3894086009456479, + "velocityY": -1.170660688773468, + "timestamp": 0.9030069408645846 + }, + { + "x": 2.4722324605852046, + "y": 4.389105547577967, + "heading": -0.6756681828251195, + "angularVelocity": -0.5000928963223903, + "velocityX": 1.1755817875613708, + "velocityY": -0.9239423801653125, + "timestamp": 0.9782575192699666 + }, + { + "x": 2.5446045503525854, + "y": 4.33814344893939, + "heading": -0.7028480663782789, + "angularVelocity": -0.36119168954076036, + "velocityX": 0.9617479533181219, + "velocityY": -0.6772319857003534, + "timestamp": 1.0535080976753486 + }, + { + "x": 2.600885196215428, + "y": 4.305746063716301, + "heading": -0.7195737337553716, + "angularVelocity": -0.22226629657236585, + "velocityX": 0.7479098108675392, + "velocityY": -0.4305267269649537, + "timestamp": 1.1287586760807307 + }, + { + "x": 2.6410742311964075, + "y": 4.29191316967755, + "heading": -0.7258433495686913, + "angularVelocity": -0.08331651325714302, + "velocityX": 0.5340694494662552, + "velocityY": -0.1838244214447216, + "timestamp": 1.2040092544861127 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.057384071798383285, - "velocityX": 0.3111454080847126, - "velocityY": 0.06699234114487357, - "timestamp": 1.3523691316897029 - }, - { - "x": 2.6741706927639637, - "y": 4.318416553359305, - "heading": -0.7079150843864638, - "angularVelocity": 0.1820368667672511, - "velocityX": 0.11922552840405579, - "velocityY": 0.2884478427083078, - "timestamp": 1.4278485164558985 - }, - { - "x": 2.6686789976139393, - "y": 4.35689985788212, - "heading": -0.6847718372969428, - "angularVelocity": 0.306616795565399, - "velocityX": -0.0727575505371169, - "velocityY": 0.5098518574324268, - "timestamp": 1.5033279012220941 - }, - { - "x": 2.648690219871855, - "y": 4.4120894355267595, - "heading": -0.6522316141278892, - "angularVelocity": 0.4311140488042709, - "velocityX": -0.26482433323531773, - "velocityY": 0.7311874336773603, - "timestamp": 1.5788072859882898 - }, - { - "x": 2.6141955291979047, - "y": 4.483978018466003, - "heading": -0.6103014314016028, - "angularVelocity": 0.5555183425901422, - "velocityX": -0.4570081060213073, - "velocityY": 0.9524267210180263, - "timestamp": 1.6542866707544854 - }, - { - "x": 2.565181654912471, - "y": 4.572554575453533, - "heading": -0.5589890545924265, - "angularVelocity": 0.6798197543188936, - "velocityX": -0.6493676974062975, - "velocityY": 1.173519859229229, - "timestamp": 1.729766055520681 - }, - { - "x": 2.501626381973055, - "y": 4.6778004085012075, - "heading": -0.49830336104516576, - "angularVelocity": 0.8040035532069214, - "velocityX": -0.842021608129967, - "velocityY": 1.394365274326458, - "timestamp": 1.8052454402868767 - }, - { - "x": 2.4234850468656797, - "y": 4.799677413625942, - "heading": -0.42825793399699796, - "angularVelocity": 0.9280073925294743, - "velocityX": -1.0352672501499576, - "velocityY": 1.614705863061015, - "timestamp": 1.8807248250530724 - }, - { - "x": 2.330623714307083, - "y": 4.938069448938432, - "heading": -0.3489003984671445, - "angularVelocity": 1.0513802646058852, - "velocityX": -1.2302873539560828, - "velocityY": 1.8335077284709782, - "timestamp": 1.956204209819268 - }, - { - "x": 2.2158437322138607, - "y": 5.0727178717793375, - "heading": -0.2714542834500897, - "angularVelocity": 1.0260565220540956, - "velocityX": -1.5206798842301303, - "velocityY": 1.7839099147754294, - "timestamp": 2.0316835945854637 - }, - { - "x": 2.115226855971239, - "y": 5.19038091333927, - "heading": -0.20366940407384615, - "angularVelocity": 0.8980581861967815, - "velocityX": -1.333037842725262, - "velocityY": 1.5588765320192262, - "timestamp": 2.107162979351659 - }, - { - "x": 2.028914050239155, - "y": 5.291175475979272, - "heading": -0.14552121166963816, - "angularVelocity": 0.7703850870803018, - "velocityX": -1.1435282096780108, - "velocityY": 1.3353919478119864, - "timestamp": 2.1826423641178545 - }, - { - "x": 1.956951239222395, - "y": 5.375140495354652, - "heading": -0.09702855704513491, - "angularVelocity": 0.6424622401999767, - "velocityX": -0.9534101428822267, - "velocityY": 1.1124232085021941, - "timestamp": 2.25812174888405 - }, - { - "x": 1.8993610593828132, - "y": 5.442295470240139, - "heading": -0.05821541772623594, - "angularVelocity": 0.5142217234562786, - "velocityX": -0.7629921735099331, - "velocityY": 0.8897128016480877, - "timestamp": 2.3336011336502454 - }, - { - "x": 1.856157020573883, - "y": 5.492652116983436, - "heading": -0.029101141017864884, - "angularVelocity": 0.3857248810290673, - "velocityX": -0.5723952168077603, - "velocityY": 0.6671576205923756, - "timestamp": 2.4090805184164408 - }, - { - "x": 1.8273481914604963, - "y": 5.526218220177199, - "heading": -0.00969618965979829, - "angularVelocity": 0.2570894214180142, - "velocityX": -0.38167811253199, - "velocityY": 0.44470557488567636, - "timestamp": 2.484559903182636 - }, - { - "x": 1.8129411935806279, + "angularVelocity": 0.05565717315833812, + "velocityX": 0.3202286619480617, + "velocityY": 0.06287683198324669, + "timestamp": 1.2792598328914948 + }, + { + "x": 2.6731094248331972, + "y": 4.320288497186145, + "heading": -0.7067986870474628, + "angularVelocity": 0.19580509628770462, + "velocityX": 0.10461882238047351, + "velocityY": 0.31162123137301345, + "timestamp": 1.355133384348122 + }, + { + "x": 2.6646882165451484, + "y": 4.3628057123101, + "heading": -0.6813129128721624, + "angularVelocity": 0.33589799984345314, + "velocityX": -0.11099003706005371, + "velocityY": 0.5603693817898957, + "timestamp": 1.431006935804749 + }, + { + "x": 2.6399081500891253, + "y": 4.424196738035329, + "heading": -0.6452019314670412, + "angularVelocity": 0.47593635347047025, + "velocityX": -0.32659689681441373, + "velocityY": 0.8091228701785946, + "timestamp": 1.5068804872613761 + }, + { + "x": 2.5987694540063986, + "y": 4.504462102028234, + "heading": -0.5984691810768386, + "angularVelocity": 0.6159293916394221, + "velocityX": -0.5422007444352149, + "velocityY": 1.057883313117199, + "timestamp": 1.5827540387180032 + }, + { + "x": 2.5412724297468685, + "y": 4.603602434973848, + "heading": -0.5411158632570503, + "angularVelocity": 0.7559065935192721, + "velocityX": -0.7578006189995966, + "velocityY": 1.30665206837309, + "timestamp": 1.6586275901746304 + }, + { + "x": 2.4674174122162253, + "y": 4.7216183838553025, + "heading": -0.4731379534998271, + "angularVelocity": 0.8959368377013766, + "velocityX": -0.973396079566176, + "velocityY": 1.5554293507523191, + "timestamp": 1.7345011416312575 + }, + { + "x": 2.3772045458929667, + "y": 4.8585103095087545, + "heading": -0.3945211417087051, + "angularVelocity": 1.0361556864260284, + "velocityX": -1.1889896359316807, + "velocityY": 1.8042113888883347, + "timestamp": 1.8103746930878846 + }, + { + "x": 2.270632337424553, + "y": 5.014276859543665, + "heading": -0.3052336675309315, + "angularVelocity": 1.176793130987344, + "velocityX": -1.4046028744197523, + "velocityY": 2.0529756027560517, + "timestamp": 1.8862482445445117 + }, + { + "x": 2.1562113194234667, + "y": 5.146461372900032, + "heading": -0.22910323394931606, + "angularVelocity": 1.0033856610117322, + "velocityX": -1.5080487970369372, + "velocityY": 1.7421685266957554, + "timestamp": 1.9621217960011388 + }, + { + "x": 2.05813446327852, + "y": 5.259759383516201, + "heading": -0.1637433708814866, + "angularVelocity": 0.861431444990317, + "velocityX": -1.2926356320754544, + "velocityY": 1.49324775815898, + "timestamp": 2.037995347457766 + }, + { + "x": 1.9764036449833426, + "y": 5.354173277082403, + "heading": -0.10920518207306891, + "angularVelocity": 0.7188036906324902, + "velocityX": -1.077197741849454, + "velocityY": 1.2443584325978703, + "timestamp": 2.1138688989143932 + }, + { + "x": 1.9110189705233616, + "y": 5.429703862947866, + "heading": -0.06553401357536529, + "angularVelocity": 0.5755782833319475, + "velocityX": -0.8617584547542609, + "velocityY": 0.9954797741165524, + "timestamp": 2.1897424503710203 + }, + { + "x": 1.8619803278190585, + "y": 5.486351581956986, + "heading": -0.032764600292238745, + "angularVelocity": 0.43189507613676265, + "velocityX": -0.6463206448473131, + "velocityY": 0.7466069258864354, + "timestamp": 2.2656160018276474 + }, + { + "x": 1.8292876878125428, + "y": 5.524116686024045, + "heading": -0.010918035230093376, + "angularVelocity": 0.287933866844679, + "velocityX": -0.43088321791822976, + "velocityY": 0.4977373978420955, + "timestamp": 2.3414895532842745 + }, + { + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.264631493815946e-17, - "angularVelocity": 0.12846142943609742, - "velocityX": -0.19087328181241156, - "velocityY": 0.22232623455065786, - "timestamp": 2.5600392879488316 + "heading": 2.4600717676583652e-21, + "angularVelocity": 0.1438977749227231, + "velocityX": -0.21544390526203344, + "velocityY": 0.24886908799668503, + "timestamp": 2.4173631047409017 }, { - "x": 1.8129411935806274, + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 6.021257273210334e-18, - "angularVelocity": -5.990893004521945e-18, - "velocityX": -3.48414823730851e-16, - "velocityY": -1.72070697740397e-16, - "timestamp": 2.635518672715027 - }, - { - "x": 1.8467557558007979, - "y": 5.543052711491933, - "heading": -1.0880372931867694e-19, - "angularVelocity": -6.586811222708636e-17, - "velocityX": 0.3633408180161408, - "velocityY": 0.000574260143737945, - "timestamp": 2.7285843579986113 - }, - { - "x": 1.914384879084952, - "y": 5.543159599317721, - "heading": -3.505509945985968e-18, - "angularVelocity": -3.6497944505922797e-17, - "velocityX": 0.7266816236089445, - "velocityY": 0.0011485202678409793, - "timestamp": 2.8216500432821956 - }, - { - "x": 2.015828561351954, - "y": 5.543319931052201, - "heading": -4.327530361987157e-18, - "angularVelocity": -8.832690735719102e-18, - "velocityX": 1.090022406839742, - "velocityY": 0.0017227803566008666, - "timestamp": 2.91471572856578 - }, - { - "x": 2.1510867977458235, - "y": 5.543533706687698, - "heading": -2.821606902688151e-18, - "angularVelocity": 1.6181296623948357e-17, - "velocityX": 1.453363137892531, - "velocityY": 0.0022970403628934204, - "timestamp": 3.007781413849364 - }, - { - "x": 2.3201595639866595, - "y": 5.5438009261858365, - "heading": -4.181254623727939e-19, - "angularVelocity": 2.582564597014347e-17, - "velocityX": 1.8167036080554047, - "velocityY": 0.002871299956849509, - "timestamp": 3.1008470991329484 - }, - { - "x": 2.4554178003805287, - "y": 5.544014701821333, - "heading": 5.073319506573115e-18, - "angularVelocity": 5.900611973144966e-17, - "velocityX": 1.4533631378925314, - "velocityY": 0.0022970403628935166, - "timestamp": 3.1939127844165327 - }, - { - "x": 2.556861482647531, - "y": 5.544175033555812, - "heading": 6.802500422384327e-18, - "angularVelocity": 1.8580220092148724e-17, - "velocityX": 1.0900224068397424, - "velocityY": 0.001722780356600995, - "timestamp": 3.286978469700117 - }, - { - "x": 2.6244906059316855, - "y": 5.544281921381602, - "heading": 5.0686461896360996e-18, - "angularVelocity": -1.8630435349857743e-17, - "velocityX": 0.726681623608945, - "velocityY": 0.0011485202678411268, - "timestamp": 3.3800441549837013 - }, - { - "x": 2.6583051681518555, + "heading": 1.1389483058862038e-21, + "angularVelocity": -1.912999393800189e-21, + "velocityX": -4.5659699774939935e-20, + "velocityY": -6.062533120754166e-20, + "timestamp": 2.4932366561975288 + }, + { + "x": 1.8507557561824508, + "y": 5.5430527114919315, + "heading": -3.663099117513338e-19, + "angularVelocity": -3.946860070844576e-18, + "velocityX": 0.4061756707874381, + "velocityY": 0.0005740544395093343, + "timestamp": 2.586335690239902 + }, + { + "x": 1.9263848800936068, + "y": 5.543159599317717, + "heading": -1.3979636927804314e-18, + "angularVelocity": -1.108125118245985e-17, + "velocityX": 0.8123513276919049, + "velocityY": 0.0011481088593976478, + "timestamp": 2.6794347242822756 + }, + { + "x": 2.039828562987611, + "y": 5.543319931052194, + "heading": -3.639885752731889e-18, + "angularVelocity": -2.408104534113515e-17, + "velocityX": 1.2185269596070252, + "velocityY": 0.0017221632439681264, + "timestamp": 2.772533758324649 + }, + { + "x": 2.1910867994360017, + "y": 5.543533706687691, + "heading": -8.31799511662456e-18, + "angularVelocity": -5.0248742235094303e-17, + "velocityX": 1.6247025332136762, + "velocityY": 0.002296217546130332, + "timestamp": 2.8656327923670224 + }, + { + "x": 2.380159562296481, + "y": 5.5438009261858445, + "heading": -4.964025869598509e-18, + "angularVelocity": 3.602582219554166e-17, + "velocityX": 2.0308778152781284, + "velocityY": 0.0028702714362513803, + "timestamp": 2.958731826409396 + }, + { + "x": 2.5314177987448714, + "y": 5.544014701821341, + "heading": -1.164993974430083e-17, + "angularVelocity": -7.181507244960338e-17, + "velocityX": 1.6247025332136764, + "velocityY": 0.0022962175461303325, + "timestamp": 3.051830860451769 + }, + { + "x": 2.6448614816388756, + "y": 5.544175033555818, + "heading": -5.397697309971077e-18, + "angularVelocity": 6.71568990871082e-17, + "velocityX": 1.218526959607025, + "velocityY": 0.0017221632439681266, + "timestamp": 3.1449298944941426 + }, + { + "x": 2.7204906055500317, + "y": 5.544281921381604, + "heading": -1.7112683408390196e-18, + "angularVelocity": 3.959685518772811e-17, + "velocityX": 0.8123513276919049, + "velocityY": 0.001148108859397648, + "timestamp": 3.238028928536516 + }, + { + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -4.658926801043138e-23, - "angularVelocity": -5.446360561015685e-17, - "velocityX": 0.36334081801614126, - "velocityY": 0.0005742601437381063, - "timestamp": 3.4731098402672855 + "heading": 2.8361562565882696e-27, + "angularVelocity": 1.838116110697215e-17, + "velocityX": 0.4061756707874381, + "velocityY": 0.0005740544395093341, + "timestamp": 3.3311279625788894 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -4.8718739199241653e-23, - "angularVelocity": -2.2883735459903696e-23, - "velocityX": 1.6624416710559118e-21, - "velocityY": 9.919860077979884e-23, - "timestamp": 3.56617552555087 - }, - { - "x": 2.6433995634924674, - "y": 5.563703580385797, - "heading": 0.004845900450156495, - "angularVelocity": 0.061212191042318315, - "velocityX": -0.18828383484070044, - "velocityY": 0.24465440313021325, - "timestamp": 3.6453411356628544 - }, - { - "x": 2.613808012454941, - "y": 5.602604430475709, - "heading": 0.014722463610686642, - "angularVelocity": 0.12475825230878822, - "velocityX": -0.37379300172975427, - "velocityY": 0.4913857170415791, - "timestamp": 3.724506745774839 - }, - { - "x": 2.5698581719130473, - "y": 5.6612759426030985, - "heading": 0.02990473352989547, - "angularVelocity": 0.19177860055310075, - "velocityX": -0.5551632897128445, - "velocityY": 0.7411237284017034, - "timestamp": 3.8036723558868237 - }, - { - "x": 2.512090436903312, - "y": 5.74009257508762, - "heading": 0.05083960434186738, - "angularVelocity": 0.2644440026718447, - "velocityX": -0.7297074440280276, - "velocityY": 0.9955918027162334, - "timestamp": 3.8828379659988084 - }, - { - "x": 2.4415596148175696, - "y": 5.839724514645118, - "heading": 0.07837099812745095, - "angularVelocity": 0.34776961545093493, - "velocityX": -0.8909275376766785, - "velocityY": 1.2585255064233432, - "timestamp": 3.962003576110793 - }, - { - "x": 2.3611631890754614, - "y": 5.961650149114452, - "heading": 0.11464704720334487, - "angularVelocity": 0.4582298933157867, - "velocityX": -1.0155473522958047, - "velocityY": 1.5401338320624671, - "timestamp": 4.041169186222778 - }, - { - "x": 2.2902542502823713, - "y": 6.1057913956854275, - "heading": 0.1670437133636231, - "angularVelocity": 0.6618614583549578, - "velocityX": -0.8957038124608023, - "velocityY": 1.8207558353567832, - "timestamp": 4.120334796334762 - }, - { - "x": 2.241416295543653, - "y": 6.239392989227262, - "heading": 0.22223140963064505, - "angularVelocity": 0.6971170460122205, - "velocityX": -0.6169087141453696, - "velocityY": 1.6876215992379322, - "timestamp": 4.199500406446747 - }, - { - "x": 2.2118788438375976, - "y": 6.357956546172331, - "heading": 0.2767017364027319, - "angularVelocity": 0.688055415666413, - "velocityX": -0.3731096326330706, - "velocityY": 1.4976649175993666, - "timestamp": 4.278666016558732 - }, - { - "x": 2.2005734730100426, - "y": 6.460208388316336, - "heading": 0.32930652128307913, - "angularVelocity": 0.664490361483155, - "velocityX": -0.14280659002770119, - "velocityY": 1.2916194544495236, - "timestamp": 4.357831626670716 - }, - { - "x": 2.206951813042022, - "y": 6.545559671097223, - "heading": 0.37948004279494146, - "angularVelocity": 0.6337792564333017, - "velocityX": 0.08056958094502178, - "velocityY": 1.0781358554573413, - "timestamp": 4.436997236782701 - }, - { - "x": 2.2306819541123337, - "y": 6.613672661059128, - "heading": 0.4268844504712619, - "angularVelocity": 0.598800509580661, - "velocityX": 0.29975315085356163, - "velocityY": 0.8603860927182078, - "timestamp": 4.5161628468946855 - }, - { - "x": 2.271541794524346, - "y": 6.6643287076043505, - "heading": 0.4712947460181937, - "angularVelocity": 0.5609796410854498, - "velocityX": 0.516131188204235, - "velocityY": 0.6398743908316679, - "timestamp": 4.59532845700667 + "heading": 2.9956768552498905e-27, + "angularVelocity": 1.2964344677835282e-27, + "velocityX": -1.3759431197263344e-22, + "velocityY": -2.5705739302596115e-25, + "timestamp": 3.424226996621263 + }, + { + "x": 2.7425045239965504, + "y": 5.564231116573788, + "heading": 0.0035475080989150498, + "angularVelocity": 0.04647392993401548, + "velocityX": -0.20699544832908057, + "velocityY": 0.26064316841975155, + "timestamp": 3.5005602893716685 + }, + { + "x": 2.7110743119163434, + "y": 5.604155225406367, + "heading": 0.01083868300587181, + "angularVelocity": 0.09551762598264181, + "velocityX": -0.4117497221425694, + "velocityY": 0.523023537883034, + "timestamp": 3.576893582122074 + }, + { + "x": 2.6642693453223805, + "y": 5.6643007250236845, + "heading": 0.022164936239812177, + "angularVelocity": 0.14837894221299866, + "velocityX": -0.6131658272230635, + "velocityY": 0.7879327283048184, + "timestamp": 3.65322687487248 + }, + { + "x": 2.60250917282708, + "y": 5.744974159519079, + "heading": 0.038001263541227126, + "angularVelocity": 0.20746291337380784, + "velocityX": -0.8090856593497568, + "velocityY": 1.0568577823464311, + "timestamp": 3.7295601676228856 + }, + { + "x": 2.526611736042362, + "y": 5.846734963341242, + "heading": 0.05925383993510262, + "angularVelocity": 0.27841817938297336, + "velocityX": -0.994290093483679, + "velocityY": 1.333111675856818, + "timestamp": 3.8058934603732912 + }, + { + "x": 2.4388385302471143, + "y": 5.970894906525885, + "heading": 0.08830658815011594, + "angularVelocity": 0.3806038907558967, + "velocityX": -1.149867936160551, + "velocityY": 1.6265503387967306, + "timestamp": 3.882226753123697 + }, + { + "x": 2.357791815962569, + "y": 6.119054185868648, + "heading": 0.1383597950882533, + "angularVelocity": 0.655719216800998, + "velocityX": -1.0617479131884362, + "velocityY": 1.9409522896806517, + "timestamp": 3.9585600458741026 + }, + { + "x": 2.2985967230184703, + "y": 6.254273676601559, + "heading": 0.19308840139911443, + "angularVelocity": 0.7169690228065029, + "velocityX": -0.7754819792414106, + "velocityY": 1.7714353182044804, + "timestamp": 4.034893338624508 + }, + { + "x": 2.2587354140798723, + "y": 6.372988574603886, + "heading": 0.24867936598579424, + "angularVelocity": 0.7282662988016366, + "velocityX": -0.5222008314109583, + "velocityY": 1.5552178312351754, + "timestamp": 4.111226631374914 + }, + { + "x": 2.2373189281980324, + "y": 6.474209987526736, + "heading": 0.30392383910912424, + "angularVelocity": 0.7237271069121585, + "velocityX": -0.2805654663931197, + "velocityY": 1.3260454157771489, + "timestamp": 4.1875599241253205 + }, + { + "x": 2.233901000873839, + "y": 6.557480302062636, + "heading": 0.35823088728392755, + "angularVelocity": 0.7114464242015108, + "velocityX": -0.044776364297153176, + "velocityY": 1.090878062972822, + "timestamp": 4.263893216875727 + }, + { + "x": 2.248214127214941, + "y": 6.6225364653992, + "heading": 0.41124898234582946, + "angularVelocity": 0.6945605665834471, + "velocityX": 0.1875083050315006, + "velocityY": 0.8522646016238898, + "timestamp": 4.340226509626133 + }, + { + "x": 2.280080256208058, + "y": 6.669207835860357, + "heading": 0.4627444117016496, + "angularVelocity": 0.6746129703090314, + "velocityX": 0.41746042709454745, + "velocityY": 0.6114156586138921, + "timestamp": 4.416559802376539 }, { "x": 2.3293724060058594, "y": 6.6973748207092285, "heading": 0.5125504196, - "angularVelocity": 0.5211312528691123, - "velocityX": 0.7305016837451087, - "velocityY": 0.4174301575915679, - "timestamp": 4.674494067118655 - }, - { - "x": 2.3973241454559178, - "y": 6.73620444415628, - "heading": 0.5391342040901619, - "angularVelocity": 0.37821212966035134, - "velocityX": 0.9667612262295853, - "velocityY": 0.5524358122619254, - "timestamp": 4.744782094406503 + "angularVelocity": 0.652480799710893, + "velocityX": 0.6457490306225382, + "velocityY": 0.3689999976939454, + "timestamp": 4.492893095126945 }, { - "x": 2.48097335716082, - "y": 6.784004065158346, - "heading": 0.5372874666531788, - "angularVelocity": -0.02627385499695578, - "velocityX": 1.1900918966232588, - "velocityY": 0.6800535289789074, - "timestamp": 4.815070121694351 + "x": 2.4053669705551046, + "y": 6.740800351083451, + "heading": 0.5568713225103131, + "angularVelocity": 0.5531681306272371, + "velocityX": 0.948486345023332, + "velocityY": 0.5419930073900953, + "timestamp": 4.573015033569164 }, { - "x": 2.5479307987525304, - "y": 6.822265517551258, - "heading": 0.5291862310389086, - "angularVelocity": -0.11525768935146673, - "velocityX": 0.952615177510983, - "velocityY": 0.5443523437671909, - "timestamp": 4.885358148982199 + "x": 2.502567960679766, + "y": 6.796343857044693, + "heading": 0.5404821418694866, + "angularVelocity": -0.204552972125677, + "velocityX": 1.213163236118634, + "velocityY": 0.6932371712561282, + "timestamp": 4.653136972011383 }, { - "x": 2.5981417297757714, - "y": 6.850957521028096, - "heading": 0.5214396908605601, - "angularVelocity": -0.11021137563904544, - "velocityX": 0.7143596564122449, - "velocityY": 0.4082061281836414, - "timestamp": 4.955646176270047 + "x": 2.575459179910046, + "y": 6.837996044585651, + "heading": 0.5268532537172577, + "angularVelocity": -0.17010182750455535, + "velocityX": 0.9097535662201399, + "velocityY": 0.5198599578440831, + "timestamp": 4.7332589104536025 }, { - "x": 2.6316113826745062, - "y": 6.870083065561309, - "heading": 0.5156519889527618, - "angularVelocity": -0.08234264256835937, - "velocityX": 0.4761785782046117, - "velocityY": 0.27210245145860107, - "timestamp": 5.025934203557895 + "x": 2.6240500842159764, + "y": 6.865762317125851, + "heading": 0.5173922106041889, + "angularVelocity": -0.11808305311898519, + "velocityX": 0.6064619160577619, + "velocityY": 0.34655018438206425, + "timestamp": 4.813380848895822 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.044126567104521494, - "velocityX": 0.23806521146364915, - "velocityY": 0.13603746705807154, - "timestamp": 5.096222230845743 + "angularVelocity": -0.06043027787801988, + "velocityX": 0.30321823224102623, + "velocityY": 0.17326782030138713, + "timestamp": 4.893502787338041 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": 5.036255157215061e-18, - "velocityX": -7.194949871952118e-18, - "velocityY": -3.0975432357907215e-18, - "timestamp": 5.1665102581335915 - }, - { - "x": 2.654709594748088, - "y": 6.868188525457388, - "heading": 0.5112832103988345, - "angularVelocity": -0.02186863812315321, - "velocityX": 0.10984420500359952, - "velocityY": -0.19770584790906034, - "timestamp": 5.224456674093623 - }, - { - "x": 2.6675869589575, - "y": 6.845358903138629, - "heading": 0.5087434985964555, - "angularVelocity": -0.0438286261591562, - "velocityX": 0.2222288297915463, - "velocityY": -0.393978159658854, - "timestamp": 5.282403090053654 - }, - { - "x": 2.6871400637397453, - "y": 6.811251205912889, - "heading": 0.5049256931937168, - "angularVelocity": -0.06588509987168945, - "velocityX": 0.3374342391724767, - "velocityY": -0.5886075378547303, - "timestamp": 5.340349506013685 - }, - { - "x": 2.7135513143277277, - "y": 6.765975453621827, - "heading": 0.4998239367817842, - "angularVelocity": -0.08804265677871101, - "velocityX": 0.45578747452127555, - "velocityY": -0.7813382681388139, - "timestamp": 5.398295921973716 - }, - { - "x": 2.747025360653779, - "y": 6.709659972264173, - "heading": 0.49343216509494003, - "angularVelocity": -0.1103048666762139, - "velocityX": 0.5776724197945273, - "velocityY": -0.9718544352509818, - "timestamp": 5.456242337933747 - }, - { - "x": 2.7877931285642683, - "y": 6.642455993729292, - "heading": 0.4857442111344985, - "angularVelocity": -0.1326735024601774, - "velocityX": 0.703542526920187, - "velocityY": -1.159760744844629, - "timestamp": 5.514188753893778 - }, - { - "x": 2.836116770448945, - "y": 6.564543815779258, - "heading": 0.4767539799591755, - "angularVelocity": -0.15514732061277248, - "velocityX": 0.8339366824344734, - "velocityY": -1.3445555977745738, - "timestamp": 5.5721351698538095 - }, - { - "x": 2.8922957481362634, - "y": 6.4761411900512496, - "heading": 0.4664557366916879, - "angularVelocity": -0.17772010739333818, - "velocityX": 0.969498747361149, - "velocityY": -1.5255926404315645, - "timestamp": 5.630081585813841 - }, - { - "x": 2.9566742703700464, - "y": 6.377514946767211, - "heading": 0.4548445780033014, - "angularVelocity": -0.2003775125002176, - "velocityX": 1.111000933658922, - "velocityY": -1.7020249078401652, - "timestamp": 5.688028001773872 - }, - { - "x": 3.0296502478902108, - "y": 6.268997399320861, - "heading": 0.44191720438373994, - "angularVelocity": -0.2230918583207183, - "velocityX": 1.2593699940044558, - "velocityY": -1.8727223357731155, - "timestamp": 5.745974417733903 - }, - { - "x": 3.1116856852367225, - "y": 6.1510099051267035, - "heading": 0.4276731903359535, - "angularVelocity": -0.24581354708112682, - "velocityX": 1.4157120157888006, - "velocityY": -2.036148262828567, - "timestamp": 5.803920833693934 - }, - { - "x": 3.2033177251351583, - "y": 6.024097216910698, - "heading": 0.41211708268393216, - "angularVelocity": -0.26845676983265077, - "velocityX": 1.5813236829287303, - "velocityY": -2.1901732162959853, - "timestamp": 5.861867249653965 - }, - { - "x": 3.3051677702115847, - "y": 5.888977978331293, - "heading": 0.3952618666988256, - "angularVelocity": -0.2908759015695001, - "velocityX": 1.7576590957183351, - "velocityY": -2.3317963042374306, - "timestamp": 5.919813665613996 - }, - { - "x": 3.417941892615677, - "y": 5.746618387863477, - "heading": 0.37713462149882643, - "angularVelocity": -0.31282772022506217, - "velocityX": 1.9461794234501006, - "velocityY": -2.456745393295919, - "timestamp": 5.9777600815740275 - }, - { - "x": 3.542406775687406, - "y": 5.598335086950221, - "heading": 0.35778539162317, - "angularVelocity": -0.3339159041172737, - "velocityX": 2.147930652994672, - "velocityY": -2.5589727760822365, - "timestamp": 6.035706497534059 - }, - { - "x": 3.679310043544789, - "y": 5.445921240793454, - "heading": 0.337299824049247, - "angularVelocity": -0.3535260504816983, - "velocityX": 2.3625838732082087, - "velocityY": -2.6302549283098875, - "timestamp": 6.09365291349409 - }, - { - "x": 3.8292016626883134, - "y": 5.291748474573123, - "heading": 0.3158135059859917, - "angularVelocity": -0.3707963246264704, - "velocityX": 2.5867280427993067, - "velocityY": -2.660609179464564, - "timestamp": 6.151599329454121 - }, - { - "x": 3.9921508426109233, - "y": 5.138721146636673, - "heading": 0.29352014457634995, - "angularVelocity": -0.3847237320945793, - "velocityX": 2.8120665829446008, - "velocityY": -2.640841981357415, - "timestamp": 6.209545745414152 + "angularVelocity": 1.1533672139704627e-20, + "velocityX": 1.3759845110959627e-18, + "velocityY": -2.227992646788969e-18, + "timestamp": 4.97362472578026 + }, + { + "x": 2.6571120181256966, + "y": 6.867792271481583, + "heading": 0.507044677895467, + "angularVelocity": -0.09452799306194601, + "velocityX": 0.15052909368010856, + "velocityY": -0.20349709127294008, + "timestamp": 5.031869289387202 + }, + { + "x": 2.6746521578330125, + "y": 6.844087252656105, + "heading": 0.49617547030513726, + "angularVelocity": -0.18661325482116445, + "velocityX": 0.30114638381847736, + "velocityY": -0.40699109680773127, + "timestamp": 5.090113852994143 + }, + { + "x": 2.7009708210230947, + "y": 6.808530072252695, + "heading": 0.4801087501886783, + "angularVelocity": -0.27584926594838455, + "velocityX": 0.45186471595342564, + "velocityY": -0.6104806732412771, + "timestamp": 5.148358416601085 + }, + { + "x": 2.73607484037394, + "y": 6.7611211343580875, + "heading": 0.4590398225372461, + "angularVelocity": -0.361732088742461, + "velocityX": 0.602700358229852, + "velocityY": -0.8139633119159942, + "timestamp": 5.206602980208027 + }, + { + "x": 2.7799722580023465, + "y": 6.701861088549211, + "heading": 0.4332010241003249, + "angularVelocity": -0.4436259255248601, + "velocityX": 0.7536740754835817, + "velocityY": -1.0174347980145875, + "timestamp": 5.264847543814969 + }, + { + "x": 2.832672676333526, + "y": 6.630750980608444, + "heading": 0.40287238280523807, + "angularVelocity": -0.5207119672104833, + "velocityX": 0.9048126566253185, + "velocityY": -1.2208883290919084, + "timestamp": 5.32309210742191 + }, + { + "x": 2.8941877403139116, + "y": 6.547792490679094, + "heading": 0.3683969681281308, + "angularVelocity": -0.5919078544353333, + "velocityX": 1.0561511696699046, + "velocityY": -1.4243130138151228, + "timestamp": 5.381336671028852 + }, + { + "x": 2.9645318213846927, + "y": 6.452988329970027, + "heading": 0.33020407419243936, + "angularVelocity": -0.6557331975810253, + "velocityX": 1.2077364257631285, + "velocityY": -1.627691149835798, + "timestamp": 5.439581234635794 + }, + { + "x": 3.04372302387486, + "y": 6.346342948824998, + "heading": 0.28884632079593436, + "angularVelocity": -0.7100706200771626, + "velocityX": 1.3596325147971153, + "velocityY": -1.8309928779742386, + "timestamp": 5.497825798242736 + }, + { + "x": 3.1317847243622103, + "y": 6.227863919029491, + "heading": 0.24506332676078993, + "angularVelocity": -0.7517095386036364, + "velocityX": 1.5119299559290544, + "velocityY": -2.0341646062463727, + "timestamp": 5.556070361849677 + }, + { + "x": 3.2287480012560774, + "y": 6.097564965294096, + "heading": 0.199900944291634, + "angularVelocity": -0.7753922370151164, + "velocityX": 1.6647609817838902, + "velocityY": -2.237100695177408, + "timestamp": 5.614314925456619 + }, + { + "x": 3.334655409720646, + "y": 5.955473736284568, + "heading": 0.1549620363959118, + "angularVelocity": -0.7715554055652005, + "velocityX": 1.8183226365858802, + "velocityY": -2.4395620845993164, + "timestamp": 5.672559489063561 + }, + { + "x": 3.4495650121656865, + "y": 5.8016567423537415, + "heading": 0.11303057394210353, + "angularVelocity": -0.7199206218932102, + "velocityX": 1.972881163991502, + "velocityY": -2.6408815588154475, + "timestamp": 5.730804052670503 + }, + { + "x": 3.5735262627096924, + "y": 5.636333431818966, + "heading": 0.08010242298971296, + "angularVelocity": -0.565342907787981, + "velocityX": 2.1282887683826965, + "velocityY": -2.838433328309314, + "timestamp": 5.789048616277444 + }, + { + "x": 3.7050107789119795, + "y": 5.46057947422046, + "heading": 0.07776405618458973, + "angularVelocity": -0.04014738303996017, + "velocityX": 2.2574555985962665, + "velocityY": -3.017516944320591, + "timestamp": 5.847293179884386 + }, + { + "x": 3.8485069507845076, + "y": 5.2934525910716275, + "heading": 0.07776403563237232, + "angularVelocity": -3.5286069899218235e-7, + "velocityX": 2.463683526601701, + "velocityY": -2.8693988382619477, + "timestamp": 5.905537743491328 + }, + { + "x": 4.002902013863093, + "y": 5.136338903873299, + "heading": 0.07776401516857159, + "angularVelocity": -3.5134267388587325e-7, + "velocityX": 2.650806418956214, + "velocityY": -2.697482433873059, + "timestamp": 5.96378230709827 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.2706621692829818, - "angularVelocity": -0.3944674560916278, - "velocityX": 3.026040866948455, - "velocityY": -2.5674297245815976, - "timestamp": 6.267492161374183 - }, - { - "x": 4.262820189871526, - "y": 4.913621110649205, - "heading": 0.2585516106232336, - "angularVelocity": -0.39720693073137303, - "velocityX": 3.1263802397054365, - "velocityY": -2.5033930490319927, - "timestamp": 6.297981454794659 - }, - { - "x": 4.3609649027366135, - "y": 4.839574515326335, - "heading": 0.24639337135429903, - "angularVelocity": -0.3987707783588398, - "velocityX": 3.218989417418724, - "velocityY": -2.4286097516822993, - "timestamp": 6.3284707482151346 - }, - { - "x": 4.461671433105771, - "y": 4.768098656214637, - "heading": 0.234223352498736, - "angularVelocity": -0.3991571299366988, - "velocityX": 3.3030129291721897, - "velocityY": -2.344293720617927, - "timestamp": 6.35896004163561 - }, - { - "x": 4.564664131810475, - "y": 4.699441485698375, - "heading": 0.22207645249799987, - "angularVelocity": -0.3983988685196772, - "velocityX": 3.3779955896103364, - "velocityY": -2.251845248409488, - "timestamp": 6.389449335056086 - }, - { - "x": 4.669665105994921, - "y": 4.6338072685219265, - "heading": 0.20998565945136316, - "angularVelocity": -0.39655865027233667, - "velocityX": 3.4438638093832386, - "velocityY": -2.1526972196859626, - "timestamp": 6.419938628476562 - }, - { - "x": 4.7764036789009445, - "y": 4.571359258447394, - "heading": 0.19798140822304883, - "angularVelocity": -0.39372021721567657, - "velocityX": 3.500854264938127, - "velocityY": -2.0481947289927476, - "timestamp": 6.450427921897037 - }, - { - "x": 4.88462301845396, - "y": 4.512224581779958, - "heading": 0.1860912294102901, - "angularVelocity": -0.3899788246561105, - "velocityX": 3.5494210397259125, - "velocityY": -1.9395226990639944, - "timestamp": 6.480917215317513 - }, - { - "x": 4.994084052884085, - "y": 4.456499991660438, - "heading": 0.17433965070276516, - "angularVelocity": -0.3854329631526573, - "velocityX": 3.590146643300487, - "velocityY": -1.827677321052551, - "timestamp": 6.511406508737989 - }, - { - "x": 5.1045672063720575, - "y": 4.404257556871473, - "heading": 0.162748286386498, - "angularVelocity": -0.380178187679756, - "velocityX": 3.6236705116221697, - "velocityY": -1.71346820237825, - "timestamp": 6.541895802158464 - }, - { - "x": 5.215872594429427, - "y": 4.35554976560558, - "heading": 0.15133604569057543, - "angularVelocity": -0.3743032197731828, - "velocityX": 3.650638488808694, - "velocityY": -1.5975375550415924, - "timestamp": 6.57238509557894 - }, - { - "x": 5.327819247886724, - "y": 4.310413837972956, - "heading": 0.14011940090490202, - "angularVelocity": -0.36788798713571186, - "velocityX": 3.6716709670326204, - "velocityY": -1.4803861476931233, - "timestamp": 6.602874388999416 - }, - { - "x": 5.440243798322744, - "y": 4.268875232037166, - "heading": 0.1291126733064667, - "angularVelocity": -0.36100303954643975, - "velocityX": 3.6873452226518335, - "velocityY": -1.3623997566271735, - "timestamp": 6.633363682419891 - }, - { - "x": 5.552998921854952, - "y": 4.230950423566602, - "heading": 0.1183283109495842, - "angularVelocity": -0.3537098157093855, - "velocityX": 3.6981874908418817, - "velocityY": -1.243872986741426, - "timestamp": 6.663852975840367 + "heading": 0.07776399412255879, + "angularVelocity": -3.6133866407320583e-7, + "velocityX": 2.8259641989395843, + "velocityY": -2.513386639709188, + "timestamp": 6.022026870705211 + }, + { + "x": 4.268976588700125, + "y": 4.906724140265527, + "heading": 0.07776397443770015, + "angularVelocity": -5.672615420255969e-7, + "velocityX": 2.9242931040000055, + "velocityY": -2.398268643648094, + "timestamp": 6.056728427523216 + }, + { + "x": 4.37370366170879, + "y": 4.827628597038176, + "heading": 0.07776395550809555, + "angularVelocity": -5.454972726636976e-7, + "velocityX": 3.0179358683506776, + "velocityY": -2.2793082063198216, + "timestamp": 6.09142998434122 + }, + { + "x": 4.4815124411311835, + "y": 4.752787886123784, + "heading": 0.0777639371726549, + "angularVelocity": -5.283751607380448e-7, + "velocityX": 3.106741867167675, + "velocityY": -2.1566960614159445, + "timestamp": 6.126131541159224 + }, + { + "x": 4.592230128734662, + "y": 4.682321875197502, + "heading": 0.07776391928788347, + "angularVelocity": -5.153881575305611e-7, + "velocityX": 3.1905683132358433, + "velocityY": -2.030629671626745, + "timestamp": 6.160833097977228 + }, + { + "x": 4.705679213251417, + "y": 4.616343315466854, + "heading": 0.07776390172302422, + "angularVelocity": -5.061691995708879e-7, + "velocityX": 3.269279390309479, + "velocityY": -1.9013141132738893, + "timestamp": 6.195534654795233 + }, + { + "x": 4.821677528011508, + "y": 4.554957192131842, + "heading": 0.07776388435588834, + "angularVelocity": -5.004713755679868e-7, + "velocityX": 3.3427409429626933, + "velocityY": -1.7689731805681452, + "timestamp": 6.230236211613237 + }, + { + "x": 4.939562213059443, + "y": 4.4972764656411774, + "heading": 0.07776386706661859, + "angularVelocity": -4.982274959213974e-7, + "velocityX": 3.397100760239454, + "velocityY": -1.6621942004843482, + "timestamp": 6.264937768431241 + }, + { + "x": 5.0574486320523055, + "y": 4.439599283026335, + "heading": 0.07776384977740043, + "angularVelocity": -4.982260088149746e-7, + "velocityX": 3.397150727592071, + "velocityY": -1.6620920760798994, + "timestamp": 6.299639325249245 + }, + { + "x": 5.175337486723699, + "y": 4.381927078962276, + "heading": 0.07776383248820665, + "angularVelocity": -4.9822530674329e-7, + "velocityX": 3.3972209169079175, + "velocityY": -1.6619486084306254, + "timestamp": 6.3343408820672495 + }, + { + "x": 5.295110440251067, + "y": 4.328277244219036, + "heading": 0.07776381519347733, + "angularVelocity": -4.983848248451832e-7, + "velocityX": 3.451515277989629, + "velocityY": -1.5460353846547075, + "timestamp": 6.369042438885254 + }, + { + "x": 5.416934261342807, + "y": 4.279463089154562, + "heading": 0.07776379778914431, + "angularVelocity": -5.015432909319555e-7, + "velocityX": 3.5106154381100505, + "velocityY": -1.4066848735485782, + "timestamp": 6.403743995703258 + }, + { + "x": 5.540614064021598, + "y": 4.235563528216825, + "heading": 0.07776378015516597, + "angularVelocity": -5.081610154974924e-7, + "velocityX": 3.5640995396097326, + "velocityY": -1.26506027288553, + "timestamp": 6.438445552521262 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.10777714440346366, - "angularVelocity": -0.3460613665458462, - "velocityX": 3.704671190903868, - "velocityY": -1.1250293189517762, - "timestamp": 6.694342269260843 - }, - { - "x": 5.798356559097709, - "y": 4.161450057515513, - "heading": 0.09575175638596573, - "angularVelocity": -0.3367399103461797, - "velocityX": 3.7076550554500285, - "velocityY": -0.9856574960167961, - "timestamp": 6.730053474401752 - }, - { - "x": 5.9304803491986915, - "y": 4.131221114350942, - "heading": 0.0841228883191355, - "angularVelocity": -0.32563639398191835, - "velocityX": 3.699785251705895, - "velocityY": -0.8464834229282695, - "timestamp": 6.765764679542662 - }, - { - "x": 6.061872954800802, - "y": 4.105915842420486, - "heading": 0.07296106661592697, - "angularVelocity": -0.3125579677070111, - "velocityX": 3.679310319650609, - "velocityY": -0.7086087358465492, - "timestamp": 6.801475884683572 - }, - { - "x": 6.19201941031956, - "y": 4.085429275069175, - "heading": 0.062341869522057725, - "angularVelocity": -0.29736316800199314, - "velocityX": 3.6444151073934408, - "velocityY": -0.5736733686380603, - "timestamp": 6.837187089824481 - }, - { - "x": 6.320346294042347, - "y": 4.069574852509192, - "heading": 0.052342014729276326, - "angularVelocity": -0.28002008762580527, - "velocityX": 3.593462702152777, - "velocityY": -0.4439621261008996, - "timestamp": 6.872898294965391 - }, - { - "x": 6.446241767080791, - "y": 4.05806211105344, - "heading": 0.043033222480069355, - "angularVelocity": -0.2606686672285132, - "velocityX": 3.525377330215658, - "velocityY": -0.32238456838199153, - "timestamp": 6.908609500106301 - }, - { - "x": 6.569091568872717, - "y": 4.050483885589985, - "heading": 0.034475036219150636, - "angularVelocity": -0.2396498865593223, - "velocityX": 3.440091178866215, - "velocityY": -0.21220861725478038, - "timestamp": 6.94432070524721 - }, - { - "x": 6.688325744738062, - "y": 4.046323173396873, - "heading": 0.026709152612686058, - "angularVelocity": -0.21746349852429026, - "velocityX": 3.338844919819132, - "velocityY": -0.11650999109930767, - "timestamp": 6.98003191038812 - }, - { - "x": 6.803462305567311, - "y": 4.044983622354132, - "heading": 0.019757511591013074, - "angularVelocity": -0.19466273944659512, - "velocityX": 3.2241018015197396, - "velocityY": -0.03751066471868068, - "timestamp": 7.0157431155290295 - }, - { - "x": 6.914132827604236, - "y": 4.045835628751743, - "heading": 0.013624262542277448, - "angularVelocity": -0.17174578747848585, - "velocityX": 3.099041928163472, - "velocityY": 0.023858237050529835, - "timestamp": 7.051454320669939 - }, - { - "x": 7.020084299428127, - "y": 4.048262286463527, - "heading": 0.00829997269110531, - "angularVelocity": -0.1490929760045692, - "velocityX": 2.966897123909054, - "velocityY": 0.06795227722528199, - "timestamp": 7.087165525810849 - }, - { - "x": 7.1211630661166305, - "y": 4.051692576367301, - "heading": 0.003766298606880027, - "angularVelocity": -0.1269538248942407, - "velocityX": 2.8304496106940285, - "velocityY": 0.09605640275200633, - "timestamp": 7.122876730951758 + "heading": 0.07776376214569573, + "angularVelocity": -5.189816214982998e-7, + "velocityX": 3.6118744025389584, + "velocityY": -1.1214036841768227, + "timestamp": 6.4731471093392665 + }, + { + "x": 5.801257373802826, + "y": 4.160893829303833, + "heading": 0.07776374465549807, + "angularVelocity": -4.7264774236323325e-7, + "velocityX": 3.6564428162530316, + "velocityY": -0.9662347026048084, + "timestamp": 6.510151829987593 + }, + { + "x": 5.937965681779902, + "y": 4.1309456443700405, + "heading": 0.07776372758116286, + "angularVelocity": -4.614096500770092e-7, + "velocityX": 3.6943477908205034, + "velocityY": -0.8093071480907553, + "timestamp": 6.547156550635919 + }, + { + "x": 6.075827506190729, + "y": 4.1068588699437765, + "heading": 0.07776371078252527, + "angularVelocity": -4.53959313742904e-7, + "velocityX": 3.7255199335509808, + "velocityY": -0.6509108568923265, + "timestamp": 6.584161271284246 + }, + { + "x": 6.2145915217996235, + "y": 4.088676368882122, + "heading": 0.0777636941280861, + "angularVelocity": -4.5006255582880887e-7, + "velocityX": 3.7499003688646537, + "velocityY": -0.4913562578799624, + "timestamp": 6.621165991932572 + }, + { + "x": 6.354004262869319, + "y": 4.076423083568819, + "heading": 0.07776367749075697, + "angularVelocity": -4.496001822495745e-7, + "velocityX": 3.7674312527474547, + "velocityY": -0.33112762638453114, + "timestamp": 6.658170712580898 + }, + { + "x": 6.49381285233259, + "y": 4.070129387117659, + "heading": 0.07776362451928018, + "angularVelocity": -0.0000014314788993599032, + "velocityX": 3.778128493170848, + "velocityY": -0.17007820464237144, + "timestamp": 6.695175433229225 + }, + { + "x": 6.629142082966274, + "y": 4.065515785319137, + "heading": 0.06238171659626575, + "angularVelocity": -0.4156742073314349, + "velocityX": 3.657080184979125, + "velocityY": -0.12467603369760688, + "timestamp": 6.732180153877551 + }, + { + "x": 6.758586834582697, + "y": 4.061933354582955, + "heading": 0.046880370354106624, + "angularVelocity": -0.4189018582108937, + "velocityX": 3.4980605000804377, + "velocityY": -0.0968101008038334, + "timestamp": 6.769184874525878 + }, + { + "x": 6.882125546140559, + "y": 4.059204781585899, + "heading": 0.032519289448839375, + "angularVelocity": -0.3880878075461594, + "velocityX": 3.338458158674076, + "velocityY": -0.07373580854685112, + "timestamp": 6.806189595174204 + }, + { + "x": 6.999755813322696, + "y": 4.05726367776388, + "heading": 0.01976964076410941, + "angularVelocity": -0.34454114127480795, + "velocityX": 3.1787908440123913, + "velocityY": -0.0524555729109991, + "timestamp": 6.84319431582253 + }, + { + "x": 7.111477612575904, + "y": 4.056075301962013, + "heading": 0.008878948528675913, + "angularVelocity": -0.2943054843984055, + "velocityX": 3.0191228928587734, + "velocityY": -0.03211416762637815, + "timestamp": 6.880199036470857 }, { "x": 7.217291355133057, "y": 4.0556182861328125, - "heading": -1.0909016317355304e-17, - "angularVelocity": -0.10546545802684394, - "velocityX": 2.6918242786016986, - "velocityY": 0.10992935550682857, - "timestamp": 7.158587936092668 - }, - { - "x": 7.368999916586432, - "y": 4.062586724556833, - "heading": -0.004298989973913876, - "angularVelocity": -0.06943407043774014, - "velocityX": 2.450283207423163, - "velocityY": 0.11254900507105683, - "timestamp": 7.2205026408883315 - }, - { - "x": 7.505752960791366, - "y": 4.0691868405641705, - "heading": -0.006878875376785559, - "angularVelocity": -0.041668379287055086, - "velocityX": 2.208732879471196, - "velocityY": 0.1066001368999474, - "timestamp": 7.282417345683995 - }, - { - "x": 7.627563527820959, - "y": 4.075012053328196, - "heading": -0.008132420768696622, - "angularVelocity": -0.02024632752507115, - "velocityX": 1.9673931650260594, - "velocityY": 0.09408447933734543, - "timestamp": 7.344332050479658 - }, - { - "x": 7.734450186690855, - "y": 4.079741519123993, - "heading": -0.008370221734057366, - "angularVelocity": -0.0038407833186891597, - "velocityX": 1.726353363432043, - "velocityY": 0.07638679391923996, - "timestamp": 7.406246755275322 - }, - { - "x": 7.826433301006775, - "y": 4.083115946879114, - "heading": -0.007843874074281608, - "angularVelocity": 0.00850117369553605, - "velocityX": 1.485642459565828, - "velocityY": 0.05450123304726754, - "timestamp": 7.468161460070985 - }, - { - "x": 7.903533270640132, - "y": 4.084921624600937, - "heading": -0.00676122659439774, - "angularVelocity": 0.01748611228070863, - "velocityX": 1.2452610391636303, - "velocityY": 0.029163955925833308, - "timestamp": 7.5300761648666485 - }, - { - "x": 7.965769715964552, - "y": 4.0849794807879745, - "heading": -0.005296815188372533, - "angularVelocity": 0.023652077658421113, - "velocityX": 1.0051965123603401, - "velocityY": 0.000934449856933646, - "timestamp": 7.591990869662312 - }, - { - "x": 8.013161126878536, - "y": 4.083137358866124, - "heading": -0.0035992352155136144, - "angularVelocity": 0.027418041941110364, - "velocityX": 0.7654306205672696, - "velocityY": -0.029752575384646835, - "timestamp": 7.653905574457975 - }, - { - "x": 8.045724744989489, - "y": 4.079264414164358, - "heading": -0.001796495631167538, - "angularVelocity": 0.02911650132703844, - "velocityX": 0.5259432023203758, - "velocityY": -0.06255290588153085, - "timestamp": 7.715820279253639 + "heading": -2.4974876469169754e-20, + "angularVelocity": -0.23994096896600625, + "velocityX": 2.8594660546893573, + "velocityY": -0.012350203465742983, + "timestamp": 6.917203757119183 + }, + { + "x": 7.386543487491071, + "y": 4.0570668323942645, + "heading": -0.009095721053457144, + "angularVelocity": -0.13843657850499821, + "velocityX": 2.57601194788322, + "velocityY": 0.022046826970953105, + "timestamp": 6.982906919908118 + }, + { + "x": 7.537031363324535, + "y": 4.05969750834718, + "heading": -0.013766029362825559, + "angularVelocity": -0.07108194052044785, + "velocityX": 2.2904205740732744, + "velocityY": 0.04003880241453354, + "timestamp": 7.048610082697054 + }, + { + "x": 7.6687074557367385, + "y": 4.062834301345331, + "heading": -0.015395893103822788, + "angularVelocity": -0.02480647311048019, + "velocityX": 2.0041058424417098, + "velocityY": 0.04774188737652573, + "timestamp": 7.114313245485989 + }, + { + "x": 7.781556988474885, + "y": 4.066015749956383, + "heading": -0.01492426229398527, + "angularVelocity": 0.007178205581253095, + "velocityX": 1.7175662167232906, + "velocityY": 0.04842154435201139, + "timestamp": 7.180016408274924 + }, + { + "x": 7.8755781593330285, + "y": 4.068907471663171, + "heading": -0.013029646010250598, + "angularVelocity": 0.028835998197239424, + "velocityX": 1.4309991614890225, + "velocityY": 0.044011910295353764, + "timestamp": 7.2457195710638596 + }, + { + "x": 7.950774573438036, + "y": 4.07125628842811, + "heading": -0.010225311537090487, + "angularVelocity": 0.042681879442679815, + "velocityX": 1.144486976168374, + "velocityY": 0.035748914743771835, + "timestamp": 7.311422733852795 + }, + { + "x": 8.007152043467745, + "y": 4.072863968231118, + "heading": -0.006913103442788773, + "angularVelocity": 0.0504116994328239, + "velocityX": 0.8580632596152882, + "velocityY": 0.024468834296048715, + "timestamp": 7.37712589664173 + }, + { + "x": 8.044717148266278, + "y": 4.073571148367015, + "heading": -0.0034161675760999402, + "angularVelocity": 0.053223250118451255, + "velocityX": 0.5717396728557544, + "velocityY": 0.010763258660280774, + "timestamp": 7.442829059430665 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 2.0297812570188607e-18, - "angularVelocity": 0.029015653665741454, - "velocityX": 0.2867140781676597, - "velocityY": -0.09718948531913073, - "timestamp": 7.777734984049302 - }, - { - "x": 8.063706917248451, - "y": 4.062947102090978, - "heading": 0.001985154079980339, - "angularVelocity": 0.027021345733527664, - "velocityX": 0.003135522507827122, - "velocityY": -0.1401986439325952, - "timestamp": 7.851201128169473 - }, - { - "x": 8.043035079312682, - "y": 4.049980112254385, - "heading": 0.003821402596114743, - "angularVelocity": 0.024994486074167273, - "velocityX": -0.2813791057545805, - "velocityY": -0.17650293195437572, - "timestamp": 7.924667272289644 - }, - { - "x": 8.001396508446343, - "y": 4.034912807351801, - "heading": 0.0055060818689868455, - "angularVelocity": 0.022931369177568325, - "velocityX": -0.5667722372665807, - "velocityY": -0.2050918158701582, - "timestamp": 7.998133416409814 - }, - { - "x": 7.938735716407508, - "y": 4.018402810035911, - "heading": 0.007036218210445593, - "angularVelocity": 0.020827775294097833, - "velocityX": -0.8529206587504942, - "velocityY": -0.2247293295927428, - "timestamp": 8.071599560529984 - }, - { - "x": 7.855014011007738, - "y": 4.00121976392101, - "heading": 0.008408488071179075, - "angularVelocity": 0.018678942214372884, - "velocityX": -1.139595747162445, - "velocityY": -0.2338906760479261, - "timestamp": 8.145065704650154 - }, - { - "x": 7.750221962754556, - "y": 3.9842725768704743, - "heading": 0.00961918059388702, - "angularVelocity": 0.016479598013576947, - "velocityX": -1.4263991871108765, - "velocityY": -0.23068023037679872, - "timestamp": 8.218531848770324 - }, - { - "x": 7.62439956654467, - "y": 3.9686442308926377, - "heading": 0.010664172857875897, - "angularVelocity": 0.014224133803450148, - "velocityX": -1.7126582280413951, - "velocityY": -0.21272854544091283, - "timestamp": 8.291997992890494 - }, - { - "x": 7.477668976002092, - "y": 3.955635460155464, - "heading": 0.011538940075399833, - "angularVelocity": 0.01190707948538909, - "velocityX": -1.9972545490146683, - "velocityY": -0.17707164154273572, - "timestamp": 8.365464137010663 - }, - { - "x": 7.3102875993376655, - "y": 3.9468171160612413, - "heading": 0.012238647278142662, - "angularVelocity": 0.009524212970784032, - "velocityX": -2.2783471035397658, - "velocityY": -0.12003276066589603, - "timestamp": 8.438930281130833 - }, - { - "x": 7.122733099589339, - "y": 3.9440864335777195, - "heading": 0.012758416420278173, - "angularVelocity": 0.0070749479009718985, - "velocityX": -2.5529378463301264, - "velocityY": -0.037169263695877315, - "timestamp": 8.512396425251003 - }, - { - "x": 6.915834286392237, - "y": 3.9497111054839014, - "heading": 0.013093946664375935, - "angularVelocity": 0.004567141070435388, - "velocityX": -2.816247071013707, - "velocityY": 0.07656141442487652, - "timestamp": 8.585862569371173 - }, - { - "x": 6.690954998420113, - "y": 3.9663229006089162, - "heading": 0.01324277694660248, - "angularVelocity": 0.00202583494763381, - "velocityX": -3.060992116372438, - "velocityY": 0.22611497205899783, - "timestamp": 8.659328713491343 - }, - { - "x": 6.450203582137421, - "y": 3.9967943470851464, - "heading": 0.013206490081435995, - "angularVelocity": -0.0004939263602446482, - "velocityX": -3.277038956732079, - "velocityY": 0.4147685555184044, - "timestamp": 8.732794857611513 - }, - { - "x": 6.196564054073483, - "y": 4.043936461503449, - "heading": 0.012993660347291816, - "angularVelocity": -0.0028969770592021916, - "velocityX": -3.4524682233091277, - "velocityY": 0.6416848874114194, - "timestamp": 8.806261001731682 - }, - { - "x": 5.933782874794111, - "y": 4.110063969722605, - "heading": 0.012622011909817094, - "angularVelocity": -0.005058771518848182, - "velocityX": -3.576901747416247, - "velocityY": 0.9001086011944357, - "timestamp": 8.879727145851852 + "heading": 5.107015447929928e-20, + "angularVelocity": 0.05199395936347874, + "velocityX": 0.28551767430107416, + "velocityY": -0.004934199232898976, + "timestamp": 7.508532222219601 + }, + { + "x": 8.06095184830656, + "y": 4.071458641245482, + "heading": 0.0034408492569327057, + "angularVelocity": 0.04684199855212652, + "velocityX": -0.034370194612683154, + "velocityY": -0.02434521970346025, + "timestamp": 7.581988722012275 + }, + { + "x": 8.034920990250148, + "y": 4.068388525754522, + "heading": 0.006505362431754428, + "angularVelocity": 0.041718747605332625, + "velocityX": -0.3543710649143617, + "velocityY": -0.041795014731511364, + "timestamp": 7.655445221804949 + }, + { + "x": 7.985375296011398, + "y": 4.064207377475085, + "heading": 0.009196043333328236, + "angularVelocity": 0.036629582258453454, + "velocityX": -0.6744902680986578, + "velocityY": -0.05692005869104256, + "timestamp": 7.728901721597623 + }, + { + "x": 7.912305948559603, + "y": 4.059120814150924, + "heading": 0.011515917649017375, + "angularVelocity": 0.03158160710402538, + "velocityX": -0.9947295019232808, + "velocityY": -0.06924592566372595, + "timestamp": 7.802358221390297 + }, + { + "x": 7.815704588785296, + "y": 4.0533811177027905, + "heading": 0.013468719297185195, + "angularVelocity": 0.026584463644190544, + "velocityX": -1.3150825324778364, + "velocityY": -0.0781373529140697, + "timestamp": 7.875814721182971 + }, + { + "x": 7.695564554163692, + "y": 4.047305026722375, + "heading": 0.015059175300172275, + "angularVelocity": 0.021651671499132708, + "velocityX": -1.6355262633081036, + "velocityY": -0.08271685960487347, + "timestamp": 7.949271220975645 + }, + { + "x": 7.551883548681463, + "y": 4.04130179428642, + "heading": 0.016293463732843762, + "angularVelocity": 0.016802984571211377, + "velocityX": -1.9560012509139277, + "velocityY": -0.08172499986929728, + "timestamp": 8.02272772076832 + }, + { + "x": 7.384669657766205, + "y": 4.035920047959674, + "heading": 0.017179998132389497, + "angularVelocity": 0.012068835324959903, + "velocityX": -2.276366167557798, + "velocityY": -0.07326439922859854, + "timestamp": 8.096184220560993 + }, + { + "x": 7.19395595849013, + "y": 4.0319320028167684, + "heading": 0.017730886759136543, + "angularVelocity": 0.007499521870792628, + "velocityX": -2.5962807895060482, + "velocityY": -0.054291249299417726, + "timestamp": 8.169640720353666 + }, + { + "x": 6.979840325568109, + "y": 4.030500043249597, + "heading": 0.017964968086133067, + "angularVelocity": 0.00318666595409774, + "velocityX": -2.9148629941032773, + "velocityY": -0.01949398039945934, + "timestamp": 8.243097220146339 + }, + { + "x": 6.742614358141766, + "y": 4.033551118259164, + "heading": 0.017915208666011314, + "angularVelocity": -0.0006773998252331979, + "velocityX": -3.22947551402392, + "velocityY": 0.041535807153613755, + "timestamp": 8.316553719939012 + }, + { + "x": 6.483307205091183, + "y": 4.044763583343556, + "heading": 0.017651500017929662, + "angularVelocity": -0.003589997465520818, + "velocityX": -3.530077716505134, + "velocityY": 0.1526408842789682, + "timestamp": 8.390010219731685 + }, + { + "x": 6.206941447635851, + "y": 4.072199279636492, + "heading": 0.017375156748179146, + "angularVelocity": -0.0037619988773032, + "velocityX": -3.762305013652376, + "velocityY": 0.37349582910118817, + "timestamp": 8.463466719524359 + }, + { + "x": 5.9338077856619895, + "y": 4.122953167788701, + "heading": 0.017375155210986027, + "angularVelocity": -2.0926577278102936e-8, + "velocityX": -3.7183048844521998, + "velocityY": 0.690938014953861, + "timestamp": 8.536923219317032 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.012117456973340398, - "angularVelocity": -0.0068678565143064085, - "velocityX": -3.6456404399720617, - "velocityY": 1.1785715157475694, - "timestamp": 8.953193289972022 - }, - { - "x": 5.55330454345789, - "y": 4.2367101473374795, - "heading": 0.011887459641735761, - "angularVelocity": -0.007456725508590102, - "velocityX": -3.6521255907888213, - "velocityY": 1.2988169090424007, - "timestamp": 8.98403757025828 - }, - { - "x": 5.440517148405761, - "y": 4.28048284495712, - "heading": 0.011638752310585518, - "angularVelocity": -0.008063320941259012, - "velocityX": -3.6566713181625206, - "velocityY": 1.4191512077245991, - "timestamp": 9.014881850544539 - }, - { - "x": 5.327667581738716, - "y": 4.327969293579141, - "heading": 0.011370630767981954, - "angularVelocity": -0.008692747573138404, - "velocityX": -3.6586869792296546, - "velocityY": 1.5395544386611915, - "timestamp": 9.045726130830797 - }, - { - "x": 5.214861744178507, - "y": 4.379169755070302, - "heading": 0.011082156069971438, - "angularVelocity": -0.009352615633539172, - "velocityX": -3.6572692412752423, - "velocityY": 1.659966159559613, - "timestamp": 9.076570411117055 - }, - { - "x": 5.1022510887558585, - "y": 4.434079353427009, - "heading": 0.010772019648005773, - "angularVelocity": -0.010054908692547623, - "velocityX": -3.650941256451206, - "velocityY": 1.7802197959265815, - "timestamp": 9.107414691403314 - }, - { - "x": 4.990068460344294, - "y": 4.4926784730980485, - "heading": 0.010438291328946931, - "angularVelocity": -0.010819779743978382, - "velocityX": -3.637064226184596, - "velocityY": 1.8998374780411038, - "timestamp": 9.138258971689572 - }, - { - "x": 4.878710505114531, - "y": 4.5548991474962115, - "heading": 0.010077925553591442, - "angularVelocity": -0.011683390632267332, - "velocityX": -3.6103275614238326, - "velocityY": 2.0172516207448092, - "timestamp": 9.16910325197583 - }, - { - "x": 4.768951534707332, - "y": 4.620472273914983, - "heading": 0.009685974313613142, - "angularVelocity": -0.012707420511702754, - "velocityX": -3.558486999487417, - "velocityY": 2.1259412056368423, - "timestamp": 9.199947532262089 - }, - { - "x": 4.662298486421541, - "y": 4.688082271388773, - "heading": 0.009258221595846085, - "angularVelocity": -0.013868137424417113, - "velocityX": -3.4577901411855687, - "velocityY": 2.19197844288524, - "timestamp": 9.230791812548347 - }, - { - "x": 4.559169523977437, - "y": 4.754519038672898, - "heading": 0.008807731583591003, - "angularVelocity": -0.014605301471592353, - "velocityX": -3.3435360295972387, - "velocityY": 2.1539412386199186, - "timestamp": 9.261636092834605 - }, - { - "x": 4.45859769074999, - "y": 4.818261954197611, - "heading": 0.008349427916360263, - "angularVelocity": -0.014858627368748727, - "velocityX": -3.260631543160092, - "velocityY": 2.0666040813119846, - "timestamp": 9.292480373120863 - }, - { - "x": 4.359992719048881, - "y": 4.878854116841475, - "heading": 0.007890638381237635, - "angularVelocity": -0.014874379653690404, - "velocityX": -3.1968640793682743, - "velocityY": 1.964453768462782, - "timestamp": 9.323324653407122 - }, - { - "x": 4.263026916364364, - "y": 4.936113272994566, - "heading": 0.007435172084260176, - "angularVelocity": -0.014766637209570182, - "velocityX": -3.1437207088185697, - "velocityY": 1.856394625573387, - "timestamp": 9.35416893369338 + "heading": 0.017375154199310676, + "angularVelocity": -1.377244159549356e-8, + "velocityX": -3.646458211284153, + "velocityY": 1.003259166632557, + "timestamp": 8.610379719109705 + }, + { + "x": 5.541092339335072, + "y": 4.2364336029082, + "heading": 0.01737515315796098, + "angularVelocity": -3.005342133477009e-8, + "velocityX": -3.6034502673557895, + "velocityY": 1.148184128750311, + "timestamp": 8.645029674075602 + }, + { + "x": 5.417922638814364, + "y": 4.281176215218788, + "heading": 0.017375152214225534, + "angularVelocity": -2.7236267527590686e-8, + "velocityX": -3.5546857316822638, + "velocityY": 1.2912747608077488, + "timestamp": 8.679679629041498 + }, + { + "x": 5.296639420951804, + "y": 4.33080542124118, + "heading": 0.017375151348717865, + "angularVelocity": -2.4978608726563434e-8, + "velocityX": -3.500241716964063, + "velocityY": 1.4323021796489428, + "timestamp": 8.714329584007395 + }, + { + "x": 5.177436464453021, + "y": 4.385241922289634, + "heading": 0.017375150546348885, + "angularVelocity": -2.3156422113484916e-8, + "velocityX": -3.4402052359405113, + "velocityY": 1.571041033156597, + "timestamp": 8.748979538973291 + }, + { + "x": 5.0605042229676425, + "y": 4.444398737399711, + "heading": 0.017375149795171643, + "angularVelocity": -2.167902498222188e-8, + "velocityX": -3.374672249948487, + "velocityY": 1.7072696102577922, + "timestamp": 8.783629493939188 + }, + { + "x": 4.946029519855076, + "y": 4.5081813412028975, + "heading": 0.017375149085535665, + "angularVelocity": -2.0480141439242773e-8, + "velocityX": -3.3037475294046756, + "velocityY": 1.8407701789500717, + "timestamp": 8.818279448905084 + }, + { + "x": 4.834195247479412, + "y": 4.576487812332227, + "heading": 0.017375148409491465, + "angularVelocity": -1.9510680466596443e-8, + "velocityX": -3.2275445230947857, + "velocityY": 1.9713292902272803, + "timestamp": 8.85292940387098 + }, + { + "x": 4.725180068322848, + "y": 4.649208988191292, + "heading": 0.01737514776036044, + "angularVelocity": -1.8733964504925207e-8, + "velocityX": -3.146185305691103, + "velocityY": 2.0987379617271817, + "timestamp": 8.887579358836877 + }, + { + "x": 4.619158095512492, + "y": 4.726228597192443, + "heading": 0.017375147132049722, + "angularVelocity": -1.813308899206411e-8, + "velocityX": -3.059801171883295, + "velocityY": 2.222791027490615, + "timestamp": 8.922229313802774 + }, + { + "x": 4.51014571232548, + "y": 4.798953964824882, + "heading": 0.01737514649311973, + "angularVelocity": -1.8439562026762692e-8, + "velocityX": -3.146104613824239, + "velocityY": 2.0988589365849712, + "timestamp": 8.95687926876867 + }, + { + "x": 4.398314099643168, + "y": 4.867264790943429, + "heading": 0.017375145827920196, + "angularVelocity": -1.9197702730409006e-8, + "velocityX": -3.2274677641681775, + "velocityY": 1.9714549755050896, + "timestamp": 8.991529223734567 + }, + { + "x": 4.2838418935996625, + "y": 4.931051865994745, + "heading": 0.017375140544826046, + "angularVelocity": -1.5247044777404565e-7, + "velocityX": -3.3036754638258423, + "velocityY": 1.840899219467885, + "timestamp": 9.026179178700463 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.006985288099960263, - "angularVelocity": -0.014585653486632353, - "velocityX": -3.097100988534025, - "velocityY": 1.7453648577216743, - "timestamp": 9.385013213979638 - }, - { - "x": 3.9486678661427668, - "y": 5.097895325970227, - "heading": 0.00595988192546784, - "angularVelocity": -0.014043783913272705, - "velocityX": -2.9970738935334476, - "velocityY": 1.4784305228888592, - "timestamp": 9.458028163570491 - }, - { - "x": 3.7418449106776346, - "y": 5.188842546635376, - "heading": 0.004995539128350481, - "angularVelocity": -0.01320747055940148, - "velocityX": -2.8326110834026332, - "velocityY": 1.2455972533677366, - "timestamp": 9.531043113161344 - }, - { - "x": 3.5499887292854875, - "y": 5.265325878573737, - "heading": 0.004104715734621832, - "angularVelocity": -0.01220056164827184, - "velocityX": -2.6276287591409173, - "velocityY": 1.0475023589955905, - "timestamp": 9.604058062752197 - }, - { - "x": 3.374901152496024, - "y": 5.329479478594356, - "heading": 0.0032944686159963646, - "angularVelocity": -0.011097003054385247, - "velocityX": -2.397968878573332, - "velocityY": 0.8786365036216773, - "timestamp": 9.67707301234305 - }, - { - "x": 3.217704545762821, - "y": 5.382997215745527, - "heading": 0.002568947278727608, - "angularVelocity": -0.0099366135474215, - "velocityX": -2.1529372767367914, - "velocityY": 0.7329695829561359, - "timestamp": 9.750087961933902 - }, - { - "x": 3.079126437625259, - "y": 5.42721119539832, - "heading": 0.0019307216893178917, - "angularVelocity": -0.008741026227999762, - "velocityX": -1.8979415710631973, - "velocityY": 0.6055469448455648, - "timestamp": 9.823102911524755 - }, - { - "x": 2.959658381908625, - "y": 5.463178096937211, - "heading": 0.0013814652459932491, - "angularVelocity": -0.007522520338676778, - "velocityX": -1.6362136300317622, - "velocityY": 0.49259640307136626, - "timestamp": 9.896117861115608 - }, - { - "x": 2.8596453147458623, - "y": 5.491748253962881, - "heading": 0.0009223163485468854, - "angularVelocity": -0.006288423124569153, - "velocityX": -1.369761503954984, - "velocityY": 0.3912918818100397, - "timestamp": 9.96913281070646 - }, - { - "x": 2.779337430145008, - "y": 5.5136166054710944, - "heading": 0.0005540788250022006, - "angularVelocity": -0.0050433168222144735, - "velocityX": -1.0998827644320655, - "velocityY": 0.29950512368706583, - "timestamp": 10.042147760297313 - }, - { - "x": 2.718921448994663, - "y": 5.529359550737521, - "heading": 0.00027733842586429876, - "angularVelocity": -0.003790188183223442, - "velocityX": -0.8274467282233734, - "velocityY": 0.21561262939498888, - "timestamp": 10.115162709888166 - }, - { - "x": 2.678540177748154, - "y": 5.539461677753878, - "heading": 0.0000925335672086805, - "angularVelocity": -0.002531055074216922, - "velocityX": -0.5530548397662466, - "velocityY": 0.13835696762054583, - "timestamp": 10.188177659479019 + "heading": 0.01686674274932993, + "angularVelocity": -0.014672394119890892, + "velocityX": -3.3576617434279026, + "velocityY": 1.6997404450060698, + "timestamp": 9.06082913366636 + }, + { + "x": 3.941206209015057, + "y": 5.0867984564258695, + "heading": 0.014981559594595455, + "angularVelocity": -0.027398080889315734, + "velocityX": -3.288799800865747, + "velocityY": 1.4075673366199248, + "timestamp": 9.129636257871532 + }, + { + "x": 3.7303087168982803, + "y": 5.169878519937419, + "heading": 0.012882808496808885, + "angularVelocity": -0.03050194470456946, + "velocityX": -3.0650531402520937, + "velocityY": 1.2074340334849432, + "timestamp": 9.198443382076704 + }, + { + "x": 3.5370906486542313, + "y": 5.2422774011944036, + "heading": 0.010786769401609648, + "angularVelocity": -0.030462530143669606, + "velocityX": -2.808111376198552, + "velocityY": 1.0522003657804835, + "timestamp": 9.267250506281876 + }, + { + "x": 3.362377869511596, + "y": 5.305500032913324, + "heading": 0.008787829989227822, + "angularVelocity": -0.029051343672224058, + "velocityX": -2.5391669999413216, + "velocityY": 0.9188384553087892, + "timestamp": 9.336057630487048 + }, + { + "x": 3.2065764087500264, + "y": 5.360415401059238, + "heading": 0.0069378788339289135, + "angularVelocity": -0.026886040895746154, + "velocityX": -2.2643216463602664, + "velocityY": 0.7981058470364881, + "timestamp": 9.40486475469222 + }, + { + "x": 3.0699232813800306, + "y": 5.407585492361195, + "heading": 0.005269555505228705, + "angularVelocity": -0.024246374891727204, + "velocityX": -1.9860316638509532, + "velocityY": 0.6855408047763067, + "timestamp": 9.473671878897392 + }, + { + "x": 2.9525724844135883, + "y": 5.4474025208162065, + "heading": 0.00380523783536115, + "angularVelocity": -0.021281483375198414, + "velocityX": -1.70550358443293, + "velocityY": 0.5786759571041632, + "timestamp": 9.542479003102564 + }, + { + "x": 2.854631606493464, + "y": 5.480155404840544, + "heading": 0.0025612082007498414, + "angularVelocity": -0.018079953914391628, + "velocityX": -1.423411878515389, + "velocityY": 0.47601007021705255, + "timestamp": 9.611286127307736 + }, + { + "x": 2.776179840063629, + "y": 5.506065679558788, + "heading": 0.0015498402303536507, + "angularVelocity": -0.014698593816833106, + "velocityX": -1.1401692388117297, + "velocityY": 0.376563837212327, + "timestamp": 9.680093251512908 + }, + { + "x": 2.717277807675311, + "y": 5.525308541082813, + "heading": 0.000780854270914727, + "angularVelocity": -0.01117596423803359, + "velocityX": -0.8560455486074661, + "velocityY": 0.2796637956651817, + "timestamp": 9.74890037571808 + }, + { + "x": 2.677973353536199, + "y": 5.538025975345556, + "heading": 0.00026208876345230333, + "angularVelocity": -0.007539415626724292, + "velocityX": -0.5712265204096013, + "velocityY": 0.1848272894652728, + "timestamp": 9.817707499923252 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -1.9416138419813845e-19, - "angularVelocity": -0.0012673235786260742, - "velocityX": -0.27713515806951416, - "velocityY": 0.06674917354380465, - "timestamp": 10.261192609069871 + "heading": -9.768710288964573e-20, + "angularVelocity": -0.003809035277666901, + "velocityX": -0.2858451884385741, + "velocityY": 0.09169675411867828, + "timestamp": 9.886514624128424 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -9.371198129604789e-20, - "angularVelocity": 9.233837824624604e-20, - "velocityX": 1.3188409475375661e-20, - "velocityY": 7.071902639587051e-21, - "timestamp": 10.334207558660724 - }, - { - "x": 2.6757817592371826, - "y": 5.540525112473874, - "heading": 2.9656831471890883e-18, - "angularVelocity": 4.5198899479057065e-17, - "velocityX": 0.25819570553696475, - "velocityY": -0.05629192276270124, - "timestamp": 10.401894939630061 - }, - { - "x": 2.7106551649219846, - "y": 5.532555521074409, - "heading": 6.8667018003075775e-18, - "angularVelocity": 5.763287923477954e-17, - "velocityX": 0.5152128090256504, - "velocityY": -0.11774116955531974, - "timestamp": 10.469582320599399 - }, - { - "x": 2.762825274578906, - "y": 5.520028158351496, - "heading": 1.4433249035955584e-17, - "angularVelocity": 1.1178667467062074e-16, - "velocityX": 0.7707508978749655, - "velocityY": -0.18507678305041053, - "timestamp": 10.537269701568736 - }, - { - "x": 2.832164805136809, - "y": 5.502484798756892, - "heading": 1.639060638651272e-17, - "angularVelocity": 2.8917610972365257e-17, - "velocityX": 1.0244085317662608, - "velocityY": -0.259182130308041, - "timestamp": 10.604957082538073 - }, - { - "x": 2.9185095362845113, - "y": 5.47939415323771, - "heading": 1.573002181515029e-17, - "angularVelocity": -9.759346009756976e-18, - "velocityX": 1.2756400072092744, - "velocityY": -0.34113663711767017, - "timestamp": 10.67264446350741 - }, - { - "x": 3.021644093219496, - "y": 5.450134961207605, - "heading": 1.4178342885926476e-17, - "angularVelocity": -2.2924198084943018e-17, - "velocityX": 1.52368957785063, - "velocityY": -0.4322695251476669, - "timestamp": 10.740331844476747 - }, - { - "x": 3.141280830981249, - "y": 5.413974520765915, - "heading": 1.5889264278443728e-17, - "angularVelocity": 2.5276814794004328e-17, - "velocityX": 1.7674895386475231, - "velocityY": -0.5342272063690965, - "timestamp": 10.808019225446085 - }, - { - "x": 3.27702786794977, - "y": 5.370041857723201, - "heading": 1.8375553239227874e-17, - "angularVelocity": 3.6731942133725327e-17, - "velocityX": 2.0054999177766217, - "velocityY": -0.6490524882712743, - "timestamp": 10.875706606415422 - }, - { - "x": 3.4283399225131155, - "y": 5.317295603371642, - "heading": 2.6053093742707064e-17, - "angularVelocity": 1.1342646729407338e-16, - "velocityX": 2.235454414049344, - "velocityY": -0.7792627458204298, - "timestamp": 10.94339398738476 - }, - { - "x": 3.594442154384911, - "y": 5.254489538104451, - "heading": 3.5054878226049046e-17, - "angularVelocity": 1.3299058635211391e-16, - "velocityX": 2.453961572941345, - "velocityY": -0.9278844057453234, - "timestamp": 11.011081368354096 - }, - { - "x": 3.774213631656668, - "y": 5.18014653899489, - "heading": 3.926192172002768e-17, - "angularVelocity": 6.215402980533945e-17, - "velocityX": 2.6559083051712955, - "velocityY": -1.098328788097711, - "timestamp": 11.078768749323434 - }, - { - "x": 3.9660187105503817, - "y": 5.092569435745629, - "heading": 4.466170927343128e-17, - "angularVelocity": 7.977539500755919e-17, - "velocityX": 2.8336903592207485, - "velocityY": -1.2938468292772884, - "timestamp": 11.146456130292771 + "heading": -4.776521402733554e-20, + "angularVelocity": 3.1435024538818593e-20, + "velocityX": 5.229513890071112e-19, + "velocityY": 2.1429873638516558e-18, + "timestamp": 9.955321748333596 + }, + { + "x": 2.6751665057839835, + "y": 5.538944468629318, + "heading": -4.314568505907974e-18, + "angularVelocity": -6.69844600274702e-17, + "velocityX": 0.2647058041795454, + "velocityY": -0.08463157955674895, + "timestamp": 10.01902015375538 + }, + { + "x": 2.7088624712986666, + "y": 5.528079837461981, + "heading": -9.548636187746045e-18, + "angularVelocity": -8.216952445023882e-17, + "velocityX": 0.5289922925316966, + "velocityY": -0.1705636286402427, + "timestamp": 10.082718559177163 + }, + { + "x": 2.7593613029745305, + "y": 5.511644735540107, + "heading": -1.3305788264629273e-17, + "angularVelocity": -5.898345573823082e-17, + "velocityX": 0.7927801542516099, + "velocityY": -0.25801433824046893, + "timestamp": 10.146416964598947 + }, + { + "x": 2.8266246280660523, + "y": 5.489524723354373, + "heading": -1.7929345665528888e-17, + "angularVelocity": -7.258513569049044e-17, + "velocityX": 1.0559656030026554, + "velocityY": -0.34726163142175154, + "timestamp": 10.21011537002073 + }, + { + "x": 2.9106051995536752, + "y": 5.461582331644453, + "heading": -2.2921460737278194e-17, + "angularVelocity": -7.837111523596277e-17, + "velocityX": 1.31840932173323, + "velocityY": -0.43866705178720505, + "timestamp": 10.273813775442514 + }, + { + "x": 3.0112434875257597, + "y": 5.427649372766817, + "heading": -2.8632725416108676e-17, + "angularVelocity": -8.966103061640699e-17, + "velocityX": 1.5799184815647955, + "velocityY": -0.5327128466237959, + "timestamp": 10.337512180864298 + }, + { + "x": 3.1284623112048178, + "y": 5.387515427846554, + "heading": -3.496086628632705e-17, + "angularVelocity": -9.93453576777077e-17, + "velocityX": 1.8402159819054262, + "velocityY": -0.6300620031932188, + "timestamp": 10.401210586286082 + }, + { + "x": 3.2621579049769083, + "y": 5.340909820689256, + "heading": -4.1773207656590006e-17, + "angularVelocity": -1.0694681169901965e-16, + "velocityX": 2.0988844679362666, + "velocityY": -0.7316604999559271, + "timestamp": 10.464908991707865 + }, + { + "x": 3.4121839694430376, + "y": 5.287471786804592, + "heading": -4.8890897323491556e-17, + "angularVelocity": -1.1174046853574415e-16, + "velocityX": 2.3552562026116513, + "velocityY": -0.8389226312781303, + "timestamp": 10.52860739712965 + }, + { + "x": 3.5783204640757695, + "y": 5.226697519809261, + "heading": -5.788296277144039e-17, + "angularVelocity": -1.4116625664827146e-16, + "velocityX": 2.608173525422577, + "velocityY": -0.954094008992977, + "timestamp": 10.592305802551433 + }, + { + "x": 3.76020443223388, + "y": 5.157837260218167, + "heading": -6.653949543666233e-17, + "angularVelocity": -1.3589873416470021e-16, + "velocityX": 2.85539279914075, + "velocityY": -1.0810358459545435, + "timestamp": 10.656004207973217 + }, + { + "x": 3.9571467965572817, + "y": 5.079670509622146, + "heading": -7.420373602789392e-17, + "angularVelocity": -1.2032076062800333e-16, + "velocityX": 3.0917942610859868, + "velocityY": -1.2271382631705205, + "timestamp": 10.719702613395 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 4.913032791004192e-17, - "angularVelocity": 6.601848931159295e-17, - "velocityX": 2.976630975573702, - "velocityY": -1.516111842533804, - "timestamp": 11.214143511262108 - }, - { - "x": 4.261627503076193, - "y": 4.9391622522038245, - "heading": 5.1271717716207476e-17, - "angularVelocity": 6.871367197331748e-17, - "velocityX": 3.0204265353966933, - "velocityY": -1.6296244523245107, - "timestamp": 11.245307466608786 - }, - { - "x": 4.357244155674493, - "y": 4.884889313220403, - "heading": 5.244866899623222e-17, - "angularVelocity": 3.776642810800902e-17, - "velocityX": 3.0681809011286347, - "velocityY": -1.7415292243770975, - "timestamp": 11.276471421955463 - }, - { - "x": 4.454522517564495, - "y": 4.827208233471932, - "heading": 5.393508555206354e-17, - "angularVelocity": 4.76966591102462e-17, - "velocityX": 3.121502415461902, - "velocityY": -1.8508908483152524, - "timestamp": 11.307635377302141 - }, - { - "x": 4.553721568715114, - "y": 4.766257957931851, - "heading": 5.435767688071947e-17, - "angularVelocity": 1.3560259702022703e-17, - "velocityX": 3.183134170457424, - "velocityY": -1.9557939568983076, - "timestamp": 11.338799332648819 - }, - { - "x": 4.6552566098247095, - "y": 4.702321075934724, - "heading": 5.3497639174451405e-17, - "angularVelocity": -2.7597193512319988e-17, - "velocityX": 3.2580922408625277, - "velocityY": -2.0516292391602082, - "timestamp": 11.369963287995496 - }, - { - "x": 4.7598200255019725, - "y": 4.636102620452091, - "heading": 5.221139321258901e-17, - "angularVelocity": -4.1273514455672286e-17, - "velocityX": 3.355267793002091, - "velocityY": -2.1248411745556077, - "timestamp": 11.401127243342174 - }, - { - "x": 4.868161384056621, - "y": 4.569562096793982, - "heading": 4.9900416681055594e-17, - "angularVelocity": -7.415543069862901e-17, - "velocityX": 3.476495757660606, - "velocityY": -2.135175811859932, - "timestamp": 11.432291198688851 - }, - { - "x": 4.979472809142361, - "y": 4.50537854159068, - "heading": 4.580479975569673e-17, - "angularVelocity": -1.3142160161653158e-16, - "velocityX": 3.5718003009398434, - "velocityY": -2.059544576075285, - "timestamp": 11.463455154035529 - }, - { - "x": 5.092478993411361, - "y": 4.444586805435104, - "heading": 4.211690012859059e-17, - "angularVelocity": -1.1833862502052825e-16, - "velocityX": 3.6261823318601563, - "velocityY": -1.950706689164141, - "timestamp": 11.494619109382207 - }, - { - "x": 5.206458430184718, - "y": 4.387459691402226, - "heading": 3.8483549973003e-17, - "angularVelocity": -1.1658822232196594e-16, - "velocityX": 3.657412401776814, - "velocityY": -1.8331150008841193, - "timestamp": 11.525783064728884 - }, - { - "x": 5.321007172735708, - "y": 4.334081255049934, - "heading": 3.4624290514440134e-17, - "angularVelocity": -1.2383727976295539e-16, - "velocityX": 3.675680486533673, - "velocityY": -1.7128261081911138, - "timestamp": 11.556947020075562 - }, - { - "x": 5.435875810085393, - "y": 4.2844809626648175, - "heading": 3.000921461469742e-17, - "angularVelocity": -1.4809018452502327e-16, - "velocityX": 3.685945383756665, - "velocityY": -1.5915916908925372, - "timestamp": 11.58811097542224 - }, - { - "x": 5.550897094649703, - "y": 4.2386692598409, - "heading": 2.554076914069394e-17, - "angularVelocity": -1.4338505567167463e-16, - "velocityX": 3.690843581463868, - "velocityY": -1.4700220916855413, - "timestamp": 11.619274930768917 + "heading": -8.561193314262968e-17, + "angularVelocity": -1.7909705963758273e-16, + "velocityX": 3.3023160854502294, + "velocityY": -1.4085550989874924, + "timestamp": 10.783401018816784 + }, + { + "x": 4.2842075611264425, + "y": 4.936417577245955, + "heading": -9.453694204366925e-17, + "angularVelocity": -2.5755488045645486e-16, + "velocityX": 3.3679341946759767, + "velocityY": -1.5447568972758807, + "timestamp": 10.81805386206763 + }, + { + "x": 4.401356584826297, + "y": 4.8776669667131465, + "heading": -1.0560165699964911e-16, + "angularVelocity": -3.1930179223080245e-16, + "velocityX": 3.380646801528862, + "velocityY": -1.6954051968412056, + "timestamp": 10.852706705318477 + }, + { + "x": 4.516063976001339, + "y": 4.814281195629902, + "heading": -9.662380875053523e-17, + "angularVelocity": 2.590797004510181e-16, + "velocityX": 3.3101869980680285, + "velocityY": -1.8291650882556678, + "timestamp": 10.887359548569323 + }, + { + "x": 4.628146410125643, + "y": 4.746361612927903, + "heading": -9.053238794887956e-17, + "angularVelocity": 1.7578415593930746e-16, + "velocityX": 3.234436877602137, + "velocityY": -1.9600002865663488, + "timestamp": 10.92201239182017 + }, + { + "x": 4.737424855337869, + "y": 4.674016859733605, + "heading": -7.951670404623386e-17, + "angularVelocity": 3.1788687072504797e-16, + "velocityX": 3.1535203163901047, + "velocityY": -2.087700356089278, + "timestamp": 10.956665235071016 + }, + { + "x": 4.844483096398324, + "y": 4.598425359059355, + "heading": -7.540763427184281e-17, + "angularVelocity": 1.1857814219565296e-16, + "velocityX": 3.089450417833756, + "velocityY": -2.181393893916722, + "timestamp": 10.991318078321862 + }, + { + "x": 4.954476829355994, + "y": 4.5271728465414585, + "heading": -1.0264313607639127e-16, + "angularVelocity": -7.859528757078724e-16, + "velocityX": 3.174161847599147, + "velocityY": -2.0561808450207573, + "timestamp": 11.025970921572709 + }, + { + "x": 5.06723038345343, + "y": 4.460373312794279, + "heading": -9.620613179989556e-17, + "angularVelocity": 1.8575688667134074e-16, + "velocityX": 3.25380383021476, + "velocityY": -1.9276782936288397, + "timestamp": 11.060623764823555 + }, + { + "x": 5.18256360847748, + "y": 4.398133545147613, + "heading": -8.10849749390595e-17, + "angularVelocity": 4.36361217215796e-16, + "velocityX": 3.3282470990668904, + "velocityY": -1.796094109684511, + "timestamp": 11.095276608074402 + }, + { + "x": 5.300292214676822, + "y": 4.340553023175948, + "heading": -6.741688231750934e-17, + "angularVelocity": 3.9442918212224324e-16, + "velocityX": 3.397372196766743, + "velocityY": -1.6616391779124173, + "timestamp": 11.129929451325248 + }, + { + "x": 5.4202280776683445, + "y": 4.287723772710641, + "heading": -7.654370000647077e-17, + "angularVelocity": -2.6337861002637873e-16, + "velocityX": 3.461068464810413, + "velocityY": -1.5245285959043562, + "timestamp": 11.164582294576094 + }, + { + "x": 5.542179542504675, + "y": 4.239730223009142, + "heading": -8.402477364947206e-17, + "angularVelocity": -2.1588628640817297e-16, + "velocityX": 3.5192340193716007, + "velocityY": -1.3849815830141687, + "timestamp": 11.19923513782694 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 2.1323021995570948e-17, - "angularVelocity": -1.353405592009617e-16, - "velocityX": 3.6919137154187807, - "velocityY": -1.3483585385427075, - "timestamp": 11.650438886115595 - }, - { - "x": 5.935813024971261, - "y": 4.118882926647287, - "heading": 1.853226702760062e-17, - "angularVelocity": -3.8136239390164817e-17, - "velocityX": 3.6877099925560546, - "velocityY": -1.0626903702408637, - "timestamp": 11.723617443186296 - }, - { - "x": 6.200932724790644, - "y": 4.061479044678689, - "heading": 1.7565627376800837e-17, - "angularVelocity": -1.3209329241757196e-17, - "velocityX": 3.622915105626314, - "velocityY": -0.7844358274670208, - "timestamp": 11.796796000256997 - }, - { - "x": 6.456598704926571, - "y": 4.0227227304075965, - "heading": 1.267506754253446e-17, - "angularVelocity": -6.683050380301111e-17, - "velocityX": 3.493728086069183, - "velocityY": -0.5296129880457807, - "timestamp": 11.869974557327698 - }, - { - "x": 6.698616689948628, - "y": 3.99980424542816, - "heading": 8.945633316051437e-18, - "angularVelocity": -5.0963484094285186e-17, - "velocityX": 3.307225432010503, - "velocityY": -0.31318580055211104, - "timestamp": 11.943153114398399 - }, - { - "x": 6.923883976637485, - "y": 3.9893965055012433, - "heading": 2.783843782577358e-18, - "angularVelocity": -8.420211852775213e-17, - "velocityX": 3.078323701726083, - "velocityY": -0.1422239019670945, - "timestamp": 12.0163316714691 - }, - { - "x": 7.130423561054918, - "y": 3.988282733586895, - "heading": 2.8545928007524544e-18, - "angularVelocity": 9.667998274288902e-19, - "velocityX": 2.822405806907136, - "velocityY": -0.015219921776707517, - "timestamp": 12.089510228539801 - }, - { - "x": 7.317098326514511, - "y": 3.993687679254755, - "heading": 5.295405456553911e-18, - "angularVelocity": 3.335420582506506e-17, - "velocityX": 2.5509489792103515, - "velocityY": 0.07385969174874675, - "timestamp": 12.162688785610502 - }, - { - "x": 7.4833023364759494, - "y": 4.003341497002791, - "heading": 9.255539418426772e-18, - "angularVelocity": 5.4116043269433334e-17, - "velocityX": 2.2712119043405083, - "velocityY": 0.13192140067353622, - "timestamp": 12.235867342681203 - }, - { - "x": 7.628740531129743, - "y": 4.015421358409773, - "heading": 1.1054283152069948e-17, - "angularVelocity": 2.458020224219918e-17, - "velocityX": 1.9874427766221006, - "velocityY": 0.16507378514324764, - "timestamp": 12.309045899751904 - }, - { - "x": 7.753293848479071, - "y": 4.028463870964022, - "heading": 1.0645910526563799e-17, - "angularVelocity": -5.5804957540262804e-18, - "velocityX": 1.7020466422833305, - "velocityY": 0.1782286106249456, - "timestamp": 12.382224456822605 - }, - { - "x": 7.856941288758158, - "y": 4.041284750258868, - "heading": 1.2932065845592816e-17, - "angularVelocity": 3.1240781584055425e-17, - "velocityX": 1.4163635418357237, - "velocityY": 0.17519994665183228, - "timestamp": 12.455403013893307 - }, - { - "x": 7.939715627162556, - "y": 4.0529146415487, - "heading": 1.2915183661697564e-17, - "angularVelocity": -2.306985186705733e-19, - "velocityX": 1.1311283211614112, - "velocityY": 0.15892485114999141, - "timestamp": 12.528581570964008 - }, - { - "x": 8.001678238256412, - "y": 4.062550327487904, - "heading": 8.098550414220998e-18, - "angularVelocity": -6.582028180467396e-17, - "velocityX": 0.8467317965013967, - "velocityY": 0.13167362578486927, - "timestamp": 12.601760128034709 - }, - { - "x": 8.042904705550558, - "y": 4.0695182051166965, - "heading": 2.8097102514123304e-18, - "angularVelocity": -7.227308620193683e-17, - "velocityX": 0.5633681360281035, - "velocityY": 0.09521747773817571, - "timestamp": 12.67493868510541 + "heading": -9.639099903602133e-17, + "angularVelocity": -3.5686033890323503e-16, + "velocityX": 3.5717757824418066, + "velocityY": -1.2432211735943417, + "timestamp": 11.233887981077787 + }, + { + "x": 5.93683892832179, + "y": 4.127579524293423, + "heading": -7.782590018599437e-17, + "angularVelocity": 2.51158487397316e-16, + "velocityX": 3.664705468644969, + "velocityY": -0.9344094480089544, + "timestamp": 11.307805844823916 + }, + { + "x": 6.212625572925941, + "y": 4.081838891930783, + "heading": -5.4414649647687885e-17, + "angularVelocity": 3.167197934552236e-16, + "velocityX": 3.7309877562390543, + "velocityY": -0.6188034941017393, + "timestamp": 11.381723708570044 + }, + { + "x": 6.491306353239059, + "y": 4.059759739930169, + "heading": -8.220332638334233e-17, + "angularVelocity": -3.7593993288383394e-16, + "velocityX": 3.7701411565443874, + "velocityY": -0.2986984590956788, + "timestamp": 11.455641572316173 + }, + { + "x": 6.752875972710037, + "y": 4.054277278014819, + "heading": -7.798662141044023e-17, + "angularVelocity": 5.704581760375439e-17, + "velocityX": 3.5386523123739373, + "velocityY": -0.07416964773468374, + "timestamp": 11.529559436062302 + }, + { + "x": 6.9910920962877245, + "y": 4.053575874325361, + "heading": -6.860310501240552e-17, + "angularVelocity": 1.269451783709716e-16, + "velocityX": 3.2227138543376115, + "velocityY": -0.00948896050171146, + "timestamp": 11.60347729980843 + }, + { + "x": 7.205567187241113, + "y": 4.055021140911149, + "heading": -5.885294926916e-17, + "angularVelocity": 1.3190526956719489e-16, + "velocityX": 2.901532594204896, + "velocityY": 0.019552331635990624, + "timestamp": 11.677395163554559 + }, + { + "x": 7.396227777452995, + "y": 4.057522153925315, + "heading": -4.8982438983156224e-17, + "angularVelocity": 1.3353348954892513e-16, + "velocityX": 2.5793574184815227, + "velocityY": 0.033835028333003926, + "timestamp": 11.751313027300688 + }, + { + "x": 7.563054971618611, + "y": 4.060486200727262, + "heading": -3.912779424726568e-17, + "angularVelocity": 1.333188519862415e-16, + "velocityX": 2.256926617070343, + "velocityY": 0.04009919458884174, + "timestamp": 11.825230891046816 + }, + { + "x": 7.70604444565388, + "y": 4.063541585038157, + "heading": -2.9177079820924904e-17, + "angularVelocity": 1.3461853362720707e-16, + "velocityX": 1.9344373171601388, + "velocityY": 0.041334856772760636, + "timestamp": 11.899148754792945 + }, + { + "x": 7.825196583277639, + "y": 4.066433602906459, + "heading": -1.9253434148047247e-17, + "angularVelocity": 1.3425233319755054e-16, + "velocityX": 1.6119532084015245, + "velocityY": 0.03912474903541938, + "timestamp": 11.973066618539074 + }, + { + "x": 7.920513375065405, + "y": 4.0689768762979055, + "heading": -9.302606124041008e-18, + "angularVelocity": 1.346200704365749e-16, + "velocityX": 1.2894960292025357, + "velocityY": 0.034406749093584967, + "timestamp": 12.046984482285202 + }, + { + "x": 7.991997299424133, + "y": 4.071030463180752, + "heading": 8.179153045928767e-19, + "angularVelocity": 1.369157726664237e-16, + "velocityX": 0.9670723792051265, + "velocityY": 0.027782010717991884, + "timestamp": 12.12090234603133 + }, + { + "x": 8.03965088919926, + "y": 4.072483600942051, + "heading": 2.229877553000587e-19, + "angularVelocity": -8.048494898574051e-18, + "velocityX": 0.6446829948819026, + "velocityY": 0.019658817066051014, + "timestamp": 12.19482020977746 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 3.1031533755807955e-28, - "angularVelocity": -3.8395267193036997e-17, - "velocityX": 0.2811186469496163, - "velocityY": 0.05095414427593911, - "timestamp": 12.74811724217611 + "heading": 3.803306693773524e-29, + "angularVelocity": -3.0166964241453683e-18, + "velocityX": 0.32232632402054306, + "velocityY": 0.01032706968038683, + "timestamp": 12.268738073523588 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 1.493594231911397e-28, - "angularVelocity": -1.5846843190307173e-28, - "velocityX": -6.450761173739877e-21, - "velocityY": 2.2636697046974056e-20, - "timestamp": 12.821295799246812 + "heading": 1.8619056951323718e-29, + "angularVelocity": -1.0754545583434217e-29, + "velocityX": -2.9997098934675086e-25, + "velocityY": 4.422144616548637e-24, + "timestamp": 12.342655937269717 } ] } \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java index 85773305..9a1bc077 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java @@ -27,14 +27,10 @@ public CenterLanePCBAFSprint(Drivetrain drivetrain, Shooter shooter, Indexer ind Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), - drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, shooter, indexer, 1), - drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, shooter, indexer, 2), - drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, shooter, indexer, 3), - drivetrain.commandCopyVisionPose() + createSegmentSequence(drivetrain, shooter, indexer, 0, false, false, false), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, false, true), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, false, true), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, false, true) ) ), getPath(4).getCommand(drivetrain) diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java index bf3b2661..9560fe16 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java @@ -54,8 +54,7 @@ public void execute() { Rotation2d targetRot = drivetrain.getPose() .minus(target.get()).getTranslation().getAngle() - .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0)) - .plus(Rotation2d.fromDegrees(5)); + .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0)); // Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); drivetrain.setTargetPose(new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); Logger.recordOutput("Targeting/rotErrorDegrees", Math.abs(targetRot.minus(drivetrain.getRotation()).getDegrees())); diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java index 3bef39de..dbb592bf 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java @@ -2,6 +2,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import org.team1540.robot2024.Constants; @@ -45,8 +47,16 @@ public ShootSequence(Shooter shooter, Indexer indexer, Supplier ); } - public static ShootSequence forAutoSubwoofer(Shooter shooter, Indexer indexer) { - return new ShootSequence(shooter, indexer, () -> HUB_SHOOT, 1.5); + public static Command forAutoSubwoofer(Shooter shooter, Indexer indexer) { + return Commands.race( + new PrepareShooterCommand(shooter, ()-> shooter.lerp.get(Units.feetToMeters(3))), + Commands.sequence( + Commands.waitSeconds(1.5), + Commands.runOnce(shooter::zeroPivotToCancoder), + IntakeAndFeed.withDefaults(indexer).withTimeout(0.5) + ) + + ); } diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index fe861722..1503c9f0 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -84,7 +84,7 @@ public void setPathIndex(int index){ * @param pathIndex * @return the commands to run in the segment */ - protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw) { + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry) { return Commands.sequence( Commands.deadline( getPath(pathIndex).getCommand(drivetrain), @@ -95,7 +95,7 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Commands.waitSeconds(0.25), new InstantCommand(shooter::zeroPivotToCancoder) ).onlyIf(()->shouldZeroCancoder), - drivetrain.commandCopyVisionPose(), + drivetrain.commandCopyVisionPose().onlyIf(()->shouldResetOdometry), Commands.parallel( new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4).onlyIf(()->shouldRealignYaw), Commands.sequence( @@ -112,11 +112,11 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, ); } protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex){ - return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, false, false); + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, false, true, true); } protected Command createCancoderSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex) { - return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, true, false); + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, true, true, true); } public List toTrajectory() { From cf072fa7720c8937a59de3d2920c4beec70cd1b6 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Fri, 22 Mar 2024 02:24:01 -0700 Subject: [PATCH 54/75] feat: final auto updates and new camera positions --- logs/sim_2024-03-19_03-53-33.hoot | Bin 0 -> 85063 bytes logs/sim_2024-03-19_03-56-20.hoot | Bin 0 -> 81920 bytes paths.chor | 50084 +++++++--------- src/main/deploy/choreo/AmpLanePSprint.traj | 436 - .../choreo/AmpLanePSubASubDSubESub.1.traj | 672 +- .../choreo/AmpLanePSubASubDSubESub.2.traj | 1260 +- .../choreo/AmpLanePSubASubDSubESub.3.traj | 1648 +- .../choreo/AmpLanePSubASubDSubESub.traj | 3576 +- src/main/deploy/choreo/AmpLaneSprint.1.traj | 299 +- src/main/deploy/choreo/AmpLaneSprint.traj | 299 +- src/main/deploy/choreo/AmpLaneTaxi.1.traj | 187 +- src/main/deploy/choreo/AmpLaneTaxi.traj | 187 +- src/main/deploy/choreo/CenterLanePBDA.1.traj | 160 +- src/main/deploy/choreo/CenterLanePBDA.2.traj | 995 +- src/main/deploy/choreo/CenterLanePBDA.3.traj | 403 +- src/main/deploy/choreo/CenterLanePBDA.traj | 886 + src/main/deploy/choreo/CenterLanePBc.traj | 940 - src/main/deploy/choreo/CenterLanePCBA.1.traj | 600 +- src/main/deploy/choreo/CenterLanePCBA.2.traj | 137 +- src/main/deploy/choreo/CenterLanePCBA.3.traj | 327 +- src/main/deploy/choreo/CenterLanePCBA.traj | 1078 +- src/main/deploy/choreo/CenterLanePCBADEF.traj | 1912 - .../deploy/choreo/CenterLanePCBAFE.1.traj | 292 + .../deploy/choreo/CenterLanePCBAFE.2.traj | 94 + .../deploy/choreo/CenterLanePCBAFE.3.traj | 184 + .../deploy/choreo/CenterLanePCBAFE.4.traj | 688 + .../deploy/choreo/CenterLanePCBAFE.5.traj | 607 + src/main/deploy/choreo/CenterLanePCBAFE.traj | 1813 + .../choreo/CenterLanePCBAFSprint.1.traj | 572 +- .../choreo/CenterLanePCBAFSprint.2.traj | 78 +- .../choreo/CenterLanePCBAFSprint.3.traj | 360 +- .../choreo/CenterLanePCBAFSprint.4.traj | 1620 +- .../choreo/CenterLanePCBAFSprint.5.traj | 726 +- .../deploy/choreo/CenterLanePCBAFSprint.traj | 3358 +- .../deploy/choreo/CenterLanePCBFSprint.1.traj | 590 +- .../deploy/choreo/CenterLanePCBFSprint.2.traj | 133 +- .../deploy/choreo/CenterLanePCBFSprint.3.traj | 833 +- .../deploy/choreo/CenterLanePCBFSprint.4.traj | 726 +- .../deploy/choreo/CenterLanePCBFSprint.5.traj | 728 +- .../deploy/choreo/CenterLanePCBFSprint.traj | 3022 +- .../CenterLanePSubCSubBSubASubFSub.1.traj | 602 +- .../CenterLanePSubCSubBSubASubFSub.2.traj | 412 +- .../CenterLanePSubCSubBSubASubFSub.3.traj | 642 +- .../CenterLanePSubCSubBSubASubFSub.4.traj | 1940 +- .../CenterLanePSubCSubBSubASubFSub.traj | 3586 +- .../CenterLanePSubCSubBSubASubSprint.1.traj | 606 +- .../CenterLanePSubCSubBSubASubSprint.2.traj | 394 +- .../CenterLanePSubCSubBSubASubSprint.3.traj | 608 +- .../CenterLanePSubCSubBSubASubSprint.4.traj | 918 +- .../CenterLanePSubCSubBSubASubSprint.traj | 2514 +- .../deploy/choreo/CenterLaneSprint.1.traj | 791 +- src/main/deploy/choreo/CenterLaneSprint.traj | 791 +- .../deploy/choreo/CenterLaneSprintBonus.traj | 877 - src/main/deploy/choreo/CenterLaneTaxi.1.traj | 166 +- src/main/deploy/choreo/CenterLaneTaxi.traj | 166 +- src/main/deploy/choreo/SQUARFE (1).traj | 805 - src/main/deploy/choreo/SQUARFE.traj | 805 - .../deploy/choreo/SourceLanePHGFSprint.traj | 1336 - .../choreo/SourceLanePSubHSubGSub.1.traj | 1773 +- .../choreo/SourceLanePSubHSubGSub.2.traj | 1827 +- .../deploy/choreo/SourceLanePSubHSubGSub.traj | 3598 +- .../choreo/SourceLanePSubHSubSprint.1.traj | 1773 +- .../choreo/SourceLanePSubHSubSprint.2.traj | 851 +- .../choreo/SourceLanePSubHSubSprint.traj | 2622 +- .../deploy/choreo/SourceLaneSprint.1.traj | 299 +- src/main/deploy/choreo/SourceLaneSprint.traj | 299 +- src/main/deploy/choreo/SourceLaneTaxi.1.traj | 195 +- src/main/deploy/choreo/SourceLaneTaxi.traj | 195 +- src/main/deploy/choreo/StraightForward.traj | 3181 - .../org/team1540/robot2024/Constants.java | 4 +- .../team1540/robot2024/RobotContainer.java | 5 +- .../commands/autos/AmpLanePADESprint.java | 3 +- .../commands/autos/CenterLanePCBAFE.java | 42 + .../commands/autos/CenterLanePCBAFSprint.java | 3 +- .../commands/autos/PathVisualising.java | 21 + .../commands/autos/SourceLanePHG.java | 2 + .../commands/shooter/ShootSequence.java | 2 +- .../robot2024/util/auto/AutoCommand.java | 2 +- 78 files changed, 50379 insertions(+), 66787 deletions(-) create mode 100644 logs/sim_2024-03-19_03-53-33.hoot create mode 100644 logs/sim_2024-03-19_03-56-20.hoot delete mode 100644 src/main/deploy/choreo/AmpLanePSprint.traj create mode 100644 src/main/deploy/choreo/CenterLanePBDA.traj delete mode 100644 src/main/deploy/choreo/CenterLanePBc.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBADEF.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFE.1.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFE.2.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFE.3.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFE.4.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFE.5.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFE.traj delete mode 100644 src/main/deploy/choreo/CenterLaneSprintBonus.traj delete mode 100644 src/main/deploy/choreo/SQUARFE (1).traj delete mode 100644 src/main/deploy/choreo/SQUARFE.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGFSprint.traj delete mode 100644 src/main/deploy/choreo/StraightForward.traj create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFE.java create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/PathVisualising.java diff --git a/logs/sim_2024-03-19_03-53-33.hoot b/logs/sim_2024-03-19_03-53-33.hoot new file mode 100644 index 0000000000000000000000000000000000000000..759596b293f384866b3ce04eb3a560120a2abb0b GIT binary patch literal 85063 zcmbq+d0dUz8~AP0rhQd6?OK%*ZZt~!W(#4INZChjDb+9)rA=42v@ng*f-I3SBZ-ps z42qfXHMDDCR5F91`+J`A-uqtj`~3dFzUMsW+0JvG^PK0LyK2XGk-InV+Yz>h z@&EBmV_e$vF%$um-kc1$y(6~$=!L+a+JoHRO!0WU%m2j1tW+|LdNipX7<#P&U%|8d zul6mmKAE=Pu6;F;w-Vn#J)IR$ufaLG{gmFf3}ub-QqMo_ZY8O!UAn+)XJ_BD~ zmuOY+%%hfONx?g1dM=6GRF8Mmdgx)9c-=_9Ok(fSndeKzG7!r&&n{7^e%BFf)iw4z zw(^}-Zj;kIsUEcwXtXx`^)CWgV{GZ84IxGkt3SmxRFgNl7H`fdzNj}!pkY_A&k)!X z3<5j0>KBHnJ@->e(wP6T^lFg+8-?7Q$N49nqkYcot`_-FB61L9q~cW__$BTQ0r{gf z@HWBn&x)zqzsZZxF3visutv=??Yem}=agmb!l6siV~6)J5(+#I-g1t%u)jk9yXPKv zw#flj2KJoSMp}{WpKc6I6VM7wv(THY77-6Ocn_2;xG3zk@6$y73mLr|d)D%TC>~ec z-l$(Q&meIRW9_}etw!_COi1<24qq54Hs&GrDK=>=S6VYttZJ6RnwWU^Wi$0_29EeF zAmV^#jmk^@WK`r!wtMgNg^oJCy55ZNc2OKVme)M#ui6=T^AqnGdDGlCpj5vh*t1Kn zPFHSdRIPqPvFaak-6tJZr%Gtq$0Uxav!wTl2?P|LGdj)GMAlNG+-4r|w7!Yp1MF2_ zrI8`&nWX!JgOhALFw7>rBRc-bq4tuyi-G~87PmeTt>o&YJh4i}7s36-)5MoWrnH5`1qt zlwRUs84cfOFU0!Y-KNJB{fdqNavf}-qp9-ZSjj6VK~0ozH?}D4 z?tPI;O~A*3;FIqapRt7jWam6YvZoI0WgZ1k#`^%&G_htgaa48bK)abm?1Cf{NkPSD zNz$!KK%+CV=TgC@P{Izy8_;9KSbSOu$Jqedr365`R0*5rg^I@js6O&}+T3R2Lrg)_ zjJ{~EdkKPXQJYQ=B9~8N@AK{Egd-|y`=O8<)@_fvO$yU2!kEeG0AgYUdLLM^mnk}g z{Fp?31Z)gv+0&)R31%}qfLPgE!2D|oDi3Ucf}|Hf>bIUBZ#oDHCly7dXDt!Gs)*DM zA+=_EQ%!p^{gRN3bw~#LoP|X`YD9f6boxe9CSr!5@ZGOU>~d6xE2s_^6dy51km2gc zaK=-Xm7JjBQj_M!R9C2|SPd2Xsq zA2_p}Gf|`u=tv=cJ!YIO>&IzW*-h+5JvXSVj3Ve`{gpVT2USxMa~-AP7$k!fmd-lD z+>Fgg#k&vga7r9%kZSU^j4iI&fncH$Oo_qL8mqL@b%>)i;wa_grDg8EiiPamh-i)- z&fb4(6Nj$Qez#i_i0vIftynjmgMe!g2PLBfZeIq97IoBpVe`0?G*oo%gNhk_qQxz! z10SQ{+eF`81tPHvD6v%66I)Pxr0~S>0b5Y8d&(VPt;CMMMa){lnQN7id)E-HqfCub zq64JF(ROCipVZYhbLc{Fqee3p#IeyFx6D3G!e2yKzm2^<(;2ysGVNncd9snAQiP%Q z`YT-S+ud5Dl{yZ#m$LYIfj41xiT$$)tQBSPgQC#lo4=_Z2fIFl2Zr#EoOMlN$Fn7J z4?WIrGfccbQMAi}5ocF)d975Kee;9p_|+eJn#Ha$&}L5md{lC~X_dpm8nkn^V{=gj zH$>axgg4{E426fHDXX+*a_Fjz-u$G(Enwq)=Klb8h7sG(ssOO9O#rq@VlgL+8G=op zV$;%y8=5uUe}K*J@7V^_tuPM%P@i@ltKWs{v(6Rm>v9{Y^SZ3~A}hHNjF{q*0H=6E zCn(z{Ksm$L^FzOs-YU9mS;cC*d&omjmXqg9Na;)LB(@*vV7ALR#>xY7?c#vkab54- z`ff<{i3aFyT5SpIHB&LY1MuodW1F;pF0x}(3)vy5aE>)%S}JIFa-CwtMX_8V4R%oA}B4@$3D5BU69t#+@$QPCtC?vhiU`0 zCq?s8l;zFVoD;l&S?cN`)QC&Yh&Si-oIIqT6t-$B@0 z#M)3uOO4uy#0Icl@G*yGXp+#k%8~oN z54<+pDR8FprRKf(J<{;w*Y1?k>86|2dyM!Vq{_#I#iO&eteeC}#hL_$5A$7xyPmY>Kzh*sh}?RcX-@=je5f zdYc@aD!J&EXU<(3W_Do4+Y#OUDGY32I6BPR!mM@stp-e_^W#ou+Mo*8DyFq_SHCA*JJo6N{< zzS}1G@%9A@IiAlrh~-ka@`1WqyH*ue95?F2=Js`4WY?Kf4ShAYI4+q!s+iiOV zD7+Y-q*n+kpFxZhhtGl>C?zOtS@7*l1q-?5fF|{n#Uit9Q_m`Nr^U>1E`3Nb;xYRj zS#p#Nj9<~3GcMrU%Q1J^p3O=3J~1jf26oyE>7p!>XO%Kp3 z4N+Xd?Y=Wa%RyrddFP$*oNjMqJ^gzHi>Se_a@esTP+i4{pw+xw5Aa2NN`wcG!$zw*zOERU*V zQ|@0JQ4PwCDDIsZzO?r$2$R38y1EP3KjJFT@-k7|xTn8pL)U65gTN8$*}w88YDmnM z|4Hs`bkN-Cx8;0E?~C5MIS)dAi}Yh4_d|QyJmTj6!W&JgxSq)#5Sv1dnYX6UbRRK6 z5q@+(z?jJW9#jJawRu0_ zEArGn3A^Sn)8Bc#8=6ti)9;a}m^+78P3HCTa+K>`>aV>n5ZzZ^-J*{Hhs~j}K1Ue^ z)eG}#kG7G47|Tme^R)>!c|4ixCBlk~w2GT?>-9~9Hu^=^+_o`A$LEnteGa3*{D6%) zJMhE)CGLK=;uX|R1UGP!6=y1M5#1}}WT0RY=tss*G5{B#H;Kx2!AFGU7B|cbGShc3 z%5teUW5i7dJ_0Wkof>FfM0`oHuW4jjFB1v4aw24RlZ%u>s%=(;O!u>Sic;I3k1GNp zXZj07?VO`IatZ5Bycp3qv}!FckGnB~5MC?0{M_(tlN9^N_i^#Tloi_FgUy2N8lYJS zM@8h^rX8`KlS%ojl6bt(xH{@{u{C~P+_Zl4%oz@eW0^y;HfI^%MX~#zB8pbd_h=M< z{s+b7tzdI@=xsfUYq}efT21=b{TH+`SMfh@nvsZIX)KBD_FEM8A@D1gQdY%JJ2vVt z(%GY*nCz_OsxOM zn+vmCRsDT4rx|~rYw`+QFaJQodYC2g*$ZTSK~ofPbc{bhg+#R9C!Tz5-r@E7HE!ZNmWh1JV0+)a%M}#0;#pgJUgcwv0bjKk--I&}tdbLe z<$Lh?;3$e`4yqP;y$ZL9zVq8f#TN!ic4rwGE;%hdvKHn)LzqFe6Ii}PqU@%Xx#Tj$ z!w>P`hZPV&DFWb1OBXUlLTnK;1HjBs)`~N6rF%1W=ms55lk|v5%oLrs6;@a)OYJ52 zF1(;<>UjJ~cnDPf=!}*1ma(7Miz4vb1o$%5-ApmMNE~>?BnUCt)XhrTgm5+!u-kfC z?;sgRfs8lxiTy2t0*NYYMSR?$i@|OYH4mL6?2?pcsW&i3og9#0MI<=i-b$$-`*6lS zhzC!XZLdSpH;q)O0Zv-(`!Vd660UM)$S9AJWgRvee zN)!8#86lZe+;hmDfI~@am^rN(Aq*n~@X}d8ug^lH_mI(?@EXLZ6EWi6XFIGwcwWfS zCbq~*A{A01p#$vL13OkpwIvX5BSdu55yk<8(aN!GPDL2wD9a<3&1_>Fg^d~@_LJPc zLw1tT`Gq;qIq}m0qCe5q^t3q$@lrj32(bf%n1m2Fr|C7@AVf7(q$B94rvXG`GLW0c zJ<@Q{lhQM;is(Gz6L@=Y!M;;Z-iS1S>N-~d(dCfOTS8-H7ir~_wfFk+;2!Y})R{0rP(b5CH){Sp(_f0+ulgmB7gq)bSnZwlwVv1Z{kSGseWWLogYy{kPW7Iofl@mP4x;UA`r4Vs*DRNU}6^u;5(<5Z@_vhn-u~|l0z3LpXcheKA1?vH9qor z12pf=eo$T?_g5URcI7ifDJ|y!jnd08;&M!H0Q>xoXP*4ZV3FckhSd{a+xcE*?L9ZT zWg5)7FSmjguNcabS#ji>^K8Ga)}Gl%+_NeBu2PAP{pNWmR||x*-_ak&o_SZPsH!%s zY+!j~xl|!_a(CAGF<$LBFEAP%$?B&@zYAOxIpP)kxYVzs@HsCJ7t^^Jt8r$K6#bSF z1B39pr=AkqP4}KFxqGt)G%a&?si++XVmtk)Pa%eCMG}fF_+&b;x>e#3Z#bnSJff1dJzTBxBCrp%czmkR7hr9s@fq zf~pwUK|sa`hy-?k`aklKZT!FvwAeVqgcGZo#HxTEK5J@f!wq=mG|I z=mG|I=mH7sAc!vjf*;ty5JD!fU~nSqZmT}Bx&=6}O7cty!BZZ=iS2_5DSUFvhEHd*`+8kb#@h+aR^PIc0et!_P=+^Yu6vJzz&cO>vz?WY1=1ApFcu)hF`l7EQft1#(`1YU%EkS57A0XO~D7EjNe+=OHR5VBTnw{6M4pVNkIj_ zF#Qn_=;oG>;`N_##!F&*Wp6rB7+dU%1d>tBr{SlR@BsBvq*n}D*>U=gsiHJSx1L-b zAJw1eBxnC<(top8DeY$$dcJ`*dzNJ?*q@+(h+S6syiU4jwx~N-MlUma{8-NL>>zjU z$Vhx`LD!U{(;BGbD36PStN2#1d6tZel?!|CdC}8B6qybr^0U*XUVnCkDNa^wuvD}( zitFvo@U>}FwRp6ld#N8CJD7{Ebnyag{|iCm!D|(B*i|}FDmv!T_i^3HnaaK6dTV-k zac|;!$#SQvZ^?PV((h)_z*vA$^WoL;{l&hj-_OHETV-9+>d9??^Kja{%{El4`{nhA z(;^uKXmAsnQ)q*`z~MCLtHY5}veznm_r=|`{Iz}^FTe0miB{EQ$z(orGdB~N)2O=Y z(PVDGc262d>u%V$wlK0oeo|{Bjj>9*Z;aB4^bylry!7{EL93w!gc7QFg}*hoP3G#` zm($y*5-}GO$8yc>%Nd_Lq8Fb&bA6rnvhx>4-6!~gon>TE+N(c{w)pRFbB1DlW;K+G zP9<%AySdTZsC?+bV;(A5O`4H_P-gC#WY?haQuEI|UfdvISzC$;4Mgr(4n$fv8L8n~ zS{I~Yxk)&ugnsY5G>o0drJ0oX2B*HZ8R1AOd20W~p%@tLC9&7oIIX}lqa2qiMkP+> zpI|mWGdx=%NT_DIxVid=TOx`#k~DXmiTzhA%~Hb+;pWcC%={Ld;=sB19%s1TLzciI zsw?Za;G?8#8v6wXF393<#C}ZjWB_PylF{pI=Op^wR zIMB#@5zYaGlrdDu&@`1^0dtS_OV7JC@ISW(dMNtDf~8T97iVMCjBCt~nmPBlKNq+h zH-5m*jiU@XQxj9QJ7Q&j98IA&q6#(MpaC(cFqB8qV1!(VH$&)@BdNfRLPN%wP?E&$ z|5=J>F^F=(dK@88gD7tjxj&fdL7_~>Q0U2MfI-ReTBlSVt2FzeLbZ3^)^-FO_T{w*ye zGA9=G@T%J!npF=b(K@uvzmrnp_=vasoZ<_uyV`7wV-=#qyT2Ljr*rB3~E0YfG)yW9yp(48p zYAG!AvmspNEg=SC;oE`}akvaq(NrW+$@PBQZ5rDq{S}xPpn=hp)996fa^W>H zS=HudH{=W zTNg$zrmbXq?es-%yGjX>8;`r_8yC1*HVfhLSn*exHj})40^p_ zX&849m4g~$wert!d6{J|)ZCo!7bLFK{ zut}3wyo|xIE5WIU0lSoE2|GX}`c7-YXr7NFM|PYWk<-gmDL;k*L8jv^2nh0OUz5QV zGxmta>rWUPm-ZUG2g-jTbiA#{nf*Xt@dDf)XvuO;H-qt{7+iW?U_Bzfpb|=K6275g z;b~2tVUu~2JhaD;HAhGu6Da2ji%Zb-GL=rXIR?#Kuvi&TMzPKM3-%|zq5MvGN6;q(I|Bn=D)M~rWnaqcptmtyBST)xYSiN=yG zGxS@uM1`dPfwuekfnhr9tne{T{W)SWw9*#-hDtlHEw;q*M;>w`JmV^@^O38cZ&+6s z8}Sk6Y`KdlMWX9*&(WP}T|+7bZVa6hGL7;=eLd(-UVedCg=hjNR_qCT^Su+7 zhGDlRU%7ZecMvW=oKv8~R+pT0Mclh>lj;${UyVYX$AXnhuGa%|Ogzc9!$Pcu1u!4{27{K6wUIG$se4UQzM%xfh6|b)dSF=6+5L z5aTt7&8R0Q7KdF1dnjYd_Y3$Q7cR{B6QoBCd2igr%H8$b5S=K~ykZgb`+B=OJ=JVQ zL-5qDgdXYX3aP`h?Fx?mH4-pod%hC8c|Wy$@u}d3J-8h-gCG`*-!1vi8?a@;{TA;8ibpF>xFLz0bRdCii`sU5BFt|BiewHcqT=ItD zb&^v5z-WwCgWN|s7Q3o=^AnYYkMR&UvkXjT?fy*dF^sU+7~fNJQ#@4)W(nf9D)!AJgf6t)e)w%PXwg2xX1 zh=6kuFh9#R1ZKIbHCj4F(&narMy|BqYXIQ52#%jo7LP4XVGCRRo@~Q-->bljn2jK7 zewr)x(2qSFK3wA9?3tjo9wP&nL26V=8P*Sl`YV1gtsL+KvWr`QY%PpFzLL^|-d6TQ zZyEcFH}z>c(VsEP)t4?!XP#9gCFB?hJR-kL$AJk^o1Mdr5K1pVAsbT25K{W>j+i-T zp^O)R$+`~>`H@E?c^FB)bB~>>hm!Vsg&1I->k>iTX9-Cs4S^DiyU_62!@3q5LE+om z39nd!g4V?vHuGB`(fErTy3L=}Fpwm46*F4Rgk@UrX%QLj7}*l!c`t2NAQ7BfZ=a}fj!lg6;>R;>E@!`NLU`S)a`A2V5@qQ5Ry#O)}A ze-Pnk>#XOfxB*Dzb_98q*_`7FHFMWtO_Iur7zZQ9i}pydQ6E!r41Kp!X%NM1jM{N+ z-FPD#TWMk|5`yFcH4R7&wI3y@Nn((az$_mFWR}M=smGYDRVjA=t2ufAc7oI-SZG(P ziPh50!}u0td^;B2q~<4k^V8ch95Z4qlKZn1U{9Gc%f$Qwe2V>wxwZ&)F8 z51xLn)A2r6Tc$*8O5f(zfZ8-@URwb9Zt&FB5VRT75bdzfaa+mMq*%9UAETU0TwPHW ztWPTQYCA~oQRy7?9KPTxu<`VQuiOgFlP_f4!hqtuRo)cCO26rXi83Uid5BSQ!aYZF zdlZs%$i@K#!mxUKdzYn+aDh*abkvbL{H zyX(?x&}4JfIFd4zroBCd>nG(13u1Pk003y$4V@0xtkG^ z0RV<0ABH08djM9hCBkAo{#c)O7#M4u9s&b(i>0{-emj8Q1|>u|47$YMfx~_#ywJ7c zCuJaH6GWMxSB-edlCCW+J`dUFLwyaz3%B45fnFaipm#f?S2bD3c&H~7ntSP?FSZ1- zDA&0ps9_aRtP(uF+3wjU>|Y$Q+0@;=Q|1m5Hcp`9i4UynfOR!y84zJ7HmZZRfJ?dx z;_`DwiqAZ{q!xVAmUb zY47SQQXki~k6FKEd!#gh zt;-o`8zP56AfPD&zXbuFut}9ov7SYPZ9@?beeo{GNY;h*%^w zQI_j~>8}41-%vAl*d(T>#JASmCiB15GwC_s7WN=e;z!JxCz}2@?jD_oeN+)K}GVrlBDoxPrba-Fh)=G#YYdf!(?e=6!emo zKr~3V)o3Za;Ln~Jv9K`ncj}xZAwm*_dhYp%4&g+6$;^M@oREDxFz_FKi;jcHeChgc zUk*JOi7N?73r-91A?wVxX{SHA$C*tPqDA{@#X+&ETpdTdBJL`C2Mwmw?+X|j zy&4r83?Z5JK+7aV&8Wxw8>2JVt6a{2IQd~WKL>Cg#3NcJNnCI|8l8vQ%wZr5w(m&z z%T3Le#;?})*lF0jNy@uTHpn0b7`^FtVK6OZBPhV_uiyBR4VwTnq3W(3L(I&e#cDHggmk7UlyodUb*eYZ@*@kgS=MT~f^maw`E{8!z8PEC;;!H7A z-_ueKCSz3H?O;jmZXO01oKuQ#iv)P38kqQPF^8>_6-NMD!P%0}V6Y78kbiu8RZnDg z$X2K}0cdW{Z8d_{Hcxe{R=xYTreNP}gOh73x*iHP8tkMvPGG)?QRhcC9C^Czs!%aR@O7 zV-=}c6r0p#aBYT|?0FtmmF;Q`< zmyHky^sn&|3V#1MCPA#Idd!a!FA_PEoVXy5(sbWi)b!2!gnG5W0Vi9#;4)ExD*8U! z!1gwSR0Y2Mt=fS*N490hVgH=JQ|54uM7qq#JBq#3Cf)(FGD!QF!d=Ovh9XE{;9CeI zha<3BOkYw?j*ft`c!#Wj-Fm*VC~t(FsH7a7GSMkm^{hGJM{nfMeGit`yYZrMZ@wJE z5vhhA<+u}N=p!@x49u_I?kDLQgd^(HIuTv!v098QSl)S@WH?Z38E+3^i^7j&r)qKM z@wBOXJV#j9?h@B7gUlDPPhbVo(ry53zM|h81KfjC{__UaCdYRfoiOp>J*whZiy56h z-~sm7HrgCnN-6F;oDDAj=M7oiiAS5O;c^l*B(lzfhIsk0i$UV`S&O|TaGMG`>M#ja&ZnhU}8|l2;Uesj~!WPVSHh(}b>(g?flLor?5X*X7ZM z0=-qVO#65k*j=J#i!AQ@2;vKuFB7tO?mG~9^|b%I36O;bD-9?eJrw_(3v-hC%G)`# zhzn*yaJK)WKix?LnO ^j?DozLdBiW(O>U2{?q`>0xEXwLK(0Y*401u@_<=>N;+S zKjqw5{|`pFY^nirkZ*WNI?j2gd|Ky%&ZAcLD4kjT^&mGsvWgbrgF%=OEOZKle3?)7 zqSyDVHviQAH|~00l|Z$kSQ_*irM$RbSiMqL8B*j#Z}<`02yuRC$$pBs4-TN-4aWSE zADPQK^w!d>X#W>|!)0TpL?3a}9P%S-TF3~e)Nu)(&iVO2Z-~Qik1Ld##XH<73C<{h zOxFw^GRa*Wb&ww}KL;Un^$q`dlk5sCS>i+ud7!V_)nAxFbK(au=HE;R+5BJsKO>7J&ud2^L&^;~u1!3diWK zC9_=_TSzgLNKd}fk2tV5rCJd1nilz-H@IB*s~})HdO#?>*QsxU z$?-eOhvaP5@h4Wa1AGhjw&a6vJ`o7(JxPvu8r%$>P~x-x0&Y~g?9bSU8hgu$;TxJW z8cepgL1j8N{~<&wmF!09!z+ zFsoirQP7~aySK+$Ojn>Wt$jhVIQlgaCiRrX;p%+h(SYfA{_{j>6J}ktNbIeoX5ws^ zj&*-T?OHA(kaItb@8&@gJWOE-rTGsbrHo+(&g_(~3j`AHq*EM$p&>EROEYl)I0Rag zI2uB<(vk9Tmp-B?<_e%GXREH?6MYqIg%08n{2ReLi#DG%W&1q8`H-STp5X-ES0l9=)b~aPa+DKl(}q z=@FG=NBsk?TYaH1mNX_v(q#dQM%}R-_XMx4z!%g^V8TP{mYoA!_zprg52F=by@AN$ zD;GeM;^Jy+g+Wg@Pkw44OPD%4X7HAl``XvQnH=R9 zA^2d-tT8(I6?m8tFKGTokB`g$AcxGS%L>~(!9_|>l9L8W+0DBIDWh*!N!O{TqXVtuqm!GNd3`OVG;c?oWY}n3Ne*16O z*f}GuY#0RtH1fUBsyE_3gwp7Kn&YGY{sFniU;f{X0qYxo2$iPmpUTzGT?{V&0Hb9* zzQLvb)DIeBQy9#7^5|`FyzRY=vmNu8km4}9<`EMv#wJdm1UFCV3Ss~!Xf)QA-=cIgH5zXhq z6WrUxe6AxBXqanw5YDg;g#@RR4C^aBI64VI!R8Mla5fjo3b_YlVS)xB%62Od#VJVK zYv_S{hS@WQsgX_+9di~S@-$OG=fyOHFMMkuf9a6~U|l=Cnli`DU%==z-o^8+@!bB6>59XfQ0r~9-Q3v4p*&ehbx3vD$M1<6bw zUffGpPwj^fa0dv(9wYEeK$@m|nL008A>g1Rm9>~UKFow2p8osCq#R zt5O#Lg@c1Dc?cND7~=M^5RMgoq;4fU0&U&$psn3-fYmhXDg?Yw8Ua%`0C@vrihVLC zMNSqS>I~G#5ibnpe|NL+Zw77^Z3{{jD+l#gs!5wmyvphdBGN>znlA(vjlh^}` z1veb2p5wwbN9$CKK>FlfGCinoqX+c|l9e}bkZf}x8}`>$F{3yP>`e`O^E8o}&k>j7Z&G1HHiDrxsZbY`J%D6t zlZviQj*3CuOH2fdN5$v@9u=btcvOrokUc77sy%c_9TnpMLn2XjMgT=lPrJDcd1yN-AO1W;uPL z=qR2}7htaiGf56NPkDk%$E@`$6Ri)dyC+ zX^~FQ3J0e}G$fC{R>bH1Rk)ThJMdG5{F=43%5QjLc9_DMJ%-lCDh!TfxVec%kf8fyB-BO#CPB$+^e^)(x{nynYlbRL6T?{T? z6Io4p<+*`%q`JHO7-B!N3r0HWY|p=ELhL7Tr4aMcgJ3^TTou;1W&JBmm8N>Y*`uSY zeZ}h_1{LYrwIN?WYjO9>lP?Y#WdYgk!~7G%wwQ#cF^=!3$0rt39&4h#f^f5;?-L8o zuFN0H!U?H~mp3V%KT}!hfDiv8TK_e5*J%QSUXZj$y_cOHabCQzrC~PN0YHtyLn>p# z=e;Wgsx}9O4~uzbJcrcnG%;ia74gs|37Fq-krZmN8b(3}y{5i2Lyq+~LQaSCuXBdQ zs-QLsC9qtr_Z9GYJkXKY!cj0|s3TM3cZh zIYELhi(7<_M^JKJP$$-R!v+%=2Ng4E`B{+dp&Xf&^pxzlh(z_Z zR}Pmr&gEIBL(W|Sj7h=tI`3A_u}r`w=*V%%5xEQ~t~|Ia*238PE1g1*e2H&1<&{jZ z!AJ4j-nsSzWW~pY7cw5X6i;9JNd{U{HzZxCv7b8eR~g7t#*Q{R61p~RpNE4hag%;gN>|Xa|LXmue1Y`gxB-^ z1y!{WYM-dC$J!8sK0z6>DLvzp{uc@MFKrgsn^HV&6Ld4;>ML&m@I%2)?BuPGX8@ zS+7l(C|m|wE!2TlLGnnB;KfqQ#h$0-P|&Hx(Ak|j{WzIBfaPpLAm`_?{=I^4cz!z& zw%&`kgH{iSTeMdKkaS0Y%Fm)4hHiV8L$@lj!JJ6F7eIPrA&^ck3w~Cp&om{IT;bzjGSH<)5p3dC{a0-5deaXAV>}+(qHumV30W( zIIF%NISVydxAvuLrbX4rK@6q?QOcVGbn%KLl8i z97;if%ArJX+n^^h8vartp5_mBvK^9rD@Clo6Jpnp8 zqMOm@jErU>z2}(C*#iiGC=e!4Vo#FDF_?_$-xG6Ks2MiMWlR>q4-rs_x^oZ3F^IzH zwC6~A(Y7eNA^V|5T8Y#!@NrbTrnFlj*6;_)E)s{wu*e^RSD3MH&VnK-< z-@c1Nj0ZZRo-6jEdIuxs8&)UU69jSGHl|r~kaSHXkfcuD5R}L?jHS<=@QuZB^h0UP zu0@Ed)32UG0whZqI()(ZTDS6;Ql7EPLp9^N;06n8358=w*7?_E;I z8D$KGcJaXr^bNeZCI^K{1Llx&Vd`>fQY7v-yIY~j_Gp%>p)**`=tjQ5sqIicv1--4 zzRTpW#*?`w^o?-*vt{^da-09ZwXo2$GDYK#yKJL?a>}*4b>I-Ul~Ja+;y6s^2afyb z;w5;=g?BeR$KQul+ne^4b@z{DZaNphxA5jkIm4Hfsifdh>$V)GnTG(Ro6K!5ebAv`phBE*bLz7jfX6bk<}rTIYWY z3vfqvj^V8*m`76kn%wx|T`?IfbSTKVcC%8AMW3?Y@xTj;J^Y0BcD}%=-4pXtbjX=% za6HQL)o#*zj5XI#>`%0fqlEkphK(I%pVzdM0uNwtLDe-ikfdbHeMgpyt8FUj_2gVK zH(g9ig{;qyahObcKL3db?wcyhl`3Fg= zgTazx&Qpn}zlF^#fBEIu2?`rzz3>exp1C;9O5DrNZ?mEyXpm%{^GDAKInzY~8Bwlk zi$2W-&HzG2S1DHDi5vY8C13^hSiDCOEQU8^)jovtTJpd$?&71cPogt zesVITN!a!ZSKh|bT9qY^_GFq5Hn{PG1l94Jf4s19$=|5?!TZucDm=I^5rv{wvI@jT z(PRAuB^xlTx(Fki^Wfq{Od*nq`--6>WP^9ZPy+{!En$R=@JLEvENY`;H-5hfCJ3NCly-1yA-dt3=V9A?LNHPik0{G9kplccfaq;PgI?hIA zv}Ojq#r&ubK4yf+$6EWgLjqW)AI0N;Q0F%#6cL?w^e0XkQwfF*MIDdYUF}UTdeRZp z$-})!?nM+k<#bwqx&+%1dKW0pVQkMlYYojB@W`RQgp2USKR8IWqJrt;-%2}UWyJ!8 zV<6!mi~;H5eU&5-L)~1^uPjtvj|p;O%02!GCjP<#;SY)8Mq1geZi7u(lA^wd-1&Z$ zc`E%!*GjRelk;vRB}_+wfc=1Ht^Z9h+&)%M0S~y!kQ^G$y}Lp5gyN!j&$}&}yPltX zk=2sO*8$xj*aa!X4`B~Oeb0RygP5=&_Jvog z_zYJS(iy2bW4JA(WGEatx6obeU^S3KQ__EMz)K!yqCw*E8g`*@_Vy}LLuDN?dP)(( z;ShKLXDS!@N;;7z5wyDTTNs7y|H3^Cmo{3jp-y8W5o$Z~+@!+F#{9@WKG~ir$fNgZ z4P)RqqYb&{^x;!bUQJWOuj*{tNft5dW6IK|{vshvRc$|d z_x<2Y$Ps$9+mpKJ0ak;Vbw^f{EWM9J)9RmO(g^(g&pOk;Hm^Aj90B^#z> zNI?jA;GQQxg&|>zr|b6yPBA(kU*{)bsua4I`*@DsE#p--<|)MTuF@$NEo}}D+=iM|{|afL z9-iL-t~mVRx=&G866G5Yo5UTR;Seu&!zd0kwrOAWvW?He9#D4ml- zbCHU$r`4@EFECIKUZlf1Pkh7lGIBeHv}m}8JS<~pSbYU$6gER-bA@e|k6@VB`Dmh0 zS1huXy=k0t$Ze~E*(=WDmtIjcqDmV$9)GKAk0 z-fRG`NlkAJbtNcVwV|#@3u)Usw9+|>dLSecAN_sF+0{a`KCu^K^V>g{$z;xKkNl|` zV>Oyneu96|V1lkjPt2l$b^VJI&${SrBBJI=oOtTsJ5D^uhH>KgFu3$QnRp(NiRW64 zP}liC}zDRSvCKQg#`#&ag-HyNuiVHzz#P#ZI zrPy!ciLO_o_bF=hEFn3&b`zx}DM7lEBqiuTUBNUIap@ zYZ;vb5|l_blDIwIAcTv5#{s*Zm~(Le6`{%9=glVx4I6c>j%$u^5I&8;=w8$w01qjV z!3pkg>Ha0_EXp@ifFn9G*FaB=y@QNM)+lZjp1=6SlTK^;7Zp{_YJL{x0>ZT??a3lu zLPOe0yuSkNOmTX?fee;q^SS%zF&~sb60z%>rbDL`%O1Q3O_J4?#$I6JB1Ww&P#sb? z3&o9ytAbj0qe<5buR6j_6IE>^0ly1J@le#*5xFd)(bq8)=fvTc9^t%F7cVFgZP$IG zH#|ah%-dZ9^P{YayeHvCXM(#t=;y?WC7qOhf{6+`8MPi%*!d7woZP(v+r7bukR0;F zX%KtsEbR;}sOL{&#X`jOQ>U_~bm27q@5_y{0&u(TMDxtASjB#ZOWf&KJe0pm#L3)z zv{ax8+T^~20kKG$)Syo|uLZ8BcmfXM+8yu>eLtM_8cCjxP11DegnB3+qF8$O& z=4(eP!M?OHl1p(yXzoL6v!^?AHO}i3m@*}9k;0`)BxOn>UXyN1Ij3%t!A2d0KatVg z3SaALK`8ANQbu3p*}Rr@;mZUTEMu4`xw%`QRG=~4p)ikT#d|2DS3;zGy+Nq6qeLX^ z7Orbt7dh;!y7?<;RiwVk6@6MOyuSg9>Tpi@BK9S$c;AodNeliL2W(_uzYfrW9t1p! z4l~le_nGt7GaPT3iFatc)tIy2=*DN_!zGGl4ZZzD7OvjoNk(KFA9g$U+|C3wqGmUR zc9Mr|N?s9&aZwo4ZC&Y3E7tqPoV4#f=NRdDk!@>5>D1)J6`fr1fVZP1Ro8>JBpZ=^ zb5FQEA?k`tR{Mk7y)W91=%pF1QWZC#nRx_#S5r~$zQ{Vz z(;{+#FihporCn)(I$C2Y*C1)Mi%_rIMFzEx9yO@Dn*Csxy;j_yb)Z>HSwY%d+(7yR z4s=Dpg$(-(IM7>^QYa-HL9^vO>npV0p$b@>5;sVl3i(ny3H#D>W=VU+TdmQcuAi(| z`{xhSa&G?wgw}oXAJg>I+CBq&)9AEQI0$FpsSz2od0B931e{*7C16p4NA5++gwo3C z*aLCyEG=5s*Scf+aX_iC%F{>0`Val573Ubl=|!G6D^qtMSm9;ZiMzp=?B0n^r~iuk zs5O*|ROlMQ_4q8%WxU702v$js%5Q?+@?Livy1$0C(a7Byw4@%qhb4-I$TrXFh21=! z!U{B358gu}So>ml_lbh&*mdma@a(4rp!xH&f};K}h%nE2{ikh{d{p52>y&@X?smB_>4=+c@@<1#_<7jGDn*)y*77cNvK%>S;YCOOC$cwv6aMJM$fd9c^ znuxBr;{o_CDy@=#_(5}9QcMWBfqt49;zFLySEUVR6=K&4 zDkT>pL7tguqx>5d?jo?wer!1BD6;T`W-pRIryO9%{}mrI25_A7R|6b!i2@uW9-q6H ze-S|p%ttfvF2l5)-TX6C?>_Tt9Wb+?v7rv|r=GQgHgJfc8DB_t7lC5}wH()5r$c)5 zG$G1xK4f^dWkrNA=n#XBe#E^95&>v0`DstAXaC#>k>EJ8KNMcX0jIg;)F=+PAkR0r z=ktYMSGJr49;{BhqR-}1iL9bF4b}xtkfPL(g%Y5idyck{ z*5uh+ec^C5#KNvY5bYGqCA>>;F6XOiSHH1^0Kzvb10+r7V-5flXDS01Rgf(OCn$~B za!Xe>P1Dl@a7-ns?>^0&Z6oMFi0-HP;J6B{&!UCMWaBc#eSg5Wwe@?GJ2r3w4J`vP<45W9f<|!xiam2`UIq z8^Ilt>J=4biqI_tmyd(nTXniUGTvQ1fH|=F4)&0i2H0sanFC7BGLQ2?5Gy+d;OIZu zsh9)xi;JQDgiGpd4ED`xVGJQ&!U8TMAHBB9;Pz|V&GdF<8UzD;zAx_w;m zjdj01`!Lce1x&amtCD4iP?Qh~Zc`vbEC@s7XRcsBUg?RfR!5)>hw``Rq-+C{Lze)_ za$7k*;`VfHX0uPQJYAPuKMs`mJ_Jg%wKVOwLNgm|Hv45=1Yxr{%7E-tAdhAtSD7*# z7lsa^qmSrVS=$#d@#7KvST458mII|r3@phNL~~GQMX&>M|2Qy8viE2eE7g1>CQgD=19VyAi^TRgrMKZZ;b_qe|po zgMfLtE0EDF&LVvWr28~dN^T$rAc?32^G|4oBW>Y`KBf;K53H~nxo{R6X&?yPvS<|4 z%jgqd#X`Lz75@~EK-{?q;ky`3f?x|W&fjtx0T+TSHS`oUZ2l3sF%8irn-&P&7W-lc zIH}qrz;xhE+c5=4CZZvQTKl8taUyGdR{%fz?-oLGk|M$&|6YbL5)p=eJ3GJ&5ixd# znq=DowK9ijCH_?q>XHG{f`4Cz@KX`5bhRKRs^B=bl3OKi&rw2s(LmvnYiALy8mvkF zzYOIPjl+$)cNX?!jXjb^1?!sSQ+QrYUQK9f9-*oE^_vN$BgaIMduag|- zy3#JRUtDr&-`-ZX&KIwL<3d1&K@K#7@mq!D`i2@f;w-O?CYM|>c~MwpZ6yUXD|!Q6emLaSUl6WogfHa!5Z-HWn}@5niq z9G9PHkPjZ?=FHCG`;nO00H#&qML2Cp;UMMGu%5~QdSBt{DVS2Et_48U?_+@QzYdfV7(9lvgWYZ*cPty#rTUO z1in}!h%f&n)-bHWvMy!=7x^1kD-Rxki&(B)$Q~tUnS){HzpAq}l96=py=Uzx^F`L| z%uz;e|CXcwLY_d{VQfiB#QF;eJ)uUZFlBL|Q9VXBU20PiJZ)a#2b>ZwYzP*j*7Fo} z__fjCLh8>ybby@sSCAY&a^aZ#@x1b@H&#>%LpM4d)Z_CWmo{z5hewB&-{);HO1$3r z?tY};-$1r44U_KuN=IHmQitY*z`p_rS8l=ivm?4foF*|ith8()U=3dElI07&F?z_o zkDkXbyjBt_4=bhI5|xkWcxZaP!pop|CM0`YX6~?1J|qU`>9w#+BrZb-49tmr6=J3J z3XX~oMX$hcHO&G%kBBBOTv+q7pxDHgwU%52<6L>-u=7XtZ6cMuLg&j8_W!hZO922D`k&lBoXvp0TT4WCJ#K?{8A#BYK; z18bnj$MC07I?q5&@B;D-Sf}N-LT8ouU2dAZ@hU00n9*nHVE+VkL03yb$e)ph0`sIzP@tG|a2F_5h7luz zfBa{H5XBKZUKx*ADIr#oSNf1_>dS`?mX#5fAXQb8`V$cVm^zC9gee?Feh|C9DLsg6 z9C1P5kamEY@m?RQ;s1}KJR6YtFrXMzKWj zGfGuajDRI*5Wbl`_wM2Gi#>PG?Ck8E-Px(hxV(;#IUeBqsv~?XL5goh;al}`wi&?< zFq;Zwk=Z3G`GWe0i3M30bUr|0Z9-zru_6Wk;2wVlcN30gW(q6@Y4h^vhWmlZK4)P- zZyijg=7(qfoMTQ;VwUQFwPD&EkHSeZGan{YbLuu*RLq@|1AS>|3#CCC=!-cBA++QkN%ekw< z7AUg}Bor}+8BckAZo6n}lblW2Ll>2F*aOpP8+foCT|e!K@z&ptL-y(Q{kV(XnhItN z$3B%!^q!=ioej0*Abazb4WV3hM0FZ58J=EU zNOU|a-tT;5A74&d+oml3MRWDsJ8eX_Sg;jEhS*OAE6{iye2!d4k9D?DUC9FC(Xjq> zUdm+0WHmUCy9|_=^Dfe`I3n;k3z+3dww^3;Xn@mjmKNtdyfzCEi{@CCp6m$d4NgVa z7LucvnE)|V_C|4@!N?|0@cR%~JR-v8)8Do)?f4oNlP@R(70bKhq%MpFo6(5OlS=)l-us(Vp`+t72)11&_$A3Op{thJJ%WGw1rZS;M^7L=)}!GQF=vaiS^B?%xo~3CuNv((@$8r906SsN9U4xgqQ&;gwoBq zxA&JL_$mJRKu?xy-v>>H%#5?p0d?G{y2*a%36g=H4TI`-l2=yVEi{knpvLZE%9ASw z=RH0xe2akw+OF59y)XSR1CY9<^HU+FjmBiH9r|A7T%Kk$?vBD+>4YFNHEbOQ;{OE} z%}qKd0!h!63SU&TCz*4TT-^USHPx~VBg1vOj+|xkfr0>5J9|MWsYy^V*ZGmX-R!Ll zo01q}0p-Wso+#M{WgKHkK$5<{jXh@GS#)8l*+qDJT+;GFCZw+LMIBEO?Wx+D+X(sd zM)YN4OXewCRDchFn0YcV=~Gjfw46SjLiH!Y5vdng1`GWaZbGZX*M{;{=EyjDmNvq+MQd&EN^#rqJq@$+_0hS_riye!C}Z~Bm{p(&Ia%=3!(r$1>=f55t5V| zbklo&XCdyxM8ENJ|Hmv2AK4Q_x_!%uY1lI?8==B{SF+418J>V zC`pCT0>&4yQjgV7(+4>6_^a#Z!NEffei#0z=@2y&b|?YMS#*gdkKY73g75Y%no|;OG$;luh z+x~#NLFMhmYpvu7acb(zZfupp{0!isv{oGEMLvJ$?^D*&&v2{ivOI(bg8vsh!{(^i z##{T1E}-3?JfN#O{-@<5Y(zdo)s_{#9C&Gi#)bWMb4%BGl?S zcuGBXz2ezYm%vp0a?sJ&*`GPZ+`K-XfneBCIO6OyAA8{`#)RO8I&Gn|a2|s<+{AO? z4d(A1xi+dv=BiWw32~1@Xl+7=B#{Kb4lkJZPlMvP)7=ETBY4u)NnomQ%N#4XoJSmb zOxHo0c+hz#u}{`x7*=Nk@*3J@Gr^)fnu!LAjSkARkc`{i&Gx9`h2!1P?YC*Dp)&cY zC|MBeFLG5Bje%PHXqZ1AVqQ)E_%r)tDGFXch#n$&#CH~FA8hz^bhp4v?sw?lL!!40 z;}2oQO-+ys#&UjUKDc8q>Pk+tq#{mNecH$g%wPML4!E9p+0MkzNGBIc+d~N+gE$t= z5j4Xm#@X0o=6g6M&67>3sPl^|Z13_ehmj|WQG%*X2_fHz$?YgSnqp}SbuN(cjT}@= zA>36NAuP9*U;;9MRu*#WbzBCQo%aZT_LrQ5fd(V)NpTIwpl&k%&_)@GaVD$lW1ldU zX5)5+CQo{7n`&->Yzg_yq+&i#O?7T_hQmPQ&s|KJla^0^Cj0!&{k|?UVbYtm=#%@8 zEr6!O8WHH+w#L^dI28ou$+271CSy)^fu4a~RR8n~@!s4rcI$MtH#kEM8I28ay2EDV zyk;@j7v9aj`8N>`qEh#_%e0?thi(@muRf=%dJavKm7Yh4uUhk`A@J_dFL_S%7Uk$0 z)p@#r^AlPjx7Pec)nV&n(2t?8`L8)xJP>(!-t)gmVuGuXm;lbEFr`YyxK#~iCA}>u z(q<*ZG0;ddl((~I(CJ_P2jYkt#;+0rJ{nD|Q=W zq^+MROOqku!r&0_hU(WmNKUHZ;5QjyEVXt589iLGZu?VoG*n;r8$-o6 z=&#nc>(;bDcc+~!4nAoAy!qfJcbw|3<%htEUi;>cLEYsM+anS?Pqw2(XeV}hF}ez3 z@dg;NvA%~A&&e9i^e*-v;1wCLRR_NLi@24%b;{>^y&Z(jBnIsSf; z`{$_DvcGOlXc_!{(G35AX!MH_Sn z+~%JY@q$M}_y}Xsg@vmZ_Z01Dyi=w*_y>tl-v$O>_M>9lpjO#GraW4<50(j{=$;dS zcL}*ITnt}3ZCIaYLomTTxlc~EAA48IGj7wT&bz^Pcg<%adW(MS&zP+EZruz15UGQI z45AF%9e9(t{K{UeyfJ*`IZ69(1NzxD*PipXCEt-5LqF2xM=5o3G1yFG**u0529Ohs zIgR_ol&rw^y*-K7z)-m;!YFo_g32MM?r$O=wBR@JRjYw~18ZH=!CkL?VJCb*cqU$+ zoU(q(c)m-e(2r_6sdtj1)d>spEhZGa!1i|S7Ml10Tt>aM{|~5yf+g$~uS@sCswrqz zFslYO#Pr}UKG@qvsr=V7R8Z6n-X{L6;Dsdt61c{v z+sxMwEWU{m6)kt%K5c#N*_0XN)e&eX3Fy$~FV{|U6*7Bl#o1Q&Nepb9@ zG3;S33kI32b3)&zn47h2;g5UChv0D?X?&2=y1--|TU|atv(ycf_{9QKZ72it5Anb{ zuCkYYB%g5GtIbwnfF1rXIjhy3U*ICB29pdp+VRu+r=gs_-?)WASrVj*TTv+W%hwIe z97oT)Dv{iUJeGC}?w6cklkm6XOb&aNk_4fmb*?WkAtdVN1*^ws_8Vp|+c%!a=6L%D z1nN4?(kf>EE(W#3>cwDfZfP^7nct>7NC;TefyaPNOVhYK|7hct(%*i~USH)Q-SXOJ*XJxOU zB(21WD&B9|YxNo3!nPWgIsU?GOlF}hCs=pfhcEFG|G>;E4#r1{Noo)Q8(JpkatD!N zW~)Joo%d6DP=GLir$u?>DI|7sT$??OsP9qY^^9Ymyq@yfvB$8CZ!iD6Y%y)Ur)MJe zyyH7DHbYA7>$jR?+v31ecxPPxLK|yy!^D_NXp^5ANvUEjK#uO&1MJc=3Y>Xr`3td1 zI4GZTl8$jk11^ilJS;j6EXR_oot;F<7|A?2z64xTdNR&1G+ai2i%YRw;*RH4iPm`l z$O)z3|A>xkP1$)Qy zj2LZL&3=*Uzu?xA;+PG^Xpp8-jxTtYzU_Km7~aT|mOBEUm7t#+yz5#gh;o%BQLOg@~S2m~>a{VAU}7BtA`K65-bN4XkiOB;Kr< zUrMa6VG0@pM~VG(wKN%p-$-sfePMSsszgqzeBqls26sJ=oF@2*{JG2pLHVw55bgc$ z7dXLEeAY126&r_JU7e2aF!7RSgqhab)H__!*5rF|$UZ|u#V_;%^V?Q*wnUjl&s`F&rfkM`fA4U@8^8j|93c#LkE0{ov z%Je}GZ6WCKqtx9N=UB~H=piV9Jvc>WU_4)89FJ2}28t@IOO6XRp+#kINHau%DJp~b z89;x=QVB$`6I6joGeu@vQdFh}el`%ppRuS+2eeXWz>hO$WULTI)a7}bH-I%! z;a3&pbPdG9*uy0Q8w4eiqXZGO#PK<9MzkGd+i_K*VTVz|4Gn^CVZV&+A2@~pB7ikz zYN7QOTf{~QGnbzM$dH;cFe^iGT!d);`w?wx?%gAAMc4dU3K1e_+Y{UsANVP;HBgg_oaz)RWm&hPYJ&;+o z^=oT!?ndG`9)?8YbWC}GEDQcGxpF18qOnCPS|gNYu>gg?r72< zj*>_`G*QA#)8-=aS{t$m#2;`hh>Jb}-4gkcSR_daZ5cF=hD6|8_{f4omJ-Cb2(bq* zqXD4DS6n9aGD4h2$dtj$2xq(pXUupRAv|S-PP~i&#KoNmf_NDr-;QI~ucN(5xG-X> zxE8W0=FK=(G^aNI*kwmJbk##tFgm&g z=uyIh6e!W?z_m8iM)+9V8WA~yZw02RHJjlVmyO@ROz({LcsZn%9QIUcR@Em9hkPq4`$-U*J=6#kOsV0AEg@^ zlXZh{#aeck?uB|^>m+)IA)RR3g^}?-2Xg^%CN25U;i->2z-k$&;n!|z9PQWR=RJyU z`zuSRM$rGJiHx!9p^t9RX-g=0|5OZ~>b3jzPHYUKo4@l51{@upr=J*P0Jp{PdF(cbK^{Z3eh@;sDU!KW&d>y_qDxb3|CEG9=FH zkHP@LB+aW9!xh7{S%8t)E-?af5!=7I>KT47UxzCciJckDf zq+_PMq_|bE$v2CO-(Ld1t(N309L%x*+>P{eV&`s+Xw zuA0gZjMopkhTdv^&4%jCAbUY>TF}?lUA)TJ@(@lr$%y;pe7onT-2fF%rW>8Z;G#d{ z6IfCcpKp%hN@&)7sJoqex|=b)BD6W=p6__UWHDE4eEA)$6>o3-;YkENRB1*&!>zf> zZ<8+CATU922uOap=_jcq{Dp+&tt#gL2I4TP2R`$Ao*2K1B)nT1pvw)S8FsH_!B=j9 zSQFncTbLzs0%B$V9=BP2;lR$ifY)$ApBSj;gV7Qh1H4SeY;AdPkeH88V9AS5?Is*uyP zxe)C9po--;i+?fuw-N7|Ys9MoZiyB4dYqL3;!1JY*~5J-)YA+TROth?*fp?O=1aUVAa=fs`;+%VVSXG z8CtG*N0_-)l7#ox+!j$%#|g1mG0GFsA4Y~9K!W%82QSmq`WjCjnWtx)T*)myvS*|2 zx3uq50`MUw3ol-OvfbY)^gdj>7EZ!_XS`3xX&AoIl34hOdl&nPU!J1_OTi9w%~_*S zM=7{z67=-i3eqewNG0-F$t-sj#=wRrZTgh|o=Jb+z3F`q+A+_D(6pX+Zd?U-Xo zq?p+Qs?ew#FSr2e_rII`%fHMDttqyN zJj}i}Tm<`Cix%@0s{V`01xtY|#7*P$82Zzyi@>@99OpsQ0H!YFWxtAg5GC3*j?%GB z{ts@;=S{D&Jzk+lv_>+!cUVk;O4K1}+INH8bV9a$8jqqnCp3ReREhP+sqNR{#wAnl z$tjTs(neMVyCW?8X)*XuYjm;Szw$+NTCN>tzwY!uA>6L<+W))+Lsng#{)BjI1qK(< zsoei>R6J<$SL<|(&`PmzK=n3tiD2Z9jg43=$lAUt>{QFI11E}U3$dK7{~k^`yL!$s z1SH(#ws<2rj?9%n(q|6DC=zx0DmMxCd)$u`&iSL(ljDf23|9tlI^`qPV-Eucrb3H3 z@kE`Qp}=SO6n|voum=Asq4gD#HTgnxEnkf^RCoj~GqFKkvB{Tuagp zp5#eSEh|0)*ZibTxKzx+mp!BF+OD5bL14p{T_O+MoQxrs2>VANxDdNKU1y#`XkW}A z?b=4~xe7~YJ3Rb|v#`FG1q{DyAV8PnjI0EcSc8~Ocm=u3$eacVOAP14rvimz12$;D z179zEu*M}eJvbJSG1Nn3@RA<}^bl>?GuXjAy1W$`E3Kc}M}qQ6wAj zL&k2Fkuadu9-BsQXLX#8iWN+Ui6lbmL?C(x@rwU5jL_EtuM#(81Ufnhzb(GvwPQZF zfq_H|xQ+IOBZR>6uMh-!A#Vc^1|0w*QqNL?cPEmg4iO`9MA*{97T)kr!{pq-<(El$ z1}mt20@lH(bixYJkii*{Fe03`Of9s^YX(orY~{9^PN5Gn&Q%zwa6MSBBNQ8(v4K%U z&k*7S1Crde zzcMI`wj!`*9&%q0auX&bC19|jBWs_rh;&5O=zSf~d{qK*Ou-?ysC4G)lOZ$^rMzI{ zUqb=o9A=&1zK_TWU{5HUhag1|q>Qd){UH6NJ2ae-Kq~^^lTxz(K@bfvX{SZaqc*Kb ztQ7nV`uR#)0=d5tmt>@B2vQYupYGIe&~=)8zdRpETE$gu9&AbA+R?DG6C$k!xh|ipxEtJb`#!V{%#(C%hvj*el*83 zlU`nF^CfUtzCINUbpaZb-|sWFwgvUa)y_*yrCAtc>*@9M9Vct2@*59lz28TKc>uid z8;D$vFE&4=X24Dqxi&f>vdmxl0h9yw?Pa0(RUthYOAU0oRWcSj1%oVm_WH((i05a>JoKk+4~Y$`$@{pcZ_f9K!ysOcmlnz;T?{hqlB2` zrFvfP`=tAN3JpCV?;jZ7Geqq_fi5pR@dg(CqUj?dxU|H?v4&w$jxK5ov3 zIM*zSo&SsG4k%)%+kz|b@6;%X(BfOpb||{13sS1trAxaZ(ro23!VZd`(R|n=SMxcx zy8JRYkyQ$WFJ*nDCkFKYHrjZl^4IpvLh$|IQ^3W){4eAtxB{OLfDkG}*(;Tlv)4!e0&-t++@GI`@0Xg|ZY*^ZI-3vC zVWyiji3UUg*By6Iu4*PVFV@f&+@}W*{XtoJ`zOrd;AGniUtvn>{xn{>ORvYSU9P-Q zVULeqfUL~pRXLX^4%{1;YSk5oFU?n#bXlSme=2-wkE$~~n}GlFi(KGYA3iJMfoEPe z`1+-+Jubzub)RF&I@!%kqzhSek0iaBq34&0u0G)Mc&5IG@?8FVtJTlfoiqhP67|kd zKsd{>FOd19*FELuYK_4A>SEJx>;l$L>_n_wUIPZp00X^;H|LHJI31lQubPw3eenRQtj*e*!3Zyzbb7T$PAP$BT?j;|@c&(~!O--ZQ+K|B-K4aJGLDov;j z??ZoYut96{?PXcwDXzbF_HJB)CWsY7!~EP(W-HJ@k=XLo@IJiiFCQR&CH_ZP$YE8m zRlQr(c8R2LE9Fl=0oN?yc@wSOkQYal@zSKterjUZ$*>RK26Oi9C3tuY*W<x>&A9bOE&X=8_&aVb+6n(h z*c%cmV<9kK{relao4>BZ?Tk13#8iV8fX5N|rkEcIvLGrM5Y)fjYN7p{C-Bm!?-z0n zR={a$d1(~dMI%DHNF|??$LP%;WLUqEtU)Og5pYWUICmHHe*wBZU+HUMQE=tMzZmoD zs8$&dJbI6OsG1}Tfx@*gV&zxbLevWi9TnZ&BdC$6OwEFi-g~qB?wHfZnST)8w}=1>;!z8w zUM_$+P~aLJ{0>RfJ0@6G*=Dcn%I`sz254Hew5<$j z3%I8%Xxw1S%}S6zPAtMNaQDg<4qiZxwJPLgwZ;O5C?Ajt=QR$qz>voC^c~z`MbA3ChN#;`=V)5cg_b%u`5t`0Jq8-KHlTR(-9p1HbMH7GxI0_)oqOCaDy&=^sgHH&xQ_NbS;dR!G$hCPk`ZCLDX-FcnV)3 zv=Jk9k{lIzDJiWjG9iuYG8YaXpkDRkvr)*hSF+l70Y6qd1aAP1ybBD^`eA%Xyn`>Z zklut3eqPHi-6ZU3iAhb&7lq1!CL+RoYRq63`!iDNN%|b1RQD87vW#|JorMm$GGg;F zSf%Zd2_>EXIr4$TM6>Y2?&62M@NU?Q3D~91i+6IE5~RuP!-%z=X=m6BOnrs-skA@~ z)F&Hf&0YZt2(#Vkl*pG?Gkelb_L0eD!vCkB*Bg_>FEM!4u90UxE_l0lFqOUyLM(`M zEw56UNv&eC*H#AMwlU?rihy1p(Mb1|-J_u5%4-qk%Q#2GA`CB1H`XfS!HIMftL+C(e+C@k{M# za6$(Q*d>hqe0WV7k7JbNM9Mau=1SjbD9J%4Ef%=NSyWc65?w%alhxwC0XV`m+G+Ec zXD~-=CPC4bkgL+)3#TyfVOvQT<7tlkx(t)P)s(lK+nRMc)&(Fi@^p)Fc}&(g+c)~ z3vMmSuRh$C0ja(&e}DR~_56ZV>D#f8+?KDRlVo0sLI;)i)i0(_)vi3;g>q?vBxn60 zY^@M|jl!Zis|j#BmcUgitf$_Idzx-~1y?C;Ayt+eEHGR`slVQd*dd1S%oA=*BJ);# zgzy^kD4Ln$utnr4Dzb0)fxC{cs2@9GGS&voi3p@zp!^w3t+yHyDUj`tK^bni;AfUX zr(b~0AWoUi1L_Ka38TkQjL_Tq5}|icC=0XwUt_R1tgg5s%_+GPn+7T#e}x5nOx7Iu z^`gw$F5w53KlcuX10STf);Yy0*qnGc626n$oS~MncHAlonx9+X*BLgMD$VPGC6)r% z;)uA+{+3E82mLKHP}3$iqhGl0eF=n|W+$e@?LIoNv6tI=uawr-=?DhsEcod^8>D*l zTDh%s50~))y_8vsaNV~@v zZMI2j|Nc<@HH>u246x9Qvjp{4)=6Q2_h-6@GVie}huS8y8t9& zr#jmr8U10dgN+I4Jn!og2HpVJiX?#YlT~e74I z66T93AVD--e`D{1@b2v9(VPOrH&h7vUgsD4!5Jqz#{+0jlKR7IQTQ6FxbTBq-weag z^nGC|aS@j6-6EYZ0llR7ekmKAWknoLs@+{eHwk`hp8-EIIxcK2!4|*nufaILUL3%# zt*9UQ;9~)hlF922gzX51#Ux6uw*b;S6R}LXRS|FH2e@=2E{hM<`QfsM)&nm8`CU5V zENu}j7cNglAhJYOdjCUwtsKe!s5K%&1L;6oB(*<|{s>23pDmn$jTP81vXgG6jjHfY zGV#JX80^PV%Zzu(fJ|$4v;4uwNIV53bVk3Ya3;=l9?sM`Ve_vDL4W7nsI29uYz=REd9@Vj+vO3)t2Dv#~ z#}(n!+RFT%%2>-h@J8!n@CUooegL^wdwB#q0&z1q?@nGLFsFu>!uDKaJ8gUZ0^75^ z$V|Ew7Iouo%6dM#S#I>#hg24G3xL}7bDvnA#R5I2=1&_1jCQ`2(;na!t>C$Zh0uV? zGnI$zp1qHVg2QgXbklDC7o*x{=QcTEgyE=E-Maim-NIbM1yr_g^jD%ds=h|Ii1mT& z2q!S-jQ(M|J(V5271f$0aEM;Qo-%L}@NA9(dx zE~+wwYPWmpI-EdjFJ_YD0~~GI0nE>2Xo0Rr1F$O(DFovNQdzC(HEAY83;hu?v_ym) zfl*{P0z(d2W~e>Kp0?OC;)KZsOZ+V!F<9JG61mb13IfsTTuBtM=hi;FjwKihk zszB(gDQ288=3;vnP)ZX*lsGn#75IKEyq_Mtw%7UoDa~6if+Q`Jd8c5^i^Di3SVs)! zL?Q~t*vz&$(RXD>i)R8t5<^I1GMH-(>97zdM{=#v7Rj|nTbOIj9&wxrIAX4~(*VWL z0HKhQFto*Sf`k`+S0YfxLb4i=bxZ_gNv^eSX4T-l4iAQp+;Y{D+OflCQLOg?$gIsHNLRJIx$5U?uoEvA@jw31TkQDt6{WY#q zcU|=1jRgD%oI-zf2t3o*>I3+S;JT%+SM=(0H%UYu8013|`3TpRniL=#vqagB5Xka@xglDI}f&x;P&lek7AX=UK^C$5oa053NIKpI^mui~;Q;obD*{8*z<1LVFU|NTl={q%?7jbOy0mxaJ-an@!%L*qn9fICEWq`zt7M+rru1 z?IzI;eT_SwjybHri>0HuhVD$OzYW4!G+U7Fj@7@w^&i-bWN zJc+TYCwr_(Pd@}T$sVNRNOlr8BufLj>lWQXsV%R(LfS@PmEujf5B#alUR#zAfI+BT zERNliHPHPoK&FLGj>($krpvx$@&Q9y+=S!89Af&wFanF{sJehJEBvBi8V{5N(1(Nc z%3JnKeEkhB9rBl^U31>SiFS11FYE#4XAN2t2j^e9n3(N9u{e9Jw<>cKRFTS~tK_Ej z58Af+D5!$%Vc5xIAiRj3S6aX~(#-P-ulm6mEIm@9z!Y;eh|9#5T4hz^L%Xrjb& z9>Z8xXZ9S{#Bd(z9V{bdFWIW{!l3boC#*ONH z>~kPBnBT-e;F@@2u3Bt6(`o=^NW1~38h_-Sy+F9`eo9kZF333UouN?3E5XEPWkx*y zeiVN{x2K}3Q8`k8`{39H(Iou-FT)G%58(y=9e#5U;oDE3(OR3i?(fF8s;(t!j3(gSt#^~56 zC<*BUd^2bw|B?6Ep4>mKA{zb%ulTR;DQ<`W68l!I>=4=vM%ajGP?UDnM04-ILY|l! zIYFs_S{rM&Ec|i%tO~Q2+{?W7W8-45zUwA^m`Cnk7qg^S^T0<^BY6zo_KR4(Aw|NmLCCxCYbOdG_bl z1N_j8`7o6fyQ9G{l|PlXz*Im!Zb9M)mzt>-_r}j}j*tavq-Qrp#~zYuc63|=uRXD! zO_1zMLnKM-nVDnOY48Y66<|rd5?xepX_->2%l37E2DVVklv7i24LBq(aS0%`_L@JF zFg*hicI}2RjBJl~+*h7?>kEwc|F{+^A8nT@kR0EJilgM2bPfH#;flqXZdzuetm$B- z0F65mFqcr<)PDFXjCAJ|NIn>GPVFxc2F5(GGXQGfHMN*I#es%dN-Os(s$L*FH&{N3 zU35M&LLNk8oztNs6kP|s)KQnc>}rN@ZLa^Muy42!*7qjye})n*1dZljVLkhmG)8`Kbb#*3M;nL0iJZGTW^+C*xV z3wlPqI&D2Vm6Is zlXhpEMz%{$6#>@j9ke{vA_7Dy2&#()ffQ2sB8f;VDx|bu4|)bAK+k8wb@!0@)1-BP zuwUy^`_VN$k1v>A?yk!q7>;EC$4=o^#3xcH`QXAC6_9I<0E(TiI`GUAXl4tN8%|;Z z08-I5>*;;uNcJl1FJ&4(du@1xo)*5Lj&F2~*YFED?*(u3P`-V1FzZN654}rdDP19d zFvH0G<7;xE<8{TWMf$OT>v>_zSN!uq?E!KtvUF=~@G_f&s!N+s$pMFG^JjeJnYnH# zi)cI0|3I*ZUEZ-}aAE4=ZKg}`+Mako>ADmW8~iA#A9EnfoMC0+SDH7+hedD&x#c-! znX7(p{1yZDKwbr(SNtANn{!wG2%I3lN(CmDH0;pgE*GYPOuDEK)^A47eIaM0#;ypU z&cVPQ{M;PZL*F)t6RN}oxP_B~I3YzHpplFLOyWnE43Lb|z^f~~kDb9obZ+2lWVf5L zm=3vrvch#TzWV*?rz=6w`2KkkWs$ph01Hi^@ zhBeUhu}abmCnR6wI53mtAD|0D(sqnkfYKOrK|!JW+7%~QV0$s}Tw&S+uzrDe^p-s= z8JXl;12y&0pLBbtaBAk#uzx83q-Tly%#Q*El0Dbe!qp&gH*)Yx}c z_TDQ6y>lU4YFB_LkYY@&COTGUR&$)J31HoJkO#1ChA1hds7wO1+Dsr9GbpdeQh8Y3 zmcz(IK3QIty(01Ty8DvwWl9-_--P8c)V78NI3?3|3O{d+M)>I%D5E5#0L$z?IAeZP zYAzc>&X*p<6Q{q=A+jZCUmEX0dSo!!H;&X!TMhim9kbXcn_i=e#9OIDC_G&f6R+ta z9)~mm4@a@rxLwqxV%C1lA!UEbgCr;>l<}ivIp2qR1NI-p9f&p4xH`(iWvXA zF^N?l-(7}1ONc!zqKQ4+n3M)r;mMojbwu02Y`5RJ2mXgXtcJ4Zb|{G+IlBG^+qV$ytUX$n!^qjT zj3k^o(`LAiXyP*&lZMG<+Mg5m9JMzzZE|1&B2V%>7IbsH(5s^{YPv2ntb#=T3T(r3 zGeGw^1G|=F7R+hMGwOM{Sz>!(zTNLU3$5yto`8jQ>|y~OLe*t4WVJ|0Jji(sM1_kE zXIlQ`%0@GZD*laE_C>A{N||KmwM$P8>wI};O4mMOd!ek)?|uiy$6 z6?_6JN7HWxERIyLJ)kkAC}G}aLz4h@rG zTc0|^K(P)mkc|94z9L!`gT#krNmk2I#uG=;yqi%3AyQe;3EARGsFctW(mvQMkEvC%S``?Nk`wB=bn;B3 zcAt1*SZC4oDZEo;t7vq?odU9dh<&P{(Yp(Ng}f+^bN9grn8U~QO(==)iXhpBZ$D`F zC6VgiKz_0lKF?VU*yQ_vTd$OK<6%IaQO%Ekj0EN_t%XbmrRsSc$_-0{@INBd!g-tQ z34&@2mEeE?r*z1Z;FQ3^Tl`D}oNSgk0F9Pk2L~o&J0Qa?D9*B41Lr9uBbg#24Rgoc z_DPvAU4UwHa%yG?cVi_i5U`H^BFLE>utDa*D7+bOo2(F&vVG$CHwZd|NejD4@sBB7 zaqrHIB-lm|Z}^=@%kN#A21y%we#l7cO(+vc1?yVmPb~9;Dt9O^7JWr~bDqej3dfjN zq?XRZo_vP%=wW~Qzvzr8w2Xr#aH!(C+vLJ7S|Ny~KG1u5X&t)=o~SO2;|*l^VwmLP zo6Btxbz!*XJW4&LmK{EU_#{l?dF9}RFy4xwg{9xN?SX_@L3(WpYVyizQV%No)j?NN zqPH&AN%(%N)@-<{J1RPM^Ua6wNtIPXi_d{ejST!uxwna=K&jGeBMa9~vdRc}AGwG( z`26pvY!x|EA-K6qP1ss@wvZ=`X0&kC?-(EL;%z0`WDj!xqY$wNUX8hp#xJro^g#H`KOw=XfeVi`?3;HKpy)nHpnw$ZoTvhS#>NIWjy&39c^D=r(6_X%lH zC{ABEq=JNU$yQ$%s~~yA5_v+y?NAu+F-YI5q6#P|c$)(X#g4bu;sVK?^!u!69)PQIi z!HtSS@SB(_{Dv1SO*n#v`C6Gx65~VeJ~Ojes`c=6D_P* zlTfPi&Jf(yt#Qy(5#{{X&Fqc@BhEVg9?X>qN$Js42qiJ_%G3ihm6^Q>(s|>Ri5G~T z_rUTIteKfZjW!^pr@VbeDJIga9a3?9ACB1kaG*V0~Rs28C>6> z_8E^jx0znR_YMBoZ>czLnFXeiqch9t^*@cAD{2c0Xl|@U& zM~}3``J0hzXu}&*n%%1K5%QUV4KTpWtr_`Bcf#^Y7;YzYvFt>? zfm=d3+_Qdv5J)onH;*1_sDUd3oTPaN6DX-Z9E$?TzBVoZ z*^cLAI}&c(rVa=PWnSLaG{9^};}V8=el*Vji#6n*she@xgIwv9NaD-ZzB~%c4^H%X!{h_*hojSv^LV6aE6u2Mul|inw)$&Mg4q zfw{wyhqmf0jepcdrdwhP+k~aDrxx1B+JBSsU}_F$EsjJsQ>xh+8`W87)d%xg!vm!8 z)ru!um<7)x=C)BhWTei+vWy*JnV_#0bBaY2??sTqV($bD(GcPH+kBYk(Iu0(lFkA( zaUd^9#;~t4-$fwyy0wC`w>I78LY~~k=gviD*5oNZoA;u%;Xw9Kd z{Li5*9v+v;?27*qhaX{8hDE=S-AQ_cU6t;=kcO?*K9;WH<;R$Nt4Yt4)U)a*^{rdH z0vPrp`4mOfMOjjpkv%`fD_{{8JqOphoI@HEm!e#Z(N3|HDZ6^+vEF0Ag7S0s z15dF0I)<``Qzv@nep|(S|M5h9Zezd@-u9W`s^{w(r7iME{y#puKBFtig`(pSdwsN5 zV+G6P=r5$VJNqm-;8ruDWO(*j?$*Yb9Ns>3EH8n|sK11ty(Xqo_7A{8K|#U#ago%G2V|#G3DI* z8I$JCFlyXF{(I|pjEVJDNT9G8Dq1QgkQw;5Ap4o>3a`MEwK|Ki-`9B4C){h;`e-Ow zoWPC`1^*!QIaZ~dTzMwY&$CX%n$rU3sycfq7PVm{)&r3sreyC2oTi(Atp~lVFwWWY zZ9N=gT&Ga26X_(ig4a(ySwq(+SQK=}I+f-WlkCl?TusqBa+b$B5|p!|uM_E{AhQ8( zM+OaIbnv1fa*$b>0Pymum*BjS=Vw<=Dx%6lmf*b}+tpv?O$UI6upV{|G97M%PQ~5a zv`~~}lSYkLMQkuFY3n`(+h>V>J6)NDmq_sQ1P=_jU{Y1Aap?sC-h#G4%t+xaWGX)b zT3e%kH45Kyb!x-`SHSwD2f0SWeG%?!F5DJTc$jszvf!)GdUVGVroc^TQ4Do)Yb+>% z%80Y5Pfg28@_qat8VZx+Ih=X}iKm^EewKLBLu{_NWdX&+ha_b!7E>f#VEr@Uf{C6W zI$0*Or>KbHicbIEr_fGU7@KsKn<2O!N-XW(!%obGc$&7&|G=*F5E|bGkLI1_W_{1e zoX0Me;d|rAJ?@vpSMb)Hy4^K^nO|0~r2Ndu_5OS_3+@Zq)Wo*{sUykSCjQ#A68+y> zwSK_-(>ZY_6uN(PRGDQ}=<&@!xgPW54^S)hwtd+O;tmfUyK}yL^d|dMgSyHuEv-8j*sVTH$ z4a-Za#h#H&gqHoOXJe~h!N>6Sbt)k-Mk>^U7to@m?2DeH;&RBFK<3k0bK6T|`>LNg zGYoM?o$>1YZbEEP%c@;F3up33{MD#qy%_dXyR!B$?%v}(fp_^Q-ud0!4eb64G?*P; z*+HWdz7r%HHyCWhBzX9lj%mnWo0fV)>}nO;n@JQn(ZY_g=_9H^#S1qsAgP-nCC`|L z1x@T(_!y=A=!rs_mUACQeQPKOc98G~hpo6nx!5XQBeH)sA)=^sAUon&!VJP9!3(}w zK;Stj_&n96P(Es z$&L;-xVbA?M4L$B=q~k)ZafTLMuU#OHpOxyzrf0{l!9}Crz&sA^fiYWgZfO{doG2^ z2~xSSgq?vFDm}Kv7eHFhiE6lEwlA^mm@$Da2qDr9VaV6n+Dvpqp3;66(78Dl5Q`KY zbU(}(#Vs*@r;Zup+55rG`bXXZN=`L$Mun=^(?w_MvX^PrhhbfI6hTiL!FuK<89*F^ zE{MHLOwH)}q;G~u1qF9ucG||vP(ag4=pYXCD9p6DHiVHq=O4^u>TBa>!c3Qc)Vi$!hbLFz}Xa zfb89gX+|4&e8nr=qIDdOg1v6f?<~KC5ykp>vuTe!wh)Eg@&5qUU!P%;et8EgN`fM- z_u$+RblLDMm-IXKAuEcWVxJhIuei+F)jr&v5PUyClM?+dZzXC-7ZXiN)?03iNN7J@ zE~UHVXSA)KbGclKS@1EtzRbHkxe?4awsqL`?b~8bN9){YtzOU9C3NQ=X`^zgrs+0yWicMg^A_@3bvG2{rv zMKUvL3FJMoaPz_P8h||f1)pCJEN(lJ|ClStX+O$L;`bnr0!hI*4{Zc; zBYUFaT0c$!%2e_SNNGZXEHN7v`tGthkU;26C;M*U19H!pGwl(R0DtI>Tk50(slt7} z#8989LbzTP($l}phP5y0dm;1MrEPGNKO|h=r8PLV<5J~~D$AV6ubA$Czq0G^Q)=mOTRHzz9X`qZ-(tzCJi#zBQtSLrIs^_GQb-9#Zn zN{W{wu$*nFd2~6q?}61^y9W3ZNeq9OtWLrC0?TgbE#p4lmvS5NmI;-Z4)f1X3c}Od zVApvYWN`MpJy#IbeMi0)!EzP~i9gq#xx%&c<@YH*_%_osN-r53=P+29`~#b(&x35b zKqU8{^c&y@ro+_V=!PY-lg0BNxH`7S0T9OVV(P;I@SgfyVobmI+nBv&fj7(le}P8M z)n+|qpjX7MypNEAO%5__|lO&Zt!I5Ve3-d7^TvDu4b9Rl^B5EN&*5ssJ~jO&++ z$B9)EuqP6AlaN}9KrQ0o3ypSNY=AOwa19JGD2p>RO$3W9_rD8}5zkHT>(NjL{b zQN$T*DW8G)rO6>{NbCv~BuFNg0LmC2f{QQhh)80VQ=AR?i4|1VmknW?!9gRL-;g+UzebffkaP0Zo=>os1dte8oQAsr=;I0 z=tt@>=D}~-NUWxa=OVq?+*XuB@+)K*4J6%v%*_UHW_j{3@J~XEGZ5VhoJoCVQ-nH> z6@p`VTV?3FBLx%)f+FSNxGpV-6O>^@8fs)BILL8|5cKc|LxqF=INvs$uW(LH%P3OD z8!;k*Ag(xJAp%d(AsbhPwA!$eG7dZ&*#}dez+xUfgo^@&l>MMvqXfKkY`7)zgdY5) zuo%{vNqK^A%|UdD8!uwGh%Cy_!V$KO;<*IS9WO3MHq;!UM?&E~*x-5p=K=q;{ z2C}Sv>183L!v$!2<466o=ifPvg_bg=jhybm6+U?58a2D~4@HBg!2jU~ zQGIslCy3KW$`M8&BZ}sh3~q~PV--tT;`CuY!oE;e)EV6avX*z=Dmuc`UPvq#Ug1WM z<%f8NdbPme&%xm~$qAo-A%|!OpOYx?5^js!J?5`vc|y_O0Jey^p{~3>y-BLVn|Vv& z`f$Yis9f=>no}7PAAG=J-dYcK1B|WA0)KgLhqd$s#ikuJIVUJIU7fK>PEdgJ5aHkn z>L6hbfG&!+!Nxh{XFO{|xI0{|%?aFw z3RvFR}%)2|WpvjB}` z3D?VmJ8cTQJClUS2)F<)Yf9i<&68kVn}N-EY}OY^W*kJsY;hj7H4&nJwIU~~*3>57~XC7yV59w3>Mj7Va1X=-F>&>l-lcikeYc&_MfgQso<)%+T$o$ z0!|q>aI$l8vLqk|;mHzs#%if5+?LmY=#8tuw5ga3M$)CDn{^eXMYBOcjn0BM%`&Z)pN?P#hs%81F-f?eYkb#8V=~ z)d*2)uyUS@D?q%2M2H{;vjOJ>w+aHOk%*xiI z+uyZ+AVZ{W340{Hz(B(JDHN6JDxzBVlwq$njC-lEo0|T&;3+&-4FWysj+?(>F&ve{ zSK0H({qkR+!L2z;$1wRl3Jx6YV0ZcwI45BuozO1!P-^@WrPG|2qox0oxZJm}W9MgP z>yD|q4p%C7&y7^iu;Cdb{Vmmyxnn+A_xr-(;qL zi9u1zK&ykK4%PufFr=5|yFb^0tGy^%{vItdHS81B1DFINr+q%{<3EfS-c98!l0lfs z*QRd11<@`qA{jQWnBF5rgEKRT($B7DIsk~r_0{hK0A9N|q?<*ku=h%!4oMcYto(*U zP`$_MDWPO08mj)>dWclAyb?$&(Yk*IHGKTl=?~E9IWi)M6ls0!NET3}KBbU*zcqP6 zit{Y?>~H{Umg@dT>(*Os1iv@cRqab&t>6Zo7R%Ui5J9B3i(U4{1H%xDBHz>jIl7xZf|vgf0WB2x z>wh6cOIy*N0=IsNbpifXV)?XeA7lm`G-aoihE7Qua`w#duh9de&n$$HdN&bDvVz$G z@=2scqKf^-5^K*Xj2&*1V#(SHZb3sA1*Sc*dI~vOyALJPrMf%}AN_eTZIDVPmun)24`(iy0>Vo>$f zX`4gN)BJjSzB12^e4h+dwM!RZZYJTAEXGJkV4YY+CV>gp&%Q%dm z{;UKRc-2z))^ntS0CWDGg!f}wiogXYerX%Cr!j8pf;NSeIX*yOE2>0VJg{W1+oA_8 zf!m>W;X&F#a)^X1%053Jd#OzQCI_x6*ZNuAuRJ|6%g$JGq?%3nfwJE2N=T4im$k|&MiltM?d=<^h$p(~e z$};6@r`w;v-|6@^*ngRPQU+)GUTE|lhC}>UbWO^VC*m!uNbp|gYQbCd2ovZp29@pN!CRSvR z{*|#V`E)!-Vb3Bt06_G_mR|Dx5-vh;~-~ z2{dJ+opl8iFw+^-*5%<7I4Ze)CLnT5Y=EATOd&p(Tqm0GIe!vQK&50&CU-x?lQzgW z(_a@ZL347jRV3>l&X=viOoK@^qdw3=KZL(A1c_Ipl|HycvdpI~B`<&_2TOj=+|K}G zEIA2*%jsa7KATYP_HiS?@~6FBb|b>)(;4va|{>Dm$CLObWb#nNxlK`u;9fKhT^ zw-sl{_CZhA^QMOe1Sa~S9_^)%Zp_j^xXjKy+BARq?K5OK@9xpo{nq{kjaqIx0(4=D z-r?qcUMrb}>#e1llK~|sC(&6tJLyf6VEssx zcW1v9A9B=+xib@u$2t&ol1CX-1xO*UtMxf_q96LmM758?L>Elrbv$pTel0!`sVdN-Bxyg7h1TXLvY*3)cf?*g(FLq!ar!$VmuPQg#jPEUB`?kw(U~xb z3{$TjU`b(~3E#l2p4OQ)sx#kUBB-0Xv<#rwsr^{1uw%(h-WV)BUscBb#;FnExi6CI z22lNuL?v`Q%vHt$s(9_PxztKr30v$=%a27S8o+3Wu z=TFRx_ejxh=#Z^*OPgDUw`q`iC``LXC_-7-61D z`)}U^!Hx>jbuIS>A*U{1iYA?!=3b2QyJD|%^X3G|JwbAX1H_SnZ!^Z}Bs=QdB$h@V z#v8?=aDX~`5FMSNL`aS@`il&G^T+$#EruK3p!5x zurY_ux;q+VI0fMqk;Fs|CL+7T()49j=fALiT%1z2vRdgUv3TbnVQm}Zydr7cSTMR3 zXlYHG5`OI(TIv);(Me(eRuySYpG^I3{RM0Z0yA$CCk@bog$mpL%#}u+$55KcNl+t{ z%>Rzkga`bNdP!#+t*6nC&re+VfMz&iM){c1bO}wZIRcPwzkmT zf8$kCX&UGB;yyc3tjnw;@$^fbGyQksq9DEWW~YDB6uM`xNMNBsVrjU@C2Bg**}VRS zP5?`k240PUNG7HcOBurC12Ry7Miyb9BI4KQVyi|L+d7+w`nJL>Ntv`eeI9BzJBuc8 zVBZbq?7$UEVbaBJGnlu1@#KkUVJO&4GKLzc&0_hcdxP`NJC3$nfx|Hube){W!hBU& z!aX)@A1&grBoNy|TtgxlZ_S^j=z^-IkhF?>Z3diTu3n$Q$<5T&=ZXN0!PIm~%a!ADXVK7ahJPFB7`tT?XU-74zm3EzT(gMb1nJY)6mAT|bsrCP5J$XRRS=Y4Cf_k2M5~4-XLM2<-9;Ll3g=8#|3`XCp zj3rx}_N0Z1(TIw%D`wuw&|6v%1{o&i^->WHhH64+`OZ1lb8p}K`Ahft-FweH=iGD8 zcF(Odbs@(%OCxz1*#p+^@q*b#pLCP#T<5BijYDM?>pwn(%TssfU=!Tty5Ad{fUT=^ zWdfjWkXt%T+Jyb6WOCsl6PGf2rTdVhOVZ+S6L=`QIVDNAcejf98{?>6I7Vn}@rrWo=F*I_y$(@M)su>?MTYM)y4`a2p_28>G5|++8YJSs!7Imv` zHvR6_iP78Z`x|_@E>{706*n9TMT5_-0R8vF{Cb7>I!CPU%7qH`BdR=j-9`0@q)8_E znYt(>%c_?xwk5G%`vezFc}m9Y#2HCktiQ$FkV{cV)Tofx$Wa12uZJLdVg{Ko^q66? zp?aw^Tb@@JN>o{z8mEEWlFp!M&hC=VF`W^;XRcdm=E3Kg{Rohg=xpjm$nm12lJuM) z3sOASkf^sz3F3p5E}9Ic%WJREd@)y_1Z*9!?qTLvNxg!azjHp9l~l3lYll51)B7e| z9gu}WEIP!Dc-Uqpb1&6<^4ZURfRILPtXMWwZ-itT|J6a1+UiW{r3p3HD5!OT5NUTf zU6C))-M9mzBm3`NOX@0KOLrS3XTAK&0xBMZF_&M>!wtvUahEQx%|3-vAYc~rlKhr^ zN`Y0_{|e%C)7q;UL1IAVSET_MINkq57SM90FF zbj4qkFEG_ke4Ru5Rg`XQ^+~h6+cN%zlYw23s`kwr+neV?3ay^6x>)MsA#!~4Jv4!V z7PSKpKqpPfLN2j#z*@-qpJf>wiz(sgU5hJY;49Oc+^Tv#!N*MGEy}x3MMi+dCQ}=E zQn8u8?L_PExl0DEobg_jqp6te6-qyua(a+UM87UH&)&<@_{30F0Phe)F=Bg9g*;~G zOViI0>oJQdMAMYd<%K>@-*zM3uxBf(<^~g3hu4OfQ9d!44fcQzK$mh?PBXU-=>9S) zmD3#F*aPNtGIH}n0ekx`9?A2^truu#>7g#y(kUArR$jk1gZH*!k;+vC*EpBm#qxUg z#xr|NNqaG69LI-Yi{^>qpKnH2UvGqa5_)ENf^cVsQ0;d6rWt;1tDbg4a?c`j_Yqdv zXhb-?7vJ4njplgagOQAP=1&H!sRX>QrY#c6^keBmL86OP;Z|wCK;&>TX>N{Zcw0=j z8SZRMp}k=^kWR47Z&Ub6&whXmqD5z(?a8Ea^N#9#HBXvQmoq;Z!@6owSJ~m19$isM zk)4S-)s%YieFWqXEx91zc={F{aPq+)6|8B}G^3w)3jn?W9aa4QkCV zebo)Qc&GQ8rtuCE61UFMYgE2NeJhIFho)&%zC(vz#?G9r#21lU0&2B}g{v6--y|&7 zKB{KrKf=&kG-Putva)}Pxjd%g?w!z~mmTRe9uAOqUmW}tucPz$wKk4b;cPM1t?LSGt?0+q~Zh8#qmiGmUYBtcZ z?VUM-qmd=@U(Xx&u_pH;)gbR<#-R$hUjnW;Qzb9H=vs2c9XB#7B?W3?t-d!<&H;xZgSbfasCPgWaM0D>R|I3mFEqXCNx-fClV{uTLO^$NjJR%E{mni z{D8-|&E5{OhJGeD!Yd~+Q}89IFy=hNjYGPsX&B^JEUP!-OC5gbKd4Up5|YlrVFyi0 z1v1_HZpd>m>*v6k4l2q(k>l8xVK7 zzdWigYZ}R+@}dXlbayenThWe!H$(gd;h$SaHTg~&LX{SNJ*r8%WF_g62m22s(V9Rt zxbBz5@z0?I*rdEyqb-ArwVN?k<#F?uJBswrYOHJ#<(R=nazKsVai-!gVw1;a7kYe+ zC$c2-nNypa>t|#_uiZY@&srg2^v2IV)?A5#x|pDZ#-^+{&L*eG6>|pL19~0o{`maG zTrEyO#1({w99Hb;S3B^$a@coq4j#8wr-@q8%icIC_H*rL5cL|<1v((L8Lk2(IDJ9K zQil0B?}7Y;en2|Xi=bb$JNI|tDYw%2TC8HnoTaltrW!4P@|+5?zC+s7V@|SUSG)2v zz63qlFAoiZ`#XTrJ)Z{ib1!x(=x@#o|4bp7{jLT}!PQul_zF;Qmx!D2LJ>TNH(`kQdBaa=z>WIbm2@U+ybh#d@|+F6gdDt1H!k@oSR>#QdJE^S z5&cckL~>mC9nB9R7l3BD$5^@_!d}(%j2xHGG1Zm|l}XMm8*7;z07=*@wKnBL!HA@7 zYYi5`e_G%wT<^m1mf5-RR!LPqY=SUMm2?sBFAu`94koVa=2@3c1MV2H6~6I+!_N55 zkjxvm{{_G8dVFkg_${yv#8q=Z^WpAxORFoV`_L=5=8$>y(aSY2%IVPbXX5tlWw|0dW1 z?xNb+R{lS9ugZqWU7JS&^B%|<2i%8n&XAzvHevc7toxx&cvLJaV5C%Kan9x*pF1EI z+8++$cfH)`^+NkOc0|2+A5wJdU8_0tw<-PHa;PV;k^w1XPK9InZLQ&?VKV3nFl>M0!wR%8RI; z2Z|uXxqd}Hn;!@kjWq>gi(+T#N_SS`1B6NN6g+;HuXLx{CoL>xJ1^7tzpAOc%qi7iUCbW6PW4YH zM)&IqT);Egpm1MuXXH5BM7>LbMDP3na0fLi4PDb4S5dCvz~Krwj!JJxABQ!}1xe@5 zQ`^UG{s9*uqN%aLT5d%q1%=0ke zy8vDn!919qrfi!%WJNufW(b}x_!01DW%kjOpZM&NrzH&@{9xZo%;Hy%4EY7tlX!7iV#J}y{S)aS4HH0z zp;y=T_GAJ}eh}56RQEc-SOE*C)Px3DO7u-NZ+foda4E$yi7XL&BW%nAW1eOYTE7##>Bt+an`d3?7%q zYP9q&tSE91S_@mo*TGV1?6yLzrJ5fnR9afVY)&8*Oc9ex$Ih3H1mu$%gHngWp z_<<>wEj5Pg|H2*ki*81{0&%xH@YG#av8NqT#k}~%(`{RbINvtR7;H-B?F zP+uqa1`zgiu)dV*(qK+OeJg!^Df|RPHCCF@K&H@h;-XCTegYYcLl)>u4Li*`vA|JS z?*y$#S=8a0)5~E)tBC8+qWW>|d4*t^$gzS!InqHCMzKLO>O zty>B$FLyK@9N|90sg~d>FFHqKol|vUt@$U#H4FewEM zN6Q&#rgZyFQXkL9nq7|4No^-@fG33^CwiBX4w;x%pHZ`P8R*UH$9*X`GT0jm3%274 zYBtcTL18k>nW&64Bhtu&h%|XBW}GzNZUvTGK0K07x2_Ej*}dA+i8XAfzOu}@7e`jW z6ErO6>p3N)cKLSVHB+lU#y3Dv%C1Cpm_P|Digv<&BJXHev<|D6s)RqNF?`>)lu5kA)!*?;&~5jXtK+hdlc5Sp}W$cHjo`>6q%~4_{pDCXH!z0 zlPqcj`3b*+WX!^1J*!VEPInqk2CX!HXwmwQ7;aE|AhF8M#oJWphx=KhYu;uhxax;gl++FKdbR;w^OXPTR?3UyP&l{bO|(p?y9#6o3PZJ z`g&}FQgeR2unF=OwA4?XNd{fCFX*C8iw9cO0Sxgf-_WmE->mXXGT03HU?1tjuj~!O z?Y~(2m7!-ZW^-*7nGG)YXGOv%kC(tFh6}(Eo=^;*&*-1^V%ZFSYA- znilb+?J2YoGIB$LQrO}9rUJkkdn&*95l*G3kl^CAD5+!VAa zejl%1DYgJnI3`0dR_#f=+-h({0Ah6n&-!SMEle!#qsj;So-why;sl9R|MZa@&e{)v zugtmb9E7tUFrINDjZyh1doIq>lS|7zHR zWD6TRg1XxrqVDCv-aTf6>8qqOdo-XekTm{hJ_h7wgB-H3$P+?}ICo7#BF9Vu$@U^` z9a-l?qE>oF5;t_e_-rPcI`gyO$S^=#`Qnk>I(^0D){~857Lr@B|8H8In^Mk5DfCW4 zRhIL-wpE|-^j(=gn;XTfEQU*!DjyPFssy>C*&*PjG8NI5%bn>A3zK8{mSyD$B)3{f zZUs8&`zYj=VG9cIk)>iCCHNZJB^@_bt>pvyM$<;@W)?C%c4@WKo=;y&!d>QV;8T-Z zLI$hDdoffII^Xi9Z_1;+{)lo~3vbNg%USgn))yPR#2GR>(f$sFLHxBese z`dyT>IE8PkwxG_x>5v(Cb0Fa;IBK|{EV>92@0CedM2?)Ljr=dtr<8Z|ky4+bmOejf zxaFs@o!t;F@Ej$6)RHuy$}z2h5lD4A#`*^FzaW&5sO`-W_HzQJ96FlpW#lNuV!TtD z5;)JYr3N`U@*bO+JG-w&ULM3A;A&P))P4@xItjd6Twv_|K@LdptSug2USW`AfC~0NM-eKKzN<$gg^h@pB zm?=RgnDJLjJ6Mc}oM6}X4D2R{CkwM>+Sqh)5m5~V)x7ovivd)A!jy-$bM<6FXdWAi z^*1gAx05NT^bOo)@GI68pSM#znuIJRnjzpI>4WMP&8c0lJLGT|!&fIZLZJk+GcHN9 zOLVWLnpJn6rbbxf%b1tcqWRZubMR&4WaU2XeLJu`kV+rX&6kvkqV_5t;}}l29aZZG z5>_ztZcw=RzTii=F*#7JTzvimTxl7cK9r-Cg|<%gJ0m(#rLdP*WZg|~yCNVG=8&z* zKTnN@q8WarG2(08A1N0qO3Dy%4}Htal+C)x$~{K#>Uef2OfAUnOa1foyf8uNBfnvy zW2;hf1bH0k$(Z$iGt7H+gHk7lT<7`{F zXJ9ez2c_Nu*y5jn!8H?Sad_m92*SXP4V?3ikTzJ=R4M;-{_pN~5XGuP1>d~6`nb(c z<)#=iY#1bnC)Aa36j%IBVGX6eyi;xpS%W(@fkszQzqB%YBnD{5!=T|vr5K?Njdg#v$i2_YQ$rSzJ-rK<}6B=;3a|KB%e2ol<^Mcm1TW&jIZU)>FX;%Djce6RFf zdJ$>|#f=q1vWmawa>ODU?+<`9!*91XbG9g?iZ=7t59`Q>!q?~=DslBMg9l!{KE!a< z=K=Bcp%>2n#FvNg-Eno595v?VzDG&5p*d>gbvz4~kDx%=)i%F{Me5XoeyQ!N(T_aS zvYa%9?UXd2?d3w_rT%~qX#DZ7%Xr1$z;;@g1zBzO8vP)qI#?m}+Qt2RERi2CIpT>^z^IxBU!yS1LV=LZ!rxDm?M zX^@?CF3;0nA+27_rNYnp5;y<0RWC7e8@p>+xk9Own~=xg#D0OP=Xga*IbAf+3x4UY zrpw92Ko5;e+!cQj6J1L*u|fNnO_gg40Z{(z+J#vy*9yKjEfAPVSJXZGjp5|V{Rl@xmcENve$Y=VpbYhqAko4c0>d*ToDfXN z)R+z*g>}?Oe-c9mxK4&M;dO<%VEOYDh-6@9ree7w7j7(0e zx1ZpEQSlIv>a;^x{Pczg5~!;WWYyWrKRrvqFUzmya9o$pBVB8{qk3}J%`QyNIFsze zfgXuzm}bd4#OF)N*<6*`qiGTG>J^y@;PE6ms~YD{gFSWh&(RvKVk_pXsr3n6H)hXl ze`|OJwB?rJd4pr7nmAyBE&KVW&Y%wa(YcpCy*o`C%KzSqtW3TW|QTj=nnsZD9+b zjT~1Wp@(AUZtjGF6!W9R-p*Nr5@^&N4iC8AC8{Zfq!dm33j|MIf`5@>-K!>8^RTX?F%vWiQFRU{ca4WzuOG}#7f1K)jZ&PRB7 ze>doplPj%wi|j51H@^?7%7a4Csc;1{%ieLG6`wU2|m z=|*{zIoL|9PfE$r9k%mNQO*uk1WuZIG>M-g&(T%%=bfIj<8GD4{=&frYZYo~s5y&H z|DdqKxxPmNYXa_e!>_MQ#))59(Z{=o#F!WHb-r0?f=-Tq;H^;4V|)&%%ZVDQ0_|U49c?@I(CtT`>gypXR}q$(TEJMpErt($fD)*IBh7u z@UosO>q<2#l;pb@CW|TbRv+ub5kLDS6qCyw=pX<|WgK|Tg1w>eH;N|D-8q1Ve|BSa z;MoPb{CIss-i?dyf>|DJ2h;Mfiyxoh?8%{U-(=}(fxJp$hnO{@gV~xWj9;@j389`NxQOQcQA;U z_~O{L@)F!I*-I!&Buk6k#${M3B%1{%iiYe3c*Ak}Ox-uMyI)f_)olJ3NRz`I4IG$8 zaPfo8?Js*oc3?Cyul1p5KE4|rcNuXSZ;+=zng3n2F9-JQ0=(M2SQ5iOe^Ef5z+(+5 zyeazwI$EOq>kriW<>vj z5WXK0@al|tdKLdd^Al{l@_4CO1zq)NcT#6ayp2e_mLz3JWWN2qbIN)Di(R;fGj>aX zG{?K~^c`4OLA<`1LYg|8ipimH3m%J2s7vNc8Q6%LewgL1ZD?(zK7k7pn&-_06Bv*x zYvdz9XO~EF)L<)vl?NNw#7N-Y&~R$pnRbGOVTma&Gbi=ZIPgApb*I z{Ai5no6!e`93bV!kfPDOAF;98S3~M}*2L#U{JO9ETB;LpYsv(&QKr#DT2oAy>oj9Q z>F!dq<@{>qLvpz|WXH^GTlE4<@0(?)i1p^;*&FLUMzlP_7BihyqJ=_A zC!OtA@G%>AK*=rR8L9^qax?Wi!hF56>H&0jfn&#O+Cc%;y&s|FRo%{+3c2|uG^1k3 zXDR&*8^Y3G!2R(aR8OeCs>0`h7wF*Th}w$inZ$IjFs2<;rNS^+ONWPi-!cv()Bel@ zqi42dx8d=0F6}a>W&Y9cAsZmUwGZ`%D{&CSa*yoC(3_4P-plHC!K7fBJOOm{+R6cy zZza7xbD)!Gszo=^($#`sh0G5SzPc!}E0|nCl{1p~>fQbVWi)oKM6knq;+{5>LQeA6 zC2+FXT9pZqBTy%LC`kZ1zQsaejX4tq1fOUCqcTCG3*q4SJRD`f@t4{KNz5w^7l}^I z6b(Dr1VFZS;ZE>(j~768VDR;MeFas+xcU^8hi7H+4Ea|ndaBYc;Elr|h?1^oD8!3` zM#)MHr|;_M_Ly*;T_yL56een zljFL?lpC-< zT+SyMewyMbuQRJG(6a+7m5GS~C*!&{+UEW01)ZLw4|Ul`F^SLbyP+Oqx@`lWmTuEH zFVqhuoT_feghqG%7b;>&@e)1<3(Yn5cg?{BN~}NQw_VW32X8V(Woj{|rQ!@qGDCZ2 z>rMf`QEPLeSl)ZoO1A%kCCULsNW=F(H$ z&c@D&wOiNrbL2nD&w-&8HDeHmnfZp2)UFG>m}1g=>y0uCN}8{VjhaiYn~z`DL4nS* z;BN_p|6one@-p})cQsTuPlazVLq6GD`3K4}(lhHb>q5tCxS#wzb_d6e*C_nS4qUJL z_#enI%F@w?|3H`{Q%TTZ<&IIZ4{5f_S1edg{IkeVjdr-_Y#iEq7Yu~18h7QVSRWMf ziSo+q!F8mb7_SmxJBwWbmZ;!o=&1?m1D_EpN|)tOxDv_>%`oCzW_Xy++05wzIot5; zYjYORq>~`6HyR9kfA{+ccsxbjRN(G*_9kfg7^Kt#n|Q9@923RJc9C4y3%tKN&^x(DNpr94e?f^VRwIwnj|+SjIGqF^H0!9xfnGBDbqLIN4!9**M<1M+r;Z~gQG(q&bPVTKc z=Z)Rl7Cfgnl~OGn6*61i57oHLtLCC52V0I@;O&`R+a%~u*2 z%~i_}5Z=?&xg{F(@d?SqbM~6PcunVNCtka*;eWxKv>B;Md;x~D$ob?jPlal>s1uW@ zySk*|gHTY_s!`iFO>umD*%tVsXlsck57rL>Nx^S zTMXfLoboQwFl`tFNCrF*jAlCp_0RiHvWM8C}5Mza*UF>;1ys?t4}FreM_%TMvzLZ_ECG?5&!$fzK!Z1{Au{BOa)8 zhBzGFZ{|9m%AQPwVkNLpmtmz5_1BxrEr{1sZ_vSJHA+nP29NgpvaD$5h!5Qu$e!F= zEI=P~hgHm`r;)Yx8Lb6vr$|QI_xQ5Ix=O)_Q4tvD-Cn6dw7by-8^Od)EH&UPDQm4W zMAMT);JEw6WQHKqd15ZdAT?>{lG-jTJmTXmW`K)AGodz>AV@6}IM1vk9K}PH4*PA< z%}7DTIV_B5InFV!^m8}xo9tBbw+a~+?>QNwap#P$T;MW{xw8KydHP7P=N@iDZVHjS zQ0!B!A_CJ4nwo6hc0UC#zyUjZ?i+Id30htyP0iunILchUc4NRT(s?53C5_<1o!_GT z`I1Iv_v?ULTntvPShsbp^8`LX={QtkO|*WqC$~fxWd~ooHIBxU55thfqSpmw4wO@5 z=A)7G?MJYA=3A@sAp;gNF?243p0#VaL+<=xLG=cB?nAa;wT>oV-`P_{ss>M8(SJWm z1he{G-3lS!m!KU9~|r~)>1@g+9YRtH*j!( z{gR~7pAQw#gx)yS!8zpEWaXZRj$>piE&)5V?>;e5J|?L}YubhqO~z2rQc2UB16pc_ zJ78d_=%G(jyH^3(ATtoso+ed?@Yhfa$#pN=sl#z~`xclW$tL&201s$@N2XUCFzX5Y zQFig8Iq-)6gy*I8q{X5CHRO_=L&T7eQLV7n5JR?+-gMKvj$neVvfpVi|C~85<`}0C zug`vlXRWi5;x7`)Mt8+n$MMSY>`5G=+}n5_EcLj0fr%CJS_J zKnF%Tw6N?V(q_yHD;zLGr1(2{;9)o)*zf{rKVuBUTF+p@?#H8 literal 0 HcmV?d00001 diff --git a/logs/sim_2024-03-19_03-56-20.hoot b/logs/sim_2024-03-19_03-56-20.hoot new file mode 100644 index 0000000000000000000000000000000000000000..c32b2b2c401a71c41c9f2f829020bcd6dd75d5a2 GIT binary patch literal 81920 zcmbrncUV-{^EkXTrAV*J(u)d86C)^*-VqTI8;F8fq6sVt0#PY~G+Bx?4YAN-MKR_h zqKGu9QL#ic5veMm5fF?bu)mpecklXr|9$WCB#&qBxpStSIdf*_9M%Bo?mlplEdM|RCC;qS#J(pwko8;Vf%Vc%~$x5 z!+9S0cs$RktGt|Qa^C#6>hb0^>F9@LgE5RuVHTIdQ}{++U*mq?OMq%>(@&TDODk zvXyK!=X<|0uMXav{nt}Y@IPdLnlOsk*jxDF3h#mtqP!5?FauMaEMZLW3L&4nJ7 zFp?)xvne)E2VggA{kC2-#9zUS2Hy_=a{Y$^>#SV_Xh222vuH?ogNuY_ru_Mg4Wc2k z?&m~88?96}dW$#us6XCZr}d<2(mM9|C({2!%;RyTpG2>h&q&)i{zrZank4aAWvyZ#};IT^U_R^?l zf+NmC_(n-NhkUe79X?tW5_i0#YLfGUz;F5pT^lzzFOIeGEV%+&SLkx6+Ah-lf1OnV zex}k+c{z5bOIL+7E-e20Dd!pCtF;mGS}#ZPW%*1EMLDa(C_|=etnTPfjG0|F0as+Y z>bB+l#CXUl8`=)Z)v z`C}8}J(oL824{t?Qok_9NG}BXQX`+fAL0yC0)8H#u60|49rBX!FdDSQ%aNd+m;0ok z6-GT*cU~1a|I9Gq#9Yf}N-;L{C1YTFT&d>LG7xF+>T3jNjS87Ax!RJSVtqdMGznd2pd5GUhYOi2 z_^GRP$x8Gr?(i6zwTn5Vv_+}`*lK>fN?;R%q1RSg$~ zbtk|NdRw7`yn~FgOt+{Fd@qNPBHyw?k?b2{x()dBHq4-oO1VvBc7nKZ=i9rmvE5ZucY!rosI z*yoS6EE8Z|@M+hV49QvSwW3I5U;&J4alh`k5Wq>@21wpA(XvF0O!1=uVZ%04?`vs$79@3V&Zf*`eI)M)}`>+4E~Of(?(kroSfeghNvzYq9-T}Wc7Gcx=;;Npvi zcbgHW-Y2w!lJ4U_XMeLJ{WYGd+pW)B(oHmxCCt*-`9((;JEov3bsuxzv4G3a^%MXfxng_2iSd{qS7!-csdO8K3V2Zc|fP?9-#Bx36-z}RAJIOdisc(b5tY> zrX2?pPk#bn+^mJcE#@;*oQ_sLdFos^)AyzgLnx4BO zEry`$0_-Y~9HexZjW)NpUsd`c<84-7C4$Wbuv|mb0iWK+PmkXp2poyu490wH)tymB zx_xeQkA&d`?EVXMKY1qpk-u`T(0o`*Bn-dY6Ee3IW(f$e8FbrBtLL~*tOwn`*4?{6 zT0)&}TUdboRya2Z$n8SpC`&Xh-m~~RAm?>>K}DiMH4G=hM(n28y;t|yEOG*T1f2mN z$sKZA`&7;V*>O5R_EBpM=4|9%cQb&hy~`4>g~dY2dn|qSF6Pdjmb1`NOAk6mY_kh{ z0yJhKjfV3#Y$WWEL3S{$&h(xu&csDV6u%3!ZLg4OK%iO#Xge!K6PFpgO!&0VVvF29 zltD7$uD0QvC32ZGBqfhA@hj38P>p2MjoMb#rn65`I9UL~Qh}MoSdRSA1jJsCW!Dol zOYz;>*=5_AC@6EpL}>o0An0e94xHHJ^5xcTU`wtOaAB2bobYi?!HvmuS8+dWH;WRV zvq%LK#Ud3REJ6@vA&u;_Ci&Rc0>S>Cp6vbyfKo(;UbgetcMdV_AgXbV<#!Taq3l=9 zZ)!|IrWFx6`CSi{LTpX~HZK%YVx_{Mbuk&G)ZdWe)gUle97mUh(R1o|roLVvjgZzk zpww2gm~E-QAz)hsJg-Udh!2P~6*+K^^^aH&>Yp~UTTQko3RxM$b}7ah`6%wg=qHO=Fz_pA*4I&KFiAM5wFGavr(`FWxebPoLpeNaw-%V zl3rGt`3Ym@}<60=ZHS@MEQ`}<|< zHs5BMse_-^2hLdHDw{t4>6rJFBYVZ)d4KhQpG}`Y_-C+8iM~Gkml9`QnpfxAx$%Fg zA9rf&+m`E(KgGsyVqS8DW;ip~dhh3TrYqIer^?0O6UrQMU+Oso-d46}MXrCA`>^nork2&El8$4ud3ysViJonGVoUl2{3PNGq#riVTbz)T zyS@F`>|K!~%&6NqiksOEAoNtRgG@$;UF3%6P3{7I~udy zfa8^EL^DQ>psihC-u0A1n^O^0oH%kdJM7QM*gZ~bU9(abmI1Y67c)m`$JsO{K4Ii) zSf#l-xTz18ZBmyu)V_CUJr_X!s#}P->#~1^DtAb%(>#;)Sk^zpwC~02DS6$5Bt`pA z2Z_Uu!vyxmOLwP!Z?e!k=!ff8>b`}e|M?TdIGoehVZ-*k@%e!X&L0G_@r4K+HoLBN zc6Y|Caojn{D!!uq`>Yqpy&?V1+30vj3b9g|w#dK6rr9#hDNhi~i>3z;^03_bDLt>L zzSU6w4p&|rox_OH*bz2h6lG#p@-UE~D9!17g1@$u7_XAUL+#*=RFA_r`t{r zmoUILcNFC#QyYTR)FqDasL^OqMN!YuI*9uV11*`~np=XR-7`(-Z)&~>i}h?9Q1sLw zTSe5S$lAeMxPyhK=4;}%58E8o!ZfPgc6C;4bAQJ{6&R2Y)3~enachd}*~TaMiSP*h ziFLN}(dwk!sLuz-mrYqCFZ62P2;uH*xxv4SGjmqvFMZ}Ja15AcAP}%JcF&lXp4zVd zW55-I)MFopsBt;vVU&w9+=f}X2*ECi>#&dp_Hq;6*f9MQ72dqtRWt6 z-`z$8m#6e9Oh5lZ=2UO|lHaJUwdL(~a}+%VMpTZ@`HJijHykR24K#VIegvwfCCD2BO}jy03v; z>Ukb{$s@!y{@1nZl|SaMd5%8@eBd1HZOH2sTUs3~5#b3&`A>>7Z&&7(B{{5oS7>n1 z)PU78o$u}BU2T2ZqCzjCcwqEuRRTcag3TYVyZYbNqT2flC(|Z5%Zr9DZ`lik8tNq^ zB?xWy6sYp?@6WWCvZT~j&x+5Y^G)`m(qb%8mbi_!^n8DBGXJea`OJ?d!i|>!<~Pp7 zYa7628@gp$fBV5v3?DfsA!X zWc$jqWjF3>JpfU(^^_2@V|Hk`KejItjq+5S;W&@P?0AK`yFBX+&BLtQo>C#B!u!M- z`bM{6!yDYhdbHw1+ZQro=Gc(V{c_E;Y;S!=&ZIhO{`S`RZ%$BfblP%RpuVOskl<u#SRXkLl|08CcnizR|gE@m1PSJrZ^Aoajt!@ZWrmYRWE)m<5_u}jZGgrNk(06 zS4y|S8QHphdqvIBu$!cG^AWVd4>a{otc^X|&eCQmS|yBRChI`+UXz7kyUr2(o~64_ z{NWB|=ToCHb@tlq?VbXg?sxWC;+JOve_0zlD`VIyUrJibO1J3wYlSl`f#oMudbJXg z9L)bN6f{4xVyimN{lwq@TsYbGi4!T>qt$V-aBXZvzixu6CF+OUl|ouSc78ko{#1Xq zxYyC|FR;AVTUsgWXNzklRLhIXUg21c6kq?lXFY;p)Hi~?xxmpC4KLb zs;ZxKYs{$N)O?Q>pq|3>yi0&f8T~O+f2c0bNKzDpUOZcT`vn@~+(L!(Z&$NF#n`P1 zVPyhXjh1}VO_Z8Q6#eR@b=%R=z*`_S9m94>g=l|H9QYMRr=I*FM7{CEd__&g!=FbP ziy6Pe&eHH>iJ-ZH1zYMvJM0>+v>z9LXaT#Qt-himXLp|RiB+;Gd^8feo!6woFgxg` zO$M_G!M+8sXIOC=#k#ZS?to20^lMjYmJi!JPc(K5AOB{0QhPnfbDj5^PL$d0JF;uh#5B!Qvd_rE`R}RYuBi2mDr?j zgy>#cl()dVrfYQ>r;hNNt|L3Tel#2p;MGSa4Tjk6ZrP7yI}S}yYd z2(D`df+csb-Gwc248nDGpPuP#UW1DJ?15b z1H`oe%IWt08*7=+b+HM8RI}e^gz~(*9R~mI@8n7!y5*}?X88o6bfaVhw)a(OizBE(jq0Pt=i#05cJhsGcIr`_Rn}S+}ahf<25{ zsD)(fr}w4Wq8^cV>3x;*b@jX!24IeZlG1{R_i^G_!m3aNk+43GeI03YFK*tFA_AFF{0t7c^Vu89(UidDWMZ}VfjFHui z%j)|RacD%Yk(dh-tVjr!NY*BT##WAo#$gK_n1P&;Pcb8cjs@tO-etY9z>g3={T0`k z^HX795yX$gT$%yA8?tFff2HQ1$U)MI_(!Ife)Yd|qWju;Z@Lh$q9>nPosrtt1q0@1(;ca&6j0eY>*mK?EK8*8OKbOKXAX=Z~!+7RV+$v;# z3l}cgW70sD^QnFJP7&C>_F!==!=~=0mkw(6WEdhUs+`jaWL<8k%%*V}yMsYzBDKC_LBOf4at- zvggkK4FGfWqqG#y%=y`;ucTb3s%wZLc0LyEwrgy%&-&Tn6WkoZ3F6muN9h4?spokl zu0~(FbA85l_HioY>NbBbmnH7Cm-0HUnpNvb_`+b^{pRl|}xID;$*Pda-1 zVc#1pX9H}kOw70EHK=e{D=2WefQ!g3MsGpl>7Xc#g))HuvFeuWHr_kG?!LFF9@`P> zYlJ%J#}?jDiQI`0fTmRVovXc>u#U2%@ddv!g^Hde<5MjV&}!|TY)t+W(u z&VinmLeP`D3Lw~W1j`MAwZO1`h!V3++&WqcP|7m|lm?RIHnV}w%VCJ0O=0iVV&U&^ z;|~Ynhjd$oy;d9S<8p7qRM3FIFNbCVP-<)p;tGc|h|718Dj(>r@}YQ%53vyDX$GL$`~wp&JzeL*SwS zoJ7F10N;6t?~}NNbDepwz6LS#9kF^_-bw}bc?uDZB z7evu0FD^s|hKnHsvqXttu-lKasTK%24nXH98=XXA7s~(!`rgc@aMU&%;Lk~Ow&ASv zFph*UVch|{NMxNR06??979cyKhG3`*MQ9E$E+0{A;{99FdR|i^BGZ9-W6(6Z9hGJm z3c5+GEVLOK|DYIFnIPBlRDqby>a&^u_Rhw`3cp@Vd>lG;-O8 zn3ZP1BvG|Po%{O{z)DLsL0CWT24QR%ZA5YY&m1Tz6AfeP@ERnV1r6WhPB4!xW?WbZ z>Lm1|f>~F=z$7Gq2b+0(l-Qy7 zVJ+;x3r3EEt>h*ewQ$2>w|oj6*(=klpAbKG$!FJa-v0Kx0NAu{mDPd~xSQ6Zm1kk% z&5`vM_*^`C5VmxsHhJF~NCSe@vyK0sRl#uMBJYmVg&eY+Z_g6%-V`zQFxH6;E0Kv3 z_T{?PZ!65d*A2X?xvqy3)4;hkg5iX4NeM8ZlzBWoJl_X$Au=`wxP4#gTXrLG9~w_? zuxcR*RxL1epzUefS6mkZf0F@Ee1?OTZo8p_fz1!f=S#>U276D(W|i~h<*F^*s6@&4 zxcS@`giL{CJAv3RPlsl*WJaz`m-yXP28+Q%|cE@FhWdj?8*2e(=7Hi1pMl zXvCbyHi-2sR{yLebyq|3ErbypA$H7@TprRv1 z)kJt^9d8;}vP{Fzsbsa-;YF*Y#r{@eCBoRojfBa`j{oo$;hTpqC5$M^FOVgXJlwOH zEs#AB{?cWJ(yOLESoL!yH_?;_d?331({5T$)<2VVw&;CC%anR#_sF`Ge5xJ#t7LHOl zi?8P-8=}HKA_}|L6?soRs$J&Q>o)lU?GUfVM|xV`0E!4&QY}af4E&|3)^GHr6{CXd z63!$STQA|DzSbEo=*1Ea9e44?WgAX(g?WmC7`ZhPro@G*G7_gHyZSx`8w68m*4Ff( z%5HRZU}Dh%dYUpW+P&c3alJdk=ekHe^+cn0YP462ueTovJ{Fq+pPK}qs^?BjEO|I- zQ>gtLIBP|(#aVZ)A)Zox(~cHOX*9Z4^VYLzKK%Y4DCp%mvwKPWtZeY|{b-n^l!D|t zk}Z9s)0M8b53YZT!7Z;%b+iyvpd4GY@OnXOP_#t; zICifh{R7mg*|sUVnm5}tT7WMS6}S1LHf>sg*7U^fXcBb=&#%bgP8ZGElxJ*kg*%cC zRzpCDQ=>pkt6B3i$emITAN$Wbc(5Jf+w--5M5YG}T?ctow>AXv0+ZCCt~BKxtH(DvM8tMlSKcBYVf`vXZhb z81z(0d8|5pVlrkMjCv0KO-8-29@Y!N(Z_Ul*P3-D2SPqFhogl$!?WB{70L<%Q-AI4 zsxqH=JqGM(E3^8B9X)9M`(@7GK-+hq2Tew*WfpcIPfZ6*mCFbJK~(-AzpTFB2Z5mm z$2AkPx-;&5#Gp%f+J$A4w>TPxk2mki`HM!RW|(kqq1Gdsd;T%FExD@sh^n6v?q?&D zDs9dIlep{2Yg{0;wGt?;7$fsC(iS^VPueujOF@aVpcGRa2%Wa+3iHuT%fN7#rs()A z%r9W*iD45GYw;y$;dYFG1PhE~y%1Dg?ZR55HA-dtfj@@({ImIf;^3bHg#9jEeD+5W z_P>n;_CriK$|FQCb3+Ine(al0Krc2088T}eIXI;Cjly7Pqc?dAQ46eL+6E$6i7Lw7 zpA?V5=ZlYd4@u(cf|g0cdk89%_7HkC{zGgGR~{UMpQy)$U~}p7Bh~5B&oQv}_$u5P zpEZT7P`XC5!a5rlepjt4%VG^**KW_Aw-&Zm&uv@*Z|Ye#iK2HqGU1Dh%QX^<9~B&? zhZ!So2ZK#b6lA)#M=UV?2?rJ$IoXSR&K)H7Z-o3r!M-(v_Isu7pd_v?0|N6bfoU!M ze#kUxb-88@7QOZAH&EhFL3A$H`DPcL)M)g`fSE}_H2xq_-&OAxt6tp^kjk0=WL#vgX#R@MDkn> zg2c%vFbjh3x%7s;AX7_pqZ2p|T+<$F|x{gtX}q`-|NI#}N=pIkD{ zfi)oTSZW8)u)S*B8NdqJX4m-E?=QdkA||LK=I2ys>VkhphgQ=75aO!49x6pqfF z$FGC*5^f|k|Fj=7CAb0vn=Iv{Qk_xSt1ee;vUEAgNR7lc*xMESxX|CF}eNxvWbXSg>5S096wzR3+z<+3{oCy2q3NnQf1pdOtlU}c_WEhSqh z9%O>%%87r+=hY>3LkO3V3}5+X<7N2<-Ad=g5@T3*LK zN%J${5xUIYMLYI0*0&!C)`Kd+!OJqub3wM;h{y$WhwUJG&C}czmcp^}5kObuAo9R2 z_8cD?nK$B!MNZB!uQ>s*eacm~EheeDz&IyQSUbwIz9EH)q&Vj9r$~X-!d-umuQM4R z(S=(V@hF}49a^oxfLjFZ=4-ms_wY-5*3SLwXM=MjZ~<2mD(G^pK7q#T^siJRRR0)z zreqq8flTZ-BEcE%YtsC-TndbnkU3TkP>ec%k)GlQrlB}^VZkH5A@aE178Gr$yLOUZ ztsp-Qj-X?(+bf<@JSd9*LzsU23JquH+7-VuyWAY_+!5$ETf!WZN*P>yvw!$+Qeqs0 zN-#VfV4&71NBk9wv5Cvb>W&W2*g-9=*HZ_2MQ zHw#=J(kr0qL}o!(oqasQr|Mhtdvkk7GSAq=-w|lAtx-|dzdN3})j9r-;*9&G@yCEv zvV^d8-_q-FDcK2#>Ds<}Yn=8e23V9;>qN8YLvLGK&*g;9x2k7UMuU$12|7ELo=%hV z%DNl^lQMO-uNb;xp^x;!QRmD;`3Qc6-N0Yz7w8WHKERw97QBFBAx}&-wU73CN)tt; z&g7CV-09G0;S492-_XrIA9C0F84P7ozxEYl%9L>k@O<91_8UB5B{JzPrX{%y_s*3g z9W>NmGI9qOANi~U>f!z~L3#}(+0mZ`J)~3i7Y%KO9q}X1 z&QC1GI(xy>=O$-`zMm&5Z2oVhIOG7=`y3K7w~J&;#^Dyv(bk7AN+0x@ZVTe5+!^~( z8gxpG{4(IxLCll&OvXiURSt_hTxMhc+Cnx~9J$)8VDpNRp26}haGtdu(s69Y1>i5~UJHCpoISK?{xc$x0KO-{d$S)N0OQZb~ zM>1{hrQM;DfHEkowa{<`w_%=K~%>wn-yVl zb?M9zX4kdai+2Q4Wv%aZn$O!T6_V1s?iC~K#9XN;)x|mW_JO01mc?4URqnGZZUMV~ zE$-MHZ&0 zGj-fWL$Z|1LwIG^+hIf9S~ns2(5u^G`R(JdL@tnc2KCVp-aWDiOykB%U>j9-4J6nm zf|=lAmm7uiph|xOt)Irz_9==mCIq50}sVqMFA6-(0tIcLe5b3}xjciAckMhPgN(HyWG;*m;YANT6`{ zbH~0+Uh~V@hCChUoaTTZ$67RQdn2led=iDeI=?RHw3vl`U68Pz2eX@Mk!f;>s!3>} zoe6w-@fZ%n{(Yn$zsr>cB(We;m)G$1yn2m^%$bE>NvV6Wo~&^Jg8$M)`l(njuSsPD zky$bT<{s;|?rQ}468R{#=m`_~as!6Del{*E2tPs|Vr_OI&>t=VC?CmP#X$%I8xW-RnUeE`UMf*p(>mI1H8&xg|DzK`&Ym+cr7y8<~z1K zs^fD708)0muh$_P2H1;C@2lL^!$cse2m}hVoe_p4!btOKFa_qc9R(JUs4#Z>h}|4g z`zk%LUljHu_1N?~3-$^vE4sxN?XW`9o+D|i^p6tJTtm$m-rB?h5We3bh|e7sM!GPT z;V&p%3=N|Oc_0V+ENT^`aID?faak>qN*VHZvGfs?3iSd?uu&?(L@RCOR4ls?hqZ{U z%Nq;z1q2?^$jaCSL@FNGA~o1ZCTwmawbIUtO7lqE?B zx`5ctbU^H+?h&W3o-;7}1#AjEzGF{kgE zrXxViMny{R`>fMw%WF{?L0Bq#XBWfD$4fqqqM5dvRSahOPYx8%%{4)b!7E2X=c%y% z61{*qk7bHNwYteLo^a{Iop9aU_DQlqt6V971v;b!)@(b;7f#&ee|umWs_#l`&jK)R zT}%)<3aPQ1UzJNu3LT^Nlc)cA0~JtWX3c}Dn)kvD^aZyS_h1>7N=xEwgq#@d&=Jbi+fOmL*TD* zKX^55=@heL2CHZ{W*|SQfMP~R(1L^;bH1OPMVGO{_XB>|h5G(g zV}8@Puk5$qoJD6?uMhnrZnUN7`VwPE3ErmDn+3{sn}>77VfpiNPCD=!Hj%rpK#q$g zVLbaI*!An{4Y{SWeN0qDu!d*h<%)YnVcUU&dgE>)yFSJOsZ0F#pK^uFVAJpE4Jw^8KDswAYvq)>xFb} zLcx=@jMu^=M(r@JT{oadmNI-Ji1d@n769G?z{yG$grfWp2qc9q$O;dH$LgrFL>=1- zLtQtNt3~WY%7CcN3EBj^ZlJbRLitY?g0_H9NOlvo?QRrcx0|_sjyJyYCeWage;aag z0tRwU*mVkZpagW_M>Es@0syU82ZV=T?L*!fCcrTD8TVLP$c1D?G`BDE8e&5lnBGLr z$pGiB|FV^huzV0!lnN8M;|1ei4>Yy74;(PSIYR38cfjzuIDE-IrgRPh3r5IIA`V$c zkT%cg2Q4Klur+UBTE-iYXnCQ5Fxk_oFTY!;RF%<03F$dGUv1~sIOh`3KbZ0 zR)wTwZ-HV6X)iY_Ru}mL+B&ttT)G~_VfTF4V&2uR_J%O|vzeW~Bxh!7TjWCr%yyCe zy@Xt8k@fE{IHn49oBjINVG8jcy80?D7ojWHFuI_lzrTMSKdGpTVNkdB1#|{*Vx=y8 zlzD~!Q9P9LLiPB9l7)1qn}@;C_)BrOn~2MM?c5v&#Jv}S$#wAkCl_izIGn^MWXrC0 zpcIp@U#&NiU1%B0mrh;YBKwChmaoC&@d>DN`Ozl_Xr>kfjn!DIIL~VhAI<$5auE0t zQ!It^wS8Y;6*bc;;TN8?eS~GsK-v3zxfG9ahM&;NKTWHqv#YPB(bu@yX{g9ePhvp4 z$kdxZNFj+Ioi|0CIGha&{@oTV5mXxX1y)#*Jg-u^0MbV~1tjly&KpW@v?s(&zSq?j zTXzA%`Y$+@B)BUgcYzkq&PiAh8x`zItE&FxF8Y1bs2-{h7bt4}k>=w~90Rf@sh;6~ zM&kXA>Suh$^LsfEekpcD8F6>H>KSLjHPW#=4^`ojq@Gwt)@>a|mn*+V=Vdl&X^e9E z2|g7{XKBFsGvK@oY&ys1UeGwAqY-LAGX`QrZnl^C8sI>pho!OppsUo9_h5nqi51?t zImVRDXxMArekzni@x#=-df=!%miSO16X!R`q4xSxA53pPAbI#)W+jT7dLE&gnmq#T z3g`Y3eHtd9p6J3-!Ee&Fomr?o+||^BsL!IT^#+qmzH(F`E3n4QO$4(7Ago3wCmPW! zR222+xqR{M#SlTt24JW27}q^)zRi9c?XK+)lM-fl-rg#2tXjVy*03-P96K}j1ilu% z#S~wrO#;qGj&=EJ2u;bRiK3o!V(LhEery62mwHr*N@*U-Ays2X!3~yq<0is8kzqJq zXh_eg+-Rs^5R-R<`UHv(H3_T1aiD_K2EZvA2w02l=xXGtT*hY*ZSF%6WjYOcTKPRO z+eVctcnV}YoB~l;(O9jRT$6Tl3ib!u2{mbaAB}|OgjDf!@>oA3IdMf*<>H!Q4o)by z#G7d(&jA$xyc9kZ#*688Vh>8mo4hcx87BWOkg+9FO2k)w zYqU$rGXG0x3{yxv)gY*<^C>klT)u#%ERtnu=njMLsB~D{{~awF+8{6;<5wE$=)Q0hCe)d184ldhRv^qe6%ij+xX{uaF zY|n55MKz1W_GG!O7Lu?QtR13N;lMPiLTTJSib_~?WCT!;xI3!8FK7`5R4r(E)ljEx zF5SJUg2!z@MlFhb#dDs*z%R=Xl+_=~UtG4qW()HW+k5@ze1_eiR{Uc=4@3^)jzA%_ zY6B*rj&n$uQPxu#64lhWxhkIYxYjFBPcAluR;gf4G8lGb{oVUbyRWd|NK*sjNE$id zVr%m+InuO}OpiZ^Tr!#-IQ0qIcm*5Q--lslHkRYUQ*Ixc_X(}G#Ovz zU4N3q7+}%e4}s)_80;Wg`d$Lol#o{sVG!TI;wpasepEy% zYIuyS$*Y`^Z9StIeax_rH!So4E3tII@_oqs4Dp-fNzS-B0_R*cYp@tSG(3sv9zY*d ztMNuNFRrwC5-%CS`$FnrgSm8JOcJT6s1Xuce+5~Y2YAp~8>XAe*vPH!O(V{lRUOymSCmL)f zi6Oy7p>Uh#q4AwVXmcF%Joxl?kY(%INDh4#ao==;;lG#7<4Yb4BsGlt$_SM5d13Y* zZQZp_cMaafAh1FQ9?PXk4mgi1^hkUzB^)DE>o{Sy-v8~%bN z&fyNvHVf%LnmvWl>-J)uLu4jC9{-Dx0t4{;w^EX|)@($ysK?J5wmrkkuMnP*qK8}v z321D^9^AFMl(;OC^9GA+_Hs~+J=uIQTIV-;!95k3eiXZK?US@ac2ZAn6inTPz2B(B z4cyj#k<&lKyRb#}hiD1o806{;@=X&tHk6hRiT^sx)K9W7-;=5ySLlgSn|{9ZZw(`hexgfXjS_@ z;d;KI^!OcI&zs(p8Fq93BRVPn9pT4TxB0;Mk7V1@knEy?t_YIm>f}Pt2Uu)d>|J0w z;bKdM3XU05_%Bbazw`k=9c{aXKjA(>cCszw7*}Mc4?-BKawQO)R(folIPF+Wqvz{1 z`S!yko*px-U?XB+wqwTNfAZAgqkE24DvcK0mojWi@pSCfOC|RdASZG0D*YZ3Z)Yl# zgrn?AD2)8HMl)l@vQ~CSbe_CuH!l zVK!f@qKSW1^^QN${A1)4|I0gco=Z4b)rsF1nyx`Vu9T)xN|FA&7Fuz~yZp)Yij*7_b-PWzeejxtfw10jcXC7*ZE)P= zHxu(2tg>0#*1n^?CKlzWL_Sxz#T)!qxj&Mt(5bLu@>cnD031(ubTh3G!aa$et7!$T zz3Vrv0KcbIQ64_!E;oLE1nr8J@b=&j(DK9+GH#ufX>>8h=(C@o^rpf8UHSb1Baw&s{V=0@KDg392vTCt$lfl!G++tgp%0B|dF!^LQp@4aaik zx2v7U1C$Vj^BD0NGeIr?P=;5&(zes)v0(#CAYGF#(kmA z`7RBe!_Vbbz}8cE;qyjPAhG5dSq6m>6vZjd?_V$sJf?T6v*UC(hJlp?Jpj4w4r6Ve ztZQVy_0yRwAWOTuMme&1y`DU`^#w;aG*xE8bmPUqAUip ztI&>?*89-XTRnsmU&M2FMR83U$M}B`xUS4qT7gGQu|%kIs~nAN+VnNZ{YxJ2^jEu3 zerJyO3(o9wXzdS7;|5nvpRWL&xcGUSq|jQ}cz92Anu$l?fr%K)!s~?i50ibnqFq!> zQlRW9MwqjcR1Bx8VIPV)F_6NIHdNZe{~_YkBB>Bollwb{{yVp=Cy_FhZoLTZVO7r( zOlgnGt+3Hac+)A945iX_4^wFrB*By{HvRSovO<6R1E}rgC*S-qaY!4&{0js<-!UXl z^f@pL93YL*Ay(KaFi)fJl1av#EJ$DP7n~mv``s;zF%KMn+uankGXuxIKShdF7KHMV zuOMdaEWAhe3judazn8(WQjyp5J%zg={-`?DU0r= zsl_fY-}h%QW}E}(-2TQ?keq>icO!ddY{uFWqOP(}0%tF%SAS}89t@ScQ9E)2787_L zjANZ zfwOlp#+zA%_;L=uD(;;B#`82e&!u!Iga#4sP%Ghj0;` z9)gP(?G+Sc3Lq+H@C)v3XxVU4T)ehkM68pNA9G+^tv!q&o(dz(V=@P1?Zlrn!{%~1 z0-(-c0ik(pcufxBw!piCSuk~kWPXF&&@1iGts`-L z3h{KS&q89g6hdVH`yLv3xUi9^IE0cbe`!$@-Qu3YB_ASCZi)gpq$i9VDzG$??Z@6) zgdunU3lXrxAy-`W+NTS=akK^A{J3jte*t`^C<32dKPdEo1;hI);P6`q()%7FOA>T| zCDbuo1lfTgskA%1bikKoQXVZ9*mY79T6hXe9|oAf0T!1`S$YB=hmN46WCSgxOhF9p zi;#=t92SDCAXwscXh)C)CB!9YL3=Q=LIrUl$qKrKN3oV8lsQBzah!#(wt^0y&uF0MNrw7pcqzGC&BrlGFtX z#v8@-UDkqqgywYUNRHwHx`s;;-D^5+tRh5e45_42?}&a9^gNl>7jHfrDO1O;O_Ewf zE#(Q426fEi0N(=zkNFEE+6-qjbw~@@=!$HV8K_+3l7&5Sg56_HW%X);j`-Vyj!?OG zERl`7O54KIg;+xFrkK6}%dO3RvWA zj03=#Ag7=^VUgdOWeobXum5wLDr|~6$N8j}MuT&n^$3=AE-#$N2)|rw_)7K#_7^N6 zR2c4-)Zqn2FBNs^k9PX5AU;Zb0~{YaK8n@HUrI{2X(e#i1H#^8{e_d;1;XJNP?itz zSfiTt-6sA|SN>yxmmuS!w+_67bnHPYeHZ2W@U{uB9N&{#6TMuYj?)|8bIKuoQt0i< zJz;H>Hn@1ltn+voNheSP{#}j43-B3p?`p0iK)>T>s8aaf zb*NtO)0@e4QV$ZD5hdc?%H>~dO7zoUk*mG!LofcYH_x#zACFcSe&r3rr{ECGN; z9lQ?phf90!23%mgS4nH_fOk*v9XWY@7g~S@d7~zm2g#$o;VgvL}?~8P8?}5JJDS!bvf{oJ+lu$iq#(;y_$J zcLOfuA`ZGm$`ufbHcE&b#>UUG@Uy}GN{cLPeZm$CZz06df5y?V@(vol5!g^Vn~dxv z4X-Vgy9nWxy)YOlS*U`csTSVEF@%hcM+rBv<|i~Er6NeFj-B~x1iuU+;+bs3Y7JtA zrFRJ42j7eov;17JpCPu+F`JH~J_OSVXy)m8m{6XY*awt1KUDTPXZI5+olQ? zCd$(knNOX}MtC|1kJR4X1Q6GdRrgo|Yx+_3gHcfinvyiw$d04Pj#av?Oyr^pj)m0= z|DsaF0{oNjn!^egAi@j;nK@ulu8S;qiy+Bu9H>7LaVq{ZehP2sy_7y$|OFT{6>ws5e zueRcOY3fY-6zBD!UOMsJHH$t1t{QiXj=2nCnBTF6O_%=;e@`5?FN@ zA0BH}V=(irZ}|x4Mj0mD*uTF-{H{j&8*o1im*#lWN6l{&BxPz1Xe9GzFcOg{;|o@J z31>C9VuD+94Py5^AGH^nX9;UkxKe6rq1BHRp3 zT(3AGWU1Gh#YfB^b1#hxvgw?(`ve?Ud+x2fj>NX%Ws0mZjhFKjN#qEN)1*>)w~L8( z7w~v4s$`_C*1FgyM&7Psu?x!zHbLpQ9^g9c#LK4F1t{S@vf2ldsiOz?Kw>?b53K_^ zyG^PZIqN)S{e=Yo??1#fi+Y|%fiEza?8aZj?&3{(u38J$-K{}WC2V_Murg20+Wbt` zk%n=l;F40hl%SOqFS2YmQp-62d~hE)Z^xJ^4kDI#OpBuY@wl(}BA$ByQLayz zH;A+=Ar&kl2hm@>li)(s_c#Z`q@|)+SmnV{9wEW(@3r^>KHmkCC;V3#z>GRzjyKI8 zP9Y+s;#WPSXkFay7+)<<5VaHvEFo$M@y?QcMj!}o2DE+zwR}gRJ4w&yH^MnQeW0|8 zm!GeRU!PiUA731HUT~y;FS8>8>`1O*KIjm2aM}|BgL~8Ydf@s!XiY+}!v*lbIRO=9 z#_hM^lR%w(Rfq*_Jf>?F!6ae0Tp&p$2!zQ=@%2ZCitE1q=G)>wf_|&KF%qpy^37G} zlwD~uip%^rN`U<|^*Y`UQ2BbEE2?tF zjR=r|*w21C=P|GL{2|s9+1^VpAqj=eq(DG#zs?{=jpR^slzVhf_^(oD_z~2Y+c6c`}bromoKf>JL0EHySPHvai28S;w;BIh&>y; zD7d>DhTF|SF;OHILE4ntmG492=+b;;Sf-OBGKeraKebqBg#dFsM6{6?@@_TBQ?eF-U1S)+h)bbnH-F)40VJrsAu91%rJ?? zPA-CY#iV@l+I}App6IbmVa?h@y`yBWHOHRIDh)9HriV8|3~AH$Okpf)K`r&(&WC@9 zgpM`l>SCU><{eJ`I6-=HwrDRf*@D*oWF8nN?*Z& z&>lOAQKY!(e=;sbP#F>eNpr|*0Z&0H*mKJ%+_T#VWMjVesc^4?<(_E66@~oqya}D^ zo`h=af63@AJqUt&P86LU2iP_kwR$|#JH;uYST*YL<+j} zMpNJe>Pedpgvtk5G3#tbgg^X2jzqA@;$ZVyXN3X&Jy<(7>Vm4f2l~U`n2`PxCz+^? zI5o+pF$$V&OSK+6kzWXRORAJ)3geM;Ol!+&CQ=iQx~6p2!f&(_^A*C^kZ=gFTElHFIsOP|F ziUJ(KQ&e*$ixtFlUFXa24aVJ-u$ZUitmMnS72*WR!n7=`ek%Pz7+ONV|2{V zC`%agZdlL&kR3)eX~8|(gUd_kh$b;+70=i(7X(ImEmAJ&-&aLU+ZJNlM4ndCX(+!v z$fMkHmfCbn#|(z@jgxP}Tdp;#eQ_Cl-o%1oVy-tB{HqnGV*2dnRHRL>mVoB#<=wAN zcDb2_(pzV1n-##fi^ePbc$d@u0QF2kr-pfvhY!8v29cODJ_;Vhx^|j0hL7)@t#LF)uTiW*Qws4+2pMAx8I$MQjJQEl20d4Kp z#!7NK3Z7rbQi*Mial>yPP~nB_p1a|9;T*VOP5>XYa@k9muB}sH`QXB@IBQ*O%JEhU{JSccpx@$$T%?|8VyleH{ip-exLP_$tuH6pfPY9hWs48D zMY4NdX{|I_J3pW|m%G8I94b6Wa$owMA07U`Vsi^_fQd5gUrv+?!ake*kg?ne88S}n zvPJ=)+1}rSd<-@80a-A^czhTZ^W|3%WH?}pmc51;{%v{5z^ExsTVV8XTnmlQfl*M1 zKp~HElHDh=4Br|?N16r>L3ZuSx(4WvD=!)~M+^i!atu?WE9oFav%hg4{Y@^Z;A#~V zp+!d>ELIF(P|2Rjo z4Wk?K3JbAP6ih3__vP&Ic1VPe+GWThrDN_|28WsF+%ZaVBN*-8_RnoYW|5dn84QnKz zE=EcIK58#li!$C~0x>`fRh~RGX&HFepl>o^)#+|b+iuUN8qHW#zUy5WVLMgrF!7F0 zQ*IPixkYpiEbqR8n^lkB0eif-8}4k+I$L?eWUz<2tRMlje&9hZF$q{YbICtUZ3M23 zn%7v5y7eE}d+-q!z6geJb#G)Pxz$N}eo~U~TJ>_FQ2Sjz6Hk`ng$nnq_%ui0(ek#V zjLoMs%vM2Y}upl>AlG8$}KeVCNVym6oiP+&oxX<2x z-3ZE#ED8%v+b!(U-~l@rW5I0!0fS zKLo{B;FU!NfJfIjPZD{j~nJ+09EgJ;gO)J1G5;Ka}*12m~gooFj&pjlgd4 zvak_C=4MvvvgIE1f!^aSHr? z=<_wkyPcr!MDRfkF#LpNW{li#s7yLtzuD)kx4w;Y$!p0!?hQgL{^xT!=SLdPH7uPm z4tAZGx!cOU)H`qv|HFW;{4Eftw{10$KjZ$?5syaO-#4#zbjmXGSA&fXRc4^v4<#{* z3X{xWFMY&1`4|zggfdI|IaV5V|5q&<`+oz!wz0^J&edLff~f!!1ga*{9mXFeDRNG>FKx(lLjKm(!qj} zetxrs_k4!yVY4I;gVlC{KD`Fk?C-ZI9%Zy`Sgu&j2kH=X4nG2IU=KhlyHfLMzw$)gYqq2sN zXTbkJ;J)T|-_Wu*1Hr$Qff;?IDg2as65Z?*+;3*u;0&{Nzvx2wGXgIKK8j*Kaj9La zLyRj2D{1VS!MO4Zzy=AKPMPcVsS6wmu@b2x6J<#~`=+rv{j^!D@DylETJ|wBygWhr zE*v9)XXgj7sfs>M^!$F%g(1aM=*2a%r>m8Jskax@E%sozs~-3dnBHn|j(!{PKkzNg zZTT~xedQ7e=vdro73No{oLRuv_Fy!OVh(F){BC{zYdp_#4d}qt0~78ogqeCn)=eCv z=Bx9q@!&@C0%~=sT={Z5laW${b2s+sM&QMKG#xNd)hy=J~V>5xy(S=n0+Cf#Y zuX9|ZKY9Ekr+i;zo26;1u!~E-m#no}*$tx}>cVHY>cmMQKk@Gj{F9uCW$7dfpVw zg|x!mrNMA0H@e^AKD-;yPaK7MQUZFD4z*kf0_afCH27=b>L z3%;R2IVdOfN4E;Fzd!URH%tK8hHog`@rH@5@bLfvMsAoe0c^tXQ*y&ZI{-R?E%GO~ z=oV(XW}^<30sBa{3yJbT6iK#=ZgH<0h7T|I!H_36kZc#PIj^0WFB%4&|3c(Rva1%D zw;Tt=Ke>gR`Fs;N^PRa#SUbGwMZC$GT$J5u9Q^9oJ=QK{$Q05*-8c*ol~yA%)TQ(A z(h8|3mrP(+(!#$0xC91>MAj>L&oFlaBg7(5PNH$lB$TOMCklZa%teZ}5}`fGRcuEr z<=z1@GK@=K8!MtN0zM6 zcKEm*Nee@Wcs>^?YX+pqrStG+6p1GH4`V;lN^fOZZA5@w1YqG-aTIsj4IB(=T?i8q zI9@@F-PEz0Vp=T=Suc%zq7LaIJe)Svt@8*X2|m4RrN7eDE`wQLXP@7#5FenI`_!OC2*%*^Jq+IM_HLnGUk&9{%Wt8CBcEiL** zT~2s>9xj=mW3FBQ_+}(?r1Y80-ADIc`WxW3MZ4tLL*Ayw|A|pGENiJ->|<4)oi7l@ zluaP`ivLSHiuVGCJi+ybW1L2+9L)5p&+=9%>V)o&l4Y>Oz2H=M{Zrq1Z2YL-VT{48 zzemGob+B=r49qnZ&YX7>Jb&MS|Hsvv2lSkM|KpV!EtX0fO1nz5*-9uaTBSl5(V{G) z_fTjmhDvGEE*04#W2cyz5G_ipv5)ebK^vkWStbVk&f`9NJ)eEP{ndRv?>+aNbI;w* zIrp4n!+2HtsME`%Xa_&*79jm}@7jft(W@4t9O4=veS1TP)#`2+Q_?pMHyS2B=2m9= z&>!hwYOYhs1LuOzKG9QSr$nz`@8OceO6$LDuNp^3NC_jEozNusEoAQp6J5;Mm8X#1 z9gG+1sSUav;2Bqmo?0QjzkmwuW%6iFb4ksemRsZGacEJKyeNsl!cSBAyPr4HOo(mU z$)C5|PEpz+^}f*(8MXep9e*~HK=t)Z9@t%RojAMp#mhryxBdJp*xPSQz+TM6r&FT$~AOTBNj8?W4nYId;~GKHZcfH%{~zgYNS)dg>Ky$D^h+)g18R zWMHcZjwqE1x{V{0xUfm*38gqf2{~=Uf{4zfr-9N!pb7L;NsY}DV^fE&Gdv$Cs!`px zW7H%6a9Yo$O?FaN+Y$N;q*vkNGir_~`a96iUg`(gR7$89hmAxyOKS@Zrx1n%X>HN# zhgNFFrL_gKS&^l+1TEiZyU#l)Y6!%rzBFkor(a{SfY!~ar>Vj<+gavY_fGv8&P(xGS zDU9AM(3{z2c{Upzp$auwIHWKk*W3%q9PzZoFac5q5kd=oL!8;{S z(03w7B;+ADm*BC4wHS#O2m}J^1HEtzCPF2F!OIB*QbZPr+FVzfleV&&xDa-OxS-t- zx-8}4d(}K*Fp?<$^c55n#<5#j-|9*{&R30+fX)!Un5md?*B2$x%%x7$I0#Bb$<}kkWV?smS4?V&?p+VJ@AD7 ziF-t={KqMNd3*IU8TDIy{^VD71$Kw9tYGy=y>Ft?mJzcTybA2dj&s1-UZfqH&J6ph zGSm1anKR?$#ty&ZcbB;#&Xt=g`31YGUgC6Sy`md{j{g*Dz%9YIaSTX@+21*epqN!|w!fspgjDa<-b*Xlo~9v!}1@6C}-byn` zA(F=2W53S`3?L}nA2UqEDy-rded-t5{e#RV(>6!(9h%_^5;swOID3dX9-biD;!dEj zKb>+erYGb?sPniUJOL}xBC3r@hPBS*ST4Ut8vZJ{*B`VcZnJk}&-?qm*alVQc1Yk( zKPgoTt1i(G&_)1tKcN27_*ZR_+e)|6zdoVA4P#CK6Z{+b!7hrzEq~u6hS&T*7+%Z& z5QLIh03WEW-U*jdh+wRtIpY|jq0HAOS%n?p*1P`xI%fVf_X^~>+7MeSM1SJ{R5cg4m-bLrME1K z$sMn#pBO)XVMnCE@mKjbLIcm)CKmnVv_0%shPlLqc9tT5jsV7oEnbUKFF<&b!!;;| zuDSJVUs6Uirs(~|X*!u){fb30!!3-x+8a;lF_+$3w!3HpZ^tNRCa$KjoxHg!gvCkD z+7-qg_R@hN-hSdw%w$0`?UO>|fUz&~TfgSEe@J(!lN!V0{px2LkHdjaIht8`f|rTH z=PP}f2(4}(1a#|K+`kqrrGXlVwLv~)FCG8pO->(JWRs!y|7OB%GJpCkz2k>oGo)*x z;2L64SIWKWg?Ub5w8d+j1T}M+w%fg~lpk>xQY}Y*^iMEdlGfO9SibOLChj8e_qKh; ztP^-g^!~ii^f3eWxzpwrj>@=eUYS3kMj`tyZ%{c4mIU|q!(W(On{QseM4fpQOa zs&z3FXe*<0@fq(8mxyz)=%>l{HbhmD1Ce?_Ucptbpf*a0ptn<@PH}kxRWX3M{*3FD*qwg(SrLc)U z5=*y_`_SKie3k`0v6hH^jR)mSou%0Ons>nna$m18nq-Und-xw*X*%NtMmirL)BWb* zlYcU?Au#JCCv$U!tPoXAqNzexp?B(7(l_5b-^B}{@B<%9Qe{n#YQ5z6IG z=}~;a&&ado@97E=H>!1n|3gYjA*_@2c0S0LRSl=U5_32DT3|1 zF^(JgaXPVreA}`Xik1K`TA6T2N!~t+RcU47{c8D-dmAp6aaT-8yv+|> z#P><}dilsSdhnq!_H}P0EdJZR%YA*Q_F^8q=|{|X-@2UXu*bt_G&>IH;9Wr^3BmPq z`$^{GzZ)9KX4zfN{64XG4<8@*N*|N2Ve2AAYYdl^_EgyE&QwZD3%8OPGX!)Vvo+Ge zx|bzOV2&W2brf5dd|8jJNEm>(c7WLn|Qnva)f95_bDpSkR1c9NNbRf zv2d2$QO35I8`xrQS7;#;X1{3#JQezO&UuVo#V?x0VUr!GIfJcU(*6SOZc*?d(2kz- z_=Ra#e>AQ*rPWJJ$uob&%}!ldH_SDGh*#mGd?ks*YH$61*P>-iO)j6(Xlo`b4%92{12wMgD@y$L&iC28X$7*E99V9s)i zF}-tXufvP}_*}KfA;?6yC{K--2b+iKnRI1!*j2`i>(oP}sxBXmm0Y%IquA}6&61-S z^pUO5!Ra9<_ivdeu0%XvqV?1Emkg3Q2fH@y7!Pgg4326Q#(LbiapuFo_zqfixIgb% zxXoR$qj@ploJCu<#aUE<_XJ7LFWmj2G>d2ZMlSNfrL5_n-9B&MMH=f;H;#d1X*JGq z6DZ4i=oGp>c%u<9haAZ>B0h3ACYu-J9eM8$IxJic%e*QIn?ZIR$DDPj!v4xR&C;6E zRp&%{H@CYcSi3IzUQqq2zm^@-JA)IhvR#jsm7wD5E4YK>P+I!zt?8$@>_+;D`!o1Z zK%rBN!&(~o$QYNyVy=ot>)d?|w#k^96(WBQiW|~s8qiIy>I33(W`Ca0D}6NRSuaW!V;x*sgy8!pqy;{V{kn-b8L%T zDLay#dKGb=?=vS7cWwEZ&2j_NT#1$o$>P=2)qv%+=^~`GLFHq&<94HO2iH)Puq1^o z%yT1}I!1WiNaWcy;F>jZF8IU#wtu9BNAbGc7GA}<;38NWrM1$O`8=wu z={QXh=#ww-*l~#`l22Mv8x9B=?{E=QscA|17V^brE~#uJS9G(maIm3oD{Fow65YJD z^z2PRsh%6)auOS*e=t{OZsPdX{&5pMq;NFY!WOWd@I3YUUQ}Wa16)KUwz9m~T*4#9 z<(b#NTfYM5V)z^Ewi&>Yx|ci2Ox{s70wpf9NKq}>b3;I8>slP|WgKsl+JYF(bRDKJKmw^I*J%1|a2`bJR{Rny)ZpTwSk?<8 z&yirB8$ij;0W6K%>fUh^&7$NI+aWwNb#3wLQphYL&s+do0jGx-uc z{txpd?Dsjl)(E5^^}wV8y#c`oz4vboOZeS6vwDTtroee>aqK3PsK>Xk$-KX*mA#HP4SA3Mkiq_?h-Z>stqPMA?GD}P~E z>t(oF8`g}oX0rMC5#Ckay-@4HZqvVq!}YJZyO?<_wVz?a=3QJn2pELLv1*+4or7Kd z345@ZWLD{qqjULp#68pa~*kyKcOvvbhi z{bHYcpgzNImXa5z4eT(7&E7J z!?PagF09m>$+HL>23<0(@RanjSMV?V64P}NhJ20L0^7LU8!iqdyys2u790|aw6gY# z($-7+2=z}KP#>}0u`+qTm__s0MJJf1F6=Zq<^G~Hb?+@pyb~bw8TTumL7yM0Z7tqU z;nJ#<;<_a_X#bGZa^fh#m8UIfN%}UIo{UhAx;TGA zj{9@_KvpYbsp@XYjLOHyU6)Tr5ZRgX7kFl{3?aun2aUoK>}Ijrs|UeApTVkOe4jS# zyn0Hpj{5Zef^&9-_Fw^ohe3l$iNMtATffr#&BJvb(H7laso9s;Mm0{l&I{R4L>|gH zP|5%B@KY%{HmP+pm#3Vobb{)q81PeOQgjWozjKEJ-TsrDxg7`6FKDxaw0+1&)J55C5nJ{?W*G%>2-6N(xGZXW2b#)bMDaPw&Wo{S3y`TKb{4|I8s zp)k?-%L}Vgs=osDnVSbF(*|W?ZAuCIb)(T+2JpF?6q~wIjQ`yCt}@h&O2!JP%rc4a zCH2*<^_9dxR|jI?qj;J6OoCFALQpP-#pZP8iAti#>(nqs6<`AAG!>7i-blQz$Rb{| zywU|!TWSw5NNd+O9|{B}qV7j|Rf81LSxBVMa0VV*{X^7_z{E!DJ;^#lXv_7&|$m?P=&B19)*}G4$X{J?xdp zO>i{?UPb_^4!hV>;(5Fl@%$e(*^T+sopLOwXNEDpBx`>2FvHUwc)~K)PR+E(Q!|!h zxP%5fvu6`u@tG2<9tVv*P(DoY_METIU| zeWnC-YbX>(0%8M*W9BiUO#)=&RWo>J3k(s~q+aypNSOM36%Y+7Q`;pMSVE500^Hw{ z8-aQe!*Av=#zgi3o*qC!9ek2>@$ZNL>TT$VMK!4=;K{lSo;|9? z6B&jj6I~(WG+^jJezP@}rj4pWD?Mt(IeHL=#%wz2)=I6#MD;O1=Ll*7h#r8bq8z{= z?q?9`Ni!f;0V~c*Mi6Cz)o`!0##kjl+{KAwx)Q|EI1fXs3qOxmx(8|PpzVMAzicg> z{EpVfFvsuy@7j1OYtUBlANie52u$OT45n{L!mE9DcjEnGsnBj^j+ZrtwN;y19| zm*kT9z{!S(=y#H=D0`NF6(F7%N1IOjTw&+(zB=`4@I7h1$&(&37$~la<0)IujAe>z z2mgYTOz7k)4`et-LeHyO;bEWX)*n2yT0mx=HEohju|iiRxX;u->xdE^H)|JMp&Y%o z1-^VN&4?drAH4r_jrUo^Pb*|{KLcei5*nTxVlM;@ZTz>pufGZzxS?JLY zy((f;<0%c=s>~E0iga}4Wr!G3qmcB9-|tqi%apw0Lr!_kwDEywm$N{wZ|}nx;1!8Gx$>17I%g=J=s0`~ca#lSv^ULLNJ?W?D zC2ZJPq@UiV$2f0u2}<76^qXjCrj4U>>hTT%UImP0zS#CQy5m`52OK>=otJ1%|qGj+7BWn$E_}eTX7%9Q&=9z=8(f6 zD9D_*XQS70Uh3H4`cvWmBTwD+ZM>vD{Eu#vS-L6wIM8W{PG)htYdDE`gOgcVaOhQb z`S}kgGl_<_D0bWV7qXMc*JLL%laf+i3O>vxj;HLJNE~NbL}hD`p}ggeKxN}QS~=`? zrU|_tATOZvh3!0kxz7=kb%cgMHdCh-#-+WwAi6-c$3MOVs_ z@A-x{zYu7{!DXl=LIh?NFhf;P;Gp|@Q>@HhG?sJ_O$78Emg*fL5+JX$6 zpLv!#m2D;b&v8pGOJrVSCoSNymW|s(?QniFa4{q;h8MqLHf*A)cu_|bvwM=3KhsB8 zSX4V`?gbl#@lEA=p&vL@$IFRn9VxUMujvT=UBFD3VmUx4Em3}NXnB>>Ef^4PT3XKi zOTy(i@|OY1=x+AvXR?SzCfwuT;!e$3G~UJ!{6y2l^dmN=po*?2(JL?-O+~F@0g7Dj zpX42oG^pzgn1$>E2e6}MPPGF!;^3iS50BWq>!+lKj|&rSeqf5h|B|<)ObI#ryqE0G zDT*P&T&$+2ES2YlbE`T})QlmA9eY(L;SIAY%`c4a>cbyB6mG;RU( z#UqVEvyijg14%BXRP;@Ooo_(n`JUHNBHdy>xo~TBa$P}n_2L>W^_KtVm)UzrXY zBLeS}LbP59y@p|@Sievh__Ni5Ii?A;@tCPQ~W%1~WeOug{TafU1YiQsF(KbEcL=k6I zJo@YMhu=tEWrFwI6s_f^-d8Am!lA}hqVvMgW8w57?b84KC6+?R&B?ua*5sM#krG-= z)_*B~+@Ca^F733&WE2Xm6IS3FfBZ-l508GKBT7Y~@JsEIE3{pC9v=J)6cbgR0X)(x z4?$i1)N9Z$jPUoz&+~+{fNItgLc&G+;xV>r<_!XD@caX1JS}ueyD6r3b^+5)q$FME zaa+xDwLB0yncOd#PdTB4KJvHw&Rd_exw%`UBWgl|-zxQKqB9SLnWH5wXv99zk>}tE zxD|FX@m0f+B_y- zR4AIBzBs=W3jW4IW>(nT%yMD`-@(~RyCUEMHp^T`SQX;r6K>*Y<)^}qH~N1K#sG#v z#n$tqzjl5@q~6nSSMg)9F^Z<^+vbT+{)K5ks}gU%F#X#9 z%9tFrH|%Pr6eye@w;%a&JNQ?xir%Sv%WCC(R$NI%F2LW25$jVo1Wb;Wcc|T8J%+Te zNek=Fb1T3Jg`g<6oD8DWG+_a^^Iz$q%a)>!HjmB0t7IMU(UcKJtotfWN=4^7VRF2o z&kuv!%z8{78SkkP9T0YHZz6@Wn@r2;9>2Rbu@?PB5fV!OX7tck&R}e- zSTec?2aZ4Gh1IJilH|grZIB$?r&oF7`vl3EjNFeCss{}-bF+J(_9SF31@L&Xh7=Z+ z4XZfyBXsxj(v=($*se^jO}(mYAUQxd&EhM41TI-AmJYm`l!z(y;5&k@#sUX)dr{CDI_6k z96)bbnJHpU(EvCT0E6tR44lOBQNIE8Go@6WBA4{2(Mibg-Hlbz-wkz&jLVRi3Vbwy zkL4caZSNU;8`S^e8R({{NMBX-@l|>QY3_DJCIP{_KyXLABqad`xT65K(?(N$=vYzd zyCJnkk|Hn&y=8%qz=)~xAUP6{oGUWV))4y?F;?HVbp(`ReDveMp>7My`=Hbg zb4%A*Lbnz(Ksg?m-L*;^>I!5G8EbuQm8<}=3Ludk_y`y_3@jRzWKoMTFh@mb^B^>O zy)=&FtxMDm8qgi%1WT}pnn|cR+RDUvBLqMZ0ucU}d>hbo#k7A^sM7?78jAydF{KAa zD;Km1NY<(~2c<4;;0n}vyMc)XYO)B0kjo&96pK)pK&KwX*#lc6Erfo>bnMwPDnwWa z&owg(t!}fhe>$(|?D36@rVav&m#>IiCrRE5)f>)_#*-k}-e$K7Yw(pm(mLo!4ds_R zzt0Btq3d0C?zri@4Z5)p@hLZ1(u_4xuVz#U{k}me;$-b6VGkgOXu2EfG_Dfd-(QmX z&W?VmfQ75!P&L2nO-Jjd!ofeRB9oo+Zk($qr$PG{OA7x}IZB`Qf~2@=`&a|lkng@Q zN=NP%C=Hr5hvt%E&P}>KM!-x}ut3;q=h{#oA%lNl>KcW6LVGS%;?HYoi=&LW5NmP2 zX`D|g#eDi%>0gzf3R{{w$q=;1vk;Ki(ZP|Xu>d|~fj8^Cz@OxoQxHRA@7erNKjE%_KlBQi z9$kS0P-4UC$wK}y+Q?-a2GQy{>v{5)R}x0k!ak9vMmbAqEPwd&`v-*ix)gg+eu(z| z>{A-+3EYy*KwWZCJixRdLC(c@M0cwEKj9;EO220AdxORQm0zP%&iRboRCvSYXleb_ z;eKyfZ*$NQ-ZmY;TrnmTD~0ej{YaR!EuB{im7f0EP8rP{CMY)3^N&#{((@?-W_HC~ zV(qVVR)Ul7E4}lCUw{9;N6-=sS8S%2ApQOG{(+-TNoK`d6Pwv|htC@ylN!Ap{@EA! z`rv+31_Rc=o{Q_Dq<5aDr-(Vv)$7EhzA&xnCl56_ziAGc{^?JFi%xi?BY-I5{*2$zL_TAXmC95lbR+CmsHYYK z5@^UpN15VK!lRSJ;sI%EWifO}^Xw)+9_aka;LX2>Z7I1nkK^|yE@79ubF=U!fu#&D z+L4$ly2jr6Fp8cvxVlu&6{)H|<=`v)73i<{HuL#`A5T(b#^v)zkUx{E>-c*^w+7PKzOijToqY)2DD+%;Tz3O*k!@@imzo9oZesbLPT3 zEU>d1N)!s03tI{Y_#TZ|iSnja_aFLRb_POI}waewfBWD$;6L zn6OtM=@RGOS){j$?~QxoxT8A6Vf$USXvAwQo5rH)L`Jg+S7lEJ4cZ5+zAn<|72rFD zEH}?S1@fTWxQuTqYXt=Vqw7FX&vjv7KmW?^FXNJ_zmjjv;U?rz-CS~Eq?@(5G%K8KQbbI?EmLVkHh z37aDA%v+-Ks4hRp5AHXh7dI`7NNnK1PD^Z@LZyK1To&R@*+{V^@tZT_j{c1iH z7UyEn<}~}=or6>Ufw#X$VIFS~=<6DXHF$$U%~sK~8vUvQftV}dKhuTZHqGiC!cC3O zzMn^L)*4&Qc=mnx8DmtEr1_3t=6{33OwQUbdFd3>vK({GdRr4En+uFGj;rT6yZGf> z$`z&aSI}(ur!%yr#`8d))I3~FuLDvFE%*$0$GXleQ{gEsKf;o1y_GQ2g!B8o_j??Q z@;S0-?4G=+T}I8L%gnemUX$sXsMaXMXyy2HX1d5p`Uk#zH`Ojdoyy|KY->sST8 zCf;(4;%lKBNYmKUND%S35w?85Lj z4zwaU;1bxD&1u@oG6zyZ%PwVemL!gBgnKj`n>eEr?zI=V=UL|x`5}7KMoHOzuaFCx zojdiRi}-oU@nEktfFj+rN1gkd4lePE9?yo9u98@!Me8`}93-9!IQvcbr-^)+G$Wo3 z@!b4#n=(u0EAH<<`*Y*Rek@PJr#|Ce)&xbJQlBb}0QYKW?gl{^@}uZ+oXWV;{9Bzk zh>Dd8y=cyZzVS;&(^gS1C6>}C&jts(MIQ%59viSschJCOc}rfOu@WuZuthMhEw2($ zO?Yip@ip1C{6L6+Tnh8}ZwQ*K`L(5w$`8`$8Hk^{wzzt8X zJvonQ|4e$;!wZGcDZ3VZ?Dw)0O+O(%RL*{C^bhG>msjtbRNPMCcVnq>!bwVHW*5(_ zxPC{&Pm_{u0ReE|OhIJ|>=3PG`v;0DBd>Gwz!$RXSsb=_!iCW}MlMO)4Syx`%c-0* zTr#pnU&AO05tmJ&=0pfLt7stGf7wF%>Cj2b*wU2Sy6|iN_G6pbym4swQ?#0W`dO83 z*>$BM;tl7Wrh2@jKxY#Qf_O(;lQqzkZ1E`JJ$=J2lBOg@!JviT1LYpQHM{D#GdpDY z{zkvK!Sqhj?>~yMrTTW2TermC$5quXzp;>ESRbL7ecPtDY_Y0peeU&NIo~@dXXQ6! zojH}QP;Y8&{Tji(%0JR3bGQBGo@R3r&}0HGL;-uag7&@EL0CQ_9pI^Gsv~mP0_BI> zsT=gkM(J0l)a%RCEbCRGPre#;Zo_iYddcV%1TlYY^E+u|ESN)`F#FDUoZ}?tc$Bax z?0IWoE$FFn`Hii>qgX(NIIDUe=AT{295hS92zhixf0NHFCvX3n*z6GT{uvWqPkQ~i zVNav!A-c;M=7flppz1{Nmm;ANQ`QFLy8gy*6HwO1&!50^*@h`=*PalDhAC^47YZZe zK`ydq4tdveV+&={vPTq_@eY0wC)QIEK0@+V0aE_8i28ymt&5$5U;aG^6brU{=f{>p zo0k;jg5C!1j%*neNa!3q;!;Mx6I%R?vq)BcF3*QounSKJSizHY?hQhDxlZFrD!&Gw z-mxVE2ovu<{7mtv%0o&vZ$5tcvxVW8flPZQhz1`3f<-$B%p_GoE_2ZXCe`lMnUW<>xfJ1Q}YBNKrmmKsmZ$_G~^zSKjQ9B({ zLJ0w$%NdWL$>g)NB}ndeTJdoPG#a1Y#fmw(phw$jEnLEmBYLZV9s3$oS>rX%s1Dc{ zu|5ZsV}P}nL2i__iDIBpM4SiLgaYc<*aP4c6Bnq5k7!BSK<#VLe!QvI`Osl}ktxSO zP$5WQ`MN;R9T3lyV}SHMLAoe6D(eVefL~j)BgJ;qBbM#mF?6B&W~IA)nTuaUB%RR# zwM;vPK6JptcFPrt8%sqcr0d#5xH;AlZU|u1O#>mDOAKV{F#@SB{^ZJ@anL?*Wz~UD%8nn;~UTw!gpC{Nh zVso$*Ls9~4vEEtb8%{_{k(3SO>LC2T;1dh?fWg}iO2tY-efJWOMn3>SvMw;C(-agS zfDvPRBLRU^+hu~$1{llYbke}|w_tkNhCAvQZ2fEq(NW8^S6~lUV)`%wnBxxcOdke} z^C1S%hXIcQ03m(Y?KvYZ(MP#o@bUPJX0PHo^4qYQdjL_(JC++1<4nELViQ_;z8PxYx>?}IbsUeiV*W#NL`CzDwZMl58)cKnF*}oZ-`G4Z3t}{)*m-&b- zZbnz!A4PbDWq)A1q~E50rdwl<>YzRT4xT{ESq@sNXA=Q0u#*n}dHCO-?|p2sDYxl- z%KzLi&X#GG!b*lVPtnF^`jIJ_=4c4Ki1H)-qV%0Pzll{j=b1&WBcqqa|AZ4t5e%7k zrqZzRaVp8(ot~#2KgKiI?*8Z#1x{@S!^Q z{3ulEWLK4!=G657k*}F+|IOW#KDWQ`ccr7QVq}n<=gwItL%qp?FSF8#JLwErNV{r( zBYY(b6VrJtpM0gWoTBIlzn%9e(^eZD<&3A!7Dne^oMkjMgI}jXE;4@jf#+C%ZPWV|7tCh_U(#p>fd5SCTtHaFmI7h6VyH*nOU15YsXENHSb<%M+hbNCABYevz*=2QQ<-DC(D zdNoAk5^ty<_xp(Vw4@krP}g{zL)WS=hjM8v#qjETj0G$5QOyf11?0|R)%C!nqZ>cSJA z$Uid?bA(i_cC-h9)K>z?++1~!5%h77Cy2fu zV>H?cB{_63#Nfb6zJUB*05Tf&Sbi^XHV8w{@_Q|!A)CZ#ha`LXx3PM+Pu63(Ulk0pssFg5YyU27t^% zMeW8=Ul;HtMg|{|^b;Z_rw&fQKOP`Apynh>`U#L3kVGW?1V}re%q9IKkUrADk|q5F zmUbMLl=Kr=YGai7q@S)M&j^ci53cKS^uxe)m0dpbk#^Z7+K1ns=KsldL5nrQqU}9v z9EtxW%NFLoPMbhVnx(M31>jw!dz-Ln#=$wYELm7PM}0(CMN8S-Nl(uOkV}l|dSRVr z89Fw{OmE@dRnkBF`o zUV93=M{*m^)^ToRLfJGssIjwlh~h|@{Uci>zTs}}H=Db-zm1F`C%X7g8I#0*_+QES z5Oriz)6(QF?tKWX;-JJ=g)&a@<{(^7s(MPVpVL#P5*@d0`BKRWr3LPE28-L!$RCV~ zk$_XVazKSbdr|F6@+4WBK^vZs2VTvlHF?WArIM1)Uh;vF&Lq7I+qWn>I`e@^IW3US zUN0Iq)d^<|&O!2yuJN+>(_i-Ej^L1)GYP5JVTn2$M4X~c9Dg>ZtALqzSu1Ho;rE z@~A19qAD^C`8_+~8D1~CYJ2gshs$1eQa7o@aeG)lPLB@sRqXNoe21d(1WwUt?r&nl zn?fViK+cUo+=fh4cGwfCHA{BUwk4BQ;Y^6AtTd;Up?%(rS~{rNQ6teIb!01@@#SLF z_-SGjhjX@KZ9Xk(Gdq2rfvmMS>UM=#PDD8Y8S(3O*RF8P|JJ}2H1*Wf_v z=o%zvJL0)^ZUo*9d+^^7Si8nJlNUwZNJ<>&6Ry<~wshluC0pV|&M>`Y(Y=jn6P++5 zff>XW2kpjUqD;=pnJ&_2^~PKx1#DoKg%HySi37g86n-#gfm7_xHEt5!0;aEFYlZ3Z zV@GT*J?e+_D7lzx9BPf7QX4vo#G~?P9_3PQD?KhPN7pnZZ+FT?UYl$>f{@r_m)s@x z4@IK&BJ_$f0%zEc)pfAiQ}ce$FmLytaH6Ka?Z1apG*m0y8otY))GwoB!zb`5cyT$O4_w4D zJIUtn>k>)Dn{7aHQWz2N$21+2MJVl1KQ`o|a^PP3Ni^!0^%A&P#GYxNSik=p`VWT_k z95a&5`B5B>72jd-4@@T;=q+^2N3_S#_8OtkR7F?5`zWrIE4 zCYz57f|R{aGnl1<%3Y2p^!}+6?y=iUH+w>V z{7}OtHiNE&f1`=9&@*_3m{mc7Z`%zRAgd4aU$I8EB~0K4-tP_Q%W^+9WQz7t+tigy z89DQ)*mq~6x+|gnr&M;A*zjdt8M-l+H34`xwxLXrlR#YMn_6%30;&36{er)iwMUrk zZ)BW3)+0zl3X7kTl`{0+or7@{H-rLQw-cjoXj?1vDoTw_py{_xQk7&%oDSib2b~r1 zU1I+;8>S8<)k|qaX2$!c(MHo7Dva0l6B?8P;jclz*#EL0DSp1R-PpvraVB0*?TX%x zaj-MICq71QewrMnW3krmF0pyeX%yAfm?SuGE7YzjhI9(@UZipxu(3@jz5i%BmS+Z|k%ZhN#!c zh{|i+iybr;xMS!~I8N3$G<=d0yOqWd`8y`GpnKN|mKurVm#7`%QLgksF*st@344k0oiL$4Y}_=^b6UA)+vPuGSGCfJs>JY6TfU1d&maFX3-U( zJkgg>Ce>*cwq$)x6O#87l|LZvo5fL3PI{V0p_Lrfemu7E6Wje2{}SMG>7UFuOxE}s z+6R&LnczT6OR{HmsQA3~osMx`yt!%A8Jur|M=B6gS_`dAyipp8QmFbvqj`KOO;_Tn zJW1Sgr#RYz>fTnwsM9H=L#v7(Py;XPG+MEv3A*n#?JMPRr+!dzz%a!=NbSfBYU zrAsU?XXyhC+Wo!y0{MmY-mi49aGT~Q)V~}Kx3E-GA{1tpG1kH!(MHbRDstVzEK6i> zQ|`X1Ft4v=}&NxSrisOXgy>b*qt~t z+Q5P+1;vZ&Ji4{rq*v5mtjVKjv&wYW^<_)zX-2LJ_SxL%M*D)!S%G3R8}bb^HeZa1 z|BOQE)hpi<_ja4&;<;yW6ooG7^)=TJ8~)6A;!~J&i#7~86tKK}6VK9)+VKUBdy1n2 z_)#GL9;&YYw=}356b;26{@Dv{GRNkHZK%^w~y6ds{FMFL+@7+6l+bQajq<&Yv zESEHGQPeU1K4rn;OxAcr9V@h1`lvt2=dWcty8|ZV;tfJ)gBcI*5=A?Gl{K7iXwwB- z-H5s7Tn)0`l|P2g&ObKZ_;B$?_GSO0oCo6nmj(UFkAPxzN%Qr&VTS$&|9lo*Q$N^ZR0|*DS7A4N2juj z?zRLT${Xee+PXQ966zu= zOPHLa6BmdMZwFJE3yEZKelkKwjXU=HuuK7J`nhmCa!}jv&MdmVSPTO%nsKaS0aWM8 ztYoZu&fFu5;>}rp)l4Kb$(2Qvld!$$LDWkJLB3sS7EOX_ab9DDEbH1@KI)0mm zTOp8x`8{|-D}Cc&3kLq+Q}zIFcHd+^p5UjS;dOhs_F&74 zk*zEubXn`Ikcx7sh^zvdzGJs|vfl?Q4qe5cPW2w)P7)gy@Ea7w3C`BYY@?>i^`CeV zmy+px36i%<_c}voWG0JWbf?aYg>ke$%FN^61?BNU=j7Mc+J%lhRaR%VuQ5HL>x}b? zeEwTldIH@AZx~+?cP{-i$5G7PyhaI*I8)zqTxn)LXExe-j4Q>=5yx4Y!zzS1;vAM2 z)x^uGl!1~Z+r_jMV_QV>pMDSOpRKZ$A!6UuqA?coi7S5--P_w)+{Pk9fBu)r>>al3 z$>^3B=@&oZ<}35v0Up||$L-o=avE|gBzj>;w5@G>8qpCra^?kOlQPI&LmF|P@Td91 zZy(yJ9=1@U`*;L}F6^$DJoZq)4y`W*Yt$`UseP#gwI46-cl5Zr6yUM)&It!5hQHmZ zJ_o1PFtJ-9qKlSOZ?np_1-RN`1;Il zbjMCYCR;6d5ldw=`tbAY7WT%IP*m zF*B+T>t0|BW(MBjhV|hbC)JQ zXVB-1Np^cy!m42}L1?$R7G?r#3HgeU=$({nrS>M?ZeErY7*fytcC8fZLf)%(^uHwr zZ%YcEW^13=UZ}Gxp=;8%YN2jw6d2+hNYY`MrtGH<(a5N_7<))#fuvO%^%J4k7!o~g zZerLj8k;tki5J*4*AmJ?gJK$+KHZv7cMM`I`e655al%|bZ^@1O>l~ri7^AQvG#cIy7w_n=MY*I(hCgvGRhjOID7#gg=Zg@3> zZj2dynsYOBjX#jTgHmV#nRq0pQ0g*2UJwsneV9vW-DZ|&Ni15VO8xp9)J_Ol9)c56Ubg9Mvk2#D#3`_SLx)F z6N7p(7s5v@yB{$VRYVop8Gh4NJ>H@2Dsw#4?sHB?=52_C+>a0<`-X!Uj2ijrF5(TQ zakgo|d}k?*+s-91jb#6fSDrq=p_de$c_rJl7RB!4xpM0R$V)Wet;8;iGYE9!)SU#H z9ADIuSG^DT2Pr&`Doj}3i{Qm)dWGtQEX^Y4<~WZ6v(V-i_I zV?q(0eD%m3?sH7eebHGvb0#7;SWThnirb%T^jvSzzL)zlhfb@_(Tlt(dlX?U6SL4e zdVE^@j3NxV-kb?Wbko-n8v*!1&II2nZRm&&a&iq;3FMI^cLDFvkuqiSZVk+`kd!9x z#+~I)eV4=YFP#f4p*X?i&cT-c#TBiSp10{s$-gkjc-8G69Cd0}&;TdM`_kJqj|B9W zAWlGw&O-khoa4@;z4&ELTaU`zr}|P_3Ac1=4p8ue9NF(&E+J#-@FOCLUi6SX3qxV)kX*mI)Oki6PF6Y&qd@t8YB#o0(0BED+qEpJLLwhG~xkY z_!kea->^Z_mp zIjVL-AJ^qNDX&_f9>>PlGl-JQLC*Yr#Fd@&YO=L?6kh!d#ka&Wz0O{e-bTkuJoNMq zY&SuGWas9+b73*AK92TbMw~bTOh$eK{#b}Eb&vVUsLeSXGbTYsFo?~mR;^EwU z(MEP|F#G-&zMQQy9b?d}$AclY>`&mwHr zvk+4XyKE*y?rriPf#b40t693tpDvR7dV+)ax2BHXc3Fykto5x)l6%TW?Rmc3WeR}X zt+(9Y=l;X&YNc;ik4LnSsWE?LBJbSr0--i1H=EmEv!3teE|`5>gCJBsKj8&=ly zU!A^Cy7Ec?EwRYvj>Freg83Ywy4y7FeAuHpCts!yu46;Jguyby8=5$eDI8r}VeD+x z+MCZ#E!PkyX~*m)7ICx(S%l&Yf6in4Xx#$e^#h;H80*~rW#_cHn*1Qz+-I}fR&!Nh z`KEp_{-dus&-m1Bxcsj3HD|4jKj8eVJ5ajPN9SjWYGsB{c?vXvt*yxnA@W&6nEe-` z5^k2KEY1qVn1CrQx_tb#$^8tkm|0=GaJ_=`DOQfN!dsBJdv@EI*EGEUbMvu!ewa+w z7T7!#$1zuu!z=%qKNSY7(kXWOP7nz}Yj;u{uAS#!RDI#tXUVpw1n>MBlxGyPS_Td^SYa2(->5%*ah4b71M-TRhhu$BA)+F=j zp8FU3N3x-h(0?_Pu)9k#M}5`*iiN+_a2}6J?n|~yAG|BnKk*oDmmb9W(!g=<<4<7V zeWn=-10TrVaYz`}GYNl-)i-GhU!nn37_QHa_H0O{!U$0bx4Jtrq7q}HlxG&t{2CbP zd_tw`WRRKxE2@&j|HG0~+obDw0Ze4+ReqGNV}Wytu)3u2{Ft~gk>2d3V8yS=pW(Ud z;_2WaislqRyUI0BzKik{hEkJq29HwFcG`l@2Of$hfd+CTBErbW!{H^6lj{+9#@S3- zD+Z4jp5MmC7#JWCFs;6dl?DPTPuP^a+auIZh5N6M<(Cfr5g&=BWa1!HxD1Osq8qtZlkBINshY< z*kbkuYe!G4Avc1L8B6L0!F|QFQUP|m3trr6g0Qbs>Jk{iW#CD45hlmRC|DgL?$*nQ zvOSQ*i&H2Qv1WHYHGNRCV8}o#=+KgbgBP`{pepmeP00!7^as0IJQ)nokuUi{F>LMEectVtGMhq zZiOvl@gP6V>|k3}#5%l>QLC{Z6u?|YhX2UcAg`GMmcNISFH~pgcrK-cA3aJcc3$pB z%KQ%B?6J{fdfmbTz**0;^(@Ox{?Qu?={=2p!Md;^-{?(@W4uO~NKaW_#)f4{i#DI1 z;=)^;I3=<*!j6>d{loMR7n-uhM%#?JjbKOfSJiXcM!HTs(c4U(1OYm`{t_+I+`89b z5YWLZiHf24(BE$s#$NH$gWNO(AT%=&u-C9;8^h3zVMlBhunW(GxCxiY9b4xEIqvGG zSPJNJDH^(~GkMk7as&#?uRg$`fVVist()M`_EQTZAIVu3Qk41lO@Gx5?^{QXzp~Lr zmqd#Zofu&x_jmmfx6br><~mZl`!6;HvvHoNGuLOiLuxy)0dD1MLTP-yMpToHk#AlW zj~3^Y^r?FHUznb_MfOPB{)hzvaq^AD1Pr!&s5Z4~ht3lV8{%od&Bf&Yuk=#qt?Q4X z>zV<(S~%t`7PyeY+-~cQFNt39ChTu}+@Py)>CzX{F;2Rum`OTD8kjwa{J^Alunj5E zK^Pd8%IMT?#Y~pUh;Dm6=JoZ%F};y@KBs_hYg*eCOJ}wE8tco1{yK;2dW56t*0!yDOwZ5hMX_jC0050 z-g}kJAB8S0ZOCi7=fZrR$Is*ML4ki>_$Bv|pY6S6iY)_TN_g4_f9az%a;p#ib@q== ziUtSQYI>BYd?SZFN_4xX#t)FqMdnK#8d%3G<^#C|kMsW!u&8F|>(VffAK%wL?ceP1 za9N_2!SttoQMB!+%?ul|J0Sgsg8OvrQQl`IfMzjOLz;vMkiUgigCc)n0MZf9>y+9? zVWe-6Cx6P+K|k~%6m0e&s0Ac0!K(!rxBjK4!zm*MHA#YXMPk%Oj{UvNj-c|aj@c19 zVMoYJv< z{bPDj+vDEWa^x(U z4kh0XYTaAbeMyi3wV7mSP}?ZX5Az7xmQyzfKPnD?$BpD!_{8xa zyq38opz)KJH5qIzdjw1g480UQ`G3F=G2)Q|Lri`^FxJh3P)}j68k1H3PgkVQwCF72 zLsF!LZzUJXf4w(ROy#?mX-<%JaXE5W1ilt3B_ha8TbYt**=5mBjgRIQiu5P1ex{8? zrk9{F2>siy!dv&_iezD9!B{+S<3nkDp!jAr)q~3Hsm~T8{^rDO6>)<>6scA%^N%u; zI9fH*X51mEj1{CledjI7JyyU+=c`JjS*Hxvcj-mB{EB3;Nzs-?t*wSvNlDyVgp|d=FFwp8{h1vfCSdF-^MPZqErNZ9f;iDMTg71 zxjMF^7J4@Os^$za;D2zCSCxM%4U~!^slJ7FYqoB*yXf;k*#1oVT!YW#0HzzKbexi$ z|4wWroJ*?G57<*@%Fm0W@mir1I4AcIaa_!PTYLiv&IYp6(!@c6w5cN(_6sCbf%0MM zzsOGnt^ZOx1nk!q6;#LTIRTfWguAi9lBH?jRwgm%X629xX)|Pv(A#@z#l2AgL>)kE zQqPqRL&IOFA$!#5OrK`Zr$;gIdyi0F!&CQ&S$LqpupnB#zZUbnls*_Y(TB>0v9*VD zi0XKvdbfRLsPv(1Dk+1d>~#WQA0pTbh8_!22U0PF6y7s3I)D-0CQz+CU0B% z=oA*4AD?E>ryHSv$+Tkd4+9#WB+vj(+5}Q9%sZA4I`MxH)|9v!oRf>Fc@f1#Bv*FppjxfQM!@SS)WfSUnPiB8rzQRqMP? z)!3VZ-53){>)bT52LhoGLg=4T?P><@Ce_1>1AsdZte3p4VSJkg88(VluXRSRe*+xS zR%a2o04Bt>nnokQ#c-+^-VtXz+^T!tdN0qp$rRFcq-|3gp3l=Re(6U1q+)0tC&bN3 zesQbp<96C${+vq0BVl!Vde<5AXIRcMxyMu8vpydCo!&*$`nC8OTd`FCBXdHq&oqAh zvH6H_=nv)$`eh5^1=G7TehuryqXzTc^Y~Sc1^RD16(X#IADKqjzvU>u;CADxK z2VppE9GTwp;&$`9l-%*#gN_8O^NajMYv@)n&yy! zH$Fj06n63`nb>;YX6Ouroh+iFY7Z6J^SB9c-5W`=%{`A>NERfeaWo^i=W!=U91Uk4 zr6mW?}9nf3pFaJ#yt5szP8c2wy1&LMH>(Gw?`8Fk@NA9nG+*`x*~8Dy_7?jGl2s4QV#Va zs6My%5Z#KPdrUNW`#zV>px)lr0-sHV{_Sd7^tmk^pULE^K9Mi6?FUVF;#=*~h%jez zF^Ox{rl}iX7q*+A$bc#qbrLkPl4u8_h7-_mFZZzCo8FZLMmWE2h*6YKkQmFJ$w36>fUWGATn>Sh0?1VReNoqL%6tIX2=L=a*{1<34bZ0hM`hZCcxRn(8J zb}fdKu_o=7l}ZfR<$(5Mtd%RZebGj3f1~6vXtxRU8msJR2Z5JCpn_gZX8R;ymkR8< z)N6<61EF;QVQE6XG^qL~s0#rj9-P)Mj(4UOObycC`-0eoe^$7V&X_E@5onPWz~nhQ}= z4Y}TFSQ|XYm1^pbfb68Trz<6pxELo;caJhhNlfiHKmBTrlNc?MXsg)r8$f#h$`hptlGfr!>$qoMt0bHiSOj7pJIM@wIcxk6nx15xXr(&` z3;Jtm&kUV2H<^@Kqx+lpJ#L8IMJu_cKeyv8enz`lyv%5RwJx_{nxn^=nLNCWc${6O zo5^ngnzs;=NHa;;ydti>rz2lVE-i^yob~nL1>;)U_P9+VLnjw^ggh}}gE)a0HP^%k z{lNCZC1L_N?lHL&2sqtOxvkeLtB!XhCCf~}a)=Z5yG$esv8WPSI^#OqrZUly>3#l% z-X~8{RDxD^axAx{yoFX)W+`8S9ce9J!fjS@{1Sol9eDPiDf}?_sZGjS6kn z_NS-Q%9<9f#B^tbNsW{^KVUWwai!WREy&F!E@^X0v@l@xNb_*+w9w-VWF<8|CDgc# zr8K8;f};>qH1Kfg?+%9y9Vr9)vEO~o1^hqL9qmDd0PJUGg6W7Awr}sNP^X$i7bPOO z^+(!Mq*c5n2eQlL#O3q-`cWL*q!*c#5?E8G@!3Ey`fMkHA$#!zc+&+=N#`VCIwmBr2~7HqUfIk$ zBs$AtktCX@4Er0YLTctL_vY31Uu4q(CzrjmDHS6~^&3x#Vfx1ef_{A8E90}07{BL% zO9&X})ApK}C+RM9oNDAdXtsMk8oPpWT-+sw)f5ZuMEf7(^Km8XS6R-R72KuA$Pz%a zZDOekS}RZ!51evQU?_jZGK)%GkI*Rr{}YIjT%=RNT^Q8XA>X+`Tc;!%cNMHawR__V zs)CCp3f9(@@~(3+9opMEvAO#!n7y6l*$#wE?2COge!Jl`=Ox61Y$Mi3$|DviD6`B<23o z0M%XYR51wE*qS#S@Jq;5c7x8N1Im*_&nW`$7`Vi9|Es_3z^1o zy|e*ii8Vfo(X;4;2Kl;jWa)eXr~1jB0=jejP))SWLcA3E1m(=AoLDXN9V_cP9;-Ty zPVp>JX@RV~;dYLEu$QfnpVqjmEuX!NB~SdU7SBQ86--Hv;jLyI`!BI+2lfB?&jwz<{iyjbEiN!OO+^p zZwK>!NeC%imd(ts8@8EiC-ZkN+{NU4T}6ZCBebp~5YTKw(Q9OW93jkbmeYx3 zUr6DjJX3&A2U&z+jt2bHuuP^!dyM!fdCcYOoKc(>CeLjERa4LGQkAb(aliQ4d|xO1 zg;6R>rAYQ**c~O}jL-Ve3MLtg6$y#u_j2lLG@K?b%4u5xP0rZl3IQ?7eXCb|4Ty1v z;aiH`z_G3y{cB_d;YB4T=FjL@<{KU1u(o`JN?`MaVnI8H+dWuPnj;qU{yp-$J3Qm) zSjP7ozY+A?b$4y}tar1rJh8jZkqH@eEAKWXXh{{-D{} z459TEE^yI?E*R9rEDFqcl3W(cbD(-rRiw~)KeoNHyzns?*lYKTV_#sAvt;MF%h!Q2 zc~wH2bl>$)3IyaG9Ji{ZKOztG-m6N*G!2Ojs+0HoEZu=iYq0qE7~?%{A;F;FA7LcE z3nv{CWSfT?#p<>nfIRDlRW4GY9sh(E#IOtEqaLM3y0vDikOJH;qFg^^kqp)LDX4rSf`0> z!9b5(fQAj!6q{(?mFLU1l^>Cd8D6LPo8Bs=ntgQFHR$Q_J!bwML=~s*qM`f@8IwJg z;LE2vbQeBY;=MVZZ<|GQE352sX%ZH2a6UTkol(m3^S)b$PG{cJOLzDcq~#~8oABjBp0q}FisL|@xeb(ySugkU?#wGd z<(X)){D`isv7797=3{7K_8V~=H<80kuMdoBJa5EH+=D-%v)%dOY`acHL!&{C^n_c| zz_ULC@o)mwiEUz~j>MZoOH7AzJ4!0yjpA4*`T?gBl=I3~I*Cb(bHP3QkH19&@u<M+Y9S9kby)!h({jxt7TkjM`3dkk-4E{2kvKKk&t#?!Zt~)~T;^gK zq)~6%y!yEUn2Oik0#0y(%CZ&@xAElt|G`wUI-A7+X96Mn@4*2-S-fb1*x~X~KZ)-r zJ?TB?Dd;9P>nEXw-9Y^(3(|XDTZ1Nc>pefK(z&*vRISQWy-0x?K;^UIhPvty!|Qn& z-5f&hJcyv(s6$~PTFS0`O?j+5=s;JURM`-PYb+!aZ@G3~azgjo0byRX#bp9y=e_0> z-7FezqO?aY_Mf&Eewhi%%YeXqyIy=Yn=;Tx_c)@8bqhhLaqqX2LsC}F zh@4iNcp(!LEIv~co9oRFsCC#P_hed!Z^Ff%g7{NRCcYuuE3%`h1FhuQ#|79Qe<62m z%^CcAd2cSH)zvr^`@=??1tI5$Yi%v?P&bSd8@FX0N*bD@L`xXe#mKzquiu)hz|h`q zz5P2O60b@p>kw7xO8%oAawCq_5VL#;2ll(u;<=R^gLj-}`#39@e9`CT&cXPR$iC$GtVYC2udg)y@9p#ur~ye5ctxX&Ub%lRc?k z_;gA4hCzwtbVd@VrKRTGP*_4hSd;S*E&vRo*^JbhXU_%eOkEHXCY6lkA-IMohhdJW zSCK2{vr?Hxn&lZG!EllqfB8K_B6cyc6ad`o=|1M`{|E(Rw~$;nA|jbcMC4Qdv1hLT z6k2AzoBxT<=X0KYM`717XK8r_k-2<%DL2VZwC?FX2(4(Bf&QW?)sKZQ1M;>sroVir zJ?q;vWxa=6s*?x7flg~2&!_)B?HELY%y73(qaxA9Ue*Fi*|hw!#1OIkiR7&MS$VKl zdvzPu!}wfK#20xlDe&~98;K6uY0AfUulx1*FyQ4q?vLT$N+5s4)X4pO>PuTuqhRU1 zmunQVN%^3q>b)0(k@7(87;dwIFUEnQeP;AD;FF}n&7;-t59CPuT$yQjy(YE%s+*Dm zIChQRNBQS;^+`FSta{X~BITH3A;g)h!{185```+;EgHnfP)D zPZ($wG+;HM!}SI78_P7MC#3g!I?@yI5)%Ui_&$2;?Y^q#W8d96<8AhsguabKYn&{r zGP=Avad_OCOQE36$c`7YhqId#?O!9KnNDrH+Cl2h*%%oN4ns^Z5PbbEfmdpCC91$JaUBG>2q%#R(4cEp0` z9jPZ-CL^St#L5IViWOga-;`YAlpe#OsatGn4MJn&4{!(qA`0zno%X`!fy8A6Bd+jH zDX|Fhv%W6`f~u(=O@Q7#SjTGxs3(h@^##nMfSjD)J4Q69yQTu6|EDs;lK|HAXQcpG zJ239E>?=84tDr0nEA0$Nw_ZBkt_ymHh!62REnMRxy1di&nehBtxmQ4Y@aCG!Vu=MS zUWi3j?~a?rTO69umpb5XJf&6z{Tl|r#4f7rq?}tJ{siVwx%;J)os@I=C8O4MQqHM# zy>yb(#X#Z$s*;UIw~qUQj^a&gHP=c6IAtJtG}(!kKcHS405e=3$JZ1$)gG?OIt~=H>3Mj(~l#?=Q6Rtd{i$1oHt~Ak2X!d zED_{;HR#kgCQhuthM0np`y4A%Wk(dK>>sGZ@sY=CivnK z{*Ek~aG3ZgUc3Ew1$YSN=o@l$G2#A}RUF~sc{{S{Iz9%J54ZRWUI*)w9jP_|v!H13 zC(H;jmcNJh?SyUAsxOs~Lb6u3!Iq)%eFQqXXL&cc)YE;*3n-x+h zW)*Z91y%EH;wvdJH^FOmKmSM!%WQLLvv`SdnpNp=CSZ?UJ6!YEt}YzuOS3VKN6R+k zGR5)yW~_X&zG+kDwW~ce9n0&n1l*w+q43{zP_Wy~(Hotls|yPD$;Yi1=gTm!j+Hh3 zi+Od9hbgaB!r^Tr^!V`L{tm<_)-Luk5u+3&MvhT>>^w|IVialMxR^xHdq|AbjmyEI)oP3wf;6yYBOwEAPJri|m{oy6~i{H=g1t6x(*7kqr~6UALwZ_K&uA>NSe zODEZcH`rw!(|fQ)Z-gWN$_|!{&@S6CvSWP*o)p`ehP?m#6O^2!B;HwksL;pgQpw$0#2mG#Zdl?Nwvev_POrv(UL zt{e{RU*>Gl=O}tveZSn5PvGTjBDw>8Ty2}Tv{T`YzVmp0mJ!H*|A^jn4}6D;7$4Pn z8gzp^May4R(E)~zt7!T2kC1wz<Hy zO#-6hz?qR9o(iPbU8=-3ygv;ZmIaAD#2oy`Pi1?)3iI-W8wEKshpk|YQ|sEFq2nXU z$ogG}GjuedjBGnxdXMNtb=Ypcu(z#rF?3Zif>CktHr1d((a)18l$@&+;; zxbH0xFk6pemRqwv|9aX-P*fT?YVdb-uP{Kz3(&16nYc2+88G!xf%yrJ^bwP5z$nRi znQ2 zg7k@(VqCD}Kpm=)VljKjMmJ-Ey)7~}uyH>;6-%Fl{^{ob9>Dd1jC!hnDBL2^M9M&_ zWTC;k7(Z}We`#-kYPik~n@X)(jXiU$i()10k$i@cbxN^^8R3+dUT4kxj#s zLco5_&|pRC&Y&$h%;HQTtCzM8FWqG!BOAYp*W??pdn8{hY&g`>^bL!O>4?WHj#{Dp z`JcM${>GjDqn2n{#S*%x%o9uc0!-0_3VmL*?#}UJz-*|c+3f%|0NB^;Y2{lj00m0w(242nvJTb)|`iWdfKNPIsVV2ha@jCUn+BwV<= ze8Vut<|lZ#=U<4Yqsw*A*Wb_0$qEUv|K z=O{>wV?8kHUOi?T{BEFFv_I3Nd1e5XvfE`K9z-uQFcqC}L6{v|T)N|z!a`+o=fW%+K z0BION=XFwmmz5V(nJH#^iIPzGx|&XJz4Qsn86Mo^CLr++O6f+}D)WsctD~fQ`buo? zTHqBMXRZ8o*W(qN92?R?98VHmu_jVax?&BaC!*i!j$)#l+QGlr)@3+fsBR`LRKE!p z3d{*4JUMHbTllgGHSv9N1=rJeP{0`_!V@RQ$fzmgP zN)-MY|L@s5nzwj;$(srRrrWYOTcg5sOW``)fP5(pxu<2<&A5Tp!Db4aq)FF`xNg9V z>Ga26^^nLGJIM=9E;;w=ZL0#rjPeWXeYsSsd3gXF%2y>O?oa|3DsMXJi@~j@Kuhu< zW8U)k3yR16(h5mMmqorSO7K5j1~)Opf97G2uktSjGY($s+etVcXgd2GVP!kfihB8B zF-O`_iJEJDX9Nd!pe(;aYXD!jVFoJMA8u9?EvcBbGrLr?Qwwp59#h29KH;5WXd@be zNPa!jrv|s@ z@x!9%j(-%8KyQQBF8Mjnogh1uhC6e!I^qhtIuDD-6!p>;4F{Mi3Yw@EzzlVF z@gF6ejO})oftl=laf98nh`qdldbi||1q{k>aqnO2A@#()@1rX9qz$Wbdn89KBe&VA zXFV$Z_@KaQ){7&>;vCn4;66E0AmGl=zaOp{-0gp%SE?iSCbM7~+Z^{mO(I@h@|r@v zPb={{7lnW@i0oUcDMs&g122U!HZ~j+px=Nzuib1r?GKVGZ#wSL!f{LYUK*PY&J`5!Wlyk~AlKi;_e4Ye4jPOD1 zP{^la+@k)E43lw8>>VU#v(g((*S;48%nj*AyuSaCN5#@Z3cUX&DvJfaL$!9ShB=4c z?ziO8qm#vkd<)YcKL~hAlez(xdICd|3u&yl}*S>m3^xK>H55C zrub~tEl@ybY}~_-QEdc%P3Y*I4Ezm0-Spz9QHlH7gLBl*^N zw+5XfKIBG$rwSah?OUVxUy+pFYtIe&SF%EeMnlW53QsaLx^In=s1tIa?%t_im3OFk zHcCojs!~RRy1RI%O7u2wK8o-*&SK)VtW^2hqzvILl7#ZbDlZRZ(iM@fSld28EPDM=@0t>6BrOChJ1(T zMtg1b{|>n}TTGmVhG!)l%fMuhvx;g*kIh!o%GX+W$*u*2b`5Va+>W()ms*#rG7wJ3 zyHq_)hrx_0Mjx+>U?59DdSYE9^`yJ>pwyFGs49c>xz2pn2jCDlW=`VJ(cMZ&*Z9R8 zEDN6%C>Tv%BPP;D_m+9}eX;o8K`c5y)mAjb{bm8?C#7idmYE-^wC2j^TjWiL+gYv- zhJd5y|NNi2yuK*`yoR^UP-8w&b=o#yeZnH|Rq8hLnL^7`2ixMXO%vKETKeuDy^*cC z9SSVeVjrb9^4W#j<=gyJjQGOR&e27q(oKV7sd@+4vXrbu!{-W%pYANFwbEcf59$lo z*9{+2F-8U(W0`1T6`gIBrT}#IVM?llLxVc66O&XKYKj(;d6lh02{$Pvcn~7_6FNS{ znW=RmI>-1EDt&zT6DfV7l?9ec;nJKnF-*=G1ZLPMi?U2br7y-f4_wI)jjR$I!m89c zEZ8l2bH5m1t1d8 zX;Xmb)`_8Fsw*HwEo?#?ml5T8uw?m2`cw~Enxj@?@Yf^_8;4u9 zrRA+BKZ~`=VlrR7ntK1)!X@lGa2=i<+Et7&D;G*Cn;jB8Dv8~43Y*&E0O}KTCS8_Q z%vYS-g1lsN)pO>%*MaK&7Wki>_1dw5(C~&v0_DnJbftaDd*Tj$eNpO(mz;|3NAcN- zs2F)_RlvWr{PZwjD7||ZiI!7z{^Hf61w{VBTdscJoVGOCdj$r-9B>lr&$_=KR3pJf zja)o4Lmw{fdA;88mTh!%7U<{m&*Tz6aa-+~E?75EEq%MJj^p=x47(ql5dFUcI7f9J z01P=+o5k8jk=VMf&t#b85IT%qR&fmERgogN>#8=A|8I~@R`)L3tE8RDR~^?u>^W&? zYKKWZQ8o?hCG{lnIX!%kLegPu2tq#CZ_>N|cpEIs*x3+KcL)yfJzv4b z0zMvGJFc6F0bdgy)<=c${z;#}!`x0;0ID0RR`#r~<;dtw>ZT}PRmCgI|B$2A!HuFj z!PREo{$P}IEXy&J$GYt6-o(Td6^Sul|3bUQjU5dBi>OM2({zG(-YDYl((1*YI!|xcJsW2Pfi=^N6H47j%pp3-VO?y z`iX}xN*KF$5_7a0Kl`s1)G5Xv@8ctpk3YWu8w|LqcTIRpN*}tTub1@jK`8)yLbQEX z_2PDy6->4~cUO(uE#hv#p%03)_mC)ysr?kMfDIM?rlY`o^k7M0mFOE)L0hAyN=>`H z`ZIc4Ydfk~ zpReFT3|3?cPK~b~kEwPbHmo-511N<&m0do+pZs|?_HptO#?&va8`?gR?F z)un`d|mSS@F1DxXZ?n@Ki>3>0OV>R|Y6 z4~AMT-zSRoWNk0d`gCx7Jx&>n(?fgmuctJlt-*YVh1k2ED3E4ai_dGaJ%7Z>qj~kb zFL>_O$Iaz(any(pGVuQY*_~HvV|3G>7%s= zv|K2P2BwPv{rwxG^z+`+h5f5_zzo@j2L6}M$`3#qO zlCSj;sVBMrANtXr5B#go% z7yIuHR0e3}8@ptA?(_s-hWybkSs}E{0#nfq++!h-;mk4)3E9vmV0T`iGP!dLDMtEF zE=rY6fymg`qbp*?Zg6`*YLDkuqR9(=9#_s0n>Xzvs*rLsMjR{$j&t-@f6=#=WhRUl zQ)0H<35(%#n6(@DQEf9bzWLm?!wpkwy;v;tGpau)y%rE6SK^A9*9qR>E{vkSqx6LS z8ErEc(iN)n4wbic{0Y4)-(Le|#pun{0Jj%2NcC*1<73-(@pz z`06LrO|;13QO(n{K#W*LX3FdVFyiXBc!^AiHsS-w9)hCgy!mO-ogJUKaX<52y811! zj5Frzi?8b0@&H-!|2}G2)*W#F?$wH-f`5ooYk4jXwpS|A;D?%BP*6h+g-` zx1ZNYKIm=_P*Bs*2@sM1X{k1E{3d literal 0 HcmV?d00001 diff --git a/paths.chor b/paths.chor index b1758313..16258b2d 100644 --- a/paths.chor +++ b/paths.chor @@ -13,1443 +13,174 @@ "wheelRadius": 0.0500888 }, "paths": { - "SourceLanePHGFSprint": { + "SourceLaneTaxi": { "waypoints": [ { - "x": 1.433172345161438, - "y": 2.136303424835205, + "x": 1.468971848487854, + "y": 1.9215065240859985, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 27 + "controlIntervalCount": 14 }, { - "x": 7.841280937194824, - "y": 0.7759228944778442, + "x": 3.468971848487854, + "y": 1.9215065240859985, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.468971848487854, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -6.616666962933087e-41, + "timestamp": 0 }, { - "x": 5.800710201263428, - "y": 1.4382134675979614, - "heading": -0.6435017481374367, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 21 + "x": 1.5097881789229493, + "y": 1.9215065240859983, + "heading": 2.3748244992157107e-19, + "angularVelocity": 2.455567735557075e-18, + "velocityX": 0.42198942031848213, + "velocityY": 1.4827190365964012e-17, + "timestamp": 0.09672358706123588 }, { - "x": 7.948679447174072, - "y": 2.45849871635437, - "heading": 0.2914571610744201, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 + "x": 1.5914208389649156, + "y": 1.9215065240859985, + "heading": 6.823321419293863e-18, + "angularVelocity": 6.808983664986858e-17, + "velocityX": 0.843978832074171, + "velocityY": 2.965467782674968e-17, + "timestamp": 0.19344717412247175 }, { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 + "x": 1.7138698274174293, + "y": 1.9215065240859985, + "heading": 1.3350406511427553e-17, + "angularVelocity": 6.748261205621051e-17, + "velocityX": 1.2659682314613812, + "velocityY": 4.448259121719168e-17, + "timestamp": 0.29017076118370766 }, { - "x": 7.87708044052124, - "y": 4.087375640869141, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 + "x": 1.8771351424005533, + "y": 1.9215065240859985, + "heading": 2.3094752603641018e-17, + "angularVelocity": 1.0074517896422799e-16, + "velocityX": 1.6879576114124106, + "velocityY": 5.931116840042e-17, + "timestamp": 0.3868943482449435 }, { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 20 + "x": 2.081216780530401, + "y": 1.9215065240859985, + "heading": 2.7994689255823954e-17, + "angularVelocity": 5.0660149601833395e-17, + "velocityX": 2.1099469563783173, + "velocityY": 7.414094299456716e-17, + "timestamp": 0.48361793530617936 }, { - "x": 7.80548095703125, - "y": 2.7090952396392822, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ + "x": 2.326114733911239, + "y": 1.9215065240859985, + "heading": 3.194288143588596e-17, + "angularVelocity": 4.082043898429076e-17, + "velocityX": 2.531936219712287, + "velocityY": 8.89740008515513e-17, + "timestamp": 0.5803415223674152 + }, { - "x": 1.433172345161438, - "y": 2.136303424835205, - "heading": 2.8770383139324086e-29, - "angularVelocity": 0, - "velocityX": 1.701446192977867e-30, - "velocityY": -1.266914834653595e-29, - "timestamp": 0 + "x": 2.6118289630644647, + "y": 1.9215065240859985, + "heading": 2.800063510694937e-17, + "angularVelocity": -4.0776590509092634e-17, + "velocityX": 2.953925074887259, + "velocityY": 1.036106690795216e-16, + "timestamp": 0.6770651094286511 }, { - "x": 1.4678872089966617, - "y": 2.127700564422531, - "heading": 0.004117315336523342, - "angularVelocity": 0.04300890670031375, - "velocityX": 0.3626266675757136, - "velocityY": -0.08986429034885272, - "timestamp": 0.09573169030115974 - }, - { - "x": 1.537317326749348, - "y": 2.110494823513309, - "heading": 0.0122988440120433, - "angularVelocity": 0.0854631172768069, - "velocityX": 0.7252574098965585, - "velocityY": -0.17972879049106513, - "timestamp": 0.1914633806023195 - }, - { - "x": 1.6414631627627712, - "y": 2.0846861763424815, - "heading": 0.024480626754227467, - "angularVelocity": 0.1272492181380779, - "velocityX": 1.0878930026830405, - "velocityY": -0.26959355977151667, - "timestamp": 0.28719507090347923 - }, - { - "x": 1.780325283643077, - "y": 2.0502745948695433, - "heading": 0.04058342242477801, - "angularVelocity": 0.16820757702525263, - "velocityX": 1.450534514158978, - "velocityY": -0.35945862195920303, - "timestamp": 0.382926761204639 - }, - { - "x": 1.9539044005410915, - "y": 2.0072600541327383, - "heading": 0.060505710064861905, - "angularVelocity": 0.20810546201361826, - "velocityX": 1.8131834542181169, - "velocityY": -0.44932394489394517, - "timestamp": 0.4786584515057987 - }, - { - "x": 2.1622014359513058, - "y": 1.9556425412505571, - "heading": 0.08411192731013951, - "angularVelocity": 0.2465872812881091, - "velocityX": 2.175842030527155, - "velocityY": -0.5391894023871501, - "timestamp": 0.5743901418069585 - }, - { - "x": 2.4052176349352066, - "y": 1.8954220721835344, - "heading": 0.11121111812581518, - "angularVelocity": 0.28307440022634617, - "velocityX": 2.5385136125763252, - "velocityY": -0.6290546931588193, - "timestamp": 0.6701218321081182 - }, - { - "x": 2.682954761527793, - "y": 1.8265987279247267, - "heading": 0.1415138814765405, - "angularVelocity": 0.3165384759644085, - "velocityX": 2.9012036214911956, - "velocityY": -0.7189191378882197, - "timestamp": 0.765853522409278 - }, - { - "x": 2.9954154621622653, - "y": 1.749172751537101, - "heading": 0.17453107861154096, - "angularVelocity": 0.3448930759444131, - "velocityX": 3.2639212746813424, - "velocityY": -0.8087810436154711, - "timestamp": 0.8615852127104378 - }, - { - "x": 3.3426038443567645, - "y": 1.6631449161907195, - "heading": 0.20926651570903568, - "angularVelocity": 0.36284157300443215, - "velocityX": 3.6266818344380534, - "velocityY": -0.8986348729151596, - "timestamp": 0.9573169030115976 - }, - { - "x": 3.724521896188089, - "y": 1.5685193860859707, - "heading": 0.24264187046069935, - "angularVelocity": 0.3486343408669774, - "velocityX": 3.989463161370044, - "velocityY": -0.9884452035533644, - "timestamp": 1.0530485933127574 - }, - { - "x": 4.117233745301501, - "y": 1.4711990601083857, - "heading": 0.24264187604727303, - "angularVelocity": 5.835657624563612e-8, - "velocityX": 4.102213675301073, - "velocityY": -1.0165946686272007, - "timestamp": 1.1487802836139172 - }, - { - "x": 4.509945594349689, - "y": 1.373878733867449, - "heading": 0.24264188163378278, - "angularVelocity": 5.835590832713292e-8, - "velocityX": 4.102213674619734, - "velocityY": -1.0165946713781402, - "timestamp": 1.244511973915077 - }, - { - "x": 4.902657443397878, - "y": 1.27655840762652, - "heading": 0.24264188722029242, - "angularVelocity": 5.835590717483892e-8, - "velocityX": 4.102213674619755, - "velocityY": -1.0165946713780565, - "timestamp": 1.3402436642162368 - }, - { - "x": 5.295369292588233, - "y": 1.1792380819592594, - "heading": 0.24264189280680207, - "angularVelocity": 5.8355907494596246e-8, - "velocityX": 4.102213676104793, - "velocityY": -1.0165946653856088, - "timestamp": 1.4359753545173966 - }, - { - "x": 5.688086680864775, - "y": 1.0819401068348615, - "heading": 0.242641898394392, - "angularVelocity": 5.83671917480604e-8, - "velocityX": 4.102271536518705, - "velocityY": -1.0163611951659655, - "timestamp": 1.5317070448185564 - }, - { - "x": 6.057380727938785, - "y": 1.0111000403294017, - "heading": 0.24430232120402645, - "angularVelocity": 0.017344547115249777, - "velocityX": 3.8575945532093883, - "velocityY": -0.7399855394762825, - "timestamp": 1.6274387351197162 - }, - { - "x": 6.391965385523026, - "y": 0.9488653299205841, - "heading": 0.24119901543014724, - "angularVelocity": -0.03241670299487622, - "velocityX": 3.495025069879339, - "velocityY": -0.6500951795986862, - "timestamp": 1.723170425420876 - }, - { - "x": 6.691840373882702, - "y": 0.895234975869734, - "heading": 0.2333347965847716, - "angularVelocity": -0.08214854265046012, - "velocityX": 3.1324526644833015, - "velocityY": -0.5602152628325792, - "timestamp": 1.8189021157220358 - }, - { - "x": 6.957005576380099, - "y": 0.8502086242600702, - "heading": 0.2207147600128324, - "angularVelocity": -0.1318271570356455, - "velocityX": 2.7698790407075005, - "velocityY": -0.4703390430254154, - "timestamp": 1.9146338060231956 - }, - { - "x": 7.18746090816696, - "y": 0.8137860741035156, - "heading": 0.20334602215443712, - "angularVelocity": -0.18143143407058088, - "velocityX": 2.4073045306193745, - "velocityY": -0.38046492270850996, - "timestamp": 2.010365496324355 - }, - { - "x": 7.383206290326008, - "y": 0.7859671775168647, - "heading": 0.18123751114726283, - "angularVelocity": -0.2309424490133121, - "velocityX": 2.0447291961727903, - "velocityY": -0.29059234715707755, - "timestamp": 2.1060971866255147 - }, - { - "x": 7.544241641769053, - "y": 0.7667518051986745, - "heading": 0.15439971280375697, - "angularVelocity": -0.28034393060874496, - "velocityX": 1.6821530146905204, - "velocityY": -0.2007211222692816, - "timestamp": 2.2018288769266743 - }, - { - "x": 7.670566876398333, - "y": 0.7561398304498085, - "heading": 0.12284435298340343, - "angularVelocity": -0.32962292549710004, - "velocityX": 1.31957593384315, - "velocityY": -0.11085122086217061, - "timestamp": 2.297560567227834 - }, - { - "x": 7.762181902442319, - "y": 0.7541311201311741, - "heading": 0.08658400694043104, - "angularVelocity": -0.37877056101837964, - "velocityX": 0.9569978943632882, - "velocityY": -0.020982710206882894, - "timestamp": 2.3932922575289934 - }, - { - "x": 7.81908662296772, - "y": 0.7607255287810747, - "heading": 0.045631629218029345, - "angularVelocity": -0.42778287517258756, - "velocityX": 0.5944188423317792, - "velocityY": 0.06888428097620071, - "timestamp": 2.489023947830153 + "x": 2.8567269164453046, + "y": 1.9215065240859985, + "heading": 2.2632732501440496e-17, + "angularVelocity": -5.54925246223202e-17, + "velocityX": 2.531936219712306, + "velocityY": 8.894540117079268e-17, + "timestamp": 0.7737886964898869 }, { - "x": 7.841280937194824, - "y": 0.7759228944778442, - "heading": 0, - "angularVelocity": -0.4766616892938097, - "velocityX": 0.2318387375694728, - "velocityY": 0.15874958074840423, - "timestamp": 2.5847556381313126 - }, - { - "x": 7.835403168803218, - "y": 0.7947828328368709, - "heading": -0.04166817387070667, - "angularVelocity": -0.5176676769900772, - "velocityX": -0.0730228955314616, - "velocityY": 0.2343078558429211, - "timestamp": 2.6652477679723754 - }, - { - "x": 7.804986392936784, - "y": 0.8197245003639376, - "heading": -0.0866323351495184, - "angularVelocity": -0.5586156232617521, - "velocityX": -0.3778850916668647, - "velocityY": 0.30986467339008555, - "timestamp": 2.745739897813438 - }, - { - "x": 7.750030563561768, - "y": 0.8507477655512394, - "heading": -0.13488866933437027, - "angularVelocity": -0.5995161797814921, - "velocityX": -0.6827478597060769, - "velocityY": 0.38541985714018384, - "timestamp": 2.826232027654501 - }, - { - "x": 7.6705356351825085, - "y": 0.8878524818601358, - "heading": -0.18643456038885645, - "angularVelocity": -0.640384235777116, - "velocityX": -0.9876111929640158, - "velocityY": 0.4609732203565098, - "timestamp": 2.906724157495564 - }, - { - "x": 7.5665015641225395, - "y": 0.9310384864662067, - "heading": -0.24126895121607964, - "angularVelocity": -0.6812391588502915, - "velocityX": -1.292475068839824, - "velocityY": 0.5365245607114574, - "timestamp": 2.9872162873366266 - }, - { - "x": 7.437928309633229, - "y": 0.9803055976729939, - "heading": -0.29939271538639584, - "angularVelocity": -0.7221049347901444, - "velocityX": -1.5973394509593273, - "velocityY": 0.6120736437468451, - "timestamp": 3.0677084171776894 - }, - { - "x": 7.284815834214756, - "y": 1.0356536080910796, - "heading": -0.36080902567537027, - "angularVelocity": -0.7630101279446713, - "velocityX": -1.9022042989606411, - "velocityY": 0.6876201502642577, - "timestamp": 3.148200547018752 - }, - { - "x": 7.107164100687513, - "y": 1.0970822617404554, - "heading": -0.4255236836274993, - "angularVelocity": -0.8039873970214162, - "velocityX": -2.207069608865607, - "velocityY": 0.763163476594831, - "timestamp": 3.228692676859815 - }, - { - "x": 6.904973048833601, - "y": 1.1645911316931183, - "heading": -0.49354522243909305, - "angularVelocity": -0.8450706789009879, - "velocityX": -2.5119356668921826, - "velocityY": 0.8387014989532872, - "timestamp": 3.309184806700878 - }, - { - "x": 6.684120642089045, - "y": 1.2193164102467027, - "heading": -0.5235123583408667, - "angularVelocity": -0.3722989559393653, - "velocityX": -2.7437764062379517, - "velocityY": 0.6798835943938059, - "timestamp": 3.3896769365419406 - }, - { - "x": 6.487807289631828, - "y": 1.267960625172924, - "heading": -0.5501610284793903, - "angularVelocity": -0.3310717481395217, - "velocityX": -2.4389136285238937, - "velocityY": 0.6043350452407918, - "timestamp": 3.4701690663830034 - }, - { - "x": 6.316033066880454, - "y": 1.310524076323902, - "heading": -0.5734871955472854, - "angularVelocity": -0.2897943825495106, - "velocityX": -2.134049913842353, - "velocityY": 0.5287902213231489, - "timestamp": 3.550661196224066 - }, - { - "x": 6.168797996381023, - "y": 1.3470068919007985, - "heading": -0.5934869971532756, - "angularVelocity": -0.2484690322524527, - "velocityX": -1.8291859190576596, - "velocityY": 0.45324699012227004, - "timestamp": 3.631153326065129 - }, - { - "x": 6.046102088284196, - "y": 1.3774091505678683, - "heading": -0.6101571315963537, - "angularVelocity": -0.20710266302584004, - "velocityX": -1.5243217981654285, - "velocityY": 0.3777047362126793, - "timestamp": 3.7116454559061918 - }, - { - "x": 5.947945348853021, - "y": 1.401730906160479, - "heading": -0.6234950110327551, - "angularVelocity": -0.16570414352854168, - "velocityX": -1.219457599463783, - "velocityY": 0.30216315113081255, - "timestamp": 3.7921375857472546 - }, - { - "x": 5.874327783550542, - "y": 1.4199721961221465, - "heading": -0.6334988572334701, - "angularVelocity": -0.12428353207005839, - "velocityX": -0.914593332891598, - "velocityY": 0.2266220312312794, - "timestamp": 3.8726297155883174 - }, - { - "x": 5.825249398499615, - "y": 1.4321330452450582, - "heading": -0.6401677660826286, - "angularVelocity": -0.08285168826039573, - "velocityX": -0.6097289902516339, - "velocityY": 0.15108121933934396, - "timestamp": 3.95312184542938 - }, - { - "x": 5.800710201263428, - "y": 1.4382134675979614, - "heading": -0.6435017481374367, - "angularVelocity": -0.04141997560898668, - "velocityX": -0.30486455364984805, - "velocityY": 0.07554058223155366, - "timestamp": 4.033613975270443 - }, - { - "x": 5.800710201263428, - "y": 1.4382134675979614, - "heading": -0.6435017481374367, - "angularVelocity": -1.8014229783540224e-27, - "velocityX": -3.376915136198235e-29, - "velocityY": 5.736032048058663e-30, - "timestamp": 4.114106105111506 - }, - { - "x": 5.818547555700618, - "y": 1.4390540038628774, - "heading": -0.6376275065659812, - "angularVelocity": 0.08671691701318579, - "velocityX": 0.26331916480604495, - "velocityY": 0.012408191363030088, - "timestamp": 4.1818465388559645 - }, - { - "x": 5.854212795257727, - "y": 1.4409016567123936, - "heading": -0.6258375764635414, - "angularVelocity": 0.1740456836594175, - "velocityX": 0.5264985413534782, - "velocityY": 0.0272754800550328, - "timestamp": 4.249586972600423 - }, - { - "x": 5.907692159338857, - "y": 1.443961759263086, - "heading": -0.6080852705803181, - "angularVelocity": 0.2620636583195098, - "velocityX": 0.7894747807915273, - "velocityY": 0.04517394385510083, - "timestamp": 4.317327406344882 - }, - { - "x": 5.978964930251624, - "y": 1.4484936039085805, - "heading": -0.5843154945280162, - "angularVelocity": 0.3508949491225589, - "velocityX": 1.0521451808477307, - "velocityY": 0.06690014213062502, - "timestamp": 4.385067840089341 - }, - { - "x": 6.067998493941586, - "y": 1.4548346984588374, - "heading": -0.5544599081943509, - "angularVelocity": 0.44073509251935744, - "velocityX": 1.31433412466517, - "velocityY": 0.09360870900499038, - "timestamp": 4.452808273833799 - }, - { - "x": 6.174738434598293, - "y": 1.4634418593409828, - "heading": -0.5184292628909575, - "angularVelocity": 0.5318927457611795, - "velocityX": 1.575719769663251, - "velocityY": 0.12706090596665875, - "timestamp": 4.520548707578258 - }, - { - "x": 6.2990864932887405, - "y": 1.4749664049356752, - "heading": -0.47610067620246677, - "angularVelocity": 0.6248644177297312, - "velocityX": 1.8356548934943828, - "velocityY": 0.1701280159818153, - "timestamp": 4.588289141322717 - }, - { - "x": 6.440844047185778, - "y": 1.490406322856265, - "heading": -0.42729541631250667, - "angularVelocity": 0.7204745702407376, - "velocityX": 2.0926579010668633, - "velocityY": 0.22792765069729165, - "timestamp": 4.656029575067175 - }, - { - "x": 6.59953463470863, - "y": 1.5114574086941215, - "heading": -0.371739955153997, - "angularVelocity": 0.8201226075416767, - "velocityX": 2.3426272722358106, - "velocityY": 0.31076101338926715, - "timestamp": 4.723770008811634 - }, - { - "x": 6.773654182639981, - "y": 1.5414451590022467, - "heading": -0.3090282649921381, - "angularVelocity": 0.9257645204698558, - "velocityX": 2.570393165597269, - "velocityY": 0.4426861277748774, - "timestamp": 4.791510442556093 - }, - { - "x": 6.956422392745004, - "y": 1.5870356897360083, - "heading": -0.23940243254396396, - "angularVelocity": 1.027832693112474, - "velocityX": 2.698066723258537, - "velocityY": 0.6730179925588539, - "timestamp": 4.8592508763005515 - }, - { - "x": 7.13098273232138, - "y": 1.6485480325873274, - "heading": -0.1686125340995527, - "angularVelocity": 1.0450169054342962, - "velocityX": 2.576900234133126, - "velocityY": 0.9080594772003634, - "timestamp": 4.92699131004501 - }, - { - "x": 7.290659982684113, - "y": 1.7200152367861081, - "heading": -0.1002049626024793, - "angularVelocity": 1.0098484422926985, - "velocityX": 2.357192618002618, - "velocityY": 1.0550154501280693, - "timestamp": 4.994731743789469 - }, - { - "x": 7.433825813884328, - "y": 1.798376978611666, - "heading": -0.035376660914373755, - "angularVelocity": 0.9570104309142923, - "velocityX": 2.1134472173633743, - "velocityY": 1.1567942142379297, - "timestamp": 5.062472177533928 - }, - { - "x": 7.559908602192434, - "y": 1.882036919563645, - "heading": 0.025328755206427432, - "angularVelocity": 0.8961474375821659, - "velocityX": 1.8612633746004037, - "velocityY": 1.235007459024761, - "timestamp": 5.130212611278386 - }, - { - "x": 7.6686462166126725, - "y": 1.9700426296704405, - "heading": 0.08160707257360432, - "angularVelocity": 0.8307935786103601, - "velocityX": 1.6052098932586671, - "velocityY": 1.2991607115889543, - "timestamp": 5.197953045022845 - }, - { - "x": 7.759896555956456, - "y": 2.061767068594048, - "heading": 0.13326449164713097, - "angularVelocity": 0.762578805863526, - "velocityX": 1.347058681200838, - "velocityY": 1.354057449198286, - "timestamp": 5.265693478767304 - }, - { - "x": 7.833573521261895, - "y": 2.156767827393036, - "heading": 0.1801664809201526, - "angularVelocity": 0.6923780477986421, - "velocityX": 1.0876364562910028, - "velocityY": 1.4024232433669601, - "timestamp": 5.3334339125117625 - }, - { - "x": 7.8896206787548335, - "y": 2.2547166504175005, - "heading": 0.22221394940579448, - "angularVelocity": 0.6207144855945275, - "velocityX": 0.8273811429133712, - "velocityY": 1.4459432514701964, - "timestamp": 5.401174346256221 - }, - { - "x": 7.9279988186375325, - "y": 2.3553605545122482, - "heading": 0.2593308440003242, - "angularVelocity": 0.547928209827353, - "velocityX": 0.566547005404121, - "velocityY": 1.485728663539606, - "timestamp": 5.46891478000068 - }, - { - "x": 7.948679447174072, - "y": 2.45849871635437, - "heading": 0.2914571610744201, - "angularVelocity": 0.47425614656215387, - "velocityX": 0.30529223675411965, - "velocityY": 1.5225494751214026, - "timestamp": 5.536655213745139 - }, - { - "x": 7.946831051825066, - "y": 2.5911250937356964, - "heading": 0.3237506602012709, - "angularVelocity": 0.38120746378091935, - "velocityX": -0.02181931726540404, - "velocityY": 1.5655833625644262, - "timestamp": 5.621368925293583 - }, - { - "x": 7.917248965751211, - "y": 2.7272096797694707, - "heading": 0.34809099666884863, - "angularVelocity": 0.28732463756659415, - "velocityX": -0.34920068467238213, - "velocityY": 1.6064056638098445, - "timestamp": 5.7060826368420265 - }, - { - "x": 7.859906635586575, - "y": 2.8665210553288647, - "heading": 0.3644006813152481, - "angularVelocity": 0.19252709329200723, - "velocityX": -0.6768955003446485, - "velocityY": 1.6444961861897385, - "timestamp": 5.7907963483904705 - }, - { - "x": 7.774773151306567, - "y": 3.008766263060034, - "heading": 0.37259167904917256, - "angularVelocity": 0.09669034190811442, - "velocityX": -1.004955192304655, - "velocityY": 1.679128503888361, - "timestamp": 5.8755100599389145 - }, - { - "x": 7.6618127476379465, - "y": 3.1535627303797917, - "heading": 0.3725589270974435, - "angularVelocity": -0.00038661925124551516, - "velocityX": -1.333437074162696, - "velocityY": 1.7092447571129605, - "timestamp": 5.9602237714873585 - }, - { - "x": 7.520985197062152, - "y": 3.300390177076741, - "heading": 0.3641689647090754, - "angularVelocity": -0.09903901310675314, - "velocityX": -1.6623938203352226, - "velocityY": 1.7332193810559793, - "timestamp": 6.0449374830358025 - }, - { - "x": 7.352249696631812, - "y": 3.448501059595182, - "heading": 0.34723906580696273, - "angularVelocity": -0.19984839045130479, - "velocityX": -1.991832223451176, - "velocityY": 1.748369653639153, - "timestamp": 6.1296511945842465 - }, - { - "x": 7.155582548717868, - "y": 3.5967336644018815, - "heading": 0.32149579845629134, - "angularVelocity": -0.30388548536148036, - "velocityX": -2.3215503643879303, - "velocityY": 1.7498065200688502, - "timestamp": 6.21436490613269 - }, - { - "x": 6.931058620807543, - "y": 3.7430527877883204, - "heading": 0.28648334175757967, - "angularVelocity": -0.4133033018945168, - "velocityX": -2.6503847347300935, - "velocityY": 1.7272188965863609, - "timestamp": 6.299078617681134 - }, - { - "x": 6.679342452608786, - "y": 3.8831106231055927, - "heading": 0.24134794483523736, - "angularVelocity": -0.5327991903238861, - "velocityX": -2.9713745696859344, - "velocityY": 1.6533077438966797, - "timestamp": 6.383792329229578 - }, - { - "x": 6.406788011208051, - "y": 4.004645601729296, - "heading": 0.18499930261907258, - "angularVelocity": -0.6651655462403078, - "velocityX": -3.2173592257833326, - "velocityY": 1.4346553397581023, - "timestamp": 6.468506040778022 - }, - { - "x": 6.145744610358307, - "y": 4.100645232567065, - "heading": 0.1258988121859892, - "angularVelocity": -0.6976496408056244, - "velocityX": -3.0814775563276378, - "velocityY": 1.1332242335158502, - "timestamp": 6.553219752326466 - }, - { - "x": 5.907416819572644, - "y": 4.18026386145866, - "heading": 0.07010067718233673, - "angularVelocity": -0.6586671033973509, - "velocityX": -2.8133319439011144, - "velocityY": 0.9398552776909607, - "timestamp": 6.63793346387491 - }, - { - "x": 5.694140881693529, - "y": 4.247396241924432, - "heading": 0.01923390264073781, - "angularVelocity": -0.6004550339234085, - "velocityX": -2.517608235794895, - "velocityY": 0.7924618015040228, - "timestamp": 6.722647175423354 - }, - { - "x": 5.50676824724975, - "y": 4.303942898252954, - "heading": -0.02601095760308067, - "angularVelocity": -0.5340913462154806, - "velocityX": -2.2118336101544562, - "velocityY": 0.6675029967986442, - "timestamp": 6.807360886971798 - }, - { - "x": 5.345718150174197, - "y": 4.351008976671829, - "heading": -0.06525584873836304, - "angularVelocity": -0.4632649239177752, - "velocityX": -1.901110152439194, - "velocityY": 0.5555898514960005, - "timestamp": 6.892074598520242 - }, - { - "x": 5.21123496648668, - "y": 4.389313273102533, - "heading": -0.09826172817354481, - "angularVelocity": -0.38961673183575596, - "velocityX": -1.5875019666752757, - "velocityY": 0.45216170712576903, - "timestamp": 6.976788310068686 - }, - { - "x": 5.103477083411047, - "y": 4.419359601001064, - "heading": -0.1248629898593969, - "angularVelocity": -0.3140136490258714, - "velocityX": -1.2720241045513758, - "velocityY": 0.3546808108076878, - "timestamp": 7.06150202161713 - }, - { - "x": 5.022554847546882, - "y": 4.44152029736655, - "heading": -0.14493751186900491, - "angularVelocity": -0.23696898226597282, - "velocityX": -0.9552436599108176, - "velocityY": 0.26159515337505695, - "timestamp": 7.146215733165574 - }, - { - "x": 4.968549251002919, - "y": 4.456081570178744, - "heading": -0.15839111246886314, - "angularVelocity": -0.15881255057706564, - "velocityX": -0.6375071467985308, - "velocityY": 0.17188802787689772, - "timestamp": 7.230929444714018 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": -0.07976943545791065, - "velocityX": -0.3190407913838158, - "velocityY": 0.08485777647782772, - "timestamp": 7.315643156262462 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": -5.429647481375564e-36, - "velocityX": -2.0130702505219768e-38, - "velocityY": 9.290884594011316e-38, - "timestamp": 7.400356867810906 - }, - { - "x": 4.970877788406757, - "y": 4.459511277906968, - "heading": -0.16349815793624287, - "angularVelocity": 0.018956158928001787, - "velocityX": 0.33714881644675093, - "velocityY": -0.04317094482113791, - "timestamp": 7.487427222340686 - }, - { - "x": 5.029589121543177, - "y": 4.45199345668782, - "heading": -0.16019712382497864, - "angularVelocity": 0.03791226220556215, - "velocityX": 0.6742976234964045, - "velocityY": -0.0863419157961063, - "timestamp": 7.574497576870465 - }, - { - "x": 5.117656119781506, - "y": 4.440716720346877, - "heading": -0.15524559648557837, - "angularVelocity": 0.05686811965037711, - "velocityX": 1.0114464184042, - "velocityY": -0.12951292551686924, - "timestamp": 7.661567931400245 - }, - { - "x": 5.235078781735307, - "y": 4.425681064380957, - "heading": -0.14864360395133913, - "angularVelocity": 0.0758236551337492, - "velocityX": 1.3485951973888195, - "velocityY": -0.17268398695652012, - "timestamp": 7.748638285930024 - }, - { - "x": 5.381857105533103, - "y": 4.406886483098539, - "heading": -0.14039116676025787, - "angularVelocity": 0.09477895473893824, - "velocityX": 1.6857439548795636, - "velocityY": -0.21585511376308925, - "timestamp": 7.835708640459804 - }, - { - "x": 5.557991088523465, - "y": 4.3843329695031, - "heading": -0.13048827839881783, - "angularVelocity": 0.11373432915164189, - "velocityX": 2.0228926819187647, - "velocityY": -0.25902632092448025, - "timestamp": 7.922778994989583 - }, - { - "x": 5.763480726622518, - "y": 4.3580205150215345, - "heading": -0.11893487664373759, - "angularVelocity": 0.13269041819657243, - "velocityX": 2.360041362054773, - "velocityY": -0.30219762654769566, - "timestamp": 8.009849349519362 - }, - { - "x": 5.998326012444723, - "y": 4.327949108694172, - "heading": -0.10573079137043012, - "angularVelocity": 0.15164846111648134, - "velocityX": 2.697189957368156, - "velocityY": -0.34536905804233253, - "timestamp": 8.09691970404914 - }, - { - "x": 6.262526926435642, - "y": 4.2941187332410315, - "heading": -0.09087557162555815, - "angularVelocity": 0.17061168321981837, - "velocityX": 3.034338328099486, - "velocityY": -0.38854068799696395, - "timestamp": 8.18399005857892 - }, - { - "x": 6.556082683402125, - "y": 4.256529028141766, - "heading": -0.0743557876801526, - "angularVelocity": 0.18972914529428694, - "velocityX": 3.371477680914724, - "velocityY": -0.4317164585152746, - "timestamp": 8.271060413108698 - }, - { - "x": 6.820282808642692, - "y": 4.222698267435462, - "heading": -0.05948639298755931, - "angularVelocity": 0.17077448200245657, - "velocityX": 3.0343292693290502, - "velocityY": -0.3885451126161881, - "timestamp": 8.358130767638476 - }, - { - "x": 7.055127288351479, - "y": 4.192626473165152, - "heading": -0.04626808716200777, - "angularVelocity": 0.15181178366548068, - "velocityX": 2.6971806991835345, - "velocityY": -0.3453735135536204, - "timestamp": 8.445201122168255 - }, - { - "x": 7.260616119038252, - "y": 4.166313656843632, - "heading": -0.034701331761684315, - "angularVelocity": 0.13284378434875296, - "velocityX": 2.360032088952761, - "velocityY": -0.3022017822670142, - "timestamp": 8.532271476698034 - }, - { - "x": 7.436749299548261, - "y": 4.1437598266696405, - "heading": -0.02478650882076325, - "angularVelocity": 0.11387139738279192, - "velocityX": 2.022883465459762, - "velocityY": -0.25902995681816454, - "timestamp": 8.619341831227812 - }, - { - "x": 7.583526829501114, - "y": 4.124964988926465, - "heading": -0.016523936768266155, - "angularVelocity": 0.09489535327056917, - "velocityX": 1.68573483759795, - "velocityY": -0.2158580592059934, - "timestamp": 8.706412185757591 - }, - { - "x": 7.70094870876185, - "y": 4.10992914844881, - "heading": -0.009913866219354965, - "angularVelocity": 0.07591643085190908, - "velocityX": 1.3485862081861149, - "velocityY": -0.1726861060673939, - "timestamp": 8.79348254028737 - }, - { - "x": 7.789014937206106, - "y": 4.09865230882672, - "heading": -0.004956472404176458, - "angularVelocity": 0.05693549592109463, - "velocityX": 1.011437577345994, - "velocityY": -0.12951411169725838, - "timestamp": 8.880552894817148 - }, - { - "x": 7.847725514598055, - "y": 4.091134472510321, - "heading": -0.0016518483286155007, - "angularVelocity": 0.03795349282092027, - "velocityX": 0.6742889437973931, - "velocityY": -0.08634208918752465, - "timestamp": 8.967623249346927 - }, - { - "x": 7.87708044052124, - "y": 4.087375640869141, - "heading": 7.60993628191321e-37, - "angularVelocity": 0.01897142072679338, - "velocityX": 0.3371403054657971, - "velocityY": -0.043170050948797965, - "timestamp": 9.054693603876705 - }, - { - "x": 7.87366387419082, - "y": 4.087813222475328, - "heading": -0.0001932265358713677, - "angularVelocity": -0.002007825984855413, - "velocityX": -0.035501700769334996, - "velocityY": 0.004546930965948054, - "timestamp": 9.150930298355625 - }, - { - "x": 7.834385473616069, - "y": 4.092842926972847, - "heading": -0.0024054804575218995, - "angularVelocity": -0.02298763412052877, - "velocityX": -0.4081437001491553, - "velocityY": 0.05226389502209074, - "timestamp": 9.247166992834545 - }, - { - "x": 7.759245239147118, - "y": 4.102464751168547, - "heading": -0.006636690667772024, - "angularVelocity": -0.04396670348207906, - "velocityX": -0.780785695890768, - "velocityY": 0.09998082589804004, - "timestamp": 9.343403687313465 - }, - { - "x": 7.6482431709074055, - "y": 4.116678690374747, - "heading": -0.012886671317606013, - "angularVelocity": -0.06494384167780186, - "velocityX": -1.153427690349718, - "velocityY": 0.14769770806408986, - "timestamp": 9.439640381792385 - }, - { - "x": 7.501379268940608, - "y": 4.135484738377977, - "heading": -0.02115513791608259, - "angularVelocity": -0.08591802371482843, - "velocityX": -1.526069684354818, - "velocityY": 0.19541452566566683, - "timestamp": 9.535877076271305 - }, - { - "x": 7.318653533488854, - "y": 4.158882887388937, - "heading": -0.031441729230877964, - "angularVelocity": -0.10688845216985864, - "velocityX": -1.8987116758439624, - "velocityY": 0.24313126232826374, - "timestamp": 9.632113770750225 - }, - { - "x": 7.100065965603603, - "y": 4.1868731279492994, - "heading": -0.04374603503260887, - "angularVelocity": -0.12785461791215239, - "velocityX": -2.2713536564073333, - "velocityY": 0.2908479007089456, - "timestamp": 9.728350465229145 - }, - { - "x": 6.845616568893901, - "y": 4.219455448693913, - "heading": -0.05806762969505058, - "angularVelocity": -0.14881636095240902, - "velocityX": -2.643995599468943, - "velocityY": 0.3385644209938051, - "timestamp": 9.824587159708065 - }, - { - "x": 6.555305358042951, - "y": 4.256629835248091, - "heading": -0.07440611137609496, - "angularVelocity": -0.16977392843249786, - "velocityX": -3.016637389956695, - "velocityY": 0.38628079191062686, - "timestamp": 9.920823854186985 - }, - { - "x": 6.23254875309289, - "y": 4.297957965433844, - "heading": -0.09255419315316445, - "angularVelocity": -0.18857756779088714, - "velocityX": -3.353778999763519, - "velocityY": 0.4294425365451959, - "timestamp": 10.017060548665905 - }, - { - "x": 5.945653966420611, - "y": 4.334694055593217, - "heading": -0.10868589302504747, - "angularVelocity": -0.16762524896796518, - "velocityX": -2.981137166292844, - "velocityY": 0.3817264335426585, - "timestamp": 10.113297243144824 - }, - { - "x": 5.694621016414327, - "y": 4.366838114244224, - "heading": -0.12280128203638066, - "angularVelocity": -0.14667366837319035, - "velocityX": -2.6084951417493896, - "velocityY": 0.3340104190512136, - "timestamp": 10.209533937623744 - }, - { - "x": 5.479449909856628, - "y": 4.394390149838104, - "heading": -0.13490034942614496, - "angularVelocity": -0.12572197596016224, - "velocityX": -2.2358530467277298, - "velocityY": 0.28629449237696436, - "timestamp": 10.305770632102664 - }, - { - "x": 5.300140650438581, - "y": 4.417350169666037, - "heading": -0.1449830377404426, - "angularVelocity": -0.10476968654099145, - "velocityX": -1.863210913351998, - "velocityY": 0.23857864146571758, - "timestamp": 10.402007326581584 - }, - { - "x": 5.156693240503607, - "y": 4.435718179635196, - "heading": -0.1530492718346854, - "angularVelocity": -0.08381661629088484, - "velocityX": -1.4905687556256995, - "velocityY": 0.19086285193619654, - "timestamp": 10.498244021060504 - }, - { - "x": 5.04910768165416, - "y": 4.449494184192496, - "heading": -0.1590989820260221, - "angularVelocity": -0.06286282196300745, - "velocityX": -1.117926581248191, - "velocityY": 0.14314710861477242, - "timestamp": 10.594480715539424 - }, - { - "x": 4.97738397502577, - "y": 4.458678186291814, - "heading": -0.16313212141892855, - "angularVelocity": -0.0419085403415415, - "velocityX": -0.7452843950713719, - "velocityY": 0.09543139598720737, - "timestamp": 10.690717410018344 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": -0.020954127805584536, - "velocityX": -0.37264220046732904, - "velocityY": 0.047715698372427014, - "timestamp": 10.786954104497264 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": 0, - "velocityX": 6.569525640446563e-39, - "velocityY": 1.8902971795210484e-36, - "timestamp": 10.883190798976184 - }, - { - "x": 4.970161732141451, - "y": 4.445728451981636, - "heading": -0.16350561334854938, - "angularVelocity": 0.017714526683228992, - "velocityX": 0.3087750250459319, - "velocityY": -0.18912442074741204, - "timestamp": 10.975943153891373 - }, - { - "x": 5.027440952562264, - "y": 4.4106449800780645, - "heading": -0.1602193291584108, - "angularVelocity": 0.03543073588960003, - "velocityX": 0.6175500392759685, - "velocityY": -0.378248853472785, - "timestamp": 11.068695508806563 - }, - { - "x": 5.113359780805984, - "y": 4.358019770281013, - "heading": -0.1552893983395244, - "angularVelocity": 0.053151543412500564, - "velocityX": 0.9263250331733618, - "velocityY": -0.5673733011433544, - "timestamp": 11.161447863721753 - }, - { - "x": 5.227918214012291, - "y": 4.287852820966911, - "heading": -0.14871510376483507, - "angularVelocity": 0.07088008256718281, - "velocityX": 1.2350999962325, - "velocityY": -0.756497766318278, - "timestamp": 11.254200218636942 - }, - { - "x": 5.371116248190762, - "y": 4.200144130331831, - "heading": -0.1404954056786583, - "angularVelocity": 0.0886198317411194, - "velocityX": 1.5438749162693188, - "velocityY": -0.9456222509420716, - "timestamp": 11.346952573552132 - }, - { - "x": 5.5429538779355045, - "y": 4.094893696480259, - "heading": -0.1306288879973164, - "angularVelocity": 0.1063748482759657, - "velocityX": 1.8526497780230582, - "velocityY": -1.1347467560021554, - "timestamp": 11.439704928467322 - }, - { - "x": 5.7434310958415224, - "y": 3.9721015175775665, - "heading": -0.11911365753726154, - "angularVelocity": 0.12415027597503053, - "velocityX": 2.1614245599405897, - "velocityY": -1.3238712808420907, - "timestamp": 11.532457283382511 - }, - { - "x": 5.972547891098849, - "y": 3.83176759216481, - "heading": -0.10594711713280619, - "angularVelocity": 0.1419537047495384, - "velocityX": 2.470199225311589, - "velocityY": -1.5129958214114747, - "timestamp": 11.6252096382977 - }, - { - "x": 6.230304245042205, - "y": 3.673891920053355, - "heading": -0.091125277264113, - "angularVelocity": 0.15980014612292848, - "velocityX": 2.7789736894447628, - "velocityY": -1.702120364014608, - "timestamp": 11.71796199321289 - }, - { - "x": 6.5167001058009015, - "y": 3.498474507164359, - "heading": -0.07463891609789546, - "angularVelocity": 0.17774601174592225, - "velocityX": 3.087747594339454, - "velocityY": -1.8912448427793878, - "timestamp": 11.81071434812808 - }, - { - "x": 6.774456304633716, - "y": 3.340598685286401, - "heading": -0.059730001301582306, - "angularVelocity": 0.16073893552293816, - "velocityX": 2.7789720171363874, - "velocityY": -1.7021219787067958, - "timestamp": 11.90346670304327 - }, - { - "x": 7.003572909925525, - "y": 3.200264603321266, - "heading": -0.04646689079252882, - "angularVelocity": 0.14299486542612352, - "velocityX": 2.4701971772178397, - "velocityY": -1.512997509264889, - "timestamp": 11.99621905795846 - }, - { - "x": 7.204049930227038, - "y": 3.0774722725779653, - "heading": -0.03485583277229418, - "angularVelocity": 0.12518343098513846, - "velocityX": 2.161422429487887, - "velocityY": -1.3238729178960327, - "timestamp": 12.088971412873649 - }, - { - "x": 7.375887370453985, - "y": 2.9722216978386684, - "heading": -0.024899933451460868, - "angularVelocity": 0.107338507253393, - "velocityX": 1.8526477347563841, - "velocityY": -1.1347482749687028, - "timestamp": 12.181723767788839 - }, - { - "x": 7.519085234016694, - "y": 2.884512881951178, - "heading": -0.016601225810711995, - "angularVelocity": 0.0894716651489553, - "velocityX": 1.5438730767929685, - "velocityY": -0.9456236013380901, - "timestamp": 12.274476122704028 - }, - { - "x": 7.633643523301954, - "y": 2.814345826878192, - "heading": -0.009961123896120593, - "angularVelocity": 0.07158957765183391, - "velocityX": 1.235098444562514, - "velocityY": -0.7564989065468437, - "timestamp": 12.367228477619218 - }, - { - "x": 7.7195622398456, - "y": 2.761720534080977, - "heading": -0.004980577858606377, - "angularVelocity": 0.05369724620003789, - "velocityX": 0.92632382889046, - "velocityY": -0.5673741960011052, - "timestamp": 12.459980832534407 - }, - { - "x": 7.776841384411153, - "y": 2.726637004697944, - "heading": -0.0016601403967374856, - "angularVelocity": 0.035798955885325216, - "velocityX": 0.6175492214502578, - "velocityY": -0.37824947318170576, - "timestamp": 12.552733187449597 - }, - { - "x": 7.80548095703125, - "y": 2.7090952396392822, - "heading": 2.2544024955772584e-32, - "angularVelocity": 0.017898633390553583, - "velocityX": 0.3087746143619144, - "velocityY": -0.1891247405491907, - "timestamp": 12.645485542364787 - }, - { - "x": 7.80548095703125, - "y": 2.7090952396392822, - "heading": 1.2738822120559367e-32, - "angularVelocity": -1.0571378529733318e-31, - "velocityX": -6.060794460308241e-34, - "velocityY": -2.7880656703838183e-33, - "timestamp": 12.738237897279976 - } - ], - "constraints": [ + "x": 3.0608085545751536, + "y": 1.9215065240859985, + "heading": 1.7892029047767983e-17, + "angularVelocity": -4.9009641288663106e-17, + "velocityX": 2.109946956378327, + "velocityY": 7.412152260849424e-17, + "timestamp": 0.8705122835511228 + }, { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 3.224073869558278, + "y": 1.9215065240859985, + "heading": 2.083425694040062e-17, + "angularVelocity": 3.042138426747003e-17, + "velocityX": 1.687957611412416, + "velocityY": 5.929781651761968e-17, + "timestamp": 0.9672358706123586 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 3.3465228580107924, + "y": 1.9215065240859985, + "heading": 1.131382345905398e-17, + "angularVelocity": -9.842749204538218e-17, + "velocityX": 1.265968231461384, + "velocityY": 4.4473700308316656e-17, + "timestamp": 1.0639594576735945 }, { - "scope": [ - 2 - ], - "type": "StopPoint" + "x": 3.4281555180527588, + "y": 1.9215065240859983, + "heading": -4.743251660877572e-19, + "angularVelocity": -1.2187342421598578e-16, + "velocityX": 0.8439788320741727, + "velocityY": 2.964930533250184e-17, + "timestamp": 1.1606830447348304 + }, + { + "x": 3.468971848487854, + "y": 1.9215065240859983, + "heading": -3.025400677588882e-41, + "angularVelocity": 4.904504421960316e-18, + "velocityX": 0.42198942031848286, + "velocityY": 1.4824720643168207e-17, + "timestamp": 1.2574066317960664 }, + { + "x": 3.468971848487854, + "y": 1.9215065240859983, + "heading": 4.753747394144705e-41, + "angularVelocity": 8.043397133285877e-40, + "velocityX": -3.5738155514754405e-40, + "velocityY": 2.012790081694559e-42, + "timestamp": 1.3541302188573023 + } + ], + "constraints": [ { "scope": [ - 4 + "first" ], "type": "StopPoint" }, { "scope": [ - 6 + "last" ], "type": "StopPoint" } @@ -1459,2043 +190,162 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "CenterLanePCBADEF": { + "CenterLaneTaxi": { "waypoints": [ { - "x": 1.3078742027282715, - "y": 5.537254810333252, + "x": 1.4490834474563599, + "y": 5.489123821258545, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 14 }, { - "x": 2.542956590652466, - "y": 4.3021721839904785, - "heading": -0.5144511222328438, + "x": 3.3202035427093506, + "y": 5.489123821258545, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 - }, + "controlIntervalCount": 40 + } + ], + "trajectory": [ { - "x": 2.017073631286621, - "y": 5.013424873352051, + "x": 1.44908344745636, + "y": 5.489123821258545, "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 9 + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -2.0318283346913183e-41, + "timestamp": 0 }, { - "x": 2.542956590652466, - "y": 5.519354820251465, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 1.4872695756258638, + "y": 5.489123821258545, + "heading": 2.754950983259061e-24, + "angularVelocity": 2.9447241753048086e-23, + "velocityX": 0.40816653670481284, + "velocityY": 5.876249720718061e-34, + "timestamp": 0.09355526417669109 }, { - "x": 2.578756093978882, - "y": 6.951334476470947, - "heading": 0.643500664804274, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 27 + "x": 1.5636418311637772, + "y": 5.489123821258545, + "heading": 4.052554305374892e-24, + "angularVelocity": 1.3869873668873725e-23, + "velocityX": 0.8163330648468328, + "velocityY": 2.351179297170128e-34, + "timestamp": 0.18711052835338218 }, { - "x": 7.894979953765869, - "y": 7.4704270362854, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 + "x": 1.6782002129129638, + "y": 5.489123821258545, + "heading": -2.072394992605464e-24, + "angularVelocity": -6.546869835666046e-23, + "velocityX": 1.2244995806203742, + "velocityY": -1.9003523177361593e-34, + "timestamp": 0.2806657925300733 }, { - "x": 5.1921186447143555, - "y": 6.503840923309326, - "heading": 0.26299425401034693, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 + "x": 1.8309447190550667, + "y": 5.489123821258545, + "heading": -2.4578798145232008e-23, + "angularVelocity": -2.4056824153148397e-22, + "velocityX": 1.632666076957736, + "velocityY": 3.860759321784845e-34, + "timestamp": 0.37422105670676437 }, { - "x": 7.87708044052124, - "y": 5.859449863433838, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 2.0218753463170436, + "y": 5.489123821258545, + "heading": -7.973182418935717e-23, + "angularVelocity": -5.895236107850817e-22, + "velocityX": 2.0408325383099775, + "velocityY": 1.2704796254278893e-34, + "timestamp": 0.46777632088345544 }, { - "x": 5.585913181304932, - "y": 6.485940456390381, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 + "x": 2.250992087061798, + "y": 5.489123821258545, + "heading": -2.0707507158653019e-22, + "angularVelocity": -1.3611552826942191e-21, + "velocityX": 2.4489989180302882, + "velocityY": -1.7750165055103215e-35, + "timestamp": 0.5613315850601466 }, { - "x": 4.440329551696777, - "y": 6.181644916534424, - "heading": 0.18998779389424572, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 2.5182949031039126, + "y": 5.489123821258545, + "heading": -2.160490651108523e-22, + "angularVelocity": -9.592193197806397e-23, + "velocityX": 2.8571648895916577, + "velocityY": 2.8715559113727715e-34, + "timestamp": 0.6548868492368377 }, { - "x": 5.1563191413879395, - "y": 4.373770713806152, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 18 + "x": 2.747411643848667, + "y": 5.489123821258545, + "heading": -1.6667699647956365e-22, + "angularVelocity": 5.277318832756414e-22, + "velocityX": 2.448998918030288, + "velocityY": 2.947986532686387e-35, + "timestamp": 0.7484421134135288 }, { - "x": 7.984478950500488, - "y": 4.087375164031982, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 + "x": 2.938342271110644, + "y": 5.489123821258545, + "heading": -1.1298656729823459e-22, + "angularVelocity": 5.738899818074355e-22, + "velocityX": 2.0408325383099775, + "velocityY": -2.8596027354894936e-34, + "timestamp": 0.84199737759022 }, { - "x": 5.1563191413879395, - "y": 4.4990692138671875, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 + "x": 3.0910867772527473, + "y": 5.489123821258545, + "heading": -6.615705102278205e-23, + "angularVelocity": 5.00554598927309e-22, + "velocityX": 1.632666076957736, + "velocityY": 3.1581487960600027e-34, + "timestamp": 0.9355526417669111 }, { - "x": 4.010735511779785, - "y": 5.250858306884766, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ + "x": 3.2056451590019335, + "y": 5.489123821258545, + "heading": -3.1148929262698523e-23, + "angularVelocity": 3.7419731471853073e-22, + "velocityX": 1.2244995806203742, + "velocityY": 5.822081879901906e-34, + "timestamp": 1.0291079059436021 + }, { - "x": 1.3078742027282717, - "y": 5.537254810333252, - "heading": 5.4444853034280076e-17, - "angularVelocity": -1.7134882602513402e-15, - "velocityX": 6.870651213375127e-14, - "velocityY": -6.867248083045425e-14, - "timestamp": 0 + "x": 3.2820174145398466, + "y": 5.489123821258545, + "heading": -1.119480593141529e-23, + "angularVelocity": 2.1328705390671606e-22, + "velocityX": 0.8163330648468328, + "velocityY": 1.1078164340378654e-33, + "timestamp": 1.1226631701202932 }, { - "x": 1.327170103173586, - "y": 5.517955423153612, - "heading": -0.008175377183837335, - "angularVelocity": -0.09765213543106893, - "velocityX": 0.23048305187379453, - "velocityY": -0.23052469974457607, - "timestamp": 0.08371938972710291 - }, - { - "x": 1.3657619180934388, - "y": 5.479356717010402, - "heading": -0.024521335360667328, - "angularVelocity": -0.19524698197290347, - "velocityX": 0.4609662713219617, - "velocityY": -0.4610485846711018, - "timestamp": 0.16743877945420582 - }, - { - "x": 1.423649754196992, - "y": 5.421458744466686, - "heading": -0.04902545949966477, - "angularVelocity": -0.2926935351401013, - "velocityX": 0.6914507653752389, - "velocityY": -0.6915718417495008, - "timestamp": 0.25115816918130873 - }, - { - "x": 1.500833817418447, - "y": 5.344261559488766, - "heading": -0.08166832650675512, - "angularVelocity": -0.3899080859702319, - "velocityX": 0.9219377192434074, - "velocityY": -0.9220944542185107, - "timestamp": 0.33487755890841164 - }, - { - "x": 1.5973144196195872, - "y": 5.247765239032123, - "heading": -0.1224243942133242, - "angularVelocity": -0.48681754417250844, - "velocityX": 1.1524283981958594, - "velocityY": -1.1526161474801648, - "timestamp": 0.41859694863551455 - }, - { - "x": 1.71309198631675, - "y": 5.131969908089758, - "heading": -0.17126309032246767, - "angularVelocity": -0.5833618265534611, - "velocityX": 1.3829241597980892, - "velocityY": -1.3831363477423682, - "timestamp": 0.5023163383626175 - }, - { - "x": 1.84816708196567, - "y": 4.996875770190428, - "heading": -0.22814932169203733, - "angularVelocity": -0.6794869331346065, - "velocityX": 1.6134266636345465, - "velocityY": -1.613654116921907, - "timestamp": 0.5860357280897204 - }, - { - "x": 2.0025627636590344, - "y": 4.842495557885179, - "heading": -0.2918324673553054, - "angularVelocity": -0.7606737921866519, - "velocityX": 1.8442045767013158, - "velocityY": -1.8440198000543986, - "timestamp": 0.6697551178168233 - }, - { - "x": 2.137660080489021, - "y": 4.707413614165848, - "heading": -0.34751647944173375, - "angularVelocity": -0.6651268274642137, - "velocityX": 1.6136920881812329, - "velocityY": -1.613508461536256, - "timestamp": 0.7534745075439262 - }, - { - "x": 2.2534584850572914, - "y": 4.591629667252782, - "heading": -0.3952239272052681, - "angularVelocity": -0.569849444902123, - "velocityX": 1.3831730611717787, - "velocityY": -1.3830003693347765, - "timestamp": 0.8371938972710291 - }, - { - "x": 2.3499575912437507, - "y": 4.495143461778056, - "heading": -0.4349689949602177, - "angularVelocity": -0.4747414892118174, - "velocityX": 1.15264942208745, - "velocityY": -1.1524953274174494, - "timestamp": 0.920913286998132 - }, - { - "x": 2.4271571129829432, - "y": 4.417954772578404, - "heading": -0.46676067803032645, - "angularVelocity": -0.3797409796432922, - "velocityX": 0.9221223660473082, - "velocityY": -0.9219929749997143, - "timestamp": 1.004632676725235 - }, - { - "x": 2.485056851699902, - "y": 4.3600634144135135, - "heading": -0.49060405496538323, - "angularVelocity": -0.28480113164677934, - "velocityX": 0.6915929380958452, - "velocityY": -0.691492835215309, - "timestamp": 1.0883520664523378 - }, - { - "x": 2.523656690441612, - "y": 4.321469248073772, - "heading": -0.5065010886126352, - "angularVelocity": -0.1898847292015727, - "velocityX": 0.4610621131799047, - "velocityY": -0.460994358243006, - "timestamp": 1.1720714561794408 - }, - { - "x": 2.5429565906524716, - "y": 4.302172183990473, - "heading": -0.514451122232844, - "angularVelocity": -0.09496048222667672, - "velocityX": 0.23053082773023703, - "velocityY": -0.23049695113880436, - "timestamp": 1.2557908459065437 - }, - { - "x": 2.542956590652466, - "y": 4.3021721839904785, - "heading": -0.5144511222328438, - "angularVelocity": 1.7361053619799633e-15, - "velocityX": -6.870229148895338e-14, - "velocityY": 6.86710519982077e-14, - "timestamp": 1.3395102356336466 - }, - { - "x": 2.5305584868573745, - "y": 4.308773452126341, - "heading": -0.5122554014922367, - "angularVelocity": 0.03659296401704725, - "velocityX": -0.20662161524599887, - "velocityY": 0.11001397531805143, - "timestamp": 1.399514140744592 - }, - { - "x": 2.5059487154966082, - "y": 4.322311826419086, - "heading": -0.50771784412737, - "angularVelocity": 0.07562103427230116, - "velocityX": -0.4101361622258381, - "velocityY": 0.22562488670882788, - "timestamp": 1.4595180458555372 - }, - { - "x": 2.4694009892818585, - "y": 4.343244277958484, - "heading": -0.5006377479710926, - "angularVelocity": 0.11799392294862207, - "velocityX": -0.6090891275688425, - "velocityY": 0.3488514872606191, - "timestamp": 1.5195219509664826 - }, - { - "x": 2.4213500625117965, - "y": 4.372224481954429, - "heading": -0.49072373538892167, - "angularVelocity": 0.16522278948078847, - "velocityX": -0.800796659504371, - "velocityY": 0.48297196561394073, - "timestamp": 1.579525856077428 - }, - { - "x": 2.36256927737863, - "y": 4.410248403268193, - "heading": -0.4775208921950913, - "angularVelocity": 0.2200330656716209, - "velocityX": -0.9796159937337826, - "velocityY": 0.6336907780161798, - "timestamp": 1.6395297611883732 - }, - { - "x": 2.294685044293501, - "y": 4.45893547211586, - "heading": -0.46025532607703995, - "angularVelocity": 0.28774070764440873, - "velocityX": -1.1313302519163222, - "velocityY": 0.8113983374522957, - "timestamp": 1.6995336662993186 - }, - { - "x": 2.2219430126998723, - "y": 4.5207468069818875, - "heading": -0.4375774026440522, - "angularVelocity": 0.3779407922043896, - "velocityX": -1.2122882912225668, - "velocityY": 1.0301218687640459, - "timestamp": 1.759537571410264 - }, - { - "x": 2.1543482605183306, - "y": 4.595554934892757, - "heading": -0.4086588228612356, - "angularVelocity": 0.48194496223782435, - "velocityX": -1.1265058841847213, - "velocityY": 1.2467209887848296, - "timestamp": 1.8195414765212092 - }, - { - "x": 2.099014749930773, - "y": 4.677184514185738, - "heading": -0.3762413759174227, - "angularVelocity": 0.5402556197613143, - "velocityX": -0.9221651571718094, - "velocityY": 1.3604044460448101, - "timestamp": 1.8795453816321546 - }, - { - "x": 2.057477466278187, - "y": 4.7614747474435095, - "heading": -0.3421230482764429, - "angularVelocity": 0.5686017864653305, - "velocityX": -0.692243006114092, - "velocityY": 1.4047457928266707, - "timestamp": 1.9395492867431 - }, - { - "x": 2.029984208014376, - "y": 4.846326580474425, - "heading": -0.30718701619967975, - "angularVelocity": 0.5822293067787422, - "velocityX": -0.4581911496089643, - "velocityY": 1.4141051798883366, - "timestamp": 1.9995531918540452 - }, - { - "x": 2.0165337281988327, - "y": 4.930557757850057, - "heading": -0.2719247119892122, - "angularVelocity": 0.5876668217721605, - "velocityX": -0.2241600740930749, - "velocityY": 1.4037615921812139, - "timestamp": 2.0595570969649906 - }, - { - "x": 2.017073631286621, - "y": 5.013424873352051, - "heading": -0.23664369858950388, - "angularVelocity": 0.58797862129931, - "velocityX": 0.008997799173072052, - "velocityY": 1.3810287071945508, - "timestamp": 2.119561002075936 - }, - { - "x": 2.0456987990727344, - "y": 5.12687213061024, - "heading": -0.1871316885421809, - "angularVelocity": 0.5834829718812528, - "velocityX": 0.3373383135622466, - "velocityY": 1.336939113429588, - "timestamp": 2.2044169660600903 - }, - { - "x": 2.101800262534532, - "y": 5.2343841203738455, - "heading": -0.13890470594146578, - "angularVelocity": 0.5683393404112499, - "velocityX": 0.6611375421092794, - "velocityY": 1.2669939119857474, - "timestamp": 2.2892729300442447 - }, - { - "x": 2.1841232780581077, - "y": 5.331790166507821, - "heading": -0.09372578148319055, - "angularVelocity": 0.5324189642900253, - "velocityX": 0.9701500243276792, - "velocityY": 1.1478986456646028, - "timestamp": 2.374128894028399 - }, - { - "x": 2.2868206827051014, - "y": 5.409909264516836, - "heading": -0.05582004781301006, - "angularVelocity": 0.4467067709849929, - "velocityX": 1.2102555887076, - "velocityY": 0.9206082205795442, - "timestamp": 2.4589848580125535 - }, - { - "x": 2.385467579367521, - "y": 5.4603756892360344, - "heading": -0.030726479319374042, - "angularVelocity": 0.2957195618957538, - "velocityX": 1.1625216664893496, - "velocityY": 0.5947304390840709, - "timestamp": 2.543840821996708 - }, - { - "x": 2.463261391908942, - "y": 5.492112879652653, - "heading": -0.014444831338496888, - "angularVelocity": 0.1918739380995954, - "velocityX": 0.916774836898297, - "velocityY": 0.3740124904189999, - "timestamp": 2.6286967859808623 - }, - { - "x": 2.5161956459202934, - "y": 5.510821266131426, - "heading": -0.0045926513861969465, - "angularVelocity": 0.11610474372950008, - "velocityX": 0.6238130064875114, - "velocityY": 0.22047226382658144, - "timestamp": 2.7135527499650167 - }, - { - "x": 2.542956590652466, - "y": 5.519354820251465, - "heading": -1.9593529845154978e-18, - "angularVelocity": 0.05412290628216246, - "velocityX": 0.3153690498073838, - "velocityY": 0.10056516618716548, - "timestamp": 2.798408713949171 - }, - { - "x": 2.542956590652466, - "y": 5.519354820251465, - "heading": -3.879161841617201e-19, - "angularVelocity": 1.851886937303712e-17, - "velocityX": 1.8638350784782603e-17, - "velocityY": 7.390265321449622e-16, - "timestamp": 2.8832646779333255 - }, - { - "x": 2.54352414805122, - "y": 5.5417247332646085, - "heading": 0.010369864258801478, - "angularVelocity": 0.1365642797123967, - "velocityX": 0.007474356984997478, - "velocityY": 0.294597015122544, - "timestamp": 2.9591986211331163 - }, - { - "x": 2.5446584508658887, - "y": 5.5864645661514665, - "heading": 0.031108645745408125, - "angularVelocity": 0.27311608765055906, - "velocityX": 0.01493802069101894, - "velocityY": 0.5891941205943013, - "timestamp": 3.035132564332907 - }, - { - "x": 2.5463582976769072, - "y": 5.6535744095701785, - "heading": 0.062210562054685774, - "angularVelocity": 0.4095917451230601, - "velocityX": 0.022385862492957306, - "velocityY": 0.8837924199740047, - "timestamp": 3.1110665075326978 - }, - { - "x": 2.5486221305372125, - "y": 5.743054499123618, - "heading": 0.10366116200111275, - "angularVelocity": 0.545877090003949, - "velocityX": 0.029813187158595614, - "velocityY": 1.1783938220883299, - "timestamp": 3.1870004507324885 - }, - { - "x": 2.551448101584002, - "y": 5.8549052924787315, - "heading": 0.15543248064293438, - "angularVelocity": 0.6817941550276853, - "velocityX": 0.03721617668864778, - "velocityY": 1.473001251374778, - "timestamp": 3.2629343939322792 - }, - { - "x": 2.5548341868258384, - "y": 5.989127553824463, - "heading": 0.2174776706164971, - "angularVelocity": 0.8170942711392554, - "velocityX": 0.04459251158506785, - "velocityY": 1.7676187445261111, - "timestamp": 3.33886833713207 - }, - { - "x": 2.5587783438313068, - "y": 6.145722446236129, - "heading": 0.28972525218567036, - "angularVelocity": 0.9514530462231059, - "velocityX": 0.05194194900547891, - "velocityY": 2.0622515546130344, - "timestamp": 3.4148022803318607 - }, - { - "x": 2.563218286954705, - "y": 6.324743248037854, - "heading": 0.36865194803097623, - "angularVelocity": 1.039412580453524, - "velocityX": 0.058471125511242075, - "velocityY": 2.357586005124216, - "timestamp": 3.4907362235316515 - }, - { - "x": 2.5671026763479077, - "y": 6.481388915794399, - "heading": 0.43754245115183227, - "angularVelocity": 0.9072425349964675, - "velocityX": 0.05115484893208649, - "velocityY": 2.0629202324498515, - "timestamp": 3.566670166731442 - }, - { - "x": 2.5704320555482925, - "y": 6.615658176157516, - "heading": 0.4964771667239727, - "angularVelocity": 0.7761313727258498, - "velocityX": 0.04384573038206363, - "velocityY": 1.7682376906180024, - "timestamp": 3.642604109931233 - }, - { - "x": 2.573206618251443, - "y": 6.727550075342484, - "heading": 0.5455200541615356, - "angularVelocity": 0.6458625137973618, - "velocityX": 0.03653916267524383, - "velocityY": 1.4735425880698358, - "timestamp": 3.7185380531310237 - }, - { - "x": 2.575426370371336, - "y": 6.8170639437544045, - "heading": 0.5847189679540402, - "angularVelocity": 0.5162238669651036, - "velocityX": 0.02923267285162808, - "velocityY": 1.1788386673980507, - "timestamp": 3.7944719963308144 - }, - { - "x": 2.577091240828593, - "y": 6.884199374047621, - "heading": 0.6141061452751087, - "angularVelocity": 0.38700976246877483, - "velocityX": 0.021925246959405044, - "velocityY": 0.884129382252347, - "timestamp": 3.870405939530605 - }, - { - "x": 2.5782011573891666, - "y": 6.928956199415487, - "heading": 0.6336989699349357, - "angularVelocity": 0.25802459129872973, - "velocityX": 0.014616869792382615, - "velocityY": 0.5894179003730387, - "timestamp": 3.946339882730396 - }, - { - "x": 2.578756093978882, - "y": 6.951334476470947, - "heading": 0.643500664804274, - "angularVelocity": 0.1290818632129897, - "velocityX": 0.007308149245661792, - "velocityY": 0.2947071640488953, - "timestamp": 4.022273825930187 - }, - { - "x": 2.578756093978882, - "y": 6.951334476470947, - "heading": 0.643500664804274, - "angularVelocity": -1.7225163541747852e-17, - "velocityX": -8.122490732634945e-17, - "velocityY": -7.511091870024743e-16, - "timestamp": 4.098207769129978 - }, - { - "x": 2.605882805940747, - "y": 6.95657005522081, - "heading": 0.6386264747117201, - "angularVelocity": -0.05791461301218251, - "velocityX": 0.3223167327766511, - "velocityY": 0.06220859495330271, - "timestamp": 4.182369431257094 - }, - { - "x": 2.6601362026337743, - "y": 6.967041240264821, - "heading": 0.6288779935837058, - "angularVelocity": -0.11583042541734088, - "velocityX": 0.6446331420009866, - "velocityY": 0.12441751718491452, - "timestamp": 4.266531093384209 - }, - { - "x": 2.7415162653471867, - "y": 6.98274808058871, - "heading": 0.6142571418365809, - "angularVelocity": -0.17372341963781532, - "velocityX": 0.966949328905814, - "velocityY": 0.1866270214597963, - "timestamp": 4.350692755511325 - }, - { - "x": 2.8500229829470065, - "y": 7.003690647779458, - "heading": 0.594767720039105, - "angularVelocity": -0.23157125590081054, - "velocityX": 1.2892653835179018, - "velocityY": 0.24883737632363517, - "timestamp": 4.434854417638441 - }, - { - "x": 2.9856563505646374, - "y": 7.029869037377032, - "heading": 0.5704152085572207, - "angularVelocity": -0.28935397503322835, - "velocityX": 1.6115813802818453, - "velocityY": 0.3110488663832933, - "timestamp": 4.5190160797655565 - }, - { - "x": 3.1484163679588817, - "y": 7.06128337029242, - "heading": 0.5412065071898937, - "angularVelocity": -0.3470547114814702, - "velocityX": 1.933897374179897, - "velocityY": 0.37326179309459206, - "timestamp": 4.603177741892672 - }, - { - "x": 3.3383030376303036, - "y": 7.097933794128878, - "heading": 0.5071496172056923, - "angularVelocity": -0.40466037769979885, - "velocityX": 2.2562133977893946, - "velocityY": 0.4354764736121926, - "timestamp": 4.687339404019788 - }, - { - "x": 3.555316362844474, - "y": 7.139820484190559, - "heading": 0.4682532762402473, - "angularVelocity": -0.46216222425238096, - "velocityX": 2.578529460200051, - "velocityY": 0.49769323707530094, - "timestamp": 4.771501066146904 - }, - { - "x": 3.799456345915655, - "y": 7.186943643856573, - "heading": 0.424526577651669, - "angularVelocity": -0.5195560244822011, - "velocityX": 2.9008455501085413, - "velocityY": 0.5599124170675296, - "timestamp": 4.8556627282740195 - }, - { - "x": 4.07072298772812, - "y": 7.239303503682716, - "heading": 0.37597867364929377, - "angularVelocity": -0.5768410791251948, - "velocityX": 3.2231616505238603, - "velocityY": 0.6221343364994486, - "timestamp": 4.939824390401135 - }, - { - "x": 4.369116292263885, - "y": 7.296900317207954, - "heading": 0.32261896114654104, - "angularVelocity": -0.634014480633234, - "velocityX": 3.5454777982530907, - "velocityY": 0.6843592684546252, - "timestamp": 5.023986052528251 - }, - { - "x": 4.69463630229783, - "y": 7.359734340352348, - "heading": 0.2644605433432946, - "angularVelocity": -0.6910321913011374, - "velocityX": 3.8677944542289113, - "velocityY": 0.7465872412250016, - "timestamp": 5.108147714655367 - }, - { - "x": 5.043878143874846, - "y": 7.42716808821782, - "heading": 0.26446053600249464, - "angularVelocity": -8.722261133391044e-8, - "velocityX": 4.149654756693505, - "velocityY": 0.8012406856179081, - "timestamp": 5.192309376782482 - }, - { - "x": 5.3943988333147095, - "y": 7.487603347084882, - "heading": 0.2644605286726163, - "angularVelocity": -8.709284181708098e-8, - "velocityX": 4.164849892228206, - "velocityY": 0.7180853768760186, - "timestamp": 5.276471038909598 - }, - { - "x": 5.748554706480114, - "y": 7.520626966261676, - "heading": 0.26445951238259063, - "angularVelocity": -0.000012075450982753597, - "velocityX": 4.2080427621604946, - "velocityY": 0.3923831628576489, - "timestamp": 5.360632701036714 - }, - { - "x": 6.076359506533188, - "y": 7.546997553632611, - "heading": 0.24369373540901995, - "angularVelocity": -0.24673677359421306, - "velocityX": 3.894942088453133, - "velocityY": 0.3133325400715727, - "timestamp": 5.44479436316383 - }, - { - "x": 6.37718666957215, - "y": 7.567469602956491, - "heading": 0.21707426667956767, - "angularVelocity": -0.31628972214506673, - "velocityX": 3.574396648495391, - "velocityY": 0.24324673261514576, - "timestamp": 5.528956025290945 - }, - { - "x": 6.650951360645961, - "y": 7.582275601726408, - "heading": 0.18847076354087783, - "angularVelocity": -0.33986380990774573, - "velocityX": 3.2528432085897303, - "velocityY": 0.17592331705087103, - "timestamp": 5.613117687418061 - }, - { - "x": 6.897625912379422, - "y": 7.591527252808584, - "heading": 0.15968573269508482, - "angularVelocity": -0.3420207029932105, - "velocityX": 2.9309610278477014, - "velocityY": 0.1099271431712301, - "timestamp": 5.697279349545177 - }, - { - "x": 7.1171977964953355, - "y": 7.595290076757919, - "heading": 0.13176491488072156, - "angularVelocity": -0.3317522148290329, - "velocityX": 2.6089299874362895, - "velocityY": 0.04470947761999478, - "timestamp": 5.7814410116722925 - }, - { - "x": 7.309660237553315, - "y": 7.5936071064013415, - "heading": 0.10539226300609161, - "angularVelocity": -0.3133570702869136, - "velocityX": 2.286818441956269, - "velocityY": -0.019996876416657455, - "timestamp": 5.865602673799408 - }, - { - "x": 7.475009136714085, - "y": 7.586508749225222, - "heading": 0.0810505950391384, - "angularVelocity": -0.28922513353156276, - "velocityX": 1.9646581944999189, - "velocityY": -0.08434193190479512, - "timestamp": 5.949764335926524 - }, - { - "x": 7.613241814863639, - "y": 7.574017622800665, - "heading": 0.059099291100008985, - "angularVelocity": -0.26082308006197424, - "velocityX": 1.642466114093265, - "velocityY": -0.14841824779661225, - "timestamp": 6.03392599805364 - }, - { - "x": 7.724356418372183, - "y": 7.556151203473209, - "heading": 0.03981643413778623, - "angularVelocity": -0.2291168742972126, - "velocityX": 1.3202520090527614, - "velocityY": -0.21228691159250246, - "timestamp": 6.1180876601807554 - }, - { - "x": 7.80835160711804, - "y": 7.5329233986954955, - "heading": 0.023423620240304886, - "angularVelocity": -0.1947776871697487, - "velocityX": 0.998021980827717, - "velocityY": -0.27599032850171645, - "timestamp": 6.202249322307871 - }, - { - "x": 7.865226377203999, - "y": 7.5043455392513225, - "heading": 0.010101499773974827, - "angularVelocity": -0.15829203142647919, - "velocityX": 0.6757800244018113, - "velocityY": -0.3395591142319701, - "timestamp": 6.286410984434987 - }, - { - "x": 7.894979953765869, - "y": 7.4704270362854, - "heading": -6.199321830779289e-18, - "angularVelocity": -0.12002495576570035, - "velocityX": 0.35352886112124365, - "velocityY": -0.4030160777325445, - "timestamp": 6.370572646562103 - }, - { - "x": 7.894584049642754, - "y": 7.426198672978825, - "heading": -0.007089479197593018, - "angularVelocity": -0.07587613336913762, - "velocityX": -0.004237218730680758, - "velocityY": -0.4733601861879681, - "timestamp": 6.464007550039625 - }, - { - "x": 7.86075117401306, - "y": 7.375444727785397, - "heading": -0.010043840448864623, - "angularVelocity": -0.03161946062246776, - "velocityX": -0.3621010390173313, - "velocityY": -0.5432011304601843, - "timestamp": 6.557442453517147 - }, - { - "x": 7.793469713318101, - "y": 7.318225470876191, - "heading": -0.00885143445531551, - "angularVelocity": 0.012761890355416958, - "velocityX": -0.7200891550248647, - "velocityY": -0.6123970248759323, - "timestamp": 6.65087735699467 - }, - { - "x": 7.692724412768832, - "y": 7.254620943238055, - "heading": -0.0034978695073770924, - "angularVelocity": 0.057297270598950276, - "velocityX": -1.078240537525724, - "velocityY": -0.6807362695401956, - "timestamp": 6.744312260472192 - }, - { - "x": 7.558494360533822, - "y": 7.184742577510254, - "heading": 0.006036114483891398, - "angularVelocity": 0.10203878461288343, - "velocityX": -1.4366157317998562, - "velocityY": -0.747882890943545, - "timestamp": 6.8377471639497145 - }, - { - "x": 7.390749153984831, - "y": 7.108756149139622, - "heading": 0.019778831050343372, - "angularVelocity": 0.14708332812435573, - "velocityX": -1.795316314415032, - "velocityY": -0.8132552776587466, - "timestamp": 6.931182067427237 - }, - { - "x": 7.189440632055284, - "y": 7.026934307092558, - "heading": 0.03777709910485055, - "angularVelocity": 0.19262895753766113, - "velocityX": -2.1545323475180176, - "velocityY": -0.8757096010351999, - "timestamp": 7.024616970904759 - }, - { - "x": 6.954481412279556, - "y": 6.93980890428099, - "heading": 0.06012313662850344, - "angularVelocity": 0.2391615626705131, - "velocityX": -2.514683603566329, - "velocityY": -0.93247169493281, - "timestamp": 7.118051874382282 - }, - { - "x": 6.685672117824161, - "y": 6.848872549717405, - "heading": 0.08707457656091541, - "angularVelocity": 0.2884515200349684, - "velocityX": -2.8769687177989223, - "velocityY": -0.9732589340712626, - "timestamp": 7.211486777859804 - }, - { - "x": 6.387993943545244, - "y": 6.775960862611549, - "heading": 0.12150620101602463, - "angularVelocity": 0.3685092312787836, - "velocityX": -3.1859419039323758, - "velocityY": -0.780347433262947, - "timestamp": 7.304921681337326 - }, - { - "x": 6.122643180061621, - "y": 6.713816410019983, - "heading": 0.15262946767199945, - "angularVelocity": 0.3331010735561154, - "velocityX": -2.839953310889416, - "velocityY": -0.6651096140589043, - "timestamp": 7.398356584814849 - }, - { - "x": 5.890214769411301, - "y": 6.660443600401627, - "heading": 0.1800531937910235, - "angularVelocity": 0.29350622838307167, - "velocityX": -2.4875972682546292, - "velocityY": -0.5712298898151609, - "timestamp": 7.491791488292371 - }, - { - "x": 5.69086823658003, - "y": 6.615216731280533, - "heading": 0.2036583525973359, - "angularVelocity": 0.2526374826511273, - "velocityX": -2.1335338873574874, - "velocityY": -0.4840468330121896, - "timestamp": 7.585226391769893 - }, - { - "x": 5.5246769076205275, - "y": 6.577830247082936, - "heading": 0.22338706488015242, - "angularVelocity": 0.21114927664652222, - "velocityX": -1.7786857242217098, - "velocityY": -0.4001340270725567, - "timestamp": 7.678661295247416 - }, - { - "x": 5.391682817900511, - "y": 6.548103124628764, - "heading": 0.2392048833965071, - "angularVelocity": 0.16929239425136144, - "velocityX": -1.4233876717388545, - "velocityY": -0.31815864679865835, - "timestamp": 7.772096198724938 - }, - { - "x": 5.291913195980534, - "y": 6.525915639931743, - "heading": 0.2510888771367695, - "angularVelocity": 0.12719008954850897, - "velocityX": -1.067798201814147, - "velocityY": -0.23746462907578247, - "timestamp": 7.8655311022024605 - }, - { - "x": 5.225387107672126, - "y": 6.511182746987271, - "heading": 0.25902271171762453, - "angularVelocity": 0.08491296384507606, - "velocityX": -0.7120046773999501, - "velocityY": -0.15768082800039224, - "timestamp": 7.958966005679983 - }, - { - "x": 5.1921186447143555, - "y": 6.503840923309326, - "heading": 0.26299425401034693, - "angularVelocity": 0.04250598165039902, - "velocityX": -0.3560603341959137, - "velocityY": -0.07857688513276403, - "timestamp": 8.052400909157505 - }, - { - "x": 5.1921186447143555, - "y": 6.503840923309326, - "heading": 0.26299425401034693, - "angularVelocity": -1.0095416875346628e-18, - "velocityX": 2.3925664461171115e-17, - "velocityY": 7.068676560436054e-18, - "timestamp": 8.145835812635028 - }, - { - "x": 5.218809865013869, - "y": 6.495313073233126, - "heading": 0.260871879579823, - "angularVelocity": -0.025050084717691923, - "velocityX": 0.3150326917368419, - "velocityY": -0.10065300627272344, - "timestamp": 8.23056105226765 - }, - { - "x": 5.272196468754811, - "y": 6.478270476254072, - "heading": 0.2566236198235182, - "angularVelocity": -0.05014160803469802, - "velocityX": 0.6301145204478967, - "velocityY": -0.2011513576468166, - "timestamp": 8.315286291900271 - }, - { - "x": 5.352283792023045, - "y": 6.452729954483983, - "heading": 0.25024542978948194, - "angularVelocity": -0.07528087334651229, - "velocityX": 0.9452593302244072, - "velocityY": -0.3014511600183777, - "timestamp": 8.400011531532893 - }, - { - "x": 5.4590789190058935, - "y": 6.418713893785019, - "heading": 0.2417322157383651, - "angularVelocity": -0.10048025934221186, - "velocityX": 1.2604877536602386, - "velocityY": -0.40148674523037453, - "timestamp": 8.484736771165515 - }, - { - "x": 5.592591707275698, - "y": 6.376253550611183, - "heading": 0.23107691549234363, - "angularVelocity": -0.12576299922223963, - "velocityX": 1.575832524625849, - "velocityY": -0.5011534149439888, - "timestamp": 8.569462010798137 - }, - { - "x": 5.752836810532502, - "y": 6.32539562702347, - "heading": 0.21826867910988845, - "angularVelocity": -0.15117379942497813, - "velocityX": 1.8913502511370301, - "velocityY": -0.6002688668481637, - "timestamp": 8.654187250430759 - }, - { - "x": 5.939838300939574, - "y": 6.266217475974801, - "heading": 0.20328868573896017, - "angularVelocity": -0.17680673947790815, - "velocityX": 2.207152097980862, - "velocityY": -0.6984713328079369, - "timestamp": 8.738912490063381 - }, - { - "x": 6.153643038134906, - "y": 6.198872009915045, - "heading": 0.18609787015363735, - "angularVelocity": -0.20290076085785252, - "velocityX": 2.5235070224931055, - "velocityY": -0.7948689947856541, - "timestamp": 8.823637729696003 - }, - { - "x": 6.394381892977596, - "y": 6.123803995127001, - "heading": 0.16657845141587072, - "angularVelocity": -0.23038493396306808, - "velocityX": 2.841406597214261, - "velocityY": -0.8860171433394348, - "timestamp": 8.908362969328625 - }, - { - "x": 6.661972791946675, - "y": 6.056622100950942, - "heading": 0.1411655347825555, - "angularVelocity": -0.2999450546673997, - "velocityX": 3.1583374697951134, - "velocityY": -0.7929383790163071, - "timestamp": 8.993088208961247 - }, - { - "x": 6.9032496475776535, - "y": 5.999073073447199, - "heading": 0.11754236952388923, - "angularVelocity": -0.27882087275408163, - "velocityX": 2.847756544297579, - "velocityY": -0.6792430184120029, - "timestamp": 9.077813448593869 - }, - { - "x": 7.118031418712295, - "y": 5.950645420596591, - "heading": 0.09585021181159172, - "angularVelocity": -0.2560294642583155, - "velocityX": 2.5350388156582375, - "velocityY": -0.5715847256448627, - "timestamp": 9.16253868822649 - }, - { - "x": 7.306261982278755, - "y": 5.911173870778648, - "heading": 0.07613235590314016, - "angularVelocity": -0.23272705977522684, - "velocityX": 2.221658674353119, - "velocityY": -0.46587711039940277, - "timestamp": 9.247263927859112 - }, - { - "x": 7.467914023047444, - "y": 5.8805767421157285, - "heading": 0.05840904716752805, - "angularVelocity": -0.20918570207015538, - "velocityX": 1.9079561352630119, - "velocityY": -0.36113357478352465, - "timestamp": 9.331989167491734 - }, - { - "x": 7.602971384357183, - "y": 5.858805325942835, - "heading": 0.0426917303890982, - "angularVelocity": -0.18550926319691677, - "velocityX": 1.594062901389981, - "velocityY": -0.2569649406398561, - "timestamp": 9.416714407124356 - }, - { - "x": 7.711423390458376, - "y": 5.845827273377796, - "heading": 0.028987684559485204, - "angularVelocity": -0.16174691141666067, - "velocityX": 1.2800436631569734, - "velocityY": -0.15317811576944246, - "timestamp": 9.501439646756978 - }, - { - "x": 7.793262462229127, - "y": 5.841619537994678, - "heading": 0.017301971212697442, - "angularVelocity": -0.13792481906759255, - "velocityX": 0.9659349696219826, - "velocityY": -0.0496633045992219, - "timestamp": 9.5861648863896 - }, - { - "x": 7.848482940858505, - "y": 5.846164867627585, - "heading": 0.0076383901458884716, - "angularVelocity": -0.11405787825105443, - "velocityX": 0.6517594859432665, - "velocityY": 0.053647881701079494, - "timestamp": 9.670890126022222 - }, - { - "x": 7.87708044052124, - "y": 5.859449863433838, - "heading": 2.497416150959083e-27, - "angularVelocity": -0.09015483672881196, - "velocityX": 0.3375322369902693, - "velocityY": 0.15680092335953588, - "timestamp": 9.755615365654844 - }, - { - "x": 7.881596231266855, - "y": 5.877948058282651, - "heading": -0.005158165074008103, - "angularVelocity": -0.06905249312983469, - "velocityX": 0.060453010898891046, - "velocityY": 0.2476358267688033, - "timestamp": 9.830314552320118 - }, - { - "x": 7.865360693168207, - "y": 5.903065257170116, - "heading": -0.008731993343707898, - "angularVelocity": -0.0478429341635813, - "velocityX": -0.21734558063394657, - "velocityY": 0.3362446099984395, - "timestamp": 9.905013738985392 - }, - { - "x": 7.82831181618014, - "y": 5.934603651644639, - "heading": -0.010711784706445375, - "angularVelocity": -0.026503519664931297, - "velocityX": -0.4959743022917116, - "velocityY": 0.42220532622189155, - "timestamp": 9.979712925650666 - }, - { - "x": 7.770377377578464, - "y": 5.972324090639225, - "heading": -0.0110857390814715, - "angularVelocity": -0.005006137171235987, - "velocityX": -0.7755698714803699, - "velocityY": 0.5049645207465326, - "timestamp": 10.05441211231594 - }, - { - "x": 7.691472435431706, - "y": 6.0159317447407945, - "heading": -0.009839381087694242, - "angularVelocity": 0.016685027634399323, - "velocityX": -1.0563025605664207, - "velocityY": 0.583776826071393, - "timestamp": 10.129111298981215 - }, - { - "x": 7.591496122933511, - "y": 6.065054366948533, - "heading": -0.006954729975421036, - "angularVelocity": 0.038616901214725004, - "velocityX": -1.338385556273679, - "velocityY": 0.6576058508890599, - "timestamp": 10.203810485646489 - }, - { - "x": 7.470327733756537, - "y": 6.119207749983016, - "heading": -0.0024090670382981736, - "angularVelocity": 0.06085291072166403, - "velocityX": -1.6220844507976708, - "velocityY": 0.7249527799699275, - "timestamp": 10.278509672311763 - }, - { - "x": 7.32782268400487, - "y": 6.177737571853534, - "heading": 0.0038269526830036, - "angularVelocity": 0.08348176198015718, - "velocityX": -1.9077188937843252, - "velocityY": 0.7835402831464292, - "timestamp": 10.353208858977037 - }, - { - "x": 7.163811403874334, - "y": 6.2397140922834655, - "heading": 0.01179227386310508, - "angularVelocity": 0.10663196663430786, - "velocityX": -2.195623372252593, - "velocityY": 0.8296813284948795, - "timestamp": 10.427908045642312 - }, - { - "x": 6.978114754903294, - "y": 6.303722941599131, - "heading": 0.021540119487110404, - "angularVelocity": 0.13049466880657248, - "velocityX": -2.4859259820745265, - "velocityY": 0.8568881693784349, - "timestamp": 10.502607232307586 - }, - { - "x": 6.770640654116627, - "y": 6.367398692968795, - "heading": 0.03314423110651253, - "angularVelocity": 0.15534455109129794, - "velocityX": -2.7774613091351856, - "velocityY": 0.8524289783099493, - "timestamp": 10.57730641897286 - }, - { - "x": 6.541925002807842, - "y": 6.426263625769268, - "heading": 0.046694627071435006, - "angularVelocity": 0.18139951142495733, - "velocityX": -3.06182251131669, - "velocityY": 0.7880264220846899, - "timestamp": 10.652005605638134 - }, - { - "x": 6.296286014325702, - "y": 6.471418454274109, - "heading": 0.06214080908224435, - "angularVelocity": 0.20677844967741577, - "velocityX": -3.288375676469458, - "velocityY": 0.6044888909859091, - "timestamp": 10.726704792303408 - }, - { - "x": 6.048501736882419, - "y": 6.494895270234614, - "heading": 0.07847895732781966, - "angularVelocity": 0.2187192254018271, - "velocityX": -3.3170947168889495, - "velocityY": 0.3142847600965746, - "timestamp": 10.801403978968683 - }, - { - "x": 5.810207804194887, - "y": 6.498763072278465, - "heading": 0.09455495650408928, - "angularVelocity": 0.2152098288339583, - "velocityX": -3.190047219059051, - "velocityY": 0.051778368902233175, - "timestamp": 10.876103165633957 - }, - { - "x": 5.585913181304932, - "y": 6.485940456390381, - "heading": 0.10987224560749582, - "angularVelocity": 0.2050529568955417, - "velocityX": -3.002638086208546, - "velocityY": -0.17165670016652868, - "timestamp": 10.950802352299231 - }, - { - "x": 5.393948798137783, - "y": 6.461779934393033, - "heading": 0.12306987524609604, - "angularVelocity": 0.19297917140210155, - "velocityX": -2.8069531133047367, - "velocityY": -0.3532814333608597, - "timestamp": 11.019191233558862 - }, - { - "x": 5.21822858427444, - "y": 6.42928387175211, - "heading": 0.13522346577307512, - "angularVelocity": 0.17771296010588833, - "velocityX": -2.569426646946333, - "velocityY": -0.4751658755398492, - "timestamp": 11.087580114818493 - }, - { - "x": 5.060196677201855, - "y": 6.392264431165203, - "heading": 0.1462160897383734, - "angularVelocity": 0.1607370052387037, - "velocityX": -2.3107836268389246, - "velocityY": -0.5413078837533164, - "timestamp": 11.155968996078125 - }, - { - "x": 4.920363237530942, - "y": 6.353780654626919, - "heading": 0.15599475116563047, - "angularVelocity": 0.14298612942845923, - "velocityX": -2.0446809056585025, - "velocityY": -0.5627197846998563, - "timestamp": 11.224357877337756 - }, - { - "x": 4.798767231888284, - "y": 6.3161460308896284, - "heading": 0.164539456070254, - "angularVelocity": 0.12494289637791432, - "velocityX": -1.7780084043345064, - "velocityY": -0.550303251699868, - "timestamp": 11.292746758597387 - }, - { - "x": 4.695244909056443, - "y": 6.281093323544826, - "heading": 0.17184599738491455, - "angularVelocity": 0.10683814649521928, - "velocityX": -1.5137303158803064, - "velocityY": -0.5125497990196586, - "timestamp": 11.361135639857018 - }, - { - "x": 4.609559458166785, - "y": 6.2499355875386025, - "heading": 0.17791742648618403, - "angularVelocity": 0.08877801463398721, - "velocityX": -1.2529149375080866, - "velocityY": -0.45559651557885517, - "timestamp": 11.42952452111665 - }, - { - "x": 4.541458842404482, - "y": 6.223686967257069, - "heading": 0.18275993511057495, - "angularVelocity": 0.070808419953631, - "velocityX": -0.9957849069612339, - "velocityY": -0.3838141492896176, - "timestamp": 11.49791340237628 - }, - { - "x": 4.490700080737798, - "y": 6.203146864655871, - "heading": 0.18638086356776906, - "angularVelocity": 0.052946157189611806, - "velocityX": -0.7422078082252005, - "velocityY": -0.30034271979417115, - "timestamp": 11.566302283635912 - }, - { - "x": 4.4570583798792995, - "y": 6.188957712133428, - "heading": 0.1887877351300412, - "angularVelocity": 0.03519390166852904, - "velocityX": -0.49191769537480406, - "velocityY": -0.2074774767637344, - "timestamp": 11.634691164895543 - }, - { - "x": 4.440329551696777, - "y": 6.181644916534424, - "heading": 0.18998779389424572, - "angularVelocity": 0.017547571214809383, - "velocityX": -0.2446132744738579, - "velocityY": -0.10692959826674406, - "timestamp": 11.703080046155174 - }, - { - "x": 4.440329551696777, - "y": 6.181644916534424, - "heading": 0.18998779389424572, - "angularVelocity": -4.688613187627218e-28, - "velocityX": 1.0698378725405786e-25, - "velocityY": 1.9964070921469186e-25, - "timestamp": 11.771468927414805 - }, - { - "x": 4.439579335848565, - "y": 6.164768781343018, - "heading": 0.18962382517380036, - "angularVelocity": -0.00553315604255771, - "velocityX": -0.011404994771739275, - "velocityY": -0.2565558086834744, - "timestamp": 11.837248514624331 - }, - { - "x": 4.438629166641586, - "y": 6.131001049566098, - "heading": 0.18887821684872066, - "angularVelocity": -0.01133494989417828, - "velocityX": -0.014444742621334671, - "velocityY": -0.5133466658791909, - "timestamp": 11.903028101833858 - }, - { - "x": 4.4381503145293495, - "y": 6.080347141602359, - "heading": 0.18773000934555084, - "angularVelocity": -0.01745537714477353, - "velocityX": -0.00727964605054284, - "velocityY": -0.7700551206317605, - "timestamp": 11.968807689043384 - }, - { - "x": 4.438975373866779, - "y": 6.012850914220267, - "heading": 0.18615416077885757, - "angularVelocity": -0.023956498262503952, - "velocityX": 0.012542786788878593, - "velocityY": -1.0260968523122063, - "timestamp": 12.03458727625291 - }, - { - "x": 4.442155378938134, - "y": 5.928626966420744, - "heading": 0.1841206539292539, - "angularVelocity": -0.030913949689686845, - "velocityX": 0.04834334185201353, - "velocityY": -1.2803964173755829, - "timestamp": 12.100366863462437 - }, - { - "x": 4.449039762750971, - "y": 5.827921573409061, - "heading": 0.1815937471088899, - "angularVelocity": -0.038414756424589545, - "velocityX": 0.10465836142917671, - "velocityY": -1.5309520366995943, - "timestamp": 12.166146450671963 - }, - { - "x": 4.461381608820963, - "y": 5.71122949511904, - "heading": 0.1785321677552386, - "angularVelocity": -0.04654300039766565, - "velocityX": 0.18762425538913388, - "velocityY": -1.7739861747432784, - "timestamp": 12.23192603788149 - }, - { - "x": 4.481448613268838, - "y": 5.57951486602773, - "heading": 0.1748924161672784, - "angularVelocity": -0.055332539201965195, - "velocityX": 0.30506431096863224, - "velocityY": -2.0023632661567494, - "timestamp": 12.297705625091016 - }, - { - "x": 4.5120438147722846, - "y": 5.434590034225561, - "heading": 0.17063962874888192, - "angularVelocity": -0.06465208431378763, - "velocityX": 0.4651169580312549, - "velocityY": -2.2031885262603184, - "timestamp": 12.363485212300542 - }, - { - "x": 4.556188469541101, - "y": 5.279577397974022, - "heading": 0.1657694617290444, - "angularVelocity": -0.07403766466829045, - "velocityX": 0.6710996015861679, - "velocityY": -2.356546199625419, - "timestamp": 12.429264799510069 - }, - { - "x": 4.616267960626566, - "y": 5.118958057204039, - "heading": 0.16032836999773611, - "angularVelocity": -0.08271702456837958, - "velocityX": 0.9133455169625041, - "velocityY": -2.441780916903065, - "timestamp": 12.495044386719595 - }, - { - "x": 4.6932263289877705, - "y": 4.957658238915064, - "heading": 0.15440456088239551, - "angularVelocity": -0.09005543158049287, - "velocityX": 1.1699430115921938, - "velocityY": -2.4521257297524386, - "timestamp": 12.560823973929121 - }, - { - "x": 4.786707207470846, - "y": 4.799875743117793, - "heading": 0.14809694695472803, - "angularVelocity": -0.09589014153548765, - "velocityX": 1.4211229113571657, - "velocityY": -2.39865439250461, - "timestamp": 12.626603561138648 - }, - { - "x": 4.895747846043762, - "y": 4.648670678033537, - "heading": 0.14149381469729397, - "angularVelocity": -0.10038269526382383, - "velocityX": 1.6576668112189705, - "velocityY": -2.298662419431504, - "timestamp": 12.692383148348174 - }, - { - "x": 5.019277488115815, - "y": 4.506151226786177, - "heading": 0.13466693251764544, - "angularVelocity": -0.10378420524140636, - "velocityX": 1.8779327647431951, - "velocityY": -2.1666212467006614, - "timestamp": 12.7581627355577 - }, - { - "x": 5.1563191413879395, - "y": 4.373770713806152, - "heading": 0.1276729143915235, - "angularVelocity": -0.10632505345228195, - "velocityX": 2.083346203368645, - "velocityY": -2.0124862224865496, - "timestamp": 12.823942322767227 - }, - { - "x": 5.333320265467547, - "y": 4.234610501686042, - "heading": 0.11935234394669658, - "angularVelocity": -0.10849739642365235, - "velocityX": 2.30803419717898, - "velocityY": -1.814601631091569, - "timestamp": 12.900631448212573 - }, - { - "x": 5.526705635502563, - "y": 4.1115365335688585, - "heading": 0.11088465842948947, - "angularVelocity": -0.11041572671527947, - "velocityX": 2.5216791678350265, - "velocityY": -1.604842504103067, - "timestamp": 12.97732057365792 - }, - { - "x": 5.7352637837384215, - "y": 4.005696194354983, - "heading": 0.10229707398048027, - "angularVelocity": -0.11197916783037644, - "velocityX": 2.7195270127899818, - "velocityY": -1.3801218699423634, - "timestamp": 13.054009699103267 - }, - { - "x": 5.95717408037646, - "y": 3.9185355692390273, - "heading": 0.09362936542456811, - "angularVelocity": -0.11302395881524778, - "velocityX": 2.8936344670690923, - "velocityY": -1.1365447788040335, - "timestamp": 13.130698824548613 - }, - { - "x": 6.189557120584961, - "y": 3.851808580822914, - "heading": 0.08494227942173509, - "angularVelocity": -0.11327663410406259, - "velocityX": 3.030195465902305, - "velocityY": -0.8700971360492012, - "timestamp": 13.20738794999396 - }, - { - "x": 6.427716553529618, - "y": 3.8073042106200683, - "heading": 0.0763299644889769, - "angularVelocity": -0.11230164489091633, - "velocityX": 3.1055176540562526, - "velocityY": -0.5803217854474332, - "timestamp": 13.284077075439306 - }, - { - "x": 6.664399710640609, - "y": 3.7857133712502065, - "heading": 0.06792429760501602, - "angularVelocity": -0.109607024922345, - "velocityX": 3.0862675214580135, - "velocityY": -0.2815371702895352, - "timestamp": 13.360766200884653 - }, - { - "x": 6.890876802597326, - "y": 3.7846905044197223, - "heading": 0.059852862986743374, - "angularVelocity": -0.1052487503462913, - "velocityX": 2.9531839180787136, - "velocityY": -0.013337834074180547, - "timestamp": 13.43745532633 - }, - { - "x": 7.100368603662355, - "y": 3.799117631492698, - "heading": 0.05217739052380199, - "angularVelocity": -0.10008553909525866, - "velocityX": 2.7317015267611535, - "velocityY": 0.18812480895035505, - "timestamp": 13.514144451775346 - }, - { - "x": 7.289321103149217, - "y": 3.823808235084369, - "heading": 0.044905901451856856, - "angularVelocity": -0.09481773367108355, - "velocityX": 2.4638760500864425, - "velocityY": 0.3219570369108888, - "timestamp": 13.590833577220693 - }, - { - "x": 7.456197806965785, - "y": 3.8548124388815017, - "heading": 0.038028185708090405, - "angularVelocity": -0.08968306397845134, - "velocityX": 2.176015215292753, - "velocityY": 0.40428422696290434, - "timestamp": 13.667522702666039 - }, - { - "x": 7.600383975861882, - "y": 3.8893298407285344, - "heading": 0.0315311794356911, - "angularVelocity": -0.08471874251623218, - "velocityX": 1.880138390662096, - "velocityY": 0.4500951294800189, - "timestamp": 13.744211828111386 - }, - { - "x": 7.721659266950539, - "y": 3.9253622327898867, - "heading": 0.02540302978123265, - "angularVelocity": -0.07990897821394244, - "velocityX": 1.581388370051051, - "velocityY": 0.4698500843777596, - "timestamp": 13.820900953556732 - }, - { - "x": 7.819973686957294, - "y": 3.9614436370035824, - "heading": 0.019633766687128053, - "angularVelocity": -0.07522922005697107, - "velocityX": 1.281986454217959, - "velocityY": 0.4704891861025283, - "timestamp": 13.897590079002079 - }, - { - "x": 7.895351647860016, - "y": 3.9964652092207733, - "heading": 0.014215138878361277, - "angularVelocity": -0.07065705570770218, - "velocityX": 0.9829028622375049, - "velocityY": 0.45666933889016026, - "timestamp": 13.974279204447425 - }, - { - "x": 7.947849278823351, - "y": 4.029564484128199, - "heading": 0.009140302774238713, - "angularVelocity": -0.0661741293130177, - "velocityX": 0.6845511753911964, - "velocityY": 0.4316032385975494, - "timestamp": 14.050968329892772 - }, - { - "x": 7.9775347399255425, - "y": 4.060054180581902, - "heading": 0.004403534947127983, - "angularVelocity": -0.06176583445962591, - "velocityX": 0.3870882726827797, - "velocityY": 0.39757522695226577, - "timestamp": 14.127657455338118 + "x": 3.3202035427093506, + "y": 5.489123821258545, + "heading": 3.079038293636101e-40, + "angularVelocity": 1.1965981728795564e-22, + "velocityX": 0.40816653670481284, + "velocityY": 4.557991704503881e-34, + "timestamp": 1.2162184342969842 }, { - "x": 7.984478950500488, - "y": 4.087375164031982, - "heading": 2.1272994684354894e-27, - "angularVelocity": -0.05742059153179738, - "velocityX": 0.09055013386343501, - "velocityY": 0.35625629176787293, - "timestamp": 14.204346580783465 - }, - { - "x": 7.971946711920367, - "y": 4.1092761415431145, - "heading": -0.003743193123138188, - "angularVelocity": -0.05350584756338805, - "velocityX": -0.1791379779341521, - "velocityY": 0.31305634672073157, - "timestamp": 14.274305160893865 - }, - { - "x": 7.9405304961902266, - "y": 4.1282629145767356, - "heading": -0.007208614807300815, - "angularVelocity": -0.04953533474655825, - "velocityX": -0.44906880157605467, - "velocityY": 0.2714002057168399, - "timestamp": 14.344263741004266 - }, - { - "x": 7.890211232557957, - "y": 4.14446189431145, - "heading": -0.010391724637129625, - "angularVelocity": -0.045499920450151686, - "velocityX": -0.7192722258350864, - "velocityY": 0.2315510078842599, - "timestamp": 14.414222321114666 - }, - { - "x": 7.820967420002681, - "y": 4.158023023328294, - "heading": -0.013287169417379553, - "angularVelocity": -0.04138798665840062, - "velocityX": -0.9897829893917608, - "velocityY": 0.19384511514446137, - "timestamp": 14.484180901225066 - }, - { - "x": 7.732774758572793, - "y": 4.169126981913827, - "heading": -0.015888540423835095, - "angularVelocity": -0.03718444545830319, - "velocityX": -1.2606411006443015, - "velocityY": 0.1587218975572353, - "timestamp": 14.554139481335467 - }, - { - "x": 7.6256058033641905, - "y": 4.177995649727882, - "heading": -0.0181880242087685, - "angularVelocity": -0.032869217489900605, - "velocityX": -1.5318915140856217, - "velocityY": 0.1267702660640051, - "timestamp": 14.624098061445867 - }, - { - "x": 7.499429791688496, - "y": 4.184907877485426, - "heading": -0.020175882010708173, - "angularVelocity": -0.0284147819867506, - "velocityX": -1.8035816546959227, - "velocityY": 0.09880457474459138, - "timestamp": 14.694056641556267 - }, - { - "x": 7.3542130604785445, - "y": 4.190224383970643, - "heading": -0.021839639250263897, - "angularVelocity": -0.023782032696063358, - "velocityX": -2.075752980989419, - "velocityY": 0.07599505988868352, - "timestamp": 14.764015221666668 - }, - { - "x": 7.189921237384926, - "y": 4.194429374442866, - "heading": -0.023162754266758116, - "angularVelocity": -0.01891283405704093, - "velocityX": -2.348415631568725, - "velocityY": 0.060106858452354715, - "timestamp": 14.833973801777068 - }, - { - "x": 7.006526810014663, - "y": 4.198205365374793, - "heading": -0.024122279434321083, - "angularVelocity": -0.01371561809929168, - "velocityX": -2.6214715490344274, - "velocityY": 0.05397466509423875, - "timestamp": 14.903932381887468 - }, - { - "x": 6.804034419528801, - "y": 4.202581059133061, - "heading": -0.024684383598055692, - "angularVelocity": -0.008034813783349493, - "velocityX": -2.8944611249443675, - "velocityY": 0.06254692064022167, - "timestamp": 14.973890961997869 - }, - { - "x": 6.582574314888401, - "y": 4.209262107218236, - "heading": -0.024794803569428318, - "angularVelocity": -0.0015783620993789793, - "velocityX": -3.1655888997592005, - "velocityY": 0.09550005266877039, - "timestamp": 15.04384954210827 - }, - { - "x": 6.342829267680752, - "y": 4.221487838378473, - "heading": -0.02435617244589905, - "angularVelocity": 0.006269868868651565, - "velocityX": -3.4269570198438615, - "velocityY": 0.17475670805416932, - "timestamp": 15.11380812221867 - }, - { - "x": 6.088597301309428, - "y": 4.246171426870893, - "heading": -0.02318863403361665, - "angularVelocity": 0.01668899526605478, - "velocityX": -3.6340355388877676, - "velocityY": 0.3528314676122184, - "timestamp": 15.18376670232907 - }, - { - "x": 5.834918091687161, - "y": 4.289953926308307, - "heading": -0.021332710171534618, - "angularVelocity": 0.02652889551436317, - "velocityX": -3.626134338660665, - "velocityY": 0.6258345919588656, - "timestamp": 15.25372528243947 - }, - { - "x": 5.593079348347317, - "y": 4.348733091622046, - "heading": -0.01919697004887221, - "angularVelocity": 0.030528637363594544, - "velocityX": -3.4568846731623317, - "velocityY": 0.8401995183575885, - "timestamp": 15.32368386254987 - }, - { - "x": 5.366458906870789, - "y": 4.419066666987696, - "heading": -0.01697675272885257, - "angularVelocity": 0.031736168980501495, - "velocityX": -3.239351643771207, - "velocityY": 1.005360246801131, - "timestamp": 15.393642442660271 - }, - { - "x": 5.1563191413879395, - "y": 4.4990692138671875, - "heading": -0.014762910019925463, - "angularVelocity": 0.03164504919101363, - "velocityX": -3.003774021016862, - "velocityY": 1.1435701918655279, - "timestamp": 15.463601022770671 - }, - { - "x": 4.95401886311352, - "y": 4.5927689377457765, - "heading": -0.01249503476608761, - "angularVelocity": 0.03078994837779946, - "velocityX": -2.7465422158220725, - "velocityY": 1.2721200852443764, - "timestamp": 15.537257374058129 - }, - { - "x": 4.772461025085408, - "y": 4.690756196606082, - "heading": -0.010343440097087077, - "angularVelocity": 0.02921125783985097, - "velocityX": -2.4649311954043354, - "velocityY": 1.3303300685896478, - "timestamp": 15.610913725345586 - }, - { - "x": 4.612079283555691, - "y": 4.78829172244435, - "heading": -0.008348351574694893, - "angularVelocity": 0.027086442479427285, - "velocityX": -2.177432614110885, - "velocityY": 1.3241970873308493, - "timestamp": 15.684570076633044 - }, - { - "x": 4.472451682258585, - "y": 4.881596852952091, - "heading": -0.006537499869867372, - "angularVelocity": 0.024585139952973775, - "velocityX": -1.8956627481068924, - "velocityY": 1.2667628639871322, - "timestamp": 15.758226427920501 - }, - { - "x": 4.352795824787486, - "y": 4.967847931994665, - "heading": -0.004929288926669055, - "angularVelocity": 0.021833975143867725, - "velocityX": -1.6245151352138218, - "velocityY": 1.1709930988295092, - "timestamp": 15.831882779207959 - }, - { - "x": 4.252252080841926, - "y": 5.0449690027725635, - "heading": -0.0035358640735745368, - "angularVelocity": 0.018917918533005222, - "velocityX": -1.3650383461593192, - "velocityY": 1.0470389780362597, - "timestamp": 15.905539130495416 - }, - { - "x": 4.170005928828097, - "y": 5.111421057915132, - "heading": -0.0023653291529936566, - "angularVelocity": 0.015891839605422012, - "velocityX": -1.1166199598029207, - "velocityY": 0.9021904286736582, - "timestamp": 15.979195481782874 - }, - { - "x": 4.105328367937727, - "y": 5.166042041203556, - "heading": -0.00142318887082745, - "angularVelocity": 0.012791025698372453, - "velocityX": -0.8780988979206226, - "velocityY": 0.7415651513235484, - "timestamp": 16.05285183307033 - }, - { - "x": 4.057581454405702, - "y": 5.207935779542024, - "heading": -0.0007132554579850938, - "angularVelocity": 0.009638454803058576, - "velocityX": -0.6482389189451429, - "velocityY": 0.56877292461815, - "timestamp": 16.12650818435779 - }, - { - "x": 4.026210974061915, - "y": 5.236396648720956, - "heading": -0.00023821729765836225, - "angularVelocity": 0.006449384907389903, - "velocityX": -0.42590326286131003, - "velocityY": 0.3864007472737587, - "timestamp": 16.200164535645246 - }, - { - "x": 4.010735511779785, - "y": 5.250858306884766, - "heading": -1.8831642522301752e-27, - "angularVelocity": 0.0032341718466161605, - "velocityX": -0.2101035689608668, - "velocityY": 0.1963395947672006, - "timestamp": 16.273820886932704 - }, - { - "x": 4.010735511779785, - "y": 5.250858306884766, - "heading": -9.309999607017892e-28, - "angularVelocity": 2.912789507524277e-28, - "velocityX": -3.6885695211923144e-25, - "velocityY": -3.7751755858346917e-25, - "timestamp": 16.34747723822016 + "x": 3.3202035427093506, + "y": 5.489123821258545, + "heading": 1.1605365063908143e-41, + "angularVelocity": -3.167017137026152e-39, + "velocityX": -4.508017624963005e-40, + "velocityY": -1.70250903211491e-50, + "timestamp": 1.3097736984736752 } ], "constraints": [ @@ -3510,42 +360,6 @@ "last" ], "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - }, - { - "scope": [ - 6 - ], - "type": "StopPoint" - }, - { - "scope": [ - 9 - ], - "type": "StopPoint" - }, - { - "scope": [ - 13 - ], - "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -3553,20 +367,20 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "SourceLaneTaxi": { + "AmpLaneTaxi": { "waypoints": [ { "x": 1.468971848487854, - "y": 1.9215065240859985, + "y": 7.309329509735107, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 14 }, { "x": 3.468971848487854, - "y": 1.9215065240859985, + "y": 7.309329509735107, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -3577,510 +391,138 @@ "trajectory": [ { "x": 1.468971848487854, - "y": 1.9215065240859983, + "y": 7.309329509735107, "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": 9.261974312050868e-43, + "velocityY": -5.968004408679696e-41, "timestamp": 0 }, { - "x": 1.5046861371964564, - "y": 1.9215065240859983, - "heading": -2.0425159024477705e-24, - "angularVelocity": -2.1355387753237974e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.2842385813221e-33, - "timestamp": 0.09564415279842821 + "x": 1.5097881789229493, + "y": 7.309329509735107, + "heading": 1.867923413309381e-18, + "angularVelocity": 1.9312421328909037e-17, + "velocityX": 0.42198942031848213, + "velocityY": -8.175734433695576e-17, + "timestamp": 0.09672358706123589 }, { - "x": 1.5761147139482576, - "y": 1.9215065240859983, - "heading": -7.39456542057925e-24, - "angularVelocity": -5.595811575686501e-23, - "velocityX": 0.7468159282286507, - "velocityY": -1.2474076386660863e-32, - "timestamp": 0.19128830559685642 + "x": 1.5914208389649154, + "y": 7.309329509735107, + "heading": -2.7784523382293792e-18, + "angularVelocity": -4.803678645072435e-17, + "velocityX": 0.843978832074171, + "velocityY": -1.635143003079991e-16, + "timestamp": 0.19344717412247178 }, { - "x": 1.6832575778116918, - "y": 1.9215065240859983, - "heading": -1.797881654717191e-23, - "angularVelocity": -1.1066309979696055e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.866390782935744e-32, - "timestamp": 0.28693245839528464 + "x": 1.713869827417429, + "y": 7.309329509735108, + "heading": -6.149022858043543e-18, + "angularVelocity": -3.4846149955338706e-17, + "velocityX": 1.2659682314613812, + "velocityY": -2.4527069236460427e-16, + "timestamp": 0.29017076118370766 }, { - "x": 1.8261147273894114, - "y": 1.9215065240859983, - "heading": -3.609466520226707e-23, - "angularVelocity": -1.8940924580147868e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4777348604111533e-32, - "timestamp": 0.38257661119371283 + "x": 1.8771351424005531, + "y": 7.309329509735107, + "heading": 9.334684137308459e-20, + "angularVelocity": 6.453994057032915e-17, + "velocityX": 1.6879576114124106, + "velocityY": -3.2702618744361184e-16, + "timestamp": 0.38689434824494356 + }, + { + "x": 2.081216780530401, + "y": 7.309329509735108, + "heading": 7.900764051063292e-18, + "angularVelocity": 8.072096604598862e-17, + "velocityX": 2.1099469563783173, + "velocityY": -4.087800036020465e-16, + "timestamp": 0.48361793530617947 + }, + { + "x": 2.326114733911239, + "y": 7.309329509735108, + "heading": -4.0292085158169106e-19, + "angularVelocity": -8.584682780302335e-17, + "velocityX": 2.5319362197122866, + "velocityY": -4.905289237309452e-16, + "timestamp": 0.5803415223674153 + }, + { + "x": 2.611828963064465, + "y": 7.309329509735108, + "heading": 4.895920849005717e-18, + "angularVelocity": 5.476420787705161e-17, + "velocityX": 2.9539250748872585, + "velocityY": -5.724892251787591e-16, + "timestamp": 0.6770651094286512 + }, + { + "x": 2.856726916445305, + "y": 7.309329509735108, + "heading": 1.0580912487609066e-17, + "angularVelocity": 5.877889823861001e-17, + "velocityX": 2.531936219712306, + "velocityY": -4.90569760600763e-16, + "timestamp": 0.773788696489887 + }, + { + "x": 3.0608085545751536, + "y": 7.309329509735108, + "heading": 1.2560262621019845e-17, + "angularVelocity": 2.0466206900583654e-17, + "velocityX": 2.109946956378327, + "velocityY": -4.088065981186001e-16, + "timestamp": 0.8705122835511229 + }, + { + "x": 3.224073869558278, + "y": 7.309329509735107, + "heading": 1.163556067743937e-17, + "angularVelocity": -9.558506562343336e-18, + "velocityX": 1.687957611412416, + "velocityY": -3.270442505924889e-16, + "timestamp": 0.9672358706123587 }, { - "x": 2.004686160352503, - "y": 1.9215065240859983, - "heading": -6.568172939485326e-23, - "angularVelocity": -3.093458575185046e-22, - "velocityX": 1.867039727346783, - "velocityY": -3.0967151425129867e-32, - "timestamp": 0.47822076399214103 + "x": 3.3465228580107924, + "y": 7.309329509735107, + "heading": 1.1661599473339483e-17, + "angularVelocity": 2.705282706656373e-19, + "velocityX": 1.2659682314613843, + "velocityY": -2.4528264910978726e-16, + "timestamp": 1.0639594576735947 }, { - "x": 2.21897187204314, - "y": 1.9215065240859983, - "heading": -1.1554278307693229e-22, - "angularVelocity": -5.213187002510948e-22, - "velocityX": 2.2404475905834893, - "velocityY": -3.7086764301179e-32, - "timestamp": 0.5738649167905693 + "x": 3.4281555180527588, + "y": 7.309329509735107, + "heading": 1.4643178138706357e-18, + "angularVelocity": -1.0542614870618343e-16, + "velocityX": 0.8439788320741728, + "velocityY": -1.6352149946538548e-16, + "timestamp": 1.1606830447348306 }, { - "x": 2.468971848487854, - "y": 1.9215065240859983, - "heading": -1.644337522360258e-22, - "angularVelocity": -5.111750826050956e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.332586658245059e-32, - "timestamp": 0.6695090695889975 - }, - { - "x": 2.7189718249325674, - "y": 1.9215065240859983, - "heading": -1.1817502956999641e-22, - "angularVelocity": 4.836559085941933e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.2931003905648025e-32, - "timestamp": 0.7651532223874257 - }, - { - "x": 2.933257536623205, - "y": 1.9215065240859983, - "heading": -8.188453918715617e-23, - "angularVelocity": 3.79432828279643e-22, - "velocityX": 2.2404475905834897, - "velocityY": -3.6782254895688684e-32, - "timestamp": 0.8607973751858539 - }, - { - "x": 3.1118289695862966, - "y": 1.9215065240859983, - "heading": -5.2607955008850217e-23, - "angularVelocity": 3.0609884687768117e-22, - "velocityX": 1.8670397273467831, - "velocityY": -3.06686637514316e-32, - "timestamp": 0.9564415279842821 - }, - { - "x": 3.2546861191640164, - "y": 1.9215065240859983, - "heading": -2.971367837778933e-23, - "angularVelocity": 2.3936923829343535e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4588855517580793e-32, - "timestamp": 1.0520856807827104 - }, - { - "x": 3.3618289830274506, - "y": 1.9215065240859983, - "heading": -1.3562846674979416e-23, - "angularVelocity": 1.6886357418178234e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.8475316069845286e-32, - "timestamp": 1.1477298335811386 - }, - { - "x": 3.4332575597792516, - "y": 1.9215065240859983, - "heading": -3.7629345495763975e-24, - "angularVelocity": 1.0246209621359342e-22, - "velocityX": 0.7468159282286508, - "velocityY": -1.2285406220691536e-32, - "timestamp": 1.2433739863795668 - }, - { - "x": 3.468971848487854, - "y": 1.9215065240859983, - "heading": -4.805430418918825e-41, - "angularVelocity": 3.9342997042908397e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.171880734353278e-33, - "timestamp": 1.339018139177995 - }, - { - "x": 3.468971848487854, - "y": 1.9215065240859983, - "heading": -6.447284134666629e-41, - "angularVelocity": -1.716658280508069e-40, - "velocityX": 0, - "velocityY": 0, - "timestamp": 1.4346622919764231 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "CenterLaneTaxi": { - "waypoints": [ - { - "x": 1.4490834474563599, - "y": 5.489123821258545, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 3.3202035427093506, - "y": 5.489123821258545, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.44908344745636, - "y": 5.489123821258545, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 4.665881846916086e-41, - "timestamp": 0 - }, - { - "x": 1.48726957542182, - "y": 5.489123821258545, - "heading": -7.108379598069269e-25, - "angularVelocity": -7.187577831060394e-24, - "velocityX": 0.3861138590221928, - "velocityY": -1.886047206290218e-34, - "timestamp": 0.09889862037628933 - }, - { - "x": 1.5636418305949276, - "y": 5.489123821258545, - "heading": -4.643559244941043e-24, - "angularVelocity": -3.976545068254259e-23, - "velocityX": 0.7722277103818697, - "velocityY": -1.8726511377848079e-34, - "timestamp": 0.19779724075257865 - }, - { - "x": 1.6782002118810653, - "y": 5.489123821258545, - "heading": -1.5517298527540587e-23, - "angularVelocity": -1.099488986748608e-22, - "velocityX": 1.1583415506734676, - "velocityY": -1.8670080146173677e-34, - "timestamp": 0.296695861128868 - }, - { - "x": 1.8309447175601195, - "y": 5.489123821258545, - "heading": -3.870026243741996e-23, - "angularVelocity": -2.344121636116204e-22, - "velocityX": 1.5444553735723716, - "velocityY": -1.869661321607696e-34, - "timestamp": 0.3955944815051573 - }, - { - "x": 2.021875344535886, - "y": 5.489123821258545, - "heading": -8.470852813064393e-23, - "angularVelocity": -4.652074531485588e-22, - "velocityX": 1.9305691651644288, - "velocityY": -1.8932805917377586e-34, - "timestamp": 0.49449310188144663 - }, - { - "x": 2.2509920855838907, - "y": 5.489123821258545, - "heading": -1.763106339588755e-22, - "angularVelocity": -9.262236889121556e-22, - "velocityX": 2.3166828837071916, - "velocityY": -1.9667253699171086e-34, - "timestamp": 0.593391722257736 - }, - { - "x": 2.5182949045818197, - "y": 5.489123821258545, - "heading": -1.9581239719515217e-22, - "angularVelocity": -1.9718812851378736e-22, - "velocityX": 2.7027962370040624, - "velocityY": -1.941321300199066e-34, - "timestamp": 0.6922903426340252 - }, - { - "x": 2.7474116456298243, - "y": 5.489123821258545, - "heading": -1.6406643315943827e-22, - "angularVelocity": 3.2099681933784467e-22, - "velocityX": 2.3166828837071916, - "velocityY": -2.5239482095290997e-34, - "timestamp": 0.7911889630103146 - }, - { - "x": 2.938342272605591, - "y": 5.489123821258545, - "heading": -1.2356782768830726e-22, - "angularVelocity": 4.0949693081452393e-22, - "velocityX": 1.930569165164429, - "velocityY": 4.651553097843434e-35, - "timestamp": 0.890087583386604 - }, - { - "x": 3.0910867782846454, - "y": 5.489123821258545, - "heading": -8.304773508097308e-23, - "angularVelocity": 4.0971396195220896e-22, - "velocityX": 1.5444553735723718, - "velocityY": 3.44484956688297e-35, - "timestamp": 0.9889862037628934 - }, - { - "x": 3.205645159570783, - "y": 5.489123821258545, - "heading": -4.640632580637159e-23, - "angularVelocity": 3.704945408904745e-22, - "velocityX": 1.1583415506734678, - "velocityY": 2.4410393142900414e-35, - "timestamp": 1.0878848241391827 - }, - { - "x": 3.2820174147438905, - "y": 5.489123821258545, - "heading": -1.5379540807100434e-23, - "angularVelocity": 3.1372300226566687e-22, - "velocityX": 0.7722277103818695, - "velocityY": 1.5631443217200112e-35, - "timestamp": 1.1867834445154721 - }, - { - "x": 3.3202035427093506, - "y": 5.489123821258545, - "heading": -2.9036533712144826e-40, - "angularVelocity": 1.555081061138629e-22, - "velocityX": 0.3861138590221928, - "velocityY": 7.652078130049953e-36, - "timestamp": 1.2856820648917615 - }, - { - "x": 3.3202035427093506, - "y": 5.489123821258545, - "heading": -5.67935929133569e-41, - "angularVelocity": 2.3616091621946912e-39, - "velocityX": 9.293663335236278e-40, - "velocityY": -6.396511271061404e-51, - "timestamp": 1.384580685268051 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "AmpLaneTaxi": { - "waypoints": [ - { - "x": 1.468971848487854, - "y": 7.309329509735107, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 3.468971848487854, - "y": 7.309329509735107, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.468971848487854, - "y": 7.309329509735107, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 9.261974312050868e-43, - "timestamp": 0 - }, - { - "x": 1.5046861371964564, - "y": 7.309329509735107, - "heading": -2.0425159024477705e-24, - "angularVelocity": -2.1355387753237974e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.2842385813221e-33, - "timestamp": 0.09564415279842821 - }, - { - "x": 1.5761147139482576, - "y": 7.309329509735107, - "heading": -7.39456542057925e-24, - "angularVelocity": -5.595811575686501e-23, - "velocityX": 0.7468159282286507, - "velocityY": -1.2474076386660863e-32, - "timestamp": 0.19128830559685642 - }, - { - "x": 1.6832575778116918, - "y": 7.309329509735107, - "heading": -1.797881654717191e-23, - "angularVelocity": -1.1066309979696055e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.866390782935744e-32, - "timestamp": 0.28693245839528464 - }, - { - "x": 1.8261147273894114, - "y": 7.309329509735107, - "heading": -3.609466520226707e-23, - "angularVelocity": -1.8940924580147868e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4777348604111533e-32, - "timestamp": 0.38257661119371283 - }, - { - "x": 2.004686160352503, - "y": 7.309329509735107, - "heading": -6.568172939485326e-23, - "angularVelocity": -3.093458575185046e-22, - "velocityX": 1.867039727346783, - "velocityY": -3.0967151425129867e-32, - "timestamp": 0.47822076399214103 - }, - { - "x": 2.21897187204314, - "y": 7.309329509735107, - "heading": -1.1554278307693229e-22, - "angularVelocity": -5.213187002510948e-22, - "velocityX": 2.2404475905834893, - "velocityY": -3.7086764301179e-32, - "timestamp": 0.5738649167905693 - }, - { - "x": 2.468971848487854, - "y": 7.309329509735107, - "heading": -1.644337522360258e-22, - "angularVelocity": -5.111750826050956e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.332586658245059e-32, - "timestamp": 0.6695090695889975 - }, - { - "x": 2.7189718249325674, - "y": 7.309329509735107, - "heading": -1.1817502956999641e-22, - "angularVelocity": 4.836559085941933e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.2931003905648025e-32, - "timestamp": 0.7651532223874257 - }, - { - "x": 2.933257536623205, - "y": 7.309329509735107, - "heading": -8.188453918715617e-23, - "angularVelocity": 3.79432828279643e-22, - "velocityX": 2.2404475905834897, - "velocityY": -3.6782254895688684e-32, - "timestamp": 0.8607973751858539 - }, - { - "x": 3.1118289695862966, - "y": 7.309329509735107, - "heading": -5.2607955008850217e-23, - "angularVelocity": 3.0609884687768117e-22, - "velocityX": 1.8670397273467831, - "velocityY": -3.06686637514316e-32, - "timestamp": 0.9564415279842821 - }, - { - "x": 3.2546861191640164, - "y": 7.309329509735107, - "heading": -2.971367837778933e-23, - "angularVelocity": 2.3936923829343535e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4588855517580793e-32, - "timestamp": 1.0520856807827104 - }, - { - "x": 3.3618289830274506, - "y": 7.309329509735107, - "heading": -1.3562846674979416e-23, - "angularVelocity": 1.6886357418178234e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.8475316069845286e-32, - "timestamp": 1.1477298335811386 - }, - { - "x": 3.4332575597792516, - "y": 7.309329509735107, - "heading": -3.7629345495763975e-24, - "angularVelocity": 1.0246209621359342e-22, - "velocityX": 0.7468159282286508, - "velocityY": -1.2285406220691536e-32, - "timestamp": 1.2433739863795668 - }, - { - "x": 3.468971848487854, - "y": 7.309329509735107, - "heading": -4.805430418918825e-41, - "angularVelocity": 3.9342997042908397e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.171880734353278e-33, - "timestamp": 1.339018139177995 + "x": 3.468971848487854, + "y": 7.309329509735107, + "heading": -3.620479666148997e-40, + "angularVelocity": -1.5138749754669695e-17, + "velocityX": 0.42198942031848286, + "velocityY": -8.176064575101043e-17, + "timestamp": 1.2574066317960666 }, { "x": 3.468971848487854, "y": 7.309329509735107, - "heading": -6.447284134666629e-41, - "angularVelocity": -1.716658280508069e-40, - "velocityX": 0, - "velocityY": 0, - "timestamp": 1.4346622919764231 + "heading": -2.966209034098039e-40, + "angularVelocity": 6.774689529655474e-40, + "velocityX": 3.003387584304273e-39, + "velocityY": 2.5761438166078746e-41, + "timestamp": 1.3541302188573026 } ], "constraints": [ @@ -4111,7 +553,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 26 + "controlIntervalCount": 25 }, { "x": 7.493931531906128, @@ -4129,243 +571,234 @@ "y": 1.5100655555725098, "heading": 0, "angularVelocity": 0, - "velocityX": 0, + "velocityX": 2.82118644197349e-37, "velocityY": 0, "timestamp": 0 }, { - "x": 1.530136363451793, - "y": 1.5100655555725098, - "heading": 1.63546146728146e-19, - "angularVelocity": 1.69832040317376e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.2961741231159756e-22, - "timestamp": 0.096298758216477 - }, - { - "x": 1.602546026150824, + "x": 1.5360008147282442, "y": 1.5100655555725098, - "heading": 5.000550672728244e-19, - "angularVelocity": 3.494426359967847e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.641598990275319e-22, - "timestamp": 0.192597516432954 + "heading": 3.2333963174943666e-19, + "angularVelocity": 3.292766805519721e-18, + "velocityX": 0.4284174372625368, + "velocityY": 1.8392808793575992e-26, + "timestamp": 0.09819694336189172 }, { - "x": 1.7111605195237438, + "x": 1.6201393797142145, "y": 1.5100655555725098, - "heading": 1.021045803758983e-18, - "angularVelocity": 5.410150084422451e-18, - "velocityX": 1.1278909031075715, - "velocityY": 4.0472192553510183e-22, - "timestamp": 0.288896274649431 + "heading": 7.027380416999147e-19, + "angularVelocity": 3.863647922469736e-18, + "velocityX": 0.8568348678215871, + "velocityY": 3.963342552313322e-26, + "timestamp": 0.19639388672378344 }, { - "x": 1.8559798429712064, + "x": 1.746347225986537, "y": 1.5100655555725098, - "heading": 1.7409021964949573e-18, - "angularVelocity": 7.475240644111669e-18, - "velocityX": 1.5038545265756458, - "velocityY": 5.528083834903345e-22, - "timestamp": 0.385195032865908 + "heading": 8.361393183209098e-19, + "angularVelocity": 1.3585074996311424e-18, + "velocityX": 1.2852522894444904, + "velocityY": 6.468173032965746e-26, + "timestamp": 0.29459083008567516 }, { - "x": 2.0370039957226234, + "x": 1.9146243523169562, "y": 1.5100655555725098, - "heading": 2.678153616617339e-18, - "angularVelocity": 9.732746683078845e-18, - "velocityX": 1.8798181420416615, - "velocityY": 7.105691212840138e-22, - "timestamp": 0.481493791082385 + "heading": 3.80235401897157e-19, + "angularVelocity": -4.642750516315599e-18, + "velocityX": 1.7136696985593167, + "velocityY": 9.506575448004094e-26, + "timestamp": 0.3927877734475669 }, { - "x": 2.254232976750543, + "x": 2.12497075686256, "y": 1.5100655555725098, - "heading": 3.8575512078730386e-18, - "angularVelocity": 1.2247277267116202e-17, - "velocityX": 2.25578174683826, - "velocityY": 8.812289177822076e-22, - "timestamp": 0.577792549298862 + "heading": -1.0623970199278788e-18, + "angularVelocity": -1.4691215100259113e-17, + "velocityX": 2.1420870889066284, + "velocityY": 1.3343481989110454e-25, + "timestamp": 0.4909847168094586 }, { - "x": 2.5076667846165317, + "x": 2.3773864365383615, "y": 1.5100655555725098, - "heading": 5.3137068489139784e-18, - "angularVelocity": 1.512122972072063e-17, - "velocityX": 2.6317453366976604, - "velocityY": 1.0699474323795816e-21, - "timestamp": 0.6740913075153391 + "heading": -3.9564889545725385e-18, + "angularVelocity": -2.947232200024047e-17, + "velocityX": 2.5705044478376236, + "velocityY": 1.8500903675379378e-25, + "timestamp": 0.5891816601713503 }, { - "x": 2.7973054171629355, + "x": 2.6718713848686027, "y": 1.5100655555725098, - "heading": 7.098464960583512e-18, - "angularVelocity": 1.8533552797092878e-17, - "velocityX": 3.007708904151223, - "velocityY": 1.2857541020722067e-21, - "timestamp": 0.7703900657318161 + "heading": -8.853646546888228e-18, + "angularVelocity": -4.987077429157769e-17, + "velocityX": 2.9989217408219764, + "velocityY": 2.625793984498987e-25, + "timestamp": 0.6873786035332421 }, { - "x": 3.1231488707936497, + "x": 3.0084255847706767, "y": 1.5100655555725098, - "heading": 9.299048469122608e-18, - "angularVelocity": 2.285162928757611e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.5467079012007584e-21, - "timestamp": 0.8666888239482932 + "heading": -1.6413613329692498e-17, + "angularVelocity": -7.698780165331962e-17, + "velocityX": 3.4273388598436147, + "velocityY": 4.161152605292971e-25, + "timestamp": 0.7855755468951338 }, { - "x": 3.4851971383163978, + "x": 3.3798020724552167, "y": 1.5100655555725098, - "heading": 1.2088326363522958e-17, - "angularVelocity": 2.896483790183752e-17, - "velocityX": 3.7596358896848248, - "velocityY": 1.8979567079276025e-21, - "timestamp": 0.9629875821647702 + "heading": -2.58741769005412e-17, + "angularVelocity": -9.634275005875749e-17, + "velocityX": 3.781955679779987, + "velocityY": -5.487818039619291e-25, + "timestamp": 0.8837724902570254 }, { - "x": 3.883450198153611, + "x": 3.7511785589388484, "y": 1.5100655555725098, - "heading": 1.5983950666179298e-17, - "angularVelocity": 4.0453526414309363e-17, - "velocityX": 4.1355991210390295, - "velocityY": 2.520086451941594e-21, - "timestamp": 1.0592863403812471 + "heading": 5.146033898598045e-18, + "angularVelocity": 3.158979265252523e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.295977316525143e-24, + "timestamp": 0.9819694336189171 }, { - "x": 4.290437753988616, + "x": 4.122555045422488, "y": 1.5100655555725098, - "heading": -1.0811293578876384e-19, - "angularVelocity": -1.671056190093009e-16, - "velocityX": 4.226301183657094, - "velocityY": -2.0251172410052617e-20, - "timestamp": 1.155585098597724 + "heading": 4.0038826563950476e-17, + "angularVelocity": 3.553348146159671e-16, + "velocityX": 3.7819556675504797, + "velocityY": 7.623936784532972e-25, + "timestamp": 1.0801663769808088 }, { - "x": 4.697425309823639, + "x": 4.493931531906129, "y": 1.5100655555725098, - "heading": -8.423356676535005e-18, - "angularVelocity": -8.634840048564003e-17, - "velocityX": 4.226301183657286, - "velocityY": 2.1724036271938957e-21, - "timestamp": 1.251883856814201 + "heading": 6.570668459860429e-17, + "angularVelocity": 2.613916192895976e-16, + "velocityX": 3.7819556675504797, + "velocityY": 3.4278643163770276e-26, + "timestamp": 1.1783633203427004 }, { - "x": 5.104412865658644, + "x": 4.865308018389769, "y": 1.5100655555725098, - "heading": -8.042445280935507e-18, - "angularVelocity": 3.955517211854611e-18, - "velocityX": 4.226301183657094, - "velocityY": -6.018603432474332e-22, - "timestamp": 1.348182615030678 + "heading": 6.663782988380241e-17, + "angularVelocity": 9.482426370100795e-18, + "velocityX": 3.7819556675504797, + "velocityY": 2.693381196266241e-25, + "timestamp": 1.276560263704592 }, { - "x": 5.502665925495857, + "x": 5.236684504873409, "y": 1.5100655555725098, - "heading": -6.3335804348522236e-18, - "angularVelocity": 1.7745450464376603e-17, - "velocityX": 4.13559912103903, - "velocityY": 1.2361742339090949e-21, - "timestamp": 1.4444813732471549 + "heading": 5.2326006182614025e-17, + "angularVelocity": -1.4574612214194992e-16, + "velocityX": 3.7819556675504797, + "velocityY": 5.124149697811417e-24, + "timestamp": 1.3747572070664837 }, { - "x": 5.864714193018606, + "x": 5.60806099135704, "y": 1.5100655555725098, - "heading": -5.002513564509544e-18, - "angularVelocity": 1.3822263999404139e-17, - "velocityX": 3.759635889684825, - "velocityY": 1.1237973471200677e-21, - "timestamp": 1.5407801314636318 + "heading": 2.884064553605338e-17, + "angularVelocity": -2.3916590315859936e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.1014556301472372e-23, + "timestamp": 1.4729541504283754 }, { - "x": 6.19055764664932, + "x": 5.9794374790415805, "y": 1.5100655555725098, - "heading": -3.8974854517760974e-18, - "angularVelocity": 1.1474998421478588e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.0114186808354653e-21, - "timestamp": 1.6370788896801087 + "heading": 1.6502190572837753e-17, + "angularVelocity": -1.2565009195595738e-16, + "velocityX": 3.7819556797799856, + "velocityY": 6.524035237986936e-24, + "timestamp": 1.571151093790267 }, { - "x": 6.480196279195724, + "x": 6.315991678943653, "y": 1.5100655555725098, - "heading": -2.9674142808273095e-18, - "angularVelocity": 9.658184315185454e-18, - "velocityX": 3.0077089041512233, - "velocityY": 8.990394180618527e-22, - "timestamp": 1.7333776478965857 + "heading": 9.967918627860597e-18, + "angularVelocity": -6.65425189408073e-17, + "velocityX": 3.427338859843601, + "velocityY": -3.262675265981889e-25, + "timestamp": 1.6693480371521587 }, { - "x": 6.733630087061712, + "x": 6.610476627273894, "y": 1.5100655555725098, - "heading": -2.1863127731116462e-18, - "angularVelocity": 8.111231213379657e-18, - "velocityX": 2.631745336697661, - "velocityY": 7.866598563444379e-22, - "timestamp": 1.8296764061130626 + "heading": 5.2731830670839714e-18, + "angularVelocity": -4.78093860085579e-17, + "velocityX": 2.9989217408219764, + "velocityY": -2.379324733264266e-25, + "timestamp": 1.7675449805140504 }, { - "x": 6.950859068089632, + "x": 6.862892306949696, "y": 1.5100655555725098, - "heading": -1.5381923338445708e-18, - "angularVelocity": 6.730309280881326e-18, - "velocityX": 2.2557817468382604, - "velocityY": 6.742801150833687e-22, - "timestamp": 1.9259751643295395 + "heading": 2.2289001785398693e-18, + "angularVelocity": -3.10018090849907e-17, + "velocityX": 2.5705044478376236, + "velocityY": -1.7314309405461854e-25, + "timestamp": 1.865741923875942 }, { - "x": 7.13188322084105, + "x": 7.0732387114953, "y": 1.5100655555725098, - "heading": -1.0119189398601327e-18, - "angularVelocity": 5.4650069455448464e-18, - "velocityX": 1.8798181420416618, - "velocityY": 5.619002540306745e-22, - "timestamp": 2.0222739225460167 + "heading": 5.002812433628901e-19, + "angularVelocity": -1.760359206249271e-17, + "velocityX": 2.1420870889066284, + "velocityY": -1.2661949502166272e-25, + "timestamp": 1.9639388672378337 }, { - "x": 7.276702544288512, + "x": 7.24151583782572, "y": 1.5100655555725098, - "heading": -6.000129613332895e-19, - "angularVelocity": 4.277375706520152e-18, - "velocityX": 1.5038545265756458, - "velocityY": 4.495203072508259e-22, - "timestamp": 2.118572680762494 + "heading": -2.663585350994398e-19, + "angularVelocity": -7.807165345874594e-18, + "velocityX": 1.7136696985593165, + "velocityY": -9.096401200333518e-26, + "timestamp": 2.0621358105997256 }, { - "x": 7.385317037661432, + "x": 7.367723684098041, "y": 1.5100655555725098, - "heading": -2.9682943520502703e-19, - "angularVelocity": 3.1483637414904354e-18, - "velocityX": 1.1278909031075715, - "velocityY": 3.3714029617848224e-22, - "timestamp": 2.214871438978971 + "heading": -4.029353027704155e-19, + "angularVelocity": -1.390845344762092e-18, + "velocityX": 1.2852522894444902, + "velocityY": -6.226126404979919e-26, + "timestamp": 2.1603327539616175 }, { - "x": 7.457726700360463, + "x": 7.451862249084011, "y": 1.5100655555725098, - "heading": -9.799024750037137e-20, - "angularVelocity": 2.064815645768494e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.2476023506664887e-22, - "timestamp": 2.311170197195448 + "heading": -2.1995708883691042e-19, + "angularVelocity": 1.8633799906775463e-18, + "velocityX": 0.8568348678215871, + "velocityY": -3.833262935804246e-26, + "timestamp": 2.2585296973235094 }, { "x": 7.493931531906128, "y": 1.5100655555725098, "heading": 0, - "angularVelocity": 1.017564989623581e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.1238013392767656e-22, - "timestamp": 2.4074689554119253 + "angularVelocity": 2.2399586452459634e-18, + "velocityX": 0.42841743726253684, + "velocityY": -1.7862050890834192e-26, + "timestamp": 2.3567266406854013 }, { "x": 7.493931531906128, "y": 1.5100655555725098, "heading": 0, "angularVelocity": 0, - "velocityX": -2.537301157014256e-39, + "velocityX": -2.1810930337522913e-39, "velocityY": 0, - "timestamp": 2.5037677136284024 + "timestamp": 2.454923584047293 } ], "constraints": [ @@ -4396,7 +829,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { "x": 4.028205871582031, @@ -4405,7 +838,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 12 }, { "x": 4.9537672996521, @@ -4414,7 +847,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { "x": 7.598227500915527, @@ -4430,434 +863,407 @@ { "x": 1.2955970764160156, "y": 5.586942195892334, - "heading": 4.584052603316453e-36, - "angularVelocity": 0, - "velocityX": -1.1087002227609576e-30, - "velocityY": 4.436024859486677e-30, + "heading": 0, + "angularVelocity": -1.1592130893720681e-31, + "velocityX": 0, + "velocityY": 0, "timestamp": 0 }, { - "x": 1.3181821407757048, - "y": 5.598273732258848, - "heading": -7.069763767214106e-20, - "angularVelocity": -8.787949104114132e-19, - "velocityX": 0.28073436408544955, - "velocityY": 0.14085200756269842, - "timestamp": 0.08044994574644204 - }, - { - "x": 1.3636517023728314, - "y": 5.620319213488495, - "heading": -2.0052541119733845e-19, - "angularVelocity": -1.6138052893300047e-18, - "velocityX": 0.5651907055441345, - "velocityY": 0.2740272976628797, - "timestamp": 0.16089989149288408 - }, - { - "x": 1.4323621310520749, - "y": 5.6522818969499085, - "heading": -2.5664724388774436e-18, - "angularVelocity": -2.9409059245655595e-17, - "velocityX": 0.8540767559525704, - "velocityY": 0.39729900580333494, - "timestamp": 0.24134983723932613 - }, - { - "x": 1.524738455066009, - "y": 5.693099757868794, - "heading": -4.957647208916547e-18, - "angularVelocity": -2.9722661390485903e-17, - "velocityX": 1.1482459454437492, - "velocityY": 0.507369651290102, - "timestamp": 0.32179978298576817 - }, - { - "x": 1.6412810261673692, - "y": 5.741298758765306, - "heading": -7.348054486157485e-18, - "angularVelocity": -2.9713141519150695e-17, - "velocityX": 1.4486345518419708, - "velocityY": 0.5991178794505583, - "timestamp": 0.4022497287322102 - }, - { - "x": 1.7825444409555524, - "y": 5.794729255821419, - "heading": -1.1630259555579544e-17, - "angularVelocity": -5.3228447997874313e-17, - "velocityX": 1.75591684342883, - "velocityY": 0.664145843251592, - "timestamp": 0.48269967447865225 - }, - { - "x": 1.9490035629897984, - "y": 5.8500741494423, - "heading": -1.6535392715689588e-17, - "angularVelocity": -6.097141104334343e-17, - "velocityX": 2.0691017314048668, - "velocityY": 0.6879419632751668, - "timestamp": 0.5631496202250943 - }, - { - "x": 2.140495465443084, - "y": 5.901976898384377, - "heading": -2.1721290723634077e-17, - "angularVelocity": -6.446138546893251e-17, - "velocityX": 2.380261424391423, - "velocityY": 0.6451557979517949, - "timestamp": 0.6435995659715363 - }, - { - "x": 2.3543799781199475, - "y": 5.942172413663534, - "heading": -2.6571028223887664e-17, - "angularVelocity": -6.02829067757922e-17, - "velocityX": 2.658603566399057, - "velocityY": 0.4996338394841148, - "timestamp": 0.7240495117179784 - }, - { - "x": 2.5827650177602375, - "y": 5.961674379906129, - "heading": -2.745308531544403e-17, - "angularVelocity": -1.0964182829837211e-17, - "velocityX": 2.8388464096705315, - "velocityY": 0.24241117953620092, - "timestamp": 0.8044994574644204 - }, - { - "x": 2.8152635583029264, - "y": 5.956245091375537, - "heading": -2.787857803251133e-17, - "angularVelocity": -5.289064001063628e-18, - "velocityX": 2.8899775927024023, - "velocityY": -0.06748654062169376, - "timestamp": 0.8849494032108625 - }, - { - "x": 3.0443318857824635, - "y": 5.925781383839179, - "heading": -2.6153565480502514e-17, - "angularVelocity": 2.144199289966682e-17, - "velocityX": 2.8473397384110632, - "velocityY": -0.3786666013850943, - "timestamp": 0.9653993489573045 - }, - { - "x": 3.265462860964513, - "y": 5.871328365222942, - "heading": -2.40553850185398e-17, - "angularVelocity": 2.608049948713598e-17, - "velocityX": 2.748677741502352, - "velocityY": -0.6768558774270661, - "timestamp": 1.0458492947037465 - }, - { - "x": 3.4759237911438947, - "y": 5.793970350971247, - "heading": -2.5315322230553355e-17, - "angularVelocity": -1.5661336184343358e-17, - "velocityX": 2.6160481305744674, - "velocityY": -0.9615670157972775, - "timestamp": 1.1262992404501886 - }, - { - "x": 3.6739512617428174, - "y": 5.694614659038136, - "heading": -2.7826462563542755e-17, - "angularVelocity": -3.1213956935036674e-17, - "velocityX": 2.461499119188376, - "velocityY": -1.2350001110858715, - "timestamp": 1.2067491861966306 - }, - { - "x": 3.858335998672662, - "y": 5.573990102564811, - "heading": -3.317033762126302e-17, - "angularVelocity": -6.642521120825162e-17, - "velocityX": 2.291918723041152, - "velocityY": -1.499373994035365, - "timestamp": 1.2871991319430727 + "x": 1.3207451593695585, + "y": 5.599086645701892, + "heading": 2.231825504271677e-18, + "angularVelocity": 2.789547782680952e-17, + "velocityX": 0.3143242018214309, + "velocityY": 0.15179266348424167, + "timestamp": 0.08000682991577572 + }, + { + "x": 1.3713809631464564, + "y": 5.62264555119355, + "heading": 4.51507105043022e-18, + "angularVelocity": 2.853816960464376e-17, + "velocityX": 0.6328935145938026, + "velocityY": 0.2944611793396252, + "timestamp": 0.16001365983155144 + }, + { + "x": 1.4479102774130088, + "y": 5.656665804231403, + "heading": 6.004653783100575e-18, + "angularVelocity": 1.8618332407574406e-17, + "velocityX": 0.9565347651837746, + "velocityY": 0.4252168605416788, + "timestamp": 0.24002048974732715 + }, + { + "x": 1.5508175668842004, + "y": 5.699857537973527, + "heading": 7.57580271600204e-18, + "angularVelocity": 1.963779951822175e-17, + "velocityX": 1.2862313077471652, + "velocityY": 0.5398505826146306, + "timestamp": 0.3200273196631029 + }, + { + "x": 1.6806682557862314, + "y": 5.750395481752386, + "heading": 9.251664351913254e-18, + "angularVelocity": 2.0946531612097715e-17, + "velocityX": 1.6229950497837156, + "velocityY": 0.6316703690584408, + "timestamp": 0.4000341495788786 + }, + { + "x": 1.8380612948775077, + "y": 5.805552130674172, + "heading": 1.2501573477685283e-17, + "angularVelocity": 4.062041901915404e-17, + "velocityX": 1.9672450371704346, + "velocityY": 0.6893992548031757, + "timestamp": 0.48004097949465435 + }, + { + "x": 2.0233797317677533, + "y": 5.861001231409725, + "heading": 1.6054120788359745e-17, + "angularVelocity": 4.440302445729553e-17, + "velocityX": 2.316282710935199, + "velocityY": 0.6930545904033714, + "timestamp": 0.5600478094104301 + }, + { + "x": 2.235784948485807, + "y": 5.909651835150332, + "heading": 2.213686509511287e-17, + "angularVelocity": 7.602767686101118e-17, + "velocityX": 2.6548385549342215, + "velocityY": 0.6080806325227727, + "timestamp": 0.6400546393262058 + }, + { + "x": 2.4702985640404234, + "y": 5.941239855854083, + "heading": 2.864477544319077e-17, + "angularVelocity": 8.134175236038742e-17, + "velocityX": 2.9311699488835434, + "velocityY": 0.39481655174808844, + "timestamp": 0.7200614692419814 + }, + { + "x": 2.715877888033922, + "y": 5.947186832463751, + "heading": 3.119004227784184e-17, + "angularVelocity": 3.1813081703857335e-17, + "velocityX": 3.0694794963261316, + "velocityY": 0.07433086169371976, + "timestamp": 0.8000682991577571 + }, + { + "x": 2.961603624456153, + "y": 5.925207266559582, + "heading": 3.402854212881078e-17, + "angularVelocity": 3.5478294907307905e-17, + "velocityX": 3.0713094954425415, + "velocityY": -0.2747211197853527, + "timestamp": 0.8800751290735328 + }, + { + "x": 3.2005883629277774, + "y": 5.876126550770939, + "heading": 3.9287160074963035e-17, + "angularVelocity": 6.572712324724996e-17, + "velocityX": 2.987054214261441, + "velocityY": -0.6134565741588879, + "timestamp": 0.9600819589893085 + }, + { + "x": 3.428817982052201, + "y": 5.801272963497273, + "heading": 4.077287726704947e-17, + "angularVelocity": 1.856989731461567e-17, + "velocityX": 2.8526266990269726, + "velocityY": -0.9355899659232499, + "timestamp": 1.0400887889050843 + }, + { + "x": 3.6438287356883494, + "y": 5.701819067812634, + "heading": 4.10496280483666e-17, + "angularVelocity": 3.459161663925654e-18, + "velocityX": 2.6874049860525546, + "velocityY": -1.243067570484904, + "timestamp": 1.12009561882086 + }, + { + "x": 3.8440012077207406, + "y": 5.578706367507132, + "heading": 4.4578411240497065e-17, + "angularVelocity": 4.41060783161696e-17, + "velocityX": 2.5019422996871006, + "velocityY": -1.538777382333539, + "timestamp": 1.2001024487366356 }, { "x": 4.028205871582031, "y": 5.432682037353516, - "heading": -4.136266271080991e-17, - "angularVelocity": -1.0183169434818875e-16, - "velocityX": 2.1114976688519658, - "velocityY": -1.7564718521622575, - "timestamp": 1.3676490776895147 - }, - { - "x": 4.107931819415156, - "y": 5.358451922494366, - "heading": -4.607311297857423e-17, - "angularVelocity": -1.1928519771528388e-16, - "velocityX": 2.018941679878885, - "velocityY": -1.8797678405182021, - "timestamp": 1.4071380577960628 - }, - { - "x": 4.184009699685142, - "y": 5.2793478225384876, - "heading": -5.07069292162639e-17, - "angularVelocity": -1.1734455595125913e-16, - "velocityX": 1.9265597659044507, - "velocityY": -2.0031943023709897, - "timestamp": 1.446627037902611 - }, - { - "x": 4.256449183576903, - "y": 5.195362514292594, - "heading": -5.599760363210485e-17, - "angularVelocity": -1.3397851916939406e-16, - "velocityX": 1.8344227603800494, - "velocityY": -2.126803680907542, - "timestamp": 1.486116018009159 - }, - { - "x": 4.325264882493694, - "y": 5.106485141313743, - "heading": -6.116421864840668e-17, - "angularVelocity": -1.3083689819279164e-16, - "velocityX": 1.7426557670265217, - "velocityY": -2.250687982800167, - "timestamp": 1.5256049981157072 - }, - { - "x": 4.390481430565823, - "y": 5.012697552943817, - "heading": -6.615883447942732e-17, - "angularVelocity": -1.2648127296403954e-16, - "velocityX": 1.6515125965785218, - "velocityY": -2.375031923257045, - "timestamp": 1.5650939782222553 - }, - { - "x": 4.452149171763237, - "y": 4.913963243889134, - "heading": -7.13027960351743e-17, - "angularVelocity": -1.3026323063398506e-16, - "velocityX": 1.5616443126888815, - "velocityY": -2.500300306269253, - "timestamp": 1.6045829583288034 - }, - { - "x": 4.510428130257496, - "y": 4.810171252999288, - "heading": -7.511831906490934e-17, - "angularVelocity": -9.662248553573966e-17, - "velocityX": 1.475828404199086, - "velocityY": -2.6283786162559313, - "timestamp": 1.6440719384353515 - }, - { - "x": 4.574696256850231, - "y": 4.707471171992527, - "heading": -7.369504337579853e-17, - "angularVelocity": 3.604235274007527e-17, - "velocityX": 1.6274952257440216, - "velocityY": -2.6007276138734374, - "timestamp": 1.6835609185418996 - }, - { - "x": 4.6429183139641825, - "y": 4.609400393977656, - "heading": -7.177735720537442e-17, - "angularVelocity": 4.8562568904755854e-17, - "velocityX": 1.7276226666283043, - "velocityY": -2.483497364335036, - "timestamp": 1.7230498986484477 - }, - { - "x": 4.714970653212224, - "y": 4.516061742260872, - "heading": -7.032849060398364e-17, - "angularVelocity": 3.669040576501961e-17, - "velocityX": 1.8246188950379656, - "velocityY": -2.3636632666873396, - "timestamp": 1.7625388787549958 - }, - { - "x": 4.790809657597791, - "y": 4.427490193568245, - "heading": -6.79306191406781e-17, - "angularVelocity": 6.072255395566305e-17, - "velocityX": 1.920510587538043, - "velocityY": -2.2429434351976525, - "timestamp": 1.802027858861544 - }, - { - "x": 4.870413047012439, - "y": 4.343703362363669, - "heading": -6.538034983995767e-17, - "angularVelocity": 6.458180589038589e-17, - "velocityX": 2.015838069253688, - "velocityY": -2.121777543469226, - "timestamp": 1.841516838968092 + "heading": 4.825304787734528e-17, + "angularVelocity": 4.592915652788645e-17, + "velocityX": 2.302361736529145, + "velocityY": -1.825148306820832, + "timestamp": 1.2801092786524113 + }, + { + "x": 4.11697825249087, + "y": 5.353217174122665, + "heading": 5.003311226824959e-17, + "angularVelocity": 4.404060645079727e-17, + "velocityX": 2.1963179706677165, + "velocityY": -1.9660406239470811, + "timestamp": 1.3205280076771686 + }, + { + "x": 4.201472100752748, + "y": 5.268051894541109, + "heading": 5.106612036315502e-17, + "angularVelocity": 2.555768903480938e-17, + "velocityX": 2.0904627706110244, + "velocityY": -2.1070746566406697, + "timestamp": 1.360946736701926 + }, + { + "x": 4.281698714286708, + "y": 5.177177737015351, + "heading": 5.2015291988095576e-17, + "angularVelocity": 2.3483498091432662e-17, + "velocityX": 1.9848870924323019, + "velocityY": -2.2483180376661425, + "timestamp": 1.4013654657266832 + }, + { + "x": 4.357676572309284, + "y": 5.080580936550585, + "heading": 5.3034148842228453e-17, + "angularVelocity": 2.5207570315775117e-17, + "velocityX": 1.8797686086529983, + "velocityY": -2.3899019785048825, + "timestamp": 1.4417841947514405 + }, + { + "x": 4.429441369664381, + "y": 4.978235165876808, + "heading": 5.4156772615538654e-17, + "angularVelocity": 2.7774872934374724e-17, + "velocityX": 1.7755332512056952, + "velocityY": -2.5321372824774984, + "timestamp": 1.4822029237761978 + }, + { + "x": 4.497091014290783, + "y": 4.870069946485337, + "heading": 5.562432201646814e-17, + "angularVelocity": 3.6308680200224936e-17, + "velocityX": 1.6737202346179283, + "velocityY": -2.6761162956262776, + "timestamp": 1.522621652800955 + }, + { + "x": 4.561997626649892, + "y": 4.755326331353034, + "heading": 5.3639840433255375e-17, + "angularVelocity": -4.9098048541470276e-17, + "velocityX": 1.6058548580143424, + "velocityY": -2.8388724212920464, + "timestamp": 1.5630403818257124 + }, + { + "x": 4.631511261933969, + "y": 4.646021074802578, + "heading": 5.11321781778172e-17, + "angularVelocity": -6.20420508556326e-17, + "velocityX": 1.7198372378713005, + "velocityY": -2.704321961318929, + "timestamp": 1.6034591108504697 + }, + { + "x": 4.705480207686577, + "y": 4.542279156484067, + "heading": 4.966571246690934e-17, + "angularVelocity": -3.628181222560111e-17, + "velocityX": 1.8300660989972783, + "velocityY": -2.566679379125897, + "timestamp": 1.643877839875227 + }, + { + "x": 4.783858959652748, + "y": 4.44413671743755, + "heading": 4.8920670981753807e-17, + "angularVelocity": -1.843305932153545e-17, + "velocityX": 1.939169139097368, + "velocityY": -2.4281426312595698, + "timestamp": 1.6842965688999842 + }, + { + "x": 4.86662561684807, + "y": 4.3516109378583465, + "heading": 4.74522093373318e-17, + "angularVelocity": -3.633120321770328e-17, + "velocityX": 2.0477303268199805, + "velocityY": -2.2891808280849295, + "timestamp": 1.7247152979247415 }, { "x": 4.9537672996521, "y": 4.264711856842041, - "heading": -6.195211405748592e-17, - "angularVelocity": 8.681501204664676e-17, - "velocityX": 2.110823131285873, - "velocityY": -2.000343014899152, - "timestamp": 1.8810058190746402 - }, - { - "x": 5.140611798119327, - "y": 4.122815195708918, - "heading": -5.763201257610311e-17, - "angularVelocity": 5.330138299303481e-17, - "velocityX": 2.305304898128395, - "velocityY": -1.7507342770944088, - "timestamp": 1.9620556180018687 - }, - { - "x": 5.342270775871548, - "y": 4.001853598441547, - "heading": -4.9592622237969717e-17, - "angularVelocity": 9.919017350845832e-17, - "velocityX": 2.488087329199462, - "velocityY": -1.492435501001034, - "timestamp": 2.043105416929097 - }, - { - "x": 5.557422233205499, - "y": 3.9027025181083355, - "heading": -4.2459537824589503e-17, - "angularVelocity": 8.80081167138009e-17, - "velocityX": 2.6545588042826695, - "velocityY": -1.2233353030661955, - "timestamp": 2.1241552158563257 - }, - { - "x": 5.784130404385857, - "y": 3.8264465892983552, - "heading": -3.761401308233599e-17, - "angularVelocity": 5.978405423676299e-17, - "velocityX": 2.7971466206678173, - "velocityY": -0.9408527821223643, - "timestamp": 2.2052050147835542 - }, - { - "x": 6.019396811829555, - "y": 3.7743671389893727, - "heading": -3.166083335314611e-17, - "angularVelocity": 7.345025936850683e-17, - "velocityX": 2.9027389401800168, - "velocityY": -0.6425611290798379, - "timestamp": 2.2862548137107828 - }, - { - "x": 6.258339960092501, - "y": 3.747669315728899, - "heading": -3.233315846876214e-17, - "angularVelocity": -8.295259833338512e-18, - "velocityX": 2.948102912326617, - "velocityY": -0.32940024053407974, - "timestamp": 2.3673046126380113 - }, - { - "x": 6.493116863691712, - "y": 3.7462773751063128, - "heading": -3.323952522012367e-17, - "angularVelocity": -1.118285892488459e-17, - "velocityX": 2.89669939600846, - "velocityY": -0.017173893603773912, - "timestamp": 2.44835441156524 - }, - { - "x": 6.713625196897221, - "y": 3.7661963030569305, - "heading": -3.285398522445789e-17, - "angularVelocity": 4.756788622971773e-18, - "velocityX": 2.72065243992446, - "velocityY": 0.24576159613832255, - "timestamp": 2.5294042104924683 - }, - { - "x": 6.912235405370678, - "y": 3.7994650405286214, - "heading": -3.0556799809969664e-17, - "angularVelocity": 2.834291294084942e-17, - "velocityX": 2.450471328784962, - "velocityY": 0.41047279466928444, - "timestamp": 2.610454009419697 - }, - { - "x": 7.0858734676149435, - "y": 3.8385764928806374, - "heading": -2.668740545037057e-17, - "angularVelocity": 4.7740713033428904e-17, - "velocityX": 2.1423626528855833, - "velocityY": 0.4825607573534232, - "timestamp": 2.6915038083469254 - }, - { - "x": 7.233873696789502, - "y": 3.8783565470591044, - "heading": -2.1696316515086995e-17, - "angularVelocity": 6.158022408908137e-17, - "velocityX": 1.8260406704834464, - "velocityY": 0.490810029223817, - "timestamp": 2.772553607274154 - }, - { - "x": 7.356370026927679, - "y": 3.9154353558580755, - "heading": -1.8133459969897106e-17, - "angularVelocity": 4.395875811080922e-17, - "velocityX": 1.5113711787120845, - "velocityY": 0.4574818110732585, - "timestamp": 2.8536034062013824 - }, - { - "x": 7.453704189310843, - "y": 3.9475532207908426, - "heading": -1.3708657227789814e-17, - "angularVelocity": 5.459339466075284e-17, - "velocityX": 1.2009179994537236, - "velocityY": 0.39627322163647194, - "timestamp": 2.934653205128611 - }, - { - "x": 7.526241371253178, - "y": 3.973124678040023, - "heading": -8.453544536238416e-18, - "angularVelocity": 6.483779098448022e-17, - "velocityX": 0.8949705354351214, - "velocityY": 0.3155030313273332, - "timestamp": 3.0157030040558395 - }, - { - "x": 7.574317422270549, - "y": 3.9909890736877442, - "heading": -3.967819924258361e-18, - "angularVelocity": 5.534514397033646e-17, - "velocityX": 0.593166814156423, - "velocityY": 0.22041258441994602, - "timestamp": 3.096752802983068 + "heading": 4.682424401401172e-17, + "angularVelocity": -1.5536488446652234e-17, + "velocityX": 2.1559728597813255, + "velocityY": -2.1499706475905587, + "timestamp": 1.7651340269494988 + }, + { + "x": 5.149371577077341, + "y": 4.111086251410353, + "heading": 4.245818870652888e-17, + "angularVelocity": -5.3034667663063016e-17, + "velocityX": 2.376013169452415, + "velocityY": -1.8660965214215293, + "timestamp": 1.847458603234601 + }, + { + "x": 5.361995748730517, + "y": 3.981639454553548, + "heading": 3.8311842606234285e-17, + "angularVelocity": -5.0365753698371845e-17, + "velocityX": 2.582754521741994, + "velocityY": -1.5723955433397911, + "timestamp": 1.9297831795197031 + }, + { + "x": 5.590089166249063, + "y": 3.8773917441987824, + "heading": 3.443521699897252e-17, + "angularVelocity": -4.708934879277126e-17, + "velocityX": 2.7706600848603538, + "velocityY": -1.2663012074886832, + "timestamp": 2.0121077558048053 + }, + { + "x": 5.831330212934442, + "y": 3.799628582145428, + "heading": 2.9264449434168703e-17, + "angularVelocity": -6.280941218977205e-17, + "velocityX": 2.9303648748534723, + "velocityY": -0.9445923145111385, + "timestamp": 2.0944323320899074 + }, + { + "x": 6.082008486894108, + "y": 3.749887377260942, + "heading": 2.126679829101086e-17, + "angularVelocity": -9.714783202978127e-17, + "velocityX": 3.044999261088646, + "velocityY": -0.6042084530518639, + "timestamp": 2.1767569083750096 + }, + { + "x": 6.335839306220575, + "y": 3.729546047320075, + "heading": 1.712845100944366e-17, + "angularVelocity": -5.02686298415513e-17, + "velocityX": 3.0832933587884717, + "velocityY": -0.2470869679456106, + "timestamp": 2.2590814846601117 + }, + { + "x": 6.582471637063415, + "y": 3.7378835035769398, + "heading": 1.3745368058462902e-17, + "angularVelocity": -4.109437406088766e-17, + "velocityX": 2.9958530243527477, + "velocityY": 0.10127542263348481, + "timestamp": 2.341406060945214 + }, + { + "x": 6.809467380291713, + "y": 3.768327415425299, + "heading": 1.1162328016061889e-17, + "angularVelocity": -3.137619156324536e-17, + "velocityX": 2.757326590330181, + "velocityY": 0.36980344415440275, + "timestamp": 2.423730637230316 + }, + { + "x": 7.009276028148806, + "y": 3.8103968563967796, + "heading": 8.26924402312721e-18, + "angularVelocity": -3.5142311707247735e-17, + "velocityX": 2.427083829324394, + "velocityY": 0.5110192225827506, + "timestamp": 2.506055213515418 + }, + { + "x": 7.179701211541178, + "y": 3.8557683077640763, + "heading": 5.91482335399215e-18, + "angularVelocity": -2.8599155256386034e-17, + "velocityX": 2.07016168297691, + "velocityY": 0.5511288781087693, + "timestamp": 2.5883797898005203 + }, + { + "x": 7.320624454413459, + "y": 3.899158119195988, + "heading": 3.995408556984211e-18, + "angularVelocity": -2.3315109304438273e-17, + "velocityX": 1.7118004031318457, + "velocityY": 0.5270578166492144, + "timestamp": 2.6707043660856224 + }, + { + "x": 7.432462903898932, + "y": 3.9372219303084806, + "heading": 2.438823399699254e-18, + "angularVelocity": -1.890781257081445e-17, + "velocityX": 1.35850622660075, + "velocityY": 0.4623626726238319, + "timestamp": 2.7530289423707246 + }, + { + "x": 7.515713681888788, + "y": 3.967733484062364, + "heading": 1.1936770855475268e-18, + "angularVelocity": -1.5124745408572555e-17, + "velocityX": 1.0112506100436387, + "velocityY": 0.37062509315884895, + "timestamp": 2.8353535186558267 + }, + { + "x": 7.570835923487974, + "y": 3.989130376969442, + "heading": 2.2208685009288135e-19, + "angularVelocity": -1.1801843204007997e-17, + "velocityX": 0.6695721264120479, + "velocityY": 0.25990893452889574, + "timestamp": 2.917678094940929 }, { "x": 7.598227500915527, "y": 4.000265598297119, - "heading": -3.691475085618823e-33, - "angularVelocity": 4.8955194718279566e-17, - "velocityX": 0.29500478670651564, - "velocityY": 0.114454628295526, - "timestamp": 3.1778026019102965 + "heading": -1.415925420811752e-32, + "angularVelocity": -2.6977014897899104e-18, + "velocityX": 0.3327266129215478, + "velocityY": 0.13525998955023022, + "timestamp": 3.000002671226031 }, { "x": 7.598227500915527, "y": 4.000265598297119, - "heading": -4.750113876264814e-34, - "angularVelocity": 3.9684907899163274e-32, - "velocityX": 8.530993042269065e-31, - "velocityY": -1.6956516929480016e-30, - "timestamp": 3.258852400837525 + "heading": -2.7014243414553336e-34, + "angularVelocity": 1.6871197561712535e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 3.082327247511133 } ], "constraints": [ @@ -4888,7 +1294,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 26 + "controlIntervalCount": 25 }, { "x": 7.493931531906128, @@ -4906,243 +1312,234 @@ "y": 7.39399003982544, "heading": 0, "angularVelocity": 0, - "velocityX": 0, + "velocityX": 2.82118644197349e-37, "velocityY": 0, "timestamp": 0 }, { - "x": 1.530136363451793, + "x": 1.5360008147282442, "y": 7.39399003982544, - "heading": 1.63546146728146e-19, - "angularVelocity": 1.69832040317376e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.2961741231159756e-22, - "timestamp": 0.096298758216477 + "heading": 3.2333963174943666e-19, + "angularVelocity": 3.292766805519721e-18, + "velocityX": 0.4284174372625368, + "velocityY": 1.8392808793575992e-26, + "timestamp": 0.09819694336189172 }, { - "x": 1.602546026150824, + "x": 1.6201393797142145, "y": 7.39399003982544, - "heading": 5.000550672728244e-19, - "angularVelocity": 3.494426359967847e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.641598990275319e-22, - "timestamp": 0.192597516432954 + "heading": 7.027380416999147e-19, + "angularVelocity": 3.863647922469736e-18, + "velocityX": 0.8568348678215871, + "velocityY": 3.963342552313322e-26, + "timestamp": 0.19639388672378344 }, { - "x": 1.7111605195237438, + "x": 1.746347225986537, "y": 7.39399003982544, - "heading": 1.021045803758983e-18, - "angularVelocity": 5.410150084422451e-18, - "velocityX": 1.1278909031075715, - "velocityY": 4.0472192553510183e-22, - "timestamp": 0.288896274649431 + "heading": 8.361393183209098e-19, + "angularVelocity": 1.3585074996311424e-18, + "velocityX": 1.2852522894444904, + "velocityY": 6.468173032965746e-26, + "timestamp": 0.29459083008567516 }, { - "x": 1.8559798429712064, + "x": 1.9146243523169562, "y": 7.39399003982544, - "heading": 1.7409021964949573e-18, - "angularVelocity": 7.475240644111669e-18, - "velocityX": 1.5038545265756458, - "velocityY": 5.528083834903345e-22, - "timestamp": 0.385195032865908 + "heading": 3.80235401897157e-19, + "angularVelocity": -4.642750516315599e-18, + "velocityX": 1.7136696985593167, + "velocityY": 9.506575448004094e-26, + "timestamp": 0.3927877734475669 }, { - "x": 2.0370039957226234, + "x": 2.12497075686256, "y": 7.39399003982544, - "heading": 2.678153616617339e-18, - "angularVelocity": 9.732746683078845e-18, - "velocityX": 1.8798181420416615, - "velocityY": 7.105691212840138e-22, - "timestamp": 0.481493791082385 + "heading": -1.0623970199278788e-18, + "angularVelocity": -1.4691215100259113e-17, + "velocityX": 2.1420870889066284, + "velocityY": 1.3343481989110454e-25, + "timestamp": 0.4909847168094586 }, { - "x": 2.254232976750543, + "x": 2.3773864365383615, "y": 7.39399003982544, - "heading": 3.8575512078730386e-18, - "angularVelocity": 1.2247277267116202e-17, - "velocityX": 2.25578174683826, - "velocityY": 8.812289177822076e-22, - "timestamp": 0.577792549298862 + "heading": -3.9564889545725385e-18, + "angularVelocity": -2.947232200024047e-17, + "velocityX": 2.5705044478376236, + "velocityY": 1.8500903675379378e-25, + "timestamp": 0.5891816601713503 }, { - "x": 2.5076667846165317, + "x": 2.6718713848686027, "y": 7.39399003982544, - "heading": 5.3137068489139784e-18, - "angularVelocity": 1.512122972072063e-17, - "velocityX": 2.6317453366976604, - "velocityY": 1.0699474323795816e-21, - "timestamp": 0.6740913075153391 + "heading": -8.853646546888228e-18, + "angularVelocity": -4.987077429157769e-17, + "velocityX": 2.9989217408219764, + "velocityY": 2.625793984498987e-25, + "timestamp": 0.6873786035332421 }, { - "x": 2.7973054171629355, + "x": 3.0084255847706767, "y": 7.39399003982544, - "heading": 7.098464960583512e-18, - "angularVelocity": 1.8533552797092878e-17, - "velocityX": 3.007708904151223, - "velocityY": 1.2857541020722067e-21, - "timestamp": 0.7703900657318161 + "heading": -1.6413613329692498e-17, + "angularVelocity": -7.698780165331962e-17, + "velocityX": 3.4273388598436147, + "velocityY": 4.161152605292971e-25, + "timestamp": 0.7855755468951338 }, { - "x": 3.1231488707936497, + "x": 3.3798020724552167, "y": 7.39399003982544, - "heading": 9.299048469122608e-18, - "angularVelocity": 2.285162928757611e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.5467079012007584e-21, - "timestamp": 0.8666888239482932 + "heading": -2.58741769005412e-17, + "angularVelocity": -9.634275005875749e-17, + "velocityX": 3.781955679779987, + "velocityY": -5.487818039619291e-25, + "timestamp": 0.8837724902570254 }, { - "x": 3.4851971383163978, + "x": 3.7511785589388484, "y": 7.39399003982544, - "heading": 1.2088326363522958e-17, - "angularVelocity": 2.896483790183752e-17, - "velocityX": 3.7596358896848248, - "velocityY": 1.8979567079276025e-21, - "timestamp": 0.9629875821647702 + "heading": 5.146033898598045e-18, + "angularVelocity": 3.158979265252523e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.295977316525143e-24, + "timestamp": 0.9819694336189171 }, { - "x": 3.883450198153611, + "x": 4.122555045422488, "y": 7.39399003982544, - "heading": 1.5983950666179298e-17, - "angularVelocity": 4.0453526414309363e-17, - "velocityX": 4.1355991210390295, - "velocityY": 2.520086451941594e-21, - "timestamp": 1.0592863403812471 + "heading": 4.0038826563950476e-17, + "angularVelocity": 3.553348146159671e-16, + "velocityX": 3.7819556675504797, + "velocityY": 7.623936784532972e-25, + "timestamp": 1.0801663769808088 }, { - "x": 4.290437753988616, + "x": 4.493931531906129, "y": 7.39399003982544, - "heading": -1.0811293578876384e-19, - "angularVelocity": -1.671056190093009e-16, - "velocityX": 4.226301183657094, - "velocityY": -2.0251172410052617e-20, - "timestamp": 1.155585098597724 + "heading": 6.570668459860429e-17, + "angularVelocity": 2.613916192895976e-16, + "velocityX": 3.7819556675504797, + "velocityY": 3.4278643163770276e-26, + "timestamp": 1.1783633203427004 }, { - "x": 4.697425309823639, + "x": 4.865308018389769, "y": 7.39399003982544, - "heading": -8.423356676535005e-18, - "angularVelocity": -8.634840048564003e-17, - "velocityX": 4.226301183657286, - "velocityY": 2.1724036271938957e-21, - "timestamp": 1.251883856814201 + "heading": 6.663782988380241e-17, + "angularVelocity": 9.482426370100795e-18, + "velocityX": 3.7819556675504797, + "velocityY": 2.693381196266241e-25, + "timestamp": 1.276560263704592 }, { - "x": 5.104412865658644, + "x": 5.236684504873409, "y": 7.39399003982544, - "heading": -8.042445280935507e-18, - "angularVelocity": 3.955517211854611e-18, - "velocityX": 4.226301183657094, - "velocityY": -6.018603432474332e-22, - "timestamp": 1.348182615030678 + "heading": 5.2326006182614025e-17, + "angularVelocity": -1.4574612214194992e-16, + "velocityX": 3.7819556675504797, + "velocityY": 5.124149697811417e-24, + "timestamp": 1.3747572070664837 }, { - "x": 5.502665925495857, + "x": 5.60806099135704, "y": 7.39399003982544, - "heading": -6.3335804348522236e-18, - "angularVelocity": 1.7745450464376603e-17, - "velocityX": 4.13559912103903, - "velocityY": 1.2361742339090949e-21, - "timestamp": 1.4444813732471549 + "heading": 2.884064553605338e-17, + "angularVelocity": -2.3916590315859936e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.1014556301472372e-23, + "timestamp": 1.4729541504283754 }, { - "x": 5.864714193018606, + "x": 5.9794374790415805, "y": 7.39399003982544, - "heading": -5.002513564509544e-18, - "angularVelocity": 1.3822263999404139e-17, - "velocityX": 3.759635889684825, - "velocityY": 1.1237973471200677e-21, - "timestamp": 1.5407801314636318 + "heading": 1.6502190572837753e-17, + "angularVelocity": -1.2565009195595738e-16, + "velocityX": 3.7819556797799856, + "velocityY": 6.524035237986936e-24, + "timestamp": 1.571151093790267 }, { - "x": 6.19055764664932, + "x": 6.315991678943653, "y": 7.39399003982544, - "heading": -3.8974854517760974e-18, - "angularVelocity": 1.1474998421478588e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.0114186808354653e-21, - "timestamp": 1.6370788896801087 + "heading": 9.967918627860597e-18, + "angularVelocity": -6.65425189408073e-17, + "velocityX": 3.427338859843601, + "velocityY": -3.262675265981889e-25, + "timestamp": 1.6693480371521587 }, { - "x": 6.480196279195724, + "x": 6.610476627273894, "y": 7.39399003982544, - "heading": -2.9674142808273095e-18, - "angularVelocity": 9.658184315185454e-18, - "velocityX": 3.0077089041512233, - "velocityY": 8.990394180618527e-22, - "timestamp": 1.7333776478965857 + "heading": 5.2731830670839714e-18, + "angularVelocity": -4.78093860085579e-17, + "velocityX": 2.9989217408219764, + "velocityY": -2.379324733264266e-25, + "timestamp": 1.7675449805140504 }, { - "x": 6.733630087061712, + "x": 6.862892306949696, "y": 7.39399003982544, - "heading": -2.1863127731116462e-18, - "angularVelocity": 8.111231213379657e-18, - "velocityX": 2.631745336697661, - "velocityY": 7.866598563444379e-22, - "timestamp": 1.8296764061130626 + "heading": 2.2289001785398693e-18, + "angularVelocity": -3.10018090849907e-17, + "velocityX": 2.5705044478376236, + "velocityY": -1.7314309405461854e-25, + "timestamp": 1.865741923875942 }, { - "x": 6.950859068089632, + "x": 7.0732387114953, "y": 7.39399003982544, - "heading": -1.5381923338445708e-18, - "angularVelocity": 6.730309280881326e-18, - "velocityX": 2.2557817468382604, - "velocityY": 6.742801150833687e-22, - "timestamp": 1.9259751643295395 + "heading": 5.002812433628901e-19, + "angularVelocity": -1.760359206249271e-17, + "velocityX": 2.1420870889066284, + "velocityY": -1.2661949502166272e-25, + "timestamp": 1.9639388672378337 }, { - "x": 7.13188322084105, + "x": 7.24151583782572, "y": 7.39399003982544, - "heading": -1.0119189398601327e-18, - "angularVelocity": 5.4650069455448464e-18, - "velocityX": 1.8798181420416618, - "velocityY": 5.619002540306745e-22, - "timestamp": 2.0222739225460167 + "heading": -2.663585350994398e-19, + "angularVelocity": -7.807165345874594e-18, + "velocityX": 1.7136696985593165, + "velocityY": -9.096401200333518e-26, + "timestamp": 2.0621358105997256 }, { - "x": 7.276702544288512, + "x": 7.367723684098041, "y": 7.39399003982544, - "heading": -6.000129613332895e-19, - "angularVelocity": 4.277375706520152e-18, - "velocityX": 1.5038545265756458, - "velocityY": 4.495203072508259e-22, - "timestamp": 2.118572680762494 + "heading": -4.029353027704155e-19, + "angularVelocity": -1.390845344762092e-18, + "velocityX": 1.2852522894444902, + "velocityY": -6.226126404979919e-26, + "timestamp": 2.1603327539616175 }, { - "x": 7.385317037661432, + "x": 7.451862249084011, "y": 7.39399003982544, - "heading": -2.9682943520502703e-19, - "angularVelocity": 3.1483637414904354e-18, - "velocityX": 1.1278909031075715, - "velocityY": 3.3714029617848224e-22, - "timestamp": 2.214871438978971 - }, - { - "x": 7.457726700360463, - "y": 7.39399003982544, - "heading": -9.799024750037137e-20, - "angularVelocity": 2.064815645768494e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.2476023506664887e-22, - "timestamp": 2.311170197195448 + "heading": -2.1995708883691042e-19, + "angularVelocity": 1.8633799906775463e-18, + "velocityX": 0.8568348678215871, + "velocityY": -3.833262935804246e-26, + "timestamp": 2.2585296973235094 }, { "x": 7.493931531906128, "y": 7.39399003982544, "heading": 0, - "angularVelocity": 1.017564989623581e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.1238013392767656e-22, - "timestamp": 2.4074689554119253 + "angularVelocity": 2.2399586452459634e-18, + "velocityX": 0.42841743726253684, + "velocityY": -1.7862050890834192e-26, + "timestamp": 2.3567266406854013 }, { "x": 7.493931531906128, "y": 7.39399003982544, "heading": 0, "angularVelocity": 0, - "velocityX": -2.537301157014256e-39, + "velocityX": -2.1810930337522913e-39, "velocityY": 0, - "timestamp": 2.5037677136284024 + "timestamp": 2.454923584047293 } ], "constraints": [ @@ -5164,1788 +1561,30 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "SQUARFE": { + "Tag14": { "waypoints": [ { - "x": 0, - "y": 2, - "heading": 0, + "x": 6.224338054656982, + "y": 4.105148, + "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 16 }, { - "x": 1, - "y": 2, - "heading": 0, + "x": 5.77, + "y": 4.105148, + "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 6 }, { - "x": 1, - "y": 1, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 - }, - { - "x": 0, - "y": 1, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 - }, - { - "x": 0, - "y": 2, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0, - "y": 2, - "heading": 1.066025235117479e-35, - "angularVelocity": 0, - "velocityX": -3.176387274976944e-34, - "velocityY": -5.951320349991925e-34, - "timestamp": 0 - }, - { - "x": 0.005019255586936127, - "y": 2.002260189779503, - "heading": -2.3828228381393305e-18, - "angularVelocity": -2.979840326637856e-17, - "velocityX": 0.0627683285032655, - "velocityY": 0.028264815788379395, - "timestamp": 0.07996478011478358 - }, - { - "x": 0.015089028014644117, - "y": 2.006709823197785, - "heading": -4.5974330969027984e-18, - "angularVelocity": -2.769481997390876e-17, - "velocityX": 0.12592759478927565, - "velocityY": 0.055644915322655746, - "timestamp": 0.15992956022956717 - }, - { - "x": 0.030245129831638854, - "y": 2.0132643642969383, - "heading": -6.642004718817398e-18, - "angularVelocity": -2.5568400908806152e-17, - "velocityX": 0.18953471509881312, - "velocityY": 0.08196784996775677, - "timestamp": 0.23989434034435075 - }, - { - "x": 0.050528852743670936, - "y": 2.0218211251492875, - "heading": -7.257064379564418e-18, - "angularVelocity": -7.691631736143833e-18, - "velocityX": 0.2536582090629937, - "velocityY": 0.10700662016536089, - "timestamp": 0.31985912045913434 - }, - { - "x": 0.07598806396033884, - "y": 2.032252906444552, - "heading": -7.528860856473167e-18, - "angularVelocity": -3.3989522858672543e-18, - "velocityX": 0.31838030668155515, - "velocityY": 0.13045469868472426, - "timestamp": 0.3998239005739179 - }, - { - "x": 0.1066783911012953, - "y": 2.044398398790724, - "heading": -7.622887589894659e-18, - "angularVelocity": -1.1758518205026792e-18, - "velocityX": 0.38379805580535253, - "velocityY": 0.151885521710158, - "timestamp": 0.4797886806887015 - }, - { - "x": 0.14266416316439484, - "y": 2.0580471012266317, - "heading": -7.535437739911399e-18, - "angularVelocity": 1.0936045537373189e-18, - "velocityX": 0.4500202715676151, - "velocityY": 0.17068392380140238, - "timestamp": 0.5597534608034851 - }, - { - "x": 0.18401799909562916, - "y": 2.0729145182854087, - "heading": -7.261962767216934e-18, - "angularVelocity": 3.4199427139841472e-18, - "velocityX": 0.5171506239606228, - "velocityY": 0.1859245662582475, - "timestamp": 0.6397182409182687 - }, - { - "x": 0.23081547811659442, - "y": 2.088599322388309, - "heading": -5.496441495113289e-18, - "angularVelocity": 2.2078735582384192e-17, - "velocityX": 0.5852261327273193, - "velocityY": 0.19614640445938436, - "timestamp": 0.7196830210330523 - }, - { - "x": 0.2831131435217446, - "y": 2.1045062001347263, - "heading": -3.615528657766951e-18, - "angularVelocity": 2.352176542156873e-17, - "velocityX": 0.6540087439755441, - "velocityY": 0.19892354763665143, - "timestamp": 0.7996478011478358 - }, - { - "x": 0.3408701135805606, - "y": 2.1197078453164817, - "heading": -1.7358822624727144e-18, - "angularVelocity": 2.350592756240704e-17, - "velocityX": 0.7222801085166501, - "velocityY": 0.19010425789872726, - "timestamp": 0.8796125812626194 - }, - { - "x": 0.403692097997754, - "y": 2.13275380476459, - "heading": -9.4361640863198e-19, - "angularVelocity": 9.907684705793237e-18, - "velocityX": 0.7856206735892604, - "velocityY": 0.16314631803378768, - "timestamp": 0.959577361377403 - }, - { - "x": 0.4702607204262372, - "y": 2.1417668933259137, - "heading": -6.362257579471842e-19, - "angularVelocity": 3.844075127130267e-18, - "velocityX": 0.8324742759616, - "velocityY": 0.11271322885382787, - "timestamp": 1.0395421414921866 - }, - { - "x": 0.5382796795158337, - "y": 2.145469815282364, - "heading": -2.2167383910431224e-19, - "angularVelocity": 5.184181008606903e-18, - "velocityX": 0.850611469098773, - "velocityY": 0.04630691100676233, - "timestamp": 1.1195069216069702 - }, - { - "x": 0.6057260502322735, - "y": 2.143697931570498, - "heading": 3.6199909161643833e-19, - "angularVelocity": 7.299124814607158e-18, - "velocityX": 0.8434509620313541, - "velocityY": -0.022158301558787858, - "timestamp": 1.1994717017217538 - }, - { - "x": 0.6713533698714674, - "y": 2.136730625262623, - "heading": 1.6921521064800653e-18, - "angularVelocity": 1.6634235500642562e-17, - "velocityX": 0.8207028087239234, - "velocityY": -0.08712968756837854, - "timestamp": 1.2794364818365374 - }, - { - "x": 0.734431350428031, - "y": 2.1248845651211443, - "heading": 2.0691864465641914e-18, - "angularVelocity": 4.7150049781463826e-18, - "velocityX": 0.7888220347260337, - "velocityY": -0.14814097061824272, - "timestamp": 1.359401261951321 - }, - { - "x": 0.7945085091672697, - "y": 2.10842369823011, - "heading": 1.8471511862263127e-18, - "angularVelocity": -2.7766630484638473e-18, - "velocityX": 0.7512952408923314, - "velocityY": -0.20585146194869802, - "timestamp": 1.4393660420661045 - }, - { - "x": 0.851286615700003, - "y": 2.0875562872881996, - "heading": 1.5362209056083268e-18, - "angularVelocity": -3.8883400349055174e-18, - "velocityX": 0.7100389252772585, - "velocityY": -0.260957522948938, - "timestamp": 1.519330822180888 - }, - { - "x": 0.9045570425926258, - "y": 2.0624465230223628, - "heading": 1.13056857408644e-18, - "angularVelocity": -5.0728873391071146e-18, - "velocityX": 0.6661736181373479, - "velocityY": -0.3140102959051894, - "timestamp": 1.5992956022956717 - }, - { - "x": 0.9541669202924247, - "y": 2.0332255838005793, - "heading": 6.201980366388928e-19, - "angularVelocity": -6.382441609832398e-18, - "velocityX": 0.6203965999604764, - "velocityY": -0.36542261705514717, - "timestamp": 1.6792603824104553 - }, - { - "x": 1, - "y": 2, - "heading": 0, - "angularVelocity": -7.755889945091827e-18, - "velocityX": 0.5731658317797576, - "velocityY": -0.41550272198442756, - "timestamp": 1.7592251625252389 - }, - { - "x": 1.0360682198258142, - "y": 1.9690252357457936, - "heading": -9.890855738376694e-19, - "angularVelocity": -1.4593472195699707e-17, - "velocityX": 0.5321688938798576, - "velocityY": -0.45701745500488516, - "timestamp": 1.8270010514218133 - }, - { - "x": 1.0692669632152427, - "y": 1.9353295083481672, - "heading": -2.0665277353015157e-18, - "angularVelocity": -1.5897129978073518e-17, - "velocityX": 0.4898311763950358, - "velocityY": -0.4971639316903078, - "timestamp": 1.8947769403183878 - }, - { - "x": 1.0995000427990536, - "y": 1.8990179878910807, - "heading": -2.6852884598983727e-18, - "angularVelocity": -9.129510523370085e-18, - "velocityX": 0.4460742614522743, - "velocityY": -0.5357586753675423, - "timestamp": 1.9625528292149623 - }, - { - "x": 1.1266661316570161, - "y": 1.860210249756036, - "heading": -3.1299706826939245e-18, - "angularVelocity": -6.561067790129639e-18, - "velocityX": 0.40082231749727, - "velocityY": -0.5725891429364366, - "timestamp": 2.030328718111537 - }, - { - "x": 1.1506592197210015, - "y": 1.8190425115321425, - "heading": -3.75029364893705e-18, - "angularVelocity": -9.152560722079088e-18, - "velocityX": 0.3540062469796368, - "velocityY": -0.6074097867859044, - "timestamp": 2.098104607008111 - }, - { - "x": 1.1713694688316625, - "y": 1.77567011857456, - "heading": -4.734849315417307e-18, - "angularVelocity": -1.452663543228679e-17, - "velocityX": 0.3055695682909384, - "velocityY": -0.6399383861090825, - "timestamp": 2.1658804959046853 - }, - { - "x": 1.1886846128926962, - "y": 1.7302702159613776, - "heading": -4.365253326644485e-18, - "angularVelocity": 5.453207120625985e-18, - "velocityX": 0.25547645841217265, - "velocityY": -0.6698532966858195, - "timestamp": 2.2336563848012596 - }, - { - "x": 1.2024920726171215, - "y": 1.6830444754865612, - "heading": -3.8567761493787476e-18, - "angularVelocity": 7.502330767109902e-18, - "velocityX": 0.20372229636848332, - "velocityY": -0.6967926388524772, - "timestamp": 2.301432273697834 - }, - { - "x": 1.2126819581312744, - "y": 1.6342216471211168, - "heading": -3.4963000828679734e-18, - "angularVelocity": 5.318647051824287e-18, - "velocityX": 0.1503467631343443, - "velocityY": -0.720356887387306, - "timestamp": 2.369208162594408 - }, - { - "x": 1.2191510957893172, - "y": 1.5840595772212502, - "heading": -3.2938741554653124e-18, - "angularVelocity": 2.986694853995746e-18, - "velocityX": 0.0954489533573625, - "velocityY": -0.7401167393970102, - "timestamp": 2.4369840514909824 - }, - { - "x": 1.2218081103679437, - "y": 1.5328461987368172, - "heading": -3.259573872067373e-18, - "angularVelocity": 5.060835243168827e-19, - "velocityX": 0.03920294697544978, - "velocityY": -0.7556282819482338, - "timestamp": 2.5047599403875567 - }, - { - "x": 1.220579396344816, - "y": 1.4808988958720275, - "heading": -3.3453058153274248e-18, - "angularVelocity": -1.2649331292104237e-18, - "velocityX": -0.018129072788743095, - "velocityY": -0.7664569762273552, - "timestamp": 2.572535829284131 - }, - { - "x": 1.2154155233457054, - "y": 1.4285616503210614, - "heading": -3.618440035746402e-18, - "angularVelocity": -4.029961768868618e-18, - "velocityX": -0.07619041348156665, - "velocityY": -0.7722103893145882, - "timestamp": 2.640311718180705 - }, - { - "x": 1.2062972964706975, - "y": 1.3761995720847502, - "heading": -4.087599432286243e-18, - "angularVelocity": -6.9222164854494395e-18, - "velocityX": -0.13453496550848654, - "velocityY": -0.7725767834076737, - "timestamp": 2.7080876070772795 - }, - { - "x": 1.1932404566060013, - "y": 1.3241908475189814, - "heading": -3.555359678860336e-18, - "angularVelocity": 7.852936361221584e-18, - "velocityX": -0.19264726847951472, - "velocityY": -0.7673632233010427, - "timestamp": 2.7758634959738537 - }, - { - "x": 1.1762980179515987, - "y": 1.2729167338053733, - "heading": -3.2328805268101095e-18, - "angularVelocity": 4.7580215665228305e-18, - "velocityX": -0.24997737293061145, - "velocityY": -0.7565244004671337, - "timestamp": 2.843639384870428 - }, - { - "x": 1.155559596070278, - "y": 1.2227507908644146, - "heading": -3.1247964605575482e-18, - "angularVelocity": 1.5947273141037222e-18, - "velocityX": -0.3059852437046957, - "velocityY": -0.7401738842187882, - "timestamp": 2.9114152737670023 - }, - { - "x": 1.131147710143389, - "y": 1.1740488128086175, - "heading": -3.234068776048195e-18, - "angularVelocity": -1.612259410180699e-18, - "velocityX": -0.36018540404746696, - "velocityY": -0.7185738003394676, - "timestamp": 2.9791911626635765 - }, - { - "x": 1.1032117091845473, - "y": 1.127140753581476, - "heading": -2.267931270616541e-18, - "angularVelocity": 1.4254885098319373e-17, - "velocityX": -0.4121819929425308, - "velocityY": -0.6921054078497251, - "timestamp": 3.046967051560151 - }, - { - "x": 1.0719204023306301, - "y": 1.0823254089032228, - "heading": -1.2961485271050903e-18, - "angularVelocity": 1.4338177791253964e-17, - "velocityX": -0.4616878858153752, - "velocityY": -0.6612284310522442, - "timestamp": 3.114742940456725 - }, - { - "x": 1.0374545343277675, - "y": 1.0398679644991287, - "heading": -5.410966813185065e-19, - "angularVelocity": 1.1140419716282834e-17, - "velocityX": -0.5085269786052852, - "velocityY": -0.6264387689386715, - "timestamp": 3.1825188293532993 - }, - { - "x": 1, - "y": 1, - "heading": 0, - "angularVelocity": 7.983616269770876e-18, - "velocityX": -0.5526232844385539, - "velocityY": -0.5882322629507188, - "timestamp": 3.2502947182498736 - }, - { - "x": 0.9601002931841937, - "y": 0.9632066842447741, - "heading": 3.2977160442109246e-19, - "angularVelocity": 4.906429958430405e-18, - "velocityX": -0.5936384674356385, - "velocityY": -0.547420753682207, - "timestamp": 3.3175068498830584 - }, - { - "x": 0.9176402744830485, - "y": 0.9293405687986124, - "heading": 1.3352918677177412e-18, - "angularVelocity": 1.4960397928012193e-17, - "velocityX": -0.6317314697423027, - "velocityY": -0.5038690876669165, - "timestamp": 3.3847189815162433 - }, - { - "x": 0.8728479847439143, - "y": 0.8985863996924396, - "heading": 2.1525838940809656e-18, - "angularVelocity": 1.2159888120347238e-17, - "velocityX": -0.6664316195711695, - "velocityY": -0.4575687209865014, - "timestamp": 3.451931113149428 - }, - { - "x": 0.8259849242635205, - "y": 0.8711239788151253, - "heading": 2.7942768738430748e-18, - "angularVelocity": 9.547278761162172e-18, - "velocityX": -0.6972410983206457, - "velocityY": -0.4085932139035895, - "timestamp": 3.519143244782613 - }, - { - "x": 0.7773460963851867, - "y": 0.8471213769660901, - "heading": 3.275531455850251e-18, - "angularVelocity": 7.160233562268939e-18, - "velocityX": -0.7236614387382273, - "velocityY": -0.35711710469221697, - "timestamp": 3.586355376415798 - }, - { - "x": 0.7272575825238243, - "y": 0.8267273909004392, - "heading": 3.656107749997039e-18, - "angularVelocity": 5.662315468533709e-18, - "velocityX": -0.7452302529954558, - "velocityY": -0.3034271577183785, - "timestamp": 3.6535675080489827 - }, - { - "x": 0.6760712290601476, - "y": 0.8100641481084726, - "heading": 3.9141657523662e-18, - "angularVelocity": 3.839455683091591e-18, - "velocityX": -0.7615642030672349, - "velocityY": -0.2479201654086435, - "timestamp": 3.7207796396821675 - }, - { - "x": 0.6241565565906713, - "y": 0.7972210163794645, - "heading": 3.9620361753801646e-18, - "angularVelocity": 7.122289175726647e-19, - "velocityX": -0.7724003272624098, - "velocityY": -0.19108353532216987, - "timestamp": 3.7879917713153524 - }, - { - "x": 0.5718906668220771, - "y": 0.7882509123495861, - "heading": 3.933061205617742e-18, - "angularVelocity": -4.310973365379806e-19, - "velocityX": -0.777625831804279, - "velocityY": -0.1334595974255566, - "timestamp": 3.855203902948537 - }, - { - "x": 0.5196474628149044, - "y": 0.7831696619611915, - "heading": 3.841828847804768e-18, - "angularVelocity": -1.3573793776640569e-18, - "velocityX": -0.7772883070022795, - "velocityY": -0.07560019694250741, - "timestamp": 3.922416034581722 - }, - { - "x": 0.46778766247295284, - "y": 0.7819583856222608, - "heading": 3.2741304171392934e-18, - "angularVelocity": -8.446368348915311e-18, - "velocityX": -0.7715839251309572, - "velocityY": -0.018021692059127074, - "timestamp": 3.989628166214907 - }, - { - "x": 0.41665080107502755, - "y": 0.784568242242575, - "heading": 2.3398005355380153e-18, - "angularVelocity": -1.390120864955836e-17, - "velocityX": -0.760827846928116, - "velocityY": 0.03883014207252925, - "timestamp": 4.056840297848092 - }, - { - "x": 0.3665498423457967, - "y": 0.7909265123693979, - "heading": 1.4204312415958456e-18, - "angularVelocity": -1.3678621115538104e-17, - "velocityX": -0.7454154110549615, - "velocityY": 0.09460003681364573, - "timestamp": 4.124052429481277 - }, - { - "x": 0.3177684131174127, - "y": 0.8009429979668188, - "heading": 2.911000402406795e-19, - "angularVelocity": -1.6802490157769785e-17, - "velocityX": -0.7257830996138033, - "velocityY": 0.14902794114738804, - "timestamp": 4.191264561114462 - }, - { - "x": 0.2705602477366799, - "y": 0.8145159621306034, - "heading": -8.90875593988358e-21, - "angularVelocity": -4.4636106745506625e-18, - "velocityX": -0.7023756609651165, - "velocityY": 0.201942176716847, - "timestamp": 4.258476692747647 - }, - { - "x": 0.22515023866998407, - "y": 0.8315371625385247, - "heading": 8.015184743221423e-21, - "angularVelocity": 2.5179924008277626e-19, - "velocityX": -0.6756222122893548, - "velocityY": 0.25324595418005347, - "timestamp": 4.325688824380832 - }, - { - "x": 0.1817364922151659, - "y": 0.8518958223545926, - "heading": -1.0221415362403761e-19, - "angularVelocity": -1.6400213737530966e-18, - "velocityX": -0.6459212853380655, - "velocityY": 0.30290156436603854, - "timestamp": 4.392900956014016 - }, - { - "x": 0.14049289628323774, - "y": 0.8754815778933299, - "heading": 1.279799371183032e-19, - "angularVelocity": 3.4248888768950546e-18, - "velocityX": -0.6136332077223501, - "velocityY": 0.35091515423820036, - "timestamp": 4.460113087647201 - }, - { - "x": 0.1015718469138536, - "y": 0.9021865445377336, - "heading": 2.2387831921490547e-19, - "angularVelocity": 1.4268016689731864e-18, - "velocityX": -0.5790777412297899, - "velocityY": 0.39732360803772554, - "timestamp": 4.527325219280386 - }, - { - "x": 0.06510690798404553, - "y": 0.931906676568605, - "heading": -1.120819147896167e-19, - "angularVelocity": -4.998505735207955e-18, - "velocityX": -0.542535075792835, - "velocityY": 0.4421840419088505, - "timestamp": 4.594537350913571 - }, - { - "x": 0.031215277251384486, - "y": 0.9645425908347474, - "heading": -3.927086503171987e-19, - "angularVelocity": -4.1752392884434696e-18, - "velocityX": -0.5042487108968212, - "velocityY": 0.4855658267804262, - "timestamp": 4.661749482546756 - }, - { - "x": 0, - "y": 1, - "heading": 0, - "angularVelocity": 5.84282387256885e-18, - "velocityX": -0.46442921081188193, - "velocityY": 0.527544779546109, - "timestamp": 4.728961614179941 - }, - { - "x": -0.034523228131504415, - "y": 1.0482920245716583, - "heading": 6.961793842243958e-19, - "angularVelocity": 8.333545907574826e-18, - "velocityX": -0.4132568741634686, - "velocityY": 0.5780748846976158, - "timestamp": 4.81250100648016 - }, - { - "x": -0.06466937577608543, - "y": 1.1006992615641917, - "heading": 2.448822269391661e-18, - "angularVelocity": 2.0979837004191778e-17, - "velocityX": -0.3608614668424137, - "velocityY": 0.6273356263377532, - "timestamp": 4.896040398780379 - }, - { - "x": -0.09031335806620472, - "y": 1.1570844769783872, - "heading": 3.582191703035754e-18, - "angularVelocity": 1.3566885979268263e-17, - "velocityX": -0.30696874353552017, - "velocityY": 0.6749536220177591, - "timestamp": 4.979579791080598 - }, - { - "x": -0.11129933776190112, - "y": 1.217264005888493, - "heading": 4.527037428957119e-18, - "angularVelocity": 1.1310181537234104e-17, - "velocityX": -0.2512105860224372, - "velocityY": 0.7203730749421273, - "timestamp": 5.063119183380817 - }, - { - "x": -0.12742973186409806, - "y": 1.2809814846007999, - "heading": 5.3660105917774544e-18, - "angularVelocity": 1.0042844899524535e-17, - "velocityX": -0.19308728083906038, - "velocityY": 0.7627237517280782, - "timestamp": 5.146658575681036 - }, - { - "x": -0.13845089687734538, - "y": 1.3478595921295984, - "heading": 5.804477769789897e-18, - "angularVelocity": 5.248628037181788e-18, - "velocityX": -0.13192776138040377, - "velocityY": 0.8005577451228699, - "timestamp": 5.230197967981255 - }, - { - "x": -0.14404059723021467, - "y": 1.41730538004403, - "heading": 5.60459161147457e-18, - "angularVelocity": -2.3927171307864518e-18, - "velocityX": -0.06691095301221847, - "velocityY": 0.8312939082063359, - "timestamp": 5.313737360281474 - }, - { - "x": -0.14383032525510003, - "y": 1.488317725972261, - "heading": 4.431739846980368e-18, - "angularVelocity": -1.40395049470865e-17, - "velocityX": 0.002517039797931534, - "velocityY": 0.8500462353500342, - "timestamp": 5.397276752581693 - }, - { - "x": -0.13761503207018486, - "y": 1.559147061402284, - "heading": 3.662869249400604e-18, - "angularVelocity": -9.203688693849832e-18, - "velocityX": 0.07439954988634583, - "velocityY": 0.8478555263543226, - "timestamp": 5.480816144881912 - }, - { - "x": -0.12607515499835087, - "y": 1.627193849249961, - "heading": 1.3243020605488167e-18, - "angularVelocity": -2.7993585531762966e-17, - "velocityX": 0.1381369525691848, - "velocityY": 0.8145473168290962, - "timestamp": 5.564355537182131 - }, - { - "x": -0.11116381180741976, - "y": 1.690268013573467, - "heading": 2.379879606606124e-19, - "angularVelocity": -1.3003614752838011e-17, - "velocityX": 0.17849475295850228, - "velocityY": 0.7550230207185785, - "timestamp": 5.64789492948235 - }, - { - "x": -0.09482890835601043, - "y": 1.747505461760242, - "heading": -2.2107636185603513e-19, - "angularVelocity": -5.4951837817255995e-18, - "velocityX": 0.1955353396958621, - "velocityY": 0.6851551897945106, - "timestamp": 5.731434321782569 - }, - { - "x": -0.07835937233604903, - "y": 1.7987366080747376, - "heading": -5.60981270871949e-19, - "angularVelocity": -4.068797903654375e-18, - "velocityX": 0.19714694548859107, - "velocityY": 0.6132573496630682, - "timestamp": 5.814973714082788 - }, - { - "x": -0.06255809380104226, - "y": 1.843997224564569, - "heading": -8.808563650504602e-19, - "angularVelocity": -3.8290331719286885e-18, - "velocityX": 0.18914763562345505, - "velocityY": 0.5417877152754039, - "timestamp": 5.898513106383007 - }, - { - "x": -0.04794693255938144, - "y": 1.8833690873915618, - "heading": -1.2134869024792098e-18, - "angularVelocity": -3.981720931889883e-18, - "velocityX": 0.17490145474307478, - "velocityY": 0.4712969743124334, - "timestamp": 5.982052498683226 - }, - { - "x": -0.034884553374127325, - "y": 1.9169362042125617, - "heading": -1.5815565915485662e-18, - "angularVelocity": -4.405941868057666e-18, - "velocityX": 0.1563619129321798, - "velocityY": 0.40181183866370773, - "timestamp": 6.065591890983445 - }, - { - "x": -0.0236298825386545, - "y": 1.9447739236006856, - "heading": -7.689111560877529e-19, - "angularVelocity": 9.727690984238907e-18, - "velocityX": 0.13472291963803598, - "velocityY": 0.3332286556273034, - "timestamp": 6.149131283283664 - }, - { - "x": -0.01437751052078003, - "y": 1.9669473155185608, - "heading": -4.414002390832506e-19, - "angularVelocity": 3.9204366617451644e-18, - "velocityX": 0.11075460047188025, - "velocityY": 0.26542438611702496, - "timestamp": 6.232670675583883 - }, - { - "x": -0.007278507844366605, - "y": 1.9835120705105598, - "heading": -6.316031204453377e-19, - "angularVelocity": -2.2768046075674146e-18, - "velocityX": 0.08497790660125235, - "velocityY": 0.19828675473804738, - "timestamp": 6.316210067884102 - }, - { - "x": -0.0024533007608353937, - "y": 1.9945159206791379, - "heading": -4.823898230322713e-19, - "angularVelocity": 1.786142874866039e-18, - "velocityX": 0.05775966224641234, - "velocityY": 0.13172049575166966, - "timestamp": 6.399749460184321 - }, - { - "x": -9.534714777782856e-35, - "y": 2, - "heading": -4.51332949370556e-37, - "angularVelocity": 5.774399435321978e-18, - "velocityX": 0.029366993142814022, - "velocityY": 0.06564662693683221, - "timestamp": 6.48328885248454 - }, - { - "x": 0, - "y": 2, - "heading": -6.22058063415872e-36, - "angularVelocity": -6.906021002654526e-35, - "velocityX": 1.1413436685370076e-33, - "velocityY": -9.106343514179098e-35, - "timestamp": 6.566828244784759 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "SQUARFE (1)": { - "waypoints": [ - { - "x": 0, - "y": 2, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 - }, - { - "x": 1, - "y": 2, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 - }, - { - "x": 1, - "y": 1, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 - }, - { - "x": 0, - "y": 1, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 - }, - { - "x": 0, - "y": 2, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0, - "y": 2, - "heading": 6.834778036374476e-42, - "angularVelocity": 0, - "velocityX": 6.685663635549999e-41, - "velocityY": 1.8473219086701058e-41, - "timestamp": 0 - }, - { - "x": 0.008264463615126694, - "y": 2, - "heading": 1.7039918296957192e-26, - "angularVelocity": 1.739180661511921e-25, - "velocityX": 0.08434788783387884, - "velocityY": -2.7623927453648295e-18, - "timestamp": 0.09798068247308528 - }, - { - "x": 0.024793390752145244, - "y": 2, - "heading": 7.683593549129969e-26, - "angularVelocity": 6.102971583946312e-25, - "velocityX": 0.16869577471619418, - "velocityY": -5.524785459565961e-18, - "timestamp": 0.19596136494617056 - }, - { - "x": 0.04958678129588319, - "y": 2, - "heading": 5.293972470528129e-26, - "angularVelocity": -2.438680388678047e-25, - "velocityX": 0.2530436604230487, - "velocityY": -8.287178135270759e-18, - "timestamp": 0.29394204741925584 - }, - { - "x": 0.08264463510045543, - "y": 2, - "heading": -7.599657248983579e-26, - "angularVelocity": -1.3159126117680205e-24, - "velocityX": 0.33739154464098625, - "velocityY": -1.1049570762213534e-17, - "timestamp": 0.3919227298923411 - }, - { - "x": 0.12396695197508915, - "y": 2, - "heading": -6.356500212054052e-25, - "angularVelocity": -5.711849121394279e-24, - "velocityX": 0.42173942691187855, - "velocityY": -1.3811963325390594e-17, - "timestamp": 0.4899034123654264 - }, - { - "x": 0.1735537316596396, - "y": 2, - "heading": -1.7701677559872392e-24, - "angularVelocity": -1.1578935811088608e-23, - "velocityX": 0.5060873065277093, - "velocityY": -1.6574355801614406e-17, - "timestamp": 0.5878840948385117 - }, - { - "x": 0.23140497377834218, - "y": 2, - "heading": -2.69685519918364e-24, - "angularVelocity": -9.45777203938324e-24, - "velocityX": 0.5904351823084512, - "velocityY": -1.9336748152239084e-17, - "timestamp": 0.6858647773115969 - }, - { - "x": 0.29752067774070967, - "y": 2, - "heading": -3.3710373572859254e-24, - "angularVelocity": -6.88078025268394e-24, - "velocityX": 0.6747830520626255, - "velocityY": -2.2099140305493712e-17, - "timestamp": 0.7838454597846822 - }, - { - "x": 0.37190084248386523, - "y": 2, - "heading": -3.785377470906936e-24, - "angularVelocity": -4.228794191414106e-24, - "velocityX": 0.7591309109689794, - "velocityY": -2.4861532103482302e-17, - "timestamp": 0.8818261422577676 - }, - { - "x": 0.454545465527764, - "y": 2, - "heading": -3.6811470837306276e-24, - "angularVelocity": 1.0637753265934456e-24, - "velocityX": 0.8434787445637631, - "velocityY": -2.7623923072517135e-17, - "timestamp": 0.9798068247308529 - }, - { - "x": 0.5454545344722358, - "y": 2, - "heading": -3.223140769415779e-24, - "angularVelocity": 4.674291630935048e-24, - "velocityX": 0.9278264516012495, - "velocityY": -3.038630989680133e-17, - "timestamp": 1.0777875072039382 - }, - { - "x": 0.6280991575161347, - "y": 2, - "heading": -3.145189007341093e-24, - "angularVelocity": 7.953315368066196e-25, - "velocityX": 0.8434787445637631, - "velocityY": -2.7623923072517132e-17, - "timestamp": 1.1757681896770236 - }, - { - "x": 0.7024793222592902, - "y": 2, - "heading": -2.7590363290180994e-24, - "angularVelocity": 3.940934765433298e-24, - "velocityX": 0.7591309109689796, - "velocityY": -2.48615321034823e-17, - "timestamp": 1.273748872150109 - }, - { - "x": 0.7685950262216578, - "y": 2, - "heading": -2.36523859209739e-24, - "angularVelocity": 4.0191634044675645e-24, - "velocityX": 0.6747830520626255, - "velocityY": -2.209914030549371e-17, - "timestamp": 1.3717295546231942 - }, - { - "x": 0.8264462683403603, - "y": 2, - "heading": -1.912083934393757e-24, - "angularVelocity": 4.624982515888689e-24, - "velocityX": 0.5904351823084512, - "velocityY": -1.9336748152239084e-17, - "timestamp": 1.4697102370962796 - }, - { - "x": 0.8760330480249108, - "y": 2, - "heading": -1.4947650795710447e-24, - "angularVelocity": 4.259255952533442e-24, - "velocityX": 0.5060873065277093, - "velocityY": -1.6574355801614403e-17, - "timestamp": 1.567690919569365 - }, - { - "x": 0.9173553648995445, - "y": 2, - "heading": -1.0599672683839614e-24, - "angularVelocity": 4.4376648384860465e-24, - "velocityX": 0.42173942691187855, - "velocityY": -1.3811963325390594e-17, - "timestamp": 1.6656716020424502 - }, - { - "x": 0.9504132187041168, - "y": 2, - "heading": -6.474555283607888e-25, - "angularVelocity": 4.210197162464856e-24, - "velocityX": 0.33739154464098625, - "velocityY": -1.1049570762213536e-17, - "timestamp": 1.7636522845155356 - }, - { - "x": 0.9752066092478547, - "y": 2, - "heading": -3.2958736625896835e-25, - "angularVelocity": 3.2442420699516962e-24, - "velocityX": 0.2530436604230487, - "velocityY": -8.287178135270759e-18, - "timestamp": 1.861632966988621 - }, - { - "x": 0.9917355363848733, - "y": 2, - "heading": -1.1216781384407191e-25, - "angularVelocity": 2.219040288078826e-24, - "velocityX": 0.16869577471619418, - "velocityY": -5.524785459565961e-18, - "timestamp": 1.9596136494617062 - }, - { - "x": 1, - "y": 2, - "heading": 1.4025423606350947e-41, - "angularVelocity": 1.1448173504163706e-24, - "velocityX": 0.08434788783387884, - "velocityY": -2.7623927453648302e-18, - "timestamp": 2.0575943319347916 - }, - { - "x": 1, - "y": 2, - "heading": 4.353206627192277e-42, - "angularVelocity": -9.871558911369574e-41, - "velocityX": -6.752513577763152e-40, - "velocityY": -2.739881155941097e-39, - "timestamp": 2.155575014407877 - }, - { - "x": 1, - "y": 1.9917355363848732, - "heading": 2.678019676818953e-25, - "angularVelocity": 2.73325426399564e-24, - "velocityX": -7.221388058205676e-18, - "velocityY": -0.08434788783387888, - "timestamp": 2.2535556968809622 - }, - { - "x": 1, - "y": 1.9752066092478548, - "heading": 5.868915118029391e-25, - "angularVelocity": 3.2567428149199723e-24, - "velocityX": -1.4442776034943873e-17, - "velocityY": -0.16869577471619424, - "timestamp": 2.3515363793540476 - }, - { - "x": 1, - "y": 1.9504132187041168, - "heading": 7.292918744038093e-25, - "angularVelocity": 1.4534178819580077e-24, - "velocityX": -2.1664163911045784e-17, - "velocityY": -0.2530436604230488, - "timestamp": 2.449517061827133 - }, - { - "x": 1, - "y": 1.9173553648995445, - "heading": 6.385291298407452e-25, - "angularVelocity": -9.262537240715945e-25, - "velocityX": -2.888555165967506e-17, - "velocityY": -0.33739154464098636, - "timestamp": 2.5474977443002182 - }, - { - "x": 1, - "y": 1.8760330480249108, - "heading": 1.3873456572049932e-25, - "angularVelocity": -5.1008572041181364e-24, - "velocityX": -3.6106939241609364e-17, - "velocityY": -0.4217394269118787, - "timestamp": 2.6454784267733036 - }, - { - "x": 1, - "y": 1.8264462683403602, - "heading": -1.4606320867425958e-24, - "angularVelocity": -1.6323178027645798e-23, - "velocityX": -4.332832659623233e-17, - "velocityY": -0.5060873065277094, - "timestamp": 2.743459109246389 - }, - { - "x": 1, - "y": 1.7685950262216577, - "heading": -2.249822541344819e-24, - "angularVelocity": -8.054428449773172e-24, - "velocityX": -5.054971362251674e-17, - "velocityY": -0.5904351823084514, - "timestamp": 2.8414397917194743 - }, - { - "x": 1, - "y": 1.7024793222592902, - "heading": -1.6693794150187738e-24, - "angularVelocity": 5.924134836898355e-24, - "velocityX": -5.777110013284059e-17, - "velocityY": -0.6747830520626257, - "timestamp": 2.9394204741925596 - }, - { - "x": 1, - "y": 1.6280991575161345, - "heading": -4.17992965329782e-24, - "angularVelocity": -2.5622809982354645e-23, - "velocityX": -6.499248571443555e-17, - "velocityY": -0.7591309109689798, - "timestamp": 3.037401156665645 - }, - { - "x": 1, - "y": 1.545454534472236, - "heading": -1.2224446341565232e-23, - "angularVelocity": -8.210320576787122e-23, - "velocityX": -7.221386912899733e-17, - "velocityY": -0.8434787445637635, - "timestamp": 3.1353818391387303 - }, - { - "x": 1, - "y": 1.454545465527764, - "heading": -1.4447403666940808e-23, - "angularVelocity": -2.2688260164068862e-23, - "velocityX": -7.943524170844061e-17, - "velocityY": -0.9278264516012499, - "timestamp": 3.2333625216118156 - }, - { - "x": 1, - "y": 1.3719008424838655, - "heading": -1.4038421369615485e-23, - "angularVelocity": 4.174104041070465e-24, - "velocityX": -7.221386912899733e-17, - "velocityY": -0.8434787445637635, - "timestamp": 3.331343204084901 - }, - { - "x": 1, - "y": 1.2975206777407098, - "heading": -1.2608666390151249e-23, - "angularVelocity": 1.4592214374369727e-23, - "velocityX": -6.499248571443555e-17, - "velocityY": -0.7591309109689798, - "timestamp": 3.4293238865579863 - }, - { - "x": 1, - "y": 1.2314049737783423, - "heading": -9.223958699370151e-24, - "angularVelocity": 3.4544645258213173e-23, - "velocityX": -5.777110013284059e-17, - "velocityY": -0.6747830520626257, - "timestamp": 3.5273045690310716 - }, - { - "x": 1, - "y": 1.1735537316596398, - "heading": -6.510980866302344e-24, - "angularVelocity": 2.768890658272266e-23, - "velocityX": -5.054971362251675e-17, - "velocityY": -0.5904351823084515, - "timestamp": 3.625285251504157 - }, - { - "x": 1, - "y": 1.1239669519750892, - "heading": -4.570495528808751e-24, - "angularVelocity": 1.980477298357616e-23, - "velocityX": -4.332832659623233e-17, - "velocityY": -0.5060873065277095, - "timestamp": 3.7232659339772423 - }, - { - "x": 1, - "y": 1.0826446351004555, - "heading": -2.7074347601637494e-24, - "angularVelocity": 1.9014567030490677e-23, - "velocityX": -3.610693924160936e-17, - "velocityY": -0.42173942691187877, - "timestamp": 3.8212466164503276 - }, - { - "x": 1, - "y": 1.0495867812958832, - "heading": -1.1867771133422862e-24, - "angularVelocity": 1.551993357249157e-23, - "velocityX": -2.888555165967506e-17, - "velocityY": -0.33739154464098636, - "timestamp": 3.919227298923413 - }, - { - "x": 1, - "y": 1.0247933907521452, - "heading": -6.71373293397456e-25, - "angularVelocity": 5.260245478860946e-24, - "velocityX": -2.166416391104578e-17, - "velocityY": -0.2530436604230488, - "timestamp": 4.017207981396498 - }, - { - "x": 1, - "y": 1.0082644636151268, - "heading": -2.683389851754243e-25, - "angularVelocity": 4.1133864703848926e-24, - "velocityX": -1.444277603494387e-17, - "velocityY": -0.16869577471619424, - "timestamp": 4.115188663869583 - }, - { - "x": 1, - "y": 1, - "heading": 1.3844887180664726e-41, - "angularVelocity": 2.738667729952811e-24, - "velocityX": -7.221388058205674e-18, - "velocityY": -0.08434788783387888, - "timestamp": 4.213169346342668 - }, - { - "x": 1, - "y": 1, - "heading": -1.2548656701086555e-41, - "angularVelocity": -9.97631116732387e-41, - "velocityX": 4.048108670644253e-37, - "velocityY": -1.1182831125559368e-40, - "timestamp": 4.311150028815753 - }, - { - "x": 0.9917355363848733, - "y": 1, - "heading": -1.1243665350537136e-24, - "angularVelocity": -1.1475372005589736e-23, - "velocityX": -0.08434788783387885, - "velocityY": 1.1036137138901194e-18, - "timestamp": 4.409130711288838 - }, - { - "x": 0.9752066092478547, - "y": 1, - "heading": -3.378137956306648e-24, - "angularVelocity": -2.3002166920042362e-23, - "velocityX": -0.16869577471619418, - "velocityY": 2.207227415329915e-18, - "timestamp": 4.507111393761923 - }, - { - "x": 0.9504132187041168, - "y": 1, - "heading": -6.761085388842748e-24, - "angularVelocity": -3.4526627166894346e-23, - "velocityX": -0.2530436604230487, - "velocityY": 3.3108411013898984e-18, - "timestamp": 4.605092076235008 - }, - { - "x": 0.9173553648995445, - "y": 1, - "heading": -1.1133132425652307e-23, - "angularVelocity": -4.46214562056447e-23, - "velocityX": -0.33739154464098625, - "velocityY": 4.414454767968787e-18, - "timestamp": 4.7030727587080925 - }, - { - "x": 0.8760330480249108, - "y": 1, - "heading": -1.6100751630919242e-23, - "angularVelocity": -5.0699909707958593e-23, - "velocityX": -0.42173942691187855, - "velocityY": 5.518068409072398e-18, - "timestamp": 4.801053441181177 - }, - { - "x": 0.8264462683403604, - "y": 1, - "heading": -2.1677679346560742e-23, - "angularVelocity": -5.691865345905286e-23, - "velocityX": -0.5060873065277093, - "velocityY": 6.621682015436997e-18, - "timestamp": 4.899034123654262 - }, - { - "x": 0.7685950262216578, - "y": 1, - "heading": -2.70563073072413e-23, - "angularVelocity": -5.489487623439359e-23, - "velocityX": -0.5904351823084512, - "velocityY": 7.725295571623022e-18, - "timestamp": 4.997014806127347 - }, - { - "x": 0.7024793222592902, - "y": 1, - "heading": -3.2234788403103226e-23, - "angularVelocity": -5.285225361991524e-23, - "velocityX": -0.6747830520626255, - "velocityY": 8.828909048957011e-18, - "timestamp": 5.094995488600432 - }, - { - "x": 0.6280991575161347, - "y": 1, - "heading": -3.5244044696367074e-23, - "angularVelocity": -3.071298695388889e-23, - "velocityX": -0.7591309109689796, - "velocityY": 9.932522384357355e-18, - "timestamp": 5.192976171073517 - }, - { - "x": 0.5454545344722359, - "y": 1, - "heading": -3.185198887062514e-23, - "angularVelocity": 3.4619333983565953e-23, - "velocityX": -0.8434787445637631, - "velocityY": 1.1036135388579327e-17, - "timestamp": 5.290956853546602 - }, - { - "x": 0.4545454655277641, - "y": 1, - "heading": -1.1536119074148839e-23, - "angularVelocity": 2.07345235284118e-22, - "velocityX": -0.9278264516012495, - "velocityY": 1.2139746736916687e-17, - "timestamp": 5.388937536019687 - }, - { - "x": 0.3719008424838653, - "y": 1, - "heading": -1.7080978536028252e-24, - "angularVelocity": 1.0030590652108129e-22, - "velocityX": -0.8434787445637631, - "velocityY": 1.1036135388579327e-17, - "timestamp": 5.486918218492772 - }, - { - "x": 0.2975206777407097, - "y": 1, - "heading": 3.9485980786107975e-24, - "angularVelocity": 5.773286957268272e-23, - "velocityX": -0.7591309109689796, - "velocityY": 9.932522384357355e-18, - "timestamp": 5.5848989009658565 - }, - { - "x": 0.23140497377834224, - "y": 1, - "heading": 6.2504476669030164e-24, - "angularVelocity": 2.3493130652605514e-23, - "velocityX": -0.6747830520626255, - "velocityY": 8.828909048957011e-18, - "timestamp": 5.682879583438941 - }, - { - "x": 0.17355373165963964, - "y": 1, - "heading": 6.985240619947539e-24, - "angularVelocity": 7.499550312311534e-24, - "velocityX": -0.5904351823084512, - "velocityY": 7.725295571623022e-18, - "timestamp": 5.780860265912026 - }, - { - "x": 0.12396695197508918, - "y": 1, - "heading": 6.271694392407826e-24, - "angularVelocity": -7.282389940082428e-24, - "velocityX": -0.5060873065277093, - "velocityY": 6.621682015436996e-18, - "timestamp": 5.878840948385111 - }, - { - "x": 0.08264463510045544, - "y": 1, - "heading": 4.81506621376513e-24, - "angularVelocity": -1.486641000682138e-23, - "velocityX": -0.4217394269118786, - "velocityY": 5.5180684090723976e-18, - "timestamp": 5.976821630858196 - }, - { - "x": 0.04958678129588319, - "y": 1, - "heading": 3.1680135607204943e-24, - "angularVelocity": -1.6809957018793716e-23, - "velocityX": -0.33739154464098625, - "velocityY": 4.414454767968786e-18, - "timestamp": 6.074802313331281 - }, - { - "x": 0.02479339075214524, - "y": 1, - "heading": 1.7613266611845955e-24, - "angularVelocity": -1.4356757129038765e-23, - "velocityX": -0.2530436604230487, - "velocityY": 3.310841101389898e-18, - "timestamp": 6.172782995804366 - }, - { - "x": 0.008264463615126694, - "y": 1, - "heading": 6.558100798178395e-25, - "angularVelocity": -1.1282980364373314e-23, - "velocityX": -0.16869577471619418, - "velocityY": 2.207227415329915e-18, - "timestamp": 6.270763678277451 - }, - { - "x": -2.582873329443503e-41, - "y": 1, - "heading": 0, - "angularVelocity": -6.693229593886659e-24, - "velocityX": -0.08434788783387885, - "velocityY": 1.1036137138901196e-18, - "timestamp": 6.368744360750536 - }, - { - "x": 0, - "y": 1, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.5254761443447583e-40, - "velocityY": -1.330214233321333e-39, - "timestamp": 6.4667250432236205 - }, - { - "x": 1.117551892465105e-37, - "y": 1.0082644636151268, - "heading": 5.777226208013994e-25, - "angularVelocity": 5.8962777231441345e-24, - "velocityX": 1.1405739034082113e-36, - "velocityY": 0.08434788783387888, - "timestamp": 6.564705725696705 - }, - { - "x": 2.6897755866899145e-37, - "y": 1.0247933907521452, - "heading": 1.878956892361082e-24, - "angularVelocity": 1.3280490783242518e-23, - "velocityX": 1.604618665494541e-36, - "velocityY": 0.16869577471619424, - "timestamp": 6.66268640816979 - }, - { - "x": 4.617201760898304e-37, - "y": 1.0495867812958832, - "heading": 3.957176037517467e-24, - "angularVelocity": 2.121045567762179e-23, - "velocityX": 1.9672540809484087e-36, - "velocityY": 0.2530436604230488, - "timestamp": 6.760667090642875 - }, - { - "x": 7.822490165552593e-37, - "y": 1.0826446351004555, - "heading": 6.68318309457519e-24, - "angularVelocity": 2.7821884471994474e-23, - "velocityX": 3.2713410852659234e-36, - "velocityY": 0.33739154464098636, - "timestamp": 6.85864777311596 - }, - { - "x": 1.1548715422163086e-36, - "y": 1.1239669519750892, - "heading": 1.0156145042962959e-23, - "angularVelocity": 3.544538859114664e-23, - "velocityX": 3.802953813650495e-36, - "velocityY": 0.42173942691187877, - "timestamp": 6.956628455589045 - }, - { - "x": 1.590303807530914e-36, - "y": 1.1735537316596398, - "heading": 1.604994022058709e-23, - "angularVelocity": 6.015264831157614e-23, - "velocityX": 4.4441607272626095e-36, - "velocityY": 0.5060873065277095, - "timestamp": 7.05460913806213 - }, - { - "x": 2.0969602398345516e-36, - "y": 1.2314049737783423, - "heading": 2.689554282848633e-23, - "angularVelocity": 1.106911690454712e-22, - "velocityX": 5.170849268642284e-36, - "velocityY": 0.5904351823084514, - "timestamp": 7.152589820535215 - }, - { - "x": 2.6970018922938464e-36, - "y": 1.2975206777407098, - "heading": 4.114017114855317e-23, - "angularVelocity": 1.4538184971887797e-22, - "velocityX": 6.124058670873409e-36, - "velocityY": 0.6747830520626257, - "timestamp": 7.2505705030083 - }, - { - "x": 3.420493410463521e-36, - "y": 1.3719008424838655, - "heading": 5.947449866584252e-23, - "angularVelocity": 1.8712166065083759e-22, - "velocityX": 7.384122751685e-36, - "velocityY": 0.7591309109689798, - "timestamp": 7.3485511854813845 - }, - { - "x": 1.9831416217837412e-36, - "y": 1.454545465527764, - "heading": 8.166369448762052e-23, - "angularVelocity": 2.2646486737171235e-22, - "velocityX": -1.466983946444783e-35, - "velocityY": 0.8434787445637635, - "timestamp": 7.446531867954469 - }, - { - "x": 1.0861613719346016e-36, - "y": 1.545454534472236, - "heading": 9.281000307136208e-23, - "angularVelocity": 1.1376045389293476e-22, - "velocityX": -9.154736228879551e-36, - "velocityY": 0.9278264516012499, - "timestamp": 7.544512550427554 - }, - { - "x": 6.8567251290811176e-37, - "y": 1.6280991575161345, - "heading": 7.9681702937135e-23, - "angularVelocity": -1.3398862607068113e-22, - "velocityX": -4.087402783562853e-36, - "velocityY": 0.8434787445637635, - "timestamp": 7.642493232900639 - }, - { - "x": 4.416311613052699e-37, - "y": 1.7024793222592902, - "heading": 6.631726452847393e-23, - "angularVelocity": -1.3639872730018692e-22, - "velocityX": -2.490693450174919e-36, - "velocityY": 0.7591309109689798, - "timestamp": 7.740473915373724 - }, - { - "x": 2.8341582058058063e-37, - "y": 1.7685950262216577, - "heading": 5.520332634625129e-23, - "angularVelocity": -1.1342981734081153e-22, - "velocityX": -1.6148009261310582e-36, - "velocityY": 0.6747830520626257, - "timestamp": 7.838454597846809 - }, - { - "x": 1.7315008828738255e-37, - "y": 1.8264462683403602, - "heading": 4.288088447351943e-23, - "angularVelocity": -1.2576387583905343e-22, - "velocityX": -1.125317731608965e-36, - "velocityY": 0.5904351823084514, - "timestamp": 7.936435280319894 - }, - { - "x": 9.563231995227316e-38, - "y": 1.8760330480249108, - "heading": 3.053579758781607e-23, - "angularVelocity": -1.25995006403652e-22, - "velocityX": -7.911627994010943e-37, - "velocityY": 0.5060873065277095, - "timestamp": 8.034415962792979 - }, - { - "x": 4.825237858288704e-38, - "y": 1.9173553648995445, - "heading": 2.0171064793054303e-23, - "angularVelocity": -1.0578336973328714e-22, - "velocityX": -4.835712844569225e-37, - "velocityY": 0.4217394269118787, - "timestamp": 8.132396645266065 - }, - { - "x": 1.9830009986802493e-38, - "y": 1.9504132187041168, - "heading": 1.190680907263644e-23, - "angularVelocity": -8.434574430528315e-23, - "velocityX": -2.900681543335251e-37, - "velocityY": 0.33739154464098636, - "timestamp": 8.23037732773915 - }, - { - "x": 7.503247022033376e-39, - "y": 1.9752066092478548, - "heading": 5.867706563960283e-24, - "angularVelocity": -6.163563013249786e-23, - "velocityX": -1.2580315029942821e-37, - "velocityY": 0.2530436604230488, - "timestamp": 8.328358010212236 - }, - { - "x": 2.19006134392397e-39, - "y": 1.9917355363848732, - "heading": 1.9081673701003045e-24, - "angularVelocity": -4.041141844838844e-23, - "velocityX": -5.422599062203887e-38, - "velocityY": 0.16869577471619424, - "timestamp": 8.426338692685322 - }, - { - "x": 1.871946327660505e-43, - "y": 2, - "heading": 3.8918994248492545e-43, - "angularVelocity": -1.9474930664723247e-23, - "velocityX": -2.235046387745111e-38, - "velocityY": 0.08434788783387888, - "timestamp": 8.524319375158408 - }, - { - "x": 0, - "y": 2, - "heading": -2.5991331833497144e-43, - "angularVelocity": -6.625057575927064e-42, - "velocityX": -1.9101859645151015e-42, - "velocityY": 4.591774807899561e-41, - "timestamp": 8.622300057631493 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "Tag14": { - "waypoints": [ - { - "x": 6.224338054656982, - "y": 4.105148, - "heading": 3.141592653589793, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 5.77, - "y": 4.105148, - "heading": 3.141592653589793, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 6 - }, - { - "x": 5.826, - "y": 4.105148, - "heading": 3.141592653589793, + "x": 5.826, + "y": 4.105148, + "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -7714,548 +2353,74 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "AmpLanePSprint": { + "SourceLanePSubHSubSprint": { "waypoints": [ { - "x": 1.4685733318328857, - "y": 7.0094075202941895, - "heading": 0, + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 24 }, { - "x": 0.6499583125114441, - "y": 6.67806339263916, - "heading": 1.0265157581506656, + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 32 + "headingConstrained": false, + "controlIntervalCount": 21 }, { - "x": 7.354804515838623, - "y": 6.366209983825684, + "x": 7.340087890625, + "y": 0.7580231428146362, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.4685733318328855, - "y": 7.0094075202941895, - "heading": 6.049320357152304e-20, - "angularVelocity": 5.735208962067612e-20, - "velocityX": 1.2024934428735394e-18, - "velocityY": 5.740194914400006e-19, - "timestamp": 0 - }, - { - "x": 1.4539549354530197, - "y": 7.003550802171312, - "heading": 0.018695171024518124, - "angularVelocity": 0.28848806436406915, - "velocityX": -0.22557872673127724, - "velocityY": -0.09037591967352629, - "timestamp": 0.06480396707478588 - }, - { - "x": 1.4247253207548556, - "y": 6.991839516204374, - "heading": 0.05623662341413578, - "angularVelocity": 0.5793079356745707, - "velocityX": -0.45104668768861206, - "velocityY": -0.1807186580633629, - "timestamp": 0.12960793414957175 - }, - { - "x": 1.3808979484465476, - "y": 6.974274102040644, - "heading": 0.11287119216832771, - "angularVelocity": 0.8739367558907741, - "velocityX": -0.6763069343845826, - "velocityY": -0.2710546121884731, - "timestamp": 0.19441190122435764 - }, - { - "x": 1.3224921483061116, - "y": 6.950848437335187, - "heading": 0.18890117826912542, - "angularVelocity": 1.1732304291349192, - "velocityX": -0.9012689002979366, - "velocityY": -0.36148504116147834, - "timestamp": 0.2592158682991435 - }, - { - "x": 1.2495302432323667, - "y": 6.921544701913893, - "heading": 0.28459715171144206, - "angularVelocity": 1.4766993096561234, - "velocityX": -1.1258863981204084, - "velocityY": -0.45219045598668955, - "timestamp": 0.32401983537392937 - }, - { - "x": 1.1620307559069942, - "y": 6.886330686053142, - "heading": 0.40005759835574417, - "angularVelocity": 1.7816879406634767, - "velocityX": -1.3502180695881552, - "velocityY": -0.5433929040194694, - "timestamp": 0.38882380244871523 - }, - { - "x": 1.0599976129357116, - "y": 6.845163950858531, - "heading": 0.5350484398222678, - "angularVelocity": 2.083064472129233, - "velocityX": -1.574489148998107, - "velocityY": -0.6352502331701971, - "timestamp": 0.4536277695235011 - }, - { - "x": 0.9575825133734215, - "y": 6.803331405068127, - "heading": 0.6590788452444744, - "angularVelocity": 1.9139322949021502, - "velocityX": -1.5803831800003518, - "velocityY": -0.6455244590524392, - "timestamp": 0.518431736598287 - }, - { - "x": 0.8697427157925169, - "y": 6.767497218853931, - "heading": 0.7646238641532083, - "angularVelocity": 1.6286814476486076, - "velocityX": -1.3554694495714084, - "velocityY": -0.5529628482904638, - "timestamp": 0.5832357036730729 - }, - { - "x": 0.7965061565345849, - "y": 6.737660077605358, - "heading": 0.8521458046235697, - "angularVelocity": 1.3505645475277608, - "velocityX": -1.130124629151394, - "velocityY": -0.4604215234869663, - "timestamp": 0.6480396707478587 - }, - { - "x": 0.7378960834776489, - "y": 6.713809764371052, - "heading": 0.9219596832810217, - "angularVelocity": 1.0773087174876965, - "velocityX": -0.9044210671438955, - "velocityY": -0.3680378580339382, - "timestamp": 0.7128436378226446 - }, - { - "x": 0.6939290380184983, - "y": 6.695933388139099, - "heading": 0.9742459780095203, - "angularVelocity": 0.80683786948041, - "velocityX": -0.6784622522940782, - "velocityY": -0.27585311577181243, - "timestamp": 0.7776476048974305 - }, - { - "x": 0.6646150045031116, - "y": 6.6840200000677585, - "heading": 1.0090892197053958, - "angularVelocity": 0.5376714307577112, - "velocityX": -0.4523493674632192, - "velocityY": -0.18383732677339054, - "timestamp": 0.8424515719722163 - }, - { - "x": 0.6499583125114444, - "y": 6.67806339263916, - "heading": 1.0265157581506656, - "angularVelocity": 0.26891159958091726, - "velocityX": -0.22616967221702353, - "velocityY": -0.09191732693964001, - "timestamp": 0.9072555390470022 - }, - { - "x": 0.6499583125114444, - "y": 6.67806339263916, - "heading": 1.0265157581506656, - "angularVelocity": 9.703439676406597e-18, - "velocityX": 1.8974989673442822e-18, - "velocityY": 3.469574557132838e-18, - "timestamp": 0.972059506121788 - }, - { - "x": 0.677098524254357, - "y": 6.676790336244545, - "heading": 1.0174178053772396, - "angularVelocity": -0.10888280660674853, - "velocityX": 0.32480960278249144, - "velocityY": -0.015235730132528697, - "timestamp": 1.055616800950558 - }, - { - "x": 0.7313843379611403, - "y": 6.674244414674947, - "heading": 0.999511909828794, - "angularVelocity": -0.21429482111807416, - "velocityX": 0.649683714845345, - "velocityY": -0.03046917177986014, - "timestamp": 1.139174095779328 - }, - { - "x": 0.8128220453410844, - "y": 6.670425841513566, - "heading": 0.9731501881376037, - "angularVelocity": -0.31549276152616484, - "velocityX": 0.9746331250530577, - "velocityY": -0.04570005729847779, - "timestamp": 1.2227313906080979 - }, - { - "x": 0.9214189734172925, - "y": 6.665334889481153, - "heading": 0.938761218692723, - "angularVelocity": -0.41156154606662587, - "velocityX": 1.299670223871541, - "velocityY": -0.06092767893988849, - "timestamp": 1.3062886854368678 - }, - { - "x": 1.057183650080325, - "y": 6.658971921820971, - "heading": 0.8968713117735069, - "angularVelocity": -0.5013315355056764, - "velocityX": 1.6248093830855639, - "velocityY": -0.07615095334551644, - "timestamp": 1.3898459802656378 - }, - { - "x": 1.2201259982302939, - "y": 6.651337410962851, - "heading": 0.8481374363541635, - "angularVelocity": -0.5832390280130826, - "velocityX": 1.9500672979406355, - "velocityY": -0.09136857378837147, - "timestamp": 1.4734032750944077 - }, - { - "x": 1.410257523594535, - "y": 6.642431937989669, - "heading": 0.7934008809085091, - "angularVelocity": -0.65507811804839, - "velocityX": 2.275462911453366, - "velocityY": -0.1065792399267018, - "timestamp": 1.5569605699231777 - }, - { - "x": 1.6275913452117106, - "y": 6.632256164958325, - "heading": 0.7337801464779199, - "angularVelocity": -0.7135311710696056, - "velocityX": 2.601015531469118, - "velocityY": -0.12178198267664266, - "timestamp": 1.6405178647519476 - }, - { - "x": 1.8721414897299389, - "y": 6.620810753247128, - "heading": 0.6708446221029851, - "angularVelocity": -0.7532020334537907, - "velocityX": 2.9267360201089847, - "velocityY": -0.13697681015944838, - "timestamp": 1.7240751595807176 - }, - { - "x": 2.1439190987685945, - "y": 6.6080960989852695, - "heading": 0.6069767178684699, - "angularVelocity": -0.76436060269055, - "velocityX": 3.2525898498221513, - "velocityY": -0.15216689683305046, - "timestamp": 1.8076324544094875 - }, - { - "x": 2.442913985874264, - "y": 6.5941112960890615, - "heading": 0.5462664171235281, - "angularVelocity": -0.7265709220164723, - "velocityX": 3.578321769731578, - "velocityY": -0.16736782736764083, - "timestamp": 1.8911897492382574 - }, - { - "x": 2.7689778327808057, - "y": 6.578852444576511, - "heading": 0.4974709825138351, - "angularVelocity": -0.5839757583068491, - "velocityX": 3.9022786409579977, - "velocityY": -0.1826154322470609, - "timestamp": 1.9747470440670274 - }, - { - "x": 3.120091446148044, - "y": 6.56305392023088, - "heading": 0.49252318839031584, - "angularVelocity": -0.059214388566174905, - "velocityX": 4.202070137463798, - "velocityY": -0.18907414819982873, - "timestamp": 2.0583043388957973 - }, - { - "x": 3.4728474074143234, - "y": 6.5466252324603085, - "heading": 0.4925231817165128, - "angularVelocity": -7.987099095905687e-8, - "velocityX": 4.221725487752611, - "velocityY": -0.19661584071429156, - "timestamp": 2.141861633724567 - }, - { - "x": 3.825603367792492, - "y": 6.5301965258536905, - "heading": 0.49252317502370946, - "angularVelocity": -8.009838379406554e-8, - "velocityX": 4.221725477123844, - "velocityY": -0.1966160661410119, - "timestamp": 2.225418928553337 - }, - { - "x": 4.1783593283685585, - "y": 6.513767823497187, - "heading": 0.49252316833088156, - "angularVelocity": -8.009867777354286e-8, - "velocityX": 4.221725479492243, - "velocityY": -0.19661601527633685, - "timestamp": 2.3089762233821065 - }, - { - "x": 4.5311152888161, - "y": 6.497339118379854, - "heading": 0.49252316163811777, - "angularVelocity": -8.009791003841919e-8, - "velocityX": 4.2217254779540765, - "velocityY": -0.19661604831749765, - "timestamp": 2.3925335182108762 - }, - { - "x": 4.883871249955156, - "y": 6.480910427791649, - "heading": 0.49252315497752247, - "angularVelocity": -7.971292311350099e-8, - "velocityX": 4.221725486230021, - "velocityY": -0.19661587443527756, - "timestamp": 2.476090813039646 - }, - { - "x": 5.235292142896319, - "y": 6.465020831099543, - "heading": 0.4884937210472112, - "angularVelocity": -0.04822360439721594, - "velocityX": 4.205747609006577, - "velocityY": -0.19016408710534566, - "timestamp": 2.5596481078684157 - }, - { - "x": 5.56143349165223, - "y": 6.449807932309173, - "heading": 0.4427260149092047, - "angularVelocity": -0.5477404005457096, - "velocityX": 3.903206170379902, - "velocityY": -0.1820654776048722, - "timestamp": 2.6432054026971854 - }, - { - "x": 5.860479824683085, - "y": 6.4358664343174405, - "heading": 0.38609345994808875, - "angularVelocity": -0.6777691292804887, - "velocityX": 3.578937466126377, - "velocityY": -0.16684956137344756, - "timestamp": 2.726762697525955 - }, - { - "x": 6.132305844443899, - "y": 6.42319455429606, - "heading": 0.3267902425497731, - "angularVelocity": -0.7097311793045699, - "velocityX": 3.253169221404942, - "velocityY": -0.1516549817385581, - "timestamp": 2.810319992354725 - }, - { - "x": 6.376904781169397, - "y": 6.41179175824138, - "heading": 0.2685758916504145, - "angularVelocity": -0.6966998036336328, - "velocityX": 2.927319957243016, - "velocityY": -0.13646679297179057, - "timestamp": 2.8938772871834946 - }, - { - "x": 6.594287281116567, - "y": 6.401657882345162, - "heading": 0.21360285288348452, - "angularVelocity": -0.6579083116511133, - "velocityX": 2.6015981057385837, - "velocityY": -0.12128056463513756, - "timestamp": 2.9774345820122643 - }, - { - "x": 6.784465944209141, - "y": 6.392792582186275, - "heading": 0.16326745687666674, - "angularVelocity": -0.6024057637333325, - "velocityX": 2.2760270480548472, - "velocityY": -0.10609845827410212, - "timestamp": 3.060991876841034 - }, - { - "x": 6.947452313541236, - "y": 6.385195340051002, - "heading": 0.11855161426546505, - "angularVelocity": -0.5351518703762895, - "velocityX": 1.950594136228255, - "velocityY": -0.09092254782591588, - "timestamp": 3.1445491716698037 - }, - { - "x": 7.083256400212696, - "y": 6.3788655571350015, - "heading": 0.08018557174620769, - "angularVelocity": -0.45915850432779387, - "velocityX": 1.6252810356026588, - "velocityY": -0.07575380376988836, - "timestamp": 3.2281064664985735 - }, - { - "x": 7.191886785288102, - "y": 6.373802632634511, - "heading": 0.04873492465610617, - "angularVelocity": -0.37639618604873454, - "velocityX": 1.300070631750541, - "velocityY": -0.06059225003472367, - "timestamp": 3.311663761327343 - }, - { - "x": 7.273350833513733, - "y": 6.370006018118368, - "heading": 0.024651280459860023, - "angularVelocity": -0.2882291037018025, - "velocityX": 0.9749483679739933, - "velocityY": -0.04543725983378117, - "timestamp": 3.395221056156113 - }, - { - "x": 7.32765490169191, - "y": 6.367475252448548, - "heading": 0.008303807824017625, - "angularVelocity": -0.1956438713022077, - "velocityX": 0.6499021813650045, - "velocityY": -0.03028778845710075, - "timestamp": 3.4787783509848826 - }, - { - "x": 7.354804515838622, - "y": 6.366209983825684, - "heading": -4.8930235830534376e-17, - "angularVelocity": -0.09937861010260368, - "velocityX": 0.32492212920904096, - "velocityY": -0.015142527357507724, - "timestamp": 3.5623356458136524 - }, - { - "x": 7.354804515838623, - "y": 6.366209983825684, - "heading": -2.4216548868392683e-17, - "angularVelocity": 5.9907409116151e-18, - "velocityX": -8.630903523575738e-17, - "velocityY": 6.541583629386208e-18, - "timestamp": 3.645892940642422 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 0 - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" + "controlIntervalCount": 7 }, { - "scope": [ - 1 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "CenterLanePBc": { - "waypoints": [ - { - "x": 1.5270464420318604, - "y": 4.085783958435059, + "x": 7.841280937194824, + "y": 0.7580231428146362, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 27 + "controlIntervalCount": 23 }, { - "x": 1.3516290187835693, - "y": 5.567087173461914, + "x": 2.757753372192383, + "y": 1.617210865020752, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 23 + "headingConstrained": false, + "controlIntervalCount": 24 }, { - "x": 2.462606430053711, - "y": 5.547596454620361, - "heading": 0, + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 23 + "controlIntervalCount": 24 }, { - "x": 1.332138180732727, - "y": 5.567087173461914, + "x": 2.757753372192383, + "y": 1.617210865020752, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 30 + "headingConstrained": false, + "controlIntervalCount": 24 }, { - "x": 2.4820973873138428, - "y": 4.124765396118164, + "x": 8.235074996948242, + "y": 0.7759228944778442, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -8265,976 +2430,1354 @@ ], "trajectory": [ { - "x": 1.5270464420318604, - "y": 4.085783958435059, - "heading": 1.4625077505834915e-24, - "angularVelocity": -1.6968405368267337e-23, - "velocityX": 1.1721587912394621e-19, - "velocityY": -9.898260288411433e-19, + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": 0, + "velocityX": 2.1342037757525677e-32, + "velocityY": 1.1024409975331184e-32, "timestamp": 0 }, { - "x": 1.5260826099479785, - "y": 4.093922987832265, - "heading": -1.5564984556705778e-16, - "angularVelocity": -1.5951860339453124e-15, - "velocityX": -0.009878020828395265, - "velocityY": 0.08341442794136009, - "timestamp": 0.09757340064631857 - }, - { - "x": 1.524154945788772, - "y": 4.11020104655442, - "heading": -4.825478082550605e-16, - "angularVelocity": -3.3502446331081196e-15, - "velocityX": -0.019756041569095335, - "velocityY": 0.16682885514218276, - "timestamp": 0.19514680129263715 - }, - { - "x": 1.5212634495643529, - "y": 4.1346181345161295, - "heading": -9.964831586930951e-16, - "angularVelocity": -5.2671216122656385e-15, - "velocityX": -0.02963406220615563, - "velocityY": 0.2502432814678248, - "timestamp": 0.2927202019389557 - }, - { - "x": 1.5174081212868562, - "y": 4.16717425161492, - "heading": -1.717782516967208e-15, - "angularVelocity": -7.392318655053113e-15, - "velocityX": -0.0395120827188482, - "velocityY": 0.33365770674325024, - "timestamp": 0.3902936025852743 - }, - { - "x": 1.5125889609711136, - "y": 4.207869397725548, - "heading": -2.6688719095730424e-15, - "angularVelocity": -9.74735165634569e-15, - "velocityX": -0.049390103079535765, - "velocityY": 0.4170721307350775, - "timestamp": 0.4878670032315929 - }, - { - "x": 1.5068059686356647, - "y": 4.256703572691455, - "heading": -3.876697094067346e-15, - "angularVelocity": -1.2378541573688779e-14, - "velocityX": -0.05926812325021708, - "velocityY": 0.500486553122407, - "timestamp": 0.5854404038779114 - }, - { - "x": 1.5000591443043463, - "y": 4.313676776311357, - "heading": -5.376745558038763e-15, - "angularVelocity": -1.5373429438679563e-14, - "velocityX": -0.06914614317660467, - "velocityY": 0.5839009734468111, - "timestamp": 0.68301380452423 - }, - { - "x": 1.49234848800894, - "y": 4.378789008316868, - "heading": -7.221997958230257e-15, - "angularVelocity": -1.8911296572887638e-14, - "velocityX": -0.07902416277726727, - "velocityY": 0.667315391020648, - "timestamp": 0.7805872051705486 - }, - { - "x": 1.4836739997939412, - "y": 4.452040268332257, - "heading": -9.479155947701628e-15, - "angularVelocity": -2.3132763955751144e-14, - "velocityX": -0.08890218192191494, - "velocityY": 0.7507298047436908, - "timestamp": 0.8781606058168672 - }, - { - "x": 1.4740356797260918, - "y": 4.533430555793919, - "heading": -1.2250780387615259e-14, - "angularVelocity": -2.8405339345394318e-14, - "velocityX": -0.09878020038254028, - "velocityY": 0.8341442126905433, - "timestamp": 0.9757340064631858 - }, - { - "x": 1.4634335279166297, - "y": 4.622959869762518, - "heading": -1.570747306998558e-14, - "angularVelocity": -3.542634883593584e-14, - "velocityX": -0.1086582177031285, - "velocityY": 0.9175586110104128, - "timestamp": 1.0733074071095043 - }, - { - "x": 1.451867544588029, - "y": 4.720628208359378, - "heading": -2.0193306127373128e-14, - "angularVelocity": -4.5973625402297885e-14, - "velocityX": -0.11853623274364325, - "velocityY": 1.000972990076323, - "timestamp": 1.1708808077558228 - }, - { - "x": 1.439337730407712, - "y": 4.826435565948485, - "heading": -2.6736262460658644e-14, - "angularVelocity": -6.705631534728917e-14, - "velocityX": -0.12841424094394735, - "velocityY": 1.0843873113804372, - "timestamp": 1.2684542084021413 - }, - { - "x": 1.4268079162273966, - "y": 4.9322429235375935, - "heading": -2.0195304822343385e-14, - "angularVelocity": 6.703581864794123e-14, - "velocityX": -0.12841424094393503, - "velocityY": 1.0843873113804385, - "timestamp": 1.3660276090484598 - }, - { - "x": 1.4152419328987969, - "y": 5.029911262134455, - "heading": -1.571219926412934e-14, - "angularVelocity": 4.594566272297136e-14, - "velocityX": -0.1185362327436348, - "velocityY": 1.0009729900763238, - "timestamp": 1.4636010096947782 - }, - { - "x": 1.4046397810893352, - "y": 5.119440576103053, - "heading": -1.2258131187906243e-14, - "angularVelocity": 3.539944307707758e-14, - "velocityX": -0.10865821770312199, - "velocityY": 0.9175586110104136, - "timestamp": 1.5611744103410967 - }, - { - "x": 1.3950014610214865, - "y": 5.200830863564716, - "heading": -9.488622066072567e-15, - "angularVelocity": 2.8383655202850955e-14, - "velocityX": -0.09878020038253506, - "velocityY": 0.8341442126905438, - "timestamp": 1.6587478109874152 - }, - { - "x": 1.386326972806488, - "y": 5.274082123580104, - "heading": -7.232328083000986e-15, - "angularVelocity": 2.3123906034472235e-14, - "velocityX": -0.0889021819219107, - "velocityY": 0.7507298047436913, - "timestamp": 1.7563212116337337 - }, - { - "x": 1.378616316511082, - "y": 5.339194355585617, - "heading": -5.387907876762194e-15, - "angularVelocity": 1.8902766491774747e-14, - "velocityX": -0.0790241627772638, - "velocityY": 0.6673153910206483, - "timestamp": 1.8538946122800521 - }, - { - "x": 1.3718694921797638, - "y": 5.396167559205518, - "heading": -3.885859488553591e-15, - "angularVelocity": 1.53939266022293e-14, - "velocityX": -0.06914614317660184, - "velocityY": 0.5839009734468115, - "timestamp": 1.9514680129263706 - }, - { - "x": 1.3660864998443152, - "y": 5.4450017341714245, - "heading": -2.6761448049829828e-15, - "angularVelocity": 1.2397908698261985e-14, - "velocityX": -0.05926812325021481, - "velocityY": 0.5004865531224073, - "timestamp": 2.0490414135726893 - }, - { - "x": 1.361267339528573, - "y": 5.4856968802820525, - "heading": -1.7234986300483674e-15, - "angularVelocity": 9.76331049910467e-15, - "velocityX": -0.04939010307953397, - "velocityY": 0.4170721307350777, - "timestamp": 2.146614814219008 - }, - { - "x": 1.3574120112510764, - "y": 5.518252997380844, - "heading": -1.0006590963793495e-15, - "angularVelocity": 7.408108892000521e-15, - "velocityX": -0.03951208271884683, - "velocityY": 0.3336577067432504, - "timestamp": 2.2441882148653263 - }, - { - "x": 1.3545205150266573, - "y": 5.542670085342553, - "heading": -4.849115866184459e-16, - "angularVelocity": 5.285700891601152e-15, - "velocityX": -0.029634062206154655, - "velocityY": 0.25024328146782493, - "timestamp": 2.3417616155116447 - }, - { - "x": 1.3525928508674507, - "y": 5.558948144064708, - "heading": -1.5693409310506777e-16, - "angularVelocity": 3.361316960958839e-15, - "velocityX": -0.019756041569094714, - "velocityY": 0.1668288551421828, - "timestamp": 2.439335016157963 - }, - { - "x": 1.3516290187835691, - "y": 5.567087173461914, - "heading": -6.1887132115889425e-24, - "angularVelocity": 1.608357872874533e-15, - "velocityX": -0.009878020828394967, - "velocityY": 0.08341442794136011, - "timestamp": 2.5369084168042817 - }, - { - "x": 1.3516290187835691, - "y": 5.567087173461914, - "heading": -1.4648141818546668e-24, - "angularVelocity": 3.340172165504719e-23, - "velocityX": -1.5435615328511767e-18, - "velocityY": -2.1040451608667207e-18, - "timestamp": 2.6344818174506 - }, - { - "x": 1.3600455150653603, - "y": 5.566939516487978, - "heading": 6.686728970627398e-20, - "angularVelocity": 6.762246861371845e-19, - "velocityX": 0.08511363372225911, - "velocityY": -0.0014932129921227434, - "timestamp": 2.733367224097152 - }, - { - "x": 1.376878507539919, - "y": 5.566644202541668, - "heading": 1.580133567010756e-19, - "angularVelocity": 9.217342317784428e-19, - "velocityX": 0.17022726654424447, - "velocityY": -0.0029864259684513, - "timestamp": 2.8322526307437035 - }, - { - "x": 1.4021279960984379, - "y": 5.566201231624893, - "heading": 2.8447751001050527e-19, - "angularVelocity": 1.2788959837210388e-18, - "velocityX": 0.2553408982658953, - "velocityY": -0.004479638925475853, - "timestamp": 2.931138037390255 - }, - { - "x": 1.435793980604908, - "y": 5.565610603740038, - "heading": 3.7920321307665064e-19, - "angularVelocity": 9.579340703122642e-19, - "velocityX": 0.3404545286121278, - "velocityY": -0.005972851858370403, - "timestamp": 3.030023444036807 - }, - { - "x": 1.4778764608844615, - "y": 5.564872318890172, - "heading": 4.3118128283917494e-19, - "angularVelocity": 5.256394158249627e-19, - "velocityX": 0.42556815718996566, - "velocityY": -0.007466064760240663, - "timestamp": 3.1289088506833584 - }, - { - "x": 1.52837543670394, - "y": 5.563986377079386, - "heading": 3.7231429179967247e-19, - "angularVelocity": -5.953051342426901e-19, - "velocityX": 0.5106817834099439, - "velocityY": -0.008959277620745203, - "timestamp": 3.22779425732991 - }, - { - "x": 1.5872909077369224, - "y": 5.562952778313405, - "heading": 3.665507864267625e-19, - "angularVelocity": -5.828470212315425e-20, - "velocityX": 0.5957954063289188, - "velocityY": -0.01045249042333774, - "timestamp": 3.3266796639764618 - }, - { - "x": 1.6546228734937773, - "y": 5.56177152260082, - "heading": 3.3289358715677737e-19, - "angularVelocity": -3.403656887203302e-19, - "velocityX": 0.6809090242963888, - "velocityY": -0.011945703139062274, - "timestamp": 3.4255650706230134 - }, - { - "x": 1.7303713331584527, - "y": 5.560442609955947, - "heading": 2.1844998249066e-19, - "angularVelocity": -1.1573356581936454e-18, - "velocityX": 0.766022634011352, - "velocityY": -0.013438915710006823, - "timestamp": 3.524450477269565 - }, - { - "x": 1.8145362850988431, - "y": 5.55896604040742, - "heading": 5.382114736148874e-20, - "angularVelocity": -1.6648446043389792e-18, - "velocityX": 0.851136227221305, - "velocityY": -0.01493212799139147, - "timestamp": 3.6233358839161167 - }, - { - "x": 1.90711772441864, - "y": 5.557341814041138, - "heading": -1.1309409770731682e-19, - "angularVelocity": -1.6879664104157646e-18, - "velocityX": 0.9362497709162765, - "velocityY": -0.016425339404097267, - "timestamp": 3.7222212905626684 - }, - { - "x": 1.9996991637384367, - "y": 5.555717587674857, - "heading": -1.8044549355651484e-19, - "angularVelocity": -6.811054697269865e-19, - "velocityX": 0.9362497709162764, - "velocityY": -0.016425339404097263, - "timestamp": 3.82110669720922 - }, - { - "x": 2.083864115678827, - "y": 5.554241018126329, - "heading": -2.080650691294579e-19, - "angularVelocity": -2.793088809488762e-19, - "velocityX": 0.8511362272213049, - "velocityY": -0.014932127991391469, - "timestamp": 3.9199921038557717 - }, - { - "x": 2.1596125753435023, - "y": 5.552912105481456, - "heading": -1.7750016719575358e-19, - "angularVelocity": 3.090941806706561e-19, - "velocityX": 0.7660226340113518, - "velocityY": -0.013438915710006822, - "timestamp": 4.018877510502323 - }, - { - "x": 2.226944541100357, - "y": 5.551730849768871, - "heading": -2.0769787641047505e-19, - "angularVelocity": -3.0538082503662157e-19, - "velocityX": 0.6809090242963888, - "velocityY": -0.011945703139062272, - "timestamp": 4.117762917148874 - }, - { - "x": 2.2858600121333397, - "y": 5.550697251002891, - "heading": -2.0325843947083703e-19, - "angularVelocity": 4.4894778552600243e-20, - "velocityX": 0.5957954063289187, - "velocityY": -0.010452490423337736, - "timestamp": 4.216648323795425 - }, - { - "x": 2.3363589879528184, - "y": 5.549811309192104, - "heading": -2.3083936836877355e-19, - "angularVelocity": -2.7891805869836823e-19, - "velocityX": 0.5106817834099439, - "velocityY": -0.008959277620745202, - "timestamp": 4.3155337304419765 - }, - { - "x": 2.3784414682323716, - "y": 5.5490730243422375, - "heading": -2.907085889576077e-19, - "angularVelocity": -6.054403614475218e-19, - "velocityX": 0.42556815718996566, - "velocityY": -0.00746606476024066, - "timestamp": 4.414419137088528 - }, - { - "x": 2.4121074527388418, - "y": 5.548482396457383, - "heading": -2.2908841084806075e-19, - "angularVelocity": 6.231473438855849e-19, - "velocityX": 0.3404545286121278, - "velocityY": -0.0059728518583704, - "timestamp": 4.513304543735079 - }, - { - "x": 2.437356941297361, - "y": 5.548039425540607, - "heading": -1.3373199383404307e-19, - "angularVelocity": 9.643123213362854e-19, - "velocityX": 0.2553408982658953, - "velocityY": -0.004479638925475851, - "timestamp": 4.61218995038163 - }, - { - "x": 2.4541899337719197, - "y": 5.547744111594297, - "heading": -3.289437685355794e-20, - "angularVelocity": 1.019742123971898e-18, - "velocityX": 0.17022726654424447, - "velocityY": -0.002986425968451297, - "timestamp": 4.711075357028181 - }, - { - "x": 2.462606430053711, - "y": 5.547596454620361, - "heading": -2.886570593000217e-29, - "angularVelocity": 3.3265147049069624e-19, - "velocityX": 0.08511363372225912, - "velocityY": -0.001493212992122741, - "timestamp": 4.809960763674733 - }, - { - "x": 2.462606430053711, - "y": 5.547596454620361, - "heading": -7.821656462951895e-30, - "angularVelocity": 2.943074068153252e-29, - "velocityX": -2.2275173681467236e-19, - "velocityY": 4.4340986172999205e-21, - "timestamp": 4.908846170321284 - }, - { - "x": 2.454042275901417, - "y": 5.547744111594184, - "heading": 1.4460933754378984e-20, - "angularVelocity": 1.4497351179100133e-19, - "velocityX": -0.08585722436689831, - "velocityY": 0.0014802883863889518, - "timestamp": 5.008594959260656 - }, - { - "x": 2.436913967686631, - "y": 5.548039425540282, - "heading": 4.286650670020682e-20, - "angularVelocity": 2.8477109182723563e-19, - "velocityX": -0.17171444783351814, - "velocityY": 0.0029605767572559506, - "timestamp": 5.108343748200029 - }, - { - "x": 2.41122150551911, - "y": 5.548482396456763, - "heading": 5.855738018623403e-21, - "angularVelocity": -3.710397725109959e-19, - "velocityX": -0.2575716701997976, - "velocityY": 0.004440865109151674, - "timestamp": 5.208092537139401 - }, - { - "x": 2.3769648895360507, - "y": 5.549073024341261, - "heading": -6.291625399026022e-20, - "angularVelocity": -6.894518818577401e-19, - "velocityX": -0.3434288911906516, - "velocityY": 0.005921153437333303, - "timestamp": 5.307841326078774 - }, - { - "x": 2.33414411991385, - "y": 5.549811309190735, - "heading": -1.2017788841555738e-19, - "angularVelocity": -5.740584286417081e-19, - "velocityX": -0.42928611041310155, - "velocityY": 0.007401441735025384, - "timestamp": 5.407590115018146 - }, - { - "x": 2.282759196887703, - "y": 5.5506972510011305, - "heading": -1.0823396901286469e-19, - "angularVelocity": 1.1973998537101906e-19, - "velocityX": -0.5151433272776794, - "velocityY": 0.008881729992064734, - "timestamp": 5.5073389039575185 - }, - { - "x": 2.2228101207868822, - "y": 5.551730849766769, - "heading": -8.802649819919278e-20, - "angularVelocity": 2.0258361490738492e-19, - "velocityX": -0.6010005408412367, - "velocityY": 0.010362018192190264, - "timestamp": 5.607087692896891 - }, - { - "x": 2.1542968921052967, - "y": 5.552912105479136, - "heading": -6.239601383466543e-20, - "angularVelocity": 2.569503383512215e-19, - "velocityX": -0.6868577494532631, - "velocityY": 0.01184230630694507, - "timestamp": 5.706836481836263 - }, - { - "x": 2.0772195116661285, - "y": 5.554241018124038, - "heading": -3.607669582656793e-20, - "angularVelocity": 2.6385601752371007e-19, - "velocityX": -0.7727149498127389, - "velocityY": 0.013322594279415347, - "timestamp": 5.806585270775636 - }, - { - "x": 1.991577981115741, - "y": 5.55571758767309, - "heading": -6.282144315835921e-20, - "angularVelocity": -2.6812099635964853e-19, - "velocityX": -0.8585721336671177, - "velocityY": 0.014802881967316633, - "timestamp": 5.906334059715008 - }, - { - "x": 1.8973723053932194, - "y": 5.557341814041137, - "heading": -1.267501677946567e-19, - "angularVelocity": -6.408972052260157e-19, - "velocityX": -0.9444292680062534, - "velocityY": 0.016283168801511785, - "timestamp": 6.006082848654381 - }, - { - "x": 1.8031666296706979, - "y": 5.558966040409183, - "heading": -3.795289314001396e-21, - "angularVelocity": 1.2326452916170473e-18, - "velocityX": -0.9444292680062534, - "velocityY": 0.016283168801511785, - "timestamp": 6.105831637593753 - }, - { - "x": 1.7175250991203104, - "y": 5.560442609958236, - "heading": 8.700208420675153e-20, - "angularVelocity": 9.1026038606046e-19, - "velocityX": -0.8585721336671177, - "velocityY": 0.014802881967316633, - "timestamp": 6.205580426533126 - }, - { - "x": 1.640447718681142, - "y": 5.561771522603139, - "heading": 1.2976083579479636e-19, - "angularVelocity": 4.2866435269702167e-19, - "velocityX": -0.7727149498127389, - "velocityY": 0.013322594279415347, - "timestamp": 6.305329215472498 - }, - { - "x": 1.5719344899995562, - "y": 5.5629527783155055, - "heading": 7.490403325878171e-20, - "angularVelocity": -5.49949547949794e-19, - "velocityX": -0.686857749453263, - "velocityY": 0.01184230630694507, - "timestamp": 6.4050780044118705 - }, - { - "x": 1.5119854138987352, - "y": 5.563986377081145, - "heading": 7.478581489863291e-20, - "angularVelocity": -1.1851672756803286e-21, - "velocityX": -0.6010005408412366, - "velocityY": 0.010362018192190266, - "timestamp": 6.504826793351243 - }, - { - "x": 1.460600490872588, - "y": 5.56487231889154, - "heading": 7.910534561996949e-20, - "angularVelocity": 4.3304083992516454e-20, - "velocityX": -0.5151433272776794, - "velocityY": 0.008881729992064737, - "timestamp": 6.604575582290615 - }, - { - "x": 1.4177797212503878, - "y": 5.565610603741014, - "heading": 5.432779100593734e-20, - "angularVelocity": -2.483995467741431e-19, - "velocityX": -0.4292861104131015, - "velocityY": 0.007401441735025386, - "timestamp": 6.704324371229988 - }, - { - "x": 1.383523105267329, - "y": 5.566201231625511, - "heading": -1.3462949300383398e-21, - "angularVelocity": -5.581429670757211e-19, - "velocityX": -0.34342889119065156, - "velocityY": 0.005921153437333306, - "timestamp": 6.80407316016936 - }, - { - "x": 1.3578306430998077, - "y": 5.566644202541992, - "heading": -3.946401324540515e-20, - "angularVelocity": -3.821371505085094e-19, - "velocityX": -0.25757167019979754, - "velocityY": 0.004440865109151676, - "timestamp": 6.903821949108733 - }, - { - "x": 1.3407023348850213, - "y": 5.566939516488091, - "heading": -2.4791971136884522e-20, - "angularVelocity": 1.470899198317256e-19, - "velocityX": -0.1717144478335181, - "velocityY": 0.0029605767572559527, - "timestamp": 7.003570738048105 - }, - { - "x": 1.332138180732727, - "y": 5.567087173461914, - "heading": 7.598893748852645e-29, - "angularVelocity": 2.4854406877542406e-19, - "velocityX": -0.0858572243668983, - "velocityY": 0.001480288386388954, - "timestamp": 7.103319526987478 - }, - { - "x": 1.332138180732727, - "y": 5.567087173461914, - "heading": 1.0804753349489245e-28, - "angularVelocity": 2.1800931515228082e-28, - "velocityX": 1.941111784767018e-19, - "velocityY": 2.1082261122464193e-18, - "timestamp": 7.20306831592685 - }, - { - "x": 1.3372491110466118, - "y": 5.560676853815777, - "heading": 3.558639537501783e-19, - "angularVelocity": 3.646584120755799e-18, - "velocityX": 0.05237236819901584, - "velocityY": -0.06568737982374474, - "timestamp": 7.300656614673684 - }, - { - "x": 1.3474709716341546, - "y": 5.547856214573958, - "heading": 1.0721703768123472e-18, - "angularVelocity": 7.340084933358087e-18, - "velocityX": 0.10474473598582199, - "velocityY": -0.1313747591304807, - "timestamp": 7.398244913420519 - }, - { - "x": 1.3628037624486922, - "y": 5.528625255794984, - "heading": -1.2934737138196825e-18, - "angularVelocity": -2.4241062607509123e-17, - "velocityX": 0.15711710329446488, - "velocityY": -0.19706213783748644, - "timestamp": 7.495833212167353 - }, - { - "x": 1.383247483435446, - "y": 5.50298397754756, - "heading": -3.2871298566762286e-18, - "angularVelocity": -2.0429253778468544e-17, - "velocityX": 0.20948947004178572, - "velocityY": -0.262749515840461, - "timestamp": 7.593421510914188 - }, - { - "x": 1.4088021345292037, - "y": 5.470932379913476, - "heading": -6.392169366146804e-18, - "angularVelocity": -3.181774355862696e-17, - "velocityX": 0.261861836120866, - "velocityY": -0.32843689300530343, - "timestamp": 7.691009809661022 - }, - { - "x": 1.439467715651024, - "y": 5.432470462991745, - "heading": -9.037790663968063e-18, - "angularVelocity": -2.7110025471414772e-17, - "velocityX": 0.31423420139102354, - "velocityY": -0.39412426915556464, - "timestamp": 7.788598108407856 - }, - { - "x": 1.4752442267033907, - "y": 5.387598226904674, - "heading": -1.128391773258776e-17, - "angularVelocity": -2.301635620338134e-17, - "velocityX": 0.36660656566192357, - "velocityY": -0.45981164405251984, - "timestamp": 7.886186407154691 - }, - { - "x": 1.516131667562784, - "y": 5.336315671807188, - "heading": -1.3116491479431834e-17, - "angularVelocity": -1.877862095234715e-17, - "velocityX": 0.41897892866709746, - "velocityY": -0.525499017361954, - "timestamp": 7.983774705901525 - }, - { - "x": 1.562130038067678, - "y": 5.278622797901877, - "heading": -1.4552519190706605e-17, - "angularVelocity": -1.4715162201094296e-17, - "velocityX": 0.47135129001709103, - "velocityY": -0.5911863885953993, - "timestamp": 8.08136300464836 - }, - { - "x": 1.613239337997809, - "y": 5.214519605465006, - "heading": -1.5531545587943304e-17, - "angularVelocity": -1.0032210364686455e-17, - "velocityX": 0.5237236491100207, - "velocityY": -0.6568737569979507, - "timestamp": 8.178951303395195 - }, - { - "x": 1.6694595670350196, - "y": 5.1440060948956186, - "heading": -1.6968450650800506e-17, - "angularVelocity": -1.4724152370496227e-17, - "velocityX": 0.5760960049427472, - "velocityY": -0.7225611213114336, - "timestamp": 8.27653960214203 - }, - { - "x": 1.7307907246793477, - "y": 5.067082266820785, - "heading": -1.785524132033205e-17, - "angularVelocity": -9.08705871531393e-18, - "velocityX": 0.6284683556522977, - "velocityY": -0.788248479199238, - "timestamp": 8.374127900888865 - }, - { - "x": 1.7972328100308623, - "y": 4.983748122369234, - "heading": -1.8156964270181303e-17, - "angularVelocity": -3.091793819261216e-18, - "velocityX": 0.680840697140134, - "velocityY": -0.8539358255208239, - "timestamp": 8.4717161996357 - }, - { - "x": 1.8687858209897246, - "y": 4.894003664174662, - "heading": -1.7567148894927193e-17, - "angularVelocity": 6.043915190863691e-18, - "velocityX": 0.7332130171106522, - "velocityY": -0.919623144854586, - "timestamp": 8.569304498382536 - }, - { - "x": 1.9454497470568448, - "y": 4.797848905405417, - "heading": -1.7590364829650386e-17, - "angularVelocity": -2.378972880130156e-19, - "velocityX": 0.7855852294956268, - "velocityY": -0.985310329250542, - "timestamp": 8.666892797129371 - }, - { - "x": 2.0170027580157073, - "y": 4.708104447210844, - "heading": -1.8525240985906295e-17, - "angularVelocity": -9.57979819187636e-18, - "velocityX": 0.7332130171106522, - "velocityY": -0.9196231448545861, - "timestamp": 8.764481095876206 - }, - { - "x": 2.083444843367222, - "y": 4.624770302759293, - "heading": -1.62920217217427e-17, - "angularVelocity": 2.2884087968719853e-17, - "velocityX": 0.6808406971401341, - "velocityY": -0.8539358255208239, - "timestamp": 8.862069394623042 - }, - { - "x": 2.14477600101155, - "y": 4.54784647468446, - "heading": -1.4137613991552986e-17, - "angularVelocity": 2.207649592959214e-17, - "velocityX": 0.6284683556522977, - "velocityY": -0.7882484791992381, - "timestamp": 8.959657693369877 - }, - { - "x": 2.200996230048761, - "y": 4.477332964115072, - "heading": -1.2021193262691405e-17, - "angularVelocity": 2.1687238185481475e-17, - "velocityX": 0.5760960049427472, - "velocityY": -0.7225611213114337, - "timestamp": 9.057245992116712 - }, - { - "x": 2.252105529978892, - "y": 4.413229771678201, - "heading": -9.999661721633516e-18, - "angularVelocity": 2.07148963451225e-17, - "velocityX": 0.5237236491100207, - "velocityY": -0.6568737569979508, - "timestamp": 9.154834290863548 - }, - { - "x": 2.2981039004837855, - "y": 4.3555368977728905, - "heading": -8.10923369851807e-18, - "angularVelocity": 1.9371461559461523e-17, - "velocityX": 0.471351290017091, - "velocityY": -0.5911863885953994, - "timestamp": 9.252422589610383 - }, - { - "x": 2.338991341343179, - "y": 4.304254342675404, - "heading": -6.3749798263727294e-18, - "angularVelocity": 1.7771124740362523e-17, - "velocityX": 0.41897892866709735, - "velocityY": -0.525499017361954, - "timestamp": 9.350010888357218 - }, - { - "x": 2.374767852395546, - "y": 4.259382106588334, - "heading": -4.81528393523032e-18, - "angularVelocity": 1.5982406426947388e-17, - "velocityX": 0.3666065656619235, - "velocityY": -0.4598116440525199, - "timestamp": 9.447599187104053 - }, - { - "x": 2.405433433517366, - "y": 4.220920189666603, - "heading": -3.4442054151676063e-18, - "angularVelocity": 1.4049619717228675e-17, - "velocityX": 0.3142342013910235, - "velocityY": -0.39412426915556475, - "timestamp": 9.545187485850889 - }, - { - "x": 2.430988084611124, - "y": 4.188868592032519, - "heading": -2.2728437711124333e-18, - "angularVelocity": 1.2003094938541682e-17, - "velocityX": 0.261861836120866, - "velocityY": -0.3284368930053035, - "timestamp": 9.642775784597724 - }, - { - "x": 2.4514318055978777, - "y": 4.163227313785095, - "heading": -1.3744434339694666e-18, - "angularVelocity": 9.206024983663883e-18, - "velocityX": 0.2094894700417857, - "velocityY": -0.2627495158404611, - "timestamp": 9.74036408334456 - }, - { - "x": 2.4667645964124154, - "y": 4.14399635500612, - "heading": -6.921668969846993e-19, - "angularVelocity": 6.991376321961435e-18, - "velocityX": 0.15711710329446488, - "velocityY": -0.19706213783748644, - "timestamp": 9.837952382091395 - }, - { - "x": 2.4769864569999585, - "y": 4.131175715764301, - "heading": -2.3224840986113696e-19, - "angularVelocity": 4.712844493605461e-18, - "velocityX": 0.10474473598582197, - "velocityY": -0.1313747591304807, - "timestamp": 9.93554068083823 - }, - { - "x": 2.4820973873138428, - "y": 4.124765396118164, - "heading": -2.507555799405664e-28, - "angularVelocity": 2.3798796279739872e-18, - "velocityX": 0.05237236819901583, - "velocityY": -0.06568737982374474, - "timestamp": 10.033128979585065 - }, - { - "x": 2.4820973873138428, - "y": 4.124765396118164, - "heading": -1.183597031867973e-28, - "angularVelocity": 1.4383049425452364e-28, - "velocityX": -1.8276514384427982e-18, - "velocityY": 2.2923091413933995e-18, - "timestamp": 10.1307172783319 - } - ], - "constraints": [ + "x": 0.6705320996855912, + "y": 4.398099342118358, + "heading": -1.0386114127976833, + "angularVelocity": 0.15927551181553457, + "velocityX": 0.12605002427557185, + "velocityY": -0.20514712032687507, + "timestamp": 0.05592000838348859 + }, + { + "x": 0.6846335258864488, + "y": 4.375148415401169, + "heading": -1.0210111049652704, + "angularVelocity": 0.3147407938803157, + "velocityX": 0.25217138924859706, + "velocityY": -0.41042423598715294, + "timestamp": 0.11184001676697718 + }, + { + "x": 0.7057920861320249, + "y": 4.340710150429397, + "heading": -0.9949634737692402, + "angularVelocity": 0.4658016325283909, + "velocityX": 0.3783719076090775, + "velocityY": -0.6158487090273922, + "timestamp": 0.16776002515046576 + }, + { + "x": 0.7340126708459819, + "y": 4.294775101740391, + "heading": -0.9607567098648513, + "angularVelocity": 0.6117088479280168, + "velocityX": 0.5046598798845962, + "velocityY": -0.8214420923185943, + "timestamp": 0.22368003353395435 + }, + { + "x": 0.7693006604451206, + "y": 4.237332365746024, + "heading": -0.9187315737842261, + "angularVelocity": 0.7515223494321577, + "velocityX": 0.6310440684690272, + "velocityY": -1.0272304610620715, + "timestamp": 0.2796000419174429 + }, + { + "x": 0.8116619611973516, + "y": 4.168369327121192, + "heading": -0.8692948358062461, + "angularVelocity": 0.8840617054087766, + "velocityX": 0.7575338769931041, + "velocityY": -1.2332444257142752, + "timestamp": 0.3355200503009315 + }, + { + "x": 0.8611030812908155, + "y": 4.087871426229515, + "heading": -0.8129373545696884, + "angularVelocity": 1.0078231900479868, + "velocityX": 0.8841400694078276, + "velocityY": -1.4395187557848361, + "timestamp": 0.3914400586844201 + }, + { + "x": 0.9176312901451578, + "y": 3.995821968070834, + "heading": -0.7502611156342618, + "angularVelocity": 1.120819555419328, + "velocityX": 1.0108762585778377, + "velocityY": -1.6460916373156267, + "timestamp": 0.4473600670679087 + }, + { + "x": 0.9812548770105831, + "y": 3.8922019971306105, + "heading": -0.6820246305048668, + "angularVelocity": 1.220251697057024, + "velocityX": 1.1377606818136927, + "velocityY": -1.8530034943775027, + "timestamp": 0.5032800754513973 + }, + { + "x": 1.0519833618359966, + "y": 3.7769903402374796, + "heading": -0.6092267013635415, + "angularVelocity": 1.3018225720227237, + "velocityX": 1.2648153473148889, + "velocityY": -2.0602939846330393, + "timestamp": 0.5592000838348858 + }, + { + "x": 1.1298269082953458, + "y": 3.6501643798944903, + "heading": -0.5332724156478241, + "angularVelocity": 1.3582667083101572, + "velocityX": 1.392051766614792, + "velocityY": -2.26798893650447, + "timestamp": 0.6151200922183744 + }, + { + "x": 1.2147919549002646, + "y": 3.511704401985981, + "heading": -0.45632702327380176, + "angularVelocity": 1.375990358340898, + "velocityX": 1.5194033238021896, + "velocityY": -2.4760364297333126, + "timestamp": 0.6710401006018629 + }, + { + "x": 1.3068601679390515, + "y": 3.3616165956403266, + "heading": -0.38215505844470493, + "angularVelocity": 1.3263940219829704, + "velocityX": 1.6464270249639619, + "velocityY": -2.683973244717373, + "timestamp": 0.7269601089853515 + }, + { + "x": 1.4058792704184795, + "y": 3.2000739494751453, + "heading": -0.3184323584737973, + "angularVelocity": 1.139533090444293, + "velocityX": 1.7707276043375095, + "velocityY": -2.8888165584195344, + "timestamp": 0.78288011736884 + }, + { + "x": 1.512169170978474, + "y": 3.0300749520186443, + "heading": -0.28662541962876376, + "angularVelocity": 0.5687935278354703, + "velocityX": 1.900749009747616, + "velocityY": -3.0400388406718526, + "timestamp": 0.8388001257523285 + }, + { + "x": 1.6244650109400058, + "y": 2.850864532661705, + "heading": -0.2866253125429214, + "angularVelocity": 0.0000019149825876044375, + "velocityX": 2.008151343458836, + "velocityY": -3.2047638141959607, + "timestamp": 0.8947201341358171 + }, + { + "x": 1.7352882055291936, + "y": 2.6707396888206167, + "heading": -0.2866252783553297, + "angularVelocity": 6.113659974508405e-7, + "velocityX": 1.9818164873864779, + "velocityY": -3.221116180917336, + "timestamp": 0.9506401425193056 + }, + { + "x": 1.8506362591615508, + "y": 2.493478394875359, + "heading": -0.2866252441530604, + "angularVelocity": 6.116284725625113e-7, + "velocityX": 2.0627331248114724, + "velocityY": -3.169908214777688, + "timestamp": 1.0065601509027942 + }, + { + "x": 1.9771720781105002, + "y": 2.324022128559085, + "heading": -0.2866252095630558, + "angularVelocity": 6.185622200419991e-7, + "velocityX": 2.2628004288052184, + "velocityY": -3.0303333496335663, + "timestamp": 1.0624801592862827 + }, + { + "x": 2.114370117744969, + "y": 2.1630766803808767, + "heading": -0.28662517392184755, + "angularVelocity": 6.373605669269748e-7, + "velocityX": 2.4534695827223696, + "velocityY": -2.8781370538157938, + "timestamp": 1.1184001676697712 + }, + { + "x": 2.2616596068294545, + "y": 2.011311910787294, + "heading": -0.28662513647276167, + "angularVelocity": 6.696902771759525e-7, + "velocityX": 2.6339318133574414, + "velocityY": -2.713961853381871, + "timestamp": 1.1743201760532598 + }, + { + "x": 2.418427669547749, + "y": 1.8693594092228303, + "heading": -0.2866250962912349, + "angularVelocity": 7.185536621043778e-7, + "velocityX": 2.8034341776776834, + "velocityY": -2.538492136678183, + "timestamp": 1.2302401844367483 + }, + { + "x": 2.5840219554137547, + "y": 1.7378099138847216, + "heading": -0.28662505217416917, + "angularVelocity": 7.889316725786441e-7, + "velocityX": 2.961270762521926, + "velocityY": -2.3524584337678855, + "timestamp": 1.2861601928202369 + }, { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.28662500163792387, + "angularVelocity": 9.037238500487202e-7, + "velocityX": 3.1067845266977017, + "velocityY": -2.1566350283233993, + "timestamp": 1.3420802012037254 + }, + { + "x": 2.964172022537395, + "y": 1.4952512181839386, + "heading": -0.2866249560195876, + "angularVelocity": 7.195928187708238e-7, + "velocityX": 3.256089340971223, + "velocityY": -1.923816018708688, + "timestamp": 1.4054748531095118 + }, + { + "x": 3.1789518158724, + "y": 1.3887033110695066, + "heading": -0.28662491569189824, + "angularVelocity": 6.361370890269852e-7, + "velocityX": 3.387979693526802, + "velocityY": -1.6807081340674828, + "timestamp": 1.4688695050152982 + }, + { + "x": 3.4009440542760783, + "y": 1.2981369637223683, + "heading": -0.2866248790058733, + "angularVelocity": 5.786927429417107e-7, + "velocityX": 3.5017502538477667, + "velocityY": -1.4286117933375972, + "timestamp": 1.5322641569210846 + }, + { + "x": 3.6289614625982565, + "y": 1.2240364953903844, + "heading": -0.286624844765475, + "angularVelocity": 5.401149349395126e-7, + "velocityX": 3.5967924969608642, + "velocityY": -1.1688757033023536, + "timestamp": 1.595658808826871 + }, + { + "x": 3.861784529858251, + "y": 1.1667980667810438, + "heading": -0.2866248120410044, + "angularVelocity": 5.162023864828723e-7, + "velocityX": 3.672597928386812, + "velocityY": -0.9028904945231823, + "timestamp": 1.6590534607326575 + }, + { + "x": 4.0981679647792335, + "y": 1.1267270015307351, + "heading": -0.2866247800525485, + "angularVelocity": 5.045923424053189e-7, + "velocityX": 3.728759884544863, + "velocityY": -0.6320890492444079, + "timestamp": 1.7224481126384439 + }, + { + "x": 4.3358126512762265, + "y": 1.0949807955483848, + "heading": -0.2866247482072254, + "angularVelocity": 5.023345364444787e-7, + "velocityX": 3.7486551207847545, + "velocityY": -0.5007710434238178, + "timestamp": 1.7858427645442303 + }, + { + "x": 4.573457419312962, + "y": 1.0632351999594603, + "heading": -0.28662471636190656, + "angularVelocity": 5.023344691113163e-7, + "velocityX": 3.7486564070090496, + "velocityY": -0.5007614149550478, + "timestamp": 1.8492374164500167 + }, + { + "x": 4.811102187354235, + "y": 1.0314896044045174, + "heading": -0.28662468451658774, + "angularVelocity": 5.023344695763415e-7, + "velocityX": 3.748656407080655, + "velocityY": -0.5007614144190159, + "timestamp": 1.9126320683558031 + }, + { + "x": 5.04874695539551, + "y": 0.9997440088495765, + "heading": -0.28662465267126885, + "angularVelocity": 5.023344699297441e-7, + "velocityX": 3.7486564070806594, + "velocityY": -0.5007614144189861, + "timestamp": 1.9760267202615895 + }, + { + "x": 5.286391723436784, + "y": 0.9679984132946355, + "heading": -0.28662462082595, + "angularVelocity": 5.023344691743294e-7, + "velocityX": 3.748656407080659, + "velocityY": -0.5007614144189861, + "timestamp": 2.039421372167376 + }, + { + "x": 5.524036491478059, + "y": 0.936252817739695, + "heading": -0.28662458898063115, + "angularVelocity": 5.023344697631327e-7, + "velocityX": 3.74865640708066, + "velocityY": -0.5007614144189809, + "timestamp": 2.1028160240731624 + }, + { + "x": 5.761681259520114, + "y": 0.9045072221906025, + "heading": -0.2866245571353122, + "angularVelocity": 5.023344707942377e-7, + "velocityX": 3.748656407092983, + "velocityY": -0.5007614143267316, + "timestamp": 2.166210675978949 + }, + { + "x": 5.99932604159473, + "y": 0.8727617316882137, + "heading": -0.2866245252899934, + "angularVelocity": 5.023344694954779e-7, + "velocityX": 3.748656628445413, + "velocityY": -0.500759757298884, + "timestamp": 2.229605327884735 + }, + { + "x": 6.237212534637086, + "y": 0.8428813906316718, + "heading": -0.2866244915581877, + "angularVelocity": 5.320922919551869e-7, + "velocityX": 3.752469425905036, + "velocityY": -0.47133851450037834, + "timestamp": 2.2929999797905216 + }, + { + "x": 6.4638245505635705, + "y": 0.8220107849523658, + "heading": -0.24424436180847547, + "angularVelocity": 0.6685126974542774, + "velocityX": 3.574623554416899, + "velocityY": -0.3292171350719418, + "timestamp": 2.356394631696308 + }, + { + "x": 6.673647733933235, + "y": 0.8041360764949451, + "heading": -0.18514255243378605, + "angularVelocity": 0.9322838377994912, + "velocityX": 3.309793130207447, + "velocityY": -0.2819592492436898, + "timestamp": 2.4197892836020944 + }, + { + "x": 6.866141134149738, + "y": 0.7888964331390943, + "heading": -0.1276691267098072, + "angularVelocity": 0.9065973863125333, + "velocityX": 3.0364296424022625, + "velocityY": -0.240393201913927, + "timestamp": 2.483183935507881 + }, + { + "x": 7.04134978279503, + "y": 0.7761674362974819, + "heading": -0.07636429138531178, + "angularVelocity": 0.8092927996630026, + "velocityX": 2.7637764918352534, + "velocityY": -0.2007897584252007, + "timestamp": 2.5465785874136673 + }, + { + "x": 7.19932046714385, + "y": 0.765888430525111, + "heading": -0.03338684229285246, + "angularVelocity": 0.6779349330023924, + "velocityX": 2.4918613731579056, + "velocityY": -0.16214310613530794, + "timestamp": 2.6099732393194537 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 7.340087890625, + "y": 0.7580231428146362, + "heading": 0, + "angularVelocity": 0.5266507708326894, + "velocityX": 2.220493673351971, + "velocityY": -0.12406863156475356, + "timestamp": 2.67336789122524 + }, + { + "x": 7.464023522460178, + "y": 0.7525372143128384, + "heading": 0.02301011936038792, + "angularVelocity": 0.3617896361662683, + "velocityX": 1.9486481772395665, + "velocityY": -0.08625561847872572, + "timestamp": 2.736968716290925 + }, + { + "x": 7.570582319535539, + "y": 0.7491648354718851, + "heading": 0.036983700221316605, + "angularVelocity": 0.21970754068830917, + "velocityX": 1.675431049287655, + "velocityY": -0.053024136675436706, + "timestamp": 2.8005695413566096 + }, + { + "x": 7.659708944521439, + "y": 0.7476928551810322, + "heading": 0.04295335113137197, + "angularVelocity": 0.09386121805637201, + "velocityX": 1.4013438488235506, + "velocityY": -0.02314404395434427, + "timestamp": 2.8641703664222944 + }, + { + "x": 7.731365798341117, + "y": 0.7479588271083956, + "heading": 0.041693436231508264, + "angularVelocity": -0.019809725716020715, + "velocityX": 1.1266654755763426, + "velocityY": 0.0041818942928592215, + "timestamp": 2.927771191487979 + }, + { + "x": 7.785525986892142, + "y": 0.7498349370538775, + "heading": 0.03380693253041853, + "angularVelocity": -0.12400002190136777, + "velocityX": 0.8515642445690119, + "velocityY": 0.029498201376229192, + "timestamp": 2.991372016553664 + }, + { + "x": 7.822169492500932, + "y": 0.7532180353995639, + "heading": 0.0197770843540876, + "angularVelocity": -0.220592235428419, + "velocityX": 0.5761482743493489, + "velocityY": 0.053192680160869574, + "timestamp": 3.0549728416193487 }, { - "scope": [ - 0 - ], - "type": "StopPoint" + "x": 7.841280937194824, + "y": 0.7580231428146362, + "heading": -3.3329541710174495e-31, + "angularVelocity": -0.3109564118651419, + "velocityX": 0.3004905152433859, + "velocityY": 0.07555102327854826, + "timestamp": 3.1185736666850334 + }, + { + "x": 7.837626456422163, + "y": 0.7661832239405314, + "heading": -0.033360531548023514, + "angularVelocity": -0.4178611188210231, + "velocityX": -0.04577461309858743, + "velocityY": 0.10221002096529454, + "timestamp": 3.1984100765480727 + }, + { + "x": 7.806322741144082, + "y": 0.7764707746744536, + "heading": -0.07501113915617397, + "angularVelocity": -0.521699406068016, + "velocityX": -0.3920982335225694, + "velocityY": 0.12885788265743237, + "timestamp": 3.278246486411112 + }, + { + "x": 7.74736425619444, + "y": 0.7888847549403223, + "heading": -0.12465356813987527, + "angularVelocity": -0.6218018704606526, + "velocityX": -0.7384911852973625, + "velocityY": 0.15549271675874526, + "timestamp": 3.3580828962741514 + }, + { + "x": 7.660744366778727, + "y": 0.8034239689194147, + "heading": -0.18191677738707465, + "angularVelocity": -0.7172568173523234, + "velocityX": -1.0849672419427567, + "velocityY": 0.182112572497119, + "timestamp": 3.4379193061371907 + }, + { + "x": 7.5464550145644145, + "y": 0.8200870575241466, + "heading": -0.24632578527266566, + "angularVelocity": -0.8067623280666746, + "velocityX": -1.4315442341455173, + "velocityY": 0.20871540482992249, + "timestamp": 3.51775571600023 + }, + { + "x": 7.404486266909035, + "y": 0.8388724767291539, + "heading": -0.3172478637527479, + "angularVelocity": -0.8883425319569104, + "velocityX": -1.7782456387872363, + "velocityY": 0.23529889729803757, + "timestamp": 3.5975921258632693 + }, + { + "x": 7.234825726560123, + "y": 0.8597784078771568, + "heading": -0.39379130752240776, + "angularVelocity": -0.958753579989021, + "velocityX": -2.125102326619727, + "velocityY": 0.26185960996827495, + "timestamp": 3.6774285357263086 + }, + { + "x": 7.037458048630302, + "y": 0.8828023770067689, + "heading": -0.47459284644074845, + "angularVelocity": -1.0120888333650886, + "velocityX": -2.4721512185781784, + "velocityY": 0.28838933475478157, + "timestamp": 3.757264945589348 + }, + { + "x": 6.812366694532574, + "y": 0.9079395818644891, + "heading": -0.5572950195818098, + "angularVelocity": -1.0358954427301785, + "velocityX": -2.8194072664825045, + "velocityY": 0.31485890837079844, + "timestamp": 3.837101355452387 + }, + { + "x": 6.559557151686244, + "y": 0.9351738572850867, + "heading": -0.6368741954392069, + "angularVelocity": -0.9967779863087077, + "velocityX": -3.1665945810943685, + "velocityY": 0.3411260033776371, + "timestamp": 3.9169377653154265 + }, + { + "x": 6.2794552371906684, + "y": 0.9644007497338526, + "heading": -0.6963848347023109, + "angularVelocity": -0.7454072567290455, + "velocityX": -3.5084482753682624, + "velocityY": 0.36608475379723326, + "timestamp": 3.996774175178466 + }, + { + "x": 5.978414020607044, + "y": 0.9876514129078965, + "heading": -0.6963848502753706, + "angularVelocity": -1.9506212382871808e-7, + "velocityX": -3.7707258768281893, + "velocityY": 0.2912288167006853, + "timestamp": 4.0766105850415055 + }, + { + "x": 5.677372687056552, + "y": 1.0109005615970947, + "heading": -0.6963848658464015, + "angularVelocity": -1.950367123738679e-7, + "velocityX": -3.770727341909936, + "velocityY": 0.29120984684910617, + "timestamp": 4.156446994904545 + }, + { + "x": 5.376331353502739, + "y": 1.0341497102432655, + "heading": -0.6963848814174324, + "angularVelocity": -1.950367127895845e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.29120984631016156, + "timestamp": 4.236283404767585 + }, + { + "x": 5.075290019948924, + "y": 1.0573988588894352, + "heading": -0.6963848969884634, + "angularVelocity": -1.950367129001825e-7, + "velocityX": -3.770727341951559, + "velocityY": 0.2912098463101467, + "timestamp": 4.316119814630625 + }, + { + "x": 4.774248686395111, + "y": 1.0806480075356062, + "heading": -0.6963849125594943, + "angularVelocity": -1.9503671289084429e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.2912098463101629, + "timestamp": 4.3959562244936645 + }, + { + "x": 4.473207352844907, + "y": 1.1038971562285251, + "heading": -0.6963849281305252, + "angularVelocity": -1.950367124712723e-7, + "velocityX": -3.770727341906337, + "velocityY": 0.2912098468957114, + "timestamp": 4.475792634356704 + }, + { + "x": 4.172166146375442, + "y": 1.1271479503662634, + "heading": -0.6963849437015552, + "angularVelocity": -1.9503670107200537e-7, + "velocityX": -3.770725750142127, + "velocityY": 0.29123045710128126, + "timestamp": 4.555629044219744 + }, + { + "x": 3.8735860999949274, + "y": 1.1720518831992321, + "heading": -0.6963849593703558, + "angularVelocity": -1.9626133737167217e-7, + "velocityX": -3.7398982105123864, + "velocityY": 0.5624492998871315, + "timestamp": 4.635465454082784 + }, + { + "x": 3.580403442801602, + "y": 1.244234542261993, + "heading": -0.6963849756495777, + "angularVelocity": -2.0390723857242194e-7, + "velocityX": -3.672292600535076, + "velocityY": 0.9041320769131688, + "timestamp": 4.7153018639458235 + }, + { + "x": 3.295105029858622, + "y": 1.3430840304472003, + "heading": -0.6963849932202171, + "angularVelocity": -2.20083034395915e-7, + "velocityX": -3.5735376056164516, + "velocityY": 1.238150467371779, + "timestamp": 4.795138273808863 + }, + { + "x": 3.0201108216375614, + "y": 1.4677619739384065, + "heading": -0.6963850130039021, + "angularVelocity": -2.4780278968053093e-7, + "velocityX": -3.444471121544888, + "velocityY": 1.5616677115753714, + "timestamp": 4.874974683671903 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.696385035506177, + "angularVelocity": -2.8185479518318006e-7, + "velocityX": -3.286187967310359, + "velocityY": 1.8719390230438346, + "timestamp": 4.954811093534943 + }, + { + "x": 2.5741841985169076, + "y": 1.7387873379942203, + "heading": -0.6963850589279509, + "angularVelocity": -4.023107769663099e-7, + "velocityX": -3.153128257888759, + "velocityY": 2.0882929565548642, + "timestamp": 5.0130292054104135 + }, + { + "x": 2.3991894720533877, + "y": 1.872411165939459, + "heading": -0.6963850794155533, + "angularVelocity": -3.519111438487621e-7, + "velocityX": -3.0058468202788267, + "velocityY": 2.2952277846293376, + "timestamp": 5.071247317285884 + }, + { + "x": 2.2335584908852413, + "y": 2.017479635272907, + "heading": -0.6963850978369923, + "angularVelocity": -3.1642110134470593e-7, + "velocityX": -2.845007779064253, + "velocityY": 2.4918099309670456, + "timestamp": 5.129465429161355 + }, + { + "x": 2.0780383093475274, + "y": 2.173338406648706, + "heading": -0.696385114808838, + "angularVelocity": -2.9152174806319146e-7, + "velocityX": -2.671336746034877, + "velocityY": 2.6771526309404154, + "timestamp": 5.187683541036826 + }, + { + "x": 1.9333303562340614, + "y": 2.339284459115452, + "heading": -0.6963851307944144, + "angularVelocity": -2.7458081036886754e-7, + "velocityX": -2.485617421310377, + "velocityY": 2.8504196910698067, + "timestamp": 5.245901652912297 + }, + { + "x": 1.8000872249814166, + "y": 2.5145692341351995, + "heading": -0.6963851461633941, + "angularVelocity": -2.6398966215373687e-7, + "velocityX": -2.2886886393302137, + "velocityY": 3.0108289220145825, + "timestamp": 5.304119764787767 + }, + { + "x": 1.6789094155739503, + "y": 2.698401829383423, + "heading": -0.6963851612308563, + "angularVelocity": -2.588105610313673e-7, + "velocityX": -2.0814451981312536, + "velocityY": 3.157652993649885, + "timestamp": 5.362337876663238 + }, + { + "x": 1.5665736982883989, + "y": 2.8877670089616614, + "heading": -0.6963851762457391, + "angularVelocity": -2.5790741716981964e-7, + "velocityX": -1.929566481404267, + "velocityY": 3.2526850060560912, + "timestamp": 5.420555988538709 + }, + { + "x": 1.4540456075762054, + "y": 3.0767463777174253, + "heading": -0.6971250426595832, + "angularVelocity": -0.012708526436353156, + "velocityX": -1.9328708384238364, + "velocityY": 3.24605801644674, + "timestamp": 5.47877410041418 + }, + { + "x": 1.3486317039814464, + "y": 3.254480208772174, + "heading": -0.7253513181524345, + "angularVelocity": -0.48483666995638336, + "velocityX": -1.8106719747325544, + "velocityY": 3.0528958313681422, + "timestamp": 5.53699221228965 + }, + { + "x": 1.2507109072486164, + "y": 3.4195717896549707, + "heading": -0.7601787682651481, + "angularVelocity": -0.5982236281934066, + "velocityX": -1.6819644879978934, + "velocityY": 2.835742616248544, + "timestamp": 5.595210324165121 + }, + { + "x": 1.160328320301069, + "y": 3.5719513486090504, + "heading": -0.7969704460064527, + "angularVelocity": -0.631962744171482, + "velocityX": -1.5524822780387872, + "velocityY": 2.6173909466528555, + "timestamp": 5.653428436040592 + }, + { + "x": 1.0774888591394964, + "y": 3.71161238889488, + "heading": -0.8336344675120989, + "angularVelocity": -0.6297700204374729, + "velocityX": -1.422915626991949, + "velocityY": 2.3989276839579965, + "timestamp": 5.711646547916063 + }, + { + "x": 1.0021908033919844, + "y": 3.838558158350536, + "heading": -0.8689739346571358, + "angularVelocity": -0.6070184347549548, + "velocityX": -1.2933785264038764, + "velocityY": 2.1805202086799977, + "timestamp": 5.7698646597915335 + }, + { + "x": 0.9344313462846832, + "y": 3.9527935365952946, + "heading": -0.9022118432658016, + "angularVelocity": -0.5709204152783613, + "velocityX": -1.1638896371672067, + "velocityY": 1.962196549573947, + "timestamp": 5.828082771667004 + }, + { + "x": 0.8742077702690397, + "y": 4.054323289061519, + "heading": -0.9328020557936417, + "angularVelocity": -0.5254415085338586, + "velocityX": -1.034447426678191, + "velocityY": 1.7439547452758104, + "timestamp": 5.886300883542475 + }, + { + "x": 0.8215176865940929, + "y": 4.143151679881797, + "heading": -0.9603390876580659, + "angularVelocity": -0.4729976802292441, + "velocityX": -0.9050462472512312, + "velocityY": 1.5257861850670007, + "timestamp": 5.944518995417946 + }, + { + "x": 0.7763590444591775, + "y": 4.21928243186123, + "heading": -0.984509600370908, + "angularVelocity": -0.41517170403161197, + "velocityX": -0.7756802939866982, + "velocityY": 1.3076815706816212, + "timestamp": 6.002737107293417 + }, + { + "x": 0.7387300854642734, + "y": 4.282718776761074, + "heading": -1.0050639989905081, + "angularVelocity": -0.3530584891445192, + "velocityX": -0.6463445443815322, + "velocityY": 1.0896324675649907, + "timestamp": 6.060955219168887 + }, + { + "x": 0.7086292911290721, + "y": 4.333463522832413, + "heading": -1.0217986916878445, + "angularVelocity": -0.28744822115035373, + "velocityX": -0.5170348773863939, + "velocityY": 0.8716316011739073, + "timestamp": 6.119173331044358 + }, + { + "x": 0.6860553360496396, + "y": 4.371519118466469, + "heading": -1.0345444282293665, + "angularVelocity": -0.21893077825652074, + "velocityX": -0.3877479765698806, + "velocityY": 0.6536727902728675, + "timestamp": 6.177391442919829 + }, + { + "x": 0.6710070489868055, + "y": 4.396887706764773, + "heading": -1.0431583071392123, + "angularVelocity": -0.14795874741302203, + "velocityX": -0.25848119387696106, + "velocityY": 0.4357507909663603, + "timestamp": 6.2356095547953 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -0.07488723824993983, + "velocityX": -0.12923242394972312, + "velocityY": 0.2178611369128849, + "timestamp": 6.29382766667077 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -4.5881817726943837e-32, + "velocityX": 7.1951222626888875e-34, + "velocityY": 0, + "timestamp": 6.352045778546241 + }, + { + "x": 0.6704852067504524, + "y": 4.397985362145517, + "heading": -1.0417684453451177, + "angularVelocity": 0.10293302663784343, + "velocityX": 0.12534996227388928, + "velocityY": -0.20741457823419143, + "timestamp": 6.40790399622796 + }, + { + "x": 0.684490344241533, + "y": 4.374811471822251, + "heading": -1.0303744644017594, + "angularVelocity": 0.20398038849505432, + "velocityX": 0.25072653715666804, + "velocityY": -0.4148698487895252, + "timestamp": 6.463762213909678 + }, + { + "x": 0.7055004573054668, + "y": 4.340046908929331, + "heading": -1.0134578875635165, + "angularVelocity": 0.3028484892703545, + "velocityX": 0.3761328938859112, + "velocityY": -0.6223715029901059, + "timestamp": 6.519620431591397 + }, + { + "x": 0.7335174330880717, + "y": 4.293688705050122, + "heading": -0.9911609198549822, + "angularVelocity": 0.3991707690278106, + "velocityX": 0.5015730351843025, + "velocityY": -0.8299262991769599, + "timestamp": 6.575478649273116 + }, + { + "x": 0.768543442750825, + "y": 4.235733440885856, + "heading": -0.963651742055662, + "angularVelocity": 0.49248219762521406, + "velocityX": 0.6270520456333297, + "velocityY": -1.0375423092533531, + "timestamp": 6.631336866954834 + }, + { + "x": 0.8105810218384888, + "y": 4.16617715590748, + "heading": -0.9311321668224927, + "angularVelocity": 0.5821806814257259, + "velocityX": 0.7525764485933851, + "velocityY": -1.24522922257758, + "timestamp": 6.687195084636553 + }, + { + "x": 0.8596331801158593, + "y": 4.085015237354172, + "heading": -0.8938487217946981, + "angularVelocity": 0.667465711853477, + "velocityX": 0.8781547337738402, + "velocityY": -1.4529987157085942, + "timestamp": 6.7430533023182715 + }, + { + "x": 0.9157035549190764, + "y": 3.992242285598939, + "heading": -0.8521094876337724, + "angularVelocity": 0.7472353378469229, + "velocityX": 1.0037981362510981, + "velocityY": -1.6608648754934532, + "timestamp": 6.79891151999999 + }, + { + "x": 0.9787966256099634, + "y": 3.88785195918136, + "heading": -0.8063112110880574, + "angularVelocity": 0.8199022175514963, + "velocityX": 1.1295217303637053, + "velocityY": -1.868844563075711, + "timestamp": 6.854769737681709 + }, + { + "x": 1.0489179988671087, + "y": 3.7718368324625953, + "heading": -0.756986124262755, + "angularVelocity": 0.8830408285913816, + "velocityX": 1.2553456978648867, + "velocityY": -2.0769571879257267, + "timestamp": 6.910627955363427 + }, + { + "x": 1.1260747058581024, + "y": 3.644188427808813, + "heading": -0.7048901245652565, + "angularVelocity": 0.9326470098695786, + "velocityX": 1.381295540624565, + "velocityY": -2.2852215833510225, + "timestamp": 6.966486173045146 + }, + { + "x": 1.210275065586603, + "y": 3.504898185530833, + "heading": -0.6511892097412871, + "angularVelocity": 0.9613789528688276, + "velocityX": 1.5073943140877994, + "velocityY": -2.4936392183449136, + "timestamp": 7.022344390726865 + }, + { + "x": 1.3015253174439423, + "y": 3.3539635614800978, + "heading": -0.5979268038301901, + "angularVelocity": 0.95352856073188, + "velocityX": 1.6336047880597582, + "velocityY": -2.7021023998790135, + "timestamp": 7.078202608408583 + }, + { + "x": 1.3998011054306767, + "y": 3.1914330516367446, + "heading": -0.5495841694257507, + "angularVelocity": 0.865452504766571, + "velocityX": 1.7593792295112722, + "velocityY": -2.909697383641142, + "timestamp": 7.134060826090302 + }, + { + "x": 1.504860441802437, + "y": 3.018388823031747, + "heading": -0.5232691895419826, + "angularVelocity": 0.4711031066854229, + "velocityX": 1.8808214929160718, + "velocityY": -3.0979189058807544, + "timestamp": 7.18991904377202 + }, + { + "x": 1.6140923684634172, + "y": 2.8375675393930413, + "heading": -0.5232691688026271, + "angularVelocity": 3.712856650457212e-7, + "velocityX": 1.9555211604385054, + "velocityY": -3.237147391079152, + "timestamp": 7.245777261453739 + }, + { + "x": 1.7233256383679056, + "y": 2.6567470671976134, + "heading": -0.5232691480644303, + "angularVelocity": 3.7126492252401944e-7, + "velocityX": 1.9555452078135154, + "velocityY": -3.2371328642411785, + "timestamp": 7.301635479135458 + }, + { + "x": 1.84078732146222, + "y": 2.4811603268646896, + "heading": -0.5232691272677354, + "angularVelocity": 3.72312181556421e-7, + "velocityX": 2.102854118325333, + "velocityY": -3.143436142796787, + "timestamp": 7.357493696817176 + }, + { + "x": 1.9693130123787856, + "y": 2.313502648911294, + "heading": -0.5232691060970931, + "angularVelocity": 3.790067651022537e-7, + "velocityX": 2.3009271733106282, + "velocityY": -3.0014863508304788, + "timestamp": 7.413351914498895 + }, + { + "x": 2.1083695489461487, + "y": 2.154470483882732, + "heading": -0.5232690841533676, + "angularVelocity": 3.928468612451328e-7, + "velocityX": 2.4894553091491622, + "velocityY": -2.847068374697022, + "timestamp": 7.4692101321806135 + }, + { + "x": 2.2573796500994945, + "y": 2.0047242347352134, + "heading": -0.5232690609733034, + "angularVelocity": 4.1498037571099624e-7, + "velocityX": 2.6676486887284834, + "velocityY": -2.6808275552359513, + "timestamp": 7.525068349862332 + }, + { + "x": 2.4157246359676448, + "y": 1.8648857059384452, + "heading": -0.5232690359846437, + "angularVelocity": 4.47358702857604e-7, + "velocityX": 2.8347661712087695, + "velocityY": -2.5034549006481424, + "timestamp": 7.580926567544051 + }, + { + "x": 2.582747044375024, + "y": 1.7355355500002163, + "heading": -0.523269008439205, + "angularVelocity": 4.931313565110266e-7, + "velocityX": 2.9901134575950428, + "velocityY": -2.3156871326484083, + "timestamp": 7.636784785225769 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.5232689753097743, + "angularVelocity": 5.930985989560848e-7, + "velocityX": 3.133045325838173, + "velocityY": -2.1183039826598433, + "timestamp": 7.692643002907488 + }, + { + "x": 3.027691141146945, + "y": 1.4695236072310565, + "heading": -0.5232689479285146, + "angularVelocity": 3.365468040910692e-7, + "velocityX": 3.3178420048715034, + "velocityY": -1.8152442667680868, + "timestamp": 7.7740024433384125 + }, + { + "x": 3.3102859882061892, + "y": 1.3477940719025636, + "heading": -0.523268924510295, + "angularVelocity": 2.8783653775450794e-7, + "velocityX": 3.473411881429686, + "velocityY": -1.4961943529078627, + "timestamp": 7.855361883769337 + }, + { + "x": 3.6030485594101127, + "y": 1.2530945172715204, + "heading": -0.5232689034957836, + "angularVelocity": 2.582922326531371e-7, + "velocityX": 3.5983847682984376, + "velocityY": -1.1639651665432065, + "timestamp": 7.936721324200262 + }, + { + "x": 3.9033999272897986, + "y": 1.1862590073495238, + "heading": -0.5232688838271395, + "angularVelocity": 2.4174999152857024e-7, + "velocityX": 3.6916597052396023, + "velocityY": -0.8214843854382324, + "timestamp": 8.018080764631186 + }, + { + "x": 4.2086942600952995, + "y": 1.1478754132776305, + "heading": -0.5232688646920325, + "angularVelocity": 2.3519221562308788e-7, + "velocityX": 3.752414362592638, + "velocityY": -0.471777999806691, + "timestamp": 8.09944020506211 + }, + { + "x": 4.515097551396115, + "y": 1.1196805012022473, + "heading": -0.5232688456111896, + "angularVelocity": 2.345252478429259e-7, + "velocityX": 3.7660447229963117, + "velocityY": -0.3465475171172218, + "timestamp": 8.180799645493035 + }, + { + "x": 4.821500870663513, + "y": 1.0914858930504614, + "heading": -0.5232688265303463, + "angularVelocity": 2.3452525108540152e-7, + "velocityX": 3.7660450667373904, + "velocityY": -0.3465437815507533, + "timestamp": 8.26215908592396 + }, + { + "x": 5.127904189931636, + "y": 1.0632912849065406, + "heading": -0.5232688074495031, + "angularVelocity": 2.3452525107290264e-7, + "velocityX": 3.7660450667462855, + "velocityY": -0.34654378145408427, + "timestamp": 8.343518526354885 + }, + { + "x": 5.4343075091997575, + "y": 1.0350966767626197, + "heading": -0.5232687883686599, + "angularVelocity": 2.3452525075472094e-7, + "velocityX": 3.7660450667462855, + "velocityY": -0.3465437814540822, + "timestamp": 8.42487796678581 + }, + { + "x": 5.740710828467879, + "y": 1.0069020686186991, + "heading": -0.5232687692878167, + "angularVelocity": 2.3452525040262446e-7, + "velocityX": 3.7660450667462855, + "velocityY": -0.3465437814540818, + "timestamp": 8.506237407216734 + }, + { + "x": 6.047114147736001, + "y": 0.9787074604747784, + "heading": -0.5232687502069736, + "angularVelocity": 2.345252500146494e-7, + "velocityX": 3.766045066746286, + "velocityY": -0.34654378145408327, + "timestamp": 8.587596847647658 + }, + { + "x": 6.353517467003837, + "y": 0.9505128523277552, + "heading": -0.5232687311261306, + "angularVelocity": 2.3452524994005725e-7, + "velocityX": 3.7660450667427767, + "velocityY": -0.3465437814922157, + "timestamp": 8.668956288078583 + }, + { + "x": 6.659920775239498, + "y": 0.9223181242925758, + "heading": -0.5232687120445318, + "angularVelocity": 2.3453453739742123e-7, + "velocityX": 3.7660449311448136, + "velocityY": -0.3465452550539253, + "timestamp": 8.750315728509507 + }, + { + "x": 6.946350295372631, + "y": 0.8955257235290481, + "heading": -0.4609901004295649, + "angularVelocity": 0.7654749256522044, + "velocityX": 3.5205443721840264, + "velocityY": -0.3293090589318255, + "timestamp": 8.831675168940432 + }, + { + "x": 7.204305307998309, + "y": 0.8715129005451068, + "heading": -0.38283013166037505, + "angularVelocity": 0.9606748566019065, + "velocityX": 3.170560309404857, + "velocityY": -0.2951448886171808, + "timestamp": 8.913034609371357 + }, + { + "x": 7.433510095635826, + "y": 0.8502195253793108, + "heading": -0.30543025852913497, + "angularVelocity": 0.9513324172497697, + "velocityX": 2.8171873653938824, + "velocityY": -0.2617197838752883, + "timestamp": 8.994394049802281 + }, + { + "x": 7.633994056446923, + "y": 0.8316157363547472, + "heading": -0.2335287634090791, + "angularVelocity": 0.8837511017679792, + "velocityX": 2.4641757582060952, + "velocityY": -0.22866171308489489, + "timestamp": 9.075753490233206 + }, + { + "x": 7.805788503499911, + "y": 0.8156861724274476, + "heading": -0.16939985967365537, + "angularVelocity": 0.7882171189447914, + "velocityX": 2.1115490242198023, + "velocityY": -0.19579244698498122, + "timestamp": 9.15711293066413 + }, + { + "x": 7.948917170139319, + "y": 0.802421883536533, + "heading": -0.1143881084380515, + "angularVelocity": 0.6761569517222741, + "velocityX": 1.759214000014262, + "velocityY": -0.16303318730634864, + "timestamp": 9.238472371095055 + }, + { + "x": 8.063397872753223, + "y": 0.7918170647898031, + "heading": -0.0693839808587397, + "angularVelocity": 0.5531518818338115, + "velocityX": 1.407097959469132, + "velocityY": -0.13034527635097967, + "timestamp": 9.31983181152598 + }, + { + "x": 8.149244274991434, + "y": 0.7838676118817843, + "heading": -0.0350206058237791, + "angularVelocity": 0.4223649382659615, + "velocityX": 1.0551498607109575, + "velocityY": -0.09770781197503392, + "timestamp": 9.401191251956904 + }, + { + "x": 8.20646710981356, + "y": 0.7785704017832406, + "heading": -0.011770656016753542, + "angularVelocity": 0.2857683101540642, + "velocityX": 0.703333682225969, + "velocityY": -0.06510873317818848, + "timestamp": 9.482550692387829 + }, + { + "x": 8.235074996948242, + "y": 0.7759228944778442, + "heading": -3.5413188929792264e-38, + "angularVelocity": 0.1446747415470122, + "velocityX": 0.35162345000356726, + "velocityY": -0.03254087406911382, + "timestamp": 9.563910132818753 }, + { + "x": 8.235074996948242, + "y": 0.7759228944778442, + "heading": -7.544724392466369e-38, + "angularVelocity": -4.920640404901405e-37, + "velocityX": -6.041520920103385e-38, + "velocityY": 3.1783876166730987e-37, + "timestamp": 9.645269573249678 + } + ], + "constraints": [ { "scope": [ - 1 + "first" ], "type": "StopPoint" }, { "scope": [ - 2 + "last" ], "type": "StopPoint" }, { "scope": [ - 3 + 5 ], "type": "StopPoint" } @@ -9244,56 +3787,101 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "CenterLaneSprintBonus": { + "CenterLanePSubCSubBSubASubSprint": { "waypoints": [ { - "x": 1.3954176902770996, - "y": 5.518980503082275, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 17 }, { - "x": 3.866603136062622, - "y": 7.085535526275635, - "heading": 0, + "x": 2.614555597305298, + "y": 4.302172660827637, + "heading": -0.7216551150961179, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, + "headingConstrained": true, "controlIntervalCount": 17 }, { - "x": 6.403980731964111, - "y": 6.931086540222168, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 2.525056838989258, + "y": 5.573054313659668, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 19 + "headingConstrained": true, + "controlIntervalCount": 11 }, { - "x": 7.598227500915527, - "y": 4.000265598297119, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 17 + }, + { + "x": 2.68615460395813, + "y": 6.772337436676025, + "heading": 0.8050032485420815, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 6.426044940948486, - "y": 6.842829704284668, + "x": 4.16749906539917, + "y": 4.98994779586792, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 24 + "controlIntervalCount": 13 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 1.4015250205993652, - "y": 5.514177322387695, + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -9303,877 +3891,1282 @@ ], "trajectory": [ { - "x": 1.3954176902770996, - "y": 5.518980503082275, - "heading": -4.154669365501339e-31, - "angularVelocity": -1.673896254624468e-30, - "velocityX": -8.167537270046595e-18, - "velocityY": 1.3237751910388058e-17, + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": -7.2072188294937895e-31, + "velocityX": -1.1048793733131498e-32, + "velocityY": -5.425421859025623e-32, "timestamp": 0 }, { - "x": 1.4086689901208764, - "y": 5.531099909602429, - "heading": 5.4865363809901704e-18, - "angularVelocity": 8.089770653155719e-17, - "velocityX": 0.19538732001957015, - "velocityY": 0.1786978174305604, - "timestamp": 0.06782067455784145 - }, - { - "x": 1.435302968552215, - "y": 5.555193491205879, - "heading": 8.977091372122931e-18, - "angularVelocity": 5.146742470079397e-17, - "velocityX": 0.39271178891950187, - "velocityY": 0.35525423125810013, - "timestamp": 0.1356413491156829 - }, - { - "x": 1.4754682380563253, - "y": 5.591093050497596, - "heading": 1.4654590389523967e-17, - "angularVelocity": 8.37134055043427e-17, - "velocityX": 0.5922275141904324, - "velocityY": 0.5293306137953979, - "timestamp": 0.20346202367352434 - }, - { - "x": 1.5293341329115746, - "y": 5.638601649509276, - "heading": 2.0391035972700556e-17, - "angularVelocity": 8.458255853655089e-17, - "velocityX": 0.7942400338307883, - "velocityY": 0.7005031920047591, - "timestamp": 0.2712826982313658 - }, - { - "x": 1.597095145056665, - "y": 5.697485776780742, - "heading": 2.0873484642332193e-17, - "angularVelocity": 7.113597162801294e-18, - "velocityX": 0.9991202916641198, - "velocityY": 0.8682326982939436, - "timestamp": 0.33910337278920727 - }, - { - "x": 1.6789766146521206, - "y": 5.767464479151507, - "heading": 2.2177946405449375e-17, - "angularVelocity": 1.9233987436627017e-17, - "velocityX": 1.2073231375134688, - "velocityY": 1.031819615876666, - "timestamp": 0.40692404734704873 - }, - { - "x": 1.7752420754207336, - "y": 5.848193888395741, - "heading": 2.5659685154236302e-17, - "angularVelocity": 5.133742988390828e-17, - "velocityX": 1.4194117265305242, - "velocityY": 1.1903362768146415, - "timestamp": 0.4747447219048902 - }, - { - "x": 1.8862027378041668, - "y": 5.939244534569037, - "heading": 2.687644733119201e-17, - "angularVelocity": 1.7940873991424383e-17, - "velocityX": 1.6360890407953306, - "velocityY": 1.3425205037682877, - "timestamp": 0.5425653964627316 - }, - { - "x": 2.012229564399117, - "y": 6.040066954163349, - "heading": 2.5975996915224922e-17, - "angularVelocity": -1.3276929351019487e-17, - "velocityX": 1.8582361118727206, - "velocityY": 1.486603019678443, - "timestamp": 0.6103860710205731 - }, - { - "x": 2.1537679046461733, - "y": 6.149937565607546, - "heading": 2.3459157805274306e-17, - "angularVelocity": -3.711020537728529e-17, - "velocityX": 2.0869497563953514, - "velocityY": 1.6200164943876396, - "timestamp": 0.6782067455784145 - }, - { - "x": 2.311352553455493, - "y": 6.2678699868034595, - "heading": 2.1605195532052067e-17, - "angularVelocity": -2.7336238682566866e-17, - "velocityX": 2.3235488269129725, - "velocityY": 1.7388859955283122, - "timestamp": 0.746027420136256 - }, - { - "x": 2.485613520095621, - "y": 6.392464027187378, - "heading": 1.967921307517928e-17, - "angularVelocity": -2.839815790860674e-17, - "velocityX": 2.5694372368931395, - "velocityY": 1.8371100139627066, - "timestamp": 0.8138480946940975 - }, - { - "x": 2.6772369323815792, - "y": 6.5216430608077145, - "heading": 1.8045458271823762e-17, - "angularVelocity": -2.4089331717730728e-17, - "velocityX": 2.825442441191459, - "velocityY": 1.9047146679462823, - "timestamp": 0.8816687692519389 - }, - { - "x": 2.886763562184598, - "y": 6.652219019276088, - "heading": 1.683660040648187e-17, - "angularVelocity": -1.7824326418575357e-17, - "velocityX": 3.0894212003792894, - "velocityY": 1.9253119984379097, - "timestamp": 0.9494894438097804 - }, - { - "x": 3.113920279000272, - "y": 6.779380811677554, - "heading": 1.476992837613865e-17, - "angularVelocity": -3.0472596800531746e-17, - "velocityX": 3.3493727140967953, - "velocityY": 1.87497091750402, - "timestamp": 1.0173101183676219 - }, - { - "x": 3.356286094849282, - "y": 6.896994843265456, - "heading": 1.2661893348816599e-17, - "angularVelocity": -3.108248574533372e-17, - "velocityX": 3.5736273257250715, - "velocityY": 1.7341914151500553, - "timestamp": 1.0851307929254632 - }, - { - "x": 3.6089329789773132, - "y": 6.999885557033016, - "heading": 1.0426963344280672e-17, - "angularVelocity": -3.295352531869201e-17, - "velocityX": 3.725219275319372, - "velocityY": 1.5170995340042566, - "timestamp": 1.1529514674833046 - }, - { - "x": 3.866603136062622, - "y": 7.085535526275635, - "heading": 5.130524196392721e-18, - "angularVelocity": -7.809476623133778e-17, - "velocityX": 3.7992862613826874, - "velocityY": 1.262888784297902, - "timestamp": 1.220772142041146 - }, - { - "x": 4.03577288165141, - "y": 7.133931780153134, - "heading": 1.53240025945702e-18, - "angularVelocity": -8.102449650854585e-17, - "velocityX": 3.809456142072583, - "velocityY": 1.0898131101710078, - "timestamp": 1.2651799890617301 - }, - { - "x": 4.204491733807284, - "y": 7.174642072343406, - "heading": -1.8107357967918406e-18, - "angularVelocity": -7.528254097814744e-17, - "velocityX": 3.7993026790437296, - "velocityY": 0.9167364536138055, - "timestamp": 1.3095878360823143 - }, - { - "x": 4.372041951477231, - "y": 7.207742396297824, - "heading": -3.82115164020192e-18, - "angularVelocity": -4.5271625051994066e-17, - "velocityX": 3.7729867334545513, - "velocityY": 0.7453710588373391, - "timestamp": 1.3539956831028985 - }, - { - "x": 4.537853181541874, - "y": 7.2333425032674095, - "heading": -5.87160481310551e-18, - "angularVelocity": -4.617321158640688e-17, - "velocityX": 3.7338272667842607, - "velocityY": 0.5764771022950116, - "timestamp": 1.3984035301234827 - }, - { - "x": 4.701468659536594, - "y": 7.251563178434466, - "heading": -5.388451020675662e-18, - "angularVelocity": 1.0879921481471181e-17, - "velocityX": 3.684382130006832, - "velocityY": 0.410303052039688, - "timestamp": 1.442811377144067 - }, - { - "x": 4.8625183641051555, - "y": 7.262524779655412, - "heading": -5.872012470604977e-18, - "angularVelocity": -1.0889098042063212e-17, - "velocityX": 3.626604651513686, - "velocityY": 0.24683928531516408, - "timestamp": 1.4872192241646511 - }, - { - "x": 5.020698725587354, - "y": 7.266341856818374, - "heading": -7.150563317225696e-18, - "angularVelocity": -2.879110223357202e-17, - "velocityX": 3.561991226660372, - "velocityY": 0.08595501513940547, - "timestamp": 1.5316270711852353 - }, - { - "x": 5.175757540528759, - "y": 7.263120943781581, - "heading": -8.096150179100186e-18, - "angularVelocity": -2.1293241253900183e-17, - "velocityX": 3.4916985475456466, - "velocityY": -0.07253026779929446, - "timestamp": 1.5760349182058195 - }, - { - "x": 5.327482769552672, - "y": 7.252959960990395, - "heading": -8.592025684797133e-18, - "angularVelocity": -1.116639798884869e-17, - "velocityX": 3.41663105066957, - "velocityY": -0.22881052500644172, - "timestamp": 1.6204427652264037 - }, - { - "x": 5.475694166399093, - "y": 7.235948405191675, - "heading": -8.833226442519028e-18, - "angularVelocity": -5.431493574531005e-18, - "velocityX": 3.3375046706884697, - "velocityY": -0.38307544589665543, - "timestamp": 1.664850612246988 - }, - { - "x": 5.620236960766286, - "y": 7.212167896274621, - "heading": -9.572307077519199e-18, - "angularVelocity": -1.6643020598536764e-17, - "velocityX": 3.254893089056846, - "velocityY": -0.5355024058255178, - "timestamp": 1.7092584592675721 - }, - { - "x": 5.7609770366577315, - "y": 7.1816928593209, - "heading": -1.1397932886275862e-17, - "angularVelocity": -4.111043115967244e-17, - "velocityX": 3.169261410628764, - "velocityY": -0.6862534213738285, - "timestamp": 1.7536663062881563 - }, - { - "x": 5.897797208582785, - "y": 7.144591229805976, - "heading": -1.3242662306872499e-17, - "angularVelocity": -4.154061444960465e-17, - "velocityX": 3.080990885724137, - "velocityY": -0.8354746290160561, - "timestamp": 1.7980741533087405 - }, - { - "x": 6.030594311960715, - "y": 7.100925127890339, - "heading": -1.2969314838489408e-17, - "angularVelocity": 6.1553878370471014e-18, - "velocityX": 2.990397244801673, - "velocityY": -0.9832969811708759, - "timestamp": 1.8424820003293247 - }, - { - "x": 6.159276904080374, - "y": 7.050751478229476, - "heading": -1.2712775294894754e-17, - "angularVelocity": 5.77689752908346e-18, - "velocityX": 2.8977444472822667, - "velocityY": -1.1298374730395475, - "timestamp": 1.886889847349909 - }, - { - "x": 6.283763428080075, - "y": 6.994122567551955, - "heading": -1.1520483238842908e-17, - "angularVelocity": 2.6848678729359197e-17, - "velocityX": 2.8032551080892625, - "velocityY": -1.27520054397759, - "timestamp": 1.9312976943704931 - }, - { - "x": 6.403980731964111, - "y": 6.931086540222168, - "heading": -1.0278212817380173e-17, - "angularVelocity": 2.797411882250772e-17, - "velocityX": 2.707118492556311, - "velocityY": -1.4194794739895686, - "timestamp": 1.9757055413910773 - }, - { - "x": 6.60729389364524, - "y": 6.796210534013358, - "heading": -8.076224683373957e-18, - "angularVelocity": 2.7406592243829792e-17, - "velocityX": 2.5304956120267805, - "velocityY": -1.6787065778573356, - "timestamp": 2.0560507357138165 - }, - { - "x": 6.795704830022524, - "y": 6.641009825084134, - "heading": -5.924369506882037e-18, - "angularVelocity": 2.6782622499519642e-17, - "velocityX": 2.3450181179530984, - "velocityY": -1.9316738261382134, - "timestamp": 2.1363959300365556 - }, - { - "x": 6.968323808401379, - "y": 6.4661677340473895, - "heading": 1.813427886027723e-18, - "angularVelocity": 9.630691182046325e-17, - "velocityX": 2.148466748184869, - "velocityY": -2.176136264409528, - "timestamp": 2.2167411243592947 - }, - { - "x": 7.12401657856057, - "y": 6.272652848277024, - "heading": 5.514219590575789e-18, - "angularVelocity": 4.606114294192909e-17, - "velocityX": 1.9377981654234078, - "velocityY": -2.4085433783759767, - "timestamp": 2.297086318682034 - }, - { - "x": 7.261313462923536, - "y": 6.061911206598393, - "heading": 9.2916222876539e-18, - "angularVelocity": 4.7014669947754546e-17, - "velocityX": 1.7088375418131223, - "velocityY": -2.622952666367338, - "timestamp": 2.377431513004773 - }, - { - "x": 7.378307598032396, - "y": 5.836237450824618, - "heading": 1.3317359546362108e-17, - "angularVelocity": 5.010551054488734e-17, - "velocityX": 1.4561435328528385, - "velocityY": -2.8088021651582906, - "timestamp": 2.457776707327512 - }, - { - "x": 7.472656845707877, - "y": 5.59950120754036, - "heading": 1.7242988626530655e-17, - "angularVelocity": 4.8859538047431847e-17, - "velocityX": 1.1742985809019115, - "velocityY": -2.946489149473078, - "timestamp": 2.538121901650251 - }, - { - "x": 7.542192569709091, - "y": 5.358353787993265, - "heading": 1.7116315403461932e-17, - "angularVelocity": -1.5766120825832005e-18, - "velocityX": 0.8654621422893939, - "velocityY": -3.001391951065907, - "timestamp": 2.6184670959729903 - }, - { - "x": 7.5871412458956256, - "y": 5.1227422896934245, - "heading": 1.6858435017814408e-17, - "angularVelocity": -3.209652588562192e-18, - "velocityX": 0.5594444890627913, - "velocityY": -2.932490241462491, - "timestamp": 2.6988122902957294 - }, - { - "x": 7.612106015705454, - "y": 4.902486990914375, - "heading": 1.5879880365887054e-17, - "angularVelocity": -1.2179381518025204e-17, - "velocityX": 0.3107188926514608, - "velocityY": -2.741362450307933, - "timestamp": 2.7791574846184686 - }, - { - "x": 7.623350532741401, - "y": 4.703372273008907, - "heading": 1.7045873071267902e-17, - "angularVelocity": 1.45122904961207e-17, - "velocityX": 0.13995257750920653, - "velocityY": -2.4782405417509445, - "timestamp": 2.8595026789412077 - }, - { - "x": 7.625915530957838, - "y": 4.527918399643226, - "heading": 1.7155064370003262e-17, - "angularVelocity": 1.3590256508962654e-18, - "velocityX": 0.03192472478357754, - "velocityY": -2.1837506878246002, - "timestamp": 2.939847873263947 - }, - { - "x": 7.6233153599672425, - "y": 4.377132131065689, - "heading": 1.6703747348721444e-17, - "angularVelocity": -5.617224306630994e-18, - "velocityX": -0.032362495510961514, - "velocityY": -1.8767303987322825, - "timestamp": 3.020193067586686 - }, - { - "x": 7.6179855647139085, - "y": 4.251400181756933, - "heading": 1.5614701617946926e-17, - "angularVelocity": -1.355458489040667e-17, - "velocityX": -0.06633620465122089, - "velocityY": -1.5648969470868832, - "timestamp": 3.100538261909425 - }, - { - "x": 7.611666193625904, - "y": 4.150851372626415, - "heading": 1.448138270462699e-17, - "angularVelocity": -1.4105621493743314e-17, - "velocityX": -0.07865275753296536, - "velocityY": -1.251460152384776, - "timestamp": 3.180883456232164 - }, - { - "x": 7.605645305191298, - "y": 4.075503372077343, - "heading": 1.0097210956722963e-17, - "angularVelocity": -5.456669753373492e-17, - "velocityX": -0.07493775434062062, - "velocityY": -0.9378034515220308, - "timestamp": 3.2612286505549033 - }, - { - "x": 7.600907710154459, - "y": 4.0253252549341, - "heading": 5.681220340527481e-18, - "angularVelocity": -5.496272423373191e-17, - "velocityX": -0.058965505986702256, - "velocityY": -0.624531654521636, - "timestamp": 3.3415738448776424 + "x": 1.3063416372973582, + "y": 5.5750227907482355, + "heading": -0.009007895171544168, + "angularVelocity": -0.12420787311314503, + "velocityX": 0.22568350897133957, + "velocityY": -0.21967610709029223, + "timestamp": 0.07252273906455099 + }, + { + "x": 1.339076097606074, + "y": 5.5431599730454195, + "heading": -0.02701891548605667, + "angularVelocity": -0.24834997335784384, + "velocityX": 0.4513682292059063, + "velocityY": -0.43934934220379995, + "timestamp": 0.14504547812910198 + }, + { + "x": 1.388178099815531, + "y": 5.4953660820311345, + "heading": -0.05401968965283247, + "angularVelocity": -0.3723076998340179, + "velocityX": 0.677056642410746, + "velocityY": -0.6590193866197215, + "timestamp": 0.217568217193653 + }, + { + "x": 1.4536480974194865, + "y": 5.431641407654605, + "heading": -0.08998953758011774, + "angularVelocity": -0.49598027310141063, + "velocityX": 0.9027513087406556, + "velocityY": -0.8786854329899796, + "timestamp": 0.29009095625820397 + }, + { + "x": 1.5354867341790108, + "y": 5.351986341420191, + "heading": -0.13490221177586337, + "angularVelocity": -0.6192909255093908, + "velocityX": 1.1284548517495026, + "velocityY": -1.0983460809932237, + "timestamp": 0.36261369532275495 + }, + { + "x": 1.6336948546750298, + "y": 5.2564014251813616, + "heading": -0.18872783549465025, + "angularVelocity": -0.7421896140863778, + "velocityX": 1.3541700404978287, + "velocityY": -1.317999257498918, + "timestamp": 0.4351364343873059 + }, + { + "x": 1.7482735602297292, + "y": 5.144887415721353, + "heading": -0.25143410509925246, + "angularVelocity": -0.8646428749583058, + "velocityX": 1.579900415134063, + "velocityY": -1.5376419988880228, + "timestamp": 0.507659173451857 + }, + { + "x": 1.87922458951427, + "y": 5.017445479597787, + "heading": -0.32298143653104017, + "angularVelocity": -0.9865503200055588, + "velocityX": 1.8056547639221738, + "velocityY": -1.7572686548722531, + "timestamp": 0.5801819125164079 + }, + { + "x": 2.026417113452321, + "y": 4.874266246183284, + "heading": -0.4026826239248217, + "angularVelocity": -1.0989820354525424, + "velocityX": 2.0296051395270776, + "velocityY": -1.9742667646213359, + "timestamp": 0.6527046515809589 + }, + { + "x": 2.1572394289562786, + "y": 4.747014797378468, + "heading": -0.4735071808339005, + "angularVelocity": -0.9765841420579419, + "velocityX": 1.8038799580839464, + "velocityY": -1.7546420673865035, + "timestamp": 0.7252273906455099 + }, + { + "x": 2.27168995904481, + "y": 4.635690037684074, + "heading": -0.535478944048102, + "angularVelocity": -0.8545149288832774, + "velocityX": 1.578133031994185, + "velocityY": -1.5350324757486038, + "timestamp": 0.7977501297100609 + }, + { + "x": 2.369767924446142, + "y": 4.540291261559593, + "heading": -0.5886064902849578, + "angularVelocity": -0.732563978168669, + "velocityX": 1.35237536069838, + "velocityY": -1.3154326126526381, + "timestamp": 0.8702728687746119 + }, + { + "x": 2.4514728066095985, + "y": 4.460817939321001, + "heading": -0.6328935848970949, + "angularVelocity": -0.6106649470681784, + "velocityX": 1.1266105392220715, + "velocityY": -1.0958400532524892, + "timestamp": 0.9427956078391628 + }, + { + "x": 2.5168042340609067, + "y": 4.397269668318764, + "heading": -0.6683418026498548, + "angularVelocity": -0.48878763005849607, + "velocityX": 0.9008405955692155, + "velocityY": -0.8762530458976289, + "timestamp": 1.015318346903714 + }, + { + "x": 2.5657619428441123, + "y": 4.349646151783931, + "heading": -0.6949516025618677, + "angularVelocity": -0.36691664235512766, + "velocityX": 0.6750670122861077, + "velocityY": -0.6566701306241847, + "timestamp": 1.087841085968265 + }, + { + "x": 2.5983457601971756, + "y": 4.317947187379743, + "heading": -0.7127228546140986, + "angularVelocity": -0.24504386184805346, + "velocityX": 0.4492910468254882, + "velocityY": -0.4370900053275183, + "timestamp": 1.160363825032816 }, { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -1.0462507114010796e-31, - "angularVelocity": -7.071014853118142e-17, - "velocityX": -0.03335867517061736, - "velocityY": -0.3118998821051884, - "timestamp": 3.4219190392003815 + "x": 2.614555597305298, + "y": 4.302172660827637, + "heading": -0.7216551150961179, + "angularVelocity": -0.12316496311606469, + "velocityX": 0.22351385671724336, + "velocityY": -0.21751145579268175, + "timestamp": 1.2328865640973672 + }, + { + "x": 2.6122418058900085, + "y": 4.3044120467003895, + "heading": -0.7206083682746678, + "angularVelocity": 0.01292691365951148, + "velocityX": -0.028574418607166113, + "velocityY": 0.027655539271243604, + "timestamp": 1.3138607895750445 + }, + { + "x": 2.589515477392911, + "y": 4.326503828401164, + "heading": -0.7085432954922883, + "angularVelocity": 0.148998927882974, + "velocityX": -0.2806612642842681, + "velocityY": 0.2728248596434546, + "timestamp": 1.3948350150527218 + }, + { + "x": 2.54637684601469, + "y": 4.3684483333856905, + "heading": -0.6854613533337227, + "angularVelocity": 0.2850529538548107, + "velocityX": -0.5327452176745692, + "velocityY": 0.5179982239657722, + "timestamp": 1.475809240530399 + }, + { + "x": 2.4828262685453337, + "y": 4.430246041385043, + "heading": -0.6513636060932391, + "angularVelocity": 0.42109383620888585, + "velocityX": -0.784824764848103, + "velocityY": 0.7631775127791823, + "timestamp": 1.5567834660080764 + }, + { + "x": 2.398864238908538, + "y": 4.511897605460961, + "heading": -0.6062501465002338, + "angularVelocity": 0.5571335733911974, + "velocityX": -1.0368982122572787, + "velocityY": 1.0083648666494986, + "timestamp": 1.6377576914857537 + }, + { + "x": 2.2944914266815206, + "y": 4.613403885366389, + "heading": -0.5501188636580988, + "angularVelocity": 0.6931993793206396, + "velocityX": -1.2889633906498084, + "velocityY": 1.2535628381323076, + "timestamp": 1.718731916963431 + }, + { + "x": 2.1697087770291006, + "y": 4.73476601037198, + "heading": -0.48296289489208905, + "angularVelocity": 0.8293499365978624, + "velocityX": -1.5410168966266635, + "velocityY": 1.4987747556657698, + "timestamp": 1.7997061424411083 + }, + { + "x": 2.0245178868987646, + "y": 4.87598561985106, + "heading": -0.4047647421109401, + "angularVelocity": 0.9657165884553096, + "velocityX": -1.7930506809269309, + "velocityY": 1.7440069188183338, + "timestamp": 1.8806803679187856 + }, + { + "x": 1.8612798092271896, + "y": 5.034863868368553, + "heading": -0.31505899294827, + "angularVelocity": 1.1078309009257712, + "velocityX": -2.0159263853239904, + "velocityY": 1.9620842012417825, + "timestamp": 1.9616545933964629 + }, + { + "x": 1.7184500483895917, + "y": 5.1738844559195085, + "heading": -0.23644393225782434, + "angularVelocity": 0.9708652380027345, + "velocityX": -1.7638916580562496, + "velocityY": 1.7168498584691285, + "timestamp": 2.04262881887414 + }, + { + "x": 1.5960267405860193, + "y": 5.293046114041946, + "heading": -0.168968478820667, + "angularVelocity": 0.8332954472751476, + "velocityX": -1.5118799479886125, + "velocityY": 1.4715998506873489, + "timestamp": 2.1236030443518175 + }, + { + "x": 1.4940086408037947, + "y": 5.392348179003114, + "heading": -0.1126782173092116, + "angularVelocity": 0.6951627036802206, + "velocityX": -1.2598836133403888, + "velocityY": 1.2263416460657963, + "timestamp": 2.2045772698294948 + }, + { + "x": 1.4123948378953157, + "y": 5.471790254952654, + "heading": -0.06761272810355828, + "angularVelocity": 0.5565411578785837, + "velocityX": -1.0078985310085185, + "velocityY": 0.9810785528472173, + "timestamp": 2.285551495307172 + }, + { + "x": 1.3511847156576737, + "y": 5.531372091014257, + "heading": -0.03380194743073204, + "angularVelocity": 0.417549911387026, + "velocityX": -0.7559210585414546, + "velocityY": 0.7358123614043836, + "timestamp": 2.3665257207848494 + }, + { + "x": 1.3103779403872662, + "y": 5.571093500047757, + "heading": -0.011263323026729973, + "angularVelocity": 0.27834319218308196, + "velocityX": -0.50394770718331, + "velocityY": 0.4905438588537352, + "timestamp": 2.4474999462625266 }, { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -7.6740532227992184e-31, - "angularVelocity": -3.211824120166074e-30, - "velocityX": 1.9649506717636078e-19, - "velocityY": 1.3301278158953467e-20, - "timestamp": 3.5022642335231207 - }, - { - "x": 7.601080260023373, - "y": 4.024833528540064, - "heading": -2.306467806437822e-19, - "angularVelocity": -2.8978205760100555e-18, - "velocityX": 0.035841753611820425, - "velocityY": 0.308668790189073, - "timestamp": 3.5818574124379605 - }, - { - "x": 7.6061588306375265, - "y": 4.074034103309106, - "heading": -6.659912056470629e-19, - "angularVelocity": -5.469619135125979e-18, - "velocityX": 0.06380660608606212, - "velocityY": 0.6181506435581036, - "timestamp": 3.6614505913528004 - }, - { - "x": 7.612681282533822, - "y": 4.147925498197174, - "heading": -1.328067367399e-18, - "angularVelocity": -8.318252856515268e-18, - "velocityX": 0.08194737269231386, - "velocityY": 0.9283634087178491, - "timestamp": 3.7410437702676402 - }, - { - "x": 7.619648669627935, - "y": 4.2465458921824055, - "heading": -2.2445697547086026e-18, - "angularVelocity": -1.151483703155595e-17, - "velocityX": 0.0875374898843103, - "velocityY": 1.2390558503857954, - "timestamp": 3.82063694918248 - }, - { - "x": 7.625747861911206, - "y": 4.369884044272492, - "heading": -3.6731106086224716e-18, - "angularVelocity": -1.7948034238446692e-17, - "velocityX": 0.07662958517843273, - "velocityY": 1.5496070614550481, - "timestamp": 3.90023012809732 - }, - { - "x": 7.629194369652072, - "y": 4.517812530748991, - "heading": -5.438708340419596e-18, - "angularVelocity": -2.2182783637629958e-17, - "velocityX": 0.04330154653780812, - "velocityY": 1.8585573348537896, - "timestamp": 3.97982330701216 - }, - { - "x": 7.627474126240345, - "y": 4.689928325847814, - "heading": -7.604279409389531e-18, - "angularVelocity": -2.7208006214780384e-17, - "velocityX": -0.021612950194643966, - "velocityY": 2.1624440366049984, - "timestamp": 4.059416485927 - }, - { - "x": 7.616940876905995, - "y": 4.885153687224004, - "heading": -1.141400124068985e-17, - "angularVelocity": -4.7864941206348986e-17, - "velocityX": -0.1323385933059981, - "velocityY": 2.452790101336185, - "timestamp": 4.139009664841839 - }, - { - "x": 7.592385072354184, - "y": 5.10075278062736, - "heading": -1.6698122492004076e-17, - "angularVelocity": -6.638915655253322e-17, - "velocityX": -0.3085164443310098, - "velocityY": 2.708763443586594, - "timestamp": 4.2186028437566785 - }, - { - "x": 7.547559201834021, - "y": 5.330523864977108, - "heading": -1.75098708974119e-17, - "angularVelocity": -1.0198737275031467e-17, - "velocityX": -0.5631873375496935, - "velocityY": 2.88681878877579, - "timestamp": 4.298196022671518 - }, - { - "x": 7.478361687755516, - "y": 5.564507297546682, - "heading": -1.8964828080994495e-17, - "angularVelocity": -1.827994481974364e-17, - "velocityX": -0.8693900032884319, - "velocityY": 2.93974227138131, - "timestamp": 4.377789201586357 - }, - { - "x": 7.384986158389392, - "y": 5.79328068583031, - "heading": -2.0967248801545294e-17, - "angularVelocity": -2.515821870147438e-17, - "velocityX": -1.173159944598191, - "velocityY": 2.8742838444534518, - "timestamp": 4.457382380501197 - }, - { - "x": 7.269611510863885, - "y": 6.010750981441168, - "heading": -2.341183721366507e-17, - "angularVelocity": -3.071356740399464e-17, - "velocityX": -1.4495544605518746, - "velocityY": 2.732273023590656, - "timestamp": 4.536975559416036 - }, - { - "x": 7.134451807687368, - "y": 6.213379687795212, - "heading": -1.8377494488650744e-17, - "angularVelocity": 6.325092910440118e-17, - "velocityX": -1.6981317371571643, - "velocityY": 2.545804918419267, - "timestamp": 4.6165687383308756 - }, - { - "x": 6.981289024355567, - "y": 6.399049314780469, - "heading": -1.3636102854748774e-17, - "angularVelocity": 5.957033241098293e-17, - "velocityX": -1.9243204684119384, - "velocityY": 2.332732898932091, - "timestamp": 4.696161917245715 - }, - { - "x": 6.8114962888064765, - "y": 6.566411402116164, - "heading": -1.0217857426728479e-17, - "angularVelocity": 4.294646070658522e-17, - "velocityX": -2.1332573703427147, - "velocityY": 2.1027189718699257, - "timestamp": 4.775755096160554 - }, - { - "x": 6.626136187340535, - "y": 6.714554267951228, - "heading": 1.5840075957918408e-18, - "angularVelocity": 1.4827738452159993e-16, - "velocityX": -2.328844054140172, - "velocityY": 1.8612507736817756, - "timestamp": 4.855348275075394 - }, - { - "x": 6.426044940948486, - "y": 6.842829704284668, - "heading": 1.3036775744196506e-17, - "angularVelocity": 1.4389137918114227e-16, - "velocityX": -2.513924548812549, - "velocityY": 1.6116385610211605, - "timestamp": 4.934941453990233 - }, - { - "x": 6.196280073025626, - "y": 6.956629849882612, - "heading": 2.0458443333295487e-17, - "angularVelocity": 8.729748510308141e-17, - "velocityX": -2.702613571061956, - "velocityY": 1.338576348343764, - "timestamp": 5.019957258833017 - }, - { - "x": 5.950838280027335, - "y": 7.046967618713607, - "heading": 2.793959259584985e-17, - "angularVelocity": 8.799713979563742e-17, - "velocityX": -2.887013696478903, - "velocityY": 1.0625997012916502, - "timestamp": 5.104973063675801 - }, - { - "x": 5.6901710071441105, - "y": 7.113547571189235, - "heading": 3.9506385392959274e-17, - "angularVelocity": 1.360545935414649e-16, - "velocityX": -3.0661036893700855, - "velocityY": 0.783147940536072, - "timestamp": 5.189988868518585 - }, - { - "x": 5.414850837059096, - "y": 7.156012349089107, - "heading": 4.700202282082566e-17, - "angularVelocity": 8.816755094375023e-17, - "velocityX": -3.238458667704845, - "velocityY": 0.4994927470064634, - "timestamp": 5.2750046733613685 - }, - { - "x": 5.12562598325124, - "y": 7.173922926375987, - "heading": 5.0881573025403224e-17, - "angularVelocity": 4.563326938276916e-17, - "velocityX": -3.402012770951351, - "velocityY": 0.21067350147440403, - "timestamp": 5.360020478204152 - }, - { - "x": 4.82351161445262, - "y": 7.166731524808041, - "heading": 5.499719430693057e-17, - "angularVelocity": 4.841005753815757e-17, - "velocityX": -3.5536259329345636, - "velocityY": -0.0845889959076519, - "timestamp": 5.445036283046936 - }, - { - "x": 4.5099531855637025, - "y": 7.1337470554057925, - "heading": 6.306907946999819e-17, - "angularVelocity": 9.494568681598964e-17, - "velocityX": -3.688236904523418, - "velocityY": -0.387980440380867, - "timestamp": 5.53005208788972 - }, - { - "x": 4.187141754472217, - "y": 7.074104928642276, - "heading": 6.708348504544881e-17, - "angularVelocity": 4.721950773543392e-17, - "velocityX": -3.797075516587084, - "velocityY": -0.7015416353912106, - "timestamp": 5.615067892732504 - }, - { - "x": 3.8586724966260957, - "y": 6.986817954786897, - "heading": 7.185623411631233e-17, - "angularVelocity": 5.613953431429229e-17, - "velocityX": -3.8636258099718415, - "velocityY": -1.0267146681348074, - "timestamp": 5.7000836975752875 - }, - { - "x": 3.5309501359202713, - "y": 6.871322995246275, - "heading": 7.621484395675117e-17, - "angularVelocity": 5.126820454622351e-17, - "velocityX": -3.8548404183416, - "velocityY": -1.3585116291517334, - "timestamp": 5.785099502418071 - }, - { - "x": 3.2151594653612365, - "y": 6.73025690445887, - "heading": 7.966221541391077e-17, - "angularVelocity": 4.054976338417916e-17, - "velocityX": -3.714493689061746, - "velocityY": -1.6592925403490721, - "timestamp": 5.870115307260855 - }, - { - "x": 2.9234333026279216, - "y": 6.574454892414151, - "heading": 7.863320324730858e-17, - "angularVelocity": -1.2103771141102572e-17, - "velocityX": -3.4314344641303904, - "velocityY": -1.83262409069529, - "timestamp": 5.955131112103639 - }, - { - "x": 2.659851278869089, - "y": 6.416614338182214, - "heading": 7.578110084515079e-17, - "angularVelocity": -3.354789840476959e-17, - "velocityX": -3.1003885012470134, - "velocityY": -1.856602481430881, - "timestamp": 6.040146916946423 - }, - { - "x": 2.4239206377137292, - "y": 6.264399576901125, - "heading": 7.251198491757422e-17, - "angularVelocity": -3.845302447267169e-17, - "velocityX": -2.775138594425478, - "velocityY": -1.7904289862642684, - "timestamp": 6.125162721789207 - }, - { - "x": 2.2144464260119667, - "y": 6.121998379809068, - "heading": 6.761005309686019e-17, - "angularVelocity": -5.765904670925165e-17, - "velocityX": -2.4639443464556736, - "velocityY": -1.6749967533143706, - "timestamp": 6.21017852663199 - }, - { - "x": 2.030377404257279, - "y": 5.9918782719805765, - "heading": 5.994279183003849e-17, - "angularVelocity": -9.018628912533282e-17, - "velocityX": -2.1651153229105353, - "velocityY": -1.530540210365924, - "timestamp": 6.295194331474774 - }, - { - "x": 1.8708849952976385, - "y": 5.875623192871665, - "heading": 4.994168872232487e-17, - "angularVelocity": -1.176381435863843e-16, - "velocityX": -1.8760324536664574, - "velocityY": -1.3674525498394738, - "timestamp": 6.380210136317558 - }, - { - "x": 1.735321954953519, - "y": 5.774322521108853, - "heading": 3.80276915684229e-17, - "angularVelocity": -1.4013859419532884e-16, - "velocityX": -1.5945628062312347, - "velocityY": -1.1915510527736544, - "timestamp": 6.465225941160342 - }, - { - "x": 1.6231760146266, - "y": 5.6887663231669565, - "heading": 2.9027956357036794e-17, - "angularVelocity": -1.0585954002347058e-16, - "velocityX": -1.3191187277977714, - "velocityY": -1.0063563839702132, - "timestamp": 6.550241746003126 - }, - { - "x": 1.5340345289934116, - "y": 5.619551610175038, - "heading": 2.339141109649791e-17, - "angularVelocity": -6.629995787223342e-17, - "velocityX": -1.048528397725959, - "velocityY": -0.8141393605567464, - "timestamp": 6.6352575508459095 - }, - { - "x": 1.467559291615285, - "y": 5.567144344510584, - "heading": 1.436014992750064e-17, - "angularVelocity": -1.0623036243813761e-16, - "velocityX": -0.7819162272362828, - "velocityY": -0.6164414459331455, - "timestamp": 6.720273355688693 - }, - { - "x": 1.4234687066815632, - "y": 5.531917778907625, - "heading": 5.2334973544914086e-18, - "angularVelocity": -1.0735240881555591e-16, - "velocityX": -0.5186163327543208, - "velocityY": -0.414353139020498, - "timestamp": 6.805289160531477 - }, - { - "x": 1.4015250205993652, - "y": 5.514177322387695, - "heading": 1.924648421034324e-30, - "angularVelocity": -6.15591051597521e-17, - "velocityX": -0.2581130193706609, - "velocityY": -0.20867245276024773, - "timestamp": 6.890304965374261 - }, - { - "x": 1.4015250205993652, - "y": 5.514177322387695, - "heading": 1.0779182942243083e-30, - "angularVelocity": 2.7193521636113197e-30, - "velocityX": -1.8832789283152474e-18, - "velocityY": 1.3879124381036463e-18, - "timestamp": 6.975320770217045 + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 4.4390879396333275e-31, + "angularVelocity": 0.13909763212046558, + "velocityX": -0.25197510938629825, + "velocityY": 0.24527315422336363, + "timestamp": 2.528474171740204 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 2.494908675164405e-31, + "angularVelocity": -2.400971097599731e-30, + "velocityX": 9.117669624782265e-34, + "velocityY": -1.69916768015881e-32, + "timestamp": 2.6094483972178812 + }, + { + "x": 1.3242823472826057, + "y": 5.590457081039532, + "heading": -3.1525835471969304e-19, + "angularVelocity": -3.554932323947037e-18, + "velocityX": 0.3868644475376546, + "velocityY": -0.00560680796813751, + "timestamp": 2.6981303540012136 + }, + { + "x": 1.3928981385918453, + "y": 5.589462635652003, + "heading": -9.247304393859629e-19, + "angularVelocity": -6.8725601776619685e-18, + "velocityX": 0.7737288823800034, + "velocityY": -0.011213615752282564, + "timestamp": 2.786812310784546 + }, + { + "x": 1.4958218231349187, + "y": 5.587970967605794, + "heading": -1.5461935946313141e-18, + "angularVelocity": -7.007774498748477e-18, + "velocityX": 1.1605932962726162, + "velocityY": -0.016820423232804093, + "timestamp": 2.8754942675678783 + }, + { + "x": 1.6330533972641115, + "y": 5.585982076953771, + "heading": -2.4826620193785056e-18, + "angularVelocity": -1.0559852970262818e-17, + "velocityX": 1.5474576690326838, + "velocityY": -0.022427230117193615, + "timestamp": 2.9641762243512106 + }, + { + "x": 1.8045928505515578, + "y": 5.5834959638470645, + "heading": -3.548549667861495e-18, + "angularVelocity": -1.201921661569969e-17, + "velocityX": 1.934321924205522, + "velocityY": -0.028034035297397054, + "timestamp": 3.052858181134543 + }, + { + "x": 2.0104398981448357, + "y": 5.5805126324140275, + "heading": -4.504226548304647e-18, + "angularVelocity": -1.0776452337176765e-17, + "velocityX": 2.321182967310963, + "velocityY": -0.033640793925260974, + "timestamp": 3.1415401379178753 + }, + { + "x": 2.1819790701409207, + "y": 5.578026523384064, + "heading": -4.574305172786567e-18, + "angularVelocity": -7.902241563425968e-19, + "velocityX": 1.934318752293539, + "velocityY": -0.02803398932702808, + "timestamp": 3.2302220947012077 + }, + { + "x": 2.31921035343729, + "y": 5.576037636947067, + "heading": -3.929985868072649e-18, + "angularVelocity": 7.2655061760547e-18, + "velocityX": 1.547454389528782, + "velocityY": -0.022427182587500143, + "timestamp": 3.31890405148454 + }, + { + "x": 2.422133743893467, + "y": 5.574545973163046, + "heading": -2.934544233389305e-18, + "angularVelocity": 1.1224849685211679e-17, + "velocityX": 1.1605899800749648, + "velocityY": -0.016820375171309878, + "timestamp": 3.4075860082678724 + }, + { + "x": 2.490749239479307, + "y": 5.573551532061422, + "heading": -1.705878005512682e-18, + "angularVelocity": 1.385474872728055e-17, + "velocityX": 0.7737255477287375, + "velocityY": -0.011213567423341012, + "timestamp": 3.4962679650512047 + }, + { + "x": 2.525056838989258, + "y": 5.573054313659668, + "heading": 0, + "angularVelocity": 1.9235908491289617e-17, + "velocityX": 0.3868611017883933, + "velocityY": -0.005606759478353257, + "timestamp": 3.584949921834537 + }, + { + "x": 2.5176787496862993, + "y": 5.573161243955685, + "heading": 2.6679509724403924e-18, + "angularVelocity": 2.5453436150449368e-17, + "velocityX": -0.07039024589473145, + "velocityY": 0.0010201624731186972, + "timestamp": 3.689766849367258 + }, + { + "x": 2.4623729801708616, + "y": 5.573962787832815, + "heading": 6.066089004278022e-18, + "angularVelocity": 3.2419744709429903e-17, + "velocityX": -0.5276415824931802, + "velocityY": 0.007647084263940974, + "timestamp": 3.794583776899979 + }, + { + "x": 2.3591395323793796, + "y": 5.575458945262994, + "heading": 1.0152873197492895e-17, + "angularVelocity": 3.898973466799139e-17, + "velocityX": -0.9848929006171766, + "velocityY": 0.014274005787013901, + "timestamp": 3.8994007044326997 + }, + { + "x": 2.207978410184721, + "y": 5.577649716190093, + "heading": 1.5606892400662525e-17, + "angularVelocity": 5.2033763358184983e-17, + "velocityX": -1.4421441817922973, + "velocityY": 0.020900926774588543, + "timestamp": 4.004217631965421 + }, + { + "x": 2.0088896252054647, + "y": 5.580535100445723, + "heading": 1.7204191476600093e-18, + "angularVelocity": -1.324831168006478e-16, + "velocityX": -1.8993953521210223, + "velocityY": 0.02752784615567168, + "timestamp": 4.109034559498142 + }, + { + "x": 1.769251248155021, + "y": 5.584008168003288, + "heading": 6.282149498472273e-20, + "angularVelocity": -1.5814217146918653e-17, + "velocityX": -2.2862564539075523, + "velocityY": 0.03313460563399779, + "timestamp": 4.213851487030864 + }, + { + "x": 1.577540533889042, + "y": 5.586786622232322, + "heading": -2.476430565711122e-19, + "angularVelocity": -2.961969587010803e-18, + "velocityX": -1.829005283580098, + "velocityY": 0.02650768625293313, + "timestamp": 4.318668414563585 + }, + { + "x": 1.4337574940262061, + "y": 5.588870462964437, + "heading": -3.350775678748249e-19, + "angularVelocity": -8.341640359227092e-19, + "velocityX": -1.3717540024052963, + "velocityY": 0.01988076526536314, + "timestamp": 4.423485342096306 + }, + { + "x": 1.3379021324393994, + "y": 5.590259690143503, + "heading": -2.0793104080618583e-19, + "angularVelocity": 1.2130342880824037e-18, + "velocityX": -0.914502684281442, + "velocityY": 0.013253843742292293, + "timestamp": 4.528302269629028 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -1.4049323971929142e-31, + "angularVelocity": 1.9837543963605952e-18, + "velocityX": -0.4572513476830733, + "velocityY": 0.006626921951471195, + "timestamp": 4.633119197161749 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 4.571690374498227e-31, + "angularVelocity": 5.701965460392164e-30, + "velocityX": 1.9882633246635272e-32, + "velocityY": -1.2353765948799823e-31, + "timestamp": 4.73793612469447 + }, + { + "x": 1.3072266031673745, + "y": 5.605560947932051, + "heading": 0.009988018032587274, + "angularVelocity": 0.1383542475734168, + "velocityX": 0.2389771940088317, + "velocityY": 0.20233155967170455, + "timestamp": 4.810127750233767 + }, + { + "x": 1.3417310637547577, + "y": 5.63477381293564, + "heading": 0.029960721397497427, + "angularVelocity": 0.2766623305087681, + "velocityX": 0.4779565542351512, + "velocityY": 0.40465725470744984, + "timestamp": 4.882319375773064 + }, + { + "x": 1.393488225984334, + "y": 5.678592353413093, + "heading": 0.05990698047800166, + "angularVelocity": 0.4148162457460571, + "velocityX": 0.7169413604826331, + "velocityY": 0.606975395693467, + "timestamp": 4.954511001312361 + }, + { + "x": 1.4624987017850304, + "y": 5.737015858733095, + "heading": 0.09981017735525757, + "angularVelocity": 0.5527399692025371, + "velocityX": 0.9559346431831991, + "velocityY": 0.8092836929987566, + "timestamp": 5.026702626851658 + }, + { + "x": 1.54876329298479, + "y": 5.810043407637518, + "heading": 0.1496511403376383, + "angularVelocity": 0.6903981259608908, + "velocityX": 1.1949390328222467, + "velocityY": 1.0115792290159562, + "timestamp": 5.098894252390955 + }, + { + "x": 1.6522829608961203, + "y": 5.897673825591776, + "heading": 0.20941128647918397, + "angularVelocity": 0.8277988713391718, + "velocityX": 1.4339567385834835, + "velocityY": 1.2138584953540306, + "timestamp": 5.171085877930252 + }, + { + "x": 1.7730588392541233, + "y": 5.999905624129209, + "heading": 0.2790750774652724, + "angularVelocity": 0.9649843796382769, + "velocityX": 1.6729901488685341, + "velocityY": 1.4161171434188378, + "timestamp": 5.243277503469549 + }, + { + "x": 1.911092570994838, + "y": 6.1167366962116585, + "heading": 0.35862892849655154, + "angularVelocity": 1.1019817109951129, + "velocityX": 1.9120463171400826, + "velocityY": 1.6183466047443176, + "timestamp": 5.315469129008846 + }, + { + "x": 2.0662405750494774, + "y": 6.247971693585017, + "heading": 0.4478813931179114, + "angularVelocity": 1.2363271218036194, + "velocityX": 2.1491135972568896, + "velocityY": 1.8178700977147564, + "timestamp": 5.3876607545481425 + }, + { + "x": 2.204132997305319, + "y": 6.36460684022272, + "heading": 0.5272152358459444, + "angularVelocity": 1.0989341510922093, + "velocityX": 1.9100888950161639, + "velocityY": 1.6156326411317472, + "timestamp": 5.4598523800874394 + }, + { + "x": 2.3247684211138937, + "y": 6.466643771235888, + "heading": 0.5966353508762373, + "angularVelocity": 0.9616089748867341, + "velocityX": 1.6710445693293852, + "velocityY": 1.4134178341442696, + "timestamp": 5.532044005626736 + }, + { + "x": 2.428146136331826, + "y": 6.5540833745423095, + "heading": 0.6561424196084651, + "angularVelocity": 0.8242932374446845, + "velocityX": 1.4319904067208047, + "velocityY": 1.2112153266137329, + "timestamp": 5.604235631166033 + }, + { + "x": 2.51426563036811, + "y": 6.626926253781894, + "heading": 0.705736875442574, + "angularVelocity": 0.686983503468318, + "velocityX": 1.192929143690227, + "velocityY": 1.0090211807166112, + "timestamp": 5.67642725670533 + }, + { + "x": 2.5831264968268224, + "y": 6.6851728424603465, + "heading": 0.7454193798278042, + "angularVelocity": 0.5496829319015093, + "velocityX": 0.95386225125429, + "velocityY": 0.8068330397501333, + "timestamp": 5.748618882244627 + }, + { + "x": 2.6347284176402783, + "y": 6.72882344947858, + "heading": 0.7751907373365541, + "angularVelocity": 0.4123935052894287, + "velocityX": 0.7147909529377248, + "velocityY": 0.6046491776859777, + "timestamp": 5.820810507783924 + }, + { + "x": 2.669071164543137, + "y": 6.75787827866861, + "heading": 0.7950517689437672, + "angularVelocity": 0.27511545084049754, + "velocityX": 0.4757164926845912, + "velocityY": 0.4024681391074233, + "timestamp": 5.893002133323221 + }, + { + "x": 2.68615460395813, + "y": 6.772337436676025, + "heading": 0.8050032485420815, + "angularVelocity": 0.13784811636956903, + "velocityX": 0.23664018211616192, + "velocityY": 0.20028857778593961, + "timestamp": 5.965193758862518 + }, + { + "x": 2.6837128162746895, + "y": 6.770286154899203, + "heading": 0.8037616945531193, + "angularVelocity": -0.015403237566514301, + "velocityX": -0.03029383829428867, + "velocityY": -0.025449058845075586, + "timestamp": 6.045797202361987 + }, + { + "x": 2.6597554401054357, + "y": 6.750039490992799, + "heading": 0.790165886718463, + "angularVelocity": -0.1686752729700538, + "velocityX": -0.29722521928375617, + "velocityY": -0.2511885724392666, + "timestamp": 6.126400645861456 + }, + { + "x": 2.614282818196885, + "y": 6.71159714288196, + "heading": 0.7642144593793307, + "angularVelocity": -0.32196425130707784, + "velocityX": -0.5641523480167241, + "velocityY": -0.4769318337003116, + "timestamp": 6.207004089360925 + }, + { + "x": 2.547295402718566, + "y": 6.65495864191724, + "heading": 0.7259067765041173, + "angularVelocity": -0.4752611205179619, + "velocityX": -0.8310738669455855, + "velocityY": -0.702680909222201, + "timestamp": 6.287607532860394 + }, + { + "x": 2.45879373609576, + "y": 6.580123315301835, + "heading": 0.6752436075608932, + "angularVelocity": -0.6285484433864407, + "velocityX": -1.0979886563197567, + "velocityY": -0.9284383317423857, + "timestamp": 6.3682109763598636 + }, + { + "x": 2.3487784541804713, + "y": 6.487090205965796, + "heading": 0.612227929672179, + "angularVelocity": -0.7817988308289465, + "velocityX": -1.3648955570482664, + "velocityY": -1.1542076280722422, + "timestamp": 6.448814419859333 + }, + { + "x": 2.2172503640031413, + "y": 6.3758579034961445, + "heading": 0.536865388228733, + "angularVelocity": -0.934979179194793, + "velocityX": -1.6317924454199069, + "velocityY": -1.3799944225748537, + "timestamp": 6.529417863358802 + }, + { + "x": 2.0642108270205712, + "y": 6.2464240090225065, + "heading": 0.4491644340858314, + "angularVelocity": -1.0880546827189126, + "velocityX": -1.8986724429895898, + "velocityY": -1.605810978465707, + "timestamp": 6.610021306858271 + }, + { + "x": 1.892150582189426, + "y": 6.100772712319997, + "heading": 0.34952884105977006, + "angularVelocity": -1.236120799562961, + "velocityX": -2.134651292315194, + "velocityY": -1.8070108469188693, + "timestamp": 6.69062475035774 + }, + { + "x": 1.7416020056028705, + "y": 5.973323101720471, + "heading": 0.2622670318762804, + "angularVelocity": -1.0826064668575972, + "velocityX": -1.8677685474764083, + "velocityY": -1.5811931235965593, + "timestamp": 6.771228193857209 + }, + { + "x": 1.6125631547118695, + "y": 5.864077706390992, + "heading": 0.18740079602910192, + "angularVelocity": -0.9288218045882161, + "velocityX": -1.600909902712647, + "velocityY": -1.3553440223708513, + "timestamp": 6.851831637356678 + }, + { + "x": 1.5050325714338102, + "y": 5.7730380802152625, + "heading": 0.12496122653203612, + "angularVelocity": -0.7746513893974771, + "velocityX": -1.3340693475310352, + "velocityY": -1.1294756430146329, + "timestamp": 6.9324350808561475 + }, + { + "x": 1.4190090841759289, + "y": 5.700205295684954, + "heading": 0.0749803520329015, + "angularVelocity": -0.6200836134188114, + "velocityX": -1.0672433276187945, + "velocityY": -0.9035939578786114, + "timestamp": 7.013038524355617 + }, + { + "x": 1.3544918382019193, + "y": 5.645580092417221, + "heading": 0.037484747068995454, + "angularVelocity": -0.4651861426263309, + "velocityX": -0.8004279119223182, + "velocityY": -0.6777030967431115, + "timestamp": 7.093641967855086 + }, + { + "x": 1.3114803343182175, + "y": 5.609162978300701, + "heading": 0.012490529490997385, + "angularVelocity": -0.31008870704351826, + "velocityX": -0.5336186894297898, + "velocityY": -0.45180593452897394, + "timestamp": 7.174245411354555 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.0757846254441862e-30, + "angularVelocity": -0.1549627280016075, + "velocityX": -0.2668109738177861, + "velocityY": -0.22590442503082891, + "timestamp": 7.254848854854024 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 2.1112287936414426e-30, + "angularVelocity": 1.2846097126015132e-29, + "velocityX": 1.1473496118122496e-30, + "velocityY": -2.480854756847108e-31, + "timestamp": 7.335452298353493 + }, + { + "x": 1.3107332172473851, + "y": 5.587721554520521, + "heading": -5.6112723310966165e-9, + "angularVelocity": -8.086181485047517e-8, + "velocityX": 0.2991463288589595, + "velocityY": -0.04658586416314624, + "timestamp": 7.40484564929817 + }, + { + "x": 1.3522507006814268, + "y": 5.581255743814154, + "heading": -1.6136427820687844e-8, + "angularVelocity": -1.516738315771432e-7, + "velocityX": 0.5982919525984769, + "velocityY": -0.09317622824587016, + "timestamp": 7.4742390002428465 + }, + { + "x": 1.414526842614822, + "y": 5.571556496717738, + "heading": -3.074141680025046e-8, + "angularVelocity": -2.1046669010163338e-7, + "velocityX": 0.8974367296809845, + "velocityY": -0.13977199493000497, + "timestamp": 7.543632351187523 + }, + { + "x": 1.4975615711863124, + "y": 5.558623354737258, + "heading": -4.841098342837698e-8, + "angularVelocity": -2.546291003036043e-7, + "velocityX": 1.196580471199459, + "velocityY": -0.1863743687891775, + "timestamp": 7.6130257021322 + }, + { + "x": 1.6013547964876313, + "y": 5.542455744326475, + "heading": -6.788238796631325e-8, + "angularVelocity": -2.8059467012358587e-7, + "velocityX": 1.4957229170856399, + "velocityY": -0.2329850077952488, + "timestamp": 7.682419053076877 + }, + { + "x": 1.7259064027890496, + "y": 5.5230529273691795, + "heading": -8.754228960685774e-8, + "angularVelocity": -2.8331102694259857e-7, + "velocityX": 1.7948636952366863, + "velocityY": -0.27960628350061884, + "timestamp": 7.751812404021553 + }, + { + "x": 1.8712162355508042, + "y": 5.500413918486819, + "heading": -1.0525703637468966e-7, + "angularVelocity": -2.552801733617599e-7, + "velocityX": 2.0940022463766295, + "velocityY": -0.3262417591047122, + "timestamp": 7.82120575496623 + }, + { + "x": 2.0372840779688866, + "y": 5.474537335801765, + "heading": -1.1807218385471473e-7, + "angularVelocity": -1.8467399582926014e-7, + "velocityX": 2.393137673240176, + "velocityY": -0.3728971483980547, + "timestamp": 7.890599105910907 + }, + { + "x": 2.2241096038561166, + "y": 5.445421101366719, + "heading": -1.2162497814922265e-7, + "angularVelocity": -5.119790624873975e-8, + "velocityX": 2.692268399549916, + "velocityY": -0.41958248216406707, + "timestamp": 7.959992456855583 + }, + { + "x": 2.431692266948392, + "y": 5.413061738269229, + "heading": -1.0881824696224333e-7, + "angularVelocity": 1.845527118847122e-7, + "velocityX": 2.99139125386478, + "velocityY": -0.46631792033351455, + "timestamp": 8.02938580780026 + }, + { + "x": 2.660030964874599, + "y": 5.377452241496065, + "heading": -6.605232487729304e-8, + "angularVelocity": 6.162827019703426e-7, + "velocityX": 3.290498222347668, + "velocityY": -0.5131543049644982, + "timestamp": 8.098779158744936 + }, + { + "x": 2.9091223110729234, + "y": 5.338571186157936, + "heading": 4.364254321616404e-8, + "angularVelocity": 0.0000015807691363697316, + "velocityX": 3.5895563884342283, + "velocityY": -0.5602994351597471, + "timestamp": 8.168172509689612 + }, + { + "x": 3.168557402823767, + "y": 5.298953853406429, + "heading": 4.3641894183803e-8, + "angularVelocity": -9.352947371519385e-12, + "velocityX": 3.738615994458649, + "velocityY": -0.5709096363289035, + "timestamp": 8.237565860634287 + }, + { + "x": 3.4266802039415243, + "y": 5.251533194096367, + "heading": 4.3642354317485314e-8, + "angularVelocity": 6.630803609714539e-12, + "velocityX": 3.719705095716507, + "velocityY": -0.6833602739240744, + "timestamp": 8.306959211578963 + }, + { + "x": 3.6803449026923616, + "y": 5.18422554387417, + "heading": 4.364233337760302e-8, + "angularVelocity": -3.0175631893268995e-13, + "velocityX": 3.6554611543845263, + "velocityY": -0.9699437958523844, + "timestamp": 8.376352562523639 + }, + { + "x": 3.9278131468583686, + "y": 5.096843813495861, + "heading": 4.364231130349348e-8, + "angularVelocity": -3.1810121673972006e-13, + "velocityX": 3.5661665101502296, + "velocityY": -1.2592233865168847, + "timestamp": 8.445745913468315 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 4.364228801520247e-8, + "angularVelocity": -3.3559830331814666e-13, + "velocityX": 3.45401850865928, + "velocityY": -1.5404360240963577, + "timestamp": 8.51513926441299 + }, + { + "x": 4.284862555065841, + "y": 4.931880235034004, + "heading": 4.364226510492993e-8, + "angularVelocity": -6.617055985872344e-13, + "velocityX": 3.3897491580338537, + "velocityY": -1.67713541924223, + "timestamp": 8.549762324762018 + }, + { + "x": 4.399813643173519, + "y": 4.869172357324464, + "heading": 4.364224321511573e-8, + "angularVelocity": -6.322322259809295e-13, + "velocityX": 3.3200730076683276, + "velocityY": -1.81115929895842, + "timestamp": 8.584385385111045 + }, + { + "x": 4.512168964860153, + "y": 4.801924216205689, + "heading": 4.364222211690104e-8, + "angularVelocity": -6.093688654191888e-13, + "velocityX": 3.245100824537311, + "velocityY": -1.9422933859936342, + "timestamp": 8.619008445460071 + }, + { + "x": 4.621749311428581, + "y": 4.730243126546711, + "heading": 4.364220167371938e-8, + "angularVelocity": -5.904498836065766e-13, + "velocityX": 3.164952649008305, + "velocityY": -2.070327952999433, + "timestamp": 8.653631505809098 + }, + { + "x": 4.728650036507715, + "y": 4.654623908914118, + "heading": 4.364241447304788e-8, + "angularVelocity": 6.146173210402922e-12, + "velocityX": 3.0875585231776617, + "velocityY": -2.184070872715801, + "timestamp": 8.688254566158125 + }, + { + "x": 4.8372727310201835, + "y": 4.581499759061361, + "heading": 4.3642159997006347e-8, + "angularVelocity": -7.349900240613306e-12, + "velocityX": 3.137293278452735, + "velocityY": -2.112007116517435, + "timestamp": 8.722877626507152 + }, + { + "x": 4.948612503037568, + "y": 4.5125832438429105, + "heading": 4.3642137671211746e-8, + "angularVelocity": -6.448244222387315e-13, + "velocityX": 3.2157692270698366, + "velocityY": -1.9904801748811223, + "timestamp": 8.75750068685618 + }, + { + "x": 5.062589095284458, + "y": 4.448121106677303, + "heading": 4.364211455765666e-8, + "angularVelocity": -6.675769025667281e-13, + "velocityX": 3.2919271461828314, + "velocityY": -1.8618266703110185, + "timestamp": 8.792123747205206 + }, + { + "x": 5.17904891222934, + "y": 4.388261758970167, + "heading": 4.3642090495999965e-8, + "angularVelocity": -6.94960432877984e-13, + "velocityX": 3.3636488447548265, + "velocityY": -1.728886675634883, + "timestamp": 8.826746807554233 + }, + { + "x": 5.297806179962019, + "y": 4.333100707972697, + "heading": 4.364206516158606e-8, + "angularVelocity": -7.317208234311228e-13, + "velocityX": 3.4300049312658345, + "velocityY": -1.5931881942671235, + "timestamp": 8.86136986790326 + }, + { + "x": 5.418671454426997, + "y": 4.282725959570945, + "heading": 4.364203830903825e-8, + "angularVelocity": -7.755683028589255e-13, + "velocityX": 3.4908894028015367, + "velocityY": -1.454947884269515, + "timestamp": 8.895992928252287 + }, + { + "x": 5.541451926168614, + "y": 4.237217881100571, + "heading": 4.364200955008555e-8, + "angularVelocity": -8.306300130132497e-13, + "velocityX": 3.546205058244276, + "velocityY": -1.3143863659542425, + "timestamp": 8.930615988601314 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 4.364197825891865e-8, + "angularVelocity": -9.037666511803314e-13, + "velocityX": 3.5958636064268057, + "velocityY": -1.171727921713515, + "timestamp": 8.965239048950341 + }, + { + "x": 5.80048142988227, + "y": 4.159069716430897, + "heading": 4.364194761543742e-8, + "angularVelocity": -8.296994367988738e-13, + "velocityX": 3.642511096804761, + "velocityY": -1.0174944855729342, + "timestamp": 9.00217227920995 + }, + { + "x": 5.936489765010674, + "y": 4.127254911109845, + "heading": 4.364191906311607e-8, + "angularVelocity": -7.730794282349154e-13, + "velocityX": 3.6825464269543904, + "velocityY": -0.8614141004570673, + "timestamp": 9.039105509469557 + }, + { + "x": 6.073729843507157, + "y": 4.101262406827453, + "heading": 4.3641892220156324e-8, + "angularVelocity": -7.267969466454947e-13, + "velocityX": 3.7158969722335238, + "velocityY": -0.7037701305758657, + "timestamp": 9.076038739729166 + }, + { + "x": 6.211952539114824, + "y": 4.081139381676687, + "heading": 4.3641866716107116e-8, + "angularVelocity": -6.905447558160659e-13, + "velocityX": 3.742502202923576, + "velocityY": -0.5448487719411382, + "timestamp": 9.112971969988774 + }, + { + "x": 6.350906942398405, + "y": 4.066922357709394, + "heading": 4.364184226449404e-8, + "angularVelocity": -6.620491141311732e-13, + "velocityX": 3.7623138378867345, + "velocityY": -0.38493854632702584, + "timestamp": 9.149905200248382 + }, + { + "x": 6.490340706014696, + "y": 4.058637143370339, + "heading": 4.364062068850958e-8, + "angularVelocity": -3.3075253340927824e-11, + "velocityX": 3.775292944489131, + "velocityY": -0.22432953415709472, + "timestamp": 9.18683843050799 + }, + { + "x": 6.625895301013595, + "y": 4.054865050461972, + "heading": 3.8021532173824235e-8, + "angularVelocity": -1.521418068678583e-7, + "velocityX": 3.6702610101003303, + "velocityY": -0.10213276450108619, + "timestamp": 9.223771660767598 + }, + { + "x": 6.755934035567496, + "y": 4.053327166371606, + "heading": 3.0189413996037156e-8, + "angularVelocity": -2.1206154817940447e-7, + "velocityX": 3.52091419136215, + "velocityY": -0.04163957713841639, + "timestamp": 9.260704891027206 + }, + { + "x": 6.880153762983055, + "y": 4.0530365385095966, + "heading": 2.1734826824788128e-8, + "angularVelocity": -2.2891544824669977e-7, + "velocityX": 3.3633594067565498, + "velocityY": -0.007869007394285612, + "timestamp": 9.297638121286814 + }, + { + "x": 6.998470106919647, + "y": 4.0534986388519805, + "heading": 1.3540571224920633e-8, + "angularVelocity": -2.2186673987685247e-7, + "velocityX": 3.2035200578159904, + "velocityY": 0.012511777040293628, + "timestamp": 9.334571351546423 + }, + { + "x": 7.110853220415867, + "y": 4.054422625435363, + "heading": 6.1666659449803414e-9, + "angularVelocity": -1.9965502711211782e-7, + "velocityX": 3.042872575896168, + "velocityY": 0.025017757095422304, + "timestamp": 9.37150458180603 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": -3.6086824530888626e-33, + "angularVelocity": -1.6696795107584883e-7, + "velocityX": 2.8819069972765123, + "velocityY": 0.03237357493636809, + "timestamp": 9.408437812065639 + }, + { + "x": 7.405345242374078, + "y": 4.058539790558069, + "heading": -6.074177156590073e-9, + "angularVelocity": -8.274407475492158e-8, + "velocityX": 2.561720622548375, + "velocityY": 0.03979751891783301, + "timestamp": 9.481847022104299 + }, + { + "x": 7.569888594263639, + "y": 4.061600743201228, + "heading": -8.336905285122374e-9, + "angularVelocity": -3.0823491082343837e-8, + "velocityX": 2.241453787650163, + "velocityY": 0.04169712004183413, + "timestamp": 9.555256232142959 + }, + { + "x": 7.71092123884848, + "y": 4.064554734769034, + "heading": -8.259320686300401e-9, + "angularVelocity": 1.056878269798436e-9, + "velocityX": 1.9211846103583805, + "velocityY": 0.04024006750993344, + "timestamp": 9.62866544218162 + }, + { + "x": 7.828444510796415, + "y": 4.067236236264996, + "heading": -6.843181194470064e-9, + "angularVelocity": 1.929103299991646e-8, + "velocityX": 1.600933614270509, + "velocityY": 0.03652813447454146, + "timestamp": 9.70207465222028 + }, + { + "x": 7.922460087151161, + "y": 4.069526407614099, + "heading": -4.8146064610859125e-9, + "angularVelocity": 2.7633790562623428e-8, + "velocityX": 1.2807054633231172, + "velocityY": 0.031197329979384493, + "timestamp": 9.77548386225894 + }, + { + "x": 7.992969626740666, + "y": 4.071335791937442, + "heading": -2.7240893137969417e-9, + "angularVelocity": 2.8477586783070736e-8, + "velocityX": 0.9604999093761202, + "velocityY": 0.024647919823550288, + "timestamp": 9.8488930722976 + }, + { + "x": 8.039974659720452, + "y": 4.072594619838684, + "heading": -1.0033424996203534e-9, + "angularVelocity": 2.3440475863128373e-8, + "velocityX": 0.6403151996190951, + "velocityY": 0.01714809218869153, + "timestamp": 9.92230228233626 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 1.0901032811452175e-35, + "angularVelocity": 1.3667801358727972e-8, + "velocityX": 0.32014923968219366, + "velocityY": 0.00888629686322534, + "timestamp": 9.99571149237492 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 2.780011652171749e-35, + "angularVelocity": 2.3020443776927775e-34, + "velocityX": -1.517814230631444e-35, + "velocityY": 4.908282159537256e-34, + "timestamp": 10.06912070241358 } ], "constraints": [ @@ -10191,13 +5184,19 @@ }, { "scope": [ - 3 + 2 ], "type": "StopPoint" }, { "scope": [ - 3 + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 ], "type": "StopPoint" } @@ -10207,3297 +5206,2001 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "StraightForward": { + "CenterLanePSubCSubBSubASubFSub": { "waypoints": [ { - "x": 0, - "y": 0, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 69 + "controlIntervalCount": 17 }, { - "x": 4, - "y": 0, - "heading": 0, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 69 + "controlIntervalCount": 17 }, { - "x": 0, - "y": 0, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 49 + "controlIntervalCount": 12 }, { - "x": 0, - "y": 2, + "x": 2.6583051681518555, + "y": 5.54433536529541, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 49 + "controlIntervalCount": 12 }, { - "x": 0, - "y": 0, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 58 + "controlIntervalCount": 18 }, { - "x": 2, - "y": 2, - "heading": 0, + "x": 2.7351951599121094, + "y": 6.870687484741211, + "heading": 0.8050032485420815, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 58 + "controlIntervalCount": 18 }, { - "x": 0, - "y": 0, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": -1.1376170994794325e-18, - "y": -7.908497077335199e-28, - "heading": -7.532778409384681e-27, - "angularVelocity": -8.199365209469061e-27, - "velocityX": -6.394191701786484e-18, - "velocityY": 1.7031182398553385e-25, - "timestamp": 0 + "controlIntervalCount": 17 }, { - "x": 0.0033613448640062186, - "y": -3.207353843920882e-24, - "heading": 1.8540534582969372e-19, - "angularVelocity": 1.8765611031481612e-18, - "velocityX": 0.03402149614781158, - "velocityY": -3.300241746816345e-23, - "timestamp": 0.09880061856781212 - }, - { - "x": 0.010084034581535006, - "y": -9.734755015964764e-24, - "heading": 6.933840485443198e-19, - "angularVelocity": 5.141453424525524e-18, - "velocityX": 0.06804299218951397, - "velocityY": -6.718410081091478e-23, - "timestamp": 0.19760123713562425 - }, - { - "x": 0.02016806914144748, - "y": -1.9685611056691934e-23, - "heading": 1.6696757328056013e-18, - "angularVelocity": 9.881433945283818e-18, - "velocityX": 0.10206448811847535, - "velocityY": -1.0242470180298394e-22, - "timestamp": 0.29640185570343636 - }, - { - "x": 0.033613448531886116, - "y": -3.317040886768565e-23, - "heading": 2.0919453510145205e-18, - "angularVelocity": 4.27395869496419e-18, - "velocityX": 0.1360859839274221, - "velocityY": -1.388002418111676e-22, - "timestamp": 0.3952024742712485 - }, - { - "x": 0.050420172740202905, - "y": -5.030578939773999e-23, - "heading": 2.1256769428547757e-18, - "angularVelocity": 3.4141256618882077e-19, - "velocityX": 0.17010747960835326, - "velocityY": -1.7638794972644125e-22, - "timestamp": 0.49400309283906063 - }, - { - "x": 0.07058824175287753, - "y": -7.121786937435362e-23, - "heading": 1.9477652127659873e-18, - "angularVelocity": -1.800712610315598e-18, - "velocityX": 0.20412897515244016, - "velocityY": -2.1525580928579397e-22, - "timestamp": 0.5928037114068727 - }, - { - "x": 0.09411765555542399, - "y": -9.604275159196487e-23, - "heading": 1.7477397651697942e-18, - "angularVelocity": -2.0245340371444115e-18, - "velocityX": 0.23815047054990815, - "velocityY": -2.555345814319257e-22, - "timestamp": 0.6916043299746848 - }, - { - "x": 0.12100841413228318, - "y": -1.2492405496996858e-22, - "heading": 5.516927538791495e-19, - "angularVelocity": -1.210566046231088e-17, - "velocityX": 0.27217196578989666, - "velocityY": -2.972647097568963e-22, - "timestamp": 0.7904049485424969 - }, - { - "x": 0.1512605174666992, - "y": -1.5800976588220565e-22, - "heading": -2.4367433435029e-19, - "angularVelocity": -8.050220986716945e-18, - "velocityX": 0.30619346086029187, - "velocityY": -3.4057685817351433e-22, - "timestamp": 0.889205567110309 - }, - { - "x": 0.18487396554057559, - "y": -1.9547245627985932e-22, - "heading": -4.015998529188928e-19, - "angularVelocity": -1.598423665927227e-18, - "velocityX": 0.34021495574752625, - "velocityY": -3.856342021902462e-22, - "timestamp": 0.988006185678121 - }, - { - "x": 0.22184875833430798, - "y": -2.3748820340521924e-22, - "heading": -8.43069725371826e-19, - "angularVelocity": -4.468287727535479e-18, - "velocityX": 0.37423645043633647, - "velocityY": -4.325560385300792e-22, - "timestamp": 1.0868068042459331 - }, - { - "x": 0.2621848958265872, - "y": -2.842690964569586e-22, - "heading": -1.2892913052717086e-18, - "angularVelocity": -4.516381645740685e-18, - "velocityX": 0.4082579449094682, - "velocityY": -4.815024197737192e-22, - "timestamp": 1.1856074228137452 - }, - { - "x": 0.30588237799416684, - "y": -3.3601955996258784e-22, - "heading": -1.4361286105818133e-18, - "angularVelocity": -1.4861955425735956e-18, - "velocityX": 0.44227943914731443, - "velocityY": -5.325974076598438e-22, - "timestamp": 1.2844080413815573 - }, - { - "x": 0.35294120481158675, - "y": -3.9297468573778265e-22, - "heading": -2.1279601842953803e-18, - "angularVelocity": -7.002297360670298e-18, - "velocityX": 0.47630093312746685, - "velocityY": -5.861971021361979e-22, - "timestamp": 1.3832086599493694 - }, - { - "x": 0.4033613762508404, - "y": -4.553879204439828e-22, - "heading": -1.820906029131428e-18, - "angularVelocity": 3.10781837556822e-18, - "velocityX": 0.5103224268241561, - "velocityY": -6.423935446828857e-22, - "timestamp": 1.4820092785171814 - }, - { - "x": 0.4571428922809735, - "y": -5.235567689200076e-22, - "heading": -1.2874752262796756e-18, - "angularVelocity": 5.399065310541327e-18, - "velocityX": 0.5443439202075441, - "velocityY": -7.01517753563082e-22, - "timestamp": 1.5808098970849935 - }, - { - "x": 0.5142857528675923, - "y": -5.977742188885252e-22, - "heading": -1.2551861792254937e-18, - "angularVelocity": 3.268118073802405e-19, - "velocityX": 0.5783654132428195, - "velocityY": -7.638949232743287e-22, - "timestamp": 1.6796105156528056 - }, - { - "x": 0.5747899579722567, - "y": -6.783964811322378e-22, - "heading": -1.2211517140311946e-18, - "angularVelocity": 3.4447739303320164e-19, - "velocityX": 0.6123869058890279, - "velocityY": -8.298034991350254e-22, - "timestamp": 1.7784111342206177 - }, - { - "x": 0.6386555075517218, - "y": -7.658133530250519e-22, - "heading": -6.19635872521098e-19, - "angularVelocity": 6.0881793581750656e-18, - "velocityX": 0.6464083980975359, - "velocityY": -8.997961755349937e-22, - "timestamp": 1.8772117527884298 - }, - { - "x": 0.7058824015569765, - "y": -8.604791608615287e-22, - "heading": 1.304515627908251e-20, - "angularVelocity": 6.403613779661501e-18, - "velocityX": 0.6804298898099835, - "velocityY": -9.743947632554384e-22, - "timestamp": 1.9760123713562419 - }, - { - "x": 0.7764706399320078, - "y": -9.628899194740571e-22, - "heading": 1.4096290346844686e-18, - "angularVelocity": 1.4135374581608934e-17, - "velocityX": 0.714451380955505, - "velocityY": -1.0541932211126647e-21, - "timestamp": 2.074812989924054 - }, - { - "x": 0.8504202226121862, - "y": -1.0736383554782084e-21, - "heading": 2.060605710264541e-18, - "angularVelocity": 6.5887893423673715e-18, - "velocityX": 0.7484728714468809, - "velocityY": -1.1398796868710084e-21, - "timestamp": 2.1736136084918662 - }, - { - "x": 0.9277311495221099, - "y": -1.1933843266651194e-21, - "heading": 2.952299975389915e-18, - "angularVelocity": 9.025185671520631e-18, - "velocityX": 0.782494361175087, - "velocityY": -1.2326251827346421e-21, - "timestamp": 2.2724142270596785 - }, - { - "x": 1.0084034205726677, - "y": -1.3228608726632435e-21, - "heading": 4.072843680930929e-18, - "angularVelocity": 1.134145955927035e-17, - "velocityX": 0.8165158500013651, - "velocityY": -1.3332101260360877e-21, - "timestamp": 2.371214845627491 - }, - { - "x": 1.0924370356569266, - "y": -1.463114094622031e-21, - "heading": 5.6434310876510846e-18, - "angularVelocity": 1.5896527419660045e-17, - "velocityX": 0.8505373377453298, - "velocityY": -1.4438898247438702e-21, - "timestamp": 2.470015464195303 - }, - { - "x": 1.17983199464419, - "y": -1.6151196326348776e-21, - "heading": 7.018518492787492e-18, - "angularVelocity": 1.3917792832553574e-17, - "velocityX": 0.8845588241664668, - "velocityY": -1.5652186056968467e-21, - "timestamp": 2.5688160827631155 - }, - { - "x": 1.270588297371088, - "y": -1.7802259481782284e-21, - "heading": 7.979758659218145e-18, - "angularVelocity": 9.729079426406841e-18, - "velocityX": 0.9185803089340697, - "velocityY": -1.7002423732627545e-21, - "timestamp": 2.6676167013309278 - }, - { - "x": 1.3647059436275732, - "y": -1.960029507975355e-21, - "heading": 8.919068700069961e-18, - "angularVelocity": 9.507112968894936e-18, - "velocityX": 0.9526017915757001, - "velocityY": -1.851885594364366e-21, - "timestamp": 2.76641731989874 - }, - { - "x": 1.4621849331335823, - "y": -2.156646942783595e-21, - "heading": 9.966114158544211e-18, - "angularVelocity": 1.0597541833276519e-17, - "velocityX": 0.9866232713827009, - "velocityY": -2.0254826724633154e-21, - "timestamp": 2.8652179384665524 - }, - { - "x": 1.5630252654970274, - "y": -2.372489218605291e-21, - "heading": 1.1537140907678108e-17, - "angularVelocity": 1.5900958141426103e-17, - "velocityX": 1.0206447472212212, - "velocityY": -2.2237643073670865e-21, - "timestamp": 2.9640185570343647 - }, - { - "x": 1.6672269401297757, - "y": -2.6101775109831374e-21, - "heading": 1.3077485198475941e-17, - "angularVelocity": 1.5590402638929695e-17, - "velocityX": 1.054666217107023, - "velocityY": -2.4507675580642354e-21, - "timestamp": 3.062819175602177 - }, - { - "x": 1.7747899560516078, - "y": -2.8712245936259582e-21, - "heading": 1.4443190701622367e-17, - "angularVelocity": 1.3822805901452837e-17, - "velocityX": 1.0886876770716345, - "velocityY": -2.6936515218666954e-21, - "timestamp": 3.1616197941699893 - }, - { - "x": 1.8857143113020887, - "y": -3.1425844237227276e-21, - "heading": 1.6507544936655253e-17, - "angularVelocity": 2.0894091122735124e-17, - "velocityX": 1.122709117193911, - "velocityY": -2.8091008420292473e-21, - "timestamp": 3.2604204127378016 - }, - { - "x": 2, - "y": -3.164655249070787e-21, - "heading": 1.8228167907282414e-17, - "angularVelocity": 1.7415022670654974e-17, - "velocityX": 1.156730497790058, - "velocityY": -3.0799511941989887e-22, - "timestamp": 3.359221031305614 - }, - { - "x": 2.1142856886979113, - "y": -3.147499024006811e-21, - "heading": 1.9005172299965157e-17, - "angularVelocity": 7.864349059131218e-18, - "velocityX": 1.156730497790058, - "velocityY": 2.5626451006413984e-22, - "timestamp": 3.458021649873426 - }, - { - "x": 2.225210043948392, - "y": -2.8708197340377503e-21, - "heading": 1.976235400694375e-17, - "angularVelocity": 7.66372977455926e-18, - "velocityX": 1.122709117193911, - "velocityY": 2.8612962139149132e-21, - "timestamp": 3.5568222684412385 - }, - { - "x": 2.3327730598702243, - "y": -2.607451498389937e-21, - "heading": 2.027561255801402e-17, - "angularVelocity": 5.194893740089378e-18, - "velocityX": 1.0886876770716345, - "velocityY": 2.7166086454579756e-21, - "timestamp": 3.655622887009051 - }, - { - "x": 2.4369747345029724, - "y": -2.368709002669498e-21, - "heading": 2.0492359746230776e-17, - "angularVelocity": 2.1937892093060286e-18, - "velocityX": 1.054666217107023, - "velocityY": 2.4606042161289694e-21, - "timestamp": 3.754423505576863 - }, - { - "x": 2.5378150668664174, - "y": -2.1523875052863635e-21, - "heading": 2.0979083463355325e-17, - "angularVelocity": 4.926330516473677e-18, - "velocityX": 1.0206447472212212, - "velocityY": 2.228247008672144e-21, - "timestamp": 3.8532241241446754 - }, - { - "x": 2.6352940563724263, - "y": -1.9555762507063697e-21, - "heading": 2.089955236742629e-17, - "angularVelocity": -8.049557622963381e-19, - "velocityX": 0.9866232713827009, - "velocityY": 2.0274855500697654e-21, - "timestamp": 3.9520247427124877 - }, - { - "x": 2.7294117026289118, - "y": -1.7756973118970903e-21, - "heading": 2.048181439249147e-17, - "angularVelocity": -4.228079574039095e-18, - "velocityX": 0.9526017915757001, - "velocityY": 1.8530533449651495e-21, - "timestamp": 4.0508253612803 - }, - { - "x": 2.82016800535581, - "y": -1.610696178925322e-21, - "heading": 2.0393670363820926e-17, - "angularVelocity": -8.921285320598318e-19, - "velocityX": 0.9185803089340697, - "velocityY": 1.6996791445230123e-21, - "timestamp": 4.149625979848111 - }, - { - "x": 2.907562964343073, - "y": -1.458877869554151e-21, - "heading": 2.084174445721108e-17, - "angularVelocity": 4.53514691585319e-18, - "velocityX": 0.8845588241664668, - "velocityY": 1.5641372351679697e-21, - "timestamp": 4.248426598415923 - }, - { - "x": 2.991596579427332, - "y": -1.3188231303239282e-21, - "heading": 2.0547699398998952e-17, - "angularVelocity": -2.9761329639527713e-18, - "velocityX": 0.8505373377453298, - "velocityY": 1.4429357708430696e-21, - "timestamp": 4.347227216983735 - }, - { - "x": 3.0722688504778892, - "y": -1.1894052177930912e-21, - "heading": 2.0372841992969962e-17, - "angularVelocity": -1.769787561100399e-18, - "velocityX": 0.8165158500013651, - "velocityY": 1.3327558109827944e-21, - "timestamp": 4.446027835551547 - }, - { - "x": 3.149579777387813, - "y": -1.0697504752144254e-21, - "heading": 1.984446413733655e-17, - "angularVelocity": -5.347907206549522e-18, - "velocityX": 0.782494361175087, - "velocityY": 1.2321009132712108e-21, - "timestamp": 4.544828454119359 - }, - { - "x": 3.2235293600679915, - "y": -9.590738101698616e-22, - "heading": 1.9549867930164684e-17, - "angularVelocity": -2.981711079527443e-18, - "velocityX": 0.7484728714468809, - "velocityY": 1.139418959561847e-21, - "timestamp": 4.643629072687171 - }, - { - "x": 3.2941175984430227, - "y": -8.567435506711912e-22, - "heading": 1.880855841586028e-17, - "angularVelocity": -7.503072559944813e-18, - "velocityX": 0.714451380955505, - "velocityY": 1.053479684788194e-21, - "timestamp": 4.742429691254983 - }, - { - "x": 3.361344492448277, - "y": -7.621603632941542e-22, - "heading": 1.804651402975059e-17, - "angularVelocity": -7.71293857627177e-18, - "velocityX": 0.6804298898099835, - "velocityY": 9.736267688058442e-22, - "timestamp": 4.8412303098227945 - }, - { - "x": 3.4252100420277425, - "y": -6.74848663914946e-22, - "heading": 1.7632923835461265e-17, - "angularVelocity": -4.186096719140597e-18, - "velocityX": 0.6464083980975359, - "velocityY": 8.98840146425094e-22, - "timestamp": 4.940030928390606 - }, - { - "x": 3.4857142471324067, - "y": -5.943339267652739e-22, - "heading": 1.6713354403567037e-17, - "angularVelocity": -9.30731224773011e-18, - "velocityX": 0.6123869058890279, - "velocityY": 8.286419249206994e-22, - "timestamp": 5.038831546958418 - }, - { - "x": 3.5428571077190254, - "y": -5.202415794373983e-22, - "heading": 1.5572837882186598e-17, - "angularVelocity": -1.1543605179767327e-17, - "velocityX": 0.5783654132428195, - "velocityY": 7.624593234334322e-22, - "timestamp": 5.13763216552623 - }, - { - "x": 3.596638623749159, - "y": -4.522125741089791e-22, - "heading": 1.4464732933422248e-17, - "angularVelocity": -1.1215555338005426e-17, - "velocityX": 0.5443439202075441, - "velocityY": 6.999916566838223e-22, - "timestamp": 5.236432784094042 - }, - { - "x": 3.6470587951884124, - "y": -3.899342272220007e-22, - "heading": 1.3615733384952931e-17, - "angularVelocity": -8.593048010242285e-18, - "velocityX": 0.5103224268241561, - "velocityY": 6.4066863404930395e-22, - "timestamp": 5.335233402661854 - }, - { - "x": 3.694117622005832, - "y": -3.3313328500098465e-22, - "heading": 1.2052388332633744e-17, - "angularVelocity": -1.5823220790511242e-17, - "velocityX": 0.4763009331274668, - "velocityY": 5.842312496177269e-22, - "timestamp": 5.434034021229666 - }, - { - "x": 3.737815104173412, - "y": -2.8156433257179758e-22, - "heading": 1.1136769768708063e-17, - "angularVelocity": -9.267326418690942e-18, - "velocityX": 0.4422794391473144, - "velocityY": 5.304122695590917e-22, - "timestamp": 5.5328346397974775 - }, - { - "x": 3.7781512416656913, - "y": -2.3498113666642085e-22, - "heading": 9.859209548379765e-18, - "angularVelocity": -1.2930681107294836e-17, - "velocityX": 0.40825794490946815, - "velocityY": 4.790511156155067e-22, - "timestamp": 5.631635258365289 - }, - { - "x": 3.815126034459424, - "y": -1.9316819160355648e-22, - "heading": 8.372929242622771e-18, - "angularVelocity": -1.50432199482529e-17, - "velocityX": 0.3742364504363364, - "velocityY": 4.29918013017057e-22, - "timestamp": 5.730435876933101 - }, - { - "x": 3.8487394825333006, - "y": -1.5593072898156008e-22, - "heading": 6.818382919293722e-18, - "angularVelocity": -1.5734167867569838e-17, - "velocityX": 0.3402149557475262, - "velocityY": 3.8283016928136736e-22, - "timestamp": 5.829236495500913 - }, - { - "x": 3.8789915858677166, - "y": -1.2308917074135604e-22, - "heading": 5.324791099236139e-18, - "angularVelocity": -1.5117224017561152e-17, - "velocityX": 0.3061934608602918, - "velocityY": 3.37574987627076e-22, - "timestamp": 5.928037114068725 - }, - { - "x": 3.905882344444576, - "y": -9.448393998536425e-23, - "heading": 4.011436152983029e-18, - "angularVelocity": -1.3292976330431885e-17, - "velocityX": 0.2721719657898966, - "velocityY": 2.9401710775658265e-22, - "timestamp": 6.026837732636537 - }, - { - "x": 3.9294117582471224, - "y": -6.995384807456761e-23, - "heading": 2.9887621240859383e-18, - "angularVelocity": -1.0350880993468448e-17, - "velocityX": 0.23815047054990812, - "velocityY": 2.5206100605913985e-22, - "timestamp": 6.125638351204349 - }, - { - "x": 3.949579827259797, - "y": -4.935913599830647e-23, - "heading": 2.359326118608253e-18, - "angularVelocity": -6.3707648062754864e-18, - "velocityX": 0.20412897515244013, - "velocityY": 2.1159029061948054e-22, - "timestamp": 6.2244389697721605 - }, - { - "x": 3.966386551468114, - "y": -3.25642428787695e-23, - "heading": 2.218613212934694e-18, - "angularVelocity": -1.4242065657400621e-18, - "velocityX": 0.17010747960835323, - "velocityY": 1.725072970802276e-22, - "timestamp": 6.323239588339972 - }, - { - "x": 3.979831930858553, - "y": -1.944612833644722e-23, - "heading": 1.4782676158808526e-18, - "angularVelocity": -7.493326036397043e-18, - "velocityX": 0.1360859839274221, - "velocityY": 1.3470636183720975e-22, - "timestamp": 6.422040206907784 - }, - { - "x": 3.989915965418465, - "y": -9.887317530316752e-24, - "heading": 2.216610663986935e-19, - "angularVelocity": -1.2718607311216777e-17, - "velocityX": 0.10206448811847536, - "velocityY": 9.811604675618584e-23, - "timestamp": 6.520840825475596 - }, - { - "x": 3.996638655135994, - "y": -3.7771668643641844e-24, - "heading": -2.955699978047428e-19, - "angularVelocity": -5.2350975881402005e-18, - "velocityX": 0.06804299218951398, - "velocityY": 6.266873493281363e-23, - "timestamp": 6.619641444043408 - }, - { - "x": 4, - "y": -1.0128885732606577e-24, - "heading": 1.6809468171434698e-26, - "angularVelocity": 2.991581476965099e-18, - "velocityX": 0.03402149614781159, - "velocityY": 2.828535780318287e-23, - "timestamp": 6.71844206261122 - }, - { - "x": 4, - "y": -1.4975010105831042e-24, - "heading": 7.569049534824136e-27, - "angularVelocity": -1.729958968573892e-26, - "velocityX": -1.9187470113508293e-19, - "velocityY": -5.092260525072701e-24, - "timestamp": 6.817242681179032 - }, - { - "x": 3.9966386551359934, - "y": 1.622382271786415e-20, - "heading": 3.3801616172272874e-20, - "angularVelocity": 3.421193955466234e-19, - "velocityX": -0.034021496147811595, - "velocityY": 1.6425392695916164e-19, - "timestamp": 6.9160432997468435 - }, - { - "x": 3.989915965418465, - "y": 4.867644146454126e-20, - "heading": 1.0140943353587003e-19, - "angularVelocity": 6.842853514451412e-19, - "velocityX": -0.06804299218951398, - "velocityY": 3.285129050412422e-19, - "timestamp": 7.014843918314655 - }, - { - "x": 3.9798319308585524, - "y": 9.735635325747916e-20, - "heading": 2.0282865518075705e-19, - "angularVelocity": 1.0265038932822472e-18, - "velocityX": -0.10206448811847536, - "velocityY": 4.927718248471109e-19, - "timestamp": 7.113644536882467 - }, - { - "x": 3.966386551468114, - "y": 1.6226355072631345e-19, - "heading": 3.3806475714931607e-19, - "angularVelocity": 1.368777860282825e-18, - "velocityX": -0.1360859839274221, - "velocityY": 6.570306670773842e-19, - "timestamp": 7.212445155450279 - }, - { - "x": 3.949579827259797, - "y": 2.4339802438242795e-19, - "heading": 5.071235363962782e-19, - "angularVelocity": 1.7111105005542439e-18, - "velocityX": -0.17010747960835323, - "velocityY": 8.212894100973546e-19, - "timestamp": 7.311245774018091 - }, - { - "x": 3.9294117582471224, - "y": 3.4075976233216677e-19, - "heading": 7.100112314845174e-19, - "angularVelocity": 2.0535062837733624e-18, - "velocityX": -0.2041289751524401, - "velocityY": 9.855480293458379e-19, - "timestamp": 7.410046392585903 - }, - { - "x": 3.9058823444445765, - "y": 4.543487499464003e-19, - "heading": 9.467344638444388e-19, - "angularVelocity": 2.3959690850953575e-18, - "velocityX": -0.23815047054990812, - "velocityY": 1.1498064968884804e-18, - "timestamp": 7.508847011153715 - }, - { - "x": 3.8789915858677175, - "y": 5.84164969472011e-19, - "heading": 1.2173004091506322e-18, - "angularVelocity": 2.7385045142480053e-18, - "velocityX": -0.2721719657898966, - "velocityY": 1.3140647808442577e-18, - "timestamp": 7.6076476297215265 - }, - { - "x": 3.8487394825333014, - "y": 7.302083995695225e-19, - "heading": 1.5217167130383014e-18, - "angularVelocity": 3.0811173278628614e-18, - "velocityX": -0.30619346086029176, - "velocityY": 1.478322844607657e-18, - "timestamp": 7.706448248289338 - }, - { - "x": 3.815126034459425, - "y": 8.9247901475715e-19, - "heading": 1.859991687922629e-18, - "angularVelocity": 3.42381427473618e-18, - "velocityX": -0.3402149557475262, - "velocityY": 1.6425806459233442e-18, - "timestamp": 7.80524886685715 - }, - { - "x": 3.7781512416656926, - "y": 1.0709767847438179e-18, - "heading": 2.232134281072569e-18, - "angularVelocity": 3.7666017804132156e-18, - "velocityX": -0.3742364504363364, - "velocityY": 1.8068381357194375e-18, - "timestamp": 7.904049485424962 - }, - { - "x": 3.7378151041734133, - "y": 1.2657016736040118e-18, - "heading": 2.638154283334348e-18, - "angularVelocity": 4.1094883831216016e-18, - "velocityX": -0.40825794490946815, - "velocityY": 1.9710952565763296e-18, - "timestamp": 8.002850103992774 - }, - { - "x": 3.694117622005834, - "y": 1.476653638799985e-18, - "heading": 3.078062355478974e-18, - "angularVelocity": 4.452482887685102e-18, - "velocityX": -0.4422794391473143, - "velocityY": 2.135351941018014e-18, - "timestamp": 8.101650722560587 - }, - { - "x": 3.6470587951884137, - "y": 1.7038326299290477e-18, - "heading": 3.551870199581173e-18, - "angularVelocity": 4.795595833539573e-18, - "velocityX": -0.47630093312746674, - "velocityY": 2.299608108984575e-18, - "timestamp": 8.2004513411284 - }, - { - "x": 3.5966386237491603, - "y": 1.9472385872002766e-18, - "heading": 4.059590654442284e-18, - "angularVelocity": 5.138838726011681e-18, - "velocityX": -0.5103224268241561, - "velocityY": 2.4638636646972548e-18, - "timestamp": 8.299251959696212 - }, - { - "x": 3.542857107719027, - "y": 2.206871439463151e-18, - "heading": 3.423761751446479e-18, - "angularVelocity": -6.435474798217317e-18, - "velocityX": -0.5443439202075441, - "velocityY": 2.62811849254691e-18, - "timestamp": 8.398052578264025 - }, - { - "x": 3.4857142471324085, - "y": 2.4827311017573516e-18, - "heading": 2.8218754348898888e-18, - "angularVelocity": -6.091928510129713e-18, - "velocityX": -0.5783654132428195, - "velocityY": 2.7923724518212657e-18, - "timestamp": 8.496853196831838 - }, - { - "x": 3.425210042027744, - "y": 2.7748174720594974e-18, - "heading": 2.2539492010975555e-18, - "angularVelocity": -5.748205134856798e-18, - "velocityX": -0.6123869058890279, - "velocityY": 2.956625368836816e-18, - "timestamp": 8.59565381539965 - }, - { - "x": 3.3613444924482785, - "y": 3.0831304270690512e-18, - "heading": 1.7200028141341935e-18, - "angularVelocity": -5.4042817196730335e-18, - "velocityX": -0.6464083980975359, - "velocityY": 3.1208770270543982e-18, - "timestamp": 8.694454433967463 - }, - { - "x": 3.2941175984430235, - "y": 3.407669816567964e-18, - "heading": 1.2200587363363886e-18, - "angularVelocity": -5.0601309543877636e-18, - "velocityX": -0.6804298898099835, - "velocityY": 3.2851271525026197e-18, - "timestamp": 8.793255052535276 - }, - { - "x": 3.2235293600679924, - "y": 3.748435455691416e-18, - "heading": 7.541425169569603e-19, - "angularVelocity": -4.715721595037053e-18, - "velocityX": -0.714451380955505, - "velocityY": 3.4493753928478014e-18, - "timestamp": 8.892055671103089 - }, - { - "x": 3.149579777387814, - "y": 4.105427114281349e-18, - "heading": 3.2228393306074877e-19, - "angularVelocity": -4.371010850288281e-18, - "velocityX": -0.7484728714468808, - "velocityY": 3.61362128746775e-18, - "timestamp": 8.990856289670901 - }, - { - "x": 3.0722688504778906, - "y": 4.478644501633054e-18, - "heading": -7.548243860660323e-20, - "angularVelocity": -4.025950140734063e-18, - "velocityX": -0.782494361175087, - "velocityY": 3.7778642208016786e-18, - "timestamp": 9.089656908238714 - }, - { - "x": 2.9915965794273327, - "y": 4.868087244242979e-18, - "heading": -4.391158104077786e-19, - "angularVelocity": -3.680476603421153e-18, - "velocityX": -0.8165158500013651, - "velocityY": 3.942103351400059e-18, - "timestamp": 9.188457526806527 - }, - { - "x": 2.907562964343074, - "y": 5.2737548521275295e-18, - "heading": -7.685671750620224e-19, - "angularVelocity": -3.334507044128586e-18, - "velocityX": -0.8505373377453298, - "velocityY": 4.1063374959021146e-18, - "timestamp": 9.28725814537434 - }, - { - "x": 2.8201680053558102, - "y": 5.695646665833697e-18, - "heading": -1.0637766148592117e-18, - "angularVelocity": -2.9879310340772823e-18, - "velocityX": -0.8845588241664668, - "velocityY": 4.270564932988105e-18, - "timestamp": 9.386058763942152 - }, - { - "x": 2.729411702628912, - "y": 6.1337617692699354e-18, - "heading": -1.3246696022153147e-18, - "angularVelocity": -2.6406007010458607e-18, - "velocityX": -0.9185803089340697, - "velocityY": 4.434783057031529e-18, - "timestamp": 9.484859382509965 - }, - { - "x": 2.6352940563724268, - "y": 6.588098837620084e-18, - "heading": -1.5511502459600477e-18, - "angularVelocity": -2.2922998157391772e-18, - "velocityX": -0.9526017915757001, - "velocityY": 4.59898771937502e-18, - "timestamp": 9.583660001077778 - }, - { - "x": 2.5378150668664174, - "y": 7.058655852182776e-18, - "heading": -1.7430903085127116e-18, - "angularVelocity": -1.9427009874027487e-18, - "velocityX": -0.9866232713827009, - "velocityY": 4.763171879327714e-18, - "timestamp": 9.68246061964559 - }, - { - "x": 2.4369747345029724, - "y": 7.545429512244077e-18, - "heading": -1.9003111994243256e-18, - "angularVelocity": -1.591294574936401e-18, - "velocityX": -1.0206447472212212, - "velocityY": 4.927322533296983e-18, - "timestamp": 9.781261238213403 - }, - { - "x": 2.3327730598702243, - "y": 8.04841385541656e-18, - "heading": -2.022543272012013e-18, - "angularVelocity": -1.2371589623518452e-18, - "velocityX": -1.054666217107023, - "velocityY": 5.091412692060407e-18, - "timestamp": 9.880061856781216 - }, - { - "x": 2.225210043948392, - "y": 8.567596345936824e-18, - "heading": -2.1093375884193087e-18, - "angularVelocity": -8.784794728630001e-19, - "velocityX": -1.0886876770716345, - "velocityY": 5.255375725743804e-18, - "timestamp": 9.978862475349029 - }, - { - "x": 2.1142856886979113, - "y": 9.102942798044225e-18, - "heading": -2.159797685075532e-18, - "angularVelocity": -5.107265229524464e-19, - "velocityX": -1.122709117193911, - "velocityY": 5.418992260996229e-18, - "timestamp": 10.077663093916842 - }, - { - "x": 2, - "y": 9.654290125645511e-18, - "heading": -2.171249723825065e-18, - "angularVelocity": -1.1591060473957685e-19, - "velocityX": -1.156730497790058, - "velocityY": 5.580956667058148e-18, - "timestamp": 10.176463712484654 - }, - { - "x": 1.8857143113020887, - "y": 9.61185739696908e-18, - "heading": -2.2270648319021466e-18, - "angularVelocity": -5.649266874918011e-19, - "velocityX": -1.1567304977900583, - "velocityY": -4.2949032487006095e-19, - "timestamp": 10.275264331052467 - }, - { - "x": 1.7747899560516078, - "y": 9.553513369992968e-18, - "heading": -2.3111613029527945e-18, - "angularVelocity": -8.511735047427319e-19, - "velocityX": -1.1227091171939112, - "velocityY": -5.905538912325038e-19, - "timestamp": 10.37406494962028 - }, - { - "x": 1.6672269401297757, - "y": 9.479013543281256e-18, - "heading": -2.4262130872250256e-18, - "angularVelocity": -1.1644844250684416e-18, - "velocityX": -1.0886876770716345, - "velocityY": -7.540907241750166e-19, - "timestamp": 10.472865568188093 - }, - { - "x": 1.5630252654970274, - "y": 9.388316488623372e-18, - "heading": -2.5731140577432874e-18, - "angularVelocity": -1.4868425859374024e-18, - "velocityX": -1.054666217107023, - "velocityY": -9.180463912283675e-19, - "timestamp": 10.571666186755905 - }, - { - "x": 1.4621849331335826, - "y": 9.281408255613739e-18, - "heading": -2.7523119274054557e-18, - "angularVelocity": -1.8137322254046327e-18, - "velocityX": -1.0206447472212212, - "velocityY": -1.082143017049038e-18, - "timestamp": 10.670466805323718 - }, - { - "x": 1.3647059436275732, - "y": 9.158282543727594e-18, - "heading": -2.964075868359904e-18, - "angularVelocity": -2.143346262102346e-18, - "velocityX": -0.986623271382701, - "velocityY": -1.246303269806804e-18, - "timestamp": 10.769267423891531 - }, - { - "x": 1.2705882973710878, - "y": 9.018935994247637e-18, - "heading": -3.2085845705889515e-18, - "angularVelocity": -2.4747688902482226e-18, - "velocityX": -0.9526017915757001, - "velocityY": -1.4104974218850865e-18, - "timestamp": 10.868068042459344 - }, - { - "x": 1.17983199464419, - "y": 8.86336661456802e-18, - "heading": -3.485966591694146e-18, - "angularVelocity": -2.8074927005328834e-18, - "velocityX": -0.9185803089340697, - "velocityY": -1.5747116736319965e-18, - "timestamp": 10.966868661027156 - }, - { - "x": 1.0924370356569264, - "y": 8.69157313096237e-18, - "heading": -3.796317507759278e-18, - "angularVelocity": -3.141183873969326e-18, - "velocityX": -0.8845588241664669, - "velocityY": -1.7389387660362588e-18, - "timestamp": 11.06566927959497 - }, - { - "x": 1.0084034205726677, - "y": 8.503554683319867e-18, - "heading": -4.139711774475018e-18, - "angularVelocity": -3.475628642739219e-18, - "velocityX": -0.8505373377453298, - "velocityY": -1.903174524206938e-18, - "timestamp": 11.164469898162782 - }, - { - "x": 0.9277311495221099, - "y": 8.299310665997281e-18, - "heading": -4.516208836106112e-18, - "angularVelocity": -3.810675070331667e-18, - "velocityX": -0.8165158500013652, - "velocityY": -2.067416380452275e-18, - "timestamp": 11.263270516730595 - }, - { - "x": 0.8504202226121863, - "y": 8.0788406383874e-18, - "heading": -4.9258577031240064e-18, - "angularVelocity": -4.1462175521934945e-18, - "velocityX": -0.782494361175087, - "velocityY": -2.2316626702442053e-18, - "timestamp": 11.362071135298407 - }, - { - "x": 0.7764706399320079, - "y": 7.842144271446874e-18, - "heading": -5.368698751428868e-18, - "angularVelocity": -4.482168694424708e-18, - "velocityX": -0.7484728714468809, - "velocityY": -2.395912268619746e-18, - "timestamp": 11.46087175386622 - }, - { - "x": 0.7058824015569765, - "y": 7.589221314273757e-18, - "heading": -5.844766537688045e-18, - "angularVelocity": -4.8184695983098995e-18, - "velocityX": -0.714451380955505, - "velocityY": -2.5601643880906105e-18, - "timestamp": 11.559672372434033 - }, - { - "x": 0.6386555075517218, - "y": 7.320071572246599e-18, - "heading": -6.3540907090448196e-18, - "angularVelocity": -5.155070572582827e-18, - "velocityX": -0.6804298898099835, - "velocityY": -2.7244184616393568e-18, - "timestamp": 11.658472991001846 - }, - { - "x": 0.5747899579722568, - "y": 7.034694892326384e-18, - "heading": -5.719220693211024e-18, - "angularVelocity": 6.4257695231037726e-18, - "velocityX": -0.6464083980975359, - "velocityY": -2.888674070537039e-18, - "timestamp": 11.757273609569658 - }, - { - "x": 0.5142857528675924, - "y": 6.733091152832895e-18, - "heading": -5.1176550171087195e-18, - "angularVelocity": 6.0886831811172465e-18, - "velocityX": -0.6123869058890279, - "velocityY": -3.0529308990878334e-18, - "timestamp": 11.856074228137471 - }, - { - "x": 0.4571428922809735, - "y": 6.415260256155639e-18, - "heading": -4.549413483741888e-18, - "angularVelocity": 5.751396405133998e-18, - "velocityX": -0.5783654132428195, - "velocityY": -3.2171887049931132e-18, - "timestamp": 11.954874846705284 - }, - { - "x": 0.4033613762508404, - "y": 6.081202123490567e-18, - "heading": -4.014513636196958e-18, - "angularVelocity": 5.413932068682268e-18, - "velocityX": -0.5443439202075441, - "velocityY": -3.381447299080878e-18, - "timestamp": 12.053675465273097 - }, - { - "x": 0.35294120481158675, - "y": 5.730916690946256e-18, - "heading": -3.512971170353614e-18, - "angularVelocity": 5.076308868051237e-18, - "velocityX": -0.5103224268241561, - "velocityY": -3.545706531606395e-18, - "timestamp": 12.15247608384091 - }, - { - "x": 0.3058823779941669, - "y": 5.364403906543085e-18, - "heading": -3.0448003053157668e-18, - "angularVelocity": 4.738541750260109e-18, - "velocityX": -0.47630093312746674, - "velocityY": -3.709966282517656e-18, - "timestamp": 12.251276702408722 - }, - { - "x": 0.2621848958265872, - "y": 4.981663728010729e-18, - "heading": -2.6100139125080014e-18, - "angularVelocity": 4.400644355727739e-18, - "velocityX": -0.4422794391473143, - "velocityY": -3.874226454538646e-18, - "timestamp": 12.350077320976535 - }, - { - "x": 0.22184875833430798, - "y": 4.582696121005342e-18, - "heading": -2.208623621378623e-18, - "angularVelocity": 4.062629254983044e-18, - "velocityX": -0.4082579449094682, - "velocityY": -4.038486967950237e-18, - "timestamp": 12.448877939544348 - }, - { - "x": 0.18487396554057556, - "y": 4.167501057752707e-18, - "heading": -1.8406400335009087e-18, - "angularVelocity": 3.724506851511534e-18, - "velocityX": -0.3742364504363364, - "velocityY": -4.202747756784724e-18, - "timestamp": 12.54767855811216 - }, - { - "x": 0.15126051746669916, - "y": 3.736078515959616e-18, - "heading": -1.506072956037892e-18, - "angularVelocity": 3.3862851859073145e-18, - "velocityX": -0.3402149557475262, - "velocityY": -4.367008766061658e-18, - "timestamp": 12.646479176679973 - }, - { - "x": 0.12100841413228317, - "y": 3.2884284779303413e-18, - "heading": -1.2049313957041987e-18, - "angularVelocity": 3.0479723597881684e-18, - "velocityX": -0.3061934608602918, - "velocityY": -4.531269949657919e-18, - "timestamp": 12.745279795247786 - }, - { - "x": 0.09411765555542398, - "y": 2.824550929875594e-18, - "heading": -9.372236288478572e-19, - "angularVelocity": 2.7095757655060014e-18, - "velocityX": -0.2721719657898966, - "velocityY": -4.695531268401831e-18, - "timestamp": 12.844080413815599 - }, - { - "x": 0.07058824175287753, - "y": 2.3444458613366558e-18, - "heading": -7.029573657694057e-19, - "angularVelocity": 2.371101132177252e-18, - "velocityX": -0.23815047054990812, - "velocityY": -4.859792688975063e-18, - "timestamp": 12.942881032383411 - }, - { - "x": 0.050420172740202905, - "y": 1.8481132646899755e-18, - "heading": -5.021397823279256e-19, - "angularVelocity": 2.032553869215878e-18, - "velocityX": -0.2041289751524401, - "velocityY": -5.024054182861873e-18, - "timestamp": 13.041681650951224 - }, - { - "x": 0.03361344853188612, - "y": 1.3355531347764025e-18, - "heading": -3.3477751504246894e-19, - "angularVelocity": 1.6939394354207508e-18, - "velocityX": -0.17010747960835323, - "velocityY": -5.188315725440333e-18, - "timestamp": 13.140482269519037 - }, - { - "x": 0.020168069141447482, - "y": 8.067654685706554e-19, - "heading": -2.008767863019259e-19, - "angularVelocity": 1.355262022365565e-18, - "velocityX": -0.1360859839274221, - "velocityY": -5.352577295354402e-18, - "timestamp": 13.23928288808685 - }, - { - "x": 0.010084034581535006, - "y": 2.6175026490285327e-19, - "heading": -1.0044337813369068e-19, - "angularVelocity": 1.016526087149893e-18, - "velocityX": -0.10206448811847536, - "velocityY": -5.516838874018515e-18, - "timestamp": 13.338083506654662 - }, - { - "x": 0.0033613448640062178, - "y": -2.9949247577400035e-19, - "heading": -3.3482745638057493e-20, - "angularVelocity": 6.777349387241571e-19, - "velocityX": -0.06804299218951398, - "velocityY": -5.6811004451979164e-18, - "timestamp": 13.436884125222475 - }, - { - "x": -2.9258798480024608e-18, - "y": -8.769627515656533e-19, - "heading": -1.2196216569957611e-26, - "angularVelocity": 3.388919345415541e-19, - "velocityX": -0.03402149614781159, - "velocityY": -5.845361994494316e-18, - "timestamp": 13.535684743790288 - }, - { - "x": -1.1376141000811762e-18, - "y": -1.4706605593137037e-18, - "heading": -9.263715163460755e-27, - "angularVelocity": 2.929800407564323e-26, - "velocityX": 6.5860729270801154e-18, - "velocityY": -6.0096235092764735e-18, - "timestamp": 13.6344853623581 - }, - { - "x": -5.135794547435122e-19, - "y": 0.0033333336521360404, - "heading": -2.1647079011876355e-19, - "angularVelocity": -2.2001741500814918e-18, - "velocityX": 6.3431154311892524e-18, - "velocityY": 0.03387944331015331, - "timestamp": 13.732873450427336 - }, - { - "x": 8.655817806035818e-20, - "y": 0.010000000941341826, - "heading": -5.697678805519529e-19, - "angularVelocity": -3.590855508881197e-18, - "velocityX": 6.100208278091576e-18, - "velocityY": 0.06775888646717527, - "timestamp": 13.831261538496571 - }, - { - "x": 6.628042064573855e-19, - "y": 0.020000001851181393, - "heading": -9.256562256435773e-19, - "angularVelocity": -3.617194481532914e-18, - "velocityX": 5.857356086283696e-18, - "velocityY": 0.10163832945714488, - "timestamp": 13.929649626565807 - }, - { - "x": 1.2151645581634748e-18, - "y": 0.033333336363653446, - "heading": -1.117382623336341e-18, - "angularVelocity": -1.9486820947092033e-18, - "velocityX": 5.614564136214341e-18, - "velocityY": 0.13551777226415238, - "timestamp": 14.028037714635042 - }, - { - "x": 1.7436457586956917e-18, - "y": 0.05000000445895656, - "heading": -2.3967423989417355e-19, - "angularVelocity": 8.920870961137473e-18, - "velocityX": 5.371838503153778e-18, - "velocityY": 0.16939721486990153, - "timestamp": 14.126425802704278 - }, - { - "x": 2.2482550261017366e-18, - "y": 0.07000000611520496, - "heading": 3.6125235146058906e-19, - "angularVelocity": 6.107704969814862e-18, - "velocityX": 5.129186225139142e-18, - "velocityY": 0.20327665725320723, - "timestamp": 14.224813890773513 - }, - { - "x": 2.7290003868346387e-18, - "y": 0.09333334130808112, - "heading": 2.3536802574991945e-18, - "angularVelocity": 2.0250687712038572e-17, - "velocityX": 4.886615517778109e-18, - "velocityY": 0.23715609938935361, - "timestamp": 14.323201978842748 - }, - { - "x": 3.185890819000824e-18, - "y": 0.12000001001040657, - "heading": 3.40725878345287e-18, - "angularVelocity": 1.070837766681137e-17, - "velocityX": 4.64413605242649e-18, - "velocityY": 0.27103554124926305, - "timestamp": 14.421590066911984 - }, - { - "x": 3.618936431576692e-18, - "y": 0.15000001219160564, - "heading": 4.731756190632852e-18, - "angularVelocity": 1.3461948548984505e-17, - "velocityX": 4.4017593218063314e-18, - "velocityY": 0.30491498279840595, - "timestamp": 14.519978154981219 - }, - { - "x": 4.028148691625798e-18, - "y": 0.18333334781702582, - "heading": 5.109272897761843e-18, - "angularVelocity": 3.836992583636047e-18, - "velocityX": 4.1594991279037615e-18, - "velocityY": 0.3387944239953468, - "timestamp": 14.618366243050454 - }, - { - "x": 4.413540716810058e-18, - "y": 0.22000001684706427, - "heading": 6.997707395181324e-18, - "angularVelocity": 1.919370316508542e-17, - "velocityX": 3.91737224524324e-18, - "velocityY": 0.37267386478977105, - "timestamp": 14.71675433111969 - }, - { - "x": 4.775127658373826e-18, - "y": 0.2600000192360254, - "heading": 7.882590747581164e-18, - "angularVelocity": 8.993773931256457e-18, - "velocityX": 3.6753993405321354e-18, - "velocityY": 0.406553305119753, - "timestamp": 14.815142419188925 - }, - { - "x": 5.112927212518244e-18, - "y": 0.3033333549305977, - "heading": 8.987118747688736e-18, - "angularVelocity": 1.1226200578912163e-17, - "velocityX": 3.433606277613366e-18, - "velocityY": 0.4404327449078857, - "timestamp": 14.91353050725816 - }, - { - "x": 5.426960319003839e-18, - "y": 0.3500000238677766, - "heading": 1.0530500974465385e-17, - "angularVelocity": 1.5686635916726856e-17, - "velocityX": 3.1920260198981374e-18, - "velocityY": 0.47431218405565095, - "timestamp": 15.011918595327396 - }, - { - "x": 5.7172521411776675e-18, - "y": 0.4000000259719568, - "heading": 1.2181097094013063e-17, - "angularVelocity": 1.6776333979129697e-17, - "velocityX": 2.9507014905610136e-18, - "velocityY": 0.5081916224349754, - "timestamp": 15.110306683396631 - }, - { - "x": 5.983833485064665e-18, - "y": 0.45333336115073153, - "heading": 1.4802873414250385e-17, - "angularVelocity": 2.6647239353050688e-17, - "velocityX": 2.7096900347694933e-18, - "velocityY": 0.5420710598750945, - "timestamp": 15.208694771465867 - }, - { - "x": 6.226742934460706e-18, - "y": 0.5100000292885927, - "heading": 1.7753206373705575e-17, - "angularVelocity": 2.9986626187410924e-17, - "velocityX": 2.469070695949251e-18, - "velocityY": 0.575950496141207, - "timestamp": 15.307082859535102 - }, - { - "x": 6.446030218838556e-18, - "y": 0.5700000302370294, - "heading": 2.0426163520318335e-17, - "angularVelocity": 2.7167415779249895e-17, - "velocityX": 2.22895674498737e-18, - "velocityY": 0.6098299308978823, - "timestamp": 15.405470947604337 - }, - { - "x": 6.6417618540851514e-18, - "y": 0.6333333637980277, - "heading": 2.266086393224807e-17, - "angularVelocity": 2.2713036643856925e-17, - "velocityX": 1.9895187779423867e-18, - "velocityY": 0.643709363641975, - "timestamp": 15.503859035673573 - }, - { - "x": 6.814031362443908e-18, - "y": 0.7000000296943679, - "heading": 2.4278538293185775e-17, - "angularVelocity": 1.6441673304388865e-17, - "velocityX": 1.7510312541171544e-18, - "velocityY": 0.6775887935684523, - "timestamp": 15.602247123742808 - }, - { - "x": 6.962979904521674e-18, - "y": 0.7700000275102201, - "heading": 2.5576668981263964e-17, - "angularVelocity": 1.3193866152308666e-17, - "velocityX": 1.5139783111446211e-18, - "velocityY": 0.7114682192685073, - "timestamp": 15.700635211812044 - }, - { - "x": 7.08884511898635e-18, - "y": 0.8433333565525355, - "heading": 2.687208646774119e-17, - "angularVelocity": 1.3166263419656072e-17, - "velocityX": 1.2793404346458061e-18, - "velocityY": 0.7453476379245281, - "timestamp": 15.799023299881279 - }, - { - "x": 7.192111469900735e-18, - "y": 0.9200000154352173, - "heading": 2.792648282176678e-17, - "angularVelocity": 1.07165211280523e-17, - "velocityX": 1.0496260639054993e-18, - "velocityY": 0.7792270424924943, - "timestamp": 15.897411387950514 - }, - { - "x": 7.274312354830099e-18, - "y": 1, - "heading": 3.000827595607593e-17, - "angularVelocity": 2.1158704872256568e-17, - "velocityX": 8.354953731965381e-19, - "velocityY": 0.813106404796549, - "timestamp": 15.99579947601975 - }, - { - "x": 6.687149573241866e-18, - "y": 1.0799999845647827, - "heading": 2.930546078718981e-17, - "angularVelocity": -7.143039017142222e-18, - "velocityX": -5.968361601177678e-18, - "velocityY": 0.813106404796549, - "timestamp": 16.094187564088987 - }, - { - "x": 6.126769558515328e-18, - "y": 1.1566666434474644, - "heading": 2.90297401967108e-17, - "angularVelocity": -2.8021955879817803e-18, - "velocityX": -5.696128244278018e-18, - "velocityY": 0.7792270424924943, - "timestamp": 16.192575652158222 - }, - { - "x": 5.591823986937553e-18, - "y": 1.2299999724897799, - "heading": 2.857601134904138e-17, - "angularVelocity": -4.611479301985519e-18, - "velocityX": -5.437597031717557e-18, - "velocityY": 0.745347637924528, - "timestamp": 16.290963740227458 - }, - { - "x": 5.081849281816435e-18, - "y": 1.299999970305632, - "heading": 2.647194627599618e-17, - "angularVelocity": -2.1385244054143357e-17, - "velocityX": -5.183776870340226e-18, - "velocityY": 0.7114682192685073, - "timestamp": 16.389351828296693 - }, - { - "x": 4.596610451039836e-18, - "y": 1.3666666362019722, - "heading": 2.5047603581040707e-17, - "angularVelocity": -1.4476678683571795e-17, - "velocityX": -4.932344796891426e-18, - "velocityY": 0.6775887935684523, - "timestamp": 16.48773991636593 - }, - { - "x": 4.135965398247791e-18, - "y": 1.4299999697629706, - "heading": 2.302762325152366e-17, - "angularVelocity": -2.0530653971935406e-17, - "velocityX": -4.682356767449444e-18, - "velocityY": 0.6437093636419748, - "timestamp": 16.586128004435164 - }, - { - "x": 3.699818913378491e-18, - "y": 1.4899999707114076, - "heading": 2.142223093747466e-17, - "angularVelocity": -1.6316863078396375e-17, - "velocityX": -4.433336304057429e-18, - "velocityY": 0.6098299308978822, - "timestamp": 16.6845160925044 - }, - { - "x": 3.2881027461238596e-18, - "y": 1.5466666388492685, - "heading": 2.064122419774981e-17, - "angularVelocity": -7.937956363543176e-18, - "velocityX": -4.185009430290064e-18, - "velocityY": 0.5759504961412069, - "timestamp": 16.782904180573635 - }, - { - "x": 2.9007655712244052e-18, - "y": 1.5999999740280433, - "heading": 1.9969619559651016e-17, - "angularVelocity": -6.826019925002285e-18, - "velocityX": -3.937204146623221e-18, - "velocityY": 0.5420710598750945, - "timestamp": 16.88129226864287 - }, - { - "x": 2.5377673846695666e-18, - "y": 1.6499999761322235, - "heading": 1.9374646420177126e-17, - "angularVelocity": -6.047157286094339e-18, - "velocityX": -3.689805401521427e-18, - "velocityY": 0.5081916224349754, - "timestamp": 16.979680356712105 - }, - { - "x": 2.1990761279741485e-18, - "y": 1.6966666450694023, - "heading": 1.8916317262571595e-17, - "angularVelocity": -4.65833693251108e-18, - "velocityX": -3.4427324491167103e-18, - "velocityY": 0.4743121840556509, - "timestamp": 17.07806844478134 - }, - { - "x": 1.8846655332659204e-18, - "y": 1.7399999807639748, - "heading": 1.7548485280288527e-17, - "angularVelocity": -1.3902376050525728e-17, - "velocityX": -3.1959264426763933e-18, - "velocityY": 0.44043274490788564, - "timestamp": 17.176456532850576 - }, - { - "x": 1.5945136831032818e-18, - "y": 1.7799999831529358, - "heading": 1.5621235661695857e-17, - "angularVelocity": -1.9588207602447508e-17, - "velocityX": -2.9493431711978625e-18, - "velocityY": 0.40655330511975296, - "timestamp": 17.27484462091981 - }, - { - "x": 1.3286020112835303e-18, - "y": 1.8166666521829742, - "heading": 1.3232319504175871e-17, - "angularVelocity": -2.4280513008707978e-17, - "velocityX": -2.7029485778613663e-18, - "velocityY": 0.37267386478977105, - "timestamp": 17.373232708989047 - }, - { - "x": 1.086914587899011e-18, - "y": 1.8499999878083944, - "heading": 1.2132419497554097e-17, - "angularVelocity": -1.1179173608616076e-17, - "velocityX": -2.4567158715211472e-18, - "velocityY": 0.3387944239953468, - "timestamp": 17.471620797058282 - }, - { - "x": 8.694375942744545e-19, - "y": 1.8799999899895934, - "heading": 1.0540845375160962e-17, - "angularVelocity": -1.617647020627435e-17, - "velocityX": -2.21062359685979e-18, - "velocityY": 0.304914982798406, - "timestamp": 17.570008885127518 - }, - { - "x": 6.761589286442026e-19, - "y": 1.9066666586919188, - "heading": 8.883338993825269e-18, - "angularVelocity": -1.6846597854180293e-17, - "velocityX": -1.964654305865992e-18, - "velocityY": 0.27103554124926305, - "timestamp": 17.668396973196753 - }, - { - "x": 5.070679042760459e-19, - "y": 1.929999993884795, - "heading": 6.529061416277828e-18, - "angularVelocity": -2.392846609653831e-17, - "velocityX": -1.7187936183501359e-18, - "velocityY": 0.2371560993893536, - "timestamp": 17.76678506126599 - }, - { - "x": 3.6215501445535984e-19, - "y": 1.9499999955410434, - "heading": 5.175466168401875e-18, - "angularVelocity": -1.3757701954247557e-17, - "velocityX": -1.4730295424471154e-18, - "velocityY": 0.20327665725320723, - "timestamp": 17.865173149335224 - }, - { - "x": 2.414117467704845e-19, - "y": 1.9666666636363466, - "heading": 2.4887675690357e-18, - "angularVelocity": -2.7307142780637332e-17, - "velocityX": -1.2273519735815864e-18, - "velocityY": 0.1693972148699015, - "timestamp": 17.96356123740446 - }, - { - "x": 1.4483043443847653e-19, - "y": 1.9799999981488186, - "heading": 8.601988500829106e-19, - "angularVelocity": -1.655249097200501e-17, - "velocityX": -9.817523181071624e-19, - "velocityY": 0.13551777226415235, - "timestamp": 18.061949325473694 - }, - { - "x": 7.240413585822682e-20, - "y": 1.989999999058658, - "heading": 3.266565799202321e-19, - "angularVelocity": -5.4228284055470716e-18, - "velocityX": -7.3622320639297e-19, - "velocityY": 0.10163832945714488, - "timestamp": 18.16033741354293 - }, - { - "x": 2.4126536001171076e-20, - "y": 1.9966666663478638, - "heading": 2.6734077366946317e-19, - "angularVelocity": -6.028723729689646e-19, - "velocityX": -4.907582709261484e-19, - "velocityY": 0.06775888646717529, - "timestamp": 18.258725501612165 - }, - { - "x": -8.135093947234693e-24, - "y": 1.9999999999999998, - "heading": -1.6954777581594528e-26, - "angularVelocity": -2.7172052030925932e-18, - "velocityX": -2.4535197248634874e-19, - "velocityY": 0.033879443310153315, - "timestamp": 18.3571135896814 - }, - { - "x": -5.008838848500651e-24, - "y": 2, - "heading": -9.913062195911202e-27, - "angularVelocity": -2.918386915613901e-26, - "velocityX": 2.1783781061156283e-24, - "velocityY": -2.5283992697715324e-19, - "timestamp": 18.455501677750636 - }, - { - "x": 2.0753539310053916e-20, - "y": 1.9966666663478638, - "heading": -3.3924267442427026e-25, - "angularVelocity": -3.3472516337821726e-24, - "velocityX": 2.1102334506622982e-19, - "velocityY": -0.03387944331015331, - "timestamp": 18.55388976581987 - }, - { - "x": 6.227260339405634e-20, - "y": 1.989999999058658, - "heading": -9.415127970314296e-25, - "angularVelocity": -6.1213762440798274e-24, - "velocityX": 4.2204537503841897e-19, - "velocityY": -0.06775888646717527, - "timestamp": 18.652277853889107 - }, - { - "x": 1.2455227342885217e-19, - "y": 1.9799999981488186, - "heading": -1.7962723582885153e-24, - "angularVelocity": -8.687657753919161e-24, - "velocityX": 6.330683169707924e-19, - "velocityY": -0.10163832945714488, - "timestamp": 18.750665941958342 - }, - { - "x": 2.075926447807237e-19, - "y": 1.9666666636363466, - "heading": -2.8366172681319197e-24, - "angularVelocity": -1.0573910042856395e-23, - "velocityX": 8.440922249651074e-19, - "velocityY": -0.13551777226415238, - "timestamp": 18.849054030027578 - }, - { - "x": 3.1139381878486925e-19, - "y": 1.9499999955410434, - "heading": -3.972041686262479e-24, - "angularVelocity": -1.1540279609110701e-23, - "velocityX": 1.0551171593656732e-18, - "velocityY": -0.1693972148699015, - "timestamp": 18.947442118096813 - }, - { - "x": 4.359559034453201e-19, - "y": 1.929999993884795, - "heading": -5.159478281485765e-24, - "angularVelocity": -1.20689097210531e-23, - "velocityX": 1.2661431875802598e-18, - "velocityY": -0.20327665725320723, - "timestamp": 19.04583020616605 - }, - { - "x": 5.812790142432268e-19, - "y": 1.9066666586919188, - "heading": -6.324264281794437e-24, - "angularVelocity": -1.1838680158880217e-23, - "velocityX": 1.4771703851722756e-18, - "velocityY": -0.2371560993893536, - "timestamp": 19.144218294235284 - }, - { - "x": 7.473632750557841e-19, - "y": 1.8799999899895934, - "heading": -7.433822752952983e-24, - "angularVelocity": -1.1277360292414862e-23, - "velocityX": 1.6881988369545231e-18, - "velocityY": -0.27103554124926305, - "timestamp": 19.24260638230452 - }, - { - "x": 9.342088191957516e-19, - "y": 1.8499999878083944, - "heading": -8.289463274563344e-24, - "angularVelocity": -8.696581833507738e-24, - "velocityX": 1.899228638237177e-18, - "velocityY": -0.304914982798406, - "timestamp": 19.340994470373754 - }, - { - "x": 1.1418157905706857e-18, - "y": 1.8166666521829742, - "heading": -8.791094438531766e-24, - "angularVelocity": -5.098477457853364e-24, - "velocityX": 2.1102598959558546e-18, - "velocityY": -0.33879442399534687, - "timestamp": 19.43938255844299 - }, - { - "x": 1.3701843449632601e-18, - "y": 1.7799999831529358, - "heading": -8.958538336930905e-24, - "angularVelocity": -1.7018687746695977e-24, - "velocityX": 2.3212927299416833e-18, - "velocityY": -0.37267386478977105, - "timestamp": 19.537770646512225 - }, - { - "x": 1.6193146513766686e-18, - "y": 1.7399999807639748, - "heading": -8.662387950504875e-24, - "angularVelocity": 3.010025947478922e-24, - "velocityX": 2.532327273455358e-18, - "velocityY": -0.406553305119753, - "timestamp": 19.63615873458146 - }, - { - "x": 1.8892068933023243e-18, - "y": 1.6966666450694023, - "heading": -7.763746676879998e-24, - "angularVelocity": 9.133641008650935e-24, - "velocityX": 2.743363672401436e-18, - "velocityY": -0.44043274490788564, - "timestamp": 19.734546822650696 - }, - { - "x": 2.179861269683059e-18, - "y": 1.6499999761322235, - "heading": -5.979891084403687e-24, - "angularVelocity": 1.8130823090544997e-23, - "velocityX": 2.9544020820640314e-18, - "velocityY": -0.47431218405565095, - "timestamp": 19.83293491071993 - }, - { - "x": 2.491277994923378e-18, - "y": 1.5999999740280433, - "heading": -3.1318625511206987e-24, - "angularVelocity": 2.8946883296905834e-23, - "velocityX": 3.1654426573706926e-18, - "velocityY": -0.5081916224349754, - "timestamp": 19.931322998789167 - }, - { - "x": 2.8234572966317407e-18, - "y": 1.5466666388492685, - "heading": 1.2746740644139744e-24, - "angularVelocity": 4.47872963412253e-23, - "velocityX": 3.376485529499522e-18, - "velocityY": -0.5420710598750945, - "timestamp": 20.029711086858402 - }, - { - "x": 3.1763994081437363e-18, - "y": 1.4899999707114076, - "heading": 7.137353173597414e-24, - "angularVelocity": 5.958725558661365e-23, - "velocityX": 3.587530752531668e-18, - "velocityY": -0.575950496141207, - "timestamp": 20.128099174927637 - }, - { - "x": 3.550104548721676e-18, - "y": 1.4299999697629708, - "heading": 1.4784368588518567e-23, - "angularVelocity": 7.772294744741366e-23, - "velocityX": 3.798578177501792e-18, - "velocityY": -0.6098299308978822, - "timestamp": 20.226487262996873 - }, - { - "x": 3.944572873439836e-18, - "y": 1.3666666362019724, - "heading": 2.4685522368597076e-23, - "angularVelocity": 1.0063362932558388e-22, - "velocityX": 4.009627143145663e-18, - "velocityY": -0.643709363641975, - "timestamp": 20.324875351066108 - }, - { - "x": 4.35980434250334e-18, - "y": 1.2999999703056322, - "heading": 3.740510491747365e-23, - "angularVelocity": 1.2927969167310106e-22, - "velocityX": 4.220675654736974e-18, - "velocityY": -0.6775887935684523, - "timestamp": 20.423263439135344 - }, - { - "x": 4.7957983473533305e-18, - "y": 1.22999997248978, - "heading": 5.318650036063457e-23, - "angularVelocity": 1.6039947437582512e-22, - "velocityX": 4.431717906486655e-18, - "velocityY": -0.7114682192685073, - "timestamp": 20.52165152720458 - }, - { - "x": 5.252552446679876e-18, - "y": 1.1566666434474648, - "heading": 7.27694765189339e-23, - "angularVelocity": 1.9903820911278035e-22, - "velocityX": 4.642735221235561e-18, - "velocityY": -0.7453476379245281, - "timestamp": 20.620039615273814 - }, - { - "x": 5.730056595973869e-18, - "y": 1.079999984564783, - "heading": 1.0096630665085409e-22, - "angularVelocity": 2.865878607555212e-22, - "velocityX": 4.853650197261181e-18, - "velocityY": -0.7792270424924945, - "timestamp": 20.71842770334305 - }, - { - "x": 6.2282427168358e-18, - "y": 1, - "heading": 1.3642695798435716e-22, - "angularVelocity": 3.6041629217015064e-22, - "velocityX": 5.063872492830655e-18, - "velocityY": -0.8131064047965492, - "timestamp": 20.816815791412285 - }, - { - "x": 6.183163776953066e-18, - "y": 0.9200000154352174, - "heading": 1.0949873427318893e-22, - "angularVelocity": -2.7369386218952554e-22, - "velocityX": -4.58180081175704e-19, - "velocityY": -0.8131064047965493, - "timestamp": 20.91520387948152 - }, - { - "x": 6.117523635853445e-18, - "y": 0.8433333565525356, - "heading": 9.207244155443994e-23, - "angularVelocity": -1.771178319990209e-22, - "velocityX": -6.671778045636001e-19, - "velocityY": -0.7792270424924946, - "timestamp": 21.013591967550756 - }, - { - "x": 6.031184241871937e-18, - "y": 0.7700000275102202, - "heading": 7.870338506115835e-23, - "angularVelocity": -1.3588087365283406e-22, - "velocityX": -8.775779476826745e-19, - "velocityY": -0.7453476379245282, - "timestamp": 21.11198005561999 - }, - { - "x": 5.924117435021876e-18, - "y": 0.7000000296943679, - "heading": 6.771187556227619e-23, - "angularVelocity": -1.1171597807921007e-22, - "velocityX": -1.0882640657264913e-18, - "velocityY": -0.7114682192685075, - "timestamp": 21.210368143689227 - }, - { - "x": 5.796312293836418e-18, - "y": 0.6333333637980278, - "heading": 5.866324558507834e-23, - "angularVelocity": -9.196882191601766e-23, - "velocityX": -1.2990610684817378e-18, - "velocityY": -0.6775887935684524, - "timestamp": 21.308756231758462 - }, - { - "x": 5.647763255268458e-18, - "y": 0.5700000302370295, - "heading": 5.1400871049015666e-23, - "angularVelocity": -7.381359399345974e-23, - "velocityX": -1.5099145415853494e-18, - "velocityY": -0.6437093636419751, - "timestamp": 21.407144319827697 - }, - { - "x": 5.4784670203584905e-18, - "y": 0.5100000292885928, - "heading": 4.54530402227702e-23, - "angularVelocity": -6.045276480742016e-23, - "velocityX": -1.7208014970199695e-18, - "velocityY": -0.6098299308978823, - "timestamp": 21.505532407896933 - }, - { - "x": 5.288421433894345e-18, - "y": 0.45333336115073164, - "heading": 4.030799828317159e-23, - "angularVelocity": -5.2293407257414914e-23, - "velocityX": -1.9317103236247645e-18, - "velocityY": -0.5759504961412071, - "timestamp": 21.603920495966168 - }, - { - "x": 5.077624990656839e-18, - "y": 0.4000000259719569, - "heading": 3.58579938101029e-23, - "angularVelocity": -4.5229107826418413e-23, - "velocityX": -2.1426344234041997e-18, - "velocityY": -0.5420710598750946, - "timestamp": 21.702308584035404 - }, - { - "x": 4.846076586985391e-18, - "y": 0.3500000238677767, - "heading": 3.180712014523294e-23, - "angularVelocity": -4.1172381156419403e-23, - "velocityX": -2.353569720709197e-18, - "velocityY": -0.5081916224349755, - "timestamp": 21.80069667210464 - }, - { - "x": 4.593775383282716e-18, - "y": 0.3033333549305977, - "heading": 2.770876605483693e-23, - "angularVelocity": -4.165496316836929e-23, - "velocityX": -2.564513535686176e-18, - "velocityY": -0.474312184055651, - "timestamp": 21.899084760173874 - }, - { - "x": 4.320720722178402e-18, - "y": 0.26000001923602545, - "heading": 2.3786765227246354e-23, - "angularVelocity": -3.986256457760449e-23, - "velocityX": -2.7754640190164052e-18, - "velocityY": -0.4404327449078857, - "timestamp": 21.99747284824311 - }, - { - "x": 4.02691207701313e-18, - "y": 0.22000001684706427, - "heading": 2.020229881065505e-23, - "angularVelocity": -3.6431920742617227e-23, - "velocityX": -2.9864198443486014e-18, - "velocityY": -0.4065533051197531, - "timestamp": 22.095860936312345 - }, - { - "x": 3.712349017892417e-18, - "y": 0.18333334781702584, - "heading": 1.6882521656893888e-23, - "angularVelocity": -3.374167614218178e-23, - "velocityX": -3.197380029916969e-18, - "velocityY": -0.37267386478977116, - "timestamp": 22.19424902438158 - }, - { - "x": 3.377031188434149e-18, - "y": 0.15000001219160564, - "heading": 1.3838247028470944e-23, - "angularVelocity": -3.0941513829444673e-23, - "velocityX": -3.4083438298267012e-18, - "velocityY": -0.33879442399534687, - "timestamp": 22.292637112450816 - }, - { - "x": 3.0209582893555436e-18, - "y": 0.12000001001040657, - "heading": 1.1063576902336286e-23, - "angularVelocity": -2.8201330931181093e-23, - "velocityX": -3.619310664732038e-18, - "velocityY": -0.304914982798406, - "timestamp": 22.39102520052005 - }, - { - "x": 2.6441300665755467e-18, - "y": 0.09333334130808112, - "heading": 8.606488657282892e-24, - "angularVelocity": -2.4973453877481673e-23, - "velocityX": -3.83028007615491e-18, - "velocityY": -0.27103554124926305, - "timestamp": 22.489413288589287 - }, - { - "x": 2.2465463023701263e-18, - "y": 0.07000000611520496, - "heading": 6.4325429496626686e-24, - "angularVelocity": -2.2095612525455652e-23, - "velocityX": -4.041251695195559e-18, - "velocityY": -0.23715609938935361, - "timestamp": 22.587801376658522 - }, - { - "x": 1.8282068086937753e-18, - "y": 0.05000000445895656, - "heading": 4.560020147095621e-24, - "angularVelocity": -1.9031993197120398e-23, - "velocityX": -4.252225220837285e-18, - "velocityY": -0.20327665725320723, - "timestamp": 22.686189464727757 - }, - { - "x": 1.3891114220101059e-18, - "y": 0.033333336363653446, - "heading": 3.049320875422628e-24, - "angularVelocity": -1.5354485508607965e-23, - "velocityX": -4.463200404415354e-18, - "velocityY": -0.16939721486990153, - "timestamp": 22.784577552796993 - }, - { - "x": 9.292599992741813e-19, - "y": 0.02000000185118139, - "heading": 1.8599640085541418e-24, - "angularVelocity": -1.2088421222550768e-23, - "velocityX": -4.674177038146959e-18, - "velocityY": -0.13551777226415238, - "timestamp": 22.882965640866228 - }, - { - "x": 4.48652414721063e-19, - "y": 0.010000000941341822, - "heading": 9.448880778978016e-25, - "angularVelocity": -9.300681800302624e-24, - "velocityX": -4.885154946696719e-18, - "velocityY": -0.10163832945714489, - "timestamp": 22.981353728935463 - }, - { - "x": -5.271144269665744e-20, - "y": 0.0033333336521360374, - "heading": 3.2591541895178886e-25, - "angularVelocity": -6.291130188677765e-24, - "velocityX": -5.096133980824773e-18, - "velocityY": -0.06775888646717527, - "timestamp": 23.0797418170047 - }, - { - "x": -5.748316713988906e-19, - "y": -4.132250866556525e-18, - "heading": -1.2567862526727601e-29, - "angularVelocity": -3.3126782172732724e-24, - "velocityX": -5.307114012445354e-18, - "velocityY": -0.03387944331015331, - "timestamp": 23.178129905073934 - }, - { - "x": -1.1177083588923526e-18, - "y": -2.588370317815696e-18, - "heading": -1.4472783150300173e-29, - "angularVelocity": -1.9354755320059638e-29, - "velocityX": -5.51809493087181e-18, - "velocityY": 7.443696375437466e-19, - "timestamp": 23.27651799314317 - }, - { - "x": 0.0023781215211549377, - "y": 0.0023781215211549364, - "heading": -5.418034508365842e-21, - "angularVelocity": -5.482324599836796e-20, - "velocityX": 0.0240633670172817, - "velocityY": 0.024063367017281703, - "timestamp": 23.375345456458966 - }, - { - "x": 0.007134364554539963, - "y": 0.007134364554539962, - "heading": 2.784928879289317e-19, - "angularVelocity": 2.872793614299252e-18, - "velocityX": 0.04812673394425598, - "velocityY": 0.04812673394425598, - "timestamp": 23.474172919774762 - }, - { - "x": 0.01426872909055665, - "y": 0.014268729090556648, - "heading": 2.6290919679734843e-19, - "angularVelocity": -1.5768594792595913e-19, - "velocityX": 0.07219010077410719, - "velocityY": 0.07219010077410717, - "timestamp": 23.57300038309056 - }, - { - "x": 0.02378121511885375, - "y": 0.023781215118853744, - "heading": 2.4261266154684275e-19, - "angularVelocity": -2.053734849348351e-19, - "velocityX": 0.09625346749921782, - "velocityY": 0.0962534674992178, - "timestamp": 23.671827846406355 - }, - { - "x": 0.03567182262823502, - "y": 0.03567182262823501, - "heading": -1.6278307756301597e-19, - "angularVelocity": -4.102055402193371e-18, - "velocityX": 0.12031683411103763, - "velocityY": 0.12031683411103761, - "timestamp": 23.77065530972215 - }, - { - "x": 0.049940551606551344, - "y": 0.04994055160655134, - "heading": -8.668505353601748e-19, - "angularVelocity": -7.124208314248421e-18, - "velocityX": 0.14438020059992485, - "velocityY": 0.14438020059992482, - "timestamp": 23.869482773037948 - }, - { - "x": 0.06658740204057369, - "y": 0.06658740204057367, - "heading": -1.2802085811795093e-18, - "angularVelocity": -4.182623038241345e-18, - "velocityX": 0.16844356695495213, - "velocityY": 0.1684435669549521, - "timestamp": 23.968310236353744 - }, - { - "x": 0.0856123739158424, - "y": 0.08561237391584238, - "heading": -1.394541841050903e-18, - "angularVelocity": -1.1568975558424374e-18, - "velocityX": 0.1925069331636674, - "velocityY": 0.19250693316366735, - "timestamp": 24.06713769966954 - }, - { - "x": 0.10701546721648714, - "y": 0.10701546721648711, - "heading": -1.5119540621698801e-18, - "angularVelocity": -1.1880524815792804e-18, - "velocityX": 0.21657029921179627, - "velocityY": 0.21657029921179627, - "timestamp": 24.165965162985337 - }, - { - "x": 0.13079668192500984, - "y": 0.1307966819250098, - "heading": -1.9265469652509166e-18, - "angularVelocity": -4.195118191853065e-18, - "velocityX": 0.2406336650828684, - "velocityY": 0.2406336650828684, - "timestamp": 24.264792626301134 - }, - { - "x": 0.15695601802202072, - "y": 0.15695601802202067, - "heading": -2.1496546222771247e-18, - "angularVelocity": -2.2575471033153812e-18, - "velocityX": 0.26469703075774254, - "velocityY": 0.2646970307577425, - "timestamp": 24.36362008961693 - }, - { - "x": 0.1854934754859141, - "y": 0.18549347548591405, - "heading": -2.374412701432123e-18, - "angularVelocity": -2.274247218263183e-18, - "velocityX": 0.28876039621399596, - "velocityY": 0.2887603962139959, - "timestamp": 24.462447552932726 - }, - { - "x": 0.21640905429246535, - "y": 0.2164090542924653, - "heading": -2.8947586701185075e-18, - "angularVelocity": -5.265196027705901e-18, - "velocityX": 0.3128237614251293, - "velocityY": 0.31282376142512924, - "timestamp": 24.561275016248523 - }, - { - "x": 0.24970275441432413, - "y": 0.24970275441432405, - "heading": -3.4154953875764575e-18, - "angularVelocity": -5.2691499554024646e-18, - "velocityX": 0.33688712635951407, - "velocityY": 0.33688712635951407, - "timestamp": 24.66010247956432 - }, - { - "x": 0.2853745758203676, - "y": 0.28537457582036757, - "heading": -3.908799891042607e-18, - "angularVelocity": -4.9915731422981935e-18, - "velocityX": 0.36095049097897813, - "velocityY": 0.3609504909789781, - "timestamp": 24.758929942880116 - }, - { - "x": 0.3234245184748622, - "y": 0.32342451847486214, - "heading": -4.106333508592265e-18, - "angularVelocity": -1.998772747832259e-18, - "velocityX": 0.38501385523686654, - "velocityY": 0.38501385523686654, - "timestamp": 24.857757406195912 - }, - { - "x": 0.3638525823363569, - "y": 0.36385258233635687, - "heading": -4.301612868352148e-18, - "angularVelocity": -1.9759627937070713e-18, - "velocityX": 0.4090772190753272, - "velocityY": 0.4090772190753271, - "timestamp": 24.95658486951171 - }, - { - "x": 0.4066587673561919, - "y": 0.40665876735619183, - "heading": -4.4934499230572835e-18, - "angularVelocity": -1.9411313422842306e-18, - "velocityX": 0.4331405824214162, - "velocityY": 0.4331405824214161, - "timestamp": 25.055412332827505 - }, - { - "x": 0.45184307347643876, - "y": 0.4518430734764387, - "heading": -4.38590045106494e-18, - "angularVelocity": 1.0882545061626265e-18, - "velocityX": 0.4572039451813483, - "velocityY": 0.45720394518134827, - "timestamp": 25.1542397961433 - }, - { - "x": 0.49940550062697375, - "y": 0.49940550062697364, - "heading": -4.075459454626922e-18, - "angularVelocity": 3.1412417917185753e-18, - "velocityX": 0.48126730723172234, - "velocityY": 0.48126730723172234, - "timestamp": 25.253067259459097 - }, - { - "x": 0.5493460487211731, - "y": 0.549346048721173, - "heading": -3.461804526195924e-18, - "angularVelocity": 6.209355725483015e-18, - "velocityX": 0.5053306684055835, - "velocityY": 0.5053306684055834, - "timestamp": 25.351894722774894 - }, - { - "x": 0.6016647176493138, - "y": 0.6016647176493137, - "heading": -3.1313136201207807e-18, - "angularVelocity": 3.3441196837755998e-18, - "velocityX": 0.5293940284691947, - "velocityY": 0.5293940284691947, - "timestamp": 25.45072218609069 - }, - { - "x": 0.6563615072679119, - "y": 0.6563615072679118, - "heading": -2.785953977666397e-18, - "angularVelocity": 3.494571203940914e-18, - "velocityX": 0.5534573870809412, - "velocityY": 0.5534573870809412, - "timestamp": 25.549549649406487 - }, - { - "x": 0.7134364173813069, - "y": 0.7134364173813067, - "heading": -2.715478020612081e-18, - "angularVelocity": 7.131208814821819e-19, - "velocityX": 0.5775207437128721, - "velocityY": 0.577520743712872, - "timestamp": 25.648377112722283 - }, - { - "x": 0.772889447706879, - "y": 0.7728894477068786, - "heading": -2.308002641013774e-18, - "angularVelocity": 4.123098255203255e-18, - "velocityX": 0.60158409748507, - "velocityY": 0.60158409748507, - "timestamp": 25.74720457603808 - }, - { - "x": 0.8347205978005107, - "y": 0.8347205978005103, - "heading": -2.1882520138105614e-18, - "angularVelocity": 1.2117138211518718e-18, - "velocityX": 0.6256474467634036, - "velocityY": 0.6256474467634034, - "timestamp": 25.846032039353876 - }, - { - "x": 0.8989298668627916, - "y": 0.8989298668627914, - "heading": -2.600558892907419e-18, - "angularVelocity": -4.171986778366762e-18, - "velocityX": 0.6497107879527877, - "velocityY": 0.6497107879527876, - "timestamp": 25.944859502669672 - }, - { - "x": 0.9655172530284372, - "y": 0.9655172530284369, - "heading": -2.9104158106151282e-18, - "angularVelocity": -3.1353319319610485e-18, - "velocityX": 0.6737741102680166, - "velocityY": 0.6737741102680165, - "timestamp": 26.04368696598547 - }, - { - "x": 1.034482746971563, - "y": 1.0344827469715627, - "heading": -2.8870355286032125e-18, - "angularVelocity": 2.365768387844758e-19, - "velocityX": 0.6978373382179309, - "velocityY": 0.6978373382179308, - "timestamp": 26.142514429301265 - }, - { - "x": 1.1010701331372088, - "y": 1.1010701331372084, - "heading": -2.5044778062473253e-18, - "angularVelocity": 3.870965844879766e-18, - "velocityX": 0.6737741102680164, - "velocityY": 0.6737741102680164, - "timestamp": 26.24134189261706 - }, - { - "x": 1.1652794021994897, - "y": 1.1652794021994894, - "heading": -1.9025969173015906e-18, - "angularVelocity": 6.090218909244497e-18, - "velocityX": 0.6497107879527875, - "velocityY": 0.6497107879527875, - "timestamp": 26.340169355932858 - }, - { - "x": 1.2271105522931214, - "y": 1.2271105522931212, - "heading": -9.264679815140483e-19, - "angularVelocity": 9.877101951914267e-18, - "velocityX": 0.6256474467634033, - "velocityY": 0.6256474467634033, - "timestamp": 26.438996819248654 - }, - { - "x": 1.2865635826186934, - "y": 1.2865635826186932, - "heading": -1.846677135454343e-19, - "angularVelocity": 7.506013398090202e-18, - "velocityX": 0.60158409748507, - "velocityY": 0.6015840974850699, - "timestamp": 26.53782428256445 - }, - { - "x": 1.3436384927320884, - "y": 1.3436384927320884, - "heading": 2.985033438459339e-19, - "angularVelocity": 4.889036286936694e-18, - "velocityX": 0.5775207437128721, - "velocityY": 0.577520743712872, - "timestamp": 26.636651745880247 - }, - { - "x": 1.3983352823506863, - "y": 1.3983352823506863, - "heading": 5.055652462787692e-19, - "angularVelocity": 2.095185898822513e-18, - "velocityX": 0.5534573870809412, - "velocityY": 0.553457387080941, - "timestamp": 26.735479209196043 - }, - { - "x": 1.450653951278827, - "y": 1.450653951278827, - "heading": 5.279093823877594e-19, - "angularVelocity": 2.260923516848411e-19, - "velocityX": 0.5293940284691947, - "velocityY": 0.5293940284691946, - "timestamp": 26.83430667251184 - }, - { - "x": 1.5005944993730265, - "y": 1.5005944993730265, - "heading": 5.844300351339039e-19, - "angularVelocity": 5.719124628116133e-19, - "velocityX": 0.5053306684055834, - "velocityY": 0.5053306684055834, - "timestamp": 26.933134135827636 - }, - { - "x": 1.5481569265235613, - "y": 1.5481569265235613, - "heading": 3.779197885193655e-19, - "angularVelocity": -2.0896037166931965e-18, - "velocityX": 0.4812673072317223, - "velocityY": 0.4812673072317223, - "timestamp": 27.031961599143433 - }, - { - "x": 1.5933412326438083, - "y": 1.5933412326438083, - "heading": 2.039493487749352e-20, - "angularVelocity": -3.6176667234042734e-18, - "velocityX": 0.45720394518134827, - "velocityY": 0.45720394518134827, - "timestamp": 27.13078906245923 - }, - { - "x": 1.6361474176636432, - "y": 1.6361474176636432, - "heading": -6.968095113053582e-21, - "angularVelocity": -2.7687653315843575e-19, - "velocityX": 0.43314058242141606, - "velocityY": 0.43314058242141606, - "timestamp": 27.229616525775025 - }, - { - "x": 1.676575481525138, - "y": 1.676575481525138, - "heading": 1.028814691598036e-19, - "angularVelocity": 1.111528889365859e-18, - "velocityX": 0.4090772190753271, - "velocityY": 0.4090772190753271, - "timestamp": 27.32844398909082 - }, - { - "x": 1.7146254241796324, - "y": 1.7146254241796324, - "heading": -5.672999077177864e-20, - "angularVelocity": -1.6150514296064684e-18, - "velocityX": 0.3850138552368665, - "velocityY": 0.3850138552368665, - "timestamp": 27.427271452406618 - }, - { - "x": 1.7502972455856758, - "y": 1.750297245585676, - "heading": -1.922855653141426e-19, - "angularVelocity": -1.371638460144624e-18, - "velocityX": 0.36095049097897813, - "velocityY": 0.3609504909789781, - "timestamp": 27.526098915722415 - }, - { - "x": 1.7835909457075345, - "y": 1.7835909457075347, - "heading": -3.046581816575936e-19, - "angularVelocity": -1.1370583307666246e-18, - "velocityX": 0.33688712635951407, - "velocityY": 0.336887126359514, - "timestamp": 27.62492637903821 - }, - { - "x": 1.814506524514086, - "y": 1.8145065245140861, - "heading": -1.0007879634751174e-19, - "angularVelocity": 2.0700663400412595e-18, - "velocityX": 0.3128237614251293, - "velocityY": 0.3128237614251292, - "timestamp": 27.723753842354007 - }, - { - "x": 1.8430439819779791, - "y": 1.8430439819779794, - "heading": 4.207844233499018e-19, - "angularVelocity": 5.2704300636110826e-18, - "velocityX": 0.28876039621399596, - "velocityY": 0.2887603962139959, - "timestamp": 27.822581305669804 - }, - { - "x": 1.86920331807499, - "y": 1.8692033180749903, - "heading": 1.25733988172306e-18, - "angularVelocity": 8.46480757118372e-18, - "velocityX": 0.2646970307577425, - "velocityY": 0.2646970307577425, - "timestamp": 27.9214087689856 - }, - { - "x": 1.8929845327835126, - "y": 1.8929845327835129, - "heading": 1.52547262188251e-18, - "angularVelocity": 2.713140055177162e-18, - "velocityX": 0.24063366508286838, - "velocityY": 0.24063366508286835, - "timestamp": 28.020236232301396 - }, - { - "x": 1.9143876260841575, - "y": 1.9143876260841575, - "heading": 1.5351626707181141e-18, - "angularVelocity": 9.805025681151685e-20, - "velocityX": 0.21657029921179624, - "velocityY": 0.21657029921179624, - "timestamp": 28.119063695617193 - }, - { - "x": 1.9334125979594263, - "y": 1.9334125979594263, - "heading": 1.5661943887692378e-18, - "angularVelocity": 3.139989977967844e-19, - "velocityX": 0.19250693316366735, - "velocityY": 0.19250693316366735, - "timestamp": 28.21789115893299 - }, - { - "x": 1.9500594483934486, - "y": 1.9500594483934488, - "heading": 1.9111016259033982e-18, - "angularVelocity": 3.48999383435869e-18, - "velocityX": 0.1684435669549521, - "velocityY": 0.1684435669549521, - "timestamp": 28.316718622248786 - }, - { - "x": 1.9643281773717647, - "y": 1.9643281773717651, - "heading": 1.8525436426975768e-18, - "angularVelocity": -5.925273851352638e-19, - "velocityX": 0.14438020059992485, - "velocityY": 0.14438020059992482, - "timestamp": 28.415546085564582 - }, - { - "x": 1.976218784881146, - "y": 1.9762187848811463, - "heading": 1.5181268192693785e-18, - "angularVelocity": -3.383844909664733e-18, - "velocityX": 0.12031683411103762, - "velocityY": 0.12031683411103762, - "timestamp": 28.51437354888038 - }, - { - "x": 1.9857312709094432, - "y": 1.9857312709094435, - "heading": 1.218007859467631e-18, - "angularVelocity": -3.03679700352538e-18, - "velocityX": 0.09625346749921782, - "velocityY": 0.09625346749921782, - "timestamp": 28.613201012196175 - }, - { - "x": 1.9928656354454601, - "y": 1.99286563544546, - "heading": 6.414597240846549e-19, - "angularVelocity": -5.833885501885998e-18, - "velocityX": 0.07219010077410719, - "velocityY": 0.07219010077410719, - "timestamp": 28.71202847551197 - }, - { - "x": 1.997621878478845, - "y": 1.9976218784788449, - "heading": 3.1192356934373525e-19, - "angularVelocity": -3.3344590469642965e-18, - "velocityX": 0.04812673394425598, - "velocityY": 0.04812673394425598, - "timestamp": 28.810855938827768 - }, - { - "x": 2, - "y": 2, - "heading": 3.095704809390742e-29, - "angularVelocity": -3.1562436810261706e-18, - "velocityX": 0.024063367017281703, - "velocityY": 0.0240633670172817, - "timestamp": 28.909683402143564 - }, - { - "x": 2, - "y": 2, - "heading": 9.24554742891244e-30, - "angularVelocity": -7.339590862305549e-29, - "velocityX": -1.9532349903527946e-19, - "velocityY": -1.953180778215209e-19, - "timestamp": 29.00851086545936 - }, - { - "x": 1.997621878478845, - "y": 1.997621878478845, - "heading": 2.9176550698701047e-19, - "angularVelocity": 2.952271403840556e-18, - "velocityX": -0.024063367017281706, - "velocityY": -0.0240633670172817, - "timestamp": 29.107338328775157 - }, - { - "x": 1.99286563544546, - "y": 1.99286563544546, - "heading": 5.967411588481832e-19, - "angularVelocity": 3.0859402387521106e-18, - "velocityX": -0.04812673394425599, - "velocityY": -0.04812673394425598, - "timestamp": 29.206165792090953 - }, - { - "x": 1.9857312709094432, - "y": 1.9857312709094432, - "heading": 8.990553363945518e-19, - "angularVelocity": 3.0590097309406617e-18, - "velocityX": -0.07219010077410719, - "velocityY": -0.07219010077410717, - "timestamp": 29.30499325540675 - }, - { - "x": 1.9762187848811459, - "y": 1.9762187848811459, - "heading": 1.4932940433223978e-18, - "angularVelocity": 6.01289029503833e-18, - "velocityX": -0.09625346749921782, - "velocityY": -0.0962534674992178, - "timestamp": 29.403820718722546 - }, - { - "x": 1.9643281773717647, - "y": 1.9643281773717647, - "heading": 2.301350529247808e-18, - "angularVelocity": 8.176436550349542e-18, - "velocityX": -0.12031683411103763, - "velocityY": -0.1203168341110376, - "timestamp": 29.502648182038342 - }, - { - "x": 1.9500594483934484, - "y": 1.9500594483934484, - "heading": 3.1069311821375085e-18, - "angularVelocity": 8.151384411515975e-18, - "velocityX": -0.14438020059992485, - "velocityY": -0.1443802005999248, - "timestamp": 29.60147564535414 - }, - { - "x": 1.933412597959426, - "y": 1.933412597959426, - "heading": 3.615579822445298e-18, - "angularVelocity": 5.146834914364682e-18, - "velocityX": -0.1684435669549521, - "velocityY": -0.16844356695495205, - "timestamp": 29.700303108669935 - }, - { - "x": 1.9143876260841575, - "y": 1.9143876260841575, - "heading": 4.121905172237148e-18, - "angularVelocity": 5.123326299163994e-18, - "velocityX": -0.19250693316366735, - "velocityY": -0.1925069331636673, - "timestamp": 29.79913057198573 - }, - { - "x": 1.8929845327835126, - "y": 1.8929845327835126, - "heading": 4.625994654512789e-18, - "angularVelocity": 5.100702286243361e-18, - "velocityX": -0.2165702992117962, - "velocityY": -0.21657029921179619, - "timestamp": 29.897958035301528 - }, - { - "x": 1.8692033180749899, - "y": 1.8692033180749899, - "heading": 5.127944634100554e-18, - "angularVelocity": 5.079053493036733e-18, - "velocityX": -0.24063366508286835, - "velocityY": -0.24063366508286832, - "timestamp": 29.996785498617324 - }, - { - "x": 1.8430439819779791, - "y": 1.8430439819779791, - "heading": 5.333332886477307e-18, - "angularVelocity": 2.0782507129575468e-18, - "velocityX": -0.2646970307577425, - "velocityY": -0.2646970307577424, - "timestamp": 30.09561296193312 - }, - { - "x": 1.8145065245140857, - "y": 1.8145065245140857, - "heading": 5.2582030051247135e-18, - "angularVelocity": -7.602127045758556e-19, - "velocityX": -0.2887603962139959, - "velocityY": -0.28876039621399585, - "timestamp": 30.194440425248917 - }, - { - "x": 1.7835909457075345, - "y": 1.7835909457075345, - "heading": 5.181292957950482e-18, - "angularVelocity": -7.782256292200617e-19, - "velocityX": -0.3128237614251292, - "velocityY": -0.3128237614251292, - "timestamp": 30.293267888564714 - }, - { - "x": 1.7502972455856758, - "y": 1.7502972455856758, - "heading": 5.1027534114594344e-18, - "angularVelocity": -7.947139485337703e-19, - "velocityX": -0.336887126359514, - "velocityY": -0.33688712635951396, - "timestamp": 30.39209535188051 - }, - { - "x": 1.7146254241796322, - "y": 1.7146254241796322, - "heading": 5.022755773866078e-18, - "angularVelocity": -8.094678491276822e-19, - "velocityX": -0.360950490978978, - "velocityY": -0.360950490978978, - "timestamp": 30.490922815196306 - }, - { - "x": 1.6765754815251377, - "y": 1.6765754815251377, - "heading": 4.646967759716646e-18, - "angularVelocity": -3.802465605838998e-18, - "velocityX": -0.38501385523686643, - "velocityY": -0.38501385523686643, - "timestamp": 30.589750278512103 - }, - { - "x": 1.636147417663643, - "y": 1.6361474176636428, - "heading": 4.5646761078242135e-18, - "angularVelocity": -8.326802921818095e-19, - "velocityX": -0.40907721907532707, - "velocityY": -0.40907721907532707, - "timestamp": 30.6885777418279 - }, - { - "x": 1.5933412326438081, - "y": 1.593341232643808, - "heading": 4.481619773950942e-18, - "angularVelocity": -8.404179011597725e-19, - "velocityX": -0.433140582421416, - "velocityY": -0.433140582421416, - "timestamp": 30.787405205143695 - }, - { - "x": 1.548156926523561, - "y": 1.548156926523561, - "heading": 4.69264716566721e-18, - "angularVelocity": 2.135310765625931e-18, - "velocityX": -0.4572039451813482, - "velocityY": -0.4572039451813482, - "timestamp": 30.886232668459492 - }, - { - "x": 1.5005944993730262, - "y": 1.5005944993730262, - "heading": 5.1981444287811744e-18, - "angularVelocity": 5.114946741660759e-18, - "velocityX": -0.48126730723172223, - "velocityY": -0.48126730723172223, - "timestamp": 30.98506013177529 - }, - { - "x": 1.450653951278827, - "y": 1.4506539512788268, - "heading": 5.998588984822106e-18, - "angularVelocity": 8.099413645686185e-18, - "velocityX": -0.5053306684055833, - "velocityY": -0.5053306684055833, - "timestamp": 31.083887595091085 - }, - { - "x": 1.3983352823506863, - "y": 1.398335282350686, - "heading": 6.800057219972362e-18, - "angularVelocity": 8.109771959924199e-18, - "velocityX": -0.5293940284691946, - "velocityY": -0.5293940284691946, - "timestamp": 31.18271505840688 - }, - { - "x": 1.3436384927320881, - "y": 1.3436384927320881, - "heading": 7.308809795922512e-18, - "angularVelocity": 5.147886314223004e-18, - "velocityX": -0.553457387080941, - "velocityY": -0.553457387080941, - "timestamp": 31.281542521722677 - }, - { - "x": 1.2865635826186932, - "y": 1.286563582618693, - "heading": 7.52592489681066e-18, - "angularVelocity": 2.1969102927676096e-18, - "velocityX": -0.5775207437128719, - "velocityY": -0.577520743712872, - "timestamp": 31.380369985038474 - }, - { - "x": 1.2271105522931212, - "y": 1.2271105522931212, - "heading": 8.042018199779756e-18, - "angularVelocity": 5.222164534781162e-18, - "velocityX": -0.6015840974850699, - "velocityY": -0.6015840974850699, - "timestamp": 31.47919744835427 - }, - { - "x": 1.1652794021994897, - "y": 1.1652794021994894, - "heading": 8.270479544084847e-18, - "angularVelocity": 2.3117189802704113e-18, - "velocityX": -0.6256474467634033, - "velocityY": -0.6256474467634034, - "timestamp": 31.578024911670067 - }, - { - "x": 1.1010701331372086, - "y": 1.1010701331372084, - "heading": 8.510243075879087e-18, - "angularVelocity": 2.4260818071067678e-18, - "velocityX": -0.6497107879527876, - "velocityY": -0.6497107879527876, - "timestamp": 31.676852374985863 - }, - { - "x": 1.0344827469715632, - "y": 1.034482746971563, - "heading": 8.487156635946621e-18, - "angularVelocity": -2.3360355366177763e-19, - "velocityX": -0.6737741102680165, - "velocityY": -0.6737741102680165, - "timestamp": 31.77567983830166 - }, - { - "x": 0.9655172530284372, - "y": 0.965517253028437, - "heading": 8.5480561716338e-18, - "angularVelocity": 6.162207686069967e-19, - "velocityX": -0.6978373382179308, - "velocityY": -0.6978373382179308, - "timestamp": 31.874507301617456 - }, - { - "x": 0.8989298668627916, - "y": 0.8989298668627914, - "heading": 8.406703773978591e-18, - "angularVelocity": -1.4302945801324534e-18, - "velocityX": -0.6737741102680165, - "velocityY": -0.6737741102680164, - "timestamp": 31.973334764933252 - }, - { - "x": 0.8347205978005106, - "y": 0.8347205978005104, - "heading": 8.29516939176197e-18, - "angularVelocity": -1.1285766777045508e-18, - "velocityX": -0.6497107879527876, - "velocityY": -0.6497107879527875, - "timestamp": 32.07216222824905 - }, - { - "x": 0.7728894477068788, - "y": 0.7728894477068787, - "heading": 8.497700879239251e-18, - "angularVelocity": 2.0493442633946093e-18, - "velocityX": -0.6256474467634034, - "velocityY": -0.6256474467634033, - "timestamp": 32.17098969156484 - }, - { - "x": 0.7134364173813069, - "y": 0.7134364173813067, - "heading": 8.715363483429085e-18, - "angularVelocity": 2.2024506538239383e-18, - "velocityX": -0.6015840974850699, - "velocityY": -0.6015840974850699, - "timestamp": 32.269817154880634 - }, - { - "x": 0.6563615072679118, - "y": 0.6563615072679116, - "heading": 8.945709750816389e-18, - "angularVelocity": 2.330792062318582e-18, - "velocityX": -0.577520743712872, - "velocityY": -0.577520743712872, - "timestamp": 32.36864461819643 - }, - { - "x": 0.6016647176493137, - "y": 0.6016647176493136, - "heading": 9.481711280544972e-18, - "angularVelocity": 5.4236090257494665e-18, - "velocityX": -0.553457387080941, - "velocityY": -0.553457387080941, - "timestamp": 32.46747208151222 - }, - { - "x": 0.549346048721173, - "y": 0.5493460487211729, - "heading": 9.733232170684809e-18, - "angularVelocity": 2.5450506077548204e-18, - "velocityX": -0.5293940284691947, - "velocityY": -0.5293940284691946, - "timestamp": 32.56629954482801 - }, - { - "x": 0.4994055006269736, - "y": 0.4994055006269735, - "heading": 9.699481252660535e-18, - "angularVelocity": -3.415135021970597e-19, - "velocityX": -0.5053306684055833, - "velocityY": -0.5053306684055833, - "timestamp": 32.665127008143806 - }, - { - "x": 0.45184307347643865, - "y": 0.45184307347643854, - "heading": 9.37985428465038e-18, - "angularVelocity": -3.234191651445345e-18, - "velocityX": -0.4812673072317223, - "velocityY": -0.48126730723172223, - "timestamp": 32.7639544714596 - }, - { - "x": 0.40665876735619183, - "y": 0.4066587673561917, - "heading": 9.362931683815324e-18, - "angularVelocity": -1.712336792194834e-19, - "velocityX": -0.45720394518134827, - "velocityY": -0.4572039451813482, - "timestamp": 32.86278193477539 - }, - { - "x": 0.3638525823363568, - "y": 0.36385258233635676, - "heading": 9.059269008897951e-18, - "angularVelocity": -3.072654581500084e-18, - "velocityX": -0.43314058242141606, - "velocityY": -0.433140582421416, - "timestamp": 32.961609398091184 - }, - { - "x": 0.32342451847486214, - "y": 0.32342451847486203, - "heading": 8.763075814779251e-18, - "angularVelocity": -2.9970734945597385e-18, - "velocityX": -0.40907721907532707, - "velocityY": -0.409077219075327, - "timestamp": 33.06043686140698 - }, - { - "x": 0.28537457582036757, - "y": 0.28537457582036746, - "heading": 8.179554689955981e-18, - "angularVelocity": -5.9044427782956105e-18, - "velocityX": -0.3850138552368665, - "velocityY": -0.38501385523686643, - "timestamp": 33.15926432472277 - }, - { - "x": 0.2497027544143241, - "y": 0.24970275441432402, - "heading": 7.603006050284837e-18, - "angularVelocity": -5.8338906138842846e-18, - "velocityX": -0.3609504909789781, - "velocityY": -0.360950490978978, - "timestamp": 33.25809178803856 - }, - { - "x": 0.21640905429246532, - "y": 0.21640905429246526, - "heading": 7.033233234769523e-18, - "angularVelocity": -5.765328461225564e-18, - "velocityX": -0.336887126359514, - "velocityY": -0.33688712635951396, - "timestamp": 33.356919251354356 - }, - { - "x": 0.18549347548591408, - "y": 0.18549347548591402, - "heading": 6.175535578468071e-18, - "angularVelocity": -8.67873752787532e-18, - "velocityX": -0.31282376142512924, - "velocityY": -0.3128237614251292, - "timestamp": 33.45574671467015 - }, - { - "x": 0.15695601802202072, - "y": 0.15695601802202067, - "heading": 5.3242912654674056e-18, - "angularVelocity": -8.613438444393486e-18, - "velocityX": -0.2887603962139959, - "velocityY": -0.2887603962139959, - "timestamp": 33.55457417798594 - }, - { - "x": 0.13079668192500982, - "y": 0.13079668192500976, - "heading": 4.479366707374499e-18, - "angularVelocity": -8.549491094513284e-18, - "velocityX": -0.26469703075774254, - "velocityY": -0.2646970307577425, - "timestamp": 33.653401641301734 - }, - { - "x": 0.10701546721648711, - "y": 0.10701546721648708, - "heading": 3.64064272114218e-18, - "angularVelocity": -8.486749715566848e-18, - "velocityX": -0.2406336650828684, - "velocityY": -0.24063366508286835, - "timestamp": 33.75222910461753 - }, - { - "x": 0.08561237391584237, - "y": 0.08561237391584235, - "heading": 3.1105036447238866e-18, - "angularVelocity": -5.364288719895572e-18, - "velocityX": -0.2165702992117963, - "velocityY": -0.21657029921179624, - "timestamp": 33.85105656793332 - }, - { - "x": 0.06658740204057367, - "y": 0.06658740204057366, - "heading": 2.5863618381776226e-18, - "angularVelocity": -5.303604412403694e-18, - "velocityX": -0.19250693316366738, - "velocityY": -0.19250693316366735, - "timestamp": 33.94988403124911 - }, - { - "x": 0.04994055160655133, - "y": 0.04994055160655132, - "heading": 2.06812977982204e-18, - "angularVelocity": -5.2438058439965976e-18, - "velocityX": -0.1684435669549521, - "velocityY": -0.16844356695495208, - "timestamp": 34.048711494564905 - }, - { - "x": 0.03567182262823501, - "y": 0.035671822628235005, - "heading": 1.5557277971478494e-18, - "angularVelocity": -5.184813384293766e-18, - "velocityX": -0.14438020059992485, - "velocityY": -0.1443802005999248, - "timestamp": 34.1475389578807 - }, - { - "x": 0.023781215118853744, - "y": 0.02378121511885374, - "heading": 1.2333207349325143e-18, - "angularVelocity": -3.2623223165160492e-18, - "velocityX": -0.12031683411103762, - "velocityY": -0.12031683411103758, - "timestamp": 34.24636642119649 - }, - { - "x": 0.014268729090556646, - "y": 0.014268729090556643, - "heading": 9.166038261970496e-19, - "angularVelocity": -3.2047456798497867e-18, - "velocityX": -0.09625346749921782, - "velocityY": -0.09625346749921779, - "timestamp": 34.345193884512284 - }, - { - "x": 0.0071343645545399615, - "y": 0.00713436455453996, - "heading": 6.055153925645065e-19, - "angularVelocity": -3.1477932139121737e-18, - "velocityX": -0.07219010077410719, - "velocityY": -0.07219010077410716, - "timestamp": 34.44402134782808 - }, - { - "x": 0.0023781215211549364, - "y": 0.0023781215211549356, - "heading": 2.99998430529326e-19, - "angularVelocity": -3.0914174770892124e-18, - "velocityX": -0.04812673394425598, - "velocityY": -0.04812673394425597, - "timestamp": 34.54284881114387 - }, - { - "x": -2.5177140124632005e-18, - "y": -2.5177139656237954e-18, - "heading": 1.1418576549630119e-29, - "angularVelocity": -3.0355774220989457e-18, - "velocityX": -0.024063367017281703, - "velocityY": -0.024063367017281696, - "timestamp": 34.64167627445966 - }, - { - "x": -1.1177094777187861e-18, - "y": -1.1177094535843756e-18, - "heading": 5.214297458751026e-30, - "angularVelocity": -1.0017271530534466e-29, - "velocityX": 2.856704072481815e-18, - "velocityY": 2.856704086919547e-18, - "timestamp": 34.740503737775455 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "scope": [ - 3 - ], - "type": "StopPoint" + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 + }, + { + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 2.462606906890869, + "y": 6.522137641906738, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": -6.079995614896932e-33, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.306947763000364, + "y": 5.574973108257302, + "heading": -0.008994433988091095, + "angularVelocity": -0.12277149964907928, + "velocityX": 0.23168094435597542, + "velocityY": -0.21813883323536412, + "timestamp": 0.07326157955062942 + }, + { + "x": 1.3408944736698363, + "y": 5.543010933982932, + "heading": -0.026978879063552612, + "angularVelocity": -0.24548262794460876, + "velocityX": 0.463363073492191, + "velocityY": -0.4362747086592839, + "timestamp": 0.14652315910125885 + }, + { + "x": 1.3918148440551168, + "y": 5.495068027805583, + "heading": -0.05394057952659419, + "angularVelocity": -0.3680196445178875, + "velocityX": 0.695048764954495, + "velocityY": -0.6544072141417159, + "timestamp": 0.21978473865188827 + }, + { + "x": 1.4597093144768722, + "y": 5.431144700814373, + "heading": -0.08985971213865897, + "angularVelocity": -0.4902860794482582, + "velocityX": 0.9267404666703231, + "velocityY": -0.8725354733449758, + "timestamp": 0.2930463182025177 + }, + { + "x": 1.5445785095442424, + "y": 5.351241370164591, + "heading": -0.13471107898519388, + "angularVelocity": -0.6122085699167754, + "velocityX": 1.1584406941256162, + "velocityY": -1.0906580384956333, + "timestamp": 0.3663078977531471 + }, + { + "x": 1.6464232512218273, + "y": 5.2553586089581215, + "heading": -0.1884659824129613, + "angularVelocity": -0.733739345472597, + "velocityX": 1.390152141167011, + "velocityY": -1.3087727809664234, + "timestamp": 0.4395694773037765 + }, + { + "x": 1.7652446226105383, + "y": 5.143497220947543, + "heading": -0.25109340653071877, + "angularVelocity": -0.8548467628175698, + "velocityX": 1.6218783722318773, + "velocityY": -1.5268765524400676, + "timestamp": 0.5128310568544059 + }, + { + "x": 1.9010443771824763, + "y": 5.015658495720495, + "heading": -0.32255561727496634, + "angularVelocity": -0.9754391207858365, + "velocityX": 1.8536285376988066, + "velocityY": -1.7449627214043517, + "timestamp": 0.5860926364050353 + }, + { + "x": 2.053859595250169, + "y": 4.871864096281773, + "heading": -0.4022671776466062, + "angularVelocity": -1.0880404280193408, + "velocityX": 2.0858848390251463, + "velocityY": -1.9627531964329257, + "timestamp": 0.6593542159556647 + }, + { + "x": 2.1896989198822134, + "y": 4.744047518400832, + "heading": -0.47311179344304544, + "angularVelocity": -0.9670091230763074, + "velocityX": 1.8541686579139196, + "velocityY": -1.7446604163456465, + "timestamp": 0.7326157955062941 + }, + { + "x": 2.308560686135844, + "y": 4.632207510590712, + "heading": -0.535112719027594, + "angularVelocity": -0.846295233666115, + "velocityX": 1.6224297508012064, + "velocityY": -1.5265847187041606, + "timestamp": 0.8058773750569235 + }, + { + "x": 2.410444090432368, + "y": 4.536343322874329, + "heading": -0.5882784511543253, + "angularVelocity": -0.72569732256441, + "velocityX": 1.3906798750648661, + "velocityY": -1.3085192580393974, + "timestamp": 0.8791389546075529 + }, + { + "x": 2.49534860091136, + "y": 4.4564544045860375, + "heading": -0.6326127919617512, + "angularVelocity": -0.6051513095863228, + "velocityX": 1.158922739582985, + "velocityY": -1.090461313806123, + "timestamp": 0.9524005341581823 + }, + { + "x": 2.5632738362111045, + "y": 4.3925403400214185, + "heading": -0.6681173722205684, + "angularVelocity": -0.48462755617056985, + "velocityX": 0.9271603986207089, + "velocityY": -0.8724090438215177, + "timestamp": 1.0256621137088118 + }, + { + "x": 2.6142195241645787, + "y": 4.344600823178412, + "heading": -0.6947927082053813, + "angularVelocity": -0.36411084975827024, + "velocityX": 0.6953943426549579, + "velocityY": -0.6543609506791417, + "timestamp": 1.0989236932594413 + }, + { + "x": 2.6481854848919673, + "y": 4.312635645188571, + "heading": -0.7126387384821254, + "angularVelocity": -0.24359330478823768, + "velocityX": 0.4636258313802147, + "velocityY": -0.43631570853248003, + "timestamp": 1.1721852728100708 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": -0.12307101033443453, + "velocityX": 0.23185602115327572, + "velocityY": -0.2182720825031122, + "timestamp": 1.2454468523607003 + }, + { + "x": 2.662943710448623, + "y": 4.298728558024191, + "heading": -0.72070100113944, + "angularVelocity": 0.011651349233264986, + "velocityX": -0.027206592771411266, + "velocityY": 0.025447590705983056, + "timestamp": 1.3273355622067569 + }, + { + "x": 2.639501593010729, + "y": 4.320770481747221, + "heading": -0.7087160822503835, + "angularVelocity": 0.14635618159801306, + "velocityX": -0.2862680030246291, + "velocityY": 0.26916926355864296, + "timestamp": 1.4092242720528134 + }, + { + "x": 2.594845480264057, + "y": 4.36277075661854, + "heading": -0.6857017147672647, + "angularVelocity": 0.2810444507720759, + "velocityX": -0.5453268567867454, + "velocityY": 0.5128945730159264, + "timestamp": 1.49111298189887 + }, + { + "x": 2.5289756911084322, + "y": 4.424729822216665, + "heading": -0.6516589308540864, + "angularVelocity": 0.4157201154734962, + "velocityX": -0.8043818162412647, + "velocityY": 0.7566252504722968, + "timestamp": 1.5730016917449265 + }, + { + "x": 2.4418926576481637, + "y": 4.5066482714289275, + "heading": -0.6065878922273316, + "angularVelocity": 0.550393805342452, + "velocityX": -1.0634314989694753, + "velocityY": 1.000363168088272, + "timestamp": 1.654890401590983 + }, + { + "x": 2.3335969392795284, + "y": 4.608526864652899, + "heading": -0.5504868101397262, + "angularVelocity": 0.6850893388486693, + "velocityX": -1.322474350520624, + "velocityY": 1.2441103714479649, + "timestamp": 1.7367791114370397 + }, + { + "x": 2.2040892544412327, + "y": 4.730366540742225, + "heading": -0.48334990238896663, + "angularVelocity": 0.8198554828494793, + "velocityX": -1.5815084287169574, + "velocityY": 1.4878690398025962, + "timestamp": 1.8186678212830962 + }, + { + "x": 2.0533705390200705, + "y": 4.8721684079929855, + "heading": -0.4051637468918412, + "angularVelocity": 0.9547855332451622, + "velocityX": -1.840531078148622, + "velocityY": 1.7316412423316419, + "timestamp": 1.9005565311291528 + }, + { + "x": 1.8837222142414454, + "y": 5.031895792612386, + "heading": -0.31535335280985394, + "angularVelocity": 1.0967371967493782, + "velocityX": -2.071693705976669, + "velocityY": 1.9505422044097782, + "timestamp": 1.9824452409752094 + }, + { + "x": 1.7352824931464605, + "y": 5.171658722106698, + "heading": -0.2366561880408508, + "angularVelocity": 0.9610258229363577, + "velocityX": -1.8127006930996745, + "velocityY": 1.706742355045694, + "timestamp": 2.0643339508212657 + }, + { + "x": 1.6080502362813274, + "y": 5.291456457157948, + "heading": -0.16911546139257225, + "angularVelocity": 0.8247867963147685, + "velocityX": -1.5537215948855285, + "velocityY": 1.4629334773555265, + "timestamp": 2.146222660667322 + }, + { + "x": 1.5020244624691528, + "y": 5.391288489879848, + "heading": -0.11277393629567076, + "angularVelocity": 0.6880255556940454, + "velocityX": -1.294754478504957, + "velocityY": 1.219118397512621, + "timestamp": 2.2281113705133784 + }, + { + "x": 1.4172044018013137, + "y": 5.471154488105965, + "heading": -0.06766925261840594, + "angularVelocity": 0.550804668458663, + "velocityX": -1.0357967640141517, + "velocityY": 0.9752992613543129, + "timestamp": 2.3100000803594347 + }, + { + "x": 1.3535895239653573, + "y": 5.531054232218378, + "heading": -0.03382997301748837, + "angularVelocity": 0.4132349827532073, + "velocityX": -0.7768455255376048, + "velocityY": 0.7314774432888099, + "timestamp": 2.391888790205491 + }, + { + "x": 1.3111795494900627, + "y": 5.570987556283822, + "heading": -0.011272655480386088, + "angularVelocity": 0.27546309594458174, + "velocityX": -0.5178976998785506, + "velocityY": 0.48765359889678683, + "timestamp": 2.4737775000515474 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": 0.13765823764445279, + "velocityX": -0.2589502077253713, + "velocityY": 0.24382784263139995, + "timestamp": 2.5556662098976037 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.169901203643428e-30, + "velocityY": 4.744348570353821e-29, + "timestamp": 2.63755491974366 + }, + { + "x": 1.3279764392732873, + "y": 5.589659578477318, + "heading": -3.8286967101155775e-19, + "angularVelocity": -4.101142681562708e-18, + "velocityX": 0.40706315074964167, + "velocityY": -0.013868615044214471, + "timestamp": 2.7309114128077048 + }, + { + "x": 1.4039804148734387, + "y": 5.5870701279768555, + "heading": -4.0712541473530154e-19, + "angularVelocity": -2.5982712070316094e-19, + "velocityX": 0.814126292755424, + "velocityY": -0.027737229790526226, + "timestamp": 2.8242679058717495 + }, + { + "x": 1.517986376641016, + "y": 5.583185952281787, + "heading": 4.833998363566258e-19, + "angularVelocity": 9.538831821902836e-18, + "velocityX": 1.2211894216447965, + "velocityY": -0.04160584408996275, + "timestamp": 2.917624398935794 + }, + { + "x": 1.6699943225350276, + "y": 5.578007051461648, + "heading": 1.3619244496265011e-18, + "angularVelocity": 9.410228943340493e-18, + "velocityX": 1.6282525286718303, + "velocityY": -0.05547445764455082, + "timestamp": 3.010980891999839 + }, + { + "x": 1.8600042484729113, + "y": 5.571533425655531, + "heading": 3.711758987250246e-18, + "angularVelocity": 2.5170201581763002e-17, + "velocityX": 2.0353155919679775, + "velocityY": -0.06934306970923045, + "timestamp": 3.1043373850638836 + }, + { + "x": 2.0880161422023416, + "y": 5.563765075280871, + "heading": -9.287085834288012e-18, + "angularVelocity": -1.392382283532996e-16, + "velocityX": 2.4423785240217755, + "velocityY": -0.08321167730249175, + "timestamp": 3.1976938781279283 + }, + { + "x": 2.2780692584306133, + "y": 5.5572899779843725, + "heading": -5.547928961434018e-18, + "angularVelocity": 4.0052703333031833e-17, + "velocityX": 2.03577823010496, + "velocityY": -0.06935883176043627, + "timestamp": 3.291050371191973 + }, + { + "x": 2.430120406839514, + "y": 5.552109605257364, + "heading": -3.2776580295073633e-18, + "angularVelocity": 2.4318713794052425e-17, + "velocityX": 1.6287152977531212, + "velocityY": -0.05549022415702071, + "timestamp": 3.3844068642560177 + }, + { + "x": 2.5441695751975866, + "y": 5.548223957516569, + "heading": -1.9211838299437728e-18, + "angularVelocity": 1.4530579976060665e-17, + "velocityX": 1.2216522343824612, + "velocityY": -0.04162161208980244, + "timestamp": 3.4777633573200624 + }, + { + "x": 2.6202167594261327, + "y": 5.545633034900951, + "heading": -1.8132356413213059e-19, + "angularVelocity": 1.8636270440618356e-17, + "velocityX": 0.8145891273223099, + "velocityY": -0.02775299853408606, + "timestamp": 3.571119850384107 + }, + { + "x": 2.6582619574855144, + "y": 5.5443368374799995, + "heading": 8.843883513334677e-20, + "angularVelocity": 2.8892814546014257e-18, + "velocityX": 0.40752599841430864, + "velocityY": -0.013884384234014852, + "timestamp": 3.6644763434481518 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 1.7598923552723236e-34, + "angularVelocity": -9.476131898054967e-19, + "velocityX": 0.0004628563966037854, + "velocityY": -0.000015769487296898133, + "timestamp": 3.7578328365121965 + }, + { + "x": 2.620331993851312, + "y": 5.545629108871589, + "heading": 2.945836613273263e-19, + "angularVelocity": 3.1551525697239727e-18, + "velocityX": -0.4066774370940851, + "velocityY": 0.013855473805082683, + "timestamp": 3.8512070216080794 + }, + { + "x": 2.5443424272440374, + "y": 5.548218068458602, + "heading": 2.309166242357916e-19, + "angularVelocity": -6.815922301609038e-19, + "velocityX": -0.8138177218425701, + "velocityY": 0.027726716799615945, + "timestamp": 3.9445812067039623 + }, + { + "x": 2.4303364695545335, + "y": 5.552102244014731, + "heading": -7.469284722560017e-19, + "angularVelocity": -1.047218864069531e-17, + "velocityX": -1.220957993477128, + "velocityY": 0.041597959347358555, + "timestamp": 4.037955391799845 + }, + { + "x": 2.278314122823792, + "y": 5.55728163547044, + "heading": -1.7117540142209262e-18, + "angularVelocity": -1.0332835908519937e-17, + "velocityX": -1.628098243253484, + "velocityY": 0.055469201150393666, + "timestamp": 4.131329576895728 + }, + { + "x": 2.0882753911343768, + "y": 5.563756242686637, + "heading": -3.8779782019319956e-18, + "angularVelocity": -2.3199468985970604e-17, + "velocityX": -2.035238449307225, + "velocityY": 0.06934044146380212, + "timestamp": 4.22470376199161 + }, + { + "x": 1.8602202867386202, + "y": 5.571526065245884, + "heading": -6.955816575429636e-18, + "angularVelocity": -3.296264638305206e-17, + "velocityX": -2.4423785241434, + "velocityY": 0.08321167730663664, + "timestamp": 4.318077947087493 + }, + { + "x": 1.6701383566026529, + "y": 5.578002144230341, + "heading": -1.819668166700424e-18, + "angularVelocity": 5.5006116981871155e-17, + "velocityX": -2.035701087485777, + "velocityY": 0.0693562035164242, + "timestamp": 4.411452132183375 + }, + { + "x": 1.518072799200871, + "y": 5.5831830078708, + "heading": 4.234959937878501e-18, + "angularVelocity": 6.484281229150569e-17, + "velocityX": -1.6285610123513967, + "velocityY": 0.05548496766342984, + "timestamp": 4.5048263172792575 + }, + { + "x": 1.4040236267647241, + "y": 5.587068655750534, + "heading": 2.524941481520431e-18, + "angularVelocity": -1.831387557299539e-17, + "velocityX": -1.221420806223103, + "velocityY": 0.041613727347481366, + "timestamp": 4.59820050237514 + }, + { + "x": 1.327990843372909, + "y": 5.589659087730582, + "heading": 8.045224971714285e-19, + "angularVelocity": -1.842525382897137e-17, + "velocityX": -0.814280556413611, + "velocityY": 0.027742485543317336, + "timestamp": 4.6915746874710225 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 6.74699155537553e-28, + "angularVelocity": -8.616220285509517e-18, + "velocityX": -0.40714028476041414, + "velocityY": 0.013871242994939689, + "timestamp": 4.784948872566905 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": -7.225786160817796e-27, + "velocityX": -2.979354413794432e-29, + "velocityY": 7.541641997510845e-28, + "timestamp": 4.878323057662787 + }, + { + "x": 1.3078091735186845, + "y": 5.606755773559667, + "heading": 0.009999668662888749, + "angularVelocity": 0.1349541802858926, + "velocityX": 0.24069501005494717, + "velocityY": 0.21325450657673967, + "timestamp": 4.952419826226246 + }, + { + "x": 1.3434786990122702, + "y": 5.638358364996936, + "heading": 0.029996210094567113, + "angularVelocity": 0.2698706275619814, + "velocityX": 0.48139110769232624, + "velocityY": 0.42650431390462895, + "timestamp": 5.0265165947897055 + }, + { + "x": 1.3969833303762573, + "y": 5.6857616519517675, + "heading": 0.05997803368561842, + "angularVelocity": 0.4046306495359042, + "velocityX": 0.7220912922884387, + "velocityY": 0.6397483706242104, + "timestamp": 5.1006133633531645 + }, + { + "x": 1.468323578276302, + "y": 5.748965089703653, + "heading": 0.09992693775422122, + "angularVelocity": 0.5391450240657001, + "velocityX": 0.9627983687594002, + "velocityY": 0.8529850758692316, + "timestamp": 5.1747101319166235 + }, + { + "x": 1.5575001323238937, + "y": 5.827967973754339, + "heading": 0.14982100646683655, + "angularVelocity": 0.6733636254605087, + "velocityX": 1.203514752134039, + "velocityY": 1.0662122733008166, + "timestamp": 5.248806900480083 + }, + { + "x": 1.6645138155479717, + "y": 5.922769410023865, + "heading": 0.20963796260623796, + "angularVelocity": 0.8072815765348765, + "velocityX": 1.444242242996271, + "velocityY": 1.279427404349801, + "timestamp": 5.322903669043542 + }, + { + "x": 1.789365525357909, + "y": 6.033368309242042, + "heading": 0.27935847839394334, + "angularVelocity": 0.9409386825166515, + "velocityX": 1.6849818451743819, + "velocityY": 1.4926278347551774, + "timestamp": 5.397000437607001 + }, + { + "x": 1.9320561732696155, + "y": 6.159763414266322, + "heading": 0.35896870362448313, + "angularVelocity": 1.074408867993172, + "velocityX": 1.925733749048249, + "velocityY": 1.7058112989342795, + "timestamp": 5.47109720617046 + }, + { + "x": 2.092586653477802, + "y": 6.301953347709479, + "heading": 0.44846110031344777, + "angularVelocity": 1.2077773218503376, + "velocityX": 2.1664977208531133, + "velocityY": 1.9189761740812865, + "timestamp": 5.545193974733919 + }, + { + "x": 2.2353547101625333, + "y": 6.428313752108457, + "heading": 0.5276057235865483, + "angularVelocity": 1.068125166619866, + "velocityX": 1.9267784472209708, + "velocityY": 1.7053429837972895, + "timestamp": 5.619290743297378 + }, + { + "x": 2.360281889838346, + "y": 6.538881119368732, + "heading": 0.5968684037316069, + "angularVelocity": 0.9347597942868423, + "velocityX": 1.686000376116786, + "velocityY": 1.4922022835269577, + "timestamp": 5.693387511860837 + }, + { + "x": 2.4673676751793194, + "y": 6.633656126085495, + "heading": 0.6562491552260078, + "angularVelocity": 0.8013946173366472, + "velocityX": 1.4452153232519684, + "velocityY": 1.279070714493898, + "timestamp": 5.767484280424296 + }, + { + "x": 2.5566116515465542, + "y": 6.712639287159663, + "heading": 0.7057476703680625, + "angularVelocity": 0.668025287685273, + "velocityX": 1.2044246744765545, + "velocityY": 1.0659460944941062, + "timestamp": 5.841581048987755 + }, + { + "x": 2.6280134773537864, + "y": 6.7758309931109295, + "heading": 0.7453637127245453, + "angularVelocity": 0.5346527671626246, + "velocityX": 0.9636294156990158, + "velocityY": 0.8528267448954308, + "timestamp": 5.915677817551214 + }, + { + "x": 2.6815728846604703, + "y": 6.823231527897903, + "heading": 0.7750971751130776, + "angularVelocity": 0.4012788001208449, + "velocityX": 0.7228305408320154, + "velocityY": 0.6397112275638166, + "timestamp": 5.989774586114673 + }, + { + "x": 2.7172896860156874, + "y": 6.854841077120622, + "heading": 0.7949480902508361, + "angularVelocity": 0.26790527462553493, + "velocityX": 0.4820291361527109, + "velocityY": 0.42659821506532714, + "timestamp": 6.063871354678132 + }, + { + "x": 2.7351637810076403, + "y": 6.870659730936303, + "heading": 0.8049166539682622, + "angularVelocity": 0.13453439203137166, + "velocityX": 0.2412263763293308, + "velocityY": 0.2134864193164304, + "timestamp": 6.137968123241591 + }, + { + "x": 2.73519515991211, + "y": 6.870687484741211, + "heading": 0.8050032485420815, + "angularVelocity": 0.0011686685922978992, + "velocityX": 0.00042348526922062564, + "velocityY": 0.00037456143614395893, + "timestamp": 6.21206489180505 + }, + { + "x": 2.71737666825008, + "y": 6.854917835875481, + "heading": 0.7952044521096777, + "angularVelocity": -0.13221640762197384, + "velocityX": -0.2404271762292292, + "velocityY": -0.21278187956754013, + "timestamp": 6.286176695336588 + }, + { + "x": 2.6817084896089662, + "y": 6.823350601971179, + "heading": 0.775519887263024, + "angularVelocity": -0.2656063393700981, + "velocityX": -0.4812752750383339, + "velocityY": -0.4259407056452727, + "timestamp": 6.360288498868125 + }, + { + "x": 2.628190888379515, + "y": 6.7759855045171085, + "heading": 0.7459495915696174, + "angularVelocity": -0.398995764215558, + "velocityX": -0.7221198063894422, + "velocityY": -0.6391032897100266, + "timestamp": 6.434400302399663 + }, + { + "x": 2.5568241932220688, + "y": 6.712822154407591, + "heading": 0.7064940900377142, + "angularVelocity": -0.532378105058251, + "velocityX": -0.9629599030846991, + "velocityY": -0.8522711240272193, + "timestamp": 6.5085121059312 + }, + { + "x": 2.467608789693866, + "y": 6.633860035088962, + "heading": 0.657154460511053, + "angularVelocity": -0.6657459024188197, + "velocityX": -1.2037947974070313, + "velocityY": -1.0654459282687603, + "timestamp": 6.582623909462738 + }, + { + "x": 2.3605451226537615, + "y": 6.539098475645005, + "heading": 0.597932194997477, + "angularVelocity": -0.7990935679285951, + "velocityX": -1.4446236891921902, + "velocityY": -1.2786297852682258, + "timestamp": 6.6567357129942755 + }, + { + "x": 2.2356337183339816, + "y": 6.428536615850716, + "heading": 0.528828452368811, + "angularVelocity": -0.9324255966349123, + "velocityX": -1.685445480458886, + "velocityY": -1.4918252493931579, + "timestamp": 6.730847516525813 + }, + { + "x": 2.092875237629583, + "y": 6.30217337339218, + "heading": 0.4498420161681241, + "angularVelocity": -1.0657740391526649, + "velocityX": -1.9262583541843843, + "velocityY": -1.7050353173174484, + "timestamp": 6.804959320057351 + }, + { + "x": 1.9322705754719112, + "y": 6.16000743597858, + "heading": 0.3609649957464903, + "angularVelocity": -1.1992289512105054, + "velocityX": -2.167059152487097, + "velocityY": -1.9182630921487032, + "timestamp": 6.879071123588888 + }, + { + "x": 1.789533871896463, + "y": 6.033554554877999, + "heading": 0.28090803782331497, + "angularVelocity": -1.0802187250553423, + "velocityX": -1.9259645128083558, + "velocityY": -1.7062448231010843, + "timestamp": 6.953182927120426 + }, + { + "x": 1.6646414904257845, + "y": 5.922906567233019, + "heading": 0.2107799682947157, + "angularVelocity": -0.9462469699444712, + "velocityX": -1.6851888029309197, + "velocityY": -1.4929873836216447, + "timestamp": 7.027294730651963 + }, + { + "x": 1.5575924667954766, + "y": 5.828064283268442, + "heading": 0.15061002754742392, + "angularVelocity": -0.8118806705635195, + "velocityX": -1.4444261039959587, + "velocityY": -1.2797190116759578, + "timestamp": 7.101406534183501 + }, + { + "x": 1.4683859542310973, + "y": 5.7490283016453585, + "heading": 0.10042779042808991, + "angularVelocity": -0.6771153140084615, + "velocityX": -1.203674830696421, + "velocityY": -1.066442561920003, + "timestamp": 7.175518337715038 + }, + { + "x": 1.3970212709065528, + "y": 5.685799049936121, + "heading": 0.06025974538395147, + "angularVelocity": -0.5419925454610606, + "velocityX": -0.9629327573631904, + "velocityY": -0.8531603428758896, + "timestamp": 7.249630141246576 + }, + { + "x": 1.343497933013845, + "y": 5.638376829349959, + "heading": 0.03012622810608221, + "angularVelocity": -0.40659538484416025, + "velocityX": -0.7221972121128929, + "velocityY": -0.639874059577299, + "timestamp": 7.323741944778114 + }, + { + "x": 1.3078156736079332, + "y": 5.606761857880495, + "heading": 0.010039060501896319, + "angularVelocity": -0.2710387097248632, + "velocityX": -0.4814652687860333, + "velocityY": -0.4265848348630388, + "timestamp": 7.397853748309651 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -5.584258539451151e-28, + "angularVelocity": -0.1354583213971952, + "velocityX": -0.24073388709378457, + "velocityY": -0.21329334041956155, + "timestamp": 7.471965551841189 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": 7.53493083126559e-27, + "velocityX": -3.871179527516382e-30, + "velocityY": -3.125557960458497e-29, + "timestamp": 7.546077355372726 + }, + { + "x": 1.310726436274015, + "y": 5.58769762128994, + "heading": -1.5579970000423894e-18, + "angularVelocity": -2.2453280466561467e-17, + "velocityX": 0.299069985980355, + "velocityY": -0.04693411089639644, + "timestamp": 7.615465746658217 + }, + { + "x": 1.3522304064004054, + "y": 5.581184256432646, + "heading": -2.2539666819865276e-18, + "angularVelocity": -1.003005957705595e-17, + "velocityX": 0.598139967759544, + "velocityY": -0.09386822113363098, + "timestamp": 7.6848541379437085 + }, + { + "x": 1.4144863610944205, + "y": 5.571414209224462, + "heading": -1.1138395195686094e-18, + "angularVelocity": 1.6431093577313864e-17, + "velocityX": 0.8972099444973367, + "velocityY": -0.1408023305798717, + "timestamp": 7.7542425292292 + }, + { + "x": 1.49749429992851, + "y": 5.558387479732467, + "heading": 1.8047013110476196e-18, + "angularVelocity": 4.206093786115289e-17, + "velocityX": 1.1962799150734271, + "velocityY": -0.18773643905934295, + "timestamp": 7.823630920514691 + }, + { + "x": 1.6012542223682356, + "y": 5.5421040680405165, + "heading": 7.934655780307681e-18, + "angularVelocity": 8.834265128972616e-17, + "velocityX": 1.4953498779473966, + "velocityY": -0.23467054633035336, + "timestamp": 7.893019311800182 + }, + { + "x": 1.725766127726464, + "y": 5.522563974256421, + "heading": 1.6517348693937324e-17, + "angularVelocity": 1.236906166709818e-16, + "velocityX": 1.7944198309186505, + "velocityY": -0.28160465204763024, + "timestamp": 7.962407703085673 + }, + { + "x": 1.8710300150870187, + "y": 5.499767198523928, + "heading": 2.797873673898563e-17, + "angularVelocity": 1.6517731335259598e-16, + "velocityX": 2.0934897706863036, + "velocityY": -0.3285387556932655, + "timestamp": 8.031796094371165 + }, + { + "x": 2.0370458831672553, + "y": 5.4737137410442855, + "heading": 4.626535556608686e-17, + "angularVelocity": 2.635400319271784e-16, + "velocityX": 2.3925596919689536, + "velocityY": -0.3754728564386086, + "timestamp": 8.101184485656656 + }, + { + "x": 2.223813730043212, + "y": 5.444403602119362, + "heading": 6.626560991799023e-17, + "angularVelocity": 2.882363175884601e-16, + "velocityX": 2.691629585524184, + "velocityY": -0.42240695283352697, + "timestamp": 8.170572876942147 + }, + { + "x": 2.4313335525083035, + "y": 5.411836782252268, + "heading": 9.137945639838438e-17, + "angularVelocity": 3.6193152862799336e-16, + "velocityX": 2.99069943286728, + "velocityY": -0.46934104197777365, + "timestamp": 8.239961268227638 + }, + { + "x": 2.65960534414942, + "y": 5.376013282449221, + "heading": 1.1821641145167503e-16, + "angularVelocity": 3.8676433675745036e-16, + "velocityX": 3.2897691877869866, + "velocityY": -0.5162751166208155, + "timestamp": 8.30934965951313 + }, + { + "x": 2.9086290857277297, + "y": 5.336933105728788, + "heading": 1.4538386371763974e-16, + "angularVelocity": 3.915273420502118e-16, + "velocityX": 3.5888386654437228, + "velocityY": -0.5632091477613738, + "timestamp": 8.37873805079862 + }, + { + "x": 3.1678798362557536, + "y": 5.296247694105415, + "heading": 1.2970532145326214e-16, + "angularVelocity": -2.259533930819528e-16, + "velocityX": 3.73622656074219, + "velocityY": -0.5863432033749367, + "timestamp": 8.448126442084112 + }, + { + "x": 3.426192624956805, + "y": 5.24997937308727, + "heading": 1.1963113847649017e-16, + "angularVelocity": -1.451854235288838e-16, + "velocityX": 3.7227089995248686, + "velocityY": -0.6668020422577368, + "timestamp": 8.517514833369603 + }, + { + "x": 3.6799776944966163, + "y": 5.18320068927956, + "heading": 1.442058714433213e-16, + "angularVelocity": 3.5416202208408226e-16, + "velocityX": 3.657457174581828, + "velocityY": -0.9623898547086362, + "timestamp": 8.586903224655094 + }, + { + "x": 3.9276087380924305, + "y": 5.096338039673236, + "heading": 1.2678863352315073e-16, + "angularVelocity": -2.510108333324681e-16, + "velocityX": 3.568767613835609, + "velocityY": -1.251832590395956, + "timestamp": 8.656291615940585 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 9.872947837544061e-17, + "angularVelocity": -4.0437823428221097e-16, + "velocityX": 3.457211254829872, + "velocityY": -1.5332571030157462, + "timestamp": 8.725680007226076 + }, + { + "x": 4.284983330328794, + "y": 4.932123965200671, + "heading": 8.520616401533837e-17, + "angularVelocity": -3.905855769726077e-16, + "velocityX": 3.3932258158637794, + "velocityY": -1.6700901615188843, + "timestamp": 8.760303186261309 + }, + { + "x": 4.400064822274762, + "y": 4.86965478591225, + "heading": 8.38275920998652e-17, + "angularVelocity": -3.9816445337684444e-17, + "velocityX": 3.3238280005674867, + "velocityY": -1.8042589106232223, + "timestamp": 8.794926365296542 + }, + { + "x": 4.512559967686789, + "y": 4.802639932227702, + "heading": 8.775922770148411e-17, + "angularVelocity": 1.1355501462237473e-16, + "velocityX": 3.2491281432462653, + "velocityY": -1.935548830347224, + "timestamp": 8.829549544331774 + }, + { + "x": 4.622289335106214, + "y": 4.731186349760053, + "heading": 1.0501758472421099e-16, + "angularVelocity": 4.984625185678289e-16, + "velocityX": 3.169245877386488, + "velocityY": -2.06374990566114, + "timestamp": 8.864172723367007 + }, + { + "x": 4.729077970033436, + "y": 4.65540814642046, + "heading": 1.123300173637084e-16, + "angularVelocity": 2.11200497564256e-16, + "velocityX": 3.0843105082451987, + "velocityY": -2.188655272309947, + "timestamp": 8.89879590240224 + }, + { + "x": 4.83750746703025, + "y": 4.5819970246859665, + "heading": 1.0947895650457139e-16, + "angularVelocity": -8.234543847737888e-17, + "velocityX": 3.131702518895681, + "velocityY": -2.120288309163928, + "timestamp": 8.933419081437473 + }, + { + "x": 4.94878188699609, + "y": 4.512974188419722, + "heading": 1.0086901564782932e-16, + "angularVelocity": -2.486756299293195e-16, + "velocityX": 3.213870680465465, + "velocityY": -1.9935441571094976, + "timestamp": 8.968042260472705 + }, + { + "x": 5.062723794471585, + "y": 4.448449851931572, + "heading": 9.759628790443595e-17, + "angularVelocity": -9.452418393074315e-17, + "velocityX": 3.2909140827174443, + "velocityY": -1.8636167528836842, + "timestamp": 9.002665439507938 + }, + { + "x": 5.179151445124142, + "y": 4.388526983063662, + "heading": 9.598506413460085e-17, + "angularVelocity": -4.6535985854895585e-17, + "velocityX": 3.362708275114807, + "velocityY": -1.7307153917591276, + "timestamp": 9.03728861854317 + }, + { + "x": 5.297879115709132, + "y": 4.333301191879142, + "heading": 9.429439507749846e-17, + "angularVelocity": -4.883055525842942e-17, + "velocityX": 3.429138337186562, + "velocityY": -1.5950525839444993, + "timestamp": 9.071911797578403 + }, + { + "x": 5.418717408418424, + "y": 4.28286058857882, + "heading": 9.044194331783773e-17, + "angularVelocity": -1.112679963830143e-16, + "velocityX": 3.4900981387736523, + "velocityY": -1.4568449433541117, + "timestamp": 9.106534976613636 + }, + { + "x": 5.54147355571485, + "y": 4.237285646363086, + "heading": 8.606851716017984e-17, + "angularVelocity": -1.2631497971943828e-16, + "velocityX": 3.545490354063358, + "velocityY": -1.3163130447772213, + "timestamp": 9.141158155648869 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 8.12244185045661e-17, + "angularVelocity": -1.3990912419348855e-16, + "velocityX": 3.5952265671295462, + "velocityY": -1.1736811275270398, + "timestamp": 9.175781334684102 + }, + { + "x": 5.800884886979515, + "y": 4.158897293130728, + "heading": 7.604329686230843e-17, + "angularVelocity": -1.3984796334066921e-16, + "velocityX": 3.642093094613744, + "velocityY": -1.0189897302407367, + "timestamp": 9.212829579924803 + }, + { + "x": 5.9373078989205395, + "y": 4.126945510909642, + "heading": 7.099587364491845e-17, + "angularVelocity": -1.3623919795923125e-16, + "velocityX": 3.682306977150727, + "velocityY": -0.8624371279529385, + "timestamp": 9.249877825165504 + }, + { + "x": 6.074971576496844, + "y": 4.100852086793861, + "heading": 6.753360793468667e-17, + "angularVelocity": -9.345289332160286e-17, + "velocityX": 3.7157948151635507, + "velocityY": -0.7043093119863827, + "timestamp": 9.286926070406205 + }, + { + "x": 6.213624465766887, + "y": 4.080664678033529, + "heading": 6.709732769415932e-17, + "angularVelocity": -1.177600282260163e-17, + "velocityX": 3.742495450708113, + "velocityY": -0.5448951395450716, + "timestamp": 9.323974315646906 + }, + { + "x": 6.353013304056433, + "y": 4.066420153436822, + "heading": 4.721163336657454e-17, + "angularVelocity": -5.367513143575007e-16, + "velocityX": 3.7623600627760427, + "velocityY": -0.38448581043883095, + "timestamp": 9.361022560887607 + }, + { + "x": 6.4925719820191485, + "y": 4.0581614935185435, + "heading": 3.328638124108273e-17, + "angularVelocity": -3.758680611067538e-16, + "velocityX": 3.766944346648758, + "velocityY": -0.22291635851097177, + "timestamp": 9.398070806128308 + }, + { + "x": 6.627754792512696, + "y": 4.053990782696842, + "heading": 2.356760119630702e-17, + "angularVelocity": -2.6232767457636995e-16, + "velocityX": 3.6488316684169346, + "velocityY": -0.1125751245330268, + "timestamp": 9.435119051369009 + }, + { + "x": 6.757419046645761, + "y": 4.0521449831028535, + "heading": 1.6377013350687517e-17, + "angularVelocity": -1.940871361679193e-16, + "velocityX": 3.4998757239551144, + "velocityY": -0.049821511977060404, + "timestamp": 9.47216729660971 + }, + { + "x": 6.881278674219902, + "y": 4.051770935128315, + "heading": 1.0749401519445311e-17, + "angularVelocity": -1.51899550746631e-16, + "velocityX": 3.3431982208450166, + "velocityY": -0.010096239973268065, + "timestamp": 9.509215541850411 + }, + { + "x": 6.999234961376727, + "y": 4.052402049117112, + "heading": 6.6780180524147395e-18, + "angularVelocity": -1.0989409673710028e-16, + "velocityX": 3.183856249883563, + "velocityY": 0.017034922563732322, + "timestamp": 9.546263787091112 + }, + { + "x": 7.111245945109509, + "y": 4.053749520879657, + "heading": 3.4043054479450288e-18, + "angularVelocity": -8.836349922350792e-17, + "velocityX": 3.0233816205072856, + "velocityY": 0.03637073102354394, + "timestamp": 9.583312032331813 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, + "angularVelocity": -9.188843930234661e-17, + "velocityX": 2.862359859004765, + "velocityY": 0.0504413971839862, + "timestamp": 9.620360277572514 + }, + { + "x": 7.386141990779784, + "y": 4.060115199818634, + "heading": -6.285690223777642e-18, + "angularVelocity": -9.593548047553597e-17, + "velocityX": 2.5770864103628295, + "velocityY": 0.06863424057552284, + "timestamp": 9.685880251977624 + }, + { + "x": 7.536265872308939, + "y": 4.064907354049287, + "heading": -1.0202276969724947e-17, + "angularVelocity": -5.97769883515376e-17, + "velocityX": 2.2912689281735665, + "velocityY": 0.07314035565737474, + "timestamp": 9.751400226382733 + }, + { + "x": 7.667662893519374, + "y": 4.069410994150776, + "heading": -1.2426686282698196e-17, + "angularVelocity": -3.395009435762481e-17, + "velocityX": 2.005449825087065, + "velocityY": 0.06873690263742, + "timestamp": 9.816920200787843 + }, + { + "x": 7.780343830854776, + "y": 4.073216939396029, + "heading": -1.3104250722395789e-17, + "angularVelocity": -1.03413416429204e-17, + "velocityX": 1.7197951977010522, + "velocityY": 0.05808832008572811, + "timestamp": 9.882440175192952 + }, + { + "x": 7.874322400829606, + "y": 4.07602285690707, + "heading": -1.2397928827700396e-17, + "angularVelocity": 1.0780252923201902e-17, + "velocityX": 1.4343499189081053, + "velocityY": 0.04282537556703236, + "timestamp": 9.947960149598062 + }, + { + "x": 7.949612474283929, + "y": 4.07759641137373, + "heading": -1.096542742535041e-17, + "angularVelocity": 2.1863583068378234e-17, + "velocityX": 1.1491163441060581, + "velocityY": 0.024016408445632246, + "timestamp": 10.013480124003172 + }, + { + "x": 8.006227098004604, + "y": 4.077753556862197, + "heading": -9.354890651543402e-18, + "angularVelocity": 2.458085169441397e-17, + "velocityX": 0.8640818961654191, + "velocityY": 0.0023984363530761407, + "timestamp": 10.079000098408281 + }, + { + "x": 8.044178200157203, + "y": 4.076344939580228, + "heading": -5.6627659945325885e-18, + "angularVelocity": 5.635113094762732e-17, + "velocityX": 0.579229502104909, + "velocityY": -0.021499051163544333, + "timestamp": 10.14452007281339 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 0, + "angularVelocity": 8.642808619399734e-17, + "velocityX": 0.2945416648589691, + "velocityY": -0.04728304210698771, + "timestamp": 10.2100400472185 + }, + { + "x": 8.06151161206475, + "y": 4.067466722329622, + "heading": 7.499406153547556e-18, + "angularVelocity": 1.0142249804726799e-16, + "velocityX": -0.026574128354115305, + "velocityY": -0.07817228633622807, + "timestamp": 10.283982281901235 + }, + { + "x": 8.0357781795929, + "y": 4.059672922438978, + "heading": 2.0171548175080756e-17, + "angularVelocity": 1.7137894295700475e-16, + "velocityX": -0.34802075677407873, + "velocityY": -0.10540389973451367, + "timestamp": 10.357924516583969 + }, + { + "x": 7.986251299714503, + "y": 4.050185891028673, + "heading": 3.259640252688688e-17, + "angularVelocity": 1.6803460705902343e-16, + "velocityX": -0.6698050186189494, + "velocityY": -0.12830328229881344, + "timestamp": 10.431866751266703 + }, + { + "x": 7.9129066919285, + "y": 4.039390810897977, + "heading": 4.5982022945133473e-17, + "angularVelocity": 1.81028075140569e-16, + "velocityX": -0.9919176516736913, + "velocityY": -0.1459934255032282, + "timestamp": 10.505808985949438 + }, + { + "x": 7.8157231380567715, + "y": 4.027759201442648, + "heading": 5.916672321533067e-17, + "angularVelocity": 1.783108168799044e-16, + "velocityX": -1.314317240866671, + "velocityY": -0.1573067071239705, + "timestamp": 10.579751220632172 + }, + { + "x": 7.694687242510101, + "y": 4.015880811506709, + "heading": 7.241021515042493e-17, + "angularVelocity": 1.7910591950468916e-16, + "velocityX": -1.6368979929535623, + "velocityY": -0.16064418375919837, + "timestamp": 10.653693455314906 + }, + { + "x": 7.549803190746082, + "y": 4.0045128268534045, + "heading": 8.516404983291186e-17, + "angularVelocity": 1.7248376026751216e-16, + "velocityX": -1.9594221406165937, + "velocityY": -0.1537414266972227, + "timestamp": 10.72763568999764 + }, + { + "x": 7.381113647590609, + "y": 3.9946593063008904, + "heading": 9.839417244429125e-17, + "angularVelocity": 1.7892511182976314e-16, + "velocityX": -2.281369286163345, + "velocityY": -0.13325970732143547, + "timestamp": 10.801577924680375 + }, + { + "x": 7.188747477257485, + "y": 3.987706179482555, + "heading": 1.110446332225851e-16, + "angularVelocity": 1.7108572429643452e-16, + "velocityX": -2.6015736629885633, + "velocityY": -0.09403457777777122, + "timestamp": 10.87552015936311 + }, + { + "x": 6.973038504473885, + "y": 3.9856630874408094, + "heading": 1.1423152127106693e-16, + "angularVelocity": 4.309969887337601e-17, + "velocityX": -2.91726337064529, + "velocityY": -0.027630920954873416, + "timestamp": 10.949462394045844 + }, + { + "x": 6.734853840508573, + "y": 3.9916098664093003, + "heading": 1.1357543736188786e-16, + "angularVelocity": -8.872925078647214e-18, + "velocityX": -3.221226204310621, + "velocityY": 0.0804246584378634, + "timestamp": 11.023404628728578 + }, + { + "x": 6.47658832080339, + "y": 4.010431184705039, + "heading": 1.1486840283254658e-16, + "angularVelocity": 1.7486156277295078e-17, + "velocityX": -3.4928011144554563, + "velocityY": 0.25454083686402557, + "timestamp": 11.097346863411312 + }, + { + "x": 6.204860010324235, + "y": 4.048943809403862, + "heading": 1.1487825258491134e-16, + "angularVelocity": 1.3320874674868913e-19, + "velocityX": -3.6748728469603784, + "velocityY": 0.5208474542873691, + "timestamp": 11.171289098094046 + }, + { + "x": 5.932253242931649, + "y": 4.111293853023339, + "heading": 1.0773394940295777e-16, + "angularVelocity": -9.662006041077537e-17, + "velocityX": -3.686753160250877, + "velocityY": 0.8432263899921877, + "timestamp": 11.24523133277678 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 9.686455716091523e-17, + "angularVelocity": -1.4699842774133078e-16, + "velocityX": -3.6014804699029255, + "velocityY": 1.1543500395591086, + "timestamp": 11.319173567459515 + }, + { + "x": 5.543077137245883, + "y": 4.241512673300566, + "heading": 9.161999350902719e-17, + "angularVelocity": -1.5163138697871951e-16, + "velocityX": -3.552563374503243, + "velocityY": 1.2971011802444727, + "timestamp": 11.353761153554894 + }, + { + "x": 5.422090048254055, + "y": 4.291242278322373, + "heading": 8.675864551293739e-17, + "angularVelocity": -1.4055181482408883e-16, + "velocityX": -3.4979916973158245, + "velocityY": 1.4377876757479933, + "timestamp": 11.388348739650272 + }, + { + "x": 5.303183074500814, + "y": 4.34575871765036, + "heading": 8.202370382364629e-17, + "angularVelocity": -1.368971421201274e-16, + "velocityX": -3.437851182367712, + "velocityY": 1.5761851427750155, + "timestamp": 11.42293632574565 + }, + { + "x": 5.186545516053187, + "y": 4.404975197881964, + "heading": 7.882281033080331e-17, + "angularVelocity": -9.254457608045447e-17, + "velocityX": -3.372237603572297, + "velocityY": 1.712073229635336, + "timestamp": 11.45752391184103 + }, + { + "x": 5.072363058490256, + "y": 4.468797441531519, + "heading": 7.597641724828609e-17, + "angularVelocity": -8.229522218370685e-17, + "velocityX": -3.3012554633926476, + "velocityY": 1.845235555715285, + "timestamp": 11.492111497936408 + }, + { + "x": 4.960817475466504, + "y": 4.537123834649348, + "heading": 7.51892087087232e-17, + "angularVelocity": -2.2759857753359948e-17, + "velocityX": -3.225017863812662, + "velocityY": 1.975460008380286, + "timestamp": 11.526699084031787 + }, + { + "x": 4.852086330068725, + "y": 4.609845576159071, + "heading": 7.00186010980759e-17, + "angularVelocity": -1.4949316197981958e-16, + "velocityX": -3.143646541216975, + "velocityY": 2.1025387926519685, + "timestamp": 11.561286670127165 + }, + { + "x": 4.744615410553515, + "y": 4.6844171206048495, + "heading": 6.888474965661026e-17, + "angularVelocity": -3.278203452357036e-17, + "velocityX": -3.10721075529382, + "velocityY": 2.1560204935996916, + "timestamp": 11.595874256222544 + }, + { + "x": 4.634255259438232, + "y": 4.75464211813236, + "heading": 6.027771904333573e-17, + "angularVelocity": -2.4884739251649313e-16, + "velocityX": -3.190744529293135, + "velocityY": 2.0303526627692405, + "timestamp": 11.630461842317922 + }, + { + "x": 4.521181551584758, + "y": 4.820408744704174, + "heading": 5.1990889261648227e-17, + "angularVelocity": -2.395897117201502e-16, + "velocityX": -3.269199172838061, + "velocityY": 1.9014517633713166, + "timestamp": 11.665049428413301 + }, + { + "x": 4.405574295139685, + "y": 4.881612290783718, + "heading": 4.5533169228459885e-17, + "angularVelocity": -1.867062944312063e-16, + "velocityX": -3.3424494015360238, + "velocityY": 1.7695234906179804, + "timestamp": 11.69963701450868 + }, + { + "x": 4.287617535211261, + "y": 4.938155315408326, + "heading": 4.203003409755996e-17, + "angularVelocity": -1.0128301874666137e-16, + "velocityX": -3.410378498318693, + "velocityY": 1.6347779942979137, + "timestamp": 11.734224600604058 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 4.0833427337575637e-17, + "angularVelocity": -3.459642302543387e-17, + "velocityX": -3.4728780863994273, + "velocityY": 1.4974297517257, + "timestamp": 11.768812186699437 + }, + { + "x": 3.9262086603019655, + "y": 5.072405406382895, + "heading": 4.69185026085022e-17, + "angularVelocity": 9.025220132506432e-17, + "velocityX": -3.5787544523378267, + "velocityY": 1.2229891223429616, + "timestamp": 11.83623519889844 + }, + { + "x": 3.6792395049411635, + "y": 5.135860556463708, + "heading": 5.846977032472703e-17, + "angularVelocity": 1.713252988776416e-16, + "velocityX": -3.6629801503359496, + "velocityY": 0.9411497352494828, + "timestamp": 11.903658211097442 + }, + { + "x": 3.4280856473907875, + "y": 5.179929383329658, + "heading": 6.25257434115025e-17, + "angularVelocity": 6.01571029606942e-17, + "velocityX": -3.7250465287591408, + "velocityY": 0.6536169985387049, + "timestamp": 11.971081223296444 + }, + { + "x": 3.174266449354605, + "y": 5.204345315411431, + "heading": 8.981508430066906e-17, + "angularVelocity": 4.0474817127156825e-16, + "velocityX": -3.7645781426529163, + "velocityY": 0.3621305439411197, + "timestamp": 12.038504235495447 + }, + { + "x": 2.919317391668449, + "y": 5.208960723831647, + "heading": 3.637405660157601e-17, + "angularVelocity": -7.926229629337798e-16, + "velocityX": -3.781335917381794, + "velocityY": 0.06845449750291784, + "timestamp": 12.10592724769445 + }, + { + "x": 2.6778199582724502, + "y": 5.213921183762201, + "heading": 2.780922480909488e-17, + "angularVelocity": -1.270312837495845e-16, + "velocityX": -3.581825040435896, + "velocityY": 0.07357220878699586, + "timestamp": 12.173350259893452 + }, + { + "x": 2.4555441151084385, + "y": 5.223767512233129, + "heading": 2.258282570608997e-17, + "angularVelocity": -7.751654714759439e-17, + "velocityX": -3.2967355790624753, + "velocityY": 0.14603809811799462, + "timestamp": 12.240773272092454 + }, + { + "x": 2.2524898779476, + "y": 5.238499719208596, + "heading": 1.8179159660483545e-17, + "angularVelocity": -6.531399151152503e-17, + "velocityX": -3.0116458837750555, + "velocityY": 0.2185041352347682, + "timestamp": 12.308196284291457 + }, + { + "x": 2.068657252210669, + "y": 5.258117807366149, + "heading": 1.4559970052520395e-17, + "angularVelocity": -5.367884777868486e-17, + "velocityX": -2.726556108088765, + "velocityY": 0.29097021206423906, + "timestamp": 12.37561929649046 + }, + { + "x": 1.9040462406637386, + "y": 5.282621777825342, + "heading": 1.0447672435760193e-17, + "angularVelocity": -6.099249326267761e-17, + "velocityX": -2.441466291376519, + "velocityY": 0.3634363054985942, + "timestamp": 12.443042308689462 + }, + { + "x": 1.7586568449915738, + "y": 5.312011631159126, + "heading": 6.39006439635328e-18, + "angularVelocity": -6.018135214654068e-17, + "velocityX": -2.156376449676296, + "velocityY": 0.4359024074308457, + "timestamp": 12.510465320888464 + }, + { + "x": 1.6324890663305314, + "y": 5.34628736769762, + "heading": 2.9527164791572072e-18, + "angularVelocity": -5.0981820681006015e-17, + "velocityX": -1.8712865911219376, + "velocityY": 0.508368514259319, + "timestamp": 12.577888333087467 + }, + { + "x": 1.5255429054998768, + "y": 5.385448987646794, + "heading": 6.750997693764207e-19, + "angularVelocity": -3.378099903552108e-17, + "velocityX": -1.5861967204164489, + "velocityY": 0.5808346241426653, + "timestamp": 12.64531134528647 + }, + { + "x": 1.437818363118664, + "y": 5.4294964911430075, + "heading": 3.7918321325869045e-19, + "angularVelocity": -4.388954848629936e-18, + "velocityX": -1.301106840529317, + "velocityY": 0.6533007360484864, + "timestamp": 12.712734357485472 + }, + { + "x": 1.3693154396712461, + "y": 5.478429878281045, + "heading": 1.633332658934955e-18, + "angularVelocity": 1.8601207579968088e-17, + "velocityX": -1.0160169534583994, + "velocityY": 0.7257668493601166, + "timestamp": 12.780157369684474 + }, + { + "x": 1.3200341355468752, + "y": 5.532249149129769, + "heading": 3.8843995459700536e-19, + "angularVelocity": -1.8463914088315652e-17, + "velocityX": -0.7309270606141895, + "velocityY": 0.798232963693069, + "timestamp": 12.847580381883477 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": -5.761237033299429e-18, + "velocityX": -0.4458371630310619, + "velocityY": 0.8706990788013996, + "timestamp": 12.915003394082479 + }, + { + "x": 1.2868335136673428, + "y": 5.68644048793425, + "heading": -1.3391788419363526e-18, + "angularVelocity": -1.3686362473728894e-17, + "velocityX": -0.03210027395244838, + "velocityY": 0.9758655723245745, + "timestamp": 13.012851076079642 + }, + { + "x": 1.3242854311298142, + "y": 5.791775499822582, + "heading": 2.339658407779995e-18, + "angularVelocity": 3.759759224271494e-17, + "velocityX": 0.3827573295354856, + "velocityY": 1.076520258204863, + "timestamp": 13.110698758076804 + }, + { + "x": 1.4024881161679177, + "y": 5.906284066156856, + "heading": 5.091444284753208e-18, + "angularVelocity": 2.812315857202659e-17, + "velocityX": 0.7992287956333111, + "velocityY": 1.1702736743175466, + "timestamp": 13.208546440073967 + }, + { + "x": 1.5216854983005004, + "y": 6.028805982329465, + "heading": 6.6702155316476646e-18, + "angularVelocity": 1.6134988737668432e-17, + "velocityX": 1.218193213162059, + "velocityY": 1.2521698385881244, + "timestamp": 13.30639412207113 + }, + { + "x": 1.682281091295048, + "y": 6.156892671597042, + "heading": 8.737231811568005e-18, + "angularVelocity": 2.1124836460875052e-17, + "velocityX": 1.6412815277443633, + "velocityY": 1.3090416313724325, + "timestamp": 13.404241804068292 + }, + { + "x": 1.884561264278339, + "y": 6.2823011062588545, + "heading": 4.474450456421642e-18, + "angularVelocity": -4.3565481249463154e-17, + "velocityX": 2.06729652511499, + "velocityY": 1.2816699599020622, + "timestamp": 13.502089486065454 + }, + { + "x": 2.074597433027926, + "y": 6.367773862583125, + "heading": 1.3148438373880832e-18, + "angularVelocity": -3.229107274750916e-17, + "velocityX": 1.9421632160391744, + "velocityY": 0.8735286782445029, + "timestamp": 13.599937168062617 + }, + { + "x": 2.2289686427209534, + "y": 6.431502913374048, + "heading": 4.376002899474807e-18, + "angularVelocity": 3.128494207400489e-17, + "velocityX": 1.5776685409625097, + "velocityY": 0.6513087432441247, + "timestamp": 13.69778485005978 + }, + { + "x": 2.345509714236833, + "y": 6.4775215317101145, + "heading": 6.666442968036081e-18, + "angularVelocity": 2.3408220019295113e-17, + "velocityX": 1.1910458085175606, + "velocityY": 0.4703087226675544, + "timestamp": 13.795632532056942 + }, + { + "x": 2.4235057296596247, + "y": 6.507444880215651, + "heading": 4.695809387720839e-18, + "angularVelocity": -2.013980853353183e-17, + "velocityX": 0.7971166391560869, + "velocityY": 0.30581560947355857, + "timestamp": 13.893480214054104 + }, + { + "x": 2.462606906890869, + "y": 6.522137641906738, + "heading": 1.18409559006069e-37, + "angularVelocity": -4.799101310235197e-17, + "velocityX": 0.3996127085808573, + "velocityY": 0.15015952745322653, + "timestamp": 13.991327896051267 }, + { + "x": 2.462606906890869, + "y": 6.522137641906738, + "heading": 7.445601745662792e-38, + "angularVelocity": -4.492041573884033e-37, + "velocityX": -6.88570869510922e-34, + "velocityY": -3.3118407683240263e-34, + "timestamp": 14.08917557804843 + } + ], + "constraints": [ { "scope": [ - 5 + "first" ], "type": "StopPoint" }, { "scope": [ - 1 + "last" ], "type": "StopPoint" }, { "scope": [ - 6 + 2 ], "type": "StopPoint" }, { "scope": [ - 2 + 4 ], "type": "StopPoint" }, { "scope": [ - 4 + 6 ], "type": "StopPoint" } @@ -13507,7 +7210,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "SourceLanePSubHSubSprint": { + "SourceLanePSubHSubGSub": { "waypoints": [ { "x": 0.6634833812713623, @@ -13516,7 +7219,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 25 + "controlIntervalCount": 24 }, { "x": 2.757753372192383, @@ -13525,7 +7228,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 22 + "controlIntervalCount": 21 }, { "x": 7.340087890625, @@ -13534,7 +7237,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 7 }, { "x": 7.841280937194824, @@ -13543,7 +7246,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 24 + "controlIntervalCount": 23 }, { "x": 2.757753372192383, @@ -13552,7 +7255,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 25 + "controlIntervalCount": 24 }, { "x": 0.6634833812713623, @@ -13561,7 +7264,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 25 + "controlIntervalCount": 24 }, { "x": 2.757753372192383, @@ -13570,14 +7273,41 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, + "controlIntervalCount": 23 + }, + { + "x": 7.481724262237549, + "y": 2.2045881748199463, + "heading": 0.23374329695765314, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 7.975332260131836, + "y": 2.3279902935028076, + "heading": 0.23374329695765314, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, "controlIntervalCount": 24 }, { - "x": 8.235074996948242, - "y": 0.7759228944778442, + "x": 2.757753372192383, + "y": 1.617210865020752, "heading": 0, "isInitialGuess": false, "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "isInitialGuess": false, + "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 40 } @@ -13588,1386 +7318,1818 @@ "y": 4.409571170806885, "heading": -1.0475181007536924, "angularVelocity": 0, - "velocityX": 6.43793008966589e-32, - "velocityY": 2.60583916914808e-32, + "velocityX": -1.7128958192175968e-32, + "velocityY": -9.692960229070848e-33, "timestamp": 0 }, { - "x": 0.6686935342644011, - "y": 4.399440637290449, - "heading": -1.042618248425991, - "angularVelocity": 0.09045075655350086, - "velocityX": 0.09617887406842093, - "velocityY": -0.18700857894671152, - "timestamp": 0.05417149081338269 - }, - { - "x": 0.6791748314486301, - "y": 4.379207946886432, - "heading": -1.0329282708117224, - "angularVelocity": 0.17887596351464607, - "velocityX": 0.19348363921414355, - "velocityY": -0.37349332832129434, - "timestamp": 0.10834298162676538 - }, - { - "x": 0.6949946213377666, - "y": 4.348905028536486, - "heading": -1.0185695824101737, - "angularVelocity": 0.2650598716401143, - "velocityX": 0.29203165081121235, - "velocityY": -0.5593886728046312, - "timestamp": 0.16251447244014808 - }, - { - "x": 0.7162276537845403, - "y": 4.3085680597239815, - "heading": -0.9996772134807529, - "angularVelocity": 0.34875113543586445, - "velocityX": 0.39195953679621004, - "velocityY": -0.7446161847651872, - "timestamp": 0.21668596325353076 - }, - { - "x": 0.742957361589069, - "y": 4.258238351873032, - "heading": -0.9764021807302357, - "angularVelocity": 0.42965464677164156, - "velocityX": 0.4934275834610263, - "velocityY": -0.9290810921990941, - "timestamp": 0.2708574540669135 - }, - { - "x": 0.775277449766474, - "y": 4.197963491949002, - "heading": -0.9489144431022858, - "angularVelocity": 0.507420733954753, - "velocityX": 0.5966254148098966, - "velocityY": -1.1126675492774045, - "timestamp": 0.32502894488029616 - }, - { - "x": 0.8132938881778625, - "y": 4.1277988368888145, - "heading": -0.9174066544487739, - "angularVelocity": 0.5816304513762527, - "velocityX": 0.7017794386045728, - "velocityY": -1.295232123145776, - "timestamp": 0.37920043569367884 - }, - { - "x": 0.8571274399080208, - "y": 4.047809503293585, - "heading": -0.8820990289920404, - "angularVelocity": 0.6517750375075625, - "velocityX": 0.8091627361920356, - "velocityY": -1.4765946514336408, - "timestamp": 0.4333719265070615 - }, - { - "x": 0.9069169109204649, - "y": 3.958073067611011, - "heading": -0.8432457931383238, - "angularVelocity": 0.7172266310255936, - "velocityX": 0.919108377208329, - "velocityY": -1.6565251266890444, - "timestamp": 0.4875434173204442 - }, - { - "x": 0.9628233844277805, - "y": 3.8586833111714633, - "heading": -0.8011439375928472, - "angularVelocity": 0.7771958074869153, - "velocityX": 1.0320275973188553, - "velocityY": -1.8347244084889383, - "timestamp": 0.541714908133827 - }, - { - "x": 1.0250358185457515, - "y": 3.7497555457988567, - "heading": -0.7561453551591197, - "angularVelocity": 0.8306690799547085, - "velocityX": 1.148434964293097, - "velocityY": -2.010795046196093, - "timestamp": 0.5958863989472096 - }, - { - "x": 1.0937785557910409, - "y": 3.6314344082082353, - "heading": -0.7086740357677194, - "angularVelocity": 0.876315543076626, - "velocityX": 1.2689836704347575, - "velocityY": -2.184195705416893, - "timestamp": 0.6500578897605923 - }, - { - "x": 1.1693215384166689, - "y": 3.503905652509146, - "heading": -0.65925094255443, - "angularVelocity": 0.9123450817247932, - "velocityX": 1.39451548206175, - "velocityY": -2.354167363390805, - "timestamp": 0.704229380573975 - }, - { - "x": 1.2519943506884508, - "y": 3.3674146850873483, - "heading": -0.6085307966283009, - "angularVelocity": 0.9362885378373057, - "velocityX": 1.5261313844321562, - "velocityY": -2.519608845398026, - "timestamp": 0.7584008713873577 - }, - { - "x": 1.3422055318396924, - "y": 3.2222969888664816, - "heading": -0.5573578262997513, - "angularVelocity": 0.9446476284885243, - "velocityX": 1.6652888779083848, - "velocityY": -2.6788573480613262, - "timestamp": 0.8125723622007404 - }, - { - "x": 1.440468425966884, - "y": 3.0690305293119637, - "heading": -0.5068529144587157, - "angularVelocity": 0.9323153393548251, - "velocityX": 1.8139226491975393, - "velocityY": -2.8292826587053166, - "timestamp": 0.866743853014123 - }, - { - "x": 1.5474319674576553, - "y": 2.908330661716264, - "heading": -0.4585565550407312, - "angularVelocity": 0.8915456948445868, - "velocityX": 1.9745356807560286, - "velocityY": -2.9665025861905914, - "timestamp": 0.9209153438275057 - }, - { - "x": 1.66389994755801, - "y": 2.7413290540117057, - "heading": -0.41468584741790443, - "angularVelocity": 0.8098486300470928, - "velocityX": 2.1499866138367625, - "velocityY": -3.0828320431473393, - "timestamp": 0.9750868346408884 - }, - { - "x": 1.7907579832157325, - "y": 2.5699020484114805, - "heading": -0.3786367444287472, - "angularVelocity": 0.6654626344573777, - "velocityX": 2.3417859422540332, - "velocityY": -3.1645244209870533, - "timestamp": 1.029258325454271 - }, - { - "x": 1.9285625356473743, - "y": 2.397072387394727, - "heading": -0.3552615550516524, - "angularVelocity": 0.4315035275218978, - "velocityX": 2.5438574859674787, - "velocityY": -3.1904172918581897, - "timestamp": 1.083429816267654 - }, - { - "x": 2.0769698348954466, - "y": 2.226810347381072, - "heading": -0.3460632427651378, - "angularVelocity": 0.16979987348331044, - "velocityX": 2.739583072567186, - "velocityY": -3.1430192792772944, - "timestamp": 1.1376013070810367 - }, - { - "x": 2.2352281984049327, - "y": 2.0620956413219225, - "heading": -0.3444813887227629, - "angularVelocity": 0.029200858581211304, - "velocityX": 2.9214326785776583, - "velocityY": -3.0406160802659317, - "timestamp": 1.1917727978944195 - }, - { - "x": 2.401905636235413, - "y": 1.90514203890439, - "heading": -0.3444812653381511, - "angularVelocity": 0.0000022776669023009663, - "velocityX": 3.076847901503659, - "velocityY": -2.8973469266007075, - "timestamp": 1.2459442887078023 - }, - { - "x": 2.5762262115041326, - "y": 1.7567232171949336, - "heading": -0.34448120853135994, - "angularVelocity": 0.0000010486473662298194, - "velocityX": 3.217939411511546, - "velocityY": -2.739795775987586, - "timestamp": 1.300115779521185 + "x": 0.6705320996855912, + "y": 4.398099342118358, + "heading": -1.0386114127976835, + "angularVelocity": 0.15927551181553445, + "velocityX": 0.1260500242755718, + "velocityY": -0.20514712032687513, + "timestamp": 0.05592000838348859 + }, + { + "x": 0.6846335258864488, + "y": 4.375148415401169, + "heading": -1.0210111049652701, + "angularVelocity": 0.31474079388031545, + "velocityX": 0.25217138924859694, + "velocityY": -0.41042423598715305, + "timestamp": 0.11184001676697718 + }, + { + "x": 0.7057920861320249, + "y": 4.340710150429397, + "heading": -0.99496347376924, + "angularVelocity": 0.4658016325283906, + "velocityX": 0.3783719076090773, + "velocityY": -0.6158487090273924, + "timestamp": 0.16776002515046576 + }, + { + "x": 0.7340126708459819, + "y": 4.29477510174039, + "heading": -0.9607567098648512, + "angularVelocity": 0.6117088479280167, + "velocityX": 0.5046598798845959, + "velocityY": -0.8214420923185946, + "timestamp": 0.22368003353395435 + }, + { + "x": 0.7693006604451206, + "y": 4.237332365746024, + "heading": -0.918731573784226, + "angularVelocity": 0.7515223494321578, + "velocityX": 0.6310440684690269, + "velocityY": -1.0272304610620717, + "timestamp": 0.2796000419174429 + }, + { + "x": 0.8116619611973517, + "y": 4.168369327121192, + "heading": -0.8692948358062459, + "angularVelocity": 0.8840617054087768, + "velocityX": 0.7575338769931039, + "velocityY": -1.2332444257142754, + "timestamp": 0.3355200503009315 + }, + { + "x": 0.8611030812908155, + "y": 4.087871426229515, + "heading": -0.8129373545696883, + "angularVelocity": 1.0078231900479873, + "velocityX": 0.8841400694078274, + "velocityY": -1.4395187557848361, + "timestamp": 0.3914400586844201 + }, + { + "x": 0.9176312901451578, + "y": 3.995821968070834, + "heading": -0.7502611156342615, + "angularVelocity": 1.120819555419329, + "velocityX": 1.0108762585778373, + "velocityY": -1.6460916373156267, + "timestamp": 0.4473600670679087 + }, + { + "x": 0.9812548770105831, + "y": 3.8922019971306105, + "heading": -0.6820246305048665, + "angularVelocity": 1.2202516970570252, + "velocityX": 1.1377606818136925, + "velocityY": -1.8530034943775027, + "timestamp": 0.5032800754513973 + }, + { + "x": 1.0519833618359966, + "y": 3.77699034023748, + "heading": -0.6092267013635411, + "angularVelocity": 1.3018225720227254, + "velocityX": 1.2648153473148886, + "velocityY": -2.060293984633039, + "timestamp": 0.5592000838348858 + }, + { + "x": 1.129826908295346, + "y": 3.6501643798944907, + "heading": -0.5332724156478236, + "angularVelocity": 1.3582667083101592, + "velocityX": 1.3920517666147918, + "velocityY": -2.26798893650447, + "timestamp": 0.6151200922183744 + }, + { + "x": 1.214791954900265, + "y": 3.511704401985981, + "heading": -0.45632702327380104, + "angularVelocity": 1.3759903583409006, + "velocityX": 1.5194033238021893, + "velocityY": -2.4760364297333126, + "timestamp": 0.6710401006018629 + }, + { + "x": 1.306860167939052, + "y": 3.361616595640326, + "heading": -0.38215505844470404, + "angularVelocity": 1.3263940219829728, + "velocityX": 1.6464270249639614, + "velocityY": -2.6839732447173734, + "timestamp": 0.7269601089853515 + }, + { + "x": 1.4058792704184802, + "y": 3.2000739494751453, + "heading": -0.3184323584737964, + "angularVelocity": 1.1395330904442935, + "velocityX": 1.770727604337509, + "velocityY": -2.888816558419535, + "timestamp": 0.78288011736884 + }, + { + "x": 1.5121691709784744, + "y": 3.0300749520186443, + "heading": -0.28662541962876287, + "angularVelocity": 0.5687935278354703, + "velocityX": 1.9007490097476158, + "velocityY": -3.040038840671852, + "timestamp": 0.8388001257523285 + }, + { + "x": 1.6244650109400063, + "y": 2.8508645326617055, + "heading": -0.2866253125429205, + "angularVelocity": 0.0000019149825875479786, + "velocityX": 2.00815134345883, + "velocityY": -3.2047638141959642, + "timestamp": 0.8947201341358171 + }, + { + "x": 1.735288205529194, + "y": 2.670739688820617, + "heading": -0.28662527835532886, + "angularVelocity": 6.113659969296124e-7, + "velocityX": 1.9818164873864772, + "velocityY": -3.2211161809173365, + "timestamp": 0.9506401425193056 + }, + { + "x": 1.8506362591615513, + "y": 2.493478394875359, + "heading": -0.2866252441530595, + "angularVelocity": 6.116284727574992e-7, + "velocityX": 2.062733124811472, + "velocityY": -3.1699082147776885, + "timestamp": 1.0065601509027942 + }, + { + "x": 1.9771720781105007, + "y": 2.324022128559085, + "heading": -0.2866252095630549, + "angularVelocity": 6.185622213406466e-7, + "velocityX": 2.2628004288052175, + "velocityY": -3.0303333496335663, + "timestamp": 1.0624801592862827 + }, + { + "x": 2.1143701177449694, + "y": 2.1630766803808767, + "heading": -0.28662517392184667, + "angularVelocity": 6.373605666632096e-7, + "velocityX": 2.4534695827223683, + "velocityY": -2.878137053815794, + "timestamp": 1.1184001676697712 + }, + { + "x": 2.261659606829455, + "y": 2.011311910787294, + "heading": -0.2866251364727607, + "angularVelocity": 6.696902775127292e-7, + "velocityX": 2.633931813357441, + "velocityY": -2.7139618533818717, + "timestamp": 1.1743201760532598 + }, + { + "x": 2.418427669547749, + "y": 1.8693594092228303, + "heading": -0.2866250962912339, + "angularVelocity": 7.185536621676126e-7, + "velocityX": 2.803434177677683, + "velocityY": -2.538492136678184, + "timestamp": 1.2302401844367483 + }, + { + "x": 2.5840219554137547, + "y": 1.7378099138847216, + "heading": -0.28662505217416817, + "angularVelocity": 7.889316720181228e-7, + "velocityX": 2.961270762521925, + "velocityY": -2.3524584337678864, + "timestamp": 1.2861601928202369 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.3444811403993986, - "angularVelocity": 0.0000012577088124740217, - "velocityX": 3.3509722173532026, - "velocityY": -2.575383288874084, - "timestamp": 1.3542872703345679 - }, - { - "x": 2.960734343821119, - "y": 1.4779114817648484, - "heading": -0.34448108051633547, - "angularVelocity": 0.0000010280352851970435, - "velocityX": 3.4846514266808644, - "velocityY": -2.391405414524714, - "timestamp": 1.4125372778029122 - }, - { - "x": 3.170914398347007, - "y": 1.349732150860621, - "heading": -0.34448103072468966, - "angularVelocity": 8.54792093887579e-7, - "velocityX": 3.6082408167949036, - "velocityY": -2.200503252705787, - "timestamp": 1.4707872852712565 - }, - { - "x": 3.387684960924611, - "y": 1.2330440117382415, - "heading": -0.3444809881196843, - "angularVelocity": 7.31416307943616e-7, - "velocityX": 3.7213825714169735, - "velocityY": -2.00322959934028, - "timestamp": 1.5290372927396008 - }, - { - "x": 3.610418374312036, - "y": 1.1281849298444195, - "heading": -0.3444809507762438, - "angularVelocity": 6.410890256196379e-7, - "velocityX": 3.8237490957999993, - "velocityY": -1.80015568153888, - "timestamp": 1.587287300207945 - }, - { - "x": 3.8384697161434858, - "y": 1.0354585179376077, - "heading": -0.34448091735896624, - "angularVelocity": 5.736870952656925e-7, - "velocityX": 3.9150439930051975, - "velocityY": -1.591869528209129, - "timestamp": 1.6455373076762894 - }, - { - "x": 4.071178666241736, - "y": 0.9551332560900274, - "heading": -0.3444808869037787, - "angularVelocity": 5.228357697616013e-7, - "velocityX": 3.995002922956148, - "velocityY": -1.3789742755180425, - "timestamp": 1.7037873151446337 - }, - { - "x": 4.307871418551335, - "y": 0.8874417125102056, - "heading": -0.34448085868861794, - "angularVelocity": 4.843803799821142e-7, - "velocityX": 4.063394368459522, - "velocityY": -1.1620864360679888, - "timestamp": 1.762037322612978 - }, - { - "x": 4.547862632165302, - "y": 0.8325798662811171, - "heading": -0.3444808321530075, - "angularVelocity": 4.555469028757283e-7, - "velocityX": 4.120020306338885, - "velocityY": -0.9418341492729086, - "timestamp": 1.8202873300813223 - }, - { - "x": 4.7904574158495095, - "y": 0.7907065304618335, - "heading": -0.3444808068457288, - "angularVelocity": 4.3445966396412105e-7, - "velocityX": 4.164716782500731, - "velocityY": -0.7188554583798012, - "timestamp": 1.8785373375496666 - }, - { - "x": 5.03495334055107, - "y": 0.7619428639166234, - "heading": -0.34448078238917407, - "angularVelocity": 4.198549626958455e-7, - "velocityX": 4.197354392348067, - "velocityY": -0.4937967872509155, - "timestamp": 1.9367873450180109 - }, - { - "x": 5.280642475492497, - "y": 0.7463718997323922, - "heading": -0.3444807584537773, - "angularVelocity": 4.109080464550885e-7, - "velocityX": 4.217838685685062, - "velocityY": -0.26731265558537776, - "timestamp": 1.9950373524863552 - }, - { - "x": 5.526813463070627, - "y": 0.7440371744430943, - "heading": -0.34448073473848534, - "angularVelocity": 4.0712942424247657e-7, - "velocityX": 4.226110832894084, - "velocityY": -0.040081115707442425, - "timestamp": 2.0532873599546995 - }, - { - "x": 5.766738873929576, - "y": 0.7458613898059425, - "heading": -0.32467851072076565, - "angularVelocity": 0.33995230006590477, - "velocityX": 4.118890645453325, - "velocityY": 0.03131699792210607, - "timestamp": 2.1115373674230438 - }, - { - "x": 5.994439408520209, - "y": 0.7477937318314798, - "heading": -0.2833894042686398, - "angularVelocity": 0.7088257709591617, - "velocityX": 3.9090215518749, - "velocityY": 0.033173249404078294, - "timestamp": 2.169787374891388 - }, - { - "x": 6.208925057122028, - "y": 0.7495883201901966, - "heading": -0.23844368677979322, - "angularVelocity": 0.7716002013093647, - "velocityX": 3.6821565854456084, - "velocityY": 0.030808379890630424, - "timestamp": 2.2280373823597324 - }, - { - "x": 6.410164836489075, - "y": 0.7512394294870858, - "heading": -0.1938705620031709, - "angularVelocity": 0.7652037607178938, - "velocityX": 3.454759717866313, - "velocityY": 0.028345220346736016, - "timestamp": 2.2862873898280767 - }, - { - "x": 6.598169956514198, - "y": 0.752743774939267, - "heading": -0.15150662184462524, - "angularVelocity": 0.7272778493902874, - "velocityX": 3.227555294774738, - "velocityY": 0.02582566968766568, - "timestamp": 2.344537397296421 - }, - { - "x": 6.772954120733039, - "y": 0.754099580476696, - "heading": -0.11241242955567449, - "angularVelocity": 0.6711448459503878, - "velocityX": 3.000586125483803, - "velocityY": 0.023275628559629353, - "timestamp": 2.4027874047647653 - }, - { - "x": 6.934529138966309, - "y": 0.7553058783616067, - "heading": -0.07728200268237817, - "angularVelocity": 0.6030973797280247, - "velocityX": 2.77381969987003, - "velocityY": 0.020708973909851786, - "timestamp": 2.4610374122331096 - }, - { - "x": 7.082904706348492, - "y": 0.7563621152126468, - "heading": -0.046606204008256526, - "angularVelocity": 0.52662308568445, - "velocityX": 2.547219714311956, - "velocityY": 0.018132819152241828, - "timestamp": 2.519287419701454 - }, - { - "x": 7.218088767056845, - "y": 0.7572679488303273, - "heading": -0.020751033175153387, - "angularVelocity": 0.4438655367924897, - "velocityX": 2.3207561094617386, - "velocityY": 0.015550789726041245, - "timestamp": 2.577537427169798 + "heading": -0.2866250016379229, + "angularVelocity": 9.037238488587988e-7, + "velocityX": 3.1067845266977008, + "velocityY": -2.1566350283233997, + "timestamp": 1.3420802012037254 + }, + { + "x": 2.964172022537395, + "y": 1.4952512181839386, + "heading": -0.28662495601958665, + "angularVelocity": 7.195928192095116e-7, + "velocityX": 3.256089340971223, + "velocityY": -1.9238160187086888, + "timestamp": 1.4054748531095118 + }, + { + "x": 3.1789518158723995, + "y": 1.3887033110695066, + "heading": -0.2866249156918973, + "angularVelocity": 6.361370893313497e-7, + "velocityX": 3.3879796935268014, + "velocityY": -1.6807081340674839, + "timestamp": 1.4688695050152982 + }, + { + "x": 3.4009440542760783, + "y": 1.2981369637223683, + "heading": -0.28662487900587236, + "angularVelocity": 5.786927430417384e-7, + "velocityX": 3.501750253847766, + "velocityY": -1.4286117933375984, + "timestamp": 1.5322641569210846 + }, + { + "x": 3.6289614625982565, + "y": 1.2240364953903842, + "heading": -0.2866248447654741, + "angularVelocity": 5.401149339588049e-7, + "velocityX": 3.596792496960864, + "velocityY": -1.168875703302355, + "timestamp": 1.595658808826871 + }, + { + "x": 3.8617845298582507, + "y": 1.1667980667810436, + "heading": -0.2866248120410035, + "angularVelocity": 5.162023862741612e-7, + "velocityX": 3.672597928386812, + "velocityY": -0.9028904945231835, + "timestamp": 1.6590534607326575 + }, + { + "x": 4.0981679647792335, + "y": 1.1267270015307347, + "heading": -0.28662478005254766, + "angularVelocity": 5.045923416827531e-7, + "velocityX": 3.728759884544863, + "velocityY": -0.6320890492444092, + "timestamp": 1.7224481126384439 + }, + { + "x": 4.3358126512762265, + "y": 1.0949807955483843, + "heading": -0.28662474820722456, + "angularVelocity": 5.023345368252209e-7, + "velocityX": 3.7486551207847545, + "velocityY": -0.5007710434238176, + "timestamp": 1.7858427645442303 + }, + { + "x": 4.573457419312962, + "y": 1.06323519995946, + "heading": -0.28662471636190573, + "angularVelocity": 5.023344693474633e-7, + "velocityX": 3.7486564070090496, + "velocityY": -0.5007614149550472, + "timestamp": 1.8492374164500167 + }, + { + "x": 4.811102187354235, + "y": 1.0314896044045172, + "heading": -0.28662468451658685, + "angularVelocity": 5.02334469511756e-7, + "velocityX": 3.748656407080655, + "velocityY": -0.5007614144190154, + "timestamp": 1.9126320683558031 + }, + { + "x": 5.04874695539551, + "y": 0.9997440088495764, + "heading": -0.286624652671268, + "angularVelocity": 5.023344701827313e-7, + "velocityX": 3.748656407080659, + "velocityY": -0.5007614144189856, + "timestamp": 1.9760267202615895 + }, + { + "x": 5.286391723436784, + "y": 0.9679984132946354, + "heading": -0.2866246208259492, + "angularVelocity": 5.023344694262712e-7, + "velocityX": 3.7486564070806585, + "velocityY": -0.500761414418986, + "timestamp": 2.039421372167376 + }, + { + "x": 5.524036491478058, + "y": 0.9362528177396949, + "heading": -0.28662458898063026, + "angularVelocity": 5.023344704301764e-7, + "velocityX": 3.74865640708066, + "velocityY": -0.5007614144189806, + "timestamp": 2.1028160240731624 + }, + { + "x": 5.761681259520114, + "y": 0.9045072221906024, + "heading": -0.2866245571353113, + "angularVelocity": 5.02334470827004e-7, + "velocityX": 3.748656407092983, + "velocityY": -0.5007614143267314, + "timestamp": 2.166210675978949 + }, + { + "x": 5.99932604159473, + "y": 0.8727617316882137, + "heading": -0.28662452528999255, + "angularVelocity": 5.023344683092169e-7, + "velocityX": 3.748656628445413, + "velocityY": -0.5007597572988834, + "timestamp": 2.229605327884735 + }, + { + "x": 6.237212534637086, + "y": 0.8428813906316717, + "heading": -0.286624491558187, + "angularVelocity": 5.320922916664829e-7, + "velocityX": 3.7524694259050357, + "velocityY": -0.47133851450037906, + "timestamp": 2.2929999797905216 + }, + { + "x": 6.4638245505635705, + "y": 0.8220107849523657, + "heading": -0.24424436180847475, + "angularVelocity": 0.6685126974542769, + "velocityX": 3.5746235544169, + "velocityY": -0.32921713507194117, + "timestamp": 2.356394631696308 + }, + { + "x": 6.673647733933235, + "y": 0.8041360764949451, + "heading": -0.18514255243378552, + "angularVelocity": 0.9322838377994884, + "velocityX": 3.309793130207448, + "velocityY": -0.2819592492436893, + "timestamp": 2.4197892836020944 + }, + { + "x": 6.866141134149738, + "y": 0.7888964331390943, + "heading": -0.12766912670980685, + "angularVelocity": 0.9065973863125305, + "velocityX": 3.0364296424022634, + "velocityY": -0.24039320191392657, + "timestamp": 2.483183935507881 + }, + { + "x": 7.04134978279503, + "y": 0.7761674362974819, + "heading": -0.07636429138531158, + "angularVelocity": 0.8092927996630003, + "velocityX": 2.7637764918352543, + "velocityY": -0.20078975842520036, + "timestamp": 2.5465785874136673 + }, + { + "x": 7.19932046714385, + "y": 0.765888430525111, + "heading": -0.03338684229285238, + "angularVelocity": 0.6779349330023905, + "velocityX": 2.4918613731579065, + "velocityY": -0.16214310613530766, + "timestamp": 2.6099732393194537 }, { "x": 7.340087890625, "y": 0.7580231428146362, "heading": 0, - "angularVelocity": 0.35624086720384496, - "velocityX": 2.0944052862903964, - "velocityY": 0.012964701931058227, - "timestamp": 2.6357874346381425 - }, - { - "x": 7.452016074368774, - "y": 0.7586421596982608, - "heading": 0.015744597720939148, - "angularVelocity": 0.26174771257837953, - "velocityX": 1.8607618046043786, - "velocityY": 0.010290910965644757, - "timestamp": 2.6959392409933733 - }, - { - "x": 7.549878171503854, - "y": 0.7590935449079357, - "heading": 0.02631445528043402, - "angularVelocity": 0.17571970319684052, - "velocityX": 1.626918675677823, - "velocityY": 0.007504100658407957, - "timestamp": 2.756091047348604 - }, - { - "x": 7.633665265890791, - "y": 0.7593717259908211, - "heading": 0.032122080946348684, - "angularVelocity": 0.09654948068587346, - "velocityX": 1.3929273194578256, - "velocityY": 0.004624650525748951, - "timestamp": 2.816242853703835 - }, - { - "x": 7.703370537390185, - "y": 0.7594720434745997, - "heading": 0.03350862227223397, - "angularVelocity": 0.023050701382050013, - "velocityX": 1.1588225811166206, - "velocityY": 0.001667738507906577, - "timestamp": 2.876394660059066 - }, - { - "x": 7.758988639574762, - "y": 0.7593905387990408, - "heading": 0.0307610262741512, - "angularVelocity": -0.04567769722253501, - "velocityX": 0.9246289605355835, - "velocityY": -0.0013549830087877977, - "timestamp": 2.936546466414297 - }, - { - "x": 7.800515294542927, - "y": 0.7591238044421035, - "heading": 0.024124065366989432, - "angularVelocity": -0.11033685119889487, - "velocityX": 0.6903642215318662, - "velocityY": -0.004434353232251388, - "timestamp": 2.9966982727695277 - }, - { - "x": 7.827947019588505, - "y": 0.7586688745154345, - "heading": 0.01380900494204492, - "angularVelocity": -0.17148380156745927, - "velocityX": 0.4560415839148344, - "velocityY": -0.007563030177051137, - "timestamp": 3.0568500791247586 + "angularVelocity": 0.5266507708326882, + "velocityX": 2.220493673351972, + "velocityY": -0.12406863156475333, + "timestamp": 2.67336789122524 + }, + { + "x": 7.464023522460179, + "y": 0.7525372143128384, + "heading": 0.023010119360387892, + "angularVelocity": 0.36178963616626764, + "velocityX": 1.9486481772395672, + "velocityY": -0.08625561847872552, + "timestamp": 2.736968716290925 + }, + { + "x": 7.57058231953554, + "y": 0.7491648354718851, + "heading": 0.03698370022131657, + "angularVelocity": 0.21970754068830903, + "velocityX": 1.6754310492876554, + "velocityY": -0.05302413667543652, + "timestamp": 2.8005695413566096 + }, + { + "x": 7.659708944521439, + "y": 0.7476928551810322, + "heading": 0.04295335113137195, + "angularVelocity": 0.09386121805637215, + "velocityX": 1.4013438488235506, + "velocityY": -0.02314404395434409, + "timestamp": 2.8641703664222944 + }, + { + "x": 7.731365798341117, + "y": 0.7479588271083956, + "heading": 0.041693436231508264, + "angularVelocity": -0.019809725716020507, + "velocityX": 1.1266654755763426, + "velocityY": 0.004181894292859409, + "timestamp": 2.927771191487979 + }, + { + "x": 7.785525986892143, + "y": 0.7498349370538775, + "heading": 0.03380693253041854, + "angularVelocity": -0.1240000219013676, + "velocityX": 0.8515642445690118, + "velocityY": 0.029498201376229376, + "timestamp": 2.991372016553664 + }, + { + "x": 7.822169492500932, + "y": 0.7532180353995639, + "heading": 0.01977708435408761, + "angularVelocity": -0.22059223542841888, + "velocityX": 0.5761482743493486, + "velocityY": 0.05319268016086978, + "timestamp": 3.0549728416193487 }, { "x": 7.841280937194824, "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": -0.2295692478542545, - "velocityX": 0.22167110872074697, - "velocityY": -0.010735034239617657, - "timestamp": 3.1170018854799895 - }, - { - "x": 7.835826713549701, - "y": 0.7569095778214322, - "heading": -0.022558536561860222, - "angularVelocity": -0.299041202712131, - "velocityX": -0.07230245606695226, - "velocityY": -0.014761676314980546, - "timestamp": 3.1924381008150915 - }, - { - "x": 7.80819402127163, - "y": 0.7554921737702587, - "heading": -0.05020545416339097, - "angularVelocity": -0.3664939641883925, - "velocityX": -0.36630538999500056, - "velocityY": -0.018789437472137644, - "timestamp": 3.2678743161501935 - }, - { - "x": 7.758380386118448, - "y": 0.7537708531649631, - "heading": -0.08276554497145149, - "angularVelocity": -0.43162412991455607, - "velocityX": -0.6603411230520984, - "velocityY": -0.02281822593630055, - "timestamp": 3.3433105314852956 - }, - { - "x": 7.686383032454013, - "y": 0.7517455632733434, - "heading": -0.12003503692740472, - "angularVelocity": -0.494053045879821, - "velocityX": -0.9544136505869183, - "velocityY": -0.026847713430781565, - "timestamp": 3.4187467468203976 - }, - { - "x": 7.592198832311897, - "y": 0.7494162995505907, - "heading": -0.1617736436152976, - "angularVelocity": -0.5532966692785698, - "velocityX": -1.2485276431715386, - "velocityY": -0.030877261172313774, - "timestamp": 3.4941829621554996 - }, - { - "x": 7.475824246300941, - "y": 0.7467831362501152, - "heading": -0.2076930920063058, - "angularVelocity": -0.6087188784196724, - "velocityX": -1.542688554747834, - "velocityY": -0.0349058245933778, - "timestamp": 3.5696191774906016 - }, - { - "x": 7.337255260610424, - "y": 0.7438462658074466, - "heading": -0.257440012481047, - "angularVelocity": -0.6594567377718513, - "velocityX": -1.8369026743318102, - "velocityY": -0.03893183704435113, - "timestamp": 3.6450553928257037 - }, - { - "x": 7.1764873367279565, - "y": 0.7406060479084567, - "heading": -0.31056943932971864, - "angularVelocity": -0.7042960282758145, - "velocityX": -2.131176957490571, - "velocityY": -0.04295308141581948, - "timestamp": 3.7204916081608057 - }, - { - "x": 6.9935154268341, - "y": 0.7370630667353528, - "heading": -0.3665018715735503, - "angularVelocity": -0.741453319143453, - "velocityX": -2.425518155716889, - "velocityY": -0.04696658173219917, - "timestamp": 3.7959278234959077 - }, - { - "x": 6.788334219229139, - "y": 0.7332181873732633, - "heading": -0.42444960466432774, - "angularVelocity": -0.768168615476831, - "velocityX": -2.719929767068861, - "velocityY": -0.050968614279096394, - "timestamp": 3.8713640388310098 - }, - { - "x": 6.560939151808715, - "y": 0.7290725776308253, - "heading": -0.48328030857318144, - "angularVelocity": -0.7798734818219146, - "velocityX": -3.0144018547363567, - "velocityY": -0.05495516608332171, - "timestamp": 3.946800254166112 - }, - { - "x": 6.311330185153471, - "y": 0.724627567965158, - "heading": -0.5412355901112249, - "angularVelocity": -0.7682686794478624, - "velocityX": -3.3088744649560358, - "velocityY": -0.05892408103881697, - "timestamp": 4.022236469501213 - }, - { - "x": 6.039527461477683, - "y": 0.7198838011192613, - "heading": -0.5952481542708341, - "angularVelocity": -0.7160031016889598, - "velocityX": -3.6030800653027115, - "velocityY": -0.06288447564374298, - "timestamp": 4.097672684836315 - }, - { - "x": 5.745659621866784, - "y": 0.7148372933474686, - "heading": -0.638783124761932, - "angularVelocity": -0.5771096852845462, - "velocityX": -3.895580369527838, - "velocityY": -0.06689767970695078, - "timestamp": 4.173108900171417 - }, - { - "x": 5.431003034025849, - "y": 0.7096634777046316, - "heading": -0.6507371958403388, - "angularVelocity": -0.15846594404696066, - "velocityX": -4.17116084685812, - "velocityY": -0.0685853024287286, - "timestamp": 4.248545115506518 - }, - { - "x": 5.1125615609654815, - "y": 0.7251156489118818, - "heading": -0.6507372106959876, - "angularVelocity": -1.9692993052752515e-7, - "velocityX": -4.221334164840962, - "velocityY": 0.2048375722271958, - "timestamp": 4.32398133084162 - }, - { - "x": 4.795969345130646, - "y": 0.7627070022190265, - "heading": -0.6507372258766643, - "angularVelocity": -2.012385798908336e-7, - "velocityX": -4.196819981337514, - "velocityY": 0.49831971474386, - "timestamp": 4.399417546176721 - }, - { - "x": 4.482763788472813, - "y": 0.8222554661406114, - "heading": -0.6507372417212374, - "angularVelocity": -2.1003934401896774e-7, - "velocityX": -4.151925640311049, - "velocityY": 0.7893882753404204, - "timestamp": 4.474853761511823 - }, - { - "x": 4.174465853368726, - "y": 0.9034719819930799, - "heading": -0.6507372586342053, - "angularVelocity": -2.2420223152003646e-7, - "velocityX": -4.0868690685842415, - "velocityY": 1.0766250068576386, - "timestamp": 4.5502899768469245 - }, - { - "x": 3.8725726714422697, - "y": 1.0059621985442242, - "heading": -0.6507372771356286, - "angularVelocity": -2.452591658573351e-7, - "velocityX": -4.0019661721546855, - "velocityY": 1.3586341268031932, - "timestamp": 4.625726192182026 - }, - { - "x": 3.5785502725752965, - "y": 1.1292284338722989, - "heading": -0.6507372979388782, - "angularVelocity": -2.757727119617825e-7, - "velocityX": -3.897629242942143, - "velocityY": 1.6340458595450806, - "timestamp": 4.701162407517128 - }, - { - "x": 3.293826465550643, - "y": 1.2726721052481516, - "heading": -0.6507373220853944, - "angularVelocity": -3.200918277895427e-7, - "velocityX": -3.7743649487166713, - "velocityY": 1.9015226405334464, - "timestamp": 4.776598622852229 - }, - { - "x": 3.019783904481099, - "y": 1.4355966406757221, - "heading": -0.6507373511975181, - "angularVelocity": -3.859170793282021e-7, - "velocityX": -3.632771870277356, - "velocityY": 2.1597655012758525, - "timestamp": 4.852034838187331 + "heading": 2.046055081284657e-30, + "angularVelocity": -0.310956411865142, + "velocityX": 0.30049051524338544, + "velocityY": 0.07555102327854848, + "timestamp": 3.1185736666850334 + }, + { + "x": 7.837626456422163, + "y": 0.7661832239405314, + "heading": -0.033360531548023535, + "angularVelocity": -0.41786111882102345, + "velocityX": -0.045774613098587845, + "velocityY": 0.10221002096529477, + "timestamp": 3.1984100765480727 + }, + { + "x": 7.806322741144082, + "y": 0.7764707746744537, + "heading": -0.07501113915617402, + "angularVelocity": -0.5216994060680163, + "velocityX": -0.3920982335225698, + "velocityY": 0.12885788265743264, + "timestamp": 3.278246486411112 + }, + { + "x": 7.74736425619444, + "y": 0.7888847549403224, + "heading": -0.12465356813987538, + "angularVelocity": -0.6218018704606533, + "velocityX": -0.738491185297363, + "velocityY": 0.15549271675874554, + "timestamp": 3.3580828962741514 + }, + { + "x": 7.660744366778727, + "y": 0.8034239689194149, + "heading": -0.18191677738707485, + "angularVelocity": -0.7172568173523243, + "velocityX": -1.084967241942757, + "velocityY": 0.18211257249711932, + "timestamp": 3.4379193061371907 + }, + { + "x": 7.5464550145644145, + "y": 0.8200870575241468, + "heading": -0.24632578527266594, + "angularVelocity": -0.8067623280666757, + "velocityX": -1.4315442341455176, + "velocityY": 0.20871540482992282, + "timestamp": 3.51775571600023 + }, + { + "x": 7.404486266909035, + "y": 0.8388724767291542, + "heading": -0.3172478637527482, + "angularVelocity": -0.8883425319569115, + "velocityX": -1.7782456387872365, + "velocityY": 0.23529889729803788, + "timestamp": 3.5975921258632693 + }, + { + "x": 7.234825726560123, + "y": 0.8597784078771571, + "heading": -0.3937913075224082, + "angularVelocity": -0.958753579989022, + "velocityX": -2.1251023266197273, + "velocityY": 0.2618596099682753, + "timestamp": 3.6774285357263086 + }, + { + "x": 7.037458048630302, + "y": 0.8828023770067691, + "heading": -0.474592846440749, + "angularVelocity": -1.0120888333650895, + "velocityX": -2.472151218578179, + "velocityY": 0.28838933475478196, + "timestamp": 3.757264945589348 + }, + { + "x": 6.812366694532574, + "y": 0.9079395818644894, + "heading": -0.5572950195818104, + "angularVelocity": -1.0358954427301792, + "velocityX": -2.8194072664825045, + "velocityY": 0.3148589083707988, + "timestamp": 3.837101355452387 + }, + { + "x": 6.559557151686243, + "y": 0.9351738572850871, + "heading": -0.6368741954392075, + "angularVelocity": -0.996777986308708, + "velocityX": -3.1665945810943685, + "velocityY": 0.3411260033776375, + "timestamp": 3.9169377653154265 + }, + { + "x": 6.2794552371906684, + "y": 0.9644007497338529, + "heading": -0.6963848347023115, + "angularVelocity": -0.7454072567290441, + "velocityX": -3.5084482753682624, + "velocityY": 0.3660847537972336, + "timestamp": 3.996774175178466 + }, + { + "x": 5.978414020607044, + "y": 0.9876514129078968, + "heading": -0.6963848502753711, + "angularVelocity": -1.950621226343756e-7, + "velocityX": -3.770725876828189, + "velocityY": 0.2912288167006853, + "timestamp": 4.0766105850415055 + }, + { + "x": 5.677372687056552, + "y": 1.010900561597095, + "heading": -0.6963848658464019, + "angularVelocity": -1.9503671222417687e-7, + "velocityX": -3.770727341909935, + "velocityY": 0.29120984684910667, + "timestamp": 4.156446994904545 + }, + { + "x": 5.376331353502739, + "y": 1.034149710243266, + "heading": -0.6963848814174328, + "angularVelocity": -1.9503671215554748e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.2912098463101618, + "timestamp": 4.236283404767585 + }, + { + "x": 5.075290019948924, + "y": 1.0573988588894354, + "heading": -0.6963848969884637, + "angularVelocity": -1.9503671305455908e-7, + "velocityX": -3.770727341951559, + "velocityY": 0.2912098463101465, + "timestamp": 4.316119814630625 + }, + { + "x": 4.774248686395111, + "y": 1.0806480075356062, + "heading": -0.6963849125594946, + "angularVelocity": -1.95036712978891e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.29120984631016333, + "timestamp": 4.3959562244936645 + }, + { + "x": 4.473207352844907, + "y": 1.1038971562285251, + "heading": -0.6963849281305255, + "angularVelocity": -1.9503671242042344e-7, + "velocityX": -3.770727341906336, + "velocityY": 0.29120984689571133, + "timestamp": 4.475792634356704 + }, + { + "x": 4.172166146375442, + "y": 1.1271479503662634, + "heading": -0.6963849437015556, + "angularVelocity": -1.9503670063864683e-7, + "velocityX": -3.770725750142127, + "velocityY": 0.2912304571012814, + "timestamp": 4.555629044219744 + }, + { + "x": 3.8735860999949274, + "y": 1.1720518831992321, + "heading": -0.696384959370356, + "angularVelocity": -1.9626133629188135e-7, + "velocityX": -3.7398982105123864, + "velocityY": 0.5624492998871312, + "timestamp": 4.635465454082784 + }, + { + "x": 3.580403442801602, + "y": 1.244234542261993, + "heading": -0.6963849756495779, + "angularVelocity": -2.0390723854783083e-7, + "velocityX": -3.672292600535076, + "velocityY": 0.9041320769131684, + "timestamp": 4.7153018639458235 + }, + { + "x": 3.295105029858622, + "y": 1.3430840304472003, + "heading": -0.6963849932202171, + "angularVelocity": -2.2008303364523734e-7, + "velocityX": -3.573537605616451, + "velocityY": 1.2381504673717785, + "timestamp": 4.795138273808863 + }, + { + "x": 3.020110821637561, + "y": 1.4677619739384067, + "heading": -0.6963850130039023, + "angularVelocity": -2.4780279138256264e-7, + "velocityX": -3.4444711215448884, + "velocityY": 1.5616677115753714, + "timestamp": 4.874974683671903 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.6507373850576595, - "angularVelocity": -4.4885790275828266e-7, - "velocityX": -3.473537625459172, - "velocityY": 2.40752036058895, - "timestamp": 4.927471053522432 - }, - { - "x": 2.56955568155037, - "y": 1.762694428612382, - "heading": -0.6507374187199763, - "angularVelocity": -5.980784656519904e-7, - "velocityX": -3.34370885712208, - "velocityY": 2.5848068511762112, - "timestamp": 4.98375516756578 - }, - { - "x": 2.389174031729705, - "y": 1.917763125748239, - "heading": -0.6507374476220785, - "angularVelocity": -5.135037275394257e-7, - "velocityX": -3.2048412396034367, - "velocityY": 2.7551059436847196, - "timestamp": 5.040039281609128 - }, - { - "x": 2.21709605626624, - "y": 2.0819977501361024, - "heading": -0.6507374788972222, - "angularVelocity": -5.556655576645871e-7, - "velocityX": -3.0573098357902078, - "velocityY": 2.917956996913463, - "timestamp": 5.096323395652476 - }, - { - "x": 2.053846284333076, - "y": 2.2548939585643604, - "heading": -0.6510333038480676, - "angularVelocity": -0.005255922667952023, - "velocityX": -2.900459120799771, - "velocityY": 3.071847382995126, - "timestamp": 5.152607509695824 - }, - { - "x": 1.9011339963459661, - "y": 2.4339551936894854, - "heading": -0.6596386701119549, - "angularVelocity": -0.15289156470082665, - "velocityX": -2.7132396162351555, - "velocityY": 3.1813814282875046, - "timestamp": 5.208891623739172 - }, - { - "x": 1.7604826838741388, - "y": 2.6146352624702294, - "heading": -0.6772772697494215, - "angularVelocity": -0.3133850454478603, - "velocityX": -2.4989522330137985, - "velocityY": 3.2101432500401494, - "timestamp": 5.26517573778252 - }, - { - "x": 1.6317544686478245, - "y": 2.792388897989417, - "heading": -0.7009679816171792, - "angularVelocity": -0.4209129391201239, - "velocityX": -2.287114533368532, - "velocityY": 3.158149302701766, - "timestamp": 5.321459851825868 - }, - { - "x": 1.5139177763229132, - "y": 2.964350304348354, - "heading": -0.7282265000249424, - "angularVelocity": -0.48430216715802665, - "velocityX": -2.0936048177671953, - "velocityY": 3.0552387522080946, - "timestamp": 5.377743965869215 - }, - { - "x": 1.405939966332392, - "y": 3.1288586513458805, - "heading": -0.7573592985064115, - "angularVelocity": -0.5176025060824792, - "velocityX": -1.9184420297947733, - "velocityY": 2.922820227228369, - "timestamp": 5.434028079912563 - }, - { - "x": 1.3069897960630168, - "y": 3.2849152039001472, - "heading": -0.7872314504581, - "angularVelocity": -0.5307386011030019, - "velocityX": -1.7580479314850108, - "velocityY": 2.7726571734624392, - "timestamp": 5.490312193955911 - }, - { - "x": 1.2164244610459272, - "y": 3.431880015003849, - "heading": -0.81704954760011, - "angularVelocity": -0.5297782091594296, - "velocityX": -1.609074541838559, - "velocityY": 2.6111241795600484, - "timestamp": 5.546596307999259 - }, - { - "x": 1.133744514509131, - "y": 3.569317973113813, - "heading": -0.8462342157208796, - "angularVelocity": -0.5185240740982935, - "velocityX": -1.468974824283782, - "velocityY": 2.4418605577430528, - "timestamp": 5.602880422042607 - }, - { - "x": 1.0585554462199285, - "y": 3.696918241093708, - "heading": -0.8743466079980574, - "angularVelocity": -0.4994729464076927, - "velocityX": -1.3358843710553012, - "velocityY": 2.2670742917197226, - "timestamp": 5.659164536085955 - }, - { - "x": 0.990539814673681, - "y": 3.8144496515109747, - "heading": -0.9010441731548517, - "angularVelocity": -0.4743357092950377, - "velocityX": -1.2084339018619632, - "velocityY": 2.088180873323288, - "timestamp": 5.715448650129303 - }, - { - "x": 0.9294376679920158, - "y": 3.9217345696371626, - "heading": -0.9260527900785384, - "angularVelocity": -0.4443281616625532, - "velocityX": -1.0856019983650476, - "velocityY": 1.9061314182463815, - "timestamp": 5.771732764172651 - }, - { - "x": 0.8750327428643173, - "y": 4.018632780962349, - "heading": -0.949148487110952, - "angularVelocity": -0.4103413089993085, - "velocityX": -0.9666124456680172, - "velocityY": 1.7215907716084669, - "timestamp": 5.8280168782159985 - }, - { - "x": 0.8271425931537745, - "y": 4.105031112353838, - "heading": -0.9701450242376531, - "angularVelocity": -0.37304552951709063, - "velocityX": -0.8508644139562936, - "velocityY": 1.5350393776287605, - "timestamp": 5.884300992259346 - }, - { - "x": 0.7856114013922731, - "y": 4.180836489375294, - "heading": -0.9888852009339675, - "angularVelocity": -0.3329567679057974, - "velocityX": -0.7378847916041681, - "velocityY": 1.346834329897688, - "timestamp": 5.940585106302694 - }, - { - "x": 0.7503046474773418, - "y": 4.245971138892709, - "heading": -1.0052346109229755, - "angularVelocity": -0.2904800096243145, - "velocityX": -0.6272951882611009, - "velocityY": 1.157247486693155, - "timestamp": 5.996869220346042 - }, - { - "x": 0.7211050852240694, - "y": 4.300369179737215, - "heading": -1.0190770505330764, - "angularVelocity": -0.24593866040851983, - "velocityX": -0.5187886981890432, - "velocityY": 0.9664901325906992, - "timestamp": 6.05315333438939 - }, - { - "x": 0.6979096564056603, - "y": 4.343974139962992, - "heading": -1.0303110725490423, - "angularVelocity": -0.1995948982569619, - "velocityX": -0.4121132438994187, - "velocityY": 0.7747294412805966, - "timestamp": 6.109437448432738 - }, - { - "x": 0.6806270884657889, - "y": 4.376737109843248, - "heading": -1.038847350819041, - "angularVelocity": -0.15166407813444996, - "velocityX": -0.30705942935444774, - "velocityY": 0.5820997707279092, - "timestamp": 6.165721562476086 - }, - { - "x": 0.6691759989582504, - "y": 4.398615341726796, - "heading": -1.044606629501852, - "angularVelocity": -0.1023251192756673, - "velocityX": -0.20345153694200915, - "velocityY": 0.3887106025458067, - "timestamp": 6.222005676519434 + "heading": -0.6963850355061774, + "angularVelocity": -2.8185479639389566e-7, + "velocityX": -3.2861879673103584, + "velocityY": 1.8719390230438342, + "timestamp": 4.954811093534943 + }, + { + "x": 2.5741841985169076, + "y": 1.73878733799422, + "heading": -0.6963850589279512, + "angularVelocity": -4.0231077616908087e-7, + "velocityX": -3.153128257888759, + "velocityY": 2.0882929565548642, + "timestamp": 5.0130292054104135 + }, + { + "x": 2.3991894720533877, + "y": 1.872411165939459, + "heading": -0.6963850794155536, + "angularVelocity": -3.519111451084776e-7, + "velocityX": -3.0058468202788267, + "velocityY": 2.2952277846293376, + "timestamp": 5.071247317285884 + }, + { + "x": 2.2335584908852413, + "y": 2.017479635272907, + "heading": -0.6963850978369928, + "angularVelocity": -3.164211012595542e-7, + "velocityX": -2.845007779064253, + "velocityY": 2.491809930967045, + "timestamp": 5.129465429161355 + }, + { + "x": 2.0780383093475274, + "y": 2.173338406648706, + "heading": -0.6963851148088384, + "angularVelocity": -2.9152174806156023e-7, + "velocityX": -2.6713367460348776, + "velocityY": 2.6771526309404154, + "timestamp": 5.187683541036826 + }, + { + "x": 1.9333303562340614, + "y": 2.339284459115452, + "heading": -0.6963851307944149, + "angularVelocity": -2.745808111537906e-7, + "velocityX": -2.4856174213103777, + "velocityY": 2.850419691069807, + "timestamp": 5.245901652912297 + }, + { + "x": 1.8000872249814166, + "y": 2.514569234135199, + "heading": -0.6963851461633945, + "angularVelocity": -2.639896616028141e-7, + "velocityX": -2.288688639330214, + "velocityY": 3.0108289220145816, + "timestamp": 5.304119764787767 + }, + { + "x": 1.6789094155739503, + "y": 2.6984018293834224, + "heading": -0.6963851612308567, + "angularVelocity": -2.588105615203023e-7, + "velocityX": -2.0814451981312545, + "velocityY": 3.1576529936498847, + "timestamp": 5.362337876663238 + }, + { + "x": 1.5665736982883989, + "y": 2.8877670089616614, + "heading": -0.6963851762457395, + "angularVelocity": -2.5790741626665006e-7, + "velocityX": -1.9295664814042677, + "velocityY": 3.2526850060560912, + "timestamp": 5.420555988538709 + }, + { + "x": 1.4540456075762054, + "y": 3.0767463777174253, + "heading": -0.6971250426595835, + "angularVelocity": -0.012708526436355243, + "velocityX": -1.9328708384238367, + "velocityY": 3.246058016446739, + "timestamp": 5.47877410041418 + }, + { + "x": 1.3486317039814464, + "y": 3.2544802087721734, + "heading": -0.7253513181524348, + "angularVelocity": -0.48483666995638414, + "velocityX": -1.8106719747325546, + "velocityY": 3.052895831368142, + "timestamp": 5.53699221228965 + }, + { + "x": 1.2507109072486164, + "y": 3.4195717896549707, + "heading": -0.7601787682651484, + "angularVelocity": -0.598223628193407, + "velocityX": -1.6819644879978937, + "velocityY": 2.8357426162485435, + "timestamp": 5.595210324165121 + }, + { + "x": 1.160328320301069, + "y": 3.57195134860905, + "heading": -0.7969704460064531, + "angularVelocity": -0.6319627441714823, + "velocityX": -1.5524822780387875, + "velocityY": 2.617390946652855, + "timestamp": 5.653428436040592 + }, + { + "x": 1.0774888591394964, + "y": 3.71161238889488, + "heading": -0.8336344675120992, + "angularVelocity": -0.6297700204374731, + "velocityX": -1.422915626991949, + "velocityY": 2.398927683957996, + "timestamp": 5.711646547916063 + }, + { + "x": 1.0021908033919844, + "y": 3.838558158350536, + "heading": -0.8689739346571361, + "angularVelocity": -0.6070184347549549, + "velocityX": -1.2933785264038766, + "velocityY": 2.1805202086799973, + "timestamp": 5.7698646597915335 + }, + { + "x": 0.9344313462846833, + "y": 3.952793536595294, + "heading": -0.9022118432658017, + "angularVelocity": -0.5709204152783613, + "velocityX": -1.1638896371672067, + "velocityY": 1.9621965495739466, + "timestamp": 5.828082771667004 + }, + { + "x": 0.8742077702690397, + "y": 4.05432328906152, + "heading": -0.932802055793642, + "angularVelocity": -0.5254415085338586, + "velocityX": -1.0344474266781911, + "velocityY": 1.74395474527581, + "timestamp": 5.886300883542475 + }, + { + "x": 0.8215176865940929, + "y": 4.143151679881797, + "heading": -0.9603390876580661, + "angularVelocity": -0.47299768022924404, + "velocityX": -0.9050462472512313, + "velocityY": 1.5257861850670003, + "timestamp": 5.944518995417946 + }, + { + "x": 0.7763590444591775, + "y": 4.21928243186123, + "heading": -0.9845096003709082, + "angularVelocity": -0.4151717040316119, + "velocityX": -0.7756802939866984, + "velocityY": 1.307681570681621, + "timestamp": 6.002737107293417 + }, + { + "x": 0.7387300854642734, + "y": 4.282718776761074, + "heading": -1.0050639989905084, + "angularVelocity": -0.35305848914451915, + "velocityX": -0.6463445443815323, + "velocityY": 1.0896324675649904, + "timestamp": 6.060955219168887 + }, + { + "x": 0.7086292911290722, + "y": 4.333463522832413, + "heading": -1.0217986916878445, + "angularVelocity": -0.28744822115035373, + "velocityX": -0.5170348773863939, + "velocityY": 0.871631601173907, + "timestamp": 6.119173331044358 + }, + { + "x": 0.6860553360496396, + "y": 4.371519118466469, + "heading": -1.0345444282293665, + "angularVelocity": -0.21893077825652071, + "velocityX": -0.3877479765698806, + "velocityY": 0.6536727902728674, + "timestamp": 6.177391442919829 + }, + { + "x": 0.6710070489868055, + "y": 4.396887706764773, + "heading": -1.0431583071392123, + "angularVelocity": -0.14795874741302198, + "velocityX": -0.2584811938769611, + "velocityY": 0.43575079096636016, + "timestamp": 6.2356095547953 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -0.05172811727299292, - "velocityX": -0.10114075318843706, - "velocityY": 0.19465224364467987, - "timestamp": 6.2782897905627815 + "angularVelocity": -0.07488723824993972, + "velocityX": -0.12923242394972312, + "velocityY": 0.2178611369128848, + "timestamp": 6.29382766667077 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": 3.206426664477851e-31, - "velocityX": -6.202690310891061e-31, - "velocityY": -1.8966043908205797e-31, - "timestamp": 6.334573904606129 - }, - { - "x": 0.6687462669994619, - "y": 4.399468321543485, - "heading": -1.0440928198775505, - "angularVelocity": 0.06332262739330675, - "velocityX": 0.09729413850238322, - "velocityY": -0.18676978112239445, - "timestamp": 6.388666430724142 - }, - { - "x": 0.6793295244169436, - "y": 4.379291243512293, - "heading": -1.0373158700882419, - "angularVelocity": 0.1252844020356336, - "velocityX": 0.19565101090554332, - "velocityY": -0.3730104596559098, - "timestamp": 6.442758956842155 - }, - { - "x": 0.69529661974622, - "y": 4.349072044732176, - "heading": -1.0272687237370097, - "angularVelocity": 0.1857400101686365, - "velocityX": 0.2951811733539195, - "velocityY": -0.5586575623080092, - "timestamp": 6.496851482960168 - }, - { - "x": 0.716717978867713, - "y": 4.308846987227399, - "heading": -1.0140420515401747, - "angularVelocity": 0.24451940306772088, - "velocityX": 0.3960132879476041, - "velocityY": -0.7436342946359814, - "timestamp": 6.550944009078181 - }, - { - "x": 0.7436721888450865, - "y": 4.258657330572967, - "heading": -0.9977373757966207, - "angularVelocity": 0.30142196923729686, - "velocityX": 0.49829822919416444, - "velocityY": -0.9278482677074609, - "timestamp": 6.605036535196194 - }, - { - "x": 0.7762474872180782, - "y": 4.1985504141499295, - "heading": -0.9784691435647803, - "angularVelocity": 0.35620877068584267, - "velocityX": 0.6022144039229431, - "velocityY": -1.1111870851067975, - "timestamp": 6.659129061314207 - }, - { - "x": 0.8145436288745613, - "y": 4.128581066891007, - "heading": -0.9563673669616457, - "angularVelocity": 0.4085920586303395, - "velocityX": 0.7079747315326336, - "velocityY": -1.2935122886733275, - "timestamp": 6.71322158743222 - }, - { - "x": 0.8586742551972165, - "y": 4.0488134751688944, - "heading": -0.9315810408217653, - "angularVelocity": 0.4582209025648986, - "velocityX": 0.8158359294628605, - "velocityY": -1.4746508888900018, - "timestamp": 6.767314113550233 - }, - { - "x": 0.9087699412940919, - "y": 3.9593237057932655, - "heading": -0.9042826435219333, - "angularVelocity": 0.5046611659473402, - "velocityX": 0.926111048824076, - "velocityY": -1.6543832539918422, - "timestamp": 6.821406639668246 - }, - { - "x": 0.9649821731331605, - "y": 3.8602031891787973, - "heading": -0.8746741747762036, - "angularVelocity": 0.5473670924712444, - "velocityX": 1.0391866653860837, - "velocityY": -1.8324253594360185, - "timestamp": 6.875499165786259 - }, - { - "x": 1.0274886208992116, - "y": 3.751563649709137, - "heading": -0.8429954192038807, - "angularVelocity": 0.5856401585518797, - "velocityX": 1.1555468426359812, - "velocityY": -2.008402033818445, - "timestamp": 6.929591691904272 - }, - { - "x": 1.0965002487203657, - "y": 3.6335442878114614, - "heading": -0.8095355084845481, - "angularVelocity": 0.6185680928678664, - "velocityX": 1.275807080453678, - "velocityY": -2.181805331855407, - "timestamp": 6.983684218022285 - }, - { - "x": 1.1722710636909373, - "y": 3.5063225947671968, - "heading": -0.7746495035161819, - "angularVelocity": 0.6449320723557629, - "velocityX": 1.4007631073674283, - "velocityY": -2.351927376562676, - "timestamp": 7.037776744140298 - }, - { - "x": 1.2551116907799804, - "y": 3.370131275203577, - "heading": -0.738782856357727, - "angularVelocity": 0.6630610498773233, - "velocityX": 1.531461609098938, - "velocityY": -2.5177474475235027, - "timestamp": 7.091869270258311 - }, - { - "x": 1.3454084529571924, - "y": 3.2252859282148103, - "heading": -0.7025087034544132, - "angularVelocity": 0.6705945443190738, - "velocityX": 1.669302002648637, - "velocityY": -2.6777330877992154, - "timestamp": 7.145961796376324 - }, - { - "x": 1.4436499618435756, - "y": 3.0722326748820543, - "heading": -0.6665870109674555, - "angularVelocity": 0.6640786641844795, - "velocityX": 1.8161752821823995, - "velocityY": -2.829471358000162, - "timestamp": 7.200054322494337 - }, - { - "x": 1.5504618658475082, - "y": 2.9116347206115476, - "heading": -0.6320630523320445, - "angularVelocity": 0.6382389788923385, - "velocityX": 1.9746148251778084, - "velocityY": -2.96894905444256, - "timestamp": 7.25414684861235 - }, - { - "x": 1.666640324199563, - "y": 2.7445376431595645, - "heading": -0.6004402859276634, - "angularVelocity": 0.5846050956330832, - "velocityX": 2.147772838315358, - "velocityY": -3.0890973197909535, - "timestamp": 7.308239374730363 - }, - { - "x": 1.7931265785030042, - "y": 2.572686348472304, - "heading": -0.5739736412013764, - "angularVelocity": 0.48928468728727015, - "velocityX": 2.338331436539341, - "velocityY": -3.1769877840815965, - "timestamp": 7.362331900848376 - }, - { - "x": 1.9307039385810119, - "y": 2.3990237133183436, - "heading": -0.5558132076436828, - "angularVelocity": 0.33572907129927604, - "velocityX": 2.5433709599366927, - "velocityY": -3.2104737496406606, - "timestamp": 7.416424426966389 - }, - { - "x": 2.079064797054882, - "y": 2.227910687938415, - "heading": -0.5489525363882142, - "angularVelocity": 0.12683214758631672, - "velocityX": 2.742723794233161, - "velocityY": -3.16333951581913, - "timestamp": 7.470516953084402 - }, - { - "x": 2.237064005369136, - "y": 2.0627082520134143, - "heading": -0.548898243902152, - "angularVelocity": 0.0010036966332050488, - "velocityX": 2.920906447773384, - "velocityY": -3.0540713806611905, - "timestamp": 7.5246094792024145 - }, - { - "x": 2.40313061190331, - "y": 1.9055936598291874, - "heading": -0.5488982087618529, - "angularVelocity": 6.496331729658335e-7, - "velocityX": 3.0700471664443327, - "velocityY": -2.904552688872191, - "timestamp": 7.5787020053204275 - }, - { - "x": 2.5768383133172232, - "y": 1.7569708294797155, - "heading": -0.5488981704735678, - "angularVelocity": 7.078294853357702e-7, - "velocityX": 3.211306882491981, - "velocityY": -2.7475668269761684, - "timestamp": 7.63279453143844 + "angularVelocity": -7.364462273467936e-32, + "velocityX": -8.426727085890477e-35, + "velocityY": -7.327805300908389e-34, + "timestamp": 6.352045778546241 + }, + { + "x": 0.6687542994396359, + "y": 4.396916362187497, + "heading": -1.0350125956919525, + "angularVelocity": 0.2204003026897336, + "velocityX": 0.09289604490221867, + "velocityY": -0.22303166776742134, + "timestamp": 6.408785741736511 + }, + { + "x": 0.6793565646010321, + "y": 4.371607473302922, + "heading": -1.0104451675948027, + "angularVelocity": 0.4329827993501856, + "velocityX": 0.18685710327027932, + "velocityY": -0.4460504988292779, + "timestamp": 6.465525704926781 + }, + { + "x": 0.6953596015342451, + "y": 4.333646306767502, + "heading": -0.974331857670356, + "angularVelocity": 0.6364704503481122, + "velocityX": 0.2820417221553139, + "velocityY": -0.6690375601429702, + "timestamp": 6.522265668117051 + }, + { + "x": 0.7168434967725004, + "y": 4.283036502515157, + "heading": -0.9272707047547418, + "angularVelocity": 0.8294181079709282, + "velocityX": 0.3786378071168583, + "velocityY": -0.8919604703061096, + "timestamp": 6.5790056313073215 + }, + { + "x": 0.7439015517455688, + "y": 4.2197848874550585, + "heading": -0.8699519201955506, + "angularVelocity": 1.0102012996903078, + "velocityX": 0.4768782609592592, + "velocityY": -1.114763061230607, + "timestamp": 6.635745594497592 + }, + { + "x": 0.7766442528059767, + "y": 4.143903696796417, + "heading": -0.8031709855452435, + "angularVelocity": 1.1769647157923862, + "velocityX": 0.5770659552705253, + "velocityY": -1.3373500156174554, + "timestamp": 6.692485557687862 + }, + { + "x": 0.8152053931020986, + "y": 4.055414010177326, + "heading": -0.7278520306671347, + "angularVelocity": 1.3274410246890034, + "velocityX": 0.6796116551364678, + "velocityY": -1.559565456931146, + "timestamp": 6.749225520878132 + }, + { + "x": 0.8597512927978146, + "y": 3.9543509740987406, + "heading": -0.6450962807712558, + "angularVelocity": 1.4585090515192565, + "velocityX": 0.7850886252135366, + "velocityY": -1.7811614670894944, + "timestamp": 6.805965484068402 + }, + { + "x": 0.9104943772436287, + "y": 3.840772171273386, + "heading": -0.5562797497368643, + "angularVelocity": 1.5653258486713548, + "velocityX": 0.8943094354089278, + "velocityY": -2.0017426244088754, + "timestamp": 6.862705447258672 + }, + { + "x": 0.9677131149809225, + "y": 3.7147727983516208, + "heading": -0.46323645073147074, + "angularVelocity": 1.6398195165087535, + "velocityX": 1.0084380482486026, + "velocityY": -2.2206460109824513, + "timestamp": 6.919445410448942 + }, + { + "x": 1.0317820620609501, + "y": 3.576517638239885, + "heading": -0.3685846112186535, + "angularVelocity": 1.6681688564973882, + "velocityX": 1.1291679352201949, + "velocityY": -2.436645220373414, + "timestamp": 6.976185373639212 + }, + { + "x": 1.1032183238971194, + "y": 3.4263179326907856, + "heading": -0.2763023679431359, + "angularVelocity": 1.6264064706221368, + "velocityX": 1.2590114236876868, + "velocityY": -2.64715902344566, + "timestamp": 7.032925336829482 + }, + { + "x": 1.1827463587467486, + "y": 3.2648378432524847, + "heading": -0.1927401719055838, + "angularVelocity": 1.4727220699339676, + "velocityX": 1.4016229545821517, + "velocityY": -2.8459674691151595, + "timestamp": 7.089665300019752 + }, + { + "x": 1.2713975131080564, + "y": 3.0937718159398226, + "heading": -0.12815970051626668, + "angularVelocity": 1.1381831738726118, + "velocityX": 1.562411206789599, + "velocityY": -3.014912553591437, + "timestamp": 7.146405263210022 + }, + { + "x": 1.3706976001434645, + "y": 2.9168916321975784, + "heading": -0.09351951840137682, + "angularVelocity": 0.610507659279375, + "velocityX": 1.7500907905494714, + "velocityY": -3.117382772158287, + "timestamp": 7.203145226400292 + }, + { + "x": 1.4825331259415389, + "y": 2.737551336103052, + "heading": -0.084031813825935, + "angularVelocity": 0.1672137950394149, + "velocityX": 1.9710186526390192, + "velocityY": -3.1607404377956887, + "timestamp": 7.2598851895905625 + }, + { + "x": 1.6073609242840539, + "y": 2.563006200971128, + "heading": -0.08403157292142222, + "angularVelocity": 0.000004245764347455466, + "velocityX": 2.199997873173106, + "velocityY": -3.0762292627263235, + "timestamp": 7.316625152780833 + }, + { + "x": 1.7433400400906298, + "y": 2.3970011200993575, + "heading": -0.08403150231700539, + "angularVelocity": 0.0000012443507689652163, + "velocityX": 2.3965316182984986, + "velocityY": -2.925717105509803, + "timestamp": 7.373365115971103 + }, + { + "x": 1.889887864003564, + "y": 2.2402473630588187, + "heading": -0.08403142284388228, + "angularVelocity": 0.0000014006551755862827, + "velocityX": 2.5827973032253166, + "velocityY": -2.7626693467333516, + "timestamp": 7.430105079161373 + }, + { + "x": 2.046376539489404, + "y": 2.0934165181715283, + "heading": -0.08403133019563887, + "angularVelocity": 0.0000016328569530158415, + "velocityX": 2.757997479855154, + "velocityY": -2.58778533914292, + "timestamp": 7.486845042351643 + }, + { + "x": 2.2121356179273546, + "y": 1.9571376609651308, + "heading": -0.08403121735664319, + "angularVelocity": 0.00000198870406896459, + "velocityX": 2.921381494064391, + "velocityY": -2.401814339381988, + "timestamp": 7.543585005541913 + }, + { + "x": 2.3864549305613383, + "y": 1.8319946594051495, + "heading": -0.08403107170612155, + "angularVelocity": 0.0000025669830126630976, + "velocityX": 3.0722493077661266, + "velocityY": -2.2055530973879893, + "timestamp": 7.600324968732183 + }, + { + "x": 2.5685876291674634, + "y": 1.7185236734546225, + "heading": -0.08403086720089675, + "angularVelocity": 0.000003604253744603396, + "velocityX": 3.2099544723948052, + "velocityY": -1.9998424315154402, + "timestamp": 7.657064931922453 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.54889811849075, - "angularVelocity": 9.609981531028035e-7, - "velocityX": 3.344548163283859, - "velocityY": -2.583720422924227, - "timestamp": 7.686887057556453 - }, - { - "x": 3.040083158490555, - "y": 1.4307079955900162, - "heading": -0.5488980816236001, - "angularVelocity": 4.6047885659418846e-7, - "velocityX": 3.5263614649855297, - "velocityY": -2.3294620822502163, - "timestamp": 7.766949691265819 - }, - { - "x": 3.3354250115679163, - "y": 1.2655819134364221, - "heading": -0.5488980521717318, - "angularVelocity": 3.678603468591226e-7, - "velocityX": 3.688885056533252, - "velocityY": -2.062461281917701, - "timestamp": 7.847012324975185 - }, - { - "x": 3.642163402363816, - "y": 1.1227358560558156, - "heading": -0.5488980275036914, - "angularVelocity": 3.08109279752253e-7, - "velocityX": 3.8312303328569812, - "velocityY": -1.7841788455210303, - "timestamp": 7.927074958684551 - }, - { - "x": 3.958620462965578, - "y": 1.0029511838587135, - "heading": -0.548898006032631, - "angularVelocity": 2.6817829186367307e-7, - "velocityX": 3.9526186679093493, - "velocityY": -1.496137044807694, - "timestamp": 8.007137592393917 - }, - { - "x": 4.283065164425698, - "y": 0.9068831020971607, - "heading": -0.5488979867253696, - "angularVelocity": 2.4115196404363913e-7, - "velocityX": 4.052386068609446, - "velocityY": -1.199911585604899, - "timestamp": 8.087200226103283 - }, - { - "x": 4.613722785487234, - "y": 0.8350570656894738, - "heading": -0.5488979688602151, - "angularVelocity": 2.2313973197090786e-7, - "velocityX": 4.129986808350196, - "velocityY": -0.8971230782737135, - "timestamp": 8.167262859812649 - }, - { - "x": 4.948784620508865, - "y": 0.7878658651941135, - "heading": -0.5488979518957929, - "angularVelocity": 2.118893824822783e-7, - "velocityX": 4.184996414655578, - "velocityY": -0.5894285300060232, - "timestamp": 8.247325493522014 - }, - { - "x": 5.286417874607001, - "y": 0.7655672355617388, - "heading": -0.5488979353932385, - "angularVelocity": 2.0612055273986908e-7, - "velocityX": 4.217114007562478, - "velocityY": -0.2785148152093341, - "timestamp": 8.32738812723138 - }, - { - "x": 5.624784390347261, - "y": 0.7668102763452317, - "heading": -0.5488979189757273, - "angularVelocity": 2.0505834761632137e-7, - "velocityX": 4.226272607625093, - "velocityY": 0.015525854397220307, - "timestamp": 8.407450760940746 - }, - { - "x": 5.963150845273002, - "y": 0.7680694375278687, - "heading": -0.5488979025584118, - "angularVelocity": 2.050559016505279e-7, - "velocityX": 4.226271848038887, - "velocityY": 0.01572720160901255, - "timestamp": 8.487513394650112 - }, - { - "x": 6.287499241199154, - "y": 0.7691154701328422, - "heading": -0.5071298401601563, - "angularVelocity": 0.5216923359002789, - "velocityX": 4.051183191195162, - "velocityY": 0.013065178543461153, - "timestamp": 8.567576028359477 - }, - { - "x": 6.587273814834465, - "y": 0.7701306302032382, - "heading": -0.44617628297839135, - "angularVelocity": 0.7613234084075665, - "velocityX": 3.7442507165521994, - "velocityY": 0.012679573760330002, - "timestamp": 8.647638662068843 - }, - { - "x": 6.86203616618243, - "y": 0.7710774941037043, - "heading": -0.38194080023757476, - "angularVelocity": 0.8023153844019356, - "velocityX": 3.431842529005624, - "velocityY": 0.011826539495826505, - "timestamp": 8.727701295778209 - }, - { - "x": 7.11177505601951, - "y": 0.7719477065280066, - "heading": -0.31898594689669185, - "angularVelocity": 0.7863200399997604, - "velocityX": 3.1192939610715853, - "velocityY": 0.010869145608779591, - "timestamp": 8.807763929487574 - }, - { - "x": 7.336503129467627, - "y": 0.7727363298825438, - "heading": -0.2594660169270826, - "angularVelocity": 0.7434170874971441, - "velocityX": 2.80690333350678, - "velocityY": 0.009850080085754406, - "timestamp": 8.88782656319694 - }, - { - "x": 7.536233129593461, - "y": 0.7734405408906285, - "heading": -0.20463677394358387, - "angularVelocity": 0.6848293697469102, - "velocityX": 2.494671869662653, - "velocityY": 0.008795751212569192, - "timestamp": 8.967889196906306 - }, - { - "x": 7.7109755860070575, - "y": 0.7740586905401403, - "heading": -0.15532295555151873, - "angularVelocity": 0.6159404969245411, - "velocityX": 2.1825719229765794, - "velocityY": 0.007720825813338177, - "timestamp": 9.047951830615672 - }, - { - "x": 7.860738998574353, - "y": 0.774589785610018, - "heading": -0.1121095095684322, - "angularVelocity": 0.5397454965062622, - "velocityX": 1.8705781414957077, - "velocityY": 0.006633494863411078, - "timestamp": 9.128014464325037 - }, - { - "x": 7.985530277012728, - "y": 0.7750332018658471, - "heading": -0.07543399366174408, - "angularVelocity": 0.4580853040608456, - "velocityX": 1.558670663912856, - "velocityY": 0.005538367087607409, - "timestamp": 9.208077098034403 - }, - { - "x": 8.085355113462763, - "y": 0.7753885191044545, - "heading": -0.04563658013807198, - "angularVelocity": 0.3721762842804108, - "velocityX": 1.2468342824247154, - "velocityY": 0.004437990881245155, - "timestamp": 9.288139731743769 - }, - { - "x": 8.160218261803882, - "y": 0.7756554219775593, - "heading": -0.022989482111851405, - "angularVelocity": 0.2828672625037418, - "velocityX": 0.9350572779417692, - "velocityY": 0.0033336759051113586, - "timestamp": 9.368202365453135 - }, - { - "x": 8.210123742972717, - "y": 0.7758336375934914, - "heading": -0.007715443356935036, - "angularVelocity": 0.19077612173431838, - "velocityX": 0.623330495846574, - "velocityY": 0.0022259524516894435, - "timestamp": 9.4482649991625 + "heading": -0.08403051576382121, + "angularVelocity": 0.000006193819237400721, + "velocityX": 3.3339066927233554, + "velocityY": -1.7855635206200389, + "timestamp": 7.713804895112723 + }, + { + "x": 2.9656624873675215, + "y": 1.523823440642391, + "heading": -0.08403020563552344, + "angularVelocity": 0.0000051460695851667095, + "velocityX": 3.449910187925088, + "velocityY": -1.5496108793286638, + "timestamp": 7.7740699756075236 + }, + { + "x": 3.179557704009846, + "y": 1.4451070785194118, + "heading": -0.08403001029754076, + "angularVelocity": 0.0000032413128973800796, + "velocityX": 3.5492397070767696, + "velocityY": -1.306168704607808, + "timestamp": 7.834335056102324 + }, + { + "x": 3.3984052007772028, + "y": 1.3814422389869163, + "heading": -0.0840298717822531, + "angularVelocity": 0.0000022984336290020716, + "velocityX": 3.631414659542976, + "velocityY": -1.0564134156924954, + "timestamp": 7.894600136597124 + }, + { + "x": 3.6211472313814954, + "y": 1.3331366287340103, + "heading": -0.08402976608446186, + "angularVelocity": 0.000001753881192375512, + "velocityX": 3.6960380501525916, + "velocityY": -0.8015522398094749, + "timestamp": 7.954865217091925 + }, + { + "x": 3.846707230492903, + "y": 1.3004237167488812, + "heading": -0.08402968105378755, + "angularVelocity": 0.0000014109443416028125, + "velocityX": 3.7427976078264336, + "velocityY": -0.5428170296387713, + "timestamp": 8.015130297586726 + }, + { + "x": 4.073995015295796, + "y": 1.2834616061839175, + "heading": -0.08402960975207145, + "angularVelocity": 0.0000011831348355128416, + "velocityX": 3.7714673727599504, + "velocityY": -0.2814583574052856, + "timestamp": 8.075395378081527 + }, + { + "x": 4.301912054355543, + "y": 1.282332269070522, + "heading": -0.08402954787131725, + "angularVelocity": 0.0000010268094506712622, + "velocityX": 3.78190881333695, + "velocityY": -0.018739493984294066, + "timestamp": 8.135660458576329 + }, + { + "x": 4.529356777936335, + "y": 1.2970411469267544, + "heading": -0.08402949255215464, + "angularVelocity": 9.179306183548953e-7, + "velocityX": 3.7740715139410477, + "velocityY": 0.2440696624889034, + "timestamp": 8.19592553907113 + }, + { + "x": 4.755229905146536, + "y": 1.32751711580047, + "heading": -0.08402944177914254, + "angularVelocity": 8.424947198098048e-7, + "velocityX": 3.7479934541809494, + "velocityY": 0.5056986338273413, + "timestamp": 8.256190619565931 + }, + { + "x": 4.978439767588294, + "y": 1.373612800948183, + "heading": -0.08402939404329389, + "angularVelocity": 7.920979819424461e-7, + "velocityX": 3.7038009508842378, + "velocityY": 0.7648821634228096, + "timestamp": 8.316455700060732 + }, + { + "x": 5.197907642868858, + "y": 1.435105133457297, + "heading": -0.08402934813807361, + "angularVelocity": 7.617217119968608e-7, + "velocityX": 3.6417088217363065, + "velocityY": 1.0203642308985088, + "timestamp": 8.376720780555534 + }, + { + "x": 5.41257465992254, + "y": 1.51169186312203, + "heading": -0.08402930302434115, + "angularVelocity": 7.485882719571497e-7, + "velocityX": 3.562046466895548, + "velocityY": 1.2708309527826855, + "timestamp": 8.436985861050335 + }, + { + "x": 5.626738902818376, + "y": 1.589673479175761, + "heading": -0.0840292579538456, + "angularVelocity": 7.478708262765028e-7, + "velocityX": 3.5537037557647206, + "velocityY": 1.2939768007189265, + "timestamp": 8.497250941545136 + }, + { + "x": 5.8409031120271635, + "y": 1.6676551877456105, + "heading": -0.0840292128833514, + "angularVelocity": 7.478708040156826e-7, + "velocityX": 3.5537031967834927, + "velocityY": 1.2939783358719235, + "timestamp": 8.557516022039938 + }, + { + "x": 6.055067321252931, + "y": 1.745636896268829, + "heading": -0.08402916781285703, + "angularVelocity": 7.478708068018038e-7, + "velocityX": 3.5537031970652393, + "velocityY": 1.2939783350981522, + "timestamp": 8.617781102534739 + }, + { + "x": 6.269231822230628, + "y": 1.8236178035375321, + "heading": -0.08402912274221092, + "angularVelocity": 7.478733249018271e-7, + "velocityX": 3.5537080382091872, + "velocityY": 1.2939650395958724, + "timestamp": 8.67804618302954 + }, + { + "x": 6.484552473362613, + "y": 1.8927933712391167, + "heading": -0.0785738032137305, + "angularVelocity": 0.09052206491205224, + "velocityX": 3.572892450555367, + "velocityY": 1.1478548959633867, + "timestamp": 8.738311263524341 + }, + { + "x": 6.6874523544155435, + "y": 1.9584473392764923, + "heading": -0.032553192185028265, + "angularVelocity": 0.7636364317587296, + "velocityX": 3.3667901774467204, + "velocityY": 1.0894197352485022, + "timestamp": 8.798576344019143 + }, + { + "x": 6.875915672715089, + "y": 2.018652626020107, + "heading": 0.028144572860875648, + "angularVelocity": 1.0071796892586995, + "velocityX": 3.1272391366971615, + "velocityY": 0.9990078209355214, + "timestamp": 8.858841424513944 + }, + { + "x": 7.049532373282227, + "y": 2.073323850727038, + "heading": 0.08948102802542592, + "angularVelocity": 1.0177777024597514, + "velocityX": 2.8808839072589665, + "velocityY": 0.9071791534676087, + "timestamp": 8.919106505008745 + }, + { + "x": 7.208331847820926, + "y": 2.1225088918275805, + "heading": 0.1460274817443076, + "angularVelocity": 0.9382954980664153, + "velocityX": 2.635016384859868, + "velocityY": 0.8161449498899447, + "timestamp": 8.979371585503547 + }, + { + "x": 7.352376622997373, + "y": 2.1662517609495984, + "heading": 0.19476972653717045, + "angularVelocity": 0.8087974726436816, + "velocityX": 2.3901863897597297, + "velocityY": 0.72584104696901, + "timestamp": 9.039636665998348 }, { - "x": 8.235074996948242, - "y": 0.7759228944778442, - "heading": -1.0986669650236063e-32, - "angularVelocity": 0.09636759371342492, - "velocityX": 0.3116466798494725, - "velocityY": 0.0011148382228791801, - "timestamp": 9.528327632871866 + "x": 7.481724262237549, + "y": 2.2045881748199463, + "heading": 0.2337432969576531, + "angularVelocity": 0.6467023706015796, + "velocityX": 2.146311565141514, + "velocityY": 0.6361298044504436, + "timestamp": 9.099901746493149 + }, + { + "x": 7.603777233505286, + "y": 2.239565945400217, + "heading": 0.2627306680277325, + "angularVelocity": 0.44772760939813167, + "velocityX": 1.8851825132237892, + "velocityY": 0.540252980034664, + "timestamp": 9.164645064666104 + }, + { + "x": 7.70877923669531, + "y": 2.2685458571337813, + "heading": 0.28047750188422, + "angularVelocity": 0.2741106628035178, + "velocityX": 1.621819921393637, + "velocityY": 0.4476123954003698, + "timestamp": 9.229388382839058 + }, + { + "x": 7.796631047750002, + "y": 2.291686366419086, + "heading": 0.28817114093339696, + "angularVelocity": 0.11883294317143692, + "velocityX": 1.3569247535321998, + "velocityY": 0.3574192663942211, + "timestamp": 9.294131701012013 + }, + { + "x": 7.867260828377704, + "y": 2.309110963970804, + "heading": 0.2867085865097153, + "angularVelocity": -0.022590044269504023, + "velocityX": 1.0909199994819943, + "velocityY": 0.2691335267242617, + "timestamp": 9.358875019184968 + }, + { + "x": 7.920614376230513, + "y": 2.320918360587503, + "heading": 0.2767910958513383, + "angularVelocity": -0.15318168636157245, + "velocityX": 0.824078057140675, + "velocityY": 0.1823724354867997, + "timestamp": 9.423618337357922 + }, + { + "x": 7.956649448780364, + "y": 2.327189146390105, + "heading": 0.25898192892634386, + "angularVelocity": -0.27507343502876075, + "velocityX": 0.5565836532132435, + "velocityY": 0.09685610777394495, + "timestamp": 9.488361655530877 }, { - "x": 8.235074996948242, - "y": 0.7759228944778442, - "heading": 1.3508572737206773e-32, - "angularVelocity": 3.0595153521375356e-31, - "velocityX": -1.0447785999071724e-32, - "velocityY": -1.7698938774257706e-30, - "timestamp": 9.608390266581232 + "x": 7.975332260131836, + "y": 2.3279902935028076, + "heading": 0.2337432969576531, + "angularVelocity": -0.38982604971324913, + "velocityX": 0.2885674055438917, + "velocityY": 0.012374205328225838, + "timestamp": 9.553104973703832 + }, + { + "x": 7.972208451820267, + "y": 2.320885188130861, + "heading": 0.19238106068409147, + "angularVelocity": -0.5227816803364386, + "velocityX": -0.039482143745094074, + "velocityY": -0.08980217850764118, + "timestamp": 9.632224494967359 + }, + { + "x": 7.9431197679460075, + "y": 2.3056970040477323, + "heading": 0.14088004342725263, + "angularVelocity": -0.6509268058549238, + "velocityX": -0.3676549530345614, + "velocityY": -0.19196506551829257, + "timestamp": 9.711344016230886 + }, + { + "x": 7.888054782175393, + "y": 2.282427289475609, + "heading": 0.07969868788669623, + "angularVelocity": -0.7732776255909873, + "velocityX": -0.695972180964121, + "velocityY": -0.2941083843849025, + "timestamp": 9.790463537494412 + }, + { + "x": 7.806999942498959, + "y": 2.251078456184475, + "heading": 0.009399898258955654, + "angularVelocity": -0.8885138396324784, + "velocityX": -1.0244606941750847, + "velocityY": -0.39622122063553056, + "timestamp": 9.86958305875794 + }, + { + "x": 7.699938927127902, + "y": 2.2116544317191673, + "heading": -0.06930512925958275, + "angularVelocity": -0.9947611697041394, + "velocityX": -1.353155500201559, + "velocityY": -0.49828441623143704, + "timestamp": 9.948702580021466 + }, + { + "x": 7.56685172715319, + "y": 2.1641617607796966, + "heading": -0.15547855671843705, + "angularVelocity": -1.089155066697531, + "velocityX": -1.682103200946216, + "velocityY": -0.6002648926714905, + "timestamp": 10.027822101284993 + }, + { + "x": 7.407713494064462, + "y": 2.1086115226344657, + "heading": -0.24780538645569025, + "angularVelocity": -1.1669285691167814, + "velocityX": -2.011364964642267, + "velocityY": -0.702105337066011, + "timestamp": 10.10694162254852 + }, + { + "x": 7.222493943520295, + "y": 2.045023086580113, + "heading": -0.3442810817864959, + "angularVelocity": -1.2193665202986854, + "velocityX": -2.3410094953335943, + "velocityY": -0.8037009708710834, + "timestamp": 10.186061143812047 + }, + { + "x": 7.0111623875747595, + "y": 1.9734335261753837, + "heading": -0.4414827777687788, + "angularVelocity": -1.2285425193427022, + "velocityX": -2.6710418942202843, + "velocityY": -0.9048280280448349, + "timestamp": 10.265180665075574 + }, + { + "x": 6.773734962048751, + "y": 1.893931948844204, + "heading": -0.5324651154460041, + "angularVelocity": -1.1499353917244444, + "velocityX": -3.0008703507595227, + "velocityY": -1.0048288470601288, + "timestamp": 10.344300186339101 + }, + { + "x": 6.510837595414292, + "y": 1.8068437015263887, + "heading": -0.5976366951472135, + "angularVelocity": -0.8237104909184032, + "velocityX": -3.3227876311184024, + "velocityY": -1.1007175716817783, + "timestamp": 10.423419707602628 + }, + { + "x": 6.227373100797986, + "y": 1.7110085398670838, + "heading": -0.597636758184297, + "angularVelocity": -7.967323658608173e-7, + "velocityX": -3.5827377376583796, + "velocityY": -1.2112707474568936, + "timestamp": 10.502539228866155 + }, + { + "x": 5.941831354312318, + "y": 1.6215523108704284, + "heading": -0.5976367779578059, + "angularVelocity": -2.499194721356179e-7, + "velocityX": -3.6089923438060088, + "velocityY": -1.1306467426502582, + "timestamp": 10.581658750129682 + }, + { + "x": 5.656289545112644, + "y": 1.532096282055343, + "heading": -0.5976367977313131, + "angularVelocity": -2.499194483006569e-7, + "velocityX": -3.6089931364549517, + "velocityY": -1.130644212534215, + "timestamp": 10.660778271393209 + }, + { + "x": 5.370747722448539, + "y": 1.442640296218458, + "heading": -0.5976368175048198, + "angularVelocity": -2.499194443736624e-7, + "velocityX": -3.6089933066333257, + "velocityY": -1.130643669328191, + "timestamp": 10.739897792656736 + }, + { + "x": 5.084751687474933, + "y": 1.3546472631644115, + "heading": -0.5976368372766676, + "angularVelocity": -2.4989847693362694e-7, + "velocityX": -3.6147341440682292, + "velocityY": -1.112153254327249, + "timestamp": 10.819017313920263 + }, + { + "x": 4.791925527807106, + "y": 1.2930896001142946, + "heading": -0.5976368572220272, + "angularVelocity": -2.52091508667939e-7, + "velocityX": -3.7010608126974813, + "velocityY": -0.7780338160172059, + "timestamp": 10.89813683518379 + }, + { + "x": 4.494706489260304, + "y": 1.2584868813594625, + "heading": -0.5976368780658494, + "angularVelocity": -2.63447274854176e-7, + "velocityX": -3.756582873610133, + "velocityY": -0.4373474232683845, + "timestamp": 10.977256356447317 + }, + { + "x": 4.195570491935266, + "y": 1.2511276025347224, + "heading": -0.597636900694303, + "angularVelocity": -2.860034177860552e-7, + "velocityX": -3.780811518420208, + "velocityY": -0.09301470366874595, + "timestamp": 11.056375877710844 + }, + { + "x": 3.897009479956395, + "y": 1.2710731343763437, + "heading": -0.597636926337917, + "angularVelocity": -3.24112350243062e-7, + "velocityX": -3.7735442177972423, + "velocityY": 0.2520936871595517, + "timestamp": 11.13549539897437 + }, + { + "x": 3.6015106196507847, + "y": 1.3181573465478027, + "heading": -0.5976369569136154, + "angularVelocity": -3.8644948741127607e-7, + "velocityX": -3.7348413588269285, + "velocityY": 0.5951023390881358, + "timestamp": 11.214614920237898 + }, + { + "x": 3.311535572783971, + "y": 1.3919880157013045, + "heading": -0.5976369957684174, + "angularVelocity": -4.910899536256082e-7, + "velocityX": -3.6650252963611574, + "velocityY": 0.933153638627185, + "timestamp": 11.293734441501424 + }, + { + "x": 3.029499988052695, + "y": 1.4919500996437052, + "heading": -0.5976370495734, + "angularVelocity": -6.80046867328716e-7, + "velocityX": -3.5646775944445412, + "velocityY": 1.263431354816361, + "timestamp": 11.372853962764951 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.5976371219829064, + "angularVelocity": -9.151914125801383e-7, + "velocityX": -3.434634228323893, + "velocityY": 1.583184066038948, + "timestamp": 11.451973484028478 + }, + { + "x": 2.562615182382705, + "y": 1.7237517111402614, + "heading": -0.5976372044989885, + "angularVelocity": -0.000001403654433041047, + "velocityX": -3.319432744203681, + "velocityY": 1.8123319353797018, + "timestamp": 11.510760091514479 + }, + { + "x": 2.3751466995867516, + "y": 1.8432734201994623, + "heading": -0.5976372636960788, + "angularVelocity": -0.000001006982590926024, + "velocityX": -3.188965834447897, + "velocityY": 2.033145203823246, + "timestamp": 11.56954669900048 + }, + { + "x": 2.1962100914661193, + "y": 1.9752263102479175, + "heading": -0.5976373093137471, + "angularVelocity": -7.759874278787006e-7, + "velocityX": -3.04383286896162, + "velocityY": 2.2446080100791628, + "timestamp": 11.62833330648648 + }, + { + "x": 2.026628285101079, + "y": 2.1190035289778706, + "heading": -0.5976373463774368, + "angularVelocity": -6.304784584021761e-7, + "velocityX": -2.884701356604495, + "velocityY": 2.4457478476571914, + "timestamp": 11.68711991397248 + }, + { + "x": 1.8671811829950287, + "y": 2.2739438442752706, + "heading": -0.5976373777665476, + "angularVelocity": -5.339500279584693e-7, + "velocityX": -2.7123031745627153, + "velocityY": 2.6356396792296386, + "timestamp": 11.745906521458481 + }, + { + "x": 1.7186020763048488, + "y": 2.439334685025019, + "heading": -0.5976374052703787, + "angularVelocity": -4.678587919148875e-7, + "velocityX": -2.5274312134028762, + "velocityY": 2.813410193625064, + "timestamp": 11.804693128944482 + }, + { + "x": 1.5815742720495027, + "y": 2.6144154181152923, + "heading": -0.597637430079118, + "angularVelocity": -4.220134537614776e-7, + "velocityX": -2.330935737157126, + "velocityY": 2.978241823734581, + "timestamp": 11.863479736430483 + }, + { + "x": 1.456727954325337, + "y": 2.7983808405830617, + "heading": -0.5976374777093088, + "angularVelocity": -8.102217978593512e-7, + "velocityX": -2.123720402710731, + "velocityY": 3.129376406209191, + "timestamp": 11.922266343916483 + }, + { + "x": 1.3455561973341574, + "y": 2.9885105140912174, + "heading": -0.6039795556256176, + "angularVelocity": -0.10788303982023785, + "velocityX": -1.8911068650739042, + "velocityY": 3.234234490456602, + "timestamp": 11.981052951402484 + }, + { + "x": 1.246887053690835, + "y": 3.1733277865183314, + "heading": -0.6383468335999772, + "angularVelocity": -0.584610669743856, + "velocityX": -1.6784289460285728, + "velocityY": 3.1438669508378774, + "timestamp": 12.039839558888485 + }, + { + "x": 1.158565022655154, + "y": 3.3474027501759167, + "heading": -0.6816254514815488, + "angularVelocity": -0.736198595775039, + "velocityX": -1.5024175541464038, + "velocityY": 2.9611330046395477, + "timestamp": 12.098626166374485 + }, + { + "x": 1.0790333486785466, + "y": 3.5092497722088574, + "heading": -0.7278331515121171, + "angularVelocity": -0.7860242665231638, + "velocityX": -1.3528876282841755, + "velocityY": 2.7531274375968073, + "timestamp": 12.157412773860486 + }, + { + "x": 1.0074427197106477, + "y": 3.6582801720807683, + "heading": -0.7741804064083878, + "angularVelocity": -0.7883981892867038, + "velocityX": -1.217805075500369, + "velocityY": 2.5351080160120225, + "timestamp": 12.216199381346486 + }, + { + "x": 0.9432732996874974, + "y": 3.7941912605500616, + "heading": -0.8190469180938782, + "angularVelocity": -0.7632097446033984, + "velocityX": -1.0915652861654275, + "velocityY": 2.3119396454653414, + "timestamp": 12.274985988832487 + }, + { + "x": 0.8861770840921692, + "y": 3.9168021213870996, + "heading": -0.8613760150853045, + "angularVelocity": -0.7200466024767106, + "velocityX": -0.971245289310589, + "velocityY": 2.085693767347223, + "timestamp": 12.333772596318488 + }, + { + "x": 0.8359056858624994, + "y": 4.025993630774074, + "heading": -0.9004245404660403, + "angularVelocity": -0.6642418579781981, + "velocityX": -0.8551505245755496, + "velocityY": 1.857421512424882, + "timestamp": 12.392559203804488 + }, + { + "x": 0.7922732558571624, + "y": 4.121681912181887, + "heading": -0.9356409495537121, + "angularVelocity": -0.5990549649604897, + "velocityX": -0.7422171795800264, + "velocityY": 1.627722460946589, + "timestamp": 12.451345811290489 + }, + { + "x": 0.7551356458246514, + "y": 4.203804939460416, + "heading": -0.9665991512691937, + "angularVelocity": -0.5266199741642557, + "velocityX": -0.6317358939509348, + "velocityY": 1.3969683026544202, + "timestamp": 12.51013241877649 + }, + { + "x": 0.7243778479811152, + "y": 4.272315098350931, + "heading": -0.9929596716344966, + "angularVelocity": -0.44841030113160957, + "velocityX": -0.5232109686013307, + "velocityY": 1.1654041935798078, + "timestamp": 12.56891902626249 + }, + { + "x": 0.6999059919554825, + "y": 4.327174746467568, + "heading": -1.0144454156380707, + "angularVelocity": -0.3654870543208486, + "velocityX": -0.41628284182686576, + "velocityY": 0.9331997620325617, + "timestamp": 12.62770563374849 + }, + { + "x": 0.6816420121384978, + "y": 4.368353408748977, + "heading": -1.0308257653496888, + "angularVelocity": -0.27864084035669345, + "velocityX": -0.31068266392705546, + "velocityY": 0.7004769290559099, + "timestamp": 12.686492241234491 + }, + { + "x": 0.6695199611090151, + "y": 4.395825923599214, + "heading": -1.0419057024338247, + "angularVelocity": -0.1884772324508513, + "velocityX": -0.2062042963164626, + "velocityY": 0.46732608029439393, + "timestamp": 12.745278848720492 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -0.09547069579077468, + "velocityX": -0.1026863106378516, + "velocityY": 0.23381596243572259, + "timestamp": 12.804065456206493 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -7.821518056781921e-37, + "velocityX": -6.302822662985753e-37, + "velocityY": 5.041148502478119e-38, + "timestamp": 12.862852063692493 } ], "constraints": [ @@ -14995,102 +9157,93 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "CenterLanePSubCSubBSubASubSprint": { + "AmpLanePSubASubDSubESub": { "waypoints": [ { - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 19 }, { - "x": 2.614555597305298, - "y": 4.302172660827637, - "heading": -0.7216551150961179, + "x": 2.6514174938201904, + "y": 6.946751594543457, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 19 }, { - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 32 }, { - "x": 2.525056838989258, - "y": 5.573054313659668, + "x": 7.323064804077148, + "y": 7.475617408752441, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 7 }, { - "x": 1.2899744510650635, - "y": 5.590954303741455, + "x": 7.799044132232666, + "y": 7.457988739013672, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 - }, - { - "x": 2.68615460395813, - "y": 6.772337436676025, - "heading": 0.8050032485420815, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 33 }, { - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 28 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, + "x": 5.7460856437683105, + "y": 6.598193168640137, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 19 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, + "x": 7.981563568115234, + "y": 5.947082996368408, + "heading": -0.6889234822214203, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, + "x": 5.7460856437683105, + "y": 6.598193168640137, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "headingConstrained": false, + "controlIntervalCount": 28 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -15099,1372 +9252,1849 @@ ], "trajectory": [ { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -1.748186257820146e-28, - "velocityX": 0, - "velocityY": 0, + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0, + "velocityX": 2.1425070675381243e-37, + "velocityY": 2.00620509434581e-36, "timestamp": 0 }, { - "x": 1.3063204215499635, - "y": 5.5750430661668995, - "heading": -0.008999106947679492, - "angularVelocity": -0.11745777195159221, - "velocityX": 0.21335020071190533, - "velocityY": -0.20767599777855084, - "timestamp": 0.07661567898070912 - }, - { - "x": 1.3390123980907098, - "y": 5.543220800833044, - "heading": -0.026994200234242757, - "angularVelocity": -0.23487481322228163, - "velocityX": 0.42670086570142546, - "velocityY": -0.41534925701509284, - "timestamp": 0.15323135796141824 - }, - { - "x": 1.3880505863886679, - "y": 5.4954877356301095, - "heading": -0.053973713874731634, - "angularVelocity": -0.3521408934222409, - "velocityX": 0.6400542155377871, - "velocityY": -0.6230195417923181, - "timestamp": 0.22984703694212735 - }, - { - "x": 1.4534353620178047, - "y": 5.431844149183211, - "heading": -0.08991902767117105, - "angularVelocity": -0.4691639398438398, - "velocityX": 0.853412467425134, - "velocityY": -0.8306861899103606, - "timestamp": 0.30646271592283647 - }, - { - "x": 1.5351672628403068, - "y": 5.352290408430599, - "heading": -0.13480641974722582, - "angularVelocity": -0.5858773645717358, - "velocityX": 1.066777739575042, - "velocityY": -1.0383480486921168, - "timestamp": 0.38307839490354556 - }, - { - "x": 1.6332469708393256, - "y": 5.256827004871127, - "heading": -0.1886095129083549, - "angularVelocity": -0.7022465099717149, - "velocityX": 1.2801519130772017, - "velocityY": -1.2460034923528938, - "timestamp": 0.45969407388425465 - }, - { - "x": 1.7476752821470238, - "y": 5.145454581666917, - "heading": -0.25130200812811576, - "angularVelocity": -0.818272396306973, - "velocityX": 1.493536477822563, - "velocityY": -1.4536505413596468, - "timestamp": 0.5363097528649637 - }, - { - "x": 1.8784530688655205, - "y": 5.018173944363308, - "heading": -0.3228603099280309, - "angularVelocity": -0.9339903107164581, - "velocityX": 1.7069324253829399, - "velocityY": -1.66128707626095, - "timestamp": 0.6129254318456728 - }, - { - "x": 2.0255812507970647, - "y": 4.874986059010151, - "heading": -0.4032653297049151, - "angularVelocity": -1.049459077659022, - "velocityX": 1.9203403783475084, - "velocityY": -1.8689110021266238, - "timestamp": 0.6895411108263819 - }, - { - "x": 2.156432157972857, - "y": 4.747720771234375, - "heading": -0.47395861648198423, - "angularVelocity": -0.9226999968520704, - "velocityX": 1.7078868044203455, - "velocityY": -1.6610867310239321, - "timestamp": 0.766156789807091 - }, - { - "x": 2.270931566035856, - "y": 4.636361300878039, - "heading": -0.5358188421760707, - "angularVelocity": -0.8074094819002076, - "velocityX": 1.494464443178203, - "velocityY": -1.4534814779709395, - "timestamp": 0.8427724687878001 - }, - { - "x": 2.369078961091203, - "y": 4.540907071428243, - "heading": -0.5888499820546531, - "angularVelocity": -0.6921708527593945, - "velocityX": 1.2810353744995546, - "velocityY": -1.2458837496554307, - "timestamp": 0.9193881477685092 - }, - { - "x": 2.4508739613497577, - "y": 4.461357622382182, - "heading": -0.6330536039339079, - "angularVelocity": -0.5769526870004522, - "velocityX": 1.0676013226487002, - "velocityY": -1.0382920318954914, - "timestamp": 0.9960038267492183 - }, - { - "x": 2.516316276718558, - "y": 4.3977125949651334, - "heading": -0.6684300724749328, - "angularVelocity": -0.4617392808541994, - "velocityX": 0.8541634844585878, - "velocityY": -0.8307049969200896, - "timestamp": 1.0726195057299275 - }, - { - "x": 2.5654056965461667, - "y": 4.349971723161198, - "heading": -0.6949791200920894, - "angularVelocity": -0.34652238213663356, - "velocityX": 0.6407228968125727, - "velocityY": -0.6231214340263868, - "timestamp": 1.1492351847106366 - }, - { - "x": 2.5981420853275283, - "y": 4.3181348275927744, - "heading": -0.7127001553640687, - "angularVelocity": -0.2312977640935645, - "velocityX": 0.4272805405331163, - "velocityY": -0.41554021239755634, - "timestamp": 1.2258508636913457 - }, - { - "x": 2.6145253815163745, - "y": 4.302201811854818, - "heading": -0.7215924300890252, - "angularVelocity": -0.11606338040076232, - "velocityX": 0.21383738094203186, - "velocityY": -0.20796024906265667, - "timestamp": 1.3024665426720548 - }, - { - "x": 2.614555597305298, - "y": 4.302172660827637, - "heading": -0.7216551150961179, - "angularVelocity": -0.0008181742381802744, - "velocityX": 0.0003943804906386705, - "velocityY": -0.0003804830723595261, - "timestamp": 1.3790822216527638 - }, - { - "x": 2.5982259689280283, - "y": 4.318054102166664, - "heading": -0.712883613809901, - "angularVelocity": 0.11446303533186522, - "velocityX": -0.213092236881309, - "velocityY": 0.20724365439564615, - "timestamp": 1.4557139589685733 - }, - { - "x": 2.565536606931669, - "y": 4.349846333909782, - "heading": -0.695279424271666, - "angularVelocity": 0.2297245257202117, - "velocityX": -0.426577330596217, - "velocityY": 0.4148702979331944, - "timestamp": 1.5323456962843827 - }, - { - "x": 2.5164876982610407, - "y": 4.39754963766033, - "heading": -0.6688439584276578, - "angularVelocity": 0.34496759102452434, - "velocityX": -0.640059984788952, - "velocityY": 0.6225006162319799, - "timestamp": 1.608977433600192 - }, - { - "x": 2.4510795013292355, - "y": 4.461164391501365, - "heading": -0.6335783197571896, - "angularVelocity": 0.4601962565606419, - "velocityX": -0.8535392668397416, - "velocityY": 0.8301358682967486, - "timestamp": 1.6856091709160015 - }, - { - "x": 2.369312351807588, - "y": 4.540691078953442, - "heading": -0.5894828757685662, - "angularVelocity": 0.5754201263263005, - "velocityX": -1.0670141685730552, - "velocityY": 1.0377774300260596, - "timestamp": 1.762240908231811 - }, - { - "x": 2.2711866760947044, - "y": 4.636130298345392, - "heading": -0.5365564475452359, - "angularVelocity": 0.6906593802750026, - "velocityX": -1.28048350605026, - "velocityY": 1.2454267996017627, - "timestamp": 1.8388726455476203 - }, - { - "x": 2.1567030158826075, - "y": 4.747482768262345, - "heading": -0.47479485869558546, - "angularVelocity": 0.8059531337295586, - "velocityX": -1.4939457747586862, - "velocityY": 1.4530855462992425, - "timestamp": 1.9155043828634297 - }, - { - "x": 2.0258620666320337, - "y": 4.874749320539599, - "heading": -0.4041884988158368, - "angularVelocity": 0.9213722974191146, - "velocityX": -1.7073989681051045, - "velocityY": 1.6607551479679548, - "timestamp": 1.9921361201792391 - }, - { - "x": 1.8786647355617998, - "y": 5.01793086600198, - "heading": -0.3247184712590107, - "angularVelocity": 1.0370380514754407, - "velocityX": -1.9208403227681095, - "velocityY": 1.8684366353759239, - "timestamp": 2.0687678574950485 - }, - { - "x": 1.7478414567124814, - "y": 5.145267849282916, - "heading": -0.25271823102965235, - "angularVelocity": 0.9395616327984198, - "velocityX": -1.7071683803424498, - "velocityY": 1.6616742327261775, - "timestamp": 2.145399594810858 - }, - { - "x": 1.6333728466995314, - "y": 5.256688511191179, - "heading": -0.18963629669305965, - "angularVelocity": 0.8231828815865951, - "velocityX": -1.4937493791208316, - "velocityY": 1.4539754132374219, - "timestamp": 2.222031332126668 - }, - { - "x": 1.535258100629332, - "y": 5.352192443129829, - "heading": -0.13550580636159482, - "angularVelocity": 0.70637169709355, - "velocityX": -1.2803408812977723, - "velocityY": 1.2462712617938094, - "timestamp": 2.2986630694424774 - }, - { - "x": 1.4534965485667528, - "y": 5.431779376588513, - "heading": -0.09035761814526168, - "angularVelocity": 0.5891578320466108, - "velocityX": -1.0669411257422734, - "velocityY": 1.0385636064447687, - "timestamp": 2.375294806758287 - }, - { - "x": 1.3880876748821938, - "y": 5.495449138780266, - "heading": -0.05421785896865615, - "angularVelocity": 0.4716030256909087, - "velocityX": -0.8535480988559686, - "velocityY": 0.8308536963253964, - "timestamp": 2.4519265440740967 - }, - { - "x": 1.339031128559986, - "y": 5.543201612325557, - "heading": -0.02710586002264422, - "angularVelocity": 0.35379595843554007, - "velocityX": -0.6401596525218931, - "velocityY": 0.6231422544707841, - "timestamp": 2.5285582813899063 - }, - { - "x": 1.3063267261975056, - "y": 5.575036700785004, - "heading": -0.009032648967396358, - "angularVelocity": 0.23584498657035882, - "velocityX": -0.4267735994864031, - "velocityY": 0.4154295541377912, - "timestamp": 2.605190018705716 + "x": 0.6967295535333877, + "y": 6.66750179345797, + "heading": 0.982893163249758, + "angularVelocity": -0.14737604826251527, + "velocityX": 0.29094834799603586, + "velocityY": 0.04143420364739335, + "timestamp": 0.06786122358481142 + }, + { + "x": 0.7362175895643961, + "y": 6.673125696218821, + "heading": 0.9628974920292898, + "angularVelocity": -0.2946553298656345, + "velocityX": 0.5818939586560375, + "velocityY": 0.08287358323008266, + "timestamp": 0.13572244716962284 + }, + { + "x": 0.7954495269492323, + "y": 6.681562379154691, + "heading": 0.9329256679312776, + "angularVelocity": -0.44166347900512215, + "velocityX": 0.8728392188627362, + "velocityY": 0.1243225879257299, + "timestamp": 0.20358367075443426 + }, + { + "x": 0.8744254360510917, + "y": 6.692812849791712, + "heading": 0.8930052032103389, + "angularVelocity": -0.5882662087730711, + "velocityX": 1.1637855159383756, + "velocityY": 0.1657864394820339, + "timestamp": 0.2714448943392457 + }, + { + "x": 0.9731453894549041, + "y": 6.7068785148813665, + "heading": 0.843168820166726, + "angularVelocity": -0.7343867441666224, + "velocityX": 1.4547328826223493, + "velocityY": 0.20727102086621554, + "timestamp": 0.33930611792405707 + }, + { + "x": 1.0916093532691815, + "y": 6.7237612036761645, + "heading": 0.7834495219946941, + "angularVelocity": -0.8800209459441316, + "velocityX": 1.7456797498828438, + "velocityY": 0.2487825580347626, + "timestamp": 0.40716734150886846 + }, + { + "x": 1.2298170767085217, + "y": 6.743463170039788, + "heading": 0.7138752049500412, + "angularVelocity": -1.025244069725628, + "velocityX": 2.0366229215807086, + "velocityY": 0.29032730803917456, + "timestamp": 0.47502856509367986 + }, + { + "x": 1.3877679992818084, + "y": 6.765987144948878, + "heading": 0.6344639927011461, + "angularVelocity": -1.1702001239286361, + "velocityX": 2.3275578339061793, + "velocityY": 0.3319123015950357, + "timestamp": 0.5428897886784912 + }, + { + "x": 1.5654611541888466, + "y": 6.791337011948423, + "heading": 0.5452227484246948, + "angularVelocity": -1.3150550426624645, + "velocityX": 2.618478499506586, + "velocityY": 0.3735545228986894, + "timestamp": 0.6107510122633026 + }, + { + "x": 1.7628875214635322, + "y": 6.819592757066269, + "heading": 0.4462737595759067, + "angularVelocity": -1.4581079388455742, + "velocityX": 2.909266247284013, + "velocityY": 0.41637541478357637, + "timestamp": 0.678612235848114 + }, + { + "x": 1.9405727781814897, + "y": 6.845027020375549, + "heading": 0.357188812683824, + "angularVelocity": -1.3127518512946696, + "velocityX": 2.6183621121403804, + "velocityY": 0.3747981831994576, + "timestamp": 0.7464734594329254 + }, + { + "x": 2.098517679535123, + "y": 6.867636881191052, + "heading": 0.2779750280562074, + "angularVelocity": -1.1672908392024057, + "velocityX": 2.3274691054787353, + "velocityY": 0.3331779125268215, + "timestamp": 0.8143346830177368 + }, + { + "x": 2.236722632634811, + "y": 6.887421048029417, + "heading": 0.20863780165516146, + "angularVelocity": -1.0217503124503762, + "velocityX": 2.036582097977683, + "velocityY": 0.29153861061228065, + "timestamp": 0.8821959066025482 + }, + { + "x": 2.3551878284960197, + "y": 6.9043786966768, + "heading": 0.1491789973537457, + "angularVelocity": -0.8761823197470863, + "velocityX": 1.745697905272119, + "velocityY": 0.24988716311885253, + "timestamp": 0.9500571301873596 + }, + { + "x": 2.4539133200632923, + "y": 6.918509209574482, + "heading": 0.09959774943286472, + "angularVelocity": -0.7306270842422328, + "velocityX": 1.454814492755025, + "velocityY": 0.20822661530737346, + "timestamp": 1.017918353772171 + }, + { + "x": 2.5328990844805293, + "y": 6.929812092655753, + "heading": 0.059891727352470836, + "angularVelocity": -0.5851061914728107, + "velocityX": 1.163930743431445, + "velocityY": 0.1665587869506308, + "timestamp": 1.0857795773569825 + }, + { + "x": 2.592145068506356, + "y": 6.938286954912776, + "heading": 0.030058218690118557, + "angularVelocity": -0.43962526884100517, + "velocityX": 0.8730462095453193, + "velocityY": 0.12488519671960763, + "timestamp": 1.153640800941794 + }, + { + "x": 2.63165121786692, + "y": 6.943933511859112, + "heading": 0.010094853186468074, + "angularVelocity": -0.2941792742463702, + "velocityX": 0.582160875882081, + "velocityY": 0.08320741430898287, + "timestamp": 1.2215020245266055 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -2.143037766767562e-26, - "angularVelocity": 0.11787086245509872, - "velocityX": -0.21338776476374371, - "velocityY": 0.20771554341884355, - "timestamp": 2.6818217560215256 + "x": 2.6514174938201904, + "y": 6.946751594543457, + "heading": 0, + "angularVelocity": -0.14875731166049128, + "velocityX": 0.2912749713180046, + "velocityY": 0.041527142239385896, + "timestamp": 1.289363248111417 + }, + { + "x": 2.649146332814054, + "y": 6.946410455915093, + "heading": 0.0008990727847708372, + "angularVelocity": 0.011985225672082133, + "velocityX": -0.030276055128411465, + "velocityY": -0.004547600056030398, + "timestamp": 1.3643783383040946 + }, + { + "x": 2.62275408560099, + "y": 6.9426132004627625, + "heading": 0.013859521326443051, + "angularVelocity": 0.17277121854260333, + "velocityX": -0.35182584124441, + "velocityY": -0.050619887846258024, + "timestamp": 1.4393934284967722 + }, + { + "x": 2.572240863242063, + "y": 6.93536015641962, + "heading": 0.03888546297295733, + "angularVelocity": 0.3336120983422759, + "velocityX": -0.6733741468440916, + "velocityY": -0.09668780007480728, + "timestamp": 1.5144085186894498 + }, + { + "x": 2.4976067609609713, + "y": 6.924651766873113, + "heading": 0.07598096942338527, + "angularVelocity": 0.4945072565419495, + "velocityX": -0.994921183049867, + "velocityY": -0.14274980565911422, + "timestamp": 1.5894236088821274 + }, + { + "x": 2.398851800518621, + "y": 6.910488564953806, + "heading": 0.12514851990507653, + "angularVelocity": 0.6554354644565975, + "velocityX": -1.3164679291686043, + "velocityY": -0.18880470426588675, + "timestamp": 1.664438699074805 + }, + { + "x": 2.275975845765142, + "y": 6.89287117810912, + "heading": 0.18638682928509862, + "angularVelocity": 0.8163465407124133, + "velocityX": -1.6380164902537586, + "velocityY": -0.23485123858993803, + "timestamp": 1.7394537892674826 + }, + { + "x": 2.1289785005722504, + "y": 6.871800394460987, + "heading": 0.2596886166153821, + "angularVelocity": 0.9771605571893133, + "velocityX": -1.95957033198688, + "velocityY": -0.28088726673543657, + "timestamp": 1.8144688794601602 + }, + { + "x": 1.9578590220057892, + "y": 6.847277322910046, + "heading": 0.34503981582540255, + "angularVelocity": 1.1377870637866898, + "velocityX": -2.2811340775161044, + "velocityY": -0.3269085125133225, + "timestamp": 1.8894839696528378 + }, + { + "x": 1.7626163173700675, + "y": 6.8193036552908985, + "heading": 0.4424228597913738, + "angularVelocity": 1.298179389184776, + "velocityX": -2.602712389390415, + "velocityY": -0.3729072050343056, + "timestamp": 1.9644990598455154 + }, + { + "x": 1.5454887933501882, + "y": 6.788364934906598, + "heading": 0.5521114777288451, + "angularVelocity": 1.4622207032709544, + "velocityX": -2.8944512825643915, + "velocityY": -0.4124332891533426, + "timestamp": 2.039514150038193 + }, + { + "x": 1.3524867399067801, + "y": 6.760871330718944, + "heading": 0.6497679920992822, + "angularVelocity": 1.3018249277525966, + "velocityX": -2.5728430499473998, + "velocityY": -0.3665076468885929, + "timestamp": 2.114529240230871 + }, + { + "x": 1.1836105740084832, + "y": 6.736819962879357, + "heading": 0.7353592838772256, + "angularVelocity": 1.140987654058677, + "velocityX": -2.251229258866918, + "velocityY": -0.3206203948806917, + "timestamp": 2.1895443304235487 + }, + { + "x": 1.0388602648983114, + "y": 6.7162082631102376, + "heading": 0.8088372428736526, + "angularVelocity": 0.9795090402170741, + "velocityX": -1.9296158777970915, + "velocityY": -0.2747673796855824, + "timestamp": 2.2645594206162265 + }, + { + "x": 0.9182354949315064, + "y": 6.6990341480262074, + "heading": 0.8701477848986364, + "angularVelocity": 0.8173094489056358, + "velocityX": -1.6080067311387376, + "velocityY": -0.22894213737419178, + "timestamp": 2.3395745108089043 + }, + { + "x": 0.8217358494246105, + "y": 6.685296075556227, + "heading": 0.919240073281317, + "angularVelocity": 0.6544321716682079, + "velocityX": -1.286403112480899, + "velocityY": -0.18313745187394112, + "timestamp": 2.414589601001582 + }, + { + "x": 0.7493609915443247, + "y": 6.674993018423988, + "heading": 0.956074222977567, + "angularVelocity": 0.49102320082053863, + "velocityX": -0.9648039840302745, + "velocityY": -0.13734646063578734, + "timestamp": 2.48960469119426 + }, + { + "x": 0.7011107973318162, + "y": 6.66812439818581, + "heading": 0.9806268225378217, + "angularVelocity": 0.32730214010529163, + "velocityX": -0.6432065080316098, + "velocityY": -0.09156318042856706, + "timestamp": 2.564619781386938 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.6489738584934847e-26, - "angularVelocity": 6.447188733213427e-26, - "velocityX": -2.3207888161938065e-29, - "velocityY": -7.774803790845182e-27, - "timestamp": 2.7584534933373352 - }, - { - "x": 1.3242756726096836, - "y": 5.5904571777752485, - "heading": -6.485537692324558e-18, - "angularVelocity": -6.918907801272556e-17, - "velocityX": 0.3659270961745246, - "velocityY": -0.005303363934441593, - "timestamp": 2.8521913496417257 - }, - { - "x": 1.3928781149651148, - "y": 5.58946292585347, - "heading": -1.1202216291674618e-17, - "angularVelocity": -5.031804212956606e-17, - "velocityX": 0.7318541845207394, - "velocityY": -0.010606727755427862, - "timestamp": 2.945929205946116 - }, - { - "x": 1.4957817770305903, - "y": 5.587971547992073, - "heading": -1.4372763095426617e-17, - "angularVelocity": -3.382497812599112e-17, - "velocityX": 1.0977812611239222, - "velocityY": -0.015910091406222935, - "timestamp": 3.0396670622505066 - }, - { - "x": 1.6329866569713576, - "y": 5.585983044217648, - "heading": -1.5623455517606444e-17, - "angularVelocity": -1.3343083564783124e-17, - "velocityX": 1.4637083181538741, - "velocityY": -0.021213454773344104, - "timestamp": 3.133404918554897 - }, - { - "x": 1.8044927511173796, - "y": 5.583497414583386, - "heading": -1.5212462606875703e-17, - "angularVelocity": 4.38274958491921e-18, - "velocityX": 1.8296353360316953, - "velocityY": -0.02651681757303531, - "timestamp": 3.2271427748592876 - }, - { - "x": 2.0103000484542934, - "y": 5.580514659248916, - "heading": -1.3020782793974884e-17, - "angularVelocity": 2.337996988615278e-17, - "velocityX": 2.1955622364077554, - "velocityY": -0.031820178669779105, - "timestamp": 3.320880631163678 - }, - { - "x": 2.181845878009789, - "y": 5.578028453731261, - "heading": -1.0808447718251693e-17, - "angularVelocity": 2.360324840579071e-17, - "velocityX": 1.8300592334557981, - "velocityY": -0.026522961098165376, - "timestamp": 3.4146184874680685 - }, - { - "x": 2.3190905043488854, - "y": 5.576039373914184, - "heading": -1.0180400900611551e-17, - "angularVelocity": 6.701278506879843e-18, - "velocityX": 1.4641323328076266, - "velocityY": -0.021219599997477887, - "timestamp": 3.508356343772459 - }, - { - "x": 2.4220339164763502, - "y": 5.574547419957035, - "heading": -1.1310869476605041e-17, - "angularVelocity": -1.2059406429055699e-17, - "velocityX": 1.0982053148617765, - "velocityY": -0.015916237196800742, - "timestamp": 3.6020942000768494 - }, - { - "x": 2.490676110725689, - "y": 5.573552591912956, - "heading": -5.948170117055629e-18, - "angularVelocity": 5.721104182574728e-17, - "velocityX": 0.7322782578015892, - "velocityY": -0.010612873829241372, - "timestamp": 3.69583205638124 - }, - { - "x": 2.5250170852633893, - "y": 5.573054889808518, - "heading": -2.1807385415410986e-18, - "angularVelocity": 4.0191949957409276e-17, - "velocityX": 0.36635118118139864, - "velocityY": -0.0053095101781998125, - "timestamp": 3.7895699126856304 + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.16353322568319786, + "velocityX": -0.32160668782044516, + "velocityY": -0.04578252824590232, + "timestamp": 2.6396348715796156 }, { - "x": 2.525056838989258, - "y": 5.573054313659668, - "heading": -2.8826489417868528e-33, - "angularVelocity": 2.326435027113688e-17, - "velocityX": 0.0004240928242990202, - "velocityY": -0.000006146357055788897, - "timestamp": 3.883307768990021 - }, - { - "x": 2.4907821260381073, - "y": 5.573551055438095, - "heading": 8.600966160221058e-18, - "angularVelocity": 9.173661299705183e-17, - "velocityX": -0.3655736854951678, - "velocityY": 0.00529824197033788, - "timestamp": 3.9770637316095705 - }, - { - "x": 2.422192939812278, - "y": 5.57454511523942, - "heading": 1.5616353080474726e-17, - "angularVelocity": 7.482552449285854e-17, - "velocityX": -0.7315714559878368, - "velocityY": 0.010602630184298141, - "timestamp": 4.070819694229121 - }, - { - "x": 2.3192892814125363, - "y": 5.57603649304769, - "heading": 2.109129083859237e-17, - "angularVelocity": 5.839395174918292e-17, - "velocityX": -1.0975692147397424, - "velocityY": 0.015907018228100082, - "timestamp": 4.164575656848671 - }, - { - "x": 2.182071152673635, - "y": 5.578025188836313, - "heading": 2.4503256173748732e-17, - "angularVelocity": 3.639115358132533e-17, - "velocityX": -1.4635669539221987, - "velocityY": 0.021211405988282927, - "timestamp": 4.258331619468221 - }, - { - "x": 2.010538557265611, - "y": 5.5805112025521, - "heading": 2.631777380152167e-17, - "angularVelocity": 1.9353493618514127e-17, - "velocityX": -1.8295646539600892, - "velocityY": 0.026515793181145452, - "timestamp": 4.352087582087771 - }, - { - "x": 1.8046915062028241, - "y": 5.58349453403542, - "heading": 2.6479996589742425e-17, - "angularVelocity": 1.7308836974551526e-18, - "velocityX": -2.1955622365189336, - "velocityY": 0.03182017867138977, - "timestamp": 4.445843544707321 - }, - { - "x": 1.6331191680538633, - "y": 5.585981123740855, - "heading": 1.776473605055726e-17, - "angularVelocity": -9.295501651565233e-17, - "velocityX": -1.82998855142187, - "velocityY": 0.026521936706821576, - "timestamp": 4.539599507326871 - }, - { - "x": 1.495861285585166, - "y": 5.587970395678386, - "heading": 1.0702828905507157e-17, - "angularVelocity": -7.532110300046076e-17, - "velocityX": -1.4639909685910264, - "velocityY": 0.021217551212635195, - "timestamp": 4.633355469946421 - }, - { - "x": 1.3929178697919677, - "y": 5.589462349688662, - "heading": 5.2389574400340815e-18, - "angularVelocity": -5.827717130868218e-17, - "velocityX": -1.097993268485135, - "velocityY": 0.01591316401878714, - "timestamp": 4.727111432565971 - }, - { - "x": 1.3242889243407625, - "y": 5.590456985718543, - "heading": 1.764781469065966e-18, - "angularVelocity": -3.705591096010782e-17, - "velocityX": -0.7319955292724558, - "velocityY": 0.010608776258166279, - "timestamp": 4.820867395185521 + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": -9.098710086763465e-32, + "velocityX": -1.2738111607141702e-32, + "velocityY": 8.271252813231263e-33, + "timestamp": 2.7146499617722935 + }, + { + "x": 0.6985020843742907, + "y": 6.667569321822967, + "heading": 0.9798246448067045, + "angularVelocity": -0.18429871952070437, + "velocityX": 0.3034123592993086, + "velocityY": 0.04060189632535011, + "timestamp": 2.7855654702426254 + }, + { + "x": 0.7415443471125377, + "y": 6.673333307735536, + "heading": 0.9539749013573504, + "angularVelocity": -0.36451467396822734, + "velocityX": 0.6069513378199061, + "velocityY": 0.081279624681546, + "timestamp": 2.8564809787129573 + }, + { + "x": 0.8061231921601518, + "y": 6.681988659505296, + "heading": 0.9157093279961269, + "angularVelocity": -0.5395938658076773, + "velocityX": 0.9106448848862336, + "velocityY": 0.12205160699625474, + "timestamp": 2.927396487183289 + }, + { + "x": 0.8922518573326212, + "y": 6.693543414119111, + "heading": 0.8654790283080297, + "angularVelocity": -0.7083119161320186, + "velocityX": 1.2145251021996393, + "velocityY": 0.16293692117639821, + "timestamp": 2.998311995653621 + }, + { + "x": 0.9999464058047857, + "y": 6.708007002320034, + "heading": 0.8038456081958845, + "angularVelocity": -0.8691105999462724, + "velocityX": 1.5186318309657076, + "velocityY": 0.20395522097926097, + "timestamp": 3.069227504123953 + }, + { + "x": 1.1292266926067027, + "y": 6.725390389005233, + "heading": 0.7315277652632032, + "angularVelocity": -1.0197747219557198, + "velocityX": 1.8230185412264566, + "velocityY": 0.2451281399536478, + "timestamp": 3.140143012594285 + }, + { + "x": 1.280117879093583, + "y": 6.7457065764596775, + "heading": 0.6494907937479546, + "angularVelocity": -1.1568269520279806, + "velocityX": 2.127760059000449, + "velocityY": 0.28648440789144525, + "timestamp": 3.2110585210646168 + }, + { + "x": 1.452652377278095, + "y": 6.768971929861101, + "heading": 0.5591180429519423, + "angularVelocity": -1.2743721753587984, + "velocityX": 2.4329586278958, + "velocityY": 0.3280714459116738, + "timestamp": 3.2819740295349487 + }, + { + "x": 1.6468709656789369, + "y": 6.795208953072923, + "heading": 0.46255966504991763, + "angularVelocity": -1.361597483890574, + "velocityX": 2.738732226422582, + "velocityY": 0.3699758173883681, + "timestamp": 3.3528895380052806 + }, + { + "x": 1.8628151056303066, + "y": 6.824451100190135, + "heading": 0.3635782548385496, + "angularVelocity": -1.3957653600238602, + "velocityX": 3.045090483158721, + "velocityY": 0.41235193468922193, + "timestamp": 3.4238050464756125 + }, + { + "x": 2.100434676341306, + "y": 6.8567478442017284, + "heading": 0.27059443428530877, + "angularVelocity": -1.3111916216766784, + "velocityX": 3.3507419721937035, + "velocityY": 0.4554256848500797, + "timestamp": 3.4947205549459444 + }, + { + "x": 2.35596385739112, + "y": 6.892112273565999, + "heading": 0.23346194806127452, + "angularVelocity": -0.5236158778945916, + "velocityX": 3.603290543375525, + "velocityY": 0.4986839991292765, + "timestamp": 3.5656360634162763 + }, + { + "x": 2.621781213038304, + "y": 6.927777327664925, + "heading": 0.23346192227272974, + "angularVelocity": -3.6365169422849345e-7, + "velocityX": 3.7483670551187007, + "velocityY": 0.5029231950560806, + "timestamp": 3.636551571886608 + }, + { + "x": 2.887598578486195, + "y": 6.963442308718448, + "heading": 0.23346189648452345, + "angularVelocity": -3.636469208413295e-7, + "velocityX": 3.748367193321304, + "velocityY": 0.502922165021807, + "timestamp": 3.70746708035694 + }, + { + "x": 3.153415943934485, + "y": 6.999107289769, + "heading": 0.23346187069631716, + "angularVelocity": -3.636469205820025e-7, + "velocityX": 3.7483671933269203, + "velocityY": 0.5029221649799431, + "timestamp": 3.778382588827272 + }, + { + "x": 3.4192333093827747, + "y": 7.034772270819553, + "heading": 0.2334618449081109, + "angularVelocity": -3.6364692054215583e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799415, + "timestamp": 3.849298097297604 + }, + { + "x": 3.6850506748310643, + "y": 7.070437251870106, + "heading": 0.23346181911990457, + "angularVelocity": -3.6364692122502775e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799416, + "timestamp": 3.9202136057679358 + }, + { + "x": 3.950868040279354, + "y": 7.106102232920659, + "heading": 0.2334617933316983, + "angularVelocity": -3.636469204641407e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799416, + "timestamp": 3.9911291142382677 + }, + { + "x": 4.216685405727644, + "y": 7.141767213971211, + "heading": 0.23346176754349196, + "angularVelocity": -3.6364692150109834e-7, + "velocityX": 3.7483671933269203, + "velocityY": 0.5029221649799415, + "timestamp": 4.0620446227086 + }, + { + "x": 4.482502771175934, + "y": 7.1774321950217645, + "heading": 0.2334617417552857, + "angularVelocity": -3.636469207728054e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799417, + "timestamp": 4.1329601311789315 + }, + { + "x": 4.7483201366242245, + "y": 7.213097176072317, + "heading": 0.2334617159670794, + "angularVelocity": -3.636469206602435e-7, + "velocityX": 3.7483671933269203, + "velocityY": 0.5029221649799417, + "timestamp": 4.203875639649263 + }, + { + "x": 5.014137502072515, + "y": 7.2487621571228695, + "heading": 0.2334616901788731, + "angularVelocity": -3.6364692079095153e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799416, + "timestamp": 4.274791148119595 + }, + { + "x": 5.279954867520805, + "y": 7.284427138173423, + "heading": 0.23346166439066685, + "angularVelocity": -3.636469205268936e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799417, + "timestamp": 4.345706656589927 + }, + { + "x": 5.545772232969095, + "y": 7.320092119223974, + "heading": 0.23346163860246044, + "angularVelocity": -3.6364692217914213e-7, + "velocityX": 3.748367193326924, + "velocityY": 0.5029221649799175, + "timestamp": 4.416622165060259 + }, + { + "x": 5.81158959842306, + "y": 7.355757100232221, + "heading": 0.23346161281425412, + "angularVelocity": -3.636469211715136e-7, + "velocityX": 3.748367193406962, + "velocityY": 0.5029221643833769, + "timestamp": 4.487537673530591 + }, + { + "x": 6.077407103529163, + "y": 7.391421040372378, + "heading": 0.23346158702575517, + "angularVelocity": -3.63651047258994e-7, + "velocityX": 3.748369162681932, + "velocityY": 0.5029074868027953, + "timestamp": 4.558453182000923 + }, + { + "x": 6.338444265915037, + "y": 7.413466456985474, + "heading": 0.21125647663717811, + "angularVelocity": -0.3131206539662162, + "velocityX": 3.680960173825454, + "velocityY": 0.310868766065739, + "timestamp": 4.629368690471255 + }, + { + "x": 6.578740605850335, + "y": 7.432100572083313, + "heading": 0.16288336837633732, + "angularVelocity": -0.6821231251705402, + "velocityX": 3.388487865610223, + "velocityY": 0.26276502135825913, + "timestamp": 4.700284198941587 + }, + { + "x": 6.797330438016248, + "y": 7.447554226429222, + "heading": 0.11319688856078455, + "angularVelocity": -0.7006433555551449, + "velocityX": 3.082398150714271, + "velocityY": 0.21791642870860864, + "timestamp": 4.771199707411919 + }, + { + "x": 6.99422444679472, + "y": 7.45992830223967, + "heading": 0.06789887708325426, + "angularVelocity": -0.638760300174411, + "velocityX": 2.7764590993639087, + "velocityY": 0.1744904052351846, + "timestamp": 4.8421152158822505 + }, + { + "x": 7.169459125576906, + "y": 7.4692727675317165, + "heading": 0.029649915927990267, + "angularVelocity": -0.539359612309142, + "velocityX": 2.4710346518279107, + "velocityY": 0.13176899515507345, + "timestamp": 4.913030724352582 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -1.882241835148467e-17, - "velocityX": -0.3659977705035494, - "velocityY": 0.00530438821411795, - "timestamp": 4.9146233578050715 + "x": 7.323064804077148, + "y": 7.475617408752441, + "heading": -2.508046324740631e-30, + "angularVelocity": -0.41810200007794424, + "velocityX": 2.166037892325118, + "velocityY": 0.08946761234010188, + "timestamp": 4.983946232822914 + }, + { + "x": 7.442670757639025, + "y": 7.478903848182195, + "heading": -0.0187747254681701, + "angularVelocity": -0.297438409871576, + "velocityX": 1.8948561830587192, + "velocityY": 0.05206538544539075, + "timestamp": 5.04706762147034 + }, + { + "x": 7.5451140174844875, + "y": 7.480040188586648, + "heading": -0.030609169580020613, + "angularVelocity": -0.1874870684158837, + "velocityX": 1.6229563708994625, + "velocityY": 0.01800246206270259, + "timestamp": 5.110189010117766 + }, + { + "x": 7.630363412663352, + "y": 7.479184974890449, + "heading": -0.03600750245908947, + "angularVelocity": -0.08552303735288487, + "velocityX": 1.3505627332580479, + "velocityY": -0.013548714857401807, + "timestamp": 5.173310398765192 + }, + { + "x": 7.698396363869973, + "y": 7.476461754192338, + "heading": -0.03536054977456298, + "angularVelocity": 0.01024934175875415, + "velocityX": 1.0778113831814056, + "velocityY": -0.04314259803938343, + "timestamp": 5.236431787412618 + }, + { + "x": 7.74919584977197, + "y": 7.471969501335234, + "heading": -0.02898050014133947, + "angularVelocity": 0.10107587570456572, + "velocityX": 0.8047903728082677, + "velocityY": -0.0711684732126695, + "timestamp": 5.299553176060044 + }, + { + "x": 7.782748628307226, + "y": 7.465789287122271, + "heading": -0.017122702576199005, + "angularVelocity": 0.1878570452777808, + "velocityX": 0.5315595751917994, + "velocityY": -0.09790998495750272, + "timestamp": 5.36267456470747 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, + "x": 7.799044132232666, + "y": 7.457988739013672, "heading": 0, - "angularVelocity": 0, - "velocityX": -1.5626059218246172e-29, - "velocityY": -2.5406263809505943e-29, - "timestamp": 5.008379320424622 - }, - { - "x": 1.3072041550588713, - "y": 5.60554242334907, - "heading": 0.009976426632135488, - "angularVelocity": 0.13081140582840126, - "velocityX": 0.22591674199036743, - "velocityY": 0.19128015517323346, - "timestamp": 5.084645044894665 - }, - { - "x": 1.341663665671124, - "y": 5.634718250195267, - "heading": 0.02992724315217698, - "angularVelocity": 0.26159610576452386, - "velocityX": 0.4518348296001005, - "velocityY": 0.3825549033430322, - "timestamp": 5.160910769364708 - }, - { - "x": 1.3933533134136329, - "y": 5.678481259628573, - "heading": 0.05984264331407307, - "angularVelocity": 0.3922522257112071, - "velocityX": 0.6777572508972288, - "velocityY": 0.5738227722565116, - "timestamp": 5.237176493834752 - }, - { - "x": 1.4622736330550405, - "y": 5.7368307766764, - "heading": 0.09970739234331524, - "angularVelocity": 0.5227085865313741, - "velocityX": 0.9036866841075589, - "velocityY": 0.7650817908955898, - "timestamp": 5.313442218304795 - }, - { - "x": 1.548425323078623, - "y": 5.809765941188483, - "heading": 0.1495038423997354, - "angularVelocity": 0.652933547987283, - "velocityX": 1.1296252758655412, - "velocityY": 0.9563295312013731, - "timestamp": 5.389707942774838 - }, - { - "x": 1.6518091876527043, - "y": 5.897285689503854, - "heading": 0.20921528267987785, - "angularVelocity": 0.7829393965514132, - "velocityX": 1.3555744119188056, - "velocityY": 1.1475633247313204, - "timestamp": 5.4659736672448815 - }, - { - "x": 1.7724260670196361, - "y": 5.999388766272788, - "heading": 0.27882903202196435, - "angularVelocity": 0.9127789690876109, - "velocityX": 1.5815345652717379, - "velocityY": 1.3387806580529145, - "timestamp": 5.542239391714925 - }, - { - "x": 1.910276772460734, - "y": 6.11607377416316, - "heading": 0.3583384554068513, - "angularVelocity": 1.042531542921605, - "velocityX": 1.807505356185654, - "velocityY": 1.5299796694859533, - "timestamp": 5.618505116184968 - }, - { - "x": 2.065362059261307, - "y": 6.247339244509118, - "heading": 0.4477430189105342, - "angularVelocity": 1.172277115920654, - "velocityX": 2.033486050437564, - "velocityY": 1.7211594236996055, - "timestamp": 5.6947708406550115 - }, - { - "x": 2.203284027987246, - "y": 6.363982908806699, - "heading": 0.5270469936467461, - "angularVelocity": 1.0398376893804102, - "velocityX": 1.8084397630070208, - "velocityY": 1.5294375694163855, - "timestamp": 5.771036565125055 - }, - { - "x": 2.323970561231782, - "y": 6.4660479017472055, - "heading": 0.5964497874037734, - "angularVelocity": 0.9100129083298684, - "velocityX": 1.5824478694623139, - "velocityY": 1.3382813001795226, - "timestamp": 5.847302289595098 - }, - { - "x": 2.427421209360281, - "y": 6.553534871396325, - "heading": 0.6559501239234673, - "angularVelocity": 0.7801713929154328, - "velocityX": 1.3564500802865749, - "velocityY": 1.147133528411577, - "timestamp": 5.923568014065141 - }, - { - "x": 2.5136355947658133, - "y": 6.6264442966918295, - "heading": 0.7055475231333362, - "angularVelocity": 0.6503235827654283, - "velocityX": 1.1304473399101456, - "velocityY": 0.955992036499409, - "timestamp": 5.999833738535185 - }, - { - "x": 2.582613392420247, - "y": 6.6847765351095845, - "heading": 0.7452421346528295, - "angularVelocity": 0.5204777344326409, - "velocityX": 0.9044403377486854, - "velocityY": 0.7648552318285594, - "timestamp": 6.076099463005228 - }, - { - "x": 2.634354338508759, - "y": 6.728531844563373, - "heading": 0.7750344345644219, - "angularVelocity": 0.3906381285168185, - "velocityX": 0.6784298764341418, - "velocityY": 0.5737218089360453, - "timestamp": 6.152365187475271 - }, - { - "x": 2.6688582435593817, - "y": 6.757710392622863, - "heading": 0.7949250057401149, - "angularVelocity": 0.26080616524767886, - "velocityX": 0.4524169307537305, - "velocityY": 0.3825905832253757, - "timestamp": 6.228630911945315 - }, - { - "x": 2.686125003210816, - "y": 6.772312259170968, - "heading": 0.8049144291757789, - "angularVelocity": 0.13098182002535327, - "velocityX": 0.22640261668277428, - "velocityY": 0.19146040494338912, - "timestamp": 6.304896636415358 - }, - { - "x": 2.68615460395813, - "y": 6.772337436676025, - "heading": 0.8050032485420815, - "angularVelocity": 0.0011646034399446482, - "velocityX": 0.0003881255345023293, - "velocityY": 0.00033012790587399484, - "timestamp": 6.381162360885401 - }, - { - "x": 2.668940238370334, - "y": 6.757780008152215, - "heading": 0.7951880324492147, - "angularVelocity": -0.12867188925922995, - "velocityX": -0.22567052234080015, - "velocityY": -0.1908395916273825, - "timestamp": 6.457443327229603 - }, - { - "x": 2.6344820969778255, - "y": 6.728639813103283, - "heading": 0.7754676749092158, - "angularVelocity": -0.2585226492096862, - "velocityX": -0.4517265968129291, - "velocityY": -0.3820113508147273, - "timestamp": 6.533724293573805 - }, - { - "x": 2.582780444824575, - "y": 6.6849166026467195, - "heading": 0.7458414179763078, - "angularVelocity": -0.3883833453977379, - "velocityX": -0.6777791967096236, - "velocityY": -0.5731863727099495, - "timestamp": 6.610005259918006 - }, - { - "x": 2.513835599527289, - "y": 6.6266100250455136, - "heading": 0.7063091354285856, - "angularVelocity": -0.518245696283539, - "velocityX": -0.9038276328322399, - "velocityY": -0.7643660056769808, - "timestamp": 6.686286226262208 - }, - { - "x": 2.427647916545295, - "y": 6.553719605348423, - "heading": 0.6568717276359699, - "angularVelocity": -0.6480962438334487, - "velocityX": -1.1298714090855582, - "velocityY": -0.9555518649841811, - "timestamp": 6.76256719260641 - }, - { - "x": 2.3242177842468106, - "y": 6.466244710155765, - "heading": 0.5975314416664378, - "angularVelocity": -0.7779173336036685, - "velocityX": -1.3559100940527828, - "velocityY": -1.1467460278003494, - "timestamp": 6.838848158950611 - }, - { - "x": 2.2035456412981804, - "y": 6.364184497615964, - "heading": 0.5282916867852259, - "angularVelocity": -0.907693729851007, - "velocityX": -1.5819430283540434, - "velocityY": -1.3379512268110498, - "timestamp": 6.915129125294813 - }, - { - "x": 2.0656320318873616, - "y": 6.247537861954298, - "heading": 0.4491555706593054, - "angularVelocity": -1.0374293863583475, - "velocityX": -1.8079688286610023, - "velocityY": -1.529170921898447, - "timestamp": 6.991410091639015 - }, - { - "x": 1.9104777180009005, - "y": 6.1163033962265265, - "heading": 0.3601220361949963, - "angularVelocity": -1.1671789007706412, - "velocityX": -2.0339846397576693, - "velocityY": -1.7204090612237575, - "timestamp": 7.067691057983216 - }, - { - "x": 1.7725837401718476, - "y": 5.99956344062706, - "heading": 0.2802286748074377, - "angularVelocity": -1.047356440677276, - "velocityX": -1.8077114707660742, - "velocityY": -1.530394293900403, - "timestamp": 7.143972024327418 - }, - { - "x": 1.6519287666788485, - "y": 5.897413817352657, - "heading": 0.21025683762829658, - "angularVelocity": -0.9172909120831881, - "velocityX": -1.581717947527036, - "velocityY": -1.3391233513887155, - "timestamp": 7.22025299067162 - }, - { - "x": 1.5485118574908003, - "y": 5.80985551317875, - "heading": 0.15022948194911198, - "angularVelocity": -0.7869244264698967, - "velocityX": -1.3557367475609723, - "velocityY": -1.1478394728127326, - "timestamp": 7.296533957015821 - }, - { - "x": 1.4623321603791597, - "y": 5.736889288541215, - "heading": 0.10017126259617518, - "angularVelocity": -0.6562347301302999, - "velocityX": -1.1297667198257733, - "velocityY": -0.9565456253555378, - "timestamp": 7.372814923360023 - }, - { - "x": 1.393388970887201, - "y": 5.678515706716561, - "heading": 0.06010509802987578, - "angularVelocity": -0.5252445858334249, - "velocityX": -0.9038059271169646, - "velocityY": -0.7652443936641671, - "timestamp": 7.449095889704225 - }, - { - "x": 1.3416817772407035, - "y": 5.634735172181737, - "heading": 0.030048985724868114, - "angularVelocity": -0.39401850489866874, - "velocityX": -0.6778518434766467, - "velocityY": -0.5739378595104004, - "timestamp": 7.525376856048426 - }, - { - "x": 1.307210288791871, - "y": 5.605547971029586, - "heading": 0.010013480371388656, - "angularVelocity": -0.2626540579888315, - "velocityX": -0.4519015700633713, - "velocityY": -0.38262757484247345, - "timestamp": 7.601657822392628 + "angularVelocity": 0.2712662528992439, + "velocityX": 0.25816136612733137, + "velocityY": -0.12358010930615114, + "timestamp": 5.425795953354896 + }, + { + "x": 7.789461840084544, + "y": 7.44456429977742, + "heading": 0.032498029722470304, + "angularVelocity": 0.38031209369790614, + "velocityX": -0.11213792406398307, + "velocityY": -0.15710111155221265, + "timestamp": 5.511246903406188 + }, + { + "x": 7.748231627408751, + "y": 7.428284690558898, + "heading": 0.07405407961857692, + "angularVelocity": 0.48631466205092383, + "velocityX": -0.48250151286758974, + "velocityY": -0.1905140809873915, + "timestamp": 5.59669785345748 + }, + { + "x": 7.675346865346751, + "y": 7.40916132937864, + "heading": 0.1243473311272727, + "angularVelocity": 0.5885628126840949, + "velocityX": -0.8529426766838836, + "velocityY": -0.22379342966671825, + "timestamp": 5.682148803508771 + }, + { + "x": 7.570799431207237, + "y": 7.387208662011206, + "heading": 0.1829724416238929, + "angularVelocity": 0.6860673925971562, + "velocityX": -1.2234788972702, + "velocityY": -0.2569037249352633, + "timestamp": 5.767599753560063 + }, + { + "x": 7.434579177146227, + "y": 7.362445545124385, + "heading": 0.24939989698187892, + "angularVelocity": 0.7773752698843693, + "velocityX": -1.5941338742188234, + "velocityY": -0.28979334778557075, + "timestamp": 5.853050703611355 + }, + { + "x": 7.266673120974145, + "y": 7.334897700728802, + "heading": 0.32290448608070227, + "angularVelocity": 0.8601962769834678, + "velocityX": -1.9649407767992484, + "velocityY": -0.3223819557190156, + "timestamp": 5.938501653662646 + }, + { + "x": 7.067064218914486, + "y": 7.304602559914413, + "heading": 0.4024227499708516, + "angularVelocity": 0.9305720280744717, + "velocityX": -2.3359471362200686, + "velocityY": -0.3545325218292511, + "timestamp": 6.023952603713938 + }, + { + "x": 6.835729836802405, + "y": 7.27162039096164, + "heading": 0.48622750779609625, + "angularVelocity": 0.9807352378746566, + "velocityX": -2.7072183746718483, + "velocityY": -0.385977791153593, + "timestamp": 6.10940355376523 + }, + { + "x": 6.572643492795426, + "y": 7.236066659207437, + "heading": 0.571010808696293, + "angularVelocity": 0.9921867556667185, + "velocityX": -3.078799520063915, + "velocityY": -0.4160718135124634, + "timestamp": 6.194854503816521 + }, + { + "x": 6.277837622424222, + "y": 7.198257703016904, + "heading": 0.6480492113566031, + "angularVelocity": 0.901551154360156, + "velocityX": -3.450001084764899, + "velocityY": -0.4424638481823218, + "timestamp": 6.280305453867813 + }, + { + "x": 5.957362245190181, + "y": 7.156598764006078, + "heading": 0.6480492838419923, + "angularVelocity": 8.482689682334955e-7, + "velocityX": -3.7504015700432527, + "velocityY": -0.487518734265005, + "timestamp": 6.365756403919105 + }, + { + "x": 5.635584365409571, + "y": 7.126616390006558, + "heading": 0.6480492987117265, + "angularVelocity": 1.7401484932642463e-7, + "velocityX": -3.7656442624390154, + "velocityY": -0.3508723306355692, + "timestamp": 6.451207353970396 + }, + { + "x": 5.313806456205795, + "y": 7.096634331785028, + "heading": 0.6480493135814611, + "angularVelocity": 1.7401485546331995e-7, + "velocityX": -3.7656446067671383, + "velocityY": -0.3508686352057408, + "timestamp": 6.536658304021688 + }, + { + "x": 4.992028547001338, + "y": 7.066652273570816, + "heading": 0.6480493284511959, + "angularVelocity": 1.7401485563507226e-7, + "velocityX": -3.7656446067751177, + "velocityY": -0.3508686351200953, + "timestamp": 6.62210925407298 + }, + { + "x": 4.67025063779688, + "y": 7.036670215356605, + "heading": 0.6480493433209307, + "angularVelocity": 1.7401485639450837e-7, + "velocityX": -3.765644606775118, + "velocityY": -0.35086863512009386, + "timestamp": 6.707560204124271 + }, + { + "x": 4.348472728592423, + "y": 7.006688157142394, + "heading": 0.6480493581906654, + "angularVelocity": 1.740148559122135e-7, + "velocityX": -3.7656446067751186, + "velocityY": -0.35086863512009364, + "timestamp": 6.793011154175563 + }, + { + "x": 4.0266948193879655, + "y": 6.976706098928182, + "heading": 0.6480493730604002, + "angularVelocity": 1.740148565017462e-7, + "velocityX": -3.765644606775118, + "velocityY": -0.35086863512009375, + "timestamp": 6.878462104226855 + }, + { + "x": 3.7049169101835084, + "y": 6.946724040713971, + "heading": 0.648049387930135, + "angularVelocity": 1.7401485552645738e-7, + "velocityX": -3.765644606775118, + "velocityY": -0.3508686351200936, + "timestamp": 6.963913054278146 + }, + { + "x": 3.3831390009790514, + "y": 6.91674198249976, + "heading": 0.6480494027998698, + "angularVelocity": 1.7401485630496073e-7, + "velocityX": -3.7656446067751177, + "velocityY": -0.35086863512009364, + "timestamp": 7.049364004329438 + }, + { + "x": 3.0613610917745944, + "y": 6.886759924285549, + "heading": 0.6480494176696047, + "angularVelocity": 1.7401485676252163e-7, + "velocityX": -3.7656446067751177, + "velocityY": -0.3508686351200934, + "timestamp": 7.1348149543807295 + }, + { + "x": 2.7395831825701378, + "y": 6.85677786607133, + "heading": 0.6480494325393393, + "angularVelocity": 1.740148561275008e-7, + "velocityX": -3.7656446067751106, + "velocityY": -0.35086863512017596, + "timestamp": 7.220265904432021 + }, + { + "x": 2.4178052733940274, + "y": 6.826795807553092, + "heading": 0.648049447409112, + "angularVelocity": 1.740152990535353e-7, + "velocityX": -3.765644606443389, + "velocityY": -0.3508686386780044, + "timestamp": 7.305716854483313 + }, + { + "x": 2.101806987486401, + "y": 6.79733725091672, + "heading": 0.6651218958442715, + "angularVelocity": 0.19979237708727937, + "velocityX": -3.698007871392473, + "velocityY": -0.34474229506742526, + "timestamp": 7.3911678045346045 + }, + { + "x": 1.8167368698048942, + "y": 6.770773756148669, + "heading": 0.7108427694079615, + "angularVelocity": 0.5350540109419666, + "velocityX": -3.3360672702926424, + "velocityY": -0.31086248604736477, + "timestamp": 7.476618754585896 + }, + { + "x": 1.563349701636476, + "y": 6.747170828438536, + "heading": 0.7627235430971158, + "angularVelocity": 0.6071409815583658, + "velocityX": -2.9652937505825308, + "velocityY": -0.2762160946831818, + "timestamp": 7.562069704637188 + }, + { + "x": 1.3416809560568124, + "y": 6.726531033314501, + "heading": 0.8140209677583794, + "angularVelocity": 0.6003142695377961, + "velocityX": -2.5941051029464517, + "velocityY": -0.24153967991752093, + "timestamp": 7.6475206546884795 + }, + { + "x": 1.1517177468979243, + "y": 6.708849739610871, + "heading": 0.8615071361475246, + "angularVelocity": 0.5557125855327181, + "velocityX": -2.2230672572379273, + "velocityY": -0.20691746192426994, + "timestamp": 7.732971604739771 + }, + { + "x": 0.993443621158485, + "y": 6.694122461254965, + "heading": 0.9032823319492421, + "angularVelocity": 0.48887924331700494, + "velocityX": -1.852221954752358, + "velocityY": -0.17234774273507145, + "timestamp": 7.818422554791063 + }, + { + "x": 0.8668441821472115, + "y": 6.682345527785774, + "heading": 0.9380916664803222, + "angularVelocity": 0.40736041565660025, + "velocityX": -1.4815451312746155, + "velocityY": -0.13782097755638892, + "timestamp": 7.9038735048423545 + }, + { + "x": 0.7719075388323822, + "y": 6.673515992217611, + "heading": 0.9650448121199915, + "angularVelocity": 0.3154224221444248, + "velocityX": -1.111007464022926, + "velocityY": -0.10332869983146084, + "timestamp": 7.989324454893646 + }, + { + "x": 0.7086239541540638, + "y": 6.667631472022659, + "heading": 0.9834806533033196, + "angularVelocity": 0.21574764437724195, + "velocityX": -0.7405837458838845, + "velocityY": -0.06886430392430233, + "timestamp": 8.074775404944939 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -6.199521898254585e-29, - "angularVelocity": -0.13127102150595574, - "velocityX": -0.22595201079473404, - "velocityY": -0.19131466197828328, - "timestamp": 7.67793878873683 + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.11016412225954963, + "velocityX": -0.37025347871120917, + "velocityY": -0.03442272228334841, + "timestamp": 8.160226354996231 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.3170947337368488e-29, - "angularVelocity": 6.400644992589977e-28, - "velocityX": 2.2164409811256884e-29, - "velocityY": 1.733201459680474e-29, - "timestamp": 7.7542197550810315 - }, - { - "x": 1.3080029680398912, - "y": 5.59126500545377, - "heading": 8.890962339325397e-19, - "angularVelocity": 1.3082751589925716e-17, - "velocityX": 0.2652835543681239, - "velocityY": 0.004571871036655683, - "timestamp": 7.82217917690712 - }, - { - "x": 1.344062678768715, - "y": 5.591577976323001, - "heading": 2.6666437965292957e-18, - "angularVelocity": 2.6156013615677146e-17, - "velocityX": 0.5306064966400389, - "velocityY": 0.004605260916318704, - "timestamp": 7.890138598733209 - }, - { - "x": 1.398150210579935, - "y": 5.5915422166660145, - "heading": 8.768948399955272e-19, - "angularVelocity": -2.6335553017414816e-17, - "velocityX": 0.7958798111854469, - "velocityY": -0.000526191306891862, - "timestamp": 7.958098020559298 - }, - { - "x": 1.4702532783568034, - "y": 5.590755190471311, - "heading": -4.372782060498752e-19, - "angularVelocity": -1.9337613692601342e-17, - "velocityX": 1.060972354376158, - "velocityY": -0.011580825344825147, - "timestamp": 8.026057442385387 - }, - { - "x": 1.560346433856685, - "y": 5.58875133578056, - "heading": -1.4651998990372215e-18, - "angularVelocity": -1.512552145628718e-17, - "velocityX": 1.3256904352487537, - "velocityY": -0.029486046774778202, - "timestamp": 8.094016864211476 - }, - { - "x": 1.6683846538857667, - "y": 5.584987203919277, - "heading": -2.5518211074829943e-18, - "angularVelocity": -1.5989265053292237e-17, - "velocityX": 1.5897460149904699, - "velocityY": -0.05538793238877104, - "timestamp": 8.161976286037564 - }, - { - "x": 1.7942934964417667, - "y": 5.578822081824432, - "heading": -4.145349651168243e-18, - "angularVelocity": -2.3448235739309385e-17, - "velocityX": 1.852706205744448, - "velocityY": -0.0907176949006653, - "timestamp": 8.229935707863653 - }, - { - "x": 1.937953698284471, - "y": 5.569492606449359, - "heading": -1.0979652144550452e-17, - "angularVelocity": -1.0056445905133748e-16, - "velocityX": 2.1139114780925645, - "velocityY": -0.13728008750497092, - "timestamp": 8.297895129689742 - }, - { - "x": 2.099176605031056, - "y": 5.556079595057618, - "heading": -1.979270285045548e-17, - "angularVelocity": -1.2968107245612037e-16, - "velocityX": 2.3723407647457755, - "velocityY": -0.19736794444875114, - "timestamp": 8.365854551515831 - }, - { - "x": 2.2776642824157327, - "y": 5.537465473885052, - "heading": -2.5427652949352827e-17, - "angularVelocity": -8.291639256904635e-17, - "velocityX": 2.6263860490372255, - "velocityY": -0.2739005228767256, - "timestamp": 8.43381397334192 - }, - { - "x": 2.472944009712466, - "y": 5.512282419629533, - "heading": -2.829850545817078e-17, - "angularVelocity": -4.224362761891433e-17, - "velocityX": 2.8734754070813326, - "velocityY": -0.3705601604434455, - "timestamp": 8.501773395168009 - }, - { - "x": 2.6842610977752166, - "y": 5.47885759062817, - "heading": -2.887338329485776e-17, - "angularVelocity": -8.459133718914728e-18, - "velocityX": 3.109459768558945, - "velocityY": -0.49183509958190536, - "timestamp": 8.569732816994097 - }, - { - "x": 2.9104101060267347, - "y": 5.435179084259705, - "heading": -3.3223671712936815e-17, - "angularVelocity": -6.40130286748416e-17, - "velocityX": 3.32770647799568, - "velocityY": -0.6427145080816032, - "timestamp": 8.637692238820186 - }, - { - "x": 3.1494985869220757, - "y": 5.378942960028073, - "heading": -3.8165211155711695e-17, - "angularVelocity": -7.271308833998824e-17, - "velocityX": 3.5181064592806273, - "velocityY": -0.8274956248971932, - "timestamp": 8.705651660646275 - }, - { - "x": 3.3987095123055417, - "y": 5.307785046252999, - "heading": -4.255972014068572e-17, - "angularVelocity": -6.466371942097715e-17, - "velocityX": 3.667054820172052, - "velocityY": -1.0470647316154307, - "timestamp": 8.773611082472364 - }, - { - "x": 3.6542758900932353, - "y": 5.21975313201646, - "heading": -4.248444848952633e-17, - "angularVelocity": 1.107596991510937e-18, - "velocityX": 3.7605731614624154, - "velocityY": -1.2953599643889915, - "timestamp": 8.841570504298453 - }, - { - "x": 3.9119052041412368, - "y": 5.113808430044301, - "heading": -4.228369470192057e-17, - "angularVelocity": 2.9540243605887717e-18, - "velocityX": 3.790928573631551, - "velocityY": -1.5589406019856407, - "timestamp": 8.909529926124542 + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": -1.2387955760475958e-29, + "velocityX": 1.44513347457817e-31, + "velocityY": -4.5041627226851734e-30, + "timestamp": 8.245677305047524 + }, + { + "x": 0.6937129747352162, + "y": 6.6645584157609425, + "heading": 0.9790660169157249, + "angularVelocity": -0.22114767057901943, + "velocityX": 0.2675140141228054, + "velocityY": -0.002104635808322496, + "timestamp": 8.308206859439915 + }, + { + "x": 0.7271833460248389, + "y": 6.664303045137076, + "heading": 0.9517582917370493, + "angularVelocity": -0.43671709232583583, + "velocityX": 0.5352728260237692, + "velocityY": -0.004083998780231037, + "timestamp": 8.370736413832306 + }, + { + "x": 0.7774150547311363, + "y": 6.663933728618546, + "heading": 0.9114059815034875, + "angularVelocity": -0.645331805506542, + "velocityX": 0.8033274696166545, + "velocityY": -0.005906271396278706, + "timestamp": 8.433265968224697 + }, + { + "x": 0.8444295954022788, + "y": 6.663462332060666, + "heading": 0.8585277843526951, + "angularVelocity": -0.8456512710608117, + "velocityX": 1.0717258634310107, + "velocityY": -0.007538780061052878, + "timestamp": 8.495795522617088 + }, + { + "x": 0.928251455530078, + "y": 6.662902493101364, + "heading": 0.793730196165171, + "angularVelocity": -1.0362713890602748, + "velocityX": 1.3405158719313994, + "velocityY": -0.008953189651546989, + "timestamp": 8.55832507700948 + }, + { + "x": 1.0289086002870482, + "y": 6.66226903623183, + "heading": 0.7177284863656903, + "angularVelocity": -1.2154526053799588, + "velocityX": 1.6097531117096386, + "velocityY": -0.010130519490973162, + "timestamp": 8.62085463140187 + }, + { + "x": 1.1464337376402622, + "y": 6.661577427221888, + "heading": 0.6314012242837341, + "angularVelocity": -1.3805833564753438, + "velocityX": 1.8795134316120001, + "velocityY": -0.011060513970755739, + "timestamp": 8.683384185794262 + }, + { + "x": 1.2808663505867972, + "y": 6.660844036954463, + "heading": 0.5359002148977876, + "angularVelocity": -1.5272939382655537, + "velocityX": 2.149905180883418, + "velocityY": -0.011728698126054097, + "timestamp": 8.745913740186653 + }, + { + "x": 1.4322545638154922, + "y": 6.660088220029616, + "heading": 0.43284520187288844, + "angularVelocity": -1.6481008704811424, + "velocityX": 2.4210665612405937, + "velocityY": -0.012087355046610722, + "timestamp": 8.808443294579044 + }, + { + "x": 1.6006535211132875, + "y": 6.65933694718367, + "heading": 0.3246624035723361, + "angularVelocity": -1.7301066567926002, + "velocityX": 2.69310982517228, + "velocityY": -0.012014684148101123, + "timestamp": 8.870972848971435 + }, + { + "x": 1.7861082105175972, + "y": 6.658632447498398, + "heading": 0.21527496164562404, + "angularVelocity": -1.7493718448763058, + "velocityX": 2.965872557487375, + "velocityY": -0.011266667292259528, + "timestamp": 8.933502403363827 + }, + { + "x": 1.9885470293511631, + "y": 6.658050737392514, + "heading": 0.11225233797545536, + "angularVelocity": -1.6475828857450405, + "velocityX": 3.2374901884507423, + "velocityY": -0.009302962599615121, + "timestamp": 8.996031957756218 + }, + { + "x": 2.206257706753187, + "y": 6.657808122247979, + "heading": 0.04234893056424459, + "angularVelocity": -1.1179258846552083, + "velocityX": 3.4817244344302054, + "velocityY": -0.0038800075722999405, + "timestamp": 9.058561512148609 + }, + { + "x": 2.437370646545927, + "y": 6.657710110655747, + "heading": 0.021561241595864127, + "angularVelocity": -0.33244581974679344, + "velocityX": 3.6960592801035483, + "velocityY": -0.0015674442779168942, + "timestamp": 9.121091066541 + }, + { + "x": 2.6738489696177092, + "y": 6.656073614188434, + "heading": 0.021561206140011186, + "angularVelocity": -5.670255175242252e-7, + "velocityX": 3.7818648376703368, + "velocityY": -0.02617156772049459, + "timestamp": 9.183620620933391 + }, + { + "x": 2.910327291911898, + "y": 6.654437005347333, + "heading": 0.021561170684433324, + "angularVelocity": -5.670211183604034e-7, + "velocityX": 3.7818648252347256, + "velocityY": -0.026173364851293936, + "timestamp": 9.246150175325782 + }, + { + "x": 3.14680561420604, + "y": 6.652800396499498, + "heading": 0.021561135228855402, + "angularVelocity": -5.670211192982607e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958984518, + "timestamp": 9.308679729718174 + }, + { + "x": 3.3832839365001823, + "y": 6.6511637876516625, + "heading": 0.02156109977327751, + "angularVelocity": -5.670211188277871e-7, + "velocityX": 3.7818648252339804, + "velocityY": -0.02617336495899097, + "timestamp": 9.371209284110565 + }, + { + "x": 3.6197622587943243, + "y": 6.649527178803828, + "heading": 0.02156106431769964, + "angularVelocity": -5.670211185570758e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958990947, + "timestamp": 9.433738838502956 + }, + { + "x": 3.8562405810884663, + "y": 6.647890569955993, + "heading": 0.02156102886212173, + "angularVelocity": -5.670211191121069e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958990954, + "timestamp": 9.496268392895347 + }, + { + "x": 4.092718903382608, + "y": 6.646253961108158, + "heading": 0.02156099340654377, + "angularVelocity": -5.670211198937042e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.02617336495899093, + "timestamp": 9.558797947287738 + }, + { + "x": 4.329197225676751, + "y": 6.6446173522603225, + "heading": 0.021560957950965815, + "angularVelocity": -5.67021119881002e-7, + "velocityX": 3.7818648252339804, + "velocityY": -0.026173364958990898, + "timestamp": 9.62132750168013 + }, + { + "x": 4.565675547970892, + "y": 6.642980743412488, + "heading": 0.021560922495387855, + "angularVelocity": -5.670211198956703e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958990943, + "timestamp": 9.68385705607252 + }, + { + "x": 4.802153870265035, + "y": 6.641344134564649, + "heading": 0.02156088703980993, + "angularVelocity": -5.670211193772164e-7, + "velocityX": 3.7818648252339795, + "velocityY": -0.026173364959042097, + "timestamp": 9.746386610464912 + }, + { + "x": 5.038632192558808, + "y": 6.639707525663405, + "heading": 0.021560851584232044, + "angularVelocity": -5.67021118724796e-7, + "velocityX": 3.781864825228069, + "velocityY": -0.02617336581314012, + "timestamp": 9.808916164857303 + }, + { + "x": 5.275110508682838, + "y": 6.638070025521481, + "heading": 0.021560816128648946, + "angularVelocity": -5.670212021371837e-7, + "velocityX": 3.781864726558842, + "velocityY": -0.02618761892412933, + "timestamp": 9.871445719249694 + }, + { + "x": 5.511317621528859, + "y": 6.626629955813148, + "heading": 0.02156078050955198, + "angularVelocity": -5.696361874718345e-7, + "velocityX": 3.777527525044398, + "velocityY": -0.18295460153997642, + "timestamp": 9.933975273642085 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -4.208890664344976e-17, - "angularVelocity": 2.8662406659266305e-18, - "velocityX": 3.7609775714691667, - "velocityY": -1.8225675093785412, - "timestamp": 8.97748934795063 - }, - { - "x": 4.282012195193535, - "y": 4.930397905958738, - "heading": -4.1602494438105564e-17, - "angularVelocity": 1.582195542444361e-17, - "velocityX": 3.7248687742898503, - "velocityY": -1.9370313765192848, - "timestamp": 9.008232210782408 - }, - { - "x": 4.3952613097643365, - "y": 4.867381383869913, - "heading": -4.188915157617957e-17, - "angularVelocity": -9.32434756133803e-18, - "velocityX": 3.6837530450723373, - "velocityY": -2.049793554804778, - "timestamp": 9.038975073614186 - }, - { - "x": 4.506961712126711, - "y": 4.801015720721852, - "heading": -4.2434270081658926e-17, - "angularVelocity": -1.7731546618224465e-17, - "velocityX": 3.6333767279120854, - "velocityY": -2.158733996609517, - "timestamp": 9.069717936445963 - }, - { - "x": 4.616614691776683, - "y": 4.73158032272052, - "heading": -4.19288728234691e-17, - "angularVelocity": 1.6439498850683494e-17, - "velocityX": 3.5667784178065634, - "velocityY": -2.258585948266306, - "timestamp": 9.10046079927774 - }, - { - "x": 4.722598987733627, - "y": 4.661750165167655, - "heading": -3.986850310029329e-17, - "angularVelocity": 6.701944885386403e-17, - "velocityX": 3.4474439331457423, - "velocityY": -2.271426637622177, - "timestamp": 9.131203662109519 - }, - { - "x": 4.827229210309334, - "y": 4.595352451522488, - "heading": -3.770620807844877e-17, - "angularVelocity": 7.033486223050351e-17, - "velocityX": 3.4033988034307483, - "velocityY": -2.1597765311737476, - "timestamp": 9.161946524941296 - }, - { - "x": 4.9314726269952285, - "y": 4.532624292130925, - "heading": -3.5259240937147126e-17, - "angularVelocity": 7.959464135435417e-17, - "velocityX": 3.390816829789258, - "velocityY": -2.04041372902732, - "timestamp": 9.192689387773074 - }, - { - "x": 5.035668651391688, - "y": 4.473585715832336, - "heading": -3.400468055587437e-17, - "angularVelocity": 4.080818328915737e-17, - "velocityX": 3.389275259321544, - "velocityY": -1.9203994312970292, - "timestamp": 9.223432250604851 - }, - { - "x": 5.139985480442779, - "y": 4.418235050432876, - "heading": -3.272140280940892e-17, - "angularVelocity": 4.1742298155096073e-17, - "velocityX": 3.3932047780294075, - "velocityY": -1.8004395264792032, - "timestamp": 9.254175113436629 - }, - { - "x": 5.244522953443759, - "y": 4.366567672079215, - "heading": -3.123998369139628e-17, - "angularVelocity": 4.8187415925402766e-17, - "velocityX": 3.400381856855714, - "velocityY": -1.680630025784517, - "timestamp": 9.284917976268407 - }, - { - "x": 5.3493470515644415, - "y": 4.31857903504105, - "heading": -3.003359756723782e-17, - "angularVelocity": 3.924117707448193e-17, - "velocityX": 3.409705162927411, - "velocityY": -1.5609683880370886, - "timestamp": 9.315660839100184 - }, - { - "x": 5.454504582553393, - "y": 4.274265192865899, - "heading": -2.87901792623276e-17, - "angularVelocity": 4.044575522175447e-17, - "velocityX": 3.4205510255946114, - "velocityY": -1.4414351200027105, - "timestamp": 9.346403701931962 - }, - { - "x": 5.560030462239174, - "y": 4.233622810392966, - "heading": -2.7523978224966667e-17, - "angularVelocity": 4.118682909556349e-17, - "velocityX": 3.432532625969479, - "velocityY": -1.3220103376619752, - "timestamp": 9.37714656476374 + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0.021560744143059335, + "angularVelocity": -5.815888661825855e-7, + "velocityX": 3.7545129582438856, + "velocityY": -0.45477354587499197, + "timestamp": 9.996504828034476 + }, + { + "x": 5.947788996667301, + "y": 6.560838784941361, + "heading": 0.021560710996949464, + "angularVelocity": -6.111012841975544e-7, + "velocityX": 3.718722301384556, + "velocityY": -0.6886875092486842, + "timestamp": 10.050744789115704 + }, + { + "x": 6.146761517861249, + "y": 6.510943609310236, + "heading": 0.021560678630883793, + "angularVelocity": -5.967199279900907e-7, + "velocityX": 3.6683750730568514, + "velocityY": -0.9198969659363234, + "timestamp": 10.104984750196932 + }, + { + "x": 6.342740574859562, + "y": 6.4503482133201375, + "heading": 0.021560646512747805, + "angularVelocity": -5.921489497600962e-7, + "velocityX": 3.6131857968117713, + "velocityY": -1.1171725565834012, + "timestamp": 10.15922471127816 + }, + { + "x": 6.53435736566076, + "y": 6.392349027665542, + "heading": 0.005697481882948994, + "angularVelocity": -0.2924626845886352, + "velocityX": 3.532760477358042, + "velocityY": -1.0693072874395997, + "timestamp": 10.213464672359388 + }, + { + "x": 6.715860798361231, + "y": 6.336732624061585, + "heading": -0.044321515304948156, + "angularVelocity": -0.9221798133850251, + "velocityX": 3.3463046263742644, + "velocityY": -1.0253769083769828, + "timestamp": 10.267704633440616 + }, + { + "x": 6.885664142970145, + "y": 6.284563070375092, + "heading": -0.11100118706044729, + "angularVelocity": -1.2293458628342813, + "velocityX": 3.1305948828876353, + "velocityY": -0.9618287448319874, + "timestamp": 10.321944594521844 + }, + { + "x": 7.043230365082517, + "y": 6.236112426000239, + "heading": -0.18201634611185183, + "angularVelocity": -1.309277470628262, + "velocityX": 2.9049840555085917, + "velocityY": -0.8932647334000697, + "timestamp": 10.376184555603071 + }, + { + "x": 7.188521106986847, + "y": 6.19141127344733, + "heading": -0.25302239943673055, + "angularVelocity": -1.3091095920689053, + "velocityX": 2.678666042675613, + "velocityY": -0.8241368847217142, + "timestamp": 10.4304245166843 + }, + { + "x": 7.3215520348116, + "y": 6.150465382048361, + "heading": -0.3216933073087357, + "angularVelocity": -1.2660574694949878, + "velocityX": 2.4526368598519452, + "velocityY": -0.7549026692266663, + "timestamp": 10.484664477765527 + }, + { + "x": 7.4423459735084, + "y": 6.113273952325478, + "heading": -0.38654982711023245, + "angularVelocity": -1.1957331551984454, + "velocityX": 2.2270284913351, + "velocityY": -0.685683193378148, + "timestamp": 10.538904438846755 + }, + { + "x": 7.550924553999962, + "y": 6.079834322504852, + "heading": -0.4465624539391904, + "angularVelocity": -1.1064282796789968, + "velocityX": 2.001818923301949, + "velocityY": -0.6165127915661526, + "timestamp": 10.593144399927983 + }, + { + "x": 7.647306628463083, + "y": 6.050143536065283, + "heading": -0.5009752926426013, + "angularVelocity": -1.0031872740823848, + "velocityX": 1.7769569251493995, + "velocityY": -0.5473968979274295, + "timestamp": 10.64738436100921 + }, + { + "x": 7.7315082292215935, + "y": 6.024198841891014, + "heading": -0.5492132046279071, + "angularVelocity": -0.8893426732564748, + "velocityX": 1.552390508400531, + "velocityY": -0.47833172548586056, + "timestamp": 10.701624322090439 + }, + { + "x": 7.803542912083633, + "y": 6.001997808520636, + "heading": -0.5908266102991006, + "angularVelocity": -0.7672093571172569, + "velocityX": 1.3280740145473924, + "velocityY": -0.4093113809047783, + "timestamp": 10.755864283171666 + }, + { + "x": 7.863422173267542, + "y": 5.983538316289689, + "heading": -0.6254559233807054, + "angularVelocity": -0.6384464957440807, + "velocityX": 1.103969471774435, + "velocityY": -0.34033011571123517, + "timestamp": 10.810104244252894 + }, + { + "x": 7.9111558450934325, + "y": 5.968818528744131, + "heading": -0.6528074420175133, + "angularVelocity": -0.5042687732730391, + "velocityX": 0.8800462034699242, + "velocityY": -0.2713827084704847, + "timestamp": 10.864344205334122 + }, + { + "x": 7.946752441580289, + "y": 5.95783687514921, + "heading": -0.6726364642205706, + "angularVelocity": -0.36557958021692255, + "velocityX": 0.6562799046545997, + "velocityY": -0.2024642602245795, + "timestamp": 10.91858416641535 + }, + { + "x": 7.970219449253945, + "y": 5.950592046499088, + "heading": -0.6847351436740425, + "angularVelocity": -0.22305840956179235, + "velocityX": 0.43265163185705313, + "velocityY": -0.1335699455844713, + "timestamp": 10.972824127496578 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -2.6172162518713254e-17, - "angularVelocity": 4.3971692345317756e-17, - "velocityX": 3.4453937215027324, - "velocityY": -1.2026770584391975, - "timestamp": 9.407889427595517 - }, - { - "x": 5.792835589324764, - "y": 4.157776846380009, - "heading": -2.4556205828219047e-17, - "angularVelocity": 4.408549337244093e-17, - "velocityX": 3.461564053183983, - "velocityY": -1.060487182383484, - "timestamp": 9.444544494078327 - }, - { - "x": 5.920107057755266, - "y": 4.124135852213366, - "heading": -2.3047234390121686e-17, - "angularVelocity": 4.1166790375489185e-17, - "velocityX": 3.472138523884214, - "velocityY": -0.917772013383704, - "timestamp": 9.481199560561137 - }, - { - "x": 6.047516391811881, - "y": 4.095738620024666, - "heading": -2.1456366912742873e-17, - "angularVelocity": 4.3401025561499594e-17, - "velocityX": 3.475899685419091, - "velocityY": -0.7747150643422807, - "timestamp": 9.517854627043947 - }, - { - "x": 6.174755293992793, - "y": 4.0725841920992965, - "heading": -1.9852024700254233e-17, - "angularVelocity": 4.3768634637209445e-17, - "velocityX": 3.4712500723626603, - "velocityY": -0.631684243056213, - "timestamp": 9.554509693526757 - }, - { - "x": 6.30143822417369, - "y": 4.054645790921562, - "heading": -1.7231399343635e-17, - "angularVelocity": 7.149421916471826e-17, - "velocityX": 3.4560824010592928, - "velocityY": -0.48938394876864877, - "timestamp": 9.591164760009567 - }, - { - "x": 6.427078572160238, - "y": 4.041848309685929, - "heading": -1.4890621747726543e-17, - "angularVelocity": 6.385959215232344e-17, - "velocityX": 3.427639342721413, - "velocityY": -0.3491326701490028, - "timestamp": 9.627819826492377 - }, - { - "x": 6.551062514369357, - "y": 4.034028011406828, - "heading": -1.2144086622031307e-17, - "angularVelocity": 7.492920868069778e-17, - "velocityX": 3.382450343317836, - "velocityY": -0.21334835888971543, - "timestamp": 9.664474892975187 - }, - { - "x": 6.672633087813251, - "y": 4.030865145435154, - "heading": -9.61251747475391e-18, - "angularVelocity": 6.906464481423604e-17, - "velocityX": 3.316610365470415, - "velocityY": -0.08628727963586283, - "timestamp": 9.701129959457997 - }, - { - "x": 6.790917094390862, - "y": 4.031790616181229, - "heading": -6.811165774297211e-18, - "angularVelocity": 7.642467929421218e-17, - "velocityX": 3.2269483574141518, - "velocityY": 0.025248098963559784, - "timestamp": 9.737785025940807 - }, - { - "x": 6.905041403639001, - "y": 4.0359118549290525, - "heading": -4.3348375779027234e-18, - "angularVelocity": 6.755759664372386e-17, - "velocityX": 3.113466164402112, - "velocityY": 0.11243299066875824, - "timestamp": 9.774440092423617 - }, - { - "x": 7.014327663163953, - "y": 4.042060231899537, - "heading": -2.4762499879984938e-18, - "angularVelocity": 5.070479385914931e-17, - "velocityX": 2.9814775967247074, - "velocityY": 0.16773607472154842, - "timestamp": 9.811095158906427 - }, - { - "x": 7.1184276572510345, - "y": 4.048995163672738, - "heading": -1.0562416558517327e-18, - "angularVelocity": 3.873975601197627e-17, - "velocityX": 2.8399892313904544, - "velocityY": 0.1891943580692654, - "timestamp": 9.847750225389237 + "x": 7.981563568115234, + "y": 5.947082996368408, + "heading": -0.6889234822214203, + "angularVelocity": -0.07721868644237204, + "velocityX": 0.2091468842372586, + "velocityY": -0.06469492346102504, + "timestamp": 11.027064088577806 + }, + { + "x": 7.980191051552582, + "y": 5.9474933088994915, + "heading": -0.684492260818683, + "angularVelocity": 0.07820368392725321, + "velocityX": -0.024222633376945003, + "velocityY": 0.007241333387762343, + "timestamp": 11.083726654587604 + }, + { + "x": 7.965589050123253, + "y": 5.951981018210289, + "heading": -0.6714197034271625, + "angularVelocity": 0.23070888440280377, + "velocityX": -0.257701026579075, + "velocityY": 0.07920060150509887, + "timestamp": 11.140389220597402 + }, + { + "x": 7.9377506970473615, + "y": 5.960547682425779, + "heading": -0.6498948487141165, + "angularVelocity": 0.37987786697348064, + "velocityX": -0.4913006070194107, + "velocityY": 0.15118736793544205, + "timestamp": 11.1970517866072 + }, + { + "x": 7.896668195667252, + "y": 5.973195099193875, + "heading": -0.6201381150730912, + "angularVelocity": 0.5251568316881386, + "velocityX": -0.7250377854932653, + "velocityY": 0.22320585986009217, + "timestamp": 11.253714352617 + }, + { + "x": 7.842332542279827, + "y": 5.989925310225715, + "heading": -0.5824114694602323, + "angularVelocity": 0.6658125155563156, + "velocityX": -0.9589338643440537, + "velocityY": 0.2952603845887764, + "timestamp": 11.310376918626798 + }, + { + "x": 7.774733202893158, + "y": 6.010740641182104, + "heading": -0.5370321516926094, + "angularVelocity": 0.8008694445602113, + "velocityX": -1.1930158506231556, + "velocityY": 0.367355953360645, + "timestamp": 11.367039484636596 + }, + { + "x": 7.693857750948662, + "y": 6.035643782735694, + "heading": -0.48439126574422153, + "angularVelocity": 0.9290240392446375, + "velocityX": -1.4273171449826625, + "velocityY": 0.43949900802735137, + "timestamp": 11.423702050646394 + }, + { + "x": 7.5996914832698845, + "y": 6.064637893376439, + "heading": -0.4249792327859352, + "angularVelocity": 1.0485235163549331, + "velocityX": -1.6618779259395846, + "velocityY": 0.5116978047858136, + "timestamp": 11.480364616656193 + }, + { + "x": 7.492217045643208, + "y": 6.097726656744903, + "heading": -0.35942190954654185, + "angularVelocity": 1.156977663667012, + "velocityX": -1.8967449798883769, + "velocityY": 0.583961611670435, + "timestamp": 11.537027182665991 + }, + { + "x": 7.371414140499637, + "y": 6.1349141479804095, + "heading": -0.28853586070822274, + "angularVelocity": 1.251020803153572, + "velocityX": -2.131970252153457, + "velocityY": 0.6562973379828368, + "timestamp": 11.59368974867579 + }, + { + "x": 7.237259574639344, + "y": 6.176204237589326, + "heading": -0.21342323245219458, + "angularVelocity": 1.3256128965822067, + "velocityX": -2.367604845800615, + "velocityY": 0.7287013722918435, + "timestamp": 11.650352314685588 + }, + { + "x": 7.0897287772168305, + "y": 6.221599002117526, + "heading": -0.13565860715800612, + "angularVelocity": 1.372416231215901, + "velocityX": -2.6036730739833454, + "velocityY": 0.8011420541800114, + "timestamp": 11.707014880695386 + }, + { + "x": 6.928804270313556, + "y": 6.271094657477637, + "heading": -0.05771767479301098, + "angularVelocity": 1.3755277576295868, + "velocityX": -2.8400497583439677, + "velocityY": 0.8735159532230397, + "timestamp": 11.763677446705184 + }, + { + "x": 6.754523159333334, + "y": 6.324666148768191, + "heading": 0.015819291394400217, + "angularVelocity": 1.297805083071882, + "velocityX": -3.0757715940730104, + "velocityY": 0.9454476749480591, + "timestamp": 11.820340012714983 + }, + { + "x": 6.56735113931042, + "y": 6.382062750762148, + "heading": 0.07327050352161106, + "angularVelocity": 1.0139182916156146, + "velocityX": -3.3032746873932677, + "velocityY": 1.012954513638379, + "timestamp": 11.877002578724781 + }, + { + "x": 6.368937324018353, + "y": 6.441765360809047, + "heading": 0.09523150855737006, + "angularVelocity": 0.38757519438780325, + "velocityX": -3.5016736668395723, + "velocityY": 1.053651718430401, + "timestamp": 11.93366514473458 + }, + { + "x": 6.164194285631387, + "y": 6.50503273811371, + "heading": 0.09523154205905172, + "angularVelocity": 5.912489325251288e-7, + "velocityX": -3.6133739222393086, + "velocityY": 1.1165639285330458, + "timestamp": 11.990327710744378 + }, + { + "x": 5.956663061240617, + "y": 6.558448687512427, + "heading": 0.09523157576982937, + "angularVelocity": 5.949391283503271e-7, + "velocityX": -3.6625807654896136, + "velocityY": 0.9427026193886323, + "timestamp": 12.046990276754176 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 3.646325715989205e-37, - "angularVelocity": 2.881570700048058e-17, - "velocityX": 2.697135958773559, - "velocityY": 0.18068777649551113, - "timestamp": 9.884405291872048 - }, - { - "x": 7.386231209731221, - "y": 4.063934540767167, - "heading": 6.43663529397786e-19, - "angularVelocity": 9.269678756083754e-18, - "velocityX": 2.432976407238177, - "velocityY": 0.11976600412078256, - "timestamp": 9.95384281460986 - }, - { - "x": 7.536591060669406, - "y": 4.069229370335429, - "heading": -1.8193245120818273e-19, - "angularVelocity": -1.1889767204445143e-17, - "velocityX": 2.1653976842740037, - "velocityY": 0.07625314613043933, - "timestamp": 10.023280337347673 - }, - { - "x": 7.668251510768197, - "y": 4.072361333543041, - "heading": -2.313295063693691e-18, - "angularVelocity": -3.069468103770411e-17, - "velocityX": 1.8960994705401897, - "velocityY": 0.04510476589779892, - "timestamp": 10.092717860085486 - }, - { - "x": 7.78114979643099, - "y": 4.073967912827302, - "heading": -5.036275587489351e-18, - "angularVelocity": -3.9214828185580025e-17, - "velocityX": 1.6258973709226552, - "velocityY": 0.023137047822506123, - "timestamp": 10.162155382823299 - }, - { - "x": 7.875252481646468, - "y": 4.074539583547341, - "heading": -6.689419008155875e-18, - "angularVelocity": -2.380763822621624e-17, - "velocityX": 1.3552137447471682, - "velocityY": 0.008232878960810142, - "timestamp": 10.231592905561111 - }, - { - "x": 7.950542203811188, - "y": 4.074464670768735, - "heading": -5.15013780810139e-18, - "angularVelocity": 2.2167858808364967e-17, - "velocityX": 1.0842800721592976, - "velocityY": -0.001078851543840728, - "timestamp": 10.301030428298924 - }, - { - "x": 8.007010780474163, - "y": 4.074057877238601, - "heading": -4.959999016913383e-18, - "angularVelocity": 2.738271523682432e-18, - "velocityX": 0.8132285605319336, - "velocityY": -0.005858410756812446, - "timestamp": 10.370467951036737 - }, - { - "x": 8.044655422701718, - "y": 4.073579204097417, - "heading": -1.6663567301752781e-18, - "angularVelocity": 4.7433176715915545e-17, - "velocityX": 0.5421368842563122, - "velocityY": -0.006893580333953516, - "timestamp": 10.43990547377455 + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0.09523161032614703, + "angularVelocity": 6.098615032122225e-7, + "velocityX": -3.716341004321884, + "velocityY": 0.7014239545882616, + "timestamp": 12.103652842763974 + }, + { + "x": 5.5027055023001905, + "y": 6.62552340619542, + "heading": 0.09523164151085345, + "angularVelocity": 4.815615607402516e-7, + "velocityX": -3.7583333072466103, + "velocityY": 0.4220399473818025, + "timestamp": 12.16841030879256 + }, + { + "x": 5.257964269234122, + "y": 6.634609890080181, + "heading": 0.09523167208339899, + "angularVelocity": 4.721084288711453e-7, + "velocityX": -3.7793516033817274, + "velocityY": 0.140315618291052, + "timestamp": 12.233167774821144 + }, + { + "x": 5.013061040594942, + "y": 6.636410996869712, + "heading": 0.09523170256730593, + "angularVelocity": 4.70739650837411e-7, + "velocityX": -3.781853177069622, + "velocityY": 0.02781311406988273, + "timestamp": 12.297925240849729 + }, + { + "x": 4.7681578086493825, + "y": 6.6382116540225224, + "heading": 0.09523173305121174, + "angularVelocity": 4.7073963340734453e-7, + "velocityX": -3.781853228127505, + "velocityY": 0.027806170674068813, + "timestamp": 12.362682706878314 + }, + { + "x": 4.5232545767036525, + "y": 6.640012311152163, + "heading": 0.09523176353511749, + "angularVelocity": 4.707396321471799e-7, + "velocityX": -3.781853228130136, + "velocityY": 0.02780617031625758, + "timestamp": 12.427440172906898 + }, + { + "x": 4.2783513447579224, + "y": 6.641812968281802, + "heading": 0.09523179401902328, + "angularVelocity": 4.707396330675372e-7, + "velocityX": -3.781853228130136, + "velocityY": 0.027806170316239148, + "timestamp": 12.492197638935483 + }, + { + "x": 4.033448112812192, + "y": 6.64361362541144, + "heading": 0.09523182450292901, + "angularVelocity": 4.707396322379774e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.027806170316239207, + "timestamp": 12.556955104964068 + }, + { + "x": 3.788544880866463, + "y": 6.645414282541079, + "heading": 0.09523185498683476, + "angularVelocity": 4.7073963225441474e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.02780617031623923, + "timestamp": 12.621712570992653 + }, + { + "x": 3.5436416489207327, + "y": 6.647214939670717, + "heading": 0.09523188547074045, + "angularVelocity": 4.707396314544476e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.02780617031623925, + "timestamp": 12.686470037021238 + }, + { + "x": 3.2987384169750027, + "y": 6.649015596800356, + "heading": 0.09523191595464614, + "angularVelocity": 4.7073963141347726e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.027806170316239207, + "timestamp": 12.751227503049822 + }, + { + "x": 3.0538351850292726, + "y": 6.650816253929995, + "heading": 0.09523194643855185, + "angularVelocity": 4.7073963175583744e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.027806170316239227, + "timestamp": 12.815984969078407 + }, + { + "x": 2.8089319530835426, + "y": 6.652616911059632, + "heading": 0.09523197692245748, + "angularVelocity": 4.7073963058043316e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.02780617031621524, + "timestamp": 12.880742435106992 + }, + { + "x": 2.5640287211375905, + "y": 6.654417568159032, + "heading": 0.09523200740636317, + "angularVelocity": 4.707396315238404e-7, + "velocityX": -3.78185322813357, + "velocityY": 0.027806169849281307, + "timestamp": 12.945499901135577 + }, + { + "x": 2.319125484878767, + "y": 6.656217638494738, + "heading": 0.09523203789215427, + "angularVelocity": 4.707687462497284e-7, + "velocityX": -3.7818532947339327, + "velocityY": 0.02779710890650077, + "timestamp": 13.010257367164161 + }, + { + "x": 2.086765993499063, + "y": 6.65613072972364, + "heading": 0.1392662443558495, + "angularVelocity": 0.6799865585268291, + "velocityX": -3.588149840161109, + "velocityY": -0.0013420656555584189, + "timestamp": 13.075014833192746 + }, + { + "x": 1.8694746583337432, + "y": 6.65699037980207, + "heading": 0.2298866992043259, + "angularVelocity": 1.399382347797175, + "velocityX": -3.3554638328406146, + "velocityY": 0.013274918417153406, + "timestamp": 13.139772299221331 + }, + { + "x": 1.6701637818051993, + "y": 6.657989601285878, + "heading": 0.3342097566920158, + "angularVelocity": 1.6109811560823084, + "velocityX": -3.077805367501045, + "velocityY": 0.015430212840099346, + "timestamp": 13.204529765249916 + }, + { + "x": 1.4891465820964496, + "y": 6.659013009703433, + "heading": 0.4396201310941332, + "angularVelocity": 1.6277717592530314, + "velocityX": -2.7953101134137524, + "velocityY": 0.01580371315183694, + "timestamp": 13.2692872312785 + }, + { + "x": 1.326396709734208, + "y": 6.660011981353038, + "heading": 0.5409065147827995, + "angularVelocity": 1.5640881260541826, + "velocityX": -2.5132217540816426, + "velocityY": 0.015426354841678473, + "timestamp": 13.334044697307085 + }, + { + "x": 1.1818610470561817, + "y": 6.660956545539143, + "heading": 0.6351810352227076, + "angularVelocity": 1.4558092868904808, + "velocityX": -2.231953650166405, + "velocityY": 0.014586182011631477, + "timestamp": 13.39880216333567 + }, + { + "x": 1.0554921038308631, + "y": 6.661825694692396, + "heading": 0.7206099929025545, + "angularVelocity": 1.3192140291922123, + "velocityX": -1.9514189015601402, + "velocityY": 0.013421605361615121, + "timestamp": 13.463559629364255 + }, + { + "x": 0.9472506642705061, + "y": 6.66260279420219, + "heading": 0.7959103128896443, + "angularVelocity": 1.1628052270274385, + "velocityX": -1.6714897323588642, + "velocityY": 0.012000153147596032, + "timestamp": 13.52831709539284 + }, + { + "x": 0.8571042421570496, + "y": 6.663273874000479, + "heading": 0.8601164354198797, + "angularVelocity": 0.9914860242044418, + "velocityX": -1.3920622229669246, + "velocityY": 0.010362971861702049, + "timestamp": 13.593074561421425 + }, + { + "x": 0.7850255723133412, + "y": 6.6638271232273425, + "heading": 0.912467741181329, + "angularVelocity": 0.8084211593199297, + "velocityX": -1.1130557488443436, + "velocityY": 0.00854340450286573, + "timestamp": 13.65783202745001 + }, + { + "x": 0.7309916972965728, + "y": 6.664252697034555, + "heading": 0.952354006476902, + "angularVelocity": 0.6159330767817102, + "velocityX": -0.8344037889456237, + "velocityY": 0.0065718106855080505, + "timestamp": 13.722589493478594 + }, + { + "x": 0.6949834963208631, + "y": 6.664542513080523, + "heading": 0.9792885970471746, + "angularVelocity": 0.41593027371366526, + "velocityX": -0.556047096713378, + "velocityY": 0.0044754074509330615, + "timestamp": 13.787346959507179 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 4.9523840537519984e-39, - "angularVelocity": 2.3997928849898842e-17, - "velocityX": 0.27105142948932054, - "velocityY": -0.004784851370482414, - "timestamp": 10.509342996512363 + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.21010218588179536, + "velocityX": -0.2779301721676604, + "velocityY": 0.0022778009813890303, + "timestamp": 13.852104425535764 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": -7.535317609356433e-39, - "angularVelocity": -1.798408291474874e-37, - "velocityX": 0, - "velocityY": 1.827412307849029e-40, - "timestamp": 10.578780519250175 + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": -1.2401951851203077e-33, + "velocityX": -4.2513783914380976e-39, + "velocityY": -1.4668246371137104e-32, + "timestamp": 13.916861891564348 } ], "constraints": [ @@ -16488,13 +11118,7 @@ }, { "scope": [ - 4 - ], - "type": "StopPoint" - }, - { - "scope": [ - 6 + 5 ], "type": "StopPoint" } @@ -16504,7 +11128,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "CenterLanePSubCSubBSubASubFSub": { + "CenterLanePCBAFSprint": { "waypoints": [ { "x": 1.2899744510650635, @@ -16513,7 +11137,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 17 }, { "x": 2.6651716232299805, @@ -16522,48 +11146,39 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 16 }, { - "x": 1.2899744510650635, - "y": 5.590954303741455, + "x": 1.8129411935806274, + "y": 5.542999267578125, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 10 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 - }, - { - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 14 }, { - "x": 2.7351951599121094, - "y": 6.870687484741211, - "heading": 0.8050032485420815, + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 8 }, { - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -16576,7 +11191,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 13 }, { "x": 5.665951728820801, @@ -16585,16 +11200,16 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 12 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, + "x": 7.196889877319336, + "y": 3.9978766441345215, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 9 }, { "x": 8.0634765625, @@ -16603,7 +11218,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 15 }, { "x": 5.665951728820801, @@ -16612,7 +11227,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 13 }, { "x": 4.16749906539917, @@ -16621,11 +11236,11 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 18 + "controlIntervalCount": 13 }, { - "x": 1.2899744510650635, - "y": 5.590954303741455, + "x": 2.6583051681518555, + "y": 5.54433536529541, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -16633,8 +11248,26 @@ "controlIntervalCount": 13 }, { - "x": 2.462606906890869, - "y": 6.522137641906738, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -16647,1936 +11280,1801 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": -1.960896274672709e-29, - "velocityX": 0, - "velocityY": 0, + "angularVelocity": -8.928261679081423e-33, + "velocityX": -3.922322521208882e-33, + "velocityY": -1.0855099176840522e-33, "timestamp": 0 }, { - "x": 1.3069454293752283, - "y": 5.5749747243722725, - "heading": -0.008996462194644695, - "angularVelocity": -0.11617133047486594, - "velocityX": 0.21914626962200023, - "velocityY": -0.20634433353719148, - "timestamp": 0.07744132875022905 - }, - { - "x": 1.340887418869277, - "y": 5.543015778149598, - "heading": -0.02698653550593814, - "angularVelocity": -0.23230584497508605, - "velocityX": 0.4382929637423898, - "velocityY": -0.4126859228589361, - "timestamp": 0.1548826575004581 - }, - { - "x": 1.391800616271645, - "y": 5.495077701168919, - "heading": -0.05395916302114244, - "angularVelocity": -0.34829758154372603, - "velocityX": 0.6574421981657251, - "velocityY": -0.6190244634792922, - "timestamp": 0.23232398625068715 - }, - { - "x": 1.459685380828832, - "y": 5.431160783444383, - "heading": -0.08989644453732974, - "angularVelocity": -0.4640581727637966, - "velocityX": 0.8765960715396762, - "velocityY": -0.8253592591464762, - "timestamp": 0.3097653150009162 - }, - { - "x": 1.5445422260314938, - "y": 5.351265403464515, - "heading": -0.13477553337937057, - "angularVelocity": -0.579523744831976, - "velocityX": 1.0957565756192689, - "velocityY": -1.0316891673907769, - "timestamp": 0.3872066437511452 - }, - { - "x": 1.6463718016735291, - "y": 5.255392061024326, - "heading": -0.18857099282512366, - "angularVelocity": -0.6946608524671277, - "velocityX": 1.3149254704884978, - "velocityY": -1.2380126217812786, - "timestamp": 0.46464797250137424 - }, - { - "x": 1.7651748655395048, - "y": 5.143541401111082, - "heading": -0.25125740651293493, - "angularVelocity": -0.8094697585842596, - "velocityX": 1.5341041506310307, - "velocityY": -1.4443277474502212, - "timestamp": 0.5420893012516033 - }, - { - "x": 1.9009522484980113, - "y": 5.015714222493906, - "heading": -0.3228118649000377, - "angularVelocity": -0.9239828337396184, - "velocityX": 1.7532935597809811, - "velocityY": -1.6506325586122341, - "timestamp": 0.6195306300018323 - }, - { - "x": 2.053704829775847, - "y": 4.871911475725846, - "heading": -0.4032156639032303, - "angularVelocity": -1.0382543830392266, - "velocityX": 1.9724943223879574, - "velocityY": -1.8569250952812175, - "timestamp": 0.6969719587520613 - }, - { - "x": 2.1895539905378896, - "y": 4.744100163466478, - "heading": -0.47392252589131595, - "angularVelocity": -0.9130378201026126, - "velocityX": 1.7542204266665722, - "velocityY": -1.6504276763046646, - "timestamp": 0.7744132875022903 - }, - { - "x": 2.3084268719206142, - "y": 4.632263014410205, - "heading": -0.535794025352391, - "angularVelocity": -0.7989467698895206, - "velocityX": 1.5350057043302445, - "velocityY": -1.4441532817287048, - "timestamp": 0.8518546162525193 - }, - { - "x": 2.410322968991003, - "y": 4.536399460392939, - "heading": -0.5888342696498629, - "angularVelocity": -0.6849087580665111, - "velocityX": 1.3157844617930656, - "velocityY": -1.237886223856486, - "timestamp": 0.9292959450027484 - }, - { - "x": 2.495241902358175, - "y": 4.456509045521313, - "heading": -0.6330449707790676, - "angularVelocity": -0.5708928532419475, - "velocityX": 1.096558320180854, - "velocityY": -1.0316250529360251, - "timestamp": 1.0067372737529774 - }, - { - "x": 2.5631833814386416, - "y": 4.392591413105038, - "heading": -0.6684266086422785, - "angularVelocity": -0.4568831453961845, - "velocityX": 0.8773284262635802, - "velocityY": -0.8253684879578497, - "timestamp": 1.0841786025032065 - }, - { - "x": 2.6141471938399374, - "y": 4.34464629797767, - "heading": -0.694979006837532, - "angularVelocity": -0.34287115967158255, - "velocityX": 0.6580957897220853, - "velocityY": -0.6191153470743593, - "timestamp": 1.1616199312534357 - }, - { - "x": 2.648133201868436, - "y": 4.3126735215310195, - "heading": -0.7127016614525554, - "angularVelocity": -0.22885266692700632, - "velocityX": 0.4388613751425609, - "velocityY": -0.4128645125587087, - "timestamp": 1.2390612600036648 - }, - { - "x": 2.6651413416897705, - "y": 4.296672988888067, - "heading": -0.7215939290593514, - "angularVelocity": -0.11482586559568532, - "velocityX": 0.21962613626976676, - "velocityY": -0.20661490319016262, - "timestamp": 1.316502588753894 + "x": 1.306057392567738, + "y": 5.572382489557579, + "heading": -0.01069490392439116, + "angularVelocity": -0.14212387666683293, + "velocityX": 0.21372515459002697, + "velocityY": -0.2467996203807809, + "timestamp": 0.07525057840535954 + }, + { + "x": 1.33822347033388, + "y": 5.535239164732594, + "heading": -0.0320761792409761, + "angularVelocity": -0.28413436507302803, + "velocityX": 0.4274528973434579, + "velocityY": -0.49359520700160897, + "timestamp": 0.15050115681071907 + }, + { + "x": 1.3864731960688501, + "y": 5.4795246417936365, + "heading": -0.06412173122646007, + "angularVelocity": -0.42585123815077036, + "velocityX": 0.6411874401158554, + "velocityY": -0.7403866404698544, + "timestamp": 0.2257517352160786 + }, + { + "x": 1.4508074313630048, + "y": 5.405239333647035, + "heading": -0.1067984096254206, + "angularVelocity": -0.567127579658856, + "velocityX": 0.8549334325059841, + "velocityY": -0.9871725868529632, + "timestamp": 0.30100231362143814 + }, + { + "x": 1.5312274241077561, + "y": 5.312383875814834, + "heading": -0.16006570142282744, + "angularVelocity": -0.707865546367854, + "velocityX": 1.0686960080432237, + "velocityY": -1.233950087825352, + "timestamp": 0.3762528920267977 + }, + { + "x": 1.6277348707066417, + "y": 5.200959293585713, + "heading": -0.2238805012390217, + "angularVelocity": -0.848030688514272, + "velocityX": 1.2824811269757883, + "velocityY": -1.4807139638036912, + "timestamp": 0.4515034704321572 + }, + { + "x": 1.7403321345867286, + "y": 5.070967309799712, + "heading": -0.29820239377845176, + "angularVelocity": -0.9876587544493436, + "velocityX": 1.4962976533356125, + "velocityY": -1.727454945073796, + "timestamp": 0.5267540488375168 + }, + { + "x": 1.8690235919905007, + "y": 4.922411591690569, + "heading": -0.3829978316122351, + "angularVelocity": -1.126841010802703, + "velocityX": 1.7101723352947207, + "velocityY": -1.9741471927152945, + "timestamp": 0.6020046272428763 + }, + { + "x": 2.021840076397225, + "y": 4.778612689187088, + "heading": -0.46243678907347296, + "angularVelocity": -1.055659094516408, + "velocityX": 2.030768236538101, + "velocityY": -1.9109341821781907, + "timestamp": 0.6772552056482358 + }, + { + "x": 2.158571900473102, + "y": 4.653385130963448, + "heading": -0.5314200615226269, + "angularVelocity": -0.916714182282495, + "velocityX": 1.8170202405532427, + "velocityY": -1.6641408063212961, + "timestamp": 0.7525057840535954 + }, + { + "x": 2.2792154502461437, + "y": 4.546725640046215, + "heading": -0.5899528620106769, + "angularVelocity": -0.7778385459410783, + "velocityX": 1.6032242187317085, + "velocityY": -1.4173909779494496, + "timestamp": 0.8277563624589549 + }, + { + "x": 2.383769251108642, + "y": 4.458632746099449, + "heading": -0.6380359031198586, + "angularVelocity": -0.6389723790582461, + "velocityX": 1.3894086009450728, + "velocityY": -1.1706606887754087, + "timestamp": 0.9030069408643144 + }, + { + "x": 2.472232460585404, + "y": 4.389105547578634, + "heading": -0.6756681828246474, + "angularVelocity": -0.5000928963239504, + "velocityX": 1.175581787560874, + "velocityY": -0.9239423801673171, + "timestamp": 0.978257519269674 + }, + { + "x": 2.544604550352729, + "y": 4.338143448939913, + "heading": -0.7028480663779157, + "angularVelocity": -0.3611916895423175, + "velocityX": 0.9617479533176606, + "velocityY": -0.677231985702458, + "timestamp": 1.0535080976750335 + }, + { + "x": 2.6008851962155197, + "y": 4.305746063716667, + "heading": -0.7195737337551226, + "angularVelocity": -0.22226629657394859, + "velocityX": 0.7479098108670842, + "velocityY": -0.4305267269671826, + "timestamp": 1.128758676080393 + }, + { + "x": 2.641074231196452, + "y": 4.291913169677742, + "heading": -0.7258433495685629, + "angularVelocity": -0.08331651325877153, + "velocityX": 0.5340694494657862, + "velocityY": -0.18382442144709177, + "timestamp": 1.2040092544857526 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": -0.0007900953867185666, - "velocityX": 0.00039102557193096025, - "velocityY": -0.00036545389408097024, - "timestamp": 1.393943917504123 - }, - { - "x": 2.648217264320523, - "y": 4.31259514704442, - "heading": -0.7128809139831602, - "angularVelocity": 0.1132783500941083, - "velocityX": -0.21888736985187585, - "velocityY": 0.2059266364981681, - "timestamp": 1.471400917058759 - }, - { - "x": 2.6142783804156173, - "y": 4.344524561384219, - "heading": -0.6952725965601283, - "angularVelocity": 0.2273302286961744, - "velocityX": -0.43816419561488745, - "velocityY": 0.4122211617168144, - "timestamp": 1.548857916613395 - }, - { - "x": 2.5633551628668956, - "y": 4.3924332087312665, - "heading": -0.6688313471237388, - "angularVelocity": 0.341366817566861, - "velocityX": -0.6574385509538746, - "velocityY": 0.618519276788985, - "timestamp": 1.6263149161680308 - }, - { - "x": 2.4954478733663135, - "y": 4.456321462374885, - "heading": -0.6335580214350119, - "angularVelocity": 0.45539235822983737, - "velocityX": -0.876709527747899, - "velocityY": 0.8248222111715229, - "timestamp": 1.7037719157226667 - }, - { - "x": 2.41055684840719, - "y": 4.536189798526633, - "heading": -0.5894527108517198, - "angularVelocity": 0.5694167194284366, - "velocityX": -1.0959761602822577, - "velocityY": 1.0311312936305541, - "timestamp": 1.7812289152773026 - }, - { - "x": 2.3086825106199456, - "y": 4.632038804702452, - "heading": -0.5365139521148367, - "angularVelocity": 0.6834599718703669, - "velocityX": -1.3152373364934584, - "velocityY": 1.2374479611490161, - "timestamp": 1.8586859148319386 - }, - { - "x": 2.1898253903863423, - "y": 4.743869185084351, - "heading": -0.47473734314041516, - "angularVelocity": 0.7975600569287656, - "velocityX": -1.5344916652697551, - "velocityY": 1.4437737199311238, - "timestamp": 1.9361429143865745 - }, - { - "x": 2.053986160891222, - "y": 4.871681755197305, - "heading": -0.40411323356241463, - "angularVelocity": 0.91178473196739, - "velocityX": -1.7537373029678356, - "velocityY": 1.6501100074580153, - "timestamp": 2.0135999139412104 - }, - { - "x": 1.901165693105442, - "y": 5.0154774121652865, - "heading": -0.3246230604276491, - "angularVelocity": 1.026249061953906, - "velocityX": -1.9729716960932775, - "velocityY": 1.8564578772043625, - "timestamp": 2.0910569134958465 - }, - { - "x": 1.7653423329783178, - "y": 5.143359507076652, - "heading": -0.25263854799609164, - "angularVelocity": 0.9293480620925062, - "velocityX": -1.7535324232572806, - "velocityY": 1.6510075996592735, - "timestamp": 2.1685139130504827 - }, - { - "x": 1.6464985778710517, - "y": 5.2552571877344505, - "heading": -0.18957308544395354, - "angularVelocity": 0.8141996580657658, - "velocityX": -1.5343191162933176, - "velocityY": 1.4446425926813151, - "timestamp": 2.245970912605119 - }, - { - "x": 1.5446336589350755, - "y": 5.351170031650349, - "heading": -0.135458741257284, - "angularVelocity": 0.6986372374073464, - "velocityX": -1.315115735463508, - "velocityY": 1.2382721312145724, - "timestamp": 2.323427912159755 - }, - { - "x": 1.459746934416973, - "y": 5.431097753001726, - "heading": -0.09032536640283424, - "angularVelocity": 0.5826894291541963, - "velocityX": -1.0959206399199872, - "velocityY": 1.031897979666631, - "timestamp": 2.400884911714391 - }, - { - "x": 1.391837908901594, - "y": 5.49504016165852, - "heading": -0.054198235453019356, - "angularVelocity": 0.4664153163377203, - "velocityX": -0.8767319403787999, - "velocityY": 0.8255213734655752, - "timestamp": 2.4783419112690273 - }, - { - "x": 1.3409062442336714, - "y": 5.542997125969119, - "heading": -0.027096040975571573, - "angularVelocity": 0.3498998751992024, - "velocityX": -0.6575476065536807, - "velocityY": 0.6191430675910059, - "timestamp": 2.5557989108236634 - }, - { - "x": 1.3069517635064558, - "y": 5.574968540706391, - "heading": -0.0090294167092662, - "angularVelocity": 0.2332471483558819, - "velocityX": -0.43836555666351823, - "velocityY": 0.41276340319322796, - "timestamp": 2.6332559103782995 + "angularVelocity": 0.055657173156648594, + "velocityX": 0.320228661947563, + "velocityY": 0.06287683198072233, + "timestamp": 1.2792598328911122 + }, + { + "x": 2.6731094248331484, + "y": 4.3202884971859605, + "heading": -0.7067986870475855, + "angularVelocity": 0.19580509628602769, + "velocityX": 0.1046188223798005, + "velocityY": 0.3116212313704759, + "timestamp": 1.355133384347763 + }, + { + "x": 2.6646882165450285, + "y": 4.362805712309731, + "heading": -0.6813129128724049, + "angularVelocity": 0.3358979998417677, + "velocityX": -0.11099003706094854, + "velocityY": 0.5603693817873046, + "timestamp": 1.431006935804414 + }, + { + "x": 2.6399081500889086, + "y": 4.424196738034775, + "heading": -0.6452019314674031, + "angularVelocity": 0.47593635346874824, + "velocityX": -0.3265968968155938, + "velocityY": 0.8091228701758959, + "timestamp": 1.5068804872610648 + }, + { + "x": 2.5987694540060504, + "y": 4.504462102027486, + "heading": -0.5984691810773222, + "angularVelocity": 0.6159293916376241, + "velocityX": -0.542200744436769, + "velocityY": 1.0578833131143173, + "timestamp": 1.5827540387177157 + }, + { + "x": 2.5412724297463463, + "y": 4.603602434972891, + "heading": -0.5411158632576627, + "angularVelocity": 0.7559065935173384, + "velocityX": -0.757800619001658, + "velocityY": 1.3066520683699112, + "timestamp": 1.6586275901743666 + }, + { + "x": 2.4674174122154677, + "y": 4.7216183838541035, + "heading": -0.4731379535005828, + "angularVelocity": 0.8959368376992063, + "velocityX": -0.9733960795689668, + "velocityY": 1.5554293507486547, + "timestamp": 1.7345011416310174 + }, + { + "x": 2.37720454589188, + "y": 4.858510309507254, + "heading": -0.39452114170963426, + "angularVelocity": 1.0361556864234158, + "velocityX": -1.188989635935645, + "velocityY": 1.8042113888838065, + "timestamp": 1.8103746930876683 + }, + { + "x": 2.270632337422942, + "y": 5.01427685954172, + "heading": -0.305233667532112, + "angularVelocity": 1.1767931309836617, + "velocityX": -1.4046028744262238, + "velocityY": 2.0529756027495263, + "timestamp": 1.8862482445443192 + }, + { + "x": 2.156211319422429, + "y": 5.146461372898718, + "heading": -0.2291032339501207, + "angularVelocity": 1.003385661016371, + "velocityX": -1.508048797028907, + "velocityY": 1.7421685267035296, + "timestamp": 1.96212179600097 + }, + { + "x": 2.058134463277846, + "y": 5.259759383515319, + "heading": -0.16374337088202898, + "angularVelocity": 0.8614314449935028, + "velocityX": -1.2926356320702503, + "velocityY": 1.493247758164214, + "timestamp": 2.037995347457621 + }, + { + "x": 1.976403644982925, + "y": 5.354173277081842, + "heading": -0.10920518207341517, + "angularVelocity": 0.7188036906348487, + "velocityX": -1.0771977418457426, + "velocityY": 1.2443584326016996, + "timestamp": 2.113868898914272 + }, + { + "x": 1.9110189705231253, + "y": 5.429703862947542, + "heading": -0.06553401357556585, + "angularVelocity": 0.5755782833336871, + "velocityX": -0.861758454751598, + "velocityY": 0.9954797741193555, + "timestamp": 2.189742450370923 + }, + { + "x": 1.8619803278189464, + "y": 5.486351581956828, + "heading": -0.032764600292336035, + "angularVelocity": 0.43189507613798817, + "velocityX": -0.6463206448454762, + "velocityY": 0.7466069258884014, + "timestamp": 2.265616001827574 + }, + { + "x": 1.829287687812507, + "y": 5.524116686023993, + "heading": -0.010918035230124941, + "angularVelocity": 0.28793386684545474, + "velocityX": -0.43088321791708567, + "velocityY": 0.4977373978433378, + "timestamp": 2.3414895532842253 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 7.540408157375041e-27, - "angularVelocity": 0.11657328273999748, - "velocityX": -0.21918370888400043, - "velocityY": 0.20638242027186274, - "timestamp": 2.7107129099329357 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -6.884486248240525e-32, + "angularVelocity": 0.14389777492309389, + "velocityX": -0.21544390526149312, + "velocityY": 0.2488690879972792, + "timestamp": 2.4173631047408763 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 7.897631948719018e-27, - "angularVelocity": 4.6120160844672454e-27, - "velocityX": -1.8573844509297025e-30, - "velocityY": 4.869606321032096e-28, - "timestamp": 2.788169909487572 - }, - { - "x": 1.3279766485932036, - "y": 5.589659571345802, - "heading": -4.710153046711685e-19, - "angularVelocity": -4.772747206025981e-18, - "velocityX": 0.38507114894200895, - "velocityY": -0.01311934897441659, - "timestamp": 2.886858678128688 - }, - { - "x": 1.4039810428772954, - "y": 5.587070106580806, - "heading": -1.4132708274578583e-18, - "angularVelocity": -9.547773272073244e-18, - "velocityX": 0.7701422900595405, - "velocityY": -0.026238697682253745, - "timestamp": 2.985547446769804 - }, - { - "x": 1.5179876327590038, - "y": 5.5831859094859295, - "heading": 6.100569101954107e-18, - "angularVelocity": 7.613688998468879e-17, - "velocityX": 1.155213419439816, - "velocityY": -0.03935804599020335, - "timestamp": 3.0842362154109204 - }, - { - "x": 1.669996416307628, - "y": 5.578006980126951, - "heading": 1.3142262461800744e-17, - "angularVelocity": 7.135267989328175e-17, - "velocityX": 1.5402845292565597, - "velocityY": -0.0524773936316247, - "timestamp": 3.1829249840520366 - }, - { - "x": 1.8600073896612332, - "y": 5.571533318635448, - "heading": 1.9239838663580868e-17, - "angularVelocity": 6.178604539434395e-17, - "velocityX": 1.925355599940842, - "velocityY": -0.06559673993980564, - "timestamp": 3.281613752693153 - }, - { - "x": 2.0880205412297537, - "y": 5.563764925406294, - "heading": 2.4860743738508815e-17, - "angularVelocity": 5.695598852986624e-17, - "velocityX": 2.310426553184545, - "velocityY": -0.07871608224679369, - "timestamp": 3.380302521334269 - }, - { - "x": 2.2780734489522008, - "y": 5.557289835213575, - "heading": 1.9389925148359925e-17, - "angularVelocity": -5.54351783373119e-17, - "velocityX": 1.9257805152317913, - "velocityY": -0.06561121677610336, - "timestamp": 3.478991289975385 - }, - { - "x": 2.430124178434165, - "y": 5.55210947675938, - "heading": 1.4382200259206915e-17, - "angularVelocity": -5.074270981417207e-17, - "velocityX": 1.5407095617290139, - "velocityY": -0.05249187446028865, - "timestamp": 3.5776800586165014 - }, - { - "x": 2.544172718104756, - "y": 5.548223850437927, - "heading": 9.840962526531424e-18, - "angularVelocity": -4.6015845846918525e-17, - "velocityX": 1.155638490979962, - "velocityY": -0.03937252814990102, - "timestamp": 3.6763688272576176 - }, - { - "x": 2.6202190641055907, - "y": 5.545632956380672, - "heading": 6.0923440108598725e-18, - "angularVelocity": -3.798432272839268e-17, - "velocityX": 0.7705673811344316, - "velocityY": -0.026253180507498905, - "timestamp": 3.775057595898734 - }, - { - "x": 2.6582632145072105, - "y": 5.5443367946533515, - "heading": 2.8119102345957467e-18, - "angularVelocity": -3.324026254038647e-17, - "velocityX": 0.38549625173796304, - "velocityY": -0.013133832198997603, - "timestamp": 3.87374636453985 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -2.79027732569672e-32, + "angularVelocity": 5.39778462149558e-31, + "velocityX": -1.372141826224318e-33, + "velocityY": 4.2150961560958763e-32, + "timestamp": 2.4932366561975274 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 3.967855365558804e-35, - "angularVelocity": -2.8492764136925344e-17, - "velocityX": 0.0004251106100680654, - "velocityY": -0.000014483490807365214, - "timestamp": 3.972435133180966 - }, - { - "x": 2.6203309461495814, - "y": 5.545629144566718, - "heading": -2.3435066939433916e-18, - "angularVelocity": -2.3742025219812006e-17, - "velocityX": -0.3847168901074824, - "velocityY": 0.013107279398989128, - "timestamp": 4.0711420602145845 - }, - { - "x": 2.544340541557253, - "y": 5.548218132703827, - "heading": -4.218037035355937e-18, - "angularVelocity": -1.899082968522677e-17, - "velocityX": -0.769858883001995, - "velocityY": 0.02622904202225523, - "timestamp": 4.169848987248202 - }, - { - "x": 2.4303339555332055, - "y": 5.552102329667273, - "heading": -5.623520937192281e-18, - "angularVelocity": -1.4238932090618574e-17, - "velocityX": -1.1550008641614116, - "velocityY": 0.039350804245707374, - "timestamp": 4.26855591428182 - }, - { - "x": 2.27831119000814, - "y": 5.557281735391277, - "heading": -6.559843392694925e-18, - "angularVelocity": -9.485869363616628e-18, - "velocityX": -1.5401428257608962, - "velocityY": 0.052472565802753904, - "timestamp": 4.367262841315438 - }, - { - "x": 2.0882722488439907, - "y": 5.563756349744264, - "heading": -7.026773676733008e-18, - "angularVelocity": -4.730469162706724e-18, - "velocityX": -1.9252847482351219, - "velocityY": 0.06559432602680541, - "timestamp": 4.465969768349056 - }, - { - "x": 1.8602171436308217, - "y": 5.571526172331361, - "heading": -7.023625037813593e-18, - "angularVelocity": 3.1898506726958296e-20, - "velocityX": -2.3104265532903905, - "velocityY": 0.07871608225040072, - "timestamp": 4.564676695382674 - }, - { - "x": 1.6701362603825125, - "y": 5.578002215648425, - "heading": -4.6832619459775276e-18, - "angularVelocity": 2.3710179076002214e-17, - "velocityX": -1.9257096635619557, - "velocityY": 0.06560880286432572, - "timestamp": 4.6633836224162915 - }, - { - "x": 1.5180715412087855, - "y": 5.583183050730508, - "heading": -2.810229256311612e-18, - "angularVelocity": 1.8975666640147773e-17, - "velocityX": -1.5405678582477078, - "velocityY": 0.05248704663190702, - "timestamp": 4.762090549449909 - }, - { - "x": 1.4040229976805338, - "y": 5.587068677183391, - "heading": -1.405203585438858e-18, - "angularVelocity": 1.4234289424631298e-17, - "velocityX": -1.1554259357087366, - "velocityY": 0.03936528640564963, - "timestamp": 4.860797476483527 - }, - { - "x": 1.3279906336561405, - "y": 5.589659094875619, - "heading": -4.684238337733874e-19, - "angularVelocity": 9.490501893267791e-18, - "velocityX": -0.7702839740804757, - "velocityY": 0.02624352484762269, - "timestamp": 4.959504403517145 + "x": 1.8507557561824508, + "y": 5.5430527114919315, + "heading": -1.045695782000071e-18, + "angularVelocity": -1.1232079825061295e-17, + "velocityX": 0.4061756707874381, + "velocityY": 0.0005740544395097007, + "timestamp": 2.586335690239901 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 3.33815545782367e-31, - "angularVelocity": 4.745589981208042e-18, - "velocityX": -0.3851419929048723, - "velocityY": 0.013121762623619058, - "timestamp": 5.058211330550763 + "x": 1.9263848800936068, + "y": 5.543159599317717, + "heading": -3.434421194050575e-18, + "angularVelocity": -2.5657896847386088e-17, + "velocityX": 0.812351327691905, + "velocityY": 0.0011481088593983807, + "timestamp": 2.6794347242822742 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 2.0232376930557863e-31, - "angularVelocity": -1.3321178776125106e-30, - "velocityX": -6.421692970708144e-31, - "velocityY": -1.2392296625905867e-30, - "timestamp": 5.156918257584381 - }, - { - "x": 1.3044225360616175, - "y": 5.603754712691982, - "heading": 0.008101518995482268, - "angularVelocity": 0.11491508578445629, - "velocityX": 0.20493723803226965, - "velocityY": 0.18156596231474714, - "timestamp": 5.227418304990499 - }, - { - "x": 1.3333187786755418, - "y": 5.6293552812892855, - "heading": 0.024302645459921106, - "angularVelocity": 0.22980305773571622, - "velocityX": 0.40987550614635165, - "velocityY": 0.36312838840846096, - "timestamp": 5.297918352396617 - }, - { - "x": 1.3766634045969632, - "y": 5.667755708974662, - "heading": 0.048595399874496285, - "angularVelocity": 0.34457784509896805, - "velocityX": 0.6148169755366727, - "velocityY": 0.5446865512610124, - "timestamp": 5.368418399802735 - }, - { - "x": 1.434456789320205, - "y": 5.7189556181427035, - "heading": 0.08096686680516672, - "angularVelocity": 0.45916943493943385, - "velocityX": 0.8197637710840284, - "velocityY": 0.7262393580121056, - "timestamp": 5.438918447208853 - }, - { - "x": 1.5066994506601286, - "y": 5.782954524001268, - "heading": 0.12140070801121562, - "angularVelocity": 0.5735292768404597, - "velocityX": 1.0247179115180916, - "velocityY": 0.9077852882835168, - "timestamp": 5.509418494614971 - }, - { - "x": 1.59339203880409, - "y": 5.859751802427408, - "heading": 0.16987894409259566, - "angularVelocity": 0.6876340919619466, - "velocityX": 1.2296812744616465, - "velocityY": 1.0893223657531423, - "timestamp": 5.5799185420210895 - }, - { - "x": 1.6945353339175246, - "y": 5.949346653618572, - "heading": 0.22638374394908448, - "angularVelocity": 0.8014859838460967, - "velocityX": 1.4346557035741434, - "velocityY": 1.2708480985133195, - "timestamp": 5.650418589427208 - }, - { - "x": 1.8101302937852244, - "y": 6.051738034585355, - "heading": 0.290898605978136, - "angularVelocity": 0.9151038106032966, - "velocityX": 1.6396437182773986, - "velocityY": 1.4523590371075072, - "timestamp": 5.720918636833326 - }, - { - "x": 1.940178425831269, - "y": 6.166924341281603, - "heading": 0.36340632675450085, - "angularVelocity": 1.0284776173082797, - "velocityX": 1.8446531148681091, - "velocityY": 1.6338472232892616, - "timestamp": 5.791418684239444 - }, - { - "x": 2.0847170007855893, - "y": 6.294873766483704, - "heading": 0.443594985050904, - "angularVelocity": 1.1374270124170804, - "velocityX": 2.0501911739392336, - "velocityY": 1.8148842434820598, - "timestamp": 5.861918731645562 - }, - { - "x": 2.2148049129108704, - "y": 6.410027050354294, - "heading": 0.5157685134494568, - "angularVelocity": 1.0237373030800216, - "velocityX": 1.8452173709317587, - "velocityY": 1.6333788147296693, - "timestamp": 5.93241877905168 - }, - { - "x": 2.3304407554674365, - "y": 6.512385621001077, - "heading": 0.5799353875370489, - "angularVelocity": 0.9101678147527572, - "velocityX": 1.6402236141834323, - "velocityY": 1.4518936428105123, - "timestamp": 6.002918826457798 - }, - { - "x": 2.431623857347168, - "y": 6.601950249920576, - "heading": 0.6360978126199414, - "angularVelocity": 0.796629607344333, - "velocityX": 1.435220338177405, - "velocityY": 1.2704194140971152, - "timestamp": 6.0734188738639165 - }, - { - "x": 2.518353756343433, - "y": 6.678721476402507, - "heading": 0.6842565298983911, - "angularVelocity": 0.683101913407659, - "velocityX": 1.2302105060533437, - "velocityY": 1.088952834878071, - "timestamp": 6.143918921270035 - }, - { - "x": 2.5906300952596166, - "y": 6.742699702654635, - "heading": 0.7244118519902635, - "angularVelocity": 0.5695786537639655, - "velocityX": 1.0251956073140427, - "velocityY": 0.907491960729887, - "timestamp": 6.214418968676153 - }, - { - "x": 2.648452591103255, - "y": 6.7938852314203, - "heading": 0.7565639740344317, - "angularVelocity": 0.45605816204569094, - "velocityX": 0.820176694499928, - "velocityY": 0.7260353808105814, - "timestamp": 6.284919016082271 - }, - { - "x": 2.6918210260454423, - "y": 6.832278283879232, - "heading": 0.7807130811975228, - "angularVelocity": 0.34254029680263026, - "velocityX": 0.6151546919161919, - "velocityY": 0.5445819381903092, - "timestamp": 6.355419063488389 - }, - { - "x": 2.720735245788176, - "y": 6.85787900840029, - "heading": 0.7968594022841035, - "angularVelocity": 0.22902567701223253, - "velocityX": 0.4101305007097693, - "velocityY": 0.3631306000914257, - "timestamp": 6.425919110894507 + "x": 2.039828562987611, + "y": 5.543319931052194, + "heading": -7.689792330773154e-18, + "angularVelocity": -4.570800524939683e-17, + "velocityX": 1.2185269596070252, + "velocityY": 0.0017221632439692253, + "timestamp": 2.7725337583246477 }, { - "x": 2.73519515991211, - "y": 6.870687484741211, - "heading": 0.8050032485420815, - "angularVelocity": 0.11551547208280752, - "velocityX": 0.2051050269602972, - "velocityY": 0.18168039330721092, - "timestamp": 6.496419158300625 - }, - { - "x": 2.733517887090967, - "y": 6.869215556772689, - "heading": 0.8042279993622282, - "angularVelocity": -0.009948237722316289, - "velocityX": -0.02152328462065399, - "velocityY": -0.018888235836320173, - "timestamp": 6.5743474508779345 - }, - { - "x": 2.714179970265771, - "y": 6.85211357425893, - "heading": 0.7936748330656245, - "angularVelocity": -0.13542150029957753, - "velocityX": -0.24815014143946865, - "velocityY": -0.21945793944853148, - "timestamp": 6.6522757434552435 - }, - { - "x": 2.677181604439607, - "y": 6.819381358340049, - "heading": 0.7733432924791085, - "angularVelocity": -0.26090062946453, - "velocityX": -0.47477449591828125, - "velocityY": -0.4200299382462307, - "timestamp": 6.730204036032553 - }, - { - "x": 2.6225230574768488, - "y": 6.771018630930062, - "heading": 0.7432332740659877, - "angularVelocity": -0.3863810872444029, - "velocityX": -0.7013954130784187, - "velocityY": -0.6206055055294315, - "timestamp": 6.808132328609862 - }, - { - "x": 2.5502046603277093, - "y": 6.707025005806598, - "heading": 0.7033451105326386, - "angularVelocity": -0.5118572756329512, - "velocityX": -0.9280120833828756, - "velocityY": -0.8211860289378066, - "timestamp": 6.886060621187171 - }, - { - "x": 2.4602268003518986, - "y": 6.627399971822084, - "heading": 0.653679622983667, - "angularVelocity": -0.6373229273527391, - "velocityX": -1.1546237829674975, - "velocityY": -1.0217731115502302, - "timestamp": 6.96398891376448 - }, - { - "x": 2.3525899246520714, - "y": 6.53214286652055, - "heading": 0.5942379589890165, - "angularVelocity": -0.7627738530993845, - "velocityX": -1.3812297451922877, - "velocityY": -1.2223686950029513, - "timestamp": 7.041917206341789 - }, - { - "x": 2.22729456327189, - "y": 6.421252842872556, - "heading": 0.5250208143201779, - "angularVelocity": -0.8882158504906453, - "velocityX": -1.6078289057325443, - "velocityY": -1.4229751477999506, - "timestamp": 7.119845498919098 - }, - { - "x": 2.0843413856896924, - "y": 6.294728842395834, - "heading": 0.4460263730973391, - "angularVelocity": -1.0136811498143847, - "velocityX": -1.8344194753193157, - "velocityY": -1.623595183369405, - "timestamp": 7.197773791496407 - }, - { - "x": 1.925462021024861, - "y": 6.1539791801359245, - "heading": 0.3570280981408959, - "angularVelocity": -1.1420534444297203, - "velocityX": -2.038789243421619, - "velocityY": -1.8061432838436418, - "timestamp": 7.275702084073716 - }, - { - "x": 1.7842386496681704, - "y": 6.028865963268039, - "heading": 0.2778322483955418, - "angularVelocity": -1.016265686390949, - "velocityX": -1.8122220657739765, - "velocityY": -1.6054915709049453, - "timestamp": 7.353630376651025 - }, - { - "x": 1.6606703031584165, - "y": 5.9193901868323495, - "heading": 0.2084629069388522, - "angularVelocity": -0.8901688868374888, - "velocityX": -1.5856673157206287, - "velocityY": -1.4048270893024366, - "timestamp": 7.431558669228334 - }, - { - "x": 1.554756060685191, - "y": 5.825552639839227, - "heading": 0.14894782916930657, - "angularVelocity": -0.7637159214094572, - "velocityX": -1.3591243817917928, - "velocityY": -1.2041524828743098, - "timestamp": 7.509486961805643 - }, - { - "x": 1.4664951152377614, - "y": 5.747353916497971, - "heading": 0.09931505664018385, - "angularVelocity": -0.6369031180797928, - "velocityX": -1.1325918036747624, - "velocityY": -1.0034702513683131, - "timestamp": 7.587415254382952 - }, - { - "x": 1.3958868227418013, - "y": 5.684794447832575, - "heading": 0.05958948406530847, - "angularVelocity": -0.5097708580675663, - "velocityX": -0.9060674905190907, - "velocityY": -0.8027824888288644, - "timestamp": 7.665343546960261 - }, - { - "x": 1.3429307344840264, - "y": 5.637874543820294, - "heading": 0.029789872263071265, - "angularVelocity": -0.38239785341985083, - "velocityX": -0.6795489353913313, - "velocityY": -0.6020907485652136, - "timestamp": 7.74327183953757 - }, - { - "x": 1.3076266152194531, - "y": 5.606594434817311, - "heading": 0.009926565476430555, - "angularVelocity": -0.25489210824085684, - "velocityX": -0.4530333990001086, - "velocityY": -0.4013960522996289, - "timestamp": 7.821200132114879 + "x": 2.1910867994360017, + "y": 5.543533706687691, + "heading": -1.506248986539688e-17, + "angularVelocity": -7.919198744068695e-17, + "velocityX": 1.6247025332136764, + "velocityY": 0.0022962175461317975, + "timestamp": 2.865632792367021 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -0.12738076439417462, - "velocityX": -0.2265180407600671, - "velocityY": -0.20069900877579375, - "timestamp": 7.899128424692188 + "x": 2.380159562296481, + "y": 5.5438009261858445, + "heading": -1.506353706937017e-17, + "angularVelocity": -1.1248279684723702e-20, + "velocityX": 2.0308778152781284, + "velocityY": 0.0028702714362532117, + "timestamp": 2.9587318264093945 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, + "x": 2.5314177987448714, + "y": 5.544014701821341, + "heading": -7.704513323430137e-18, + "angularVelocity": 7.904511385790122e-17, + "velocityX": 1.6247025332136762, + "velocityY": 0.002296217546131797, + "timestamp": 3.051830860451768 + }, + { + "x": 2.6448614816388756, + "y": 5.544175033555818, + "heading": -3.433430554917341e-18, + "angularVelocity": 4.587676781446345e-17, + "velocityX": 1.218526959607025, + "velocityY": 0.0017221632439692251, + "timestamp": 3.1449298944941413 + }, + { + "x": 2.7204906055500317, + "y": 5.544281921381604, + "heading": -1.0420071252828256e-18, + "angularVelocity": 2.5686876928777523e-17, + "velocityX": 0.812351327691905, + "velocityY": 0.0011481088593983805, + "timestamp": 3.2380289285365147 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.1539777836664478e-35, + "angularVelocity": 1.1192459041074064e-17, + "velocityX": 0.4061756707874381, + "velocityY": 0.0005740544395097005, + "timestamp": 3.331127962578888 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, "heading": 0, - "angularVelocity": 0, - "velocityX": 1.105033972502462e-31, - "velocityY": 4.1576254509102287e-32, - "timestamp": 7.977056717269497 - }, - { - "x": 1.3079814373562102, - "y": 5.591183871815198, - "heading": -3.6007722166496235e-18, - "angularVelocity": -5.30176004915721e-17, - "velocityX": 0.26513400990373515, - "velocityY": 0.0033801494016308404, - "timestamp": 8.044973263689492 - }, - { - "x": 1.343996733723379, - "y": 5.591342598530086, - "heading": -8.55205935993227e-18, - "angularVelocity": -7.290251850967679e-17, - "velocityX": 0.5302875111530345, - "velocityY": 0.0023370846024297455, - "timestamp": 8.112889810109488 - }, - { - "x": 1.398015752012773, - "y": 5.591088723884006, - "heading": -1.2570686122392014e-17, - "angularVelocity": -5.917007079271892e-17, - "velocityX": 0.7953734566439974, - "velocityY": -0.003738038216918613, - "timestamp": 8.180806356529484 - }, - { - "x": 1.4700252438968529, - "y": 5.590030427465675, - "heading": -1.5707006622298704e-17, - "angularVelocity": -4.617903467384599e-17, - "velocityX": 1.0602643343902365, - "velocityY": -0.015582306140636595, - "timestamp": 8.24872290294948 - }, - { - "x": 1.5599992284903603, - "y": 5.587714662715152, - "heading": -1.765002346502037e-17, - "angularVelocity": -2.8608887247798064e-17, - "velocityX": 1.3247726708173502, - "velocityY": -0.03409721007015733, - "timestamp": 8.316639449369475 - }, - { - "x": 1.6678928587399022, - "y": 5.583612697159567, - "heading": -1.9324789020346336e-17, - "angularVelocity": -2.4659167675098536e-17, - "velocityX": 1.5886206813628097, - "velocityY": -0.060397145788562055, - "timestamp": 8.384555995789471 - }, - { - "x": 1.7936330139942434, - "y": 5.577101220041518, - "heading": -2.0699583000572945e-17, - "angularVelocity": -2.024240007004298e-17, - "velocityX": 1.8513920669164408, - "velocityY": -0.09587467946002654, - "timestamp": 8.452472542209467 - }, - { - "x": 1.9371035847765512, - "y": 5.567437513465235, - "heading": -2.4072402865280595e-17, - "angularVelocity": -4.9661238919294396e-17, - "velocityX": 2.112453862054281, - "velocityY": -0.1422879561119276, - "timestamp": 8.520389088629463 - }, - { - "x": 2.098121982759825, - "y": 5.553726841653449, - "heading": -2.991372900129062e-17, - "angularVelocity": -8.600740883406488e-17, - "velocityX": 2.370827235347597, - "velocityY": -0.20187527980293163, - "timestamp": 8.588305635049458 - }, - { - "x": 2.276400921296343, - "y": 5.534880229318232, - "heading": -3.3606634340190096e-17, - "angularVelocity": -5.437416182505347e-17, - "velocityX": 2.624970613700564, - "velocityY": -0.2774966238517093, - "timestamp": 8.656222181469454 - }, - { - "x": 2.471485361845495, - "y": 5.509562201282429, - "heading": -3.628143679663613e-17, - "angularVelocity": -3.938366258883085e-17, - "velocityX": 2.8724140262190536, - "velocityY": -0.37278144090596593, - "timestamp": 8.72413872788945 - }, - { - "x": 2.682648461637503, - "y": 5.476133636589416, - "heading": -3.7916749117831765e-17, - "angularVelocity": -2.4078260416782975e-17, - "velocityX": 3.1091554403573096, - "velocityY": -0.4922005969839973, - "timestamp": 8.792055274309446 - }, - { - "x": 2.9087251812985304, - "y": 5.4326111135896715, - "heading": -3.8585043209768917e-17, - "angularVelocity": -9.83992917098953e-18, - "velocityX": 3.328742870153774, - "velocityY": -0.6408235591162347, - "timestamp": 8.859971820729442 - }, - { - "x": 3.1478726960604213, - "y": 5.376700760732656, - "heading": -3.7931961364229683e-17, - "angularVelocity": 9.615948273285425e-18, - "velocityX": 3.5211966356917612, - "velocityY": -0.8232213768831518, - "timestamp": 8.927888367149437 - }, - { - "x": 3.397314191935591, - "y": 5.306014145470732, - "heading": -3.37531662670372e-17, - "angularVelocity": 6.152838186169992e-17, - "velocityX": 3.6727647241163455, - "velocityY": -1.0407863619094957, - "timestamp": 8.995804913569433 - }, - { - "x": 3.6532783223184353, - "y": 5.218541845963183, - "heading": -2.785799913755649e-17, - "angularVelocity": 8.680016148087689e-17, - "velocityX": 3.7688036844506647, - "velocityY": -1.287937978568902, - "timestamp": 9.063721459989429 - }, - { - "x": 3.911405346241733, - "y": 5.11319146350464, - "heading": -1.864507789949357e-17, - "angularVelocity": 1.3565061404938756e-16, - "velocityX": 3.8006500260929266, - "velocityY": -1.5511740218217003, - "timestamp": 9.131638006409425 + "angularVelocity": 7.156872912951633e-35, + "velocityX": -1.751436869795613e-36, + "velocityY": -4.841667244220732e-36, + "timestamp": 3.4242269966212615 + }, + { + "x": 2.7396220511907092, + "y": 5.562601673573336, + "heading": 0.0037078860807813565, + "angularVelocity": 0.0478983711971462, + "velocityX": -0.2413479950107084, + "velocityY": 0.23596367181626482, + "timestamp": 3.5016385241556733 + }, + { + "x": 2.702380969511178, + "y": 5.599259386918254, + "heading": 0.011292704027819494, + "angularVelocity": 0.09798047123752286, + "velocityX": -0.4810792767650385, + "velocityY": 0.47354334053959213, + "timestamp": 3.579050051690085 + }, + { + "x": 2.6467699433072402, + "y": 5.6544931206396525, + "heading": 0.023007945503191744, + "angularVelocity": 0.15133716965040475, + "velocityX": -0.7183817187849313, + "velocityY": 0.7135078647924301, + "timestamp": 3.656461579224497 + }, + { + "x": 2.573102880230356, + "y": 5.72860253774289, + "heading": 0.03927230490424403, + "angularVelocity": 0.21010255086133378, + "velocityX": -0.9516291103303323, + "velocityY": 0.9573434275701879, + "timestamp": 3.733873106758909 + }, + { + "x": 2.4820081686264115, + "y": 5.82215722498829, + "heading": 0.060903903359814396, + "angularVelocity": 0.27943639848670454, + "velocityX": -1.1767589983733413, + "velocityY": 1.2085368965728291, + "timestamp": 3.8112846342933207 + }, + { + "x": 2.3753503305689, + "y": 5.936638406081269, + "heading": 0.09019255741494601, + "angularVelocity": 0.3783500337480349, + "velocityX": -1.3778030411568694, + "velocityY": 1.478864772976981, + "timestamp": 3.8886961618277325 + }, + { + "x": 2.2779168004384966, + "y": 6.074876724828902, + "heading": 0.14378193450398408, + "angularVelocity": 0.6922661106928292, + "velocityX": -1.25864368310122, + "velocityY": 1.7857588288279347, + "timestamp": 3.9661076893621443 + }, + { + "x": 2.2032159820535764, + "y": 6.200217981312059, + "heading": 0.2000311193060264, + "angularVelocity": 0.7266254341388357, + "velocityX": -0.9649831331866354, + "velocityY": 1.6191549304777415, + "timestamp": 4.043519216896556 + }, + { + "x": 2.149623841782867, + "y": 6.310133396102646, + "heading": 0.2561023628726984, + "angularVelocity": 0.7243267941166335, + "velocityX": -0.6923018053982481, + "velocityY": 1.4198843284900602, + "timestamp": 4.120930744430967 + }, + { + "x": 2.1165539042639407, + "y": 6.4038530923195935, + "heading": 0.3110558787207687, + "angularVelocity": 0.7098880179523891, + "velocityX": -0.42719655033581183, + "velocityY": 1.2106684779639116, + "timestamp": 4.1983422719653785 + }, + { + "x": 2.1037067095365876, + "y": 6.481006876335726, + "heading": 0.36442032903480814, + "angularVelocity": 0.6893605127520233, + "velocityX": -0.16595971086660646, + "velocityY": 0.9966704762651166, + "timestamp": 4.27575379949979 + }, + { + "x": 2.1109008821512623, + "y": 6.541377447734871, + "heading": 0.4159102858737852, + "angularVelocity": 0.6651458571992167, + "velocityX": 0.09293412549541542, + "velocityY": 0.7798653937207066, + "timestamp": 4.353165327034201 + }, + { + "x": 2.1380148682221702, + "y": 6.5848219723004595, + "heading": 0.465333297367931, + "angularVelocity": 0.6384451136450677, + "velocityX": 0.3502577320781457, + "velocityY": 0.5612151826648336, + "timestamp": 4.430576854568613 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.6099494963599527, + "velocityX": 0.6064560493947925, + "velocityY": 0.34126004006795546, + "timestamp": 4.507988382103024 + }, + { + "x": 2.250707976446258, + "y": 6.648678224064634, + "heading": 0.55208552781375, + "angularVelocity": 0.5325541984016288, + "velocityX": 0.8856313560565373, + "velocityY": 0.5043159387113171, + "timestamp": 4.582225160880485 + }, + { + "x": 2.3371670619589073, + "y": 6.698226995892447, + "heading": 0.5853560107794161, + "angularVelocity": 0.44816711492022887, + "velocityX": 1.1646395080237382, + "velocityY": 0.6674423734944788, + "timestamp": 4.656461939657946 + }, + { + "x": 2.4409073566894506, + "y": 6.758690881227722, + "heading": 0.5617681472290976, + "angularVelocity": -0.31773824159353153, + "velocityX": 1.3974245170513666, + "velocityY": 0.8144734501011613, + "timestamp": 4.7306987184354075 + }, + { + "x": 2.523886913207481, + "y": 6.807069780587539, + "heading": 0.5422957792802172, + "angularVelocity": -0.2623008200187737, + "velocityX": 1.1177688186980357, + "velocityY": 0.6516837092951017, + "timestamp": 4.804935497212869 + }, + { + "x": 2.5861173110961473, + "y": 6.843356427328165, + "heading": 0.5274956073850987, + "angularVelocity": -0.1993644139582728, + "velocityX": 0.8382691021011789, + "velocityY": 0.4887960838037212, + "timestamp": 4.87917227599033 + }, + { + "x": 2.627602512984248, + "y": 6.8675485030782895, + "heading": 0.5175502299027057, + "angularVelocity": -0.13396833276139558, + "velocityX": 0.5588227637470666, + "velocityY": 0.3258772288954636, + "timestamp": 4.953409054767791 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06734950498988643, + "velocityX": 0.2794033376916902, + "velocityY": 0.16294305705300594, + "timestamp": 5.027645833545252 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -4.084231073354503e-32, + "velocityX": -1.1079933009257942e-33, + "velocityY": -1.0209152810451872e-34, + "timestamp": 5.101882612322713 + }, + { + "x": 2.6570277255317225, + "y": 6.867726793502589, + "heading": 0.5066730395869481, + "angularVelocity": -0.10087167460547103, + "velocityX": 0.14902725506901718, + "velocityY": -0.20454631284120778, + "timestamp": 5.160148523303567 + }, + { + "x": 2.6744037356282937, + "y": 6.843893366933042, + "heading": 0.49507655406382295, + "angularVelocity": -0.1990269323504748, + "velocityX": 0.2982191439910878, + "velocityY": -0.4090458068591583, + "timestamp": 5.218414434284421 + }, + { + "x": 2.7004835818665893, + "y": 6.808147821587349, + "heading": 0.4779456476925187, + "angularVelocity": -0.29401250375942084, + "velocityX": 0.44760042019879576, + "velocityY": -0.6134898561431401, + "timestamp": 5.276680345265275 + }, + { + "x": 2.735280123191829, + "y": 6.760494081762779, + "heading": 0.4554977531812837, + "angularVelocity": -0.38526634413407634, + "velocityX": 0.5972023905482879, + "velocityY": -0.8178665539139939, + "timestamp": 5.334946256246129 + }, + { + "x": 2.77880855180183, + "y": 6.700937036637074, + "heading": 0.4279915994110024, + "angularVelocity": -0.4720796999006835, + "velocityX": 0.7470650999399128, + "velocityY": -1.022159340223411, + "timestamp": 5.393212167226983 + }, + { + "x": 2.8310870878518544, + "y": 6.629482937316093, + "heading": 0.39573903851012515, + "angularVelocity": -0.5535408330177088, + "velocityX": 0.8972405162806606, + "velocityY": -1.2263448407158972, + "timestamp": 5.4514780782078365 + }, + { + "x": 2.8921379557757043, + "y": 6.546140000901142, + "heading": 0.3591220542576228, + "angularVelocity": -0.6284460954285072, + "velocityX": 1.0477973637777922, + "velocityY": -1.4303893136132022, + "timestamp": 5.50974398918869 + }, + { + "x": 2.9619888074879523, + "y": 6.4509193752842275, + "heading": 0.3186184742438743, + "angularVelocity": -0.6951505491273999, + "velocityX": 1.1988287926228676, + "velocityY": -1.6342424586513933, + "timestamp": 5.568009900169544 + }, + { + "x": 3.0406748905144103, + "y": 6.3438367809482505, + "heading": 0.27484324456730397, + "angularVelocity": -0.7513008711209391, + "velocityX": 1.3504651639672256, + "velocityY": -1.8378257978522827, + "timestamp": 5.626275811150398 + }, + { + "x": 3.12824252650645, + "y": 6.224915546783436, + "heading": 0.22861956489914778, + "angularVelocity": -0.7933228690673962, + "velocityX": 1.502896539639008, + "velocityY": -2.0410087504491825, + "timestamp": 5.684541722131252 + }, + { + "x": 3.224755029145573, + "y": 6.094192875417537, + "heading": 0.18111264418965425, + "angularVelocity": -0.815346742370587, + "velocityX": 1.6564145486515685, + "velocityY": -2.243553205730433, + "timestamp": 5.742807633112106 + }, + { + "x": 3.3303033780389395, + "y": 5.951734868373249, + "heading": 0.13411173965139422, + "angularVelocity": -0.8066621416715647, + "velocityX": 1.8114940128208097, + "velocityY": -2.444963180805693, + "timestamp": 5.80107354409296 + }, + { + "x": 3.4450256229008205, + "y": 5.797681352790493, + "heading": 0.09073046456880836, + "angularVelocity": -0.7445395489798686, + "velocityX": 1.9689427819909868, + "velocityY": -2.643973345467458, + "timestamp": 5.859339455073814 + }, + { + "x": 3.5691217392378216, + "y": 5.632434411388871, + "heading": 0.05763038975845234, + "angularVelocity": -0.5680864548952619, + "velocityX": 2.1298236695857957, + "velocityY": -2.8360826874554848, + "timestamp": 5.9176053660546675 + }, + { + "x": 3.7017085484583103, + "y": 5.4576517803314095, + "heading": 0.0547360803841417, + "angularVelocity": -0.049674146093104596, + "velocityX": 2.2755468332771343, + "velocityY": -2.999740810967421, + "timestamp": 5.975871277035521 + }, + { + "x": 3.8463662629346644, + "y": 5.2914222205797, + "heading": 0.05473605711060691, + "angularVelocity": -3.9943655557036433e-7, + "velocityX": 2.4827160863217212, + "velocityY": -2.8529470655034146, + "timestamp": 6.034137188016375 + }, + { + "x": 4.001863921193704, + "y": 5.135285744872655, + "heading": 0.05473603479828713, + "angularVelocity": -3.8293951656527263e-7, + "velocityX": 2.668758724293121, + "velocityY": -2.679722552666384, + "timestamp": 6.092403098997229 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -3.9291869622836704e-18, - "angularVelocity": 2.1667607761478547e-16, - "velocityX": 3.770711743405716, - "velocityY": -1.8146339019446507, - "timestamp": 9.19955455282942 - }, - { - "x": 4.28210524468114, - "y": 4.930751598714327, - "heading": 3.591781681737572e-18, - "angularVelocity": 2.4504750021143145e-16, - "velocityX": 3.734087818922699, - "velocityY": -1.9287249614518895, - "timestamp": 9.230246433909775 - }, - { - "x": 4.395415253823689, - "y": 4.868113726791297, - "heading": 1.252716982122126e-17, - "angularVelocity": 2.9113198354077836e-16, - "velocityX": 3.691856124618014, - "velocityY": -2.0408612870301757, - "timestamp": 9.26093831499013 - }, - { - "x": 4.5071149827963914, - "y": 4.802169472812339, - "heading": 2.3134869211764074e-17, - "angularVelocity": 3.4561906984236055e-16, - "velocityX": 3.639390126667849, - "velocityY": -2.1485895180653474, - "timestamp": 9.291630196070484 - }, - { - "x": 4.616896016370763, - "y": 4.73308774688056, - "heading": 3.142037619794973e-17, - "angularVelocity": 2.699576125240905e-16, - "velocityX": 3.576875372576209, - "velocityY": -2.2508143359119486, - "timestamp": 9.322322077150838 - }, - { - "x": 4.7230901119791495, - "y": 4.663194322009908, - "heading": 3.6606203768648707e-17, - "angularVelocity": 1.6896414210849805e-16, - "velocityX": 3.460006095108622, - "velocityY": -2.277261034853729, - "timestamp": 9.353013958231193 - }, - { - "x": 4.827842225607654, - "y": 4.596684057361499, - "heading": 4.0451980836943306e-17, - "angularVelocity": 1.2530273569920946e-16, - "velocityX": 3.413023573050007, - "velocityY": -2.1670312247815544, - "timestamp": 9.383705839311547 - }, - { - "x": 4.932161852632399, - "y": 4.533825930261183, - "heading": 4.329689113733961e-17, - "angularVelocity": 9.26925861582003e-17, - "velocityX": 3.3989323349592793, - "velocityY": -2.0480376206250996, - "timestamp": 9.414397720391902 - }, - { - "x": 5.036392029451142, - "y": 4.474644374374991, - "heading": 4.527403793938572e-17, - "angularVelocity": 6.441920024435287e-17, - "velocityX": 3.396017876709991, - "velocityY": -1.9282479210462729, - "timestamp": 9.445089601472256 - }, - { - "x": 5.1407004011750175, - "y": 4.419139647910205, - "heading": 4.6239659419552744e-17, - "angularVelocity": 3.1461773577369364e-17, - "velocityX": 3.3985656157987063, - "velocityY": -1.8084498085819396, - "timestamp": 9.47578148255261 - }, - { - "x": 5.245185751752097, - "y": 4.367308322392446, - "heading": 4.6966543072994365e-17, - "angularVelocity": 2.3683241137198615e-17, - "velocityX": 3.4043319242482535, - "velocityY": -1.68876340234931, - "timestamp": 9.506473363632965 - }, - { - "x": 5.349913071755195, - "y": 4.319146690519462, - "heading": 4.64672982849528e-17, - "angularVelocity": -1.6266355263468505e-17, - "velocityX": 3.4122157494652257, - "velocityY": -1.5691977870920355, - "timestamp": 9.53716524471332 - }, - { - "x": 5.454928326528494, - "y": 4.274651432535085, - "heading": 4.6021079264842955e-17, - "angularVelocity": -1.4538674934985797e-17, - "velocityX": 3.421597213229158, - "velocityY": -1.449740335819904, - "timestamp": 9.567857125793674 - }, - { - "x": 5.560265728804007, - "y": 4.233819701178179, - "heading": 4.5613400126539626e-17, - "angularVelocity": -1.32829717492163e-17, - "velocityX": 3.432093393029018, - "velocityY": -1.3303756537438909, - "timestamp": 9.598549006874029 + "heading": 0.0547360118053875, + "angularVelocity": -3.946201001676767e-7, + "velocityX": 2.842745293382521, + "velocityY": -2.4943907433712678, + "timestamp": 6.150669009978083 + }, + { + "x": 4.269399861624288, + "y": 4.9075045981351995, + "heading": 0.054735989995402, + "angularVelocity": -6.29291522756053e-7, + "velocityX": 2.940181103085103, + "velocityY": -2.3787638667331756, + "timestamp": 6.185327009826526 + }, + { + "x": 4.374514707093794, + "y": 4.829200558555379, + "heading": 0.05473596913891511, + "angularVelocity": -6.017798771949806e-7, + "velocityX": 3.0329172464990677, + "velocityY": -2.2593352161762894, + "timestamp": 6.21998500967497 + }, + { + "x": 4.482675566428291, + "y": 4.7551608256734985, + "heading": 0.0547359490445404, + "angularVelocity": -5.797903745228634e-7, + "velocityX": 3.120805003389626, + "velocityY": -2.1362956086805514, + "timestamp": 6.254643009523413 + }, + { + "x": 4.593709527898866, + "y": 4.685503714898617, + "heading": 0.05473592954464412, + "angularVelocity": -5.62637669848401e-7, + "velocityX": 3.203703674652828, + "velocityY": -2.0098422032282808, + "timestamp": 6.289301009371856 + }, + { + "x": 4.707439069036232, + "y": 4.620340494746092, + "heading": 0.05473591048922828, + "angularVelocity": -5.498129124049459e-7, + "velocityX": 3.2814802249032198, + "velocityY": -1.8801783264319567, + "timestamp": 6.3239590092202995 + }, + { + "x": 4.823682298536361, + "y": 4.559775113095544, + "heading": 0.054735891740807854, + "angularVelocity": -5.409550611531198e-7, + "velocityX": 3.3540085985473036, + "velocityY": -1.747515203283397, + "timestamp": 6.358617009068743 + }, + { + "x": 4.942252940190184, + "y": 4.503903323846341, + "heading": 0.05473587317010921, + "angularVelocity": -5.358271892357539e-7, + "velocityX": 3.4211622763091856, + "velocityY": -1.6120892577045927, + "timestamp": 6.393275008917186 + }, + { + "x": 5.062124990174475, + "y": 4.450881200757823, + "heading": 0.05473585465308408, + "angularVelocity": -5.342785275702722e-7, + "velocityX": 3.4587122888938238, + "velocityY": -1.52986679324772, + "timestamp": 6.4279330087656295 + }, + { + "x": 5.1819977334562, + "y": 4.397860645099044, + "heading": 0.05473583613607813, + "angularVelocity": -5.3427797428813e-7, + "velocityX": 3.4587322928593447, + "velocityY": -1.529821567621728, + "timestamp": 6.462591008614073 + }, + { + "x": 5.301870477238367, + "y": 4.344840090571707, + "heading": 0.05473581761907218, + "angularVelocity": -5.342779742919752e-7, + "velocityX": 3.458732307298825, + "velocityY": -1.5298215349758078, + "timestamp": 6.497249008462516 + }, + { + "x": 5.421743975168144, + "y": 4.291821241109097, + "heading": 0.0547357991020742, + "angularVelocity": -5.342777443988636e-7, + "velocityX": 3.4587540669967396, + "velocityY": -1.5297723381170152, + "timestamp": 6.531907008310959 + }, + { + "x": 5.5428966637680075, + "y": 4.241794448876403, + "heading": 0.05473578057951323, + "angularVelocity": -5.34438255269497e-7, + "velocityX": 3.4956630252656034, + "velocityY": -1.4434414118372951, + "timestamp": 6.566565008159403 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 4.5318461812156785e-17, - "angularVelocity": -9.609659048938486e-18, - "velocityX": 3.443451372045305, - "velocityY": -1.2110898816015587, - "timestamp": 9.629240887954383 - }, - { - "x": 5.79328341664379, - "y": 4.157319683069999, - "heading": 4.51013680218967e-17, - "angularVelocity": -5.8955361022936136e-18, - "velocityX": 3.457901563090961, - "velocityY": -1.0680543595600307, - "timestamp": 9.666064286625446 - }, - { - "x": 5.920925042987332, - "y": 4.123275074482791, - "heading": 4.436771325922853e-17, - "angularVelocity": -1.9923600143581754e-17, - "velocityX": 3.4663184537566436, - "velocityY": -0.9245373815525213, - "timestamp": 9.702887685296508 - }, - { - "x": 6.048609186312294, - "y": 4.094524158555703, - "heading": 4.179578855252982e-17, - "angularVelocity": -6.984484950544979e-17, - "velocityX": 3.4674730723674467, - "velocityY": -0.780778444268955, - "timestamp": 9.73971108396757 - }, - { - "x": 6.176010603965038, - "y": 4.0710595506921905, - "heading": 3.752283423078513e-17, - "angularVelocity": -1.1603911512742033e-16, - "velocityX": 3.4597951913890252, - "velocityY": -0.6372200478591985, - "timestamp": 9.776534482638633 - }, - { - "x": 6.3027303874933365, - "y": 4.052844738846322, - "heading": 3.3662688725727524e-17, - "angularVelocity": -1.0482861149720267e-16, - "velocityX": 3.4412842948111595, - "velocityY": -0.4946531961533159, - "timestamp": 9.813357881309695 - }, - { - "x": 6.428277540039444, - "y": 4.03979228265205, - "heading": 2.875531522756203e-17, - "angularVelocity": -1.3326781276580096e-16, - "velocityX": 3.409439570410085, - "velocityY": -0.35446093150903635, - "timestamp": 9.850181279980758 - }, - { - "x": 6.552051846294861, - "y": 4.031728011576002, - "heading": 2.3979849407990237e-17, - "angularVelocity": -1.2968564213358396e-16, - "velocityX": 3.36129501138861, - "velocityY": -0.21899855437257756, - "timestamp": 9.88700467865182 - }, - { - "x": 6.673339149172373, - "y": 4.028337045978646, - "heading": 1.9327950159963868e-17, - "angularVelocity": -1.2632998799939155e-16, - "velocityX": 3.2937563412044475, - "velocityY": -0.09208725212048961, - "timestamp": 9.923828077322883 - }, - { - "x": 6.791341292583958, - "y": 4.029097308231655, - "heading": 1.4785687788943395e-17, - "angularVelocity": -1.2335261767272074e-16, - "velocityX": 3.2045424287333875, - "velocityY": 0.020646172826134297, - "timestamp": 9.960651475993945 - }, - { - "x": 6.905265249942477, - "y": 4.033232975759379, - "heading": 1.0332836121843877e-17, - "angularVelocity": -1.2092452080116043e-16, - "velocityX": 3.0937925740147887, - "velocityY": 0.11231085877402444, - "timestamp": 9.997474874665008 - }, - { - "x": 7.014459322141266, - "y": 4.039746278237479, - "heading": 6.270033366373878e-18, - "angularVelocity": -1.103320979751085e-16, - "velocityX": 2.9653447573973857, - "velocityY": 0.17687944929482444, - "timestamp": 10.03429827333607 - }, - { - "x": 7.1185188980352505, - "y": 4.047548732117647, - "heading": 2.7758268202913238e-18, - "angularVelocity": -9.489093222865076e-17, - "velocityX": 2.8259090591700105, - "velocityY": 0.21188847748320577, - "timestamp": 10.071121672007132 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, - "angularVelocity": -7.538214933530801e-17, - "velocityX": 2.6823286459819884, - "velocityY": 0.21914202127972096, - "timestamp": 10.107945070678195 - }, - { - "x": 7.3677982829654445, - "y": 4.067452321816921, - "heading": -1.899704607683099e-18, - "angularVelocity": -3.08402414017173e-17, - "velocityX": 2.4433640695715435, - "velocityY": 0.19211645606626163, - "timestamp": 10.169543311278819 - }, - { - "x": 7.503615163186003, - "y": 4.077376831566324, - "heading": -3.815984956526995e-18, - "angularVelocity": -3.1109335739679044e-17, - "velocityX": 2.204882459243212, - "velocityY": 0.16111677302195881, - "timestamp": 10.231141551879443 - }, - { - "x": 7.624766058972102, - "y": 4.085215025650736, - "heading": -3.719599241310952e-18, - "angularVelocity": 1.5647479451753584e-18, - "velocityX": 1.9667914960686657, - "velocityY": 0.12724704485038288, - "timestamp": 10.292739792480067 - }, - { - "x": 7.731270598361674, - "y": 4.090833292880733, - "heading": -3.1356688291160894e-18, - "angularVelocity": 9.479660768691952e-18, - "velocityX": 1.7290191789746205, - "velocityY": 0.09120824191105437, - "timestamp": 10.35433803308069 - }, - { - "x": 7.8231450024295786, - "y": 4.094127127737666, - "heading": -2.8973098167797552e-18, - "angularVelocity": 3.86957486173007e-18, - "velocityX": 1.491510198539231, - "velocityY": 0.05347287235504842, - "timestamp": 10.415936273681314 - }, - { - "x": 7.900402858238064, - "y": 4.09501256603324, - "heading": -2.6735415540223583e-18, - "angularVelocity": 3.632705288594772e-18, - "velocityX": 1.254221793596197, - "velocityY": 0.014374408862020004, - "timestamp": 10.477534514281938 - }, - { - "x": 7.963055690401912, - "y": 4.09342067695553, - "heading": -2.4640875687476226e-18, - "angularVelocity": 3.4003240257243437e-18, - "velocityX": 1.017120481899185, - "velocityY": -0.025843093279754936, - "timestamp": 10.539132754882562 - }, - { - "x": 8.011113385697886, - "y": 4.089293860980006, - "heading": -2.2687163840908643e-18, - "angularVelocity": 3.171700683847968e-18, - "velocityX": 0.7801796744092016, - "velocityY": -0.06699567934545418, - "timestamp": 10.600730995483186 - }, - { - "x": 8.044584512204576, - "y": 4.082583270186536, - "heading": -1.2051953127305475e-18, - "angularVelocity": 1.7265445662875882e-17, - "velocityX": 0.5433779630769335, - "velocityY": -0.108941273777265, - "timestamp": 10.66232923608381 + "heading": 0.054735761954551196, + "angularVelocity": -5.373928706734811e-7, + "velocityX": 3.550552991831721, + "velocityY": -1.3025960678451716, + "timestamp": 6.601223008007846 + }, + { + "x": 5.798180192000891, + "y": 4.154397155999577, + "heading": 0.05473574376724012, + "angularVelocity": -4.955056404583076e-7, + "velocityX": 3.6025088632415367, + "velocityY": -1.15113574961205, + "timestamp": 6.637927557031803 + }, + { + "x": 5.93207840031246, + "y": 4.117779387908136, + "heading": 0.054735725716885786, + "angularVelocity": -4.917743118523635e-7, + "velocityX": 3.6480003670436316, + "velocityY": -0.9976356899941846, + "timestamp": 6.6746321060557605 + }, + { + "x": 6.067406110251661, + "y": 4.086860105171276, + "heading": 0.0547357076680171, + "angularVelocity": -4.917338358563753e-7, + "velocityX": 3.686946537631365, + "velocityY": -0.8423828533264233, + "timestamp": 6.711336655079718 + }, + { + "x": 6.203921243902459, + "y": 4.061696453595828, + "heading": 0.05473568948478555, + "angularVelocity": -4.953944957116945e-7, + "velocityX": 3.719297397216157, + "velocityY": -0.6855731031873736, + "timestamp": 6.748041204103675 + }, + { + "x": 6.341379198553944, + "y": 4.042334078664949, + "heading": 0.054735669406532635, + "angularVelocity": -5.47023555566929e-7, + "velocityX": 3.7449841588235726, + "velocityY": -0.5275197610585458, + "timestamp": 6.784745753127632 + }, + { + "x": 6.479285910845981, + "y": 4.028828408214569, + "heading": 0.05386070036293022, + "angularVelocity": -0.023838163575618188, + "velocityX": 3.7572103719902135, + "velocityY": -0.3679563108530375, + "timestamp": 6.8214503021515895 + }, + { + "x": 6.612620100679574, + "y": 4.0185345658565055, + "heading": 0.045620785713098924, + "angularVelocity": -0.22449300887617385, + "velocityX": 3.6326339208408243, + "velocityY": -0.2804514053923919, + "timestamp": 6.858154851175547 + }, + { + "x": 6.740581673709006, + "y": 4.010579483858113, + "heading": 0.035519685699448925, + "angularVelocity": -0.27520022128747124, + "velocityX": 3.4862592357669087, + "velocityY": -0.2167328630900888, + "timestamp": 6.894859400199504 + }, + { + "x": 6.863028396494948, + "y": 4.00465723612987, + "heading": 0.025264871332947036, + "angularVelocity": -0.27938810417772464, + "velocityX": 3.3360094604627837, + "velocityY": -0.16134914842236953, + "timestamp": 6.931563949223461 + }, + { + "x": 6.979909878212025, + "y": 4.000619227079583, + "heading": 0.015648348920582984, + "angularVelocity": -0.26199810835673765, + "velocityX": 3.184386808315938, + "velocityY": -0.1100138581637957, + "timestamp": 6.968268498247419 + }, + { + "x": 7.091201711972647, + "y": 3.998378156691166, + "heading": 0.00712732553533738, + "angularVelocity": -0.23215169813648398, + "velocityX": 3.0320992007824574, + "velocityY": -0.06105702012450316, + "timestamp": 7.004973047271376 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -3.597785008712915e-31, + "angularVelocity": -0.19418098641357107, + "velocityX": 2.87942961178202, + "velocityY": -0.013663498666533047, + "timestamp": 7.041677596295333 + }, + { + "x": 7.37119660747943, + "y": 4.00262802828422, + "heading": -0.0076676970964569885, + "angularVelocity": -0.11437965193876058, + "velocityX": 2.6001474595947878, + "velocityY": 0.07087677805125822, + "timestamp": 7.10871484432405 + }, + { + "x": 7.526209738417895, + "y": 4.010745052233057, + "heading": -0.011651875074249012, + "angularVelocity": -0.059432302114867865, + "velocityX": 2.3123432941647923, + "velocityY": 0.1210822965966538, + "timestamp": 7.1757520923527665 + }, + { + "x": 7.661711723306385, + "y": 4.020683594487604, + "heading": -0.013062756224948863, + "angularVelocity": -0.021046227167551262, + "velocityX": 2.0212939652660666, + "velocityY": 0.1482540311065431, + "timestamp": 7.242789340381483 + }, + { + "x": 7.777626027630173, + "y": 4.031355931329743, + "heading": -0.012680116865926582, + "angularVelocity": 0.005707861976350362, + "velocityX": 1.729102964879078, + "velocityY": 0.15920010376271715, + "timestamp": 7.3098265884102 + }, + { + "x": 7.873936260933145, + "y": 4.041961506483741, + "heading": -0.011077902998110952, + "angularVelocity": 0.023900352638719452, + "velocityX": 1.4366674667449708, + "velocityY": 0.15820421431164344, + "timestamp": 7.3768638364389165 + }, + { + "x": 7.950652622834928, + "y": 4.051889363613367, + "heading": -0.008694806000859931, + "angularVelocity": 0.035548848846393165, + "velocityX": 1.1443841171541105, + "velocityY": 0.14809464024199548, + "timestamp": 7.443901084467633 + }, + { + "x": 8.007796949441587, + "y": 4.060659248403223, + "heading": -0.005876390393834335, + "angularVelocity": 0.04204253142698025, + "velocityX": 0.8524264985069137, + "velocityY": 0.1308210740706325, + "timestamp": 7.51093833249635 + }, + { + "x": 8.045395699194648, + "y": 4.067884383860418, + "heading": -0.00290156854092843, + "angularVelocity": 0.04437565592835165, + "velocityX": 0.5608635625518943, + "velocityY": 0.1077779245069823, + "timestamp": 7.577975580525067 }, { "x": 8.0634765625, "y": 4.073246955871582, "heading": 0, - "angularVelocity": 1.956541790475601e-17, - "velocityX": 0.30669788797884484, - "velocityY": -0.15156787310674, - "timestamp": 10.723927476684434 - }, - { - "x": 8.064977337405587, - "y": 4.058030189469969, - "heading": 1.4400664949819045e-18, - "angularVelocity": 1.9298542246489556e-17, - "velocityX": 0.0201121046014515, - "velocityY": -0.20392211811774588, - "timestamp": 10.79854795700343 - }, - { - "x": 8.04498852817461, - "y": 4.039529293710826, - "heading": 2.8532799754877234e-18, - "angularVelocity": 1.8938681059523328e-17, - "velocityX": -0.26787296390383064, - "velocityY": -0.2479332172622413, - "timestamp": 10.873168437322427 - }, - { - "x": 8.003412006395102, - "y": 4.018467462910651, - "heading": 4.2316573286544775e-18, - "angularVelocity": 1.847183698342028e-17, - "velocityX": -0.5571730656486096, - "velocityY": -0.2822526833134443, - "timestamp": 10.947788917641423 - }, - { - "x": 7.940164057594933, - "y": 3.9956918759860844, - "heading": 5.565943858803391e-18, - "angularVelocity": 1.7880969384827547e-17, - "velocityX": -0.8475950373113531, - "velocityY": -0.30521898046224694, - "timestamp": 11.02240939796042 - }, - { - "x": 7.85518871719706, - "y": 3.9722034430196183, - "heading": 6.5199606818876674e-18, - "angularVelocity": 1.2784919284944702e-17, - "velocityX": -1.138766998478306, - "velocityY": -0.31477193481005983, - "timestamp": 11.097029878279416 - }, - { - "x": 7.748479579667781, - "y": 3.949194323259055, - "heading": 7.228395857909563e-18, - "angularVelocity": 9.493843624470073e-18, - "velocityX": -1.4300248011418268, - "velocityY": -0.3083485882454945, - "timestamp": 11.171650358598413 - }, - { - "x": 7.6201153258458145, - "y": 3.928094024739179, - "heading": 7.8541038899261e-18, - "angularVelocity": 8.385205006937927e-18, - "velocityX": -1.7202281903469572, - "velocityY": -0.28276819486653393, - "timestamp": 11.24627083891741 - }, - { - "x": 7.470317035092631, - "y": 3.9106227342866235, - "heading": 8.379134437333569e-18, - "angularVelocity": 7.036011272928214e-18, - "velocityX": -2.00746886260729, - "velocityY": -0.234135325554959, - "timestamp": 11.320891319236406 - }, - { - "x": 7.299538426697331, - "y": 3.8988447632701435, - "heading": 8.78216866852809e-18, - "angularVelocity": 5.401120751271277e-18, - "velocityX": -2.2886291761355184, - "velocityY": -0.15783831685523833, - "timestamp": 11.395511799555402 - }, - { - "x": 7.108600470483675, - "y": 3.8952024104802057, - "heading": 9.038204388282958e-18, - "angularVelocity": 3.4311721461170713e-18, - "velocityX": -2.5587875526586417, - "velocityY": -0.04881170389640219, - "timestamp": 11.470132279874399 - }, - { - "x": 6.89887047602872, - "y": 3.902489232444511, - "heading": 9.11887229683693e-18, - "angularVelocity": 1.0810424542935678e-18, - "velocityX": -2.810622413020914, - "velocityY": 0.09765176977090569, - "timestamp": 11.544752760193395 - }, - { - "x": 6.672447487864324, - "y": 3.9237019834694964, - "heading": 8.994132821099478e-18, - "angularVelocity": -1.6716520183319338e-18, - "velocityX": -3.0343276697825954, - "velocityY": 0.28427518737888835, - "timestamp": 11.619373240512392 - }, - { - "x": 6.4322495960952635, - "y": 3.961732568936921, - "heading": 8.636009892152731e-18, - "angularVelocity": -4.799257891447058e-18, - "velocityX": -3.218927173106323, - "velocityY": 0.5096534531116348, - "timestamp": 11.693993720831388 - }, - { - "x": 6.181871022214133, - "y": 4.01897099744758, - "heading": 8.023682097716734e-18, - "angularVelocity": -8.205894562430798e-18, - "velocityX": -3.3553599871078905, - "velocityY": 0.7670605746032516, - "timestamp": 11.768614201150385 - }, - { - "x": 5.925195916976863, - "y": 4.0970166657243565, - "heading": 7.147197665220672e-18, - "angularVelocity": -1.17458963959573e-17, - "velocityX": -3.4397407272106473, - "velocityY": 1.0459014461329927, - "timestamp": 11.843234681469381 + "angularVelocity": 0.043282930404384794, + "velocityX": 0.26971368660010536, + "velocityY": 0.07999391635030323, + "timestamp": 7.645012828553783 + }, + { + "x": 8.059946431342839, + "y": 4.076566093038108, + "heading": 0.0028549561404960684, + "angularVelocity": 0.03895223157485018, + "velocityX": -0.048164132671898556, + "velocityY": 0.045285389048645555, + "timestamp": 7.718306604675698 + }, + { + "x": 8.033099887642729, + "y": 4.077511417867968, + "heading": 0.00539411397159862, + "angularVelocity": 0.03464356682727019, + "velocityX": -0.3662868134321987, + "velocityY": 0.012897750394093884, + "timestamp": 7.791600380797613 + }, + { + "x": 7.9829172655770035, + "y": 4.07628474252501, + "heading": 0.007619394466085314, + "angularVelocity": 0.03036111130070867, + "velocityX": -0.6846778092351761, + "velocityY": -0.01673641894119354, + "timestamp": 7.864894156919528 + }, + { + "x": 7.909377202284024, + "y": 4.0731292983210805, + "heading": 0.00953312535115696, + "angularVelocity": 0.026110414639960534, + "velocityX": -1.0033602740109082, + "velocityY": -0.04305200756310189, + "timestamp": 7.938187933041442 + }, + { + "x": 7.81245694795258, + "y": 4.068343833996595, + "heading": 0.011138189892816102, + "angularVelocity": 0.021899056462711293, + "velocityX": -1.3223531309156478, + "velocityY": -0.06529155103873027, + "timestamp": 8.011481709163357 + }, + { + "x": 7.692133347423482, + "y": 4.062303858835979, + "heading": 0.012438255979270885, + "angularVelocity": 0.01773774193721827, + "velocityX": -1.641661910404963, + "velocityY": -0.0824077497462929, + "timestamp": 8.084775485285272 + }, + { + "x": 7.548385409224001, + "y": 4.055495143379703, + "heading": 0.013438147372137587, + "angularVelocity": 0.013642241480415802, + "velocityX": -1.9612570917396126, + "velocityY": -0.0928962296191558, + "timestamp": 8.158069261407187 + }, + { + "x": 7.381200809360299, + "y": 4.048569614878887, + "heading": 0.014144485416621113, + "angularVelocity": 0.009637080825370731, + "velocityX": -2.2810204182359235, + "velocityY": -0.09448999447506434, + "timestamp": 8.231363037529102 + }, + { + "x": 7.190592956257051, + "y": 4.04244548860493, + "heading": 0.014566894857523213, + "angularVelocity": 0.00576323752509999, + "velocityX": -2.6006008038962976, + "velocityY": -0.08355588425094046, + "timestamp": 8.304656813651016 + }, + { + "x": 6.9766499613051005, + "y": 4.038503705706744, + "heading": 0.01472053802802316, + "angularVelocity": 0.002096264903098756, + "velocityX": -2.918979022121646, + "velocityY": -0.05378059511668448, + "timestamp": 8.377950589772931 + }, + { + "x": 6.739697889009689, + "y": 4.039020009641981, + "heading": 0.014632337527092197, + "angularVelocity": -0.0012033832283965633, + "velocityX": -3.232908506464046, + "velocityY": 0.007044308023879432, + "timestamp": 8.451244365894846 + }, + { + "x": 6.480981935824527, + "y": 4.048232688232657, + "heading": 0.01435998143770215, + "angularVelocity": -0.003715951118919239, + "velocityX": -3.529848874955213, + "velocityY": 0.12569523741486752, + "timestamp": 8.52453814201676 + }, + { + "x": 6.206211729235204, + "y": 4.074520375352005, + "heading": 0.014063196474504363, + "angularVelocity": -0.004049251913342814, + "velocityX": -3.7488886659663403, + "velocityY": 0.3586619288876869, + "timestamp": 8.597831918138676 + }, + { + "x": 5.93349789020665, + "y": 4.124154607710869, + "heading": 0.014063194566661911, + "angularVelocity": -2.6030074482550523e-8, + "velocityX": -3.7208321559927344, + "velocityY": 0.6771957318218086, + "timestamp": 8.67112569426059 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 1.1380657643949956e-17, - "angularVelocity": 5.673321785133305e-17, - "velocityX": -3.4741693841666152, - "velocityY": 1.3351885220272963, - "timestamp": 11.917855161788378 - }, - { - "x": 5.562354883757379, - "y": 4.239983693882295, - "heading": 1.257506167197437e-17, - "angularVelocity": 4.001149623529015e-17, - "velocityX": -3.4704042300707463, - "velocityY": 1.4516720671542953, - "timestamp": 11.947706682877985 - }, - { - "x": 5.458854001324223, - "y": 4.2867960161311025, - "heading": 1.368919721006296e-17, - "angularVelocity": 3.7322571632702434e-17, - "velocityX": -3.467189565398273, - "velocityY": 1.5681720910732326, - "timestamp": 11.977558203967593 - }, - { - "x": 5.355427026400021, - "y": 4.337086579385366, - "heading": 1.4765466722121167e-17, - "angularVelocity": 3.605409269066309e-17, - "velocityX": -3.46471372811238, - "velocityY": 1.6846901403550338, - "timestamp": 12.007409725057201 - }, - { - "x": 5.252042805813138, - "y": 4.3908559053171485, - "heading": 1.577172278579381e-17, - "angularVelocity": 3.370870306062832e-17, - "velocityX": -3.4632814949879767, - "velocityY": 1.8012256651973733, - "timestamp": 12.037261246146809 - }, - { - "x": 5.148654011351914, - "y": 4.4481042525220715, - "heading": 1.6688127577708658e-17, - "angularVelocity": 3.0698763599309256e-17, - "velocityX": -3.4634347158013203, - "velocityY": 1.917769852768155, - "timestamp": 12.067112767236416 - }, - { - "x": 5.045180193102648, - "y": 4.508830583010288, - "heading": 1.7559230594262742e-17, - "angularVelocity": 2.9181193522489866e-17, - "velocityX": -3.4662829387708096, - "velocityY": 2.034279268581587, - "timestamp": 12.096964288326024 - }, - { - "x": 4.941454797962972, - "y": 4.5730268239608955, - "heading": 1.8097620255598777e-17, - "angularVelocity": 1.8035585476593816e-17, - "velocityX": -3.4747105458483385, - "velocityY": 2.1505182519143307, - "timestamp": 12.126815809415632 - }, - { - "x": 4.836940518620058, - "y": 4.640611448605407, - "heading": 1.8315935161814963e-17, - "angularVelocity": 7.313359523284407e-18, - "velocityX": -3.5011374807060482, - "velocityY": 2.264026159391885, - "timestamp": 12.15666733050524 - }, - { - "x": 4.729371052476075, - "y": 4.706532042565262, - "heading": 1.8014793483289063e-17, - "angularVelocity": -1.0087984381831229e-17, - "velocityX": -3.603483581995116, - "velocityY": 2.2082825783642597, - "timestamp": 12.186518851594847 - }, - { - "x": 4.620024826639761, - "y": 4.769461542081556, - "heading": 1.7990520308788652e-17, - "angularVelocity": -8.131302397490335e-19, - "velocityX": -3.663003486759619, - "velocityY": 2.1080835153221735, - "timestamp": 12.216370372684455 - }, - { - "x": 4.508985011067914, - "y": 4.829352072952906, - "heading": 1.7846349965295797e-17, - "angularVelocity": -4.829581147982843e-18, - "velocityX": -3.7197372702894755, - "velocityY": 2.0062807081612415, - "timestamp": 12.246221893774063 - }, - { - "x": 4.396354195056893, - "y": 4.886148687843668, - "heading": 1.7996479956698872e-17, - "angularVelocity": 5.029224125294331e-18, - "velocityX": -3.7730344015946646, - "velocityY": 1.9026372130341678, - "timestamp": 12.27607341486367 - }, - { - "x": 4.28245727872322, - "y": 4.939704839757233, - "heading": 1.8249745379226758e-17, - "angularVelocity": 8.484171416768941e-18, - "velocityX": -3.8154476615036823, - "velocityY": 1.794084520946214, - "timestamp": 12.305924935953279 + "heading": 0.01406319364211473, + "angularVelocity": -1.2614265905768763e-8, + "velocityX": -3.6503257922039465, + "velocityY": 0.9890944453863133, + "timestamp": 8.744419470382505 + }, + { + "x": 5.5408846553597755, + "y": 4.23596798413579, + "heading": 0.01406319265930284, + "angularVelocity": -2.835157538408451e-8, + "velocityX": -3.6078608681964144, + "velocityY": 1.1342486182256648, + "timestamp": 8.779084630162778 + }, + { + "x": 5.417489605980159, + "y": 4.2802558166388724, + "heading": 0.014063191785023112, + "angularVelocity": -2.5220703836459854e-8, + "velocityX": -3.5596273076991327, + "velocityY": 1.2775891639848806, + "timestamp": 8.813749789943051 + }, + { + "x": 5.295963908009713, + "y": 4.329441746794644, + "heading": 0.014063190996401449, + "angularVelocity": -2.2749690676604448e-8, + "velocityX": -3.5057013653114657, + "velocityY": 1.4188865843266973, + "timestamp": 8.848414949723324 + }, + { + "x": 5.176501898681268, + "y": 4.383447115848144, + "heading": 0.014063190276173353, + "angularVelocity": -2.077671352365433e-8, + "velocityX": -3.446169297521196, + "velocityY": 1.5579149034885322, + "timestamp": 8.883080109503597 + }, + { + "x": 5.05929461402676, + "y": 4.442185556946357, + "heading": 0.01406318961105745, + "angularVelocity": -1.9186869677954374e-8, + "velocityX": -3.381126335416613, + "velocityY": 1.6944517628226294, + "timestamp": 8.91774526928387 + }, + { + "x": 4.944529482696556, + "y": 4.505563132440428, + "heading": 0.014063188990615918, + "angularVelocity": -1.7898129854823097e-8, + "velocityX": -3.310676542604923, + "velocityY": 1.828278764494154, + "timestamp": 8.952410429064143 + }, + { + "x": 4.832390024623587, + "y": 4.573478482189006, + "heading": 0.014063188406478968, + "angularVelocity": -1.685083682180617e-8, + "velocityX": -3.2349326754519327, + "velocityY": 1.9591817888352978, + "timestamp": 8.987075588844416 + }, + { + "x": 4.723055552753515, + "y": 4.64582297983034, + "heading": 0.014063187851803343, + "angularVelocity": -1.600095394099115e-8, + "velocityX": -3.1540160946348355, + "velocityY": 2.086951224223198, + "timestamp": 9.021740748624689 + }, + { + "x": 4.616700862639107, + "y": 4.722480877082754, + "heading": 0.014063187320883878, + "angularVelocity": -1.5315650301874817e-8, + "velocityX": -3.068057115228709, + "velocityY": 2.2113816217294997, + "timestamp": 9.056405908404962 + }, + { + "x": 4.508256477944758, + "y": 4.796152911545187, + "heading": 0.014063186782201262, + "angularVelocity": -1.5539597107094908e-8, + "velocityX": -3.128339386915466, + "velocityY": 2.125247220246655, + "timestamp": 9.091071068185235 + }, + { + "x": 4.396953351691581, + "y": 4.865430375769311, + "heading": 0.014063186216514915, + "angularVelocity": -1.6318584724554417e-8, + "velocityX": -3.2108066704055713, + "velocityY": 1.998475260556751, + "timestamp": 9.125736227965508 + }, + { + "x": 4.28296944450211, + "y": 4.930202424249945, + "heading": 0.014063179833248412, + "angularVelocity": -1.8414069176204727e-7, + "velocityX": -3.288140251248267, + "velocityY": 1.8685056953781265, + "timestamp": 9.16040138774578 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 1.8296114714140825e-17, - "angularVelocity": 1.5533324384911975e-18, - "velocityX": -3.8510001878620828, - "velocityY": 1.6830953424405728, - "timestamp": 12.335776457042886 - }, - { - "x": 3.9173080803752227, - "y": 5.082092147187362, - "heading": 1.4829639745730528e-17, - "angularVelocity": -5.4272726613113606e-17, - "velocityX": -3.9171051476985386, - "velocityY": 1.4426543500365565, - "timestamp": 12.399647855841293 - }, - { - "x": 3.6655222616013243, - "y": 5.158389406763847, - "heading": 1.1586116724078029e-17, - "angularVelocity": -5.078208837148102e-17, - "velocityX": -3.9420745985005192, - "velocityY": 1.1945449921536402, - "timestamp": 12.4635192546397 - }, - { - "x": 3.415686594925907, - "y": 5.218879368068439, - "heading": 9.697216184828002e-18, - "angularVelocity": -2.957349571999372e-17, - "velocityX": -3.911542120189908, - "velocityY": 0.9470586591584124, - "timestamp": 12.527390653438106 - }, - { - "x": 3.1720421714343097, - "y": 5.264694784984283, - "heading": 8.965708374731706e-18, - "angularVelocity": -1.1452822775166807e-17, - "velocityX": -3.814609168973968, - "velocityY": 0.7173072420168656, - "timestamp": 12.591262052236512 - }, - { - "x": 2.938638877133611, - "y": 5.298312127499332, - "heading": 9.343063585543127e-18, - "angularVelocity": 5.9080467348504174e-18, - "velocityX": -3.654269339510983, - "velocityY": 0.5263285781661498, - "timestamp": 12.655133451034919 - }, - { - "x": 2.718391564246048, - "y": 5.322952057954089, - "heading": 1.0698464289017726e-17, - "angularVelocity": 2.12207768493952e-17, - "velocityX": -3.4482932428443296, - "velocityY": 0.3857740854012944, - "timestamp": 12.719004849833325 - }, - { - "x": 2.5129378876059687, - "y": 5.341690959630172, - "heading": 1.2882468345126029e-17, - "angularVelocity": 3.4193772081618774e-17, - "velocityX": -3.216677268780943, - "velocityY": 0.29338486440898187, - "timestamp": 12.782876248631732 - }, - { - "x": 2.323045360609233, - "y": 5.357035089630098, - "heading": 1.4586731405592636e-17, - "angularVelocity": 2.6682726421160203e-17, - "velocityX": -2.973044751940999, - "velocityY": 0.24023475747502873, - "timestamp": 12.846747647430139 - }, - { - "x": 2.149011712470278, - "y": 5.370906179422888, - "heading": 1.3182509626883337e-17, - "angularVelocity": -2.1985142076939443e-17, - "velocityX": -2.724750849566412, - "velocityY": 0.21717216240355022, - "timestamp": 12.910619046228545 - }, - { - "x": 1.9909052012063986, - "y": 5.384762303843791, - "heading": 1.2288275023072503e-17, - "angularVelocity": -1.400054828244982e-17, - "velocityX": -2.4753882682748096, - "velocityY": 0.2169378576573164, - "timestamp": 12.974490445026952 - }, - { - "x": 1.8486875999585186, - "y": 5.399721366691689, - "heading": 1.1833725700471485e-17, - "angularVelocity": -7.116633256521739e-18, - "velocityX": -2.2266241842730174, - "velocityY": 0.23420596901458723, - "timestamp": 13.038361843825358 - }, - { - "x": 1.7222741786291598, - "y": 5.416655739466683, - "heading": 1.1765320590646606e-17, - "angularVelocity": -1.070981842258772e-18, - "velocityX": -1.9791866736526023, - "velocityY": 0.2651323298624305, - "timestamp": 13.102233242623765 - }, - { - "x": 1.6115626522567748, - "y": 5.436259280896622, - "heading": 1.2039434559055613e-17, - "angularVelocity": 4.291654398939878e-18, - "velocityX": -1.7333505834405774, - "velocityY": 0.30692206212380396, - "timestamp": 13.166104641422171 - }, - { - "x": 1.5164470746291163, - "y": 5.459093977101566, - "heading": 9.126879142292297e-18, - "angularVelocity": -4.560030730930113e-17, - "velocityX": -1.4891732358620327, - "velocityY": 0.3575105075906568, - "timestamp": 13.229976040220578 - }, - { - "x": 1.4368243680409203, - "y": 5.485622578468376, - "heading": 6.490550146878691e-18, - "angularVelocity": -4.1275579397551e-17, - "velocityX": -1.2466097202521527, - "velocityY": 0.41534398597625355, - "timestamp": 13.293847439018984 - }, - { - "x": 1.3725972060727116, - "y": 5.516231768358422, - "heading": 4.104803215844785e-18, - "angularVelocity": -3.735235133440495e-17, - "velocityX": -1.0055699918350633, - "velocityY": 0.47923155693921204, - "timestamp": 13.35771883781739 - }, - { - "x": 1.3236750778063309, - "y": 5.551248899658597, - "heading": 1.947639425952167e-18, - "angularVelocity": -3.3773548532536126e-17, - "velocityX": -0.7659473439870959, - "velocityY": 0.5482443152794746, - "timestamp": 13.421590236615797 + "heading": 0.013676554706226966, + "angularVelocity": -0.011153132697846535, + "velocityX": -3.331021112692202, + "velocityY": 1.7234990981340637, + "timestamp": 9.195066547526054 + }, + { + "x": 3.942209538282738, + "y": 5.088292732030669, + "heading": 0.012204358572730108, + "angularVelocity": -0.02138935040268071, + "velocityX": -3.2732028891433576, + "velocityY": 1.4288410708686299, + "timestamp": 9.263895011494382 + }, + { + "x": 3.7316526106102703, + "y": 5.172141912516415, + "heading": 0.010528895655226616, + "angularVelocity": -0.024342587657839698, + "velocityX": -3.05915482538382, + "velocityY": 1.2182340800795661, + "timestamp": 9.33272347546271 + }, + { + "x": 3.53847572791013, + "y": 5.244805689896845, + "heading": 0.00883705460181792, + "angularVelocity": -0.024580543511579803, + "velocityX": -2.806642362221417, + "velocityY": 1.0557227808231526, + "timestamp": 9.40155193943104 + }, + { + "x": 3.3636567049982276, + "y": 5.307973779066906, + "heading": 0.0072129294254924, + "angularVelocity": -0.0235967081449452, + "velocityX": -2.5399233519484916, + "velocityY": 0.9177611343924164, + "timestamp": 9.470380403399368 + }, + { + "x": 3.2076759026568156, + "y": 5.362638919945403, + "heading": 0.005703178674060931, + "angularVelocity": -0.02193497667078823, + "velocityX": -2.2662252409582635, + "velocityY": 0.7942228799941261, + "timestamp": 9.539208867367696 + }, + { + "x": 3.070812323282922, + "y": 5.409448485662979, + "heading": 0.00433725990535669, + "angularVelocity": -0.01984526008502491, + "velocityX": -1.9884735396226798, + "velocityY": 0.6800902274837387, + "timestamp": 9.608037331336025 + }, + { + "x": 2.953246241272587, + "y": 5.448856394346129, + "heading": 0.0031354038045633293, + "angularVelocity": -0.01746161444698809, + "velocityX": -1.7081026545127194, + "velocityY": 0.572552493707886, + "timestamp": 9.676865795304353 + }, + { + "x": 2.855102952665587, + "y": 5.48119799246298, + "heading": 0.002112343078266857, + "angularVelocity": -0.014863919188538589, + "velocityX": -1.4259113591749097, + "velocityY": 0.4698869661210702, + "timestamp": 9.745694259272682 + }, + { + "x": 2.7764742584987756, + "y": 5.506730918230691, + "heading": 0.0012792785012801142, + "angularVelocity": -0.012103489297248894, + "velocityX": -1.1423862982470774, + "velocityY": 0.3709646314271869, + "timestamp": 9.81452272324101 + }, + { + "x": 2.717430148167725, + "y": 5.5256592019268735, + "heading": 0.0006450106297669135, + "angularVelocity": -0.009215197244632127, + "velocityX": -0.8578443702916283, + "velocityY": 0.275006626689982, + "timestamp": 9.883351187209339 + }, + { + "x": 2.6780256614742735, + "y": 5.538148367640626, + "heading": 0.00021663624249770164, + "angularVelocity": -0.006223796995765153, + "velocityX": -0.5725027760547413, + "velocityY": 0.18145350039337144, + "timestamp": 9.952179651177667 }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -3.0493138713173094e-17, - "velocityX": -0.5276325143220081, - "velocityY": 0.6216460705389873, - "timestamp": 13.485461635414204 - }, - { - "x": 1.2740430355469938, - "y": 5.664007440684529, - "heading": -2.547208860482595e-18, - "angularVelocity": -2.5831234528334732e-17, - "velocityX": -0.16156041888930542, - "velocityY": 0.7408315596510565, - "timestamp": 13.584071280199696 - }, - { - "x": 1.2943003151600432, - "y": 5.748531863021803, - "heading": -4.6379232663069386e-18, - "angularVelocity": -2.1201926150692658e-17, - "velocityX": 0.20542898878822005, - "velocityY": 0.8571618173977016, - "timestamp": 13.682680924985188 - }, - { - "x": 1.3508752960803845, - "y": 5.844112348620496, - "heading": -6.276905103665225e-18, - "angularVelocity": -1.6620908041208103e-17, - "velocityX": 0.5737266475648491, - "velocityY": 0.9692813092128183, - "timestamp": 13.78129056977068 - }, - { - "x": 1.4439661676967965, - "y": 5.9500755871992865, - "heading": -7.471930527081998e-18, - "angularVelocity": -1.2118747882558993e-17, - "velocityX": 0.9440341441135343, - "velocityY": 1.0745727642493252, - "timestamp": 13.879900214556173 - }, - { - "x": 1.5739124190504803, - "y": 6.065143361581136, - "heading": -8.237944726800887e-18, - "angularVelocity": -7.768146836290714e-18, - "velocityX": 1.3177843976251842, - "velocityY": 1.1669018241791527, - "timestamp": 13.978509859341665 - }, - { - "x": 1.7413805206507829, - "y": 6.185983632591009, - "heading": -6.8828411912414524e-18, - "angularVelocity": 1.3742099353654783e-17, - "velocityX": 1.698293325816142, - "velocityY": 1.2254406886135676, - "timestamp": 14.077119504127158 - }, - { - "x": 1.9428580652109537, - "y": 6.289954848196762, - "heading": -5.594323946529715e-18, - "angularVelocity": 1.3066848045055797e-17, - "velocityX": 2.0431829462366347, - "velocityY": 1.0543716675171477, - "timestamp": 14.17572914891265 - }, - { - "x": 2.114807562690416, - "y": 6.370065959572112, - "heading": -4.6183818981391425e-18, - "angularVelocity": 9.897024283736211e-18, - "velocityX": 1.7437391428952727, - "velocityY": 0.8124064491826988, - "timestamp": 14.274338793698142 - }, - { - "x": 2.2534346833326833, - "y": 6.431988322349882, - "heading": -2.7955204631360402e-18, - "angularVelocity": 1.8485630277436967e-17, - "velocityX": 1.4058170571836621, - "velocityY": 0.6279544248686071, - "timestamp": 14.372948438483634 - }, - { - "x": 2.357842355716001, - "y": 6.477471673664644, - "heading": -1.484023928114358e-18, - "angularVelocity": 1.3299880944166003e-17, - "velocityX": 1.0587977738935974, - "velocityY": 0.4612464776006573, - "timestamp": 14.471558083269127 - }, - { - "x": 2.427639134257716, - "y": 6.507357014317594, - "heading": -4.934983093022998e-19, - "angularVelocity": 1.0044916213421236e-17, - "velocityX": 0.7078088425685477, - "velocityY": 0.30306711597998603, - "timestamp": 14.570167728054619 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -3.0114656219279604e-34, + "angularVelocity": -0.003147480417366101, + "velocityX": -0.28651653960332735, + "velocityY": 0.08989010211866776, + "timestamp": 10.021008115145996 }, { - "x": 2.462606906890869, - "y": 6.522137641906738, - "heading": 1.7665246335106923e-37, - "angularVelocity": 5.004564321614195e-18, - "velocityX": 0.35460803767440047, - "velocityY": 0.14989028326080325, - "timestamp": 14.668777372840111 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -4.388516089708894e-34, + "angularVelocity": -2.000698896485297e-33, + "velocityX": 9.851967688482278e-36, + "velocityY": -1.337407020908214e-34, + "timestamp": 10.089836579114325 + }, + { + "x": 2.6750735555579293, + "y": 5.538716972759925, + "heading": 4.633169711684461e-19, + "angularVelocity": 7.277213797999712e-18, + "velocityX": 0.2633772294028884, + "velocityY": -0.0882468077495622, + "timestamp": 10.15350338677302 + }, + { + "x": 2.7085920650608806, + "y": 5.52742596598787, + "heading": -4.184429803904338e-20, + "angularVelocity": -7.934452625857426e-18, + "velocityX": 0.5264675697678677, + "velocityY": -0.1773452633683705, + "timestamp": 10.217170194431715 + }, + { + "x": 2.7588390755328, + "y": 5.51039889888726, + "heading": 3.955178903113731e-20, + "angularVelocity": 1.278469740567629e-18, + "velocityX": 0.7892183120172118, + "velocityY": -0.2674402522565472, + "timestamp": 10.28083700209041 + }, + { + "x": 2.8257885982701296, + "y": 5.487560532002736, + "heading": 8.970064045939625e-19, + "angularVelocity": 1.3467843717867433e-17, + "velocityX": 1.0515608556382, + "velocityY": -0.3587170100777762, + "timestamp": 10.344503809749105 + }, + { + "x": 2.9094088162269203, + "y": 5.4588202139112365, + "heading": 2.18904914594727e-18, + "angularVelocity": 2.0293820106069983e-17, + "velocityX": 1.313403656188678, + "velocityY": -0.4514176090871547, + "timestamp": 10.4081706174078 + }, + { + "x": 3.0096598962036025, + "y": 5.424066617313393, + "heading": 3.904306808877207e-18, + "angularVelocity": 2.694116017446798e-17, + "velocityX": 1.5746208057754196, + "velocityY": -0.5458668005493709, + "timestamp": 10.471837425066495 + }, + { + "x": 3.126490562172417, + "y": 5.383159777949491, + "heading": 4.8450172295989666e-18, + "angularVelocity": 1.4775523625508697e-17, + "velocityX": 1.8350325745107399, + "velocityY": -0.6425143786570184, + "timestamp": 10.53550423272519 + }, + { + "x": 3.2598324200617776, + "y": 5.335918462965805, + "heading": 4.235614277481238e-18, + "angularVelocity": -9.5717529200556e-18, + "velocityX": 2.0943700931917317, + "velocityY": -0.7420085397863493, + "timestamp": 10.599171040383885 + }, + { + "x": 3.4095898607657253, + "y": 5.282098877479993, + "heading": 2.3874123899446895e-18, + "angularVelocity": -2.902928473254694e-17, + "velocityX": 2.3522059014921575, + "velocityY": -0.8453319314253612, + "timestamp": 10.66283784804258 + }, + { + "x": 3.5756203018815462, + "y": 5.22135582426949, + "heading": 2.5233707069051096e-18, + "angularVelocity": 2.135466217958737e-18, + "velocityX": 2.607802200573299, + "velocityY": -0.9540772569615117, + "timestamp": 10.726504655701275 + }, + { + "x": 3.757690030648967, + "y": 5.153163870724839, + "heading": 3.693577308512614e-18, + "angularVelocity": 1.8380167698697197e-17, + "velocityX": 2.8597276267323055, + "velocityY": -1.0710754324327665, + "timestamp": 10.79017146335997 + }, + { + "x": 3.955353841452516, + "y": 5.076631094268888, + "heading": 4.677490606421779e-18, + "angularVelocity": 1.5454101345613767e-17, + "velocityX": 3.1046603100187773, + "velocityY": -1.2020828320186725, + "timestamp": 10.853838271018665 }, { - "x": 2.462606906890869, - "y": 6.522137641906738, - "heading": -7.406725507450864e-38, - "angularVelocity": -2.5425466979238586e-36, - "velocityX": -6.628025513542663e-35, - "velocityY": 2.827272125617884e-34, - "timestamp": 14.767387017625603 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 5.777516683198087e-18, + "angularVelocity": 1.7277858231456074e-17, + "velocityX": 3.332116557247906, + "velocityY": -1.36151476081006, + "timestamp": 10.91750507867736 + }, + { + "x": 4.285701077560922, + "y": 4.938388492372474, + "heading": 6.5114072312405955e-18, + "angularVelocity": 2.119278869815072e-17, + "velocityX": 3.413356765694629, + "velocityY": -1.4888942599373958, + "timestamp": 10.952134336699762 + }, + { + "x": 4.403715149774036, + "y": 4.881600772971698, + "heading": 6.4908057437571035e-18, + "angularVelocity": -5.949156482118572e-19, + "velocityX": 3.40792956455403, + "velocityY": -1.6398768741750078, + "timestamp": 10.986763594722165 + }, + { + "x": 4.519367006147829, + "y": 4.8201449517293415, + "heading": 6.136184222547225e-18, + "angularVelocity": -1.0240517454358069e-17, + "velocityX": 3.3397151131270637, + "velocityY": -1.7746791225673975, + "timestamp": 11.021392852744567 + }, + { + "x": 4.6324721141561005, + "y": 4.754119165100519, + "heading": 6.684498529619524e-18, + "angularVelocity": 1.5833845088958816e-17, + "velocityX": 3.2661718577713303, + "velocityY": -1.906647453610145, + "timestamp": 11.05602211076697 + }, + { + "x": 4.742850082889288, + "y": 4.683628924272633, + "heading": 1.0669160659402455e-17, + "angularVelocity": 1.1506634439597858e-16, + "velocityX": 3.187419397256002, + "velocityY": -2.03556890483432, + "timestamp": 11.090651368789372 + }, + { + "x": 4.850325220314879, + "y": 4.608787325463071, + "heading": 1.658478005292643e-17, + "angularVelocity": 1.708272059914504e-16, + "velocityX": 3.1035934225348787, + "velocityY": -2.161224440937961, + "timestamp": 11.125280626811774 + }, + { + "x": 4.959315849281407, + "y": 4.536170434444089, + "heading": 2.4155748869411193e-17, + "angularVelocity": 2.1862925308959841e-16, + "velocityX": 3.1473567494868284, + "velocityY": -2.0969808527807587, + "timestamp": 11.159909884834176 + }, + { + "x": 5.0711194796065495, + "y": 4.467963995966004, + "heading": 2.3436163975709297e-17, + "angularVelocity": -2.0779679808224236e-17, + "velocityX": 3.228588676454313, + "velocityY": -1.969618824462286, + "timestamp": 11.194539142856579 + }, + { + "x": 5.1855579490393096, + "y": 4.4042772106470105, + "heading": 2.204718666214906e-17, + "angularVelocity": -4.0109935727230666e-17, + "velocityX": 3.3046757559381796, + "velocityY": -1.8391033754691897, + "timestamp": 11.229168400878981 + }, + { + "x": 5.302448702578612, + "y": 4.345211810292891, + "heading": 2.1117777344899902e-17, + "angularVelocity": -2.6838845829382515e-17, + "velocityX": 3.375491137109673, + "velocityY": -1.7056501850518673, + "timestamp": 11.263797658901384 + }, + { + "x": 5.421605230513571, + "y": 4.290862095789443, + "heading": 1.9153982489912048e-17, + "angularVelocity": -5.670912306921066e-17, + "velocityX": 3.440920618566971, + "velocityY": -1.569473838229202, + "timestamp": 11.298426916923786 + }, + { + "x": 5.542837392691082, + "y": 4.241314822773421, + "heading": 1.5300631909411777e-17, + "angularVelocity": -1.112744193943597e-16, + "velocityX": 3.5008593628856843, + "velocityY": -1.430792221536174, + "timestamp": 11.333056174946188 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 9.183754863330666e-18, + "angularVelocity": -1.766389866662468e-16, + "velocityX": 3.555211493416193, + "velocityY": -1.2898268911821011, + "timestamp": 11.36768543296859 + }, + { + "x": 5.934272395491136, + "y": 4.124337934678792, + "heading": -1.4789774171207362e-18, + "angularVelocity": -1.4511299214964166e-16, + "velocityX": 3.651673302114514, + "velocityY": -0.9841085377661543, + "timestamp": 11.441164258830412 + }, + { + "x": 6.207753017493673, + "y": 4.075010140740357, + "heading": -1.5184517487424443e-17, + "angularVelocity": -1.8652366732257328e-16, + "velocityX": 3.7218970063133985, + "velocityY": -0.6713198443208211, + "timestamp": 11.514643084692233 + }, + { + "x": 6.4844286250858945, + "y": 4.049019985828209, + "heading": -2.210208594102609e-17, + "angularVelocity": -9.414369884747846e-17, + "velocityX": 3.7653787243758563, + "velocityY": -0.35370944768528717, + "timestamp": 11.588121910554054 + }, + { + "x": 6.743277363747609, + "y": 4.038426394199003, + "heading": -1.998169117841033e-17, + "angularVelocity": 2.885722162467868e-17, + "velocityX": 3.522766397335807, + "velocityY": -0.14417203194192602, + "timestamp": 11.661600736415876 + }, + { + "x": 6.978672822837657, + "y": 4.030025299692623, + "heading": -1.7754526303263836e-17, + "angularVelocity": 3.031029482336482e-17, + "velocityX": 3.2035822065626665, + "velocityY": -0.1143335431376982, + "timestamp": 11.735079562277697 + }, + { + "x": 7.19055269256627, + "y": 4.022995759457438, + "heading": -1.4995601737739213e-17, + "angularVelocity": 3.754720537740805e-17, + "velocityX": 2.8835500192539434, + "velocityY": -0.095667563447506, + "timestamp": 11.808558388139518 + }, + { + "x": 7.3789017577018114, + "y": 4.017045849522277, + "heading": -1.2218093470845859e-17, + "angularVelocity": 3.780011771168623e-17, + "velocityX": 2.5633107623376494, + "velocityY": -0.08097448299391073, + "timestamp": 11.88203721400134 + }, + { + "x": 7.54371362899692, + "y": 4.012025997027571, + "heading": -9.378231648873842e-18, + "angularVelocity": 3.864870986523972e-17, + "velocityX": 2.2429845518359373, + "velocityY": -0.06831699385270533, + "timestamp": 11.955516039863161 + }, + { + "x": 7.684984887810538, + "y": 4.007845278835319, + "heading": -6.610589160747635e-18, + "angularVelocity": 3.7665850749041794e-17, + "velocityX": 1.9226118158077508, + "velocityY": -0.05689691068436128, + "timestamp": 12.028994865724982 + }, + { + "x": 7.8027134339680275, + "y": 4.0044425825275765, + "heading": -4.4053913927362146e-18, + "angularVelocity": 3.0011336492479294e-17, + "velocityX": 1.6022104977409417, + "velocityY": -0.046308528584020826, + "timestamp": 12.102473691586804 + }, + { + "x": 7.896897856926731, + "y": 4.001774011196325, + "heading": -2.659547727350374e-18, + "angularVelocity": 2.375981985162547e-17, + "velocityX": 1.281789983087365, + "velocityY": -0.03631755543113615, + "timestamp": 12.175952517448625 + }, + { + "x": 7.967537148527336, + "y": 3.999806507404438, + "heading": -1.3934136908294963e-18, + "angularVelocity": 1.723127746899314e-17, + "velocityX": 0.9613557480279207, + "velocityY": -0.026776472933672534, + "timestamp": 12.249431343310446 + }, + { + "x": 8.014630554462236, + "y": 3.9985142796609896, + "heading": -4.425448526284405e-19, + "angularVelocity": 1.2940719003719293e-17, + "velocityX": 0.6409112473226126, + "velocityY": -0.017586396193627105, + "timestamp": 12.322910169172268 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 3.3992773241588707e-43, + "angularVelocity": 6.022753459080235e-18, + "velocityX": 0.320458791984776, + "velocityY": -0.008677813220192955, + "timestamp": 12.396388995034089 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": -7.143469642081013e-44, + "angularVelocity": -5.598380381353948e-42, + "velocityX": -5.65394748040853e-41, + "velocityY": -2.108446218115532e-39, + "timestamp": 12.46986782089591 } ], "constraints": [ @@ -18594,21 +13092,34 @@ }, { "scope": [ - 2 + 3 ], "type": "StopPoint" }, { "scope": [ - 4 + 5 ], "type": "StopPoint" }, { "scope": [ - 6 + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 12 ], "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "WptVelocityDirection", + "direction": 0.5125504196 } ], "usesControlIntervalGuessing": true, @@ -18616,102 +13127,93 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "SourceLanePSubHSubGSub": { + "CenterLanePCBADSprint": { "waypoints": [ { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 25 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 22 + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 7.340087890625, - "y": 0.7580231428146362, - "heading": 0, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 16 }, { - "x": 7.841280937194824, - "y": 0.7580231428146362, + "x": 1.8129411935806274, + "y": 5.542999267578125, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 24 + "controlIntervalCount": 9 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, + "x": 2.6583051681518555, + "y": 5.54433536529541, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 25 + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 25 + "controlIntervalCount": 6 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": 0, + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, + "headingConstrained": true, "controlIntervalCount": 24 }, { - "x": 7.481724262237549, - "y": 2.2045881748199463, - "heading": 0.23374329695765314, + "x": 7.374384880065918, + "y": 7.465639114379883, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 8 }, { - "x": 7.975332260131836, - "y": 2.3279902935028076, - "heading": 0.23374329695765314, + "x": 7.923961639404297, + "y": 7.465639114379883, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 25 + "controlIntervalCount": 23 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": 0, + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 25 + "headingConstrained": true, + "controlIntervalCount": 20 }, { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -18720,5996 +13222,1484 @@ ], "trajectory": [ { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": -1.2693923486855146e-35, - "velocityY": 4.115755350063261e-34, + "x": 1.289974451065063, + "y": 5.590954303741456, + "heading": 6.19779206460361e-17, + "angularVelocity": 3.0320144312848013e-18, + "velocityX": 2.9896394667646305e-17, + "velocityY": -6.392586992550233e-17, "timestamp": 0 }, { - "x": 0.6686935325209526, - "y": 4.399440636010588, - "heading": -1.042618245961612, - "angularVelocity": 0.09045080045353641, - "velocityX": 0.09617884019158171, - "velocityY": -0.1870085992810291, - "timestamp": 0.05417149176692242 - }, - { - "x": 0.6791748263382973, - "y": 4.379207943091365, - "heading": -1.0329282635016275, - "angularVelocity": 0.17887604981743244, - "velocityX": 0.19348357365607174, - "velocityY": -0.37349336817742057, - "timestamp": 0.10834298353384483 - }, - { - "x": 0.6949946113692655, - "y": 4.348905021041836, - "heading": -1.0185695679650135, - "angularVelocity": 0.26505999868701274, - "velocityX": 0.2920315559895287, - "velocityY": -0.5593887312520465, - "timestamp": 0.16251447530076724 - }, - { - "x": 0.7162276376127428, - "y": 4.308568047404174, - "heading": -0.9996771897134135, - "angularVelocity": 0.3487513013835064, - "velocityX": 0.3919594153846487, - "velocityY": -0.7446162607301866, - "timestamp": 0.21668596706768967 - }, - { - "x": 0.7429573380312586, - "y": 4.258238333670889, - "heading": -0.9764021455676966, - "angularVelocity": 0.4296548495629363, - "velocityX": 0.49342743843058046, - "velocityY": -0.9290811844324458, - "timestamp": 0.2708574588346121 - }, - { - "x": 0.775277417821335, - "y": 4.197963466887649, - "heading": -0.9489143945994782, - "angularVelocity": 0.5074209712829532, - "velocityX": 0.5966252494787523, - "velocityY": -1.1126676563122382, - "timestamp": 0.3250289506015345 - }, - { - "x": 0.8132938470478027, - "y": 4.127798804086614, - "heading": -0.91740659080495, - "angularVelocity": 0.5816307206398024, - "velocityX": 0.701779256699018, - "velocityY": -1.2952322432419845, - "timestamp": 0.37920044236845696 - }, - { - "x": 0.8571273890255799, - "y": 4.047809461983153, - "heading": -0.8820989485703652, - "angularVelocity": 0.651775335752228, - "velocityX": 0.809162541921033, - "velocityY": -1.4765947825033474, - "timestamp": 0.4333719341353794 - }, - { - "x": 0.9069168499797303, - "y": 3.95807301716377, - "heading": -0.843245694489665, - "angularVelocity": 0.7172269548689871, - "velocityX": 0.9191081753549234, - "velocityY": -1.6565252661950285, - "timestamp": 0.4875434259023018 - }, - { - "x": 0.9628233134219261, - "y": 3.8586832511299094, - "heading": -0.8011438194850887, - "angularVelocity": 0.7771961530194386, - "velocityX": 1.0320273933518045, - "velocityY": -1.8347245533036767, - "timestamp": 0.5417149176692242 - }, - { - "x": 1.025035737811962, - "y": 3.749755475919721, - "heading": -0.7561452166136415, - "angularVelocity": 0.8306694426112097, - "velocityX": 1.1484347645014206, - "velocityY": -2.0107951924023055, - "timestamp": 0.5958864094361466 - }, - { - "x": 1.0937784660640726, - "y": 3.631434328521385, - "heading": -0.7086738761047954, - "angularVelocity": 0.8763159174773277, - "velocityX": 1.268983482084674, - "velocityY": -2.1841958480195354, - "timestamp": 0.6500579012030689 - }, - { - "x": 1.1693214408917312, - "y": 3.5039055633996625, - "heading": -0.6592507614486313, - "angularVelocity": 0.9123454614986682, - "velocityX": 1.3945153135654589, - "velocityY": -2.3541674958929617, - "timestamp": 0.7042293929699913 - }, - { - "x": 1.2519942470920469, - "y": 3.3674145874122954, - "heading": -0.6085305941721398, - "angularVelocity": 0.9362889154819557, - "velocityX": 1.5261312454902076, - "velocityY": -2.519608959166787, - "timestamp": 0.7584008847369137 - }, - { - "x": 1.3422054245010013, - "y": 3.222296884123663, - "heading": -0.557357603065156, - "angularVelocity": 0.9446479954282978, - "velocityX": 1.665288779513328, - "velocityY": -2.678857431377627, - "timestamp": 0.8125723765038361 - }, - { - "x": 1.4404683178656266, - "y": 3.0690304198840344, - "heading": -0.506852671522414, - "angularVelocity": 0.9323156866353964, - "velocityX": 1.813922603191545, - "velocityY": -2.82928269539025, - "timestamp": 0.8667438682707584 - }, - { - "x": 1.5474318621807708, - "y": 2.9083305512073854, - "heading": -0.4585562938775888, - "angularVelocity": 0.8915460156169343, - "velocityX": 1.974535698137392, - "velocityY": -2.9665025539277043, - "timestamp": 0.9209153600376808 - }, - { - "x": 1.6638998489942354, - "y": 2.7413289476125455, - "heading": -0.41468556937766105, - "angularVelocity": 0.8098489273414394, - "velocityX": 2.1499866999154866, - "velocityY": -3.082831913017614, - "timestamp": 0.9750868518046032 - }, - { - "x": 1.7907578946957254, - "y": 2.5699019528024616, - "heading": -0.37863644859559736, - "angularVelocity": 0.6654629511989103, - "velocityX": 2.3417860864402265, - "velocityY": -3.164524166099463, - "timestamp": 1.0292583435715257 - }, - { - "x": 1.9285624590284545, - "y": 2.3970723087204746, - "heading": -0.35526123126492787, - "angularVelocity": 0.43150403594649805, - "velocityX": 2.543857660882686, - "velocityY": -3.1904169230857033, - "timestamp": 1.0834298353384482 - }, - { - "x": 2.076969770307468, - "y": 2.2268102881561207, - "heading": -0.3460628874086392, - "angularVelocity": 0.16980045326913482, - "velocityX": 2.7395832464342806, - "velocityY": -3.1430188649210824, - "timestamp": 1.1376013271053707 - }, - { - "x": 2.235228147006563, - "y": 2.062095599555062, - "heading": -0.34448101087817407, - "angularVelocity": 0.02920127319497364, - "velocityX": 2.9214328706326906, - "velocityY": -3.0406157044697584, - "timestamp": 1.1917728188722931 - }, - { - "x": 2.4019056025260537, - "y": 1.9051420100449306, - "heading": -0.3444808874915784, - "angularVelocity": 0.0000022777034862099955, - "velocityX": 3.076848173881473, - "velocityY": -2.897346637331644, - "timestamp": 1.2459443106392156 - }, - { - "x": 2.5762261949471483, - "y": 1.7567232022649006, - "heading": -0.34448083068467217, - "angularVelocity": 0.0000010486494725236109, - "velocityX": 3.217939671499618, - "velocityY": -2.7397954706253063, - "timestamp": 1.300115802406138 + "x": 1.3085927366636296, + "y": 5.570883327334207, + "heading": -0.01041389258493421, + "angularVelocity": -0.12226255374555442, + "velocityX": 0.21858484952476373, + "velocityY": -0.23563992154741434, + "timestamp": 0.08517646872162599 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.34448076255255106, - "angularVelocity": 0.000001257711738653551, - "velocityX": 3.3509724640087613, - "velocityY": -2.575382967934733, - "timestamp": 1.3542872941730606 - }, - { - "x": 2.9607343532453942, - "y": 1.47791150359439, - "heading": -0.34448070266933767, - "angularVelocity": 0.0000010280378855043595, - "velocityX": 3.484651653320559, - "velocityY": -2.391405084272886, - "timestamp": 1.4125373005573685 - }, - { - "x": 3.170914415879318, - "y": 1.3497321947844976, - "heading": -0.344480652877576, - "angularVelocity": 8.547940977947146e-7, - "velocityX": 3.608241023138232, - "velocityY": -2.2005029143554395, - "timestamp": 1.4707873069416764 - }, - { - "x": 3.387684985247893, - "y": 1.2330440779434721, - "heading": -0.34448061027247806, - "angularVelocity": 7.314179104628634e-7, - "velocityX": 3.721382757255318, - "velocityY": -2.003229254108037, - "timestamp": 1.5290373133259842 - }, - { - "x": 3.6104184041129233, - "y": 1.1281850184409496, - "heading": -0.3444805729289611, - "angularVelocity": 6.41090349411582e-7, - "velocityX": 3.8237492609963484, - "velocityY": -1.8001553306399407, - "timestamp": 1.5872873197102921 - }, - { - "x": 3.8384697501167597, - "y": 1.0354586289592775, - "heading": -0.34448053951161883, - "angularVelocity": 5.736882162731552e-7, - "velocityX": 3.915044137493387, - "velocityY": -1.5918691728530348, - "timestamp": 1.6455373260946 - }, - { - "x": 4.071178703094687, - "y": 0.9551333894958018, - "heading": -0.3444805090563753, - "angularVelocity": 5.228367414213026e-7, - "velocityX": 3.99500304673988, - "velocityY": -1.3789739169044062, - "timestamp": 1.703787332478908 - }, - { - "x": 4.307871457008001, - "y": 0.8874418681856828, - "heading": -0.3444804808411649, - "angularVelocity": 4.843812402300459e-7, - "velocityX": 4.063394471611187, - "velocityY": -1.1620860753820388, - "timestamp": 1.7620373388632158 - }, - { - "x": 4.547862670970626, - "y": 0.8325800440403511, - "heading": -0.3444804543055099, - "angularVelocity": 4.5554767570145907e-7, - "velocityX": 4.120020388998225, - "velocityY": -0.9418337876802539, - "timestamp": 1.8202873452475237 - }, - { - "x": 4.7904574537733415, - "y": 0.7907067300496627, - "heading": -0.34448042899819054, - "angularVelocity": 4.344603701427382e-7, - "velocityX": 4.1647168448735075, - "velocityY": -0.7188550970179598, - "timestamp": 1.8785373516318316 - }, - { - "x": 5.03495337639213, - "y": 0.7619430850125104, - "heading": -0.3444804045415982, - "angularVelocity": 4.1985561744498176e-7, - "velocityX": 4.1973544347053195, - "velocityY": -0.49379642720350075, - "timestamp": 1.9367873580161394 - }, - { - "x": 5.280642508082443, - "y": 0.7463721419648577, - "heading": -0.344480380606166, - "angularVelocity": 4.109086623403158e-7, - "velocityX": 4.217838708366216, - "velocityY": -0.2673122977004072, - "timestamp": 1.9950373644004473 - }, - { - "x": 5.5268134912780935, - "y": 0.7440374377079143, - "heading": -0.3444803568901529, - "angularVelocity": 4.071418107502121e-7, - "velocityX": 4.2261108363065745, - "velocityY": -0.04008075538292679, - "timestamp": 2.053287370784755 - }, - { - "x": 5.766738904442569, - "y": 0.7458615462006624, - "heading": -0.3246781449807404, - "angularVelocity": 0.3399520985245257, - "velocityX": 4.118890761685976, - "velocityY": 0.03131516382527713, - "timestamp": 2.111537377169063 - }, - { - "x": 5.994439434226097, - "y": 0.7477938306057788, - "heading": -0.2833891296763232, - "angularVelocity": 0.7088242193830927, - "velocityX": 3.909021542096679, - "velocityY": 0.033172260829777846, - "timestamp": 2.1697873835533708 - }, - { - "x": 6.2089250785606716, - "y": 0.7495883833677881, - "heading": -0.23844348738786494, - "angularVelocity": 0.7715989246752472, - "velocityX": 3.682156580713365, - "velocityY": 0.030807769361768905, - "timestamp": 2.2280373899376786 - }, - { - "x": 6.410164853990332, - "y": 0.7512394694030761, - "heading": -0.19387042094561185, - "angularVelocity": 0.7652027735100942, - "velocityX": 3.4547597145649904, - "velocityY": 0.02834482153349081, - "timestamp": 2.2862873963219865 - }, - { - "x": 6.598169970341291, - "y": 0.7527437994565946, - "heading": -0.15150652534345024, - "angularVelocity": 0.7272770980085961, - "velocityX": 3.227555291763987, - "velocityY": 0.025825405813585094, - "timestamp": 2.3445374027062944 - }, - { - "x": 6.772954131152115, - "y": 0.7540995948883421, - "heading": -0.11241236663435353, - "angularVelocity": 0.6711442819623177, - "velocityX": 3.000586122818176, - "velocityY": 0.02327545550471737, - "timestamp": 2.4027874090906023 - }, - { - "x": 6.934529146268428, - "y": 0.7553058862941308, - "heading": -0.07728196459640024, - "angularVelocity": 0.6030969645939325, - "velocityX": 2.773819697981016, - "velocityY": 0.020708863065698472, - "timestamp": 2.46103741547491 - }, - { - "x": 7.082904710855205, - "y": 0.7563621191254241, - "heading": -0.046606183781962664, - "angularVelocity": 0.5266227888809539, - "velocityX": 2.5472197137260486, - "velocityY": 0.018132750481171057, - "timestamp": 2.519287421859218 - }, - { - "x": 7.2180887691200715, - "y": 0.7572679503200676, - "heading": -0.02075102525586853, - "angularVelocity": 0.44386533377375426, - "velocityX": 2.3207561107029173, - "velocityY": 0.01555074841824514, - "timestamp": 2.577537428243526 + "x": 1.3442657042493709, + "y": 5.532508491008622, + "heading": -0.03089006103804589, + "angularVelocity": -0.24039701058788923, + "velocityX": 0.4188124739278457, + "velocityY": -0.45053330927585317, + "timestamp": 0.17035293744325197 }, { - "x": 7.340087890625, - "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": 0.35624073788013705, - "velocityX": 2.094405289847217, - "velocityY": 0.012964676597393893, - "timestamp": 2.635787434627834 - }, - { - "x": 7.452016076049052, - "y": 0.7586421586100276, - "heading": 0.015744593535721572, - "angularVelocity": 0.2617476388383187, - "velocityX": 1.8607618029473847, - "velocityY": 0.010290892710547365, - "timestamp": 2.6959392419396355 - }, - { - "x": 7.549878174356822, - "y": 0.7590935431078216, - "heading": 0.02631444920223928, - "angularVelocity": 0.17571966893244026, - "velocityX": 1.6269186693010698, - "velocityY": 0.007504088704338438, - "timestamp": 2.756091049251437 - }, - { - "x": 7.633665269428441, - "y": 0.7593717238160655, - "heading": 0.03212207455289435, - "angularVelocity": 0.09654947390941697, - "velocityX": 1.3929273086892113, - "velocityY": 0.004624644223938123, - "timestamp": 2.816242856563239 - }, - { - "x": 7.703370541137993, - "y": 0.7594720412293507, - "heading": 0.03350861659317102, - "angularVelocity": 0.023050712891957155, - "velocityX": 1.158822566182069, - "velocityY": 0.0016677373094563682, - "timestamp": 2.8763946638750406 - }, - { - "x": 7.758988643067764, - "y": 0.759390536759096, - "heading": 0.030761021907964082, - "angularVelocity": -0.04567767467009867, - "velocityX": 0.9246289415955516, - "velocityY": -0.001354979574134501, - "timestamp": 2.9365464711868423 - }, - { - "x": 7.800515297323124, - "y": 0.759123802858703, - "heading": 0.024124062565761484, - "angularVelocity": -0.11033682342742264, - "velocityX": 0.6903641987031875, - "velocityY": -0.004434345571870156, - "timestamp": 2.996698278498644 - }, - { - "x": 7.827947021203066, - "y": 0.7586688736183343, - "heading": 0.013809003674597087, - "angularVelocity": -0.17148377334192952, - "velocityX": 0.4560415572843554, - "velocityY": -0.007563018647310543, - "timestamp": 3.0568500858104457 + "x": 1.3955881762749254, + "y": 5.477434538773948, + "heading": -0.060869932572704226, + "angularVelocity": -0.35197363760921707, + "velocityX": 0.6025428477586634, + "velocityY": -0.646586469963527, + "timestamp": 0.25552940616487796 }, { - "x": 7.841280937194824, - "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": -0.22956922313268238, - "velocityX": 0.2216710783541506, - "velocityY": -0.010735019154968724, - "timestamp": 3.1170018931222474 - }, - { - "x": 7.835826711308599, - "y": 0.7569095792763894, - "heading": -0.022558535214212153, - "angularVelocity": -0.29904118534160484, - "velocityX": -0.07230248589501269, - "velocityY": -0.014761657052128235, - "timestamp": 3.19243810833268 - }, - { - "x": 7.808194016867113, - "y": 0.7554921769260273, - "heading": -0.05020545199265331, - "angularVelocity": -0.366493953883011, - "velocityX": -0.3663054192790896, - "velocityY": -0.018789414956835414, - "timestamp": 3.2678743235431122 - }, - { - "x": 7.758380379628707, - "y": 0.7537708581865793, - "heading": -0.082765542477478, - "angularVelocity": -0.4316241263429897, - "velocityX": -0.6603411517856365, - "velocityY": -0.022818201239900495, - "timestamp": 3.3433105387535447 - }, - { - "x": 7.686383023957851, - "y": 0.7517455702312359, - "heading": -0.1200350345811684, - "angularVelocity": -0.4940530486547548, - "velocityX": -0.9544136787617978, - "velocityY": -0.026847687807424973, - "timestamp": 3.418746753963977 - }, - { - "x": 7.5921988218888945, - "y": 0.7494163084029398, - "heading": -0.1617736418535849, - "angularVelocity": -0.5532966779415547, - "velocityX": -1.2485276707775705, - "velocityY": -0.030877236109982744, - "timestamp": 3.4941829691744095 - }, - { - "x": 7.475824234031671, - "y": 0.7467831468197799, - "heading": -0.2076930912246962, - "angularVelocity": -0.6087188924181447, - "velocityX": -1.5426885817719311, - "velocityY": -0.03490580188593299, - "timestamp": 3.569619184384842 - }, - { - "x": 7.337255246576761, - "y": 0.7438462777509637, - "heading": -0.25744001302414304, - "angularVelocity": -0.659456756422306, - "velocityX": -1.8369027007567336, - "velocityY": -0.03893181889658235, - "timestamp": 3.6450553995952744 - }, - { - "x": 7.17648732101356, - "y": 0.7406060606730164, - "heading": -0.3105694414767273, - "angularVelocity": -0.7042960507016083, - "velocityX": -2.1311769832928764, - "velocityY": -0.04295307060287463, - "timestamp": 3.720491614805707 - }, - { - "x": 6.9935154095251395, - "y": 0.7370630794965956, - "heading": -0.3665018755155811, - "angularVelocity": -0.7414533441640445, - "velocityX": -2.425518180863289, - "velocityY": -0.04696658185378862, - "timestamp": 3.795927830016139 - }, - { - "x": 6.788334200415596, - "y": 0.7332181989405221, - "heading": -0.4244496104660046, - "angularVelocity": -0.7681686413982437, - "velocityX": -2.719929791509048, - "velocityY": -0.050968630191055964, - "timestamp": 3.8713640452265716 - }, - { - "x": 6.5609391315868795, - "y": 0.7290725862923455, - "heading": -0.4832803161020974, - "angularVelocity": -0.7798735060074524, - "velocityX": -3.0144018783867756, - "velocityY": -0.05495520469329157, - "timestamp": 3.946800260437004 - }, - { - "x": 6.311330163631764, - "y": 0.7246275712093064, - "heading": -0.5412355988933977, - "angularVelocity": -0.7682686973310038, - "velocityX": -3.3088744876558285, - "velocityY": -0.05892415295013936, - "timestamp": 4.0222364756474365 - }, - { - "x": 6.039527438794169, - "y": 0.7198837950524889, - "heading": -0.5952481631436163, - "angularVelocity": -0.7160031040733982, - "velocityX": -3.603080086658513, - "velocityY": -0.06288459917540226, - "timestamp": 4.097672690857869 - }, - { - "x": 5.745659598268619, - "y": 0.7148372711288513, - "heading": -0.638783130826649, - "angularVelocity": -0.5771096490139386, - "velocityX": -3.8955803880906834, - "velocityY": -0.06689789393012663, - "timestamp": 4.173108906068301 - }, - { - "x": 5.431003009987887, - "y": 0.7096634209299225, - "heading": -0.6507371973275397, - "angularVelocity": -0.15846588362823302, - "velocityX": -4.171160859581617, - "velocityY": -0.06858576062566446, - "timestamp": 4.248545121278734 - }, - { - "x": 5.112561537797849, - "y": 0.7251155992021675, - "heading": -0.6507372121831845, - "angularVelocity": -1.969298784568896e-7, - "velocityX": -4.221334160280071, - "velocityY": 0.20483766622093208, - "timestamp": 4.323981336489166 - }, - { - "x": 4.795969323317984, - "y": 0.7627069594521951, - "heading": -0.6507372273638599, - "angularVelocity": -2.0123856138904834e-7, - "velocityX": -4.196819970311576, - "velocityY": 0.49831980760388084, - "timestamp": 4.399417551699599 - }, - { - "x": 4.48276376848833, - "y": 0.8222554301685094, - "heading": -0.6507372432084315, - "angularVelocity": -2.1003932248991623e-7, - "velocityX": -4.15192562293793, - "velocityY": 0.789388366717515, - "timestamp": 4.474853766910031 - }, - { - "x": 4.174465835671775, - "y": 0.9034719526361538, - "heading": -0.6507372601213973, - "angularVelocity": -2.242022056467609e-7, - "velocityX": -4.0868690450143115, - "velocityY": 1.076625096329228, - "timestamp": 4.550289982120463 - }, - { - "x": 3.872572656476166, - "y": 1.0059621755922359, - "heading": -0.650737278622818, - "angularVelocity": -2.4525913278926714e-7, - "velocityX": -4.001966142567798, - "velocityY": 1.3586342139538858, - "timestamp": 4.625726197330896 - }, - { - "x": 3.5785502607652493, - "y": 1.1292284170855964, - "heading": -0.6507372994260644, - "angularVelocity": -2.7577266985991995e-7, - "velocityX": -3.8976292075461214, - "velocityY": 1.6340459439740451, - "timestamp": 4.701162412541328 - }, - { - "x": 3.2938264573018237, - "y": 1.2726720943591394, - "heading": -0.6507373235725763, - "angularVelocity": -3.200917719298211e-7, - "velocityX": -3.7743649077459236, - "velocityY": 1.9015227218571442, - "timestamp": 4.776598627751761 - }, - { - "x": 3.0197839001768125, - "y": 1.4355966353904746, - "heading": -0.6507373526846939, - "angularVelocity": -3.859169970324026e-7, - "velocityX": -3.6327718239914044, - "velocityY": 2.1597655791299997, - "timestamp": 4.852034842962193 + "x": 1.4613425796478687, + "y": 5.4070770106059785, + "heading": -0.09976067247727309, + "angularVelocity": -0.45659018844361554, + "velocityX": 0.7719785095557636, + "velocityY": -0.826020721731408, + "timestamp": 0.34070587488650395 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.6507373865448278, - "angularVelocity": -4.4885780393316453e-7, - "velocityX": -3.473537574141072, - "velocityY": 2.407520434630195, - "timestamp": 4.9274710581726255 - }, - { - "x": 2.569555685248769, - "y": 1.762694432110511, - "heading": -0.6507374202071338, - "angularVelocity": -5.980782803410117e-7, - "velocityX": -3.3437088024696617, - "velocityY": 2.584806921874972, - "timestamp": 4.983755172029852 - }, - { - "x": 2.389174039276651, - "y": 1.9177631325165114, - "heading": -0.6507374491092287, - "angularVelocity": -5.135035974859511e-7, - "velocityX": -3.2048411818241203, - "velocityY": 2.755106010896001, - "timestamp": 5.040039285887079 - }, - { - "x": 2.2170960677982245, - "y": 2.081997759940487, - "heading": -0.6507374803842504, - "angularVelocity": -5.556633928977291e-7, - "velocityX": -3.0573097750979614, - "velocityY": 2.917957060505229, - "timestamp": 5.096323399744305 - }, - { - "x": 2.0538462988954582, - "y": 2.2548939722714945, - "heading": -0.6510332999359818, - "angularVelocity": -0.005255826759246614, - "velocityX": -2.9004590765499803, - "velocityY": 3.0718474624933285, - "timestamp": 5.152607513601532 - }, - { - "x": 1.9011340137811743, - "y": 2.4339552115414294, - "heading": -0.6596386590017012, - "angularVelocity": -0.15289143731654284, - "velocityX": -2.713239574165845, - "velocityY": 3.181381512448628, - "timestamp": 5.208891627458758 - }, - { - "x": 1.7604827039783657, - "y": 2.6146352839408076, - "heading": -0.6772772556341842, - "angularVelocity": -0.3133849930946056, - "velocityX": -2.498952193856903, - "velocityY": 3.2101433249477958, - "timestamp": 5.265175741315985 - }, - { - "x": 1.631754490870844, - "y": 2.792388921478468, - "heading": -0.7009679668217429, - "angularVelocity": -0.42091292842690614, - "velocityX": -2.2871145032870293, - "velocityY": 3.158149349007424, - "timestamp": 5.321459855173211 - }, - { - "x": 1.513917799766492, - "y": 2.964350328382096, - "heading": -0.7282264856039945, - "angularVelocity": -0.4843021754130653, - "velocityX": -2.093604803004671, - "velocityY": 3.0552387719887317, - "timestamp": 5.377743969030438 - }, - { - "x": 1.4059399900768588, - "y": 3.128858674851357, - "heading": -0.757359284956742, - "angularVelocity": -0.5176025232741039, - "velocityX": -1.9184420307928356, - "velocityY": 2.92282022750792, - "timestamp": 5.434028082887664 - }, - { - "x": 1.3069898193149363, - "y": 3.2849152261509276, - "heading": -0.787231438028103, - "angularVelocity": -0.5307386227512847, - "velocityX": -1.7580479460496543, - "velocityY": 2.77265716033891, - "timestamp": 5.490312196744891 - }, - { - "x": 1.2164244831734237, - "y": 3.431880035528415, - "heading": -0.817049536411403, - "angularVelocity": -0.5297782329653102, - "velocityX": -1.6090745671371067, - "velocityY": 2.611124157524907, - "timestamp": 5.546596310602117 - }, - { - "x": 1.133744535036132, - "y": 3.5693179916241067, - "heading": -0.8462342058218654, - "angularVelocity": -0.5185240987269294, - "velocityX": -1.4689748575774313, - "velocityY": 2.441860530030267, - "timestamp": 5.602880424459344 - }, - { - "x": 1.058555464808831, - "y": 3.696918257435487, - "heading": -0.8743465993903727, - "angularVelocity": -0.4994729710024167, - "velocityX": -1.3358844099070253, - "velocityY": 2.267074260688507, - "timestamp": 5.6591645383165705 - }, - { - "x": 0.9905398311066821, - "y": 3.8144496656298092, - "heading": -0.9010441658077244, - "angularVelocity": -0.47433573325990525, - "velocityX": -1.2084339441619472, - "velocityY": 2.088180840733473, - "timestamp": 5.715448652173797 - }, - { - "x": 0.9294376821542455, - "y": 3.921734581554844, - "heading": -0.9260527839372265, - "angularVelocity": -0.44432818455559475, - "velocityX": -1.0856020422997397, - "velocityY": 1.9061313854417152, - "timestamp": 5.771732766031024 - }, - { - "x": 0.8750327547294673, - "y": 4.018632790760157, - "heading": -0.9491484821020626, - "angularVelocity": -0.4103413304759842, - "velocityX": -0.9666124896766538, - "velocityY": 1.721590739637662, - "timestamp": 5.82801687988825 - }, - { - "x": 0.8271426027720992, - "y": 4.105031120160407, - "heading": -0.9701450202727028, - "angularVelocity": -0.37304554929835076, - "velocityX": -0.8508644566893013, - "velocityY": 1.5350393473265314, - "timestamp": 5.884300993745477 - }, - { - "x": 0.7856114088806271, - "y": 4.180836495357623, - "heading": -0.9888851979118991, - "angularVelocity": -0.33295678575901055, - "velocityX": -0.7378848318874176, - "velocityY": 1.3468343019401061, - "timestamp": 5.940585107602703 - }, - { - "x": 0.750304653010895, - "y": 4.2459711432493465, - "heading": -1.0052346087320225, - "angularVelocity": -0.2904800253513115, - "velocityX": -0.6272952250664012, - "velocityY": 1.1572474616362955, - "timestamp": 5.99686922145993 - }, - { - "x": 0.7211050890293871, - "y": 4.300369182693025, - "heading": -1.0190770490522025, - "angularVelocity": -0.24593867383776238, - "velocityX": -0.5187887306101521, - "velocityY": 0.9664901108981995, - "timestamp": 6.053153335317156 - }, - { - "x": 0.6979096587548851, - "y": 4.343974141765046, - "heading": -1.0303110716490886, - "angularVelocity": -0.19959490923820775, - "velocityX": -0.41211327113260987, - "velocityY": 0.7747294233437039, - "timestamp": 6.109437449174383 - }, - { - "x": 0.6806270896717198, - "y": 4.3767371107575626, - "heading": -1.0388473503636761, - "angularVelocity": -0.15166408653498495, - "velocityX": -0.3070594506827464, - "velocityY": 0.5820997568803311, - "timestamp": 6.165721563031609 - }, - { - "x": 0.6691759993701499, - "y": 4.398615342035701, - "heading": -1.0446066293483676, - "angularVelocity": -0.10232512497755435, - "velocityX": -0.20345155172234425, - "velocityY": 0.3887105930749016, - "timestamp": 6.222005676888836 + "x": 1.5403946295377764, + "y": 5.322786826751108, + "heading": -0.14691970563793563, + "angularVelocity": -0.5536626942681541, + "velocityX": 0.9280972911458275, + "velocityY": -0.9895947216401798, + "timestamp": 0.42588234360812993 }, { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -0.05172812017100072, - "velocityX": -0.10114076084111032, - "velocityY": 0.19465223880001714, - "timestamp": 6.278289790746062 + "x": 1.6316218404845053, + "y": 5.225944550707253, + "heading": -0.20162745404412188, + "angularVelocity": -0.6422871155293178, + "velocityX": 1.0710377210503823, + "velocityY": -1.1369604480828668, + "timestamp": 0.5110588123297559 }, { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": 6.011381134786383e-35, - "velocityY": 3.397969437810217e-36, - "timestamp": 6.334573904603289 - }, - { - "x": 0.6659136967977343, - "y": 4.396697343009468, - "heading": -1.044698875889832, - "angularVelocity": 0.048632422759591634, - "velocityX": 0.04192362717597145, - "velocityY": -0.22207715461221028, - "timestamp": 6.392543972798269 - }, - { - "x": 0.6710310256277597, - "y": 4.371001166581999, - "heading": -1.0390353111823831, - "angularVelocity": 0.09769808599154256, - "velocityX": 0.08827536329288825, - "velocityY": -0.4432662790914817, - "timestamp": 6.45051404099325 - }, - { - "x": 0.6791263180734669, - "y": 4.3325474770670445, - "heading": -1.0304995454316488, - "angularVelocity": 0.14724436276363576, - "velocityX": 0.13964607422021558, - "velocityY": -0.6633369722046352, - "timestamp": 6.50848410918823 - }, - { - "x": 0.6905315555019766, - "y": 4.281418801002382, - "heading": -1.0190608629623887, - "angularVelocity": 0.1973204935827632, - "velocityX": 0.19674355721212186, - "velocityY": -0.8819840593026762, - "timestamp": 6.566454177383211 - }, - { - "x": 0.7056282186927765, - "y": 4.217721440877341, - "heading": -1.0046856737535708, - "angularVelocity": 0.24797606172322137, - "velocityX": 0.26042169107034036, - "velocityY": -1.0987973985263735, - "timestamp": 6.624424245578191 - }, - { - "x": 0.7248578813432716, - "y": 4.141594157346286, - "heading": -0.9873376775101346, - "angularVelocity": 0.2992578201751781, - "velocityX": 0.3317170955503569, - "velocityY": -1.313217077389022, - "timestamp": 6.682394313773171 - }, - { - "x": 0.7487354042613769, - "y": 4.053220761716442, - "heading": -0.966978385263236, - "angularVelocity": 0.3512035241776958, - "velocityX": 0.41189399394518583, - "velocityY": -1.5244659594431005, - "timestamp": 6.740364381968152 - }, - { - "x": 0.7778651202320954, - "y": 3.9528486632227207, - "heading": -0.9435683140016388, - "angularVelocity": 0.4038303212419584, - "velocityX": 0.5024958030537675, - "velocityY": -1.7314469625276876, - "timestamp": 6.798334450163132 - }, - { - "x": 0.8129599622873139, - "y": 3.840816527291042, - "heading": -0.9170694264777192, - "angularVelocity": 0.4571132715385337, - "velocityX": 0.6053959077153058, - "velocityY": -1.932585891651215, - "timestamp": 6.856304518358113 - }, - { - "x": 0.854862177504241, - "y": 3.717595744583205, - "heading": -0.8874498393217566, - "angularVelocity": 0.510946218940748, - "velocityX": 0.7228250116247967, - "velocityY": -2.1255931991210963, - "timestamp": 6.914274586553093 - }, - { - "x": 0.904560986315067, - "y": 3.5838520361766597, - "heading": -0.8546925510341115, - "angularVelocity": 0.5650724469991482, - "velocityX": 0.8573184465415072, - "velocityY": -2.307116630546341, - "timestamp": 6.972244654748073 - }, - { - "x": 0.9631953960515734, - "y": 3.4405334281831, - "heading": -0.8188108593180419, - "angularVelocity": 0.6189692859318806, - "velocityX": 1.0114600786614798, - "velocityY": -2.4722863445927272, - "timestamp": 7.030214722943054 - }, - { - "x": 1.0320177450129453, - "y": 3.288983389093817, - "heading": -0.7798733754078085, - "angularVelocity": 0.671682561753563, - "velocityX": 1.18720489908499, - "velocityY": -2.6142808488606364, - "timestamp": 7.088184791138034 - }, - { - "x": 1.1122806132492202, - "y": 3.131051679274452, - "heading": -0.7380383985495225, - "angularVelocity": 0.721665131004769, - "velocityX": 1.3845570780823164, - "velocityY": -2.724366465952162, - "timestamp": 7.146154859333015 - }, - { - "x": 1.2050235127281292, - "y": 2.9691249632164602, - "heading": -0.6935876595861372, - "angularVelocity": 0.7667877638831885, - "velocityX": 1.5998411312363996, - "velocityY": -2.7932814485115998, - "timestamp": 7.204124927527995 - }, - { - "x": 1.3108176746302633, - "y": 2.8059720420366694, - "heading": -0.6469378343676327, - "angularVelocity": 0.8047226210188198, - "velocityX": 1.8249790831071382, - "velocityY": -2.814433832146465, - "timestamp": 7.2620949957229755 - }, - { - "x": 1.42962965449819, - "y": 2.6444006527037383, - "heading": -0.5986101263227683, - "angularVelocity": 0.8336665722440691, - "velocityX": 2.0495401086696328, - "velocityY": -2.7871519624488035, - "timestamp": 7.320065063917956 - }, - { - "x": 1.5609092304168717, - "y": 2.4869058396466244, - "heading": -0.5491623613282449, - "angularVelocity": 0.8529878700195282, - "velocityX": 2.2646096512622123, - "velocityY": -2.7168298737787384, - "timestamp": 7.378035132112936 - }, - { - "x": 1.7038131780379508, - "y": 2.335492698782744, - "heading": -0.4991191344613761, - "angularVelocity": 0.863259754992006, - "velocityX": 2.4651333364043317, - "velocityY": -2.6119193159236365, - "timestamp": 7.436005200307917 - }, - { - "x": 1.8574111825796424, - "y": 2.19168017622197, - "heading": -0.44893240803487616, - "angularVelocity": 0.8657351628033007, - "velocityX": 2.649608829595809, - "velocityY": -2.4808065099572545, - "timestamp": 7.493975268502897 - }, - { - "x": 2.020808784771142, - "y": 2.056591090907756, - "heading": -0.3989735055068982, - "angularVelocity": 0.8618051364014737, - "velocityX": 2.8186546484975037, - "velocityY": -2.3303247610447158, - "timestamp": 7.5519453366978775 - }, - { - "x": 2.1931999690795068, - "y": 1.9310536028968317, - "heading": -0.3495416314047453, - "angularVelocity": 0.8527137476514646, - "velocityX": 2.973796472492188, - "velocityY": -2.165557017954213, - "timestamp": 7.609915404892858 - }, - { - "x": 2.3738798200885083, - "y": 1.815684583131589, - "heading": -0.3008771196490498, - "angularVelocity": 0.8394765310955621, - "velocityX": 3.1167783070616735, - "velocityY": -1.9901480774043983, - "timestamp": 7.667885473087838 - }, - { - "x": 2.5622392362768553, - "y": 1.7109506397451384, - "heading": -0.253174013671076, - "angularVelocity": 0.8228920107101786, - "velocityX": 3.249252968873622, - "velocityY": -1.8066900151675016, - "timestamp": 7.725855541282819 + "x": 1.733844381505324, + "y": 5.118072938931053, + "heading": -0.2630340547325837, + "angularVelocity": -0.7209338636607621, + "velocityX": 1.2001265438099284, + "velocityY": -1.2664485085516666, + "timestamp": 0.5962352810513819 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.20659037596839774, - "angularVelocity": 0.8035808677332508, - "velocityX": 3.3726738988458935, - "velocityY": -1.6170375099283227, - "timestamp": 7.783825609477799 - }, - { - "x": 2.954766850408611, - "y": 1.5365275146757538, - "heading": -0.16235575982643646, - "angularVelocity": 0.7825616402901594, - "velocityX": 3.48539682535889, - "velocityY": -1.4273820029873086, - "timestamp": 7.840351017655946 - }, - { - "x": 3.157939243604707, - "y": 1.4666859169715192, - "heading": -0.11962489482799478, - "angularVelocity": 0.7559585392779498, - "velocityX": 3.594355171320012, - "velocityY": -1.2355788300390564, - "timestamp": 7.896876425834093 - }, - { - "x": 3.3670172328271493, - "y": 1.4078228373530197, - "heading": -0.07876775443844967, - "angularVelocity": 0.7228101787567607, - "velocityX": 3.698832011323254, - "velocityY": -1.0413561178184736, - "timestamp": 7.95340183401224 - }, - { - "x": 3.5816948765446446, - "y": 1.3600926772574, - "heading": -0.04022391942526907, - "angularVelocity": 0.6818851248575686, - "velocityX": 3.797896390963022, - "velocityY": -0.8444018651787949, - "timestamp": 8.009927242190386 - }, - { - "x": 3.8015965172009225, - "y": 1.323669626598405, - "heading": -0.004523858036694332, - "angularVelocity": 0.6315754726805621, - "velocityX": 3.890314952936408, - "velocityY": -0.6443659910283791, - "timestamp": 8.066452650368532 - }, - { - "x": 4.026252074416622, - "y": 1.298748947880293, - "heading": 0.02768066779446809, - "angularVelocity": 0.5697353963312556, - "velocityX": 3.9744172480395035, - "velocityY": -0.44087569681180167, - "timestamp": 8.122978058546678 - }, - { - "x": 4.255060536192153, - "y": 1.2855455255338215, - "heading": 0.05557184744847584, - "angularVelocity": 0.49342730203920493, - "velocityX": 4.0478869441227845, - "velocityY": -0.23358384790180253, - "timestamp": 8.179503466724825 - }, - { - "x": 4.487235082115286, - "y": 1.284285135117098, - "heading": 0.07809760377772143, - "angularVelocity": 0.39850674334368213, - "velocityX": 4.107436874960846, - "velocityY": -0.02229776762957918, - "timestamp": 8.23602887490297 - }, - { - "x": 4.721720600054119, - "y": 1.2951774473138997, - "heading": 0.0938646398689849, - "angularVelocity": 0.27893714701841205, - "velocityX": 4.148320649004834, - "velocityY": 0.19269763010774343, - "timestamp": 8.292554283081117 - }, - { - "x": 4.957338422985755, - "y": 1.31837138535252, - "heading": 0.1023163328663219, - "angularVelocity": 0.14952024708429268, - "velocityX": 4.168352436997136, - "velocityY": 0.4103276523987602, - "timestamp": 8.349079691259263 - }, - { - "x": 5.192892039115628, - "y": 1.3539608804787848, - "heading": 0.10486380159502258, - "angularVelocity": 0.04506767506520281, - "velocityX": 4.16721654423966, - "velocityY": 0.6296194273219583, - "timestamp": 8.405605099437409 - }, - { - "x": 5.426922141496893, - "y": 1.4019164045228902, - "heading": 0.10486553946063551, - "angularVelocity": 0.0000307448573823463, - "velocityX": 4.140263819832846, - "velocityY": 0.8483888146896288, - "timestamp": 8.462130507615555 - }, - { - "x": 5.657546782205124, - "y": 1.4618615011178113, - "heading": 0.10700306943729049, - "angularVelocity": 0.037815383303704875, - "velocityX": 4.080017254919927, - "velocityY": 1.0604982525026048, - "timestamp": 8.518655915793701 - }, - { - "x": 5.881407902963978, - "y": 1.532219235085926, - "heading": 0.11413761051896783, - "angularVelocity": 0.12621830273550538, - "velocityX": 3.9603627461358366, - "velocityY": 1.2447098788985973, - "timestamp": 8.575181323971847 - }, - { - "x": 6.0948633682195945, - "y": 1.6093965656168685, - "heading": 0.12496970465048783, - "angularVelocity": 0.19163230272272128, - "velocityX": 3.7762746371133757, - "velocityY": 1.3653564479836866, - "timestamp": 8.631706732149993 - }, - { - "x": 6.296160249007556, - "y": 1.6893086925404435, - "heading": 0.13798616586845183, - "angularVelocity": 0.23027628879637688, - "velocityX": 3.5611751825577227, - "velocityY": 1.41373816659089, - "timestamp": 8.68823214032814 - }, - { - "x": 6.484990958449558, - "y": 1.7688506297276971, - "heading": 0.1520046167268183, - "angularVelocity": 0.24800264713145628, - "velocityX": 3.3406341595425215, - "velocityY": 1.4071890809981862, - "timestamp": 8.744757548506286 - }, - { - "x": 6.661585058300773, - "y": 1.8459701168111895, - "heading": 0.1662218219427335, - "angularVelocity": 0.25151884213039066, - "velocityX": 3.124154350104949, - "velocityY": 1.364333130340974, - "timestamp": 8.801282956684432 - }, - { - "x": 6.826289993460413, - "y": 1.9193152685714405, - "heading": 0.18009164346035805, - "angularVelocity": 0.24537322178925483, - "velocityX": 2.9138212437237407, - "velocityY": 1.2975607629244343, - "timestamp": 8.857808364862578 - }, - { - "x": 6.979441082007995, - "y": 1.9879614203209928, - "heading": 0.1932294988279346, - "angularVelocity": 0.23242389203402142, - "velocityX": 2.7094203029000408, - "velocityY": 1.2144300052324348, - "timestamp": 8.914333773040724 - }, - { - "x": 7.121330748513538, - "y": 2.0512482937052026, - "heading": 0.20535415779786798, - "angularVelocity": 0.21449927317147402, - "velocityX": 2.510192691724767, - "velocityY": 1.1196181579928337, - "timestamp": 8.97085918121887 - }, - { - "x": 7.25220694864691, - "y": 2.1086857599712796, - "heading": 0.21625288773093435, - "angularVelocity": 0.19281116730228098, - "velocityX": 2.3153517038019378, - "velocityY": 1.0161353649151172, - "timestamp": 9.027384589397016 - }, - { - "x": 7.372278984681028, - "y": 2.1598978527910453, - "heading": 0.22576002013784088, - "angularVelocity": 0.1681921937997094, - "velocityX": 2.124213515729, - "velocityY": 0.9060012916379937, - "timestamp": 9.083909997575162 + "x": 1.8457280635804503, + "y": 5.001046382736992, + "heading": -0.33003861392476214, + "angularVelocity": -0.7866557536115174, + "velocityX": 1.3135515448613582, + "velocityY": -1.373930593160977, + "timestamp": 0.6814117497730079 }, { - "x": 7.481724262237549, - "y": 2.2045881748199463, - "heading": 0.2337432969576531, - "angularVelocity": 0.14123342187378718, - "velocityX": 1.9362138387677101, - "velocityY": 0.7906236057253049, - "timestamp": 9.140435405753308 - }, - { - "x": 7.586144701324698, - "y": 2.244425533220676, - "heading": 0.24038146155956527, - "angularVelocity": 0.11057598681783708, - "velocityX": 1.739395418529896, - "velocityY": 0.6635953582873915, - "timestamp": 9.200468011321169 - }, - { - "x": 7.678582360185057, - "y": 2.2769030569359834, - "heading": 0.24515554508543944, - "angularVelocity": 0.07952484288688075, - "velocityX": 1.5397908850694146, - "velocityY": 0.5409980694340338, - "timestamp": 9.26050061688903 - }, - { - "x": 7.7589012203408085, - "y": 2.3022472705839294, - "heading": 0.2480460603725282, - "angularVelocity": 0.04814908931149622, - "velocityX": 1.3379206082427928, - "velocityY": 0.42217414033940376, - "timestamp": 9.32053322245689 - }, - { - "x": 7.826988761652501, - "y": 2.320653171453341, - "heading": 0.24903661097421825, - "angularVelocity": 0.016500210049526117, - "velocityX": 1.1341760143115402, - "velocityY": 0.30659840090741436, - "timestamp": 9.38056582802475 - }, - { - "x": 7.882750526752601, - "y": 2.332290238336933, - "heading": 0.2481131524247365, - "angularVelocity": -0.015382616508921566, - "velocityX": 0.9288579859667665, - "velocityY": 0.19384577386762514, - "timestamp": 9.44059843359261 - }, - { - "x": 7.926106207329853, - "y": 2.337307039185365, - "heading": 0.24526345998864965, - "angularVelocity": -0.047469077997382, - "velocityX": 0.7222022127332522, - "velocityY": 0.08356793447456733, - "timestamp": 9.50063103916047 - }, - { - "x": 7.956986763254213, - "y": 2.3358348134459193, - "heading": 0.2404767337370615, - "angularVelocity": -0.07973544053784674, - "velocityX": 0.5143963956295903, - "velocityY": -0.02452376880063506, - "timestamp": 9.560663644728331 + "x": 1.9656142072747882, + "y": 4.877758551128575, + "heading": -0.40085023097520717, + "angularVelocity": -0.8313518758551945, + "velocityX": 1.4075030990795228, + "velocityY": -1.447440043697368, + "timestamp": 0.7665882184946339 }, { - "x": 7.975332260131836, - "y": 2.3279902935028076, - "heading": 0.2337432969576531, - "angularVelocity": -0.11216299402158979, - "velocityX": 0.3055922144989234, - "velocityY": -0.13067098902186064, - "timestamp": 9.620696250296191 - }, - { - "x": 7.978374325239172, - "y": 2.3079982956389222, - "heading": 0.2220767476081581, - "angularVelocity": -0.15342778134763177, - "velocityX": 0.04000645659240227, - "velocityY": -0.2629164618495627, - "timestamp": 9.696735604101754 - }, - { - "x": 7.9611015630205095, - "y": 2.2781926707436404, - "heading": 0.20738275399488582, - "angularVelocity": -0.19324195798462207, - "velocityX": -0.2271555629316675, - "velocityY": -0.3919763044212194, - "timestamp": 9.772774957907316 - }, - { - "x": 7.923380278914192, - "y": 2.238852677448683, - "heading": 0.18978782188047655, - "angularVelocity": -0.23139244659286162, - "velocityX": -0.49607581099088477, - "velocityY": -0.5173635930093835, - "timestamp": 9.848814311712879 - }, - { - "x": 7.865060691710785, - "y": 2.19030356664395, - "heading": 0.16943867046954186, - "angularVelocity": -0.2676134184802368, - "velocityX": -0.7669658444564572, - "velocityY": -0.6384734795205669, - "timestamp": 9.924853665518441 - }, - { - "x": 7.785974313825651, - "y": 2.132928607276668, - "heading": 0.14650757608805504, - "angularVelocity": -0.30156876977312563, - "velocityX": -1.04007167245744, - "velocityY": -0.7545429635553378, - "timestamp": 10.000893019324003 - }, - { - "x": 7.685930994427759, - "y": 2.067185547524282, - "heading": 0.12119962682948068, - "angularVelocity": -0.33282699013051165, - "velocityX": -1.3156781901870174, - "velocityY": -0.8645925624314937, - "timestamp": 10.076932373129566 - }, - { - "x": 7.564715810849877, - "y": 1.9936296675417498, - "heading": 0.09376277438448252, - "angularVelocity": -0.3608243767451793, - "velocityX": -1.5941111741669691, - "velocityY": -0.9673396248292669, - "timestamp": 10.152971726935128 - }, - { - "x": 7.422086372418718, - "y": 1.912946891741318, - "heading": 0.06450207380614308, - "angularVelocity": -0.3848099584481087, - "velocityX": -1.8757318584778, - "velocityY": -1.0610660370252898, - "timestamp": 10.22901108074069 - }, - { - "x": 7.2577720289721634, - "y": 1.8260026827906595, - "heading": 0.03380034441899183, - "angularVelocity": -0.4037610507008996, - "velocityX": -2.160911886057286, - "velocityY": -1.1434106761740845, - "timestamp": 10.305050434546253 - }, - { - "x": 7.0714787796722804, - "y": 1.7339163522481107, - "heading": 0.002148903485203126, - "angularVelocity": -0.41625078791073866, - "velocityX": -2.4499583436262102, - "velocityY": -1.2110351539548843, - "timestamp": 10.381089788351815 - }, - { - "x": 6.8629095324946725, - "y": 1.6381770650545604, - "heading": -0.029805628870875043, - "angularVelocity": -0.4202367689471438, - "velocityX": -2.7429118836403266, - "velocityY": -1.259075497121691, - "timestamp": 10.457129142157378 - }, - { - "x": 6.631824574679955, - "y": 1.5408277464126163, - "heading": -0.06118892010520367, - "angularVelocity": -0.4127243284389006, - "velocityX": -3.0390179065121625, - "velocityY": -1.2802491574411938, - "timestamp": 10.53316849596294 - }, - { - "x": 6.378206231452605, - "y": 1.444749972258303, - "heading": -0.09078814079587186, - "angularVelocity": -0.3892618651962165, - "velocityX": -3.335356371858044, - "velocityY": -1.26352696789074, - "timestamp": 10.609207849768502 - }, - { - "x": 6.102679405785972, - "y": 1.3540414004691264, - "heading": -0.11690114137793724, - "angularVelocity": -0.343414288459606, - "velocityX": -3.6234766851277205, - "velocityY": -1.1929161315747718, - "timestamp": 10.685247203574065 - }, - { - "x": 5.807422454316563, - "y": 1.2742067055061217, - "heading": -0.1372503142523278, - "angularVelocity": -0.26761370074796764, - "velocityX": -3.8829492452605625, - "velocityY": -1.0499128539038807, - "timestamp": 10.761286557379627 - }, - { - "x": 5.497293853879703, - "y": 1.211233933640027, - "heading": -0.14936074429275475, - "angularVelocity": -0.15926529401333597, - "velocityX": -4.078527563896464, - "velocityY": -0.8281602711553835, - "timestamp": 10.83732591118519 - }, - { - "x": 5.179311523814037, - "y": 1.1692856386862889, - "heading": -0.15140362446925906, - "angularVelocity": -0.026866090705190943, - "velocityX": -4.181812629270475, - "velocityY": -0.5516655896498385, - "timestamp": 10.913365264990752 - }, - { - "x": 4.858542014963219, - "y": 1.1497329672294159, - "heading": -0.1514046806841633, - "angularVelocity": -0.000013890371910813042, - "velocityX": -4.21846705419206, - "velocityY": -0.2571388429584845, - "timestamp": 10.989404618796314 - }, - { - "x": 4.538318225528884, - "y": 1.1527274796290288, - "heading": -0.15538772135714082, - "angularVelocity": -0.052381306174200754, - "velocityX": -4.211290251797366, - "velocityY": 0.03938108689442748, - "timestamp": 11.065443972601877 - }, - { - "x": 4.221401569033501, - "y": 1.177956926117284, - "heading": -0.16806437438072017, - "angularVelocity": -0.1667117405546922, - "velocityX": -4.167797865638882, - "velocityY": 0.33179459353072166, - "timestamp": 11.141483326407439 - }, - { - "x": 3.910015091156467, - "y": 1.2249289706320923, - "heading": -0.19130842148638338, - "angularVelocity": -0.30568443762817654, - "velocityX": -4.095070016945054, - "velocityY": 0.6177333467998585, - "timestamp": 11.217522680213001 - }, - { - "x": 3.606469436454312, - "y": 1.2929813536267714, - "heading": -0.22267226856626002, - "angularVelocity": -0.41246861671228957, - "velocityX": -3.9919546854427725, - "velocityY": 0.8949626685241646, - "timestamp": 11.293562034018564 - }, - { - "x": 3.3124526239021637, - "y": 1.3814436970961244, - "heading": -0.26028249186707797, - "angularVelocity": -0.49461524090525394, - "velocityX": -3.8666400730333566, - "velocityY": 1.163375792166213, - "timestamp": 11.369601387824126 - }, - { - "x": 3.0292255135853545, - "y": 1.48970128529521, - "heading": -0.302685251705683, - "angularVelocity": -0.5576422959489076, - "velocityX": -3.724743782555543, - "velocityY": 1.4237047368380822, - "timestamp": 11.445640741629688 + "x": 2.0856915448639635, + "y": 4.7606487505271025, + "heading": -0.4673031995363478, + "angularVelocity": -0.7801798966134944, + "velocityX": 1.4097477788332962, + "velocityY": -1.3749079101202415, + "timestamp": 0.8517646872162599 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.34872944068091694, - "angularVelocity": -0.6055310397951567, - "velocityX": -3.5701531878761683, - "velocityY": 1.676889312494585, - "timestamp": 11.52168009543525 - }, - { - "x": 2.5541569719261092, - "y": 1.727757046354288, - "heading": -0.38618529156646786, - "angularVelocity": -0.6332589444448175, - "velocityX": -3.442165602360439, - "velocityY": 1.8689832549148022, - "timestamp": 11.580827859891052 - }, - { - "x": 2.3587219086882643, - "y": 1.8492493966793417, - "heading": -0.4251400494897362, - "angularVelocity": -0.658600680544372, - "velocityX": -3.304183430024454, - "velocityY": 2.0540480513991684, - "timestamp": 11.639975624346853 - }, - { - "x": 2.172129075786119, - "y": 1.9811469849343935, - "heading": -0.4654182614046572, - "angularVelocity": -0.6809760653763983, - "velocityX": -3.1546895240914963, - "velocityY": 2.2299674293457925, - "timestamp": 11.699123388802654 - }, - { - "x": 1.995157304922952, - "y": 2.1227357753454896, - "heading": -0.5067996859573998, - "angularVelocity": -0.6996278715430715, - "velocityX": -2.9920280587343844, - "velocityY": 2.3938147403170578, - "timestamp": 11.758271153258455 - }, - { - "x": 1.8286811439598407, - "y": 2.273062802113803, - "heading": -0.549006464745395, - "angularVelocity": -0.7135819785637939, - "velocityX": -2.8145807790844684, - "velocityY": 2.5415504398420463, - "timestamp": 11.817418917714257 - }, - { - "x": 1.673644229855571, - "y": 2.430852006094566, - "heading": -0.5916896444353141, - "angularVelocity": -0.7216363979709688, - "velocityX": -2.6211796089119246, - "velocityY": 2.6677120501937903, - "timestamp": 11.876566682170058 - }, - { - "x": 1.5309859321329615, - "y": 2.594413834176532, - "heading": -0.634420502020555, - "angularVelocity": -0.7224424790758148, - "velocityX": -2.411896696944723, - "velocityY": 2.7653087075538, - "timestamp": 11.935714446625859 - }, - { - "x": 1.4014991059285755, - "y": 2.7615886598812227, - "heading": -0.6766978032526942, - "angularVelocity": -0.7147742881090955, - "velocityX": -2.189209134034951, - "velocityY": 2.826392970940027, - "timestamp": 11.99486221108166 - }, - { - "x": 1.2856293975453275, - "y": 2.9297974775958746, - "heading": -0.7179836021581544, - "angularVelocity": -0.6980111469185274, - "velocityX": -1.9589871138719785, - "velocityY": 2.8438744771216182, - "timestamp": 12.054009975537461 - }, - { - "x": 1.1833007632826107, - "y": 3.0962581686871435, - "heading": -0.7577647191568426, - "angularVelocity": -0.6725717762066157, - "velocityX": -1.7300507500868296, - "velocityY": 2.814319232904572, - "timestamp": 12.113157739993262 - }, - { - "x": 1.0938911269423066, - "y": 3.2583116792220825, - "heading": -0.7956106235538323, - "angularVelocity": -0.6398535049498092, - "velocityX": -1.5116317102249486, - "velocityY": 2.7398078697638226, - "timestamp": 12.172305504449064 - }, - { - "x": 1.016385611128122, - "y": 3.4136921446071793, - "heading": -0.8311990080013264, - "angularVelocity": -0.601686044687091, - "velocityX": -1.3103710094081817, - "velocityY": 2.62698796505164, - "timestamp": 12.231453268904865 - }, - { - "x": 0.9495982361573998, - "y": 3.5606291449139458, - "heading": -0.864309152764572, - "angularVelocity": -0.5597869178637827, - "velocityX": -1.1291614414375832, - "velocityY": 2.4842359074546003, - "timestamp": 12.290601033360666 - }, - { - "x": 0.8923465849102798, - "y": 3.697809371688421, - "heading": -0.8947997322814718, - "angularVelocity": -0.5154984266511812, - "velocityX": -0.9679427747417668, - "velocityY": 2.319279993700968, - "timestamp": 12.349748797816467 - }, - { - "x": 0.8435455769066035, - "y": 3.8242806095123454, - "heading": -0.9225850102577191, - "angularVelocity": -0.46976040822321335, - "velocityX": -0.8250693572729036, - "velocityY": 2.138225155042558, - "timestamp": 12.408896562272268 - }, - { - "x": 0.8022404458446034, - "y": 3.939354876295257, - "heading": -0.947615738236735, - "angularVelocity": -0.42318975551004434, - "velocityX": -0.698337992010946, - "velocityY": 1.9455387340785029, - "timestamp": 12.46804432672807 - }, - { - "x": 0.7676071594331643, - "y": 4.0425313748152725, - "heading": -0.9698657760492436, - "angularVelocity": -0.3761771559284466, - "velocityX": -0.5855383839116994, - "velocityY": 1.7443854297674437, - "timestamp": 12.52719209118387 - }, - { - "x": 0.7389393229135627, - "y": 4.133440839855761, - "heading": -0.9893233232577529, - "angularVelocity": -0.32896504859535863, - "velocityX": -0.4846816575971237, - "velocityY": 1.5369890286964785, - "timestamp": 12.586339855639672 - }, - { - "x": 0.7156313779093327, - "y": 4.21180687471968, - "heading": -1.0059853372092005, - "angularVelocity": -0.28170149970585456, - "velocityX": -0.39406299153786883, - "velocityY": 1.324919641256768, - "timestamp": 12.645487620095473 - }, - { - "x": 0.6971623458837157, - "y": 4.277419392368828, - "heading": -1.0198540109194538, - "angularVelocity": -0.23447502771836987, - "velocityX": -0.31225241047644936, - "velocityY": 1.1092983522340676, - "timestamp": 12.704635384551274 - }, - { - "x": 0.6830816107540019, - "y": 4.330116323499302, - "heading": -1.0309345469547266, - "angularVelocity": -0.1873365145279998, - "velocityX": -0.23806030978966433, - "velocityY": 0.8909369883261355, - "timestamp": 12.763783149007075 - }, - { - "x": 0.6729970259294025, - "y": 4.3697708967031526, - "heading": -1.039233740627151, - "angularVelocity": -0.14031288838694966, - "velocityX": -0.17049815690219822, - "velocityY": 0.6704323243439457, - "timestamp": 12.822930913462876 - }, - { - "x": 0.6665651633052443, - "y": 4.396282678408227, - "heading": -1.0447590694290692, - "angularVelocity": -0.09341568278623903, - "velocityX": -0.10874227763865159, - "velocityY": 0.448229649066216, - "timestamp": 12.882078677918678 + "x": 2.1993786689736172, + "y": 4.652829385838518, + "heading": -0.5273883646655675, + "angularVelocity": -0.7054197718103367, + "velocityX": 1.3347245526367961, + "velocityY": -1.2658351104101313, + "timestamp": 0.9369411559378859 }, { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -0.046646417662750905, - "velocityX": -0.052103102496543485, - "velocityY": 0.22466601267048775, - "timestamp": 12.941226442374479 + "x": 2.3039823973737983, + "y": 4.5566291473122265, + "heading": -0.5809972368261334, + "angularVelocity": -0.6293859438546427, + "velocityX": 1.2280824736001708, + "velocityY": -1.1294227146313731, + "timestamp": 1.0221176246595118 }, { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -2.054852244630119e-40, - "velocityX": -3.6573844759845184e-41, - "velocityY": 1.4664679602509001e-41, - "timestamp": 13.00037420683028 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 2.3979860520770147, + "y": 4.47341696133058, + "heading": -0.6272322006251169, + "angularVelocity": -0.5428138134029581, + "velocityX": 1.1036340918339758, + "velocityY": -0.9769386689838181, + "timestamp": 1.107294093381138 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 2.480103614094797, + "y": 4.40444635319333, + "heading": -0.6652761118925774, + "angularVelocity": -0.4466481393094119, + "velocityX": 0.964087420507652, + "velocityY": -0.8097378204614404, + "timestamp": 1.192470562102764 }, { - "scope": [ - 5 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "AmpLanePSubASubDSubESub": { - "waypoints": [ - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119463, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 20 + "x": 2.549135601344952, + "y": 4.350942377101982, + "heading": -0.6944118607427208, + "angularVelocity": -0.3420633572561569, + "velocityX": 0.810458431609397, + "velocityY": -0.6281544292028495, + "timestamp": 1.2776470308243901 }, { - "x": 2.6514174938201904, - "y": 6.946751594543457, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 20 + "x": 2.6038790433453562, + "y": 4.3141752600275325, + "heading": -0.7139618385994653, + "angularVelocity": -0.2295232257237371, + "velocityX": 0.6427061701667566, + "velocityY": -0.431658151908278, + "timestamp": 1.3628234995460162 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119463, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 32 + "x": 2.6430389310079985, + "y": 4.2955467477546305, + "heading": -0.7232599969808831, + "angularVelocity": -0.1091634640525668, + "velocityX": 0.459750072412904, + "velocityY": -0.2187049140741477, + "timestamp": 1.4479999682676423 }, { - "x": 7.323064804077148, - "y": 7.475617408752441, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 7 + "x": 2.665171623229982, + "y": 4.296644687652586, + "heading": -0.721655115096118, + "angularVelocity": 0.018841845745096026, + "velocityX": 0.2598451491845503, + "velocityY": 0.012890178642452559, + "timestamp": 1.5331764369892684 }, { - "x": 7.799044132232666, - "y": 7.457988739013672, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 33 + "x": 2.66906909991143, + "y": 4.318945956778202, + "heading": -0.7091344221786624, + "angularVelocity": 0.14601587711521094, + "velocityX": 0.045452234946537245, + "velocityY": 0.2600766102664744, + "timestamp": 1.6189252826957972 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119463, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 28 + "x": 2.6560642905413947, + "y": 4.360643314893585, + "heading": -0.6858248901313277, + "angularVelocity": 0.2718349367303499, + "velocityX": -0.15166162602986216, + "velocityY": 0.4862731127377579, + "timestamp": 1.704674128402326 }, { - "x": 5.7460856437683105, - "y": 6.598193168640137, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 19 + "x": 2.627632992233998, + "y": 4.419910426276983, + "heading": -0.652529270737317, + "angularVelocity": 0.38829233349639936, + "velocityX": -0.331564793358298, + "velocityY": 0.6911709527407293, + "timestamp": 1.7904229741088546 }, { - "x": 7.981563568115234, - "y": 5.947082996368408, - "heading": -0.6889234822214203, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 + "x": 2.5850645171946978, + "y": 4.495119603214681, + "heading": -0.6100207421436514, + "angularVelocity": 0.4957329541105318, + "velocityX": -0.4964320474352394, + "velocityY": 0.877086756305711, + "timestamp": 1.8761718198153834 }, { - "x": 5.7460856437683105, - "y": 6.598193168640137, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 28 + "x": 2.5295631514165375, + "y": 4.584706937933874, + "heading": -0.5590704924389158, + "angularVelocity": 0.5941800065637087, + "velocityX": -0.6472549609368695, + "velocityY": 1.0447643228435226, + "timestamp": 1.9619206655219121 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119463, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ + "x": 2.46234032691493, + "y": 4.687033037499603, + "heading": -0.5004982071284244, + "angularVelocity": 0.6830679157006081, + "velocityX": -0.7839501972035283, + "velocityY": 1.1933233470679585, + "timestamp": 2.047669511228441 + }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0, - "velocityX": 2.456521210166087e-32, - "velocityY": -5.061818134196095e-29, - "timestamp": 0 + "x": 2.384701887418879, + "y": 4.800204071292841, + "heading": -0.43526625936464175, + "angularVelocity": 0.7607326632364927, + "velocityX": -0.9054167301769336, + "velocityY": 1.319796585723857, + "timestamp": 2.1334183569349694 }, { - "x": 0.6967277255263876, - "y": 6.6675008319457, - "heading": 0.9828953878926869, - "angularVelocity": -0.13938944197774958, - "velocityX": 0.2752170097278352, - "velocityY": 0.039184115430369315, - "timestamp": 0.07173351280273003 - }, - { - "x": 0.7362120737078192, - "y": 6.6731227516141445, - "heading": 0.9629028188307202, - "angularVelocity": -0.27870612052394816, - "velocityX": 0.5504309860819994, - "velocityY": 0.07837229001553121, - "timestamp": 0.14346702560546007 - }, - { - "x": 0.7954384350293103, - "y": 6.681556356569816, - "heading": 0.9329336821586108, - "angularVelocity": -0.417784317335511, - "velocityX": 0.8256442353927022, - "velocityY": 0.11756854819039611, - "timestamp": 0.2152005384081901 - }, - { - "x": 0.8744068559927786, - "y": 6.692802561177285, - "heading": 0.8930142547719868, - "angularVelocity": -0.5564962014627703, - "velocityX": 1.1008581329627185, - "velocityY": 0.15677755305130064, - "timestamp": 0.28693405121092014 - }, - { - "x": 0.9731173912556694, - "y": 6.706862646881597, - "heading": 0.8431760672320958, - "angularVelocity": -0.6947685342560019, - "velocityX": 1.3760727924840699, - "velocityY": 0.19600442187834377, - "timestamp": 0.3586675640136502 - }, - { - "x": 1.0915699973864605, - "y": 6.72373826040205, - "heading": 0.7834509327491487, - "angularVelocity": -0.832597375451512, - "velocityX": 1.6512868465489152, - "velocityY": 0.2352542467718665, - "timestamp": 0.43040107681638023 - }, - { - "x": 1.2297644280001299, - "y": 6.743431356930112, - "heading": 0.7138654539660069, - "angularVelocity": -0.9700553628884108, - "velocityX": 1.9264974658869234, - "velocityY": 0.2745313279837331, - "timestamp": 0.5021345896191103 - }, - { - "x": 1.3877001578372599, - "y": 6.765944077192238, - "heading": 0.6344360529195374, - "angularVelocity": -1.1072844191579285, - "velocityX": 2.20170076285362, - "velocityY": 0.3138382519616173, - "timestamp": 0.5738681024218403 - }, - { - "x": 1.565376370208164, - "y": 6.791278571412394, - "heading": 0.545166048889513, - "angularVelocity": -1.2444672030567556, - "velocityX": 2.476892674769556, - "velocityY": 0.3531751511051906, - "timestamp": 0.6456016152245703 - }, - { - "x": 1.7627920329701503, - "y": 6.819436846094517, - "heading": 0.4460464917361558, - "angularVelocity": -1.381774756252758, - "velocityX": 2.7520701981153106, - "velocityY": 0.3925400218865401, - "timestamp": 0.7173351280273003 - }, - { - "x": 1.9404742499696706, - "y": 6.844889308454019, - "heading": 0.35698857434522513, - "angularVelocity": -1.241510611911704, - "velocityX": 2.4769763814130292, - "velocityY": 0.35481968414999915, - "timestamp": 0.7890686408300303 - }, - { - "x": 2.098418416235195, - "y": 6.8675162488116115, - "heading": 0.277805044936203, - "angularVelocity": -1.103856848738075, - "velocityX": 2.201818369967191, - "velocityY": 0.3154305354505728, - "timestamp": 0.8608021536327602 - }, - { - "x": 2.2366248722942026, - "y": 6.887316924283159, - "heading": 0.20850199362833047, - "angularVelocity": -0.9661181863426805, - "velocityX": 1.9266651056396258, - "velocityY": 0.2760310305869847, - "timestamp": 0.9325356664354902 - }, - { - "x": 2.3550937746268814, - "y": 6.9042907996604885, - "heading": 0.14908158317430456, - "angularVelocity": -0.8283493741238005, - "velocityX": 1.6515140227218135, - "velocityY": 0.2366240646620762, - "timestamp": 1.0042691792382203 - }, - { - "x": 2.453825154537734, - "y": 6.918437433437714, - "heading": 0.09954309354100455, - "angularVelocity": -0.6905905995159123, - "velocityX": 1.3763633762039458, - "velocityY": 0.19721094393093683, - "timestamp": 1.0760026920409504 - }, - { - "x": 2.5328189743834146, - "y": 6.929756449824835, - "heading": 0.05988427758276665, - "angularVelocity": -0.5528631512095152, - "velocityX": 1.101212204919556, - "velocityY": 0.1577925845048924, - "timestamp": 1.1477362048436806 - }, - { - "x": 2.5920751704296263, - "y": 6.938247542788083, - "heading": 0.030102483585013936, - "angularVelocity": -0.41517266909175576, - "velocityX": 0.8260601451480881, - "velocityY": 0.11836995881959977, - "timestamp": 1.2194697176464107 - }, - { - "x": 2.6315936809069487, - "y": 6.943910491933908, - "heading": 0.010195398523530809, - "angularVelocity": -0.27751443165456446, - "velocityX": 0.550907224280912, - "velocityY": 0.07894426074125552, - "timestamp": 1.2912032304491408 - }, - { - "x": 2.6513744618221935, - "y": 6.946745178456401, - "heading": 0.00016145502701412444, - "angularVelocity": -0.13987804413429022, - "velocityX": 0.27575369012733186, - "velocityY": 0.03951690645092257, - "timestamp": 1.3629367432518709 + "x": 2.298178863290851, + "y": 4.921641059920068, + "heading": -0.36471802922759106, + "angularVelocity": 0.8227309598836902, + "velocityX": -1.0090284413174404, + "velocityY": 1.416193858082253, + "timestamp": 2.219167202641498 }, { - "x": 2.6514174938201904, - "y": 6.946751594543457, - "heading": 0, - "angularVelocity": -0.0022507610818009623, - "velocityX": 0.0005998861532762668, - "velocityY": 0.00008944325609789668, - "timestamp": 1.434670256054601 - }, - { - "x": 2.631713436254265, - "y": 6.943928504258672, - "heading": 0.00971593718624953, - "angularVelocity": 0.13541279143473714, - "velocityX": -0.27461904975679574, - "velocityY": -0.03934592501075656, - "timestamp": 1.506420768370774 - }, - { - "x": 2.592262356755882, - "y": 6.938276121500362, - "heading": 0.029312137508313962, - "angularVelocity": 0.27311582404564183, - "velocityX": -0.5498369025337515, - "velocityY": -0.07877829117207064, - "timestamp": 1.578171280686947 - }, - { - "x": 2.533064330899613, - "y": 6.929794745308614, - "heading": 0.058791626722930285, - "angularVelocity": 0.41086102718133133, - "velocityX": -0.8250537020140929, - "velocityY": -0.11820648955702064, - "timestamp": 1.64992179300312 - }, - { - "x": 2.4541194029599502, - "y": 6.918484740607142, - "heading": 0.09815683939208576, - "angularVelocity": 0.5486401612526653, - "velocityX": -1.1002698844327412, - "velocityY": -0.15762960193345896, - "timestamp": 1.721672305319293 - }, - { - "x": 2.355427542032357, - "y": 6.9043465299042746, - "heading": 0.1474084549881005, - "angularVelocity": 0.6864287652544176, - "velocityX": -1.3754864975735484, - "velocityY": -0.19704682573406979, - "timestamp": 1.793422817635466 - }, - { - "x": 2.23698858427003, - "y": 6.887380609317409, - "heading": 0.20654397577631692, - "angularVelocity": 0.8241825578726235, - "velocityX": -1.650705394277179, - "velocityY": -0.23645713505578408, - "timestamp": 1.865173329951639 - }, - { - "x": 2.098802169913165, - "y": 6.86758760630337, - "heading": 0.27555650233579015, - "angularVelocity": 0.9618401920015576, - "velocityX": -1.9259293050189108, - "velocityY": -0.2758586993970149, - "timestamp": 1.9369238422678121 - }, - { - "x": 1.9408676962603424, - "y": 6.844968393093841, - "heading": 0.3544346454359525, - "angularVelocity": 1.0993390923447317, - "velocityX": -2.2011616157204346, - "velocityY": -0.3152481072856972, - "timestamp": 2.0086743545839854 - }, - { - "x": 1.7631843231894517, - "y": 6.819524255776015, - "heading": 0.44316502681503017, - "angularVelocity": 1.2366515374122595, - "velocityX": -2.4764056353572657, - "velocityY": -0.35461959074183413, - "timestamp": 2.0804248669001586 - }, - { - "x": 1.5657510840322728, - "y": 6.791257095186479, - "heading": 0.5417391237624924, - "angularVelocity": 1.373845199915075, - "velocityX": -2.751663127625576, - "velocityY": -0.3939645818900745, - "timestamp": 2.152175379216332 - }, - { - "x": 1.387996398598156, - "y": 6.765933063404474, - "heading": 0.6316668804185087, - "angularVelocity": 1.2533395757259034, - "velocityX": -2.477399529588293, - "velocityY": -0.352945658067143, - "timestamp": 2.223925891532505 - }, - { - "x": 1.2299927628795557, - "y": 6.743427858422989, - "heading": 0.7117267031520905, - "angularVelocity": 1.115808377635633, - "velocityX": -2.202125540862631, - "velocityY": -0.3136591539003019, - "timestamp": 2.2956764038486783 - }, - { - "x": 1.091740243274482, - "y": 6.723739456363059, - "heading": 0.7818852162777382, - "angularVelocity": 0.9778120165089851, - "velocityX": -1.9268506267358863, - "velocityY": -0.2744008569054998, - "timestamp": 2.3674269161648516 - }, - { - "x": 0.9732386614039245, - "y": 6.706866137144222, - "heading": 0.842102674742542, - "angularVelocity": 0.8392617213147597, - "velocityX": -1.6515781987100508, - "velocityY": -0.2351665329867072, - "timestamp": 2.439177428481025 - }, - { - "x": 0.8744877098342877, - "y": 6.692806537214817, - "heading": 0.8923385972076003, - "angularVelocity": 0.7001472303020632, - "velocityX": -1.3763100554424192, - "velocityY": -0.19595121313677477, - "timestamp": 2.510927940797198 - }, - { - "x": 0.7954870665575497, - "y": 6.68155965440474, - "heading": 0.9325570015240306, - "angularVelocity": 0.5605312495048624, - "velocityX": -1.1010464000594427, - "velocityY": -0.15674986071479513, - "timestamp": 2.5826784531133713 - }, - { - "x": 0.736236493451904, - "y": 6.673124818793258, - "heading": 0.962730596118369, - "angularVelocity": 0.42053490106098795, - "velocityX": -0.8257860634235991, - "velocityY": -0.11755784510057492, - "timestamp": 2.6544289654295445 - }, - { - "x": 0.6967359103772861, - "y": 6.667501649956908, - "heading": 0.9828437467864926, - "angularVelocity": 0.28032065585715155, - "velocityX": -0.5505268437144837, - "velocityY": -0.07837113151770157, - "timestamp": 2.726179477745718 + "x": 2.2047510720717436, + "y": 5.044849013310237, + "heading": -0.2923595970689714, + "angularVelocity": 0.8438414717122018, + "velocityX": -1.0895515904535924, + "velocityY": 1.4368467863910614, + "timestamp": 2.3049160483480264 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.14007614861882017, - "velocityX": -0.2752658776108563, - "velocityY": -0.0391862324932835, - "timestamp": 2.797929990061891 + "x": 2.116347360692902, + "y": 5.158956328540576, + "heading": -0.22539524544258233, + "angularVelocity": 0.7809358956920709, + "velocityX": -1.0309609493916598, + "velocityY": 1.3307154666649113, + "timestamp": 2.390664894054555 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0, - "velocityX": -1.632165481129405e-29, - "velocityY": 0, - "timestamp": 2.8696805023780643 - }, - { - "x": 0.6961023495919454, - "y": 6.667365584217812, - "heading": 0.984880267445541, - "angularVelocity": -0.11368287336653356, - "velocityX": 0.27118304316961456, - "velocityY": 0.03795427117017962, - "timestamp": 2.940174979946086 - }, - { - "x": 0.7343390561697011, - "y": 6.672718275572268, - "heading": 0.9689850788648127, - "angularVelocity": -0.2254813302984037, - "velocityX": 0.5424071203430141, - "velocityY": 0.07593064789069806, - "timestamp": 3.0106694575141075 - }, - { - "x": 0.7916989777682265, - "y": 6.680749944426173, - "heading": 0.945369919685869, - "angularVelocity": -0.33499303766251487, - "velocityX": 0.8136796466528573, - "velocityY": 0.11393330557213717, - "timestamp": 3.081163935082129 - }, - { - "x": 0.8681861161076095, - "y": 6.691462782987562, - "heading": 0.9142283558404658, - "angularVelocity": -0.441758913885903, - "velocityX": 1.0850089393964117, - "velocityY": 0.1519670608389448, - "timestamp": 3.151658412650151 - }, - { - "x": 0.9638051470354019, - "y": 6.704859377756673, - "heading": 0.875792088105272, - "angularVelocity": -0.5452379968077065, - "velocityX": 1.3564045614144413, - "velocityY": 0.19003750692646, - "timestamp": 3.2221528902181724 - }, - { - "x": 1.078561545508893, - "y": 6.720942777481531, - "heading": 0.830339664632417, - "angularVelocity": -0.6447657325922732, - "velocityX": 1.627877848484808, - "velocityY": 0.2281512010545656, - "timestamp": 3.292647367786194 - }, - { - "x": 1.2124617638424005, - "y": 6.739716584680947, - "heading": 0.7782100744037193, - "angularVelocity": -0.7394847373455183, - "velocityX": 1.8994426649137832, - "velocityY": 0.26631599874331907, - "timestamp": 3.3631418453542157 - }, - { - "x": 1.3655134797863553, - "y": 6.761185092164687, - "heading": 0.7198245324523541, - "angularVelocity": -0.8282285927295209, - "velocityX": 2.1711163941355873, - "velocityY": 0.30454169212084536, - "timestamp": 3.4336363229222373 - }, - { - "x": 1.5377259215824886, - "y": 6.785353503086406, - "heading": 0.6557224279689098, - "angularVelocity": -0.909320938247837, - "velocityX": 2.4429210306575064, - "velocityY": 0.3428411948779657, - "timestamp": 3.504130800490259 - }, - { - "x": 1.7291102325420633, - "y": 6.812228299208335, - "heading": 0.5866232271908158, - "angularVelocity": -0.9802072894492893, - "velocityX": 2.714883740714364, - "velocityY": 0.3812326447273405, - "timestamp": 3.5746252780582806 - }, - { - "x": 1.9396796543838148, - "y": 6.8418178634961455, - "heading": 0.5135407387337584, - "angularVelocity": -1.0367122500700692, - "velocityX": 2.987034291282877, - "velocityY": 0.4197430112062252, - "timestamp": 3.645119755626302 - }, - { - "x": 2.1694484773986367, - "y": 6.874133526026527, - "heading": 0.4380194057033538, - "angularVelocity": -1.0713085001237521, - "velocityX": 3.259387556892149, - "velocityY": 0.45841410058255366, - "timestamp": 3.715614233194324 - }, - { - "x": 2.418423965072986, - "y": 6.9091911928007494, - "heading": 0.362723869672428, - "angularVelocity": -1.0681054549027866, - "velocityX": 3.531843858749197, - "velocityY": 0.4973108246726942, - "timestamp": 3.7861087107623455 - }, - { - "x": 2.6865429147293822, - "y": 6.947011884235923, - "heading": 0.29352452528672673, - "angularVelocity": -0.9816278774309596, - "velocityX": 3.803403598497242, - "velocityY": 0.5365057340651915, - "timestamp": 3.856603188330367 - }, - { - "x": 2.9720561979068543, - "y": 6.987623777247046, - "heading": 0.25938945355425436, - "angularVelocity": -0.48422334500649106, - "velocityX": 4.050151061861196, - "velocityY": 0.5761003473206351, - "timestamp": 3.9270976658983887 - }, - { - "x": 3.2671083841789206, - "y": 7.028939851633195, - "heading": 0.25938943415021554, - "angularVelocity": -2.752561543209048e-7, - "velocityX": 4.185465251336388, - "velocityY": 0.5860895180942723, - "timestamp": 3.9975921434664103 - }, - { - "x": 3.562160580898253, - "y": 7.070255851413062, - "heading": 0.25938941474662836, - "angularVelocity": -2.7524974860351036e-7, - "velocityX": 4.185465399536173, - "velocityY": 0.5860884597662348, - "timestamp": 4.068086621034432 - }, - { - "x": 3.8572127776182663, - "y": 7.111571851188067, - "heading": 0.25938939534304106, - "angularVelocity": -2.7524974884736384e-7, - "velocityX": 4.185465399545831, - "velocityY": 0.5860884596972535, - "timestamp": 4.138581098602454 - }, - { - "x": 4.15226497433828, - "y": 7.1528878509630704, - "heading": 0.25938937593945377, - "angularVelocity": -2.752497497523569e-7, - "velocityX": 4.185465399545832, - "velocityY": 0.5860884596972492, - "timestamp": 4.209075576170475 - }, - { - "x": 4.447317171058294, - "y": 7.194203850738075, - "heading": 0.25938935653586653, - "angularVelocity": -2.7524974896088886e-7, - "velocityX": 4.185465399545833, - "velocityY": 0.5860884596972492, - "timestamp": 4.279570053738497 - }, - { - "x": 4.742369367778308, - "y": 7.2355198505130724, - "heading": 0.2593893371322792, - "angularVelocity": -2.752497503383158e-7, - "velocityX": 4.185465399545844, - "velocityY": 0.5860884596971616, - "timestamp": 4.3500645313065185 - }, - { - "x": 5.037421564511595, - "y": 7.276835850193286, - "heading": 0.2593893177286919, - "angularVelocity": -2.7524974902410266e-7, - "velocityX": 4.185465399734123, - "velocityY": 0.5860884583526031, - "timestamp": 4.42055900887454 - }, - { - "x": 5.3324739648733965, - "y": 7.318150395663973, - "heading": 0.2593892983250951, - "angularVelocity": -2.7524988424278287e-7, - "velocityX": 4.185468288308109, - "velocityY": 0.5860678296512251, - "timestamp": 4.491053486442562 - }, - { - "x": 5.628942809727294, - "y": 7.346853994253202, - "heading": 0.25912444072738805, - "angularVelocity": -0.0037571396631956044, - "velocityX": 4.205561273474639, - "velocityY": 0.4071751373933052, - "timestamp": 4.561547964010583 - }, - { - "x": 5.9078202008377065, - "y": 7.3726601599906, - "heading": 0.2260522693823342, - "angularVelocity": -0.4691455626881114, - "velocityX": 3.9560175595502307, - "velocityY": 0.3660735794870859, - "timestamp": 4.632042441578605 - }, - { - "x": 6.167565330820035, - "y": 7.395632554305088, - "heading": 0.18684536926234946, - "angularVelocity": -0.5561698089350797, - "velocityX": 3.684616709609561, - "velocityY": 0.32587509131225256, - "timestamp": 4.702536919146627 - }, - { - "x": 6.408110614275644, - "y": 7.415818749602641, - "heading": 0.14706352599281394, - "angularVelocity": -0.5643256697824207, - "velocityX": 3.412257126432383, - "velocityY": 0.28635144189947087, - "timestamp": 4.773031396714648 - }, - { - "x": 6.629457805462712, - "y": 7.433245452372472, - "heading": 0.10922390474114492, - "angularVelocity": -0.536774263135107, - "velocityX": 3.1399224282992484, - "velocityY": 0.2472066376123601, - "timestamp": 4.84352587428267 - }, - { - "x": 6.831617157798908, - "y": 7.447929110099442, - "heading": 0.07477835738023303, - "angularVelocity": -0.48862759962544244, - "velocityX": 2.867733180108019, - "velocityY": 0.2082951492590427, - "timestamp": 4.9140203518506915 - }, - { - "x": 7.014599005404042, - "y": 7.459880739385679, - "heading": 0.04467522695346215, - "angularVelocity": -0.427028207957479, - "velocityX": 2.59569052665965, - "velocityY": 0.169539937007193, - "timestamp": 4.984514829418713 - }, - { - "x": 7.178412353011271, - "y": 7.469108205463054, - "heading": 0.019583414088526187, - "angularVelocity": -0.35594012085165694, - "velocityX": 2.3237756099286244, - "velocityY": 0.1308962970676859, - "timestamp": 5.055009306986735 + "x": 2.0362102002706437, + "y": 5.261312637562149, + "heading": -0.16556699187625293, + "angularVelocity": 0.6977149729932085, + "velocityX": -0.9345567250727145, + "velocityY": 1.1936756486715054, + "timestamp": 2.4764137397610835 }, { - "x": 7.323064804077148, - "y": 7.475617408752441, - "heading": 2.960760167883615e-32, - "angularVelocity": -0.2778006840270544, - "velocityX": 2.051968552094028, - "velocityY": 0.09233635759774637, - "timestamp": 5.125503784554756 - }, - { - "x": 7.4453229174359326, - "y": 7.479377686242559, - "heading": -0.013449699218163923, - "angularVelocity": -0.19675250253186094, - "velocityX": 1.7884853310086555, - "velocityY": 0.055008219469756155, - "timestamp": 5.193862250368862 - }, - { - "x": 7.549531921420882, - "y": 7.480799104036024, - "heading": -0.02185072478369098, - "angularVelocity": -0.12289663709499957, - "velocityX": 1.5244491336060073, - "velocityY": 0.020793588278158227, - "timestamp": 5.262220716182967 - }, - { - "x": 7.635663569022224, - "y": 7.480054976594767, - "heading": -0.02560203298601703, - "angularVelocity": -0.05487700985752643, - "velocityX": 1.259999717307419, - "velocityY": -0.010885666206756989, - "timestamp": 5.330579181997073 - }, - { - "x": 7.703696133906866, - "y": 7.477289120755522, - "heading": -0.025033985130795043, - "angularVelocity": 0.008309839146576752, - "velocityX": 0.9952324715661438, - "velocityY": -0.0404610578412588, - "timestamp": 5.398937647811178 - }, - { - "x": 7.753612501256561, - "y": 7.472622767647336, - "heading": -0.02042473052193369, - "angularVelocity": 0.06742770707282683, - "velocityX": 0.7302148571537285, - "velocityY": -0.06826298765796995, - "timestamp": 5.467296113625284 - }, - { - "x": 7.785398915822185, - "y": 7.46615947927951, - "heading": -0.012011716304774139, - "angularVelocity": 0.12307201627429676, - "velocityX": 0.46499602042070654, - "velocityY": -0.09454993307489655, - "timestamp": 5.535654579439389 + "x": 1.9661275657868833, + "y": 5.350222584355773, + "heading": -0.1134489428170574, + "angularVelocity": 0.6077988412528272, + "velocityX": -0.8173011998740509, + "velocityY": 1.0368646488597075, + "timestamp": 2.562162585467612 }, { - "x": 7.799044132232666, - "y": 7.457988739013672, - "heading": 0, - "angularVelocity": 0.17571658699068654, - "velocityX": 0.19961267778578498, - "velocityY": -0.11952784733433494, - "timestamp": 5.604013045253494 - }, - { - "x": 7.787627597101066, - "y": 7.445185981968696, - "heading": 0.02043091826358884, - "angularVelocity": 0.23842630400056083, - "velocityX": -0.1332295612366535, - "velocityY": -0.14940660051930396, - "timestamp": 5.6897037510220185 - }, - { - "x": 7.7476875027283105, - "y": 7.429823751171704, - "heading": 0.04606214764205663, - "angularVelocity": 0.299113295293718, - "velocityX": -0.466095990394167, - "velocityY": -0.17927534449873192, - "timestamp": 5.775394456790543 - }, - { - "x": 7.679221480716383, - "y": 7.41190305033205, - "heading": 0.07668903088242017, - "angularVelocity": 0.35741196160871463, - "velocityX": -0.7989900584653148, - "velocityY": -0.20913237531340728, - "timestamp": 5.861085162559067 - }, - { - "x": 7.582226808615669, - "y": 7.391425060459508, - "heading": 0.11206636753872803, - "angularVelocity": 0.41284916886870227, - "velocityX": -1.1319158971887242, - "velocityY": -0.2389756238892322, - "timestamp": 5.946775868327591 - }, - { - "x": 7.456700337139081, - "y": 7.36839118812004, - "heading": 0.1518948027840275, - "angularVelocity": 0.4647929421060861, - "velocityX": -1.4648784877052072, - "velocityY": -0.26880245801323005, - "timestamp": 6.032466574096115 - }, - { - "x": 7.302638403414429, - "y": 7.342803144016317, - "heading": 0.19579992682168443, - "angularVelocity": 0.5123673990532579, - "velocityX": -1.7978838234897747, - "velocityY": -0.2986093284474086, - "timestamp": 6.118157279864639 - }, - { - "x": 7.12003674107026, - "y": 7.314663079535023, - "heading": 0.24329864617596214, - "angularVelocity": 0.5543042145385703, - "velocityX": -2.1309389473046063, - "velocityY": -0.3283910924634972, - "timestamp": 6.203847985633163 - }, - { - "x": 6.908890435767407, - "y": 7.283973843410994, - "heading": 0.29374179011633406, - "angularVelocity": 0.5886652874190753, - "velocityX": -2.4640514208532833, - "velocityY": -0.35813961209433026, - "timestamp": 6.289538691401687 - }, - { - "x": 6.669194114784965, - "y": 7.250739514431078, - "heading": 0.34620821502428967, - "angularVelocity": 0.6122767275331199, - "velocityX": -2.797226593394282, - "velocityY": -0.3878405327841778, - "timestamp": 6.375229397170211 - }, - { - "x": 6.400943140096472, - "y": 7.214966665234931, - "heading": 0.3992869291597751, - "angularVelocity": 0.6194220675328131, - "velocityX": -3.1304558911338245, - "velocityY": -0.41746475157736485, - "timestamp": 6.4609201029387355 - }, - { - "x": 6.104139554987064, - "y": 7.176668008880963, - "heading": 0.4505490205752772, - "angularVelocity": 0.5982223037580753, - "velocityX": -3.4636613439870674, - "velocityY": -0.44694061054210893, - "timestamp": 6.54661080870726 - }, - { - "x": 5.778829029190598, - "y": 7.135876707023435, - "heading": 0.4948586572551853, - "angularVelocity": 0.5170880118504484, - "velocityX": -3.796333836661644, - "velocityY": -0.4760294770790753, - "timestamp": 6.632301514475784 - }, - { - "x": 5.425610556727763, - "y": 7.092704257460775, - "heading": 0.5143700783127158, - "angularVelocity": 0.22769588466497723, - "velocityX": -4.122016142765593, - "velocityY": -0.5038171780179022, - "timestamp": 6.717992220244308 - }, - { - "x": 5.0649174675452295, - "y": 7.060199768590457, - "heading": 0.5143700896531384, - "angularVelocity": 1.323413376854098e-7, - "velocityX": -4.209244000823966, - "velocityY": -0.3793233884444995, - "timestamp": 6.803682926012832 - }, - { - "x": 4.704224330497994, - "y": 7.027695810865106, - "heading": 0.5143701009934816, - "angularVelocity": 1.3234040971535758e-7, - "velocityX": -4.209244559399155, - "velocityY": -0.3793171900480436, - "timestamp": 6.889373631781356 - }, - { - "x": 4.343531193449039, - "y": 6.995191853158838, - "heading": 0.5143701123338247, - "angularVelocity": 1.3234040961236046e-7, - "velocityX": -4.209244559419223, - "velocityY": -0.3793171898253618, - "timestamp": 6.97506433754988 - }, - { - "x": 3.9828380564000834, - "y": 6.96268789545257, - "heading": 0.5143701236741678, - "angularVelocity": 1.3234040939269306e-7, - "velocityX": -4.209244559419224, - "velocityY": -0.3793171898253537, - "timestamp": 7.060755043318404 - }, - { - "x": 3.622144919351128, - "y": 6.9301839377463015, - "heading": 0.5143701350145109, - "angularVelocity": 1.3234041017948015e-7, - "velocityX": -4.209244559419224, - "velocityY": -0.3793171898253545, - "timestamp": 7.146445749086928 - }, - { - "x": 3.261451782302309, - "y": 6.897679980038524, - "heading": 0.5143701463548541, - "angularVelocity": 1.323404096872133e-7, - "velocityX": -4.209244559417636, - "velocityY": -0.3793171898429695, - "timestamp": 7.2321364548554525 - }, - { - "x": 2.90075864904001, - "y": 6.8651759803147625, - "heading": 0.5143701576957388, - "angularVelocity": 1.323467305471061e-7, - "velocityX": -4.209244515229405, - "velocityY": -0.3793176801643355, - "timestamp": 7.317827160623977 - }, - { - "x": 2.5585689575787693, - "y": 6.834269234981658, - "heading": 0.5701176468934943, - "angularVelocity": 0.6505663443634795, - "velocityX": -3.9933116245488067, - "velocityY": -0.3606779178198484, - "timestamp": 7.403517866392501 - }, - { - "x": 2.2448896661211246, - "y": 6.805966809821415, - "heading": 0.6318849384659164, - "angularVelocity": 0.7208166978957257, - "velocityX": -3.6605987620755966, - "velocityY": -0.3302858216233596, - "timestamp": 7.489208572161025 - }, - { - "x": 1.9597589348765612, - "y": 6.780255296283315, - "heading": 0.6923555719808244, - "angularVelocity": 0.7056848577984265, - "velocityX": -3.327440574649768, - "velocityY": -0.30005020156507956, - "timestamp": 7.574899277929549 - }, - { - "x": 1.7031654665176974, - "y": 6.7571261299629715, - "heading": 0.7492015690369473, - "angularVelocity": 0.6633857960007982, - "velocityX": -2.9944142256453965, - "velocityY": -0.2699145270529415, - "timestamp": 7.660589983698073 - }, - { - "x": 1.4750995700489262, - "y": 6.736574281325872, - "heading": 0.8012653996481951, - "angularVelocity": 0.6075785015925457, - "velocityX": -2.661500969368176, - "velocityY": -0.23983754659012796, - "timestamp": 7.746280689466597 - }, - { - "x": 1.2755541466650087, - "y": 6.718596481742583, - "heading": 0.8478475160673707, - "angularVelocity": 0.5436075709891741, - "velocityX": -2.3286705552752447, - "velocityY": -0.20979871063090086, - "timestamp": 7.831971395235121 - }, - { - "x": 1.1045239164164753, - "y": 6.70319044879611, - "heading": 0.8884763550283885, - "angularVelocity": 0.4741335550528443, - "velocityX": -1.995901757543409, - "velocityY": -0.17978651019737726, - "timestamp": 7.917662101003645 - }, - { - "x": 0.9620048380820003, - "y": 6.6903545095834955, - "heading": 0.922811925478865, - "angularVelocity": 0.4006918853396639, - "velocityX": -1.6631801203675514, - "velocityY": -0.14979383233565072, - "timestamp": 8.00335280677217 - }, - { - "x": 0.8479937432156998, - "y": 6.68008739430419, - "heading": 0.9505984608842503, - "angularVelocity": 0.32426545161671033, - "velocityX": -1.3304954585655717, - "velocityY": -0.11981597289022426, - "timestamp": 8.089043512540695 - }, - { - "x": 0.7624881048917089, - "y": 6.672388111768695, - "heading": 0.9716385035370365, - "angularVelocity": 0.2455347107260553, - "velocityX": -0.9978402856777302, - "velocityY": -0.08984968050435474, - "timestamp": 8.174734218309219 - }, - { - "x": 0.7054858870274207, - "y": 6.667255869051189, - "heading": 0.9857775356805354, - "angularVelocity": 0.16500076661397245, - "velocityX": -0.6652088736235632, - "velocityY": -0.05989264146534099, - "timestamp": 8.260424924077743 + "x": 1.9075019379268083, + "y": 5.42424212633875, + "heading": -0.06991588076878923, + "angularVelocity": 0.5076810269523403, + "velocityX": -0.6836899946235904, + "velocityY": 0.8632132756201296, + "timestamp": 2.6479114311741405 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.08305155696389553, - "velocityX": -0.33259668167527334, - "velocityY": -0.02994316977531714, - "timestamp": 8.346115629846267 + "x": 1.861590779787906, + "y": 5.48200360252104, + "heading": -0.03588037308734902, + "angularVelocity": 0.3969208844854328, + "velocityX": -0.5354142992901785, + "velocityY": 0.673612288379681, + "timestamp": 2.733660276880669 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -6.062522351787066e-32, - "velocityX": -8.072462222466964e-33, - "velocityY": 1.3269824927509524e-31, - "timestamp": 8.431806335614791 - }, - { - "x": 0.6915272700897366, - "y": 6.664883318998584, - "heading": 0.9848413600463822, - "angularVelocity": -0.13135885163926445, - "velocityX": 0.23720554048226594, - "velocityY": 0.0031531208242943447, - "timestamp": 8.493111090224545 - }, - { - "x": 0.7206154156192573, - "y": 6.665271312791946, - "heading": 0.9688837454817516, - "angularVelocity": -0.26029978696125183, - "velocityX": 0.47448433183831634, - "velocityY": 0.006328934775645454, - "timestamp": 8.554415844834299 - }, - { - "x": 0.7642550785259183, - "y": 6.665855624480287, - "heading": 0.9451981943153414, - "angularVelocity": -0.38635749082081516, - "velocityX": 0.7118479338912146, - "velocityY": 0.009531262168170858, - "timestamp": 8.615720599444053 - }, - { - "x": 0.82245219308383, - "y": 6.6666381477571255, - "heading": 0.9139918901741649, - "angularVelocity": -0.5090356260264929, - "velocityX": 0.9493083355177907, - "velocityY": 0.012764479391858926, - "timestamp": 8.677025354053807 - }, - { - "x": 0.8952134669088825, - "y": 6.667621071916812, - "heading": 0.8755051863660224, - "angularVelocity": -0.6277931304535281, - "velocityX": 1.1868781514293212, - "velocityY": 0.01603340827221871, - "timestamp": 8.738330108663561 - }, - { - "x": 0.982546442157363, - "y": 6.668806895228603, - "heading": 0.8300158681177399, - "angularVelocity": -0.7420194165665126, - "velocityX": 1.4245709946057887, - "velocityY": 0.019343088792048747, - "timestamp": 8.799634863273315 - }, - { - "x": 1.0844595931321694, - "y": 6.670198423358961, - "heading": 0.777846020332524, - "angularVelocity": -0.8509918703257573, - "velocityX": 1.6624020701747062, - "velocityY": 0.022698535198719637, - "timestamp": 8.86093961788307 - }, - { - "x": 1.2009624731033306, - "y": 6.67179876210547, - "heading": 0.7193731323687549, - "angularVelocity": -0.9538067371118101, - "velocityX": 1.9003889781923289, - "velocityY": 0.026104643215625493, - "timestamp": 8.922244372492823 - }, - { - "x": 1.3320659135249422, - "y": 6.673611326998587, - "heading": 0.6550478077481424, - "angularVelocity": -1.0492713824578042, - "velocityX": 2.1385525683313547, - "velocityY": 0.029566465189439763, - "timestamp": 8.983549127102577 - }, - { - "x": 1.477782250869489, - "y": 6.675639904928793, - "heading": 0.5854215593608406, - "angularVelocity": -1.1357397779425142, - "velocityX": 2.376917390374211, - "velocityY": 0.03309005872578241, - "timestamp": 9.044853881712331 - }, - { - "x": 1.638125489580401, - "y": 6.677888808467273, - "heading": 0.5111902482189525, - "angularVelocity": -1.2108573244346357, - "velocityX": 2.615510652177078, - "velocityY": 0.03668399870117081, - "timestamp": 9.106158636322085 - }, - { - "x": 1.8131111470662002, - "y": 6.680363155292976, - "heading": 0.4332634603071458, - "angularVelocity": -1.271137751188529, - "velocityX": 2.8543570331486037, - "velocityY": 0.04036141799202673, - "timestamp": 9.16746339093184 - }, - { - "x": 2.002755069495207, - "y": 6.68306928605088, - "heading": 0.3528824516114434, - "angularVelocity": -1.3111708742230885, - "velocityX": 3.0934618960016964, - "velocityY": 0.04414226555721556, - "timestamp": 9.228768145541594 - }, - { - "x": 2.2070689242126256, - "y": 6.686015356595887, - "heading": 0.2718454606802179, - "angularVelocity": -1.3218712226658607, - "velocityX": 3.332757075988891, - "velocityY": 0.048056151007561554, - "timestamp": 9.290072900151348 - }, - { - "x": 2.4260429630037788, - "y": 6.689212530783897, - "heading": 0.19302675484783927, - "angularVelocity": -1.2856866703751342, - "velocityX": 3.571893243600301, - "velocityY": 0.0521521407003806, - "timestamp": 9.351377654761102 - }, - { - "x": 2.659559040385731, - "y": 6.692680446587641, - "heading": 0.12201145373865255, - "angularVelocity": -1.1583979344056987, - "velocityX": 3.809102228178563, - "velocityY": 0.05656846399302349, - "timestamp": 9.412682409370856 - }, - { - "x": 2.9064602664817145, - "y": 6.696506622995245, - "heading": 0.07684542868597768, - "angularVelocity": -0.7367458746093568, - "velocityX": 4.027440084666806, - "velocityY": 0.06241239251280606, - "timestamp": 9.47398716398061 - }, - { - "x": 3.164834883229884, - "y": 6.7006990530219355, - "heading": 0.07426007030156662, - "angularVelocity": -0.042172232820580385, - "velocityX": 4.214593442105751, - "velocityY": 0.06838670268526725, - "timestamp": 9.535291918590364 - }, - { - "x": 3.423902662418304, - "y": 6.704266454413081, - "heading": 0.07426004244315124, - "angularVelocity": -4.5442503703406033e-7, - "velocityX": 4.225900272133267, - "velocityY": 0.05819126777122497, - "timestamp": 9.596596673200118 - }, - { - "x": 3.6829704424934793, - "y": 6.7078337914078086, - "heading": 0.07426001458477352, - "angularVelocity": -4.5442442246011853e-7, - "velocityX": 4.22590028659796, - "velocityY": 0.0581902173401686, - "timestamp": 9.657901427809872 - }, - { - "x": 3.9420382225687436, - "y": 6.71140112839604, - "heading": 0.07425998672639592, - "angularVelocity": -4.5442442083032825e-7, - "velocityX": 4.225900286599419, - "velocityY": 0.05819021723419636, - "timestamp": 9.719206182419626 - }, - { - "x": 4.201106002644009, - "y": 6.714968465384217, - "heading": 0.07425995886801827, - "angularVelocity": -4.5442442166666195e-7, - "velocityX": 4.225900286599431, - "velocityY": 0.05819021723333154, - "timestamp": 9.78051093702938 - }, - { - "x": 4.46017378272642, - "y": 6.718535801853429, - "heading": 0.07425993100964065, - "angularVelocity": -4.5442442082699014e-7, - "velocityX": 4.225900286715997, - "velocityY": 0.05819020876799694, - "timestamp": 9.841815691639134 - }, - { - "x": 4.719241633592179, - "y": 6.7220979941697125, - "heading": 0.07425990315123465, - "angularVelocity": -4.544248838946695e-7, - "velocityX": 4.225901441330322, - "velocityY": 0.058106297610346594, - "timestamp": 9.903120446248888 - }, - { - "x": 4.978174118425727, - "y": 6.712998060028401, - "heading": 0.07425987514278792, - "angularVelocity": -4.568723405675715e-7, - "velocityX": 4.223693357584233, - "velocityY": -0.1484376570665475, - "timestamp": 9.964425200858642 - }, - { - "x": 5.236176330694113, - "y": 6.689255719100931, - "heading": 0.0742598466206584, - "angularVelocity": -4.652515078556177e-7, - "velocityX": 4.208518799410349, - "velocityY": -0.3872838424785543, - "timestamp": 10.025729955468396 - }, - { - "x": 5.4924208313627965, - "y": 6.650946506335976, - "heading": 0.07425981717735317, - "angularVelocity": -4.802776788917255e-7, - "velocityX": 4.179847098318136, - "velocityY": -0.6248979056978337, - "timestamp": 10.08703471007815 + "x": 1.8296344375134035, + "y": 5.522099910983847, + "heading": -0.012259641477970167, + "angularVelocity": 0.2754641349951199, + "velocityX": -0.3726737311878495, + "velocityY": 0.4676017284248481, + "timestamp": 2.8194091225871976 }, { - "x": 5.7460856437683105, - "y": 6.598193168640137, - "heading": 0.0742592023389195, - "angularVelocity": -0.000010029212865619508, - "velocityX": 4.137767356223229, - "velocityY": -0.8605097276981168, - "timestamp": 10.148339464687904 - }, - { - "x": 5.9699164008558405, - "y": 6.543017986029004, - "heading": 0.05198341258682499, - "angularVelocity": -0.39649724748489784, - "velocityX": 3.984068806329142, - "velocityY": -0.9820889978876548, - "timestamp": 10.2045209133446 - }, - { - "x": 6.183022450140719, - "y": 6.4863677530582065, - "heading": 0.0038718915492881255, - "angularVelocity": -0.8563595668657032, - "velocityX": 3.7931746934310255, - "velocityY": -1.0083441122525065, - "timestamp": 10.260702362001297 - }, - { - "x": 6.384034644805308, - "y": 6.430498554479071, - "heading": -0.05330736038793806, - "angularVelocity": -1.0177603693814712, - "velocityX": 3.5779104930685666, - "velocityY": -0.9944421141670974, - "timestamp": 10.316883810657993 - }, - { - "x": 6.5728983417799105, - "y": 6.37648577029989, - "heading": -0.11411359843449789, - "angularVelocity": -1.082318799184461, - "velocityX": 3.36167367503596, - "velocityY": -0.9613989220753596, - "timestamp": 10.37306525931469 - }, - { - "x": 6.74968082080873, - "y": 6.324889922313049, - "heading": -0.17585578783167868, - "angularVelocity": -1.098978237006013, - "velocityX": 3.1466344007800133, - "velocityY": -0.9183787392547113, - "timestamp": 10.429246707971386 - }, - { - "x": 6.914453957859755, - "y": 6.2760504067352585, - "heading": -0.2368981836253953, - "angularVelocity": -1.0865222818785416, - "velocityX": 2.932874480647331, - "velocityY": -0.8693174837165495, - "timestamp": 10.485428156628082 - }, - { - "x": 7.067279207384589, - "y": 6.230193439539363, - "heading": -0.2961318737080175, - "angularVelocity": -1.0543282791544983, - "velocityX": 2.72020841717146, - "velocityY": -0.8162297037961601, - "timestamp": 10.541609605284778 - }, - { - "x": 7.2082072429421995, - "y": 6.1874800327074295, - "heading": -0.3527517462037597, - "angularVelocity": -1.0078037118929446, - "velocityX": 2.5084443161793004, - "velocityY": -0.7602759959597819, - "timestamp": 10.597791053941474 - }, - { - "x": 7.337279834118698, - "y": 6.148030397062498, - "heading": -0.4061452108111807, - "angularVelocity": -0.9503753620467785, - "velocityX": 2.2974236916746635, - "velocityY": -0.7021825992062444, - "timestamp": 10.65397250259817 - }, - { - "x": 7.454531717952861, - "y": 6.111937602538107, - "heading": -0.45583024333208966, - "angularVelocity": -0.8843672370307636, - "velocityX": 2.087021368043439, - "velocityY": -0.6424326069792218, - "timestamp": 10.710153951254867 - }, - { - "x": 7.559992100674049, - "y": 6.079275791201654, - "heading": -0.5014179550675222, - "angularVelocity": -0.8114370993528713, - "velocityX": 1.8771388998104257, - "velocityY": -0.5813629252608888, - "timestamp": 10.766335399911563 - }, - { - "x": 7.653685805762832, - "y": 6.050105397874059, - "heading": -0.5425884668253612, - "angularVelocity": -0.7328132816477557, - "velocityX": 1.6676982763707042, - "velocityY": -0.5192175357713573, - "timestamp": 10.82251684856826 - }, - { - "x": 7.735634146205772, - "y": 6.02447662157365, - "heading": -0.5790745351653838, - "angularVelocity": -0.649432672392886, - "velocityX": 1.4586370127921349, - "velocityY": -0.4561786303699093, - "timestamp": 10.878698297224956 - }, - { - "x": 7.8058555921649635, - "y": 6.002431822282749, - "heading": -0.6106499848823828, - "angularVelocity": -0.5620262643981344, - "velocityX": 1.249904508306431, - "velocityY": -0.39238573974139485, - "timestamp": 10.934879745881652 - }, - { - "x": 7.864366288021797, - "y": 5.984007229520149, - "heading": -0.6371212833352664, - "angularVelocity": -0.4711750779984228, - "velocityX": 1.0414593652500725, - "velocityY": -0.32794798288639887, - "timestamp": 10.991061194538348 - }, - { - "x": 7.911180457736072, - "y": 5.969234194186617, - "heading": -0.6583212554852172, - "angularVelocity": -0.37734826454005344, - "velocityX": 0.833267401136955, - "velocityY": -0.26295219661928954, - "timestamp": 11.047242643195045 - }, - { - "x": 7.946310726470828, - "y": 5.958140127507551, - "heading": -0.6741042985083451, - "angularVelocity": -0.2809297980116511, - "velocityX": 0.6253001582323476, - "velocityY": -0.19746850507286262, - "timestamp": 11.10342409185174 - }, - { - "x": 7.969768378758335, - "y": 5.950749219301424, - "heading": -0.6843426613939789, - "angularVelocity": -0.1822374312238999, - "velocityX": 0.41753377401939834, - "velocityY": -0.13155424758250212, - "timestamp": 11.159605540508437 + "x": 1.8129411935806854, + "y": 5.542999267578127, + "heading": 2.3632850028431835e-17, + "angularVelocity": 0.14297150447865262, + "velocityX": -0.1946760191950624, + "velocityY": 0.24372755600475027, + "timestamp": 2.905157968293726 }, { - "x": 7.981563568115234, - "y": 5.947082996368408, - "heading": -0.6889234822214203, - "angularVelocity": -0.08153618208446338, - "velocityX": 0.2099481170194555, - "velocityY": -0.06525682446209594, - "timestamp": 11.215786989165133 - }, - { - "x": 7.98107279837352, - "y": 5.947370479695571, - "heading": -0.6873686383472613, - "angularVelocity": 0.02630231709530022, - "velocityX": -0.008302043428202248, - "velocityY": 0.004863174853957543, - "timestamp": 11.274901319987226 - }, - { - "x": 7.967670719276277, - "y": 5.9517826740935895, - "heading": -0.6795422167344298, - "angularVelocity": 0.1323946580125472, - "velocityX": -0.22671455315256586, - "velocityY": 0.07463832097325668, - "timestamp": 11.334015650809318 - }, - { - "x": 7.941346705017116, - "y": 5.960296828520997, - "heading": -0.6655606707736306, - "angularVelocity": 0.23651703007308753, - "velocityX": -0.4453068129686613, - "velocityY": 0.14402860201583023, - "timestamp": 11.39312998163141 - }, - { - "x": 7.902088914435571, - "y": 5.972887353790235, - "heading": -0.6455561559979053, - "angularVelocity": 0.338403809998799, - "velocityX": -0.6640993822579894, - "velocityY": 0.21298600684714283, - "timestamp": 11.452244312453503 - }, - { - "x": 7.849884062968043, - "y": 5.989525223865173, - "heading": -0.6196795941981793, - "angularVelocity": 0.43773754079366983, - "velocityX": -0.8831166781645818, - "velocityY": 0.281452396458829, - "timestamp": 11.511358643275596 - }, - { - "x": 7.78471714102411, - "y": 6.010177207449998, - "heading": -0.5881045676669778, - "angularVelocity": 0.5341348889870459, - "velocityX": -1.1023878818835982, - "velocityY": 0.34935663311453924, - "timestamp": 11.570472974097688 - }, - { - "x": 7.706571062429961, - "y": 6.034804864474398, - "heading": -0.5510323547922651, - "angularVelocity": 0.6271273371305467, - "velocityX": -1.3219481216717965, - "velocityY": 0.4166106032481136, - "timestamp": 11.62958730491978 - }, - { - "x": 7.615426220504745, - "y": 6.063363207617709, - "heading": -0.5086985338851634, - "angularVelocity": 0.7161346549706845, - "velocityX": -1.5418400353633617, - "velocityY": 0.48310355113818987, - "timestamp": 11.688701635741873 - }, - { - "x": 7.511259920040068, - "y": 6.095798872359664, - "heading": -0.46138178697105986, - "angularVelocity": 0.8004276840501866, - "velocityX": -1.7621158696386412, - "velocityY": 0.5486937649615388, - "timestamp": 11.747815966563966 - }, - { - "x": 7.39404563921687, - "y": 6.132047543004427, - "heading": -0.4094159214388897, - "angularVelocity": 0.8790739032226302, - "velocityX": -1.982840356866446, - "velocityY": 0.6131959905603276, - "timestamp": 11.806930297386058 - }, - { - "x": 7.263752054530884, - "y": 6.172030212832479, - "heading": -0.35320687491635055, - "angularVelocity": 0.9508531305497406, - "velocityX": -2.2040947241391, - "velocityY": 0.6763617091155382, - "timestamp": 11.86604462820815 - }, - { - "x": 7.120341734246255, - "y": 6.215647542637923, - "heading": -0.293257939808125, - "angularVelocity": 1.0141184764257163, - "velocityX": -2.425982300573299, - "velocityY": 0.7378469687276493, - "timestamp": 11.925158959030243 - }, - { - "x": 6.963769381897745, - "y": 6.262770961252908, - "heading": -0.23020941920008114, - "angularVelocity": 1.066552217224485, - "velocityX": -2.64863612885552, - "velocityY": 0.7971572706592246, - "timestamp": 11.984273289852336 - }, - { - "x": 6.793979543954, - "y": 6.313227822369206, - "heading": -0.16490527005597488, - "angularVelocity": 1.1047092682253812, - "velocityX": -2.8722280296927845, - "velocityY": 0.8535470234476921, - "timestamp": 12.043387620674428 - }, - { - "x": 6.610904026180611, - "y": 6.366774787590835, - "heading": -0.09851409628601165, - "angularVelocity": 1.1230977809724585, - "velocityX": -3.0969735295552177, - "velocityY": 0.9058203734519391, - "timestamp": 12.10250195149652 - }, - { - "x": 6.414461060118511, - "y": 6.423045155535491, - "heading": -0.03277169507390691, - "angularVelocity": 1.112122903157957, - "velocityX": -3.3231022550742657, - "velocityY": 0.9518904665267254, - "timestamp": 12.161616282318613 - }, - { - "x": 6.20456835252053, - "y": 6.481428804370567, - "heading": 0.029452709079620762, - "angularVelocity": 1.0526111568579828, - "velocityX": -3.5506230837606094, - "velocityY": 0.9876395118264049, - "timestamp": 12.220730613140706 - }, - { - "x": 5.981258970375398, - "y": 6.540731988125068, - "heading": 0.08236638837911917, - "angularVelocity": 0.8951074733256352, - "velocityX": -3.7775845389706695, - "velocityY": 1.0031947064236981, - "timestamp": 12.279844943962798 + "x": 1.812941193580624, + "y": 5.542999267578126, + "heading": -4.920156603511903e-17, + "angularVelocity": -5.733397110540358e-17, + "velocityX": -7.214388151371406e-13, + "velocityY": -9.03683027618772e-16, + "timestamp": 2.9909068140002546 }, { - "x": 5.7460856437683105, - "y": 6.598193168640137, - "heading": 0.10673916956816744, - "angularVelocity": 0.41229902884969827, - "velocityX": -3.9782794347255015, - "velocityY": 0.9720346947341876, - "timestamp": 12.33895927478489 - }, - { - "x": 5.482731803813418, - "y": 6.650320477040453, - "heading": 0.10673940582411118, - "angularVelocity": 0.0000037192747979075137, - "velocityX": -4.145865218900198, - "velocityY": 0.8206175952808282, - "timestamp": 12.402481320497886 - }, - { - "x": 5.216773824306616, - "y": 6.6869112798369965, - "heading": 0.10673943110586125, - "angularVelocity": 3.9799962033997064e-7, - "velocityX": -4.186861057788582, - "velocityY": 0.5760331296927703, - "timestamp": 12.466003366210881 - }, - { - "x": 4.9491275418069325, - "y": 6.707839765678222, - "heading": 0.10673945565224871, - "angularVelocity": 3.8642312574461983e-7, - "velocityX": -4.213439279159305, - "velocityY": 0.32946807059370153, - "timestamp": 12.529525411923876 - }, - { - "x": 4.680714529727759, - "y": 6.71303487609764, - "heading": 0.1067394798338107, - "angularVelocity": 3.8067983666652777e-7, - "velocityX": -4.225509570203694, - "velocityY": 0.08178436889281773, - "timestamp": 12.593047457636871 - }, - { - "x": 4.4122692055154475, - "y": 6.709929692152406, - "heading": 0.10673950394272769, - "angularVelocity": 3.795362179203617e-7, - "velocityX": -4.226018246093679, - "velocityY": -0.04888356334214293, - "timestamp": 12.656569503349866 - }, - { - "x": 4.143823892978554, - "y": 6.706823499022996, - "heading": 0.10673952805164147, - "angularVelocity": 3.795361675287501e-7, - "velocityX": -4.226018062292654, - "velocityY": -0.048899450490688265, - "timestamp": 12.720091549062861 - }, - { - "x": 3.875378580442675, - "y": 6.703717305805851, - "heading": 0.10673955216055508, - "angularVelocity": 3.7953616500379395e-7, - "velocityX": -4.226018062276673, - "velocityY": -0.04889945187186359, - "timestamp": 12.783613594775856 - }, - { - "x": 3.606933267906796, - "y": 6.700611112588698, - "heading": 0.10673957626946878, - "angularVelocity": 3.7953616612214584e-7, - "velocityX": -4.2260180622766725, - "velocityY": -0.04889945187199676, - "timestamp": 12.847135640488851 - }, - { - "x": 3.3384879553710274, - "y": 6.697504919362028, - "heading": 0.10673960037838261, - "angularVelocity": 3.79536168074433e-7, - "velocityX": -4.226018062274939, - "velocityY": -0.04889945202181832, - "timestamp": 12.910657686201846 - }, - { - "x": 3.0700426441020716, - "y": 6.694398616665168, - "heading": 0.10673962448757092, - "angularVelocity": 3.795404893645561e-7, - "velocityX": -4.22601804233206, - "velocityY": -0.0489011753634946, - "timestamp": 12.974179731914841 - }, - { - "x": 2.806808379331506, - "y": 6.690344844376677, - "heading": 0.1255043030972768, - "angularVelocity": 0.2954041923411685, - "velocityX": -4.143982798663499, - "velocityY": -0.06381677798612607, - "timestamp": 13.037701777627836 - }, - { - "x": 2.556272153813946, - "y": 6.687083030964068, - "heading": 0.1867455578699796, - "angularVelocity": 0.964094497985838, - "velocityX": -3.9440830770711344, - "velocityY": -0.05134931307701316, - "timestamp": 13.101223823340831 - }, - { - "x": 2.3210540999050693, - "y": 6.6841316507490856, - "heading": 0.2629720657012657, - "angularVelocity": 1.2000008339733441, - "velocityX": -3.7029357488208183, - "velocityY": -0.046462297960575334, - "timestamp": 13.164745869053826 - }, - { - "x": 2.1015488183646034, - "y": 6.68142600254672, - "heading": 0.34363574720189305, - "angularVelocity": 1.2698533335195523, - "velocityX": -3.4555763920487643, - "velocityY": -0.04259384552239664, - "timestamp": 13.228267914766821 - }, - { - "x": 1.8977951102037987, - "y": 6.678949586554031, - "heading": 0.4245682813102029, - "angularVelocity": 1.2740857634525757, - "velocityX": -3.207606207794462, - "velocityY": -0.038985142321737026, - "timestamp": 13.291789960479816 - }, - { - "x": 1.7097851332749783, - "y": 6.676691054407077, - "heading": 0.5034534441826332, - "angularVelocity": 1.2418548865515076, - "velocityX": -2.9597594790679125, - "velocityY": -0.03555509148993883, - "timestamp": 13.355312006192811 - }, - { - "x": 1.5375030148010667, - "y": 6.674642046081059, - "heading": 0.5788036623200312, - "angularVelocity": 1.1862057855920731, - "velocityX": -2.7121626285827998, - "velocityY": -0.03225664890069088, - "timestamp": 13.418834051905806 - }, - { - "x": 1.3809327880417555, - "y": 6.67279615304415, - "heading": 0.6495791212494125, - "angularVelocity": 1.1141873366163184, - "velocityX": -2.46481713556153, - "velocityY": -0.02905909304700025, - "timestamp": 13.482356097618801 - }, - { - "x": 1.2400601320509559, - "y": 6.671148237175822, - "heading": 0.7150087917511168, - "angularVelocity": 1.0300309092268365, - "velocityX": -2.2176970909798954, - "velocityY": -0.025942424395041352, - "timestamp": 13.545878143331796 - }, - { - "x": 1.1148725609299421, - "y": 6.669694026119316, - "heading": 0.7744947973618744, - "angularVelocity": 0.9364623721271129, - "velocityX": -1.9707736064835946, - "velocityY": -0.022893013601536772, - "timestamp": 13.609400189044791 - }, - { - "x": 1.0053592312326722, - "y": 6.668429889682977, - "heading": 0.8275574496076633, - "angularVelocity": 0.8353423075436972, - "velocityX": -1.7240208256527763, - "velocityY": -0.019900751339958393, - "timestamp": 13.672922234757786 - }, - { - "x": 0.9115106985701406, - "y": 6.667352716414464, - "heading": 0.873802307501142, - "angularVelocity": 0.7280127296658919, - "velocityX": -1.4774167237396383, - "velocityY": -0.016957471322305857, - "timestamp": 13.736444280470781 - }, - { - "x": 0.8333187122321524, - "y": 6.66645983627761, - "heading": 0.9128998853211554, - "angularVelocity": 0.6154962010616661, - "velocityX": -1.2309425091766681, - "velocityY": -0.014056224525380053, - "timestamp": 13.799966326183776 - }, - { - "x": 0.7707760609431589, - "y": 6.665748959829805, - "heading": 0.9445728281790685, - "angularVelocity": 0.49861339480497757, - "velocityX": -0.984581818595295, - "velocityY": -0.011191019429984484, - "timestamp": 13.863488371896771 - }, - { - "x": 0.7238764608943239, - "y": 6.665218122050303, - "heading": 0.9685875457966591, - "angularVelocity": 0.37805327816572476, - "velocityX": -0.7383200512895466, - "velocityY": -0.008356748803399568, - "timestamp": 13.927010417609766 - }, - { - "x": 0.692614473543579, - "y": 6.664865629845864, - "heading": 0.9847485268863282, - "angularVelocity": 0.2544153121687507, - "velocityX": -0.4921439005915036, - "velocityY": -0.005549131808990834, - "timestamp": 13.990532463322761 + "x": 1.8513325137310372, + "y": 5.5430599450572595, + "heading": -0.000016850565291482023, + "angularVelocity": -0.00016991086246254008, + "velocityX": 0.3871147468977978, + "velocityY": 0.0006118348336243464, + "timestamp": 3.090079786505359 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.12823509120632537, - "velocityX": -0.24604105125639145, - "velocityY": -0.002764585801632022, - "timestamp": 14.054054509035756 + "x": 1.9275402570059652, + "y": 5.543180391388335, + "heading": -0.000015746696122309923, + "angularVelocity": 0.00001113074602288469, + "velocityX": 0.7684325814778618, + "velocityY": 0.0012145076227106227, + "timestamp": 3.189252759010463 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -8.295390282600847e-37, - "velocityX": 2.201674205631738e-38, - "velocityY": -2.6267125304593916e-36, - "timestamp": 14.117576554748752 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 2.0338963277323163, + "y": 5.54334848714078, + "heading": 0.000009170336978677823, + "angularVelocity": 0.00025124822289482134, + "velocityX": 1.0724299982122594, + "velocityY": 0.0016949754373461064, + "timestamp": 3.2884257315155674 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 2.163383514243291, + "y": 5.5435531416216355, + "heading": 0.000033654463962368084, + "angularVelocity": 0.0002468830606297171, + "velocityX": 1.305670115961409, + "velocityY": 0.00206361144256778, + "timestamp": 3.3875987040206716 }, { - "scope": [ - 2 - ], - "type": "StopPoint" + "x": 2.3079360710490797, + "y": 5.54378160701675, + "heading": 0.00003317773344428256, + "angularVelocity": -0.000004807060894986283, + "velocityX": 1.457580156714077, + "velocityY": 0.0023037062351123753, + "timestamp": 3.486771676525776 }, { - "scope": [ - 5 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "CenterLanePCBAFSprint": { - "waypoints": [ - { - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 2.437398077457075, + "y": 5.543986221690917, + "heading": 0.000022146124274066103, + "angularVelocity": -0.00011123604437218979, + "velocityX": 1.3054162151016944, + "velocityY": 0.002063210056106694, + "timestamp": 3.58594464903088 }, { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 2.5437229473484053, + "y": 5.544154268122903, + "heading": 0.000015406432122086, + "angularVelocity": -0.00006795896080977589, + "velocityX": 1.0721153879486738, + "velocityY": 0.0016944781198031775, + "timestamp": 3.6851176215359844 }, { - "x": 1.8129411935806274, - "y": 5.542999267578125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "x": 2.619913870706316, + "y": 5.544274687859312, + "heading": 0.000009213432317016853, + "angularVelocity": -0.00006244644733859944, + "velocityX": 0.7682629796539725, + "velocityY": 0.0012142394582550931, + "timestamp": 3.7842905940410887 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 + "x": 2.6583051681519314, + "y": 5.5443353652954785, + "heading": 6.370510606485668e-14, + "angularVelocity": -0.00009290265301587805, + "velocityX": 0.38711451795641516, + "velocityY": 0.0006118344003786568, + "timestamp": 3.883463566546193 }, { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 6 + "x": 2.658305168152, + "y": 5.544335365295488, + "heading": 6.625668653050138e-14, + "angularVelocity": 2.6906565131836242e-14, + "velocityX": 7.155954565396242e-13, + "velocityY": 8.671180221391495e-14, + "timestamp": 3.982636539051297 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 + "x": 2.6398879819889713, + "y": 5.568034006444528, + "heading": 0.008388499707182374, + "angularVelocity": 0.0943787022324955, + "velocityX": -0.20721108536177554, + "velocityY": 0.26663254150593735, + "timestamp": 4.071517817439562 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.6055876283476542, + "y": 5.613232266775393, + "heading": 0.025296828917652628, + "angularVelocity": 0.19023499118162804, + "velocityX": -0.3859120195327329, + "velocityY": 0.5085239675934979, + "timestamp": 4.160399095827827 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 + "x": 2.5578825329127857, + "y": 5.677978135359711, + "heading": 0.05006181260458975, + "angularVelocity": 0.27862992225075944, + "velocityX": -0.5367282773164671, + "velocityY": 0.7284533903912082, + "timestamp": 4.2492803742160925 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "x": 2.4993543830957403, + "y": 5.760558609750369, + "heading": 0.0821470752651059, + "angularVelocity": 0.36099011222914734, + "velocityX": -0.6584980648176972, + "velocityY": 0.9291098855468343, + "timestamp": 4.338161652604358 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 + "x": 2.433098392034208, + "y": 5.859358208334035, + "heading": 0.12114426250239245, + "angularVelocity": 0.43875592188117873, + "velocityX": -0.7454437229429913, + "velocityY": 1.1115906563809108, + "timestamp": 4.427042930992623 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.3637465533640976, + "y": 5.972668871410117, + "heading": 0.16678728284828395, + "angularVelocity": 0.5135279461940396, + "velocityX": -0.7802749907275444, + "velocityY": 1.2748541102325668, + "timestamp": 4.515924209380888 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.300988842467506, + "y": 6.095835737634532, + "heading": 0.21776795840799054, + "angularVelocity": 0.5735817090404455, + "velocityX": -0.7060847012414084, + "velocityY": 1.385745889998316, + "timestamp": 4.604805487769153 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 + "x": 2.2515752134889366, + "y": 6.217231027383832, + "heading": 0.2695393710632505, + "angularVelocity": 0.582478263071738, + "velocityX": -0.5559509254902292, + "velocityY": 1.3658139481175915, + "timestamp": 4.693686766157418 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.217045589242532, + "y": 6.33145065782542, + "heading": 0.3198059590268783, + "angularVelocity": 0.5655475357136119, + "velocityX": -0.3884915346931451, + "velocityY": 1.2850808686903799, + "timestamp": 4.782568044545683 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 15 + "x": 2.19907919431313, + "y": 6.435243176299548, + "heading": 0.36734143796630997, + "angularVelocity": 0.5348199283510537, + "velocityX": -0.20213924974232386, + "velocityY": 1.1677658147606698, + "timestamp": 4.871449322933948 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ + "x": 2.1993959276283994, + "y": 6.526053318679287, + "heading": 0.41157070153690833, + "angularVelocity": 0.4976218206191788, + "velocityX": 0.0035635549001480525, + "velocityY": 1.021701577951703, + "timestamp": 4.960330601322213 + }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.2424307943085809e-21, - "angularVelocity": -1.3399063907175068e-21, - "velocityX": -6.613526246981409e-20, - "velocityY": -3.127454467901795e-20, - "timestamp": 0 + "x": 2.219770279567901, + "y": 6.601608618777817, + "heading": 0.45152771224881855, + "angularVelocity": 0.4495548605556116, + "velocityX": 0.2292310856552352, + "velocityY": 0.8500699075051399, + "timestamp": 5.049211879710478 }, { - "x": 1.3060573925677244, - "y": 5.572382489557548, - "heading": -0.010694903924415845, - "angularVelocity": -0.14212387666711845, - "velocityX": 0.21372515458978017, - "velocityY": -0.2467996203811156, - "timestamp": 0.07525057840538203 - }, - { - "x": 1.3382234703338376, - "y": 5.5352391647325, - "heading": -0.03207617924105129, - "angularVelocity": -0.2841343650736142, - "velocityX": 0.4274528973429488, - "velocityY": -0.49359520700229137, - "timestamp": 0.15050115681076406 - }, - { - "x": 1.3864731960687624, - "y": 5.479524641793448, - "heading": -0.06412173122661301, - "angularVelocity": -0.4258512381516761, - "velocityX": 0.6411874401150632, - "velocityY": -0.7403866404709017, - "timestamp": 0.2257517352161461 - }, - { - "x": 1.4508074313628532, - "y": 5.405239333646717, - "heading": -0.10679840962568042, - "angularVelocity": -0.5671275796601069, - "velocityX": 0.8549334325048802, - "velocityY": -0.9871725868543993, - "timestamp": 0.30100231362152813 - }, - { - "x": 1.5312274241075188, - "y": 5.312383875814348, - "heading": -0.16006570142322632, - "angularVelocity": -0.7078655463694897, - "velocityX": 1.0686960080417647, - "velocityY": -1.2339500878272134, - "timestamp": 0.37625289202691015 - }, - { - "x": 1.6277348707062913, - "y": 5.200959293585018, - "heading": -0.22388050123959674, - "angularVelocity": -0.8480306885163599, - "velocityX": 1.282481126973902, - "velocityY": -1.480713963806038, - "timestamp": 0.45150347043229216 - }, - { - "x": 1.740332134586227, - "y": 5.070967309798755, - "heading": -0.29820239377925056, - "angularVelocity": -0.9876587544520216, - "velocityX": 1.4962976533331545, - "velocityY": -1.7274549450767482, - "timestamp": 0.5267540488376742 - }, - { - "x": 1.869023591989777, - "y": 4.922411591689274, - "heading": -0.38299783161333695, - "angularVelocity": -1.126841010806393, - "velocityX": 1.7101723352912572, - "velocityY": -1.9741471927192127, - "timestamp": 0.6020046272430563 - }, - { - "x": 2.0218400763966784, - "y": 4.778612689185939, - "heading": -0.4624367890743981, - "angularVelocity": -1.0556590945137452, - "velocityX": 2.030768236539849, - "velocityY": -1.9109341821756607, - "timestamp": 0.6772552056484383 - }, - { - "x": 2.158571900472673, - "y": 4.653385130962418, - "heading": -0.5314200615234237, - "angularVelocity": -0.9167141822805146, - "velocityX": 1.817020240554264, - "velocityY": -1.664140806319243, - "timestamp": 0.7525057840538204 - }, - { - "x": 2.2792154502458053, - "y": 4.546725640045298, - "heading": -0.5899528620113615, - "angularVelocity": -0.777838545939356, - "velocityX": 1.6032242187324346, - "velocityY": -1.4173909779475127, - "timestamp": 0.8277563624592025 - }, - { - "x": 2.383769251108378, - "y": 4.458632746098652, - "heading": -0.6380359031204368, - "angularVelocity": -0.6389723790566404, - "velocityX": 1.3894086009456479, - "velocityY": -1.170660688773468, - "timestamp": 0.9030069408645846 - }, - { - "x": 2.4722324605852046, - "y": 4.389105547577967, - "heading": -0.6756681828251195, - "angularVelocity": -0.5000928963223903, - "velocityX": 1.1755817875613708, - "velocityY": -0.9239423801653125, - "timestamp": 0.9782575192699666 - }, - { - "x": 2.5446045503525854, - "y": 4.33814344893939, - "heading": -0.7028480663782789, - "angularVelocity": -0.36119168954076036, - "velocityX": 0.9617479533181219, - "velocityY": -0.6772319857003534, - "timestamp": 1.0535080976753486 - }, - { - "x": 2.600885196215428, - "y": 4.305746063716301, - "heading": -0.7195737337553716, - "angularVelocity": -0.22226629657236585, - "velocityX": 0.7479098108675392, - "velocityY": -0.4305267269649537, - "timestamp": 1.1287586760807307 - }, - { - "x": 2.6410742311964075, - "y": 4.29191316967755, - "heading": -0.7258433495686913, - "angularVelocity": -0.08331651325714302, - "velocityX": 0.5340694494662552, - "velocityY": -0.1838244214447216, - "timestamp": 1.2040092544861127 + "x": 2.2626490848637184, + "y": 6.659247151615926, + "heading": 0.48436572022362784, + "angularVelocity": 0.36945922212429394, + "velocityX": 0.4824278641490618, + "velocityY": 0.648489016846146, + "timestamp": 5.1380931580987435 }, { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.05565717315833812, - "velocityX": 0.3202286619480617, - "velocityY": 0.06287683198324669, - "timestamp": 1.2792598328914948 - }, - { - "x": 2.6731094248331972, - "y": 4.320288497186145, - "heading": -0.7067986870474628, - "angularVelocity": 0.19580509628770462, - "velocityX": 0.10461882238047351, - "velocityY": 0.31162123137301345, - "timestamp": 1.355133384348122 - }, - { - "x": 2.6646882165451484, - "y": 4.3628057123101, - "heading": -0.6813129128721624, - "angularVelocity": 0.33589799984345314, - "velocityX": -0.11099003706005371, - "velocityY": 0.5603693817898957, - "timestamp": 1.431006935804749 - }, - { - "x": 2.6399081500891253, - "y": 4.424196738035329, - "heading": -0.6452019314670412, - "angularVelocity": 0.47593635347047025, - "velocityX": -0.32659689681441373, - "velocityY": 0.8091228701785946, - "timestamp": 1.5068804872613761 - }, - { - "x": 2.5987694540063986, - "y": 4.504462102028234, - "heading": -0.5984691810768386, - "angularVelocity": 0.6159293916394221, - "velocityX": -0.5422007444352149, - "velocityY": 1.057883313117199, - "timestamp": 1.5827540387180032 - }, - { - "x": 2.5412724297468685, - "y": 4.603602434973848, - "heading": -0.5411158632570503, - "angularVelocity": 0.7559065935192721, - "velocityX": -0.7578006189995966, - "velocityY": 1.30665206837309, - "timestamp": 1.6586275901746304 - }, - { - "x": 2.4674174122162253, - "y": 4.7216183838553025, - "heading": -0.4731379534998271, - "angularVelocity": 0.8959368377013766, - "velocityX": -0.973396079566176, - "velocityY": 1.5554293507523191, - "timestamp": 1.7345011416312575 - }, - { - "x": 2.3772045458929667, - "y": 4.8585103095087545, - "heading": -0.3945211417087051, - "angularVelocity": 1.0361556864260284, - "velocityX": -1.1889896359316807, - "velocityY": 1.8042113888883347, - "timestamp": 1.8103746930878846 - }, - { - "x": 2.270632337424553, - "y": 5.014276859543665, - "heading": -0.3052336675309315, - "angularVelocity": 1.176793130987344, - "velocityX": -1.4046028744197523, - "velocityY": 2.0529756027560517, - "timestamp": 1.8862482445445117 - }, - { - "x": 2.1562113194234667, - "y": 5.146461372900032, - "heading": -0.22910323394931606, - "angularVelocity": 1.0033856610117322, - "velocityX": -1.5080487970369372, - "velocityY": 1.7421685266957554, - "timestamp": 1.9621217960011388 - }, - { - "x": 2.05813446327852, - "y": 5.259759383516201, - "heading": -0.1637433708814866, - "angularVelocity": 0.861431444990317, - "velocityX": -1.2926356320754544, - "velocityY": 1.49324775815898, - "timestamp": 2.037995347457766 - }, - { - "x": 1.9764036449833426, - "y": 5.354173277082403, - "heading": -0.10920518207306891, - "angularVelocity": 0.7188036906324902, - "velocityX": -1.077197741849454, - "velocityY": 1.2443584325978703, - "timestamp": 2.1138688989143932 - }, - { - "x": 1.9110189705233616, - "y": 5.429703862947866, - "heading": -0.06553401357536529, - "angularVelocity": 0.5755782833319475, - "velocityX": -0.8617584547542609, - "velocityY": 0.9954797741165524, - "timestamp": 2.1897424503710203 - }, - { - "x": 1.8619803278190585, - "y": 5.486351581956986, - "heading": -0.032764600292238745, - "angularVelocity": 0.43189507613676265, - "velocityX": -0.6463206448473131, - "velocityY": 0.7466069258864354, - "timestamp": 2.2656160018276474 - }, - { - "x": 1.8292876878125428, - "y": 5.524116686024045, - "heading": -0.010918035230093376, - "angularVelocity": 0.287933866844679, - "velocityX": -0.43088321791822976, - "velocityY": 0.4977373978420955, - "timestamp": 2.3414895532842745 + "x": 2.329372405994089, + "y": 6.697374820688125, + "heading": 0.5125504195998649, + "angularVelocity": 0.31710501792120865, + "velocityX": 0.7507016363871187, + "velocityY": 0.4289730049288785, + "timestamp": 5.2269744364870085 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.4600717676583652e-21, - "angularVelocity": 0.1438977749227231, - "velocityX": -0.21544390526203344, - "velocityY": 0.24886908799668503, - "timestamp": 2.4173631047409017 + "x": 2.399251093648798, + "y": 6.73730555904706, + "heading": 0.5293913369356736, + "angularVelocity": 0.21745079806450165, + "velocityX": 0.902277239279105, + "velocityY": 0.5155877648973247, + "timestamp": 5.304421461219836 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 1.1389483058862038e-21, - "angularVelocity": -1.912999393800189e-21, - "velocityX": -4.5659699774939935e-20, - "velocityY": -6.062533120754166e-20, - "timestamp": 2.4932366561975288 + "x": 2.4725481079431484, + "y": 6.779189629834486, + "heading": 0.507422235287016, + "angularVelocity": -0.28366617987293774, + "velocityX": 0.9464148501358945, + "velocityY": 0.5408092945202858, + "timestamp": 5.381868485952664 }, { - "x": 1.8507557561824508, - "y": 5.5430527114919315, - "heading": -3.663099117513338e-19, - "angularVelocity": -3.946860070844576e-18, - "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395093343, - "timestamp": 2.586335690239902 + "x": 2.537702213604298, + "y": 6.816420603018375, + "heading": 0.5016725044650684, + "angularVelocity": -0.07424082257084566, + "velocityX": 0.8412731915467285, + "velocityY": 0.48072825693890747, + "timestamp": 5.459315510685491 }, { - "x": 1.9263848800936068, - "y": 5.543159599317717, - "heading": -1.3979636927804314e-18, - "angularVelocity": -1.108125118245985e-17, - "velocityX": 0.8123513276919049, - "velocityY": 0.0011481088593976478, - "timestamp": 2.6794347242822756 + "x": 2.5904059035391898, + "y": 6.846537042294488, + "heading": 0.5036538467311155, + "angularVelocity": 0.025583194098508056, + "velocityX": 0.680512777970975, + "velocityY": 0.38886502613926416, + "timestamp": 5.536762535418319 }, { - "x": 2.039828562987611, - "y": 5.543319931052194, - "heading": -3.639885752731889e-18, - "angularVelocity": -2.408104534113515e-17, - "velocityX": 1.2185269596070252, - "velocityY": 0.0017221632439681264, - "timestamp": 2.772533758324649 + "x": 2.6280877308244723, + "y": 6.868069547224279, + "heading": 0.508503710225543, + "angularVelocity": 0.06262168896947905, + "velocityX": 0.4865497083575037, + "velocityY": 0.27802882066934376, + "timestamp": 5.614209560151147 }, { - "x": 2.1910867994360017, - "y": 5.543533706687691, - "heading": -8.31799511662456e-18, - "angularVelocity": -5.0248742235094303e-17, - "velocityX": 1.6247025332136762, - "velocityY": 0.002296217546130332, - "timestamp": 2.8656327923670224 + "x": 2.648344516781087, + "y": 6.8796448707953655, + "heading": 0.5125504196001363, + "angularVelocity": 0.052251321320124576, + "velocityX": 0.26155667100350394, + "velocityY": 0.14946117855453953, + "timestamp": 5.691656584883974 }, { - "x": 2.380159562296481, - "y": 5.5438009261858445, - "heading": -4.964025869598509e-18, - "angularVelocity": 3.602582219554166e-17, - "velocityX": 2.0308778152781284, - "velocityY": 0.0028702714362513803, - "timestamp": 2.958731826409396 + "x": 2.6483445167657806, + "y": 6.879644870779064, + "heading": 0.5125504196000689, + "angularVelocity": 1.8669291195039983e-14, + "velocityX": -4.749030905139852e-11, + "velocityY": 6.098431731550748e-11, + "timestamp": 5.769103609616802 }, { - "x": 2.5314177987448714, - "y": 5.544014701821341, - "heading": -1.164993974430083e-17, - "angularVelocity": -7.181507244960338e-17, - "velocityX": 1.6247025332136764, - "velocityY": 0.0022962175461303325, - "timestamp": 3.051830860451769 + "x": 2.6747339306487334, + "y": 6.883481123686636, + "heading": 0.5098224624623802, + "angularVelocity": -0.03296702980330207, + "velocityX": 0.3189128531205414, + "velocityY": 0.046360649216292196, + "timestamp": 5.851851650734406 }, { - "x": 2.6448614816388756, - "y": 5.544175033555818, - "heading": -5.397697309971077e-18, - "angularVelocity": 6.71568990871082e-17, - "velocityX": 1.218526959607025, - "velocityY": 0.0017221632439681266, - "timestamp": 3.1449298944941426 + "x": 2.7264961186821273, + "y": 6.89099146776212, + "heading": 0.5039121473834008, + "angularVelocity": -0.07142543798202369, + "velocityX": 0.6255397388782451, + "velocityY": 0.09076159355636307, + "timestamp": 5.93459969185201 }, { - "x": 2.7204906055500317, - "y": 5.544281921381604, - "heading": -1.7112683408390196e-18, - "angularVelocity": 3.959685518772811e-17, - "velocityX": 0.8123513276919049, - "velocityY": 0.001148108859397648, - "timestamp": 3.238028928536516 + "x": 2.8018050306182753, + "y": 6.9018959167453104, + "heading": 0.4949465844489951, + "angularVelocity": -0.10834773625231511, + "velocityX": 0.9100990297657242, + "velocityY": 0.13177893803804336, + "timestamp": 6.017347732969614 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 2.8361562565882696e-27, - "angularVelocity": 1.838116110697215e-17, - "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395093341, - "timestamp": 3.3311279625788894 + "x": 2.899101313212433, + "y": 6.915952840560763, + "heading": 0.4831136364117761, + "angularVelocity": -0.14299973603485935, + "velocityX": 1.1758137265856894, + "velocityY": 0.16987621248329202, + "timestamp": 6.100095774087218 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 2.9956768552498905e-27, - "angularVelocity": 1.2964344677835282e-27, - "velocityX": -1.3759431197263344e-22, - "velocityY": -2.5705739302596115e-25, - "timestamp": 3.424226996621263 - }, - { - "x": 2.7425045239965504, - "y": 5.564231116573788, - "heading": 0.0035475080989150498, - "angularVelocity": 0.04647392993401548, - "velocityX": -0.20699544832908057, - "velocityY": 0.26064316841975155, - "timestamp": 3.5005602893716685 - }, - { - "x": 2.7110743119163434, - "y": 5.604155225406367, - "heading": 0.01083868300587181, - "angularVelocity": 0.09551762598264181, - "velocityX": -0.4117497221425694, - "velocityY": 0.523023537883034, - "timestamp": 3.576893582122074 - }, - { - "x": 2.6642693453223805, - "y": 5.6643007250236845, - "heading": 0.022164936239812177, - "angularVelocity": 0.14837894221299866, - "velocityX": -0.6131658272230635, - "velocityY": 0.7879327283048184, - "timestamp": 3.65322687487248 - }, - { - "x": 2.60250917282708, - "y": 5.744974159519079, - "heading": 0.038001263541227126, - "angularVelocity": 0.20746291337380784, - "velocityX": -0.8090856593497568, - "velocityY": 1.0568577823464311, - "timestamp": 3.7295601676228856 - }, - { - "x": 2.526611736042362, - "y": 5.846734963341242, - "heading": 0.05925383993510262, - "angularVelocity": 0.27841817938297336, - "velocityX": -0.994290093483679, - "velocityY": 1.333111675856818, - "timestamp": 3.8058934603732912 - }, - { - "x": 2.4388385302471143, - "y": 5.970894906525885, - "heading": 0.08830658815011594, - "angularVelocity": 0.3806038907558967, - "velocityX": -1.149867936160551, - "velocityY": 1.6265503387967306, - "timestamp": 3.882226753123697 - }, - { - "x": 2.357791815962569, - "y": 6.119054185868648, - "heading": 0.1383597950882533, - "angularVelocity": 0.655719216800998, - "velocityX": -1.0617479131884362, - "velocityY": 1.9409522896806517, - "timestamp": 3.9585600458741026 - }, - { - "x": 2.2985967230184703, - "y": 6.254273676601559, - "heading": 0.19308840139911443, - "angularVelocity": 0.7169690228065029, - "velocityX": -0.7754819792414106, - "velocityY": 1.7714353182044804, - "timestamp": 4.034893338624508 - }, - { - "x": 2.2587354140798723, - "y": 6.372988574603886, - "heading": 0.24867936598579424, - "angularVelocity": 0.7282662988016366, - "velocityX": -0.5222008314109583, - "velocityY": 1.5552178312351754, - "timestamp": 4.111226631374914 - }, - { - "x": 2.2373189281980324, - "y": 6.474209987526736, - "heading": 0.30392383910912424, - "angularVelocity": 0.7237271069121585, - "velocityX": -0.2805654663931197, - "velocityY": 1.3260454157771489, - "timestamp": 4.1875599241253205 - }, - { - "x": 2.233901000873839, - "y": 6.557480302062636, - "heading": 0.35823088728392755, - "angularVelocity": 0.7114464242015108, - "velocityX": -0.044776364297153176, - "velocityY": 1.090878062972822, - "timestamp": 4.263893216875727 - }, - { - "x": 2.248214127214941, - "y": 6.6225364653992, - "heading": 0.41124898234582946, - "angularVelocity": 0.6945605665834471, - "velocityX": 0.1875083050315006, - "velocityY": 0.8522646016238898, - "timestamp": 4.340226509626133 - }, - { - "x": 2.280080256208058, - "y": 6.669207835860357, - "heading": 0.4627444117016496, - "angularVelocity": 0.6746129703090314, - "velocityX": 0.41746042709454745, - "velocityY": 0.6114156586138921, - "timestamp": 4.416559802376539 + "x": 3.017081432389269, + "y": 6.932956858771751, + "heading": 0.4685947114251132, + "angularVelocity": -0.1754594403754914, + "velocityX": 1.42577537284728, + "velocityY": 0.20549148936151598, + "timestamp": 6.182843815204822 }, { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "angularVelocity": 0.652480799710893, - "velocityX": 0.6457490306225382, - "velocityY": 0.3689999976939454, - "timestamp": 4.492893095126945 - }, - { - "x": 2.4053669705551046, - "y": 6.740800351083451, - "heading": 0.5568713225103131, - "angularVelocity": 0.5531681306272371, - "velocityX": 0.948486345023332, - "velocityY": 0.5419930073900953, - "timestamp": 4.573015033569164 + "x": 3.1546294358748312, + "y": 6.9527283369160715, + "heading": 0.4515640044783447, + "angularVelocity": -0.20581401948312003, + "velocityX": 1.6622508717768, + "velocityY": 0.23893590563959213, + "timestamp": 6.265591856322426 }, { - "x": 2.502567960679766, - "y": 6.796343857044693, - "heading": 0.5404821418694866, - "angularVelocity": -0.204552972125677, - "velocityX": 1.213163236118634, - "velocityY": 0.6932371712561282, - "timestamp": 4.653136972011383 + "x": 3.3107722570719647, + "y": 6.975106161489216, + "heading": 0.43219034104896137, + "angularVelocity": -0.2341283632545328, + "velocityX": 1.8869669793779915, + "velocityY": 0.2704332848357982, + "timestamp": 6.34833989744003 }, { - "x": 2.575459179910046, - "y": 6.837996044585651, - "heading": 0.5268532537172577, - "angularVelocity": -0.17010182750455535, - "velocityX": 0.9097535662201399, - "velocityY": 0.5198599578440831, - "timestamp": 4.7332589104536025 + "x": 3.484600968601402, + "y": 6.999935624537231, + "heading": 0.4106444392061959, + "angularVelocity": -0.2603795999490034, + "velocityX": 2.1006988102882644, + "velocityY": 0.3000610372485155, + "timestamp": 6.431087938557634 }, { - "x": 2.6240500842159764, - "y": 6.865762317125851, - "heading": 0.5173922106041889, - "angularVelocity": -0.11808305311898519, - "velocityX": 0.6064619160577619, - "velocityY": 0.34655018438206425, - "timestamp": 4.813380848895822 + "x": 3.675234170530874, + "y": 7.027062068947035, + "heading": 0.38710417033209454, + "angularVelocity": -0.28448128265230255, + "velocityX": 2.3037790303523367, + "velocityY": 0.32781977728352046, + "timestamp": 6.513835979675238 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.06043027787801988, - "velocityX": 0.30321823224102623, - "velocityY": 0.17326782030138713, - "timestamp": 4.893502787338041 + "x": 3.8817881153060045, + "y": 7.056325014551516, + "heading": 0.36176128472936936, + "angularVelocity": -0.3062656862983269, + "velocityX": 2.4961792688429623, + "velocityY": 0.3536391340419632, + "timestamp": 6.596584020792842 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": 1.1533672139704627e-20, - "velocityX": 1.3759845110959627e-18, - "velocityY": -2.227992646788969e-18, - "timestamp": 4.97362472578026 - }, - { - "x": 2.6571120181256966, - "y": 6.867792271481583, - "heading": 0.507044677895467, - "angularVelocity": -0.09452799306194601, - "velocityX": 0.15052909368010856, - "velocityY": -0.20349709127294008, - "timestamp": 5.031869289387202 - }, - { - "x": 2.6746521578330125, - "y": 6.844087252656105, - "heading": 0.49617547030513726, - "angularVelocity": -0.18661325482116445, - "velocityX": 0.30114638381847736, - "velocityY": -0.40699109680773127, - "timestamp": 5.090113852994143 - }, - { - "x": 2.7009708210230947, - "y": 6.808530072252695, - "heading": 0.4801087501886783, - "angularVelocity": -0.27584926594838455, - "velocityX": 0.45186471595342564, - "velocityY": -0.6104806732412771, - "timestamp": 5.148358416601085 - }, - { - "x": 2.73607484037394, - "y": 6.7611211343580875, - "heading": 0.4590398225372461, - "angularVelocity": -0.361732088742461, - "velocityX": 0.602700358229852, - "velocityY": -0.8139633119159942, - "timestamp": 5.206602980208027 - }, - { - "x": 2.7799722580023465, - "y": 6.701861088549211, - "heading": 0.4332010241003249, - "angularVelocity": -0.4436259255248601, - "velocityX": 0.7536740754835817, - "velocityY": -1.0174347980145875, - "timestamp": 5.264847543814969 - }, - { - "x": 2.832672676333526, - "y": 6.630750980608444, - "heading": 0.40287238280523807, - "angularVelocity": -0.5207119672104833, - "velocityX": 0.9048126566253185, - "velocityY": -1.2208883290919084, - "timestamp": 5.32309210742191 - }, - { - "x": 2.8941877403139116, - "y": 6.547792490679094, - "heading": 0.3683969681281308, - "angularVelocity": -0.5919078544353333, - "velocityX": 1.0561511696699046, - "velocityY": -1.4243130138151228, - "timestamp": 5.381336671028852 - }, - { - "x": 2.9645318213846927, - "y": 6.452988329970027, - "heading": 0.33020407419243936, - "angularVelocity": -0.6557331975810253, - "velocityX": 1.2077364257631285, - "velocityY": -1.627691149835798, - "timestamp": 5.439581234635794 - }, - { - "x": 3.04372302387486, - "y": 6.346342948824998, - "heading": 0.28884632079593436, - "angularVelocity": -0.7100706200771626, - "velocityX": 1.3596325147971153, - "velocityY": -1.8309928779742386, - "timestamp": 5.497825798242736 - }, - { - "x": 3.1317847243622103, - "y": 6.227863919029491, - "heading": 0.24506332676078993, - "angularVelocity": -0.7517095386036364, - "velocityX": 1.5119299559290544, - "velocityY": -2.0341646062463727, - "timestamp": 5.556070361849677 - }, - { - "x": 3.2287480012560774, - "y": 6.097564965294096, - "heading": 0.199900944291634, - "angularVelocity": -0.7753922370151164, - "velocityX": 1.6647609817838902, - "velocityY": -2.237100695177408, - "timestamp": 5.614314925456619 - }, - { - "x": 3.334655409720646, - "y": 5.955473736284568, - "heading": 0.1549620363959118, - "angularVelocity": -0.7715554055652005, - "velocityX": 1.8183226365858802, - "velocityY": -2.4395620845993164, - "timestamp": 5.672559489063561 - }, - { - "x": 3.4495650121656865, - "y": 5.8016567423537415, - "heading": 0.11303057394210353, - "angularVelocity": -0.7199206218932102, - "velocityX": 1.972881163991502, - "velocityY": -2.6408815588154475, - "timestamp": 5.730804052670503 - }, - { - "x": 3.5735262627096924, - "y": 5.636333431818966, - "heading": 0.08010242298971296, - "angularVelocity": -0.565342907787981, - "velocityX": 2.1282887683826965, - "velocityY": -2.838433328309314, - "timestamp": 5.789048616277444 - }, - { - "x": 3.7050107789119795, - "y": 5.46057947422046, - "heading": 0.07776405618458973, - "angularVelocity": -0.04014738303996017, - "velocityX": 2.2574555985962665, - "velocityY": -3.017516944320591, - "timestamp": 5.847293179884386 - }, - { - "x": 3.8485069507845076, - "y": 5.2934525910716275, - "heading": 0.07776403563237232, - "angularVelocity": -3.5286069899218235e-7, - "velocityX": 2.463683526601701, - "velocityY": -2.8693988382619477, - "timestamp": 5.905537743491328 - }, - { - "x": 4.002902013863093, - "y": 5.136338903873299, - "heading": 0.07776401516857159, - "angularVelocity": -3.5134267388587325e-7, - "velocityX": 2.650806418956214, - "velocityY": -2.697482433873059, - "timestamp": 5.96378230709827 + "x": 4.1033458782194, + "y": 7.087551279488626, + "heading": 0.3348309137177563, + "angularVelocity": -0.32545025414364576, + "velocityX": 2.677498583906162, + "velocityY": 0.3773656090873216, + "timestamp": 6.679332061910446 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.07776399412255879, - "angularVelocity": -3.6133866407320583e-7, - "velocityX": 2.8259641989395843, - "velocityY": -2.513386639709188, - "timestamp": 6.022026870705211 - }, - { - "x": 4.268976588700125, - "y": 4.906724140265527, - "heading": 0.07776397443770015, - "angularVelocity": -5.672615420255969e-7, - "velocityX": 2.9242931040000055, - "velocityY": -2.398268643648094, - "timestamp": 6.056728427523216 - }, - { - "x": 4.37370366170879, - "y": 4.827628597038176, - "heading": 0.07776395550809555, - "angularVelocity": -5.454972726636976e-7, - "velocityX": 3.0179358683506776, - "velocityY": -2.2793082063198216, - "timestamp": 6.09142998434122 - }, - { - "x": 4.4815124411311835, - "y": 4.752787886123784, - "heading": 0.0777639371726549, - "angularVelocity": -5.283751607380448e-7, - "velocityX": 3.106741867167675, - "velocityY": -2.1566960614159445, - "timestamp": 6.126131541159224 - }, - { - "x": 4.592230128734662, - "y": 4.682321875197502, - "heading": 0.07776391928788347, - "angularVelocity": -5.153881575305611e-7, - "velocityX": 3.1905683132358433, - "velocityY": -2.030629671626745, - "timestamp": 6.160833097977228 - }, - { - "x": 4.705679213251417, - "y": 4.616343315466854, - "heading": 0.07776390172302422, - "angularVelocity": -5.061691995708879e-7, - "velocityX": 3.269279390309479, - "velocityY": -1.9013141132738893, - "timestamp": 6.195534654795233 - }, - { - "x": 4.821677528011508, - "y": 4.554957192131842, - "heading": 0.07776388435588834, - "angularVelocity": -5.004713755679868e-7, - "velocityX": 3.3427409429626933, - "velocityY": -1.7689731805681452, - "timestamp": 6.230236211613237 - }, - { - "x": 4.939562213059443, - "y": 4.4972764656411774, - "heading": 0.07776386706661859, - "angularVelocity": -4.982274959213974e-7, - "velocityX": 3.397100760239454, - "velocityY": -1.6621942004843482, - "timestamp": 6.264937768431241 - }, - { - "x": 5.0574486320523055, - "y": 4.439599283026335, - "heading": 0.07776384977740043, - "angularVelocity": -4.982260088149746e-7, - "velocityX": 3.397150727592071, - "velocityY": -1.6620920760798994, - "timestamp": 6.299639325249245 - }, - { - "x": 5.175337486723699, - "y": 4.381927078962276, - "heading": 0.07776383248820665, - "angularVelocity": -4.9822530674329e-7, - "velocityX": 3.3972209169079175, - "velocityY": -1.6619486084306254, - "timestamp": 6.3343408820672495 - }, - { - "x": 5.295110440251067, - "y": 4.328277244219036, - "heading": 0.07776381519347733, - "angularVelocity": -4.983848248451832e-7, - "velocityX": 3.451515277989629, - "velocityY": -1.5460353846547075, - "timestamp": 6.369042438885254 - }, - { - "x": 5.416934261342807, - "y": 4.279463089154562, - "heading": 0.07776379778914431, - "angularVelocity": -5.015432909319555e-7, - "velocityX": 3.5106154381100505, - "velocityY": -1.4066848735485782, - "timestamp": 6.403743995703258 - }, - { - "x": 5.540614064021598, - "y": 4.235563528216825, - "heading": 0.07776378015516597, - "angularVelocity": -5.081610154974924e-7, - "velocityX": 3.5640995396097326, - "velocityY": -1.26506027288553, - "timestamp": 6.438445552521262 + "x": 4.338922324182521, + "y": 7.120545790816314, + "heading": 0.30656596895157695, + "angularVelocity": -0.34157841544560996, + "velocityX": 2.846912661392294, + "velocityY": 0.39873465138360303, + "timestamp": 6.76208010302805 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.07776376214569573, - "angularVelocity": -5.189816214982998e-7, - "velocityX": 3.6118744025389584, - "velocityY": -1.1214036841768227, - "timestamp": 6.4731471093392665 - }, - { - "x": 5.801257373802826, - "y": 4.160893829303833, - "heading": 0.07776374465549807, - "angularVelocity": -4.7264774236323325e-7, - "velocityX": 3.6564428162530316, - "velocityY": -0.9662347026048084, - "timestamp": 6.510151829987593 - }, - { - "x": 5.937965681779902, - "y": 4.1309456443700405, - "heading": 0.07776372758116286, - "angularVelocity": -4.614096500770092e-7, - "velocityX": 3.6943477908205034, - "velocityY": -0.8093071480907553, - "timestamp": 6.547156550635919 - }, - { - "x": 6.075827506190729, - "y": 4.1068588699437765, - "heading": 0.07776371078252527, - "angularVelocity": -4.53959313742904e-7, - "velocityX": 3.7255199335509808, - "velocityY": -0.6509108568923265, - "timestamp": 6.584161271284246 - }, - { - "x": 6.2145915217996235, - "y": 4.088676368882122, - "heading": 0.0777636941280861, - "angularVelocity": -4.5006255582880887e-7, - "velocityX": 3.7499003688646537, - "velocityY": -0.4913562578799624, - "timestamp": 6.621165991932572 - }, - { - "x": 6.354004262869319, - "y": 4.076423083568819, - "heading": 0.07776367749075697, - "angularVelocity": -4.496001822495745e-7, - "velocityX": 3.7674312527474547, - "velocityY": -0.33112762638453114, - "timestamp": 6.658170712580898 - }, - { - "x": 6.49381285233259, - "y": 4.070129387117659, - "heading": 0.07776362451928018, - "angularVelocity": -0.0000014314788993599032, - "velocityX": 3.778128493170848, - "velocityY": -0.17007820464237144, - "timestamp": 6.695175433229225 - }, - { - "x": 6.629142082966274, - "y": 4.065515785319137, - "heading": 0.06238171659626575, - "angularVelocity": -0.4156742073314349, - "velocityX": 3.657080184979125, - "velocityY": -0.12467603369760688, - "timestamp": 6.732180153877551 - }, - { - "x": 6.758586834582697, - "y": 4.061933354582955, - "heading": 0.046880370354106624, - "angularVelocity": -0.4189018582108937, - "velocityX": 3.4980605000804377, - "velocityY": -0.0968101008038334, - "timestamp": 6.769184874525878 - }, - { - "x": 6.882125546140559, - "y": 4.059204781585899, - "heading": 0.032519289448839375, - "angularVelocity": -0.3880878075461594, - "velocityX": 3.338458158674076, - "velocityY": -0.07373580854685112, - "timestamp": 6.806189595174204 - }, - { - "x": 6.999755813322696, - "y": 4.05726367776388, - "heading": 0.01976964076410941, - "angularVelocity": -0.34454114127480795, - "velocityX": 3.1787908440123913, - "velocityY": -0.0524555729109991, - "timestamp": 6.84319431582253 - }, - { - "x": 7.111477612575904, - "y": 4.056075301962013, - "heading": 0.008878948528675913, - "angularVelocity": -0.2943054843984055, - "velocityX": 3.0191228928587734, - "velocityY": -0.03211416762637815, - "timestamp": 6.880199036470857 + "x": 4.587417768628414, + "y": 7.155077045432509, + "heading": 0.27728164973070885, + "angularVelocity": -0.3538974315929522, + "velocityX": 3.00303718480442, + "velocityY": 0.4173060068834074, + "timestamp": 6.844828144145654 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -2.4974876469169754e-20, - "angularVelocity": -0.23994096896600625, - "velocityX": 2.8594660546893573, - "velocityY": -0.012350203465742983, - "timestamp": 6.917203757119183 - }, - { - "x": 7.386543487491071, - "y": 4.0570668323942645, - "heading": -0.009095721053457144, - "angularVelocity": -0.13843657850499821, - "velocityX": 2.57601194788322, - "velocityY": 0.022046826970953105, - "timestamp": 6.982906919908118 - }, - { - "x": 7.537031363324535, - "y": 4.05969750834718, - "heading": -0.013766029362825559, - "angularVelocity": -0.07108194052044785, - "velocityX": 2.2904205740732744, - "velocityY": 0.04003880241453354, - "timestamp": 7.048610082697054 - }, - { - "x": 7.6687074557367385, - "y": 4.062834301345331, - "heading": -0.015395893103822788, - "angularVelocity": -0.02480647311048019, - "velocityX": 2.0041058424417098, - "velocityY": 0.04774188737652573, - "timestamp": 7.114313245485989 - }, - { - "x": 7.781556988474885, - "y": 4.066015749956383, - "heading": -0.01492426229398527, - "angularVelocity": 0.007178205581253095, - "velocityX": 1.7175662167232906, - "velocityY": 0.04842154435201139, - "timestamp": 7.180016408274924 - }, - { - "x": 7.8755781593330285, - "y": 4.068907471663171, - "heading": -0.013029646010250598, - "angularVelocity": 0.028835998197239424, - "velocityX": 1.4309991614890225, - "velocityY": 0.044011910295353764, - "timestamp": 7.2457195710638596 - }, - { - "x": 7.950774573438036, - "y": 4.07125628842811, - "heading": -0.010225311537090487, - "angularVelocity": 0.042681879442679815, - "velocityX": 1.144486976168374, - "velocityY": 0.035748914743771835, - "timestamp": 7.311422733852795 - }, - { - "x": 8.007152043467745, - "y": 4.072863968231118, - "heading": -0.006913103442788773, - "angularVelocity": 0.0504116994328239, - "velocityX": 0.8580632596152882, - "velocityY": 0.024468834296048715, - "timestamp": 7.37712589664173 - }, - { - "x": 8.044717148266278, - "y": 4.073571148367015, - "heading": -0.0034161675760999402, - "angularVelocity": 0.053223250118451255, - "velocityX": 0.5717396728557544, - "velocityY": 0.010763258660280774, - "timestamp": 7.442829059430665 + "x": 4.847543704393879, + "y": 7.190849002148663, + "heading": 0.2474070078170501, + "angularVelocity": -0.36103140944690143, + "velocityX": 3.1435902560613274, + "velocityY": 0.4322997406709641, + "timestamp": 6.927576185263258 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 5.107015447929928e-20, - "angularVelocity": 0.05199395936347874, - "velocityX": 0.28551767430107416, - "velocityY": -0.004934199232898976, - "timestamp": 7.508532222219601 - }, - { - "x": 8.06095184830656, - "y": 4.071458641245482, - "heading": 0.0034408492569327057, - "angularVelocity": 0.04684199855212652, - "velocityX": -0.034370194612683154, - "velocityY": -0.02434521970346025, - "timestamp": 7.581988722012275 - }, - { - "x": 8.034920990250148, - "y": 4.068388525754522, - "heading": 0.006505362431754428, - "angularVelocity": 0.041718747605332625, - "velocityX": -0.3543710649143617, - "velocityY": -0.041795014731511364, - "timestamp": 7.655445221804949 - }, - { - "x": 7.985375296011398, - "y": 4.064207377475085, - "heading": 0.009196043333328236, - "angularVelocity": 0.036629582258453454, - "velocityX": -0.6744902680986578, - "velocityY": -0.05692005869104256, - "timestamp": 7.728901721597623 - }, - { - "x": 7.912305948559603, - "y": 4.059120814150924, - "heading": 0.011515917649017375, - "angularVelocity": 0.03158160710402538, - "velocityX": -0.9947295019232808, - "velocityY": -0.06924592566372595, - "timestamp": 7.802358221390297 - }, - { - "x": 7.815704588785296, - "y": 4.0533811177027905, - "heading": 0.013468719297185195, - "angularVelocity": 0.026584463644190544, - "velocityX": -1.3150825324778364, - "velocityY": -0.0781373529140697, - "timestamp": 7.875814721182971 - }, - { - "x": 7.695564554163692, - "y": 4.047305026722375, - "heading": 0.015059175300172275, - "angularVelocity": 0.021651671499132708, - "velocityX": -1.6355262633081036, - "velocityY": -0.08271685960487347, - "timestamp": 7.949271220975645 - }, - { - "x": 7.551883548681463, - "y": 4.04130179428642, - "heading": 0.016293463732843762, - "angularVelocity": 0.016802984571211377, - "velocityX": -1.9560012509139277, - "velocityY": -0.08172499986929728, - "timestamp": 8.02272772076832 - }, - { - "x": 7.384669657766205, - "y": 4.035920047959674, - "heading": 0.017179998132389497, - "angularVelocity": 0.012068835324959903, - "velocityX": -2.276366167557798, - "velocityY": -0.07326439922859854, - "timestamp": 8.096184220560993 - }, - { - "x": 7.19395595849013, - "y": 4.0319320028167684, - "heading": 0.017730886759136543, - "angularVelocity": 0.007499521870792628, - "velocityX": -2.5962807895060482, - "velocityY": -0.054291249299417726, - "timestamp": 8.169640720353666 - }, - { - "x": 6.979840325568109, - "y": 4.030500043249597, - "heading": 0.017964968086133067, - "angularVelocity": 0.00318666595409774, - "velocityX": -2.9148629941032773, - "velocityY": -0.01949398039945934, - "timestamp": 8.243097220146339 - }, - { - "x": 6.742614358141766, - "y": 4.033551118259164, - "heading": 0.017915208666011314, - "angularVelocity": -0.0006773998252331979, - "velocityX": -3.22947551402392, - "velocityY": 0.041535807153613755, - "timestamp": 8.316553719939012 - }, - { - "x": 6.483307205091183, - "y": 4.044763583343556, - "heading": 0.017651500017929662, - "angularVelocity": -0.003589997465520818, - "velocityX": -3.530077716505134, - "velocityY": 0.1526408842789682, - "timestamp": 8.390010219731685 - }, - { - "x": 6.206941447635851, - "y": 4.072199279636492, - "heading": 0.017375156748179146, - "angularVelocity": -0.0037619988773032, - "velocityX": -3.762305013652376, - "velocityY": 0.37349582910118817, - "timestamp": 8.463466719524359 - }, - { - "x": 5.9338077856619895, - "y": 4.122953167788701, - "heading": 0.017375155210986027, - "angularVelocity": -2.0926577278102936e-8, - "velocityX": -3.7183048844521998, - "velocityY": 0.690938014953861, - "timestamp": 8.536923219317032 + "x": 5.117670126803491, + "y": 7.227429623630717, + "heading": 0.21762399464747098, + "angularVelocity": -0.35992408723308145, + "velocityX": 3.264444919314729, + "velocityY": 0.4420723558891399, + "timestamp": 7.010324226380862 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.017375154199310676, - "angularVelocity": -1.377244159549356e-8, - "velocityX": -3.646458211284153, - "velocityY": 1.003259166632557, - "timestamp": 8.610379719109705 - }, - { - "x": 5.541092339335072, - "y": 4.2364336029082, - "heading": 0.01737515315796098, - "angularVelocity": -3.005342133477009e-8, - "velocityX": -3.6034502673557895, - "velocityY": 1.148184128750311, - "timestamp": 8.645029674075602 - }, - { - "x": 5.417922638814364, - "y": 4.281176215218788, - "heading": 0.017375152214225534, - "angularVelocity": -2.7236267527590686e-8, - "velocityX": -3.5546857316822638, - "velocityY": 1.2912747608077488, - "timestamp": 8.679679629041498 - }, - { - "x": 5.296639420951804, - "y": 4.33080542124118, - "heading": 0.017375151348717865, - "angularVelocity": -2.4978608726563434e-8, - "velocityX": -3.500241716964063, - "velocityY": 1.4323021796489428, - "timestamp": 8.714329584007395 - }, - { - "x": 5.177436464453021, - "y": 4.385241922289634, - "heading": 0.017375150546348885, - "angularVelocity": -2.3156422113484916e-8, - "velocityX": -3.4402052359405113, - "velocityY": 1.571041033156597, - "timestamp": 8.748979538973291 - }, - { - "x": 5.0605042229676425, - "y": 4.444398737399711, - "heading": 0.017375149795171643, - "angularVelocity": -2.167902498222188e-8, - "velocityX": -3.374672249948487, - "velocityY": 1.7072696102577922, - "timestamp": 8.783629493939188 - }, - { - "x": 4.946029519855076, - "y": 4.5081813412028975, - "heading": 0.017375149085535665, - "angularVelocity": -2.0480141439242773e-8, - "velocityX": -3.3037475294046756, - "velocityY": 1.8407701789500717, - "timestamp": 8.818279448905084 - }, - { - "x": 4.834195247479412, - "y": 4.576487812332227, - "heading": 0.017375148409491465, - "angularVelocity": -1.9510680466596443e-8, - "velocityX": -3.2275445230947857, - "velocityY": 1.9713292902272803, - "timestamp": 8.85292940387098 - }, - { - "x": 4.725180068322848, - "y": 4.649208988191292, - "heading": 0.01737514776036044, - "angularVelocity": -1.8733964504925207e-8, - "velocityX": -3.146185305691103, - "velocityY": 2.0987379617271817, - "timestamp": 8.887579358836877 - }, - { - "x": 4.619158095512492, - "y": 4.726228597192443, - "heading": 0.017375147132049722, - "angularVelocity": -1.813308899206411e-8, - "velocityX": -3.059801171883295, - "velocityY": 2.222791027490615, - "timestamp": 8.922229313802774 - }, - { - "x": 4.51014571232548, - "y": 4.798953964824882, - "heading": 0.01737514649311973, - "angularVelocity": -1.8439562026762692e-8, - "velocityX": -3.146104613824239, - "velocityY": 2.0988589365849712, - "timestamp": 8.95687926876867 - }, - { - "x": 4.398314099643168, - "y": 4.867264790943429, - "heading": 0.017375145827920196, - "angularVelocity": -1.9197702730409006e-8, - "velocityX": -3.2274677641681775, - "velocityY": 1.9714549755050896, - "timestamp": 8.991529223734567 - }, - { - "x": 4.2838418935996625, - "y": 4.931051865994745, - "heading": 0.017375140544826046, - "angularVelocity": -1.5247044777404565e-7, - "velocityX": -3.3036754638258423, - "velocityY": 1.840899219467885, - "timestamp": 9.026179178700463 + "x": 5.395335267694769, + "y": 7.263944271068559, + "heading": 0.1895241870207491, + "angularVelocity": -0.33958275322536735, + "velocityX": 3.3555494141142153, + "velocityY": 0.44127506759904395, + "timestamp": 7.093072267498466 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.01686674274932993, - "angularVelocity": -0.014672394119890892, - "velocityX": -3.3576617434279026, - "velocityY": 1.6997404450060698, - "timestamp": 9.06082913366636 - }, - { - "x": 3.941206209015057, - "y": 5.0867984564258695, - "heading": 0.014981559594595455, - "angularVelocity": -0.027398080889315734, - "velocityX": -3.288799800865747, - "velocityY": 1.4075673366199248, - "timestamp": 9.129636257871532 - }, - { - "x": 3.7303087168982803, - "y": 5.169878519937419, - "heading": 0.012882808496808885, - "angularVelocity": -0.03050194470456946, - "velocityX": -3.0650531402520937, - "velocityY": 1.2074340334849432, - "timestamp": 9.198443382076704 - }, - { - "x": 3.5370906486542313, - "y": 5.2422774011944036, - "heading": 0.010786769401609648, - "angularVelocity": -0.030462530143669606, - "velocityX": -2.808111376198552, - "velocityY": 1.0522003657804835, - "timestamp": 9.267250506281876 - }, - { - "x": 3.362377869511596, - "y": 5.305500032913324, - "heading": 0.008787829989227822, - "angularVelocity": -0.029051343672224058, - "velocityX": -2.5391669999413216, - "velocityY": 0.9188384553087892, - "timestamp": 9.336057630487048 - }, - { - "x": 3.2065764087500264, - "y": 5.360415401059238, - "heading": 0.0069378788339289135, - "angularVelocity": -0.026886040895746154, - "velocityX": -2.2643216463602664, - "velocityY": 0.7981058470364881, - "timestamp": 9.40486475469222 - }, - { - "x": 3.0699232813800306, - "y": 5.407585492361195, - "heading": 0.005269555505228705, - "angularVelocity": -0.024246374891727204, - "velocityX": -1.9860316638509532, - "velocityY": 0.6855408047763067, - "timestamp": 9.473671878897392 - }, - { - "x": 2.9525724844135883, - "y": 5.4474025208162065, - "heading": 0.00380523783536115, - "angularVelocity": -0.021281483375198414, - "velocityX": -1.70550358443293, - "velocityY": 0.5786759571041632, - "timestamp": 9.542479003102564 - }, - { - "x": 2.854631606493464, - "y": 5.480155404840544, - "heading": 0.0025612082007498414, - "angularVelocity": -0.018079953914391628, - "velocityX": -1.423411878515389, - "velocityY": 0.47601007021705255, - "timestamp": 9.611286127307736 - }, - { - "x": 2.776179840063629, - "y": 5.506065679558788, - "heading": 0.0015498402303536507, - "angularVelocity": -0.014698593816833106, - "velocityX": -1.1401692388117297, - "velocityY": 0.376563837212327, - "timestamp": 9.680093251512908 - }, - { - "x": 2.717277807675311, - "y": 5.525308541082813, - "heading": 0.000780854270914727, - "angularVelocity": -0.01117596423803359, - "velocityX": -0.8560455486074661, - "velocityY": 0.2796637956651817, - "timestamp": 9.74890037571808 - }, - { - "x": 2.677973353536199, - "y": 5.538025975345556, - "heading": 0.00026208876345230333, - "angularVelocity": -0.007539415626724292, - "velocityX": -0.5712265204096013, - "velocityY": 0.1848272894652728, - "timestamp": 9.817707499923252 + "x": 5.667910261992729, + "y": 7.2967916361655485, + "heading": 0.1669091076503975, + "angularVelocity": -0.27330048016738406, + "velocityX": 3.294035612402776, + "velocityY": 0.3969564071048189, + "timestamp": 7.17582030861607 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -9.768710288964573e-20, - "angularVelocity": -0.003809035277666901, - "velocityX": -0.2858451884385741, - "velocityY": 0.09169675411867828, - "timestamp": 9.886514624128424 + "x": 5.931901746285159, + "y": 7.327414779180602, + "heading": 0.14778373205342693, + "angularVelocity": -0.23112783503586568, + "velocityX": 3.1903049392702143, + "velocityY": 0.3700769541063461, + "timestamp": 7.258568349733674 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -4.776521402733554e-20, - "angularVelocity": 3.1435024538818593e-20, - "velocityX": 5.229513890071112e-19, - "velocityY": 2.1429873638516558e-18, - "timestamp": 9.955321748333596 + "x": 6.1801220482371395, + "y": 7.354810947360831, + "heading": 0.11632647881372146, + "angularVelocity": -0.38015707459464126, + "velocityX": 2.999712121271843, + "velocityY": 0.33107935620263895, + "timestamp": 7.341316390851278 }, { - "x": 2.6751665057839835, - "y": 5.538944468629318, - "heading": -4.314568505907974e-18, - "angularVelocity": -6.69844600274702e-17, - "velocityX": 0.2647058041795454, - "velocityY": -0.08463157955674895, - "timestamp": 10.01902015375538 + "x": 6.415697256681246, + "y": 7.379780463537999, + "heading": 0.08899977607154176, + "angularVelocity": -0.3302398748429845, + "velocityX": 2.84689770612573, + "velocityY": 0.3017535622589267, + "timestamp": 7.424064431968882 }, { - "x": 2.7088624712986666, - "y": 5.528079837461981, - "heading": -9.548636187746045e-18, - "angularVelocity": -8.216952445023882e-17, - "velocityX": 0.5289922925316966, - "velocityY": -0.1705636286402427, - "timestamp": 10.082718559177163 + "x": 6.637987945996846, + "y": 7.402299511961184, + "heading": 0.06474721752358573, + "angularVelocity": -0.2930892166194916, + "velocityX": 2.686355910222349, + "velocityY": 0.2721399578655483, + "timestamp": 7.5068124730864865 }, { - "x": 2.7593613029745305, - "y": 5.511644735540107, - "heading": -1.3305788264629273e-17, - "angularVelocity": -5.898345573823082e-17, - "velocityX": 0.7927801542516099, - "velocityY": -0.25801433824046893, - "timestamp": 10.146416964598947 + "x": 6.845920803268571, + "y": 7.422253782487123, + "heading": 0.04362794173118966, + "angularVelocity": -0.2552238760840361, + "velocityX": 2.5128432584428544, + "velocityY": 0.24114492931132497, + "timestamp": 7.5895605142040905 }, { - "x": 2.8266246280660523, - "y": 5.489524723354373, - "heading": -1.7929345665528888e-17, - "angularVelocity": -7.258513569049044e-17, - "velocityX": 1.0559656030026554, - "velocityY": -0.34726163142175154, - "timestamp": 10.21011537002073 + "x": 7.038548371158306, + "y": 7.439534256583214, + "heading": 0.025735796286898687, + "angularVelocity": -0.2162243988212622, + "velocityX": 2.3278807001118484, + "velocityY": 0.2088324250667052, + "timestamp": 7.6723085553216945 }, { - "x": 2.9106051995536752, - "y": 5.461582331644453, - "heading": -2.2921460737278194e-17, - "angularVelocity": -7.837111523596277e-17, - "velocityX": 1.31840932173323, - "velocityY": -0.43866705178720505, - "timestamp": 10.273813775442514 + "x": 7.2149794023417675, + "y": 7.454031322663086, + "heading": 0.011159229205506762, + "angularVelocity": -0.17615603807074215, + "velocityX": 2.1321475264013814, + "velocityY": 0.1751952781490545, + "timestamp": 7.755056596439299 }, { - "x": 3.0112434875257597, - "y": 5.427649372766817, - "heading": -2.8632725416108676e-17, - "angularVelocity": -8.966103061640699e-17, - "velocityX": 1.5799184815647955, - "velocityY": -0.5327128466237959, - "timestamp": 10.337512180864298 + "x": 7.374384880065902, + "y": 7.465639114379918, + "heading": -1.1165566205788637e-16, + "angularVelocity": -0.134857925997875, + "velocityX": 1.9263957861864134, + "velocityY": 0.14027874932231896, + "timestamp": 7.837804637556903 }, { - "x": 3.1284623112048178, - "y": 5.387515427846554, - "heading": -3.496086628632705e-17, - "angularVelocity": -9.93453576777077e-17, - "velocityX": 1.8402159819054262, - "velocityY": -0.6300620031932188, - "timestamp": 10.401210586286082 + "x": 7.500924464216497, + "y": 7.473646968954779, + "heading": -0.0070937662662868915, + "angularVelocity": -0.09816609550664941, + "velocityX": 1.7511003938959169, + "velocityY": 0.11081557912801578, + "timestamp": 7.9100675327008405 }, { - "x": 3.2621579049769083, - "y": 5.340909820689256, - "heading": -4.1773207656590006e-17, - "angularVelocity": -1.0694681169901965e-16, - "velocityX": 2.0988844679362666, - "velocityY": -0.7316604999559271, - "timestamp": 10.464908991707865 + "x": 7.613899532464792, + "y": 7.47952275118133, + "heading": -0.01171150501248939, + "angularVelocity": -0.06390193386252535, + "velocityX": 1.5633897316635108, + "velocityY": 0.08131119317636289, + "timestamp": 7.982330427844778 }, { - "x": 3.4121839694430376, - "y": 5.287471786804592, - "heading": -4.8890897323491556e-17, - "angularVelocity": -1.1174046853574415e-16, - "velocityX": 2.3552562026116513, - "velocityY": -0.8389226312781303, - "timestamp": 10.52860739712965 + "x": 7.712046212988902, + "y": 7.483209835153203, + "heading": -0.014057088311073107, + "angularVelocity": -0.032459027470621904, + "velocityX": 1.3581891554251948, + "velocityY": 0.05102319751389575, + "timestamp": 8.054593322988717 }, { - "x": 3.5783204640757695, - "y": 5.226697519809261, - "heading": -5.788296277144039e-17, - "angularVelocity": -1.4116625664827146e-16, - "velocityX": 2.608173525422577, - "velocityY": -0.954094008992977, - "timestamp": 10.592305802551433 + "x": 7.79365194754084, + "y": 7.484595849351514, + "heading": -0.014345050898318633, + "angularVelocity": -0.003984930117620908, + "velocityX": 1.129289580626482, + "velocityY": 0.019180164253482204, + "timestamp": 8.126856218132655 }, { - "x": 3.76020443223388, - "y": 5.157837260218167, - "heading": -6.653949543666233e-17, - "angularVelocity": -1.3589873416470021e-16, - "velocityX": 2.85539279914075, - "velocityY": -1.0810358459545435, - "timestamp": 10.656004207973217 + "x": 7.856538953365895, + "y": 7.483519387813377, + "heading": -0.012796446625875068, + "angularVelocity": 0.02143014432730538, + "velocityX": 0.8702530627897808, + "velocityY": -0.0148964629220964, + "timestamp": 8.199119113276593 }, { - "x": 3.9571467965572817, - "y": 5.079670509622146, - "heading": -7.420373602789392e-17, - "angularVelocity": -1.2032076062800333e-16, - "velocityX": 3.0917942610859868, - "velocityY": -1.2271382631705205, - "timestamp": 10.719702613395 + "x": 7.899249386445817, + "y": 7.479924646329156, + "heading": -0.009762276595150929, + "angularVelocity": 0.04198793896480952, + "velocityX": 0.5910423737502203, + "velocityY": -0.04974532887286408, + "timestamp": 8.271382008420531 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -8.561193314262968e-17, - "angularVelocity": -1.7909705963758273e-16, - "velocityX": 3.3023160854502294, - "velocityY": -1.4085550989874924, - "timestamp": 10.783401018816784 + "x": 7.921731862807926, + "y": 7.4739282664160145, + "heading": -0.005439516570392937, + "angularVelocity": 0.0598199119499377, + "velocityX": 0.31112061476836195, + "velocityY": -0.08298006744976984, + "timestamp": 8.343644903564469 }, { - "x": 4.2842075611264425, - "y": 4.936417577245955, - "heading": -9.453694204366925e-17, - "angularVelocity": -2.5755488045645486e-16, - "velocityX": 3.3679341946759767, - "velocityY": -1.5447568972758807, - "timestamp": 10.81805386206763 + "x": 7.923961639404313, + "y": 7.465639114379864, + "heading": -8.923477228591696e-17, + "angularVelocity": 0.07527399171536123, + "velocityX": 0.030856452567537763, + "velocityY": -0.11470827482980998, + "timestamp": 8.415907798708407 }, { - "x": 4.401356584826297, - "y": 4.8776669667131465, - "heading": -1.0560165699964911e-16, - "angularVelocity": -3.1930179223080245e-16, - "velocityX": 3.380646801528862, - "velocityY": -1.6954051968412056, - "timestamp": 10.852706705318477 + "x": 7.888861809774048, + "y": 7.450123409205229, + "heading": 0.009559512590155543, + "angularVelocity": 0.09629823646465466, + "velocityX": -0.3535799196587898, + "velocityY": -0.15629824551531185, + "timestamp": 8.515177660142104 }, { - "x": 4.516063976001339, - "y": 4.814281195629902, - "heading": -9.662380875053523e-17, - "angularVelocity": 2.590797004510181e-16, - "velocityX": 3.3101869980680285, - "velocityY": -1.8291650882556678, - "timestamp": 10.887359548569323 + "x": 7.817152523236329, + "y": 7.430666435185926, + "heading": 0.02163779335227315, + "angularVelocity": 0.12167117579976462, + "velocityX": -0.72236714650414, + "velocityY": -0.19600081775371833, + "timestamp": 8.614447521575801 }, { - "x": 4.628146410125643, - "y": 4.746361612927903, - "heading": -9.053238794887956e-17, - "angularVelocity": 1.7578415593930746e-16, - "velocityX": 3.234436877602137, - "velocityY": -1.9600002865663488, - "timestamp": 10.92201239182017 - }, - { - "x": 4.737424855337869, - "y": 4.674016859733605, - "heading": -7.951670404623386e-17, - "angularVelocity": 3.1788687072504797e-16, - "velocityX": 3.1535203163901047, - "velocityY": -2.087700356089278, - "timestamp": 10.956665235071016 + "x": 7.711448594725456, + "y": 7.4075722780062225, + "heading": 0.036035719670363044, + "angularVelocity": 0.14503824333134957, + "velocityX": -1.064813902067085, + "velocityY": -0.23264016737978235, + "timestamp": 8.713717383009499 }, { - "x": 4.844483096398324, - "y": 4.598425359059355, - "heading": -7.540763427184281e-17, - "angularVelocity": 1.1857814219565296e-16, - "velocityX": 3.089450417833756, - "velocityY": -2.181393893916722, - "timestamp": 10.991318078321862 + "x": 7.574048351882708, + "y": 7.3811128444264495, + "heading": 0.05252610728763101, + "angularVelocity": 0.16611675869298972, + "velocityX": -1.3841083371967766, + "velocityY": -0.2665404504210516, + "timestamp": 8.812987244443196 }, { - "x": 4.954476829355994, - "y": 4.5271728465414585, - "heading": -1.0264313607639127e-16, - "angularVelocity": -7.859528757078724e-16, - "velocityX": 3.174161847599147, - "velocityY": -2.0561808450207573, - "timestamp": 11.025970921572709 + "x": 7.407019371926434, + "y": 7.351538272778396, + "heading": 0.07089974064841498, + "angularVelocity": 0.18508773050978647, + "velocityX": -1.682574928019174, + "velocityY": -0.2979209522500118, + "timestamp": 8.912257105876893 }, { - "x": 5.06723038345343, - "y": 4.460373312794279, - "heading": -9.620613179989556e-17, - "angularVelocity": 1.8575688667134074e-16, - "velocityX": 3.25380383021476, - "velocityY": -1.9276782936288397, - "timestamp": 11.060623764823555 + "x": 7.212241390572204, + "y": 7.319083353098022, + "heading": 0.09094586321763254, + "angularVelocity": 0.20193563564714437, + "velocityX": -1.96210590546985, + "velocityY": -0.3269362847056093, + "timestamp": 9.01152696731059 }, { - "x": 5.18256360847748, - "y": 4.398133545147613, - "heading": -8.10849749390595e-17, - "angularVelocity": 4.36361217215796e-16, - "velocityX": 3.3282470990668904, - "velocityY": -1.796094109684511, - "timestamp": 11.095276608074402 + "x": 6.990210935369373, + "y": 7.283848196853213, + "heading": 0.11243058039021528, + "angularVelocity": 0.216427391579795, + "velocityX": -2.236635087388812, + "velocityY": -0.35494313919580667, + "timestamp": 9.110796828744288 }, { - "x": 5.300292214676822, - "y": 4.340553023175948, - "heading": -6.741688231750934e-17, - "angularVelocity": 3.9442918212224324e-16, - "velocityX": 3.397372196766743, - "velocityY": -1.6616391779124173, - "timestamp": 11.129929451325248 + "x": 6.744250267253016, + "y": 7.2462331722899656, + "heading": 0.1352094476478159, + "angularVelocity": 0.2294640783075409, + "velocityX": -2.4776973047417434, + "velocityY": -0.3789168637892277, + "timestamp": 9.210066690177985 }, { - "x": 5.4202280776683445, - "y": 4.287723772710641, - "heading": -7.654370000647077e-17, - "angularVelocity": -2.6337861002637873e-16, - "velocityX": 3.461068464810413, - "velocityY": -1.5245285959043562, - "timestamp": 11.164582294576094 + "x": 6.474537469267562, + "y": 7.206343597573894, + "heading": 0.1591116816121769, + "angularVelocity": 0.24078036998495714, + "velocityX": -2.7169655934857664, + "velocityY": -0.401829660482739, + "timestamp": 9.309336551611683 }, { - "x": 5.542179542504675, - "y": 4.239730223009142, - "heading": -8.402477364947206e-17, - "angularVelocity": -2.1588628640817297e-16, - "velocityX": 3.5192340193716007, - "velocityY": -1.3849815830141687, - "timestamp": 11.19923513782694 + "x": 6.185093329086759, + "y": 7.164701733979632, + "heading": 0.18384126922195682, + "angularVelocity": 0.24911475902781344, + "velocityX": -2.915730273020737, + "velocityY": -0.4194814316536095, + "timestamp": 9.40860641304538 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -9.639099903602133e-17, - "angularVelocity": -3.5686033890323503e-16, - "velocityX": 3.5717757824418066, - "velocityY": -1.2432211735943417, - "timestamp": 11.233887981077787 + "x": 5.8780260788239564, + "y": 7.121745615538567, + "heading": 0.2121770427035368, + "angularVelocity": 0.28544185589002363, + "velocityX": -3.0932575690950848, + "velocityY": -0.43272064472210836, + "timestamp": 9.507876274479077 }, { - "x": 5.93683892832179, - "y": 4.127579524293423, - "heading": -7.782590018599437e-17, - "angularVelocity": 2.51158487397316e-16, - "velocityX": 3.664705468644969, - "velocityY": -0.9344094480089544, - "timestamp": 11.307805844823916 + "x": 5.557966335758099, + "y": 7.0784922097584015, + "heading": 0.24028820463072503, + "angularVelocity": 0.283179219968629, + "velocityX": -3.224138106404308, + "velocityY": -0.4357153838585266, + "timestamp": 9.607146135912775 }, { - "x": 6.212625572925941, - "y": 4.081838891930783, - "heading": -5.4414649647687885e-17, - "angularVelocity": 3.167197934552236e-16, - "velocityX": 3.7309877562390543, - "velocityY": -0.6188034941017393, - "timestamp": 11.381723708570044 + "x": 5.245908943265439, + "y": 7.038535334063528, + "heading": 0.26569993334648234, + "angularVelocity": 0.25598634216619676, + "velocityX": -3.1435260207458486, + "velocityY": -0.40250762031696863, + "timestamp": 9.706415997346472 }, { - "x": 6.491306353239059, - "y": 4.059759739930169, - "heading": -8.220332638334233e-17, - "angularVelocity": -3.7593993288383394e-16, - "velocityX": 3.7701411565443874, - "velocityY": -0.2986984590956788, - "timestamp": 11.455641572316173 - }, - { - "x": 6.752875972710037, - "y": 4.054277278014819, - "heading": -7.798662141044023e-17, - "angularVelocity": 5.704581760375439e-17, - "velocityX": 3.5386523123739373, - "velocityY": -0.07416964773468374, - "timestamp": 11.529559436062302 - }, - { - "x": 6.9910920962877245, - "y": 4.053575874325361, - "heading": -6.860310501240552e-17, - "angularVelocity": 1.269451783709716e-16, - "velocityX": 3.2227138543376115, - "velocityY": -0.00948896050171146, - "timestamp": 11.60347729980843 + "x": 4.948601311707987, + "y": 7.0011016986080925, + "heading": 0.2891526852975207, + "angularVelocity": 0.23625249005411966, + "velocityX": -2.9949435534976283, + "velocityY": -0.37708963138260865, + "timestamp": 9.80568585878017 }, { - "x": 7.205567187241113, - "y": 4.055021140911149, - "heading": -5.885294926916e-17, - "angularVelocity": 1.3190526956719489e-16, - "velocityX": 2.901532594204896, - "velocityY": 0.019552331635990624, - "timestamp": 11.677395163554559 + "x": 4.669544920179785, + "y": 6.966326786089845, + "heading": 0.31063199175620987, + "angularVelocity": 0.21637288647809105, + "velocityX": -2.811088758440401, + "velocityY": -0.35030685059909655, + "timestamp": 9.904955720213866 }, { - "x": 7.396227777452995, - "y": 4.057522153925315, - "heading": -4.8982438983156224e-17, - "angularVelocity": 1.3353348954892513e-16, - "velocityX": 2.5793574184815227, - "velocityY": 0.033835028333003926, - "timestamp": 11.751313027300688 + "x": 4.411446504016625, + "y": 6.934401644375056, + "heading": 0.3300269329519647, + "angularVelocity": 0.19537592694947853, + "velocityX": -2.599967527259491, + "velocityY": -0.3215995394142002, + "timestamp": 10.004225581647564 }, { - "x": 7.563054971618611, - "y": 4.060486200727262, - "heading": -3.912779424726568e-17, - "angularVelocity": 1.333188519862415e-16, - "velocityX": 2.256926617070343, - "velocityY": 0.04009919458884174, - "timestamp": 11.825230891046816 + "x": 4.176717541970206, + "y": 6.9055345106659285, + "heading": 0.3471896751796373, + "angularVelocity": 0.17288975707028345, + "velocityX": -2.364554142177353, + "velocityY": -0.2907945401778102, + "timestamp": 10.103495443081261 }, { - "x": 7.70604444565388, - "y": 4.063541585038157, - "heading": -2.9177079820924904e-17, - "angularVelocity": 1.3461853362720707e-16, - "velocityX": 1.9344373171601388, - "velocityY": 0.041334856772760636, - "timestamp": 11.899148754792945 + "x": 3.9677270788540704, + "y": 6.879953869869358, + "heading": 0.3619526945831296, + "angularVelocity": 0.14871602710307644, + "velocityX": -2.1052760636290624, + "velocityY": -0.2576878866065069, + "timestamp": 10.202765304514958 }, { - "x": 7.825196583277639, - "y": 4.066433602906459, - "heading": -1.9253434148047247e-17, - "angularVelocity": 1.3425233319755054e-16, - "velocityX": 1.6119532084015245, - "velocityY": 0.03912474903541938, - "timestamp": 11.973066618539074 + "x": 3.7869910923072463, + "y": 6.857920125982094, + "heading": 0.37415675707125545, + "angularVelocity": 0.1229382444164791, + "velocityX": -1.8206531563211636, + "velocityY": -0.22195804012459744, + "timestamp": 10.302035165948656 }, { - "x": 7.920513375065405, - "y": 4.0689768762979055, - "heading": -9.302606124041008e-18, - "angularVelocity": 1.346200704365749e-16, - "velocityX": 1.2894960292025357, - "velocityY": 0.034406749093584967, - "timestamp": 12.046984482285202 + "x": 3.6375222511124736, + "y": 6.83976261955988, + "heading": 0.3836657545916857, + "angularVelocity": 0.09578937033957077, + "velocityX": -1.5056819767458316, + "velocityY": -0.18291056479757375, + "timestamp": 10.401305027382353 }, { - "x": 7.991997299424133, - "y": 4.071030463180752, - "heading": 8.179153045928767e-19, - "angularVelocity": 1.369157726664237e-16, - "velocityX": 0.9670723792051265, - "velocityY": 0.027782010717991884, - "timestamp": 12.12090234603133 + "x": 3.5230866973435258, + "y": 6.825906628204701, + "heading": 0.39040681994356247, + "angularVelocity": 0.06790646480733925, + "velocityX": -1.1527723733691484, + "velocityY": -0.1395790339088257, + "timestamp": 10.50057488881605 }, { - "x": 8.03965088919926, - "y": 4.072483600942051, - "heading": 2.229877553000587e-19, - "angularVelocity": -8.048494898574051e-18, - "velocityX": 0.6446829948819026, - "velocityY": 0.019658817066051014, - "timestamp": 12.19482020977746 + "x": 3.4467104911360567, + "y": 6.816686862116541, + "heading": 0.39477159352312596, + "angularVelocity": 0.04396876873328538, + "velocityX": -0.7693795992500979, + "velocityY": -0.09287578279051523, + "timestamp": 10.599844750249748 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 3.803306693773524e-29, - "angularVelocity": -3.0166964241453683e-18, - "velocityX": 0.32232632402054306, - "velocityY": 0.01032706968038683, - "timestamp": 12.268738073523588 + "x": 3.4085204601287793, + "y": 6.812088489532471, + "heading": 0.39694086360000014, + "angularVelocity": 0.021852252491790684, + "velocityX": -0.3847092204594867, + "velocityY": -0.046321940190690435, + "timestamp": 10.699114611683445 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 1.8619056951323718e-29, - "angularVelocity": -1.0754545583434217e-29, - "velocityX": -2.9997098934675086e-25, - "velocityY": 4.422144616548637e-24, - "timestamp": 12.342655937269717 - } - ], - "constraints": [ + "x": 3.408520460128781, + "y": 6.812088489532471, + "heading": 0.39694086360000014, + "angularVelocity": -9.258859562419926e-18, + "velocityX": 7.880837628913899e-16, + "velocityY": -2.4194099006901583e-16, + "timestamp": 10.798384473117142 + }, { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 3.4451800140480633, + "y": 6.811382414928076, + "heading": 0.39378662700224637, + "angularVelocity": -0.032542531675579224, + "velocityX": 0.37821978715244237, + "velocityY": -0.00728463273656313, + "timestamp": 10.895311062271263 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 3.518496947445585, + "y": 6.809970351527808, + "heading": 0.38746820532810716, + "angularVelocity": -0.06518770266528738, + "velocityX": 0.756417140408642, + "velocityY": -0.014568380179178244, + "timestamp": 10.992237651425384 }, { - "scope": [ - 3 - ], - "type": "StopPoint" + "x": 3.6283397734394334, + "y": 6.8078551393922675, + "heading": 0.37750274880248313, + "angularVelocity": -0.1028144765290181, + "velocityX": 1.1332579321365746, + "velocityY": -0.021822826471036318, + "timestamp": 11.089164240579505 }, { - "scope": [ - 5 - ], - "type": "StopPoint" + "x": 3.7707535238798844, + "y": 6.805113681812527, + "heading": 0.36197971834397535, + "angularVelocity": -0.16015244726939834, + "velocityX": 1.469294975540766, + "velocityY": -0.028283854860319504, + "timestamp": 11.186090829733626 }, { - "scope": [ - 4, - 5 - ], - "type": "StraightLine" + "x": 3.9413941020263046, + "y": 6.801829282896055, + "heading": 0.341973429419134, + "angularVelocity": -0.20640661246244818, + "velocityX": 1.7605135973070385, + "velocityY": -0.03388542757086655, + "timestamp": 11.283017418887747 }, { - "scope": [ - 2 - ], - "type": "StopPoint" + "x": 4.137139185749371, + "y": 6.798061838763412, + "heading": 0.3182906699990001, + "angularVelocity": -0.24433707640817395, + "velocityX": 2.0195189517276724, + "velocityY": -0.038869046827304486, + "timestamp": 11.379944008041868 }, { - "scope": [ - 12 - ], - "type": "StopPoint" - } + "x": 4.3554217491364975, + "y": 6.793860650651487, + "heading": 0.2915337558008742, + "angularVelocity": -0.2760533970258723, + "velocityX": 2.2520400778783602, + "velocityY": -0.043344020960483176, + "timestamp": 11.476870597195989 + }, + { + "x": 4.593770534553052, + "y": 6.7892732393783435, + "heading": 0.26218341148270125, + "angularVelocity": -0.30281003978695553, + "velocityX": 2.459065025362269, + "velocityY": -0.04732871870534226, + "timestamp": 11.57379718635011 + }, + { + "x": 4.849438377602218, + "y": 6.784352472846038, + "heading": 0.23067476418411942, + "angularVelocity": -0.32507743822988416, + "velocityX": 2.6377472402607576, + "velocityY": -0.05076797373402456, + "timestamp": 11.67072377550423 + }, + { + "x": 5.118739979240287, + "y": 6.77916928922838, + "heading": 0.197506319622179, + "angularVelocity": -0.3422017100921637, + "velocityX": 2.7784079063161986, + "velocityY": -0.053475353490631775, + "timestamp": 11.767650364658351 + }, + { + "x": 5.388165163887155, + "y": 6.773984054928517, + "heading": 0.16458046580315022, + "angularVelocity": -0.33969888042458984, + "velocityX": 2.779682922902244, + "velocityY": -0.053496510556234986, + "timestamp": 11.864576953812472 + }, + { + "x": 5.643920085833601, + "y": 6.769061952936388, + "heading": 0.1333147392066983, + "angularVelocity": -0.32257120434452946, + "velocityX": 2.6386456407723093, + "velocityY": -0.05078175178849002, + "timestamp": 11.961503542966593 + }, + { + "x": 5.882330711464664, + "y": 6.764473684698037, + "heading": 0.10420435480854875, + "angularVelocity": -0.3003343525465651, + "velocityX": 2.4597030362017005, + "velocityY": -0.04733756008947892, + "timestamp": 12.058430132120714 + }, + { + "x": 6.100655045491889, + "y": 6.760272005917349, + "heading": 0.07768097943341207, + "angularVelocity": -0.27364395679871373, + "velocityX": 2.252471029183505, + "velocityY": -0.04334908323253468, + "timestamp": 12.155356721274835 + }, + { + "x": 6.29642574105686, + "y": 6.756504387895065, + "heading": 0.054221932212767415, + "angularVelocity": -0.2420290183052161, + "velocityX": 2.019783191314849, + "velocityY": -0.038870840861772844, + "timestamp": 12.252283310428956 + }, + { + "x": 6.467079365349485, + "y": 6.7532201591852425, + "heading": 0.03442675668029697, + "angularVelocity": -0.20422853734174792, + "velocityX": 1.7606481955253344, + "velocityY": -0.03388367153413224, + "timestamp": 12.349209899583077 + }, + { + "x": 6.60949406591357, + "y": 6.750479406444339, + "heading": 0.019100936295010484, + "angularVelocity": -0.15811781389436272, + "velocityX": 1.4693047780484314, + "velocityY": -0.028276582977095337, + "timestamp": 12.446136488737197 + }, + { + "x": 6.719322188320328, + "y": 6.748365796127614, + "heading": 0.009318208717894495, + "angularVelocity": -0.10092924616960162, + "velocityX": 1.1331062339573879, + "velocityY": -0.021806300367839485, + "timestamp": 12.543063077891318 + }, + { + "x": 6.792644320795612, + "y": 6.746954731447967, + "heading": 0.0031061365662838365, + "angularVelocity": -0.06409048544701255, + "velocityX": 0.756470779743405, + "velocityY": -0.014558076292195965, + "timestamp": 12.63998966704544 + }, + { + "x": 6.829305171966555, + "y": 6.746249198913574, + "heading": -6.996174209709525e-17, + "angularVelocity": -0.03204627949246174, + "velocityX": 0.3782331710099824, + "velocityY": -0.007279040153471663, + "timestamp": 12.73691625619956 + }, + { + "x": 6.8293051719665545, + "y": 6.746249198913574, + "heading": -3.495236266448189e-17, + "angularVelocity": 5.858001175456844e-19, + "velocityX": -7.463612085764823e-16, + "velocityY": 1.4437890171198592e-17, + "timestamp": 12.833842845353681 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4, + 5 + ], + "type": "StraightLine" + }, + { + "scope": [ + 8 + ], + "type": "StopPoint" + } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "CenterLanePCBADSprint": { + "SourceLanePGHSprint": { "waypoints": [ { - "x": 1.2899744510650635, - "y": 5.590954303741455, + "x": 0.43324199318885803, + "y": 4.121134281158447, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 19 }, { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 22 }, { - "x": 1.8129411935806274, - "y": 5.542999267578125, + "x": 5.5030035972595215, + "y": 1.563418984413147, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "headingConstrained": false, + "controlIntervalCount": 17 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 0, + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 9 }, { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 19 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 25 + "headingConstrained": false, + "controlIntervalCount": 22 }, { - "x": 7.374384880065918, - "y": 7.465639114379883, - "heading": 0, + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 21 }, { - "x": 7.923961639404297, - "y": 7.465639114379883, - "heading": 0, + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 24 + "controlIntervalCount": 14 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 9 }, { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": 0, + "x": 8.288774490356445, + "y": 0.7401233315467834, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 16 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -24718,5303 +14708,5539 @@ ], "trajectory": [ { - "x": 1.289974451065063, - "y": 5.590954303741456, - "heading": 6.19779206460361e-17, - "angularVelocity": 3.0320144312848013e-18, - "velocityX": 2.9896394667646305e-17, - "velocityY": -6.392586992550233e-17, + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": -6.349582694240188e-38, + "angularVelocity": 0, + "velocityX": -2.609211774957871e-36, + "velocityY": -5.016830702674688e-35, "timestamp": 0 }, { - "x": 1.3085927366636296, - "y": 5.570883327334207, - "heading": -0.01041389258493421, - "angularVelocity": -0.12226255374555442, - "velocityX": 0.21858484952476373, - "velocityY": -0.23563992154741434, - "timestamp": 0.08517646872162599 - }, - { - "x": 1.3442657042493709, - "y": 5.532508491008622, - "heading": -0.03089006103804589, - "angularVelocity": -0.24039701058788923, - "velocityX": 0.4188124739278457, - "velocityY": -0.45053330927585317, - "timestamp": 0.17035293744325197 + "x": 0.45469188032137553, + "y": 4.109218526997661, + "heading": -0.008937300145995164, + "angularVelocity": -0.11893237262991438, + "velocityX": 0.2854425752342375, + "velocityY": -0.1585678997982562, + "timestamp": 0.07514606787342622 }, { - "x": 1.3955881762749254, - "y": 5.477434538773948, - "heading": -0.060869932572704226, - "angularVelocity": -0.35197363760921707, - "velocityX": 0.6025428477586634, - "velocityY": -0.646586469963527, - "timestamp": 0.25552940616487796 + "x": 0.4975916635116251, + "y": 4.0853873108981436, + "heading": -0.026812413296636577, + "angularVelocity": -0.2378715700833438, + "velocityX": 0.570885269240018, + "velocityY": -0.31713191087600345, + "timestamp": 0.15029213574685243 }, { - "x": 1.4613425796478687, - "y": 5.4070770106059785, - "heading": -0.09976067247727309, - "angularVelocity": -0.45659018844361554, - "velocityX": 0.7719785095557636, - "velocityY": -0.826020721731408, - "timestamp": 0.34070587488650395 + "x": 0.5619414537480725, + "y": 4.049641027140498, + "heading": -0.05362171709802473, + "angularVelocity": -0.35676256336585627, + "velocityX": 0.856329440215504, + "velocityY": -0.4756906750976657, + "timestamp": 0.22543820362027867 }, { - "x": 1.5403946295377764, - "y": 5.322786826751108, - "heading": -0.14691970563793563, - "angularVelocity": -0.5536626942681541, - "velocityX": 0.9280972911458275, - "velocityY": -0.9895947216401798, - "timestamp": 0.42588234360812993 + "x": 0.6477414523773106, + "y": 4.001980177860296, + "heading": -0.08935853104911506, + "angularVelocity": -0.4755646564406316, + "velocityX": 1.141776290593902, + "velocityY": -0.6342427571923069, + "timestamp": 0.30058427149370487 }, { - "x": 1.6316218404845053, - "y": 5.225944550707253, - "heading": -0.20162745404412188, - "angularVelocity": -0.6422871155293178, - "velocityX": 1.0710377210503823, - "velocityY": -1.1369604480828668, - "timestamp": 0.5110588123297559 + "x": 0.7549919358777414, + "y": 3.942405375853288, + "heading": -0.13401442200653152, + "angularVelocity": -0.5942545261667384, + "velocityX": 1.4272268201854585, + "velocityY": -0.7927866845588475, + "timestamp": 0.37573033936713107 }, { - "x": 1.733844381505324, - "y": 5.118072938931053, - "heading": -0.2630340547325837, - "angularVelocity": -0.7209338636607621, - "velocityX": 1.2001265438099284, - "velocityY": -1.2664485085516666, - "timestamp": 0.5962352810513819 + "x": 0.8836932393269182, + "y": 3.8709173416225418, + "heading": -0.18758057158075755, + "angularVelocity": -0.7128270459134503, + "velocityX": 1.7126818087934743, + "velocityY": -0.9513210238912078, + "timestamp": 0.4508764072405573 }, { - "x": 1.8457280635804503, - "y": 5.001046382736992, - "heading": -0.33003861392476214, - "angularVelocity": -0.7866557536115174, - "velocityX": 1.3135515448613582, - "velocityY": -1.373930593160977, - "timestamp": 0.6814117497730079 + "x": 1.0338457421402325, + "y": 3.7875168932975933, + "heading": -0.2500489995023069, + "angularVelocity": -0.8312933688928248, + "velocityX": 1.998141846440012, + "velocityY": -1.109844476033343, + "timestamp": 0.5260224751139835 }, { - "x": 1.9656142072747882, - "y": 4.877758551128575, - "heading": -0.40085023097520717, - "angularVelocity": -0.8313518758551945, - "velocityX": 1.4075030990795228, - "velocityY": -1.447440043697368, - "timestamp": 0.7665882184946339 + "x": 1.2054498603190866, + "y": 3.692204931169301, + "heading": -0.32141344961465557, + "angularVelocity": -0.9496764385936038, + "velocityX": 2.283607419990343, + "velocityY": -1.2683559476303248, + "timestamp": 0.6011685429874097 }, { - "x": 2.0856915448639635, - "y": 4.7606487505271025, - "heading": -0.4673031995363478, - "angularVelocity": -0.7801798966134944, - "velocityX": 1.4097477788332962, - "velocityY": -1.3749079101202415, - "timestamp": 0.8517646872162599 + "x": 1.3985060453231253, + "y": 3.5849824249792595, + "heading": -0.40166985798593763, + "angularVelocity": -1.0680054278616893, + "velocityX": 2.5690790013020615, + "velocityY": -1.4268545144723213, + "timestamp": 0.676314610860836 }, { - "x": 2.1993786689736172, - "y": 4.652829385838518, - "heading": -0.5273883646655675, - "angularVelocity": -0.7054197718103367, - "velocityX": 1.3347245526367961, - "velocityY": -1.2658351104101313, - "timestamp": 0.9369411559378859 + "x": 1.5915937569821006, + "y": 3.4778198154599647, + "heading": -0.48176859267038563, + "angularVelocity": -1.065907198489266, + "velocityX": 2.5694985396202825, + "velocityY": -1.4260574445464824, + "timestamp": 0.7514606787342623 }, { - "x": 2.3039823973737983, - "y": 4.5566291473122265, - "heading": -0.5809972368261334, - "angularVelocity": -0.6293859438546427, - "velocityX": 1.2280824736001708, - "velocityY": -1.1294227146313731, - "timestamp": 1.0221176246595118 + "x": 1.7632284858900336, + "y": 3.382566036520044, + "heading": -0.5529765476804537, + "angularVelocity": -0.9475938931363465, + "velocityX": 2.2840147697019786, + "velocityY": -1.2675816797275796, + "timestamp": 0.8266067466076885 }, { - "x": 2.3979860520770147, - "y": 4.47341696133058, - "heading": -0.6272322006251169, - "angularVelocity": -0.5428138134029581, - "velocityX": 1.1036340918339758, - "velocityY": -0.9769386689838181, - "timestamp": 1.107294093381138 + "x": 1.9134098098562766, + "y": 3.2992204007390877, + "heading": -0.6152918418017524, + "angularVelocity": -0.8292555536806094, + "velocityX": 1.9985253815170194, + "velocityY": -1.1091150626981792, + "timestamp": 0.9017528144811148 }, { - "x": 2.480103614094797, - "y": 4.40444635319333, - "heading": -0.6652761118925774, - "angularVelocity": -0.4466481393094119, - "velocityX": 0.964087420507652, - "velocityY": -0.8097378204614404, - "timestamp": 1.192470562102764 + "x": 2.0421373577646693, + "y": 3.2277823059512354, + "heading": -0.6687115003742364, + "angularVelocity": -0.7108776291856339, + "velocityX": 1.7130310547348626, + "velocityY": -0.9506564589404816, + "timestamp": 0.976898882354541 }, { - "x": 2.549135601344952, - "y": 4.350942377101982, - "heading": -0.6944118607427208, - "angularVelocity": -0.3420633572561569, - "velocityX": 0.810458431609397, - "velocityY": -0.6281544292028495, - "timestamp": 1.2776470308243901 + "x": 2.149410808485792, + "y": 3.1682512394298143, + "heading": -0.713232041811307, + "angularVelocity": -0.5924533737687984, + "velocityX": 1.4275324545498613, + "velocityY": -0.792204678250007, + "timestamp": 1.0520449502279672 }, { - "x": 2.6038790433453562, - "y": 4.3141752600275325, - "heading": -0.7139618385994653, - "angularVelocity": -0.2295232257237371, - "velocityX": 0.6427061701667566, - "velocityY": -0.431658151908278, - "timestamp": 1.3628234995460162 + "x": 2.2352298936713018, + "y": 3.120626784362015, + "heading": -0.7488501306819736, + "angularVelocity": -0.4739847323836121, + "velocityX": 1.1420302833417786, + "velocityY": -0.6337584442610682, + "timestamp": 1.1271910181013933 }, { - "x": 2.6430389310079985, - "y": 4.2955467477546305, - "heading": -0.7232599969808831, - "angularVelocity": -0.1091634640525668, - "velocityX": 0.459750072412904, - "velocityY": -0.2187049140741477, - "timestamp": 1.4479999682676423 + "x": 2.2995943987852443, + "y": 3.0849086278583613, + "heading": -0.7755632624607879, + "angularVelocity": -0.3554827622359307, + "velocityX": 0.8565252572144717, + "velocityY": -0.4753163740226096, + "timestamp": 1.2023370859748195 }, { - "x": 2.665171623229982, - "y": 4.296644687652586, - "heading": -0.721655115096118, - "angularVelocity": 0.018841845745096026, - "velocityX": 0.2598451491845503, - "velocityY": 0.012890178642452559, - "timestamp": 1.5331764369892684 + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080857, + "angularVelocity": -0.23696688264902363, + "velocityX": 0.5710180813549187, + "velocityY": -0.3168769856057537, + "timestamp": 1.2774831538482456 }, { - "x": 2.66906909991143, - "y": 4.318945956778202, - "heading": -0.7091344221786624, - "angularVelocity": 0.14601587711521094, - "velocityX": 0.045452234946537245, - "velocityY": 0.2600766102664744, - "timestamp": 1.6189252826957972 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288599550474, + "velocityX": 0.2855094397331478, + "velocityY": -0.15843872456938407, + "timestamp": 1.3526292217216718 }, { - "x": 2.6560642905413947, - "y": 4.360643314893585, - "heading": -0.6858248901313277, - "angularVelocity": 0.2718349367303499, - "velocityX": -0.15166162602986216, - "velocityY": 0.4862731127377579, - "timestamp": 1.704674128402326 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 3.467177167133168e-32, + "velocityX": -1.439882477346126e-32, + "velocityY": -1.3815367235364851e-32, + "timestamp": 1.427775289595098 }, { - "x": 2.627632992233998, - "y": 4.419910426276983, - "heading": -0.652529270737317, - "angularVelocity": 0.38829233349639936, - "velocityX": -0.331564793358298, - "velocityY": 0.6911709527407293, - "timestamp": 1.7904229741088546 + "x": 2.3773546785315864, + "y": 3.0407954088560687, + "heading": -0.7860770412156382, + "angularVelocity": 0.2650004205849919, + "velocityX": 0.21918861143707746, + "velocityY": -0.13736692695017716, + "timestamp": 1.4888897992558612 }, { - "x": 2.5850645171946978, - "y": 4.495119603214681, - "heading": -0.6100207421436514, - "angularVelocity": 0.4957329541105318, - "velocityX": -0.4964320474352394, - "velocityY": 0.877086756305711, - "timestamp": 1.8761718198153834 + "x": 2.4041715283150715, + "y": 3.0239975454455843, + "heading": -0.75411979281041, + "angularVelocity": 0.5229077118120985, + "velocityX": 0.438796775632191, + "velocityY": -0.2748588429118795, + "timestamp": 1.5500043089166244 }, { - "x": 2.5295631514165375, - "y": 4.584706937933874, - "heading": -0.5590704924389158, - "angularVelocity": 0.5941800065637087, - "velocityX": -0.6472549609368695, - "velocityY": 1.0447643228435226, - "timestamp": 1.9619206655219121 + "x": 2.4444388170293236, + "y": 2.998788446329585, + "heading": -0.7069088657797715, + "angularVelocity": 0.772499481591177, + "velocityX": 0.6588826276733534, + "velocityY": -0.41248959135778174, + "timestamp": 1.6111188185773877 }, { - "x": 2.46234032691493, - "y": 4.687033037499603, - "heading": -0.5004982071284244, - "angularVelocity": 0.6830679157006081, - "velocityX": -0.7839501972035283, - "velocityY": 1.1933233470679585, - "timestamp": 2.047669511228441 + "x": 2.498190798561039, + "y": 2.965157152573787, + "heading": -0.6450804787000075, + "angularVelocity": 1.0116809808826641, + "velocityX": 0.8795289666903056, + "velocityY": -0.5502996578468837, + "timestamp": 1.672233328238151 }, { - "x": 2.384701887418879, - "y": 4.800204071292841, - "heading": -0.43526625936464175, - "angularVelocity": 0.7607326632364927, - "velocityX": -0.9054167301769336, - "velocityY": 1.319796585723857, - "timestamp": 2.1334183569349694 + "x": 2.5654677170302604, + "y": 2.923088008476279, + "heading": -0.5694581806203519, + "angularVelocity": 1.237386972413308, + "velocityX": 1.1008338092322851, + "velocityY": -0.6883658943027919, + "timestamp": 1.7333478378989142 }, { - "x": 2.298178863290851, - "y": 4.921641059920068, - "heading": -0.36471802922759106, - "angularVelocity": 0.8227309598836902, - "velocityX": -1.0090284413174404, - "velocityY": 1.416193858082253, - "timestamp": 2.219167202641498 + "x": 2.6463157759141644, + "y": 2.8725583813996622, + "heading": -0.4811095285927534, + "angularVelocity": 1.445624819997864, + "velocityX": 1.3228946666295571, + "velocityY": -0.8268024624119354, + "timestamp": 1.7944623475596775 }, { - "x": 2.2047510720717436, - "y": 5.044849013310237, - "heading": -0.2923595970689714, - "angularVelocity": 0.8438414717122018, - "velocityX": -1.0895515904535924, - "velocityY": 1.4368467863910614, - "timestamp": 2.3049160483480264 + "x": 2.7407862870930626, + "y": 2.813537598490058, + "heading": -0.38139476987658943, + "angularVelocity": 1.6316053138553275, + "velocityX": 1.5457951262848801, + "velocityY": -0.9657409220366661, + "timestamp": 1.8555768572204407 }, { - "x": 2.116347360692902, - "y": 5.158956328540576, - "heading": -0.22539524544258233, - "angularVelocity": 0.7809358956920709, - "velocityX": -1.0309609493916598, - "velocityY": 1.3307154666649113, - "timestamp": 2.390664894054555 + "x": 2.84893500974016, + "y": 2.7459886359527177, + "heading": -0.27202745092011676, + "angularVelocity": 1.7895475160244771, + "velocityX": 1.7696079580350594, + "velocityY": -1.105285191884772, + "timestamp": 1.916691366881204 }, { - "x": 2.0362102002706437, - "y": 5.261312637562149, - "heading": -0.16556699187625293, - "angularVelocity": 0.6977149729932085, - "velocityX": -0.9345567250727145, - "velocityY": 1.1936756486715054, - "timestamp": 2.4764137397610835 + "x": 2.970822636317052, + "y": 2.669873093439706, + "heading": -0.15523119808259794, + "angularVelocity": 1.9111051284848073, + "velocityX": 1.9944138839282306, + "velocityY": -1.245457796119401, + "timestamp": 1.9778058765419673 }, { - "x": 1.9661275657868833, - "y": 5.350222584355773, - "heading": -0.1134489428170574, - "angularVelocity": 0.6077988412528272, - "velocityX": -0.8173011998740509, - "velocityY": 1.0368646488597075, - "timestamp": 2.562162585467612 + "x": 3.106512263881207, + "y": 2.5851587340080315, + "heading": -0.03419676462946541, + "angularVelocity": 1.980453318286855, + "velocityX": 2.220252249708728, + "velocityY": -1.3861578846318265, + "timestamp": 2.0389203862027303 }, { - "x": 1.9075019379268083, - "y": 5.42424212633875, - "heading": -0.06991588076878923, - "angularVelocity": 0.5076810269523403, - "velocityX": -0.6836899946235904, - "velocityY": 0.8632132756201296, - "timestamp": 2.6479114311741405 + "x": 3.256035657144846, + "y": 2.4918371927335334, + "heading": 0.08572826850512172, + "angularVelocity": 1.962300504418209, + "velocityX": 2.4466103727841246, + "velocityY": -1.5269948461095477, + "timestamp": 2.1000348958634936 }, { - "x": 1.861590779787906, - "y": 5.48200360252104, - "heading": -0.03588037308734902, - "angularVelocity": 0.3969208844854328, - "velocityX": -0.5354142992901785, - "velocityY": 0.673612288379681, - "timestamp": 2.733660276880669 + "x": 3.419180860242083, + "y": 2.390029027608911, + "heading": 0.19415018142326176, + "angularVelocity": 1.7740780956923756, + "velocityX": 2.6695003200194134, + "velocityY": -1.6658591501387068, + "timestamp": 2.161149405524257 }, { - "x": 1.8296344375134035, - "y": 5.522099910983847, - "heading": -0.012259641477970167, - "angularVelocity": 0.2754641349951199, - "velocityX": -0.3726737311878495, - "velocityY": 0.4676017284248481, - "timestamp": 2.8194091225871976 + "x": 3.5929644641848295, + "y": 2.279455345020229, + "heading": 0.26705766200125053, + "angularVelocity": 1.1929651564364412, + "velocityX": 2.8435735622750062, + "velocityY": -1.8092869140644157, + "timestamp": 2.22226391518502 }, { - "x": 1.8129411935806854, - "y": 5.542999267578127, - "heading": 2.3632850028431835e-17, - "angularVelocity": 0.14297150447865262, - "velocityX": -0.1946760191950624, - "velocityY": 0.24372755600475027, - "timestamp": 2.905157968293726 + "x": 3.7768655464749616, + "y": 2.1600020280366277, + "heading": 0.30152237044830693, + "angularVelocity": 0.5639365944088304, + "velocityX": 3.0091230922237155, + "velocityY": -1.9545819421061743, + "timestamp": 2.2833784248457833 }, { - "x": 1.812941193580624, - "y": 5.542999267578126, - "heading": -4.920156603511903e-17, - "angularVelocity": -5.733397110540358e-17, - "velocityX": -7.214388151371406e-13, - "velocityY": -9.03683027618772e-16, - "timestamp": 2.9909068140002546 + "x": 3.9728967066647076, + "y": 2.0375522576737963, + "heading": 0.30152241765502197, + "angularVelocity": 7.724305624896446e-7, + "velocityX": 3.2076042379769354, + "velocityY": -2.003612088889046, + "timestamp": 2.3444929345065466 }, { - "x": 1.8513325137310372, - "y": 5.5430599450572595, - "heading": -0.000016850565291482023, - "angularVelocity": -0.00016991086246254008, - "velocityX": 0.3871147468977978, - "velocityY": 0.0006118348336243464, - "timestamp": 3.090079786505359 + "x": 4.174386727718979, + "y": 1.9243094504352285, + "heading": 0.30152246485493484, + "angularVelocity": 7.723192600121745e-7, + "velocityX": 3.2969260847008353, + "velocityY": -1.85296107040964, + "timestamp": 2.40560744416731 }, { - "x": 1.9275402570059652, - "y": 5.543180391388335, - "heading": -0.000015746696122309923, - "angularVelocity": 0.00001113074602288469, - "velocityX": 0.7684325814778618, - "velocityY": 0.0012145076227106227, - "timestamp": 3.189252759010463 + "x": 4.383354452565466, + "y": 1.8255438101811146, + "heading": 0.30152251288342946, + "angularVelocity": 7.858771165145409e-7, + "velocityX": 3.419281705873656, + "velocityY": -1.6160751481496927, + "timestamp": 2.466721953828073 }, { - "x": 2.0338963277323163, - "y": 5.54334848714078, - "heading": 0.000009170336978677823, - "angularVelocity": 0.00025124822289482134, - "velocityX": 1.0724299982122594, - "velocityY": 0.0016949754373461064, - "timestamp": 3.2884257315155674 + "x": 4.598761540028333, + "y": 1.7417467464566943, + "heading": 0.30152256294147634, + "angularVelocity": 8.190861247953373e-7, + "velocityX": 3.5246472344874826, + "velocityY": -1.3711484259558735, + "timestamp": 2.5278364634888364 }, { - "x": 2.163383514243291, - "y": 5.5435531416216355, - "heading": 0.000033654463962368084, - "angularVelocity": 0.0002468830606297171, - "velocityX": 1.305670115961409, - "velocityY": 0.00206361144256778, - "timestamp": 3.3875987040206716 + "x": 4.819537390717514, + "y": 1.6733348748555603, + "heading": 0.30152261646908723, + "angularVelocity": 8.758576510857166e-7, + "velocityX": 3.612494838208991, + "velocityY": -1.1194047367945392, + "timestamp": 2.5889509731495997 }, { - "x": 2.3079360710490797, - "y": 5.54378160701675, - "heading": 0.00003317773344428256, - "angularVelocity": -0.000004807060894986283, - "velocityX": 1.457580156714077, - "velocityY": 0.0023037062351123753, - "timestamp": 3.486771676525776 + "x": 5.04458467733647, + "y": 1.620648272623701, + "heading": 0.30152267534909943, + "angularVelocity": 9.634375304093619e-7, + "velocityX": 3.682387175617667, + "velocityY": -0.8620964567058491, + "timestamp": 2.650065482810363 }, { - "x": 2.437398077457075, - "y": 5.543986221690917, - "heading": 0.000022146124274066103, - "angularVelocity": -0.00011123604437218979, - "velocityX": 1.3054162151016944, - "velocityY": 0.002063210056106694, - "timestamp": 3.58594464903088 + "x": 5.27278482830379, + "y": 1.5839488347993986, + "heading": 0.301522742248914, + "angularVelocity": 0.000001094663361600596, + "velocityX": 3.7339766322927477, + "velocityY": -0.6005028597630109, + "timestamp": 2.711179992471126 }, { - "x": 2.5437229473484053, - "y": 5.544154268122903, - "heading": 0.000015406432122086, - "angularVelocity": -0.00006795896080977589, - "velocityX": 1.0721153879486738, - "velocityY": 0.0016944781198031775, - "timestamp": 3.6851176215359844 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3015228161300743, + "angularVelocity": 0.0000012088972110609183, + "velocityX": 3.7670067261218194, + "velocityY": -0.33592432468507777, + "timestamp": 2.7722945021318894 }, { - "x": 2.619913870706316, - "y": 5.544274687859312, - "heading": 0.000009213432317016853, - "angularVelocity": -0.00006244644733859944, - "velocityX": 0.7682629796539725, - "velocityY": 0.0012142394582550931, - "timestamp": 3.7842905940410887 + "x": 5.642161233224445, + "y": 1.5569512204704454, + "heading": 0.30152289610635313, + "angularVelocity": 0.0000021712102282875206, + "velocityX": 3.7778762267369546, + "velocityY": -0.17558800471026415, + "timestamp": 2.809129387028348 }, { - "x": 2.6583051681519314, - "y": 5.5443353652954785, - "heading": 6.370510606485668e-14, - "angularVelocity": -0.00009290265301587805, - "velocityX": 0.38711451795641516, - "velocityY": 0.0006118344003786568, - "timestamp": 3.883463566546193 + "x": 5.781468008961295, + "y": 1.556401100893367, + "heading": 0.30152296986307525, + "angularVelocity": 0.0000020023605969229892, + "velocityX": 3.781925099764402, + "velocityY": -0.014934744023907952, + "timestamp": 2.8459642719248066 }, { - "x": 2.658305168152, - "y": 5.544335365295488, - "heading": 6.625668653050138e-14, - "angularVelocity": 2.6906565131836242e-14, - "velocityX": 7.155954565396242e-13, - "velocityY": 8.671180221391495e-14, - "timestamp": 3.982636539051297 + "x": 5.920672391336217, + "y": 1.5617696165699877, + "heading": 0.3015230386365787, + "angularVelocity": 0.0000018670752912406278, + "velocityX": 3.779145306581512, + "velocityY": 0.14574541746802047, + "timestamp": 2.882799156821265 }, { - "x": 2.6398879819889713, - "y": 5.568034006444528, - "heading": 0.008388499707182374, - "angularVelocity": 0.0943787022324955, - "velocityX": -0.20721108536177554, - "velocityY": 0.26663254150593735, - "timestamp": 4.071517817439562 + "x": 6.0595230325638285, + "y": 1.5730470711005764, + "heading": 0.3015231034083751, + "angularVelocity": 0.0000017584362380693717, + "velocityX": 3.769541879061509, + "velocityY": 0.30616233937716997, + "timestamp": 2.919634041717724 }, { - "x": 2.6055876283476542, - "y": 5.613232266775393, - "heading": 0.025296828917652628, - "angularVelocity": 0.19023499118162804, - "velocityX": -0.3859120195327329, - "velocityY": 0.5085239675934979, - "timestamp": 4.160399095827827 + "x": 6.197769224123459, + "y": 1.5902130982406153, + "heading": 0.3015231649742209, + "angularVelocity": 0.000001671400517937078, + "velocityX": 3.753132172076425, + "velocityY": 0.46602635486148125, + "timestamp": 2.9564689266141824 }, { - "x": 2.5578825329127857, - "y": 5.677978135359711, - "heading": 0.05006181260458975, - "angularVelocity": 0.27862992225075944, - "velocityX": -0.5367282773164671, - "velocityY": 0.7284533903912082, - "timestamp": 4.2492803742160925 + "x": 6.335161349542959, + "y": 1.6132366986444322, + "heading": 0.3015232239953199, + "angularVelocity": 0.0000016023152822250254, + "velocityX": 3.72994583275345, + "velocityY": 0.6250487946015117, + "timestamp": 2.993303811510641 }, { - "x": 2.4993543830957403, - "y": 5.760558609750369, - "heading": 0.0821470752651059, - "angularVelocity": 0.36099011222914734, - "velocityX": -0.6584980648176972, - "velocityY": 0.9291098855468343, - "timestamp": 4.338161652604358 + "x": 6.471451335210211, + "y": 1.6420762959087598, + "heading": 0.30152328103508963, + "angularVelocity": 0.000001548525804202316, + "velocityX": 3.7000247469309935, + "velocityY": 0.7829425107583317, + "timestamp": 3.0301386964070995 }, { - "x": 2.433098392034208, - "y": 5.859358208334035, - "heading": 0.12114426250239245, - "angularVelocity": 0.43875592188117873, - "velocityX": -0.7454437229429913, - "velocityY": 1.1115906563809108, - "timestamp": 4.427042930992623 + "x": 6.606393098338155, + "y": 1.6766798119201127, + "heading": 0.301523336612691, + "angularVelocity": 0.000001508830595721816, + "velocityX": 3.663422961881362, + "velocityY": 0.9394224010369989, + "timestamp": 3.066973581303558 }, { - "x": 2.3637465533640976, - "y": 5.972668871410117, - "heading": 0.16678728284828395, - "angularVelocity": 0.5135279461940396, - "velocityX": -0.7802749907275444, - "velocityY": 1.2748541102325668, - "timestamp": 4.515924209380888 + "x": 6.739742463422568, + "y": 1.716984614087568, + "heading": 0.3015255656708468, + "angularVelocity": 0.00006051486687697733, + "velocityX": 3.6201922568579414, + "velocityY": 1.094201930608725, + "timestamp": 3.1038084662000167 }, { - "x": 2.300988842467506, - "y": 6.095835737634532, - "heading": 0.21776795840799054, - "angularVelocity": 0.5735817090404455, - "velocityX": -0.7060847012414084, - "velocityY": 1.385745889998316, - "timestamp": 4.604805487769153 - }, - { - "x": 2.2515752134889366, - "y": 6.217231027383832, - "heading": 0.2695393710632505, - "angularVelocity": 0.582478263071738, - "velocityX": -0.5559509254902292, - "velocityY": 1.3658139481175915, - "timestamp": 4.693686766157418 + "x": 6.8694207904091344, + "y": 1.7614703171513673, + "heading": 0.3101205256410531, + "angularVelocity": 0.23333750042565204, + "velocityX": 3.520530262306672, + "velocityY": 1.207705771006116, + "timestamp": 3.1406433510964753 }, { - "x": 2.217045589242532, - "y": 6.33145065782542, - "heading": 0.3198059590268783, - "angularVelocity": 0.5655475357136119, - "velocityX": -0.3884915346931451, - "velocityY": 1.2850808686903799, - "timestamp": 4.782568044545683 + "x": 6.994764102779472, + "y": 1.808693107878132, + "heading": 0.3306180633638746, + "angularVelocity": 0.5564707961064413, + "velocityX": 3.4028425152588873, + "velocityY": 1.282012713206686, + "timestamp": 3.177478235992934 }, { - "x": 2.19907919431313, - "y": 6.435243176299548, - "heading": 0.36734143796630997, - "angularVelocity": 0.5348199283510537, - "velocityX": -0.20213924974232386, - "velocityY": 1.1677658147606698, - "timestamp": 4.871449322933948 + "x": 7.114597005425528, + "y": 1.856512920318317, + "heading": 0.35962771339750194, + "angularVelocity": 0.7875591335542964, + "velocityX": 3.253244932973218, + "velocityY": 1.2982207647615676, + "timestamp": 3.2143131208893925 }, { - "x": 2.1993959276283994, - "y": 6.526053318679287, - "heading": 0.41157070153690833, - "angularVelocity": 0.4976218206191788, - "velocityX": 0.0035635549001480525, - "velocityY": 1.021701577951703, - "timestamp": 4.960330601322213 + "x": 7.228563598795877, + "y": 1.9039637614742513, + "heading": 0.39146295222083616, + "angularVelocity": 0.8642687200685354, + "velocityX": 3.093985326429113, + "velocityY": 1.2882038667778264, + "timestamp": 3.251148005785851 }, { - "x": 2.219770279567901, - "y": 6.601608618777817, - "heading": 0.45152771224881855, - "angularVelocity": 0.4495548605556116, - "velocityX": 0.2292310856552352, - "velocityY": 0.8500699075051399, - "timestamp": 5.049211879710478 + "x": 7.336676614220082, + "y": 1.9505339221399116, + "heading": 0.42341393247502157, + "angularVelocity": 0.8674108889982484, + "velocityX": 2.935071352282109, + "velocityY": 1.2642949963483496, + "timestamp": 3.2879828906823096 }, { - "x": 2.2626490848637184, - "y": 6.659247151615926, - "heading": 0.48436572022362784, - "angularVelocity": 0.36945922212429394, - "velocityX": 0.4824278641490618, - "velocityY": 0.648489016846146, - "timestamp": 5.1380931580987435 + "x": 7.438996464546078, + "y": 1.995939510716552, + "heading": 0.45395278545537515, + "angularVelocity": 0.8290742068611583, + "velocityX": 2.77779747686502, + "velocityY": 1.2326789863542051, + "timestamp": 3.3248177755787682 }, { - "x": 2.329372405994089, - "y": 6.697374820688125, - "heading": 0.5125504195998649, - "angularVelocity": 0.31710501792120865, - "velocityX": 0.7507016363871187, - "velocityY": 0.4289730049288785, - "timestamp": 5.2269744364870085 + "x": 7.535582073954796, + "y": 2.0400031624519186, + "heading": 0.48208291467910164, + "angularVelocity": 0.7636817463336474, + "velocityX": 2.6221232855814667, + "velocityY": 1.196247846551653, + "timestamp": 3.361652660475227 }, { - "x": 2.399251093648798, - "y": 6.73730555904706, - "heading": 0.5293913369356736, - "angularVelocity": 0.21745079806450165, - "velocityX": 0.902277239279105, - "velocityY": 0.5155877648973247, - "timestamp": 5.304421461219836 + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6791276743107281, + "velocityX": 2.4678193928677437, + "velocityY": 1.1565397180800703, + "timestamp": 3.3984875453716854 }, { - "x": 2.4725481079431484, - "y": 6.779189629834486, - "heading": 0.507422235287016, - "angularVelocity": -0.28366617987293774, - "velocityX": 0.9464148501358945, - "velocityY": 0.5408092945202858, - "timestamp": 5.381868485952664 + "x": 7.766333855095899, + "y": 2.1513911601167894, + "heading": 0.5393931714770351, + "angularVelocity": 0.5089507306836832, + "velocityX": 2.2039777611896034, + "velocityY": 1.0840548028640598, + "timestamp": 3.461940970502476 }, { - "x": 2.537702213604298, - "y": 6.816420603018375, - "heading": 0.5016725044650684, - "angularVelocity": -0.07424082257084566, - "velocityX": 0.8412731915467285, - "velocityY": 0.48072825693890747, - "timestamp": 5.459315510685491 + "x": 7.8897168213730975, + "y": 2.214535022901093, + "heading": 0.5621690879226864, + "angularVelocity": 0.3589391179231937, + "velocityX": 1.9444650312080196, + "velocityY": 0.9951214241650669, + "timestamp": 3.5253943956332665 }, { - "x": 2.5904059035391898, - "y": 6.846537042294488, - "heading": 0.5036538467311155, - "angularVelocity": 0.025583194098508056, - "velocityX": 0.680512777970975, - "velocityY": 0.38886502613926416, - "timestamp": 5.536762535418319 + "x": 7.996856912991746, + "y": 2.271342778200504, + "heading": 0.5763089063837279, + "angularVelocity": 0.2228377496076287, + "velocityX": 1.6884839769927567, + "velocityY": 0.8952669644281369, + "timestamp": 3.588847820764057 }, { - "x": 2.6280877308244723, - "y": 6.868069547224279, - "heading": 0.508503710225543, - "angularVelocity": 0.06262168896947905, - "velocityX": 0.4865497083575037, - "velocityY": 0.27802882066934376, - "timestamp": 5.614209560151147 + "x": 8.08793407853681, + "y": 2.3213235528188796, + "heading": 0.5824533246990756, + "angularVelocity": 0.09683351690917834, + "velocityX": 1.4353388387992245, + "velocityY": 0.7876765441007412, + "timestamp": 3.6523012458948476 }, { - "x": 2.648344516781087, - "y": 6.8796448707953655, - "heading": 0.5125504196001363, - "angularVelocity": 0.052251321320124576, - "velocityX": 0.26155667100350394, - "velocityY": 0.14946117855453953, - "timestamp": 5.691656584883974 + "x": 8.16309393881241, + "y": 2.364112462024999, + "heading": 0.5810872026138336, + "angularVelocity": -0.021529524725041504, + "velocityX": 1.1844886248564517, + "velocityY": 0.6743356898059124, + "timestamp": 3.715754671025638 }, { - "x": 2.6483445167657806, - "y": 6.879644870779064, - "heading": 0.5125504196000689, - "angularVelocity": 1.8669291195039983e-14, - "velocityX": -4.749030905139852e-11, - "velocityY": 6.098431731550748e-11, - "timestamp": 5.769103609616802 + "x": 8.222455939995415, + "y": 2.399428067130867, + "heading": 0.5725898810376202, + "angularVelocity": -0.13391430893917436, + "velocityX": 0.9355208337555891, + "velocityY": 0.5565594770191685, + "timestamp": 3.7792080961564287 }, { - "x": 2.6747339306487334, - "y": 6.883481123686636, - "heading": 0.5098224624623802, - "angularVelocity": -0.03296702980330207, - "velocityX": 0.3189128531205414, - "velocityY": 0.046360649216292196, - "timestamp": 5.851851650734406 + "x": 8.266119462815807, + "y": 2.4270468912932266, + "heading": 0.5572661381126698, + "angularVelocity": -0.24149591441856846, + "velocityX": 0.6881192422693254, + "velocityY": 0.43526136068197446, + "timestamp": 3.8426615212872193 }, { - "x": 2.7264961186821273, - "y": 6.89099146776212, - "heading": 0.5039121473834008, - "angularVelocity": -0.07142543798202369, - "velocityX": 0.6255397388782451, - "velocityY": 0.09076159355636307, - "timestamp": 5.93459969185201 + "x": 8.294168308263885, + "y": 2.4467872814650136, + "heading": 0.5353660980042331, + "angularVelocity": -0.3451356654632318, + "velocityX": 0.4420383200790668, + "velocityY": 0.3111004666981787, + "timestamp": 3.90611494641801 }, { - "x": 2.8018050306182753, - "y": 6.9018959167453104, - "heading": 0.4949465844489951, - "angularVelocity": -0.10834773625231511, - "velocityX": 0.9100990297657242, - "velocityY": 0.13177893803804336, - "timestamp": 6.017347732969614 + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.44548570157766576, + "velocityX": 0.19708463824313602, + "velocityY": 0.18456741878341876, + "timestamp": 3.9695683715488004 }, { - "x": 2.899101313212433, - "y": 6.915952840560763, - "heading": 0.4831136364117761, - "angularVelocity": -0.14299973603485935, - "velocityX": 1.1758137265856894, - "velocityY": 0.16987621248329202, - "timestamp": 6.100095774087218 + "x": 8.303024530407173, + "y": 2.461881127582199, + "heading": 0.47119103232326837, + "angularVelocity": -0.5465136547157349, + "velocityX": -0.05554517814981655, + "velocityY": 0.05148048067312068, + "timestamp": 4.035271161880262 }, { - "x": 3.017081432389269, - "y": 6.932956858771751, - "heading": 0.4685947114251132, - "angularVelocity": -0.1754594403754914, - "velocityX": 1.42577537284728, - "velocityY": 0.20549148936151598, - "timestamp": 6.182843815204822 + "x": 8.282750419132642, + "y": 2.456558519757899, + "heading": 0.4288756816904664, + "angularVelocity": -0.6440419108431604, + "velocityX": -0.3085730632177365, + "velocityY": -0.08101037714605593, + "timestamp": 4.1009739522117235 }, { - "x": 3.1546294358748312, - "y": 6.9527283369160715, - "heading": 0.4515640044783447, - "angularVelocity": -0.20581401948312003, - "velocityX": 1.6622508717768, - "velocityY": 0.23893590563959213, - "timestamp": 6.265591856322426 + "x": 8.245821395629305, + "y": 2.442576926285189, + "heading": 0.3804195474569323, + "angularVelocity": -0.7375049672788585, + "velocityX": -0.5620617224479442, + "velocityY": -0.21280060408660287, + "timestamp": 4.166676742543185 }, { - "x": 3.3107722570719647, - "y": 6.975106161489216, - "heading": 0.43219034104896137, - "angularVelocity": -0.2341283632545328, - "velocityX": 1.8869669793779915, - "velocityY": 0.2704332848357982, - "timestamp": 6.34833989744003 + "x": 8.19220201387014, + "y": 2.4199912479232255, + "heading": 0.32613641295591117, + "angularVelocity": -0.8261922245184762, + "velocityX": -0.8160898721144456, + "velocityY": -0.34375523852216117, + "timestamp": 4.2323795328746465 }, { - "x": 3.484600968601402, - "y": 6.999935624537231, - "heading": 0.4106444392061959, - "angularVelocity": -0.2603795999490034, - "velocityX": 2.1006988102882644, - "velocityY": 0.3000610372485155, - "timestamp": 6.431087938557634 + "x": 8.121850214183777, + "y": 2.388868090056124, + "heading": 0.2664003971864745, + "angularVelocity": -0.909185370485436, + "velocityX": -1.070758172239708, + "velocityY": -0.4736961354318384, + "timestamp": 4.298082323206108 }, { - "x": 3.675234170530874, - "y": 7.027062068947035, - "heading": 0.38710417033209454, - "angularVelocity": -0.28448128265230255, - "velocityX": 2.3037790303523367, - "velocityY": 0.32781977728352046, - "timestamp": 6.513835979675238 + "x": 8.034715295558529, + "y": 2.349289919990928, + "heading": 0.20166644793914118, + "angularVelocity": -0.9852541866298153, + "velocityX": -1.3261981444876794, + "velocityY": -0.6023818754961501, + "timestamp": 4.36378511353757 }, { - "x": 3.8817881153060045, - "y": 7.056325014551516, - "heading": 0.36176128472936936, - "angularVelocity": -0.3062656862983269, - "velocityX": 2.4961792688429623, - "velocityY": 0.3536391340419632, - "timestamp": 6.596584020792842 + "x": 7.9307349853362235, + "y": 2.301361396168203, + "heading": 0.1325025874209066, + "angularVelocity": -1.052677674255729, + "velocityX": -1.5825859099398853, + "velocityY": -0.7294747084702512, + "timestamp": 4.429487903869031 }, { - "x": 4.1033458782194, - "y": 7.087551279488626, - "heading": 0.3348309137177563, - "angularVelocity": -0.32545025414364576, - "velocityX": 2.677498583906162, - "velocityY": 0.3773656090873216, - "timestamp": 6.679332061910446 + "x": 7.809831081747403, + "y": 2.2452194829459002, + "heading": 0.0596429970770519, + "angularVelocity": -1.1089268808263426, + "velocityX": -1.8401639105261376, + "velocityY": -0.8544829365552749, + "timestamp": 4.495190694200493 }, { - "x": 4.338922324182521, - "y": 7.120545790816314, - "heading": 0.30656596895157695, - "angularVelocity": -0.34157841544560996, - "velocityX": 2.846912661392294, - "velocityY": 0.39873465138360303, - "timestamp": 6.76208010302805 + "x": 7.671902780233127, + "y": 2.1810506699963903, + "heading": -0.015920256793080897, + "angularVelocity": -1.1500767850029918, + "velocityX": -2.099276161917122, + "velocityY": -0.9766527818040505, + "timestamp": 4.560893484531954 }, { - "x": 4.587417768628414, - "y": 7.155077045432509, - "heading": 0.27728164973070885, - "angularVelocity": -0.3538974315929522, - "velocityX": 3.00303718480442, - "velocityY": 0.4173060068834074, - "timestamp": 6.844828144145654 + "x": 7.516816185598606, + "y": 2.109122861945235, + "heading": -0.09276870443286889, + "angularVelocity": -1.1696375032490751, + "velocityX": -2.3604263053689314, + "velocityY": -1.0947451042534069, + "timestamp": 4.626596274863416 }, { - "x": 4.847543704393879, - "y": 7.190849002148663, - "heading": 0.2474070078170501, - "angularVelocity": -0.36103140944690143, - "velocityX": 3.1435902560613274, - "velocityY": 0.4322997406709641, - "timestamp": 6.927576185263258 + "x": 7.344387868807497, + "y": 2.02985165560778, + "heading": -0.16871929267657412, + "angularVelocity": -1.1559720349858067, + "velocityX": -2.6243682486121434, + "velocityY": -1.206512020837212, + "timestamp": 4.692299065194877 }, { - "x": 5.117670126803491, - "y": 7.227429623630717, - "heading": 0.21762399464747098, - "angularVelocity": -0.35992408723308145, - "velocityX": 3.264444919314729, - "velocityY": 0.4420723558891399, - "timestamp": 7.010324226380862 + "x": 7.154363408340053, + "y": 1.9439629823466762, + "heading": -0.2400485240661188, + "angularVelocity": -1.0856347352935642, + "velocityX": -2.8921825010596516, + "velocityY": -1.3072302230667405, + "timestamp": 4.758001855526339 }, { - "x": 5.395335267694769, - "y": 7.263944271068559, - "heading": 0.1895241870207491, - "angularVelocity": -0.33958275322536735, - "velocityX": 3.3555494141142153, - "velocityY": 0.44127506759904395, - "timestamp": 7.093072267498466 + "x": 6.9464441838510815, + "y": 1.8530162365161489, + "heading": -0.2992772419214471, + "angularVelocity": -0.9014642689682996, + "velocityX": -3.1645417712101533, + "velocityY": -1.3842143594162994, + "timestamp": 4.8237046458578 }, { - "x": 5.667910261992729, - "y": 7.2967916361655485, - "heading": 0.1669091076503975, - "angularVelocity": -0.27330048016738406, - "velocityX": 3.294035612402776, - "velocityY": 0.3969564071048189, - "timestamp": 7.17582030861607 + "x": 6.72212326982476, + "y": 1.7626751908782685, + "heading": -0.31855321839260475, + "angularVelocity": -0.2933813978662556, + "velocityX": -3.4141763674670904, + "velocityY": -1.3749955699312393, + "timestamp": 4.889407436189262 }, { - "x": 5.931901746285159, - "y": 7.327414779180602, - "heading": 0.14778373205342693, - "angularVelocity": -0.23112783503586568, - "velocityX": 3.1903049392702143, - "velocityY": 0.3700769541063461, - "timestamp": 7.258568349733674 + "x": 6.485759745047901, + "y": 1.6860130581467425, + "heading": -0.3185532921212082, + "angularVelocity": -0.0000011221533066716862, + "velocityX": -3.597465550312819, + "velocityY": -1.1668017803319484, + "timestamp": 4.955110226520723 }, { - "x": 6.1801220482371395, - "y": 7.354810947360831, - "heading": 0.11632647881372146, - "angularVelocity": -0.38015707459464126, - "velocityX": 2.999712121271843, - "velocityY": 0.33107935620263895, - "timestamp": 7.341316390851278 + "x": 6.24426876944993, + "y": 1.6274732096370486, + "heading": -0.3185533177664736, + "angularVelocity": -3.9032231775904613e-7, + "velocityX": -3.675505627381763, + "velocityY": -0.8909796405048926, + "timestamp": 5.020813016852185 }, { - "x": 6.415697256681246, - "y": 7.379780463537999, - "heading": 0.08899977607154176, - "angularVelocity": -0.3302398748429845, - "velocityX": 2.84689770612573, - "velocityY": 0.3017535622589267, - "timestamp": 7.424064431968882 + "x": 5.999037658914315, + "y": 1.5873919634602989, + "heading": -0.3185533462426243, + "angularVelocity": -4.334085446884588e-7, + "velocityX": -3.732430682143934, + "velocityY": -0.6100387209515062, + "timestamp": 5.0865158071836465 }, { - "x": 6.637987945996846, - "y": 7.402299511961184, - "heading": 0.06474721752358573, - "angularVelocity": -0.2930892166194916, - "velocityX": 2.686355910222349, - "velocityY": 0.2721399578655483, - "timestamp": 7.5068124730864865 + "x": 5.751475205523387, + "y": 1.5659995878977724, + "heading": -0.3185533789943251, + "angularVelocity": -4.984826463431063e-7, + "velocityX": -3.7679138456983283, + "velocityY": -0.325593105781428, + "timestamp": 5.152218597515108 }, { - "x": 6.845920803268571, - "y": 7.422253782487123, - "heading": 0.04362794173118966, - "angularVelocity": -0.2552238760840361, - "velocityX": 2.5128432584428544, - "velocityY": 0.24114492931132497, - "timestamp": 7.5895605142040905 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.3185534180019772, + "angularVelocity": -5.936985611348973e-7, + "velocityX": -3.781751231726404, + "velocityY": -0.039276923728911305, + "timestamp": 5.21792138784657 }, { - "x": 7.038548371158306, - "y": 7.439534256583214, - "heading": 0.025735796286898687, - "angularVelocity": -0.2162243988212622, - "velocityX": 2.3278807001118484, - "velocityY": 0.2088324250667052, - "timestamp": 7.6723085553216945 - }, - { - "x": 7.2149794023417675, - "y": 7.454031322663086, - "heading": 0.011159229205506762, - "angularVelocity": -0.17615603807074215, - "velocityX": 2.1321475264013814, - "velocityY": 0.1751952781490545, - "timestamp": 7.755056596439299 + "x": 5.26265149067387, + "y": 1.5786031053003822, + "heading": -0.31855345112044986, + "angularVelocity": -5.20084412877405e-7, + "velocityX": -3.7744308273915435, + "velocityY": 0.23844772894968475, + "timestamp": 5.28160042121006 }, { - "x": 7.374384880065902, - "y": 7.465639114379918, - "heading": -1.1165566205788637e-16, - "angularVelocity": -0.134857925997875, - "velocityX": 1.9263957861864134, - "velocityY": 0.14027874932231896, - "timestamp": 7.837804637556903 + "x": 5.0240625547753925, + "y": 1.61139052009726, + "heading": -0.3185534786660252, + "angularVelocity": -4.3256899255575315e-7, + "velocityX": -3.7467424251962935, + "velocityY": 0.5148855606793222, + "timestamp": 5.34527945457355 }, { - "x": 7.500924464216497, - "y": 7.473646968954779, - "heading": -0.0070937662662868915, - "angularVelocity": -0.09816609550664941, - "velocityX": 1.7511003938959169, - "velocityY": 0.11081557912801578, - "timestamp": 7.9100675327008405 + "x": 4.788524290519247, + "y": 1.661604289022676, + "heading": -0.31855350247111613, + "angularVelocity": -3.7382933947955595e-7, + "velocityX": -3.698835422197071, + "velocityY": 0.7885447732660761, + "timestamp": 5.40895848793704 }, { - "x": 7.613899532464792, - "y": 7.47952275118133, - "heading": -0.01171150501248939, - "angularVelocity": -0.06390193386252535, - "velocityX": 1.5633897316635108, - "velocityY": 0.08131119317636289, - "timestamp": 7.982330427844778 + "x": 4.55730773239592, + "y": 1.7289734284079028, + "heading": -0.3185535237219762, + "angularVelocity": -3.337183198598428e-7, + "velocityX": -3.6309684037367047, + "velocityY": 1.0579485244487485, + "timestamp": 5.4726375213005305 }, { - "x": 7.712046212988902, - "y": 7.483209835153203, - "heading": -0.014057088311073107, - "angularVelocity": -0.032459027470621904, - "velocityX": 1.3581891554251948, - "velocityY": 0.05102319751389575, - "timestamp": 8.054593322988717 + "x": 4.33166058516164, + "y": 1.8131343663413284, + "heading": -0.3185535432404584, + "angularVelocity": -3.0651348259884106e-7, + "velocityX": -3.543507734268667, + "velocityY": 1.321642831055895, + "timestamp": 5.536316554664021 }, { - "x": 7.79365194754084, - "y": 7.484595849351514, - "heading": -0.014345050898318633, - "angularVelocity": -0.003984930117620908, - "velocityX": 1.129289580626482, - "velocityY": 0.019180164253482204, - "timestamp": 8.126856218132655 + "x": 4.112800476187319, + "y": 1.9136328827222209, + "heading": -0.318553561635836, + "angularVelocity": -2.888765193967595e-7, + "velocityX": -3.436925741712075, + "velocityY": 1.5782041760469432, + "timestamp": 5.599995588027511 }, { - "x": 7.856538953365895, - "y": 7.483519387813377, - "heading": -0.012796446625875068, - "angularVelocity": 0.02143014432730538, - "velocityX": 0.8702530627897808, - "velocityY": -0.0148964629220964, - "timestamp": 8.199119113276593 + "x": 3.9019083093617053, + "y": 2.0299264483320134, + "heading": -0.31855357939397577, + "angularVelocity": -2.7886949297236075e-7, + "velocityX": -3.311799122668965, + "velocityY": 1.826245774585971, + "timestamp": 5.663674621391001 }, { - "x": 7.899249386445817, - "y": 7.479924646329156, - "heading": -0.009762276595150929, - "angularVelocity": 0.04198793896480952, - "velocityX": 0.5910423737502203, - "velocityY": -0.04974532887286408, - "timestamp": 8.271382008420531 + "x": 3.7001203074329077, + "y": 2.161384813738153, + "heading": -0.3185535969362317, + "angularVelocity": -2.75479306959021e-7, + "velocityX": -3.1688295388681516, + "velocityY": 2.0643900898393674, + "timestamp": 5.727353654754491 }, { - "x": 7.921731862807926, - "y": 7.4739282664160145, - "heading": -0.005439516570392937, - "angularVelocity": 0.0598199119499377, - "velocityX": 0.31112061476836195, - "velocityY": -0.08298006744976984, - "timestamp": 8.343644903564469 + "x": 3.509285404964888, + "y": 2.2879634464070207, + "heading": -0.3579155843505842, + "angularVelocity": -0.6181310446354924, + "velocityX": -2.996824737880426, + "velocityY": 1.9877599577609337, + "timestamp": 5.791032688117982 }, { - "x": 7.923961639404313, - "y": 7.465639114379864, - "heading": -8.923477228591696e-17, - "angularVelocity": 0.07527399171536123, - "velocityX": 0.030856452567537763, - "velocityY": -0.11470827482980998, - "timestamp": 8.415907798708407 + "x": 3.332909755538667, + "y": 2.4050971261724468, + "heading": -0.4129449232759771, + "angularVelocity": -0.8641673093760197, + "velocityX": -2.769760156682042, + "velocityY": 1.8394387222684185, + "timestamp": 5.854711721481472 }, { - "x": 7.888861809774048, - "y": 7.450123409205229, - "heading": 0.009559512590155543, - "angularVelocity": 0.09629823646465466, - "velocityX": -0.3535799196587898, - "velocityY": -0.15629824551531185, - "timestamp": 8.515177660142104 + "x": 3.171287726021972, + "y": 2.512479984884094, + "heading": -0.4699204658498491, + "angularVelocity": -0.8947300165290918, + "velocityX": -2.5380729100288124, + "velocityY": 1.6863142079856834, + "timestamp": 5.918390754844962 }, { - "x": 7.817152523236329, - "y": 7.430666435185926, - "heading": 0.02163779335227315, - "angularVelocity": 0.12167117579976462, - "velocityX": -0.72236714650414, - "velocityY": -0.19600081775371833, - "timestamp": 8.614447521575801 + "x": 3.024408806786741, + "y": 2.6100923699912792, + "heading": -0.5252799185719804, + "angularVelocity": -0.8693513358805374, + "velocityX": -2.306550704009953, + "velocityY": 1.5328810748429236, + "timestamp": 5.982069788208452 }, { - "x": 7.711448594725456, - "y": 7.4075722780062225, - "heading": 0.036035719670363044, - "angularVelocity": 0.14503824333134957, - "velocityX": -1.064813902067085, - "velocityY": -0.23264016737978235, - "timestamp": 8.713717383009499 + "x": 2.892255079193046, + "y": 2.6979336450739244, + "heading": -0.5773440254447586, + "angularVelocity": -0.8176020288434336, + "velocityX": -2.0753098879397367, + "velocityY": 1.3794379475145813, + "timestamp": 6.045748821571943 }, { - "x": 7.574048351882708, - "y": 7.3811128444264495, - "heading": 0.05252610728763101, - "angularVelocity": 0.16611675869298972, - "velocityX": -1.3841083371967766, - "velocityY": -0.2665404504210516, - "timestamp": 8.812987244443196 + "x": 2.7748123815012584, + "y": 2.7760062309054065, + "heading": -0.625128837055519, + "angularVelocity": -0.7504010203483636, + "velocityX": -1.8442914643726742, + "velocityY": 1.2260328354205916, + "timestamp": 6.109427854935433 }, { - "x": 7.407019371926434, - "y": 7.351538272778396, - "heading": 0.07089974064841498, - "angularVelocity": 0.18508773050978647, - "velocityX": -1.682574928019174, - "velocityY": -0.2979209522500118, - "timestamp": 8.912257105876893 + "x": 2.6720699818921556, + "y": 2.844312916696855, + "heading": -0.6679860381773631, + "angularVelocity": -0.6730190277419643, + "velocityX": -1.6134415706757543, + "velocityY": 1.0726715244175122, + "timestamp": 6.173106888298923 }, { - "x": 7.212241390572204, - "y": 7.319083353098022, - "heading": 0.09094586321763254, - "angularVelocity": 0.20193563564714437, - "velocityX": -1.96210590546985, - "velocityY": -0.3269362847056093, - "timestamp": 9.01152696731059 + "x": 2.5840196243973645, + "y": 2.9028562717726505, + "heading": -0.7054558016839698, + "angularVelocity": -0.5884160221579265, + "velocityX": -1.382721326691407, + "velocityY": 0.9193505614575078, + "timestamp": 6.236785921662413 }, { - "x": 6.990210935369373, - "y": 7.283848196853213, - "heading": 0.11243058039021528, - "angularVelocity": 0.216427391579795, - "velocityX": -2.236635087388812, - "velocityY": -0.35494313919580667, - "timestamp": 9.110796828744288 + "x": 2.510654811151598, + "y": 2.95163853061359, + "heading": -0.7371951288142529, + "angularVelocity": -0.4984266477336408, + "velocityX": -1.1521031235978156, + "velocityY": 0.7660646882386306, + "timestamp": 6.3004649550259035 }, { - "x": 6.744250267253016, - "y": 7.2462331722899656, - "heading": 0.1352094476478159, - "angularVelocity": 0.2294640783075409, - "velocityX": -2.4776973047417434, - "velocityY": -0.3789168637892277, - "timestamp": 9.210066690177985 + "x": 2.4519703126340358, + "y": 2.9906615990271557, + "heading": -0.7629386573341134, + "angularVelocity": -0.4042700895428552, + "velocityX": -0.9215670436230066, + "velocityY": 0.6128087433553882, + "timestamp": 6.364143988389394 }, { - "x": 6.474537469267562, - "y": 7.206343597573894, - "heading": 0.1591116816121769, - "angularVelocity": 0.24078036998495714, - "velocityX": -2.7169655934857664, - "velocityY": -0.401829660482739, - "timestamp": 9.309336551611683 + "x": 2.4079618327799786, + "y": 3.0199270886206313, + "heading": -0.7824753669479414, + "angularVelocity": -0.3067997201262361, + "velocityX": -0.6910984279998401, + "velocityY": 0.4595781067596215, + "timestamp": 6.427823021752884 }, { - "x": 6.185093329086759, - "y": 7.164701733979632, - "heading": 0.18384126922195682, - "angularVelocity": 0.24911475902781344, - "velocityX": -2.915730273020737, - "velocityY": -0.4194814316536095, - "timestamp": 9.40860641304538 + "x": 2.378625774904473, + "y": 3.039436353320747, + "heading": -0.795633833561549, + "angularVelocity": -0.20663734856804464, + "velocityX": -0.4606862938394533, + "velocityY": 0.3063687318988399, + "timestamp": 6.491502055116374 }, { - "x": 5.8780260788239564, - "y": 7.121745615538567, - "heading": 0.2121770427035368, - "angularVelocity": 0.28544185589002363, - "velocityX": -3.0932575690950848, - "velocityY": -0.43272064472210836, - "timestamp": 9.507876274479077 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.10425061542851642, + "velocityX": -0.2303222914890624, + "velocityY": 0.15317707264507022, + "timestamp": 6.5551810884798645 }, { - "x": 5.557966335758099, - "y": 7.0784922097584015, - "heading": 0.24028820463072503, - "angularVelocity": 0.283179219968629, - "velocityX": -3.224138106404308, - "velocityY": -0.4357153838585266, - "timestamp": 9.607146135912775 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.4092752497411054e-30, + "velocityX": 2.9428006004895336e-31, + "velocityY": 5.314710200495422e-31, + "timestamp": 6.618860121843355 }, { - "x": 5.245908943265439, - "y": 7.038535334063528, - "heading": 0.26569993334648234, - "angularVelocity": 0.25598634216619676, - "velocityX": -3.1435260207458486, - "velocityY": -0.40250762031696863, - "timestamp": 9.706415997346472 + "x": 2.3794842314620843, + "y": 3.041727640405621, + "heading": -0.7962432666233688, + "angularVelocity": 0.0957711014436812, + "velocityX": 0.24661230413774035, + "velocityY": -0.11854554422657079, + "timestamp": 6.681813823589832 }, { - "x": 4.948601311707987, - "y": 7.0011016986080925, - "heading": 0.2891526852975207, - "angularVelocity": 0.23625249005411966, - "velocityX": -2.9949435534976283, - "velocityY": -0.37708963138260865, - "timestamp": 9.80568585878017 + "x": 2.410536447869941, + "y": 3.0268006675090624, + "heading": -0.7842917338243374, + "angularVelocity": 0.18984638658997072, + "velocityX": 0.4932548134009415, + "velocityY": -0.23711032842312177, + "timestamp": 6.744767525336309 }, { - "x": 4.669544920179785, - "y": 6.966326786089845, - "heading": 0.31063199175620987, - "angularVelocity": 0.21637288647809105, - "velocityX": -2.811088758440401, - "velocityY": -0.35030685059909655, - "timestamp": 9.904955720213866 + "x": 2.4571179241280796, + "y": 3.0044081651489933, + "heading": -0.7665449064601504, + "angularVelocity": 0.28190284084732253, + "velocityX": 0.7399322830248891, + "velocityY": -0.3556979452971155, + "timestamp": 6.807721227082786 }, { - "x": 4.411446504016625, - "y": 6.934401644375056, - "heading": 0.3300269329519647, - "angularVelocity": 0.19537592694947853, - "velocityX": -2.599967527259491, - "velocityY": -0.3215995394142002, - "timestamp": 10.004225581647564 + "x": 2.5192312369840457, + "y": 2.9745484345249116, + "heading": -0.743155503226628, + "angularVelocity": 0.37153340605314533, + "velocityX": 0.9866506834833091, + "velocityY": -0.4743125470894598, + "timestamp": 6.8706749288292635 }, { - "x": 4.176717541970206, - "y": 6.9055345106659285, - "heading": 0.3471896751796373, - "angularVelocity": 0.17288975707028345, - "velocityX": -2.364554142177353, - "velocityY": -0.2907945401778102, - "timestamp": 10.103495443081261 + "x": 2.596879439737626, + "y": 2.937219467270397, + "heading": -0.714309410311632, + "angularVelocity": 0.4582112268975565, + "velocityX": 1.233417584660547, + "velocityY": -0.5929590511586358, + "timestamp": 6.933628630575741 }, { - "x": 3.9677270788540704, - "y": 6.879953869869358, - "heading": 0.3619526945831296, - "angularVelocity": 0.14871602710307644, - "velocityX": -2.1052760636290624, - "velocityY": -0.2576878866065069, - "timestamp": 10.202765304514958 + "x": 2.690066196626848, + "y": 2.892418878795792, + "heading": -0.6802368283616581, + "angularVelocity": 0.5412323819683986, + "velocityX": 1.4802426911208155, + "velocityY": -0.7116434336939077, + "timestamp": 6.996582332322218 }, { - "x": 3.7869910923072463, - "y": 6.857920125982094, - "heading": 0.37415675707125545, - "angularVelocity": 0.1229382444164791, - "velocityX": -1.8206531563211636, - "velocityY": -0.22195804012459744, - "timestamp": 10.302035165948656 + "x": 2.798795963694134, + "y": 2.8401438158547943, + "heading": -0.6412294413317386, + "angularVelocity": 0.6196202280051365, + "velocityX": 1.7271385804309867, + "velocityY": -0.8303731391605256, + "timestamp": 7.059536034068695 }, { - "x": 3.6375222511124736, - "y": 6.83976261955988, - "heading": 0.3836657545916857, - "angularVelocity": 0.09578937033957077, - "velocityX": -1.5056819767458316, - "velocityY": -0.18291056479757375, - "timestamp": 10.401305027382353 + "x": 2.9230742308720847, + "y": 2.7803908299690017, + "heading": -0.5976682545562845, + "angularVelocity": 0.6919559226378926, + "velocityX": 1.9741216756154527, + "velocityY": -0.9491576226355342, + "timestamp": 7.122489735815172 }, { - "x": 3.5230866973435258, - "y": 6.825906628204701, - "heading": 0.39040681994356247, - "angularVelocity": 0.06790646480733925, - "velocityX": -1.1527723733691484, - "velocityY": -0.1395790339088257, - "timestamp": 10.50057488881605 + "x": 3.062907829622796, + "y": 2.713155712528932, + "heading": -0.5500718966234338, + "angularVelocity": 0.7560533632244142, + "velocityX": 2.22121328645359, + "velocityY": -1.0680089585650514, + "timestamp": 7.185443437561649 }, { - "x": 3.4467104911360567, - "y": 6.816686862116541, - "heading": 0.39477159352312596, - "angularVelocity": 0.04396876873328538, - "velocityX": -0.7693795992500979, - "velocityY": -0.09287578279051523, - "timestamp": 10.599844750249748 + "x": 3.218305230273769, + "y": 2.638433320090989, + "heading": -0.4991885737635272, + "angularVelocity": 0.8082657802208455, + "velocityX": 2.4684394458133507, + "velocityY": -1.1869419964986359, + "timestamp": 7.248397139308127 }, { - "x": 3.4085204601287793, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": 0.021852252491790684, - "velocityX": -0.3847092204594867, - "velocityY": -0.046321940190690435, - "timestamp": 10.699114611683445 + "x": 3.389276272924326, + "y": 2.556217612467452, + "heading": -0.4461960441931904, + "angularVelocity": 0.8417698737358584, + "velocityX": 2.715821912094713, + "velocityY": -1.3059709809381974, + "timestamp": 7.311350841054604 }, { - "x": 3.408520460128781, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": -9.258859562419926e-18, - "velocityX": 7.880837628913899e-16, - "velocityY": -2.4194099006901583e-16, - "timestamp": 10.798384473117142 + "x": 3.575827436112715, + "y": 2.4665034787202997, + "heading": -0.3932374103043789, + "angularVelocity": 0.841231451362194, + "velocityX": 2.9633072879440143, + "velocityY": -1.4250811510408536, + "timestamp": 7.374304542801081 }, { - "x": 3.4451800140480633, - "y": 6.811382414928076, - "heading": 0.39378662700224637, - "angularVelocity": -0.032542531675579224, - "velocityX": 0.37821978715244237, - "velocityY": -0.00728463273656313, - "timestamp": 10.895311062271263 + "x": 3.777911234478158, + "y": 2.369308059755886, + "heading": -0.34555593776982274, + "angularVelocity": 0.7574053822375035, + "velocityX": 3.210038373585424, + "velocityY": -1.5439190431697394, + "timestamp": 7.437258244547558 }, { - "x": 3.518496947445585, - "y": 6.809970351527808, - "heading": 0.38746820532810716, - "angularVelocity": -0.06518770266528738, - "velocityX": 0.756417140408642, - "velocityY": -0.014568380179178244, - "timestamp": 10.992237651425384 + "x": 3.9926293770053456, + "y": 2.266439040708753, + "heading": -0.3455557491739834, + "angularVelocity": 0.000002995786333953476, + "velocityX": 3.4107310066036556, + "velocityY": -1.6340424183696203, + "timestamp": 7.500211946294035 }, { - "x": 3.6283397734394334, - "y": 6.8078551393922675, - "heading": 0.37750274880248313, - "angularVelocity": -0.1028144765290181, - "velocityX": 1.1332579321365746, - "velocityY": -0.021822826471036318, - "timestamp": 11.089164240579505 + "x": 4.207200483864901, + "y": 2.1632635581376745, + "heading": -0.34555573252846533, + "angularVelocity": 2.644088848166298e-7, + "velocityX": 3.4083953906898548, + "velocityY": -1.6389104962656516, + "timestamp": 7.5631656480405125 }, { - "x": 3.7707535238798844, - "y": 6.805113681812527, - "heading": 0.36197971834397535, - "angularVelocity": -0.16015244726939834, - "velocityX": 1.469294975540766, - "velocityY": -0.028283854860319504, - "timestamp": 11.186090829733626 + "x": 4.421771581956281, + "y": 2.0600880573316713, + "heading": -0.3455557158829472, + "angularVelocity": 2.6440888572481287e-7, + "velocityX": 3.408395251410094, + "velocityY": -1.638910785921765, + "timestamp": 7.62611934978699 }, { - "x": 3.9413941020263046, - "y": 6.801829282896055, - "heading": 0.341973429419134, - "angularVelocity": -0.20640661246244818, - "velocityX": 1.7605135973070385, - "velocityY": -0.03388542757086655, - "timestamp": 11.283017418887747 + "x": 4.636342680047139, + "y": 1.9569125565245837, + "heading": -0.3455556992374292, + "angularVelocity": 2.6440888404451857e-7, + "velocityX": 3.4083952514018088, + "velocityY": -1.6389107859389946, + "timestamp": 7.689073051533467 }, { - "x": 4.137139185749371, - "y": 6.798061838763412, - "heading": 0.3182906699990001, - "angularVelocity": -0.24433707640817395, - "velocityX": 2.0195189517276724, - "velocityY": -0.038869046827304486, - "timestamp": 11.379944008041868 + "x": 4.850913778140846, + "y": 1.853737055723424, + "heading": -0.3455556825919111, + "angularVelocity": 2.64408884846605e-7, + "velocityX": 3.408395251447087, + "velocityY": -1.63891078584483, + "timestamp": 7.752026753279944 }, { - "x": 4.3554217491364975, - "y": 6.793860650651487, - "heading": 0.2915337558008742, - "angularVelocity": -0.2760533970258723, - "velocityX": 2.2520400778783602, - "velocityY": -0.043344020960483176, - "timestamp": 11.476870597195989 + "x": 5.065484924140348, + "y": 1.7505616545506137, + "heading": -0.34555566594639303, + "angularVelocity": 2.644088848456774e-7, + "velocityX": 3.4083960124157384, + "velocityY": -1.6389092032794423, + "timestamp": 7.814980455026421 }, { - "x": 4.593770534553052, - "y": 6.7892732393783435, - "heading": 0.26218341148270125, - "angularVelocity": -0.30281003978695553, - "velocityX": 2.459065025362269, - "velocityY": -0.04732871870534226, - "timestamp": 11.57379718635011 + "x": 5.280845302209934, + "y": 1.6490439336616942, + "heading": -0.34555564929194366, + "angularVelocity": 2.6455075541297124e-7, + "velocityX": 3.4209327187283005, + "velocityY": -1.6125774668143436, + "timestamp": 7.877934156772898 }, { - "x": 4.849438377602218, - "y": 6.784352472846038, - "heading": 0.23067476418411942, - "angularVelocity": -0.32507743822988416, - "velocityX": 2.6377472402607576, - "velocityY": -0.05076797373402456, - "timestamp": 11.67072377550423 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 2.6730250915052296e-7, + "velocityX": 3.52891551864971, + "velocityY": -1.360125725304771, + "timestamp": 7.940887858519376 }, { - "x": 5.118739979240287, - "y": 6.77916928922838, - "heading": 0.197506319622179, - "angularVelocity": -0.3422017100921637, - "velocityX": 2.7784079063161986, - "velocityY": -0.053475353490631775, - "timestamp": 11.767650364658351 + "x": 5.655413318775525, + "y": 1.5130951799644636, + "heading": -0.34555563491945995, + "angularVelocity": -5.785220393859716e-8, + "velocityX": 3.5912523855702734, + "velocityY": -1.18578710714545, + "timestamp": 7.983327014878419 }, { - "x": 5.388165163887155, - "y": 6.773984054928517, - "heading": 0.16458046580315022, - "angularVelocity": -0.33969888042458984, - "velocityX": 2.779682922902244, - "velocityY": -0.053496510556234986, - "timestamp": 11.864576953812472 + "x": 5.810103118724588, + "y": 1.4702901058122384, + "heading": -0.3455556373457815, + "angularVelocity": -5.7171766263130695e-8, + "velocityX": 3.6449782045702985, + "velocityY": -1.0086221740622332, + "timestamp": 8.025766171237462 }, { - "x": 5.643920085833601, - "y": 6.769061952936388, - "heading": 0.1333147392066983, - "angularVelocity": -0.32257120434452946, - "velocityX": 2.6386456407723093, - "velocityY": -0.05078175178849002, - "timestamp": 11.961503542966593 + "x": 5.966313456902953, + "y": 1.4334181808859694, + "heading": -0.34555563976573417, + "angularVelocity": -5.702169541040829e-8, + "velocityX": 3.680806867525754, + "velocityY": -0.8688185178405917, + "timestamp": 8.068205327596505 }, { - "x": 5.882330711464664, - "y": 6.764473684698037, - "heading": 0.10420435480854875, - "angularVelocity": -0.3003343525465651, - "velocityX": 2.4597030362017005, - "velocityY": -0.04733756008947892, - "timestamp": 12.058430132120714 + "x": 6.1225244823663525, + "y": 1.3965491678093302, + "heading": -0.3455556421856862, + "angularVelocity": -5.7021679533312126e-8, + "velocityX": 3.680823062122731, + "velocityY": -0.868749905505203, + "timestamp": 8.110644483955548 }, { - "x": 6.100655045491889, - "y": 6.760272005917349, - "heading": 0.07768097943341207, - "angularVelocity": -0.27364395679871373, - "velocityX": 2.252471029183505, - "velocityY": -0.04334908323253468, - "timestamp": 12.155356721274835 + "x": 6.278735507957822, + "y": 1.3596801552753135, + "heading": -0.3455556446056381, + "angularVelocity": -5.702167859922657e-8, + "velocityX": 3.680823065140465, + "velocityY": -0.8687498927193055, + "timestamp": 8.153083640314591 }, { - "x": 6.29642574105686, - "y": 6.756504387895065, - "heading": 0.054221932212767415, - "angularVelocity": -0.2420290183052161, - "velocityX": 2.019783191314849, - "velocityY": -0.038870840861772844, - "timestamp": 12.252283310428956 + "x": 6.434946533537413, + "y": 1.3228111426909752, + "heading": -0.34555564702559005, + "angularVelocity": -5.7021678932866206e-8, + "velocityX": 3.680823064860606, + "velocityY": -0.868749893905046, + "timestamp": 8.195522796673634 }, { - "x": 6.467079365349485, - "y": 6.7532201591852425, - "heading": 0.03442675668029697, - "angularVelocity": -0.20422853734174792, - "velocityX": 1.7606481955253344, - "velocityY": -0.03388367153413224, - "timestamp": 12.349209899583077 + "x": 6.591157495254782, + "y": 1.285941859528607, + "heading": -0.3455556494455419, + "angularVelocity": -5.7021677677252914e-8, + "velocityX": 3.6808215600657594, + "velocityY": -0.8687562695744264, + "timestamp": 8.237961953032677 }, { - "x": 6.60949406591357, - "y": 6.750479406444339, - "heading": 0.019100936295010484, - "angularVelocity": -0.15811781389436272, - "velocityX": 1.4693047780484314, - "velocityY": -0.028276582977095337, - "timestamp": 12.446136488737197 + "x": 6.747031505398479, + "y": 1.2476730535838276, + "heading": -0.34555565194208476, + "angularVelocity": -5.8826401859372655e-8, + "velocityX": 3.6728819212374164, + "velocityY": -0.9017334279932018, + "timestamp": 8.28040110939172 }, { - "x": 6.719322188320328, - "y": 6.748365796127614, - "heading": 0.009318208717894495, - "angularVelocity": -0.10092924616960162, - "velocityX": 1.1331062339573879, - "velocityY": -0.021806300367839485, - "timestamp": 12.543063077891318 + "x": 6.897830261276489, + "y": 1.2034176695315386, + "heading": -0.3468060133183351, + "angularVelocity": -0.029462446559305244, + "velocityX": 3.5532929684610997, + "velocityY": -1.0427960367044076, + "timestamp": 8.322840265750763 }, { - "x": 6.792644320795612, - "y": 6.746954731447967, - "heading": 0.0031061365662838365, - "angularVelocity": -0.06409048544701255, - "velocityX": 0.756470779743405, - "velocityY": -0.014558076292195965, - "timestamp": 12.63998966704544 + "x": 7.040966542913821, + "y": 1.160898981796106, + "heading": -0.34756195184747823, + "angularVelocity": -0.017812289262957588, + "velocityX": 3.3727409759603195, + "velocityY": -1.001874009363348, + "timestamp": 8.365279422109806 }, { - "x": 6.829305171966555, - "y": 6.746249198913574, - "heading": -6.996174209709525e-17, - "angularVelocity": -0.03204627949246174, - "velocityX": 0.3782331710099824, - "velocityY": -0.007279040153471663, - "timestamp": 12.73691625619956 + "x": 7.176448137174031, + "y": 1.12015095875213, + "heading": -0.34781665101790943, + "angularVelocity": -0.006001513514462224, + "velocityX": 3.1923724664555144, + "velocityY": -0.9601515802821338, + "timestamp": 8.407718578468849 }, { - "x": 6.8293051719665545, - "y": 6.746249198913574, - "heading": -3.495236266448189e-17, - "angularVelocity": 5.858001175456844e-19, - "velocityX": -7.463612085764823e-16, - "velocityY": 1.4437890171198592e-17, - "timestamp": 12.833842845353681 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 7.30427768133993, + "y": 1.0811849503221362, + "heading": -0.3475678387477372, + "angularVelocity": 0.005862799629361019, + "velocityX": 3.0120660996283153, + "velocityY": -0.918161711329369, + "timestamp": 8.450157734827892 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 7.424456501916377, + "y": 1.044006636667021, + "heading": -0.3468143875407301, + "angularVelocity": 0.017753680130510166, + "velocityX": 2.8317909894275823, + "velocityY": -0.8760379999211017, + "timestamp": 8.492596891186935 }, { - "scope": [ - 2 - ], - "type": "StopPoint" + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.02966022853563592, + "velocityX": 2.65153469287831, + "velocityY": -0.8338339406813241, + "timestamp": 8.535036047545978 }, { - "scope": [ - 3 - ], - "type": "StopPoint" + "x": 7.689789531297121, + "y": 0.9591642629851495, + "heading": -0.34249163023542684, + "angularVelocity": 0.04769743470803589, + "velocityX": 2.378707539440784, + "velocityY": -0.769870356769085, + "timestamp": 8.599274349827635 }, { - "scope": [ - 5 - ], - "type": "StopPoint" + "x": 7.82516177952376, + "y": 0.9142093996063804, + "heading": -0.3392101704632614, + "angularVelocity": 0.05108260423473965, + "velocityX": 2.1073447369933547, + "velocityY": -0.6998140016475143, + "timestamp": 8.663512652109292 }, { - "scope": [ - 4, - 5 - ], - "type": "StraightLine" + "x": 7.943169958712402, + "y": 0.8740090692537413, + "heading": -0.33632582750820933, + "angularVelocity": 0.044900672225200826, + "velocityX": 1.837037639494695, + "velocityY": -0.6258000122166694, + "timestamp": 8.727750954390949 }, { - "scope": [ - 8 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "SourceLanePGHSprint": { - "waypoints": [ - { - "x": 0.43324199318885803, - "y": 4.121134281158447, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 + "x": 8.043864833075729, + "y": 0.8387415563520763, + "heading": -0.3342715201633078, + "angularVelocity": 0.03197947753809357, + "velocityX": 1.567520790350633, + "velocityY": -0.5490106626266708, + "timestamp": 8.791989256672606 }, { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 + "x": 8.12728568078066, + "y": 0.8085387560486261, + "heading": -0.33336873570137193, + "angularVelocity": 0.014053678722353937, + "velocityX": 1.2986153858669556, + "velocityY": -0.4701680964578417, + "timestamp": 8.856227558954263 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 17 + "x": 8.193463737140812, + "y": 0.7835021726798679, + "heading": -0.33386568134146205, + "angularVelocity": -0.007735970946293819, + "velocityX": 1.030196222652151, + "velocityY": -0.38974540857234485, + "timestamp": 8.92046586123592 }, { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "x": 8.242424408463135, + "y": 0.7637123264900073, + "heading": -0.33595980135953535, + "angularVelocity": -0.03259924287680334, + "velocityX": 0.7621725603464993, + "velocityY": -0.3080692591016908, + "timestamp": 8.984704163517577 }, { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 + "x": 8.274188750584008, + "y": 0.7492346436854774, + "heading": -0.3398119288829893, + "angularVelocity": -0.05996620998114242, + "velocityX": 0.4944766750154696, + "velocityY": -0.2253746174836856, + "timestamp": 9.048942465799234 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 22 + "x": 8.288774490356445, + "y": 0.7401233315467834, + "heading": -0.34555563246426124, + "angularVelocity": -0.08941244362418393, + "velocityX": 0.2270567442533848, + "velocityY": -0.14183612914838375, + "timestamp": 9.113180768080891 }, { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 21 + "x": 8.286058974983396, + "y": 0.7364361738247144, + "heading": -0.3533713967772531, + "angularVelocity": -0.1208300500196775, + "velocityX": -0.04198128874091145, + "velocityY": -0.05700267231030528, + "timestamp": 9.177864713107141 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 + "x": 8.26594084231072, + "y": 0.7382366486349878, + "heading": -0.3631834148956511, + "angularVelocity": -0.15169170826572306, + "velocityX": -0.3110220420927156, + "velocityY": 0.027834956719829915, + "timestamp": 9.24254865813339 }, { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "x": 8.22841989282902, + "y": 0.7455250700569162, + "heading": -0.3749497746847549, + "angularVelocity": -0.18190541384463682, + "velocityX": -0.5800658798172126, + "velocityY": 0.11267744134917045, + "timestamp": 9.30723260315964 }, { - "x": 8.288774490356445, - "y": 0.7401233315467834, - "heading": -0.34555563246426124, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 + "x": 8.173495898954863, + "y": 0.7583018091084519, + "heading": -0.38862097650887273, + "angularVelocity": -0.21135386560868902, + "velocityX": -0.8491132359330754, + "velocityY": 0.1975256618369646, + "timestamp": 9.37191654818589 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 22 + "x": 8.101168599327213, + "y": 0.7765673113174031, + "heading": -0.4041376276128682, + "angularVelocity": -0.23988411804039605, + "velocityX": -1.11816463263485, + "velocityY": 0.2823807700896826, + "timestamp": 9.43660049321214 }, { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 + "x": 8.011437691690773, + "y": 0.8003221216608263, + "heading": -0.42142708913788907, + "angularVelocity": -0.26729138920027884, + "velocityX": -1.3872207021390834, + "velocityY": 0.36724430357151194, + "timestamp": 9.50128443823839 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 16 + "x": 7.904302824322884, + "y": 0.8295669211666158, + "heading": -0.44039841224325665, + "angularVelocity": -0.29329261067284323, + "velocityX": -1.6562822092006166, + "velocityY": 0.45211836559939883, + "timestamp": 9.56596838326464 }, { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ + "x": 7.779763586783285, + "y": 0.8643025828479174, + "heading": -0.46093432615418145, + "angularVelocity": -0.317480850968364, + "velocityX": -1.9253500615811048, + "velocityY": 0.5370059242243979, + "timestamp": 9.63065232829089 + }, { - "x": 0.433241993188858, - "y": 4.121134281158447, - "heading": -6.349582694240188e-38, - "angularVelocity": 0, - "velocityX": -2.609211774957871e-36, - "velocityY": -5.016830702674688e-35, - "timestamp": 0 + "x": 7.63781950374517, + "y": 0.9045302615405436, + "heading": -0.48287779045070706, + "angularVelocity": -0.33924127985113656, + "velocityX": -2.194425262412658, + "velocityY": 0.6219113363648586, + "timestamp": 9.695336273317139 }, { - "x": 0.45469188032137553, - "y": 4.109218526997661, - "heading": -0.008937300145995164, - "angularVelocity": -0.11893237262991438, - "velocityX": 0.2854425752342375, - "velocityY": -0.1585678997982562, - "timestamp": 0.07514606787342622 + "x": 7.478470046612931, + "y": 0.9502515474489494, + "heading": -0.5060075980261466, + "angularVelocity": -0.3575818940241346, + "velocityX": -2.4635086352196294, + "velocityY": 0.7068413327271726, + "timestamp": 9.760020218343389 }, { - "x": 0.4975916635116251, - "y": 4.0853873108981436, - "heading": -0.026812413296636577, - "angularVelocity": -0.2378715700833438, - "velocityX": 0.570885269240018, - "velocityY": -0.31713191087600345, - "timestamp": 0.15029213574685243 + "x": 7.301714722310279, + "y": 1.0014687500113455, + "heading": -0.5299890975728885, + "angularVelocity": -0.37074887032647375, + "velocityX": -2.732599630880914, + "velocityY": 0.7918070325118697, + "timestamp": 9.824704163369638 }, { - "x": 0.5619414537480725, - "y": 4.049641027140498, - "heading": -0.05362171709802473, - "angularVelocity": -0.35676256336585627, - "velocityX": 0.856329440215504, - "velocityY": -0.4756906750976657, - "timestamp": 0.22543820362027867 + "x": 7.107553521342319, + "y": 1.0581854755780744, + "heading": -0.5542575516052335, + "angularVelocity": -0.37518512549746846, + "velocityX": -3.0016907733312475, + "velocityY": 0.8768284857040194, + "timestamp": 9.889388108395888 }, { - "x": 0.6477414523773106, - "y": 4.001980177860296, - "heading": -0.08935853104911506, - "angularVelocity": -0.4755646564406316, - "velocityX": 1.141776290593902, - "velocityY": -0.6342427571923069, - "timestamp": 0.30058427149370487 + "x": 6.895989597473325, + "y": 1.1204078946209601, + "heading": -0.5776591781660143, + "angularVelocity": -0.3617841575878507, + "velocityX": -3.2707331592582385, + "velocityY": 0.961945333075069, + "timestamp": 9.954072053422138 }, { - "x": 0.7549919358777414, - "y": 3.942405375853288, - "heading": -0.13401442200653152, - "angularVelocity": -0.5942545261667384, - "velocityX": 1.4272268201854585, - "velocityY": -0.7927866845588475, - "timestamp": 0.37573033936713107 + "x": 6.667061067423103, + "y": 1.188145012147935, + "heading": -0.5965312726664429, + "angularVelocity": -0.2917585575952417, + "velocityX": -3.5391862688232427, + "velocityY": 1.0472013959489617, + "timestamp": 10.018755998448388 }, { - "x": 0.8836932393269182, - "y": 3.8709173416225418, - "heading": -0.18758057158075755, - "angularVelocity": -0.7128270459134503, - "velocityX": 1.7126818087934743, - "velocityY": -0.9513210238912078, - "timestamp": 0.4508764072405573 + "x": 6.433742027972299, + "y": 1.2616769248691375, + "heading": -0.5965312781733252, + "angularVelocity": -8.513522502240808e-8, + "velocityX": -3.607062608134345, + "velocityY": 1.136787694247182, + "timestamp": 10.083439943474637 }, { - "x": 1.0338457421402325, - "y": 3.7875168932975933, - "heading": -0.2500489995023069, - "angularVelocity": -0.8312933688928248, - "velocityX": 1.998141846440012, - "velocityY": -1.109844476033343, - "timestamp": 0.5260224751139835 + "x": 6.200423078644191, + "y": 1.3352091235520722, + "heading": -0.5965312836800714, + "angularVelocity": -8.513312137357674e-8, + "velocityX": -3.6070612148566714, + "velocityY": 1.1367921151546025, + "timestamp": 10.148123888500887 }, { - "x": 1.2054498603190866, - "y": 3.692204931169301, - "heading": -0.32141344961465557, - "angularVelocity": -0.9496764385936038, - "velocityX": 2.283607419990343, - "velocityY": -1.2683559476303248, - "timestamp": 0.6011685429874097 + "x": 5.967104129330135, + "y": 1.4087413222795935, + "heading": -0.5965312891868175, + "angularVelocity": -8.513312220445073e-8, + "velocityX": -3.607061214639434, + "velocityY": 1.136792115843899, + "timestamp": 10.212807833527137 }, { - "x": 1.3985060453231253, - "y": 3.5849824249792595, - "heading": -0.40166985798593763, - "angularVelocity": -1.0680054278616893, - "velocityX": 2.5690790013020615, - "velocityY": -1.4268545144723213, - "timestamp": 0.676314610860836 + "x": 5.733785345709214, + "y": 1.4822740467522528, + "heading": -0.5965312946935636, + "angularVelocity": -8.513311865132804e-8, + "velocityX": -3.6070586530588855, + "velocityY": 1.1368002437516491, + "timestamp": 10.277491778553387 }, { - "x": 1.5915937569821006, - "y": 3.4778198154599647, - "heading": -0.48176859267038563, - "angularVelocity": -1.065907198489266, - "velocityX": 2.5694985396202825, - "velocityY": -1.4260574445464824, - "timestamp": 0.7514606787342623 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.5965313002103377, + "angularVelocity": -8.528815258110653e-8, + "velocityX": -3.567836630187545, + "velocityY": 1.254483436778072, + "timestamp": 10.342175723579636 }, { - "x": 1.7632284858900336, - "y": 3.382566036520044, - "heading": -0.5529765476804537, - "angularVelocity": -0.9475938931363465, - "velocityX": 2.2840147697019786, - "velocityY": -1.2675816797275796, - "timestamp": 0.8266067466076885 + "x": 5.285431093647359, + "y": 1.6581112689784743, + "heading": -0.5965313049795081, + "angularVelocity": -7.601302145092289e-8, + "velocityX": -3.4677610362467894, + "velocityY": 1.509245007513348, + "timestamp": 10.404917216242477 }, { - "x": 1.9134098098562766, - "y": 3.2992204007390877, - "heading": -0.6152918418017524, - "angularVelocity": -0.8292555536806094, - "velocityX": 1.9985253815170194, - "velocityY": -1.1091150626981792, - "timestamp": 0.9017528144811148 + "x": 5.070948144949037, + "y": 1.7596075143791599, + "heading": -0.5965313097283005, + "angularVelocity": -7.568822719486902e-8, + "velocityX": -3.4185184252932688, + "velocityY": 1.6176893646140194, + "timestamp": 10.467658708905319 }, { - "x": 2.0421373577646693, - "y": 3.2277823059512354, - "heading": -0.6687115003742364, - "angularVelocity": -0.7108776291856339, - "velocityX": 1.7130310547348626, - "velocityY": -0.9506564589404816, - "timestamp": 0.976898882354541 + "x": 4.856465419222307, + "y": 1.861104230964457, + "heading": -0.5965313144770923, + "angularVelocity": -7.568821760685447e-8, + "velocityX": -3.4185148714793154, + "velocityY": 1.6176968745502942, + "timestamp": 10.53040020156816 }, { - "x": 2.149410808485792, - "y": 3.1682512394298143, - "heading": -0.713232041811307, - "angularVelocity": -0.5924533737687984, - "velocityX": 1.4275324545498613, - "velocityY": -0.792204678250007, - "timestamp": 1.0520449502279672 + "x": 4.641982693508075, + "y": 1.9626009475761665, + "heading": -0.5965313192258841, + "angularVelocity": -7.568821790279648e-8, + "velocityX": -3.418514871280104, + "velocityY": 1.6176968749712675, + "timestamp": 10.593141694231 }, { - "x": 2.2352298936713018, - "y": 3.120626784362015, - "heading": -0.7488501306819736, - "angularVelocity": -0.4739847323836121, - "velocityX": 1.1420302833417786, - "velocityY": -0.6337584442610682, - "timestamp": 1.1271910181013933 + "x": 4.427499967793844, + "y": 2.064097664187878, + "heading": -0.5965313239746759, + "angularVelocity": -7.568821822441872e-8, + "velocityX": -3.4185148712800926, + "velocityY": 1.617696874971291, + "timestamp": 10.655883186893842 }, { - "x": 2.2995943987852443, - "y": 3.0849086278583613, - "heading": -0.7755632624607879, - "angularVelocity": -0.3554827622359307, - "velocityX": 0.8565252572144717, - "velocityY": -0.4753163740226096, - "timestamp": 1.2023370859748195 - }, - { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080857, - "angularVelocity": -0.23696688264902363, - "velocityX": 0.5710180813549187, - "velocityY": -0.3168769856057537, - "timestamp": 1.2774831538482456 + "x": 4.213017242079613, + "y": 2.165594380799589, + "heading": -0.5965313287234677, + "angularVelocity": -7.568821811615467e-8, + "velocityX": -3.4185148712800917, + "velocityY": 1.617696874971293, + "timestamp": 10.718624679556683 }, { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550474, - "velocityX": 0.2855094397331478, - "velocityY": -0.15843872456938407, - "timestamp": 1.3526292217216718 + "x": 3.998534516366738, + "y": 2.267091097414165, + "heading": -0.5965313334722594, + "angularVelocity": -7.56882168459371e-8, + "velocityX": -3.418514871258486, + "velocityY": 1.6176968750169503, + "timestamp": 10.781366172219524 }, { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 3.467177167133168e-32, - "velocityX": -1.439882477346126e-32, - "velocityY": -1.3815367235364851e-32, - "timestamp": 1.427775289595098 + "x": 3.7840518148370346, + "y": 2.3685878651306878, + "heading": -0.5965313382224369, + "angularVelocity": -7.571030356184493e-8, + "velocityX": -3.418514485817036, + "velocityY": 1.6176976895010247, + "timestamp": 10.844107664882365 }, { - "x": 2.3773546785315864, - "y": 3.0407954088560687, - "heading": -0.7860770412156382, - "angularVelocity": 0.2650004205849919, - "velocityX": 0.21918861143707746, - "velocityY": -0.13736692695017716, - "timestamp": 1.4888897992558612 + "x": 3.5823467745214304, + "y": 2.46403001529431, + "heading": -0.6258886394381601, + "angularVelocity": -0.46790887449049295, + "velocityX": -3.214858808022383, + "velocityY": 1.5211966772373215, + "timestamp": 10.906849157545206 }, { - "x": 2.4041715283150715, - "y": 3.0239975454455843, - "heading": -0.75411979281041, - "angularVelocity": 0.5229077118120985, - "velocityX": 0.438796775632191, - "velocityY": -0.2748588429118795, - "timestamp": 1.5500043089166244 + "x": 3.3961574491843427, + "y": 2.552130310684733, + "heading": -0.6529986278149837, + "angularVelocity": -0.43209026795882993, + "velocityX": -2.967562890759225, + "velocityY": 1.40417914288166, + "timestamp": 10.969590650208048 }, { - "x": 2.4444388170293236, - "y": 2.998788446329585, - "heading": -0.7069088657797715, - "angularVelocity": 0.772499481591177, - "velocityX": 0.6588826276733534, - "velocityY": -0.41248959135778174, - "timestamp": 1.6111188185773877 + "x": 3.225483861981861, + "y": 2.6328887858865286, + "heading": -0.6778585687680168, + "angularVelocity": -0.3962280764760425, + "velocityX": -2.7202666044246855, + "velocityY": 1.2871621597492864, + "timestamp": 11.032332142870889 }, { - "x": 2.498190798561039, - "y": 2.965157152573787, - "heading": -0.6450804787000075, - "angularVelocity": 1.0116809808826641, - "velocityX": 0.8795289666903056, - "velocityY": -0.5502996578468837, - "timestamp": 1.672233328238151 + "x": 3.0703260256030673, + "y": 2.7063054766361296, + "heading": -0.7004659993691003, + "angularVelocity": -0.36032662982009556, + "velocityX": -2.4729701158463, + "velocityY": 1.17014574619904, + "timestamp": 11.09507363553373 }, { - "x": 2.5654677170302604, - "y": 2.923088008476279, - "heading": -0.5694581806203519, - "angularVelocity": 1.237386972413308, - "velocityX": 1.1008338092322851, - "velocityY": -0.6883658943027919, - "timestamp": 1.7333478378989142 + "x": 2.9306839491479293, + "y": 2.7723804156875507, + "heading": -0.7208186227896802, + "angularVelocity": -0.32438857535555815, + "velocityX": -2.2256734822288196, + "velocityY": 1.0531298546958971, + "timestamp": 11.157815128196571 }, { - "x": 2.6463157759141644, - "y": 2.8725583813996622, - "heading": -0.4811095285927534, - "angularVelocity": 1.445624819997864, - "velocityX": 1.3228946666295571, - "velocityY": -0.8268024624119354, - "timestamp": 1.7944623475596775 + "x": 2.8065576397395007, + "y": 2.8311136320269066, + "heading": -0.7389143343573537, + "angularVelocity": -0.28841697574707026, + "velocityX": -1.9783767350811556, + "velocityY": 0.9361144251855119, + "timestamp": 11.220556620859412 }, { - "x": 2.7407862870930626, - "y": 2.813537598490058, - "heading": -0.38139476987658943, - "angularVelocity": 1.6316053138553275, - "velocityX": 1.5457951262848801, - "velocityY": -0.9657409220366661, - "timestamp": 1.8555768572204407 + "x": 2.697947103240289, + "y": 2.8825051506567756, + "heading": -0.7547512635952348, + "angularVelocity": -0.25241556370017226, + "velocityX": -1.7310798944944172, + "velocityY": 0.8190993941766099, + "timestamp": 11.283298113522253 }, { - "x": 2.84893500974016, - "y": 2.7459886359527177, - "heading": -0.27202745092011676, - "angularVelocity": 1.7895475160244771, - "velocityX": 1.7696079580350594, - "velocityY": -1.105285191884772, - "timestamp": 1.916691366881204 + "x": 2.6048523446905505, + "y": 2.9265549925489487, + "heading": -0.7683278153992709, + "angularVelocity": -0.21638872822158642, + "velocityX": -1.4837829735739574, + "velocityY": 0.702084697424841, + "timestamp": 11.346039606185094 }, { - "x": 2.970822636317052, - "y": 2.669873093439706, - "heading": -0.15523119808259794, - "angularVelocity": 1.9111051284848073, - "velocityX": 1.9944138839282306, - "velocityY": -1.245457796119401, - "timestamp": 1.9778058765419673 + "x": 2.527273368620795, + "y": 2.9632631746604594, + "heading": -0.779642706302846, + "angularVelocity": -0.18034143631837077, + "velocityX": -1.2364859804443729, + "velocityY": 0.5850702709413225, + "timestamp": 11.408781098847935 }, { - "x": 3.106512263881207, - "y": 2.5851587340080315, - "heading": -0.03419676462946541, - "angularVelocity": 1.980453318286855, - "velocityX": 2.220252249708728, - "velocityY": -1.3861578846318265, - "timestamp": 2.0389203862027303 + "x": 2.4652101792880994, + "y": 2.9926297099741825, + "heading": -0.7886949946743065, + "angularVelocity": -0.1442791362982944, + "velocityX": -0.9891889194637066, + "velocityY": 0.46805605138424683, + "timestamp": 11.471522591510777 }, { - "x": 3.256035657144846, - "y": 2.4918371927335334, - "heading": 0.08572826850512172, - "angularVelocity": 1.962300504418209, - "velocityX": 2.4466103727841246, - "velocityY": -1.5269948461095477, - "timestamp": 2.1000348958634936 + "x": 2.4186627808554446, + "y": 3.014654607545873, + "heading": -0.7954841045559687, + "angularVelocity": -0.10820765642515946, + "velocityX": -0.7418917921317392, + "velocityY": 0.3510419761615724, + "timestamp": 11.534264084173618 }, { - "x": 3.419180860242083, - "y": 2.390029027608911, - "heading": 0.19415018142326176, - "angularVelocity": 1.7740780956923756, - "velocityX": 2.6695003200194134, - "velocityY": -1.6658591501387068, - "timestamp": 2.161149405524257 + "x": 2.3876311775224326, + "y": 3.0293378725476474, + "heading": -0.8000098431420335, + "angularVelocity": -0.07213310353301693, + "velocityX": -0.49459459786475296, + "velocityY": 0.23402798337424993, + "timestamp": 11.597005576836459 }, { - "x": 3.5929644641848295, - "y": 2.279455345020229, - "heading": 0.26705766200125053, - "angularVelocity": 1.1929651564364412, - "velocityX": 2.8435735622750062, - "velocityY": -1.8092869140644157, - "timestamp": 2.22226391518502 + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": -0.036061762982130895, + "velocityX": -0.24729733470577522, + "velocityY": 0.11701401166345779, + "timestamp": 11.6597470694993 }, { - "x": 3.7768655464749616, - "y": 2.1600020280366277, - "heading": 0.30152237044830693, - "angularVelocity": 0.5639365944088304, - "velocityX": 3.0091230922237155, - "velocityY": -1.9545819421061743, - "timestamp": 2.2833784248457833 + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": 2.8180807610025885e-36, + "velocityX": -1.7033240054088048e-37, + "velocityY": 8.364918339692578e-38, + "timestamp": 11.722488562162141 }, { - "x": 3.9728967066647076, - "y": 2.0375522576737963, - "heading": 0.30152241765502197, - "angularVelocity": 7.724305624896446e-7, - "velocityX": 3.2076042379769354, - "velocityY": -2.003612088889046, - "timestamp": 2.3444929345065466 + "x": 2.3862784269967885, + "y": 3.0299921206795677, + "heading": -0.7979445022798209, + "angularVelocity": 0.07214760590413402, + "velocityX": 0.2361025217555023, + "velocityY": -0.11148080618080956, + "timestamp": 11.782475441082498 }, { - "x": 4.174386727718979, - "y": 1.9243094504352285, - "heading": 0.30152246485493484, - "angularVelocity": 7.723192600121745e-7, - "velocityX": 3.2969260847008353, - "velocityY": -1.85296107040964, - "timestamp": 2.40560744416731 + "x": 2.414605712310544, + "y": 3.016617241283919, + "heading": -0.7893555783917804, + "angularVelocity": 0.1431800427464117, + "velocityX": 0.4722246901920792, + "velocityY": -0.22296341527296182, + "timestamp": 11.842462320002856 }, { - "x": 4.383354452565466, - "y": 1.8255438101811146, - "heading": 0.30152251288342946, - "angularVelocity": 7.858771165145409e-7, - "velocityX": 3.419281705873656, - "velocityY": -1.6160751481496927, - "timestamp": 2.466721953828073 + "x": 2.457098595113841, + "y": 2.996554734272641, + "heading": -0.7765849369819304, + "angularVelocity": 0.2128905794016223, + "velocityX": 0.7083696229589314, + "velocityY": -0.33444825555792773, + "timestamp": 11.902449198923213 }, { - "x": 4.598761540028333, - "y": 1.7417467464566943, - "heading": 0.30152256294147634, - "angularVelocity": 8.190861247953373e-7, - "velocityX": 3.5246472344874826, - "velocityY": -1.3711484259558735, - "timestamp": 2.5278364634888364 + "x": 2.5137586742709717, + "y": 2.9698044458506936, + "heading": -0.759727315329062, + "angularVelocity": 0.281021816041631, + "velocityX": 0.944541209292721, + "velocityY": -0.44593565965421617, + "timestamp": 11.96243607784357 }, { - "x": 4.819537390717514, - "y": 1.6733348748555603, - "heading": 0.30152261646908723, - "angularVelocity": 8.758576510857166e-7, - "velocityX": 3.612494838208991, - "velocityY": -1.1194047367945392, - "timestamp": 2.5889509731495997 + "x": 2.5845878442638655, + "y": 2.9363662114763, + "heading": -0.7388971783301493, + "angularVelocity": 0.34724488711220036, + "velocityX": 1.180744377231747, + "velocityY": -0.5574258067133014, + "timestamp": 12.022422956763927 }, { - "x": 5.04458467733647, - "y": 1.620648272623701, - "heading": 0.30152267534909943, - "angularVelocity": 9.634375304093619e-7, - "velocityX": 3.682387175617667, - "velocityY": -0.8620964567058491, - "timestamp": 2.650065482810363 + "x": 2.669588381015972, + "y": 2.8962398712988433, + "heading": -0.7142349782126004, + "angularVelocity": 0.41112657570152883, + "velocityX": 1.4169854855252446, + "velocityY": -0.6689186185320778, + "timestamp": 12.082409835684285 }, { - "x": 5.27278482830379, - "y": 1.5839488347993986, - "heading": 0.301522742248914, - "angularVelocity": 0.000001094663361600596, - "velocityX": 3.7339766322927477, - "velocityY": -0.6005028597630109, - "timestamp": 2.711179992471126 + "x": 2.768763062860025, + "y": 2.849425297465716, + "heading": -0.6859166617272396, + "angularVelocity": 0.4720751770225952, + "velocityX": 1.653272909492815, + "velocityY": -0.7804135616937369, + "timestamp": 12.142396714604642 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.3015228161300743, - "angularVelocity": 0.0000012088972110609183, - "velocityX": 3.7670067261218194, - "velocityY": -0.33592432468507777, - "timestamp": 2.7722945021318894 + "x": 2.8821153449579997, + "y": 2.795922445610523, + "heading": -0.6541688123580982, + "angularVelocity": 0.5292465609236296, + "velocityX": 1.8896179320892514, + "velocityY": -0.8919092444570627, + "timestamp": 12.202383593525 }, { - "x": 5.642161233224445, - "y": 1.5569512204704454, - "heading": 0.30152289610635313, - "angularVelocity": 0.0000021712102282875206, - "velocityX": 3.7778762267369546, - "velocityY": -0.17558800471026415, - "timestamp": 2.809129387028348 + "x": 3.0096496153353156, + "y": 2.73573145985777, + "heading": -0.6192942663212868, + "angularVelocity": 0.5813695705541383, + "velocityX": 2.1260361044394105, + "velocityY": -1.0034025246198655, + "timestamp": 12.262370472445356 }, { - "x": 5.781468008961295, - "y": 1.556401100893367, - "heading": 0.30152296986307525, - "angularVelocity": 0.0000020023605969229892, - "velocityX": 3.781925099764402, - "velocityY": -0.014934744023907952, - "timestamp": 2.8459642719248066 + "x": 3.151371569227902, + "y": 2.6688529097388005, + "heading": -0.5817190571900679, + "angularVelocity": 0.6263904675071726, + "velocityX": 2.3625492181506123, + "velocityY": -1.1148863105173612, + "timestamp": 12.322357351365714 }, { - "x": 5.920672391336217, - "y": 1.5617696165699877, - "heading": 0.3015230386365787, - "angularVelocity": 0.0000018670752912406278, - "velocityX": 3.779145306581512, - "velocityY": 0.14574541746802047, - "timestamp": 2.882799156821265 + "x": 3.30728870131474, + "y": 2.5952884046594, + "heading": -0.5420886244414685, + "angularVelocity": 0.6606516868666429, + "velocityX": 2.5991872705003334, + "velocityY": -1.2263432671179575, + "timestamp": 12.382344230286071 }, { - "x": 6.0595230325638285, - "y": 1.5730470711005764, - "heading": 0.3015231034083751, - "angularVelocity": 0.0000017584362380693717, - "velocityX": 3.769541879061509, - "velocityY": 0.30616233937716997, - "timestamp": 2.919634041717724 + "x": 3.477410464899007, + "y": 2.5150425607670805, + "heading": -0.501501110267189, + "angularVelocity": 0.6766065330414338, + "velocityX": 2.8359829123654157, + "velocityY": -1.3377232710983202, + "timestamp": 12.442331109206428 }, { - "x": 6.197769224123459, - "y": 1.5902130982406153, - "heading": 0.3015231649742209, - "angularVelocity": 0.000001671400517937078, - "velocityX": 3.753132172076425, - "velocityY": 0.46602635486148125, - "timestamp": 2.9564689266141824 + "x": 3.661742704544347, + "y": 2.428132084038069, + "heading": -0.46226256646132613, + "angularVelocity": 0.6541187758402722, + "velocityX": 3.0728759849311573, + "velocityY": -1.4488247812392314, + "timestamp": 12.502317988126785 }, { - "x": 6.335161349542959, - "y": 1.6132366986444322, - "heading": 0.3015232239953199, - "angularVelocity": 0.0000016023152822250254, - "velocityX": 3.72994583275345, - "velocityY": 0.6250487946015117, - "timestamp": 2.993303811510641 + "x": 3.860146403975295, + "y": 2.3346891355318427, + "heading": -0.4327535611710109, + "angularVelocity": 0.4919243311440368, + "velocityX": 3.307451612782871, + "velocityY": -1.5577231252568988, + "timestamp": 12.562304867047143 }, { - "x": 6.471451335210211, - "y": 1.6420762959087598, - "heading": 0.30152328103508963, - "angularVelocity": 0.000001548525804202316, - "velocityX": 3.7000247469309935, - "velocityY": 0.7829425107583317, - "timestamp": 3.0301386964070995 + "x": 4.065242173401202, + "y": 2.237711412138144, + "heading": -0.4327535477418668, + "angularVelocity": 2.238680243649846e-7, + "velocityX": 3.41901050891823, + "velocityY": -1.6166489262168846, + "timestamp": 12.6222917459675 }, { - "x": 6.606393098338155, - "y": 1.6766798119201127, - "heading": 0.301523336612691, - "angularVelocity": 0.000001508830595721816, - "velocityX": 3.663422961881362, - "velocityY": 0.9394224010369989, - "timestamp": 3.066973581303558 + "x": 4.270337924993822, + "y": 2.140733651028875, + "heading": -0.4327535343131544, + "angularVelocity": 2.2386082909736944e-7, + "velocityX": 3.419010211631728, + "velocityY": -1.6166495549472233, + "timestamp": 12.682278624887857 }, { - "x": 6.739742463422568, - "y": 1.716984614087568, - "heading": 0.3015255656708468, - "angularVelocity": 0.00006051486687697733, - "velocityX": 3.6201922568579414, - "velocityY": 1.094201930608725, - "timestamp": 3.1038084662000167 + "x": 4.475433676585232, + "y": 2.0437558899170503, + "heading": -0.43275352088444197, + "angularVelocity": 2.2386082765717578e-7, + "velocityX": 3.419010211611586, + "velocityY": -1.6166495549898217, + "timestamp": 12.742265503808214 }, { - "x": 6.8694207904091344, - "y": 1.7614703171513673, - "heading": 0.3101205256410531, - "angularVelocity": 0.23333750042565204, - "velocityX": 3.520530262306672, - "velocityY": 1.207705771006116, - "timestamp": 3.1406433510964753 + "x": 4.6805294281766425, + "y": 1.9467781288052255, + "heading": -0.43275350745572955, + "angularVelocity": 2.2386082872977313e-7, + "velocityX": 3.4190102116115844, + "velocityY": -1.616649554989824, + "timestamp": 12.802252382728572 }, { - "x": 6.994764102779472, - "y": 1.808693107878132, - "heading": 0.3306180633638746, - "angularVelocity": 0.5564707961064413, - "velocityX": 3.4028425152588873, - "velocityY": 1.282012713206686, - "timestamp": 3.177478235992934 + "x": 4.885625179768054, + "y": 1.8498003676934025, + "heading": -0.4327534940270171, + "angularVelocity": 2.2386082836686737e-7, + "velocityX": 3.419010211611597, + "velocityY": -1.6166495549897981, + "timestamp": 12.862239261648929 }, { - "x": 7.114597005425528, - "y": 1.856512920318317, - "heading": 0.35962771339750194, - "angularVelocity": 0.7875591335542964, - "velocityX": 3.253244932973218, - "velocityY": 1.2982207647615676, - "timestamp": 3.2143131208893925 - }, - { - "x": 7.228563598795877, - "y": 1.9039637614742513, - "heading": 0.39146295222083616, - "angularVelocity": 0.8642687200685354, - "velocityX": 3.093985326429113, - "velocityY": 1.2882038667778264, - "timestamp": 3.251148005785851 + "x": 5.090720931370458, + "y": 1.7528226066048298, + "heading": -0.4327534805983047, + "angularVelocity": 2.2386082909228912e-7, + "velocityX": 3.4190102117948658, + "velocityY": -1.616649554602208, + "timestamp": 12.922226140569286 }, { - "x": 7.336676614220082, - "y": 1.9505339221399116, - "heading": 0.42341393247502157, - "angularVelocity": 0.8674108889982484, - "velocityX": 2.935071352282109, - "velocityY": 1.2642949963483496, - "timestamp": 3.2879828906823096 + "x": 5.295816845235169, + "y": 1.6558451886813434, + "heading": -0.4327534671695918, + "angularVelocity": 2.2386083672590555e-7, + "velocityX": 3.4190129167581613, + "velocityY": -1.6166438339330798, + "timestamp": 12.982213019489643 }, { - "x": 7.438996464546078, - "y": 1.995939510716552, - "heading": 0.45395278545537515, - "angularVelocity": 0.8290742068611583, - "velocityX": 2.77779747686502, - "velocityY": 1.2326789863542051, - "timestamp": 3.3248177755787682 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.4327534537085892, + "angularVelocity": 2.2439911588435075e-7, + "velocityX": 3.4538678416562716, + "velocityY": -1.5407736813730113, + "timestamp": 13.04219989841 }, { - "x": 7.535582073954796, - "y": 2.0400031624519186, - "heading": 0.48208291467910164, - "angularVelocity": 0.7636817463336474, - "velocityX": 2.6221232855814667, - "velocityY": 1.196247846551653, - "timestamp": 3.361652660475227 + "x": 5.767637732978587, + "y": 1.4715486447521433, + "heading": -0.43275344189521775, + "angularVelocity": 1.5949042736003556e-7, + "velocityX": 3.5727828839624562, + "velocityY": -1.2403266728714895, + "timestamp": 13.116269368711977 }, { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.6791276743107281, - "velocityX": 2.4678193928677437, - "velocityY": 1.1565397180800703, - "timestamp": 3.3984875453716854 + "x": 6.033774192023134, + "y": 1.384125705645109, + "heading": -0.43275343009903516, + "angularVelocity": 1.5925836231633141e-7, + "velocityX": 3.5930655094403474, + "velocityY": -1.1802830336252825, + "timestamp": 13.190338839013954 }, { - "x": 7.766333855095899, - "y": 2.1513911601167894, - "heading": 0.5393931714770351, - "angularVelocity": 0.5089507306836832, - "velocityX": 2.2039777611896034, - "velocityY": 1.0840548028640598, - "timestamp": 3.461940970502476 + "x": 6.2999107025526015, + "y": 1.2967029232707166, + "heading": -0.43275341830285263, + "angularVelocity": 1.5925836222768275e-7, + "velocityX": 3.593066204529984, + "velocityY": -1.1802809176031077, + "timestamp": 13.26440830931593 }, { - "x": 7.8897168213730975, - "y": 2.214535022901093, - "heading": 0.5621690879226864, - "angularVelocity": 0.3589391179231937, - "velocityX": 1.9444650312080196, - "velocityY": 0.9951214241650669, - "timestamp": 3.5253943956332665 + "x": 6.566047160708243, + "y": 1.2092799814598572, + "heading": -0.4327534065048956, + "angularVelocity": 1.5928231960994732e-7, + "velocityX": 3.5930654974393894, + "velocityY": -1.1802830701291929, + "timestamp": 13.338477779617907 }, { - "x": 7.996856912991746, - "y": 2.271342778200504, - "heading": 0.5763089063837279, - "angularVelocity": 0.2228377496076287, - "velocityX": 1.6884839769927567, - "velocityY": 0.8952669644281369, - "timestamp": 3.588847820764057 + "x": 6.815604168443372, + "y": 1.1270295237235333, + "heading": -0.36800659105834677, + "angularVelocity": 0.874136336908857, + "velocityX": 3.369229005117782, + "velocityY": -1.1104501949452927, + "timestamp": 13.412547249919884 }, { - "x": 8.08793407853681, - "y": 2.3213235528188796, - "heading": 0.5824533246990756, - "angularVelocity": 0.09683351690917834, - "velocityX": 1.4353388387992245, - "velocityY": 0.7876765441007412, - "timestamp": 3.6523012458948476 + "x": 7.0424486777806585, + "y": 1.0523256982782683, + "heading": -0.30458898646479193, + "angularVelocity": 0.8561908750664163, + "velocityX": 3.0625912189253857, + "velocityY": -1.0085643267152118, + "timestamp": 13.48661672022186 }, { - "x": 8.16309393881241, - "y": 2.364112462024999, - "heading": 0.5810872026138336, - "angularVelocity": -0.021529524725041504, - "velocityX": 1.1844886248564517, - "velocityY": 0.6743356898059124, - "timestamp": 3.715754671025638 + "x": 7.2465905745815595, + "y": 0.9851213052520007, + "heading": -0.2456798424413219, + "angularVelocity": 0.7953228743678266, + "velocityX": 2.756086900157761, + "velocityY": -0.9073156963628867, + "timestamp": 13.560686190523837 }, { - "x": 8.222455939995415, - "y": 2.399428067130867, - "heading": 0.5725898810376202, - "angularVelocity": -0.13391430893917436, - "velocityX": 0.9355208337555891, - "velocityY": 0.5565594770191685, - "timestamp": 3.7792080961564287 + "x": 7.428038548794458, + "y": 0.9253999088853845, + "heading": -0.1923064776337199, + "angularVelocity": 0.7205852099387569, + "velocityX": 2.449699902984963, + "velocityY": -0.8062889625528059, + "timestamp": 13.634755660825814 }, { - "x": 8.266119462815807, - "y": 2.4270468912932266, - "heading": 0.5572661381126698, - "angularVelocity": -0.24149591441856846, - "velocityX": 0.6881192422693254, - "velocityY": 0.43526136068197446, - "timestamp": 3.8426615212872193 + "x": 7.586797949690042, + "y": 0.8731533407207944, + "heading": -0.14497999296920738, + "angularVelocity": 0.6389472541327177, + "velocityX": 2.1433851254549703, + "velocityY": -0.7053725097747339, + "timestamp": 13.70882513112779 }, { - "x": 8.294168308263885, - "y": 2.4467872814650136, - "heading": 0.5353660980042331, - "angularVelocity": -0.3451356654632318, - "velocityX": 0.4420383200790668, - "velocityY": 0.3111004666981787, - "timestamp": 3.90611494641801 + "x": 7.722872327084394, + "y": 0.8283767866857153, + "heading": -0.10400779573923787, + "angularVelocity": 0.5531590419497899, + "velocityX": 1.837118273420693, + "velocityY": -0.6045210510150557, + "timestamp": 13.782894601429767 }, { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.44548570157766576, - "velocityX": 0.19708463824313602, - "velocityY": 0.18456741878341876, - "timestamp": 3.9695683715488004 + "x": 7.836264194581955, + "y": 0.7910670998170554, + "heading": -0.06959570799723021, + "angularVelocity": 0.46459205934255987, + "velocityX": 1.5308853571555237, + "velocityY": -0.5037120788977025, + "timestamp": 13.856964071731744 }, { - "x": 8.303024530407173, - "y": 2.461881127582199, - "heading": 0.47119103232326837, - "angularVelocity": -0.5465136547157349, - "velocityX": -0.05554517814981655, - "velocityY": 0.05148048067312068, - "timestamp": 4.035271161880262 + "x": 7.926975414267803, + "y": 0.7612220655404804, + "heading": -0.04189104317473832, + "angularVelocity": 0.3740362218001813, + "velocityX": 1.2246775806013535, + "velocityY": -0.4029330053920843, + "timestamp": 13.93103354203372 }, { - "x": 8.282750419132642, - "y": 2.456558519757899, - "heading": 0.4288756816904664, - "angularVelocity": -0.6440419108431604, - "velocityX": -0.3085730632177365, - "velocityY": -0.08101037714605593, - "timestamp": 4.1009739522117235 + "x": 7.99500740651227, + "y": 0.7388400306235591, + "heading": -0.021003806792755153, + "angularVelocity": 0.28199521741990785, + "velocityX": 0.9184889802384881, + "velocityY": -0.30217625191150127, + "timestamp": 14.005103012335697 }, { - "x": 8.245821395629305, - "y": 2.442576926285189, - "heading": 0.3804195474569323, - "angularVelocity": -0.7375049672788585, - "velocityX": -0.5620617224479442, - "velocityY": -0.21280060408660287, - "timestamp": 4.166676742543185 + "x": 8.040361272813582, + "y": 0.7239196956147146, + "heading": -0.007018297206773477, + "angularVelocity": 0.18881611450660732, + "velocityX": 0.6123152510259342, + "velocityY": -0.20143704211755917, + "timestamp": 14.079172482637674 }, { - "x": 8.19220201387014, - "y": 2.4199912479232255, - "heading": 0.32613641295591117, - "angularVelocity": -0.8261922245184762, - "velocityX": -0.8160898721144456, - "velocityY": -0.34375523852216117, - "timestamp": 4.2323795328746465 + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": 2.0779165225884802e-41, + "angularVelocity": 0.09475290127174345, + "velocityX": 0.3061531209608937, + "velocityY": -0.10071229126619018, + "timestamp": 14.15324195293965 }, { - "x": 8.121850214183777, - "y": 2.388868090056124, - "heading": 0.2664003971864745, - "angularVelocity": -0.909185370485436, - "velocityX": -1.070758172239708, - "velocityY": -0.4736961354318384, - "timestamp": 4.298082323206108 - }, - { - "x": 8.034715295558529, - "y": 2.349289919990928, - "heading": 0.20166644793914118, - "angularVelocity": -0.9852541866298153, - "velocityX": -1.3261981444876794, - "velocityY": -0.6023818754961501, - "timestamp": 4.36378511353757 - }, - { - "x": 7.9307349853362235, - "y": 2.301361396168203, - "heading": 0.1325025874209066, - "angularVelocity": -1.052677674255729, - "velocityX": -1.5825859099398853, - "velocityY": -0.7294747084702512, - "timestamp": 4.429487903869031 - }, - { - "x": 7.809831081747403, - "y": 2.2452194829459002, - "heading": 0.0596429970770519, - "angularVelocity": -1.1089268808263426, - "velocityX": -1.8401639105261376, - "velocityY": -0.8544829365552749, - "timestamp": 4.495190694200493 - }, - { - "x": 7.671902780233127, - "y": 2.1810506699963903, - "heading": -0.015920256793080897, - "angularVelocity": -1.1500767850029918, - "velocityX": -2.099276161917122, - "velocityY": -0.9766527818040505, - "timestamp": 4.560893484531954 - }, - { - "x": 7.516816185598606, - "y": 2.109122861945235, - "heading": -0.09276870443286889, - "angularVelocity": -1.1696375032490751, - "velocityX": -2.3604263053689314, - "velocityY": -1.0947451042534069, - "timestamp": 4.626596274863416 - }, - { - "x": 7.344387868807497, - "y": 2.02985165560778, - "heading": -0.16871929267657412, - "angularVelocity": -1.1559720349858067, - "velocityX": -2.6243682486121434, - "velocityY": -1.206512020837212, - "timestamp": 4.692299065194877 - }, - { - "x": 7.154363408340053, - "y": 1.9439629823466762, - "heading": -0.2400485240661188, - "angularVelocity": -1.0856347352935642, - "velocityX": -2.8921825010596516, - "velocityY": -1.3072302230667405, - "timestamp": 4.758001855526339 - }, - { - "x": 6.9464441838510815, - "y": 1.8530162365161489, - "heading": -0.2992772419214471, - "angularVelocity": -0.9014642689682996, - "velocityX": -3.1645417712101533, - "velocityY": -1.3842143594162994, - "timestamp": 4.8237046458578 - }, - { - "x": 6.72212326982476, - "y": 1.7626751908782685, - "heading": -0.31855321839260475, - "angularVelocity": -0.2933813978662556, - "velocityX": -3.4141763674670904, - "velocityY": -1.3749955699312393, - "timestamp": 4.889407436189262 - }, - { - "x": 6.485759745047901, - "y": 1.6860130581467425, - "heading": -0.3185532921212082, - "angularVelocity": -0.0000011221533066716862, - "velocityX": -3.597465550312819, - "velocityY": -1.1668017803319484, - "timestamp": 4.955110226520723 - }, - { - "x": 6.24426876944993, - "y": 1.6274732096370486, - "heading": -0.3185533177664736, - "angularVelocity": -3.9032231775904613e-7, - "velocityX": -3.675505627381763, - "velocityY": -0.8909796405048926, - "timestamp": 5.020813016852185 - }, - { - "x": 5.999037658914315, - "y": 1.5873919634602989, - "heading": -0.3185533462426243, - "angularVelocity": -4.334085446884588e-7, - "velocityX": -3.732430682143934, - "velocityY": -0.6100387209515062, - "timestamp": 5.0865158071836465 - }, - { - "x": 5.751475205523387, - "y": 1.5659995878977724, - "heading": -0.3185533789943251, - "angularVelocity": -4.984826463431063e-7, - "velocityX": -3.7679138456983283, - "velocityY": -0.325593105781428, - "timestamp": 5.152218597515108 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.3185534180019772, - "angularVelocity": -5.936985611348973e-7, - "velocityX": -3.781751231726404, - "velocityY": -0.039276923728911305, - "timestamp": 5.21792138784657 - }, - { - "x": 5.26265149067387, - "y": 1.5786031053003822, - "heading": -0.31855345112044986, - "angularVelocity": -5.20084412877405e-7, - "velocityX": -3.7744308273915435, - "velocityY": 0.23844772894968475, - "timestamp": 5.28160042121006 - }, - { - "x": 5.0240625547753925, - "y": 1.61139052009726, - "heading": -0.3185534786660252, - "angularVelocity": -4.3256899255575315e-7, - "velocityX": -3.7467424251962935, - "velocityY": 0.5148855606793222, - "timestamp": 5.34527945457355 - }, - { - "x": 4.788524290519247, - "y": 1.661604289022676, - "heading": -0.31855350247111613, - "angularVelocity": -3.7382933947955595e-7, - "velocityX": -3.698835422197071, - "velocityY": 0.7885447732660761, - "timestamp": 5.40895848793704 - }, - { - "x": 4.55730773239592, - "y": 1.7289734284079028, - "heading": -0.3185535237219762, - "angularVelocity": -3.337183198598428e-7, - "velocityX": -3.6309684037367047, - "velocityY": 1.0579485244487485, - "timestamp": 5.4726375213005305 - }, - { - "x": 4.33166058516164, - "y": 1.8131343663413284, - "heading": -0.3185535432404584, - "angularVelocity": -3.0651348259884106e-7, - "velocityX": -3.543507734268667, - "velocityY": 1.321642831055895, - "timestamp": 5.536316554664021 - }, - { - "x": 4.112800476187319, - "y": 1.9136328827222209, - "heading": -0.318553561635836, - "angularVelocity": -2.888765193967595e-7, - "velocityX": -3.436925741712075, - "velocityY": 1.5782041760469432, - "timestamp": 5.599995588027511 - }, - { - "x": 3.9019083093617053, - "y": 2.0299264483320134, - "heading": -0.31855357939397577, - "angularVelocity": -2.7886949297236075e-7, - "velocityX": -3.311799122668965, - "velocityY": 1.826245774585971, - "timestamp": 5.663674621391001 - }, - { - "x": 3.7001203074329077, - "y": 2.161384813738153, - "heading": -0.3185535969362317, - "angularVelocity": -2.75479306959021e-7, - "velocityX": -3.1688295388681516, - "velocityY": 2.0643900898393674, - "timestamp": 5.727353654754491 - }, - { - "x": 3.509285404964888, - "y": 2.2879634464070207, - "heading": -0.3579155843505842, - "angularVelocity": -0.6181310446354924, - "velocityX": -2.996824737880426, - "velocityY": 1.9877599577609337, - "timestamp": 5.791032688117982 - }, - { - "x": 3.332909755538667, - "y": 2.4050971261724468, - "heading": -0.4129449232759771, - "angularVelocity": -0.8641673093760197, - "velocityX": -2.769760156682042, - "velocityY": 1.8394387222684185, - "timestamp": 5.854711721481472 - }, - { - "x": 3.171287726021972, - "y": 2.512479984884094, - "heading": -0.4699204658498491, - "angularVelocity": -0.8947300165290918, - "velocityX": -2.5380729100288124, - "velocityY": 1.6863142079856834, - "timestamp": 5.918390754844962 - }, - { - "x": 3.024408806786741, - "y": 2.6100923699912792, - "heading": -0.5252799185719804, - "angularVelocity": -0.8693513358805374, - "velocityX": -2.306550704009953, - "velocityY": 1.5328810748429236, - "timestamp": 5.982069788208452 - }, - { - "x": 2.892255079193046, - "y": 2.6979336450739244, - "heading": -0.5773440254447586, - "angularVelocity": -0.8176020288434336, - "velocityX": -2.0753098879397367, - "velocityY": 1.3794379475145813, - "timestamp": 6.045748821571943 - }, - { - "x": 2.7748123815012584, - "y": 2.7760062309054065, - "heading": -0.625128837055519, - "angularVelocity": -0.7504010203483636, - "velocityX": -1.8442914643726742, - "velocityY": 1.2260328354205916, - "timestamp": 6.109427854935433 - }, - { - "x": 2.6720699818921556, - "y": 2.844312916696855, - "heading": -0.6679860381773631, - "angularVelocity": -0.6730190277419643, - "velocityX": -1.6134415706757543, - "velocityY": 1.0726715244175122, - "timestamp": 6.173106888298923 - }, - { - "x": 2.5840196243973645, - "y": 2.9028562717726505, - "heading": -0.7054558016839698, - "angularVelocity": -0.5884160221579265, - "velocityX": -1.382721326691407, - "velocityY": 0.9193505614575078, - "timestamp": 6.236785921662413 - }, - { - "x": 2.510654811151598, - "y": 2.95163853061359, - "heading": -0.7371951288142529, - "angularVelocity": -0.4984266477336408, - "velocityX": -1.1521031235978156, - "velocityY": 0.7660646882386306, - "timestamp": 6.3004649550259035 - }, - { - "x": 2.4519703126340358, - "y": 2.9906615990271557, - "heading": -0.7629386573341134, - "angularVelocity": -0.4042700895428552, - "velocityX": -0.9215670436230066, - "velocityY": 0.6128087433553882, - "timestamp": 6.364143988389394 - }, - { - "x": 2.4079618327799786, - "y": 3.0199270886206313, - "heading": -0.7824753669479414, - "angularVelocity": -0.3067997201262361, - "velocityX": -0.6910984279998401, - "velocityY": 0.4595781067596215, - "timestamp": 6.427823021752884 - }, - { - "x": 2.378625774904473, - "y": 3.039436353320747, - "heading": -0.795633833561549, - "angularVelocity": -0.20663734856804464, - "velocityX": -0.4606862938394533, - "velocityY": 0.3063687318988399, - "timestamp": 6.491502055116374 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.10425061542851642, - "velocityX": -0.2303222914890624, - "velocityY": 0.15317707264507022, - "timestamp": 6.5551810884798645 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -1.4092752497411054e-30, - "velocityX": 2.9428006004895336e-31, - "velocityY": 5.314710200495422e-31, - "timestamp": 6.618860121843355 - }, - { - "x": 2.3794842314620843, - "y": 3.041727640405621, - "heading": -0.7962432666233688, - "angularVelocity": 0.0957711014436812, - "velocityX": 0.24661230413774035, - "velocityY": -0.11854554422657079, - "timestamp": 6.681813823589832 - }, - { - "x": 2.410536447869941, - "y": 3.0268006675090624, - "heading": -0.7842917338243374, - "angularVelocity": 0.18984638658997072, - "velocityX": 0.4932548134009415, - "velocityY": -0.23711032842312177, - "timestamp": 6.744767525336309 - }, - { - "x": 2.4571179241280796, - "y": 3.0044081651489933, - "heading": -0.7665449064601504, - "angularVelocity": 0.28190284084732253, - "velocityX": 0.7399322830248891, - "velocityY": -0.3556979452971155, - "timestamp": 6.807721227082786 - }, - { - "x": 2.5192312369840457, - "y": 2.9745484345249116, - "heading": -0.743155503226628, - "angularVelocity": 0.37153340605314533, - "velocityX": 0.9866506834833091, - "velocityY": -0.4743125470894598, - "timestamp": 6.8706749288292635 - }, - { - "x": 2.596879439737626, - "y": 2.937219467270397, - "heading": -0.714309410311632, - "angularVelocity": 0.4582112268975565, - "velocityX": 1.233417584660547, - "velocityY": -0.5929590511586358, - "timestamp": 6.933628630575741 - }, - { - "x": 2.690066196626848, - "y": 2.892418878795792, - "heading": -0.6802368283616581, - "angularVelocity": 0.5412323819683986, - "velocityX": 1.4802426911208155, - "velocityY": -0.7116434336939077, - "timestamp": 6.996582332322218 - }, - { - "x": 2.798795963694134, - "y": 2.8401438158547943, - "heading": -0.6412294413317386, - "angularVelocity": 0.6196202280051365, - "velocityX": 1.7271385804309867, - "velocityY": -0.8303731391605256, - "timestamp": 7.059536034068695 - }, - { - "x": 2.9230742308720847, - "y": 2.7803908299690017, - "heading": -0.5976682545562845, - "angularVelocity": 0.6919559226378926, - "velocityX": 1.9741216756154527, - "velocityY": -0.9491576226355342, - "timestamp": 7.122489735815172 - }, - { - "x": 3.062907829622796, - "y": 2.713155712528932, - "heading": -0.5500718966234338, - "angularVelocity": 0.7560533632244142, - "velocityX": 2.22121328645359, - "velocityY": -1.0680089585650514, - "timestamp": 7.185443437561649 - }, - { - "x": 3.218305230273769, - "y": 2.638433320090989, - "heading": -0.4991885737635272, - "angularVelocity": 0.8082657802208455, - "velocityX": 2.4684394458133507, - "velocityY": -1.1869419964986359, - "timestamp": 7.248397139308127 - }, - { - "x": 3.389276272924326, - "y": 2.556217612467452, - "heading": -0.4461960441931904, - "angularVelocity": 0.8417698737358584, - "velocityX": 2.715821912094713, - "velocityY": -1.3059709809381974, - "timestamp": 7.311350841054604 - }, - { - "x": 3.575827436112715, - "y": 2.4665034787202997, - "heading": -0.3932374103043789, - "angularVelocity": 0.841231451362194, - "velocityX": 2.9633072879440143, - "velocityY": -1.4250811510408536, - "timestamp": 7.374304542801081 - }, - { - "x": 3.777911234478158, - "y": 2.369308059755886, - "heading": -0.34555593776982274, - "angularVelocity": 0.7574053822375035, - "velocityX": 3.210038373585424, - "velocityY": -1.5439190431697394, - "timestamp": 7.437258244547558 - }, - { - "x": 3.9926293770053456, - "y": 2.266439040708753, - "heading": -0.3455557491739834, - "angularVelocity": 0.000002995786333953476, - "velocityX": 3.4107310066036556, - "velocityY": -1.6340424183696203, - "timestamp": 7.500211946294035 - }, - { - "x": 4.207200483864901, - "y": 2.1632635581376745, - "heading": -0.34555573252846533, - "angularVelocity": 2.644088848166298e-7, - "velocityX": 3.4083953906898548, - "velocityY": -1.6389104962656516, - "timestamp": 7.5631656480405125 - }, - { - "x": 4.421771581956281, - "y": 2.0600880573316713, - "heading": -0.3455557158829472, - "angularVelocity": 2.6440888572481287e-7, - "velocityX": 3.408395251410094, - "velocityY": -1.638910785921765, - "timestamp": 7.62611934978699 - }, - { - "x": 4.636342680047139, - "y": 1.9569125565245837, - "heading": -0.3455556992374292, - "angularVelocity": 2.6440888404451857e-7, - "velocityX": 3.4083952514018088, - "velocityY": -1.6389107859389946, - "timestamp": 7.689073051533467 - }, - { - "x": 4.850913778140846, - "y": 1.853737055723424, - "heading": -0.3455556825919111, - "angularVelocity": 2.64408884846605e-7, - "velocityX": 3.408395251447087, - "velocityY": -1.63891078584483, - "timestamp": 7.752026753279944 - }, - { - "x": 5.065484924140348, - "y": 1.7505616545506137, - "heading": -0.34555566594639303, - "angularVelocity": 2.644088848456774e-7, - "velocityX": 3.4083960124157384, - "velocityY": -1.6389092032794423, - "timestamp": 7.814980455026421 - }, - { - "x": 5.280845302209934, - "y": 1.6490439336616942, - "heading": -0.34555564929194366, - "angularVelocity": 2.6455075541297124e-7, - "velocityX": 3.4209327187283005, - "velocityY": -1.6125774668143436, - "timestamp": 7.877934156772898 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, - "angularVelocity": 2.6730250915052296e-7, - "velocityX": 3.52891551864971, - "velocityY": -1.360125725304771, - "timestamp": 7.940887858519376 - }, - { - "x": 5.655413318775525, - "y": 1.5130951799644636, - "heading": -0.34555563491945995, - "angularVelocity": -5.785220393859716e-8, - "velocityX": 3.5912523855702734, - "velocityY": -1.18578710714545, - "timestamp": 7.983327014878419 - }, - { - "x": 5.810103118724588, - "y": 1.4702901058122384, - "heading": -0.3455556373457815, - "angularVelocity": -5.7171766263130695e-8, - "velocityX": 3.6449782045702985, - "velocityY": -1.0086221740622332, - "timestamp": 8.025766171237462 - }, - { - "x": 5.966313456902953, - "y": 1.4334181808859694, - "heading": -0.34555563976573417, - "angularVelocity": -5.702169541040829e-8, - "velocityX": 3.680806867525754, - "velocityY": -0.8688185178405917, - "timestamp": 8.068205327596505 - }, - { - "x": 6.1225244823663525, - "y": 1.3965491678093302, - "heading": -0.3455556421856862, - "angularVelocity": -5.7021679533312126e-8, - "velocityX": 3.680823062122731, - "velocityY": -0.868749905505203, - "timestamp": 8.110644483955548 - }, - { - "x": 6.278735507957822, - "y": 1.3596801552753135, - "heading": -0.3455556446056381, - "angularVelocity": -5.702167859922657e-8, - "velocityX": 3.680823065140465, - "velocityY": -0.8687498927193055, - "timestamp": 8.153083640314591 - }, - { - "x": 6.434946533537413, - "y": 1.3228111426909752, - "heading": -0.34555564702559005, - "angularVelocity": -5.7021678932866206e-8, - "velocityX": 3.680823064860606, - "velocityY": -0.868749893905046, - "timestamp": 8.195522796673634 - }, - { - "x": 6.591157495254782, - "y": 1.285941859528607, - "heading": -0.3455556494455419, - "angularVelocity": -5.7021677677252914e-8, - "velocityX": 3.6808215600657594, - "velocityY": -0.8687562695744264, - "timestamp": 8.237961953032677 - }, - { - "x": 6.747031505398479, - "y": 1.2476730535838276, - "heading": -0.34555565194208476, - "angularVelocity": -5.8826401859372655e-8, - "velocityX": 3.6728819212374164, - "velocityY": -0.9017334279932018, - "timestamp": 8.28040110939172 - }, - { - "x": 6.897830261276489, - "y": 1.2034176695315386, - "heading": -0.3468060133183351, - "angularVelocity": -0.029462446559305244, - "velocityX": 3.5532929684610997, - "velocityY": -1.0427960367044076, - "timestamp": 8.322840265750763 - }, - { - "x": 7.040966542913821, - "y": 1.160898981796106, - "heading": -0.34756195184747823, - "angularVelocity": -0.017812289262957588, - "velocityX": 3.3727409759603195, - "velocityY": -1.001874009363348, - "timestamp": 8.365279422109806 - }, - { - "x": 7.176448137174031, - "y": 1.12015095875213, - "heading": -0.34781665101790943, - "angularVelocity": -0.006001513514462224, - "velocityX": 3.1923724664555144, - "velocityY": -0.9601515802821338, - "timestamp": 8.407718578468849 - }, - { - "x": 7.30427768133993, - "y": 1.0811849503221362, - "heading": -0.3475678387477372, - "angularVelocity": 0.005862799629361019, - "velocityX": 3.0120660996283153, - "velocityY": -0.918161711329369, - "timestamp": 8.450157734827892 - }, - { - "x": 7.424456501916377, - "y": 1.044006636667021, - "heading": -0.3468143875407301, - "angularVelocity": 0.017753680130510166, - "velocityX": 2.8317909894275823, - "velocityY": -0.8760379999211017, - "timestamp": 8.492596891186935 - }, - { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, - "angularVelocity": 0.02966022853563592, - "velocityX": 2.65153469287831, - "velocityY": -0.8338339406813241, - "timestamp": 8.535036047545978 - }, - { - "x": 7.689789531297121, - "y": 0.9591642629851495, - "heading": -0.34249163023542684, - "angularVelocity": 0.04769743470803589, - "velocityX": 2.378707539440784, - "velocityY": -0.769870356769085, - "timestamp": 8.599274349827635 - }, - { - "x": 7.82516177952376, - "y": 0.9142093996063804, - "heading": -0.3392101704632614, - "angularVelocity": 0.05108260423473965, - "velocityX": 2.1073447369933547, - "velocityY": -0.6998140016475143, - "timestamp": 8.663512652109292 - }, - { - "x": 7.943169958712402, - "y": 0.8740090692537413, - "heading": -0.33632582750820933, - "angularVelocity": 0.044900672225200826, - "velocityX": 1.837037639494695, - "velocityY": -0.6258000122166694, - "timestamp": 8.727750954390949 - }, - { - "x": 8.043864833075729, - "y": 0.8387415563520763, - "heading": -0.3342715201633078, - "angularVelocity": 0.03197947753809357, - "velocityX": 1.567520790350633, - "velocityY": -0.5490106626266708, - "timestamp": 8.791989256672606 - }, - { - "x": 8.12728568078066, - "y": 0.8085387560486261, - "heading": -0.33336873570137193, - "angularVelocity": 0.014053678722353937, - "velocityX": 1.2986153858669556, - "velocityY": -0.4701680964578417, - "timestamp": 8.856227558954263 - }, - { - "x": 8.193463737140812, - "y": 0.7835021726798679, - "heading": -0.33386568134146205, - "angularVelocity": -0.007735970946293819, - "velocityX": 1.030196222652151, - "velocityY": -0.38974540857234485, - "timestamp": 8.92046586123592 - }, - { - "x": 8.242424408463135, - "y": 0.7637123264900073, - "heading": -0.33595980135953535, - "angularVelocity": -0.03259924287680334, - "velocityX": 0.7621725603464993, - "velocityY": -0.3080692591016908, - "timestamp": 8.984704163517577 - }, - { - "x": 8.274188750584008, - "y": 0.7492346436854774, - "heading": -0.3398119288829893, - "angularVelocity": -0.05996620998114242, - "velocityX": 0.4944766750154696, - "velocityY": -0.2253746174836856, - "timestamp": 9.048942465799234 - }, - { - "x": 8.288774490356445, - "y": 0.7401233315467834, - "heading": -0.34555563246426124, - "angularVelocity": -0.08941244362418393, - "velocityX": 0.2270567442533848, - "velocityY": -0.14183612914838375, - "timestamp": 9.113180768080891 - }, - { - "x": 8.286058974983396, - "y": 0.7364361738247144, - "heading": -0.3533713967772531, - "angularVelocity": -0.1208300500196775, - "velocityX": -0.04198128874091145, - "velocityY": -0.05700267231030528, - "timestamp": 9.177864713107141 - }, - { - "x": 8.26594084231072, - "y": 0.7382366486349878, - "heading": -0.3631834148956511, - "angularVelocity": -0.15169170826572306, - "velocityX": -0.3110220420927156, - "velocityY": 0.027834956719829915, - "timestamp": 9.24254865813339 - }, + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": 1.169210048317657e-41, + "angularVelocity": -1.2268302369298258e-40, + "velocityX": -9.944733020002764e-42, + "velocityY": 5.460591898189625e-42, + "timestamp": 14.227311423241627 + } + ], + "constraints": [ { - "x": 8.22841989282902, - "y": 0.7455250700569162, - "heading": -0.3749497746847549, - "angularVelocity": -0.18190541384463682, - "velocityX": -0.5800658798172126, - "velocityY": 0.11267744134917045, - "timestamp": 9.30723260315964 + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 8.173495898954863, - "y": 0.7583018091084519, - "heading": -0.38862097650887273, - "angularVelocity": -0.21135386560868902, - "velocityX": -0.8491132359330754, - "velocityY": 0.1975256618369646, - "timestamp": 9.37191654818589 + "scope": [ + "last" + ], + "type": "StopPoint" }, { - "x": 8.101168599327213, - "y": 0.7765673113174031, - "heading": -0.4041376276128682, - "angularVelocity": -0.23988411804039605, - "velocityX": -1.11816463263485, - "velocityY": 0.2823807700896826, - "timestamp": 9.43660049321214 + "scope": [ + 6 + ], + "type": "StopPoint" }, { - "x": 8.011437691690773, - "y": 0.8003221216608263, - "heading": -0.42142708913788907, - "angularVelocity": -0.26729138920027884, - "velocityX": -1.3872207021390834, - "velocityY": 0.36724430357151194, - "timestamp": 9.50128443823839 + "scope": [ + 1 + ], + "type": "StopPoint" }, { - "x": 7.904302824322884, - "y": 0.8295669211666158, - "heading": -0.44039841224325665, - "angularVelocity": -0.29329261067284323, - "velocityX": -1.6562822092006166, - "velocityY": 0.45211836559939883, - "timestamp": 9.56596838326464 - }, + "scope": [ + 11 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "AmpLanePADESprint": { + "waypoints": [ { - "x": 7.779763586783285, - "y": 0.8643025828479174, - "heading": -0.46093432615418145, - "angularVelocity": -0.317480850968364, - "velocityX": -1.9253500615811048, - "velocityY": 0.5370059242243979, - "timestamp": 9.63065232829089 + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 7.63781950374517, - "y": 0.9045302615405436, - "heading": -0.48287779045070706, - "angularVelocity": -0.33924127985113656, - "velocityX": -2.194425262412658, - "velocityY": 0.6219113363648586, - "timestamp": 9.695336273317139 + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 }, { - "x": 7.478470046612931, - "y": 0.9502515474489494, - "heading": -0.5060075980261466, - "angularVelocity": -0.3575818940241346, - "velocityX": -2.4635086352196294, - "velocityY": 0.7068413327271726, - "timestamp": 9.760020218343389 + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 }, { - "x": 7.301714722310279, - "y": 1.0014687500113455, - "heading": -0.5299890975728885, - "angularVelocity": -0.37074887032647375, - "velocityX": -2.732599630880914, - "velocityY": 0.7918070325118697, - "timestamp": 9.824704163369638 + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 }, { - "x": 7.107553521342319, - "y": 1.0581854755780744, - "heading": -0.5542575516052335, - "angularVelocity": -0.37518512549746846, - "velocityX": -3.0016907733312475, - "velocityY": 0.8768284857040194, - "timestamp": 9.889388108395888 + "x": 8.017494201660156, + "y": 7.457698345184326, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 }, { - "x": 6.895989597473325, - "y": 1.1204078946209601, - "heading": -0.5776591781660143, - "angularVelocity": -0.3617841575878507, - "velocityX": -3.2707331592582385, - "velocityY": 0.961945333075069, - "timestamp": 9.954072053422138 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 6.667061067423103, - "y": 1.188145012147935, - "heading": -0.5965312726664429, - "angularVelocity": -0.2917585575952417, - "velocityX": -3.5391862688232427, - "velocityY": 1.0472013959489617, - "timestamp": 10.018755998448388 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 14 }, { - "x": 6.433742027972299, - "y": 1.2616769248691375, - "heading": -0.5965312781733252, - "angularVelocity": -8.513522502240808e-8, - "velocityX": -3.607062608134345, - "velocityY": 1.136787694247182, - "timestamp": 10.083439943474637 + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 6.200423078644191, - "y": 1.3352091235520722, - "heading": -0.5965312836800714, - "angularVelocity": -8.513312137357674e-8, - "velocityX": -3.6070612148566714, - "velocityY": 1.1367921151546025, - "timestamp": 10.148123888500887 + "x": 7.985864639282227, + "y": 5.863699913024902, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 5.967104129330135, - "y": 1.4087413222795935, - "heading": -0.5965312891868175, - "angularVelocity": -8.513312220445073e-8, - "velocityX": -3.607061214639434, - "velocityY": 1.136792115843899, - "timestamp": 10.212807833527137 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 }, { - "x": 5.733785345709214, - "y": 1.4822740467522528, - "heading": -0.5965312946935636, - "angularVelocity": -8.513311865132804e-8, - "velocityX": -3.6070586530588855, - "velocityY": 1.1368002437516491, - "timestamp": 10.277491778553387 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.5965313002103377, - "angularVelocity": -8.528815258110653e-8, - "velocityX": -3.567836630187545, - "velocityY": 1.254483436778072, - "timestamp": 10.342175723579636 - }, + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ { - "x": 5.285431093647359, - "y": 1.6581112689784743, - "heading": -0.5965313049795081, - "angularVelocity": -7.601302145092289e-8, - "velocityX": -3.4677610362467894, - "velocityY": 1.509245007513348, - "timestamp": 10.404917216242477 + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": 0, + "angularVelocity": 2.735053895521923e-38, + "velocityX": -3.8673469053295564e-39, + "velocityY": 1.858643532249755e-38, + "timestamp": 0 }, { - "x": 5.070948144949037, - "y": 1.7596075143791599, - "heading": -0.5965313097283005, - "angularVelocity": -7.568822719486902e-8, - "velocityX": -3.4185184252932688, - "velocityY": 1.6176893646140194, - "timestamp": 10.467658708905319 + "x": 0.46193105617219093, + "y": 6.976128411049919, + "heading": 0.007924686694784022, + "angularVelocity": 0.0965568963773332, + "velocityX": 0.35283526252830383, + "velocityY": -0.05604467125266167, + "timestamp": 0.08207271559159549 }, { - "x": 4.856465419222307, - "y": 1.861104230964457, - "heading": -0.5965313144770923, - "angularVelocity": -7.568821760685447e-8, - "velocityX": -3.4185148714793154, - "velocityY": 1.6176968745502942, - "timestamp": 10.53040020156816 + "x": 0.5198473628641517, + "y": 6.966928755942856, + "heading": 0.023773564700110772, + "angularVelocity": 0.19310775683593598, + "velocityX": 0.7056706516226394, + "velocityY": -0.11209151592888836, + "timestamp": 0.16414543118319097 }, { - "x": 4.641982693508075, - "y": 1.9626009475761665, - "heading": -0.5965313192258841, - "angularVelocity": -7.568821790279648e-8, - "velocityX": -3.418514871280104, - "velocityY": 1.6176968749712675, - "timestamp": 10.593141694231 + "x": 0.6067218129336875, + "y": 6.9531289220308485, + "heading": 0.04754806846022335, + "angularVelocity": 0.2896760950181983, + "velocityX": 1.0585058559757474, + "velocityY": -0.16814155365198188, + "timestamp": 0.24621814677478646 }, { - "x": 4.427499967793844, - "y": 2.064097664187878, - "heading": -0.5965313239746759, - "angularVelocity": -7.568821822441872e-8, - "velocityX": -3.4185148712800926, - "velocityY": 1.617696874971291, - "timestamp": 10.655883186893842 + "x": 0.7225543617111448, + "y": 6.934728565336742, + "heading": 0.07925217561750111, + "angularVelocity": 0.38629289805690764, + "velocityX": 1.4113405160596262, + "velocityY": -0.22419578250180797, + "timestamp": 0.32829086236638194 }, { - "x": 4.213017242079613, - "y": 2.165594380799589, - "heading": -0.5965313287234677, - "angularVelocity": -7.568821811615467e-8, - "velocityX": -3.4185148712800917, - "velocityY": 1.617696874971293, - "timestamp": 10.718624679556683 + "x": 0.8673449286095066, + "y": 6.9117272629835975, + "heading": 0.11889328943914418, + "angularVelocity": 0.48299990485147826, + "velocityX": 1.764174194245729, + "velocityY": -0.2802551638184133, + "timestamp": 0.4103635779579774 }, { - "x": 3.998534516366738, - "y": 2.267091097414165, - "heading": -0.5965313334722594, - "angularVelocity": -7.56882168459371e-8, - "velocityX": -3.418514871258486, - "velocityY": 1.6176968750169503, - "timestamp": 10.781366172219524 + "x": 1.0410933872613453, + "y": 6.884124518124938, + "heading": 0.16648343863275697, + "angularVelocity": 0.5798534732349732, + "velocityX": 2.117006332731008, + "velocityY": -0.33632059887008814, + "timestamp": 0.49243629354957286 }, { - "x": 3.7840518148370346, - "y": 2.3685878651306878, - "heading": -0.5965313382224369, - "angularVelocity": -7.571030356184493e-8, - "velocityX": -3.418514485817036, - "velocityY": 1.6176976895010247, - "timestamp": 10.844107664882365 + "x": 1.243799549589456, + "y": 6.851919767180746, + "heading": 0.22204089753091144, + "angularVelocity": 0.6769297018830912, + "velocityX": 2.4698361796239694, + "velocityY": -0.39239290075946437, + "timestamp": 0.5745090091411683 }, { - "x": 3.5823467745214304, - "y": 2.46403001529431, - "heading": -0.6258886394381601, - "angularVelocity": -0.46790887449049295, - "velocityX": -3.214858808022383, - "velocityY": 1.5211966772373215, - "timestamp": 10.906849157545206 + "x": 1.4754631289031934, + "y": 6.815112383244328, + "heading": 0.28559273385120976, + "angularVelocity": 0.7743357321882771, + "velocityX": 2.8226625333871667, + "velocityY": -0.44847284107882956, + "timestamp": 0.6565817247327638 }, { - "x": 3.3961574491843427, - "y": 2.552130310684733, - "heading": -0.6529986278149837, - "angularVelocity": -0.43209026795882993, - "velocityX": -2.967562890759225, - "velocityY": 1.40417914288166, - "timestamp": 10.969590650208048 + "x": 1.6781593090979035, + "y": 6.782884452250807, + "heading": 0.3422497740441209, + "angularVelocity": 0.6903273491624664, + "velocityX": 2.4697145541443093, + "velocityY": -0.3926753338331338, + "timestamp": 0.7386544403243592 }, { - "x": 3.225483861981861, - "y": 2.6328887858865286, - "heading": -0.6778585687680168, - "angularVelocity": -0.3962280764760425, - "velocityX": -2.7202666044246855, - "velocityY": 1.2871621597492864, - "timestamp": 11.032332142870889 + "x": 1.8518984587746443, + "y": 6.755259929387126, + "heading": 0.39086385083963315, + "angularVelocity": 0.5923293319234401, + "velocityX": 2.1168929092256374, + "velocityY": -0.33658594899118865, + "timestamp": 0.8207271559159547 }, { - "x": 3.0703260256030673, - "y": 2.7063054766361296, - "heading": -0.7004659993691003, - "angularVelocity": -0.36032662982009556, - "velocityX": -2.4729701158463, - "velocityY": 1.17014574619904, - "timestamp": 11.09507363553373 + "x": 1.9966808291576696, + "y": 6.732239139219163, + "heading": 0.43140805286474204, + "angularVelocity": 0.4940034179795163, + "velocityX": 1.7640743253029585, + "velocityY": -0.28049260953564537, + "timestamp": 0.9027998715075501 }, { - "x": 2.9306839491479293, - "y": 2.7723804156875507, - "heading": -0.7208186227896802, - "angularVelocity": -0.32438857535555815, - "velocityX": -2.2256734822288196, - "velocityY": 1.0531298546958971, - "timestamp": 11.157815128196571 + "x": 2.1125066103123933, + "y": 6.7138223061958735, + "heading": 0.463861007608982, + "angularVelocity": 0.39541709458878044, + "velocityX": 1.4112580571975715, + "velocityY": -0.22439653532283038, + "timestamp": 0.9848725870991456 }, { - "x": 2.8065576397395007, - "y": 2.8311136320269066, - "heading": -0.7389143343573537, - "angularVelocity": -0.28841697574707026, - "velocityX": -1.9783767350811556, - "velocityY": 0.9361144251855119, - "timestamp": 11.220556620859412 + "x": 2.199375929341993, + "y": 6.700009583150749, + "heading": 0.4882072246931022, + "angularVelocity": 0.29664203150374724, + "velocityX": 1.0584433377576143, + "velocityY": -0.16829859893826818, + "timestamp": 1.0669453026907412 }, { - "x": 2.697947103240289, - "y": 2.8825051506567756, - "heading": -0.7547512635952348, - "angularVelocity": -0.25241556370017226, - "velocityX": -1.7310798944944172, - "velocityY": 0.8190993941766099, - "timestamp": 11.283298113522253 + "x": 2.257288845375976, + "y": 6.690801066111525, + "heading": 0.5044377142261048, + "angularVelocity": 0.19775743273523133, + "velocityX": 0.7056293387703308, + "velocityY": -0.11219949252132488, + "timestamp": 1.1490180182823366 }, { - "x": 2.6048523446905505, - "y": 2.9265549925489487, - "heading": -0.7683278153992709, - "angularVelocity": -0.21638872822158642, - "velocityX": -1.4837829735739574, - "velocityY": 0.702084697424841, - "timestamp": 11.346039606185094 + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.0988477756026137, + "velocityX": 0.3528151893675758, + "velocityY": -0.0560997894575031, + "timestamp": 1.231090733873932 }, { - "x": 2.527273368620795, - "y": 2.9632631746604594, - "heading": -0.779642706302846, - "angularVelocity": -0.18034143631837077, - "velocityX": -1.2364859804443729, - "velocityY": 0.5850702709413225, - "timestamp": 11.408781098847935 + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0, + "velocityX": 3.087311543739499e-36, + "velocityY": 0, + "timestamp": 1.3131634494655275 }, { - "x": 2.4652101792880994, - "y": 2.9926297099741825, - "heading": -0.7886949946743065, - "angularVelocity": -0.1442791362982944, - "velocityX": -0.9891889194637066, - "velocityY": 0.46805605138424683, - "timestamp": 11.471522591510777 + "x": 2.3177158256290964, + "y": 6.704097470773247, + "heading": 0.5125504196, + "angularVelocity": -9.1973743139294e-17, + "velocityX": 0.34546318060283326, + "velocityY": 0.1965022887733569, + "timestamp": 1.4042599289943862 }, { - "x": 2.4186627808554446, - "y": 3.014654607545873, - "heading": -0.7954841045559687, - "angularVelocity": -0.10820765642515946, - "velocityX": -0.7418917921317392, - "velocityY": 0.3510419761615724, - "timestamp": 11.534264084173618 + "x": 2.380656782769881, + "y": 6.7398988031009575, + "heading": 0.5125504196, + "angularVelocity": -1.3336684994607708e-16, + "velocityX": 0.6909263394843395, + "velocityY": 0.3930045651914449, + "timestamp": 1.495356408523245 }, { - "x": 2.3876311775224326, - "y": 3.0293378725476474, - "heading": -0.8000098431420335, - "angularVelocity": -0.07213310353301693, - "velocityX": -0.49459459786475296, - "velocityY": 0.23402798337424993, - "timestamp": 11.597005576836459 + "x": 2.475068211555481, + "y": 6.793600797653198, + "heading": 0.5125504196, + "angularVelocity": 6.653839443641591e-19, + "velocityX": 1.0363894332018742, + "velocityY": 0.5895068045437317, + "timestamp": 1.5864528880521036 }, { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -0.036061762982130895, - "velocityX": -0.24729733470577522, - "velocityY": 0.11701401166345779, - "timestamp": 11.6597470694993 + "x": 2.569479640341081, + "y": 6.847302792205439, + "heading": 0.5125504196, + "angularVelocity": 5.174999729369457e-17, + "velocityX": 1.0363894332018742, + "velocityY": 0.5895068045437317, + "timestamp": 1.6775493675809623 }, { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": 2.8180807610025885e-36, - "velocityX": -1.7033240054088048e-37, - "velocityY": 8.364918339692578e-38, - "timestamp": 11.722488562162141 + "x": 2.6324205974818655, + "y": 6.8831041245331495, + "heading": 0.5125504196, + "angularVelocity": -1.3901806543706914e-17, + "velocityX": 0.6909263394843393, + "velocityY": 0.39300456519144494, + "timestamp": 1.768645847109821 }, { - "x": 2.3862784269967885, - "y": 3.0299921206795677, - "heading": -0.7979445022798209, - "angularVelocity": 0.07214760590413402, - "velocityX": 0.2361025217555023, - "velocityY": -0.11148080618080956, - "timestamp": 11.782475441082498 + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "angularVelocity": -1.8725484396545262e-17, + "velocityX": 0.3454631806028332, + "velocityY": 0.19650228877335693, + "timestamp": 1.8597423266386797 }, { - "x": 2.414605712310544, - "y": 3.016617241283919, - "heading": -0.7893555783917804, - "angularVelocity": 0.1431800427464117, - "velocityX": 0.4722246901920792, - "velocityY": -0.22296341527296182, - "timestamp": 11.842462320002856 + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "angularVelocity": 1.431796971368011e-33, + "velocityX": 3.743360269099248e-35, + "velocityY": -8.057233109044706e-35, + "timestamp": 1.9508388061675384 }, { - "x": 2.457098595113841, - "y": 2.996554734272641, - "heading": -0.7765849369819304, - "angularVelocity": 0.2128905794016223, - "velocityX": 0.7083696229589314, - "velocityY": -0.33444825555792773, - "timestamp": 11.902449198923213 + "x": 2.6835271230542426, + "y": 6.903699548014665, + "heading": 0.5052880772246282, + "angularVelocity": -0.10752621184517612, + "velocityX": 0.2907312178663514, + "velocityY": 0.039898557617057115, + "timestamp": 2.018379011023157 }, { - "x": 2.5137586742709717, - "y": 2.9698044458506936, - "heading": -0.759727315329062, - "angularVelocity": 0.281021816041631, - "velocityX": 0.944541209292721, - "velocityY": -0.44593565965421617, - "timestamp": 11.96243607784357 + "x": 2.7228033814945674, + "y": 6.909089957964811, + "heading": 0.49095749727137794, + "angularVelocity": -0.21217850884350908, + "velocityX": 0.5815241236576919, + "velocityY": 0.0798103879262595, + "timestamp": 2.0859192158787754 }, { - "x": 2.5845878442638655, - "y": 2.9363662114763, - "heading": -0.7388971783301493, - "angularVelocity": 0.34724488711220036, - "velocityX": 1.180744377231747, - "velocityY": -0.5574258067133014, - "timestamp": 12.022422956763927 + "x": 2.7817246923505206, + "y": 6.91717710303389, + "heading": 0.46979167660622734, + "angularVelocity": -0.31338105518627096, + "velocityX": 0.8723886902906202, + "velocityY": 0.11973823719319023, + "timestamp": 2.153459420734394 }, { - "x": 2.669588381015972, - "y": 2.8962398712988433, - "heading": -0.7142349782126004, - "angularVelocity": 0.41112657570152883, - "velocityX": 1.4169854855252446, - "velocityY": -0.6689186185320778, - "timestamp": 12.082409835684285 + "x": 2.860296723174767, + "y": 6.927962260865171, + "heading": 0.4420737054643263, + "angularVelocity": -0.41039216865205086, + "velocityX": 1.1633371706853932, + "velocityY": 0.15968500324119947, + "timestamp": 2.220999625590012 }, { - "x": 2.768763062860025, - "y": 2.849425297465716, - "heading": -0.6859166617272396, - "angularVelocity": 0.4720751770225952, - "velocityX": 1.653272909492815, - "velocityY": -0.7804135616937369, - "timestamp": 12.142396714604642 + "x": 2.9585261643123166, + "y": 6.941446920815512, + "heading": 0.40815310467896726, + "angularVelocity": -0.502228278073356, + "velocityX": 1.454384708301312, + "velocityY": 0.19965382070083557, + "timestamp": 2.2885398304456306 + }, + { + "x": 3.0764209718394073, + "y": 6.9576328058659405, + "heading": 0.3684706356327604, + "angularVelocity": -0.5875384762459123, + "velocityX": 1.7455500435498434, + "velocityY": 0.23964814861057657, + "timestamp": 2.356080035301249 }, { - "x": 2.8821153449579997, - "y": 2.795922445610523, - "heading": -0.6541688123580982, - "angularVelocity": 0.5292465609236296, - "velocityX": 1.8896179320892514, - "velocityY": -0.8919092444570627, - "timestamp": 12.202383593525 + "x": 3.2139906503876703, + "y": 6.9765218958360915, + "heading": 0.323597871318007, + "angularVelocity": -0.6643859670055544, + "velocityX": 2.0368561043358877, + "velocityY": 0.27967178972185397, + "timestamp": 2.4236202401568674 }, { - "x": 3.0096496153353156, - "y": 2.73573145985777, - "heading": -0.6192942663212868, - "angularVelocity": 0.5813695705541383, - "velocityX": 2.1260361044394105, - "velocityY": -1.0034025246198655, - "timestamp": 12.262370472445356 + "x": 3.3712465090237984, + "y": 6.998116434011635, + "heading": 0.27430452270447037, + "angularVelocity": -0.7298371202591344, + "velocityX": 2.3283296071176522, + "velocityY": 0.3197286449116718, + "timestamp": 2.491160445012486 }, { - "x": 3.151371569227902, - "y": 2.6688529097388005, - "heading": -0.5817190571900679, - "angularVelocity": 0.6263904675071726, - "velocityX": 2.3625492181506123, - "velocityY": -1.1148863105173612, - "timestamp": 12.322357351365714 + "x": 3.5482015820439243, + "y": 7.022418860702876, + "heading": 0.22168358710657873, + "angularVelocity": -0.7791053596947181, + "velocityX": 2.619996095635254, + "velocityY": 0.3598216313260006, + "timestamp": 2.558700649868104 }, { - "x": 3.30728870131474, - "y": 2.5952884046594, - "heading": -0.5420886244414685, - "angularVelocity": 0.6606516868666429, - "velocityX": 2.5991872705003334, - "velocityY": -1.2263432671179575, - "timestamp": 12.382344230286071 + "x": 3.7448688058303015, + "y": 7.04943148246024, + "heading": 0.16741520227188003, + "angularVelocity": -0.8034974864335817, + "velocityX": 2.911854120176202, + "velocityY": 0.3999487685166161, + "timestamp": 2.6262408547237226 }, { - "x": 3.477410464899007, - "y": 2.5150425607670805, - "heading": -0.501501110267189, - "angularVelocity": 0.6766065330414338, - "velocityX": 2.8359829123654157, - "velocityY": -1.3377232710983202, - "timestamp": 12.442331109206428 + "x": 3.961249724160237, + "y": 7.079155012062387, + "heading": 0.1144423105156959, + "angularVelocity": -0.7843164211512963, + "velocityX": 3.2037350018777047, + "velocityY": 0.4400864591051642, + "timestamp": 2.693781059579341 }, { - "x": 3.661742704544347, - "y": 2.428132084038069, - "heading": -0.46226256646132613, - "angularVelocity": 0.6541187758402722, - "velocityX": 3.0728759849311573, - "velocityY": -1.4488247812392314, - "timestamp": 12.502317988126785 + "x": 4.197248092224393, + "y": 7.111577952654102, + "heading": 0.06945280731873603, + "angularVelocity": -0.6661144024234814, + "velocityX": 3.494190883321295, + "velocityY": 0.4800539273019009, + "timestamp": 2.7613212644349594 }, { - "x": 3.860146403975295, - "y": 2.3346891355318427, - "heading": -0.4327535611710109, - "angularVelocity": 0.4919243311440368, - "velocityX": 3.307451612782871, - "velocityY": -1.5577231252568988, - "timestamp": 12.562304867047143 + "x": 4.450279816565845, + "y": 7.14615999986413, + "heading": 0.06926458523360347, + "angularVelocity": -0.0027868154314146247, + "velocityX": 3.746386687490236, + "velocityY": 0.5120216511625231, + "timestamp": 2.828861469290578 }, { - "x": 4.065242173401202, - "y": 2.237711412138144, - "heading": -0.4327535477418668, - "angularVelocity": 2.238680243649846e-7, - "velocityX": 3.41901050891823, - "velocityY": -1.6166489262168846, - "timestamp": 12.6222917459675 + "x": 4.703335353284152, + "y": 7.180937109425349, + "heading": 0.06926456657564138, + "angularVelocity": -2.762497114544702e-7, + "velocityX": 3.746739253445653, + "velocityY": 0.5149097435455242, + "timestamp": 2.896401674146196 }, { - "x": 4.270337924993822, - "y": 2.140733651028875, - "heading": -0.4327535343131544, - "angularVelocity": 2.2386082909736944e-7, - "velocityX": 3.419010211631728, - "velocityY": -1.6166495549472233, - "timestamp": 12.682278624887857 + "x": 4.956390888818978, + "y": 7.215714227598158, + "heading": 0.06926454791768001, + "angularVelocity": -2.7624970048363726e-7, + "velocityX": 3.746739235923052, + "velocityY": 0.5149098710487069, + "timestamp": 2.9639418790018146 }, { - "x": 4.475433676585232, - "y": 2.0437558899170503, - "heading": -0.43275352088444197, - "angularVelocity": 2.2386082765717578e-7, - "velocityX": 3.419010211611586, - "velocityY": -1.6166495549898217, - "timestamp": 12.742265503808214 + "x": 5.209446424353711, + "y": 7.250491345771652, + "heading": 0.0692645292597187, + "angularVelocity": -2.7624969990656975e-7, + "velocityX": 3.7467392359216607, + "velocityY": 0.5149098710588286, + "timestamp": 3.031482083857433 }, { - "x": 4.6805294281766425, - "y": 1.9467781288052255, - "heading": -0.43275350745572955, - "angularVelocity": 2.2386082872977313e-7, - "velocityX": 3.4190102116115844, - "velocityY": -1.616649554989824, - "timestamp": 12.802252382728572 + "x": 5.462501959888314, + "y": 7.285268463946087, + "heading": 0.0692645106017574, + "angularVelocity": -2.7624969939846597e-7, + "velocityX": 3.7467392359197462, + "velocityY": 0.5149098710727624, + "timestamp": 3.0990222887130514 }, { - "x": 4.885625179768054, - "y": 1.8498003676934025, - "heading": -0.4327534940270171, - "angularVelocity": 2.2386082836686737e-7, - "velocityX": 3.419010211611597, - "velocityY": -1.6166495549897981, - "timestamp": 12.862239261648929 + "x": 5.715557495424461, + "y": 7.320045582109288, + "heading": 0.06926449194379608, + "angularVelocity": -2.762496999067462e-7, + "velocityX": 3.746739235942603, + "velocityY": 0.5149098709064452, + "timestamp": 3.16656249356867 }, { - "x": 5.090720931370458, - "y": 1.7528226066048298, - "heading": -0.4327534805983047, - "angularVelocity": 2.2386082909228912e-7, - "velocityX": 3.4190102117948658, - "velocityY": -1.616649554602208, - "timestamp": 12.922226140569286 + "x": 5.968613025759916, + "y": 7.354822738115272, + "heading": 0.06926447328583468, + "angularVelocity": -2.762497009102298e-7, + "velocityX": 3.7467391589411925, + "velocityY": 0.5149104312065347, + "timestamp": 3.234102698424288 }, { - "x": 5.295816845235169, - "y": 1.6558451886813434, - "heading": -0.4327534671695918, - "angularVelocity": 2.2386083672590555e-7, - "velocityX": 3.4190129167581613, - "velocityY": -1.6166438339330798, - "timestamp": 12.982213019489643 + "x": 6.221690653746151, + "y": 7.389438720345797, + "heading": 0.06926445462738323, + "angularVelocity": -2.7625695670270355e-7, + "velocityX": 3.74706633666926, + "velocityY": 0.5125240929387752, + "timestamp": 3.3016429032799066 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.4327534537085892, - "angularVelocity": 2.2439911588435075e-7, - "velocityX": 3.4538678416562716, - "velocityY": -1.5407736813730113, - "timestamp": 13.04219989841 + "x": 6.476102895472122, + "y": 7.412263085598601, + "heading": 0.06926437909315673, + "angularVelocity": -0.0000011183594520291744, + "velocityX": 3.766826622303432, + "velocityY": 0.33793745964490024, + "timestamp": 3.369183108135525 }, { - "x": 5.767637732978587, - "y": 1.4715486447521433, - "heading": -0.43275344189521775, - "angularVelocity": 1.5949042736003556e-7, - "velocityX": 3.5727828839624562, - "velocityY": -1.2403266728714895, - "timestamp": 13.116269368711977 + "x": 6.712136074986816, + "y": 7.430641724558031, + "heading": 0.04341074490415746, + "angularVelocity": -0.38278880326565307, + "velocityX": 3.4947063015172035, + "velocityY": 0.27211405412107026, + "timestamp": 3.4367233129911434 }, { - "x": 6.033774192023134, - "y": 1.384125705645109, - "heading": -0.43275343009903516, - "angularVelocity": 1.5925836231633141e-7, - "velocityX": 3.5930655094403474, - "velocityY": -1.1802830336252825, - "timestamp": 13.190338839013954 + "x": 6.9285315703702866, + "y": 7.445814297629393, + "heading": 0.019185726579151432, + "angularVelocity": -0.3586755233685199, + "velocityX": 3.203950829673405, + "velocityY": 0.2246450555457597, + "timestamp": 3.504263517846762 }, { - "x": 6.2999107025526015, - "y": 1.2967029232707166, - "heading": -0.43275341830285263, - "angularVelocity": 1.5925836222768275e-7, - "velocityX": 3.593066204529984, - "velocityY": -1.1802809176031077, - "timestamp": 13.26440830931593 + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, + "angularVelocity": -0.2840637901552857, + "velocityX": 2.913217404608591, + "velocityY": 0.18042146002750595, + "timestamp": 3.5718037227023802 }, { - "x": 6.566047160708243, - "y": 1.2092799814598572, - "heading": -0.4327534065048956, - "angularVelocity": 1.5928231960994732e-7, - "velocityX": 3.5930654974393894, - "velocityY": -1.1802830701291929, - "timestamp": 13.338477779617907 + "x": 7.284551146493795, + "y": 7.4665356658331685, + "heading": -0.011983560395956015, + "angularVelocity": -0.19978893791899283, + "velocityX": 2.6551742811725556, + "velocityY": 0.1423059220209307, + "timestamp": 3.6317848233617918 }, { - "x": 6.815604168443372, - "y": 1.1270295237235333, - "heading": -0.36800659105834677, - "angularVelocity": 0.874136336908857, - "velocityX": 3.369229005117782, - "velocityY": -1.1104501949452927, - "timestamp": 13.412547249919884 + "x": 7.428286998104642, + "y": 7.47296871812108, + "heading": -0.020071347748117502, + "angularVelocity": -0.13483892864997732, + "velocityX": 2.396352351501819, + "velocityY": 0.10725132111930623, + "timestamp": 3.6917659240212033 }, { - "x": 7.0424486777806585, - "y": 1.0523256982782683, - "heading": -0.30458898646479193, - "angularVelocity": 0.8561908750664163, - "velocityX": 3.0625912189253857, - "velocityY": -1.0085643267152118, - "timestamp": 13.48661672022186 + "x": 7.556472866857842, + "y": 7.4774155432867, + "heading": -0.02498803078182015, + "angularVelocity": -0.08197053704667534, + "velocityX": 2.137104310257201, + "velocityY": 0.0741371051336659, + "timestamp": 3.751747024680615 }, { - "x": 7.2465905745815595, - "y": 0.9851213052520007, - "heading": -0.2456798424413219, - "angularVelocity": 0.7953228743678266, - "velocityX": 2.756086900157761, - "velocityY": -0.9073156963628867, - "timestamp": 13.560686190523837 + "x": 7.669092898448606, + "y": 7.47995645101358, + "heading": -0.027230015787281982, + "angularVelocity": -0.037378190476904, + "velocityX": 1.877591947341067, + "velocityY": 0.04236180561785803, + "timestamp": 3.8117281253400264 }, { - "x": 7.428038548794458, - "y": 0.9253999088853845, - "heading": -0.1923064776337199, - "angularVelocity": 0.7205852099387569, - "velocityX": 2.449699902984963, - "velocityY": -0.8062889625528059, - "timestamp": 13.634755660825814 + "x": 7.766136403587462, + "y": 7.480650177988498, + "heading": -0.027158837231048453, + "angularVelocity": 0.001186683062681687, + "velocityX": 1.6179013734658474, + "velocityY": 0.011565759335732523, + "timestamp": 3.871709225999438 }, { - "x": 7.586797949690042, - "y": 0.8731533407207944, - "heading": -0.14497999296920738, - "angularVelocity": 0.6389472541327177, - "velocityX": 2.1433851254549703, - "velocityY": -0.7053725097747339, - "timestamp": 13.70882513112779 + "x": 7.847595735447885, + "y": 7.479541543526011, + "heading": -0.025049657080079103, + "angularVelocity": 0.03516407881452247, + "velocityX": 1.3580833123248393, + "velocityY": -0.018483063003166142, + "timestamp": 3.9316903266588494 }, { - "x": 7.722872327084394, - "y": 0.8283767866857153, - "heading": -0.10400779573923787, - "angularVelocity": 0.5531590419497899, - "velocityX": 1.837118273420693, - "velocityY": -0.6045210510150557, - "timestamp": 13.782894601429767 + "x": 7.913465176456017, + "y": 7.4766658686921605, + "heading": -0.021118970688138776, + "angularVelocity": 0.06553208175121362, + "velocityX": 1.0981699282605077, + "velocityY": -0.04794301542047442, + "timestamp": 3.991671427318261 }, { - "x": 7.836264194581955, - "y": 0.7910670998170554, - "heading": -0.06959570799723021, - "angularVelocity": 0.46459205934255987, - "velocityX": 1.5308853571555237, - "velocityY": -0.5037120788977025, - "timestamp": 13.856964071731744 + "x": 7.963740302847386, + "y": 7.472051704673601, + "heading": -0.015541591171907823, + "angularVelocity": 0.09298561471722216, + "velocityX": 0.8381827915570343, + "velocityY": -0.07692696479112789, + "timestamp": 4.051652527977672 }, { - "x": 7.926975414267803, - "y": 0.7612220655404804, - "heading": -0.04189104317473832, - "angularVelocity": 0.3740362218001813, - "velocityX": 1.2246775806013535, - "velocityY": -0.4029330053920843, - "timestamp": 13.93103354203372 + "x": 7.998417597522527, + "y": 7.465722607991469, + "heading": -0.008461644038298986, + "angularVelocity": 0.11803629903043378, + "velocityX": 0.5781370180592035, + "velocityY": -0.10551818176978627, + "timestamp": 4.111633628637083 }, { - "x": 7.99500740651227, - "y": 0.7388400306235591, - "heading": -0.021003806792755153, - "angularVelocity": 0.28199521741990785, - "velocityX": 0.9184889802384881, - "velocityY": -0.30217625191150127, - "timestamp": 14.005103012335697 + "x": 8.017494201660156, + "y": 7.457698345184326, + "heading": 0, + "angularVelocity": 0.14107183671647575, + "velocityX": 0.3180435825269393, + "velocityY": -0.1337798526356988, + "timestamp": 4.171614729296494 }, { - "x": 8.040361272813582, - "y": 0.7239196956147146, - "heading": -0.007018297206773477, - "angularVelocity": 0.18881611450660732, - "velocityX": 0.6123152510259342, - "velocityY": -0.20143704211755917, - "timestamp": 14.079172482637674 + "x": 8.008975668857596, + "y": 7.440959395735479, + "heading": 0.016440344425850486, + "angularVelocity": 0.17454816573497248, + "velocityX": -0.09044179592138195, + "velocityY": -0.17771847395317164, + "timestamp": 4.265802739560738 }, { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": 2.0779165225884802e-41, - "angularVelocity": 0.09475290127174345, - "velocityX": 0.3061531209608937, - "velocityY": -0.10071229126619018, - "timestamp": 14.15324195293965 + "x": 7.961981832760565, + "y": 7.4200887947149115, + "heading": 0.0360050890089686, + "angularVelocity": 0.20772011775415367, + "velocityX": -0.4989364990850843, + "velocityY": -0.22158447728129313, + "timestamp": 4.3599907498249815 }, { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": 1.169210048317657e-41, - "angularVelocity": -1.2268302369298258e-40, - "velocityX": -9.944733020002764e-42, - "velocityY": 5.460591898189625e-42, - "timestamp": 14.227311423241627 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 7.876511570645417, + "y": 7.39509531765153, + "heading": 0.058657775076795544, + "angularVelocity": 0.24050498576490828, + "velocityX": -0.9074431222759898, + "velocityY": -0.2653573102697685, + "timestamp": 4.454178760089225 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 7.752563396875145, + "y": 7.365990633137338, + "heading": 0.08435039467917142, + "angularVelocity": 0.2727801503641011, + "velocityX": -1.315965518567977, + "velocityY": -0.3090062570866462, + "timestamp": 4.548366770353469 }, { - "scope": [ - 6 - ], - "type": "StopPoint" + "x": 7.590135252234061, + "y": 7.332791018782474, + "heading": 0.11301654484596554, + "angularVelocity": 0.3043503104733978, + "velocityX": -1.7245097776818146, + "velocityY": -0.35248238349789773, + "timestamp": 4.642554780617712 }, { - "scope": [ - 1 - ], - "type": "StopPoint" + "x": 7.3892240901384, + "y": 7.295520773303632, + "heading": 0.14455765680612448, + "angularVelocity": 0.33487395977121315, + "velocityX": -2.13308638256617, + "velocityY": -0.39570052891318314, + "timestamp": 4.736742790881956 }, { - "scope": [ - 11 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "AmpLanePADESprint": { - "waypoints": [ + "x": 7.149824938411661, + "y": 7.254220108341429, + "heading": 0.1788109128374065, + "angularVelocity": 0.36366896312157765, + "velocityX": -2.54171577735964, + "velocityY": -0.4384917448232911, + "timestamp": 4.8309308011461995 + }, { - "x": 0.43297290802001953, - "y": 6.9807281494140625, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 6.871928226350113, + "y": 7.208968463075644, + "heading": 0.21545413209734332, + "angularVelocity": 0.3890433523028524, + "velocityX": -2.9504467849136042, + "velocityY": -0.4804395499897761, + "timestamp": 4.925118811410443 }, { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 7 + "x": 6.555508023323144, + "y": 7.15999689291417, + "heading": 0.25354713749374036, + "angularVelocity": 0.40443582245264, + "velocityX": -3.3594530995957568, + "velocityY": -0.5199342254293745, + "timestamp": 5.019306821674687 }, { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 24 + "x": 6.201946981022424, + "y": 7.116596127430346, + "heading": 0.25354714311943394, + "angularVelocity": 5.972834101491316e-8, + "velocityX": -3.753779714730222, + "velocityY": -0.46078864350212084, + "timestamp": 5.11349483193893 }, { - "x": 7.125290870666504, - "y": 7.458, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "x": 5.847829680383322, + "y": 7.077995922009697, + "heading": 0.25354714640605686, + "angularVelocity": 3.4894280958051e-8, + "velocityX": -3.7596855443238373, + "velocityY": -0.4098207968546854, + "timestamp": 5.207682842203174 }, { - "x": 8.017494201660156, - "y": 7.457698345184326, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 23 + "x": 5.493712379149646, + "y": 7.039395722043647, + "heading": 0.25354714969267966, + "angularVelocity": 3.4894280048226024e-8, + "velocityX": -3.759685550636459, + "velocityY": -0.40982073894285953, + "timestamp": 5.301870852467418 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 5.13959507794107, + "y": 7.0007955218474756, + "heading": 0.25354715297933, + "angularVelocity": 3.489457219723442e-8, + "velocityX": -3.7596855503699804, + "velocityY": -0.4098207413860709, + "timestamp": 5.396058862731661 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 14 + "x": 4.793375065328026, + "y": 6.963049240971564, + "heading": 0.2809926873119857, + "angularVelocity": 0.2913909557666365, + "velocityX": -3.675839543076952, + "velocityY": -0.4007546265178986, + "timestamp": 5.490246872995905 }, { - "x": 7.214789390563965, - "y": 6.145846366882324, - "heading": -0.344987478573796, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "x": 4.4856271372174605, + "y": 6.929500155423233, + "heading": 0.3062072463057812, + "angularVelocity": 0.2677045509620205, + "velocityX": -3.2673790140292933, + "velocityY": -0.3561927410315718, + "timestamp": 5.5844348832601485 }, { - "x": 7.985864639282227, - "y": 5.863699913024902, - "heading": -0.344987478573796, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 4.216349125910067, + "y": 6.900146018877801, + "heading": 0.32859640421091696, + "angularVelocity": 0.23770709076795637, + "velocityX": -2.858941499580855, + "velocityY": -0.31165470491497754, + "timestamp": 5.678622893524392 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 17 + "x": 3.9855401854111148, + "y": 6.874986045056756, + "heading": 0.34796083888612495, + "angularVelocity": 0.2055934149249067, + "velocityX": -2.450512967111424, + "velocityY": -0.26712501676655553, + "timestamp": 5.772810903788636 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 20 + "x": 3.7931998687162816, + "y": 6.854019825581219, + "heading": 0.36419988308525797, + "angularVelocity": 0.17241094862896578, + "velocityX": -2.042089180514842, + "velocityY": -0.22259966440226955, + "timestamp": 5.866998914052879 }, { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ + "x": 3.639327899901621, + "y": 6.837247108195556, + "heading": 0.37725261237367164, + "angularVelocity": 0.13858164379727655, + "velocityX": -1.6336683234200817, + "velocityY": -0.17807699025180046, + "timestamp": 5.961186924317123 + }, { - "x": 0.43297290802001953, - "y": 6.9807281494140625, - "heading": 0, - "angularVelocity": 2.735053895521923e-38, - "velocityX": -3.8673469053295564e-39, - "velocityY": 1.858643532249755e-38, - "timestamp": 0 + "x": 3.5239240923326007, + "y": 6.824667722043819, + "heading": 0.38707830352299477, + "angularVelocity": 0.10431997790119188, + "velocityX": -1.2252494478358333, + "velocityY": -0.13355613008965372, + "timestamp": 6.0553749345813666 }, { - "x": 0.46193105617219093, - "y": 6.976128411049919, - "heading": 0.007924686694784022, - "angularVelocity": 0.0965568963773332, - "velocityX": 0.35283526252830383, - "velocityY": -0.05604467125266167, - "timestamp": 0.08207271559159549 + "x": 3.4469883122501845, + "y": 6.8162815454148475, + "heading": 0.39364810050149374, + "angularVelocity": 0.06975194571015403, + "velocityX": -0.8168319923796448, + "velocityY": -0.08903656214250527, + "timestamp": 6.14956294484561 }, { - "x": 0.5198473628641517, - "y": 6.966928755942856, - "heading": 0.023773564700110772, - "angularVelocity": 0.19310775683593598, - "velocityX": 0.7056706516226394, - "velocityY": -0.11209151592888836, - "timestamp": 0.16414543118319097 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.03495947190378589, + "velocityX": -0.4084155935928461, + "velocityY": -0.04451793673752315, + "timestamp": 6.243750955109854 }, { - "x": 0.6067218129336875, - "y": 6.9531289220308485, - "heading": 0.04754806846022335, - "angularVelocity": 0.2896760950181983, - "velocityX": 1.0585058559757474, - "velocityY": -0.16814155365198188, - "timestamp": 0.24621814677478646 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0, + "velocityX": -1.9952146739162206e-34, + "velocityY": -3.951795765453016e-33, + "timestamp": 6.337938965374097 }, { - "x": 0.7225543617111448, - "y": 6.934728565336742, - "heading": 0.07925217561750111, - "angularVelocity": 0.38629289805690764, - "velocityX": 1.4113405160596262, - "velocityY": -0.22419578250180797, - "timestamp": 0.32829086236638194 + "x": 3.425127749537185, + "y": 6.810478633677757, + "heading": 0.3886892234850989, + "angularVelocity": -0.13293807639169805, + "velocityX": 0.2675517930120232, + "velocityY": -0.025935588272518423, + "timestamp": 6.400010272097332 }, { - "x": 0.8673449286095066, - "y": 6.9117272629835975, - "heading": 0.11889328943914418, - "angularVelocity": 0.48299990485147826, - "velocityX": 1.764174194245729, - "velocityY": -0.2802551638184133, - "timestamp": 0.4103635779579774 + "x": 3.458347138585669, + "y": 6.807258412874914, + "heading": 0.3723514064813247, + "angularVelocity": -0.26321045691257755, + "velocityX": 0.535181081278085, + "velocityY": -0.05187937829625554, + "timestamp": 6.462081578820567 }, { - "x": 1.0410933872613453, - "y": 6.884124518124938, - "heading": 0.16648343863275697, - "angularVelocity": 0.5798534732349732, - "velocityX": 2.117006332731008, - "velocityY": -0.33632059887008814, - "timestamp": 0.49243629354957286 + "x": 3.508184213080087, + "y": 6.802427294600358, + "heading": 0.34812438720722594, + "angularVelocity": -0.3903094771652888, + "velocityX": 0.8029003596884762, + "velocityY": -0.07783174754300737, + "timestamp": 6.524152885543802 }, { - "x": 1.243799549589456, - "y": 6.851919767180746, - "heading": 0.22204089753091144, - "angularVelocity": 0.6769297018830912, - "velocityX": 2.4698361796239694, - "velocityY": -0.39239290075946437, - "timestamp": 0.5745090091411683 + "x": 3.5746455076736363, + "y": 6.795984642746885, + "heading": 0.31624502791908643, + "angularVelocity": -0.5135925272251962, + "velocityX": 1.070724914651617, + "velocityY": -0.10379436479708294, + "timestamp": 6.586224192267037 }, { - "x": 1.4754631289031934, - "y": 6.815112383244328, - "heading": 0.28559273385120976, - "angularVelocity": 0.7743357321882771, - "velocityX": 2.8226625333871667, - "velocityY": -0.44847284107882956, - "timestamp": 0.6565817247327638 + "x": 3.6577387353786484, + "y": 6.787929625128416, + "heading": 0.2770021560106679, + "angularVelocity": -0.6322224225666108, + "velocityX": 1.3386737301264124, + "velocityY": -0.12977038898779852, + "timestamp": 6.648295498990271 }, { - "x": 1.6781593090979035, - "y": 6.782884452250807, - "heading": 0.3422497740441209, - "angularVelocity": 0.6903273491624664, - "velocityX": 2.4697145541443093, - "velocityY": -0.3926753338331338, - "timestamp": 0.7386544403243592 + "x": 3.7574730901724314, + "y": 6.7782611086258875, + "heading": 0.23075444743671972, + "angularVelocity": -0.7450738677080124, + "velocityX": 1.6067706652041787, + "velocityY": -0.1557646682971112, + "timestamp": 6.710366805713506 }, { - "x": 1.8518984587746443, - "y": 6.755259929387126, - "heading": 0.39086385083963315, - "angularVelocity": 0.5923293319234401, - "velocityX": 2.1168929092256374, - "velocityY": -0.33658594899118865, - "timestamp": 0.8207271559159547 + "x": 3.873859634378807, + "y": 6.766977536857145, + "heading": 0.17795757602156642, + "angularVelocity": -0.8505841781386996, + "velocityX": 1.875045819887845, + "velocityY": -0.18178402170643457, + "timestamp": 6.772438112436741 }, { - "x": 1.9966808291576696, - "y": 6.732239139219163, - "heading": 0.43140805286474204, - "angularVelocity": 0.4940034179795163, - "velocityX": 1.7640743253029585, - "velocityY": -0.28049260953564537, - "timestamp": 0.9027998715075501 + "x": 4.006911762109537, + "y": 6.754076769706622, + "heading": 0.11920696294593584, + "angularVelocity": -0.9465019535937051, + "velocityX": 2.1435367604550115, + "velocityY": -0.2078378534549514, + "timestamp": 6.834509419159976 }, { - "x": 2.1125066103123933, - "y": 6.7138223061958735, - "heading": 0.463861007608982, - "angularVelocity": 0.39541709458878044, - "velocityX": 1.4112580571975715, - "velocityY": -0.22439653532283038, - "timestamp": 0.9848725870991456 + "x": 4.156645657189765, + "y": 6.73955582918857, + "heading": 0.055309270826440785, + "angularVelocity": -1.029423988195197, + "velocityX": 2.4122884305927053, + "velocityY": -0.23393966205310818, + "timestamp": 6.896580725883211 }, { - "x": 2.199375929341993, - "y": 6.700009583150749, - "heading": 0.4882072246931022, - "angularVelocity": 0.29664203150374724, - "velocityX": 1.0584433377576143, - "velocityY": -0.16829859893826818, - "timestamp": 1.0669453026907412 + "x": 4.323080375842652, + "y": 6.723410435666096, + "heading": -0.012586184407094544, + "angularVelocity": -1.0938299645646508, + "velocityX": 2.681347106079046, + "velocityY": -0.2601104177560936, + "timestamp": 6.958652032606445 }, { - "x": 2.257288845375976, - "y": 6.690801066111525, - "heading": 0.5044377142261048, - "angularVelocity": 0.19775743273523133, - "velocityX": 0.7056293387703308, - "velocityY": -0.11219949252132488, - "timestamp": 1.1490180182823366 + "x": 4.506235840968647, + "y": 6.705634179985004, + "heading": -0.08270962096165607, + "angularVelocity": -1.1297238652836274, + "velocityX": 2.9507267495214404, + "velocityY": -0.2863844281597597, + "timestamp": 7.02072333932968 }, { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": 0.0988477756026137, - "velocityX": 0.3528151893675758, - "velocityY": -0.0560997894575031, - "timestamp": 1.231090733873932 + "x": 4.706118949857515, + "y": 6.686217722826257, + "heading": -0.15194437984101536, + "angularVelocity": -1.1154068205470853, + "velocityX": 3.2202175117742615, + "velocityY": -0.3128088996947802, + "timestamp": 7.082794646052915 }, { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": 0, - "velocityX": 3.087311543739499e-36, - "velocityY": 0, - "timestamp": 1.3131634494655275 + "x": 4.922607059053276, + "y": 6.66515425622058, + "heading": -0.21301282950448638, + "angularVelocity": -0.9838434678965439, + "velocityX": 3.4877324262085807, + "velocityY": -0.3393430510421988, + "timestamp": 7.14486595277615 }, { - "x": 2.3177158256290964, - "y": 6.704097470773247, - "heading": 0.5125504196, - "angularVelocity": -9.1973743139294e-17, - "velocityX": 0.34546318060283326, - "velocityY": 0.1965022887733569, - "timestamp": 1.4042599289943862 + "x": 5.152321807662402, + "y": 6.642265811014449, + "heading": -0.22692060137612477, + "angularVelocity": -0.22406120647088962, + "velocityX": 3.7008202458728654, + "velocityY": -0.3687443750482909, + "timestamp": 7.206937259499385 }, { - "x": 2.380656782769881, - "y": 6.7398988031009575, - "heading": 0.5125504196, - "angularVelocity": -1.3336684994607708e-16, - "velocityX": 0.6909263394843395, - "velocityY": 0.3930045651914449, - "timestamp": 1.495356408523245 + "x": 5.385997622338085, + "y": 6.619824669792801, + "heading": -0.22692062699124488, + "angularVelocity": -4.12672480414244e-7, + "velocityX": 3.7646350143327725, + "velocityY": -0.36153808267225507, + "timestamp": 7.269008566222619 }, { - "x": 2.475068211555481, - "y": 6.793600797653198, - "heading": 0.5125504196, - "angularVelocity": 6.653839443641591e-19, - "velocityX": 1.0363894332018742, - "velocityY": 0.5895068045437317, - "timestamp": 1.5864528880521036 + "x": 5.6195399174914655, + "y": 6.5960341766076915, + "heading": -0.2269206525768778, + "angularVelocity": -4.121974272362167e-7, + "velocityX": 3.7624839476103156, + "velocityY": -0.3832768221124699, + "timestamp": 7.331079872945854 }, { - "x": 2.569479640341081, - "y": 6.847302792205439, - "heading": 0.5125504196, - "angularVelocity": 5.174999729369457e-17, - "velocityX": 1.0363894332018742, - "velocityY": 0.5895068045437317, - "timestamp": 1.6775493675809623 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.22692067835854435, + "angularVelocity": -4.1535562716932914e-7, + "velocityX": 3.729650183615213, + "velocityY": -0.6268142199732243, + "timestamp": 7.393151179669089 }, { - "x": 2.6324205974818655, - "y": 6.8831041245331495, - "heading": 0.5125504196, - "angularVelocity": -1.3901806543706914e-17, - "velocityX": 0.6909263394843393, - "velocityY": 0.39300456519144494, - "timestamp": 1.768645847109821 + "x": 5.959383441341322, + "y": 6.535142573313356, + "heading": -0.22692070161190955, + "angularVelocity": -7.95525127627382e-7, + "velocityX": 3.706414340926548, + "velocityY": -0.7521132022680976, + "timestamp": 7.422381387996315 }, { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "angularVelocity": -1.8725484396545262e-17, - "velocityX": 0.3454631806028332, - "velocityY": 0.19650228877335693, - "timestamp": 1.8597423266386797 + "x": 6.066940887967902, + "y": 6.509606194124557, + "heading": -0.22692072465630755, + "angularVelocity": -7.883761123678933e-7, + "velocityX": 3.6796674667011553, + "velocityY": -0.8736297361581465, + "timestamp": 7.4516115963235405 }, { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "angularVelocity": 1.431796971368011e-33, - "velocityX": 3.743360269099248e-35, - "velocityY": -8.057233109044706e-35, - "timestamp": 1.9508388061675384 + "x": 6.17368571603366, + "y": 6.480861582677863, + "heading": -0.2269207475995078, + "angularVelocity": -7.849140177187675e-7, + "velocityX": 3.651866824580282, + "velocityY": -0.9833871563580519, + "timestamp": 7.480841804650766 }, { - "x": 2.6835271230542426, - "y": 6.903699548014665, - "heading": 0.5052880772246282, - "angularVelocity": -0.10752621184517612, - "velocityX": 0.2907312178663514, - "velocityY": 0.039898557617057115, - "timestamp": 2.018379011023157 + "x": 6.2799139038286755, + "y": 6.450262838777624, + "heading": -0.2269207705194487, + "angularVelocity": -7.841182881547448e-7, + "velocityX": 3.63419194984251, + "velocityY": -1.0468192206387272, + "timestamp": 7.510072012977992 }, { - "x": 2.7228033814945674, - "y": 6.909089957964811, - "heading": 0.49095749727137794, - "angularVelocity": -0.21217850884350908, - "velocityX": 0.5815241236576919, - "velocityY": 0.0798103879262595, - "timestamp": 2.0859192158787754 + "x": 6.38600768248991, + "y": 6.419201268635774, + "heading": -0.22692079344039778, + "angularVelocity": -7.841527793264923e-7, + "velocityX": 3.629593654398156, + "velocityY": -1.0626530537902528, + "timestamp": 7.539302221305218 }, { - "x": 2.7817246923505206, - "y": 6.91717710303389, - "heading": 0.46979167660622734, - "angularVelocity": -0.31338105518627096, - "velocityX": 0.8723886902906202, - "velocityY": 0.11973823719319023, - "timestamp": 2.153459420734394 + "x": 6.4915378820851375, + "y": 6.386275784732387, + "heading": -0.22692082347226308, + "angularVelocity": -0.0000010274256324387564, + "velocityX": 3.6103129479556895, + "velocityY": -1.1264197481865303, + "timestamp": 7.568532429632444 }, { - "x": 2.860296723174767, - "y": 6.927962260865171, - "heading": 0.4420737054643263, - "angularVelocity": -0.41039216865205086, - "velocityX": 1.1633371706853932, - "velocityY": 0.15968500324119947, - "timestamp": 2.220999625590012 + "x": 6.594254502115419, + "y": 6.3529873750963075, + "heading": -0.23642872835410836, + "angularVelocity": -0.32527667183951403, + "velocityX": 3.514057063171563, + "velocityY": -1.1388358667671954, + "timestamp": 7.597762637959669 }, { - "x": 2.9585261643123166, - "y": 6.941446920815512, - "heading": 0.40815310467896726, - "angularVelocity": -0.502228278073356, - "velocityX": 1.454384708301312, - "velocityY": 0.19965382070083557, - "timestamp": 2.2885398304456306 + "x": 6.69366972769076, + "y": 6.320452523543764, + "heading": -0.25267195567048195, + "angularVelocity": -0.5557000187824542, + "velocityX": 3.4011124540202067, + "velocityY": -1.1130557534298289, + "timestamp": 7.626992846286895 }, { - "x": 3.0764209718394073, - "y": 6.9576328058659405, - "heading": 0.3684706356327604, - "angularVelocity": -0.5875384762459123, - "velocityX": 1.7455500435498434, - "velocityY": 0.23964814861057657, - "timestamp": 2.356080035301249 + "x": 6.789488054498416, + "y": 6.288864279277486, + "heading": -0.26990654049679197, + "angularVelocity": -0.5896155317621415, + "velocityX": 3.278058292810801, + "velocityY": -1.0806711985311153, + "timestamp": 7.656223054614121 }, { - "x": 3.2139906503876703, - "y": 6.9765218958360915, - "heading": 0.323597871318007, - "angularVelocity": -0.6643859670055544, - "velocityX": 2.0368561043358877, - "velocityY": 0.27967178972185397, - "timestamp": 2.4236202401568674 + "x": 6.881710669103943, + "y": 6.258257632428043, + "heading": -0.28694313831911705, + "angularVelocity": -0.5828421621787924, + "velocityX": 3.1550447254125045, + "velocityY": -1.047089589879878, + "timestamp": 7.685453262941347 }, { - "x": 3.3712465090237984, - "y": 6.998116434011635, - "heading": 0.27430452270447037, - "angularVelocity": -0.7298371202591344, - "velocityX": 2.3283296071176522, - "velocityY": 0.3197286449116718, - "timestamp": 2.491160445012486 + "x": 6.970345859458022, + "y": 6.228646491338886, + "heading": -0.3032367291527766, + "angularVelocity": -0.5574230142767849, + "velocityX": 3.0323146986098455, + "velocityY": -1.0130321603485573, + "timestamp": 7.714683471268573 }, { - "x": 3.5482015820439243, - "y": 7.022418860702876, - "heading": 0.22168358710657873, - "angularVelocity": -0.7791053596947181, - "velocityX": 2.619996095635254, - "velocityY": 0.3598216313260006, - "timestamp": 2.558700649868104 + "x": 7.055400617247705, + "y": 6.200038168598195, + "heading": -0.3184704659597927, + "angularVelocity": -0.5211641544429054, + "velocityX": 2.909823865690951, + "velocityY": -0.978724558526454, + "timestamp": 7.743913679595798 }, { - "x": 3.7448688058303015, - "y": 7.04943148246024, - "heading": 0.16741520227188003, - "angularVelocity": -0.8034974864335817, - "velocityX": 2.911854120176202, - "velocityY": 0.3999487685166161, - "timestamp": 2.6262408547237226 + "x": 7.136880378911089, + "y": 6.172437128670844, + "heading": -0.33243641899697507, + "angularVelocity": -0.477791772157844, + "velocityX": 2.7875190197494866, + "velocityY": -0.9442642220804569, + "timestamp": 7.773143887923024 }, { - "x": 3.961249724160237, - "y": 7.079155012062387, - "heading": 0.1144423105156959, - "angularVelocity": -0.7843164211512963, - "velocityX": 3.2037350018777047, - "velocityY": 0.4400864591051642, - "timestamp": 2.693781059579341 + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.42938659336036833, + "velocityX": 2.665359438451487, + "velocityY": -0.9097014120066562, + "timestamp": 7.80237409625025 }, { - "x": 4.197248092224393, - "y": 7.111577952654102, - "heading": 0.06945280731873603, - "angularVelocity": -0.6661144024234814, - "velocityX": 3.494190883321295, - "velocityY": 0.4800539273019009, - "timestamp": 2.7613212644349594 + "x": 7.370577859130934, + "y": 6.0916577592619054, + "heading": -0.3653758531922844, + "angularVelocity": -0.3132570133567013, + "velocityX": 2.393610638018096, + "velocityY": -0.8325804140238011, + "timestamp": 7.867459230019526 }, { - "x": 4.450279816565845, - "y": 7.14615999986413, - "heading": 0.06926458523360347, - "angularVelocity": -0.0027868154314146247, - "velocityX": 3.746386687490236, - "velocityY": 0.5120216511625231, - "timestamp": 2.828861469290578 + "x": 7.5087533836626745, + "y": 6.042843250581257, + "heading": -0.37947223160146565, + "angularVelocity": -0.21658368958955537, + "velocityX": 2.1229967049244562, + "velocityY": -0.7500101152698444, + "timestamp": 7.932544363788803 }, { - "x": 4.703335353284152, - "y": 7.180937109425349, - "heading": 0.06926456657564138, - "angularVelocity": -2.762497114544702e-7, - "velocityX": 3.746739253445653, - "velocityY": 0.5149097435455242, - "timestamp": 2.896401674146196 + "x": 7.629367562165718, + "y": 5.999619792415774, + "heading": -0.38804895009153, + "angularVelocity": -0.1317769203712161, + "velocityX": 1.85317554897581, + "velocityY": -0.6641064658284028, + "timestamp": 7.997629497558079 }, { - "x": 4.956390888818978, - "y": 7.215714227598158, - "heading": 0.06926454791768001, - "angularVelocity": -2.7624970048363726e-7, - "velocityX": 3.746739235923052, - "velocityY": 0.5149098710487069, - "timestamp": 2.9639418790018146 + "x": 7.7324579600950285, + "y": 5.962133690972344, + "heading": -0.39162657356882036, + "angularVelocity": -0.05496836635495089, + "velocityX": 1.5839315671495955, + "velocityY": -0.5759548958801737, + "timestamp": 8.062714631327355 }, { - "x": 5.209446424353711, - "y": 7.250491345771652, - "heading": 0.0692645292597187, - "angularVelocity": -2.7624969990656975e-7, - "velocityX": 3.7467392359216607, - "velocityY": 0.5149098710588286, - "timestamp": 3.031482083857433 + "x": 7.81805300550911, + "y": 5.930490256491027, + "heading": -0.39057997729782273, + "angularVelocity": 0.01608041975771331, + "velocityX": 1.3151243679933469, + "velocityY": -0.4861852876186942, + "timestamp": 8.127799765096631 }, { - "x": 5.462501959888314, - "y": 7.285268463946087, - "heading": 0.0692645106017574, - "angularVelocity": -2.7624969939846597e-7, - "velocityX": 3.7467392359197462, - "velocityY": 0.5149098710727624, - "timestamp": 3.0990222887130514 + "x": 7.886174910867723, + "y": 5.904768911877698, + "heading": -0.38519213768662347, + "angularVelocity": 0.08278141718657105, + "velocityX": 1.0466584519915365, + "velocityY": -0.39519538677620675, + "timestamp": 8.192884898865907 }, { - "x": 5.715557495424461, - "y": 7.320045582109288, - "heading": 0.06926449194379608, - "angularVelocity": -2.762496999067462e-7, - "velocityX": 3.746739235942603, - "velocityY": 0.5149098709064452, - "timestamp": 3.16656249356867 + "x": 7.936841490598973, + "y": 5.885031694896893, + "heading": -0.3756842946663964, + "angularVelocity": 0.1460831755210139, + "velocityX": 0.7784662456231665, + "velocityY": -0.3032523072130836, + "timestamp": 8.257970032635184 }, { - "x": 5.968613025759916, - "y": 7.354822738115272, - "heading": 0.06926447328583468, - "angularVelocity": -2.762497009102298e-7, - "velocityX": 3.7467391589411925, - "velocityY": 0.5149104312065347, - "timestamp": 3.234102698424288 + "x": 7.970067341476633, + "y": 5.8713284070268905, + "heading": -0.36223415085714655, + "angularVelocity": 0.206654623418748, + "velocityX": 0.510498311264806, + "velocityY": -0.21054405324847428, + "timestamp": 8.32305516640446 + }, + { + "x": 7.985864639282227, + "y": 5.863699913024902, + "heading": -0.344987478573796, + "angularVelocity": 0.2649863537883773, + "velocityX": 0.24271745160106833, + "velocityY": -0.1172079330593501, + "timestamp": 8.388140300173736 }, { - "x": 6.221690653746151, - "y": 7.389438720345797, - "heading": 0.06926445462738323, - "angularVelocity": -2.7625695670270355e-7, - "velocityX": 3.74706633666926, - "velocityY": 0.5125240929387752, - "timestamp": 3.3016429032799066 + "x": 7.985045656249217, + "y": 5.861990715165632, + "heading": -0.3251381624172612, + "angularVelocity": 0.31897227576671205, + "velocityX": -0.01316080008970204, + "velocityY": -0.02746627272232015, + "timestamp": 8.450369270101682 }, { - "x": 6.476102895472122, - "y": 7.412263085598601, - "heading": 0.06926437909315673, - "angularVelocity": -0.0000011183594520291744, - "velocityX": 3.766826622303432, - "velocityY": 0.33793745964490024, - "timestamp": 3.369183108135525 + "x": 7.968302412952749, + "y": 5.865866788787815, + "heading": -0.3020436028872691, + "angularVelocity": 0.37112231741475454, + "velocityX": -0.26905866055398503, + "velocityY": 0.062287285594966935, + "timestamp": 8.512598240029627 }, { - "x": 6.712136074986816, - "y": 7.430641724558031, - "heading": 0.04341074490415746, - "angularVelocity": -0.38278880326565307, - "velocityX": 3.4947063015172035, - "velocityY": 0.27211405412107026, - "timestamp": 3.4367233129911434 + "x": 7.935633552055529, + "y": 5.875328955639361, + "heading": -0.2758355383990043, + "angularVelocity": 0.4211553641111372, + "velocityX": -0.5249783330022537, + "velocityY": 0.1520540491430483, + "timestamp": 8.574827209957572 }, { - "x": 6.9285315703702866, - "y": 7.445814297629393, - "heading": 0.019185726579151432, - "angularVelocity": -0.3586755233685199, - "velocityX": 3.203950829673405, - "velocityY": 0.2246450555457597, - "timestamp": 3.504263517846762 + "x": 7.887037558008595, + "y": 5.8903781286161365, + "heading": -0.24666763452179902, + "angularVelocity": 0.468719053376244, + "velocityX": -0.7809223598462517, + "velocityY": 0.24183548264097016, + "timestamp": 8.637056179885517 }, { - "x": 7.125290870666504, - "y": 7.458, - "heading": 0, - "angularVelocity": -0.2840637901552857, - "velocityX": 2.913217404608591, - "velocityY": 0.18042146002750595, - "timestamp": 3.5718037227023802 + "x": 7.822512734192608, + "y": 5.911015324029987, + "heading": -0.21472164060862464, + "angularVelocity": 0.5133620876283883, + "velocityX": -1.0368936508301385, + "velocityY": 0.33163324795100674, + "timestamp": 8.699285149813463 }, { - "x": 7.284551146493795, - "y": 7.4665356658331685, - "heading": -0.011983560395956015, - "angularVelocity": -0.19978893791899283, - "velocityX": 2.6551742811725556, - "velocityY": 0.1423059220209307, - "timestamp": 3.6317848233617918 + "x": 7.742057181096897, + "y": 5.937241677003945, + "heading": -0.1802161756247138, + "angularVelocity": 0.5544919837796514, + "velocityX": -1.29289546635384, + "velocityY": 0.4214492543316279, + "timestamp": 8.761514119741408 }, { - "x": 7.428286998104642, - "y": 7.47296871812108, - "heading": -0.020071347748117502, - "angularVelocity": -0.13483892864997732, - "velocityX": 2.396352351501819, - "velocityY": 0.10725132111930623, - "timestamp": 3.6917659240212033 + "x": 7.645668783226514, + "y": 5.9690584605532635, + "heading": -0.14341967249459378, + "angularVelocity": 0.5913082471512265, + "velocityX": -1.5489312772168387, + "velocityY": 0.5112857176674774, + "timestamp": 8.823743089669353 }, { - "x": 7.556472866857842, - "y": 7.4774155432867, - "heading": -0.02498803078182015, - "angularVelocity": -0.08197053704667534, - "velocityX": 2.137104310257201, - "velocityY": 0.0741371051336659, - "timestamp": 3.751747024680615 + "x": 7.533345224831275, + "y": 6.006467108033043, + "heading": -0.10467019490543185, + "angularVelocity": 0.6226919332592115, + "velocityX": -1.8050043014579413, + "velocityY": 0.6011452145695896, + "timestamp": 8.885972059597298 }, { - "x": 7.669092898448606, - "y": 7.47995645101358, - "heading": -0.027230015787281982, - "angularVelocity": -0.037378190476904, - "velocityX": 1.877591947341067, - "velocityY": 0.04236180561785803, - "timestamp": 3.8117281253400264 + "x": 7.405084087505747, + "y": 6.04946923502926, + "heading": -0.06440725119965417, + "angularVelocity": 0.6470128583583754, + "velocityX": -2.0611161887146876, + "velocityY": 0.6910306734308482, + "timestamp": 8.948201029525244 }, { - "x": 7.766136403587462, - "y": 7.480650177988498, - "heading": -0.027158837231048453, - "angularVelocity": 0.001186683062681687, - "velocityX": 1.6179013734658474, - "velocityY": 0.011565759335732523, - "timestamp": 3.871709225999438 + "x": 7.260883177292953, + "y": 6.098066643466027, + "heading": -0.023226072117979224, + "angularVelocity": 0.6617686124221249, + "velocityX": -2.3172633321708815, + "velocityY": 0.7809450886466088, + "timestamp": 9.010429999453189 }, { - "x": 7.847595735447885, - "y": 7.479541543526011, - "heading": -0.025049657080079103, - "angularVelocity": 0.03516407881452247, - "velocityX": 1.3580833123248393, - "velocityY": -0.018483063003166142, - "timestamp": 3.9316903266588494 + "x": 7.100741532540205, + "y": 6.152261239581916, + "heading": 0.018021968863737303, + "angularVelocity": 0.6628430621538008, + "velocityX": -2.5734259290837285, + "velocityY": 0.8708901365174432, + "timestamp": 9.072658969381134 }, { - "x": 7.913465176456017, - "y": 7.4766658686921605, - "heading": -0.021118970688138776, - "angularVelocity": 0.06553208175121362, - "velocityX": 1.0981699282605077, - "velocityY": -0.04794301542047442, - "timestamp": 3.991671427318261 + "x": 6.924662708731195, + "y": 6.212054571383467, + "heading": 0.05802124542072086, + "angularVelocity": 0.6427758100977419, + "velocityX": -2.829531390490462, + "velocityY": 0.9608600603671181, + "timestamp": 9.13488793930908 }, { - "x": 7.963740302847386, - "y": 7.472051704673601, - "heading": -0.015541591171907823, - "angularVelocity": 0.09298561471722216, - "velocityX": 0.8381827915570343, - "velocityY": -0.07692696479112789, - "timestamp": 4.051652527977672 + "x": 6.732667433541327, + "y": 6.277445378277029, + "heading": 0.09448040335619008, + "angularVelocity": 0.5858872158366861, + "velocityX": -3.0853037646642187, + "velocityY": 1.050809726872821, + "timestamp": 9.197116909237025 }, { - "x": 7.998417597522527, - "y": 7.465722607991469, - "heading": -0.008461644038298986, - "angularVelocity": 0.11803629903043378, - "velocityX": 0.5781370180592035, - "velocityY": -0.10551818176978627, - "timestamp": 4.111633628637083 + "x": 6.524862060286313, + "y": 6.348410321942265, + "heading": 0.12245319566137218, + "angularVelocity": 0.44951398581032764, + "velocityX": -3.33936707446111, + "velocityY": 1.1403843539014786, + "timestamp": 9.25934587916497 }, { - "x": 8.017494201660156, - "y": 7.457698345184326, - "heading": 0, - "angularVelocity": 0.14107183671647575, - "velocityX": 0.3180435825269393, - "velocityY": -0.1337798526356988, - "timestamp": 4.171614729296494 + "x": 6.30228026524581, + "y": 6.424202518084616, + "heading": 0.1231057628389814, + "angularVelocity": 0.010486549566300301, + "velocityX": -3.5768195311320152, + "velocityY": 1.2179567849204198, + "timestamp": 9.321574849092915 }, { - "x": 8.008975668857596, - "y": 7.440959395735479, - "heading": 0.016440344425850486, - "angularVelocity": 0.17454816573497248, - "velocityX": -0.09044179592138195, - "velocityY": -0.17771847395317164, - "timestamp": 4.265802739560738 + "x": 6.078984969036473, + "y": 6.498549743391548, + "heading": 0.1231057748040032, + "angularVelocity": 1.9227414178241924e-7, + "velocityX": -3.5882852707972814, + "velocityY": 1.194736557475109, + "timestamp": 9.38380381902086 }, { - "x": 7.961981832760565, - "y": 7.4200887947149115, - "heading": 0.0360050890089686, - "angularVelocity": 0.20772011775415367, - "velocityX": -0.4989364990850843, - "velocityY": -0.22158447728129313, - "timestamp": 4.3599907498249815 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0.12310578689683861, + "angularVelocity": 1.9432806685756468e-7, + "velocityX": -3.662936913327793, + "velocityY": 0.941318096340742, + "timestamp": 9.446032788948806 }, { - "x": 7.876511570645417, - "y": 7.39509531765153, - "heading": 0.058657775076795544, - "angularVelocity": 0.24050498576490828, - "velocityX": -0.9074431222759898, - "velocityY": -0.2653573102697685, - "timestamp": 4.454178760089225 + "x": 5.606427257807288, + "y": 6.600564902613298, + "heading": 0.1231057972666647, + "angularVelocity": 1.578555424299444e-7, + "velocityX": -3.7237014740499577, + "velocityY": 0.6612371129043467, + "timestamp": 9.511724660841988 }, { - "x": 7.752563396875145, - "y": 7.365990633137338, - "heading": 0.08435039467917142, - "angularVelocity": 0.2727801503641011, - "velocityX": -1.315965518567977, - "velocityY": -0.3090062570866462, - "timestamp": 4.548366770353469 + "x": 5.35931577124569, + "y": 6.62625930787543, + "heading": 0.12310580746861746, + "angularVelocity": 1.5530007712924032e-7, + "velocityX": -3.7616752185629396, + "velocityY": 0.39113522756534835, + "timestamp": 9.577416532735171 }, { - "x": 7.590135252234061, - "y": 7.332791018782474, - "heading": 0.11301654484596554, - "angularVelocity": 0.3043503104733978, - "velocityX": -1.7245097776818146, - "velocityY": -0.35248238349789773, - "timestamp": 4.642554780617712 + "x": 5.111994168115865, + "y": 6.649845615003531, + "heading": 0.1231058176602883, + "angularVelocity": 1.551435596748186e-7, + "velocityX": -3.764873735550995, + "velocityY": 0.35904452785959023, + "timestamp": 9.643108404628354 }, { - "x": 7.3892240901384, - "y": 7.295520773303632, - "heading": 0.14455765680612448, - "angularVelocity": 0.33487395977121315, - "velocityX": -2.13308638256617, - "velocityY": -0.39570052891318314, - "timestamp": 4.736742790881956 + "x": 4.868859374012009, + "y": 6.67294055859981, + "heading": 0.13716866748145323, + "angularVelocity": 0.21407290454489739, + "velocityX": -3.701139685883833, + "velocityY": 0.35156470550620644, + "timestamp": 9.708800276521536 }, { - "x": 7.149824938411661, - "y": 7.254220108341429, - "heading": 0.1788109128374065, - "angularVelocity": 0.36366896312157765, - "velocityX": -2.54171577735964, - "velocityY": -0.4384917448232911, - "timestamp": 4.8309308011461995 + "x": 4.644141674488456, + "y": 6.6943246178055915, + "heading": 0.16550217221046287, + "angularVelocity": 0.43130913935716303, + "velocityX": -3.4207839272559015, + "velocityY": 0.3255206251475534, + "timestamp": 9.77449214841472 }, { - "x": 6.871928226350113, - "y": 7.208968463075644, - "heading": 0.21545413209734332, - "angularVelocity": 0.3890433523028524, - "velocityX": -2.9504467849136042, - "velocityY": -0.4804395499897761, - "timestamp": 4.925118811410443 + "x": 4.438149971299598, + "y": 6.713938607045374, + "heading": 0.1971398735951766, + "angularVelocity": 0.48160754858923427, + "velocityX": -3.1357258859027066, + "velocityY": 0.2985755874284469, + "timestamp": 9.840184020307902 }, { - "x": 6.555508023323144, - "y": 7.15999689291417, - "heading": 0.25354713749374036, - "angularVelocity": 0.40443582245264, - "velocityX": -3.3594530995957568, - "velocityY": -0.5199342254293745, - "timestamp": 5.019306821674687 + "x": 4.2509016025753406, + "y": 6.731775563046276, + "heading": 0.22901708759854217, + "angularVelocity": 0.48525354940713283, + "velocityX": -2.8504039134206605, + "velocityY": 0.27152455070706877, + "timestamp": 9.905875892201085 }, { - "x": 6.201946981022424, - "y": 7.116596127430346, - "heading": 0.25354714311943394, - "angularVelocity": 5.972834101491316e-8, - "velocityX": -3.753779714730222, - "velocityY": -0.46078864350212084, - "timestamp": 5.11349483193893 + "x": 4.082393919350406, + "y": 6.747832566507683, + "heading": 0.25967107343501483, + "angularVelocity": 0.46663285659323517, + "velocityX": -2.565122265033534, + "velocityY": 0.24442907468850525, + "timestamp": 9.971567764094267 }, { - "x": 5.847829680383322, - "y": 7.077995922009697, - "heading": 0.25354714640605686, - "angularVelocity": 3.4894280958051e-8, - "velocityX": -3.7596855443238373, - "velocityY": -0.4098207968546854, - "timestamp": 5.207682842203174 + "x": 3.9326220842038664, + "y": 6.762107971713798, + "heading": 0.28824128245235414, + "angularVelocity": 0.43491239013245725, + "velocityX": -2.279914254690053, + "velocityY": 0.21730854662395976, + "timestamp": 10.03725963598745 }, { - "x": 5.493712379149646, - "y": 7.039395722043647, - "heading": 0.25354714969267966, - "angularVelocity": 3.4894280048226024e-8, - "velocityX": -3.759685550636459, - "velocityY": -0.40982073894285953, - "timestamp": 5.301870852467418 + "x": 3.801581586235111, + "y": 6.77460070477556, + "heading": 0.314159509734349, + "angularVelocity": 0.3945423769342238, + "velocityX": -1.9947749119073828, + "velocityY": 0.19017167119358822, + "timestamp": 10.102951507880633 }, { - "x": 5.13959507794107, - "y": 7.0007955218474756, - "heading": 0.25354715297933, - "angularVelocity": 3.489457219723442e-8, - "velocityX": -3.7596855503699804, - "velocityY": -0.4098207413860709, - "timestamp": 5.396058862731661 + "x": 3.6892685916749244, + "y": 6.785310000165767, + "heading": 0.3370219943875935, + "angularVelocity": 0.34802607985383205, + "velocityX": -1.7096939289964497, + "velocityY": 0.16302314246152652, + "timestamp": 10.168643379773815 }, { - "x": 4.793375065328026, - "y": 6.963049240971564, - "heading": 0.2809926873119857, - "angularVelocity": 0.2913909557666365, - "velocityX": -3.675839543076952, - "velocityY": -0.4007546265178986, - "timestamp": 5.490246872995905 + "x": 3.595679898494151, + "y": 6.794235280663889, + "heading": 0.35652698961990326, + "angularVelocity": 0.2969164170572818, + "velocityX": -1.4246616892414212, + "velocityY": 0.135865826941808, + "timestamp": 10.234335251666998 }, { - "x": 4.4856271372174605, - "y": 6.929500155423233, - "heading": 0.3062072463057812, - "angularVelocity": 0.2677045509620205, - "velocityX": -3.2673790140292933, - "velocityY": -0.3561927410315718, - "timestamp": 5.5844348832601485 + "x": 3.520812828526041, + "y": 6.801376095406927, + "heading": 0.37244068726433127, + "angularVelocity": 0.2422475899347831, + "velocityX": -1.1396702181031793, + "velocityY": 0.10870164812244498, + "timestamp": 10.30002712356018 }, { - "x": 4.216349125910067, - "y": 6.900146018877801, - "heading": 0.32859640421091696, - "angularVelocity": 0.23770709076795637, - "velocityX": -2.858941499580855, - "velocityY": -0.31165470491497754, - "timestamp": 5.678622893524392 + "x": 3.464665127058438, + "y": 6.806732084957018, + "heading": 0.38457706026543664, + "angularVelocity": 0.18474695044220302, + "velocityX": -0.8547130695088268, + "velocityY": 0.08153199773025213, + "timestamp": 10.365718995453364 }, { - "x": 3.9855401854111148, - "y": 6.874986045056756, - "heading": 0.34796083888612495, - "angularVelocity": 0.2055934149249067, - "velocityX": -2.450512967111424, - "velocityY": -0.26712501676655553, - "timestamp": 5.772810903788636 + "x": 3.4272348818352074, + "y": 6.810302960282004, + "heading": 0.39278518447772715, + "angularVelocity": 0.12494885555456782, + "velocityX": -0.5697850303930024, + "velocityY": 0.05435794752192765, + "timestamp": 10.431410867346546 }, { - "x": 3.7931998687162816, - "y": 6.854019825581219, - "heading": 0.36419988308525797, - "angularVelocity": 0.17241094862896578, - "velocityX": -2.042089180514842, - "velocityY": -0.22259966440226955, - "timestamp": 5.866998914052879 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.0632601721112495, + "velocityX": -0.28488184560874424, + "velocityY": 0.027180367966521624, + "timestamp": 10.497102739239729 }, { - "x": 3.639327899901621, - "y": 6.837247108195556, - "heading": 0.37725261237367164, - "angularVelocity": 0.13858164379727655, - "velocityX": -1.6336683234200817, - "velocityY": -0.17807699025180046, - "timestamp": 5.961186924317123 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0, + "velocityX": 1.9034366224257262e-34, + "velocityY": -2.547324229323533e-33, + "timestamp": 10.562794611132912 }, { - "x": 3.5239240923326007, - "y": 6.824667722043819, - "heading": 0.38707830352299477, - "angularVelocity": 0.10431997790119188, - "velocityX": -1.2252494478358333, - "velocityY": -0.13355613008965372, - "timestamp": 6.0553749345813666 + "x": 3.4428034429775143, + "y": 6.811427919070607, + "heading": 0.39249813327278055, + "angularVelocity": -0.050100574262489476, + "velocityX": 0.38660845958378076, + "velocityY": -0.007449238878501296, + "timestamp": 10.651470846737375 }, { - "x": 3.4469883122501845, - "y": 6.8162815454148475, - "heading": 0.39364810050149374, - "angularVelocity": 0.06975194571015403, - "velocityX": -0.8168319923796448, - "velocityY": -0.08903656214250527, - "timestamp": 6.14956294484561 + "x": 3.5113694006298597, + "y": 6.810106800279428, + "heading": 0.3836131655700934, + "angularVelocity": -0.10019558951868686, + "velocityX": 0.7732168284429818, + "velocityY": -0.014898228168725063, + "timestamp": 10.740147082341839 + }, + { + "x": 3.614218332551768, + "y": 6.808125167746075, + "heading": 0.37028807906085554, + "angularVelocity": -0.1502667137188131, + "velocityX": 1.1598251912796755, + "velocityY": -0.022346827420505266, + "timestamp": 10.828823317946302 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0.03495947190378589, - "velocityX": -0.4084155935928461, - "velocityY": -0.04451793673752315, - "timestamp": 6.243750955109854 + "x": 3.7513502451988723, + "y": 6.8054830693306245, + "heading": 0.3525264976102127, + "angularVelocity": -0.20029697166969943, + "velocityX": 1.5464336269164098, + "velocityY": -0.02979488695523812, + "timestamp": 10.917499553550766 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": -1.9952146739162206e-34, - "velocityY": -3.951795765453016e-33, - "timestamp": 6.337938965374097 + "x": 3.9227651512282486, + "y": 6.802180567172425, + "heading": 0.3303333796886753, + "angularVelocity": -0.2502713130553812, + "velocityX": 1.9330422052867193, + "velocityY": -0.03724224574585722, + "timestamp": 11.006175789155229 }, { - "x": 3.425127749537185, - "y": 6.810478633677757, - "heading": 0.3886892234850989, - "angularVelocity": -0.13293807639169805, - "velocityX": 0.2675517930120232, - "velocityY": -0.025935588272518423, - "timestamp": 6.400010272097332 + "x": 4.128463068361194, + "y": 6.79821773882544, + "heading": 0.3037147939289921, + "angularVelocity": -0.3001772186001935, + "velocityX": 2.319650983499712, + "velocityY": -0.04468872996211646, + "timestamp": 11.094852024759692 }, { - "x": 3.458347138585669, - "y": 6.807258412874914, - "heading": 0.3723514064813247, - "angularVelocity": -0.26321045691257755, - "velocityX": 0.535181081278085, - "velocityY": -0.05187937829625554, - "timestamp": 6.462081578820567 + "x": 4.368444017678072, + "y": 6.793594678448866, + "heading": 0.2726776385751228, + "angularVelocity": -0.3500053327963678, + "velocityX": 2.706259999435507, + "velocityY": -0.05213415234714548, + "timestamp": 11.183528260364156 }, { - "x": 3.508184213080087, - "y": 6.802427294600358, - "heading": 0.34812438720722594, - "angularVelocity": -0.3903094771652888, - "velocityX": 0.8029003596884762, - "velocityY": -0.07783174754300737, - "timestamp": 6.524152885543802 + "x": 4.642708020512335, + "y": 6.788311497970363, + "heading": 0.23722930806330966, + "angularVelocity": -0.3997500600942169, + "velocityX": 3.092869255948188, + "velocityY": -0.05957831252634942, + "timestamp": 11.27220449596862 }, { - "x": 3.5746455076736363, - "y": 6.795984642746885, - "heading": 0.31624502791908643, - "angularVelocity": -0.5135925272251962, - "velocityX": 1.070724914651617, - "velocityY": -0.10379436479708294, - "timestamp": 6.586224192267037 + "x": 4.951255088173746, + "y": 6.782368328182758, + "heading": 0.19737735072702745, + "angularVelocity": -0.44940966499796203, + "velocityX": 3.479478640000833, + "velocityY": -0.0670209977576627, + "timestamp": 11.360880731573083 }, { - "x": 3.6577387353786484, - "y": 6.787929625128416, - "heading": 0.2770021560106679, - "angularVelocity": -0.6322224225666108, - "velocityX": 1.3386737301264124, - "velocityY": -0.12977038898779852, - "timestamp": 6.648295498990271 + "x": 5.286562692788269, + "y": 6.775920770205836, + "heading": 0.19737734601955828, + "angularVelocity": -5.3086028753514235e-8, + "velocityX": 3.781256639153567, + "velocityY": -0.07270897251075666, + "timestamp": 11.449556967177546 }, { - "x": 3.7574730901724314, - "y": 6.7782611086258875, - "heading": 0.23075444743671972, - "angularVelocity": -0.7450738677080124, - "velocityX": 1.6067706652041787, - "velocityY": -0.1557646682971112, - "timestamp": 6.710366805713506 + "x": 5.595111114675473, + "y": 6.769985794431735, + "heading": 0.15789301075905496, + "angularVelocity": -0.4452639987631135, + "velocityX": 3.4794939115759345, + "velocityY": -0.06692859404378032, + "timestamp": 11.53823320278201 }, { - "x": 3.873859634378807, - "y": 6.766977536857145, - "heading": 0.17795757602156642, - "angularVelocity": -0.8505841781386996, - "velocityX": 1.875045819887845, - "velocityY": -0.18178402170643457, - "timestamp": 6.772438112436741 + "x": 5.8693764275565705, + "y": 6.76471055259859, + "heading": 0.12280143262174693, + "angularVelocity": -0.3957269712466434, + "velocityX": 3.092884029318143, + "velocityY": -0.059488788593539996, + "timestamp": 11.626909438386473 }, { - "x": 4.006911762109537, - "y": 6.754076769706622, - "heading": 0.11920696294593584, - "angularVelocity": -0.9465019535937051, - "velocityX": 2.1435367604550115, - "velocityY": -0.2078378534549514, - "timestamp": 6.834509419159976 + "x": 6.109358599544783, + "y": 6.760094936608861, + "heading": 0.09209943623091729, + "angularVelocity": -0.3462257523850543, + "velocityX": 2.7062737874738274, + "velocityY": -0.05205020215694004, + "timestamp": 11.715585673990937 }, { - "x": 4.156645657189765, - "y": 6.73955582918857, - "heading": 0.055309270826440785, - "angularVelocity": -1.029423988195197, - "velocityX": 2.4122884305927053, - "velocityY": -0.23393966205310818, - "timestamp": 6.896580725883211 + "x": 6.315057615509001, + "y": 6.756138852616793, + "heading": 0.06578495581515914, + "angularVelocity": -0.29674782918315123, + "velocityX": 2.3196633749963063, + "velocityY": -0.04461267401690494, + "timestamp": 11.8042619095954 }, { - "x": 4.323080375842652, - "y": 6.723410435666096, - "heading": -0.012586184407094544, - "angularVelocity": -1.0938299645646508, - "velocityX": 2.681347106079046, - "velocityY": -0.2601104177560936, - "timestamp": 6.958652032606445 + "x": 6.4864734670806214, + "y": 6.752842221795132, + "heading": 0.043856783005957245, + "angularVelocity": -0.24728353272698234, + "velocityX": 1.9330528681462538, + "velocityY": -0.037176034810112814, + "timestamp": 11.892938145199864 }, { - "x": 4.506235840968647, - "y": 6.705634179985004, - "heading": -0.08270962096165607, - "angularVelocity": -1.1297238652836274, - "velocityX": 2.9507267495214404, - "velocityY": -0.2863844281597597, - "timestamp": 7.02072333932968 + "x": 6.623606149810205, + "y": 6.750204980767773, + "heading": 0.026314311963007715, + "angularVelocity": -0.19782606831887856, + "velocityX": 1.5464423111199512, + "velocityY": -0.029740110294281013, + "timestamp": 11.981614380804327 }, { - "x": 4.706118949857515, - "y": 6.686217722826257, - "heading": -0.15194437984101536, - "angularVelocity": -1.1154068205470853, - "velocityX": 3.2202175117742615, - "velocityY": -0.3128088996947802, - "timestamp": 7.082794646052915 + "x": 6.726455661694478, + "y": 6.74822708185799, + "heading": 0.013157327563715553, + "angularVelocity": -0.14837102984364775, + "velocityX": 1.1598317315027789, + "velocityY": -0.022304723427886322, + "timestamp": 12.07029061640879 }, { - "x": 4.922607059053276, - "y": 6.66515425622058, - "heading": -0.21301282950448638, - "angularVelocity": -0.9838434678965439, - "velocityX": 3.4877324262085807, - "velocityY": -0.3393430510421988, - "timestamp": 7.14486595277615 + "x": 6.795022002255089, + "y": 6.7469084932218335, + "heading": 0.004385844550104781, + "angularVelocity": -0.09891582512292933, + "velocityX": 0.7732211464912397, + "velocityY": -0.014869695664985829, + "timestamp": 12.158966852013254 }, { - "x": 5.152321807662402, - "y": 6.642265811014449, - "heading": -0.22692060137612477, - "angularVelocity": -0.22406120647088962, - "velocityX": 3.7008202458728654, - "velocityY": -0.3687443750482909, - "timestamp": 7.206937259499385 + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -4.086871260037018e-38, + "angularVelocity": -0.04945907457853377, + "velocityX": 0.3866105668308079, + "velocityY": -0.007434847721776456, + "timestamp": 12.247643087617718 }, { - "x": 5.385997622338085, - "y": 6.619824669792801, - "heading": -0.22692062699124488, - "angularVelocity": -4.12672480414244e-7, - "velocityX": 3.7646350143327725, - "velocityY": -0.36153808267225507, - "timestamp": 7.269008566222619 - }, + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -5.480350490542979e-38, + "angularVelocity": -1.571392014077605e-37, + "velocityX": 4.425248303324229e-37, + "velocityY": 1.8999126670467387e-37, + "timestamp": 12.336319323222181 + } + ], + "constraints": [ { - "x": 5.6195399174914655, - "y": 6.5960341766076915, - "heading": -0.2269206525768778, - "angularVelocity": -4.121974272362167e-7, - "velocityX": 3.7624839476103156, - "velocityY": -0.3832768221124699, - "timestamp": 7.331079872945854 + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": -0.22692067835854435, - "angularVelocity": -4.1535562716932914e-7, - "velocityX": 3.729650183615213, - "velocityY": -0.6268142199732243, - "timestamp": 7.393151179669089 + "scope": [ + "last" + ], + "type": "StopPoint" }, { - "x": 5.959383441341322, - "y": 6.535142573313356, - "heading": -0.22692070161190955, - "angularVelocity": -7.95525127627382e-7, - "velocityX": 3.706414340926548, - "velocityY": -0.7521132022680976, - "timestamp": 7.422381387996315 + "scope": [ + 1 + ], + "type": "StopPoint" }, { - "x": 6.066940887967902, - "y": 6.509606194124557, - "heading": -0.22692072465630755, - "angularVelocity": -7.883761123678933e-7, - "velocityX": 3.6796674667011553, - "velocityY": -0.8736297361581465, - "timestamp": 7.4516115963235405 + "scope": [ + 5 + ], + "type": "StopPoint" }, { - "x": 6.17368571603366, - "y": 6.480861582677863, - "heading": -0.2269207475995078, - "angularVelocity": -7.849140177187675e-7, - "velocityX": 3.651866824580282, - "velocityY": -0.9833871563580519, - "timestamp": 7.480841804650766 + "scope": [ + 10 + ], + "type": "StopPoint" }, { - "x": 6.2799139038286755, - "y": 6.450262838777624, - "heading": -0.2269207705194487, - "angularVelocity": -7.841182881547448e-7, - "velocityX": 3.63419194984251, - "velocityY": -1.0468192206387272, - "timestamp": 7.510072012977992 - }, + "scope": [ + 2 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "CenterLanePCBFSprint": { + "waypoints": [ { - "x": 6.38600768248991, - "y": 6.419201268635774, - "heading": -0.22692079344039778, - "angularVelocity": -7.841527793264923e-7, - "velocityX": 3.629593654398156, - "velocityY": -1.0626530537902528, - "timestamp": 7.539302221305218 + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 6.4915378820851375, - "y": 6.386275784732387, - "heading": -0.22692082347226308, - "angularVelocity": -0.0000010274256324387564, - "velocityX": 3.6103129479556895, - "velocityY": -1.1264197481865303, - "timestamp": 7.568532429632444 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 6.594254502115419, - "y": 6.3529873750963075, - "heading": -0.23642872835410836, - "angularVelocity": -0.32527667183951403, - "velocityX": 3.514057063171563, - "velocityY": -1.1388358667671954, - "timestamp": 7.597762637959669 + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 6.69366972769076, - "y": 6.320452523543764, - "heading": -0.25267195567048195, - "angularVelocity": -0.5557000187824542, - "velocityX": 3.4011124540202067, - "velocityY": -1.1130557534298289, - "timestamp": 7.626992846286895 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 }, { - "x": 6.789488054498416, - "y": 6.288864279277486, - "heading": -0.26990654049679197, - "angularVelocity": -0.5896155317621415, - "velocityX": 3.278058292810801, - "velocityY": -1.0806711985311153, - "timestamp": 7.656223054614121 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { - "x": 6.881710669103943, - "y": 6.258257632428043, - "heading": -0.28694313831911705, - "angularVelocity": -0.5828421621787924, - "velocityX": 3.1550447254125045, - "velocityY": -1.047089589879878, - "timestamp": 7.685453262941347 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 6.970345859458022, - "y": 6.228646491338886, - "heading": -0.3032367291527766, - "angularVelocity": -0.5574230142767849, - "velocityX": 3.0323146986098455, - "velocityY": -1.0130321603485573, - "timestamp": 7.714683471268573 + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 7.055400617247705, - "y": 6.200038168598195, - "heading": -0.3184704659597927, - "angularVelocity": -0.5211641544429054, - "velocityX": 2.909823865690951, - "velocityY": -0.978724558526454, - "timestamp": 7.743913679595798 + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 7.136880378911089, - "y": 6.172437128670844, - "heading": -0.33243641899697507, - "angularVelocity": -0.477791772157844, - "velocityX": 2.7875190197494866, - "velocityY": -0.9442642220804569, - "timestamp": 7.773143887923024 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { - "x": 7.214789390563965, - "y": 6.145846366882324, - "heading": -0.344987478573796, - "angularVelocity": -0.42938659336036833, - "velocityX": 2.665359438451487, - "velocityY": -0.9097014120066562, - "timestamp": 7.80237409625025 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { - "x": 7.370577859130934, - "y": 6.0916577592619054, - "heading": -0.3653758531922844, - "angularVelocity": -0.3132570133567013, - "velocityX": 2.393610638018096, - "velocityY": -0.8325804140238011, - "timestamp": 7.867459230019526 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 }, { - "x": 7.5087533836626745, - "y": 6.042843250581257, - "heading": -0.37947223160146565, - "angularVelocity": -0.21658368958955537, - "velocityX": 2.1229967049244562, - "velocityY": -0.7500101152698444, - "timestamp": 7.932544363788803 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { - "x": 7.629367562165718, - "y": 5.999619792415774, - "heading": -0.38804895009153, - "angularVelocity": -0.1317769203712161, - "velocityX": 1.85317554897581, - "velocityY": -0.6641064658284028, - "timestamp": 7.997629497558079 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 }, { - "x": 7.7324579600950285, - "y": 5.962133690972344, - "heading": -0.39162657356882036, - "angularVelocity": -0.05496836635495089, - "velocityX": 1.5839315671495955, - "velocityY": -0.5759548958801737, - "timestamp": 8.062714631327355 + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": 2.3560143253921187e-30, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 }, { - "x": 7.81805300550911, - "y": 5.930490256491027, - "heading": -0.39057997729782273, - "angularVelocity": 0.01608041975771331, - "velocityX": 1.3151243679933469, - "velocityY": -0.4861852876186942, - "timestamp": 8.127799765096631 + "x": 1.3060574270397207, + "y": 5.572382522338357, + "heading": -0.010694879272561806, + "angularVelocity": -0.14212355715817127, + "velocityX": 0.21372562484439114, + "velocityY": -0.24679919880328743, + "timestamp": 0.07525057412377358 + }, + { + "x": 1.3382235764672503, + "y": 5.535239265365289, + "heading": -0.0320761034546414, + "angularVelocity": -0.2841337017193578, + "velocityX": 0.42745387396382534, + "velocityY": -0.49359433341007053, + "timestamp": 0.15050114824754715 + }, + { + "x": 1.3864734146735522, + "y": 5.47952484840506, + "heading": -0.06412157549775398, + "angularVelocity": -0.42585020003622126, + "velocityX": 0.6411889712155109, + "velocityY": -0.7403852742575773, + "timestamp": 0.22575172237132074 + }, + { + "x": 1.4508078083146234, + "y": 5.405239688642535, + "heading": -0.10679814197023921, + "angularVelocity": -0.5671261245497303, + "velocityX": 0.8549355854022526, + "velocityY": -0.9871706711626929, + "timestamp": 0.3010022964950943 + }, + { + "x": 1.5312280128772082, + "y": 5.3123844280164345, + "heading": -0.16006528510741636, + "angularVelocity": -0.7078636111159992, + "velocityX": 1.0686988836704838, + "velocityY": -1.2339475373861764, + "timestamp": 0.37625287061886786 + }, + { + "x": 1.6277357374217547, + "y": 5.200960102506698, + "heading": -0.22387989152223717, + "angularVelocity": -0.8480281666778853, + "velocityX": 1.2824848935314492, + "velocityY": -1.4807106365400295, + "timestamp": 0.4515034447426414 + }, + { + "x": 1.7403333706916029, + "y": 5.070968456330507, + "heading": -0.29820152963685287, + "angularVelocity": -0.9876554296244249, + "velocityX": 1.496302647246288, + "velocityY": -1.7274505569061047, + "timestamp": 0.526754018866415 + }, + { + "x": 1.8690253649035526, + "y": 4.922413220845796, + "heading": -0.38299660252732504, + "angularVelocity": -1.1268362252301867, + "velocityX": 1.7101795661770514, + "velocityY": -1.9741408915022984, + "timestamp": 0.6020045929901886 + }, + { + "x": 2.021841436297539, + "y": 4.7786139697645345, + "heading": -0.46243583568438273, + "angularVelocity": -1.055662818268618, + "velocityX": 2.0307628636084356, + "velocityY": -1.9109389231128335, + "timestamp": 0.6772551671139622 + }, + { + "x": 2.158572983780592, + "y": 4.6533861744648695, + "heading": -0.5314192950572162, + "angularVelocity": -0.9167167184470773, + "velocityX": 1.8170166683292757, + "velocityY": -1.6641440514810064, + "timestamp": 0.7525057412377357 + }, + { + "x": 2.2792163174605955, + "y": 4.5467264938443535, + "heading": -0.5899522434559231, + "angularVelocity": -0.777840555764763, + "velocityX": 1.6032214383174732, + "velocityY": -1.4173935795391805, + "timestamp": 0.8277563153615093 + }, + { + "x": 2.3837699374210346, + "y": 4.458633436191598, + "heading": -0.6380354100231267, + "angularVelocity": -0.6389740826105205, + "velocityX": 1.3894062760138433, + "velocityY": -1.1706629308520584, + "timestamp": 0.9030068894852828 + }, + { + "x": 2.472232988532522, + "y": 4.389106089275013, + "heading": -0.6756678010096412, + "angularVelocity": -0.5000944035873048, + "velocityX": 1.1755797499519767, + "velocityY": -0.9239444047507308, + "timestamp": 0.9782574636090564 + }, + { + "x": 2.5446049348789113, + "y": 4.338143851138861, + "heading": -0.7028477866363936, + "angularVelocity": -0.36119306653537947, + "velocityX": 0.9617461021356389, + "velocityY": -0.6772338779998112, + "timestamp": 1.05350803773283 + }, + { + "x": 2.6008854472033804, + "y": 4.305746331042048, + "heading": -0.7195735501934102, + "angularVelocity": -0.222267587342223, + "velocityX": 0.7479080788470353, + "velocityY": -0.43052854378169836, + "timestamp": 1.1287586118566038 + }, + { + "x": 2.6410743549129108, + "y": 4.291913303700176, + "heading": -0.7258432586618085, + "angularVelocity": -0.0833177492798597, + "velocityX": 0.5340677885590666, + "velocityY": -0.18382620335314903, + "timestamp": 1.2040091859803774 }, { - "x": 7.886174910867723, - "y": 5.904768911877698, - "heading": -0.38519213768662347, - "angularVelocity": 0.08278141718657105, - "velocityX": 1.0466584519915365, - "velocityY": -0.39519538677620675, - "timestamp": 8.192884898865907 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05565596827444861, + "velocityX": 0.3202270361150376, + "velocityY": 0.06287505455011895, + "timestamp": 1.279259760104151 + }, + { + "x": 2.6731093005797457, + "y": 4.320288364477002, + "heading": -0.7067987753568107, + "angularVelocity": 0.19580392065911026, + "velocityX": 0.10461717847139641, + "velocityY": 0.3116194636271141, + "timestamp": 1.3551333161029784 + }, + { + "x": 2.664687962821916, + "y": 4.362805446311989, + "heading": -0.6813130877895294, + "angularVelocity": 0.3358968382514228, + "velocityX": -0.11099173681211887, + "velocityY": 0.5603675915105346, + "timestamp": 1.4310068721018057 + }, + { + "x": 2.6399077578590466, + "y": 4.424196334908807, + "heading": -0.645202192832585, + "angularVelocity": 0.47593518560166614, + "velocityX": -0.32659870276641295, + "velocityY": 0.8091210144040187, + "timestamp": 1.506880428100633 + }, + { + "x": 2.5987689088828363, + "y": 4.504461553370171, + "heading": -0.5984695308975294, + "angularVelocity": 0.6159281889382251, + "velocityX": -0.5422027270940676, + "velocityY": 1.0578813316982019, + "timestamp": 1.5827539840994602 + }, + { + "x": 2.541271709317337, + "y": 4.60360172553535, + "heading": -0.5411163068190713, + "angularVelocity": 0.7559053127670017, + "velocityX": -0.7578028841452508, + "velocityY": 1.3066498710827958, + "timestamp": 1.6586275400982875 + }, + { + "x": 2.4674164806885255, + "y": 4.721617486978178, + "heading": -0.47313850163583826, + "angularVelocity": 0.8959354057929544, + "velocityX": -0.9733988035419044, + "velocityY": 1.5554267872170555, + "timestamp": 1.7345010960971148 + }, + { + "x": 2.377203340708699, + "y": 4.858509175719519, + "heading": -0.39452181655964974, + "angularVelocity": 1.0361539543083473, + "velocityX": -1.188993171512905, + "velocityY": 1.804208158404987, + "timestamp": 1.810374652095942 + }, + { + "x": 2.270630715712783, + "y": 5.014275370945325, + "heading": -0.30523452580590593, + "angularVelocity": 1.1767906430312367, + "velocityX": -1.4046082801148865, + "velocityY": 2.052970803512055, + "timestamp": 1.8862482080947693 + }, + { + "x": 2.156210238547871, + "y": 5.1464603718429265, + "heading": -0.22910381899713708, + "angularVelocity": 1.0033892020445663, + "velocityX": -1.508041578600413, + "velocityY": 1.7421748481347223, + "timestamp": 1.9621217640935966 + }, + { + "x": 2.0581337448378676, + "y": 5.259758714146455, + "heading": -0.16374376524716946, + "angularVelocity": 0.8614339065879271, + "velocityX": -1.2926307778416481, + "velocityY": 1.4932520403643688, + "timestamp": 2.0379953200924237 + }, + { + "x": 1.9764031908936424, + "y": 5.354172852013204, + "heading": -0.10920543384793971, + "angularVelocity": 0.7188055269306859, + "velocityX": -1.077194193248253, + "velocityY": 1.24436157795548, + "timestamp": 2.1138688760912507 + }, + { + "x": 1.9110187094739324, + "y": 5.42970361761196, + "heading": -0.0655341593932664, + "angularVelocity": 0.5755796453747686, + "velocityX": -0.8617558589167408, + "velocityY": 0.995482083384748, + "timestamp": 2.189742432090078 + }, + { + "x": 1.8619802018876241, + "y": 5.486351463195586, + "heading": -0.03276467101544991, + "angularVelocity": 0.4318960400201182, + "velocityX": -0.646318825315813, + "velocityY": 0.7466085494271616, + "timestamp": 2.265615988088905 + }, + { + "x": 1.8292876471115809, + "y": 5.524116647521472, + "heading": -0.010918058172762177, + "angularVelocity": 0.2879344793491674, + "velocityX": -0.43088206879563057, + "velocityY": 0.4977384258464482, + "timestamp": 2.341489544087732 }, { - "x": 7.936841490598973, - "y": 5.885031694896893, - "heading": -0.3756842946663964, - "angularVelocity": 0.1460831755210139, - "velocityX": 0.7784662456231665, - "velocityY": -0.3032523072130836, - "timestamp": 8.257970032635184 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 2.8937989389363637e-30, + "angularVelocity": 0.14389806868982188, + "velocityX": -0.21544335593050407, + "velocityY": 0.24886958055725772, + "timestamp": 2.417363100086559 }, { - "x": 7.970067341476633, - "y": 5.8713284070268905, - "heading": -0.36223415085714655, - "angularVelocity": 0.206654623418748, - "velocityX": 0.510498311264806, - "velocityY": -0.21054405324847428, - "timestamp": 8.32305516640446 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.8511737998863575e-30, + "angularVelocity": -5.603397953128688e-29, + "velocityX": 0, + "velocityY": 2.843834189498754e-31, + "timestamp": 2.493236656085386 }, { - "x": 7.985864639282227, - "y": 5.863699913024902, - "heading": -0.344987478573796, - "angularVelocity": 0.2649863537883773, - "velocityX": 0.24271745160106833, - "velocityY": -0.1172079330593501, - "timestamp": 8.388140300173736 + "x": 1.855209395178807, + "y": 5.543066072468525, + "heading": -4.900273115413656e-27, + "angularVelocity": -4.980371579437602e-26, + "velocityX": 0.42942883009919236, + "velocityY": 0.0006787122433542538, + "timestamp": 2.591665542633167 }, { - "x": 7.985045656249217, - "y": 5.861990715165632, - "heading": -0.3251381624172612, - "angularVelocity": 0.31897227576671205, - "velocityX": -0.01316080008970204, - "velocityY": -0.02746627272232015, - "timestamp": 8.450369270101682 + "x": 1.939745796780934, + "y": 5.543199682246804, + "heading": 4.528417643864408e-20, + "angularVelocity": 4.600700356082414e-19, + "velocityX": 0.8588576440015887, + "velocityY": 0.0013574244611094727, + "timestamp": 2.6900944291809483 }, { - "x": 7.968302412952749, - "y": 5.865866788787815, - "heading": -0.3020436028872691, - "angularVelocity": 0.37112231741475454, - "velocityX": -0.26905866055398503, - "velocityY": 0.062287285594966935, - "timestamp": 8.512598240029627 + "x": 2.066550395198543, + "y": 5.5434000969079245, + "heading": 6.339784356887693e-20, + "angularVelocity": 1.8402795932716144e-19, + "velocityX": 1.2882864255103954, + "velocityY": 0.0020361366276666244, + "timestamp": 2.7885233157287295 }, { - "x": 7.935633552055529, - "y": 5.875328955639361, - "heading": -0.2758355383990043, - "angularVelocity": 0.4211553641111372, - "velocityX": -0.5249783330022537, - "velocityY": 0.1520540491430483, - "timestamp": 8.574827209957572 + "x": 2.2356231808662415, + "y": 5.543667316436768, + "heading": 5.434100687876101e-20, + "angularVelocity": -9.20140114123854e-20, + "velocityX": 1.7177151098384495, + "velocityY": 0.002714848640629602, + "timestamp": 2.8869522022765106 }, { - "x": 7.887037558008595, - "y": 5.8903781286161365, - "heading": -0.24666763452179902, - "angularVelocity": 0.468719053376244, - "velocityX": -0.7809223598462517, - "velocityY": 0.24183548264097016, - "timestamp": 8.637056179885517 + "x": 2.4046959665339394, + "y": 5.543934535965611, + "heading": 4.338082196797977e-27, + "angularVelocity": -5.5208388966484585e-19, + "velocityX": 1.7177151098384498, + "velocityY": 0.0027148486406296025, + "timestamp": 2.9853810888242918 }, { - "x": 7.822512734192608, - "y": 5.911015324029987, - "heading": -0.21472164060862464, - "angularVelocity": 0.5133620876283883, - "velocityX": -1.0368936508301385, - "velocityY": 0.33163324795100674, - "timestamp": 8.699285149813463 + "x": 2.5315005649515485, + "y": 5.544134950626731, + "heading": -2.717050695881145e-20, + "angularVelocity": -2.760420466983953e-19, + "velocityX": 1.2882864255103954, + "velocityY": 0.0020361366276666244, + "timestamp": 3.083809975372073 }, { - "x": 7.742057181096897, - "y": 5.937241677003945, - "heading": -0.1802161756247138, - "angularVelocity": 0.5544919837796514, - "velocityX": -1.29289546635384, - "velocityY": 0.4214492543316279, - "timestamp": 8.761514119741408 + "x": 2.6160369665536756, + "y": 5.54426856040501, + "heading": 3.0657579714860263e-27, + "angularVelocity": 2.760420337720658e-19, + "velocityX": 0.8588576440015887, + "velocityY": 0.0013574244611094727, + "timestamp": 3.182238861919854 }, { - "x": 7.645668783226514, - "y": 5.9690584605532635, - "heading": -0.14341967249459378, - "angularVelocity": 0.5913082471512265, - "velocityX": -1.5489312772168387, - "velocityY": 0.5112857176674774, - "timestamp": 8.823743089669353 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 1.737489718984482e-38, + "angularVelocity": -3.114693341043594e-26, + "velocityX": 0.42942883009919236, + "velocityY": 0.0006787122433542539, + "timestamp": 3.280667748467635 }, { - "x": 7.533345224831275, - "y": 6.006467108033043, - "heading": -0.10467019490543185, - "angularVelocity": 0.6226919332592115, - "velocityX": -1.8050043014579413, - "velocityY": 0.6011452145695896, - "timestamp": 8.885972059597298 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 0, + "angularVelocity": -1.1323659927897137e-37, + "velocityX": 0, + "velocityY": 3.593395183653217e-38, + "timestamp": 3.3790966350154164 + }, + { + "x": 2.6753320649100156, + "y": 5.539342065750356, + "heading": 1.5795430701564016e-18, + "angularVelocity": 2.476798764658572e-17, + "velocityX": 0.2669898507573085, + "velocityY": -0.07829731508072835, + "timestamp": 3.442870208092684 + }, + { + "x": 2.7093452100309583, + "y": 5.5292188917484815, + "heading": 4.421175983497276e-18, + "angularVelocity": 4.455815749727496e-17, + "velocityX": 0.5333423153150382, + "velocityY": -0.15873618982597607, + "timestamp": 3.506643781169952 + }, + { + "x": 2.7602957743928864, + "y": 5.513806855363461, + "heading": 7.811064020414863e-18, + "angularVelocity": 5.3155058958518925e-17, + "velocityX": 0.7989291159865928, + "velocityY": -0.24166807097898507, + "timestamp": 3.5704173542472195 + }, + { + "x": 2.8281241133670565, + "y": 5.492918602334042, + "heading": 1.1328926559518176e-17, + "angularVelocity": 5.516176010462958e-17, + "velocityX": 1.0635806604718465, + "velocityY": -0.3275377561817081, + "timestamp": 3.6341909273244872 + }, + { + "x": 2.9127559105445995, + "y": 5.466330173559929, + "heading": 1.5973697872969206e-17, + "angularVelocity": 7.283222641176841e-17, + "velocityX": 1.3270668882705299, + "velocityY": -0.41691922674457305, + "timestamp": 3.697964500401755 + }, + { + "x": 3.0140963269470453, + "y": 5.433769288149837, + "heading": 1.634340357557127e-17, + "angularVelocity": 5.797161500644316e-18, + "velocityX": 1.5890659957167883, + "velocityY": -0.5105701913650192, + "timestamp": 3.7617380734790227 + }, + { + "x": 3.132020733935084, + "y": 5.394898138842126, + "heading": 1.753294263521122e-17, + "angularVelocity": 1.8652539010143133e-17, + "velocityX": 1.8491108667403897, + "velocityY": -0.6095181347392088, + "timestamp": 3.8255116465562904 + }, + { + "x": 3.2663592482856187, + "y": 5.34928716553499, + "heading": 1.3361291372353952e-17, + "angularVelocity": -6.54134786803134e-17, + "velocityX": 2.1064918879136747, + "velocityY": -0.7152017851010761, + "timestamp": 3.889285219633558 + }, + { + "x": 3.4168692069208895, + "y": 5.296373281622491, + "heading": 9.91837272236996e-18, + "angularVelocity": -5.398660422887962e-17, + "velocityX": 2.36006783645184, + "velocityY": -0.8297149016315647, + "timestamp": 3.953058792710826 + }, + { + "x": 3.5831820501397047, + "y": 5.235389893074956, + "heading": 8.197617273877545e-18, + "angularVelocity": -2.6982264995683312e-17, + "velocityX": 2.6078645933372964, + "velocityY": -0.9562485776615884, + "timestamp": 4.016832365788094 + }, + { + "x": 3.7646897201959026, + "y": 5.165243249909899, + "heading": 6.703480606914857e-18, + "angularVelocity": -2.342877456705142e-17, + "velocityX": 2.8461267151565104, + "velocityY": -1.0999327743494527, + "timestamp": 4.080605938865362 + }, + { + "x": 3.960268086544922, + "y": 5.08428626917176, + "heading": 4.5447273483145725e-18, + "angularVelocity": -3.385027926825969e-17, + "velocityX": 3.066761934634853, + "velocityY": -1.2694440162550131, + "timestamp": 4.144379511942629 }, { - "x": 7.405084087505747, - "y": 6.04946923502926, - "heading": -0.06440725119965417, - "angularVelocity": 0.6470128583583754, - "velocityX": -2.0611161887146876, - "velocityY": 0.6910306734308482, - "timestamp": 8.948201029525244 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 2.916776997028848e-18, + "angularVelocity": -2.5527036870167268e-17, + "velocityX": 3.2494804486361053, + "velocityY": -1.4792721930373927, + "timestamp": 4.208153085019897 + }, + { + "x": 4.281965617918933, + "y": 4.933633775459018, + "heading": 2.5007263674842835e-18, + "angularVelocity": -1.1988032936813997e-17, + "velocityX": 3.298225514699012, + "velocityY": -1.6226254295188278, + "timestamp": 4.242858581053939 + }, + { + "x": 4.397154085355019, + "y": 4.872114684924303, + "heading": 2.80280710947709e-18, + "angularVelocity": 8.704118266930975e-18, + "velocityX": 3.31902668450838, + "velocityY": -1.772603695805773, + "timestamp": 4.277564077087981 + }, + { + "x": 4.510378349293925, + "y": 4.805721616464948, + "heading": 5.6088052918749416e-18, + "angularVelocity": 8.085169506424825e-17, + "velocityX": 3.2624303605355687, + "velocityY": -1.9130419111206762, + "timestamp": 4.312269573122023 + }, + { + "x": 4.620854325513756, + "y": 4.734849659733664, + "heading": 8.920577651651403e-18, + "angularVelocity": 9.542501154652829e-17, + "velocityX": 3.183241527839498, + "velocityY": -2.0420960605711302, + "timestamp": 4.346975069156065 + }, + { + "x": 4.728404969374526, + "y": 4.659612469130667, + "heading": 1.0355720889195964e-17, + "angularVelocity": 4.1352045109421545e-17, + "velocityX": 3.0989513521223016, + "velocityY": -2.167875385765943, + "timestamp": 4.381680565190107 + }, + { + "x": 4.836822070451743, + "y": 4.585629308888239, + "heading": 8.659234104719362e-18, + "angularVelocity": -4.888236672406423e-17, + "velocityX": 3.1239173464304306, + "velocityY": -2.131741905370206, + "timestamp": 4.4163860612241495 + }, + { + "x": 4.9481136166787945, + "y": 4.516045063115851, + "heading": 6.20990752321007e-18, + "angularVelocity": -7.057460233695506e-17, + "velocityX": 3.2067412642045734, + "velocityY": -2.0049921114550315, + "timestamp": 4.4510915572581915 + }, + { + "x": 5.062101259723399, + "y": 4.450971317120919, + "heading": 3.721439651007284e-18, + "angularVelocity": -7.170241479222451e-17, + "velocityX": 3.2844262745242743, + "velocityY": -1.8750271118759503, + "timestamp": 4.4857970532922335 + }, + { + "x": 5.178602303777092, + "y": 4.39051239286923, + "heading": 1.522860892330953e-18, + "angularVelocity": -6.334958464560708e-17, + "velocityX": 3.356847109732077, + "velocityY": -1.7420561916875086, + "timestamp": 4.520502549326276 + }, + { + "x": 5.2974300176780025, + "y": 4.334765207042833, + "heading": -2.608193001732958e-19, + "angularVelocity": -5.139474712462447e-17, + "velocityX": 3.42388749563912, + "velocityY": -1.606292725847098, + "timestamp": 4.555208045360318 + }, + { + "x": 5.4183939381282835, + "y": 4.283819120698816, + "heading": -2.822662772810701e-18, + "angularVelocity": -7.38166505421658e-17, + "velocityX": 3.485439894926986, + "velocityY": -1.4679544212261044, + "timestamp": 4.58991354139436 + }, + { + "x": 5.5413001762936585, + "y": 4.23775579768525, + "heading": -5.4175777354584686e-18, + "angularVelocity": -7.476956848858907e-17, + "velocityX": 3.5414056045998694, + "velocityY": -1.3272630642818801, + "timestamp": 4.624619037428402 }, { - "x": 7.260883177292953, - "y": 6.098066643466027, - "heading": -0.023226072117979224, - "angularVelocity": 0.6617686124221249, - "velocityX": -2.3172633321708815, - "velocityY": 0.7809450886466088, - "timestamp": 9.010429999453189 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -7.665872207711091e-18, + "angularVelocity": -6.478208725348017e-17, + "velocityX": 3.591694883279384, + "velocityY": -1.1844441897757751, + "timestamp": 4.659324533462444 + }, + { + "x": 5.800468403278387, + "y": 4.158565232862714, + "heading": -9.714096811795654e-18, + "angularVelocity": -5.540829464294432e-17, + "velocityX": 3.6389268627432543, + "velocityY": -1.0302389285738007, + "timestamp": 4.6962905620558795 + }, + { + "x": 5.936486438230336, + "y": 4.1262510009162545, + "heading": -1.1401685441264777e-17, + "angularVelocity": -4.5652419090776246e-17, + "velocityX": 3.67954146353996, + "velocityY": -0.8741602269982254, + "timestamp": 4.733256590649315 + }, + { + "x": 6.0737584862384155, + "y": 4.099765139631346, + "heading": -1.2627280609090703e-17, + "angularVelocity": -3.3154634524185157e-17, + "velocityX": 3.713464854930551, + "velocityY": -0.7164919330720612, + "timestamp": 4.770222619242751 + }, + { + "x": 6.212034919724313, + "y": 4.079155810434451, + "heading": -1.3669283004490658e-17, + "angularVelocity": -2.818810770451562e-17, + "velocityX": 3.7406353548742786, + "velocityY": -0.5575207827587797, + "timestamp": 4.8071886478361865 + }, + { + "x": 6.351064277842407, + "y": 4.06446048840337, + "heading": -1.3375402013759826e-17, + "angularVelocity": 7.950028767305027e-18, + "velocityX": 3.761003370072177, + "velocityY": -0.3975358617152357, + "timestamp": 4.844154676429622 + }, + { + "x": 6.490086441164715, + "y": 4.055726900176193, + "heading": -1.4576849056839345e-17, + "angularVelocity": -3.2501382723403574e-17, + "velocityX": 3.7608087374308066, + "velocityY": -0.23625984612066567, + "timestamp": 4.881120705023058 + }, + { + "x": 6.625430002884342, + "y": 4.051684818750479, + "heading": -1.3372180037518883e-17, + "angularVelocity": 3.2588543188391143e-17, + "velocityX": 3.661295704988517, + "velocityY": -0.10934583939674931, + "timestamp": 4.9180867336164935 + }, + { + "x": 6.755501460439252, + "y": 4.050426142633095, + "heading": -1.1630354881514603e-17, + "angularVelocity": 4.7119618262525755e-17, + "velocityX": 3.518675457011608, + "velocityY": -0.03404953589216864, + "timestamp": 4.955052762209929 + }, + { + "x": 6.8798297740402194, + "y": 4.050767085715321, + "heading": -9.335447591441833e-18, + "angularVelocity": 6.208152126139743e-17, + "velocityX": 3.3633127044392923, + "velocityY": 0.009223146093816154, + "timestamp": 4.992018790803365 + }, + { + "x": 6.998265582982919, + "y": 4.052014068285868, + "heading": -6.4179877740703305e-18, + "angularVelocity": 7.892272793106084e-17, + "velocityX": 3.203909466318293, + "velocityY": 0.03373320364655125, + "timestamp": 5.0289848193968005 + }, + { + "x": 7.110758098746119, + "y": 4.053729886887297, + "heading": -3.1976006784105176e-18, + "angularVelocity": 8.71174756444278e-17, + "velocityX": 3.0431323039981164, + "velocityY": 0.046416092469654044, + "timestamp": 5.065950847990236 }, { - "x": 7.100741532540205, - "y": 6.152261239581916, - "heading": 0.018021968863737303, - "angularVelocity": 0.6628430621538008, - "velocityX": -2.5734259290837285, - "velocityY": 0.8708901365174432, - "timestamp": 9.072658969381134 + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": -8.646914705534956e-42, + "angularVelocity": 8.650106057047145e-17, + "velocityX": 2.8819232262850263, + "velocityY": 0.051084720684648116, + "timestamp": 5.102916876583672 + }, + { + "x": 7.405338431031862, + "y": 4.059210715077462, + "heading": 6.035426461355953e-18, + "angularVelocity": 8.221730826644903e-17, + "velocityX": 2.561662296238626, + "velocityY": 0.04893769145541128, + "timestamp": 5.176325099117501 + }, + { + "x": 7.569876923635766, + "y": 4.062517669134614, + "heading": 1.1021958875984658e-17, + "angularVelocity": 6.792879928853616e-17, + "velocityX": 2.2414177448320305, + "velocityY": 0.04504882345608618, + "timestamp": 5.24973332165133 + }, + { + "x": 7.710907923869345, + "y": 4.065460118931574, + "heading": 1.4466516010520595e-17, + "angularVelocity": 4.692331479554e-17, + "velocityX": 1.9211880544932918, + "velocityY": 0.04008338160760226, + "timestamp": 5.32314154418516 + }, + { + "x": 7.828432324355203, + "y": 4.067984380811152, + "heading": 1.5902740281761073e-17, + "angularVelocity": 1.956489643348339e-17, + "velocityX": 1.600970523863292, + "velocityY": 0.03438663670702133, + "timestamp": 5.396549766718989 + }, + { + "x": 7.922450847470263, + "y": 4.070051609380546, + "heading": 1.5219645547347743e-17, + "angularVelocity": -9.30542534385069e-18, + "velocityX": 1.2807628337783643, + "velocityY": 0.02816072230112734, + "timestamp": 5.469957989252818 + }, + { + "x": 7.992964082884089, + "y": 4.071632392724107, + "heading": 1.2631622975679932e-17, + "angularVelocity": -3.5255213685022036e-17, + "velocityX": 0.9605631764388524, + "velocityY": 0.021534145481204726, + "timestamp": 5.543366211786648 + }, + { + "x": 8.039972518291798, + "y": 4.072703688242487, + "heading": 7.596971431802736e-18, + "angularVelocity": -6.858429982495519e-17, + "velocityX": 0.6403701627027665, + "velocityY": 0.014593671953933919, + "timestamp": 5.616774434320477 }, { - "x": 6.924662708731195, - "y": 6.212054571383467, - "heading": 0.05802124542072086, - "angularVelocity": 0.6427758100977419, - "velocityX": -2.829531390490462, - "velocityY": 0.9608600603671181, - "timestamp": 9.13488793930908 + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 8.652482395157843e-45, + "angularVelocity": -1.0348937993018065e-16, + "velocityX": 0.32018271791516006, + "velocityY": 0.007400637290262982, + "timestamp": 5.690182656854306 }, { - "x": 6.732667433541327, - "y": 6.277445378277029, - "heading": 0.09448040335619008, - "angularVelocity": 0.5858872158366861, - "velocityX": -3.0853037646642187, - "velocityY": 1.050809726872821, - "timestamp": 9.197116909237025 + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 0, + "angularVelocity": 5.394624259127602e-43, + "velocityX": 0, + "velocityY": -7.823862347152674e-43, + "timestamp": 5.763590879388135 + }, + { + "x": 8.042595807906332, + "y": 4.071940125417329, + "heading": 2.4014031773112573e-19, + "angularVelocity": 3.467784621738616e-18, + "velocityX": -0.30153187250835495, + "velocityY": -0.01887149394694188, + "timestamp": 5.832839793010322 + }, + { + "x": 8.000826969409328, + "y": 4.069449378892138, + "heading": 7.295832460611546e-19, + "angularVelocity": 7.067878797353639e-18, + "velocityX": -0.6031695850838968, + "velocityY": -0.03596802310545437, + "timestamp": 5.902088706632509 + }, + { + "x": 7.938162262991824, + "y": 4.0659211372128246, + "heading": 1.4792287784714074e-18, + "angularVelocity": 1.0825376070160765e-17, + "velocityX": -0.9049197040027859, + "velocityY": -0.0509501376232871, + "timestamp": 5.971337620254696 + }, + { + "x": 7.8545936374958405, + "y": 4.061532754567933, + "heading": 2.6959287825754293e-18, + "angularVelocity": 1.756995078279752e-17, + "velocityX": -1.206786086954711, + "velocityY": -0.06337114064826968, + "timestamp": 6.0405865338768825 + }, + { + "x": 7.7501132248715185, + "y": 4.056503424652652, + "heading": 4.202275116563444e-18, + "angularVelocity": 2.17526348818473e-17, + "velocityX": -1.5087660897375712, + "velocityY": -0.07262684210066948, + "timestamp": 6.109835447499069 + }, + { + "x": 7.624714358960079, + "y": 4.051110859560637, + "heading": 6.0188244090330196e-18, + "angularVelocity": 2.6232170260178098e-17, + "velocityX": -1.8108423562512328, + "velocityY": -0.07787219769881376, + "timestamp": 6.179084361121256 + }, + { + "x": 7.478393887159083, + "y": 4.045718105040055, + "heading": 8.17237550228445e-18, + "angularVelocity": 3.1098698601987103e-17, + "velocityX": -2.1129641484240804, + "velocityY": -0.07787493317230812, + "timestamp": 6.248333274743443 + }, + { + "x": 7.311157646174121, + "y": 4.040819461213868, + "heading": 1.0532118174804612e-17, + "angularVelocity": 3.4076241042489266e-17, + "velocityX": -2.4150016547173685, + "velocityY": -0.07073964875337486, + "timestamp": 6.31758218836563 + }, + { + "x": 7.123034533164843, + "y": 4.037125830040512, + "heading": 8.283291482043014e-18, + "angularVelocity": -3.247454111743766e-17, + "velocityX": -2.716621866960299, + "velocityY": -0.053338471033761534, + "timestamp": 6.386831101987816 + }, + { + "x": 6.914117681129227, + "y": 4.035741699141579, + "heading": 5.798379315469736e-18, + "angularVelocity": -3.588377111777714e-17, + "velocityX": -3.016897177267489, + "velocityY": -0.019987763367446825, + "timestamp": 6.456080015610003 + }, + { + "x": 6.684712125750405, + "y": 4.03859150608828, + "heading": 3.611288298800787e-18, + "angularVelocity": -3.158303722425788e-17, + "velocityX": -3.312767571061532, + "velocityY": 0.041153092483867636, + "timestamp": 6.52532892923219 + }, + { + "x": 6.43606725460267, + "y": 4.049660896865006, + "heading": 2.3554139869933522e-18, + "angularVelocity": -1.8135653631468075e-17, + "velocityX": -3.5905959839934547, + "velocityY": 0.15984930589841134, + "timestamp": 6.594577842854377 + }, + { + "x": 6.175716481433539, + "y": 4.078070527288863, + "heading": 2.6764824042330446e-18, + "angularVelocity": 4.636439771335605e-18, + "velocityX": -3.759636932206204, + "velocityY": 0.41025380670743106, + "timestamp": 6.663826756476563 + }, + { + "x": 5.918464051979435, + "y": 4.12717103366876, + "heading": 2.4806366122903936e-18, + "angularVelocity": -2.8281424458318553e-18, + "velocityX": -3.7148948048144055, + "velocityY": 0.7090437064151481, + "timestamp": 6.73307567009875 }, { - "x": 6.524862060286313, - "y": 6.348410321942265, - "heading": 0.12245319566137218, - "angularVelocity": 0.44951398581032764, - "velocityX": -3.33936707446111, - "velocityY": 1.1403843539014786, - "timestamp": 9.25934587916497 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 3.0661477431883128e-18, + "angularVelocity": 8.455167023881293e-18, + "velocityX": -3.6464445426004524, + "velocityY": 1.0033087488526664, + "timestamp": 6.802324583720937 + }, + { + "x": 5.541093538436437, + "y": 4.236435061250764, + "heading": 3.354859743531915e-18, + "angularVelocity": 8.33229202553877e-18, + "velocityX": -3.6034349206984246, + "velocityY": 1.1482323536375627, + "timestamp": 6.83697435349121 + }, + { + "x": 5.417925084630127, + "y": 4.281179055760442, + "heading": 3.9245083052882345e-18, + "angularVelocity": 1.6440183168115343e-17, + "velocityX": -3.554668750266214, + "velocityY": 1.2913215529664346, + "timestamp": 6.871624123261483 + }, + { + "x": 5.296643156826778, + "y": 4.330809566180666, + "heading": 4.682534245858068e-18, + "angularVelocity": 2.1876795880478242e-17, + "velocityX": -3.500223193615562, + "velocityY": 1.4323474802075717, + "timestamp": 6.906273893031757 + }, + { + "x": 5.177441529342695, + "y": 4.385247292254255, + "heading": 5.748631283020555e-18, + "angularVelocity": 3.076779569476711e-17, + "velocityX": -3.4401852674457762, + "velocityY": 1.5710847845312972, + "timestamp": 6.94092366280203 + }, + { + "x": 5.060510651244131, + "y": 4.444405251423305, + "heading": 6.298107237796981e-18, + "angularVelocity": 1.5857997280196432e-17, + "velocityX": -3.374650939207116, + "velocityY": 1.7073117530438153, + "timestamp": 6.975573432572303 + }, + { + "x": 4.946037340921014, + "y": 4.5081889164365, + "heading": 6.498139126492573e-18, + "angularVelocity": 5.772964438776829e-18, + "velocityX": -3.303724991019314, + "velocityY": 1.840810644228753, + "timestamp": 7.010223202342576 + }, + { + "x": 4.834204484779198, + "y": 4.576496362942444, + "heading": 6.284318947548799e-18, + "angularVelocity": -6.170897537311036e-18, + "velocityX": -3.227520900810133, + "velocityY": 1.971367976145888, + "timestamp": 7.04487297211285 + }, + { + "x": 4.725190735681975, + "y": 4.649218420709879, + "heading": 4.218601510181254e-18, + "angularVelocity": -5.961706098087163e-17, + "velocityX": -3.146160849551947, + "velocityY": 2.098774631103723, + "timestamp": 7.079522741883123 + }, + { + "x": 4.619170166207779, + "y": 4.726238769851069, + "heading": 2.3861247317221027e-18, + "angularVelocity": -5.2885684107236655e-17, + "velocityX": -3.059777025276331, + "velocityY": 2.22282426843907, + "timestamp": 7.114172511653396 + }, + { + "x": 4.5101566662173465, + "y": 4.798961201296667, + "heading": 8.546946946648484e-19, + "angularVelocity": -4.419740873346574e-17, + "velocityX": -3.146153660275008, + "velocityY": 2.0987854155379675, + "timestamp": 7.148822281423669 + }, + { + "x": 4.398324060290128, + "y": 4.867269057737162, + "heading": -1.0977695137154222e-18, + "angularVelocity": -5.634854780637897e-17, + "velocityX": -3.2275136795616595, + "velocityY": 1.9713798069474653, + "timestamp": 7.1834720511939425 + }, + { + "x": 4.283850992630547, + "y": 4.931053147799285, + "heading": -2.453093242554037e-18, + "angularVelocity": -3.911494182571376e-17, + "velocityX": -3.3037179876961247, + "velocityY": 1.8408229112346155, + "timestamp": 7.218121820964216 }, { - "x": 6.30228026524581, - "y": 6.424202518084616, - "heading": 0.1231057628389814, - "angularVelocity": 0.010486549566300301, - "velocityX": -3.5768195311320152, - "velocityY": 1.2179567849204198, - "timestamp": 9.321574849092915 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -1.5610631193350687e-18, + "angularVelocity": 2.5744186155726097e-17, + "velocityX": -3.3579422894520032, + "velocityY": 1.6997125365941679, + "timestamp": 7.252771590734489 + }, + { + "x": 3.941206853693096, + "y": 5.086799964367495, + "heading": 3.827307300721309e-18, + "angularVelocity": 7.831135017939884e-17, + "velocityX": -3.2887955452772286, + "velocityY": 1.4075914407764616, + "timestamp": 7.321578607951182 + }, + { + "x": 3.730310577999757, + "y": 5.1698821724733195, + "heading": 7.528976478562542e-18, + "angularVelocity": 5.37978439929115e-17, + "velocityX": -3.0650402273530597, + "velocityY": 1.2074670791813904, + "timestamp": 7.390385625167876 + }, + { + "x": 3.5370931608840936, + "y": 5.242282386882321, + "heading": 5.4493611354862e-18, + "angularVelocity": -3.022388452804216e-17, + "velocityX": -2.808106279439004, + "velocityY": 1.0522213770870446, + "timestamp": 7.459192642384569 + }, + { + "x": 3.3623805585841375, + "y": 5.305505508408588, + "heading": 3.90044719086442e-18, + "angularVelocity": -2.2510988083436363e-17, + "velocityX": -2.5391683779829535, + "velocityY": 0.9188470025834666, + "timestamp": 7.527999659601263 + }, + { + "x": 3.2065789543659307, + "y": 5.3604207146722205, + "heading": 2.734702057316729e-18, + "angularVelocity": -1.6942241950067733e-17, + "velocityX": -2.2643272520816113, + "velocityY": 0.7981047353162919, + "timestamp": 7.596806676817956 + }, + { + "x": 3.0699254867036734, + "y": 5.407590197311926, + "heading": 1.8584817371846494e-18, + "angularVelocity": -1.2734461622898273e-17, + "velocityX": -1.9860396975485235, + "velocityY": 0.6855330247953545, + "timestamp": 7.66561369403465 + }, + { + "x": 2.952574245770292, + "y": 5.4474063504067525, + "heading": 1.2072006114656177e-18, + "angularVelocity": -9.465330021034912e-18, + "velocityX": -1.7055126886812788, + "velocityY": 0.5786641349302285, + "timestamp": 7.734420711251343 + }, + { + "x": 2.8546328911186802, + "y": 5.480158244239293, + "heading": 7.336690244970979e-19, + "angularVelocity": -6.882024626605053e-18, + "velocityX": -1.4234210203178261, + "velocityY": 0.4759964195133704, + "timestamp": 7.803227728468037 + }, + { + "x": 2.7761806703190945, + "y": 5.5060675412854945, + "heading": 4.019181330238403e-19, + "angularVelocity": -4.821468868916063e-18, + "velocityX": -1.1401776152062642, + "velocityY": 0.37655021383364773, + "timestamp": 7.87203474568473 + }, + { + "x": 2.7172782497291417, + "y": 5.525309545004062, + "heading": 1.8363150875881817e-19, + "angularVelocity": -3.1724471295939874e-18, + "velocityX": -0.856052521568438, + "velocityY": 0.2796517636852234, + "timestamp": 7.940841762901424 + }, + { + "x": 2.6779735090707537, + "y": 5.538026332588319, + "heading": 5.594227372773024e-20, + "angularVelocity": -1.8557589065219566e-18, + "velocityX": -0.5712315727130863, + "velocityY": 0.18481817841642284, + "timestamp": 8.009648780118116 }, { - "x": 6.078984969036473, - "y": 6.498549743391548, - "heading": 0.1231057748040032, - "angularVelocity": 1.9227414178241924e-7, - "velocityX": -3.5882852707972814, - "velocityY": 1.194736557475109, - "timestamp": 9.38380381902086 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -4.379352373324341e-45, + "angularVelocity": -8.130315190317274e-19, + "velocityX": -0.2858478933472287, + "velocityY": 0.09169170474607218, + "timestamp": 8.07845579733481 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": 0.12310578689683861, - "angularVelocity": 1.9432806685756468e-7, - "velocityX": -3.662936913327793, - "velocityY": 0.941318096340742, - "timestamp": 9.446032788948806 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -7.265203918119814e-45, + "angularVelocity": -4.19412391684243e-44, + "velocityX": 0, + "velocityY": -6.607621745560535e-42, + "timestamp": 8.147262814551503 }, { - "x": 5.606427257807288, - "y": 6.600564902613298, - "heading": 0.1231057972666647, - "angularVelocity": 1.578555424299444e-7, - "velocityX": -3.7237014740499577, - "velocityY": 0.6612371129043467, - "timestamp": 9.511724660841988 + "x": 2.6751665057839835, + "y": 5.538944468629318, + "heading": 4.149201789004472e-19, + "angularVelocity": 6.513823637389646e-18, + "velocityX": 0.2647058041795455, + "velocityY": -0.08463157955674916, + "timestamp": 8.210961219973287 }, { - "x": 5.35931577124569, - "y": 6.62625930787543, - "heading": 0.12310580746861746, - "angularVelocity": 1.5530007712924032e-7, - "velocityX": -3.7616752185629396, - "velocityY": 0.39113522756534835, - "timestamp": 9.577416532735171 + "x": 2.7088624712986666, + "y": 5.5280798374619815, + "heading": 1.747454129766702e-18, + "angularVelocity": 2.0919423995667872e-17, + "velocityX": 0.5289922925316967, + "velocityY": -0.1705636286402431, + "timestamp": 8.27465962539507 }, { - "x": 5.111994168115865, - "y": 6.649845615003531, - "heading": 0.1231058176602883, - "angularVelocity": 1.551435596748186e-7, - "velocityX": -3.764873735550995, - "velocityY": 0.35904452785959023, - "timestamp": 9.643108404628354 + "x": 2.7593613029745305, + "y": 5.511644735540107, + "heading": 3.0764256336987273e-18, + "angularVelocity": 2.086349721209079e-17, + "velocityX": 0.79278015425161, + "velocityY": -0.2580143382404696, + "timestamp": 8.338358030816854 }, { - "x": 4.868859374012009, - "y": 6.67294055859981, - "heading": 0.13716866748145323, - "angularVelocity": 0.21407290454489739, - "velocityX": -3.701139685883833, - "velocityY": 0.35156470550620644, - "timestamp": 9.708800276521536 + "x": 2.8266246280660527, + "y": 5.489524723354374, + "heading": 4.563957990922327e-18, + "angularVelocity": 2.3352740894748934e-17, + "velocityX": 1.0559656030026554, + "velocityY": -0.3472616314217525, + "timestamp": 8.402056436238638 }, { - "x": 4.644141674488456, - "y": 6.6943246178055915, - "heading": 0.16550217221046287, - "angularVelocity": 0.43130913935716303, - "velocityX": -3.4207839272559015, - "velocityY": 0.3255206251475534, - "timestamp": 9.77449214841472 + "x": 2.9106051995536757, + "y": 5.461582331644454, + "heading": 6.988237520643839e-18, + "angularVelocity": 3.8058716127490844e-17, + "velocityX": 1.31840932173323, + "velocityY": -0.43866705178720633, + "timestamp": 8.465754841660422 }, { - "x": 4.438149971299598, - "y": 6.713938607045374, - "heading": 0.1971398735951766, - "angularVelocity": 0.48160754858923427, - "velocityX": -3.1357258859027066, - "velocityY": 0.2985755874284469, - "timestamp": 9.840184020307902 + "x": 3.0112434875257597, + "y": 5.427649372766819, + "heading": 9.422486137058152e-18, + "angularVelocity": 3.821522062123431e-17, + "velocityX": 1.5799184815647955, + "velocityY": -0.5327128466237975, + "timestamp": 8.529453247082206 }, { - "x": 4.2509016025753406, - "y": 6.731775563046276, - "heading": 0.22901708759854217, - "angularVelocity": 0.48525354940713283, - "velocityX": -2.8504039134206605, - "velocityY": 0.27152455070706877, - "timestamp": 9.905875892201085 + "x": 3.128462311204818, + "y": 5.387515427846556, + "heading": 1.2002270824368104e-17, + "angularVelocity": 4.0499988504071535e-17, + "velocityX": 1.8402159819054258, + "velocityY": -0.6300620031932208, + "timestamp": 8.59315165250399 }, { - "x": 4.082393919350406, - "y": 6.747832566507683, - "heading": 0.25967107343501483, - "angularVelocity": 0.46663285659323517, - "velocityX": -2.565122265033534, - "velocityY": 0.24442907468850525, - "timestamp": 9.971567764094267 + "x": 3.2621579049769087, + "y": 5.340909820689257, + "heading": 1.4607724763792015e-17, + "angularVelocity": 4.0902969582545765e-17, + "velocityX": 2.0988844679362657, + "velocityY": -0.7316604999559294, + "timestamp": 8.656850057925773 }, { - "x": 3.9326220842038664, - "y": 6.762107971713798, - "heading": 0.28824128245235414, - "angularVelocity": 0.43491239013245725, - "velocityX": -2.279914254690053, - "velocityY": 0.21730854662395976, - "timestamp": 10.03725963598745 + "x": 3.4121839694430376, + "y": 5.287471786804592, + "heading": 1.2983801342724242e-17, + "angularVelocity": -2.5493941493744293e-17, + "velocityX": 2.3552562026116504, + "velocityY": -0.8389226312781332, + "timestamp": 8.720548463347557 }, { - "x": 3.801581586235111, - "y": 6.77460070477556, - "heading": 0.314159509734349, - "angularVelocity": 0.3945423769342238, - "velocityX": -1.9947749119073828, - "velocityY": 0.19017167119358822, - "timestamp": 10.102951507880633 + "x": 3.5783204640757695, + "y": 5.226697519809262, + "heading": 1.1414351740343512e-17, + "angularVelocity": -2.4638758097451353e-17, + "velocityX": 2.608173525422576, + "velocityY": -0.9540940089929807, + "timestamp": 8.78424686876934 }, { - "x": 3.6892685916749244, - "y": 6.785310000165767, - "heading": 0.3370219943875935, - "angularVelocity": 0.34802607985383205, - "velocityX": -1.7096939289964497, - "velocityY": 0.16302314246152652, - "timestamp": 10.168643379773815 + "x": 3.7602044322338806, + "y": 5.157837260218168, + "heading": 1.0662286375073935e-17, + "angularVelocity": -1.1806659213675993e-17, + "velocityX": 2.855392799140749, + "velocityY": -1.081035845954548, + "timestamp": 8.847945274191124 }, { - "x": 3.595679898494151, - "y": 6.794235280663889, - "heading": 0.35652698961990326, - "angularVelocity": 0.2969164170572818, - "velocityX": -1.4246616892414212, - "velocityY": 0.135865826941808, - "timestamp": 10.234335251666998 + "x": 3.9571467965572817, + "y": 5.079670509622147, + "heading": 1.0033604735305209e-17, + "angularVelocity": -9.869660560666429e-18, + "velocityX": 3.091794261085985, + "velocityY": -1.2271382631705257, + "timestamp": 8.911643679612908 }, { - "x": 3.520812828526041, - "y": 6.801376095406927, - "heading": 0.37244068726433127, - "angularVelocity": 0.2422475899347831, - "velocityX": -1.1396702181031793, - "velocityY": 0.10870164812244498, - "timestamp": 10.30002712356018 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 9.614004929327239e-18, + "angularVelocity": -6.58728900981987e-18, + "velocityX": 3.3023160854502263, + "velocityY": -1.4085550989874989, + "timestamp": 8.975342085034692 }, { - "x": 3.464665127058438, - "y": 6.806732084957018, - "heading": 0.38457706026543664, - "angularVelocity": 0.18474695044220302, - "velocityX": -0.8547130695088268, - "velocityY": 0.08153199773025213, - "timestamp": 10.365718995453364 + "x": 4.2842075611264425, + "y": 4.936417577245955, + "heading": 8.433110849470374e-18, + "angularVelocity": -3.4077840923717525e-17, + "velocityX": 3.3679341946759727, + "velocityY": -1.5447568972758872, + "timestamp": 9.009994928285538 }, { - "x": 3.4272348818352074, - "y": 6.810302960282004, - "heading": 0.39278518447772715, - "angularVelocity": 0.12494885555456782, - "velocityX": -0.5697850303930024, - "velocityY": 0.05435794752192765, - "timestamp": 10.431410867346546 + "x": 4.401356584826297, + "y": 4.877666966713146, + "heading": 7.494016874909782e-18, + "angularVelocity": -2.7100055477775347e-17, + "velocityX": 3.3806468015288584, + "velocityY": -1.695405196841212, + "timestamp": 9.044647771536384 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0.0632601721112495, - "velocityX": -0.28488184560874424, - "velocityY": 0.027180367966521624, - "timestamp": 10.497102739239729 + "x": 4.516063976001338, + "y": 4.814281195629901, + "heading": 5.449898069000543e-18, + "angularVelocity": -5.898848735476586e-17, + "velocityX": 3.310186998068025, + "velocityY": -1.8291650882556743, + "timestamp": 9.07930061478723 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": 1.9034366224257262e-34, - "velocityY": -2.547324229323533e-33, - "timestamp": 10.562794611132912 + "x": 4.628146410125642, + "y": 4.746361612927903, + "heading": 3.873819251911276e-18, + "angularVelocity": -4.5481948066433736e-17, + "velocityX": 3.234436877602133, + "velocityY": -1.9600002865663548, + "timestamp": 9.113953458038077 }, { - "x": 3.4428034429775143, - "y": 6.811427919070607, - "heading": 0.39249813327278055, - "angularVelocity": -0.050100574262489476, - "velocityX": 0.38660845958378076, - "velocityY": -0.007449238878501296, - "timestamp": 10.651470846737375 + "x": 4.737424855337868, + "y": 4.674016859733605, + "heading": 3.562150006954802e-18, + "angularVelocity": -8.994045386127452e-18, + "velocityX": 3.1535203163901007, + "velocityY": -2.0877003560892837, + "timestamp": 9.148606301288924 }, { - "x": 3.5113694006298597, - "y": 6.810106800279428, - "heading": 0.3836131655700934, - "angularVelocity": -0.10019558951868686, - "velocityX": 0.7732168284429818, - "velocityY": -0.014898228168725063, - "timestamp": 10.740147082341839 + "x": 4.844483096398323, + "y": 4.598425359059355, + "heading": 4.67240858769656e-18, + "angularVelocity": 3.2039465642249735e-17, + "velocityX": 3.0894504178337554, + "velocityY": -2.181393893916723, + "timestamp": 9.18325914453977 }, { - "x": 3.614218332551768, - "y": 6.808125167746075, - "heading": 0.37028807906085554, - "angularVelocity": -0.1502667137188131, - "velocityX": 1.1598251912796755, - "velocityY": -0.022346827420505266, - "timestamp": 10.828823317946302 + "x": 4.954476829355994, + "y": 4.5271728465414585, + "heading": 6.725892368510384e-18, + "angularVelocity": 5.925873862496592e-17, + "velocityX": 3.174161847599147, + "velocityY": -2.0561808450207577, + "timestamp": 9.217911987790616 }, { - "x": 3.7513502451988723, - "y": 6.8054830693306245, - "heading": 0.3525264976102127, - "angularVelocity": -0.20029697166969943, - "velocityX": 1.5464336269164098, - "velocityY": -0.02979488695523812, - "timestamp": 10.917499553550766 + "x": 5.06723038345343, + "y": 4.460373312794278, + "heading": 8.84315284493419e-18, + "angularVelocity": 6.109918488065446e-17, + "velocityX": 3.2538038302147596, + "velocityY": -1.9276782936288406, + "timestamp": 9.252564831041463 }, { - "x": 3.9227651512282486, - "y": 6.802180567172425, - "heading": 0.3303333796886753, - "angularVelocity": -0.2502713130553812, - "velocityX": 1.9330422052867193, - "velocityY": -0.03724224574585722, - "timestamp": 11.006175789155229 + "x": 5.18256360847748, + "y": 4.398133545147612, + "heading": 1.1174088851587773e-17, + "angularVelocity": 6.726536087617125e-17, + "velocityX": 3.32824709906689, + "velocityY": -1.796094109684512, + "timestamp": 9.28721767429231 }, { - "x": 4.128463068361194, - "y": 6.79821773882544, - "heading": 0.3037147939289921, - "angularVelocity": -0.3001772186001935, - "velocityX": 2.319650983499712, - "velocityY": -0.04468872996211646, - "timestamp": 11.094852024759692 + "x": 5.300292214676822, + "y": 4.340553023175947, + "heading": 1.351328882085302e-17, + "angularVelocity": 6.750383950696769e-17, + "velocityX": 3.3973721967667427, + "velocityY": -1.6616391779124182, + "timestamp": 9.321870517543156 }, { - "x": 4.368444017678072, - "y": 6.793594678448866, - "heading": 0.2726776385751228, - "angularVelocity": -0.3500053327963678, - "velocityX": 2.706259999435507, - "velocityY": -0.05213415234714548, - "timestamp": 11.183528260364156 + "x": 5.420228077668345, + "y": 4.287723772710641, + "heading": 1.5740193529975693e-17, + "angularVelocity": 6.4263260968298e-17, + "velocityX": 3.461068464810413, + "velocityY": -1.5245285959043573, + "timestamp": 9.356523360794002 }, { - "x": 4.642708020512335, - "y": 6.788311497970363, - "heading": 0.23722930806330966, - "angularVelocity": -0.3997500600942169, - "velocityX": 3.092869255948188, - "velocityY": -0.05957831252634942, - "timestamp": 11.27220449596862 + "x": 5.542179542504676, + "y": 4.239730223009142, + "heading": 1.83851008704834e-17, + "angularVelocity": 7.632583916308507e-17, + "velocityX": 3.5192340193716003, + "velocityY": -1.3849815830141698, + "timestamp": 9.391176204044848 }, { - "x": 4.951255088173746, - "y": 6.782368328182758, - "heading": 0.19737735072702745, - "angularVelocity": -0.44940966499796203, - "velocityX": 3.479478640000833, - "velocityY": -0.0670209977576627, - "timestamp": 11.360880731573083 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 2.1337719863838943e-17, + "angularVelocity": 8.520567769813293e-17, + "velocityX": 3.571775782441806, + "velocityY": -1.2432211735943428, + "timestamp": 9.425829047295695 }, { - "x": 5.286562692788269, - "y": 6.775920770205836, - "heading": 0.19737734601955828, - "angularVelocity": -5.3086028753514235e-8, - "velocityX": 3.781256639153567, - "velocityY": -0.07270897251075666, - "timestamp": 11.449556967177546 + "x": 5.936838928321791, + "y": 4.127579524293423, + "heading": 2.7128761029914745e-17, + "angularVelocity": 7.834427123009397e-17, + "velocityX": 3.6647054686449687, + "velocityY": -0.9344094480089555, + "timestamp": 9.499746911041823 }, { - "x": 5.595111114675473, - "y": 6.769985794431735, - "heading": 0.15789301075905496, - "angularVelocity": -0.4452639987631135, - "velocityX": 3.4794939115759345, - "velocityY": -0.06692859404378032, - "timestamp": 11.53823320278201 + "x": 6.212625572925942, + "y": 4.081838891930782, + "heading": 3.048119806681363e-17, + "angularVelocity": 4.535354333849354e-17, + "velocityX": 3.730987756239054, + "velocityY": -0.6188034941017404, + "timestamp": 9.573664774787952 }, { - "x": 5.8693764275565705, - "y": 6.76471055259859, - "heading": 0.12280143262174693, - "angularVelocity": -0.3957269712466434, - "velocityX": 3.092884029318143, - "velocityY": -0.059488788593539996, - "timestamp": 11.626909438386473 + "x": 6.491306353239061, + "y": 4.059759739930169, + "heading": 2.990728340154358e-17, + "angularVelocity": -7.764221477519524e-18, + "velocityX": 3.770141156544387, + "velocityY": -0.2986984590956799, + "timestamp": 9.64758263853408 + }, + { + "x": 6.752875972710039, + "y": 4.054277278014817, + "heading": 2.289191300833474e-17, + "angularVelocity": -9.490764529266178e-17, + "velocityX": 3.538652312373937, + "velocityY": -0.07416964773468503, + "timestamp": 9.72150050228021 + }, + { + "x": 6.991092096287726, + "y": 4.05357587432536, + "heading": 1.9322766501808003e-17, + "angularVelocity": -4.8285303790501985e-17, + "velocityX": 3.2227138543376115, + "velocityY": -0.009488960501710962, + "timestamp": 9.795418366026338 }, { - "x": 6.109358599544783, - "y": 6.760094936608861, - "heading": 0.09209943623091729, - "angularVelocity": -0.3462257523850543, - "velocityX": 2.7062737874738274, - "velocityY": -0.05205020215694004, - "timestamp": 11.715585673990937 + "x": 7.205567187241114, + "y": 4.0550211409111485, + "heading": 1.4185957310010018e-17, + "angularVelocity": -6.949347466859182e-17, + "velocityX": 2.9015325942048964, + "velocityY": 0.01955233163599203, + "timestamp": 9.869336229772466 }, { - "x": 6.315057615509001, - "y": 6.756138852616793, - "heading": 0.06578495581515914, - "angularVelocity": -0.29674782918315123, - "velocityX": 2.3196633749963063, - "velocityY": -0.04461267401690494, - "timestamp": 11.8042619095954 + "x": 7.396227777452996, + "y": 4.057522153925314, + "heading": 7.96211441465592e-18, + "angularVelocity": -8.419944219072616e-17, + "velocityX": 2.579357418481523, + "velocityY": 0.03383502833300578, + "timestamp": 9.943254093518595 }, { - "x": 6.4864734670806214, - "y": 6.752842221795132, - "heading": 0.043856783005957245, - "angularVelocity": -0.24728353272698234, - "velocityX": 1.9330528681462538, - "velocityY": -0.037176034810112814, - "timestamp": 11.892938145199864 + "x": 7.5630549716186115, + "y": 4.0604862007272615, + "heading": 9.124881669040973e-19, + "angularVelocity": -9.537107663127069e-17, + "velocityX": 2.2569266170703433, + "velocityY": 0.040099194588843756, + "timestamp": 10.017171957264724 }, { - "x": 6.623606149810205, - "y": 6.750204980767773, - "heading": 0.026314311963007715, - "angularVelocity": -0.19782606831887856, - "velocityX": 1.5464423111199512, - "velocityY": -0.029740110294281013, - "timestamp": 11.981614380804327 + "x": 7.70604444565388, + "y": 4.0635415850381555, + "heading": -6.759455196513469e-18, + "angularVelocity": -1.0379011208666613e-16, + "velocityX": 1.934437317160139, + "velocityY": 0.04133485677276264, + "timestamp": 10.091089821010852 }, { - "x": 6.726455661694478, - "y": 6.74822708185799, - "heading": 0.013157327563715553, - "angularVelocity": -0.14837102984364775, - "velocityX": 1.1598317315027789, - "velocityY": -0.022304723427886322, - "timestamp": 12.07029061640879 + "x": 7.825196583277639, + "y": 4.066433602906457, + "heading": -4.535875150648457e-18, + "angularVelocity": 3.0081768238080076e-17, + "velocityX": 1.6119532084015247, + "velocityY": 0.03912474903542123, + "timestamp": 10.165007684756981 }, { - "x": 6.795022002255089, - "y": 6.7469084932218335, - "heading": 0.004385844550104781, - "angularVelocity": -0.09891582512292933, - "velocityX": 0.7732211464912397, - "velocityY": -0.014869695664985829, - "timestamp": 12.158966852013254 + "x": 7.920513375065405, + "y": 4.068976876297905, + "heading": -2.8949605954376388e-18, + "angularVelocity": 2.219916096123338e-17, + "velocityX": 1.2894960292025357, + "velocityY": 0.03440674909358656, + "timestamp": 10.23892554850311 }, { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": -4.086871260037018e-38, - "angularVelocity": -0.04945907457853377, - "velocityX": 0.3866105668308079, - "velocityY": -0.007434847721776456, - "timestamp": 12.247643087617718 + "x": 7.991997299424134, + "y": 4.071030463180751, + "heading": -1.5603341535814708e-18, + "angularVelocity": 1.8055533185319825e-17, + "velocityX": 0.9670723792051263, + "velocityY": 0.02778201071799316, + "timestamp": 10.312843412249238 }, { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": -5.480350490542979e-38, - "angularVelocity": -1.571392014077605e-37, - "velocityX": 4.425248303324229e-37, - "velocityY": 1.8999126670467387e-37, - "timestamp": 12.336319323222181 + "x": 8.03965088919926, + "y": 4.07248360094205, + "heading": -5.821232156931401e-19, + "angularVelocity": 1.3233755526918469e-17, + "velocityX": 0.6446829948819025, + "velocityY": 0.019658817066051913, + "timestamp": 10.386761275995367 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 8.014500942600024e-44, + "angularVelocity": 7.875271094040935e-18, + "velocityX": 0.322326324020543, + "velocityY": 0.0103270696803873, + "timestamp": 10.460679139741496 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": -7.373298274013602e-44, + "angularVelocity": -1.7505506389116263e-42, + "velocityX": -2.259320082617454e-45, + "velocityY": 0, + "timestamp": 10.534597003487624 } ], "constraints": [ @@ -30032,13 +20258,13 @@ }, { "scope": [ - 1 + 3 ], "type": "StopPoint" }, { "scope": [ - 5 + 2 ], "type": "StopPoint" }, @@ -30050,7 +20276,7 @@ }, { "scope": [ - 2 + 7 ], "type": "StopPoint" } @@ -30060,7 +20286,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "CenterLanePCBFSprint": { + "CenterLanePBDA": { "waypoints": [ { "x": 1.2899744510650635, @@ -30069,83 +20295,47 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 - }, - { - "x": 1.8129411935806274, - "y": 5.542999267578125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 12 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, + "x": 2.668184518814087, + "y": 5.552821636199951, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 15 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, + "x": 4.897946357727051, + "y": 6.407341003417969, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 20 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, + "x": 8.008931159973145, + "y": 7.328619956970215, + "heading": 0.4756948301053548, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 21 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 16 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 14 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, + "x": 1.8804243803024292, + "y": 6.046840667724609, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -30153,36 +20343,9 @@ "controlIntervalCount": 13 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 14 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 16 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -30193,1640 +20356,884 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 2.598230881890789e-31, - "angularVelocity": 0, - "velocityX": -2.3904076278416207e-31, - "velocityY": 5.789752915577876e-30, - "timestamp": 0 - }, - { - "x": 1.3043755674296562, - "y": 5.57443357397875, - "heading": -0.009496998569604356, - "angularVelocity": -0.1264048181794536, - "velocityX": 0.191678505815799, - "velocityY": -0.2198905081992682, - "timestamp": 0.07513161845108943 - }, - { - "x": 1.3331814181481576, - "y": 5.541395353902405, - "heading": -0.028483550113124877, - "angularVelocity": -0.2527105356630103, - "velocityX": 0.38340516698036353, - "velocityY": -0.4397378994111958, - "timestamp": 0.15026323690217885 - }, - { - "x": 1.376396862320767, - "y": 5.49184373452604, - "heading": -0.056940765199465976, - "angularVelocity": -0.378764835281948, - "velocityX": 0.5751965026490216, - "velocityY": -0.6595308393324731, - "timestamp": 0.22539485535326828 - }, - { - "x": 1.4340285202431444, - "y": 5.425784148354236, - "heading": -0.09483975957151446, - "angularVelocity": -0.504434686147033, - "velocityX": 0.7670759542900392, - "velocityY": -0.8792514727514934, - "timestamp": 0.3005264738043577 - }, - { - "x": 1.5060856912258154, - "y": 5.34322422127702, - "heading": -0.1421437039065203, - "angularVelocity": -0.6296143396295107, - "velocityX": 0.9590791795253663, - "velocityY": -1.0988706057618698, - "timestamp": 0.3756580922554471 - }, - { - "x": 1.5925821714791422, - "y": 5.244175418310136, - "heading": -0.1988101671423871, - "angularVelocity": -0.7542292367162176, - "velocityX": 1.1512660319811892, - "velocityY": -1.318337139694803, - "timestamp": 0.4507897107065365 - }, - { - "x": 1.6935404980129676, - "y": 5.128656797964587, - "heading": -0.26479264404095926, - "angularVelocity": -0.8782251502241449, - "velocityX": 1.343752851483489, - "velocityY": -1.5375500053152549, - "timestamp": 0.5259213291576259 - }, - { - "x": 1.80900475066288, - "y": 4.99670620601025, - "heading": -0.34003720346134453, - "angularVelocity": -1.001503241571098, - "velocityX": 1.5368263725127875, - "velocityY": -1.7562591446699305, - "timestamp": 0.6010529476087153 - }, - { - "x": 1.9391049044022353, - "y": 4.8484368766348975, - "heading": -0.42444837010558617, - "angularVelocity": -1.1235105590497523, - "velocityX": 1.7316298572389015, - "velocityY": -1.9734611397719364, - "timestamp": 0.6761845660598047 - }, - { - "x": 2.0768333663443252, - "y": 4.720578669009404, - "heading": -0.4949304356074761, - "angularVelocity": -0.938114564170018, - "velocityX": 1.8331624525981682, - "velocityY": -1.7017896094422644, - "timestamp": 0.7513161845108941 - }, - { - "x": 2.2004381051303543, - "y": 4.609474436663166, - "heading": -0.5559710621989035, - "angularVelocity": -0.8124492437076775, - "velocityX": 1.6451760435605478, - "velocityY": -1.4787946091584414, - "timestamp": 0.8264478029619835 - }, - { - "x": 2.309790549713842, - "y": 4.515017160876509, - "heading": -0.6076423779450478, - "angularVelocity": -0.687743946017975, - "velocityX": 1.4554783570583396, - "velocityY": -1.2572240253997977, - "timestamp": 0.9015794214130729 - }, - { - "x": 2.404847575381414, - "y": 4.437170463185578, - "heading": -0.6499678697321144, - "angularVelocity": -0.5633512582056228, - "velocityX": 1.2652066816823961, - "velocityY": -1.0361376380998464, - "timestamp": 0.9767110398641623 - }, - { - "x": 2.4855875270959125, - "y": 4.375915949453414, - "heading": -0.6829583691226987, - "angularVelocity": -0.43910273822475193, - "velocityX": 1.0746467782947702, - "velocityY": -0.8152960763101967, - "timestamp": 1.0518426583152518 - }, - { - "x": 2.5519973911227014, - "y": 4.331242517358978, - "heading": -0.7066196510488874, - "angularVelocity": -0.31493108244769824, - "velocityX": 0.8839136624193856, - "velocityY": -0.5946022861403544, - "timestamp": 1.1269742767663413 - }, - { - "x": 2.604068513564513, - "y": 4.303142764963195, - "heading": -0.7209550318751375, - "angularVelocity": -0.1908035673998086, - "velocityX": 0.693065363384342, - "velocityY": -0.3740070155974736, - "timestamp": 1.2021058952174308 - }, - { - "x": 2.6417947649723117, - "y": 4.2916114443192095, - "heading": -0.7259664734930259, - "angularVelocity": -0.06670216508038034, - "velocityX": 0.5021354815334699, - "velocityY": -0.15348159509809242, - "timestamp": 1.2772375136685203 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.057384074600866586, - "velocityX": 0.311145410428376, - "velocityY": 0.06699234541878851, - "timestamp": 1.3523691321196099 - }, - { - "x": 2.674170692930372, - "y": 4.3184165536581745, - "heading": -0.7079150841905615, - "angularVelocity": 0.18203686942568226, - "velocityX": 0.1192255306502172, - "velocityY": 0.28844784676769203, - "timestamp": 1.4278485168598245 - }, - { - "x": 2.6686789979466896, - "y": 4.3568998584598715, - "heading": -0.6847718369181163, - "angularVelocity": 0.3066167980947624, - "velocityX": -0.07275754835791173, - "velocityY": 0.5098518613031996, - "timestamp": 1.5033279016000392 - }, - { - "x": 2.648690220373955, - "y": 4.4120894363660135, - "heading": -0.6522316135776112, - "angularVelocity": 0.4311140512244905, - "velocityX": -0.26482433108237524, - "velocityY": 0.7311874373940757, - "timestamp": 1.5788072863402538 - }, - { - "x": 2.614195529876659, - "y": 4.483978019553035, - "heading": -0.6103014306892409, - "angularVelocity": 0.5555183449290733, - "velocityX": -0.4570081038377115, - "velocityY": 0.9524267246290559, - "timestamp": 1.6542866710804685 - }, - { - "x": 2.565181655781624, - "y": 4.572554576780094, - "heading": -0.5589890537241904, - "angularVelocity": 0.6798197566183578, - "velocityX": -0.6493676951067825, - "velocityY": 1.1735198628071262, - "timestamp": 1.729766055820683 - }, - { - "x": 2.5016263830570615, - "y": 4.677800410068182, - "heading": -0.49830336002198045, - "angularVelocity": 0.8040035555369023, - "velocityX": -0.8420216055727019, - "velocityY": 1.3943652779921303, - "timestamp": 1.8052454405608978 - }, - { - "x": 2.4234850482103365, - "y": 4.799677415452469, - "heading": -0.42825793280914776, - "angularVelocity": 0.9280073950309173, - "velocityX": -1.0352672470523452, - "velocityY": 1.6147058670562038, - "timestamp": 1.8807248253011124 - }, - { - "x": 2.3306237160215493, - "y": 4.938069451098362, - "heading": -0.34890039707276027, - "angularVelocity": 1.0513802677046542, - "velocityX": -1.2302873494790678, - "velocityY": 1.8335077335201588, - "timestamp": 1.956204210041327 - }, - { - "x": 2.2158437334233896, - "y": 5.072717873356149, - "heading": -0.27145428242562397, - "angularVelocity": 1.0260565175055525, - "velocityX": -1.5206798914447346, - "velocityY": 1.7839099076626925, - "timestamp": 2.031683594781542 - }, - { - "x": 2.115226856826332, - "y": 5.190380914478299, - "heading": -0.20366940333084457, - "angularVelocity": 0.8980581827763024, - "velocityX": -1.3330378478809115, - "velocityY": 1.5588765267548796, - "timestamp": 2.107162979521757 - }, - { - "x": 2.0289140508238517, - "y": 5.291175476770988, - "heading": -0.14552121115180353, - "angularVelocity": 0.770385084361868, - "velocityX": -1.1435282136547764, - "velocityY": 1.33539194366953, - "timestamp": 2.1826423642619717 - }, - { - "x": 1.956951239598818, - "y": 5.375140495871239, - "heading": -0.09702855670668947, - "angularVelocity": 0.6424622380441027, - "velocityX": -0.953410145970343, - "velocityY": 1.1124232052394887, - "timestamp": 2.2581217490021865 - }, - { - "x": 1.8993610596021686, - "y": 5.4422954705446385, - "heading": -0.05821541752658778, - "angularVelocity": 0.5142217217941352, - "velocityX": -0.7629921758539466, - "velocityY": 0.8897127991440643, - "timestamp": 2.3336011337424014 - }, - { - "x": 1.8561570206808402, - "y": 5.492652117133405, - "heading": -0.029101140919542866, - "angularVelocity": 0.3857248798192106, - "velocityX": -0.5723952184942321, - "velocityY": 0.6671576187743994, - "timestamp": 2.4090805184826163 - }, - { - "x": 1.827348191495371, - "y": 5.5262182202265375, - "heading": -0.009696189627478835, - "angularVelocity": 0.25708942063193824, - "velocityX": -0.3816781136185715, - "velocityY": 0.44470557370534397, - "timestamp": 2.484559903222831 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": -6.802256144415086e-28, - "angularVelocity": 0.12846142905206573, - "velocityX": -0.19087328234026563, - "velocityY": 0.22232623397342613, - "timestamp": 2.560039287963046 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": -6.934045530869177e-28, - "angularVelocity": -1.67846607717031e-28, - "velocityX": 1.7578194682085736e-30, - "velocityY": -2.585635481662347e-29, - "timestamp": 2.635518672703261 - }, - { - "x": 1.8467557558007974, - "y": 5.543052711491933, - "heading": 1.9216741877895313e-20, - "angularVelocity": 2.064858009548948e-19, - "velocityX": 0.36334081801614104, - "velocityY": 0.0005742601437372874, - "timestamp": 2.728584357986845 - }, - { - "x": 1.9143848790849516, - "y": 5.543159599317722, - "heading": 3.7322144176989406e-20, - "angularVelocity": 1.945443397736918e-19, - "velocityX": 0.7266816236089446, - "velocityY": 0.0011485202678394907, - "timestamp": 2.8216500432704295 - }, - { - "x": 2.0158285613519538, - "y": 5.543319931052202, - "heading": 3.5705254993467834e-20, - "angularVelocity": -1.7373634318302704e-20, - "velocityX": 1.090022406839742, - "velocityY": 0.0017227803565985438, - "timestamp": 2.9147157285540137 - }, - { - "x": 2.151086797745823, - "y": 5.543533706687699, - "heading": 2.4928931377494695e-20, - "angularVelocity": -1.157926638948406e-19, - "velocityX": 1.4533631378925307, - "velocityY": 0.0022970403628902575, - "timestamp": 3.007781413837598 - }, - { - "x": 2.3201595639866595, - "y": 5.5438009261858365, - "heading": 1.159960983349273e-20, - "angularVelocity": -1.432248793219564e-19, - "velocityX": 1.8167036080554038, - "velocityY": 0.0028712999568454734, - "timestamp": 3.1008470991211823 - }, - { - "x": 2.4554178003805283, - "y": 5.544014701821333, - "heading": 2.0604388914513833e-20, - "angularVelocity": 9.675724251840524e-20, - "velocityX": 1.4533631378925305, - "velocityY": 0.0022970403628902575, - "timestamp": 3.1939127844047666 - }, - { - "x": 2.556861482647531, - "y": 5.544175033555813, - "heading": 2.9920340295234286e-20, - "angularVelocity": 1.0010081967860847e-19, - "velocityX": 1.090022406839742, - "velocityY": 0.001722780356598544, - "timestamp": 3.286978469688351 - }, - { - "x": 2.624490605931685, - "y": 5.544281921381602, - "heading": 2.1256868508528512e-20, - "angularVelocity": -9.308986185732237e-20, - "velocityX": 0.7266816236089446, - "velocityY": 0.0011485202678394905, - "timestamp": 3.380044154971935 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 4.4426104536241024e-33, - "angularVelocity": -2.284071561257056e-19, - "velocityX": 0.36334081801614104, - "velocityY": 0.0005742601437372874, - "timestamp": 3.4731098402555194 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, "heading": 0, - "angularVelocity": -4.668434863943491e-32, - "velocityX": -5.247197089772187e-38, - "velocityY": -1.940144776771126e-37, - "timestamp": 3.5661755255391037 - }, - { - "x": 2.6759672111325252, - "y": 5.540929157802402, - "heading": -2.5855780446225044e-18, - "angularVelocity": -3.809209550134996e-17, - "velocityX": 0.2602065056082181, - "velocityY": -0.05018204010160352, - "timestamp": 3.634052548595794 - }, - { - "x": 2.7112094706297696, - "y": 5.533716903598895, - "heading": -4.117772689767123e-18, - "angularVelocity": -2.2573097288976327e-17, - "velocityX": 0.5192075007150861, - "velocityY": -0.10625472182954283, - "timestamp": 3.7019295716524847 - }, - { - "x": 2.763926012188838, - "y": 5.522238566618234, - "heading": -6.491645699921123e-18, - "angularVelocity": -3.4973145598497126e-17, - "velocityX": 0.7766478137239484, - "velocityY": -0.16910489682311705, - "timestamp": 3.7698065947091752 - }, - { - "x": 2.833977839229424, - "y": 5.505960452216563, - "heading": -8.342526928581605e-18, - "angularVelocity": -2.7268155633677806e-17, - "velocityX": 1.0320403560138376, - "velocityY": -0.23981774197838637, - "timestamp": 3.8376836177658658 - }, - { - "x": 2.921179778977005, - "y": 5.484257984516274, - "heading": -1.1098333459478279e-17, - "angularVelocity": -4.0599991083804175e-17, - "velocityX": 1.284704835607638, - "velocityY": -0.31973216742524313, - "timestamp": 3.9055606408225563 - }, - { - "x": 3.025280889014475, - "y": 5.456393698526154, - "heading": -1.451838596617181e-17, - "angularVelocity": -5.0386012124268725e-17, - "velocityX": 1.5336722995427363, - "velocityY": -0.4105113149533374, - "timestamp": 3.973437663879247 - }, - { - "x": 3.1459346095433047, - "y": 5.4214894529386894, - "heading": -1.4363311491036705e-17, - "angularVelocity": 2.284638720910668e-18, - "velocityX": 1.7775340622711318, - "velocityY": -0.5142277020356774, - "timestamp": 4.041314686935937 - }, - { - "x": 3.2826525372630417, - "y": 5.378492545637339, - "heading": -1.4597384083135608e-17, - "angularVelocity": -3.4484805248903604e-18, - "velocityX": 2.0142004107273697, - "velocityY": -0.6334530503118778, - "timestamp": 4.109191709992627 - }, - { - "x": 3.434732199390872, - "y": 5.326137913197681, - "heading": -1.5798470616654896e-17, - "angularVelocity": -1.7695038459712723e-17, - "velocityX": 2.240517560718812, - "velocityY": -0.7713159782498521, - "timestamp": 4.1770687330493175 - }, - { - "x": 3.6011452044637537, - "y": 5.262915661085215, - "heading": -1.3130445812596166e-17, - "angularVelocity": 3.9306744519879925e-17, - "velocityX": 2.4516839068486322, - "velocityY": -0.9314234665192118, - "timestamp": 4.2449457561060076 - }, - { - "x": 3.7803722058519504, - "y": 5.187069964685383, - "heading": -9.049091305666531e-18, - "angularVelocity": 6.012866096854611e-17, - "velocityX": 2.640466439409227, - "velocityY": -1.1173986863932572, - "timestamp": 4.312822779162698 - }, - { - "x": 3.9701912865835975, - "y": 5.0966861229155755, - "heading": -3.2662453848887855e-18, - "angularVelocity": 8.519592728674139e-17, - "velocityX": 2.796514522640622, - "velocityY": -1.3315822895520983, - "timestamp": 4.380699802219388 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 1.549943268403634e-18, - "angularVelocity": 7.09546240599044e-17, - "velocityX": 2.9068419610975353, - "velocityY": -1.5725251674415364, - "timestamp": 4.448576825276078 - }, - { - "x": 4.262376057274491, - "y": 4.935051597962846, - "heading": 3.5392756001694805e-18, - "angularVelocity": 6.14731881972971e-17, - "velocityX": 2.9318335021132986, - "velocityY": -1.6963703103934957, - "timestamp": 4.480937800688539 - }, - { - "x": 4.358262696439126, - "y": 4.876193489161229, - "heading": 6.0915874857662914e-18, - "angularVelocity": 7.887005422678292e-17, - "velocityX": 2.9630330341559845, - "velocityY": -1.8187989716450665, - "timestamp": 4.513298776101001 - }, - { - "x": 4.455482915426748, - "y": 4.813470451403357, - "heading": 8.792871908848605e-18, - "angularVelocity": 8.347351674874434e-17, - "velocityX": 3.0042425405441757, - "velocityY": -1.938230753505593, - "timestamp": 4.545659751513463 - }, - { - "x": 4.554624947873469, - "y": 4.7471387133987335, - "heading": 1.0200765098673088e-17, - "angularVelocity": 4.3505894735366074e-17, - "velocityX": 3.0636292998925567, - "velocityY": -2.0497447051327327, - "timestamp": 4.578020726925924 - }, - { - "x": 4.656862547860915, - "y": 4.678136115394173, - "heading": 1.0067733420891333e-17, - "angularVelocity": -4.1108673671660525e-18, - "velocityX": 3.1592867237269773, - "velocityY": -2.1322780640903733, - "timestamp": 4.610381702338386 - }, - { - "x": 4.763145340600117, - "y": 4.609727154178217, - "heading": 9.332936021050385e-18, - "angularVelocity": -2.2706280960159642e-17, - "velocityX": 3.28428891232596, - "velocityY": -2.1139338460611237, - "timestamp": 4.642742677750848 - }, - { - "x": 4.872109686799802, - "y": 4.544404504995177, - "heading": 8.161678901765097e-18, - "angularVelocity": -3.6193504811759114e-17, - "velocityX": 3.3671527143685642, - "velocityY": -2.018562430534307, - "timestamp": 4.675103653163309 - }, - { - "x": 4.982798416209946, - "y": 4.482788956670884, - "heading": 7.225847948253543e-18, - "angularVelocity": -2.891850265932224e-17, - "velocityX": 3.4204385992493553, - "velocityY": -1.9040077605499512, - "timestamp": 4.707464628575771 - }, - { - "x": 5.094726607705277, - "y": 4.425069541031477, - "heading": 5.520990037711479e-18, - "angularVelocity": -5.268252544393214e-17, - "velocityX": 3.458739734162278, - "velocityY": -1.7836117392548974, - "timestamp": 4.7398256039882325 - }, - { - "x": 5.207616568786535, - "y": 4.371323930618111, - "heading": 3.484375827946225e-18, - "angularVelocity": -6.293426523223445e-17, - "velocityX": 3.48845977732135, - "velocityY": -1.6608155263660398, - "timestamp": 4.772186579400694 - }, - { - "x": 5.321291012099858, - "y": 4.321590890995498, - "heading": 1.6420175756244674e-18, - "angularVelocity": -5.693148086098641e-17, - "velocityX": 3.5127013900065562, - "velocityY": -1.5368214025916205, - "timestamp": 4.804547554813156 - }, - { - "x": 5.4356276249643685, - "y": 4.275892411291382, - "heading": -1.9852645585834772e-19, - "angularVelocity": -5.687541886629066e-17, - "velocityX": 3.5331633675195255, - "velocityY": -1.4121477836084664, - "timestamp": 4.836908530225617 - }, - { - "x": 5.5505371629105715, - "y": 4.234242129103919, - "heading": -1.7642698806491036e-18, - "angularVelocity": -4.838369069076164e-17, - "velocityX": 3.5508675644539367, - "velocityY": -1.2870527435160222, - "timestamp": 4.869269505638079 + "angularVelocity": 2.2921355473422064e-38, + "velocityX": 1.1360383232068741e-38, + "velocityY": -2.144062321628808e-38, + "timestamp": 0 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -2.8015689175743048e-18, - "angularVelocity": -3.2054010230453e-17, - "velocityX": 3.5664736442333616, - "velocityY": -1.1616786598774413, - "timestamp": 4.901630481050541 - }, - { - "x": 5.795001659778632, - "y": 4.159833423852274, - "heading": -3.918726614896288e-18, - "angularVelocity": -3.100871682049742e-17, - "velocityX": 3.5820124359680223, - "velocityY": -1.0218844572378571, - "timestamp": 4.937657695714008 - }, - { - "x": 5.924325054788198, - "y": 4.128077802262558, - "heading": -4.913044441347591e-18, - "angularVelocity": -2.7599075747243218e-17, - "velocityX": 3.5896029215020695, - "velocityY": -0.8814342681316839, - "timestamp": 4.973684910377475 - }, - { - "x": 6.053574493908213, - "y": 4.101389053934096, - "heading": -5.793955566552838e-18, - "angularVelocity": -2.4451269226363692e-17, - "velocityX": 3.5875501430610877, - "velocityY": -0.7407941073925298, - "timestamp": 5.009712125040942 - }, - { - "x": 6.182326457133652, - "y": 4.079743240098328, - "heading": -6.4165955709959285e-18, - "angularVelocity": -1.7282490757751136e-17, - "velocityX": 3.5737418068012827, - "velocityY": -0.6008184101369575, - "timestamp": 5.04573933970441 - }, - { - "x": 6.310065721160521, - "y": 4.063062615773879, - "heading": -6.572943704568109e-18, - "angularVelocity": -4.339723040934764e-18, - "velocityX": 3.5456325230826065, - "velocityY": -0.46300066436618775, - "timestamp": 5.081766554367877 - }, - { - "x": 6.436174783136967, - "y": 4.051180022659281, - "heading": -6.55887717200026e-18, - "angularVelocity": 3.904418561563001e-19, - "velocityX": 3.500383339495921, - "velocityY": -0.3298226972469103, - "timestamp": 5.117793769031344 - }, - { - "x": 6.559940504232512, - "y": 4.043790469879042, - "heading": -6.336632318587662e-18, - "angularVelocity": 6.168804762936026e-18, - "velocityX": 3.4353397078194354, - "velocityY": -0.2051102992353164, - "timestamp": 5.1538209836948115 - }, - { - "x": 6.680596023490012, - "y": 4.040401579407175, - "heading": -5.869996108311954e-18, - "angularVelocity": 1.2952325474912863e-17, - "velocityX": 3.349010474013899, - "velocityY": -0.09406473699182225, - "timestamp": 5.189848198358279 - }, - { - "x": 6.797407433038957, - "y": 4.040314426809989, - "heading": -5.129903283273234e-18, - "angularVelocity": 2.0542604582494022e-17, - "velocityX": 3.242310310138806, - "velocityY": -0.002419076745183701, - "timestamp": 5.225875413021746 - }, - { - "x": 6.9097792291334015, - "y": 4.042670322539034, - "heading": -4.098685939880453e-18, - "angularVelocity": 2.8623288062328696e-17, - "velocityX": 3.1190808710613775, - "velocityY": 0.06539211401857271, - "timestamp": 5.261902627685213 - }, - { - "x": 7.0173204584579265, - "y": 4.046557322402495, - "heading": -2.7703450666011344e-18, - "angularVelocity": 3.687048487350159e-17, - "velocityX": 2.9849998210817597, - "velocityY": 0.10789065709827174, - "timestamp": 5.2979298423486805 - }, - { - "x": 7.119839418159034, - "y": 4.051119205104076, - "heading": -1.5285714179208822e-18, - "angularVelocity": 3.4467656195020295e-17, - "velocityX": 2.845597714359634, - "velocityY": 0.1266232414632443, - "timestamp": 5.333957057012148 + "x": 1.32825806778149, + "y": 5.589895062873393, + "heading": -2.8062774074031584e-19, + "angularVelocity": -2.995198561755265e-18, + "velocityX": 0.40860904707084805, + "velocityY": -0.011305499292899219, + "timestamp": 0.09369253321938388 + }, + { + "x": 1.4048253002214361, + "y": 5.587776581164742, + "heading": -1.768868342127426e-19, + "angularVelocity": 1.1072483896415275e-18, + "velocityX": 0.8172180835441991, + "velocityY": -0.022610998292584186, + "timestamp": 0.18738506643876776 + }, + { + "x": 1.519676146824621, + "y": 5.584598858658671, + "heading": -2.386873521248768e-19, + "angularVelocity": -6.596098514057519e-19, + "velocityX": 1.225827103364341, + "velocityY": -0.03391649683150392, + "timestamp": 0.28107759965815166 + }, + { + "x": 1.6728106047825377, + "y": 5.580361895432887, + "heading": -7.795008789281329e-19, + "angularVelocity": -5.772215866345043e-18, + "velocityX": 1.6344360932087083, + "velocityY": -0.04522199454104627, + "timestamp": 0.3747701328775355 + }, + { + "x": 1.8642286675420066, + "y": 5.575065691668706, + "heading": -1.0638718286937245e-18, + "angularVelocity": -3.0351506143991226e-18, + "velocityX": 2.0430450131096105, + "velocityY": -0.056527490315375004, + "timestamp": 0.4684626660969194 + }, + { + "x": 2.0939303023371445, + "y": 5.5687102482727004, + "heading": -1.0113200205360793e-18, + "angularVelocity": 5.608964381023318e-19, + "velocityX": 2.451653583293393, + "velocityY": -0.0678329764136413, + "timestamp": 0.5621551993163033 + }, + { + "x": 2.285348365096613, + "y": 5.563414044508519, + "heading": -7.732946400687193e-19, + "angularVelocity": 2.5404946615264457e-18, + "velocityX": 2.04304501310961, + "velocityY": -0.056527490315375004, + "timestamp": 0.6558477325356872 + }, + { + "x": 2.43848282305453, + "y": 5.559177081282735, + "heading": -4.312009437616493e-19, + "angularVelocity": 3.651237559230762e-18, + "velocityX": 1.634436093208708, + "velocityY": -0.04522199454104628, + "timestamp": 0.749540265755071 + }, + { + "x": 2.553333669657715, + "y": 5.555999358776664, + "heading": -7.015602955490743e-20, + "angularVelocity": 3.853507870950435e-18, + "velocityX": 1.2258271033643409, + "velocityY": -0.033916496831503926, + "timestamp": 0.8432327989744549 + }, + { + "x": 2.6299009020976607, + "y": 5.553880877068013, + "heading": -2.7026882274997263e-20, + "angularVelocity": 4.603264080766157e-19, + "velocityX": 0.817218083544199, + "velocityY": -0.022610998292584186, + "timestamp": 0.9369253321938388 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, + "x": 2.668184518814087, + "y": 5.552821636199951, "heading": 0, - "angularVelocity": 4.242824298811395e-17, - "velocityX": 2.704953404928098, - "velocityY": 0.12488006832508823, - "timestamp": 5.369984271675615 - }, - { - "x": 7.386413080563964, - "y": 4.061807740730578, - "heading": 2.2517150943375065e-18, - "angularVelocity": 3.243733929990829e-17, - "velocityX": 2.436302356627287, - "velocityY": 0.08916289604043352, - "timestamp": 5.439401651242998 - }, - { - "x": 7.536811709047552, - "y": 4.066158661383703, - "heading": 2.5863681690768457e-18, - "angularVelocity": 4.820883139423778e-18, - "velocityX": 2.166584642360316, - "velocityY": 0.06267768504430034, - "timestamp": 5.508819030810382 - }, - { - "x": 7.668447638906394, - "y": 4.069132723210473, - "heading": 2.8797714291084188e-18, - "angularVelocity": 4.226654216280414e-18, - "velocityX": 1.89629644160023, - "velocityY": 0.04284318776225898, - "timestamp": 5.578236410377765 - }, - { - "x": 7.781298581594194, - "y": 4.071077765012589, - "heading": 3.948563192438554e-18, - "angularVelocity": 1.5396601974766197e-17, - "velocityX": 1.6256871606375698, - "velocityY": 0.028019522117344105, - "timestamp": 5.647653789945148 - }, - { - "x": 7.875351641584324, - "y": 4.072265013256325, - "heading": 5.745276308795261e-18, - "angularVelocity": 2.5882756271616716e-17, - "velocityX": 1.3548921116913086, - "velocityY": 0.017103040350048473, - "timestamp": 5.717071169512532 - }, - { - "x": 7.950599316309057, - "y": 4.072911756307987, - "heading": 3.5817600850337305e-18, - "angularVelocity": -3.116678038331375e-17, - "velocityX": 1.0839889836476846, - "velocityY": 0.009316730992924663, - "timestamp": 5.786488549079915 - }, - { - "x": 8.007037333813425, - "y": 4.073195912702993, - "heading": 1.7956945946791005e-18, - "angularVelocity": -2.5729370677570625e-17, - "velocityX": 0.8130243154682115, - "velocityY": 0.004093447444678863, - "timestamp": 5.8559059286472985 - }, - { - "x": 8.044663419582989, - "y": 4.0732658050755175, - "heading": 5.777314545574126e-19, - "angularVelocity": -1.754550730252714e-17, - "velocityX": 0.542026881510858, - "velocityY": 0.0010068425653567271, - "timestamp": 5.925323308214682 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": -2.803956986151352e-39, - "angularVelocity": -8.322576538583876e-18, - "velocityX": 0.2710148817811471, - "velocityY": -0.0002715343629009974, - "timestamp": 5.994740687782065 + "angularVelocity": 2.884635663770645e-19, + "velocityX": 0.40860904707084805, + "velocityY": -0.011305499292899219, + "timestamp": 1.0306178654132228 }, { - "x": 8.0634765625, - "y": 4.073246955871582, + "x": 2.668184518814087, + "y": 5.552821636199951, "heading": 0, - "angularVelocity": -3.588476339344369e-38, - "velocityX": -2.7715613376406244e-35, - "velocityY": -8.161774363102711e-35, - "timestamp": 6.064158067349449 - }, - { - "x": 8.045268218494023, - "y": 4.069202722176317, - "heading": -4.783612799247601e-20, - "angularVelocity": -6.920780898714677e-19, - "velocityX": -0.2634326077352512, - "velocityY": -0.05851070411920324, - "timestamp": 6.133277620021823 - }, - { - "x": 8.008750233843994, - "y": 4.061600867158929, - "heading": -1.3711751824964603e-19, - "angularVelocity": -1.2916951396895015e-18, - "velocityX": -0.5283307434457009, - "velocityY": -0.10998125311111973, - "timestamp": 6.202397172694197 - }, - { - "x": 7.953821890093904, - "y": 4.051005973704918, - "heading": -2.6045835174744157e-19, - "angularVelocity": -1.7844564773553645e-18, - "velocityX": -0.7946860421746129, - "velocityY": -0.1532835940682337, - "timestamp": 6.271516725366571 - }, - { - "x": 7.880387860483353, - "y": 4.038078874831867, - "heading": -4.092475364172468e-19, - "angularVelocity": -2.152635237102946e-18, - "velocityX": -1.062420498561777, - "velocityY": -0.18702521028060204, - "timestamp": 6.340636278038946 - }, - { - "x": 7.788366410256325, - "y": 4.023600204310839, - "heading": -5.733506163788985e-19, - "angularVelocity": -2.3741918689373813e-18, - "velocityX": -1.3313374677525447, - "velocityY": -0.20947286203740642, - "timestamp": 6.40975583071132 - }, - { - "x": 7.677703229907308, - "y": 4.008500675956516, - "heading": -1.077796513774058e-18, - "angularVelocity": -7.298164960190344e-18, - "velocityX": -1.6010401698280374, - "velocityY": -0.21845523835917138, - "timestamp": 6.478875383383694 - }, - { - "x": 7.548394643155177, - "y": 3.993899515872939, - "heading": -1.3825329983857112e-18, - "angularVelocity": -4.4088318398420175e-18, - "velocityX": -1.8707960591853112, - "velocityY": -0.2112450025940816, - "timestamp": 6.547994936056068 - }, - { - "x": 7.40052628679877, - "y": 3.9811515921807197, - "heading": -2.4834039360007183e-18, - "angularVelocity": -1.5927055298250076e-17, - "velocityX": -2.1393129822078065, - "velocityY": -0.18443295998520684, - "timestamp": 6.6171144887284425 - }, - { - "x": 7.23433662009932, - "y": 3.9719006814395397, - "heading": -2.9994646007909418e-18, - "angularVelocity": -7.466203770052342e-18, - "velocityX": -2.4043799514615647, - "velocityY": -0.13383927388866498, - "timestamp": 6.686234041400817 - }, - { - "x": 7.050317591694833, - "y": 3.968128214859164, - "heading": -3.4395184001583614e-18, - "angularVelocity": -6.366560290082446e-18, - "velocityX": -2.662329562182405, - "velocityY": -0.05457886277500885, - "timestamp": 6.755353594073191 - }, - { - "x": 6.849362271663259, - "y": 3.972169933507745, - "heading": -3.772315951879389e-18, - "angularVelocity": -4.8148105541753904e-18, - "velocityX": -2.907358515239499, - "velocityY": 0.05847431721293695, - "timestamp": 6.824473146745565 - }, - { - "x": 6.632947899656103, - "y": 3.9866479902960563, - "heading": -6.760060231429226e-18, - "angularVelocity": -4.322574675534167e-17, - "velocityX": -3.131015228541142, - "velocityY": 0.20946398274504574, - "timestamp": 6.8935926994179395 - }, - { - "x": 6.4032866668448785, - "y": 4.014256930642455, - "heading": -9.565460422027674e-18, - "angularVelocity": -4.058764969139539e-17, - "velocityX": -3.3226666541060275, - "velocityY": 0.39943748590597694, - "timestamp": 6.962712252090314 - }, - { - "x": 6.1633110990497455, - "y": 4.057406588823124, - "heading": -1.2152070169754713e-17, - "angularVelocity": -3.7422258214923894e-17, - "velocityX": -3.4718912162613744, - "velocityY": 0.6242757152263072, - "timestamp": 7.031831804762688 - }, - { - "x": 5.916400554770506, - "y": 4.117871140121953, - "heading": -1.4492027796446373e-17, - "angularVelocity": -3.385377272110881e-17, - "velocityX": -3.5722242800035118, - "velocityY": 0.8747821558601414, - "timestamp": 7.100951357435062 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -1.65695066909008e-17, - "angularVelocity": -3.005631278245188e-17, - "velocityX": -3.6234150289835947, - "velocityY": 1.1397344367359599, - "timestamp": 7.1700709101074365 - }, - { - "x": 5.553208867110479, - "y": 4.235853588954207, - "heading": -1.751752246117206e-17, - "angularVelocity": -3.049424202704779e-17, - "velocityX": -3.6265305068647917, - "velocityY": 1.2610675773235673, - "timestamp": 7.201159263423893 - }, - { - "x": 5.440423618005098, - "y": 4.27883115567922, - "heading": -1.8382942207466584e-17, - "angularVelocity": -2.7837426365803457e-17, - "velocityX": -3.6278939562128056, - "velocityY": 1.382433038106979, - "timestamp": 7.2322476167403495 - }, - { - "x": 5.327666323513458, - "y": 4.3255819087456695, - "heading": -1.9161761421911296e-17, - "angularVelocity": -2.5051800155064728e-17, - "velocityX": -3.626994757292344, - "velocityY": 1.5038028097069258, - "timestamp": 7.263335970056806 - }, - { - "x": 5.215031204096495, - "y": 4.376103972546304, - "heading": -2.1196555001876326e-17, - "angularVelocity": -6.545195749690707e-17, - "velocityX": -3.6230648265741094, - "velocityY": 1.6251122497984019, - "timestamp": 7.294424323373263 - }, - { - "x": 5.102650689013506, - "y": 4.430390725009075, - "heading": -2.311672068470027e-17, - "angularVelocity": -6.176479221122079e-17, - "velocityX": -3.6148751250680937, - "velocityY": 1.746208681758474, - "timestamp": 7.325512676689719 - }, - { - "x": 4.990723576065449, - "y": 4.488423424572919, - "heading": -2.4422095497687565e-17, - "angularVelocity": -4.198919124707493e-17, - "velocityX": -3.6002908165872247, - "velocityY": 1.8667022654147425, - "timestamp": 7.356601030006176 - }, - { - "x": 4.87957749498313, - "y": 4.550147690824094, - "heading": -2.55240391966328e-17, - "angularVelocity": -3.544554733112049e-17, - "velocityX": -3.5751678434342686, - "velocityY": 1.9854466276443046, - "timestamp": 7.387689383322632 - }, - { - "x": 4.769828431698842, - "y": 4.615377093855064, - "heading": -2.67892153006201e-17, - "angularVelocity": -4.069614402184995e-17, - "velocityX": -3.5302308284753074, - "velocityY": 2.0981942133435845, - "timestamp": 7.418777736639089 - }, - { - "x": 4.662763212803556, - "y": 4.683258770801351, - "heading": -2.756635025118312e-17, - "angularVelocity": -2.4997623471627382e-17, - "velocityX": -3.443901251553593, - "velocityY": 2.1835082821949876, - "timestamp": 7.449866089955545 - }, - { - "x": 4.559444563014645, - "y": 4.750692382461376, - "heading": -2.7843158304647625e-17, - "angularVelocity": -8.903914937396602e-18, - "velocityX": -3.32338766023415, - "velocityY": 2.1690956408530244, - "timestamp": 7.480954443272002 - }, - { - "x": 4.458844422855551, - "y": 4.815509255321042, - "heading": -2.805863064985095e-17, - "angularVelocity": -6.930966815860972e-18, - "velocityX": -3.2359430277653423, - "velocityY": 2.084924608256891, - "timestamp": 7.512042796588458 - }, - { - "x": 4.36022139727515, - "y": 4.877112301745061, - "heading": -2.8000369092306777e-17, - "angularVelocity": 1.8740637997727053e-18, - "velocityX": -3.1723463953362434, - "velocityY": 1.9815474237874686, - "timestamp": 7.543131149904915 - }, - { - "x": 4.263183288640869, - "y": 4.935291063699046, - "heading": -2.7736137264454843e-17, - "angularVelocity": 8.499383198535807e-18, - "velocityX": -3.1213653436868745, - "velocityY": 1.871400564763493, - "timestamp": 7.5742195032213715 + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -1.1410946614987667e-37, + "timestamp": 1.1243103986326066 + }, + { + "x": 2.687410064097537, + "y": 5.560258515634833, + "heading": 0.0028176822125938034, + "angularVelocity": 0.04098002734374148, + "velocityX": 0.2796139919160097, + "velocityY": 0.1081610698436357, + "timestamp": 1.1930678494014684 + }, + { + "x": 2.7258611531960137, + "y": 5.575132263104083, + "heading": 0.008453116109374614, + "angularVelocity": 0.08196106507388369, + "velocityX": 0.5592279624754555, + "velocityY": 0.2163219738796251, + "timestamp": 1.2618253001703301 + }, + { + "x": 2.783537789303066, + "y": 5.597442863001631, + "heading": 0.016905769734324238, + "angularVelocity": 0.12293436610040698, + "velocityX": 0.8388419794814749, + "velocityY": 0.3244826509427688, + "timestamp": 1.3305827509391919 + }, + { + "x": 2.8604399802132514, + "y": 5.627190295278175, + "heading": 0.028174540960364546, + "angularVelocity": 0.1638916379247738, + "velocityX": 1.118456109850604, + "velocityY": 0.4326430364113507, + "timestamp": 1.3993402017080536 + }, + { + "x": 2.9565677382168056, + "y": 5.664374535134068, + "heading": 0.042257801458542396, + "angularVelocity": 0.20482522753092394, + "velocityX": 1.3980704189674162, + "velocityY": 0.5408030611968617, + "timestamp": 1.4680976524769154 + }, + { + "x": 3.0719210799387406, + "y": 5.708995552649304, + "heading": 0.05915345386800317, + "angularVelocity": 0.2457283133759373, + "velocityX": 1.6776849698763825, + "velocityY": 0.648962650829455, + "timestamp": 1.5368551032457771 + }, + { + "x": 3.2065000261043313, + "y": 5.761053312357379, + "heading": 0.07885900262196403, + "angularVelocity": 0.2865951040012339, + "velocityX": 1.9572998222112317, + "velocityY": 0.757121724641519, + "timestamp": 1.6056125540146389 + }, + { + "x": 3.360304601198153, + "y": 5.820547772766793, + "heading": 0.1013716386695389, + "angularVelocity": 0.32742104013214957, + "velocityX": 2.2369150306467707, + "velocityY": 0.8652801950062525, + "timestamp": 1.6743700047835006 + }, + { + "x": 3.5333348329448953, + "y": 5.8874788858214675, + "heading": 0.12668833767209217, + "angularVelocity": 0.36820299064984424, + "velocityX": 2.5165306423068015, + "velocityY": 0.973437966449241, + "timestamp": 1.7431274555523624 + }, + { + "x": 3.7255907514118967, + "y": 5.961846596243587, + "heading": 0.15480596913918515, + "angularVelocity": 0.4089394116954055, + "velocityX": 2.796146691262561, + "velocityY": 1.0815949339384094, + "timestamp": 1.8118849063212241 + }, + { + "x": 3.937072386955544, + "y": 6.04365084047756, + "heading": 0.18572140510730675, + "angularVelocity": 0.44963034002016317, + "velocityX": 3.075763181717921, + "velocityY": 1.1897509770827353, + "timestamp": 1.8806423570900859 + }, + { + "x": 4.167779761570079, + "y": 6.132891543122838, + "heading": 0.21943155266112502, + "angularVelocity": 0.4902762853605699, + "velocityX": 3.355379992054849, + "velocityY": 1.2979059236107295, + "timestamp": 1.9493998078589476 + }, + { + "x": 4.410308649457132, + "y": 6.2266961328710915, + "heading": 0.21943155790785696, + "angularVelocity": 7.630783127279499e-8, + "velocityX": 3.5273106430654275, + "velocityY": 1.3642825424635276, + "timestamp": 2.0181572586278094 + }, + { + "x": 4.652837676050608, + "y": 6.320500363997887, + "heading": 0.21943156315423873, + "angularVelocity": 7.630273806741925e-8, + "velocityX": 3.5273126603947684, + "velocityY": 1.3642773267166644, + "timestamp": 2.0869147093966713 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -2.7306183592260526e-17, - "angularVelocity": 1.3830056156762042e-17, - "velocityX": -3.077815742368318, - "velocityY": 1.758109592119851, - "timestamp": 7.605307856537828 - }, - { - "x": 3.9493379060680507, - "y": 5.0986210510281875, - "heading": -2.5526201955845404e-17, - "angularVelocity": 2.4369765067325514e-17, - "velocityX": -2.986848903907283, - "velocityY": 1.487847763803033, - "timestamp": 7.678348430852026 - }, - { - "x": 3.742771574255505, - "y": 5.189991789335339, - "heading": -2.3046567381049293e-17, - "angularVelocity": 3.3948727787048854e-17, - "velocityX": -2.828103882698977, - "velocityY": 1.2509586509287764, - "timestamp": 7.7513890051662235 - }, - { - "x": 3.5509457192311733, - "y": 5.266647381491633, - "heading": -1.9951230504969442e-17, - "angularVelocity": 4.2378320613588127e-17, - "velocityX": -2.6262917128657457, - "velocityY": 1.0494932833707686, - "timestamp": 7.824429579480421 - }, - { - "x": 3.3757737671252985, - "y": 5.330794386309625, - "heading": -1.631318519146512e-17, - "angularVelocity": 4.980855295391938e-17, - "velocityX": -2.3982827866651224, - "velocityY": 0.8782379577418434, - "timestamp": 7.897470153794619 - }, - { - "x": 3.218440544499334, - "y": 5.384189785860755, - "heading": -1.2190567343791479e-17, - "angularVelocity": 5.64428454512083e-17, - "velocityX": -2.1540523757270402, - "velocityY": 0.7310375096646903, - "timestamp": 7.970510728108817 - }, - { - "x": 3.0797086044379114, - "y": 5.42821450731098, - "heading": -8.80046228629925e-18, - "angularVelocity": 4.6413997826950126e-17, - "velocityX": -1.8993818348777063, - "velocityY": 0.6027433637206029, - "timestamp": 8.043551302423014 - }, - { - "x": 2.960089565045244, - "y": 5.463961887551477, - "heading": -5.007321106893989e-18, - "angularVelocity": 5.193197363283625e-17, - "velocityX": -1.6377067200773132, - "velocityY": 0.4894181155630415, - "timestamp": 8.11659187673721 - }, - { - "x": 2.8599401252574177, - "y": 5.4923098040640115, - "heading": -3.3892541193893154e-18, - "angularVelocity": 2.2152988290333294e-17, - "velocityX": -1.3711480328319254, - "velocityY": 0.38811190600159495, - "timestamp": 8.189632451051407 - }, - { - "x": 2.779517497378913, - "y": 5.513974137504595, - "heading": -1.420472445548671e-18, - "angularVelocity": 2.695463024933353e-17, - "velocityX": -1.1010678466539927, - "velocityY": 0.2966068331745371, - "timestamp": 8.262673025365604 - }, - { - "x": 2.7190126242063695, - "y": 5.529547456775984, - "heading": 8.795935357239642e-19, - "angularVelocity": 3.149025049250684e-17, - "velocityX": -0.828373458733645, - "velocityY": 0.21321463334061347, - "timestamp": 8.335713599679801 - }, - { - "x": 2.6785708390344105, - "y": 5.539527036788551, - "heading": 2.892310957635007e-19, - "angularVelocity": -8.082664265777e-18, - "velocityX": -0.5536893096978018, - "velocityY": 0.1366306344969068, - "timestamp": 8.408754173993998 + "x": 4.897946357727051, + "y": 6.407341003417969, + "heading": 0.21943156841217684, + "angularVelocity": 7.647081243446701e-8, + "velocityX": 3.5648308501199604, + "velocityY": 1.2629996960185401, + "timestamp": 2.1556721601655333 + }, + { + "x": 5.1308682473529785, + "y": 6.471009903235158, + "heading": 0.21943157303192085, + "angularVelocity": 7.23563158909772e-8, + "velocityX": 3.6481177055177683, + "velocityY": 0.9972082962532426, + "timestamp": 2.2195193022886155 + }, + { + "x": 5.365529567427774, + "y": 6.527934417793533, + "heading": 0.21943157763716262, + "angularVelocity": 7.21291764013049e-8, + "velocityX": 3.6753613751798433, + "velocityY": 0.8915749815181846, + "timestamp": 2.2833664444116977 + }, + { + "x": 5.600190991292763, + "y": 6.584858504493345, + "heading": 0.21943158224240422, + "angularVelocity": 7.21291734995501e-8, + "velocityX": 3.6753630007842206, + "velocityY": 0.8915682802227483, + "timestamp": 2.34721358653478 + }, + { + "x": 5.8348524151632635, + "y": 6.641782591170481, + "heading": 0.2194315868476458, + "angularVelocity": 7.212917336838375e-8, + "velocityX": 3.6753630008705414, + "velocityY": 0.8915682798675781, + "timestamp": 2.411060728657862 + }, + { + "x": 6.069513839029734, + "y": 6.698706677864191, + "heading": 0.21943159145288735, + "angularVelocity": 7.212917280605725e-8, + "velocityX": 3.6753630008074327, + "velocityY": 0.891568280127171, + "timestamp": 2.474907870780944 + }, + { + "x": 6.3041751870414915, + "y": 6.75563107725722, + "heading": 0.21943159605812906, + "angularVelocity": 7.212917561549481e-8, + "velocityX": 3.6753618127399528, + "velocityY": 0.8915731777514998, + "timestamp": 2.5387550129040264 + }, + { + "x": 6.537489271202827, + "y": 6.817847413489264, + "heading": 0.21943160514124108, + "angularVelocity": 1.4226340838423697e-7, + "velocityX": 3.6542604164106804, + "velocityY": 0.9744576524998471, + "timestamp": 2.6026021550271086 + }, + { + "x": 6.7545327794045615, + "y": 6.881509207420976, + "heading": 0.24890077922126197, + "angularVelocity": 0.46155823268064305, + "velocityX": 3.399424014677561, + "velocityY": 0.9970970009742466, + "timestamp": 2.6664492971501907 + }, + { + "x": 6.954212307545749, + "y": 6.941327014961267, + "heading": 0.2778274290995393, + "angularVelocity": 0.45306099719431336, + "velocityX": 3.1274622716276532, + "velocityY": 0.9368909171372027, + "timestamp": 2.730296439273273 + }, + { + "x": 7.1365689728963595, + "y": 6.997132012930522, + "heading": 0.305376589845945, + "angularVelocity": 0.43148620017020184, + "velocityX": 2.856144524042602, + "velocityY": 0.8740406557535503, + "timestamp": 2.794143581396355 + }, + { + "x": 7.301617943292834, + "y": 7.048866027505986, + "heading": 0.33125825392103403, + "angularVelocity": 0.40536918669284566, + "velocityX": 2.585064341302851, + "velocityY": 0.8102792522134878, + "timestamp": 2.8579907235194373 + }, + { + "x": 7.449367094221676, + "y": 7.096499619737108, + "heading": 0.35532461486989275, + "angularVelocity": 0.37693716818945566, + "velocityX": 2.3141075076472033, + "velocityY": 0.7460567638139751, + "timestamp": 2.9218378656425195 + }, + { + "x": 7.579821246213137, + "y": 7.140015020198173, + "heading": 0.3774857294479878, + "angularVelocity": 0.347096421878647, + "velocityX": 2.04322617510391, + "velocityY": 0.681555963416076, + "timestamp": 2.9856850077656016 + }, + { + "x": 7.69298365301254, + "y": 7.179400342359022, + "heading": 0.3976808814927073, + "angularVelocity": 0.3163047142468991, + "velocityX": 1.7723958040479335, + "velocityY": 0.6168689913312067, + "timestamp": 3.049532149888684 + }, + { + "x": 7.788856658439436, + "y": 7.214647078189013, + "heading": 0.41586620005922853, + "angularVelocity": 0.2848258819708253, + "velocityX": 1.5016021428504343, + "velocityY": 0.5520487629977263, + "timestamp": 3.113379292011766 + }, + { + "x": 7.867442031134741, + "y": 7.2457488378850705, + "heading": 0.43200844627235874, + "angularVelocity": 0.2528264488645923, + "velocityX": 1.2308361828288252, + "velocityY": 0.48712845496042256, + "timestamp": 3.177226434134848 + }, + { + "x": 7.928741153018892, + "y": 7.27270064652237, + "heading": 0.4460815614823235, + "angularVelocity": 0.22041887454940703, + "velocityX": 0.9600918670092122, + "velocityY": 0.4221302276198955, + "timestamp": 3.2410735762579304 + }, + { + "x": 7.9727551334716695, + "y": 7.29549852060884, + "heading": 0.45806460123681797, + "angularVelocity": 0.1876832596734667, + "velocityX": 0.6893649267484526, + "velocityY": 0.35706960920065395, + "timestamp": 3.3049207183810125 + }, + { + "x": 7.999484882530383, + "y": 7.314139197912392, + "heading": 0.4679404256802237, + "angularVelocity": 0.1546791933829663, + "velocityX": 0.4186522398635212, + "velocityY": 0.2919578963709263, + "timestamp": 3.3687678605040947 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -8.28795440431569e-39, - "angularVelocity": -3.959868860281201e-18, - "velocityX": -0.2774577154251034, - "velocityY": 0.06583092414052888, - "timestamp": 8.481794748308195 + "x": 8.008931159973145, + "y": 7.328619956970215, + "heading": 0.4756948301053548, + "angularVelocity": 0.12145264717068183, + "velocityX": 0.1479514529335022, + "velocityY": 0.22680355887981873, + "timestamp": 3.432615002627177 + }, + { + "x": 7.9838028545378235, + "y": 7.340994975747599, + "heading": 0.4825391441548679, + "angularVelocity": 0.07100818067139564, + "velocityX": -0.26070037689812714, + "velocityY": 0.12838796741348574, + "timestamp": 3.5290026861982695 + }, + { + "x": 7.919285560188609, + "y": 7.343883953710174, + "heading": 0.4845188810802152, + "angularVelocity": 0.02053931427751041, + "velocityX": -0.6693520578449088, + "velocityY": 0.029972480461632738, + "timestamp": 3.625390369769362 + }, + { + "x": 7.815379289936097, + "y": 7.337286883355744, + "heading": 0.4816330205236378, + "angularVelocity": -0.029940138092932247, + "velocityX": -1.0780036038097405, + "velocityY": -0.06844308432376209, + "timestamp": 3.7217780533404548 + }, + { + "x": 7.6720840556149295, + "y": 7.321203739342901, + "heading": 0.47388185121605153, + "angularVelocity": -0.08041659494667612, + "velocityX": -1.4866550269929244, + "velocityY": -0.16685891202042144, + "timestamp": 3.8181657369115474 + }, + { + "x": 7.489399868443907, + "y": 7.295634478019173, + "heading": 0.46126687890088164, + "angularVelocity": -0.13087743005945188, + "velocityX": -1.895306333783606, + "velocityY": -0.2652751926014197, + "timestamp": 3.91455342048264 + }, + { + "x": 7.267326740077932, + "y": 7.2605790366684495, + "heading": 0.44379066346613116, + "angularVelocity": -0.1813117069242423, + "velocityX": -2.3039575196573705, + "velocityY": -0.36369212384761535, + "timestamp": 4.010941104053733 + }, + { + "x": 7.00586468444221, + "y": 7.216037332217007, + "heading": 0.4214565844390286, + "angularVelocity": -0.23171092197300747, + "velocityX": -2.7126085610603474, + "velocityY": -0.4621099169645314, + "timestamp": 4.107328787624826 + }, + { + "x": 6.7050137215236445, + "y": 7.162009258221821, + "heading": 0.3942685380548319, + "angularVelocity": -0.2820697144790415, + "velocityX": -3.1212593951037992, + "velocityY": -0.5605288144032944, + "timestamp": 4.203716471195919 + }, + { + "x": 6.364773891396992, + "y": 7.098494671595923, + "heading": 0.3622306012435557, + "angularVelocity": -0.3323862097758952, + "velocityX": -3.529909813380894, + "velocityY": -0.658949196336373, + "timestamp": 4.300104154767012 + }, + { + "x": 6.010374312951181, + "y": 7.013136980735367, + "heading": 0.36223059672201996, + "angularVelocity": -4.690989126354215e-8, + "velocityX": -3.6768139384158545, + "velocityY": -0.8855663680059631, + "timestamp": 4.396491838338105 + }, + { + "x": 5.655974859532265, + "y": 6.927778770772817, + "heading": 0.36223059220056536, + "angularVelocity": -4.690904952415849e-8, + "velocityX": -3.676812641290635, + "velocityY": -0.885571753569461, + "timestamp": 4.492879521909198 + }, + { + "x": 5.301574928644714, + "y": 6.842422689744232, + "heading": 0.3622305506125328, + "angularVelocity": -4.314662515297724e-7, + "velocityX": -3.6768175949176816, + "velocityY": -0.8855496663702899, + "timestamp": 4.589267205480291 + }, + { + "x": 4.986369114322999, + "y": 6.766504438890574, + "heading": 0.34532942408176365, + "angularVelocity": -0.1753452921016411, + "velocityX": -3.270187669664538, + "velocityY": -0.7876343536948325, + "timestamp": 4.685654889051384 + }, + { + "x": 4.710566832932512, + "y": 6.70007668327783, + "heading": 0.3264669766789321, + "angularVelocity": -0.1956935440711402, + "velocityX": -2.861385097890258, + "velocityY": -0.6891726530988762, + "timestamp": 4.782042572622477 + }, + { + "x": 4.474170501345758, + "y": 6.643140019477641, + "heading": 0.30840376738356945, + "angularVelocity": -0.18740163292794487, + "velocityX": -2.4525574516208333, + "velocityY": -0.5907047632096585, + "timestamp": 4.87843025619357 + }, + { + "x": 4.277177710352041, + "y": 6.5956938222588235, + "heading": 0.2923060351860776, + "angularVelocity": -0.16701026107365813, + "velocityX": -2.0437548003570405, + "velocityY": -0.4922433599498598, + "timestamp": 4.974817939764663 + }, + { + "x": 4.1195862527073634, + "y": 6.557737539015946, + "heading": 0.27881764120910085, + "angularVelocity": -0.1399389784798502, + "velocityX": -1.6349750487409798, + "velocityY": -0.3937876898440308, + "timestamp": 5.071205623335756 + }, + { + "x": 4.0013944038572795, + "y": 6.529270748726535, + "heading": 0.26834692670698207, + "angularVelocity": -0.10863124949357753, + "velocityX": -1.226213188979785, + "velocityY": -0.2953363877493204, + "timestamp": 5.167593306906849 + }, + { + "x": 3.9226008250519366, + "y": 6.510293128915594, + "heading": 0.26117603215990287, + "angularVelocity": -0.07439637805789222, + "velocityX": -0.817465218439728, + "velocityY": -0.19688843125838204, + "timestamp": 5.263980990477942 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 1.1194999943590282e-38, - "angularVelocity": 2.6674152729925026e-37, - "velocityX": 1.474313292548536e-35, - "velocityY": 1.9984936751410376e-34, - "timestamp": 8.554835322622392 - }, - { - "x": 2.6757817592371826, - "y": 5.540525112473874, - "heading": -2.263521696805234e-19, - "angularVelocity": -3.3440822564023192e-18, - "velocityX": 0.258195705536965, - "velocityY": -0.056291922762700355, - "timestamp": 8.622522703591729 - }, - { - "x": 2.710655164921984, - "y": 5.532555521074409, - "heading": -6.817251938952584e-19, - "angularVelocity": -6.727591135839588e-18, - "velocityX": 0.5152128090256508, - "velocityY": -0.11774116955531802, - "timestamp": 8.690210084561066 - }, - { - "x": 2.762825274578906, - "y": 5.520028158351496, - "heading": -1.3690954482745913e-18, - "angularVelocity": -1.01550724010861e-17, - "velocityX": 0.7707508978749663, - "velocityY": -0.18507678305040806, - "timestamp": 8.757897465530403 - }, - { - "x": 2.832164805136809, - "y": 5.502484798756891, - "heading": -2.291792592298148e-18, - "angularVelocity": -1.3631745397966465e-17, - "velocityX": 1.024408531766262, - "velocityY": -0.25918213030803783, - "timestamp": 8.82558484649974 - }, - { - "x": 2.9185095362845113, - "y": 5.479394153237709, - "heading": -3.2554537025043674e-18, - "angularVelocity": -1.4236938945256572e-17, - "velocityX": 1.2756400072092755, - "velocityY": -0.34113663711766645, - "timestamp": 8.893272227469078 - }, - { - "x": 3.021644093219496, - "y": 5.4501349612076035, - "heading": -4.462356984669075e-18, - "angularVelocity": -1.7830550759992628e-17, - "velocityX": 1.5236895778506316, - "velocityY": -0.4322695251476627, - "timestamp": 8.960959608438415 - }, - { - "x": 3.1412808309812497, - "y": 5.413974520765914, - "heading": -5.917171487112631e-18, - "angularVelocity": -2.149314217260227e-17, - "velocityX": 1.767489538647525, - "velocityY": -0.5342272063690919, - "timestamp": 9.028646989407752 - }, - { - "x": 3.2770278679497706, - "y": 5.3700418577232005, - "heading": -2.5772013184954745e-18, - "angularVelocity": 4.9344059717056357e-17, - "velocityX": 2.0054999177766235, - "velocityY": -0.6490524882712698, - "timestamp": 9.09633437037709 - }, - { - "x": 3.428339922513116, - "y": 5.317295603371641, - "heading": 5.041855021881816e-19, - "angularVelocity": 4.552380039793498e-17, - "velocityX": 2.2354544140493458, - "velocityY": -0.7792627458204253, - "timestamp": 9.164021751346427 - }, - { - "x": 3.594442154384912, - "y": 5.254489538104451, - "heading": 3.3214379722263567e-18, - "angularVelocity": 4.1621531660920524e-17, - "velocityX": 2.453961572941347, - "velocityY": -0.9278844057453193, - "timestamp": 9.231709132315764 - }, - { - "x": 3.774213631656669, - "y": 5.18014653899489, - "heading": 5.8698404540777154e-18, - "angularVelocity": 3.764959502608201e-17, - "velocityX": 2.6559083051712964, - "velocityY": -1.0983287880977073, - "timestamp": 9.299396513285101 - }, - { - "x": 3.966018710550382, - "y": 5.092569435745629, - "heading": 8.147436412478004e-18, - "angularVelocity": 3.364875292561275e-17, - "velocityX": 2.8336903592207485, - "velocityY": -1.2938468292772856, - "timestamp": 9.367083894254439 + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": -0.038017450198398375, + "velocityX": -0.40872820518438957, + "velocityY": -0.09844312341738799, + "timestamp": 5.3603686740490355 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 1.0158795753511172e-17, - "angularVelocity": 2.971542571519468e-17, - "velocityX": 2.9766309755737, - "velocityY": -1.5161118425338018, - "timestamp": 9.434771275223776 - }, - { - "x": 4.261627503076192, - "y": 4.9391622522038245, - "heading": 1.103199717945149e-17, - "angularVelocity": 2.8019595592305015e-17, - "velocityX": 3.020426535396691, - "velocityY": -1.629624452324509, - "timestamp": 9.465935230570453 - }, - { - "x": 4.357244155674493, - "y": 4.884889313220403, - "heading": 1.184832535139553e-17, - "angularVelocity": 2.619462654200126e-17, - "velocityX": 3.068180901128632, - "velocityY": -1.7415292243770963, - "timestamp": 9.497099185917131 - }, - { - "x": 4.454522517564495, - "y": 4.827208233471932, - "heading": 1.2602189603982203e-17, - "angularVelocity": 2.4190262248102175e-17, - "velocityX": 3.1215024154618987, - "velocityY": -1.8508908483152517, - "timestamp": 9.528263141263809 - }, - { - "x": 4.553721568715114, - "y": 4.766257957931852, - "heading": 1.3285416566033512e-17, - "angularVelocity": 2.1923627934667792e-17, - "velocityX": 3.18313417045742, - "velocityY": -1.9557939568983074, - "timestamp": 9.559427096610486 - }, - { - "x": 4.6552566098247095, - "y": 4.702321075934725, - "heading": 1.3885398069122134e-17, - "angularVelocity": 1.9252418261752656e-17, - "velocityX": 3.258092240862523, - "velocityY": -2.0516292391602087, - "timestamp": 9.590591051957164 - }, - { - "x": 4.7598200255019725, - "y": 4.636102620452092, - "heading": 1.4382811423998524e-17, - "angularVelocity": 1.5961175314853973e-17, - "velocityX": 3.3552677930020853, - "velocityY": -2.124841174555608, - "timestamp": 9.621755007303841 - }, - { - "x": 4.868161384056622, - "y": 4.569562096793983, - "heading": 1.476085010345509e-17, - "angularVelocity": 1.2130638591480905e-17, - "velocityX": 3.476495757660601, - "velocityY": -2.135175811859933, - "timestamp": 9.652918962650519 - }, - { - "x": 4.97947280914236, - "y": 4.50537854159068, - "heading": 1.3849367053817892e-17, - "angularVelocity": -2.9247989846367464e-17, - "velocityX": 3.571800300939838, - "velocityY": -2.0595445760752855, - "timestamp": 9.684082917997197 - }, - { - "x": 5.09247899341136, - "y": 4.444586805435104, - "heading": 1.2651254648975303e-17, - "angularVelocity": -3.84454537766135e-17, - "velocityX": 3.6261823318601505, - "velocityY": -1.9507066891641414, - "timestamp": 9.715246873343874 - }, - { - "x": 5.206458430184718, - "y": 4.387459691402226, - "heading": 1.1425663620436537e-17, - "angularVelocity": -3.932719755362708e-17, - "velocityX": 3.657412401776808, - "velocityY": -1.8331150008841195, - "timestamp": 9.746410828690552 - }, - { - "x": 5.321007172735707, - "y": 4.334081255049934, - "heading": 1.0655255032861655e-17, - "angularVelocity": -2.472114270913484e-17, - "velocityX": 3.675680486533668, - "velocityY": -1.712826108191114, - "timestamp": 9.77757478403723 - }, - { - "x": 5.435875810085393, - "y": 4.284480962664817, - "heading": 9.880141449219796e-18, - "angularVelocity": -2.4872118283779253e-17, - "velocityX": 3.68594538375666, - "velocityY": -1.5915916908925372, - "timestamp": 9.808738739383907 - }, - { - "x": 5.550897094649704, - "y": 4.2386692598409, - "heading": 9.106151253635088e-18, - "angularVelocity": -2.483607061124937e-17, - "velocityX": 3.6908435814638634, - "velocityY": -1.470022091685541, - "timestamp": 9.839902694730585 + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -1.0638409913121086e-32, + "timestamp": 5.456756357620129 + }, + { + "x": 3.856532695000848, + "y": 6.488812456981272, + "heading": 0.25799867286334044, + "angularVelocity": 0.005949010116156059, + "velocityX": -0.32577575495196986, + "velocityY": -0.14647295299154703, + "timestamp": 5.53862790495835 + }, + { + "x": 3.8031475649180457, + "y": 6.464921556304023, + "heading": 0.25900179360543873, + "angularVelocity": 0.01225237307357005, + "velocityX": -0.6520596204474033, + "velocityY": -0.29180956576467965, + "timestamp": 5.620499452296571 + }, + { + "x": 3.7229939182765985, + "y": 6.429256431279799, + "heading": 0.2605597691266567, + "angularVelocity": 0.019029511128961094, + "velocityX": -0.9790171219107733, + "velocityY": -0.43562295063126366, + "timestamp": 5.702370999634793 + }, + { + "x": 3.6159951636151835, + "y": 6.38199294529669, + "heading": 0.26272711898900253, + "angularVelocity": 0.02647256504622092, + "velocityX": -1.3069101310543174, + "velocityY": -0.5772882951369854, + "timestamp": 5.784242546973014 + }, + { + "x": 3.4820378233980955, + "y": 6.3233976602765205, + "heading": 0.2655861022623468, + "angularVelocity": 0.03492035226271501, + "velocityX": -1.6361891838161302, + "velocityY": -0.7156977842143984, + "timestamp": 5.866114094311236 + }, + { + "x": 3.3209367433633594, + "y": 6.253922161955361, + "heading": 0.26927508652336807, + "angularVelocity": 0.04505819641812337, + "velocityX": -1.9677297580465591, + "velocityY": -0.8485914897167804, + "timestamp": 5.947985641649457 + }, + { + "x": 3.132337992654885, + "y": 6.1744965280297155, + "heading": 0.27407536868463933, + "angularVelocity": 0.0586318729440976, + "velocityX": -2.303593334193982, + "velocityY": -0.9701249885693223, + "timestamp": 6.029857188987679 + }, + { + "x": 2.9153402053678152, + "y": 6.088110219485557, + "heading": 0.2808618012562985, + "angularVelocity": 0.08289122158182176, + "velocityX": -2.650466423831284, + "velocityY": -1.0551444470358684, + "timestamp": 6.1117287363259 + }, + { + "x": 2.6980153975230565, + "y": 6.030900368674856, + "heading": 0.29585281339155545, + "angularVelocity": 0.1831040529053057, + "velocityX": -2.6544607364871555, + "velocityY": -0.6987757367569974, + "timestamp": 6.1936002836641215 + }, + { + "x": 2.5044902704926533, + "y": 5.990681526275272, + "heading": 0.31201148477172486, + "angularVelocity": 0.1973661412971212, + "velocityX": -2.3637653534374596, + "velocityY": -0.4912432182750236, + "timestamp": 6.275471831002343 + }, + { + "x": 2.3360827419423367, + "y": 5.965439222430923, + "heading": 0.328645437097879, + "angularVelocity": 0.20317134422093217, + "velocityX": -2.0569725872482194, + "velocityY": -0.30831594937456525, + "timestamp": 6.357343378340564 + }, + { + "x": 2.1932220965526215, + "y": 5.95442921664246, + "heading": 0.34550838612461254, + "angularVelocity": 0.2059683684378234, + "velocityX": -1.744936428275136, + "velocityY": -0.13447902411053952, + "timestamp": 6.439214925678786 + }, + { + "x": 2.076119719939017, + "y": 5.957265361506873, + "heading": 0.3624743968160351, + "angularVelocity": 0.20722718994590164, + "velocityX": -1.4303183513784072, + "velocityY": 0.03464139810008477, + "timestamp": 6.521086473017007 + }, + { + "x": 1.984901205969126, + "y": 5.973711453340982, + "heading": 0.37946704953669896, + "angularVelocity": 0.20755260249895005, + "velocityX": -1.1141662388895142, + "velocityY": 0.20087676816670857, + "timestamp": 6.602958020355229 + }, + { + "x": 1.9196497213527057, + "y": 6.003608138127345, + "heading": 0.39643505242128974, + "angularVelocity": 0.207251523102328, + "velocityX": -0.7969983069558729, + "velocityY": 0.3651657475442049, + "timestamp": 6.68482956769345 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 8.34871718979539e-18, - "angularVelocity": -2.430481161157101e-17, - "velocityX": 3.691913715418776, - "velocityY": -1.3483585385427073, - "timestamp": 9.871066650077262 - }, - { - "x": 5.935813024971261, - "y": 4.118882926647287, - "heading": 6.616284346114717e-18, - "angularVelocity": -2.3674050338447945e-17, - "velocityX": 3.6877099925560515, - "velocityY": -1.062690370240863, - "timestamp": 9.944245207147963 - }, - { - "x": 6.200932724790643, - "y": 4.061479044678689, - "heading": 4.986300781220053e-18, - "angularVelocity": -2.2274059919222546e-17, - "velocityX": 3.6229151056263116, - "velocityY": -0.7844358274670198, - "timestamp": 10.017423764218664 - }, - { - "x": 6.456598704926571, - "y": 4.022722730407595, - "heading": 3.514116805070136e-18, - "angularVelocity": -2.0117696155433974e-17, - "velocityX": 3.4937280860691824, - "velocityY": -0.5296129880457788, - "timestamp": 10.090602321289365 - }, - { - "x": 6.6986166899486275, - "y": 3.999804245428159, - "heading": 2.243253754519844e-18, - "angularVelocity": -1.7366604392085868e-17, - "velocityX": 3.3072254320105032, - "velocityY": -0.31318580055210804, - "timestamp": 10.163780878360066 - }, - { - "x": 6.923883976637484, - "y": 3.9893965055012424, - "heading": 1.2009079614811148e-18, - "angularVelocity": -1.42438691719364e-17, - "velocityX": 3.078323701726084, - "velocityY": -0.1422239019670904, - "timestamp": 10.236959435430768 - }, - { - "x": 7.1304235610549185, - "y": 3.9882827335868942, - "heading": 4.0101770388899655e-19, - "angularVelocity": -1.0930664523394714e-17, - "velocityX": 2.822405806907137, - "velocityY": -0.01521992177670267, - "timestamp": 10.310137992501469 - }, - { - "x": 7.317098326514511, - "y": 3.9936876792547547, - "heading": -1.5059558453470426e-19, - "angularVelocity": -7.537908788371176e-18, - "velocityX": 2.5509489792103524, - "velocityY": 0.07385969174875201, - "timestamp": 10.38331654957217 - }, - { - "x": 7.48330233647595, - "y": 4.00334149700279, - "heading": -4.524148120436366e-19, - "angularVelocity": -4.1244216832965696e-18, - "velocityX": 2.2712119043405092, - "velocityY": 0.13192140067354147, - "timestamp": 10.45649510664287 - }, - { - "x": 7.628740531129745, - "y": 4.015421358409772, - "heading": -5.050368427376048e-19, - "angularVelocity": -7.190908486340082e-19, - "velocityX": 1.987442776622101, - "velocityY": 0.1650737851432527, - "timestamp": 10.529673663713572 - }, - { - "x": 7.753293848479072, - "y": 4.028463870964023, - "heading": -3.1002864921528455e-19, - "angularVelocity": 2.6648269843554143e-18, - "velocityX": 1.7020466422833307, - "velocityY": 0.17822861062495024, - "timestamp": 10.602852220784273 - }, - { - "x": 7.856941288758159, - "y": 4.0412847502588685, - "heading": 2.203678311282748e-19, - "angularVelocity": 7.247976752183959e-18, - "velocityX": 1.4163635418357239, - "velocityY": 0.17519994665183636, - "timestamp": 10.676030777854974 - }, - { - "x": 7.939715627162556, - "y": 4.052914641548701, - "heading": 3.4450417763013896e-19, - "angularVelocity": 1.6963486504629427e-18, - "velocityX": 1.1311283211614112, - "velocityY": 0.15892485114999483, - "timestamp": 10.749209334925675 - }, - { - "x": 8.00167823825641, - "y": 4.062550327487905, - "heading": 7.102203419824833e-19, - "angularVelocity": 4.997586438339274e-18, - "velocityX": 0.8467317965013967, - "velocityY": 0.1316736257848719, - "timestamp": 10.822387891996376 - }, - { - "x": 8.042904705550558, - "y": 4.0695182051166965, - "heading": 2.3618432486443475e-19, - "angularVelocity": -6.477799455470437e-18, - "velocityX": 0.5633681360281035, - "velocityY": 0.09521747773817751, - "timestamp": 10.895566449067077 + "x": 1.8804243803024292, + "y": 6.046840667724609, + "heading": 0.41334160220463595, + "angularVelocity": 0.20650091922048514, + "velocityX": -0.4791083389230676, + "velocityY": 0.5280531638014023, + "timestamp": 6.766701115031672 + }, + { + "x": 1.8671378435769306, + "y": 6.1025986442210955, + "heading": 0.4299850055686104, + "angularVelocity": 0.20542336557580967, + "velocityX": -0.16399080352197462, + "velocityY": 0.6882000597544763, + "timestamp": 6.84772112615515 + }, + { + "x": 1.8794728316218305, + "y": 6.171151657888672, + "heading": 0.4464831819952248, + "angularVelocity": 0.20363088325759873, + "velocityX": 0.1522461904639937, + "velocityY": 0.8461244662518951, + "timestamp": 6.928741137278629 + }, + { + "x": 1.9175617070085949, + "y": 6.252231071793212, + "heading": 0.46275009292634184, + "angularVelocity": 0.2007764588716919, + "velocityX": 0.4701168866629144, + "velocityY": 1.0007331865330475, + "timestamp": 7.009761148402108 + }, + { + "x": 1.9816154560716552, + "y": 6.34539325484771, + "heading": 0.47864458300524776, + "angularVelocity": 0.19617980618000597, + "velocityX": 0.7905917090709768, + "velocityY": 1.1498663325596743, + "timestamp": 7.090781159525586 + }, + { + "x": 2.0720214338634078, + "y": 6.449767500665198, + "heading": 0.4938927703980073, + "angularVelocity": 0.18820273141558316, + "velocityX": 1.115847511474288, + "velocityY": 1.2882526720271141, + "timestamp": 7.171801170649065 + }, + { + "x": 2.1896944819931505, + "y": 6.562891621237937, + "heading": 0.5077401370806992, + "angularVelocity": 0.17091292003882794, + "velocityX": 1.4523948651451515, + "velocityY": 1.3962491365291643, + "timestamp": 7.252821181772544 + }, + { + "x": 2.3309802432224416, + "y": 6.659921406601519, + "heading": 0.5136983140040886, + "angularVelocity": 0.07353957177701248, + "velocityX": 1.7438378404313606, + "velocityY": 1.1976027158982174, + "timestamp": 7.333841192896022 + }, + { + "x": 2.4512963274996546, + "y": 6.737453803425126, + "heading": 0.5174619765612319, + "angularVelocity": 0.04645349346357551, + "velocityX": 1.4850168817410538, + "velocityY": 0.9569536679703065, + "timestamp": 7.414861204019501 + }, + { + "x": 2.5483430242698013, + "y": 6.798294060832995, + "heading": 0.5200641230005525, + "angularVelocity": 0.03211733006744314, + "velocityX": 1.197811447128081, + "velocityY": 0.7509287713518723, + "timestamp": 7.49588121514298 + }, + { + "x": 2.62147124688145, + "y": 6.843385043467462, + "heading": 0.5218324598046723, + "angularVelocity": 0.02182592645444906, + "velocityX": 0.9025945762979182, + "velocityY": 0.556541303922395, + "timestamp": 7.576901226266458 + }, + { + "x": 2.6703780246763387, + "y": 6.8731963037812145, + "heading": 0.5229271765337971, + "angularVelocity": 0.013511683273610298, + "velocityX": 0.6036382508063606, + "velocityY": 0.367949348566719, + "timestamp": 7.657921237389937 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 2.0025514182915097e-40, - "angularVelocity": -3.2275072689602567e-18, - "velocityX": 0.2811186469496163, - "velocityY": 0.05095414427594005, - "timestamp": 10.968745006137778 + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": 0.006368644721814939, + "velocityX": 0.30252191195191025, + "velocityY": 0.18282262031723026, + "timestamp": 7.738941248513416 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 3.2506461802069203e-40, - "angularVelocity": 1.7055464658974957e-39, - "velocityX": 1.2054870824295223e-36, - "velocityY": -6.907165459298352e-35, - "timestamp": 11.04192356320848 + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": -2.018007055520513e-33, + "velocityX": -1.0144673376047062e-35, + "velocityY": 0, + "timestamp": 7.819961259636894 } ], "constraints": [ @@ -31844,25 +21251,13 @@ }, { "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" - }, - { - "scope": [ - 10 + 4 ], "type": "StopPoint" }, { "scope": [ - 7 + 1 ], "type": "StopPoint" } @@ -31872,7 +21267,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "CenterLanePBDA": { + "CenterLanePCBA": { "waypoints": [ { "x": 1.2899744510650635, @@ -31881,57 +21276,48 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 17 }, { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 16 }, { - "x": 4.897946357727051, - "y": 6.407341003417969, + "x": 1.8129411935806274, + "y": 5.542999267578125, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 21 - }, - { - "x": 8.008931159973145, - "y": 7.328619956970215, - "heading": 0.4756948301053548, - "isInitialGuess": false, - "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 9 }, { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 14 }, { - "x": 1.8804243803024292, - "y": 6.046840667724609, - "heading": 0, + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 14 + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -31942,920 +21328,569 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 4.86675280124204e-44, - "velocityX": -2.693796222451342e-38, - "velocityY": -1.1654231803151638e-36, + "heading": -1.7297728380351187e-19, + "angularVelocity": 4.2448351957919954e-19, + "velocityX": -3.109246931729259e-17, + "velocityY": 1.4154954258989747e-18, "timestamp": 0 }, { - "x": 1.3282580675803197, - "y": 5.589895062878959, - "heading": -7.401619546168889e-20, - "angularVelocity": -7.4730824316044545e-19, - "velocityX": 0.3865324611915868, - "velocityY": -0.010694678686168042, - "timestamp": 0.09904372946385107 - }, - { - "x": 1.4048252996715709, - "y": 5.587776581179956, - "heading": -2.950926029442004e-19, - "angularVelocity": -2.23210907625606e-18, - "velocityX": 0.7730649128998782, - "velocityY": -0.021389357109949856, - "timestamp": 0.19808745892770213 - }, - { - "x": 1.5196761458628356, - "y": 5.584598858685282, - "heading": -2.379692913245565e-19, - "angularVelocity": 5.76748391128508e-19, - "velocityX": 1.159597349705849, - "velocityY": -0.03208403512141046, - "timestamp": 0.29713118839155317 - }, - { - "x": 1.6728106034973478, - "y": 5.580361895468446, - "heading": -4.645307250560927e-19, - "angularVelocity": -2.2874889198737866e-18, - "velocityX": 1.5461297596876444, - "velocityY": -0.04277871239069293, - "timestamp": 0.39617491785540426 - }, - { - "x": 1.8642286663759866, - "y": 5.575065691700967, - "heading": -6.651013199966167e-20, - "angularVelocity": 4.018634952571123e-18, - "velocityX": 1.9326621070797045, - "velocityY": -0.05347338792822665, - "timestamp": 0.49521864731925536 - }, - { - "x": 2.0939303035031647, - "y": 5.568710248240439, - "heading": 1.5670193439704088e-19, - "angularVelocity": 2.2536718639837896e-18, - "velocityX": 2.3191941415232606, - "velocityY": -0.06416805480702138, - "timestamp": 0.5942623767831064 - }, - { - "x": 2.285348366381803, - "y": 5.56341404447296, - "heading": 5.516454674221488e-20, - "angularVelocity": -1.0251773454460286e-18, - "velocityX": 1.9326621070797048, - "velocityY": -0.05347338792822665, - "timestamp": 0.6933061062469575 - }, - { - "x": 2.438482824016315, - "y": 5.5591770812561245, - "heading": 6.884550294722241e-20, - "angularVelocity": 1.3813046296889445e-19, - "velocityX": 1.5461297596876444, - "velocityY": -0.04277871239069293, - "timestamp": 0.7923498357108086 - }, - { - "x": 2.55333367020758, - "y": 5.555999358761451, - "heading": 2.3879991444087165e-19, - "angularVelocity": 1.7159532704761406e-18, - "velocityX": 1.1595973497058492, - "velocityY": -0.032084035121410455, - "timestamp": 0.8913935651746597 - }, - { - "x": 2.629900902298831, - "y": 5.553880877062447, - "heading": 4.476119744497135e-19, - "angularVelocity": 2.1082814746495792e-18, - "velocityX": 0.7730649128998782, - "velocityY": -0.021389357109949853, - "timestamp": 0.9904372946385108 + "x": 1.3060810448259774, + "y": 5.572403683390414, + "heading": -0.01067910748684314, + "angularVelocity": -0.14191654637088458, + "velocityX": 0.21404337049401068, + "velocityY": -0.246522471704611, + "timestamp": 0.07524920638169826 + }, + { + "x": 1.338296279992689, + "y": 5.535304313687478, + "heading": -0.032027558921844806, + "angularVelocity": -0.28370334335098873, + "velocityX": 0.4281139525027307, + "velocityY": -0.4930200793709309, + "timestamp": 0.1504984127633965 + }, + { + "x": 1.3866231363779933, + "y": 5.479658598357654, + "heading": -0.06402169471627948, + "angularVelocity": -0.42517572387591435, + "velocityX": 0.6422241337702286, + "velocityY": -0.7394857435109029, + "timestamp": 0.22574761914509478 + }, + { + "x": 1.4510659275520246, + "y": 5.405469878379882, + "heading": -0.1066262273585151, + "angularVelocity": -0.5661791624235272, + "velocityX": 0.8563916388334698, + "velocityY": -0.9859070087913582, + "timestamp": 0.300996825526793 + }, + { + "x": 1.5316310742105061, + "y": 5.312743180460654, + "heading": -0.15979745091713102, + "angularVelocity": -0.7066017851257455, + "velocityX": 1.0706444696567097, + "velocityY": -1.2322614732810149, + "timestamp": 0.37624603190849126 + }, + { + "x": 1.6283288838862067, + "y": 5.201486845299536, + "heading": -0.22348688396132305, + "angularVelocity": -0.8463801295274643, + "velocityX": 1.2850342791041873, + "velocityY": -1.4785050967404112, + "timestamp": 0.4514952382901895 + }, + { + "x": 1.7411789077586277, + "y": 5.071717216156576, + "heading": -0.2976431936929222, + "angularVelocity": -0.9854763033040427, + "velocityX": 1.4996839076304158, + "velocityY": -1.7245315317294676, + "timestamp": 0.5267444446718877 + }, + { + "x": 1.8702368010806378, + "y": 4.923481749892843, + "heading": -0.3821997162379228, + "angularVelocity": -1.1236865690804896, + "velocityX": 1.7150731486446302, + "velocityY": -1.9699273041075376, + "timestamp": 0.601993651053586 + }, + { + "x": 2.022770759979851, + "y": 4.779444476824575, + "heading": -0.46182371324885607, + "angularVelocity": -1.0581373656886817, + "velocityX": 2.0270507322765163, + "velocityY": -1.914136772919067, + "timestamp": 0.6772428574352843 + }, + { + "x": 2.159313623811303, + "y": 4.6540559405768205, + "heading": -0.5309316944889129, + "angularVelocity": -0.9183881739497737, + "velocityX": 1.8145422443221764, + "velocityY": -1.666310414116386, + "timestamp": 0.7524920638169826 + }, + { + "x": 2.2798095691663756, + "y": 4.547269113259306, + "heading": -0.5895622742606058, + "angularVelocity": -0.7791521344995035, + "velocityX": 1.601291909229005, + "velocityY": -1.4191090172553904, + "timestamp": 0.8277412701986809 + }, + { + "x": 2.3842397515062683, + "y": 4.459067906121371, + "heading": -0.6377272819831894, + "angularVelocity": -0.6400732982918503, + "velocityX": 1.3877911457302048, + "velocityY": -1.172121426644562, + "timestamp": 0.9029904765803792 + }, + { + "x": 2.47259465372947, + "y": 4.389444105411809, + "heading": -0.675431270833758, + "angularVelocity": -0.5010549700591305, + "velocityX": 1.1741639077877584, + "velocityY": -0.9252429900248669, + "timestamp": 0.9782396829620775 + }, + { + "x": 2.5448685468329297, + "y": 4.338392728081247, + "heading": -0.7026759403224371, + "angularVelocity": -0.36205922691677456, + "velocityX": 0.9604605361114344, + "velocityY": -0.6784307740288329, + "timestamp": 1.0534888893437757 + }, + { + "x": 2.601057641331632, + "y": 4.305910462744002, + "heading": -0.7194616948028688, + "angularVelocity": -0.2230688573014308, + "velocityX": 0.7467068052991092, + "velocityY": -0.43166256362226185, + "timestamp": 1.128738095725474 + }, + { + "x": 2.641159295921209, + "y": 4.291994997972796, + "heading": -0.7257882885399572, + "angularVelocity": -0.08407522206898749, + "velocityX": 0.5329179737262918, + "velocityY": -0.18492507018124238, + "timestamp": 1.2039873021071723 }, { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, - "angularVelocity": -4.519336831041715e-18, - "velocityX": 0.3865324611915868, - "velocityY": -0.010694678686168042, - "timestamp": 1.0894810241023618 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.054926472219667455, + "velocityX": 0.31910406053771284, + "velocityY": 0.061790547746105144, + "timestamp": 1.2792365084888706 + }, + { + "x": 2.6730242117812852, + "y": 4.320207160810404, + "heading": -0.7068522062263088, + "angularVelocity": 0.19509567726207716, + "velocityX": 0.10349358326673892, + "velocityY": 0.31054279257798323, + "timestamp": 1.3551116333379198 + }, + { + "x": 2.6645156104831624, + "y": 4.362642495661916, + "heading": -0.6814188000083412, + "angularVelocity": 0.33520084834820957, + "velocityX": -0.1121395360458166, + "velocityY": 0.5592786164892918, + "timestamp": 1.430986758186969 + }, + { + "x": 2.6396435810539995, + "y": 4.423949055779942, + "heading": -0.6453599487845141, + "angularVelocity": 0.47523943183649325, + "velocityX": -0.32780215490271986, + "velocityY": 0.807992873026772, + "timestamp": 1.5068618830360183 + }, + { + "x": 2.5984050053739445, + "y": 4.5041245094115245, + "heading": -0.5986803757122448, + "angularVelocity": 0.615215766235525, + "velocityX": -0.5435058691760214, + "velocityY": 1.0566763981096883, + "timestamp": 1.5827370078850675 + }, + { + "x": 2.540795163397632, + "y": 4.603165195262531, + "heading": -0.541383229045499, + "angularVelocity": 0.755150608064052, + "velocityX": -0.7592717915230448, + "velocityY": 1.3053116689836102, + "timestamp": 1.6586121327341168 + }, + { + "x": 2.4668060178101583, + "y": 4.721064600465768, + "heading": -0.4734677826566921, + "angularVelocity": 0.8950950199273465, + "velocityX": -0.9751436420635095, + "velocityY": 1.5538611031988796, + "timestamp": 1.734487257583166 + }, + { + "x": 2.3764209643089336, + "y": 4.857808746061696, + "heading": -0.3949264705539342, + "angularVelocity": 1.0351391481588907, + "velocityX": -1.1912343298399446, + "velocityY": 1.8022263010181736, + "timestamp": 1.8103623824322153 + }, + { + "x": 2.269588305263905, + "y": 5.013353141552007, + "heading": -0.3057482687442009, + "angularVelocity": 1.175328567657553, + "velocityX": -1.4080063691172056, + "velocityY": 2.0500051340898513, + "timestamp": 1.8862375072812645 + }, + { + "x": 2.155512806030656, + "y": 5.145840094887286, + "heading": -0.22945454409592414, + "angularVelocity": 1.0055169569743476, + "velocityX": -1.5034637433586158, + "velocityY": 1.7461184228535829, + "timestamp": 1.9621126321303137 + }, + { + "x": 2.0576690248190195, + "y": 5.259344023095087, + "heading": -0.1639803843714035, + "angularVelocity": 0.8629199603276121, + "velocityX": -1.289537004472555, + "velocityY": 1.4959306944600357, + "timestamp": 2.037987756979363 + }, + { + "x": 1.9761088976814785, + "y": 5.353909559223238, + "heading": -0.10935658983885492, + "angularVelocity": 0.7199170300053718, + "velocityX": -1.0749257717858158, + "velocityY": 1.2463312095522094, + "timestamp": 2.113862881828412 + }, + { + "x": 1.9108492511031485, + "y": 5.429551680381221, + "heading": -0.06562174275393094, + "angularVelocity": 0.5764056029164016, + "velocityX": -0.8600927735973349, + "velocityY": 0.9969291162074897, + "timestamp": 2.1897380066774614 + }, + { + "x": 1.8618983395529738, + "y": 5.486277925937523, + "heading": -0.03280716495777854, + "angularVelocity": 0.4324813680566584, + "velocityX": -0.6451509852228532, + "velocityY": 0.7476263883465671, + "timestamp": 2.2656131315265107 + }, + { + "x": 1.8292611562031649, + "y": 5.524092810268815, + "heading": -0.010931847108471086, + "angularVelocity": 0.2883068448697581, + "velocityX": -0.430143389085703, + "velocityY": 0.4983831579385639, + "timestamp": 2.34148825637556 }, { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.8370589120240526e-33, - "velocityY": -9.456793291047022e-33, - "timestamp": 1.188524753566213 - }, - { - "x": 2.6845520739340656, - "y": 5.55917649064291, - "heading": 0.0014320304181618415, - "angularVelocity": 0.02135174975700707, - "velocityX": 0.2440422609907963, - "velocityY": 0.0947516617576028, - "timestamp": 1.255593281179126 - }, - { - "x": 2.7172893704980314, - "y": 5.5718808969789135, - "heading": 0.004271918603718149, - "angularVelocity": 0.042343082316444396, - "velocityX": 0.4881171203415973, - "velocityY": 0.189424261843461, - "timestamp": 1.322661808792039 - }, - { - "x": 2.766398904551383, - "y": 5.590928790480421, - "heading": 0.008491864035516146, - "angularVelocity": 0.06291990568442865, - "velocityX": 0.7322291960364448, - "velocityY": 0.2840064360953652, - "timestamp": 1.389730336404952 - }, - { - "x": 2.8318835522169303, - "y": 5.616313167431409, - "heading": 0.014059653133075762, - "angularVelocity": 0.08301642060332945, - "velocityX": 0.9763841550763344, - "velocityY": 0.3784841840795224, - "timestamp": 1.456798864017865 - }, - { - "x": 2.9137466632036926, - "y": 5.648025848972547, - "heading": 0.020937610104521436, - "angularVelocity": 0.10255118482907341, - "velocityX": 1.2205890586898858, - "velocityY": 0.47283998426459056, - "timestamp": 1.5238673916307781 - }, - { - "x": 3.011992187841884, - "y": 5.68605715842892, - "heading": 0.02908116230944334, - "angularVelocity": 0.12142136550130508, - "velocityX": 1.4648528622130628, - "velocityY": 0.5670515040358618, - "timestamp": 1.5909359192436912 - }, - { - "x": 3.126624854139339, - "y": 5.730395468508435, - "heading": 0.03843682536129391, - "angularVelocity": 0.13949408738845287, - "velocityX": 1.709187160914865, - "velocityY": 0.6610896594512152, - "timestamp": 1.6580044468566042 - }, - { - "x": 3.257650422147747, - "y": 5.781026545419884, - "heading": 0.04893928266503782, - "angularVelocity": 0.1565929308059208, - "velocityX": 1.9536073426961744, - "velocityY": 0.7549155872880825, - "timestamp": 1.7250729744695172 - }, - { - "x": 3.405076062959565, - "y": 5.837932561689472, - "heading": 0.0605069886709794, - "angularVelocity": 0.1724759200426279, - "velocityX": 2.1981344463484804, - "velocityY": 0.8484757052968557, - "timestamp": 1.7921415020824303 - }, - { - "x": 3.568910949214301, - "y": 5.901090538280509, - "heading": 0.07303523263359421, - "angularVelocity": 0.18679765917811303, - "velocityX": 2.4427983151846027, - "velocityY": 0.9416932030408492, - "timestamp": 1.8592100296953433 - }, - { - "x": 3.749167228301205, - "y": 5.970469734020464, - "heading": 0.08638452799993987, - "angularVelocity": 0.19903963664434834, - "velocityX": 2.6876433030892812, - "velocityY": 1.0344523461194588, - "timestamp": 1.9262785573082564 - }, - { - "x": 3.9458617473368536, - "y": 6.0460269127291015, - "heading": 0.10035960589660178, - "angularVelocity": 0.20837013117865452, - "velocityX": 2.9327394835752725, - "velocityY": 1.1265668324301998, - "timestamp": 1.9933470849211694 - }, - { - "x": 4.159019426107489, - "y": 6.127696782408767, - "heading": 0.1146671212326001, - "angularVelocity": 0.2133268143081109, - "velocityX": 3.178207221140708, - "velocityY": 1.2177078070212743, - "timestamp": 2.0604156125340825 - }, - { - "x": 4.388680844705306, - "y": 6.21536934931964, - "heading": 0.12881599036360453, - "angularVelocity": 0.21096137986903263, - "velocityX": 3.4242800128744704, - "velocityY": 1.3072087614160277, - "timestamp": 2.1274841401469953 - }, - { - "x": 4.63492344353525, - "y": 6.3088202142113, - "heading": 0.14181307281667466, - "angularVelocity": 0.19378809876493772, - "velocityX": 3.671507450575592, - "velocityY": 1.3933638953729879, - "timestamp": 2.194552667759908 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -1.6776667047563746e-20, + "angularVelocity": 0.1440768253130289, + "velocityX": -0.2150897630156809, + "velocityY": 0.24917859900600148, + "timestamp": 2.417363381224609 }, { - "x": 4.897946357727051, - "y": 6.407341003417969, - "heading": 0.1505657563591837, - "angularVelocity": 0.13050358870296755, - "velocityX": 3.9217040175660647, - "velocityY": 1.4689570908024474, - "timestamp": 2.261621195372821 - }, - { - "x": 5.1485624399320535, - "y": 6.485405317317771, - "heading": 0.15056576180436299, - "angularVelocity": 8.767085583792628e-8, - "velocityX": 4.035078615644034, - "velocityY": 1.2568851962354253, - "timestamp": 2.323730538480379 - }, - { - "x": 5.403242092396325, - "y": 6.54897207571473, - "heading": 0.150565765506882, - "angularVelocity": 5.961291546210121e-8, - "velocityX": 4.100504686118055, - "velocityY": 1.0234653148229949, - "timestamp": 2.3858398815879367 - }, - { - "x": 5.658526059829905, - "y": 6.6100667082463955, - "heading": 0.15056576920637427, - "angularVelocity": 5.956418276221387e-8, - "velocityX": 4.110234542192673, - "velocityY": 0.9836625131562424, - "timestamp": 2.4479492246954946 - }, - { - "x": 5.912767823919927, - "y": 6.675362821260306, - "heading": 0.1505657770066318, - "angularVelocity": 1.2558911624775388e-7, - "velocityX": 4.093454404271187, - "velocityY": 1.0513090260965858, - "timestamp": 2.5100585678030525 - }, - { - "x": 6.15335684749113, - "y": 6.741282299763531, - "heading": 0.17760143548847274, - "angularVelocity": 0.4352913286334883, - "velocityX": 3.8736365824150507, - "velocityY": 1.061345607681999, - "timestamp": 2.5721679109106104 - }, - { - "x": 6.3792484159879335, - "y": 6.80391531011499, - "heading": 0.2045043567072466, - "angularVelocity": 0.4331541741181402, - "velocityX": 3.6369981905237077, - "velocityY": 1.0084313763066106, - "timestamp": 2.6342772540181683 - }, - { - "x": 6.59046786405772, - "y": 6.863155314435484, - "heading": 0.2306695247633623, - "angularVelocity": 0.42127587810427364, - "velocityX": 3.400767702598441, - "velocityY": 0.9538018171903946, - "timestamp": 2.696386597125726 - }, - { - "x": 6.787024437672222, - "y": 6.918965755644967, - "heading": 0.2558883618424076, - "angularVelocity": 0.4060393463729393, - "velocityX": 3.1646860807095556, - "velocityY": 0.8985836657912935, - "timestamp": 2.758495940233284 - }, - { - "x": 6.968922917887586, - "y": 6.971328161049253, - "heading": 0.28005507074306385, - "angularVelocity": 0.3890994122865805, - "velocityX": 2.928681436871152, - "velocityY": 0.8430680922450434, - "timestamp": 2.820605283340842 - }, - { - "x": 7.136166224572301, - "y": 7.020231388365112, - "heading": 0.3031055255244397, - "angularVelocity": 0.3711270096909292, - "velocityX": 2.6927238047759023, - "velocityY": 0.7873731208389769, - "timestamp": 2.8827146264484 - }, - { - "x": 7.288756325694741, - "y": 7.065667987349534, - "heading": 0.3249965680879426, - "angularVelocity": 0.3524597342076698, - "velocityX": 2.4567978582254835, - "velocityY": 0.7315581957730963, - "timestamp": 2.9448239695559577 - }, - { - "x": 7.426694637421038, - "y": 7.107632627152973, - "heading": 0.34569706172661535, - "angularVelocity": 0.33329113790214354, - "velocityX": 2.2208947128521856, - "velocityY": 0.6756574406327761, - "timestamp": 3.0069333126635156 - }, - { - "x": 7.549982227615165, - "y": 7.146121305576968, - "heading": 0.3651834005383923, - "angularVelocity": 0.3137424715317206, - "velocityX": 1.9850087607692757, - "velocityY": 0.6196922475470147, - "timestamp": 3.0690426557710735 - }, - { - "x": 7.6586199302579505, - "y": 7.181130908016494, - "heading": 0.38343701279157455, - "angularVelocity": 0.2938947884470677, - "velocityX": 1.7491362363091372, - "velocityY": 0.5636769073357482, - "timestamp": 3.1311519988786314 - }, - { - "x": 7.752608414705494, - "y": 7.212658942032951, - "heading": 0.40044286570608717, - "angularVelocity": 0.27380506802434795, - "velocityX": 1.5132744889086964, - "velocityY": 0.5076214372748387, - "timestamp": 3.1932613419861893 - }, - { - "x": 7.831948230057004, - "y": 7.240703368053924, - "heading": 0.41618851788690064, - "angularVelocity": 0.2535150332140168, - "velocityX": 1.2774215823553952, - "velocityY": 0.45153312879840174, - "timestamp": 3.255370685093747 - }, - { - "x": 7.896639834885265, - "y": 7.265262486298443, - "heading": 0.43066349152059785, - "angularVelocity": 0.2330562989312256, - "velocityX": 1.0415760591161314, - "velocityY": 0.39541745276528995, - "timestamp": 3.317480028201305 - }, - { - "x": 7.946683617901462, - "y": 7.286334858378348, - "heading": 0.4438588411976607, - "angularVelocity": 0.2124535378551918, - "velocityX": 0.8057367943746069, - "velocityY": 0.3392786177662535, - "timestamp": 3.379589371308863 - }, - { - "x": 7.982079912761947, - "y": 7.303919251231725, - "heading": 0.4557668488029474, - "angularVelocity": 0.19172651020740672, - "velocityX": 0.5699029017130068, - "velocityY": 0.2831199296846922, - "timestamp": 3.441698714416421 - }, - { - "x": 8.002829008953428, - "y": 7.318014595966873, - "heading": 0.4663808020502884, - "angularVelocity": 0.17089141047523468, - "velocityX": 0.3340736699718424, - "velocityY": 0.22694403176565744, - "timestamp": 3.5038080575239787 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.1170600847702852e-20, + "angularVelocity": 5.938902420193292e-19, + "velocityX": -2.686779016810018e-17, + "velocityY": -1.2969153357483792e-17, + "timestamp": 2.4932385060736584 + }, + { + "x": 1.8552093951788071, + "y": 5.543066072468525, + "heading": -1.0867013680305264e-17, + "angularVelocity": -1.1051820926914384e-16, + "velocityX": 0.42942883009919236, + "velocityY": 0.0006787122433536914, + "timestamp": 2.5916673926214395 + }, + { + "x": 1.9397457967809342, + "y": 5.543199682246804, + "heading": -3.2635799657096195e-17, + "angularVelocity": -2.2116257473548814e-16, + "velocityX": 0.8588576440015888, + "velocityY": 0.001357424461108283, + "timestamp": 2.6900962791692207 + }, + { + "x": 2.0665503951985436, + "y": 5.5434000969079245, + "heading": -4.603800029490119e-17, + "angularVelocity": -1.3616125416355767e-16, + "velocityX": 1.2882864255103954, + "velocityY": 0.002036136627664645, + "timestamp": 2.788525165717002 + }, + { + "x": 2.235623180866242, + "y": 5.543667316436768, + "heading": -4.798133527402523e-17, + "angularVelocity": -1.9743543259972466e-17, + "velocityX": 1.7177151098384498, + "velocityY": 0.002714848640626296, + "timestamp": 2.886954052264783 + }, + { + "x": 2.40469596653394, + "y": 5.54393453596561, + "heading": -3.9330353852399465e-17, + "angularVelocity": 8.789067645262313e-17, + "velocityX": 1.7177151098384498, + "velocityY": 0.0027148486406302456, + "timestamp": 2.985382938812564 + }, + { + "x": 2.531500564951549, + "y": 5.544134950626731, + "heading": -2.0009563266152006e-17, + "angularVelocity": 1.96291873894211e-16, + "velocityX": 1.2882864255103954, + "velocityY": 0.002036136627666603, + "timestamp": 3.0838118253603453 + }, + { + "x": 2.616036966553676, + "y": 5.54426856040501, + "heading": -7.02269200923652e-18, + "angularVelocity": 1.3194166582538958e-16, + "velocityX": 0.8588576440015888, + "velocityY": 0.0013574244611093242, + "timestamp": 3.1822407119081264 }, { - "x": 8.008931159973145, - "y": 7.328619956970215, - "heading": 0.4756948301053548, - "angularVelocity": 0.14996178656947964, - "velocityX": 0.09824851969771953, - "velocityY": 0.17075306987185124, - "timestamp": 3.5659174006315366 - }, - { - "x": 7.983403951662257, - "y": 7.3366726271050196, - "heading": 0.4869682842338856, - "angularVelocity": 0.11749578140218661, - "velocityX": -0.2660532657788933, - "velocityY": 0.08392767283885226, - "timestamp": 3.661865135894831 - }, - { - "x": 7.922922818311127, - "y": 7.336394611278309, - "heading": 0.4951243670003803, - "angularVelocity": 0.08500547453374917, - "velocityX": -0.6303549863387643, - "velocityY": -0.0028975757056328576, - "timestamp": 3.7578128711581256 - }, - { - "x": 7.827487765779536, - "y": 7.327785918399699, - "heading": 0.5001613028102507, - "angularVelocity": 0.052496661813313965, - "velocityX": -0.9946566458260192, - "velocityY": -0.08972273139104701, - "timestamp": 3.85376060642142 - }, - { - "x": 7.6970987994751, - "y": 7.310846552031537, - "heading": 0.5020779070051818, - "angularVelocity": 0.019975502180138435, - "velocityX": -1.3589582489532488, - "velocityY": -0.17654784994847797, - "timestamp": 3.9497083416847145 - }, - { - "x": 7.531755924357651, - "y": 7.285576510321804, - "heading": 0.500873602063457, - "angularVelocity": -0.012551676581213792, - "velocityX": -1.7232598003874058, - "velocityY": -0.26337298780827745, - "timestamp": 4.045656076948009 - }, - { - "x": 7.331459145051584, - "y": 7.25197578585626, - "heading": 0.4965484153181755, - "angularVelocity": -0.045078570467686534, - "velocityX": -2.0875613036245544, - "velocityY": -0.3501982029418286, - "timestamp": 4.1416038122113035 - }, - { - "x": 7.096208466110642, - "y": 7.210044365383248, - "heading": 0.4891029581030025, - "angularVelocity": -0.07759909282633375, - "velocityX": -2.4518627594010316, - "velocityY": -0.43702355618864164, - "timestamp": 4.237551547474598 - }, - { - "x": 6.826003892546235, - "y": 7.159782229296342, - "heading": 0.47853838599717985, - "angularVelocity": -0.1101075713442515, - "velocityX": -2.8161641629468988, - "velocityY": -0.5238491137803141, - "timestamp": 4.333499282737892 - }, - { - "x": 6.520845430952262, - "y": 7.101189350526738, - "heading": 0.4648563396258396, - "angularVelocity": -0.14259895070784834, - "velocityX": -3.1804654977688984, - "velocityY": -0.6106749534922914, - "timestamp": 4.429447018001187 - }, - { - "x": 6.180733092557776, - "y": 7.034265691442456, - "heading": 0.448058865196963, - "angularVelocity": -0.17506900379442902, - "velocityX": -3.5447667155578824, - "velocityY": -0.6975011854176053, - "timestamp": 4.525394753264481 - }, - { - "x": 5.805666907523616, - "y": 6.959011188942715, - "heading": 0.428148311466242, - "angularVelocity": -0.20751457734863166, - "velocityX": -3.909067619000319, - "velocityY": -0.7843280750007373, - "timestamp": 4.621342488527776 - }, - { - "x": 5.456128243723212, - "y": 6.875701277677367, - "heading": 0.3971165613376697, - "angularVelocity": -0.3234234767857393, - "velocityX": -3.6430110918326317, - "velocityY": -0.868284290783268, - "timestamp": 4.71729022379107 - }, - { - "x": 5.141543451324252, - "y": 6.800722173314044, - "heading": 0.3691918402141841, - "angularVelocity": -0.2910409614865449, - "velocityX": -3.27870993031459, - "velocityY": -0.7814577817555671, - "timestamp": 4.813237959054365 - }, - { - "x": 4.861912536840719, - "y": 6.7340739575810815, - "heading": 0.3443722286575812, - "angularVelocity": -0.2586784512265385, - "velocityX": -2.914408700905603, - "velocityY": -0.6946304209273098, - "timestamp": 4.909185694317659 - }, - { - "x": 4.61723549857855, - "y": 6.67575668123621, - "heading": 0.3226563957175653, - "angularVelocity": -0.2263298125841594, - "velocityX": -2.550107489152699, - "velocityY": -0.6078025310846741, - "timestamp": 5.005133429580954 - }, - { - "x": 4.40751233283897, - "y": 6.6257703832205594, - "heading": 0.30404347575272556, - "angularVelocity": -0.19399019595161143, - "velocityX": -2.185806315949698, - "velocityY": -0.5209742353843284, - "timestamp": 5.101081164844248 - }, - { - "x": 4.232743035438881, - "y": 6.584115094460369, - "heading": 0.288532951546725, - "angularVelocity": -0.1616559699240165, - "velocityX": -1.821505186344387, - "velocityY": -0.4341456173601488, - "timestamp": 5.197028900107543 - }, - { - "x": 4.092927602397226, - "y": 6.550790839135286, - "heading": 0.27612455451484763, - "angularVelocity": -0.12932454317787676, - "velocityX": -1.4572040982309902, - "velocityY": -0.34731674732746287, - "timestamp": 5.292976635370837 - }, - { - "x": 3.9880660303380817, - "y": 6.525797635232698, - "heading": 0.26681818412976743, - "angularVelocity": -0.09699416416179282, - "velocityX": -1.0929030453026178, - "velocityY": -0.260487689824076, - "timestamp": 5.388924370634132 - }, - { - "x": 3.918158316749324, - "y": 6.5091354948352524, - "heading": 0.26061384732395215, - "angularVelocity": -0.06466371289316737, - "velocityX": -0.7286020185564593, - "velocityY": -0.1736585063912411, - "timestamp": 5.484872105897426 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -3.8911537114769476e-19, + "angularVelocity": 6.739461219451495e-17, + "velocityX": 0.4294288300991924, + "velocityY": 0.0006787122433541438, + "timestamp": 3.2806695984559076 }, { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "angularVelocity": -0.032332489302005434, - "velocityX": -0.36430100730739107, - "velocityY": -0.08682925685012162, - "timestamp": 5.580819841160721 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -4.27312910681112e-19, + "angularVelocity": -3.880724585667238e-19, + "velocityX": -2.3908482729518656e-18, + "velocityY": 6.838225330043828e-18, + "timestamp": 3.3790984850036887 + }, + { + "x": 2.6438893629989613, + "y": 5.563919104052164, + "heading": 0.003139586303789949, + "angularVelocity": 0.042042846518139325, + "velocityX": -0.19304501448070868, + "velocityY": 0.26224987725536203, + "timestamp": 3.453774358875246 + }, + { + "x": 2.6152669032432256, + "y": 5.6032366820496495, + "heading": 0.009644303445155527, + "angularVelocity": 0.0871060063194363, + "velocityX": -0.38328925088933385, + "velocityY": 0.5265097809918172, + "timestamp": 3.5284502327468035 + }, + { + "x": 2.5727487902328847, + "y": 5.662504910556293, + "heading": 0.019847811829768587, + "angularVelocity": 0.13663728130135686, + "velocityX": -0.5693688042201159, + "velocityY": 0.7936730490571009, + "timestamp": 3.603126106618361 + }, + { + "x": 2.5168455054942007, + "y": 5.742063733198033, + "heading": 0.03429127316793741, + "angularVelocity": 0.19341536415109115, + "velocityX": -0.7486123943435556, + "velocityY": 1.0653885721991296, + "timestamp": 3.6778019804899182 + }, + { + "x": 2.4485453653591587, + "y": 5.842518348175502, + "heading": 0.05399784068706335, + "angularVelocity": 0.2638947024981649, + "velocityX": -0.9146212370077963, + "velocityY": 1.3452084290335973, + "timestamp": 3.7524778543614756 + }, + { + "x": 2.370517685528427, + "y": 5.96519344251776, + "heading": 0.08158842838664396, + "angularVelocity": 0.369471239761256, + "velocityX": -1.0448847236115244, + "velocityY": 1.642767442578038, + "timestamp": 3.827153728233033 + }, + { + "x": 2.3003293154277267, + "y": 6.110432122884094, + "heading": 0.1284873855211158, + "angularVelocity": 0.6280335897392839, + "velocityX": -0.9399069131942741, + "velocityY": 1.94492106802986, + "timestamp": 3.9018296021045904 + }, + { + "x": 2.2517603849650345, + "y": 6.244622565856429, + "heading": 0.18176076028404833, + "angularVelocity": 0.7133947284575884, + "velocityX": -0.6503965463628967, + "velocityY": 1.796971846665519, + "timestamp": 3.976505475976148 + }, + { + "x": 2.22192407028561, + "y": 6.363296472335711, + "heading": 0.23706248397325824, + "angularVelocity": 0.7405567664909981, + "velocityX": -0.39954423206005096, + "velocityY": 1.5891867122091021, + "timestamp": 4.051181349847705 + }, + { + "x": 2.2097448289139274, + "y": 6.465233576489708, + "heading": 0.2929845885143906, + "angularVelocity": 0.7488644141924427, + "velocityX": -0.16309472846117115, + "velocityY": 1.3650607467858884, + "timestamp": 4.125857223719263 + }, + { + "x": 2.2146777318065234, + "y": 6.549876478070479, + "heading": 0.348839415183568, + "angularVelocity": 0.747963482359082, + "velocityX": 0.06605751813605938, + "velocityY": 1.1334705198944957, + "timestamp": 4.20053309759082 + }, + { + "x": 2.236395128424509, + "y": 6.61690736376859, + "heading": 0.4042192057980219, + "angularVelocity": 0.7416021767580177, + "velocityX": 0.2908221289159501, + "velocityY": 0.8976243895505626, + "timestamp": 4.275208971462377 + }, + { + "x": 2.274678638341779, + "y": 6.666121192465354, + "heading": 0.45885367997336884, + "angularVelocity": 0.7316214908889929, + "velocityX": 0.5126623624534666, + "velocityY": 0.6590325113759182, + "timestamp": 4.349884845333935 }, { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "angularVelocity": 0, - "velocityX": -7.785191054784601e-32, - "velocityY": 5.489773194245446e-31, - "timestamp": 5.676767576424015 - }, - { - "x": 3.8559784848389835, - "y": 6.488638862424018, - "heading": 0.25803498288661586, - "angularVelocity": 0.005988366435265192, - "velocityX": -0.3115210499554562, - "velocityY": -0.13919900249831443, - "timestamp": 5.7641644804500904 - }, - { - "x": 3.801482560046824, - "y": 6.464406720640634, - "heading": 0.2591128207382253, - "angularVelocity": 0.012332677726064934, - "velocityX": -0.6235452548284794, - "velocityY": -0.27726544839797734, - "timestamp": 5.851561384476166 - }, - { - "x": 3.719658287933927, - "y": 6.428240975751334, - "heading": 0.2607868123006951, - "angularVelocity": 0.01915390002797383, - "velocityX": -0.9362376508037902, - "velocityY": -0.4138103665378143, - "timestamp": 5.938958288502241 - }, - { - "x": 3.6104243848456496, - "y": 6.380329724667359, - "heading": 0.2631157133611349, - "angularVelocity": 0.026647409154732543, - "velocityX": -1.249860098656203, - "velocityY": -0.5482030698671013, - "timestamp": 6.0263551925283165 - }, - { - "x": 3.473660034873262, - "y": 6.320959355583345, - "heading": 0.26618855611424136, - "angularVelocity": 0.035159629363869176, - "velocityX": -1.564864928528632, - "velocityY": -0.6793189043206871, - "timestamp": 6.113752096554392 - }, - { - "x": 3.309167154695596, - "y": 6.250618578205828, - "heading": 0.2701561408711645, - "angularVelocity": 0.0453973147119656, - "velocityX": -1.8821362382423572, - "velocityY": -0.8048428964546626, - "timestamp": 6.201149000580467 - }, - { - "x": 3.1165641229075383, - "y": 6.170327957016436, - "heading": 0.2753290776437782, - "angularVelocity": 0.05918901625016675, - "velocityX": -2.203774080264828, - "velocityY": -0.918689535792211, - "timestamp": 6.288545904606543 - }, - { - "x": 2.8948724304190634, - "y": 6.083494017796222, - "heading": 0.2827078223400479, - "angularVelocity": 0.08442798722101476, - "velocityX": -2.536608075067875, - "velocityY": -0.9935585269051026, - "timestamp": 6.375942808632618 - }, - { - "x": 2.678270569612192, - "y": 6.025989262552558, - "heading": 0.29748746051655606, - "angularVelocity": 0.16910940200008223, - "velocityX": -2.4783699516661057, - "velocityY": -0.6579724520505627, - "timestamp": 6.463339712658693 - }, - { - "x": 2.486014315145345, - "y": 5.985703051488859, - "heading": 0.3134280719543648, - "angularVelocity": 0.18239331948249285, - "velocityX": -2.199806235807695, - "velocityY": -0.46095695851742674, - "timestamp": 6.550736616684769 - }, - { - "x": 2.3193881959437443, - "y": 5.960659974875319, - "heading": 0.3298457414331986, - "angularVelocity": 0.1878518428288434, - "velocityX": -1.9065448720230032, - "velocityY": -0.2865442076308354, - "timestamp": 6.638133520710844 - }, - { - "x": 2.1788238735004155, - "y": 5.950110033535781, - "heading": 0.3464901333226275, - "angularVelocity": 0.19044601264666053, - "velocityX": -1.6083444145960923, - "velocityY": -0.1207129869999774, - "timestamp": 6.725530424736919 - }, - { - "x": 2.064536434770172, - "y": 5.9536596082772615, - "heading": 0.36323174785688694, - "angularVelocity": 0.19155843929282074, - "velocityX": -1.3076829208520429, - "velocityY": 0.04061442199853097, - "timestamp": 6.812927328762995 - }, - { - "x": 1.9766544759033122, - "y": 5.971066392483775, - "heading": 0.3799915015950661, - "angularVelocity": 0.19176598902380804, - "velocityX": -1.005550023152301, - "velocityY": 0.19916934587659238, - "timestamp": 6.90032423278907 - }, - { - "x": 1.9152634831181174, - "y": 6.002166252794233, - "heading": 0.3967161004530197, - "angularVelocity": 0.19136374502423678, - "velocityX": -0.7024389876199576, - "velocityY": 0.3558462471528812, - "timestamp": 6.9877211368151455 + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.719064094502462, + "velocityX": 0.7324155022029422, + "velocityY": 0.41852376977375944, + "timestamp": 4.424560719205492 }, { - "x": 1.8804243803024292, - "y": 6.046840667724609, - "heading": 0.4133671975206445, - "angularVelocity": 0.19052273365034633, - "velocityX": -0.3986308577394652, - "velocityY": 0.5111670193379727, - "timestamp": 7.075118040841221 - }, - { - "x": 1.870594888269871, - "y": 6.097873553344943, - "heading": 0.42824707526951206, - "angularVelocity": 0.18946968270504025, - "velocityX": -0.12516169608330563, - "velocityY": 0.6498161348632773, - "timestamp": 7.153652387531568 - }, - { - "x": 1.8823139330546126, - "y": 6.159652306802076, - "heading": 0.44299770211925266, - "angularVelocity": 0.18782389452987927, - "velocityX": 0.1492219045374961, - "velocityY": 0.7866463026772258, - "timestamp": 7.232186734221915 - }, - { - "x": 1.9156816491401238, - "y": 6.231973676949003, - "heading": 0.45755311107567953, - "angularVelocity": 0.18533813000084504, - "velocityX": 0.4248805457958017, - "velocityY": 0.9208884162757981, - "timestamp": 7.310721080912262 - }, - { - "x": 1.970847190876043, - "y": 6.314525559081429, - "heading": 0.47181258180189295, - "angularVelocity": 0.18156986499724873, - "velocityX": 0.7024384114816875, - "velocityY": 1.0511564125938428, - "timestamp": 7.389255427602609 - }, - { - "x": 2.048055657917003, - "y": 6.406768140751847, - "heading": 0.4856034696411581, - "angularVelocity": 0.17560326685649172, - "velocityX": 0.9831172002408801, - "velocityY": 1.1745508246744762, - "timestamp": 7.467789774292956 - }, - { - "x": 2.1477789670097485, - "y": 6.507546080988, - "heading": 0.4985623758309784, - "angularVelocity": 0.16500940971618508, - "velocityX": 1.2698050381184776, - "velocityY": 1.2832339541004922, - "timestamp": 7.546324120983303 - }, - { - "x": 2.2711613032626046, - "y": 6.612773705527785, - "heading": 0.5094584510742339, - "angularVelocity": 0.13874280111118417, - "velocityX": 1.5710621078868952, - "velocityY": 1.3398930401075082, - "timestamp": 7.62485846767365 - }, - { - "x": 2.3900797492067456, - "y": 6.694381968039411, - "heading": 0.5145158846551874, - "angularVelocity": 0.06439772907126085, - "velocityX": 1.5142221328080179, - "velocityY": 1.0391410376583359, - "timestamp": 7.703392814363997 - }, - { - "x": 2.4908787774979912, - "y": 6.760136892165139, - "heading": 0.5179138832466764, - "angularVelocity": 0.04326767503251818, - "velocityX": 1.2835024742573689, - "velocityY": 0.8372760059365338, - "timestamp": 7.781927161054344 - }, - { - "x": 2.572148554688276, - "y": 6.811807824685022, - "heading": 0.5203056131504431, - "angularVelocity": 0.030454571847360265, - "velocityX": 1.0348310085359687, - "velocityY": 0.6579405661018212, - "timestamp": 7.860461507744691 - }, - { - "x": 2.6333915518956847, - "y": 6.850110641517779, - "heading": 0.5219437860517318, - "angularVelocity": 0.02085931786951444, - "velocityX": 0.7798243671508998, - "velocityY": 0.48772057637126504, - "timestamp": 7.938995854435038 - }, - { - "x": 2.6743555300979214, - "y": 6.8754307237892345, - "heading": 0.5229620854603486, - "angularVelocity": 0.012966293749560319, - "velocityX": 0.5216058951092302, - "velocityY": 0.3224077532762846, - "timestamp": 8.017530201125384 + "x": 2.4110936768539344, + "y": 6.744072759574757, + "heading": 0.5643107226604884, + "angularVelocity": 0.653664165942785, + "velocityX": 1.0320315606781885, + "velocityY": 0.5897332019890127, + "timestamp": 4.503745575523619 }, { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, - "angularVelocity": 0.006125711359747557, - "velocityX": 0.26145023311667365, - "velocityY": 0.16015757759211213, - "timestamp": 8.09606454781573 + "x": 2.50599401367539, + "y": 6.798301604540289, + "heading": 0.5436072588792712, + "angularVelocity": -0.2614573637418783, + "velocityX": 1.198465732389427, + "velocityY": 0.6848385851414024, + "timestamp": 4.5829304318417465 }, { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, - "angularVelocity": 1.0302971803870337e-32, - "velocityX": -3.572244982752581e-35, - "velocityY": 2.604330852389062e-35, - "timestamp": 8.174598894506078 + "x": 2.577169265515479, + "y": 6.838973237821007, + "heading": 0.528079022912807, + "angularVelocity": -0.19610108155123726, + "velocityX": 0.8988492894921869, + "velocityY": 0.51362893325611, + "timestamp": 4.662115288159874 + }, + { + "x": 2.6246194330616937, + "y": 6.8660876598097635, + "heading": 0.5177266590936774, + "angularVelocity": -0.13073666229232678, + "velocityX": 0.5992328552770498, + "velocityY": 0.3424192863320269, + "timestamp": 4.741300144478001 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06536905835734146, + "velocityX": 0.2996164266200211, + "velocityY": 0.1712096425840104, + "timestamp": 4.820485000796128 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -8.381816905081638e-20, + "velocityX": -7.007327358505968e-19, + "velocityY": 8.954594613208896e-19, + "timestamp": 4.899669857114255 } ], "constraints": [ @@ -32873,13 +21908,26 @@ }, { "scope": [ - 4 + 3 ], "type": "StopPoint" }, { "scope": [ - 1 + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4, + 5 + ], + "type": "StraightLine" + }, + { + "scope": [ + 2 ], "type": "StopPoint" } @@ -32889,7 +21937,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "CenterLanePCBA": { + "CenterLanePCBAFE": { "waypoints": [ { "x": 1.2899744510650635, @@ -32898,7 +21946,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 16 }, { "x": 2.6651716232299805, @@ -32907,7 +21955,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 15 }, { "x": 1.8129411935806274, @@ -32916,16 +21964,16 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 9 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 13 }, { "x": 2.3293724060058594, @@ -32934,7 +21982,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { "x": 2.6483445167541504, @@ -32943,612 +21991,1917 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 + }, + { + "x": 7.984478950500488, + "y": 5.483555793762207, + "heading": 0.7044936776046891, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "x": 8.217175483703613, + "y": 5.698352336883545, + "heading": 0.7044936776046891, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, "controlIntervalCount": 40 } ], "trajectory": [ { - "x": 1.2899744510650635, + "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -5.658132549917657e-18, - "angularVelocity": -1.3627953112859022e-17, - "velocityX": -2.0031548351625804e-16, - "velocityY": -3.76327335107334e-16, + "heading": 0, + "angularVelocity": -8.364664059409844e-30, + "velocityX": -6.773677606947568e-30, + "velocityY": -2.308132540143278e-30, "timestamp": 0 }, { - "x": 1.3043755674296558, - "y": 5.57443357397875, - "heading": -0.00949699856960441, - "angularVelocity": -0.12640481817945412, - "velocityX": 0.19167850581579765, - "velocityY": -0.21989050819926978, - "timestamp": 0.07513161845108944 - }, - { - "x": 1.333181418148157, - "y": 5.541395353902404, - "heading": -0.028483550113125006, - "angularVelocity": -0.2527105356630112, - "velocityX": 0.3834051669803611, - "velocityY": -0.4397378994111986, - "timestamp": 0.15026323690217888 - }, - { - "x": 1.3763968623207659, - "y": 5.491843734526039, - "heading": -0.05694076519946621, - "angularVelocity": -0.3787648352819493, - "velocityX": 0.575196502649018, - "velocityY": -0.6595308393324772, - "timestamp": 0.2253948553532683 - }, - { - "x": 1.4340285202431429, - "y": 5.425784148354235, - "heading": -0.09483975957151482, - "angularVelocity": -0.5044346861470347, - "velocityX": 0.7670759542900343, - "velocityY": -0.8792514727514988, - "timestamp": 0.30052647380435776 - }, - { - "x": 1.506085691225813, - "y": 5.343224221277017, - "heading": -0.14214370390652084, - "angularVelocity": -0.6296143396295131, - "velocityX": 0.9590791795253601, - "velocityY": -1.0988706057618765, - "timestamp": 0.3756580922554472 - }, - { - "x": 1.5925821714791393, - "y": 5.2441754183101335, - "heading": -0.1988101671423879, - "angularVelocity": -0.7542292367162207, - "velocityX": 1.1512660319811814, - "velocityY": -1.3183371396948111, - "timestamp": 0.4507897107065367 - }, - { - "x": 1.6935404980129638, - "y": 5.128656797964584, - "heading": -0.2647926440409603, - "angularVelocity": -0.8782251502241488, - "velocityX": 1.3437528514834798, - "velocityY": -1.537550005315265, - "timestamp": 0.5259213291576261 - }, - { - "x": 1.8090047506628752, - "y": 4.996706206010246, - "heading": -0.340037203461346, - "angularVelocity": -1.0015032415711032, - "velocityX": 1.5368263725127755, - "velocityY": -1.7562591446699427, - "timestamp": 0.6010529476087155 - }, - { - "x": 1.9391049044022293, - "y": 4.848436876634891, - "heading": -0.4244483701055882, - "angularVelocity": -1.1235105590497598, - "velocityX": 1.7316298572388857, - "velocityY": -1.9734611397719521, - "timestamp": 0.6761845660598049 - }, - { - "x": 2.0768333663443204, - "y": 4.7205786690094, - "heading": -0.4949304356074777, - "angularVelocity": -0.9381145641700113, - "velocityX": 1.8331624525981836, - "velocityY": -1.701789609442247, - "timestamp": 0.7513161845108943 - }, - { - "x": 2.2004381051303508, - "y": 4.609474436663161, - "heading": -0.5559710621989046, - "angularVelocity": -0.8124492437076729, - "velocityX": 1.64517604356056, - "velocityY": -1.478794609158427, - "timestamp": 0.8264478029619837 - }, - { - "x": 2.3097905497138385, - "y": 4.515017160876504, - "heading": -0.6076423779450488, - "angularVelocity": -0.6877439460179713, - "velocityX": 1.4554783570583507, - "velocityY": -1.2572240253997846, - "timestamp": 0.9015794214130731 - }, - { - "x": 2.4048475753814116, - "y": 4.437170463185575, - "heading": -0.6499678697321152, - "angularVelocity": -0.5633512582056197, - "velocityX": 1.2652066816824061, - "velocityY": -1.0361376380998342, - "timestamp": 0.9767110398641625 - }, - { - "x": 2.4855875270959107, - "y": 4.375915949453412, - "heading": -0.6829583691226992, - "angularVelocity": -0.43910273822474927, - "velocityX": 1.07464677829478, - "velocityY": -0.8152960763101849, - "timestamp": 1.051842658315252 - }, - { - "x": 2.5519973911226996, - "y": 4.3312425173589775, - "heading": -0.7066196510488878, - "angularVelocity": -0.31493108244769585, - "velocityX": 0.883913662419395, - "velocityY": -0.5946022861403426, - "timestamp": 1.1269742767663415 - }, - { - "x": 2.604068513564512, - "y": 4.303142764963194, - "heading": -0.7209550318751378, - "angularVelocity": -0.1908035673998065, - "velocityX": 0.6930653633843512, - "velocityY": -0.3740070155974622, - "timestamp": 1.202105895217431 - }, - { - "x": 2.6417947649723112, - "y": 4.291611444319209, - "heading": -0.725966473493026, - "angularVelocity": -0.06670216508037836, - "velocityX": 0.5021354815334791, - "velocityY": -0.15348159509808112, - "timestamp": 1.2772375136685206 + "x": 1.3079899492454155, + "y": 5.5701428985236205, + "heading": -0.011966271801548406, + "angularVelocity": -0.16457159783011938, + "velocityX": 0.24776633609486393, + "velocityY": -0.2862183198381729, + "timestamp": 0.07271164623703327 + }, + { + "x": 1.3440211933652915, + "y": 5.528520469164234, + "heading": -0.03588810704483216, + "angularVelocity": -0.328995924069994, + "velocityX": 0.49553607960947427, + "velocityY": -0.5724313987522216, + "timestamp": 0.14542329247406655 + }, + { + "x": 1.398068831056723, + "y": 5.466087418740183, + "heading": -0.07173813692543105, + "angularVelocity": -0.49304384837450815, + "velocityX": 0.7433147299769343, + "velocityY": -0.8586389341691013, + "timestamp": 0.21813493871109982 + }, + { + "x": 1.4701339496615213, + "y": 5.382844295736326, + "heading": -0.11947590052159912, + "angularVelocity": -0.6565353154432494, + "velocityX": 0.9911083345055308, + "velocityY": -1.1448389262999967, + "timestamp": 0.2908465849481331 + }, + { + "x": 1.5602181142824867, + "y": 5.2787919544216955, + "heading": -0.17905308310612672, + "angularVelocity": -0.819362312243441, + "velocityX": 1.2389234638197153, + "velocityY": -1.431027169709696, + "timestamp": 0.3635582311851664 + }, + { + "x": 1.668323429942649, + "y": 5.153931763984036, + "heading": -0.25042016061718175, + "angularVelocity": -0.9815082068514337, + "velocityX": 1.4867675434162453, + "velocityY": -1.717196582745939, + "timestamp": 0.4362698774221997 + }, + { + "x": 1.7944527928954905, + "y": 5.008265979195592, + "heading": -0.33353344500312104, + "angularVelocity": -1.1430532616554276, + "velocityX": 1.7346514550472283, + "velocityY": -2.003334986008902, + "timestamp": 0.508981523659233 + }, + { + "x": 1.938611553409707, + "y": 4.8417992681651185, + "heading": -0.42836015839962444, + "angularVelocity": -1.3041475239812899, + "velocityX": 1.98260894865471, + "velocityY": -2.2894091890060286, + "timestamp": 0.5816931698962663 + }, + { + "x": 2.09250704020051, + "y": 4.700832043022275, + "heading": -0.5059805202146961, + "angularVelocity": -1.0675093444479318, + "velocityX": 2.116517707491784, + "velocityY": -1.9387159062918606, + "timestamp": 0.6544048161332996 + }, + { + "x": 2.228385575629257, + "y": 4.5806756631537535, + "heading": -0.5718968876072178, + "angularVelocity": -0.906544835648882, + "velocityX": 1.8687313857908063, + "velocityY": -1.652505287385456, + "timestamp": 0.7271164623703329 + }, + { + "x": 2.346242849494598, + "y": 4.481326236786965, + "heading": -0.6261128487399352, + "angularVelocity": -0.7456296746751315, + "velocityX": 1.6208857861127495, + "velocityY": -1.3663481918284743, + "timestamp": 0.7998281086073662 + }, + { + "x": 2.446077177803767, + "y": 4.402782122456306, + "heading": -0.6686283268670905, + "angularVelocity": -0.5847134582152551, + "velocityX": 1.3730170265673516, + "velocityY": -1.080213671266156, + "timestamp": 0.8725397548443995 + }, + { + "x": 2.5278876403134056, + "y": 4.3450423723526415, + "heading": -0.6994420265312672, + "angularVelocity": -0.423779425385222, + "velocityX": 1.12513561096113, + "velocityY": -0.7940921858762265, + "timestamp": 0.9452514010814328 + }, + { + "x": 2.591673696422735, + "y": 4.308106404528037, + "heading": -0.718552253950641, + "angularVelocity": -0.2628220980484028, + "velocityX": 0.8772467604995492, + "velocityY": -0.5079787039876672, + "timestamp": 1.017963047318466 + }, + { + "x": 2.637435055651834, + "y": 4.2919738867991235, + "heading": -0.7259572155509334, + "angularVelocity": -0.10184010376417504, + "velocityX": 0.6293539150825374, + "velocityY": -0.22186979060047113, + "timestamp": 1.0906746935554992 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.057384074600868515, - "velocityX": 0.3111454104283852, - "velocityY": 0.06699234541879971, - "timestamp": 1.35236913211961 - }, - { - "x": 2.6741706929303724, - "y": 4.3184165536581745, - "heading": -0.7079150841905613, - "angularVelocity": 0.18203686942568406, - "velocityX": 0.11922553065022659, - "velocityY": 0.28844784676770313, - "timestamp": 1.4278485168598247 - }, - { - "x": 2.66867899794669, - "y": 4.356899858459873, - "heading": -0.684771836918116, - "angularVelocity": 0.30661679809476405, - "velocityX": -0.0727575483579021, - "velocityY": 0.5098518613032106, - "timestamp": 1.5033279016000394 - }, - { - "x": 2.6486902203739557, - "y": 4.412089436366014, - "heading": -0.6522316135776107, - "angularVelocity": 0.43111405122449203, - "velocityX": -0.2648243310823653, - "velocityY": 0.7311874373940866, - "timestamp": 1.578807286340254 - }, - { - "x": 2.6141955298766604, - "y": 4.483978019553037, - "heading": -0.6103014306892406, - "angularVelocity": 0.5555183449290747, - "velocityX": -0.4570081038377013, - "velocityY": 0.9524267246290669, - "timestamp": 1.6542866710804687 - }, - { - "x": 2.565181655781626, - "y": 4.572554576780097, - "heading": -0.5589890537241899, - "angularVelocity": 0.679819756618359, - "velocityX": -0.649367695106772, - "velocityY": 1.1735198628071373, - "timestamp": 1.7297660558206833 - }, - { - "x": 2.5016263830570638, - "y": 4.677800410068183, - "heading": -0.49830336002197995, - "angularVelocity": 0.8040035555369035, - "velocityX": -0.842021605572691, - "velocityY": 1.3943652779921414, - "timestamp": 1.805245440560898 - }, - { - "x": 2.423485048210339, - "y": 4.799677415452472, - "heading": -0.42825793280914715, - "angularVelocity": 0.9280073950309187, - "velocityX": -1.0352672470523336, - "velocityY": 1.6147058670562153, - "timestamp": 1.8807248253011126 - }, - { - "x": 2.330623716021553, - "y": 4.938069451098365, - "heading": -0.3489003970727596, - "angularVelocity": 1.0513802677046555, - "velocityX": -1.2302873494790556, - "velocityY": 1.8335077335201706, - "timestamp": 1.9562042100413273 - }, - { - "x": 2.2158437334233922, - "y": 5.072717873356153, - "heading": -0.27145428242562347, - "angularVelocity": 1.026056517505551, - "velocityX": -1.5206798914447361, - "velocityY": 1.7839099076626914, - "timestamp": 2.031683594781542 - }, - { - "x": 2.115226856826334, - "y": 5.190380914478301, - "heading": -0.20366940333084413, - "angularVelocity": 0.8980581827763012, - "velocityX": -1.333037847880913, - "velocityY": 1.5588765267548788, - "timestamp": 2.1071629795217564 - }, - { - "x": 2.028914050823854, - "y": 5.291175476770991, - "heading": -0.1455212111518032, - "angularVelocity": 0.7703850843618669, - "velocityX": -1.1435282136547777, - "velocityY": 1.3353919436695292, - "timestamp": 2.182642364261971 - }, - { - "x": 1.95695123959882, - "y": 5.375140495871242, - "heading": -0.09702855670668924, - "angularVelocity": 0.6424622380441017, - "velocityX": -0.9534101459703443, - "velocityY": 1.1124232052394878, - "timestamp": 2.258121749002185 - }, - { - "x": 1.89936105960217, - "y": 5.44229547054464, - "heading": -0.05821541752658763, - "angularVelocity": 0.5142217217941343, - "velocityX": -0.7629921758539479, - "velocityY": 0.8897127991440635, - "timestamp": 2.3336011337423996 - }, - { - "x": 1.8561570206808413, - "y": 5.492652117133406, - "heading": -0.029101140919542783, - "angularVelocity": 0.38572487981921, - "velocityX": -0.572395218494233, - "velocityY": 0.6671576187743987, - "timestamp": 2.409080518482614 - }, - { - "x": 1.8273481914953718, - "y": 5.526218220226538, - "heading": -0.009696189627478797, - "angularVelocity": 0.2570894206319378, - "velocityX": -0.38167811361857223, - "velocityY": 0.4447055737053434, - "timestamp": 2.4845599032228285 - }, - { - "x": 1.8129411935806279, + "angularVelocity": 0.05916659407406203, + "velocityX": 0.38145976628941564, + "velocityY": 0.06423731409121712, + "timestamp": 1.1633863397925324 + }, + { + "x": 2.6747076115982096, + "y": 4.323065260999168, + "heading": -0.7050775877835139, + "angularVelocity": 0.2234613634683759, + "velocityX": 0.12854299211755085, + "velocityY": 0.35614342430781004, + "timestamp": 1.237571545918339 + }, + { + "x": 2.6654811031724366, + "y": 4.37114137531458, + "heading": -0.6763165724415564, + "angularVelocity": 0.38769205939503093, + "velocityX": -0.1243712717061632, + "velocityY": 0.6480552770879449, + "timestamp": 1.3117567520441455 + }, + { + "x": 2.6374924128586628, + "y": 4.440873637695316, + "heading": -0.6353766207501428, + "angularVelocity": 0.5518613996935385, + "velocityX": -0.3772812906679928, + "velocityY": 0.9399753133925751, + "timestamp": 1.3859419581699521 + }, + { + "x": 2.590742000006679, + "y": 4.532262849422754, + "heading": -0.5822609370446827, + "angularVelocity": 0.7159875462663353, + "velocityX": -0.6301851176982644, + "velocityY": 1.231906150790299, + "timestamp": 1.4601271642957587 + }, + { + "x": 2.525230488767617, + "y": 4.645309988591984, + "heading": -0.5169687243847326, + "angularVelocity": 0.8801244354174206, + "velocityX": -0.8830805313123746, + "velocityY": 1.5238501726898892, + "timestamp": 1.5343123704215653 + }, + { + "x": 2.4409586345992826, + "y": 4.780016081558229, + "heading": -0.4394900359726193, + "angularVelocity": 1.0443954051766633, + "velocityX": -1.1359657615273402, + "velocityY": 1.8158080296257462, + "timestamp": 1.608497576547372 + }, + { + "x": 2.337926784653071, + "y": 4.936381539754754, + "heading": -0.3497973159677186, + "angularVelocity": 1.2090378215045976, + "velocityX": -1.3888463122287895, + "velocityY": 2.107771432581833, + "timestamp": 1.6826827826731785 + }, + { + "x": 2.2066781223966365, + "y": 5.088037063238321, + "heading": -0.26257787440108193, + "angularVelocity": 1.1756985809634768, + "velocityX": -1.7692026362756765, + "velocityY": 2.044282565348452, + "timestamp": 1.756867988798985 + }, + { + "x": 2.094180452686652, + "y": 5.218026595083209, + "heading": -0.18768356705291486, + "angularVelocity": 1.009558526050363, + "velocityX": -1.5164434471606913, + "velocityY": 1.7522298398549971, + "timestamp": 1.8310531949247917 + }, + { + "x": 2.0004335399095625, + "y": 5.326350877049268, + "heading": -0.12517882612598913, + "angularVelocity": 0.8425499394477148, + "velocityX": -1.2636874340540831, + "velocityY": 1.460187113126797, + "timestamp": 1.9052384010505983 + }, + { + "x": 1.9254366143080366, + "y": 5.4130101547737155, + "heading": -0.07512185913342963, + "angularVelocity": 0.6747567285776663, + "velocityX": -1.0109417971010437, + "velocityY": 1.1681476975361738, + "timestamp": 1.9794236071764049 + }, + { + "x": 1.869189070291752, + "y": 5.478004602849777, + "heading": -0.037557912625022415, + "angularVelocity": 0.5063536043287745, + "velocityX": -0.7582043233686032, + "velocityY": 0.876110635424555, + "timestamp": 2.0536088133022115 + }, + { + "x": 1.8316905948823166, + "y": 5.5213343174096465, + "heading": -0.012514751718529437, + "angularVelocity": 0.3375762124977515, + "velocityX": -0.5054710685139713, + "velocityY": 0.5840748691542834, + "timestamp": 2.127794019428018 + }, + { + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.24734731593596e-17, - "angularVelocity": 0.12846142905206553, - "velocityX": -0.19087328234026615, - "velocityY": 0.22232623397342577, - "timestamp": 2.560039287963043 + "heading": 2.368719711095728e-29, + "angularVelocity": 0.1686960564308085, + "velocityX": -0.2527377395078362, + "velocityY": 0.2920386866937519, + "timestamp": 2.2019792255538246 }, { - "x": 1.8129411935806274, + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 5.939064303009515e-18, - "angularVelocity": -5.9066042058701954e-18, - "velocityX": -3.4367552729151325e-16, - "velocityY": -1.697930821204867e-16, - "timestamp": 2.6355186727032573 - }, - { - "x": 1.8467557558007979, - "y": 5.543052711491932, - "heading": 5.160367277754293e-18, - "angularVelocity": -8.367176611695404e-18, - "velocityX": 0.36334081801614093, - "velocityY": 0.0005742601437371532, - "timestamp": 2.7285843579868416 - }, - { - "x": 1.914384879084952, - "y": 5.543159599317722, - "heading": 4.087331547357737e-18, - "angularVelocity": -1.1529875132039326e-17, - "velocityX": 0.7266816236089447, - "velocityY": 0.0011485202678393933, - "timestamp": 2.821650043270426 - }, - { - "x": 2.0158285613519547, - "y": 5.543319931052202, - "heading": 2.611621451738615e-18, - "angularVelocity": -1.5856651043003625e-17, - "velocityX": 1.0900224068397422, - "velocityY": 0.0017227803565984868, - "timestamp": 2.91471572855401 - }, - { - "x": 2.1510867977458235, - "y": 5.543533706687698, - "heading": 4.496026882117787e-19, - "angularVelocity": -2.3231105610401644e-17, - "velocityX": 1.4533631378925314, - "velocityY": 0.0022970403628902463, - "timestamp": 3.0077814138375945 - }, - { - "x": 2.32015956398666, - "y": 5.543800926185837, - "heading": -3.8747303229957764e-18, - "angularVelocity": -4.646538622728347e-17, - "velocityX": 1.8167036080554047, - "velocityY": 0.00287129995684554, - "timestamp": 3.1008470991211787 - }, - { - "x": 2.4554178003805287, - "y": 5.544014701821333, - "heading": -2.0140989785604273e-18, - "angularVelocity": 1.9992667960964244e-17, - "velocityX": 1.4533631378925314, - "velocityY": 0.002297040362890341, - "timestamp": 3.193912784404763 - }, - { - "x": 2.556861482647531, - "y": 5.544175033555813, - "heading": -9.161136142082278e-19, - "angularVelocity": 1.1797961418381928e-17, - "velocityX": 1.0900224068397426, - "velocityY": 0.001722780356598613, - "timestamp": 3.2869784696883473 - }, - { - "x": 2.6244906059316855, - "y": 5.544281921381602, - "heading": -2.8482092694126203e-19, - "angularVelocity": 6.783302409926777e-18, - "velocityX": 0.7266816236089451, - "velocityY": 0.0011485202678395388, - "timestamp": 3.3800441549719316 + "heading": 1.1956060311838367e-29, + "angularVelocity": -1.025164181333131e-28, + "velocityX": 1.21885672142624e-30, + "velocityY": -5.1343833235782786e-30, + "timestamp": 2.2761644316796312 + }, + { + "x": 1.8602093956334294, + "y": 5.5430660724686875, + "heading": -1.6999094202020734e-20, + "angularVelocity": -1.7890242544799916e-19, + "velocityX": 0.49746156397034486, + "velocityY": 0.0007030702226192673, + "timestamp": 2.3711832340882935 + }, + { + "x": 1.9547457978922331, + "y": 5.543199682247202, + "heading": -3.210902716840507e-20, + "angularVelocity": -1.5902045262092918e-19, + "velocityX": 0.9949231085045297, + "velocityY": 0.001406140417769105, + "timestamp": 2.4662020364969557 + }, + { + "x": 2.0965503966634373, + "y": 5.543400096908449, + "heading": -4.532979164623425e-20, + "angularVelocity": -1.3913840358532023e-19, + "velocityX": 1.492384614166397, + "velocityY": 0.0021092105579800877, + "timestamp": 2.561220838905618 + }, + { + "x": 2.2856231808662413, + "y": 5.543667316436768, + "heading": -7.554638691710094e-20, + "angularVelocity": -3.1800648403053284e-19, + "velocityX": 1.9898460032113345, + "velocityY": 0.0028122805333745363, + "timestamp": 2.65623964131428 + }, + { + "x": 2.474695965069045, + "y": 5.543934535965086, + "heading": -6.169486717931722e-20, + "angularVelocity": 1.457766188026957e-19, + "velocityX": 1.9898460032113345, + "velocityY": 0.002812280533374536, + "timestamp": 2.7512584437229424 + }, + { + "x": 2.6165005638402494, + "y": 5.544134950626333, + "heading": -3.084743805748469e-20, + "angularVelocity": 3.2464552635862006e-19, + "velocityX": 1.492384614166397, + "velocityY": 0.0021092105579800877, + "timestamp": 2.8462772461316046 + }, + { + "x": 2.711036966099053, + "y": 5.544268560404848, + "heading": -1.8891361886639493e-21, + "angularVelocity": 3.047639112965043e-19, + "velocityX": 0.9949231085045297, + "velocityY": 0.001406140417769105, + "timestamp": 2.941296048540267 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -1.8432742635406086e-23, - "angularVelocity": 3.0602309898573723e-18, - "velocityX": 0.36334081801614126, - "velocityY": 0.0005742601437373122, - "timestamp": 3.473109840255516 + "heading": -8.137968740525874e-34, + "angularVelocity": 1.9881709101649526e-20, + "velocityX": 0.49746156397034486, + "velocityY": 0.0007030702226192673, + "timestamp": 3.036314850948929 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -1.9267281699534778e-23, - "angularVelocity": -8.967222433260206e-24, - "velocityX": 3.3495247448012114e-21, - "velocityY": 4.428006153501746e-23, - "timestamp": 3.5661755255391 - }, - { - "x": 2.6433995634924674, - "y": 5.563703580385797, - "heading": 0.004845900450156499, - "angularVelocity": 0.06121219104231838, - "velocityX": -0.18828383484070044, - "velocityY": 0.24465440313021314, - "timestamp": 3.6453411356510848 - }, - { - "x": 2.613808012454941, - "y": 5.602604430475708, - "heading": 0.01472246361068666, - "angularVelocity": 0.12475825230878844, - "velocityX": -0.3737930017297544, - "velocityY": 0.4913857170415789, - "timestamp": 3.7245067457630694 - }, - { - "x": 2.5698581719130473, - "y": 5.661275942603098, - "heading": 0.02990473352989552, - "angularVelocity": 0.19177860055310114, - "velocityX": -0.5551632897128446, - "velocityY": 0.7411237284017032, - "timestamp": 3.803672355875054 - }, - { - "x": 2.5120904369033115, - "y": 5.740092575087619, - "heading": 0.05083960434186745, - "angularVelocity": 0.26444400267184504, - "velocityX": -0.7297074440280278, - "velocityY": 0.995591802716233, - "timestamp": 3.8828379659870387 - }, - { - "x": 2.441559614817569, - "y": 5.839724514645118, - "heading": 0.07837099812745103, - "angularVelocity": 0.3477696154509352, - "velocityX": -0.8909275376766786, - "velocityY": 1.2585255064233427, - "timestamp": 3.9620035760990233 - }, - { - "x": 2.3611631890754614, - "y": 5.961650149114452, - "heading": 0.11464704720334498, - "angularVelocity": 0.458229893315787, - "velocityX": -1.0155473522958047, - "velocityY": 1.5401338320624665, - "timestamp": 4.041169186211008 - }, - { - "x": 2.2902542502823713, - "y": 6.1057913956854275, - "heading": 0.16704371336362323, - "angularVelocity": 0.6618614583549582, - "velocityX": -0.8957038124608021, - "velocityY": 1.8207558353567825, - "timestamp": 4.120334796322993 - }, - { - "x": 2.241416295543653, - "y": 6.2393929892272615, - "heading": 0.22223140963064517, - "angularVelocity": 0.6971170460122206, - "velocityX": -0.6169087141453696, - "velocityY": 1.6876215992379315, - "timestamp": 4.199500406434978 - }, - { - "x": 2.211878843837597, - "y": 6.357956546172331, - "heading": 0.276701736402732, - "angularVelocity": 0.6880554156664132, - "velocityX": -0.3731096326330706, - "velocityY": 1.497664917599366, - "timestamp": 4.278666016546962 - }, - { - "x": 2.200573473010042, - "y": 6.460208388316336, - "heading": 0.32930652128307925, - "angularVelocity": 0.6644903614831552, - "velocityX": -0.14280659002770119, - "velocityY": 1.291619454449523, - "timestamp": 4.357831626658947 - }, - { - "x": 2.206951813042022, - "y": 6.545559671097223, - "heading": 0.3794800427949416, - "angularVelocity": 0.6337792564333018, - "velocityX": 0.08056958094502174, - "velocityY": 1.0781358554573408, - "timestamp": 4.436997236770932 - }, - { - "x": 2.230681954112334, - "y": 6.613672661059127, - "heading": 0.42688445047126194, - "angularVelocity": 0.598800509580661, - "velocityX": 0.2997531508535616, - "velocityY": 0.8603860927182075, - "timestamp": 4.516162846882916 - }, - { - "x": 2.271541794524346, - "y": 6.6643287076043505, - "heading": 0.47129474601819377, - "angularVelocity": 0.5609796410854497, - "velocityX": 0.5161311882042349, - "velocityY": 0.6398743908316677, - "timestamp": 4.595328456994901 + "heading": -8.137968740525874e-34, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -9.422453604168856e-36, + "timestamp": 3.1313336533575913 + }, + { + "x": 2.744670053824415, + "y": 5.560420833392183, + "heading": 0.008590361313936763, + "angularVelocity": 0.13502733079351698, + "velocityX": -0.21432312628245512, + "velocityY": 0.2528389368381776, + "timestamp": 3.1949530796133994 + }, + { + "x": 2.717653230778774, + "y": 5.592802243922747, + "heading": 0.025816975580253556, + "angularVelocity": 0.27077600789812356, + "velocityX": -0.4246631042695725, + "velocityY": 0.5089862080862002, + "timestamp": 3.2585725058692074 + }, + { + "x": 2.6776153836811907, + "y": 5.641767759246714, + "heading": 0.051750317714318324, + "angularVelocity": 0.4076324427980389, + "velocityX": -0.629333671394567, + "velocityY": 0.7696629505440931, + "timestamp": 3.3221919321250155 + }, + { + "x": 2.6251092372323575, + "y": 5.707734732807458, + "heading": 0.0864931852991208, + "angularVelocity": 0.5461046983527398, + "velocityX": -0.8253162522672026, + "velocityY": 1.0368998502988076, + "timestamp": 3.3858113583808236 + }, + { + "x": 2.5610825168108655, + "y": 5.791356317104455, + "heading": 0.1301885817245676, + "angularVelocity": 0.686824748304888, + "velocityX": -1.006402040849065, + "velocityY": 1.314403307580339, + "timestamp": 3.4494307846366317 + }, + { + "x": 2.48749773492398, + "y": 5.89376509224382, + "heading": 0.18299253621322714, + "angularVelocity": 0.8299973388055953, + "velocityX": -1.1566401367878931, + "velocityY": 1.6097091905165586, + "timestamp": 3.5130502108924397 + }, + { + "x": 2.410156329475762, + "y": 6.016926517894053, + "heading": 0.24447965431113647, + "angularVelocity": 0.9664833796311684, + "velocityX": -1.215688508997792, + "velocityY": 1.9359090909592849, + "timestamp": 3.576669637148248 + }, + { + "x": 2.3496693828620137, + "y": 6.152928215304285, + "heading": 0.3070071930810449, + "angularVelocity": 0.9828372000478389, + "velocityX": -0.9507622148388409, + "velocityY": 2.137738508728141, + "timestamp": 3.640289063404056 + }, + { + "x": 2.309701308910896, + "y": 6.283843034176789, + "heading": 0.36362933631377437, + "angularVelocity": 0.8900134214517557, + "velocityX": -0.6282369443322133, + "velocityY": 2.0577805644789557, + "timestamp": 3.703908489659864 + }, + { + "x": 2.288494901023514, + "y": 6.40507103844379, + "heading": 0.41290495871744193, + "angularVelocity": 0.7745373591634508, + "velocityX": -0.3333322718459105, + "velocityY": 1.905518666256975, + "timestamp": 3.767527915915672 + }, + { + "x": 2.285060268530139, + "y": 6.514915678699493, + "heading": 0.45431002431284384, + "angularVelocity": 0.6508242534114624, + "velocityX": -0.05398716548566081, + "velocityY": 1.7265896082436825, + "timestamp": 3.83114734217148 + }, + { + "x": 2.2988127872129502, + "y": 6.612520892931977, + "heading": 0.48757772897770557, + "angularVelocity": 0.522917395248037, + "velocityX": 0.2161685430408261, + "velocityY": 1.5342045657567287, + "timestamp": 3.894766768427288 }, { "x": 2.3293724060058594, "y": 6.6973748207092285, "heading": 0.5125504196, - "angularVelocity": 0.521131252869112, - "velocityX": 0.7305016837451085, - "velocityY": 0.4174301575915678, - "timestamp": 4.6744940671068855 - }, - { - "x": 2.3973241454559178, - "y": 6.736204444156279, - "heading": 0.5391342040901619, - "angularVelocity": 0.37821212966035084, - "velocityX": 0.9667612262295852, - "velocityY": 0.5524358122619253, - "timestamp": 4.744782094394734 + "angularVelocity": 0.3925324714794108, + "velocityX": 0.48035043054352317, + "velocityY": 1.3337738607711032, + "timestamp": 3.9583861946830963 }, { - "x": 2.48097335716082, - "y": 6.784004065158346, - "heading": 0.5372874666531787, - "angularVelocity": -0.026273854996956482, - "velocityX": 1.1900918966232585, - "velocityY": 0.6800535289789073, - "timestamp": 4.815070121682582 + "x": 2.3906193814869496, + "y": 6.780920533993851, + "heading": 0.5304788719493444, + "angularVelocity": 0.23275934723222522, + "velocityX": 0.7951498408867412, + "velocityY": 1.0846472026287595, + "timestamp": 4.035411897678633 }, { - "x": 2.54793079875253, - "y": 6.822265517551258, - "heading": 0.5291862310389086, - "angularVelocity": -0.11525768935146737, - "velocityX": 0.952615177510983, - "velocityY": 0.5443523437671908, - "timestamp": 4.88535814897043 + "x": 2.471491792404757, + "y": 6.8405967325057775, + "heading": 0.5351323155604377, + "angularVelocity": 0.06041416605263601, + "velocityX": 1.0499405753231936, + "velocityY": 0.7747569472411664, + "timestamp": 4.112437600674169 }, { - "x": 2.598141729775772, - "y": 6.850957521028096, - "heading": 0.5214396908605601, - "angularVelocity": -0.11021137563904597, - "velocityX": 0.7143596564122447, - "velocityY": 0.40820612818364127, - "timestamp": 4.955646176258278 + "x": 2.556148280048404, + "y": 6.869576729136414, + "heading": 0.5277899567279947, + "angularVelocity": -0.09532349004160981, + "velocityX": 1.0990680299088356, + "velocityY": 0.37623800242778493, + "timestamp": 4.189463303669705 }, { - "x": 2.631611382674506, - "y": 6.870083065561309, - "heading": 0.5156519889527617, - "angularVelocity": -0.08234264256835963, - "velocityX": 0.4761785782046116, - "velocityY": 0.2721024514586011, - "timestamp": 5.025934203546126 + "x": 2.6173499244947225, + "y": 6.878199709364964, + "heading": 0.518346888239368, + "angularVelocity": -0.12259632981439711, + "velocityX": 0.794561322599876, + "velocityY": 0.11194938693450328, + "timestamp": 4.266489006665242 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.044126567104521564, - "velocityX": 0.23806521146364917, - "velocityY": 0.13603746705807154, - "timestamp": 5.096222230833974 + "angularVelocity": -0.0752536934288536, + "velocityX": 0.4023928513995403, + "velocityY": 0.0187620669061233, + "timestamp": 4.343514709660778 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -7.689800874803388e-26, - "velocityX": 2.07998453448468e-19, - "velocityY": -3.6391739324491333e-19, - "timestamp": 5.166510258121822 + "angularVelocity": -4.4995568742972564e-34, + "velocityX": 2.8230892463303505e-33, + "velocityY": 2.0807279166194578e-33, + "timestamp": 4.420540412656314 + }, + { + "x": 2.658706958910465, + "y": 6.86593478207681, + "heading": 0.5043163629464208, + "angularVelocity": -0.14325761626715308, + "velocityX": 0.1802876546124509, + "velocityY": -0.23853061812890558, + "timestamp": 4.4780176824038715 + }, + { + "x": 2.679434758469343, + "y": 6.838509950906704, + "heading": 0.48805940847309726, + "angularVelocity": -0.2828414527120879, + "velocityX": 0.36062602920974224, + "velocityY": -0.4771422040496659, + "timestamp": 4.535494952151429 + }, + { + "x": 2.7105313297582017, + "y": 6.797364836772264, + "heading": 0.464037646160856, + "angularVelocity": -0.4179349926979131, + "velocityX": 0.5410238068968174, + "velocityY": -0.7158501841710775, + "timestamp": 4.592972221898986 + }, + { + "x": 2.7520007950829926, + "y": 6.742492873007072, + "heading": 0.4325704761896421, + "angularVelocity": -0.5474715502218408, + "velocityX": 0.721493305213118, + "velocityY": -0.954672412350015, + "timestamp": 4.650449491646543 + }, + { + "x": 2.803848241216662, + "y": 6.673886273182379, + "heading": 0.39405966194845793, + "angularVelocity": -0.6700181551824853, + "velocityX": 0.9020513041309295, + "velocityY": -1.1936301102334148, + "timestamp": 4.7079267613941 + }, + { + "x": 2.866080023237396, + "y": 6.591535755483362, + "heading": 0.3490218715149645, + "angularVelocity": -0.7835756748937736, + "velocityX": 1.0827198698556684, + "velocityY": -1.4327492948204654, + "timestamp": 4.7654040311416574 + }, + { + "x": 2.938704098816793, + "y": 6.495430149531881, + "heading": 0.29814315673038183, + "angularVelocity": -0.8851971398092591, + "velocityX": 1.2635268845991694, + "velocityY": -1.6720628236793682, + "timestamp": 4.822881300889215 + }, + { + "x": 3.0217302964606527, + "y": 6.385555893825644, + "heading": 0.2423799369377359, + "angularVelocity": -0.9701786469253116, + "velocityX": 1.4445048974753851, + "velocityY": -1.9116122980233827, + "timestamp": 4.880358570636772 + }, + { + "x": 3.1151700095083186, + "y": 6.261896797084101, + "heading": 0.1831716018579807, + "angularVelocity": -1.0301173897055516, + "velocityX": 1.6256811337430896, + "velocityY": -2.151443471213232, + "timestamp": 4.937835840384329 + }, + { + "x": 3.2190322842248182, + "y": 6.124436959606287, + "heading": 0.12297302537717429, + "angularVelocity": -1.0473457898261622, + "velocityX": 1.8070147585761873, + "velocityY": -2.39155127029422, + "timestamp": 4.995313110131886 + }, + { + "x": 3.3332908123673026, + "y": 5.973192452697109, + "heading": 0.06701518728601086, + "angularVelocity": -0.9735646515036827, + "velocityX": 1.9878906678120518, + "velocityY": -2.6313794578874745, + "timestamp": 5.052790379879443 + }, + { + "x": 3.457219648102922, + "y": 5.80867115561654, + "heading": 0.033419490428721374, + "angularVelocity": -0.5845040483802287, + "velocityX": 2.1561364393249893, + "velocityY": -2.8623714696810656, + "timestamp": 5.1102676496270005 + }, + { + "x": 3.588371868131788, + "y": 5.635317112607293, + "heading": 0.03341946780631197, + "angularVelocity": -3.935887960456646e-7, + "velocityX": 2.2818101939234974, + "velocityY": -3.0160451909185437, + "timestamp": 5.167744919374558 + }, + { + "x": 3.719524526285539, + "y": 5.461963401064903, + "heading": 0.033419445184933325, + "angularVelocity": -3.9357086276096175e-7, + "velocityX": 2.281817816499982, + "velocityY": -3.0160394239978356, + "timestamp": 5.225222189122115 + }, + { + "x": 3.8561567314585825, + "y": 5.292894866069507, + "heading": 0.0334194225694706, + "angularVelocity": -3.9346793646231113e-7, + "velocityX": 2.3771519728257604, + "velocityY": -2.9414851425259596, + "timestamp": 5.282699458869672 + }, + { + "x": 4.00579710900262, + "y": 5.135223415824853, + "heading": 0.03341939963642931, + "angularVelocity": -3.9899322628139097e-7, + "velocityX": 2.6034705232392743, + "velocityY": -2.743196587749476, + "timestamp": 5.340176728617229 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.03341937596859772, + "angularVelocity": -4.117772415638071e-7, + "velocityX": 2.813320067337111, + "velocityY": -2.527531676347718, + "timestamp": 5.397653998364786 + }, + { + "x": 4.277933740289957, + "y": 4.900652577427371, + "heading": 0.03341935253431191, + "angularVelocity": -6.240523322284166e-7, + "velocityX": 2.940862673655656, + "velocityY": -2.37792138300258, + "timestamp": 5.435205794264698 + }, + { + "x": 4.3928594316694705, + "y": 4.81721673895127, + "heading": 0.033419330076064284, + "angularVelocity": -5.980605477839765e-7, + "velocityX": 3.060457925523134, + "velocityY": -2.2218867693700792, + "timestamp": 5.47275759016461 + }, + { + "x": 4.5119655190903245, + "y": 4.739865631807423, + "heading": 0.03341930829497473, + "angularVelocity": -5.800279064172658e-7, + "velocityX": 3.1717813906506853, + "velocityY": -2.0598510747665353, + "timestamp": 5.510309386064522 + }, + { + "x": 4.634929904279705, + "y": 4.6688078109694935, + "heading": 0.033419286924895195, + "angularVelocity": -5.690827567994812e-7, + "velocityX": 3.2745274158690463, + "velocityY": -1.8922615852334157, + "timestamp": 5.547861181964434 + }, + { + "x": 4.761195179582567, + "y": 4.6037960104372555, + "heading": 0.03341926571852252, + "angularVelocity": -5.647232617468814e-7, + "velocityX": 3.362429739429795, + "velocityY": -1.7312567608088656, + "timestamp": 5.585412977864346 + }, + { + "x": 4.8874652266060234, + "y": 4.538793478329322, + "heading": 0.03341924451230877, + "angularVelocity": -5.647190300189245e-7, + "velocityX": 3.3625568097996403, + "velocityY": -1.7310099437370914, + "timestamp": 5.622964773764258 + }, + { + "x": 5.0137352745443575, + "y": 4.473790947998577, + "heading": 0.03341922330609504, + "angularVelocity": -5.647190293196981e-7, + "velocityX": 3.3625568341627456, + "velocityY": -1.731009896410775, + "timestamp": 5.6605165696641695 + }, + { + "x": 5.140005505333594, + "y": 4.408788772864344, + "heading": 0.03341920209988365, + "angularVelocity": -5.64718967252867e-7, + "velocityX": 3.362561703461214, + "velocityY": -1.7310004375685482, + "timestamp": 5.698068365564081 + }, + { + "x": 5.267176463182282, + "y": 4.34556692987905, + "heading": 0.033419180889344834, + "angularVelocity": -5.648342058496427e-7, + "velocityX": 3.3865479613183083, + "velocityY": -1.6835903974819586, + "timestamp": 5.735620161463993 + }, + { + "x": 5.39745989861742, + "y": 4.289036580908955, + "heading": 0.03341915955151481, + "angularVelocity": -5.682239561025287e-7, + "velocityX": 3.4694328809835504, + "velocityY": -1.5053966825119904, + "timestamp": 5.773171957363905 + }, + { + "x": 5.530504784906373, + "y": 4.239352458442723, + "heading": 0.03341913783750815, + "angularVelocity": -5.782414966353427e-7, + "velocityX": 3.542969999186219, + "velocityY": -1.323082459189313, + "timestamp": 5.810723753263817 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.033419115463141504, + "angularVelocity": -5.958268068735077e-7, + "velocityX": 3.6069365171092738, + "velocityY": -1.1371861948253605, + "timestamp": 5.848275549163729 + }, + { + "x": 5.808220957669321, + "y": 4.160073254115033, + "heading": 0.03341909344161441, + "angularVelocity": -5.669632847998976e-7, + "velocityX": 3.6628444951256323, + "velocityY": -0.941676170142482, + "timestamp": 5.887116736398908 + }, + { + "x": 5.952250404538064, + "y": 4.131196918833519, + "heading": 0.033419072126499465, + "angularVelocity": -5.487760919877281e-7, + "velocityX": 3.708162832321777, + "velocityY": -0.7434462573626127, + "timestamp": 5.925957923634087 + }, + { + "x": 6.09762364792411, + "y": 4.110103210641541, + "heading": 0.03341905123842176, + "angularVelocity": -5.377816485869475e-7, + "velocityX": 3.7427600373239653, + "velocityY": -0.5430757835556856, + "timestamp": 5.964799110869266 + }, + { + "x": 6.24392007770515, + "y": 4.096848811559285, + "heading": 0.0334190305197359, + "angularVelocity": -5.334205087871439e-7, + "velocityX": 3.7665282704987906, + "velocityY": -0.3412459820551187, + "timestamp": 6.003640298104445 + }, + { + "x": 6.390314919287249, + "y": 4.0847296192319185, + "heading": 0.03341900981240726, + "angularVelocity": -5.331281075500129e-7, + "velocityX": 3.7690619675370143, + "velocityY": -0.31201910111516795, + "timestamp": 6.042481485339624 + }, + { + "x": 6.536709800200056, + "y": 4.07261090201266, + "heading": 0.033418989105078956, + "angularVelocity": -5.331280988936517e-7, + "velocityX": 3.769062980140168, + "velocityY": -0.31200686904551156, + "timestamp": 6.081322672574803 + }, + { + "x": 6.68323027882942, + "y": 4.062119297351164, + "heading": 0.03341896797261103, + "angularVelocity": -5.440736864773728e-7, + "velocityX": 3.772296602114613, + "velocityY": -0.27011544724343456, + "timestamp": 6.120163859809982 + }, + { + "x": 6.828478445747191, + "y": 4.059283961769526, + "heading": 0.02725441210059553, + "angularVelocity": -0.15871182913873783, + "velocityX": 3.739539835337918, + "velocityY": -0.07299816981572388, + "timestamp": 6.159005047045161 + }, + { + "x": 6.965940131033012, + "y": 4.0573223410737915, + "heading": 0.017080975304275284, + "angularVelocity": -0.2619239400361572, + "velocityX": 3.5390701229986914, + "velocityY": -0.05050362348240824, + "timestamp": 6.19784623428034 + }, + { + "x": 7.095541107200079, + "y": 4.056112825219225, + "heading": 0.007563330070755733, + "angularVelocity": -0.2450400183673897, + "velocityX": 3.336689359734222, + "velocityY": -0.03114003305929124, + "timestamp": 6.236687421515519 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, + "angularVelocity": -0.194724482157576, + "velocityX": 3.1345655629883886, + "velocityY": -0.012732337027157752, + "timestamp": 6.275528608750698 + }, + { + "x": 7.3857970557158446, + "y": 4.056503208949951, + "heading": -0.005539516561442828, + "angularVelocity": -0.09285314353970942, + "velocityX": 2.8244854636552765, + "velocityY": 0.014833039029661928, + "timestamp": 6.335187510434235 + }, + { + "x": 7.5357095386456, + "y": 4.058265721996549, + "heading": -0.007640794437617486, + "angularVelocity": -0.03522153135371121, + "velocityX": 2.5128267316246156, + "velocityY": 0.029543169533142722, + "timestamp": 6.394846412117771 + }, + { + "x": 7.667000937583321, + "y": 4.060490426598279, + "heading": -0.007708640614146542, + "angularVelocity": -0.0011372347564986972, + "velocityX": 2.200700905192029, + "velocityY": 0.037290404934565556, + "timestamp": 6.454505313801308 + }, + { + "x": 7.779661923967275, + "y": 4.062917456014214, + "heading": -0.0066168114481254744, + "angularVelocity": 0.018301194544491067, + "velocityX": 1.888418713800162, + "velocityY": 0.0406817649578728, + "timestamp": 6.514164215484844 + }, + { + "x": 7.873689685619936, + "y": 4.06536905162046, + "heading": -0.004961765552273349, + "angularVelocity": 0.027741809673791765, + "velocityX": 1.5760893848068933, + "velocityY": 0.04109354240631638, + "timestamp": 6.573823117168381 + }, + { + "x": 7.949083994059634, + "y": 4.067716008149933, + "heading": -0.003176764942269326, + "angularVelocity": 0.029920105124841948, + "velocityX": 1.263756226013528, + "velocityY": 0.03933958660390468, + "timestamp": 6.633482018851917 + }, + { + "x": 8.005845696502893, + "y": 4.069860191657625, + "heading": -0.0015908607397525072, + "angularVelocity": 0.02658285951909277, + "velocityX": 0.951437268227904, + "velocityY": 0.035940713744034375, + "timestamp": 6.693140920535454 + }, + { + "x": 8.043976076014772, + "y": 4.071724543898811, + "heading": -0.00046248549538235704, + "angularVelocity": 0.018913778372181077, + "velocityX": 0.6391398171247351, + "velocityY": 0.03125019382816773, + "timestamp": 6.75279982221899 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 3.4463826664070866e-33, + "angularVelocity": 0.00775216241552071, + "velocityX": 0.32686633402451676, + "velocityY": 0.02551860543538527, + "timestamp": 6.812458723902527 + }, + { + "x": 8.059339450711764, + "y": 4.074523581107775, + "heading": -0.0006944740465081056, + "angularVelocity": -0.009481698382290156, + "velocityX": -0.0564842507032717, + "velocityY": 0.017429845647464255, + "timestamp": 6.88570235795497 + }, + { + "x": 8.027124349390371, + "y": 4.075207755344852, + "heading": -0.002651211191279339, + "angularVelocity": -0.026715456845985846, + "velocityX": -0.43983482985464195, + "velocityY": 0.009341074428224473, + "timestamp": 6.958945992007413 + }, + { + "x": 7.96683125907587, + "y": 4.075299477238389, + "heading": -0.0058702138612145086, + "angularVelocity": -0.043949248444312826, + "velocityX": -0.8231854016327222, + "velocityY": 0.0012522848534625944, + "timestamp": 7.032189626059856 + }, + { + "x": 7.878460180490193, + "y": 4.074798744928973, + "heading": -0.0103514975430217, + "angularVelocity": -0.06118325148365151, + "velocityX": -1.2065359635542152, + "velocityY": -0.006836530107951673, + "timestamp": 7.105433260112299 + }, + { + "x": 7.7620111146201785, + "y": 4.073705556029158, + "heading": -0.01609509527793809, + "angularVelocity": -0.07841770563710548, + "velocityX": -1.5898865120023296, + "velocityY": -0.01492537766534737, + "timestamp": 7.1786768941647425 + }, + { + "x": 7.617484062863899, + "y": 4.072019907599753, + "heading": -0.023101063438175296, + "angularVelocity": -0.0956529294439546, + "velocityX": -1.9732370413624691, + "velocityY": -0.02301426535169652, + "timestamp": 7.251920528217186 + }, + { + "x": 7.444879027316714, + "y": 4.06974179610174, + "heading": -0.031369488604002356, + "angularVelocity": -0.1128893353367305, + "velocityX": -2.356587542114559, + "velocityY": -0.031103201356480925, + "timestamp": 7.325164162269629 + }, + { + "x": 7.244196011426359, + "y": 4.066871217282435, + "heading": -0.040900495023888005, + "angularVelocity": -0.13012743760176224, + "velocityX": -2.7399379957944494, + "velocityY": -0.0391921954234215, + "timestamp": 7.398407796322072 + }, + { + "x": 7.015435021934675, + "y": 4.063408165830539, + "heading": -0.05169425070794916, + "angularVelocity": -0.14736783371961967, + "velocityX": -3.123288357427606, + "velocityY": -0.047281262005875575, + "timestamp": 7.471651430374515 + }, + { + "x": 6.758596078532163, + "y": 4.059352633648335, + "heading": -0.06375095974291681, + "angularVelocity": -0.1646110162466115, + "velocityX": -3.5066384502250503, + "velocityY": -0.05537043914696755, + "timestamp": 7.544895064426958 + }, + { + "x": 6.481653456514826, + "y": 4.053513800761945, + "heading": -0.06375096147352038, + "angularVelocity": -2.3628040945559003e-8, + "velocityX": -3.7811152545904956, + "velocityY": -0.07971795722490971, + "timestamp": 7.6181386984794015 + }, + { + "x": 6.205372409181029, + "y": 4.073516064315128, + "heading": -0.06375096323470678, + "angularVelocity": -2.4045589953178915e-8, + "velocityX": -3.7720827333058655, + "velocityY": 0.2730921780705308, + "timestamp": 7.691382332531845 + }, + { + "x": 5.9325368870621995, + "y": 4.121391787593611, + "heading": -0.0637509651063581, + "angularVelocity": -2.555377459766275e-8, + "velocityX": -3.725040758128897, + "velocityY": 0.6536502987304462, + "timestamp": 7.764625966584288 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.063750967187294, + "angularVelocity": -2.84111505129003e-8, + "velocityX": -3.639704142076304, + "velocityY": 1.0274925313911556, + "timestamp": 7.837869600636731 + }, + { + "x": 5.417504232216337, + "y": 4.293821782165803, + "heading": -0.06375097411514968, + "angularVelocity": -9.821339780633191e-8, + "velocityX": -3.5221393138546646, + "velocityY": 1.3775780331429743, + "timestamp": 7.90840840517157 + }, + { + "x": 5.186014168311204, + "y": 4.410724788288498, + "heading": -0.06922328693791815, + "angularVelocity": -0.07757875766190163, + "velocityX": -3.281740673543775, + "velocityY": 1.6572864665569722, + "timestamp": 7.9789472097064085 + }, + { + "x": 4.979961148964822, + "y": 4.522014233902487, + "heading": -0.07551597396386121, + "angularVelocity": -0.08920886974821321, + "velocityX": -2.9211300177991313, + "velocityY": 1.5777052977843462, + "timestamp": 8.049486014241246 + }, + { + "x": 4.7980291404215, + "y": 4.623466282585381, + "heading": -0.08170492719079357, + "angularVelocity": -0.08773827778546625, + "velocityX": -2.579176238427287, + "velocityY": 1.4382445145180842, + "timestamp": 8.120024818776084 + }, + { + "x": 4.639563192627529, + "y": 4.713607705102715, + "heading": -0.0874491132160183, + "angularVelocity": -0.08143299369906003, + "velocityX": -2.246507420120879, + "velocityY": 1.27789835838249, + "timestamp": 8.190563623310922 + }, + { + "x": 4.504192007690659, + "y": 4.791698947450768, + "heading": -0.09257307344830198, + "angularVelocity": -0.07264030438385177, + "velocityX": -1.919102341322099, + "velocityY": 1.1070678453231335, + "timestamp": 8.26110242784576 + }, + { + "x": 4.391678654940531, + "y": 4.857296763925047, + "heading": -0.09697023729098278, + "angularVelocity": -0.0623368069770602, + "velocityX": -1.5950561324662005, + "velocityY": 0.9299536178257779, + "timestamp": 8.331641232380598 + }, + { + "x": 4.301859289367306, + "y": 4.910106206970951, + "heading": -0.10056909340764136, + "angularVelocity": -0.051019522380495494, + "velocityX": -1.2733326878096392, + "velocityY": 0.7486580385668955, + "timestamp": 8.402180036915436 + }, + { + "x": 4.234614001747884, + "y": 4.949916980366573, + "heading": -0.10331834169263278, + "angularVelocity": -0.03897497700905296, + "velocityX": -0.9533091475375062, + "velocityY": 0.5643811751297677, + "timestamp": 8.472718841450273 + }, + { + "x": 4.18985129168719, + "y": 4.976571613170809, + "heading": -0.10517936365869324, + "angularVelocity": -0.026382953019019773, + "velocityX": -0.6345827712261082, + "velocityY": 0.37787191007854753, + "timestamp": 8.543257645985111 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.013363372792080578, + "velocityX": -0.31687843925651377, + "velocityY": 0.18962871266842563, + "timestamp": 8.61379645051995 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": 0, + "velocityX": 8.237561081237056e-34, + "velocityY": 0, + "timestamp": 8.684335255054787 + }, + { + "x": 4.18294196758914, + "y": 4.969364336221789, + "heading": -0.10045524830547124, + "angularVelocity": 0.08077125535022475, + "velocityX": 0.22011597884885625, + "velocityY": -0.29338710511594707, + "timestamp": 8.754493279341492 + }, + { + "x": 4.2148538086473515, + "y": 4.929011600999615, + "heading": -0.08896968745487453, + "angularVelocity": 0.16370986736542367, + "velocityX": 0.4548566095276789, + "velocityY": -0.5751692074062589, + "timestamp": 8.824651303628197 + }, + { + "x": 4.264519080629496, + "y": 4.870038214957966, + "heading": -0.07148588735689271, + "angularVelocity": 0.24920599283886813, + "velocityX": 0.7079057953397331, + "velocityY": -0.8405793441481644, + "timestamp": 8.894809327914903 + }, + { + "x": 4.333555645892384, + "y": 4.794138441539978, + "heading": -0.04780159185128592, + "angularVelocity": 0.3375849839901328, + "velocityX": 0.9840152422303832, + "velocityY": -1.081840234095224, + "timestamp": 8.964967352201608 + }, + { + "x": 4.423967613250648, + "y": 4.703932489290492, + "heading": -0.017716503350327845, + "angularVelocity": 0.4288189242332906, + "velocityX": 1.2886903284047762, + "velocityY": -1.2857538844146224, + "timestamp": 9.035125376488313 + }, + { + "x": 4.53803034325044, + "y": 4.603622363293204, + "heading": 0.0188848190363467, + "angularVelocity": 0.5216983054867799, + "velocityX": 1.6257973504736565, + "velocityY": -1.429774099500926, + "timestamp": 9.105283400775019 + }, + { + "x": 4.677578748555051, + "y": 4.49984021599331, + "heading": 0.06181064388576721, + "angularVelocity": 0.6118448357952799, + "velocityX": 1.9890583682108496, + "velocityY": -1.4792626838489913, + "timestamp": 9.175441425061724 + }, + { + "x": 4.842258553230926, + "y": 4.40159118744757, + "heading": 0.11019595374155315, + "angularVelocity": 0.6896618077221814, + "velocityX": 2.3472697007957355, + "velocityY": -1.4003961705682948, + "timestamp": 9.245599449348429 + }, + { + "x": 5.028363428376419, + "y": 4.3176283558190685, + "heading": 0.16247927063581383, + "angularVelocity": 0.7452221955481739, + "velocityX": 2.6526527369836272, + "velocityY": -1.1967673332045712, + "timestamp": 9.315757473635134 + }, + { + "x": 5.230618576677741, + "y": 4.253738965485636, + "heading": 0.2169576127744663, + "angularVelocity": 0.7765090692409344, + "velocityX": 2.8828512541173206, + "velocityY": -0.9106497935623799, + "timestamp": 9.38591549792184 + }, + { + "x": 5.444331250681613, + "y": 4.2129309832647035, + "heading": 0.2722364606827335, + "angularVelocity": 0.7879191079037022, + "velocityX": 3.0461615214607765, + "velocityY": -0.5816580873795356, + "timestamp": 9.456073522208545 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.3273042274639755, + "angularVelocity": 0.7849104552346565, + "velocityX": 3.1588757008538453, + "velocityY": -0.23207478938864265, + "timestamp": 9.52623154649525 + }, + { + "x": 5.81887557997068, + "y": 4.1972183908996925, + "heading": 0.36426543181084436, + "angularVelocity": 0.7759151841130933, + "velocityX": 3.210283328613804, + "velocityY": 0.011951482768166844, + "timestamp": 9.573867170985725 + }, + { + "x": 5.973953798857342, + "y": 4.209469324927334, + "heading": 0.4004931510026103, + "angularVelocity": 0.7605173560600648, + "velocityX": 3.255509307276357, + "velocityY": 0.257180086514691, + "timestamp": 9.6215027954762 + }, + { + "x": 6.130819634438986, + "y": 4.233461866975936, + "heading": 0.4356102472173979, + "angularVelocity": 0.7372023898166727, + "velocityX": 3.2930361942250057, + "velocityY": 0.5036680489703445, + "timestamp": 9.669138419966675 + }, + { + "x": 6.289005438456121, + "y": 4.269254559307002, + "heading": 0.4691390733673604, + "angularVelocity": 0.7038603253047925, + "velocityX": 3.32074588523062, + "velocityY": 0.7513849710991581, + "timestamp": 9.71677404445715 + }, + { + "x": 6.447898093638549, + "y": 4.316894151635714, + "heading": 0.5004573197150548, + "angularVelocity": 0.6574543040567628, + "velocityX": 3.335584594135005, + "velocityY": 1.0000832956066072, + "timestamp": 9.764409668947625 + }, + { + "x": 6.606667388474461, + "y": 4.376390356038531, + "heading": 0.5287266543286071, + "angularVelocity": 0.5934494386486953, + "velocityX": 3.33299492835787, + "velocityY": 1.248985502745193, + "timestamp": 9.8120452934381 + }, + { + "x": 6.764149093497097, + "y": 4.447653029340476, + "heading": 0.5527726659639904, + "angularVelocity": 0.5047905195447082, + "velocityX": 3.3059649518004255, + "velocityY": 1.4959953619626436, + "timestamp": 9.859680917928575 + }, + { + "x": 6.918662738022944, + "y": 4.530331648811429, + "heading": 0.5708777234010636, + "angularVelocity": 0.3800738970199359, + "velocityX": 3.243657371527556, + "velocityY": 1.735646805417357, + "timestamp": 9.90731654241905 + }, + { + "x": 7.06846751913844, + "y": 4.623866769358016, + "heading": 0.5845734852773292, + "angularVelocity": 0.2875109127414552, + "velocityX": 3.144805651607445, + "velocityY": 1.9635539902555645, + "timestamp": 9.954952166909525 + }, + { + "x": 7.211529043645413, + "y": 4.727181705822193, + "heading": 0.5988053175713824, + "angularVelocity": 0.2987644739893997, + "velocityX": 3.0032465415789793, + "velocityY": 2.168858655035301, + "timestamp": 10.0025877914 + }, + { + "x": 7.344594975677938, + "y": 4.83690117801315, + "heading": 0.6149173466892092, + "angularVelocity": 0.3382348670803817, + "velocityX": 2.7934121459693166, + "velocityY": 2.303307101870708, + "timestamp": 10.050223415890475 + }, + { + "x": 7.465817541114002, + "y": 4.947544737637474, + "heading": 0.6314993734499258, + "angularVelocity": 0.34810138290582165, + "velocityX": 2.5447879970651583, + "velocityY": 2.322706184873219, + "timestamp": 10.09785904038095 + }, + { + "x": 7.575645879536293, + "y": 5.054829291458012, + "heading": 0.6474951019450449, + "angularVelocity": 0.33579340391176205, + "velocityX": 2.305592497149964, + "velocityY": 2.252191610125547, + "timestamp": 10.145494664871425 + }, + { + "x": 7.675112095346534, + "y": 5.156311134408354, + "heading": 0.6623068665871409, + "angularVelocity": 0.31093881523601674, + "velocityX": 2.0880636472002183, + "velocityY": 2.1303770872287977, + "timestamp": 10.1931302893619 + }, + { + "x": 7.76511711692712, + "y": 5.250618542879285, + "heading": 0.6755917653702976, + "angularVelocity": 0.27888579031462285, + "velocityX": 1.8894477094255981, + "velocityY": 1.979766392897571, + "timestamp": 10.240765913852375 + }, + { + "x": 7.846351174360072, + "y": 5.336925095447112, + "heading": 0.6871379054751507, + "angularVelocity": 0.2423845646688635, + "velocityX": 1.7053215592711606, + "velocityY": 1.8118068880378715, + "timestamp": 10.28840153834285 + }, + { + "x": 7.919337423332426, + "y": 5.414694341101313, + "heading": 0.6968047845571967, + "angularVelocity": 0.2029338165594727, + "velocityX": 1.5321778553978798, + "velocityY": 1.6325858322640792, + "timestamp": 10.336037162833325 + }, + { + "x": 7.984478950500488, + "y": 5.483555793762207, + "heading": 0.7044936776046891, + "angularVelocity": 0.16141056467161, + "velocityX": 1.3674960256076552, + "velocityY": 1.445587276276031, + "timestamp": 10.3836727873238 + }, + { + "x": 8.05911422318913, + "y": 5.560200385763573, + "heading": 0.7111561296070518, + "angularVelocity": 0.10289890039880417, + "velocityX": 1.1527118676281318, + "velocityY": 1.1837449989371336, + "timestamp": 10.44842034027666 + }, + { + "x": 8.119756898561732, + "y": 5.61995838055026, + "heading": 0.7143078455831314, + "angularVelocity": 0.04867698982190534, + "velocityX": 0.9366018112955389, + "velocityY": 0.9229382742880431, + "timestamp": 10.513167893229522 + }, + { + "x": 8.166344572702991, + "y": 5.662879129934428, + "heading": 0.7141510891264902, + "angularVelocity": -0.002421040633850211, + "velocityX": 0.7195279515069205, + "velocityY": 0.6628937685941038, + "timestamp": 10.577915446182383 + }, + { + "x": 8.198829842984775, + "y": 5.689000472186179, + "heading": 0.7108395294312745, + "angularVelocity": -0.0511457119873956, + "velocityX": 0.5017219771291601, + "velocityY": 0.4034336598136588, + "timestamp": 10.642662999135243 + }, + { + "x": 8.217175483703613, + "y": 5.698352336883545, + "heading": 0.7044936776046891, + "angularVelocity": -0.09800913759946452, + "velocityX": 0.28334106668393205, + "velocityY": 0.1444358013680284, + "timestamp": 10.707410552088104 + }, + { + "x": 8.221318658765867, + "y": 5.6908798741594095, + "heading": 0.6951703689604403, + "angularVelocity": -0.14352542875947455, + "velocityX": 0.06378111032529457, + "velocityY": -0.11503302714668015, + "timestamp": 10.77236983299625 + }, + { + "x": 8.210964366460544, + "y": 5.666753242665305, + "heading": 0.6829793287804198, + "angularVelocity": -0.18767203099521718, + "velocityX": -0.15939665834608732, + "velocityY": -0.3714116159663335, + "timestamp": 10.837329113904394 + }, + { + "x": 8.185835060378174, + "y": 5.6262170290983935, + "heading": 0.6680275277365855, + "angularVelocity": -0.2301718989927961, + "velocityX": -0.3868470483517696, + "velocityY": -0.6240249738021217, + "timestamp": 10.902288394812539 + }, + { + "x": 8.14559839699319, + "y": 5.569575312921383, + "heading": 0.6504460796726778, + "angularVelocity": -0.2706533665107588, + "velocityX": -0.6194136206938888, + "velocityY": -0.8719572536078877, + "timestamp": 10.967247675720683 + }, + { + "x": 8.08985004858283, + "y": 5.497215477528086, + "heading": 0.6303993620580922, + "angularVelocity": -0.30860436467780866, + "velocityX": -0.858204518753663, + "velocityY": -1.11392605308573, + "timestamp": 11.032206956628828 + }, + { + "x": 8.018089118716903, + "y": 5.40964606854142, + "heading": 0.6080990792438148, + "angularVelocity": -0.3432963312172538, + "velocityX": -1.1047063462324833, + "velocityY": -1.348066169489957, + "timestamp": 11.097166237536973 + }, + { + "x": 7.9296824410081745, + "y": 5.307560214485479, + "heading": 0.5838269457597958, + "angularVelocity": -0.3736515112958315, + "velocityX": -1.3609553011176063, + "velocityY": -1.5715360858180332, + "timestamp": 11.162125518445118 + }, + { + "x": 7.823812920396748, + "y": 5.191948689740781, + "heading": 0.5579732566194568, + "angularVelocity": -0.3979983888198644, + "velocityX": -1.6297828290483853, + "velocityY": -1.779753764641825, + "timestamp": 11.227084799353262 + }, + { + "x": 7.699409898040676, + "y": 5.064316508380435, + "heading": 0.5311065320658849, + "angularVelocity": -0.41359331842916214, + "velocityX": -1.9150923565792266, + "velocityY": -1.9648028669039912, + "timestamp": 11.292044080261407 + }, + { + "x": 7.555088273925506, + "y": 4.927129260587564, + "heading": 0.5041069990932381, + "angularVelocity": -0.4156378056405048, + "velocityX": -2.221724472585296, + "velocityY": -2.111896035100176, + "timestamp": 11.357003361169552 + }, + { + "x": 7.389294037285908, + "y": 4.784758435393721, + "heading": 0.47842631175838873, + "angularVelocity": -0.3953351542047192, + "velocityX": -2.5522794329271568, + "velocityY": -2.1916933685759186, + "timestamp": 11.421962642077697 + }, + { + "x": 7.201584646398764, + "y": 4.645022669025462, + "heading": 0.45644938425338794, + "angularVelocity": -0.33831851581111005, + "velocityX": -2.8896469952087562, + "velocityY": -2.1511285903218442, + "timestamp": 11.486921922985841 + }, + { + "x": 6.996114167384252, + "y": 4.5183305598699945, + "heading": 0.4409940125357126, + "angularVelocity": -0.23792399641137907, + "velocityX": -3.16306578739771, + "velocityY": -1.9503311518274693, + "timestamp": 11.551881203893986 + }, + { + "x": 6.77954298581377, + "y": 4.410738153252425, + "heading": 0.4262239025498049, + "angularVelocity": -0.2273748997744145, + "velocityX": -3.3339528785227577, + "velocityY": -1.656305382593562, + "timestamp": 11.61684048480213 + }, + { + "x": 6.556274698008425, + "y": 4.324091773722086, + "heading": 0.4025359766205174, + "angularVelocity": -0.3646580688413556, + "velocityX": -3.43704986699365, + "velocityY": -1.3338568149000936, + "timestamp": 11.681799765710275 + }, + { + "x": 6.330620508913663, + "y": 4.259333422241355, + "heading": 0.3712479640794565, + "angularVelocity": -0.4816557711792279, + "velocityX": -3.4737790495840706, + "velocityY": -0.996906840337434, + "timestamp": 11.74675904661842 + }, + { + "x": 6.105727463248204, + "y": 4.216621857922548, + "heading": 0.33499279771425994, + "angularVelocity": -0.5581214240419701, + "velocityX": -3.462061810435755, + "velocityY": -0.6575128868683483, + "timestamp": 11.811718327526565 + }, + { + "x": 5.88368699546657, + "y": 4.195803829122771, + "heading": 0.29554108787827377, + "angularVelocity": -0.6073298423942238, + "velocityX": -3.4181484874441033, + "velocityY": -0.3204781288945418, + "timestamp": 11.87667760843471 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.2541387867771501, + "angularVelocity": -0.6373577496904229, + "velocityX": -3.351873105763798, + "velocityY": 0.013011927161991938, + "timestamp": 11.941636889342854 + }, + { + "x": 5.421845873128357, + "y": 4.226158090162152, + "heading": 0.20498652451135702, + "angularVelocity": -0.6557520326399592, + "velocityX": -3.256674335436179, + "velocityY": 0.3936868024775833, + "timestamp": 12.01659245153595 + }, + { + "x": 5.1888462982312005, + "y": 4.282903811630084, + "heading": 0.15538584116955914, + "angularVelocity": -0.6617345249712095, + "velocityX": -3.1085027992574967, + "velocityY": 0.7570581796417871, + "timestamp": 12.091548013729044 + }, + { + "x": 4.97232882862877, + "y": 4.364011234145266, + "heading": 0.10667736071804554, + "angularVelocity": -0.6498314338038612, + "velocityX": -2.8886111086013, + "velocityY": 1.08207343313948, + "timestamp": 12.166503575922139 + }, + { + "x": 4.778685496187651, + "y": 4.463599711895904, + "heading": 0.06061512765816293, + "angularVelocity": -0.6145272173560689, + "velocityX": -2.5834417990524554, + "velocityY": 1.3286335908479392, + "timestamp": 12.241459138115234 + }, + { + "x": 4.613047571450946, + "y": 4.572114931147961, + "heading": 0.018966273025947666, + "angularVelocity": -0.5556472850530078, + "velocityX": -2.2098149875789206, + "velocityY": 1.4477273744209653, + "timestamp": 12.316414700308329 + }, + { + "x": 4.476747715125104, + "y": 4.67904248790438, + "heading": -0.017090034062622824, + "angularVelocity": -0.4810357768471503, + "velocityX": -1.8184088323521106, + "velocityY": 1.4265459910894789, + "timestamp": 12.391370262501423 + }, + { + "x": 4.368193539344783, + "y": 4.776321878924991, + "heading": -0.04707218407807139, + "angularVelocity": -0.3999990012510431, + "velocityX": -1.4482471027389752, + "velocityY": 1.2978275150549403, + "timestamp": 12.466325824694518 + }, + { + "x": 4.2849948544017495, + "y": 4.858769971637843, + "heading": -0.07089061350138144, + "angularVelocity": -0.3177673374252157, + "velocityX": -1.1099734630593825, + "velocityY": 1.0999596334219341, + "timestamp": 12.541281386887613 + }, + { + "x": 4.224952920077694, + "y": 4.923143218233113, + "heading": -0.08860420951463882, + "angularVelocity": -0.23632130151495423, + "velocityX": -0.8010337400896302, + "velocityY": 0.8588188082618416, + "timestamp": 12.616236949080708 + }, + { + "x": 4.186266834046781, + "y": 4.967343359425017, + "heading": -0.10031361199761639, + "angularVelocity": -0.1562179262002281, + "velocityX": -0.5161202837923176, + "velocityY": 0.5896846064343946, + "timestamp": 12.691192511273803 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.07749108715135035, + "velocityX": -0.250385269598314, + "velocityY": 0.3015711680564945, + "timestamp": 12.766148073466898 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -7.372861805121809e-39, + "velocityX": 4.014609310130765e-40, + "velocityY": -1.5891469244205483e-39, + "timestamp": 12.841103635659993 } ], "constraints": [ @@ -33578,14 +23931,13 @@ }, { "scope": [ - 4, - 5 + 2 ], - "type": "StraightLine" + "type": "StopPoint" }, { "scope": [ - 2 + 11 ], "type": "StopPoint" } diff --git a/src/main/deploy/choreo/AmpLanePSprint.traj b/src/main/deploy/choreo/AmpLanePSprint.traj deleted file mode 100644 index 8b41dd0f..00000000 --- a/src/main/deploy/choreo/AmpLanePSprint.traj +++ /dev/null @@ -1,436 +0,0 @@ -{ - "samples": [ - { - "x": 1.4685733318328855, - "y": 7.0094075202941895, - "heading": 6.049320357152304e-20, - "angularVelocity": 5.735208962067612e-20, - "velocityX": 1.2024934428735394e-18, - "velocityY": 5.740194914400006e-19, - "timestamp": 0 - }, - { - "x": 1.4539549354530197, - "y": 7.003550802171312, - "heading": 0.018695171024518124, - "angularVelocity": 0.28848806436406915, - "velocityX": -0.22557872673127724, - "velocityY": -0.09037591967352629, - "timestamp": 0.06480396707478588 - }, - { - "x": 1.4247253207548556, - "y": 6.991839516204374, - "heading": 0.05623662341413578, - "angularVelocity": 0.5793079356745707, - "velocityX": -0.45104668768861206, - "velocityY": -0.1807186580633629, - "timestamp": 0.12960793414957175 - }, - { - "x": 1.3808979484465476, - "y": 6.974274102040644, - "heading": 0.11287119216832771, - "angularVelocity": 0.8739367558907741, - "velocityX": -0.6763069343845826, - "velocityY": -0.2710546121884731, - "timestamp": 0.19441190122435764 - }, - { - "x": 1.3224921483061116, - "y": 6.950848437335187, - "heading": 0.18890117826912542, - "angularVelocity": 1.1732304291349192, - "velocityX": -0.9012689002979366, - "velocityY": -0.36148504116147834, - "timestamp": 0.2592158682991435 - }, - { - "x": 1.2495302432323667, - "y": 6.921544701913893, - "heading": 0.28459715171144206, - "angularVelocity": 1.4766993096561234, - "velocityX": -1.1258863981204084, - "velocityY": -0.45219045598668955, - "timestamp": 0.32401983537392937 - }, - { - "x": 1.1620307559069942, - "y": 6.886330686053142, - "heading": 0.40005759835574417, - "angularVelocity": 1.7816879406634767, - "velocityX": -1.3502180695881552, - "velocityY": -0.5433929040194694, - "timestamp": 0.38882380244871523 - }, - { - "x": 1.0599976129357116, - "y": 6.845163950858531, - "heading": 0.5350484398222678, - "angularVelocity": 2.083064472129233, - "velocityX": -1.574489148998107, - "velocityY": -0.6352502331701971, - "timestamp": 0.4536277695235011 - }, - { - "x": 0.9575825133734215, - "y": 6.803331405068127, - "heading": 0.6590788452444744, - "angularVelocity": 1.9139322949021502, - "velocityX": -1.5803831800003518, - "velocityY": -0.6455244590524392, - "timestamp": 0.518431736598287 - }, - { - "x": 0.8697427157925169, - "y": 6.767497218853931, - "heading": 0.7646238641532083, - "angularVelocity": 1.6286814476486076, - "velocityX": -1.3554694495714084, - "velocityY": -0.5529628482904638, - "timestamp": 0.5832357036730729 - }, - { - "x": 0.7965061565345849, - "y": 6.737660077605358, - "heading": 0.8521458046235697, - "angularVelocity": 1.3505645475277608, - "velocityX": -1.130124629151394, - "velocityY": -0.4604215234869663, - "timestamp": 0.6480396707478587 - }, - { - "x": 0.7378960834776489, - "y": 6.713809764371052, - "heading": 0.9219596832810217, - "angularVelocity": 1.0773087174876965, - "velocityX": -0.9044210671438955, - "velocityY": -0.3680378580339382, - "timestamp": 0.7128436378226446 - }, - { - "x": 0.6939290380184983, - "y": 6.695933388139099, - "heading": 0.9742459780095203, - "angularVelocity": 0.80683786948041, - "velocityX": -0.6784622522940782, - "velocityY": -0.27585311577181243, - "timestamp": 0.7776476048974305 - }, - { - "x": 0.6646150045031116, - "y": 6.6840200000677585, - "heading": 1.0090892197053958, - "angularVelocity": 0.5376714307577112, - "velocityX": -0.4523493674632192, - "velocityY": -0.18383732677339054, - "timestamp": 0.8424515719722163 - }, - { - "x": 0.6499583125114444, - "y": 6.67806339263916, - "heading": 1.0265157581506656, - "angularVelocity": 0.26891159958091726, - "velocityX": -0.22616967221702353, - "velocityY": -0.09191732693964001, - "timestamp": 0.9072555390470022 - }, - { - "x": 0.6499583125114444, - "y": 6.67806339263916, - "heading": 1.0265157581506656, - "angularVelocity": 9.703439676406597e-18, - "velocityX": 1.8974989673442822e-18, - "velocityY": 3.469574557132838e-18, - "timestamp": 0.972059506121788 - }, - { - "x": 0.677098524254357, - "y": 6.676790336244545, - "heading": 1.0174178053772396, - "angularVelocity": -0.10888280660674853, - "velocityX": 0.32480960278249144, - "velocityY": -0.015235730132528697, - "timestamp": 1.055616800950558 - }, - { - "x": 0.7313843379611403, - "y": 6.674244414674947, - "heading": 0.999511909828794, - "angularVelocity": -0.21429482111807416, - "velocityX": 0.649683714845345, - "velocityY": -0.03046917177986014, - "timestamp": 1.139174095779328 - }, - { - "x": 0.8128220453410844, - "y": 6.670425841513566, - "heading": 0.9731501881376037, - "angularVelocity": -0.31549276152616484, - "velocityX": 0.9746331250530577, - "velocityY": -0.04570005729847779, - "timestamp": 1.2227313906080979 - }, - { - "x": 0.9214189734172925, - "y": 6.665334889481153, - "heading": 0.938761218692723, - "angularVelocity": -0.41156154606662587, - "velocityX": 1.299670223871541, - "velocityY": -0.06092767893988849, - "timestamp": 1.3062886854368678 - }, - { - "x": 1.057183650080325, - "y": 6.658971921820971, - "heading": 0.8968713117735069, - "angularVelocity": -0.5013315355056764, - "velocityX": 1.6248093830855639, - "velocityY": -0.07615095334551644, - "timestamp": 1.3898459802656378 - }, - { - "x": 1.2201259982302939, - "y": 6.651337410962851, - "heading": 0.8481374363541635, - "angularVelocity": -0.5832390280130826, - "velocityX": 1.9500672979406355, - "velocityY": -0.09136857378837147, - "timestamp": 1.4734032750944077 - }, - { - "x": 1.410257523594535, - "y": 6.642431937989669, - "heading": 0.7934008809085091, - "angularVelocity": -0.65507811804839, - "velocityX": 2.275462911453366, - "velocityY": -0.1065792399267018, - "timestamp": 1.5569605699231777 - }, - { - "x": 1.6275913452117106, - "y": 6.632256164958325, - "heading": 0.7337801464779199, - "angularVelocity": -0.7135311710696056, - "velocityX": 2.601015531469118, - "velocityY": -0.12178198267664266, - "timestamp": 1.6405178647519476 - }, - { - "x": 1.8721414897299389, - "y": 6.620810753247128, - "heading": 0.6708446221029851, - "angularVelocity": -0.7532020334537907, - "velocityX": 2.9267360201089847, - "velocityY": -0.13697681015944838, - "timestamp": 1.7240751595807176 - }, - { - "x": 2.1439190987685945, - "y": 6.6080960989852695, - "heading": 0.6069767178684699, - "angularVelocity": -0.76436060269055, - "velocityX": 3.2525898498221513, - "velocityY": -0.15216689683305046, - "timestamp": 1.8076324544094875 - }, - { - "x": 2.442913985874264, - "y": 6.5941112960890615, - "heading": 0.5462664171235281, - "angularVelocity": -0.7265709220164723, - "velocityX": 3.578321769731578, - "velocityY": -0.16736782736764083, - "timestamp": 1.8911897492382574 - }, - { - "x": 2.7689778327808057, - "y": 6.578852444576511, - "heading": 0.4974709825138351, - "angularVelocity": -0.5839757583068491, - "velocityX": 3.9022786409579977, - "velocityY": -0.1826154322470609, - "timestamp": 1.9747470440670274 - }, - { - "x": 3.120091446148044, - "y": 6.56305392023088, - "heading": 0.49252318839031584, - "angularVelocity": -0.059214388566174905, - "velocityX": 4.202070137463798, - "velocityY": -0.18907414819982873, - "timestamp": 2.0583043388957973 - }, - { - "x": 3.4728474074143234, - "y": 6.5466252324603085, - "heading": 0.4925231817165128, - "angularVelocity": -7.987099095905687e-8, - "velocityX": 4.221725487752611, - "velocityY": -0.19661584071429156, - "timestamp": 2.141861633724567 - }, - { - "x": 3.825603367792492, - "y": 6.5301965258536905, - "heading": 0.49252317502370946, - "angularVelocity": -8.009838379406554e-8, - "velocityX": 4.221725477123844, - "velocityY": -0.1966160661410119, - "timestamp": 2.225418928553337 - }, - { - "x": 4.1783593283685585, - "y": 6.513767823497187, - "heading": 0.49252316833088156, - "angularVelocity": -8.009867777354286e-8, - "velocityX": 4.221725479492243, - "velocityY": -0.19661601527633685, - "timestamp": 2.3089762233821065 - }, - { - "x": 4.5311152888161, - "y": 6.497339118379854, - "heading": 0.49252316163811777, - "angularVelocity": -8.009791003841919e-8, - "velocityX": 4.2217254779540765, - "velocityY": -0.19661604831749765, - "timestamp": 2.3925335182108762 - }, - { - "x": 4.883871249955156, - "y": 6.480910427791649, - "heading": 0.49252315497752247, - "angularVelocity": -7.971292311350099e-8, - "velocityX": 4.221725486230021, - "velocityY": -0.19661587443527756, - "timestamp": 2.476090813039646 - }, - { - "x": 5.235292142896319, - "y": 6.465020831099543, - "heading": 0.4884937210472112, - "angularVelocity": -0.04822360439721594, - "velocityX": 4.205747609006577, - "velocityY": -0.19016408710534566, - "timestamp": 2.5596481078684157 - }, - { - "x": 5.56143349165223, - "y": 6.449807932309173, - "heading": 0.4427260149092047, - "angularVelocity": -0.5477404005457096, - "velocityX": 3.903206170379902, - "velocityY": -0.1820654776048722, - "timestamp": 2.6432054026971854 - }, - { - "x": 5.860479824683085, - "y": 6.4358664343174405, - "heading": 0.38609345994808875, - "angularVelocity": -0.6777691292804887, - "velocityX": 3.578937466126377, - "velocityY": -0.16684956137344756, - "timestamp": 2.726762697525955 - }, - { - "x": 6.132305844443899, - "y": 6.42319455429606, - "heading": 0.3267902425497731, - "angularVelocity": -0.7097311793045699, - "velocityX": 3.253169221404942, - "velocityY": -0.1516549817385581, - "timestamp": 2.810319992354725 - }, - { - "x": 6.376904781169397, - "y": 6.41179175824138, - "heading": 0.2685758916504145, - "angularVelocity": -0.6966998036336328, - "velocityX": 2.927319957243016, - "velocityY": -0.13646679297179057, - "timestamp": 2.8938772871834946 - }, - { - "x": 6.594287281116567, - "y": 6.401657882345162, - "heading": 0.21360285288348452, - "angularVelocity": -0.6579083116511133, - "velocityX": 2.6015981057385837, - "velocityY": -0.12128056463513756, - "timestamp": 2.9774345820122643 - }, - { - "x": 6.784465944209141, - "y": 6.392792582186275, - "heading": 0.16326745687666674, - "angularVelocity": -0.6024057637333325, - "velocityX": 2.2760270480548472, - "velocityY": -0.10609845827410212, - "timestamp": 3.060991876841034 - }, - { - "x": 6.947452313541236, - "y": 6.385195340051002, - "heading": 0.11855161426546505, - "angularVelocity": -0.5351518703762895, - "velocityX": 1.950594136228255, - "velocityY": -0.09092254782591588, - "timestamp": 3.1445491716698037 - }, - { - "x": 7.083256400212696, - "y": 6.3788655571350015, - "heading": 0.08018557174620769, - "angularVelocity": -0.45915850432779387, - "velocityX": 1.6252810356026588, - "velocityY": -0.07575380376988836, - "timestamp": 3.2281064664985735 - }, - { - "x": 7.191886785288102, - "y": 6.373802632634511, - "heading": 0.04873492465610617, - "angularVelocity": -0.37639618604873454, - "velocityX": 1.300070631750541, - "velocityY": -0.06059225003472367, - "timestamp": 3.311663761327343 - }, - { - "x": 7.273350833513733, - "y": 6.370006018118368, - "heading": 0.024651280459860023, - "angularVelocity": -0.2882291037018025, - "velocityX": 0.9749483679739933, - "velocityY": -0.04543725983378117, - "timestamp": 3.395221056156113 - }, - { - "x": 7.32765490169191, - "y": 6.367475252448548, - "heading": 0.008303807824017625, - "angularVelocity": -0.1956438713022077, - "velocityX": 0.6499021813650045, - "velocityY": -0.03028778845710075, - "timestamp": 3.4787783509848826 - }, - { - "x": 7.354804515838622, - "y": 6.366209983825684, - "heading": -4.8930235830534376e-17, - "angularVelocity": -0.09937861010260368, - "velocityX": 0.32492212920904096, - "velocityY": -0.015142527357507724, - "timestamp": 3.5623356458136524 - }, - { - "x": 7.354804515838623, - "y": 6.366209983825684, - "heading": -2.4216548868392683e-17, - "angularVelocity": 5.9907409116151e-18, - "velocityX": -8.630903523575738e-17, - "velocityY": 6.541583629386208e-18, - "timestamp": 3.645892940642422 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.1.traj b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.1.traj index d3c12844..e90ff055 100644 --- a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.1.traj +++ b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.1.traj @@ -5,369 +5,351 @@ "y": 6.664690017700195, "heading": 0.9928942822119464, "angularVelocity": 0, - "velocityX": 2.456521210166087e-32, - "velocityY": -5.061818134196095e-29, + "velocityX": 2.1425070675381243e-37, + "velocityY": 2.00620509434581e-36, "timestamp": 0 }, { - "x": 0.6967277255263876, - "y": 6.6675008319457, - "heading": 0.9828953878926869, - "angularVelocity": -0.13938944197774958, - "velocityX": 0.2752170097278352, - "velocityY": 0.039184115430369315, - "timestamp": 0.07173351280273003 - }, - { - "x": 0.7362120737078192, - "y": 6.6731227516141445, - "heading": 0.9629028188307202, - "angularVelocity": -0.27870612052394816, - "velocityX": 0.5504309860819994, - "velocityY": 0.07837229001553121, - "timestamp": 0.14346702560546007 - }, - { - "x": 0.7954384350293103, - "y": 6.681556356569816, - "heading": 0.9329336821586108, - "angularVelocity": -0.417784317335511, - "velocityX": 0.8256442353927022, - "velocityY": 0.11756854819039611, - "timestamp": 0.2152005384081901 - }, - { - "x": 0.8744068559927786, - "y": 6.692802561177285, - "heading": 0.8930142547719868, - "angularVelocity": -0.5564962014627703, - "velocityX": 1.1008581329627185, - "velocityY": 0.15677755305130064, - "timestamp": 0.28693405121092014 - }, - { - "x": 0.9731173912556694, - "y": 6.706862646881597, - "heading": 0.8431760672320958, - "angularVelocity": -0.6947685342560019, - "velocityX": 1.3760727924840699, - "velocityY": 0.19600442187834377, - "timestamp": 0.3586675640136502 - }, - { - "x": 1.0915699973864605, - "y": 6.72373826040205, - "heading": 0.7834509327491487, - "angularVelocity": -0.832597375451512, - "velocityX": 1.6512868465489152, - "velocityY": 0.2352542467718665, - "timestamp": 0.43040107681638023 - }, - { - "x": 1.2297644280001299, - "y": 6.743431356930112, - "heading": 0.7138654539660069, - "angularVelocity": -0.9700553628884108, - "velocityX": 1.9264974658869234, - "velocityY": 0.2745313279837331, - "timestamp": 0.5021345896191103 - }, - { - "x": 1.3877001578372599, - "y": 6.765944077192238, - "heading": 0.6344360529195374, - "angularVelocity": -1.1072844191579285, - "velocityX": 2.20170076285362, - "velocityY": 0.3138382519616173, - "timestamp": 0.5738681024218403 - }, - { - "x": 1.565376370208164, - "y": 6.791278571412394, - "heading": 0.545166048889513, - "angularVelocity": -1.2444672030567556, - "velocityX": 2.476892674769556, - "velocityY": 0.3531751511051906, - "timestamp": 0.6456016152245703 - }, - { - "x": 1.7627920329701503, - "y": 6.819436846094517, - "heading": 0.4460464917361558, - "angularVelocity": -1.381774756252758, - "velocityX": 2.7520701981153106, - "velocityY": 0.3925400218865401, - "timestamp": 0.7173351280273003 - }, - { - "x": 1.9404742499696706, - "y": 6.844889308454019, - "heading": 0.35698857434522513, - "angularVelocity": -1.241510611911704, - "velocityX": 2.4769763814130292, - "velocityY": 0.35481968414999915, - "timestamp": 0.7890686408300303 - }, - { - "x": 2.098418416235195, - "y": 6.8675162488116115, - "heading": 0.277805044936203, - "angularVelocity": -1.103856848738075, - "velocityX": 2.201818369967191, - "velocityY": 0.3154305354505728, - "timestamp": 0.8608021536327602 - }, - { - "x": 2.2366248722942026, - "y": 6.887316924283159, - "heading": 0.20850199362833047, - "angularVelocity": -0.9661181863426805, - "velocityX": 1.9266651056396258, - "velocityY": 0.2760310305869847, - "timestamp": 0.9325356664354902 - }, - { - "x": 2.3550937746268814, - "y": 6.9042907996604885, - "heading": 0.14908158317430456, - "angularVelocity": -0.8283493741238005, - "velocityX": 1.6515140227218135, - "velocityY": 0.2366240646620762, - "timestamp": 1.0042691792382203 - }, - { - "x": 2.453825154537734, - "y": 6.918437433437714, - "heading": 0.09954309354100455, - "angularVelocity": -0.6905905995159123, - "velocityX": 1.3763633762039458, - "velocityY": 0.19721094393093683, - "timestamp": 1.0760026920409504 - }, - { - "x": 2.5328189743834146, - "y": 6.929756449824835, - "heading": 0.05988427758276665, - "angularVelocity": -0.5528631512095152, - "velocityX": 1.101212204919556, - "velocityY": 0.1577925845048924, - "timestamp": 1.1477362048436806 - }, - { - "x": 2.5920751704296263, - "y": 6.938247542788083, - "heading": 0.030102483585013936, - "angularVelocity": -0.41517266909175576, - "velocityX": 0.8260601451480881, - "velocityY": 0.11836995881959977, - "timestamp": 1.2194697176464107 - }, - { - "x": 2.6315936809069487, - "y": 6.943910491933908, - "heading": 0.010195398523530809, - "angularVelocity": -0.27751443165456446, - "velocityX": 0.550907224280912, - "velocityY": 0.07894426074125552, - "timestamp": 1.2912032304491408 - }, - { - "x": 2.6513744618221935, - "y": 6.946745178456401, - "heading": 0.00016145502701412444, - "angularVelocity": -0.13987804413429022, - "velocityX": 0.27575369012733186, - "velocityY": 0.03951690645092257, - "timestamp": 1.3629367432518709 + "x": 0.6967295535333877, + "y": 6.66750179345797, + "heading": 0.982893163249758, + "angularVelocity": -0.14737604826251527, + "velocityX": 0.29094834799603586, + "velocityY": 0.04143420364739335, + "timestamp": 0.06786122358481142 + }, + { + "x": 0.7362175895643961, + "y": 6.673125696218821, + "heading": 0.9628974920292898, + "angularVelocity": -0.2946553298656345, + "velocityX": 0.5818939586560375, + "velocityY": 0.08287358323008266, + "timestamp": 0.13572244716962284 + }, + { + "x": 0.7954495269492323, + "y": 6.681562379154691, + "heading": 0.9329256679312776, + "angularVelocity": -0.44166347900512215, + "velocityX": 0.8728392188627362, + "velocityY": 0.1243225879257299, + "timestamp": 0.20358367075443426 + }, + { + "x": 0.8744254360510917, + "y": 6.692812849791712, + "heading": 0.8930052032103389, + "angularVelocity": -0.5882662087730711, + "velocityX": 1.1637855159383756, + "velocityY": 0.1657864394820339, + "timestamp": 0.2714448943392457 + }, + { + "x": 0.9731453894549041, + "y": 6.7068785148813665, + "heading": 0.843168820166726, + "angularVelocity": -0.7343867441666224, + "velocityX": 1.4547328826223493, + "velocityY": 0.20727102086621554, + "timestamp": 0.33930611792405707 + }, + { + "x": 1.0916093532691815, + "y": 6.7237612036761645, + "heading": 0.7834495219946941, + "angularVelocity": -0.8800209459441316, + "velocityX": 1.7456797498828438, + "velocityY": 0.2487825580347626, + "timestamp": 0.40716734150886846 + }, + { + "x": 1.2298170767085217, + "y": 6.743463170039788, + "heading": 0.7138752049500412, + "angularVelocity": -1.025244069725628, + "velocityX": 2.0366229215807086, + "velocityY": 0.29032730803917456, + "timestamp": 0.47502856509367986 + }, + { + "x": 1.3877679992818084, + "y": 6.765987144948878, + "heading": 0.6344639927011461, + "angularVelocity": -1.1702001239286361, + "velocityX": 2.3275578339061793, + "velocityY": 0.3319123015950357, + "timestamp": 0.5428897886784912 + }, + { + "x": 1.5654611541888466, + "y": 6.791337011948423, + "heading": 0.5452227484246948, + "angularVelocity": -1.3150550426624645, + "velocityX": 2.618478499506586, + "velocityY": 0.3735545228986894, + "timestamp": 0.6107510122633026 + }, + { + "x": 1.7628875214635322, + "y": 6.819592757066269, + "heading": 0.4462737595759067, + "angularVelocity": -1.4581079388455742, + "velocityX": 2.909266247284013, + "velocityY": 0.41637541478357637, + "timestamp": 0.678612235848114 + }, + { + "x": 1.9405727781814897, + "y": 6.845027020375549, + "heading": 0.357188812683824, + "angularVelocity": -1.3127518512946696, + "velocityX": 2.6183621121403804, + "velocityY": 0.3747981831994576, + "timestamp": 0.7464734594329254 + }, + { + "x": 2.098517679535123, + "y": 6.867636881191052, + "heading": 0.2779750280562074, + "angularVelocity": -1.1672908392024057, + "velocityX": 2.3274691054787353, + "velocityY": 0.3331779125268215, + "timestamp": 0.8143346830177368 + }, + { + "x": 2.236722632634811, + "y": 6.887421048029417, + "heading": 0.20863780165516146, + "angularVelocity": -1.0217503124503762, + "velocityX": 2.036582097977683, + "velocityY": 0.29153861061228065, + "timestamp": 0.8821959066025482 + }, + { + "x": 2.3551878284960197, + "y": 6.9043786966768, + "heading": 0.1491789973537457, + "angularVelocity": -0.8761823197470863, + "velocityX": 1.745697905272119, + "velocityY": 0.24988716311885253, + "timestamp": 0.9500571301873596 + }, + { + "x": 2.4539133200632923, + "y": 6.918509209574482, + "heading": 0.09959774943286472, + "angularVelocity": -0.7306270842422328, + "velocityX": 1.454814492755025, + "velocityY": 0.20822661530737346, + "timestamp": 1.017918353772171 + }, + { + "x": 2.5328990844805293, + "y": 6.929812092655753, + "heading": 0.059891727352470836, + "angularVelocity": -0.5851061914728107, + "velocityX": 1.163930743431445, + "velocityY": 0.1665587869506308, + "timestamp": 1.0857795773569825 + }, + { + "x": 2.592145068506356, + "y": 6.938286954912776, + "heading": 0.030058218690118557, + "angularVelocity": -0.43962526884100517, + "velocityX": 0.8730462095453193, + "velocityY": 0.12488519671960763, + "timestamp": 1.153640800941794 + }, + { + "x": 2.63165121786692, + "y": 6.943933511859112, + "heading": 0.010094853186468074, + "angularVelocity": -0.2941792742463702, + "velocityX": 0.582160875882081, + "velocityY": 0.08320741430898287, + "timestamp": 1.2215020245266055 }, { "x": 2.6514174938201904, "y": 6.946751594543457, "heading": 0, - "angularVelocity": -0.0022507610818009623, - "velocityX": 0.0005998861532762668, - "velocityY": 0.00008944325609789668, - "timestamp": 1.434670256054601 - }, - { - "x": 2.631713436254265, - "y": 6.943928504258672, - "heading": 0.00971593718624953, - "angularVelocity": 0.13541279143473714, - "velocityX": -0.27461904975679574, - "velocityY": -0.03934592501075656, - "timestamp": 1.506420768370774 - }, - { - "x": 2.592262356755882, - "y": 6.938276121500362, - "heading": 0.029312137508313962, - "angularVelocity": 0.27311582404564183, - "velocityX": -0.5498369025337515, - "velocityY": -0.07877829117207064, - "timestamp": 1.578171280686947 - }, - { - "x": 2.533064330899613, - "y": 6.929794745308614, - "heading": 0.058791626722930285, - "angularVelocity": 0.41086102718133133, - "velocityX": -0.8250537020140929, - "velocityY": -0.11820648955702064, - "timestamp": 1.64992179300312 - }, - { - "x": 2.4541194029599502, - "y": 6.918484740607142, - "heading": 0.09815683939208576, - "angularVelocity": 0.5486401612526653, - "velocityX": -1.1002698844327412, - "velocityY": -0.15762960193345896, - "timestamp": 1.721672305319293 - }, - { - "x": 2.355427542032357, - "y": 6.9043465299042746, - "heading": 0.1474084549881005, - "angularVelocity": 0.6864287652544176, - "velocityX": -1.3754864975735484, - "velocityY": -0.19704682573406979, - "timestamp": 1.793422817635466 - }, - { - "x": 2.23698858427003, - "y": 6.887380609317409, - "heading": 0.20654397577631692, - "angularVelocity": 0.8241825578726235, - "velocityX": -1.650705394277179, - "velocityY": -0.23645713505578408, - "timestamp": 1.865173329951639 - }, - { - "x": 2.098802169913165, - "y": 6.86758760630337, - "heading": 0.27555650233579015, - "angularVelocity": 0.9618401920015576, - "velocityX": -1.9259293050189108, - "velocityY": -0.2758586993970149, - "timestamp": 1.9369238422678121 - }, - { - "x": 1.9408676962603424, - "y": 6.844968393093841, - "heading": 0.3544346454359525, - "angularVelocity": 1.0993390923447317, - "velocityX": -2.2011616157204346, - "velocityY": -0.3152481072856972, - "timestamp": 2.0086743545839854 - }, - { - "x": 1.7631843231894517, - "y": 6.819524255776015, - "heading": 0.44316502681503017, - "angularVelocity": 1.2366515374122595, - "velocityX": -2.4764056353572657, - "velocityY": -0.35461959074183413, - "timestamp": 2.0804248669001586 - }, - { - "x": 1.5657510840322728, - "y": 6.791257095186479, - "heading": 0.5417391237624924, - "angularVelocity": 1.373845199915075, - "velocityX": -2.751663127625576, - "velocityY": -0.3939645818900745, - "timestamp": 2.152175379216332 - }, - { - "x": 1.387996398598156, - "y": 6.765933063404474, - "heading": 0.6316668804185087, - "angularVelocity": 1.2533395757259034, - "velocityX": -2.477399529588293, - "velocityY": -0.352945658067143, - "timestamp": 2.223925891532505 - }, - { - "x": 1.2299927628795557, - "y": 6.743427858422989, - "heading": 0.7117267031520905, - "angularVelocity": 1.115808377635633, - "velocityX": -2.202125540862631, - "velocityY": -0.3136591539003019, - "timestamp": 2.2956764038486783 - }, - { - "x": 1.091740243274482, - "y": 6.723739456363059, - "heading": 0.7818852162777382, - "angularVelocity": 0.9778120165089851, - "velocityX": -1.9268506267358863, - "velocityY": -0.2744008569054998, - "timestamp": 2.3674269161648516 - }, - { - "x": 0.9732386614039245, - "y": 6.706866137144222, - "heading": 0.842102674742542, - "angularVelocity": 0.8392617213147597, - "velocityX": -1.6515781987100508, - "velocityY": -0.2351665329867072, - "timestamp": 2.439177428481025 - }, - { - "x": 0.8744877098342877, - "y": 6.692806537214817, - "heading": 0.8923385972076003, - "angularVelocity": 0.7001472303020632, - "velocityX": -1.3763100554424192, - "velocityY": -0.19595121313677477, - "timestamp": 2.510927940797198 - }, - { - "x": 0.7954870665575497, - "y": 6.68155965440474, - "heading": 0.9325570015240306, - "angularVelocity": 0.5605312495048624, - "velocityX": -1.1010464000594427, - "velocityY": -0.15674986071479513, - "timestamp": 2.5826784531133713 - }, - { - "x": 0.736236493451904, - "y": 6.673124818793258, - "heading": 0.962730596118369, - "angularVelocity": 0.42053490106098795, - "velocityX": -0.8257860634235991, - "velocityY": -0.11755784510057492, - "timestamp": 2.6544289654295445 - }, - { - "x": 0.6967359103772861, - "y": 6.667501649956908, - "heading": 0.9828437467864926, - "angularVelocity": 0.28032065585715155, - "velocityX": -0.5505268437144837, - "velocityY": -0.07837113151770157, - "timestamp": 2.726179477745718 + "angularVelocity": -0.14875731166049128, + "velocityX": 0.2912749713180046, + "velocityY": 0.041527142239385896, + "timestamp": 1.289363248111417 + }, + { + "x": 2.649146332814054, + "y": 6.946410455915093, + "heading": 0.0008990727847708372, + "angularVelocity": 0.011985225672082133, + "velocityX": -0.030276055128411465, + "velocityY": -0.004547600056030398, + "timestamp": 1.3643783383040946 + }, + { + "x": 2.62275408560099, + "y": 6.9426132004627625, + "heading": 0.013859521326443051, + "angularVelocity": 0.17277121854260333, + "velocityX": -0.35182584124441, + "velocityY": -0.050619887846258024, + "timestamp": 1.4393934284967722 + }, + { + "x": 2.572240863242063, + "y": 6.93536015641962, + "heading": 0.03888546297295733, + "angularVelocity": 0.3336120983422759, + "velocityX": -0.6733741468440916, + "velocityY": -0.09668780007480728, + "timestamp": 1.5144085186894498 + }, + { + "x": 2.4976067609609713, + "y": 6.924651766873113, + "heading": 0.07598096942338527, + "angularVelocity": 0.4945072565419495, + "velocityX": -0.994921183049867, + "velocityY": -0.14274980565911422, + "timestamp": 1.5894236088821274 + }, + { + "x": 2.398851800518621, + "y": 6.910488564953806, + "heading": 0.12514851990507653, + "angularVelocity": 0.6554354644565975, + "velocityX": -1.3164679291686043, + "velocityY": -0.18880470426588675, + "timestamp": 1.664438699074805 + }, + { + "x": 2.275975845765142, + "y": 6.89287117810912, + "heading": 0.18638682928509862, + "angularVelocity": 0.8163465407124133, + "velocityX": -1.6380164902537586, + "velocityY": -0.23485123858993803, + "timestamp": 1.7394537892674826 + }, + { + "x": 2.1289785005722504, + "y": 6.871800394460987, + "heading": 0.2596886166153821, + "angularVelocity": 0.9771605571893133, + "velocityX": -1.95957033198688, + "velocityY": -0.28088726673543657, + "timestamp": 1.8144688794601602 + }, + { + "x": 1.9578590220057892, + "y": 6.847277322910046, + "heading": 0.34503981582540255, + "angularVelocity": 1.1377870637866898, + "velocityX": -2.2811340775161044, + "velocityY": -0.3269085125133225, + "timestamp": 1.8894839696528378 + }, + { + "x": 1.7626163173700675, + "y": 6.8193036552908985, + "heading": 0.4424228597913738, + "angularVelocity": 1.298179389184776, + "velocityX": -2.602712389390415, + "velocityY": -0.3729072050343056, + "timestamp": 1.9644990598455154 + }, + { + "x": 1.5454887933501882, + "y": 6.788364934906598, + "heading": 0.5521114777288451, + "angularVelocity": 1.4622207032709544, + "velocityX": -2.8944512825643915, + "velocityY": -0.4124332891533426, + "timestamp": 2.039514150038193 + }, + { + "x": 1.3524867399067801, + "y": 6.760871330718944, + "heading": 0.6497679920992822, + "angularVelocity": 1.3018249277525966, + "velocityX": -2.5728430499473998, + "velocityY": -0.3665076468885929, + "timestamp": 2.114529240230871 + }, + { + "x": 1.1836105740084832, + "y": 6.736819962879357, + "heading": 0.7353592838772256, + "angularVelocity": 1.140987654058677, + "velocityX": -2.251229258866918, + "velocityY": -0.3206203948806917, + "timestamp": 2.1895443304235487 + }, + { + "x": 1.0388602648983114, + "y": 6.7162082631102376, + "heading": 0.8088372428736526, + "angularVelocity": 0.9795090402170741, + "velocityX": -1.9296158777970915, + "velocityY": -0.2747673796855824, + "timestamp": 2.2645594206162265 + }, + { + "x": 0.9182354949315064, + "y": 6.6990341480262074, + "heading": 0.8701477848986364, + "angularVelocity": 0.8173094489056358, + "velocityX": -1.6080067311387376, + "velocityY": -0.22894213737419178, + "timestamp": 2.3395745108089043 + }, + { + "x": 0.8217358494246105, + "y": 6.685296075556227, + "heading": 0.919240073281317, + "angularVelocity": 0.6544321716682079, + "velocityX": -1.286403112480899, + "velocityY": -0.18313745187394112, + "timestamp": 2.414589601001582 + }, + { + "x": 0.7493609915443247, + "y": 6.674993018423988, + "heading": 0.956074222977567, + "angularVelocity": 0.49102320082053863, + "velocityX": -0.9648039840302745, + "velocityY": -0.13734646063578734, + "timestamp": 2.48960469119426 + }, + { + "x": 0.7011107973318162, + "y": 6.66812439818581, + "heading": 0.9806268225378217, + "angularVelocity": 0.32730214010529163, + "velocityX": -0.6432065080316098, + "velocityY": -0.09156318042856706, + "timestamp": 2.564619781386938 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": 0.14007614861882017, - "velocityX": -0.2752658776108563, - "velocityY": -0.0391862324932835, - "timestamp": 2.797929990061891 + "angularVelocity": 0.16353322568319786, + "velocityX": -0.32160668782044516, + "velocityY": -0.04578252824590232, + "timestamp": 2.6396348715796156 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": 0, - "velocityX": -1.632165481129405e-29, - "velocityY": 0, - "timestamp": 2.8696805023780643 + "angularVelocity": -9.098710086763465e-32, + "velocityX": -1.2738111607141702e-32, + "velocityY": 8.271252813231263e-33, + "timestamp": 2.7146499617722935 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.2.traj b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.2.traj index 03b8354a..4123f0eb 100644 --- a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.2.traj +++ b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.2.traj @@ -4,658 +4,658 @@ "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": 0, - "velocityX": -1.632165481129405e-29, - "velocityY": 0, + "angularVelocity": -9.098710086763465e-32, + "velocityX": -1.2738111607141702e-32, + "velocityY": 8.271252813231263e-33, "timestamp": 0 }, { - "x": 0.6961023495919454, - "y": 6.667365584217812, - "heading": 0.984880267445541, - "angularVelocity": -0.11368287336653356, - "velocityX": 0.27118304316961456, - "velocityY": 0.03795427117017962, - "timestamp": 0.07049447756802163 - }, - { - "x": 0.7343390561697011, - "y": 6.672718275572268, - "heading": 0.9689850788648127, - "angularVelocity": -0.2254813302984037, - "velocityX": 0.5424071203430141, - "velocityY": 0.07593064789069806, - "timestamp": 0.14098895513604326 - }, - { - "x": 0.7916989777682265, - "y": 6.680749944426173, - "heading": 0.945369919685869, - "angularVelocity": -0.33499303766251487, - "velocityX": 0.8136796466528573, - "velocityY": 0.11393330557213717, - "timestamp": 0.2114834327040649 - }, - { - "x": 0.8681861161076095, - "y": 6.691462782987562, - "heading": 0.9142283558404658, - "angularVelocity": -0.441758913885903, - "velocityX": 1.0850089393964117, - "velocityY": 0.1519670608389448, - "timestamp": 0.2819779102720865 - }, - { - "x": 0.9638051470354019, - "y": 6.704859377756673, - "heading": 0.875792088105272, - "angularVelocity": -0.5452379968077065, - "velocityX": 1.3564045614144413, - "velocityY": 0.19003750692646, - "timestamp": 0.35247238784010815 - }, - { - "x": 1.078561545508893, - "y": 6.720942777481531, - "heading": 0.830339664632417, - "angularVelocity": -0.6447657325922732, - "velocityX": 1.627877848484808, - "velocityY": 0.2281512010545656, - "timestamp": 0.4229668654081298 - }, - { - "x": 1.2124617638424005, - "y": 6.739716584680947, - "heading": 0.7782100744037193, - "angularVelocity": -0.7394847373455183, - "velocityX": 1.8994426649137832, - "velocityY": 0.26631599874331907, - "timestamp": 0.4934613429761514 - }, - { - "x": 1.3655134797863553, - "y": 6.761185092164687, - "heading": 0.7198245324523541, - "angularVelocity": -0.8282285927295209, - "velocityX": 2.1711163941355873, - "velocityY": 0.30454169212084536, - "timestamp": 0.563955820544173 - }, - { - "x": 1.5377259215824886, - "y": 6.785353503086406, - "heading": 0.6557224279689098, - "angularVelocity": -0.909320938247837, - "velocityX": 2.4429210306575064, - "velocityY": 0.3428411948779657, - "timestamp": 0.6344502981121947 - }, - { - "x": 1.7291102325420633, - "y": 6.812228299208335, - "heading": 0.5866232271908158, - "angularVelocity": -0.9802072894492893, - "velocityX": 2.714883740714364, - "velocityY": 0.3812326447273405, - "timestamp": 0.7049447756802163 - }, - { - "x": 1.9396796543838148, - "y": 6.8418178634961455, - "heading": 0.5135407387337584, - "angularVelocity": -1.0367122500700692, - "velocityX": 2.987034291282877, - "velocityY": 0.4197430112062252, - "timestamp": 0.7754392532482379 - }, - { - "x": 2.1694484773986367, - "y": 6.874133526026527, - "heading": 0.4380194057033538, - "angularVelocity": -1.0713085001237521, - "velocityX": 3.259387556892149, - "velocityY": 0.45841410058255366, - "timestamp": 0.8459337308162596 - }, - { - "x": 2.418423965072986, - "y": 6.9091911928007494, - "heading": 0.362723869672428, - "angularVelocity": -1.0681054549027866, - "velocityX": 3.531843858749197, - "velocityY": 0.4973108246726942, - "timestamp": 0.9164282083842812 - }, - { - "x": 2.6865429147293822, - "y": 6.947011884235923, - "heading": 0.29352452528672673, - "angularVelocity": -0.9816278774309596, - "velocityX": 3.803403598497242, - "velocityY": 0.5365057340651915, - "timestamp": 0.9869226859523028 - }, - { - "x": 2.9720561979068543, - "y": 6.987623777247046, - "heading": 0.25938945355425436, - "angularVelocity": -0.48422334500649106, - "velocityX": 4.050151061861196, - "velocityY": 0.5761003473206351, - "timestamp": 1.0574171635203244 - }, - { - "x": 3.2671083841789206, - "y": 7.028939851633195, - "heading": 0.25938943415021554, - "angularVelocity": -2.752561543209048e-7, - "velocityX": 4.185465251336388, - "velocityY": 0.5860895180942723, - "timestamp": 1.127911641088346 - }, - { - "x": 3.562160580898253, - "y": 7.070255851413062, - "heading": 0.25938941474662836, - "angularVelocity": -2.7524974860351036e-7, - "velocityX": 4.185465399536173, - "velocityY": 0.5860884597662348, - "timestamp": 1.1984061186563677 - }, - { - "x": 3.8572127776182663, - "y": 7.111571851188067, - "heading": 0.25938939534304106, - "angularVelocity": -2.7524974884736384e-7, - "velocityX": 4.185465399545831, - "velocityY": 0.5860884596972535, - "timestamp": 1.2689005962243893 - }, - { - "x": 4.15226497433828, - "y": 7.1528878509630704, - "heading": 0.25938937593945377, - "angularVelocity": -2.752497497523569e-7, - "velocityX": 4.185465399545832, - "velocityY": 0.5860884596972492, - "timestamp": 1.339395073792411 - }, - { - "x": 4.447317171058294, - "y": 7.194203850738075, - "heading": 0.25938935653586653, - "angularVelocity": -2.7524974896088886e-7, - "velocityX": 4.185465399545833, - "velocityY": 0.5860884596972492, - "timestamp": 1.4098895513604326 - }, - { - "x": 4.742369367778308, - "y": 7.2355198505130724, - "heading": 0.2593893371322792, - "angularVelocity": -2.752497503383158e-7, - "velocityX": 4.185465399545844, - "velocityY": 0.5860884596971616, - "timestamp": 1.4803840289284542 - }, - { - "x": 5.037421564511595, - "y": 7.276835850193286, - "heading": 0.2593893177286919, - "angularVelocity": -2.7524974902410266e-7, - "velocityX": 4.185465399734123, - "velocityY": 0.5860884583526031, - "timestamp": 1.5508785064964759 - }, - { - "x": 5.3324739648733965, - "y": 7.318150395663973, - "heading": 0.2593892983250951, - "angularVelocity": -2.7524988424278287e-7, - "velocityX": 4.185468288308109, - "velocityY": 0.5860678296512251, - "timestamp": 1.6213729840644975 - }, - { - "x": 5.628942809727294, - "y": 7.346853994253202, - "heading": 0.25912444072738805, - "angularVelocity": -0.0037571396631956044, - "velocityX": 4.205561273474639, - "velocityY": 0.4071751373933052, - "timestamp": 1.691867461632519 - }, - { - "x": 5.9078202008377065, - "y": 7.3726601599906, - "heading": 0.2260522693823342, - "angularVelocity": -0.4691455626881114, - "velocityX": 3.9560175595502307, - "velocityY": 0.3660735794870859, - "timestamp": 1.7623619392005407 - }, - { - "x": 6.167565330820035, - "y": 7.395632554305088, - "heading": 0.18684536926234946, - "angularVelocity": -0.5561698089350797, - "velocityX": 3.684616709609561, - "velocityY": 0.32587509131225256, - "timestamp": 1.8328564167685624 - }, - { - "x": 6.408110614275644, - "y": 7.415818749602641, - "heading": 0.14706352599281394, - "angularVelocity": -0.5643256697824207, - "velocityX": 3.412257126432383, - "velocityY": 0.28635144189947087, - "timestamp": 1.903350894336584 - }, - { - "x": 6.629457805462712, - "y": 7.433245452372472, - "heading": 0.10922390474114492, - "angularVelocity": -0.536774263135107, - "velocityX": 3.1399224282992484, - "velocityY": 0.2472066376123601, - "timestamp": 1.9738453719046056 - }, - { - "x": 6.831617157798908, - "y": 7.447929110099442, - "heading": 0.07477835738023303, - "angularVelocity": -0.48862759962544244, - "velocityX": 2.867733180108019, - "velocityY": 0.2082951492590427, - "timestamp": 2.0443398494726273 - }, - { - "x": 7.014599005404042, - "y": 7.459880739385679, - "heading": 0.04467522695346215, - "angularVelocity": -0.427028207957479, - "velocityX": 2.59569052665965, - "velocityY": 0.169539937007193, - "timestamp": 2.114834327040649 - }, - { - "x": 7.178412353011271, - "y": 7.469108205463054, - "heading": 0.019583414088526187, - "angularVelocity": -0.35594012085165694, - "velocityX": 2.3237756099286244, - "velocityY": 0.1308962970676859, - "timestamp": 2.1853288046086705 + "x": 0.6985020843742907, + "y": 6.667569321822967, + "heading": 0.9798246448067045, + "angularVelocity": -0.18429871952070437, + "velocityX": 0.3034123592993086, + "velocityY": 0.04060189632535011, + "timestamp": 0.0709155084703319 + }, + { + "x": 0.7415443471125377, + "y": 6.673333307735536, + "heading": 0.9539749013573504, + "angularVelocity": -0.36451467396822734, + "velocityX": 0.6069513378199061, + "velocityY": 0.081279624681546, + "timestamp": 0.1418310169406638 + }, + { + "x": 0.8061231921601518, + "y": 6.681988659505296, + "heading": 0.9157093279961269, + "angularVelocity": -0.5395938658076773, + "velocityX": 0.9106448848862336, + "velocityY": 0.12205160699625474, + "timestamp": 0.2127465254109957 + }, + { + "x": 0.8922518573326212, + "y": 6.693543414119111, + "heading": 0.8654790283080297, + "angularVelocity": -0.7083119161320186, + "velocityX": 1.2145251021996393, + "velocityY": 0.16293692117639821, + "timestamp": 0.2836620338813276 + }, + { + "x": 0.9999464058047857, + "y": 6.708007002320034, + "heading": 0.8038456081958845, + "angularVelocity": -0.8691105999462724, + "velocityX": 1.5186318309657076, + "velocityY": 0.20395522097926097, + "timestamp": 0.3545775423516595 + }, + { + "x": 1.1292266926067027, + "y": 6.725390389005233, + "heading": 0.7315277652632032, + "angularVelocity": -1.0197747219557198, + "velocityX": 1.8230185412264566, + "velocityY": 0.2451281399536478, + "timestamp": 0.4254930508219914 + }, + { + "x": 1.280117879093583, + "y": 6.7457065764596775, + "heading": 0.6494907937479546, + "angularVelocity": -1.1568269520279806, + "velocityX": 2.127760059000449, + "velocityY": 0.28648440789144525, + "timestamp": 0.4964085592923233 + }, + { + "x": 1.452652377278095, + "y": 6.768971929861101, + "heading": 0.5591180429519423, + "angularVelocity": -1.2743721753587984, + "velocityX": 2.4329586278958, + "velocityY": 0.3280714459116738, + "timestamp": 0.5673240677626552 + }, + { + "x": 1.6468709656789369, + "y": 6.795208953072923, + "heading": 0.46255966504991763, + "angularVelocity": -1.361597483890574, + "velocityX": 2.738732226422582, + "velocityY": 0.3699758173883681, + "timestamp": 0.6382395762329871 + }, + { + "x": 1.8628151056303066, + "y": 6.824451100190135, + "heading": 0.3635782548385496, + "angularVelocity": -1.3957653600238602, + "velocityX": 3.045090483158721, + "velocityY": 0.41235193468922193, + "timestamp": 0.709155084703319 + }, + { + "x": 2.100434676341306, + "y": 6.8567478442017284, + "heading": 0.27059443428530877, + "angularVelocity": -1.3111916216766784, + "velocityX": 3.3507419721937035, + "velocityY": 0.4554256848500797, + "timestamp": 0.7800705931736509 + }, + { + "x": 2.35596385739112, + "y": 6.892112273565999, + "heading": 0.23346194806127452, + "angularVelocity": -0.5236158778945916, + "velocityX": 3.603290543375525, + "velocityY": 0.4986839991292765, + "timestamp": 0.8509861016439828 + }, + { + "x": 2.621781213038304, + "y": 6.927777327664925, + "heading": 0.23346192227272974, + "angularVelocity": -3.6365169422849345e-7, + "velocityX": 3.7483670551187007, + "velocityY": 0.5029231950560806, + "timestamp": 0.9219016101143147 + }, + { + "x": 2.887598578486195, + "y": 6.963442308718448, + "heading": 0.23346189648452345, + "angularVelocity": -3.636469208413295e-7, + "velocityX": 3.748367193321304, + "velocityY": 0.502922165021807, + "timestamp": 0.9928171185846466 + }, + { + "x": 3.153415943934485, + "y": 6.999107289769, + "heading": 0.23346187069631716, + "angularVelocity": -3.636469205820025e-7, + "velocityX": 3.7483671933269203, + "velocityY": 0.5029221649799431, + "timestamp": 1.0637326270549785 + }, + { + "x": 3.4192333093827747, + "y": 7.034772270819553, + "heading": 0.2334618449081109, + "angularVelocity": -3.6364692054215583e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799415, + "timestamp": 1.1346481355253104 + }, + { + "x": 3.6850506748310643, + "y": 7.070437251870106, + "heading": 0.23346181911990457, + "angularVelocity": -3.6364692122502775e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799416, + "timestamp": 1.2055636439956423 + }, + { + "x": 3.950868040279354, + "y": 7.106102232920659, + "heading": 0.2334617933316983, + "angularVelocity": -3.636469204641407e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799416, + "timestamp": 1.2764791524659742 + }, + { + "x": 4.216685405727644, + "y": 7.141767213971211, + "heading": 0.23346176754349196, + "angularVelocity": -3.6364692150109834e-7, + "velocityX": 3.7483671933269203, + "velocityY": 0.5029221649799415, + "timestamp": 1.347394660936306 + }, + { + "x": 4.482502771175934, + "y": 7.1774321950217645, + "heading": 0.2334617417552857, + "angularVelocity": -3.636469207728054e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799417, + "timestamp": 1.418310169406638 + }, + { + "x": 4.7483201366242245, + "y": 7.213097176072317, + "heading": 0.2334617159670794, + "angularVelocity": -3.636469206602435e-7, + "velocityX": 3.7483671933269203, + "velocityY": 0.5029221649799417, + "timestamp": 1.48922567787697 + }, + { + "x": 5.014137502072515, + "y": 7.2487621571228695, + "heading": 0.2334616901788731, + "angularVelocity": -3.6364692079095153e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799416, + "timestamp": 1.5601411863473018 + }, + { + "x": 5.279954867520805, + "y": 7.284427138173423, + "heading": 0.23346166439066685, + "angularVelocity": -3.636469205268936e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799417, + "timestamp": 1.6310566948176337 + }, + { + "x": 5.545772232969095, + "y": 7.320092119223974, + "heading": 0.23346163860246044, + "angularVelocity": -3.6364692217914213e-7, + "velocityX": 3.748367193326924, + "velocityY": 0.5029221649799175, + "timestamp": 1.7019722032879656 + }, + { + "x": 5.81158959842306, + "y": 7.355757100232221, + "heading": 0.23346161281425412, + "angularVelocity": -3.636469211715136e-7, + "velocityX": 3.748367193406962, + "velocityY": 0.5029221643833769, + "timestamp": 1.7728877117582975 + }, + { + "x": 6.077407103529163, + "y": 7.391421040372378, + "heading": 0.23346158702575517, + "angularVelocity": -3.63651047258994e-7, + "velocityX": 3.748369162681932, + "velocityY": 0.5029074868027953, + "timestamp": 1.8438032202286294 + }, + { + "x": 6.338444265915037, + "y": 7.413466456985474, + "heading": 0.21125647663717811, + "angularVelocity": -0.3131206539662162, + "velocityX": 3.680960173825454, + "velocityY": 0.310868766065739, + "timestamp": 1.9147187286989613 + }, + { + "x": 6.578740605850335, + "y": 7.432100572083313, + "heading": 0.16288336837633732, + "angularVelocity": -0.6821231251705402, + "velocityX": 3.388487865610223, + "velocityY": 0.26276502135825913, + "timestamp": 1.9856342371692932 + }, + { + "x": 6.797330438016248, + "y": 7.447554226429222, + "heading": 0.11319688856078455, + "angularVelocity": -0.7006433555551449, + "velocityX": 3.082398150714271, + "velocityY": 0.21791642870860864, + "timestamp": 2.056549745639625 + }, + { + "x": 6.99422444679472, + "y": 7.45992830223967, + "heading": 0.06789887708325426, + "angularVelocity": -0.638760300174411, + "velocityX": 2.7764590993639087, + "velocityY": 0.1744904052351846, + "timestamp": 2.127465254109957 + }, + { + "x": 7.169459125576906, + "y": 7.4692727675317165, + "heading": 0.029649915927990267, + "angularVelocity": -0.539359612309142, + "velocityX": 2.4710346518279107, + "velocityY": 0.13176899515507345, + "timestamp": 2.198380762580289 }, { "x": 7.323064804077148, "y": 7.475617408752441, - "heading": 2.960760167883615e-32, - "angularVelocity": -0.2778006840270544, - "velocityX": 2.051968552094028, - "velocityY": 0.09233635759774637, - "timestamp": 2.255823282176692 - }, - { - "x": 7.4453229174359326, - "y": 7.479377686242559, - "heading": -0.013449699218163923, - "angularVelocity": -0.19675250253186094, - "velocityX": 1.7884853310086555, - "velocityY": 0.055008219469756155, - "timestamp": 2.3241817479907976 - }, - { - "x": 7.549531921420882, - "y": 7.480799104036024, - "heading": -0.02185072478369098, - "angularVelocity": -0.12289663709499957, - "velocityX": 1.5244491336060073, - "velocityY": 0.020793588278158227, - "timestamp": 2.392540213804903 - }, - { - "x": 7.635663569022224, - "y": 7.480054976594767, - "heading": -0.02560203298601703, - "angularVelocity": -0.05487700985752643, - "velocityX": 1.259999717307419, - "velocityY": -0.010885666206756989, - "timestamp": 2.4608986796190084 - }, - { - "x": 7.703696133906866, - "y": 7.477289120755522, - "heading": -0.025033985130795043, - "angularVelocity": 0.008309839146576752, - "velocityX": 0.9952324715661438, - "velocityY": -0.0404610578412588, - "timestamp": 2.529257145433114 - }, - { - "x": 7.753612501256561, - "y": 7.472622767647336, - "heading": -0.02042473052193369, - "angularVelocity": 0.06742770707282683, - "velocityX": 0.7302148571537285, - "velocityY": -0.06826298765796995, - "timestamp": 2.5976156112472193 - }, - { - "x": 7.785398915822185, - "y": 7.46615947927951, - "heading": -0.012011716304774139, - "angularVelocity": 0.12307201627429676, - "velocityX": 0.46499602042070654, - "velocityY": -0.09454993307489655, - "timestamp": 2.6659740770613247 + "heading": -2.508046324740631e-30, + "angularVelocity": -0.41810200007794424, + "velocityX": 2.166037892325118, + "velocityY": 0.08946761234010188, + "timestamp": 2.269296271050621 + }, + { + "x": 7.442670757639025, + "y": 7.478903848182195, + "heading": -0.0187747254681701, + "angularVelocity": -0.297438409871576, + "velocityX": 1.8948561830587192, + "velocityY": 0.05206538544539075, + "timestamp": 2.332417659698047 + }, + { + "x": 7.5451140174844875, + "y": 7.480040188586648, + "heading": -0.030609169580020613, + "angularVelocity": -0.1874870684158837, + "velocityX": 1.6229563708994625, + "velocityY": 0.01800246206270259, + "timestamp": 2.395539048345473 + }, + { + "x": 7.630363412663352, + "y": 7.479184974890449, + "heading": -0.03600750245908947, + "angularVelocity": -0.08552303735288487, + "velocityX": 1.3505627332580479, + "velocityY": -0.013548714857401807, + "timestamp": 2.458660436992899 + }, + { + "x": 7.698396363869973, + "y": 7.476461754192338, + "heading": -0.03536054977456298, + "angularVelocity": 0.01024934175875415, + "velocityX": 1.0778113831814056, + "velocityY": -0.04314259803938343, + "timestamp": 2.521781825640325 + }, + { + "x": 7.74919584977197, + "y": 7.471969501335234, + "heading": -0.02898050014133947, + "angularVelocity": 0.10107587570456572, + "velocityX": 0.8047903728082677, + "velocityY": -0.0711684732126695, + "timestamp": 2.584903214287751 + }, + { + "x": 7.782748628307226, + "y": 7.465789287122271, + "heading": -0.017122702576199005, + "angularVelocity": 0.1878570452777808, + "velocityX": 0.5315595751917994, + "velocityY": -0.09790998495750272, + "timestamp": 2.648024602935177 }, { "x": 7.799044132232666, "y": 7.457988739013672, "heading": 0, - "angularVelocity": 0.17571658699068654, - "velocityX": 0.19961267778578498, - "velocityY": -0.11952784733433494, - "timestamp": 2.73433254287543 - }, - { - "x": 7.787627597101066, - "y": 7.445185981968696, - "heading": 0.02043091826358884, - "angularVelocity": 0.23842630400056083, - "velocityX": -0.1332295612366535, - "velocityY": -0.14940660051930396, - "timestamp": 2.8200232486439543 - }, - { - "x": 7.7476875027283105, - "y": 7.429823751171704, - "heading": 0.04606214764205663, - "angularVelocity": 0.299113295293718, - "velocityX": -0.466095990394167, - "velocityY": -0.17927534449873192, - "timestamp": 2.9057139544124784 - }, - { - "x": 7.679221480716383, - "y": 7.41190305033205, - "heading": 0.07668903088242017, - "angularVelocity": 0.35741196160871463, - "velocityX": -0.7989900584653148, - "velocityY": -0.20913237531340728, - "timestamp": 2.9914046601810025 - }, - { - "x": 7.582226808615669, - "y": 7.391425060459508, - "heading": 0.11206636753872803, - "angularVelocity": 0.41284916886870227, - "velocityX": -1.1319158971887242, - "velocityY": -0.2389756238892322, - "timestamp": 3.0770953659495266 - }, - { - "x": 7.456700337139081, - "y": 7.36839118812004, - "heading": 0.1518948027840275, - "angularVelocity": 0.4647929421060861, - "velocityX": -1.4648784877052072, - "velocityY": -0.26880245801323005, - "timestamp": 3.1627860717180507 - }, - { - "x": 7.302638403414429, - "y": 7.342803144016317, - "heading": 0.19579992682168443, - "angularVelocity": 0.5123673990532579, - "velocityX": -1.7978838234897747, - "velocityY": -0.2986093284474086, - "timestamp": 3.248476777486575 - }, - { - "x": 7.12003674107026, - "y": 7.314663079535023, - "heading": 0.24329864617596214, - "angularVelocity": 0.5543042145385703, - "velocityX": -2.1309389473046063, - "velocityY": -0.3283910924634972, - "timestamp": 3.334167483255099 - }, - { - "x": 6.908890435767407, - "y": 7.283973843410994, - "heading": 0.29374179011633406, - "angularVelocity": 0.5886652874190753, - "velocityX": -2.4640514208532833, - "velocityY": -0.35813961209433026, - "timestamp": 3.419858189023623 - }, - { - "x": 6.669194114784965, - "y": 7.250739514431078, - "heading": 0.34620821502428967, - "angularVelocity": 0.6122767275331199, - "velocityX": -2.797226593394282, - "velocityY": -0.3878405327841778, - "timestamp": 3.505548894792147 - }, - { - "x": 6.400943140096472, - "y": 7.214966665234931, - "heading": 0.3992869291597751, - "angularVelocity": 0.6194220675328131, - "velocityX": -3.1304558911338245, - "velocityY": -0.41746475157736485, - "timestamp": 3.5912396005606713 - }, - { - "x": 6.104139554987064, - "y": 7.176668008880963, - "heading": 0.4505490205752772, - "angularVelocity": 0.5982223037580753, - "velocityX": -3.4636613439870674, - "velocityY": -0.44694061054210893, - "timestamp": 3.6769303063291954 - }, - { - "x": 5.778829029190598, - "y": 7.135876707023435, - "heading": 0.4948586572551853, - "angularVelocity": 0.5170880118504484, - "velocityX": -3.796333836661644, - "velocityY": -0.4760294770790753, - "timestamp": 3.7626210120977195 - }, - { - "x": 5.425610556727763, - "y": 7.092704257460775, - "heading": 0.5143700783127158, - "angularVelocity": 0.22769588466497723, - "velocityX": -4.122016142765593, - "velocityY": -0.5038171780179022, - "timestamp": 3.8483117178662436 - }, - { - "x": 5.0649174675452295, - "y": 7.060199768590457, - "heading": 0.5143700896531384, - "angularVelocity": 1.323413376854098e-7, - "velocityX": -4.209244000823966, - "velocityY": -0.3793233884444995, - "timestamp": 3.9340024236347677 - }, - { - "x": 4.704224330497994, - "y": 7.027695810865106, - "heading": 0.5143701009934816, - "angularVelocity": 1.3234040971535758e-7, - "velocityX": -4.209244559399155, - "velocityY": -0.3793171900480436, - "timestamp": 4.019693129403292 - }, - { - "x": 4.343531193449039, - "y": 6.995191853158838, - "heading": 0.5143701123338247, - "angularVelocity": 1.3234040961236046e-7, - "velocityX": -4.209244559419223, - "velocityY": -0.3793171898253618, - "timestamp": 4.105383835171816 - }, - { - "x": 3.9828380564000834, - "y": 6.96268789545257, - "heading": 0.5143701236741678, - "angularVelocity": 1.3234040939269306e-7, - "velocityX": -4.209244559419224, - "velocityY": -0.3793171898253537, - "timestamp": 4.19107454094034 - }, - { - "x": 3.622144919351128, - "y": 6.9301839377463015, - "heading": 0.5143701350145109, - "angularVelocity": 1.3234041017948015e-7, - "velocityX": -4.209244559419224, - "velocityY": -0.3793171898253545, - "timestamp": 4.276765246708864 - }, - { - "x": 3.261451782302309, - "y": 6.897679980038524, - "heading": 0.5143701463548541, - "angularVelocity": 1.323404096872133e-7, - "velocityX": -4.209244559417636, - "velocityY": -0.3793171898429695, - "timestamp": 4.362455952477388 - }, - { - "x": 2.90075864904001, - "y": 6.8651759803147625, - "heading": 0.5143701576957388, - "angularVelocity": 1.323467305471061e-7, - "velocityX": -4.209244515229405, - "velocityY": -0.3793176801643355, - "timestamp": 4.448146658245912 - }, - { - "x": 2.5585689575787693, - "y": 6.834269234981658, - "heading": 0.5701176468934943, - "angularVelocity": 0.6505663443634795, - "velocityX": -3.9933116245488067, - "velocityY": -0.3606779178198484, - "timestamp": 4.5338373640144365 - }, - { - "x": 2.2448896661211246, - "y": 6.805966809821415, - "heading": 0.6318849384659164, - "angularVelocity": 0.7208166978957257, - "velocityX": -3.6605987620755966, - "velocityY": -0.3302858216233596, - "timestamp": 4.619528069782961 - }, - { - "x": 1.9597589348765612, - "y": 6.780255296283315, - "heading": 0.6923555719808244, - "angularVelocity": 0.7056848577984265, - "velocityX": -3.327440574649768, - "velocityY": -0.30005020156507956, - "timestamp": 4.705218775551485 - }, - { - "x": 1.7031654665176974, - "y": 6.7571261299629715, - "heading": 0.7492015690369473, - "angularVelocity": 0.6633857960007982, - "velocityX": -2.9944142256453965, - "velocityY": -0.2699145270529415, - "timestamp": 4.790909481320009 - }, - { - "x": 1.4750995700489262, - "y": 6.736574281325872, - "heading": 0.8012653996481951, - "angularVelocity": 0.6075785015925457, - "velocityX": -2.661500969368176, - "velocityY": -0.23983754659012796, - "timestamp": 4.876600187088533 - }, - { - "x": 1.2755541466650087, - "y": 6.718596481742583, - "heading": 0.8478475160673707, - "angularVelocity": 0.5436075709891741, - "velocityX": -2.3286705552752447, - "velocityY": -0.20979871063090086, - "timestamp": 4.962290892857057 - }, - { - "x": 1.1045239164164753, - "y": 6.70319044879611, - "heading": 0.8884763550283885, - "angularVelocity": 0.4741335550528443, - "velocityX": -1.995901757543409, - "velocityY": -0.17978651019737726, - "timestamp": 5.047981598625581 - }, - { - "x": 0.9620048380820003, - "y": 6.6903545095834955, - "heading": 0.922811925478865, - "angularVelocity": 0.4006918853396639, - "velocityX": -1.6631801203675514, - "velocityY": -0.14979383233565072, - "timestamp": 5.133672304394106 - }, - { - "x": 0.8479937432156998, - "y": 6.68008739430419, - "heading": 0.9505984608842503, - "angularVelocity": 0.32426545161671033, - "velocityX": -1.3304954585655717, - "velocityY": -0.11981597289022426, - "timestamp": 5.21936301016263 - }, - { - "x": 0.7624881048917089, - "y": 6.672388111768695, - "heading": 0.9716385035370365, - "angularVelocity": 0.2455347107260553, - "velocityX": -0.9978402856777302, - "velocityY": -0.08984968050435474, - "timestamp": 5.305053715931154 - }, - { - "x": 0.7054858870274207, - "y": 6.667255869051189, - "heading": 0.9857775356805354, - "angularVelocity": 0.16500076661397245, - "velocityX": -0.6652088736235632, - "velocityY": -0.05989264146534099, - "timestamp": 5.3907444216996785 + "angularVelocity": 0.2712662528992439, + "velocityX": 0.25816136612733137, + "velocityY": -0.12358010930615114, + "timestamp": 2.711145991582603 + }, + { + "x": 7.789461840084544, + "y": 7.44456429977742, + "heading": 0.032498029722470304, + "angularVelocity": 0.38031209369790614, + "velocityX": -0.11213792406398307, + "velocityY": -0.15710111155221265, + "timestamp": 2.7965969416338945 + }, + { + "x": 7.748231627408751, + "y": 7.428284690558898, + "heading": 0.07405407961857692, + "angularVelocity": 0.48631466205092383, + "velocityX": -0.48250151286758974, + "velocityY": -0.1905140809873915, + "timestamp": 2.882047891685186 + }, + { + "x": 7.675346865346751, + "y": 7.40916132937864, + "heading": 0.1243473311272727, + "angularVelocity": 0.5885628126840949, + "velocityX": -0.8529426766838836, + "velocityY": -0.22379342966671825, + "timestamp": 2.967498841736478 + }, + { + "x": 7.570799431207237, + "y": 7.387208662011206, + "heading": 0.1829724416238929, + "angularVelocity": 0.6860673925971562, + "velocityX": -1.2234788972702, + "velocityY": -0.2569037249352633, + "timestamp": 3.0529497917877695 + }, + { + "x": 7.434579177146227, + "y": 7.362445545124385, + "heading": 0.24939989698187892, + "angularVelocity": 0.7773752698843693, + "velocityX": -1.5941338742188234, + "velocityY": -0.28979334778557075, + "timestamp": 3.138400741839061 + }, + { + "x": 7.266673120974145, + "y": 7.334897700728802, + "heading": 0.32290448608070227, + "angularVelocity": 0.8601962769834678, + "velocityX": -1.9649407767992484, + "velocityY": -0.3223819557190156, + "timestamp": 3.223851691890353 + }, + { + "x": 7.067064218914486, + "y": 7.304602559914413, + "heading": 0.4024227499708516, + "angularVelocity": 0.9305720280744717, + "velocityX": -2.3359471362200686, + "velocityY": -0.3545325218292511, + "timestamp": 3.3093026419416445 + }, + { + "x": 6.835729836802405, + "y": 7.27162039096164, + "heading": 0.48622750779609625, + "angularVelocity": 0.9807352378746566, + "velocityX": -2.7072183746718483, + "velocityY": -0.385977791153593, + "timestamp": 3.394753591992936 + }, + { + "x": 6.572643492795426, + "y": 7.236066659207437, + "heading": 0.571010808696293, + "angularVelocity": 0.9921867556667185, + "velocityX": -3.078799520063915, + "velocityY": -0.4160718135124634, + "timestamp": 3.480204542044228 + }, + { + "x": 6.277837622424222, + "y": 7.198257703016904, + "heading": 0.6480492113566031, + "angularVelocity": 0.901551154360156, + "velocityX": -3.450001084764899, + "velocityY": -0.4424638481823218, + "timestamp": 3.5656554920955195 + }, + { + "x": 5.957362245190181, + "y": 7.156598764006078, + "heading": 0.6480492838419923, + "angularVelocity": 8.482689682334955e-7, + "velocityX": -3.7504015700432527, + "velocityY": -0.487518734265005, + "timestamp": 3.651106442146811 + }, + { + "x": 5.635584365409571, + "y": 7.126616390006558, + "heading": 0.6480492987117265, + "angularVelocity": 1.7401484932642463e-7, + "velocityX": -3.7656442624390154, + "velocityY": -0.3508723306355692, + "timestamp": 3.736557392198103 + }, + { + "x": 5.313806456205795, + "y": 7.096634331785028, + "heading": 0.6480493135814611, + "angularVelocity": 1.7401485546331995e-7, + "velocityX": -3.7656446067671383, + "velocityY": -0.3508686352057408, + "timestamp": 3.8220083422493945 + }, + { + "x": 4.992028547001338, + "y": 7.066652273570816, + "heading": 0.6480493284511959, + "angularVelocity": 1.7401485563507226e-7, + "velocityX": -3.7656446067751177, + "velocityY": -0.3508686351200953, + "timestamp": 3.907459292300686 + }, + { + "x": 4.67025063779688, + "y": 7.036670215356605, + "heading": 0.6480493433209307, + "angularVelocity": 1.7401485639450837e-7, + "velocityX": -3.765644606775118, + "velocityY": -0.35086863512009386, + "timestamp": 3.992910242351978 + }, + { + "x": 4.348472728592423, + "y": 7.006688157142394, + "heading": 0.6480493581906654, + "angularVelocity": 1.740148559122135e-7, + "velocityX": -3.7656446067751186, + "velocityY": -0.35086863512009364, + "timestamp": 4.078361192403269 + }, + { + "x": 4.0266948193879655, + "y": 6.976706098928182, + "heading": 0.6480493730604002, + "angularVelocity": 1.740148565017462e-7, + "velocityX": -3.765644606775118, + "velocityY": -0.35086863512009375, + "timestamp": 4.163812142454561 + }, + { + "x": 3.7049169101835084, + "y": 6.946724040713971, + "heading": 0.648049387930135, + "angularVelocity": 1.7401485552645738e-7, + "velocityX": -3.765644606775118, + "velocityY": -0.3508686351200936, + "timestamp": 4.249263092505853 + }, + { + "x": 3.3831390009790514, + "y": 6.91674198249976, + "heading": 0.6480494027998698, + "angularVelocity": 1.7401485630496073e-7, + "velocityX": -3.7656446067751177, + "velocityY": -0.35086863512009364, + "timestamp": 4.334714042557144 + }, + { + "x": 3.0613610917745944, + "y": 6.886759924285549, + "heading": 0.6480494176696047, + "angularVelocity": 1.7401485676252163e-7, + "velocityX": -3.7656446067751177, + "velocityY": -0.3508686351200934, + "timestamp": 4.420164992608436 + }, + { + "x": 2.7395831825701378, + "y": 6.85677786607133, + "heading": 0.6480494325393393, + "angularVelocity": 1.740148561275008e-7, + "velocityX": -3.7656446067751106, + "velocityY": -0.35086863512017596, + "timestamp": 4.505615942659728 + }, + { + "x": 2.4178052733940274, + "y": 6.826795807553092, + "heading": 0.648049447409112, + "angularVelocity": 1.740152990535353e-7, + "velocityX": -3.765644606443389, + "velocityY": -0.3508686386780044, + "timestamp": 4.591066892711019 + }, + { + "x": 2.101806987486401, + "y": 6.79733725091672, + "heading": 0.6651218958442715, + "angularVelocity": 0.19979237708727937, + "velocityX": -3.698007871392473, + "velocityY": -0.34474229506742526, + "timestamp": 4.676517842762311 + }, + { + "x": 1.8167368698048942, + "y": 6.770773756148669, + "heading": 0.7108427694079615, + "angularVelocity": 0.5350540109419666, + "velocityX": -3.3360672702926424, + "velocityY": -0.31086248604736477, + "timestamp": 4.761968792813603 + }, + { + "x": 1.563349701636476, + "y": 6.747170828438536, + "heading": 0.7627235430971158, + "angularVelocity": 0.6071409815583658, + "velocityX": -2.9652937505825308, + "velocityY": -0.2762160946831818, + "timestamp": 4.847419742864894 + }, + { + "x": 1.3416809560568124, + "y": 6.726531033314501, + "heading": 0.8140209677583794, + "angularVelocity": 0.6003142695377961, + "velocityX": -2.5941051029464517, + "velocityY": -0.24153967991752093, + "timestamp": 4.932870692916186 + }, + { + "x": 1.1517177468979243, + "y": 6.708849739610871, + "heading": 0.8615071361475246, + "angularVelocity": 0.5557125855327181, + "velocityX": -2.2230672572379273, + "velocityY": -0.20691746192426994, + "timestamp": 5.018321642967478 + }, + { + "x": 0.993443621158485, + "y": 6.694122461254965, + "heading": 0.9032823319492421, + "angularVelocity": 0.48887924331700494, + "velocityX": -1.852221954752358, + "velocityY": -0.17234774273507145, + "timestamp": 5.103772593018769 + }, + { + "x": 0.8668441821472115, + "y": 6.682345527785774, + "heading": 0.9380916664803222, + "angularVelocity": 0.40736041565660025, + "velocityX": -1.4815451312746155, + "velocityY": -0.13782097755638892, + "timestamp": 5.189223543070061 + }, + { + "x": 0.7719075388323822, + "y": 6.673515992217611, + "heading": 0.9650448121199915, + "angularVelocity": 0.3154224221444248, + "velocityX": -1.111007464022926, + "velocityY": -0.10332869983146084, + "timestamp": 5.274674493121353 + }, + { + "x": 0.7086239541540638, + "y": 6.667631472022659, + "heading": 0.9834806533033196, + "angularVelocity": 0.21574764437724195, + "velocityX": -0.7405837458838845, + "velocityY": -0.06886430392430233, + "timestamp": 5.360125443172645 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": 0.08305155696389553, - "velocityX": -0.33259668167527334, - "velocityY": -0.02994316977531714, - "timestamp": 5.476435127468203 + "angularVelocity": 0.11016412225954963, + "velocityX": -0.37025347871120917, + "velocityY": -0.03442272228334841, + "timestamp": 5.445576393223938 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": -6.062522351787066e-32, - "velocityX": -8.072462222466964e-33, - "velocityY": 1.3269824927509524e-31, - "timestamp": 5.562125833236727 + "angularVelocity": -1.2387955760475958e-29, + "velocityX": 1.44513347457817e-31, + "velocityY": -4.5041627226851734e-30, + "timestamp": 5.53102734327523 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.3.traj b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.3.traj index 8fb819cd..4311bf73 100644 --- a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.3.traj +++ b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.3.traj @@ -4,856 +4,856 @@ "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": -6.062522351787066e-32, - "velocityX": -8.072462222466964e-33, - "velocityY": 1.3269824927509524e-31, + "angularVelocity": -1.2387955760475958e-29, + "velocityX": 1.44513347457817e-31, + "velocityY": -4.5041627226851734e-30, "timestamp": 0 }, { - "x": 0.6915272700897366, - "y": 6.664883318998584, - "heading": 0.9848413600463822, - "angularVelocity": -0.13135885163926445, - "velocityX": 0.23720554048226594, - "velocityY": 0.0031531208242943447, - "timestamp": 0.06130475460975404 - }, - { - "x": 0.7206154156192573, - "y": 6.665271312791946, - "heading": 0.9688837454817516, - "angularVelocity": -0.26029978696125183, - "velocityX": 0.47448433183831634, - "velocityY": 0.006328934775645454, - "timestamp": 0.12260950921950808 - }, - { - "x": 0.7642550785259183, - "y": 6.665855624480287, - "heading": 0.9451981943153414, - "angularVelocity": -0.38635749082081516, - "velocityX": 0.7118479338912146, - "velocityY": 0.009531262168170858, - "timestamp": 0.18391426382926213 - }, - { - "x": 0.82245219308383, - "y": 6.6666381477571255, - "heading": 0.9139918901741649, - "angularVelocity": -0.5090356260264929, - "velocityX": 0.9493083355177907, - "velocityY": 0.012764479391858926, - "timestamp": 0.24521901843901617 - }, - { - "x": 0.8952134669088825, - "y": 6.667621071916812, - "heading": 0.8755051863660224, - "angularVelocity": -0.6277931304535281, - "velocityX": 1.1868781514293212, - "velocityY": 0.01603340827221871, - "timestamp": 0.3065237730487702 - }, - { - "x": 0.982546442157363, - "y": 6.668806895228603, - "heading": 0.8300158681177399, - "angularVelocity": -0.7420194165665126, - "velocityX": 1.4245709946057887, - "velocityY": 0.019343088792048747, - "timestamp": 0.36782852765852425 - }, - { - "x": 1.0844595931321694, - "y": 6.670198423358961, - "heading": 0.777846020332524, - "angularVelocity": -0.8509918703257573, - "velocityX": 1.6624020701747062, - "velocityY": 0.022698535198719637, - "timestamp": 0.4291332822682783 - }, - { - "x": 1.2009624731033306, - "y": 6.67179876210547, - "heading": 0.7193731323687549, - "angularVelocity": -0.9538067371118101, - "velocityX": 1.9003889781923289, - "velocityY": 0.026104643215625493, - "timestamp": 0.49043803687803234 - }, - { - "x": 1.3320659135249422, - "y": 6.673611326998587, - "heading": 0.6550478077481424, - "angularVelocity": -1.0492713824578042, - "velocityX": 2.1385525683313547, - "velocityY": 0.029566465189439763, - "timestamp": 0.5517427914877864 - }, - { - "x": 1.477782250869489, - "y": 6.675639904928793, - "heading": 0.5854215593608406, - "angularVelocity": -1.1357397779425142, - "velocityX": 2.376917390374211, - "velocityY": 0.03309005872578241, - "timestamp": 0.6130475460975404 - }, - { - "x": 1.638125489580401, - "y": 6.677888808467273, - "heading": 0.5111902482189525, - "angularVelocity": -1.2108573244346357, - "velocityX": 2.615510652177078, - "velocityY": 0.03668399870117081, - "timestamp": 0.6743523007072945 - }, - { - "x": 1.8131111470662002, - "y": 6.680363155292976, - "heading": 0.4332634603071458, - "angularVelocity": -1.271137751188529, - "velocityX": 2.8543570331486037, - "velocityY": 0.04036141799202673, - "timestamp": 0.7356570553170485 - }, - { - "x": 2.002755069495207, - "y": 6.68306928605088, - "heading": 0.3528824516114434, - "angularVelocity": -1.3111708742230885, - "velocityX": 3.0934618960016964, - "velocityY": 0.04414226555721556, - "timestamp": 0.7969618099268025 - }, - { - "x": 2.2070689242126256, - "y": 6.686015356595887, - "heading": 0.2718454606802179, - "angularVelocity": -1.3218712226658607, - "velocityX": 3.332757075988891, - "velocityY": 0.048056151007561554, - "timestamp": 0.8582665645365566 - }, - { - "x": 2.4260429630037788, - "y": 6.689212530783897, - "heading": 0.19302675484783927, - "angularVelocity": -1.2856866703751342, - "velocityX": 3.571893243600301, - "velocityY": 0.0521521407003806, - "timestamp": 0.9195713191463106 - }, - { - "x": 2.659559040385731, - "y": 6.692680446587641, - "heading": 0.12201145373865255, - "angularVelocity": -1.1583979344056987, - "velocityX": 3.809102228178563, - "velocityY": 0.05656846399302349, - "timestamp": 0.9808760737560647 - }, - { - "x": 2.9064602664817145, - "y": 6.696506622995245, - "heading": 0.07684542868597768, - "angularVelocity": -0.7367458746093568, - "velocityX": 4.027440084666806, - "velocityY": 0.06241239251280606, - "timestamp": 1.0421808283658187 - }, - { - "x": 3.164834883229884, - "y": 6.7006990530219355, - "heading": 0.07426007030156662, - "angularVelocity": -0.042172232820580385, - "velocityX": 4.214593442105751, - "velocityY": 0.06838670268526725, - "timestamp": 1.1034855829755728 - }, - { - "x": 3.423902662418304, - "y": 6.704266454413081, - "heading": 0.07426004244315124, - "angularVelocity": -4.5442503703406033e-7, - "velocityX": 4.225900272133267, - "velocityY": 0.05819126777122497, - "timestamp": 1.1647903375853268 - }, - { - "x": 3.6829704424934793, - "y": 6.7078337914078086, - "heading": 0.07426001458477352, - "angularVelocity": -4.5442442246011853e-7, - "velocityX": 4.22590028659796, - "velocityY": 0.0581902173401686, - "timestamp": 1.2260950921950808 - }, - { - "x": 3.9420382225687436, - "y": 6.71140112839604, - "heading": 0.07425998672639592, - "angularVelocity": -4.5442442083032825e-7, - "velocityX": 4.225900286599419, - "velocityY": 0.05819021723419636, - "timestamp": 1.2873998468048349 - }, - { - "x": 4.201106002644009, - "y": 6.714968465384217, - "heading": 0.07425995886801827, - "angularVelocity": -4.5442442166666195e-7, - "velocityX": 4.225900286599431, - "velocityY": 0.05819021723333154, - "timestamp": 1.348704601414589 - }, - { - "x": 4.46017378272642, - "y": 6.718535801853429, - "heading": 0.07425993100964065, - "angularVelocity": -4.5442442082699014e-7, - "velocityX": 4.225900286715997, - "velocityY": 0.05819020876799694, - "timestamp": 1.410009356024343 - }, - { - "x": 4.719241633592179, - "y": 6.7220979941697125, - "heading": 0.07425990315123465, - "angularVelocity": -4.544248838946695e-7, - "velocityX": 4.225901441330322, - "velocityY": 0.058106297610346594, - "timestamp": 1.471314110634097 - }, - { - "x": 4.978174118425727, - "y": 6.712998060028401, - "heading": 0.07425987514278792, - "angularVelocity": -4.568723405675715e-7, - "velocityX": 4.223693357584233, - "velocityY": -0.1484376570665475, - "timestamp": 1.532618865243851 - }, - { - "x": 5.236176330694113, - "y": 6.689255719100931, - "heading": 0.0742598466206584, - "angularVelocity": -4.652515078556177e-7, - "velocityX": 4.208518799410349, - "velocityY": -0.3872838424785543, - "timestamp": 1.593923619853605 - }, - { - "x": 5.4924208313627965, - "y": 6.650946506335976, - "heading": 0.07425981717735317, - "angularVelocity": -4.802776788917255e-7, - "velocityX": 4.179847098318136, - "velocityY": -0.6248979056978337, - "timestamp": 1.6552283744633591 + "x": 0.6937129747352162, + "y": 6.6645584157609425, + "heading": 0.9790660169157249, + "angularVelocity": -0.22114767057901943, + "velocityX": 0.2675140141228054, + "velocityY": -0.002104635808322496, + "timestamp": 0.06252955439239116 + }, + { + "x": 0.7271833460248389, + "y": 6.664303045137076, + "heading": 0.9517582917370493, + "angularVelocity": -0.43671709232583583, + "velocityX": 0.5352728260237692, + "velocityY": -0.004083998780231037, + "timestamp": 0.12505910878478232 + }, + { + "x": 0.7774150547311363, + "y": 6.663933728618546, + "heading": 0.9114059815034875, + "angularVelocity": -0.645331805506542, + "velocityX": 0.8033274696166545, + "velocityY": -0.005906271396278706, + "timestamp": 0.1875886631771735 + }, + { + "x": 0.8444295954022788, + "y": 6.663462332060666, + "heading": 0.8585277843526951, + "angularVelocity": -0.8456512710608117, + "velocityX": 1.0717258634310107, + "velocityY": -0.007538780061052878, + "timestamp": 0.25011821756956465 + }, + { + "x": 0.928251455530078, + "y": 6.662902493101364, + "heading": 0.793730196165171, + "angularVelocity": -1.0362713890602748, + "velocityX": 1.3405158719313994, + "velocityY": -0.008953189651546989, + "timestamp": 0.3126477719619558 + }, + { + "x": 1.0289086002870482, + "y": 6.66226903623183, + "heading": 0.7177284863656903, + "angularVelocity": -1.2154526053799588, + "velocityX": 1.6097531117096386, + "velocityY": -0.010130519490973162, + "timestamp": 0.375177326354347 + }, + { + "x": 1.1464337376402622, + "y": 6.661577427221888, + "heading": 0.6314012242837341, + "angularVelocity": -1.3805833564753438, + "velocityX": 1.8795134316120001, + "velocityY": -0.011060513970755739, + "timestamp": 0.43770688074673814 + }, + { + "x": 1.2808663505867972, + "y": 6.660844036954463, + "heading": 0.5359002148977876, + "angularVelocity": -1.5272939382655537, + "velocityX": 2.149905180883418, + "velocityY": -0.011728698126054097, + "timestamp": 0.5002364351391293 + }, + { + "x": 1.4322545638154922, + "y": 6.660088220029616, + "heading": 0.43284520187288844, + "angularVelocity": -1.6481008704811424, + "velocityX": 2.4210665612405937, + "velocityY": -0.012087355046610722, + "timestamp": 0.5627659895315205 + }, + { + "x": 1.6006535211132875, + "y": 6.65933694718367, + "heading": 0.3246624035723361, + "angularVelocity": -1.7301066567926002, + "velocityX": 2.69310982517228, + "velocityY": -0.012014684148101123, + "timestamp": 0.6252955439239116 + }, + { + "x": 1.7861082105175972, + "y": 6.658632447498398, + "heading": 0.21527496164562404, + "angularVelocity": -1.7493718448763058, + "velocityX": 2.965872557487375, + "velocityY": -0.011266667292259528, + "timestamp": 0.6878250983163028 + }, + { + "x": 1.9885470293511631, + "y": 6.658050737392514, + "heading": 0.11225233797545536, + "angularVelocity": -1.6475828857450405, + "velocityX": 3.2374901884507423, + "velocityY": -0.009302962599615121, + "timestamp": 0.750354652708694 + }, + { + "x": 2.206257706753187, + "y": 6.657808122247979, + "heading": 0.04234893056424459, + "angularVelocity": -1.1179258846552083, + "velocityX": 3.4817244344302054, + "velocityY": -0.0038800075722999405, + "timestamp": 0.8128842071010851 + }, + { + "x": 2.437370646545927, + "y": 6.657710110655747, + "heading": 0.021561241595864127, + "angularVelocity": -0.33244581974679344, + "velocityX": 3.6960592801035483, + "velocityY": -0.0015674442779168942, + "timestamp": 0.8754137614934763 + }, + { + "x": 2.6738489696177092, + "y": 6.656073614188434, + "heading": 0.021561206140011186, + "angularVelocity": -5.670255175242252e-7, + "velocityX": 3.7818648376703368, + "velocityY": -0.02617156772049459, + "timestamp": 0.9379433158858674 + }, + { + "x": 2.910327291911898, + "y": 6.654437005347333, + "heading": 0.021561170684433324, + "angularVelocity": -5.670211183604034e-7, + "velocityX": 3.7818648252347256, + "velocityY": -0.026173364851293936, + "timestamp": 1.0004728702782586 + }, + { + "x": 3.14680561420604, + "y": 6.652800396499498, + "heading": 0.021561135228855402, + "angularVelocity": -5.670211192982607e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958984518, + "timestamp": 1.0630024246706498 + }, + { + "x": 3.3832839365001823, + "y": 6.6511637876516625, + "heading": 0.02156109977327751, + "angularVelocity": -5.670211188277871e-7, + "velocityX": 3.7818648252339804, + "velocityY": -0.02617336495899097, + "timestamp": 1.125531979063041 + }, + { + "x": 3.6197622587943243, + "y": 6.649527178803828, + "heading": 0.02156106431769964, + "angularVelocity": -5.670211185570758e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958990947, + "timestamp": 1.188061533455432 + }, + { + "x": 3.8562405810884663, + "y": 6.647890569955993, + "heading": 0.02156102886212173, + "angularVelocity": -5.670211191121069e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958990954, + "timestamp": 1.2505910878478232 + }, + { + "x": 4.092718903382608, + "y": 6.646253961108158, + "heading": 0.02156099340654377, + "angularVelocity": -5.670211198937042e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.02617336495899093, + "timestamp": 1.3131206422402144 + }, + { + "x": 4.329197225676751, + "y": 6.6446173522603225, + "heading": 0.021560957950965815, + "angularVelocity": -5.67021119881002e-7, + "velocityX": 3.7818648252339804, + "velocityY": -0.026173364958990898, + "timestamp": 1.3756501966326056 + }, + { + "x": 4.565675547970892, + "y": 6.642980743412488, + "heading": 0.021560922495387855, + "angularVelocity": -5.670211198956703e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958990943, + "timestamp": 1.4381797510249967 + }, + { + "x": 4.802153870265035, + "y": 6.641344134564649, + "heading": 0.02156088703980993, + "angularVelocity": -5.670211193772164e-7, + "velocityX": 3.7818648252339795, + "velocityY": -0.026173364959042097, + "timestamp": 1.500709305417388 + }, + { + "x": 5.038632192558808, + "y": 6.639707525663405, + "heading": 0.021560851584232044, + "angularVelocity": -5.67021118724796e-7, + "velocityX": 3.781864825228069, + "velocityY": -0.02617336581314012, + "timestamp": 1.563238859809779 + }, + { + "x": 5.275110508682838, + "y": 6.638070025521481, + "heading": 0.021560816128648946, + "angularVelocity": -5.670212021371837e-7, + "velocityX": 3.781864726558842, + "velocityY": -0.02618761892412933, + "timestamp": 1.6257684142021702 + }, + { + "x": 5.511317621528859, + "y": 6.626629955813148, + "heading": 0.02156078050955198, + "angularVelocity": -5.696361874718345e-7, + "velocityX": 3.777527525044398, + "velocityY": -0.18295460153997642, + "timestamp": 1.6882979685945614 }, { "x": 5.7460856437683105, "y": 6.598193168640137, - "heading": 0.0742592023389195, - "angularVelocity": -0.000010029212865619508, - "velocityX": 4.137767356223229, - "velocityY": -0.8605097276981168, - "timestamp": 1.7165331290731132 - }, - { - "x": 5.9699164008558405, - "y": 6.543017986029004, - "heading": 0.05198341258682499, - "angularVelocity": -0.39649724748489784, - "velocityX": 3.984068806329142, - "velocityY": -0.9820889978876548, - "timestamp": 1.7727145777298094 - }, - { - "x": 6.183022450140719, - "y": 6.4863677530582065, - "heading": 0.0038718915492881255, - "angularVelocity": -0.8563595668657032, - "velocityX": 3.7931746934310255, - "velocityY": -1.0083441122525065, - "timestamp": 1.8288960263865057 - }, - { - "x": 6.384034644805308, - "y": 6.430498554479071, - "heading": -0.05330736038793806, - "angularVelocity": -1.0177603693814712, - "velocityX": 3.5779104930685666, - "velocityY": -0.9944421141670974, - "timestamp": 1.885077475043202 - }, - { - "x": 6.5728983417799105, - "y": 6.37648577029989, - "heading": -0.11411359843449789, - "angularVelocity": -1.082318799184461, - "velocityX": 3.36167367503596, - "velocityY": -0.9613989220753596, - "timestamp": 1.9412589236998983 - }, - { - "x": 6.74968082080873, - "y": 6.324889922313049, - "heading": -0.17585578783167868, - "angularVelocity": -1.098978237006013, - "velocityX": 3.1466344007800133, - "velocityY": -0.9183787392547113, - "timestamp": 1.9974403723565946 - }, - { - "x": 6.914453957859755, - "y": 6.2760504067352585, - "heading": -0.2368981836253953, - "angularVelocity": -1.0865222818785416, - "velocityX": 2.932874480647331, - "velocityY": -0.8693174837165495, - "timestamp": 2.053621821013291 - }, - { - "x": 7.067279207384589, - "y": 6.230193439539363, - "heading": -0.2961318737080175, - "angularVelocity": -1.0543282791544983, - "velocityX": 2.72020841717146, - "velocityY": -0.8162297037961601, - "timestamp": 2.109803269669987 - }, - { - "x": 7.2082072429421995, - "y": 6.1874800327074295, - "heading": -0.3527517462037597, - "angularVelocity": -1.0078037118929446, - "velocityX": 2.5084443161793004, - "velocityY": -0.7602759959597819, - "timestamp": 2.1659847183266834 - }, - { - "x": 7.337279834118698, - "y": 6.148030397062498, - "heading": -0.4061452108111807, - "angularVelocity": -0.9503753620467785, - "velocityX": 2.2974236916746635, - "velocityY": -0.7021825992062444, - "timestamp": 2.2221661669833797 - }, - { - "x": 7.454531717952861, - "y": 6.111937602538107, - "heading": -0.45583024333208966, - "angularVelocity": -0.8843672370307636, - "velocityX": 2.087021368043439, - "velocityY": -0.6424326069792218, - "timestamp": 2.278347615640076 - }, - { - "x": 7.559992100674049, - "y": 6.079275791201654, - "heading": -0.5014179550675222, - "angularVelocity": -0.8114370993528713, - "velocityX": 1.8771388998104257, - "velocityY": -0.5813629252608888, - "timestamp": 2.334529064296772 - }, - { - "x": 7.653685805762832, - "y": 6.050105397874059, - "heading": -0.5425884668253612, - "angularVelocity": -0.7328132816477557, - "velocityX": 1.6676982763707042, - "velocityY": -0.5192175357713573, - "timestamp": 2.3907105129534685 - }, - { - "x": 7.735634146205772, - "y": 6.02447662157365, - "heading": -0.5790745351653838, - "angularVelocity": -0.649432672392886, - "velocityX": 1.4586370127921349, - "velocityY": -0.4561786303699093, - "timestamp": 2.4468919616101648 - }, - { - "x": 7.8058555921649635, - "y": 6.002431822282749, - "heading": -0.6106499848823828, - "angularVelocity": -0.5620262643981344, - "velocityX": 1.249904508306431, - "velocityY": -0.39238573974139485, - "timestamp": 2.503073410266861 - }, - { - "x": 7.864366288021797, - "y": 5.984007229520149, - "heading": -0.6371212833352664, - "angularVelocity": -0.4711750779984228, - "velocityX": 1.0414593652500725, - "velocityY": -0.32794798288639887, - "timestamp": 2.5592548589235573 - }, - { - "x": 7.911180457736072, - "y": 5.969234194186617, - "heading": -0.6583212554852172, - "angularVelocity": -0.37734826454005344, - "velocityX": 0.833267401136955, - "velocityY": -0.26295219661928954, - "timestamp": 2.6154363075802536 - }, - { - "x": 7.946310726470828, - "y": 5.958140127507551, - "heading": -0.6741042985083451, - "angularVelocity": -0.2809297980116511, - "velocityX": 0.6253001582323476, - "velocityY": -0.19746850507286262, - "timestamp": 2.67161775623695 - }, - { - "x": 7.969768378758335, - "y": 5.950749219301424, - "heading": -0.6843426613939789, - "angularVelocity": -0.1822374312238999, - "velocityX": 0.41753377401939834, - "velocityY": -0.13155424758250212, - "timestamp": 2.727799204893646 + "heading": 0.021560744143059335, + "angularVelocity": -5.815888661825855e-7, + "velocityX": 3.7545129582438856, + "velocityY": -0.45477354587499197, + "timestamp": 1.7508275229869525 + }, + { + "x": 5.947788996667301, + "y": 6.560838784941361, + "heading": 0.021560710996949464, + "angularVelocity": -6.111012841975544e-7, + "velocityX": 3.718722301384556, + "velocityY": -0.6886875092486842, + "timestamp": 1.8050674840681804 + }, + { + "x": 6.146761517861249, + "y": 6.510943609310236, + "heading": 0.021560678630883793, + "angularVelocity": -5.967199279900907e-7, + "velocityX": 3.6683750730568514, + "velocityY": -0.9198969659363234, + "timestamp": 1.8593074451494083 + }, + { + "x": 6.342740574859562, + "y": 6.4503482133201375, + "heading": 0.021560646512747805, + "angularVelocity": -5.921489497600962e-7, + "velocityX": 3.6131857968117713, + "velocityY": -1.1171725565834012, + "timestamp": 1.9135474062306361 + }, + { + "x": 6.53435736566076, + "y": 6.392349027665542, + "heading": 0.005697481882948994, + "angularVelocity": -0.2924626845886352, + "velocityX": 3.532760477358042, + "velocityY": -1.0693072874395997, + "timestamp": 1.967787367311864 + }, + { + "x": 6.715860798361231, + "y": 6.336732624061585, + "heading": -0.044321515304948156, + "angularVelocity": -0.9221798133850251, + "velocityX": 3.3463046263742644, + "velocityY": -1.0253769083769828, + "timestamp": 2.022027328393092 + }, + { + "x": 6.885664142970145, + "y": 6.284563070375092, + "heading": -0.11100118706044729, + "angularVelocity": -1.2293458628342813, + "velocityX": 3.1305948828876353, + "velocityY": -0.9618287448319874, + "timestamp": 2.0762672894743197 + }, + { + "x": 7.043230365082517, + "y": 6.236112426000239, + "heading": -0.18201634611185183, + "angularVelocity": -1.309277470628262, + "velocityX": 2.9049840555085917, + "velocityY": -0.8932647334000697, + "timestamp": 2.1305072505555476 + }, + { + "x": 7.188521106986847, + "y": 6.19141127344733, + "heading": -0.25302239943673055, + "angularVelocity": -1.3091095920689053, + "velocityX": 2.678666042675613, + "velocityY": -0.8241368847217142, + "timestamp": 2.1847472116367754 + }, + { + "x": 7.3215520348116, + "y": 6.150465382048361, + "heading": -0.3216933073087357, + "angularVelocity": -1.2660574694949878, + "velocityX": 2.4526368598519452, + "velocityY": -0.7549026692266663, + "timestamp": 2.2389871727180033 + }, + { + "x": 7.4423459735084, + "y": 6.113273952325478, + "heading": -0.38654982711023245, + "angularVelocity": -1.1957331551984454, + "velocityX": 2.2270284913351, + "velocityY": -0.685683193378148, + "timestamp": 2.293227133799231 + }, + { + "x": 7.550924553999962, + "y": 6.079834322504852, + "heading": -0.4465624539391904, + "angularVelocity": -1.1064282796789968, + "velocityX": 2.001818923301949, + "velocityY": -0.6165127915661526, + "timestamp": 2.347467094880459 + }, + { + "x": 7.647306628463083, + "y": 6.050143536065283, + "heading": -0.5009752926426013, + "angularVelocity": -1.0031872740823848, + "velocityX": 1.7769569251493995, + "velocityY": -0.5473968979274295, + "timestamp": 2.401707055961687 + }, + { + "x": 7.7315082292215935, + "y": 6.024198841891014, + "heading": -0.5492132046279071, + "angularVelocity": -0.8893426732564748, + "velocityX": 1.552390508400531, + "velocityY": -0.47833172548586056, + "timestamp": 2.4559470170429147 + }, + { + "x": 7.803542912083633, + "y": 6.001997808520636, + "heading": -0.5908266102991006, + "angularVelocity": -0.7672093571172569, + "velocityX": 1.3280740145473924, + "velocityY": -0.4093113809047783, + "timestamp": 2.5101869781241426 + }, + { + "x": 7.863422173267542, + "y": 5.983538316289689, + "heading": -0.6254559233807054, + "angularVelocity": -0.6384464957440807, + "velocityX": 1.103969471774435, + "velocityY": -0.34033011571123517, + "timestamp": 2.5644269392053705 + }, + { + "x": 7.9111558450934325, + "y": 5.968818528744131, + "heading": -0.6528074420175133, + "angularVelocity": -0.5042687732730391, + "velocityX": 0.8800462034699242, + "velocityY": -0.2713827084704847, + "timestamp": 2.6186669002865983 + }, + { + "x": 7.946752441580289, + "y": 5.95783687514921, + "heading": -0.6726364642205706, + "angularVelocity": -0.36557958021692255, + "velocityX": 0.6562799046545997, + "velocityY": -0.2024642602245795, + "timestamp": 2.672906861367826 + }, + { + "x": 7.970219449253945, + "y": 5.950592046499088, + "heading": -0.6847351436740425, + "angularVelocity": -0.22305840956179235, + "velocityX": 0.43265163185705313, + "velocityY": -0.1335699455844713, + "timestamp": 2.727146822449054 }, { "x": 7.981563568115234, "y": 5.947082996368408, "heading": -0.6889234822214203, - "angularVelocity": -0.08153618208446338, - "velocityX": 0.2099481170194555, - "velocityY": -0.06525682446209594, - "timestamp": 2.7839806535503424 - }, - { - "x": 7.98107279837352, - "y": 5.947370479695571, - "heading": -0.6873686383472613, - "angularVelocity": 0.02630231709530022, - "velocityX": -0.008302043428202248, - "velocityY": 0.004863174853957543, - "timestamp": 2.843094984372435 - }, - { - "x": 7.967670719276277, - "y": 5.9517826740935895, - "heading": -0.6795422167344298, - "angularVelocity": 0.1323946580125472, - "velocityX": -0.22671455315256586, - "velocityY": 0.07463832097325668, - "timestamp": 2.9022093151945274 - }, - { - "x": 7.941346705017116, - "y": 5.960296828520997, - "heading": -0.6655606707736306, - "angularVelocity": 0.23651703007308753, - "velocityX": -0.4453068129686613, - "velocityY": 0.14402860201583023, - "timestamp": 2.96132364601662 - }, - { - "x": 7.902088914435571, - "y": 5.972887353790235, - "heading": -0.6455561559979053, - "angularVelocity": 0.338403809998799, - "velocityX": -0.6640993822579894, - "velocityY": 0.21298600684714283, - "timestamp": 3.0204379768387124 - }, - { - "x": 7.849884062968043, - "y": 5.989525223865173, - "heading": -0.6196795941981793, - "angularVelocity": 0.43773754079366983, - "velocityX": -0.8831166781645818, - "velocityY": 0.281452396458829, - "timestamp": 3.079552307660805 - }, - { - "x": 7.78471714102411, - "y": 6.010177207449998, - "heading": -0.5881045676669778, - "angularVelocity": 0.5341348889870459, - "velocityX": -1.1023878818835982, - "velocityY": 0.34935663311453924, - "timestamp": 3.1386666384828974 - }, - { - "x": 7.706571062429961, - "y": 6.034804864474398, - "heading": -0.5510323547922651, - "angularVelocity": 0.6271273371305467, - "velocityX": -1.3219481216717965, - "velocityY": 0.4166106032481136, - "timestamp": 3.19778096930499 - }, - { - "x": 7.615426220504745, - "y": 6.063363207617709, - "heading": -0.5086985338851634, - "angularVelocity": 0.7161346549706845, - "velocityX": -1.5418400353633617, - "velocityY": 0.48310355113818987, - "timestamp": 3.2568953001270824 - }, - { - "x": 7.511259920040068, - "y": 6.095798872359664, - "heading": -0.46138178697105986, - "angularVelocity": 0.8004276840501866, - "velocityX": -1.7621158696386412, - "velocityY": 0.5486937649615388, - "timestamp": 3.316009630949175 - }, - { - "x": 7.39404563921687, - "y": 6.132047543004427, - "heading": -0.4094159214388897, - "angularVelocity": 0.8790739032226302, - "velocityX": -1.982840356866446, - "velocityY": 0.6131959905603276, - "timestamp": 3.3751239617712674 - }, - { - "x": 7.263752054530884, - "y": 6.172030212832479, - "heading": -0.35320687491635055, - "angularVelocity": 0.9508531305497406, - "velocityX": -2.2040947241391, - "velocityY": 0.6763617091155382, - "timestamp": 3.43423829259336 - }, - { - "x": 7.120341734246255, - "y": 6.215647542637923, - "heading": -0.293257939808125, - "angularVelocity": 1.0141184764257163, - "velocityX": -2.425982300573299, - "velocityY": 0.7378469687276493, - "timestamp": 3.4933526234154524 - }, - { - "x": 6.963769381897745, - "y": 6.262770961252908, - "heading": -0.23020941920008114, - "angularVelocity": 1.066552217224485, - "velocityX": -2.64863612885552, - "velocityY": 0.7971572706592246, - "timestamp": 3.552466954237545 - }, - { - "x": 6.793979543954, - "y": 6.313227822369206, - "heading": -0.16490527005597488, - "angularVelocity": 1.1047092682253812, - "velocityX": -2.8722280296927845, - "velocityY": 0.8535470234476921, - "timestamp": 3.6115812850596374 - }, - { - "x": 6.610904026180611, - "y": 6.366774787590835, - "heading": -0.09851409628601165, - "angularVelocity": 1.1230977809724585, - "velocityX": -3.0969735295552177, - "velocityY": 0.9058203734519391, - "timestamp": 3.67069561588173 - }, - { - "x": 6.414461060118511, - "y": 6.423045155535491, - "heading": -0.03277169507390691, - "angularVelocity": 1.112122903157957, - "velocityX": -3.3231022550742657, - "velocityY": 0.9518904665267254, - "timestamp": 3.7298099467038224 - }, - { - "x": 6.20456835252053, - "y": 6.481428804370567, - "heading": 0.029452709079620762, - "angularVelocity": 1.0526111568579828, - "velocityX": -3.5506230837606094, - "velocityY": 0.9876395118264049, - "timestamp": 3.788924277525915 - }, - { - "x": 5.981258970375398, - "y": 6.540731988125068, - "heading": 0.08236638837911917, - "angularVelocity": 0.8951074733256352, - "velocityX": -3.7775845389706695, - "velocityY": 1.0031947064236981, - "timestamp": 3.8480386083480074 + "angularVelocity": -0.07721868644237204, + "velocityX": 0.2091468842372586, + "velocityY": -0.06469492346102504, + "timestamp": 2.781386783530282 + }, + { + "x": 7.980191051552582, + "y": 5.9474933088994915, + "heading": -0.684492260818683, + "angularVelocity": 0.07820368392725321, + "velocityX": -0.024222633376945003, + "velocityY": 0.007241333387762343, + "timestamp": 2.8380493495400803 + }, + { + "x": 7.965589050123253, + "y": 5.951981018210289, + "heading": -0.6714197034271625, + "angularVelocity": 0.23070888440280377, + "velocityX": -0.257701026579075, + "velocityY": 0.07920060150509887, + "timestamp": 2.8947119155498786 + }, + { + "x": 7.9377506970473615, + "y": 5.960547682425779, + "heading": -0.6498948487141165, + "angularVelocity": 0.37987786697348064, + "velocityX": -0.4913006070194107, + "velocityY": 0.15118736793544205, + "timestamp": 2.951374481559677 + }, + { + "x": 7.896668195667252, + "y": 5.973195099193875, + "heading": -0.6201381150730912, + "angularVelocity": 0.5251568316881386, + "velocityX": -0.7250377854932653, + "velocityY": 0.22320585986009217, + "timestamp": 3.0080370475694753 + }, + { + "x": 7.842332542279827, + "y": 5.989925310225715, + "heading": -0.5824114694602323, + "angularVelocity": 0.6658125155563156, + "velocityX": -0.9589338643440537, + "velocityY": 0.2952603845887764, + "timestamp": 3.0646996135792737 + }, + { + "x": 7.774733202893158, + "y": 6.010740641182104, + "heading": -0.5370321516926094, + "angularVelocity": 0.8008694445602113, + "velocityX": -1.1930158506231556, + "velocityY": 0.367355953360645, + "timestamp": 3.121362179589072 + }, + { + "x": 7.693857750948662, + "y": 6.035643782735694, + "heading": -0.48439126574422153, + "angularVelocity": 0.9290240392446375, + "velocityX": -1.4273171449826625, + "velocityY": 0.43949900802735137, + "timestamp": 3.1780247455988704 + }, + { + "x": 7.5996914832698845, + "y": 6.064637893376439, + "heading": -0.4249792327859352, + "angularVelocity": 1.0485235163549331, + "velocityX": -1.6618779259395846, + "velocityY": 0.5116978047858136, + "timestamp": 3.2346873116086687 + }, + { + "x": 7.492217045643208, + "y": 6.097726656744903, + "heading": -0.35942190954654185, + "angularVelocity": 1.156977663667012, + "velocityX": -1.8967449798883769, + "velocityY": 0.583961611670435, + "timestamp": 3.291349877618467 + }, + { + "x": 7.371414140499637, + "y": 6.1349141479804095, + "heading": -0.28853586070822274, + "angularVelocity": 1.251020803153572, + "velocityX": -2.131970252153457, + "velocityY": 0.6562973379828368, + "timestamp": 3.3480124436282654 + }, + { + "x": 7.237259574639344, + "y": 6.176204237589326, + "heading": -0.21342323245219458, + "angularVelocity": 1.3256128965822067, + "velocityX": -2.367604845800615, + "velocityY": 0.7287013722918435, + "timestamp": 3.404675009638064 + }, + { + "x": 7.0897287772168305, + "y": 6.221599002117526, + "heading": -0.13565860715800612, + "angularVelocity": 1.372416231215901, + "velocityX": -2.6036730739833454, + "velocityY": 0.8011420541800114, + "timestamp": 3.461337575647862 + }, + { + "x": 6.928804270313556, + "y": 6.271094657477637, + "heading": -0.05771767479301098, + "angularVelocity": 1.3755277576295868, + "velocityX": -2.8400497583439677, + "velocityY": 0.8735159532230397, + "timestamp": 3.5180001416576605 + }, + { + "x": 6.754523159333334, + "y": 6.324666148768191, + "heading": 0.015819291394400217, + "angularVelocity": 1.297805083071882, + "velocityX": -3.0757715940730104, + "velocityY": 0.9454476749480591, + "timestamp": 3.574662707667459 + }, + { + "x": 6.56735113931042, + "y": 6.382062750762148, + "heading": 0.07327050352161106, + "angularVelocity": 1.0139182916156146, + "velocityX": -3.3032746873932677, + "velocityY": 1.012954513638379, + "timestamp": 3.631325273677257 + }, + { + "x": 6.368937324018353, + "y": 6.441765360809047, + "heading": 0.09523150855737006, + "angularVelocity": 0.38757519438780325, + "velocityX": -3.5016736668395723, + "velocityY": 1.053651718430401, + "timestamp": 3.6879878396870556 + }, + { + "x": 6.164194285631387, + "y": 6.50503273811371, + "heading": 0.09523154205905172, + "angularVelocity": 5.912489325251288e-7, + "velocityX": -3.6133739222393086, + "velocityY": 1.1165639285330458, + "timestamp": 3.744650405696854 + }, + { + "x": 5.956663061240617, + "y": 6.558448687512427, + "heading": 0.09523157576982937, + "angularVelocity": 5.949391283503271e-7, + "velocityX": -3.6625807654896136, + "velocityY": 0.9427026193886323, + "timestamp": 3.8013129717066523 }, { "x": 5.7460856437683105, "y": 6.598193168640137, - "heading": 0.10673916956816744, - "angularVelocity": 0.41229902884969827, - "velocityX": -3.9782794347255015, - "velocityY": 0.9720346947341876, - "timestamp": 3.9071529391701 - }, - { - "x": 5.482731803813418, - "y": 6.650320477040453, - "heading": 0.10673940582411118, - "angularVelocity": 0.0000037192747979075137, - "velocityX": -4.145865218900198, - "velocityY": 0.8206175952808282, - "timestamp": 3.970674984883095 - }, - { - "x": 5.216773824306616, - "y": 6.6869112798369965, - "heading": 0.10673943110586125, - "angularVelocity": 3.9799962033997064e-7, - "velocityX": -4.186861057788582, - "velocityY": 0.5760331296927703, - "timestamp": 4.03419703059609 - }, - { - "x": 4.9491275418069325, - "y": 6.707839765678222, - "heading": 0.10673945565224871, - "angularVelocity": 3.8642312574461983e-7, - "velocityX": -4.213439279159305, - "velocityY": 0.32946807059370153, - "timestamp": 4.097719076309085 - }, - { - "x": 4.680714529727759, - "y": 6.71303487609764, - "heading": 0.1067394798338107, - "angularVelocity": 3.8067983666652777e-7, - "velocityX": -4.225509570203694, - "velocityY": 0.08178436889281773, - "timestamp": 4.16124112202208 - }, - { - "x": 4.4122692055154475, - "y": 6.709929692152406, - "heading": 0.10673950394272769, - "angularVelocity": 3.795362179203617e-7, - "velocityX": -4.226018246093679, - "velocityY": -0.04888356334214293, - "timestamp": 4.224763167735075 - }, - { - "x": 4.143823892978554, - "y": 6.706823499022996, - "heading": 0.10673952805164147, - "angularVelocity": 3.795361675287501e-7, - "velocityX": -4.226018062292654, - "velocityY": -0.048899450490688265, - "timestamp": 4.28828521344807 - }, - { - "x": 3.875378580442675, - "y": 6.703717305805851, - "heading": 0.10673955216055508, - "angularVelocity": 3.7953616500379395e-7, - "velocityX": -4.226018062276673, - "velocityY": -0.04889945187186359, - "timestamp": 4.351807259161065 - }, - { - "x": 3.606933267906796, - "y": 6.700611112588698, - "heading": 0.10673957626946878, - "angularVelocity": 3.7953616612214584e-7, - "velocityX": -4.2260180622766725, - "velocityY": -0.04889945187199676, - "timestamp": 4.41532930487406 - }, - { - "x": 3.3384879553710274, - "y": 6.697504919362028, - "heading": 0.10673960037838261, - "angularVelocity": 3.79536168074433e-7, - "velocityX": -4.226018062274939, - "velocityY": -0.04889945202181832, - "timestamp": 4.478851350587055 - }, - { - "x": 3.0700426441020716, - "y": 6.694398616665168, - "heading": 0.10673962448757092, - "angularVelocity": 3.795404893645561e-7, - "velocityX": -4.22601804233206, - "velocityY": -0.0489011753634946, - "timestamp": 4.54237339630005 - }, - { - "x": 2.806808379331506, - "y": 6.690344844376677, - "heading": 0.1255043030972768, - "angularVelocity": 0.2954041923411685, - "velocityX": -4.143982798663499, - "velocityY": -0.06381677798612607, - "timestamp": 4.605895442013045 - }, - { - "x": 2.556272153813946, - "y": 6.687083030964068, - "heading": 0.1867455578699796, - "angularVelocity": 0.964094497985838, - "velocityX": -3.9440830770711344, - "velocityY": -0.05134931307701316, - "timestamp": 4.66941748772604 - }, - { - "x": 2.3210540999050693, - "y": 6.6841316507490856, - "heading": 0.2629720657012657, - "angularVelocity": 1.2000008339733441, - "velocityX": -3.7029357488208183, - "velocityY": -0.046462297960575334, - "timestamp": 4.732939533439035 - }, - { - "x": 2.1015488183646034, - "y": 6.68142600254672, - "heading": 0.34363574720189305, - "angularVelocity": 1.2698533335195523, - "velocityX": -3.4555763920487643, - "velocityY": -0.04259384552239664, - "timestamp": 4.79646157915203 - }, - { - "x": 1.8977951102037987, - "y": 6.678949586554031, - "heading": 0.4245682813102029, - "angularVelocity": 1.2740857634525757, - "velocityX": -3.207606207794462, - "velocityY": -0.038985142321737026, - "timestamp": 4.859983624865025 - }, - { - "x": 1.7097851332749783, - "y": 6.676691054407077, - "heading": 0.5034534441826332, - "angularVelocity": 1.2418548865515076, - "velocityX": -2.9597594790679125, - "velocityY": -0.03555509148993883, - "timestamp": 4.92350567057802 - }, - { - "x": 1.5375030148010667, - "y": 6.674642046081059, - "heading": 0.5788036623200312, - "angularVelocity": 1.1862057855920731, - "velocityX": -2.7121626285827998, - "velocityY": -0.03225664890069088, - "timestamp": 4.987027716291015 - }, - { - "x": 1.3809327880417555, - "y": 6.67279615304415, - "heading": 0.6495791212494125, - "angularVelocity": 1.1141873366163184, - "velocityX": -2.46481713556153, - "velocityY": -0.02905909304700025, - "timestamp": 5.05054976200401 - }, - { - "x": 1.2400601320509559, - "y": 6.671148237175822, - "heading": 0.7150087917511168, - "angularVelocity": 1.0300309092268365, - "velocityX": -2.2176970909798954, - "velocityY": -0.025942424395041352, - "timestamp": 5.114071807717005 - }, - { - "x": 1.1148725609299421, - "y": 6.669694026119316, - "heading": 0.7744947973618744, - "angularVelocity": 0.9364623721271129, - "velocityX": -1.9707736064835946, - "velocityY": -0.022893013601536772, - "timestamp": 5.17759385343 - }, - { - "x": 1.0053592312326722, - "y": 6.668429889682977, - "heading": 0.8275574496076633, - "angularVelocity": 0.8353423075436972, - "velocityX": -1.7240208256527763, - "velocityY": -0.019900751339958393, - "timestamp": 5.241115899142995 - }, - { - "x": 0.9115106985701406, - "y": 6.667352716414464, - "heading": 0.873802307501142, - "angularVelocity": 0.7280127296658919, - "velocityX": -1.4774167237396383, - "velocityY": -0.016957471322305857, - "timestamp": 5.30463794485599 - }, - { - "x": 0.8333187122321524, - "y": 6.66645983627761, - "heading": 0.9128998853211554, - "angularVelocity": 0.6154962010616661, - "velocityX": -1.2309425091766681, - "velocityY": -0.014056224525380053, - "timestamp": 5.368159990568985 - }, - { - "x": 0.7707760609431589, - "y": 6.665748959829805, - "heading": 0.9445728281790685, - "angularVelocity": 0.49861339480497757, - "velocityX": -0.984581818595295, - "velocityY": -0.011191019429984484, - "timestamp": 5.43168203628198 - }, - { - "x": 0.7238764608943239, - "y": 6.665218122050303, - "heading": 0.9685875457966591, - "angularVelocity": 0.37805327816572476, - "velocityX": -0.7383200512895466, - "velocityY": -0.008356748803399568, - "timestamp": 5.4952040819949755 - }, - { - "x": 0.692614473543579, - "y": 6.664865629845864, - "heading": 0.9847485268863282, - "angularVelocity": 0.2544153121687507, - "velocityX": -0.4921439005915036, - "velocityY": -0.005549131808990834, - "timestamp": 5.5587261277079705 + "heading": 0.09523161032614703, + "angularVelocity": 6.098615032122225e-7, + "velocityX": -3.716341004321884, + "velocityY": 0.7014239545882616, + "timestamp": 3.8579755377164506 + }, + { + "x": 5.5027055023001905, + "y": 6.62552340619542, + "heading": 0.09523164151085345, + "angularVelocity": 4.815615607402516e-7, + "velocityX": -3.7583333072466103, + "velocityY": 0.4220399473818025, + "timestamp": 3.9227330037450354 + }, + { + "x": 5.257964269234122, + "y": 6.634609890080181, + "heading": 0.09523167208339899, + "angularVelocity": 4.721084288711453e-7, + "velocityX": -3.7793516033817274, + "velocityY": 0.140315618291052, + "timestamp": 3.98749046977362 + }, + { + "x": 5.013061040594942, + "y": 6.636410996869712, + "heading": 0.09523170256730593, + "angularVelocity": 4.70739650837411e-7, + "velocityX": -3.781853177069622, + "velocityY": 0.02781311406988273, + "timestamp": 4.052247935802205 + }, + { + "x": 4.7681578086493825, + "y": 6.6382116540225224, + "heading": 0.09523173305121174, + "angularVelocity": 4.7073963340734453e-7, + "velocityX": -3.781853228127505, + "velocityY": 0.027806170674068813, + "timestamp": 4.11700540183079 + }, + { + "x": 4.5232545767036525, + "y": 6.640012311152163, + "heading": 0.09523176353511749, + "angularVelocity": 4.707396321471799e-7, + "velocityX": -3.781853228130136, + "velocityY": 0.02780617031625758, + "timestamp": 4.181762867859375 + }, + { + "x": 4.2783513447579224, + "y": 6.641812968281802, + "heading": 0.09523179401902328, + "angularVelocity": 4.707396330675372e-7, + "velocityX": -3.781853228130136, + "velocityY": 0.027806170316239148, + "timestamp": 4.246520333887959 + }, + { + "x": 4.033448112812192, + "y": 6.64361362541144, + "heading": 0.09523182450292901, + "angularVelocity": 4.707396322379774e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.027806170316239207, + "timestamp": 4.311277799916544 + }, + { + "x": 3.788544880866463, + "y": 6.645414282541079, + "heading": 0.09523185498683476, + "angularVelocity": 4.7073963225441474e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.02780617031623923, + "timestamp": 4.376035265945129 + }, + { + "x": 3.5436416489207327, + "y": 6.647214939670717, + "heading": 0.09523188547074045, + "angularVelocity": 4.707396314544476e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.02780617031623925, + "timestamp": 4.440792731973714 + }, + { + "x": 3.2987384169750027, + "y": 6.649015596800356, + "heading": 0.09523191595464614, + "angularVelocity": 4.7073963141347726e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.027806170316239207, + "timestamp": 4.5055501980022985 + }, + { + "x": 3.0538351850292726, + "y": 6.650816253929995, + "heading": 0.09523194643855185, + "angularVelocity": 4.7073963175583744e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.027806170316239227, + "timestamp": 4.570307664030883 + }, + { + "x": 2.8089319530835426, + "y": 6.652616911059632, + "heading": 0.09523197692245748, + "angularVelocity": 4.7073963058043316e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.02780617031621524, + "timestamp": 4.635065130059468 + }, + { + "x": 2.5640287211375905, + "y": 6.654417568159032, + "heading": 0.09523200740636317, + "angularVelocity": 4.707396315238404e-7, + "velocityX": -3.78185322813357, + "velocityY": 0.027806169849281307, + "timestamp": 4.699822596088053 + }, + { + "x": 2.319125484878767, + "y": 6.656217638494738, + "heading": 0.09523203789215427, + "angularVelocity": 4.707687462497284e-7, + "velocityX": -3.7818532947339327, + "velocityY": 0.02779710890650077, + "timestamp": 4.764580062116638 + }, + { + "x": 2.086765993499063, + "y": 6.65613072972364, + "heading": 0.1392662443558495, + "angularVelocity": 0.6799865585268291, + "velocityX": -3.588149840161109, + "velocityY": -0.0013420656555584189, + "timestamp": 4.829337528145222 + }, + { + "x": 1.8694746583337432, + "y": 6.65699037980207, + "heading": 0.2298866992043259, + "angularVelocity": 1.399382347797175, + "velocityX": -3.3554638328406146, + "velocityY": 0.013274918417153406, + "timestamp": 4.894094994173807 + }, + { + "x": 1.6701637818051993, + "y": 6.657989601285878, + "heading": 0.3342097566920158, + "angularVelocity": 1.6109811560823084, + "velocityX": -3.077805367501045, + "velocityY": 0.015430212840099346, + "timestamp": 4.958852460202392 + }, + { + "x": 1.4891465820964496, + "y": 6.659013009703433, + "heading": 0.4396201310941332, + "angularVelocity": 1.6277717592530314, + "velocityX": -2.7953101134137524, + "velocityY": 0.01580371315183694, + "timestamp": 5.023609926230977 + }, + { + "x": 1.326396709734208, + "y": 6.660011981353038, + "heading": 0.5409065147827995, + "angularVelocity": 1.5640881260541826, + "velocityX": -2.5132217540816426, + "velocityY": 0.015426354841678473, + "timestamp": 5.088367392259562 + }, + { + "x": 1.1818610470561817, + "y": 6.660956545539143, + "heading": 0.6351810352227076, + "angularVelocity": 1.4558092868904808, + "velocityX": -2.231953650166405, + "velocityY": 0.014586182011631477, + "timestamp": 5.153124858288146 + }, + { + "x": 1.0554921038308631, + "y": 6.661825694692396, + "heading": 0.7206099929025545, + "angularVelocity": 1.3192140291922123, + "velocityX": -1.9514189015601402, + "velocityY": 0.013421605361615121, + "timestamp": 5.217882324316731 + }, + { + "x": 0.9472506642705061, + "y": 6.66260279420219, + "heading": 0.7959103128896443, + "angularVelocity": 1.1628052270274385, + "velocityX": -1.6714897323588642, + "velocityY": 0.012000153147596032, + "timestamp": 5.282639790345316 + }, + { + "x": 0.8571042421570496, + "y": 6.663273874000479, + "heading": 0.8601164354198797, + "angularVelocity": 0.9914860242044418, + "velocityX": -1.3920622229669246, + "velocityY": 0.010362971861702049, + "timestamp": 5.347397256373901 + }, + { + "x": 0.7850255723133412, + "y": 6.6638271232273425, + "heading": 0.912467741181329, + "angularVelocity": 0.8084211593199297, + "velocityX": -1.1130557488443436, + "velocityY": 0.00854340450286573, + "timestamp": 5.4121547224024855 + }, + { + "x": 0.7309916972965728, + "y": 6.664252697034555, + "heading": 0.952354006476902, + "angularVelocity": 0.6159330767817102, + "velocityX": -0.8344037889456237, + "velocityY": 0.0065718106855080505, + "timestamp": 5.47691218843107 + }, + { + "x": 0.6949834963208631, + "y": 6.664542513080523, + "heading": 0.9792885970471746, + "angularVelocity": 0.41593027371366526, + "velocityX": -0.556047096713378, + "velocityY": 0.0044754074509330615, + "timestamp": 5.541669654459655 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": 0.12823509120632537, - "velocityX": -0.24604105125639145, - "velocityY": -0.002764585801632022, - "timestamp": 5.6222481734209655 + "angularVelocity": 0.21010218588179536, + "velocityX": -0.2779301721676604, + "velocityY": 0.0022778009813890303, + "timestamp": 5.60642712048824 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": -8.295390282600847e-37, - "velocityX": 2.201674205631738e-38, - "velocityY": -2.6267125304593916e-36, - "timestamp": 5.6857702191339605 + "angularVelocity": -1.2401951851203077e-33, + "velocityX": -4.2513783914380976e-39, + "velocityY": -1.4668246371137104e-32, + "timestamp": 5.671184586516825 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.traj b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.traj index 642b3c7a..a5e6f83f 100644 --- a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.traj +++ b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.traj @@ -5,1863 +5,1845 @@ "y": 6.664690017700195, "heading": 0.9928942822119464, "angularVelocity": 0, - "velocityX": 2.456521210166087e-32, - "velocityY": -5.061818134196095e-29, + "velocityX": 2.1425070675381243e-37, + "velocityY": 2.00620509434581e-36, "timestamp": 0 }, { - "x": 0.6967277255263876, - "y": 6.6675008319457, - "heading": 0.9828953878926869, - "angularVelocity": -0.13938944197774958, - "velocityX": 0.2752170097278352, - "velocityY": 0.039184115430369315, - "timestamp": 0.07173351280273003 - }, - { - "x": 0.7362120737078192, - "y": 6.6731227516141445, - "heading": 0.9629028188307202, - "angularVelocity": -0.27870612052394816, - "velocityX": 0.5504309860819994, - "velocityY": 0.07837229001553121, - "timestamp": 0.14346702560546007 - }, - { - "x": 0.7954384350293103, - "y": 6.681556356569816, - "heading": 0.9329336821586108, - "angularVelocity": -0.417784317335511, - "velocityX": 0.8256442353927022, - "velocityY": 0.11756854819039611, - "timestamp": 0.2152005384081901 - }, - { - "x": 0.8744068559927786, - "y": 6.692802561177285, - "heading": 0.8930142547719868, - "angularVelocity": -0.5564962014627703, - "velocityX": 1.1008581329627185, - "velocityY": 0.15677755305130064, - "timestamp": 0.28693405121092014 - }, - { - "x": 0.9731173912556694, - "y": 6.706862646881597, - "heading": 0.8431760672320958, - "angularVelocity": -0.6947685342560019, - "velocityX": 1.3760727924840699, - "velocityY": 0.19600442187834377, - "timestamp": 0.3586675640136502 - }, - { - "x": 1.0915699973864605, - "y": 6.72373826040205, - "heading": 0.7834509327491487, - "angularVelocity": -0.832597375451512, - "velocityX": 1.6512868465489152, - "velocityY": 0.2352542467718665, - "timestamp": 0.43040107681638023 - }, - { - "x": 1.2297644280001299, - "y": 6.743431356930112, - "heading": 0.7138654539660069, - "angularVelocity": -0.9700553628884108, - "velocityX": 1.9264974658869234, - "velocityY": 0.2745313279837331, - "timestamp": 0.5021345896191103 - }, - { - "x": 1.3877001578372599, - "y": 6.765944077192238, - "heading": 0.6344360529195374, - "angularVelocity": -1.1072844191579285, - "velocityX": 2.20170076285362, - "velocityY": 0.3138382519616173, - "timestamp": 0.5738681024218403 - }, - { - "x": 1.565376370208164, - "y": 6.791278571412394, - "heading": 0.545166048889513, - "angularVelocity": -1.2444672030567556, - "velocityX": 2.476892674769556, - "velocityY": 0.3531751511051906, - "timestamp": 0.6456016152245703 - }, - { - "x": 1.7627920329701503, - "y": 6.819436846094517, - "heading": 0.4460464917361558, - "angularVelocity": -1.381774756252758, - "velocityX": 2.7520701981153106, - "velocityY": 0.3925400218865401, - "timestamp": 0.7173351280273003 - }, - { - "x": 1.9404742499696706, - "y": 6.844889308454019, - "heading": 0.35698857434522513, - "angularVelocity": -1.241510611911704, - "velocityX": 2.4769763814130292, - "velocityY": 0.35481968414999915, - "timestamp": 0.7890686408300303 - }, - { - "x": 2.098418416235195, - "y": 6.8675162488116115, - "heading": 0.277805044936203, - "angularVelocity": -1.103856848738075, - "velocityX": 2.201818369967191, - "velocityY": 0.3154305354505728, - "timestamp": 0.8608021536327602 - }, - { - "x": 2.2366248722942026, - "y": 6.887316924283159, - "heading": 0.20850199362833047, - "angularVelocity": -0.9661181863426805, - "velocityX": 1.9266651056396258, - "velocityY": 0.2760310305869847, - "timestamp": 0.9325356664354902 - }, - { - "x": 2.3550937746268814, - "y": 6.9042907996604885, - "heading": 0.14908158317430456, - "angularVelocity": -0.8283493741238005, - "velocityX": 1.6515140227218135, - "velocityY": 0.2366240646620762, - "timestamp": 1.0042691792382203 - }, - { - "x": 2.453825154537734, - "y": 6.918437433437714, - "heading": 0.09954309354100455, - "angularVelocity": -0.6905905995159123, - "velocityX": 1.3763633762039458, - "velocityY": 0.19721094393093683, - "timestamp": 1.0760026920409504 - }, - { - "x": 2.5328189743834146, - "y": 6.929756449824835, - "heading": 0.05988427758276665, - "angularVelocity": -0.5528631512095152, - "velocityX": 1.101212204919556, - "velocityY": 0.1577925845048924, - "timestamp": 1.1477362048436806 - }, - { - "x": 2.5920751704296263, - "y": 6.938247542788083, - "heading": 0.030102483585013936, - "angularVelocity": -0.41517266909175576, - "velocityX": 0.8260601451480881, - "velocityY": 0.11836995881959977, - "timestamp": 1.2194697176464107 - }, - { - "x": 2.6315936809069487, - "y": 6.943910491933908, - "heading": 0.010195398523530809, - "angularVelocity": -0.27751443165456446, - "velocityX": 0.550907224280912, - "velocityY": 0.07894426074125552, - "timestamp": 1.2912032304491408 - }, - { - "x": 2.6513744618221935, - "y": 6.946745178456401, - "heading": 0.00016145502701412444, - "angularVelocity": -0.13987804413429022, - "velocityX": 0.27575369012733186, - "velocityY": 0.03951690645092257, - "timestamp": 1.3629367432518709 + "x": 0.6967295535333877, + "y": 6.66750179345797, + "heading": 0.982893163249758, + "angularVelocity": -0.14737604826251527, + "velocityX": 0.29094834799603586, + "velocityY": 0.04143420364739335, + "timestamp": 0.06786122358481142 + }, + { + "x": 0.7362175895643961, + "y": 6.673125696218821, + "heading": 0.9628974920292898, + "angularVelocity": -0.2946553298656345, + "velocityX": 0.5818939586560375, + "velocityY": 0.08287358323008266, + "timestamp": 0.13572244716962284 + }, + { + "x": 0.7954495269492323, + "y": 6.681562379154691, + "heading": 0.9329256679312776, + "angularVelocity": -0.44166347900512215, + "velocityX": 0.8728392188627362, + "velocityY": 0.1243225879257299, + "timestamp": 0.20358367075443426 + }, + { + "x": 0.8744254360510917, + "y": 6.692812849791712, + "heading": 0.8930052032103389, + "angularVelocity": -0.5882662087730711, + "velocityX": 1.1637855159383756, + "velocityY": 0.1657864394820339, + "timestamp": 0.2714448943392457 + }, + { + "x": 0.9731453894549041, + "y": 6.7068785148813665, + "heading": 0.843168820166726, + "angularVelocity": -0.7343867441666224, + "velocityX": 1.4547328826223493, + "velocityY": 0.20727102086621554, + "timestamp": 0.33930611792405707 + }, + { + "x": 1.0916093532691815, + "y": 6.7237612036761645, + "heading": 0.7834495219946941, + "angularVelocity": -0.8800209459441316, + "velocityX": 1.7456797498828438, + "velocityY": 0.2487825580347626, + "timestamp": 0.40716734150886846 + }, + { + "x": 1.2298170767085217, + "y": 6.743463170039788, + "heading": 0.7138752049500412, + "angularVelocity": -1.025244069725628, + "velocityX": 2.0366229215807086, + "velocityY": 0.29032730803917456, + "timestamp": 0.47502856509367986 + }, + { + "x": 1.3877679992818084, + "y": 6.765987144948878, + "heading": 0.6344639927011461, + "angularVelocity": -1.1702001239286361, + "velocityX": 2.3275578339061793, + "velocityY": 0.3319123015950357, + "timestamp": 0.5428897886784912 + }, + { + "x": 1.5654611541888466, + "y": 6.791337011948423, + "heading": 0.5452227484246948, + "angularVelocity": -1.3150550426624645, + "velocityX": 2.618478499506586, + "velocityY": 0.3735545228986894, + "timestamp": 0.6107510122633026 + }, + { + "x": 1.7628875214635322, + "y": 6.819592757066269, + "heading": 0.4462737595759067, + "angularVelocity": -1.4581079388455742, + "velocityX": 2.909266247284013, + "velocityY": 0.41637541478357637, + "timestamp": 0.678612235848114 + }, + { + "x": 1.9405727781814897, + "y": 6.845027020375549, + "heading": 0.357188812683824, + "angularVelocity": -1.3127518512946696, + "velocityX": 2.6183621121403804, + "velocityY": 0.3747981831994576, + "timestamp": 0.7464734594329254 + }, + { + "x": 2.098517679535123, + "y": 6.867636881191052, + "heading": 0.2779750280562074, + "angularVelocity": -1.1672908392024057, + "velocityX": 2.3274691054787353, + "velocityY": 0.3331779125268215, + "timestamp": 0.8143346830177368 + }, + { + "x": 2.236722632634811, + "y": 6.887421048029417, + "heading": 0.20863780165516146, + "angularVelocity": -1.0217503124503762, + "velocityX": 2.036582097977683, + "velocityY": 0.29153861061228065, + "timestamp": 0.8821959066025482 + }, + { + "x": 2.3551878284960197, + "y": 6.9043786966768, + "heading": 0.1491789973537457, + "angularVelocity": -0.8761823197470863, + "velocityX": 1.745697905272119, + "velocityY": 0.24988716311885253, + "timestamp": 0.9500571301873596 + }, + { + "x": 2.4539133200632923, + "y": 6.918509209574482, + "heading": 0.09959774943286472, + "angularVelocity": -0.7306270842422328, + "velocityX": 1.454814492755025, + "velocityY": 0.20822661530737346, + "timestamp": 1.017918353772171 + }, + { + "x": 2.5328990844805293, + "y": 6.929812092655753, + "heading": 0.059891727352470836, + "angularVelocity": -0.5851061914728107, + "velocityX": 1.163930743431445, + "velocityY": 0.1665587869506308, + "timestamp": 1.0857795773569825 + }, + { + "x": 2.592145068506356, + "y": 6.938286954912776, + "heading": 0.030058218690118557, + "angularVelocity": -0.43962526884100517, + "velocityX": 0.8730462095453193, + "velocityY": 0.12488519671960763, + "timestamp": 1.153640800941794 + }, + { + "x": 2.63165121786692, + "y": 6.943933511859112, + "heading": 0.010094853186468074, + "angularVelocity": -0.2941792742463702, + "velocityX": 0.582160875882081, + "velocityY": 0.08320741430898287, + "timestamp": 1.2215020245266055 }, { "x": 2.6514174938201904, "y": 6.946751594543457, "heading": 0, - "angularVelocity": -0.0022507610818009623, - "velocityX": 0.0005998861532762668, - "velocityY": 0.00008944325609789668, - "timestamp": 1.434670256054601 - }, - { - "x": 2.631713436254265, - "y": 6.943928504258672, - "heading": 0.00971593718624953, - "angularVelocity": 0.13541279143473714, - "velocityX": -0.27461904975679574, - "velocityY": -0.03934592501075656, - "timestamp": 1.506420768370774 - }, - { - "x": 2.592262356755882, - "y": 6.938276121500362, - "heading": 0.029312137508313962, - "angularVelocity": 0.27311582404564183, - "velocityX": -0.5498369025337515, - "velocityY": -0.07877829117207064, - "timestamp": 1.578171280686947 - }, - { - "x": 2.533064330899613, - "y": 6.929794745308614, - "heading": 0.058791626722930285, - "angularVelocity": 0.41086102718133133, - "velocityX": -0.8250537020140929, - "velocityY": -0.11820648955702064, - "timestamp": 1.64992179300312 - }, - { - "x": 2.4541194029599502, - "y": 6.918484740607142, - "heading": 0.09815683939208576, - "angularVelocity": 0.5486401612526653, - "velocityX": -1.1002698844327412, - "velocityY": -0.15762960193345896, - "timestamp": 1.721672305319293 - }, - { - "x": 2.355427542032357, - "y": 6.9043465299042746, - "heading": 0.1474084549881005, - "angularVelocity": 0.6864287652544176, - "velocityX": -1.3754864975735484, - "velocityY": -0.19704682573406979, - "timestamp": 1.793422817635466 - }, - { - "x": 2.23698858427003, - "y": 6.887380609317409, - "heading": 0.20654397577631692, - "angularVelocity": 0.8241825578726235, - "velocityX": -1.650705394277179, - "velocityY": -0.23645713505578408, - "timestamp": 1.865173329951639 - }, - { - "x": 2.098802169913165, - "y": 6.86758760630337, - "heading": 0.27555650233579015, - "angularVelocity": 0.9618401920015576, - "velocityX": -1.9259293050189108, - "velocityY": -0.2758586993970149, - "timestamp": 1.9369238422678121 - }, - { - "x": 1.9408676962603424, - "y": 6.844968393093841, - "heading": 0.3544346454359525, - "angularVelocity": 1.0993390923447317, - "velocityX": -2.2011616157204346, - "velocityY": -0.3152481072856972, - "timestamp": 2.0086743545839854 - }, - { - "x": 1.7631843231894517, - "y": 6.819524255776015, - "heading": 0.44316502681503017, - "angularVelocity": 1.2366515374122595, - "velocityX": -2.4764056353572657, - "velocityY": -0.35461959074183413, - "timestamp": 2.0804248669001586 - }, - { - "x": 1.5657510840322728, - "y": 6.791257095186479, - "heading": 0.5417391237624924, - "angularVelocity": 1.373845199915075, - "velocityX": -2.751663127625576, - "velocityY": -0.3939645818900745, - "timestamp": 2.152175379216332 - }, - { - "x": 1.387996398598156, - "y": 6.765933063404474, - "heading": 0.6316668804185087, - "angularVelocity": 1.2533395757259034, - "velocityX": -2.477399529588293, - "velocityY": -0.352945658067143, - "timestamp": 2.223925891532505 - }, - { - "x": 1.2299927628795557, - "y": 6.743427858422989, - "heading": 0.7117267031520905, - "angularVelocity": 1.115808377635633, - "velocityX": -2.202125540862631, - "velocityY": -0.3136591539003019, - "timestamp": 2.2956764038486783 - }, - { - "x": 1.091740243274482, - "y": 6.723739456363059, - "heading": 0.7818852162777382, - "angularVelocity": 0.9778120165089851, - "velocityX": -1.9268506267358863, - "velocityY": -0.2744008569054998, - "timestamp": 2.3674269161648516 - }, - { - "x": 0.9732386614039245, - "y": 6.706866137144222, - "heading": 0.842102674742542, - "angularVelocity": 0.8392617213147597, - "velocityX": -1.6515781987100508, - "velocityY": -0.2351665329867072, - "timestamp": 2.439177428481025 - }, - { - "x": 0.8744877098342877, - "y": 6.692806537214817, - "heading": 0.8923385972076003, - "angularVelocity": 0.7001472303020632, - "velocityX": -1.3763100554424192, - "velocityY": -0.19595121313677477, - "timestamp": 2.510927940797198 - }, - { - "x": 0.7954870665575497, - "y": 6.68155965440474, - "heading": 0.9325570015240306, - "angularVelocity": 0.5605312495048624, - "velocityX": -1.1010464000594427, - "velocityY": -0.15674986071479513, - "timestamp": 2.5826784531133713 - }, - { - "x": 0.736236493451904, - "y": 6.673124818793258, - "heading": 0.962730596118369, - "angularVelocity": 0.42053490106098795, - "velocityX": -0.8257860634235991, - "velocityY": -0.11755784510057492, - "timestamp": 2.6544289654295445 - }, - { - "x": 0.6967359103772861, - "y": 6.667501649956908, - "heading": 0.9828437467864926, - "angularVelocity": 0.28032065585715155, - "velocityX": -0.5505268437144837, - "velocityY": -0.07837113151770157, - "timestamp": 2.726179477745718 + "angularVelocity": -0.14875731166049128, + "velocityX": 0.2912749713180046, + "velocityY": 0.041527142239385896, + "timestamp": 1.289363248111417 + }, + { + "x": 2.649146332814054, + "y": 6.946410455915093, + "heading": 0.0008990727847708372, + "angularVelocity": 0.011985225672082133, + "velocityX": -0.030276055128411465, + "velocityY": -0.004547600056030398, + "timestamp": 1.3643783383040946 + }, + { + "x": 2.62275408560099, + "y": 6.9426132004627625, + "heading": 0.013859521326443051, + "angularVelocity": 0.17277121854260333, + "velocityX": -0.35182584124441, + "velocityY": -0.050619887846258024, + "timestamp": 1.4393934284967722 + }, + { + "x": 2.572240863242063, + "y": 6.93536015641962, + "heading": 0.03888546297295733, + "angularVelocity": 0.3336120983422759, + "velocityX": -0.6733741468440916, + "velocityY": -0.09668780007480728, + "timestamp": 1.5144085186894498 + }, + { + "x": 2.4976067609609713, + "y": 6.924651766873113, + "heading": 0.07598096942338527, + "angularVelocity": 0.4945072565419495, + "velocityX": -0.994921183049867, + "velocityY": -0.14274980565911422, + "timestamp": 1.5894236088821274 + }, + { + "x": 2.398851800518621, + "y": 6.910488564953806, + "heading": 0.12514851990507653, + "angularVelocity": 0.6554354644565975, + "velocityX": -1.3164679291686043, + "velocityY": -0.18880470426588675, + "timestamp": 1.664438699074805 + }, + { + "x": 2.275975845765142, + "y": 6.89287117810912, + "heading": 0.18638682928509862, + "angularVelocity": 0.8163465407124133, + "velocityX": -1.6380164902537586, + "velocityY": -0.23485123858993803, + "timestamp": 1.7394537892674826 + }, + { + "x": 2.1289785005722504, + "y": 6.871800394460987, + "heading": 0.2596886166153821, + "angularVelocity": 0.9771605571893133, + "velocityX": -1.95957033198688, + "velocityY": -0.28088726673543657, + "timestamp": 1.8144688794601602 + }, + { + "x": 1.9578590220057892, + "y": 6.847277322910046, + "heading": 0.34503981582540255, + "angularVelocity": 1.1377870637866898, + "velocityX": -2.2811340775161044, + "velocityY": -0.3269085125133225, + "timestamp": 1.8894839696528378 + }, + { + "x": 1.7626163173700675, + "y": 6.8193036552908985, + "heading": 0.4424228597913738, + "angularVelocity": 1.298179389184776, + "velocityX": -2.602712389390415, + "velocityY": -0.3729072050343056, + "timestamp": 1.9644990598455154 + }, + { + "x": 1.5454887933501882, + "y": 6.788364934906598, + "heading": 0.5521114777288451, + "angularVelocity": 1.4622207032709544, + "velocityX": -2.8944512825643915, + "velocityY": -0.4124332891533426, + "timestamp": 2.039514150038193 + }, + { + "x": 1.3524867399067801, + "y": 6.760871330718944, + "heading": 0.6497679920992822, + "angularVelocity": 1.3018249277525966, + "velocityX": -2.5728430499473998, + "velocityY": -0.3665076468885929, + "timestamp": 2.114529240230871 + }, + { + "x": 1.1836105740084832, + "y": 6.736819962879357, + "heading": 0.7353592838772256, + "angularVelocity": 1.140987654058677, + "velocityX": -2.251229258866918, + "velocityY": -0.3206203948806917, + "timestamp": 2.1895443304235487 + }, + { + "x": 1.0388602648983114, + "y": 6.7162082631102376, + "heading": 0.8088372428736526, + "angularVelocity": 0.9795090402170741, + "velocityX": -1.9296158777970915, + "velocityY": -0.2747673796855824, + "timestamp": 2.2645594206162265 + }, + { + "x": 0.9182354949315064, + "y": 6.6990341480262074, + "heading": 0.8701477848986364, + "angularVelocity": 0.8173094489056358, + "velocityX": -1.6080067311387376, + "velocityY": -0.22894213737419178, + "timestamp": 2.3395745108089043 + }, + { + "x": 0.8217358494246105, + "y": 6.685296075556227, + "heading": 0.919240073281317, + "angularVelocity": 0.6544321716682079, + "velocityX": -1.286403112480899, + "velocityY": -0.18313745187394112, + "timestamp": 2.414589601001582 + }, + { + "x": 0.7493609915443247, + "y": 6.674993018423988, + "heading": 0.956074222977567, + "angularVelocity": 0.49102320082053863, + "velocityX": -0.9648039840302745, + "velocityY": -0.13734646063578734, + "timestamp": 2.48960469119426 + }, + { + "x": 0.7011107973318162, + "y": 6.66812439818581, + "heading": 0.9806268225378217, + "angularVelocity": 0.32730214010529163, + "velocityX": -0.6432065080316098, + "velocityY": -0.09156318042856706, + "timestamp": 2.564619781386938 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": 0.14007614861882017, - "velocityX": -0.2752658776108563, - "velocityY": -0.0391862324932835, - "timestamp": 2.797929990061891 + "angularVelocity": 0.16353322568319786, + "velocityX": -0.32160668782044516, + "velocityY": -0.04578252824590232, + "timestamp": 2.6396348715796156 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": 0, - "velocityX": -1.632165481129405e-29, - "velocityY": 0, - "timestamp": 2.8696805023780643 - }, - { - "x": 0.6961023495919454, - "y": 6.667365584217812, - "heading": 0.984880267445541, - "angularVelocity": -0.11368287336653356, - "velocityX": 0.27118304316961456, - "velocityY": 0.03795427117017962, - "timestamp": 2.940174979946086 - }, - { - "x": 0.7343390561697011, - "y": 6.672718275572268, - "heading": 0.9689850788648127, - "angularVelocity": -0.2254813302984037, - "velocityX": 0.5424071203430141, - "velocityY": 0.07593064789069806, - "timestamp": 3.0106694575141075 - }, - { - "x": 0.7916989777682265, - "y": 6.680749944426173, - "heading": 0.945369919685869, - "angularVelocity": -0.33499303766251487, - "velocityX": 0.8136796466528573, - "velocityY": 0.11393330557213717, - "timestamp": 3.081163935082129 - }, - { - "x": 0.8681861161076095, - "y": 6.691462782987562, - "heading": 0.9142283558404658, - "angularVelocity": -0.441758913885903, - "velocityX": 1.0850089393964117, - "velocityY": 0.1519670608389448, - "timestamp": 3.151658412650151 - }, - { - "x": 0.9638051470354019, - "y": 6.704859377756673, - "heading": 0.875792088105272, - "angularVelocity": -0.5452379968077065, - "velocityX": 1.3564045614144413, - "velocityY": 0.19003750692646, - "timestamp": 3.2221528902181724 - }, - { - "x": 1.078561545508893, - "y": 6.720942777481531, - "heading": 0.830339664632417, - "angularVelocity": -0.6447657325922732, - "velocityX": 1.627877848484808, - "velocityY": 0.2281512010545656, - "timestamp": 3.292647367786194 - }, - { - "x": 1.2124617638424005, - "y": 6.739716584680947, - "heading": 0.7782100744037193, - "angularVelocity": -0.7394847373455183, - "velocityX": 1.8994426649137832, - "velocityY": 0.26631599874331907, - "timestamp": 3.3631418453542157 - }, - { - "x": 1.3655134797863553, - "y": 6.761185092164687, - "heading": 0.7198245324523541, - "angularVelocity": -0.8282285927295209, - "velocityX": 2.1711163941355873, - "velocityY": 0.30454169212084536, - "timestamp": 3.4336363229222373 - }, - { - "x": 1.5377259215824886, - "y": 6.785353503086406, - "heading": 0.6557224279689098, - "angularVelocity": -0.909320938247837, - "velocityX": 2.4429210306575064, - "velocityY": 0.3428411948779657, - "timestamp": 3.504130800490259 - }, - { - "x": 1.7291102325420633, - "y": 6.812228299208335, - "heading": 0.5866232271908158, - "angularVelocity": -0.9802072894492893, - "velocityX": 2.714883740714364, - "velocityY": 0.3812326447273405, - "timestamp": 3.5746252780582806 - }, - { - "x": 1.9396796543838148, - "y": 6.8418178634961455, - "heading": 0.5135407387337584, - "angularVelocity": -1.0367122500700692, - "velocityX": 2.987034291282877, - "velocityY": 0.4197430112062252, - "timestamp": 3.645119755626302 - }, - { - "x": 2.1694484773986367, - "y": 6.874133526026527, - "heading": 0.4380194057033538, - "angularVelocity": -1.0713085001237521, - "velocityX": 3.259387556892149, - "velocityY": 0.45841410058255366, - "timestamp": 3.715614233194324 - }, - { - "x": 2.418423965072986, - "y": 6.9091911928007494, - "heading": 0.362723869672428, - "angularVelocity": -1.0681054549027866, - "velocityX": 3.531843858749197, - "velocityY": 0.4973108246726942, - "timestamp": 3.7861087107623455 - }, - { - "x": 2.6865429147293822, - "y": 6.947011884235923, - "heading": 0.29352452528672673, - "angularVelocity": -0.9816278774309596, - "velocityX": 3.803403598497242, - "velocityY": 0.5365057340651915, - "timestamp": 3.856603188330367 - }, - { - "x": 2.9720561979068543, - "y": 6.987623777247046, - "heading": 0.25938945355425436, - "angularVelocity": -0.48422334500649106, - "velocityX": 4.050151061861196, - "velocityY": 0.5761003473206351, - "timestamp": 3.9270976658983887 - }, - { - "x": 3.2671083841789206, - "y": 7.028939851633195, - "heading": 0.25938943415021554, - "angularVelocity": -2.752561543209048e-7, - "velocityX": 4.185465251336388, - "velocityY": 0.5860895180942723, - "timestamp": 3.9975921434664103 - }, - { - "x": 3.562160580898253, - "y": 7.070255851413062, - "heading": 0.25938941474662836, - "angularVelocity": -2.7524974860351036e-7, - "velocityX": 4.185465399536173, - "velocityY": 0.5860884597662348, - "timestamp": 4.068086621034432 - }, - { - "x": 3.8572127776182663, - "y": 7.111571851188067, - "heading": 0.25938939534304106, - "angularVelocity": -2.7524974884736384e-7, - "velocityX": 4.185465399545831, - "velocityY": 0.5860884596972535, - "timestamp": 4.138581098602454 - }, - { - "x": 4.15226497433828, - "y": 7.1528878509630704, - "heading": 0.25938937593945377, - "angularVelocity": -2.752497497523569e-7, - "velocityX": 4.185465399545832, - "velocityY": 0.5860884596972492, - "timestamp": 4.209075576170475 - }, - { - "x": 4.447317171058294, - "y": 7.194203850738075, - "heading": 0.25938935653586653, - "angularVelocity": -2.7524974896088886e-7, - "velocityX": 4.185465399545833, - "velocityY": 0.5860884596972492, - "timestamp": 4.279570053738497 - }, - { - "x": 4.742369367778308, - "y": 7.2355198505130724, - "heading": 0.2593893371322792, - "angularVelocity": -2.752497503383158e-7, - "velocityX": 4.185465399545844, - "velocityY": 0.5860884596971616, - "timestamp": 4.3500645313065185 - }, - { - "x": 5.037421564511595, - "y": 7.276835850193286, - "heading": 0.2593893177286919, - "angularVelocity": -2.7524974902410266e-7, - "velocityX": 4.185465399734123, - "velocityY": 0.5860884583526031, - "timestamp": 4.42055900887454 - }, - { - "x": 5.3324739648733965, - "y": 7.318150395663973, - "heading": 0.2593892983250951, - "angularVelocity": -2.7524988424278287e-7, - "velocityX": 4.185468288308109, - "velocityY": 0.5860678296512251, - "timestamp": 4.491053486442562 - }, - { - "x": 5.628942809727294, - "y": 7.346853994253202, - "heading": 0.25912444072738805, - "angularVelocity": -0.0037571396631956044, - "velocityX": 4.205561273474639, - "velocityY": 0.4071751373933052, - "timestamp": 4.561547964010583 - }, - { - "x": 5.9078202008377065, - "y": 7.3726601599906, - "heading": 0.2260522693823342, - "angularVelocity": -0.4691455626881114, - "velocityX": 3.9560175595502307, - "velocityY": 0.3660735794870859, - "timestamp": 4.632042441578605 - }, - { - "x": 6.167565330820035, - "y": 7.395632554305088, - "heading": 0.18684536926234946, - "angularVelocity": -0.5561698089350797, - "velocityX": 3.684616709609561, - "velocityY": 0.32587509131225256, - "timestamp": 4.702536919146627 - }, - { - "x": 6.408110614275644, - "y": 7.415818749602641, - "heading": 0.14706352599281394, - "angularVelocity": -0.5643256697824207, - "velocityX": 3.412257126432383, - "velocityY": 0.28635144189947087, - "timestamp": 4.773031396714648 - }, - { - "x": 6.629457805462712, - "y": 7.433245452372472, - "heading": 0.10922390474114492, - "angularVelocity": -0.536774263135107, - "velocityX": 3.1399224282992484, - "velocityY": 0.2472066376123601, - "timestamp": 4.84352587428267 - }, - { - "x": 6.831617157798908, - "y": 7.447929110099442, - "heading": 0.07477835738023303, - "angularVelocity": -0.48862759962544244, - "velocityX": 2.867733180108019, - "velocityY": 0.2082951492590427, - "timestamp": 4.9140203518506915 - }, - { - "x": 7.014599005404042, - "y": 7.459880739385679, - "heading": 0.04467522695346215, - "angularVelocity": -0.427028207957479, - "velocityX": 2.59569052665965, - "velocityY": 0.169539937007193, - "timestamp": 4.984514829418713 - }, - { - "x": 7.178412353011271, - "y": 7.469108205463054, - "heading": 0.019583414088526187, - "angularVelocity": -0.35594012085165694, - "velocityX": 2.3237756099286244, - "velocityY": 0.1308962970676859, - "timestamp": 5.055009306986735 + "angularVelocity": -9.098710086763465e-32, + "velocityX": -1.2738111607141702e-32, + "velocityY": 8.271252813231263e-33, + "timestamp": 2.7146499617722935 + }, + { + "x": 0.6985020843742907, + "y": 6.667569321822967, + "heading": 0.9798246448067045, + "angularVelocity": -0.18429871952070437, + "velocityX": 0.3034123592993086, + "velocityY": 0.04060189632535011, + "timestamp": 2.7855654702426254 + }, + { + "x": 0.7415443471125377, + "y": 6.673333307735536, + "heading": 0.9539749013573504, + "angularVelocity": -0.36451467396822734, + "velocityX": 0.6069513378199061, + "velocityY": 0.081279624681546, + "timestamp": 2.8564809787129573 + }, + { + "x": 0.8061231921601518, + "y": 6.681988659505296, + "heading": 0.9157093279961269, + "angularVelocity": -0.5395938658076773, + "velocityX": 0.9106448848862336, + "velocityY": 0.12205160699625474, + "timestamp": 2.927396487183289 + }, + { + "x": 0.8922518573326212, + "y": 6.693543414119111, + "heading": 0.8654790283080297, + "angularVelocity": -0.7083119161320186, + "velocityX": 1.2145251021996393, + "velocityY": 0.16293692117639821, + "timestamp": 2.998311995653621 + }, + { + "x": 0.9999464058047857, + "y": 6.708007002320034, + "heading": 0.8038456081958845, + "angularVelocity": -0.8691105999462724, + "velocityX": 1.5186318309657076, + "velocityY": 0.20395522097926097, + "timestamp": 3.069227504123953 + }, + { + "x": 1.1292266926067027, + "y": 6.725390389005233, + "heading": 0.7315277652632032, + "angularVelocity": -1.0197747219557198, + "velocityX": 1.8230185412264566, + "velocityY": 0.2451281399536478, + "timestamp": 3.140143012594285 + }, + { + "x": 1.280117879093583, + "y": 6.7457065764596775, + "heading": 0.6494907937479546, + "angularVelocity": -1.1568269520279806, + "velocityX": 2.127760059000449, + "velocityY": 0.28648440789144525, + "timestamp": 3.2110585210646168 + }, + { + "x": 1.452652377278095, + "y": 6.768971929861101, + "heading": 0.5591180429519423, + "angularVelocity": -1.2743721753587984, + "velocityX": 2.4329586278958, + "velocityY": 0.3280714459116738, + "timestamp": 3.2819740295349487 + }, + { + "x": 1.6468709656789369, + "y": 6.795208953072923, + "heading": 0.46255966504991763, + "angularVelocity": -1.361597483890574, + "velocityX": 2.738732226422582, + "velocityY": 0.3699758173883681, + "timestamp": 3.3528895380052806 + }, + { + "x": 1.8628151056303066, + "y": 6.824451100190135, + "heading": 0.3635782548385496, + "angularVelocity": -1.3957653600238602, + "velocityX": 3.045090483158721, + "velocityY": 0.41235193468922193, + "timestamp": 3.4238050464756125 + }, + { + "x": 2.100434676341306, + "y": 6.8567478442017284, + "heading": 0.27059443428530877, + "angularVelocity": -1.3111916216766784, + "velocityX": 3.3507419721937035, + "velocityY": 0.4554256848500797, + "timestamp": 3.4947205549459444 + }, + { + "x": 2.35596385739112, + "y": 6.892112273565999, + "heading": 0.23346194806127452, + "angularVelocity": -0.5236158778945916, + "velocityX": 3.603290543375525, + "velocityY": 0.4986839991292765, + "timestamp": 3.5656360634162763 + }, + { + "x": 2.621781213038304, + "y": 6.927777327664925, + "heading": 0.23346192227272974, + "angularVelocity": -3.6365169422849345e-7, + "velocityX": 3.7483670551187007, + "velocityY": 0.5029231950560806, + "timestamp": 3.636551571886608 + }, + { + "x": 2.887598578486195, + "y": 6.963442308718448, + "heading": 0.23346189648452345, + "angularVelocity": -3.636469208413295e-7, + "velocityX": 3.748367193321304, + "velocityY": 0.502922165021807, + "timestamp": 3.70746708035694 + }, + { + "x": 3.153415943934485, + "y": 6.999107289769, + "heading": 0.23346187069631716, + "angularVelocity": -3.636469205820025e-7, + "velocityX": 3.7483671933269203, + "velocityY": 0.5029221649799431, + "timestamp": 3.778382588827272 + }, + { + "x": 3.4192333093827747, + "y": 7.034772270819553, + "heading": 0.2334618449081109, + "angularVelocity": -3.6364692054215583e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799415, + "timestamp": 3.849298097297604 + }, + { + "x": 3.6850506748310643, + "y": 7.070437251870106, + "heading": 0.23346181911990457, + "angularVelocity": -3.6364692122502775e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799416, + "timestamp": 3.9202136057679358 + }, + { + "x": 3.950868040279354, + "y": 7.106102232920659, + "heading": 0.2334617933316983, + "angularVelocity": -3.636469204641407e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799416, + "timestamp": 3.9911291142382677 + }, + { + "x": 4.216685405727644, + "y": 7.141767213971211, + "heading": 0.23346176754349196, + "angularVelocity": -3.6364692150109834e-7, + "velocityX": 3.7483671933269203, + "velocityY": 0.5029221649799415, + "timestamp": 4.0620446227086 + }, + { + "x": 4.482502771175934, + "y": 7.1774321950217645, + "heading": 0.2334617417552857, + "angularVelocity": -3.636469207728054e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799417, + "timestamp": 4.1329601311789315 + }, + { + "x": 4.7483201366242245, + "y": 7.213097176072317, + "heading": 0.2334617159670794, + "angularVelocity": -3.636469206602435e-7, + "velocityX": 3.7483671933269203, + "velocityY": 0.5029221649799417, + "timestamp": 4.203875639649263 + }, + { + "x": 5.014137502072515, + "y": 7.2487621571228695, + "heading": 0.2334616901788731, + "angularVelocity": -3.6364692079095153e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799416, + "timestamp": 4.274791148119595 + }, + { + "x": 5.279954867520805, + "y": 7.284427138173423, + "heading": 0.23346166439066685, + "angularVelocity": -3.636469205268936e-7, + "velocityX": 3.7483671933269207, + "velocityY": 0.5029221649799417, + "timestamp": 4.345706656589927 + }, + { + "x": 5.545772232969095, + "y": 7.320092119223974, + "heading": 0.23346163860246044, + "angularVelocity": -3.6364692217914213e-7, + "velocityX": 3.748367193326924, + "velocityY": 0.5029221649799175, + "timestamp": 4.416622165060259 + }, + { + "x": 5.81158959842306, + "y": 7.355757100232221, + "heading": 0.23346161281425412, + "angularVelocity": -3.636469211715136e-7, + "velocityX": 3.748367193406962, + "velocityY": 0.5029221643833769, + "timestamp": 4.487537673530591 + }, + { + "x": 6.077407103529163, + "y": 7.391421040372378, + "heading": 0.23346158702575517, + "angularVelocity": -3.63651047258994e-7, + "velocityX": 3.748369162681932, + "velocityY": 0.5029074868027953, + "timestamp": 4.558453182000923 + }, + { + "x": 6.338444265915037, + "y": 7.413466456985474, + "heading": 0.21125647663717811, + "angularVelocity": -0.3131206539662162, + "velocityX": 3.680960173825454, + "velocityY": 0.310868766065739, + "timestamp": 4.629368690471255 + }, + { + "x": 6.578740605850335, + "y": 7.432100572083313, + "heading": 0.16288336837633732, + "angularVelocity": -0.6821231251705402, + "velocityX": 3.388487865610223, + "velocityY": 0.26276502135825913, + "timestamp": 4.700284198941587 + }, + { + "x": 6.797330438016248, + "y": 7.447554226429222, + "heading": 0.11319688856078455, + "angularVelocity": -0.7006433555551449, + "velocityX": 3.082398150714271, + "velocityY": 0.21791642870860864, + "timestamp": 4.771199707411919 + }, + { + "x": 6.99422444679472, + "y": 7.45992830223967, + "heading": 0.06789887708325426, + "angularVelocity": -0.638760300174411, + "velocityX": 2.7764590993639087, + "velocityY": 0.1744904052351846, + "timestamp": 4.8421152158822505 + }, + { + "x": 7.169459125576906, + "y": 7.4692727675317165, + "heading": 0.029649915927990267, + "angularVelocity": -0.539359612309142, + "velocityX": 2.4710346518279107, + "velocityY": 0.13176899515507345, + "timestamp": 4.913030724352582 }, { "x": 7.323064804077148, "y": 7.475617408752441, - "heading": 2.960760167883615e-32, - "angularVelocity": -0.2778006840270544, - "velocityX": 2.051968552094028, - "velocityY": 0.09233635759774637, - "timestamp": 5.125503784554756 - }, - { - "x": 7.4453229174359326, - "y": 7.479377686242559, - "heading": -0.013449699218163923, - "angularVelocity": -0.19675250253186094, - "velocityX": 1.7884853310086555, - "velocityY": 0.055008219469756155, - "timestamp": 5.193862250368862 - }, - { - "x": 7.549531921420882, - "y": 7.480799104036024, - "heading": -0.02185072478369098, - "angularVelocity": -0.12289663709499957, - "velocityX": 1.5244491336060073, - "velocityY": 0.020793588278158227, - "timestamp": 5.262220716182967 - }, - { - "x": 7.635663569022224, - "y": 7.480054976594767, - "heading": -0.02560203298601703, - "angularVelocity": -0.05487700985752643, - "velocityX": 1.259999717307419, - "velocityY": -0.010885666206756989, - "timestamp": 5.330579181997073 - }, - { - "x": 7.703696133906866, - "y": 7.477289120755522, - "heading": -0.025033985130795043, - "angularVelocity": 0.008309839146576752, - "velocityX": 0.9952324715661438, - "velocityY": -0.0404610578412588, - "timestamp": 5.398937647811178 - }, - { - "x": 7.753612501256561, - "y": 7.472622767647336, - "heading": -0.02042473052193369, - "angularVelocity": 0.06742770707282683, - "velocityX": 0.7302148571537285, - "velocityY": -0.06826298765796995, - "timestamp": 5.467296113625284 - }, - { - "x": 7.785398915822185, - "y": 7.46615947927951, - "heading": -0.012011716304774139, - "angularVelocity": 0.12307201627429676, - "velocityX": 0.46499602042070654, - "velocityY": -0.09454993307489655, - "timestamp": 5.535654579439389 + "heading": -2.508046324740631e-30, + "angularVelocity": -0.41810200007794424, + "velocityX": 2.166037892325118, + "velocityY": 0.08946761234010188, + "timestamp": 4.983946232822914 + }, + { + "x": 7.442670757639025, + "y": 7.478903848182195, + "heading": -0.0187747254681701, + "angularVelocity": -0.297438409871576, + "velocityX": 1.8948561830587192, + "velocityY": 0.05206538544539075, + "timestamp": 5.04706762147034 + }, + { + "x": 7.5451140174844875, + "y": 7.480040188586648, + "heading": -0.030609169580020613, + "angularVelocity": -0.1874870684158837, + "velocityX": 1.6229563708994625, + "velocityY": 0.01800246206270259, + "timestamp": 5.110189010117766 + }, + { + "x": 7.630363412663352, + "y": 7.479184974890449, + "heading": -0.03600750245908947, + "angularVelocity": -0.08552303735288487, + "velocityX": 1.3505627332580479, + "velocityY": -0.013548714857401807, + "timestamp": 5.173310398765192 + }, + { + "x": 7.698396363869973, + "y": 7.476461754192338, + "heading": -0.03536054977456298, + "angularVelocity": 0.01024934175875415, + "velocityX": 1.0778113831814056, + "velocityY": -0.04314259803938343, + "timestamp": 5.236431787412618 + }, + { + "x": 7.74919584977197, + "y": 7.471969501335234, + "heading": -0.02898050014133947, + "angularVelocity": 0.10107587570456572, + "velocityX": 0.8047903728082677, + "velocityY": -0.0711684732126695, + "timestamp": 5.299553176060044 + }, + { + "x": 7.782748628307226, + "y": 7.465789287122271, + "heading": -0.017122702576199005, + "angularVelocity": 0.1878570452777808, + "velocityX": 0.5315595751917994, + "velocityY": -0.09790998495750272, + "timestamp": 5.36267456470747 }, { "x": 7.799044132232666, "y": 7.457988739013672, "heading": 0, - "angularVelocity": 0.17571658699068654, - "velocityX": 0.19961267778578498, - "velocityY": -0.11952784733433494, - "timestamp": 5.604013045253494 - }, - { - "x": 7.787627597101066, - "y": 7.445185981968696, - "heading": 0.02043091826358884, - "angularVelocity": 0.23842630400056083, - "velocityX": -0.1332295612366535, - "velocityY": -0.14940660051930396, - "timestamp": 5.6897037510220185 - }, - { - "x": 7.7476875027283105, - "y": 7.429823751171704, - "heading": 0.04606214764205663, - "angularVelocity": 0.299113295293718, - "velocityX": -0.466095990394167, - "velocityY": -0.17927534449873192, - "timestamp": 5.775394456790543 - }, - { - "x": 7.679221480716383, - "y": 7.41190305033205, - "heading": 0.07668903088242017, - "angularVelocity": 0.35741196160871463, - "velocityX": -0.7989900584653148, - "velocityY": -0.20913237531340728, - "timestamp": 5.861085162559067 - }, - { - "x": 7.582226808615669, - "y": 7.391425060459508, - "heading": 0.11206636753872803, - "angularVelocity": 0.41284916886870227, - "velocityX": -1.1319158971887242, - "velocityY": -0.2389756238892322, - "timestamp": 5.946775868327591 - }, - { - "x": 7.456700337139081, - "y": 7.36839118812004, - "heading": 0.1518948027840275, - "angularVelocity": 0.4647929421060861, - "velocityX": -1.4648784877052072, - "velocityY": -0.26880245801323005, - "timestamp": 6.032466574096115 - }, - { - "x": 7.302638403414429, - "y": 7.342803144016317, - "heading": 0.19579992682168443, - "angularVelocity": 0.5123673990532579, - "velocityX": -1.7978838234897747, - "velocityY": -0.2986093284474086, - "timestamp": 6.118157279864639 - }, - { - "x": 7.12003674107026, - "y": 7.314663079535023, - "heading": 0.24329864617596214, - "angularVelocity": 0.5543042145385703, - "velocityX": -2.1309389473046063, - "velocityY": -0.3283910924634972, - "timestamp": 6.203847985633163 - }, - { - "x": 6.908890435767407, - "y": 7.283973843410994, - "heading": 0.29374179011633406, - "angularVelocity": 0.5886652874190753, - "velocityX": -2.4640514208532833, - "velocityY": -0.35813961209433026, - "timestamp": 6.289538691401687 - }, - { - "x": 6.669194114784965, - "y": 7.250739514431078, - "heading": 0.34620821502428967, - "angularVelocity": 0.6122767275331199, - "velocityX": -2.797226593394282, - "velocityY": -0.3878405327841778, - "timestamp": 6.375229397170211 - }, - { - "x": 6.400943140096472, - "y": 7.214966665234931, - "heading": 0.3992869291597751, - "angularVelocity": 0.6194220675328131, - "velocityX": -3.1304558911338245, - "velocityY": -0.41746475157736485, - "timestamp": 6.4609201029387355 - }, - { - "x": 6.104139554987064, - "y": 7.176668008880963, - "heading": 0.4505490205752772, - "angularVelocity": 0.5982223037580753, - "velocityX": -3.4636613439870674, - "velocityY": -0.44694061054210893, - "timestamp": 6.54661080870726 - }, - { - "x": 5.778829029190598, - "y": 7.135876707023435, - "heading": 0.4948586572551853, - "angularVelocity": 0.5170880118504484, - "velocityX": -3.796333836661644, - "velocityY": -0.4760294770790753, - "timestamp": 6.632301514475784 - }, - { - "x": 5.425610556727763, - "y": 7.092704257460775, - "heading": 0.5143700783127158, - "angularVelocity": 0.22769588466497723, - "velocityX": -4.122016142765593, - "velocityY": -0.5038171780179022, - "timestamp": 6.717992220244308 - }, - { - "x": 5.0649174675452295, - "y": 7.060199768590457, - "heading": 0.5143700896531384, - "angularVelocity": 1.323413376854098e-7, - "velocityX": -4.209244000823966, - "velocityY": -0.3793233884444995, - "timestamp": 6.803682926012832 - }, - { - "x": 4.704224330497994, - "y": 7.027695810865106, - "heading": 0.5143701009934816, - "angularVelocity": 1.3234040971535758e-7, - "velocityX": -4.209244559399155, - "velocityY": -0.3793171900480436, - "timestamp": 6.889373631781356 - }, - { - "x": 4.343531193449039, - "y": 6.995191853158838, - "heading": 0.5143701123338247, - "angularVelocity": 1.3234040961236046e-7, - "velocityX": -4.209244559419223, - "velocityY": -0.3793171898253618, - "timestamp": 6.97506433754988 - }, - { - "x": 3.9828380564000834, - "y": 6.96268789545257, - "heading": 0.5143701236741678, - "angularVelocity": 1.3234040939269306e-7, - "velocityX": -4.209244559419224, - "velocityY": -0.3793171898253537, - "timestamp": 7.060755043318404 - }, - { - "x": 3.622144919351128, - "y": 6.9301839377463015, - "heading": 0.5143701350145109, - "angularVelocity": 1.3234041017948015e-7, - "velocityX": -4.209244559419224, - "velocityY": -0.3793171898253545, - "timestamp": 7.146445749086928 - }, - { - "x": 3.261451782302309, - "y": 6.897679980038524, - "heading": 0.5143701463548541, - "angularVelocity": 1.323404096872133e-7, - "velocityX": -4.209244559417636, - "velocityY": -0.3793171898429695, - "timestamp": 7.2321364548554525 - }, - { - "x": 2.90075864904001, - "y": 6.8651759803147625, - "heading": 0.5143701576957388, - "angularVelocity": 1.323467305471061e-7, - "velocityX": -4.209244515229405, - "velocityY": -0.3793176801643355, - "timestamp": 7.317827160623977 - }, - { - "x": 2.5585689575787693, - "y": 6.834269234981658, - "heading": 0.5701176468934943, - "angularVelocity": 0.6505663443634795, - "velocityX": -3.9933116245488067, - "velocityY": -0.3606779178198484, - "timestamp": 7.403517866392501 - }, - { - "x": 2.2448896661211246, - "y": 6.805966809821415, - "heading": 0.6318849384659164, - "angularVelocity": 0.7208166978957257, - "velocityX": -3.6605987620755966, - "velocityY": -0.3302858216233596, - "timestamp": 7.489208572161025 - }, - { - "x": 1.9597589348765612, - "y": 6.780255296283315, - "heading": 0.6923555719808244, - "angularVelocity": 0.7056848577984265, - "velocityX": -3.327440574649768, - "velocityY": -0.30005020156507956, - "timestamp": 7.574899277929549 - }, - { - "x": 1.7031654665176974, - "y": 6.7571261299629715, - "heading": 0.7492015690369473, - "angularVelocity": 0.6633857960007982, - "velocityX": -2.9944142256453965, - "velocityY": -0.2699145270529415, - "timestamp": 7.660589983698073 - }, - { - "x": 1.4750995700489262, - "y": 6.736574281325872, - "heading": 0.8012653996481951, - "angularVelocity": 0.6075785015925457, - "velocityX": -2.661500969368176, - "velocityY": -0.23983754659012796, - "timestamp": 7.746280689466597 - }, - { - "x": 1.2755541466650087, - "y": 6.718596481742583, - "heading": 0.8478475160673707, - "angularVelocity": 0.5436075709891741, - "velocityX": -2.3286705552752447, - "velocityY": -0.20979871063090086, - "timestamp": 7.831971395235121 - }, - { - "x": 1.1045239164164753, - "y": 6.70319044879611, - "heading": 0.8884763550283885, - "angularVelocity": 0.4741335550528443, - "velocityX": -1.995901757543409, - "velocityY": -0.17978651019737726, - "timestamp": 7.917662101003645 - }, - { - "x": 0.9620048380820003, - "y": 6.6903545095834955, - "heading": 0.922811925478865, - "angularVelocity": 0.4006918853396639, - "velocityX": -1.6631801203675514, - "velocityY": -0.14979383233565072, - "timestamp": 8.00335280677217 - }, - { - "x": 0.8479937432156998, - "y": 6.68008739430419, - "heading": 0.9505984608842503, - "angularVelocity": 0.32426545161671033, - "velocityX": -1.3304954585655717, - "velocityY": -0.11981597289022426, - "timestamp": 8.089043512540695 - }, - { - "x": 0.7624881048917089, - "y": 6.672388111768695, - "heading": 0.9716385035370365, - "angularVelocity": 0.2455347107260553, - "velocityX": -0.9978402856777302, - "velocityY": -0.08984968050435474, - "timestamp": 8.174734218309219 - }, - { - "x": 0.7054858870274207, - "y": 6.667255869051189, - "heading": 0.9857775356805354, - "angularVelocity": 0.16500076661397245, - "velocityX": -0.6652088736235632, - "velocityY": -0.05989264146534099, - "timestamp": 8.260424924077743 + "angularVelocity": 0.2712662528992439, + "velocityX": 0.25816136612733137, + "velocityY": -0.12358010930615114, + "timestamp": 5.425795953354896 + }, + { + "x": 7.789461840084544, + "y": 7.44456429977742, + "heading": 0.032498029722470304, + "angularVelocity": 0.38031209369790614, + "velocityX": -0.11213792406398307, + "velocityY": -0.15710111155221265, + "timestamp": 5.511246903406188 + }, + { + "x": 7.748231627408751, + "y": 7.428284690558898, + "heading": 0.07405407961857692, + "angularVelocity": 0.48631466205092383, + "velocityX": -0.48250151286758974, + "velocityY": -0.1905140809873915, + "timestamp": 5.59669785345748 + }, + { + "x": 7.675346865346751, + "y": 7.40916132937864, + "heading": 0.1243473311272727, + "angularVelocity": 0.5885628126840949, + "velocityX": -0.8529426766838836, + "velocityY": -0.22379342966671825, + "timestamp": 5.682148803508771 + }, + { + "x": 7.570799431207237, + "y": 7.387208662011206, + "heading": 0.1829724416238929, + "angularVelocity": 0.6860673925971562, + "velocityX": -1.2234788972702, + "velocityY": -0.2569037249352633, + "timestamp": 5.767599753560063 + }, + { + "x": 7.434579177146227, + "y": 7.362445545124385, + "heading": 0.24939989698187892, + "angularVelocity": 0.7773752698843693, + "velocityX": -1.5941338742188234, + "velocityY": -0.28979334778557075, + "timestamp": 5.853050703611355 + }, + { + "x": 7.266673120974145, + "y": 7.334897700728802, + "heading": 0.32290448608070227, + "angularVelocity": 0.8601962769834678, + "velocityX": -1.9649407767992484, + "velocityY": -0.3223819557190156, + "timestamp": 5.938501653662646 + }, + { + "x": 7.067064218914486, + "y": 7.304602559914413, + "heading": 0.4024227499708516, + "angularVelocity": 0.9305720280744717, + "velocityX": -2.3359471362200686, + "velocityY": -0.3545325218292511, + "timestamp": 6.023952603713938 + }, + { + "x": 6.835729836802405, + "y": 7.27162039096164, + "heading": 0.48622750779609625, + "angularVelocity": 0.9807352378746566, + "velocityX": -2.7072183746718483, + "velocityY": -0.385977791153593, + "timestamp": 6.10940355376523 + }, + { + "x": 6.572643492795426, + "y": 7.236066659207437, + "heading": 0.571010808696293, + "angularVelocity": 0.9921867556667185, + "velocityX": -3.078799520063915, + "velocityY": -0.4160718135124634, + "timestamp": 6.194854503816521 + }, + { + "x": 6.277837622424222, + "y": 7.198257703016904, + "heading": 0.6480492113566031, + "angularVelocity": 0.901551154360156, + "velocityX": -3.450001084764899, + "velocityY": -0.4424638481823218, + "timestamp": 6.280305453867813 + }, + { + "x": 5.957362245190181, + "y": 7.156598764006078, + "heading": 0.6480492838419923, + "angularVelocity": 8.482689682334955e-7, + "velocityX": -3.7504015700432527, + "velocityY": -0.487518734265005, + "timestamp": 6.365756403919105 + }, + { + "x": 5.635584365409571, + "y": 7.126616390006558, + "heading": 0.6480492987117265, + "angularVelocity": 1.7401484932642463e-7, + "velocityX": -3.7656442624390154, + "velocityY": -0.3508723306355692, + "timestamp": 6.451207353970396 + }, + { + "x": 5.313806456205795, + "y": 7.096634331785028, + "heading": 0.6480493135814611, + "angularVelocity": 1.7401485546331995e-7, + "velocityX": -3.7656446067671383, + "velocityY": -0.3508686352057408, + "timestamp": 6.536658304021688 + }, + { + "x": 4.992028547001338, + "y": 7.066652273570816, + "heading": 0.6480493284511959, + "angularVelocity": 1.7401485563507226e-7, + "velocityX": -3.7656446067751177, + "velocityY": -0.3508686351200953, + "timestamp": 6.62210925407298 + }, + { + "x": 4.67025063779688, + "y": 7.036670215356605, + "heading": 0.6480493433209307, + "angularVelocity": 1.7401485639450837e-7, + "velocityX": -3.765644606775118, + "velocityY": -0.35086863512009386, + "timestamp": 6.707560204124271 + }, + { + "x": 4.348472728592423, + "y": 7.006688157142394, + "heading": 0.6480493581906654, + "angularVelocity": 1.740148559122135e-7, + "velocityX": -3.7656446067751186, + "velocityY": -0.35086863512009364, + "timestamp": 6.793011154175563 + }, + { + "x": 4.0266948193879655, + "y": 6.976706098928182, + "heading": 0.6480493730604002, + "angularVelocity": 1.740148565017462e-7, + "velocityX": -3.765644606775118, + "velocityY": -0.35086863512009375, + "timestamp": 6.878462104226855 + }, + { + "x": 3.7049169101835084, + "y": 6.946724040713971, + "heading": 0.648049387930135, + "angularVelocity": 1.7401485552645738e-7, + "velocityX": -3.765644606775118, + "velocityY": -0.3508686351200936, + "timestamp": 6.963913054278146 + }, + { + "x": 3.3831390009790514, + "y": 6.91674198249976, + "heading": 0.6480494027998698, + "angularVelocity": 1.7401485630496073e-7, + "velocityX": -3.7656446067751177, + "velocityY": -0.35086863512009364, + "timestamp": 7.049364004329438 + }, + { + "x": 3.0613610917745944, + "y": 6.886759924285549, + "heading": 0.6480494176696047, + "angularVelocity": 1.7401485676252163e-7, + "velocityX": -3.7656446067751177, + "velocityY": -0.3508686351200934, + "timestamp": 7.1348149543807295 + }, + { + "x": 2.7395831825701378, + "y": 6.85677786607133, + "heading": 0.6480494325393393, + "angularVelocity": 1.740148561275008e-7, + "velocityX": -3.7656446067751106, + "velocityY": -0.35086863512017596, + "timestamp": 7.220265904432021 + }, + { + "x": 2.4178052733940274, + "y": 6.826795807553092, + "heading": 0.648049447409112, + "angularVelocity": 1.740152990535353e-7, + "velocityX": -3.765644606443389, + "velocityY": -0.3508686386780044, + "timestamp": 7.305716854483313 + }, + { + "x": 2.101806987486401, + "y": 6.79733725091672, + "heading": 0.6651218958442715, + "angularVelocity": 0.19979237708727937, + "velocityX": -3.698007871392473, + "velocityY": -0.34474229506742526, + "timestamp": 7.3911678045346045 + }, + { + "x": 1.8167368698048942, + "y": 6.770773756148669, + "heading": 0.7108427694079615, + "angularVelocity": 0.5350540109419666, + "velocityX": -3.3360672702926424, + "velocityY": -0.31086248604736477, + "timestamp": 7.476618754585896 + }, + { + "x": 1.563349701636476, + "y": 6.747170828438536, + "heading": 0.7627235430971158, + "angularVelocity": 0.6071409815583658, + "velocityX": -2.9652937505825308, + "velocityY": -0.2762160946831818, + "timestamp": 7.562069704637188 + }, + { + "x": 1.3416809560568124, + "y": 6.726531033314501, + "heading": 0.8140209677583794, + "angularVelocity": 0.6003142695377961, + "velocityX": -2.5941051029464517, + "velocityY": -0.24153967991752093, + "timestamp": 7.6475206546884795 + }, + { + "x": 1.1517177468979243, + "y": 6.708849739610871, + "heading": 0.8615071361475246, + "angularVelocity": 0.5557125855327181, + "velocityX": -2.2230672572379273, + "velocityY": -0.20691746192426994, + "timestamp": 7.732971604739771 + }, + { + "x": 0.993443621158485, + "y": 6.694122461254965, + "heading": 0.9032823319492421, + "angularVelocity": 0.48887924331700494, + "velocityX": -1.852221954752358, + "velocityY": -0.17234774273507145, + "timestamp": 7.818422554791063 + }, + { + "x": 0.8668441821472115, + "y": 6.682345527785774, + "heading": 0.9380916664803222, + "angularVelocity": 0.40736041565660025, + "velocityX": -1.4815451312746155, + "velocityY": -0.13782097755638892, + "timestamp": 7.9038735048423545 + }, + { + "x": 0.7719075388323822, + "y": 6.673515992217611, + "heading": 0.9650448121199915, + "angularVelocity": 0.3154224221444248, + "velocityX": -1.111007464022926, + "velocityY": -0.10332869983146084, + "timestamp": 7.989324454893646 + }, + { + "x": 0.7086239541540638, + "y": 6.667631472022659, + "heading": 0.9834806533033196, + "angularVelocity": 0.21574764437724195, + "velocityX": -0.7405837458838845, + "velocityY": -0.06886430392430233, + "timestamp": 8.074775404944939 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": 0.08305155696389553, - "velocityX": -0.33259668167527334, - "velocityY": -0.02994316977531714, - "timestamp": 8.346115629846267 + "angularVelocity": 0.11016412225954963, + "velocityX": -0.37025347871120917, + "velocityY": -0.03442272228334841, + "timestamp": 8.160226354996231 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": -6.062522351787066e-32, - "velocityX": -8.072462222466964e-33, - "velocityY": 1.3269824927509524e-31, - "timestamp": 8.431806335614791 - }, - { - "x": 0.6915272700897366, - "y": 6.664883318998584, - "heading": 0.9848413600463822, - "angularVelocity": -0.13135885163926445, - "velocityX": 0.23720554048226594, - "velocityY": 0.0031531208242943447, - "timestamp": 8.493111090224545 - }, - { - "x": 0.7206154156192573, - "y": 6.665271312791946, - "heading": 0.9688837454817516, - "angularVelocity": -0.26029978696125183, - "velocityX": 0.47448433183831634, - "velocityY": 0.006328934775645454, - "timestamp": 8.554415844834299 - }, - { - "x": 0.7642550785259183, - "y": 6.665855624480287, - "heading": 0.9451981943153414, - "angularVelocity": -0.38635749082081516, - "velocityX": 0.7118479338912146, - "velocityY": 0.009531262168170858, - "timestamp": 8.615720599444053 - }, - { - "x": 0.82245219308383, - "y": 6.6666381477571255, - "heading": 0.9139918901741649, - "angularVelocity": -0.5090356260264929, - "velocityX": 0.9493083355177907, - "velocityY": 0.012764479391858926, - "timestamp": 8.677025354053807 - }, - { - "x": 0.8952134669088825, - "y": 6.667621071916812, - "heading": 0.8755051863660224, - "angularVelocity": -0.6277931304535281, - "velocityX": 1.1868781514293212, - "velocityY": 0.01603340827221871, - "timestamp": 8.738330108663561 - }, - { - "x": 0.982546442157363, - "y": 6.668806895228603, - "heading": 0.8300158681177399, - "angularVelocity": -0.7420194165665126, - "velocityX": 1.4245709946057887, - "velocityY": 0.019343088792048747, - "timestamp": 8.799634863273315 - }, - { - "x": 1.0844595931321694, - "y": 6.670198423358961, - "heading": 0.777846020332524, - "angularVelocity": -0.8509918703257573, - "velocityX": 1.6624020701747062, - "velocityY": 0.022698535198719637, - "timestamp": 8.86093961788307 - }, - { - "x": 1.2009624731033306, - "y": 6.67179876210547, - "heading": 0.7193731323687549, - "angularVelocity": -0.9538067371118101, - "velocityX": 1.9003889781923289, - "velocityY": 0.026104643215625493, - "timestamp": 8.922244372492823 - }, - { - "x": 1.3320659135249422, - "y": 6.673611326998587, - "heading": 0.6550478077481424, - "angularVelocity": -1.0492713824578042, - "velocityX": 2.1385525683313547, - "velocityY": 0.029566465189439763, - "timestamp": 8.983549127102577 - }, - { - "x": 1.477782250869489, - "y": 6.675639904928793, - "heading": 0.5854215593608406, - "angularVelocity": -1.1357397779425142, - "velocityX": 2.376917390374211, - "velocityY": 0.03309005872578241, - "timestamp": 9.044853881712331 - }, - { - "x": 1.638125489580401, - "y": 6.677888808467273, - "heading": 0.5111902482189525, - "angularVelocity": -1.2108573244346357, - "velocityX": 2.615510652177078, - "velocityY": 0.03668399870117081, - "timestamp": 9.106158636322085 - }, - { - "x": 1.8131111470662002, - "y": 6.680363155292976, - "heading": 0.4332634603071458, - "angularVelocity": -1.271137751188529, - "velocityX": 2.8543570331486037, - "velocityY": 0.04036141799202673, - "timestamp": 9.16746339093184 - }, - { - "x": 2.002755069495207, - "y": 6.68306928605088, - "heading": 0.3528824516114434, - "angularVelocity": -1.3111708742230885, - "velocityX": 3.0934618960016964, - "velocityY": 0.04414226555721556, - "timestamp": 9.228768145541594 - }, - { - "x": 2.2070689242126256, - "y": 6.686015356595887, - "heading": 0.2718454606802179, - "angularVelocity": -1.3218712226658607, - "velocityX": 3.332757075988891, - "velocityY": 0.048056151007561554, - "timestamp": 9.290072900151348 - }, - { - "x": 2.4260429630037788, - "y": 6.689212530783897, - "heading": 0.19302675484783927, - "angularVelocity": -1.2856866703751342, - "velocityX": 3.571893243600301, - "velocityY": 0.0521521407003806, - "timestamp": 9.351377654761102 - }, - { - "x": 2.659559040385731, - "y": 6.692680446587641, - "heading": 0.12201145373865255, - "angularVelocity": -1.1583979344056987, - "velocityX": 3.809102228178563, - "velocityY": 0.05656846399302349, - "timestamp": 9.412682409370856 - }, - { - "x": 2.9064602664817145, - "y": 6.696506622995245, - "heading": 0.07684542868597768, - "angularVelocity": -0.7367458746093568, - "velocityX": 4.027440084666806, - "velocityY": 0.06241239251280606, - "timestamp": 9.47398716398061 - }, - { - "x": 3.164834883229884, - "y": 6.7006990530219355, - "heading": 0.07426007030156662, - "angularVelocity": -0.042172232820580385, - "velocityX": 4.214593442105751, - "velocityY": 0.06838670268526725, - "timestamp": 9.535291918590364 - }, - { - "x": 3.423902662418304, - "y": 6.704266454413081, - "heading": 0.07426004244315124, - "angularVelocity": -4.5442503703406033e-7, - "velocityX": 4.225900272133267, - "velocityY": 0.05819126777122497, - "timestamp": 9.596596673200118 - }, - { - "x": 3.6829704424934793, - "y": 6.7078337914078086, - "heading": 0.07426001458477352, - "angularVelocity": -4.5442442246011853e-7, - "velocityX": 4.22590028659796, - "velocityY": 0.0581902173401686, - "timestamp": 9.657901427809872 - }, - { - "x": 3.9420382225687436, - "y": 6.71140112839604, - "heading": 0.07425998672639592, - "angularVelocity": -4.5442442083032825e-7, - "velocityX": 4.225900286599419, - "velocityY": 0.05819021723419636, - "timestamp": 9.719206182419626 - }, - { - "x": 4.201106002644009, - "y": 6.714968465384217, - "heading": 0.07425995886801827, - "angularVelocity": -4.5442442166666195e-7, - "velocityX": 4.225900286599431, - "velocityY": 0.05819021723333154, - "timestamp": 9.78051093702938 - }, - { - "x": 4.46017378272642, - "y": 6.718535801853429, - "heading": 0.07425993100964065, - "angularVelocity": -4.5442442082699014e-7, - "velocityX": 4.225900286715997, - "velocityY": 0.05819020876799694, - "timestamp": 9.841815691639134 - }, - { - "x": 4.719241633592179, - "y": 6.7220979941697125, - "heading": 0.07425990315123465, - "angularVelocity": -4.544248838946695e-7, - "velocityX": 4.225901441330322, - "velocityY": 0.058106297610346594, - "timestamp": 9.903120446248888 - }, - { - "x": 4.978174118425727, - "y": 6.712998060028401, - "heading": 0.07425987514278792, - "angularVelocity": -4.568723405675715e-7, - "velocityX": 4.223693357584233, - "velocityY": -0.1484376570665475, - "timestamp": 9.964425200858642 - }, - { - "x": 5.236176330694113, - "y": 6.689255719100931, - "heading": 0.0742598466206584, - "angularVelocity": -4.652515078556177e-7, - "velocityX": 4.208518799410349, - "velocityY": -0.3872838424785543, - "timestamp": 10.025729955468396 - }, - { - "x": 5.4924208313627965, - "y": 6.650946506335976, - "heading": 0.07425981717735317, - "angularVelocity": -4.802776788917255e-7, - "velocityX": 4.179847098318136, - "velocityY": -0.6248979056978337, - "timestamp": 10.08703471007815 + "angularVelocity": -1.2387955760475958e-29, + "velocityX": 1.44513347457817e-31, + "velocityY": -4.5041627226851734e-30, + "timestamp": 8.245677305047524 + }, + { + "x": 0.6937129747352162, + "y": 6.6645584157609425, + "heading": 0.9790660169157249, + "angularVelocity": -0.22114767057901943, + "velocityX": 0.2675140141228054, + "velocityY": -0.002104635808322496, + "timestamp": 8.308206859439915 + }, + { + "x": 0.7271833460248389, + "y": 6.664303045137076, + "heading": 0.9517582917370493, + "angularVelocity": -0.43671709232583583, + "velocityX": 0.5352728260237692, + "velocityY": -0.004083998780231037, + "timestamp": 8.370736413832306 + }, + { + "x": 0.7774150547311363, + "y": 6.663933728618546, + "heading": 0.9114059815034875, + "angularVelocity": -0.645331805506542, + "velocityX": 0.8033274696166545, + "velocityY": -0.005906271396278706, + "timestamp": 8.433265968224697 + }, + { + "x": 0.8444295954022788, + "y": 6.663462332060666, + "heading": 0.8585277843526951, + "angularVelocity": -0.8456512710608117, + "velocityX": 1.0717258634310107, + "velocityY": -0.007538780061052878, + "timestamp": 8.495795522617088 + }, + { + "x": 0.928251455530078, + "y": 6.662902493101364, + "heading": 0.793730196165171, + "angularVelocity": -1.0362713890602748, + "velocityX": 1.3405158719313994, + "velocityY": -0.008953189651546989, + "timestamp": 8.55832507700948 + }, + { + "x": 1.0289086002870482, + "y": 6.66226903623183, + "heading": 0.7177284863656903, + "angularVelocity": -1.2154526053799588, + "velocityX": 1.6097531117096386, + "velocityY": -0.010130519490973162, + "timestamp": 8.62085463140187 + }, + { + "x": 1.1464337376402622, + "y": 6.661577427221888, + "heading": 0.6314012242837341, + "angularVelocity": -1.3805833564753438, + "velocityX": 1.8795134316120001, + "velocityY": -0.011060513970755739, + "timestamp": 8.683384185794262 + }, + { + "x": 1.2808663505867972, + "y": 6.660844036954463, + "heading": 0.5359002148977876, + "angularVelocity": -1.5272939382655537, + "velocityX": 2.149905180883418, + "velocityY": -0.011728698126054097, + "timestamp": 8.745913740186653 + }, + { + "x": 1.4322545638154922, + "y": 6.660088220029616, + "heading": 0.43284520187288844, + "angularVelocity": -1.6481008704811424, + "velocityX": 2.4210665612405937, + "velocityY": -0.012087355046610722, + "timestamp": 8.808443294579044 + }, + { + "x": 1.6006535211132875, + "y": 6.65933694718367, + "heading": 0.3246624035723361, + "angularVelocity": -1.7301066567926002, + "velocityX": 2.69310982517228, + "velocityY": -0.012014684148101123, + "timestamp": 8.870972848971435 + }, + { + "x": 1.7861082105175972, + "y": 6.658632447498398, + "heading": 0.21527496164562404, + "angularVelocity": -1.7493718448763058, + "velocityX": 2.965872557487375, + "velocityY": -0.011266667292259528, + "timestamp": 8.933502403363827 + }, + { + "x": 1.9885470293511631, + "y": 6.658050737392514, + "heading": 0.11225233797545536, + "angularVelocity": -1.6475828857450405, + "velocityX": 3.2374901884507423, + "velocityY": -0.009302962599615121, + "timestamp": 8.996031957756218 + }, + { + "x": 2.206257706753187, + "y": 6.657808122247979, + "heading": 0.04234893056424459, + "angularVelocity": -1.1179258846552083, + "velocityX": 3.4817244344302054, + "velocityY": -0.0038800075722999405, + "timestamp": 9.058561512148609 + }, + { + "x": 2.437370646545927, + "y": 6.657710110655747, + "heading": 0.021561241595864127, + "angularVelocity": -0.33244581974679344, + "velocityX": 3.6960592801035483, + "velocityY": -0.0015674442779168942, + "timestamp": 9.121091066541 + }, + { + "x": 2.6738489696177092, + "y": 6.656073614188434, + "heading": 0.021561206140011186, + "angularVelocity": -5.670255175242252e-7, + "velocityX": 3.7818648376703368, + "velocityY": -0.02617156772049459, + "timestamp": 9.183620620933391 + }, + { + "x": 2.910327291911898, + "y": 6.654437005347333, + "heading": 0.021561170684433324, + "angularVelocity": -5.670211183604034e-7, + "velocityX": 3.7818648252347256, + "velocityY": -0.026173364851293936, + "timestamp": 9.246150175325782 + }, + { + "x": 3.14680561420604, + "y": 6.652800396499498, + "heading": 0.021561135228855402, + "angularVelocity": -5.670211192982607e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958984518, + "timestamp": 9.308679729718174 + }, + { + "x": 3.3832839365001823, + "y": 6.6511637876516625, + "heading": 0.02156109977327751, + "angularVelocity": -5.670211188277871e-7, + "velocityX": 3.7818648252339804, + "velocityY": -0.02617336495899097, + "timestamp": 9.371209284110565 + }, + { + "x": 3.6197622587943243, + "y": 6.649527178803828, + "heading": 0.02156106431769964, + "angularVelocity": -5.670211185570758e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958990947, + "timestamp": 9.433738838502956 + }, + { + "x": 3.8562405810884663, + "y": 6.647890569955993, + "heading": 0.02156102886212173, + "angularVelocity": -5.670211191121069e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958990954, + "timestamp": 9.496268392895347 + }, + { + "x": 4.092718903382608, + "y": 6.646253961108158, + "heading": 0.02156099340654377, + "angularVelocity": -5.670211198937042e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.02617336495899093, + "timestamp": 9.558797947287738 + }, + { + "x": 4.329197225676751, + "y": 6.6446173522603225, + "heading": 0.021560957950965815, + "angularVelocity": -5.67021119881002e-7, + "velocityX": 3.7818648252339804, + "velocityY": -0.026173364958990898, + "timestamp": 9.62132750168013 + }, + { + "x": 4.565675547970892, + "y": 6.642980743412488, + "heading": 0.021560922495387855, + "angularVelocity": -5.670211198956703e-7, + "velocityX": 3.78186482523398, + "velocityY": -0.026173364958990943, + "timestamp": 9.68385705607252 + }, + { + "x": 4.802153870265035, + "y": 6.641344134564649, + "heading": 0.02156088703980993, + "angularVelocity": -5.670211193772164e-7, + "velocityX": 3.7818648252339795, + "velocityY": -0.026173364959042097, + "timestamp": 9.746386610464912 + }, + { + "x": 5.038632192558808, + "y": 6.639707525663405, + "heading": 0.021560851584232044, + "angularVelocity": -5.67021118724796e-7, + "velocityX": 3.781864825228069, + "velocityY": -0.02617336581314012, + "timestamp": 9.808916164857303 + }, + { + "x": 5.275110508682838, + "y": 6.638070025521481, + "heading": 0.021560816128648946, + "angularVelocity": -5.670212021371837e-7, + "velocityX": 3.781864726558842, + "velocityY": -0.02618761892412933, + "timestamp": 9.871445719249694 + }, + { + "x": 5.511317621528859, + "y": 6.626629955813148, + "heading": 0.02156078050955198, + "angularVelocity": -5.696361874718345e-7, + "velocityX": 3.777527525044398, + "velocityY": -0.18295460153997642, + "timestamp": 9.933975273642085 }, { "x": 5.7460856437683105, "y": 6.598193168640137, - "heading": 0.0742592023389195, - "angularVelocity": -0.000010029212865619508, - "velocityX": 4.137767356223229, - "velocityY": -0.8605097276981168, - "timestamp": 10.148339464687904 - }, - { - "x": 5.9699164008558405, - "y": 6.543017986029004, - "heading": 0.05198341258682499, - "angularVelocity": -0.39649724748489784, - "velocityX": 3.984068806329142, - "velocityY": -0.9820889978876548, - "timestamp": 10.2045209133446 - }, - { - "x": 6.183022450140719, - "y": 6.4863677530582065, - "heading": 0.0038718915492881255, - "angularVelocity": -0.8563595668657032, - "velocityX": 3.7931746934310255, - "velocityY": -1.0083441122525065, - "timestamp": 10.260702362001297 - }, - { - "x": 6.384034644805308, - "y": 6.430498554479071, - "heading": -0.05330736038793806, - "angularVelocity": -1.0177603693814712, - "velocityX": 3.5779104930685666, - "velocityY": -0.9944421141670974, - "timestamp": 10.316883810657993 - }, - { - "x": 6.5728983417799105, - "y": 6.37648577029989, - "heading": -0.11411359843449789, - "angularVelocity": -1.082318799184461, - "velocityX": 3.36167367503596, - "velocityY": -0.9613989220753596, - "timestamp": 10.37306525931469 - }, - { - "x": 6.74968082080873, - "y": 6.324889922313049, - "heading": -0.17585578783167868, - "angularVelocity": -1.098978237006013, - "velocityX": 3.1466344007800133, - "velocityY": -0.9183787392547113, - "timestamp": 10.429246707971386 - }, - { - "x": 6.914453957859755, - "y": 6.2760504067352585, - "heading": -0.2368981836253953, - "angularVelocity": -1.0865222818785416, - "velocityX": 2.932874480647331, - "velocityY": -0.8693174837165495, - "timestamp": 10.485428156628082 - }, - { - "x": 7.067279207384589, - "y": 6.230193439539363, - "heading": -0.2961318737080175, - "angularVelocity": -1.0543282791544983, - "velocityX": 2.72020841717146, - "velocityY": -0.8162297037961601, - "timestamp": 10.541609605284778 - }, - { - "x": 7.2082072429421995, - "y": 6.1874800327074295, - "heading": -0.3527517462037597, - "angularVelocity": -1.0078037118929446, - "velocityX": 2.5084443161793004, - "velocityY": -0.7602759959597819, - "timestamp": 10.597791053941474 - }, - { - "x": 7.337279834118698, - "y": 6.148030397062498, - "heading": -0.4061452108111807, - "angularVelocity": -0.9503753620467785, - "velocityX": 2.2974236916746635, - "velocityY": -0.7021825992062444, - "timestamp": 10.65397250259817 - }, - { - "x": 7.454531717952861, - "y": 6.111937602538107, - "heading": -0.45583024333208966, - "angularVelocity": -0.8843672370307636, - "velocityX": 2.087021368043439, - "velocityY": -0.6424326069792218, - "timestamp": 10.710153951254867 - }, - { - "x": 7.559992100674049, - "y": 6.079275791201654, - "heading": -0.5014179550675222, - "angularVelocity": -0.8114370993528713, - "velocityX": 1.8771388998104257, - "velocityY": -0.5813629252608888, - "timestamp": 10.766335399911563 - }, - { - "x": 7.653685805762832, - "y": 6.050105397874059, - "heading": -0.5425884668253612, - "angularVelocity": -0.7328132816477557, - "velocityX": 1.6676982763707042, - "velocityY": -0.5192175357713573, - "timestamp": 10.82251684856826 - }, - { - "x": 7.735634146205772, - "y": 6.02447662157365, - "heading": -0.5790745351653838, - "angularVelocity": -0.649432672392886, - "velocityX": 1.4586370127921349, - "velocityY": -0.4561786303699093, - "timestamp": 10.878698297224956 - }, - { - "x": 7.8058555921649635, - "y": 6.002431822282749, - "heading": -0.6106499848823828, - "angularVelocity": -0.5620262643981344, - "velocityX": 1.249904508306431, - "velocityY": -0.39238573974139485, - "timestamp": 10.934879745881652 - }, - { - "x": 7.864366288021797, - "y": 5.984007229520149, - "heading": -0.6371212833352664, - "angularVelocity": -0.4711750779984228, - "velocityX": 1.0414593652500725, - "velocityY": -0.32794798288639887, - "timestamp": 10.991061194538348 - }, - { - "x": 7.911180457736072, - "y": 5.969234194186617, - "heading": -0.6583212554852172, - "angularVelocity": -0.37734826454005344, - "velocityX": 0.833267401136955, - "velocityY": -0.26295219661928954, - "timestamp": 11.047242643195045 - }, - { - "x": 7.946310726470828, - "y": 5.958140127507551, - "heading": -0.6741042985083451, - "angularVelocity": -0.2809297980116511, - "velocityX": 0.6253001582323476, - "velocityY": -0.19746850507286262, - "timestamp": 11.10342409185174 - }, - { - "x": 7.969768378758335, - "y": 5.950749219301424, - "heading": -0.6843426613939789, - "angularVelocity": -0.1822374312238999, - "velocityX": 0.41753377401939834, - "velocityY": -0.13155424758250212, - "timestamp": 11.159605540508437 + "heading": 0.021560744143059335, + "angularVelocity": -5.815888661825855e-7, + "velocityX": 3.7545129582438856, + "velocityY": -0.45477354587499197, + "timestamp": 9.996504828034476 + }, + { + "x": 5.947788996667301, + "y": 6.560838784941361, + "heading": 0.021560710996949464, + "angularVelocity": -6.111012841975544e-7, + "velocityX": 3.718722301384556, + "velocityY": -0.6886875092486842, + "timestamp": 10.050744789115704 + }, + { + "x": 6.146761517861249, + "y": 6.510943609310236, + "heading": 0.021560678630883793, + "angularVelocity": -5.967199279900907e-7, + "velocityX": 3.6683750730568514, + "velocityY": -0.9198969659363234, + "timestamp": 10.104984750196932 + }, + { + "x": 6.342740574859562, + "y": 6.4503482133201375, + "heading": 0.021560646512747805, + "angularVelocity": -5.921489497600962e-7, + "velocityX": 3.6131857968117713, + "velocityY": -1.1171725565834012, + "timestamp": 10.15922471127816 + }, + { + "x": 6.53435736566076, + "y": 6.392349027665542, + "heading": 0.005697481882948994, + "angularVelocity": -0.2924626845886352, + "velocityX": 3.532760477358042, + "velocityY": -1.0693072874395997, + "timestamp": 10.213464672359388 + }, + { + "x": 6.715860798361231, + "y": 6.336732624061585, + "heading": -0.044321515304948156, + "angularVelocity": -0.9221798133850251, + "velocityX": 3.3463046263742644, + "velocityY": -1.0253769083769828, + "timestamp": 10.267704633440616 + }, + { + "x": 6.885664142970145, + "y": 6.284563070375092, + "heading": -0.11100118706044729, + "angularVelocity": -1.2293458628342813, + "velocityX": 3.1305948828876353, + "velocityY": -0.9618287448319874, + "timestamp": 10.321944594521844 + }, + { + "x": 7.043230365082517, + "y": 6.236112426000239, + "heading": -0.18201634611185183, + "angularVelocity": -1.309277470628262, + "velocityX": 2.9049840555085917, + "velocityY": -0.8932647334000697, + "timestamp": 10.376184555603071 + }, + { + "x": 7.188521106986847, + "y": 6.19141127344733, + "heading": -0.25302239943673055, + "angularVelocity": -1.3091095920689053, + "velocityX": 2.678666042675613, + "velocityY": -0.8241368847217142, + "timestamp": 10.4304245166843 + }, + { + "x": 7.3215520348116, + "y": 6.150465382048361, + "heading": -0.3216933073087357, + "angularVelocity": -1.2660574694949878, + "velocityX": 2.4526368598519452, + "velocityY": -0.7549026692266663, + "timestamp": 10.484664477765527 + }, + { + "x": 7.4423459735084, + "y": 6.113273952325478, + "heading": -0.38654982711023245, + "angularVelocity": -1.1957331551984454, + "velocityX": 2.2270284913351, + "velocityY": -0.685683193378148, + "timestamp": 10.538904438846755 + }, + { + "x": 7.550924553999962, + "y": 6.079834322504852, + "heading": -0.4465624539391904, + "angularVelocity": -1.1064282796789968, + "velocityX": 2.001818923301949, + "velocityY": -0.6165127915661526, + "timestamp": 10.593144399927983 + }, + { + "x": 7.647306628463083, + "y": 6.050143536065283, + "heading": -0.5009752926426013, + "angularVelocity": -1.0031872740823848, + "velocityX": 1.7769569251493995, + "velocityY": -0.5473968979274295, + "timestamp": 10.64738436100921 + }, + { + "x": 7.7315082292215935, + "y": 6.024198841891014, + "heading": -0.5492132046279071, + "angularVelocity": -0.8893426732564748, + "velocityX": 1.552390508400531, + "velocityY": -0.47833172548586056, + "timestamp": 10.701624322090439 + }, + { + "x": 7.803542912083633, + "y": 6.001997808520636, + "heading": -0.5908266102991006, + "angularVelocity": -0.7672093571172569, + "velocityX": 1.3280740145473924, + "velocityY": -0.4093113809047783, + "timestamp": 10.755864283171666 + }, + { + "x": 7.863422173267542, + "y": 5.983538316289689, + "heading": -0.6254559233807054, + "angularVelocity": -0.6384464957440807, + "velocityX": 1.103969471774435, + "velocityY": -0.34033011571123517, + "timestamp": 10.810104244252894 + }, + { + "x": 7.9111558450934325, + "y": 5.968818528744131, + "heading": -0.6528074420175133, + "angularVelocity": -0.5042687732730391, + "velocityX": 0.8800462034699242, + "velocityY": -0.2713827084704847, + "timestamp": 10.864344205334122 + }, + { + "x": 7.946752441580289, + "y": 5.95783687514921, + "heading": -0.6726364642205706, + "angularVelocity": -0.36557958021692255, + "velocityX": 0.6562799046545997, + "velocityY": -0.2024642602245795, + "timestamp": 10.91858416641535 + }, + { + "x": 7.970219449253945, + "y": 5.950592046499088, + "heading": -0.6847351436740425, + "angularVelocity": -0.22305840956179235, + "velocityX": 0.43265163185705313, + "velocityY": -0.1335699455844713, + "timestamp": 10.972824127496578 }, { "x": 7.981563568115234, "y": 5.947082996368408, "heading": -0.6889234822214203, - "angularVelocity": -0.08153618208446338, - "velocityX": 0.2099481170194555, - "velocityY": -0.06525682446209594, - "timestamp": 11.215786989165133 - }, - { - "x": 7.98107279837352, - "y": 5.947370479695571, - "heading": -0.6873686383472613, - "angularVelocity": 0.02630231709530022, - "velocityX": -0.008302043428202248, - "velocityY": 0.004863174853957543, - "timestamp": 11.274901319987226 - }, - { - "x": 7.967670719276277, - "y": 5.9517826740935895, - "heading": -0.6795422167344298, - "angularVelocity": 0.1323946580125472, - "velocityX": -0.22671455315256586, - "velocityY": 0.07463832097325668, - "timestamp": 11.334015650809318 - }, - { - "x": 7.941346705017116, - "y": 5.960296828520997, - "heading": -0.6655606707736306, - "angularVelocity": 0.23651703007308753, - "velocityX": -0.4453068129686613, - "velocityY": 0.14402860201583023, - "timestamp": 11.39312998163141 - }, - { - "x": 7.902088914435571, - "y": 5.972887353790235, - "heading": -0.6455561559979053, - "angularVelocity": 0.338403809998799, - "velocityX": -0.6640993822579894, - "velocityY": 0.21298600684714283, - "timestamp": 11.452244312453503 - }, - { - "x": 7.849884062968043, - "y": 5.989525223865173, - "heading": -0.6196795941981793, - "angularVelocity": 0.43773754079366983, - "velocityX": -0.8831166781645818, - "velocityY": 0.281452396458829, - "timestamp": 11.511358643275596 - }, - { - "x": 7.78471714102411, - "y": 6.010177207449998, - "heading": -0.5881045676669778, - "angularVelocity": 0.5341348889870459, - "velocityX": -1.1023878818835982, - "velocityY": 0.34935663311453924, - "timestamp": 11.570472974097688 - }, - { - "x": 7.706571062429961, - "y": 6.034804864474398, - "heading": -0.5510323547922651, - "angularVelocity": 0.6271273371305467, - "velocityX": -1.3219481216717965, - "velocityY": 0.4166106032481136, - "timestamp": 11.62958730491978 - }, - { - "x": 7.615426220504745, - "y": 6.063363207617709, - "heading": -0.5086985338851634, - "angularVelocity": 0.7161346549706845, - "velocityX": -1.5418400353633617, - "velocityY": 0.48310355113818987, - "timestamp": 11.688701635741873 - }, - { - "x": 7.511259920040068, - "y": 6.095798872359664, - "heading": -0.46138178697105986, - "angularVelocity": 0.8004276840501866, - "velocityX": -1.7621158696386412, - "velocityY": 0.5486937649615388, - "timestamp": 11.747815966563966 - }, - { - "x": 7.39404563921687, - "y": 6.132047543004427, - "heading": -0.4094159214388897, - "angularVelocity": 0.8790739032226302, - "velocityX": -1.982840356866446, - "velocityY": 0.6131959905603276, - "timestamp": 11.806930297386058 - }, - { - "x": 7.263752054530884, - "y": 6.172030212832479, - "heading": -0.35320687491635055, - "angularVelocity": 0.9508531305497406, - "velocityX": -2.2040947241391, - "velocityY": 0.6763617091155382, - "timestamp": 11.86604462820815 - }, - { - "x": 7.120341734246255, - "y": 6.215647542637923, - "heading": -0.293257939808125, - "angularVelocity": 1.0141184764257163, - "velocityX": -2.425982300573299, - "velocityY": 0.7378469687276493, - "timestamp": 11.925158959030243 - }, - { - "x": 6.963769381897745, - "y": 6.262770961252908, - "heading": -0.23020941920008114, - "angularVelocity": 1.066552217224485, - "velocityX": -2.64863612885552, - "velocityY": 0.7971572706592246, - "timestamp": 11.984273289852336 - }, - { - "x": 6.793979543954, - "y": 6.313227822369206, - "heading": -0.16490527005597488, - "angularVelocity": 1.1047092682253812, - "velocityX": -2.8722280296927845, - "velocityY": 0.8535470234476921, - "timestamp": 12.043387620674428 - }, - { - "x": 6.610904026180611, - "y": 6.366774787590835, - "heading": -0.09851409628601165, - "angularVelocity": 1.1230977809724585, - "velocityX": -3.0969735295552177, - "velocityY": 0.9058203734519391, - "timestamp": 12.10250195149652 - }, - { - "x": 6.414461060118511, - "y": 6.423045155535491, - "heading": -0.03277169507390691, - "angularVelocity": 1.112122903157957, - "velocityX": -3.3231022550742657, - "velocityY": 0.9518904665267254, - "timestamp": 12.161616282318613 - }, - { - "x": 6.20456835252053, - "y": 6.481428804370567, - "heading": 0.029452709079620762, - "angularVelocity": 1.0526111568579828, - "velocityX": -3.5506230837606094, - "velocityY": 0.9876395118264049, - "timestamp": 12.220730613140706 - }, - { - "x": 5.981258970375398, - "y": 6.540731988125068, - "heading": 0.08236638837911917, - "angularVelocity": 0.8951074733256352, - "velocityX": -3.7775845389706695, - "velocityY": 1.0031947064236981, - "timestamp": 12.279844943962798 + "angularVelocity": -0.07721868644237204, + "velocityX": 0.2091468842372586, + "velocityY": -0.06469492346102504, + "timestamp": 11.027064088577806 + }, + { + "x": 7.980191051552582, + "y": 5.9474933088994915, + "heading": -0.684492260818683, + "angularVelocity": 0.07820368392725321, + "velocityX": -0.024222633376945003, + "velocityY": 0.007241333387762343, + "timestamp": 11.083726654587604 + }, + { + "x": 7.965589050123253, + "y": 5.951981018210289, + "heading": -0.6714197034271625, + "angularVelocity": 0.23070888440280377, + "velocityX": -0.257701026579075, + "velocityY": 0.07920060150509887, + "timestamp": 11.140389220597402 + }, + { + "x": 7.9377506970473615, + "y": 5.960547682425779, + "heading": -0.6498948487141165, + "angularVelocity": 0.37987786697348064, + "velocityX": -0.4913006070194107, + "velocityY": 0.15118736793544205, + "timestamp": 11.1970517866072 + }, + { + "x": 7.896668195667252, + "y": 5.973195099193875, + "heading": -0.6201381150730912, + "angularVelocity": 0.5251568316881386, + "velocityX": -0.7250377854932653, + "velocityY": 0.22320585986009217, + "timestamp": 11.253714352617 + }, + { + "x": 7.842332542279827, + "y": 5.989925310225715, + "heading": -0.5824114694602323, + "angularVelocity": 0.6658125155563156, + "velocityX": -0.9589338643440537, + "velocityY": 0.2952603845887764, + "timestamp": 11.310376918626798 + }, + { + "x": 7.774733202893158, + "y": 6.010740641182104, + "heading": -0.5370321516926094, + "angularVelocity": 0.8008694445602113, + "velocityX": -1.1930158506231556, + "velocityY": 0.367355953360645, + "timestamp": 11.367039484636596 + }, + { + "x": 7.693857750948662, + "y": 6.035643782735694, + "heading": -0.48439126574422153, + "angularVelocity": 0.9290240392446375, + "velocityX": -1.4273171449826625, + "velocityY": 0.43949900802735137, + "timestamp": 11.423702050646394 + }, + { + "x": 7.5996914832698845, + "y": 6.064637893376439, + "heading": -0.4249792327859352, + "angularVelocity": 1.0485235163549331, + "velocityX": -1.6618779259395846, + "velocityY": 0.5116978047858136, + "timestamp": 11.480364616656193 + }, + { + "x": 7.492217045643208, + "y": 6.097726656744903, + "heading": -0.35942190954654185, + "angularVelocity": 1.156977663667012, + "velocityX": -1.8967449798883769, + "velocityY": 0.583961611670435, + "timestamp": 11.537027182665991 + }, + { + "x": 7.371414140499637, + "y": 6.1349141479804095, + "heading": -0.28853586070822274, + "angularVelocity": 1.251020803153572, + "velocityX": -2.131970252153457, + "velocityY": 0.6562973379828368, + "timestamp": 11.59368974867579 + }, + { + "x": 7.237259574639344, + "y": 6.176204237589326, + "heading": -0.21342323245219458, + "angularVelocity": 1.3256128965822067, + "velocityX": -2.367604845800615, + "velocityY": 0.7287013722918435, + "timestamp": 11.650352314685588 + }, + { + "x": 7.0897287772168305, + "y": 6.221599002117526, + "heading": -0.13565860715800612, + "angularVelocity": 1.372416231215901, + "velocityX": -2.6036730739833454, + "velocityY": 0.8011420541800114, + "timestamp": 11.707014880695386 + }, + { + "x": 6.928804270313556, + "y": 6.271094657477637, + "heading": -0.05771767479301098, + "angularVelocity": 1.3755277576295868, + "velocityX": -2.8400497583439677, + "velocityY": 0.8735159532230397, + "timestamp": 11.763677446705184 + }, + { + "x": 6.754523159333334, + "y": 6.324666148768191, + "heading": 0.015819291394400217, + "angularVelocity": 1.297805083071882, + "velocityX": -3.0757715940730104, + "velocityY": 0.9454476749480591, + "timestamp": 11.820340012714983 + }, + { + "x": 6.56735113931042, + "y": 6.382062750762148, + "heading": 0.07327050352161106, + "angularVelocity": 1.0139182916156146, + "velocityX": -3.3032746873932677, + "velocityY": 1.012954513638379, + "timestamp": 11.877002578724781 + }, + { + "x": 6.368937324018353, + "y": 6.441765360809047, + "heading": 0.09523150855737006, + "angularVelocity": 0.38757519438780325, + "velocityX": -3.5016736668395723, + "velocityY": 1.053651718430401, + "timestamp": 11.93366514473458 + }, + { + "x": 6.164194285631387, + "y": 6.50503273811371, + "heading": 0.09523154205905172, + "angularVelocity": 5.912489325251288e-7, + "velocityX": -3.6133739222393086, + "velocityY": 1.1165639285330458, + "timestamp": 11.990327710744378 + }, + { + "x": 5.956663061240617, + "y": 6.558448687512427, + "heading": 0.09523157576982937, + "angularVelocity": 5.949391283503271e-7, + "velocityX": -3.6625807654896136, + "velocityY": 0.9427026193886323, + "timestamp": 12.046990276754176 }, { "x": 5.7460856437683105, "y": 6.598193168640137, - "heading": 0.10673916956816744, - "angularVelocity": 0.41229902884969827, - "velocityX": -3.9782794347255015, - "velocityY": 0.9720346947341876, - "timestamp": 12.33895927478489 - }, - { - "x": 5.482731803813418, - "y": 6.650320477040453, - "heading": 0.10673940582411118, - "angularVelocity": 0.0000037192747979075137, - "velocityX": -4.145865218900198, - "velocityY": 0.8206175952808282, - "timestamp": 12.402481320497886 - }, - { - "x": 5.216773824306616, - "y": 6.6869112798369965, - "heading": 0.10673943110586125, - "angularVelocity": 3.9799962033997064e-7, - "velocityX": -4.186861057788582, - "velocityY": 0.5760331296927703, - "timestamp": 12.466003366210881 - }, - { - "x": 4.9491275418069325, - "y": 6.707839765678222, - "heading": 0.10673945565224871, - "angularVelocity": 3.8642312574461983e-7, - "velocityX": -4.213439279159305, - "velocityY": 0.32946807059370153, - "timestamp": 12.529525411923876 - }, - { - "x": 4.680714529727759, - "y": 6.71303487609764, - "heading": 0.1067394798338107, - "angularVelocity": 3.8067983666652777e-7, - "velocityX": -4.225509570203694, - "velocityY": 0.08178436889281773, - "timestamp": 12.593047457636871 - }, - { - "x": 4.4122692055154475, - "y": 6.709929692152406, - "heading": 0.10673950394272769, - "angularVelocity": 3.795362179203617e-7, - "velocityX": -4.226018246093679, - "velocityY": -0.04888356334214293, - "timestamp": 12.656569503349866 - }, - { - "x": 4.143823892978554, - "y": 6.706823499022996, - "heading": 0.10673952805164147, - "angularVelocity": 3.795361675287501e-7, - "velocityX": -4.226018062292654, - "velocityY": -0.048899450490688265, - "timestamp": 12.720091549062861 - }, - { - "x": 3.875378580442675, - "y": 6.703717305805851, - "heading": 0.10673955216055508, - "angularVelocity": 3.7953616500379395e-7, - "velocityX": -4.226018062276673, - "velocityY": -0.04889945187186359, - "timestamp": 12.783613594775856 - }, - { - "x": 3.606933267906796, - "y": 6.700611112588698, - "heading": 0.10673957626946878, - "angularVelocity": 3.7953616612214584e-7, - "velocityX": -4.2260180622766725, - "velocityY": -0.04889945187199676, - "timestamp": 12.847135640488851 - }, - { - "x": 3.3384879553710274, - "y": 6.697504919362028, - "heading": 0.10673960037838261, - "angularVelocity": 3.79536168074433e-7, - "velocityX": -4.226018062274939, - "velocityY": -0.04889945202181832, - "timestamp": 12.910657686201846 - }, - { - "x": 3.0700426441020716, - "y": 6.694398616665168, - "heading": 0.10673962448757092, - "angularVelocity": 3.795404893645561e-7, - "velocityX": -4.22601804233206, - "velocityY": -0.0489011753634946, - "timestamp": 12.974179731914841 - }, - { - "x": 2.806808379331506, - "y": 6.690344844376677, - "heading": 0.1255043030972768, - "angularVelocity": 0.2954041923411685, - "velocityX": -4.143982798663499, - "velocityY": -0.06381677798612607, - "timestamp": 13.037701777627836 - }, - { - "x": 2.556272153813946, - "y": 6.687083030964068, - "heading": 0.1867455578699796, - "angularVelocity": 0.964094497985838, - "velocityX": -3.9440830770711344, - "velocityY": -0.05134931307701316, - "timestamp": 13.101223823340831 - }, - { - "x": 2.3210540999050693, - "y": 6.6841316507490856, - "heading": 0.2629720657012657, - "angularVelocity": 1.2000008339733441, - "velocityX": -3.7029357488208183, - "velocityY": -0.046462297960575334, - "timestamp": 13.164745869053826 - }, - { - "x": 2.1015488183646034, - "y": 6.68142600254672, - "heading": 0.34363574720189305, - "angularVelocity": 1.2698533335195523, - "velocityX": -3.4555763920487643, - "velocityY": -0.04259384552239664, - "timestamp": 13.228267914766821 - }, - { - "x": 1.8977951102037987, - "y": 6.678949586554031, - "heading": 0.4245682813102029, - "angularVelocity": 1.2740857634525757, - "velocityX": -3.207606207794462, - "velocityY": -0.038985142321737026, - "timestamp": 13.291789960479816 - }, - { - "x": 1.7097851332749783, - "y": 6.676691054407077, - "heading": 0.5034534441826332, - "angularVelocity": 1.2418548865515076, - "velocityX": -2.9597594790679125, - "velocityY": -0.03555509148993883, - "timestamp": 13.355312006192811 - }, - { - "x": 1.5375030148010667, - "y": 6.674642046081059, - "heading": 0.5788036623200312, - "angularVelocity": 1.1862057855920731, - "velocityX": -2.7121626285827998, - "velocityY": -0.03225664890069088, - "timestamp": 13.418834051905806 - }, - { - "x": 1.3809327880417555, - "y": 6.67279615304415, - "heading": 0.6495791212494125, - "angularVelocity": 1.1141873366163184, - "velocityX": -2.46481713556153, - "velocityY": -0.02905909304700025, - "timestamp": 13.482356097618801 - }, - { - "x": 1.2400601320509559, - "y": 6.671148237175822, - "heading": 0.7150087917511168, - "angularVelocity": 1.0300309092268365, - "velocityX": -2.2176970909798954, - "velocityY": -0.025942424395041352, - "timestamp": 13.545878143331796 - }, - { - "x": 1.1148725609299421, - "y": 6.669694026119316, - "heading": 0.7744947973618744, - "angularVelocity": 0.9364623721271129, - "velocityX": -1.9707736064835946, - "velocityY": -0.022893013601536772, - "timestamp": 13.609400189044791 - }, - { - "x": 1.0053592312326722, - "y": 6.668429889682977, - "heading": 0.8275574496076633, - "angularVelocity": 0.8353423075436972, - "velocityX": -1.7240208256527763, - "velocityY": -0.019900751339958393, - "timestamp": 13.672922234757786 - }, - { - "x": 0.9115106985701406, - "y": 6.667352716414464, - "heading": 0.873802307501142, - "angularVelocity": 0.7280127296658919, - "velocityX": -1.4774167237396383, - "velocityY": -0.016957471322305857, - "timestamp": 13.736444280470781 - }, - { - "x": 0.8333187122321524, - "y": 6.66645983627761, - "heading": 0.9128998853211554, - "angularVelocity": 0.6154962010616661, - "velocityX": -1.2309425091766681, - "velocityY": -0.014056224525380053, - "timestamp": 13.799966326183776 - }, - { - "x": 0.7707760609431589, - "y": 6.665748959829805, - "heading": 0.9445728281790685, - "angularVelocity": 0.49861339480497757, - "velocityX": -0.984581818595295, - "velocityY": -0.011191019429984484, - "timestamp": 13.863488371896771 - }, - { - "x": 0.7238764608943239, - "y": 6.665218122050303, - "heading": 0.9685875457966591, - "angularVelocity": 0.37805327816572476, - "velocityX": -0.7383200512895466, - "velocityY": -0.008356748803399568, - "timestamp": 13.927010417609766 - }, - { - "x": 0.692614473543579, - "y": 6.664865629845864, - "heading": 0.9847485268863282, - "angularVelocity": 0.2544153121687507, - "velocityX": -0.4921439005915036, - "velocityY": -0.005549131808990834, - "timestamp": 13.990532463322761 + "heading": 0.09523161032614703, + "angularVelocity": 6.098615032122225e-7, + "velocityX": -3.716341004321884, + "velocityY": 0.7014239545882616, + "timestamp": 12.103652842763974 + }, + { + "x": 5.5027055023001905, + "y": 6.62552340619542, + "heading": 0.09523164151085345, + "angularVelocity": 4.815615607402516e-7, + "velocityX": -3.7583333072466103, + "velocityY": 0.4220399473818025, + "timestamp": 12.16841030879256 + }, + { + "x": 5.257964269234122, + "y": 6.634609890080181, + "heading": 0.09523167208339899, + "angularVelocity": 4.721084288711453e-7, + "velocityX": -3.7793516033817274, + "velocityY": 0.140315618291052, + "timestamp": 12.233167774821144 + }, + { + "x": 5.013061040594942, + "y": 6.636410996869712, + "heading": 0.09523170256730593, + "angularVelocity": 4.70739650837411e-7, + "velocityX": -3.781853177069622, + "velocityY": 0.02781311406988273, + "timestamp": 12.297925240849729 + }, + { + "x": 4.7681578086493825, + "y": 6.6382116540225224, + "heading": 0.09523173305121174, + "angularVelocity": 4.7073963340734453e-7, + "velocityX": -3.781853228127505, + "velocityY": 0.027806170674068813, + "timestamp": 12.362682706878314 + }, + { + "x": 4.5232545767036525, + "y": 6.640012311152163, + "heading": 0.09523176353511749, + "angularVelocity": 4.707396321471799e-7, + "velocityX": -3.781853228130136, + "velocityY": 0.02780617031625758, + "timestamp": 12.427440172906898 + }, + { + "x": 4.2783513447579224, + "y": 6.641812968281802, + "heading": 0.09523179401902328, + "angularVelocity": 4.707396330675372e-7, + "velocityX": -3.781853228130136, + "velocityY": 0.027806170316239148, + "timestamp": 12.492197638935483 + }, + { + "x": 4.033448112812192, + "y": 6.64361362541144, + "heading": 0.09523182450292901, + "angularVelocity": 4.707396322379774e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.027806170316239207, + "timestamp": 12.556955104964068 + }, + { + "x": 3.788544880866463, + "y": 6.645414282541079, + "heading": 0.09523185498683476, + "angularVelocity": 4.7073963225441474e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.02780617031623923, + "timestamp": 12.621712570992653 + }, + { + "x": 3.5436416489207327, + "y": 6.647214939670717, + "heading": 0.09523188547074045, + "angularVelocity": 4.707396314544476e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.02780617031623925, + "timestamp": 12.686470037021238 + }, + { + "x": 3.2987384169750027, + "y": 6.649015596800356, + "heading": 0.09523191595464614, + "angularVelocity": 4.7073963141347726e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.027806170316239207, + "timestamp": 12.751227503049822 + }, + { + "x": 3.0538351850292726, + "y": 6.650816253929995, + "heading": 0.09523194643855185, + "angularVelocity": 4.7073963175583744e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.027806170316239227, + "timestamp": 12.815984969078407 + }, + { + "x": 2.8089319530835426, + "y": 6.652616911059632, + "heading": 0.09523197692245748, + "angularVelocity": 4.7073963058043316e-7, + "velocityX": -3.7818532281301365, + "velocityY": 0.02780617031621524, + "timestamp": 12.880742435106992 + }, + { + "x": 2.5640287211375905, + "y": 6.654417568159032, + "heading": 0.09523200740636317, + "angularVelocity": 4.707396315238404e-7, + "velocityX": -3.78185322813357, + "velocityY": 0.027806169849281307, + "timestamp": 12.945499901135577 + }, + { + "x": 2.319125484878767, + "y": 6.656217638494738, + "heading": 0.09523203789215427, + "angularVelocity": 4.707687462497284e-7, + "velocityX": -3.7818532947339327, + "velocityY": 0.02779710890650077, + "timestamp": 13.010257367164161 + }, + { + "x": 2.086765993499063, + "y": 6.65613072972364, + "heading": 0.1392662443558495, + "angularVelocity": 0.6799865585268291, + "velocityX": -3.588149840161109, + "velocityY": -0.0013420656555584189, + "timestamp": 13.075014833192746 + }, + { + "x": 1.8694746583337432, + "y": 6.65699037980207, + "heading": 0.2298866992043259, + "angularVelocity": 1.399382347797175, + "velocityX": -3.3554638328406146, + "velocityY": 0.013274918417153406, + "timestamp": 13.139772299221331 + }, + { + "x": 1.6701637818051993, + "y": 6.657989601285878, + "heading": 0.3342097566920158, + "angularVelocity": 1.6109811560823084, + "velocityX": -3.077805367501045, + "velocityY": 0.015430212840099346, + "timestamp": 13.204529765249916 + }, + { + "x": 1.4891465820964496, + "y": 6.659013009703433, + "heading": 0.4396201310941332, + "angularVelocity": 1.6277717592530314, + "velocityX": -2.7953101134137524, + "velocityY": 0.01580371315183694, + "timestamp": 13.2692872312785 + }, + { + "x": 1.326396709734208, + "y": 6.660011981353038, + "heading": 0.5409065147827995, + "angularVelocity": 1.5640881260541826, + "velocityX": -2.5132217540816426, + "velocityY": 0.015426354841678473, + "timestamp": 13.334044697307085 + }, + { + "x": 1.1818610470561817, + "y": 6.660956545539143, + "heading": 0.6351810352227076, + "angularVelocity": 1.4558092868904808, + "velocityX": -2.231953650166405, + "velocityY": 0.014586182011631477, + "timestamp": 13.39880216333567 + }, + { + "x": 1.0554921038308631, + "y": 6.661825694692396, + "heading": 0.7206099929025545, + "angularVelocity": 1.3192140291922123, + "velocityX": -1.9514189015601402, + "velocityY": 0.013421605361615121, + "timestamp": 13.463559629364255 + }, + { + "x": 0.9472506642705061, + "y": 6.66260279420219, + "heading": 0.7959103128896443, + "angularVelocity": 1.1628052270274385, + "velocityX": -1.6714897323588642, + "velocityY": 0.012000153147596032, + "timestamp": 13.52831709539284 + }, + { + "x": 0.8571042421570496, + "y": 6.663273874000479, + "heading": 0.8601164354198797, + "angularVelocity": 0.9914860242044418, + "velocityX": -1.3920622229669246, + "velocityY": 0.010362971861702049, + "timestamp": 13.593074561421425 + }, + { + "x": 0.7850255723133412, + "y": 6.6638271232273425, + "heading": 0.912467741181329, + "angularVelocity": 0.8084211593199297, + "velocityX": -1.1130557488443436, + "velocityY": 0.00854340450286573, + "timestamp": 13.65783202745001 + }, + { + "x": 0.7309916972965728, + "y": 6.664252697034555, + "heading": 0.952354006476902, + "angularVelocity": 0.6159330767817102, + "velocityX": -0.8344037889456237, + "velocityY": 0.0065718106855080505, + "timestamp": 13.722589493478594 + }, + { + "x": 0.6949834963208631, + "y": 6.664542513080523, + "heading": 0.9792885970471746, + "angularVelocity": 0.41593027371366526, + "velocityX": -0.556047096713378, + "velocityY": 0.0044754074509330615, + "timestamp": 13.787346959507179 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": 0.12823509120632537, - "velocityX": -0.24604105125639145, - "velocityY": -0.002764585801632022, - "timestamp": 14.054054509035756 + "angularVelocity": 0.21010218588179536, + "velocityX": -0.2779301721676604, + "velocityY": 0.0022778009813890303, + "timestamp": 13.852104425535764 }, { "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": -8.295390282600847e-37, - "velocityX": 2.201674205631738e-38, - "velocityY": -2.6267125304593916e-36, - "timestamp": 14.117576554748752 + "angularVelocity": -1.2401951851203077e-33, + "velocityX": -4.2513783914380976e-39, + "velocityY": -1.4668246371137104e-32, + "timestamp": 13.916861891564348 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLaneSprint.1.traj b/src/main/deploy/choreo/AmpLaneSprint.1.traj index d5da70dc..d2e23d21 100644 --- a/src/main/deploy/choreo/AmpLaneSprint.1.traj +++ b/src/main/deploy/choreo/AmpLaneSprint.1.traj @@ -5,243 +5,234 @@ "y": 7.39399003982544, "heading": 0, "angularVelocity": 0, - "velocityX": 0, + "velocityX": 2.82118644197349e-37, "velocityY": 0, "timestamp": 0 }, { - "x": 1.530136363451793, + "x": 1.5360008147282442, "y": 7.39399003982544, - "heading": 1.63546146728146e-19, - "angularVelocity": 1.69832040317376e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.2961741231159756e-22, - "timestamp": 0.096298758216477 + "heading": 3.2333963174943666e-19, + "angularVelocity": 3.292766805519721e-18, + "velocityX": 0.4284174372625368, + "velocityY": 1.8392808793575992e-26, + "timestamp": 0.09819694336189172 }, { - "x": 1.602546026150824, + "x": 1.6201393797142145, "y": 7.39399003982544, - "heading": 5.000550672728244e-19, - "angularVelocity": 3.494426359967847e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.641598990275319e-22, - "timestamp": 0.192597516432954 + "heading": 7.027380416999147e-19, + "angularVelocity": 3.863647922469736e-18, + "velocityX": 0.8568348678215871, + "velocityY": 3.963342552313322e-26, + "timestamp": 0.19639388672378344 }, { - "x": 1.7111605195237438, + "x": 1.746347225986537, "y": 7.39399003982544, - "heading": 1.021045803758983e-18, - "angularVelocity": 5.410150084422451e-18, - "velocityX": 1.1278909031075715, - "velocityY": 4.0472192553510183e-22, - "timestamp": 0.288896274649431 + "heading": 8.361393183209098e-19, + "angularVelocity": 1.3585074996311424e-18, + "velocityX": 1.2852522894444904, + "velocityY": 6.468173032965746e-26, + "timestamp": 0.29459083008567516 }, { - "x": 1.8559798429712064, + "x": 1.9146243523169562, "y": 7.39399003982544, - "heading": 1.7409021964949573e-18, - "angularVelocity": 7.475240644111669e-18, - "velocityX": 1.5038545265756458, - "velocityY": 5.528083834903345e-22, - "timestamp": 0.385195032865908 + "heading": 3.80235401897157e-19, + "angularVelocity": -4.642750516315599e-18, + "velocityX": 1.7136696985593167, + "velocityY": 9.506575448004094e-26, + "timestamp": 0.3927877734475669 }, { - "x": 2.0370039957226234, + "x": 2.12497075686256, "y": 7.39399003982544, - "heading": 2.678153616617339e-18, - "angularVelocity": 9.732746683078845e-18, - "velocityX": 1.8798181420416615, - "velocityY": 7.105691212840138e-22, - "timestamp": 0.481493791082385 + "heading": -1.0623970199278788e-18, + "angularVelocity": -1.4691215100259113e-17, + "velocityX": 2.1420870889066284, + "velocityY": 1.3343481989110454e-25, + "timestamp": 0.4909847168094586 }, { - "x": 2.254232976750543, + "x": 2.3773864365383615, "y": 7.39399003982544, - "heading": 3.8575512078730386e-18, - "angularVelocity": 1.2247277267116202e-17, - "velocityX": 2.25578174683826, - "velocityY": 8.812289177822076e-22, - "timestamp": 0.577792549298862 + "heading": -3.9564889545725385e-18, + "angularVelocity": -2.947232200024047e-17, + "velocityX": 2.5705044478376236, + "velocityY": 1.8500903675379378e-25, + "timestamp": 0.5891816601713503 }, { - "x": 2.5076667846165317, + "x": 2.6718713848686027, "y": 7.39399003982544, - "heading": 5.3137068489139784e-18, - "angularVelocity": 1.512122972072063e-17, - "velocityX": 2.6317453366976604, - "velocityY": 1.0699474323795816e-21, - "timestamp": 0.6740913075153391 + "heading": -8.853646546888228e-18, + "angularVelocity": -4.987077429157769e-17, + "velocityX": 2.9989217408219764, + "velocityY": 2.625793984498987e-25, + "timestamp": 0.6873786035332421 }, { - "x": 2.7973054171629355, + "x": 3.0084255847706767, "y": 7.39399003982544, - "heading": 7.098464960583512e-18, - "angularVelocity": 1.8533552797092878e-17, - "velocityX": 3.007708904151223, - "velocityY": 1.2857541020722067e-21, - "timestamp": 0.7703900657318161 + "heading": -1.6413613329692498e-17, + "angularVelocity": -7.698780165331962e-17, + "velocityX": 3.4273388598436147, + "velocityY": 4.161152605292971e-25, + "timestamp": 0.7855755468951338 }, { - "x": 3.1231488707936497, + "x": 3.3798020724552167, "y": 7.39399003982544, - "heading": 9.299048469122608e-18, - "angularVelocity": 2.285162928757611e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.5467079012007584e-21, - "timestamp": 0.8666888239482932 + "heading": -2.58741769005412e-17, + "angularVelocity": -9.634275005875749e-17, + "velocityX": 3.781955679779987, + "velocityY": -5.487818039619291e-25, + "timestamp": 0.8837724902570254 }, { - "x": 3.4851971383163978, + "x": 3.7511785589388484, "y": 7.39399003982544, - "heading": 1.2088326363522958e-17, - "angularVelocity": 2.896483790183752e-17, - "velocityX": 3.7596358896848248, - "velocityY": 1.8979567079276025e-21, - "timestamp": 0.9629875821647702 + "heading": 5.146033898598045e-18, + "angularVelocity": 3.158979265252523e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.295977316525143e-24, + "timestamp": 0.9819694336189171 }, { - "x": 3.883450198153611, + "x": 4.122555045422488, "y": 7.39399003982544, - "heading": 1.5983950666179298e-17, - "angularVelocity": 4.0453526414309363e-17, - "velocityX": 4.1355991210390295, - "velocityY": 2.520086451941594e-21, - "timestamp": 1.0592863403812471 + "heading": 4.0038826563950476e-17, + "angularVelocity": 3.553348146159671e-16, + "velocityX": 3.7819556675504797, + "velocityY": 7.623936784532972e-25, + "timestamp": 1.0801663769808088 }, { - "x": 4.290437753988616, + "x": 4.493931531906129, "y": 7.39399003982544, - "heading": -1.0811293578876384e-19, - "angularVelocity": -1.671056190093009e-16, - "velocityX": 4.226301183657094, - "velocityY": -2.0251172410052617e-20, - "timestamp": 1.155585098597724 + "heading": 6.570668459860429e-17, + "angularVelocity": 2.613916192895976e-16, + "velocityX": 3.7819556675504797, + "velocityY": 3.4278643163770276e-26, + "timestamp": 1.1783633203427004 }, { - "x": 4.697425309823639, + "x": 4.865308018389769, "y": 7.39399003982544, - "heading": -8.423356676535005e-18, - "angularVelocity": -8.634840048564003e-17, - "velocityX": 4.226301183657286, - "velocityY": 2.1724036271938957e-21, - "timestamp": 1.251883856814201 + "heading": 6.663782988380241e-17, + "angularVelocity": 9.482426370100795e-18, + "velocityX": 3.7819556675504797, + "velocityY": 2.693381196266241e-25, + "timestamp": 1.276560263704592 }, { - "x": 5.104412865658644, + "x": 5.236684504873409, "y": 7.39399003982544, - "heading": -8.042445280935507e-18, - "angularVelocity": 3.955517211854611e-18, - "velocityX": 4.226301183657094, - "velocityY": -6.018603432474332e-22, - "timestamp": 1.348182615030678 + "heading": 5.2326006182614025e-17, + "angularVelocity": -1.4574612214194992e-16, + "velocityX": 3.7819556675504797, + "velocityY": 5.124149697811417e-24, + "timestamp": 1.3747572070664837 }, { - "x": 5.502665925495857, + "x": 5.60806099135704, "y": 7.39399003982544, - "heading": -6.3335804348522236e-18, - "angularVelocity": 1.7745450464376603e-17, - "velocityX": 4.13559912103903, - "velocityY": 1.2361742339090949e-21, - "timestamp": 1.4444813732471549 + "heading": 2.884064553605338e-17, + "angularVelocity": -2.3916590315859936e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.1014556301472372e-23, + "timestamp": 1.4729541504283754 }, { - "x": 5.864714193018606, + "x": 5.9794374790415805, "y": 7.39399003982544, - "heading": -5.002513564509544e-18, - "angularVelocity": 1.3822263999404139e-17, - "velocityX": 3.759635889684825, - "velocityY": 1.1237973471200677e-21, - "timestamp": 1.5407801314636318 + "heading": 1.6502190572837753e-17, + "angularVelocity": -1.2565009195595738e-16, + "velocityX": 3.7819556797799856, + "velocityY": 6.524035237986936e-24, + "timestamp": 1.571151093790267 }, { - "x": 6.19055764664932, + "x": 6.315991678943653, "y": 7.39399003982544, - "heading": -3.8974854517760974e-18, - "angularVelocity": 1.1474998421478588e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.0114186808354653e-21, - "timestamp": 1.6370788896801087 + "heading": 9.967918627860597e-18, + "angularVelocity": -6.65425189408073e-17, + "velocityX": 3.427338859843601, + "velocityY": -3.262675265981889e-25, + "timestamp": 1.6693480371521587 }, { - "x": 6.480196279195724, + "x": 6.610476627273894, "y": 7.39399003982544, - "heading": -2.9674142808273095e-18, - "angularVelocity": 9.658184315185454e-18, - "velocityX": 3.0077089041512233, - "velocityY": 8.990394180618527e-22, - "timestamp": 1.7333776478965857 + "heading": 5.2731830670839714e-18, + "angularVelocity": -4.78093860085579e-17, + "velocityX": 2.9989217408219764, + "velocityY": -2.379324733264266e-25, + "timestamp": 1.7675449805140504 }, { - "x": 6.733630087061712, + "x": 6.862892306949696, "y": 7.39399003982544, - "heading": -2.1863127731116462e-18, - "angularVelocity": 8.111231213379657e-18, - "velocityX": 2.631745336697661, - "velocityY": 7.866598563444379e-22, - "timestamp": 1.8296764061130626 + "heading": 2.2289001785398693e-18, + "angularVelocity": -3.10018090849907e-17, + "velocityX": 2.5705044478376236, + "velocityY": -1.7314309405461854e-25, + "timestamp": 1.865741923875942 }, { - "x": 6.950859068089632, + "x": 7.0732387114953, "y": 7.39399003982544, - "heading": -1.5381923338445708e-18, - "angularVelocity": 6.730309280881326e-18, - "velocityX": 2.2557817468382604, - "velocityY": 6.742801150833687e-22, - "timestamp": 1.9259751643295395 + "heading": 5.002812433628901e-19, + "angularVelocity": -1.760359206249271e-17, + "velocityX": 2.1420870889066284, + "velocityY": -1.2661949502166272e-25, + "timestamp": 1.9639388672378337 }, { - "x": 7.13188322084105, + "x": 7.24151583782572, "y": 7.39399003982544, - "heading": -1.0119189398601327e-18, - "angularVelocity": 5.4650069455448464e-18, - "velocityX": 1.8798181420416618, - "velocityY": 5.619002540306745e-22, - "timestamp": 2.0222739225460167 + "heading": -2.663585350994398e-19, + "angularVelocity": -7.807165345874594e-18, + "velocityX": 1.7136696985593165, + "velocityY": -9.096401200333518e-26, + "timestamp": 2.0621358105997256 }, { - "x": 7.276702544288512, + "x": 7.367723684098041, "y": 7.39399003982544, - "heading": -6.000129613332895e-19, - "angularVelocity": 4.277375706520152e-18, - "velocityX": 1.5038545265756458, - "velocityY": 4.495203072508259e-22, - "timestamp": 2.118572680762494 + "heading": -4.029353027704155e-19, + "angularVelocity": -1.390845344762092e-18, + "velocityX": 1.2852522894444902, + "velocityY": -6.226126404979919e-26, + "timestamp": 2.1603327539616175 }, { - "x": 7.385317037661432, + "x": 7.451862249084011, "y": 7.39399003982544, - "heading": -2.9682943520502703e-19, - "angularVelocity": 3.1483637414904354e-18, - "velocityX": 1.1278909031075715, - "velocityY": 3.3714029617848224e-22, - "timestamp": 2.214871438978971 - }, - { - "x": 7.457726700360463, - "y": 7.39399003982544, - "heading": -9.799024750037137e-20, - "angularVelocity": 2.064815645768494e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.2476023506664887e-22, - "timestamp": 2.311170197195448 + "heading": -2.1995708883691042e-19, + "angularVelocity": 1.8633799906775463e-18, + "velocityX": 0.8568348678215871, + "velocityY": -3.833262935804246e-26, + "timestamp": 2.2585296973235094 }, { "x": 7.493931531906128, "y": 7.39399003982544, "heading": 0, - "angularVelocity": 1.017564989623581e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.1238013392767656e-22, - "timestamp": 2.4074689554119253 + "angularVelocity": 2.2399586452459634e-18, + "velocityX": 0.42841743726253684, + "velocityY": -1.7862050890834192e-26, + "timestamp": 2.3567266406854013 }, { "x": 7.493931531906128, "y": 7.39399003982544, "heading": 0, "angularVelocity": 0, - "velocityX": -2.537301157014256e-39, + "velocityX": -2.1810930337522913e-39, "velocityY": 0, - "timestamp": 2.5037677136284024 + "timestamp": 2.454923584047293 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLaneSprint.traj b/src/main/deploy/choreo/AmpLaneSprint.traj index d5da70dc..d2e23d21 100644 --- a/src/main/deploy/choreo/AmpLaneSprint.traj +++ b/src/main/deploy/choreo/AmpLaneSprint.traj @@ -5,243 +5,234 @@ "y": 7.39399003982544, "heading": 0, "angularVelocity": 0, - "velocityX": 0, + "velocityX": 2.82118644197349e-37, "velocityY": 0, "timestamp": 0 }, { - "x": 1.530136363451793, + "x": 1.5360008147282442, "y": 7.39399003982544, - "heading": 1.63546146728146e-19, - "angularVelocity": 1.69832040317376e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.2961741231159756e-22, - "timestamp": 0.096298758216477 + "heading": 3.2333963174943666e-19, + "angularVelocity": 3.292766805519721e-18, + "velocityX": 0.4284174372625368, + "velocityY": 1.8392808793575992e-26, + "timestamp": 0.09819694336189172 }, { - "x": 1.602546026150824, + "x": 1.6201393797142145, "y": 7.39399003982544, - "heading": 5.000550672728244e-19, - "angularVelocity": 3.494426359967847e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.641598990275319e-22, - "timestamp": 0.192597516432954 + "heading": 7.027380416999147e-19, + "angularVelocity": 3.863647922469736e-18, + "velocityX": 0.8568348678215871, + "velocityY": 3.963342552313322e-26, + "timestamp": 0.19639388672378344 }, { - "x": 1.7111605195237438, + "x": 1.746347225986537, "y": 7.39399003982544, - "heading": 1.021045803758983e-18, - "angularVelocity": 5.410150084422451e-18, - "velocityX": 1.1278909031075715, - "velocityY": 4.0472192553510183e-22, - "timestamp": 0.288896274649431 + "heading": 8.361393183209098e-19, + "angularVelocity": 1.3585074996311424e-18, + "velocityX": 1.2852522894444904, + "velocityY": 6.468173032965746e-26, + "timestamp": 0.29459083008567516 }, { - "x": 1.8559798429712064, + "x": 1.9146243523169562, "y": 7.39399003982544, - "heading": 1.7409021964949573e-18, - "angularVelocity": 7.475240644111669e-18, - "velocityX": 1.5038545265756458, - "velocityY": 5.528083834903345e-22, - "timestamp": 0.385195032865908 + "heading": 3.80235401897157e-19, + "angularVelocity": -4.642750516315599e-18, + "velocityX": 1.7136696985593167, + "velocityY": 9.506575448004094e-26, + "timestamp": 0.3927877734475669 }, { - "x": 2.0370039957226234, + "x": 2.12497075686256, "y": 7.39399003982544, - "heading": 2.678153616617339e-18, - "angularVelocity": 9.732746683078845e-18, - "velocityX": 1.8798181420416615, - "velocityY": 7.105691212840138e-22, - "timestamp": 0.481493791082385 + "heading": -1.0623970199278788e-18, + "angularVelocity": -1.4691215100259113e-17, + "velocityX": 2.1420870889066284, + "velocityY": 1.3343481989110454e-25, + "timestamp": 0.4909847168094586 }, { - "x": 2.254232976750543, + "x": 2.3773864365383615, "y": 7.39399003982544, - "heading": 3.8575512078730386e-18, - "angularVelocity": 1.2247277267116202e-17, - "velocityX": 2.25578174683826, - "velocityY": 8.812289177822076e-22, - "timestamp": 0.577792549298862 + "heading": -3.9564889545725385e-18, + "angularVelocity": -2.947232200024047e-17, + "velocityX": 2.5705044478376236, + "velocityY": 1.8500903675379378e-25, + "timestamp": 0.5891816601713503 }, { - "x": 2.5076667846165317, + "x": 2.6718713848686027, "y": 7.39399003982544, - "heading": 5.3137068489139784e-18, - "angularVelocity": 1.512122972072063e-17, - "velocityX": 2.6317453366976604, - "velocityY": 1.0699474323795816e-21, - "timestamp": 0.6740913075153391 + "heading": -8.853646546888228e-18, + "angularVelocity": -4.987077429157769e-17, + "velocityX": 2.9989217408219764, + "velocityY": 2.625793984498987e-25, + "timestamp": 0.6873786035332421 }, { - "x": 2.7973054171629355, + "x": 3.0084255847706767, "y": 7.39399003982544, - "heading": 7.098464960583512e-18, - "angularVelocity": 1.8533552797092878e-17, - "velocityX": 3.007708904151223, - "velocityY": 1.2857541020722067e-21, - "timestamp": 0.7703900657318161 + "heading": -1.6413613329692498e-17, + "angularVelocity": -7.698780165331962e-17, + "velocityX": 3.4273388598436147, + "velocityY": 4.161152605292971e-25, + "timestamp": 0.7855755468951338 }, { - "x": 3.1231488707936497, + "x": 3.3798020724552167, "y": 7.39399003982544, - "heading": 9.299048469122608e-18, - "angularVelocity": 2.285162928757611e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.5467079012007584e-21, - "timestamp": 0.8666888239482932 + "heading": -2.58741769005412e-17, + "angularVelocity": -9.634275005875749e-17, + "velocityX": 3.781955679779987, + "velocityY": -5.487818039619291e-25, + "timestamp": 0.8837724902570254 }, { - "x": 3.4851971383163978, + "x": 3.7511785589388484, "y": 7.39399003982544, - "heading": 1.2088326363522958e-17, - "angularVelocity": 2.896483790183752e-17, - "velocityX": 3.7596358896848248, - "velocityY": 1.8979567079276025e-21, - "timestamp": 0.9629875821647702 + "heading": 5.146033898598045e-18, + "angularVelocity": 3.158979265252523e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.295977316525143e-24, + "timestamp": 0.9819694336189171 }, { - "x": 3.883450198153611, + "x": 4.122555045422488, "y": 7.39399003982544, - "heading": 1.5983950666179298e-17, - "angularVelocity": 4.0453526414309363e-17, - "velocityX": 4.1355991210390295, - "velocityY": 2.520086451941594e-21, - "timestamp": 1.0592863403812471 + "heading": 4.0038826563950476e-17, + "angularVelocity": 3.553348146159671e-16, + "velocityX": 3.7819556675504797, + "velocityY": 7.623936784532972e-25, + "timestamp": 1.0801663769808088 }, { - "x": 4.290437753988616, + "x": 4.493931531906129, "y": 7.39399003982544, - "heading": -1.0811293578876384e-19, - "angularVelocity": -1.671056190093009e-16, - "velocityX": 4.226301183657094, - "velocityY": -2.0251172410052617e-20, - "timestamp": 1.155585098597724 + "heading": 6.570668459860429e-17, + "angularVelocity": 2.613916192895976e-16, + "velocityX": 3.7819556675504797, + "velocityY": 3.4278643163770276e-26, + "timestamp": 1.1783633203427004 }, { - "x": 4.697425309823639, + "x": 4.865308018389769, "y": 7.39399003982544, - "heading": -8.423356676535005e-18, - "angularVelocity": -8.634840048564003e-17, - "velocityX": 4.226301183657286, - "velocityY": 2.1724036271938957e-21, - "timestamp": 1.251883856814201 + "heading": 6.663782988380241e-17, + "angularVelocity": 9.482426370100795e-18, + "velocityX": 3.7819556675504797, + "velocityY": 2.693381196266241e-25, + "timestamp": 1.276560263704592 }, { - "x": 5.104412865658644, + "x": 5.236684504873409, "y": 7.39399003982544, - "heading": -8.042445280935507e-18, - "angularVelocity": 3.955517211854611e-18, - "velocityX": 4.226301183657094, - "velocityY": -6.018603432474332e-22, - "timestamp": 1.348182615030678 + "heading": 5.2326006182614025e-17, + "angularVelocity": -1.4574612214194992e-16, + "velocityX": 3.7819556675504797, + "velocityY": 5.124149697811417e-24, + "timestamp": 1.3747572070664837 }, { - "x": 5.502665925495857, + "x": 5.60806099135704, "y": 7.39399003982544, - "heading": -6.3335804348522236e-18, - "angularVelocity": 1.7745450464376603e-17, - "velocityX": 4.13559912103903, - "velocityY": 1.2361742339090949e-21, - "timestamp": 1.4444813732471549 + "heading": 2.884064553605338e-17, + "angularVelocity": -2.3916590315859936e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.1014556301472372e-23, + "timestamp": 1.4729541504283754 }, { - "x": 5.864714193018606, + "x": 5.9794374790415805, "y": 7.39399003982544, - "heading": -5.002513564509544e-18, - "angularVelocity": 1.3822263999404139e-17, - "velocityX": 3.759635889684825, - "velocityY": 1.1237973471200677e-21, - "timestamp": 1.5407801314636318 + "heading": 1.6502190572837753e-17, + "angularVelocity": -1.2565009195595738e-16, + "velocityX": 3.7819556797799856, + "velocityY": 6.524035237986936e-24, + "timestamp": 1.571151093790267 }, { - "x": 6.19055764664932, + "x": 6.315991678943653, "y": 7.39399003982544, - "heading": -3.8974854517760974e-18, - "angularVelocity": 1.1474998421478588e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.0114186808354653e-21, - "timestamp": 1.6370788896801087 + "heading": 9.967918627860597e-18, + "angularVelocity": -6.65425189408073e-17, + "velocityX": 3.427338859843601, + "velocityY": -3.262675265981889e-25, + "timestamp": 1.6693480371521587 }, { - "x": 6.480196279195724, + "x": 6.610476627273894, "y": 7.39399003982544, - "heading": -2.9674142808273095e-18, - "angularVelocity": 9.658184315185454e-18, - "velocityX": 3.0077089041512233, - "velocityY": 8.990394180618527e-22, - "timestamp": 1.7333776478965857 + "heading": 5.2731830670839714e-18, + "angularVelocity": -4.78093860085579e-17, + "velocityX": 2.9989217408219764, + "velocityY": -2.379324733264266e-25, + "timestamp": 1.7675449805140504 }, { - "x": 6.733630087061712, + "x": 6.862892306949696, "y": 7.39399003982544, - "heading": -2.1863127731116462e-18, - "angularVelocity": 8.111231213379657e-18, - "velocityX": 2.631745336697661, - "velocityY": 7.866598563444379e-22, - "timestamp": 1.8296764061130626 + "heading": 2.2289001785398693e-18, + "angularVelocity": -3.10018090849907e-17, + "velocityX": 2.5705044478376236, + "velocityY": -1.7314309405461854e-25, + "timestamp": 1.865741923875942 }, { - "x": 6.950859068089632, + "x": 7.0732387114953, "y": 7.39399003982544, - "heading": -1.5381923338445708e-18, - "angularVelocity": 6.730309280881326e-18, - "velocityX": 2.2557817468382604, - "velocityY": 6.742801150833687e-22, - "timestamp": 1.9259751643295395 + "heading": 5.002812433628901e-19, + "angularVelocity": -1.760359206249271e-17, + "velocityX": 2.1420870889066284, + "velocityY": -1.2661949502166272e-25, + "timestamp": 1.9639388672378337 }, { - "x": 7.13188322084105, + "x": 7.24151583782572, "y": 7.39399003982544, - "heading": -1.0119189398601327e-18, - "angularVelocity": 5.4650069455448464e-18, - "velocityX": 1.8798181420416618, - "velocityY": 5.619002540306745e-22, - "timestamp": 2.0222739225460167 + "heading": -2.663585350994398e-19, + "angularVelocity": -7.807165345874594e-18, + "velocityX": 1.7136696985593165, + "velocityY": -9.096401200333518e-26, + "timestamp": 2.0621358105997256 }, { - "x": 7.276702544288512, + "x": 7.367723684098041, "y": 7.39399003982544, - "heading": -6.000129613332895e-19, - "angularVelocity": 4.277375706520152e-18, - "velocityX": 1.5038545265756458, - "velocityY": 4.495203072508259e-22, - "timestamp": 2.118572680762494 + "heading": -4.029353027704155e-19, + "angularVelocity": -1.390845344762092e-18, + "velocityX": 1.2852522894444902, + "velocityY": -6.226126404979919e-26, + "timestamp": 2.1603327539616175 }, { - "x": 7.385317037661432, + "x": 7.451862249084011, "y": 7.39399003982544, - "heading": -2.9682943520502703e-19, - "angularVelocity": 3.1483637414904354e-18, - "velocityX": 1.1278909031075715, - "velocityY": 3.3714029617848224e-22, - "timestamp": 2.214871438978971 - }, - { - "x": 7.457726700360463, - "y": 7.39399003982544, - "heading": -9.799024750037137e-20, - "angularVelocity": 2.064815645768494e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.2476023506664887e-22, - "timestamp": 2.311170197195448 + "heading": -2.1995708883691042e-19, + "angularVelocity": 1.8633799906775463e-18, + "velocityX": 0.8568348678215871, + "velocityY": -3.833262935804246e-26, + "timestamp": 2.2585296973235094 }, { "x": 7.493931531906128, "y": 7.39399003982544, "heading": 0, - "angularVelocity": 1.017564989623581e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.1238013392767656e-22, - "timestamp": 2.4074689554119253 + "angularVelocity": 2.2399586452459634e-18, + "velocityX": 0.42841743726253684, + "velocityY": -1.7862050890834192e-26, + "timestamp": 2.3567266406854013 }, { "x": 7.493931531906128, "y": 7.39399003982544, "heading": 0, "angularVelocity": 0, - "velocityX": -2.537301157014256e-39, + "velocityX": -2.1810930337522913e-39, "velocityY": 0, - "timestamp": 2.5037677136284024 + "timestamp": 2.454923584047293 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLaneTaxi.1.traj b/src/main/deploy/choreo/AmpLaneTaxi.1.traj index d2c18e05..7e838880 100644 --- a/src/main/deploy/choreo/AmpLaneTaxi.1.traj +++ b/src/main/deploy/choreo/AmpLaneTaxi.1.traj @@ -6,143 +6,134 @@ "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": 9.261974312050868e-43, + "velocityY": -5.968004408679696e-41, "timestamp": 0 }, { - "x": 1.5046861371964564, + "x": 1.5097881789229493, "y": 7.309329509735107, - "heading": -2.0425159024477705e-24, - "angularVelocity": -2.1355387753237974e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.2842385813221e-33, - "timestamp": 0.09564415279842821 + "heading": 1.867923413309381e-18, + "angularVelocity": 1.9312421328909037e-17, + "velocityX": 0.42198942031848213, + "velocityY": -8.175734433695576e-17, + "timestamp": 0.09672358706123589 }, { - "x": 1.5761147139482576, + "x": 1.5914208389649154, "y": 7.309329509735107, - "heading": -7.39456542057925e-24, - "angularVelocity": -5.595811575686501e-23, - "velocityX": 0.7468159282286507, - "velocityY": -1.2474076386660863e-32, - "timestamp": 0.19128830559685642 + "heading": -2.7784523382293792e-18, + "angularVelocity": -4.803678645072435e-17, + "velocityX": 0.843978832074171, + "velocityY": -1.635143003079991e-16, + "timestamp": 0.19344717412247178 }, { - "x": 1.6832575778116918, - "y": 7.309329509735107, - "heading": -1.797881654717191e-23, - "angularVelocity": -1.1066309979696055e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.866390782935744e-32, - "timestamp": 0.28693245839528464 + "x": 1.713869827417429, + "y": 7.309329509735108, + "heading": -6.149022858043543e-18, + "angularVelocity": -3.4846149955338706e-17, + "velocityX": 1.2659682314613812, + "velocityY": -2.4527069236460427e-16, + "timestamp": 0.29017076118370766 }, { - "x": 1.8261147273894114, + "x": 1.8771351424005531, "y": 7.309329509735107, - "heading": -3.609466520226707e-23, - "angularVelocity": -1.8940924580147868e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4777348604111533e-32, - "timestamp": 0.38257661119371283 + "heading": 9.334684137308459e-20, + "angularVelocity": 6.453994057032915e-17, + "velocityX": 1.6879576114124106, + "velocityY": -3.2702618744361184e-16, + "timestamp": 0.38689434824494356 }, { - "x": 2.004686160352503, - "y": 7.309329509735107, - "heading": -6.568172939485326e-23, - "angularVelocity": -3.093458575185046e-22, - "velocityX": 1.867039727346783, - "velocityY": -3.0967151425129867e-32, - "timestamp": 0.47822076399214103 - }, - { - "x": 2.21897187204314, - "y": 7.309329509735107, - "heading": -1.1554278307693229e-22, - "angularVelocity": -5.213187002510948e-22, - "velocityX": 2.2404475905834893, - "velocityY": -3.7086764301179e-32, - "timestamp": 0.5738649167905693 + "x": 2.081216780530401, + "y": 7.309329509735108, + "heading": 7.900764051063292e-18, + "angularVelocity": 8.072096604598862e-17, + "velocityX": 2.1099469563783173, + "velocityY": -4.087800036020465e-16, + "timestamp": 0.48361793530617947 }, { - "x": 2.468971848487854, - "y": 7.309329509735107, - "heading": -1.644337522360258e-22, - "angularVelocity": -5.111750826050956e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.332586658245059e-32, - "timestamp": 0.6695090695889975 + "x": 2.326114733911239, + "y": 7.309329509735108, + "heading": -4.0292085158169106e-19, + "angularVelocity": -8.584682780302335e-17, + "velocityX": 2.5319362197122866, + "velocityY": -4.905289237309452e-16, + "timestamp": 0.5803415223674153 }, { - "x": 2.7189718249325674, - "y": 7.309329509735107, - "heading": -1.1817502956999641e-22, - "angularVelocity": 4.836559085941933e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.2931003905648025e-32, - "timestamp": 0.7651532223874257 + "x": 2.611828963064465, + "y": 7.309329509735108, + "heading": 4.895920849005717e-18, + "angularVelocity": 5.476420787705161e-17, + "velocityX": 2.9539250748872585, + "velocityY": -5.724892251787591e-16, + "timestamp": 0.6770651094286512 }, { - "x": 2.933257536623205, - "y": 7.309329509735107, - "heading": -8.188453918715617e-23, - "angularVelocity": 3.79432828279643e-22, - "velocityX": 2.2404475905834897, - "velocityY": -3.6782254895688684e-32, - "timestamp": 0.8607973751858539 + "x": 2.856726916445305, + "y": 7.309329509735108, + "heading": 1.0580912487609066e-17, + "angularVelocity": 5.877889823861001e-17, + "velocityX": 2.531936219712306, + "velocityY": -4.90569760600763e-16, + "timestamp": 0.773788696489887 }, { - "x": 3.1118289695862966, - "y": 7.309329509735107, - "heading": -5.2607955008850217e-23, - "angularVelocity": 3.0609884687768117e-22, - "velocityX": 1.8670397273467831, - "velocityY": -3.06686637514316e-32, - "timestamp": 0.9564415279842821 + "x": 3.0608085545751536, + "y": 7.309329509735108, + "heading": 1.2560262621019845e-17, + "angularVelocity": 2.0466206900583654e-17, + "velocityX": 2.109946956378327, + "velocityY": -4.088065981186001e-16, + "timestamp": 0.8705122835511229 }, { - "x": 3.2546861191640164, + "x": 3.224073869558278, "y": 7.309329509735107, - "heading": -2.971367837778933e-23, - "angularVelocity": 2.3936923829343535e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4588855517580793e-32, - "timestamp": 1.0520856807827104 + "heading": 1.163556067743937e-17, + "angularVelocity": -9.558506562343336e-18, + "velocityX": 1.687957611412416, + "velocityY": -3.270442505924889e-16, + "timestamp": 0.9672358706123587 }, { - "x": 3.3618289830274506, + "x": 3.3465228580107924, "y": 7.309329509735107, - "heading": -1.3562846674979416e-23, - "angularVelocity": 1.6886357418178234e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.8475316069845286e-32, - "timestamp": 1.1477298335811386 + "heading": 1.1661599473339483e-17, + "angularVelocity": 2.705282706656373e-19, + "velocityX": 1.2659682314613843, + "velocityY": -2.4528264910978726e-16, + "timestamp": 1.0639594576735947 }, { - "x": 3.4332575597792516, + "x": 3.4281555180527588, "y": 7.309329509735107, - "heading": -3.7629345495763975e-24, - "angularVelocity": 1.0246209621359342e-22, - "velocityX": 0.7468159282286508, - "velocityY": -1.2285406220691536e-32, - "timestamp": 1.2433739863795668 + "heading": 1.4643178138706357e-18, + "angularVelocity": -1.0542614870618343e-16, + "velocityX": 0.8439788320741728, + "velocityY": -1.6352149946538548e-16, + "timestamp": 1.1606830447348306 }, { "x": 3.468971848487854, "y": 7.309329509735107, - "heading": -4.805430418918825e-41, - "angularVelocity": 3.9342997042908397e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.171880734353278e-33, - "timestamp": 1.339018139177995 + "heading": -3.620479666148997e-40, + "angularVelocity": -1.5138749754669695e-17, + "velocityX": 0.42198942031848286, + "velocityY": -8.176064575101043e-17, + "timestamp": 1.2574066317960666 }, { "x": 3.468971848487854, "y": 7.309329509735107, - "heading": -6.447284134666629e-41, - "angularVelocity": -1.716658280508069e-40, - "velocityX": 0, - "velocityY": 0, - "timestamp": 1.4346622919764231 + "heading": -2.966209034098039e-40, + "angularVelocity": 6.774689529655474e-40, + "velocityX": 3.003387584304273e-39, + "velocityY": 2.5761438166078746e-41, + "timestamp": 1.3541302188573026 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLaneTaxi.traj b/src/main/deploy/choreo/AmpLaneTaxi.traj index d2c18e05..7e838880 100644 --- a/src/main/deploy/choreo/AmpLaneTaxi.traj +++ b/src/main/deploy/choreo/AmpLaneTaxi.traj @@ -6,143 +6,134 @@ "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": 9.261974312050868e-43, + "velocityY": -5.968004408679696e-41, "timestamp": 0 }, { - "x": 1.5046861371964564, + "x": 1.5097881789229493, "y": 7.309329509735107, - "heading": -2.0425159024477705e-24, - "angularVelocity": -2.1355387753237974e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.2842385813221e-33, - "timestamp": 0.09564415279842821 + "heading": 1.867923413309381e-18, + "angularVelocity": 1.9312421328909037e-17, + "velocityX": 0.42198942031848213, + "velocityY": -8.175734433695576e-17, + "timestamp": 0.09672358706123589 }, { - "x": 1.5761147139482576, + "x": 1.5914208389649154, "y": 7.309329509735107, - "heading": -7.39456542057925e-24, - "angularVelocity": -5.595811575686501e-23, - "velocityX": 0.7468159282286507, - "velocityY": -1.2474076386660863e-32, - "timestamp": 0.19128830559685642 + "heading": -2.7784523382293792e-18, + "angularVelocity": -4.803678645072435e-17, + "velocityX": 0.843978832074171, + "velocityY": -1.635143003079991e-16, + "timestamp": 0.19344717412247178 }, { - "x": 1.6832575778116918, - "y": 7.309329509735107, - "heading": -1.797881654717191e-23, - "angularVelocity": -1.1066309979696055e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.866390782935744e-32, - "timestamp": 0.28693245839528464 + "x": 1.713869827417429, + "y": 7.309329509735108, + "heading": -6.149022858043543e-18, + "angularVelocity": -3.4846149955338706e-17, + "velocityX": 1.2659682314613812, + "velocityY": -2.4527069236460427e-16, + "timestamp": 0.29017076118370766 }, { - "x": 1.8261147273894114, + "x": 1.8771351424005531, "y": 7.309329509735107, - "heading": -3.609466520226707e-23, - "angularVelocity": -1.8940924580147868e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4777348604111533e-32, - "timestamp": 0.38257661119371283 + "heading": 9.334684137308459e-20, + "angularVelocity": 6.453994057032915e-17, + "velocityX": 1.6879576114124106, + "velocityY": -3.2702618744361184e-16, + "timestamp": 0.38689434824494356 }, { - "x": 2.004686160352503, - "y": 7.309329509735107, - "heading": -6.568172939485326e-23, - "angularVelocity": -3.093458575185046e-22, - "velocityX": 1.867039727346783, - "velocityY": -3.0967151425129867e-32, - "timestamp": 0.47822076399214103 - }, - { - "x": 2.21897187204314, - "y": 7.309329509735107, - "heading": -1.1554278307693229e-22, - "angularVelocity": -5.213187002510948e-22, - "velocityX": 2.2404475905834893, - "velocityY": -3.7086764301179e-32, - "timestamp": 0.5738649167905693 + "x": 2.081216780530401, + "y": 7.309329509735108, + "heading": 7.900764051063292e-18, + "angularVelocity": 8.072096604598862e-17, + "velocityX": 2.1099469563783173, + "velocityY": -4.087800036020465e-16, + "timestamp": 0.48361793530617947 }, { - "x": 2.468971848487854, - "y": 7.309329509735107, - "heading": -1.644337522360258e-22, - "angularVelocity": -5.111750826050956e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.332586658245059e-32, - "timestamp": 0.6695090695889975 + "x": 2.326114733911239, + "y": 7.309329509735108, + "heading": -4.0292085158169106e-19, + "angularVelocity": -8.584682780302335e-17, + "velocityX": 2.5319362197122866, + "velocityY": -4.905289237309452e-16, + "timestamp": 0.5803415223674153 }, { - "x": 2.7189718249325674, - "y": 7.309329509735107, - "heading": -1.1817502956999641e-22, - "angularVelocity": 4.836559085941933e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.2931003905648025e-32, - "timestamp": 0.7651532223874257 + "x": 2.611828963064465, + "y": 7.309329509735108, + "heading": 4.895920849005717e-18, + "angularVelocity": 5.476420787705161e-17, + "velocityX": 2.9539250748872585, + "velocityY": -5.724892251787591e-16, + "timestamp": 0.6770651094286512 }, { - "x": 2.933257536623205, - "y": 7.309329509735107, - "heading": -8.188453918715617e-23, - "angularVelocity": 3.79432828279643e-22, - "velocityX": 2.2404475905834897, - "velocityY": -3.6782254895688684e-32, - "timestamp": 0.8607973751858539 + "x": 2.856726916445305, + "y": 7.309329509735108, + "heading": 1.0580912487609066e-17, + "angularVelocity": 5.877889823861001e-17, + "velocityX": 2.531936219712306, + "velocityY": -4.90569760600763e-16, + "timestamp": 0.773788696489887 }, { - "x": 3.1118289695862966, - "y": 7.309329509735107, - "heading": -5.2607955008850217e-23, - "angularVelocity": 3.0609884687768117e-22, - "velocityX": 1.8670397273467831, - "velocityY": -3.06686637514316e-32, - "timestamp": 0.9564415279842821 + "x": 3.0608085545751536, + "y": 7.309329509735108, + "heading": 1.2560262621019845e-17, + "angularVelocity": 2.0466206900583654e-17, + "velocityX": 2.109946956378327, + "velocityY": -4.088065981186001e-16, + "timestamp": 0.8705122835511229 }, { - "x": 3.2546861191640164, + "x": 3.224073869558278, "y": 7.309329509735107, - "heading": -2.971367837778933e-23, - "angularVelocity": 2.3936923829343535e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4588855517580793e-32, - "timestamp": 1.0520856807827104 + "heading": 1.163556067743937e-17, + "angularVelocity": -9.558506562343336e-18, + "velocityX": 1.687957611412416, + "velocityY": -3.270442505924889e-16, + "timestamp": 0.9672358706123587 }, { - "x": 3.3618289830274506, + "x": 3.3465228580107924, "y": 7.309329509735107, - "heading": -1.3562846674979416e-23, - "angularVelocity": 1.6886357418178234e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.8475316069845286e-32, - "timestamp": 1.1477298335811386 + "heading": 1.1661599473339483e-17, + "angularVelocity": 2.705282706656373e-19, + "velocityX": 1.2659682314613843, + "velocityY": -2.4528264910978726e-16, + "timestamp": 1.0639594576735947 }, { - "x": 3.4332575597792516, + "x": 3.4281555180527588, "y": 7.309329509735107, - "heading": -3.7629345495763975e-24, - "angularVelocity": 1.0246209621359342e-22, - "velocityX": 0.7468159282286508, - "velocityY": -1.2285406220691536e-32, - "timestamp": 1.2433739863795668 + "heading": 1.4643178138706357e-18, + "angularVelocity": -1.0542614870618343e-16, + "velocityX": 0.8439788320741728, + "velocityY": -1.6352149946538548e-16, + "timestamp": 1.1606830447348306 }, { "x": 3.468971848487854, "y": 7.309329509735107, - "heading": -4.805430418918825e-41, - "angularVelocity": 3.9342997042908397e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.171880734353278e-33, - "timestamp": 1.339018139177995 + "heading": -3.620479666148997e-40, + "angularVelocity": -1.5138749754669695e-17, + "velocityX": 0.42198942031848286, + "velocityY": -8.176064575101043e-17, + "timestamp": 1.2574066317960666 }, { "x": 3.468971848487854, "y": 7.309329509735107, - "heading": -6.447284134666629e-41, - "angularVelocity": -1.716658280508069e-40, - "velocityX": 0, - "velocityY": 0, - "timestamp": 1.4346622919764231 + "heading": -2.966209034098039e-40, + "angularVelocity": 6.774689529655474e-40, + "velocityX": 3.003387584304273e-39, + "velocityY": 2.5761438166078746e-41, + "timestamp": 1.3541302188573026 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBDA.1.traj b/src/main/deploy/choreo/CenterLanePBDA.1.traj index 00ef4e13..40f0d8e3 100644 --- a/src/main/deploy/choreo/CenterLanePBDA.1.traj +++ b/src/main/deploy/choreo/CenterLanePBDA.1.traj @@ -4,118 +4,118 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": 4.86675280124204e-44, - "velocityX": -2.693796222451342e-38, - "velocityY": -1.1654231803151638e-36, + "angularVelocity": 2.2921355473422064e-38, + "velocityX": 1.1360383232068741e-38, + "velocityY": -2.144062321628808e-38, "timestamp": 0 }, { - "x": 1.3282580675803197, - "y": 5.589895062878959, - "heading": -7.401619546168889e-20, - "angularVelocity": -7.4730824316044545e-19, - "velocityX": 0.3865324611915868, - "velocityY": -0.010694678686168042, - "timestamp": 0.09904372946385107 + "x": 1.32825806778149, + "y": 5.589895062873393, + "heading": -2.8062774074031584e-19, + "angularVelocity": -2.995198561755265e-18, + "velocityX": 0.40860904707084805, + "velocityY": -0.011305499292899219, + "timestamp": 0.09369253321938388 }, { - "x": 1.4048252996715709, - "y": 5.587776581179956, - "heading": -2.950926029442004e-19, - "angularVelocity": -2.23210907625606e-18, - "velocityX": 0.7730649128998782, - "velocityY": -0.021389357109949856, - "timestamp": 0.19808745892770213 + "x": 1.4048253002214361, + "y": 5.587776581164742, + "heading": -1.768868342127426e-19, + "angularVelocity": 1.1072483896415275e-18, + "velocityX": 0.8172180835441991, + "velocityY": -0.022610998292584186, + "timestamp": 0.18738506643876776 }, { - "x": 1.5196761458628356, - "y": 5.584598858685282, - "heading": -2.379692913245565e-19, - "angularVelocity": 5.76748391128508e-19, - "velocityX": 1.159597349705849, - "velocityY": -0.03208403512141046, - "timestamp": 0.29713118839155317 + "x": 1.519676146824621, + "y": 5.584598858658671, + "heading": -2.386873521248768e-19, + "angularVelocity": -6.596098514057519e-19, + "velocityX": 1.225827103364341, + "velocityY": -0.03391649683150392, + "timestamp": 0.28107759965815166 }, { - "x": 1.6728106034973478, - "y": 5.580361895468446, - "heading": -4.645307250560927e-19, - "angularVelocity": -2.2874889198737866e-18, - "velocityX": 1.5461297596876444, - "velocityY": -0.04277871239069293, - "timestamp": 0.39617491785540426 + "x": 1.6728106047825377, + "y": 5.580361895432887, + "heading": -7.795008789281329e-19, + "angularVelocity": -5.772215866345043e-18, + "velocityX": 1.6344360932087083, + "velocityY": -0.04522199454104627, + "timestamp": 0.3747701328775355 }, { - "x": 1.8642286663759866, - "y": 5.575065691700967, - "heading": -6.651013199966167e-20, - "angularVelocity": 4.018634952571123e-18, - "velocityX": 1.9326621070797045, - "velocityY": -0.05347338792822665, - "timestamp": 0.49521864731925536 + "x": 1.8642286675420066, + "y": 5.575065691668706, + "heading": -1.0638718286937245e-18, + "angularVelocity": -3.0351506143991226e-18, + "velocityX": 2.0430450131096105, + "velocityY": -0.056527490315375004, + "timestamp": 0.4684626660969194 }, { - "x": 2.0939303035031647, - "y": 5.568710248240439, - "heading": 1.5670193439704088e-19, - "angularVelocity": 2.2536718639837896e-18, - "velocityX": 2.3191941415232606, - "velocityY": -0.06416805480702138, - "timestamp": 0.5942623767831064 + "x": 2.0939303023371445, + "y": 5.5687102482727004, + "heading": -1.0113200205360793e-18, + "angularVelocity": 5.608964381023318e-19, + "velocityX": 2.451653583293393, + "velocityY": -0.0678329764136413, + "timestamp": 0.5621551993163033 }, { - "x": 2.285348366381803, - "y": 5.56341404447296, - "heading": 5.516454674221488e-20, - "angularVelocity": -1.0251773454460286e-18, - "velocityX": 1.9326621070797048, - "velocityY": -0.05347338792822665, - "timestamp": 0.6933061062469575 + "x": 2.285348365096613, + "y": 5.563414044508519, + "heading": -7.732946400687193e-19, + "angularVelocity": 2.5404946615264457e-18, + "velocityX": 2.04304501310961, + "velocityY": -0.056527490315375004, + "timestamp": 0.6558477325356872 }, { - "x": 2.438482824016315, - "y": 5.5591770812561245, - "heading": 6.884550294722241e-20, - "angularVelocity": 1.3813046296889445e-19, - "velocityX": 1.5461297596876444, - "velocityY": -0.04277871239069293, - "timestamp": 0.7923498357108086 + "x": 2.43848282305453, + "y": 5.559177081282735, + "heading": -4.312009437616493e-19, + "angularVelocity": 3.651237559230762e-18, + "velocityX": 1.634436093208708, + "velocityY": -0.04522199454104628, + "timestamp": 0.749540265755071 }, { - "x": 2.55333367020758, - "y": 5.555999358761451, - "heading": 2.3879991444087165e-19, - "angularVelocity": 1.7159532704761406e-18, - "velocityX": 1.1595973497058492, - "velocityY": -0.032084035121410455, - "timestamp": 0.8913935651746597 + "x": 2.553333669657715, + "y": 5.555999358776664, + "heading": -7.015602955490743e-20, + "angularVelocity": 3.853507870950435e-18, + "velocityX": 1.2258271033643409, + "velocityY": -0.033916496831503926, + "timestamp": 0.8432327989744549 }, { - "x": 2.629900902298831, - "y": 5.553880877062447, - "heading": 4.476119744497135e-19, - "angularVelocity": 2.1082814746495792e-18, - "velocityX": 0.7730649128998782, - "velocityY": -0.021389357109949853, - "timestamp": 0.9904372946385108 + "x": 2.6299009020976607, + "y": 5.553880877068013, + "heading": -2.7026882274997263e-20, + "angularVelocity": 4.603264080766157e-19, + "velocityX": 0.817218083544199, + "velocityY": -0.022610998292584186, + "timestamp": 0.9369253321938388 }, { "x": 2.668184518814087, "y": 5.552821636199951, "heading": 0, - "angularVelocity": -4.519336831041715e-18, - "velocityX": 0.3865324611915868, - "velocityY": -0.010694678686168042, - "timestamp": 1.0894810241023618 + "angularVelocity": 2.884635663770645e-19, + "velocityX": 0.40860904707084805, + "velocityY": -0.011305499292899219, + "timestamp": 1.0306178654132228 }, { "x": 2.668184518814087, "y": 5.552821636199951, "heading": 0, "angularVelocity": 0, - "velocityX": -1.8370589120240526e-33, - "velocityY": -9.456793291047022e-33, - "timestamp": 1.188524753566213 + "velocityX": 0, + "velocityY": -1.1410946614987667e-37, + "timestamp": 1.1243103986326066 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBDA.2.traj b/src/main/deploy/choreo/CenterLanePBDA.2.traj index 4eb93e7b..3b4bdef8 100644 --- a/src/main/deploy/choreo/CenterLanePBDA.2.traj +++ b/src/main/deploy/choreo/CenterLanePBDA.2.traj @@ -5,540 +5,513 @@ "y": 5.552821636199951, "heading": 0, "angularVelocity": 0, - "velocityX": -1.8370589120240526e-33, - "velocityY": -9.456793291047022e-33, + "velocityX": 0, + "velocityY": -1.1410946614987667e-37, "timestamp": 0 }, { - "x": 2.6845520739340656, - "y": 5.55917649064291, - "heading": 0.0014320304181618415, - "angularVelocity": 0.02135174975700707, - "velocityX": 0.2440422609907963, - "velocityY": 0.0947516617576028, - "timestamp": 0.06706852761291304 - }, - { - "x": 2.7172893704980314, - "y": 5.5718808969789135, - "heading": 0.004271918603718149, - "angularVelocity": 0.042343082316444396, - "velocityX": 0.4881171203415973, - "velocityY": 0.189424261843461, - "timestamp": 0.13413705522582609 - }, - { - "x": 2.766398904551383, - "y": 5.590928790480421, - "heading": 0.008491864035516146, - "angularVelocity": 0.06291990568442865, - "velocityX": 0.7322291960364448, - "velocityY": 0.2840064360953652, - "timestamp": 0.20120558283873913 - }, - { - "x": 2.8318835522169303, - "y": 5.616313167431409, - "heading": 0.014059653133075762, - "angularVelocity": 0.08301642060332945, - "velocityX": 0.9763841550763344, - "velocityY": 0.3784841840795224, - "timestamp": 0.26827411045165217 - }, - { - "x": 2.9137466632036926, - "y": 5.648025848972547, - "heading": 0.020937610104521436, - "angularVelocity": 0.10255118482907341, - "velocityX": 1.2205890586898858, - "velocityY": 0.47283998426459056, - "timestamp": 0.3353426380645652 - }, - { - "x": 3.011992187841884, - "y": 5.68605715842892, - "heading": 0.02908116230944334, - "angularVelocity": 0.12142136550130508, - "velocityX": 1.4648528622130628, - "velocityY": 0.5670515040358618, - "timestamp": 0.40241116567747826 - }, - { - "x": 3.126624854139339, - "y": 5.730395468508435, - "heading": 0.03843682536129391, - "angularVelocity": 0.13949408738845287, - "velocityX": 1.709187160914865, - "velocityY": 0.6610896594512152, - "timestamp": 0.4694796932903913 - }, - { - "x": 3.257650422147747, - "y": 5.781026545419884, - "heading": 0.04893928266503782, - "angularVelocity": 0.1565929308059208, - "velocityX": 1.9536073426961744, - "velocityY": 0.7549155872880825, - "timestamp": 0.5365482209033043 - }, - { - "x": 3.405076062959565, - "y": 5.837932561689472, - "heading": 0.0605069886709794, - "angularVelocity": 0.1724759200426279, - "velocityX": 2.1981344463484804, - "velocityY": 0.8484757052968557, - "timestamp": 0.6036167485162174 - }, - { - "x": 3.568910949214301, - "y": 5.901090538280509, - "heading": 0.07303523263359421, - "angularVelocity": 0.18679765917811303, - "velocityX": 2.4427983151846027, - "velocityY": 0.9416932030408492, - "timestamp": 0.6706852761291304 - }, - { - "x": 3.749167228301205, - "y": 5.970469734020464, - "heading": 0.08638452799993987, - "angularVelocity": 0.19903963664434834, - "velocityX": 2.6876433030892812, - "velocityY": 1.0344523461194588, - "timestamp": 0.7377538037420435 - }, - { - "x": 3.9458617473368536, - "y": 6.0460269127291015, - "heading": 0.10035960589660178, - "angularVelocity": 0.20837013117865452, - "velocityX": 2.9327394835752725, - "velocityY": 1.1265668324301998, - "timestamp": 0.8048223313549565 - }, - { - "x": 4.159019426107489, - "y": 6.127696782408767, - "heading": 0.1146671212326001, - "angularVelocity": 0.2133268143081109, - "velocityX": 3.178207221140708, - "velocityY": 1.2177078070212743, - "timestamp": 0.8718908589678696 - }, - { - "x": 4.388680844705306, - "y": 6.21536934931964, - "heading": 0.12881599036360453, - "angularVelocity": 0.21096137986903263, - "velocityX": 3.4242800128744704, - "velocityY": 1.3072087614160277, - "timestamp": 0.9389593865807824 - }, - { - "x": 4.63492344353525, - "y": 6.3088202142113, - "heading": 0.14181307281667466, - "angularVelocity": 0.19378809876493772, - "velocityX": 3.671507450575592, - "velocityY": 1.3933638953729879, - "timestamp": 1.0060279141936952 + "x": 2.687410064097537, + "y": 5.560258515634833, + "heading": 0.0028176822125938034, + "angularVelocity": 0.04098002734374148, + "velocityX": 0.2796139919160097, + "velocityY": 0.1081610698436357, + "timestamp": 0.06875745076886175 + }, + { + "x": 2.7258611531960137, + "y": 5.575132263104083, + "heading": 0.008453116109374614, + "angularVelocity": 0.08196106507388369, + "velocityX": 0.5592279624754555, + "velocityY": 0.2163219738796251, + "timestamp": 0.1375149015377235 + }, + { + "x": 2.783537789303066, + "y": 5.597442863001631, + "heading": 0.016905769734324238, + "angularVelocity": 0.12293436610040698, + "velocityX": 0.8388419794814749, + "velocityY": 0.3244826509427688, + "timestamp": 0.20627235230658525 + }, + { + "x": 2.8604399802132514, + "y": 5.627190295278175, + "heading": 0.028174540960364546, + "angularVelocity": 0.1638916379247738, + "velocityX": 1.118456109850604, + "velocityY": 0.4326430364113507, + "timestamp": 0.275029803075447 + }, + { + "x": 2.9565677382168056, + "y": 5.664374535134068, + "heading": 0.042257801458542396, + "angularVelocity": 0.20482522753092394, + "velocityX": 1.3980704189674162, + "velocityY": 0.5408030611968617, + "timestamp": 0.34378725384430875 + }, + { + "x": 3.0719210799387406, + "y": 5.708995552649304, + "heading": 0.05915345386800317, + "angularVelocity": 0.2457283133759373, + "velocityX": 1.6776849698763825, + "velocityY": 0.648962650829455, + "timestamp": 0.4125447046131705 + }, + { + "x": 3.2065000261043313, + "y": 5.761053312357379, + "heading": 0.07885900262196403, + "angularVelocity": 0.2865951040012339, + "velocityX": 1.9572998222112317, + "velocityY": 0.757121724641519, + "timestamp": 0.48130215538203225 + }, + { + "x": 3.360304601198153, + "y": 5.820547772766793, + "heading": 0.1013716386695389, + "angularVelocity": 0.32742104013214957, + "velocityX": 2.2369150306467707, + "velocityY": 0.8652801950062525, + "timestamp": 0.550059606150894 + }, + { + "x": 3.5333348329448953, + "y": 5.8874788858214675, + "heading": 0.12668833767209217, + "angularVelocity": 0.36820299064984424, + "velocityX": 2.5165306423068015, + "velocityY": 0.973437966449241, + "timestamp": 0.6188170569197557 + }, + { + "x": 3.7255907514118967, + "y": 5.961846596243587, + "heading": 0.15480596913918515, + "angularVelocity": 0.4089394116954055, + "velocityX": 2.796146691262561, + "velocityY": 1.0815949339384094, + "timestamp": 0.6875745076886175 + }, + { + "x": 3.937072386955544, + "y": 6.04365084047756, + "heading": 0.18572140510730675, + "angularVelocity": 0.44963034002016317, + "velocityX": 3.075763181717921, + "velocityY": 1.1897509770827353, + "timestamp": 0.7563319584574792 + }, + { + "x": 4.167779761570079, + "y": 6.132891543122838, + "heading": 0.21943155266112502, + "angularVelocity": 0.4902762853605699, + "velocityX": 3.355379992054849, + "velocityY": 1.2979059236107295, + "timestamp": 0.825089409226341 + }, + { + "x": 4.410308649457132, + "y": 6.2266961328710915, + "heading": 0.21943155790785696, + "angularVelocity": 7.630783127279499e-8, + "velocityX": 3.5273106430654275, + "velocityY": 1.3642825424635276, + "timestamp": 0.8938468599952027 + }, + { + "x": 4.652837676050608, + "y": 6.320500363997887, + "heading": 0.21943156315423873, + "angularVelocity": 7.630273806741925e-8, + "velocityX": 3.5273126603947684, + "velocityY": 1.3642773267166644, + "timestamp": 0.9626043107640647 }, { "x": 4.897946357727051, "y": 6.407341003417969, - "heading": 0.1505657563591837, - "angularVelocity": 0.13050358870296755, - "velocityX": 3.9217040175660647, - "velocityY": 1.4689570908024474, - "timestamp": 1.073096441806608 - }, - { - "x": 5.1485624399320535, - "y": 6.485405317317771, - "heading": 0.15056576180436299, - "angularVelocity": 8.767085583792628e-8, - "velocityX": 4.035078615644034, - "velocityY": 1.2568851962354253, - "timestamp": 1.135205784914166 - }, - { - "x": 5.403242092396325, - "y": 6.54897207571473, - "heading": 0.150565765506882, - "angularVelocity": 5.961291546210121e-8, - "velocityX": 4.100504686118055, - "velocityY": 1.0234653148229949, - "timestamp": 1.1973151280217238 - }, - { - "x": 5.658526059829905, - "y": 6.6100667082463955, - "heading": 0.15056576920637427, - "angularVelocity": 5.956418276221387e-8, - "velocityX": 4.110234542192673, - "velocityY": 0.9836625131562424, - "timestamp": 1.2594244711292817 - }, - { - "x": 5.912767823919927, - "y": 6.675362821260306, - "heading": 0.1505657770066318, - "angularVelocity": 1.2558911624775388e-7, - "velocityX": 4.093454404271187, - "velocityY": 1.0513090260965858, - "timestamp": 1.3215338142368396 - }, - { - "x": 6.15335684749113, - "y": 6.741282299763531, - "heading": 0.17760143548847274, - "angularVelocity": 0.4352913286334883, - "velocityX": 3.8736365824150507, - "velocityY": 1.061345607681999, - "timestamp": 1.3836431573443975 - }, - { - "x": 6.3792484159879335, - "y": 6.80391531011499, - "heading": 0.2045043567072466, - "angularVelocity": 0.4331541741181402, - "velocityX": 3.6369981905237077, - "velocityY": 1.0084313763066106, - "timestamp": 1.4457525004519554 - }, - { - "x": 6.59046786405772, - "y": 6.863155314435484, - "heading": 0.2306695247633623, - "angularVelocity": 0.42127587810427364, - "velocityX": 3.400767702598441, - "velocityY": 0.9538018171903946, - "timestamp": 1.5078618435595132 - }, - { - "x": 6.787024437672222, - "y": 6.918965755644967, - "heading": 0.2558883618424076, - "angularVelocity": 0.4060393463729393, - "velocityX": 3.1646860807095556, - "velocityY": 0.8985836657912935, - "timestamp": 1.5699711866670711 - }, - { - "x": 6.968922917887586, - "y": 6.971328161049253, - "heading": 0.28005507074306385, - "angularVelocity": 0.3890994122865805, - "velocityX": 2.928681436871152, - "velocityY": 0.8430680922450434, - "timestamp": 1.632080529774629 - }, - { - "x": 7.136166224572301, - "y": 7.020231388365112, - "heading": 0.3031055255244397, - "angularVelocity": 0.3711270096909292, - "velocityX": 2.6927238047759023, - "velocityY": 0.7873731208389769, - "timestamp": 1.694189872882187 - }, - { - "x": 7.288756325694741, - "y": 7.065667987349534, - "heading": 0.3249965680879426, - "angularVelocity": 0.3524597342076698, - "velocityX": 2.4567978582254835, - "velocityY": 0.7315581957730963, - "timestamp": 1.7562992159897448 - }, - { - "x": 7.426694637421038, - "y": 7.107632627152973, - "heading": 0.34569706172661535, - "angularVelocity": 0.33329113790214354, - "velocityX": 2.2208947128521856, - "velocityY": 0.6756574406327761, - "timestamp": 1.8184085590973027 - }, - { - "x": 7.549982227615165, - "y": 7.146121305576968, - "heading": 0.3651834005383923, - "angularVelocity": 0.3137424715317206, - "velocityX": 1.9850087607692757, - "velocityY": 0.6196922475470147, - "timestamp": 1.8805179022048606 - }, - { - "x": 7.6586199302579505, - "y": 7.181130908016494, - "heading": 0.38343701279157455, - "angularVelocity": 0.2938947884470677, - "velocityX": 1.7491362363091372, - "velocityY": 0.5636769073357482, - "timestamp": 1.9426272453124185 - }, - { - "x": 7.752608414705494, - "y": 7.212658942032951, - "heading": 0.40044286570608717, - "angularVelocity": 0.27380506802434795, - "velocityX": 1.5132744889086964, - "velocityY": 0.5076214372748387, - "timestamp": 2.004736588419976 - }, - { - "x": 7.831948230057004, - "y": 7.240703368053924, - "heading": 0.41618851788690064, - "angularVelocity": 0.2535150332140168, - "velocityX": 1.2774215823553952, - "velocityY": 0.45153312879840174, - "timestamp": 2.066845931527534 - }, - { - "x": 7.896639834885265, - "y": 7.265262486298443, - "heading": 0.43066349152059785, - "angularVelocity": 0.2330562989312256, - "velocityX": 1.0415760591161314, - "velocityY": 0.39541745276528995, - "timestamp": 2.128955274635092 - }, - { - "x": 7.946683617901462, - "y": 7.286334858378348, - "heading": 0.4438588411976607, - "angularVelocity": 0.2124535378551918, - "velocityX": 0.8057367943746069, - "velocityY": 0.3392786177662535, - "timestamp": 2.19106461774265 - }, - { - "x": 7.982079912761947, - "y": 7.303919251231725, - "heading": 0.4557668488029474, - "angularVelocity": 0.19172651020740672, - "velocityX": 0.5699029017130068, - "velocityY": 0.2831199296846922, - "timestamp": 2.2531739608502077 - }, - { - "x": 8.002829008953428, - "y": 7.318014595966873, - "heading": 0.4663808020502884, - "angularVelocity": 0.17089141047523468, - "velocityX": 0.3340736699718424, - "velocityY": 0.22694403176565744, - "timestamp": 2.3152833039577656 + "heading": 0.21943156841217684, + "angularVelocity": 7.647081243446701e-8, + "velocityX": 3.5648308501199604, + "velocityY": 1.2629996960185401, + "timestamp": 1.0313617615329267 + }, + { + "x": 5.1308682473529785, + "y": 6.471009903235158, + "heading": 0.21943157303192085, + "angularVelocity": 7.23563158909772e-8, + "velocityX": 3.6481177055177683, + "velocityY": 0.9972082962532426, + "timestamp": 1.0952089036560089 + }, + { + "x": 5.365529567427774, + "y": 6.527934417793533, + "heading": 0.21943157763716262, + "angularVelocity": 7.21291764013049e-8, + "velocityX": 3.6753613751798433, + "velocityY": 0.8915749815181846, + "timestamp": 1.159056045779091 + }, + { + "x": 5.600190991292763, + "y": 6.584858504493345, + "heading": 0.21943158224240422, + "angularVelocity": 7.21291734995501e-8, + "velocityX": 3.6753630007842206, + "velocityY": 0.8915682802227483, + "timestamp": 1.2229031879021732 + }, + { + "x": 5.8348524151632635, + "y": 6.641782591170481, + "heading": 0.2194315868476458, + "angularVelocity": 7.212917336838375e-8, + "velocityX": 3.6753630008705414, + "velocityY": 0.8915682798675781, + "timestamp": 1.2867503300252554 + }, + { + "x": 6.069513839029734, + "y": 6.698706677864191, + "heading": 0.21943159145288735, + "angularVelocity": 7.212917280605725e-8, + "velocityX": 3.6753630008074327, + "velocityY": 0.891568280127171, + "timestamp": 1.3505974721483376 + }, + { + "x": 6.3041751870414915, + "y": 6.75563107725722, + "heading": 0.21943159605812906, + "angularVelocity": 7.212917561549481e-8, + "velocityX": 3.6753618127399528, + "velocityY": 0.8915731777514998, + "timestamp": 1.4144446142714198 + }, + { + "x": 6.537489271202827, + "y": 6.817847413489264, + "heading": 0.21943160514124108, + "angularVelocity": 1.4226340838423697e-7, + "velocityX": 3.6542604164106804, + "velocityY": 0.9744576524998471, + "timestamp": 1.478291756394502 + }, + { + "x": 6.7545327794045615, + "y": 6.881509207420976, + "heading": 0.24890077922126197, + "angularVelocity": 0.46155823268064305, + "velocityX": 3.399424014677561, + "velocityY": 0.9970970009742466, + "timestamp": 1.5421388985175841 + }, + { + "x": 6.954212307545749, + "y": 6.941327014961267, + "heading": 0.2778274290995393, + "angularVelocity": 0.45306099719431336, + "velocityX": 3.1274622716276532, + "velocityY": 0.9368909171372027, + "timestamp": 1.6059860406406663 + }, + { + "x": 7.1365689728963595, + "y": 6.997132012930522, + "heading": 0.305376589845945, + "angularVelocity": 0.43148620017020184, + "velocityX": 2.856144524042602, + "velocityY": 0.8740406557535503, + "timestamp": 1.6698331827637485 + }, + { + "x": 7.301617943292834, + "y": 7.048866027505986, + "heading": 0.33125825392103403, + "angularVelocity": 0.40536918669284566, + "velocityX": 2.585064341302851, + "velocityY": 0.8102792522134878, + "timestamp": 1.7336803248868307 + }, + { + "x": 7.449367094221676, + "y": 7.096499619737108, + "heading": 0.35532461486989275, + "angularVelocity": 0.37693716818945566, + "velocityX": 2.3141075076472033, + "velocityY": 0.7460567638139751, + "timestamp": 1.7975274670099128 + }, + { + "x": 7.579821246213137, + "y": 7.140015020198173, + "heading": 0.3774857294479878, + "angularVelocity": 0.347096421878647, + "velocityX": 2.04322617510391, + "velocityY": 0.681555963416076, + "timestamp": 1.861374609132995 + }, + { + "x": 7.69298365301254, + "y": 7.179400342359022, + "heading": 0.3976808814927073, + "angularVelocity": 0.3163047142468991, + "velocityX": 1.7723958040479335, + "velocityY": 0.6168689913312067, + "timestamp": 1.9252217512560772 + }, + { + "x": 7.788856658439436, + "y": 7.214647078189013, + "heading": 0.41586620005922853, + "angularVelocity": 0.2848258819708253, + "velocityX": 1.5016021428504343, + "velocityY": 0.5520487629977263, + "timestamp": 1.9890688933791594 + }, + { + "x": 7.867442031134741, + "y": 7.2457488378850705, + "heading": 0.43200844627235874, + "angularVelocity": 0.2528264488645923, + "velocityX": 1.2308361828288252, + "velocityY": 0.48712845496042256, + "timestamp": 2.0529160355022418 + }, + { + "x": 7.928741153018892, + "y": 7.27270064652237, + "heading": 0.4460815614823235, + "angularVelocity": 0.22041887454940703, + "velocityX": 0.9600918670092122, + "velocityY": 0.4221302276198955, + "timestamp": 2.116763177625324 + }, + { + "x": 7.9727551334716695, + "y": 7.29549852060884, + "heading": 0.45806460123681797, + "angularVelocity": 0.1876832596734667, + "velocityX": 0.6893649267484526, + "velocityY": 0.35706960920065395, + "timestamp": 2.180610319748406 + }, + { + "x": 7.999484882530383, + "y": 7.314139197912392, + "heading": 0.4679404256802237, + "angularVelocity": 0.1546791933829663, + "velocityX": 0.4186522398635212, + "velocityY": 0.2919578963709263, + "timestamp": 2.2444574618714883 }, { "x": 8.008931159973145, "y": 7.328619956970215, "heading": 0.4756948301053548, - "angularVelocity": 0.14996178656947964, - "velocityX": 0.09824851969771953, - "velocityY": 0.17075306987185124, - "timestamp": 2.3773926470653235 - }, - { - "x": 7.983403951662257, - "y": 7.3366726271050196, - "heading": 0.4869682842338856, - "angularVelocity": 0.11749578140218661, - "velocityX": -0.2660532657788933, - "velocityY": 0.08392767283885226, - "timestamp": 2.473340382328618 - }, - { - "x": 7.922922818311127, - "y": 7.336394611278309, - "heading": 0.4951243670003803, - "angularVelocity": 0.08500547453374917, - "velocityX": -0.6303549863387643, - "velocityY": -0.0028975757056328576, - "timestamp": 2.5692881175919124 - }, - { - "x": 7.827487765779536, - "y": 7.327785918399699, - "heading": 0.5001613028102507, - "angularVelocity": 0.052496661813313965, - "velocityX": -0.9946566458260192, - "velocityY": -0.08972273139104701, - "timestamp": 2.665235852855207 - }, - { - "x": 7.6970987994751, - "y": 7.310846552031537, - "heading": 0.5020779070051818, - "angularVelocity": 0.019975502180138435, - "velocityX": -1.3589582489532488, - "velocityY": -0.17654784994847797, - "timestamp": 2.7611835881185014 - }, - { - "x": 7.531755924357651, - "y": 7.285576510321804, - "heading": 0.500873602063457, - "angularVelocity": -0.012551676581213792, - "velocityX": -1.7232598003874058, - "velocityY": -0.26337298780827745, - "timestamp": 2.857131323381796 - }, - { - "x": 7.331459145051584, - "y": 7.25197578585626, - "heading": 0.4965484153181755, - "angularVelocity": -0.045078570467686534, - "velocityX": -2.0875613036245544, - "velocityY": -0.3501982029418286, - "timestamp": 2.9530790586450903 - }, - { - "x": 7.096208466110642, - "y": 7.210044365383248, - "heading": 0.4891029581030025, - "angularVelocity": -0.07759909282633375, - "velocityX": -2.4518627594010316, - "velocityY": -0.43702355618864164, - "timestamp": 3.049026793908385 - }, - { - "x": 6.826003892546235, - "y": 7.159782229296342, - "heading": 0.47853838599717985, - "angularVelocity": -0.1101075713442515, - "velocityX": -2.8161641629468988, - "velocityY": -0.5238491137803141, - "timestamp": 3.1449745291716793 - }, - { - "x": 6.520845430952262, - "y": 7.101189350526738, - "heading": 0.4648563396258396, - "angularVelocity": -0.14259895070784834, - "velocityX": -3.1804654977688984, - "velocityY": -0.6106749534922914, - "timestamp": 3.2409222644349738 - }, - { - "x": 6.180733092557776, - "y": 7.034265691442456, - "heading": 0.448058865196963, - "angularVelocity": -0.17506900379442902, - "velocityX": -3.5447667155578824, - "velocityY": -0.6975011854176053, - "timestamp": 3.3368699996982683 - }, - { - "x": 5.805666907523616, - "y": 6.959011188942715, - "heading": 0.428148311466242, - "angularVelocity": -0.20751457734863166, - "velocityX": -3.909067619000319, - "velocityY": -0.7843280750007373, - "timestamp": 3.4328177349615627 - }, - { - "x": 5.456128243723212, - "y": 6.875701277677367, - "heading": 0.3971165613376697, - "angularVelocity": -0.3234234767857393, - "velocityX": -3.6430110918326317, - "velocityY": -0.868284290783268, - "timestamp": 3.528765470224857 - }, - { - "x": 5.141543451324252, - "y": 6.800722173314044, - "heading": 0.3691918402141841, - "angularVelocity": -0.2910409614865449, - "velocityX": -3.27870993031459, - "velocityY": -0.7814577817555671, - "timestamp": 3.6247132054881517 - }, - { - "x": 4.861912536840719, - "y": 6.7340739575810815, - "heading": 0.3443722286575812, - "angularVelocity": -0.2586784512265385, - "velocityX": -2.914408700905603, - "velocityY": -0.6946304209273098, - "timestamp": 3.720660940751446 - }, - { - "x": 4.61723549857855, - "y": 6.67575668123621, - "heading": 0.3226563957175653, - "angularVelocity": -0.2263298125841594, - "velocityX": -2.550107489152699, - "velocityY": -0.6078025310846741, - "timestamp": 3.8166086760147406 - }, - { - "x": 4.40751233283897, - "y": 6.6257703832205594, - "heading": 0.30404347575272556, - "angularVelocity": -0.19399019595161143, - "velocityX": -2.185806315949698, - "velocityY": -0.5209742353843284, - "timestamp": 3.912556411278035 - }, - { - "x": 4.232743035438881, - "y": 6.584115094460369, - "heading": 0.288532951546725, - "angularVelocity": -0.1616559699240165, - "velocityX": -1.821505186344387, - "velocityY": -0.4341456173601488, - "timestamp": 4.00850414654133 - }, - { - "x": 4.092927602397226, - "y": 6.550790839135286, - "heading": 0.27612455451484763, - "angularVelocity": -0.12932454317787676, - "velocityX": -1.4572040982309902, - "velocityY": -0.34731674732746287, - "timestamp": 4.104451881804624 - }, - { - "x": 3.9880660303380817, - "y": 6.525797635232698, - "heading": 0.26681818412976743, - "angularVelocity": -0.09699416416179282, - "velocityX": -1.0929030453026178, - "velocityY": -0.260487689824076, - "timestamp": 4.2003996170679185 - }, - { - "x": 3.918158316749324, - "y": 6.5091354948352524, - "heading": 0.26061384732395215, - "angularVelocity": -0.06466371289316737, - "velocityX": -0.7286020185564593, - "velocityY": -0.1736585063912411, - "timestamp": 4.296347352331213 + "angularVelocity": 0.12145264717068183, + "velocityX": 0.1479514529335022, + "velocityY": 0.22680355887981873, + "timestamp": 2.3083046039945705 + }, + { + "x": 7.9838028545378235, + "y": 7.340994975747599, + "heading": 0.4825391441548679, + "angularVelocity": 0.07100818067139564, + "velocityX": -0.26070037689812714, + "velocityY": 0.12838796741348574, + "timestamp": 2.4046922875656627 + }, + { + "x": 7.919285560188609, + "y": 7.343883953710174, + "heading": 0.4845188810802152, + "angularVelocity": 0.02053931427751041, + "velocityX": -0.6693520578449088, + "velocityY": 0.029972480461632738, + "timestamp": 2.5010799711367557 + }, + { + "x": 7.815379289936097, + "y": 7.337286883355744, + "heading": 0.4816330205236378, + "angularVelocity": -0.029940138092932247, + "velocityX": -1.0780036038097405, + "velocityY": -0.06844308432376209, + "timestamp": 2.597467654707848 + }, + { + "x": 7.6720840556149295, + "y": 7.321203739342901, + "heading": 0.47388185121605153, + "angularVelocity": -0.08041659494667612, + "velocityX": -1.4866550269929244, + "velocityY": -0.16685891202042144, + "timestamp": 2.693855338278941 + }, + { + "x": 7.489399868443907, + "y": 7.295634478019173, + "heading": 0.46126687890088164, + "angularVelocity": -0.13087743005945188, + "velocityX": -1.895306333783606, + "velocityY": -0.2652751926014197, + "timestamp": 2.790243021850033 + }, + { + "x": 7.267326740077932, + "y": 7.2605790366684495, + "heading": 0.44379066346613116, + "angularVelocity": -0.1813117069242423, + "velocityX": -2.3039575196573705, + "velocityY": -0.36369212384761535, + "timestamp": 2.886630705421126 + }, + { + "x": 7.00586468444221, + "y": 7.216037332217007, + "heading": 0.4214565844390286, + "angularVelocity": -0.23171092197300747, + "velocityX": -2.7126085610603474, + "velocityY": -0.4621099169645314, + "timestamp": 2.9830183889922193 + }, + { + "x": 6.7050137215236445, + "y": 7.162009258221821, + "heading": 0.3942685380548319, + "angularVelocity": -0.2820697144790415, + "velocityX": -3.1212593951037992, + "velocityY": -0.5605288144032944, + "timestamp": 3.0794060725633123 + }, + { + "x": 6.364773891396992, + "y": 7.098494671595923, + "heading": 0.3622306012435557, + "angularVelocity": -0.3323862097758952, + "velocityX": -3.529909813380894, + "velocityY": -0.658949196336373, + "timestamp": 3.1757937561344054 + }, + { + "x": 6.010374312951181, + "y": 7.013136980735367, + "heading": 0.36223059672201996, + "angularVelocity": -4.690989126354215e-8, + "velocityX": -3.6768139384158545, + "velocityY": -0.8855663680059631, + "timestamp": 3.2721814397054985 + }, + { + "x": 5.655974859532265, + "y": 6.927778770772817, + "heading": 0.36223059220056536, + "angularVelocity": -4.690904952415849e-8, + "velocityX": -3.676812641290635, + "velocityY": -0.885571753569461, + "timestamp": 3.3685691232765915 + }, + { + "x": 5.301574928644714, + "y": 6.842422689744232, + "heading": 0.3622305506125328, + "angularVelocity": -4.314662515297724e-7, + "velocityX": -3.6768175949176816, + "velocityY": -0.8855496663702899, + "timestamp": 3.4649568068476846 + }, + { + "x": 4.986369114322999, + "y": 6.766504438890574, + "heading": 0.34532942408176365, + "angularVelocity": -0.1753452921016411, + "velocityX": -3.270187669664538, + "velocityY": -0.7876343536948325, + "timestamp": 3.5613444904187777 + }, + { + "x": 4.710566832932512, + "y": 6.70007668327783, + "heading": 0.3264669766789321, + "angularVelocity": -0.1956935440711402, + "velocityX": -2.861385097890258, + "velocityY": -0.6891726530988762, + "timestamp": 3.6577321739898707 + }, + { + "x": 4.474170501345758, + "y": 6.643140019477641, + "heading": 0.30840376738356945, + "angularVelocity": -0.18740163292794487, + "velocityX": -2.4525574516208333, + "velocityY": -0.5907047632096585, + "timestamp": 3.754119857560964 + }, + { + "x": 4.277177710352041, + "y": 6.5956938222588235, + "heading": 0.2923060351860776, + "angularVelocity": -0.16701026107365813, + "velocityX": -2.0437548003570405, + "velocityY": -0.4922433599498598, + "timestamp": 3.850507541132057 + }, + { + "x": 4.1195862527073634, + "y": 6.557737539015946, + "heading": 0.27881764120910085, + "angularVelocity": -0.1399389784798502, + "velocityX": -1.6349750487409798, + "velocityY": -0.3937876898440308, + "timestamp": 3.94689522470315 + }, + { + "x": 4.0013944038572795, + "y": 6.529270748726535, + "heading": 0.26834692670698207, + "angularVelocity": -0.10863124949357753, + "velocityX": -1.226213188979785, + "velocityY": -0.2953363877493204, + "timestamp": 4.043282908274243 + }, + { + "x": 3.9226008250519366, + "y": 6.510293128915594, + "heading": 0.26117603215990287, + "angularVelocity": -0.07439637805789222, + "velocityX": -0.817465218439728, + "velocityY": -0.19688843125838204, + "timestamp": 4.139670591845336 }, { "x": 3.883204460144043, "y": 6.500804424285889, "heading": 0.2575116182, - "angularVelocity": -0.032332489302005434, - "velocityX": -0.36430100730739107, - "velocityY": -0.08682925685012162, - "timestamp": 4.3922950875945075 + "angularVelocity": -0.038017450198398375, + "velocityX": -0.40872820518438957, + "velocityY": -0.09844312341738799, + "timestamp": 4.236058275416429 }, { "x": 3.883204460144043, "y": 6.500804424285889, "heading": 0.2575116182, "angularVelocity": 0, - "velocityX": -7.785191054784601e-32, - "velocityY": 5.489773194245446e-31, - "timestamp": 4.488242822857802 + "velocityX": 0, + "velocityY": -1.0638409913121086e-32, + "timestamp": 4.332445958987522 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBDA.3.traj b/src/main/deploy/choreo/CenterLanePBDA.3.traj index 3c4e5016..78de849d 100644 --- a/src/main/deploy/choreo/CenterLanePBDA.3.traj +++ b/src/main/deploy/choreo/CenterLanePBDA.3.traj @@ -5,279 +5,270 @@ "y": 6.500804424285889, "heading": 0.2575116182, "angularVelocity": 0, - "velocityX": -7.785191054784601e-32, - "velocityY": 5.489773194245446e-31, + "velocityX": 0, + "velocityY": -1.0638409913121086e-32, "timestamp": 0 }, { - "x": 3.8559784848389835, - "y": 6.488638862424018, - "heading": 0.25803498288661586, - "angularVelocity": 0.005988366435265192, - "velocityX": -0.3115210499554562, - "velocityY": -0.13919900249831443, - "timestamp": 0.08739690402607536 + "x": 3.856532695000848, + "y": 6.488812456981272, + "heading": 0.25799867286334044, + "angularVelocity": 0.005949010116156059, + "velocityX": -0.32577575495196986, + "velocityY": -0.14647295299154703, + "timestamp": 0.08187154733822144 }, { - "x": 3.801482560046824, - "y": 6.464406720640634, - "heading": 0.2591128207382253, - "angularVelocity": 0.012332677726064934, - "velocityX": -0.6235452548284794, - "velocityY": -0.27726544839797734, - "timestamp": 0.17479380805215072 + "x": 3.8031475649180457, + "y": 6.464921556304023, + "heading": 0.25900179360543873, + "angularVelocity": 0.01225237307357005, + "velocityX": -0.6520596204474033, + "velocityY": -0.29180956576467965, + "timestamp": 0.16374309467644288 }, { - "x": 3.719658287933927, - "y": 6.428240975751334, - "heading": 0.2607868123006951, - "angularVelocity": 0.01915390002797383, - "velocityX": -0.9362376508037902, - "velocityY": -0.4138103665378143, - "timestamp": 0.2621907120782261 + "x": 3.7229939182765985, + "y": 6.429256431279799, + "heading": 0.2605597691266567, + "angularVelocity": 0.019029511128961094, + "velocityX": -0.9790171219107733, + "velocityY": -0.43562295063126366, + "timestamp": 0.24561464201466432 }, { - "x": 3.6104243848456496, - "y": 6.380329724667359, - "heading": 0.2631157133611349, - "angularVelocity": 0.026647409154732543, - "velocityX": -1.249860098656203, - "velocityY": -0.5482030698671013, - "timestamp": 0.34958761610430145 + "x": 3.6159951636151835, + "y": 6.38199294529669, + "heading": 0.26272711898900253, + "angularVelocity": 0.02647256504622092, + "velocityX": -1.3069101310543174, + "velocityY": -0.5772882951369854, + "timestamp": 0.32748618935288576 }, { - "x": 3.473660034873262, - "y": 6.320959355583345, - "heading": 0.26618855611424136, - "angularVelocity": 0.035159629363869176, - "velocityX": -1.564864928528632, - "velocityY": -0.6793189043206871, - "timestamp": 0.4369845201303768 + "x": 3.4820378233980955, + "y": 6.3233976602765205, + "heading": 0.2655861022623468, + "angularVelocity": 0.03492035226271501, + "velocityX": -1.6361891838161302, + "velocityY": -0.7156977842143984, + "timestamp": 0.4093577366911072 }, { - "x": 3.309167154695596, - "y": 6.250618578205828, - "heading": 0.2701561408711645, - "angularVelocity": 0.0453973147119656, - "velocityX": -1.8821362382423572, - "velocityY": -0.8048428964546626, - "timestamp": 0.5243814241564522 + "x": 3.3209367433633594, + "y": 6.253922161955361, + "heading": 0.26927508652336807, + "angularVelocity": 0.04505819641812337, + "velocityX": -1.9677297580465591, + "velocityY": -0.8485914897167804, + "timestamp": 0.49122928402932864 }, { - "x": 3.1165641229075383, - "y": 6.170327957016436, - "heading": 0.2753290776437782, - "angularVelocity": 0.05918901625016675, - "velocityX": -2.203774080264828, - "velocityY": -0.918689535792211, - "timestamp": 0.6117783281825275 + "x": 3.132337992654885, + "y": 6.1744965280297155, + "heading": 0.27407536868463933, + "angularVelocity": 0.0586318729440976, + "velocityX": -2.303593334193982, + "velocityY": -0.9701249885693223, + "timestamp": 0.5731008313675501 }, { - "x": 2.8948724304190634, - "y": 6.083494017796222, - "heading": 0.2827078223400479, - "angularVelocity": 0.08442798722101476, - "velocityX": -2.536608075067875, - "velocityY": -0.9935585269051026, - "timestamp": 0.6991752322086029 + "x": 2.9153402053678152, + "y": 6.088110219485557, + "heading": 0.2808618012562985, + "angularVelocity": 0.08289122158182176, + "velocityX": -2.650466423831284, + "velocityY": -1.0551444470358684, + "timestamp": 0.6549723787057715 }, { - "x": 2.678270569612192, - "y": 6.025989262552558, - "heading": 0.29748746051655606, - "angularVelocity": 0.16910940200008223, - "velocityX": -2.4783699516661057, - "velocityY": -0.6579724520505627, - "timestamp": 0.7865721362346783 + "x": 2.6980153975230565, + "y": 6.030900368674856, + "heading": 0.29585281339155545, + "angularVelocity": 0.1831040529053057, + "velocityX": -2.6544607364871555, + "velocityY": -0.6987757367569974, + "timestamp": 0.736843926043993 }, { - "x": 2.486014315145345, - "y": 5.985703051488859, - "heading": 0.3134280719543648, - "angularVelocity": 0.18239331948249285, - "velocityX": -2.199806235807695, - "velocityY": -0.46095695851742674, - "timestamp": 0.8739690402607536 + "x": 2.5044902704926533, + "y": 5.990681526275272, + "heading": 0.31201148477172486, + "angularVelocity": 0.1973661412971212, + "velocityX": -2.3637653534374596, + "velocityY": -0.4912432182750236, + "timestamp": 0.8187154733822144 }, { - "x": 2.3193881959437443, - "y": 5.960659974875319, - "heading": 0.3298457414331986, - "angularVelocity": 0.1878518428288434, - "velocityX": -1.9065448720230032, - "velocityY": -0.2865442076308354, - "timestamp": 0.961365944286829 + "x": 2.3360827419423367, + "y": 5.965439222430923, + "heading": 0.328645437097879, + "angularVelocity": 0.20317134422093217, + "velocityX": -2.0569725872482194, + "velocityY": -0.30831594937456525, + "timestamp": 0.9005870207204358 }, { - "x": 2.1788238735004155, - "y": 5.950110033535781, - "heading": 0.3464901333226275, - "angularVelocity": 0.19044601264666053, - "velocityX": -1.6083444145960923, - "velocityY": -0.1207129869999774, - "timestamp": 1.0487628483129043 + "x": 2.1932220965526215, + "y": 5.95442921664246, + "heading": 0.34550838612461254, + "angularVelocity": 0.2059683684378234, + "velocityX": -1.744936428275136, + "velocityY": -0.13447902411053952, + "timestamp": 0.9824585680586573 }, { - "x": 2.064536434770172, - "y": 5.9536596082772615, - "heading": 0.36323174785688694, - "angularVelocity": 0.19155843929282074, - "velocityX": -1.3076829208520429, - "velocityY": 0.04061442199853097, - "timestamp": 1.1361597523389797 + "x": 2.076119719939017, + "y": 5.957265361506873, + "heading": 0.3624743968160351, + "angularVelocity": 0.20722718994590164, + "velocityX": -1.4303183513784072, + "velocityY": 0.03464139810008477, + "timestamp": 1.0643301153968787 }, { - "x": 1.9766544759033122, - "y": 5.971066392483775, - "heading": 0.3799915015950661, - "angularVelocity": 0.19176598902380804, - "velocityX": -1.005550023152301, - "velocityY": 0.19916934587659238, - "timestamp": 1.223556656365055 + "x": 1.984901205969126, + "y": 5.973711453340982, + "heading": 0.37946704953669896, + "angularVelocity": 0.20755260249895005, + "velocityX": -1.1141662388895142, + "velocityY": 0.20087676816670857, + "timestamp": 1.1462016627351002 }, { - "x": 1.9152634831181174, - "y": 6.002166252794233, - "heading": 0.3967161004530197, - "angularVelocity": 0.19136374502423678, - "velocityX": -0.7024389876199576, - "velocityY": 0.3558462471528812, - "timestamp": 1.3109535603911304 + "x": 1.9196497213527057, + "y": 6.003608138127345, + "heading": 0.39643505242128974, + "angularVelocity": 0.207251523102328, + "velocityX": -0.7969983069558729, + "velocityY": 0.3651657475442049, + "timestamp": 1.2280732100733216 }, { "x": 1.8804243803024292, "y": 6.046840667724609, - "heading": 0.4133671975206445, - "angularVelocity": 0.19052273365034633, - "velocityX": -0.3986308577394652, - "velocityY": 0.5111670193379727, - "timestamp": 1.3983504644172058 + "heading": 0.41334160220463595, + "angularVelocity": 0.20650091922048514, + "velocityX": -0.4791083389230676, + "velocityY": 0.5280531638014023, + "timestamp": 1.309944757411543 }, { - "x": 1.870594888269871, - "y": 6.097873553344943, - "heading": 0.42824707526951206, - "angularVelocity": 0.18946968270504025, - "velocityX": -0.12516169608330563, - "velocityY": 0.6498161348632773, - "timestamp": 1.4768848111075528 + "x": 1.8671378435769306, + "y": 6.1025986442210955, + "heading": 0.4299850055686104, + "angularVelocity": 0.20542336557580967, + "velocityX": -0.16399080352197462, + "velocityY": 0.6882000597544763, + "timestamp": 1.3909647685350217 }, { - "x": 1.8823139330546126, - "y": 6.159652306802076, - "heading": 0.44299770211925266, - "angularVelocity": 0.18782389452987927, - "velocityX": 0.1492219045374961, - "velocityY": 0.7866463026772258, - "timestamp": 1.5554191577978997 + "x": 1.8794728316218305, + "y": 6.171151657888672, + "heading": 0.4464831819952248, + "angularVelocity": 0.20363088325759873, + "velocityX": 0.1522461904639937, + "velocityY": 0.8461244662518951, + "timestamp": 1.4719847796585004 }, { - "x": 1.9156816491401238, - "y": 6.231973676949003, - "heading": 0.45755311107567953, - "angularVelocity": 0.18533813000084504, - "velocityX": 0.4248805457958017, - "velocityY": 0.9208884162757981, - "timestamp": 1.6339535044882467 + "x": 1.9175617070085949, + "y": 6.252231071793212, + "heading": 0.46275009292634184, + "angularVelocity": 0.2007764588716919, + "velocityX": 0.4701168866629144, + "velocityY": 1.0007331865330475, + "timestamp": 1.553004790781979 }, { - "x": 1.970847190876043, - "y": 6.314525559081429, - "heading": 0.47181258180189295, - "angularVelocity": 0.18156986499724873, - "velocityX": 0.7024384114816875, - "velocityY": 1.0511564125938428, - "timestamp": 1.7124878511785937 + "x": 1.9816154560716552, + "y": 6.34539325484771, + "heading": 0.47864458300524776, + "angularVelocity": 0.19617980618000597, + "velocityX": 0.7905917090709768, + "velocityY": 1.1498663325596743, + "timestamp": 1.6340248019054577 }, { - "x": 2.048055657917003, - "y": 6.406768140751847, - "heading": 0.4856034696411581, - "angularVelocity": 0.17560326685649172, - "velocityX": 0.9831172002408801, - "velocityY": 1.1745508246744762, - "timestamp": 1.7910221978689407 + "x": 2.0720214338634078, + "y": 6.449767500665198, + "heading": 0.4938927703980073, + "angularVelocity": 0.18820273141558316, + "velocityX": 1.115847511474288, + "velocityY": 1.2882526720271141, + "timestamp": 1.7150448130289364 }, { - "x": 2.1477789670097485, - "y": 6.507546080988, - "heading": 0.4985623758309784, - "angularVelocity": 0.16500940971618508, - "velocityX": 1.2698050381184776, - "velocityY": 1.2832339541004922, - "timestamp": 1.8695565445592877 + "x": 2.1896944819931505, + "y": 6.562891621237937, + "heading": 0.5077401370806992, + "angularVelocity": 0.17091292003882794, + "velocityX": 1.4523948651451515, + "velocityY": 1.3962491365291643, + "timestamp": 1.796064824152415 }, { - "x": 2.2711613032626046, - "y": 6.612773705527785, - "heading": 0.5094584510742339, - "angularVelocity": 0.13874280111118417, - "velocityX": 1.5710621078868952, - "velocityY": 1.3398930401075082, - "timestamp": 1.9480908912496346 + "x": 2.3309802432224416, + "y": 6.659921406601519, + "heading": 0.5136983140040886, + "angularVelocity": 0.07353957177701248, + "velocityX": 1.7438378404313606, + "velocityY": 1.1976027158982174, + "timestamp": 1.8770848352758938 }, { - "x": 2.3900797492067456, - "y": 6.694381968039411, - "heading": 0.5145158846551874, - "angularVelocity": 0.06439772907126085, - "velocityX": 1.5142221328080179, - "velocityY": 1.0391410376583359, - "timestamp": 2.0266252379399816 + "x": 2.4512963274996546, + "y": 6.737453803425126, + "heading": 0.5174619765612319, + "angularVelocity": 0.04645349346357551, + "velocityX": 1.4850168817410538, + "velocityY": 0.9569536679703065, + "timestamp": 1.9581048463993724 }, { - "x": 2.4908787774979912, - "y": 6.760136892165139, - "heading": 0.5179138832466764, - "angularVelocity": 0.04326767503251818, - "velocityX": 1.2835024742573689, - "velocityY": 0.8372760059365338, - "timestamp": 2.1051595846303286 + "x": 2.5483430242698013, + "y": 6.798294060832995, + "heading": 0.5200641230005525, + "angularVelocity": 0.03211733006744314, + "velocityX": 1.197811447128081, + "velocityY": 0.7509287713518723, + "timestamp": 2.039124857522851 }, { - "x": 2.572148554688276, - "y": 6.811807824685022, - "heading": 0.5203056131504431, - "angularVelocity": 0.030454571847360265, - "velocityX": 1.0348310085359687, - "velocityY": 0.6579405661018212, - "timestamp": 2.1836939313206756 + "x": 2.62147124688145, + "y": 6.843385043467462, + "heading": 0.5218324598046723, + "angularVelocity": 0.02182592645444906, + "velocityX": 0.9025945762979182, + "velocityY": 0.556541303922395, + "timestamp": 2.12014486864633 }, { - "x": 2.6333915518956847, - "y": 6.850110641517779, - "heading": 0.5219437860517318, - "angularVelocity": 0.02085931786951444, - "velocityX": 0.7798243671508998, - "velocityY": 0.48772057637126504, - "timestamp": 2.2622282780110226 - }, - { - "x": 2.6743555300979214, - "y": 6.8754307237892345, - "heading": 0.5229620854603486, - "angularVelocity": 0.012966293749560319, - "velocityX": 0.5216058951092302, - "velocityY": 0.3224077532762846, - "timestamp": 2.3407626247013686 + "x": 2.6703780246763387, + "y": 6.8731963037812145, + "heading": 0.5229271765337971, + "angularVelocity": 0.013511683273610298, + "velocityX": 0.6036382508063606, + "velocityY": 0.367949348566719, + "timestamp": 2.2011648797698085 }, { "x": 2.6948883533477783, "y": 6.8880085945129395, "heading": 0.5234431642, - "angularVelocity": 0.006125711359747557, - "velocityX": 0.26145023311667365, - "velocityY": 0.16015757759211213, - "timestamp": 2.4192969713917156 + "angularVelocity": 0.006368644721814939, + "velocityX": 0.30252191195191025, + "velocityY": 0.18282262031723026, + "timestamp": 2.282184890893287 }, { "x": 2.6948883533477783, "y": 6.8880085945129395, "heading": 0.5234431642, - "angularVelocity": 1.0302971803870337e-32, - "velocityX": -3.572244982752581e-35, - "velocityY": 2.604330852389062e-35, - "timestamp": 2.4978313180820626 + "angularVelocity": -2.018007055520513e-33, + "velocityX": -1.0144673376047062e-35, + "velocityY": 0, + "timestamp": 2.363204902016766 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBDA.traj b/src/main/deploy/choreo/CenterLanePBDA.traj new file mode 100644 index 00000000..cb20734f --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePBDA.traj @@ -0,0 +1,886 @@ +{ + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": 2.2921355473422064e-38, + "velocityX": 1.1360383232068741e-38, + "velocityY": -2.144062321628808e-38, + "timestamp": 0 + }, + { + "x": 1.32825806778149, + "y": 5.589895062873393, + "heading": -2.8062774074031584e-19, + "angularVelocity": -2.995198561755265e-18, + "velocityX": 0.40860904707084805, + "velocityY": -0.011305499292899219, + "timestamp": 0.09369253321938388 + }, + { + "x": 1.4048253002214361, + "y": 5.587776581164742, + "heading": -1.768868342127426e-19, + "angularVelocity": 1.1072483896415275e-18, + "velocityX": 0.8172180835441991, + "velocityY": -0.022610998292584186, + "timestamp": 0.18738506643876776 + }, + { + "x": 1.519676146824621, + "y": 5.584598858658671, + "heading": -2.386873521248768e-19, + "angularVelocity": -6.596098514057519e-19, + "velocityX": 1.225827103364341, + "velocityY": -0.03391649683150392, + "timestamp": 0.28107759965815166 + }, + { + "x": 1.6728106047825377, + "y": 5.580361895432887, + "heading": -7.795008789281329e-19, + "angularVelocity": -5.772215866345043e-18, + "velocityX": 1.6344360932087083, + "velocityY": -0.04522199454104627, + "timestamp": 0.3747701328775355 + }, + { + "x": 1.8642286675420066, + "y": 5.575065691668706, + "heading": -1.0638718286937245e-18, + "angularVelocity": -3.0351506143991226e-18, + "velocityX": 2.0430450131096105, + "velocityY": -0.056527490315375004, + "timestamp": 0.4684626660969194 + }, + { + "x": 2.0939303023371445, + "y": 5.5687102482727004, + "heading": -1.0113200205360793e-18, + "angularVelocity": 5.608964381023318e-19, + "velocityX": 2.451653583293393, + "velocityY": -0.0678329764136413, + "timestamp": 0.5621551993163033 + }, + { + "x": 2.285348365096613, + "y": 5.563414044508519, + "heading": -7.732946400687193e-19, + "angularVelocity": 2.5404946615264457e-18, + "velocityX": 2.04304501310961, + "velocityY": -0.056527490315375004, + "timestamp": 0.6558477325356872 + }, + { + "x": 2.43848282305453, + "y": 5.559177081282735, + "heading": -4.312009437616493e-19, + "angularVelocity": 3.651237559230762e-18, + "velocityX": 1.634436093208708, + "velocityY": -0.04522199454104628, + "timestamp": 0.749540265755071 + }, + { + "x": 2.553333669657715, + "y": 5.555999358776664, + "heading": -7.015602955490743e-20, + "angularVelocity": 3.853507870950435e-18, + "velocityX": 1.2258271033643409, + "velocityY": -0.033916496831503926, + "timestamp": 0.8432327989744549 + }, + { + "x": 2.6299009020976607, + "y": 5.553880877068013, + "heading": -2.7026882274997263e-20, + "angularVelocity": 4.603264080766157e-19, + "velocityX": 0.817218083544199, + "velocityY": -0.022610998292584186, + "timestamp": 0.9369253321938388 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": 0, + "angularVelocity": 2.884635663770645e-19, + "velocityX": 0.40860904707084805, + "velocityY": -0.011305499292899219, + "timestamp": 1.0306178654132228 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -1.1410946614987667e-37, + "timestamp": 1.1243103986326066 + }, + { + "x": 2.687410064097537, + "y": 5.560258515634833, + "heading": 0.0028176822125938034, + "angularVelocity": 0.04098002734374148, + "velocityX": 0.2796139919160097, + "velocityY": 0.1081610698436357, + "timestamp": 1.1930678494014684 + }, + { + "x": 2.7258611531960137, + "y": 5.575132263104083, + "heading": 0.008453116109374614, + "angularVelocity": 0.08196106507388369, + "velocityX": 0.5592279624754555, + "velocityY": 0.2163219738796251, + "timestamp": 1.2618253001703301 + }, + { + "x": 2.783537789303066, + "y": 5.597442863001631, + "heading": 0.016905769734324238, + "angularVelocity": 0.12293436610040698, + "velocityX": 0.8388419794814749, + "velocityY": 0.3244826509427688, + "timestamp": 1.3305827509391919 + }, + { + "x": 2.8604399802132514, + "y": 5.627190295278175, + "heading": 0.028174540960364546, + "angularVelocity": 0.1638916379247738, + "velocityX": 1.118456109850604, + "velocityY": 0.4326430364113507, + "timestamp": 1.3993402017080536 + }, + { + "x": 2.9565677382168056, + "y": 5.664374535134068, + "heading": 0.042257801458542396, + "angularVelocity": 0.20482522753092394, + "velocityX": 1.3980704189674162, + "velocityY": 0.5408030611968617, + "timestamp": 1.4680976524769154 + }, + { + "x": 3.0719210799387406, + "y": 5.708995552649304, + "heading": 0.05915345386800317, + "angularVelocity": 0.2457283133759373, + "velocityX": 1.6776849698763825, + "velocityY": 0.648962650829455, + "timestamp": 1.5368551032457771 + }, + { + "x": 3.2065000261043313, + "y": 5.761053312357379, + "heading": 0.07885900262196403, + "angularVelocity": 0.2865951040012339, + "velocityX": 1.9572998222112317, + "velocityY": 0.757121724641519, + "timestamp": 1.6056125540146389 + }, + { + "x": 3.360304601198153, + "y": 5.820547772766793, + "heading": 0.1013716386695389, + "angularVelocity": 0.32742104013214957, + "velocityX": 2.2369150306467707, + "velocityY": 0.8652801950062525, + "timestamp": 1.6743700047835006 + }, + { + "x": 3.5333348329448953, + "y": 5.8874788858214675, + "heading": 0.12668833767209217, + "angularVelocity": 0.36820299064984424, + "velocityX": 2.5165306423068015, + "velocityY": 0.973437966449241, + "timestamp": 1.7431274555523624 + }, + { + "x": 3.7255907514118967, + "y": 5.961846596243587, + "heading": 0.15480596913918515, + "angularVelocity": 0.4089394116954055, + "velocityX": 2.796146691262561, + "velocityY": 1.0815949339384094, + "timestamp": 1.8118849063212241 + }, + { + "x": 3.937072386955544, + "y": 6.04365084047756, + "heading": 0.18572140510730675, + "angularVelocity": 0.44963034002016317, + "velocityX": 3.075763181717921, + "velocityY": 1.1897509770827353, + "timestamp": 1.8806423570900859 + }, + { + "x": 4.167779761570079, + "y": 6.132891543122838, + "heading": 0.21943155266112502, + "angularVelocity": 0.4902762853605699, + "velocityX": 3.355379992054849, + "velocityY": 1.2979059236107295, + "timestamp": 1.9493998078589476 + }, + { + "x": 4.410308649457132, + "y": 6.2266961328710915, + "heading": 0.21943155790785696, + "angularVelocity": 7.630783127279499e-8, + "velocityX": 3.5273106430654275, + "velocityY": 1.3642825424635276, + "timestamp": 2.0181572586278094 + }, + { + "x": 4.652837676050608, + "y": 6.320500363997887, + "heading": 0.21943156315423873, + "angularVelocity": 7.630273806741925e-8, + "velocityX": 3.5273126603947684, + "velocityY": 1.3642773267166644, + "timestamp": 2.0869147093966713 + }, + { + "x": 4.897946357727051, + "y": 6.407341003417969, + "heading": 0.21943156841217684, + "angularVelocity": 7.647081243446701e-8, + "velocityX": 3.5648308501199604, + "velocityY": 1.2629996960185401, + "timestamp": 2.1556721601655333 + }, + { + "x": 5.1308682473529785, + "y": 6.471009903235158, + "heading": 0.21943157303192085, + "angularVelocity": 7.23563158909772e-8, + "velocityX": 3.6481177055177683, + "velocityY": 0.9972082962532426, + "timestamp": 2.2195193022886155 + }, + { + "x": 5.365529567427774, + "y": 6.527934417793533, + "heading": 0.21943157763716262, + "angularVelocity": 7.21291764013049e-8, + "velocityX": 3.6753613751798433, + "velocityY": 0.8915749815181846, + "timestamp": 2.2833664444116977 + }, + { + "x": 5.600190991292763, + "y": 6.584858504493345, + "heading": 0.21943158224240422, + "angularVelocity": 7.21291734995501e-8, + "velocityX": 3.6753630007842206, + "velocityY": 0.8915682802227483, + "timestamp": 2.34721358653478 + }, + { + "x": 5.8348524151632635, + "y": 6.641782591170481, + "heading": 0.2194315868476458, + "angularVelocity": 7.212917336838375e-8, + "velocityX": 3.6753630008705414, + "velocityY": 0.8915682798675781, + "timestamp": 2.411060728657862 + }, + { + "x": 6.069513839029734, + "y": 6.698706677864191, + "heading": 0.21943159145288735, + "angularVelocity": 7.212917280605725e-8, + "velocityX": 3.6753630008074327, + "velocityY": 0.891568280127171, + "timestamp": 2.474907870780944 + }, + { + "x": 6.3041751870414915, + "y": 6.75563107725722, + "heading": 0.21943159605812906, + "angularVelocity": 7.212917561549481e-8, + "velocityX": 3.6753618127399528, + "velocityY": 0.8915731777514998, + "timestamp": 2.5387550129040264 + }, + { + "x": 6.537489271202827, + "y": 6.817847413489264, + "heading": 0.21943160514124108, + "angularVelocity": 1.4226340838423697e-7, + "velocityX": 3.6542604164106804, + "velocityY": 0.9744576524998471, + "timestamp": 2.6026021550271086 + }, + { + "x": 6.7545327794045615, + "y": 6.881509207420976, + "heading": 0.24890077922126197, + "angularVelocity": 0.46155823268064305, + "velocityX": 3.399424014677561, + "velocityY": 0.9970970009742466, + "timestamp": 2.6664492971501907 + }, + { + "x": 6.954212307545749, + "y": 6.941327014961267, + "heading": 0.2778274290995393, + "angularVelocity": 0.45306099719431336, + "velocityX": 3.1274622716276532, + "velocityY": 0.9368909171372027, + "timestamp": 2.730296439273273 + }, + { + "x": 7.1365689728963595, + "y": 6.997132012930522, + "heading": 0.305376589845945, + "angularVelocity": 0.43148620017020184, + "velocityX": 2.856144524042602, + "velocityY": 0.8740406557535503, + "timestamp": 2.794143581396355 + }, + { + "x": 7.301617943292834, + "y": 7.048866027505986, + "heading": 0.33125825392103403, + "angularVelocity": 0.40536918669284566, + "velocityX": 2.585064341302851, + "velocityY": 0.8102792522134878, + "timestamp": 2.8579907235194373 + }, + { + "x": 7.449367094221676, + "y": 7.096499619737108, + "heading": 0.35532461486989275, + "angularVelocity": 0.37693716818945566, + "velocityX": 2.3141075076472033, + "velocityY": 0.7460567638139751, + "timestamp": 2.9218378656425195 + }, + { + "x": 7.579821246213137, + "y": 7.140015020198173, + "heading": 0.3774857294479878, + "angularVelocity": 0.347096421878647, + "velocityX": 2.04322617510391, + "velocityY": 0.681555963416076, + "timestamp": 2.9856850077656016 + }, + { + "x": 7.69298365301254, + "y": 7.179400342359022, + "heading": 0.3976808814927073, + "angularVelocity": 0.3163047142468991, + "velocityX": 1.7723958040479335, + "velocityY": 0.6168689913312067, + "timestamp": 3.049532149888684 + }, + { + "x": 7.788856658439436, + "y": 7.214647078189013, + "heading": 0.41586620005922853, + "angularVelocity": 0.2848258819708253, + "velocityX": 1.5016021428504343, + "velocityY": 0.5520487629977263, + "timestamp": 3.113379292011766 + }, + { + "x": 7.867442031134741, + "y": 7.2457488378850705, + "heading": 0.43200844627235874, + "angularVelocity": 0.2528264488645923, + "velocityX": 1.2308361828288252, + "velocityY": 0.48712845496042256, + "timestamp": 3.177226434134848 + }, + { + "x": 7.928741153018892, + "y": 7.27270064652237, + "heading": 0.4460815614823235, + "angularVelocity": 0.22041887454940703, + "velocityX": 0.9600918670092122, + "velocityY": 0.4221302276198955, + "timestamp": 3.2410735762579304 + }, + { + "x": 7.9727551334716695, + "y": 7.29549852060884, + "heading": 0.45806460123681797, + "angularVelocity": 0.1876832596734667, + "velocityX": 0.6893649267484526, + "velocityY": 0.35706960920065395, + "timestamp": 3.3049207183810125 + }, + { + "x": 7.999484882530383, + "y": 7.314139197912392, + "heading": 0.4679404256802237, + "angularVelocity": 0.1546791933829663, + "velocityX": 0.4186522398635212, + "velocityY": 0.2919578963709263, + "timestamp": 3.3687678605040947 + }, + { + "x": 8.008931159973145, + "y": 7.328619956970215, + "heading": 0.4756948301053548, + "angularVelocity": 0.12145264717068183, + "velocityX": 0.1479514529335022, + "velocityY": 0.22680355887981873, + "timestamp": 3.432615002627177 + }, + { + "x": 7.9838028545378235, + "y": 7.340994975747599, + "heading": 0.4825391441548679, + "angularVelocity": 0.07100818067139564, + "velocityX": -0.26070037689812714, + "velocityY": 0.12838796741348574, + "timestamp": 3.5290026861982695 + }, + { + "x": 7.919285560188609, + "y": 7.343883953710174, + "heading": 0.4845188810802152, + "angularVelocity": 0.02053931427751041, + "velocityX": -0.6693520578449088, + "velocityY": 0.029972480461632738, + "timestamp": 3.625390369769362 + }, + { + "x": 7.815379289936097, + "y": 7.337286883355744, + "heading": 0.4816330205236378, + "angularVelocity": -0.029940138092932247, + "velocityX": -1.0780036038097405, + "velocityY": -0.06844308432376209, + "timestamp": 3.7217780533404548 + }, + { + "x": 7.6720840556149295, + "y": 7.321203739342901, + "heading": 0.47388185121605153, + "angularVelocity": -0.08041659494667612, + "velocityX": -1.4866550269929244, + "velocityY": -0.16685891202042144, + "timestamp": 3.8181657369115474 + }, + { + "x": 7.489399868443907, + "y": 7.295634478019173, + "heading": 0.46126687890088164, + "angularVelocity": -0.13087743005945188, + "velocityX": -1.895306333783606, + "velocityY": -0.2652751926014197, + "timestamp": 3.91455342048264 + }, + { + "x": 7.267326740077932, + "y": 7.2605790366684495, + "heading": 0.44379066346613116, + "angularVelocity": -0.1813117069242423, + "velocityX": -2.3039575196573705, + "velocityY": -0.36369212384761535, + "timestamp": 4.010941104053733 + }, + { + "x": 7.00586468444221, + "y": 7.216037332217007, + "heading": 0.4214565844390286, + "angularVelocity": -0.23171092197300747, + "velocityX": -2.7126085610603474, + "velocityY": -0.4621099169645314, + "timestamp": 4.107328787624826 + }, + { + "x": 6.7050137215236445, + "y": 7.162009258221821, + "heading": 0.3942685380548319, + "angularVelocity": -0.2820697144790415, + "velocityX": -3.1212593951037992, + "velocityY": -0.5605288144032944, + "timestamp": 4.203716471195919 + }, + { + "x": 6.364773891396992, + "y": 7.098494671595923, + "heading": 0.3622306012435557, + "angularVelocity": -0.3323862097758952, + "velocityX": -3.529909813380894, + "velocityY": -0.658949196336373, + "timestamp": 4.300104154767012 + }, + { + "x": 6.010374312951181, + "y": 7.013136980735367, + "heading": 0.36223059672201996, + "angularVelocity": -4.690989126354215e-8, + "velocityX": -3.6768139384158545, + "velocityY": -0.8855663680059631, + "timestamp": 4.396491838338105 + }, + { + "x": 5.655974859532265, + "y": 6.927778770772817, + "heading": 0.36223059220056536, + "angularVelocity": -4.690904952415849e-8, + "velocityX": -3.676812641290635, + "velocityY": -0.885571753569461, + "timestamp": 4.492879521909198 + }, + { + "x": 5.301574928644714, + "y": 6.842422689744232, + "heading": 0.3622305506125328, + "angularVelocity": -4.314662515297724e-7, + "velocityX": -3.6768175949176816, + "velocityY": -0.8855496663702899, + "timestamp": 4.589267205480291 + }, + { + "x": 4.986369114322999, + "y": 6.766504438890574, + "heading": 0.34532942408176365, + "angularVelocity": -0.1753452921016411, + "velocityX": -3.270187669664538, + "velocityY": -0.7876343536948325, + "timestamp": 4.685654889051384 + }, + { + "x": 4.710566832932512, + "y": 6.70007668327783, + "heading": 0.3264669766789321, + "angularVelocity": -0.1956935440711402, + "velocityX": -2.861385097890258, + "velocityY": -0.6891726530988762, + "timestamp": 4.782042572622477 + }, + { + "x": 4.474170501345758, + "y": 6.643140019477641, + "heading": 0.30840376738356945, + "angularVelocity": -0.18740163292794487, + "velocityX": -2.4525574516208333, + "velocityY": -0.5907047632096585, + "timestamp": 4.87843025619357 + }, + { + "x": 4.277177710352041, + "y": 6.5956938222588235, + "heading": 0.2923060351860776, + "angularVelocity": -0.16701026107365813, + "velocityX": -2.0437548003570405, + "velocityY": -0.4922433599498598, + "timestamp": 4.974817939764663 + }, + { + "x": 4.1195862527073634, + "y": 6.557737539015946, + "heading": 0.27881764120910085, + "angularVelocity": -0.1399389784798502, + "velocityX": -1.6349750487409798, + "velocityY": -0.3937876898440308, + "timestamp": 5.071205623335756 + }, + { + "x": 4.0013944038572795, + "y": 6.529270748726535, + "heading": 0.26834692670698207, + "angularVelocity": -0.10863124949357753, + "velocityX": -1.226213188979785, + "velocityY": -0.2953363877493204, + "timestamp": 5.167593306906849 + }, + { + "x": 3.9226008250519366, + "y": 6.510293128915594, + "heading": 0.26117603215990287, + "angularVelocity": -0.07439637805789222, + "velocityX": -0.817465218439728, + "velocityY": -0.19688843125838204, + "timestamp": 5.263980990477942 + }, + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": -0.038017450198398375, + "velocityX": -0.40872820518438957, + "velocityY": -0.09844312341738799, + "timestamp": 5.3603686740490355 + }, + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -1.0638409913121086e-32, + "timestamp": 5.456756357620129 + }, + { + "x": 3.856532695000848, + "y": 6.488812456981272, + "heading": 0.25799867286334044, + "angularVelocity": 0.005949010116156059, + "velocityX": -0.32577575495196986, + "velocityY": -0.14647295299154703, + "timestamp": 5.53862790495835 + }, + { + "x": 3.8031475649180457, + "y": 6.464921556304023, + "heading": 0.25900179360543873, + "angularVelocity": 0.01225237307357005, + "velocityX": -0.6520596204474033, + "velocityY": -0.29180956576467965, + "timestamp": 5.620499452296571 + }, + { + "x": 3.7229939182765985, + "y": 6.429256431279799, + "heading": 0.2605597691266567, + "angularVelocity": 0.019029511128961094, + "velocityX": -0.9790171219107733, + "velocityY": -0.43562295063126366, + "timestamp": 5.702370999634793 + }, + { + "x": 3.6159951636151835, + "y": 6.38199294529669, + "heading": 0.26272711898900253, + "angularVelocity": 0.02647256504622092, + "velocityX": -1.3069101310543174, + "velocityY": -0.5772882951369854, + "timestamp": 5.784242546973014 + }, + { + "x": 3.4820378233980955, + "y": 6.3233976602765205, + "heading": 0.2655861022623468, + "angularVelocity": 0.03492035226271501, + "velocityX": -1.6361891838161302, + "velocityY": -0.7156977842143984, + "timestamp": 5.866114094311236 + }, + { + "x": 3.3209367433633594, + "y": 6.253922161955361, + "heading": 0.26927508652336807, + "angularVelocity": 0.04505819641812337, + "velocityX": -1.9677297580465591, + "velocityY": -0.8485914897167804, + "timestamp": 5.947985641649457 + }, + { + "x": 3.132337992654885, + "y": 6.1744965280297155, + "heading": 0.27407536868463933, + "angularVelocity": 0.0586318729440976, + "velocityX": -2.303593334193982, + "velocityY": -0.9701249885693223, + "timestamp": 6.029857188987679 + }, + { + "x": 2.9153402053678152, + "y": 6.088110219485557, + "heading": 0.2808618012562985, + "angularVelocity": 0.08289122158182176, + "velocityX": -2.650466423831284, + "velocityY": -1.0551444470358684, + "timestamp": 6.1117287363259 + }, + { + "x": 2.6980153975230565, + "y": 6.030900368674856, + "heading": 0.29585281339155545, + "angularVelocity": 0.1831040529053057, + "velocityX": -2.6544607364871555, + "velocityY": -0.6987757367569974, + "timestamp": 6.1936002836641215 + }, + { + "x": 2.5044902704926533, + "y": 5.990681526275272, + "heading": 0.31201148477172486, + "angularVelocity": 0.1973661412971212, + "velocityX": -2.3637653534374596, + "velocityY": -0.4912432182750236, + "timestamp": 6.275471831002343 + }, + { + "x": 2.3360827419423367, + "y": 5.965439222430923, + "heading": 0.328645437097879, + "angularVelocity": 0.20317134422093217, + "velocityX": -2.0569725872482194, + "velocityY": -0.30831594937456525, + "timestamp": 6.357343378340564 + }, + { + "x": 2.1932220965526215, + "y": 5.95442921664246, + "heading": 0.34550838612461254, + "angularVelocity": 0.2059683684378234, + "velocityX": -1.744936428275136, + "velocityY": -0.13447902411053952, + "timestamp": 6.439214925678786 + }, + { + "x": 2.076119719939017, + "y": 5.957265361506873, + "heading": 0.3624743968160351, + "angularVelocity": 0.20722718994590164, + "velocityX": -1.4303183513784072, + "velocityY": 0.03464139810008477, + "timestamp": 6.521086473017007 + }, + { + "x": 1.984901205969126, + "y": 5.973711453340982, + "heading": 0.37946704953669896, + "angularVelocity": 0.20755260249895005, + "velocityX": -1.1141662388895142, + "velocityY": 0.20087676816670857, + "timestamp": 6.602958020355229 + }, + { + "x": 1.9196497213527057, + "y": 6.003608138127345, + "heading": 0.39643505242128974, + "angularVelocity": 0.207251523102328, + "velocityX": -0.7969983069558729, + "velocityY": 0.3651657475442049, + "timestamp": 6.68482956769345 + }, + { + "x": 1.8804243803024292, + "y": 6.046840667724609, + "heading": 0.41334160220463595, + "angularVelocity": 0.20650091922048514, + "velocityX": -0.4791083389230676, + "velocityY": 0.5280531638014023, + "timestamp": 6.766701115031672 + }, + { + "x": 1.8671378435769306, + "y": 6.1025986442210955, + "heading": 0.4299850055686104, + "angularVelocity": 0.20542336557580967, + "velocityX": -0.16399080352197462, + "velocityY": 0.6882000597544763, + "timestamp": 6.84772112615515 + }, + { + "x": 1.8794728316218305, + "y": 6.171151657888672, + "heading": 0.4464831819952248, + "angularVelocity": 0.20363088325759873, + "velocityX": 0.1522461904639937, + "velocityY": 0.8461244662518951, + "timestamp": 6.928741137278629 + }, + { + "x": 1.9175617070085949, + "y": 6.252231071793212, + "heading": 0.46275009292634184, + "angularVelocity": 0.2007764588716919, + "velocityX": 0.4701168866629144, + "velocityY": 1.0007331865330475, + "timestamp": 7.009761148402108 + }, + { + "x": 1.9816154560716552, + "y": 6.34539325484771, + "heading": 0.47864458300524776, + "angularVelocity": 0.19617980618000597, + "velocityX": 0.7905917090709768, + "velocityY": 1.1498663325596743, + "timestamp": 7.090781159525586 + }, + { + "x": 2.0720214338634078, + "y": 6.449767500665198, + "heading": 0.4938927703980073, + "angularVelocity": 0.18820273141558316, + "velocityX": 1.115847511474288, + "velocityY": 1.2882526720271141, + "timestamp": 7.171801170649065 + }, + { + "x": 2.1896944819931505, + "y": 6.562891621237937, + "heading": 0.5077401370806992, + "angularVelocity": 0.17091292003882794, + "velocityX": 1.4523948651451515, + "velocityY": 1.3962491365291643, + "timestamp": 7.252821181772544 + }, + { + "x": 2.3309802432224416, + "y": 6.659921406601519, + "heading": 0.5136983140040886, + "angularVelocity": 0.07353957177701248, + "velocityX": 1.7438378404313606, + "velocityY": 1.1976027158982174, + "timestamp": 7.333841192896022 + }, + { + "x": 2.4512963274996546, + "y": 6.737453803425126, + "heading": 0.5174619765612319, + "angularVelocity": 0.04645349346357551, + "velocityX": 1.4850168817410538, + "velocityY": 0.9569536679703065, + "timestamp": 7.414861204019501 + }, + { + "x": 2.5483430242698013, + "y": 6.798294060832995, + "heading": 0.5200641230005525, + "angularVelocity": 0.03211733006744314, + "velocityX": 1.197811447128081, + "velocityY": 0.7509287713518723, + "timestamp": 7.49588121514298 + }, + { + "x": 2.62147124688145, + "y": 6.843385043467462, + "heading": 0.5218324598046723, + "angularVelocity": 0.02182592645444906, + "velocityX": 0.9025945762979182, + "velocityY": 0.556541303922395, + "timestamp": 7.576901226266458 + }, + { + "x": 2.6703780246763387, + "y": 6.8731963037812145, + "heading": 0.5229271765337971, + "angularVelocity": 0.013511683273610298, + "velocityX": 0.6036382508063606, + "velocityY": 0.367949348566719, + "timestamp": 7.657921237389937 + }, + { + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": 0.006368644721814939, + "velocityX": 0.30252191195191025, + "velocityY": 0.18282262031723026, + "timestamp": 7.738941248513416 + }, + { + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": -2.018007055520513e-33, + "velocityX": -1.0144673376047062e-35, + "velocityY": 0, + "timestamp": 7.819961259636894 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBc.traj b/src/main/deploy/choreo/CenterLanePBc.traj deleted file mode 100644 index 84e6d4bd..00000000 --- a/src/main/deploy/choreo/CenterLanePBc.traj +++ /dev/null @@ -1,940 +0,0 @@ -{ - "samples": [ - { - "x": 1.5270464420318604, - "y": 4.085783958435059, - "heading": 1.4625077505834915e-24, - "angularVelocity": -1.6968405368267337e-23, - "velocityX": 1.1721587912394621e-19, - "velocityY": -9.898260288411433e-19, - "timestamp": 0 - }, - { - "x": 1.5260826099479785, - "y": 4.093922987832265, - "heading": -1.5564984556705778e-16, - "angularVelocity": -1.5951860339453124e-15, - "velocityX": -0.009878020828395265, - "velocityY": 0.08341442794136009, - "timestamp": 0.09757340064631857 - }, - { - "x": 1.524154945788772, - "y": 4.11020104655442, - "heading": -4.825478082550605e-16, - "angularVelocity": -3.3502446331081196e-15, - "velocityX": -0.019756041569095335, - "velocityY": 0.16682885514218276, - "timestamp": 0.19514680129263715 - }, - { - "x": 1.5212634495643529, - "y": 4.1346181345161295, - "heading": -9.964831586930951e-16, - "angularVelocity": -5.2671216122656385e-15, - "velocityX": -0.02963406220615563, - "velocityY": 0.2502432814678248, - "timestamp": 0.2927202019389557 - }, - { - "x": 1.5174081212868562, - "y": 4.16717425161492, - "heading": -1.717782516967208e-15, - "angularVelocity": -7.392318655053113e-15, - "velocityX": -0.0395120827188482, - "velocityY": 0.33365770674325024, - "timestamp": 0.3902936025852743 - }, - { - "x": 1.5125889609711136, - "y": 4.207869397725548, - "heading": -2.6688719095730424e-15, - "angularVelocity": -9.74735165634569e-15, - "velocityX": -0.049390103079535765, - "velocityY": 0.4170721307350775, - "timestamp": 0.4878670032315929 - }, - { - "x": 1.5068059686356647, - "y": 4.256703572691455, - "heading": -3.876697094067346e-15, - "angularVelocity": -1.2378541573688779e-14, - "velocityX": -0.05926812325021708, - "velocityY": 0.500486553122407, - "timestamp": 0.5854404038779114 - }, - { - "x": 1.5000591443043463, - "y": 4.313676776311357, - "heading": -5.376745558038763e-15, - "angularVelocity": -1.5373429438679563e-14, - "velocityX": -0.06914614317660467, - "velocityY": 0.5839009734468111, - "timestamp": 0.68301380452423 - }, - { - "x": 1.49234848800894, - "y": 4.378789008316868, - "heading": -7.221997958230257e-15, - "angularVelocity": -1.8911296572887638e-14, - "velocityX": -0.07902416277726727, - "velocityY": 0.667315391020648, - "timestamp": 0.7805872051705486 - }, - { - "x": 1.4836739997939412, - "y": 4.452040268332257, - "heading": -9.479155947701628e-15, - "angularVelocity": -2.3132763955751144e-14, - "velocityX": -0.08890218192191494, - "velocityY": 0.7507298047436908, - "timestamp": 0.8781606058168672 - }, - { - "x": 1.4740356797260918, - "y": 4.533430555793919, - "heading": -1.2250780387615259e-14, - "angularVelocity": -2.8405339345394318e-14, - "velocityX": -0.09878020038254028, - "velocityY": 0.8341442126905433, - "timestamp": 0.9757340064631858 - }, - { - "x": 1.4634335279166297, - "y": 4.622959869762518, - "heading": -1.570747306998558e-14, - "angularVelocity": -3.542634883593584e-14, - "velocityX": -0.1086582177031285, - "velocityY": 0.9175586110104128, - "timestamp": 1.0733074071095043 - }, - { - "x": 1.451867544588029, - "y": 4.720628208359378, - "heading": -2.0193306127373128e-14, - "angularVelocity": -4.5973625402297885e-14, - "velocityX": -0.11853623274364325, - "velocityY": 1.000972990076323, - "timestamp": 1.1708808077558228 - }, - { - "x": 1.439337730407712, - "y": 4.826435565948485, - "heading": -2.6736262460658644e-14, - "angularVelocity": -6.705631534728917e-14, - "velocityX": -0.12841424094394735, - "velocityY": 1.0843873113804372, - "timestamp": 1.2684542084021413 - }, - { - "x": 1.4268079162273966, - "y": 4.9322429235375935, - "heading": -2.0195304822343385e-14, - "angularVelocity": 6.703581864794123e-14, - "velocityX": -0.12841424094393503, - "velocityY": 1.0843873113804385, - "timestamp": 1.3660276090484598 - }, - { - "x": 1.4152419328987969, - "y": 5.029911262134455, - "heading": -1.571219926412934e-14, - "angularVelocity": 4.594566272297136e-14, - "velocityX": -0.1185362327436348, - "velocityY": 1.0009729900763238, - "timestamp": 1.4636010096947782 - }, - { - "x": 1.4046397810893352, - "y": 5.119440576103053, - "heading": -1.2258131187906243e-14, - "angularVelocity": 3.539944307707758e-14, - "velocityX": -0.10865821770312199, - "velocityY": 0.9175586110104136, - "timestamp": 1.5611744103410967 - }, - { - "x": 1.3950014610214865, - "y": 5.200830863564716, - "heading": -9.488622066072567e-15, - "angularVelocity": 2.8383655202850955e-14, - "velocityX": -0.09878020038253506, - "velocityY": 0.8341442126905438, - "timestamp": 1.6587478109874152 - }, - { - "x": 1.386326972806488, - "y": 5.274082123580104, - "heading": -7.232328083000986e-15, - "angularVelocity": 2.3123906034472235e-14, - "velocityX": -0.0889021819219107, - "velocityY": 0.7507298047436913, - "timestamp": 1.7563212116337337 - }, - { - "x": 1.378616316511082, - "y": 5.339194355585617, - "heading": -5.387907876762194e-15, - "angularVelocity": 1.8902766491774747e-14, - "velocityX": -0.0790241627772638, - "velocityY": 0.6673153910206483, - "timestamp": 1.8538946122800521 - }, - { - "x": 1.3718694921797638, - "y": 5.396167559205518, - "heading": -3.885859488553591e-15, - "angularVelocity": 1.53939266022293e-14, - "velocityX": -0.06914614317660184, - "velocityY": 0.5839009734468115, - "timestamp": 1.9514680129263706 - }, - { - "x": 1.3660864998443152, - "y": 5.4450017341714245, - "heading": -2.6761448049829828e-15, - "angularVelocity": 1.2397908698261985e-14, - "velocityX": -0.05926812325021481, - "velocityY": 0.5004865531224073, - "timestamp": 2.0490414135726893 - }, - { - "x": 1.361267339528573, - "y": 5.4856968802820525, - "heading": -1.7234986300483674e-15, - "angularVelocity": 9.76331049910467e-15, - "velocityX": -0.04939010307953397, - "velocityY": 0.4170721307350777, - "timestamp": 2.146614814219008 - }, - { - "x": 1.3574120112510764, - "y": 5.518252997380844, - "heading": -1.0006590963793495e-15, - "angularVelocity": 7.408108892000521e-15, - "velocityX": -0.03951208271884683, - "velocityY": 0.3336577067432504, - "timestamp": 2.2441882148653263 - }, - { - "x": 1.3545205150266573, - "y": 5.542670085342553, - "heading": -4.849115866184459e-16, - "angularVelocity": 5.285700891601152e-15, - "velocityX": -0.029634062206154655, - "velocityY": 0.25024328146782493, - "timestamp": 2.3417616155116447 - }, - { - "x": 1.3525928508674507, - "y": 5.558948144064708, - "heading": -1.5693409310506777e-16, - "angularVelocity": 3.361316960958839e-15, - "velocityX": -0.019756041569094714, - "velocityY": 0.1668288551421828, - "timestamp": 2.439335016157963 - }, - { - "x": 1.3516290187835691, - "y": 5.567087173461914, - "heading": -6.1887132115889425e-24, - "angularVelocity": 1.608357872874533e-15, - "velocityX": -0.009878020828394967, - "velocityY": 0.08341442794136011, - "timestamp": 2.5369084168042817 - }, - { - "x": 1.3516290187835691, - "y": 5.567087173461914, - "heading": -1.4648141818546668e-24, - "angularVelocity": 3.340172165504719e-23, - "velocityX": -1.5435615328511767e-18, - "velocityY": -2.1040451608667207e-18, - "timestamp": 2.6344818174506 - }, - { - "x": 1.3600455150653603, - "y": 5.566939516487978, - "heading": 6.686728970627398e-20, - "angularVelocity": 6.762246861371845e-19, - "velocityX": 0.08511363372225911, - "velocityY": -0.0014932129921227434, - "timestamp": 2.733367224097152 - }, - { - "x": 1.376878507539919, - "y": 5.566644202541668, - "heading": 1.580133567010756e-19, - "angularVelocity": 9.217342317784428e-19, - "velocityX": 0.17022726654424447, - "velocityY": -0.0029864259684513, - "timestamp": 2.8322526307437035 - }, - { - "x": 1.4021279960984379, - "y": 5.566201231624893, - "heading": 2.8447751001050527e-19, - "angularVelocity": 1.2788959837210388e-18, - "velocityX": 0.2553408982658953, - "velocityY": -0.004479638925475853, - "timestamp": 2.931138037390255 - }, - { - "x": 1.435793980604908, - "y": 5.565610603740038, - "heading": 3.7920321307665064e-19, - "angularVelocity": 9.579340703122642e-19, - "velocityX": 0.3404545286121278, - "velocityY": -0.005972851858370403, - "timestamp": 3.030023444036807 - }, - { - "x": 1.4778764608844615, - "y": 5.564872318890172, - "heading": 4.3118128283917494e-19, - "angularVelocity": 5.256394158249627e-19, - "velocityX": 0.42556815718996566, - "velocityY": -0.007466064760240663, - "timestamp": 3.1289088506833584 - }, - { - "x": 1.52837543670394, - "y": 5.563986377079386, - "heading": 3.7231429179967247e-19, - "angularVelocity": -5.953051342426901e-19, - "velocityX": 0.5106817834099439, - "velocityY": -0.008959277620745203, - "timestamp": 3.22779425732991 - }, - { - "x": 1.5872909077369224, - "y": 5.562952778313405, - "heading": 3.665507864267625e-19, - "angularVelocity": -5.828470212315425e-20, - "velocityX": 0.5957954063289188, - "velocityY": -0.01045249042333774, - "timestamp": 3.3266796639764618 - }, - { - "x": 1.6546228734937773, - "y": 5.56177152260082, - "heading": 3.3289358715677737e-19, - "angularVelocity": -3.403656887203302e-19, - "velocityX": 0.6809090242963888, - "velocityY": -0.011945703139062274, - "timestamp": 3.4255650706230134 - }, - { - "x": 1.7303713331584527, - "y": 5.560442609955947, - "heading": 2.1844998249066e-19, - "angularVelocity": -1.1573356581936454e-18, - "velocityX": 0.766022634011352, - "velocityY": -0.013438915710006823, - "timestamp": 3.524450477269565 - }, - { - "x": 1.8145362850988431, - "y": 5.55896604040742, - "heading": 5.382114736148874e-20, - "angularVelocity": -1.6648446043389792e-18, - "velocityX": 0.851136227221305, - "velocityY": -0.01493212799139147, - "timestamp": 3.6233358839161167 - }, - { - "x": 1.90711772441864, - "y": 5.557341814041138, - "heading": -1.1309409770731682e-19, - "angularVelocity": -1.6879664104157646e-18, - "velocityX": 0.9362497709162765, - "velocityY": -0.016425339404097267, - "timestamp": 3.7222212905626684 - }, - { - "x": 1.9996991637384367, - "y": 5.555717587674857, - "heading": -1.8044549355651484e-19, - "angularVelocity": -6.811054697269865e-19, - "velocityX": 0.9362497709162764, - "velocityY": -0.016425339404097263, - "timestamp": 3.82110669720922 - }, - { - "x": 2.083864115678827, - "y": 5.554241018126329, - "heading": -2.080650691294579e-19, - "angularVelocity": -2.793088809488762e-19, - "velocityX": 0.8511362272213049, - "velocityY": -0.014932127991391469, - "timestamp": 3.9199921038557717 - }, - { - "x": 2.1596125753435023, - "y": 5.552912105481456, - "heading": -1.7750016719575358e-19, - "angularVelocity": 3.090941806706561e-19, - "velocityX": 0.7660226340113518, - "velocityY": -0.013438915710006822, - "timestamp": 4.018877510502323 - }, - { - "x": 2.226944541100357, - "y": 5.551730849768871, - "heading": -2.0769787641047505e-19, - "angularVelocity": -3.0538082503662157e-19, - "velocityX": 0.6809090242963888, - "velocityY": -0.011945703139062272, - "timestamp": 4.117762917148874 - }, - { - "x": 2.2858600121333397, - "y": 5.550697251002891, - "heading": -2.0325843947083703e-19, - "angularVelocity": 4.4894778552600243e-20, - "velocityX": 0.5957954063289187, - "velocityY": -0.010452490423337736, - "timestamp": 4.216648323795425 - }, - { - "x": 2.3363589879528184, - "y": 5.549811309192104, - "heading": -2.3083936836877355e-19, - "angularVelocity": -2.7891805869836823e-19, - "velocityX": 0.5106817834099439, - "velocityY": -0.008959277620745202, - "timestamp": 4.3155337304419765 - }, - { - "x": 2.3784414682323716, - "y": 5.5490730243422375, - "heading": -2.907085889576077e-19, - "angularVelocity": -6.054403614475218e-19, - "velocityX": 0.42556815718996566, - "velocityY": -0.00746606476024066, - "timestamp": 4.414419137088528 - }, - { - "x": 2.4121074527388418, - "y": 5.548482396457383, - "heading": -2.2908841084806075e-19, - "angularVelocity": 6.231473438855849e-19, - "velocityX": 0.3404545286121278, - "velocityY": -0.0059728518583704, - "timestamp": 4.513304543735079 - }, - { - "x": 2.437356941297361, - "y": 5.548039425540607, - "heading": -1.3373199383404307e-19, - "angularVelocity": 9.643123213362854e-19, - "velocityX": 0.2553408982658953, - "velocityY": -0.004479638925475851, - "timestamp": 4.61218995038163 - }, - { - "x": 2.4541899337719197, - "y": 5.547744111594297, - "heading": -3.289437685355794e-20, - "angularVelocity": 1.019742123971898e-18, - "velocityX": 0.17022726654424447, - "velocityY": -0.002986425968451297, - "timestamp": 4.711075357028181 - }, - { - "x": 2.462606430053711, - "y": 5.547596454620361, - "heading": -2.886570593000217e-29, - "angularVelocity": 3.3265147049069624e-19, - "velocityX": 0.08511363372225912, - "velocityY": -0.001493212992122741, - "timestamp": 4.809960763674733 - }, - { - "x": 2.462606430053711, - "y": 5.547596454620361, - "heading": -7.821656462951895e-30, - "angularVelocity": 2.943074068153252e-29, - "velocityX": -2.2275173681467236e-19, - "velocityY": 4.4340986172999205e-21, - "timestamp": 4.908846170321284 - }, - { - "x": 2.454042275901417, - "y": 5.547744111594184, - "heading": 1.4460933754378984e-20, - "angularVelocity": 1.4497351179100133e-19, - "velocityX": -0.08585722436689831, - "velocityY": 0.0014802883863889518, - "timestamp": 5.008594959260656 - }, - { - "x": 2.436913967686631, - "y": 5.548039425540282, - "heading": 4.286650670020682e-20, - "angularVelocity": 2.8477109182723563e-19, - "velocityX": -0.17171444783351814, - "velocityY": 0.0029605767572559506, - "timestamp": 5.108343748200029 - }, - { - "x": 2.41122150551911, - "y": 5.548482396456763, - "heading": 5.855738018623403e-21, - "angularVelocity": -3.710397725109959e-19, - "velocityX": -0.2575716701997976, - "velocityY": 0.004440865109151674, - "timestamp": 5.208092537139401 - }, - { - "x": 2.3769648895360507, - "y": 5.549073024341261, - "heading": -6.291625399026022e-20, - "angularVelocity": -6.894518818577401e-19, - "velocityX": -0.3434288911906516, - "velocityY": 0.005921153437333303, - "timestamp": 5.307841326078774 - }, - { - "x": 2.33414411991385, - "y": 5.549811309190735, - "heading": -1.2017788841555738e-19, - "angularVelocity": -5.740584286417081e-19, - "velocityX": -0.42928611041310155, - "velocityY": 0.007401441735025384, - "timestamp": 5.407590115018146 - }, - { - "x": 2.282759196887703, - "y": 5.5506972510011305, - "heading": -1.0823396901286469e-19, - "angularVelocity": 1.1973998537101906e-19, - "velocityX": -0.5151433272776794, - "velocityY": 0.008881729992064734, - "timestamp": 5.5073389039575185 - }, - { - "x": 2.2228101207868822, - "y": 5.551730849766769, - "heading": -8.802649819919278e-20, - "angularVelocity": 2.0258361490738492e-19, - "velocityX": -0.6010005408412367, - "velocityY": 0.010362018192190264, - "timestamp": 5.607087692896891 - }, - { - "x": 2.1542968921052967, - "y": 5.552912105479136, - "heading": -6.239601383466543e-20, - "angularVelocity": 2.569503383512215e-19, - "velocityX": -0.6868577494532631, - "velocityY": 0.01184230630694507, - "timestamp": 5.706836481836263 - }, - { - "x": 2.0772195116661285, - "y": 5.554241018124038, - "heading": -3.607669582656793e-20, - "angularVelocity": 2.6385601752371007e-19, - "velocityX": -0.7727149498127389, - "velocityY": 0.013322594279415347, - "timestamp": 5.806585270775636 - }, - { - "x": 1.991577981115741, - "y": 5.55571758767309, - "heading": -6.282144315835921e-20, - "angularVelocity": -2.6812099635964853e-19, - "velocityX": -0.8585721336671177, - "velocityY": 0.014802881967316633, - "timestamp": 5.906334059715008 - }, - { - "x": 1.8973723053932194, - "y": 5.557341814041137, - "heading": -1.267501677946567e-19, - "angularVelocity": -6.408972052260157e-19, - "velocityX": -0.9444292680062534, - "velocityY": 0.016283168801511785, - "timestamp": 6.006082848654381 - }, - { - "x": 1.8031666296706979, - "y": 5.558966040409183, - "heading": -3.795289314001396e-21, - "angularVelocity": 1.2326452916170473e-18, - "velocityX": -0.9444292680062534, - "velocityY": 0.016283168801511785, - "timestamp": 6.105831637593753 - }, - { - "x": 1.7175250991203104, - "y": 5.560442609958236, - "heading": 8.700208420675153e-20, - "angularVelocity": 9.1026038606046e-19, - "velocityX": -0.8585721336671177, - "velocityY": 0.014802881967316633, - "timestamp": 6.205580426533126 - }, - { - "x": 1.640447718681142, - "y": 5.561771522603139, - "heading": 1.2976083579479636e-19, - "angularVelocity": 4.2866435269702167e-19, - "velocityX": -0.7727149498127389, - "velocityY": 0.013322594279415347, - "timestamp": 6.305329215472498 - }, - { - "x": 1.5719344899995562, - "y": 5.5629527783155055, - "heading": 7.490403325878171e-20, - "angularVelocity": -5.49949547949794e-19, - "velocityX": -0.686857749453263, - "velocityY": 0.01184230630694507, - "timestamp": 6.4050780044118705 - }, - { - "x": 1.5119854138987352, - "y": 5.563986377081145, - "heading": 7.478581489863291e-20, - "angularVelocity": -1.1851672756803286e-21, - "velocityX": -0.6010005408412366, - "velocityY": 0.010362018192190266, - "timestamp": 6.504826793351243 - }, - { - "x": 1.460600490872588, - "y": 5.56487231889154, - "heading": 7.910534561996949e-20, - "angularVelocity": 4.3304083992516454e-20, - "velocityX": -0.5151433272776794, - "velocityY": 0.008881729992064737, - "timestamp": 6.604575582290615 - }, - { - "x": 1.4177797212503878, - "y": 5.565610603741014, - "heading": 5.432779100593734e-20, - "angularVelocity": -2.483995467741431e-19, - "velocityX": -0.4292861104131015, - "velocityY": 0.007401441735025386, - "timestamp": 6.704324371229988 - }, - { - "x": 1.383523105267329, - "y": 5.566201231625511, - "heading": -1.3462949300383398e-21, - "angularVelocity": -5.581429670757211e-19, - "velocityX": -0.34342889119065156, - "velocityY": 0.005921153437333306, - "timestamp": 6.80407316016936 - }, - { - "x": 1.3578306430998077, - "y": 5.566644202541992, - "heading": -3.946401324540515e-20, - "angularVelocity": -3.821371505085094e-19, - "velocityX": -0.25757167019979754, - "velocityY": 0.004440865109151676, - "timestamp": 6.903821949108733 - }, - { - "x": 1.3407023348850213, - "y": 5.566939516488091, - "heading": -2.4791971136884522e-20, - "angularVelocity": 1.470899198317256e-19, - "velocityX": -0.1717144478335181, - "velocityY": 0.0029605767572559527, - "timestamp": 7.003570738048105 - }, - { - "x": 1.332138180732727, - "y": 5.567087173461914, - "heading": 7.598893748852645e-29, - "angularVelocity": 2.4854406877542406e-19, - "velocityX": -0.0858572243668983, - "velocityY": 0.001480288386388954, - "timestamp": 7.103319526987478 - }, - { - "x": 1.332138180732727, - "y": 5.567087173461914, - "heading": 1.0804753349489245e-28, - "angularVelocity": 2.1800931515228082e-28, - "velocityX": 1.941111784767018e-19, - "velocityY": 2.1082261122464193e-18, - "timestamp": 7.20306831592685 - }, - { - "x": 1.3372491110466118, - "y": 5.560676853815777, - "heading": 3.558639537501783e-19, - "angularVelocity": 3.646584120755799e-18, - "velocityX": 0.05237236819901584, - "velocityY": -0.06568737982374474, - "timestamp": 7.300656614673684 - }, - { - "x": 1.3474709716341546, - "y": 5.547856214573958, - "heading": 1.0721703768123472e-18, - "angularVelocity": 7.340084933358087e-18, - "velocityX": 0.10474473598582199, - "velocityY": -0.1313747591304807, - "timestamp": 7.398244913420519 - }, - { - "x": 1.3628037624486922, - "y": 5.528625255794984, - "heading": -1.2934737138196825e-18, - "angularVelocity": -2.4241062607509123e-17, - "velocityX": 0.15711710329446488, - "velocityY": -0.19706213783748644, - "timestamp": 7.495833212167353 - }, - { - "x": 1.383247483435446, - "y": 5.50298397754756, - "heading": -3.2871298566762286e-18, - "angularVelocity": -2.0429253778468544e-17, - "velocityX": 0.20948947004178572, - "velocityY": -0.262749515840461, - "timestamp": 7.593421510914188 - }, - { - "x": 1.4088021345292037, - "y": 5.470932379913476, - "heading": -6.392169366146804e-18, - "angularVelocity": -3.181774355862696e-17, - "velocityX": 0.261861836120866, - "velocityY": -0.32843689300530343, - "timestamp": 7.691009809661022 - }, - { - "x": 1.439467715651024, - "y": 5.432470462991745, - "heading": -9.037790663968063e-18, - "angularVelocity": -2.7110025471414772e-17, - "velocityX": 0.31423420139102354, - "velocityY": -0.39412426915556464, - "timestamp": 7.788598108407856 - }, - { - "x": 1.4752442267033907, - "y": 5.387598226904674, - "heading": -1.128391773258776e-17, - "angularVelocity": -2.301635620338134e-17, - "velocityX": 0.36660656566192357, - "velocityY": -0.45981164405251984, - "timestamp": 7.886186407154691 - }, - { - "x": 1.516131667562784, - "y": 5.336315671807188, - "heading": -1.3116491479431834e-17, - "angularVelocity": -1.877862095234715e-17, - "velocityX": 0.41897892866709746, - "velocityY": -0.525499017361954, - "timestamp": 7.983774705901525 - }, - { - "x": 1.562130038067678, - "y": 5.278622797901877, - "heading": -1.4552519190706605e-17, - "angularVelocity": -1.4715162201094296e-17, - "velocityX": 0.47135129001709103, - "velocityY": -0.5911863885953993, - "timestamp": 8.08136300464836 - }, - { - "x": 1.613239337997809, - "y": 5.214519605465006, - "heading": -1.5531545587943304e-17, - "angularVelocity": -1.0032210364686455e-17, - "velocityX": 0.5237236491100207, - "velocityY": -0.6568737569979507, - "timestamp": 8.178951303395195 - }, - { - "x": 1.6694595670350196, - "y": 5.1440060948956186, - "heading": -1.6968450650800506e-17, - "angularVelocity": -1.4724152370496227e-17, - "velocityX": 0.5760960049427472, - "velocityY": -0.7225611213114336, - "timestamp": 8.27653960214203 - }, - { - "x": 1.7307907246793477, - "y": 5.067082266820785, - "heading": -1.785524132033205e-17, - "angularVelocity": -9.08705871531393e-18, - "velocityX": 0.6284683556522977, - "velocityY": -0.788248479199238, - "timestamp": 8.374127900888865 - }, - { - "x": 1.7972328100308623, - "y": 4.983748122369234, - "heading": -1.8156964270181303e-17, - "angularVelocity": -3.091793819261216e-18, - "velocityX": 0.680840697140134, - "velocityY": -0.8539358255208239, - "timestamp": 8.4717161996357 - }, - { - "x": 1.8687858209897246, - "y": 4.894003664174662, - "heading": -1.7567148894927193e-17, - "angularVelocity": 6.043915190863691e-18, - "velocityX": 0.7332130171106522, - "velocityY": -0.919623144854586, - "timestamp": 8.569304498382536 - }, - { - "x": 1.9454497470568448, - "y": 4.797848905405417, - "heading": -1.7590364829650386e-17, - "angularVelocity": -2.378972880130156e-19, - "velocityX": 0.7855852294956268, - "velocityY": -0.985310329250542, - "timestamp": 8.666892797129371 - }, - { - "x": 2.0170027580157073, - "y": 4.708104447210844, - "heading": -1.8525240985906295e-17, - "angularVelocity": -9.57979819187636e-18, - "velocityX": 0.7332130171106522, - "velocityY": -0.9196231448545861, - "timestamp": 8.764481095876206 - }, - { - "x": 2.083444843367222, - "y": 4.624770302759293, - "heading": -1.62920217217427e-17, - "angularVelocity": 2.2884087968719853e-17, - "velocityX": 0.6808406971401341, - "velocityY": -0.8539358255208239, - "timestamp": 8.862069394623042 - }, - { - "x": 2.14477600101155, - "y": 4.54784647468446, - "heading": -1.4137613991552986e-17, - "angularVelocity": 2.207649592959214e-17, - "velocityX": 0.6284683556522977, - "velocityY": -0.7882484791992381, - "timestamp": 8.959657693369877 - }, - { - "x": 2.200996230048761, - "y": 4.477332964115072, - "heading": -1.2021193262691405e-17, - "angularVelocity": 2.1687238185481475e-17, - "velocityX": 0.5760960049427472, - "velocityY": -0.7225611213114337, - "timestamp": 9.057245992116712 - }, - { - "x": 2.252105529978892, - "y": 4.413229771678201, - "heading": -9.999661721633516e-18, - "angularVelocity": 2.07148963451225e-17, - "velocityX": 0.5237236491100207, - "velocityY": -0.6568737569979508, - "timestamp": 9.154834290863548 - }, - { - "x": 2.2981039004837855, - "y": 4.3555368977728905, - "heading": -8.10923369851807e-18, - "angularVelocity": 1.9371461559461523e-17, - "velocityX": 0.471351290017091, - "velocityY": -0.5911863885953994, - "timestamp": 9.252422589610383 - }, - { - "x": 2.338991341343179, - "y": 4.304254342675404, - "heading": -6.3749798263727294e-18, - "angularVelocity": 1.7771124740362523e-17, - "velocityX": 0.41897892866709735, - "velocityY": -0.525499017361954, - "timestamp": 9.350010888357218 - }, - { - "x": 2.374767852395546, - "y": 4.259382106588334, - "heading": -4.81528393523032e-18, - "angularVelocity": 1.5982406426947388e-17, - "velocityX": 0.3666065656619235, - "velocityY": -0.4598116440525199, - "timestamp": 9.447599187104053 - }, - { - "x": 2.405433433517366, - "y": 4.220920189666603, - "heading": -3.4442054151676063e-18, - "angularVelocity": 1.4049619717228675e-17, - "velocityX": 0.3142342013910235, - "velocityY": -0.39412426915556475, - "timestamp": 9.545187485850889 - }, - { - "x": 2.430988084611124, - "y": 4.188868592032519, - "heading": -2.2728437711124333e-18, - "angularVelocity": 1.2003094938541682e-17, - "velocityX": 0.261861836120866, - "velocityY": -0.3284368930053035, - "timestamp": 9.642775784597724 - }, - { - "x": 2.4514318055978777, - "y": 4.163227313785095, - "heading": -1.3744434339694666e-18, - "angularVelocity": 9.206024983663883e-18, - "velocityX": 0.2094894700417857, - "velocityY": -0.2627495158404611, - "timestamp": 9.74036408334456 - }, - { - "x": 2.4667645964124154, - "y": 4.14399635500612, - "heading": -6.921668969846993e-19, - "angularVelocity": 6.991376321961435e-18, - "velocityX": 0.15711710329446488, - "velocityY": -0.19706213783748644, - "timestamp": 9.837952382091395 - }, - { - "x": 2.4769864569999585, - "y": 4.131175715764301, - "heading": -2.3224840986113696e-19, - "angularVelocity": 4.712844493605461e-18, - "velocityX": 0.10474473598582197, - "velocityY": -0.1313747591304807, - "timestamp": 9.93554068083823 - }, - { - "x": 2.4820973873138428, - "y": 4.124765396118164, - "heading": -2.507555799405664e-28, - "angularVelocity": 2.3798796279739872e-18, - "velocityX": 0.05237236819901583, - "velocityY": -0.06568737982374474, - "timestamp": 10.033128979585065 - }, - { - "x": 2.4820973873138428, - "y": 4.124765396118164, - "heading": -1.183597031867973e-28, - "angularVelocity": 1.4383049425452364e-28, - "velocityX": -1.8276514384427982e-18, - "velocityY": 2.2923091413933995e-18, - "timestamp": 10.1307172783319 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBA.1.traj b/src/main/deploy/choreo/CenterLanePCBA.1.traj index 848083ee..4199a7d3 100644 --- a/src/main/deploy/choreo/CenterLanePCBA.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBA.1.traj @@ -1,328 +1,310 @@ { "samples": [ { - "x": 1.2899744510650635, + "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -5.658132549917657e-18, - "angularVelocity": -1.3627953112859022e-17, - "velocityX": -2.0031548351625804e-16, - "velocityY": -3.76327335107334e-16, + "heading": -1.7297728380351187e-19, + "angularVelocity": 4.2448351957919954e-19, + "velocityX": -3.109246931729259e-17, + "velocityY": 1.4154954258989747e-18, "timestamp": 0 }, { - "x": 1.3043755674296558, - "y": 5.57443357397875, - "heading": -0.00949699856960441, - "angularVelocity": -0.12640481817945412, - "velocityX": 0.19167850581579765, - "velocityY": -0.21989050819926978, - "timestamp": 0.07513161845108944 - }, - { - "x": 1.333181418148157, - "y": 5.541395353902404, - "heading": -0.028483550113125006, - "angularVelocity": -0.2527105356630112, - "velocityX": 0.3834051669803611, - "velocityY": -0.4397378994111986, - "timestamp": 0.15026323690217888 - }, - { - "x": 1.3763968623207659, - "y": 5.491843734526039, - "heading": -0.05694076519946621, - "angularVelocity": -0.3787648352819493, - "velocityX": 0.575196502649018, - "velocityY": -0.6595308393324772, - "timestamp": 0.2253948553532683 - }, - { - "x": 1.4340285202431429, - "y": 5.425784148354235, - "heading": -0.09483975957151482, - "angularVelocity": -0.5044346861470347, - "velocityX": 0.7670759542900343, - "velocityY": -0.8792514727514988, - "timestamp": 0.30052647380435776 - }, - { - "x": 1.506085691225813, - "y": 5.343224221277017, - "heading": -0.14214370390652084, - "angularVelocity": -0.6296143396295131, - "velocityX": 0.9590791795253601, - "velocityY": -1.0988706057618765, - "timestamp": 0.3756580922554472 - }, - { - "x": 1.5925821714791393, - "y": 5.2441754183101335, - "heading": -0.1988101671423879, - "angularVelocity": -0.7542292367162207, - "velocityX": 1.1512660319811814, - "velocityY": -1.3183371396948111, - "timestamp": 0.4507897107065367 - }, - { - "x": 1.6935404980129638, - "y": 5.128656797964584, - "heading": -0.2647926440409603, - "angularVelocity": -0.8782251502241488, - "velocityX": 1.3437528514834798, - "velocityY": -1.537550005315265, - "timestamp": 0.5259213291576261 - }, - { - "x": 1.8090047506628752, - "y": 4.996706206010246, - "heading": -0.340037203461346, - "angularVelocity": -1.0015032415711032, - "velocityX": 1.5368263725127755, - "velocityY": -1.7562591446699427, - "timestamp": 0.6010529476087155 - }, - { - "x": 1.9391049044022293, - "y": 4.848436876634891, - "heading": -0.4244483701055882, - "angularVelocity": -1.1235105590497598, - "velocityX": 1.7316298572388857, - "velocityY": -1.9734611397719521, - "timestamp": 0.6761845660598049 - }, - { - "x": 2.0768333663443204, - "y": 4.7205786690094, - "heading": -0.4949304356074777, - "angularVelocity": -0.9381145641700113, - "velocityX": 1.8331624525981836, - "velocityY": -1.701789609442247, - "timestamp": 0.7513161845108943 - }, - { - "x": 2.2004381051303508, - "y": 4.609474436663161, - "heading": -0.5559710621989046, - "angularVelocity": -0.8124492437076729, - "velocityX": 1.64517604356056, - "velocityY": -1.478794609158427, - "timestamp": 0.8264478029619837 - }, - { - "x": 2.3097905497138385, - "y": 4.515017160876504, - "heading": -0.6076423779450488, - "angularVelocity": -0.6877439460179713, - "velocityX": 1.4554783570583507, - "velocityY": -1.2572240253997846, - "timestamp": 0.9015794214130731 - }, - { - "x": 2.4048475753814116, - "y": 4.437170463185575, - "heading": -0.6499678697321152, - "angularVelocity": -0.5633512582056197, - "velocityX": 1.2652066816824061, - "velocityY": -1.0361376380998342, - "timestamp": 0.9767110398641625 - }, - { - "x": 2.4855875270959107, - "y": 4.375915949453412, - "heading": -0.6829583691226992, - "angularVelocity": -0.43910273822474927, - "velocityX": 1.07464677829478, - "velocityY": -0.8152960763101849, - "timestamp": 1.051842658315252 - }, - { - "x": 2.5519973911226996, - "y": 4.3312425173589775, - "heading": -0.7066196510488878, - "angularVelocity": -0.31493108244769585, - "velocityX": 0.883913662419395, - "velocityY": -0.5946022861403426, - "timestamp": 1.1269742767663415 - }, - { - "x": 2.604068513564512, - "y": 4.303142764963194, - "heading": -0.7209550318751378, - "angularVelocity": -0.1908035673998065, - "velocityX": 0.6930653633843512, - "velocityY": -0.3740070155974622, - "timestamp": 1.202105895217431 - }, - { - "x": 2.6417947649723112, - "y": 4.291611444319209, - "heading": -0.725966473493026, - "angularVelocity": -0.06670216508037836, - "velocityX": 0.5021354815334791, - "velocityY": -0.15348159509808112, - "timestamp": 1.2772375136685206 + "x": 1.3060810448259774, + "y": 5.572403683390414, + "heading": -0.01067910748684314, + "angularVelocity": -0.14191654637088458, + "velocityX": 0.21404337049401068, + "velocityY": -0.246522471704611, + "timestamp": 0.07524920638169826 + }, + { + "x": 1.338296279992689, + "y": 5.535304313687478, + "heading": -0.032027558921844806, + "angularVelocity": -0.28370334335098873, + "velocityX": 0.4281139525027307, + "velocityY": -0.4930200793709309, + "timestamp": 0.1504984127633965 + }, + { + "x": 1.3866231363779933, + "y": 5.479658598357654, + "heading": -0.06402169471627948, + "angularVelocity": -0.42517572387591435, + "velocityX": 0.6422241337702286, + "velocityY": -0.7394857435109029, + "timestamp": 0.22574761914509478 + }, + { + "x": 1.4510659275520246, + "y": 5.405469878379882, + "heading": -0.1066262273585151, + "angularVelocity": -0.5661791624235272, + "velocityX": 0.8563916388334698, + "velocityY": -0.9859070087913582, + "timestamp": 0.300996825526793 + }, + { + "x": 1.5316310742105061, + "y": 5.312743180460654, + "heading": -0.15979745091713102, + "angularVelocity": -0.7066017851257455, + "velocityX": 1.0706444696567097, + "velocityY": -1.2322614732810149, + "timestamp": 0.37624603190849126 + }, + { + "x": 1.6283288838862067, + "y": 5.201486845299536, + "heading": -0.22348688396132305, + "angularVelocity": -0.8463801295274643, + "velocityX": 1.2850342791041873, + "velocityY": -1.4785050967404112, + "timestamp": 0.4514952382901895 + }, + { + "x": 1.7411789077586277, + "y": 5.071717216156576, + "heading": -0.2976431936929222, + "angularVelocity": -0.9854763033040427, + "velocityX": 1.4996839076304158, + "velocityY": -1.7245315317294676, + "timestamp": 0.5267444446718877 + }, + { + "x": 1.8702368010806378, + "y": 4.923481749892843, + "heading": -0.3821997162379228, + "angularVelocity": -1.1236865690804896, + "velocityX": 1.7150731486446302, + "velocityY": -1.9699273041075376, + "timestamp": 0.601993651053586 + }, + { + "x": 2.022770759979851, + "y": 4.779444476824575, + "heading": -0.46182371324885607, + "angularVelocity": -1.0581373656886817, + "velocityX": 2.0270507322765163, + "velocityY": -1.914136772919067, + "timestamp": 0.6772428574352843 + }, + { + "x": 2.159313623811303, + "y": 4.6540559405768205, + "heading": -0.5309316944889129, + "angularVelocity": -0.9183881739497737, + "velocityX": 1.8145422443221764, + "velocityY": -1.666310414116386, + "timestamp": 0.7524920638169826 + }, + { + "x": 2.2798095691663756, + "y": 4.547269113259306, + "heading": -0.5895622742606058, + "angularVelocity": -0.7791521344995035, + "velocityX": 1.601291909229005, + "velocityY": -1.4191090172553904, + "timestamp": 0.8277412701986809 + }, + { + "x": 2.3842397515062683, + "y": 4.459067906121371, + "heading": -0.6377272819831894, + "angularVelocity": -0.6400732982918503, + "velocityX": 1.3877911457302048, + "velocityY": -1.172121426644562, + "timestamp": 0.9029904765803792 + }, + { + "x": 2.47259465372947, + "y": 4.389444105411809, + "heading": -0.675431270833758, + "angularVelocity": -0.5010549700591305, + "velocityX": 1.1741639077877584, + "velocityY": -0.9252429900248669, + "timestamp": 0.9782396829620775 + }, + { + "x": 2.5448685468329297, + "y": 4.338392728081247, + "heading": -0.7026759403224371, + "angularVelocity": -0.36205922691677456, + "velocityX": 0.9604605361114344, + "velocityY": -0.6784307740288329, + "timestamp": 1.0534888893437757 + }, + { + "x": 2.601057641331632, + "y": 4.305910462744002, + "heading": -0.7194616948028688, + "angularVelocity": -0.2230688573014308, + "velocityX": 0.7467068052991092, + "velocityY": -0.43166256362226185, + "timestamp": 1.128738095725474 + }, + { + "x": 2.641159295921209, + "y": 4.291994997972796, + "heading": -0.7257882885399572, + "angularVelocity": -0.08407522206898749, + "velocityX": 0.5329179737262918, + "velocityY": -0.18492507018124238, + "timestamp": 1.2039873021071723 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.057384074600868515, - "velocityX": 0.3111454104283852, - "velocityY": 0.06699234541879971, - "timestamp": 1.35236913211961 - }, - { - "x": 2.6741706929303724, - "y": 4.3184165536581745, - "heading": -0.7079150841905613, - "angularVelocity": 0.18203686942568406, - "velocityX": 0.11922553065022659, - "velocityY": 0.28844784676770313, - "timestamp": 1.4278485168598247 - }, - { - "x": 2.66867899794669, - "y": 4.356899858459873, - "heading": -0.684771836918116, - "angularVelocity": 0.30661679809476405, - "velocityX": -0.0727575483579021, - "velocityY": 0.5098518613032106, - "timestamp": 1.5033279016000394 - }, - { - "x": 2.6486902203739557, - "y": 4.412089436366014, - "heading": -0.6522316135776107, - "angularVelocity": 0.43111405122449203, - "velocityX": -0.2648243310823653, - "velocityY": 0.7311874373940866, - "timestamp": 1.578807286340254 - }, - { - "x": 2.6141955298766604, - "y": 4.483978019553037, - "heading": -0.6103014306892406, - "angularVelocity": 0.5555183449290747, - "velocityX": -0.4570081038377013, - "velocityY": 0.9524267246290669, - "timestamp": 1.6542866710804687 - }, - { - "x": 2.565181655781626, - "y": 4.572554576780097, - "heading": -0.5589890537241899, - "angularVelocity": 0.679819756618359, - "velocityX": -0.649367695106772, - "velocityY": 1.1735198628071373, - "timestamp": 1.7297660558206833 - }, - { - "x": 2.5016263830570638, - "y": 4.677800410068183, - "heading": -0.49830336002197995, - "angularVelocity": 0.8040035555369035, - "velocityX": -0.842021605572691, - "velocityY": 1.3943652779921414, - "timestamp": 1.805245440560898 - }, - { - "x": 2.423485048210339, - "y": 4.799677415452472, - "heading": -0.42825793280914715, - "angularVelocity": 0.9280073950309187, - "velocityX": -1.0352672470523336, - "velocityY": 1.6147058670562153, - "timestamp": 1.8807248253011126 - }, - { - "x": 2.330623716021553, - "y": 4.938069451098365, - "heading": -0.3489003970727596, - "angularVelocity": 1.0513802677046555, - "velocityX": -1.2302873494790556, - "velocityY": 1.8335077335201706, - "timestamp": 1.9562042100413273 - }, - { - "x": 2.2158437334233922, - "y": 5.072717873356153, - "heading": -0.27145428242562347, - "angularVelocity": 1.026056517505551, - "velocityX": -1.5206798914447361, - "velocityY": 1.7839099076626914, - "timestamp": 2.031683594781542 - }, - { - "x": 2.115226856826334, - "y": 5.190380914478301, - "heading": -0.20366940333084413, - "angularVelocity": 0.8980581827763012, - "velocityX": -1.333037847880913, - "velocityY": 1.5588765267548788, - "timestamp": 2.1071629795217564 - }, - { - "x": 2.028914050823854, - "y": 5.291175476770991, - "heading": -0.1455212111518032, - "angularVelocity": 0.7703850843618669, - "velocityX": -1.1435282136547777, - "velocityY": 1.3353919436695292, - "timestamp": 2.182642364261971 - }, - { - "x": 1.95695123959882, - "y": 5.375140495871242, - "heading": -0.09702855670668924, - "angularVelocity": 0.6424622380441017, - "velocityX": -0.9534101459703443, - "velocityY": 1.1124232052394878, - "timestamp": 2.258121749002185 - }, - { - "x": 1.89936105960217, - "y": 5.44229547054464, - "heading": -0.05821541752658763, - "angularVelocity": 0.5142217217941343, - "velocityX": -0.7629921758539479, - "velocityY": 0.8897127991440635, - "timestamp": 2.3336011337423996 - }, - { - "x": 1.8561570206808413, - "y": 5.492652117133406, - "heading": -0.029101140919542783, - "angularVelocity": 0.38572487981921, - "velocityX": -0.572395218494233, - "velocityY": 0.6671576187743987, - "timestamp": 2.409080518482614 - }, - { - "x": 1.8273481914953718, - "y": 5.526218220226538, - "heading": -0.009696189627478797, - "angularVelocity": 0.2570894206319378, - "velocityX": -0.38167811361857223, - "velocityY": 0.4447055737053434, - "timestamp": 2.4845599032228285 - }, - { - "x": 1.8129411935806279, + "angularVelocity": 0.054926472219667455, + "velocityX": 0.31910406053771284, + "velocityY": 0.061790547746105144, + "timestamp": 1.2792365084888706 + }, + { + "x": 2.6730242117812852, + "y": 4.320207160810404, + "heading": -0.7068522062263088, + "angularVelocity": 0.19509567726207716, + "velocityX": 0.10349358326673892, + "velocityY": 0.31054279257798323, + "timestamp": 1.3551116333379198 + }, + { + "x": 2.6645156104831624, + "y": 4.362642495661916, + "heading": -0.6814188000083412, + "angularVelocity": 0.33520084834820957, + "velocityX": -0.1121395360458166, + "velocityY": 0.5592786164892918, + "timestamp": 1.430986758186969 + }, + { + "x": 2.6396435810539995, + "y": 4.423949055779942, + "heading": -0.6453599487845141, + "angularVelocity": 0.47523943183649325, + "velocityX": -0.32780215490271986, + "velocityY": 0.807992873026772, + "timestamp": 1.5068618830360183 + }, + { + "x": 2.5984050053739445, + "y": 4.5041245094115245, + "heading": -0.5986803757122448, + "angularVelocity": 0.615215766235525, + "velocityX": -0.5435058691760214, + "velocityY": 1.0566763981096883, + "timestamp": 1.5827370078850675 + }, + { + "x": 2.540795163397632, + "y": 4.603165195262531, + "heading": -0.541383229045499, + "angularVelocity": 0.755150608064052, + "velocityX": -0.7592717915230448, + "velocityY": 1.3053116689836102, + "timestamp": 1.6586121327341168 + }, + { + "x": 2.4668060178101583, + "y": 4.721064600465768, + "heading": -0.4734677826566921, + "angularVelocity": 0.8950950199273465, + "velocityX": -0.9751436420635095, + "velocityY": 1.5538611031988796, + "timestamp": 1.734487257583166 + }, + { + "x": 2.3764209643089336, + "y": 4.857808746061696, + "heading": -0.3949264705539342, + "angularVelocity": 1.0351391481588907, + "velocityX": -1.1912343298399446, + "velocityY": 1.8022263010181736, + "timestamp": 1.8103623824322153 + }, + { + "x": 2.269588305263905, + "y": 5.013353141552007, + "heading": -0.3057482687442009, + "angularVelocity": 1.175328567657553, + "velocityX": -1.4080063691172056, + "velocityY": 2.0500051340898513, + "timestamp": 1.8862375072812645 + }, + { + "x": 2.155512806030656, + "y": 5.145840094887286, + "heading": -0.22945454409592414, + "angularVelocity": 1.0055169569743476, + "velocityX": -1.5034637433586158, + "velocityY": 1.7461184228535829, + "timestamp": 1.9621126321303137 + }, + { + "x": 2.0576690248190195, + "y": 5.259344023095087, + "heading": -0.1639803843714035, + "angularVelocity": 0.8629199603276121, + "velocityX": -1.289537004472555, + "velocityY": 1.4959306944600357, + "timestamp": 2.037987756979363 + }, + { + "x": 1.9761088976814785, + "y": 5.353909559223238, + "heading": -0.10935658983885492, + "angularVelocity": 0.7199170300053718, + "velocityX": -1.0749257717858158, + "velocityY": 1.2463312095522094, + "timestamp": 2.113862881828412 + }, + { + "x": 1.9108492511031485, + "y": 5.429551680381221, + "heading": -0.06562174275393094, + "angularVelocity": 0.5764056029164016, + "velocityX": -0.8600927735973349, + "velocityY": 0.9969291162074897, + "timestamp": 2.1897380066774614 + }, + { + "x": 1.8618983395529738, + "y": 5.486277925937523, + "heading": -0.03280716495777854, + "angularVelocity": 0.4324813680566584, + "velocityX": -0.6451509852228532, + "velocityY": 0.7476263883465671, + "timestamp": 2.2656131315265107 + }, + { + "x": 1.8292611562031649, + "y": 5.524092810268815, + "heading": -0.010931847108471086, + "angularVelocity": 0.2883068448697581, + "velocityX": -0.430143389085703, + "velocityY": 0.4983831579385639, + "timestamp": 2.34148825637556 + }, + { + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.24734731593596e-17, - "angularVelocity": 0.12846142905206553, - "velocityX": -0.19087328234026615, - "velocityY": 0.22232623397342577, - "timestamp": 2.560039287963043 + "heading": -1.6776667047563746e-20, + "angularVelocity": 0.1440768253130289, + "velocityX": -0.2150897630156809, + "velocityY": 0.24917859900600148, + "timestamp": 2.417363381224609 }, { - "x": 1.8129411935806274, + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 5.939064303009515e-18, - "angularVelocity": -5.9066042058701954e-18, - "velocityX": -3.4367552729151325e-16, - "velocityY": -1.697930821204867e-16, - "timestamp": 2.6355186727032573 + "heading": 1.1170600847702852e-20, + "angularVelocity": 5.938902420193292e-19, + "velocityX": -2.686779016810018e-17, + "velocityY": -1.2969153357483792e-17, + "timestamp": 2.4932385060736584 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBA.2.traj b/src/main/deploy/choreo/CenterLanePCBA.2.traj index c70e7d0b..637f6b56 100644 --- a/src/main/deploy/choreo/CenterLanePCBA.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBA.2.traj @@ -1,103 +1,94 @@ { "samples": [ { - "x": 1.8129411935806274, + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 5.939064303009515e-18, - "angularVelocity": -5.9066042058701954e-18, - "velocityX": -3.4367552729151325e-16, - "velocityY": -1.697930821204867e-16, + "heading": 1.1170600847702852e-20, + "angularVelocity": 5.938902420193292e-19, + "velocityX": -2.686779016810018e-17, + "velocityY": -1.2969153357483792e-17, "timestamp": 0 }, { - "x": 1.8467557558007979, - "y": 5.543052711491932, - "heading": 5.160367277754293e-18, - "angularVelocity": -8.367176611695404e-18, - "velocityX": 0.36334081801614093, - "velocityY": 0.0005742601437371532, - "timestamp": 0.09306568528358428 + "x": 1.8552093951788071, + "y": 5.543066072468525, + "heading": -1.0867013680305264e-17, + "angularVelocity": -1.1051820926914384e-16, + "velocityX": 0.42942883009919236, + "velocityY": 0.0006787122433536914, + "timestamp": 0.09842888654778115 }, { - "x": 1.914384879084952, - "y": 5.543159599317722, - "heading": 4.087331547357737e-18, - "angularVelocity": -1.1529875132039326e-17, - "velocityX": 0.7266816236089447, - "velocityY": 0.0011485202678393933, - "timestamp": 0.18613137056716855 + "x": 1.9397457967809342, + "y": 5.543199682246804, + "heading": -3.2635799657096195e-17, + "angularVelocity": -2.2116257473548814e-16, + "velocityX": 0.8588576440015888, + "velocityY": 0.001357424461108283, + "timestamp": 0.1968577730955623 }, { - "x": 2.0158285613519547, - "y": 5.543319931052202, - "heading": 2.611621451738615e-18, - "angularVelocity": -1.5856651043003625e-17, - "velocityX": 1.0900224068397422, - "velocityY": 0.0017227803565984868, - "timestamp": 0.27919705585075283 + "x": 2.0665503951985436, + "y": 5.5434000969079245, + "heading": -4.603800029490119e-17, + "angularVelocity": -1.3616125416355767e-16, + "velocityX": 1.2882864255103954, + "velocityY": 0.002036136627664645, + "timestamp": 0.29528665964334344 }, { - "x": 2.1510867977458235, - "y": 5.543533706687698, - "heading": 4.496026882117787e-19, - "angularVelocity": -2.3231105610401644e-17, - "velocityX": 1.4533631378925314, - "velocityY": 0.0022970403628902463, - "timestamp": 0.3722627411343371 + "x": 2.235623180866242, + "y": 5.543667316436768, + "heading": -4.798133527402523e-17, + "angularVelocity": -1.9743543259972466e-17, + "velocityX": 1.7177151098384498, + "velocityY": 0.002714848640626296, + "timestamp": 0.3937155461911246 }, { - "x": 2.32015956398666, - "y": 5.543800926185837, - "heading": -3.8747303229957764e-18, - "angularVelocity": -4.646538622728347e-17, - "velocityX": 1.8167036080554047, - "velocityY": 0.00287129995684554, - "timestamp": 0.4653284264179214 + "x": 2.40469596653394, + "y": 5.54393453596561, + "heading": -3.9330353852399465e-17, + "angularVelocity": 8.789067645262313e-17, + "velocityX": 1.7177151098384498, + "velocityY": 0.0027148486406302456, + "timestamp": 0.49214443273890573 }, { - "x": 2.4554178003805287, - "y": 5.544014701821333, - "heading": -2.0140989785604273e-18, - "angularVelocity": 1.9992667960964244e-17, - "velocityX": 1.4533631378925314, - "velocityY": 0.002297040362890341, - "timestamp": 0.5583941117015057 + "x": 2.531500564951549, + "y": 5.544134950626731, + "heading": -2.0009563266152006e-17, + "angularVelocity": 1.96291873894211e-16, + "velocityX": 1.2882864255103954, + "velocityY": 0.002036136627666603, + "timestamp": 0.5905733192866869 }, { - "x": 2.556861482647531, - "y": 5.544175033555813, - "heading": -9.161136142082278e-19, - "angularVelocity": 1.1797961418381928e-17, - "velocityX": 1.0900224068397426, - "velocityY": 0.001722780356598613, - "timestamp": 0.6514597969850899 - }, - { - "x": 2.6244906059316855, - "y": 5.544281921381602, - "heading": -2.8482092694126203e-19, - "angularVelocity": 6.783302409926777e-18, - "velocityX": 0.7266816236089451, - "velocityY": 0.0011485202678395388, - "timestamp": 0.7445254822686742 + "x": 2.616036966553676, + "y": 5.54426856040501, + "heading": -7.02269200923652e-18, + "angularVelocity": 1.3194166582538958e-16, + "velocityX": 0.8588576440015888, + "velocityY": 0.0013574244611093242, + "timestamp": 0.689002205834468 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -1.8432742635406086e-23, - "angularVelocity": 3.0602309898573723e-18, - "velocityX": 0.36334081801614126, - "velocityY": 0.0005742601437373122, - "timestamp": 0.8375911675522585 + "heading": -3.8911537114769476e-19, + "angularVelocity": 6.739461219451495e-17, + "velocityX": 0.4294288300991924, + "velocityY": 0.0006787122433541438, + "timestamp": 0.7874310923822492 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -1.9267281699534778e-23, - "angularVelocity": -8.967222433260206e-24, - "velocityX": 3.3495247448012114e-21, - "velocityY": 4.428006153501746e-23, - "timestamp": 0.9306568528358428 + "heading": -4.27312910681112e-19, + "angularVelocity": -3.880724585667238e-19, + "velocityX": -2.3908482729518656e-18, + "velocityY": 6.838225330043828e-18, + "timestamp": 0.8858599789300303 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBA.3.traj b/src/main/deploy/choreo/CenterLanePCBA.3.traj index 347488af..d48ace6f 100644 --- a/src/main/deploy/choreo/CenterLanePCBA.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBA.3.traj @@ -3,200 +3,191 @@ { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -1.9267281699534778e-23, - "angularVelocity": -8.967222433260206e-24, - "velocityX": 3.3495247448012114e-21, - "velocityY": 4.428006153501746e-23, + "heading": -4.27312910681112e-19, + "angularVelocity": -3.880724585667238e-19, + "velocityX": -2.3908482729518656e-18, + "velocityY": 6.838225330043828e-18, "timestamp": 0 }, { - "x": 2.6433995634924674, - "y": 5.563703580385797, - "heading": 0.004845900450156499, - "angularVelocity": 0.06121219104231838, - "velocityX": -0.18828383484070044, - "velocityY": 0.24465440313021314, - "timestamp": 0.07916561011198464 - }, - { - "x": 2.613808012454941, - "y": 5.602604430475708, - "heading": 0.01472246361068666, - "angularVelocity": 0.12475825230878844, - "velocityX": -0.3737930017297544, - "velocityY": 0.4913857170415789, - "timestamp": 0.15833122022396928 - }, - { - "x": 2.5698581719130473, - "y": 5.661275942603098, - "heading": 0.02990473352989552, - "angularVelocity": 0.19177860055310114, - "velocityX": -0.5551632897128446, - "velocityY": 0.7411237284017032, - "timestamp": 0.23749683033595392 - }, - { - "x": 2.5120904369033115, - "y": 5.740092575087619, - "heading": 0.05083960434186745, - "angularVelocity": 0.26444400267184504, - "velocityX": -0.7297074440280278, - "velocityY": 0.995591802716233, - "timestamp": 0.31666244044793856 - }, - { - "x": 2.441559614817569, - "y": 5.839724514645118, - "heading": 0.07837099812745103, - "angularVelocity": 0.3477696154509352, - "velocityX": -0.8909275376766786, - "velocityY": 1.2585255064233427, - "timestamp": 0.3958280505599232 - }, - { - "x": 2.3611631890754614, - "y": 5.961650149114452, - "heading": 0.11464704720334498, - "angularVelocity": 0.458229893315787, - "velocityX": -1.0155473522958047, - "velocityY": 1.5401338320624665, - "timestamp": 0.4749936606719083 - }, - { - "x": 2.2902542502823713, - "y": 6.1057913956854275, - "heading": 0.16704371336362323, - "angularVelocity": 0.6618614583549582, - "velocityX": -0.8957038124608021, - "velocityY": 1.8207558353567825, - "timestamp": 0.5541592707838929 - }, - { - "x": 2.241416295543653, - "y": 6.2393929892272615, - "heading": 0.22223140963064517, - "angularVelocity": 0.6971170460122206, - "velocityX": -0.6169087141453696, - "velocityY": 1.6876215992379315, - "timestamp": 0.6333248808958776 - }, - { - "x": 2.211878843837597, - "y": 6.357956546172331, - "heading": 0.276701736402732, - "angularVelocity": 0.6880554156664132, - "velocityX": -0.3731096326330706, - "velocityY": 1.497664917599366, - "timestamp": 0.7124904910078622 - }, - { - "x": 2.200573473010042, - "y": 6.460208388316336, - "heading": 0.32930652128307925, - "angularVelocity": 0.6644903614831552, - "velocityX": -0.14280659002770119, - "velocityY": 1.291619454449523, - "timestamp": 0.7916561011198469 - }, - { - "x": 2.206951813042022, - "y": 6.545559671097223, - "heading": 0.3794800427949416, - "angularVelocity": 0.6337792564333018, - "velocityX": 0.08056958094502174, - "velocityY": 1.0781358554573408, - "timestamp": 0.8708217112318315 - }, - { - "x": 2.230681954112334, - "y": 6.613672661059127, - "heading": 0.42688445047126194, - "angularVelocity": 0.598800509580661, - "velocityX": 0.2997531508535616, - "velocityY": 0.8603860927182075, - "timestamp": 0.9499873213438161 - }, - { - "x": 2.271541794524346, - "y": 6.6643287076043505, - "heading": 0.47129474601819377, - "angularVelocity": 0.5609796410854497, - "velocityX": 0.5161311882042349, - "velocityY": 0.6398743908316677, - "timestamp": 1.0291529314558008 + "x": 2.6438893629989613, + "y": 5.563919104052164, + "heading": 0.003139586303789949, + "angularVelocity": 0.042042846518139325, + "velocityX": -0.19304501448070868, + "velocityY": 0.26224987725536203, + "timestamp": 0.07467587387155739 + }, + { + "x": 2.6152669032432256, + "y": 5.6032366820496495, + "heading": 0.009644303445155527, + "angularVelocity": 0.0871060063194363, + "velocityX": -0.38328925088933385, + "velocityY": 0.5265097809918172, + "timestamp": 0.14935174774311477 + }, + { + "x": 2.5727487902328847, + "y": 5.662504910556293, + "heading": 0.019847811829768587, + "angularVelocity": 0.13663728130135686, + "velocityX": -0.5693688042201159, + "velocityY": 0.7936730490571009, + "timestamp": 0.22402762161467216 + }, + { + "x": 2.5168455054942007, + "y": 5.742063733198033, + "heading": 0.03429127316793741, + "angularVelocity": 0.19341536415109115, + "velocityX": -0.7486123943435556, + "velocityY": 1.0653885721991296, + "timestamp": 0.29870349548622954 + }, + { + "x": 2.4485453653591587, + "y": 5.842518348175502, + "heading": 0.05399784068706335, + "angularVelocity": 0.2638947024981649, + "velocityX": -0.9146212370077963, + "velocityY": 1.3452084290335973, + "timestamp": 0.3733793693577869 + }, + { + "x": 2.370517685528427, + "y": 5.96519344251776, + "heading": 0.08158842838664396, + "angularVelocity": 0.369471239761256, + "velocityX": -1.0448847236115244, + "velocityY": 1.642767442578038, + "timestamp": 0.4480552432293443 + }, + { + "x": 2.3003293154277267, + "y": 6.110432122884094, + "heading": 0.1284873855211158, + "angularVelocity": 0.6280335897392839, + "velocityX": -0.9399069131942741, + "velocityY": 1.94492106802986, + "timestamp": 0.5227311171009017 + }, + { + "x": 2.2517603849650345, + "y": 6.244622565856429, + "heading": 0.18176076028404833, + "angularVelocity": 0.7133947284575884, + "velocityX": -0.6503965463628967, + "velocityY": 1.796971846665519, + "timestamp": 0.5974069909724591 + }, + { + "x": 2.22192407028561, + "y": 6.363296472335711, + "heading": 0.23706248397325824, + "angularVelocity": 0.7405567664909981, + "velocityX": -0.39954423206005096, + "velocityY": 1.5891867122091021, + "timestamp": 0.6720828648440165 + }, + { + "x": 2.2097448289139274, + "y": 6.465233576489708, + "heading": 0.2929845885143906, + "angularVelocity": 0.7488644141924427, + "velocityX": -0.16309472846117115, + "velocityY": 1.3650607467858884, + "timestamp": 0.7467587387155739 + }, + { + "x": 2.2146777318065234, + "y": 6.549876478070479, + "heading": 0.348839415183568, + "angularVelocity": 0.747963482359082, + "velocityX": 0.06605751813605938, + "velocityY": 1.1334705198944957, + "timestamp": 0.8214346125871312 + }, + { + "x": 2.236395128424509, + "y": 6.61690736376859, + "heading": 0.4042192057980219, + "angularVelocity": 0.7416021767580177, + "velocityX": 0.2908221289159501, + "velocityY": 0.8976243895505626, + "timestamp": 0.8961104864586886 + }, + { + "x": 2.274678638341779, + "y": 6.666121192465354, + "heading": 0.45885367997336884, + "angularVelocity": 0.7316214908889929, + "velocityX": 0.5126623624534666, + "velocityY": 0.6590325113759182, + "timestamp": 0.970786360330246 }, { "x": 2.3293724060058594, "y": 6.6973748207092285, "heading": 0.5125504196, - "angularVelocity": 0.521131252869112, - "velocityX": 0.7305016837451085, - "velocityY": 0.4174301575915678, - "timestamp": 1.1083185415677854 + "angularVelocity": 0.719064094502462, + "velocityX": 0.7324155022029422, + "velocityY": 0.41852376977375944, + "timestamp": 1.0454622342018034 }, { - "x": 2.3973241454559178, - "y": 6.736204444156279, - "heading": 0.5391342040901619, - "angularVelocity": 0.37821212966035084, - "velocityX": 0.9667612262295852, - "velocityY": 0.5524358122619253, - "timestamp": 1.1786065688556335 + "x": 2.4110936768539344, + "y": 6.744072759574757, + "heading": 0.5643107226604884, + "angularVelocity": 0.653664165942785, + "velocityX": 1.0320315606781885, + "velocityY": 0.5897332019890127, + "timestamp": 1.1246470905199306 }, { - "x": 2.48097335716082, - "y": 6.784004065158346, - "heading": 0.5372874666531787, - "angularVelocity": -0.026273854996956482, - "velocityX": 1.1900918966232585, - "velocityY": 0.6800535289789073, - "timestamp": 1.2488945961434816 + "x": 2.50599401367539, + "y": 6.798301604540289, + "heading": 0.5436072588792712, + "angularVelocity": -0.2614573637418783, + "velocityX": 1.198465732389427, + "velocityY": 0.6848385851414024, + "timestamp": 1.2038319468380578 }, { - "x": 2.54793079875253, - "y": 6.822265517551258, - "heading": 0.5291862310389086, - "angularVelocity": -0.11525768935146737, - "velocityX": 0.952615177510983, - "velocityY": 0.5443523437671908, - "timestamp": 1.3191826234313297 + "x": 2.577169265515479, + "y": 6.838973237821007, + "heading": 0.528079022912807, + "angularVelocity": -0.19610108155123726, + "velocityX": 0.8988492894921869, + "velocityY": 0.51362893325611, + "timestamp": 1.283016803156185 }, { - "x": 2.598141729775772, - "y": 6.850957521028096, - "heading": 0.5214396908605601, - "angularVelocity": -0.11021137563904597, - "velocityX": 0.7143596564122447, - "velocityY": 0.40820612818364127, - "timestamp": 1.3894706507191779 - }, - { - "x": 2.631611382674506, - "y": 6.870083065561309, - "heading": 0.5156519889527617, - "angularVelocity": -0.08234264256835963, - "velocityX": 0.4761785782046116, - "velocityY": 0.2721024514586011, - "timestamp": 1.459758678007026 + "x": 2.6246194330616937, + "y": 6.8660876598097635, + "heading": 0.5177266590936774, + "angularVelocity": -0.13073666229232678, + "velocityX": 0.5992328552770498, + "velocityY": 0.3424192863320269, + "timestamp": 1.3622016594743123 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.044126567104521564, - "velocityX": 0.23806521146364917, - "velocityY": 0.13603746705807154, - "timestamp": 1.530046705294874 + "angularVelocity": -0.06536905835734146, + "velocityX": 0.2996164266200211, + "velocityY": 0.1712096425840104, + "timestamp": 1.4413865157924395 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -7.689800874803388e-26, - "velocityX": 2.07998453448468e-19, - "velocityY": -3.6391739324491333e-19, - "timestamp": 1.6003347325827222 + "angularVelocity": -8.381816905081638e-20, + "velocityX": -7.007327358505968e-19, + "velocityY": 8.954594613208896e-19, + "timestamp": 1.5205713721105667 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBA.traj b/src/main/deploy/choreo/CenterLanePCBA.traj index 3fbd0b1c..7ac60797 100644 --- a/src/main/deploy/choreo/CenterLanePCBA.traj +++ b/src/main/deploy/choreo/CenterLanePCBA.traj @@ -1,607 +1,571 @@ { "samples": [ { - "x": 1.2899744510650635, + "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -5.658132549917657e-18, - "angularVelocity": -1.3627953112859022e-17, - "velocityX": -2.0031548351625804e-16, - "velocityY": -3.76327335107334e-16, + "heading": -1.7297728380351187e-19, + "angularVelocity": 4.2448351957919954e-19, + "velocityX": -3.109246931729259e-17, + "velocityY": 1.4154954258989747e-18, "timestamp": 0 }, { - "x": 1.3043755674296558, - "y": 5.57443357397875, - "heading": -0.00949699856960441, - "angularVelocity": -0.12640481817945412, - "velocityX": 0.19167850581579765, - "velocityY": -0.21989050819926978, - "timestamp": 0.07513161845108944 - }, - { - "x": 1.333181418148157, - "y": 5.541395353902404, - "heading": -0.028483550113125006, - "angularVelocity": -0.2527105356630112, - "velocityX": 0.3834051669803611, - "velocityY": -0.4397378994111986, - "timestamp": 0.15026323690217888 - }, - { - "x": 1.3763968623207659, - "y": 5.491843734526039, - "heading": -0.05694076519946621, - "angularVelocity": -0.3787648352819493, - "velocityX": 0.575196502649018, - "velocityY": -0.6595308393324772, - "timestamp": 0.2253948553532683 - }, - { - "x": 1.4340285202431429, - "y": 5.425784148354235, - "heading": -0.09483975957151482, - "angularVelocity": -0.5044346861470347, - "velocityX": 0.7670759542900343, - "velocityY": -0.8792514727514988, - "timestamp": 0.30052647380435776 - }, - { - "x": 1.506085691225813, - "y": 5.343224221277017, - "heading": -0.14214370390652084, - "angularVelocity": -0.6296143396295131, - "velocityX": 0.9590791795253601, - "velocityY": -1.0988706057618765, - "timestamp": 0.3756580922554472 - }, - { - "x": 1.5925821714791393, - "y": 5.2441754183101335, - "heading": -0.1988101671423879, - "angularVelocity": -0.7542292367162207, - "velocityX": 1.1512660319811814, - "velocityY": -1.3183371396948111, - "timestamp": 0.4507897107065367 - }, - { - "x": 1.6935404980129638, - "y": 5.128656797964584, - "heading": -0.2647926440409603, - "angularVelocity": -0.8782251502241488, - "velocityX": 1.3437528514834798, - "velocityY": -1.537550005315265, - "timestamp": 0.5259213291576261 - }, - { - "x": 1.8090047506628752, - "y": 4.996706206010246, - "heading": -0.340037203461346, - "angularVelocity": -1.0015032415711032, - "velocityX": 1.5368263725127755, - "velocityY": -1.7562591446699427, - "timestamp": 0.6010529476087155 - }, - { - "x": 1.9391049044022293, - "y": 4.848436876634891, - "heading": -0.4244483701055882, - "angularVelocity": -1.1235105590497598, - "velocityX": 1.7316298572388857, - "velocityY": -1.9734611397719521, - "timestamp": 0.6761845660598049 - }, - { - "x": 2.0768333663443204, - "y": 4.7205786690094, - "heading": -0.4949304356074777, - "angularVelocity": -0.9381145641700113, - "velocityX": 1.8331624525981836, - "velocityY": -1.701789609442247, - "timestamp": 0.7513161845108943 - }, - { - "x": 2.2004381051303508, - "y": 4.609474436663161, - "heading": -0.5559710621989046, - "angularVelocity": -0.8124492437076729, - "velocityX": 1.64517604356056, - "velocityY": -1.478794609158427, - "timestamp": 0.8264478029619837 - }, - { - "x": 2.3097905497138385, - "y": 4.515017160876504, - "heading": -0.6076423779450488, - "angularVelocity": -0.6877439460179713, - "velocityX": 1.4554783570583507, - "velocityY": -1.2572240253997846, - "timestamp": 0.9015794214130731 - }, - { - "x": 2.4048475753814116, - "y": 4.437170463185575, - "heading": -0.6499678697321152, - "angularVelocity": -0.5633512582056197, - "velocityX": 1.2652066816824061, - "velocityY": -1.0361376380998342, - "timestamp": 0.9767110398641625 - }, - { - "x": 2.4855875270959107, - "y": 4.375915949453412, - "heading": -0.6829583691226992, - "angularVelocity": -0.43910273822474927, - "velocityX": 1.07464677829478, - "velocityY": -0.8152960763101849, - "timestamp": 1.051842658315252 - }, - { - "x": 2.5519973911226996, - "y": 4.3312425173589775, - "heading": -0.7066196510488878, - "angularVelocity": -0.31493108244769585, - "velocityX": 0.883913662419395, - "velocityY": -0.5946022861403426, - "timestamp": 1.1269742767663415 - }, - { - "x": 2.604068513564512, - "y": 4.303142764963194, - "heading": -0.7209550318751378, - "angularVelocity": -0.1908035673998065, - "velocityX": 0.6930653633843512, - "velocityY": -0.3740070155974622, - "timestamp": 1.202105895217431 - }, - { - "x": 2.6417947649723112, - "y": 4.291611444319209, - "heading": -0.725966473493026, - "angularVelocity": -0.06670216508037836, - "velocityX": 0.5021354815334791, - "velocityY": -0.15348159509808112, - "timestamp": 1.2772375136685206 + "x": 1.3060810448259774, + "y": 5.572403683390414, + "heading": -0.01067910748684314, + "angularVelocity": -0.14191654637088458, + "velocityX": 0.21404337049401068, + "velocityY": -0.246522471704611, + "timestamp": 0.07524920638169826 + }, + { + "x": 1.338296279992689, + "y": 5.535304313687478, + "heading": -0.032027558921844806, + "angularVelocity": -0.28370334335098873, + "velocityX": 0.4281139525027307, + "velocityY": -0.4930200793709309, + "timestamp": 0.1504984127633965 + }, + { + "x": 1.3866231363779933, + "y": 5.479658598357654, + "heading": -0.06402169471627948, + "angularVelocity": -0.42517572387591435, + "velocityX": 0.6422241337702286, + "velocityY": -0.7394857435109029, + "timestamp": 0.22574761914509478 + }, + { + "x": 1.4510659275520246, + "y": 5.405469878379882, + "heading": -0.1066262273585151, + "angularVelocity": -0.5661791624235272, + "velocityX": 0.8563916388334698, + "velocityY": -0.9859070087913582, + "timestamp": 0.300996825526793 + }, + { + "x": 1.5316310742105061, + "y": 5.312743180460654, + "heading": -0.15979745091713102, + "angularVelocity": -0.7066017851257455, + "velocityX": 1.0706444696567097, + "velocityY": -1.2322614732810149, + "timestamp": 0.37624603190849126 + }, + { + "x": 1.6283288838862067, + "y": 5.201486845299536, + "heading": -0.22348688396132305, + "angularVelocity": -0.8463801295274643, + "velocityX": 1.2850342791041873, + "velocityY": -1.4785050967404112, + "timestamp": 0.4514952382901895 + }, + { + "x": 1.7411789077586277, + "y": 5.071717216156576, + "heading": -0.2976431936929222, + "angularVelocity": -0.9854763033040427, + "velocityX": 1.4996839076304158, + "velocityY": -1.7245315317294676, + "timestamp": 0.5267444446718877 + }, + { + "x": 1.8702368010806378, + "y": 4.923481749892843, + "heading": -0.3821997162379228, + "angularVelocity": -1.1236865690804896, + "velocityX": 1.7150731486446302, + "velocityY": -1.9699273041075376, + "timestamp": 0.601993651053586 + }, + { + "x": 2.022770759979851, + "y": 4.779444476824575, + "heading": -0.46182371324885607, + "angularVelocity": -1.0581373656886817, + "velocityX": 2.0270507322765163, + "velocityY": -1.914136772919067, + "timestamp": 0.6772428574352843 + }, + { + "x": 2.159313623811303, + "y": 4.6540559405768205, + "heading": -0.5309316944889129, + "angularVelocity": -0.9183881739497737, + "velocityX": 1.8145422443221764, + "velocityY": -1.666310414116386, + "timestamp": 0.7524920638169826 + }, + { + "x": 2.2798095691663756, + "y": 4.547269113259306, + "heading": -0.5895622742606058, + "angularVelocity": -0.7791521344995035, + "velocityX": 1.601291909229005, + "velocityY": -1.4191090172553904, + "timestamp": 0.8277412701986809 + }, + { + "x": 2.3842397515062683, + "y": 4.459067906121371, + "heading": -0.6377272819831894, + "angularVelocity": -0.6400732982918503, + "velocityX": 1.3877911457302048, + "velocityY": -1.172121426644562, + "timestamp": 0.9029904765803792 + }, + { + "x": 2.47259465372947, + "y": 4.389444105411809, + "heading": -0.675431270833758, + "angularVelocity": -0.5010549700591305, + "velocityX": 1.1741639077877584, + "velocityY": -0.9252429900248669, + "timestamp": 0.9782396829620775 + }, + { + "x": 2.5448685468329297, + "y": 4.338392728081247, + "heading": -0.7026759403224371, + "angularVelocity": -0.36205922691677456, + "velocityX": 0.9604605361114344, + "velocityY": -0.6784307740288329, + "timestamp": 1.0534888893437757 + }, + { + "x": 2.601057641331632, + "y": 4.305910462744002, + "heading": -0.7194616948028688, + "angularVelocity": -0.2230688573014308, + "velocityX": 0.7467068052991092, + "velocityY": -0.43166256362226185, + "timestamp": 1.128738095725474 + }, + { + "x": 2.641159295921209, + "y": 4.291994997972796, + "heading": -0.7257882885399572, + "angularVelocity": -0.08407522206898749, + "velocityX": 0.5329179737262918, + "velocityY": -0.18492507018124238, + "timestamp": 1.2039873021071723 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.057384074600868515, - "velocityX": 0.3111454104283852, - "velocityY": 0.06699234541879971, - "timestamp": 1.35236913211961 - }, - { - "x": 2.6741706929303724, - "y": 4.3184165536581745, - "heading": -0.7079150841905613, - "angularVelocity": 0.18203686942568406, - "velocityX": 0.11922553065022659, - "velocityY": 0.28844784676770313, - "timestamp": 1.4278485168598247 - }, - { - "x": 2.66867899794669, - "y": 4.356899858459873, - "heading": -0.684771836918116, - "angularVelocity": 0.30661679809476405, - "velocityX": -0.0727575483579021, - "velocityY": 0.5098518613032106, - "timestamp": 1.5033279016000394 - }, - { - "x": 2.6486902203739557, - "y": 4.412089436366014, - "heading": -0.6522316135776107, - "angularVelocity": 0.43111405122449203, - "velocityX": -0.2648243310823653, - "velocityY": 0.7311874373940866, - "timestamp": 1.578807286340254 - }, - { - "x": 2.6141955298766604, - "y": 4.483978019553037, - "heading": -0.6103014306892406, - "angularVelocity": 0.5555183449290747, - "velocityX": -0.4570081038377013, - "velocityY": 0.9524267246290669, - "timestamp": 1.6542866710804687 - }, - { - "x": 2.565181655781626, - "y": 4.572554576780097, - "heading": -0.5589890537241899, - "angularVelocity": 0.679819756618359, - "velocityX": -0.649367695106772, - "velocityY": 1.1735198628071373, - "timestamp": 1.7297660558206833 - }, - { - "x": 2.5016263830570638, - "y": 4.677800410068183, - "heading": -0.49830336002197995, - "angularVelocity": 0.8040035555369035, - "velocityX": -0.842021605572691, - "velocityY": 1.3943652779921414, - "timestamp": 1.805245440560898 - }, - { - "x": 2.423485048210339, - "y": 4.799677415452472, - "heading": -0.42825793280914715, - "angularVelocity": 0.9280073950309187, - "velocityX": -1.0352672470523336, - "velocityY": 1.6147058670562153, - "timestamp": 1.8807248253011126 - }, - { - "x": 2.330623716021553, - "y": 4.938069451098365, - "heading": -0.3489003970727596, - "angularVelocity": 1.0513802677046555, - "velocityX": -1.2302873494790556, - "velocityY": 1.8335077335201706, - "timestamp": 1.9562042100413273 - }, - { - "x": 2.2158437334233922, - "y": 5.072717873356153, - "heading": -0.27145428242562347, - "angularVelocity": 1.026056517505551, - "velocityX": -1.5206798914447361, - "velocityY": 1.7839099076626914, - "timestamp": 2.031683594781542 - }, - { - "x": 2.115226856826334, - "y": 5.190380914478301, - "heading": -0.20366940333084413, - "angularVelocity": 0.8980581827763012, - "velocityX": -1.333037847880913, - "velocityY": 1.5588765267548788, - "timestamp": 2.1071629795217564 - }, - { - "x": 2.028914050823854, - "y": 5.291175476770991, - "heading": -0.1455212111518032, - "angularVelocity": 0.7703850843618669, - "velocityX": -1.1435282136547777, - "velocityY": 1.3353919436695292, - "timestamp": 2.182642364261971 - }, - { - "x": 1.95695123959882, - "y": 5.375140495871242, - "heading": -0.09702855670668924, - "angularVelocity": 0.6424622380441017, - "velocityX": -0.9534101459703443, - "velocityY": 1.1124232052394878, - "timestamp": 2.258121749002185 - }, - { - "x": 1.89936105960217, - "y": 5.44229547054464, - "heading": -0.05821541752658763, - "angularVelocity": 0.5142217217941343, - "velocityX": -0.7629921758539479, - "velocityY": 0.8897127991440635, - "timestamp": 2.3336011337423996 - }, - { - "x": 1.8561570206808413, - "y": 5.492652117133406, - "heading": -0.029101140919542783, - "angularVelocity": 0.38572487981921, - "velocityX": -0.572395218494233, - "velocityY": 0.6671576187743987, - "timestamp": 2.409080518482614 - }, - { - "x": 1.8273481914953718, - "y": 5.526218220226538, - "heading": -0.009696189627478797, - "angularVelocity": 0.2570894206319378, - "velocityX": -0.38167811361857223, - "velocityY": 0.4447055737053434, - "timestamp": 2.4845599032228285 - }, - { - "x": 1.8129411935806279, + "angularVelocity": 0.054926472219667455, + "velocityX": 0.31910406053771284, + "velocityY": 0.061790547746105144, + "timestamp": 1.2792365084888706 + }, + { + "x": 2.6730242117812852, + "y": 4.320207160810404, + "heading": -0.7068522062263088, + "angularVelocity": 0.19509567726207716, + "velocityX": 0.10349358326673892, + "velocityY": 0.31054279257798323, + "timestamp": 1.3551116333379198 + }, + { + "x": 2.6645156104831624, + "y": 4.362642495661916, + "heading": -0.6814188000083412, + "angularVelocity": 0.33520084834820957, + "velocityX": -0.1121395360458166, + "velocityY": 0.5592786164892918, + "timestamp": 1.430986758186969 + }, + { + "x": 2.6396435810539995, + "y": 4.423949055779942, + "heading": -0.6453599487845141, + "angularVelocity": 0.47523943183649325, + "velocityX": -0.32780215490271986, + "velocityY": 0.807992873026772, + "timestamp": 1.5068618830360183 + }, + { + "x": 2.5984050053739445, + "y": 4.5041245094115245, + "heading": -0.5986803757122448, + "angularVelocity": 0.615215766235525, + "velocityX": -0.5435058691760214, + "velocityY": 1.0566763981096883, + "timestamp": 1.5827370078850675 + }, + { + "x": 2.540795163397632, + "y": 4.603165195262531, + "heading": -0.541383229045499, + "angularVelocity": 0.755150608064052, + "velocityX": -0.7592717915230448, + "velocityY": 1.3053116689836102, + "timestamp": 1.6586121327341168 + }, + { + "x": 2.4668060178101583, + "y": 4.721064600465768, + "heading": -0.4734677826566921, + "angularVelocity": 0.8950950199273465, + "velocityX": -0.9751436420635095, + "velocityY": 1.5538611031988796, + "timestamp": 1.734487257583166 + }, + { + "x": 2.3764209643089336, + "y": 4.857808746061696, + "heading": -0.3949264705539342, + "angularVelocity": 1.0351391481588907, + "velocityX": -1.1912343298399446, + "velocityY": 1.8022263010181736, + "timestamp": 1.8103623824322153 + }, + { + "x": 2.269588305263905, + "y": 5.013353141552007, + "heading": -0.3057482687442009, + "angularVelocity": 1.175328567657553, + "velocityX": -1.4080063691172056, + "velocityY": 2.0500051340898513, + "timestamp": 1.8862375072812645 + }, + { + "x": 2.155512806030656, + "y": 5.145840094887286, + "heading": -0.22945454409592414, + "angularVelocity": 1.0055169569743476, + "velocityX": -1.5034637433586158, + "velocityY": 1.7461184228535829, + "timestamp": 1.9621126321303137 + }, + { + "x": 2.0576690248190195, + "y": 5.259344023095087, + "heading": -0.1639803843714035, + "angularVelocity": 0.8629199603276121, + "velocityX": -1.289537004472555, + "velocityY": 1.4959306944600357, + "timestamp": 2.037987756979363 + }, + { + "x": 1.9761088976814785, + "y": 5.353909559223238, + "heading": -0.10935658983885492, + "angularVelocity": 0.7199170300053718, + "velocityX": -1.0749257717858158, + "velocityY": 1.2463312095522094, + "timestamp": 2.113862881828412 + }, + { + "x": 1.9108492511031485, + "y": 5.429551680381221, + "heading": -0.06562174275393094, + "angularVelocity": 0.5764056029164016, + "velocityX": -0.8600927735973349, + "velocityY": 0.9969291162074897, + "timestamp": 2.1897380066774614 + }, + { + "x": 1.8618983395529738, + "y": 5.486277925937523, + "heading": -0.03280716495777854, + "angularVelocity": 0.4324813680566584, + "velocityX": -0.6451509852228532, + "velocityY": 0.7476263883465671, + "timestamp": 2.2656131315265107 + }, + { + "x": 1.8292611562031649, + "y": 5.524092810268815, + "heading": -0.010931847108471086, + "angularVelocity": 0.2883068448697581, + "velocityX": -0.430143389085703, + "velocityY": 0.4983831579385639, + "timestamp": 2.34148825637556 + }, + { + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.24734731593596e-17, - "angularVelocity": 0.12846142905206553, - "velocityX": -0.19087328234026615, - "velocityY": 0.22232623397342577, - "timestamp": 2.560039287963043 + "heading": -1.6776667047563746e-20, + "angularVelocity": 0.1440768253130289, + "velocityX": -0.2150897630156809, + "velocityY": 0.24917859900600148, + "timestamp": 2.417363381224609 }, { - "x": 1.8129411935806274, + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 5.939064303009515e-18, - "angularVelocity": -5.9066042058701954e-18, - "velocityX": -3.4367552729151325e-16, - "velocityY": -1.697930821204867e-16, - "timestamp": 2.6355186727032573 - }, - { - "x": 1.8467557558007979, - "y": 5.543052711491932, - "heading": 5.160367277754293e-18, - "angularVelocity": -8.367176611695404e-18, - "velocityX": 0.36334081801614093, - "velocityY": 0.0005742601437371532, - "timestamp": 2.7285843579868416 - }, - { - "x": 1.914384879084952, - "y": 5.543159599317722, - "heading": 4.087331547357737e-18, - "angularVelocity": -1.1529875132039326e-17, - "velocityX": 0.7266816236089447, - "velocityY": 0.0011485202678393933, - "timestamp": 2.821650043270426 - }, - { - "x": 2.0158285613519547, - "y": 5.543319931052202, - "heading": 2.611621451738615e-18, - "angularVelocity": -1.5856651043003625e-17, - "velocityX": 1.0900224068397422, - "velocityY": 0.0017227803565984868, - "timestamp": 2.91471572855401 - }, - { - "x": 2.1510867977458235, - "y": 5.543533706687698, - "heading": 4.496026882117787e-19, - "angularVelocity": -2.3231105610401644e-17, - "velocityX": 1.4533631378925314, - "velocityY": 0.0022970403628902463, - "timestamp": 3.0077814138375945 - }, - { - "x": 2.32015956398666, - "y": 5.543800926185837, - "heading": -3.8747303229957764e-18, - "angularVelocity": -4.646538622728347e-17, - "velocityX": 1.8167036080554047, - "velocityY": 0.00287129995684554, - "timestamp": 3.1008470991211787 - }, - { - "x": 2.4554178003805287, - "y": 5.544014701821333, - "heading": -2.0140989785604273e-18, - "angularVelocity": 1.9992667960964244e-17, - "velocityX": 1.4533631378925314, - "velocityY": 0.002297040362890341, - "timestamp": 3.193912784404763 - }, - { - "x": 2.556861482647531, - "y": 5.544175033555813, - "heading": -9.161136142082278e-19, - "angularVelocity": 1.1797961418381928e-17, - "velocityX": 1.0900224068397426, - "velocityY": 0.001722780356598613, - "timestamp": 3.2869784696883473 - }, - { - "x": 2.6244906059316855, - "y": 5.544281921381602, - "heading": -2.8482092694126203e-19, - "angularVelocity": 6.783302409926777e-18, - "velocityX": 0.7266816236089451, - "velocityY": 0.0011485202678395388, - "timestamp": 3.3800441549719316 + "heading": 1.1170600847702852e-20, + "angularVelocity": 5.938902420193292e-19, + "velocityX": -2.686779016810018e-17, + "velocityY": -1.2969153357483792e-17, + "timestamp": 2.4932385060736584 + }, + { + "x": 1.8552093951788071, + "y": 5.543066072468525, + "heading": -1.0867013680305264e-17, + "angularVelocity": -1.1051820926914384e-16, + "velocityX": 0.42942883009919236, + "velocityY": 0.0006787122433536914, + "timestamp": 2.5916673926214395 + }, + { + "x": 1.9397457967809342, + "y": 5.543199682246804, + "heading": -3.2635799657096195e-17, + "angularVelocity": -2.2116257473548814e-16, + "velocityX": 0.8588576440015888, + "velocityY": 0.001357424461108283, + "timestamp": 2.6900962791692207 + }, + { + "x": 2.0665503951985436, + "y": 5.5434000969079245, + "heading": -4.603800029490119e-17, + "angularVelocity": -1.3616125416355767e-16, + "velocityX": 1.2882864255103954, + "velocityY": 0.002036136627664645, + "timestamp": 2.788525165717002 + }, + { + "x": 2.235623180866242, + "y": 5.543667316436768, + "heading": -4.798133527402523e-17, + "angularVelocity": -1.9743543259972466e-17, + "velocityX": 1.7177151098384498, + "velocityY": 0.002714848640626296, + "timestamp": 2.886954052264783 + }, + { + "x": 2.40469596653394, + "y": 5.54393453596561, + "heading": -3.9330353852399465e-17, + "angularVelocity": 8.789067645262313e-17, + "velocityX": 1.7177151098384498, + "velocityY": 0.0027148486406302456, + "timestamp": 2.985382938812564 + }, + { + "x": 2.531500564951549, + "y": 5.544134950626731, + "heading": -2.0009563266152006e-17, + "angularVelocity": 1.96291873894211e-16, + "velocityX": 1.2882864255103954, + "velocityY": 0.002036136627666603, + "timestamp": 3.0838118253603453 + }, + { + "x": 2.616036966553676, + "y": 5.54426856040501, + "heading": -7.02269200923652e-18, + "angularVelocity": 1.3194166582538958e-16, + "velocityX": 0.8588576440015888, + "velocityY": 0.0013574244611093242, + "timestamp": 3.1822407119081264 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -1.8432742635406086e-23, - "angularVelocity": 3.0602309898573723e-18, - "velocityX": 0.36334081801614126, - "velocityY": 0.0005742601437373122, - "timestamp": 3.473109840255516 + "heading": -3.8911537114769476e-19, + "angularVelocity": 6.739461219451495e-17, + "velocityX": 0.4294288300991924, + "velocityY": 0.0006787122433541438, + "timestamp": 3.2806695984559076 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -1.9267281699534778e-23, - "angularVelocity": -8.967222433260206e-24, - "velocityX": 3.3495247448012114e-21, - "velocityY": 4.428006153501746e-23, - "timestamp": 3.5661755255391 - }, - { - "x": 2.6433995634924674, - "y": 5.563703580385797, - "heading": 0.004845900450156499, - "angularVelocity": 0.06121219104231838, - "velocityX": -0.18828383484070044, - "velocityY": 0.24465440313021314, - "timestamp": 3.6453411356510848 - }, - { - "x": 2.613808012454941, - "y": 5.602604430475708, - "heading": 0.01472246361068666, - "angularVelocity": 0.12475825230878844, - "velocityX": -0.3737930017297544, - "velocityY": 0.4913857170415789, - "timestamp": 3.7245067457630694 - }, - { - "x": 2.5698581719130473, - "y": 5.661275942603098, - "heading": 0.02990473352989552, - "angularVelocity": 0.19177860055310114, - "velocityX": -0.5551632897128446, - "velocityY": 0.7411237284017032, - "timestamp": 3.803672355875054 - }, - { - "x": 2.5120904369033115, - "y": 5.740092575087619, - "heading": 0.05083960434186745, - "angularVelocity": 0.26444400267184504, - "velocityX": -0.7297074440280278, - "velocityY": 0.995591802716233, - "timestamp": 3.8828379659870387 - }, - { - "x": 2.441559614817569, - "y": 5.839724514645118, - "heading": 0.07837099812745103, - "angularVelocity": 0.3477696154509352, - "velocityX": -0.8909275376766786, - "velocityY": 1.2585255064233427, - "timestamp": 3.9620035760990233 - }, - { - "x": 2.3611631890754614, - "y": 5.961650149114452, - "heading": 0.11464704720334498, - "angularVelocity": 0.458229893315787, - "velocityX": -1.0155473522958047, - "velocityY": 1.5401338320624665, - "timestamp": 4.041169186211008 - }, - { - "x": 2.2902542502823713, - "y": 6.1057913956854275, - "heading": 0.16704371336362323, - "angularVelocity": 0.6618614583549582, - "velocityX": -0.8957038124608021, - "velocityY": 1.8207558353567825, - "timestamp": 4.120334796322993 - }, - { - "x": 2.241416295543653, - "y": 6.2393929892272615, - "heading": 0.22223140963064517, - "angularVelocity": 0.6971170460122206, - "velocityX": -0.6169087141453696, - "velocityY": 1.6876215992379315, - "timestamp": 4.199500406434978 - }, - { - "x": 2.211878843837597, - "y": 6.357956546172331, - "heading": 0.276701736402732, - "angularVelocity": 0.6880554156664132, - "velocityX": -0.3731096326330706, - "velocityY": 1.497664917599366, - "timestamp": 4.278666016546962 - }, - { - "x": 2.200573473010042, - "y": 6.460208388316336, - "heading": 0.32930652128307925, - "angularVelocity": 0.6644903614831552, - "velocityX": -0.14280659002770119, - "velocityY": 1.291619454449523, - "timestamp": 4.357831626658947 - }, - { - "x": 2.206951813042022, - "y": 6.545559671097223, - "heading": 0.3794800427949416, - "angularVelocity": 0.6337792564333018, - "velocityX": 0.08056958094502174, - "velocityY": 1.0781358554573408, - "timestamp": 4.436997236770932 - }, - { - "x": 2.230681954112334, - "y": 6.613672661059127, - "heading": 0.42688445047126194, - "angularVelocity": 0.598800509580661, - "velocityX": 0.2997531508535616, - "velocityY": 0.8603860927182075, - "timestamp": 4.516162846882916 - }, - { - "x": 2.271541794524346, - "y": 6.6643287076043505, - "heading": 0.47129474601819377, - "angularVelocity": 0.5609796410854497, - "velocityX": 0.5161311882042349, - "velocityY": 0.6398743908316677, - "timestamp": 4.595328456994901 + "heading": -4.27312910681112e-19, + "angularVelocity": -3.880724585667238e-19, + "velocityX": -2.3908482729518656e-18, + "velocityY": 6.838225330043828e-18, + "timestamp": 3.3790984850036887 + }, + { + "x": 2.6438893629989613, + "y": 5.563919104052164, + "heading": 0.003139586303789949, + "angularVelocity": 0.042042846518139325, + "velocityX": -0.19304501448070868, + "velocityY": 0.26224987725536203, + "timestamp": 3.453774358875246 + }, + { + "x": 2.6152669032432256, + "y": 5.6032366820496495, + "heading": 0.009644303445155527, + "angularVelocity": 0.0871060063194363, + "velocityX": -0.38328925088933385, + "velocityY": 0.5265097809918172, + "timestamp": 3.5284502327468035 + }, + { + "x": 2.5727487902328847, + "y": 5.662504910556293, + "heading": 0.019847811829768587, + "angularVelocity": 0.13663728130135686, + "velocityX": -0.5693688042201159, + "velocityY": 0.7936730490571009, + "timestamp": 3.603126106618361 + }, + { + "x": 2.5168455054942007, + "y": 5.742063733198033, + "heading": 0.03429127316793741, + "angularVelocity": 0.19341536415109115, + "velocityX": -0.7486123943435556, + "velocityY": 1.0653885721991296, + "timestamp": 3.6778019804899182 + }, + { + "x": 2.4485453653591587, + "y": 5.842518348175502, + "heading": 0.05399784068706335, + "angularVelocity": 0.2638947024981649, + "velocityX": -0.9146212370077963, + "velocityY": 1.3452084290335973, + "timestamp": 3.7524778543614756 + }, + { + "x": 2.370517685528427, + "y": 5.96519344251776, + "heading": 0.08158842838664396, + "angularVelocity": 0.369471239761256, + "velocityX": -1.0448847236115244, + "velocityY": 1.642767442578038, + "timestamp": 3.827153728233033 + }, + { + "x": 2.3003293154277267, + "y": 6.110432122884094, + "heading": 0.1284873855211158, + "angularVelocity": 0.6280335897392839, + "velocityX": -0.9399069131942741, + "velocityY": 1.94492106802986, + "timestamp": 3.9018296021045904 + }, + { + "x": 2.2517603849650345, + "y": 6.244622565856429, + "heading": 0.18176076028404833, + "angularVelocity": 0.7133947284575884, + "velocityX": -0.6503965463628967, + "velocityY": 1.796971846665519, + "timestamp": 3.976505475976148 + }, + { + "x": 2.22192407028561, + "y": 6.363296472335711, + "heading": 0.23706248397325824, + "angularVelocity": 0.7405567664909981, + "velocityX": -0.39954423206005096, + "velocityY": 1.5891867122091021, + "timestamp": 4.051181349847705 + }, + { + "x": 2.2097448289139274, + "y": 6.465233576489708, + "heading": 0.2929845885143906, + "angularVelocity": 0.7488644141924427, + "velocityX": -0.16309472846117115, + "velocityY": 1.3650607467858884, + "timestamp": 4.125857223719263 + }, + { + "x": 2.2146777318065234, + "y": 6.549876478070479, + "heading": 0.348839415183568, + "angularVelocity": 0.747963482359082, + "velocityX": 0.06605751813605938, + "velocityY": 1.1334705198944957, + "timestamp": 4.20053309759082 + }, + { + "x": 2.236395128424509, + "y": 6.61690736376859, + "heading": 0.4042192057980219, + "angularVelocity": 0.7416021767580177, + "velocityX": 0.2908221289159501, + "velocityY": 0.8976243895505626, + "timestamp": 4.275208971462377 + }, + { + "x": 2.274678638341779, + "y": 6.666121192465354, + "heading": 0.45885367997336884, + "angularVelocity": 0.7316214908889929, + "velocityX": 0.5126623624534666, + "velocityY": 0.6590325113759182, + "timestamp": 4.349884845333935 }, { "x": 2.3293724060058594, "y": 6.6973748207092285, "heading": 0.5125504196, - "angularVelocity": 0.521131252869112, - "velocityX": 0.7305016837451085, - "velocityY": 0.4174301575915678, - "timestamp": 4.6744940671068855 + "angularVelocity": 0.719064094502462, + "velocityX": 0.7324155022029422, + "velocityY": 0.41852376977375944, + "timestamp": 4.424560719205492 }, { - "x": 2.3973241454559178, - "y": 6.736204444156279, - "heading": 0.5391342040901619, - "angularVelocity": 0.37821212966035084, - "velocityX": 0.9667612262295852, - "velocityY": 0.5524358122619253, - "timestamp": 4.744782094394734 + "x": 2.4110936768539344, + "y": 6.744072759574757, + "heading": 0.5643107226604884, + "angularVelocity": 0.653664165942785, + "velocityX": 1.0320315606781885, + "velocityY": 0.5897332019890127, + "timestamp": 4.503745575523619 }, { - "x": 2.48097335716082, - "y": 6.784004065158346, - "heading": 0.5372874666531787, - "angularVelocity": -0.026273854996956482, - "velocityX": 1.1900918966232585, - "velocityY": 0.6800535289789073, - "timestamp": 4.815070121682582 + "x": 2.50599401367539, + "y": 6.798301604540289, + "heading": 0.5436072588792712, + "angularVelocity": -0.2614573637418783, + "velocityX": 1.198465732389427, + "velocityY": 0.6848385851414024, + "timestamp": 4.5829304318417465 }, { - "x": 2.54793079875253, - "y": 6.822265517551258, - "heading": 0.5291862310389086, - "angularVelocity": -0.11525768935146737, - "velocityX": 0.952615177510983, - "velocityY": 0.5443523437671908, - "timestamp": 4.88535814897043 + "x": 2.577169265515479, + "y": 6.838973237821007, + "heading": 0.528079022912807, + "angularVelocity": -0.19610108155123726, + "velocityX": 0.8988492894921869, + "velocityY": 0.51362893325611, + "timestamp": 4.662115288159874 }, { - "x": 2.598141729775772, - "y": 6.850957521028096, - "heading": 0.5214396908605601, - "angularVelocity": -0.11021137563904597, - "velocityX": 0.7143596564122447, - "velocityY": 0.40820612818364127, - "timestamp": 4.955646176258278 - }, - { - "x": 2.631611382674506, - "y": 6.870083065561309, - "heading": 0.5156519889527617, - "angularVelocity": -0.08234264256835963, - "velocityX": 0.4761785782046116, - "velocityY": 0.2721024514586011, - "timestamp": 5.025934203546126 + "x": 2.6246194330616937, + "y": 6.8660876598097635, + "heading": 0.5177266590936774, + "angularVelocity": -0.13073666229232678, + "velocityX": 0.5992328552770498, + "velocityY": 0.3424192863320269, + "timestamp": 4.741300144478001 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.044126567104521564, - "velocityX": 0.23806521146364917, - "velocityY": 0.13603746705807154, - "timestamp": 5.096222230833974 + "angularVelocity": -0.06536905835734146, + "velocityX": 0.2996164266200211, + "velocityY": 0.1712096425840104, + "timestamp": 4.820485000796128 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -7.689800874803388e-26, - "velocityX": 2.07998453448468e-19, - "velocityY": -3.6391739324491333e-19, - "timestamp": 5.166510258121822 + "angularVelocity": -8.381816905081638e-20, + "velocityX": -7.007327358505968e-19, + "velocityY": 8.954594613208896e-19, + "timestamp": 4.899669857114255 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADEF.traj b/src/main/deploy/choreo/CenterLanePCBADEF.traj deleted file mode 100644 index 3ba43407..00000000 --- a/src/main/deploy/choreo/CenterLanePCBADEF.traj +++ /dev/null @@ -1,1912 +0,0 @@ -{ - "samples": [ - { - "x": 1.3078742027282717, - "y": 5.537254810333252, - "heading": 5.4444853034280076e-17, - "angularVelocity": -1.7134882602513402e-15, - "velocityX": 6.870651213375127e-14, - "velocityY": -6.867248083045425e-14, - "timestamp": 0 - }, - { - "x": 1.327170103173586, - "y": 5.517955423153612, - "heading": -0.008175377183837335, - "angularVelocity": -0.09765213543106893, - "velocityX": 0.23048305187379453, - "velocityY": -0.23052469974457607, - "timestamp": 0.08371938972710291 - }, - { - "x": 1.3657619180934388, - "y": 5.479356717010402, - "heading": -0.024521335360667328, - "angularVelocity": -0.19524698197290347, - "velocityX": 0.4609662713219617, - "velocityY": -0.4610485846711018, - "timestamp": 0.16743877945420582 - }, - { - "x": 1.423649754196992, - "y": 5.421458744466686, - "heading": -0.04902545949966477, - "angularVelocity": -0.2926935351401013, - "velocityX": 0.6914507653752389, - "velocityY": -0.6915718417495008, - "timestamp": 0.25115816918130873 - }, - { - "x": 1.500833817418447, - "y": 5.344261559488766, - "heading": -0.08166832650675512, - "angularVelocity": -0.3899080859702319, - "velocityX": 0.9219377192434074, - "velocityY": -0.9220944542185107, - "timestamp": 0.33487755890841164 - }, - { - "x": 1.5973144196195872, - "y": 5.247765239032123, - "heading": -0.1224243942133242, - "angularVelocity": -0.48681754417250844, - "velocityX": 1.1524283981958594, - "velocityY": -1.1526161474801648, - "timestamp": 0.41859694863551455 - }, - { - "x": 1.71309198631675, - "y": 5.131969908089758, - "heading": -0.17126309032246767, - "angularVelocity": -0.5833618265534611, - "velocityX": 1.3829241597980892, - "velocityY": -1.3831363477423682, - "timestamp": 0.5023163383626175 - }, - { - "x": 1.84816708196567, - "y": 4.996875770190428, - "heading": -0.22814932169203733, - "angularVelocity": -0.6794869331346065, - "velocityX": 1.6134266636345465, - "velocityY": -1.613654116921907, - "timestamp": 0.5860357280897204 - }, - { - "x": 2.0025627636590344, - "y": 4.842495557885179, - "heading": -0.2918324673553054, - "angularVelocity": -0.7606737921866519, - "velocityX": 1.8442045767013158, - "velocityY": -1.8440198000543986, - "timestamp": 0.6697551178168233 - }, - { - "x": 2.137660080489021, - "y": 4.707413614165848, - "heading": -0.34751647944173375, - "angularVelocity": -0.6651268274642137, - "velocityX": 1.6136920881812329, - "velocityY": -1.613508461536256, - "timestamp": 0.7534745075439262 - }, - { - "x": 2.2534584850572914, - "y": 4.591629667252782, - "heading": -0.3952239272052681, - "angularVelocity": -0.569849444902123, - "velocityX": 1.3831730611717787, - "velocityY": -1.3830003693347765, - "timestamp": 0.8371938972710291 - }, - { - "x": 2.3499575912437507, - "y": 4.495143461778056, - "heading": -0.4349689949602177, - "angularVelocity": -0.4747414892118174, - "velocityX": 1.15264942208745, - "velocityY": -1.1524953274174494, - "timestamp": 0.920913286998132 - }, - { - "x": 2.4271571129829432, - "y": 4.417954772578404, - "heading": -0.46676067803032645, - "angularVelocity": -0.3797409796432922, - "velocityX": 0.9221223660473082, - "velocityY": -0.9219929749997143, - "timestamp": 1.004632676725235 - }, - { - "x": 2.485056851699902, - "y": 4.3600634144135135, - "heading": -0.49060405496538323, - "angularVelocity": -0.28480113164677934, - "velocityX": 0.6915929380958452, - "velocityY": -0.691492835215309, - "timestamp": 1.0883520664523378 - }, - { - "x": 2.523656690441612, - "y": 4.321469248073772, - "heading": -0.5065010886126352, - "angularVelocity": -0.1898847292015727, - "velocityX": 0.4610621131799047, - "velocityY": -0.460994358243006, - "timestamp": 1.1720714561794408 - }, - { - "x": 2.5429565906524716, - "y": 4.302172183990473, - "heading": -0.514451122232844, - "angularVelocity": -0.09496048222667672, - "velocityX": 0.23053082773023703, - "velocityY": -0.23049695113880436, - "timestamp": 1.2557908459065437 - }, - { - "x": 2.542956590652466, - "y": 4.3021721839904785, - "heading": -0.5144511222328438, - "angularVelocity": 1.7361053619799633e-15, - "velocityX": -6.870229148895338e-14, - "velocityY": 6.86710519982077e-14, - "timestamp": 1.3395102356336466 - }, - { - "x": 2.5305584868573745, - "y": 4.308773452126341, - "heading": -0.5122554014922367, - "angularVelocity": 0.03659296401704725, - "velocityX": -0.20662161524599887, - "velocityY": 0.11001397531805143, - "timestamp": 1.399514140744592 - }, - { - "x": 2.5059487154966082, - "y": 4.322311826419086, - "heading": -0.50771784412737, - "angularVelocity": 0.07562103427230116, - "velocityX": -0.4101361622258381, - "velocityY": 0.22562488670882788, - "timestamp": 1.4595180458555372 - }, - { - "x": 2.4694009892818585, - "y": 4.343244277958484, - "heading": -0.5006377479710926, - "angularVelocity": 0.11799392294862207, - "velocityX": -0.6090891275688425, - "velocityY": 0.3488514872606191, - "timestamp": 1.5195219509664826 - }, - { - "x": 2.4213500625117965, - "y": 4.372224481954429, - "heading": -0.49072373538892167, - "angularVelocity": 0.16522278948078847, - "velocityX": -0.800796659504371, - "velocityY": 0.48297196561394073, - "timestamp": 1.579525856077428 - }, - { - "x": 2.36256927737863, - "y": 4.410248403268193, - "heading": -0.4775208921950913, - "angularVelocity": 0.2200330656716209, - "velocityX": -0.9796159937337826, - "velocityY": 0.6336907780161798, - "timestamp": 1.6395297611883732 - }, - { - "x": 2.294685044293501, - "y": 4.45893547211586, - "heading": -0.46025532607703995, - "angularVelocity": 0.28774070764440873, - "velocityX": -1.1313302519163222, - "velocityY": 0.8113983374522957, - "timestamp": 1.6995336662993186 - }, - { - "x": 2.2219430126998723, - "y": 4.5207468069818875, - "heading": -0.4375774026440522, - "angularVelocity": 0.3779407922043896, - "velocityX": -1.2122882912225668, - "velocityY": 1.0301218687640459, - "timestamp": 1.759537571410264 - }, - { - "x": 2.1543482605183306, - "y": 4.595554934892757, - "heading": -0.4086588228612356, - "angularVelocity": 0.48194496223782435, - "velocityX": -1.1265058841847213, - "velocityY": 1.2467209887848296, - "timestamp": 1.8195414765212092 - }, - { - "x": 2.099014749930773, - "y": 4.677184514185738, - "heading": -0.3762413759174227, - "angularVelocity": 0.5402556197613143, - "velocityX": -0.9221651571718094, - "velocityY": 1.3604044460448101, - "timestamp": 1.8795453816321546 - }, - { - "x": 2.057477466278187, - "y": 4.7614747474435095, - "heading": -0.3421230482764429, - "angularVelocity": 0.5686017864653305, - "velocityX": -0.692243006114092, - "velocityY": 1.4047457928266707, - "timestamp": 1.9395492867431 - }, - { - "x": 2.029984208014376, - "y": 4.846326580474425, - "heading": -0.30718701619967975, - "angularVelocity": 0.5822293067787422, - "velocityX": -0.4581911496089643, - "velocityY": 1.4141051798883366, - "timestamp": 1.9995531918540452 - }, - { - "x": 2.0165337281988327, - "y": 4.930557757850057, - "heading": -0.2719247119892122, - "angularVelocity": 0.5876668217721605, - "velocityX": -0.2241600740930749, - "velocityY": 1.4037615921812139, - "timestamp": 2.0595570969649906 - }, - { - "x": 2.017073631286621, - "y": 5.013424873352051, - "heading": -0.23664369858950388, - "angularVelocity": 0.58797862129931, - "velocityX": 0.008997799173072052, - "velocityY": 1.3810287071945508, - "timestamp": 2.119561002075936 - }, - { - "x": 2.0456987990727344, - "y": 5.12687213061024, - "heading": -0.1871316885421809, - "angularVelocity": 0.5834829718812528, - "velocityX": 0.3373383135622466, - "velocityY": 1.336939113429588, - "timestamp": 2.2044169660600903 - }, - { - "x": 2.101800262534532, - "y": 5.2343841203738455, - "heading": -0.13890470594146578, - "angularVelocity": 0.5683393404112499, - "velocityX": 0.6611375421092794, - "velocityY": 1.2669939119857474, - "timestamp": 2.2892729300442447 - }, - { - "x": 2.1841232780581077, - "y": 5.331790166507821, - "heading": -0.09372578148319055, - "angularVelocity": 0.5324189642900253, - "velocityX": 0.9701500243276792, - "velocityY": 1.1478986456646028, - "timestamp": 2.374128894028399 - }, - { - "x": 2.2868206827051014, - "y": 5.409909264516836, - "heading": -0.05582004781301006, - "angularVelocity": 0.4467067709849929, - "velocityX": 1.2102555887076, - "velocityY": 0.9206082205795442, - "timestamp": 2.4589848580125535 - }, - { - "x": 2.385467579367521, - "y": 5.4603756892360344, - "heading": -0.030726479319374042, - "angularVelocity": 0.2957195618957538, - "velocityX": 1.1625216664893496, - "velocityY": 0.5947304390840709, - "timestamp": 2.543840821996708 - }, - { - "x": 2.463261391908942, - "y": 5.492112879652653, - "heading": -0.014444831338496888, - "angularVelocity": 0.1918739380995954, - "velocityX": 0.916774836898297, - "velocityY": 0.3740124904189999, - "timestamp": 2.6286967859808623 - }, - { - "x": 2.5161956459202934, - "y": 5.510821266131426, - "heading": -0.0045926513861969465, - "angularVelocity": 0.11610474372950008, - "velocityX": 0.6238130064875114, - "velocityY": 0.22047226382658144, - "timestamp": 2.7135527499650167 - }, - { - "x": 2.542956590652466, - "y": 5.519354820251465, - "heading": -1.9593529845154978e-18, - "angularVelocity": 0.05412290628216246, - "velocityX": 0.3153690498073838, - "velocityY": 0.10056516618716548, - "timestamp": 2.798408713949171 - }, - { - "x": 2.542956590652466, - "y": 5.519354820251465, - "heading": -3.879161841617201e-19, - "angularVelocity": 1.851886937303712e-17, - "velocityX": 1.8638350784782603e-17, - "velocityY": 7.390265321449622e-16, - "timestamp": 2.8832646779333255 - }, - { - "x": 2.54352414805122, - "y": 5.5417247332646085, - "heading": 0.010369864258801478, - "angularVelocity": 0.1365642797123967, - "velocityX": 0.007474356984997478, - "velocityY": 0.294597015122544, - "timestamp": 2.9591986211331163 - }, - { - "x": 2.5446584508658887, - "y": 5.5864645661514665, - "heading": 0.031108645745408125, - "angularVelocity": 0.27311608765055906, - "velocityX": 0.01493802069101894, - "velocityY": 0.5891941205943013, - "timestamp": 3.035132564332907 - }, - { - "x": 2.5463582976769072, - "y": 5.6535744095701785, - "heading": 0.062210562054685774, - "angularVelocity": 0.4095917451230601, - "velocityX": 0.022385862492957306, - "velocityY": 0.8837924199740047, - "timestamp": 3.1110665075326978 - }, - { - "x": 2.5486221305372125, - "y": 5.743054499123618, - "heading": 0.10366116200111275, - "angularVelocity": 0.545877090003949, - "velocityX": 0.029813187158595614, - "velocityY": 1.1783938220883299, - "timestamp": 3.1870004507324885 - }, - { - "x": 2.551448101584002, - "y": 5.8549052924787315, - "heading": 0.15543248064293438, - "angularVelocity": 0.6817941550276853, - "velocityX": 0.03721617668864778, - "velocityY": 1.473001251374778, - "timestamp": 3.2629343939322792 - }, - { - "x": 2.5548341868258384, - "y": 5.989127553824463, - "heading": 0.2174776706164971, - "angularVelocity": 0.8170942711392554, - "velocityX": 0.04459251158506785, - "velocityY": 1.7676187445261111, - "timestamp": 3.33886833713207 - }, - { - "x": 2.5587783438313068, - "y": 6.145722446236129, - "heading": 0.28972525218567036, - "angularVelocity": 0.9514530462231059, - "velocityX": 0.05194194900547891, - "velocityY": 2.0622515546130344, - "timestamp": 3.4148022803318607 - }, - { - "x": 2.563218286954705, - "y": 6.324743248037854, - "heading": 0.36865194803097623, - "angularVelocity": 1.039412580453524, - "velocityX": 0.058471125511242075, - "velocityY": 2.357586005124216, - "timestamp": 3.4907362235316515 - }, - { - "x": 2.5671026763479077, - "y": 6.481388915794399, - "heading": 0.43754245115183227, - "angularVelocity": 0.9072425349964675, - "velocityX": 0.05115484893208649, - "velocityY": 2.0629202324498515, - "timestamp": 3.566670166731442 - }, - { - "x": 2.5704320555482925, - "y": 6.615658176157516, - "heading": 0.4964771667239727, - "angularVelocity": 0.7761313727258498, - "velocityX": 0.04384573038206363, - "velocityY": 1.7682376906180024, - "timestamp": 3.642604109931233 - }, - { - "x": 2.573206618251443, - "y": 6.727550075342484, - "heading": 0.5455200541615356, - "angularVelocity": 0.6458625137973618, - "velocityX": 0.03653916267524383, - "velocityY": 1.4735425880698358, - "timestamp": 3.7185380531310237 - }, - { - "x": 2.575426370371336, - "y": 6.8170639437544045, - "heading": 0.5847189679540402, - "angularVelocity": 0.5162238669651036, - "velocityX": 0.02923267285162808, - "velocityY": 1.1788386673980507, - "timestamp": 3.7944719963308144 - }, - { - "x": 2.577091240828593, - "y": 6.884199374047621, - "heading": 0.6141061452751087, - "angularVelocity": 0.38700976246877483, - "velocityX": 0.021925246959405044, - "velocityY": 0.884129382252347, - "timestamp": 3.870405939530605 - }, - { - "x": 2.5782011573891666, - "y": 6.928956199415487, - "heading": 0.6336989699349357, - "angularVelocity": 0.25802459129872973, - "velocityX": 0.014616869792382615, - "velocityY": 0.5894179003730387, - "timestamp": 3.946339882730396 - }, - { - "x": 2.578756093978882, - "y": 6.951334476470947, - "heading": 0.643500664804274, - "angularVelocity": 0.1290818632129897, - "velocityX": 0.007308149245661792, - "velocityY": 0.2947071640488953, - "timestamp": 4.022273825930187 - }, - { - "x": 2.578756093978882, - "y": 6.951334476470947, - "heading": 0.643500664804274, - "angularVelocity": -1.7225163541747852e-17, - "velocityX": -8.122490732634945e-17, - "velocityY": -7.511091870024743e-16, - "timestamp": 4.098207769129978 - }, - { - "x": 2.605882805940747, - "y": 6.95657005522081, - "heading": 0.6386264747117201, - "angularVelocity": -0.05791461301218251, - "velocityX": 0.3223167327766511, - "velocityY": 0.06220859495330271, - "timestamp": 4.182369431257094 - }, - { - "x": 2.6601362026337743, - "y": 6.967041240264821, - "heading": 0.6288779935837058, - "angularVelocity": -0.11583042541734088, - "velocityX": 0.6446331420009866, - "velocityY": 0.12441751718491452, - "timestamp": 4.266531093384209 - }, - { - "x": 2.7415162653471867, - "y": 6.98274808058871, - "heading": 0.6142571418365809, - "angularVelocity": -0.17372341963781532, - "velocityX": 0.966949328905814, - "velocityY": 0.1866270214597963, - "timestamp": 4.350692755511325 - }, - { - "x": 2.8500229829470065, - "y": 7.003690647779458, - "heading": 0.594767720039105, - "angularVelocity": -0.23157125590081054, - "velocityX": 1.2892653835179018, - "velocityY": 0.24883737632363517, - "timestamp": 4.434854417638441 - }, - { - "x": 2.9856563505646374, - "y": 7.029869037377032, - "heading": 0.5704152085572207, - "angularVelocity": -0.28935397503322835, - "velocityX": 1.6115813802818453, - "velocityY": 0.3110488663832933, - "timestamp": 4.5190160797655565 - }, - { - "x": 3.1484163679588817, - "y": 7.06128337029242, - "heading": 0.5412065071898937, - "angularVelocity": -0.3470547114814702, - "velocityX": 1.933897374179897, - "velocityY": 0.37326179309459206, - "timestamp": 4.603177741892672 - }, - { - "x": 3.3383030376303036, - "y": 7.097933794128878, - "heading": 0.5071496172056923, - "angularVelocity": -0.40466037769979885, - "velocityX": 2.2562133977893946, - "velocityY": 0.4354764736121926, - "timestamp": 4.687339404019788 - }, - { - "x": 3.555316362844474, - "y": 7.139820484190559, - "heading": 0.4682532762402473, - "angularVelocity": -0.46216222425238096, - "velocityX": 2.578529460200051, - "velocityY": 0.49769323707530094, - "timestamp": 4.771501066146904 - }, - { - "x": 3.799456345915655, - "y": 7.186943643856573, - "heading": 0.424526577651669, - "angularVelocity": -0.5195560244822011, - "velocityX": 2.9008455501085413, - "velocityY": 0.5599124170675296, - "timestamp": 4.8556627282740195 - }, - { - "x": 4.07072298772812, - "y": 7.239303503682716, - "heading": 0.37597867364929377, - "angularVelocity": -0.5768410791251948, - "velocityX": 3.2231616505238603, - "velocityY": 0.6221343364994486, - "timestamp": 4.939824390401135 - }, - { - "x": 4.369116292263885, - "y": 7.296900317207954, - "heading": 0.32261896114654104, - "angularVelocity": -0.634014480633234, - "velocityX": 3.5454777982530907, - "velocityY": 0.6843592684546252, - "timestamp": 5.023986052528251 - }, - { - "x": 4.69463630229783, - "y": 7.359734340352348, - "heading": 0.2644605433432946, - "angularVelocity": -0.6910321913011374, - "velocityX": 3.8677944542289113, - "velocityY": 0.7465872412250016, - "timestamp": 5.108147714655367 - }, - { - "x": 5.043878143874846, - "y": 7.42716808821782, - "heading": 0.26446053600249464, - "angularVelocity": -8.722261133391044e-8, - "velocityX": 4.149654756693505, - "velocityY": 0.8012406856179081, - "timestamp": 5.192309376782482 - }, - { - "x": 5.3943988333147095, - "y": 7.487603347084882, - "heading": 0.2644605286726163, - "angularVelocity": -8.709284181708098e-8, - "velocityX": 4.164849892228206, - "velocityY": 0.7180853768760186, - "timestamp": 5.276471038909598 - }, - { - "x": 5.748554706480114, - "y": 7.520626966261676, - "heading": 0.26445951238259063, - "angularVelocity": -0.000012075450982753597, - "velocityX": 4.2080427621604946, - "velocityY": 0.3923831628576489, - "timestamp": 5.360632701036714 - }, - { - "x": 6.076359506533188, - "y": 7.546997553632611, - "heading": 0.24369373540901995, - "angularVelocity": -0.24673677359421306, - "velocityX": 3.894942088453133, - "velocityY": 0.3133325400715727, - "timestamp": 5.44479436316383 - }, - { - "x": 6.37718666957215, - "y": 7.567469602956491, - "heading": 0.21707426667956767, - "angularVelocity": -0.31628972214506673, - "velocityX": 3.574396648495391, - "velocityY": 0.24324673261514576, - "timestamp": 5.528956025290945 - }, - { - "x": 6.650951360645961, - "y": 7.582275601726408, - "heading": 0.18847076354087783, - "angularVelocity": -0.33986380990774573, - "velocityX": 3.2528432085897303, - "velocityY": 0.17592331705087103, - "timestamp": 5.613117687418061 - }, - { - "x": 6.897625912379422, - "y": 7.591527252808584, - "heading": 0.15968573269508482, - "angularVelocity": -0.3420207029932105, - "velocityX": 2.9309610278477014, - "velocityY": 0.1099271431712301, - "timestamp": 5.697279349545177 - }, - { - "x": 7.1171977964953355, - "y": 7.595290076757919, - "heading": 0.13176491488072156, - "angularVelocity": -0.3317522148290329, - "velocityX": 2.6089299874362895, - "velocityY": 0.04470947761999478, - "timestamp": 5.7814410116722925 - }, - { - "x": 7.309660237553315, - "y": 7.5936071064013415, - "heading": 0.10539226300609161, - "angularVelocity": -0.3133570702869136, - "velocityX": 2.286818441956269, - "velocityY": -0.019996876416657455, - "timestamp": 5.865602673799408 - }, - { - "x": 7.475009136714085, - "y": 7.586508749225222, - "heading": 0.0810505950391384, - "angularVelocity": -0.28922513353156276, - "velocityX": 1.9646581944999189, - "velocityY": -0.08434193190479512, - "timestamp": 5.949764335926524 - }, - { - "x": 7.613241814863639, - "y": 7.574017622800665, - "heading": 0.059099291100008985, - "angularVelocity": -0.26082308006197424, - "velocityX": 1.642466114093265, - "velocityY": -0.14841824779661225, - "timestamp": 6.03392599805364 - }, - { - "x": 7.724356418372183, - "y": 7.556151203473209, - "heading": 0.03981643413778623, - "angularVelocity": -0.2291168742972126, - "velocityX": 1.3202520090527614, - "velocityY": -0.21228691159250246, - "timestamp": 6.1180876601807554 - }, - { - "x": 7.80835160711804, - "y": 7.5329233986954955, - "heading": 0.023423620240304886, - "angularVelocity": -0.1947776871697487, - "velocityX": 0.998021980827717, - "velocityY": -0.27599032850171645, - "timestamp": 6.202249322307871 - }, - { - "x": 7.865226377203999, - "y": 7.5043455392513225, - "heading": 0.010101499773974827, - "angularVelocity": -0.15829203142647919, - "velocityX": 0.6757800244018113, - "velocityY": -0.3395591142319701, - "timestamp": 6.286410984434987 - }, - { - "x": 7.894979953765869, - "y": 7.4704270362854, - "heading": -6.199321830779289e-18, - "angularVelocity": -0.12002495576570035, - "velocityX": 0.35352886112124365, - "velocityY": -0.4030160777325445, - "timestamp": 6.370572646562103 - }, - { - "x": 7.894584049642754, - "y": 7.426198672978825, - "heading": -0.007089479197593018, - "angularVelocity": -0.07587613336913762, - "velocityX": -0.004237218730680758, - "velocityY": -0.4733601861879681, - "timestamp": 6.464007550039625 - }, - { - "x": 7.86075117401306, - "y": 7.375444727785397, - "heading": -0.010043840448864623, - "angularVelocity": -0.03161946062246776, - "velocityX": -0.3621010390173313, - "velocityY": -0.5432011304601843, - "timestamp": 6.557442453517147 - }, - { - "x": 7.793469713318101, - "y": 7.318225470876191, - "heading": -0.00885143445531551, - "angularVelocity": 0.012761890355416958, - "velocityX": -0.7200891550248647, - "velocityY": -0.6123970248759323, - "timestamp": 6.65087735699467 - }, - { - "x": 7.692724412768832, - "y": 7.254620943238055, - "heading": -0.0034978695073770924, - "angularVelocity": 0.057297270598950276, - "velocityX": -1.078240537525724, - "velocityY": -0.6807362695401956, - "timestamp": 6.744312260472192 - }, - { - "x": 7.558494360533822, - "y": 7.184742577510254, - "heading": 0.006036114483891398, - "angularVelocity": 0.10203878461288343, - "velocityX": -1.4366157317998562, - "velocityY": -0.747882890943545, - "timestamp": 6.8377471639497145 - }, - { - "x": 7.390749153984831, - "y": 7.108756149139622, - "heading": 0.019778831050343372, - "angularVelocity": 0.14708332812435573, - "velocityX": -1.795316314415032, - "velocityY": -0.8132552776587466, - "timestamp": 6.931182067427237 - }, - { - "x": 7.189440632055284, - "y": 7.026934307092558, - "heading": 0.03777709910485055, - "angularVelocity": 0.19262895753766113, - "velocityX": -2.1545323475180176, - "velocityY": -0.8757096010351999, - "timestamp": 7.024616970904759 - }, - { - "x": 6.954481412279556, - "y": 6.93980890428099, - "heading": 0.06012313662850344, - "angularVelocity": 0.2391615626705131, - "velocityX": -2.514683603566329, - "velocityY": -0.93247169493281, - "timestamp": 7.118051874382282 - }, - { - "x": 6.685672117824161, - "y": 6.848872549717405, - "heading": 0.08707457656091541, - "angularVelocity": 0.2884515200349684, - "velocityX": -2.8769687177989223, - "velocityY": -0.9732589340712626, - "timestamp": 7.211486777859804 - }, - { - "x": 6.387993943545244, - "y": 6.775960862611549, - "heading": 0.12150620101602463, - "angularVelocity": 0.3685092312787836, - "velocityX": -3.1859419039323758, - "velocityY": -0.780347433262947, - "timestamp": 7.304921681337326 - }, - { - "x": 6.122643180061621, - "y": 6.713816410019983, - "heading": 0.15262946767199945, - "angularVelocity": 0.3331010735561154, - "velocityX": -2.839953310889416, - "velocityY": -0.6651096140589043, - "timestamp": 7.398356584814849 - }, - { - "x": 5.890214769411301, - "y": 6.660443600401627, - "heading": 0.1800531937910235, - "angularVelocity": 0.29350622838307167, - "velocityX": -2.4875972682546292, - "velocityY": -0.5712298898151609, - "timestamp": 7.491791488292371 - }, - { - "x": 5.69086823658003, - "y": 6.615216731280533, - "heading": 0.2036583525973359, - "angularVelocity": 0.2526374826511273, - "velocityX": -2.1335338873574874, - "velocityY": -0.4840468330121896, - "timestamp": 7.585226391769893 - }, - { - "x": 5.5246769076205275, - "y": 6.577830247082936, - "heading": 0.22338706488015242, - "angularVelocity": 0.21114927664652222, - "velocityX": -1.7786857242217098, - "velocityY": -0.4001340270725567, - "timestamp": 7.678661295247416 - }, - { - "x": 5.391682817900511, - "y": 6.548103124628764, - "heading": 0.2392048833965071, - "angularVelocity": 0.16929239425136144, - "velocityX": -1.4233876717388545, - "velocityY": -0.31815864679865835, - "timestamp": 7.772096198724938 - }, - { - "x": 5.291913195980534, - "y": 6.525915639931743, - "heading": 0.2510888771367695, - "angularVelocity": 0.12719008954850897, - "velocityX": -1.067798201814147, - "velocityY": -0.23746462907578247, - "timestamp": 7.8655311022024605 - }, - { - "x": 5.225387107672126, - "y": 6.511182746987271, - "heading": 0.25902271171762453, - "angularVelocity": 0.08491296384507606, - "velocityX": -0.7120046773999501, - "velocityY": -0.15768082800039224, - "timestamp": 7.958966005679983 - }, - { - "x": 5.1921186447143555, - "y": 6.503840923309326, - "heading": 0.26299425401034693, - "angularVelocity": 0.04250598165039902, - "velocityX": -0.3560603341959137, - "velocityY": -0.07857688513276403, - "timestamp": 8.052400909157505 - }, - { - "x": 5.1921186447143555, - "y": 6.503840923309326, - "heading": 0.26299425401034693, - "angularVelocity": -1.0095416875346628e-18, - "velocityX": 2.3925664461171115e-17, - "velocityY": 7.068676560436054e-18, - "timestamp": 8.145835812635028 - }, - { - "x": 5.218809865013869, - "y": 6.495313073233126, - "heading": 0.260871879579823, - "angularVelocity": -0.025050084717691923, - "velocityX": 0.3150326917368419, - "velocityY": -0.10065300627272344, - "timestamp": 8.23056105226765 - }, - { - "x": 5.272196468754811, - "y": 6.478270476254072, - "heading": 0.2566236198235182, - "angularVelocity": -0.05014160803469802, - "velocityX": 0.6301145204478967, - "velocityY": -0.2011513576468166, - "timestamp": 8.315286291900271 - }, - { - "x": 5.352283792023045, - "y": 6.452729954483983, - "heading": 0.25024542978948194, - "angularVelocity": -0.07528087334651229, - "velocityX": 0.9452593302244072, - "velocityY": -0.3014511600183777, - "timestamp": 8.400011531532893 - }, - { - "x": 5.4590789190058935, - "y": 6.418713893785019, - "heading": 0.2417322157383651, - "angularVelocity": -0.10048025934221186, - "velocityX": 1.2604877536602386, - "velocityY": -0.40148674523037453, - "timestamp": 8.484736771165515 - }, - { - "x": 5.592591707275698, - "y": 6.376253550611183, - "heading": 0.23107691549234363, - "angularVelocity": -0.12576299922223963, - "velocityX": 1.575832524625849, - "velocityY": -0.5011534149439888, - "timestamp": 8.569462010798137 - }, - { - "x": 5.752836810532502, - "y": 6.32539562702347, - "heading": 0.21826867910988845, - "angularVelocity": -0.15117379942497813, - "velocityX": 1.8913502511370301, - "velocityY": -0.6002688668481637, - "timestamp": 8.654187250430759 - }, - { - "x": 5.939838300939574, - "y": 6.266217475974801, - "heading": 0.20328868573896017, - "angularVelocity": -0.17680673947790815, - "velocityX": 2.207152097980862, - "velocityY": -0.6984713328079369, - "timestamp": 8.738912490063381 - }, - { - "x": 6.153643038134906, - "y": 6.198872009915045, - "heading": 0.18609787015363735, - "angularVelocity": -0.20290076085785252, - "velocityX": 2.5235070224931055, - "velocityY": -0.7948689947856541, - "timestamp": 8.823637729696003 - }, - { - "x": 6.394381892977596, - "y": 6.123803995127001, - "heading": 0.16657845141587072, - "angularVelocity": -0.23038493396306808, - "velocityX": 2.841406597214261, - "velocityY": -0.8860171433394348, - "timestamp": 8.908362969328625 - }, - { - "x": 6.661972791946675, - "y": 6.056622100950942, - "heading": 0.1411655347825555, - "angularVelocity": -0.2999450546673997, - "velocityX": 3.1583374697951134, - "velocityY": -0.7929383790163071, - "timestamp": 8.993088208961247 - }, - { - "x": 6.9032496475776535, - "y": 5.999073073447199, - "heading": 0.11754236952388923, - "angularVelocity": -0.27882087275408163, - "velocityX": 2.847756544297579, - "velocityY": -0.6792430184120029, - "timestamp": 9.077813448593869 - }, - { - "x": 7.118031418712295, - "y": 5.950645420596591, - "heading": 0.09585021181159172, - "angularVelocity": -0.2560294642583155, - "velocityX": 2.5350388156582375, - "velocityY": -0.5715847256448627, - "timestamp": 9.16253868822649 - }, - { - "x": 7.306261982278755, - "y": 5.911173870778648, - "heading": 0.07613235590314016, - "angularVelocity": -0.23272705977522684, - "velocityX": 2.221658674353119, - "velocityY": -0.46587711039940277, - "timestamp": 9.247263927859112 - }, - { - "x": 7.467914023047444, - "y": 5.8805767421157285, - "heading": 0.05840904716752805, - "angularVelocity": -0.20918570207015538, - "velocityX": 1.9079561352630119, - "velocityY": -0.36113357478352465, - "timestamp": 9.331989167491734 - }, - { - "x": 7.602971384357183, - "y": 5.858805325942835, - "heading": 0.0426917303890982, - "angularVelocity": -0.18550926319691677, - "velocityX": 1.594062901389981, - "velocityY": -0.2569649406398561, - "timestamp": 9.416714407124356 - }, - { - "x": 7.711423390458376, - "y": 5.845827273377796, - "heading": 0.028987684559485204, - "angularVelocity": -0.16174691141666067, - "velocityX": 1.2800436631569734, - "velocityY": -0.15317811576944246, - "timestamp": 9.501439646756978 - }, - { - "x": 7.793262462229127, - "y": 5.841619537994678, - "heading": 0.017301971212697442, - "angularVelocity": -0.13792481906759255, - "velocityX": 0.9659349696219826, - "velocityY": -0.0496633045992219, - "timestamp": 9.5861648863896 - }, - { - "x": 7.848482940858505, - "y": 5.846164867627585, - "heading": 0.0076383901458884716, - "angularVelocity": -0.11405787825105443, - "velocityX": 0.6517594859432665, - "velocityY": 0.053647881701079494, - "timestamp": 9.670890126022222 - }, - { - "x": 7.87708044052124, - "y": 5.859449863433838, - "heading": 2.497416150959083e-27, - "angularVelocity": -0.09015483672881196, - "velocityX": 0.3375322369902693, - "velocityY": 0.15680092335953588, - "timestamp": 9.755615365654844 - }, - { - "x": 7.881596231266855, - "y": 5.877948058282651, - "heading": -0.005158165074008103, - "angularVelocity": -0.06905249312983469, - "velocityX": 0.060453010898891046, - "velocityY": 0.2476358267688033, - "timestamp": 9.830314552320118 - }, - { - "x": 7.865360693168207, - "y": 5.903065257170116, - "heading": -0.008731993343707898, - "angularVelocity": -0.0478429341635813, - "velocityX": -0.21734558063394657, - "velocityY": 0.3362446099984395, - "timestamp": 9.905013738985392 - }, - { - "x": 7.82831181618014, - "y": 5.934603651644639, - "heading": -0.010711784706445375, - "angularVelocity": -0.026503519664931297, - "velocityX": -0.4959743022917116, - "velocityY": 0.42220532622189155, - "timestamp": 9.979712925650666 - }, - { - "x": 7.770377377578464, - "y": 5.972324090639225, - "heading": -0.0110857390814715, - "angularVelocity": -0.005006137171235987, - "velocityX": -0.7755698714803699, - "velocityY": 0.5049645207465326, - "timestamp": 10.05441211231594 - }, - { - "x": 7.691472435431706, - "y": 6.0159317447407945, - "heading": -0.009839381087694242, - "angularVelocity": 0.016685027634399323, - "velocityX": -1.0563025605664207, - "velocityY": 0.583776826071393, - "timestamp": 10.129111298981215 - }, - { - "x": 7.591496122933511, - "y": 6.065054366948533, - "heading": -0.006954729975421036, - "angularVelocity": 0.038616901214725004, - "velocityX": -1.338385556273679, - "velocityY": 0.6576058508890599, - "timestamp": 10.203810485646489 - }, - { - "x": 7.470327733756537, - "y": 6.119207749983016, - "heading": -0.0024090670382981736, - "angularVelocity": 0.06085291072166403, - "velocityX": -1.6220844507976708, - "velocityY": 0.7249527799699275, - "timestamp": 10.278509672311763 - }, - { - "x": 7.32782268400487, - "y": 6.177737571853534, - "heading": 0.0038269526830036, - "angularVelocity": 0.08348176198015718, - "velocityX": -1.9077188937843252, - "velocityY": 0.7835402831464292, - "timestamp": 10.353208858977037 - }, - { - "x": 7.163811403874334, - "y": 6.2397140922834655, - "heading": 0.01179227386310508, - "angularVelocity": 0.10663196663430786, - "velocityX": -2.195623372252593, - "velocityY": 0.8296813284948795, - "timestamp": 10.427908045642312 - }, - { - "x": 6.978114754903294, - "y": 6.303722941599131, - "heading": 0.021540119487110404, - "angularVelocity": 0.13049466880657248, - "velocityX": -2.4859259820745265, - "velocityY": 0.8568881693784349, - "timestamp": 10.502607232307586 - }, - { - "x": 6.770640654116627, - "y": 6.367398692968795, - "heading": 0.03314423110651253, - "angularVelocity": 0.15534455109129794, - "velocityX": -2.7774613091351856, - "velocityY": 0.8524289783099493, - "timestamp": 10.57730641897286 - }, - { - "x": 6.541925002807842, - "y": 6.426263625769268, - "heading": 0.046694627071435006, - "angularVelocity": 0.18139951142495733, - "velocityX": -3.06182251131669, - "velocityY": 0.7880264220846899, - "timestamp": 10.652005605638134 - }, - { - "x": 6.296286014325702, - "y": 6.471418454274109, - "heading": 0.06214080908224435, - "angularVelocity": 0.20677844967741577, - "velocityX": -3.288375676469458, - "velocityY": 0.6044888909859091, - "timestamp": 10.726704792303408 - }, - { - "x": 6.048501736882419, - "y": 6.494895270234614, - "heading": 0.07847895732781966, - "angularVelocity": 0.2187192254018271, - "velocityX": -3.3170947168889495, - "velocityY": 0.3142847600965746, - "timestamp": 10.801403978968683 - }, - { - "x": 5.810207804194887, - "y": 6.498763072278465, - "heading": 0.09455495650408928, - "angularVelocity": 0.2152098288339583, - "velocityX": -3.190047219059051, - "velocityY": 0.051778368902233175, - "timestamp": 10.876103165633957 - }, - { - "x": 5.585913181304932, - "y": 6.485940456390381, - "heading": 0.10987224560749582, - "angularVelocity": 0.2050529568955417, - "velocityX": -3.002638086208546, - "velocityY": -0.17165670016652868, - "timestamp": 10.950802352299231 - }, - { - "x": 5.393948798137783, - "y": 6.461779934393033, - "heading": 0.12306987524609604, - "angularVelocity": 0.19297917140210155, - "velocityX": -2.8069531133047367, - "velocityY": -0.3532814333608597, - "timestamp": 11.019191233558862 - }, - { - "x": 5.21822858427444, - "y": 6.42928387175211, - "heading": 0.13522346577307512, - "angularVelocity": 0.17771296010588833, - "velocityX": -2.569426646946333, - "velocityY": -0.4751658755398492, - "timestamp": 11.087580114818493 - }, - { - "x": 5.060196677201855, - "y": 6.392264431165203, - "heading": 0.1462160897383734, - "angularVelocity": 0.1607370052387037, - "velocityX": -2.3107836268389246, - "velocityY": -0.5413078837533164, - "timestamp": 11.155968996078125 - }, - { - "x": 4.920363237530942, - "y": 6.353780654626919, - "heading": 0.15599475116563047, - "angularVelocity": 0.14298612942845923, - "velocityX": -2.0446809056585025, - "velocityY": -0.5627197846998563, - "timestamp": 11.224357877337756 - }, - { - "x": 4.798767231888284, - "y": 6.3161460308896284, - "heading": 0.164539456070254, - "angularVelocity": 0.12494289637791432, - "velocityX": -1.7780084043345064, - "velocityY": -0.550303251699868, - "timestamp": 11.292746758597387 - }, - { - "x": 4.695244909056443, - "y": 6.281093323544826, - "heading": 0.17184599738491455, - "angularVelocity": 0.10683814649521928, - "velocityX": -1.5137303158803064, - "velocityY": -0.5125497990196586, - "timestamp": 11.361135639857018 - }, - { - "x": 4.609559458166785, - "y": 6.2499355875386025, - "heading": 0.17791742648618403, - "angularVelocity": 0.08877801463398721, - "velocityX": -1.2529149375080866, - "velocityY": -0.45559651557885517, - "timestamp": 11.42952452111665 - }, - { - "x": 4.541458842404482, - "y": 6.223686967257069, - "heading": 0.18275993511057495, - "angularVelocity": 0.070808419953631, - "velocityX": -0.9957849069612339, - "velocityY": -0.3838141492896176, - "timestamp": 11.49791340237628 - }, - { - "x": 4.490700080737798, - "y": 6.203146864655871, - "heading": 0.18638086356776906, - "angularVelocity": 0.052946157189611806, - "velocityX": -0.7422078082252005, - "velocityY": -0.30034271979417115, - "timestamp": 11.566302283635912 - }, - { - "x": 4.4570583798792995, - "y": 6.188957712133428, - "heading": 0.1887877351300412, - "angularVelocity": 0.03519390166852904, - "velocityX": -0.49191769537480406, - "velocityY": -0.2074774767637344, - "timestamp": 11.634691164895543 - }, - { - "x": 4.440329551696777, - "y": 6.181644916534424, - "heading": 0.18998779389424572, - "angularVelocity": 0.017547571214809383, - "velocityX": -0.2446132744738579, - "velocityY": -0.10692959826674406, - "timestamp": 11.703080046155174 - }, - { - "x": 4.440329551696777, - "y": 6.181644916534424, - "heading": 0.18998779389424572, - "angularVelocity": -4.688613187627218e-28, - "velocityX": 1.0698378725405786e-25, - "velocityY": 1.9964070921469186e-25, - "timestamp": 11.771468927414805 - }, - { - "x": 4.439579335848565, - "y": 6.164768781343018, - "heading": 0.18962382517380036, - "angularVelocity": -0.00553315604255771, - "velocityX": -0.011404994771739275, - "velocityY": -0.2565558086834744, - "timestamp": 11.837248514624331 - }, - { - "x": 4.438629166641586, - "y": 6.131001049566098, - "heading": 0.18887821684872066, - "angularVelocity": -0.01133494989417828, - "velocityX": -0.014444742621334671, - "velocityY": -0.5133466658791909, - "timestamp": 11.903028101833858 - }, - { - "x": 4.4381503145293495, - "y": 6.080347141602359, - "heading": 0.18773000934555084, - "angularVelocity": -0.01745537714477353, - "velocityX": -0.00727964605054284, - "velocityY": -0.7700551206317605, - "timestamp": 11.968807689043384 - }, - { - "x": 4.438975373866779, - "y": 6.012850914220267, - "heading": 0.18615416077885757, - "angularVelocity": -0.023956498262503952, - "velocityX": 0.012542786788878593, - "velocityY": -1.0260968523122063, - "timestamp": 12.03458727625291 - }, - { - "x": 4.442155378938134, - "y": 5.928626966420744, - "heading": 0.1841206539292539, - "angularVelocity": -0.030913949689686845, - "velocityX": 0.04834334185201353, - "velocityY": -1.2803964173755829, - "timestamp": 12.100366863462437 - }, - { - "x": 4.449039762750971, - "y": 5.827921573409061, - "heading": 0.1815937471088899, - "angularVelocity": -0.038414756424589545, - "velocityX": 0.10465836142917671, - "velocityY": -1.5309520366995943, - "timestamp": 12.166146450671963 - }, - { - "x": 4.461381608820963, - "y": 5.71122949511904, - "heading": 0.1785321677552386, - "angularVelocity": -0.04654300039766565, - "velocityX": 0.18762425538913388, - "velocityY": -1.7739861747432784, - "timestamp": 12.23192603788149 - }, - { - "x": 4.481448613268838, - "y": 5.57951486602773, - "heading": 0.1748924161672784, - "angularVelocity": -0.055332539201965195, - "velocityX": 0.30506431096863224, - "velocityY": -2.0023632661567494, - "timestamp": 12.297705625091016 - }, - { - "x": 4.5120438147722846, - "y": 5.434590034225561, - "heading": 0.17063962874888192, - "angularVelocity": -0.06465208431378763, - "velocityX": 0.4651169580312549, - "velocityY": -2.2031885262603184, - "timestamp": 12.363485212300542 - }, - { - "x": 4.556188469541101, - "y": 5.279577397974022, - "heading": 0.1657694617290444, - "angularVelocity": -0.07403766466829045, - "velocityX": 0.6710996015861679, - "velocityY": -2.356546199625419, - "timestamp": 12.429264799510069 - }, - { - "x": 4.616267960626566, - "y": 5.118958057204039, - "heading": 0.16032836999773611, - "angularVelocity": -0.08271702456837958, - "velocityX": 0.9133455169625041, - "velocityY": -2.441780916903065, - "timestamp": 12.495044386719595 - }, - { - "x": 4.6932263289877705, - "y": 4.957658238915064, - "heading": 0.15440456088239551, - "angularVelocity": -0.09005543158049287, - "velocityX": 1.1699430115921938, - "velocityY": -2.4521257297524386, - "timestamp": 12.560823973929121 - }, - { - "x": 4.786707207470846, - "y": 4.799875743117793, - "heading": 0.14809694695472803, - "angularVelocity": -0.09589014153548765, - "velocityX": 1.4211229113571657, - "velocityY": -2.39865439250461, - "timestamp": 12.626603561138648 - }, - { - "x": 4.895747846043762, - "y": 4.648670678033537, - "heading": 0.14149381469729397, - "angularVelocity": -0.10038269526382383, - "velocityX": 1.6576668112189705, - "velocityY": -2.298662419431504, - "timestamp": 12.692383148348174 - }, - { - "x": 5.019277488115815, - "y": 4.506151226786177, - "heading": 0.13466693251764544, - "angularVelocity": -0.10378420524140636, - "velocityX": 1.8779327647431951, - "velocityY": -2.1666212467006614, - "timestamp": 12.7581627355577 - }, - { - "x": 5.1563191413879395, - "y": 4.373770713806152, - "heading": 0.1276729143915235, - "angularVelocity": -0.10632505345228195, - "velocityX": 2.083346203368645, - "velocityY": -2.0124862224865496, - "timestamp": 12.823942322767227 - }, - { - "x": 5.333320265467547, - "y": 4.234610501686042, - "heading": 0.11935234394669658, - "angularVelocity": -0.10849739642365235, - "velocityX": 2.30803419717898, - "velocityY": -1.814601631091569, - "timestamp": 12.900631448212573 - }, - { - "x": 5.526705635502563, - "y": 4.1115365335688585, - "heading": 0.11088465842948947, - "angularVelocity": -0.11041572671527947, - "velocityX": 2.5216791678350265, - "velocityY": -1.604842504103067, - "timestamp": 12.97732057365792 - }, - { - "x": 5.7352637837384215, - "y": 4.005696194354983, - "heading": 0.10229707398048027, - "angularVelocity": -0.11197916783037644, - "velocityX": 2.7195270127899818, - "velocityY": -1.3801218699423634, - "timestamp": 13.054009699103267 - }, - { - "x": 5.95717408037646, - "y": 3.9185355692390273, - "heading": 0.09362936542456811, - "angularVelocity": -0.11302395881524778, - "velocityX": 2.8936344670690923, - "velocityY": -1.1365447788040335, - "timestamp": 13.130698824548613 - }, - { - "x": 6.189557120584961, - "y": 3.851808580822914, - "heading": 0.08494227942173509, - "angularVelocity": -0.11327663410406259, - "velocityX": 3.030195465902305, - "velocityY": -0.8700971360492012, - "timestamp": 13.20738794999396 - }, - { - "x": 6.427716553529618, - "y": 3.8073042106200683, - "heading": 0.0763299644889769, - "angularVelocity": -0.11230164489091633, - "velocityX": 3.1055176540562526, - "velocityY": -0.5803217854474332, - "timestamp": 13.284077075439306 - }, - { - "x": 6.664399710640609, - "y": 3.7857133712502065, - "heading": 0.06792429760501602, - "angularVelocity": -0.109607024922345, - "velocityX": 3.0862675214580135, - "velocityY": -0.2815371702895352, - "timestamp": 13.360766200884653 - }, - { - "x": 6.890876802597326, - "y": 3.7846905044197223, - "heading": 0.059852862986743374, - "angularVelocity": -0.1052487503462913, - "velocityX": 2.9531839180787136, - "velocityY": -0.013337834074180547, - "timestamp": 13.43745532633 - }, - { - "x": 7.100368603662355, - "y": 3.799117631492698, - "heading": 0.05217739052380199, - "angularVelocity": -0.10008553909525866, - "velocityX": 2.7317015267611535, - "velocityY": 0.18812480895035505, - "timestamp": 13.514144451775346 - }, - { - "x": 7.289321103149217, - "y": 3.823808235084369, - "heading": 0.044905901451856856, - "angularVelocity": -0.09481773367108355, - "velocityX": 2.4638760500864425, - "velocityY": 0.3219570369108888, - "timestamp": 13.590833577220693 - }, - { - "x": 7.456197806965785, - "y": 3.8548124388815017, - "heading": 0.038028185708090405, - "angularVelocity": -0.08968306397845134, - "velocityX": 2.176015215292753, - "velocityY": 0.40428422696290434, - "timestamp": 13.667522702666039 - }, - { - "x": 7.600383975861882, - "y": 3.8893298407285344, - "heading": 0.0315311794356911, - "angularVelocity": -0.08471874251623218, - "velocityX": 1.880138390662096, - "velocityY": 0.4500951294800189, - "timestamp": 13.744211828111386 - }, - { - "x": 7.721659266950539, - "y": 3.9253622327898867, - "heading": 0.02540302978123265, - "angularVelocity": -0.07990897821394244, - "velocityX": 1.581388370051051, - "velocityY": 0.4698500843777596, - "timestamp": 13.820900953556732 - }, - { - "x": 7.819973686957294, - "y": 3.9614436370035824, - "heading": 0.019633766687128053, - "angularVelocity": -0.07522922005697107, - "velocityX": 1.281986454217959, - "velocityY": 0.4704891861025283, - "timestamp": 13.897590079002079 - }, - { - "x": 7.895351647860016, - "y": 3.9964652092207733, - "heading": 0.014215138878361277, - "angularVelocity": -0.07065705570770218, - "velocityX": 0.9829028622375049, - "velocityY": 0.45666933889016026, - "timestamp": 13.974279204447425 - }, - { - "x": 7.947849278823351, - "y": 4.029564484128199, - "heading": 0.009140302774238713, - "angularVelocity": -0.0661741293130177, - "velocityX": 0.6845511753911964, - "velocityY": 0.4316032385975494, - "timestamp": 14.050968329892772 - }, - { - "x": 7.9775347399255425, - "y": 4.060054180581902, - "heading": 0.004403534947127983, - "angularVelocity": -0.06176583445962591, - "velocityX": 0.3870882726827797, - "velocityY": 0.39757522695226577, - "timestamp": 14.127657455338118 - }, - { - "x": 7.984478950500488, - "y": 4.087375164031982, - "heading": 2.1272994684354894e-27, - "angularVelocity": -0.05742059153179738, - "velocityX": 0.09055013386343501, - "velocityY": 0.35625629176787293, - "timestamp": 14.204346580783465 - }, - { - "x": 7.971946711920367, - "y": 4.1092761415431145, - "heading": -0.003743193123138188, - "angularVelocity": -0.05350584756338805, - "velocityX": -0.1791379779341521, - "velocityY": 0.31305634672073157, - "timestamp": 14.274305160893865 - }, - { - "x": 7.9405304961902266, - "y": 4.1282629145767356, - "heading": -0.007208614807300815, - "angularVelocity": -0.04953533474655825, - "velocityX": -0.44906880157605467, - "velocityY": 0.2714002057168399, - "timestamp": 14.344263741004266 - }, - { - "x": 7.890211232557957, - "y": 4.14446189431145, - "heading": -0.010391724637129625, - "angularVelocity": -0.045499920450151686, - "velocityX": -0.7192722258350864, - "velocityY": 0.2315510078842599, - "timestamp": 14.414222321114666 - }, - { - "x": 7.820967420002681, - "y": 4.158023023328294, - "heading": -0.013287169417379553, - "angularVelocity": -0.04138798665840062, - "velocityX": -0.9897829893917608, - "velocityY": 0.19384511514446137, - "timestamp": 14.484180901225066 - }, - { - "x": 7.732774758572793, - "y": 4.169126981913827, - "heading": -0.015888540423835095, - "angularVelocity": -0.03718444545830319, - "velocityX": -1.2606411006443015, - "velocityY": 0.1587218975572353, - "timestamp": 14.554139481335467 - }, - { - "x": 7.6256058033641905, - "y": 4.177995649727882, - "heading": -0.0181880242087685, - "angularVelocity": -0.032869217489900605, - "velocityX": -1.5318915140856217, - "velocityY": 0.1267702660640051, - "timestamp": 14.624098061445867 - }, - { - "x": 7.499429791688496, - "y": 4.184907877485426, - "heading": -0.020175882010708173, - "angularVelocity": -0.0284147819867506, - "velocityX": -1.8035816546959227, - "velocityY": 0.09880457474459138, - "timestamp": 14.694056641556267 - }, - { - "x": 7.3542130604785445, - "y": 4.190224383970643, - "heading": -0.021839639250263897, - "angularVelocity": -0.023782032696063358, - "velocityX": -2.075752980989419, - "velocityY": 0.07599505988868352, - "timestamp": 14.764015221666668 - }, - { - "x": 7.189921237384926, - "y": 4.194429374442866, - "heading": -0.023162754266758116, - "angularVelocity": -0.01891283405704093, - "velocityX": -2.348415631568725, - "velocityY": 0.060106858452354715, - "timestamp": 14.833973801777068 - }, - { - "x": 7.006526810014663, - "y": 4.198205365374793, - "heading": -0.024122279434321083, - "angularVelocity": -0.01371561809929168, - "velocityX": -2.6214715490344274, - "velocityY": 0.05397466509423875, - "timestamp": 14.903932381887468 - }, - { - "x": 6.804034419528801, - "y": 4.202581059133061, - "heading": -0.024684383598055692, - "angularVelocity": -0.008034813783349493, - "velocityX": -2.8944611249443675, - "velocityY": 0.06254692064022167, - "timestamp": 14.973890961997869 - }, - { - "x": 6.582574314888401, - "y": 4.209262107218236, - "heading": -0.024794803569428318, - "angularVelocity": -0.0015783620993789793, - "velocityX": -3.1655888997592005, - "velocityY": 0.09550005266877039, - "timestamp": 15.04384954210827 - }, - { - "x": 6.342829267680752, - "y": 4.221487838378473, - "heading": -0.02435617244589905, - "angularVelocity": 0.006269868868651565, - "velocityX": -3.4269570198438615, - "velocityY": 0.17475670805416932, - "timestamp": 15.11380812221867 - }, - { - "x": 6.088597301309428, - "y": 4.246171426870893, - "heading": -0.02318863403361665, - "angularVelocity": 0.01668899526605478, - "velocityX": -3.6340355388877676, - "velocityY": 0.3528314676122184, - "timestamp": 15.18376670232907 - }, - { - "x": 5.834918091687161, - "y": 4.289953926308307, - "heading": -0.021332710171534618, - "angularVelocity": 0.02652889551436317, - "velocityX": -3.626134338660665, - "velocityY": 0.6258345919588656, - "timestamp": 15.25372528243947 - }, - { - "x": 5.593079348347317, - "y": 4.348733091622046, - "heading": -0.01919697004887221, - "angularVelocity": 0.030528637363594544, - "velocityX": -3.4568846731623317, - "velocityY": 0.8401995183575885, - "timestamp": 15.32368386254987 - }, - { - "x": 5.366458906870789, - "y": 4.419066666987696, - "heading": -0.01697675272885257, - "angularVelocity": 0.031736168980501495, - "velocityX": -3.239351643771207, - "velocityY": 1.005360246801131, - "timestamp": 15.393642442660271 - }, - { - "x": 5.1563191413879395, - "y": 4.4990692138671875, - "heading": -0.014762910019925463, - "angularVelocity": 0.03164504919101363, - "velocityX": -3.003774021016862, - "velocityY": 1.1435701918655279, - "timestamp": 15.463601022770671 - }, - { - "x": 4.95401886311352, - "y": 4.5927689377457765, - "heading": -0.01249503476608761, - "angularVelocity": 0.03078994837779946, - "velocityX": -2.7465422158220725, - "velocityY": 1.2721200852443764, - "timestamp": 15.537257374058129 - }, - { - "x": 4.772461025085408, - "y": 4.690756196606082, - "heading": -0.010343440097087077, - "angularVelocity": 0.02921125783985097, - "velocityX": -2.4649311954043354, - "velocityY": 1.3303300685896478, - "timestamp": 15.610913725345586 - }, - { - "x": 4.612079283555691, - "y": 4.78829172244435, - "heading": -0.008348351574694893, - "angularVelocity": 0.027086442479427285, - "velocityX": -2.177432614110885, - "velocityY": 1.3241970873308493, - "timestamp": 15.684570076633044 - }, - { - "x": 4.472451682258585, - "y": 4.881596852952091, - "heading": -0.006537499869867372, - "angularVelocity": 0.024585139952973775, - "velocityX": -1.8956627481068924, - "velocityY": 1.2667628639871322, - "timestamp": 15.758226427920501 - }, - { - "x": 4.352795824787486, - "y": 4.967847931994665, - "heading": -0.004929288926669055, - "angularVelocity": 0.021833975143867725, - "velocityX": -1.6245151352138218, - "velocityY": 1.1709930988295092, - "timestamp": 15.831882779207959 - }, - { - "x": 4.252252080841926, - "y": 5.0449690027725635, - "heading": -0.0035358640735745368, - "angularVelocity": 0.018917918533005222, - "velocityX": -1.3650383461593192, - "velocityY": 1.0470389780362597, - "timestamp": 15.905539130495416 - }, - { - "x": 4.170005928828097, - "y": 5.111421057915132, - "heading": -0.0023653291529936566, - "angularVelocity": 0.015891839605422012, - "velocityX": -1.1166199598029207, - "velocityY": 0.9021904286736582, - "timestamp": 15.979195481782874 - }, - { - "x": 4.105328367937727, - "y": 5.166042041203556, - "heading": -0.00142318887082745, - "angularVelocity": 0.012791025698372453, - "velocityX": -0.8780988979206226, - "velocityY": 0.7415651513235484, - "timestamp": 16.05285183307033 - }, - { - "x": 4.057581454405702, - "y": 5.207935779542024, - "heading": -0.0007132554579850938, - "angularVelocity": 0.009638454803058576, - "velocityX": -0.6482389189451429, - "velocityY": 0.56877292461815, - "timestamp": 16.12650818435779 - }, - { - "x": 4.026210974061915, - "y": 5.236396648720956, - "heading": -0.00023821729765836225, - "angularVelocity": 0.006449384907389903, - "velocityX": -0.42590326286131003, - "velocityY": 0.3864007472737587, - "timestamp": 16.200164535645246 - }, - { - "x": 4.010735511779785, - "y": 5.250858306884766, - "heading": -1.8831642522301752e-27, - "angularVelocity": 0.0032341718466161605, - "velocityX": -0.2101035689608668, - "velocityY": 0.1963395947672006, - "timestamp": 16.273820886932704 - }, - { - "x": 4.010735511779785, - "y": 5.250858306884766, - "heading": -9.309999607017892e-28, - "angularVelocity": 2.912789507524277e-28, - "velocityX": -3.6885695211923144e-25, - "velocityY": -3.7751755858346917e-25, - "timestamp": 16.34747723822016 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.1.traj b/src/main/deploy/choreo/CenterLanePCBAFE.1.traj new file mode 100644 index 00000000..9119888f --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFE.1.traj @@ -0,0 +1,292 @@ +{ + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": -8.364664059409844e-30, + "velocityX": -6.773677606947568e-30, + "velocityY": -2.308132540143278e-30, + "timestamp": 0 + }, + { + "x": 1.3079899492454155, + "y": 5.5701428985236205, + "heading": -0.011966271801548406, + "angularVelocity": -0.16457159783011938, + "velocityX": 0.24776633609486393, + "velocityY": -0.2862183198381729, + "timestamp": 0.07271164623703327 + }, + { + "x": 1.3440211933652915, + "y": 5.528520469164234, + "heading": -0.03588810704483216, + "angularVelocity": -0.328995924069994, + "velocityX": 0.49553607960947427, + "velocityY": -0.5724313987522216, + "timestamp": 0.14542329247406655 + }, + { + "x": 1.398068831056723, + "y": 5.466087418740183, + "heading": -0.07173813692543105, + "angularVelocity": -0.49304384837450815, + "velocityX": 0.7433147299769343, + "velocityY": -0.8586389341691013, + "timestamp": 0.21813493871109982 + }, + { + "x": 1.4701339496615213, + "y": 5.382844295736326, + "heading": -0.11947590052159912, + "angularVelocity": -0.6565353154432494, + "velocityX": 0.9911083345055308, + "velocityY": -1.1448389262999967, + "timestamp": 0.2908465849481331 + }, + { + "x": 1.5602181142824867, + "y": 5.2787919544216955, + "heading": -0.17905308310612672, + "angularVelocity": -0.819362312243441, + "velocityX": 1.2389234638197153, + "velocityY": -1.431027169709696, + "timestamp": 0.3635582311851664 + }, + { + "x": 1.668323429942649, + "y": 5.153931763984036, + "heading": -0.25042016061718175, + "angularVelocity": -0.9815082068514337, + "velocityX": 1.4867675434162453, + "velocityY": -1.717196582745939, + "timestamp": 0.4362698774221997 + }, + { + "x": 1.7944527928954905, + "y": 5.008265979195592, + "heading": -0.33353344500312104, + "angularVelocity": -1.1430532616554276, + "velocityX": 1.7346514550472283, + "velocityY": -2.003334986008902, + "timestamp": 0.508981523659233 + }, + { + "x": 1.938611553409707, + "y": 4.8417992681651185, + "heading": -0.42836015839962444, + "angularVelocity": -1.3041475239812899, + "velocityX": 1.98260894865471, + "velocityY": -2.2894091890060286, + "timestamp": 0.5816931698962663 + }, + { + "x": 2.09250704020051, + "y": 4.700832043022275, + "heading": -0.5059805202146961, + "angularVelocity": -1.0675093444479318, + "velocityX": 2.116517707491784, + "velocityY": -1.9387159062918606, + "timestamp": 0.6544048161332996 + }, + { + "x": 2.228385575629257, + "y": 4.5806756631537535, + "heading": -0.5718968876072178, + "angularVelocity": -0.906544835648882, + "velocityX": 1.8687313857908063, + "velocityY": -1.652505287385456, + "timestamp": 0.7271164623703329 + }, + { + "x": 2.346242849494598, + "y": 4.481326236786965, + "heading": -0.6261128487399352, + "angularVelocity": -0.7456296746751315, + "velocityX": 1.6208857861127495, + "velocityY": -1.3663481918284743, + "timestamp": 0.7998281086073662 + }, + { + "x": 2.446077177803767, + "y": 4.402782122456306, + "heading": -0.6686283268670905, + "angularVelocity": -0.5847134582152551, + "velocityX": 1.3730170265673516, + "velocityY": -1.080213671266156, + "timestamp": 0.8725397548443995 + }, + { + "x": 2.5278876403134056, + "y": 4.3450423723526415, + "heading": -0.6994420265312672, + "angularVelocity": -0.423779425385222, + "velocityX": 1.12513561096113, + "velocityY": -0.7940921858762265, + "timestamp": 0.9452514010814328 + }, + { + "x": 2.591673696422735, + "y": 4.308106404528037, + "heading": -0.718552253950641, + "angularVelocity": -0.2628220980484028, + "velocityX": 0.8772467604995492, + "velocityY": -0.5079787039876672, + "timestamp": 1.017963047318466 + }, + { + "x": 2.637435055651834, + "y": 4.2919738867991235, + "heading": -0.7259572155509334, + "angularVelocity": -0.10184010376417504, + "velocityX": 0.6293539150825374, + "velocityY": -0.22186979060047113, + "timestamp": 1.0906746935554992 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05916659407406203, + "velocityX": 0.38145976628941564, + "velocityY": 0.06423731409121712, + "timestamp": 1.1633863397925324 + }, + { + "x": 2.6747076115982096, + "y": 4.323065260999168, + "heading": -0.7050775877835139, + "angularVelocity": 0.2234613634683759, + "velocityX": 0.12854299211755085, + "velocityY": 0.35614342430781004, + "timestamp": 1.237571545918339 + }, + { + "x": 2.6654811031724366, + "y": 4.37114137531458, + "heading": -0.6763165724415564, + "angularVelocity": 0.38769205939503093, + "velocityX": -0.1243712717061632, + "velocityY": 0.6480552770879449, + "timestamp": 1.3117567520441455 + }, + { + "x": 2.6374924128586628, + "y": 4.440873637695316, + "heading": -0.6353766207501428, + "angularVelocity": 0.5518613996935385, + "velocityX": -0.3772812906679928, + "velocityY": 0.9399753133925751, + "timestamp": 1.3859419581699521 + }, + { + "x": 2.590742000006679, + "y": 4.532262849422754, + "heading": -0.5822609370446827, + "angularVelocity": 0.7159875462663353, + "velocityX": -0.6301851176982644, + "velocityY": 1.231906150790299, + "timestamp": 1.4601271642957587 + }, + { + "x": 2.525230488767617, + "y": 4.645309988591984, + "heading": -0.5169687243847326, + "angularVelocity": 0.8801244354174206, + "velocityX": -0.8830805313123746, + "velocityY": 1.5238501726898892, + "timestamp": 1.5343123704215653 + }, + { + "x": 2.4409586345992826, + "y": 4.780016081558229, + "heading": -0.4394900359726193, + "angularVelocity": 1.0443954051766633, + "velocityX": -1.1359657615273402, + "velocityY": 1.8158080296257462, + "timestamp": 1.608497576547372 + }, + { + "x": 2.337926784653071, + "y": 4.936381539754754, + "heading": -0.3497973159677186, + "angularVelocity": 1.2090378215045976, + "velocityX": -1.3888463122287895, + "velocityY": 2.107771432581833, + "timestamp": 1.6826827826731785 + }, + { + "x": 2.2066781223966365, + "y": 5.088037063238321, + "heading": -0.26257787440108193, + "angularVelocity": 1.1756985809634768, + "velocityX": -1.7692026362756765, + "velocityY": 2.044282565348452, + "timestamp": 1.756867988798985 + }, + { + "x": 2.094180452686652, + "y": 5.218026595083209, + "heading": -0.18768356705291486, + "angularVelocity": 1.009558526050363, + "velocityX": -1.5164434471606913, + "velocityY": 1.7522298398549971, + "timestamp": 1.8310531949247917 + }, + { + "x": 2.0004335399095625, + "y": 5.326350877049268, + "heading": -0.12517882612598913, + "angularVelocity": 0.8425499394477148, + "velocityX": -1.2636874340540831, + "velocityY": 1.460187113126797, + "timestamp": 1.9052384010505983 + }, + { + "x": 1.9254366143080366, + "y": 5.4130101547737155, + "heading": -0.07512185913342963, + "angularVelocity": 0.6747567285776663, + "velocityX": -1.0109417971010437, + "velocityY": 1.1681476975361738, + "timestamp": 1.9794236071764049 + }, + { + "x": 1.869189070291752, + "y": 5.478004602849777, + "heading": -0.037557912625022415, + "angularVelocity": 0.5063536043287745, + "velocityX": -0.7582043233686032, + "velocityY": 0.876110635424555, + "timestamp": 2.0536088133022115 + }, + { + "x": 1.8316905948823166, + "y": 5.5213343174096465, + "heading": -0.012514751718529437, + "angularVelocity": 0.3375762124977515, + "velocityX": -0.5054710685139713, + "velocityY": 0.5840748691542834, + "timestamp": 2.127794019428018 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 2.368719711095728e-29, + "angularVelocity": 0.1686960564308085, + "velocityX": -0.2527377395078362, + "velocityY": 0.2920386866937519, + "timestamp": 2.2019792255538246 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.1956060311838367e-29, + "angularVelocity": -1.025164181333131e-28, + "velocityX": 1.21885672142624e-30, + "velocityY": -5.1343833235782786e-30, + "timestamp": 2.2761644316796312 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.2.traj b/src/main/deploy/choreo/CenterLanePCBAFE.2.traj new file mode 100644 index 00000000..e6682c02 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFE.2.traj @@ -0,0 +1,94 @@ +{ + "samples": [ + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.1956060311838367e-29, + "angularVelocity": -1.025164181333131e-28, + "velocityX": 1.21885672142624e-30, + "velocityY": -5.1343833235782786e-30, + "timestamp": 0 + }, + { + "x": 1.8602093956334294, + "y": 5.5430660724686875, + "heading": -1.6999094202020734e-20, + "angularVelocity": -1.7890242544799916e-19, + "velocityX": 0.49746156397034486, + "velocityY": 0.0007030702226192673, + "timestamp": 0.09501880240866223 + }, + { + "x": 1.9547457978922331, + "y": 5.543199682247202, + "heading": -3.210902716840507e-20, + "angularVelocity": -1.5902045262092918e-19, + "velocityX": 0.9949231085045297, + "velocityY": 0.001406140417769105, + "timestamp": 0.19003760481732446 + }, + { + "x": 2.0965503966634373, + "y": 5.543400096908449, + "heading": -4.532979164623425e-20, + "angularVelocity": -1.3913840358532023e-19, + "velocityX": 1.492384614166397, + "velocityY": 0.0021092105579800877, + "timestamp": 0.2850564072259867 + }, + { + "x": 2.2856231808662413, + "y": 5.543667316436768, + "heading": -7.554638691710094e-20, + "angularVelocity": -3.1800648403053284e-19, + "velocityX": 1.9898460032113345, + "velocityY": 0.0028122805333745363, + "timestamp": 0.3800752096346489 + }, + { + "x": 2.474695965069045, + "y": 5.543934535965086, + "heading": -6.169486717931722e-20, + "angularVelocity": 1.457766188026957e-19, + "velocityX": 1.9898460032113345, + "velocityY": 0.002812280533374536, + "timestamp": 0.47509401204331114 + }, + { + "x": 2.6165005638402494, + "y": 5.544134950626333, + "heading": -3.084743805748469e-20, + "angularVelocity": 3.2464552635862006e-19, + "velocityX": 1.492384614166397, + "velocityY": 0.0021092105579800877, + "timestamp": 0.5701128144519734 + }, + { + "x": 2.711036966099053, + "y": 5.544268560404848, + "heading": -1.8891361886639493e-21, + "angularVelocity": 3.047639112965043e-19, + "velocityX": 0.9949231085045297, + "velocityY": 0.001406140417769105, + "timestamp": 0.6651316168606356 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -8.137968740525874e-34, + "angularVelocity": 1.9881709101649526e-20, + "velocityX": 0.49746156397034486, + "velocityY": 0.0007030702226192673, + "timestamp": 0.7601504192692978 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -8.137968740525874e-34, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -9.422453604168856e-36, + "timestamp": 0.8551692216779601 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.3.traj b/src/main/deploy/choreo/CenterLanePCBAFE.3.traj new file mode 100644 index 00000000..61dbe9b5 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFE.3.traj @@ -0,0 +1,184 @@ +{ + "samples": [ + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -8.137968740525874e-34, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -9.422453604168856e-36, + "timestamp": 0 + }, + { + "x": 2.744670053824415, + "y": 5.560420833392183, + "heading": 0.008590361313936763, + "angularVelocity": 0.13502733079351698, + "velocityX": -0.21432312628245512, + "velocityY": 0.2528389368381776, + "timestamp": 0.06361942625580808 + }, + { + "x": 2.717653230778774, + "y": 5.592802243922747, + "heading": 0.025816975580253556, + "angularVelocity": 0.27077600789812356, + "velocityX": -0.4246631042695725, + "velocityY": 0.5089862080862002, + "timestamp": 0.12723885251161615 + }, + { + "x": 2.6776153836811907, + "y": 5.641767759246714, + "heading": 0.051750317714318324, + "angularVelocity": 0.4076324427980389, + "velocityX": -0.629333671394567, + "velocityY": 0.7696629505440931, + "timestamp": 0.19085827876742423 + }, + { + "x": 2.6251092372323575, + "y": 5.707734732807458, + "heading": 0.0864931852991208, + "angularVelocity": 0.5461046983527398, + "velocityX": -0.8253162522672026, + "velocityY": 1.0368998502988076, + "timestamp": 0.2544777050232323 + }, + { + "x": 2.5610825168108655, + "y": 5.791356317104455, + "heading": 0.1301885817245676, + "angularVelocity": 0.686824748304888, + "velocityX": -1.006402040849065, + "velocityY": 1.314403307580339, + "timestamp": 0.3180971312790404 + }, + { + "x": 2.48749773492398, + "y": 5.89376509224382, + "heading": 0.18299253621322714, + "angularVelocity": 0.8299973388055953, + "velocityX": -1.1566401367878931, + "velocityY": 1.6097091905165586, + "timestamp": 0.38171655753484846 + }, + { + "x": 2.410156329475762, + "y": 6.016926517894053, + "heading": 0.24447965431113647, + "angularVelocity": 0.9664833796311684, + "velocityX": -1.215688508997792, + "velocityY": 1.9359090909592849, + "timestamp": 0.44533598379065653 + }, + { + "x": 2.3496693828620137, + "y": 6.152928215304285, + "heading": 0.3070071930810449, + "angularVelocity": 0.9828372000478389, + "velocityX": -0.9507622148388409, + "velocityY": 2.137738508728141, + "timestamp": 0.5089554100464646 + }, + { + "x": 2.309701308910896, + "y": 6.283843034176789, + "heading": 0.36362933631377437, + "angularVelocity": 0.8900134214517557, + "velocityX": -0.6282369443322133, + "velocityY": 2.0577805644789557, + "timestamp": 0.5725748363022727 + }, + { + "x": 2.288494901023514, + "y": 6.40507103844379, + "heading": 0.41290495871744193, + "angularVelocity": 0.7745373591634508, + "velocityX": -0.3333322718459105, + "velocityY": 1.905518666256975, + "timestamp": 0.6361942625580808 + }, + { + "x": 2.285060268530139, + "y": 6.514915678699493, + "heading": 0.45431002431284384, + "angularVelocity": 0.6508242534114624, + "velocityX": -0.05398716548566081, + "velocityY": 1.7265896082436825, + "timestamp": 0.6998136888138888 + }, + { + "x": 2.2988127872129502, + "y": 6.612520892931977, + "heading": 0.48757772897770557, + "angularVelocity": 0.522917395248037, + "velocityX": 0.2161685430408261, + "velocityY": 1.5342045657567287, + "timestamp": 0.7634331150696969 + }, + { + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.3925324714794108, + "velocityX": 0.48035043054352317, + "velocityY": 1.3337738607711032, + "timestamp": 0.827052541325505 + }, + { + "x": 2.3906193814869496, + "y": 6.780920533993851, + "heading": 0.5304788719493444, + "angularVelocity": 0.23275934723222522, + "velocityX": 0.7951498408867412, + "velocityY": 1.0846472026287595, + "timestamp": 0.9040782443210413 + }, + { + "x": 2.471491792404757, + "y": 6.8405967325057775, + "heading": 0.5351323155604377, + "angularVelocity": 0.06041416605263601, + "velocityX": 1.0499405753231936, + "velocityY": 0.7747569472411664, + "timestamp": 0.9811039473165777 + }, + { + "x": 2.556148280048404, + "y": 6.869576729136414, + "heading": 0.5277899567279947, + "angularVelocity": -0.09532349004160981, + "velocityX": 1.0990680299088356, + "velocityY": 0.37623800242778493, + "timestamp": 1.058129650312114 + }, + { + "x": 2.6173499244947225, + "y": 6.878199709364964, + "heading": 0.518346888239368, + "angularVelocity": -0.12259632981439711, + "velocityX": 0.794561322599876, + "velocityY": 0.11194938693450328, + "timestamp": 1.1351553533076504 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.0752536934288536, + "velocityX": 0.4023928513995403, + "velocityY": 0.0187620669061233, + "timestamp": 1.2121810563031867 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -4.4995568742972564e-34, + "velocityX": 2.8230892463303505e-33, + "velocityY": 2.0807279166194578e-33, + "timestamp": 1.289206759298723 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.4.traj b/src/main/deploy/choreo/CenterLanePCBAFE.4.traj new file mode 100644 index 00000000..d5e91e8f --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFE.4.traj @@ -0,0 +1,688 @@ +{ + "samples": [ + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -4.4995568742972564e-34, + "velocityX": 2.8230892463303505e-33, + "velocityY": 2.0807279166194578e-33, + "timestamp": 0 + }, + { + "x": 2.658706958910465, + "y": 6.86593478207681, + "heading": 0.5043163629464208, + "angularVelocity": -0.14325761626715308, + "velocityX": 0.1802876546124509, + "velocityY": -0.23853061812890558, + "timestamp": 0.05747726974755718 + }, + { + "x": 2.679434758469343, + "y": 6.838509950906704, + "heading": 0.48805940847309726, + "angularVelocity": -0.2828414527120879, + "velocityX": 0.36062602920974224, + "velocityY": -0.4771422040496659, + "timestamp": 0.11495453949511436 + }, + { + "x": 2.7105313297582017, + "y": 6.797364836772264, + "heading": 0.464037646160856, + "angularVelocity": -0.4179349926979131, + "velocityX": 0.5410238068968174, + "velocityY": -0.7158501841710775, + "timestamp": 0.17243180924267154 + }, + { + "x": 2.7520007950829926, + "y": 6.742492873007072, + "heading": 0.4325704761896421, + "angularVelocity": -0.5474715502218408, + "velocityX": 0.721493305213118, + "velocityY": -0.954672412350015, + "timestamp": 0.22990907899022872 + }, + { + "x": 2.803848241216662, + "y": 6.673886273182379, + "heading": 0.39405966194845793, + "angularVelocity": -0.6700181551824853, + "velocityX": 0.9020513041309295, + "velocityY": -1.1936301102334148, + "timestamp": 0.2873863487377859 + }, + { + "x": 2.866080023237396, + "y": 6.591535755483362, + "heading": 0.3490218715149645, + "angularVelocity": -0.7835756748937736, + "velocityX": 1.0827198698556684, + "velocityY": -1.4327492948204654, + "timestamp": 0.3448636184853431 + }, + { + "x": 2.938704098816793, + "y": 6.495430149531881, + "heading": 0.29814315673038183, + "angularVelocity": -0.8851971398092591, + "velocityX": 1.2635268845991694, + "velocityY": -1.6720628236793682, + "timestamp": 0.40234088823290026 + }, + { + "x": 3.0217302964606527, + "y": 6.385555893825644, + "heading": 0.2423799369377359, + "angularVelocity": -0.9701786469253116, + "velocityX": 1.4445048974753851, + "velocityY": -1.9116122980233827, + "timestamp": 0.45981815798045744 + }, + { + "x": 3.1151700095083186, + "y": 6.261896797084101, + "heading": 0.1831716018579807, + "angularVelocity": -1.0301173897055516, + "velocityX": 1.6256811337430896, + "velocityY": -2.151443471213232, + "timestamp": 0.5172954277280146 + }, + { + "x": 3.2190322842248182, + "y": 6.124436959606287, + "heading": 0.12297302537717429, + "angularVelocity": -1.0473457898261622, + "velocityX": 1.8070147585761873, + "velocityY": -2.39155127029422, + "timestamp": 0.5747726974755718 + }, + { + "x": 3.3332908123673026, + "y": 5.973192452697109, + "heading": 0.06701518728601086, + "angularVelocity": -0.9735646515036827, + "velocityX": 1.9878906678120518, + "velocityY": -2.6313794578874745, + "timestamp": 0.632249967223129 + }, + { + "x": 3.457219648102922, + "y": 5.80867115561654, + "heading": 0.033419490428721374, + "angularVelocity": -0.5845040483802287, + "velocityX": 2.1561364393249893, + "velocityY": -2.8623714696810656, + "timestamp": 0.6897272369706862 + }, + { + "x": 3.588371868131788, + "y": 5.635317112607293, + "heading": 0.03341946780631197, + "angularVelocity": -3.935887960456646e-7, + "velocityX": 2.2818101939234974, + "velocityY": -3.0160451909185437, + "timestamp": 0.7472045067182433 + }, + { + "x": 3.719524526285539, + "y": 5.461963401064903, + "heading": 0.033419445184933325, + "angularVelocity": -3.9357086276096175e-7, + "velocityX": 2.281817816499982, + "velocityY": -3.0160394239978356, + "timestamp": 0.8046817764658005 + }, + { + "x": 3.8561567314585825, + "y": 5.292894866069507, + "heading": 0.0334194225694706, + "angularVelocity": -3.9346793646231113e-7, + "velocityX": 2.3771519728257604, + "velocityY": -2.9414851425259596, + "timestamp": 0.8621590462133577 + }, + { + "x": 4.00579710900262, + "y": 5.135223415824853, + "heading": 0.03341939963642931, + "angularVelocity": -3.9899322628139097e-7, + "velocityX": 2.6034705232392743, + "velocityY": -2.743196587749476, + "timestamp": 0.9196363159609149 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.03341937596859772, + "angularVelocity": -4.117772415638071e-7, + "velocityX": 2.813320067337111, + "velocityY": -2.527531676347718, + "timestamp": 0.9771135857084721 + }, + { + "x": 4.277933740289957, + "y": 4.900652577427371, + "heading": 0.03341935253431191, + "angularVelocity": -6.240523322284166e-7, + "velocityX": 2.940862673655656, + "velocityY": -2.37792138300258, + "timestamp": 1.014665381608384 + }, + { + "x": 4.3928594316694705, + "y": 4.81721673895127, + "heading": 0.033419330076064284, + "angularVelocity": -5.980605477839765e-7, + "velocityX": 3.060457925523134, + "velocityY": -2.2218867693700792, + "timestamp": 1.0522171775082958 + }, + { + "x": 4.5119655190903245, + "y": 4.739865631807423, + "heading": 0.03341930829497473, + "angularVelocity": -5.800279064172658e-7, + "velocityX": 3.1717813906506853, + "velocityY": -2.0598510747665353, + "timestamp": 1.0897689734082077 + }, + { + "x": 4.634929904279705, + "y": 4.6688078109694935, + "heading": 0.033419286924895195, + "angularVelocity": -5.690827567994812e-7, + "velocityX": 3.2745274158690463, + "velocityY": -1.8922615852334157, + "timestamp": 1.1273207693081195 + }, + { + "x": 4.761195179582567, + "y": 4.6037960104372555, + "heading": 0.03341926571852252, + "angularVelocity": -5.647232617468814e-7, + "velocityX": 3.362429739429795, + "velocityY": -1.7312567608088656, + "timestamp": 1.1648725652080314 + }, + { + "x": 4.8874652266060234, + "y": 4.538793478329322, + "heading": 0.03341924451230877, + "angularVelocity": -5.647190300189245e-7, + "velocityX": 3.3625568097996403, + "velocityY": -1.7310099437370914, + "timestamp": 1.2024243611079433 + }, + { + "x": 5.0137352745443575, + "y": 4.473790947998577, + "heading": 0.03341922330609504, + "angularVelocity": -5.647190293196981e-7, + "velocityX": 3.3625568341627456, + "velocityY": -1.731009896410775, + "timestamp": 1.2399761570078551 + }, + { + "x": 5.140005505333594, + "y": 4.408788772864344, + "heading": 0.03341920209988365, + "angularVelocity": -5.64718967252867e-7, + "velocityX": 3.362561703461214, + "velocityY": -1.7310004375685482, + "timestamp": 1.277527952907767 + }, + { + "x": 5.267176463182282, + "y": 4.34556692987905, + "heading": 0.033419180889344834, + "angularVelocity": -5.648342058496427e-7, + "velocityX": 3.3865479613183083, + "velocityY": -1.6835903974819586, + "timestamp": 1.3150797488076789 + }, + { + "x": 5.39745989861742, + "y": 4.289036580908955, + "heading": 0.03341915955151481, + "angularVelocity": -5.682239561025287e-7, + "velocityX": 3.4694328809835504, + "velocityY": -1.5053966825119904, + "timestamp": 1.3526315447075907 + }, + { + "x": 5.530504784906373, + "y": 4.239352458442723, + "heading": 0.03341913783750815, + "angularVelocity": -5.782414966353427e-7, + "velocityX": 3.542969999186219, + "velocityY": -1.323082459189313, + "timestamp": 1.3901833406075026 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.033419115463141504, + "angularVelocity": -5.958268068735077e-7, + "velocityX": 3.6069365171092738, + "velocityY": -1.1371861948253605, + "timestamp": 1.4277351365074145 + }, + { + "x": 5.808220957669321, + "y": 4.160073254115033, + "heading": 0.03341909344161441, + "angularVelocity": -5.669632847998976e-7, + "velocityX": 3.6628444951256323, + "velocityY": -0.941676170142482, + "timestamp": 1.4665763237425935 + }, + { + "x": 5.952250404538064, + "y": 4.131196918833519, + "heading": 0.033419072126499465, + "angularVelocity": -5.487760919877281e-7, + "velocityX": 3.708162832321777, + "velocityY": -0.7434462573626127, + "timestamp": 1.5054175109777725 + }, + { + "x": 6.09762364792411, + "y": 4.110103210641541, + "heading": 0.03341905123842176, + "angularVelocity": -5.377816485869475e-7, + "velocityX": 3.7427600373239653, + "velocityY": -0.5430757835556856, + "timestamp": 1.5442586982129516 + }, + { + "x": 6.24392007770515, + "y": 4.096848811559285, + "heading": 0.0334190305197359, + "angularVelocity": -5.334205087871439e-7, + "velocityX": 3.7665282704987906, + "velocityY": -0.3412459820551187, + "timestamp": 1.5830998854481306 + }, + { + "x": 6.390314919287249, + "y": 4.0847296192319185, + "heading": 0.03341900981240726, + "angularVelocity": -5.331281075500129e-7, + "velocityX": 3.7690619675370143, + "velocityY": -0.31201910111516795, + "timestamp": 1.6219410726833097 + }, + { + "x": 6.536709800200056, + "y": 4.07261090201266, + "heading": 0.033418989105078956, + "angularVelocity": -5.331280988936517e-7, + "velocityX": 3.769062980140168, + "velocityY": -0.31200686904551156, + "timestamp": 1.6607822599184887 + }, + { + "x": 6.68323027882942, + "y": 4.062119297351164, + "heading": 0.03341896797261103, + "angularVelocity": -5.440736864773728e-7, + "velocityX": 3.772296602114613, + "velocityY": -0.27011544724343456, + "timestamp": 1.6996234471536678 + }, + { + "x": 6.828478445747191, + "y": 4.059283961769526, + "heading": 0.02725441210059553, + "angularVelocity": -0.15871182913873783, + "velocityX": 3.739539835337918, + "velocityY": -0.07299816981572388, + "timestamp": 1.7384646343888468 + }, + { + "x": 6.965940131033012, + "y": 4.0573223410737915, + "heading": 0.017080975304275284, + "angularVelocity": -0.2619239400361572, + "velocityX": 3.5390701229986914, + "velocityY": -0.05050362348240824, + "timestamp": 1.7773058216240258 + }, + { + "x": 7.095541107200079, + "y": 4.056112825219225, + "heading": 0.007563330070755733, + "angularVelocity": -0.2450400183673897, + "velocityX": 3.336689359734222, + "velocityY": -0.03114003305929124, + "timestamp": 1.8161470088592049 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, + "angularVelocity": -0.194724482157576, + "velocityX": 3.1345655629883886, + "velocityY": -0.012732337027157752, + "timestamp": 1.854988196094384 + }, + { + "x": 7.3857970557158446, + "y": 4.056503208949951, + "heading": -0.005539516561442828, + "angularVelocity": -0.09285314353970942, + "velocityX": 2.8244854636552765, + "velocityY": 0.014833039029661928, + "timestamp": 1.9146470977779204 + }, + { + "x": 7.5357095386456, + "y": 4.058265721996549, + "heading": -0.007640794437617486, + "angularVelocity": -0.03522153135371121, + "velocityX": 2.5128267316246156, + "velocityY": 0.029543169533142722, + "timestamp": 1.974305999461457 + }, + { + "x": 7.667000937583321, + "y": 4.060490426598279, + "heading": -0.007708640614146542, + "angularVelocity": -0.0011372347564986972, + "velocityX": 2.200700905192029, + "velocityY": 0.037290404934565556, + "timestamp": 2.0339649011449934 + }, + { + "x": 7.779661923967275, + "y": 4.062917456014214, + "heading": -0.0066168114481254744, + "angularVelocity": 0.018301194544491067, + "velocityX": 1.888418713800162, + "velocityY": 0.0406817649578728, + "timestamp": 2.09362380282853 + }, + { + "x": 7.873689685619936, + "y": 4.06536905162046, + "heading": -0.004961765552273349, + "angularVelocity": 0.027741809673791765, + "velocityX": 1.5760893848068933, + "velocityY": 0.04109354240631638, + "timestamp": 2.1532827045120664 + }, + { + "x": 7.949083994059634, + "y": 4.067716008149933, + "heading": -0.003176764942269326, + "angularVelocity": 0.029920105124841948, + "velocityX": 1.263756226013528, + "velocityY": 0.03933958660390468, + "timestamp": 2.212941606195603 + }, + { + "x": 8.005845696502893, + "y": 4.069860191657625, + "heading": -0.0015908607397525072, + "angularVelocity": 0.02658285951909277, + "velocityX": 0.951437268227904, + "velocityY": 0.035940713744034375, + "timestamp": 2.2726005078791394 + }, + { + "x": 8.043976076014772, + "y": 4.071724543898811, + "heading": -0.00046248549538235704, + "angularVelocity": 0.018913778372181077, + "velocityX": 0.6391398171247351, + "velocityY": 0.03125019382816773, + "timestamp": 2.332259409562676 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 3.4463826664070866e-33, + "angularVelocity": 0.00775216241552071, + "velocityX": 0.32686633402451676, + "velocityY": 0.02551860543538527, + "timestamp": 2.3919183112462123 + }, + { + "x": 8.059339450711764, + "y": 4.074523581107775, + "heading": -0.0006944740465081056, + "angularVelocity": -0.009481698382290156, + "velocityX": -0.0564842507032717, + "velocityY": 0.017429845647464255, + "timestamp": 2.4651619452986555 + }, + { + "x": 8.027124349390371, + "y": 4.075207755344852, + "heading": -0.002651211191279339, + "angularVelocity": -0.026715456845985846, + "velocityX": -0.43983482985464195, + "velocityY": 0.009341074428224473, + "timestamp": 2.5384055793510987 + }, + { + "x": 7.96683125907587, + "y": 4.075299477238389, + "heading": -0.0058702138612145086, + "angularVelocity": -0.043949248444312826, + "velocityX": -0.8231854016327222, + "velocityY": 0.0012522848534625944, + "timestamp": 2.611649213403542 + }, + { + "x": 7.878460180490193, + "y": 4.074798744928973, + "heading": -0.0103514975430217, + "angularVelocity": -0.06118325148365151, + "velocityX": -1.2065359635542152, + "velocityY": -0.006836530107951673, + "timestamp": 2.684892847455985 + }, + { + "x": 7.7620111146201785, + "y": 4.073705556029158, + "heading": -0.01609509527793809, + "angularVelocity": -0.07841770563710548, + "velocityX": -1.5898865120023296, + "velocityY": -0.01492537766534737, + "timestamp": 2.758136481508428 + }, + { + "x": 7.617484062863899, + "y": 4.072019907599753, + "heading": -0.023101063438175296, + "angularVelocity": -0.0956529294439546, + "velocityX": -1.9732370413624691, + "velocityY": -0.02301426535169652, + "timestamp": 2.8313801155608713 + }, + { + "x": 7.444879027316714, + "y": 4.06974179610174, + "heading": -0.031369488604002356, + "angularVelocity": -0.1128893353367305, + "velocityX": -2.356587542114559, + "velocityY": -0.031103201356480925, + "timestamp": 2.9046237496133145 + }, + { + "x": 7.244196011426359, + "y": 4.066871217282435, + "heading": -0.040900495023888005, + "angularVelocity": -0.13012743760176224, + "velocityX": -2.7399379957944494, + "velocityY": -0.0391921954234215, + "timestamp": 2.9778673836657577 + }, + { + "x": 7.015435021934675, + "y": 4.063408165830539, + "heading": -0.05169425070794916, + "angularVelocity": -0.14736783371961967, + "velocityX": -3.123288357427606, + "velocityY": -0.047281262005875575, + "timestamp": 3.051111017718201 + }, + { + "x": 6.758596078532163, + "y": 4.059352633648335, + "heading": -0.06375095974291681, + "angularVelocity": -0.1646110162466115, + "velocityX": -3.5066384502250503, + "velocityY": -0.05537043914696755, + "timestamp": 3.124354651770644 + }, + { + "x": 6.481653456514826, + "y": 4.053513800761945, + "heading": -0.06375096147352038, + "angularVelocity": -2.3628040945559003e-8, + "velocityX": -3.7811152545904956, + "velocityY": -0.07971795722490971, + "timestamp": 3.197598285823087 + }, + { + "x": 6.205372409181029, + "y": 4.073516064315128, + "heading": -0.06375096323470678, + "angularVelocity": -2.4045589953178915e-8, + "velocityX": -3.7720827333058655, + "velocityY": 0.2730921780705308, + "timestamp": 3.2708419198755303 + }, + { + "x": 5.9325368870621995, + "y": 4.121391787593611, + "heading": -0.0637509651063581, + "angularVelocity": -2.555377459766275e-8, + "velocityX": -3.725040758128897, + "velocityY": 0.6536502987304462, + "timestamp": 3.3440855539279735 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.063750967187294, + "angularVelocity": -2.84111505129003e-8, + "velocityX": -3.639704142076304, + "velocityY": 1.0274925313911556, + "timestamp": 3.4173291879804166 + }, + { + "x": 5.417504232216337, + "y": 4.293821782165803, + "heading": -0.06375097411514968, + "angularVelocity": -9.821339780633191e-8, + "velocityX": -3.5221393138546646, + "velocityY": 1.3775780331429743, + "timestamp": 3.4878679925152554 + }, + { + "x": 5.186014168311204, + "y": 4.410724788288498, + "heading": -0.06922328693791815, + "angularVelocity": -0.07757875766190163, + "velocityX": -3.281740673543775, + "velocityY": 1.6572864665569722, + "timestamp": 3.558406797050094 + }, + { + "x": 4.979961148964822, + "y": 4.522014233902487, + "heading": -0.07551597396386121, + "angularVelocity": -0.08920886974821321, + "velocityX": -2.9211300177991313, + "velocityY": 1.5777052977843462, + "timestamp": 3.628945601584932 + }, + { + "x": 4.7980291404215, + "y": 4.623466282585381, + "heading": -0.08170492719079357, + "angularVelocity": -0.08773827778546625, + "velocityX": -2.579176238427287, + "velocityY": 1.4382445145180842, + "timestamp": 3.69948440611977 + }, + { + "x": 4.639563192627529, + "y": 4.713607705102715, + "heading": -0.0874491132160183, + "angularVelocity": -0.08143299369906003, + "velocityX": -2.246507420120879, + "velocityY": 1.27789835838249, + "timestamp": 3.7700232106546077 + }, + { + "x": 4.504192007690659, + "y": 4.791698947450768, + "heading": -0.09257307344830198, + "angularVelocity": -0.07264030438385177, + "velocityX": -1.919102341322099, + "velocityY": 1.1070678453231335, + "timestamp": 3.8405620151894455 + }, + { + "x": 4.391678654940531, + "y": 4.857296763925047, + "heading": -0.09697023729098278, + "angularVelocity": -0.0623368069770602, + "velocityX": -1.5950561324662005, + "velocityY": 0.9299536178257779, + "timestamp": 3.9111008197242834 + }, + { + "x": 4.301859289367306, + "y": 4.910106206970951, + "heading": -0.10056909340764136, + "angularVelocity": -0.051019522380495494, + "velocityX": -1.2733326878096392, + "velocityY": 0.7486580385668955, + "timestamp": 3.981639624259121 + }, + { + "x": 4.234614001747884, + "y": 4.949916980366573, + "heading": -0.10331834169263278, + "angularVelocity": -0.03897497700905296, + "velocityX": -0.9533091475375062, + "velocityY": 0.5643811751297677, + "timestamp": 4.052178428793959 + }, + { + "x": 4.18985129168719, + "y": 4.976571613170809, + "heading": -0.10517936365869324, + "angularVelocity": -0.026382953019019773, + "velocityX": -0.6345827712261082, + "velocityY": 0.37787191007854753, + "timestamp": 4.122717233328797 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.013363372792080578, + "velocityX": -0.31687843925651377, + "velocityY": 0.18962871266842563, + "timestamp": 4.193256037863635 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": 0, + "velocityX": 8.237561081237056e-34, + "velocityY": 0, + "timestamp": 4.263794842398473 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.5.traj b/src/main/deploy/choreo/CenterLanePCBAFE.5.traj new file mode 100644 index 00000000..fa564de5 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFE.5.traj @@ -0,0 +1,607 @@ +{ + "samples": [ + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": 0, + "velocityX": 8.237561081237056e-34, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 4.18294196758914, + "y": 4.969364336221789, + "heading": -0.10045524830547124, + "angularVelocity": 0.08077125535022475, + "velocityX": 0.22011597884885625, + "velocityY": -0.29338710511594707, + "timestamp": 0.07015802428670526 + }, + { + "x": 4.2148538086473515, + "y": 4.929011600999615, + "heading": -0.08896968745487453, + "angularVelocity": 0.16370986736542367, + "velocityX": 0.4548566095276789, + "velocityY": -0.5751692074062589, + "timestamp": 0.1403160485734105 + }, + { + "x": 4.264519080629496, + "y": 4.870038214957966, + "heading": -0.07148588735689271, + "angularVelocity": 0.24920599283886813, + "velocityX": 0.7079057953397331, + "velocityY": -0.8405793441481644, + "timestamp": 0.21047407286011577 + }, + { + "x": 4.333555645892384, + "y": 4.794138441539978, + "heading": -0.04780159185128592, + "angularVelocity": 0.3375849839901328, + "velocityX": 0.9840152422303832, + "velocityY": -1.081840234095224, + "timestamp": 0.280632097146821 + }, + { + "x": 4.423967613250648, + "y": 4.703932489290492, + "heading": -0.017716503350327845, + "angularVelocity": 0.4288189242332906, + "velocityX": 1.2886903284047762, + "velocityY": -1.2857538844146224, + "timestamp": 0.3507901214335263 + }, + { + "x": 4.53803034325044, + "y": 4.603622363293204, + "heading": 0.0188848190363467, + "angularVelocity": 0.5216983054867799, + "velocityX": 1.6257973504736565, + "velocityY": -1.429774099500926, + "timestamp": 0.42094814572023154 + }, + { + "x": 4.677578748555051, + "y": 4.49984021599331, + "heading": 0.06181064388576721, + "angularVelocity": 0.6118448357952799, + "velocityX": 1.9890583682108496, + "velocityY": -1.4792626838489913, + "timestamp": 0.4911061700069368 + }, + { + "x": 4.842258553230926, + "y": 4.40159118744757, + "heading": 0.11019595374155315, + "angularVelocity": 0.6896618077221814, + "velocityX": 2.3472697007957355, + "velocityY": -1.4003961705682948, + "timestamp": 0.561264194293642 + }, + { + "x": 5.028363428376419, + "y": 4.3176283558190685, + "heading": 0.16247927063581383, + "angularVelocity": 0.7452221955481739, + "velocityX": 2.6526527369836272, + "velocityY": -1.1967673332045712, + "timestamp": 0.6314222185803473 + }, + { + "x": 5.230618576677741, + "y": 4.253738965485636, + "heading": 0.2169576127744663, + "angularVelocity": 0.7765090692409344, + "velocityX": 2.8828512541173206, + "velocityY": -0.9106497935623799, + "timestamp": 0.7015802428670526 + }, + { + "x": 5.444331250681613, + "y": 4.2129309832647035, + "heading": 0.2722364606827335, + "angularVelocity": 0.7879191079037022, + "velocityX": 3.0461615214607765, + "velocityY": -0.5816580873795356, + "timestamp": 0.7717382671537578 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.3273042274639755, + "angularVelocity": 0.7849104552346565, + "velocityX": 3.1588757008538453, + "velocityY": -0.23207478938864265, + "timestamp": 0.8418962914404631 + }, + { + "x": 5.81887557997068, + "y": 4.1972183908996925, + "heading": 0.36426543181084436, + "angularVelocity": 0.7759151841130933, + "velocityX": 3.210283328613804, + "velocityY": 0.011951482768166844, + "timestamp": 0.889531915930938 + }, + { + "x": 5.973953798857342, + "y": 4.209469324927334, + "heading": 0.4004931510026103, + "angularVelocity": 0.7605173560600648, + "velocityX": 3.255509307276357, + "velocityY": 0.257180086514691, + "timestamp": 0.937167540421413 + }, + { + "x": 6.130819634438986, + "y": 4.233461866975936, + "heading": 0.4356102472173979, + "angularVelocity": 0.7372023898166727, + "velocityX": 3.2930361942250057, + "velocityY": 0.5036680489703445, + "timestamp": 0.984803164911888 + }, + { + "x": 6.289005438456121, + "y": 4.269254559307002, + "heading": 0.4691390733673604, + "angularVelocity": 0.7038603253047925, + "velocityX": 3.32074588523062, + "velocityY": 0.7513849710991581, + "timestamp": 1.032438789402363 + }, + { + "x": 6.447898093638549, + "y": 4.316894151635714, + "heading": 0.5004573197150548, + "angularVelocity": 0.6574543040567628, + "velocityX": 3.335584594135005, + "velocityY": 1.0000832956066072, + "timestamp": 1.080074413892838 + }, + { + "x": 6.606667388474461, + "y": 4.376390356038531, + "heading": 0.5287266543286071, + "angularVelocity": 0.5934494386486953, + "velocityX": 3.33299492835787, + "velocityY": 1.248985502745193, + "timestamp": 1.127710038383313 + }, + { + "x": 6.764149093497097, + "y": 4.447653029340476, + "heading": 0.5527726659639904, + "angularVelocity": 0.5047905195447082, + "velocityX": 3.3059649518004255, + "velocityY": 1.4959953619626436, + "timestamp": 1.175345662873788 + }, + { + "x": 6.918662738022944, + "y": 4.530331648811429, + "heading": 0.5708777234010636, + "angularVelocity": 0.3800738970199359, + "velocityX": 3.243657371527556, + "velocityY": 1.735646805417357, + "timestamp": 1.222981287364263 + }, + { + "x": 7.06846751913844, + "y": 4.623866769358016, + "heading": 0.5845734852773292, + "angularVelocity": 0.2875109127414552, + "velocityX": 3.144805651607445, + "velocityY": 1.9635539902555645, + "timestamp": 1.270616911854738 + }, + { + "x": 7.211529043645413, + "y": 4.727181705822193, + "heading": 0.5988053175713824, + "angularVelocity": 0.2987644739893997, + "velocityX": 3.0032465415789793, + "velocityY": 2.168858655035301, + "timestamp": 1.3182525363452129 + }, + { + "x": 7.344594975677938, + "y": 4.83690117801315, + "heading": 0.6149173466892092, + "angularVelocity": 0.3382348670803817, + "velocityX": 2.7934121459693166, + "velocityY": 2.303307101870708, + "timestamp": 1.3658881608356879 + }, + { + "x": 7.465817541114002, + "y": 4.947544737637474, + "heading": 0.6314993734499258, + "angularVelocity": 0.34810138290582165, + "velocityX": 2.5447879970651583, + "velocityY": 2.322706184873219, + "timestamp": 1.4135237853261629 + }, + { + "x": 7.575645879536293, + "y": 5.054829291458012, + "heading": 0.6474951019450449, + "angularVelocity": 0.33579340391176205, + "velocityX": 2.305592497149964, + "velocityY": 2.252191610125547, + "timestamp": 1.4611594098166378 + }, + { + "x": 7.675112095346534, + "y": 5.156311134408354, + "heading": 0.6623068665871409, + "angularVelocity": 0.31093881523601674, + "velocityX": 2.0880636472002183, + "velocityY": 2.1303770872287977, + "timestamp": 1.5087950343071128 + }, + { + "x": 7.76511711692712, + "y": 5.250618542879285, + "heading": 0.6755917653702976, + "angularVelocity": 0.27888579031462285, + "velocityX": 1.8894477094255981, + "velocityY": 1.979766392897571, + "timestamp": 1.5564306587975878 + }, + { + "x": 7.846351174360072, + "y": 5.336925095447112, + "heading": 0.6871379054751507, + "angularVelocity": 0.2423845646688635, + "velocityX": 1.7053215592711606, + "velocityY": 1.8118068880378715, + "timestamp": 1.6040662832880628 + }, + { + "x": 7.919337423332426, + "y": 5.414694341101313, + "heading": 0.6968047845571967, + "angularVelocity": 0.2029338165594727, + "velocityX": 1.5321778553978798, + "velocityY": 1.6325858322640792, + "timestamp": 1.6517019077785378 + }, + { + "x": 7.984478950500488, + "y": 5.483555793762207, + "heading": 0.7044936776046891, + "angularVelocity": 0.16141056467161, + "velocityX": 1.3674960256076552, + "velocityY": 1.445587276276031, + "timestamp": 1.6993375322690127 + }, + { + "x": 8.05911422318913, + "y": 5.560200385763573, + "heading": 0.7111561296070518, + "angularVelocity": 0.10289890039880417, + "velocityX": 1.1527118676281318, + "velocityY": 1.1837449989371336, + "timestamp": 1.7640850852218737 + }, + { + "x": 8.119756898561732, + "y": 5.61995838055026, + "heading": 0.7143078455831314, + "angularVelocity": 0.04867698982190534, + "velocityX": 0.9366018112955389, + "velocityY": 0.9229382742880431, + "timestamp": 1.8288326381747346 + }, + { + "x": 8.166344572702991, + "y": 5.662879129934428, + "heading": 0.7141510891264902, + "angularVelocity": -0.002421040633850211, + "velocityX": 0.7195279515069205, + "velocityY": 0.6628937685941038, + "timestamp": 1.8935801911275956 + }, + { + "x": 8.198829842984775, + "y": 5.689000472186179, + "heading": 0.7108395294312745, + "angularVelocity": -0.0511457119873956, + "velocityX": 0.5017219771291601, + "velocityY": 0.4034336598136588, + "timestamp": 1.9583277440804565 + }, + { + "x": 8.217175483703613, + "y": 5.698352336883545, + "heading": 0.7044936776046891, + "angularVelocity": -0.09800913759946452, + "velocityX": 0.28334106668393205, + "velocityY": 0.1444358013680284, + "timestamp": 2.0230752970333175 + }, + { + "x": 8.221318658765867, + "y": 5.6908798741594095, + "heading": 0.6951703689604403, + "angularVelocity": -0.14352542875947455, + "velocityX": 0.06378111032529457, + "velocityY": -0.11503302714668015, + "timestamp": 2.088034577941462 + }, + { + "x": 8.210964366460544, + "y": 5.666753242665305, + "heading": 0.6829793287804198, + "angularVelocity": -0.18767203099521718, + "velocityX": -0.15939665834608732, + "velocityY": -0.3714116159663335, + "timestamp": 2.152993858849607 + }, + { + "x": 8.185835060378174, + "y": 5.6262170290983935, + "heading": 0.6680275277365855, + "angularVelocity": -0.2301718989927961, + "velocityX": -0.3868470483517696, + "velocityY": -0.6240249738021217, + "timestamp": 2.2179531397577517 + }, + { + "x": 8.14559839699319, + "y": 5.569575312921383, + "heading": 0.6504460796726778, + "angularVelocity": -0.2706533665107588, + "velocityX": -0.6194136206938888, + "velocityY": -0.8719572536078877, + "timestamp": 2.2829124206658964 + }, + { + "x": 8.08985004858283, + "y": 5.497215477528086, + "heading": 0.6303993620580922, + "angularVelocity": -0.30860436467780866, + "velocityX": -0.858204518753663, + "velocityY": -1.11392605308573, + "timestamp": 2.347871701574041 + }, + { + "x": 8.018089118716903, + "y": 5.40964606854142, + "heading": 0.6080990792438148, + "angularVelocity": -0.3432963312172538, + "velocityX": -1.1047063462324833, + "velocityY": -1.348066169489957, + "timestamp": 2.412830982482186 + }, + { + "x": 7.9296824410081745, + "y": 5.307560214485479, + "heading": 0.5838269457597958, + "angularVelocity": -0.3736515112958315, + "velocityX": -1.3609553011176063, + "velocityY": -1.5715360858180332, + "timestamp": 2.4777902633903306 + }, + { + "x": 7.823812920396748, + "y": 5.191948689740781, + "heading": 0.5579732566194568, + "angularVelocity": -0.3979983888198644, + "velocityX": -1.6297828290483853, + "velocityY": -1.779753764641825, + "timestamp": 2.5427495442984753 + }, + { + "x": 7.699409898040676, + "y": 5.064316508380435, + "heading": 0.5311065320658849, + "angularVelocity": -0.41359331842916214, + "velocityX": -1.9150923565792266, + "velocityY": -1.9648028669039912, + "timestamp": 2.60770882520662 + }, + { + "x": 7.555088273925506, + "y": 4.927129260587564, + "heading": 0.5041069990932381, + "angularVelocity": -0.4156378056405048, + "velocityX": -2.221724472585296, + "velocityY": -2.111896035100176, + "timestamp": 2.672668106114765 + }, + { + "x": 7.389294037285908, + "y": 4.784758435393721, + "heading": 0.47842631175838873, + "angularVelocity": -0.3953351542047192, + "velocityX": -2.5522794329271568, + "velocityY": -2.1916933685759186, + "timestamp": 2.7376273870229095 + }, + { + "x": 7.201584646398764, + "y": 4.645022669025462, + "heading": 0.45644938425338794, + "angularVelocity": -0.33831851581111005, + "velocityX": -2.8896469952087562, + "velocityY": -2.1511285903218442, + "timestamp": 2.8025866679310543 + }, + { + "x": 6.996114167384252, + "y": 4.5183305598699945, + "heading": 0.4409940125357126, + "angularVelocity": -0.23792399641137907, + "velocityX": -3.16306578739771, + "velocityY": -1.9503311518274693, + "timestamp": 2.867545948839199 + }, + { + "x": 6.77954298581377, + "y": 4.410738153252425, + "heading": 0.4262239025498049, + "angularVelocity": -0.2273748997744145, + "velocityX": -3.3339528785227577, + "velocityY": -1.656305382593562, + "timestamp": 2.9325052297473437 + }, + { + "x": 6.556274698008425, + "y": 4.324091773722086, + "heading": 0.4025359766205174, + "angularVelocity": -0.3646580688413556, + "velocityX": -3.43704986699365, + "velocityY": -1.3338568149000936, + "timestamp": 2.9974645106554885 + }, + { + "x": 6.330620508913663, + "y": 4.259333422241355, + "heading": 0.3712479640794565, + "angularVelocity": -0.4816557711792279, + "velocityX": -3.4737790495840706, + "velocityY": -0.996906840337434, + "timestamp": 3.062423791563633 + }, + { + "x": 6.105727463248204, + "y": 4.216621857922548, + "heading": 0.33499279771425994, + "angularVelocity": -0.5581214240419701, + "velocityX": -3.462061810435755, + "velocityY": -0.6575128868683483, + "timestamp": 3.127383072471778 + }, + { + "x": 5.88368699546657, + "y": 4.195803829122771, + "heading": 0.29554108787827377, + "angularVelocity": -0.6073298423942238, + "velocityX": -3.4181484874441033, + "velocityY": -0.3204781288945418, + "timestamp": 3.1923423533799227 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.2541387867771501, + "angularVelocity": -0.6373577496904229, + "velocityX": -3.351873105763798, + "velocityY": 0.013011927161991938, + "timestamp": 3.2573016342880674 + }, + { + "x": 5.421845873128357, + "y": 4.226158090162152, + "heading": 0.20498652451135702, + "angularVelocity": -0.6557520326399592, + "velocityX": -3.256674335436179, + "velocityY": 0.3936868024775833, + "timestamp": 3.3322571964811623 + }, + { + "x": 5.1888462982312005, + "y": 4.282903811630084, + "heading": 0.15538584116955914, + "angularVelocity": -0.6617345249712095, + "velocityX": -3.1085027992574967, + "velocityY": 0.7570581796417871, + "timestamp": 3.407212758674257 + }, + { + "x": 4.97232882862877, + "y": 4.364011234145266, + "heading": 0.10667736071804554, + "angularVelocity": -0.6498314338038612, + "velocityX": -2.8886111086013, + "velocityY": 1.08207343313948, + "timestamp": 3.482168320867352 + }, + { + "x": 4.778685496187651, + "y": 4.463599711895904, + "heading": 0.06061512765816293, + "angularVelocity": -0.6145272173560689, + "velocityX": -2.5834417990524554, + "velocityY": 1.3286335908479392, + "timestamp": 3.557123883060447 + }, + { + "x": 4.613047571450946, + "y": 4.572114931147961, + "heading": 0.018966273025947666, + "angularVelocity": -0.5556472850530078, + "velocityX": -2.2098149875789206, + "velocityY": 1.4477273744209653, + "timestamp": 3.6320794452535416 + }, + { + "x": 4.476747715125104, + "y": 4.67904248790438, + "heading": -0.017090034062622824, + "angularVelocity": -0.4810357768471503, + "velocityX": -1.8184088323521106, + "velocityY": 1.4265459910894789, + "timestamp": 3.7070350074466365 + }, + { + "x": 4.368193539344783, + "y": 4.776321878924991, + "heading": -0.04707218407807139, + "angularVelocity": -0.3999990012510431, + "velocityX": -1.4482471027389752, + "velocityY": 1.2978275150549403, + "timestamp": 3.7819905696397313 + }, + { + "x": 4.2849948544017495, + "y": 4.858769971637843, + "heading": -0.07089061350138144, + "angularVelocity": -0.3177673374252157, + "velocityX": -1.1099734630593825, + "velocityY": 1.0999596334219341, + "timestamp": 3.856946131832826 + }, + { + "x": 4.224952920077694, + "y": 4.923143218233113, + "heading": -0.08860420951463882, + "angularVelocity": -0.23632130151495423, + "velocityX": -0.8010337400896302, + "velocityY": 0.8588188082618416, + "timestamp": 3.931901694025921 + }, + { + "x": 4.186266834046781, + "y": 4.967343359425017, + "heading": -0.10031361199761639, + "angularVelocity": -0.1562179262002281, + "velocityX": -0.5161202837923176, + "velocityY": 0.5896846064343946, + "timestamp": 4.006857256219016 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.07749108715135035, + "velocityX": -0.250385269598314, + "velocityY": 0.3015711680564945, + "timestamp": 4.081812818412111 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -7.372861805121809e-39, + "velocityX": 4.014609310130765e-40, + "velocityY": -1.5891469244205483e-39, + "timestamp": 4.156768380605206 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.traj b/src/main/deploy/choreo/CenterLanePCBAFE.traj new file mode 100644 index 00000000..15ead10a --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFE.traj @@ -0,0 +1,1813 @@ +{ + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": -8.364664059409844e-30, + "velocityX": -6.773677606947568e-30, + "velocityY": -2.308132540143278e-30, + "timestamp": 0 + }, + { + "x": 1.3079899492454155, + "y": 5.5701428985236205, + "heading": -0.011966271801548406, + "angularVelocity": -0.16457159783011938, + "velocityX": 0.24776633609486393, + "velocityY": -0.2862183198381729, + "timestamp": 0.07271164623703327 + }, + { + "x": 1.3440211933652915, + "y": 5.528520469164234, + "heading": -0.03588810704483216, + "angularVelocity": -0.328995924069994, + "velocityX": 0.49553607960947427, + "velocityY": -0.5724313987522216, + "timestamp": 0.14542329247406655 + }, + { + "x": 1.398068831056723, + "y": 5.466087418740183, + "heading": -0.07173813692543105, + "angularVelocity": -0.49304384837450815, + "velocityX": 0.7433147299769343, + "velocityY": -0.8586389341691013, + "timestamp": 0.21813493871109982 + }, + { + "x": 1.4701339496615213, + "y": 5.382844295736326, + "heading": -0.11947590052159912, + "angularVelocity": -0.6565353154432494, + "velocityX": 0.9911083345055308, + "velocityY": -1.1448389262999967, + "timestamp": 0.2908465849481331 + }, + { + "x": 1.5602181142824867, + "y": 5.2787919544216955, + "heading": -0.17905308310612672, + "angularVelocity": -0.819362312243441, + "velocityX": 1.2389234638197153, + "velocityY": -1.431027169709696, + "timestamp": 0.3635582311851664 + }, + { + "x": 1.668323429942649, + "y": 5.153931763984036, + "heading": -0.25042016061718175, + "angularVelocity": -0.9815082068514337, + "velocityX": 1.4867675434162453, + "velocityY": -1.717196582745939, + "timestamp": 0.4362698774221997 + }, + { + "x": 1.7944527928954905, + "y": 5.008265979195592, + "heading": -0.33353344500312104, + "angularVelocity": -1.1430532616554276, + "velocityX": 1.7346514550472283, + "velocityY": -2.003334986008902, + "timestamp": 0.508981523659233 + }, + { + "x": 1.938611553409707, + "y": 4.8417992681651185, + "heading": -0.42836015839962444, + "angularVelocity": -1.3041475239812899, + "velocityX": 1.98260894865471, + "velocityY": -2.2894091890060286, + "timestamp": 0.5816931698962663 + }, + { + "x": 2.09250704020051, + "y": 4.700832043022275, + "heading": -0.5059805202146961, + "angularVelocity": -1.0675093444479318, + "velocityX": 2.116517707491784, + "velocityY": -1.9387159062918606, + "timestamp": 0.6544048161332996 + }, + { + "x": 2.228385575629257, + "y": 4.5806756631537535, + "heading": -0.5718968876072178, + "angularVelocity": -0.906544835648882, + "velocityX": 1.8687313857908063, + "velocityY": -1.652505287385456, + "timestamp": 0.7271164623703329 + }, + { + "x": 2.346242849494598, + "y": 4.481326236786965, + "heading": -0.6261128487399352, + "angularVelocity": -0.7456296746751315, + "velocityX": 1.6208857861127495, + "velocityY": -1.3663481918284743, + "timestamp": 0.7998281086073662 + }, + { + "x": 2.446077177803767, + "y": 4.402782122456306, + "heading": -0.6686283268670905, + "angularVelocity": -0.5847134582152551, + "velocityX": 1.3730170265673516, + "velocityY": -1.080213671266156, + "timestamp": 0.8725397548443995 + }, + { + "x": 2.5278876403134056, + "y": 4.3450423723526415, + "heading": -0.6994420265312672, + "angularVelocity": -0.423779425385222, + "velocityX": 1.12513561096113, + "velocityY": -0.7940921858762265, + "timestamp": 0.9452514010814328 + }, + { + "x": 2.591673696422735, + "y": 4.308106404528037, + "heading": -0.718552253950641, + "angularVelocity": -0.2628220980484028, + "velocityX": 0.8772467604995492, + "velocityY": -0.5079787039876672, + "timestamp": 1.017963047318466 + }, + { + "x": 2.637435055651834, + "y": 4.2919738867991235, + "heading": -0.7259572155509334, + "angularVelocity": -0.10184010376417504, + "velocityX": 0.6293539150825374, + "velocityY": -0.22186979060047113, + "timestamp": 1.0906746935554992 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05916659407406203, + "velocityX": 0.38145976628941564, + "velocityY": 0.06423731409121712, + "timestamp": 1.1633863397925324 + }, + { + "x": 2.6747076115982096, + "y": 4.323065260999168, + "heading": -0.7050775877835139, + "angularVelocity": 0.2234613634683759, + "velocityX": 0.12854299211755085, + "velocityY": 0.35614342430781004, + "timestamp": 1.237571545918339 + }, + { + "x": 2.6654811031724366, + "y": 4.37114137531458, + "heading": -0.6763165724415564, + "angularVelocity": 0.38769205939503093, + "velocityX": -0.1243712717061632, + "velocityY": 0.6480552770879449, + "timestamp": 1.3117567520441455 + }, + { + "x": 2.6374924128586628, + "y": 4.440873637695316, + "heading": -0.6353766207501428, + "angularVelocity": 0.5518613996935385, + "velocityX": -0.3772812906679928, + "velocityY": 0.9399753133925751, + "timestamp": 1.3859419581699521 + }, + { + "x": 2.590742000006679, + "y": 4.532262849422754, + "heading": -0.5822609370446827, + "angularVelocity": 0.7159875462663353, + "velocityX": -0.6301851176982644, + "velocityY": 1.231906150790299, + "timestamp": 1.4601271642957587 + }, + { + "x": 2.525230488767617, + "y": 4.645309988591984, + "heading": -0.5169687243847326, + "angularVelocity": 0.8801244354174206, + "velocityX": -0.8830805313123746, + "velocityY": 1.5238501726898892, + "timestamp": 1.5343123704215653 + }, + { + "x": 2.4409586345992826, + "y": 4.780016081558229, + "heading": -0.4394900359726193, + "angularVelocity": 1.0443954051766633, + "velocityX": -1.1359657615273402, + "velocityY": 1.8158080296257462, + "timestamp": 1.608497576547372 + }, + { + "x": 2.337926784653071, + "y": 4.936381539754754, + "heading": -0.3497973159677186, + "angularVelocity": 1.2090378215045976, + "velocityX": -1.3888463122287895, + "velocityY": 2.107771432581833, + "timestamp": 1.6826827826731785 + }, + { + "x": 2.2066781223966365, + "y": 5.088037063238321, + "heading": -0.26257787440108193, + "angularVelocity": 1.1756985809634768, + "velocityX": -1.7692026362756765, + "velocityY": 2.044282565348452, + "timestamp": 1.756867988798985 + }, + { + "x": 2.094180452686652, + "y": 5.218026595083209, + "heading": -0.18768356705291486, + "angularVelocity": 1.009558526050363, + "velocityX": -1.5164434471606913, + "velocityY": 1.7522298398549971, + "timestamp": 1.8310531949247917 + }, + { + "x": 2.0004335399095625, + "y": 5.326350877049268, + "heading": -0.12517882612598913, + "angularVelocity": 0.8425499394477148, + "velocityX": -1.2636874340540831, + "velocityY": 1.460187113126797, + "timestamp": 1.9052384010505983 + }, + { + "x": 1.9254366143080366, + "y": 5.4130101547737155, + "heading": -0.07512185913342963, + "angularVelocity": 0.6747567285776663, + "velocityX": -1.0109417971010437, + "velocityY": 1.1681476975361738, + "timestamp": 1.9794236071764049 + }, + { + "x": 1.869189070291752, + "y": 5.478004602849777, + "heading": -0.037557912625022415, + "angularVelocity": 0.5063536043287745, + "velocityX": -0.7582043233686032, + "velocityY": 0.876110635424555, + "timestamp": 2.0536088133022115 + }, + { + "x": 1.8316905948823166, + "y": 5.5213343174096465, + "heading": -0.012514751718529437, + "angularVelocity": 0.3375762124977515, + "velocityX": -0.5054710685139713, + "velocityY": 0.5840748691542834, + "timestamp": 2.127794019428018 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 2.368719711095728e-29, + "angularVelocity": 0.1686960564308085, + "velocityX": -0.2527377395078362, + "velocityY": 0.2920386866937519, + "timestamp": 2.2019792255538246 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.1956060311838367e-29, + "angularVelocity": -1.025164181333131e-28, + "velocityX": 1.21885672142624e-30, + "velocityY": -5.1343833235782786e-30, + "timestamp": 2.2761644316796312 + }, + { + "x": 1.8602093956334294, + "y": 5.5430660724686875, + "heading": -1.6999094202020734e-20, + "angularVelocity": -1.7890242544799916e-19, + "velocityX": 0.49746156397034486, + "velocityY": 0.0007030702226192673, + "timestamp": 2.3711832340882935 + }, + { + "x": 1.9547457978922331, + "y": 5.543199682247202, + "heading": -3.210902716840507e-20, + "angularVelocity": -1.5902045262092918e-19, + "velocityX": 0.9949231085045297, + "velocityY": 0.001406140417769105, + "timestamp": 2.4662020364969557 + }, + { + "x": 2.0965503966634373, + "y": 5.543400096908449, + "heading": -4.532979164623425e-20, + "angularVelocity": -1.3913840358532023e-19, + "velocityX": 1.492384614166397, + "velocityY": 0.0021092105579800877, + "timestamp": 2.561220838905618 + }, + { + "x": 2.2856231808662413, + "y": 5.543667316436768, + "heading": -7.554638691710094e-20, + "angularVelocity": -3.1800648403053284e-19, + "velocityX": 1.9898460032113345, + "velocityY": 0.0028122805333745363, + "timestamp": 2.65623964131428 + }, + { + "x": 2.474695965069045, + "y": 5.543934535965086, + "heading": -6.169486717931722e-20, + "angularVelocity": 1.457766188026957e-19, + "velocityX": 1.9898460032113345, + "velocityY": 0.002812280533374536, + "timestamp": 2.7512584437229424 + }, + { + "x": 2.6165005638402494, + "y": 5.544134950626333, + "heading": -3.084743805748469e-20, + "angularVelocity": 3.2464552635862006e-19, + "velocityX": 1.492384614166397, + "velocityY": 0.0021092105579800877, + "timestamp": 2.8462772461316046 + }, + { + "x": 2.711036966099053, + "y": 5.544268560404848, + "heading": -1.8891361886639493e-21, + "angularVelocity": 3.047639112965043e-19, + "velocityX": 0.9949231085045297, + "velocityY": 0.001406140417769105, + "timestamp": 2.941296048540267 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -8.137968740525874e-34, + "angularVelocity": 1.9881709101649526e-20, + "velocityX": 0.49746156397034486, + "velocityY": 0.0007030702226192673, + "timestamp": 3.036314850948929 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -8.137968740525874e-34, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -9.422453604168856e-36, + "timestamp": 3.1313336533575913 + }, + { + "x": 2.744670053824415, + "y": 5.560420833392183, + "heading": 0.008590361313936763, + "angularVelocity": 0.13502733079351698, + "velocityX": -0.21432312628245512, + "velocityY": 0.2528389368381776, + "timestamp": 3.1949530796133994 + }, + { + "x": 2.717653230778774, + "y": 5.592802243922747, + "heading": 0.025816975580253556, + "angularVelocity": 0.27077600789812356, + "velocityX": -0.4246631042695725, + "velocityY": 0.5089862080862002, + "timestamp": 3.2585725058692074 + }, + { + "x": 2.6776153836811907, + "y": 5.641767759246714, + "heading": 0.051750317714318324, + "angularVelocity": 0.4076324427980389, + "velocityX": -0.629333671394567, + "velocityY": 0.7696629505440931, + "timestamp": 3.3221919321250155 + }, + { + "x": 2.6251092372323575, + "y": 5.707734732807458, + "heading": 0.0864931852991208, + "angularVelocity": 0.5461046983527398, + "velocityX": -0.8253162522672026, + "velocityY": 1.0368998502988076, + "timestamp": 3.3858113583808236 + }, + { + "x": 2.5610825168108655, + "y": 5.791356317104455, + "heading": 0.1301885817245676, + "angularVelocity": 0.686824748304888, + "velocityX": -1.006402040849065, + "velocityY": 1.314403307580339, + "timestamp": 3.4494307846366317 + }, + { + "x": 2.48749773492398, + "y": 5.89376509224382, + "heading": 0.18299253621322714, + "angularVelocity": 0.8299973388055953, + "velocityX": -1.1566401367878931, + "velocityY": 1.6097091905165586, + "timestamp": 3.5130502108924397 + }, + { + "x": 2.410156329475762, + "y": 6.016926517894053, + "heading": 0.24447965431113647, + "angularVelocity": 0.9664833796311684, + "velocityX": -1.215688508997792, + "velocityY": 1.9359090909592849, + "timestamp": 3.576669637148248 + }, + { + "x": 2.3496693828620137, + "y": 6.152928215304285, + "heading": 0.3070071930810449, + "angularVelocity": 0.9828372000478389, + "velocityX": -0.9507622148388409, + "velocityY": 2.137738508728141, + "timestamp": 3.640289063404056 + }, + { + "x": 2.309701308910896, + "y": 6.283843034176789, + "heading": 0.36362933631377437, + "angularVelocity": 0.8900134214517557, + "velocityX": -0.6282369443322133, + "velocityY": 2.0577805644789557, + "timestamp": 3.703908489659864 + }, + { + "x": 2.288494901023514, + "y": 6.40507103844379, + "heading": 0.41290495871744193, + "angularVelocity": 0.7745373591634508, + "velocityX": -0.3333322718459105, + "velocityY": 1.905518666256975, + "timestamp": 3.767527915915672 + }, + { + "x": 2.285060268530139, + "y": 6.514915678699493, + "heading": 0.45431002431284384, + "angularVelocity": 0.6508242534114624, + "velocityX": -0.05398716548566081, + "velocityY": 1.7265896082436825, + "timestamp": 3.83114734217148 + }, + { + "x": 2.2988127872129502, + "y": 6.612520892931977, + "heading": 0.48757772897770557, + "angularVelocity": 0.522917395248037, + "velocityX": 0.2161685430408261, + "velocityY": 1.5342045657567287, + "timestamp": 3.894766768427288 + }, + { + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.3925324714794108, + "velocityX": 0.48035043054352317, + "velocityY": 1.3337738607711032, + "timestamp": 3.9583861946830963 + }, + { + "x": 2.3906193814869496, + "y": 6.780920533993851, + "heading": 0.5304788719493444, + "angularVelocity": 0.23275934723222522, + "velocityX": 0.7951498408867412, + "velocityY": 1.0846472026287595, + "timestamp": 4.035411897678633 + }, + { + "x": 2.471491792404757, + "y": 6.8405967325057775, + "heading": 0.5351323155604377, + "angularVelocity": 0.06041416605263601, + "velocityX": 1.0499405753231936, + "velocityY": 0.7747569472411664, + "timestamp": 4.112437600674169 + }, + { + "x": 2.556148280048404, + "y": 6.869576729136414, + "heading": 0.5277899567279947, + "angularVelocity": -0.09532349004160981, + "velocityX": 1.0990680299088356, + "velocityY": 0.37623800242778493, + "timestamp": 4.189463303669705 + }, + { + "x": 2.6173499244947225, + "y": 6.878199709364964, + "heading": 0.518346888239368, + "angularVelocity": -0.12259632981439711, + "velocityX": 0.794561322599876, + "velocityY": 0.11194938693450328, + "timestamp": 4.266489006665242 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.0752536934288536, + "velocityX": 0.4023928513995403, + "velocityY": 0.0187620669061233, + "timestamp": 4.343514709660778 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -4.4995568742972564e-34, + "velocityX": 2.8230892463303505e-33, + "velocityY": 2.0807279166194578e-33, + "timestamp": 4.420540412656314 + }, + { + "x": 2.658706958910465, + "y": 6.86593478207681, + "heading": 0.5043163629464208, + "angularVelocity": -0.14325761626715308, + "velocityX": 0.1802876546124509, + "velocityY": -0.23853061812890558, + "timestamp": 4.4780176824038715 + }, + { + "x": 2.679434758469343, + "y": 6.838509950906704, + "heading": 0.48805940847309726, + "angularVelocity": -0.2828414527120879, + "velocityX": 0.36062602920974224, + "velocityY": -0.4771422040496659, + "timestamp": 4.535494952151429 + }, + { + "x": 2.7105313297582017, + "y": 6.797364836772264, + "heading": 0.464037646160856, + "angularVelocity": -0.4179349926979131, + "velocityX": 0.5410238068968174, + "velocityY": -0.7158501841710775, + "timestamp": 4.592972221898986 + }, + { + "x": 2.7520007950829926, + "y": 6.742492873007072, + "heading": 0.4325704761896421, + "angularVelocity": -0.5474715502218408, + "velocityX": 0.721493305213118, + "velocityY": -0.954672412350015, + "timestamp": 4.650449491646543 + }, + { + "x": 2.803848241216662, + "y": 6.673886273182379, + "heading": 0.39405966194845793, + "angularVelocity": -0.6700181551824853, + "velocityX": 0.9020513041309295, + "velocityY": -1.1936301102334148, + "timestamp": 4.7079267613941 + }, + { + "x": 2.866080023237396, + "y": 6.591535755483362, + "heading": 0.3490218715149645, + "angularVelocity": -0.7835756748937736, + "velocityX": 1.0827198698556684, + "velocityY": -1.4327492948204654, + "timestamp": 4.7654040311416574 + }, + { + "x": 2.938704098816793, + "y": 6.495430149531881, + "heading": 0.29814315673038183, + "angularVelocity": -0.8851971398092591, + "velocityX": 1.2635268845991694, + "velocityY": -1.6720628236793682, + "timestamp": 4.822881300889215 + }, + { + "x": 3.0217302964606527, + "y": 6.385555893825644, + "heading": 0.2423799369377359, + "angularVelocity": -0.9701786469253116, + "velocityX": 1.4445048974753851, + "velocityY": -1.9116122980233827, + "timestamp": 4.880358570636772 + }, + { + "x": 3.1151700095083186, + "y": 6.261896797084101, + "heading": 0.1831716018579807, + "angularVelocity": -1.0301173897055516, + "velocityX": 1.6256811337430896, + "velocityY": -2.151443471213232, + "timestamp": 4.937835840384329 + }, + { + "x": 3.2190322842248182, + "y": 6.124436959606287, + "heading": 0.12297302537717429, + "angularVelocity": -1.0473457898261622, + "velocityX": 1.8070147585761873, + "velocityY": -2.39155127029422, + "timestamp": 4.995313110131886 + }, + { + "x": 3.3332908123673026, + "y": 5.973192452697109, + "heading": 0.06701518728601086, + "angularVelocity": -0.9735646515036827, + "velocityX": 1.9878906678120518, + "velocityY": -2.6313794578874745, + "timestamp": 5.052790379879443 + }, + { + "x": 3.457219648102922, + "y": 5.80867115561654, + "heading": 0.033419490428721374, + "angularVelocity": -0.5845040483802287, + "velocityX": 2.1561364393249893, + "velocityY": -2.8623714696810656, + "timestamp": 5.1102676496270005 + }, + { + "x": 3.588371868131788, + "y": 5.635317112607293, + "heading": 0.03341946780631197, + "angularVelocity": -3.935887960456646e-7, + "velocityX": 2.2818101939234974, + "velocityY": -3.0160451909185437, + "timestamp": 5.167744919374558 + }, + { + "x": 3.719524526285539, + "y": 5.461963401064903, + "heading": 0.033419445184933325, + "angularVelocity": -3.9357086276096175e-7, + "velocityX": 2.281817816499982, + "velocityY": -3.0160394239978356, + "timestamp": 5.225222189122115 + }, + { + "x": 3.8561567314585825, + "y": 5.292894866069507, + "heading": 0.0334194225694706, + "angularVelocity": -3.9346793646231113e-7, + "velocityX": 2.3771519728257604, + "velocityY": -2.9414851425259596, + "timestamp": 5.282699458869672 + }, + { + "x": 4.00579710900262, + "y": 5.135223415824853, + "heading": 0.03341939963642931, + "angularVelocity": -3.9899322628139097e-7, + "velocityX": 2.6034705232392743, + "velocityY": -2.743196587749476, + "timestamp": 5.340176728617229 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.03341937596859772, + "angularVelocity": -4.117772415638071e-7, + "velocityX": 2.813320067337111, + "velocityY": -2.527531676347718, + "timestamp": 5.397653998364786 + }, + { + "x": 4.277933740289957, + "y": 4.900652577427371, + "heading": 0.03341935253431191, + "angularVelocity": -6.240523322284166e-7, + "velocityX": 2.940862673655656, + "velocityY": -2.37792138300258, + "timestamp": 5.435205794264698 + }, + { + "x": 4.3928594316694705, + "y": 4.81721673895127, + "heading": 0.033419330076064284, + "angularVelocity": -5.980605477839765e-7, + "velocityX": 3.060457925523134, + "velocityY": -2.2218867693700792, + "timestamp": 5.47275759016461 + }, + { + "x": 4.5119655190903245, + "y": 4.739865631807423, + "heading": 0.03341930829497473, + "angularVelocity": -5.800279064172658e-7, + "velocityX": 3.1717813906506853, + "velocityY": -2.0598510747665353, + "timestamp": 5.510309386064522 + }, + { + "x": 4.634929904279705, + "y": 4.6688078109694935, + "heading": 0.033419286924895195, + "angularVelocity": -5.690827567994812e-7, + "velocityX": 3.2745274158690463, + "velocityY": -1.8922615852334157, + "timestamp": 5.547861181964434 + }, + { + "x": 4.761195179582567, + "y": 4.6037960104372555, + "heading": 0.03341926571852252, + "angularVelocity": -5.647232617468814e-7, + "velocityX": 3.362429739429795, + "velocityY": -1.7312567608088656, + "timestamp": 5.585412977864346 + }, + { + "x": 4.8874652266060234, + "y": 4.538793478329322, + "heading": 0.03341924451230877, + "angularVelocity": -5.647190300189245e-7, + "velocityX": 3.3625568097996403, + "velocityY": -1.7310099437370914, + "timestamp": 5.622964773764258 + }, + { + "x": 5.0137352745443575, + "y": 4.473790947998577, + "heading": 0.03341922330609504, + "angularVelocity": -5.647190293196981e-7, + "velocityX": 3.3625568341627456, + "velocityY": -1.731009896410775, + "timestamp": 5.6605165696641695 + }, + { + "x": 5.140005505333594, + "y": 4.408788772864344, + "heading": 0.03341920209988365, + "angularVelocity": -5.64718967252867e-7, + "velocityX": 3.362561703461214, + "velocityY": -1.7310004375685482, + "timestamp": 5.698068365564081 + }, + { + "x": 5.267176463182282, + "y": 4.34556692987905, + "heading": 0.033419180889344834, + "angularVelocity": -5.648342058496427e-7, + "velocityX": 3.3865479613183083, + "velocityY": -1.6835903974819586, + "timestamp": 5.735620161463993 + }, + { + "x": 5.39745989861742, + "y": 4.289036580908955, + "heading": 0.03341915955151481, + "angularVelocity": -5.682239561025287e-7, + "velocityX": 3.4694328809835504, + "velocityY": -1.5053966825119904, + "timestamp": 5.773171957363905 + }, + { + "x": 5.530504784906373, + "y": 4.239352458442723, + "heading": 0.03341913783750815, + "angularVelocity": -5.782414966353427e-7, + "velocityX": 3.542969999186219, + "velocityY": -1.323082459189313, + "timestamp": 5.810723753263817 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.033419115463141504, + "angularVelocity": -5.958268068735077e-7, + "velocityX": 3.6069365171092738, + "velocityY": -1.1371861948253605, + "timestamp": 5.848275549163729 + }, + { + "x": 5.808220957669321, + "y": 4.160073254115033, + "heading": 0.03341909344161441, + "angularVelocity": -5.669632847998976e-7, + "velocityX": 3.6628444951256323, + "velocityY": -0.941676170142482, + "timestamp": 5.887116736398908 + }, + { + "x": 5.952250404538064, + "y": 4.131196918833519, + "heading": 0.033419072126499465, + "angularVelocity": -5.487760919877281e-7, + "velocityX": 3.708162832321777, + "velocityY": -0.7434462573626127, + "timestamp": 5.925957923634087 + }, + { + "x": 6.09762364792411, + "y": 4.110103210641541, + "heading": 0.03341905123842176, + "angularVelocity": -5.377816485869475e-7, + "velocityX": 3.7427600373239653, + "velocityY": -0.5430757835556856, + "timestamp": 5.964799110869266 + }, + { + "x": 6.24392007770515, + "y": 4.096848811559285, + "heading": 0.0334190305197359, + "angularVelocity": -5.334205087871439e-7, + "velocityX": 3.7665282704987906, + "velocityY": -0.3412459820551187, + "timestamp": 6.003640298104445 + }, + { + "x": 6.390314919287249, + "y": 4.0847296192319185, + "heading": 0.03341900981240726, + "angularVelocity": -5.331281075500129e-7, + "velocityX": 3.7690619675370143, + "velocityY": -0.31201910111516795, + "timestamp": 6.042481485339624 + }, + { + "x": 6.536709800200056, + "y": 4.07261090201266, + "heading": 0.033418989105078956, + "angularVelocity": -5.331280988936517e-7, + "velocityX": 3.769062980140168, + "velocityY": -0.31200686904551156, + "timestamp": 6.081322672574803 + }, + { + "x": 6.68323027882942, + "y": 4.062119297351164, + "heading": 0.03341896797261103, + "angularVelocity": -5.440736864773728e-7, + "velocityX": 3.772296602114613, + "velocityY": -0.27011544724343456, + "timestamp": 6.120163859809982 + }, + { + "x": 6.828478445747191, + "y": 4.059283961769526, + "heading": 0.02725441210059553, + "angularVelocity": -0.15871182913873783, + "velocityX": 3.739539835337918, + "velocityY": -0.07299816981572388, + "timestamp": 6.159005047045161 + }, + { + "x": 6.965940131033012, + "y": 4.0573223410737915, + "heading": 0.017080975304275284, + "angularVelocity": -0.2619239400361572, + "velocityX": 3.5390701229986914, + "velocityY": -0.05050362348240824, + "timestamp": 6.19784623428034 + }, + { + "x": 7.095541107200079, + "y": 4.056112825219225, + "heading": 0.007563330070755733, + "angularVelocity": -0.2450400183673897, + "velocityX": 3.336689359734222, + "velocityY": -0.03114003305929124, + "timestamp": 6.236687421515519 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, + "angularVelocity": -0.194724482157576, + "velocityX": 3.1345655629883886, + "velocityY": -0.012732337027157752, + "timestamp": 6.275528608750698 + }, + { + "x": 7.3857970557158446, + "y": 4.056503208949951, + "heading": -0.005539516561442828, + "angularVelocity": -0.09285314353970942, + "velocityX": 2.8244854636552765, + "velocityY": 0.014833039029661928, + "timestamp": 6.335187510434235 + }, + { + "x": 7.5357095386456, + "y": 4.058265721996549, + "heading": -0.007640794437617486, + "angularVelocity": -0.03522153135371121, + "velocityX": 2.5128267316246156, + "velocityY": 0.029543169533142722, + "timestamp": 6.394846412117771 + }, + { + "x": 7.667000937583321, + "y": 4.060490426598279, + "heading": -0.007708640614146542, + "angularVelocity": -0.0011372347564986972, + "velocityX": 2.200700905192029, + "velocityY": 0.037290404934565556, + "timestamp": 6.454505313801308 + }, + { + "x": 7.779661923967275, + "y": 4.062917456014214, + "heading": -0.0066168114481254744, + "angularVelocity": 0.018301194544491067, + "velocityX": 1.888418713800162, + "velocityY": 0.0406817649578728, + "timestamp": 6.514164215484844 + }, + { + "x": 7.873689685619936, + "y": 4.06536905162046, + "heading": -0.004961765552273349, + "angularVelocity": 0.027741809673791765, + "velocityX": 1.5760893848068933, + "velocityY": 0.04109354240631638, + "timestamp": 6.573823117168381 + }, + { + "x": 7.949083994059634, + "y": 4.067716008149933, + "heading": -0.003176764942269326, + "angularVelocity": 0.029920105124841948, + "velocityX": 1.263756226013528, + "velocityY": 0.03933958660390468, + "timestamp": 6.633482018851917 + }, + { + "x": 8.005845696502893, + "y": 4.069860191657625, + "heading": -0.0015908607397525072, + "angularVelocity": 0.02658285951909277, + "velocityX": 0.951437268227904, + "velocityY": 0.035940713744034375, + "timestamp": 6.693140920535454 + }, + { + "x": 8.043976076014772, + "y": 4.071724543898811, + "heading": -0.00046248549538235704, + "angularVelocity": 0.018913778372181077, + "velocityX": 0.6391398171247351, + "velocityY": 0.03125019382816773, + "timestamp": 6.75279982221899 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 3.4463826664070866e-33, + "angularVelocity": 0.00775216241552071, + "velocityX": 0.32686633402451676, + "velocityY": 0.02551860543538527, + "timestamp": 6.812458723902527 + }, + { + "x": 8.059339450711764, + "y": 4.074523581107775, + "heading": -0.0006944740465081056, + "angularVelocity": -0.009481698382290156, + "velocityX": -0.0564842507032717, + "velocityY": 0.017429845647464255, + "timestamp": 6.88570235795497 + }, + { + "x": 8.027124349390371, + "y": 4.075207755344852, + "heading": -0.002651211191279339, + "angularVelocity": -0.026715456845985846, + "velocityX": -0.43983482985464195, + "velocityY": 0.009341074428224473, + "timestamp": 6.958945992007413 + }, + { + "x": 7.96683125907587, + "y": 4.075299477238389, + "heading": -0.0058702138612145086, + "angularVelocity": -0.043949248444312826, + "velocityX": -0.8231854016327222, + "velocityY": 0.0012522848534625944, + "timestamp": 7.032189626059856 + }, + { + "x": 7.878460180490193, + "y": 4.074798744928973, + "heading": -0.0103514975430217, + "angularVelocity": -0.06118325148365151, + "velocityX": -1.2065359635542152, + "velocityY": -0.006836530107951673, + "timestamp": 7.105433260112299 + }, + { + "x": 7.7620111146201785, + "y": 4.073705556029158, + "heading": -0.01609509527793809, + "angularVelocity": -0.07841770563710548, + "velocityX": -1.5898865120023296, + "velocityY": -0.01492537766534737, + "timestamp": 7.1786768941647425 + }, + { + "x": 7.617484062863899, + "y": 4.072019907599753, + "heading": -0.023101063438175296, + "angularVelocity": -0.0956529294439546, + "velocityX": -1.9732370413624691, + "velocityY": -0.02301426535169652, + "timestamp": 7.251920528217186 + }, + { + "x": 7.444879027316714, + "y": 4.06974179610174, + "heading": -0.031369488604002356, + "angularVelocity": -0.1128893353367305, + "velocityX": -2.356587542114559, + "velocityY": -0.031103201356480925, + "timestamp": 7.325164162269629 + }, + { + "x": 7.244196011426359, + "y": 4.066871217282435, + "heading": -0.040900495023888005, + "angularVelocity": -0.13012743760176224, + "velocityX": -2.7399379957944494, + "velocityY": -0.0391921954234215, + "timestamp": 7.398407796322072 + }, + { + "x": 7.015435021934675, + "y": 4.063408165830539, + "heading": -0.05169425070794916, + "angularVelocity": -0.14736783371961967, + "velocityX": -3.123288357427606, + "velocityY": -0.047281262005875575, + "timestamp": 7.471651430374515 + }, + { + "x": 6.758596078532163, + "y": 4.059352633648335, + "heading": -0.06375095974291681, + "angularVelocity": -0.1646110162466115, + "velocityX": -3.5066384502250503, + "velocityY": -0.05537043914696755, + "timestamp": 7.544895064426958 + }, + { + "x": 6.481653456514826, + "y": 4.053513800761945, + "heading": -0.06375096147352038, + "angularVelocity": -2.3628040945559003e-8, + "velocityX": -3.7811152545904956, + "velocityY": -0.07971795722490971, + "timestamp": 7.6181386984794015 + }, + { + "x": 6.205372409181029, + "y": 4.073516064315128, + "heading": -0.06375096323470678, + "angularVelocity": -2.4045589953178915e-8, + "velocityX": -3.7720827333058655, + "velocityY": 0.2730921780705308, + "timestamp": 7.691382332531845 + }, + { + "x": 5.9325368870621995, + "y": 4.121391787593611, + "heading": -0.0637509651063581, + "angularVelocity": -2.555377459766275e-8, + "velocityX": -3.725040758128897, + "velocityY": 0.6536502987304462, + "timestamp": 7.764625966584288 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.063750967187294, + "angularVelocity": -2.84111505129003e-8, + "velocityX": -3.639704142076304, + "velocityY": 1.0274925313911556, + "timestamp": 7.837869600636731 + }, + { + "x": 5.417504232216337, + "y": 4.293821782165803, + "heading": -0.06375097411514968, + "angularVelocity": -9.821339780633191e-8, + "velocityX": -3.5221393138546646, + "velocityY": 1.3775780331429743, + "timestamp": 7.90840840517157 + }, + { + "x": 5.186014168311204, + "y": 4.410724788288498, + "heading": -0.06922328693791815, + "angularVelocity": -0.07757875766190163, + "velocityX": -3.281740673543775, + "velocityY": 1.6572864665569722, + "timestamp": 7.9789472097064085 + }, + { + "x": 4.979961148964822, + "y": 4.522014233902487, + "heading": -0.07551597396386121, + "angularVelocity": -0.08920886974821321, + "velocityX": -2.9211300177991313, + "velocityY": 1.5777052977843462, + "timestamp": 8.049486014241246 + }, + { + "x": 4.7980291404215, + "y": 4.623466282585381, + "heading": -0.08170492719079357, + "angularVelocity": -0.08773827778546625, + "velocityX": -2.579176238427287, + "velocityY": 1.4382445145180842, + "timestamp": 8.120024818776084 + }, + { + "x": 4.639563192627529, + "y": 4.713607705102715, + "heading": -0.0874491132160183, + "angularVelocity": -0.08143299369906003, + "velocityX": -2.246507420120879, + "velocityY": 1.27789835838249, + "timestamp": 8.190563623310922 + }, + { + "x": 4.504192007690659, + "y": 4.791698947450768, + "heading": -0.09257307344830198, + "angularVelocity": -0.07264030438385177, + "velocityX": -1.919102341322099, + "velocityY": 1.1070678453231335, + "timestamp": 8.26110242784576 + }, + { + "x": 4.391678654940531, + "y": 4.857296763925047, + "heading": -0.09697023729098278, + "angularVelocity": -0.0623368069770602, + "velocityX": -1.5950561324662005, + "velocityY": 0.9299536178257779, + "timestamp": 8.331641232380598 + }, + { + "x": 4.301859289367306, + "y": 4.910106206970951, + "heading": -0.10056909340764136, + "angularVelocity": -0.051019522380495494, + "velocityX": -1.2733326878096392, + "velocityY": 0.7486580385668955, + "timestamp": 8.402180036915436 + }, + { + "x": 4.234614001747884, + "y": 4.949916980366573, + "heading": -0.10331834169263278, + "angularVelocity": -0.03897497700905296, + "velocityX": -0.9533091475375062, + "velocityY": 0.5643811751297677, + "timestamp": 8.472718841450273 + }, + { + "x": 4.18985129168719, + "y": 4.976571613170809, + "heading": -0.10517936365869324, + "angularVelocity": -0.026382953019019773, + "velocityX": -0.6345827712261082, + "velocityY": 0.37787191007854753, + "timestamp": 8.543257645985111 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.013363372792080578, + "velocityX": -0.31687843925651377, + "velocityY": 0.18962871266842563, + "timestamp": 8.61379645051995 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": 0, + "velocityX": 8.237561081237056e-34, + "velocityY": 0, + "timestamp": 8.684335255054787 + }, + { + "x": 4.18294196758914, + "y": 4.969364336221789, + "heading": -0.10045524830547124, + "angularVelocity": 0.08077125535022475, + "velocityX": 0.22011597884885625, + "velocityY": -0.29338710511594707, + "timestamp": 8.754493279341492 + }, + { + "x": 4.2148538086473515, + "y": 4.929011600999615, + "heading": -0.08896968745487453, + "angularVelocity": 0.16370986736542367, + "velocityX": 0.4548566095276789, + "velocityY": -0.5751692074062589, + "timestamp": 8.824651303628197 + }, + { + "x": 4.264519080629496, + "y": 4.870038214957966, + "heading": -0.07148588735689271, + "angularVelocity": 0.24920599283886813, + "velocityX": 0.7079057953397331, + "velocityY": -0.8405793441481644, + "timestamp": 8.894809327914903 + }, + { + "x": 4.333555645892384, + "y": 4.794138441539978, + "heading": -0.04780159185128592, + "angularVelocity": 0.3375849839901328, + "velocityX": 0.9840152422303832, + "velocityY": -1.081840234095224, + "timestamp": 8.964967352201608 + }, + { + "x": 4.423967613250648, + "y": 4.703932489290492, + "heading": -0.017716503350327845, + "angularVelocity": 0.4288189242332906, + "velocityX": 1.2886903284047762, + "velocityY": -1.2857538844146224, + "timestamp": 9.035125376488313 + }, + { + "x": 4.53803034325044, + "y": 4.603622363293204, + "heading": 0.0188848190363467, + "angularVelocity": 0.5216983054867799, + "velocityX": 1.6257973504736565, + "velocityY": -1.429774099500926, + "timestamp": 9.105283400775019 + }, + { + "x": 4.677578748555051, + "y": 4.49984021599331, + "heading": 0.06181064388576721, + "angularVelocity": 0.6118448357952799, + "velocityX": 1.9890583682108496, + "velocityY": -1.4792626838489913, + "timestamp": 9.175441425061724 + }, + { + "x": 4.842258553230926, + "y": 4.40159118744757, + "heading": 0.11019595374155315, + "angularVelocity": 0.6896618077221814, + "velocityX": 2.3472697007957355, + "velocityY": -1.4003961705682948, + "timestamp": 9.245599449348429 + }, + { + "x": 5.028363428376419, + "y": 4.3176283558190685, + "heading": 0.16247927063581383, + "angularVelocity": 0.7452221955481739, + "velocityX": 2.6526527369836272, + "velocityY": -1.1967673332045712, + "timestamp": 9.315757473635134 + }, + { + "x": 5.230618576677741, + "y": 4.253738965485636, + "heading": 0.2169576127744663, + "angularVelocity": 0.7765090692409344, + "velocityX": 2.8828512541173206, + "velocityY": -0.9106497935623799, + "timestamp": 9.38591549792184 + }, + { + "x": 5.444331250681613, + "y": 4.2129309832647035, + "heading": 0.2722364606827335, + "angularVelocity": 0.7879191079037022, + "velocityX": 3.0461615214607765, + "velocityY": -0.5816580873795356, + "timestamp": 9.456073522208545 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.3273042274639755, + "angularVelocity": 0.7849104552346565, + "velocityX": 3.1588757008538453, + "velocityY": -0.23207478938864265, + "timestamp": 9.52623154649525 + }, + { + "x": 5.81887557997068, + "y": 4.1972183908996925, + "heading": 0.36426543181084436, + "angularVelocity": 0.7759151841130933, + "velocityX": 3.210283328613804, + "velocityY": 0.011951482768166844, + "timestamp": 9.573867170985725 + }, + { + "x": 5.973953798857342, + "y": 4.209469324927334, + "heading": 0.4004931510026103, + "angularVelocity": 0.7605173560600648, + "velocityX": 3.255509307276357, + "velocityY": 0.257180086514691, + "timestamp": 9.6215027954762 + }, + { + "x": 6.130819634438986, + "y": 4.233461866975936, + "heading": 0.4356102472173979, + "angularVelocity": 0.7372023898166727, + "velocityX": 3.2930361942250057, + "velocityY": 0.5036680489703445, + "timestamp": 9.669138419966675 + }, + { + "x": 6.289005438456121, + "y": 4.269254559307002, + "heading": 0.4691390733673604, + "angularVelocity": 0.7038603253047925, + "velocityX": 3.32074588523062, + "velocityY": 0.7513849710991581, + "timestamp": 9.71677404445715 + }, + { + "x": 6.447898093638549, + "y": 4.316894151635714, + "heading": 0.5004573197150548, + "angularVelocity": 0.6574543040567628, + "velocityX": 3.335584594135005, + "velocityY": 1.0000832956066072, + "timestamp": 9.764409668947625 + }, + { + "x": 6.606667388474461, + "y": 4.376390356038531, + "heading": 0.5287266543286071, + "angularVelocity": 0.5934494386486953, + "velocityX": 3.33299492835787, + "velocityY": 1.248985502745193, + "timestamp": 9.8120452934381 + }, + { + "x": 6.764149093497097, + "y": 4.447653029340476, + "heading": 0.5527726659639904, + "angularVelocity": 0.5047905195447082, + "velocityX": 3.3059649518004255, + "velocityY": 1.4959953619626436, + "timestamp": 9.859680917928575 + }, + { + "x": 6.918662738022944, + "y": 4.530331648811429, + "heading": 0.5708777234010636, + "angularVelocity": 0.3800738970199359, + "velocityX": 3.243657371527556, + "velocityY": 1.735646805417357, + "timestamp": 9.90731654241905 + }, + { + "x": 7.06846751913844, + "y": 4.623866769358016, + "heading": 0.5845734852773292, + "angularVelocity": 0.2875109127414552, + "velocityX": 3.144805651607445, + "velocityY": 1.9635539902555645, + "timestamp": 9.954952166909525 + }, + { + "x": 7.211529043645413, + "y": 4.727181705822193, + "heading": 0.5988053175713824, + "angularVelocity": 0.2987644739893997, + "velocityX": 3.0032465415789793, + "velocityY": 2.168858655035301, + "timestamp": 10.0025877914 + }, + { + "x": 7.344594975677938, + "y": 4.83690117801315, + "heading": 0.6149173466892092, + "angularVelocity": 0.3382348670803817, + "velocityX": 2.7934121459693166, + "velocityY": 2.303307101870708, + "timestamp": 10.050223415890475 + }, + { + "x": 7.465817541114002, + "y": 4.947544737637474, + "heading": 0.6314993734499258, + "angularVelocity": 0.34810138290582165, + "velocityX": 2.5447879970651583, + "velocityY": 2.322706184873219, + "timestamp": 10.09785904038095 + }, + { + "x": 7.575645879536293, + "y": 5.054829291458012, + "heading": 0.6474951019450449, + "angularVelocity": 0.33579340391176205, + "velocityX": 2.305592497149964, + "velocityY": 2.252191610125547, + "timestamp": 10.145494664871425 + }, + { + "x": 7.675112095346534, + "y": 5.156311134408354, + "heading": 0.6623068665871409, + "angularVelocity": 0.31093881523601674, + "velocityX": 2.0880636472002183, + "velocityY": 2.1303770872287977, + "timestamp": 10.1931302893619 + }, + { + "x": 7.76511711692712, + "y": 5.250618542879285, + "heading": 0.6755917653702976, + "angularVelocity": 0.27888579031462285, + "velocityX": 1.8894477094255981, + "velocityY": 1.979766392897571, + "timestamp": 10.240765913852375 + }, + { + "x": 7.846351174360072, + "y": 5.336925095447112, + "heading": 0.6871379054751507, + "angularVelocity": 0.2423845646688635, + "velocityX": 1.7053215592711606, + "velocityY": 1.8118068880378715, + "timestamp": 10.28840153834285 + }, + { + "x": 7.919337423332426, + "y": 5.414694341101313, + "heading": 0.6968047845571967, + "angularVelocity": 0.2029338165594727, + "velocityX": 1.5321778553978798, + "velocityY": 1.6325858322640792, + "timestamp": 10.336037162833325 + }, + { + "x": 7.984478950500488, + "y": 5.483555793762207, + "heading": 0.7044936776046891, + "angularVelocity": 0.16141056467161, + "velocityX": 1.3674960256076552, + "velocityY": 1.445587276276031, + "timestamp": 10.3836727873238 + }, + { + "x": 8.05911422318913, + "y": 5.560200385763573, + "heading": 0.7111561296070518, + "angularVelocity": 0.10289890039880417, + "velocityX": 1.1527118676281318, + "velocityY": 1.1837449989371336, + "timestamp": 10.44842034027666 + }, + { + "x": 8.119756898561732, + "y": 5.61995838055026, + "heading": 0.7143078455831314, + "angularVelocity": 0.04867698982190534, + "velocityX": 0.9366018112955389, + "velocityY": 0.9229382742880431, + "timestamp": 10.513167893229522 + }, + { + "x": 8.166344572702991, + "y": 5.662879129934428, + "heading": 0.7141510891264902, + "angularVelocity": -0.002421040633850211, + "velocityX": 0.7195279515069205, + "velocityY": 0.6628937685941038, + "timestamp": 10.577915446182383 + }, + { + "x": 8.198829842984775, + "y": 5.689000472186179, + "heading": 0.7108395294312745, + "angularVelocity": -0.0511457119873956, + "velocityX": 0.5017219771291601, + "velocityY": 0.4034336598136588, + "timestamp": 10.642662999135243 + }, + { + "x": 8.217175483703613, + "y": 5.698352336883545, + "heading": 0.7044936776046891, + "angularVelocity": -0.09800913759946452, + "velocityX": 0.28334106668393205, + "velocityY": 0.1444358013680284, + "timestamp": 10.707410552088104 + }, + { + "x": 8.221318658765867, + "y": 5.6908798741594095, + "heading": 0.6951703689604403, + "angularVelocity": -0.14352542875947455, + "velocityX": 0.06378111032529457, + "velocityY": -0.11503302714668015, + "timestamp": 10.77236983299625 + }, + { + "x": 8.210964366460544, + "y": 5.666753242665305, + "heading": 0.6829793287804198, + "angularVelocity": -0.18767203099521718, + "velocityX": -0.15939665834608732, + "velocityY": -0.3714116159663335, + "timestamp": 10.837329113904394 + }, + { + "x": 8.185835060378174, + "y": 5.6262170290983935, + "heading": 0.6680275277365855, + "angularVelocity": -0.2301718989927961, + "velocityX": -0.3868470483517696, + "velocityY": -0.6240249738021217, + "timestamp": 10.902288394812539 + }, + { + "x": 8.14559839699319, + "y": 5.569575312921383, + "heading": 0.6504460796726778, + "angularVelocity": -0.2706533665107588, + "velocityX": -0.6194136206938888, + "velocityY": -0.8719572536078877, + "timestamp": 10.967247675720683 + }, + { + "x": 8.08985004858283, + "y": 5.497215477528086, + "heading": 0.6303993620580922, + "angularVelocity": -0.30860436467780866, + "velocityX": -0.858204518753663, + "velocityY": -1.11392605308573, + "timestamp": 11.032206956628828 + }, + { + "x": 8.018089118716903, + "y": 5.40964606854142, + "heading": 0.6080990792438148, + "angularVelocity": -0.3432963312172538, + "velocityX": -1.1047063462324833, + "velocityY": -1.348066169489957, + "timestamp": 11.097166237536973 + }, + { + "x": 7.9296824410081745, + "y": 5.307560214485479, + "heading": 0.5838269457597958, + "angularVelocity": -0.3736515112958315, + "velocityX": -1.3609553011176063, + "velocityY": -1.5715360858180332, + "timestamp": 11.162125518445118 + }, + { + "x": 7.823812920396748, + "y": 5.191948689740781, + "heading": 0.5579732566194568, + "angularVelocity": -0.3979983888198644, + "velocityX": -1.6297828290483853, + "velocityY": -1.779753764641825, + "timestamp": 11.227084799353262 + }, + { + "x": 7.699409898040676, + "y": 5.064316508380435, + "heading": 0.5311065320658849, + "angularVelocity": -0.41359331842916214, + "velocityX": -1.9150923565792266, + "velocityY": -1.9648028669039912, + "timestamp": 11.292044080261407 + }, + { + "x": 7.555088273925506, + "y": 4.927129260587564, + "heading": 0.5041069990932381, + "angularVelocity": -0.4156378056405048, + "velocityX": -2.221724472585296, + "velocityY": -2.111896035100176, + "timestamp": 11.357003361169552 + }, + { + "x": 7.389294037285908, + "y": 4.784758435393721, + "heading": 0.47842631175838873, + "angularVelocity": -0.3953351542047192, + "velocityX": -2.5522794329271568, + "velocityY": -2.1916933685759186, + "timestamp": 11.421962642077697 + }, + { + "x": 7.201584646398764, + "y": 4.645022669025462, + "heading": 0.45644938425338794, + "angularVelocity": -0.33831851581111005, + "velocityX": -2.8896469952087562, + "velocityY": -2.1511285903218442, + "timestamp": 11.486921922985841 + }, + { + "x": 6.996114167384252, + "y": 4.5183305598699945, + "heading": 0.4409940125357126, + "angularVelocity": -0.23792399641137907, + "velocityX": -3.16306578739771, + "velocityY": -1.9503311518274693, + "timestamp": 11.551881203893986 + }, + { + "x": 6.77954298581377, + "y": 4.410738153252425, + "heading": 0.4262239025498049, + "angularVelocity": -0.2273748997744145, + "velocityX": -3.3339528785227577, + "velocityY": -1.656305382593562, + "timestamp": 11.61684048480213 + }, + { + "x": 6.556274698008425, + "y": 4.324091773722086, + "heading": 0.4025359766205174, + "angularVelocity": -0.3646580688413556, + "velocityX": -3.43704986699365, + "velocityY": -1.3338568149000936, + "timestamp": 11.681799765710275 + }, + { + "x": 6.330620508913663, + "y": 4.259333422241355, + "heading": 0.3712479640794565, + "angularVelocity": -0.4816557711792279, + "velocityX": -3.4737790495840706, + "velocityY": -0.996906840337434, + "timestamp": 11.74675904661842 + }, + { + "x": 6.105727463248204, + "y": 4.216621857922548, + "heading": 0.33499279771425994, + "angularVelocity": -0.5581214240419701, + "velocityX": -3.462061810435755, + "velocityY": -0.6575128868683483, + "timestamp": 11.811718327526565 + }, + { + "x": 5.88368699546657, + "y": 4.195803829122771, + "heading": 0.29554108787827377, + "angularVelocity": -0.6073298423942238, + "velocityX": -3.4181484874441033, + "velocityY": -0.3204781288945418, + "timestamp": 11.87667760843471 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.2541387867771501, + "angularVelocity": -0.6373577496904229, + "velocityX": -3.351873105763798, + "velocityY": 0.013011927161991938, + "timestamp": 11.941636889342854 + }, + { + "x": 5.421845873128357, + "y": 4.226158090162152, + "heading": 0.20498652451135702, + "angularVelocity": -0.6557520326399592, + "velocityX": -3.256674335436179, + "velocityY": 0.3936868024775833, + "timestamp": 12.01659245153595 + }, + { + "x": 5.1888462982312005, + "y": 4.282903811630084, + "heading": 0.15538584116955914, + "angularVelocity": -0.6617345249712095, + "velocityX": -3.1085027992574967, + "velocityY": 0.7570581796417871, + "timestamp": 12.091548013729044 + }, + { + "x": 4.97232882862877, + "y": 4.364011234145266, + "heading": 0.10667736071804554, + "angularVelocity": -0.6498314338038612, + "velocityX": -2.8886111086013, + "velocityY": 1.08207343313948, + "timestamp": 12.166503575922139 + }, + { + "x": 4.778685496187651, + "y": 4.463599711895904, + "heading": 0.06061512765816293, + "angularVelocity": -0.6145272173560689, + "velocityX": -2.5834417990524554, + "velocityY": 1.3286335908479392, + "timestamp": 12.241459138115234 + }, + { + "x": 4.613047571450946, + "y": 4.572114931147961, + "heading": 0.018966273025947666, + "angularVelocity": -0.5556472850530078, + "velocityX": -2.2098149875789206, + "velocityY": 1.4477273744209653, + "timestamp": 12.316414700308329 + }, + { + "x": 4.476747715125104, + "y": 4.67904248790438, + "heading": -0.017090034062622824, + "angularVelocity": -0.4810357768471503, + "velocityX": -1.8184088323521106, + "velocityY": 1.4265459910894789, + "timestamp": 12.391370262501423 + }, + { + "x": 4.368193539344783, + "y": 4.776321878924991, + "heading": -0.04707218407807139, + "angularVelocity": -0.3999990012510431, + "velocityX": -1.4482471027389752, + "velocityY": 1.2978275150549403, + "timestamp": 12.466325824694518 + }, + { + "x": 4.2849948544017495, + "y": 4.858769971637843, + "heading": -0.07089061350138144, + "angularVelocity": -0.3177673374252157, + "velocityX": -1.1099734630593825, + "velocityY": 1.0999596334219341, + "timestamp": 12.541281386887613 + }, + { + "x": 4.224952920077694, + "y": 4.923143218233113, + "heading": -0.08860420951463882, + "angularVelocity": -0.23632130151495423, + "velocityX": -0.8010337400896302, + "velocityY": 0.8588188082618416, + "timestamp": 12.616236949080708 + }, + { + "x": 4.186266834046781, + "y": 4.967343359425017, + "heading": -0.10031361199761639, + "angularVelocity": -0.1562179262002281, + "velocityX": -0.5161202837923176, + "velocityY": 0.5896846064343946, + "timestamp": 12.691192511273803 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.07749108715135035, + "velocityX": -0.250385269598314, + "velocityY": 0.3015711680564945, + "timestamp": 12.766148073466898 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -7.372861805121809e-39, + "velocityX": 4.014609310130765e-40, + "velocityY": -1.5891469244205483e-39, + "timestamp": 12.841103635659993 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj index e8fd9ccf..3efa5d70 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj @@ -3,308 +3,308 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.2424307943085809e-21, - "angularVelocity": -1.3399063907175068e-21, - "velocityX": -6.613526246981409e-20, - "velocityY": -3.127454467901795e-20, + "heading": 0, + "angularVelocity": -8.928261679081423e-33, + "velocityX": -3.922322521208882e-33, + "velocityY": -1.0855099176840522e-33, "timestamp": 0 }, { - "x": 1.3060573925677244, - "y": 5.572382489557548, - "heading": -0.010694903924415845, - "angularVelocity": -0.14212387666711845, - "velocityX": 0.21372515458978017, - "velocityY": -0.2467996203811156, - "timestamp": 0.07525057840538203 - }, - { - "x": 1.3382234703338376, - "y": 5.5352391647325, - "heading": -0.03207617924105129, - "angularVelocity": -0.2841343650736142, - "velocityX": 0.4274528973429488, - "velocityY": -0.49359520700229137, - "timestamp": 0.15050115681076406 - }, - { - "x": 1.3864731960687624, - "y": 5.479524641793448, - "heading": -0.06412173122661301, - "angularVelocity": -0.4258512381516761, - "velocityX": 0.6411874401150632, - "velocityY": -0.7403866404709017, - "timestamp": 0.2257517352161461 - }, - { - "x": 1.4508074313628532, - "y": 5.405239333646717, - "heading": -0.10679840962568042, - "angularVelocity": -0.5671275796601069, - "velocityX": 0.8549334325048802, - "velocityY": -0.9871725868543993, - "timestamp": 0.30100231362152813 - }, - { - "x": 1.5312274241075188, - "y": 5.312383875814348, - "heading": -0.16006570142322632, - "angularVelocity": -0.7078655463694897, - "velocityX": 1.0686960080417647, - "velocityY": -1.2339500878272134, - "timestamp": 0.37625289202691015 - }, - { - "x": 1.6277348707062913, - "y": 5.200959293585018, - "heading": -0.22388050123959674, - "angularVelocity": -0.8480306885163599, - "velocityX": 1.282481126973902, - "velocityY": -1.480713963806038, - "timestamp": 0.45150347043229216 - }, - { - "x": 1.740332134586227, - "y": 5.070967309798755, - "heading": -0.29820239377925056, - "angularVelocity": -0.9876587544520216, - "velocityX": 1.4962976533331545, - "velocityY": -1.7274549450767482, - "timestamp": 0.5267540488376742 - }, - { - "x": 1.869023591989777, - "y": 4.922411591689274, - "heading": -0.38299783161333695, - "angularVelocity": -1.126841010806393, - "velocityX": 1.7101723352912572, - "velocityY": -1.9741471927192127, - "timestamp": 0.6020046272430563 - }, - { - "x": 2.0218400763966784, - "y": 4.778612689185939, - "heading": -0.4624367890743981, - "angularVelocity": -1.0556590945137452, - "velocityX": 2.030768236539849, - "velocityY": -1.9109341821756607, - "timestamp": 0.6772552056484383 - }, - { - "x": 2.158571900472673, - "y": 4.653385130962418, - "heading": -0.5314200615234237, - "angularVelocity": -0.9167141822805146, - "velocityX": 1.817020240554264, - "velocityY": -1.664140806319243, - "timestamp": 0.7525057840538204 - }, - { - "x": 2.2792154502458053, - "y": 4.546725640045298, - "heading": -0.5899528620113615, - "angularVelocity": -0.777838545939356, - "velocityX": 1.6032242187324346, - "velocityY": -1.4173909779475127, - "timestamp": 0.8277563624592025 - }, - { - "x": 2.383769251108378, - "y": 4.458632746098652, - "heading": -0.6380359031204368, - "angularVelocity": -0.6389723790566404, - "velocityX": 1.3894086009456479, - "velocityY": -1.170660688773468, - "timestamp": 0.9030069408645846 - }, - { - "x": 2.4722324605852046, - "y": 4.389105547577967, - "heading": -0.6756681828251195, - "angularVelocity": -0.5000928963223903, - "velocityX": 1.1755817875613708, - "velocityY": -0.9239423801653125, - "timestamp": 0.9782575192699666 - }, - { - "x": 2.5446045503525854, - "y": 4.33814344893939, - "heading": -0.7028480663782789, - "angularVelocity": -0.36119168954076036, - "velocityX": 0.9617479533181219, - "velocityY": -0.6772319857003534, - "timestamp": 1.0535080976753486 - }, - { - "x": 2.600885196215428, - "y": 4.305746063716301, - "heading": -0.7195737337553716, - "angularVelocity": -0.22226629657236585, - "velocityX": 0.7479098108675392, - "velocityY": -0.4305267269649537, - "timestamp": 1.1287586760807307 - }, - { - "x": 2.6410742311964075, - "y": 4.29191316967755, - "heading": -0.7258433495686913, - "angularVelocity": -0.08331651325714302, - "velocityX": 0.5340694494662552, - "velocityY": -0.1838244214447216, - "timestamp": 1.2040092544861127 + "x": 1.306057392567738, + "y": 5.572382489557579, + "heading": -0.01069490392439116, + "angularVelocity": -0.14212387666683293, + "velocityX": 0.21372515459002697, + "velocityY": -0.2467996203807809, + "timestamp": 0.07525057840535954 + }, + { + "x": 1.33822347033388, + "y": 5.535239164732594, + "heading": -0.0320761792409761, + "angularVelocity": -0.28413436507302803, + "velocityX": 0.4274528973434579, + "velocityY": -0.49359520700160897, + "timestamp": 0.15050115681071907 + }, + { + "x": 1.3864731960688501, + "y": 5.4795246417936365, + "heading": -0.06412173122646007, + "angularVelocity": -0.42585123815077036, + "velocityX": 0.6411874401158554, + "velocityY": -0.7403866404698544, + "timestamp": 0.2257517352160786 + }, + { + "x": 1.4508074313630048, + "y": 5.405239333647035, + "heading": -0.1067984096254206, + "angularVelocity": -0.567127579658856, + "velocityX": 0.8549334325059841, + "velocityY": -0.9871725868529632, + "timestamp": 0.30100231362143814 + }, + { + "x": 1.5312274241077561, + "y": 5.312383875814834, + "heading": -0.16006570142282744, + "angularVelocity": -0.707865546367854, + "velocityX": 1.0686960080432237, + "velocityY": -1.233950087825352, + "timestamp": 0.3762528920267977 + }, + { + "x": 1.6277348707066417, + "y": 5.200959293585713, + "heading": -0.2238805012390217, + "angularVelocity": -0.848030688514272, + "velocityX": 1.2824811269757883, + "velocityY": -1.4807139638036912, + "timestamp": 0.4515034704321572 + }, + { + "x": 1.7403321345867286, + "y": 5.070967309799712, + "heading": -0.29820239377845176, + "angularVelocity": -0.9876587544493436, + "velocityX": 1.4962976533356125, + "velocityY": -1.727454945073796, + "timestamp": 0.5267540488375168 + }, + { + "x": 1.8690235919905007, + "y": 4.922411591690569, + "heading": -0.3829978316122351, + "angularVelocity": -1.126841010802703, + "velocityX": 1.7101723352947207, + "velocityY": -1.9741471927152945, + "timestamp": 0.6020046272428763 + }, + { + "x": 2.021840076397225, + "y": 4.778612689187088, + "heading": -0.46243678907347296, + "angularVelocity": -1.055659094516408, + "velocityX": 2.030768236538101, + "velocityY": -1.9109341821781907, + "timestamp": 0.6772552056482358 + }, + { + "x": 2.158571900473102, + "y": 4.653385130963448, + "heading": -0.5314200615226269, + "angularVelocity": -0.916714182282495, + "velocityX": 1.8170202405532427, + "velocityY": -1.6641408063212961, + "timestamp": 0.7525057840535954 + }, + { + "x": 2.2792154502461437, + "y": 4.546725640046215, + "heading": -0.5899528620106769, + "angularVelocity": -0.7778385459410783, + "velocityX": 1.6032242187317085, + "velocityY": -1.4173909779494496, + "timestamp": 0.8277563624589549 + }, + { + "x": 2.383769251108642, + "y": 4.458632746099449, + "heading": -0.6380359031198586, + "angularVelocity": -0.6389723790582461, + "velocityX": 1.3894086009450728, + "velocityY": -1.1706606887754087, + "timestamp": 0.9030069408643144 + }, + { + "x": 2.472232460585404, + "y": 4.389105547578634, + "heading": -0.6756681828246474, + "angularVelocity": -0.5000928963239504, + "velocityX": 1.175581787560874, + "velocityY": -0.9239423801673171, + "timestamp": 0.978257519269674 + }, + { + "x": 2.544604550352729, + "y": 4.338143448939913, + "heading": -0.7028480663779157, + "angularVelocity": -0.3611916895423175, + "velocityX": 0.9617479533176606, + "velocityY": -0.677231985702458, + "timestamp": 1.0535080976750335 + }, + { + "x": 2.6008851962155197, + "y": 4.305746063716667, + "heading": -0.7195737337551226, + "angularVelocity": -0.22226629657394859, + "velocityX": 0.7479098108670842, + "velocityY": -0.4305267269671826, + "timestamp": 1.128758676080393 + }, + { + "x": 2.641074231196452, + "y": 4.291913169677742, + "heading": -0.7258433495685629, + "angularVelocity": -0.08331651325877153, + "velocityX": 0.5340694494657862, + "velocityY": -0.18382442144709177, + "timestamp": 1.2040092544857526 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.05565717315833812, - "velocityX": 0.3202286619480617, - "velocityY": 0.06287683198324669, - "timestamp": 1.2792598328914948 - }, - { - "x": 2.6731094248331972, - "y": 4.320288497186145, - "heading": -0.7067986870474628, - "angularVelocity": 0.19580509628770462, - "velocityX": 0.10461882238047351, - "velocityY": 0.31162123137301345, - "timestamp": 1.355133384348122 - }, - { - "x": 2.6646882165451484, - "y": 4.3628057123101, - "heading": -0.6813129128721624, - "angularVelocity": 0.33589799984345314, - "velocityX": -0.11099003706005371, - "velocityY": 0.5603693817898957, - "timestamp": 1.431006935804749 - }, - { - "x": 2.6399081500891253, - "y": 4.424196738035329, - "heading": -0.6452019314670412, - "angularVelocity": 0.47593635347047025, - "velocityX": -0.32659689681441373, - "velocityY": 0.8091228701785946, - "timestamp": 1.5068804872613761 - }, - { - "x": 2.5987694540063986, - "y": 4.504462102028234, - "heading": -0.5984691810768386, - "angularVelocity": 0.6159293916394221, - "velocityX": -0.5422007444352149, - "velocityY": 1.057883313117199, - "timestamp": 1.5827540387180032 - }, - { - "x": 2.5412724297468685, - "y": 4.603602434973848, - "heading": -0.5411158632570503, - "angularVelocity": 0.7559065935192721, - "velocityX": -0.7578006189995966, - "velocityY": 1.30665206837309, - "timestamp": 1.6586275901746304 - }, - { - "x": 2.4674174122162253, - "y": 4.7216183838553025, - "heading": -0.4731379534998271, - "angularVelocity": 0.8959368377013766, - "velocityX": -0.973396079566176, - "velocityY": 1.5554293507523191, - "timestamp": 1.7345011416312575 - }, - { - "x": 2.3772045458929667, - "y": 4.8585103095087545, - "heading": -0.3945211417087051, - "angularVelocity": 1.0361556864260284, - "velocityX": -1.1889896359316807, - "velocityY": 1.8042113888883347, - "timestamp": 1.8103746930878846 - }, - { - "x": 2.270632337424553, - "y": 5.014276859543665, - "heading": -0.3052336675309315, - "angularVelocity": 1.176793130987344, - "velocityX": -1.4046028744197523, - "velocityY": 2.0529756027560517, - "timestamp": 1.8862482445445117 - }, - { - "x": 2.1562113194234667, - "y": 5.146461372900032, - "heading": -0.22910323394931606, - "angularVelocity": 1.0033856610117322, - "velocityX": -1.5080487970369372, - "velocityY": 1.7421685266957554, - "timestamp": 1.9621217960011388 - }, - { - "x": 2.05813446327852, - "y": 5.259759383516201, - "heading": -0.1637433708814866, - "angularVelocity": 0.861431444990317, - "velocityX": -1.2926356320754544, - "velocityY": 1.49324775815898, - "timestamp": 2.037995347457766 - }, - { - "x": 1.9764036449833426, - "y": 5.354173277082403, - "heading": -0.10920518207306891, - "angularVelocity": 0.7188036906324902, - "velocityX": -1.077197741849454, - "velocityY": 1.2443584325978703, - "timestamp": 2.1138688989143932 - }, - { - "x": 1.9110189705233616, - "y": 5.429703862947866, - "heading": -0.06553401357536529, - "angularVelocity": 0.5755782833319475, - "velocityX": -0.8617584547542609, - "velocityY": 0.9954797741165524, - "timestamp": 2.1897424503710203 - }, - { - "x": 1.8619803278190585, - "y": 5.486351581956986, - "heading": -0.032764600292238745, - "angularVelocity": 0.43189507613676265, - "velocityX": -0.6463206448473131, - "velocityY": 0.7466069258864354, - "timestamp": 2.2656160018276474 - }, - { - "x": 1.8292876878125428, - "y": 5.524116686024045, - "heading": -0.010918035230093376, - "angularVelocity": 0.287933866844679, - "velocityX": -0.43088321791822976, - "velocityY": 0.4977373978420955, - "timestamp": 2.3414895532842745 + "angularVelocity": 0.055657173156648594, + "velocityX": 0.320228661947563, + "velocityY": 0.06287683198072233, + "timestamp": 1.2792598328911122 + }, + { + "x": 2.6731094248331484, + "y": 4.3202884971859605, + "heading": -0.7067986870475855, + "angularVelocity": 0.19580509628602769, + "velocityX": 0.1046188223798005, + "velocityY": 0.3116212313704759, + "timestamp": 1.355133384347763 + }, + { + "x": 2.6646882165450285, + "y": 4.362805712309731, + "heading": -0.6813129128724049, + "angularVelocity": 0.3358979998417677, + "velocityX": -0.11099003706094854, + "velocityY": 0.5603693817873046, + "timestamp": 1.431006935804414 + }, + { + "x": 2.6399081500889086, + "y": 4.424196738034775, + "heading": -0.6452019314674031, + "angularVelocity": 0.47593635346874824, + "velocityX": -0.3265968968155938, + "velocityY": 0.8091228701758959, + "timestamp": 1.5068804872610648 + }, + { + "x": 2.5987694540060504, + "y": 4.504462102027486, + "heading": -0.5984691810773222, + "angularVelocity": 0.6159293916376241, + "velocityX": -0.542200744436769, + "velocityY": 1.0578833131143173, + "timestamp": 1.5827540387177157 + }, + { + "x": 2.5412724297463463, + "y": 4.603602434972891, + "heading": -0.5411158632576627, + "angularVelocity": 0.7559065935173384, + "velocityX": -0.757800619001658, + "velocityY": 1.3066520683699112, + "timestamp": 1.6586275901743666 + }, + { + "x": 2.4674174122154677, + "y": 4.7216183838541035, + "heading": -0.4731379535005828, + "angularVelocity": 0.8959368376992063, + "velocityX": -0.9733960795689668, + "velocityY": 1.5554293507486547, + "timestamp": 1.7345011416310174 + }, + { + "x": 2.37720454589188, + "y": 4.858510309507254, + "heading": -0.39452114170963426, + "angularVelocity": 1.0361556864234158, + "velocityX": -1.188989635935645, + "velocityY": 1.8042113888838065, + "timestamp": 1.8103746930876683 + }, + { + "x": 2.270632337422942, + "y": 5.01427685954172, + "heading": -0.305233667532112, + "angularVelocity": 1.1767931309836617, + "velocityX": -1.4046028744262238, + "velocityY": 2.0529756027495263, + "timestamp": 1.8862482445443192 + }, + { + "x": 2.156211319422429, + "y": 5.146461372898718, + "heading": -0.2291032339501207, + "angularVelocity": 1.003385661016371, + "velocityX": -1.508048797028907, + "velocityY": 1.7421685267035296, + "timestamp": 1.96212179600097 + }, + { + "x": 2.058134463277846, + "y": 5.259759383515319, + "heading": -0.16374337088202898, + "angularVelocity": 0.8614314449935028, + "velocityX": -1.2926356320702503, + "velocityY": 1.493247758164214, + "timestamp": 2.037995347457621 + }, + { + "x": 1.976403644982925, + "y": 5.354173277081842, + "heading": -0.10920518207341517, + "angularVelocity": 0.7188036906348487, + "velocityX": -1.0771977418457426, + "velocityY": 1.2443584326016996, + "timestamp": 2.113868898914272 + }, + { + "x": 1.9110189705231253, + "y": 5.429703862947542, + "heading": -0.06553401357556585, + "angularVelocity": 0.5755782833336871, + "velocityX": -0.861758454751598, + "velocityY": 0.9954797741193555, + "timestamp": 2.189742450370923 + }, + { + "x": 1.8619803278189464, + "y": 5.486351581956828, + "heading": -0.032764600292336035, + "angularVelocity": 0.43189507613798817, + "velocityX": -0.6463206448454762, + "velocityY": 0.7466069258884014, + "timestamp": 2.265616001827574 + }, + { + "x": 1.829287687812507, + "y": 5.524116686023993, + "heading": -0.010918035230124941, + "angularVelocity": 0.28793386684545474, + "velocityX": -0.43088321791708567, + "velocityY": 0.4977373978433378, + "timestamp": 2.3414895532842253 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 2.4600717676583652e-21, - "angularVelocity": 0.1438977749227231, - "velocityX": -0.21544390526203344, - "velocityY": 0.24886908799668503, - "timestamp": 2.4173631047409017 + "heading": -6.884486248240525e-32, + "angularVelocity": 0.14389777492309389, + "velocityX": -0.21544390526149312, + "velocityY": 0.2488690879972792, + "timestamp": 2.4173631047408763 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.1389483058862038e-21, - "angularVelocity": -1.912999393800189e-21, - "velocityX": -4.5659699774939935e-20, - "velocityY": -6.062533120754166e-20, - "timestamp": 2.4932366561975288 + "heading": -2.79027732569672e-32, + "angularVelocity": 5.39778462149558e-31, + "velocityX": -1.372141826224318e-33, + "velocityY": 4.2150961560958763e-32, + "timestamp": 2.4932366561975274 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj index 0e534664..8d6dd6ad 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj @@ -3,100 +3,100 @@ { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.1389483058862038e-21, - "angularVelocity": -1.912999393800189e-21, - "velocityX": -4.5659699774939935e-20, - "velocityY": -6.062533120754166e-20, + "heading": -2.79027732569672e-32, + "angularVelocity": 5.39778462149558e-31, + "velocityX": -1.372141826224318e-33, + "velocityY": 4.2150961560958763e-32, "timestamp": 0 }, { "x": 1.8507557561824508, "y": 5.5430527114919315, - "heading": -3.663099117513338e-19, - "angularVelocity": -3.946860070844576e-18, + "heading": -1.045695782000071e-18, + "angularVelocity": -1.1232079825061295e-17, "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395093343, + "velocityY": 0.0005740544395097007, "timestamp": 0.09309903404237341 }, { "x": 1.9263848800936068, "y": 5.543159599317717, - "heading": -1.3979636927804314e-18, - "angularVelocity": -1.108125118245985e-17, - "velocityX": 0.8123513276919049, - "velocityY": 0.0011481088593976478, + "heading": -3.434421194050575e-18, + "angularVelocity": -2.5657896847386088e-17, + "velocityX": 0.812351327691905, + "velocityY": 0.0011481088593983807, "timestamp": 0.18619806808474682 }, { "x": 2.039828562987611, "y": 5.543319931052194, - "heading": -3.639885752731889e-18, - "angularVelocity": -2.408104534113515e-17, + "heading": -7.689792330773154e-18, + "angularVelocity": -4.570800524939683e-17, "velocityX": 1.2185269596070252, - "velocityY": 0.0017221632439681264, + "velocityY": 0.0017221632439692253, "timestamp": 0.2792971021271202 }, { "x": 2.1910867994360017, "y": 5.543533706687691, - "heading": -8.31799511662456e-18, - "angularVelocity": -5.0248742235094303e-17, - "velocityX": 1.6247025332136762, - "velocityY": 0.002296217546130332, + "heading": -1.506248986539688e-17, + "angularVelocity": -7.919198744068695e-17, + "velocityX": 1.6247025332136764, + "velocityY": 0.0022962175461317975, "timestamp": 0.37239613616949363 }, { "x": 2.380159562296481, "y": 5.5438009261858445, - "heading": -4.964025869598509e-18, - "angularVelocity": 3.602582219554166e-17, + "heading": -1.506353706937017e-17, + "angularVelocity": -1.1248279684723702e-20, "velocityX": 2.0308778152781284, - "velocityY": 0.0028702714362513803, + "velocityY": 0.0028702714362532117, "timestamp": 0.46549517021186704 }, { "x": 2.5314177987448714, "y": 5.544014701821341, - "heading": -1.164993974430083e-17, - "angularVelocity": -7.181507244960338e-17, - "velocityX": 1.6247025332136764, - "velocityY": 0.0022962175461303325, + "heading": -7.704513323430137e-18, + "angularVelocity": 7.904511385790122e-17, + "velocityX": 1.6247025332136762, + "velocityY": 0.002296217546131797, "timestamp": 0.5585942042542404 }, { "x": 2.6448614816388756, "y": 5.544175033555818, - "heading": -5.397697309971077e-18, - "angularVelocity": 6.71568990871082e-17, + "heading": -3.433430554917341e-18, + "angularVelocity": 4.587676781446345e-17, "velocityX": 1.218526959607025, - "velocityY": 0.0017221632439681266, + "velocityY": 0.0017221632439692251, "timestamp": 0.6516932382966139 }, { "x": 2.7204906055500317, "y": 5.544281921381604, - "heading": -1.7112683408390196e-18, - "angularVelocity": 3.959685518772811e-17, - "velocityX": 0.8123513276919049, - "velocityY": 0.001148108859397648, + "heading": -1.0420071252828256e-18, + "angularVelocity": 2.5686876928777523e-17, + "velocityX": 0.812351327691905, + "velocityY": 0.0011481088593983805, "timestamp": 0.7447922723389873 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": 2.8361562565882696e-27, - "angularVelocity": 1.838116110697215e-17, + "heading": -1.1539777836664478e-35, + "angularVelocity": 1.1192459041074064e-17, "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395093341, + "velocityY": 0.0005740544395097005, "timestamp": 0.8378913063813607 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": 2.9956768552498905e-27, - "angularVelocity": 1.2964344677835282e-27, - "velocityX": -1.3759431197263344e-22, - "velocityY": -2.5705739302596115e-25, + "heading": 0, + "angularVelocity": 7.156872912951633e-35, + "velocityX": -1.751436869795613e-36, + "velocityY": -4.841667244220732e-36, "timestamp": 0.9309903404237341 } ] diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj index 042ccb78..565f82b6 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj @@ -3,191 +3,209 @@ { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": 2.9956768552498905e-27, - "angularVelocity": 1.2964344677835282e-27, - "velocityX": -1.3759431197263344e-22, - "velocityY": -2.5705739302596115e-25, + "heading": 0, + "angularVelocity": 7.156872912951633e-35, + "velocityX": -1.751436869795613e-36, + "velocityY": -4.841667244220732e-36, "timestamp": 0 }, { - "x": 2.7425045239965504, - "y": 5.564231116573788, - "heading": 0.0035475080989150498, - "angularVelocity": 0.04647392993401548, - "velocityX": -0.20699544832908057, - "velocityY": 0.26064316841975155, - "timestamp": 0.07633329275040568 - }, - { - "x": 2.7110743119163434, - "y": 5.604155225406367, - "heading": 0.01083868300587181, - "angularVelocity": 0.09551762598264181, - "velocityX": -0.4117497221425694, - "velocityY": 0.523023537883034, - "timestamp": 0.15266658550081136 - }, - { - "x": 2.6642693453223805, - "y": 5.6643007250236845, - "heading": 0.022164936239812177, - "angularVelocity": 0.14837894221299866, - "velocityX": -0.6131658272230635, - "velocityY": 0.7879327283048184, - "timestamp": 0.22899987825121704 - }, - { - "x": 2.60250917282708, - "y": 5.744974159519079, - "heading": 0.038001263541227126, - "angularVelocity": 0.20746291337380784, - "velocityX": -0.8090856593497568, - "velocityY": 1.0568577823464311, - "timestamp": 0.3053331710016227 - }, - { - "x": 2.526611736042362, - "y": 5.846734963341242, - "heading": 0.05925383993510262, - "angularVelocity": 0.27841817938297336, - "velocityX": -0.994290093483679, - "velocityY": 1.333111675856818, - "timestamp": 0.3816664637520284 - }, - { - "x": 2.4388385302471143, - "y": 5.970894906525885, - "heading": 0.08830658815011594, - "angularVelocity": 0.3806038907558967, - "velocityX": -1.149867936160551, - "velocityY": 1.6265503387967306, - "timestamp": 0.4579997565024341 - }, - { - "x": 2.357791815962569, - "y": 6.119054185868648, - "heading": 0.1383597950882533, - "angularVelocity": 0.655719216800998, - "velocityX": -1.0617479131884362, - "velocityY": 1.9409522896806517, - "timestamp": 0.5343330492528398 - }, - { - "x": 2.2985967230184703, - "y": 6.254273676601559, - "heading": 0.19308840139911443, - "angularVelocity": 0.7169690228065029, - "velocityX": -0.7754819792414106, - "velocityY": 1.7714353182044804, - "timestamp": 0.6106663420032454 - }, - { - "x": 2.2587354140798723, - "y": 6.372988574603886, - "heading": 0.24867936598579424, - "angularVelocity": 0.7282662988016366, - "velocityX": -0.5222008314109583, - "velocityY": 1.5552178312351754, - "timestamp": 0.6869996347536516 - }, - { - "x": 2.2373189281980324, - "y": 6.474209987526736, - "heading": 0.30392383910912424, - "angularVelocity": 0.7237271069121585, - "velocityX": -0.2805654663931197, - "velocityY": 1.3260454157771489, - "timestamp": 0.7633329275040577 - }, - { - "x": 2.233901000873839, - "y": 6.557480302062636, - "heading": 0.35823088728392755, - "angularVelocity": 0.7114464242015108, - "velocityX": -0.044776364297153176, - "velocityY": 1.090878062972822, - "timestamp": 0.8396662202544638 - }, - { - "x": 2.248214127214941, - "y": 6.6225364653992, - "heading": 0.41124898234582946, - "angularVelocity": 0.6945605665834471, - "velocityX": 0.1875083050315006, - "velocityY": 0.8522646016238898, - "timestamp": 0.9159995130048699 - }, - { - "x": 2.280080256208058, - "y": 6.669207835860357, - "heading": 0.4627444117016496, - "angularVelocity": 0.6746129703090314, - "velocityX": 0.41746042709454745, - "velocityY": 0.6114156586138921, - "timestamp": 0.992332805755276 - }, - { - "x": 2.3293724060058594, - "y": 6.6973748207092285, + "x": 2.7396220511907092, + "y": 5.562601673573336, + "heading": 0.0037078860807813565, + "angularVelocity": 0.0478983711971462, + "velocityX": -0.2413479950107084, + "velocityY": 0.23596367181626482, + "timestamp": 0.07741152753441183 + }, + { + "x": 2.702380969511178, + "y": 5.599259386918254, + "heading": 0.011292704027819494, + "angularVelocity": 0.09798047123752286, + "velocityX": -0.4810792767650385, + "velocityY": 0.47354334053959213, + "timestamp": 0.15482305506882366 + }, + { + "x": 2.6467699433072402, + "y": 5.6544931206396525, + "heading": 0.023007945503191744, + "angularVelocity": 0.15133716965040475, + "velocityX": -0.7183817187849313, + "velocityY": 0.7135078647924301, + "timestamp": 0.23223458260323548 + }, + { + "x": 2.573102880230356, + "y": 5.72860253774289, + "heading": 0.03927230490424403, + "angularVelocity": 0.21010255086133378, + "velocityX": -0.9516291103303323, + "velocityY": 0.9573434275701879, + "timestamp": 0.3096461101376473 + }, + { + "x": 2.4820081686264115, + "y": 5.82215722498829, + "heading": 0.060903903359814396, + "angularVelocity": 0.27943639848670454, + "velocityX": -1.1767589983733413, + "velocityY": 1.2085368965728291, + "timestamp": 0.38705763767205914 + }, + { + "x": 2.3753503305689, + "y": 5.936638406081269, + "heading": 0.09019255741494601, + "angularVelocity": 0.3783500337480349, + "velocityX": -1.3778030411568694, + "velocityY": 1.478864772976981, + "timestamp": 0.46446916520647097 + }, + { + "x": 2.2779168004384966, + "y": 6.074876724828902, + "heading": 0.14378193450398408, + "angularVelocity": 0.6922661106928292, + "velocityX": -1.25864368310122, + "velocityY": 1.7857588288279347, + "timestamp": 0.5418806927408828 + }, + { + "x": 2.2032159820535764, + "y": 6.200217981312059, + "heading": 0.2000311193060264, + "angularVelocity": 0.7266254341388357, + "velocityX": -0.9649831331866354, + "velocityY": 1.6191549304777415, + "timestamp": 0.6192922202752942 + }, + { + "x": 2.149623841782867, + "y": 6.310133396102646, + "heading": 0.2561023628726984, + "angularVelocity": 0.7243267941166335, + "velocityX": -0.6923018053982481, + "velocityY": 1.4198843284900602, + "timestamp": 0.6967037478097056 + }, + { + "x": 2.1165539042639407, + "y": 6.4038530923195935, + "heading": 0.3110558787207687, + "angularVelocity": 0.7098880179523891, + "velocityX": -0.42719655033581183, + "velocityY": 1.2106684779639116, + "timestamp": 0.774115275344117 + }, + { + "x": 2.1037067095365876, + "y": 6.481006876335726, + "heading": 0.36442032903480814, + "angularVelocity": 0.6893605127520233, + "velocityX": -0.16595971086660646, + "velocityY": 0.9966704762651166, + "timestamp": 0.8515268028785283 + }, + { + "x": 2.1109008821512623, + "y": 6.541377447734871, + "heading": 0.4159102858737852, + "angularVelocity": 0.6651458571992167, + "velocityX": 0.09293412549541542, + "velocityY": 0.7798653937207066, + "timestamp": 0.9289383304129397 + }, + { + "x": 2.1380148682221702, + "y": 6.5848219723004595, + "heading": 0.465333297367931, + "angularVelocity": 0.6384451136450677, + "velocityX": 0.3502577320781457, + "velocityY": 0.5612151826648336, + "timestamp": 1.006349857947351 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, "heading": 0.5125504196, - "angularVelocity": 0.652480799710893, - "velocityX": 0.6457490306225382, - "velocityY": 0.3689999976939454, - "timestamp": 1.0686660985056822 - }, - { - "x": 2.4053669705551046, - "y": 6.740800351083451, - "heading": 0.5568713225103131, - "angularVelocity": 0.5531681306272371, - "velocityX": 0.948486345023332, - "velocityY": 0.5419930073900953, - "timestamp": 1.1487880369479013 - }, - { - "x": 2.502567960679766, - "y": 6.796343857044693, - "heading": 0.5404821418694866, - "angularVelocity": -0.204552972125677, - "velocityX": 1.213163236118634, - "velocityY": 0.6932371712561282, - "timestamp": 1.2289099753901205 - }, - { - "x": 2.575459179910046, - "y": 6.837996044585651, - "heading": 0.5268532537172577, - "angularVelocity": -0.17010182750455535, - "velocityX": 0.9097535662201399, - "velocityY": 0.5198599578440831, - "timestamp": 1.3090319138323396 - }, - { - "x": 2.6240500842159764, - "y": 6.865762317125851, - "heading": 0.5173922106041889, - "angularVelocity": -0.11808305311898519, - "velocityX": 0.6064619160577619, - "velocityY": 0.34655018438206425, - "timestamp": 1.3891538522745588 + "angularVelocity": 0.6099494963599527, + "velocityX": 0.6064560493947925, + "velocityY": 0.34126004006795546, + "timestamp": 1.0837613854817625 + }, + { + "x": 2.250707976446258, + "y": 6.648678224064634, + "heading": 0.55208552781375, + "angularVelocity": 0.5325541984016288, + "velocityX": 0.8856313560565373, + "velocityY": 0.5043159387113171, + "timestamp": 1.1579981642592236 + }, + { + "x": 2.3371670619589073, + "y": 6.698226995892447, + "heading": 0.5853560107794161, + "angularVelocity": 0.44816711492022887, + "velocityX": 1.1646395080237382, + "velocityY": 0.6674423734944788, + "timestamp": 1.2322349430366848 + }, + { + "x": 2.4409073566894506, + "y": 6.758690881227722, + "heading": 0.5617681472290976, + "angularVelocity": -0.31773824159353153, + "velocityX": 1.3974245170513666, + "velocityY": 0.8144734501011613, + "timestamp": 1.306471721814146 + }, + { + "x": 2.523886913207481, + "y": 6.807069780587539, + "heading": 0.5422957792802172, + "angularVelocity": -0.2623008200187737, + "velocityX": 1.1177688186980357, + "velocityY": 0.6516837092951017, + "timestamp": 1.380708500591607 + }, + { + "x": 2.5861173110961473, + "y": 6.843356427328165, + "heading": 0.5274956073850987, + "angularVelocity": -0.1993644139582728, + "velocityX": 0.8382691021011789, + "velocityY": 0.4887960838037212, + "timestamp": 1.4549452793690683 + }, + { + "x": 2.627602512984248, + "y": 6.8675485030782895, + "heading": 0.5175502299027057, + "angularVelocity": -0.13396833276139558, + "velocityX": 0.5588227637470666, + "velocityY": 0.3258772288954636, + "timestamp": 1.5291820581465294 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.06043027787801988, - "velocityX": 0.30321823224102623, - "velocityY": 0.17326782030138713, - "timestamp": 1.469275790716778 + "angularVelocity": -0.06734950498988643, + "velocityX": 0.2794033376916902, + "velocityY": 0.16294305705300594, + "timestamp": 1.6034188369239906 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": 1.1533672139704627e-20, - "velocityX": 1.3759845110959627e-18, - "velocityY": -2.227992646788969e-18, - "timestamp": 1.549397729158997 + "angularVelocity": -4.084231073354503e-32, + "velocityX": -1.1079933009257942e-33, + "velocityY": -1.0209152810451872e-34, + "timestamp": 1.6776556157014517 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj index 0cbffe6a..d8cf383e 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj @@ -4,847 +4,847 @@ "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": 1.1533672139704627e-20, - "velocityX": 1.3759845110959627e-18, - "velocityY": -2.227992646788969e-18, + "angularVelocity": -4.084231073354503e-32, + "velocityX": -1.1079933009257942e-33, + "velocityY": -1.0209152810451872e-34, "timestamp": 0 }, { - "x": 2.6571120181256966, - "y": 6.867792271481583, - "heading": 0.507044677895467, - "angularVelocity": -0.09452799306194601, - "velocityX": 0.15052909368010856, - "velocityY": -0.20349709127294008, - "timestamp": 0.058244563606941746 - }, - { - "x": 2.6746521578330125, - "y": 6.844087252656105, - "heading": 0.49617547030513726, - "angularVelocity": -0.18661325482116445, - "velocityX": 0.30114638381847736, - "velocityY": -0.40699109680773127, - "timestamp": 0.11648912721388349 - }, - { - "x": 2.7009708210230947, - "y": 6.808530072252695, - "heading": 0.4801087501886783, - "angularVelocity": -0.27584926594838455, - "velocityX": 0.45186471595342564, - "velocityY": -0.6104806732412771, - "timestamp": 0.17473369082082524 - }, - { - "x": 2.73607484037394, - "y": 6.7611211343580875, - "heading": 0.4590398225372461, - "angularVelocity": -0.361732088742461, - "velocityX": 0.602700358229852, - "velocityY": -0.8139633119159942, - "timestamp": 0.23297825442776698 - }, - { - "x": 2.7799722580023465, - "y": 6.701861088549211, - "heading": 0.4332010241003249, - "angularVelocity": -0.4436259255248601, - "velocityX": 0.7536740754835817, - "velocityY": -1.0174347980145875, - "timestamp": 0.29122281803470873 - }, - { - "x": 2.832672676333526, - "y": 6.630750980608444, - "heading": 0.40287238280523807, - "angularVelocity": -0.5207119672104833, - "velocityX": 0.9048126566253185, - "velocityY": -1.2208883290919084, - "timestamp": 0.3494673816416505 - }, - { - "x": 2.8941877403139116, - "y": 6.547792490679094, - "heading": 0.3683969681281308, - "angularVelocity": -0.5919078544353333, - "velocityX": 1.0561511696699046, - "velocityY": -1.4243130138151228, - "timestamp": 0.4077119452485922 - }, - { - "x": 2.9645318213846927, - "y": 6.452988329970027, - "heading": 0.33020407419243936, - "angularVelocity": -0.6557331975810253, - "velocityX": 1.2077364257631285, - "velocityY": -1.627691149835798, - "timestamp": 0.46595650885553397 - }, - { - "x": 3.04372302387486, - "y": 6.346342948824998, - "heading": 0.28884632079593436, - "angularVelocity": -0.7100706200771626, - "velocityX": 1.3596325147971153, - "velocityY": -1.8309928779742386, - "timestamp": 0.5242010724624757 - }, - { - "x": 3.1317847243622103, - "y": 6.227863919029491, - "heading": 0.24506332676078993, - "angularVelocity": -0.7517095386036364, - "velocityX": 1.5119299559290544, - "velocityY": -2.0341646062463727, - "timestamp": 0.5824456360694175 - }, - { - "x": 3.2287480012560774, - "y": 6.097564965294096, - "heading": 0.199900944291634, - "angularVelocity": -0.7753922370151164, - "velocityX": 1.6647609817838902, - "velocityY": -2.237100695177408, - "timestamp": 0.6406901996763592 - }, - { - "x": 3.334655409720646, - "y": 5.955473736284568, - "heading": 0.1549620363959118, - "angularVelocity": -0.7715554055652005, - "velocityX": 1.8183226365858802, - "velocityY": -2.4395620845993164, - "timestamp": 0.698934763283301 - }, - { - "x": 3.4495650121656865, - "y": 5.8016567423537415, - "heading": 0.11303057394210353, - "angularVelocity": -0.7199206218932102, - "velocityX": 1.972881163991502, - "velocityY": -2.6408815588154475, - "timestamp": 0.7571793268902427 - }, - { - "x": 3.5735262627096924, - "y": 5.636333431818966, - "heading": 0.08010242298971296, - "angularVelocity": -0.565342907787981, - "velocityX": 2.1282887683826965, - "velocityY": -2.838433328309314, - "timestamp": 0.8154238904971844 - }, - { - "x": 3.7050107789119795, - "y": 5.46057947422046, - "heading": 0.07776405618458973, - "angularVelocity": -0.04014738303996017, - "velocityX": 2.2574555985962665, - "velocityY": -3.017516944320591, - "timestamp": 0.8736684541041262 - }, - { - "x": 3.8485069507845076, - "y": 5.2934525910716275, - "heading": 0.07776403563237232, - "angularVelocity": -3.5286069899218235e-7, - "velocityX": 2.463683526601701, - "velocityY": -2.8693988382619477, - "timestamp": 0.9319130177110679 - }, - { - "x": 4.002902013863093, - "y": 5.136338903873299, - "heading": 0.07776401516857159, - "angularVelocity": -3.5134267388587325e-7, - "velocityX": 2.650806418956214, - "velocityY": -2.697482433873059, - "timestamp": 0.9901575813180097 + "x": 2.6570277255317225, + "y": 6.867726793502589, + "heading": 0.5066730395869481, + "angularVelocity": -0.10087167460547103, + "velocityX": 0.14902725506901718, + "velocityY": -0.20454631284120778, + "timestamp": 0.058265910980853874 + }, + { + "x": 2.6744037356282937, + "y": 6.843893366933042, + "heading": 0.49507655406382295, + "angularVelocity": -0.1990269323504748, + "velocityX": 0.2982191439910878, + "velocityY": -0.4090458068591583, + "timestamp": 0.11653182196170775 + }, + { + "x": 2.7004835818665893, + "y": 6.808147821587349, + "heading": 0.4779456476925187, + "angularVelocity": -0.29401250375942084, + "velocityX": 0.44760042019879576, + "velocityY": -0.6134898561431401, + "timestamp": 0.17479773294256162 + }, + { + "x": 2.735280123191829, + "y": 6.760494081762779, + "heading": 0.4554977531812837, + "angularVelocity": -0.38526634413407634, + "velocityX": 0.5972023905482879, + "velocityY": -0.8178665539139939, + "timestamp": 0.2330636439234155 + }, + { + "x": 2.77880855180183, + "y": 6.700937036637074, + "heading": 0.4279915994110024, + "angularVelocity": -0.4720796999006835, + "velocityX": 0.7470650999399128, + "velocityY": -1.022159340223411, + "timestamp": 0.29132955490426937 + }, + { + "x": 2.8310870878518544, + "y": 6.629482937316093, + "heading": 0.39573903851012515, + "angularVelocity": -0.5535408330177088, + "velocityX": 0.8972405162806606, + "velocityY": -1.2263448407158972, + "timestamp": 0.34959546588512325 + }, + { + "x": 2.8921379557757043, + "y": 6.546140000901142, + "heading": 0.3591220542576228, + "angularVelocity": -0.6284460954285072, + "velocityX": 1.0477973637777922, + "velocityY": -1.4303893136132022, + "timestamp": 0.4078613768659771 + }, + { + "x": 2.9619888074879523, + "y": 6.4509193752842275, + "heading": 0.3186184742438743, + "angularVelocity": -0.6951505491273999, + "velocityX": 1.1988287926228676, + "velocityY": -1.6342424586513933, + "timestamp": 0.466127287846831 + }, + { + "x": 3.0406748905144103, + "y": 6.3438367809482505, + "heading": 0.27484324456730397, + "angularVelocity": -0.7513008711209391, + "velocityX": 1.3504651639672256, + "velocityY": -1.8378257978522827, + "timestamp": 0.5243931988276849 + }, + { + "x": 3.12824252650645, + "y": 6.224915546783436, + "heading": 0.22861956489914778, + "angularVelocity": -0.7933228690673962, + "velocityX": 1.502896539639008, + "velocityY": -2.0410087504491825, + "timestamp": 0.5826591098085387 + }, + { + "x": 3.224755029145573, + "y": 6.094192875417537, + "heading": 0.18111264418965425, + "angularVelocity": -0.815346742370587, + "velocityX": 1.6564145486515685, + "velocityY": -2.243553205730433, + "timestamp": 0.6409250207893926 + }, + { + "x": 3.3303033780389395, + "y": 5.951734868373249, + "heading": 0.13411173965139422, + "angularVelocity": -0.8066621416715647, + "velocityX": 1.8114940128208097, + "velocityY": -2.444963180805693, + "timestamp": 0.6991909317702465 + }, + { + "x": 3.4450256229008205, + "y": 5.797681352790493, + "heading": 0.09073046456880836, + "angularVelocity": -0.7445395489798686, + "velocityX": 1.9689427819909868, + "velocityY": -2.643973345467458, + "timestamp": 0.7574568427511004 + }, + { + "x": 3.5691217392378216, + "y": 5.632434411388871, + "heading": 0.05763038975845234, + "angularVelocity": -0.5680864548952619, + "velocityX": 2.1298236695857957, + "velocityY": -2.8360826874554848, + "timestamp": 0.8157227537319542 + }, + { + "x": 3.7017085484583103, + "y": 5.4576517803314095, + "heading": 0.0547360803841417, + "angularVelocity": -0.049674146093104596, + "velocityX": 2.2755468332771343, + "velocityY": -2.999740810967421, + "timestamp": 0.8739886647128081 + }, + { + "x": 3.8463662629346644, + "y": 5.2914222205797, + "heading": 0.05473605711060691, + "angularVelocity": -3.9943655557036433e-7, + "velocityX": 2.4827160863217212, + "velocityY": -2.8529470655034146, + "timestamp": 0.932254575693662 + }, + { + "x": 4.001863921193704, + "y": 5.135285744872655, + "heading": 0.05473603479828713, + "angularVelocity": -3.8293951656527263e-7, + "velocityX": 2.668758724293121, + "velocityY": -2.679722552666384, + "timestamp": 0.9905204866745159 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.07776399412255879, - "angularVelocity": -3.6133866407320583e-7, - "velocityX": 2.8259641989395843, - "velocityY": -2.513386639709188, - "timestamp": 1.0484021449249514 - }, - { - "x": 4.268976588700125, - "y": 4.906724140265527, - "heading": 0.07776397443770015, - "angularVelocity": -5.672615420255969e-7, - "velocityX": 2.9242931040000055, - "velocityY": -2.398268643648094, - "timestamp": 1.0831037017429557 - }, - { - "x": 4.37370366170879, - "y": 4.827628597038176, - "heading": 0.07776395550809555, - "angularVelocity": -5.454972726636976e-7, - "velocityX": 3.0179358683506776, - "velocityY": -2.2793082063198216, - "timestamp": 1.11780525856096 - }, - { - "x": 4.4815124411311835, - "y": 4.752787886123784, - "heading": 0.0777639371726549, - "angularVelocity": -5.283751607380448e-7, - "velocityX": 3.106741867167675, - "velocityY": -2.1566960614159445, - "timestamp": 1.1525068153789642 - }, - { - "x": 4.592230128734662, - "y": 4.682321875197502, - "heading": 0.07776391928788347, - "angularVelocity": -5.153881575305611e-7, - "velocityX": 3.1905683132358433, - "velocityY": -2.030629671626745, - "timestamp": 1.1872083721969684 - }, - { - "x": 4.705679213251417, - "y": 4.616343315466854, - "heading": 0.07776390172302422, - "angularVelocity": -5.061691995708879e-7, - "velocityX": 3.269279390309479, - "velocityY": -1.9013141132738893, - "timestamp": 1.2219099290149726 - }, - { - "x": 4.821677528011508, - "y": 4.554957192131842, - "heading": 0.07776388435588834, - "angularVelocity": -5.004713755679868e-7, - "velocityX": 3.3427409429626933, - "velocityY": -1.7689731805681452, - "timestamp": 1.2566114858329769 - }, - { - "x": 4.939562213059443, - "y": 4.4972764656411774, - "heading": 0.07776386706661859, - "angularVelocity": -4.982274959213974e-7, - "velocityX": 3.397100760239454, - "velocityY": -1.6621942004843482, - "timestamp": 1.2913130426509811 - }, - { - "x": 5.0574486320523055, - "y": 4.439599283026335, - "heading": 0.07776384977740043, - "angularVelocity": -4.982260088149746e-7, - "velocityX": 3.397150727592071, - "velocityY": -1.6620920760798994, - "timestamp": 1.3260145994689854 - }, - { - "x": 5.175337486723699, - "y": 4.381927078962276, - "heading": 0.07776383248820665, - "angularVelocity": -4.9822530674329e-7, - "velocityX": 3.3972209169079175, - "velocityY": -1.6619486084306254, - "timestamp": 1.3607161562869896 - }, - { - "x": 5.295110440251067, - "y": 4.328277244219036, - "heading": 0.07776381519347733, - "angularVelocity": -4.983848248451832e-7, - "velocityX": 3.451515277989629, - "velocityY": -1.5460353846547075, - "timestamp": 1.3954177131049939 - }, - { - "x": 5.416934261342807, - "y": 4.279463089154562, - "heading": 0.07776379778914431, - "angularVelocity": -5.015432909319555e-7, - "velocityX": 3.5106154381100505, - "velocityY": -1.4066848735485782, - "timestamp": 1.430119269922998 - }, - { - "x": 5.540614064021598, - "y": 4.235563528216825, - "heading": 0.07776378015516597, - "angularVelocity": -5.081610154974924e-7, - "velocityX": 3.5640995396097326, - "velocityY": -1.26506027288553, - "timestamp": 1.4648208267410023 + "heading": 0.0547360118053875, + "angularVelocity": -3.946201001676767e-7, + "velocityX": 2.842745293382521, + "velocityY": -2.4943907433712678, + "timestamp": 1.0487863976553697 + }, + { + "x": 4.269399861624288, + "y": 4.9075045981351995, + "heading": 0.054735989995402, + "angularVelocity": -6.29291522756053e-7, + "velocityX": 2.940181103085103, + "velocityY": -2.3787638667331756, + "timestamp": 1.083444397503813 + }, + { + "x": 4.374514707093794, + "y": 4.829200558555379, + "heading": 0.05473596913891511, + "angularVelocity": -6.017798771949806e-7, + "velocityX": 3.0329172464990677, + "velocityY": -2.2593352161762894, + "timestamp": 1.1181023973522564 + }, + { + "x": 4.482675566428291, + "y": 4.7551608256734985, + "heading": 0.0547359490445404, + "angularVelocity": -5.797903745228634e-7, + "velocityX": 3.120805003389626, + "velocityY": -2.1362956086805514, + "timestamp": 1.1527603972006997 + }, + { + "x": 4.593709527898866, + "y": 4.685503714898617, + "heading": 0.05473592954464412, + "angularVelocity": -5.62637669848401e-7, + "velocityX": 3.203703674652828, + "velocityY": -2.0098422032282808, + "timestamp": 1.187418397049143 + }, + { + "x": 4.707439069036232, + "y": 4.620340494746092, + "heading": 0.05473591048922828, + "angularVelocity": -5.498129124049459e-7, + "velocityX": 3.2814802249032198, + "velocityY": -1.8801783264319567, + "timestamp": 1.2220763968975863 + }, + { + "x": 4.823682298536361, + "y": 4.559775113095544, + "heading": 0.054735891740807854, + "angularVelocity": -5.409550611531198e-7, + "velocityX": 3.3540085985473036, + "velocityY": -1.747515203283397, + "timestamp": 1.2567343967460296 + }, + { + "x": 4.942252940190184, + "y": 4.503903323846341, + "heading": 0.05473587317010921, + "angularVelocity": -5.358271892357539e-7, + "velocityX": 3.4211622763091856, + "velocityY": -1.6120892577045927, + "timestamp": 1.291392396594473 + }, + { + "x": 5.062124990174475, + "y": 4.450881200757823, + "heading": 0.05473585465308408, + "angularVelocity": -5.342785275702722e-7, + "velocityX": 3.4587122888938238, + "velocityY": -1.52986679324772, + "timestamp": 1.3260503964429162 + }, + { + "x": 5.1819977334562, + "y": 4.397860645099044, + "heading": 0.05473583613607813, + "angularVelocity": -5.3427797428813e-7, + "velocityX": 3.4587322928593447, + "velocityY": -1.529821567621728, + "timestamp": 1.3607083962913595 + }, + { + "x": 5.301870477238367, + "y": 4.344840090571707, + "heading": 0.05473581761907218, + "angularVelocity": -5.342779742919752e-7, + "velocityX": 3.458732307298825, + "velocityY": -1.5298215349758078, + "timestamp": 1.3953663961398028 + }, + { + "x": 5.421743975168144, + "y": 4.291821241109097, + "heading": 0.0547357991020742, + "angularVelocity": -5.342777443988636e-7, + "velocityX": 3.4587540669967396, + "velocityY": -1.5297723381170152, + "timestamp": 1.4300243959882462 + }, + { + "x": 5.5428966637680075, + "y": 4.241794448876403, + "heading": 0.05473578057951323, + "angularVelocity": -5.34438255269497e-7, + "velocityX": 3.4956630252656034, + "velocityY": -1.4434414118372951, + "timestamp": 1.4646823958366895 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.07776376214569573, - "angularVelocity": -5.189816214982998e-7, - "velocityX": 3.6118744025389584, - "velocityY": -1.1214036841768227, - "timestamp": 1.4995223835590066 - }, - { - "x": 5.801257373802826, - "y": 4.160893829303833, - "heading": 0.07776374465549807, - "angularVelocity": -4.7264774236323325e-7, - "velocityX": 3.6564428162530316, - "velocityY": -0.9662347026048084, - "timestamp": 1.536527104207333 - }, - { - "x": 5.937965681779902, - "y": 4.1309456443700405, - "heading": 0.07776372758116286, - "angularVelocity": -4.614096500770092e-7, - "velocityX": 3.6943477908205034, - "velocityY": -0.8093071480907553, - "timestamp": 1.5735318248556593 - }, - { - "x": 6.075827506190729, - "y": 4.1068588699437765, - "heading": 0.07776371078252527, - "angularVelocity": -4.53959313742904e-7, - "velocityX": 3.7255199335509808, - "velocityY": -0.6509108568923265, - "timestamp": 1.6105365455039857 - }, - { - "x": 6.2145915217996235, - "y": 4.088676368882122, - "heading": 0.0777636941280861, - "angularVelocity": -4.5006255582880887e-7, - "velocityX": 3.7499003688646537, - "velocityY": -0.4913562578799624, - "timestamp": 1.6475412661523121 - }, - { - "x": 6.354004262869319, - "y": 4.076423083568819, - "heading": 0.07776367749075697, - "angularVelocity": -4.496001822495745e-7, - "velocityX": 3.7674312527474547, - "velocityY": -0.33112762638453114, - "timestamp": 1.6845459868006385 - }, - { - "x": 6.49381285233259, - "y": 4.070129387117659, - "heading": 0.07776362451928018, - "angularVelocity": -0.0000014314788993599032, - "velocityX": 3.778128493170848, - "velocityY": -0.17007820464237144, - "timestamp": 1.7215507074489649 - }, - { - "x": 6.629142082966274, - "y": 4.065515785319137, - "heading": 0.06238171659626575, - "angularVelocity": -0.4156742073314349, - "velocityX": 3.657080184979125, - "velocityY": -0.12467603369760688, - "timestamp": 1.7585554280972913 - }, - { - "x": 6.758586834582697, - "y": 4.061933354582955, - "heading": 0.046880370354106624, - "angularVelocity": -0.4189018582108937, - "velocityX": 3.4980605000804377, - "velocityY": -0.0968101008038334, - "timestamp": 1.7955601487456176 - }, - { - "x": 6.882125546140559, - "y": 4.059204781585899, - "heading": 0.032519289448839375, - "angularVelocity": -0.3880878075461594, - "velocityX": 3.338458158674076, - "velocityY": -0.07373580854685112, - "timestamp": 1.832564869393944 - }, - { - "x": 6.999755813322696, - "y": 4.05726367776388, - "heading": 0.01976964076410941, - "angularVelocity": -0.34454114127480795, - "velocityX": 3.1787908440123913, - "velocityY": -0.0524555729109991, - "timestamp": 1.8695695900422704 - }, - { - "x": 7.111477612575904, - "y": 4.056075301962013, - "heading": 0.008878948528675913, - "angularVelocity": -0.2943054843984055, - "velocityX": 3.0191228928587734, - "velocityY": -0.03211416762637815, - "timestamp": 1.9065743106905968 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -2.4974876469169754e-20, - "angularVelocity": -0.23994096896600625, - "velocityX": 2.8594660546893573, - "velocityY": -0.012350203465742983, - "timestamp": 1.9435790313389232 - }, - { - "x": 7.386543487491071, - "y": 4.0570668323942645, - "heading": -0.009095721053457144, - "angularVelocity": -0.13843657850499821, - "velocityX": 2.57601194788322, - "velocityY": 0.022046826970953105, - "timestamp": 2.0092821941278585 - }, - { - "x": 7.537031363324535, - "y": 4.05969750834718, - "heading": -0.013766029362825559, - "angularVelocity": -0.07108194052044785, - "velocityX": 2.2904205740732744, - "velocityY": 0.04003880241453354, - "timestamp": 2.0749853569167938 - }, - { - "x": 7.6687074557367385, - "y": 4.062834301345331, - "heading": -0.015395893103822788, - "angularVelocity": -0.02480647311048019, - "velocityX": 2.0041058424417098, - "velocityY": 0.04774188737652573, - "timestamp": 2.140688519705729 - }, - { - "x": 7.781556988474885, - "y": 4.066015749956383, - "heading": -0.01492426229398527, - "angularVelocity": 0.007178205581253095, - "velocityX": 1.7175662167232906, - "velocityY": 0.04842154435201139, - "timestamp": 2.2063916824946643 - }, - { - "x": 7.8755781593330285, - "y": 4.068907471663171, - "heading": -0.013029646010250598, - "angularVelocity": 0.028835998197239424, - "velocityX": 1.4309991614890225, - "velocityY": 0.044011910295353764, - "timestamp": 2.2720948452835996 - }, - { - "x": 7.950774573438036, - "y": 4.07125628842811, - "heading": -0.010225311537090487, - "angularVelocity": 0.042681879442679815, - "velocityX": 1.144486976168374, - "velocityY": 0.035748914743771835, - "timestamp": 2.337798008072535 - }, - { - "x": 8.007152043467745, - "y": 4.072863968231118, - "heading": -0.006913103442788773, - "angularVelocity": 0.0504116994328239, - "velocityX": 0.8580632596152882, - "velocityY": 0.024468834296048715, - "timestamp": 2.40350117086147 - }, - { - "x": 8.044717148266278, - "y": 4.073571148367015, - "heading": -0.0034161675760999402, - "angularVelocity": 0.053223250118451255, - "velocityX": 0.5717396728557544, - "velocityY": 0.010763258660280774, - "timestamp": 2.4692043336504055 + "heading": 0.054735761954551196, + "angularVelocity": -5.373928706734811e-7, + "velocityX": 3.550552991831721, + "velocityY": -1.3025960678451716, + "timestamp": 1.4993403956851328 + }, + { + "x": 5.798180192000891, + "y": 4.154397155999577, + "heading": 0.05473574376724012, + "angularVelocity": -4.955056404583076e-7, + "velocityX": 3.6025088632415367, + "velocityY": -1.15113574961205, + "timestamp": 1.53604494470909 + }, + { + "x": 5.93207840031246, + "y": 4.117779387908136, + "heading": 0.054735725716885786, + "angularVelocity": -4.917743118523635e-7, + "velocityX": 3.6480003670436316, + "velocityY": -0.9976356899941846, + "timestamp": 1.5727494937330473 + }, + { + "x": 6.067406110251661, + "y": 4.086860105171276, + "heading": 0.0547357076680171, + "angularVelocity": -4.917338358563753e-7, + "velocityX": 3.686946537631365, + "velocityY": -0.8423828533264233, + "timestamp": 1.6094540427570045 + }, + { + "x": 6.203921243902459, + "y": 4.061696453595828, + "heading": 0.05473568948478555, + "angularVelocity": -4.953944957116945e-7, + "velocityX": 3.719297397216157, + "velocityY": -0.6855731031873736, + "timestamp": 1.6461585917809618 + }, + { + "x": 6.341379198553944, + "y": 4.042334078664949, + "heading": 0.054735669406532635, + "angularVelocity": -5.47023555566929e-7, + "velocityX": 3.7449841588235726, + "velocityY": -0.5275197610585458, + "timestamp": 1.682863140804919 + }, + { + "x": 6.479285910845981, + "y": 4.028828408214569, + "heading": 0.05386070036293022, + "angularVelocity": -0.023838163575618188, + "velocityX": 3.7572103719902135, + "velocityY": -0.3679563108530375, + "timestamp": 1.7195676898288763 + }, + { + "x": 6.612620100679574, + "y": 4.0185345658565055, + "heading": 0.045620785713098924, + "angularVelocity": -0.22449300887617385, + "velocityX": 3.6326339208408243, + "velocityY": -0.2804514053923919, + "timestamp": 1.7562722388528336 + }, + { + "x": 6.740581673709006, + "y": 4.010579483858113, + "heading": 0.035519685699448925, + "angularVelocity": -0.27520022128747124, + "velocityX": 3.4862592357669087, + "velocityY": -0.2167328630900888, + "timestamp": 1.7929767878767908 + }, + { + "x": 6.863028396494948, + "y": 4.00465723612987, + "heading": 0.025264871332947036, + "angularVelocity": -0.27938810417772464, + "velocityX": 3.3360094604627837, + "velocityY": -0.16134914842236953, + "timestamp": 1.829681336900748 + }, + { + "x": 6.979909878212025, + "y": 4.000619227079583, + "heading": 0.015648348920582984, + "angularVelocity": -0.26199810835673765, + "velocityX": 3.184386808315938, + "velocityY": -0.1100138581637957, + "timestamp": 1.8663858859247053 + }, + { + "x": 7.091201711972647, + "y": 3.998378156691166, + "heading": 0.00712732553533738, + "angularVelocity": -0.23215169813648398, + "velocityX": 3.0320992007824574, + "velocityY": -0.06105702012450316, + "timestamp": 1.9030904349486626 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -3.597785008712915e-31, + "angularVelocity": -0.19418098641357107, + "velocityX": 2.87942961178202, + "velocityY": -0.013663498666533047, + "timestamp": 1.9397949839726198 + }, + { + "x": 7.37119660747943, + "y": 4.00262802828422, + "heading": -0.0076676970964569885, + "angularVelocity": -0.11437965193876058, + "velocityX": 2.6001474595947878, + "velocityY": 0.07087677805125822, + "timestamp": 2.0068322320013365 + }, + { + "x": 7.526209738417895, + "y": 4.010745052233057, + "heading": -0.011651875074249012, + "angularVelocity": -0.059432302114867865, + "velocityX": 2.3123432941647923, + "velocityY": 0.1210822965966538, + "timestamp": 2.0738694800300532 + }, + { + "x": 7.661711723306385, + "y": 4.020683594487604, + "heading": -0.013062756224948863, + "angularVelocity": -0.021046227167551262, + "velocityX": 2.0212939652660666, + "velocityY": 0.1482540311065431, + "timestamp": 2.14090672805877 + }, + { + "x": 7.777626027630173, + "y": 4.031355931329743, + "heading": -0.012680116865926582, + "angularVelocity": 0.005707861976350362, + "velocityX": 1.729102964879078, + "velocityY": 0.15920010376271715, + "timestamp": 2.2079439760874866 + }, + { + "x": 7.873936260933145, + "y": 4.041961506483741, + "heading": -0.011077902998110952, + "angularVelocity": 0.023900352638719452, + "velocityX": 1.4366674667449708, + "velocityY": 0.15820421431164344, + "timestamp": 2.2749812241162033 + }, + { + "x": 7.950652622834928, + "y": 4.051889363613367, + "heading": -0.008694806000859931, + "angularVelocity": 0.035548848846393165, + "velocityX": 1.1443841171541105, + "velocityY": 0.14809464024199548, + "timestamp": 2.34201847214492 + }, + { + "x": 8.007796949441587, + "y": 4.060659248403223, + "heading": -0.005876390393834335, + "angularVelocity": 0.04204253142698025, + "velocityX": 0.8524264985069137, + "velocityY": 0.1308210740706325, + "timestamp": 2.4090557201736367 + }, + { + "x": 8.045395699194648, + "y": 4.067884383860418, + "heading": -0.00290156854092843, + "angularVelocity": 0.04437565592835165, + "velocityX": 0.5608635625518943, + "velocityY": 0.1077779245069823, + "timestamp": 2.4760929682023534 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 5.107015447929928e-20, - "angularVelocity": 0.05199395936347874, - "velocityX": 0.28551767430107416, - "velocityY": -0.004934199232898976, - "timestamp": 2.5349074964393408 - }, - { - "x": 8.06095184830656, - "y": 4.071458641245482, - "heading": 0.0034408492569327057, - "angularVelocity": 0.04684199855212652, - "velocityX": -0.034370194612683154, - "velocityY": -0.02434521970346025, - "timestamp": 2.608363996232015 - }, - { - "x": 8.034920990250148, - "y": 4.068388525754522, - "heading": 0.006505362431754428, - "angularVelocity": 0.041718747605332625, - "velocityX": -0.3543710649143617, - "velocityY": -0.041795014731511364, - "timestamp": 2.681820496024689 - }, - { - "x": 7.985375296011398, - "y": 4.064207377475085, - "heading": 0.009196043333328236, - "angularVelocity": 0.036629582258453454, - "velocityX": -0.6744902680986578, - "velocityY": -0.05692005869104256, - "timestamp": 2.755276995817363 - }, - { - "x": 7.912305948559603, - "y": 4.059120814150924, - "heading": 0.011515917649017375, - "angularVelocity": 0.03158160710402538, - "velocityX": -0.9947295019232808, - "velocityY": -0.06924592566372595, - "timestamp": 2.828733495610037 - }, - { - "x": 7.815704588785296, - "y": 4.0533811177027905, - "heading": 0.013468719297185195, - "angularVelocity": 0.026584463644190544, - "velocityX": -1.3150825324778364, - "velocityY": -0.0781373529140697, - "timestamp": 2.9021899954027113 - }, - { - "x": 7.695564554163692, - "y": 4.047305026722375, - "heading": 0.015059175300172275, - "angularVelocity": 0.021651671499132708, - "velocityX": -1.6355262633081036, - "velocityY": -0.08271685960487347, - "timestamp": 2.9756464951953854 - }, - { - "x": 7.551883548681463, - "y": 4.04130179428642, - "heading": 0.016293463732843762, - "angularVelocity": 0.016802984571211377, - "velocityX": -1.9560012509139277, - "velocityY": -0.08172499986929728, - "timestamp": 3.0491029949880595 - }, - { - "x": 7.384669657766205, - "y": 4.035920047959674, - "heading": 0.017179998132389497, - "angularVelocity": 0.012068835324959903, - "velocityX": -2.276366167557798, - "velocityY": -0.07326439922859854, - "timestamp": 3.1225594947807327 - }, - { - "x": 7.19395595849013, - "y": 4.0319320028167684, - "heading": 0.017730886759136543, - "angularVelocity": 0.007499521870792628, - "velocityX": -2.5962807895060482, - "velocityY": -0.054291249299417726, - "timestamp": 3.196015994573406 - }, - { - "x": 6.979840325568109, - "y": 4.030500043249597, - "heading": 0.017964968086133067, - "angularVelocity": 0.00318666595409774, - "velocityX": -2.9148629941032773, - "velocityY": -0.01949398039945934, - "timestamp": 3.269472494366079 - }, - { - "x": 6.742614358141766, - "y": 4.033551118259164, - "heading": 0.017915208666011314, - "angularVelocity": -0.0006773998252331979, - "velocityX": -3.22947551402392, - "velocityY": 0.041535807153613755, - "timestamp": 3.3429289941587523 - }, - { - "x": 6.483307205091183, - "y": 4.044763583343556, - "heading": 0.017651500017929662, - "angularVelocity": -0.003589997465520818, - "velocityX": -3.530077716505134, - "velocityY": 0.1526408842789682, - "timestamp": 3.4163854939514255 - }, - { - "x": 6.206941447635851, - "y": 4.072199279636492, - "heading": 0.017375156748179146, - "angularVelocity": -0.0037619988773032, - "velocityX": -3.762305013652376, - "velocityY": 0.37349582910118817, - "timestamp": 3.4898419937440988 - }, - { - "x": 5.9338077856619895, - "y": 4.122953167788701, - "heading": 0.017375155210986027, - "angularVelocity": -2.0926577278102936e-8, - "velocityX": -3.7183048844521998, - "velocityY": 0.690938014953861, - "timestamp": 3.563298493536772 + "heading": 0, + "angularVelocity": 0.043282930404384794, + "velocityX": 0.26971368660010536, + "velocityY": 0.07999391635030323, + "timestamp": 2.54313021623107 + }, + { + "x": 8.059946431342839, + "y": 4.076566093038108, + "heading": 0.0028549561404960684, + "angularVelocity": 0.03895223157485018, + "velocityX": -0.048164132671898556, + "velocityY": 0.045285389048645555, + "timestamp": 2.616423992352985 + }, + { + "x": 8.033099887642729, + "y": 4.077511417867968, + "heading": 0.00539411397159862, + "angularVelocity": 0.03464356682727019, + "velocityX": -0.3662868134321987, + "velocityY": 0.012897750394093884, + "timestamp": 2.6897177684748996 + }, + { + "x": 7.9829172655770035, + "y": 4.07628474252501, + "heading": 0.007619394466085314, + "angularVelocity": 0.03036111130070867, + "velocityX": -0.6846778092351761, + "velocityY": -0.01673641894119354, + "timestamp": 2.7630115445968144 + }, + { + "x": 7.909377202284024, + "y": 4.0731292983210805, + "heading": 0.00953312535115696, + "angularVelocity": 0.026110414639960534, + "velocityX": -1.0033602740109082, + "velocityY": -0.04305200756310189, + "timestamp": 2.836305320718729 + }, + { + "x": 7.81245694795258, + "y": 4.068343833996595, + "heading": 0.011138189892816102, + "angularVelocity": 0.021899056462711293, + "velocityX": -1.3223531309156478, + "velocityY": -0.06529155103873027, + "timestamp": 2.909599096840644 + }, + { + "x": 7.692133347423482, + "y": 4.062303858835979, + "heading": 0.012438255979270885, + "angularVelocity": 0.01773774193721827, + "velocityX": -1.641661910404963, + "velocityY": -0.0824077497462929, + "timestamp": 2.982892872962559 + }, + { + "x": 7.548385409224001, + "y": 4.055495143379703, + "heading": 0.013438147372137587, + "angularVelocity": 0.013642241480415802, + "velocityX": -1.9612570917396126, + "velocityY": -0.0928962296191558, + "timestamp": 3.0561866490844736 + }, + { + "x": 7.381200809360299, + "y": 4.048569614878887, + "heading": 0.014144485416621113, + "angularVelocity": 0.009637080825370731, + "velocityX": -2.2810204182359235, + "velocityY": -0.09448999447506434, + "timestamp": 3.1294804252063884 + }, + { + "x": 7.190592956257051, + "y": 4.04244548860493, + "heading": 0.014566894857523213, + "angularVelocity": 0.00576323752509999, + "velocityX": -2.6006008038962976, + "velocityY": -0.08355588425094046, + "timestamp": 3.202774201328303 + }, + { + "x": 6.9766499613051005, + "y": 4.038503705706744, + "heading": 0.01472053802802316, + "angularVelocity": 0.002096264903098756, + "velocityX": -2.918979022121646, + "velocityY": -0.05378059511668448, + "timestamp": 3.276067977450218 + }, + { + "x": 6.739697889009689, + "y": 4.039020009641981, + "heading": 0.014632337527092197, + "angularVelocity": -0.0012033832283965633, + "velocityX": -3.232908506464046, + "velocityY": 0.007044308023879432, + "timestamp": 3.3493617535721327 + }, + { + "x": 6.480981935824527, + "y": 4.048232688232657, + "heading": 0.01435998143770215, + "angularVelocity": -0.003715951118919239, + "velocityX": -3.529848874955213, + "velocityY": 0.12569523741486752, + "timestamp": 3.4226555296940475 + }, + { + "x": 6.206211729235204, + "y": 4.074520375352005, + "heading": 0.014063196474504363, + "angularVelocity": -0.004049251913342814, + "velocityX": -3.7488886659663403, + "velocityY": 0.3586619288876869, + "timestamp": 3.4959493058159623 + }, + { + "x": 5.93349789020665, + "y": 4.124154607710869, + "heading": 0.014063194566661911, + "angularVelocity": -2.6030074482550523e-8, + "velocityX": -3.7208321559927344, + "velocityY": 0.6771957318218086, + "timestamp": 3.569243081937877 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.017375154199310676, - "angularVelocity": -1.377244159549356e-8, - "velocityX": -3.646458211284153, - "velocityY": 1.003259166632557, - "timestamp": 3.636754993329445 - }, - { - "x": 5.541092339335072, - "y": 4.2364336029082, - "heading": 0.01737515315796098, - "angularVelocity": -3.005342133477009e-8, - "velocityX": -3.6034502673557895, - "velocityY": 1.148184128750311, - "timestamp": 3.6714049482953417 - }, - { - "x": 5.417922638814364, - "y": 4.281176215218788, - "heading": 0.017375152214225534, - "angularVelocity": -2.7236267527590686e-8, - "velocityX": -3.5546857316822638, - "velocityY": 1.2912747608077488, - "timestamp": 3.706054903261238 - }, - { - "x": 5.296639420951804, - "y": 4.33080542124118, - "heading": 0.017375151348717865, - "angularVelocity": -2.4978608726563434e-8, - "velocityX": -3.500241716964063, - "velocityY": 1.4323021796489428, - "timestamp": 3.7407048582271347 - }, - { - "x": 5.177436464453021, - "y": 4.385241922289634, - "heading": 0.017375150546348885, - "angularVelocity": -2.3156422113484916e-8, - "velocityX": -3.4402052359405113, - "velocityY": 1.571041033156597, - "timestamp": 3.7753548131930312 - }, - { - "x": 5.0605042229676425, - "y": 4.444398737399711, - "heading": 0.017375149795171643, - "angularVelocity": -2.167902498222188e-8, - "velocityX": -3.374672249948487, - "velocityY": 1.7072696102577922, - "timestamp": 3.8100047681589277 - }, - { - "x": 4.946029519855076, - "y": 4.5081813412028975, - "heading": 0.017375149085535665, - "angularVelocity": -2.0480141439242773e-8, - "velocityX": -3.3037475294046756, - "velocityY": 1.8407701789500717, - "timestamp": 3.8446547231248243 - }, - { - "x": 4.834195247479412, - "y": 4.576487812332227, - "heading": 0.017375148409491465, - "angularVelocity": -1.9510680466596443e-8, - "velocityX": -3.2275445230947857, - "velocityY": 1.9713292902272803, - "timestamp": 3.8793046780907208 - }, - { - "x": 4.725180068322848, - "y": 4.649208988191292, - "heading": 0.01737514776036044, - "angularVelocity": -1.8733964504925207e-8, - "velocityX": -3.146185305691103, - "velocityY": 2.0987379617271817, - "timestamp": 3.9139546330566173 - }, - { - "x": 4.619158095512492, - "y": 4.726228597192443, - "heading": 0.017375147132049722, - "angularVelocity": -1.813308899206411e-8, - "velocityX": -3.059801171883295, - "velocityY": 2.222791027490615, - "timestamp": 3.948604588022514 - }, - { - "x": 4.51014571232548, - "y": 4.798953964824882, - "heading": 0.01737514649311973, - "angularVelocity": -1.8439562026762692e-8, - "velocityX": -3.146104613824239, - "velocityY": 2.0988589365849712, - "timestamp": 3.9832545429884103 - }, - { - "x": 4.398314099643168, - "y": 4.867264790943429, - "heading": 0.017375145827920196, - "angularVelocity": -1.9197702730409006e-8, - "velocityX": -3.2274677641681775, - "velocityY": 1.9714549755050896, - "timestamp": 4.017904497954307 - }, - { - "x": 4.2838418935996625, - "y": 4.931051865994745, - "heading": 0.017375140544826046, - "angularVelocity": -1.5247044777404565e-7, - "velocityX": -3.3036754638258423, - "velocityY": 1.840899219467885, - "timestamp": 4.052554452920203 + "heading": 0.01406319364211473, + "angularVelocity": -1.2614265905768763e-8, + "velocityX": -3.6503257922039465, + "velocityY": 0.9890944453863133, + "timestamp": 3.642536858059792 + }, + { + "x": 5.5408846553597755, + "y": 4.23596798413579, + "heading": 0.01406319265930284, + "angularVelocity": -2.835157538408451e-8, + "velocityX": -3.6078608681964144, + "velocityY": 1.1342486182256648, + "timestamp": 3.677202017840065 + }, + { + "x": 5.417489605980159, + "y": 4.2802558166388724, + "heading": 0.014063191785023112, + "angularVelocity": -2.5220703836459854e-8, + "velocityX": -3.5596273076991327, + "velocityY": 1.2775891639848806, + "timestamp": 3.711867177620338 + }, + { + "x": 5.295963908009713, + "y": 4.329441746794644, + "heading": 0.014063190996401449, + "angularVelocity": -2.2749690676604448e-8, + "velocityX": -3.5057013653114657, + "velocityY": 1.4188865843266973, + "timestamp": 3.7465323374006108 + }, + { + "x": 5.176501898681268, + "y": 4.383447115848144, + "heading": 0.014063190276173353, + "angularVelocity": -2.077671352365433e-8, + "velocityX": -3.446169297521196, + "velocityY": 1.5579149034885322, + "timestamp": 3.7811974971808837 + }, + { + "x": 5.05929461402676, + "y": 4.442185556946357, + "heading": 0.01406318961105745, + "angularVelocity": -1.9186869677954374e-8, + "velocityX": -3.381126335416613, + "velocityY": 1.6944517628226294, + "timestamp": 3.8158626569611567 + }, + { + "x": 4.944529482696556, + "y": 4.505563132440428, + "heading": 0.014063188990615918, + "angularVelocity": -1.7898129854823097e-8, + "velocityX": -3.310676542604923, + "velocityY": 1.828278764494154, + "timestamp": 3.8505278167414296 + }, + { + "x": 4.832390024623587, + "y": 4.573478482189006, + "heading": 0.014063188406478968, + "angularVelocity": -1.685083682180617e-8, + "velocityX": -3.2349326754519327, + "velocityY": 1.9591817888352978, + "timestamp": 3.8851929765217026 + }, + { + "x": 4.723055552753515, + "y": 4.64582297983034, + "heading": 0.014063187851803343, + "angularVelocity": -1.600095394099115e-8, + "velocityX": -3.1540160946348355, + "velocityY": 2.086951224223198, + "timestamp": 3.9198581363019755 + }, + { + "x": 4.616700862639107, + "y": 4.722480877082754, + "heading": 0.014063187320883878, + "angularVelocity": -1.5315650301874817e-8, + "velocityX": -3.068057115228709, + "velocityY": 2.2113816217294997, + "timestamp": 3.9545232960822485 + }, + { + "x": 4.508256477944758, + "y": 4.796152911545187, + "heading": 0.014063186782201262, + "angularVelocity": -1.5539597107094908e-8, + "velocityX": -3.128339386915466, + "velocityY": 2.125247220246655, + "timestamp": 3.9891884558625215 + }, + { + "x": 4.396953351691581, + "y": 4.865430375769311, + "heading": 0.014063186216514915, + "angularVelocity": -1.6318584724554417e-8, + "velocityX": -3.2108066704055713, + "velocityY": 1.998475260556751, + "timestamp": 4.023853615642794 + }, + { + "x": 4.28296944450211, + "y": 4.930202424249945, + "heading": 0.014063179833248412, + "angularVelocity": -1.8414069176204727e-7, + "velocityX": -3.288140251248267, + "velocityY": 1.8685056953781265, + "timestamp": 4.058518775423067 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.01686674274932993, - "angularVelocity": -0.014672394119890892, - "velocityX": -3.3576617434279026, - "velocityY": 1.6997404450060698, - "timestamp": 4.0872044078861 - }, - { - "x": 3.941206209015057, - "y": 5.0867984564258695, - "heading": 0.014981559594595455, - "angularVelocity": -0.027398080889315734, - "velocityX": -3.288799800865747, - "velocityY": 1.4075673366199248, - "timestamp": 4.156011532091272 - }, - { - "x": 3.7303087168982803, - "y": 5.169878519937419, - "heading": 0.012882808496808885, - "angularVelocity": -0.03050194470456946, - "velocityX": -3.0650531402520937, - "velocityY": 1.2074340334849432, - "timestamp": 4.224818656296444 - }, - { - "x": 3.5370906486542313, - "y": 5.2422774011944036, - "heading": 0.010786769401609648, - "angularVelocity": -0.030462530143669606, - "velocityX": -2.808111376198552, - "velocityY": 1.0522003657804835, - "timestamp": 4.293625780501616 - }, - { - "x": 3.362377869511596, - "y": 5.305500032913324, - "heading": 0.008787829989227822, - "angularVelocity": -0.029051343672224058, - "velocityX": -2.5391669999413216, - "velocityY": 0.9188384553087892, - "timestamp": 4.362432904706788 - }, - { - "x": 3.2065764087500264, - "y": 5.360415401059238, - "heading": 0.0069378788339289135, - "angularVelocity": -0.026886040895746154, - "velocityX": -2.2643216463602664, - "velocityY": 0.7981058470364881, - "timestamp": 4.43124002891196 - }, - { - "x": 3.0699232813800306, - "y": 5.407585492361195, - "heading": 0.005269555505228705, - "angularVelocity": -0.024246374891727204, - "velocityX": -1.9860316638509532, - "velocityY": 0.6855408047763067, - "timestamp": 4.500047153117132 - }, - { - "x": 2.9525724844135883, - "y": 5.4474025208162065, - "heading": 0.00380523783536115, - "angularVelocity": -0.021281483375198414, - "velocityX": -1.70550358443293, - "velocityY": 0.5786759571041632, - "timestamp": 4.568854277322304 - }, - { - "x": 2.854631606493464, - "y": 5.480155404840544, - "heading": 0.0025612082007498414, - "angularVelocity": -0.018079953914391628, - "velocityX": -1.423411878515389, - "velocityY": 0.47601007021705255, - "timestamp": 4.637661401527476 - }, - { - "x": 2.776179840063629, - "y": 5.506065679558788, - "heading": 0.0015498402303536507, - "angularVelocity": -0.014698593816833106, - "velocityX": -1.1401692388117297, - "velocityY": 0.376563837212327, - "timestamp": 4.706468525732648 - }, - { - "x": 2.717277807675311, - "y": 5.525308541082813, - "heading": 0.000780854270914727, - "angularVelocity": -0.01117596423803359, - "velocityX": -0.8560455486074661, - "velocityY": 0.2796637956651817, - "timestamp": 4.77527564993782 - }, - { - "x": 2.677973353536199, - "y": 5.538025975345556, - "heading": 0.00026208876345230333, - "angularVelocity": -0.007539415626724292, - "velocityX": -0.5712265204096013, - "velocityY": 0.1848272894652728, - "timestamp": 4.844082774142992 + "heading": 0.013676554706226966, + "angularVelocity": -0.011153132697846535, + "velocityX": -3.331021112692202, + "velocityY": 1.7234990981340637, + "timestamp": 4.09318393520334 + }, + { + "x": 3.942209538282738, + "y": 5.088292732030669, + "heading": 0.012204358572730108, + "angularVelocity": -0.02138935040268071, + "velocityX": -3.2732028891433576, + "velocityY": 1.4288410708686299, + "timestamp": 4.162012399171669 + }, + { + "x": 3.7316526106102703, + "y": 5.172141912516415, + "heading": 0.010528895655226616, + "angularVelocity": -0.024342587657839698, + "velocityX": -3.05915482538382, + "velocityY": 1.2182340800795661, + "timestamp": 4.230840863139997 + }, + { + "x": 3.53847572791013, + "y": 5.244805689896845, + "heading": 0.00883705460181792, + "angularVelocity": -0.024580543511579803, + "velocityX": -2.806642362221417, + "velocityY": 1.0557227808231526, + "timestamp": 4.299669327108326 + }, + { + "x": 3.3636567049982276, + "y": 5.307973779066906, + "heading": 0.0072129294254924, + "angularVelocity": -0.0235967081449452, + "velocityX": -2.5399233519484916, + "velocityY": 0.9177611343924164, + "timestamp": 4.3684977910766545 + }, + { + "x": 3.2076759026568156, + "y": 5.362638919945403, + "heading": 0.005703178674060931, + "angularVelocity": -0.02193497667078823, + "velocityX": -2.2662252409582635, + "velocityY": 0.7942228799941261, + "timestamp": 4.437326255044983 + }, + { + "x": 3.070812323282922, + "y": 5.409448485662979, + "heading": 0.00433725990535669, + "angularVelocity": -0.01984526008502491, + "velocityX": -1.9884735396226798, + "velocityY": 0.6800902274837387, + "timestamp": 4.5061547190133115 + }, + { + "x": 2.953246241272587, + "y": 5.448856394346129, + "heading": 0.0031354038045633293, + "angularVelocity": -0.01746161444698809, + "velocityX": -1.7081026545127194, + "velocityY": 0.572552493707886, + "timestamp": 4.57498318298164 + }, + { + "x": 2.855102952665587, + "y": 5.48119799246298, + "heading": 0.002112343078266857, + "angularVelocity": -0.014863919188538589, + "velocityX": -1.4259113591749097, + "velocityY": 0.4698869661210702, + "timestamp": 4.643811646949969 + }, + { + "x": 2.7764742584987756, + "y": 5.506730918230691, + "heading": 0.0012792785012801142, + "angularVelocity": -0.012103489297248894, + "velocityX": -1.1423862982470774, + "velocityY": 0.3709646314271869, + "timestamp": 4.712640110918297 + }, + { + "x": 2.717430148167725, + "y": 5.5256592019268735, + "heading": 0.0006450106297669135, + "angularVelocity": -0.009215197244632127, + "velocityX": -0.8578443702916283, + "velocityY": 0.275006626689982, + "timestamp": 4.781468574886626 + }, + { + "x": 2.6780256614742735, + "y": 5.538148367640626, + "heading": 0.00021663624249770164, + "angularVelocity": -0.006223796995765153, + "velocityX": -0.5725027760547413, + "velocityY": 0.18145350039337144, + "timestamp": 4.850297038854954 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -9.768710288964573e-20, - "angularVelocity": -0.003809035277666901, - "velocityX": -0.2858451884385741, - "velocityY": 0.09169675411867828, - "timestamp": 4.912889898348164 + "heading": -3.0114656219279604e-34, + "angularVelocity": -0.003147480417366101, + "velocityX": -0.28651653960332735, + "velocityY": 0.08989010211866776, + "timestamp": 4.919125502823283 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -4.776521402733554e-20, - "angularVelocity": 3.1435024538818593e-20, - "velocityX": 5.229513890071112e-19, - "velocityY": 2.1429873638516558e-18, - "timestamp": 4.981697022553336 + "heading": -4.388516089708894e-34, + "angularVelocity": -2.000698896485297e-33, + "velocityX": 9.851967688482278e-36, + "velocityY": -1.337407020908214e-34, + "timestamp": 4.987953966791611 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj index 9d8dfdaa..60152d9f 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj @@ -3,380 +3,380 @@ { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -4.776521402733554e-20, - "angularVelocity": 3.1435024538818593e-20, - "velocityX": 5.229513890071112e-19, - "velocityY": 2.1429873638516558e-18, + "heading": -4.388516089708894e-34, + "angularVelocity": -2.000698896485297e-33, + "velocityX": 9.851967688482278e-36, + "velocityY": -1.337407020908214e-34, "timestamp": 0 }, { - "x": 2.6751665057839835, - "y": 5.538944468629318, - "heading": -4.314568505907974e-18, - "angularVelocity": -6.69844600274702e-17, - "velocityX": 0.2647058041795454, - "velocityY": -0.08463157955674895, - "timestamp": 0.06369840542178373 - }, - { - "x": 2.7088624712986666, - "y": 5.528079837461981, - "heading": -9.548636187746045e-18, - "angularVelocity": -8.216952445023882e-17, - "velocityX": 0.5289922925316966, - "velocityY": -0.1705636286402427, - "timestamp": 0.12739681084356747 - }, - { - "x": 2.7593613029745305, - "y": 5.511644735540107, - "heading": -1.3305788264629273e-17, - "angularVelocity": -5.898345573823082e-17, - "velocityX": 0.7927801542516099, - "velocityY": -0.25801433824046893, - "timestamp": 0.1910952162653512 - }, - { - "x": 2.8266246280660523, - "y": 5.489524723354373, - "heading": -1.7929345665528888e-17, - "angularVelocity": -7.258513569049044e-17, - "velocityX": 1.0559656030026554, - "velocityY": -0.34726163142175154, - "timestamp": 0.25479362168713493 - }, - { - "x": 2.9106051995536752, - "y": 5.461582331644453, - "heading": -2.2921460737278194e-17, - "angularVelocity": -7.837111523596277e-17, - "velocityX": 1.31840932173323, - "velocityY": -0.43866705178720505, - "timestamp": 0.31849202710891866 - }, - { - "x": 3.0112434875257597, - "y": 5.427649372766817, - "heading": -2.8632725416108676e-17, - "angularVelocity": -8.966103061640699e-17, - "velocityX": 1.5799184815647955, - "velocityY": -0.5327128466237959, - "timestamp": 0.3821904325307024 - }, - { - "x": 3.1284623112048178, - "y": 5.387515427846554, - "heading": -3.496086628632705e-17, - "angularVelocity": -9.93453576777077e-17, - "velocityX": 1.8402159819054262, - "velocityY": -0.6300620031932188, - "timestamp": 0.44588883795248613 - }, - { - "x": 3.2621579049769083, - "y": 5.340909820689256, - "heading": -4.1773207656590006e-17, - "angularVelocity": -1.0694681169901965e-16, - "velocityX": 2.0988844679362666, - "velocityY": -0.7316604999559271, - "timestamp": 0.5095872433742699 - }, - { - "x": 3.4121839694430376, - "y": 5.287471786804592, - "heading": -4.8890897323491556e-17, - "angularVelocity": -1.1174046853574415e-16, - "velocityX": 2.3552562026116513, - "velocityY": -0.8389226312781303, - "timestamp": 0.5732856487960536 - }, - { - "x": 3.5783204640757695, - "y": 5.226697519809261, - "heading": -5.788296277144039e-17, - "angularVelocity": -1.4116625664827146e-16, - "velocityX": 2.608173525422577, - "velocityY": -0.954094008992977, - "timestamp": 0.6369840542178373 - }, - { - "x": 3.76020443223388, - "y": 5.157837260218167, - "heading": -6.653949543666233e-17, - "angularVelocity": -1.3589873416470021e-16, - "velocityX": 2.85539279914075, - "velocityY": -1.0810358459545435, - "timestamp": 0.7006824596396211 - }, - { - "x": 3.9571467965572817, - "y": 5.079670509622146, - "heading": -7.420373602789392e-17, - "angularVelocity": -1.2032076062800333e-16, - "velocityX": 3.0917942610859868, - "velocityY": -1.2271382631705205, - "timestamp": 0.7643808650614048 + "x": 2.6750735555579293, + "y": 5.538716972759925, + "heading": 4.633169711684461e-19, + "angularVelocity": 7.277213797999712e-18, + "velocityX": 0.2633772294028884, + "velocityY": -0.0882468077495622, + "timestamp": 0.06366680765869503 + }, + { + "x": 2.7085920650608806, + "y": 5.52742596598787, + "heading": -4.184429803904338e-20, + "angularVelocity": -7.934452625857426e-18, + "velocityX": 0.5264675697678677, + "velocityY": -0.1773452633683705, + "timestamp": 0.12733361531739007 + }, + { + "x": 2.7588390755328, + "y": 5.51039889888726, + "heading": 3.955178903113731e-20, + "angularVelocity": 1.278469740567629e-18, + "velocityX": 0.7892183120172118, + "velocityY": -0.2674402522565472, + "timestamp": 0.1910004229760851 + }, + { + "x": 2.8257885982701296, + "y": 5.487560532002736, + "heading": 8.970064045939625e-19, + "angularVelocity": 1.3467843717867433e-17, + "velocityX": 1.0515608556382, + "velocityY": -0.3587170100777762, + "timestamp": 0.25466723063478014 + }, + { + "x": 2.9094088162269203, + "y": 5.4588202139112365, + "heading": 2.18904914594727e-18, + "angularVelocity": 2.0293820106069983e-17, + "velocityX": 1.313403656188678, + "velocityY": -0.4514176090871547, + "timestamp": 0.3183340382934752 + }, + { + "x": 3.0096598962036025, + "y": 5.424066617313393, + "heading": 3.904306808877207e-18, + "angularVelocity": 2.694116017446798e-17, + "velocityX": 1.5746208057754196, + "velocityY": -0.5458668005493709, + "timestamp": 0.3820008459521702 + }, + { + "x": 3.126490562172417, + "y": 5.383159777949491, + "heading": 4.8450172295989666e-18, + "angularVelocity": 1.4775523625508697e-17, + "velocityX": 1.8350325745107399, + "velocityY": -0.6425143786570184, + "timestamp": 0.44566765361086524 + }, + { + "x": 3.2598324200617776, + "y": 5.335918462965805, + "heading": 4.235614277481238e-18, + "angularVelocity": -9.5717529200556e-18, + "velocityX": 2.0943700931917317, + "velocityY": -0.7420085397863493, + "timestamp": 0.5093344612695603 + }, + { + "x": 3.4095898607657253, + "y": 5.282098877479993, + "heading": 2.3874123899446895e-18, + "angularVelocity": -2.902928473254694e-17, + "velocityX": 2.3522059014921575, + "velocityY": -0.8453319314253612, + "timestamp": 0.5730012689282553 + }, + { + "x": 3.5756203018815462, + "y": 5.22135582426949, + "heading": 2.5233707069051096e-18, + "angularVelocity": 2.135466217958737e-18, + "velocityX": 2.607802200573299, + "velocityY": -0.9540772569615117, + "timestamp": 0.6366680765869503 + }, + { + "x": 3.757690030648967, + "y": 5.153163870724839, + "heading": 3.693577308512614e-18, + "angularVelocity": 1.8380167698697197e-17, + "velocityX": 2.8597276267323055, + "velocityY": -1.0710754324327665, + "timestamp": 0.7003348842456454 + }, + { + "x": 3.955353841452516, + "y": 5.076631094268888, + "heading": 4.677490606421779e-18, + "angularVelocity": 1.5454101345613767e-17, + "velocityX": 3.1046603100187773, + "velocityY": -1.2020828320186725, + "timestamp": 0.7640016919043404 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -8.561193314262968e-17, - "angularVelocity": -1.7909705963758273e-16, - "velocityX": 3.3023160854502294, - "velocityY": -1.4085550989874924, - "timestamp": 0.8280792704831885 - }, - { - "x": 4.2842075611264425, - "y": 4.936417577245955, - "heading": -9.453694204366925e-17, - "angularVelocity": -2.5755488045645486e-16, - "velocityX": 3.3679341946759767, - "velocityY": -1.5447568972758807, - "timestamp": 0.8627321137340349 - }, - { - "x": 4.401356584826297, - "y": 4.8776669667131465, - "heading": -1.0560165699964911e-16, - "angularVelocity": -3.1930179223080245e-16, - "velocityX": 3.380646801528862, - "velocityY": -1.6954051968412056, - "timestamp": 0.8973849569848813 - }, - { - "x": 4.516063976001339, - "y": 4.814281195629902, - "heading": -9.662380875053523e-17, - "angularVelocity": 2.590797004510181e-16, - "velocityX": 3.3101869980680285, - "velocityY": -1.8291650882556678, - "timestamp": 0.9320378002357277 - }, - { - "x": 4.628146410125643, - "y": 4.746361612927903, - "heading": -9.053238794887956e-17, - "angularVelocity": 1.7578415593930746e-16, - "velocityX": 3.234436877602137, - "velocityY": -1.9600002865663488, - "timestamp": 0.9666906434865741 - }, - { - "x": 4.737424855337869, - "y": 4.674016859733605, - "heading": -7.951670404623386e-17, - "angularVelocity": 3.1788687072504797e-16, - "velocityX": 3.1535203163901047, - "velocityY": -2.087700356089278, - "timestamp": 1.0013434867374205 - }, - { - "x": 4.844483096398324, - "y": 4.598425359059355, - "heading": -7.540763427184281e-17, - "angularVelocity": 1.1857814219565296e-16, - "velocityX": 3.089450417833756, - "velocityY": -2.181393893916722, - "timestamp": 1.0359963299882669 - }, - { - "x": 4.954476829355994, - "y": 4.5271728465414585, - "heading": -1.0264313607639127e-16, - "angularVelocity": -7.859528757078724e-16, - "velocityX": 3.174161847599147, - "velocityY": -2.0561808450207573, - "timestamp": 1.0706491732391132 - }, - { - "x": 5.06723038345343, - "y": 4.460373312794279, - "heading": -9.620613179989556e-17, - "angularVelocity": 1.8575688667134074e-16, - "velocityX": 3.25380383021476, - "velocityY": -1.9276782936288397, - "timestamp": 1.1053020164899596 - }, - { - "x": 5.18256360847748, - "y": 4.398133545147613, - "heading": -8.10849749390595e-17, - "angularVelocity": 4.36361217215796e-16, - "velocityX": 3.3282470990668904, - "velocityY": -1.796094109684511, - "timestamp": 1.139954859740806 - }, - { - "x": 5.300292214676822, - "y": 4.340553023175948, - "heading": -6.741688231750934e-17, - "angularVelocity": 3.9442918212224324e-16, - "velocityX": 3.397372196766743, - "velocityY": -1.6616391779124173, - "timestamp": 1.1746077029916524 - }, - { - "x": 5.4202280776683445, - "y": 4.287723772710641, - "heading": -7.654370000647077e-17, - "angularVelocity": -2.6337861002637873e-16, - "velocityX": 3.461068464810413, - "velocityY": -1.5245285959043562, - "timestamp": 1.2092605462424988 - }, - { - "x": 5.542179542504675, - "y": 4.239730223009142, - "heading": -8.402477364947206e-17, - "angularVelocity": -2.1588628640817297e-16, - "velocityX": 3.5192340193716007, - "velocityY": -1.3849815830141687, - "timestamp": 1.2439133894933452 + "heading": 5.777516683198087e-18, + "angularVelocity": 1.7277858231456074e-17, + "velocityX": 3.332116557247906, + "velocityY": -1.36151476081006, + "timestamp": 0.8276684995630355 + }, + { + "x": 4.285701077560922, + "y": 4.938388492372474, + "heading": 6.5114072312405955e-18, + "angularVelocity": 2.119278869815072e-17, + "velocityX": 3.413356765694629, + "velocityY": -1.4888942599373958, + "timestamp": 0.8622977575854378 + }, + { + "x": 4.403715149774036, + "y": 4.881600772971698, + "heading": 6.4908057437571035e-18, + "angularVelocity": -5.949156482118572e-19, + "velocityX": 3.40792956455403, + "velocityY": -1.6398768741750078, + "timestamp": 0.8969270156078402 + }, + { + "x": 4.519367006147829, + "y": 4.8201449517293415, + "heading": 6.136184222547225e-18, + "angularVelocity": -1.0240517454358069e-17, + "velocityX": 3.3397151131270637, + "velocityY": -1.7746791225673975, + "timestamp": 0.9315562736302425 + }, + { + "x": 4.6324721141561005, + "y": 4.754119165100519, + "heading": 6.684498529619524e-18, + "angularVelocity": 1.5833845088958816e-17, + "velocityX": 3.2661718577713303, + "velocityY": -1.906647453610145, + "timestamp": 0.9661855316526449 + }, + { + "x": 4.742850082889288, + "y": 4.683628924272633, + "heading": 1.0669160659402455e-17, + "angularVelocity": 1.1506634439597858e-16, + "velocityX": 3.187419397256002, + "velocityY": -2.03556890483432, + "timestamp": 1.0008147896750472 + }, + { + "x": 4.850325220314879, + "y": 4.608787325463071, + "heading": 1.658478005292643e-17, + "angularVelocity": 1.708272059914504e-16, + "velocityX": 3.1035934225348787, + "velocityY": -2.161224440937961, + "timestamp": 1.0354440476974496 + }, + { + "x": 4.959315849281407, + "y": 4.536170434444089, + "heading": 2.4155748869411193e-17, + "angularVelocity": 2.1862925308959841e-16, + "velocityX": 3.1473567494868284, + "velocityY": -2.0969808527807587, + "timestamp": 1.070073305719852 + }, + { + "x": 5.0711194796065495, + "y": 4.467963995966004, + "heading": 2.3436163975709297e-17, + "angularVelocity": -2.0779679808224236e-17, + "velocityX": 3.228588676454313, + "velocityY": -1.969618824462286, + "timestamp": 1.1047025637422543 + }, + { + "x": 5.1855579490393096, + "y": 4.4042772106470105, + "heading": 2.204718666214906e-17, + "angularVelocity": -4.0109935727230666e-17, + "velocityX": 3.3046757559381796, + "velocityY": -1.8391033754691897, + "timestamp": 1.1393318217646566 + }, + { + "x": 5.302448702578612, + "y": 4.345211810292891, + "heading": 2.1117777344899902e-17, + "angularVelocity": -2.6838845829382515e-17, + "velocityX": 3.375491137109673, + "velocityY": -1.7056501850518673, + "timestamp": 1.173961079787059 + }, + { + "x": 5.421605230513571, + "y": 4.290862095789443, + "heading": 1.9153982489912048e-17, + "angularVelocity": -5.670912306921066e-17, + "velocityX": 3.440920618566971, + "velocityY": -1.569473838229202, + "timestamp": 1.2085903378094613 + }, + { + "x": 5.542837392691082, + "y": 4.241314822773421, + "heading": 1.5300631909411777e-17, + "angularVelocity": -1.112744193943597e-16, + "velocityX": 3.5008593628856843, + "velocityY": -1.430792221536174, + "timestamp": 1.2432195958318637 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -9.639099903602133e-17, - "angularVelocity": -3.5686033890323503e-16, - "velocityX": 3.5717757824418066, - "velocityY": -1.2432211735943417, - "timestamp": 1.2785662327441916 - }, - { - "x": 5.93683892832179, - "y": 4.127579524293423, - "heading": -7.782590018599437e-17, - "angularVelocity": 2.51158487397316e-16, - "velocityX": 3.664705468644969, - "velocityY": -0.9344094480089544, - "timestamp": 1.3524840964903202 - }, - { - "x": 6.212625572925941, - "y": 4.081838891930783, - "heading": -5.4414649647687885e-17, - "angularVelocity": 3.167197934552236e-16, - "velocityX": 3.7309877562390543, - "velocityY": -0.6188034941017393, - "timestamp": 1.4264019602364488 - }, - { - "x": 6.491306353239059, - "y": 4.059759739930169, - "heading": -8.220332638334233e-17, - "angularVelocity": -3.7593993288383394e-16, - "velocityX": 3.7701411565443874, - "velocityY": -0.2986984590956788, - "timestamp": 1.5003198239825775 - }, - { - "x": 6.752875972710037, - "y": 4.054277278014819, - "heading": -7.798662141044023e-17, - "angularVelocity": 5.704581760375439e-17, - "velocityX": 3.5386523123739373, - "velocityY": -0.07416964773468374, - "timestamp": 1.574237687728706 - }, - { - "x": 6.9910920962877245, - "y": 4.053575874325361, - "heading": -6.860310501240552e-17, - "angularVelocity": 1.269451783709716e-16, - "velocityX": 3.2227138543376115, - "velocityY": -0.00948896050171146, - "timestamp": 1.6481555514748347 - }, - { - "x": 7.205567187241113, - "y": 4.055021140911149, - "heading": -5.885294926916e-17, - "angularVelocity": 1.3190526956719489e-16, - "velocityX": 2.901532594204896, - "velocityY": 0.019552331635990624, - "timestamp": 1.7220734152209634 - }, - { - "x": 7.396227777452995, - "y": 4.057522153925315, - "heading": -4.8982438983156224e-17, - "angularVelocity": 1.3353348954892513e-16, - "velocityX": 2.5793574184815227, - "velocityY": 0.033835028333003926, - "timestamp": 1.795991278967092 - }, - { - "x": 7.563054971618611, - "y": 4.060486200727262, - "heading": -3.912779424726568e-17, - "angularVelocity": 1.333188519862415e-16, - "velocityX": 2.256926617070343, - "velocityY": 0.04009919458884174, - "timestamp": 1.8699091427132206 - }, - { - "x": 7.70604444565388, - "y": 4.063541585038157, - "heading": -2.9177079820924904e-17, - "angularVelocity": 1.3461853362720707e-16, - "velocityX": 1.9344373171601388, - "velocityY": 0.041334856772760636, - "timestamp": 1.9438270064593492 - }, - { - "x": 7.825196583277639, - "y": 4.066433602906459, - "heading": -1.9253434148047247e-17, - "angularVelocity": 1.3425233319755054e-16, - "velocityX": 1.6119532084015245, - "velocityY": 0.03912474903541938, - "timestamp": 2.017744870205478 - }, - { - "x": 7.920513375065405, - "y": 4.0689768762979055, - "heading": -9.302606124041008e-18, - "angularVelocity": 1.346200704365749e-16, - "velocityX": 1.2894960292025357, - "velocityY": 0.034406749093584967, - "timestamp": 2.0916627339516065 - }, - { - "x": 7.991997299424133, - "y": 4.071030463180752, - "heading": 8.179153045928767e-19, - "angularVelocity": 1.369157726664237e-16, - "velocityX": 0.9670723792051265, - "velocityY": 0.027782010717991884, - "timestamp": 2.165580597697735 - }, - { - "x": 8.03965088919926, - "y": 4.072483600942051, - "heading": 2.229877553000587e-19, - "angularVelocity": -8.048494898574051e-18, - "velocityX": 0.6446829948819026, - "velocityY": 0.019658817066051014, - "timestamp": 2.2394984614438638 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 3.803306693773524e-29, - "angularVelocity": -3.0166964241453683e-18, - "velocityX": 0.32232632402054306, - "velocityY": 0.01032706968038683, - "timestamp": 2.3134163251899924 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 1.8619056951323718e-29, - "angularVelocity": -1.0754545583434217e-29, - "velocityX": -2.9997098934675086e-25, - "velocityY": 4.422144616548637e-24, - "timestamp": 2.387334188936121 + "heading": 9.183754863330666e-18, + "angularVelocity": -1.766389866662468e-16, + "velocityX": 3.555211493416193, + "velocityY": -1.2898268911821011, + "timestamp": 1.277848853854266 + }, + { + "x": 5.934272395491136, + "y": 4.124337934678792, + "heading": -1.4789774171207362e-18, + "angularVelocity": -1.4511299214964166e-16, + "velocityX": 3.651673302114514, + "velocityY": -0.9841085377661543, + "timestamp": 1.3513276797160874 + }, + { + "x": 6.207753017493673, + "y": 4.075010140740357, + "heading": -1.5184517487424443e-17, + "angularVelocity": -1.8652366732257328e-16, + "velocityX": 3.7218970063133985, + "velocityY": -0.6713198443208211, + "timestamp": 1.4248065055779087 + }, + { + "x": 6.4844286250858945, + "y": 4.049019985828209, + "heading": -2.210208594102609e-17, + "angularVelocity": -9.414369884747846e-17, + "velocityX": 3.7653787243758563, + "velocityY": -0.35370944768528717, + "timestamp": 1.49828533143973 + }, + { + "x": 6.743277363747609, + "y": 4.038426394199003, + "heading": -1.998169117841033e-17, + "angularVelocity": 2.885722162467868e-17, + "velocityX": 3.522766397335807, + "velocityY": -0.14417203194192602, + "timestamp": 1.5717641573015513 + }, + { + "x": 6.978672822837657, + "y": 4.030025299692623, + "heading": -1.7754526303263836e-17, + "angularVelocity": 3.031029482336482e-17, + "velocityX": 3.2035822065626665, + "velocityY": -0.1143335431376982, + "timestamp": 1.6452429831633726 + }, + { + "x": 7.19055269256627, + "y": 4.022995759457438, + "heading": -1.4995601737739213e-17, + "angularVelocity": 3.754720537740805e-17, + "velocityX": 2.8835500192539434, + "velocityY": -0.095667563447506, + "timestamp": 1.718721809025194 + }, + { + "x": 7.3789017577018114, + "y": 4.017045849522277, + "heading": -1.2218093470845859e-17, + "angularVelocity": 3.780011771168623e-17, + "velocityX": 2.5633107623376494, + "velocityY": -0.08097448299391073, + "timestamp": 1.7922006348870152 + }, + { + "x": 7.54371362899692, + "y": 4.012025997027571, + "heading": -9.378231648873842e-18, + "angularVelocity": 3.864870986523972e-17, + "velocityX": 2.2429845518359373, + "velocityY": -0.06831699385270533, + "timestamp": 1.8656794607488365 + }, + { + "x": 7.684984887810538, + "y": 4.007845278835319, + "heading": -6.610589160747635e-18, + "angularVelocity": 3.7665850749041794e-17, + "velocityX": 1.9226118158077508, + "velocityY": -0.05689691068436128, + "timestamp": 1.9391582866106578 + }, + { + "x": 7.8027134339680275, + "y": 4.0044425825275765, + "heading": -4.4053913927362146e-18, + "angularVelocity": 3.0011336492479294e-17, + "velocityX": 1.6022104977409417, + "velocityY": -0.046308528584020826, + "timestamp": 2.012637112472479 + }, + { + "x": 7.896897856926731, + "y": 4.001774011196325, + "heading": -2.659547727350374e-18, + "angularVelocity": 2.375981985162547e-17, + "velocityX": 1.281789983087365, + "velocityY": -0.03631755543113615, + "timestamp": 2.0861159383343004 + }, + { + "x": 7.967537148527336, + "y": 3.999806507404438, + "heading": -1.3934136908294963e-18, + "angularVelocity": 1.723127746899314e-17, + "velocityX": 0.9613557480279207, + "velocityY": -0.026776472933672534, + "timestamp": 2.1595947641961217 + }, + { + "x": 8.014630554462236, + "y": 3.9985142796609896, + "heading": -4.425448526284405e-19, + "angularVelocity": 1.2940719003719293e-17, + "velocityX": 0.6409112473226126, + "velocityY": -0.017586396193627105, + "timestamp": 2.233073590057943 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 3.3992773241588707e-43, + "angularVelocity": 6.022753459080235e-18, + "velocityX": 0.320458791984776, + "velocityY": -0.008677813220192955, + "timestamp": 2.3065524159197643 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": -7.143469642081013e-44, + "angularVelocity": -5.598380381353948e-42, + "velocityX": -5.65394748040853e-41, + "velocityY": -2.108446218115532e-39, + "timestamp": 2.3800312417815856 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.traj index 412c2ecc..8f5ce45b 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.traj @@ -3,1784 +3,1802 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.2424307943085809e-21, - "angularVelocity": -1.3399063907175068e-21, - "velocityX": -6.613526246981409e-20, - "velocityY": -3.127454467901795e-20, + "heading": 0, + "angularVelocity": -8.928261679081423e-33, + "velocityX": -3.922322521208882e-33, + "velocityY": -1.0855099176840522e-33, "timestamp": 0 }, { - "x": 1.3060573925677244, - "y": 5.572382489557548, - "heading": -0.010694903924415845, - "angularVelocity": -0.14212387666711845, - "velocityX": 0.21372515458978017, - "velocityY": -0.2467996203811156, - "timestamp": 0.07525057840538203 - }, - { - "x": 1.3382234703338376, - "y": 5.5352391647325, - "heading": -0.03207617924105129, - "angularVelocity": -0.2841343650736142, - "velocityX": 0.4274528973429488, - "velocityY": -0.49359520700229137, - "timestamp": 0.15050115681076406 - }, - { - "x": 1.3864731960687624, - "y": 5.479524641793448, - "heading": -0.06412173122661301, - "angularVelocity": -0.4258512381516761, - "velocityX": 0.6411874401150632, - "velocityY": -0.7403866404709017, - "timestamp": 0.2257517352161461 - }, - { - "x": 1.4508074313628532, - "y": 5.405239333646717, - "heading": -0.10679840962568042, - "angularVelocity": -0.5671275796601069, - "velocityX": 0.8549334325048802, - "velocityY": -0.9871725868543993, - "timestamp": 0.30100231362152813 - }, - { - "x": 1.5312274241075188, - "y": 5.312383875814348, - "heading": -0.16006570142322632, - "angularVelocity": -0.7078655463694897, - "velocityX": 1.0686960080417647, - "velocityY": -1.2339500878272134, - "timestamp": 0.37625289202691015 - }, - { - "x": 1.6277348707062913, - "y": 5.200959293585018, - "heading": -0.22388050123959674, - "angularVelocity": -0.8480306885163599, - "velocityX": 1.282481126973902, - "velocityY": -1.480713963806038, - "timestamp": 0.45150347043229216 - }, - { - "x": 1.740332134586227, - "y": 5.070967309798755, - "heading": -0.29820239377925056, - "angularVelocity": -0.9876587544520216, - "velocityX": 1.4962976533331545, - "velocityY": -1.7274549450767482, - "timestamp": 0.5267540488376742 - }, - { - "x": 1.869023591989777, - "y": 4.922411591689274, - "heading": -0.38299783161333695, - "angularVelocity": -1.126841010806393, - "velocityX": 1.7101723352912572, - "velocityY": -1.9741471927192127, - "timestamp": 0.6020046272430563 - }, - { - "x": 2.0218400763966784, - "y": 4.778612689185939, - "heading": -0.4624367890743981, - "angularVelocity": -1.0556590945137452, - "velocityX": 2.030768236539849, - "velocityY": -1.9109341821756607, - "timestamp": 0.6772552056484383 - }, - { - "x": 2.158571900472673, - "y": 4.653385130962418, - "heading": -0.5314200615234237, - "angularVelocity": -0.9167141822805146, - "velocityX": 1.817020240554264, - "velocityY": -1.664140806319243, - "timestamp": 0.7525057840538204 - }, - { - "x": 2.2792154502458053, - "y": 4.546725640045298, - "heading": -0.5899528620113615, - "angularVelocity": -0.777838545939356, - "velocityX": 1.6032242187324346, - "velocityY": -1.4173909779475127, - "timestamp": 0.8277563624592025 - }, - { - "x": 2.383769251108378, - "y": 4.458632746098652, - "heading": -0.6380359031204368, - "angularVelocity": -0.6389723790566404, - "velocityX": 1.3894086009456479, - "velocityY": -1.170660688773468, - "timestamp": 0.9030069408645846 - }, - { - "x": 2.4722324605852046, - "y": 4.389105547577967, - "heading": -0.6756681828251195, - "angularVelocity": -0.5000928963223903, - "velocityX": 1.1755817875613708, - "velocityY": -0.9239423801653125, - "timestamp": 0.9782575192699666 - }, - { - "x": 2.5446045503525854, - "y": 4.33814344893939, - "heading": -0.7028480663782789, - "angularVelocity": -0.36119168954076036, - "velocityX": 0.9617479533181219, - "velocityY": -0.6772319857003534, - "timestamp": 1.0535080976753486 - }, - { - "x": 2.600885196215428, - "y": 4.305746063716301, - "heading": -0.7195737337553716, - "angularVelocity": -0.22226629657236585, - "velocityX": 0.7479098108675392, - "velocityY": -0.4305267269649537, - "timestamp": 1.1287586760807307 - }, - { - "x": 2.6410742311964075, - "y": 4.29191316967755, - "heading": -0.7258433495686913, - "angularVelocity": -0.08331651325714302, - "velocityX": 0.5340694494662552, - "velocityY": -0.1838244214447216, - "timestamp": 1.2040092544861127 + "x": 1.306057392567738, + "y": 5.572382489557579, + "heading": -0.01069490392439116, + "angularVelocity": -0.14212387666683293, + "velocityX": 0.21372515459002697, + "velocityY": -0.2467996203807809, + "timestamp": 0.07525057840535954 + }, + { + "x": 1.33822347033388, + "y": 5.535239164732594, + "heading": -0.0320761792409761, + "angularVelocity": -0.28413436507302803, + "velocityX": 0.4274528973434579, + "velocityY": -0.49359520700160897, + "timestamp": 0.15050115681071907 + }, + { + "x": 1.3864731960688501, + "y": 5.4795246417936365, + "heading": -0.06412173122646007, + "angularVelocity": -0.42585123815077036, + "velocityX": 0.6411874401158554, + "velocityY": -0.7403866404698544, + "timestamp": 0.2257517352160786 + }, + { + "x": 1.4508074313630048, + "y": 5.405239333647035, + "heading": -0.1067984096254206, + "angularVelocity": -0.567127579658856, + "velocityX": 0.8549334325059841, + "velocityY": -0.9871725868529632, + "timestamp": 0.30100231362143814 + }, + { + "x": 1.5312274241077561, + "y": 5.312383875814834, + "heading": -0.16006570142282744, + "angularVelocity": -0.707865546367854, + "velocityX": 1.0686960080432237, + "velocityY": -1.233950087825352, + "timestamp": 0.3762528920267977 + }, + { + "x": 1.6277348707066417, + "y": 5.200959293585713, + "heading": -0.2238805012390217, + "angularVelocity": -0.848030688514272, + "velocityX": 1.2824811269757883, + "velocityY": -1.4807139638036912, + "timestamp": 0.4515034704321572 + }, + { + "x": 1.7403321345867286, + "y": 5.070967309799712, + "heading": -0.29820239377845176, + "angularVelocity": -0.9876587544493436, + "velocityX": 1.4962976533356125, + "velocityY": -1.727454945073796, + "timestamp": 0.5267540488375168 + }, + { + "x": 1.8690235919905007, + "y": 4.922411591690569, + "heading": -0.3829978316122351, + "angularVelocity": -1.126841010802703, + "velocityX": 1.7101723352947207, + "velocityY": -1.9741471927152945, + "timestamp": 0.6020046272428763 + }, + { + "x": 2.021840076397225, + "y": 4.778612689187088, + "heading": -0.46243678907347296, + "angularVelocity": -1.055659094516408, + "velocityX": 2.030768236538101, + "velocityY": -1.9109341821781907, + "timestamp": 0.6772552056482358 + }, + { + "x": 2.158571900473102, + "y": 4.653385130963448, + "heading": -0.5314200615226269, + "angularVelocity": -0.916714182282495, + "velocityX": 1.8170202405532427, + "velocityY": -1.6641408063212961, + "timestamp": 0.7525057840535954 + }, + { + "x": 2.2792154502461437, + "y": 4.546725640046215, + "heading": -0.5899528620106769, + "angularVelocity": -0.7778385459410783, + "velocityX": 1.6032242187317085, + "velocityY": -1.4173909779494496, + "timestamp": 0.8277563624589549 + }, + { + "x": 2.383769251108642, + "y": 4.458632746099449, + "heading": -0.6380359031198586, + "angularVelocity": -0.6389723790582461, + "velocityX": 1.3894086009450728, + "velocityY": -1.1706606887754087, + "timestamp": 0.9030069408643144 + }, + { + "x": 2.472232460585404, + "y": 4.389105547578634, + "heading": -0.6756681828246474, + "angularVelocity": -0.5000928963239504, + "velocityX": 1.175581787560874, + "velocityY": -0.9239423801673171, + "timestamp": 0.978257519269674 + }, + { + "x": 2.544604550352729, + "y": 4.338143448939913, + "heading": -0.7028480663779157, + "angularVelocity": -0.3611916895423175, + "velocityX": 0.9617479533176606, + "velocityY": -0.677231985702458, + "timestamp": 1.0535080976750335 + }, + { + "x": 2.6008851962155197, + "y": 4.305746063716667, + "heading": -0.7195737337551226, + "angularVelocity": -0.22226629657394859, + "velocityX": 0.7479098108670842, + "velocityY": -0.4305267269671826, + "timestamp": 1.128758676080393 + }, + { + "x": 2.641074231196452, + "y": 4.291913169677742, + "heading": -0.7258433495685629, + "angularVelocity": -0.08331651325877153, + "velocityX": 0.5340694494657862, + "velocityY": -0.18382442144709177, + "timestamp": 1.2040092544857526 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.05565717315833812, - "velocityX": 0.3202286619480617, - "velocityY": 0.06287683198324669, - "timestamp": 1.2792598328914948 - }, - { - "x": 2.6731094248331972, - "y": 4.320288497186145, - "heading": -0.7067986870474628, - "angularVelocity": 0.19580509628770462, - "velocityX": 0.10461882238047351, - "velocityY": 0.31162123137301345, - "timestamp": 1.355133384348122 - }, - { - "x": 2.6646882165451484, - "y": 4.3628057123101, - "heading": -0.6813129128721624, - "angularVelocity": 0.33589799984345314, - "velocityX": -0.11099003706005371, - "velocityY": 0.5603693817898957, - "timestamp": 1.431006935804749 - }, - { - "x": 2.6399081500891253, - "y": 4.424196738035329, - "heading": -0.6452019314670412, - "angularVelocity": 0.47593635347047025, - "velocityX": -0.32659689681441373, - "velocityY": 0.8091228701785946, - "timestamp": 1.5068804872613761 - }, - { - "x": 2.5987694540063986, - "y": 4.504462102028234, - "heading": -0.5984691810768386, - "angularVelocity": 0.6159293916394221, - "velocityX": -0.5422007444352149, - "velocityY": 1.057883313117199, - "timestamp": 1.5827540387180032 - }, - { - "x": 2.5412724297468685, - "y": 4.603602434973848, - "heading": -0.5411158632570503, - "angularVelocity": 0.7559065935192721, - "velocityX": -0.7578006189995966, - "velocityY": 1.30665206837309, - "timestamp": 1.6586275901746304 - }, - { - "x": 2.4674174122162253, - "y": 4.7216183838553025, - "heading": -0.4731379534998271, - "angularVelocity": 0.8959368377013766, - "velocityX": -0.973396079566176, - "velocityY": 1.5554293507523191, - "timestamp": 1.7345011416312575 - }, - { - "x": 2.3772045458929667, - "y": 4.8585103095087545, - "heading": -0.3945211417087051, - "angularVelocity": 1.0361556864260284, - "velocityX": -1.1889896359316807, - "velocityY": 1.8042113888883347, - "timestamp": 1.8103746930878846 - }, - { - "x": 2.270632337424553, - "y": 5.014276859543665, - "heading": -0.3052336675309315, - "angularVelocity": 1.176793130987344, - "velocityX": -1.4046028744197523, - "velocityY": 2.0529756027560517, - "timestamp": 1.8862482445445117 - }, - { - "x": 2.1562113194234667, - "y": 5.146461372900032, - "heading": -0.22910323394931606, - "angularVelocity": 1.0033856610117322, - "velocityX": -1.5080487970369372, - "velocityY": 1.7421685266957554, - "timestamp": 1.9621217960011388 - }, - { - "x": 2.05813446327852, - "y": 5.259759383516201, - "heading": -0.1637433708814866, - "angularVelocity": 0.861431444990317, - "velocityX": -1.2926356320754544, - "velocityY": 1.49324775815898, - "timestamp": 2.037995347457766 - }, - { - "x": 1.9764036449833426, - "y": 5.354173277082403, - "heading": -0.10920518207306891, - "angularVelocity": 0.7188036906324902, - "velocityX": -1.077197741849454, - "velocityY": 1.2443584325978703, - "timestamp": 2.1138688989143932 - }, - { - "x": 1.9110189705233616, - "y": 5.429703862947866, - "heading": -0.06553401357536529, - "angularVelocity": 0.5755782833319475, - "velocityX": -0.8617584547542609, - "velocityY": 0.9954797741165524, - "timestamp": 2.1897424503710203 - }, - { - "x": 1.8619803278190585, - "y": 5.486351581956986, - "heading": -0.032764600292238745, - "angularVelocity": 0.43189507613676265, - "velocityX": -0.6463206448473131, - "velocityY": 0.7466069258864354, - "timestamp": 2.2656160018276474 - }, - { - "x": 1.8292876878125428, - "y": 5.524116686024045, - "heading": -0.010918035230093376, - "angularVelocity": 0.287933866844679, - "velocityX": -0.43088321791822976, - "velocityY": 0.4977373978420955, - "timestamp": 2.3414895532842745 + "angularVelocity": 0.055657173156648594, + "velocityX": 0.320228661947563, + "velocityY": 0.06287683198072233, + "timestamp": 1.2792598328911122 + }, + { + "x": 2.6731094248331484, + "y": 4.3202884971859605, + "heading": -0.7067986870475855, + "angularVelocity": 0.19580509628602769, + "velocityX": 0.1046188223798005, + "velocityY": 0.3116212313704759, + "timestamp": 1.355133384347763 + }, + { + "x": 2.6646882165450285, + "y": 4.362805712309731, + "heading": -0.6813129128724049, + "angularVelocity": 0.3358979998417677, + "velocityX": -0.11099003706094854, + "velocityY": 0.5603693817873046, + "timestamp": 1.431006935804414 + }, + { + "x": 2.6399081500889086, + "y": 4.424196738034775, + "heading": -0.6452019314674031, + "angularVelocity": 0.47593635346874824, + "velocityX": -0.3265968968155938, + "velocityY": 0.8091228701758959, + "timestamp": 1.5068804872610648 + }, + { + "x": 2.5987694540060504, + "y": 4.504462102027486, + "heading": -0.5984691810773222, + "angularVelocity": 0.6159293916376241, + "velocityX": -0.542200744436769, + "velocityY": 1.0578833131143173, + "timestamp": 1.5827540387177157 + }, + { + "x": 2.5412724297463463, + "y": 4.603602434972891, + "heading": -0.5411158632576627, + "angularVelocity": 0.7559065935173384, + "velocityX": -0.757800619001658, + "velocityY": 1.3066520683699112, + "timestamp": 1.6586275901743666 + }, + { + "x": 2.4674174122154677, + "y": 4.7216183838541035, + "heading": -0.4731379535005828, + "angularVelocity": 0.8959368376992063, + "velocityX": -0.9733960795689668, + "velocityY": 1.5554293507486547, + "timestamp": 1.7345011416310174 + }, + { + "x": 2.37720454589188, + "y": 4.858510309507254, + "heading": -0.39452114170963426, + "angularVelocity": 1.0361556864234158, + "velocityX": -1.188989635935645, + "velocityY": 1.8042113888838065, + "timestamp": 1.8103746930876683 + }, + { + "x": 2.270632337422942, + "y": 5.01427685954172, + "heading": -0.305233667532112, + "angularVelocity": 1.1767931309836617, + "velocityX": -1.4046028744262238, + "velocityY": 2.0529756027495263, + "timestamp": 1.8862482445443192 + }, + { + "x": 2.156211319422429, + "y": 5.146461372898718, + "heading": -0.2291032339501207, + "angularVelocity": 1.003385661016371, + "velocityX": -1.508048797028907, + "velocityY": 1.7421685267035296, + "timestamp": 1.96212179600097 + }, + { + "x": 2.058134463277846, + "y": 5.259759383515319, + "heading": -0.16374337088202898, + "angularVelocity": 0.8614314449935028, + "velocityX": -1.2926356320702503, + "velocityY": 1.493247758164214, + "timestamp": 2.037995347457621 + }, + { + "x": 1.976403644982925, + "y": 5.354173277081842, + "heading": -0.10920518207341517, + "angularVelocity": 0.7188036906348487, + "velocityX": -1.0771977418457426, + "velocityY": 1.2443584326016996, + "timestamp": 2.113868898914272 + }, + { + "x": 1.9110189705231253, + "y": 5.429703862947542, + "heading": -0.06553401357556585, + "angularVelocity": 0.5755782833336871, + "velocityX": -0.861758454751598, + "velocityY": 0.9954797741193555, + "timestamp": 2.189742450370923 + }, + { + "x": 1.8619803278189464, + "y": 5.486351581956828, + "heading": -0.032764600292336035, + "angularVelocity": 0.43189507613798817, + "velocityX": -0.6463206448454762, + "velocityY": 0.7466069258884014, + "timestamp": 2.265616001827574 + }, + { + "x": 1.829287687812507, + "y": 5.524116686023993, + "heading": -0.010918035230124941, + "angularVelocity": 0.28793386684545474, + "velocityX": -0.43088321791708567, + "velocityY": 0.4977373978433378, + "timestamp": 2.3414895532842253 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 2.4600717676583652e-21, - "angularVelocity": 0.1438977749227231, - "velocityX": -0.21544390526203344, - "velocityY": 0.24886908799668503, - "timestamp": 2.4173631047409017 + "heading": -6.884486248240525e-32, + "angularVelocity": 0.14389777492309389, + "velocityX": -0.21544390526149312, + "velocityY": 0.2488690879972792, + "timestamp": 2.4173631047408763 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.1389483058862038e-21, - "angularVelocity": -1.912999393800189e-21, - "velocityX": -4.5659699774939935e-20, - "velocityY": -6.062533120754166e-20, - "timestamp": 2.4932366561975288 + "heading": -2.79027732569672e-32, + "angularVelocity": 5.39778462149558e-31, + "velocityX": -1.372141826224318e-33, + "velocityY": 4.2150961560958763e-32, + "timestamp": 2.4932366561975274 }, { "x": 1.8507557561824508, "y": 5.5430527114919315, - "heading": -3.663099117513338e-19, - "angularVelocity": -3.946860070844576e-18, + "heading": -1.045695782000071e-18, + "angularVelocity": -1.1232079825061295e-17, "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395093343, - "timestamp": 2.586335690239902 + "velocityY": 0.0005740544395097007, + "timestamp": 2.586335690239901 }, { "x": 1.9263848800936068, "y": 5.543159599317717, - "heading": -1.3979636927804314e-18, - "angularVelocity": -1.108125118245985e-17, - "velocityX": 0.8123513276919049, - "velocityY": 0.0011481088593976478, - "timestamp": 2.6794347242822756 + "heading": -3.434421194050575e-18, + "angularVelocity": -2.5657896847386088e-17, + "velocityX": 0.812351327691905, + "velocityY": 0.0011481088593983807, + "timestamp": 2.6794347242822742 }, { "x": 2.039828562987611, "y": 5.543319931052194, - "heading": -3.639885752731889e-18, - "angularVelocity": -2.408104534113515e-17, + "heading": -7.689792330773154e-18, + "angularVelocity": -4.570800524939683e-17, "velocityX": 1.2185269596070252, - "velocityY": 0.0017221632439681264, - "timestamp": 2.772533758324649 + "velocityY": 0.0017221632439692253, + "timestamp": 2.7725337583246477 }, { "x": 2.1910867994360017, "y": 5.543533706687691, - "heading": -8.31799511662456e-18, - "angularVelocity": -5.0248742235094303e-17, - "velocityX": 1.6247025332136762, - "velocityY": 0.002296217546130332, - "timestamp": 2.8656327923670224 + "heading": -1.506248986539688e-17, + "angularVelocity": -7.919198744068695e-17, + "velocityX": 1.6247025332136764, + "velocityY": 0.0022962175461317975, + "timestamp": 2.865632792367021 }, { "x": 2.380159562296481, "y": 5.5438009261858445, - "heading": -4.964025869598509e-18, - "angularVelocity": 3.602582219554166e-17, + "heading": -1.506353706937017e-17, + "angularVelocity": -1.1248279684723702e-20, "velocityX": 2.0308778152781284, - "velocityY": 0.0028702714362513803, - "timestamp": 2.958731826409396 + "velocityY": 0.0028702714362532117, + "timestamp": 2.9587318264093945 }, { "x": 2.5314177987448714, "y": 5.544014701821341, - "heading": -1.164993974430083e-17, - "angularVelocity": -7.181507244960338e-17, - "velocityX": 1.6247025332136764, - "velocityY": 0.0022962175461303325, - "timestamp": 3.051830860451769 + "heading": -7.704513323430137e-18, + "angularVelocity": 7.904511385790122e-17, + "velocityX": 1.6247025332136762, + "velocityY": 0.002296217546131797, + "timestamp": 3.051830860451768 }, { "x": 2.6448614816388756, "y": 5.544175033555818, - "heading": -5.397697309971077e-18, - "angularVelocity": 6.71568990871082e-17, + "heading": -3.433430554917341e-18, + "angularVelocity": 4.587676781446345e-17, "velocityX": 1.218526959607025, - "velocityY": 0.0017221632439681266, - "timestamp": 3.1449298944941426 + "velocityY": 0.0017221632439692251, + "timestamp": 3.1449298944941413 }, { "x": 2.7204906055500317, "y": 5.544281921381604, - "heading": -1.7112683408390196e-18, - "angularVelocity": 3.959685518772811e-17, - "velocityX": 0.8123513276919049, - "velocityY": 0.001148108859397648, - "timestamp": 3.238028928536516 + "heading": -1.0420071252828256e-18, + "angularVelocity": 2.5686876928777523e-17, + "velocityX": 0.812351327691905, + "velocityY": 0.0011481088593983805, + "timestamp": 3.2380289285365147 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": 2.8361562565882696e-27, - "angularVelocity": 1.838116110697215e-17, + "heading": -1.1539777836664478e-35, + "angularVelocity": 1.1192459041074064e-17, "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395093341, - "timestamp": 3.3311279625788894 + "velocityY": 0.0005740544395097005, + "timestamp": 3.331127962578888 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": 2.9956768552498905e-27, - "angularVelocity": 1.2964344677835282e-27, - "velocityX": -1.3759431197263344e-22, - "velocityY": -2.5705739302596115e-25, - "timestamp": 3.424226996621263 - }, - { - "x": 2.7425045239965504, - "y": 5.564231116573788, - "heading": 0.0035475080989150498, - "angularVelocity": 0.04647392993401548, - "velocityX": -0.20699544832908057, - "velocityY": 0.26064316841975155, - "timestamp": 3.5005602893716685 - }, - { - "x": 2.7110743119163434, - "y": 5.604155225406367, - "heading": 0.01083868300587181, - "angularVelocity": 0.09551762598264181, - "velocityX": -0.4117497221425694, - "velocityY": 0.523023537883034, - "timestamp": 3.576893582122074 - }, - { - "x": 2.6642693453223805, - "y": 5.6643007250236845, - "heading": 0.022164936239812177, - "angularVelocity": 0.14837894221299866, - "velocityX": -0.6131658272230635, - "velocityY": 0.7879327283048184, - "timestamp": 3.65322687487248 - }, - { - "x": 2.60250917282708, - "y": 5.744974159519079, - "heading": 0.038001263541227126, - "angularVelocity": 0.20746291337380784, - "velocityX": -0.8090856593497568, - "velocityY": 1.0568577823464311, - "timestamp": 3.7295601676228856 - }, - { - "x": 2.526611736042362, - "y": 5.846734963341242, - "heading": 0.05925383993510262, - "angularVelocity": 0.27841817938297336, - "velocityX": -0.994290093483679, - "velocityY": 1.333111675856818, - "timestamp": 3.8058934603732912 - }, - { - "x": 2.4388385302471143, - "y": 5.970894906525885, - "heading": 0.08830658815011594, - "angularVelocity": 0.3806038907558967, - "velocityX": -1.149867936160551, - "velocityY": 1.6265503387967306, - "timestamp": 3.882226753123697 - }, - { - "x": 2.357791815962569, - "y": 6.119054185868648, - "heading": 0.1383597950882533, - "angularVelocity": 0.655719216800998, - "velocityX": -1.0617479131884362, - "velocityY": 1.9409522896806517, - "timestamp": 3.9585600458741026 - }, - { - "x": 2.2985967230184703, - "y": 6.254273676601559, - "heading": 0.19308840139911443, - "angularVelocity": 0.7169690228065029, - "velocityX": -0.7754819792414106, - "velocityY": 1.7714353182044804, - "timestamp": 4.034893338624508 - }, - { - "x": 2.2587354140798723, - "y": 6.372988574603886, - "heading": 0.24867936598579424, - "angularVelocity": 0.7282662988016366, - "velocityX": -0.5222008314109583, - "velocityY": 1.5552178312351754, - "timestamp": 4.111226631374914 - }, - { - "x": 2.2373189281980324, - "y": 6.474209987526736, - "heading": 0.30392383910912424, - "angularVelocity": 0.7237271069121585, - "velocityX": -0.2805654663931197, - "velocityY": 1.3260454157771489, - "timestamp": 4.1875599241253205 - }, - { - "x": 2.233901000873839, - "y": 6.557480302062636, - "heading": 0.35823088728392755, - "angularVelocity": 0.7114464242015108, - "velocityX": -0.044776364297153176, - "velocityY": 1.090878062972822, - "timestamp": 4.263893216875727 - }, - { - "x": 2.248214127214941, - "y": 6.6225364653992, - "heading": 0.41124898234582946, - "angularVelocity": 0.6945605665834471, - "velocityX": 0.1875083050315006, - "velocityY": 0.8522646016238898, - "timestamp": 4.340226509626133 - }, - { - "x": 2.280080256208058, - "y": 6.669207835860357, - "heading": 0.4627444117016496, - "angularVelocity": 0.6746129703090314, - "velocityX": 0.41746042709454745, - "velocityY": 0.6114156586138921, - "timestamp": 4.416559802376539 - }, - { - "x": 2.3293724060058594, - "y": 6.6973748207092285, + "heading": 0, + "angularVelocity": 7.156872912951633e-35, + "velocityX": -1.751436869795613e-36, + "velocityY": -4.841667244220732e-36, + "timestamp": 3.4242269966212615 + }, + { + "x": 2.7396220511907092, + "y": 5.562601673573336, + "heading": 0.0037078860807813565, + "angularVelocity": 0.0478983711971462, + "velocityX": -0.2413479950107084, + "velocityY": 0.23596367181626482, + "timestamp": 3.5016385241556733 + }, + { + "x": 2.702380969511178, + "y": 5.599259386918254, + "heading": 0.011292704027819494, + "angularVelocity": 0.09798047123752286, + "velocityX": -0.4810792767650385, + "velocityY": 0.47354334053959213, + "timestamp": 3.579050051690085 + }, + { + "x": 2.6467699433072402, + "y": 5.6544931206396525, + "heading": 0.023007945503191744, + "angularVelocity": 0.15133716965040475, + "velocityX": -0.7183817187849313, + "velocityY": 0.7135078647924301, + "timestamp": 3.656461579224497 + }, + { + "x": 2.573102880230356, + "y": 5.72860253774289, + "heading": 0.03927230490424403, + "angularVelocity": 0.21010255086133378, + "velocityX": -0.9516291103303323, + "velocityY": 0.9573434275701879, + "timestamp": 3.733873106758909 + }, + { + "x": 2.4820081686264115, + "y": 5.82215722498829, + "heading": 0.060903903359814396, + "angularVelocity": 0.27943639848670454, + "velocityX": -1.1767589983733413, + "velocityY": 1.2085368965728291, + "timestamp": 3.8112846342933207 + }, + { + "x": 2.3753503305689, + "y": 5.936638406081269, + "heading": 0.09019255741494601, + "angularVelocity": 0.3783500337480349, + "velocityX": -1.3778030411568694, + "velocityY": 1.478864772976981, + "timestamp": 3.8886961618277325 + }, + { + "x": 2.2779168004384966, + "y": 6.074876724828902, + "heading": 0.14378193450398408, + "angularVelocity": 0.6922661106928292, + "velocityX": -1.25864368310122, + "velocityY": 1.7857588288279347, + "timestamp": 3.9661076893621443 + }, + { + "x": 2.2032159820535764, + "y": 6.200217981312059, + "heading": 0.2000311193060264, + "angularVelocity": 0.7266254341388357, + "velocityX": -0.9649831331866354, + "velocityY": 1.6191549304777415, + "timestamp": 4.043519216896556 + }, + { + "x": 2.149623841782867, + "y": 6.310133396102646, + "heading": 0.2561023628726984, + "angularVelocity": 0.7243267941166335, + "velocityX": -0.6923018053982481, + "velocityY": 1.4198843284900602, + "timestamp": 4.120930744430967 + }, + { + "x": 2.1165539042639407, + "y": 6.4038530923195935, + "heading": 0.3110558787207687, + "angularVelocity": 0.7098880179523891, + "velocityX": -0.42719655033581183, + "velocityY": 1.2106684779639116, + "timestamp": 4.1983422719653785 + }, + { + "x": 2.1037067095365876, + "y": 6.481006876335726, + "heading": 0.36442032903480814, + "angularVelocity": 0.6893605127520233, + "velocityX": -0.16595971086660646, + "velocityY": 0.9966704762651166, + "timestamp": 4.27575379949979 + }, + { + "x": 2.1109008821512623, + "y": 6.541377447734871, + "heading": 0.4159102858737852, + "angularVelocity": 0.6651458571992167, + "velocityX": 0.09293412549541542, + "velocityY": 0.7798653937207066, + "timestamp": 4.353165327034201 + }, + { + "x": 2.1380148682221702, + "y": 6.5848219723004595, + "heading": 0.465333297367931, + "angularVelocity": 0.6384451136450677, + "velocityX": 0.3502577320781457, + "velocityY": 0.5612151826648336, + "timestamp": 4.430576854568613 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, "heading": 0.5125504196, - "angularVelocity": 0.652480799710893, - "velocityX": 0.6457490306225382, - "velocityY": 0.3689999976939454, - "timestamp": 4.492893095126945 - }, - { - "x": 2.4053669705551046, - "y": 6.740800351083451, - "heading": 0.5568713225103131, - "angularVelocity": 0.5531681306272371, - "velocityX": 0.948486345023332, - "velocityY": 0.5419930073900953, - "timestamp": 4.573015033569164 - }, - { - "x": 2.502567960679766, - "y": 6.796343857044693, - "heading": 0.5404821418694866, - "angularVelocity": -0.204552972125677, - "velocityX": 1.213163236118634, - "velocityY": 0.6932371712561282, - "timestamp": 4.653136972011383 - }, - { - "x": 2.575459179910046, - "y": 6.837996044585651, - "heading": 0.5268532537172577, - "angularVelocity": -0.17010182750455535, - "velocityX": 0.9097535662201399, - "velocityY": 0.5198599578440831, - "timestamp": 4.7332589104536025 - }, - { - "x": 2.6240500842159764, - "y": 6.865762317125851, - "heading": 0.5173922106041889, - "angularVelocity": -0.11808305311898519, - "velocityX": 0.6064619160577619, - "velocityY": 0.34655018438206425, - "timestamp": 4.813380848895822 + "angularVelocity": 0.6099494963599527, + "velocityX": 0.6064560493947925, + "velocityY": 0.34126004006795546, + "timestamp": 4.507988382103024 + }, + { + "x": 2.250707976446258, + "y": 6.648678224064634, + "heading": 0.55208552781375, + "angularVelocity": 0.5325541984016288, + "velocityX": 0.8856313560565373, + "velocityY": 0.5043159387113171, + "timestamp": 4.582225160880485 + }, + { + "x": 2.3371670619589073, + "y": 6.698226995892447, + "heading": 0.5853560107794161, + "angularVelocity": 0.44816711492022887, + "velocityX": 1.1646395080237382, + "velocityY": 0.6674423734944788, + "timestamp": 4.656461939657946 + }, + { + "x": 2.4409073566894506, + "y": 6.758690881227722, + "heading": 0.5617681472290976, + "angularVelocity": -0.31773824159353153, + "velocityX": 1.3974245170513666, + "velocityY": 0.8144734501011613, + "timestamp": 4.7306987184354075 + }, + { + "x": 2.523886913207481, + "y": 6.807069780587539, + "heading": 0.5422957792802172, + "angularVelocity": -0.2623008200187737, + "velocityX": 1.1177688186980357, + "velocityY": 0.6516837092951017, + "timestamp": 4.804935497212869 + }, + { + "x": 2.5861173110961473, + "y": 6.843356427328165, + "heading": 0.5274956073850987, + "angularVelocity": -0.1993644139582728, + "velocityX": 0.8382691021011789, + "velocityY": 0.4887960838037212, + "timestamp": 4.87917227599033 + }, + { + "x": 2.627602512984248, + "y": 6.8675485030782895, + "heading": 0.5175502299027057, + "angularVelocity": -0.13396833276139558, + "velocityX": 0.5588227637470666, + "velocityY": 0.3258772288954636, + "timestamp": 4.953409054767791 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.06043027787801988, - "velocityX": 0.30321823224102623, - "velocityY": 0.17326782030138713, - "timestamp": 4.893502787338041 + "angularVelocity": -0.06734950498988643, + "velocityX": 0.2794033376916902, + "velocityY": 0.16294305705300594, + "timestamp": 5.027645833545252 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": 1.1533672139704627e-20, - "velocityX": 1.3759845110959627e-18, - "velocityY": -2.227992646788969e-18, - "timestamp": 4.97362472578026 - }, - { - "x": 2.6571120181256966, - "y": 6.867792271481583, - "heading": 0.507044677895467, - "angularVelocity": -0.09452799306194601, - "velocityX": 0.15052909368010856, - "velocityY": -0.20349709127294008, - "timestamp": 5.031869289387202 - }, - { - "x": 2.6746521578330125, - "y": 6.844087252656105, - "heading": 0.49617547030513726, - "angularVelocity": -0.18661325482116445, - "velocityX": 0.30114638381847736, - "velocityY": -0.40699109680773127, - "timestamp": 5.090113852994143 - }, - { - "x": 2.7009708210230947, - "y": 6.808530072252695, - "heading": 0.4801087501886783, - "angularVelocity": -0.27584926594838455, - "velocityX": 0.45186471595342564, - "velocityY": -0.6104806732412771, - "timestamp": 5.148358416601085 - }, - { - "x": 2.73607484037394, - "y": 6.7611211343580875, - "heading": 0.4590398225372461, - "angularVelocity": -0.361732088742461, - "velocityX": 0.602700358229852, - "velocityY": -0.8139633119159942, - "timestamp": 5.206602980208027 - }, - { - "x": 2.7799722580023465, - "y": 6.701861088549211, - "heading": 0.4332010241003249, - "angularVelocity": -0.4436259255248601, - "velocityX": 0.7536740754835817, - "velocityY": -1.0174347980145875, - "timestamp": 5.264847543814969 - }, - { - "x": 2.832672676333526, - "y": 6.630750980608444, - "heading": 0.40287238280523807, - "angularVelocity": -0.5207119672104833, - "velocityX": 0.9048126566253185, - "velocityY": -1.2208883290919084, - "timestamp": 5.32309210742191 - }, - { - "x": 2.8941877403139116, - "y": 6.547792490679094, - "heading": 0.3683969681281308, - "angularVelocity": -0.5919078544353333, - "velocityX": 1.0561511696699046, - "velocityY": -1.4243130138151228, - "timestamp": 5.381336671028852 - }, - { - "x": 2.9645318213846927, - "y": 6.452988329970027, - "heading": 0.33020407419243936, - "angularVelocity": -0.6557331975810253, - "velocityX": 1.2077364257631285, - "velocityY": -1.627691149835798, - "timestamp": 5.439581234635794 - }, - { - "x": 3.04372302387486, - "y": 6.346342948824998, - "heading": 0.28884632079593436, - "angularVelocity": -0.7100706200771626, - "velocityX": 1.3596325147971153, - "velocityY": -1.8309928779742386, - "timestamp": 5.497825798242736 - }, - { - "x": 3.1317847243622103, - "y": 6.227863919029491, - "heading": 0.24506332676078993, - "angularVelocity": -0.7517095386036364, - "velocityX": 1.5119299559290544, - "velocityY": -2.0341646062463727, - "timestamp": 5.556070361849677 - }, - { - "x": 3.2287480012560774, - "y": 6.097564965294096, - "heading": 0.199900944291634, - "angularVelocity": -0.7753922370151164, - "velocityX": 1.6647609817838902, - "velocityY": -2.237100695177408, - "timestamp": 5.614314925456619 - }, - { - "x": 3.334655409720646, - "y": 5.955473736284568, - "heading": 0.1549620363959118, - "angularVelocity": -0.7715554055652005, - "velocityX": 1.8183226365858802, - "velocityY": -2.4395620845993164, - "timestamp": 5.672559489063561 - }, - { - "x": 3.4495650121656865, - "y": 5.8016567423537415, - "heading": 0.11303057394210353, - "angularVelocity": -0.7199206218932102, - "velocityX": 1.972881163991502, - "velocityY": -2.6408815588154475, - "timestamp": 5.730804052670503 - }, - { - "x": 3.5735262627096924, - "y": 5.636333431818966, - "heading": 0.08010242298971296, - "angularVelocity": -0.565342907787981, - "velocityX": 2.1282887683826965, - "velocityY": -2.838433328309314, - "timestamp": 5.789048616277444 - }, - { - "x": 3.7050107789119795, - "y": 5.46057947422046, - "heading": 0.07776405618458973, - "angularVelocity": -0.04014738303996017, - "velocityX": 2.2574555985962665, - "velocityY": -3.017516944320591, - "timestamp": 5.847293179884386 - }, - { - "x": 3.8485069507845076, - "y": 5.2934525910716275, - "heading": 0.07776403563237232, - "angularVelocity": -3.5286069899218235e-7, - "velocityX": 2.463683526601701, - "velocityY": -2.8693988382619477, - "timestamp": 5.905537743491328 - }, - { - "x": 4.002902013863093, - "y": 5.136338903873299, - "heading": 0.07776401516857159, - "angularVelocity": -3.5134267388587325e-7, - "velocityX": 2.650806418956214, - "velocityY": -2.697482433873059, - "timestamp": 5.96378230709827 + "angularVelocity": -4.084231073354503e-32, + "velocityX": -1.1079933009257942e-33, + "velocityY": -1.0209152810451872e-34, + "timestamp": 5.101882612322713 + }, + { + "x": 2.6570277255317225, + "y": 6.867726793502589, + "heading": 0.5066730395869481, + "angularVelocity": -0.10087167460547103, + "velocityX": 0.14902725506901718, + "velocityY": -0.20454631284120778, + "timestamp": 5.160148523303567 + }, + { + "x": 2.6744037356282937, + "y": 6.843893366933042, + "heading": 0.49507655406382295, + "angularVelocity": -0.1990269323504748, + "velocityX": 0.2982191439910878, + "velocityY": -0.4090458068591583, + "timestamp": 5.218414434284421 + }, + { + "x": 2.7004835818665893, + "y": 6.808147821587349, + "heading": 0.4779456476925187, + "angularVelocity": -0.29401250375942084, + "velocityX": 0.44760042019879576, + "velocityY": -0.6134898561431401, + "timestamp": 5.276680345265275 + }, + { + "x": 2.735280123191829, + "y": 6.760494081762779, + "heading": 0.4554977531812837, + "angularVelocity": -0.38526634413407634, + "velocityX": 0.5972023905482879, + "velocityY": -0.8178665539139939, + "timestamp": 5.334946256246129 + }, + { + "x": 2.77880855180183, + "y": 6.700937036637074, + "heading": 0.4279915994110024, + "angularVelocity": -0.4720796999006835, + "velocityX": 0.7470650999399128, + "velocityY": -1.022159340223411, + "timestamp": 5.393212167226983 + }, + { + "x": 2.8310870878518544, + "y": 6.629482937316093, + "heading": 0.39573903851012515, + "angularVelocity": -0.5535408330177088, + "velocityX": 0.8972405162806606, + "velocityY": -1.2263448407158972, + "timestamp": 5.4514780782078365 + }, + { + "x": 2.8921379557757043, + "y": 6.546140000901142, + "heading": 0.3591220542576228, + "angularVelocity": -0.6284460954285072, + "velocityX": 1.0477973637777922, + "velocityY": -1.4303893136132022, + "timestamp": 5.50974398918869 + }, + { + "x": 2.9619888074879523, + "y": 6.4509193752842275, + "heading": 0.3186184742438743, + "angularVelocity": -0.6951505491273999, + "velocityX": 1.1988287926228676, + "velocityY": -1.6342424586513933, + "timestamp": 5.568009900169544 + }, + { + "x": 3.0406748905144103, + "y": 6.3438367809482505, + "heading": 0.27484324456730397, + "angularVelocity": -0.7513008711209391, + "velocityX": 1.3504651639672256, + "velocityY": -1.8378257978522827, + "timestamp": 5.626275811150398 + }, + { + "x": 3.12824252650645, + "y": 6.224915546783436, + "heading": 0.22861956489914778, + "angularVelocity": -0.7933228690673962, + "velocityX": 1.502896539639008, + "velocityY": -2.0410087504491825, + "timestamp": 5.684541722131252 + }, + { + "x": 3.224755029145573, + "y": 6.094192875417537, + "heading": 0.18111264418965425, + "angularVelocity": -0.815346742370587, + "velocityX": 1.6564145486515685, + "velocityY": -2.243553205730433, + "timestamp": 5.742807633112106 + }, + { + "x": 3.3303033780389395, + "y": 5.951734868373249, + "heading": 0.13411173965139422, + "angularVelocity": -0.8066621416715647, + "velocityX": 1.8114940128208097, + "velocityY": -2.444963180805693, + "timestamp": 5.80107354409296 + }, + { + "x": 3.4450256229008205, + "y": 5.797681352790493, + "heading": 0.09073046456880836, + "angularVelocity": -0.7445395489798686, + "velocityX": 1.9689427819909868, + "velocityY": -2.643973345467458, + "timestamp": 5.859339455073814 + }, + { + "x": 3.5691217392378216, + "y": 5.632434411388871, + "heading": 0.05763038975845234, + "angularVelocity": -0.5680864548952619, + "velocityX": 2.1298236695857957, + "velocityY": -2.8360826874554848, + "timestamp": 5.9176053660546675 + }, + { + "x": 3.7017085484583103, + "y": 5.4576517803314095, + "heading": 0.0547360803841417, + "angularVelocity": -0.049674146093104596, + "velocityX": 2.2755468332771343, + "velocityY": -2.999740810967421, + "timestamp": 5.975871277035521 + }, + { + "x": 3.8463662629346644, + "y": 5.2914222205797, + "heading": 0.05473605711060691, + "angularVelocity": -3.9943655557036433e-7, + "velocityX": 2.4827160863217212, + "velocityY": -2.8529470655034146, + "timestamp": 6.034137188016375 + }, + { + "x": 4.001863921193704, + "y": 5.135285744872655, + "heading": 0.05473603479828713, + "angularVelocity": -3.8293951656527263e-7, + "velocityX": 2.668758724293121, + "velocityY": -2.679722552666384, + "timestamp": 6.092403098997229 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.07776399412255879, - "angularVelocity": -3.6133866407320583e-7, - "velocityX": 2.8259641989395843, - "velocityY": -2.513386639709188, - "timestamp": 6.022026870705211 - }, - { - "x": 4.268976588700125, - "y": 4.906724140265527, - "heading": 0.07776397443770015, - "angularVelocity": -5.672615420255969e-7, - "velocityX": 2.9242931040000055, - "velocityY": -2.398268643648094, - "timestamp": 6.056728427523216 - }, - { - "x": 4.37370366170879, - "y": 4.827628597038176, - "heading": 0.07776395550809555, - "angularVelocity": -5.454972726636976e-7, - "velocityX": 3.0179358683506776, - "velocityY": -2.2793082063198216, - "timestamp": 6.09142998434122 - }, - { - "x": 4.4815124411311835, - "y": 4.752787886123784, - "heading": 0.0777639371726549, - "angularVelocity": -5.283751607380448e-7, - "velocityX": 3.106741867167675, - "velocityY": -2.1566960614159445, - "timestamp": 6.126131541159224 - }, - { - "x": 4.592230128734662, - "y": 4.682321875197502, - "heading": 0.07776391928788347, - "angularVelocity": -5.153881575305611e-7, - "velocityX": 3.1905683132358433, - "velocityY": -2.030629671626745, - "timestamp": 6.160833097977228 - }, - { - "x": 4.705679213251417, - "y": 4.616343315466854, - "heading": 0.07776390172302422, - "angularVelocity": -5.061691995708879e-7, - "velocityX": 3.269279390309479, - "velocityY": -1.9013141132738893, - "timestamp": 6.195534654795233 - }, - { - "x": 4.821677528011508, - "y": 4.554957192131842, - "heading": 0.07776388435588834, - "angularVelocity": -5.004713755679868e-7, - "velocityX": 3.3427409429626933, - "velocityY": -1.7689731805681452, - "timestamp": 6.230236211613237 - }, - { - "x": 4.939562213059443, - "y": 4.4972764656411774, - "heading": 0.07776386706661859, - "angularVelocity": -4.982274959213974e-7, - "velocityX": 3.397100760239454, - "velocityY": -1.6621942004843482, - "timestamp": 6.264937768431241 - }, - { - "x": 5.0574486320523055, - "y": 4.439599283026335, - "heading": 0.07776384977740043, - "angularVelocity": -4.982260088149746e-7, - "velocityX": 3.397150727592071, - "velocityY": -1.6620920760798994, - "timestamp": 6.299639325249245 - }, - { - "x": 5.175337486723699, - "y": 4.381927078962276, - "heading": 0.07776383248820665, - "angularVelocity": -4.9822530674329e-7, - "velocityX": 3.3972209169079175, - "velocityY": -1.6619486084306254, - "timestamp": 6.3343408820672495 - }, - { - "x": 5.295110440251067, - "y": 4.328277244219036, - "heading": 0.07776381519347733, - "angularVelocity": -4.983848248451832e-7, - "velocityX": 3.451515277989629, - "velocityY": -1.5460353846547075, - "timestamp": 6.369042438885254 - }, - { - "x": 5.416934261342807, - "y": 4.279463089154562, - "heading": 0.07776379778914431, - "angularVelocity": -5.015432909319555e-7, - "velocityX": 3.5106154381100505, - "velocityY": -1.4066848735485782, - "timestamp": 6.403743995703258 - }, - { - "x": 5.540614064021598, - "y": 4.235563528216825, - "heading": 0.07776378015516597, - "angularVelocity": -5.081610154974924e-7, - "velocityX": 3.5640995396097326, - "velocityY": -1.26506027288553, - "timestamp": 6.438445552521262 + "heading": 0.0547360118053875, + "angularVelocity": -3.946201001676767e-7, + "velocityX": 2.842745293382521, + "velocityY": -2.4943907433712678, + "timestamp": 6.150669009978083 + }, + { + "x": 4.269399861624288, + "y": 4.9075045981351995, + "heading": 0.054735989995402, + "angularVelocity": -6.29291522756053e-7, + "velocityX": 2.940181103085103, + "velocityY": -2.3787638667331756, + "timestamp": 6.185327009826526 + }, + { + "x": 4.374514707093794, + "y": 4.829200558555379, + "heading": 0.05473596913891511, + "angularVelocity": -6.017798771949806e-7, + "velocityX": 3.0329172464990677, + "velocityY": -2.2593352161762894, + "timestamp": 6.21998500967497 + }, + { + "x": 4.482675566428291, + "y": 4.7551608256734985, + "heading": 0.0547359490445404, + "angularVelocity": -5.797903745228634e-7, + "velocityX": 3.120805003389626, + "velocityY": -2.1362956086805514, + "timestamp": 6.254643009523413 + }, + { + "x": 4.593709527898866, + "y": 4.685503714898617, + "heading": 0.05473592954464412, + "angularVelocity": -5.62637669848401e-7, + "velocityX": 3.203703674652828, + "velocityY": -2.0098422032282808, + "timestamp": 6.289301009371856 + }, + { + "x": 4.707439069036232, + "y": 4.620340494746092, + "heading": 0.05473591048922828, + "angularVelocity": -5.498129124049459e-7, + "velocityX": 3.2814802249032198, + "velocityY": -1.8801783264319567, + "timestamp": 6.3239590092202995 + }, + { + "x": 4.823682298536361, + "y": 4.559775113095544, + "heading": 0.054735891740807854, + "angularVelocity": -5.409550611531198e-7, + "velocityX": 3.3540085985473036, + "velocityY": -1.747515203283397, + "timestamp": 6.358617009068743 + }, + { + "x": 4.942252940190184, + "y": 4.503903323846341, + "heading": 0.05473587317010921, + "angularVelocity": -5.358271892357539e-7, + "velocityX": 3.4211622763091856, + "velocityY": -1.6120892577045927, + "timestamp": 6.393275008917186 + }, + { + "x": 5.062124990174475, + "y": 4.450881200757823, + "heading": 0.05473585465308408, + "angularVelocity": -5.342785275702722e-7, + "velocityX": 3.4587122888938238, + "velocityY": -1.52986679324772, + "timestamp": 6.4279330087656295 + }, + { + "x": 5.1819977334562, + "y": 4.397860645099044, + "heading": 0.05473583613607813, + "angularVelocity": -5.3427797428813e-7, + "velocityX": 3.4587322928593447, + "velocityY": -1.529821567621728, + "timestamp": 6.462591008614073 + }, + { + "x": 5.301870477238367, + "y": 4.344840090571707, + "heading": 0.05473581761907218, + "angularVelocity": -5.342779742919752e-7, + "velocityX": 3.458732307298825, + "velocityY": -1.5298215349758078, + "timestamp": 6.497249008462516 + }, + { + "x": 5.421743975168144, + "y": 4.291821241109097, + "heading": 0.0547357991020742, + "angularVelocity": -5.342777443988636e-7, + "velocityX": 3.4587540669967396, + "velocityY": -1.5297723381170152, + "timestamp": 6.531907008310959 + }, + { + "x": 5.5428966637680075, + "y": 4.241794448876403, + "heading": 0.05473578057951323, + "angularVelocity": -5.34438255269497e-7, + "velocityX": 3.4956630252656034, + "velocityY": -1.4434414118372951, + "timestamp": 6.566565008159403 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.07776376214569573, - "angularVelocity": -5.189816214982998e-7, - "velocityX": 3.6118744025389584, - "velocityY": -1.1214036841768227, - "timestamp": 6.4731471093392665 - }, - { - "x": 5.801257373802826, - "y": 4.160893829303833, - "heading": 0.07776374465549807, - "angularVelocity": -4.7264774236323325e-7, - "velocityX": 3.6564428162530316, - "velocityY": -0.9662347026048084, - "timestamp": 6.510151829987593 - }, - { - "x": 5.937965681779902, - "y": 4.1309456443700405, - "heading": 0.07776372758116286, - "angularVelocity": -4.614096500770092e-7, - "velocityX": 3.6943477908205034, - "velocityY": -0.8093071480907553, - "timestamp": 6.547156550635919 - }, - { - "x": 6.075827506190729, - "y": 4.1068588699437765, - "heading": 0.07776371078252527, - "angularVelocity": -4.53959313742904e-7, - "velocityX": 3.7255199335509808, - "velocityY": -0.6509108568923265, - "timestamp": 6.584161271284246 - }, - { - "x": 6.2145915217996235, - "y": 4.088676368882122, - "heading": 0.0777636941280861, - "angularVelocity": -4.5006255582880887e-7, - "velocityX": 3.7499003688646537, - "velocityY": -0.4913562578799624, - "timestamp": 6.621165991932572 - }, - { - "x": 6.354004262869319, - "y": 4.076423083568819, - "heading": 0.07776367749075697, - "angularVelocity": -4.496001822495745e-7, - "velocityX": 3.7674312527474547, - "velocityY": -0.33112762638453114, - "timestamp": 6.658170712580898 - }, - { - "x": 6.49381285233259, - "y": 4.070129387117659, - "heading": 0.07776362451928018, - "angularVelocity": -0.0000014314788993599032, - "velocityX": 3.778128493170848, - "velocityY": -0.17007820464237144, - "timestamp": 6.695175433229225 - }, - { - "x": 6.629142082966274, - "y": 4.065515785319137, - "heading": 0.06238171659626575, - "angularVelocity": -0.4156742073314349, - "velocityX": 3.657080184979125, - "velocityY": -0.12467603369760688, - "timestamp": 6.732180153877551 - }, - { - "x": 6.758586834582697, - "y": 4.061933354582955, - "heading": 0.046880370354106624, - "angularVelocity": -0.4189018582108937, - "velocityX": 3.4980605000804377, - "velocityY": -0.0968101008038334, - "timestamp": 6.769184874525878 - }, - { - "x": 6.882125546140559, - "y": 4.059204781585899, - "heading": 0.032519289448839375, - "angularVelocity": -0.3880878075461594, - "velocityX": 3.338458158674076, - "velocityY": -0.07373580854685112, - "timestamp": 6.806189595174204 - }, - { - "x": 6.999755813322696, - "y": 4.05726367776388, - "heading": 0.01976964076410941, - "angularVelocity": -0.34454114127480795, - "velocityX": 3.1787908440123913, - "velocityY": -0.0524555729109991, - "timestamp": 6.84319431582253 - }, - { - "x": 7.111477612575904, - "y": 4.056075301962013, - "heading": 0.008878948528675913, - "angularVelocity": -0.2943054843984055, - "velocityX": 3.0191228928587734, - "velocityY": -0.03211416762637815, - "timestamp": 6.880199036470857 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -2.4974876469169754e-20, - "angularVelocity": -0.23994096896600625, - "velocityX": 2.8594660546893573, - "velocityY": -0.012350203465742983, - "timestamp": 6.917203757119183 - }, - { - "x": 7.386543487491071, - "y": 4.0570668323942645, - "heading": -0.009095721053457144, - "angularVelocity": -0.13843657850499821, - "velocityX": 2.57601194788322, - "velocityY": 0.022046826970953105, - "timestamp": 6.982906919908118 - }, - { - "x": 7.537031363324535, - "y": 4.05969750834718, - "heading": -0.013766029362825559, - "angularVelocity": -0.07108194052044785, - "velocityX": 2.2904205740732744, - "velocityY": 0.04003880241453354, - "timestamp": 7.048610082697054 - }, - { - "x": 7.6687074557367385, - "y": 4.062834301345331, - "heading": -0.015395893103822788, - "angularVelocity": -0.02480647311048019, - "velocityX": 2.0041058424417098, - "velocityY": 0.04774188737652573, - "timestamp": 7.114313245485989 - }, - { - "x": 7.781556988474885, - "y": 4.066015749956383, - "heading": -0.01492426229398527, - "angularVelocity": 0.007178205581253095, - "velocityX": 1.7175662167232906, - "velocityY": 0.04842154435201139, - "timestamp": 7.180016408274924 - }, - { - "x": 7.8755781593330285, - "y": 4.068907471663171, - "heading": -0.013029646010250598, - "angularVelocity": 0.028835998197239424, - "velocityX": 1.4309991614890225, - "velocityY": 0.044011910295353764, - "timestamp": 7.2457195710638596 - }, - { - "x": 7.950774573438036, - "y": 4.07125628842811, - "heading": -0.010225311537090487, - "angularVelocity": 0.042681879442679815, - "velocityX": 1.144486976168374, - "velocityY": 0.035748914743771835, - "timestamp": 7.311422733852795 - }, - { - "x": 8.007152043467745, - "y": 4.072863968231118, - "heading": -0.006913103442788773, - "angularVelocity": 0.0504116994328239, - "velocityX": 0.8580632596152882, - "velocityY": 0.024468834296048715, - "timestamp": 7.37712589664173 - }, - { - "x": 8.044717148266278, - "y": 4.073571148367015, - "heading": -0.0034161675760999402, - "angularVelocity": 0.053223250118451255, - "velocityX": 0.5717396728557544, - "velocityY": 0.010763258660280774, - "timestamp": 7.442829059430665 + "heading": 0.054735761954551196, + "angularVelocity": -5.373928706734811e-7, + "velocityX": 3.550552991831721, + "velocityY": -1.3025960678451716, + "timestamp": 6.601223008007846 + }, + { + "x": 5.798180192000891, + "y": 4.154397155999577, + "heading": 0.05473574376724012, + "angularVelocity": -4.955056404583076e-7, + "velocityX": 3.6025088632415367, + "velocityY": -1.15113574961205, + "timestamp": 6.637927557031803 + }, + { + "x": 5.93207840031246, + "y": 4.117779387908136, + "heading": 0.054735725716885786, + "angularVelocity": -4.917743118523635e-7, + "velocityX": 3.6480003670436316, + "velocityY": -0.9976356899941846, + "timestamp": 6.6746321060557605 + }, + { + "x": 6.067406110251661, + "y": 4.086860105171276, + "heading": 0.0547357076680171, + "angularVelocity": -4.917338358563753e-7, + "velocityX": 3.686946537631365, + "velocityY": -0.8423828533264233, + "timestamp": 6.711336655079718 + }, + { + "x": 6.203921243902459, + "y": 4.061696453595828, + "heading": 0.05473568948478555, + "angularVelocity": -4.953944957116945e-7, + "velocityX": 3.719297397216157, + "velocityY": -0.6855731031873736, + "timestamp": 6.748041204103675 + }, + { + "x": 6.341379198553944, + "y": 4.042334078664949, + "heading": 0.054735669406532635, + "angularVelocity": -5.47023555566929e-7, + "velocityX": 3.7449841588235726, + "velocityY": -0.5275197610585458, + "timestamp": 6.784745753127632 + }, + { + "x": 6.479285910845981, + "y": 4.028828408214569, + "heading": 0.05386070036293022, + "angularVelocity": -0.023838163575618188, + "velocityX": 3.7572103719902135, + "velocityY": -0.3679563108530375, + "timestamp": 6.8214503021515895 + }, + { + "x": 6.612620100679574, + "y": 4.0185345658565055, + "heading": 0.045620785713098924, + "angularVelocity": -0.22449300887617385, + "velocityX": 3.6326339208408243, + "velocityY": -0.2804514053923919, + "timestamp": 6.858154851175547 + }, + { + "x": 6.740581673709006, + "y": 4.010579483858113, + "heading": 0.035519685699448925, + "angularVelocity": -0.27520022128747124, + "velocityX": 3.4862592357669087, + "velocityY": -0.2167328630900888, + "timestamp": 6.894859400199504 + }, + { + "x": 6.863028396494948, + "y": 4.00465723612987, + "heading": 0.025264871332947036, + "angularVelocity": -0.27938810417772464, + "velocityX": 3.3360094604627837, + "velocityY": -0.16134914842236953, + "timestamp": 6.931563949223461 + }, + { + "x": 6.979909878212025, + "y": 4.000619227079583, + "heading": 0.015648348920582984, + "angularVelocity": -0.26199810835673765, + "velocityX": 3.184386808315938, + "velocityY": -0.1100138581637957, + "timestamp": 6.968268498247419 + }, + { + "x": 7.091201711972647, + "y": 3.998378156691166, + "heading": 0.00712732553533738, + "angularVelocity": -0.23215169813648398, + "velocityX": 3.0320992007824574, + "velocityY": -0.06105702012450316, + "timestamp": 7.004973047271376 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -3.597785008712915e-31, + "angularVelocity": -0.19418098641357107, + "velocityX": 2.87942961178202, + "velocityY": -0.013663498666533047, + "timestamp": 7.041677596295333 + }, + { + "x": 7.37119660747943, + "y": 4.00262802828422, + "heading": -0.0076676970964569885, + "angularVelocity": -0.11437965193876058, + "velocityX": 2.6001474595947878, + "velocityY": 0.07087677805125822, + "timestamp": 7.10871484432405 + }, + { + "x": 7.526209738417895, + "y": 4.010745052233057, + "heading": -0.011651875074249012, + "angularVelocity": -0.059432302114867865, + "velocityX": 2.3123432941647923, + "velocityY": 0.1210822965966538, + "timestamp": 7.1757520923527665 + }, + { + "x": 7.661711723306385, + "y": 4.020683594487604, + "heading": -0.013062756224948863, + "angularVelocity": -0.021046227167551262, + "velocityX": 2.0212939652660666, + "velocityY": 0.1482540311065431, + "timestamp": 7.242789340381483 + }, + { + "x": 7.777626027630173, + "y": 4.031355931329743, + "heading": -0.012680116865926582, + "angularVelocity": 0.005707861976350362, + "velocityX": 1.729102964879078, + "velocityY": 0.15920010376271715, + "timestamp": 7.3098265884102 + }, + { + "x": 7.873936260933145, + "y": 4.041961506483741, + "heading": -0.011077902998110952, + "angularVelocity": 0.023900352638719452, + "velocityX": 1.4366674667449708, + "velocityY": 0.15820421431164344, + "timestamp": 7.3768638364389165 + }, + { + "x": 7.950652622834928, + "y": 4.051889363613367, + "heading": -0.008694806000859931, + "angularVelocity": 0.035548848846393165, + "velocityX": 1.1443841171541105, + "velocityY": 0.14809464024199548, + "timestamp": 7.443901084467633 + }, + { + "x": 8.007796949441587, + "y": 4.060659248403223, + "heading": -0.005876390393834335, + "angularVelocity": 0.04204253142698025, + "velocityX": 0.8524264985069137, + "velocityY": 0.1308210740706325, + "timestamp": 7.51093833249635 + }, + { + "x": 8.045395699194648, + "y": 4.067884383860418, + "heading": -0.00290156854092843, + "angularVelocity": 0.04437565592835165, + "velocityX": 0.5608635625518943, + "velocityY": 0.1077779245069823, + "timestamp": 7.577975580525067 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 5.107015447929928e-20, - "angularVelocity": 0.05199395936347874, - "velocityX": 0.28551767430107416, - "velocityY": -0.004934199232898976, - "timestamp": 7.508532222219601 - }, - { - "x": 8.06095184830656, - "y": 4.071458641245482, - "heading": 0.0034408492569327057, - "angularVelocity": 0.04684199855212652, - "velocityX": -0.034370194612683154, - "velocityY": -0.02434521970346025, - "timestamp": 7.581988722012275 - }, - { - "x": 8.034920990250148, - "y": 4.068388525754522, - "heading": 0.006505362431754428, - "angularVelocity": 0.041718747605332625, - "velocityX": -0.3543710649143617, - "velocityY": -0.041795014731511364, - "timestamp": 7.655445221804949 - }, - { - "x": 7.985375296011398, - "y": 4.064207377475085, - "heading": 0.009196043333328236, - "angularVelocity": 0.036629582258453454, - "velocityX": -0.6744902680986578, - "velocityY": -0.05692005869104256, - "timestamp": 7.728901721597623 - }, - { - "x": 7.912305948559603, - "y": 4.059120814150924, - "heading": 0.011515917649017375, - "angularVelocity": 0.03158160710402538, - "velocityX": -0.9947295019232808, - "velocityY": -0.06924592566372595, - "timestamp": 7.802358221390297 - }, - { - "x": 7.815704588785296, - "y": 4.0533811177027905, - "heading": 0.013468719297185195, - "angularVelocity": 0.026584463644190544, - "velocityX": -1.3150825324778364, - "velocityY": -0.0781373529140697, - "timestamp": 7.875814721182971 - }, - { - "x": 7.695564554163692, - "y": 4.047305026722375, - "heading": 0.015059175300172275, - "angularVelocity": 0.021651671499132708, - "velocityX": -1.6355262633081036, - "velocityY": -0.08271685960487347, - "timestamp": 7.949271220975645 - }, - { - "x": 7.551883548681463, - "y": 4.04130179428642, - "heading": 0.016293463732843762, - "angularVelocity": 0.016802984571211377, - "velocityX": -1.9560012509139277, - "velocityY": -0.08172499986929728, - "timestamp": 8.02272772076832 - }, - { - "x": 7.384669657766205, - "y": 4.035920047959674, - "heading": 0.017179998132389497, - "angularVelocity": 0.012068835324959903, - "velocityX": -2.276366167557798, - "velocityY": -0.07326439922859854, - "timestamp": 8.096184220560993 - }, - { - "x": 7.19395595849013, - "y": 4.0319320028167684, - "heading": 0.017730886759136543, - "angularVelocity": 0.007499521870792628, - "velocityX": -2.5962807895060482, - "velocityY": -0.054291249299417726, - "timestamp": 8.169640720353666 - }, - { - "x": 6.979840325568109, - "y": 4.030500043249597, - "heading": 0.017964968086133067, - "angularVelocity": 0.00318666595409774, - "velocityX": -2.9148629941032773, - "velocityY": -0.01949398039945934, - "timestamp": 8.243097220146339 - }, - { - "x": 6.742614358141766, - "y": 4.033551118259164, - "heading": 0.017915208666011314, - "angularVelocity": -0.0006773998252331979, - "velocityX": -3.22947551402392, - "velocityY": 0.041535807153613755, - "timestamp": 8.316553719939012 - }, - { - "x": 6.483307205091183, - "y": 4.044763583343556, - "heading": 0.017651500017929662, - "angularVelocity": -0.003589997465520818, - "velocityX": -3.530077716505134, - "velocityY": 0.1526408842789682, - "timestamp": 8.390010219731685 - }, - { - "x": 6.206941447635851, - "y": 4.072199279636492, - "heading": 0.017375156748179146, - "angularVelocity": -0.0037619988773032, - "velocityX": -3.762305013652376, - "velocityY": 0.37349582910118817, - "timestamp": 8.463466719524359 - }, - { - "x": 5.9338077856619895, - "y": 4.122953167788701, - "heading": 0.017375155210986027, - "angularVelocity": -2.0926577278102936e-8, - "velocityX": -3.7183048844521998, - "velocityY": 0.690938014953861, - "timestamp": 8.536923219317032 + "heading": 0, + "angularVelocity": 0.043282930404384794, + "velocityX": 0.26971368660010536, + "velocityY": 0.07999391635030323, + "timestamp": 7.645012828553783 + }, + { + "x": 8.059946431342839, + "y": 4.076566093038108, + "heading": 0.0028549561404960684, + "angularVelocity": 0.03895223157485018, + "velocityX": -0.048164132671898556, + "velocityY": 0.045285389048645555, + "timestamp": 7.718306604675698 + }, + { + "x": 8.033099887642729, + "y": 4.077511417867968, + "heading": 0.00539411397159862, + "angularVelocity": 0.03464356682727019, + "velocityX": -0.3662868134321987, + "velocityY": 0.012897750394093884, + "timestamp": 7.791600380797613 + }, + { + "x": 7.9829172655770035, + "y": 4.07628474252501, + "heading": 0.007619394466085314, + "angularVelocity": 0.03036111130070867, + "velocityX": -0.6846778092351761, + "velocityY": -0.01673641894119354, + "timestamp": 7.864894156919528 + }, + { + "x": 7.909377202284024, + "y": 4.0731292983210805, + "heading": 0.00953312535115696, + "angularVelocity": 0.026110414639960534, + "velocityX": -1.0033602740109082, + "velocityY": -0.04305200756310189, + "timestamp": 7.938187933041442 + }, + { + "x": 7.81245694795258, + "y": 4.068343833996595, + "heading": 0.011138189892816102, + "angularVelocity": 0.021899056462711293, + "velocityX": -1.3223531309156478, + "velocityY": -0.06529155103873027, + "timestamp": 8.011481709163357 + }, + { + "x": 7.692133347423482, + "y": 4.062303858835979, + "heading": 0.012438255979270885, + "angularVelocity": 0.01773774193721827, + "velocityX": -1.641661910404963, + "velocityY": -0.0824077497462929, + "timestamp": 8.084775485285272 + }, + { + "x": 7.548385409224001, + "y": 4.055495143379703, + "heading": 0.013438147372137587, + "angularVelocity": 0.013642241480415802, + "velocityX": -1.9612570917396126, + "velocityY": -0.0928962296191558, + "timestamp": 8.158069261407187 + }, + { + "x": 7.381200809360299, + "y": 4.048569614878887, + "heading": 0.014144485416621113, + "angularVelocity": 0.009637080825370731, + "velocityX": -2.2810204182359235, + "velocityY": -0.09448999447506434, + "timestamp": 8.231363037529102 + }, + { + "x": 7.190592956257051, + "y": 4.04244548860493, + "heading": 0.014566894857523213, + "angularVelocity": 0.00576323752509999, + "velocityX": -2.6006008038962976, + "velocityY": -0.08355588425094046, + "timestamp": 8.304656813651016 + }, + { + "x": 6.9766499613051005, + "y": 4.038503705706744, + "heading": 0.01472053802802316, + "angularVelocity": 0.002096264903098756, + "velocityX": -2.918979022121646, + "velocityY": -0.05378059511668448, + "timestamp": 8.377950589772931 + }, + { + "x": 6.739697889009689, + "y": 4.039020009641981, + "heading": 0.014632337527092197, + "angularVelocity": -0.0012033832283965633, + "velocityX": -3.232908506464046, + "velocityY": 0.007044308023879432, + "timestamp": 8.451244365894846 + }, + { + "x": 6.480981935824527, + "y": 4.048232688232657, + "heading": 0.01435998143770215, + "angularVelocity": -0.003715951118919239, + "velocityX": -3.529848874955213, + "velocityY": 0.12569523741486752, + "timestamp": 8.52453814201676 + }, + { + "x": 6.206211729235204, + "y": 4.074520375352005, + "heading": 0.014063196474504363, + "angularVelocity": -0.004049251913342814, + "velocityX": -3.7488886659663403, + "velocityY": 0.3586619288876869, + "timestamp": 8.597831918138676 + }, + { + "x": 5.93349789020665, + "y": 4.124154607710869, + "heading": 0.014063194566661911, + "angularVelocity": -2.6030074482550523e-8, + "velocityX": -3.7208321559927344, + "velocityY": 0.6771957318218086, + "timestamp": 8.67112569426059 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.017375154199310676, - "angularVelocity": -1.377244159549356e-8, - "velocityX": -3.646458211284153, - "velocityY": 1.003259166632557, - "timestamp": 8.610379719109705 - }, - { - "x": 5.541092339335072, - "y": 4.2364336029082, - "heading": 0.01737515315796098, - "angularVelocity": -3.005342133477009e-8, - "velocityX": -3.6034502673557895, - "velocityY": 1.148184128750311, - "timestamp": 8.645029674075602 - }, - { - "x": 5.417922638814364, - "y": 4.281176215218788, - "heading": 0.017375152214225534, - "angularVelocity": -2.7236267527590686e-8, - "velocityX": -3.5546857316822638, - "velocityY": 1.2912747608077488, - "timestamp": 8.679679629041498 - }, - { - "x": 5.296639420951804, - "y": 4.33080542124118, - "heading": 0.017375151348717865, - "angularVelocity": -2.4978608726563434e-8, - "velocityX": -3.500241716964063, - "velocityY": 1.4323021796489428, - "timestamp": 8.714329584007395 - }, - { - "x": 5.177436464453021, - "y": 4.385241922289634, - "heading": 0.017375150546348885, - "angularVelocity": -2.3156422113484916e-8, - "velocityX": -3.4402052359405113, - "velocityY": 1.571041033156597, - "timestamp": 8.748979538973291 - }, - { - "x": 5.0605042229676425, - "y": 4.444398737399711, - "heading": 0.017375149795171643, - "angularVelocity": -2.167902498222188e-8, - "velocityX": -3.374672249948487, - "velocityY": 1.7072696102577922, - "timestamp": 8.783629493939188 - }, - { - "x": 4.946029519855076, - "y": 4.5081813412028975, - "heading": 0.017375149085535665, - "angularVelocity": -2.0480141439242773e-8, - "velocityX": -3.3037475294046756, - "velocityY": 1.8407701789500717, - "timestamp": 8.818279448905084 - }, - { - "x": 4.834195247479412, - "y": 4.576487812332227, - "heading": 0.017375148409491465, - "angularVelocity": -1.9510680466596443e-8, - "velocityX": -3.2275445230947857, - "velocityY": 1.9713292902272803, - "timestamp": 8.85292940387098 - }, - { - "x": 4.725180068322848, - "y": 4.649208988191292, - "heading": 0.01737514776036044, - "angularVelocity": -1.8733964504925207e-8, - "velocityX": -3.146185305691103, - "velocityY": 2.0987379617271817, - "timestamp": 8.887579358836877 - }, - { - "x": 4.619158095512492, - "y": 4.726228597192443, - "heading": 0.017375147132049722, - "angularVelocity": -1.813308899206411e-8, - "velocityX": -3.059801171883295, - "velocityY": 2.222791027490615, - "timestamp": 8.922229313802774 - }, - { - "x": 4.51014571232548, - "y": 4.798953964824882, - "heading": 0.01737514649311973, - "angularVelocity": -1.8439562026762692e-8, - "velocityX": -3.146104613824239, - "velocityY": 2.0988589365849712, - "timestamp": 8.95687926876867 - }, - { - "x": 4.398314099643168, - "y": 4.867264790943429, - "heading": 0.017375145827920196, - "angularVelocity": -1.9197702730409006e-8, - "velocityX": -3.2274677641681775, - "velocityY": 1.9714549755050896, - "timestamp": 8.991529223734567 - }, - { - "x": 4.2838418935996625, - "y": 4.931051865994745, - "heading": 0.017375140544826046, - "angularVelocity": -1.5247044777404565e-7, - "velocityX": -3.3036754638258423, - "velocityY": 1.840899219467885, - "timestamp": 9.026179178700463 + "heading": 0.01406319364211473, + "angularVelocity": -1.2614265905768763e-8, + "velocityX": -3.6503257922039465, + "velocityY": 0.9890944453863133, + "timestamp": 8.744419470382505 + }, + { + "x": 5.5408846553597755, + "y": 4.23596798413579, + "heading": 0.01406319265930284, + "angularVelocity": -2.835157538408451e-8, + "velocityX": -3.6078608681964144, + "velocityY": 1.1342486182256648, + "timestamp": 8.779084630162778 + }, + { + "x": 5.417489605980159, + "y": 4.2802558166388724, + "heading": 0.014063191785023112, + "angularVelocity": -2.5220703836459854e-8, + "velocityX": -3.5596273076991327, + "velocityY": 1.2775891639848806, + "timestamp": 8.813749789943051 + }, + { + "x": 5.295963908009713, + "y": 4.329441746794644, + "heading": 0.014063190996401449, + "angularVelocity": -2.2749690676604448e-8, + "velocityX": -3.5057013653114657, + "velocityY": 1.4188865843266973, + "timestamp": 8.848414949723324 + }, + { + "x": 5.176501898681268, + "y": 4.383447115848144, + "heading": 0.014063190276173353, + "angularVelocity": -2.077671352365433e-8, + "velocityX": -3.446169297521196, + "velocityY": 1.5579149034885322, + "timestamp": 8.883080109503597 + }, + { + "x": 5.05929461402676, + "y": 4.442185556946357, + "heading": 0.01406318961105745, + "angularVelocity": -1.9186869677954374e-8, + "velocityX": -3.381126335416613, + "velocityY": 1.6944517628226294, + "timestamp": 8.91774526928387 + }, + { + "x": 4.944529482696556, + "y": 4.505563132440428, + "heading": 0.014063188990615918, + "angularVelocity": -1.7898129854823097e-8, + "velocityX": -3.310676542604923, + "velocityY": 1.828278764494154, + "timestamp": 8.952410429064143 + }, + { + "x": 4.832390024623587, + "y": 4.573478482189006, + "heading": 0.014063188406478968, + "angularVelocity": -1.685083682180617e-8, + "velocityX": -3.2349326754519327, + "velocityY": 1.9591817888352978, + "timestamp": 8.987075588844416 + }, + { + "x": 4.723055552753515, + "y": 4.64582297983034, + "heading": 0.014063187851803343, + "angularVelocity": -1.600095394099115e-8, + "velocityX": -3.1540160946348355, + "velocityY": 2.086951224223198, + "timestamp": 9.021740748624689 + }, + { + "x": 4.616700862639107, + "y": 4.722480877082754, + "heading": 0.014063187320883878, + "angularVelocity": -1.5315650301874817e-8, + "velocityX": -3.068057115228709, + "velocityY": 2.2113816217294997, + "timestamp": 9.056405908404962 + }, + { + "x": 4.508256477944758, + "y": 4.796152911545187, + "heading": 0.014063186782201262, + "angularVelocity": -1.5539597107094908e-8, + "velocityX": -3.128339386915466, + "velocityY": 2.125247220246655, + "timestamp": 9.091071068185235 + }, + { + "x": 4.396953351691581, + "y": 4.865430375769311, + "heading": 0.014063186216514915, + "angularVelocity": -1.6318584724554417e-8, + "velocityX": -3.2108066704055713, + "velocityY": 1.998475260556751, + "timestamp": 9.125736227965508 + }, + { + "x": 4.28296944450211, + "y": 4.930202424249945, + "heading": 0.014063179833248412, + "angularVelocity": -1.8414069176204727e-7, + "velocityX": -3.288140251248267, + "velocityY": 1.8685056953781265, + "timestamp": 9.16040138774578 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.01686674274932993, - "angularVelocity": -0.014672394119890892, - "velocityX": -3.3576617434279026, - "velocityY": 1.6997404450060698, - "timestamp": 9.06082913366636 - }, - { - "x": 3.941206209015057, - "y": 5.0867984564258695, - "heading": 0.014981559594595455, - "angularVelocity": -0.027398080889315734, - "velocityX": -3.288799800865747, - "velocityY": 1.4075673366199248, - "timestamp": 9.129636257871532 - }, - { - "x": 3.7303087168982803, - "y": 5.169878519937419, - "heading": 0.012882808496808885, - "angularVelocity": -0.03050194470456946, - "velocityX": -3.0650531402520937, - "velocityY": 1.2074340334849432, - "timestamp": 9.198443382076704 - }, - { - "x": 3.5370906486542313, - "y": 5.2422774011944036, - "heading": 0.010786769401609648, - "angularVelocity": -0.030462530143669606, - "velocityX": -2.808111376198552, - "velocityY": 1.0522003657804835, - "timestamp": 9.267250506281876 - }, - { - "x": 3.362377869511596, - "y": 5.305500032913324, - "heading": 0.008787829989227822, - "angularVelocity": -0.029051343672224058, - "velocityX": -2.5391669999413216, - "velocityY": 0.9188384553087892, - "timestamp": 9.336057630487048 - }, - { - "x": 3.2065764087500264, - "y": 5.360415401059238, - "heading": 0.0069378788339289135, - "angularVelocity": -0.026886040895746154, - "velocityX": -2.2643216463602664, - "velocityY": 0.7981058470364881, - "timestamp": 9.40486475469222 - }, - { - "x": 3.0699232813800306, - "y": 5.407585492361195, - "heading": 0.005269555505228705, - "angularVelocity": -0.024246374891727204, - "velocityX": -1.9860316638509532, - "velocityY": 0.6855408047763067, - "timestamp": 9.473671878897392 - }, - { - "x": 2.9525724844135883, - "y": 5.4474025208162065, - "heading": 0.00380523783536115, - "angularVelocity": -0.021281483375198414, - "velocityX": -1.70550358443293, - "velocityY": 0.5786759571041632, - "timestamp": 9.542479003102564 - }, - { - "x": 2.854631606493464, - "y": 5.480155404840544, - "heading": 0.0025612082007498414, - "angularVelocity": -0.018079953914391628, - "velocityX": -1.423411878515389, - "velocityY": 0.47601007021705255, - "timestamp": 9.611286127307736 - }, - { - "x": 2.776179840063629, - "y": 5.506065679558788, - "heading": 0.0015498402303536507, - "angularVelocity": -0.014698593816833106, - "velocityX": -1.1401692388117297, - "velocityY": 0.376563837212327, - "timestamp": 9.680093251512908 - }, - { - "x": 2.717277807675311, - "y": 5.525308541082813, - "heading": 0.000780854270914727, - "angularVelocity": -0.01117596423803359, - "velocityX": -0.8560455486074661, - "velocityY": 0.2796637956651817, - "timestamp": 9.74890037571808 - }, - { - "x": 2.677973353536199, - "y": 5.538025975345556, - "heading": 0.00026208876345230333, - "angularVelocity": -0.007539415626724292, - "velocityX": -0.5712265204096013, - "velocityY": 0.1848272894652728, - "timestamp": 9.817707499923252 + "heading": 0.013676554706226966, + "angularVelocity": -0.011153132697846535, + "velocityX": -3.331021112692202, + "velocityY": 1.7234990981340637, + "timestamp": 9.195066547526054 + }, + { + "x": 3.942209538282738, + "y": 5.088292732030669, + "heading": 0.012204358572730108, + "angularVelocity": -0.02138935040268071, + "velocityX": -3.2732028891433576, + "velocityY": 1.4288410708686299, + "timestamp": 9.263895011494382 + }, + { + "x": 3.7316526106102703, + "y": 5.172141912516415, + "heading": 0.010528895655226616, + "angularVelocity": -0.024342587657839698, + "velocityX": -3.05915482538382, + "velocityY": 1.2182340800795661, + "timestamp": 9.33272347546271 + }, + { + "x": 3.53847572791013, + "y": 5.244805689896845, + "heading": 0.00883705460181792, + "angularVelocity": -0.024580543511579803, + "velocityX": -2.806642362221417, + "velocityY": 1.0557227808231526, + "timestamp": 9.40155193943104 + }, + { + "x": 3.3636567049982276, + "y": 5.307973779066906, + "heading": 0.0072129294254924, + "angularVelocity": -0.0235967081449452, + "velocityX": -2.5399233519484916, + "velocityY": 0.9177611343924164, + "timestamp": 9.470380403399368 + }, + { + "x": 3.2076759026568156, + "y": 5.362638919945403, + "heading": 0.005703178674060931, + "angularVelocity": -0.02193497667078823, + "velocityX": -2.2662252409582635, + "velocityY": 0.7942228799941261, + "timestamp": 9.539208867367696 + }, + { + "x": 3.070812323282922, + "y": 5.409448485662979, + "heading": 0.00433725990535669, + "angularVelocity": -0.01984526008502491, + "velocityX": -1.9884735396226798, + "velocityY": 0.6800902274837387, + "timestamp": 9.608037331336025 + }, + { + "x": 2.953246241272587, + "y": 5.448856394346129, + "heading": 0.0031354038045633293, + "angularVelocity": -0.01746161444698809, + "velocityX": -1.7081026545127194, + "velocityY": 0.572552493707886, + "timestamp": 9.676865795304353 + }, + { + "x": 2.855102952665587, + "y": 5.48119799246298, + "heading": 0.002112343078266857, + "angularVelocity": -0.014863919188538589, + "velocityX": -1.4259113591749097, + "velocityY": 0.4698869661210702, + "timestamp": 9.745694259272682 + }, + { + "x": 2.7764742584987756, + "y": 5.506730918230691, + "heading": 0.0012792785012801142, + "angularVelocity": -0.012103489297248894, + "velocityX": -1.1423862982470774, + "velocityY": 0.3709646314271869, + "timestamp": 9.81452272324101 + }, + { + "x": 2.717430148167725, + "y": 5.5256592019268735, + "heading": 0.0006450106297669135, + "angularVelocity": -0.009215197244632127, + "velocityX": -0.8578443702916283, + "velocityY": 0.275006626689982, + "timestamp": 9.883351187209339 + }, + { + "x": 2.6780256614742735, + "y": 5.538148367640626, + "heading": 0.00021663624249770164, + "angularVelocity": -0.006223796995765153, + "velocityX": -0.5725027760547413, + "velocityY": 0.18145350039337144, + "timestamp": 9.952179651177667 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -9.768710288964573e-20, - "angularVelocity": -0.003809035277666901, - "velocityX": -0.2858451884385741, - "velocityY": 0.09169675411867828, - "timestamp": 9.886514624128424 + "heading": -3.0114656219279604e-34, + "angularVelocity": -0.003147480417366101, + "velocityX": -0.28651653960332735, + "velocityY": 0.08989010211866776, + "timestamp": 10.021008115145996 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -4.776521402733554e-20, - "angularVelocity": 3.1435024538818593e-20, - "velocityX": 5.229513890071112e-19, - "velocityY": 2.1429873638516558e-18, - "timestamp": 9.955321748333596 - }, - { - "x": 2.6751665057839835, - "y": 5.538944468629318, - "heading": -4.314568505907974e-18, - "angularVelocity": -6.69844600274702e-17, - "velocityX": 0.2647058041795454, - "velocityY": -0.08463157955674895, - "timestamp": 10.01902015375538 - }, - { - "x": 2.7088624712986666, - "y": 5.528079837461981, - "heading": -9.548636187746045e-18, - "angularVelocity": -8.216952445023882e-17, - "velocityX": 0.5289922925316966, - "velocityY": -0.1705636286402427, - "timestamp": 10.082718559177163 - }, - { - "x": 2.7593613029745305, - "y": 5.511644735540107, - "heading": -1.3305788264629273e-17, - "angularVelocity": -5.898345573823082e-17, - "velocityX": 0.7927801542516099, - "velocityY": -0.25801433824046893, - "timestamp": 10.146416964598947 - }, - { - "x": 2.8266246280660523, - "y": 5.489524723354373, - "heading": -1.7929345665528888e-17, - "angularVelocity": -7.258513569049044e-17, - "velocityX": 1.0559656030026554, - "velocityY": -0.34726163142175154, - "timestamp": 10.21011537002073 - }, - { - "x": 2.9106051995536752, - "y": 5.461582331644453, - "heading": -2.2921460737278194e-17, - "angularVelocity": -7.837111523596277e-17, - "velocityX": 1.31840932173323, - "velocityY": -0.43866705178720505, - "timestamp": 10.273813775442514 - }, - { - "x": 3.0112434875257597, - "y": 5.427649372766817, - "heading": -2.8632725416108676e-17, - "angularVelocity": -8.966103061640699e-17, - "velocityX": 1.5799184815647955, - "velocityY": -0.5327128466237959, - "timestamp": 10.337512180864298 - }, - { - "x": 3.1284623112048178, - "y": 5.387515427846554, - "heading": -3.496086628632705e-17, - "angularVelocity": -9.93453576777077e-17, - "velocityX": 1.8402159819054262, - "velocityY": -0.6300620031932188, - "timestamp": 10.401210586286082 - }, - { - "x": 3.2621579049769083, - "y": 5.340909820689256, - "heading": -4.1773207656590006e-17, - "angularVelocity": -1.0694681169901965e-16, - "velocityX": 2.0988844679362666, - "velocityY": -0.7316604999559271, - "timestamp": 10.464908991707865 - }, - { - "x": 3.4121839694430376, - "y": 5.287471786804592, - "heading": -4.8890897323491556e-17, - "angularVelocity": -1.1174046853574415e-16, - "velocityX": 2.3552562026116513, - "velocityY": -0.8389226312781303, - "timestamp": 10.52860739712965 - }, - { - "x": 3.5783204640757695, - "y": 5.226697519809261, - "heading": -5.788296277144039e-17, - "angularVelocity": -1.4116625664827146e-16, - "velocityX": 2.608173525422577, - "velocityY": -0.954094008992977, - "timestamp": 10.592305802551433 - }, - { - "x": 3.76020443223388, - "y": 5.157837260218167, - "heading": -6.653949543666233e-17, - "angularVelocity": -1.3589873416470021e-16, - "velocityX": 2.85539279914075, - "velocityY": -1.0810358459545435, - "timestamp": 10.656004207973217 - }, - { - "x": 3.9571467965572817, - "y": 5.079670509622146, - "heading": -7.420373602789392e-17, - "angularVelocity": -1.2032076062800333e-16, - "velocityX": 3.0917942610859868, - "velocityY": -1.2271382631705205, - "timestamp": 10.719702613395 + "heading": -4.388516089708894e-34, + "angularVelocity": -2.000698896485297e-33, + "velocityX": 9.851967688482278e-36, + "velocityY": -1.337407020908214e-34, + "timestamp": 10.089836579114325 + }, + { + "x": 2.6750735555579293, + "y": 5.538716972759925, + "heading": 4.633169711684461e-19, + "angularVelocity": 7.277213797999712e-18, + "velocityX": 0.2633772294028884, + "velocityY": -0.0882468077495622, + "timestamp": 10.15350338677302 + }, + { + "x": 2.7085920650608806, + "y": 5.52742596598787, + "heading": -4.184429803904338e-20, + "angularVelocity": -7.934452625857426e-18, + "velocityX": 0.5264675697678677, + "velocityY": -0.1773452633683705, + "timestamp": 10.217170194431715 + }, + { + "x": 2.7588390755328, + "y": 5.51039889888726, + "heading": 3.955178903113731e-20, + "angularVelocity": 1.278469740567629e-18, + "velocityX": 0.7892183120172118, + "velocityY": -0.2674402522565472, + "timestamp": 10.28083700209041 + }, + { + "x": 2.8257885982701296, + "y": 5.487560532002736, + "heading": 8.970064045939625e-19, + "angularVelocity": 1.3467843717867433e-17, + "velocityX": 1.0515608556382, + "velocityY": -0.3587170100777762, + "timestamp": 10.344503809749105 + }, + { + "x": 2.9094088162269203, + "y": 5.4588202139112365, + "heading": 2.18904914594727e-18, + "angularVelocity": 2.0293820106069983e-17, + "velocityX": 1.313403656188678, + "velocityY": -0.4514176090871547, + "timestamp": 10.4081706174078 + }, + { + "x": 3.0096598962036025, + "y": 5.424066617313393, + "heading": 3.904306808877207e-18, + "angularVelocity": 2.694116017446798e-17, + "velocityX": 1.5746208057754196, + "velocityY": -0.5458668005493709, + "timestamp": 10.471837425066495 + }, + { + "x": 3.126490562172417, + "y": 5.383159777949491, + "heading": 4.8450172295989666e-18, + "angularVelocity": 1.4775523625508697e-17, + "velocityX": 1.8350325745107399, + "velocityY": -0.6425143786570184, + "timestamp": 10.53550423272519 + }, + { + "x": 3.2598324200617776, + "y": 5.335918462965805, + "heading": 4.235614277481238e-18, + "angularVelocity": -9.5717529200556e-18, + "velocityX": 2.0943700931917317, + "velocityY": -0.7420085397863493, + "timestamp": 10.599171040383885 + }, + { + "x": 3.4095898607657253, + "y": 5.282098877479993, + "heading": 2.3874123899446895e-18, + "angularVelocity": -2.902928473254694e-17, + "velocityX": 2.3522059014921575, + "velocityY": -0.8453319314253612, + "timestamp": 10.66283784804258 + }, + { + "x": 3.5756203018815462, + "y": 5.22135582426949, + "heading": 2.5233707069051096e-18, + "angularVelocity": 2.135466217958737e-18, + "velocityX": 2.607802200573299, + "velocityY": -0.9540772569615117, + "timestamp": 10.726504655701275 + }, + { + "x": 3.757690030648967, + "y": 5.153163870724839, + "heading": 3.693577308512614e-18, + "angularVelocity": 1.8380167698697197e-17, + "velocityX": 2.8597276267323055, + "velocityY": -1.0710754324327665, + "timestamp": 10.79017146335997 + }, + { + "x": 3.955353841452516, + "y": 5.076631094268888, + "heading": 4.677490606421779e-18, + "angularVelocity": 1.5454101345613767e-17, + "velocityX": 3.1046603100187773, + "velocityY": -1.2020828320186725, + "timestamp": 10.853838271018665 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -8.561193314262968e-17, - "angularVelocity": -1.7909705963758273e-16, - "velocityX": 3.3023160854502294, - "velocityY": -1.4085550989874924, - "timestamp": 10.783401018816784 - }, - { - "x": 4.2842075611264425, - "y": 4.936417577245955, - "heading": -9.453694204366925e-17, - "angularVelocity": -2.5755488045645486e-16, - "velocityX": 3.3679341946759767, - "velocityY": -1.5447568972758807, - "timestamp": 10.81805386206763 - }, - { - "x": 4.401356584826297, - "y": 4.8776669667131465, - "heading": -1.0560165699964911e-16, - "angularVelocity": -3.1930179223080245e-16, - "velocityX": 3.380646801528862, - "velocityY": -1.6954051968412056, - "timestamp": 10.852706705318477 - }, - { - "x": 4.516063976001339, - "y": 4.814281195629902, - "heading": -9.662380875053523e-17, - "angularVelocity": 2.590797004510181e-16, - "velocityX": 3.3101869980680285, - "velocityY": -1.8291650882556678, - "timestamp": 10.887359548569323 - }, - { - "x": 4.628146410125643, - "y": 4.746361612927903, - "heading": -9.053238794887956e-17, - "angularVelocity": 1.7578415593930746e-16, - "velocityX": 3.234436877602137, - "velocityY": -1.9600002865663488, - "timestamp": 10.92201239182017 - }, - { - "x": 4.737424855337869, - "y": 4.674016859733605, - "heading": -7.951670404623386e-17, - "angularVelocity": 3.1788687072504797e-16, - "velocityX": 3.1535203163901047, - "velocityY": -2.087700356089278, - "timestamp": 10.956665235071016 - }, - { - "x": 4.844483096398324, - "y": 4.598425359059355, - "heading": -7.540763427184281e-17, - "angularVelocity": 1.1857814219565296e-16, - "velocityX": 3.089450417833756, - "velocityY": -2.181393893916722, - "timestamp": 10.991318078321862 - }, - { - "x": 4.954476829355994, - "y": 4.5271728465414585, - "heading": -1.0264313607639127e-16, - "angularVelocity": -7.859528757078724e-16, - "velocityX": 3.174161847599147, - "velocityY": -2.0561808450207573, - "timestamp": 11.025970921572709 - }, - { - "x": 5.06723038345343, - "y": 4.460373312794279, - "heading": -9.620613179989556e-17, - "angularVelocity": 1.8575688667134074e-16, - "velocityX": 3.25380383021476, - "velocityY": -1.9276782936288397, - "timestamp": 11.060623764823555 - }, - { - "x": 5.18256360847748, - "y": 4.398133545147613, - "heading": -8.10849749390595e-17, - "angularVelocity": 4.36361217215796e-16, - "velocityX": 3.3282470990668904, - "velocityY": -1.796094109684511, - "timestamp": 11.095276608074402 - }, - { - "x": 5.300292214676822, - "y": 4.340553023175948, - "heading": -6.741688231750934e-17, - "angularVelocity": 3.9442918212224324e-16, - "velocityX": 3.397372196766743, - "velocityY": -1.6616391779124173, - "timestamp": 11.129929451325248 - }, - { - "x": 5.4202280776683445, - "y": 4.287723772710641, - "heading": -7.654370000647077e-17, - "angularVelocity": -2.6337861002637873e-16, - "velocityX": 3.461068464810413, - "velocityY": -1.5245285959043562, - "timestamp": 11.164582294576094 - }, - { - "x": 5.542179542504675, - "y": 4.239730223009142, - "heading": -8.402477364947206e-17, - "angularVelocity": -2.1588628640817297e-16, - "velocityX": 3.5192340193716007, - "velocityY": -1.3849815830141687, - "timestamp": 11.19923513782694 + "heading": 5.777516683198087e-18, + "angularVelocity": 1.7277858231456074e-17, + "velocityX": 3.332116557247906, + "velocityY": -1.36151476081006, + "timestamp": 10.91750507867736 + }, + { + "x": 4.285701077560922, + "y": 4.938388492372474, + "heading": 6.5114072312405955e-18, + "angularVelocity": 2.119278869815072e-17, + "velocityX": 3.413356765694629, + "velocityY": -1.4888942599373958, + "timestamp": 10.952134336699762 + }, + { + "x": 4.403715149774036, + "y": 4.881600772971698, + "heading": 6.4908057437571035e-18, + "angularVelocity": -5.949156482118572e-19, + "velocityX": 3.40792956455403, + "velocityY": -1.6398768741750078, + "timestamp": 10.986763594722165 + }, + { + "x": 4.519367006147829, + "y": 4.8201449517293415, + "heading": 6.136184222547225e-18, + "angularVelocity": -1.0240517454358069e-17, + "velocityX": 3.3397151131270637, + "velocityY": -1.7746791225673975, + "timestamp": 11.021392852744567 + }, + { + "x": 4.6324721141561005, + "y": 4.754119165100519, + "heading": 6.684498529619524e-18, + "angularVelocity": 1.5833845088958816e-17, + "velocityX": 3.2661718577713303, + "velocityY": -1.906647453610145, + "timestamp": 11.05602211076697 + }, + { + "x": 4.742850082889288, + "y": 4.683628924272633, + "heading": 1.0669160659402455e-17, + "angularVelocity": 1.1506634439597858e-16, + "velocityX": 3.187419397256002, + "velocityY": -2.03556890483432, + "timestamp": 11.090651368789372 + }, + { + "x": 4.850325220314879, + "y": 4.608787325463071, + "heading": 1.658478005292643e-17, + "angularVelocity": 1.708272059914504e-16, + "velocityX": 3.1035934225348787, + "velocityY": -2.161224440937961, + "timestamp": 11.125280626811774 + }, + { + "x": 4.959315849281407, + "y": 4.536170434444089, + "heading": 2.4155748869411193e-17, + "angularVelocity": 2.1862925308959841e-16, + "velocityX": 3.1473567494868284, + "velocityY": -2.0969808527807587, + "timestamp": 11.159909884834176 + }, + { + "x": 5.0711194796065495, + "y": 4.467963995966004, + "heading": 2.3436163975709297e-17, + "angularVelocity": -2.0779679808224236e-17, + "velocityX": 3.228588676454313, + "velocityY": -1.969618824462286, + "timestamp": 11.194539142856579 + }, + { + "x": 5.1855579490393096, + "y": 4.4042772106470105, + "heading": 2.204718666214906e-17, + "angularVelocity": -4.0109935727230666e-17, + "velocityX": 3.3046757559381796, + "velocityY": -1.8391033754691897, + "timestamp": 11.229168400878981 + }, + { + "x": 5.302448702578612, + "y": 4.345211810292891, + "heading": 2.1117777344899902e-17, + "angularVelocity": -2.6838845829382515e-17, + "velocityX": 3.375491137109673, + "velocityY": -1.7056501850518673, + "timestamp": 11.263797658901384 + }, + { + "x": 5.421605230513571, + "y": 4.290862095789443, + "heading": 1.9153982489912048e-17, + "angularVelocity": -5.670912306921066e-17, + "velocityX": 3.440920618566971, + "velocityY": -1.569473838229202, + "timestamp": 11.298426916923786 + }, + { + "x": 5.542837392691082, + "y": 4.241314822773421, + "heading": 1.5300631909411777e-17, + "angularVelocity": -1.112744193943597e-16, + "velocityX": 3.5008593628856843, + "velocityY": -1.430792221536174, + "timestamp": 11.333056174946188 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -9.639099903602133e-17, - "angularVelocity": -3.5686033890323503e-16, - "velocityX": 3.5717757824418066, - "velocityY": -1.2432211735943417, - "timestamp": 11.233887981077787 - }, - { - "x": 5.93683892832179, - "y": 4.127579524293423, - "heading": -7.782590018599437e-17, - "angularVelocity": 2.51158487397316e-16, - "velocityX": 3.664705468644969, - "velocityY": -0.9344094480089544, - "timestamp": 11.307805844823916 - }, - { - "x": 6.212625572925941, - "y": 4.081838891930783, - "heading": -5.4414649647687885e-17, - "angularVelocity": 3.167197934552236e-16, - "velocityX": 3.7309877562390543, - "velocityY": -0.6188034941017393, - "timestamp": 11.381723708570044 - }, - { - "x": 6.491306353239059, - "y": 4.059759739930169, - "heading": -8.220332638334233e-17, - "angularVelocity": -3.7593993288383394e-16, - "velocityX": 3.7701411565443874, - "velocityY": -0.2986984590956788, - "timestamp": 11.455641572316173 - }, - { - "x": 6.752875972710037, - "y": 4.054277278014819, - "heading": -7.798662141044023e-17, - "angularVelocity": 5.704581760375439e-17, - "velocityX": 3.5386523123739373, - "velocityY": -0.07416964773468374, - "timestamp": 11.529559436062302 - }, - { - "x": 6.9910920962877245, - "y": 4.053575874325361, - "heading": -6.860310501240552e-17, - "angularVelocity": 1.269451783709716e-16, - "velocityX": 3.2227138543376115, - "velocityY": -0.00948896050171146, - "timestamp": 11.60347729980843 - }, - { - "x": 7.205567187241113, - "y": 4.055021140911149, - "heading": -5.885294926916e-17, - "angularVelocity": 1.3190526956719489e-16, - "velocityX": 2.901532594204896, - "velocityY": 0.019552331635990624, - "timestamp": 11.677395163554559 - }, - { - "x": 7.396227777452995, - "y": 4.057522153925315, - "heading": -4.8982438983156224e-17, - "angularVelocity": 1.3353348954892513e-16, - "velocityX": 2.5793574184815227, - "velocityY": 0.033835028333003926, - "timestamp": 11.751313027300688 - }, - { - "x": 7.563054971618611, - "y": 4.060486200727262, - "heading": -3.912779424726568e-17, - "angularVelocity": 1.333188519862415e-16, - "velocityX": 2.256926617070343, - "velocityY": 0.04009919458884174, - "timestamp": 11.825230891046816 - }, - { - "x": 7.70604444565388, - "y": 4.063541585038157, - "heading": -2.9177079820924904e-17, - "angularVelocity": 1.3461853362720707e-16, - "velocityX": 1.9344373171601388, - "velocityY": 0.041334856772760636, - "timestamp": 11.899148754792945 - }, - { - "x": 7.825196583277639, - "y": 4.066433602906459, - "heading": -1.9253434148047247e-17, - "angularVelocity": 1.3425233319755054e-16, - "velocityX": 1.6119532084015245, - "velocityY": 0.03912474903541938, - "timestamp": 11.973066618539074 - }, - { - "x": 7.920513375065405, - "y": 4.0689768762979055, - "heading": -9.302606124041008e-18, - "angularVelocity": 1.346200704365749e-16, - "velocityX": 1.2894960292025357, - "velocityY": 0.034406749093584967, - "timestamp": 12.046984482285202 - }, - { - "x": 7.991997299424133, - "y": 4.071030463180752, - "heading": 8.179153045928767e-19, - "angularVelocity": 1.369157726664237e-16, - "velocityX": 0.9670723792051265, - "velocityY": 0.027782010717991884, - "timestamp": 12.12090234603133 - }, - { - "x": 8.03965088919926, - "y": 4.072483600942051, - "heading": 2.229877553000587e-19, - "angularVelocity": -8.048494898574051e-18, - "velocityX": 0.6446829948819026, - "velocityY": 0.019658817066051014, - "timestamp": 12.19482020977746 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 3.803306693773524e-29, - "angularVelocity": -3.0166964241453683e-18, - "velocityX": 0.32232632402054306, - "velocityY": 0.01032706968038683, - "timestamp": 12.268738073523588 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 1.8619056951323718e-29, - "angularVelocity": -1.0754545583434217e-29, - "velocityX": -2.9997098934675086e-25, - "velocityY": 4.422144616548637e-24, - "timestamp": 12.342655937269717 + "heading": 9.183754863330666e-18, + "angularVelocity": -1.766389866662468e-16, + "velocityX": 3.555211493416193, + "velocityY": -1.2898268911821011, + "timestamp": 11.36768543296859 + }, + { + "x": 5.934272395491136, + "y": 4.124337934678792, + "heading": -1.4789774171207362e-18, + "angularVelocity": -1.4511299214964166e-16, + "velocityX": 3.651673302114514, + "velocityY": -0.9841085377661543, + "timestamp": 11.441164258830412 + }, + { + "x": 6.207753017493673, + "y": 4.075010140740357, + "heading": -1.5184517487424443e-17, + "angularVelocity": -1.8652366732257328e-16, + "velocityX": 3.7218970063133985, + "velocityY": -0.6713198443208211, + "timestamp": 11.514643084692233 + }, + { + "x": 6.4844286250858945, + "y": 4.049019985828209, + "heading": -2.210208594102609e-17, + "angularVelocity": -9.414369884747846e-17, + "velocityX": 3.7653787243758563, + "velocityY": -0.35370944768528717, + "timestamp": 11.588121910554054 + }, + { + "x": 6.743277363747609, + "y": 4.038426394199003, + "heading": -1.998169117841033e-17, + "angularVelocity": 2.885722162467868e-17, + "velocityX": 3.522766397335807, + "velocityY": -0.14417203194192602, + "timestamp": 11.661600736415876 + }, + { + "x": 6.978672822837657, + "y": 4.030025299692623, + "heading": -1.7754526303263836e-17, + "angularVelocity": 3.031029482336482e-17, + "velocityX": 3.2035822065626665, + "velocityY": -0.1143335431376982, + "timestamp": 11.735079562277697 + }, + { + "x": 7.19055269256627, + "y": 4.022995759457438, + "heading": -1.4995601737739213e-17, + "angularVelocity": 3.754720537740805e-17, + "velocityX": 2.8835500192539434, + "velocityY": -0.095667563447506, + "timestamp": 11.808558388139518 + }, + { + "x": 7.3789017577018114, + "y": 4.017045849522277, + "heading": -1.2218093470845859e-17, + "angularVelocity": 3.780011771168623e-17, + "velocityX": 2.5633107623376494, + "velocityY": -0.08097448299391073, + "timestamp": 11.88203721400134 + }, + { + "x": 7.54371362899692, + "y": 4.012025997027571, + "heading": -9.378231648873842e-18, + "angularVelocity": 3.864870986523972e-17, + "velocityX": 2.2429845518359373, + "velocityY": -0.06831699385270533, + "timestamp": 11.955516039863161 + }, + { + "x": 7.684984887810538, + "y": 4.007845278835319, + "heading": -6.610589160747635e-18, + "angularVelocity": 3.7665850749041794e-17, + "velocityX": 1.9226118158077508, + "velocityY": -0.05689691068436128, + "timestamp": 12.028994865724982 + }, + { + "x": 7.8027134339680275, + "y": 4.0044425825275765, + "heading": -4.4053913927362146e-18, + "angularVelocity": 3.0011336492479294e-17, + "velocityX": 1.6022104977409417, + "velocityY": -0.046308528584020826, + "timestamp": 12.102473691586804 + }, + { + "x": 7.896897856926731, + "y": 4.001774011196325, + "heading": -2.659547727350374e-18, + "angularVelocity": 2.375981985162547e-17, + "velocityX": 1.281789983087365, + "velocityY": -0.03631755543113615, + "timestamp": 12.175952517448625 + }, + { + "x": 7.967537148527336, + "y": 3.999806507404438, + "heading": -1.3934136908294963e-18, + "angularVelocity": 1.723127746899314e-17, + "velocityX": 0.9613557480279207, + "velocityY": -0.026776472933672534, + "timestamp": 12.249431343310446 + }, + { + "x": 8.014630554462236, + "y": 3.9985142796609896, + "heading": -4.425448526284405e-19, + "angularVelocity": 1.2940719003719293e-17, + "velocityX": 0.6409112473226126, + "velocityY": -0.017586396193627105, + "timestamp": 12.322910169172268 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 3.3992773241588707e-43, + "angularVelocity": 6.022753459080235e-18, + "velocityX": 0.320458791984776, + "velocityY": -0.008677813220192955, + "timestamp": 12.396388995034089 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": -7.143469642081013e-44, + "angularVelocity": -5.598380381353948e-42, + "velocityX": -5.65394748040853e-41, + "velocityY": -2.108446218115532e-39, + "timestamp": 12.46986782089591 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj index 9cc6f3e6..2c19ea6b 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj @@ -3,326 +3,308 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 2.598230881890789e-31, - "angularVelocity": 0, - "velocityX": -2.3904076278416207e-31, - "velocityY": 5.789752915577876e-30, + "heading": 0, + "angularVelocity": 2.3560143253921187e-30, + "velocityX": 0, + "velocityY": 0, "timestamp": 0 }, { - "x": 1.3043755674296562, - "y": 5.57443357397875, - "heading": -0.009496998569604356, - "angularVelocity": -0.1264048181794536, - "velocityX": 0.191678505815799, - "velocityY": -0.2198905081992682, - "timestamp": 0.07513161845108943 - }, - { - "x": 1.3331814181481576, - "y": 5.541395353902405, - "heading": -0.028483550113124877, - "angularVelocity": -0.2527105356630103, - "velocityX": 0.38340516698036353, - "velocityY": -0.4397378994111958, - "timestamp": 0.15026323690217885 - }, - { - "x": 1.376396862320767, - "y": 5.49184373452604, - "heading": -0.056940765199465976, - "angularVelocity": -0.378764835281948, - "velocityX": 0.5751965026490216, - "velocityY": -0.6595308393324731, - "timestamp": 0.22539485535326828 - }, - { - "x": 1.4340285202431444, - "y": 5.425784148354236, - "heading": -0.09483975957151446, - "angularVelocity": -0.504434686147033, - "velocityX": 0.7670759542900392, - "velocityY": -0.8792514727514934, - "timestamp": 0.3005264738043577 - }, - { - "x": 1.5060856912258154, - "y": 5.34322422127702, - "heading": -0.1421437039065203, - "angularVelocity": -0.6296143396295107, - "velocityX": 0.9590791795253663, - "velocityY": -1.0988706057618698, - "timestamp": 0.3756580922554471 - }, - { - "x": 1.5925821714791422, - "y": 5.244175418310136, - "heading": -0.1988101671423871, - "angularVelocity": -0.7542292367162176, - "velocityX": 1.1512660319811892, - "velocityY": -1.318337139694803, - "timestamp": 0.4507897107065365 - }, - { - "x": 1.6935404980129676, - "y": 5.128656797964587, - "heading": -0.26479264404095926, - "angularVelocity": -0.8782251502241449, - "velocityX": 1.343752851483489, - "velocityY": -1.5375500053152549, - "timestamp": 0.5259213291576259 - }, - { - "x": 1.80900475066288, - "y": 4.99670620601025, - "heading": -0.34003720346134453, - "angularVelocity": -1.001503241571098, - "velocityX": 1.5368263725127875, - "velocityY": -1.7562591446699305, - "timestamp": 0.6010529476087153 - }, - { - "x": 1.9391049044022353, - "y": 4.8484368766348975, - "heading": -0.42444837010558617, - "angularVelocity": -1.1235105590497523, - "velocityX": 1.7316298572389015, - "velocityY": -1.9734611397719364, - "timestamp": 0.6761845660598047 - }, - { - "x": 2.0768333663443252, - "y": 4.720578669009404, - "heading": -0.4949304356074761, - "angularVelocity": -0.938114564170018, - "velocityX": 1.8331624525981682, - "velocityY": -1.7017896094422644, - "timestamp": 0.7513161845108941 - }, - { - "x": 2.2004381051303543, - "y": 4.609474436663166, - "heading": -0.5559710621989035, - "angularVelocity": -0.8124492437076775, - "velocityX": 1.6451760435605478, - "velocityY": -1.4787946091584414, - "timestamp": 0.8264478029619835 - }, - { - "x": 2.309790549713842, - "y": 4.515017160876509, - "heading": -0.6076423779450478, - "angularVelocity": -0.687743946017975, - "velocityX": 1.4554783570583396, - "velocityY": -1.2572240253997977, - "timestamp": 0.9015794214130729 - }, - { - "x": 2.404847575381414, - "y": 4.437170463185578, - "heading": -0.6499678697321144, - "angularVelocity": -0.5633512582056228, - "velocityX": 1.2652066816823961, - "velocityY": -1.0361376380998464, - "timestamp": 0.9767110398641623 - }, - { - "x": 2.4855875270959125, - "y": 4.375915949453414, - "heading": -0.6829583691226987, - "angularVelocity": -0.43910273822475193, - "velocityX": 1.0746467782947702, - "velocityY": -0.8152960763101967, - "timestamp": 1.0518426583152518 - }, - { - "x": 2.5519973911227014, - "y": 4.331242517358978, - "heading": -0.7066196510488874, - "angularVelocity": -0.31493108244769824, - "velocityX": 0.8839136624193856, - "velocityY": -0.5946022861403544, - "timestamp": 1.1269742767663413 - }, - { - "x": 2.604068513564513, - "y": 4.303142764963195, - "heading": -0.7209550318751375, - "angularVelocity": -0.1908035673998086, - "velocityX": 0.693065363384342, - "velocityY": -0.3740070155974736, - "timestamp": 1.2021058952174308 - }, - { - "x": 2.6417947649723117, - "y": 4.2916114443192095, - "heading": -0.7259664734930259, - "angularVelocity": -0.06670216508038034, - "velocityX": 0.5021354815334699, - "velocityY": -0.15348159509809242, - "timestamp": 1.2772375136685203 + "x": 1.3060574270397207, + "y": 5.572382522338357, + "heading": -0.010694879272561806, + "angularVelocity": -0.14212355715817127, + "velocityX": 0.21372562484439114, + "velocityY": -0.24679919880328743, + "timestamp": 0.07525057412377358 + }, + { + "x": 1.3382235764672503, + "y": 5.535239265365289, + "heading": -0.0320761034546414, + "angularVelocity": -0.2841337017193578, + "velocityX": 0.42745387396382534, + "velocityY": -0.49359433341007053, + "timestamp": 0.15050114824754715 + }, + { + "x": 1.3864734146735522, + "y": 5.47952484840506, + "heading": -0.06412157549775398, + "angularVelocity": -0.42585020003622126, + "velocityX": 0.6411889712155109, + "velocityY": -0.7403852742575773, + "timestamp": 0.22575172237132074 + }, + { + "x": 1.4508078083146234, + "y": 5.405239688642535, + "heading": -0.10679814197023921, + "angularVelocity": -0.5671261245497303, + "velocityX": 0.8549355854022526, + "velocityY": -0.9871706711626929, + "timestamp": 0.3010022964950943 + }, + { + "x": 1.5312280128772082, + "y": 5.3123844280164345, + "heading": -0.16006528510741636, + "angularVelocity": -0.7078636111159992, + "velocityX": 1.0686988836704838, + "velocityY": -1.2339475373861764, + "timestamp": 0.37625287061886786 + }, + { + "x": 1.6277357374217547, + "y": 5.200960102506698, + "heading": -0.22387989152223717, + "angularVelocity": -0.8480281666778853, + "velocityX": 1.2824848935314492, + "velocityY": -1.4807106365400295, + "timestamp": 0.4515034447426414 + }, + { + "x": 1.7403333706916029, + "y": 5.070968456330507, + "heading": -0.29820152963685287, + "angularVelocity": -0.9876554296244249, + "velocityX": 1.496302647246288, + "velocityY": -1.7274505569061047, + "timestamp": 0.526754018866415 + }, + { + "x": 1.8690253649035526, + "y": 4.922413220845796, + "heading": -0.38299660252732504, + "angularVelocity": -1.1268362252301867, + "velocityX": 1.7101795661770514, + "velocityY": -1.9741408915022984, + "timestamp": 0.6020045929901886 + }, + { + "x": 2.021841436297539, + "y": 4.7786139697645345, + "heading": -0.46243583568438273, + "angularVelocity": -1.055662818268618, + "velocityX": 2.0307628636084356, + "velocityY": -1.9109389231128335, + "timestamp": 0.6772551671139622 + }, + { + "x": 2.158572983780592, + "y": 4.6533861744648695, + "heading": -0.5314192950572162, + "angularVelocity": -0.9167167184470773, + "velocityX": 1.8170166683292757, + "velocityY": -1.6641440514810064, + "timestamp": 0.7525057412377357 + }, + { + "x": 2.2792163174605955, + "y": 4.5467264938443535, + "heading": -0.5899522434559231, + "angularVelocity": -0.777840555764763, + "velocityX": 1.6032214383174732, + "velocityY": -1.4173935795391805, + "timestamp": 0.8277563153615093 + }, + { + "x": 2.3837699374210346, + "y": 4.458633436191598, + "heading": -0.6380354100231267, + "angularVelocity": -0.6389740826105205, + "velocityX": 1.3894062760138433, + "velocityY": -1.1706629308520584, + "timestamp": 0.9030068894852828 + }, + { + "x": 2.472232988532522, + "y": 4.389106089275013, + "heading": -0.6756678010096412, + "angularVelocity": -0.5000944035873048, + "velocityX": 1.1755797499519767, + "velocityY": -0.9239444047507308, + "timestamp": 0.9782574636090564 + }, + { + "x": 2.5446049348789113, + "y": 4.338143851138861, + "heading": -0.7028477866363936, + "angularVelocity": -0.36119306653537947, + "velocityX": 0.9617461021356389, + "velocityY": -0.6772338779998112, + "timestamp": 1.05350803773283 + }, + { + "x": 2.6008854472033804, + "y": 4.305746331042048, + "heading": -0.7195735501934102, + "angularVelocity": -0.222267587342223, + "velocityX": 0.7479080788470353, + "velocityY": -0.43052854378169836, + "timestamp": 1.1287586118566038 + }, + { + "x": 2.6410743549129108, + "y": 4.291913303700176, + "heading": -0.7258432586618085, + "angularVelocity": -0.0833177492798597, + "velocityX": 0.5340677885590666, + "velocityY": -0.18382620335314903, + "timestamp": 1.2040091859803774 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.057384074600866586, - "velocityX": 0.311145410428376, - "velocityY": 0.06699234541878851, - "timestamp": 1.3523691321196099 - }, - { - "x": 2.674170692930372, - "y": 4.3184165536581745, - "heading": -0.7079150841905615, - "angularVelocity": 0.18203686942568226, - "velocityX": 0.1192255306502172, - "velocityY": 0.28844784676769203, - "timestamp": 1.4278485168598245 - }, - { - "x": 2.6686789979466896, - "y": 4.3568998584598715, - "heading": -0.6847718369181163, - "angularVelocity": 0.3066167980947624, - "velocityX": -0.07275754835791173, - "velocityY": 0.5098518613031996, - "timestamp": 1.5033279016000392 - }, - { - "x": 2.648690220373955, - "y": 4.4120894363660135, - "heading": -0.6522316135776112, - "angularVelocity": 0.4311140512244905, - "velocityX": -0.26482433108237524, - "velocityY": 0.7311874373940757, - "timestamp": 1.5788072863402538 - }, - { - "x": 2.614195529876659, - "y": 4.483978019553035, - "heading": -0.6103014306892409, - "angularVelocity": 0.5555183449290733, - "velocityX": -0.4570081038377115, - "velocityY": 0.9524267246290559, - "timestamp": 1.6542866710804685 - }, - { - "x": 2.565181655781624, - "y": 4.572554576780094, - "heading": -0.5589890537241904, - "angularVelocity": 0.6798197566183578, - "velocityX": -0.6493676951067825, - "velocityY": 1.1735198628071262, - "timestamp": 1.729766055820683 - }, - { - "x": 2.5016263830570615, - "y": 4.677800410068182, - "heading": -0.49830336002198045, - "angularVelocity": 0.8040035555369023, - "velocityX": -0.8420216055727019, - "velocityY": 1.3943652779921303, - "timestamp": 1.8052454405608978 - }, - { - "x": 2.4234850482103365, - "y": 4.799677415452469, - "heading": -0.42825793280914776, - "angularVelocity": 0.9280073950309173, - "velocityX": -1.0352672470523452, - "velocityY": 1.6147058670562038, - "timestamp": 1.8807248253011124 - }, - { - "x": 2.3306237160215493, - "y": 4.938069451098362, - "heading": -0.34890039707276027, - "angularVelocity": 1.0513802677046542, - "velocityX": -1.2302873494790678, - "velocityY": 1.8335077335201588, - "timestamp": 1.956204210041327 - }, - { - "x": 2.2158437334233896, - "y": 5.072717873356149, - "heading": -0.27145428242562397, - "angularVelocity": 1.0260565175055525, - "velocityX": -1.5206798914447346, - "velocityY": 1.7839099076626925, - "timestamp": 2.031683594781542 - }, - { - "x": 2.115226856826332, - "y": 5.190380914478299, - "heading": -0.20366940333084457, - "angularVelocity": 0.8980581827763024, - "velocityX": -1.3330378478809115, - "velocityY": 1.5588765267548796, - "timestamp": 2.107162979521757 - }, - { - "x": 2.0289140508238517, - "y": 5.291175476770988, - "heading": -0.14552121115180353, - "angularVelocity": 0.770385084361868, - "velocityX": -1.1435282136547764, - "velocityY": 1.33539194366953, - "timestamp": 2.1826423642619717 - }, - { - "x": 1.956951239598818, - "y": 5.375140495871239, - "heading": -0.09702855670668947, - "angularVelocity": 0.6424622380441027, - "velocityX": -0.953410145970343, - "velocityY": 1.1124232052394887, - "timestamp": 2.2581217490021865 - }, - { - "x": 1.8993610596021686, - "y": 5.4422954705446385, - "heading": -0.05821541752658778, - "angularVelocity": 0.5142217217941352, - "velocityX": -0.7629921758539466, - "velocityY": 0.8897127991440643, - "timestamp": 2.3336011337424014 - }, - { - "x": 1.8561570206808402, - "y": 5.492652117133405, - "heading": -0.029101140919542866, - "angularVelocity": 0.3857248798192106, - "velocityX": -0.5723952184942321, - "velocityY": 0.6671576187743994, - "timestamp": 2.4090805184826163 - }, - { - "x": 1.827348191495371, - "y": 5.5262182202265375, - "heading": -0.009696189627478835, - "angularVelocity": 0.25708942063193824, - "velocityX": -0.3816781136185715, - "velocityY": 0.44470557370534397, - "timestamp": 2.484559903222831 + "angularVelocity": 0.05565596827444861, + "velocityX": 0.3202270361150376, + "velocityY": 0.06287505455011895, + "timestamp": 1.279259760104151 + }, + { + "x": 2.6731093005797457, + "y": 4.320288364477002, + "heading": -0.7067987753568107, + "angularVelocity": 0.19580392065911026, + "velocityX": 0.10461717847139641, + "velocityY": 0.3116194636271141, + "timestamp": 1.3551333161029784 + }, + { + "x": 2.664687962821916, + "y": 4.362805446311989, + "heading": -0.6813130877895294, + "angularVelocity": 0.3358968382514228, + "velocityX": -0.11099173681211887, + "velocityY": 0.5603675915105346, + "timestamp": 1.4310068721018057 + }, + { + "x": 2.6399077578590466, + "y": 4.424196334908807, + "heading": -0.645202192832585, + "angularVelocity": 0.47593518560166614, + "velocityX": -0.32659870276641295, + "velocityY": 0.8091210144040187, + "timestamp": 1.506880428100633 + }, + { + "x": 2.5987689088828363, + "y": 4.504461553370171, + "heading": -0.5984695308975294, + "angularVelocity": 0.6159281889382251, + "velocityX": -0.5422027270940676, + "velocityY": 1.0578813316982019, + "timestamp": 1.5827539840994602 + }, + { + "x": 2.541271709317337, + "y": 4.60360172553535, + "heading": -0.5411163068190713, + "angularVelocity": 0.7559053127670017, + "velocityX": -0.7578028841452508, + "velocityY": 1.3066498710827958, + "timestamp": 1.6586275400982875 + }, + { + "x": 2.4674164806885255, + "y": 4.721617486978178, + "heading": -0.47313850163583826, + "angularVelocity": 0.8959354057929544, + "velocityX": -0.9733988035419044, + "velocityY": 1.5554267872170555, + "timestamp": 1.7345010960971148 + }, + { + "x": 2.377203340708699, + "y": 4.858509175719519, + "heading": -0.39452181655964974, + "angularVelocity": 1.0361539543083473, + "velocityX": -1.188993171512905, + "velocityY": 1.804208158404987, + "timestamp": 1.810374652095942 + }, + { + "x": 2.270630715712783, + "y": 5.014275370945325, + "heading": -0.30523452580590593, + "angularVelocity": 1.1767906430312367, + "velocityX": -1.4046082801148865, + "velocityY": 2.052970803512055, + "timestamp": 1.8862482080947693 + }, + { + "x": 2.156210238547871, + "y": 5.1464603718429265, + "heading": -0.22910381899713708, + "angularVelocity": 1.0033892020445663, + "velocityX": -1.508041578600413, + "velocityY": 1.7421748481347223, + "timestamp": 1.9621217640935966 + }, + { + "x": 2.0581337448378676, + "y": 5.259758714146455, + "heading": -0.16374376524716946, + "angularVelocity": 0.8614339065879271, + "velocityX": -1.2926307778416481, + "velocityY": 1.4932520403643688, + "timestamp": 2.0379953200924237 + }, + { + "x": 1.9764031908936424, + "y": 5.354172852013204, + "heading": -0.10920543384793971, + "angularVelocity": 0.7188055269306859, + "velocityX": -1.077194193248253, + "velocityY": 1.24436157795548, + "timestamp": 2.1138688760912507 + }, + { + "x": 1.9110187094739324, + "y": 5.42970361761196, + "heading": -0.0655341593932664, + "angularVelocity": 0.5755796453747686, + "velocityX": -0.8617558589167408, + "velocityY": 0.995482083384748, + "timestamp": 2.189742432090078 + }, + { + "x": 1.8619802018876241, + "y": 5.486351463195586, + "heading": -0.03276467101544991, + "angularVelocity": 0.4318960400201182, + "velocityX": -0.646318825315813, + "velocityY": 0.7466085494271616, + "timestamp": 2.265615988088905 + }, + { + "x": 1.8292876471115809, + "y": 5.524116647521472, + "heading": -0.010918058172762177, + "angularVelocity": 0.2879344793491674, + "velocityX": -0.43088206879563057, + "velocityY": 0.4977384258464482, + "timestamp": 2.341489544087732 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -6.802256144415086e-28, - "angularVelocity": 0.12846142905206573, - "velocityX": -0.19087328234026563, - "velocityY": 0.22232623397342613, - "timestamp": 2.560039287963046 + "heading": 2.8937989389363637e-30, + "angularVelocity": 0.14389806868982188, + "velocityX": -0.21544335593050407, + "velocityY": 0.24886958055725772, + "timestamp": 2.417363100086559 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -6.934045530869177e-28, - "angularVelocity": -1.67846607717031e-28, - "velocityX": 1.7578194682085736e-30, - "velocityY": -2.585635481662347e-29, - "timestamp": 2.635518672703261 + "heading": 1.8511737998863575e-30, + "angularVelocity": -5.603397953128688e-29, + "velocityX": 0, + "velocityY": 2.843834189498754e-31, + "timestamp": 2.493236656085386 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj index 0b21bd55..e3057310 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj @@ -3,101 +3,92 @@ { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -6.934045530869177e-28, - "angularVelocity": -1.67846607717031e-28, - "velocityX": 1.7578194682085736e-30, - "velocityY": -2.585635481662347e-29, + "heading": 1.8511737998863575e-30, + "angularVelocity": -5.603397953128688e-29, + "velocityX": 0, + "velocityY": 2.843834189498754e-31, "timestamp": 0 }, { - "x": 1.8467557558007974, - "y": 5.543052711491933, - "heading": 1.9216741877895313e-20, - "angularVelocity": 2.064858009548948e-19, - "velocityX": 0.36334081801614104, - "velocityY": 0.0005742601437372874, - "timestamp": 0.09306568528358428 + "x": 1.855209395178807, + "y": 5.543066072468525, + "heading": -4.900273115413656e-27, + "angularVelocity": -4.980371579437602e-26, + "velocityX": 0.42942883009919236, + "velocityY": 0.0006787122433542538, + "timestamp": 0.09842888654778115 }, { - "x": 1.9143848790849516, - "y": 5.543159599317722, - "heading": 3.7322144176989406e-20, - "angularVelocity": 1.945443397736918e-19, - "velocityX": 0.7266816236089446, - "velocityY": 0.0011485202678394907, - "timestamp": 0.18613137056716855 + "x": 1.939745796780934, + "y": 5.543199682246804, + "heading": 4.528417643864408e-20, + "angularVelocity": 4.600700356082414e-19, + "velocityX": 0.8588576440015887, + "velocityY": 0.0013574244611094727, + "timestamp": 0.1968577730955623 }, { - "x": 2.0158285613519538, - "y": 5.543319931052202, - "heading": 3.5705254993467834e-20, - "angularVelocity": -1.7373634318302704e-20, - "velocityX": 1.090022406839742, - "velocityY": 0.0017227803565985438, - "timestamp": 0.27919705585075283 + "x": 2.066550395198543, + "y": 5.5434000969079245, + "heading": 6.339784356887693e-20, + "angularVelocity": 1.8402795932716144e-19, + "velocityX": 1.2882864255103954, + "velocityY": 0.0020361366276666244, + "timestamp": 0.29528665964334344 }, { - "x": 2.151086797745823, - "y": 5.543533706687699, - "heading": 2.4928931377494695e-20, - "angularVelocity": -1.157926638948406e-19, - "velocityX": 1.4533631378925307, - "velocityY": 0.0022970403628902575, - "timestamp": 0.3722627411343371 + "x": 2.2356231808662415, + "y": 5.543667316436768, + "heading": 5.434100687876101e-20, + "angularVelocity": -9.20140114123854e-20, + "velocityX": 1.7177151098384495, + "velocityY": 0.002714848640629602, + "timestamp": 0.3937155461911246 }, { - "x": 2.3201595639866595, - "y": 5.5438009261858365, - "heading": 1.159960983349273e-20, - "angularVelocity": -1.432248793219564e-19, - "velocityX": 1.8167036080554038, - "velocityY": 0.0028712999568454734, - "timestamp": 0.4653284264179214 + "x": 2.4046959665339394, + "y": 5.543934535965611, + "heading": 4.338082196797977e-27, + "angularVelocity": -5.5208388966484585e-19, + "velocityX": 1.7177151098384498, + "velocityY": 0.0027148486406296025, + "timestamp": 0.49214443273890573 }, { - "x": 2.4554178003805283, - "y": 5.544014701821333, - "heading": 2.0604388914513833e-20, - "angularVelocity": 9.675724251840524e-20, - "velocityX": 1.4533631378925305, - "velocityY": 0.0022970403628902575, - "timestamp": 0.5583941117015057 + "x": 2.5315005649515485, + "y": 5.544134950626731, + "heading": -2.717050695881145e-20, + "angularVelocity": -2.760420466983953e-19, + "velocityX": 1.2882864255103954, + "velocityY": 0.0020361366276666244, + "timestamp": 0.5905733192866869 }, { - "x": 2.556861482647531, - "y": 5.544175033555813, - "heading": 2.9920340295234286e-20, - "angularVelocity": 1.0010081967860847e-19, - "velocityX": 1.090022406839742, - "velocityY": 0.001722780356598544, - "timestamp": 0.6514597969850899 - }, - { - "x": 2.624490605931685, - "y": 5.544281921381602, - "heading": 2.1256868508528512e-20, - "angularVelocity": -9.308986185732237e-20, - "velocityX": 0.7266816236089446, - "velocityY": 0.0011485202678394905, - "timestamp": 0.7445254822686742 + "x": 2.6160369665536756, + "y": 5.54426856040501, + "heading": 3.0657579714860263e-27, + "angularVelocity": 2.760420337720658e-19, + "velocityX": 0.8588576440015887, + "velocityY": 0.0013574244611094727, + "timestamp": 0.689002205834468 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": 4.4426104536241024e-33, - "angularVelocity": -2.284071561257056e-19, - "velocityX": 0.36334081801614104, - "velocityY": 0.0005742601437372874, - "timestamp": 0.8375911675522585 + "heading": 1.737489718984482e-38, + "angularVelocity": -3.114693341043594e-26, + "velocityX": 0.42942883009919236, + "velocityY": 0.0006787122433542539, + "timestamp": 0.7874310923822492 }, { "x": 2.6583051681518555, "y": 5.54433536529541, "heading": 0, - "angularVelocity": -4.668434863943491e-32, - "velocityX": -5.247197089772187e-38, - "velocityY": -1.940144776771126e-37, - "timestamp": 0.9306568528358428 + "angularVelocity": -1.1323659927897137e-37, + "velocityX": 0, + "velocityY": 3.593395183653217e-38, + "timestamp": 0.8858599789300303 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj index 9cf0615f..6561888b 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj @@ -4,460 +4,433 @@ "x": 2.6583051681518555, "y": 5.54433536529541, "heading": 0, - "angularVelocity": -4.668434863943491e-32, - "velocityX": -5.247197089772187e-38, - "velocityY": -1.940144776771126e-37, + "angularVelocity": -1.1323659927897137e-37, + "velocityX": 0, + "velocityY": 3.593395183653217e-38, "timestamp": 0 }, { - "x": 2.6759672111325252, - "y": 5.540929157802402, - "heading": -2.5855780446225044e-18, - "angularVelocity": -3.809209550134996e-17, - "velocityX": 0.2602065056082181, - "velocityY": -0.05018204010160352, - "timestamp": 0.06787702305669052 - }, - { - "x": 2.7112094706297696, - "y": 5.533716903598895, - "heading": -4.117772689767123e-18, - "angularVelocity": -2.2573097288976327e-17, - "velocityX": 0.5192075007150861, - "velocityY": -0.10625472182954283, - "timestamp": 0.13575404611338104 - }, - { - "x": 2.763926012188838, - "y": 5.522238566618234, - "heading": -6.491645699921123e-18, - "angularVelocity": -3.4973145598497126e-17, - "velocityX": 0.7766478137239484, - "velocityY": -0.16910489682311705, - "timestamp": 0.20363106917007157 - }, - { - "x": 2.833977839229424, - "y": 5.505960452216563, - "heading": -8.342526928581605e-18, - "angularVelocity": -2.7268155633677806e-17, - "velocityX": 1.0320403560138376, - "velocityY": -0.23981774197838637, - "timestamp": 0.2715080922267621 - }, - { - "x": 2.921179778977005, - "y": 5.484257984516274, - "heading": -1.1098333459478279e-17, - "angularVelocity": -4.0599991083804175e-17, - "velocityX": 1.284704835607638, - "velocityY": -0.31973216742524313, - "timestamp": 0.3393851152834526 - }, - { - "x": 3.025280889014475, - "y": 5.456393698526154, - "heading": -1.451838596617181e-17, - "angularVelocity": -5.0386012124268725e-17, - "velocityX": 1.5336722995427363, - "velocityY": -0.4105113149533374, - "timestamp": 0.40726213834014313 - }, - { - "x": 3.1459346095433047, - "y": 5.4214894529386894, - "heading": -1.4363311491036705e-17, - "angularVelocity": 2.284638720910668e-18, - "velocityX": 1.7775340622711318, - "velocityY": -0.5142277020356774, - "timestamp": 0.47513916139683365 - }, - { - "x": 3.2826525372630417, - "y": 5.378492545637339, - "heading": -1.4597384083135608e-17, - "angularVelocity": -3.4484805248903604e-18, - "velocityX": 2.0142004107273697, - "velocityY": -0.6334530503118778, - "timestamp": 0.5430161844535237 - }, - { - "x": 3.434732199390872, - "y": 5.326137913197681, - "heading": -1.5798470616654896e-17, - "angularVelocity": -1.7695038459712723e-17, - "velocityX": 2.240517560718812, - "velocityY": -0.7713159782498521, - "timestamp": 0.6108932075102138 - }, - { - "x": 3.6011452044637537, - "y": 5.262915661085215, - "heading": -1.3130445812596166e-17, - "angularVelocity": 3.9306744519879925e-17, - "velocityX": 2.4516839068486322, - "velocityY": -0.9314234665192118, - "timestamp": 0.6787702305669039 - }, - { - "x": 3.7803722058519504, - "y": 5.187069964685383, - "heading": -9.049091305666531e-18, - "angularVelocity": 6.012866096854611e-17, - "velocityX": 2.640466439409227, - "velocityY": -1.1173986863932572, - "timestamp": 0.746647253623594 - }, - { - "x": 3.9701912865835975, - "y": 5.0966861229155755, - "heading": -3.2662453848887855e-18, - "angularVelocity": 8.519592728674139e-17, - "velocityX": 2.796514522640622, - "velocityY": -1.3315822895520983, - "timestamp": 0.814524276680284 + "x": 2.6753320649100156, + "y": 5.539342065750356, + "heading": 1.5795430701564016e-18, + "angularVelocity": 2.476798764658572e-17, + "velocityX": 0.2669898507573085, + "velocityY": -0.07829731508072835, + "timestamp": 0.06377357307726772 + }, + { + "x": 2.7093452100309583, + "y": 5.5292188917484815, + "heading": 4.421175983497276e-18, + "angularVelocity": 4.455815749727496e-17, + "velocityX": 0.5333423153150382, + "velocityY": -0.15873618982597607, + "timestamp": 0.12754714615453544 + }, + { + "x": 2.7602957743928864, + "y": 5.513806855363461, + "heading": 7.811064020414863e-18, + "angularVelocity": 5.3155058958518925e-17, + "velocityX": 0.7989291159865928, + "velocityY": -0.24166807097898507, + "timestamp": 0.19132071923180316 + }, + { + "x": 2.8281241133670565, + "y": 5.492918602334042, + "heading": 1.1328926559518176e-17, + "angularVelocity": 5.516176010462958e-17, + "velocityX": 1.0635806604718465, + "velocityY": -0.3275377561817081, + "timestamp": 0.2550942923090709 + }, + { + "x": 2.9127559105445995, + "y": 5.466330173559929, + "heading": 1.5973697872969206e-17, + "angularVelocity": 7.283222641176841e-17, + "velocityX": 1.3270668882705299, + "velocityY": -0.41691922674457305, + "timestamp": 0.3188678653863386 + }, + { + "x": 3.0140963269470453, + "y": 5.433769288149837, + "heading": 1.634340357557127e-17, + "angularVelocity": 5.797161500644316e-18, + "velocityX": 1.5890659957167883, + "velocityY": -0.5105701913650192, + "timestamp": 0.3826414384636063 + }, + { + "x": 3.132020733935084, + "y": 5.394898138842126, + "heading": 1.753294263521122e-17, + "angularVelocity": 1.8652539010143133e-17, + "velocityX": 1.8491108667403897, + "velocityY": -0.6095181347392088, + "timestamp": 0.44641501154087404 + }, + { + "x": 3.2663592482856187, + "y": 5.34928716553499, + "heading": 1.3361291372353952e-17, + "angularVelocity": -6.54134786803134e-17, + "velocityX": 2.1064918879136747, + "velocityY": -0.7152017851010761, + "timestamp": 0.5101885846181418 + }, + { + "x": 3.4168692069208895, + "y": 5.296373281622491, + "heading": 9.91837272236996e-18, + "angularVelocity": -5.398660422887962e-17, + "velocityX": 2.36006783645184, + "velocityY": -0.8297149016315647, + "timestamp": 0.5739621576954095 + }, + { + "x": 3.5831820501397047, + "y": 5.235389893074956, + "heading": 8.197617273877545e-18, + "angularVelocity": -2.6982264995683312e-17, + "velocityX": 2.6078645933372964, + "velocityY": -0.9562485776615884, + "timestamp": 0.6377357307726776 + }, + { + "x": 3.7646897201959026, + "y": 5.165243249909899, + "heading": 6.703480606914857e-18, + "angularVelocity": -2.342877456705142e-17, + "velocityX": 2.8461267151565104, + "velocityY": -1.0999327743494527, + "timestamp": 0.7015093038499454 + }, + { + "x": 3.960268086544922, + "y": 5.08428626917176, + "heading": 4.5447273483145725e-18, + "angularVelocity": -3.385027926825969e-17, + "velocityX": 3.066761934634853, + "velocityY": -1.2694440162550131, + "timestamp": 0.7652828769272131 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 1.549943268403634e-18, - "angularVelocity": 7.09546240599044e-17, - "velocityX": 2.9068419610975353, - "velocityY": -1.5725251674415364, - "timestamp": 0.8824012997369741 - }, - { - "x": 4.262376057274491, - "y": 4.935051597962846, - "heading": 3.5392756001694805e-18, - "angularVelocity": 6.14731881972971e-17, - "velocityX": 2.9318335021132986, - "velocityY": -1.6963703103934957, - "timestamp": 0.9147622751494358 - }, - { - "x": 4.358262696439126, - "y": 4.876193489161229, - "heading": 6.0915874857662914e-18, - "angularVelocity": 7.887005422678292e-17, - "velocityX": 2.9630330341559845, - "velocityY": -1.8187989716450665, - "timestamp": 0.9471232505618974 - }, - { - "x": 4.455482915426748, - "y": 4.813470451403357, - "heading": 8.792871908848605e-18, - "angularVelocity": 8.347351674874434e-17, - "velocityX": 3.0042425405441757, - "velocityY": -1.938230753505593, - "timestamp": 0.979484225974359 - }, - { - "x": 4.554624947873469, - "y": 4.7471387133987335, - "heading": 1.0200765098673088e-17, - "angularVelocity": 4.3505894735366074e-17, - "velocityX": 3.0636292998925567, - "velocityY": -2.0497447051327327, - "timestamp": 1.0118452013868207 - }, - { - "x": 4.656862547860915, - "y": 4.678136115394173, - "heading": 1.0067733420891333e-17, - "angularVelocity": -4.1108673671660525e-18, - "velocityX": 3.1592867237269773, - "velocityY": -2.1322780640903733, - "timestamp": 1.0442061767992823 - }, - { - "x": 4.763145340600117, - "y": 4.609727154178217, - "heading": 9.332936021050385e-18, - "angularVelocity": -2.2706280960159642e-17, - "velocityX": 3.28428891232596, - "velocityY": -2.1139338460611237, - "timestamp": 1.076567152211744 - }, - { - "x": 4.872109686799802, - "y": 4.544404504995177, - "heading": 8.161678901765097e-18, - "angularVelocity": -3.6193504811759114e-17, - "velocityX": 3.3671527143685642, - "velocityY": -2.018562430534307, - "timestamp": 1.1089281276242056 - }, - { - "x": 4.982798416209946, - "y": 4.482788956670884, - "heading": 7.225847948253543e-18, - "angularVelocity": -2.891850265932224e-17, - "velocityX": 3.4204385992493553, - "velocityY": -1.9040077605499512, - "timestamp": 1.1412891030366672 - }, - { - "x": 5.094726607705277, - "y": 4.425069541031477, - "heading": 5.520990037711479e-18, - "angularVelocity": -5.268252544393214e-17, - "velocityX": 3.458739734162278, - "velocityY": -1.7836117392548974, - "timestamp": 1.1736500784491288 - }, - { - "x": 5.207616568786535, - "y": 4.371323930618111, - "heading": 3.484375827946225e-18, - "angularVelocity": -6.293426523223445e-17, - "velocityX": 3.48845977732135, - "velocityY": -1.6608155263660398, - "timestamp": 1.2060110538615905 - }, - { - "x": 5.321291012099858, - "y": 4.321590890995498, - "heading": 1.6420175756244674e-18, - "angularVelocity": -5.693148086098641e-17, - "velocityX": 3.5127013900065562, - "velocityY": -1.5368214025916205, - "timestamp": 1.238372029274052 - }, - { - "x": 5.4356276249643685, - "y": 4.275892411291382, - "heading": -1.9852645585834772e-19, - "angularVelocity": -5.687541886629066e-17, - "velocityX": 3.5331633675195255, - "velocityY": -1.4121477836084664, - "timestamp": 1.2707330046865137 - }, - { - "x": 5.5505371629105715, - "y": 4.234242129103919, - "heading": -1.7642698806491036e-18, - "angularVelocity": -4.838369069076164e-17, - "velocityX": 3.5508675644539367, - "velocityY": -1.2870527435160222, - "timestamp": 1.3030939800989754 + "heading": 2.916776997028848e-18, + "angularVelocity": -2.5527036870167268e-17, + "velocityX": 3.2494804486361053, + "velocityY": -1.4792721930373927, + "timestamp": 0.8290564500044808 + }, + { + "x": 4.281965617918933, + "y": 4.933633775459018, + "heading": 2.5007263674842835e-18, + "angularVelocity": -1.1988032936813997e-17, + "velocityX": 3.298225514699012, + "velocityY": -1.6226254295188278, + "timestamp": 0.8637619460385229 + }, + { + "x": 4.397154085355019, + "y": 4.872114684924303, + "heading": 2.80280710947709e-18, + "angularVelocity": 8.704118266930975e-18, + "velocityX": 3.31902668450838, + "velocityY": -1.772603695805773, + "timestamp": 0.8984674420725649 + }, + { + "x": 4.510378349293925, + "y": 4.805721616464948, + "heading": 5.6088052918749416e-18, + "angularVelocity": 8.085169506424825e-17, + "velocityX": 3.2624303605355687, + "velocityY": -1.9130419111206762, + "timestamp": 0.933172938106607 + }, + { + "x": 4.620854325513756, + "y": 4.734849659733664, + "heading": 8.920577651651403e-18, + "angularVelocity": 9.542501154652829e-17, + "velocityX": 3.183241527839498, + "velocityY": -2.0420960605711302, + "timestamp": 0.967878434140649 + }, + { + "x": 4.728404969374526, + "y": 4.659612469130667, + "heading": 1.0355720889195964e-17, + "angularVelocity": 4.1352045109421545e-17, + "velocityX": 3.0989513521223016, + "velocityY": -2.167875385765943, + "timestamp": 1.002583930174691 + }, + { + "x": 4.836822070451743, + "y": 4.585629308888239, + "heading": 8.659234104719362e-18, + "angularVelocity": -4.888236672406423e-17, + "velocityX": 3.1239173464304306, + "velocityY": -2.131741905370206, + "timestamp": 1.037289426208733 + }, + { + "x": 4.9481136166787945, + "y": 4.516045063115851, + "heading": 6.20990752321007e-18, + "angularVelocity": -7.057460233695506e-17, + "velocityX": 3.2067412642045734, + "velocityY": -2.0049921114550315, + "timestamp": 1.0719949222427752 + }, + { + "x": 5.062101259723399, + "y": 4.450971317120919, + "heading": 3.721439651007284e-18, + "angularVelocity": -7.170241479222451e-17, + "velocityX": 3.2844262745242743, + "velocityY": -1.8750271118759503, + "timestamp": 1.1067004182768172 + }, + { + "x": 5.178602303777092, + "y": 4.39051239286923, + "heading": 1.522860892330953e-18, + "angularVelocity": -6.334958464560708e-17, + "velocityX": 3.356847109732077, + "velocityY": -1.7420561916875086, + "timestamp": 1.1414059143108592 + }, + { + "x": 5.2974300176780025, + "y": 4.334765207042833, + "heading": -2.608193001732958e-19, + "angularVelocity": -5.139474712462447e-17, + "velocityX": 3.42388749563912, + "velocityY": -1.606292725847098, + "timestamp": 1.1761114103449013 + }, + { + "x": 5.4183939381282835, + "y": 4.283819120698816, + "heading": -2.822662772810701e-18, + "angularVelocity": -7.38166505421658e-17, + "velocityX": 3.485439894926986, + "velocityY": -1.4679544212261044, + "timestamp": 1.2108169063789433 + }, + { + "x": 5.5413001762936585, + "y": 4.23775579768525, + "heading": -5.4175777354584686e-18, + "angularVelocity": -7.476956848858907e-17, + "velocityX": 3.5414056045998694, + "velocityY": -1.3272630642818801, + "timestamp": 1.2455224024129854 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -2.8015689175743048e-18, - "angularVelocity": -3.2054010230453e-17, - "velocityX": 3.5664736442333616, - "velocityY": -1.1616786598774413, - "timestamp": 1.335454955511437 - }, - { - "x": 5.795001659778632, - "y": 4.159833423852274, - "heading": -3.918726614896288e-18, - "angularVelocity": -3.100871682049742e-17, - "velocityX": 3.5820124359680223, - "velocityY": -1.0218844572378571, - "timestamp": 1.3714821701749043 - }, - { - "x": 5.924325054788198, - "y": 4.128077802262558, - "heading": -4.913044441347591e-18, - "angularVelocity": -2.7599075747243218e-17, - "velocityX": 3.5896029215020695, - "velocityY": -0.8814342681316839, - "timestamp": 1.4075093848383715 - }, - { - "x": 6.053574493908213, - "y": 4.101389053934096, - "heading": -5.793955566552838e-18, - "angularVelocity": -2.4451269226363692e-17, - "velocityX": 3.5875501430610877, - "velocityY": -0.7407941073925298, - "timestamp": 1.4435365995018388 - }, - { - "x": 6.182326457133652, - "y": 4.079743240098328, - "heading": -6.4165955709959285e-18, - "angularVelocity": -1.7282490757751136e-17, - "velocityX": 3.5737418068012827, - "velocityY": -0.6008184101369575, - "timestamp": 1.479563814165306 - }, - { - "x": 6.310065721160521, - "y": 4.063062615773879, - "heading": -6.572943704568109e-18, - "angularVelocity": -4.339723040934764e-18, - "velocityX": 3.5456325230826065, - "velocityY": -0.46300066436618775, - "timestamp": 1.5155910288287733 - }, - { - "x": 6.436174783136967, - "y": 4.051180022659281, - "heading": -6.55887717200026e-18, - "angularVelocity": 3.904418561563001e-19, - "velocityX": 3.500383339495921, - "velocityY": -0.3298226972469103, - "timestamp": 1.5516182434922405 - }, - { - "x": 6.559940504232512, - "y": 4.043790469879042, - "heading": -6.336632318587662e-18, - "angularVelocity": 6.168804762936026e-18, - "velocityX": 3.4353397078194354, - "velocityY": -0.2051102992353164, - "timestamp": 1.5876454581557078 - }, - { - "x": 6.680596023490012, - "y": 4.040401579407175, - "heading": -5.869996108311954e-18, - "angularVelocity": 1.2952325474912863e-17, - "velocityX": 3.349010474013899, - "velocityY": -0.09406473699182225, - "timestamp": 1.623672672819175 - }, - { - "x": 6.797407433038957, - "y": 4.040314426809989, - "heading": -5.129903283273234e-18, - "angularVelocity": 2.0542604582494022e-17, - "velocityX": 3.242310310138806, - "velocityY": -0.002419076745183701, - "timestamp": 1.6596998874826423 - }, - { - "x": 6.9097792291334015, - "y": 4.042670322539034, - "heading": -4.098685939880453e-18, - "angularVelocity": 2.8623288062328696e-17, - "velocityX": 3.1190808710613775, - "velocityY": 0.06539211401857271, - "timestamp": 1.6957271021461096 - }, - { - "x": 7.0173204584579265, - "y": 4.046557322402495, - "heading": -2.7703450666011344e-18, - "angularVelocity": 3.687048487350159e-17, - "velocityX": 2.9849998210817597, - "velocityY": 0.10789065709827174, - "timestamp": 1.7317543168095768 - }, - { - "x": 7.119839418159034, - "y": 4.051119205104076, - "heading": -1.5285714179208822e-18, - "angularVelocity": 3.4467656195020295e-17, - "velocityX": 2.845597714359634, - "velocityY": 0.1266232414632443, - "timestamp": 1.767781531473044 + "heading": -7.665872207711091e-18, + "angularVelocity": -6.478208725348017e-17, + "velocityX": 3.591694883279384, + "velocityY": -1.1844441897757751, + "timestamp": 1.2802278984470274 + }, + { + "x": 5.800468403278387, + "y": 4.158565232862714, + "heading": -9.714096811795654e-18, + "angularVelocity": -5.540829464294432e-17, + "velocityX": 3.6389268627432543, + "velocityY": -1.0302389285738007, + "timestamp": 1.3171939270404631 + }, + { + "x": 5.936486438230336, + "y": 4.1262510009162545, + "heading": -1.1401685441264777e-17, + "angularVelocity": -4.5652419090776246e-17, + "velocityX": 3.67954146353996, + "velocityY": -0.8741602269982254, + "timestamp": 1.3541599556338988 + }, + { + "x": 6.0737584862384155, + "y": 4.099765139631346, + "heading": -1.2627280609090703e-17, + "angularVelocity": -3.3154634524185157e-17, + "velocityX": 3.713464854930551, + "velocityY": -0.7164919330720612, + "timestamp": 1.3911259842273345 + }, + { + "x": 6.212034919724313, + "y": 4.079155810434451, + "heading": -1.3669283004490658e-17, + "angularVelocity": -2.818810770451562e-17, + "velocityX": 3.7406353548742786, + "velocityY": -0.5575207827587797, + "timestamp": 1.4280920128207701 + }, + { + "x": 6.351064277842407, + "y": 4.06446048840337, + "heading": -1.3375402013759826e-17, + "angularVelocity": 7.950028767305027e-18, + "velocityX": 3.761003370072177, + "velocityY": -0.3975358617152357, + "timestamp": 1.4650580414142058 + }, + { + "x": 6.490086441164715, + "y": 4.055726900176193, + "heading": -1.4576849056839345e-17, + "angularVelocity": -3.2501382723403574e-17, + "velocityX": 3.7608087374308066, + "velocityY": -0.23625984612066567, + "timestamp": 1.5020240700076415 + }, + { + "x": 6.625430002884342, + "y": 4.051684818750479, + "heading": -1.3372180037518883e-17, + "angularVelocity": 3.2588543188391143e-17, + "velocityX": 3.661295704988517, + "velocityY": -0.10934583939674931, + "timestamp": 1.5389900986010772 + }, + { + "x": 6.755501460439252, + "y": 4.050426142633095, + "heading": -1.1630354881514603e-17, + "angularVelocity": 4.7119618262525755e-17, + "velocityX": 3.518675457011608, + "velocityY": -0.03404953589216864, + "timestamp": 1.5759561271945128 + }, + { + "x": 6.8798297740402194, + "y": 4.050767085715321, + "heading": -9.335447591441833e-18, + "angularVelocity": 6.208152126139743e-17, + "velocityX": 3.3633127044392923, + "velocityY": 0.009223146093816154, + "timestamp": 1.6129221557879485 + }, + { + "x": 6.998265582982919, + "y": 4.052014068285868, + "heading": -6.4179877740703305e-18, + "angularVelocity": 7.892272793106084e-17, + "velocityX": 3.203909466318293, + "velocityY": 0.03373320364655125, + "timestamp": 1.6498881843813842 + }, + { + "x": 7.110758098746119, + "y": 4.053729886887297, + "heading": -3.1976006784105176e-18, + "angularVelocity": 8.71174756444278e-17, + "velocityX": 3.0431323039981164, + "velocityY": 0.046416092469654044, + "timestamp": 1.6868542129748199 }, { "x": 7.217291355133057, "y": 4.0556182861328125, - "heading": 0, - "angularVelocity": 4.242824298811395e-17, - "velocityX": 2.704953404928098, - "velocityY": 0.12488006832508823, - "timestamp": 1.8038087461365113 - }, - { - "x": 7.386413080563964, - "y": 4.061807740730578, - "heading": 2.2517150943375065e-18, - "angularVelocity": 3.243733929990829e-17, - "velocityX": 2.436302356627287, - "velocityY": 0.08916289604043352, - "timestamp": 1.8732261257038947 - }, - { - "x": 7.536811709047552, - "y": 4.066158661383703, - "heading": 2.5863681690768457e-18, - "angularVelocity": 4.820883139423778e-18, - "velocityX": 2.166584642360316, - "velocityY": 0.06267768504430034, - "timestamp": 1.942643505271278 - }, - { - "x": 7.668447638906394, - "y": 4.069132723210473, - "heading": 2.8797714291084188e-18, - "angularVelocity": 4.226654216280414e-18, - "velocityX": 1.89629644160023, - "velocityY": 0.04284318776225898, - "timestamp": 2.0120608848386614 - }, - { - "x": 7.781298581594194, - "y": 4.071077765012589, - "heading": 3.948563192438554e-18, - "angularVelocity": 1.5396601974766197e-17, - "velocityX": 1.6256871606375698, - "velocityY": 0.028019522117344105, - "timestamp": 2.0814782644060448 - }, - { - "x": 7.875351641584324, - "y": 4.072265013256325, - "heading": 5.745276308795261e-18, - "angularVelocity": 2.5882756271616716e-17, - "velocityX": 1.3548921116913086, - "velocityY": 0.017103040350048473, - "timestamp": 2.150895643973428 - }, - { - "x": 7.950599316309057, - "y": 4.072911756307987, - "heading": 3.5817600850337305e-18, - "angularVelocity": -3.116678038331375e-17, - "velocityX": 1.0839889836476846, - "velocityY": 0.009316730992924663, - "timestamp": 2.2203130235408115 - }, - { - "x": 8.007037333813425, - "y": 4.073195912702993, - "heading": 1.7956945946791005e-18, - "angularVelocity": -2.5729370677570625e-17, - "velocityX": 0.8130243154682115, - "velocityY": 0.004093447444678863, - "timestamp": 2.289730403108195 - }, - { - "x": 8.044663419582989, - "y": 4.0732658050755175, - "heading": 5.777314545574126e-19, - "angularVelocity": -1.754550730252714e-17, - "velocityX": 0.542026881510858, - "velocityY": 0.0010068425653567271, - "timestamp": 2.3591477826755782 + "heading": -8.646914705534956e-42, + "angularVelocity": 8.650106057047145e-17, + "velocityX": 2.8819232262850263, + "velocityY": 0.051084720684648116, + "timestamp": 1.7238202415682555 + }, + { + "x": 7.405338431031862, + "y": 4.059210715077462, + "heading": 6.035426461355953e-18, + "angularVelocity": 8.221730826644903e-17, + "velocityX": 2.561662296238626, + "velocityY": 0.04893769145541128, + "timestamp": 1.7972284641020848 + }, + { + "x": 7.569876923635766, + "y": 4.062517669134614, + "heading": 1.1021958875984658e-17, + "angularVelocity": 6.792879928853616e-17, + "velocityX": 2.2414177448320305, + "velocityY": 0.04504882345608618, + "timestamp": 1.870636686635914 + }, + { + "x": 7.710907923869345, + "y": 4.065460118931574, + "heading": 1.4466516010520595e-17, + "angularVelocity": 4.692331479554e-17, + "velocityX": 1.9211880544932918, + "velocityY": 0.04008338160760226, + "timestamp": 1.9440449091697434 + }, + { + "x": 7.828432324355203, + "y": 4.067984380811152, + "heading": 1.5902740281761073e-17, + "angularVelocity": 1.956489643348339e-17, + "velocityX": 1.600970523863292, + "velocityY": 0.03438663670702133, + "timestamp": 2.0174531317035727 + }, + { + "x": 7.922450847470263, + "y": 4.070051609380546, + "heading": 1.5219645547347743e-17, + "angularVelocity": -9.30542534385069e-18, + "velocityX": 1.2807628337783643, + "velocityY": 0.02816072230112734, + "timestamp": 2.090861354237402 + }, + { + "x": 7.992964082884089, + "y": 4.071632392724107, + "heading": 1.2631622975679932e-17, + "angularVelocity": -3.5255213685022036e-17, + "velocityX": 0.9605631764388524, + "velocityY": 0.021534145481204726, + "timestamp": 2.164269576771231 + }, + { + "x": 8.039972518291798, + "y": 4.072703688242487, + "heading": 7.596971431802736e-18, + "angularVelocity": -6.858429982495519e-17, + "velocityX": 0.6403701627027665, + "velocityY": 0.014593671953933919, + "timestamp": 2.2376777993050605 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": -2.803956986151352e-39, - "angularVelocity": -8.322576538583876e-18, - "velocityX": 0.2710148817811471, - "velocityY": -0.0002715343629009974, - "timestamp": 2.4285651622429616 + "heading": 8.652482395157843e-45, + "angularVelocity": -1.0348937993018065e-16, + "velocityX": 0.32018271791516006, + "velocityY": 0.007400637290262982, + "timestamp": 2.31108602183889 }, { "x": 8.0634765625, "y": 4.073246955871582, "heading": 0, - "angularVelocity": -3.588476339344369e-38, - "velocityX": -2.7715613376406244e-35, - "velocityY": -8.161774363102711e-35, - "timestamp": 2.497982541810345 + "angularVelocity": 5.394624259127602e-43, + "velocityX": 0, + "velocityY": -7.823862347152674e-43, + "timestamp": 2.384494244372719 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj index 88448f73..f61fce96 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj @@ -4,397 +4,379 @@ "x": 8.0634765625, "y": 4.073246955871582, "heading": 0, - "angularVelocity": -3.588476339344369e-38, - "velocityX": -2.7715613376406244e-35, - "velocityY": -8.161774363102711e-35, + "angularVelocity": 5.394624259127602e-43, + "velocityX": 0, + "velocityY": -7.823862347152674e-43, "timestamp": 0 }, { - "x": 8.045268218494023, - "y": 4.069202722176317, - "heading": -4.783612799247601e-20, - "angularVelocity": -6.920780898714677e-19, - "velocityX": -0.2634326077352512, - "velocityY": -0.05851070411920324, - "timestamp": 0.06911955267237424 - }, - { - "x": 8.008750233843994, - "y": 4.061600867158929, - "heading": -1.3711751824964603e-19, - "angularVelocity": -1.2916951396895015e-18, - "velocityX": -0.5283307434457009, - "velocityY": -0.10998125311111973, - "timestamp": 0.13823910534474848 - }, - { - "x": 7.953821890093904, - "y": 4.051005973704918, - "heading": -2.6045835174744157e-19, - "angularVelocity": -1.7844564773553645e-18, - "velocityX": -0.7946860421746129, - "velocityY": -0.1532835940682337, - "timestamp": 0.20735865801712272 - }, - { - "x": 7.880387860483353, - "y": 4.038078874831867, - "heading": -4.092475364172468e-19, - "angularVelocity": -2.152635237102946e-18, - "velocityX": -1.062420498561777, - "velocityY": -0.18702521028060204, - "timestamp": 0.27647821068949696 - }, - { - "x": 7.788366410256325, - "y": 4.023600204310839, - "heading": -5.733506163788985e-19, - "angularVelocity": -2.3741918689373813e-18, - "velocityX": -1.3313374677525447, - "velocityY": -0.20947286203740642, - "timestamp": 0.3455977633618712 - }, - { - "x": 7.677703229907308, - "y": 4.008500675956516, - "heading": -1.077796513774058e-18, - "angularVelocity": -7.298164960190344e-18, - "velocityX": -1.6010401698280374, - "velocityY": -0.21845523835917138, - "timestamp": 0.41471731603424544 - }, - { - "x": 7.548394643155177, - "y": 3.993899515872939, - "heading": -1.3825329983857112e-18, - "angularVelocity": -4.4088318398420175e-18, - "velocityX": -1.8707960591853112, - "velocityY": -0.2112450025940816, - "timestamp": 0.4838368687066197 - }, - { - "x": 7.40052628679877, - "y": 3.9811515921807197, - "heading": -2.4834039360007183e-18, - "angularVelocity": -1.5927055298250076e-17, - "velocityX": -2.1393129822078065, - "velocityY": -0.18443295998520684, - "timestamp": 0.5529564213789939 - }, - { - "x": 7.23433662009932, - "y": 3.9719006814395397, - "heading": -2.9994646007909418e-18, - "angularVelocity": -7.466203770052342e-18, - "velocityX": -2.4043799514615647, - "velocityY": -0.13383927388866498, - "timestamp": 0.6220759740513682 - }, - { - "x": 7.050317591694833, - "y": 3.968128214859164, - "heading": -3.4395184001583614e-18, - "angularVelocity": -6.366560290082446e-18, - "velocityX": -2.662329562182405, - "velocityY": -0.05457886277500885, - "timestamp": 0.6911955267237424 - }, - { - "x": 6.849362271663259, - "y": 3.972169933507745, - "heading": -3.772315951879389e-18, - "angularVelocity": -4.8148105541753904e-18, - "velocityX": -2.907358515239499, - "velocityY": 0.05847431721293695, - "timestamp": 0.7603150793961166 - }, - { - "x": 6.632947899656103, - "y": 3.9866479902960563, - "heading": -6.760060231429226e-18, - "angularVelocity": -4.322574675534167e-17, - "velocityX": -3.131015228541142, - "velocityY": 0.20946398274504574, - "timestamp": 0.8294346320684909 - }, - { - "x": 6.4032866668448785, - "y": 4.014256930642455, - "heading": -9.565460422027674e-18, - "angularVelocity": -4.058764969139539e-17, - "velocityX": -3.3226666541060275, - "velocityY": 0.39943748590597694, - "timestamp": 0.8985541847408651 - }, - { - "x": 6.1633110990497455, - "y": 4.057406588823124, - "heading": -1.2152070169754713e-17, - "angularVelocity": -3.7422258214923894e-17, - "velocityX": -3.4718912162613744, - "velocityY": 0.6242757152263072, - "timestamp": 0.9676737374132394 - }, - { - "x": 5.916400554770506, - "y": 4.117871140121953, - "heading": -1.4492027796446373e-17, - "angularVelocity": -3.385377272110881e-17, - "velocityX": -3.5722242800035118, - "velocityY": 0.8747821558601414, - "timestamp": 1.0367932900856136 + "x": 8.042595807906332, + "y": 4.071940125417329, + "heading": 2.4014031773112573e-19, + "angularVelocity": 3.467784621738616e-18, + "velocityX": -0.30153187250835495, + "velocityY": -0.01887149394694188, + "timestamp": 0.06924891362218677 + }, + { + "x": 8.000826969409328, + "y": 4.069449378892138, + "heading": 7.295832460611546e-19, + "angularVelocity": 7.067878797353639e-18, + "velocityX": -0.6031695850838968, + "velocityY": -0.03596802310545437, + "timestamp": 0.13849782724437354 + }, + { + "x": 7.938162262991824, + "y": 4.0659211372128246, + "heading": 1.4792287784714074e-18, + "angularVelocity": 1.0825376070160765e-17, + "velocityX": -0.9049197040027859, + "velocityY": -0.0509501376232871, + "timestamp": 0.2077467408665603 + }, + { + "x": 7.8545936374958405, + "y": 4.061532754567933, + "heading": 2.6959287825754293e-18, + "angularVelocity": 1.756995078279752e-17, + "velocityX": -1.206786086954711, + "velocityY": -0.06337114064826968, + "timestamp": 0.2769956544887471 + }, + { + "x": 7.7501132248715185, + "y": 4.056503424652652, + "heading": 4.202275116563444e-18, + "angularVelocity": 2.17526348818473e-17, + "velocityX": -1.5087660897375712, + "velocityY": -0.07262684210066948, + "timestamp": 0.34624456811093385 + }, + { + "x": 7.624714358960079, + "y": 4.051110859560637, + "heading": 6.0188244090330196e-18, + "angularVelocity": 2.6232170260178098e-17, + "velocityX": -1.8108423562512328, + "velocityY": -0.07787219769881376, + "timestamp": 0.4154934817331206 + }, + { + "x": 7.478393887159083, + "y": 4.045718105040055, + "heading": 8.17237550228445e-18, + "angularVelocity": 3.1098698601987103e-17, + "velocityX": -2.1129641484240804, + "velocityY": -0.07787493317230812, + "timestamp": 0.4847423953553074 + }, + { + "x": 7.311157646174121, + "y": 4.040819461213868, + "heading": 1.0532118174804612e-17, + "angularVelocity": 3.4076241042489266e-17, + "velocityX": -2.4150016547173685, + "velocityY": -0.07073964875337486, + "timestamp": 0.5539913089774942 + }, + { + "x": 7.123034533164843, + "y": 4.037125830040512, + "heading": 8.283291482043014e-18, + "angularVelocity": -3.247454111743766e-17, + "velocityX": -2.716621866960299, + "velocityY": -0.053338471033761534, + "timestamp": 0.6232402225996809 + }, + { + "x": 6.914117681129227, + "y": 4.035741699141579, + "heading": 5.798379315469736e-18, + "angularVelocity": -3.588377111777714e-17, + "velocityX": -3.016897177267489, + "velocityY": -0.019987763367446825, + "timestamp": 0.6924891362218677 + }, + { + "x": 6.684712125750405, + "y": 4.03859150608828, + "heading": 3.611288298800787e-18, + "angularVelocity": -3.158303722425788e-17, + "velocityX": -3.312767571061532, + "velocityY": 0.041153092483867636, + "timestamp": 0.7617380498440545 + }, + { + "x": 6.43606725460267, + "y": 4.049660896865006, + "heading": 2.3554139869933522e-18, + "angularVelocity": -1.8135653631468075e-17, + "velocityX": -3.5905959839934547, + "velocityY": 0.15984930589841134, + "timestamp": 0.8309869634662412 + }, + { + "x": 6.175716481433539, + "y": 4.078070527288863, + "heading": 2.6764824042330446e-18, + "angularVelocity": 4.636439771335605e-18, + "velocityX": -3.759636932206204, + "velocityY": 0.41025380670743106, + "timestamp": 0.900235877088428 + }, + { + "x": 5.918464051979435, + "y": 4.12717103366876, + "heading": 2.4806366122903936e-18, + "angularVelocity": -2.8281424458318553e-18, + "velocityX": -3.7148948048144055, + "velocityY": 0.7090437064151481, + "timestamp": 0.9694847907106148 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -1.65695066909008e-17, - "angularVelocity": -3.005631278245188e-17, - "velocityX": -3.6234150289835947, - "velocityY": 1.1397344367359599, - "timestamp": 1.1059128427579878 - }, - { - "x": 5.553208867110479, - "y": 4.235853588954207, - "heading": -1.751752246117206e-17, - "angularVelocity": -3.049424202704779e-17, - "velocityX": -3.6265305068647917, - "velocityY": 1.2610675773235673, - "timestamp": 1.1370011960744444 - }, - { - "x": 5.440423618005098, - "y": 4.27883115567922, - "heading": -1.8382942207466584e-17, - "angularVelocity": -2.7837426365803457e-17, - "velocityX": -3.6278939562128056, - "velocityY": 1.382433038106979, - "timestamp": 1.168089549390901 - }, - { - "x": 5.327666323513458, - "y": 4.3255819087456695, - "heading": -1.9161761421911296e-17, - "angularVelocity": -2.5051800155064728e-17, - "velocityX": -3.626994757292344, - "velocityY": 1.5038028097069258, - "timestamp": 1.1991779027073575 - }, - { - "x": 5.215031204096495, - "y": 4.376103972546304, - "heading": -2.1196555001876326e-17, - "angularVelocity": -6.545195749690707e-17, - "velocityX": -3.6230648265741094, - "velocityY": 1.6251122497984019, - "timestamp": 1.230266256023814 - }, - { - "x": 5.102650689013506, - "y": 4.430390725009075, - "heading": -2.311672068470027e-17, - "angularVelocity": -6.176479221122079e-17, - "velocityX": -3.6148751250680937, - "velocityY": 1.746208681758474, - "timestamp": 1.2613546093402705 - }, - { - "x": 4.990723576065449, - "y": 4.488423424572919, - "heading": -2.4422095497687565e-17, - "angularVelocity": -4.198919124707493e-17, - "velocityX": -3.6002908165872247, - "velocityY": 1.8667022654147425, - "timestamp": 1.292442962656727 - }, - { - "x": 4.87957749498313, - "y": 4.550147690824094, - "heading": -2.55240391966328e-17, - "angularVelocity": -3.544554733112049e-17, - "velocityX": -3.5751678434342686, - "velocityY": 1.9854466276443046, - "timestamp": 1.3235313159731836 - }, - { - "x": 4.769828431698842, - "y": 4.615377093855064, - "heading": -2.67892153006201e-17, - "angularVelocity": -4.069614402184995e-17, - "velocityX": -3.5302308284753074, - "velocityY": 2.0981942133435845, - "timestamp": 1.3546196692896402 - }, - { - "x": 4.662763212803556, - "y": 4.683258770801351, - "heading": -2.756635025118312e-17, - "angularVelocity": -2.4997623471627382e-17, - "velocityX": -3.443901251553593, - "velocityY": 2.1835082821949876, - "timestamp": 1.3857080226060967 - }, - { - "x": 4.559444563014645, - "y": 4.750692382461376, - "heading": -2.7843158304647625e-17, - "angularVelocity": -8.903914937396602e-18, - "velocityX": -3.32338766023415, - "velocityY": 2.1690956408530244, - "timestamp": 1.4167963759225533 - }, - { - "x": 4.458844422855551, - "y": 4.815509255321042, - "heading": -2.805863064985095e-17, - "angularVelocity": -6.930966815860972e-18, - "velocityX": -3.2359430277653423, - "velocityY": 2.084924608256891, - "timestamp": 1.4478847292390098 - }, - { - "x": 4.36022139727515, - "y": 4.877112301745061, - "heading": -2.8000369092306777e-17, - "angularVelocity": 1.8740637997727053e-18, - "velocityX": -3.1723463953362434, - "velocityY": 1.9815474237874686, - "timestamp": 1.4789730825554663 - }, - { - "x": 4.263183288640869, - "y": 4.935291063699046, - "heading": -2.7736137264454843e-17, - "angularVelocity": 8.499383198535807e-18, - "velocityX": -3.1213653436868745, - "velocityY": 1.871400564763493, - "timestamp": 1.5100614358719229 + "heading": 3.0661477431883128e-18, + "angularVelocity": 8.455167023881293e-18, + "velocityX": -3.6464445426004524, + "velocityY": 1.0033087488526664, + "timestamp": 1.0387337043328015 + }, + { + "x": 5.541093538436437, + "y": 4.236435061250764, + "heading": 3.354859743531915e-18, + "angularVelocity": 8.33229202553877e-18, + "velocityX": -3.6034349206984246, + "velocityY": 1.1482323536375627, + "timestamp": 1.0733834741030748 + }, + { + "x": 5.417925084630127, + "y": 4.281179055760442, + "heading": 3.9245083052882345e-18, + "angularVelocity": 1.6440183168115343e-17, + "velocityX": -3.554668750266214, + "velocityY": 1.2913215529664346, + "timestamp": 1.108033243873348 + }, + { + "x": 5.296643156826778, + "y": 4.330809566180666, + "heading": 4.682534245858068e-18, + "angularVelocity": 2.1876795880478242e-17, + "velocityX": -3.500223193615562, + "velocityY": 1.4323474802075717, + "timestamp": 1.1426830136436212 + }, + { + "x": 5.177441529342695, + "y": 4.385247292254255, + "heading": 5.748631283020555e-18, + "angularVelocity": 3.076779569476711e-17, + "velocityX": -3.4401852674457762, + "velocityY": 1.5710847845312972, + "timestamp": 1.1773327834138945 + }, + { + "x": 5.060510651244131, + "y": 4.444405251423305, + "heading": 6.298107237796981e-18, + "angularVelocity": 1.5857997280196432e-17, + "velocityX": -3.374650939207116, + "velocityY": 1.7073117530438153, + "timestamp": 1.2119825531841677 + }, + { + "x": 4.946037340921014, + "y": 4.5081889164365, + "heading": 6.498139126492573e-18, + "angularVelocity": 5.772964438776829e-18, + "velocityX": -3.303724991019314, + "velocityY": 1.840810644228753, + "timestamp": 1.246632322954441 + }, + { + "x": 4.834204484779198, + "y": 4.576496362942444, + "heading": 6.284318947548799e-18, + "angularVelocity": -6.170897537311036e-18, + "velocityX": -3.227520900810133, + "velocityY": 1.971367976145888, + "timestamp": 1.2812820927247142 + }, + { + "x": 4.725190735681975, + "y": 4.649218420709879, + "heading": 4.218601510181254e-18, + "angularVelocity": -5.961706098087163e-17, + "velocityX": -3.146160849551947, + "velocityY": 2.098774631103723, + "timestamp": 1.3159318624949874 + }, + { + "x": 4.619170166207779, + "y": 4.726238769851069, + "heading": 2.3861247317221027e-18, + "angularVelocity": -5.2885684107236655e-17, + "velocityX": -3.059777025276331, + "velocityY": 2.22282426843907, + "timestamp": 1.3505816322652606 + }, + { + "x": 4.5101566662173465, + "y": 4.798961201296667, + "heading": 8.546946946648484e-19, + "angularVelocity": -4.419740873346574e-17, + "velocityX": -3.146153660275008, + "velocityY": 2.0987854155379675, + "timestamp": 1.3852314020355339 + }, + { + "x": 4.398324060290128, + "y": 4.867269057737162, + "heading": -1.0977695137154222e-18, + "angularVelocity": -5.634854780637897e-17, + "velocityX": -3.2275136795616595, + "velocityY": 1.9713798069474653, + "timestamp": 1.419881171805807 + }, + { + "x": 4.283850992630547, + "y": 4.931053147799285, + "heading": -2.453093242554037e-18, + "angularVelocity": -3.911494182571376e-17, + "velocityX": -3.3037179876961247, + "velocityY": 1.8408229112346155, + "timestamp": 1.4545309415760803 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -2.7306183592260526e-17, - "angularVelocity": 1.3830056156762042e-17, - "velocityX": -3.077815742368318, - "velocityY": 1.758109592119851, - "timestamp": 1.5411497891883794 - }, - { - "x": 3.9493379060680507, - "y": 5.0986210510281875, - "heading": -2.5526201955845404e-17, - "angularVelocity": 2.4369765067325514e-17, - "velocityX": -2.986848903907283, - "velocityY": 1.487847763803033, - "timestamp": 1.6141903635025772 - }, - { - "x": 3.742771574255505, - "y": 5.189991789335339, - "heading": -2.3046567381049293e-17, - "angularVelocity": 3.3948727787048854e-17, - "velocityX": -2.828103882698977, - "velocityY": 1.2509586509287764, - "timestamp": 1.687230937816775 - }, - { - "x": 3.5509457192311733, - "y": 5.266647381491633, - "heading": -1.9951230504969442e-17, - "angularVelocity": 4.2378320613588127e-17, - "velocityX": -2.6262917128657457, - "velocityY": 1.0494932833707686, - "timestamp": 1.7602715121309727 - }, - { - "x": 3.3757737671252985, - "y": 5.330794386309625, - "heading": -1.631318519146512e-17, - "angularVelocity": 4.980855295391938e-17, - "velocityX": -2.3982827866651224, - "velocityY": 0.8782379577418434, - "timestamp": 1.8333120864451704 - }, - { - "x": 3.218440544499334, - "y": 5.384189785860755, - "heading": -1.2190567343791479e-17, - "angularVelocity": 5.64428454512083e-17, - "velocityX": -2.1540523757270402, - "velocityY": 0.7310375096646903, - "timestamp": 1.9063526607593682 - }, - { - "x": 3.0797086044379114, - "y": 5.42821450731098, - "heading": -8.80046228629925e-18, - "angularVelocity": 4.6413997826950126e-17, - "velocityX": -1.8993818348777063, - "velocityY": 0.6027433637206029, - "timestamp": 1.979393235073565 - }, - { - "x": 2.960089565045244, - "y": 5.463961887551477, - "heading": -5.007321106893989e-18, - "angularVelocity": 5.193197363283625e-17, - "velocityX": -1.6377067200773132, - "velocityY": 0.4894181155630415, - "timestamp": 2.052433809387762 - }, - { - "x": 2.8599401252574177, - "y": 5.4923098040640115, - "heading": -3.3892541193893154e-18, - "angularVelocity": 2.2152988290333294e-17, - "velocityX": -1.3711480328319254, - "velocityY": 0.38811190600159495, - "timestamp": 2.1254743837019587 - }, - { - "x": 2.779517497378913, - "y": 5.513974137504595, - "heading": -1.420472445548671e-18, - "angularVelocity": 2.695463024933353e-17, - "velocityX": -1.1010678466539927, - "velocityY": 0.2966068331745371, - "timestamp": 2.1985149580161556 - }, - { - "x": 2.7190126242063695, - "y": 5.529547456775984, - "heading": 8.795935357239642e-19, - "angularVelocity": 3.149025049250684e-17, - "velocityX": -0.828373458733645, - "velocityY": 0.21321463334061347, - "timestamp": 2.2715555323303525 - }, - { - "x": 2.6785708390344105, - "y": 5.539527036788551, - "heading": 2.892310957635007e-19, - "angularVelocity": -8.082664265777e-18, - "velocityX": -0.5536893096978018, - "velocityY": 0.1366306344969068, - "timestamp": 2.3445961066445493 + "heading": -1.5610631193350687e-18, + "angularVelocity": 2.5744186155726097e-17, + "velocityX": -3.3579422894520032, + "velocityY": 1.6997125365941679, + "timestamp": 1.4891807113463535 + }, + { + "x": 3.941206853693096, + "y": 5.086799964367495, + "heading": 3.827307300721309e-18, + "angularVelocity": 7.831135017939884e-17, + "velocityX": -3.2887955452772286, + "velocityY": 1.4075914407764616, + "timestamp": 1.557987728563047 + }, + { + "x": 3.730310577999757, + "y": 5.1698821724733195, + "heading": 7.528976478562542e-18, + "angularVelocity": 5.37978439929115e-17, + "velocityX": -3.0650402273530597, + "velocityY": 1.2074670791813904, + "timestamp": 1.6267947457797405 + }, + { + "x": 3.5370931608840936, + "y": 5.242282386882321, + "heading": 5.4493611354862e-18, + "angularVelocity": -3.022388452804216e-17, + "velocityX": -2.808106279439004, + "velocityY": 1.0522213770870446, + "timestamp": 1.695601762996434 + }, + { + "x": 3.3623805585841375, + "y": 5.305505508408588, + "heading": 3.90044719086442e-18, + "angularVelocity": -2.2510988083436363e-17, + "velocityX": -2.5391683779829535, + "velocityY": 0.9188470025834666, + "timestamp": 1.7644087802131274 + }, + { + "x": 3.2065789543659307, + "y": 5.3604207146722205, + "heading": 2.734702057316729e-18, + "angularVelocity": -1.6942241950067733e-17, + "velocityX": -2.2643272520816113, + "velocityY": 0.7981047353162919, + "timestamp": 1.8332157974298209 + }, + { + "x": 3.0699254867036734, + "y": 5.407590197311926, + "heading": 1.8584817371846494e-18, + "angularVelocity": -1.2734461622898273e-17, + "velocityX": -1.9860396975485235, + "velocityY": 0.6855330247953545, + "timestamp": 1.9020228146465143 + }, + { + "x": 2.952574245770292, + "y": 5.4474063504067525, + "heading": 1.2072006114656177e-18, + "angularVelocity": -9.465330021034912e-18, + "velocityX": -1.7055126886812788, + "velocityY": 0.5786641349302285, + "timestamp": 1.9708298318632078 + }, + { + "x": 2.8546328911186802, + "y": 5.480158244239293, + "heading": 7.336690244970979e-19, + "angularVelocity": -6.882024626605053e-18, + "velocityX": -1.4234210203178261, + "velocityY": 0.4759964195133704, + "timestamp": 2.0396368490799013 + }, + { + "x": 2.7761806703190945, + "y": 5.5060675412854945, + "heading": 4.019181330238403e-19, + "angularVelocity": -4.821468868916063e-18, + "velocityX": -1.1401776152062642, + "velocityY": 0.37655021383364773, + "timestamp": 2.1084438662965947 + }, + { + "x": 2.7172782497291417, + "y": 5.525309545004062, + "heading": 1.8363150875881817e-19, + "angularVelocity": -3.1724471295939874e-18, + "velocityX": -0.856052521568438, + "velocityY": 0.2796517636852234, + "timestamp": 2.177250883513288 + }, + { + "x": 2.6779735090707537, + "y": 5.538026332588319, + "heading": 5.594227372773024e-20, + "angularVelocity": -1.8557589065219566e-18, + "velocityX": -0.5712315727130863, + "velocityY": 0.18481817841642284, + "timestamp": 2.246057900729981 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -8.28795440431569e-39, - "angularVelocity": -3.959868860281201e-18, - "velocityX": -0.2774577154251034, - "velocityY": 0.06583092414052888, - "timestamp": 2.417636680958746 + "heading": -4.379352373324341e-45, + "angularVelocity": -8.130315190317274e-19, + "velocityX": -0.2858478933472287, + "velocityY": 0.09169170474607218, + "timestamp": 2.3148649179466743 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": 1.1194999943590282e-38, - "angularVelocity": 2.6674152729925026e-37, - "velocityX": 1.474313292548536e-35, - "velocityY": 1.9984936751410376e-34, - "timestamp": 2.490677255272943 + "heading": -7.265203918119814e-45, + "angularVelocity": -4.19412391684243e-44, + "velocityX": 0, + "velocityY": -6.607621745560535e-42, + "timestamp": 2.3836719351633677 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.5.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.5.traj index 9fd96be2..5c65b031 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.5.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.5.traj @@ -3,398 +3,380 @@ { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": 1.1194999943590282e-38, - "angularVelocity": 2.6674152729925026e-37, - "velocityX": 1.474313292548536e-35, - "velocityY": 1.9984936751410376e-34, + "heading": -7.265203918119814e-45, + "angularVelocity": -4.19412391684243e-44, + "velocityX": 0, + "velocityY": -6.607621745560535e-42, "timestamp": 0 }, { - "x": 2.6757817592371826, - "y": 5.540525112473874, - "heading": -2.263521696805234e-19, - "angularVelocity": -3.3440822564023192e-18, - "velocityX": 0.258195705536965, - "velocityY": -0.056291922762700355, - "timestamp": 0.06768738096933724 - }, - { - "x": 2.710655164921984, - "y": 5.532555521074409, - "heading": -6.817251938952584e-19, - "angularVelocity": -6.727591135839588e-18, - "velocityX": 0.5152128090256508, - "velocityY": -0.11774116955531802, - "timestamp": 0.13537476193867448 - }, - { - "x": 2.762825274578906, - "y": 5.520028158351496, - "heading": -1.3690954482745913e-18, - "angularVelocity": -1.01550724010861e-17, - "velocityX": 0.7707508978749663, - "velocityY": -0.18507678305040806, - "timestamp": 0.20306214290801172 - }, - { - "x": 2.832164805136809, - "y": 5.502484798756891, - "heading": -2.291792592298148e-18, - "angularVelocity": -1.3631745397966465e-17, - "velocityX": 1.024408531766262, - "velocityY": -0.25918213030803783, - "timestamp": 0.27074952387734896 - }, - { - "x": 2.9185095362845113, - "y": 5.479394153237709, - "heading": -3.2554537025043674e-18, - "angularVelocity": -1.4236938945256572e-17, - "velocityX": 1.2756400072092755, - "velocityY": -0.34113663711766645, - "timestamp": 0.3384369048466862 - }, - { - "x": 3.021644093219496, - "y": 5.4501349612076035, - "heading": -4.462356984669075e-18, - "angularVelocity": -1.7830550759992628e-17, - "velocityX": 1.5236895778506316, - "velocityY": -0.4322695251476627, - "timestamp": 0.40612428581602344 - }, - { - "x": 3.1412808309812497, - "y": 5.413974520765914, - "heading": -5.917171487112631e-18, - "angularVelocity": -2.149314217260227e-17, - "velocityX": 1.767489538647525, - "velocityY": -0.5342272063690919, - "timestamp": 0.4738116667853607 - }, - { - "x": 3.2770278679497706, - "y": 5.3700418577232005, - "heading": -2.5772013184954745e-18, - "angularVelocity": 4.9344059717056357e-17, - "velocityX": 2.0054999177766235, - "velocityY": -0.6490524882712698, - "timestamp": 0.5414990477546979 - }, - { - "x": 3.428339922513116, - "y": 5.317295603371641, - "heading": 5.041855021881816e-19, - "angularVelocity": 4.552380039793498e-17, - "velocityX": 2.2354544140493458, - "velocityY": -0.7792627458204253, - "timestamp": 0.6091864287240352 - }, - { - "x": 3.594442154384912, - "y": 5.254489538104451, - "heading": 3.3214379722263567e-18, - "angularVelocity": 4.1621531660920524e-17, - "velocityX": 2.453961572941347, - "velocityY": -0.9278844057453193, - "timestamp": 0.6768738096933724 - }, - { - "x": 3.774213631656669, - "y": 5.18014653899489, - "heading": 5.8698404540777154e-18, - "angularVelocity": 3.764959502608201e-17, - "velocityX": 2.6559083051712964, - "velocityY": -1.0983287880977073, - "timestamp": 0.7445611906627096 - }, - { - "x": 3.966018710550382, - "y": 5.092569435745629, - "heading": 8.147436412478004e-18, - "angularVelocity": 3.364875292561275e-17, - "velocityX": 2.8336903592207485, - "velocityY": -1.2938468292772856, - "timestamp": 0.8122485716320469 + "x": 2.6751665057839835, + "y": 5.538944468629318, + "heading": 4.149201789004472e-19, + "angularVelocity": 6.513823637389646e-18, + "velocityX": 0.2647058041795455, + "velocityY": -0.08463157955674916, + "timestamp": 0.06369840542178373 + }, + { + "x": 2.7088624712986666, + "y": 5.5280798374619815, + "heading": 1.747454129766702e-18, + "angularVelocity": 2.0919423995667872e-17, + "velocityX": 0.5289922925316967, + "velocityY": -0.1705636286402431, + "timestamp": 0.12739681084356747 + }, + { + "x": 2.7593613029745305, + "y": 5.511644735540107, + "heading": 3.0764256336987273e-18, + "angularVelocity": 2.086349721209079e-17, + "velocityX": 0.79278015425161, + "velocityY": -0.2580143382404696, + "timestamp": 0.1910952162653512 + }, + { + "x": 2.8266246280660527, + "y": 5.489524723354374, + "heading": 4.563957990922327e-18, + "angularVelocity": 2.3352740894748934e-17, + "velocityX": 1.0559656030026554, + "velocityY": -0.3472616314217525, + "timestamp": 0.25479362168713493 + }, + { + "x": 2.9106051995536757, + "y": 5.461582331644454, + "heading": 6.988237520643839e-18, + "angularVelocity": 3.8058716127490844e-17, + "velocityX": 1.31840932173323, + "velocityY": -0.43866705178720633, + "timestamp": 0.31849202710891866 + }, + { + "x": 3.0112434875257597, + "y": 5.427649372766819, + "heading": 9.422486137058152e-18, + "angularVelocity": 3.821522062123431e-17, + "velocityX": 1.5799184815647955, + "velocityY": -0.5327128466237975, + "timestamp": 0.3821904325307024 + }, + { + "x": 3.128462311204818, + "y": 5.387515427846556, + "heading": 1.2002270824368104e-17, + "angularVelocity": 4.0499988504071535e-17, + "velocityX": 1.8402159819054258, + "velocityY": -0.6300620031932208, + "timestamp": 0.44588883795248613 + }, + { + "x": 3.2621579049769087, + "y": 5.340909820689257, + "heading": 1.4607724763792015e-17, + "angularVelocity": 4.0902969582545765e-17, + "velocityX": 2.0988844679362657, + "velocityY": -0.7316604999559294, + "timestamp": 0.5095872433742699 + }, + { + "x": 3.4121839694430376, + "y": 5.287471786804592, + "heading": 1.2983801342724242e-17, + "angularVelocity": -2.5493941493744293e-17, + "velocityX": 2.3552562026116504, + "velocityY": -0.8389226312781332, + "timestamp": 0.5732856487960536 + }, + { + "x": 3.5783204640757695, + "y": 5.226697519809262, + "heading": 1.1414351740343512e-17, + "angularVelocity": -2.4638758097451353e-17, + "velocityX": 2.608173525422576, + "velocityY": -0.9540940089929807, + "timestamp": 0.6369840542178373 + }, + { + "x": 3.7602044322338806, + "y": 5.157837260218168, + "heading": 1.0662286375073935e-17, + "angularVelocity": -1.1806659213675993e-17, + "velocityX": 2.855392799140749, + "velocityY": -1.081035845954548, + "timestamp": 0.7006824596396211 + }, + { + "x": 3.9571467965572817, + "y": 5.079670509622147, + "heading": 1.0033604735305209e-17, + "angularVelocity": -9.869660560666429e-18, + "velocityX": 3.091794261085985, + "velocityY": -1.2271382631705257, + "timestamp": 0.7643808650614048 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 1.0158795753511172e-17, - "angularVelocity": 2.971542571519468e-17, - "velocityX": 2.9766309755737, - "velocityY": -1.5161118425338018, - "timestamp": 0.8799359526013841 - }, - { - "x": 4.261627503076192, - "y": 4.9391622522038245, - "heading": 1.103199717945149e-17, - "angularVelocity": 2.8019595592305015e-17, - "velocityX": 3.020426535396691, - "velocityY": -1.629624452324509, - "timestamp": 0.9110999079480617 - }, - { - "x": 4.357244155674493, - "y": 4.884889313220403, - "heading": 1.184832535139553e-17, - "angularVelocity": 2.619462654200126e-17, - "velocityX": 3.068180901128632, - "velocityY": -1.7415292243770963, - "timestamp": 0.9422638632947393 - }, - { - "x": 4.454522517564495, - "y": 4.827208233471932, - "heading": 1.2602189603982203e-17, - "angularVelocity": 2.4190262248102175e-17, - "velocityX": 3.1215024154618987, - "velocityY": -1.8508908483152517, - "timestamp": 0.9734278186414169 - }, - { - "x": 4.553721568715114, - "y": 4.766257957931852, - "heading": 1.3285416566033512e-17, - "angularVelocity": 2.1923627934667792e-17, - "velocityX": 3.18313417045742, - "velocityY": -1.9557939568983074, - "timestamp": 1.0045917739880945 - }, - { - "x": 4.6552566098247095, - "y": 4.702321075934725, - "heading": 1.3885398069122134e-17, - "angularVelocity": 1.9252418261752656e-17, - "velocityX": 3.258092240862523, - "velocityY": -2.0516292391602087, - "timestamp": 1.0357557293347721 - }, - { - "x": 4.7598200255019725, - "y": 4.636102620452092, - "heading": 1.4382811423998524e-17, - "angularVelocity": 1.5961175314853973e-17, - "velocityX": 3.3552677930020853, - "velocityY": -2.124841174555608, - "timestamp": 1.0669196846814497 - }, - { - "x": 4.868161384056622, - "y": 4.569562096793983, - "heading": 1.476085010345509e-17, - "angularVelocity": 1.2130638591480905e-17, - "velocityX": 3.476495757660601, - "velocityY": -2.135175811859933, - "timestamp": 1.0980836400281273 - }, - { - "x": 4.97947280914236, - "y": 4.50537854159068, - "heading": 1.3849367053817892e-17, - "angularVelocity": -2.9247989846367464e-17, - "velocityX": 3.571800300939838, - "velocityY": -2.0595445760752855, - "timestamp": 1.129247595374805 - }, - { - "x": 5.09247899341136, - "y": 4.444586805435104, - "heading": 1.2651254648975303e-17, - "angularVelocity": -3.84454537766135e-17, - "velocityX": 3.6261823318601505, - "velocityY": -1.9507066891641414, - "timestamp": 1.1604115507214825 - }, - { - "x": 5.206458430184718, - "y": 4.387459691402226, - "heading": 1.1425663620436537e-17, - "angularVelocity": -3.932719755362708e-17, - "velocityX": 3.657412401776808, - "velocityY": -1.8331150008841195, - "timestamp": 1.19157550606816 - }, - { - "x": 5.321007172735707, - "y": 4.334081255049934, - "heading": 1.0655255032861655e-17, - "angularVelocity": -2.472114270913484e-17, - "velocityX": 3.675680486533668, - "velocityY": -1.712826108191114, - "timestamp": 1.2227394614148377 - }, - { - "x": 5.435875810085393, - "y": 4.284480962664817, - "heading": 9.880141449219796e-18, - "angularVelocity": -2.4872118283779253e-17, - "velocityX": 3.68594538375666, - "velocityY": -1.5915916908925372, - "timestamp": 1.2539034167615153 - }, - { - "x": 5.550897094649704, - "y": 4.2386692598409, - "heading": 9.106151253635088e-18, - "angularVelocity": -2.483607061124937e-17, - "velocityX": 3.6908435814638634, - "velocityY": -1.470022091685541, - "timestamp": 1.2850673721081929 + "heading": 9.614004929327239e-18, + "angularVelocity": -6.58728900981987e-18, + "velocityX": 3.3023160854502263, + "velocityY": -1.4085550989874989, + "timestamp": 0.8280792704831885 + }, + { + "x": 4.2842075611264425, + "y": 4.936417577245955, + "heading": 8.433110849470374e-18, + "angularVelocity": -3.4077840923717525e-17, + "velocityX": 3.3679341946759727, + "velocityY": -1.5447568972758872, + "timestamp": 0.8627321137340349 + }, + { + "x": 4.401356584826297, + "y": 4.877666966713146, + "heading": 7.494016874909782e-18, + "angularVelocity": -2.7100055477775347e-17, + "velocityX": 3.3806468015288584, + "velocityY": -1.695405196841212, + "timestamp": 0.8973849569848813 + }, + { + "x": 4.516063976001338, + "y": 4.814281195629901, + "heading": 5.449898069000543e-18, + "angularVelocity": -5.898848735476586e-17, + "velocityX": 3.310186998068025, + "velocityY": -1.8291650882556743, + "timestamp": 0.9320378002357277 + }, + { + "x": 4.628146410125642, + "y": 4.746361612927903, + "heading": 3.873819251911276e-18, + "angularVelocity": -4.5481948066433736e-17, + "velocityX": 3.234436877602133, + "velocityY": -1.9600002865663548, + "timestamp": 0.9666906434865741 + }, + { + "x": 4.737424855337868, + "y": 4.674016859733605, + "heading": 3.562150006954802e-18, + "angularVelocity": -8.994045386127452e-18, + "velocityX": 3.1535203163901007, + "velocityY": -2.0877003560892837, + "timestamp": 1.0013434867374205 + }, + { + "x": 4.844483096398323, + "y": 4.598425359059355, + "heading": 4.67240858769656e-18, + "angularVelocity": 3.2039465642249735e-17, + "velocityX": 3.0894504178337554, + "velocityY": -2.181393893916723, + "timestamp": 1.0359963299882669 + }, + { + "x": 4.954476829355994, + "y": 4.5271728465414585, + "heading": 6.725892368510384e-18, + "angularVelocity": 5.925873862496592e-17, + "velocityX": 3.174161847599147, + "velocityY": -2.0561808450207577, + "timestamp": 1.0706491732391132 + }, + { + "x": 5.06723038345343, + "y": 4.460373312794278, + "heading": 8.84315284493419e-18, + "angularVelocity": 6.109918488065446e-17, + "velocityX": 3.2538038302147596, + "velocityY": -1.9276782936288406, + "timestamp": 1.1053020164899596 + }, + { + "x": 5.18256360847748, + "y": 4.398133545147612, + "heading": 1.1174088851587773e-17, + "angularVelocity": 6.726536087617125e-17, + "velocityX": 3.32824709906689, + "velocityY": -1.796094109684512, + "timestamp": 1.139954859740806 + }, + { + "x": 5.300292214676822, + "y": 4.340553023175947, + "heading": 1.351328882085302e-17, + "angularVelocity": 6.750383950696769e-17, + "velocityX": 3.3973721967667427, + "velocityY": -1.6616391779124182, + "timestamp": 1.1746077029916524 + }, + { + "x": 5.420228077668345, + "y": 4.287723772710641, + "heading": 1.5740193529975693e-17, + "angularVelocity": 6.4263260968298e-17, + "velocityX": 3.461068464810413, + "velocityY": -1.5245285959043573, + "timestamp": 1.2092605462424988 + }, + { + "x": 5.542179542504676, + "y": 4.239730223009142, + "heading": 1.83851008704834e-17, + "angularVelocity": 7.632583916308507e-17, + "velocityX": 3.5192340193716003, + "velocityY": -1.3849815830141698, + "timestamp": 1.2439133894933452 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 8.34871718979539e-18, - "angularVelocity": -2.430481161157101e-17, - "velocityX": 3.691913715418776, - "velocityY": -1.3483585385427073, - "timestamp": 1.3162313274548705 - }, - { - "x": 5.935813024971261, - "y": 4.118882926647287, - "heading": 6.616284346114717e-18, - "angularVelocity": -2.3674050338447945e-17, - "velocityX": 3.6877099925560515, - "velocityY": -1.062690370240863, - "timestamp": 1.3894098845255716 - }, - { - "x": 6.200932724790643, - "y": 4.061479044678689, - "heading": 4.986300781220053e-18, - "angularVelocity": -2.2274059919222546e-17, - "velocityX": 3.6229151056263116, - "velocityY": -0.7844358274670198, - "timestamp": 1.4625884415962727 - }, - { - "x": 6.456598704926571, - "y": 4.022722730407595, - "heading": 3.514116805070136e-18, - "angularVelocity": -2.0117696155433974e-17, - "velocityX": 3.4937280860691824, - "velocityY": -0.5296129880457788, - "timestamp": 1.5357669986669737 - }, - { - "x": 6.6986166899486275, - "y": 3.999804245428159, - "heading": 2.243253754519844e-18, - "angularVelocity": -1.7366604392085868e-17, - "velocityX": 3.3072254320105032, - "velocityY": -0.31318580055210804, - "timestamp": 1.6089455557376748 - }, - { - "x": 6.923883976637484, - "y": 3.9893965055012424, - "heading": 1.2009079614811148e-18, - "angularVelocity": -1.42438691719364e-17, - "velocityX": 3.078323701726084, - "velocityY": -0.1422239019670904, - "timestamp": 1.682124112808376 - }, - { - "x": 7.1304235610549185, - "y": 3.9882827335868942, - "heading": 4.0101770388899655e-19, - "angularVelocity": -1.0930664523394714e-17, - "velocityX": 2.822405806907137, - "velocityY": -0.01521992177670267, - "timestamp": 1.755302669879077 - }, - { - "x": 7.317098326514511, - "y": 3.9936876792547547, - "heading": -1.5059558453470426e-19, - "angularVelocity": -7.537908788371176e-18, - "velocityX": 2.5509489792103524, - "velocityY": 0.07385969174875201, - "timestamp": 1.828481226949778 - }, - { - "x": 7.48330233647595, - "y": 4.00334149700279, - "heading": -4.524148120436366e-19, - "angularVelocity": -4.1244216832965696e-18, - "velocityX": 2.2712119043405092, - "velocityY": 0.13192140067354147, - "timestamp": 1.9016597840204792 - }, - { - "x": 7.628740531129745, - "y": 4.015421358409772, - "heading": -5.050368427376048e-19, - "angularVelocity": -7.190908486340082e-19, - "velocityX": 1.987442776622101, - "velocityY": 0.1650737851432527, - "timestamp": 1.9748383410911803 - }, - { - "x": 7.753293848479072, - "y": 4.028463870964023, - "heading": -3.1002864921528455e-19, - "angularVelocity": 2.6648269843554143e-18, - "velocityX": 1.7020466422833307, - "velocityY": 0.17822861062495024, - "timestamp": 2.0480168981618814 - }, - { - "x": 7.856941288758159, - "y": 4.0412847502588685, - "heading": 2.203678311282748e-19, - "angularVelocity": 7.247976752183959e-18, - "velocityX": 1.4163635418357239, - "velocityY": 0.17519994665183636, - "timestamp": 2.1211954552325825 - }, - { - "x": 7.939715627162556, - "y": 4.052914641548701, - "heading": 3.4450417763013896e-19, - "angularVelocity": 1.6963486504629427e-18, - "velocityX": 1.1311283211614112, - "velocityY": 0.15892485114999483, - "timestamp": 2.1943740123032835 - }, - { - "x": 8.00167823825641, - "y": 4.062550327487905, - "heading": 7.102203419824833e-19, - "angularVelocity": 4.997586438339274e-18, - "velocityX": 0.8467317965013967, - "velocityY": 0.1316736257848719, - "timestamp": 2.2675525693739846 - }, - { - "x": 8.042904705550558, - "y": 4.0695182051166965, - "heading": 2.3618432486443475e-19, - "angularVelocity": -6.477799455470437e-18, - "velocityX": 0.5633681360281035, - "velocityY": 0.09521747773817751, - "timestamp": 2.3407311264446857 + "heading": 2.1337719863838943e-17, + "angularVelocity": 8.520567769813293e-17, + "velocityX": 3.571775782441806, + "velocityY": -1.2432211735943428, + "timestamp": 1.2785662327441916 + }, + { + "x": 5.936838928321791, + "y": 4.127579524293423, + "heading": 2.7128761029914745e-17, + "angularVelocity": 7.834427123009397e-17, + "velocityX": 3.6647054686449687, + "velocityY": -0.9344094480089555, + "timestamp": 1.3524840964903202 + }, + { + "x": 6.212625572925942, + "y": 4.081838891930782, + "heading": 3.048119806681363e-17, + "angularVelocity": 4.535354333849354e-17, + "velocityX": 3.730987756239054, + "velocityY": -0.6188034941017404, + "timestamp": 1.4264019602364488 + }, + { + "x": 6.491306353239061, + "y": 4.059759739930169, + "heading": 2.990728340154358e-17, + "angularVelocity": -7.764221477519524e-18, + "velocityX": 3.770141156544387, + "velocityY": -0.2986984590956799, + "timestamp": 1.5003198239825775 + }, + { + "x": 6.752875972710039, + "y": 4.054277278014817, + "heading": 2.289191300833474e-17, + "angularVelocity": -9.490764529266178e-17, + "velocityX": 3.538652312373937, + "velocityY": -0.07416964773468503, + "timestamp": 1.574237687728706 + }, + { + "x": 6.991092096287726, + "y": 4.05357587432536, + "heading": 1.9322766501808003e-17, + "angularVelocity": -4.8285303790501985e-17, + "velocityX": 3.2227138543376115, + "velocityY": -0.009488960501710962, + "timestamp": 1.6481555514748347 + }, + { + "x": 7.205567187241114, + "y": 4.0550211409111485, + "heading": 1.4185957310010018e-17, + "angularVelocity": -6.949347466859182e-17, + "velocityX": 2.9015325942048964, + "velocityY": 0.01955233163599203, + "timestamp": 1.7220734152209634 + }, + { + "x": 7.396227777452996, + "y": 4.057522153925314, + "heading": 7.96211441465592e-18, + "angularVelocity": -8.419944219072616e-17, + "velocityX": 2.579357418481523, + "velocityY": 0.03383502833300578, + "timestamp": 1.795991278967092 + }, + { + "x": 7.5630549716186115, + "y": 4.0604862007272615, + "heading": 9.124881669040973e-19, + "angularVelocity": -9.537107663127069e-17, + "velocityX": 2.2569266170703433, + "velocityY": 0.040099194588843756, + "timestamp": 1.8699091427132206 + }, + { + "x": 7.70604444565388, + "y": 4.0635415850381555, + "heading": -6.759455196513469e-18, + "angularVelocity": -1.0379011208666613e-16, + "velocityX": 1.934437317160139, + "velocityY": 0.04133485677276264, + "timestamp": 1.9438270064593492 + }, + { + "x": 7.825196583277639, + "y": 4.066433602906457, + "heading": -4.535875150648457e-18, + "angularVelocity": 3.0081768238080076e-17, + "velocityX": 1.6119532084015247, + "velocityY": 0.03912474903542123, + "timestamp": 2.017744870205478 + }, + { + "x": 7.920513375065405, + "y": 4.068976876297905, + "heading": -2.8949605954376388e-18, + "angularVelocity": 2.219916096123338e-17, + "velocityX": 1.2894960292025357, + "velocityY": 0.03440674909358656, + "timestamp": 2.0916627339516065 + }, + { + "x": 7.991997299424134, + "y": 4.071030463180751, + "heading": -1.5603341535814708e-18, + "angularVelocity": 1.8055533185319825e-17, + "velocityX": 0.9670723792051263, + "velocityY": 0.02778201071799316, + "timestamp": 2.165580597697735 + }, + { + "x": 8.03965088919926, + "y": 4.07248360094205, + "heading": -5.821232156931401e-19, + "angularVelocity": 1.3233755526918469e-17, + "velocityX": 0.6446829948819025, + "velocityY": 0.019658817066051913, + "timestamp": 2.2394984614438638 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 2.0025514182915097e-40, - "angularVelocity": -3.2275072689602567e-18, - "velocityX": 0.2811186469496163, - "velocityY": 0.05095414427594005, - "timestamp": 2.413909683515387 + "heading": 8.014500942600024e-44, + "angularVelocity": 7.875271094040935e-18, + "velocityX": 0.322326324020543, + "velocityY": 0.0103270696803873, + "timestamp": 2.3134163251899924 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 3.2506461802069203e-40, - "angularVelocity": 1.7055464658974957e-39, - "velocityX": 1.2054870824295223e-36, - "velocityY": -6.907165459298352e-35, - "timestamp": 2.487088240586088 + "heading": -7.373298274013602e-44, + "angularVelocity": -1.7505506389116263e-42, + "velocityX": -2.259320082617454e-45, + "velocityY": 0, + "timestamp": 2.387334188936121 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.traj index b5078150..19f29638 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.traj @@ -3,1640 +3,1550 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 2.598230881890789e-31, - "angularVelocity": 0, - "velocityX": -2.3904076278416207e-31, - "velocityY": 5.789752915577876e-30, + "heading": 0, + "angularVelocity": 2.3560143253921187e-30, + "velocityX": 0, + "velocityY": 0, "timestamp": 0 }, { - "x": 1.3043755674296562, - "y": 5.57443357397875, - "heading": -0.009496998569604356, - "angularVelocity": -0.1264048181794536, - "velocityX": 0.191678505815799, - "velocityY": -0.2198905081992682, - "timestamp": 0.07513161845108943 - }, - { - "x": 1.3331814181481576, - "y": 5.541395353902405, - "heading": -0.028483550113124877, - "angularVelocity": -0.2527105356630103, - "velocityX": 0.38340516698036353, - "velocityY": -0.4397378994111958, - "timestamp": 0.15026323690217885 - }, - { - "x": 1.376396862320767, - "y": 5.49184373452604, - "heading": -0.056940765199465976, - "angularVelocity": -0.378764835281948, - "velocityX": 0.5751965026490216, - "velocityY": -0.6595308393324731, - "timestamp": 0.22539485535326828 - }, - { - "x": 1.4340285202431444, - "y": 5.425784148354236, - "heading": -0.09483975957151446, - "angularVelocity": -0.504434686147033, - "velocityX": 0.7670759542900392, - "velocityY": -0.8792514727514934, - "timestamp": 0.3005264738043577 - }, - { - "x": 1.5060856912258154, - "y": 5.34322422127702, - "heading": -0.1421437039065203, - "angularVelocity": -0.6296143396295107, - "velocityX": 0.9590791795253663, - "velocityY": -1.0988706057618698, - "timestamp": 0.3756580922554471 - }, - { - "x": 1.5925821714791422, - "y": 5.244175418310136, - "heading": -0.1988101671423871, - "angularVelocity": -0.7542292367162176, - "velocityX": 1.1512660319811892, - "velocityY": -1.318337139694803, - "timestamp": 0.4507897107065365 - }, - { - "x": 1.6935404980129676, - "y": 5.128656797964587, - "heading": -0.26479264404095926, - "angularVelocity": -0.8782251502241449, - "velocityX": 1.343752851483489, - "velocityY": -1.5375500053152549, - "timestamp": 0.5259213291576259 - }, - { - "x": 1.80900475066288, - "y": 4.99670620601025, - "heading": -0.34003720346134453, - "angularVelocity": -1.001503241571098, - "velocityX": 1.5368263725127875, - "velocityY": -1.7562591446699305, - "timestamp": 0.6010529476087153 - }, - { - "x": 1.9391049044022353, - "y": 4.8484368766348975, - "heading": -0.42444837010558617, - "angularVelocity": -1.1235105590497523, - "velocityX": 1.7316298572389015, - "velocityY": -1.9734611397719364, - "timestamp": 0.6761845660598047 - }, - { - "x": 2.0768333663443252, - "y": 4.720578669009404, - "heading": -0.4949304356074761, - "angularVelocity": -0.938114564170018, - "velocityX": 1.8331624525981682, - "velocityY": -1.7017896094422644, - "timestamp": 0.7513161845108941 - }, - { - "x": 2.2004381051303543, - "y": 4.609474436663166, - "heading": -0.5559710621989035, - "angularVelocity": -0.8124492437076775, - "velocityX": 1.6451760435605478, - "velocityY": -1.4787946091584414, - "timestamp": 0.8264478029619835 - }, - { - "x": 2.309790549713842, - "y": 4.515017160876509, - "heading": -0.6076423779450478, - "angularVelocity": -0.687743946017975, - "velocityX": 1.4554783570583396, - "velocityY": -1.2572240253997977, - "timestamp": 0.9015794214130729 - }, - { - "x": 2.404847575381414, - "y": 4.437170463185578, - "heading": -0.6499678697321144, - "angularVelocity": -0.5633512582056228, - "velocityX": 1.2652066816823961, - "velocityY": -1.0361376380998464, - "timestamp": 0.9767110398641623 - }, - { - "x": 2.4855875270959125, - "y": 4.375915949453414, - "heading": -0.6829583691226987, - "angularVelocity": -0.43910273822475193, - "velocityX": 1.0746467782947702, - "velocityY": -0.8152960763101967, - "timestamp": 1.0518426583152518 - }, - { - "x": 2.5519973911227014, - "y": 4.331242517358978, - "heading": -0.7066196510488874, - "angularVelocity": -0.31493108244769824, - "velocityX": 0.8839136624193856, - "velocityY": -0.5946022861403544, - "timestamp": 1.1269742767663413 - }, - { - "x": 2.604068513564513, - "y": 4.303142764963195, - "heading": -0.7209550318751375, - "angularVelocity": -0.1908035673998086, - "velocityX": 0.693065363384342, - "velocityY": -0.3740070155974736, - "timestamp": 1.2021058952174308 - }, - { - "x": 2.6417947649723117, - "y": 4.2916114443192095, - "heading": -0.7259664734930259, - "angularVelocity": -0.06670216508038034, - "velocityX": 0.5021354815334699, - "velocityY": -0.15348159509809242, - "timestamp": 1.2772375136685203 + "x": 1.3060574270397207, + "y": 5.572382522338357, + "heading": -0.010694879272561806, + "angularVelocity": -0.14212355715817127, + "velocityX": 0.21372562484439114, + "velocityY": -0.24679919880328743, + "timestamp": 0.07525057412377358 + }, + { + "x": 1.3382235764672503, + "y": 5.535239265365289, + "heading": -0.0320761034546414, + "angularVelocity": -0.2841337017193578, + "velocityX": 0.42745387396382534, + "velocityY": -0.49359433341007053, + "timestamp": 0.15050114824754715 + }, + { + "x": 1.3864734146735522, + "y": 5.47952484840506, + "heading": -0.06412157549775398, + "angularVelocity": -0.42585020003622126, + "velocityX": 0.6411889712155109, + "velocityY": -0.7403852742575773, + "timestamp": 0.22575172237132074 + }, + { + "x": 1.4508078083146234, + "y": 5.405239688642535, + "heading": -0.10679814197023921, + "angularVelocity": -0.5671261245497303, + "velocityX": 0.8549355854022526, + "velocityY": -0.9871706711626929, + "timestamp": 0.3010022964950943 + }, + { + "x": 1.5312280128772082, + "y": 5.3123844280164345, + "heading": -0.16006528510741636, + "angularVelocity": -0.7078636111159992, + "velocityX": 1.0686988836704838, + "velocityY": -1.2339475373861764, + "timestamp": 0.37625287061886786 + }, + { + "x": 1.6277357374217547, + "y": 5.200960102506698, + "heading": -0.22387989152223717, + "angularVelocity": -0.8480281666778853, + "velocityX": 1.2824848935314492, + "velocityY": -1.4807106365400295, + "timestamp": 0.4515034447426414 + }, + { + "x": 1.7403333706916029, + "y": 5.070968456330507, + "heading": -0.29820152963685287, + "angularVelocity": -0.9876554296244249, + "velocityX": 1.496302647246288, + "velocityY": -1.7274505569061047, + "timestamp": 0.526754018866415 + }, + { + "x": 1.8690253649035526, + "y": 4.922413220845796, + "heading": -0.38299660252732504, + "angularVelocity": -1.1268362252301867, + "velocityX": 1.7101795661770514, + "velocityY": -1.9741408915022984, + "timestamp": 0.6020045929901886 + }, + { + "x": 2.021841436297539, + "y": 4.7786139697645345, + "heading": -0.46243583568438273, + "angularVelocity": -1.055662818268618, + "velocityX": 2.0307628636084356, + "velocityY": -1.9109389231128335, + "timestamp": 0.6772551671139622 + }, + { + "x": 2.158572983780592, + "y": 4.6533861744648695, + "heading": -0.5314192950572162, + "angularVelocity": -0.9167167184470773, + "velocityX": 1.8170166683292757, + "velocityY": -1.6641440514810064, + "timestamp": 0.7525057412377357 + }, + { + "x": 2.2792163174605955, + "y": 4.5467264938443535, + "heading": -0.5899522434559231, + "angularVelocity": -0.777840555764763, + "velocityX": 1.6032214383174732, + "velocityY": -1.4173935795391805, + "timestamp": 0.8277563153615093 + }, + { + "x": 2.3837699374210346, + "y": 4.458633436191598, + "heading": -0.6380354100231267, + "angularVelocity": -0.6389740826105205, + "velocityX": 1.3894062760138433, + "velocityY": -1.1706629308520584, + "timestamp": 0.9030068894852828 + }, + { + "x": 2.472232988532522, + "y": 4.389106089275013, + "heading": -0.6756678010096412, + "angularVelocity": -0.5000944035873048, + "velocityX": 1.1755797499519767, + "velocityY": -0.9239444047507308, + "timestamp": 0.9782574636090564 + }, + { + "x": 2.5446049348789113, + "y": 4.338143851138861, + "heading": -0.7028477866363936, + "angularVelocity": -0.36119306653537947, + "velocityX": 0.9617461021356389, + "velocityY": -0.6772338779998112, + "timestamp": 1.05350803773283 + }, + { + "x": 2.6008854472033804, + "y": 4.305746331042048, + "heading": -0.7195735501934102, + "angularVelocity": -0.222267587342223, + "velocityX": 0.7479080788470353, + "velocityY": -0.43052854378169836, + "timestamp": 1.1287586118566038 + }, + { + "x": 2.6410743549129108, + "y": 4.291913303700176, + "heading": -0.7258432586618085, + "angularVelocity": -0.0833177492798597, + "velocityX": 0.5340677885590666, + "velocityY": -0.18382620335314903, + "timestamp": 1.2040091859803774 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.057384074600866586, - "velocityX": 0.311145410428376, - "velocityY": 0.06699234541878851, - "timestamp": 1.3523691321196099 - }, - { - "x": 2.674170692930372, - "y": 4.3184165536581745, - "heading": -0.7079150841905615, - "angularVelocity": 0.18203686942568226, - "velocityX": 0.1192255306502172, - "velocityY": 0.28844784676769203, - "timestamp": 1.4278485168598245 - }, - { - "x": 2.6686789979466896, - "y": 4.3568998584598715, - "heading": -0.6847718369181163, - "angularVelocity": 0.3066167980947624, - "velocityX": -0.07275754835791173, - "velocityY": 0.5098518613031996, - "timestamp": 1.5033279016000392 - }, - { - "x": 2.648690220373955, - "y": 4.4120894363660135, - "heading": -0.6522316135776112, - "angularVelocity": 0.4311140512244905, - "velocityX": -0.26482433108237524, - "velocityY": 0.7311874373940757, - "timestamp": 1.5788072863402538 - }, - { - "x": 2.614195529876659, - "y": 4.483978019553035, - "heading": -0.6103014306892409, - "angularVelocity": 0.5555183449290733, - "velocityX": -0.4570081038377115, - "velocityY": 0.9524267246290559, - "timestamp": 1.6542866710804685 - }, - { - "x": 2.565181655781624, - "y": 4.572554576780094, - "heading": -0.5589890537241904, - "angularVelocity": 0.6798197566183578, - "velocityX": -0.6493676951067825, - "velocityY": 1.1735198628071262, - "timestamp": 1.729766055820683 - }, - { - "x": 2.5016263830570615, - "y": 4.677800410068182, - "heading": -0.49830336002198045, - "angularVelocity": 0.8040035555369023, - "velocityX": -0.8420216055727019, - "velocityY": 1.3943652779921303, - "timestamp": 1.8052454405608978 - }, - { - "x": 2.4234850482103365, - "y": 4.799677415452469, - "heading": -0.42825793280914776, - "angularVelocity": 0.9280073950309173, - "velocityX": -1.0352672470523452, - "velocityY": 1.6147058670562038, - "timestamp": 1.8807248253011124 - }, - { - "x": 2.3306237160215493, - "y": 4.938069451098362, - "heading": -0.34890039707276027, - "angularVelocity": 1.0513802677046542, - "velocityX": -1.2302873494790678, - "velocityY": 1.8335077335201588, - "timestamp": 1.956204210041327 - }, - { - "x": 2.2158437334233896, - "y": 5.072717873356149, - "heading": -0.27145428242562397, - "angularVelocity": 1.0260565175055525, - "velocityX": -1.5206798914447346, - "velocityY": 1.7839099076626925, - "timestamp": 2.031683594781542 - }, - { - "x": 2.115226856826332, - "y": 5.190380914478299, - "heading": -0.20366940333084457, - "angularVelocity": 0.8980581827763024, - "velocityX": -1.3330378478809115, - "velocityY": 1.5588765267548796, - "timestamp": 2.107162979521757 - }, - { - "x": 2.0289140508238517, - "y": 5.291175476770988, - "heading": -0.14552121115180353, - "angularVelocity": 0.770385084361868, - "velocityX": -1.1435282136547764, - "velocityY": 1.33539194366953, - "timestamp": 2.1826423642619717 - }, - { - "x": 1.956951239598818, - "y": 5.375140495871239, - "heading": -0.09702855670668947, - "angularVelocity": 0.6424622380441027, - "velocityX": -0.953410145970343, - "velocityY": 1.1124232052394887, - "timestamp": 2.2581217490021865 - }, - { - "x": 1.8993610596021686, - "y": 5.4422954705446385, - "heading": -0.05821541752658778, - "angularVelocity": 0.5142217217941352, - "velocityX": -0.7629921758539466, - "velocityY": 0.8897127991440643, - "timestamp": 2.3336011337424014 - }, - { - "x": 1.8561570206808402, - "y": 5.492652117133405, - "heading": -0.029101140919542866, - "angularVelocity": 0.3857248798192106, - "velocityX": -0.5723952184942321, - "velocityY": 0.6671576187743994, - "timestamp": 2.4090805184826163 - }, - { - "x": 1.827348191495371, - "y": 5.5262182202265375, - "heading": -0.009696189627478835, - "angularVelocity": 0.25708942063193824, - "velocityX": -0.3816781136185715, - "velocityY": 0.44470557370534397, - "timestamp": 2.484559903222831 + "angularVelocity": 0.05565596827444861, + "velocityX": 0.3202270361150376, + "velocityY": 0.06287505455011895, + "timestamp": 1.279259760104151 + }, + { + "x": 2.6731093005797457, + "y": 4.320288364477002, + "heading": -0.7067987753568107, + "angularVelocity": 0.19580392065911026, + "velocityX": 0.10461717847139641, + "velocityY": 0.3116194636271141, + "timestamp": 1.3551333161029784 + }, + { + "x": 2.664687962821916, + "y": 4.362805446311989, + "heading": -0.6813130877895294, + "angularVelocity": 0.3358968382514228, + "velocityX": -0.11099173681211887, + "velocityY": 0.5603675915105346, + "timestamp": 1.4310068721018057 + }, + { + "x": 2.6399077578590466, + "y": 4.424196334908807, + "heading": -0.645202192832585, + "angularVelocity": 0.47593518560166614, + "velocityX": -0.32659870276641295, + "velocityY": 0.8091210144040187, + "timestamp": 1.506880428100633 + }, + { + "x": 2.5987689088828363, + "y": 4.504461553370171, + "heading": -0.5984695308975294, + "angularVelocity": 0.6159281889382251, + "velocityX": -0.5422027270940676, + "velocityY": 1.0578813316982019, + "timestamp": 1.5827539840994602 + }, + { + "x": 2.541271709317337, + "y": 4.60360172553535, + "heading": -0.5411163068190713, + "angularVelocity": 0.7559053127670017, + "velocityX": -0.7578028841452508, + "velocityY": 1.3066498710827958, + "timestamp": 1.6586275400982875 + }, + { + "x": 2.4674164806885255, + "y": 4.721617486978178, + "heading": -0.47313850163583826, + "angularVelocity": 0.8959354057929544, + "velocityX": -0.9733988035419044, + "velocityY": 1.5554267872170555, + "timestamp": 1.7345010960971148 + }, + { + "x": 2.377203340708699, + "y": 4.858509175719519, + "heading": -0.39452181655964974, + "angularVelocity": 1.0361539543083473, + "velocityX": -1.188993171512905, + "velocityY": 1.804208158404987, + "timestamp": 1.810374652095942 + }, + { + "x": 2.270630715712783, + "y": 5.014275370945325, + "heading": -0.30523452580590593, + "angularVelocity": 1.1767906430312367, + "velocityX": -1.4046082801148865, + "velocityY": 2.052970803512055, + "timestamp": 1.8862482080947693 + }, + { + "x": 2.156210238547871, + "y": 5.1464603718429265, + "heading": -0.22910381899713708, + "angularVelocity": 1.0033892020445663, + "velocityX": -1.508041578600413, + "velocityY": 1.7421748481347223, + "timestamp": 1.9621217640935966 + }, + { + "x": 2.0581337448378676, + "y": 5.259758714146455, + "heading": -0.16374376524716946, + "angularVelocity": 0.8614339065879271, + "velocityX": -1.2926307778416481, + "velocityY": 1.4932520403643688, + "timestamp": 2.0379953200924237 + }, + { + "x": 1.9764031908936424, + "y": 5.354172852013204, + "heading": -0.10920543384793971, + "angularVelocity": 0.7188055269306859, + "velocityX": -1.077194193248253, + "velocityY": 1.24436157795548, + "timestamp": 2.1138688760912507 + }, + { + "x": 1.9110187094739324, + "y": 5.42970361761196, + "heading": -0.0655341593932664, + "angularVelocity": 0.5755796453747686, + "velocityX": -0.8617558589167408, + "velocityY": 0.995482083384748, + "timestamp": 2.189742432090078 + }, + { + "x": 1.8619802018876241, + "y": 5.486351463195586, + "heading": -0.03276467101544991, + "angularVelocity": 0.4318960400201182, + "velocityX": -0.646318825315813, + "velocityY": 0.7466085494271616, + "timestamp": 2.265615988088905 + }, + { + "x": 1.8292876471115809, + "y": 5.524116647521472, + "heading": -0.010918058172762177, + "angularVelocity": 0.2879344793491674, + "velocityX": -0.43088206879563057, + "velocityY": 0.4977384258464482, + "timestamp": 2.341489544087732 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -6.802256144415086e-28, - "angularVelocity": 0.12846142905206573, - "velocityX": -0.19087328234026563, - "velocityY": 0.22232623397342613, - "timestamp": 2.560039287963046 + "heading": 2.8937989389363637e-30, + "angularVelocity": 0.14389806868982188, + "velocityX": -0.21544335593050407, + "velocityY": 0.24886958055725772, + "timestamp": 2.417363100086559 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -6.934045530869177e-28, - "angularVelocity": -1.67846607717031e-28, - "velocityX": 1.7578194682085736e-30, - "velocityY": -2.585635481662347e-29, - "timestamp": 2.635518672703261 - }, - { - "x": 1.8467557558007974, - "y": 5.543052711491933, - "heading": 1.9216741877895313e-20, - "angularVelocity": 2.064858009548948e-19, - "velocityX": 0.36334081801614104, - "velocityY": 0.0005742601437372874, - "timestamp": 2.728584357986845 - }, - { - "x": 1.9143848790849516, - "y": 5.543159599317722, - "heading": 3.7322144176989406e-20, - "angularVelocity": 1.945443397736918e-19, - "velocityX": 0.7266816236089446, - "velocityY": 0.0011485202678394907, - "timestamp": 2.8216500432704295 - }, - { - "x": 2.0158285613519538, - "y": 5.543319931052202, - "heading": 3.5705254993467834e-20, - "angularVelocity": -1.7373634318302704e-20, - "velocityX": 1.090022406839742, - "velocityY": 0.0017227803565985438, - "timestamp": 2.9147157285540137 - }, - { - "x": 2.151086797745823, - "y": 5.543533706687699, - "heading": 2.4928931377494695e-20, - "angularVelocity": -1.157926638948406e-19, - "velocityX": 1.4533631378925307, - "velocityY": 0.0022970403628902575, - "timestamp": 3.007781413837598 - }, - { - "x": 2.3201595639866595, - "y": 5.5438009261858365, - "heading": 1.159960983349273e-20, - "angularVelocity": -1.432248793219564e-19, - "velocityX": 1.8167036080554038, - "velocityY": 0.0028712999568454734, - "timestamp": 3.1008470991211823 - }, - { - "x": 2.4554178003805283, - "y": 5.544014701821333, - "heading": 2.0604388914513833e-20, - "angularVelocity": 9.675724251840524e-20, - "velocityX": 1.4533631378925305, - "velocityY": 0.0022970403628902575, - "timestamp": 3.1939127844047666 - }, - { - "x": 2.556861482647531, - "y": 5.544175033555813, - "heading": 2.9920340295234286e-20, - "angularVelocity": 1.0010081967860847e-19, - "velocityX": 1.090022406839742, - "velocityY": 0.001722780356598544, - "timestamp": 3.286978469688351 - }, - { - "x": 2.624490605931685, - "y": 5.544281921381602, - "heading": 2.1256868508528512e-20, - "angularVelocity": -9.308986185732237e-20, - "velocityX": 0.7266816236089446, - "velocityY": 0.0011485202678394905, - "timestamp": 3.380044154971935 + "heading": 1.8511737998863575e-30, + "angularVelocity": -5.603397953128688e-29, + "velocityX": 0, + "velocityY": 2.843834189498754e-31, + "timestamp": 2.493236656085386 + }, + { + "x": 1.855209395178807, + "y": 5.543066072468525, + "heading": -4.900273115413656e-27, + "angularVelocity": -4.980371579437602e-26, + "velocityX": 0.42942883009919236, + "velocityY": 0.0006787122433542538, + "timestamp": 2.591665542633167 + }, + { + "x": 1.939745796780934, + "y": 5.543199682246804, + "heading": 4.528417643864408e-20, + "angularVelocity": 4.600700356082414e-19, + "velocityX": 0.8588576440015887, + "velocityY": 0.0013574244611094727, + "timestamp": 2.6900944291809483 + }, + { + "x": 2.066550395198543, + "y": 5.5434000969079245, + "heading": 6.339784356887693e-20, + "angularVelocity": 1.8402795932716144e-19, + "velocityX": 1.2882864255103954, + "velocityY": 0.0020361366276666244, + "timestamp": 2.7885233157287295 + }, + { + "x": 2.2356231808662415, + "y": 5.543667316436768, + "heading": 5.434100687876101e-20, + "angularVelocity": -9.20140114123854e-20, + "velocityX": 1.7177151098384495, + "velocityY": 0.002714848640629602, + "timestamp": 2.8869522022765106 + }, + { + "x": 2.4046959665339394, + "y": 5.543934535965611, + "heading": 4.338082196797977e-27, + "angularVelocity": -5.5208388966484585e-19, + "velocityX": 1.7177151098384498, + "velocityY": 0.0027148486406296025, + "timestamp": 2.9853810888242918 + }, + { + "x": 2.5315005649515485, + "y": 5.544134950626731, + "heading": -2.717050695881145e-20, + "angularVelocity": -2.760420466983953e-19, + "velocityX": 1.2882864255103954, + "velocityY": 0.0020361366276666244, + "timestamp": 3.083809975372073 + }, + { + "x": 2.6160369665536756, + "y": 5.54426856040501, + "heading": 3.0657579714860263e-27, + "angularVelocity": 2.760420337720658e-19, + "velocityX": 0.8588576440015887, + "velocityY": 0.0013574244611094727, + "timestamp": 3.182238861919854 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": 4.4426104536241024e-33, - "angularVelocity": -2.284071561257056e-19, - "velocityX": 0.36334081801614104, - "velocityY": 0.0005742601437372874, - "timestamp": 3.4731098402555194 + "heading": 1.737489718984482e-38, + "angularVelocity": -3.114693341043594e-26, + "velocityX": 0.42942883009919236, + "velocityY": 0.0006787122433542539, + "timestamp": 3.280667748467635 }, { "x": 2.6583051681518555, "y": 5.54433536529541, "heading": 0, - "angularVelocity": -4.668434863943491e-32, - "velocityX": -5.247197089772187e-38, - "velocityY": -1.940144776771126e-37, - "timestamp": 3.5661755255391037 - }, - { - "x": 2.6759672111325252, - "y": 5.540929157802402, - "heading": -2.5855780446225044e-18, - "angularVelocity": -3.809209550134996e-17, - "velocityX": 0.2602065056082181, - "velocityY": -0.05018204010160352, - "timestamp": 3.634052548595794 - }, - { - "x": 2.7112094706297696, - "y": 5.533716903598895, - "heading": -4.117772689767123e-18, - "angularVelocity": -2.2573097288976327e-17, - "velocityX": 0.5192075007150861, - "velocityY": -0.10625472182954283, - "timestamp": 3.7019295716524847 - }, - { - "x": 2.763926012188838, - "y": 5.522238566618234, - "heading": -6.491645699921123e-18, - "angularVelocity": -3.4973145598497126e-17, - "velocityX": 0.7766478137239484, - "velocityY": -0.16910489682311705, - "timestamp": 3.7698065947091752 - }, - { - "x": 2.833977839229424, - "y": 5.505960452216563, - "heading": -8.342526928581605e-18, - "angularVelocity": -2.7268155633677806e-17, - "velocityX": 1.0320403560138376, - "velocityY": -0.23981774197838637, - "timestamp": 3.8376836177658658 - }, - { - "x": 2.921179778977005, - "y": 5.484257984516274, - "heading": -1.1098333459478279e-17, - "angularVelocity": -4.0599991083804175e-17, - "velocityX": 1.284704835607638, - "velocityY": -0.31973216742524313, - "timestamp": 3.9055606408225563 - }, - { - "x": 3.025280889014475, - "y": 5.456393698526154, - "heading": -1.451838596617181e-17, - "angularVelocity": -5.0386012124268725e-17, - "velocityX": 1.5336722995427363, - "velocityY": -0.4105113149533374, - "timestamp": 3.973437663879247 - }, - { - "x": 3.1459346095433047, - "y": 5.4214894529386894, - "heading": -1.4363311491036705e-17, - "angularVelocity": 2.284638720910668e-18, - "velocityX": 1.7775340622711318, - "velocityY": -0.5142277020356774, - "timestamp": 4.041314686935937 - }, - { - "x": 3.2826525372630417, - "y": 5.378492545637339, - "heading": -1.4597384083135608e-17, - "angularVelocity": -3.4484805248903604e-18, - "velocityX": 2.0142004107273697, - "velocityY": -0.6334530503118778, - "timestamp": 4.109191709992627 - }, - { - "x": 3.434732199390872, - "y": 5.326137913197681, - "heading": -1.5798470616654896e-17, - "angularVelocity": -1.7695038459712723e-17, - "velocityX": 2.240517560718812, - "velocityY": -0.7713159782498521, - "timestamp": 4.1770687330493175 - }, - { - "x": 3.6011452044637537, - "y": 5.262915661085215, - "heading": -1.3130445812596166e-17, - "angularVelocity": 3.9306744519879925e-17, - "velocityX": 2.4516839068486322, - "velocityY": -0.9314234665192118, - "timestamp": 4.2449457561060076 - }, - { - "x": 3.7803722058519504, - "y": 5.187069964685383, - "heading": -9.049091305666531e-18, - "angularVelocity": 6.012866096854611e-17, - "velocityX": 2.640466439409227, - "velocityY": -1.1173986863932572, - "timestamp": 4.312822779162698 - }, - { - "x": 3.9701912865835975, - "y": 5.0966861229155755, - "heading": -3.2662453848887855e-18, - "angularVelocity": 8.519592728674139e-17, - "velocityX": 2.796514522640622, - "velocityY": -1.3315822895520983, - "timestamp": 4.380699802219388 + "angularVelocity": -1.1323659927897137e-37, + "velocityX": 0, + "velocityY": 3.593395183653217e-38, + "timestamp": 3.3790966350154164 + }, + { + "x": 2.6753320649100156, + "y": 5.539342065750356, + "heading": 1.5795430701564016e-18, + "angularVelocity": 2.476798764658572e-17, + "velocityX": 0.2669898507573085, + "velocityY": -0.07829731508072835, + "timestamp": 3.442870208092684 + }, + { + "x": 2.7093452100309583, + "y": 5.5292188917484815, + "heading": 4.421175983497276e-18, + "angularVelocity": 4.455815749727496e-17, + "velocityX": 0.5333423153150382, + "velocityY": -0.15873618982597607, + "timestamp": 3.506643781169952 + }, + { + "x": 2.7602957743928864, + "y": 5.513806855363461, + "heading": 7.811064020414863e-18, + "angularVelocity": 5.3155058958518925e-17, + "velocityX": 0.7989291159865928, + "velocityY": -0.24166807097898507, + "timestamp": 3.5704173542472195 + }, + { + "x": 2.8281241133670565, + "y": 5.492918602334042, + "heading": 1.1328926559518176e-17, + "angularVelocity": 5.516176010462958e-17, + "velocityX": 1.0635806604718465, + "velocityY": -0.3275377561817081, + "timestamp": 3.6341909273244872 + }, + { + "x": 2.9127559105445995, + "y": 5.466330173559929, + "heading": 1.5973697872969206e-17, + "angularVelocity": 7.283222641176841e-17, + "velocityX": 1.3270668882705299, + "velocityY": -0.41691922674457305, + "timestamp": 3.697964500401755 + }, + { + "x": 3.0140963269470453, + "y": 5.433769288149837, + "heading": 1.634340357557127e-17, + "angularVelocity": 5.797161500644316e-18, + "velocityX": 1.5890659957167883, + "velocityY": -0.5105701913650192, + "timestamp": 3.7617380734790227 + }, + { + "x": 3.132020733935084, + "y": 5.394898138842126, + "heading": 1.753294263521122e-17, + "angularVelocity": 1.8652539010143133e-17, + "velocityX": 1.8491108667403897, + "velocityY": -0.6095181347392088, + "timestamp": 3.8255116465562904 + }, + { + "x": 3.2663592482856187, + "y": 5.34928716553499, + "heading": 1.3361291372353952e-17, + "angularVelocity": -6.54134786803134e-17, + "velocityX": 2.1064918879136747, + "velocityY": -0.7152017851010761, + "timestamp": 3.889285219633558 + }, + { + "x": 3.4168692069208895, + "y": 5.296373281622491, + "heading": 9.91837272236996e-18, + "angularVelocity": -5.398660422887962e-17, + "velocityX": 2.36006783645184, + "velocityY": -0.8297149016315647, + "timestamp": 3.953058792710826 + }, + { + "x": 3.5831820501397047, + "y": 5.235389893074956, + "heading": 8.197617273877545e-18, + "angularVelocity": -2.6982264995683312e-17, + "velocityX": 2.6078645933372964, + "velocityY": -0.9562485776615884, + "timestamp": 4.016832365788094 + }, + { + "x": 3.7646897201959026, + "y": 5.165243249909899, + "heading": 6.703480606914857e-18, + "angularVelocity": -2.342877456705142e-17, + "velocityX": 2.8461267151565104, + "velocityY": -1.0999327743494527, + "timestamp": 4.080605938865362 + }, + { + "x": 3.960268086544922, + "y": 5.08428626917176, + "heading": 4.5447273483145725e-18, + "angularVelocity": -3.385027926825969e-17, + "velocityX": 3.066761934634853, + "velocityY": -1.2694440162550131, + "timestamp": 4.144379511942629 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 1.549943268403634e-18, - "angularVelocity": 7.09546240599044e-17, - "velocityX": 2.9068419610975353, - "velocityY": -1.5725251674415364, - "timestamp": 4.448576825276078 - }, - { - "x": 4.262376057274491, - "y": 4.935051597962846, - "heading": 3.5392756001694805e-18, - "angularVelocity": 6.14731881972971e-17, - "velocityX": 2.9318335021132986, - "velocityY": -1.6963703103934957, - "timestamp": 4.480937800688539 - }, - { - "x": 4.358262696439126, - "y": 4.876193489161229, - "heading": 6.0915874857662914e-18, - "angularVelocity": 7.887005422678292e-17, - "velocityX": 2.9630330341559845, - "velocityY": -1.8187989716450665, - "timestamp": 4.513298776101001 - }, - { - "x": 4.455482915426748, - "y": 4.813470451403357, - "heading": 8.792871908848605e-18, - "angularVelocity": 8.347351674874434e-17, - "velocityX": 3.0042425405441757, - "velocityY": -1.938230753505593, - "timestamp": 4.545659751513463 - }, - { - "x": 4.554624947873469, - "y": 4.7471387133987335, - "heading": 1.0200765098673088e-17, - "angularVelocity": 4.3505894735366074e-17, - "velocityX": 3.0636292998925567, - "velocityY": -2.0497447051327327, - "timestamp": 4.578020726925924 - }, - { - "x": 4.656862547860915, - "y": 4.678136115394173, - "heading": 1.0067733420891333e-17, - "angularVelocity": -4.1108673671660525e-18, - "velocityX": 3.1592867237269773, - "velocityY": -2.1322780640903733, - "timestamp": 4.610381702338386 - }, - { - "x": 4.763145340600117, - "y": 4.609727154178217, - "heading": 9.332936021050385e-18, - "angularVelocity": -2.2706280960159642e-17, - "velocityX": 3.28428891232596, - "velocityY": -2.1139338460611237, - "timestamp": 4.642742677750848 - }, - { - "x": 4.872109686799802, - "y": 4.544404504995177, - "heading": 8.161678901765097e-18, - "angularVelocity": -3.6193504811759114e-17, - "velocityX": 3.3671527143685642, - "velocityY": -2.018562430534307, - "timestamp": 4.675103653163309 - }, - { - "x": 4.982798416209946, - "y": 4.482788956670884, - "heading": 7.225847948253543e-18, - "angularVelocity": -2.891850265932224e-17, - "velocityX": 3.4204385992493553, - "velocityY": -1.9040077605499512, - "timestamp": 4.707464628575771 - }, - { - "x": 5.094726607705277, - "y": 4.425069541031477, - "heading": 5.520990037711479e-18, - "angularVelocity": -5.268252544393214e-17, - "velocityX": 3.458739734162278, - "velocityY": -1.7836117392548974, - "timestamp": 4.7398256039882325 - }, - { - "x": 5.207616568786535, - "y": 4.371323930618111, - "heading": 3.484375827946225e-18, - "angularVelocity": -6.293426523223445e-17, - "velocityX": 3.48845977732135, - "velocityY": -1.6608155263660398, - "timestamp": 4.772186579400694 - }, - { - "x": 5.321291012099858, - "y": 4.321590890995498, - "heading": 1.6420175756244674e-18, - "angularVelocity": -5.693148086098641e-17, - "velocityX": 3.5127013900065562, - "velocityY": -1.5368214025916205, - "timestamp": 4.804547554813156 - }, - { - "x": 5.4356276249643685, - "y": 4.275892411291382, - "heading": -1.9852645585834772e-19, - "angularVelocity": -5.687541886629066e-17, - "velocityX": 3.5331633675195255, - "velocityY": -1.4121477836084664, - "timestamp": 4.836908530225617 - }, - { - "x": 5.5505371629105715, - "y": 4.234242129103919, - "heading": -1.7642698806491036e-18, - "angularVelocity": -4.838369069076164e-17, - "velocityX": 3.5508675644539367, - "velocityY": -1.2870527435160222, - "timestamp": 4.869269505638079 + "heading": 2.916776997028848e-18, + "angularVelocity": -2.5527036870167268e-17, + "velocityX": 3.2494804486361053, + "velocityY": -1.4792721930373927, + "timestamp": 4.208153085019897 + }, + { + "x": 4.281965617918933, + "y": 4.933633775459018, + "heading": 2.5007263674842835e-18, + "angularVelocity": -1.1988032936813997e-17, + "velocityX": 3.298225514699012, + "velocityY": -1.6226254295188278, + "timestamp": 4.242858581053939 + }, + { + "x": 4.397154085355019, + "y": 4.872114684924303, + "heading": 2.80280710947709e-18, + "angularVelocity": 8.704118266930975e-18, + "velocityX": 3.31902668450838, + "velocityY": -1.772603695805773, + "timestamp": 4.277564077087981 + }, + { + "x": 4.510378349293925, + "y": 4.805721616464948, + "heading": 5.6088052918749416e-18, + "angularVelocity": 8.085169506424825e-17, + "velocityX": 3.2624303605355687, + "velocityY": -1.9130419111206762, + "timestamp": 4.312269573122023 + }, + { + "x": 4.620854325513756, + "y": 4.734849659733664, + "heading": 8.920577651651403e-18, + "angularVelocity": 9.542501154652829e-17, + "velocityX": 3.183241527839498, + "velocityY": -2.0420960605711302, + "timestamp": 4.346975069156065 + }, + { + "x": 4.728404969374526, + "y": 4.659612469130667, + "heading": 1.0355720889195964e-17, + "angularVelocity": 4.1352045109421545e-17, + "velocityX": 3.0989513521223016, + "velocityY": -2.167875385765943, + "timestamp": 4.381680565190107 + }, + { + "x": 4.836822070451743, + "y": 4.585629308888239, + "heading": 8.659234104719362e-18, + "angularVelocity": -4.888236672406423e-17, + "velocityX": 3.1239173464304306, + "velocityY": -2.131741905370206, + "timestamp": 4.4163860612241495 + }, + { + "x": 4.9481136166787945, + "y": 4.516045063115851, + "heading": 6.20990752321007e-18, + "angularVelocity": -7.057460233695506e-17, + "velocityX": 3.2067412642045734, + "velocityY": -2.0049921114550315, + "timestamp": 4.4510915572581915 + }, + { + "x": 5.062101259723399, + "y": 4.450971317120919, + "heading": 3.721439651007284e-18, + "angularVelocity": -7.170241479222451e-17, + "velocityX": 3.2844262745242743, + "velocityY": -1.8750271118759503, + "timestamp": 4.4857970532922335 + }, + { + "x": 5.178602303777092, + "y": 4.39051239286923, + "heading": 1.522860892330953e-18, + "angularVelocity": -6.334958464560708e-17, + "velocityX": 3.356847109732077, + "velocityY": -1.7420561916875086, + "timestamp": 4.520502549326276 + }, + { + "x": 5.2974300176780025, + "y": 4.334765207042833, + "heading": -2.608193001732958e-19, + "angularVelocity": -5.139474712462447e-17, + "velocityX": 3.42388749563912, + "velocityY": -1.606292725847098, + "timestamp": 4.555208045360318 + }, + { + "x": 5.4183939381282835, + "y": 4.283819120698816, + "heading": -2.822662772810701e-18, + "angularVelocity": -7.38166505421658e-17, + "velocityX": 3.485439894926986, + "velocityY": -1.4679544212261044, + "timestamp": 4.58991354139436 + }, + { + "x": 5.5413001762936585, + "y": 4.23775579768525, + "heading": -5.4175777354584686e-18, + "angularVelocity": -7.476956848858907e-17, + "velocityX": 3.5414056045998694, + "velocityY": -1.3272630642818801, + "timestamp": 4.624619037428402 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -2.8015689175743048e-18, - "angularVelocity": -3.2054010230453e-17, - "velocityX": 3.5664736442333616, - "velocityY": -1.1616786598774413, - "timestamp": 4.901630481050541 - }, - { - "x": 5.795001659778632, - "y": 4.159833423852274, - "heading": -3.918726614896288e-18, - "angularVelocity": -3.100871682049742e-17, - "velocityX": 3.5820124359680223, - "velocityY": -1.0218844572378571, - "timestamp": 4.937657695714008 - }, - { - "x": 5.924325054788198, - "y": 4.128077802262558, - "heading": -4.913044441347591e-18, - "angularVelocity": -2.7599075747243218e-17, - "velocityX": 3.5896029215020695, - "velocityY": -0.8814342681316839, - "timestamp": 4.973684910377475 - }, - { - "x": 6.053574493908213, - "y": 4.101389053934096, - "heading": -5.793955566552838e-18, - "angularVelocity": -2.4451269226363692e-17, - "velocityX": 3.5875501430610877, - "velocityY": -0.7407941073925298, - "timestamp": 5.009712125040942 - }, - { - "x": 6.182326457133652, - "y": 4.079743240098328, - "heading": -6.4165955709959285e-18, - "angularVelocity": -1.7282490757751136e-17, - "velocityX": 3.5737418068012827, - "velocityY": -0.6008184101369575, - "timestamp": 5.04573933970441 - }, - { - "x": 6.310065721160521, - "y": 4.063062615773879, - "heading": -6.572943704568109e-18, - "angularVelocity": -4.339723040934764e-18, - "velocityX": 3.5456325230826065, - "velocityY": -0.46300066436618775, - "timestamp": 5.081766554367877 - }, - { - "x": 6.436174783136967, - "y": 4.051180022659281, - "heading": -6.55887717200026e-18, - "angularVelocity": 3.904418561563001e-19, - "velocityX": 3.500383339495921, - "velocityY": -0.3298226972469103, - "timestamp": 5.117793769031344 - }, - { - "x": 6.559940504232512, - "y": 4.043790469879042, - "heading": -6.336632318587662e-18, - "angularVelocity": 6.168804762936026e-18, - "velocityX": 3.4353397078194354, - "velocityY": -0.2051102992353164, - "timestamp": 5.1538209836948115 - }, - { - "x": 6.680596023490012, - "y": 4.040401579407175, - "heading": -5.869996108311954e-18, - "angularVelocity": 1.2952325474912863e-17, - "velocityX": 3.349010474013899, - "velocityY": -0.09406473699182225, - "timestamp": 5.189848198358279 - }, - { - "x": 6.797407433038957, - "y": 4.040314426809989, - "heading": -5.129903283273234e-18, - "angularVelocity": 2.0542604582494022e-17, - "velocityX": 3.242310310138806, - "velocityY": -0.002419076745183701, - "timestamp": 5.225875413021746 - }, - { - "x": 6.9097792291334015, - "y": 4.042670322539034, - "heading": -4.098685939880453e-18, - "angularVelocity": 2.8623288062328696e-17, - "velocityX": 3.1190808710613775, - "velocityY": 0.06539211401857271, - "timestamp": 5.261902627685213 - }, - { - "x": 7.0173204584579265, - "y": 4.046557322402495, - "heading": -2.7703450666011344e-18, - "angularVelocity": 3.687048487350159e-17, - "velocityX": 2.9849998210817597, - "velocityY": 0.10789065709827174, - "timestamp": 5.2979298423486805 - }, - { - "x": 7.119839418159034, - "y": 4.051119205104076, - "heading": -1.5285714179208822e-18, - "angularVelocity": 3.4467656195020295e-17, - "velocityX": 2.845597714359634, - "velocityY": 0.1266232414632443, - "timestamp": 5.333957057012148 + "heading": -7.665872207711091e-18, + "angularVelocity": -6.478208725348017e-17, + "velocityX": 3.591694883279384, + "velocityY": -1.1844441897757751, + "timestamp": 4.659324533462444 + }, + { + "x": 5.800468403278387, + "y": 4.158565232862714, + "heading": -9.714096811795654e-18, + "angularVelocity": -5.540829464294432e-17, + "velocityX": 3.6389268627432543, + "velocityY": -1.0302389285738007, + "timestamp": 4.6962905620558795 + }, + { + "x": 5.936486438230336, + "y": 4.1262510009162545, + "heading": -1.1401685441264777e-17, + "angularVelocity": -4.5652419090776246e-17, + "velocityX": 3.67954146353996, + "velocityY": -0.8741602269982254, + "timestamp": 4.733256590649315 + }, + { + "x": 6.0737584862384155, + "y": 4.099765139631346, + "heading": -1.2627280609090703e-17, + "angularVelocity": -3.3154634524185157e-17, + "velocityX": 3.713464854930551, + "velocityY": -0.7164919330720612, + "timestamp": 4.770222619242751 + }, + { + "x": 6.212034919724313, + "y": 4.079155810434451, + "heading": -1.3669283004490658e-17, + "angularVelocity": -2.818810770451562e-17, + "velocityX": 3.7406353548742786, + "velocityY": -0.5575207827587797, + "timestamp": 4.8071886478361865 + }, + { + "x": 6.351064277842407, + "y": 4.06446048840337, + "heading": -1.3375402013759826e-17, + "angularVelocity": 7.950028767305027e-18, + "velocityX": 3.761003370072177, + "velocityY": -0.3975358617152357, + "timestamp": 4.844154676429622 + }, + { + "x": 6.490086441164715, + "y": 4.055726900176193, + "heading": -1.4576849056839345e-17, + "angularVelocity": -3.2501382723403574e-17, + "velocityX": 3.7608087374308066, + "velocityY": -0.23625984612066567, + "timestamp": 4.881120705023058 + }, + { + "x": 6.625430002884342, + "y": 4.051684818750479, + "heading": -1.3372180037518883e-17, + "angularVelocity": 3.2588543188391143e-17, + "velocityX": 3.661295704988517, + "velocityY": -0.10934583939674931, + "timestamp": 4.9180867336164935 + }, + { + "x": 6.755501460439252, + "y": 4.050426142633095, + "heading": -1.1630354881514603e-17, + "angularVelocity": 4.7119618262525755e-17, + "velocityX": 3.518675457011608, + "velocityY": -0.03404953589216864, + "timestamp": 4.955052762209929 + }, + { + "x": 6.8798297740402194, + "y": 4.050767085715321, + "heading": -9.335447591441833e-18, + "angularVelocity": 6.208152126139743e-17, + "velocityX": 3.3633127044392923, + "velocityY": 0.009223146093816154, + "timestamp": 4.992018790803365 + }, + { + "x": 6.998265582982919, + "y": 4.052014068285868, + "heading": -6.4179877740703305e-18, + "angularVelocity": 7.892272793106084e-17, + "velocityX": 3.203909466318293, + "velocityY": 0.03373320364655125, + "timestamp": 5.0289848193968005 + }, + { + "x": 7.110758098746119, + "y": 4.053729886887297, + "heading": -3.1976006784105176e-18, + "angularVelocity": 8.71174756444278e-17, + "velocityX": 3.0431323039981164, + "velocityY": 0.046416092469654044, + "timestamp": 5.065950847990236 }, { "x": 7.217291355133057, "y": 4.0556182861328125, - "heading": 0, - "angularVelocity": 4.242824298811395e-17, - "velocityX": 2.704953404928098, - "velocityY": 0.12488006832508823, - "timestamp": 5.369984271675615 - }, - { - "x": 7.386413080563964, - "y": 4.061807740730578, - "heading": 2.2517150943375065e-18, - "angularVelocity": 3.243733929990829e-17, - "velocityX": 2.436302356627287, - "velocityY": 0.08916289604043352, - "timestamp": 5.439401651242998 - }, - { - "x": 7.536811709047552, - "y": 4.066158661383703, - "heading": 2.5863681690768457e-18, - "angularVelocity": 4.820883139423778e-18, - "velocityX": 2.166584642360316, - "velocityY": 0.06267768504430034, - "timestamp": 5.508819030810382 - }, - { - "x": 7.668447638906394, - "y": 4.069132723210473, - "heading": 2.8797714291084188e-18, - "angularVelocity": 4.226654216280414e-18, - "velocityX": 1.89629644160023, - "velocityY": 0.04284318776225898, - "timestamp": 5.578236410377765 - }, - { - "x": 7.781298581594194, - "y": 4.071077765012589, - "heading": 3.948563192438554e-18, - "angularVelocity": 1.5396601974766197e-17, - "velocityX": 1.6256871606375698, - "velocityY": 0.028019522117344105, - "timestamp": 5.647653789945148 - }, - { - "x": 7.875351641584324, - "y": 4.072265013256325, - "heading": 5.745276308795261e-18, - "angularVelocity": 2.5882756271616716e-17, - "velocityX": 1.3548921116913086, - "velocityY": 0.017103040350048473, - "timestamp": 5.717071169512532 - }, - { - "x": 7.950599316309057, - "y": 4.072911756307987, - "heading": 3.5817600850337305e-18, - "angularVelocity": -3.116678038331375e-17, - "velocityX": 1.0839889836476846, - "velocityY": 0.009316730992924663, - "timestamp": 5.786488549079915 - }, - { - "x": 8.007037333813425, - "y": 4.073195912702993, - "heading": 1.7956945946791005e-18, - "angularVelocity": -2.5729370677570625e-17, - "velocityX": 0.8130243154682115, - "velocityY": 0.004093447444678863, - "timestamp": 5.8559059286472985 - }, - { - "x": 8.044663419582989, - "y": 4.0732658050755175, - "heading": 5.777314545574126e-19, - "angularVelocity": -1.754550730252714e-17, - "velocityX": 0.542026881510858, - "velocityY": 0.0010068425653567271, - "timestamp": 5.925323308214682 + "heading": -8.646914705534956e-42, + "angularVelocity": 8.650106057047145e-17, + "velocityX": 2.8819232262850263, + "velocityY": 0.051084720684648116, + "timestamp": 5.102916876583672 + }, + { + "x": 7.405338431031862, + "y": 4.059210715077462, + "heading": 6.035426461355953e-18, + "angularVelocity": 8.221730826644903e-17, + "velocityX": 2.561662296238626, + "velocityY": 0.04893769145541128, + "timestamp": 5.176325099117501 + }, + { + "x": 7.569876923635766, + "y": 4.062517669134614, + "heading": 1.1021958875984658e-17, + "angularVelocity": 6.792879928853616e-17, + "velocityX": 2.2414177448320305, + "velocityY": 0.04504882345608618, + "timestamp": 5.24973332165133 + }, + { + "x": 7.710907923869345, + "y": 4.065460118931574, + "heading": 1.4466516010520595e-17, + "angularVelocity": 4.692331479554e-17, + "velocityX": 1.9211880544932918, + "velocityY": 0.04008338160760226, + "timestamp": 5.32314154418516 + }, + { + "x": 7.828432324355203, + "y": 4.067984380811152, + "heading": 1.5902740281761073e-17, + "angularVelocity": 1.956489643348339e-17, + "velocityX": 1.600970523863292, + "velocityY": 0.03438663670702133, + "timestamp": 5.396549766718989 + }, + { + "x": 7.922450847470263, + "y": 4.070051609380546, + "heading": 1.5219645547347743e-17, + "angularVelocity": -9.30542534385069e-18, + "velocityX": 1.2807628337783643, + "velocityY": 0.02816072230112734, + "timestamp": 5.469957989252818 + }, + { + "x": 7.992964082884089, + "y": 4.071632392724107, + "heading": 1.2631622975679932e-17, + "angularVelocity": -3.5255213685022036e-17, + "velocityX": 0.9605631764388524, + "velocityY": 0.021534145481204726, + "timestamp": 5.543366211786648 + }, + { + "x": 8.039972518291798, + "y": 4.072703688242487, + "heading": 7.596971431802736e-18, + "angularVelocity": -6.858429982495519e-17, + "velocityX": 0.6403701627027665, + "velocityY": 0.014593671953933919, + "timestamp": 5.616774434320477 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": -2.803956986151352e-39, - "angularVelocity": -8.322576538583876e-18, - "velocityX": 0.2710148817811471, - "velocityY": -0.0002715343629009974, - "timestamp": 5.994740687782065 + "heading": 8.652482395157843e-45, + "angularVelocity": -1.0348937993018065e-16, + "velocityX": 0.32018271791516006, + "velocityY": 0.007400637290262982, + "timestamp": 5.690182656854306 }, { "x": 8.0634765625, "y": 4.073246955871582, "heading": 0, - "angularVelocity": -3.588476339344369e-38, - "velocityX": -2.7715613376406244e-35, - "velocityY": -8.161774363102711e-35, - "timestamp": 6.064158067349449 - }, - { - "x": 8.045268218494023, - "y": 4.069202722176317, - "heading": -4.783612799247601e-20, - "angularVelocity": -6.920780898714677e-19, - "velocityX": -0.2634326077352512, - "velocityY": -0.05851070411920324, - "timestamp": 6.133277620021823 - }, - { - "x": 8.008750233843994, - "y": 4.061600867158929, - "heading": -1.3711751824964603e-19, - "angularVelocity": -1.2916951396895015e-18, - "velocityX": -0.5283307434457009, - "velocityY": -0.10998125311111973, - "timestamp": 6.202397172694197 - }, - { - "x": 7.953821890093904, - "y": 4.051005973704918, - "heading": -2.6045835174744157e-19, - "angularVelocity": -1.7844564773553645e-18, - "velocityX": -0.7946860421746129, - "velocityY": -0.1532835940682337, - "timestamp": 6.271516725366571 - }, - { - "x": 7.880387860483353, - "y": 4.038078874831867, - "heading": -4.092475364172468e-19, - "angularVelocity": -2.152635237102946e-18, - "velocityX": -1.062420498561777, - "velocityY": -0.18702521028060204, - "timestamp": 6.340636278038946 - }, - { - "x": 7.788366410256325, - "y": 4.023600204310839, - "heading": -5.733506163788985e-19, - "angularVelocity": -2.3741918689373813e-18, - "velocityX": -1.3313374677525447, - "velocityY": -0.20947286203740642, - "timestamp": 6.40975583071132 - }, - { - "x": 7.677703229907308, - "y": 4.008500675956516, - "heading": -1.077796513774058e-18, - "angularVelocity": -7.298164960190344e-18, - "velocityX": -1.6010401698280374, - "velocityY": -0.21845523835917138, - "timestamp": 6.478875383383694 - }, - { - "x": 7.548394643155177, - "y": 3.993899515872939, - "heading": -1.3825329983857112e-18, - "angularVelocity": -4.4088318398420175e-18, - "velocityX": -1.8707960591853112, - "velocityY": -0.2112450025940816, - "timestamp": 6.547994936056068 - }, - { - "x": 7.40052628679877, - "y": 3.9811515921807197, - "heading": -2.4834039360007183e-18, - "angularVelocity": -1.5927055298250076e-17, - "velocityX": -2.1393129822078065, - "velocityY": -0.18443295998520684, - "timestamp": 6.6171144887284425 - }, - { - "x": 7.23433662009932, - "y": 3.9719006814395397, - "heading": -2.9994646007909418e-18, - "angularVelocity": -7.466203770052342e-18, - "velocityX": -2.4043799514615647, - "velocityY": -0.13383927388866498, - "timestamp": 6.686234041400817 - }, - { - "x": 7.050317591694833, - "y": 3.968128214859164, - "heading": -3.4395184001583614e-18, - "angularVelocity": -6.366560290082446e-18, - "velocityX": -2.662329562182405, - "velocityY": -0.05457886277500885, - "timestamp": 6.755353594073191 - }, - { - "x": 6.849362271663259, - "y": 3.972169933507745, - "heading": -3.772315951879389e-18, - "angularVelocity": -4.8148105541753904e-18, - "velocityX": -2.907358515239499, - "velocityY": 0.05847431721293695, - "timestamp": 6.824473146745565 - }, - { - "x": 6.632947899656103, - "y": 3.9866479902960563, - "heading": -6.760060231429226e-18, - "angularVelocity": -4.322574675534167e-17, - "velocityX": -3.131015228541142, - "velocityY": 0.20946398274504574, - "timestamp": 6.8935926994179395 - }, - { - "x": 6.4032866668448785, - "y": 4.014256930642455, - "heading": -9.565460422027674e-18, - "angularVelocity": -4.058764969139539e-17, - "velocityX": -3.3226666541060275, - "velocityY": 0.39943748590597694, - "timestamp": 6.962712252090314 - }, - { - "x": 6.1633110990497455, - "y": 4.057406588823124, - "heading": -1.2152070169754713e-17, - "angularVelocity": -3.7422258214923894e-17, - "velocityX": -3.4718912162613744, - "velocityY": 0.6242757152263072, - "timestamp": 7.031831804762688 - }, - { - "x": 5.916400554770506, - "y": 4.117871140121953, - "heading": -1.4492027796446373e-17, - "angularVelocity": -3.385377272110881e-17, - "velocityX": -3.5722242800035118, - "velocityY": 0.8747821558601414, - "timestamp": 7.100951357435062 + "angularVelocity": 5.394624259127602e-43, + "velocityX": 0, + "velocityY": -7.823862347152674e-43, + "timestamp": 5.763590879388135 + }, + { + "x": 8.042595807906332, + "y": 4.071940125417329, + "heading": 2.4014031773112573e-19, + "angularVelocity": 3.467784621738616e-18, + "velocityX": -0.30153187250835495, + "velocityY": -0.01887149394694188, + "timestamp": 5.832839793010322 + }, + { + "x": 8.000826969409328, + "y": 4.069449378892138, + "heading": 7.295832460611546e-19, + "angularVelocity": 7.067878797353639e-18, + "velocityX": -0.6031695850838968, + "velocityY": -0.03596802310545437, + "timestamp": 5.902088706632509 + }, + { + "x": 7.938162262991824, + "y": 4.0659211372128246, + "heading": 1.4792287784714074e-18, + "angularVelocity": 1.0825376070160765e-17, + "velocityX": -0.9049197040027859, + "velocityY": -0.0509501376232871, + "timestamp": 5.971337620254696 + }, + { + "x": 7.8545936374958405, + "y": 4.061532754567933, + "heading": 2.6959287825754293e-18, + "angularVelocity": 1.756995078279752e-17, + "velocityX": -1.206786086954711, + "velocityY": -0.06337114064826968, + "timestamp": 6.0405865338768825 + }, + { + "x": 7.7501132248715185, + "y": 4.056503424652652, + "heading": 4.202275116563444e-18, + "angularVelocity": 2.17526348818473e-17, + "velocityX": -1.5087660897375712, + "velocityY": -0.07262684210066948, + "timestamp": 6.109835447499069 + }, + { + "x": 7.624714358960079, + "y": 4.051110859560637, + "heading": 6.0188244090330196e-18, + "angularVelocity": 2.6232170260178098e-17, + "velocityX": -1.8108423562512328, + "velocityY": -0.07787219769881376, + "timestamp": 6.179084361121256 + }, + { + "x": 7.478393887159083, + "y": 4.045718105040055, + "heading": 8.17237550228445e-18, + "angularVelocity": 3.1098698601987103e-17, + "velocityX": -2.1129641484240804, + "velocityY": -0.07787493317230812, + "timestamp": 6.248333274743443 + }, + { + "x": 7.311157646174121, + "y": 4.040819461213868, + "heading": 1.0532118174804612e-17, + "angularVelocity": 3.4076241042489266e-17, + "velocityX": -2.4150016547173685, + "velocityY": -0.07073964875337486, + "timestamp": 6.31758218836563 + }, + { + "x": 7.123034533164843, + "y": 4.037125830040512, + "heading": 8.283291482043014e-18, + "angularVelocity": -3.247454111743766e-17, + "velocityX": -2.716621866960299, + "velocityY": -0.053338471033761534, + "timestamp": 6.386831101987816 + }, + { + "x": 6.914117681129227, + "y": 4.035741699141579, + "heading": 5.798379315469736e-18, + "angularVelocity": -3.588377111777714e-17, + "velocityX": -3.016897177267489, + "velocityY": -0.019987763367446825, + "timestamp": 6.456080015610003 + }, + { + "x": 6.684712125750405, + "y": 4.03859150608828, + "heading": 3.611288298800787e-18, + "angularVelocity": -3.158303722425788e-17, + "velocityX": -3.312767571061532, + "velocityY": 0.041153092483867636, + "timestamp": 6.52532892923219 + }, + { + "x": 6.43606725460267, + "y": 4.049660896865006, + "heading": 2.3554139869933522e-18, + "angularVelocity": -1.8135653631468075e-17, + "velocityX": -3.5905959839934547, + "velocityY": 0.15984930589841134, + "timestamp": 6.594577842854377 + }, + { + "x": 6.175716481433539, + "y": 4.078070527288863, + "heading": 2.6764824042330446e-18, + "angularVelocity": 4.636439771335605e-18, + "velocityX": -3.759636932206204, + "velocityY": 0.41025380670743106, + "timestamp": 6.663826756476563 + }, + { + "x": 5.918464051979435, + "y": 4.12717103366876, + "heading": 2.4806366122903936e-18, + "angularVelocity": -2.8281424458318553e-18, + "velocityX": -3.7148948048144055, + "velocityY": 0.7090437064151481, + "timestamp": 6.73307567009875 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -1.65695066909008e-17, - "angularVelocity": -3.005631278245188e-17, - "velocityX": -3.6234150289835947, - "velocityY": 1.1397344367359599, - "timestamp": 7.1700709101074365 - }, - { - "x": 5.553208867110479, - "y": 4.235853588954207, - "heading": -1.751752246117206e-17, - "angularVelocity": -3.049424202704779e-17, - "velocityX": -3.6265305068647917, - "velocityY": 1.2610675773235673, - "timestamp": 7.201159263423893 - }, - { - "x": 5.440423618005098, - "y": 4.27883115567922, - "heading": -1.8382942207466584e-17, - "angularVelocity": -2.7837426365803457e-17, - "velocityX": -3.6278939562128056, - "velocityY": 1.382433038106979, - "timestamp": 7.2322476167403495 - }, - { - "x": 5.327666323513458, - "y": 4.3255819087456695, - "heading": -1.9161761421911296e-17, - "angularVelocity": -2.5051800155064728e-17, - "velocityX": -3.626994757292344, - "velocityY": 1.5038028097069258, - "timestamp": 7.263335970056806 - }, - { - "x": 5.215031204096495, - "y": 4.376103972546304, - "heading": -2.1196555001876326e-17, - "angularVelocity": -6.545195749690707e-17, - "velocityX": -3.6230648265741094, - "velocityY": 1.6251122497984019, - "timestamp": 7.294424323373263 - }, - { - "x": 5.102650689013506, - "y": 4.430390725009075, - "heading": -2.311672068470027e-17, - "angularVelocity": -6.176479221122079e-17, - "velocityX": -3.6148751250680937, - "velocityY": 1.746208681758474, - "timestamp": 7.325512676689719 - }, - { - "x": 4.990723576065449, - "y": 4.488423424572919, - "heading": -2.4422095497687565e-17, - "angularVelocity": -4.198919124707493e-17, - "velocityX": -3.6002908165872247, - "velocityY": 1.8667022654147425, - "timestamp": 7.356601030006176 - }, - { - "x": 4.87957749498313, - "y": 4.550147690824094, - "heading": -2.55240391966328e-17, - "angularVelocity": -3.544554733112049e-17, - "velocityX": -3.5751678434342686, - "velocityY": 1.9854466276443046, - "timestamp": 7.387689383322632 - }, - { - "x": 4.769828431698842, - "y": 4.615377093855064, - "heading": -2.67892153006201e-17, - "angularVelocity": -4.069614402184995e-17, - "velocityX": -3.5302308284753074, - "velocityY": 2.0981942133435845, - "timestamp": 7.418777736639089 - }, - { - "x": 4.662763212803556, - "y": 4.683258770801351, - "heading": -2.756635025118312e-17, - "angularVelocity": -2.4997623471627382e-17, - "velocityX": -3.443901251553593, - "velocityY": 2.1835082821949876, - "timestamp": 7.449866089955545 - }, - { - "x": 4.559444563014645, - "y": 4.750692382461376, - "heading": -2.7843158304647625e-17, - "angularVelocity": -8.903914937396602e-18, - "velocityX": -3.32338766023415, - "velocityY": 2.1690956408530244, - "timestamp": 7.480954443272002 - }, - { - "x": 4.458844422855551, - "y": 4.815509255321042, - "heading": -2.805863064985095e-17, - "angularVelocity": -6.930966815860972e-18, - "velocityX": -3.2359430277653423, - "velocityY": 2.084924608256891, - "timestamp": 7.512042796588458 - }, - { - "x": 4.36022139727515, - "y": 4.877112301745061, - "heading": -2.8000369092306777e-17, - "angularVelocity": 1.8740637997727053e-18, - "velocityX": -3.1723463953362434, - "velocityY": 1.9815474237874686, - "timestamp": 7.543131149904915 - }, - { - "x": 4.263183288640869, - "y": 4.935291063699046, - "heading": -2.7736137264454843e-17, - "angularVelocity": 8.499383198535807e-18, - "velocityX": -3.1213653436868745, - "velocityY": 1.871400564763493, - "timestamp": 7.5742195032213715 + "heading": 3.0661477431883128e-18, + "angularVelocity": 8.455167023881293e-18, + "velocityX": -3.6464445426004524, + "velocityY": 1.0033087488526664, + "timestamp": 6.802324583720937 + }, + { + "x": 5.541093538436437, + "y": 4.236435061250764, + "heading": 3.354859743531915e-18, + "angularVelocity": 8.33229202553877e-18, + "velocityX": -3.6034349206984246, + "velocityY": 1.1482323536375627, + "timestamp": 6.83697435349121 + }, + { + "x": 5.417925084630127, + "y": 4.281179055760442, + "heading": 3.9245083052882345e-18, + "angularVelocity": 1.6440183168115343e-17, + "velocityX": -3.554668750266214, + "velocityY": 1.2913215529664346, + "timestamp": 6.871624123261483 + }, + { + "x": 5.296643156826778, + "y": 4.330809566180666, + "heading": 4.682534245858068e-18, + "angularVelocity": 2.1876795880478242e-17, + "velocityX": -3.500223193615562, + "velocityY": 1.4323474802075717, + "timestamp": 6.906273893031757 + }, + { + "x": 5.177441529342695, + "y": 4.385247292254255, + "heading": 5.748631283020555e-18, + "angularVelocity": 3.076779569476711e-17, + "velocityX": -3.4401852674457762, + "velocityY": 1.5710847845312972, + "timestamp": 6.94092366280203 + }, + { + "x": 5.060510651244131, + "y": 4.444405251423305, + "heading": 6.298107237796981e-18, + "angularVelocity": 1.5857997280196432e-17, + "velocityX": -3.374650939207116, + "velocityY": 1.7073117530438153, + "timestamp": 6.975573432572303 + }, + { + "x": 4.946037340921014, + "y": 4.5081889164365, + "heading": 6.498139126492573e-18, + "angularVelocity": 5.772964438776829e-18, + "velocityX": -3.303724991019314, + "velocityY": 1.840810644228753, + "timestamp": 7.010223202342576 + }, + { + "x": 4.834204484779198, + "y": 4.576496362942444, + "heading": 6.284318947548799e-18, + "angularVelocity": -6.170897537311036e-18, + "velocityX": -3.227520900810133, + "velocityY": 1.971367976145888, + "timestamp": 7.04487297211285 + }, + { + "x": 4.725190735681975, + "y": 4.649218420709879, + "heading": 4.218601510181254e-18, + "angularVelocity": -5.961706098087163e-17, + "velocityX": -3.146160849551947, + "velocityY": 2.098774631103723, + "timestamp": 7.079522741883123 + }, + { + "x": 4.619170166207779, + "y": 4.726238769851069, + "heading": 2.3861247317221027e-18, + "angularVelocity": -5.2885684107236655e-17, + "velocityX": -3.059777025276331, + "velocityY": 2.22282426843907, + "timestamp": 7.114172511653396 + }, + { + "x": 4.5101566662173465, + "y": 4.798961201296667, + "heading": 8.546946946648484e-19, + "angularVelocity": -4.419740873346574e-17, + "velocityX": -3.146153660275008, + "velocityY": 2.0987854155379675, + "timestamp": 7.148822281423669 + }, + { + "x": 4.398324060290128, + "y": 4.867269057737162, + "heading": -1.0977695137154222e-18, + "angularVelocity": -5.634854780637897e-17, + "velocityX": -3.2275136795616595, + "velocityY": 1.9713798069474653, + "timestamp": 7.1834720511939425 + }, + { + "x": 4.283850992630547, + "y": 4.931053147799285, + "heading": -2.453093242554037e-18, + "angularVelocity": -3.911494182571376e-17, + "velocityX": -3.3037179876961247, + "velocityY": 1.8408229112346155, + "timestamp": 7.218121820964216 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -2.7306183592260526e-17, - "angularVelocity": 1.3830056156762042e-17, - "velocityX": -3.077815742368318, - "velocityY": 1.758109592119851, - "timestamp": 7.605307856537828 - }, - { - "x": 3.9493379060680507, - "y": 5.0986210510281875, - "heading": -2.5526201955845404e-17, - "angularVelocity": 2.4369765067325514e-17, - "velocityX": -2.986848903907283, - "velocityY": 1.487847763803033, - "timestamp": 7.678348430852026 - }, - { - "x": 3.742771574255505, - "y": 5.189991789335339, - "heading": -2.3046567381049293e-17, - "angularVelocity": 3.3948727787048854e-17, - "velocityX": -2.828103882698977, - "velocityY": 1.2509586509287764, - "timestamp": 7.7513890051662235 - }, - { - "x": 3.5509457192311733, - "y": 5.266647381491633, - "heading": -1.9951230504969442e-17, - "angularVelocity": 4.2378320613588127e-17, - "velocityX": -2.6262917128657457, - "velocityY": 1.0494932833707686, - "timestamp": 7.824429579480421 - }, - { - "x": 3.3757737671252985, - "y": 5.330794386309625, - "heading": -1.631318519146512e-17, - "angularVelocity": 4.980855295391938e-17, - "velocityX": -2.3982827866651224, - "velocityY": 0.8782379577418434, - "timestamp": 7.897470153794619 - }, - { - "x": 3.218440544499334, - "y": 5.384189785860755, - "heading": -1.2190567343791479e-17, - "angularVelocity": 5.64428454512083e-17, - "velocityX": -2.1540523757270402, - "velocityY": 0.7310375096646903, - "timestamp": 7.970510728108817 - }, - { - "x": 3.0797086044379114, - "y": 5.42821450731098, - "heading": -8.80046228629925e-18, - "angularVelocity": 4.6413997826950126e-17, - "velocityX": -1.8993818348777063, - "velocityY": 0.6027433637206029, - "timestamp": 8.043551302423014 - }, - { - "x": 2.960089565045244, - "y": 5.463961887551477, - "heading": -5.007321106893989e-18, - "angularVelocity": 5.193197363283625e-17, - "velocityX": -1.6377067200773132, - "velocityY": 0.4894181155630415, - "timestamp": 8.11659187673721 - }, - { - "x": 2.8599401252574177, - "y": 5.4923098040640115, - "heading": -3.3892541193893154e-18, - "angularVelocity": 2.2152988290333294e-17, - "velocityX": -1.3711480328319254, - "velocityY": 0.38811190600159495, - "timestamp": 8.189632451051407 - }, - { - "x": 2.779517497378913, - "y": 5.513974137504595, - "heading": -1.420472445548671e-18, - "angularVelocity": 2.695463024933353e-17, - "velocityX": -1.1010678466539927, - "velocityY": 0.2966068331745371, - "timestamp": 8.262673025365604 - }, - { - "x": 2.7190126242063695, - "y": 5.529547456775984, - "heading": 8.795935357239642e-19, - "angularVelocity": 3.149025049250684e-17, - "velocityX": -0.828373458733645, - "velocityY": 0.21321463334061347, - "timestamp": 8.335713599679801 - }, - { - "x": 2.6785708390344105, - "y": 5.539527036788551, - "heading": 2.892310957635007e-19, - "angularVelocity": -8.082664265777e-18, - "velocityX": -0.5536893096978018, - "velocityY": 0.1366306344969068, - "timestamp": 8.408754173993998 + "heading": -1.5610631193350687e-18, + "angularVelocity": 2.5744186155726097e-17, + "velocityX": -3.3579422894520032, + "velocityY": 1.6997125365941679, + "timestamp": 7.252771590734489 + }, + { + "x": 3.941206853693096, + "y": 5.086799964367495, + "heading": 3.827307300721309e-18, + "angularVelocity": 7.831135017939884e-17, + "velocityX": -3.2887955452772286, + "velocityY": 1.4075914407764616, + "timestamp": 7.321578607951182 + }, + { + "x": 3.730310577999757, + "y": 5.1698821724733195, + "heading": 7.528976478562542e-18, + "angularVelocity": 5.37978439929115e-17, + "velocityX": -3.0650402273530597, + "velocityY": 1.2074670791813904, + "timestamp": 7.390385625167876 + }, + { + "x": 3.5370931608840936, + "y": 5.242282386882321, + "heading": 5.4493611354862e-18, + "angularVelocity": -3.022388452804216e-17, + "velocityX": -2.808106279439004, + "velocityY": 1.0522213770870446, + "timestamp": 7.459192642384569 + }, + { + "x": 3.3623805585841375, + "y": 5.305505508408588, + "heading": 3.90044719086442e-18, + "angularVelocity": -2.2510988083436363e-17, + "velocityX": -2.5391683779829535, + "velocityY": 0.9188470025834666, + "timestamp": 7.527999659601263 + }, + { + "x": 3.2065789543659307, + "y": 5.3604207146722205, + "heading": 2.734702057316729e-18, + "angularVelocity": -1.6942241950067733e-17, + "velocityX": -2.2643272520816113, + "velocityY": 0.7981047353162919, + "timestamp": 7.596806676817956 + }, + { + "x": 3.0699254867036734, + "y": 5.407590197311926, + "heading": 1.8584817371846494e-18, + "angularVelocity": -1.2734461622898273e-17, + "velocityX": -1.9860396975485235, + "velocityY": 0.6855330247953545, + "timestamp": 7.66561369403465 + }, + { + "x": 2.952574245770292, + "y": 5.4474063504067525, + "heading": 1.2072006114656177e-18, + "angularVelocity": -9.465330021034912e-18, + "velocityX": -1.7055126886812788, + "velocityY": 0.5786641349302285, + "timestamp": 7.734420711251343 + }, + { + "x": 2.8546328911186802, + "y": 5.480158244239293, + "heading": 7.336690244970979e-19, + "angularVelocity": -6.882024626605053e-18, + "velocityX": -1.4234210203178261, + "velocityY": 0.4759964195133704, + "timestamp": 7.803227728468037 + }, + { + "x": 2.7761806703190945, + "y": 5.5060675412854945, + "heading": 4.019181330238403e-19, + "angularVelocity": -4.821468868916063e-18, + "velocityX": -1.1401776152062642, + "velocityY": 0.37655021383364773, + "timestamp": 7.87203474568473 + }, + { + "x": 2.7172782497291417, + "y": 5.525309545004062, + "heading": 1.8363150875881817e-19, + "angularVelocity": -3.1724471295939874e-18, + "velocityX": -0.856052521568438, + "velocityY": 0.2796517636852234, + "timestamp": 7.940841762901424 + }, + { + "x": 2.6779735090707537, + "y": 5.538026332588319, + "heading": 5.594227372773024e-20, + "angularVelocity": -1.8557589065219566e-18, + "velocityX": -0.5712315727130863, + "velocityY": 0.18481817841642284, + "timestamp": 8.009648780118116 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -8.28795440431569e-39, - "angularVelocity": -3.959868860281201e-18, - "velocityX": -0.2774577154251034, - "velocityY": 0.06583092414052888, - "timestamp": 8.481794748308195 + "heading": -4.379352373324341e-45, + "angularVelocity": -8.130315190317274e-19, + "velocityX": -0.2858478933472287, + "velocityY": 0.09169170474607218, + "timestamp": 8.07845579733481 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": 1.1194999943590282e-38, - "angularVelocity": 2.6674152729925026e-37, - "velocityX": 1.474313292548536e-35, - "velocityY": 1.9984936751410376e-34, - "timestamp": 8.554835322622392 - }, - { - "x": 2.6757817592371826, - "y": 5.540525112473874, - "heading": -2.263521696805234e-19, - "angularVelocity": -3.3440822564023192e-18, - "velocityX": 0.258195705536965, - "velocityY": -0.056291922762700355, - "timestamp": 8.622522703591729 - }, - { - "x": 2.710655164921984, - "y": 5.532555521074409, - "heading": -6.817251938952584e-19, - "angularVelocity": -6.727591135839588e-18, - "velocityX": 0.5152128090256508, - "velocityY": -0.11774116955531802, - "timestamp": 8.690210084561066 - }, - { - "x": 2.762825274578906, - "y": 5.520028158351496, - "heading": -1.3690954482745913e-18, - "angularVelocity": -1.01550724010861e-17, - "velocityX": 0.7707508978749663, - "velocityY": -0.18507678305040806, - "timestamp": 8.757897465530403 - }, - { - "x": 2.832164805136809, - "y": 5.502484798756891, - "heading": -2.291792592298148e-18, - "angularVelocity": -1.3631745397966465e-17, - "velocityX": 1.024408531766262, - "velocityY": -0.25918213030803783, - "timestamp": 8.82558484649974 - }, - { - "x": 2.9185095362845113, - "y": 5.479394153237709, - "heading": -3.2554537025043674e-18, - "angularVelocity": -1.4236938945256572e-17, - "velocityX": 1.2756400072092755, - "velocityY": -0.34113663711766645, - "timestamp": 8.893272227469078 - }, - { - "x": 3.021644093219496, - "y": 5.4501349612076035, - "heading": -4.462356984669075e-18, - "angularVelocity": -1.7830550759992628e-17, - "velocityX": 1.5236895778506316, - "velocityY": -0.4322695251476627, - "timestamp": 8.960959608438415 - }, - { - "x": 3.1412808309812497, - "y": 5.413974520765914, - "heading": -5.917171487112631e-18, - "angularVelocity": -2.149314217260227e-17, - "velocityX": 1.767489538647525, - "velocityY": -0.5342272063690919, - "timestamp": 9.028646989407752 - }, - { - "x": 3.2770278679497706, - "y": 5.3700418577232005, - "heading": -2.5772013184954745e-18, - "angularVelocity": 4.9344059717056357e-17, - "velocityX": 2.0054999177766235, - "velocityY": -0.6490524882712698, - "timestamp": 9.09633437037709 - }, - { - "x": 3.428339922513116, - "y": 5.317295603371641, - "heading": 5.041855021881816e-19, - "angularVelocity": 4.552380039793498e-17, - "velocityX": 2.2354544140493458, - "velocityY": -0.7792627458204253, - "timestamp": 9.164021751346427 - }, - { - "x": 3.594442154384912, - "y": 5.254489538104451, - "heading": 3.3214379722263567e-18, - "angularVelocity": 4.1621531660920524e-17, - "velocityX": 2.453961572941347, - "velocityY": -0.9278844057453193, - "timestamp": 9.231709132315764 - }, - { - "x": 3.774213631656669, - "y": 5.18014653899489, - "heading": 5.8698404540777154e-18, - "angularVelocity": 3.764959502608201e-17, - "velocityX": 2.6559083051712964, - "velocityY": -1.0983287880977073, - "timestamp": 9.299396513285101 - }, - { - "x": 3.966018710550382, - "y": 5.092569435745629, - "heading": 8.147436412478004e-18, - "angularVelocity": 3.364875292561275e-17, - "velocityX": 2.8336903592207485, - "velocityY": -1.2938468292772856, - "timestamp": 9.367083894254439 + "heading": -7.265203918119814e-45, + "angularVelocity": -4.19412391684243e-44, + "velocityX": 0, + "velocityY": -6.607621745560535e-42, + "timestamp": 8.147262814551503 + }, + { + "x": 2.6751665057839835, + "y": 5.538944468629318, + "heading": 4.149201789004472e-19, + "angularVelocity": 6.513823637389646e-18, + "velocityX": 0.2647058041795455, + "velocityY": -0.08463157955674916, + "timestamp": 8.210961219973287 + }, + { + "x": 2.7088624712986666, + "y": 5.5280798374619815, + "heading": 1.747454129766702e-18, + "angularVelocity": 2.0919423995667872e-17, + "velocityX": 0.5289922925316967, + "velocityY": -0.1705636286402431, + "timestamp": 8.27465962539507 + }, + { + "x": 2.7593613029745305, + "y": 5.511644735540107, + "heading": 3.0764256336987273e-18, + "angularVelocity": 2.086349721209079e-17, + "velocityX": 0.79278015425161, + "velocityY": -0.2580143382404696, + "timestamp": 8.338358030816854 + }, + { + "x": 2.8266246280660527, + "y": 5.489524723354374, + "heading": 4.563957990922327e-18, + "angularVelocity": 2.3352740894748934e-17, + "velocityX": 1.0559656030026554, + "velocityY": -0.3472616314217525, + "timestamp": 8.402056436238638 + }, + { + "x": 2.9106051995536757, + "y": 5.461582331644454, + "heading": 6.988237520643839e-18, + "angularVelocity": 3.8058716127490844e-17, + "velocityX": 1.31840932173323, + "velocityY": -0.43866705178720633, + "timestamp": 8.465754841660422 + }, + { + "x": 3.0112434875257597, + "y": 5.427649372766819, + "heading": 9.422486137058152e-18, + "angularVelocity": 3.821522062123431e-17, + "velocityX": 1.5799184815647955, + "velocityY": -0.5327128466237975, + "timestamp": 8.529453247082206 + }, + { + "x": 3.128462311204818, + "y": 5.387515427846556, + "heading": 1.2002270824368104e-17, + "angularVelocity": 4.0499988504071535e-17, + "velocityX": 1.8402159819054258, + "velocityY": -0.6300620031932208, + "timestamp": 8.59315165250399 + }, + { + "x": 3.2621579049769087, + "y": 5.340909820689257, + "heading": 1.4607724763792015e-17, + "angularVelocity": 4.0902969582545765e-17, + "velocityX": 2.0988844679362657, + "velocityY": -0.7316604999559294, + "timestamp": 8.656850057925773 + }, + { + "x": 3.4121839694430376, + "y": 5.287471786804592, + "heading": 1.2983801342724242e-17, + "angularVelocity": -2.5493941493744293e-17, + "velocityX": 2.3552562026116504, + "velocityY": -0.8389226312781332, + "timestamp": 8.720548463347557 + }, + { + "x": 3.5783204640757695, + "y": 5.226697519809262, + "heading": 1.1414351740343512e-17, + "angularVelocity": -2.4638758097451353e-17, + "velocityX": 2.608173525422576, + "velocityY": -0.9540940089929807, + "timestamp": 8.78424686876934 + }, + { + "x": 3.7602044322338806, + "y": 5.157837260218168, + "heading": 1.0662286375073935e-17, + "angularVelocity": -1.1806659213675993e-17, + "velocityX": 2.855392799140749, + "velocityY": -1.081035845954548, + "timestamp": 8.847945274191124 + }, + { + "x": 3.9571467965572817, + "y": 5.079670509622147, + "heading": 1.0033604735305209e-17, + "angularVelocity": -9.869660560666429e-18, + "velocityX": 3.091794261085985, + "velocityY": -1.2271382631705257, + "timestamp": 8.911643679612908 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 1.0158795753511172e-17, - "angularVelocity": 2.971542571519468e-17, - "velocityX": 2.9766309755737, - "velocityY": -1.5161118425338018, - "timestamp": 9.434771275223776 - }, - { - "x": 4.261627503076192, - "y": 4.9391622522038245, - "heading": 1.103199717945149e-17, - "angularVelocity": 2.8019595592305015e-17, - "velocityX": 3.020426535396691, - "velocityY": -1.629624452324509, - "timestamp": 9.465935230570453 - }, - { - "x": 4.357244155674493, - "y": 4.884889313220403, - "heading": 1.184832535139553e-17, - "angularVelocity": 2.619462654200126e-17, - "velocityX": 3.068180901128632, - "velocityY": -1.7415292243770963, - "timestamp": 9.497099185917131 - }, - { - "x": 4.454522517564495, - "y": 4.827208233471932, - "heading": 1.2602189603982203e-17, - "angularVelocity": 2.4190262248102175e-17, - "velocityX": 3.1215024154618987, - "velocityY": -1.8508908483152517, - "timestamp": 9.528263141263809 - }, - { - "x": 4.553721568715114, - "y": 4.766257957931852, - "heading": 1.3285416566033512e-17, - "angularVelocity": 2.1923627934667792e-17, - "velocityX": 3.18313417045742, - "velocityY": -1.9557939568983074, - "timestamp": 9.559427096610486 - }, - { - "x": 4.6552566098247095, - "y": 4.702321075934725, - "heading": 1.3885398069122134e-17, - "angularVelocity": 1.9252418261752656e-17, - "velocityX": 3.258092240862523, - "velocityY": -2.0516292391602087, - "timestamp": 9.590591051957164 - }, - { - "x": 4.7598200255019725, - "y": 4.636102620452092, - "heading": 1.4382811423998524e-17, - "angularVelocity": 1.5961175314853973e-17, - "velocityX": 3.3552677930020853, - "velocityY": -2.124841174555608, - "timestamp": 9.621755007303841 - }, - { - "x": 4.868161384056622, - "y": 4.569562096793983, - "heading": 1.476085010345509e-17, - "angularVelocity": 1.2130638591480905e-17, - "velocityX": 3.476495757660601, - "velocityY": -2.135175811859933, - "timestamp": 9.652918962650519 - }, - { - "x": 4.97947280914236, - "y": 4.50537854159068, - "heading": 1.3849367053817892e-17, - "angularVelocity": -2.9247989846367464e-17, - "velocityX": 3.571800300939838, - "velocityY": -2.0595445760752855, - "timestamp": 9.684082917997197 - }, - { - "x": 5.09247899341136, - "y": 4.444586805435104, - "heading": 1.2651254648975303e-17, - "angularVelocity": -3.84454537766135e-17, - "velocityX": 3.6261823318601505, - "velocityY": -1.9507066891641414, - "timestamp": 9.715246873343874 - }, - { - "x": 5.206458430184718, - "y": 4.387459691402226, - "heading": 1.1425663620436537e-17, - "angularVelocity": -3.932719755362708e-17, - "velocityX": 3.657412401776808, - "velocityY": -1.8331150008841195, - "timestamp": 9.746410828690552 - }, - { - "x": 5.321007172735707, - "y": 4.334081255049934, - "heading": 1.0655255032861655e-17, - "angularVelocity": -2.472114270913484e-17, - "velocityX": 3.675680486533668, - "velocityY": -1.712826108191114, - "timestamp": 9.77757478403723 - }, - { - "x": 5.435875810085393, - "y": 4.284480962664817, - "heading": 9.880141449219796e-18, - "angularVelocity": -2.4872118283779253e-17, - "velocityX": 3.68594538375666, - "velocityY": -1.5915916908925372, - "timestamp": 9.808738739383907 - }, - { - "x": 5.550897094649704, - "y": 4.2386692598409, - "heading": 9.106151253635088e-18, - "angularVelocity": -2.483607061124937e-17, - "velocityX": 3.6908435814638634, - "velocityY": -1.470022091685541, - "timestamp": 9.839902694730585 + "heading": 9.614004929327239e-18, + "angularVelocity": -6.58728900981987e-18, + "velocityX": 3.3023160854502263, + "velocityY": -1.4085550989874989, + "timestamp": 8.975342085034692 + }, + { + "x": 4.2842075611264425, + "y": 4.936417577245955, + "heading": 8.433110849470374e-18, + "angularVelocity": -3.4077840923717525e-17, + "velocityX": 3.3679341946759727, + "velocityY": -1.5447568972758872, + "timestamp": 9.009994928285538 + }, + { + "x": 4.401356584826297, + "y": 4.877666966713146, + "heading": 7.494016874909782e-18, + "angularVelocity": -2.7100055477775347e-17, + "velocityX": 3.3806468015288584, + "velocityY": -1.695405196841212, + "timestamp": 9.044647771536384 + }, + { + "x": 4.516063976001338, + "y": 4.814281195629901, + "heading": 5.449898069000543e-18, + "angularVelocity": -5.898848735476586e-17, + "velocityX": 3.310186998068025, + "velocityY": -1.8291650882556743, + "timestamp": 9.07930061478723 + }, + { + "x": 4.628146410125642, + "y": 4.746361612927903, + "heading": 3.873819251911276e-18, + "angularVelocity": -4.5481948066433736e-17, + "velocityX": 3.234436877602133, + "velocityY": -1.9600002865663548, + "timestamp": 9.113953458038077 + }, + { + "x": 4.737424855337868, + "y": 4.674016859733605, + "heading": 3.562150006954802e-18, + "angularVelocity": -8.994045386127452e-18, + "velocityX": 3.1535203163901007, + "velocityY": -2.0877003560892837, + "timestamp": 9.148606301288924 + }, + { + "x": 4.844483096398323, + "y": 4.598425359059355, + "heading": 4.67240858769656e-18, + "angularVelocity": 3.2039465642249735e-17, + "velocityX": 3.0894504178337554, + "velocityY": -2.181393893916723, + "timestamp": 9.18325914453977 + }, + { + "x": 4.954476829355994, + "y": 4.5271728465414585, + "heading": 6.725892368510384e-18, + "angularVelocity": 5.925873862496592e-17, + "velocityX": 3.174161847599147, + "velocityY": -2.0561808450207577, + "timestamp": 9.217911987790616 + }, + { + "x": 5.06723038345343, + "y": 4.460373312794278, + "heading": 8.84315284493419e-18, + "angularVelocity": 6.109918488065446e-17, + "velocityX": 3.2538038302147596, + "velocityY": -1.9276782936288406, + "timestamp": 9.252564831041463 + }, + { + "x": 5.18256360847748, + "y": 4.398133545147612, + "heading": 1.1174088851587773e-17, + "angularVelocity": 6.726536087617125e-17, + "velocityX": 3.32824709906689, + "velocityY": -1.796094109684512, + "timestamp": 9.28721767429231 + }, + { + "x": 5.300292214676822, + "y": 4.340553023175947, + "heading": 1.351328882085302e-17, + "angularVelocity": 6.750383950696769e-17, + "velocityX": 3.3973721967667427, + "velocityY": -1.6616391779124182, + "timestamp": 9.321870517543156 + }, + { + "x": 5.420228077668345, + "y": 4.287723772710641, + "heading": 1.5740193529975693e-17, + "angularVelocity": 6.4263260968298e-17, + "velocityX": 3.461068464810413, + "velocityY": -1.5245285959043573, + "timestamp": 9.356523360794002 + }, + { + "x": 5.542179542504676, + "y": 4.239730223009142, + "heading": 1.83851008704834e-17, + "angularVelocity": 7.632583916308507e-17, + "velocityX": 3.5192340193716003, + "velocityY": -1.3849815830141698, + "timestamp": 9.391176204044848 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 8.34871718979539e-18, - "angularVelocity": -2.430481161157101e-17, - "velocityX": 3.691913715418776, - "velocityY": -1.3483585385427073, - "timestamp": 9.871066650077262 - }, - { - "x": 5.935813024971261, - "y": 4.118882926647287, - "heading": 6.616284346114717e-18, - "angularVelocity": -2.3674050338447945e-17, - "velocityX": 3.6877099925560515, - "velocityY": -1.062690370240863, - "timestamp": 9.944245207147963 - }, - { - "x": 6.200932724790643, - "y": 4.061479044678689, - "heading": 4.986300781220053e-18, - "angularVelocity": -2.2274059919222546e-17, - "velocityX": 3.6229151056263116, - "velocityY": -0.7844358274670198, - "timestamp": 10.017423764218664 - }, - { - "x": 6.456598704926571, - "y": 4.022722730407595, - "heading": 3.514116805070136e-18, - "angularVelocity": -2.0117696155433974e-17, - "velocityX": 3.4937280860691824, - "velocityY": -0.5296129880457788, - "timestamp": 10.090602321289365 - }, - { - "x": 6.6986166899486275, - "y": 3.999804245428159, - "heading": 2.243253754519844e-18, - "angularVelocity": -1.7366604392085868e-17, - "velocityX": 3.3072254320105032, - "velocityY": -0.31318580055210804, - "timestamp": 10.163780878360066 - }, - { - "x": 6.923883976637484, - "y": 3.9893965055012424, - "heading": 1.2009079614811148e-18, - "angularVelocity": -1.42438691719364e-17, - "velocityX": 3.078323701726084, - "velocityY": -0.1422239019670904, - "timestamp": 10.236959435430768 - }, - { - "x": 7.1304235610549185, - "y": 3.9882827335868942, - "heading": 4.0101770388899655e-19, - "angularVelocity": -1.0930664523394714e-17, - "velocityX": 2.822405806907137, - "velocityY": -0.01521992177670267, - "timestamp": 10.310137992501469 - }, - { - "x": 7.317098326514511, - "y": 3.9936876792547547, - "heading": -1.5059558453470426e-19, - "angularVelocity": -7.537908788371176e-18, - "velocityX": 2.5509489792103524, - "velocityY": 0.07385969174875201, - "timestamp": 10.38331654957217 - }, - { - "x": 7.48330233647595, - "y": 4.00334149700279, - "heading": -4.524148120436366e-19, - "angularVelocity": -4.1244216832965696e-18, - "velocityX": 2.2712119043405092, - "velocityY": 0.13192140067354147, - "timestamp": 10.45649510664287 - }, - { - "x": 7.628740531129745, - "y": 4.015421358409772, - "heading": -5.050368427376048e-19, - "angularVelocity": -7.190908486340082e-19, - "velocityX": 1.987442776622101, - "velocityY": 0.1650737851432527, - "timestamp": 10.529673663713572 - }, - { - "x": 7.753293848479072, - "y": 4.028463870964023, - "heading": -3.1002864921528455e-19, - "angularVelocity": 2.6648269843554143e-18, - "velocityX": 1.7020466422833307, - "velocityY": 0.17822861062495024, - "timestamp": 10.602852220784273 - }, - { - "x": 7.856941288758159, - "y": 4.0412847502588685, - "heading": 2.203678311282748e-19, - "angularVelocity": 7.247976752183959e-18, - "velocityX": 1.4163635418357239, - "velocityY": 0.17519994665183636, - "timestamp": 10.676030777854974 - }, - { - "x": 7.939715627162556, - "y": 4.052914641548701, - "heading": 3.4450417763013896e-19, - "angularVelocity": 1.6963486504629427e-18, - "velocityX": 1.1311283211614112, - "velocityY": 0.15892485114999483, - "timestamp": 10.749209334925675 - }, - { - "x": 8.00167823825641, - "y": 4.062550327487905, - "heading": 7.102203419824833e-19, - "angularVelocity": 4.997586438339274e-18, - "velocityX": 0.8467317965013967, - "velocityY": 0.1316736257848719, - "timestamp": 10.822387891996376 - }, - { - "x": 8.042904705550558, - "y": 4.0695182051166965, - "heading": 2.3618432486443475e-19, - "angularVelocity": -6.477799455470437e-18, - "velocityX": 0.5633681360281035, - "velocityY": 0.09521747773817751, - "timestamp": 10.895566449067077 + "heading": 2.1337719863838943e-17, + "angularVelocity": 8.520567769813293e-17, + "velocityX": 3.571775782441806, + "velocityY": -1.2432211735943428, + "timestamp": 9.425829047295695 + }, + { + "x": 5.936838928321791, + "y": 4.127579524293423, + "heading": 2.7128761029914745e-17, + "angularVelocity": 7.834427123009397e-17, + "velocityX": 3.6647054686449687, + "velocityY": -0.9344094480089555, + "timestamp": 9.499746911041823 + }, + { + "x": 6.212625572925942, + "y": 4.081838891930782, + "heading": 3.048119806681363e-17, + "angularVelocity": 4.535354333849354e-17, + "velocityX": 3.730987756239054, + "velocityY": -0.6188034941017404, + "timestamp": 9.573664774787952 + }, + { + "x": 6.491306353239061, + "y": 4.059759739930169, + "heading": 2.990728340154358e-17, + "angularVelocity": -7.764221477519524e-18, + "velocityX": 3.770141156544387, + "velocityY": -0.2986984590956799, + "timestamp": 9.64758263853408 + }, + { + "x": 6.752875972710039, + "y": 4.054277278014817, + "heading": 2.289191300833474e-17, + "angularVelocity": -9.490764529266178e-17, + "velocityX": 3.538652312373937, + "velocityY": -0.07416964773468503, + "timestamp": 9.72150050228021 + }, + { + "x": 6.991092096287726, + "y": 4.05357587432536, + "heading": 1.9322766501808003e-17, + "angularVelocity": -4.8285303790501985e-17, + "velocityX": 3.2227138543376115, + "velocityY": -0.009488960501710962, + "timestamp": 9.795418366026338 + }, + { + "x": 7.205567187241114, + "y": 4.0550211409111485, + "heading": 1.4185957310010018e-17, + "angularVelocity": -6.949347466859182e-17, + "velocityX": 2.9015325942048964, + "velocityY": 0.01955233163599203, + "timestamp": 9.869336229772466 + }, + { + "x": 7.396227777452996, + "y": 4.057522153925314, + "heading": 7.96211441465592e-18, + "angularVelocity": -8.419944219072616e-17, + "velocityX": 2.579357418481523, + "velocityY": 0.03383502833300578, + "timestamp": 9.943254093518595 + }, + { + "x": 7.5630549716186115, + "y": 4.0604862007272615, + "heading": 9.124881669040973e-19, + "angularVelocity": -9.537107663127069e-17, + "velocityX": 2.2569266170703433, + "velocityY": 0.040099194588843756, + "timestamp": 10.017171957264724 + }, + { + "x": 7.70604444565388, + "y": 4.0635415850381555, + "heading": -6.759455196513469e-18, + "angularVelocity": -1.0379011208666613e-16, + "velocityX": 1.934437317160139, + "velocityY": 0.04133485677276264, + "timestamp": 10.091089821010852 + }, + { + "x": 7.825196583277639, + "y": 4.066433602906457, + "heading": -4.535875150648457e-18, + "angularVelocity": 3.0081768238080076e-17, + "velocityX": 1.6119532084015247, + "velocityY": 0.03912474903542123, + "timestamp": 10.165007684756981 + }, + { + "x": 7.920513375065405, + "y": 4.068976876297905, + "heading": -2.8949605954376388e-18, + "angularVelocity": 2.219916096123338e-17, + "velocityX": 1.2894960292025357, + "velocityY": 0.03440674909358656, + "timestamp": 10.23892554850311 + }, + { + "x": 7.991997299424134, + "y": 4.071030463180751, + "heading": -1.5603341535814708e-18, + "angularVelocity": 1.8055533185319825e-17, + "velocityX": 0.9670723792051263, + "velocityY": 0.02778201071799316, + "timestamp": 10.312843412249238 + }, + { + "x": 8.03965088919926, + "y": 4.07248360094205, + "heading": -5.821232156931401e-19, + "angularVelocity": 1.3233755526918469e-17, + "velocityX": 0.6446829948819025, + "velocityY": 0.019658817066051913, + "timestamp": 10.386761275995367 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 2.0025514182915097e-40, - "angularVelocity": -3.2275072689602567e-18, - "velocityX": 0.2811186469496163, - "velocityY": 0.05095414427594005, - "timestamp": 10.968745006137778 + "heading": 8.014500942600024e-44, + "angularVelocity": 7.875271094040935e-18, + "velocityX": 0.322326324020543, + "velocityY": 0.0103270696803873, + "timestamp": 10.460679139741496 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 3.2506461802069203e-40, - "angularVelocity": 1.7055464658974957e-39, - "velocityX": 1.2054870824295223e-36, - "velocityY": -6.907165459298352e-35, - "timestamp": 11.04192356320848 + "heading": -7.373298274013602e-44, + "angularVelocity": -1.7505506389116263e-42, + "velocityX": -2.259320082617454e-45, + "velocityY": 0, + "timestamp": 10.534597003487624 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.1.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.1.traj index 3fe80902..5044b6d7 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.1.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.1.traj @@ -4,334 +4,316 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": -1.960896274672709e-29, + "angularVelocity": -6.079995614896932e-33, "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 1.3069454293752283, - "y": 5.5749747243722725, - "heading": -0.008996462194644695, - "angularVelocity": -0.11617133047486594, - "velocityX": 0.21914626962200023, - "velocityY": -0.20634433353719148, - "timestamp": 0.07744132875022905 - }, - { - "x": 1.340887418869277, - "y": 5.543015778149598, - "heading": -0.02698653550593814, - "angularVelocity": -0.23230584497508605, - "velocityX": 0.4382929637423898, - "velocityY": -0.4126859228589361, - "timestamp": 0.1548826575004581 - }, - { - "x": 1.391800616271645, - "y": 5.495077701168919, - "heading": -0.05395916302114244, - "angularVelocity": -0.34829758154372603, - "velocityX": 0.6574421981657251, - "velocityY": -0.6190244634792922, - "timestamp": 0.23232398625068715 - }, - { - "x": 1.459685380828832, - "y": 5.431160783444383, - "heading": -0.08989644453732974, - "angularVelocity": -0.4640581727637966, - "velocityX": 0.8765960715396762, - "velocityY": -0.8253592591464762, - "timestamp": 0.3097653150009162 - }, - { - "x": 1.5445422260314938, - "y": 5.351265403464515, - "heading": -0.13477553337937057, - "angularVelocity": -0.579523744831976, - "velocityX": 1.0957565756192689, - "velocityY": -1.0316891673907769, - "timestamp": 0.3872066437511452 - }, - { - "x": 1.6463718016735291, - "y": 5.255392061024326, - "heading": -0.18857099282512366, - "angularVelocity": -0.6946608524671277, - "velocityX": 1.3149254704884978, - "velocityY": -1.2380126217812786, - "timestamp": 0.46464797250137424 - }, - { - "x": 1.7651748655395048, - "y": 5.143541401111082, - "heading": -0.25125740651293493, - "angularVelocity": -0.8094697585842596, - "velocityX": 1.5341041506310307, - "velocityY": -1.4443277474502212, - "timestamp": 0.5420893012516033 - }, - { - "x": 1.9009522484980113, - "y": 5.015714222493906, - "heading": -0.3228118649000377, - "angularVelocity": -0.9239828337396184, - "velocityX": 1.7532935597809811, - "velocityY": -1.6506325586122341, - "timestamp": 0.6195306300018323 - }, - { - "x": 2.053704829775847, - "y": 4.871911475725846, - "heading": -0.4032156639032303, - "angularVelocity": -1.0382543830392266, - "velocityX": 1.9724943223879574, - "velocityY": -1.8569250952812175, - "timestamp": 0.6969719587520613 - }, - { - "x": 2.1895539905378896, - "y": 4.744100163466478, - "heading": -0.47392252589131595, - "angularVelocity": -0.9130378201026126, - "velocityX": 1.7542204266665722, - "velocityY": -1.6504276763046646, - "timestamp": 0.7744132875022903 - }, - { - "x": 2.3084268719206142, - "y": 4.632263014410205, - "heading": -0.535794025352391, - "angularVelocity": -0.7989467698895206, - "velocityX": 1.5350057043302445, - "velocityY": -1.4441532817287048, - "timestamp": 0.8518546162525193 - }, - { - "x": 2.410322968991003, - "y": 4.536399460392939, - "heading": -0.5888342696498629, - "angularVelocity": -0.6849087580665111, - "velocityX": 1.3157844617930656, - "velocityY": -1.237886223856486, - "timestamp": 0.9292959450027484 - }, - { - "x": 2.495241902358175, - "y": 4.456509045521313, - "heading": -0.6330449707790676, - "angularVelocity": -0.5708928532419475, - "velocityX": 1.096558320180854, - "velocityY": -1.0316250529360251, - "timestamp": 1.0067372737529774 - }, - { - "x": 2.5631833814386416, - "y": 4.392591413105038, - "heading": -0.6684266086422785, - "angularVelocity": -0.4568831453961845, - "velocityX": 0.8773284262635802, - "velocityY": -0.8253684879578497, - "timestamp": 1.0841786025032065 - }, - { - "x": 2.6141471938399374, - "y": 4.34464629797767, - "heading": -0.694979006837532, - "angularVelocity": -0.34287115967158255, - "velocityX": 0.6580957897220853, - "velocityY": -0.6191153470743593, - "timestamp": 1.1616199312534357 - }, - { - "x": 2.648133201868436, - "y": 4.3126735215310195, - "heading": -0.7127016614525554, - "angularVelocity": -0.22885266692700632, - "velocityX": 0.4388613751425609, - "velocityY": -0.4128645125587087, - "timestamp": 1.2390612600036648 - }, - { - "x": 2.6651413416897705, - "y": 4.296672988888067, - "heading": -0.7215939290593514, - "angularVelocity": -0.11482586559568532, - "velocityX": 0.21962613626976676, - "velocityY": -0.20661490319016262, - "timestamp": 1.316502588753894 + "x": 1.306947763000364, + "y": 5.574973108257302, + "heading": -0.008994433988091095, + "angularVelocity": -0.12277149964907928, + "velocityX": 0.23168094435597542, + "velocityY": -0.21813883323536412, + "timestamp": 0.07326157955062942 + }, + { + "x": 1.3408944736698363, + "y": 5.543010933982932, + "heading": -0.026978879063552612, + "angularVelocity": -0.24548262794460876, + "velocityX": 0.463363073492191, + "velocityY": -0.4362747086592839, + "timestamp": 0.14652315910125885 + }, + { + "x": 1.3918148440551168, + "y": 5.495068027805583, + "heading": -0.05394057952659419, + "angularVelocity": -0.3680196445178875, + "velocityX": 0.695048764954495, + "velocityY": -0.6544072141417159, + "timestamp": 0.21978473865188827 + }, + { + "x": 1.4597093144768722, + "y": 5.431144700814373, + "heading": -0.08985971213865897, + "angularVelocity": -0.4902860794482582, + "velocityX": 0.9267404666703231, + "velocityY": -0.8725354733449758, + "timestamp": 0.2930463182025177 + }, + { + "x": 1.5445785095442424, + "y": 5.351241370164591, + "heading": -0.13471107898519388, + "angularVelocity": -0.6122085699167754, + "velocityX": 1.1584406941256162, + "velocityY": -1.0906580384956333, + "timestamp": 0.3663078977531471 + }, + { + "x": 1.6464232512218273, + "y": 5.2553586089581215, + "heading": -0.1884659824129613, + "angularVelocity": -0.733739345472597, + "velocityX": 1.390152141167011, + "velocityY": -1.3087727809664234, + "timestamp": 0.4395694773037765 + }, + { + "x": 1.7652446226105383, + "y": 5.143497220947543, + "heading": -0.25109340653071877, + "angularVelocity": -0.8548467628175698, + "velocityX": 1.6218783722318773, + "velocityY": -1.5268765524400676, + "timestamp": 0.5128310568544059 + }, + { + "x": 1.9010443771824763, + "y": 5.015658495720495, + "heading": -0.32255561727496634, + "angularVelocity": -0.9754391207858365, + "velocityX": 1.8536285376988066, + "velocityY": -1.7449627214043517, + "timestamp": 0.5860926364050353 + }, + { + "x": 2.053859595250169, + "y": 4.871864096281773, + "heading": -0.4022671776466062, + "angularVelocity": -1.0880404280193408, + "velocityX": 2.0858848390251463, + "velocityY": -1.9627531964329257, + "timestamp": 0.6593542159556647 + }, + { + "x": 2.1896989198822134, + "y": 4.744047518400832, + "heading": -0.47311179344304544, + "angularVelocity": -0.9670091230763074, + "velocityX": 1.8541686579139196, + "velocityY": -1.7446604163456465, + "timestamp": 0.7326157955062941 + }, + { + "x": 2.308560686135844, + "y": 4.632207510590712, + "heading": -0.535112719027594, + "angularVelocity": -0.846295233666115, + "velocityX": 1.6224297508012064, + "velocityY": -1.5265847187041606, + "timestamp": 0.8058773750569235 + }, + { + "x": 2.410444090432368, + "y": 4.536343322874329, + "heading": -0.5882784511543253, + "angularVelocity": -0.72569732256441, + "velocityX": 1.3906798750648661, + "velocityY": -1.3085192580393974, + "timestamp": 0.8791389546075529 + }, + { + "x": 2.49534860091136, + "y": 4.4564544045860375, + "heading": -0.6326127919617512, + "angularVelocity": -0.6051513095863228, + "velocityX": 1.158922739582985, + "velocityY": -1.090461313806123, + "timestamp": 0.9524005341581823 + }, + { + "x": 2.5632738362111045, + "y": 4.3925403400214185, + "heading": -0.6681173722205684, + "angularVelocity": -0.48462755617056985, + "velocityX": 0.9271603986207089, + "velocityY": -0.8724090438215177, + "timestamp": 1.0256621137088118 + }, + { + "x": 2.6142195241645787, + "y": 4.344600823178412, + "heading": -0.6947927082053813, + "angularVelocity": -0.36411084975827024, + "velocityX": 0.6953943426549579, + "velocityY": -0.6543609506791417, + "timestamp": 1.0989236932594413 + }, + { + "x": 2.6481854848919673, + "y": 4.312635645188571, + "heading": -0.7126387384821254, + "angularVelocity": -0.24359330478823768, + "velocityX": 0.4636258313802147, + "velocityY": -0.43631570853248003, + "timestamp": 1.1721852728100708 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": -0.0007900953867185666, - "velocityX": 0.00039102557193096025, - "velocityY": -0.00036545389408097024, - "timestamp": 1.393943917504123 - }, - { - "x": 2.648217264320523, - "y": 4.31259514704442, - "heading": -0.7128809139831602, - "angularVelocity": 0.1132783500941083, - "velocityX": -0.21888736985187585, - "velocityY": 0.2059266364981681, - "timestamp": 1.471400917058759 - }, - { - "x": 2.6142783804156173, - "y": 4.344524561384219, - "heading": -0.6952725965601283, - "angularVelocity": 0.2273302286961744, - "velocityX": -0.43816419561488745, - "velocityY": 0.4122211617168144, - "timestamp": 1.548857916613395 - }, - { - "x": 2.5633551628668956, - "y": 4.3924332087312665, - "heading": -0.6688313471237388, - "angularVelocity": 0.341366817566861, - "velocityX": -0.6574385509538746, - "velocityY": 0.618519276788985, - "timestamp": 1.6263149161680308 - }, - { - "x": 2.4954478733663135, - "y": 4.456321462374885, - "heading": -0.6335580214350119, - "angularVelocity": 0.45539235822983737, - "velocityX": -0.876709527747899, - "velocityY": 0.8248222111715229, - "timestamp": 1.7037719157226667 - }, - { - "x": 2.41055684840719, - "y": 4.536189798526633, - "heading": -0.5894527108517198, - "angularVelocity": 0.5694167194284366, - "velocityX": -1.0959761602822577, - "velocityY": 1.0311312936305541, - "timestamp": 1.7812289152773026 - }, - { - "x": 2.3086825106199456, - "y": 4.632038804702452, - "heading": -0.5365139521148367, - "angularVelocity": 0.6834599718703669, - "velocityX": -1.3152373364934584, - "velocityY": 1.2374479611490161, - "timestamp": 1.8586859148319386 - }, - { - "x": 2.1898253903863423, - "y": 4.743869185084351, - "heading": -0.47473734314041516, - "angularVelocity": 0.7975600569287656, - "velocityX": -1.5344916652697551, - "velocityY": 1.4437737199311238, - "timestamp": 1.9361429143865745 - }, - { - "x": 2.053986160891222, - "y": 4.871681755197305, - "heading": -0.40411323356241463, - "angularVelocity": 0.91178473196739, - "velocityX": -1.7537373029678356, - "velocityY": 1.6501100074580153, - "timestamp": 2.0135999139412104 - }, - { - "x": 1.901165693105442, - "y": 5.0154774121652865, - "heading": -0.3246230604276491, - "angularVelocity": 1.026249061953906, - "velocityX": -1.9729716960932775, - "velocityY": 1.8564578772043625, - "timestamp": 2.0910569134958465 - }, - { - "x": 1.7653423329783178, - "y": 5.143359507076652, - "heading": -0.25263854799609164, - "angularVelocity": 0.9293480620925062, - "velocityX": -1.7535324232572806, - "velocityY": 1.6510075996592735, - "timestamp": 2.1685139130504827 - }, - { - "x": 1.6464985778710517, - "y": 5.2552571877344505, - "heading": -0.18957308544395354, - "angularVelocity": 0.8141996580657658, - "velocityX": -1.5343191162933176, - "velocityY": 1.4446425926813151, - "timestamp": 2.245970912605119 - }, - { - "x": 1.5446336589350755, - "y": 5.351170031650349, - "heading": -0.135458741257284, - "angularVelocity": 0.6986372374073464, - "velocityX": -1.315115735463508, - "velocityY": 1.2382721312145724, - "timestamp": 2.323427912159755 - }, - { - "x": 1.459746934416973, - "y": 5.431097753001726, - "heading": -0.09032536640283424, - "angularVelocity": 0.5826894291541963, - "velocityX": -1.0959206399199872, - "velocityY": 1.031897979666631, - "timestamp": 2.400884911714391 - }, - { - "x": 1.391837908901594, - "y": 5.49504016165852, - "heading": -0.054198235453019356, - "angularVelocity": 0.4664153163377203, - "velocityX": -0.8767319403787999, - "velocityY": 0.8255213734655752, - "timestamp": 2.4783419112690273 - }, - { - "x": 1.3409062442336714, - "y": 5.542997125969119, - "heading": -0.027096040975571573, - "angularVelocity": 0.3498998751992024, - "velocityX": -0.6575476065536807, - "velocityY": 0.6191430675910059, - "timestamp": 2.5557989108236634 - }, - { - "x": 1.3069517635064558, - "y": 5.574968540706391, - "heading": -0.0090294167092662, - "angularVelocity": 0.2332471483558819, - "velocityX": -0.43836555666351823, - "velocityY": 0.41276340319322796, - "timestamp": 2.6332559103782995 + "angularVelocity": -0.12307101033443453, + "velocityX": 0.23185602115327572, + "velocityY": -0.2182720825031122, + "timestamp": 1.2454468523607003 + }, + { + "x": 2.662943710448623, + "y": 4.298728558024191, + "heading": -0.72070100113944, + "angularVelocity": 0.011651349233264986, + "velocityX": -0.027206592771411266, + "velocityY": 0.025447590705983056, + "timestamp": 1.3273355622067569 + }, + { + "x": 2.639501593010729, + "y": 4.320770481747221, + "heading": -0.7087160822503835, + "angularVelocity": 0.14635618159801306, + "velocityX": -0.2862680030246291, + "velocityY": 0.26916926355864296, + "timestamp": 1.4092242720528134 + }, + { + "x": 2.594845480264057, + "y": 4.36277075661854, + "heading": -0.6857017147672647, + "angularVelocity": 0.2810444507720759, + "velocityX": -0.5453268567867454, + "velocityY": 0.5128945730159264, + "timestamp": 1.49111298189887 + }, + { + "x": 2.5289756911084322, + "y": 4.424729822216665, + "heading": -0.6516589308540864, + "angularVelocity": 0.4157201154734962, + "velocityX": -0.8043818162412647, + "velocityY": 0.7566252504722968, + "timestamp": 1.5730016917449265 + }, + { + "x": 2.4418926576481637, + "y": 4.5066482714289275, + "heading": -0.6065878922273316, + "angularVelocity": 0.550393805342452, + "velocityX": -1.0634314989694753, + "velocityY": 1.000363168088272, + "timestamp": 1.654890401590983 + }, + { + "x": 2.3335969392795284, + "y": 4.608526864652899, + "heading": -0.5504868101397262, + "angularVelocity": 0.6850893388486693, + "velocityX": -1.322474350520624, + "velocityY": 1.2441103714479649, + "timestamp": 1.7367791114370397 + }, + { + "x": 2.2040892544412327, + "y": 4.730366540742225, + "heading": -0.48334990238896663, + "angularVelocity": 0.8198554828494793, + "velocityX": -1.5815084287169574, + "velocityY": 1.4878690398025962, + "timestamp": 1.8186678212830962 + }, + { + "x": 2.0533705390200705, + "y": 4.8721684079929855, + "heading": -0.4051637468918412, + "angularVelocity": 0.9547855332451622, + "velocityX": -1.840531078148622, + "velocityY": 1.7316412423316419, + "timestamp": 1.9005565311291528 + }, + { + "x": 1.8837222142414454, + "y": 5.031895792612386, + "heading": -0.31535335280985394, + "angularVelocity": 1.0967371967493782, + "velocityX": -2.071693705976669, + "velocityY": 1.9505422044097782, + "timestamp": 1.9824452409752094 + }, + { + "x": 1.7352824931464605, + "y": 5.171658722106698, + "heading": -0.2366561880408508, + "angularVelocity": 0.9610258229363577, + "velocityX": -1.8127006930996745, + "velocityY": 1.706742355045694, + "timestamp": 2.0643339508212657 + }, + { + "x": 1.6080502362813274, + "y": 5.291456457157948, + "heading": -0.16911546139257225, + "angularVelocity": 0.8247867963147685, + "velocityX": -1.5537215948855285, + "velocityY": 1.4629334773555265, + "timestamp": 2.146222660667322 + }, + { + "x": 1.5020244624691528, + "y": 5.391288489879848, + "heading": -0.11277393629567076, + "angularVelocity": 0.6880255556940454, + "velocityX": -1.294754478504957, + "velocityY": 1.219118397512621, + "timestamp": 2.2281113705133784 + }, + { + "x": 1.4172044018013137, + "y": 5.471154488105965, + "heading": -0.06766925261840594, + "angularVelocity": 0.550804668458663, + "velocityX": -1.0357967640141517, + "velocityY": 0.9752992613543129, + "timestamp": 2.3100000803594347 + }, + { + "x": 1.3535895239653573, + "y": 5.531054232218378, + "heading": -0.03382997301748837, + "angularVelocity": 0.4132349827532073, + "velocityX": -0.7768455255376048, + "velocityY": 0.7314774432888099, + "timestamp": 2.391888790205491 + }, + { + "x": 1.3111795494900627, + "y": 5.570987556283822, + "heading": -0.011272655480386088, + "angularVelocity": 0.27546309594458174, + "velocityX": -0.5178976998785506, + "velocityY": 0.48765359889678683, + "timestamp": 2.4737775000515474 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 7.540408157375041e-27, - "angularVelocity": 0.11657328273999748, - "velocityX": -0.21918370888400043, - "velocityY": 0.20638242027186274, - "timestamp": 2.7107129099329357 + "heading": 0, + "angularVelocity": 0.13765823764445279, + "velocityX": -0.2589502077253713, + "velocityY": 0.24382784263139995, + "timestamp": 2.5556662098976037 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 7.897631948719018e-27, - "angularVelocity": 4.6120160844672454e-27, - "velocityX": -1.8573844509297025e-30, - "velocityY": 4.869606321032096e-28, - "timestamp": 2.788169909487572 + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.169901203643428e-30, + "velocityY": 4.744348570353821e-29, + "timestamp": 2.63755491974366 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.2.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.2.traj index cc5e6798..d6a536f1 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.2.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.2.traj @@ -3,227 +3,227 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 7.897631948719018e-27, - "angularVelocity": 4.6120160844672454e-27, - "velocityX": -1.8573844509297025e-30, - "velocityY": 4.869606321032096e-28, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.169901203643428e-30, + "velocityY": 4.744348570353821e-29, "timestamp": 0 }, { - "x": 1.3279766485932036, - "y": 5.589659571345802, - "heading": -4.710153046711685e-19, - "angularVelocity": -4.772747206025981e-18, - "velocityX": 0.38507114894200895, - "velocityY": -0.01311934897441659, - "timestamp": 0.0986887686411162 - }, - { - "x": 1.4039810428772954, - "y": 5.587070106580806, - "heading": -1.4132708274578583e-18, - "angularVelocity": -9.547773272073244e-18, - "velocityX": 0.7701422900595405, - "velocityY": -0.026238697682253745, - "timestamp": 0.1973775372822324 - }, - { - "x": 1.5179876327590038, - "y": 5.5831859094859295, - "heading": 6.100569101954107e-18, - "angularVelocity": 7.613688998468879e-17, - "velocityX": 1.155213419439816, - "velocityY": -0.03935804599020335, - "timestamp": 0.2960663059233486 - }, - { - "x": 1.669996416307628, - "y": 5.578006980126951, - "heading": 1.3142262461800744e-17, - "angularVelocity": 7.135267989328175e-17, - "velocityX": 1.5402845292565597, - "velocityY": -0.0524773936316247, - "timestamp": 0.3947550745644648 - }, - { - "x": 1.8600073896612332, - "y": 5.571533318635448, - "heading": 1.9239838663580868e-17, - "angularVelocity": 6.178604539434395e-17, - "velocityX": 1.925355599940842, - "velocityY": -0.06559673993980564, - "timestamp": 0.493443843205581 - }, - { - "x": 2.0880205412297537, - "y": 5.563764925406294, - "heading": 2.4860743738508815e-17, - "angularVelocity": 5.695598852986624e-17, - "velocityX": 2.310426553184545, - "velocityY": -0.07871608224679369, - "timestamp": 0.5921326118466972 - }, - { - "x": 2.2780734489522008, - "y": 5.557289835213575, - "heading": 1.9389925148359925e-17, - "angularVelocity": -5.54351783373119e-17, - "velocityX": 1.9257805152317913, - "velocityY": -0.06561121677610336, - "timestamp": 0.6908213804878134 - }, - { - "x": 2.430124178434165, - "y": 5.55210947675938, - "heading": 1.4382200259206915e-17, - "angularVelocity": -5.074270981417207e-17, - "velocityX": 1.5407095617290139, - "velocityY": -0.05249187446028865, - "timestamp": 0.7895101491289296 - }, - { - "x": 2.544172718104756, - "y": 5.548223850437927, - "heading": 9.840962526531424e-18, - "angularVelocity": -4.6015845846918525e-17, - "velocityX": 1.155638490979962, - "velocityY": -0.03937252814990102, - "timestamp": 0.8881989177700458 - }, - { - "x": 2.6202190641055907, - "y": 5.545632956380672, - "heading": 6.0923440108598725e-18, - "angularVelocity": -3.798432272839268e-17, - "velocityX": 0.7705673811344316, - "velocityY": -0.026253180507498905, - "timestamp": 0.986887686411162 - }, - { - "x": 2.6582632145072105, - "y": 5.5443367946533515, - "heading": 2.8119102345957467e-18, - "angularVelocity": -3.324026254038647e-17, - "velocityX": 0.38549625173796304, - "velocityY": -0.013133832198997603, - "timestamp": 1.0855764550522782 + "x": 1.3279764392732873, + "y": 5.589659578477318, + "heading": -3.8286967101155775e-19, + "angularVelocity": -4.101142681562708e-18, + "velocityX": 0.40706315074964167, + "velocityY": -0.013868615044214471, + "timestamp": 0.0933564930640447 + }, + { + "x": 1.4039804148734387, + "y": 5.5870701279768555, + "heading": -4.0712541473530154e-19, + "angularVelocity": -2.5982712070316094e-19, + "velocityX": 0.814126292755424, + "velocityY": -0.027737229790526226, + "timestamp": 0.1867129861280894 + }, + { + "x": 1.517986376641016, + "y": 5.583185952281787, + "heading": 4.833998363566258e-19, + "angularVelocity": 9.538831821902836e-18, + "velocityX": 1.2211894216447965, + "velocityY": -0.04160584408996275, + "timestamp": 0.2800694791921341 + }, + { + "x": 1.6699943225350276, + "y": 5.578007051461648, + "heading": 1.3619244496265011e-18, + "angularVelocity": 9.410228943340493e-18, + "velocityX": 1.6282525286718303, + "velocityY": -0.05547445764455082, + "timestamp": 0.3734259722561788 + }, + { + "x": 1.8600042484729113, + "y": 5.571533425655531, + "heading": 3.711758987250246e-18, + "angularVelocity": 2.5170201581763002e-17, + "velocityX": 2.0353155919679775, + "velocityY": -0.06934306970923045, + "timestamp": 0.4667824653202235 + }, + { + "x": 2.0880161422023416, + "y": 5.563765075280871, + "heading": -9.287085834288012e-18, + "angularVelocity": -1.392382283532996e-16, + "velocityX": 2.4423785240217755, + "velocityY": -0.08321167730249175, + "timestamp": 0.5601389583842682 + }, + { + "x": 2.2780692584306133, + "y": 5.5572899779843725, + "heading": -5.547928961434018e-18, + "angularVelocity": 4.0052703333031833e-17, + "velocityX": 2.03577823010496, + "velocityY": -0.06935883176043627, + "timestamp": 0.6534954514483129 + }, + { + "x": 2.430120406839514, + "y": 5.552109605257364, + "heading": -3.2776580295073633e-18, + "angularVelocity": 2.4318713794052425e-17, + "velocityX": 1.6287152977531212, + "velocityY": -0.05549022415702071, + "timestamp": 0.7468519445123576 + }, + { + "x": 2.5441695751975866, + "y": 5.548223957516569, + "heading": -1.9211838299437728e-18, + "angularVelocity": 1.4530579976060665e-17, + "velocityX": 1.2216522343824612, + "velocityY": -0.04162161208980244, + "timestamp": 0.8402084375764023 + }, + { + "x": 2.6202167594261327, + "y": 5.545633034900951, + "heading": -1.8132356413213059e-19, + "angularVelocity": 1.8636270440618356e-17, + "velocityX": 0.8145891273223099, + "velocityY": -0.02775299853408606, + "timestamp": 0.933564930640447 + }, + { + "x": 2.6582619574855144, + "y": 5.5443368374799995, + "heading": 8.843883513334677e-20, + "angularVelocity": 2.8892814546014257e-18, + "velocityX": 0.40752599841430864, + "velocityY": -0.013884384234014852, + "timestamp": 1.0269214237044917 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": 3.967855365558804e-35, - "angularVelocity": -2.8492764136925344e-17, - "velocityX": 0.0004251106100680654, - "velocityY": -0.000014483490807365214, - "timestamp": 1.1842652236933944 - }, - { - "x": 2.6203309461495814, - "y": 5.545629144566718, - "heading": -2.3435066939433916e-18, - "angularVelocity": -2.3742025219812006e-17, - "velocityX": -0.3847168901074824, - "velocityY": 0.013107279398989128, - "timestamp": 1.2829721507270127 - }, - { - "x": 2.544340541557253, - "y": 5.548218132703827, - "heading": -4.218037035355937e-18, - "angularVelocity": -1.899082968522677e-17, - "velocityX": -0.769858883001995, - "velocityY": 0.02622904202225523, - "timestamp": 1.3816790777606305 - }, - { - "x": 2.4303339555332055, - "y": 5.552102329667273, - "heading": -5.623520937192281e-18, - "angularVelocity": -1.4238932090618574e-17, - "velocityX": -1.1550008641614116, - "velocityY": 0.039350804245707374, - "timestamp": 1.4803860047942483 - }, - { - "x": 2.27831119000814, - "y": 5.557281735391277, - "heading": -6.559843392694925e-18, - "angularVelocity": -9.485869363616628e-18, - "velocityX": -1.5401428257608962, - "velocityY": 0.052472565802753904, - "timestamp": 1.5790929318278661 - }, - { - "x": 2.0882722488439907, - "y": 5.563756349744264, - "heading": -7.026773676733008e-18, - "angularVelocity": -4.730469162706724e-18, - "velocityX": -1.9252847482351219, - "velocityY": 0.06559432602680541, - "timestamp": 1.677799858861484 - }, - { - "x": 1.8602171436308217, - "y": 5.571526172331361, - "heading": -7.023625037813593e-18, - "angularVelocity": 3.1898506726958296e-20, - "velocityX": -2.3104265532903905, - "velocityY": 0.07871608225040072, - "timestamp": 1.7765067858951018 - }, - { - "x": 1.6701362603825125, - "y": 5.578002215648425, - "heading": -4.6832619459775276e-18, - "angularVelocity": 2.3710179076002214e-17, - "velocityX": -1.9257096635619557, - "velocityY": 0.06560880286432572, - "timestamp": 1.8752137129287196 - }, - { - "x": 1.5180715412087855, - "y": 5.583183050730508, - "heading": -2.810229256311612e-18, - "angularVelocity": 1.8975666640147773e-17, - "velocityX": -1.5405678582477078, - "velocityY": 0.05248704663190702, - "timestamp": 1.9739206399623375 - }, - { - "x": 1.4040229976805338, - "y": 5.587068677183391, - "heading": -1.405203585438858e-18, - "angularVelocity": 1.4234289424631298e-17, - "velocityX": -1.1554259357087366, - "velocityY": 0.03936528640564963, - "timestamp": 2.0726275669959553 - }, - { - "x": 1.3279906336561405, - "y": 5.589659094875619, - "heading": -4.684238337733874e-19, - "angularVelocity": 9.490501893267791e-18, - "velocityX": -0.7702839740804757, - "velocityY": 0.02624352484762269, - "timestamp": 2.171334494029573 + "heading": 1.7598923552723236e-34, + "angularVelocity": -9.476131898054967e-19, + "velocityX": 0.0004628563966037854, + "velocityY": -0.000015769487296898133, + "timestamp": 1.1202779167685364 + }, + { + "x": 2.620331993851312, + "y": 5.545629108871589, + "heading": 2.945836613273263e-19, + "angularVelocity": 3.1551525697239727e-18, + "velocityX": -0.4066774370940851, + "velocityY": 0.013855473805082683, + "timestamp": 1.2136521018644193 + }, + { + "x": 2.5443424272440374, + "y": 5.548218068458602, + "heading": 2.309166242357916e-19, + "angularVelocity": -6.815922301609038e-19, + "velocityX": -0.8138177218425701, + "velocityY": 0.027726716799615945, + "timestamp": 1.3070262869603022 + }, + { + "x": 2.4303364695545335, + "y": 5.552102244014731, + "heading": -7.469284722560017e-19, + "angularVelocity": -1.047218864069531e-17, + "velocityX": -1.220957993477128, + "velocityY": 0.041597959347358555, + "timestamp": 1.4004004720561851 + }, + { + "x": 2.278314122823792, + "y": 5.55728163547044, + "heading": -1.7117540142209262e-18, + "angularVelocity": -1.0332835908519937e-17, + "velocityX": -1.628098243253484, + "velocityY": 0.055469201150393666, + "timestamp": 1.4937746571520676 + }, + { + "x": 2.0882753911343768, + "y": 5.563756242686637, + "heading": -3.8779782019319956e-18, + "angularVelocity": -2.3199468985970604e-17, + "velocityX": -2.035238449307225, + "velocityY": 0.06934044146380212, + "timestamp": 1.58714884224795 + }, + { + "x": 1.8602202867386202, + "y": 5.571526065245884, + "heading": -6.955816575429636e-18, + "angularVelocity": -3.296264638305206e-17, + "velocityX": -2.4423785241434, + "velocityY": 0.08321167730663664, + "timestamp": 1.6805230273438325 + }, + { + "x": 1.6701383566026529, + "y": 5.578002144230341, + "heading": -1.819668166700424e-18, + "angularVelocity": 5.5006116981871155e-17, + "velocityX": -2.035701087485777, + "velocityY": 0.0693562035164242, + "timestamp": 1.773897212439715 + }, + { + "x": 1.518072799200871, + "y": 5.5831830078708, + "heading": 4.234959937878501e-18, + "angularVelocity": 6.484281229150569e-17, + "velocityX": -1.6285610123513967, + "velocityY": 0.05548496766342984, + "timestamp": 1.8672713975355975 + }, + { + "x": 1.4040236267647241, + "y": 5.587068655750534, + "heading": 2.524941481520431e-18, + "angularVelocity": -1.831387557299539e-17, + "velocityX": -1.221420806223103, + "velocityY": 0.041613727347481366, + "timestamp": 1.96064558263148 + }, + { + "x": 1.327990843372909, + "y": 5.589659087730582, + "heading": 8.045224971714285e-19, + "angularVelocity": -1.842525382897137e-17, + "velocityX": -0.814280556413611, + "velocityY": 0.027742485543317336, + "timestamp": 2.0540197677273624 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 3.33815545782367e-31, - "angularVelocity": 4.745589981208042e-18, - "velocityX": -0.3851419929048723, - "velocityY": 0.013121762623619058, - "timestamp": 2.270041421063191 + "heading": 6.74699155537553e-28, + "angularVelocity": -8.616220285509517e-18, + "velocityX": -0.40714028476041414, + "velocityY": 0.013871242994939689, + "timestamp": 2.147393952823245 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 2.0232376930557863e-31, - "angularVelocity": -1.3321178776125106e-30, - "velocityX": -6.421692970708144e-31, - "velocityY": -1.2392296625905867e-30, - "timestamp": 2.368748348096809 + "heading": 0, + "angularVelocity": -7.225786160817796e-27, + "velocityX": -2.979354413794432e-29, + "velocityY": 7.541641997510845e-28, + "timestamp": 2.2407681379191273 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.3.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.3.traj index 86b4c757..0cf979fd 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.3.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.3.traj @@ -3,353 +3,335 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 2.0232376930557863e-31, - "angularVelocity": -1.3321178776125106e-30, - "velocityX": -6.421692970708144e-31, - "velocityY": -1.2392296625905867e-30, + "heading": 0, + "angularVelocity": -7.225786160817796e-27, + "velocityX": -2.979354413794432e-29, + "velocityY": 7.541641997510845e-28, "timestamp": 0 }, { - "x": 1.3044225360616175, - "y": 5.603754712691982, - "heading": 0.008101518995482268, - "angularVelocity": 0.11491508578445629, - "velocityX": 0.20493723803226965, - "velocityY": 0.18156596231474714, - "timestamp": 0.07050004740611815 - }, - { - "x": 1.3333187786755418, - "y": 5.6293552812892855, - "heading": 0.024302645459921106, - "angularVelocity": 0.22980305773571622, - "velocityX": 0.40987550614635165, - "velocityY": 0.36312838840846096, - "timestamp": 0.1410000948122363 - }, - { - "x": 1.3766634045969632, - "y": 5.667755708974662, - "heading": 0.048595399874496285, - "angularVelocity": 0.34457784509896805, - "velocityX": 0.6148169755366727, - "velocityY": 0.5446865512610124, - "timestamp": 0.21150014221835445 - }, - { - "x": 1.434456789320205, - "y": 5.7189556181427035, - "heading": 0.08096686680516672, - "angularVelocity": 0.45916943493943385, - "velocityX": 0.8197637710840284, - "velocityY": 0.7262393580121056, - "timestamp": 0.2820001896244726 - }, - { - "x": 1.5066994506601286, - "y": 5.782954524001268, - "heading": 0.12140070801121562, - "angularVelocity": 0.5735292768404597, - "velocityX": 1.0247179115180916, - "velocityY": 0.9077852882835168, - "timestamp": 0.35250023703059075 - }, - { - "x": 1.59339203880409, - "y": 5.859751802427408, - "heading": 0.16987894409259566, - "angularVelocity": 0.6876340919619466, - "velocityX": 1.2296812744616465, - "velocityY": 1.0893223657531423, - "timestamp": 0.4230002844367089 - }, - { - "x": 1.6945353339175246, - "y": 5.949346653618572, - "heading": 0.22638374394908448, - "angularVelocity": 0.8014859838460967, - "velocityX": 1.4346557035741434, - "velocityY": 1.2708480985133195, - "timestamp": 0.49350033184282704 - }, - { - "x": 1.8101302937852244, - "y": 6.051738034585355, - "heading": 0.290898605978136, - "angularVelocity": 0.9151038106032966, - "velocityX": 1.6396437182773986, - "velocityY": 1.4523590371075072, - "timestamp": 0.5640003792489452 - }, - { - "x": 1.940178425831269, - "y": 6.166924341281603, - "heading": 0.36340632675450085, - "angularVelocity": 1.0284776173082797, - "velocityX": 1.8446531148681091, - "velocityY": 1.6338472232892616, - "timestamp": 0.6345004266550633 - }, - { - "x": 2.0847170007855893, - "y": 6.294873766483704, - "heading": 0.443594985050904, - "angularVelocity": 1.1374270124170804, - "velocityX": 2.0501911739392336, - "velocityY": 1.8148842434820598, - "timestamp": 0.7050004740611815 - }, - { - "x": 2.2148049129108704, - "y": 6.410027050354294, - "heading": 0.5157685134494568, - "angularVelocity": 1.0237373030800216, - "velocityX": 1.8452173709317587, - "velocityY": 1.6333788147296693, - "timestamp": 0.7755005214672996 - }, - { - "x": 2.3304407554674365, - "y": 6.512385621001077, - "heading": 0.5799353875370489, - "angularVelocity": 0.9101678147527572, - "velocityX": 1.6402236141834323, - "velocityY": 1.4518936428105123, - "timestamp": 0.8460005688734178 - }, - { - "x": 2.431623857347168, - "y": 6.601950249920576, - "heading": 0.6360978126199414, - "angularVelocity": 0.796629607344333, - "velocityX": 1.435220338177405, - "velocityY": 1.2704194140971152, - "timestamp": 0.9165006162795359 - }, - { - "x": 2.518353756343433, - "y": 6.678721476402507, - "heading": 0.6842565298983911, - "angularVelocity": 0.683101913407659, - "velocityX": 1.2302105060533437, - "velocityY": 1.088952834878071, - "timestamp": 0.9870006636856541 - }, - { - "x": 2.5906300952596166, - "y": 6.742699702654635, - "heading": 0.7244118519902635, - "angularVelocity": 0.5695786537639655, - "velocityX": 1.0251956073140427, - "velocityY": 0.907491960729887, - "timestamp": 1.0575007110917722 - }, - { - "x": 2.648452591103255, - "y": 6.7938852314203, - "heading": 0.7565639740344317, - "angularVelocity": 0.45605816204569094, - "velocityX": 0.820176694499928, - "velocityY": 0.7260353808105814, - "timestamp": 1.1280007584978904 - }, - { - "x": 2.6918210260454423, - "y": 6.832278283879232, - "heading": 0.7807130811975228, - "angularVelocity": 0.34254029680263026, - "velocityX": 0.6151546919161919, - "velocityY": 0.5445819381903092, - "timestamp": 1.1985008059040085 - }, - { - "x": 2.720735245788176, - "y": 6.85787900840029, - "heading": 0.7968594022841035, - "angularVelocity": 0.22902567701223253, - "velocityX": 0.4101305007097693, - "velocityY": 0.3631306000914257, - "timestamp": 1.2690008533101267 + "x": 1.3078091735186845, + "y": 5.606755773559667, + "heading": 0.009999668662888749, + "angularVelocity": 0.1349541802858926, + "velocityX": 0.24069501005494717, + "velocityY": 0.21325450657673967, + "timestamp": 0.07409676856345904 + }, + { + "x": 1.3434786990122702, + "y": 5.638358364996936, + "heading": 0.029996210094567113, + "angularVelocity": 0.2698706275619814, + "velocityX": 0.48139110769232624, + "velocityY": 0.42650431390462895, + "timestamp": 0.14819353712691807 + }, + { + "x": 1.3969833303762573, + "y": 5.6857616519517675, + "heading": 0.05997803368561842, + "angularVelocity": 0.4046306495359042, + "velocityX": 0.7220912922884387, + "velocityY": 0.6397483706242104, + "timestamp": 0.2222903056903771 + }, + { + "x": 1.468323578276302, + "y": 5.748965089703653, + "heading": 0.09992693775422122, + "angularVelocity": 0.5391450240657001, + "velocityX": 0.9627983687594002, + "velocityY": 0.8529850758692316, + "timestamp": 0.29638707425383615 + }, + { + "x": 1.5575001323238937, + "y": 5.827967973754339, + "heading": 0.14982100646683655, + "angularVelocity": 0.6733636254605087, + "velocityX": 1.203514752134039, + "velocityY": 1.0662122733008166, + "timestamp": 0.3704838428172952 + }, + { + "x": 1.6645138155479717, + "y": 5.922769410023865, + "heading": 0.20963796260623796, + "angularVelocity": 0.8072815765348765, + "velocityX": 1.444242242996271, + "velocityY": 1.279427404349801, + "timestamp": 0.4445806113807542 + }, + { + "x": 1.789365525357909, + "y": 6.033368309242042, + "heading": 0.27935847839394334, + "angularVelocity": 0.9409386825166515, + "velocityX": 1.6849818451743819, + "velocityY": 1.4926278347551774, + "timestamp": 0.5186773799442133 + }, + { + "x": 1.9320561732696155, + "y": 6.159763414266322, + "heading": 0.35896870362448313, + "angularVelocity": 1.074408867993172, + "velocityX": 1.925733749048249, + "velocityY": 1.7058112989342795, + "timestamp": 0.5927741485076723 + }, + { + "x": 2.092586653477802, + "y": 6.301953347709479, + "heading": 0.44846110031344777, + "angularVelocity": 1.2077773218503376, + "velocityX": 2.1664977208531133, + "velocityY": 1.9189761740812865, + "timestamp": 0.6668709170711313 + }, + { + "x": 2.2353547101625333, + "y": 6.428313752108457, + "heading": 0.5276057235865483, + "angularVelocity": 1.068125166619866, + "velocityX": 1.9267784472209708, + "velocityY": 1.7053429837972895, + "timestamp": 0.7409676856345904 + }, + { + "x": 2.360281889838346, + "y": 6.538881119368732, + "heading": 0.5968684037316069, + "angularVelocity": 0.9347597942868423, + "velocityX": 1.686000376116786, + "velocityY": 1.4922022835269577, + "timestamp": 0.8150644541980494 + }, + { + "x": 2.4673676751793194, + "y": 6.633656126085495, + "heading": 0.6562491552260078, + "angularVelocity": 0.8013946173366472, + "velocityX": 1.4452153232519684, + "velocityY": 1.279070714493898, + "timestamp": 0.8891612227615084 + }, + { + "x": 2.5566116515465542, + "y": 6.712639287159663, + "heading": 0.7057476703680625, + "angularVelocity": 0.668025287685273, + "velocityX": 1.2044246744765545, + "velocityY": 1.0659460944941062, + "timestamp": 0.9632579913249675 + }, + { + "x": 2.6280134773537864, + "y": 6.7758309931109295, + "heading": 0.7453637127245453, + "angularVelocity": 0.5346527671626246, + "velocityX": 0.9636294156990158, + "velocityY": 0.8528267448954308, + "timestamp": 1.0373547598884265 + }, + { + "x": 2.6815728846604703, + "y": 6.823231527897903, + "heading": 0.7750971751130776, + "angularVelocity": 0.4012788001208449, + "velocityX": 0.7228305408320154, + "velocityY": 0.6397112275638166, + "timestamp": 1.1114515284518856 + }, + { + "x": 2.7172896860156874, + "y": 6.854841077120622, + "heading": 0.7949480902508361, + "angularVelocity": 0.26790527462553493, + "velocityX": 0.4820291361527109, + "velocityY": 0.42659821506532714, + "timestamp": 1.1855482970153446 + }, + { + "x": 2.7351637810076403, + "y": 6.870659730936303, + "heading": 0.8049166539682622, + "angularVelocity": 0.13453439203137166, + "velocityX": 0.2412263763293308, + "velocityY": 0.2134864193164304, + "timestamp": 1.2596450655788036 }, { "x": 2.73519515991211, "y": 6.870687484741211, "heading": 0.8050032485420815, - "angularVelocity": 0.11551547208280752, - "velocityX": 0.2051050269602972, - "velocityY": 0.18168039330721092, - "timestamp": 1.3395009007162448 - }, - { - "x": 2.733517887090967, - "y": 6.869215556772689, - "heading": 0.8042279993622282, - "angularVelocity": -0.009948237722316289, - "velocityX": -0.02152328462065399, - "velocityY": -0.018888235836320173, - "timestamp": 1.4174291932935539 - }, - { - "x": 2.714179970265771, - "y": 6.85211357425893, - "heading": 0.7936748330656245, - "angularVelocity": -0.13542150029957753, - "velocityX": -0.24815014143946865, - "velocityY": -0.21945793944853148, - "timestamp": 1.495357485870863 - }, - { - "x": 2.677181604439607, - "y": 6.819381358340049, - "heading": 0.7733432924791085, - "angularVelocity": -0.26090062946453, - "velocityX": -0.47477449591828125, - "velocityY": -0.4200299382462307, - "timestamp": 1.573285778448172 - }, - { - "x": 2.6225230574768488, - "y": 6.771018630930062, - "heading": 0.7432332740659877, - "angularVelocity": -0.3863810872444029, - "velocityX": -0.7013954130784187, - "velocityY": -0.6206055055294315, - "timestamp": 1.651214071025481 - }, - { - "x": 2.5502046603277093, - "y": 6.707025005806598, - "heading": 0.7033451105326386, - "angularVelocity": -0.5118572756329512, - "velocityX": -0.9280120833828756, - "velocityY": -0.8211860289378066, - "timestamp": 1.72914236360279 - }, - { - "x": 2.4602268003518986, - "y": 6.627399971822084, - "heading": 0.653679622983667, - "angularVelocity": -0.6373229273527391, - "velocityX": -1.1546237829674975, - "velocityY": -1.0217731115502302, - "timestamp": 1.807070656180099 - }, - { - "x": 2.3525899246520714, - "y": 6.53214286652055, - "heading": 0.5942379589890165, - "angularVelocity": -0.7627738530993845, - "velocityX": -1.3812297451922877, - "velocityY": -1.2223686950029513, - "timestamp": 1.8849989487574081 - }, - { - "x": 2.22729456327189, - "y": 6.421252842872556, - "heading": 0.5250208143201779, - "angularVelocity": -0.8882158504906453, - "velocityX": -1.6078289057325443, - "velocityY": -1.4229751477999506, - "timestamp": 1.9629272413347172 - }, - { - "x": 2.0843413856896924, - "y": 6.294728842395834, - "heading": 0.4460263730973391, - "angularVelocity": -1.0136811498143847, - "velocityX": -1.8344194753193157, - "velocityY": -1.623595183369405, - "timestamp": 2.0408555339120262 - }, - { - "x": 1.925462021024861, - "y": 6.1539791801359245, - "heading": 0.3570280981408959, - "angularVelocity": -1.1420534444297203, - "velocityX": -2.038789243421619, - "velocityY": -1.8061432838436418, - "timestamp": 2.1187838264893353 - }, - { - "x": 1.7842386496681704, - "y": 6.028865963268039, - "heading": 0.2778322483955418, - "angularVelocity": -1.016265686390949, - "velocityX": -1.8122220657739765, - "velocityY": -1.6054915709049453, - "timestamp": 2.1967121190666443 - }, - { - "x": 1.6606703031584165, - "y": 5.9193901868323495, - "heading": 0.2084629069388522, - "angularVelocity": -0.8901688868374888, - "velocityX": -1.5856673157206287, - "velocityY": -1.4048270893024366, - "timestamp": 2.2746404116439534 - }, - { - "x": 1.554756060685191, - "y": 5.825552639839227, - "heading": 0.14894782916930657, - "angularVelocity": -0.7637159214094572, - "velocityX": -1.3591243817917928, - "velocityY": -1.2041524828743098, - "timestamp": 2.3525687042212624 - }, - { - "x": 1.4664951152377614, - "y": 5.747353916497971, - "heading": 0.09931505664018385, - "angularVelocity": -0.6369031180797928, - "velocityX": -1.1325918036747624, - "velocityY": -1.0034702513683131, - "timestamp": 2.4304969967985715 - }, - { - "x": 1.3958868227418013, - "y": 5.684794447832575, - "heading": 0.05958948406530847, - "angularVelocity": -0.5097708580675663, - "velocityX": -0.9060674905190907, - "velocityY": -0.8027824888288644, - "timestamp": 2.5084252893758805 - }, - { - "x": 1.3429307344840264, - "y": 5.637874543820294, - "heading": 0.029789872263071265, - "angularVelocity": -0.38239785341985083, - "velocityX": -0.6795489353913313, - "velocityY": -0.6020907485652136, - "timestamp": 2.5863535819531895 - }, - { - "x": 1.3076266152194531, - "y": 5.606594434817311, - "heading": 0.009926565476430555, - "angularVelocity": -0.25489210824085684, - "velocityX": -0.4530333990001086, - "velocityY": -0.4013960522996289, - "timestamp": 2.6642818745304986 + "angularVelocity": 0.0011686685922978992, + "velocityX": 0.00042348526922062564, + "velocityY": 0.00037456143614395893, + "timestamp": 1.3337418341422627 + }, + { + "x": 2.71737666825008, + "y": 6.854917835875481, + "heading": 0.7952044521096777, + "angularVelocity": -0.13221640762197384, + "velocityX": -0.2404271762292292, + "velocityY": -0.21278187956754013, + "timestamp": 1.4078536376738002 + }, + { + "x": 2.6817084896089662, + "y": 6.823350601971179, + "heading": 0.775519887263024, + "angularVelocity": -0.2656063393700981, + "velocityX": -0.4812752750383339, + "velocityY": -0.4259407056452727, + "timestamp": 1.4819654412053378 + }, + { + "x": 2.628190888379515, + "y": 6.7759855045171085, + "heading": 0.7459495915696174, + "angularVelocity": -0.398995764215558, + "velocityX": -0.7221198063894422, + "velocityY": -0.6391032897100266, + "timestamp": 1.5560772447368754 + }, + { + "x": 2.5568241932220688, + "y": 6.712822154407591, + "heading": 0.7064940900377142, + "angularVelocity": -0.532378105058251, + "velocityX": -0.9629599030846991, + "velocityY": -0.8522711240272193, + "timestamp": 1.630189048268413 + }, + { + "x": 2.467608789693866, + "y": 6.633860035088962, + "heading": 0.657154460511053, + "angularVelocity": -0.6657459024188197, + "velocityX": -1.2037947974070313, + "velocityY": -1.0654459282687603, + "timestamp": 1.7043008517999505 + }, + { + "x": 2.3605451226537615, + "y": 6.539098475645005, + "heading": 0.597932194997477, + "angularVelocity": -0.7990935679285951, + "velocityX": -1.4446236891921902, + "velocityY": -1.2786297852682258, + "timestamp": 1.778412655331488 + }, + { + "x": 2.2356337183339816, + "y": 6.428536615850716, + "heading": 0.528828452368811, + "angularVelocity": -0.9324255966349123, + "velocityX": -1.685445480458886, + "velocityY": -1.4918252493931579, + "timestamp": 1.8525244588630256 + }, + { + "x": 2.092875237629583, + "y": 6.30217337339218, + "heading": 0.4498420161681241, + "angularVelocity": -1.0657740391526649, + "velocityX": -1.9262583541843843, + "velocityY": -1.7050353173174484, + "timestamp": 1.9266362623945632 + }, + { + "x": 1.9322705754719112, + "y": 6.16000743597858, + "heading": 0.3609649957464903, + "angularVelocity": -1.1992289512105054, + "velocityX": -2.167059152487097, + "velocityY": -1.9182630921487032, + "timestamp": 2.0007480659261008 + }, + { + "x": 1.789533871896463, + "y": 6.033554554877999, + "heading": 0.28090803782331497, + "angularVelocity": -1.0802187250553423, + "velocityX": -1.9259645128083558, + "velocityY": -1.7062448231010843, + "timestamp": 2.0748598694576383 + }, + { + "x": 1.6646414904257845, + "y": 5.922906567233019, + "heading": 0.2107799682947157, + "angularVelocity": -0.9462469699444712, + "velocityX": -1.6851888029309197, + "velocityY": -1.4929873836216447, + "timestamp": 2.148971672989176 + }, + { + "x": 1.5575924667954766, + "y": 5.828064283268442, + "heading": 0.15061002754742392, + "angularVelocity": -0.8118806705635195, + "velocityX": -1.4444261039959587, + "velocityY": -1.2797190116759578, + "timestamp": 2.2230834765207135 + }, + { + "x": 1.4683859542310973, + "y": 5.7490283016453585, + "heading": 0.10042779042808991, + "angularVelocity": -0.6771153140084615, + "velocityX": -1.203674830696421, + "velocityY": -1.066442561920003, + "timestamp": 2.297195280052251 + }, + { + "x": 1.3970212709065528, + "y": 5.685799049936121, + "heading": 0.06025974538395147, + "angularVelocity": -0.5419925454610606, + "velocityX": -0.9629327573631904, + "velocityY": -0.8531603428758896, + "timestamp": 2.3713070835837886 + }, + { + "x": 1.343497933013845, + "y": 5.638376829349959, + "heading": 0.03012622810608221, + "angularVelocity": -0.40659538484416025, + "velocityX": -0.7221972121128929, + "velocityY": -0.639874059577299, + "timestamp": 2.445418887115326 + }, + { + "x": 1.3078156736079332, + "y": 5.606761857880495, + "heading": 0.010039060501896319, + "angularVelocity": -0.2710387097248632, + "velocityX": -0.4814652687860333, + "velocityY": -0.4265848348630388, + "timestamp": 2.5195306906468637 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -0.12738076439417462, - "velocityX": -0.2265180407600671, - "velocityY": -0.20069900877579375, - "timestamp": 2.7422101671078076 + "heading": -5.584258539451151e-28, + "angularVelocity": -0.1354583213971952, + "velocityX": -0.24073388709378457, + "velocityY": -0.21329334041956155, + "timestamp": 2.5936424941784013 }, { "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": 0, - "velocityX": 1.105033972502462e-31, - "velocityY": 4.1576254509102287e-32, - "timestamp": 2.8201384596851167 + "angularVelocity": 7.53493083126559e-27, + "velocityX": -3.871179527516382e-30, + "velocityY": -3.125557960458497e-29, + "timestamp": 2.667754297709939 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.4.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.4.traj index 5023df79..5eac92d4 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.4.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.4.traj @@ -4,1054 +4,982 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": 0, - "velocityX": 1.105033972502462e-31, - "velocityY": 4.1576254509102287e-32, + "angularVelocity": 7.53493083126559e-27, + "velocityX": -3.871179527516382e-30, + "velocityY": -3.125557960458497e-29, "timestamp": 0 }, { - "x": 1.3079814373562102, - "y": 5.591183871815198, - "heading": -3.6007722166496235e-18, - "angularVelocity": -5.30176004915721e-17, - "velocityX": 0.26513400990373515, - "velocityY": 0.0033801494016308404, - "timestamp": 0.0679165464199949 - }, - { - "x": 1.343996733723379, - "y": 5.591342598530086, - "heading": -8.55205935993227e-18, - "angularVelocity": -7.290251850967679e-17, - "velocityX": 0.5302875111530345, - "velocityY": 0.0023370846024297455, - "timestamp": 0.13583309283999068 - }, - { - "x": 1.398015752012773, - "y": 5.591088723884006, - "heading": -1.2570686122392014e-17, - "angularVelocity": -5.917007079271892e-17, - "velocityX": 0.7953734566439974, - "velocityY": -0.003738038216918613, - "timestamp": 0.20374963925998646 - }, - { - "x": 1.4700252438968529, - "y": 5.590030427465675, - "heading": -1.5707006622298704e-17, - "angularVelocity": -4.617903467384599e-17, - "velocityX": 1.0602643343902365, - "velocityY": -0.015582306140636595, - "timestamp": 0.27166618567998224 - }, - { - "x": 1.5599992284903603, - "y": 5.587714662715152, - "heading": -1.765002346502037e-17, - "angularVelocity": -2.8608887247798064e-17, - "velocityX": 1.3247726708173502, - "velocityY": -0.03409721007015733, - "timestamp": 0.339582732099978 - }, - { - "x": 1.6678928587399022, - "y": 5.583612697159567, - "heading": -1.9324789020346336e-17, - "angularVelocity": -2.4659167675098536e-17, - "velocityX": 1.5886206813628097, - "velocityY": -0.060397145788562055, - "timestamp": 0.4074992785199738 - }, - { - "x": 1.7936330139942434, - "y": 5.577101220041518, - "heading": -2.0699583000572945e-17, - "angularVelocity": -2.024240007004298e-17, - "velocityX": 1.8513920669164408, - "velocityY": -0.09587467946002654, - "timestamp": 0.4754158249399696 - }, - { - "x": 1.9371035847765512, - "y": 5.567437513465235, - "heading": -2.4072402865280595e-17, - "angularVelocity": -4.9661238919294396e-17, - "velocityX": 2.112453862054281, - "velocityY": -0.1422879561119276, - "timestamp": 0.5433323713599654 - }, - { - "x": 2.098121982759825, - "y": 5.553726841653449, - "heading": -2.991372900129062e-17, - "angularVelocity": -8.600740883406488e-17, - "velocityX": 2.370827235347597, - "velocityY": -0.20187527980293163, - "timestamp": 0.6112489177799612 - }, - { - "x": 2.276400921296343, - "y": 5.534880229318232, - "heading": -3.3606634340190096e-17, - "angularVelocity": -5.437416182505347e-17, - "velocityX": 2.624970613700564, - "velocityY": -0.2774966238517093, - "timestamp": 0.6791654641999569 - }, - { - "x": 2.471485361845495, - "y": 5.509562201282429, - "heading": -3.628143679663613e-17, - "angularVelocity": -3.938366258883085e-17, - "velocityX": 2.8724140262190536, - "velocityY": -0.37278144090596593, - "timestamp": 0.7470820106199527 - }, - { - "x": 2.682648461637503, - "y": 5.476133636589416, - "heading": -3.7916749117831765e-17, - "angularVelocity": -2.4078260416782975e-17, - "velocityX": 3.1091554403573096, - "velocityY": -0.4922005969839973, - "timestamp": 0.8149985570399485 - }, - { - "x": 2.9087251812985304, - "y": 5.4326111135896715, - "heading": -3.8585043209768917e-17, - "angularVelocity": -9.83992917098953e-18, - "velocityX": 3.328742870153774, - "velocityY": -0.6408235591162347, - "timestamp": 0.8829151034599443 - }, - { - "x": 3.1478726960604213, - "y": 5.376700760732656, - "heading": -3.7931961364229683e-17, - "angularVelocity": 9.615948273285425e-18, - "velocityX": 3.5211966356917612, - "velocityY": -0.8232213768831518, - "timestamp": 0.9508316498799401 - }, - { - "x": 3.397314191935591, - "y": 5.306014145470732, - "heading": -3.37531662670372e-17, - "angularVelocity": 6.152838186169992e-17, - "velocityX": 3.6727647241163455, - "velocityY": -1.0407863619094957, - "timestamp": 1.0187481962999358 - }, - { - "x": 3.6532783223184353, - "y": 5.218541845963183, - "heading": -2.785799913755649e-17, - "angularVelocity": 8.680016148087689e-17, - "velocityX": 3.7688036844506647, - "velocityY": -1.287937978568902, - "timestamp": 1.0866647427199316 - }, - { - "x": 3.911405346241733, - "y": 5.11319146350464, - "heading": -1.864507789949357e-17, - "angularVelocity": 1.3565061404938756e-16, - "velocityX": 3.8006500260929266, - "velocityY": -1.5511740218217003, - "timestamp": 1.1545812891399274 + "x": 1.310726436274015, + "y": 5.58769762128994, + "heading": -1.5579970000423894e-18, + "angularVelocity": -2.2453280466561467e-17, + "velocityX": 0.299069985980355, + "velocityY": -0.04693411089639644, + "timestamp": 0.06938839128549112 + }, + { + "x": 1.3522304064004054, + "y": 5.581184256432646, + "heading": -2.2539666819865276e-18, + "angularVelocity": -1.003005957705595e-17, + "velocityX": 0.598139967759544, + "velocityY": -0.09386822113363098, + "timestamp": 0.13877678257098225 + }, + { + "x": 1.4144863610944205, + "y": 5.571414209224462, + "heading": -1.1138395195686094e-18, + "angularVelocity": 1.6431093577313864e-17, + "velocityX": 0.8972099444973367, + "velocityY": -0.1408023305798717, + "timestamp": 0.20816517385647337 + }, + { + "x": 1.49749429992851, + "y": 5.558387479732467, + "heading": 1.8047013110476196e-18, + "angularVelocity": 4.206093786115289e-17, + "velocityX": 1.1962799150734271, + "velocityY": -0.18773643905934295, + "timestamp": 0.2775535651419645 + }, + { + "x": 1.6012542223682356, + "y": 5.5421040680405165, + "heading": 7.934655780307681e-18, + "angularVelocity": 8.834265128972616e-17, + "velocityX": 1.4953498779473966, + "velocityY": -0.23467054633035336, + "timestamp": 0.3469419564274556 + }, + { + "x": 1.725766127726464, + "y": 5.522563974256421, + "heading": 1.6517348693937324e-17, + "angularVelocity": 1.236906166709818e-16, + "velocityX": 1.7944198309186505, + "velocityY": -0.28160465204763024, + "timestamp": 0.41633034771294675 + }, + { + "x": 1.8710300150870187, + "y": 5.499767198523928, + "heading": 2.797873673898563e-17, + "angularVelocity": 1.6517731335259598e-16, + "velocityX": 2.0934897706863036, + "velocityY": -0.3285387556932655, + "timestamp": 0.48571873899843876 + }, + { + "x": 2.0370458831672553, + "y": 5.4737137410442855, + "heading": 4.626535556608686e-17, + "angularVelocity": 2.635400319271784e-16, + "velocityX": 2.3925596919689536, + "velocityY": -0.3754728564386086, + "timestamp": 0.5551071302839299 + }, + { + "x": 2.223813730043212, + "y": 5.444403602119362, + "heading": 6.626560991799023e-17, + "angularVelocity": 2.882363175884601e-16, + "velocityX": 2.691629585524184, + "velocityY": -0.42240695283352697, + "timestamp": 0.624495521569421 + }, + { + "x": 2.4313335525083035, + "y": 5.411836782252268, + "heading": 9.137945639838438e-17, + "angularVelocity": 3.6193152862799336e-16, + "velocityX": 2.99069943286728, + "velocityY": -0.46934104197777365, + "timestamp": 0.6938839128549121 + }, + { + "x": 2.65960534414942, + "y": 5.376013282449221, + "heading": 1.1821641145167503e-16, + "angularVelocity": 3.8676433675745036e-16, + "velocityX": 3.2897691877869866, + "velocityY": -0.5162751166208155, + "timestamp": 0.7632723041404033 + }, + { + "x": 2.9086290857277297, + "y": 5.336933105728788, + "heading": 1.4538386371763974e-16, + "angularVelocity": 3.915273420502118e-16, + "velocityX": 3.5888386654437228, + "velocityY": -0.5632091477613738, + "timestamp": 0.8326606954258944 + }, + { + "x": 3.1678798362557536, + "y": 5.296247694105415, + "heading": 1.2970532145326214e-16, + "angularVelocity": -2.259533930819528e-16, + "velocityX": 3.73622656074219, + "velocityY": -0.5863432033749367, + "timestamp": 0.9020490867113855 + }, + { + "x": 3.426192624956805, + "y": 5.24997937308727, + "heading": 1.1963113847649017e-16, + "angularVelocity": -1.451854235288838e-16, + "velocityX": 3.7227089995248686, + "velocityY": -0.6668020422577368, + "timestamp": 0.9714374779968766 + }, + { + "x": 3.6799776944966163, + "y": 5.18320068927956, + "heading": 1.442058714433213e-16, + "angularVelocity": 3.5416202208408226e-16, + "velocityX": 3.657457174581828, + "velocityY": -0.9623898547086362, + "timestamp": 1.0408258692823678 + }, + { + "x": 3.9276087380924305, + "y": 5.096338039673236, + "heading": 1.2678863352315073e-16, + "angularVelocity": -2.510108333324681e-16, + "velocityX": 3.568767613835609, + "velocityY": -1.251832590395956, + "timestamp": 1.1102142605678589 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -3.9291869622836704e-18, - "angularVelocity": 2.1667607761478547e-16, - "velocityX": 3.770711743405716, - "velocityY": -1.8146339019446507, - "timestamp": 1.2224978355599232 - }, - { - "x": 4.28210524468114, - "y": 4.930751598714327, - "heading": 3.591781681737572e-18, - "angularVelocity": 2.4504750021143145e-16, - "velocityX": 3.734087818922699, - "velocityY": -1.9287249614518895, - "timestamp": 1.2531897166402777 - }, - { - "x": 4.395415253823689, - "y": 4.868113726791297, - "heading": 1.252716982122126e-17, - "angularVelocity": 2.9113198354077836e-16, - "velocityX": 3.691856124618014, - "velocityY": -2.0408612870301757, - "timestamp": 1.2838815977206322 - }, - { - "x": 4.5071149827963914, - "y": 4.802169472812339, - "heading": 2.3134869211764074e-17, - "angularVelocity": 3.4561906984236055e-16, - "velocityX": 3.639390126667849, - "velocityY": -2.1485895180653474, - "timestamp": 1.3145734788009866 - }, - { - "x": 4.616896016370763, - "y": 4.73308774688056, - "heading": 3.142037619794973e-17, - "angularVelocity": 2.699576125240905e-16, - "velocityX": 3.576875372576209, - "velocityY": -2.2508143359119486, - "timestamp": 1.345265359881341 - }, - { - "x": 4.7230901119791495, - "y": 4.663194322009908, - "heading": 3.6606203768648707e-17, - "angularVelocity": 1.6896414210849805e-16, - "velocityX": 3.460006095108622, - "velocityY": -2.277261034853729, - "timestamp": 1.3759572409616956 - }, - { - "x": 4.827842225607654, - "y": 4.596684057361499, - "heading": 4.0451980836943306e-17, - "angularVelocity": 1.2530273569920946e-16, - "velocityX": 3.413023573050007, - "velocityY": -2.1670312247815544, - "timestamp": 1.40664912204205 - }, - { - "x": 4.932161852632399, - "y": 4.533825930261183, - "heading": 4.329689113733961e-17, - "angularVelocity": 9.26925861582003e-17, - "velocityX": 3.3989323349592793, - "velocityY": -2.0480376206250996, - "timestamp": 1.4373410031224045 - }, - { - "x": 5.036392029451142, - "y": 4.474644374374991, - "heading": 4.527403793938572e-17, - "angularVelocity": 6.441920024435287e-17, - "velocityX": 3.396017876709991, - "velocityY": -1.9282479210462729, - "timestamp": 1.468032884202759 - }, - { - "x": 5.1407004011750175, - "y": 4.419139647910205, - "heading": 4.6239659419552744e-17, - "angularVelocity": 3.1461773577369364e-17, - "velocityX": 3.3985656157987063, - "velocityY": -1.8084498085819396, - "timestamp": 1.4987247652831135 - }, - { - "x": 5.245185751752097, - "y": 4.367308322392446, - "heading": 4.6966543072994365e-17, - "angularVelocity": 2.3683241137198615e-17, - "velocityX": 3.4043319242482535, - "velocityY": -1.68876340234931, - "timestamp": 1.529416646363468 - }, - { - "x": 5.349913071755195, - "y": 4.319146690519462, - "heading": 4.64672982849528e-17, - "angularVelocity": -1.6266355263468505e-17, - "velocityX": 3.4122157494652257, - "velocityY": -1.5691977870920355, - "timestamp": 1.5601085274438224 - }, - { - "x": 5.454928326528494, - "y": 4.274651432535085, - "heading": 4.6021079264842955e-17, - "angularVelocity": -1.4538674934985797e-17, - "velocityX": 3.421597213229158, - "velocityY": -1.449740335819904, - "timestamp": 1.590800408524177 - }, - { - "x": 5.560265728804007, - "y": 4.233819701178179, - "heading": 4.5613400126539626e-17, - "angularVelocity": -1.32829717492163e-17, - "velocityX": 3.432093393029018, - "velocityY": -1.3303756537438909, - "timestamp": 1.6214922896045314 + "heading": 9.872947837544061e-17, + "angularVelocity": -4.0437823428221097e-16, + "velocityX": 3.457211254829872, + "velocityY": -1.5332571030157462, + "timestamp": 1.17960265185335 + }, + { + "x": 4.284983330328794, + "y": 4.932123965200671, + "heading": 8.520616401533837e-17, + "angularVelocity": -3.905855769726077e-16, + "velocityX": 3.3932258158637794, + "velocityY": -1.6700901615188843, + "timestamp": 1.2142258308885827 + }, + { + "x": 4.400064822274762, + "y": 4.86965478591225, + "heading": 8.38275920998652e-17, + "angularVelocity": -3.9816445337684444e-17, + "velocityX": 3.3238280005674867, + "velocityY": -1.8042589106232223, + "timestamp": 1.2488490099238154 + }, + { + "x": 4.512559967686789, + "y": 4.802639932227702, + "heading": 8.775922770148411e-17, + "angularVelocity": 1.1355501462237473e-16, + "velocityX": 3.2491281432462653, + "velocityY": -1.935548830347224, + "timestamp": 1.2834721889590481 + }, + { + "x": 4.622289335106214, + "y": 4.731186349760053, + "heading": 1.0501758472421099e-16, + "angularVelocity": 4.984625185678289e-16, + "velocityX": 3.169245877386488, + "velocityY": -2.06374990566114, + "timestamp": 1.3180953679942808 + }, + { + "x": 4.729077970033436, + "y": 4.65540814642046, + "heading": 1.123300173637084e-16, + "angularVelocity": 2.11200497564256e-16, + "velocityX": 3.0843105082451987, + "velocityY": -2.188655272309947, + "timestamp": 1.3527185470295136 + }, + { + "x": 4.83750746703025, + "y": 4.5819970246859665, + "heading": 1.0947895650457139e-16, + "angularVelocity": -8.234543847737888e-17, + "velocityX": 3.131702518895681, + "velocityY": -2.120288309163928, + "timestamp": 1.3873417260647463 + }, + { + "x": 4.94878188699609, + "y": 4.512974188419722, + "heading": 1.0086901564782932e-16, + "angularVelocity": -2.486756299293195e-16, + "velocityX": 3.213870680465465, + "velocityY": -1.9935441571094976, + "timestamp": 1.421964905099979 + }, + { + "x": 5.062723794471585, + "y": 4.448449851931572, + "heading": 9.759628790443595e-17, + "angularVelocity": -9.452418393074315e-17, + "velocityX": 3.2909140827174443, + "velocityY": -1.8636167528836842, + "timestamp": 1.4565880841352117 + }, + { + "x": 5.179151445124142, + "y": 4.388526983063662, + "heading": 9.598506413460085e-17, + "angularVelocity": -4.6535985854895585e-17, + "velocityX": 3.362708275114807, + "velocityY": -1.7307153917591276, + "timestamp": 1.4912112631704444 + }, + { + "x": 5.297879115709132, + "y": 4.333301191879142, + "heading": 9.429439507749846e-17, + "angularVelocity": -4.883055525842942e-17, + "velocityX": 3.429138337186562, + "velocityY": -1.5950525839444993, + "timestamp": 1.525834442205677 + }, + { + "x": 5.418717408418424, + "y": 4.28286058857882, + "heading": 9.044194331783773e-17, + "angularVelocity": -1.112679963830143e-16, + "velocityX": 3.4900981387736523, + "velocityY": -1.4568449433541117, + "timestamp": 1.5604576212409098 + }, + { + "x": 5.54147355571485, + "y": 4.237285646363086, + "heading": 8.606851716017984e-17, + "angularVelocity": -1.2631497971943828e-16, + "velocityX": 3.545490354063358, + "velocityY": -1.3163130447772213, + "timestamp": 1.5950808002761425 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 4.5318461812156785e-17, - "angularVelocity": -9.609659048938486e-18, - "velocityX": 3.443451372045305, - "velocityY": -1.2110898816015587, - "timestamp": 1.6521841706848859 - }, - { - "x": 5.79328341664379, - "y": 4.157319683069999, - "heading": 4.51013680218967e-17, - "angularVelocity": -5.8955361022936136e-18, - "velocityX": 3.457901563090961, - "velocityY": -1.0680543595600307, - "timestamp": 1.6890075693559483 - }, - { - "x": 5.920925042987332, - "y": 4.123275074482791, - "heading": 4.436771325922853e-17, - "angularVelocity": -1.9923600143581754e-17, - "velocityX": 3.4663184537566436, - "velocityY": -0.9245373815525213, - "timestamp": 1.7258309680270107 - }, - { - "x": 6.048609186312294, - "y": 4.094524158555703, - "heading": 4.179578855252982e-17, - "angularVelocity": -6.984484950544979e-17, - "velocityX": 3.4674730723674467, - "velocityY": -0.780778444268955, - "timestamp": 1.7626543666980732 - }, - { - "x": 6.176010603965038, - "y": 4.0710595506921905, - "heading": 3.752283423078513e-17, - "angularVelocity": -1.1603911512742033e-16, - "velocityX": 3.4597951913890252, - "velocityY": -0.6372200478591985, - "timestamp": 1.7994777653691356 - }, - { - "x": 6.3027303874933365, - "y": 4.052844738846322, - "heading": 3.3662688725727524e-17, - "angularVelocity": -1.0482861149720267e-16, - "velocityX": 3.4412842948111595, - "velocityY": -0.4946531961533159, - "timestamp": 1.836301164040198 - }, - { - "x": 6.428277540039444, - "y": 4.03979228265205, - "heading": 2.875531522756203e-17, - "angularVelocity": -1.3326781276580096e-16, - "velocityX": 3.409439570410085, - "velocityY": -0.35446093150903635, - "timestamp": 1.8731245627112605 - }, - { - "x": 6.552051846294861, - "y": 4.031728011576002, - "heading": 2.3979849407990237e-17, - "angularVelocity": -1.2968564213358396e-16, - "velocityX": 3.36129501138861, - "velocityY": -0.21899855437257756, - "timestamp": 1.909947961382323 - }, - { - "x": 6.673339149172373, - "y": 4.028337045978646, - "heading": 1.9327950159963868e-17, - "angularVelocity": -1.2632998799939155e-16, - "velocityX": 3.2937563412044475, - "velocityY": -0.09208725212048961, - "timestamp": 1.9467713600533854 - }, - { - "x": 6.791341292583958, - "y": 4.029097308231655, - "heading": 1.4785687788943395e-17, - "angularVelocity": -1.2335261767272074e-16, - "velocityX": 3.2045424287333875, - "velocityY": 0.020646172826134297, - "timestamp": 1.9835947587244478 - }, - { - "x": 6.905265249942477, - "y": 4.033232975759379, - "heading": 1.0332836121843877e-17, - "angularVelocity": -1.2092452080116043e-16, - "velocityX": 3.0937925740147887, - "velocityY": 0.11231085877402444, - "timestamp": 2.0204181573955102 - }, - { - "x": 7.014459322141266, - "y": 4.039746278237479, - "heading": 6.270033366373878e-18, - "angularVelocity": -1.103320979751085e-16, - "velocityX": 2.9653447573973857, - "velocityY": 0.17687944929482444, - "timestamp": 2.0572415560665727 - }, - { - "x": 7.1185188980352505, - "y": 4.047548732117647, - "heading": 2.7758268202913238e-18, - "angularVelocity": -9.489093222865076e-17, - "velocityX": 2.8259090591700105, - "velocityY": 0.21188847748320577, - "timestamp": 2.094064954737635 + "heading": 8.12244185045661e-17, + "angularVelocity": -1.3990912419348855e-16, + "velocityX": 3.5952265671295462, + "velocityY": -1.1736811275270398, + "timestamp": 1.6297039793113752 + }, + { + "x": 5.800884886979515, + "y": 4.158897293130728, + "heading": 7.604329686230843e-17, + "angularVelocity": -1.3984796334066921e-16, + "velocityX": 3.642093094613744, + "velocityY": -1.0189897302407367, + "timestamp": 1.6667522245520763 + }, + { + "x": 5.9373078989205395, + "y": 4.126945510909642, + "heading": 7.099587364491845e-17, + "angularVelocity": -1.3623919795923125e-16, + "velocityX": 3.682306977150727, + "velocityY": -0.8624371279529385, + "timestamp": 1.7038004697927773 + }, + { + "x": 6.074971576496844, + "y": 4.100852086793861, + "heading": 6.753360793468667e-17, + "angularVelocity": -9.345289332160286e-17, + "velocityX": 3.7157948151635507, + "velocityY": -0.7043093119863827, + "timestamp": 1.7408487150334784 + }, + { + "x": 6.213624465766887, + "y": 4.080664678033529, + "heading": 6.709732769415932e-17, + "angularVelocity": -1.177600282260163e-17, + "velocityX": 3.742495450708113, + "velocityY": -0.5448951395450716, + "timestamp": 1.7778969602741794 + }, + { + "x": 6.353013304056433, + "y": 4.066420153436822, + "heading": 4.721163336657454e-17, + "angularVelocity": -5.367513143575007e-16, + "velocityX": 3.7623600627760427, + "velocityY": -0.38448581043883095, + "timestamp": 1.8149452055148805 + }, + { + "x": 6.4925719820191485, + "y": 4.0581614935185435, + "heading": 3.328638124108273e-17, + "angularVelocity": -3.758680611067538e-16, + "velocityX": 3.766944346648758, + "velocityY": -0.22291635851097177, + "timestamp": 1.8519934507555815 + }, + { + "x": 6.627754792512696, + "y": 4.053990782696842, + "heading": 2.356760119630702e-17, + "angularVelocity": -2.6232767457636995e-16, + "velocityX": 3.6488316684169346, + "velocityY": -0.1125751245330268, + "timestamp": 1.8890416959962826 + }, + { + "x": 6.757419046645761, + "y": 4.0521449831028535, + "heading": 1.6377013350687517e-17, + "angularVelocity": -1.940871361679193e-16, + "velocityX": 3.4998757239551144, + "velocityY": -0.049821511977060404, + "timestamp": 1.9260899412369836 + }, + { + "x": 6.881278674219902, + "y": 4.051770935128315, + "heading": 1.0749401519445311e-17, + "angularVelocity": -1.51899550746631e-16, + "velocityX": 3.3431982208450166, + "velocityY": -0.010096239973268065, + "timestamp": 1.9631381864776847 + }, + { + "x": 6.999234961376727, + "y": 4.052402049117112, + "heading": 6.6780180524147395e-18, + "angularVelocity": -1.0989409673710028e-16, + "velocityX": 3.183856249883563, + "velocityY": 0.017034922563732322, + "timestamp": 2.0001864317183857 + }, + { + "x": 7.111245945109509, + "y": 4.053749520879657, + "heading": 3.4043054479450288e-18, + "angularVelocity": -8.836349922350792e-17, + "velocityX": 3.0233816205072856, + "velocityY": 0.03637073102354394, + "timestamp": 2.0372346769590868 }, { "x": 7.217291355133057, "y": 4.0556182861328125, "heading": 0, - "angularVelocity": -7.538214933530801e-17, - "velocityX": 2.6823286459819884, - "velocityY": 0.21914202127972096, - "timestamp": 2.1308883534086975 - }, - { - "x": 7.3677982829654445, - "y": 4.067452321816921, - "heading": -1.899704607683099e-18, - "angularVelocity": -3.08402414017173e-17, - "velocityX": 2.4433640695715435, - "velocityY": 0.19211645606626163, - "timestamp": 2.1924865940093214 - }, - { - "x": 7.503615163186003, - "y": 4.077376831566324, - "heading": -3.815984956526995e-18, - "angularVelocity": -3.1109335739679044e-17, - "velocityX": 2.204882459243212, - "velocityY": 0.16111677302195881, - "timestamp": 2.2540848346099454 - }, - { - "x": 7.624766058972102, - "y": 4.085215025650736, - "heading": -3.719599241310952e-18, - "angularVelocity": 1.5647479451753584e-18, - "velocityX": 1.9667914960686657, - "velocityY": 0.12724704485038288, - "timestamp": 2.3156830752105693 - }, - { - "x": 7.731270598361674, - "y": 4.090833292880733, - "heading": -3.1356688291160894e-18, - "angularVelocity": 9.479660768691952e-18, - "velocityX": 1.7290191789746205, - "velocityY": 0.09120824191105437, - "timestamp": 2.377281315811193 - }, - { - "x": 7.8231450024295786, - "y": 4.094127127737666, - "heading": -2.8973098167797552e-18, - "angularVelocity": 3.86957486173007e-18, - "velocityX": 1.491510198539231, - "velocityY": 0.05347287235504842, - "timestamp": 2.438879556411817 - }, - { - "x": 7.900402858238064, - "y": 4.09501256603324, - "heading": -2.6735415540223583e-18, - "angularVelocity": 3.632705288594772e-18, - "velocityX": 1.254221793596197, - "velocityY": 0.014374408862020004, - "timestamp": 2.500477797012441 - }, - { - "x": 7.963055690401912, - "y": 4.09342067695553, - "heading": -2.4640875687476226e-18, - "angularVelocity": 3.4003240257243437e-18, - "velocityX": 1.017120481899185, - "velocityY": -0.025843093279754936, - "timestamp": 2.562076037613065 - }, - { - "x": 8.011113385697886, - "y": 4.089293860980006, - "heading": -2.2687163840908643e-18, - "angularVelocity": 3.171700683847968e-18, - "velocityX": 0.7801796744092016, - "velocityY": -0.06699567934545418, - "timestamp": 2.623674278213689 - }, - { - "x": 8.044584512204576, - "y": 4.082583270186536, - "heading": -1.2051953127305475e-18, - "angularVelocity": 1.7265445662875882e-17, - "velocityX": 0.5433779630769335, - "velocityY": -0.108941273777265, - "timestamp": 2.6852725188143127 + "angularVelocity": -9.188843930234661e-17, + "velocityX": 2.862359859004765, + "velocityY": 0.0504413971839862, + "timestamp": 2.074282922199788 + }, + { + "x": 7.386141990779784, + "y": 4.060115199818634, + "heading": -6.285690223777642e-18, + "angularVelocity": -9.593548047553597e-17, + "velocityX": 2.5770864103628295, + "velocityY": 0.06863424057552284, + "timestamp": 2.1398028966048974 + }, + { + "x": 7.536265872308939, + "y": 4.064907354049287, + "heading": -1.0202276969724947e-17, + "angularVelocity": -5.97769883515376e-17, + "velocityX": 2.2912689281735665, + "velocityY": 0.07314035565737474, + "timestamp": 2.205322871010007 + }, + { + "x": 7.667662893519374, + "y": 4.069410994150776, + "heading": -1.2426686282698196e-17, + "angularVelocity": -3.395009435762481e-17, + "velocityX": 2.005449825087065, + "velocityY": 0.06873690263742, + "timestamp": 2.2708428454151166 + }, + { + "x": 7.780343830854776, + "y": 4.073216939396029, + "heading": -1.3104250722395789e-17, + "angularVelocity": -1.03413416429204e-17, + "velocityX": 1.7197951977010522, + "velocityY": 0.05808832008572811, + "timestamp": 2.336362819820226 + }, + { + "x": 7.874322400829606, + "y": 4.07602285690707, + "heading": -1.2397928827700396e-17, + "angularVelocity": 1.0780252923201902e-17, + "velocityX": 1.4343499189081053, + "velocityY": 0.04282537556703236, + "timestamp": 2.401882794225336 + }, + { + "x": 7.949612474283929, + "y": 4.07759641137373, + "heading": -1.096542742535041e-17, + "angularVelocity": 2.1863583068378234e-17, + "velocityX": 1.1491163441060581, + "velocityY": 0.024016408445632246, + "timestamp": 2.4674027686304454 + }, + { + "x": 8.006227098004604, + "y": 4.077753556862197, + "heading": -9.354890651543402e-18, + "angularVelocity": 2.458085169441397e-17, + "velocityX": 0.8640818961654191, + "velocityY": 0.0023984363530761407, + "timestamp": 2.532922743035555 + }, + { + "x": 8.044178200157203, + "y": 4.076344939580228, + "heading": -5.6627659945325885e-18, + "angularVelocity": 5.635113094762732e-17, + "velocityX": 0.579229502104909, + "velocityY": -0.021499051163544333, + "timestamp": 2.5984427174406646 }, { "x": 8.0634765625, "y": 4.073246955871582, "heading": 0, - "angularVelocity": 1.956541790475601e-17, - "velocityX": 0.30669788797884484, - "velocityY": -0.15156787310674, - "timestamp": 2.7468707594149366 - }, - { - "x": 8.064977337405587, - "y": 4.058030189469969, - "heading": 1.4400664949819045e-18, - "angularVelocity": 1.9298542246489556e-17, - "velocityX": 0.0201121046014515, - "velocityY": -0.20392211811774588, - "timestamp": 2.821491239733933 - }, - { - "x": 8.04498852817461, - "y": 4.039529293710826, - "heading": 2.8532799754877234e-18, - "angularVelocity": 1.8938681059523328e-17, - "velocityX": -0.26787296390383064, - "velocityY": -0.2479332172622413, - "timestamp": 2.8961117200529296 - }, - { - "x": 8.003412006395102, - "y": 4.018467462910651, - "heading": 4.2316573286544775e-18, - "angularVelocity": 1.847183698342028e-17, - "velocityX": -0.5571730656486096, - "velocityY": -0.2822526833134443, - "timestamp": 2.970732200371926 - }, - { - "x": 7.940164057594933, - "y": 3.9956918759860844, - "heading": 5.565943858803391e-18, - "angularVelocity": 1.7880969384827547e-17, - "velocityX": -0.8475950373113531, - "velocityY": -0.30521898046224694, - "timestamp": 3.0453526806909226 - }, - { - "x": 7.85518871719706, - "y": 3.9722034430196183, - "heading": 6.5199606818876674e-18, - "angularVelocity": 1.2784919284944702e-17, - "velocityX": -1.138766998478306, - "velocityY": -0.31477193481005983, - "timestamp": 3.119973161009919 - }, - { - "x": 7.748479579667781, - "y": 3.949194323259055, - "heading": 7.228395857909563e-18, - "angularVelocity": 9.493843624470073e-18, - "velocityX": -1.4300248011418268, - "velocityY": -0.3083485882454945, - "timestamp": 3.1945936413289155 - }, - { - "x": 7.6201153258458145, - "y": 3.928094024739179, - "heading": 7.8541038899261e-18, - "angularVelocity": 8.385205006937927e-18, - "velocityX": -1.7202281903469572, - "velocityY": -0.28276819486653393, - "timestamp": 3.269214121647912 - }, - { - "x": 7.470317035092631, - "y": 3.9106227342866235, - "heading": 8.379134437333569e-18, - "angularVelocity": 7.036011272928214e-18, - "velocityX": -2.00746886260729, - "velocityY": -0.234135325554959, - "timestamp": 3.3438346019669085 - }, - { - "x": 7.299538426697331, - "y": 3.8988447632701435, - "heading": 8.78216866852809e-18, - "angularVelocity": 5.401120751271277e-18, - "velocityX": -2.2886291761355184, - "velocityY": -0.15783831685523833, - "timestamp": 3.418455082285905 - }, - { - "x": 7.108600470483675, - "y": 3.8952024104802057, - "heading": 9.038204388282958e-18, - "angularVelocity": 3.4311721461170713e-18, - "velocityX": -2.5587875526586417, - "velocityY": -0.04881170389640219, - "timestamp": 3.4930755626049015 - }, - { - "x": 6.89887047602872, - "y": 3.902489232444511, - "heading": 9.11887229683693e-18, - "angularVelocity": 1.0810424542935678e-18, - "velocityX": -2.810622413020914, - "velocityY": 0.09765176977090569, - "timestamp": 3.567696042923898 - }, - { - "x": 6.672447487864324, - "y": 3.9237019834694964, - "heading": 8.994132821099478e-18, - "angularVelocity": -1.6716520183319338e-18, - "velocityX": -3.0343276697825954, - "velocityY": 0.28427518737888835, - "timestamp": 3.6423165232428945 - }, - { - "x": 6.4322495960952635, - "y": 3.961732568936921, - "heading": 8.636009892152731e-18, - "angularVelocity": -4.799257891447058e-18, - "velocityX": -3.218927173106323, - "velocityY": 0.5096534531116348, - "timestamp": 3.716937003561891 - }, - { - "x": 6.181871022214133, - "y": 4.01897099744758, - "heading": 8.023682097716734e-18, - "angularVelocity": -8.205894562430798e-18, - "velocityX": -3.3553599871078905, - "velocityY": 0.7670605746032516, - "timestamp": 3.7915574838808874 - }, - { - "x": 5.925195916976863, - "y": 4.0970166657243565, - "heading": 7.147197665220672e-18, - "angularVelocity": -1.17458963959573e-17, - "velocityX": -3.4397407272106473, - "velocityY": 1.0459014461329927, - "timestamp": 3.866177964199884 + "angularVelocity": 8.642808619399734e-17, + "velocityX": 0.2945416648589691, + "velocityY": -0.04728304210698771, + "timestamp": 2.663962691845774 + }, + { + "x": 8.06151161206475, + "y": 4.067466722329622, + "heading": 7.499406153547556e-18, + "angularVelocity": 1.0142249804726799e-16, + "velocityX": -0.026574128354115305, + "velocityY": -0.07817228633622807, + "timestamp": 2.7379049265285085 + }, + { + "x": 8.0357781795929, + "y": 4.059672922438978, + "heading": 2.0171548175080756e-17, + "angularVelocity": 1.7137894295700475e-16, + "velocityX": -0.34802075677407873, + "velocityY": -0.10540389973451367, + "timestamp": 2.811847161211243 + }, + { + "x": 7.986251299714503, + "y": 4.050185891028673, + "heading": 3.259640252688688e-17, + "angularVelocity": 1.6803460705902343e-16, + "velocityX": -0.6698050186189494, + "velocityY": -0.12830328229881344, + "timestamp": 2.885789395893977 + }, + { + "x": 7.9129066919285, + "y": 4.039390810897977, + "heading": 4.5982022945133473e-17, + "angularVelocity": 1.81028075140569e-16, + "velocityX": -0.9919176516736913, + "velocityY": -0.1459934255032282, + "timestamp": 2.9597316305767114 + }, + { + "x": 7.8157231380567715, + "y": 4.027759201442648, + "heading": 5.916672321533067e-17, + "angularVelocity": 1.783108168799044e-16, + "velocityX": -1.314317240866671, + "velocityY": -0.1573067071239705, + "timestamp": 3.0336738652594457 + }, + { + "x": 7.694687242510101, + "y": 4.015880811506709, + "heading": 7.241021515042493e-17, + "angularVelocity": 1.7910591950468916e-16, + "velocityX": -1.6368979929535623, + "velocityY": -0.16064418375919837, + "timestamp": 3.10761609994218 + }, + { + "x": 7.549803190746082, + "y": 4.0045128268534045, + "heading": 8.516404983291186e-17, + "angularVelocity": 1.7248376026751216e-16, + "velocityX": -1.9594221406165937, + "velocityY": -0.1537414266972227, + "timestamp": 3.1815583346249143 + }, + { + "x": 7.381113647590609, + "y": 3.9946593063008904, + "heading": 9.839417244429125e-17, + "angularVelocity": 1.7892511182976314e-16, + "velocityX": -2.281369286163345, + "velocityY": -0.13325970732143547, + "timestamp": 3.2555005693076486 + }, + { + "x": 7.188747477257485, + "y": 3.987706179482555, + "heading": 1.110446332225851e-16, + "angularVelocity": 1.7108572429643452e-16, + "velocityX": -2.6015736629885633, + "velocityY": -0.09403457777777122, + "timestamp": 3.329442803990383 + }, + { + "x": 6.973038504473885, + "y": 3.9856630874408094, + "heading": 1.1423152127106693e-16, + "angularVelocity": 4.309969887337601e-17, + "velocityX": -2.91726337064529, + "velocityY": -0.027630920954873416, + "timestamp": 3.4033850386731173 + }, + { + "x": 6.734853840508573, + "y": 3.9916098664093003, + "heading": 1.1357543736188786e-16, + "angularVelocity": -8.872925078647214e-18, + "velocityX": -3.221226204310621, + "velocityY": 0.0804246584378634, + "timestamp": 3.4773272733558516 + }, + { + "x": 6.47658832080339, + "y": 4.010431184705039, + "heading": 1.1486840283254658e-16, + "angularVelocity": 1.7486156277295078e-17, + "velocityX": -3.4928011144554563, + "velocityY": 0.25454083686402557, + "timestamp": 3.551269508038586 + }, + { + "x": 6.204860010324235, + "y": 4.048943809403862, + "heading": 1.1487825258491134e-16, + "angularVelocity": 1.3320874674868913e-19, + "velocityX": -3.6748728469603784, + "velocityY": 0.5208474542873691, + "timestamp": 3.62521174272132 + }, + { + "x": 5.932253242931649, + "y": 4.111293853023339, + "heading": 1.0773394940295777e-16, + "angularVelocity": -9.662006041077537e-17, + "velocityX": -3.686753160250877, + "velocityY": 0.8432263899921877, + "timestamp": 3.6991539774040545 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 1.1380657643949956e-17, - "angularVelocity": 5.673321785133305e-17, - "velocityX": -3.4741693841666152, - "velocityY": 1.3351885220272963, - "timestamp": 3.9407984445188804 - }, - { - "x": 5.562354883757379, - "y": 4.239983693882295, - "heading": 1.257506167197437e-17, - "angularVelocity": 4.001149623529015e-17, - "velocityX": -3.4704042300707463, - "velocityY": 1.4516720671542953, - "timestamp": 3.970649965608488 - }, - { - "x": 5.458854001324223, - "y": 4.2867960161311025, - "heading": 1.368919721006296e-17, - "angularVelocity": 3.7322571632702434e-17, - "velocityX": -3.467189565398273, - "velocityY": 1.5681720910732326, - "timestamp": 4.000501486698096 - }, - { - "x": 5.355427026400021, - "y": 4.337086579385366, - "heading": 1.4765466722121167e-17, - "angularVelocity": 3.605409269066309e-17, - "velocityX": -3.46471372811238, - "velocityY": 1.6846901403550338, - "timestamp": 4.030353007787704 - }, - { - "x": 5.252042805813138, - "y": 4.3908559053171485, - "heading": 1.577172278579381e-17, - "angularVelocity": 3.370870306062832e-17, - "velocityX": -3.4632814949879767, - "velocityY": 1.8012256651973733, - "timestamp": 4.060204528877311 - }, - { - "x": 5.148654011351914, - "y": 4.4481042525220715, - "heading": 1.6688127577708658e-17, - "angularVelocity": 3.0698763599309256e-17, - "velocityX": -3.4634347158013203, - "velocityY": 1.917769852768155, - "timestamp": 4.090056049966919 - }, - { - "x": 5.045180193102648, - "y": 4.508830583010288, - "heading": 1.7559230594262742e-17, - "angularVelocity": 2.9181193522489866e-17, - "velocityX": -3.4662829387708096, - "velocityY": 2.034279268581587, - "timestamp": 4.119907571056527 - }, - { - "x": 4.941454797962972, - "y": 4.5730268239608955, - "heading": 1.8097620255598777e-17, - "angularVelocity": 1.8035585476593816e-17, - "velocityX": -3.4747105458483385, - "velocityY": 2.1505182519143307, - "timestamp": 4.149759092146135 - }, - { - "x": 4.836940518620058, - "y": 4.640611448605407, - "heading": 1.8315935161814963e-17, - "angularVelocity": 7.313359523284407e-18, - "velocityX": -3.5011374807060482, - "velocityY": 2.264026159391885, - "timestamp": 4.1796106132357425 - }, - { - "x": 4.729371052476075, - "y": 4.706532042565262, - "heading": 1.8014793483289063e-17, - "angularVelocity": -1.0087984381831229e-17, - "velocityX": -3.603483581995116, - "velocityY": 2.2082825783642597, - "timestamp": 4.20946213432535 - }, - { - "x": 4.620024826639761, - "y": 4.769461542081556, - "heading": 1.7990520308788652e-17, - "angularVelocity": -8.131302397490335e-19, - "velocityX": -3.663003486759619, - "velocityY": 2.1080835153221735, - "timestamp": 4.239313655414958 - }, - { - "x": 4.508985011067914, - "y": 4.829352072952906, - "heading": 1.7846349965295797e-17, - "angularVelocity": -4.829581147982843e-18, - "velocityX": -3.7197372702894755, - "velocityY": 2.0062807081612415, - "timestamp": 4.269165176504566 - }, - { - "x": 4.396354195056893, - "y": 4.886148687843668, - "heading": 1.7996479956698872e-17, - "angularVelocity": 5.029224125294331e-18, - "velocityX": -3.7730344015946646, - "velocityY": 1.9026372130341678, - "timestamp": 4.2990166975941735 - }, - { - "x": 4.28245727872322, - "y": 4.939704839757233, - "heading": 1.8249745379226758e-17, - "angularVelocity": 8.484171416768941e-18, - "velocityX": -3.8154476615036823, - "velocityY": 1.794084520946214, - "timestamp": 4.328868218683781 + "heading": 9.686455716091523e-17, + "angularVelocity": -1.4699842774133078e-16, + "velocityX": -3.6014804699029255, + "velocityY": 1.1543500395591086, + "timestamp": 3.773096212086789 + }, + { + "x": 5.543077137245883, + "y": 4.241512673300566, + "heading": 9.161999350902719e-17, + "angularVelocity": -1.5163138697871951e-16, + "velocityX": -3.552563374503243, + "velocityY": 1.2971011802444727, + "timestamp": 3.8076837981821674 + }, + { + "x": 5.422090048254055, + "y": 4.291242278322373, + "heading": 8.675864551293739e-17, + "angularVelocity": -1.4055181482408883e-16, + "velocityX": -3.4979916973158245, + "velocityY": 1.4377876757479933, + "timestamp": 3.842271384277546 + }, + { + "x": 5.303183074500814, + "y": 4.34575871765036, + "heading": 8.202370382364629e-17, + "angularVelocity": -1.368971421201274e-16, + "velocityX": -3.437851182367712, + "velocityY": 1.5761851427750155, + "timestamp": 3.8768589703729246 + }, + { + "x": 5.186545516053187, + "y": 4.404975197881964, + "heading": 7.882281033080331e-17, + "angularVelocity": -9.254457608045447e-17, + "velocityX": -3.372237603572297, + "velocityY": 1.712073229635336, + "timestamp": 3.911446556468303 + }, + { + "x": 5.072363058490256, + "y": 4.468797441531519, + "heading": 7.597641724828609e-17, + "angularVelocity": -8.229522218370685e-17, + "velocityX": -3.3012554633926476, + "velocityY": 1.845235555715285, + "timestamp": 3.946034142563682 + }, + { + "x": 4.960817475466504, + "y": 4.537123834649348, + "heading": 7.51892087087232e-17, + "angularVelocity": -2.2759857753359948e-17, + "velocityX": -3.225017863812662, + "velocityY": 1.975460008380286, + "timestamp": 3.9806217286590604 + }, + { + "x": 4.852086330068725, + "y": 4.609845576159071, + "heading": 7.00186010980759e-17, + "angularVelocity": -1.4949316197981958e-16, + "velocityX": -3.143646541216975, + "velocityY": 2.1025387926519685, + "timestamp": 4.015209314754439 + }, + { + "x": 4.744615410553515, + "y": 4.6844171206048495, + "heading": 6.888474965661026e-17, + "angularVelocity": -3.278203452357036e-17, + "velocityX": -3.10721075529382, + "velocityY": 2.1560204935996916, + "timestamp": 4.049796900849818 + }, + { + "x": 4.634255259438232, + "y": 4.75464211813236, + "heading": 6.027771904333573e-17, + "angularVelocity": -2.4884739251649313e-16, + "velocityX": -3.190744529293135, + "velocityY": 2.0303526627692405, + "timestamp": 4.084384486945196 + }, + { + "x": 4.521181551584758, + "y": 4.820408744704174, + "heading": 5.1990889261648227e-17, + "angularVelocity": -2.395897117201502e-16, + "velocityX": -3.269199172838061, + "velocityY": 1.9014517633713166, + "timestamp": 4.118972073040575 + }, + { + "x": 4.405574295139685, + "y": 4.881612290783718, + "heading": 4.5533169228459885e-17, + "angularVelocity": -1.867062944312063e-16, + "velocityX": -3.3424494015360238, + "velocityY": 1.7695234906179804, + "timestamp": 4.153559659135953 + }, + { + "x": 4.287617535211261, + "y": 4.938155315408326, + "heading": 4.203003409755996e-17, + "angularVelocity": -1.0128301874666137e-16, + "velocityX": -3.410378498318693, + "velocityY": 1.6347779942979137, + "timestamp": 4.188147245231332 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 1.8296114714140825e-17, - "angularVelocity": 1.5533324384911975e-18, - "velocityX": -3.8510001878620828, - "velocityY": 1.6830953424405728, - "timestamp": 4.358719739773389 - }, - { - "x": 3.9173080803752227, - "y": 5.082092147187362, - "heading": 1.4829639745730528e-17, - "angularVelocity": -5.4272726613113606e-17, - "velocityX": -3.9171051476985386, - "velocityY": 1.4426543500365565, - "timestamp": 4.4225911385717955 - }, - { - "x": 3.6655222616013243, - "y": 5.158389406763847, - "heading": 1.1586116724078029e-17, - "angularVelocity": -5.078208837148102e-17, - "velocityX": -3.9420745985005192, - "velocityY": 1.1945449921536402, - "timestamp": 4.486462537370202 - }, - { - "x": 3.415686594925907, - "y": 5.218879368068439, - "heading": 9.697216184828002e-18, - "angularVelocity": -2.957349571999372e-17, - "velocityX": -3.911542120189908, - "velocityY": 0.9470586591584124, - "timestamp": 4.550333936168609 - }, - { - "x": 3.1720421714343097, - "y": 5.264694784984283, - "heading": 8.965708374731706e-18, - "angularVelocity": -1.1452822775166807e-17, - "velocityX": -3.814609168973968, - "velocityY": 0.7173072420168656, - "timestamp": 4.614205334967015 - }, - { - "x": 2.938638877133611, - "y": 5.298312127499332, - "heading": 9.343063585543127e-18, - "angularVelocity": 5.9080467348504174e-18, - "velocityX": -3.654269339510983, - "velocityY": 0.5263285781661498, - "timestamp": 4.678076733765422 - }, - { - "x": 2.718391564246048, - "y": 5.322952057954089, - "heading": 1.0698464289017726e-17, - "angularVelocity": 2.12207768493952e-17, - "velocityX": -3.4482932428443296, - "velocityY": 0.3857740854012944, - "timestamp": 4.741948132563828 - }, - { - "x": 2.5129378876059687, - "y": 5.341690959630172, - "heading": 1.2882468345126029e-17, - "angularVelocity": 3.4193772081618774e-17, - "velocityX": -3.216677268780943, - "velocityY": 0.29338486440898187, - "timestamp": 4.805819531362235 - }, - { - "x": 2.323045360609233, - "y": 5.357035089630098, - "heading": 1.4586731405592636e-17, - "angularVelocity": 2.6682726421160203e-17, - "velocityX": -2.973044751940999, - "velocityY": 0.24023475747502873, - "timestamp": 4.869690930160641 - }, - { - "x": 2.149011712470278, - "y": 5.370906179422888, - "heading": 1.3182509626883337e-17, - "angularVelocity": -2.1985142076939443e-17, - "velocityX": -2.724750849566412, - "velocityY": 0.21717216240355022, - "timestamp": 4.933562328959048 - }, - { - "x": 1.9909052012063986, - "y": 5.384762303843791, - "heading": 1.2288275023072503e-17, - "angularVelocity": -1.400054828244982e-17, - "velocityX": -2.4753882682748096, - "velocityY": 0.2169378576573164, - "timestamp": 4.997433727757454 - }, - { - "x": 1.8486875999585186, - "y": 5.399721366691689, - "heading": 1.1833725700471485e-17, - "angularVelocity": -7.116633256521739e-18, - "velocityX": -2.2266241842730174, - "velocityY": 0.23420596901458723, - "timestamp": 5.061305126555861 - }, - { - "x": 1.7222741786291598, - "y": 5.416655739466683, - "heading": 1.1765320590646606e-17, - "angularVelocity": -1.070981842258772e-18, - "velocityX": -1.9791866736526023, - "velocityY": 0.2651323298624305, - "timestamp": 5.125176525354267 - }, - { - "x": 1.6115626522567748, - "y": 5.436259280896622, - "heading": 1.2039434559055613e-17, - "angularVelocity": 4.291654398939878e-18, - "velocityX": -1.7333505834405774, - "velocityY": 0.30692206212380396, - "timestamp": 5.189047924152674 - }, - { - "x": 1.5164470746291163, - "y": 5.459093977101566, - "heading": 9.126879142292297e-18, - "angularVelocity": -4.560030730930113e-17, - "velocityX": -1.4891732358620327, - "velocityY": 0.3575105075906568, - "timestamp": 5.25291932295108 - }, - { - "x": 1.4368243680409203, - "y": 5.485622578468376, - "heading": 6.490550146878691e-18, - "angularVelocity": -4.1275579397551e-17, - "velocityX": -1.2466097202521527, - "velocityY": 0.41534398597625355, - "timestamp": 5.316790721749487 - }, - { - "x": 1.3725972060727116, - "y": 5.516231768358422, - "heading": 4.104803215844785e-18, - "angularVelocity": -3.735235133440495e-17, - "velocityX": -1.0055699918350633, - "velocityY": 0.47923155693921204, - "timestamp": 5.3806621205478935 - }, - { - "x": 1.3236750778063309, - "y": 5.551248899658597, - "heading": 1.947639425952167e-18, - "angularVelocity": -3.3773548532536126e-17, - "velocityX": -0.7659473439870959, - "velocityY": 0.5482443152794746, - "timestamp": 5.4445335193463 + "heading": 4.0833427337575637e-17, + "angularVelocity": -3.459642302543387e-17, + "velocityX": -3.4728780863994273, + "velocityY": 1.4974297517257, + "timestamp": 4.222734831326711 + }, + { + "x": 3.9262086603019655, + "y": 5.072405406382895, + "heading": 4.69185026085022e-17, + "angularVelocity": 9.025220132506432e-17, + "velocityX": -3.5787544523378267, + "velocityY": 1.2229891223429616, + "timestamp": 4.290157843525713 + }, + { + "x": 3.6792395049411635, + "y": 5.135860556463708, + "heading": 5.846977032472703e-17, + "angularVelocity": 1.713252988776416e-16, + "velocityX": -3.6629801503359496, + "velocityY": 0.9411497352494828, + "timestamp": 4.3575808557247155 + }, + { + "x": 3.4280856473907875, + "y": 5.179929383329658, + "heading": 6.25257434115025e-17, + "angularVelocity": 6.01571029606942e-17, + "velocityX": -3.7250465287591408, + "velocityY": 0.6536169985387049, + "timestamp": 4.425003867923718 + }, + { + "x": 3.174266449354605, + "y": 5.204345315411431, + "heading": 8.981508430066906e-17, + "angularVelocity": 4.0474817127156825e-16, + "velocityX": -3.7645781426529163, + "velocityY": 0.3621305439411197, + "timestamp": 4.4924268801227205 + }, + { + "x": 2.919317391668449, + "y": 5.208960723831647, + "heading": 3.637405660157601e-17, + "angularVelocity": -7.926229629337798e-16, + "velocityX": -3.781335917381794, + "velocityY": 0.06845449750291784, + "timestamp": 4.559849892321723 + }, + { + "x": 2.6778199582724502, + "y": 5.213921183762201, + "heading": 2.780922480909488e-17, + "angularVelocity": -1.270312837495845e-16, + "velocityX": -3.581825040435896, + "velocityY": 0.07357220878699586, + "timestamp": 4.6272729045207255 + }, + { + "x": 2.4555441151084385, + "y": 5.223767512233129, + "heading": 2.258282570608997e-17, + "angularVelocity": -7.751654714759439e-17, + "velocityX": -3.2967355790624753, + "velocityY": 0.14603809811799462, + "timestamp": 4.694695916719728 + }, + { + "x": 2.2524898779476, + "y": 5.238499719208596, + "heading": 1.8179159660483545e-17, + "angularVelocity": -6.531399151152503e-17, + "velocityX": -3.0116458837750555, + "velocityY": 0.2185041352347682, + "timestamp": 4.7621189289187305 + }, + { + "x": 2.068657252210669, + "y": 5.258117807366149, + "heading": 1.4559970052520395e-17, + "angularVelocity": -5.367884777868486e-17, + "velocityX": -2.726556108088765, + "velocityY": 0.29097021206423906, + "timestamp": 4.829541941117733 + }, + { + "x": 1.9040462406637386, + "y": 5.282621777825342, + "heading": 1.0447672435760193e-17, + "angularVelocity": -6.099249326267761e-17, + "velocityX": -2.441466291376519, + "velocityY": 0.3634363054985942, + "timestamp": 4.896964953316735 + }, + { + "x": 1.7586568449915738, + "y": 5.312011631159126, + "heading": 6.39006439635328e-18, + "angularVelocity": -6.018135214654068e-17, + "velocityX": -2.156376449676296, + "velocityY": 0.4359024074308457, + "timestamp": 4.964387965515738 + }, + { + "x": 1.6324890663305314, + "y": 5.34628736769762, + "heading": 2.9527164791572072e-18, + "angularVelocity": -5.0981820681006015e-17, + "velocityX": -1.8712865911219376, + "velocityY": 0.508368514259319, + "timestamp": 5.03181097771474 + }, + { + "x": 1.5255429054998768, + "y": 5.385448987646794, + "heading": 6.750997693764207e-19, + "angularVelocity": -3.378099903552108e-17, + "velocityX": -1.5861967204164489, + "velocityY": 0.5808346241426653, + "timestamp": 5.099233989913743 + }, + { + "x": 1.437818363118664, + "y": 5.4294964911430075, + "heading": 3.7918321325869045e-19, + "angularVelocity": -4.388954848629936e-18, + "velocityX": -1.301106840529317, + "velocityY": 0.6533007360484864, + "timestamp": 5.166657002112745 + }, + { + "x": 1.3693154396712461, + "y": 5.478429878281045, + "heading": 1.633332658934955e-18, + "angularVelocity": 1.8601207579968088e-17, + "velocityX": -1.0160169534583994, + "velocityY": 0.7257668493601166, + "timestamp": 5.234080014311748 + }, + { + "x": 1.3200341355468752, + "y": 5.532249149129769, + "heading": 3.8843995459700536e-19, + "angularVelocity": -1.8463914088315652e-17, + "velocityX": -0.7309270606141895, + "velocityY": 0.798232963693069, + "timestamp": 5.30150302651075 }, { "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": -3.0493138713173094e-17, - "velocityX": -0.5276325143220081, - "velocityY": 0.6216460705389873, - "timestamp": 5.508404918144707 - }, - { - "x": 1.2740430355469938, - "y": 5.664007440684529, - "heading": -2.547208860482595e-18, - "angularVelocity": -2.5831234528334732e-17, - "velocityX": -0.16156041888930542, - "velocityY": 0.7408315596510565, - "timestamp": 5.607014562930199 - }, - { - "x": 1.2943003151600432, - "y": 5.748531863021803, - "heading": -4.6379232663069386e-18, - "angularVelocity": -2.1201926150692658e-17, - "velocityX": 0.20542898878822005, - "velocityY": 0.8571618173977016, - "timestamp": 5.705624207715691 - }, - { - "x": 1.3508752960803845, - "y": 5.844112348620496, - "heading": -6.276905103665225e-18, - "angularVelocity": -1.6620908041208103e-17, - "velocityX": 0.5737266475648491, - "velocityY": 0.9692813092128183, - "timestamp": 5.804233852501183 - }, - { - "x": 1.4439661676967965, - "y": 5.9500755871992865, - "heading": -7.471930527081998e-18, - "angularVelocity": -1.2118747882558993e-17, - "velocityX": 0.9440341441135343, - "velocityY": 1.0745727642493252, - "timestamp": 5.902843497286676 - }, - { - "x": 1.5739124190504803, - "y": 6.065143361581136, - "heading": -8.237944726800887e-18, - "angularVelocity": -7.768146836290714e-18, - "velocityX": 1.3177843976251842, - "velocityY": 1.1669018241791527, - "timestamp": 6.001453142072168 - }, - { - "x": 1.7413805206507829, - "y": 6.185983632591009, - "heading": -6.8828411912414524e-18, - "angularVelocity": 1.3742099353654783e-17, - "velocityX": 1.698293325816142, - "velocityY": 1.2254406886135676, - "timestamp": 6.10006278685766 - }, - { - "x": 1.9428580652109537, - "y": 6.289954848196762, - "heading": -5.594323946529715e-18, - "angularVelocity": 1.3066848045055797e-17, - "velocityX": 2.0431829462366347, - "velocityY": 1.0543716675171477, - "timestamp": 6.1986724316431525 - }, - { - "x": 2.114807562690416, - "y": 6.370065959572112, - "heading": -4.6183818981391425e-18, - "angularVelocity": 9.897024283736211e-18, - "velocityX": 1.7437391428952727, - "velocityY": 0.8124064491826988, - "timestamp": 6.297282076428645 - }, - { - "x": 2.2534346833326833, - "y": 6.431988322349882, - "heading": -2.7955204631360402e-18, - "angularVelocity": 1.8485630277436967e-17, - "velocityX": 1.4058170571836621, - "velocityY": 0.6279544248686071, - "timestamp": 6.395891721214137 - }, - { - "x": 2.357842355716001, - "y": 6.477471673664644, - "heading": -1.484023928114358e-18, - "angularVelocity": 1.3299880944166003e-17, - "velocityX": 1.0587977738935974, - "velocityY": 0.4612464776006573, - "timestamp": 6.494501365999629 - }, - { - "x": 2.427639134257716, - "y": 6.507357014317594, - "heading": -4.934983093022998e-19, - "angularVelocity": 1.0044916213421236e-17, - "velocityX": 0.7078088425685477, - "velocityY": 0.30306711597998603, - "timestamp": 6.593111010785122 + "angularVelocity": -5.761237033299429e-18, + "velocityX": -0.4458371630310619, + "velocityY": 0.8706990788013996, + "timestamp": 5.368926038709753 + }, + { + "x": 1.2868335136673428, + "y": 5.68644048793425, + "heading": -1.3391788419363526e-18, + "angularVelocity": -1.3686362473728894e-17, + "velocityX": -0.03210027395244838, + "velocityY": 0.9758655723245745, + "timestamp": 5.466773720706915 + }, + { + "x": 1.3242854311298142, + "y": 5.791775499822582, + "heading": 2.339658407779995e-18, + "angularVelocity": 3.759759224271494e-17, + "velocityX": 0.3827573295354856, + "velocityY": 1.076520258204863, + "timestamp": 5.564621402704078 + }, + { + "x": 1.4024881161679177, + "y": 5.906284066156856, + "heading": 5.091444284753208e-18, + "angularVelocity": 2.812315857202659e-17, + "velocityX": 0.7992287956333111, + "velocityY": 1.1702736743175466, + "timestamp": 5.66246908470124 + }, + { + "x": 1.5216854983005004, + "y": 6.028805982329465, + "heading": 6.6702155316476646e-18, + "angularVelocity": 1.6134988737668432e-17, + "velocityX": 1.218193213162059, + "velocityY": 1.2521698385881244, + "timestamp": 5.760316766698403 + }, + { + "x": 1.682281091295048, + "y": 6.156892671597042, + "heading": 8.737231811568005e-18, + "angularVelocity": 2.1124836460875052e-17, + "velocityX": 1.6412815277443633, + "velocityY": 1.3090416313724325, + "timestamp": 5.8581644486955655 + }, + { + "x": 1.884561264278339, + "y": 6.2823011062588545, + "heading": 4.474450456421642e-18, + "angularVelocity": -4.3565481249463154e-17, + "velocityX": 2.06729652511499, + "velocityY": 1.2816699599020622, + "timestamp": 5.956012130692728 + }, + { + "x": 2.074597433027926, + "y": 6.367773862583125, + "heading": 1.3148438373880832e-18, + "angularVelocity": -3.229107274750916e-17, + "velocityX": 1.9421632160391744, + "velocityY": 0.8735286782445029, + "timestamp": 6.053859812689891 + }, + { + "x": 2.2289686427209534, + "y": 6.431502913374048, + "heading": 4.376002899474807e-18, + "angularVelocity": 3.128494207400489e-17, + "velocityX": 1.5776685409625097, + "velocityY": 0.6513087432441247, + "timestamp": 6.151707494687053 + }, + { + "x": 2.345509714236833, + "y": 6.4775215317101145, + "heading": 6.666442968036081e-18, + "angularVelocity": 2.3408220019295113e-17, + "velocityX": 1.1910458085175606, + "velocityY": 0.4703087226675544, + "timestamp": 6.249555176684216 + }, + { + "x": 2.4235057296596247, + "y": 6.507444880215651, + "heading": 4.695809387720839e-18, + "angularVelocity": -2.013980853353183e-17, + "velocityX": 0.7971166391560869, + "velocityY": 0.30581560947355857, + "timestamp": 6.347402858681378 }, { "x": 2.462606906890869, "y": 6.522137641906738, - "heading": 1.7665246335106923e-37, - "angularVelocity": 5.004564321614195e-18, - "velocityX": 0.35460803767440047, - "velocityY": 0.14989028326080325, - "timestamp": 6.691720655570614 + "heading": 1.18409559006069e-37, + "angularVelocity": -4.799101310235197e-17, + "velocityX": 0.3996127085808573, + "velocityY": 0.15015952745322653, + "timestamp": 6.445250540678541 }, { "x": 2.462606906890869, "y": 6.522137641906738, - "heading": -7.406725507450864e-38, - "angularVelocity": -2.5425466979238586e-36, - "velocityX": -6.628025513542663e-35, - "velocityY": 2.827272125617884e-34, - "timestamp": 6.790330300356106 + "heading": 7.445601745662792e-38, + "angularVelocity": -4.492041573884033e-37, + "velocityX": -6.88570869510922e-34, + "velocityY": -3.3118407683240263e-34, + "timestamp": 6.543098222675703 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.traj index fa6dd84f..59b728c7 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.traj @@ -4,1936 +4,1828 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": -1.960896274672709e-29, + "angularVelocity": -6.079995614896932e-33, "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 1.3069454293752283, - "y": 5.5749747243722725, - "heading": -0.008996462194644695, - "angularVelocity": -0.11617133047486594, - "velocityX": 0.21914626962200023, - "velocityY": -0.20634433353719148, - "timestamp": 0.07744132875022905 - }, - { - "x": 1.340887418869277, - "y": 5.543015778149598, - "heading": -0.02698653550593814, - "angularVelocity": -0.23230584497508605, - "velocityX": 0.4382929637423898, - "velocityY": -0.4126859228589361, - "timestamp": 0.1548826575004581 - }, - { - "x": 1.391800616271645, - "y": 5.495077701168919, - "heading": -0.05395916302114244, - "angularVelocity": -0.34829758154372603, - "velocityX": 0.6574421981657251, - "velocityY": -0.6190244634792922, - "timestamp": 0.23232398625068715 - }, - { - "x": 1.459685380828832, - "y": 5.431160783444383, - "heading": -0.08989644453732974, - "angularVelocity": -0.4640581727637966, - "velocityX": 0.8765960715396762, - "velocityY": -0.8253592591464762, - "timestamp": 0.3097653150009162 - }, - { - "x": 1.5445422260314938, - "y": 5.351265403464515, - "heading": -0.13477553337937057, - "angularVelocity": -0.579523744831976, - "velocityX": 1.0957565756192689, - "velocityY": -1.0316891673907769, - "timestamp": 0.3872066437511452 - }, - { - "x": 1.6463718016735291, - "y": 5.255392061024326, - "heading": -0.18857099282512366, - "angularVelocity": -0.6946608524671277, - "velocityX": 1.3149254704884978, - "velocityY": -1.2380126217812786, - "timestamp": 0.46464797250137424 - }, - { - "x": 1.7651748655395048, - "y": 5.143541401111082, - "heading": -0.25125740651293493, - "angularVelocity": -0.8094697585842596, - "velocityX": 1.5341041506310307, - "velocityY": -1.4443277474502212, - "timestamp": 0.5420893012516033 - }, - { - "x": 1.9009522484980113, - "y": 5.015714222493906, - "heading": -0.3228118649000377, - "angularVelocity": -0.9239828337396184, - "velocityX": 1.7532935597809811, - "velocityY": -1.6506325586122341, - "timestamp": 0.6195306300018323 - }, - { - "x": 2.053704829775847, - "y": 4.871911475725846, - "heading": -0.4032156639032303, - "angularVelocity": -1.0382543830392266, - "velocityX": 1.9724943223879574, - "velocityY": -1.8569250952812175, - "timestamp": 0.6969719587520613 - }, - { - "x": 2.1895539905378896, - "y": 4.744100163466478, - "heading": -0.47392252589131595, - "angularVelocity": -0.9130378201026126, - "velocityX": 1.7542204266665722, - "velocityY": -1.6504276763046646, - "timestamp": 0.7744132875022903 - }, - { - "x": 2.3084268719206142, - "y": 4.632263014410205, - "heading": -0.535794025352391, - "angularVelocity": -0.7989467698895206, - "velocityX": 1.5350057043302445, - "velocityY": -1.4441532817287048, - "timestamp": 0.8518546162525193 - }, - { - "x": 2.410322968991003, - "y": 4.536399460392939, - "heading": -0.5888342696498629, - "angularVelocity": -0.6849087580665111, - "velocityX": 1.3157844617930656, - "velocityY": -1.237886223856486, - "timestamp": 0.9292959450027484 - }, - { - "x": 2.495241902358175, - "y": 4.456509045521313, - "heading": -0.6330449707790676, - "angularVelocity": -0.5708928532419475, - "velocityX": 1.096558320180854, - "velocityY": -1.0316250529360251, - "timestamp": 1.0067372737529774 - }, - { - "x": 2.5631833814386416, - "y": 4.392591413105038, - "heading": -0.6684266086422785, - "angularVelocity": -0.4568831453961845, - "velocityX": 0.8773284262635802, - "velocityY": -0.8253684879578497, - "timestamp": 1.0841786025032065 - }, - { - "x": 2.6141471938399374, - "y": 4.34464629797767, - "heading": -0.694979006837532, - "angularVelocity": -0.34287115967158255, - "velocityX": 0.6580957897220853, - "velocityY": -0.6191153470743593, - "timestamp": 1.1616199312534357 - }, - { - "x": 2.648133201868436, - "y": 4.3126735215310195, - "heading": -0.7127016614525554, - "angularVelocity": -0.22885266692700632, - "velocityX": 0.4388613751425609, - "velocityY": -0.4128645125587087, - "timestamp": 1.2390612600036648 - }, - { - "x": 2.6651413416897705, - "y": 4.296672988888067, - "heading": -0.7215939290593514, - "angularVelocity": -0.11482586559568532, - "velocityX": 0.21962613626976676, - "velocityY": -0.20661490319016262, - "timestamp": 1.316502588753894 + "x": 1.306947763000364, + "y": 5.574973108257302, + "heading": -0.008994433988091095, + "angularVelocity": -0.12277149964907928, + "velocityX": 0.23168094435597542, + "velocityY": -0.21813883323536412, + "timestamp": 0.07326157955062942 + }, + { + "x": 1.3408944736698363, + "y": 5.543010933982932, + "heading": -0.026978879063552612, + "angularVelocity": -0.24548262794460876, + "velocityX": 0.463363073492191, + "velocityY": -0.4362747086592839, + "timestamp": 0.14652315910125885 + }, + { + "x": 1.3918148440551168, + "y": 5.495068027805583, + "heading": -0.05394057952659419, + "angularVelocity": -0.3680196445178875, + "velocityX": 0.695048764954495, + "velocityY": -0.6544072141417159, + "timestamp": 0.21978473865188827 + }, + { + "x": 1.4597093144768722, + "y": 5.431144700814373, + "heading": -0.08985971213865897, + "angularVelocity": -0.4902860794482582, + "velocityX": 0.9267404666703231, + "velocityY": -0.8725354733449758, + "timestamp": 0.2930463182025177 + }, + { + "x": 1.5445785095442424, + "y": 5.351241370164591, + "heading": -0.13471107898519388, + "angularVelocity": -0.6122085699167754, + "velocityX": 1.1584406941256162, + "velocityY": -1.0906580384956333, + "timestamp": 0.3663078977531471 + }, + { + "x": 1.6464232512218273, + "y": 5.2553586089581215, + "heading": -0.1884659824129613, + "angularVelocity": -0.733739345472597, + "velocityX": 1.390152141167011, + "velocityY": -1.3087727809664234, + "timestamp": 0.4395694773037765 + }, + { + "x": 1.7652446226105383, + "y": 5.143497220947543, + "heading": -0.25109340653071877, + "angularVelocity": -0.8548467628175698, + "velocityX": 1.6218783722318773, + "velocityY": -1.5268765524400676, + "timestamp": 0.5128310568544059 + }, + { + "x": 1.9010443771824763, + "y": 5.015658495720495, + "heading": -0.32255561727496634, + "angularVelocity": -0.9754391207858365, + "velocityX": 1.8536285376988066, + "velocityY": -1.7449627214043517, + "timestamp": 0.5860926364050353 + }, + { + "x": 2.053859595250169, + "y": 4.871864096281773, + "heading": -0.4022671776466062, + "angularVelocity": -1.0880404280193408, + "velocityX": 2.0858848390251463, + "velocityY": -1.9627531964329257, + "timestamp": 0.6593542159556647 + }, + { + "x": 2.1896989198822134, + "y": 4.744047518400832, + "heading": -0.47311179344304544, + "angularVelocity": -0.9670091230763074, + "velocityX": 1.8541686579139196, + "velocityY": -1.7446604163456465, + "timestamp": 0.7326157955062941 + }, + { + "x": 2.308560686135844, + "y": 4.632207510590712, + "heading": -0.535112719027594, + "angularVelocity": -0.846295233666115, + "velocityX": 1.6224297508012064, + "velocityY": -1.5265847187041606, + "timestamp": 0.8058773750569235 + }, + { + "x": 2.410444090432368, + "y": 4.536343322874329, + "heading": -0.5882784511543253, + "angularVelocity": -0.72569732256441, + "velocityX": 1.3906798750648661, + "velocityY": -1.3085192580393974, + "timestamp": 0.8791389546075529 + }, + { + "x": 2.49534860091136, + "y": 4.4564544045860375, + "heading": -0.6326127919617512, + "angularVelocity": -0.6051513095863228, + "velocityX": 1.158922739582985, + "velocityY": -1.090461313806123, + "timestamp": 0.9524005341581823 + }, + { + "x": 2.5632738362111045, + "y": 4.3925403400214185, + "heading": -0.6681173722205684, + "angularVelocity": -0.48462755617056985, + "velocityX": 0.9271603986207089, + "velocityY": -0.8724090438215177, + "timestamp": 1.0256621137088118 + }, + { + "x": 2.6142195241645787, + "y": 4.344600823178412, + "heading": -0.6947927082053813, + "angularVelocity": -0.36411084975827024, + "velocityX": 0.6953943426549579, + "velocityY": -0.6543609506791417, + "timestamp": 1.0989236932594413 + }, + { + "x": 2.6481854848919673, + "y": 4.312635645188571, + "heading": -0.7126387384821254, + "angularVelocity": -0.24359330478823768, + "velocityX": 0.4636258313802147, + "velocityY": -0.43631570853248003, + "timestamp": 1.1721852728100708 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": -0.0007900953867185666, - "velocityX": 0.00039102557193096025, - "velocityY": -0.00036545389408097024, - "timestamp": 1.393943917504123 - }, - { - "x": 2.648217264320523, - "y": 4.31259514704442, - "heading": -0.7128809139831602, - "angularVelocity": 0.1132783500941083, - "velocityX": -0.21888736985187585, - "velocityY": 0.2059266364981681, - "timestamp": 1.471400917058759 - }, - { - "x": 2.6142783804156173, - "y": 4.344524561384219, - "heading": -0.6952725965601283, - "angularVelocity": 0.2273302286961744, - "velocityX": -0.43816419561488745, - "velocityY": 0.4122211617168144, - "timestamp": 1.548857916613395 - }, - { - "x": 2.5633551628668956, - "y": 4.3924332087312665, - "heading": -0.6688313471237388, - "angularVelocity": 0.341366817566861, - "velocityX": -0.6574385509538746, - "velocityY": 0.618519276788985, - "timestamp": 1.6263149161680308 - }, - { - "x": 2.4954478733663135, - "y": 4.456321462374885, - "heading": -0.6335580214350119, - "angularVelocity": 0.45539235822983737, - "velocityX": -0.876709527747899, - "velocityY": 0.8248222111715229, - "timestamp": 1.7037719157226667 - }, - { - "x": 2.41055684840719, - "y": 4.536189798526633, - "heading": -0.5894527108517198, - "angularVelocity": 0.5694167194284366, - "velocityX": -1.0959761602822577, - "velocityY": 1.0311312936305541, - "timestamp": 1.7812289152773026 - }, - { - "x": 2.3086825106199456, - "y": 4.632038804702452, - "heading": -0.5365139521148367, - "angularVelocity": 0.6834599718703669, - "velocityX": -1.3152373364934584, - "velocityY": 1.2374479611490161, - "timestamp": 1.8586859148319386 - }, - { - "x": 2.1898253903863423, - "y": 4.743869185084351, - "heading": -0.47473734314041516, - "angularVelocity": 0.7975600569287656, - "velocityX": -1.5344916652697551, - "velocityY": 1.4437737199311238, - "timestamp": 1.9361429143865745 - }, - { - "x": 2.053986160891222, - "y": 4.871681755197305, - "heading": -0.40411323356241463, - "angularVelocity": 0.91178473196739, - "velocityX": -1.7537373029678356, - "velocityY": 1.6501100074580153, - "timestamp": 2.0135999139412104 - }, - { - "x": 1.901165693105442, - "y": 5.0154774121652865, - "heading": -0.3246230604276491, - "angularVelocity": 1.026249061953906, - "velocityX": -1.9729716960932775, - "velocityY": 1.8564578772043625, - "timestamp": 2.0910569134958465 - }, - { - "x": 1.7653423329783178, - "y": 5.143359507076652, - "heading": -0.25263854799609164, - "angularVelocity": 0.9293480620925062, - "velocityX": -1.7535324232572806, - "velocityY": 1.6510075996592735, - "timestamp": 2.1685139130504827 - }, - { - "x": 1.6464985778710517, - "y": 5.2552571877344505, - "heading": -0.18957308544395354, - "angularVelocity": 0.8141996580657658, - "velocityX": -1.5343191162933176, - "velocityY": 1.4446425926813151, - "timestamp": 2.245970912605119 - }, - { - "x": 1.5446336589350755, - "y": 5.351170031650349, - "heading": -0.135458741257284, - "angularVelocity": 0.6986372374073464, - "velocityX": -1.315115735463508, - "velocityY": 1.2382721312145724, - "timestamp": 2.323427912159755 - }, - { - "x": 1.459746934416973, - "y": 5.431097753001726, - "heading": -0.09032536640283424, - "angularVelocity": 0.5826894291541963, - "velocityX": -1.0959206399199872, - "velocityY": 1.031897979666631, - "timestamp": 2.400884911714391 - }, - { - "x": 1.391837908901594, - "y": 5.49504016165852, - "heading": -0.054198235453019356, - "angularVelocity": 0.4664153163377203, - "velocityX": -0.8767319403787999, - "velocityY": 0.8255213734655752, - "timestamp": 2.4783419112690273 - }, - { - "x": 1.3409062442336714, - "y": 5.542997125969119, - "heading": -0.027096040975571573, - "angularVelocity": 0.3498998751992024, - "velocityX": -0.6575476065536807, - "velocityY": 0.6191430675910059, - "timestamp": 2.5557989108236634 - }, - { - "x": 1.3069517635064558, - "y": 5.574968540706391, - "heading": -0.0090294167092662, - "angularVelocity": 0.2332471483558819, - "velocityX": -0.43836555666351823, - "velocityY": 0.41276340319322796, - "timestamp": 2.6332559103782995 + "angularVelocity": -0.12307101033443453, + "velocityX": 0.23185602115327572, + "velocityY": -0.2182720825031122, + "timestamp": 1.2454468523607003 + }, + { + "x": 2.662943710448623, + "y": 4.298728558024191, + "heading": -0.72070100113944, + "angularVelocity": 0.011651349233264986, + "velocityX": -0.027206592771411266, + "velocityY": 0.025447590705983056, + "timestamp": 1.3273355622067569 + }, + { + "x": 2.639501593010729, + "y": 4.320770481747221, + "heading": -0.7087160822503835, + "angularVelocity": 0.14635618159801306, + "velocityX": -0.2862680030246291, + "velocityY": 0.26916926355864296, + "timestamp": 1.4092242720528134 + }, + { + "x": 2.594845480264057, + "y": 4.36277075661854, + "heading": -0.6857017147672647, + "angularVelocity": 0.2810444507720759, + "velocityX": -0.5453268567867454, + "velocityY": 0.5128945730159264, + "timestamp": 1.49111298189887 + }, + { + "x": 2.5289756911084322, + "y": 4.424729822216665, + "heading": -0.6516589308540864, + "angularVelocity": 0.4157201154734962, + "velocityX": -0.8043818162412647, + "velocityY": 0.7566252504722968, + "timestamp": 1.5730016917449265 + }, + { + "x": 2.4418926576481637, + "y": 4.5066482714289275, + "heading": -0.6065878922273316, + "angularVelocity": 0.550393805342452, + "velocityX": -1.0634314989694753, + "velocityY": 1.000363168088272, + "timestamp": 1.654890401590983 + }, + { + "x": 2.3335969392795284, + "y": 4.608526864652899, + "heading": -0.5504868101397262, + "angularVelocity": 0.6850893388486693, + "velocityX": -1.322474350520624, + "velocityY": 1.2441103714479649, + "timestamp": 1.7367791114370397 + }, + { + "x": 2.2040892544412327, + "y": 4.730366540742225, + "heading": -0.48334990238896663, + "angularVelocity": 0.8198554828494793, + "velocityX": -1.5815084287169574, + "velocityY": 1.4878690398025962, + "timestamp": 1.8186678212830962 + }, + { + "x": 2.0533705390200705, + "y": 4.8721684079929855, + "heading": -0.4051637468918412, + "angularVelocity": 0.9547855332451622, + "velocityX": -1.840531078148622, + "velocityY": 1.7316412423316419, + "timestamp": 1.9005565311291528 + }, + { + "x": 1.8837222142414454, + "y": 5.031895792612386, + "heading": -0.31535335280985394, + "angularVelocity": 1.0967371967493782, + "velocityX": -2.071693705976669, + "velocityY": 1.9505422044097782, + "timestamp": 1.9824452409752094 + }, + { + "x": 1.7352824931464605, + "y": 5.171658722106698, + "heading": -0.2366561880408508, + "angularVelocity": 0.9610258229363577, + "velocityX": -1.8127006930996745, + "velocityY": 1.706742355045694, + "timestamp": 2.0643339508212657 + }, + { + "x": 1.6080502362813274, + "y": 5.291456457157948, + "heading": -0.16911546139257225, + "angularVelocity": 0.8247867963147685, + "velocityX": -1.5537215948855285, + "velocityY": 1.4629334773555265, + "timestamp": 2.146222660667322 + }, + { + "x": 1.5020244624691528, + "y": 5.391288489879848, + "heading": -0.11277393629567076, + "angularVelocity": 0.6880255556940454, + "velocityX": -1.294754478504957, + "velocityY": 1.219118397512621, + "timestamp": 2.2281113705133784 + }, + { + "x": 1.4172044018013137, + "y": 5.471154488105965, + "heading": -0.06766925261840594, + "angularVelocity": 0.550804668458663, + "velocityX": -1.0357967640141517, + "velocityY": 0.9752992613543129, + "timestamp": 2.3100000803594347 + }, + { + "x": 1.3535895239653573, + "y": 5.531054232218378, + "heading": -0.03382997301748837, + "angularVelocity": 0.4132349827532073, + "velocityX": -0.7768455255376048, + "velocityY": 0.7314774432888099, + "timestamp": 2.391888790205491 + }, + { + "x": 1.3111795494900627, + "y": 5.570987556283822, + "heading": -0.011272655480386088, + "angularVelocity": 0.27546309594458174, + "velocityX": -0.5178976998785506, + "velocityY": 0.48765359889678683, + "timestamp": 2.4737775000515474 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 7.540408157375041e-27, - "angularVelocity": 0.11657328273999748, - "velocityX": -0.21918370888400043, - "velocityY": 0.20638242027186274, - "timestamp": 2.7107129099329357 + "heading": 0, + "angularVelocity": 0.13765823764445279, + "velocityX": -0.2589502077253713, + "velocityY": 0.24382784263139995, + "timestamp": 2.5556662098976037 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 7.897631948719018e-27, - "angularVelocity": 4.6120160844672454e-27, - "velocityX": -1.8573844509297025e-30, - "velocityY": 4.869606321032096e-28, - "timestamp": 2.788169909487572 - }, - { - "x": 1.3279766485932036, - "y": 5.589659571345802, - "heading": -4.710153046711685e-19, - "angularVelocity": -4.772747206025981e-18, - "velocityX": 0.38507114894200895, - "velocityY": -0.01311934897441659, - "timestamp": 2.886858678128688 - }, - { - "x": 1.4039810428772954, - "y": 5.587070106580806, - "heading": -1.4132708274578583e-18, - "angularVelocity": -9.547773272073244e-18, - "velocityX": 0.7701422900595405, - "velocityY": -0.026238697682253745, - "timestamp": 2.985547446769804 - }, - { - "x": 1.5179876327590038, - "y": 5.5831859094859295, - "heading": 6.100569101954107e-18, - "angularVelocity": 7.613688998468879e-17, - "velocityX": 1.155213419439816, - "velocityY": -0.03935804599020335, - "timestamp": 3.0842362154109204 - }, - { - "x": 1.669996416307628, - "y": 5.578006980126951, - "heading": 1.3142262461800744e-17, - "angularVelocity": 7.135267989328175e-17, - "velocityX": 1.5402845292565597, - "velocityY": -0.0524773936316247, - "timestamp": 3.1829249840520366 - }, - { - "x": 1.8600073896612332, - "y": 5.571533318635448, - "heading": 1.9239838663580868e-17, - "angularVelocity": 6.178604539434395e-17, - "velocityX": 1.925355599940842, - "velocityY": -0.06559673993980564, - "timestamp": 3.281613752693153 - }, - { - "x": 2.0880205412297537, - "y": 5.563764925406294, - "heading": 2.4860743738508815e-17, - "angularVelocity": 5.695598852986624e-17, - "velocityX": 2.310426553184545, - "velocityY": -0.07871608224679369, - "timestamp": 3.380302521334269 - }, - { - "x": 2.2780734489522008, - "y": 5.557289835213575, - "heading": 1.9389925148359925e-17, - "angularVelocity": -5.54351783373119e-17, - "velocityX": 1.9257805152317913, - "velocityY": -0.06561121677610336, - "timestamp": 3.478991289975385 - }, - { - "x": 2.430124178434165, - "y": 5.55210947675938, - "heading": 1.4382200259206915e-17, - "angularVelocity": -5.074270981417207e-17, - "velocityX": 1.5407095617290139, - "velocityY": -0.05249187446028865, - "timestamp": 3.5776800586165014 - }, - { - "x": 2.544172718104756, - "y": 5.548223850437927, - "heading": 9.840962526531424e-18, - "angularVelocity": -4.6015845846918525e-17, - "velocityX": 1.155638490979962, - "velocityY": -0.03937252814990102, - "timestamp": 3.6763688272576176 - }, - { - "x": 2.6202190641055907, - "y": 5.545632956380672, - "heading": 6.0923440108598725e-18, - "angularVelocity": -3.798432272839268e-17, - "velocityX": 0.7705673811344316, - "velocityY": -0.026253180507498905, - "timestamp": 3.775057595898734 - }, - { - "x": 2.6582632145072105, - "y": 5.5443367946533515, - "heading": 2.8119102345957467e-18, - "angularVelocity": -3.324026254038647e-17, - "velocityX": 0.38549625173796304, - "velocityY": -0.013133832198997603, - "timestamp": 3.87374636453985 + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.169901203643428e-30, + "velocityY": 4.744348570353821e-29, + "timestamp": 2.63755491974366 + }, + { + "x": 1.3279764392732873, + "y": 5.589659578477318, + "heading": -3.8286967101155775e-19, + "angularVelocity": -4.101142681562708e-18, + "velocityX": 0.40706315074964167, + "velocityY": -0.013868615044214471, + "timestamp": 2.7309114128077048 + }, + { + "x": 1.4039804148734387, + "y": 5.5870701279768555, + "heading": -4.0712541473530154e-19, + "angularVelocity": -2.5982712070316094e-19, + "velocityX": 0.814126292755424, + "velocityY": -0.027737229790526226, + "timestamp": 2.8242679058717495 + }, + { + "x": 1.517986376641016, + "y": 5.583185952281787, + "heading": 4.833998363566258e-19, + "angularVelocity": 9.538831821902836e-18, + "velocityX": 1.2211894216447965, + "velocityY": -0.04160584408996275, + "timestamp": 2.917624398935794 + }, + { + "x": 1.6699943225350276, + "y": 5.578007051461648, + "heading": 1.3619244496265011e-18, + "angularVelocity": 9.410228943340493e-18, + "velocityX": 1.6282525286718303, + "velocityY": -0.05547445764455082, + "timestamp": 3.010980891999839 + }, + { + "x": 1.8600042484729113, + "y": 5.571533425655531, + "heading": 3.711758987250246e-18, + "angularVelocity": 2.5170201581763002e-17, + "velocityX": 2.0353155919679775, + "velocityY": -0.06934306970923045, + "timestamp": 3.1043373850638836 + }, + { + "x": 2.0880161422023416, + "y": 5.563765075280871, + "heading": -9.287085834288012e-18, + "angularVelocity": -1.392382283532996e-16, + "velocityX": 2.4423785240217755, + "velocityY": -0.08321167730249175, + "timestamp": 3.1976938781279283 + }, + { + "x": 2.2780692584306133, + "y": 5.5572899779843725, + "heading": -5.547928961434018e-18, + "angularVelocity": 4.0052703333031833e-17, + "velocityX": 2.03577823010496, + "velocityY": -0.06935883176043627, + "timestamp": 3.291050371191973 + }, + { + "x": 2.430120406839514, + "y": 5.552109605257364, + "heading": -3.2776580295073633e-18, + "angularVelocity": 2.4318713794052425e-17, + "velocityX": 1.6287152977531212, + "velocityY": -0.05549022415702071, + "timestamp": 3.3844068642560177 + }, + { + "x": 2.5441695751975866, + "y": 5.548223957516569, + "heading": -1.9211838299437728e-18, + "angularVelocity": 1.4530579976060665e-17, + "velocityX": 1.2216522343824612, + "velocityY": -0.04162161208980244, + "timestamp": 3.4777633573200624 + }, + { + "x": 2.6202167594261327, + "y": 5.545633034900951, + "heading": -1.8132356413213059e-19, + "angularVelocity": 1.8636270440618356e-17, + "velocityX": 0.8145891273223099, + "velocityY": -0.02775299853408606, + "timestamp": 3.571119850384107 + }, + { + "x": 2.6582619574855144, + "y": 5.5443368374799995, + "heading": 8.843883513334677e-20, + "angularVelocity": 2.8892814546014257e-18, + "velocityX": 0.40752599841430864, + "velocityY": -0.013884384234014852, + "timestamp": 3.6644763434481518 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": 3.967855365558804e-35, - "angularVelocity": -2.8492764136925344e-17, - "velocityX": 0.0004251106100680654, - "velocityY": -0.000014483490807365214, - "timestamp": 3.972435133180966 - }, - { - "x": 2.6203309461495814, - "y": 5.545629144566718, - "heading": -2.3435066939433916e-18, - "angularVelocity": -2.3742025219812006e-17, - "velocityX": -0.3847168901074824, - "velocityY": 0.013107279398989128, - "timestamp": 4.0711420602145845 - }, - { - "x": 2.544340541557253, - "y": 5.548218132703827, - "heading": -4.218037035355937e-18, - "angularVelocity": -1.899082968522677e-17, - "velocityX": -0.769858883001995, - "velocityY": 0.02622904202225523, - "timestamp": 4.169848987248202 - }, - { - "x": 2.4303339555332055, - "y": 5.552102329667273, - "heading": -5.623520937192281e-18, - "angularVelocity": -1.4238932090618574e-17, - "velocityX": -1.1550008641614116, - "velocityY": 0.039350804245707374, - "timestamp": 4.26855591428182 - }, - { - "x": 2.27831119000814, - "y": 5.557281735391277, - "heading": -6.559843392694925e-18, - "angularVelocity": -9.485869363616628e-18, - "velocityX": -1.5401428257608962, - "velocityY": 0.052472565802753904, - "timestamp": 4.367262841315438 - }, - { - "x": 2.0882722488439907, - "y": 5.563756349744264, - "heading": -7.026773676733008e-18, - "angularVelocity": -4.730469162706724e-18, - "velocityX": -1.9252847482351219, - "velocityY": 0.06559432602680541, - "timestamp": 4.465969768349056 - }, - { - "x": 1.8602171436308217, - "y": 5.571526172331361, - "heading": -7.023625037813593e-18, - "angularVelocity": 3.1898506726958296e-20, - "velocityX": -2.3104265532903905, - "velocityY": 0.07871608225040072, - "timestamp": 4.564676695382674 - }, - { - "x": 1.6701362603825125, - "y": 5.578002215648425, - "heading": -4.6832619459775276e-18, - "angularVelocity": 2.3710179076002214e-17, - "velocityX": -1.9257096635619557, - "velocityY": 0.06560880286432572, - "timestamp": 4.6633836224162915 - }, - { - "x": 1.5180715412087855, - "y": 5.583183050730508, - "heading": -2.810229256311612e-18, - "angularVelocity": 1.8975666640147773e-17, - "velocityX": -1.5405678582477078, - "velocityY": 0.05248704663190702, - "timestamp": 4.762090549449909 - }, - { - "x": 1.4040229976805338, - "y": 5.587068677183391, - "heading": -1.405203585438858e-18, - "angularVelocity": 1.4234289424631298e-17, - "velocityX": -1.1554259357087366, - "velocityY": 0.03936528640564963, - "timestamp": 4.860797476483527 - }, - { - "x": 1.3279906336561405, - "y": 5.589659094875619, - "heading": -4.684238337733874e-19, - "angularVelocity": 9.490501893267791e-18, - "velocityX": -0.7702839740804757, - "velocityY": 0.02624352484762269, - "timestamp": 4.959504403517145 + "heading": 1.7598923552723236e-34, + "angularVelocity": -9.476131898054967e-19, + "velocityX": 0.0004628563966037854, + "velocityY": -0.000015769487296898133, + "timestamp": 3.7578328365121965 + }, + { + "x": 2.620331993851312, + "y": 5.545629108871589, + "heading": 2.945836613273263e-19, + "angularVelocity": 3.1551525697239727e-18, + "velocityX": -0.4066774370940851, + "velocityY": 0.013855473805082683, + "timestamp": 3.8512070216080794 + }, + { + "x": 2.5443424272440374, + "y": 5.548218068458602, + "heading": 2.309166242357916e-19, + "angularVelocity": -6.815922301609038e-19, + "velocityX": -0.8138177218425701, + "velocityY": 0.027726716799615945, + "timestamp": 3.9445812067039623 + }, + { + "x": 2.4303364695545335, + "y": 5.552102244014731, + "heading": -7.469284722560017e-19, + "angularVelocity": -1.047218864069531e-17, + "velocityX": -1.220957993477128, + "velocityY": 0.041597959347358555, + "timestamp": 4.037955391799845 + }, + { + "x": 2.278314122823792, + "y": 5.55728163547044, + "heading": -1.7117540142209262e-18, + "angularVelocity": -1.0332835908519937e-17, + "velocityX": -1.628098243253484, + "velocityY": 0.055469201150393666, + "timestamp": 4.131329576895728 + }, + { + "x": 2.0882753911343768, + "y": 5.563756242686637, + "heading": -3.8779782019319956e-18, + "angularVelocity": -2.3199468985970604e-17, + "velocityX": -2.035238449307225, + "velocityY": 0.06934044146380212, + "timestamp": 4.22470376199161 + }, + { + "x": 1.8602202867386202, + "y": 5.571526065245884, + "heading": -6.955816575429636e-18, + "angularVelocity": -3.296264638305206e-17, + "velocityX": -2.4423785241434, + "velocityY": 0.08321167730663664, + "timestamp": 4.318077947087493 + }, + { + "x": 1.6701383566026529, + "y": 5.578002144230341, + "heading": -1.819668166700424e-18, + "angularVelocity": 5.5006116981871155e-17, + "velocityX": -2.035701087485777, + "velocityY": 0.0693562035164242, + "timestamp": 4.411452132183375 + }, + { + "x": 1.518072799200871, + "y": 5.5831830078708, + "heading": 4.234959937878501e-18, + "angularVelocity": 6.484281229150569e-17, + "velocityX": -1.6285610123513967, + "velocityY": 0.05548496766342984, + "timestamp": 4.5048263172792575 + }, + { + "x": 1.4040236267647241, + "y": 5.587068655750534, + "heading": 2.524941481520431e-18, + "angularVelocity": -1.831387557299539e-17, + "velocityX": -1.221420806223103, + "velocityY": 0.041613727347481366, + "timestamp": 4.59820050237514 + }, + { + "x": 1.327990843372909, + "y": 5.589659087730582, + "heading": 8.045224971714285e-19, + "angularVelocity": -1.842525382897137e-17, + "velocityX": -0.814280556413611, + "velocityY": 0.027742485543317336, + "timestamp": 4.6915746874710225 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 3.33815545782367e-31, - "angularVelocity": 4.745589981208042e-18, - "velocityX": -0.3851419929048723, - "velocityY": 0.013121762623619058, - "timestamp": 5.058211330550763 + "heading": 6.74699155537553e-28, + "angularVelocity": -8.616220285509517e-18, + "velocityX": -0.40714028476041414, + "velocityY": 0.013871242994939689, + "timestamp": 4.784948872566905 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 2.0232376930557863e-31, - "angularVelocity": -1.3321178776125106e-30, - "velocityX": -6.421692970708144e-31, - "velocityY": -1.2392296625905867e-30, - "timestamp": 5.156918257584381 - }, - { - "x": 1.3044225360616175, - "y": 5.603754712691982, - "heading": 0.008101518995482268, - "angularVelocity": 0.11491508578445629, - "velocityX": 0.20493723803226965, - "velocityY": 0.18156596231474714, - "timestamp": 5.227418304990499 - }, - { - "x": 1.3333187786755418, - "y": 5.6293552812892855, - "heading": 0.024302645459921106, - "angularVelocity": 0.22980305773571622, - "velocityX": 0.40987550614635165, - "velocityY": 0.36312838840846096, - "timestamp": 5.297918352396617 - }, - { - "x": 1.3766634045969632, - "y": 5.667755708974662, - "heading": 0.048595399874496285, - "angularVelocity": 0.34457784509896805, - "velocityX": 0.6148169755366727, - "velocityY": 0.5446865512610124, - "timestamp": 5.368418399802735 - }, - { - "x": 1.434456789320205, - "y": 5.7189556181427035, - "heading": 0.08096686680516672, - "angularVelocity": 0.45916943493943385, - "velocityX": 0.8197637710840284, - "velocityY": 0.7262393580121056, - "timestamp": 5.438918447208853 - }, - { - "x": 1.5066994506601286, - "y": 5.782954524001268, - "heading": 0.12140070801121562, - "angularVelocity": 0.5735292768404597, - "velocityX": 1.0247179115180916, - "velocityY": 0.9077852882835168, - "timestamp": 5.509418494614971 - }, - { - "x": 1.59339203880409, - "y": 5.859751802427408, - "heading": 0.16987894409259566, - "angularVelocity": 0.6876340919619466, - "velocityX": 1.2296812744616465, - "velocityY": 1.0893223657531423, - "timestamp": 5.5799185420210895 - }, - { - "x": 1.6945353339175246, - "y": 5.949346653618572, - "heading": 0.22638374394908448, - "angularVelocity": 0.8014859838460967, - "velocityX": 1.4346557035741434, - "velocityY": 1.2708480985133195, - "timestamp": 5.650418589427208 - }, - { - "x": 1.8101302937852244, - "y": 6.051738034585355, - "heading": 0.290898605978136, - "angularVelocity": 0.9151038106032966, - "velocityX": 1.6396437182773986, - "velocityY": 1.4523590371075072, - "timestamp": 5.720918636833326 - }, - { - "x": 1.940178425831269, - "y": 6.166924341281603, - "heading": 0.36340632675450085, - "angularVelocity": 1.0284776173082797, - "velocityX": 1.8446531148681091, - "velocityY": 1.6338472232892616, - "timestamp": 5.791418684239444 - }, - { - "x": 2.0847170007855893, - "y": 6.294873766483704, - "heading": 0.443594985050904, - "angularVelocity": 1.1374270124170804, - "velocityX": 2.0501911739392336, - "velocityY": 1.8148842434820598, - "timestamp": 5.861918731645562 - }, - { - "x": 2.2148049129108704, - "y": 6.410027050354294, - "heading": 0.5157685134494568, - "angularVelocity": 1.0237373030800216, - "velocityX": 1.8452173709317587, - "velocityY": 1.6333788147296693, - "timestamp": 5.93241877905168 - }, - { - "x": 2.3304407554674365, - "y": 6.512385621001077, - "heading": 0.5799353875370489, - "angularVelocity": 0.9101678147527572, - "velocityX": 1.6402236141834323, - "velocityY": 1.4518936428105123, - "timestamp": 6.002918826457798 - }, - { - "x": 2.431623857347168, - "y": 6.601950249920576, - "heading": 0.6360978126199414, - "angularVelocity": 0.796629607344333, - "velocityX": 1.435220338177405, - "velocityY": 1.2704194140971152, - "timestamp": 6.0734188738639165 - }, - { - "x": 2.518353756343433, - "y": 6.678721476402507, - "heading": 0.6842565298983911, - "angularVelocity": 0.683101913407659, - "velocityX": 1.2302105060533437, - "velocityY": 1.088952834878071, - "timestamp": 6.143918921270035 - }, - { - "x": 2.5906300952596166, - "y": 6.742699702654635, - "heading": 0.7244118519902635, - "angularVelocity": 0.5695786537639655, - "velocityX": 1.0251956073140427, - "velocityY": 0.907491960729887, - "timestamp": 6.214418968676153 - }, - { - "x": 2.648452591103255, - "y": 6.7938852314203, - "heading": 0.7565639740344317, - "angularVelocity": 0.45605816204569094, - "velocityX": 0.820176694499928, - "velocityY": 0.7260353808105814, - "timestamp": 6.284919016082271 - }, - { - "x": 2.6918210260454423, - "y": 6.832278283879232, - "heading": 0.7807130811975228, - "angularVelocity": 0.34254029680263026, - "velocityX": 0.6151546919161919, - "velocityY": 0.5445819381903092, - "timestamp": 6.355419063488389 - }, - { - "x": 2.720735245788176, - "y": 6.85787900840029, - "heading": 0.7968594022841035, - "angularVelocity": 0.22902567701223253, - "velocityX": 0.4101305007097693, - "velocityY": 0.3631306000914257, - "timestamp": 6.425919110894507 + "heading": 0, + "angularVelocity": -7.225786160817796e-27, + "velocityX": -2.979354413794432e-29, + "velocityY": 7.541641997510845e-28, + "timestamp": 4.878323057662787 + }, + { + "x": 1.3078091735186845, + "y": 5.606755773559667, + "heading": 0.009999668662888749, + "angularVelocity": 0.1349541802858926, + "velocityX": 0.24069501005494717, + "velocityY": 0.21325450657673967, + "timestamp": 4.952419826226246 + }, + { + "x": 1.3434786990122702, + "y": 5.638358364996936, + "heading": 0.029996210094567113, + "angularVelocity": 0.2698706275619814, + "velocityX": 0.48139110769232624, + "velocityY": 0.42650431390462895, + "timestamp": 5.0265165947897055 + }, + { + "x": 1.3969833303762573, + "y": 5.6857616519517675, + "heading": 0.05997803368561842, + "angularVelocity": 0.4046306495359042, + "velocityX": 0.7220912922884387, + "velocityY": 0.6397483706242104, + "timestamp": 5.1006133633531645 + }, + { + "x": 1.468323578276302, + "y": 5.748965089703653, + "heading": 0.09992693775422122, + "angularVelocity": 0.5391450240657001, + "velocityX": 0.9627983687594002, + "velocityY": 0.8529850758692316, + "timestamp": 5.1747101319166235 + }, + { + "x": 1.5575001323238937, + "y": 5.827967973754339, + "heading": 0.14982100646683655, + "angularVelocity": 0.6733636254605087, + "velocityX": 1.203514752134039, + "velocityY": 1.0662122733008166, + "timestamp": 5.248806900480083 + }, + { + "x": 1.6645138155479717, + "y": 5.922769410023865, + "heading": 0.20963796260623796, + "angularVelocity": 0.8072815765348765, + "velocityX": 1.444242242996271, + "velocityY": 1.279427404349801, + "timestamp": 5.322903669043542 + }, + { + "x": 1.789365525357909, + "y": 6.033368309242042, + "heading": 0.27935847839394334, + "angularVelocity": 0.9409386825166515, + "velocityX": 1.6849818451743819, + "velocityY": 1.4926278347551774, + "timestamp": 5.397000437607001 + }, + { + "x": 1.9320561732696155, + "y": 6.159763414266322, + "heading": 0.35896870362448313, + "angularVelocity": 1.074408867993172, + "velocityX": 1.925733749048249, + "velocityY": 1.7058112989342795, + "timestamp": 5.47109720617046 + }, + { + "x": 2.092586653477802, + "y": 6.301953347709479, + "heading": 0.44846110031344777, + "angularVelocity": 1.2077773218503376, + "velocityX": 2.1664977208531133, + "velocityY": 1.9189761740812865, + "timestamp": 5.545193974733919 + }, + { + "x": 2.2353547101625333, + "y": 6.428313752108457, + "heading": 0.5276057235865483, + "angularVelocity": 1.068125166619866, + "velocityX": 1.9267784472209708, + "velocityY": 1.7053429837972895, + "timestamp": 5.619290743297378 + }, + { + "x": 2.360281889838346, + "y": 6.538881119368732, + "heading": 0.5968684037316069, + "angularVelocity": 0.9347597942868423, + "velocityX": 1.686000376116786, + "velocityY": 1.4922022835269577, + "timestamp": 5.693387511860837 + }, + { + "x": 2.4673676751793194, + "y": 6.633656126085495, + "heading": 0.6562491552260078, + "angularVelocity": 0.8013946173366472, + "velocityX": 1.4452153232519684, + "velocityY": 1.279070714493898, + "timestamp": 5.767484280424296 + }, + { + "x": 2.5566116515465542, + "y": 6.712639287159663, + "heading": 0.7057476703680625, + "angularVelocity": 0.668025287685273, + "velocityX": 1.2044246744765545, + "velocityY": 1.0659460944941062, + "timestamp": 5.841581048987755 + }, + { + "x": 2.6280134773537864, + "y": 6.7758309931109295, + "heading": 0.7453637127245453, + "angularVelocity": 0.5346527671626246, + "velocityX": 0.9636294156990158, + "velocityY": 0.8528267448954308, + "timestamp": 5.915677817551214 + }, + { + "x": 2.6815728846604703, + "y": 6.823231527897903, + "heading": 0.7750971751130776, + "angularVelocity": 0.4012788001208449, + "velocityX": 0.7228305408320154, + "velocityY": 0.6397112275638166, + "timestamp": 5.989774586114673 + }, + { + "x": 2.7172896860156874, + "y": 6.854841077120622, + "heading": 0.7949480902508361, + "angularVelocity": 0.26790527462553493, + "velocityX": 0.4820291361527109, + "velocityY": 0.42659821506532714, + "timestamp": 6.063871354678132 + }, + { + "x": 2.7351637810076403, + "y": 6.870659730936303, + "heading": 0.8049166539682622, + "angularVelocity": 0.13453439203137166, + "velocityX": 0.2412263763293308, + "velocityY": 0.2134864193164304, + "timestamp": 6.137968123241591 }, { "x": 2.73519515991211, "y": 6.870687484741211, "heading": 0.8050032485420815, - "angularVelocity": 0.11551547208280752, - "velocityX": 0.2051050269602972, - "velocityY": 0.18168039330721092, - "timestamp": 6.496419158300625 - }, - { - "x": 2.733517887090967, - "y": 6.869215556772689, - "heading": 0.8042279993622282, - "angularVelocity": -0.009948237722316289, - "velocityX": -0.02152328462065399, - "velocityY": -0.018888235836320173, - "timestamp": 6.5743474508779345 - }, - { - "x": 2.714179970265771, - "y": 6.85211357425893, - "heading": 0.7936748330656245, - "angularVelocity": -0.13542150029957753, - "velocityX": -0.24815014143946865, - "velocityY": -0.21945793944853148, - "timestamp": 6.6522757434552435 - }, - { - "x": 2.677181604439607, - "y": 6.819381358340049, - "heading": 0.7733432924791085, - "angularVelocity": -0.26090062946453, - "velocityX": -0.47477449591828125, - "velocityY": -0.4200299382462307, - "timestamp": 6.730204036032553 - }, - { - "x": 2.6225230574768488, - "y": 6.771018630930062, - "heading": 0.7432332740659877, - "angularVelocity": -0.3863810872444029, - "velocityX": -0.7013954130784187, - "velocityY": -0.6206055055294315, - "timestamp": 6.808132328609862 - }, - { - "x": 2.5502046603277093, - "y": 6.707025005806598, - "heading": 0.7033451105326386, - "angularVelocity": -0.5118572756329512, - "velocityX": -0.9280120833828756, - "velocityY": -0.8211860289378066, - "timestamp": 6.886060621187171 - }, - { - "x": 2.4602268003518986, - "y": 6.627399971822084, - "heading": 0.653679622983667, - "angularVelocity": -0.6373229273527391, - "velocityX": -1.1546237829674975, - "velocityY": -1.0217731115502302, - "timestamp": 6.96398891376448 - }, - { - "x": 2.3525899246520714, - "y": 6.53214286652055, - "heading": 0.5942379589890165, - "angularVelocity": -0.7627738530993845, - "velocityX": -1.3812297451922877, - "velocityY": -1.2223686950029513, - "timestamp": 7.041917206341789 - }, - { - "x": 2.22729456327189, - "y": 6.421252842872556, - "heading": 0.5250208143201779, - "angularVelocity": -0.8882158504906453, - "velocityX": -1.6078289057325443, - "velocityY": -1.4229751477999506, - "timestamp": 7.119845498919098 - }, - { - "x": 2.0843413856896924, - "y": 6.294728842395834, - "heading": 0.4460263730973391, - "angularVelocity": -1.0136811498143847, - "velocityX": -1.8344194753193157, - "velocityY": -1.623595183369405, - "timestamp": 7.197773791496407 - }, - { - "x": 1.925462021024861, - "y": 6.1539791801359245, - "heading": 0.3570280981408959, - "angularVelocity": -1.1420534444297203, - "velocityX": -2.038789243421619, - "velocityY": -1.8061432838436418, - "timestamp": 7.275702084073716 - }, - { - "x": 1.7842386496681704, - "y": 6.028865963268039, - "heading": 0.2778322483955418, - "angularVelocity": -1.016265686390949, - "velocityX": -1.8122220657739765, - "velocityY": -1.6054915709049453, - "timestamp": 7.353630376651025 - }, - { - "x": 1.6606703031584165, - "y": 5.9193901868323495, - "heading": 0.2084629069388522, - "angularVelocity": -0.8901688868374888, - "velocityX": -1.5856673157206287, - "velocityY": -1.4048270893024366, - "timestamp": 7.431558669228334 - }, - { - "x": 1.554756060685191, - "y": 5.825552639839227, - "heading": 0.14894782916930657, - "angularVelocity": -0.7637159214094572, - "velocityX": -1.3591243817917928, - "velocityY": -1.2041524828743098, - "timestamp": 7.509486961805643 - }, - { - "x": 1.4664951152377614, - "y": 5.747353916497971, - "heading": 0.09931505664018385, - "angularVelocity": -0.6369031180797928, - "velocityX": -1.1325918036747624, - "velocityY": -1.0034702513683131, - "timestamp": 7.587415254382952 - }, - { - "x": 1.3958868227418013, - "y": 5.684794447832575, - "heading": 0.05958948406530847, - "angularVelocity": -0.5097708580675663, - "velocityX": -0.9060674905190907, - "velocityY": -0.8027824888288644, - "timestamp": 7.665343546960261 - }, - { - "x": 1.3429307344840264, - "y": 5.637874543820294, - "heading": 0.029789872263071265, - "angularVelocity": -0.38239785341985083, - "velocityX": -0.6795489353913313, - "velocityY": -0.6020907485652136, - "timestamp": 7.74327183953757 - }, - { - "x": 1.3076266152194531, - "y": 5.606594434817311, - "heading": 0.009926565476430555, - "angularVelocity": -0.25489210824085684, - "velocityX": -0.4530333990001086, - "velocityY": -0.4013960522996289, - "timestamp": 7.821200132114879 + "angularVelocity": 0.0011686685922978992, + "velocityX": 0.00042348526922062564, + "velocityY": 0.00037456143614395893, + "timestamp": 6.21206489180505 + }, + { + "x": 2.71737666825008, + "y": 6.854917835875481, + "heading": 0.7952044521096777, + "angularVelocity": -0.13221640762197384, + "velocityX": -0.2404271762292292, + "velocityY": -0.21278187956754013, + "timestamp": 6.286176695336588 + }, + { + "x": 2.6817084896089662, + "y": 6.823350601971179, + "heading": 0.775519887263024, + "angularVelocity": -0.2656063393700981, + "velocityX": -0.4812752750383339, + "velocityY": -0.4259407056452727, + "timestamp": 6.360288498868125 + }, + { + "x": 2.628190888379515, + "y": 6.7759855045171085, + "heading": 0.7459495915696174, + "angularVelocity": -0.398995764215558, + "velocityX": -0.7221198063894422, + "velocityY": -0.6391032897100266, + "timestamp": 6.434400302399663 + }, + { + "x": 2.5568241932220688, + "y": 6.712822154407591, + "heading": 0.7064940900377142, + "angularVelocity": -0.532378105058251, + "velocityX": -0.9629599030846991, + "velocityY": -0.8522711240272193, + "timestamp": 6.5085121059312 + }, + { + "x": 2.467608789693866, + "y": 6.633860035088962, + "heading": 0.657154460511053, + "angularVelocity": -0.6657459024188197, + "velocityX": -1.2037947974070313, + "velocityY": -1.0654459282687603, + "timestamp": 6.582623909462738 + }, + { + "x": 2.3605451226537615, + "y": 6.539098475645005, + "heading": 0.597932194997477, + "angularVelocity": -0.7990935679285951, + "velocityX": -1.4446236891921902, + "velocityY": -1.2786297852682258, + "timestamp": 6.6567357129942755 + }, + { + "x": 2.2356337183339816, + "y": 6.428536615850716, + "heading": 0.528828452368811, + "angularVelocity": -0.9324255966349123, + "velocityX": -1.685445480458886, + "velocityY": -1.4918252493931579, + "timestamp": 6.730847516525813 + }, + { + "x": 2.092875237629583, + "y": 6.30217337339218, + "heading": 0.4498420161681241, + "angularVelocity": -1.0657740391526649, + "velocityX": -1.9262583541843843, + "velocityY": -1.7050353173174484, + "timestamp": 6.804959320057351 + }, + { + "x": 1.9322705754719112, + "y": 6.16000743597858, + "heading": 0.3609649957464903, + "angularVelocity": -1.1992289512105054, + "velocityX": -2.167059152487097, + "velocityY": -1.9182630921487032, + "timestamp": 6.879071123588888 + }, + { + "x": 1.789533871896463, + "y": 6.033554554877999, + "heading": 0.28090803782331497, + "angularVelocity": -1.0802187250553423, + "velocityX": -1.9259645128083558, + "velocityY": -1.7062448231010843, + "timestamp": 6.953182927120426 + }, + { + "x": 1.6646414904257845, + "y": 5.922906567233019, + "heading": 0.2107799682947157, + "angularVelocity": -0.9462469699444712, + "velocityX": -1.6851888029309197, + "velocityY": -1.4929873836216447, + "timestamp": 7.027294730651963 + }, + { + "x": 1.5575924667954766, + "y": 5.828064283268442, + "heading": 0.15061002754742392, + "angularVelocity": -0.8118806705635195, + "velocityX": -1.4444261039959587, + "velocityY": -1.2797190116759578, + "timestamp": 7.101406534183501 + }, + { + "x": 1.4683859542310973, + "y": 5.7490283016453585, + "heading": 0.10042779042808991, + "angularVelocity": -0.6771153140084615, + "velocityX": -1.203674830696421, + "velocityY": -1.066442561920003, + "timestamp": 7.175518337715038 + }, + { + "x": 1.3970212709065528, + "y": 5.685799049936121, + "heading": 0.06025974538395147, + "angularVelocity": -0.5419925454610606, + "velocityX": -0.9629327573631904, + "velocityY": -0.8531603428758896, + "timestamp": 7.249630141246576 + }, + { + "x": 1.343497933013845, + "y": 5.638376829349959, + "heading": 0.03012622810608221, + "angularVelocity": -0.40659538484416025, + "velocityX": -0.7221972121128929, + "velocityY": -0.639874059577299, + "timestamp": 7.323741944778114 + }, + { + "x": 1.3078156736079332, + "y": 5.606761857880495, + "heading": 0.010039060501896319, + "angularVelocity": -0.2710387097248632, + "velocityX": -0.4814652687860333, + "velocityY": -0.4265848348630388, + "timestamp": 7.397853748309651 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -0.12738076439417462, - "velocityX": -0.2265180407600671, - "velocityY": -0.20069900877579375, - "timestamp": 7.899128424692188 + "heading": -5.584258539451151e-28, + "angularVelocity": -0.1354583213971952, + "velocityX": -0.24073388709378457, + "velocityY": -0.21329334041956155, + "timestamp": 7.471965551841189 }, { "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": 0, - "velocityX": 1.105033972502462e-31, - "velocityY": 4.1576254509102287e-32, - "timestamp": 7.977056717269497 - }, - { - "x": 1.3079814373562102, - "y": 5.591183871815198, - "heading": -3.6007722166496235e-18, - "angularVelocity": -5.30176004915721e-17, - "velocityX": 0.26513400990373515, - "velocityY": 0.0033801494016308404, - "timestamp": 8.044973263689492 - }, - { - "x": 1.343996733723379, - "y": 5.591342598530086, - "heading": -8.55205935993227e-18, - "angularVelocity": -7.290251850967679e-17, - "velocityX": 0.5302875111530345, - "velocityY": 0.0023370846024297455, - "timestamp": 8.112889810109488 - }, - { - "x": 1.398015752012773, - "y": 5.591088723884006, - "heading": -1.2570686122392014e-17, - "angularVelocity": -5.917007079271892e-17, - "velocityX": 0.7953734566439974, - "velocityY": -0.003738038216918613, - "timestamp": 8.180806356529484 - }, - { - "x": 1.4700252438968529, - "y": 5.590030427465675, - "heading": -1.5707006622298704e-17, - "angularVelocity": -4.617903467384599e-17, - "velocityX": 1.0602643343902365, - "velocityY": -0.015582306140636595, - "timestamp": 8.24872290294948 - }, - { - "x": 1.5599992284903603, - "y": 5.587714662715152, - "heading": -1.765002346502037e-17, - "angularVelocity": -2.8608887247798064e-17, - "velocityX": 1.3247726708173502, - "velocityY": -0.03409721007015733, - "timestamp": 8.316639449369475 - }, - { - "x": 1.6678928587399022, - "y": 5.583612697159567, - "heading": -1.9324789020346336e-17, - "angularVelocity": -2.4659167675098536e-17, - "velocityX": 1.5886206813628097, - "velocityY": -0.060397145788562055, - "timestamp": 8.384555995789471 - }, - { - "x": 1.7936330139942434, - "y": 5.577101220041518, - "heading": -2.0699583000572945e-17, - "angularVelocity": -2.024240007004298e-17, - "velocityX": 1.8513920669164408, - "velocityY": -0.09587467946002654, - "timestamp": 8.452472542209467 - }, - { - "x": 1.9371035847765512, - "y": 5.567437513465235, - "heading": -2.4072402865280595e-17, - "angularVelocity": -4.9661238919294396e-17, - "velocityX": 2.112453862054281, - "velocityY": -0.1422879561119276, - "timestamp": 8.520389088629463 - }, - { - "x": 2.098121982759825, - "y": 5.553726841653449, - "heading": -2.991372900129062e-17, - "angularVelocity": -8.600740883406488e-17, - "velocityX": 2.370827235347597, - "velocityY": -0.20187527980293163, - "timestamp": 8.588305635049458 - }, - { - "x": 2.276400921296343, - "y": 5.534880229318232, - "heading": -3.3606634340190096e-17, - "angularVelocity": -5.437416182505347e-17, - "velocityX": 2.624970613700564, - "velocityY": -0.2774966238517093, - "timestamp": 8.656222181469454 - }, - { - "x": 2.471485361845495, - "y": 5.509562201282429, - "heading": -3.628143679663613e-17, - "angularVelocity": -3.938366258883085e-17, - "velocityX": 2.8724140262190536, - "velocityY": -0.37278144090596593, - "timestamp": 8.72413872788945 - }, - { - "x": 2.682648461637503, - "y": 5.476133636589416, - "heading": -3.7916749117831765e-17, - "angularVelocity": -2.4078260416782975e-17, - "velocityX": 3.1091554403573096, - "velocityY": -0.4922005969839973, - "timestamp": 8.792055274309446 - }, - { - "x": 2.9087251812985304, - "y": 5.4326111135896715, - "heading": -3.8585043209768917e-17, - "angularVelocity": -9.83992917098953e-18, - "velocityX": 3.328742870153774, - "velocityY": -0.6408235591162347, - "timestamp": 8.859971820729442 - }, - { - "x": 3.1478726960604213, - "y": 5.376700760732656, - "heading": -3.7931961364229683e-17, - "angularVelocity": 9.615948273285425e-18, - "velocityX": 3.5211966356917612, - "velocityY": -0.8232213768831518, - "timestamp": 8.927888367149437 - }, - { - "x": 3.397314191935591, - "y": 5.306014145470732, - "heading": -3.37531662670372e-17, - "angularVelocity": 6.152838186169992e-17, - "velocityX": 3.6727647241163455, - "velocityY": -1.0407863619094957, - "timestamp": 8.995804913569433 - }, - { - "x": 3.6532783223184353, - "y": 5.218541845963183, - "heading": -2.785799913755649e-17, - "angularVelocity": 8.680016148087689e-17, - "velocityX": 3.7688036844506647, - "velocityY": -1.287937978568902, - "timestamp": 9.063721459989429 - }, - { - "x": 3.911405346241733, - "y": 5.11319146350464, - "heading": -1.864507789949357e-17, - "angularVelocity": 1.3565061404938756e-16, - "velocityX": 3.8006500260929266, - "velocityY": -1.5511740218217003, - "timestamp": 9.131638006409425 + "angularVelocity": 7.53493083126559e-27, + "velocityX": -3.871179527516382e-30, + "velocityY": -3.125557960458497e-29, + "timestamp": 7.546077355372726 + }, + { + "x": 1.310726436274015, + "y": 5.58769762128994, + "heading": -1.5579970000423894e-18, + "angularVelocity": -2.2453280466561467e-17, + "velocityX": 0.299069985980355, + "velocityY": -0.04693411089639644, + "timestamp": 7.615465746658217 + }, + { + "x": 1.3522304064004054, + "y": 5.581184256432646, + "heading": -2.2539666819865276e-18, + "angularVelocity": -1.003005957705595e-17, + "velocityX": 0.598139967759544, + "velocityY": -0.09386822113363098, + "timestamp": 7.6848541379437085 + }, + { + "x": 1.4144863610944205, + "y": 5.571414209224462, + "heading": -1.1138395195686094e-18, + "angularVelocity": 1.6431093577313864e-17, + "velocityX": 0.8972099444973367, + "velocityY": -0.1408023305798717, + "timestamp": 7.7542425292292 + }, + { + "x": 1.49749429992851, + "y": 5.558387479732467, + "heading": 1.8047013110476196e-18, + "angularVelocity": 4.206093786115289e-17, + "velocityX": 1.1962799150734271, + "velocityY": -0.18773643905934295, + "timestamp": 7.823630920514691 + }, + { + "x": 1.6012542223682356, + "y": 5.5421040680405165, + "heading": 7.934655780307681e-18, + "angularVelocity": 8.834265128972616e-17, + "velocityX": 1.4953498779473966, + "velocityY": -0.23467054633035336, + "timestamp": 7.893019311800182 + }, + { + "x": 1.725766127726464, + "y": 5.522563974256421, + "heading": 1.6517348693937324e-17, + "angularVelocity": 1.236906166709818e-16, + "velocityX": 1.7944198309186505, + "velocityY": -0.28160465204763024, + "timestamp": 7.962407703085673 + }, + { + "x": 1.8710300150870187, + "y": 5.499767198523928, + "heading": 2.797873673898563e-17, + "angularVelocity": 1.6517731335259598e-16, + "velocityX": 2.0934897706863036, + "velocityY": -0.3285387556932655, + "timestamp": 8.031796094371165 + }, + { + "x": 2.0370458831672553, + "y": 5.4737137410442855, + "heading": 4.626535556608686e-17, + "angularVelocity": 2.635400319271784e-16, + "velocityX": 2.3925596919689536, + "velocityY": -0.3754728564386086, + "timestamp": 8.101184485656656 + }, + { + "x": 2.223813730043212, + "y": 5.444403602119362, + "heading": 6.626560991799023e-17, + "angularVelocity": 2.882363175884601e-16, + "velocityX": 2.691629585524184, + "velocityY": -0.42240695283352697, + "timestamp": 8.170572876942147 + }, + { + "x": 2.4313335525083035, + "y": 5.411836782252268, + "heading": 9.137945639838438e-17, + "angularVelocity": 3.6193152862799336e-16, + "velocityX": 2.99069943286728, + "velocityY": -0.46934104197777365, + "timestamp": 8.239961268227638 + }, + { + "x": 2.65960534414942, + "y": 5.376013282449221, + "heading": 1.1821641145167503e-16, + "angularVelocity": 3.8676433675745036e-16, + "velocityX": 3.2897691877869866, + "velocityY": -0.5162751166208155, + "timestamp": 8.30934965951313 + }, + { + "x": 2.9086290857277297, + "y": 5.336933105728788, + "heading": 1.4538386371763974e-16, + "angularVelocity": 3.915273420502118e-16, + "velocityX": 3.5888386654437228, + "velocityY": -0.5632091477613738, + "timestamp": 8.37873805079862 + }, + { + "x": 3.1678798362557536, + "y": 5.296247694105415, + "heading": 1.2970532145326214e-16, + "angularVelocity": -2.259533930819528e-16, + "velocityX": 3.73622656074219, + "velocityY": -0.5863432033749367, + "timestamp": 8.448126442084112 + }, + { + "x": 3.426192624956805, + "y": 5.24997937308727, + "heading": 1.1963113847649017e-16, + "angularVelocity": -1.451854235288838e-16, + "velocityX": 3.7227089995248686, + "velocityY": -0.6668020422577368, + "timestamp": 8.517514833369603 + }, + { + "x": 3.6799776944966163, + "y": 5.18320068927956, + "heading": 1.442058714433213e-16, + "angularVelocity": 3.5416202208408226e-16, + "velocityX": 3.657457174581828, + "velocityY": -0.9623898547086362, + "timestamp": 8.586903224655094 + }, + { + "x": 3.9276087380924305, + "y": 5.096338039673236, + "heading": 1.2678863352315073e-16, + "angularVelocity": -2.510108333324681e-16, + "velocityX": 3.568767613835609, + "velocityY": -1.251832590395956, + "timestamp": 8.656291615940585 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -3.9291869622836704e-18, - "angularVelocity": 2.1667607761478547e-16, - "velocityX": 3.770711743405716, - "velocityY": -1.8146339019446507, - "timestamp": 9.19955455282942 - }, - { - "x": 4.28210524468114, - "y": 4.930751598714327, - "heading": 3.591781681737572e-18, - "angularVelocity": 2.4504750021143145e-16, - "velocityX": 3.734087818922699, - "velocityY": -1.9287249614518895, - "timestamp": 9.230246433909775 - }, - { - "x": 4.395415253823689, - "y": 4.868113726791297, - "heading": 1.252716982122126e-17, - "angularVelocity": 2.9113198354077836e-16, - "velocityX": 3.691856124618014, - "velocityY": -2.0408612870301757, - "timestamp": 9.26093831499013 - }, - { - "x": 4.5071149827963914, - "y": 4.802169472812339, - "heading": 2.3134869211764074e-17, - "angularVelocity": 3.4561906984236055e-16, - "velocityX": 3.639390126667849, - "velocityY": -2.1485895180653474, - "timestamp": 9.291630196070484 - }, - { - "x": 4.616896016370763, - "y": 4.73308774688056, - "heading": 3.142037619794973e-17, - "angularVelocity": 2.699576125240905e-16, - "velocityX": 3.576875372576209, - "velocityY": -2.2508143359119486, - "timestamp": 9.322322077150838 - }, - { - "x": 4.7230901119791495, - "y": 4.663194322009908, - "heading": 3.6606203768648707e-17, - "angularVelocity": 1.6896414210849805e-16, - "velocityX": 3.460006095108622, - "velocityY": -2.277261034853729, - "timestamp": 9.353013958231193 - }, - { - "x": 4.827842225607654, - "y": 4.596684057361499, - "heading": 4.0451980836943306e-17, - "angularVelocity": 1.2530273569920946e-16, - "velocityX": 3.413023573050007, - "velocityY": -2.1670312247815544, - "timestamp": 9.383705839311547 - }, - { - "x": 4.932161852632399, - "y": 4.533825930261183, - "heading": 4.329689113733961e-17, - "angularVelocity": 9.26925861582003e-17, - "velocityX": 3.3989323349592793, - "velocityY": -2.0480376206250996, - "timestamp": 9.414397720391902 - }, - { - "x": 5.036392029451142, - "y": 4.474644374374991, - "heading": 4.527403793938572e-17, - "angularVelocity": 6.441920024435287e-17, - "velocityX": 3.396017876709991, - "velocityY": -1.9282479210462729, - "timestamp": 9.445089601472256 - }, - { - "x": 5.1407004011750175, - "y": 4.419139647910205, - "heading": 4.6239659419552744e-17, - "angularVelocity": 3.1461773577369364e-17, - "velocityX": 3.3985656157987063, - "velocityY": -1.8084498085819396, - "timestamp": 9.47578148255261 - }, - { - "x": 5.245185751752097, - "y": 4.367308322392446, - "heading": 4.6966543072994365e-17, - "angularVelocity": 2.3683241137198615e-17, - "velocityX": 3.4043319242482535, - "velocityY": -1.68876340234931, - "timestamp": 9.506473363632965 - }, - { - "x": 5.349913071755195, - "y": 4.319146690519462, - "heading": 4.64672982849528e-17, - "angularVelocity": -1.6266355263468505e-17, - "velocityX": 3.4122157494652257, - "velocityY": -1.5691977870920355, - "timestamp": 9.53716524471332 - }, - { - "x": 5.454928326528494, - "y": 4.274651432535085, - "heading": 4.6021079264842955e-17, - "angularVelocity": -1.4538674934985797e-17, - "velocityX": 3.421597213229158, - "velocityY": -1.449740335819904, - "timestamp": 9.567857125793674 - }, - { - "x": 5.560265728804007, - "y": 4.233819701178179, - "heading": 4.5613400126539626e-17, - "angularVelocity": -1.32829717492163e-17, - "velocityX": 3.432093393029018, - "velocityY": -1.3303756537438909, - "timestamp": 9.598549006874029 + "heading": 9.872947837544061e-17, + "angularVelocity": -4.0437823428221097e-16, + "velocityX": 3.457211254829872, + "velocityY": -1.5332571030157462, + "timestamp": 8.725680007226076 + }, + { + "x": 4.284983330328794, + "y": 4.932123965200671, + "heading": 8.520616401533837e-17, + "angularVelocity": -3.905855769726077e-16, + "velocityX": 3.3932258158637794, + "velocityY": -1.6700901615188843, + "timestamp": 8.760303186261309 + }, + { + "x": 4.400064822274762, + "y": 4.86965478591225, + "heading": 8.38275920998652e-17, + "angularVelocity": -3.9816445337684444e-17, + "velocityX": 3.3238280005674867, + "velocityY": -1.8042589106232223, + "timestamp": 8.794926365296542 + }, + { + "x": 4.512559967686789, + "y": 4.802639932227702, + "heading": 8.775922770148411e-17, + "angularVelocity": 1.1355501462237473e-16, + "velocityX": 3.2491281432462653, + "velocityY": -1.935548830347224, + "timestamp": 8.829549544331774 + }, + { + "x": 4.622289335106214, + "y": 4.731186349760053, + "heading": 1.0501758472421099e-16, + "angularVelocity": 4.984625185678289e-16, + "velocityX": 3.169245877386488, + "velocityY": -2.06374990566114, + "timestamp": 8.864172723367007 + }, + { + "x": 4.729077970033436, + "y": 4.65540814642046, + "heading": 1.123300173637084e-16, + "angularVelocity": 2.11200497564256e-16, + "velocityX": 3.0843105082451987, + "velocityY": -2.188655272309947, + "timestamp": 8.89879590240224 + }, + { + "x": 4.83750746703025, + "y": 4.5819970246859665, + "heading": 1.0947895650457139e-16, + "angularVelocity": -8.234543847737888e-17, + "velocityX": 3.131702518895681, + "velocityY": -2.120288309163928, + "timestamp": 8.933419081437473 + }, + { + "x": 4.94878188699609, + "y": 4.512974188419722, + "heading": 1.0086901564782932e-16, + "angularVelocity": -2.486756299293195e-16, + "velocityX": 3.213870680465465, + "velocityY": -1.9935441571094976, + "timestamp": 8.968042260472705 + }, + { + "x": 5.062723794471585, + "y": 4.448449851931572, + "heading": 9.759628790443595e-17, + "angularVelocity": -9.452418393074315e-17, + "velocityX": 3.2909140827174443, + "velocityY": -1.8636167528836842, + "timestamp": 9.002665439507938 + }, + { + "x": 5.179151445124142, + "y": 4.388526983063662, + "heading": 9.598506413460085e-17, + "angularVelocity": -4.6535985854895585e-17, + "velocityX": 3.362708275114807, + "velocityY": -1.7307153917591276, + "timestamp": 9.03728861854317 + }, + { + "x": 5.297879115709132, + "y": 4.333301191879142, + "heading": 9.429439507749846e-17, + "angularVelocity": -4.883055525842942e-17, + "velocityX": 3.429138337186562, + "velocityY": -1.5950525839444993, + "timestamp": 9.071911797578403 + }, + { + "x": 5.418717408418424, + "y": 4.28286058857882, + "heading": 9.044194331783773e-17, + "angularVelocity": -1.112679963830143e-16, + "velocityX": 3.4900981387736523, + "velocityY": -1.4568449433541117, + "timestamp": 9.106534976613636 + }, + { + "x": 5.54147355571485, + "y": 4.237285646363086, + "heading": 8.606851716017984e-17, + "angularVelocity": -1.2631497971943828e-16, + "velocityX": 3.545490354063358, + "velocityY": -1.3163130447772213, + "timestamp": 9.141158155648869 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 4.5318461812156785e-17, - "angularVelocity": -9.609659048938486e-18, - "velocityX": 3.443451372045305, - "velocityY": -1.2110898816015587, - "timestamp": 9.629240887954383 - }, - { - "x": 5.79328341664379, - "y": 4.157319683069999, - "heading": 4.51013680218967e-17, - "angularVelocity": -5.8955361022936136e-18, - "velocityX": 3.457901563090961, - "velocityY": -1.0680543595600307, - "timestamp": 9.666064286625446 - }, - { - "x": 5.920925042987332, - "y": 4.123275074482791, - "heading": 4.436771325922853e-17, - "angularVelocity": -1.9923600143581754e-17, - "velocityX": 3.4663184537566436, - "velocityY": -0.9245373815525213, - "timestamp": 9.702887685296508 - }, - { - "x": 6.048609186312294, - "y": 4.094524158555703, - "heading": 4.179578855252982e-17, - "angularVelocity": -6.984484950544979e-17, - "velocityX": 3.4674730723674467, - "velocityY": -0.780778444268955, - "timestamp": 9.73971108396757 - }, - { - "x": 6.176010603965038, - "y": 4.0710595506921905, - "heading": 3.752283423078513e-17, - "angularVelocity": -1.1603911512742033e-16, - "velocityX": 3.4597951913890252, - "velocityY": -0.6372200478591985, - "timestamp": 9.776534482638633 - }, - { - "x": 6.3027303874933365, - "y": 4.052844738846322, - "heading": 3.3662688725727524e-17, - "angularVelocity": -1.0482861149720267e-16, - "velocityX": 3.4412842948111595, - "velocityY": -0.4946531961533159, - "timestamp": 9.813357881309695 - }, - { - "x": 6.428277540039444, - "y": 4.03979228265205, - "heading": 2.875531522756203e-17, - "angularVelocity": -1.3326781276580096e-16, - "velocityX": 3.409439570410085, - "velocityY": -0.35446093150903635, - "timestamp": 9.850181279980758 - }, - { - "x": 6.552051846294861, - "y": 4.031728011576002, - "heading": 2.3979849407990237e-17, - "angularVelocity": -1.2968564213358396e-16, - "velocityX": 3.36129501138861, - "velocityY": -0.21899855437257756, - "timestamp": 9.88700467865182 - }, - { - "x": 6.673339149172373, - "y": 4.028337045978646, - "heading": 1.9327950159963868e-17, - "angularVelocity": -1.2632998799939155e-16, - "velocityX": 3.2937563412044475, - "velocityY": -0.09208725212048961, - "timestamp": 9.923828077322883 - }, - { - "x": 6.791341292583958, - "y": 4.029097308231655, - "heading": 1.4785687788943395e-17, - "angularVelocity": -1.2335261767272074e-16, - "velocityX": 3.2045424287333875, - "velocityY": 0.020646172826134297, - "timestamp": 9.960651475993945 - }, - { - "x": 6.905265249942477, - "y": 4.033232975759379, - "heading": 1.0332836121843877e-17, - "angularVelocity": -1.2092452080116043e-16, - "velocityX": 3.0937925740147887, - "velocityY": 0.11231085877402444, - "timestamp": 9.997474874665008 - }, - { - "x": 7.014459322141266, - "y": 4.039746278237479, - "heading": 6.270033366373878e-18, - "angularVelocity": -1.103320979751085e-16, - "velocityX": 2.9653447573973857, - "velocityY": 0.17687944929482444, - "timestamp": 10.03429827333607 - }, - { - "x": 7.1185188980352505, - "y": 4.047548732117647, - "heading": 2.7758268202913238e-18, - "angularVelocity": -9.489093222865076e-17, - "velocityX": 2.8259090591700105, - "velocityY": 0.21188847748320577, - "timestamp": 10.071121672007132 + "heading": 8.12244185045661e-17, + "angularVelocity": -1.3990912419348855e-16, + "velocityX": 3.5952265671295462, + "velocityY": -1.1736811275270398, + "timestamp": 9.175781334684102 + }, + { + "x": 5.800884886979515, + "y": 4.158897293130728, + "heading": 7.604329686230843e-17, + "angularVelocity": -1.3984796334066921e-16, + "velocityX": 3.642093094613744, + "velocityY": -1.0189897302407367, + "timestamp": 9.212829579924803 + }, + { + "x": 5.9373078989205395, + "y": 4.126945510909642, + "heading": 7.099587364491845e-17, + "angularVelocity": -1.3623919795923125e-16, + "velocityX": 3.682306977150727, + "velocityY": -0.8624371279529385, + "timestamp": 9.249877825165504 + }, + { + "x": 6.074971576496844, + "y": 4.100852086793861, + "heading": 6.753360793468667e-17, + "angularVelocity": -9.345289332160286e-17, + "velocityX": 3.7157948151635507, + "velocityY": -0.7043093119863827, + "timestamp": 9.286926070406205 + }, + { + "x": 6.213624465766887, + "y": 4.080664678033529, + "heading": 6.709732769415932e-17, + "angularVelocity": -1.177600282260163e-17, + "velocityX": 3.742495450708113, + "velocityY": -0.5448951395450716, + "timestamp": 9.323974315646906 + }, + { + "x": 6.353013304056433, + "y": 4.066420153436822, + "heading": 4.721163336657454e-17, + "angularVelocity": -5.367513143575007e-16, + "velocityX": 3.7623600627760427, + "velocityY": -0.38448581043883095, + "timestamp": 9.361022560887607 + }, + { + "x": 6.4925719820191485, + "y": 4.0581614935185435, + "heading": 3.328638124108273e-17, + "angularVelocity": -3.758680611067538e-16, + "velocityX": 3.766944346648758, + "velocityY": -0.22291635851097177, + "timestamp": 9.398070806128308 + }, + { + "x": 6.627754792512696, + "y": 4.053990782696842, + "heading": 2.356760119630702e-17, + "angularVelocity": -2.6232767457636995e-16, + "velocityX": 3.6488316684169346, + "velocityY": -0.1125751245330268, + "timestamp": 9.435119051369009 + }, + { + "x": 6.757419046645761, + "y": 4.0521449831028535, + "heading": 1.6377013350687517e-17, + "angularVelocity": -1.940871361679193e-16, + "velocityX": 3.4998757239551144, + "velocityY": -0.049821511977060404, + "timestamp": 9.47216729660971 + }, + { + "x": 6.881278674219902, + "y": 4.051770935128315, + "heading": 1.0749401519445311e-17, + "angularVelocity": -1.51899550746631e-16, + "velocityX": 3.3431982208450166, + "velocityY": -0.010096239973268065, + "timestamp": 9.509215541850411 + }, + { + "x": 6.999234961376727, + "y": 4.052402049117112, + "heading": 6.6780180524147395e-18, + "angularVelocity": -1.0989409673710028e-16, + "velocityX": 3.183856249883563, + "velocityY": 0.017034922563732322, + "timestamp": 9.546263787091112 + }, + { + "x": 7.111245945109509, + "y": 4.053749520879657, + "heading": 3.4043054479450288e-18, + "angularVelocity": -8.836349922350792e-17, + "velocityX": 3.0233816205072856, + "velocityY": 0.03637073102354394, + "timestamp": 9.583312032331813 }, { "x": 7.217291355133057, "y": 4.0556182861328125, "heading": 0, - "angularVelocity": -7.538214933530801e-17, - "velocityX": 2.6823286459819884, - "velocityY": 0.21914202127972096, - "timestamp": 10.107945070678195 - }, - { - "x": 7.3677982829654445, - "y": 4.067452321816921, - "heading": -1.899704607683099e-18, - "angularVelocity": -3.08402414017173e-17, - "velocityX": 2.4433640695715435, - "velocityY": 0.19211645606626163, - "timestamp": 10.169543311278819 - }, - { - "x": 7.503615163186003, - "y": 4.077376831566324, - "heading": -3.815984956526995e-18, - "angularVelocity": -3.1109335739679044e-17, - "velocityX": 2.204882459243212, - "velocityY": 0.16111677302195881, - "timestamp": 10.231141551879443 - }, - { - "x": 7.624766058972102, - "y": 4.085215025650736, - "heading": -3.719599241310952e-18, - "angularVelocity": 1.5647479451753584e-18, - "velocityX": 1.9667914960686657, - "velocityY": 0.12724704485038288, - "timestamp": 10.292739792480067 - }, - { - "x": 7.731270598361674, - "y": 4.090833292880733, - "heading": -3.1356688291160894e-18, - "angularVelocity": 9.479660768691952e-18, - "velocityX": 1.7290191789746205, - "velocityY": 0.09120824191105437, - "timestamp": 10.35433803308069 - }, - { - "x": 7.8231450024295786, - "y": 4.094127127737666, - "heading": -2.8973098167797552e-18, - "angularVelocity": 3.86957486173007e-18, - "velocityX": 1.491510198539231, - "velocityY": 0.05347287235504842, - "timestamp": 10.415936273681314 - }, - { - "x": 7.900402858238064, - "y": 4.09501256603324, - "heading": -2.6735415540223583e-18, - "angularVelocity": 3.632705288594772e-18, - "velocityX": 1.254221793596197, - "velocityY": 0.014374408862020004, - "timestamp": 10.477534514281938 - }, - { - "x": 7.963055690401912, - "y": 4.09342067695553, - "heading": -2.4640875687476226e-18, - "angularVelocity": 3.4003240257243437e-18, - "velocityX": 1.017120481899185, - "velocityY": -0.025843093279754936, - "timestamp": 10.539132754882562 - }, - { - "x": 8.011113385697886, - "y": 4.089293860980006, - "heading": -2.2687163840908643e-18, - "angularVelocity": 3.171700683847968e-18, - "velocityX": 0.7801796744092016, - "velocityY": -0.06699567934545418, - "timestamp": 10.600730995483186 - }, - { - "x": 8.044584512204576, - "y": 4.082583270186536, - "heading": -1.2051953127305475e-18, - "angularVelocity": 1.7265445662875882e-17, - "velocityX": 0.5433779630769335, - "velocityY": -0.108941273777265, - "timestamp": 10.66232923608381 + "angularVelocity": -9.188843930234661e-17, + "velocityX": 2.862359859004765, + "velocityY": 0.0504413971839862, + "timestamp": 9.620360277572514 + }, + { + "x": 7.386141990779784, + "y": 4.060115199818634, + "heading": -6.285690223777642e-18, + "angularVelocity": -9.593548047553597e-17, + "velocityX": 2.5770864103628295, + "velocityY": 0.06863424057552284, + "timestamp": 9.685880251977624 + }, + { + "x": 7.536265872308939, + "y": 4.064907354049287, + "heading": -1.0202276969724947e-17, + "angularVelocity": -5.97769883515376e-17, + "velocityX": 2.2912689281735665, + "velocityY": 0.07314035565737474, + "timestamp": 9.751400226382733 + }, + { + "x": 7.667662893519374, + "y": 4.069410994150776, + "heading": -1.2426686282698196e-17, + "angularVelocity": -3.395009435762481e-17, + "velocityX": 2.005449825087065, + "velocityY": 0.06873690263742, + "timestamp": 9.816920200787843 + }, + { + "x": 7.780343830854776, + "y": 4.073216939396029, + "heading": -1.3104250722395789e-17, + "angularVelocity": -1.03413416429204e-17, + "velocityX": 1.7197951977010522, + "velocityY": 0.05808832008572811, + "timestamp": 9.882440175192952 + }, + { + "x": 7.874322400829606, + "y": 4.07602285690707, + "heading": -1.2397928827700396e-17, + "angularVelocity": 1.0780252923201902e-17, + "velocityX": 1.4343499189081053, + "velocityY": 0.04282537556703236, + "timestamp": 9.947960149598062 + }, + { + "x": 7.949612474283929, + "y": 4.07759641137373, + "heading": -1.096542742535041e-17, + "angularVelocity": 2.1863583068378234e-17, + "velocityX": 1.1491163441060581, + "velocityY": 0.024016408445632246, + "timestamp": 10.013480124003172 + }, + { + "x": 8.006227098004604, + "y": 4.077753556862197, + "heading": -9.354890651543402e-18, + "angularVelocity": 2.458085169441397e-17, + "velocityX": 0.8640818961654191, + "velocityY": 0.0023984363530761407, + "timestamp": 10.079000098408281 + }, + { + "x": 8.044178200157203, + "y": 4.076344939580228, + "heading": -5.6627659945325885e-18, + "angularVelocity": 5.635113094762732e-17, + "velocityX": 0.579229502104909, + "velocityY": -0.021499051163544333, + "timestamp": 10.14452007281339 }, { "x": 8.0634765625, "y": 4.073246955871582, "heading": 0, - "angularVelocity": 1.956541790475601e-17, - "velocityX": 0.30669788797884484, - "velocityY": -0.15156787310674, - "timestamp": 10.723927476684434 - }, - { - "x": 8.064977337405587, - "y": 4.058030189469969, - "heading": 1.4400664949819045e-18, - "angularVelocity": 1.9298542246489556e-17, - "velocityX": 0.0201121046014515, - "velocityY": -0.20392211811774588, - "timestamp": 10.79854795700343 - }, - { - "x": 8.04498852817461, - "y": 4.039529293710826, - "heading": 2.8532799754877234e-18, - "angularVelocity": 1.8938681059523328e-17, - "velocityX": -0.26787296390383064, - "velocityY": -0.2479332172622413, - "timestamp": 10.873168437322427 - }, - { - "x": 8.003412006395102, - "y": 4.018467462910651, - "heading": 4.2316573286544775e-18, - "angularVelocity": 1.847183698342028e-17, - "velocityX": -0.5571730656486096, - "velocityY": -0.2822526833134443, - "timestamp": 10.947788917641423 - }, - { - "x": 7.940164057594933, - "y": 3.9956918759860844, - "heading": 5.565943858803391e-18, - "angularVelocity": 1.7880969384827547e-17, - "velocityX": -0.8475950373113531, - "velocityY": -0.30521898046224694, - "timestamp": 11.02240939796042 - }, - { - "x": 7.85518871719706, - "y": 3.9722034430196183, - "heading": 6.5199606818876674e-18, - "angularVelocity": 1.2784919284944702e-17, - "velocityX": -1.138766998478306, - "velocityY": -0.31477193481005983, - "timestamp": 11.097029878279416 - }, - { - "x": 7.748479579667781, - "y": 3.949194323259055, - "heading": 7.228395857909563e-18, - "angularVelocity": 9.493843624470073e-18, - "velocityX": -1.4300248011418268, - "velocityY": -0.3083485882454945, - "timestamp": 11.171650358598413 - }, - { - "x": 7.6201153258458145, - "y": 3.928094024739179, - "heading": 7.8541038899261e-18, - "angularVelocity": 8.385205006937927e-18, - "velocityX": -1.7202281903469572, - "velocityY": -0.28276819486653393, - "timestamp": 11.24627083891741 - }, - { - "x": 7.470317035092631, - "y": 3.9106227342866235, - "heading": 8.379134437333569e-18, - "angularVelocity": 7.036011272928214e-18, - "velocityX": -2.00746886260729, - "velocityY": -0.234135325554959, - "timestamp": 11.320891319236406 - }, - { - "x": 7.299538426697331, - "y": 3.8988447632701435, - "heading": 8.78216866852809e-18, - "angularVelocity": 5.401120751271277e-18, - "velocityX": -2.2886291761355184, - "velocityY": -0.15783831685523833, - "timestamp": 11.395511799555402 - }, - { - "x": 7.108600470483675, - "y": 3.8952024104802057, - "heading": 9.038204388282958e-18, - "angularVelocity": 3.4311721461170713e-18, - "velocityX": -2.5587875526586417, - "velocityY": -0.04881170389640219, - "timestamp": 11.470132279874399 - }, - { - "x": 6.89887047602872, - "y": 3.902489232444511, - "heading": 9.11887229683693e-18, - "angularVelocity": 1.0810424542935678e-18, - "velocityX": -2.810622413020914, - "velocityY": 0.09765176977090569, - "timestamp": 11.544752760193395 - }, - { - "x": 6.672447487864324, - "y": 3.9237019834694964, - "heading": 8.994132821099478e-18, - "angularVelocity": -1.6716520183319338e-18, - "velocityX": -3.0343276697825954, - "velocityY": 0.28427518737888835, - "timestamp": 11.619373240512392 - }, - { - "x": 6.4322495960952635, - "y": 3.961732568936921, - "heading": 8.636009892152731e-18, - "angularVelocity": -4.799257891447058e-18, - "velocityX": -3.218927173106323, - "velocityY": 0.5096534531116348, - "timestamp": 11.693993720831388 - }, - { - "x": 6.181871022214133, - "y": 4.01897099744758, - "heading": 8.023682097716734e-18, - "angularVelocity": -8.205894562430798e-18, - "velocityX": -3.3553599871078905, - "velocityY": 0.7670605746032516, - "timestamp": 11.768614201150385 - }, - { - "x": 5.925195916976863, - "y": 4.0970166657243565, - "heading": 7.147197665220672e-18, - "angularVelocity": -1.17458963959573e-17, - "velocityX": -3.4397407272106473, - "velocityY": 1.0459014461329927, - "timestamp": 11.843234681469381 + "angularVelocity": 8.642808619399734e-17, + "velocityX": 0.2945416648589691, + "velocityY": -0.04728304210698771, + "timestamp": 10.2100400472185 + }, + { + "x": 8.06151161206475, + "y": 4.067466722329622, + "heading": 7.499406153547556e-18, + "angularVelocity": 1.0142249804726799e-16, + "velocityX": -0.026574128354115305, + "velocityY": -0.07817228633622807, + "timestamp": 10.283982281901235 + }, + { + "x": 8.0357781795929, + "y": 4.059672922438978, + "heading": 2.0171548175080756e-17, + "angularVelocity": 1.7137894295700475e-16, + "velocityX": -0.34802075677407873, + "velocityY": -0.10540389973451367, + "timestamp": 10.357924516583969 + }, + { + "x": 7.986251299714503, + "y": 4.050185891028673, + "heading": 3.259640252688688e-17, + "angularVelocity": 1.6803460705902343e-16, + "velocityX": -0.6698050186189494, + "velocityY": -0.12830328229881344, + "timestamp": 10.431866751266703 + }, + { + "x": 7.9129066919285, + "y": 4.039390810897977, + "heading": 4.5982022945133473e-17, + "angularVelocity": 1.81028075140569e-16, + "velocityX": -0.9919176516736913, + "velocityY": -0.1459934255032282, + "timestamp": 10.505808985949438 + }, + { + "x": 7.8157231380567715, + "y": 4.027759201442648, + "heading": 5.916672321533067e-17, + "angularVelocity": 1.783108168799044e-16, + "velocityX": -1.314317240866671, + "velocityY": -0.1573067071239705, + "timestamp": 10.579751220632172 + }, + { + "x": 7.694687242510101, + "y": 4.015880811506709, + "heading": 7.241021515042493e-17, + "angularVelocity": 1.7910591950468916e-16, + "velocityX": -1.6368979929535623, + "velocityY": -0.16064418375919837, + "timestamp": 10.653693455314906 + }, + { + "x": 7.549803190746082, + "y": 4.0045128268534045, + "heading": 8.516404983291186e-17, + "angularVelocity": 1.7248376026751216e-16, + "velocityX": -1.9594221406165937, + "velocityY": -0.1537414266972227, + "timestamp": 10.72763568999764 + }, + { + "x": 7.381113647590609, + "y": 3.9946593063008904, + "heading": 9.839417244429125e-17, + "angularVelocity": 1.7892511182976314e-16, + "velocityX": -2.281369286163345, + "velocityY": -0.13325970732143547, + "timestamp": 10.801577924680375 + }, + { + "x": 7.188747477257485, + "y": 3.987706179482555, + "heading": 1.110446332225851e-16, + "angularVelocity": 1.7108572429643452e-16, + "velocityX": -2.6015736629885633, + "velocityY": -0.09403457777777122, + "timestamp": 10.87552015936311 + }, + { + "x": 6.973038504473885, + "y": 3.9856630874408094, + "heading": 1.1423152127106693e-16, + "angularVelocity": 4.309969887337601e-17, + "velocityX": -2.91726337064529, + "velocityY": -0.027630920954873416, + "timestamp": 10.949462394045844 + }, + { + "x": 6.734853840508573, + "y": 3.9916098664093003, + "heading": 1.1357543736188786e-16, + "angularVelocity": -8.872925078647214e-18, + "velocityX": -3.221226204310621, + "velocityY": 0.0804246584378634, + "timestamp": 11.023404628728578 + }, + { + "x": 6.47658832080339, + "y": 4.010431184705039, + "heading": 1.1486840283254658e-16, + "angularVelocity": 1.7486156277295078e-17, + "velocityX": -3.4928011144554563, + "velocityY": 0.25454083686402557, + "timestamp": 11.097346863411312 + }, + { + "x": 6.204860010324235, + "y": 4.048943809403862, + "heading": 1.1487825258491134e-16, + "angularVelocity": 1.3320874674868913e-19, + "velocityX": -3.6748728469603784, + "velocityY": 0.5208474542873691, + "timestamp": 11.171289098094046 + }, + { + "x": 5.932253242931649, + "y": 4.111293853023339, + "heading": 1.0773394940295777e-16, + "angularVelocity": -9.662006041077537e-17, + "velocityX": -3.686753160250877, + "velocityY": 0.8432263899921877, + "timestamp": 11.24523133277678 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 1.1380657643949956e-17, - "angularVelocity": 5.673321785133305e-17, - "velocityX": -3.4741693841666152, - "velocityY": 1.3351885220272963, - "timestamp": 11.917855161788378 - }, - { - "x": 5.562354883757379, - "y": 4.239983693882295, - "heading": 1.257506167197437e-17, - "angularVelocity": 4.001149623529015e-17, - "velocityX": -3.4704042300707463, - "velocityY": 1.4516720671542953, - "timestamp": 11.947706682877985 - }, - { - "x": 5.458854001324223, - "y": 4.2867960161311025, - "heading": 1.368919721006296e-17, - "angularVelocity": 3.7322571632702434e-17, - "velocityX": -3.467189565398273, - "velocityY": 1.5681720910732326, - "timestamp": 11.977558203967593 - }, - { - "x": 5.355427026400021, - "y": 4.337086579385366, - "heading": 1.4765466722121167e-17, - "angularVelocity": 3.605409269066309e-17, - "velocityX": -3.46471372811238, - "velocityY": 1.6846901403550338, - "timestamp": 12.007409725057201 - }, - { - "x": 5.252042805813138, - "y": 4.3908559053171485, - "heading": 1.577172278579381e-17, - "angularVelocity": 3.370870306062832e-17, - "velocityX": -3.4632814949879767, - "velocityY": 1.8012256651973733, - "timestamp": 12.037261246146809 - }, - { - "x": 5.148654011351914, - "y": 4.4481042525220715, - "heading": 1.6688127577708658e-17, - "angularVelocity": 3.0698763599309256e-17, - "velocityX": -3.4634347158013203, - "velocityY": 1.917769852768155, - "timestamp": 12.067112767236416 - }, - { - "x": 5.045180193102648, - "y": 4.508830583010288, - "heading": 1.7559230594262742e-17, - "angularVelocity": 2.9181193522489866e-17, - "velocityX": -3.4662829387708096, - "velocityY": 2.034279268581587, - "timestamp": 12.096964288326024 - }, - { - "x": 4.941454797962972, - "y": 4.5730268239608955, - "heading": 1.8097620255598777e-17, - "angularVelocity": 1.8035585476593816e-17, - "velocityX": -3.4747105458483385, - "velocityY": 2.1505182519143307, - "timestamp": 12.126815809415632 - }, - { - "x": 4.836940518620058, - "y": 4.640611448605407, - "heading": 1.8315935161814963e-17, - "angularVelocity": 7.313359523284407e-18, - "velocityX": -3.5011374807060482, - "velocityY": 2.264026159391885, - "timestamp": 12.15666733050524 - }, - { - "x": 4.729371052476075, - "y": 4.706532042565262, - "heading": 1.8014793483289063e-17, - "angularVelocity": -1.0087984381831229e-17, - "velocityX": -3.603483581995116, - "velocityY": 2.2082825783642597, - "timestamp": 12.186518851594847 - }, - { - "x": 4.620024826639761, - "y": 4.769461542081556, - "heading": 1.7990520308788652e-17, - "angularVelocity": -8.131302397490335e-19, - "velocityX": -3.663003486759619, - "velocityY": 2.1080835153221735, - "timestamp": 12.216370372684455 - }, - { - "x": 4.508985011067914, - "y": 4.829352072952906, - "heading": 1.7846349965295797e-17, - "angularVelocity": -4.829581147982843e-18, - "velocityX": -3.7197372702894755, - "velocityY": 2.0062807081612415, - "timestamp": 12.246221893774063 - }, - { - "x": 4.396354195056893, - "y": 4.886148687843668, - "heading": 1.7996479956698872e-17, - "angularVelocity": 5.029224125294331e-18, - "velocityX": -3.7730344015946646, - "velocityY": 1.9026372130341678, - "timestamp": 12.27607341486367 - }, - { - "x": 4.28245727872322, - "y": 4.939704839757233, - "heading": 1.8249745379226758e-17, - "angularVelocity": 8.484171416768941e-18, - "velocityX": -3.8154476615036823, - "velocityY": 1.794084520946214, - "timestamp": 12.305924935953279 + "heading": 9.686455716091523e-17, + "angularVelocity": -1.4699842774133078e-16, + "velocityX": -3.6014804699029255, + "velocityY": 1.1543500395591086, + "timestamp": 11.319173567459515 + }, + { + "x": 5.543077137245883, + "y": 4.241512673300566, + "heading": 9.161999350902719e-17, + "angularVelocity": -1.5163138697871951e-16, + "velocityX": -3.552563374503243, + "velocityY": 1.2971011802444727, + "timestamp": 11.353761153554894 + }, + { + "x": 5.422090048254055, + "y": 4.291242278322373, + "heading": 8.675864551293739e-17, + "angularVelocity": -1.4055181482408883e-16, + "velocityX": -3.4979916973158245, + "velocityY": 1.4377876757479933, + "timestamp": 11.388348739650272 + }, + { + "x": 5.303183074500814, + "y": 4.34575871765036, + "heading": 8.202370382364629e-17, + "angularVelocity": -1.368971421201274e-16, + "velocityX": -3.437851182367712, + "velocityY": 1.5761851427750155, + "timestamp": 11.42293632574565 + }, + { + "x": 5.186545516053187, + "y": 4.404975197881964, + "heading": 7.882281033080331e-17, + "angularVelocity": -9.254457608045447e-17, + "velocityX": -3.372237603572297, + "velocityY": 1.712073229635336, + "timestamp": 11.45752391184103 + }, + { + "x": 5.072363058490256, + "y": 4.468797441531519, + "heading": 7.597641724828609e-17, + "angularVelocity": -8.229522218370685e-17, + "velocityX": -3.3012554633926476, + "velocityY": 1.845235555715285, + "timestamp": 11.492111497936408 + }, + { + "x": 4.960817475466504, + "y": 4.537123834649348, + "heading": 7.51892087087232e-17, + "angularVelocity": -2.2759857753359948e-17, + "velocityX": -3.225017863812662, + "velocityY": 1.975460008380286, + "timestamp": 11.526699084031787 + }, + { + "x": 4.852086330068725, + "y": 4.609845576159071, + "heading": 7.00186010980759e-17, + "angularVelocity": -1.4949316197981958e-16, + "velocityX": -3.143646541216975, + "velocityY": 2.1025387926519685, + "timestamp": 11.561286670127165 + }, + { + "x": 4.744615410553515, + "y": 4.6844171206048495, + "heading": 6.888474965661026e-17, + "angularVelocity": -3.278203452357036e-17, + "velocityX": -3.10721075529382, + "velocityY": 2.1560204935996916, + "timestamp": 11.595874256222544 + }, + { + "x": 4.634255259438232, + "y": 4.75464211813236, + "heading": 6.027771904333573e-17, + "angularVelocity": -2.4884739251649313e-16, + "velocityX": -3.190744529293135, + "velocityY": 2.0303526627692405, + "timestamp": 11.630461842317922 + }, + { + "x": 4.521181551584758, + "y": 4.820408744704174, + "heading": 5.1990889261648227e-17, + "angularVelocity": -2.395897117201502e-16, + "velocityX": -3.269199172838061, + "velocityY": 1.9014517633713166, + "timestamp": 11.665049428413301 + }, + { + "x": 4.405574295139685, + "y": 4.881612290783718, + "heading": 4.5533169228459885e-17, + "angularVelocity": -1.867062944312063e-16, + "velocityX": -3.3424494015360238, + "velocityY": 1.7695234906179804, + "timestamp": 11.69963701450868 + }, + { + "x": 4.287617535211261, + "y": 4.938155315408326, + "heading": 4.203003409755996e-17, + "angularVelocity": -1.0128301874666137e-16, + "velocityX": -3.410378498318693, + "velocityY": 1.6347779942979137, + "timestamp": 11.734224600604058 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 1.8296114714140825e-17, - "angularVelocity": 1.5533324384911975e-18, - "velocityX": -3.8510001878620828, - "velocityY": 1.6830953424405728, - "timestamp": 12.335776457042886 - }, - { - "x": 3.9173080803752227, - "y": 5.082092147187362, - "heading": 1.4829639745730528e-17, - "angularVelocity": -5.4272726613113606e-17, - "velocityX": -3.9171051476985386, - "velocityY": 1.4426543500365565, - "timestamp": 12.399647855841293 - }, - { - "x": 3.6655222616013243, - "y": 5.158389406763847, - "heading": 1.1586116724078029e-17, - "angularVelocity": -5.078208837148102e-17, - "velocityX": -3.9420745985005192, - "velocityY": 1.1945449921536402, - "timestamp": 12.4635192546397 - }, - { - "x": 3.415686594925907, - "y": 5.218879368068439, - "heading": 9.697216184828002e-18, - "angularVelocity": -2.957349571999372e-17, - "velocityX": -3.911542120189908, - "velocityY": 0.9470586591584124, - "timestamp": 12.527390653438106 - }, - { - "x": 3.1720421714343097, - "y": 5.264694784984283, - "heading": 8.965708374731706e-18, - "angularVelocity": -1.1452822775166807e-17, - "velocityX": -3.814609168973968, - "velocityY": 0.7173072420168656, - "timestamp": 12.591262052236512 - }, - { - "x": 2.938638877133611, - "y": 5.298312127499332, - "heading": 9.343063585543127e-18, - "angularVelocity": 5.9080467348504174e-18, - "velocityX": -3.654269339510983, - "velocityY": 0.5263285781661498, - "timestamp": 12.655133451034919 - }, - { - "x": 2.718391564246048, - "y": 5.322952057954089, - "heading": 1.0698464289017726e-17, - "angularVelocity": 2.12207768493952e-17, - "velocityX": -3.4482932428443296, - "velocityY": 0.3857740854012944, - "timestamp": 12.719004849833325 - }, - { - "x": 2.5129378876059687, - "y": 5.341690959630172, - "heading": 1.2882468345126029e-17, - "angularVelocity": 3.4193772081618774e-17, - "velocityX": -3.216677268780943, - "velocityY": 0.29338486440898187, - "timestamp": 12.782876248631732 - }, - { - "x": 2.323045360609233, - "y": 5.357035089630098, - "heading": 1.4586731405592636e-17, - "angularVelocity": 2.6682726421160203e-17, - "velocityX": -2.973044751940999, - "velocityY": 0.24023475747502873, - "timestamp": 12.846747647430139 - }, - { - "x": 2.149011712470278, - "y": 5.370906179422888, - "heading": 1.3182509626883337e-17, - "angularVelocity": -2.1985142076939443e-17, - "velocityX": -2.724750849566412, - "velocityY": 0.21717216240355022, - "timestamp": 12.910619046228545 - }, - { - "x": 1.9909052012063986, - "y": 5.384762303843791, - "heading": 1.2288275023072503e-17, - "angularVelocity": -1.400054828244982e-17, - "velocityX": -2.4753882682748096, - "velocityY": 0.2169378576573164, - "timestamp": 12.974490445026952 - }, - { - "x": 1.8486875999585186, - "y": 5.399721366691689, - "heading": 1.1833725700471485e-17, - "angularVelocity": -7.116633256521739e-18, - "velocityX": -2.2266241842730174, - "velocityY": 0.23420596901458723, - "timestamp": 13.038361843825358 - }, - { - "x": 1.7222741786291598, - "y": 5.416655739466683, - "heading": 1.1765320590646606e-17, - "angularVelocity": -1.070981842258772e-18, - "velocityX": -1.9791866736526023, - "velocityY": 0.2651323298624305, - "timestamp": 13.102233242623765 - }, - { - "x": 1.6115626522567748, - "y": 5.436259280896622, - "heading": 1.2039434559055613e-17, - "angularVelocity": 4.291654398939878e-18, - "velocityX": -1.7333505834405774, - "velocityY": 0.30692206212380396, - "timestamp": 13.166104641422171 - }, - { - "x": 1.5164470746291163, - "y": 5.459093977101566, - "heading": 9.126879142292297e-18, - "angularVelocity": -4.560030730930113e-17, - "velocityX": -1.4891732358620327, - "velocityY": 0.3575105075906568, - "timestamp": 13.229976040220578 - }, - { - "x": 1.4368243680409203, - "y": 5.485622578468376, - "heading": 6.490550146878691e-18, - "angularVelocity": -4.1275579397551e-17, - "velocityX": -1.2466097202521527, - "velocityY": 0.41534398597625355, - "timestamp": 13.293847439018984 - }, - { - "x": 1.3725972060727116, - "y": 5.516231768358422, - "heading": 4.104803215844785e-18, - "angularVelocity": -3.735235133440495e-17, - "velocityX": -1.0055699918350633, - "velocityY": 0.47923155693921204, - "timestamp": 13.35771883781739 - }, - { - "x": 1.3236750778063309, - "y": 5.551248899658597, - "heading": 1.947639425952167e-18, - "angularVelocity": -3.3773548532536126e-17, - "velocityX": -0.7659473439870959, - "velocityY": 0.5482443152794746, - "timestamp": 13.421590236615797 + "heading": 4.0833427337575637e-17, + "angularVelocity": -3.459642302543387e-17, + "velocityX": -3.4728780863994273, + "velocityY": 1.4974297517257, + "timestamp": 11.768812186699437 + }, + { + "x": 3.9262086603019655, + "y": 5.072405406382895, + "heading": 4.69185026085022e-17, + "angularVelocity": 9.025220132506432e-17, + "velocityX": -3.5787544523378267, + "velocityY": 1.2229891223429616, + "timestamp": 11.83623519889844 + }, + { + "x": 3.6792395049411635, + "y": 5.135860556463708, + "heading": 5.846977032472703e-17, + "angularVelocity": 1.713252988776416e-16, + "velocityX": -3.6629801503359496, + "velocityY": 0.9411497352494828, + "timestamp": 11.903658211097442 + }, + { + "x": 3.4280856473907875, + "y": 5.179929383329658, + "heading": 6.25257434115025e-17, + "angularVelocity": 6.01571029606942e-17, + "velocityX": -3.7250465287591408, + "velocityY": 0.6536169985387049, + "timestamp": 11.971081223296444 + }, + { + "x": 3.174266449354605, + "y": 5.204345315411431, + "heading": 8.981508430066906e-17, + "angularVelocity": 4.0474817127156825e-16, + "velocityX": -3.7645781426529163, + "velocityY": 0.3621305439411197, + "timestamp": 12.038504235495447 + }, + { + "x": 2.919317391668449, + "y": 5.208960723831647, + "heading": 3.637405660157601e-17, + "angularVelocity": -7.926229629337798e-16, + "velocityX": -3.781335917381794, + "velocityY": 0.06845449750291784, + "timestamp": 12.10592724769445 + }, + { + "x": 2.6778199582724502, + "y": 5.213921183762201, + "heading": 2.780922480909488e-17, + "angularVelocity": -1.270312837495845e-16, + "velocityX": -3.581825040435896, + "velocityY": 0.07357220878699586, + "timestamp": 12.173350259893452 + }, + { + "x": 2.4555441151084385, + "y": 5.223767512233129, + "heading": 2.258282570608997e-17, + "angularVelocity": -7.751654714759439e-17, + "velocityX": -3.2967355790624753, + "velocityY": 0.14603809811799462, + "timestamp": 12.240773272092454 + }, + { + "x": 2.2524898779476, + "y": 5.238499719208596, + "heading": 1.8179159660483545e-17, + "angularVelocity": -6.531399151152503e-17, + "velocityX": -3.0116458837750555, + "velocityY": 0.2185041352347682, + "timestamp": 12.308196284291457 + }, + { + "x": 2.068657252210669, + "y": 5.258117807366149, + "heading": 1.4559970052520395e-17, + "angularVelocity": -5.367884777868486e-17, + "velocityX": -2.726556108088765, + "velocityY": 0.29097021206423906, + "timestamp": 12.37561929649046 + }, + { + "x": 1.9040462406637386, + "y": 5.282621777825342, + "heading": 1.0447672435760193e-17, + "angularVelocity": -6.099249326267761e-17, + "velocityX": -2.441466291376519, + "velocityY": 0.3634363054985942, + "timestamp": 12.443042308689462 + }, + { + "x": 1.7586568449915738, + "y": 5.312011631159126, + "heading": 6.39006439635328e-18, + "angularVelocity": -6.018135214654068e-17, + "velocityX": -2.156376449676296, + "velocityY": 0.4359024074308457, + "timestamp": 12.510465320888464 + }, + { + "x": 1.6324890663305314, + "y": 5.34628736769762, + "heading": 2.9527164791572072e-18, + "angularVelocity": -5.0981820681006015e-17, + "velocityX": -1.8712865911219376, + "velocityY": 0.508368514259319, + "timestamp": 12.577888333087467 + }, + { + "x": 1.5255429054998768, + "y": 5.385448987646794, + "heading": 6.750997693764207e-19, + "angularVelocity": -3.378099903552108e-17, + "velocityX": -1.5861967204164489, + "velocityY": 0.5808346241426653, + "timestamp": 12.64531134528647 + }, + { + "x": 1.437818363118664, + "y": 5.4294964911430075, + "heading": 3.7918321325869045e-19, + "angularVelocity": -4.388954848629936e-18, + "velocityX": -1.301106840529317, + "velocityY": 0.6533007360484864, + "timestamp": 12.712734357485472 + }, + { + "x": 1.3693154396712461, + "y": 5.478429878281045, + "heading": 1.633332658934955e-18, + "angularVelocity": 1.8601207579968088e-17, + "velocityX": -1.0160169534583994, + "velocityY": 0.7257668493601166, + "timestamp": 12.780157369684474 + }, + { + "x": 1.3200341355468752, + "y": 5.532249149129769, + "heading": 3.8843995459700536e-19, + "angularVelocity": -1.8463914088315652e-17, + "velocityX": -0.7309270606141895, + "velocityY": 0.798232963693069, + "timestamp": 12.847580381883477 }, { "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": -3.0493138713173094e-17, - "velocityX": -0.5276325143220081, - "velocityY": 0.6216460705389873, - "timestamp": 13.485461635414204 - }, - { - "x": 1.2740430355469938, - "y": 5.664007440684529, - "heading": -2.547208860482595e-18, - "angularVelocity": -2.5831234528334732e-17, - "velocityX": -0.16156041888930542, - "velocityY": 0.7408315596510565, - "timestamp": 13.584071280199696 - }, - { - "x": 1.2943003151600432, - "y": 5.748531863021803, - "heading": -4.6379232663069386e-18, - "angularVelocity": -2.1201926150692658e-17, - "velocityX": 0.20542898878822005, - "velocityY": 0.8571618173977016, - "timestamp": 13.682680924985188 - }, - { - "x": 1.3508752960803845, - "y": 5.844112348620496, - "heading": -6.276905103665225e-18, - "angularVelocity": -1.6620908041208103e-17, - "velocityX": 0.5737266475648491, - "velocityY": 0.9692813092128183, - "timestamp": 13.78129056977068 - }, - { - "x": 1.4439661676967965, - "y": 5.9500755871992865, - "heading": -7.471930527081998e-18, - "angularVelocity": -1.2118747882558993e-17, - "velocityX": 0.9440341441135343, - "velocityY": 1.0745727642493252, - "timestamp": 13.879900214556173 - }, - { - "x": 1.5739124190504803, - "y": 6.065143361581136, - "heading": -8.237944726800887e-18, - "angularVelocity": -7.768146836290714e-18, - "velocityX": 1.3177843976251842, - "velocityY": 1.1669018241791527, - "timestamp": 13.978509859341665 - }, - { - "x": 1.7413805206507829, - "y": 6.185983632591009, - "heading": -6.8828411912414524e-18, - "angularVelocity": 1.3742099353654783e-17, - "velocityX": 1.698293325816142, - "velocityY": 1.2254406886135676, - "timestamp": 14.077119504127158 - }, - { - "x": 1.9428580652109537, - "y": 6.289954848196762, - "heading": -5.594323946529715e-18, - "angularVelocity": 1.3066848045055797e-17, - "velocityX": 2.0431829462366347, - "velocityY": 1.0543716675171477, - "timestamp": 14.17572914891265 - }, - { - "x": 2.114807562690416, - "y": 6.370065959572112, - "heading": -4.6183818981391425e-18, - "angularVelocity": 9.897024283736211e-18, - "velocityX": 1.7437391428952727, - "velocityY": 0.8124064491826988, - "timestamp": 14.274338793698142 - }, - { - "x": 2.2534346833326833, - "y": 6.431988322349882, - "heading": -2.7955204631360402e-18, - "angularVelocity": 1.8485630277436967e-17, - "velocityX": 1.4058170571836621, - "velocityY": 0.6279544248686071, - "timestamp": 14.372948438483634 - }, - { - "x": 2.357842355716001, - "y": 6.477471673664644, - "heading": -1.484023928114358e-18, - "angularVelocity": 1.3299880944166003e-17, - "velocityX": 1.0587977738935974, - "velocityY": 0.4612464776006573, - "timestamp": 14.471558083269127 - }, - { - "x": 2.427639134257716, - "y": 6.507357014317594, - "heading": -4.934983093022998e-19, - "angularVelocity": 1.0044916213421236e-17, - "velocityX": 0.7078088425685477, - "velocityY": 0.30306711597998603, - "timestamp": 14.570167728054619 + "angularVelocity": -5.761237033299429e-18, + "velocityX": -0.4458371630310619, + "velocityY": 0.8706990788013996, + "timestamp": 12.915003394082479 + }, + { + "x": 1.2868335136673428, + "y": 5.68644048793425, + "heading": -1.3391788419363526e-18, + "angularVelocity": -1.3686362473728894e-17, + "velocityX": -0.03210027395244838, + "velocityY": 0.9758655723245745, + "timestamp": 13.012851076079642 + }, + { + "x": 1.3242854311298142, + "y": 5.791775499822582, + "heading": 2.339658407779995e-18, + "angularVelocity": 3.759759224271494e-17, + "velocityX": 0.3827573295354856, + "velocityY": 1.076520258204863, + "timestamp": 13.110698758076804 + }, + { + "x": 1.4024881161679177, + "y": 5.906284066156856, + "heading": 5.091444284753208e-18, + "angularVelocity": 2.812315857202659e-17, + "velocityX": 0.7992287956333111, + "velocityY": 1.1702736743175466, + "timestamp": 13.208546440073967 + }, + { + "x": 1.5216854983005004, + "y": 6.028805982329465, + "heading": 6.6702155316476646e-18, + "angularVelocity": 1.6134988737668432e-17, + "velocityX": 1.218193213162059, + "velocityY": 1.2521698385881244, + "timestamp": 13.30639412207113 + }, + { + "x": 1.682281091295048, + "y": 6.156892671597042, + "heading": 8.737231811568005e-18, + "angularVelocity": 2.1124836460875052e-17, + "velocityX": 1.6412815277443633, + "velocityY": 1.3090416313724325, + "timestamp": 13.404241804068292 + }, + { + "x": 1.884561264278339, + "y": 6.2823011062588545, + "heading": 4.474450456421642e-18, + "angularVelocity": -4.3565481249463154e-17, + "velocityX": 2.06729652511499, + "velocityY": 1.2816699599020622, + "timestamp": 13.502089486065454 + }, + { + "x": 2.074597433027926, + "y": 6.367773862583125, + "heading": 1.3148438373880832e-18, + "angularVelocity": -3.229107274750916e-17, + "velocityX": 1.9421632160391744, + "velocityY": 0.8735286782445029, + "timestamp": 13.599937168062617 + }, + { + "x": 2.2289686427209534, + "y": 6.431502913374048, + "heading": 4.376002899474807e-18, + "angularVelocity": 3.128494207400489e-17, + "velocityX": 1.5776685409625097, + "velocityY": 0.6513087432441247, + "timestamp": 13.69778485005978 + }, + { + "x": 2.345509714236833, + "y": 6.4775215317101145, + "heading": 6.666442968036081e-18, + "angularVelocity": 2.3408220019295113e-17, + "velocityX": 1.1910458085175606, + "velocityY": 0.4703087226675544, + "timestamp": 13.795632532056942 + }, + { + "x": 2.4235057296596247, + "y": 6.507444880215651, + "heading": 4.695809387720839e-18, + "angularVelocity": -2.013980853353183e-17, + "velocityX": 0.7971166391560869, + "velocityY": 0.30581560947355857, + "timestamp": 13.893480214054104 }, { "x": 2.462606906890869, "y": 6.522137641906738, - "heading": 1.7665246335106923e-37, - "angularVelocity": 5.004564321614195e-18, - "velocityX": 0.35460803767440047, - "velocityY": 0.14989028326080325, - "timestamp": 14.668777372840111 + "heading": 1.18409559006069e-37, + "angularVelocity": -4.799101310235197e-17, + "velocityX": 0.3996127085808573, + "velocityY": 0.15015952745322653, + "timestamp": 13.991327896051267 }, { "x": 2.462606906890869, "y": 6.522137641906738, - "heading": -7.406725507450864e-38, - "angularVelocity": -2.5425466979238586e-36, - "velocityX": -6.628025513542663e-35, - "velocityY": 2.827272125617884e-34, - "timestamp": 14.767387017625603 + "heading": 7.445601745662792e-38, + "angularVelocity": -4.492041573884033e-37, + "velocityX": -6.88570869510922e-34, + "velocityY": -3.3118407683240263e-34, + "timestamp": 14.08917557804843 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.1.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.1.traj index 2132ff88..8d73ed2f 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.1.traj @@ -4,334 +4,316 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": -1.748186257820146e-28, - "velocityX": 0, - "velocityY": 0, + "angularVelocity": -7.2072188294937895e-31, + "velocityX": -1.1048793733131498e-32, + "velocityY": -5.425421859025623e-32, "timestamp": 0 }, { - "x": 1.3063204215499635, - "y": 5.5750430661668995, - "heading": -0.008999106947679492, - "angularVelocity": -0.11745777195159221, - "velocityX": 0.21335020071190533, - "velocityY": -0.20767599777855084, - "timestamp": 0.07661567898070912 - }, - { - "x": 1.3390123980907098, - "y": 5.543220800833044, - "heading": -0.026994200234242757, - "angularVelocity": -0.23487481322228163, - "velocityX": 0.42670086570142546, - "velocityY": -0.41534925701509284, - "timestamp": 0.15323135796141824 - }, - { - "x": 1.3880505863886679, - "y": 5.4954877356301095, - "heading": -0.053973713874731634, - "angularVelocity": -0.3521408934222409, - "velocityX": 0.6400542155377871, - "velocityY": -0.6230195417923181, - "timestamp": 0.22984703694212735 - }, - { - "x": 1.4534353620178047, - "y": 5.431844149183211, - "heading": -0.08991902767117105, - "angularVelocity": -0.4691639398438398, - "velocityX": 0.853412467425134, - "velocityY": -0.8306861899103606, - "timestamp": 0.30646271592283647 - }, - { - "x": 1.5351672628403068, - "y": 5.352290408430599, - "heading": -0.13480641974722582, - "angularVelocity": -0.5858773645717358, - "velocityX": 1.066777739575042, - "velocityY": -1.0383480486921168, - "timestamp": 0.38307839490354556 - }, - { - "x": 1.6332469708393256, - "y": 5.256827004871127, - "heading": -0.1886095129083549, - "angularVelocity": -0.7022465099717149, - "velocityX": 1.2801519130772017, - "velocityY": -1.2460034923528938, - "timestamp": 0.45969407388425465 - }, - { - "x": 1.7476752821470238, - "y": 5.145454581666917, - "heading": -0.25130200812811576, - "angularVelocity": -0.818272396306973, - "velocityX": 1.493536477822563, - "velocityY": -1.4536505413596468, - "timestamp": 0.5363097528649637 - }, - { - "x": 1.8784530688655205, - "y": 5.018173944363308, - "heading": -0.3228603099280309, - "angularVelocity": -0.9339903107164581, - "velocityX": 1.7069324253829399, - "velocityY": -1.66128707626095, - "timestamp": 0.6129254318456728 - }, - { - "x": 2.0255812507970647, - "y": 4.874986059010151, - "heading": -0.4032653297049151, - "angularVelocity": -1.049459077659022, - "velocityX": 1.9203403783475084, - "velocityY": -1.8689110021266238, - "timestamp": 0.6895411108263819 - }, - { - "x": 2.156432157972857, - "y": 4.747720771234375, - "heading": -0.47395861648198423, - "angularVelocity": -0.9226999968520704, - "velocityX": 1.7078868044203455, - "velocityY": -1.6610867310239321, - "timestamp": 0.766156789807091 - }, - { - "x": 2.270931566035856, - "y": 4.636361300878039, - "heading": -0.5358188421760707, - "angularVelocity": -0.8074094819002076, - "velocityX": 1.494464443178203, - "velocityY": -1.4534814779709395, - "timestamp": 0.8427724687878001 - }, - { - "x": 2.369078961091203, - "y": 4.540907071428243, - "heading": -0.5888499820546531, - "angularVelocity": -0.6921708527593945, - "velocityX": 1.2810353744995546, - "velocityY": -1.2458837496554307, - "timestamp": 0.9193881477685092 - }, - { - "x": 2.4508739613497577, - "y": 4.461357622382182, - "heading": -0.6330536039339079, - "angularVelocity": -0.5769526870004522, - "velocityX": 1.0676013226487002, - "velocityY": -1.0382920318954914, - "timestamp": 0.9960038267492183 - }, - { - "x": 2.516316276718558, - "y": 4.3977125949651334, - "heading": -0.6684300724749328, - "angularVelocity": -0.4617392808541994, - "velocityX": 0.8541634844585878, - "velocityY": -0.8307049969200896, - "timestamp": 1.0726195057299275 - }, - { - "x": 2.5654056965461667, - "y": 4.349971723161198, - "heading": -0.6949791200920894, - "angularVelocity": -0.34652238213663356, - "velocityX": 0.6407228968125727, - "velocityY": -0.6231214340263868, - "timestamp": 1.1492351847106366 - }, - { - "x": 2.5981420853275283, - "y": 4.3181348275927744, - "heading": -0.7127001553640687, - "angularVelocity": -0.2312977640935645, - "velocityX": 0.4272805405331163, - "velocityY": -0.41554021239755634, - "timestamp": 1.2258508636913457 - }, - { - "x": 2.6145253815163745, - "y": 4.302201811854818, - "heading": -0.7215924300890252, - "angularVelocity": -0.11606338040076232, - "velocityX": 0.21383738094203186, - "velocityY": -0.20796024906265667, - "timestamp": 1.3024665426720548 + "x": 1.3063416372973582, + "y": 5.5750227907482355, + "heading": -0.009007895171544168, + "angularVelocity": -0.12420787311314503, + "velocityX": 0.22568350897133957, + "velocityY": -0.21967610709029223, + "timestamp": 0.07252273906455099 + }, + { + "x": 1.339076097606074, + "y": 5.5431599730454195, + "heading": -0.02701891548605667, + "angularVelocity": -0.24834997335784384, + "velocityX": 0.4513682292059063, + "velocityY": -0.43934934220379995, + "timestamp": 0.14504547812910198 + }, + { + "x": 1.388178099815531, + "y": 5.4953660820311345, + "heading": -0.05401968965283247, + "angularVelocity": -0.3723076998340179, + "velocityX": 0.677056642410746, + "velocityY": -0.6590193866197215, + "timestamp": 0.217568217193653 + }, + { + "x": 1.4536480974194865, + "y": 5.431641407654605, + "heading": -0.08998953758011774, + "angularVelocity": -0.49598027310141063, + "velocityX": 0.9027513087406556, + "velocityY": -0.8786854329899796, + "timestamp": 0.29009095625820397 + }, + { + "x": 1.5354867341790108, + "y": 5.351986341420191, + "heading": -0.13490221177586337, + "angularVelocity": -0.6192909255093908, + "velocityX": 1.1284548517495026, + "velocityY": -1.0983460809932237, + "timestamp": 0.36261369532275495 + }, + { + "x": 1.6336948546750298, + "y": 5.2564014251813616, + "heading": -0.18872783549465025, + "angularVelocity": -0.7421896140863778, + "velocityX": 1.3541700404978287, + "velocityY": -1.317999257498918, + "timestamp": 0.4351364343873059 + }, + { + "x": 1.7482735602297292, + "y": 5.144887415721353, + "heading": -0.25143410509925246, + "angularVelocity": -0.8646428749583058, + "velocityX": 1.579900415134063, + "velocityY": -1.5376419988880228, + "timestamp": 0.507659173451857 + }, + { + "x": 1.87922458951427, + "y": 5.017445479597787, + "heading": -0.32298143653104017, + "angularVelocity": -0.9865503200055588, + "velocityX": 1.8056547639221738, + "velocityY": -1.7572686548722531, + "timestamp": 0.5801819125164079 + }, + { + "x": 2.026417113452321, + "y": 4.874266246183284, + "heading": -0.4026826239248217, + "angularVelocity": -1.0989820354525424, + "velocityX": 2.0296051395270776, + "velocityY": -1.9742667646213359, + "timestamp": 0.6527046515809589 + }, + { + "x": 2.1572394289562786, + "y": 4.747014797378468, + "heading": -0.4735071808339005, + "angularVelocity": -0.9765841420579419, + "velocityX": 1.8038799580839464, + "velocityY": -1.7546420673865035, + "timestamp": 0.7252273906455099 + }, + { + "x": 2.27168995904481, + "y": 4.635690037684074, + "heading": -0.535478944048102, + "angularVelocity": -0.8545149288832774, + "velocityX": 1.578133031994185, + "velocityY": -1.5350324757486038, + "timestamp": 0.7977501297100609 + }, + { + "x": 2.369767924446142, + "y": 4.540291261559593, + "heading": -0.5886064902849578, + "angularVelocity": -0.732563978168669, + "velocityX": 1.35237536069838, + "velocityY": -1.3154326126526381, + "timestamp": 0.8702728687746119 + }, + { + "x": 2.4514728066095985, + "y": 4.460817939321001, + "heading": -0.6328935848970949, + "angularVelocity": -0.6106649470681784, + "velocityX": 1.1266105392220715, + "velocityY": -1.0958400532524892, + "timestamp": 0.9427956078391628 + }, + { + "x": 2.5168042340609067, + "y": 4.397269668318764, + "heading": -0.6683418026498548, + "angularVelocity": -0.48878763005849607, + "velocityX": 0.9008405955692155, + "velocityY": -0.8762530458976289, + "timestamp": 1.015318346903714 + }, + { + "x": 2.5657619428441123, + "y": 4.349646151783931, + "heading": -0.6949516025618677, + "angularVelocity": -0.36691664235512766, + "velocityX": 0.6750670122861077, + "velocityY": -0.6566701306241847, + "timestamp": 1.087841085968265 + }, + { + "x": 2.5983457601971756, + "y": 4.317947187379743, + "heading": -0.7127228546140986, + "angularVelocity": -0.24504386184805346, + "velocityX": 0.4492910468254882, + "velocityY": -0.4370900053275183, + "timestamp": 1.160363825032816 }, { "x": 2.614555597305298, "y": 4.302172660827637, "heading": -0.7216551150961179, - "angularVelocity": -0.0008181742381802744, - "velocityX": 0.0003943804906386705, - "velocityY": -0.0003804830723595261, - "timestamp": 1.3790822216527638 - }, - { - "x": 2.5982259689280283, - "y": 4.318054102166664, - "heading": -0.712883613809901, - "angularVelocity": 0.11446303533186522, - "velocityX": -0.213092236881309, - "velocityY": 0.20724365439564615, - "timestamp": 1.4557139589685733 - }, - { - "x": 2.565536606931669, - "y": 4.349846333909782, - "heading": -0.695279424271666, - "angularVelocity": 0.2297245257202117, - "velocityX": -0.426577330596217, - "velocityY": 0.4148702979331944, - "timestamp": 1.5323456962843827 - }, - { - "x": 2.5164876982610407, - "y": 4.39754963766033, - "heading": -0.6688439584276578, - "angularVelocity": 0.34496759102452434, - "velocityX": -0.640059984788952, - "velocityY": 0.6225006162319799, - "timestamp": 1.608977433600192 - }, - { - "x": 2.4510795013292355, - "y": 4.461164391501365, - "heading": -0.6335783197571896, - "angularVelocity": 0.4601962565606419, - "velocityX": -0.8535392668397416, - "velocityY": 0.8301358682967486, - "timestamp": 1.6856091709160015 - }, - { - "x": 2.369312351807588, - "y": 4.540691078953442, - "heading": -0.5894828757685662, - "angularVelocity": 0.5754201263263005, - "velocityX": -1.0670141685730552, - "velocityY": 1.0377774300260596, - "timestamp": 1.762240908231811 - }, - { - "x": 2.2711866760947044, - "y": 4.636130298345392, - "heading": -0.5365564475452359, - "angularVelocity": 0.6906593802750026, - "velocityX": -1.28048350605026, - "velocityY": 1.2454267996017627, - "timestamp": 1.8388726455476203 - }, - { - "x": 2.1567030158826075, - "y": 4.747482768262345, - "heading": -0.47479485869558546, - "angularVelocity": 0.8059531337295586, - "velocityX": -1.4939457747586862, - "velocityY": 1.4530855462992425, - "timestamp": 1.9155043828634297 - }, - { - "x": 2.0258620666320337, - "y": 4.874749320539599, - "heading": -0.4041884988158368, - "angularVelocity": 0.9213722974191146, - "velocityX": -1.7073989681051045, - "velocityY": 1.6607551479679548, - "timestamp": 1.9921361201792391 - }, - { - "x": 1.8786647355617998, - "y": 5.01793086600198, - "heading": -0.3247184712590107, - "angularVelocity": 1.0370380514754407, - "velocityX": -1.9208403227681095, - "velocityY": 1.8684366353759239, - "timestamp": 2.0687678574950485 - }, - { - "x": 1.7478414567124814, - "y": 5.145267849282916, - "heading": -0.25271823102965235, - "angularVelocity": 0.9395616327984198, - "velocityX": -1.7071683803424498, - "velocityY": 1.6616742327261775, - "timestamp": 2.145399594810858 - }, - { - "x": 1.6333728466995314, - "y": 5.256688511191179, - "heading": -0.18963629669305965, - "angularVelocity": 0.8231828815865951, - "velocityX": -1.4937493791208316, - "velocityY": 1.4539754132374219, - "timestamp": 2.222031332126668 - }, - { - "x": 1.535258100629332, - "y": 5.352192443129829, - "heading": -0.13550580636159482, - "angularVelocity": 0.70637169709355, - "velocityX": -1.2803408812977723, - "velocityY": 1.2462712617938094, - "timestamp": 2.2986630694424774 - }, - { - "x": 1.4534965485667528, - "y": 5.431779376588513, - "heading": -0.09035761814526168, - "angularVelocity": 0.5891578320466108, - "velocityX": -1.0669411257422734, - "velocityY": 1.0385636064447687, - "timestamp": 2.375294806758287 - }, - { - "x": 1.3880876748821938, - "y": 5.495449138780266, - "heading": -0.05421785896865615, - "angularVelocity": 0.4716030256909087, - "velocityX": -0.8535480988559686, - "velocityY": 0.8308536963253964, - "timestamp": 2.4519265440740967 - }, - { - "x": 1.339031128559986, - "y": 5.543201612325557, - "heading": -0.02710586002264422, - "angularVelocity": 0.35379595843554007, - "velocityX": -0.6401596525218931, - "velocityY": 0.6231422544707841, - "timestamp": 2.5285582813899063 - }, - { - "x": 1.3063267261975056, - "y": 5.575036700785004, - "heading": -0.009032648967396358, - "angularVelocity": 0.23584498657035882, - "velocityX": -0.4267735994864031, - "velocityY": 0.4154295541377912, - "timestamp": 2.605190018705716 + "angularVelocity": -0.12316496311606469, + "velocityX": 0.22351385671724336, + "velocityY": -0.21751145579268175, + "timestamp": 1.2328865640973672 + }, + { + "x": 2.6122418058900085, + "y": 4.3044120467003895, + "heading": -0.7206083682746678, + "angularVelocity": 0.01292691365951148, + "velocityX": -0.028574418607166113, + "velocityY": 0.027655539271243604, + "timestamp": 1.3138607895750445 + }, + { + "x": 2.589515477392911, + "y": 4.326503828401164, + "heading": -0.7085432954922883, + "angularVelocity": 0.148998927882974, + "velocityX": -0.2806612642842681, + "velocityY": 0.2728248596434546, + "timestamp": 1.3948350150527218 + }, + { + "x": 2.54637684601469, + "y": 4.3684483333856905, + "heading": -0.6854613533337227, + "angularVelocity": 0.2850529538548107, + "velocityX": -0.5327452176745692, + "velocityY": 0.5179982239657722, + "timestamp": 1.475809240530399 + }, + { + "x": 2.4828262685453337, + "y": 4.430246041385043, + "heading": -0.6513636060932391, + "angularVelocity": 0.42109383620888585, + "velocityX": -0.784824764848103, + "velocityY": 0.7631775127791823, + "timestamp": 1.5567834660080764 + }, + { + "x": 2.398864238908538, + "y": 4.511897605460961, + "heading": -0.6062501465002338, + "angularVelocity": 0.5571335733911974, + "velocityX": -1.0368982122572787, + "velocityY": 1.0083648666494986, + "timestamp": 1.6377576914857537 + }, + { + "x": 2.2944914266815206, + "y": 4.613403885366389, + "heading": -0.5501188636580988, + "angularVelocity": 0.6931993793206396, + "velocityX": -1.2889633906498084, + "velocityY": 1.2535628381323076, + "timestamp": 1.718731916963431 + }, + { + "x": 2.1697087770291006, + "y": 4.73476601037198, + "heading": -0.48296289489208905, + "angularVelocity": 0.8293499365978624, + "velocityX": -1.5410168966266635, + "velocityY": 1.4987747556657698, + "timestamp": 1.7997061424411083 + }, + { + "x": 2.0245178868987646, + "y": 4.87598561985106, + "heading": -0.4047647421109401, + "angularVelocity": 0.9657165884553096, + "velocityX": -1.7930506809269309, + "velocityY": 1.7440069188183338, + "timestamp": 1.8806803679187856 + }, + { + "x": 1.8612798092271896, + "y": 5.034863868368553, + "heading": -0.31505899294827, + "angularVelocity": 1.1078309009257712, + "velocityX": -2.0159263853239904, + "velocityY": 1.9620842012417825, + "timestamp": 1.9616545933964629 + }, + { + "x": 1.7184500483895917, + "y": 5.1738844559195085, + "heading": -0.23644393225782434, + "angularVelocity": 0.9708652380027345, + "velocityX": -1.7638916580562496, + "velocityY": 1.7168498584691285, + "timestamp": 2.04262881887414 + }, + { + "x": 1.5960267405860193, + "y": 5.293046114041946, + "heading": -0.168968478820667, + "angularVelocity": 0.8332954472751476, + "velocityX": -1.5118799479886125, + "velocityY": 1.4715998506873489, + "timestamp": 2.1236030443518175 + }, + { + "x": 1.4940086408037947, + "y": 5.392348179003114, + "heading": -0.1126782173092116, + "angularVelocity": 0.6951627036802206, + "velocityX": -1.2598836133403888, + "velocityY": 1.2263416460657963, + "timestamp": 2.2045772698294948 + }, + { + "x": 1.4123948378953157, + "y": 5.471790254952654, + "heading": -0.06761272810355828, + "angularVelocity": 0.5565411578785837, + "velocityX": -1.0078985310085185, + "velocityY": 0.9810785528472173, + "timestamp": 2.285551495307172 + }, + { + "x": 1.3511847156576737, + "y": 5.531372091014257, + "heading": -0.03380194743073204, + "angularVelocity": 0.417549911387026, + "velocityX": -0.7559210585414546, + "velocityY": 0.7358123614043836, + "timestamp": 2.3665257207848494 + }, + { + "x": 1.3103779403872662, + "y": 5.571093500047757, + "heading": -0.011263323026729973, + "angularVelocity": 0.27834319218308196, + "velocityX": -0.50394770718331, + "velocityY": 0.4905438588537352, + "timestamp": 2.4474999462625266 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -2.143037766767562e-26, - "angularVelocity": 0.11787086245509872, - "velocityX": -0.21338776476374371, - "velocityY": 0.20771554341884355, - "timestamp": 2.6818217560215256 + "heading": 4.4390879396333275e-31, + "angularVelocity": 0.13909763212046558, + "velocityX": -0.25197510938629825, + "velocityY": 0.24527315422336363, + "timestamp": 2.528474171740204 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.6489738584934847e-26, - "angularVelocity": 6.447188733213427e-26, - "velocityX": -2.3207888161938065e-29, - "velocityY": -7.774803790845182e-27, - "timestamp": 2.7584534933373352 + "heading": 2.494908675164405e-31, + "angularVelocity": -2.400971097599731e-30, + "velocityX": 9.117669624782265e-34, + "velocityY": -1.69916768015881e-32, + "timestamp": 2.6094483972178812 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.2.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.2.traj index 094d0061..0587ab22 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.2.traj @@ -3,227 +3,209 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.6489738584934847e-26, - "angularVelocity": 6.447188733213427e-26, - "velocityX": -2.3207888161938065e-29, - "velocityY": -7.774803790845182e-27, + "heading": 2.494908675164405e-31, + "angularVelocity": -2.400971097599731e-30, + "velocityX": 9.117669624782265e-34, + "velocityY": -1.69916768015881e-32, "timestamp": 0 }, { - "x": 1.3242756726096836, - "y": 5.5904571777752485, - "heading": -6.485537692324558e-18, - "angularVelocity": -6.918907801272556e-17, - "velocityX": 0.3659270961745246, - "velocityY": -0.005303363934441593, - "timestamp": 0.09373785630439047 - }, - { - "x": 1.3928781149651148, - "y": 5.58946292585347, - "heading": -1.1202216291674618e-17, - "angularVelocity": -5.031804212956606e-17, - "velocityX": 0.7318541845207394, - "velocityY": -0.010606727755427862, - "timestamp": 0.18747571260878093 - }, - { - "x": 1.4957817770305903, - "y": 5.587971547992073, - "heading": -1.4372763095426617e-17, - "angularVelocity": -3.382497812599112e-17, - "velocityX": 1.0977812611239222, - "velocityY": -0.015910091406222935, - "timestamp": 0.2812135689131714 - }, - { - "x": 1.6329866569713576, - "y": 5.585983044217648, - "heading": -1.5623455517606444e-17, - "angularVelocity": -1.3343083564783124e-17, - "velocityX": 1.4637083181538741, - "velocityY": -0.021213454773344104, - "timestamp": 0.37495142521756186 - }, - { - "x": 1.8044927511173796, - "y": 5.583497414583386, - "heading": -1.5212462606875703e-17, - "angularVelocity": 4.38274958491921e-18, - "velocityX": 1.8296353360316953, - "velocityY": -0.02651681757303531, - "timestamp": 0.46868928152195233 - }, - { - "x": 2.0103000484542934, - "y": 5.580514659248916, - "heading": -1.3020782793974884e-17, - "angularVelocity": 2.337996988615278e-17, - "velocityX": 2.1955622364077554, - "velocityY": -0.031820178669779105, - "timestamp": 0.5624271378263428 - }, - { - "x": 2.181845878009789, - "y": 5.578028453731261, - "heading": -1.0808447718251693e-17, - "angularVelocity": 2.360324840579071e-17, - "velocityX": 1.8300592334557981, - "velocityY": -0.026522961098165376, - "timestamp": 0.6561649941307333 - }, - { - "x": 2.3190905043488854, - "y": 5.576039373914184, - "heading": -1.0180400900611551e-17, - "angularVelocity": 6.701278506879843e-18, - "velocityX": 1.4641323328076266, - "velocityY": -0.021219599997477887, - "timestamp": 0.7499028504351237 - }, - { - "x": 2.4220339164763502, - "y": 5.574547419957035, - "heading": -1.1310869476605041e-17, - "angularVelocity": -1.2059406429055699e-17, - "velocityX": 1.0982053148617765, - "velocityY": -0.015916237196800742, - "timestamp": 0.8436407067395142 - }, - { - "x": 2.490676110725689, - "y": 5.573552591912956, - "heading": -5.948170117055629e-18, - "angularVelocity": 5.721104182574728e-17, - "velocityX": 0.7322782578015892, - "velocityY": -0.010612873829241372, - "timestamp": 0.9373785630439047 - }, - { - "x": 2.5250170852633893, - "y": 5.573054889808518, - "heading": -2.1807385415410986e-18, - "angularVelocity": 4.0191949957409276e-17, - "velocityX": 0.36635118118139864, - "velocityY": -0.0053095101781998125, - "timestamp": 1.0311164193482951 + "x": 1.3242823472826057, + "y": 5.590457081039532, + "heading": -3.1525835471969304e-19, + "angularVelocity": -3.554932323947037e-18, + "velocityX": 0.3868644475376546, + "velocityY": -0.00560680796813751, + "timestamp": 0.08868195678333235 + }, + { + "x": 1.3928981385918453, + "y": 5.589462635652003, + "heading": -9.247304393859629e-19, + "angularVelocity": -6.8725601776619685e-18, + "velocityX": 0.7737288823800034, + "velocityY": -0.011213615752282564, + "timestamp": 0.1773639135666647 + }, + { + "x": 1.4958218231349187, + "y": 5.587970967605794, + "heading": -1.5461935946313141e-18, + "angularVelocity": -7.007774498748477e-18, + "velocityX": 1.1605932962726162, + "velocityY": -0.016820423232804093, + "timestamp": 0.26604587034999705 + }, + { + "x": 1.6330533972641115, + "y": 5.585982076953771, + "heading": -2.4826620193785056e-18, + "angularVelocity": -1.0559852970262818e-17, + "velocityX": 1.5474576690326838, + "velocityY": -0.022427230117193615, + "timestamp": 0.3547278271333294 + }, + { + "x": 1.8045928505515578, + "y": 5.5834959638470645, + "heading": -3.548549667861495e-18, + "angularVelocity": -1.201921661569969e-17, + "velocityX": 1.934321924205522, + "velocityY": -0.028034035297397054, + "timestamp": 0.44340978391666175 + }, + { + "x": 2.0104398981448357, + "y": 5.5805126324140275, + "heading": -4.504226548304647e-18, + "angularVelocity": -1.0776452337176765e-17, + "velocityX": 2.321182967310963, + "velocityY": -0.033640793925260974, + "timestamp": 0.5320917406999941 + }, + { + "x": 2.1819790701409207, + "y": 5.578026523384064, + "heading": -4.574305172786567e-18, + "angularVelocity": -7.902241563425968e-19, + "velocityX": 1.934318752293539, + "velocityY": -0.02803398932702808, + "timestamp": 0.6207736974833264 + }, + { + "x": 2.31921035343729, + "y": 5.576037636947067, + "heading": -3.929985868072649e-18, + "angularVelocity": 7.2655061760547e-18, + "velocityX": 1.547454389528782, + "velocityY": -0.022427182587500143, + "timestamp": 0.7094556542666588 + }, + { + "x": 2.422133743893467, + "y": 5.574545973163046, + "heading": -2.934544233389305e-18, + "angularVelocity": 1.1224849685211679e-17, + "velocityX": 1.1605899800749648, + "velocityY": -0.016820375171309878, + "timestamp": 0.7981376110499911 + }, + { + "x": 2.490749239479307, + "y": 5.573551532061422, + "heading": -1.705878005512682e-18, + "angularVelocity": 1.385474872728055e-17, + "velocityX": 0.7737255477287375, + "velocityY": -0.011213567423341012, + "timestamp": 0.8868195678333235 }, { "x": 2.525056838989258, "y": 5.573054313659668, - "heading": -2.8826489417868528e-33, - "angularVelocity": 2.326435027113688e-17, - "velocityX": 0.0004240928242990202, - "velocityY": -0.000006146357055788897, - "timestamp": 1.1248542756526856 - }, - { - "x": 2.4907821260381073, - "y": 5.573551055438095, - "heading": 8.600966160221058e-18, - "angularVelocity": 9.173661299705183e-17, - "velocityX": -0.3655736854951678, - "velocityY": 0.00529824197033788, - "timestamp": 1.2186102382722352 - }, - { - "x": 2.422192939812278, - "y": 5.57454511523942, - "heading": 1.5616353080474726e-17, - "angularVelocity": 7.482552449285854e-17, - "velocityX": -0.7315714559878368, - "velocityY": 0.010602630184298141, - "timestamp": 1.3123662008917854 - }, - { - "x": 2.3192892814125363, - "y": 5.57603649304769, - "heading": 2.109129083859237e-17, - "angularVelocity": 5.839395174918292e-17, - "velocityX": -1.0975692147397424, - "velocityY": 0.015907018228100082, - "timestamp": 1.4061221635113355 - }, - { - "x": 2.182071152673635, - "y": 5.578025188836313, - "heading": 2.4503256173748732e-17, - "angularVelocity": 3.639115358132533e-17, - "velocityX": -1.4635669539221987, - "velocityY": 0.021211405988282927, - "timestamp": 1.4998781261308856 - }, - { - "x": 2.010538557265611, - "y": 5.5805112025521, - "heading": 2.631777380152167e-17, - "angularVelocity": 1.9353493618514127e-17, - "velocityX": -1.8295646539600892, - "velocityY": 0.026515793181145452, - "timestamp": 1.5936340887504357 - }, - { - "x": 1.8046915062028241, - "y": 5.58349453403542, - "heading": 2.6479996589742425e-17, - "angularVelocity": 1.7308836974551526e-18, - "velocityX": -2.1955622365189336, - "velocityY": 0.03182017867138977, - "timestamp": 1.6873900513699858 - }, - { - "x": 1.6331191680538633, - "y": 5.585981123740855, - "heading": 1.776473605055726e-17, - "angularVelocity": -9.295501651565233e-17, - "velocityX": -1.82998855142187, - "velocityY": 0.026521936706821576, - "timestamp": 1.7811460139895359 - }, - { - "x": 1.495861285585166, - "y": 5.587970395678386, - "heading": 1.0702828905507157e-17, - "angularVelocity": -7.532110300046076e-17, - "velocityX": -1.4639909685910264, - "velocityY": 0.021217551212635195, - "timestamp": 1.874901976609086 - }, - { - "x": 1.3929178697919677, - "y": 5.589462349688662, - "heading": 5.2389574400340815e-18, - "angularVelocity": -5.827717130868218e-17, - "velocityX": -1.097993268485135, - "velocityY": 0.01591316401878714, - "timestamp": 1.968657939228636 - }, - { - "x": 1.3242889243407625, - "y": 5.590456985718543, - "heading": 1.764781469065966e-18, - "angularVelocity": -3.705591096010782e-17, - "velocityX": -0.7319955292724558, - "velocityY": 0.010608776258166279, - "timestamp": 2.062413901848186 + "heading": 0, + "angularVelocity": 1.9235908491289617e-17, + "velocityX": 0.3868611017883933, + "velocityY": -0.005606759478353257, + "timestamp": 0.9755015246166558 + }, + { + "x": 2.5176787496862993, + "y": 5.573161243955685, + "heading": 2.6679509724403924e-18, + "angularVelocity": 2.5453436150449368e-17, + "velocityX": -0.07039024589473145, + "velocityY": 0.0010201624731186972, + "timestamp": 1.0803184521493767 + }, + { + "x": 2.4623729801708616, + "y": 5.573962787832815, + "heading": 6.066089004278022e-18, + "angularVelocity": 3.2419744709429903e-17, + "velocityX": -0.5276415824931802, + "velocityY": 0.007647084263940974, + "timestamp": 1.1851353796820976 + }, + { + "x": 2.3591395323793796, + "y": 5.575458945262994, + "heading": 1.0152873197492895e-17, + "angularVelocity": 3.898973466799139e-17, + "velocityX": -0.9848929006171766, + "velocityY": 0.014274005787013901, + "timestamp": 1.2899523072148185 + }, + { + "x": 2.207978410184721, + "y": 5.577649716190093, + "heading": 1.5606892400662525e-17, + "angularVelocity": 5.2033763358184983e-17, + "velocityX": -1.4421441817922973, + "velocityY": 0.020900926774588543, + "timestamp": 1.3947692347475398 + }, + { + "x": 2.0088896252054647, + "y": 5.580535100445723, + "heading": 1.7204191476600093e-18, + "angularVelocity": -1.324831168006478e-16, + "velocityX": -1.8993953521210223, + "velocityY": 0.02752784615567168, + "timestamp": 1.499586162280261 + }, + { + "x": 1.769251248155021, + "y": 5.584008168003288, + "heading": 6.282149498472273e-20, + "angularVelocity": -1.5814217146918653e-17, + "velocityX": -2.2862564539075523, + "velocityY": 0.03313460563399779, + "timestamp": 1.6044030898129824 + }, + { + "x": 1.577540533889042, + "y": 5.586786622232322, + "heading": -2.476430565711122e-19, + "angularVelocity": -2.961969587010803e-18, + "velocityX": -1.829005283580098, + "velocityY": 0.02650768625293313, + "timestamp": 1.7092200173457037 + }, + { + "x": 1.4337574940262061, + "y": 5.588870462964437, + "heading": -3.350775678748249e-19, + "angularVelocity": -8.341640359227092e-19, + "velocityX": -1.3717540024052963, + "velocityY": 0.01988076526536314, + "timestamp": 1.814036944878425 + }, + { + "x": 1.3379021324393994, + "y": 5.590259690143503, + "heading": -2.0793104080618583e-19, + "angularVelocity": 1.2130342880824037e-18, + "velocityX": -0.914502684281442, + "velocityY": 0.013253843742292293, + "timestamp": 1.9188538724111464 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -1.882241835148467e-17, - "velocityX": -0.3659977705035494, - "velocityY": 0.00530438821411795, - "timestamp": 2.1561698644677363 + "heading": -1.4049323971929142e-31, + "angularVelocity": 1.9837543963605952e-18, + "velocityX": -0.4572513476830733, + "velocityY": 0.006626921951471195, + "timestamp": 2.0236707999438677 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.5626059218246172e-29, - "velocityY": -2.5406263809505943e-29, - "timestamp": 2.2499258270872864 + "heading": 4.571690374498227e-31, + "angularVelocity": 5.701965460392164e-30, + "velocityX": 1.9882633246635272e-32, + "velocityY": -1.2353765948799823e-31, + "timestamp": 2.128487727476589 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.3.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.3.traj index 0ce184f8..caa7ef28 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.3.traj @@ -3,335 +3,317 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.5626059218246172e-29, - "velocityY": -2.5406263809505943e-29, + "heading": 4.571690374498227e-31, + "angularVelocity": 5.701965460392164e-30, + "velocityX": 1.9882633246635272e-32, + "velocityY": -1.2353765948799823e-31, "timestamp": 0 }, { - "x": 1.3072041550588713, - "y": 5.60554242334907, - "heading": 0.009976426632135488, - "angularVelocity": 0.13081140582840126, - "velocityX": 0.22591674199036743, - "velocityY": 0.19128015517323346, - "timestamp": 0.07626572447004332 - }, - { - "x": 1.341663665671124, - "y": 5.634718250195267, - "heading": 0.02992724315217698, - "angularVelocity": 0.26159610576452386, - "velocityX": 0.4518348296001005, - "velocityY": 0.3825549033430322, - "timestamp": 0.15253144894008663 - }, - { - "x": 1.3933533134136329, - "y": 5.678481259628573, - "heading": 0.05984264331407307, - "angularVelocity": 0.3922522257112071, - "velocityX": 0.6777572508972288, - "velocityY": 0.5738227722565116, - "timestamp": 0.22879717341012995 - }, - { - "x": 1.4622736330550405, - "y": 5.7368307766764, - "heading": 0.09970739234331524, - "angularVelocity": 0.5227085865313741, - "velocityX": 0.9036866841075589, - "velocityY": 0.7650817908955898, - "timestamp": 0.30506289788017327 - }, - { - "x": 1.548425323078623, - "y": 5.809765941188483, - "heading": 0.1495038423997354, - "angularVelocity": 0.652933547987283, - "velocityX": 1.1296252758655412, - "velocityY": 0.9563295312013731, - "timestamp": 0.3813286223502166 - }, - { - "x": 1.6518091876527043, - "y": 5.897285689503854, - "heading": 0.20921528267987785, - "angularVelocity": 0.7829393965514132, - "velocityX": 1.3555744119188056, - "velocityY": 1.1475633247313204, - "timestamp": 0.4575943468202599 - }, - { - "x": 1.7724260670196361, - "y": 5.999388766272788, - "heading": 0.27882903202196435, - "angularVelocity": 0.9127789690876109, - "velocityX": 1.5815345652717379, - "velocityY": 1.3387806580529145, - "timestamp": 0.5338600712903032 - }, - { - "x": 1.910276772460734, - "y": 6.11607377416316, - "heading": 0.3583384554068513, - "angularVelocity": 1.042531542921605, - "velocityX": 1.807505356185654, - "velocityY": 1.5299796694859533, - "timestamp": 0.6101257957603465 - }, - { - "x": 2.065362059261307, - "y": 6.247339244509118, - "heading": 0.4477430189105342, - "angularVelocity": 1.172277115920654, - "velocityX": 2.033486050437564, - "velocityY": 1.7211594236996055, - "timestamp": 0.6863915202303899 - }, - { - "x": 2.203284027987246, - "y": 6.363982908806699, - "heading": 0.5270469936467461, - "angularVelocity": 1.0398376893804102, - "velocityX": 1.8084397630070208, - "velocityY": 1.5294375694163855, - "timestamp": 0.7626572447004332 - }, - { - "x": 2.323970561231782, - "y": 6.4660479017472055, - "heading": 0.5964497874037734, - "angularVelocity": 0.9100129083298684, - "velocityX": 1.5824478694623139, - "velocityY": 1.3382813001795226, - "timestamp": 0.8389229691704765 - }, - { - "x": 2.427421209360281, - "y": 6.553534871396325, - "heading": 0.6559501239234673, - "angularVelocity": 0.7801713929154328, - "velocityX": 1.3564500802865749, - "velocityY": 1.147133528411577, - "timestamp": 0.9151886936405198 - }, - { - "x": 2.5136355947658133, - "y": 6.6264442966918295, - "heading": 0.7055475231333362, - "angularVelocity": 0.6503235827654283, - "velocityX": 1.1304473399101456, - "velocityY": 0.955992036499409, - "timestamp": 0.9914544181105631 - }, - { - "x": 2.582613392420247, - "y": 6.6847765351095845, - "heading": 0.7452421346528295, - "angularVelocity": 0.5204777344326409, - "velocityX": 0.9044403377486854, - "velocityY": 0.7648552318285594, - "timestamp": 1.0677201425806064 - }, - { - "x": 2.634354338508759, - "y": 6.728531844563373, - "heading": 0.7750344345644219, - "angularVelocity": 0.3906381285168185, - "velocityX": 0.6784298764341418, - "velocityY": 0.5737218089360453, - "timestamp": 1.1439858670506498 - }, - { - "x": 2.6688582435593817, - "y": 6.757710392622863, - "heading": 0.7949250057401149, - "angularVelocity": 0.26080616524767886, - "velocityX": 0.4524169307537305, - "velocityY": 0.3825905832253757, - "timestamp": 1.220251591520693 - }, - { - "x": 2.686125003210816, - "y": 6.772312259170968, - "heading": 0.8049144291757789, - "angularVelocity": 0.13098182002535327, - "velocityX": 0.22640261668277428, - "velocityY": 0.19146040494338912, - "timestamp": 1.2965173159907364 + "x": 1.3072266031673745, + "y": 5.605560947932051, + "heading": 0.009988018032587274, + "angularVelocity": 0.1383542475734168, + "velocityX": 0.2389771940088317, + "velocityY": 0.20233155967170455, + "timestamp": 0.07219162553929692 + }, + { + "x": 1.3417310637547577, + "y": 5.63477381293564, + "heading": 0.029960721397497427, + "angularVelocity": 0.2766623305087681, + "velocityX": 0.4779565542351512, + "velocityY": 0.40465725470744984, + "timestamp": 0.14438325107859384 + }, + { + "x": 1.393488225984334, + "y": 5.678592353413093, + "heading": 0.05990698047800166, + "angularVelocity": 0.4148162457460571, + "velocityX": 0.7169413604826331, + "velocityY": 0.606975395693467, + "timestamp": 0.21657487661789077 + }, + { + "x": 1.4624987017850304, + "y": 5.737015858733095, + "heading": 0.09981017735525757, + "angularVelocity": 0.5527399692025371, + "velocityX": 0.9559346431831991, + "velocityY": 0.8092836929987566, + "timestamp": 0.2887665021571877 + }, + { + "x": 1.54876329298479, + "y": 5.810043407637518, + "heading": 0.1496511403376383, + "angularVelocity": 0.6903981259608908, + "velocityX": 1.1949390328222467, + "velocityY": 1.0115792290159562, + "timestamp": 0.3609581276964846 + }, + { + "x": 1.6522829608961203, + "y": 5.897673825591776, + "heading": 0.20941128647918397, + "angularVelocity": 0.8277988713391718, + "velocityX": 1.4339567385834835, + "velocityY": 1.2138584953540306, + "timestamp": 0.43314975323578153 + }, + { + "x": 1.7730588392541233, + "y": 5.999905624129209, + "heading": 0.2790750774652724, + "angularVelocity": 0.9649843796382769, + "velocityX": 1.6729901488685341, + "velocityY": 1.4161171434188378, + "timestamp": 0.5053413787750785 + }, + { + "x": 1.911092570994838, + "y": 6.1167366962116585, + "heading": 0.35862892849655154, + "angularVelocity": 1.1019817109951129, + "velocityX": 1.9120463171400826, + "velocityY": 1.6183466047443176, + "timestamp": 0.5775330043143754 + }, + { + "x": 2.0662405750494774, + "y": 6.247971693585017, + "heading": 0.4478813931179114, + "angularVelocity": 1.2363271218036194, + "velocityX": 2.1491135972568896, + "velocityY": 1.8178700977147564, + "timestamp": 0.6497246298536723 + }, + { + "x": 2.204132997305319, + "y": 6.36460684022272, + "heading": 0.5272152358459444, + "angularVelocity": 1.0989341510922093, + "velocityX": 1.9100888950161639, + "velocityY": 1.6156326411317472, + "timestamp": 0.7219162553929692 + }, + { + "x": 2.3247684211138937, + "y": 6.466643771235888, + "heading": 0.5966353508762373, + "angularVelocity": 0.9616089748867341, + "velocityX": 1.6710445693293852, + "velocityY": 1.4134178341442696, + "timestamp": 0.7941078809322661 + }, + { + "x": 2.428146136331826, + "y": 6.5540833745423095, + "heading": 0.6561424196084651, + "angularVelocity": 0.8242932374446845, + "velocityX": 1.4319904067208047, + "velocityY": 1.2112153266137329, + "timestamp": 0.8662995064715631 + }, + { + "x": 2.51426563036811, + "y": 6.626926253781894, + "heading": 0.705736875442574, + "angularVelocity": 0.686983503468318, + "velocityX": 1.192929143690227, + "velocityY": 1.0090211807166112, + "timestamp": 0.93849113201086 + }, + { + "x": 2.5831264968268224, + "y": 6.6851728424603465, + "heading": 0.7454193798278042, + "angularVelocity": 0.5496829319015093, + "velocityX": 0.95386225125429, + "velocityY": 0.8068330397501333, + "timestamp": 1.010682757550157 + }, + { + "x": 2.6347284176402783, + "y": 6.72882344947858, + "heading": 0.7751907373365541, + "angularVelocity": 0.4123935052894287, + "velocityX": 0.7147909529377248, + "velocityY": 0.6046491776859777, + "timestamp": 1.0828743830894538 + }, + { + "x": 2.669071164543137, + "y": 6.75787827866861, + "heading": 0.7950517689437672, + "angularVelocity": 0.27511545084049754, + "velocityX": 0.4757164926845912, + "velocityY": 0.4024681391074233, + "timestamp": 1.1550660086287508 }, { "x": 2.68615460395813, "y": 6.772337436676025, "heading": 0.8050032485420815, - "angularVelocity": 0.0011646034399446482, - "velocityX": 0.0003881255345023293, - "velocityY": 0.00033012790587399484, - "timestamp": 1.3727830404607797 - }, - { - "x": 2.668940238370334, - "y": 6.757780008152215, - "heading": 0.7951880324492147, - "angularVelocity": -0.12867188925922995, - "velocityX": -0.22567052234080015, - "velocityY": -0.1908395916273825, - "timestamp": 1.4490640068049814 - }, - { - "x": 2.6344820969778255, - "y": 6.728639813103283, - "heading": 0.7754676749092158, - "angularVelocity": -0.2585226492096862, - "velocityX": -0.4517265968129291, - "velocityY": -0.3820113508147273, - "timestamp": 1.525344973149183 - }, - { - "x": 2.582780444824575, - "y": 6.6849166026467195, - "heading": 0.7458414179763078, - "angularVelocity": -0.3883833453977379, - "velocityX": -0.6777791967096236, - "velocityY": -0.5731863727099495, - "timestamp": 1.6016259394933847 - }, - { - "x": 2.513835599527289, - "y": 6.6266100250455136, - "heading": 0.7063091354285856, - "angularVelocity": -0.518245696283539, - "velocityX": -0.9038276328322399, - "velocityY": -0.7643660056769808, - "timestamp": 1.6779069058375864 - }, - { - "x": 2.427647916545295, - "y": 6.553719605348423, - "heading": 0.6568717276359699, - "angularVelocity": -0.6480962438334487, - "velocityX": -1.1298714090855582, - "velocityY": -0.9555518649841811, - "timestamp": 1.754187872181788 - }, - { - "x": 2.3242177842468106, - "y": 6.466244710155765, - "heading": 0.5975314416664378, - "angularVelocity": -0.7779173336036685, - "velocityX": -1.3559100940527828, - "velocityY": -1.1467460278003494, - "timestamp": 1.8304688385259897 - }, - { - "x": 2.2035456412981804, - "y": 6.364184497615964, - "heading": 0.5282916867852259, - "angularVelocity": -0.907693729851007, - "velocityX": -1.5819430283540434, - "velocityY": -1.3379512268110498, - "timestamp": 1.9067498048701914 - }, - { - "x": 2.0656320318873616, - "y": 6.247537861954298, - "heading": 0.4491555706593054, - "angularVelocity": -1.0374293863583475, - "velocityX": -1.8079688286610023, - "velocityY": -1.529170921898447, - "timestamp": 1.983030771214393 - }, - { - "x": 1.9104777180009005, - "y": 6.1163033962265265, - "heading": 0.3601220361949963, - "angularVelocity": -1.1671789007706412, - "velocityX": -2.0339846397576693, - "velocityY": -1.7204090612237575, - "timestamp": 2.0593117375585948 - }, - { - "x": 1.7725837401718476, - "y": 5.99956344062706, - "heading": 0.2802286748074377, - "angularVelocity": -1.047356440677276, - "velocityX": -1.8077114707660742, - "velocityY": -1.530394293900403, - "timestamp": 2.1355927039027964 - }, - { - "x": 1.6519287666788485, - "y": 5.897413817352657, - "heading": 0.21025683762829658, - "angularVelocity": -0.9172909120831881, - "velocityX": -1.581717947527036, - "velocityY": -1.3391233513887155, - "timestamp": 2.211873670246998 - }, - { - "x": 1.5485118574908003, - "y": 5.80985551317875, - "heading": 0.15022948194911198, - "angularVelocity": -0.7869244264698967, - "velocityX": -1.3557367475609723, - "velocityY": -1.1478394728127326, - "timestamp": 2.2881546365911998 - }, - { - "x": 1.4623321603791597, - "y": 5.736889288541215, - "heading": 0.10017126259617518, - "angularVelocity": -0.6562347301302999, - "velocityX": -1.1297667198257733, - "velocityY": -0.9565456253555378, - "timestamp": 2.3644356029354014 - }, - { - "x": 1.393388970887201, - "y": 5.678515706716561, - "heading": 0.06010509802987578, - "angularVelocity": -0.5252445858334249, - "velocityX": -0.9038059271169646, - "velocityY": -0.7652443936641671, - "timestamp": 2.440716569279603 - }, - { - "x": 1.3416817772407035, - "y": 5.634735172181737, - "heading": 0.030048985724868114, - "angularVelocity": -0.39401850489866874, - "velocityX": -0.6778518434766467, - "velocityY": -0.5739378595104004, - "timestamp": 2.516997535623805 - }, - { - "x": 1.307210288791871, - "y": 5.605547971029586, - "heading": 0.010013480371388656, - "angularVelocity": -0.2626540579888315, - "velocityX": -0.4519015700633713, - "velocityY": -0.38262757484247345, - "timestamp": 2.5932785019680065 + "angularVelocity": 0.13784811636956903, + "velocityX": 0.23664018211616192, + "velocityY": 0.20028857778593961, + "timestamp": 1.2272576341680477 + }, + { + "x": 2.6837128162746895, + "y": 6.770286154899203, + "heading": 0.8037616945531193, + "angularVelocity": -0.015403237566514301, + "velocityX": -0.03029383829428867, + "velocityY": -0.025449058845075586, + "timestamp": 1.3078610776675168 + }, + { + "x": 2.6597554401054357, + "y": 6.750039490992799, + "heading": 0.790165886718463, + "angularVelocity": -0.1686752729700538, + "velocityX": -0.29722521928375617, + "velocityY": -0.2511885724392666, + "timestamp": 1.388464521166986 + }, + { + "x": 2.614282818196885, + "y": 6.71159714288196, + "heading": 0.7642144593793307, + "angularVelocity": -0.32196425130707784, + "velocityX": -0.5641523480167241, + "velocityY": -0.4769318337003116, + "timestamp": 1.469067964666455 + }, + { + "x": 2.547295402718566, + "y": 6.65495864191724, + "heading": 0.7259067765041173, + "angularVelocity": -0.4752611205179619, + "velocityX": -0.8310738669455855, + "velocityY": -0.702680909222201, + "timestamp": 1.5496714081659242 + }, + { + "x": 2.45879373609576, + "y": 6.580123315301835, + "heading": 0.6752436075608932, + "angularVelocity": -0.6285484433864407, + "velocityX": -1.0979886563197567, + "velocityY": -0.9284383317423857, + "timestamp": 1.6302748516653933 + }, + { + "x": 2.3487784541804713, + "y": 6.487090205965796, + "heading": 0.612227929672179, + "angularVelocity": -0.7817988308289465, + "velocityX": -1.3648955570482664, + "velocityY": -1.1542076280722422, + "timestamp": 1.7108782951648625 + }, + { + "x": 2.2172503640031413, + "y": 6.3758579034961445, + "heading": 0.536865388228733, + "angularVelocity": -0.934979179194793, + "velocityX": -1.6317924454199069, + "velocityY": -1.3799944225748537, + "timestamp": 1.7914817386643316 + }, + { + "x": 2.0642108270205712, + "y": 6.2464240090225065, + "heading": 0.4491644340858314, + "angularVelocity": -1.0880546827189126, + "velocityX": -1.8986724429895898, + "velocityY": -1.605810978465707, + "timestamp": 1.8720851821638007 + }, + { + "x": 1.892150582189426, + "y": 6.100772712319997, + "heading": 0.34952884105977006, + "angularVelocity": -1.236120799562961, + "velocityX": -2.134651292315194, + "velocityY": -1.8070108469188693, + "timestamp": 1.9526886256632698 + }, + { + "x": 1.7416020056028705, + "y": 5.973323101720471, + "heading": 0.2622670318762804, + "angularVelocity": -1.0826064668575972, + "velocityX": -1.8677685474764083, + "velocityY": -1.5811931235965593, + "timestamp": 2.033292069162739 + }, + { + "x": 1.6125631547118695, + "y": 5.864077706390992, + "heading": 0.18740079602910192, + "angularVelocity": -0.9288218045882161, + "velocityX": -1.600909902712647, + "velocityY": -1.3553440223708513, + "timestamp": 2.113895512662208 + }, + { + "x": 1.5050325714338102, + "y": 5.7730380802152625, + "heading": 0.12496122653203612, + "angularVelocity": -0.7746513893974771, + "velocityX": -1.3340693475310352, + "velocityY": -1.1294756430146329, + "timestamp": 2.1944989561616772 + }, + { + "x": 1.4190090841759289, + "y": 5.700205295684954, + "heading": 0.0749803520329015, + "angularVelocity": -0.6200836134188114, + "velocityX": -1.0672433276187945, + "velocityY": -0.9035939578786114, + "timestamp": 2.2751023996611464 + }, + { + "x": 1.3544918382019193, + "y": 5.645580092417221, + "heading": 0.037484747068995454, + "angularVelocity": -0.4651861426263309, + "velocityX": -0.8004279119223182, + "velocityY": -0.6777030967431115, + "timestamp": 2.3557058431606155 + }, + { + "x": 1.3114803343182175, + "y": 5.609162978300701, + "heading": 0.012490529490997385, + "angularVelocity": -0.31008870704351826, + "velocityX": -0.5336186894297898, + "velocityY": -0.45180593452897394, + "timestamp": 2.4363092866600846 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -6.199521898254585e-29, - "angularVelocity": -0.13127102150595574, - "velocityX": -0.22595201079473404, - "velocityY": -0.19131466197828328, - "timestamp": 2.669559468312208 + "heading": 1.0757846254441862e-30, + "angularVelocity": -0.1549627280016075, + "velocityX": -0.2668109738177861, + "velocityY": -0.22590442503082891, + "timestamp": 2.5169127301595537 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.3170947337368488e-29, - "angularVelocity": 6.400644992589977e-28, - "velocityX": 2.2164409811256884e-29, - "velocityY": 1.733201459680474e-29, - "timestamp": 2.74584043465641 + "heading": 2.1112287936414426e-30, + "angularVelocity": 1.2846097126015132e-29, + "velocityX": 1.1473496118122496e-30, + "velocityY": -2.480854756847108e-31, + "timestamp": 2.597516173659023 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.4.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.4.traj index 9d967ddd..f1c1499b 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.4.traj @@ -3,506 +3,470 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.3170947337368488e-29, - "angularVelocity": 6.400644992589977e-28, - "velocityX": 2.2164409811256884e-29, - "velocityY": 1.733201459680474e-29, + "heading": 2.1112287936414426e-30, + "angularVelocity": 1.2846097126015132e-29, + "velocityX": 1.1473496118122496e-30, + "velocityY": -2.480854756847108e-31, "timestamp": 0 }, { - "x": 1.3080029680398912, - "y": 5.59126500545377, - "heading": 8.890962339325397e-19, - "angularVelocity": 1.3082751589925716e-17, - "velocityX": 0.2652835543681239, - "velocityY": 0.004571871036655683, - "timestamp": 0.06795942182608883 - }, - { - "x": 1.344062678768715, - "y": 5.591577976323001, - "heading": 2.6666437965292957e-18, - "angularVelocity": 2.6156013615677146e-17, - "velocityX": 0.5306064966400389, - "velocityY": 0.004605260916318704, - "timestamp": 0.13591884365217766 - }, - { - "x": 1.398150210579935, - "y": 5.5915422166660145, - "heading": 8.768948399955272e-19, - "angularVelocity": -2.6335553017414816e-17, - "velocityX": 0.7958798111854469, - "velocityY": -0.000526191306891862, - "timestamp": 0.20387826547826648 - }, - { - "x": 1.4702532783568034, - "y": 5.590755190471311, - "heading": -4.372782060498752e-19, - "angularVelocity": -1.9337613692601342e-17, - "velocityX": 1.060972354376158, - "velocityY": -0.011580825344825147, - "timestamp": 0.2718376873043553 - }, - { - "x": 1.560346433856685, - "y": 5.58875133578056, - "heading": -1.4651998990372215e-18, - "angularVelocity": -1.512552145628718e-17, - "velocityX": 1.3256904352487537, - "velocityY": -0.029486046774778202, - "timestamp": 0.33979710913044414 - }, - { - "x": 1.6683846538857667, - "y": 5.584987203919277, - "heading": -2.5518211074829943e-18, - "angularVelocity": -1.5989265053292237e-17, - "velocityX": 1.5897460149904699, - "velocityY": -0.05538793238877104, - "timestamp": 0.40775653095653297 - }, - { - "x": 1.7942934964417667, - "y": 5.578822081824432, - "heading": -4.145349651168243e-18, - "angularVelocity": -2.3448235739309385e-17, - "velocityX": 1.852706205744448, - "velocityY": -0.0907176949006653, - "timestamp": 0.4757159527826218 - }, - { - "x": 1.937953698284471, - "y": 5.569492606449359, - "heading": -1.0979652144550452e-17, - "angularVelocity": -1.0056445905133748e-16, - "velocityX": 2.1139114780925645, - "velocityY": -0.13728008750497092, - "timestamp": 0.5436753746087106 - }, - { - "x": 2.099176605031056, - "y": 5.556079595057618, - "heading": -1.979270285045548e-17, - "angularVelocity": -1.2968107245612037e-16, - "velocityX": 2.3723407647457755, - "velocityY": -0.19736794444875114, - "timestamp": 0.6116347964347995 - }, - { - "x": 2.2776642824157327, - "y": 5.537465473885052, - "heading": -2.5427652949352827e-17, - "angularVelocity": -8.291639256904635e-17, - "velocityX": 2.6263860490372255, - "velocityY": -0.2739005228767256, - "timestamp": 0.6795942182608883 - }, - { - "x": 2.472944009712466, - "y": 5.512282419629533, - "heading": -2.829850545817078e-17, - "angularVelocity": -4.224362761891433e-17, - "velocityX": 2.8734754070813326, - "velocityY": -0.3705601604434455, - "timestamp": 0.7475536400869771 - }, - { - "x": 2.6842610977752166, - "y": 5.47885759062817, - "heading": -2.887338329485776e-17, - "angularVelocity": -8.459133718914728e-18, - "velocityX": 3.109459768558945, - "velocityY": -0.49183509958190536, - "timestamp": 0.8155130619130659 - }, - { - "x": 2.9104101060267347, - "y": 5.435179084259705, - "heading": -3.3223671712936815e-17, - "angularVelocity": -6.40130286748416e-17, - "velocityX": 3.32770647799568, - "velocityY": -0.6427145080816032, - "timestamp": 0.8834724837391548 - }, - { - "x": 3.1494985869220757, - "y": 5.378942960028073, - "heading": -3.8165211155711695e-17, - "angularVelocity": -7.271308833998824e-17, - "velocityX": 3.5181064592806273, - "velocityY": -0.8274956248971932, - "timestamp": 0.9514319055652436 - }, - { - "x": 3.3987095123055417, - "y": 5.307785046252999, - "heading": -4.255972014068572e-17, - "angularVelocity": -6.466371942097715e-17, - "velocityX": 3.667054820172052, - "velocityY": -1.0470647316154307, - "timestamp": 1.0193913273913324 - }, - { - "x": 3.6542758900932353, - "y": 5.21975313201646, - "heading": -4.248444848952633e-17, - "angularVelocity": 1.107596991510937e-18, - "velocityX": 3.7605731614624154, - "velocityY": -1.2953599643889915, - "timestamp": 1.0873507492174213 - }, - { - "x": 3.9119052041412368, - "y": 5.113808430044301, - "heading": -4.228369470192057e-17, - "angularVelocity": 2.9540243605887717e-18, - "velocityX": 3.790928573631551, - "velocityY": -1.5589406019856407, - "timestamp": 1.15531017104351 + "x": 1.3107332172473851, + "y": 5.587721554520521, + "heading": -5.6112723310966165e-9, + "angularVelocity": -8.086181485047517e-8, + "velocityX": 0.2991463288589595, + "velocityY": -0.04658586416314624, + "timestamp": 0.06939335094467669 + }, + { + "x": 1.3522507006814268, + "y": 5.581255743814154, + "heading": -1.6136427820687844e-8, + "angularVelocity": -1.516738315771432e-7, + "velocityX": 0.5982919525984769, + "velocityY": -0.09317622824587016, + "timestamp": 0.13878670188935338 + }, + { + "x": 1.414526842614822, + "y": 5.571556496717738, + "heading": -3.074141680025046e-8, + "angularVelocity": -2.1046669010163338e-7, + "velocityX": 0.8974367296809845, + "velocityY": -0.13977199493000497, + "timestamp": 0.20818005283403007 + }, + { + "x": 1.4975615711863124, + "y": 5.558623354737258, + "heading": -4.841098342837698e-8, + "angularVelocity": -2.546291003036043e-7, + "velocityX": 1.196580471199459, + "velocityY": -0.1863743687891775, + "timestamp": 0.27757340377870676 + }, + { + "x": 1.6013547964876313, + "y": 5.542455744326475, + "heading": -6.788238796631325e-8, + "angularVelocity": -2.8059467012358587e-7, + "velocityX": 1.4957229170856399, + "velocityY": -0.2329850077952488, + "timestamp": 0.34696675472338345 + }, + { + "x": 1.7259064027890496, + "y": 5.5230529273691795, + "heading": -8.754228960685774e-8, + "angularVelocity": -2.8331102694259857e-7, + "velocityX": 1.7948636952366863, + "velocityY": -0.27960628350061884, + "timestamp": 0.41636010566806014 + }, + { + "x": 1.8712162355508042, + "y": 5.500413918486819, + "heading": -1.0525703637468966e-7, + "angularVelocity": -2.552801733617599e-7, + "velocityX": 2.0940022463766295, + "velocityY": -0.3262417591047122, + "timestamp": 0.48575345661273683 + }, + { + "x": 2.0372840779688866, + "y": 5.474537335801765, + "heading": -1.1807218385471473e-7, + "angularVelocity": -1.8467399582926014e-7, + "velocityX": 2.393137673240176, + "velocityY": -0.3728971483980547, + "timestamp": 0.5551468075574135 + }, + { + "x": 2.2241096038561166, + "y": 5.445421101366719, + "heading": -1.2162497814922265e-7, + "angularVelocity": -5.119790624873975e-8, + "velocityX": 2.692268399549916, + "velocityY": -0.41958248216406707, + "timestamp": 0.6245401585020902 + }, + { + "x": 2.431692266948392, + "y": 5.413061738269229, + "heading": -1.0881824696224333e-7, + "angularVelocity": 1.845527118847122e-7, + "velocityX": 2.99139125386478, + "velocityY": -0.46631792033351455, + "timestamp": 0.6939335094467669 + }, + { + "x": 2.660030964874599, + "y": 5.377452241496065, + "heading": -6.605232487729304e-8, + "angularVelocity": 6.162827019703426e-7, + "velocityX": 3.290498222347668, + "velocityY": -0.5131543049644982, + "timestamp": 0.7633268603914427 + }, + { + "x": 2.9091223110729234, + "y": 5.338571186157936, + "heading": 4.364254321616404e-8, + "angularVelocity": 0.0000015807691363697316, + "velocityX": 3.5895563884342283, + "velocityY": -0.5602994351597471, + "timestamp": 0.8327202113361185 + }, + { + "x": 3.168557402823767, + "y": 5.298953853406429, + "heading": 4.3641894183803e-8, + "angularVelocity": -9.352947371519385e-12, + "velocityX": 3.738615994458649, + "velocityY": -0.5709096363289035, + "timestamp": 0.9021135622807943 + }, + { + "x": 3.4266802039415243, + "y": 5.251533194096367, + "heading": 4.3642354317485314e-8, + "angularVelocity": 6.630803609714539e-12, + "velocityX": 3.719705095716507, + "velocityY": -0.6833602739240744, + "timestamp": 0.9715069132254701 + }, + { + "x": 3.6803449026923616, + "y": 5.18422554387417, + "heading": 4.364233337760302e-8, + "angularVelocity": -3.0175631893268995e-13, + "velocityX": 3.6554611543845263, + "velocityY": -0.9699437958523844, + "timestamp": 1.040900264170146 + }, + { + "x": 3.9278131468583686, + "y": 5.096843813495861, + "heading": 4.364231130349348e-8, + "angularVelocity": -3.1810121673972006e-13, + "velocityX": 3.5661665101502296, + "velocityY": -1.2592233865168847, + "timestamp": 1.1102936151148217 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -4.208890664344976e-17, - "angularVelocity": 2.8662406659266305e-18, - "velocityX": 3.7609775714691667, - "velocityY": -1.8225675093785412, - "timestamp": 1.223269592869599 - }, - { - "x": 4.282012195193535, - "y": 4.930397905958738, - "heading": -4.1602494438105564e-17, - "angularVelocity": 1.582195542444361e-17, - "velocityX": 3.7248687742898503, - "velocityY": -1.9370313765192848, - "timestamp": 1.2540124557013765 - }, - { - "x": 4.3952613097643365, - "y": 4.867381383869913, - "heading": -4.188915157617957e-17, - "angularVelocity": -9.32434756133803e-18, - "velocityX": 3.6837530450723373, - "velocityY": -2.049793554804778, - "timestamp": 1.2847553185331542 - }, - { - "x": 4.506961712126711, - "y": 4.801015720721852, - "heading": -4.2434270081658926e-17, - "angularVelocity": -1.7731546618224465e-17, - "velocityX": 3.6333767279120854, - "velocityY": -2.158733996609517, - "timestamp": 1.3154981813649318 - }, - { - "x": 4.616614691776683, - "y": 4.73158032272052, - "heading": -4.19288728234691e-17, - "angularVelocity": 1.6439498850683494e-17, - "velocityX": 3.5667784178065634, - "velocityY": -2.258585948266306, - "timestamp": 1.3462410441967094 - }, - { - "x": 4.722598987733627, - "y": 4.661750165167655, - "heading": -3.986850310029329e-17, - "angularVelocity": 6.701944885386403e-17, - "velocityX": 3.4474439331457423, - "velocityY": -2.271426637622177, - "timestamp": 1.376983907028487 - }, - { - "x": 4.827229210309334, - "y": 4.595352451522488, - "heading": -3.770620807844877e-17, - "angularVelocity": 7.033486223050351e-17, - "velocityX": 3.4033988034307483, - "velocityY": -2.1597765311737476, - "timestamp": 1.4077267698602647 - }, - { - "x": 4.9314726269952285, - "y": 4.532624292130925, - "heading": -3.5259240937147126e-17, - "angularVelocity": 7.959464135435417e-17, - "velocityX": 3.390816829789258, - "velocityY": -2.04041372902732, - "timestamp": 1.4384696326920423 - }, - { - "x": 5.035668651391688, - "y": 4.473585715832336, - "heading": -3.400468055587437e-17, - "angularVelocity": 4.080818328915737e-17, - "velocityX": 3.389275259321544, - "velocityY": -1.9203994312970292, - "timestamp": 1.46921249552382 - }, - { - "x": 5.139985480442779, - "y": 4.418235050432876, - "heading": -3.272140280940892e-17, - "angularVelocity": 4.1742298155096073e-17, - "velocityX": 3.3932047780294075, - "velocityY": -1.8004395264792032, - "timestamp": 1.4999553583555976 - }, - { - "x": 5.244522953443759, - "y": 4.366567672079215, - "heading": -3.123998369139628e-17, - "angularVelocity": 4.8187415925402766e-17, - "velocityX": 3.400381856855714, - "velocityY": -1.680630025784517, - "timestamp": 1.5306982211873752 - }, - { - "x": 5.3493470515644415, - "y": 4.31857903504105, - "heading": -3.003359756723782e-17, - "angularVelocity": 3.924117707448193e-17, - "velocityX": 3.409705162927411, - "velocityY": -1.5609683880370886, - "timestamp": 1.5614410840191528 - }, - { - "x": 5.454504582553393, - "y": 4.274265192865899, - "heading": -2.87901792623276e-17, - "angularVelocity": 4.044575522175447e-17, - "velocityX": 3.4205510255946114, - "velocityY": -1.4414351200027105, - "timestamp": 1.5921839468509305 - }, - { - "x": 5.560030462239174, - "y": 4.233622810392966, - "heading": -2.7523978224966667e-17, - "angularVelocity": 4.118682909556349e-17, - "velocityX": 3.432532625969479, - "velocityY": -1.3220103376619752, - "timestamp": 1.622926809682708 + "heading": 4.364228801520247e-8, + "angularVelocity": -3.3559830331814666e-13, + "velocityX": 3.45401850865928, + "velocityY": -1.5404360240963577, + "timestamp": 1.1796869660594975 + }, + { + "x": 4.284862555065841, + "y": 4.931880235034004, + "heading": 4.364226510492993e-8, + "angularVelocity": -6.617055985872344e-13, + "velocityX": 3.3897491580338537, + "velocityY": -1.67713541924223, + "timestamp": 1.2143100264085245 + }, + { + "x": 4.399813643173519, + "y": 4.869172357324464, + "heading": 4.364224321511573e-8, + "angularVelocity": -6.322322259809295e-13, + "velocityX": 3.3200730076683276, + "velocityY": -1.81115929895842, + "timestamp": 1.2489330867575514 + }, + { + "x": 4.512168964860153, + "y": 4.801924216205689, + "heading": 4.364222211690104e-8, + "angularVelocity": -6.093688654191888e-13, + "velocityX": 3.245100824537311, + "velocityY": -1.9422933859936342, + "timestamp": 1.2835561471065784 + }, + { + "x": 4.621749311428581, + "y": 4.730243126546711, + "heading": 4.364220167371938e-8, + "angularVelocity": -5.904498836065766e-13, + "velocityX": 3.164952649008305, + "velocityY": -2.070327952999433, + "timestamp": 1.3181792074556054 + }, + { + "x": 4.728650036507715, + "y": 4.654623908914118, + "heading": 4.364241447304788e-8, + "angularVelocity": 6.146173210402922e-12, + "velocityX": 3.0875585231776617, + "velocityY": -2.184070872715801, + "timestamp": 1.3528022678046323 + }, + { + "x": 4.8372727310201835, + "y": 4.581499759061361, + "heading": 4.3642159997006347e-8, + "angularVelocity": -7.349900240613306e-12, + "velocityX": 3.137293278452735, + "velocityY": -2.112007116517435, + "timestamp": 1.3874253281536593 + }, + { + "x": 4.948612503037568, + "y": 4.5125832438429105, + "heading": 4.3642137671211746e-8, + "angularVelocity": -6.448244222387315e-13, + "velocityX": 3.2157692270698366, + "velocityY": -1.9904801748811223, + "timestamp": 1.4220483885026862 + }, + { + "x": 5.062589095284458, + "y": 4.448121106677303, + "heading": 4.364211455765666e-8, + "angularVelocity": -6.675769025667281e-13, + "velocityX": 3.2919271461828314, + "velocityY": -1.8618266703110185, + "timestamp": 1.4566714488517132 + }, + { + "x": 5.17904891222934, + "y": 4.388261758970167, + "heading": 4.3642090495999965e-8, + "angularVelocity": -6.94960432877984e-13, + "velocityX": 3.3636488447548265, + "velocityY": -1.728886675634883, + "timestamp": 1.4912945092007401 + }, + { + "x": 5.297806179962019, + "y": 4.333100707972697, + "heading": 4.364206516158606e-8, + "angularVelocity": -7.317208234311228e-13, + "velocityX": 3.4300049312658345, + "velocityY": -1.5931881942671235, + "timestamp": 1.525917569549767 + }, + { + "x": 5.418671454426997, + "y": 4.282725959570945, + "heading": 4.364203830903825e-8, + "angularVelocity": -7.755683028589255e-13, + "velocityX": 3.4908894028015367, + "velocityY": -1.454947884269515, + "timestamp": 1.560540629898794 + }, + { + "x": 5.541451926168614, + "y": 4.237217881100571, + "heading": 4.364200955008555e-8, + "angularVelocity": -8.306300130132497e-13, + "velocityX": 3.546205058244276, + "velocityY": -1.3143863659542425, + "timestamp": 1.595163690247821 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -2.6172162518713254e-17, - "angularVelocity": 4.3971692345317756e-17, - "velocityX": 3.4453937215027324, - "velocityY": -1.2026770584391975, - "timestamp": 1.6536696725144857 - }, - { - "x": 5.792835589324764, - "y": 4.157776846380009, - "heading": -2.4556205828219047e-17, - "angularVelocity": 4.408549337244093e-17, - "velocityX": 3.461564053183983, - "velocityY": -1.060487182383484, - "timestamp": 1.6903247389972957 - }, - { - "x": 5.920107057755266, - "y": 4.124135852213366, - "heading": -2.3047234390121686e-17, - "angularVelocity": 4.1166790375489185e-17, - "velocityX": 3.472138523884214, - "velocityY": -0.917772013383704, - "timestamp": 1.7269798054801058 - }, - { - "x": 6.047516391811881, - "y": 4.095738620024666, - "heading": -2.1456366912742873e-17, - "angularVelocity": 4.3401025561499594e-17, - "velocityX": 3.475899685419091, - "velocityY": -0.7747150643422807, - "timestamp": 1.7636348719629158 - }, - { - "x": 6.174755293992793, - "y": 4.0725841920992965, - "heading": -1.9852024700254233e-17, - "angularVelocity": 4.3768634637209445e-17, - "velocityX": 3.4712500723626603, - "velocityY": -0.631684243056213, - "timestamp": 1.8002899384457258 - }, - { - "x": 6.30143822417369, - "y": 4.054645790921562, - "heading": -1.7231399343635e-17, - "angularVelocity": 7.149421916471826e-17, - "velocityX": 3.4560824010592928, - "velocityY": -0.48938394876864877, - "timestamp": 1.8369450049285359 - }, - { - "x": 6.427078572160238, - "y": 4.041848309685929, - "heading": -1.4890621747726543e-17, - "angularVelocity": 6.385959215232344e-17, - "velocityX": 3.427639342721413, - "velocityY": -0.3491326701490028, - "timestamp": 1.8736000714113459 - }, - { - "x": 6.551062514369357, - "y": 4.034028011406828, - "heading": -1.2144086622031307e-17, - "angularVelocity": 7.492920868069778e-17, - "velocityX": 3.382450343317836, - "velocityY": -0.21334835888971543, - "timestamp": 1.910255137894156 - }, - { - "x": 6.672633087813251, - "y": 4.030865145435154, - "heading": -9.61251747475391e-18, - "angularVelocity": 6.906464481423604e-17, - "velocityX": 3.316610365470415, - "velocityY": -0.08628727963586283, - "timestamp": 1.946910204376966 - }, - { - "x": 6.790917094390862, - "y": 4.031790616181229, - "heading": -6.811165774297211e-18, - "angularVelocity": 7.642467929421218e-17, - "velocityX": 3.2269483574141518, - "velocityY": 0.025248098963559784, - "timestamp": 1.983565270859776 - }, - { - "x": 6.905041403639001, - "y": 4.0359118549290525, - "heading": -4.3348375779027234e-18, - "angularVelocity": 6.755759664372386e-17, - "velocityX": 3.113466164402112, - "velocityY": 0.11243299066875824, - "timestamp": 2.020220337342586 - }, - { - "x": 7.014327663163953, - "y": 4.042060231899537, - "heading": -2.4762499879984938e-18, - "angularVelocity": 5.070479385914931e-17, - "velocityX": 2.9814775967247074, - "velocityY": 0.16773607472154842, - "timestamp": 2.056875403825396 - }, - { - "x": 7.1184276572510345, - "y": 4.048995163672738, - "heading": -1.0562416558517327e-18, - "angularVelocity": 3.873975601197627e-17, - "velocityX": 2.8399892313904544, - "velocityY": 0.1891943580692654, - "timestamp": 2.093530470308206 + "heading": 4.364197825891865e-8, + "angularVelocity": -9.037666511803314e-13, + "velocityX": 3.5958636064268057, + "velocityY": -1.171727921713515, + "timestamp": 1.629786750596848 + }, + { + "x": 5.80048142988227, + "y": 4.159069716430897, + "heading": 4.364194761543742e-8, + "angularVelocity": -8.296994367988738e-13, + "velocityX": 3.642511096804761, + "velocityY": -1.0174944855729342, + "timestamp": 1.6667199808564561 + }, + { + "x": 5.936489765010674, + "y": 4.127254911109845, + "heading": 4.364191906311607e-8, + "angularVelocity": -7.730794282349154e-13, + "velocityX": 3.6825464269543904, + "velocityY": -0.8614141004570673, + "timestamp": 1.7036532111160643 + }, + { + "x": 6.073729843507157, + "y": 4.101262406827453, + "heading": 4.3641892220156324e-8, + "angularVelocity": -7.267969466454947e-13, + "velocityX": 3.7158969722335238, + "velocityY": -0.7037701305758657, + "timestamp": 1.7405864413756724 + }, + { + "x": 6.211952539114824, + "y": 4.081139381676687, + "heading": 4.3641866716107116e-8, + "angularVelocity": -6.905447558160659e-13, + "velocityX": 3.742502202923576, + "velocityY": -0.5448487719411382, + "timestamp": 1.7775196716352806 + }, + { + "x": 6.350906942398405, + "y": 4.066922357709394, + "heading": 4.364184226449404e-8, + "angularVelocity": -6.620491141311732e-13, + "velocityX": 3.7623138378867345, + "velocityY": -0.38493854632702584, + "timestamp": 1.8144529018948887 + }, + { + "x": 6.490340706014696, + "y": 4.058637143370339, + "heading": 4.364062068850958e-8, + "angularVelocity": -3.3075253340927824e-11, + "velocityX": 3.775292944489131, + "velocityY": -0.22432953415709472, + "timestamp": 1.8513861321544969 + }, + { + "x": 6.625895301013595, + "y": 4.054865050461972, + "heading": 3.8021532173824235e-8, + "angularVelocity": -1.521418068678583e-7, + "velocityX": 3.6702610101003303, + "velocityY": -0.10213276450108619, + "timestamp": 1.888319362414105 + }, + { + "x": 6.755934035567496, + "y": 4.053327166371606, + "heading": 3.0189413996037156e-8, + "angularVelocity": -2.1206154817940447e-7, + "velocityX": 3.52091419136215, + "velocityY": -0.04163957713841639, + "timestamp": 1.9252525926737132 + }, + { + "x": 6.880153762983055, + "y": 4.0530365385095966, + "heading": 2.1734826824788128e-8, + "angularVelocity": -2.2891544824669977e-7, + "velocityX": 3.3633594067565498, + "velocityY": -0.007869007394285612, + "timestamp": 1.9621858229333213 + }, + { + "x": 6.998470106919647, + "y": 4.0534986388519805, + "heading": 1.3540571224920633e-8, + "angularVelocity": -2.2186673987685247e-7, + "velocityX": 3.2035200578159904, + "velocityY": 0.012511777040293628, + "timestamp": 1.9991190531929295 + }, + { + "x": 7.110853220415867, + "y": 4.054422625435363, + "heading": 6.1666659449803414e-9, + "angularVelocity": -1.9965502711211782e-7, + "velocityX": 3.042872575896168, + "velocityY": 0.025017757095422304, + "timestamp": 2.0360522834525376 }, { "x": 7.217291355133057, "y": 4.0556182861328125, - "heading": 3.646325715989205e-37, - "angularVelocity": 2.881570700048058e-17, - "velocityX": 2.697135958773559, - "velocityY": 0.18068777649551113, - "timestamp": 2.130185536791016 - }, - { - "x": 7.386231209731221, - "y": 4.063934540767167, - "heading": 6.43663529397786e-19, - "angularVelocity": 9.269678756083754e-18, - "velocityX": 2.432976407238177, - "velocityY": 0.11976600412078256, - "timestamp": 2.199623059528829 - }, - { - "x": 7.536591060669406, - "y": 4.069229370335429, - "heading": -1.8193245120818273e-19, - "angularVelocity": -1.1889767204445143e-17, - "velocityX": 2.1653976842740037, - "velocityY": 0.07625314613043933, - "timestamp": 2.2690605822666416 - }, - { - "x": 7.668251510768197, - "y": 4.072361333543041, - "heading": -2.313295063693691e-18, - "angularVelocity": -3.069468103770411e-17, - "velocityX": 1.8960994705401897, - "velocityY": 0.04510476589779892, - "timestamp": 2.3384981050044544 - }, - { - "x": 7.78114979643099, - "y": 4.073967912827302, - "heading": -5.036275587489351e-18, - "angularVelocity": -3.9214828185580025e-17, - "velocityX": 1.6258973709226552, - "velocityY": 0.023137047822506123, - "timestamp": 2.407935627742267 - }, - { - "x": 7.875252481646468, - "y": 4.074539583547341, - "heading": -6.689419008155875e-18, - "angularVelocity": -2.380763822621624e-17, - "velocityX": 1.3552137447471682, - "velocityY": 0.008232878960810142, - "timestamp": 2.47737315048008 - }, - { - "x": 7.950542203811188, - "y": 4.074464670768735, - "heading": -5.15013780810139e-18, - "angularVelocity": 2.2167858808364967e-17, - "velocityX": 1.0842800721592976, - "velocityY": -0.001078851543840728, - "timestamp": 2.546810673217893 - }, - { - "x": 8.007010780474163, - "y": 4.074057877238601, - "heading": -4.959999016913383e-18, - "angularVelocity": 2.738271523682432e-18, - "velocityX": 0.8132285605319336, - "velocityY": -0.005858410756812446, - "timestamp": 2.6162481959557056 - }, - { - "x": 8.044655422701718, - "y": 4.073579204097417, - "heading": -1.6663567301752781e-18, - "angularVelocity": 4.7433176715915545e-17, - "velocityX": 0.5421368842563122, - "velocityY": -0.006893580333953516, - "timestamp": 2.6856857186935184 + "heading": -3.6086824530888626e-33, + "angularVelocity": -1.6696795107584883e-7, + "velocityX": 2.8819069972765123, + "velocityY": 0.03237357493636809, + "timestamp": 2.072985513712146 + }, + { + "x": 7.405345242374078, + "y": 4.058539790558069, + "heading": -6.074177156590073e-9, + "angularVelocity": -8.274407475492158e-8, + "velocityX": 2.561720622548375, + "velocityY": 0.03979751891783301, + "timestamp": 2.146394723750806 + }, + { + "x": 7.569888594263639, + "y": 4.061600743201228, + "heading": -8.336905285122374e-9, + "angularVelocity": -3.0823491082343837e-8, + "velocityX": 2.241453787650163, + "velocityY": 0.04169712004183413, + "timestamp": 2.219803933789466 + }, + { + "x": 7.71092123884848, + "y": 4.064554734769034, + "heading": -8.259320686300401e-9, + "angularVelocity": 1.056878269798436e-9, + "velocityX": 1.9211846103583805, + "velocityY": 0.04024006750993344, + "timestamp": 2.293213143828126 + }, + { + "x": 7.828444510796415, + "y": 4.067236236264996, + "heading": -6.843181194470064e-9, + "angularVelocity": 1.929103299991646e-8, + "velocityX": 1.600933614270509, + "velocityY": 0.03652813447454146, + "timestamp": 2.366622353866786 + }, + { + "x": 7.922460087151161, + "y": 4.069526407614099, + "heading": -4.8146064610859125e-9, + "angularVelocity": 2.7633790562623428e-8, + "velocityX": 1.2807054633231172, + "velocityY": 0.031197329979384493, + "timestamp": 2.440031563905446 + }, + { + "x": 7.992969626740666, + "y": 4.071335791937442, + "heading": -2.7240893137969417e-9, + "angularVelocity": 2.8477586783070736e-8, + "velocityX": 0.9604999093761202, + "velocityY": 0.024647919823550288, + "timestamp": 2.5134407739441063 + }, + { + "x": 8.039974659720452, + "y": 4.072594619838684, + "heading": -1.0033424996203534e-9, + "angularVelocity": 2.3440475863128373e-8, + "velocityX": 0.6403151996190951, + "velocityY": 0.01714809218869153, + "timestamp": 2.5868499839827663 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 4.9523840537519984e-39, - "angularVelocity": 2.3997928849898842e-17, - "velocityX": 0.27105142948932054, - "velocityY": -0.004784851370482414, - "timestamp": 2.755123241431331 + "heading": 1.0901032811452175e-35, + "angularVelocity": 1.3667801358727972e-8, + "velocityX": 0.32014923968219366, + "velocityY": 0.00888629686322534, + "timestamp": 2.6602591940214264 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": -7.535317609356433e-39, - "angularVelocity": -1.798408291474874e-37, - "velocityX": 0, - "velocityY": 1.827412307849029e-40, - "timestamp": 2.824560764169144 + "heading": 2.780011652171749e-35, + "angularVelocity": 2.3020443776927775e-34, + "velocityX": -1.517814230631444e-35, + "velocityY": 4.908282159537256e-34, + "timestamp": 2.7336684040600865 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.traj index ae140579..9a2599ac 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.traj @@ -4,1369 +4,1279 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": -1.748186257820146e-28, - "velocityX": 0, - "velocityY": 0, + "angularVelocity": -7.2072188294937895e-31, + "velocityX": -1.1048793733131498e-32, + "velocityY": -5.425421859025623e-32, "timestamp": 0 }, { - "x": 1.3063204215499635, - "y": 5.5750430661668995, - "heading": -0.008999106947679492, - "angularVelocity": -0.11745777195159221, - "velocityX": 0.21335020071190533, - "velocityY": -0.20767599777855084, - "timestamp": 0.07661567898070912 - }, - { - "x": 1.3390123980907098, - "y": 5.543220800833044, - "heading": -0.026994200234242757, - "angularVelocity": -0.23487481322228163, - "velocityX": 0.42670086570142546, - "velocityY": -0.41534925701509284, - "timestamp": 0.15323135796141824 - }, - { - "x": 1.3880505863886679, - "y": 5.4954877356301095, - "heading": -0.053973713874731634, - "angularVelocity": -0.3521408934222409, - "velocityX": 0.6400542155377871, - "velocityY": -0.6230195417923181, - "timestamp": 0.22984703694212735 - }, - { - "x": 1.4534353620178047, - "y": 5.431844149183211, - "heading": -0.08991902767117105, - "angularVelocity": -0.4691639398438398, - "velocityX": 0.853412467425134, - "velocityY": -0.8306861899103606, - "timestamp": 0.30646271592283647 - }, - { - "x": 1.5351672628403068, - "y": 5.352290408430599, - "heading": -0.13480641974722582, - "angularVelocity": -0.5858773645717358, - "velocityX": 1.066777739575042, - "velocityY": -1.0383480486921168, - "timestamp": 0.38307839490354556 - }, - { - "x": 1.6332469708393256, - "y": 5.256827004871127, - "heading": -0.1886095129083549, - "angularVelocity": -0.7022465099717149, - "velocityX": 1.2801519130772017, - "velocityY": -1.2460034923528938, - "timestamp": 0.45969407388425465 - }, - { - "x": 1.7476752821470238, - "y": 5.145454581666917, - "heading": -0.25130200812811576, - "angularVelocity": -0.818272396306973, - "velocityX": 1.493536477822563, - "velocityY": -1.4536505413596468, - "timestamp": 0.5363097528649637 - }, - { - "x": 1.8784530688655205, - "y": 5.018173944363308, - "heading": -0.3228603099280309, - "angularVelocity": -0.9339903107164581, - "velocityX": 1.7069324253829399, - "velocityY": -1.66128707626095, - "timestamp": 0.6129254318456728 - }, - { - "x": 2.0255812507970647, - "y": 4.874986059010151, - "heading": -0.4032653297049151, - "angularVelocity": -1.049459077659022, - "velocityX": 1.9203403783475084, - "velocityY": -1.8689110021266238, - "timestamp": 0.6895411108263819 - }, - { - "x": 2.156432157972857, - "y": 4.747720771234375, - "heading": -0.47395861648198423, - "angularVelocity": -0.9226999968520704, - "velocityX": 1.7078868044203455, - "velocityY": -1.6610867310239321, - "timestamp": 0.766156789807091 - }, - { - "x": 2.270931566035856, - "y": 4.636361300878039, - "heading": -0.5358188421760707, - "angularVelocity": -0.8074094819002076, - "velocityX": 1.494464443178203, - "velocityY": -1.4534814779709395, - "timestamp": 0.8427724687878001 - }, - { - "x": 2.369078961091203, - "y": 4.540907071428243, - "heading": -0.5888499820546531, - "angularVelocity": -0.6921708527593945, - "velocityX": 1.2810353744995546, - "velocityY": -1.2458837496554307, - "timestamp": 0.9193881477685092 - }, - { - "x": 2.4508739613497577, - "y": 4.461357622382182, - "heading": -0.6330536039339079, - "angularVelocity": -0.5769526870004522, - "velocityX": 1.0676013226487002, - "velocityY": -1.0382920318954914, - "timestamp": 0.9960038267492183 - }, - { - "x": 2.516316276718558, - "y": 4.3977125949651334, - "heading": -0.6684300724749328, - "angularVelocity": -0.4617392808541994, - "velocityX": 0.8541634844585878, - "velocityY": -0.8307049969200896, - "timestamp": 1.0726195057299275 - }, - { - "x": 2.5654056965461667, - "y": 4.349971723161198, - "heading": -0.6949791200920894, - "angularVelocity": -0.34652238213663356, - "velocityX": 0.6407228968125727, - "velocityY": -0.6231214340263868, - "timestamp": 1.1492351847106366 - }, - { - "x": 2.5981420853275283, - "y": 4.3181348275927744, - "heading": -0.7127001553640687, - "angularVelocity": -0.2312977640935645, - "velocityX": 0.4272805405331163, - "velocityY": -0.41554021239755634, - "timestamp": 1.2258508636913457 - }, - { - "x": 2.6145253815163745, - "y": 4.302201811854818, - "heading": -0.7215924300890252, - "angularVelocity": -0.11606338040076232, - "velocityX": 0.21383738094203186, - "velocityY": -0.20796024906265667, - "timestamp": 1.3024665426720548 + "x": 1.3063416372973582, + "y": 5.5750227907482355, + "heading": -0.009007895171544168, + "angularVelocity": -0.12420787311314503, + "velocityX": 0.22568350897133957, + "velocityY": -0.21967610709029223, + "timestamp": 0.07252273906455099 + }, + { + "x": 1.339076097606074, + "y": 5.5431599730454195, + "heading": -0.02701891548605667, + "angularVelocity": -0.24834997335784384, + "velocityX": 0.4513682292059063, + "velocityY": -0.43934934220379995, + "timestamp": 0.14504547812910198 + }, + { + "x": 1.388178099815531, + "y": 5.4953660820311345, + "heading": -0.05401968965283247, + "angularVelocity": -0.3723076998340179, + "velocityX": 0.677056642410746, + "velocityY": -0.6590193866197215, + "timestamp": 0.217568217193653 + }, + { + "x": 1.4536480974194865, + "y": 5.431641407654605, + "heading": -0.08998953758011774, + "angularVelocity": -0.49598027310141063, + "velocityX": 0.9027513087406556, + "velocityY": -0.8786854329899796, + "timestamp": 0.29009095625820397 + }, + { + "x": 1.5354867341790108, + "y": 5.351986341420191, + "heading": -0.13490221177586337, + "angularVelocity": -0.6192909255093908, + "velocityX": 1.1284548517495026, + "velocityY": -1.0983460809932237, + "timestamp": 0.36261369532275495 + }, + { + "x": 1.6336948546750298, + "y": 5.2564014251813616, + "heading": -0.18872783549465025, + "angularVelocity": -0.7421896140863778, + "velocityX": 1.3541700404978287, + "velocityY": -1.317999257498918, + "timestamp": 0.4351364343873059 + }, + { + "x": 1.7482735602297292, + "y": 5.144887415721353, + "heading": -0.25143410509925246, + "angularVelocity": -0.8646428749583058, + "velocityX": 1.579900415134063, + "velocityY": -1.5376419988880228, + "timestamp": 0.507659173451857 + }, + { + "x": 1.87922458951427, + "y": 5.017445479597787, + "heading": -0.32298143653104017, + "angularVelocity": -0.9865503200055588, + "velocityX": 1.8056547639221738, + "velocityY": -1.7572686548722531, + "timestamp": 0.5801819125164079 + }, + { + "x": 2.026417113452321, + "y": 4.874266246183284, + "heading": -0.4026826239248217, + "angularVelocity": -1.0989820354525424, + "velocityX": 2.0296051395270776, + "velocityY": -1.9742667646213359, + "timestamp": 0.6527046515809589 + }, + { + "x": 2.1572394289562786, + "y": 4.747014797378468, + "heading": -0.4735071808339005, + "angularVelocity": -0.9765841420579419, + "velocityX": 1.8038799580839464, + "velocityY": -1.7546420673865035, + "timestamp": 0.7252273906455099 + }, + { + "x": 2.27168995904481, + "y": 4.635690037684074, + "heading": -0.535478944048102, + "angularVelocity": -0.8545149288832774, + "velocityX": 1.578133031994185, + "velocityY": -1.5350324757486038, + "timestamp": 0.7977501297100609 + }, + { + "x": 2.369767924446142, + "y": 4.540291261559593, + "heading": -0.5886064902849578, + "angularVelocity": -0.732563978168669, + "velocityX": 1.35237536069838, + "velocityY": -1.3154326126526381, + "timestamp": 0.8702728687746119 + }, + { + "x": 2.4514728066095985, + "y": 4.460817939321001, + "heading": -0.6328935848970949, + "angularVelocity": -0.6106649470681784, + "velocityX": 1.1266105392220715, + "velocityY": -1.0958400532524892, + "timestamp": 0.9427956078391628 + }, + { + "x": 2.5168042340609067, + "y": 4.397269668318764, + "heading": -0.6683418026498548, + "angularVelocity": -0.48878763005849607, + "velocityX": 0.9008405955692155, + "velocityY": -0.8762530458976289, + "timestamp": 1.015318346903714 + }, + { + "x": 2.5657619428441123, + "y": 4.349646151783931, + "heading": -0.6949516025618677, + "angularVelocity": -0.36691664235512766, + "velocityX": 0.6750670122861077, + "velocityY": -0.6566701306241847, + "timestamp": 1.087841085968265 + }, + { + "x": 2.5983457601971756, + "y": 4.317947187379743, + "heading": -0.7127228546140986, + "angularVelocity": -0.24504386184805346, + "velocityX": 0.4492910468254882, + "velocityY": -0.4370900053275183, + "timestamp": 1.160363825032816 }, { "x": 2.614555597305298, "y": 4.302172660827637, "heading": -0.7216551150961179, - "angularVelocity": -0.0008181742381802744, - "velocityX": 0.0003943804906386705, - "velocityY": -0.0003804830723595261, - "timestamp": 1.3790822216527638 - }, - { - "x": 2.5982259689280283, - "y": 4.318054102166664, - "heading": -0.712883613809901, - "angularVelocity": 0.11446303533186522, - "velocityX": -0.213092236881309, - "velocityY": 0.20724365439564615, - "timestamp": 1.4557139589685733 - }, - { - "x": 2.565536606931669, - "y": 4.349846333909782, - "heading": -0.695279424271666, - "angularVelocity": 0.2297245257202117, - "velocityX": -0.426577330596217, - "velocityY": 0.4148702979331944, - "timestamp": 1.5323456962843827 - }, - { - "x": 2.5164876982610407, - "y": 4.39754963766033, - "heading": -0.6688439584276578, - "angularVelocity": 0.34496759102452434, - "velocityX": -0.640059984788952, - "velocityY": 0.6225006162319799, - "timestamp": 1.608977433600192 - }, - { - "x": 2.4510795013292355, - "y": 4.461164391501365, - "heading": -0.6335783197571896, - "angularVelocity": 0.4601962565606419, - "velocityX": -0.8535392668397416, - "velocityY": 0.8301358682967486, - "timestamp": 1.6856091709160015 - }, - { - "x": 2.369312351807588, - "y": 4.540691078953442, - "heading": -0.5894828757685662, - "angularVelocity": 0.5754201263263005, - "velocityX": -1.0670141685730552, - "velocityY": 1.0377774300260596, - "timestamp": 1.762240908231811 - }, - { - "x": 2.2711866760947044, - "y": 4.636130298345392, - "heading": -0.5365564475452359, - "angularVelocity": 0.6906593802750026, - "velocityX": -1.28048350605026, - "velocityY": 1.2454267996017627, - "timestamp": 1.8388726455476203 - }, - { - "x": 2.1567030158826075, - "y": 4.747482768262345, - "heading": -0.47479485869558546, - "angularVelocity": 0.8059531337295586, - "velocityX": -1.4939457747586862, - "velocityY": 1.4530855462992425, - "timestamp": 1.9155043828634297 - }, - { - "x": 2.0258620666320337, - "y": 4.874749320539599, - "heading": -0.4041884988158368, - "angularVelocity": 0.9213722974191146, - "velocityX": -1.7073989681051045, - "velocityY": 1.6607551479679548, - "timestamp": 1.9921361201792391 - }, - { - "x": 1.8786647355617998, - "y": 5.01793086600198, - "heading": -0.3247184712590107, - "angularVelocity": 1.0370380514754407, - "velocityX": -1.9208403227681095, - "velocityY": 1.8684366353759239, - "timestamp": 2.0687678574950485 - }, - { - "x": 1.7478414567124814, - "y": 5.145267849282916, - "heading": -0.25271823102965235, - "angularVelocity": 0.9395616327984198, - "velocityX": -1.7071683803424498, - "velocityY": 1.6616742327261775, - "timestamp": 2.145399594810858 - }, - { - "x": 1.6333728466995314, - "y": 5.256688511191179, - "heading": -0.18963629669305965, - "angularVelocity": 0.8231828815865951, - "velocityX": -1.4937493791208316, - "velocityY": 1.4539754132374219, - "timestamp": 2.222031332126668 - }, - { - "x": 1.535258100629332, - "y": 5.352192443129829, - "heading": -0.13550580636159482, - "angularVelocity": 0.70637169709355, - "velocityX": -1.2803408812977723, - "velocityY": 1.2462712617938094, - "timestamp": 2.2986630694424774 - }, - { - "x": 1.4534965485667528, - "y": 5.431779376588513, - "heading": -0.09035761814526168, - "angularVelocity": 0.5891578320466108, - "velocityX": -1.0669411257422734, - "velocityY": 1.0385636064447687, - "timestamp": 2.375294806758287 - }, - { - "x": 1.3880876748821938, - "y": 5.495449138780266, - "heading": -0.05421785896865615, - "angularVelocity": 0.4716030256909087, - "velocityX": -0.8535480988559686, - "velocityY": 0.8308536963253964, - "timestamp": 2.4519265440740967 - }, - { - "x": 1.339031128559986, - "y": 5.543201612325557, - "heading": -0.02710586002264422, - "angularVelocity": 0.35379595843554007, - "velocityX": -0.6401596525218931, - "velocityY": 0.6231422544707841, - "timestamp": 2.5285582813899063 - }, - { - "x": 1.3063267261975056, - "y": 5.575036700785004, - "heading": -0.009032648967396358, - "angularVelocity": 0.23584498657035882, - "velocityX": -0.4267735994864031, - "velocityY": 0.4154295541377912, - "timestamp": 2.605190018705716 + "angularVelocity": -0.12316496311606469, + "velocityX": 0.22351385671724336, + "velocityY": -0.21751145579268175, + "timestamp": 1.2328865640973672 + }, + { + "x": 2.6122418058900085, + "y": 4.3044120467003895, + "heading": -0.7206083682746678, + "angularVelocity": 0.01292691365951148, + "velocityX": -0.028574418607166113, + "velocityY": 0.027655539271243604, + "timestamp": 1.3138607895750445 + }, + { + "x": 2.589515477392911, + "y": 4.326503828401164, + "heading": -0.7085432954922883, + "angularVelocity": 0.148998927882974, + "velocityX": -0.2806612642842681, + "velocityY": 0.2728248596434546, + "timestamp": 1.3948350150527218 + }, + { + "x": 2.54637684601469, + "y": 4.3684483333856905, + "heading": -0.6854613533337227, + "angularVelocity": 0.2850529538548107, + "velocityX": -0.5327452176745692, + "velocityY": 0.5179982239657722, + "timestamp": 1.475809240530399 + }, + { + "x": 2.4828262685453337, + "y": 4.430246041385043, + "heading": -0.6513636060932391, + "angularVelocity": 0.42109383620888585, + "velocityX": -0.784824764848103, + "velocityY": 0.7631775127791823, + "timestamp": 1.5567834660080764 + }, + { + "x": 2.398864238908538, + "y": 4.511897605460961, + "heading": -0.6062501465002338, + "angularVelocity": 0.5571335733911974, + "velocityX": -1.0368982122572787, + "velocityY": 1.0083648666494986, + "timestamp": 1.6377576914857537 + }, + { + "x": 2.2944914266815206, + "y": 4.613403885366389, + "heading": -0.5501188636580988, + "angularVelocity": 0.6931993793206396, + "velocityX": -1.2889633906498084, + "velocityY": 1.2535628381323076, + "timestamp": 1.718731916963431 + }, + { + "x": 2.1697087770291006, + "y": 4.73476601037198, + "heading": -0.48296289489208905, + "angularVelocity": 0.8293499365978624, + "velocityX": -1.5410168966266635, + "velocityY": 1.4987747556657698, + "timestamp": 1.7997061424411083 + }, + { + "x": 2.0245178868987646, + "y": 4.87598561985106, + "heading": -0.4047647421109401, + "angularVelocity": 0.9657165884553096, + "velocityX": -1.7930506809269309, + "velocityY": 1.7440069188183338, + "timestamp": 1.8806803679187856 + }, + { + "x": 1.8612798092271896, + "y": 5.034863868368553, + "heading": -0.31505899294827, + "angularVelocity": 1.1078309009257712, + "velocityX": -2.0159263853239904, + "velocityY": 1.9620842012417825, + "timestamp": 1.9616545933964629 + }, + { + "x": 1.7184500483895917, + "y": 5.1738844559195085, + "heading": -0.23644393225782434, + "angularVelocity": 0.9708652380027345, + "velocityX": -1.7638916580562496, + "velocityY": 1.7168498584691285, + "timestamp": 2.04262881887414 + }, + { + "x": 1.5960267405860193, + "y": 5.293046114041946, + "heading": -0.168968478820667, + "angularVelocity": 0.8332954472751476, + "velocityX": -1.5118799479886125, + "velocityY": 1.4715998506873489, + "timestamp": 2.1236030443518175 + }, + { + "x": 1.4940086408037947, + "y": 5.392348179003114, + "heading": -0.1126782173092116, + "angularVelocity": 0.6951627036802206, + "velocityX": -1.2598836133403888, + "velocityY": 1.2263416460657963, + "timestamp": 2.2045772698294948 + }, + { + "x": 1.4123948378953157, + "y": 5.471790254952654, + "heading": -0.06761272810355828, + "angularVelocity": 0.5565411578785837, + "velocityX": -1.0078985310085185, + "velocityY": 0.9810785528472173, + "timestamp": 2.285551495307172 + }, + { + "x": 1.3511847156576737, + "y": 5.531372091014257, + "heading": -0.03380194743073204, + "angularVelocity": 0.417549911387026, + "velocityX": -0.7559210585414546, + "velocityY": 0.7358123614043836, + "timestamp": 2.3665257207848494 + }, + { + "x": 1.3103779403872662, + "y": 5.571093500047757, + "heading": -0.011263323026729973, + "angularVelocity": 0.27834319218308196, + "velocityX": -0.50394770718331, + "velocityY": 0.4905438588537352, + "timestamp": 2.4474999462625266 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -2.143037766767562e-26, - "angularVelocity": 0.11787086245509872, - "velocityX": -0.21338776476374371, - "velocityY": 0.20771554341884355, - "timestamp": 2.6818217560215256 + "heading": 4.4390879396333275e-31, + "angularVelocity": 0.13909763212046558, + "velocityX": -0.25197510938629825, + "velocityY": 0.24527315422336363, + "timestamp": 2.528474171740204 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.6489738584934847e-26, - "angularVelocity": 6.447188733213427e-26, - "velocityX": -2.3207888161938065e-29, - "velocityY": -7.774803790845182e-27, - "timestamp": 2.7584534933373352 - }, - { - "x": 1.3242756726096836, - "y": 5.5904571777752485, - "heading": -6.485537692324558e-18, - "angularVelocity": -6.918907801272556e-17, - "velocityX": 0.3659270961745246, - "velocityY": -0.005303363934441593, - "timestamp": 2.8521913496417257 - }, - { - "x": 1.3928781149651148, - "y": 5.58946292585347, - "heading": -1.1202216291674618e-17, - "angularVelocity": -5.031804212956606e-17, - "velocityX": 0.7318541845207394, - "velocityY": -0.010606727755427862, - "timestamp": 2.945929205946116 - }, - { - "x": 1.4957817770305903, - "y": 5.587971547992073, - "heading": -1.4372763095426617e-17, - "angularVelocity": -3.382497812599112e-17, - "velocityX": 1.0977812611239222, - "velocityY": -0.015910091406222935, - "timestamp": 3.0396670622505066 - }, - { - "x": 1.6329866569713576, - "y": 5.585983044217648, - "heading": -1.5623455517606444e-17, - "angularVelocity": -1.3343083564783124e-17, - "velocityX": 1.4637083181538741, - "velocityY": -0.021213454773344104, - "timestamp": 3.133404918554897 - }, - { - "x": 1.8044927511173796, - "y": 5.583497414583386, - "heading": -1.5212462606875703e-17, - "angularVelocity": 4.38274958491921e-18, - "velocityX": 1.8296353360316953, - "velocityY": -0.02651681757303531, - "timestamp": 3.2271427748592876 - }, - { - "x": 2.0103000484542934, - "y": 5.580514659248916, - "heading": -1.3020782793974884e-17, - "angularVelocity": 2.337996988615278e-17, - "velocityX": 2.1955622364077554, - "velocityY": -0.031820178669779105, - "timestamp": 3.320880631163678 - }, - { - "x": 2.181845878009789, - "y": 5.578028453731261, - "heading": -1.0808447718251693e-17, - "angularVelocity": 2.360324840579071e-17, - "velocityX": 1.8300592334557981, - "velocityY": -0.026522961098165376, - "timestamp": 3.4146184874680685 - }, - { - "x": 2.3190905043488854, - "y": 5.576039373914184, - "heading": -1.0180400900611551e-17, - "angularVelocity": 6.701278506879843e-18, - "velocityX": 1.4641323328076266, - "velocityY": -0.021219599997477887, - "timestamp": 3.508356343772459 - }, - { - "x": 2.4220339164763502, - "y": 5.574547419957035, - "heading": -1.1310869476605041e-17, - "angularVelocity": -1.2059406429055699e-17, - "velocityX": 1.0982053148617765, - "velocityY": -0.015916237196800742, - "timestamp": 3.6020942000768494 - }, - { - "x": 2.490676110725689, - "y": 5.573552591912956, - "heading": -5.948170117055629e-18, - "angularVelocity": 5.721104182574728e-17, - "velocityX": 0.7322782578015892, - "velocityY": -0.010612873829241372, - "timestamp": 3.69583205638124 - }, - { - "x": 2.5250170852633893, - "y": 5.573054889808518, - "heading": -2.1807385415410986e-18, - "angularVelocity": 4.0191949957409276e-17, - "velocityX": 0.36635118118139864, - "velocityY": -0.0053095101781998125, - "timestamp": 3.7895699126856304 + "heading": 2.494908675164405e-31, + "angularVelocity": -2.400971097599731e-30, + "velocityX": 9.117669624782265e-34, + "velocityY": -1.69916768015881e-32, + "timestamp": 2.6094483972178812 + }, + { + "x": 1.3242823472826057, + "y": 5.590457081039532, + "heading": -3.1525835471969304e-19, + "angularVelocity": -3.554932323947037e-18, + "velocityX": 0.3868644475376546, + "velocityY": -0.00560680796813751, + "timestamp": 2.6981303540012136 + }, + { + "x": 1.3928981385918453, + "y": 5.589462635652003, + "heading": -9.247304393859629e-19, + "angularVelocity": -6.8725601776619685e-18, + "velocityX": 0.7737288823800034, + "velocityY": -0.011213615752282564, + "timestamp": 2.786812310784546 + }, + { + "x": 1.4958218231349187, + "y": 5.587970967605794, + "heading": -1.5461935946313141e-18, + "angularVelocity": -7.007774498748477e-18, + "velocityX": 1.1605932962726162, + "velocityY": -0.016820423232804093, + "timestamp": 2.8754942675678783 + }, + { + "x": 1.6330533972641115, + "y": 5.585982076953771, + "heading": -2.4826620193785056e-18, + "angularVelocity": -1.0559852970262818e-17, + "velocityX": 1.5474576690326838, + "velocityY": -0.022427230117193615, + "timestamp": 2.9641762243512106 + }, + { + "x": 1.8045928505515578, + "y": 5.5834959638470645, + "heading": -3.548549667861495e-18, + "angularVelocity": -1.201921661569969e-17, + "velocityX": 1.934321924205522, + "velocityY": -0.028034035297397054, + "timestamp": 3.052858181134543 + }, + { + "x": 2.0104398981448357, + "y": 5.5805126324140275, + "heading": -4.504226548304647e-18, + "angularVelocity": -1.0776452337176765e-17, + "velocityX": 2.321182967310963, + "velocityY": -0.033640793925260974, + "timestamp": 3.1415401379178753 + }, + { + "x": 2.1819790701409207, + "y": 5.578026523384064, + "heading": -4.574305172786567e-18, + "angularVelocity": -7.902241563425968e-19, + "velocityX": 1.934318752293539, + "velocityY": -0.02803398932702808, + "timestamp": 3.2302220947012077 + }, + { + "x": 2.31921035343729, + "y": 5.576037636947067, + "heading": -3.929985868072649e-18, + "angularVelocity": 7.2655061760547e-18, + "velocityX": 1.547454389528782, + "velocityY": -0.022427182587500143, + "timestamp": 3.31890405148454 + }, + { + "x": 2.422133743893467, + "y": 5.574545973163046, + "heading": -2.934544233389305e-18, + "angularVelocity": 1.1224849685211679e-17, + "velocityX": 1.1605899800749648, + "velocityY": -0.016820375171309878, + "timestamp": 3.4075860082678724 + }, + { + "x": 2.490749239479307, + "y": 5.573551532061422, + "heading": -1.705878005512682e-18, + "angularVelocity": 1.385474872728055e-17, + "velocityX": 0.7737255477287375, + "velocityY": -0.011213567423341012, + "timestamp": 3.4962679650512047 }, { "x": 2.525056838989258, "y": 5.573054313659668, - "heading": -2.8826489417868528e-33, - "angularVelocity": 2.326435027113688e-17, - "velocityX": 0.0004240928242990202, - "velocityY": -0.000006146357055788897, - "timestamp": 3.883307768990021 - }, - { - "x": 2.4907821260381073, - "y": 5.573551055438095, - "heading": 8.600966160221058e-18, - "angularVelocity": 9.173661299705183e-17, - "velocityX": -0.3655736854951678, - "velocityY": 0.00529824197033788, - "timestamp": 3.9770637316095705 - }, - { - "x": 2.422192939812278, - "y": 5.57454511523942, - "heading": 1.5616353080474726e-17, - "angularVelocity": 7.482552449285854e-17, - "velocityX": -0.7315714559878368, - "velocityY": 0.010602630184298141, - "timestamp": 4.070819694229121 - }, - { - "x": 2.3192892814125363, - "y": 5.57603649304769, - "heading": 2.109129083859237e-17, - "angularVelocity": 5.839395174918292e-17, - "velocityX": -1.0975692147397424, - "velocityY": 0.015907018228100082, - "timestamp": 4.164575656848671 - }, - { - "x": 2.182071152673635, - "y": 5.578025188836313, - "heading": 2.4503256173748732e-17, - "angularVelocity": 3.639115358132533e-17, - "velocityX": -1.4635669539221987, - "velocityY": 0.021211405988282927, - "timestamp": 4.258331619468221 - }, - { - "x": 2.010538557265611, - "y": 5.5805112025521, - "heading": 2.631777380152167e-17, - "angularVelocity": 1.9353493618514127e-17, - "velocityX": -1.8295646539600892, - "velocityY": 0.026515793181145452, - "timestamp": 4.352087582087771 - }, - { - "x": 1.8046915062028241, - "y": 5.58349453403542, - "heading": 2.6479996589742425e-17, - "angularVelocity": 1.7308836974551526e-18, - "velocityX": -2.1955622365189336, - "velocityY": 0.03182017867138977, - "timestamp": 4.445843544707321 - }, - { - "x": 1.6331191680538633, - "y": 5.585981123740855, - "heading": 1.776473605055726e-17, - "angularVelocity": -9.295501651565233e-17, - "velocityX": -1.82998855142187, - "velocityY": 0.026521936706821576, - "timestamp": 4.539599507326871 - }, - { - "x": 1.495861285585166, - "y": 5.587970395678386, - "heading": 1.0702828905507157e-17, - "angularVelocity": -7.532110300046076e-17, - "velocityX": -1.4639909685910264, - "velocityY": 0.021217551212635195, - "timestamp": 4.633355469946421 - }, - { - "x": 1.3929178697919677, - "y": 5.589462349688662, - "heading": 5.2389574400340815e-18, - "angularVelocity": -5.827717130868218e-17, - "velocityX": -1.097993268485135, - "velocityY": 0.01591316401878714, - "timestamp": 4.727111432565971 - }, - { - "x": 1.3242889243407625, - "y": 5.590456985718543, - "heading": 1.764781469065966e-18, - "angularVelocity": -3.705591096010782e-17, - "velocityX": -0.7319955292724558, - "velocityY": 0.010608776258166279, - "timestamp": 4.820867395185521 + "heading": 0, + "angularVelocity": 1.9235908491289617e-17, + "velocityX": 0.3868611017883933, + "velocityY": -0.005606759478353257, + "timestamp": 3.584949921834537 + }, + { + "x": 2.5176787496862993, + "y": 5.573161243955685, + "heading": 2.6679509724403924e-18, + "angularVelocity": 2.5453436150449368e-17, + "velocityX": -0.07039024589473145, + "velocityY": 0.0010201624731186972, + "timestamp": 3.689766849367258 + }, + { + "x": 2.4623729801708616, + "y": 5.573962787832815, + "heading": 6.066089004278022e-18, + "angularVelocity": 3.2419744709429903e-17, + "velocityX": -0.5276415824931802, + "velocityY": 0.007647084263940974, + "timestamp": 3.794583776899979 + }, + { + "x": 2.3591395323793796, + "y": 5.575458945262994, + "heading": 1.0152873197492895e-17, + "angularVelocity": 3.898973466799139e-17, + "velocityX": -0.9848929006171766, + "velocityY": 0.014274005787013901, + "timestamp": 3.8994007044326997 + }, + { + "x": 2.207978410184721, + "y": 5.577649716190093, + "heading": 1.5606892400662525e-17, + "angularVelocity": 5.2033763358184983e-17, + "velocityX": -1.4421441817922973, + "velocityY": 0.020900926774588543, + "timestamp": 4.004217631965421 + }, + { + "x": 2.0088896252054647, + "y": 5.580535100445723, + "heading": 1.7204191476600093e-18, + "angularVelocity": -1.324831168006478e-16, + "velocityX": -1.8993953521210223, + "velocityY": 0.02752784615567168, + "timestamp": 4.109034559498142 + }, + { + "x": 1.769251248155021, + "y": 5.584008168003288, + "heading": 6.282149498472273e-20, + "angularVelocity": -1.5814217146918653e-17, + "velocityX": -2.2862564539075523, + "velocityY": 0.03313460563399779, + "timestamp": 4.213851487030864 + }, + { + "x": 1.577540533889042, + "y": 5.586786622232322, + "heading": -2.476430565711122e-19, + "angularVelocity": -2.961969587010803e-18, + "velocityX": -1.829005283580098, + "velocityY": 0.02650768625293313, + "timestamp": 4.318668414563585 + }, + { + "x": 1.4337574940262061, + "y": 5.588870462964437, + "heading": -3.350775678748249e-19, + "angularVelocity": -8.341640359227092e-19, + "velocityX": -1.3717540024052963, + "velocityY": 0.01988076526536314, + "timestamp": 4.423485342096306 + }, + { + "x": 1.3379021324393994, + "y": 5.590259690143503, + "heading": -2.0793104080618583e-19, + "angularVelocity": 1.2130342880824037e-18, + "velocityX": -0.914502684281442, + "velocityY": 0.013253843742292293, + "timestamp": 4.528302269629028 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -1.882241835148467e-17, - "velocityX": -0.3659977705035494, - "velocityY": 0.00530438821411795, - "timestamp": 4.9146233578050715 + "heading": -1.4049323971929142e-31, + "angularVelocity": 1.9837543963605952e-18, + "velocityX": -0.4572513476830733, + "velocityY": 0.006626921951471195, + "timestamp": 4.633119197161749 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.5626059218246172e-29, - "velocityY": -2.5406263809505943e-29, - "timestamp": 5.008379320424622 - }, - { - "x": 1.3072041550588713, - "y": 5.60554242334907, - "heading": 0.009976426632135488, - "angularVelocity": 0.13081140582840126, - "velocityX": 0.22591674199036743, - "velocityY": 0.19128015517323346, - "timestamp": 5.084645044894665 - }, - { - "x": 1.341663665671124, - "y": 5.634718250195267, - "heading": 0.02992724315217698, - "angularVelocity": 0.26159610576452386, - "velocityX": 0.4518348296001005, - "velocityY": 0.3825549033430322, - "timestamp": 5.160910769364708 - }, - { - "x": 1.3933533134136329, - "y": 5.678481259628573, - "heading": 0.05984264331407307, - "angularVelocity": 0.3922522257112071, - "velocityX": 0.6777572508972288, - "velocityY": 0.5738227722565116, - "timestamp": 5.237176493834752 - }, - { - "x": 1.4622736330550405, - "y": 5.7368307766764, - "heading": 0.09970739234331524, - "angularVelocity": 0.5227085865313741, - "velocityX": 0.9036866841075589, - "velocityY": 0.7650817908955898, - "timestamp": 5.313442218304795 - }, - { - "x": 1.548425323078623, - "y": 5.809765941188483, - "heading": 0.1495038423997354, - "angularVelocity": 0.652933547987283, - "velocityX": 1.1296252758655412, - "velocityY": 0.9563295312013731, - "timestamp": 5.389707942774838 - }, - { - "x": 1.6518091876527043, - "y": 5.897285689503854, - "heading": 0.20921528267987785, - "angularVelocity": 0.7829393965514132, - "velocityX": 1.3555744119188056, - "velocityY": 1.1475633247313204, - "timestamp": 5.4659736672448815 - }, - { - "x": 1.7724260670196361, - "y": 5.999388766272788, - "heading": 0.27882903202196435, - "angularVelocity": 0.9127789690876109, - "velocityX": 1.5815345652717379, - "velocityY": 1.3387806580529145, - "timestamp": 5.542239391714925 - }, - { - "x": 1.910276772460734, - "y": 6.11607377416316, - "heading": 0.3583384554068513, - "angularVelocity": 1.042531542921605, - "velocityX": 1.807505356185654, - "velocityY": 1.5299796694859533, - "timestamp": 5.618505116184968 - }, - { - "x": 2.065362059261307, - "y": 6.247339244509118, - "heading": 0.4477430189105342, - "angularVelocity": 1.172277115920654, - "velocityX": 2.033486050437564, - "velocityY": 1.7211594236996055, - "timestamp": 5.6947708406550115 - }, - { - "x": 2.203284027987246, - "y": 6.363982908806699, - "heading": 0.5270469936467461, - "angularVelocity": 1.0398376893804102, - "velocityX": 1.8084397630070208, - "velocityY": 1.5294375694163855, - "timestamp": 5.771036565125055 - }, - { - "x": 2.323970561231782, - "y": 6.4660479017472055, - "heading": 0.5964497874037734, - "angularVelocity": 0.9100129083298684, - "velocityX": 1.5824478694623139, - "velocityY": 1.3382813001795226, - "timestamp": 5.847302289595098 - }, - { - "x": 2.427421209360281, - "y": 6.553534871396325, - "heading": 0.6559501239234673, - "angularVelocity": 0.7801713929154328, - "velocityX": 1.3564500802865749, - "velocityY": 1.147133528411577, - "timestamp": 5.923568014065141 - }, - { - "x": 2.5136355947658133, - "y": 6.6264442966918295, - "heading": 0.7055475231333362, - "angularVelocity": 0.6503235827654283, - "velocityX": 1.1304473399101456, - "velocityY": 0.955992036499409, - "timestamp": 5.999833738535185 - }, - { - "x": 2.582613392420247, - "y": 6.6847765351095845, - "heading": 0.7452421346528295, - "angularVelocity": 0.5204777344326409, - "velocityX": 0.9044403377486854, - "velocityY": 0.7648552318285594, - "timestamp": 6.076099463005228 - }, - { - "x": 2.634354338508759, - "y": 6.728531844563373, - "heading": 0.7750344345644219, - "angularVelocity": 0.3906381285168185, - "velocityX": 0.6784298764341418, - "velocityY": 0.5737218089360453, - "timestamp": 6.152365187475271 - }, - { - "x": 2.6688582435593817, - "y": 6.757710392622863, - "heading": 0.7949250057401149, - "angularVelocity": 0.26080616524767886, - "velocityX": 0.4524169307537305, - "velocityY": 0.3825905832253757, - "timestamp": 6.228630911945315 - }, - { - "x": 2.686125003210816, - "y": 6.772312259170968, - "heading": 0.8049144291757789, - "angularVelocity": 0.13098182002535327, - "velocityX": 0.22640261668277428, - "velocityY": 0.19146040494338912, - "timestamp": 6.304896636415358 + "heading": 4.571690374498227e-31, + "angularVelocity": 5.701965460392164e-30, + "velocityX": 1.9882633246635272e-32, + "velocityY": -1.2353765948799823e-31, + "timestamp": 4.73793612469447 + }, + { + "x": 1.3072266031673745, + "y": 5.605560947932051, + "heading": 0.009988018032587274, + "angularVelocity": 0.1383542475734168, + "velocityX": 0.2389771940088317, + "velocityY": 0.20233155967170455, + "timestamp": 4.810127750233767 + }, + { + "x": 1.3417310637547577, + "y": 5.63477381293564, + "heading": 0.029960721397497427, + "angularVelocity": 0.2766623305087681, + "velocityX": 0.4779565542351512, + "velocityY": 0.40465725470744984, + "timestamp": 4.882319375773064 + }, + { + "x": 1.393488225984334, + "y": 5.678592353413093, + "heading": 0.05990698047800166, + "angularVelocity": 0.4148162457460571, + "velocityX": 0.7169413604826331, + "velocityY": 0.606975395693467, + "timestamp": 4.954511001312361 + }, + { + "x": 1.4624987017850304, + "y": 5.737015858733095, + "heading": 0.09981017735525757, + "angularVelocity": 0.5527399692025371, + "velocityX": 0.9559346431831991, + "velocityY": 0.8092836929987566, + "timestamp": 5.026702626851658 + }, + { + "x": 1.54876329298479, + "y": 5.810043407637518, + "heading": 0.1496511403376383, + "angularVelocity": 0.6903981259608908, + "velocityX": 1.1949390328222467, + "velocityY": 1.0115792290159562, + "timestamp": 5.098894252390955 + }, + { + "x": 1.6522829608961203, + "y": 5.897673825591776, + "heading": 0.20941128647918397, + "angularVelocity": 0.8277988713391718, + "velocityX": 1.4339567385834835, + "velocityY": 1.2138584953540306, + "timestamp": 5.171085877930252 + }, + { + "x": 1.7730588392541233, + "y": 5.999905624129209, + "heading": 0.2790750774652724, + "angularVelocity": 0.9649843796382769, + "velocityX": 1.6729901488685341, + "velocityY": 1.4161171434188378, + "timestamp": 5.243277503469549 + }, + { + "x": 1.911092570994838, + "y": 6.1167366962116585, + "heading": 0.35862892849655154, + "angularVelocity": 1.1019817109951129, + "velocityX": 1.9120463171400826, + "velocityY": 1.6183466047443176, + "timestamp": 5.315469129008846 + }, + { + "x": 2.0662405750494774, + "y": 6.247971693585017, + "heading": 0.4478813931179114, + "angularVelocity": 1.2363271218036194, + "velocityX": 2.1491135972568896, + "velocityY": 1.8178700977147564, + "timestamp": 5.3876607545481425 + }, + { + "x": 2.204132997305319, + "y": 6.36460684022272, + "heading": 0.5272152358459444, + "angularVelocity": 1.0989341510922093, + "velocityX": 1.9100888950161639, + "velocityY": 1.6156326411317472, + "timestamp": 5.4598523800874394 + }, + { + "x": 2.3247684211138937, + "y": 6.466643771235888, + "heading": 0.5966353508762373, + "angularVelocity": 0.9616089748867341, + "velocityX": 1.6710445693293852, + "velocityY": 1.4134178341442696, + "timestamp": 5.532044005626736 + }, + { + "x": 2.428146136331826, + "y": 6.5540833745423095, + "heading": 0.6561424196084651, + "angularVelocity": 0.8242932374446845, + "velocityX": 1.4319904067208047, + "velocityY": 1.2112153266137329, + "timestamp": 5.604235631166033 + }, + { + "x": 2.51426563036811, + "y": 6.626926253781894, + "heading": 0.705736875442574, + "angularVelocity": 0.686983503468318, + "velocityX": 1.192929143690227, + "velocityY": 1.0090211807166112, + "timestamp": 5.67642725670533 + }, + { + "x": 2.5831264968268224, + "y": 6.6851728424603465, + "heading": 0.7454193798278042, + "angularVelocity": 0.5496829319015093, + "velocityX": 0.95386225125429, + "velocityY": 0.8068330397501333, + "timestamp": 5.748618882244627 + }, + { + "x": 2.6347284176402783, + "y": 6.72882344947858, + "heading": 0.7751907373365541, + "angularVelocity": 0.4123935052894287, + "velocityX": 0.7147909529377248, + "velocityY": 0.6046491776859777, + "timestamp": 5.820810507783924 + }, + { + "x": 2.669071164543137, + "y": 6.75787827866861, + "heading": 0.7950517689437672, + "angularVelocity": 0.27511545084049754, + "velocityX": 0.4757164926845912, + "velocityY": 0.4024681391074233, + "timestamp": 5.893002133323221 }, { "x": 2.68615460395813, "y": 6.772337436676025, "heading": 0.8050032485420815, - "angularVelocity": 0.0011646034399446482, - "velocityX": 0.0003881255345023293, - "velocityY": 0.00033012790587399484, - "timestamp": 6.381162360885401 - }, - { - "x": 2.668940238370334, - "y": 6.757780008152215, - "heading": 0.7951880324492147, - "angularVelocity": -0.12867188925922995, - "velocityX": -0.22567052234080015, - "velocityY": -0.1908395916273825, - "timestamp": 6.457443327229603 - }, - { - "x": 2.6344820969778255, - "y": 6.728639813103283, - "heading": 0.7754676749092158, - "angularVelocity": -0.2585226492096862, - "velocityX": -0.4517265968129291, - "velocityY": -0.3820113508147273, - "timestamp": 6.533724293573805 - }, - { - "x": 2.582780444824575, - "y": 6.6849166026467195, - "heading": 0.7458414179763078, - "angularVelocity": -0.3883833453977379, - "velocityX": -0.6777791967096236, - "velocityY": -0.5731863727099495, - "timestamp": 6.610005259918006 - }, - { - "x": 2.513835599527289, - "y": 6.6266100250455136, - "heading": 0.7063091354285856, - "angularVelocity": -0.518245696283539, - "velocityX": -0.9038276328322399, - "velocityY": -0.7643660056769808, - "timestamp": 6.686286226262208 - }, - { - "x": 2.427647916545295, - "y": 6.553719605348423, - "heading": 0.6568717276359699, - "angularVelocity": -0.6480962438334487, - "velocityX": -1.1298714090855582, - "velocityY": -0.9555518649841811, - "timestamp": 6.76256719260641 - }, - { - "x": 2.3242177842468106, - "y": 6.466244710155765, - "heading": 0.5975314416664378, - "angularVelocity": -0.7779173336036685, - "velocityX": -1.3559100940527828, - "velocityY": -1.1467460278003494, - "timestamp": 6.838848158950611 - }, - { - "x": 2.2035456412981804, - "y": 6.364184497615964, - "heading": 0.5282916867852259, - "angularVelocity": -0.907693729851007, - "velocityX": -1.5819430283540434, - "velocityY": -1.3379512268110498, - "timestamp": 6.915129125294813 - }, - { - "x": 2.0656320318873616, - "y": 6.247537861954298, - "heading": 0.4491555706593054, - "angularVelocity": -1.0374293863583475, - "velocityX": -1.8079688286610023, - "velocityY": -1.529170921898447, - "timestamp": 6.991410091639015 - }, - { - "x": 1.9104777180009005, - "y": 6.1163033962265265, - "heading": 0.3601220361949963, - "angularVelocity": -1.1671789007706412, - "velocityX": -2.0339846397576693, - "velocityY": -1.7204090612237575, - "timestamp": 7.067691057983216 - }, - { - "x": 1.7725837401718476, - "y": 5.99956344062706, - "heading": 0.2802286748074377, - "angularVelocity": -1.047356440677276, - "velocityX": -1.8077114707660742, - "velocityY": -1.530394293900403, - "timestamp": 7.143972024327418 - }, - { - "x": 1.6519287666788485, - "y": 5.897413817352657, - "heading": 0.21025683762829658, - "angularVelocity": -0.9172909120831881, - "velocityX": -1.581717947527036, - "velocityY": -1.3391233513887155, - "timestamp": 7.22025299067162 - }, - { - "x": 1.5485118574908003, - "y": 5.80985551317875, - "heading": 0.15022948194911198, - "angularVelocity": -0.7869244264698967, - "velocityX": -1.3557367475609723, - "velocityY": -1.1478394728127326, - "timestamp": 7.296533957015821 - }, - { - "x": 1.4623321603791597, - "y": 5.736889288541215, - "heading": 0.10017126259617518, - "angularVelocity": -0.6562347301302999, - "velocityX": -1.1297667198257733, - "velocityY": -0.9565456253555378, - "timestamp": 7.372814923360023 - }, - { - "x": 1.393388970887201, - "y": 5.678515706716561, - "heading": 0.06010509802987578, - "angularVelocity": -0.5252445858334249, - "velocityX": -0.9038059271169646, - "velocityY": -0.7652443936641671, - "timestamp": 7.449095889704225 - }, - { - "x": 1.3416817772407035, - "y": 5.634735172181737, - "heading": 0.030048985724868114, - "angularVelocity": -0.39401850489866874, - "velocityX": -0.6778518434766467, - "velocityY": -0.5739378595104004, - "timestamp": 7.525376856048426 - }, - { - "x": 1.307210288791871, - "y": 5.605547971029586, - "heading": 0.010013480371388656, - "angularVelocity": -0.2626540579888315, - "velocityX": -0.4519015700633713, - "velocityY": -0.38262757484247345, - "timestamp": 7.601657822392628 + "angularVelocity": 0.13784811636956903, + "velocityX": 0.23664018211616192, + "velocityY": 0.20028857778593961, + "timestamp": 5.965193758862518 + }, + { + "x": 2.6837128162746895, + "y": 6.770286154899203, + "heading": 0.8037616945531193, + "angularVelocity": -0.015403237566514301, + "velocityX": -0.03029383829428867, + "velocityY": -0.025449058845075586, + "timestamp": 6.045797202361987 + }, + { + "x": 2.6597554401054357, + "y": 6.750039490992799, + "heading": 0.790165886718463, + "angularVelocity": -0.1686752729700538, + "velocityX": -0.29722521928375617, + "velocityY": -0.2511885724392666, + "timestamp": 6.126400645861456 + }, + { + "x": 2.614282818196885, + "y": 6.71159714288196, + "heading": 0.7642144593793307, + "angularVelocity": -0.32196425130707784, + "velocityX": -0.5641523480167241, + "velocityY": -0.4769318337003116, + "timestamp": 6.207004089360925 + }, + { + "x": 2.547295402718566, + "y": 6.65495864191724, + "heading": 0.7259067765041173, + "angularVelocity": -0.4752611205179619, + "velocityX": -0.8310738669455855, + "velocityY": -0.702680909222201, + "timestamp": 6.287607532860394 + }, + { + "x": 2.45879373609576, + "y": 6.580123315301835, + "heading": 0.6752436075608932, + "angularVelocity": -0.6285484433864407, + "velocityX": -1.0979886563197567, + "velocityY": -0.9284383317423857, + "timestamp": 6.3682109763598636 + }, + { + "x": 2.3487784541804713, + "y": 6.487090205965796, + "heading": 0.612227929672179, + "angularVelocity": -0.7817988308289465, + "velocityX": -1.3648955570482664, + "velocityY": -1.1542076280722422, + "timestamp": 6.448814419859333 + }, + { + "x": 2.2172503640031413, + "y": 6.3758579034961445, + "heading": 0.536865388228733, + "angularVelocity": -0.934979179194793, + "velocityX": -1.6317924454199069, + "velocityY": -1.3799944225748537, + "timestamp": 6.529417863358802 + }, + { + "x": 2.0642108270205712, + "y": 6.2464240090225065, + "heading": 0.4491644340858314, + "angularVelocity": -1.0880546827189126, + "velocityX": -1.8986724429895898, + "velocityY": -1.605810978465707, + "timestamp": 6.610021306858271 + }, + { + "x": 1.892150582189426, + "y": 6.100772712319997, + "heading": 0.34952884105977006, + "angularVelocity": -1.236120799562961, + "velocityX": -2.134651292315194, + "velocityY": -1.8070108469188693, + "timestamp": 6.69062475035774 + }, + { + "x": 1.7416020056028705, + "y": 5.973323101720471, + "heading": 0.2622670318762804, + "angularVelocity": -1.0826064668575972, + "velocityX": -1.8677685474764083, + "velocityY": -1.5811931235965593, + "timestamp": 6.771228193857209 + }, + { + "x": 1.6125631547118695, + "y": 5.864077706390992, + "heading": 0.18740079602910192, + "angularVelocity": -0.9288218045882161, + "velocityX": -1.600909902712647, + "velocityY": -1.3553440223708513, + "timestamp": 6.851831637356678 + }, + { + "x": 1.5050325714338102, + "y": 5.7730380802152625, + "heading": 0.12496122653203612, + "angularVelocity": -0.7746513893974771, + "velocityX": -1.3340693475310352, + "velocityY": -1.1294756430146329, + "timestamp": 6.9324350808561475 + }, + { + "x": 1.4190090841759289, + "y": 5.700205295684954, + "heading": 0.0749803520329015, + "angularVelocity": -0.6200836134188114, + "velocityX": -1.0672433276187945, + "velocityY": -0.9035939578786114, + "timestamp": 7.013038524355617 + }, + { + "x": 1.3544918382019193, + "y": 5.645580092417221, + "heading": 0.037484747068995454, + "angularVelocity": -0.4651861426263309, + "velocityX": -0.8004279119223182, + "velocityY": -0.6777030967431115, + "timestamp": 7.093641967855086 + }, + { + "x": 1.3114803343182175, + "y": 5.609162978300701, + "heading": 0.012490529490997385, + "angularVelocity": -0.31008870704351826, + "velocityX": -0.5336186894297898, + "velocityY": -0.45180593452897394, + "timestamp": 7.174245411354555 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -6.199521898254585e-29, - "angularVelocity": -0.13127102150595574, - "velocityX": -0.22595201079473404, - "velocityY": -0.19131466197828328, - "timestamp": 7.67793878873683 + "heading": 1.0757846254441862e-30, + "angularVelocity": -0.1549627280016075, + "velocityX": -0.2668109738177861, + "velocityY": -0.22590442503082891, + "timestamp": 7.254848854854024 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.3170947337368488e-29, - "angularVelocity": 6.400644992589977e-28, - "velocityX": 2.2164409811256884e-29, - "velocityY": 1.733201459680474e-29, - "timestamp": 7.7542197550810315 - }, - { - "x": 1.3080029680398912, - "y": 5.59126500545377, - "heading": 8.890962339325397e-19, - "angularVelocity": 1.3082751589925716e-17, - "velocityX": 0.2652835543681239, - "velocityY": 0.004571871036655683, - "timestamp": 7.82217917690712 - }, - { - "x": 1.344062678768715, - "y": 5.591577976323001, - "heading": 2.6666437965292957e-18, - "angularVelocity": 2.6156013615677146e-17, - "velocityX": 0.5306064966400389, - "velocityY": 0.004605260916318704, - "timestamp": 7.890138598733209 - }, - { - "x": 1.398150210579935, - "y": 5.5915422166660145, - "heading": 8.768948399955272e-19, - "angularVelocity": -2.6335553017414816e-17, - "velocityX": 0.7958798111854469, - "velocityY": -0.000526191306891862, - "timestamp": 7.958098020559298 - }, - { - "x": 1.4702532783568034, - "y": 5.590755190471311, - "heading": -4.372782060498752e-19, - "angularVelocity": -1.9337613692601342e-17, - "velocityX": 1.060972354376158, - "velocityY": -0.011580825344825147, - "timestamp": 8.026057442385387 - }, - { - "x": 1.560346433856685, - "y": 5.58875133578056, - "heading": -1.4651998990372215e-18, - "angularVelocity": -1.512552145628718e-17, - "velocityX": 1.3256904352487537, - "velocityY": -0.029486046774778202, - "timestamp": 8.094016864211476 - }, - { - "x": 1.6683846538857667, - "y": 5.584987203919277, - "heading": -2.5518211074829943e-18, - "angularVelocity": -1.5989265053292237e-17, - "velocityX": 1.5897460149904699, - "velocityY": -0.05538793238877104, - "timestamp": 8.161976286037564 - }, - { - "x": 1.7942934964417667, - "y": 5.578822081824432, - "heading": -4.145349651168243e-18, - "angularVelocity": -2.3448235739309385e-17, - "velocityX": 1.852706205744448, - "velocityY": -0.0907176949006653, - "timestamp": 8.229935707863653 - }, - { - "x": 1.937953698284471, - "y": 5.569492606449359, - "heading": -1.0979652144550452e-17, - "angularVelocity": -1.0056445905133748e-16, - "velocityX": 2.1139114780925645, - "velocityY": -0.13728008750497092, - "timestamp": 8.297895129689742 - }, - { - "x": 2.099176605031056, - "y": 5.556079595057618, - "heading": -1.979270285045548e-17, - "angularVelocity": -1.2968107245612037e-16, - "velocityX": 2.3723407647457755, - "velocityY": -0.19736794444875114, - "timestamp": 8.365854551515831 - }, - { - "x": 2.2776642824157327, - "y": 5.537465473885052, - "heading": -2.5427652949352827e-17, - "angularVelocity": -8.291639256904635e-17, - "velocityX": 2.6263860490372255, - "velocityY": -0.2739005228767256, - "timestamp": 8.43381397334192 - }, - { - "x": 2.472944009712466, - "y": 5.512282419629533, - "heading": -2.829850545817078e-17, - "angularVelocity": -4.224362761891433e-17, - "velocityX": 2.8734754070813326, - "velocityY": -0.3705601604434455, - "timestamp": 8.501773395168009 - }, - { - "x": 2.6842610977752166, - "y": 5.47885759062817, - "heading": -2.887338329485776e-17, - "angularVelocity": -8.459133718914728e-18, - "velocityX": 3.109459768558945, - "velocityY": -0.49183509958190536, - "timestamp": 8.569732816994097 - }, - { - "x": 2.9104101060267347, - "y": 5.435179084259705, - "heading": -3.3223671712936815e-17, - "angularVelocity": -6.40130286748416e-17, - "velocityX": 3.32770647799568, - "velocityY": -0.6427145080816032, - "timestamp": 8.637692238820186 - }, - { - "x": 3.1494985869220757, - "y": 5.378942960028073, - "heading": -3.8165211155711695e-17, - "angularVelocity": -7.271308833998824e-17, - "velocityX": 3.5181064592806273, - "velocityY": -0.8274956248971932, - "timestamp": 8.705651660646275 - }, - { - "x": 3.3987095123055417, - "y": 5.307785046252999, - "heading": -4.255972014068572e-17, - "angularVelocity": -6.466371942097715e-17, - "velocityX": 3.667054820172052, - "velocityY": -1.0470647316154307, - "timestamp": 8.773611082472364 - }, - { - "x": 3.6542758900932353, - "y": 5.21975313201646, - "heading": -4.248444848952633e-17, - "angularVelocity": 1.107596991510937e-18, - "velocityX": 3.7605731614624154, - "velocityY": -1.2953599643889915, - "timestamp": 8.841570504298453 - }, - { - "x": 3.9119052041412368, - "y": 5.113808430044301, - "heading": -4.228369470192057e-17, - "angularVelocity": 2.9540243605887717e-18, - "velocityX": 3.790928573631551, - "velocityY": -1.5589406019856407, - "timestamp": 8.909529926124542 + "heading": 2.1112287936414426e-30, + "angularVelocity": 1.2846097126015132e-29, + "velocityX": 1.1473496118122496e-30, + "velocityY": -2.480854756847108e-31, + "timestamp": 7.335452298353493 + }, + { + "x": 1.3107332172473851, + "y": 5.587721554520521, + "heading": -5.6112723310966165e-9, + "angularVelocity": -8.086181485047517e-8, + "velocityX": 0.2991463288589595, + "velocityY": -0.04658586416314624, + "timestamp": 7.40484564929817 + }, + { + "x": 1.3522507006814268, + "y": 5.581255743814154, + "heading": -1.6136427820687844e-8, + "angularVelocity": -1.516738315771432e-7, + "velocityX": 0.5982919525984769, + "velocityY": -0.09317622824587016, + "timestamp": 7.4742390002428465 + }, + { + "x": 1.414526842614822, + "y": 5.571556496717738, + "heading": -3.074141680025046e-8, + "angularVelocity": -2.1046669010163338e-7, + "velocityX": 0.8974367296809845, + "velocityY": -0.13977199493000497, + "timestamp": 7.543632351187523 + }, + { + "x": 1.4975615711863124, + "y": 5.558623354737258, + "heading": -4.841098342837698e-8, + "angularVelocity": -2.546291003036043e-7, + "velocityX": 1.196580471199459, + "velocityY": -0.1863743687891775, + "timestamp": 7.6130257021322 + }, + { + "x": 1.6013547964876313, + "y": 5.542455744326475, + "heading": -6.788238796631325e-8, + "angularVelocity": -2.8059467012358587e-7, + "velocityX": 1.4957229170856399, + "velocityY": -0.2329850077952488, + "timestamp": 7.682419053076877 + }, + { + "x": 1.7259064027890496, + "y": 5.5230529273691795, + "heading": -8.754228960685774e-8, + "angularVelocity": -2.8331102694259857e-7, + "velocityX": 1.7948636952366863, + "velocityY": -0.27960628350061884, + "timestamp": 7.751812404021553 + }, + { + "x": 1.8712162355508042, + "y": 5.500413918486819, + "heading": -1.0525703637468966e-7, + "angularVelocity": -2.552801733617599e-7, + "velocityX": 2.0940022463766295, + "velocityY": -0.3262417591047122, + "timestamp": 7.82120575496623 + }, + { + "x": 2.0372840779688866, + "y": 5.474537335801765, + "heading": -1.1807218385471473e-7, + "angularVelocity": -1.8467399582926014e-7, + "velocityX": 2.393137673240176, + "velocityY": -0.3728971483980547, + "timestamp": 7.890599105910907 + }, + { + "x": 2.2241096038561166, + "y": 5.445421101366719, + "heading": -1.2162497814922265e-7, + "angularVelocity": -5.119790624873975e-8, + "velocityX": 2.692268399549916, + "velocityY": -0.41958248216406707, + "timestamp": 7.959992456855583 + }, + { + "x": 2.431692266948392, + "y": 5.413061738269229, + "heading": -1.0881824696224333e-7, + "angularVelocity": 1.845527118847122e-7, + "velocityX": 2.99139125386478, + "velocityY": -0.46631792033351455, + "timestamp": 8.02938580780026 + }, + { + "x": 2.660030964874599, + "y": 5.377452241496065, + "heading": -6.605232487729304e-8, + "angularVelocity": 6.162827019703426e-7, + "velocityX": 3.290498222347668, + "velocityY": -0.5131543049644982, + "timestamp": 8.098779158744936 + }, + { + "x": 2.9091223110729234, + "y": 5.338571186157936, + "heading": 4.364254321616404e-8, + "angularVelocity": 0.0000015807691363697316, + "velocityX": 3.5895563884342283, + "velocityY": -0.5602994351597471, + "timestamp": 8.168172509689612 + }, + { + "x": 3.168557402823767, + "y": 5.298953853406429, + "heading": 4.3641894183803e-8, + "angularVelocity": -9.352947371519385e-12, + "velocityX": 3.738615994458649, + "velocityY": -0.5709096363289035, + "timestamp": 8.237565860634287 + }, + { + "x": 3.4266802039415243, + "y": 5.251533194096367, + "heading": 4.3642354317485314e-8, + "angularVelocity": 6.630803609714539e-12, + "velocityX": 3.719705095716507, + "velocityY": -0.6833602739240744, + "timestamp": 8.306959211578963 + }, + { + "x": 3.6803449026923616, + "y": 5.18422554387417, + "heading": 4.364233337760302e-8, + "angularVelocity": -3.0175631893268995e-13, + "velocityX": 3.6554611543845263, + "velocityY": -0.9699437958523844, + "timestamp": 8.376352562523639 + }, + { + "x": 3.9278131468583686, + "y": 5.096843813495861, + "heading": 4.364231130349348e-8, + "angularVelocity": -3.1810121673972006e-13, + "velocityX": 3.5661665101502296, + "velocityY": -1.2592233865168847, + "timestamp": 8.445745913468315 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -4.208890664344976e-17, - "angularVelocity": 2.8662406659266305e-18, - "velocityX": 3.7609775714691667, - "velocityY": -1.8225675093785412, - "timestamp": 8.97748934795063 - }, - { - "x": 4.282012195193535, - "y": 4.930397905958738, - "heading": -4.1602494438105564e-17, - "angularVelocity": 1.582195542444361e-17, - "velocityX": 3.7248687742898503, - "velocityY": -1.9370313765192848, - "timestamp": 9.008232210782408 - }, - { - "x": 4.3952613097643365, - "y": 4.867381383869913, - "heading": -4.188915157617957e-17, - "angularVelocity": -9.32434756133803e-18, - "velocityX": 3.6837530450723373, - "velocityY": -2.049793554804778, - "timestamp": 9.038975073614186 - }, - { - "x": 4.506961712126711, - "y": 4.801015720721852, - "heading": -4.2434270081658926e-17, - "angularVelocity": -1.7731546618224465e-17, - "velocityX": 3.6333767279120854, - "velocityY": -2.158733996609517, - "timestamp": 9.069717936445963 - }, - { - "x": 4.616614691776683, - "y": 4.73158032272052, - "heading": -4.19288728234691e-17, - "angularVelocity": 1.6439498850683494e-17, - "velocityX": 3.5667784178065634, - "velocityY": -2.258585948266306, - "timestamp": 9.10046079927774 - }, - { - "x": 4.722598987733627, - "y": 4.661750165167655, - "heading": -3.986850310029329e-17, - "angularVelocity": 6.701944885386403e-17, - "velocityX": 3.4474439331457423, - "velocityY": -2.271426637622177, - "timestamp": 9.131203662109519 - }, - { - "x": 4.827229210309334, - "y": 4.595352451522488, - "heading": -3.770620807844877e-17, - "angularVelocity": 7.033486223050351e-17, - "velocityX": 3.4033988034307483, - "velocityY": -2.1597765311737476, - "timestamp": 9.161946524941296 - }, - { - "x": 4.9314726269952285, - "y": 4.532624292130925, - "heading": -3.5259240937147126e-17, - "angularVelocity": 7.959464135435417e-17, - "velocityX": 3.390816829789258, - "velocityY": -2.04041372902732, - "timestamp": 9.192689387773074 - }, - { - "x": 5.035668651391688, - "y": 4.473585715832336, - "heading": -3.400468055587437e-17, - "angularVelocity": 4.080818328915737e-17, - "velocityX": 3.389275259321544, - "velocityY": -1.9203994312970292, - "timestamp": 9.223432250604851 - }, - { - "x": 5.139985480442779, - "y": 4.418235050432876, - "heading": -3.272140280940892e-17, - "angularVelocity": 4.1742298155096073e-17, - "velocityX": 3.3932047780294075, - "velocityY": -1.8004395264792032, - "timestamp": 9.254175113436629 - }, - { - "x": 5.244522953443759, - "y": 4.366567672079215, - "heading": -3.123998369139628e-17, - "angularVelocity": 4.8187415925402766e-17, - "velocityX": 3.400381856855714, - "velocityY": -1.680630025784517, - "timestamp": 9.284917976268407 - }, - { - "x": 5.3493470515644415, - "y": 4.31857903504105, - "heading": -3.003359756723782e-17, - "angularVelocity": 3.924117707448193e-17, - "velocityX": 3.409705162927411, - "velocityY": -1.5609683880370886, - "timestamp": 9.315660839100184 - }, - { - "x": 5.454504582553393, - "y": 4.274265192865899, - "heading": -2.87901792623276e-17, - "angularVelocity": 4.044575522175447e-17, - "velocityX": 3.4205510255946114, - "velocityY": -1.4414351200027105, - "timestamp": 9.346403701931962 - }, - { - "x": 5.560030462239174, - "y": 4.233622810392966, - "heading": -2.7523978224966667e-17, - "angularVelocity": 4.118682909556349e-17, - "velocityX": 3.432532625969479, - "velocityY": -1.3220103376619752, - "timestamp": 9.37714656476374 + "heading": 4.364228801520247e-8, + "angularVelocity": -3.3559830331814666e-13, + "velocityX": 3.45401850865928, + "velocityY": -1.5404360240963577, + "timestamp": 8.51513926441299 + }, + { + "x": 4.284862555065841, + "y": 4.931880235034004, + "heading": 4.364226510492993e-8, + "angularVelocity": -6.617055985872344e-13, + "velocityX": 3.3897491580338537, + "velocityY": -1.67713541924223, + "timestamp": 8.549762324762018 + }, + { + "x": 4.399813643173519, + "y": 4.869172357324464, + "heading": 4.364224321511573e-8, + "angularVelocity": -6.322322259809295e-13, + "velocityX": 3.3200730076683276, + "velocityY": -1.81115929895842, + "timestamp": 8.584385385111045 + }, + { + "x": 4.512168964860153, + "y": 4.801924216205689, + "heading": 4.364222211690104e-8, + "angularVelocity": -6.093688654191888e-13, + "velocityX": 3.245100824537311, + "velocityY": -1.9422933859936342, + "timestamp": 8.619008445460071 + }, + { + "x": 4.621749311428581, + "y": 4.730243126546711, + "heading": 4.364220167371938e-8, + "angularVelocity": -5.904498836065766e-13, + "velocityX": 3.164952649008305, + "velocityY": -2.070327952999433, + "timestamp": 8.653631505809098 + }, + { + "x": 4.728650036507715, + "y": 4.654623908914118, + "heading": 4.364241447304788e-8, + "angularVelocity": 6.146173210402922e-12, + "velocityX": 3.0875585231776617, + "velocityY": -2.184070872715801, + "timestamp": 8.688254566158125 + }, + { + "x": 4.8372727310201835, + "y": 4.581499759061361, + "heading": 4.3642159997006347e-8, + "angularVelocity": -7.349900240613306e-12, + "velocityX": 3.137293278452735, + "velocityY": -2.112007116517435, + "timestamp": 8.722877626507152 + }, + { + "x": 4.948612503037568, + "y": 4.5125832438429105, + "heading": 4.3642137671211746e-8, + "angularVelocity": -6.448244222387315e-13, + "velocityX": 3.2157692270698366, + "velocityY": -1.9904801748811223, + "timestamp": 8.75750068685618 + }, + { + "x": 5.062589095284458, + "y": 4.448121106677303, + "heading": 4.364211455765666e-8, + "angularVelocity": -6.675769025667281e-13, + "velocityX": 3.2919271461828314, + "velocityY": -1.8618266703110185, + "timestamp": 8.792123747205206 + }, + { + "x": 5.17904891222934, + "y": 4.388261758970167, + "heading": 4.3642090495999965e-8, + "angularVelocity": -6.94960432877984e-13, + "velocityX": 3.3636488447548265, + "velocityY": -1.728886675634883, + "timestamp": 8.826746807554233 + }, + { + "x": 5.297806179962019, + "y": 4.333100707972697, + "heading": 4.364206516158606e-8, + "angularVelocity": -7.317208234311228e-13, + "velocityX": 3.4300049312658345, + "velocityY": -1.5931881942671235, + "timestamp": 8.86136986790326 + }, + { + "x": 5.418671454426997, + "y": 4.282725959570945, + "heading": 4.364203830903825e-8, + "angularVelocity": -7.755683028589255e-13, + "velocityX": 3.4908894028015367, + "velocityY": -1.454947884269515, + "timestamp": 8.895992928252287 + }, + { + "x": 5.541451926168614, + "y": 4.237217881100571, + "heading": 4.364200955008555e-8, + "angularVelocity": -8.306300130132497e-13, + "velocityX": 3.546205058244276, + "velocityY": -1.3143863659542425, + "timestamp": 8.930615988601314 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -2.6172162518713254e-17, - "angularVelocity": 4.3971692345317756e-17, - "velocityX": 3.4453937215027324, - "velocityY": -1.2026770584391975, - "timestamp": 9.407889427595517 - }, - { - "x": 5.792835589324764, - "y": 4.157776846380009, - "heading": -2.4556205828219047e-17, - "angularVelocity": 4.408549337244093e-17, - "velocityX": 3.461564053183983, - "velocityY": -1.060487182383484, - "timestamp": 9.444544494078327 - }, - { - "x": 5.920107057755266, - "y": 4.124135852213366, - "heading": -2.3047234390121686e-17, - "angularVelocity": 4.1166790375489185e-17, - "velocityX": 3.472138523884214, - "velocityY": -0.917772013383704, - "timestamp": 9.481199560561137 - }, - { - "x": 6.047516391811881, - "y": 4.095738620024666, - "heading": -2.1456366912742873e-17, - "angularVelocity": 4.3401025561499594e-17, - "velocityX": 3.475899685419091, - "velocityY": -0.7747150643422807, - "timestamp": 9.517854627043947 - }, - { - "x": 6.174755293992793, - "y": 4.0725841920992965, - "heading": -1.9852024700254233e-17, - "angularVelocity": 4.3768634637209445e-17, - "velocityX": 3.4712500723626603, - "velocityY": -0.631684243056213, - "timestamp": 9.554509693526757 - }, - { - "x": 6.30143822417369, - "y": 4.054645790921562, - "heading": -1.7231399343635e-17, - "angularVelocity": 7.149421916471826e-17, - "velocityX": 3.4560824010592928, - "velocityY": -0.48938394876864877, - "timestamp": 9.591164760009567 - }, - { - "x": 6.427078572160238, - "y": 4.041848309685929, - "heading": -1.4890621747726543e-17, - "angularVelocity": 6.385959215232344e-17, - "velocityX": 3.427639342721413, - "velocityY": -0.3491326701490028, - "timestamp": 9.627819826492377 - }, - { - "x": 6.551062514369357, - "y": 4.034028011406828, - "heading": -1.2144086622031307e-17, - "angularVelocity": 7.492920868069778e-17, - "velocityX": 3.382450343317836, - "velocityY": -0.21334835888971543, - "timestamp": 9.664474892975187 - }, - { - "x": 6.672633087813251, - "y": 4.030865145435154, - "heading": -9.61251747475391e-18, - "angularVelocity": 6.906464481423604e-17, - "velocityX": 3.316610365470415, - "velocityY": -0.08628727963586283, - "timestamp": 9.701129959457997 - }, - { - "x": 6.790917094390862, - "y": 4.031790616181229, - "heading": -6.811165774297211e-18, - "angularVelocity": 7.642467929421218e-17, - "velocityX": 3.2269483574141518, - "velocityY": 0.025248098963559784, - "timestamp": 9.737785025940807 - }, - { - "x": 6.905041403639001, - "y": 4.0359118549290525, - "heading": -4.3348375779027234e-18, - "angularVelocity": 6.755759664372386e-17, - "velocityX": 3.113466164402112, - "velocityY": 0.11243299066875824, - "timestamp": 9.774440092423617 - }, - { - "x": 7.014327663163953, - "y": 4.042060231899537, - "heading": -2.4762499879984938e-18, - "angularVelocity": 5.070479385914931e-17, - "velocityX": 2.9814775967247074, - "velocityY": 0.16773607472154842, - "timestamp": 9.811095158906427 - }, - { - "x": 7.1184276572510345, - "y": 4.048995163672738, - "heading": -1.0562416558517327e-18, - "angularVelocity": 3.873975601197627e-17, - "velocityX": 2.8399892313904544, - "velocityY": 0.1891943580692654, - "timestamp": 9.847750225389237 + "heading": 4.364197825891865e-8, + "angularVelocity": -9.037666511803314e-13, + "velocityX": 3.5958636064268057, + "velocityY": -1.171727921713515, + "timestamp": 8.965239048950341 + }, + { + "x": 5.80048142988227, + "y": 4.159069716430897, + "heading": 4.364194761543742e-8, + "angularVelocity": -8.296994367988738e-13, + "velocityX": 3.642511096804761, + "velocityY": -1.0174944855729342, + "timestamp": 9.00217227920995 + }, + { + "x": 5.936489765010674, + "y": 4.127254911109845, + "heading": 4.364191906311607e-8, + "angularVelocity": -7.730794282349154e-13, + "velocityX": 3.6825464269543904, + "velocityY": -0.8614141004570673, + "timestamp": 9.039105509469557 + }, + { + "x": 6.073729843507157, + "y": 4.101262406827453, + "heading": 4.3641892220156324e-8, + "angularVelocity": -7.267969466454947e-13, + "velocityX": 3.7158969722335238, + "velocityY": -0.7037701305758657, + "timestamp": 9.076038739729166 + }, + { + "x": 6.211952539114824, + "y": 4.081139381676687, + "heading": 4.3641866716107116e-8, + "angularVelocity": -6.905447558160659e-13, + "velocityX": 3.742502202923576, + "velocityY": -0.5448487719411382, + "timestamp": 9.112971969988774 + }, + { + "x": 6.350906942398405, + "y": 4.066922357709394, + "heading": 4.364184226449404e-8, + "angularVelocity": -6.620491141311732e-13, + "velocityX": 3.7623138378867345, + "velocityY": -0.38493854632702584, + "timestamp": 9.149905200248382 + }, + { + "x": 6.490340706014696, + "y": 4.058637143370339, + "heading": 4.364062068850958e-8, + "angularVelocity": -3.3075253340927824e-11, + "velocityX": 3.775292944489131, + "velocityY": -0.22432953415709472, + "timestamp": 9.18683843050799 + }, + { + "x": 6.625895301013595, + "y": 4.054865050461972, + "heading": 3.8021532173824235e-8, + "angularVelocity": -1.521418068678583e-7, + "velocityX": 3.6702610101003303, + "velocityY": -0.10213276450108619, + "timestamp": 9.223771660767598 + }, + { + "x": 6.755934035567496, + "y": 4.053327166371606, + "heading": 3.0189413996037156e-8, + "angularVelocity": -2.1206154817940447e-7, + "velocityX": 3.52091419136215, + "velocityY": -0.04163957713841639, + "timestamp": 9.260704891027206 + }, + { + "x": 6.880153762983055, + "y": 4.0530365385095966, + "heading": 2.1734826824788128e-8, + "angularVelocity": -2.2891544824669977e-7, + "velocityX": 3.3633594067565498, + "velocityY": -0.007869007394285612, + "timestamp": 9.297638121286814 + }, + { + "x": 6.998470106919647, + "y": 4.0534986388519805, + "heading": 1.3540571224920633e-8, + "angularVelocity": -2.2186673987685247e-7, + "velocityX": 3.2035200578159904, + "velocityY": 0.012511777040293628, + "timestamp": 9.334571351546423 + }, + { + "x": 7.110853220415867, + "y": 4.054422625435363, + "heading": 6.1666659449803414e-9, + "angularVelocity": -1.9965502711211782e-7, + "velocityX": 3.042872575896168, + "velocityY": 0.025017757095422304, + "timestamp": 9.37150458180603 }, { "x": 7.217291355133057, "y": 4.0556182861328125, - "heading": 3.646325715989205e-37, - "angularVelocity": 2.881570700048058e-17, - "velocityX": 2.697135958773559, - "velocityY": 0.18068777649551113, - "timestamp": 9.884405291872048 - }, - { - "x": 7.386231209731221, - "y": 4.063934540767167, - "heading": 6.43663529397786e-19, - "angularVelocity": 9.269678756083754e-18, - "velocityX": 2.432976407238177, - "velocityY": 0.11976600412078256, - "timestamp": 9.95384281460986 - }, - { - "x": 7.536591060669406, - "y": 4.069229370335429, - "heading": -1.8193245120818273e-19, - "angularVelocity": -1.1889767204445143e-17, - "velocityX": 2.1653976842740037, - "velocityY": 0.07625314613043933, - "timestamp": 10.023280337347673 - }, - { - "x": 7.668251510768197, - "y": 4.072361333543041, - "heading": -2.313295063693691e-18, - "angularVelocity": -3.069468103770411e-17, - "velocityX": 1.8960994705401897, - "velocityY": 0.04510476589779892, - "timestamp": 10.092717860085486 - }, - { - "x": 7.78114979643099, - "y": 4.073967912827302, - "heading": -5.036275587489351e-18, - "angularVelocity": -3.9214828185580025e-17, - "velocityX": 1.6258973709226552, - "velocityY": 0.023137047822506123, - "timestamp": 10.162155382823299 - }, - { - "x": 7.875252481646468, - "y": 4.074539583547341, - "heading": -6.689419008155875e-18, - "angularVelocity": -2.380763822621624e-17, - "velocityX": 1.3552137447471682, - "velocityY": 0.008232878960810142, - "timestamp": 10.231592905561111 - }, - { - "x": 7.950542203811188, - "y": 4.074464670768735, - "heading": -5.15013780810139e-18, - "angularVelocity": 2.2167858808364967e-17, - "velocityX": 1.0842800721592976, - "velocityY": -0.001078851543840728, - "timestamp": 10.301030428298924 - }, - { - "x": 8.007010780474163, - "y": 4.074057877238601, - "heading": -4.959999016913383e-18, - "angularVelocity": 2.738271523682432e-18, - "velocityX": 0.8132285605319336, - "velocityY": -0.005858410756812446, - "timestamp": 10.370467951036737 - }, - { - "x": 8.044655422701718, - "y": 4.073579204097417, - "heading": -1.6663567301752781e-18, - "angularVelocity": 4.7433176715915545e-17, - "velocityX": 0.5421368842563122, - "velocityY": -0.006893580333953516, - "timestamp": 10.43990547377455 + "heading": -3.6086824530888626e-33, + "angularVelocity": -1.6696795107584883e-7, + "velocityX": 2.8819069972765123, + "velocityY": 0.03237357493636809, + "timestamp": 9.408437812065639 + }, + { + "x": 7.405345242374078, + "y": 4.058539790558069, + "heading": -6.074177156590073e-9, + "angularVelocity": -8.274407475492158e-8, + "velocityX": 2.561720622548375, + "velocityY": 0.03979751891783301, + "timestamp": 9.481847022104299 + }, + { + "x": 7.569888594263639, + "y": 4.061600743201228, + "heading": -8.336905285122374e-9, + "angularVelocity": -3.0823491082343837e-8, + "velocityX": 2.241453787650163, + "velocityY": 0.04169712004183413, + "timestamp": 9.555256232142959 + }, + { + "x": 7.71092123884848, + "y": 4.064554734769034, + "heading": -8.259320686300401e-9, + "angularVelocity": 1.056878269798436e-9, + "velocityX": 1.9211846103583805, + "velocityY": 0.04024006750993344, + "timestamp": 9.62866544218162 + }, + { + "x": 7.828444510796415, + "y": 4.067236236264996, + "heading": -6.843181194470064e-9, + "angularVelocity": 1.929103299991646e-8, + "velocityX": 1.600933614270509, + "velocityY": 0.03652813447454146, + "timestamp": 9.70207465222028 + }, + { + "x": 7.922460087151161, + "y": 4.069526407614099, + "heading": -4.8146064610859125e-9, + "angularVelocity": 2.7633790562623428e-8, + "velocityX": 1.2807054633231172, + "velocityY": 0.031197329979384493, + "timestamp": 9.77548386225894 + }, + { + "x": 7.992969626740666, + "y": 4.071335791937442, + "heading": -2.7240893137969417e-9, + "angularVelocity": 2.8477586783070736e-8, + "velocityX": 0.9604999093761202, + "velocityY": 0.024647919823550288, + "timestamp": 9.8488930722976 + }, + { + "x": 8.039974659720452, + "y": 4.072594619838684, + "heading": -1.0033424996203534e-9, + "angularVelocity": 2.3440475863128373e-8, + "velocityX": 0.6403151996190951, + "velocityY": 0.01714809218869153, + "timestamp": 9.92230228233626 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 4.9523840537519984e-39, - "angularVelocity": 2.3997928849898842e-17, - "velocityX": 0.27105142948932054, - "velocityY": -0.004784851370482414, - "timestamp": 10.509342996512363 + "heading": 1.0901032811452175e-35, + "angularVelocity": 1.3667801358727972e-8, + "velocityX": 0.32014923968219366, + "velocityY": 0.00888629686322534, + "timestamp": 9.99571149237492 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": -7.535317609356433e-39, - "angularVelocity": -1.798408291474874e-37, - "velocityX": 0, - "velocityY": 1.827412307849029e-40, - "timestamp": 10.578780519250175 + "heading": 2.780011652171749e-35, + "angularVelocity": 2.3020443776927775e-34, + "velocityX": -1.517814230631444e-35, + "velocityY": 4.908282159537256e-34, + "timestamp": 10.06912070241358 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneSprint.1.traj b/src/main/deploy/choreo/CenterLaneSprint.1.traj index 054b36ac..b15ef766 100644 --- a/src/main/deploy/choreo/CenterLaneSprint.1.traj +++ b/src/main/deploy/choreo/CenterLaneSprint.1.traj @@ -3,434 +3,407 @@ { "x": 1.2955970764160156, "y": 5.586942195892334, - "heading": 4.584052603316453e-36, - "angularVelocity": 0, - "velocityX": -1.1087002227609576e-30, - "velocityY": 4.436024859486677e-30, + "heading": 0, + "angularVelocity": -1.1592130893720681e-31, + "velocityX": 0, + "velocityY": 0, "timestamp": 0 }, { - "x": 1.3181821407757048, - "y": 5.598273732258848, - "heading": -7.069763767214106e-20, - "angularVelocity": -8.787949104114132e-19, - "velocityX": 0.28073436408544955, - "velocityY": 0.14085200756269842, - "timestamp": 0.08044994574644204 - }, - { - "x": 1.3636517023728314, - "y": 5.620319213488495, - "heading": -2.0052541119733845e-19, - "angularVelocity": -1.6138052893300047e-18, - "velocityX": 0.5651907055441345, - "velocityY": 0.2740272976628797, - "timestamp": 0.16089989149288408 - }, - { - "x": 1.4323621310520749, - "y": 5.6522818969499085, - "heading": -2.5664724388774436e-18, - "angularVelocity": -2.9409059245655595e-17, - "velocityX": 0.8540767559525704, - "velocityY": 0.39729900580333494, - "timestamp": 0.24134983723932613 - }, - { - "x": 1.524738455066009, - "y": 5.693099757868794, - "heading": -4.957647208916547e-18, - "angularVelocity": -2.9722661390485903e-17, - "velocityX": 1.1482459454437492, - "velocityY": 0.507369651290102, - "timestamp": 0.32179978298576817 - }, - { - "x": 1.6412810261673692, - "y": 5.741298758765306, - "heading": -7.348054486157485e-18, - "angularVelocity": -2.9713141519150695e-17, - "velocityX": 1.4486345518419708, - "velocityY": 0.5991178794505583, - "timestamp": 0.4022497287322102 - }, - { - "x": 1.7825444409555524, - "y": 5.794729255821419, - "heading": -1.1630259555579544e-17, - "angularVelocity": -5.3228447997874313e-17, - "velocityX": 1.75591684342883, - "velocityY": 0.664145843251592, - "timestamp": 0.48269967447865225 - }, - { - "x": 1.9490035629897984, - "y": 5.8500741494423, - "heading": -1.6535392715689588e-17, - "angularVelocity": -6.097141104334343e-17, - "velocityX": 2.0691017314048668, - "velocityY": 0.6879419632751668, - "timestamp": 0.5631496202250943 - }, - { - "x": 2.140495465443084, - "y": 5.901976898384377, - "heading": -2.1721290723634077e-17, - "angularVelocity": -6.446138546893251e-17, - "velocityX": 2.380261424391423, - "velocityY": 0.6451557979517949, - "timestamp": 0.6435995659715363 - }, - { - "x": 2.3543799781199475, - "y": 5.942172413663534, - "heading": -2.6571028223887664e-17, - "angularVelocity": -6.02829067757922e-17, - "velocityX": 2.658603566399057, - "velocityY": 0.4996338394841148, - "timestamp": 0.7240495117179784 - }, - { - "x": 2.5827650177602375, - "y": 5.961674379906129, - "heading": -2.745308531544403e-17, - "angularVelocity": -1.0964182829837211e-17, - "velocityX": 2.8388464096705315, - "velocityY": 0.24241117953620092, - "timestamp": 0.8044994574644204 - }, - { - "x": 2.8152635583029264, - "y": 5.956245091375537, - "heading": -2.787857803251133e-17, - "angularVelocity": -5.289064001063628e-18, - "velocityX": 2.8899775927024023, - "velocityY": -0.06748654062169376, - "timestamp": 0.8849494032108625 - }, - { - "x": 3.0443318857824635, - "y": 5.925781383839179, - "heading": -2.6153565480502514e-17, - "angularVelocity": 2.144199289966682e-17, - "velocityX": 2.8473397384110632, - "velocityY": -0.3786666013850943, - "timestamp": 0.9653993489573045 - }, - { - "x": 3.265462860964513, - "y": 5.871328365222942, - "heading": -2.40553850185398e-17, - "angularVelocity": 2.608049948713598e-17, - "velocityX": 2.748677741502352, - "velocityY": -0.6768558774270661, - "timestamp": 1.0458492947037465 - }, - { - "x": 3.4759237911438947, - "y": 5.793970350971247, - "heading": -2.5315322230553355e-17, - "angularVelocity": -1.5661336184343358e-17, - "velocityX": 2.6160481305744674, - "velocityY": -0.9615670157972775, - "timestamp": 1.1262992404501886 - }, - { - "x": 3.6739512617428174, - "y": 5.694614659038136, - "heading": -2.7826462563542755e-17, - "angularVelocity": -3.1213956935036674e-17, - "velocityX": 2.461499119188376, - "velocityY": -1.2350001110858715, - "timestamp": 1.2067491861966306 - }, - { - "x": 3.858335998672662, - "y": 5.573990102564811, - "heading": -3.317033762126302e-17, - "angularVelocity": -6.642521120825162e-17, - "velocityX": 2.291918723041152, - "velocityY": -1.499373994035365, - "timestamp": 1.2871991319430727 + "x": 1.3207451593695585, + "y": 5.599086645701892, + "heading": 2.231825504271677e-18, + "angularVelocity": 2.789547782680952e-17, + "velocityX": 0.3143242018214309, + "velocityY": 0.15179266348424167, + "timestamp": 0.08000682991577572 + }, + { + "x": 1.3713809631464564, + "y": 5.62264555119355, + "heading": 4.51507105043022e-18, + "angularVelocity": 2.853816960464376e-17, + "velocityX": 0.6328935145938026, + "velocityY": 0.2944611793396252, + "timestamp": 0.16001365983155144 + }, + { + "x": 1.4479102774130088, + "y": 5.656665804231403, + "heading": 6.004653783100575e-18, + "angularVelocity": 1.8618332407574406e-17, + "velocityX": 0.9565347651837746, + "velocityY": 0.4252168605416788, + "timestamp": 0.24002048974732715 + }, + { + "x": 1.5508175668842004, + "y": 5.699857537973527, + "heading": 7.57580271600204e-18, + "angularVelocity": 1.963779951822175e-17, + "velocityX": 1.2862313077471652, + "velocityY": 0.5398505826146306, + "timestamp": 0.3200273196631029 + }, + { + "x": 1.6806682557862314, + "y": 5.750395481752386, + "heading": 9.251664351913254e-18, + "angularVelocity": 2.0946531612097715e-17, + "velocityX": 1.6229950497837156, + "velocityY": 0.6316703690584408, + "timestamp": 0.4000341495788786 + }, + { + "x": 1.8380612948775077, + "y": 5.805552130674172, + "heading": 1.2501573477685283e-17, + "angularVelocity": 4.062041901915404e-17, + "velocityX": 1.9672450371704346, + "velocityY": 0.6893992548031757, + "timestamp": 0.48004097949465435 + }, + { + "x": 2.0233797317677533, + "y": 5.861001231409725, + "heading": 1.6054120788359745e-17, + "angularVelocity": 4.440302445729553e-17, + "velocityX": 2.316282710935199, + "velocityY": 0.6930545904033714, + "timestamp": 0.5600478094104301 + }, + { + "x": 2.235784948485807, + "y": 5.909651835150332, + "heading": 2.213686509511287e-17, + "angularVelocity": 7.602767686101118e-17, + "velocityX": 2.6548385549342215, + "velocityY": 0.6080806325227727, + "timestamp": 0.6400546393262058 + }, + { + "x": 2.4702985640404234, + "y": 5.941239855854083, + "heading": 2.864477544319077e-17, + "angularVelocity": 8.134175236038742e-17, + "velocityX": 2.9311699488835434, + "velocityY": 0.39481655174808844, + "timestamp": 0.7200614692419814 + }, + { + "x": 2.715877888033922, + "y": 5.947186832463751, + "heading": 3.119004227784184e-17, + "angularVelocity": 3.1813081703857335e-17, + "velocityX": 3.0694794963261316, + "velocityY": 0.07433086169371976, + "timestamp": 0.8000682991577571 + }, + { + "x": 2.961603624456153, + "y": 5.925207266559582, + "heading": 3.402854212881078e-17, + "angularVelocity": 3.5478294907307905e-17, + "velocityX": 3.0713094954425415, + "velocityY": -0.2747211197853527, + "timestamp": 0.8800751290735328 + }, + { + "x": 3.2005883629277774, + "y": 5.876126550770939, + "heading": 3.9287160074963035e-17, + "angularVelocity": 6.572712324724996e-17, + "velocityX": 2.987054214261441, + "velocityY": -0.6134565741588879, + "timestamp": 0.9600819589893085 + }, + { + "x": 3.428817982052201, + "y": 5.801272963497273, + "heading": 4.077287726704947e-17, + "angularVelocity": 1.856989731461567e-17, + "velocityX": 2.8526266990269726, + "velocityY": -0.9355899659232499, + "timestamp": 1.0400887889050843 + }, + { + "x": 3.6438287356883494, + "y": 5.701819067812634, + "heading": 4.10496280483666e-17, + "angularVelocity": 3.459161663925654e-18, + "velocityX": 2.6874049860525546, + "velocityY": -1.243067570484904, + "timestamp": 1.12009561882086 + }, + { + "x": 3.8440012077207406, + "y": 5.578706367507132, + "heading": 4.4578411240497065e-17, + "angularVelocity": 4.41060783161696e-17, + "velocityX": 2.5019422996871006, + "velocityY": -1.538777382333539, + "timestamp": 1.2001024487366356 }, { "x": 4.028205871582031, "y": 5.432682037353516, - "heading": -4.136266271080991e-17, - "angularVelocity": -1.0183169434818875e-16, - "velocityX": 2.1114976688519658, - "velocityY": -1.7564718521622575, - "timestamp": 1.3676490776895147 - }, - { - "x": 4.107931819415156, - "y": 5.358451922494366, - "heading": -4.607311297857423e-17, - "angularVelocity": -1.1928519771528388e-16, - "velocityX": 2.018941679878885, - "velocityY": -1.8797678405182021, - "timestamp": 1.4071380577960628 - }, - { - "x": 4.184009699685142, - "y": 5.2793478225384876, - "heading": -5.07069292162639e-17, - "angularVelocity": -1.1734455595125913e-16, - "velocityX": 1.9265597659044507, - "velocityY": -2.0031943023709897, - "timestamp": 1.446627037902611 - }, - { - "x": 4.256449183576903, - "y": 5.195362514292594, - "heading": -5.599760363210485e-17, - "angularVelocity": -1.3397851916939406e-16, - "velocityX": 1.8344227603800494, - "velocityY": -2.126803680907542, - "timestamp": 1.486116018009159 - }, - { - "x": 4.325264882493694, - "y": 5.106485141313743, - "heading": -6.116421864840668e-17, - "angularVelocity": -1.3083689819279164e-16, - "velocityX": 1.7426557670265217, - "velocityY": -2.250687982800167, - "timestamp": 1.5256049981157072 - }, - { - "x": 4.390481430565823, - "y": 5.012697552943817, - "heading": -6.615883447942732e-17, - "angularVelocity": -1.2648127296403954e-16, - "velocityX": 1.6515125965785218, - "velocityY": -2.375031923257045, - "timestamp": 1.5650939782222553 - }, - { - "x": 4.452149171763237, - "y": 4.913963243889134, - "heading": -7.13027960351743e-17, - "angularVelocity": -1.3026323063398506e-16, - "velocityX": 1.5616443126888815, - "velocityY": -2.500300306269253, - "timestamp": 1.6045829583288034 - }, - { - "x": 4.510428130257496, - "y": 4.810171252999288, - "heading": -7.511831906490934e-17, - "angularVelocity": -9.662248553573966e-17, - "velocityX": 1.475828404199086, - "velocityY": -2.6283786162559313, - "timestamp": 1.6440719384353515 - }, - { - "x": 4.574696256850231, - "y": 4.707471171992527, - "heading": -7.369504337579853e-17, - "angularVelocity": 3.604235274007527e-17, - "velocityX": 1.6274952257440216, - "velocityY": -2.6007276138734374, - "timestamp": 1.6835609185418996 - }, - { - "x": 4.6429183139641825, - "y": 4.609400393977656, - "heading": -7.177735720537442e-17, - "angularVelocity": 4.8562568904755854e-17, - "velocityX": 1.7276226666283043, - "velocityY": -2.483497364335036, - "timestamp": 1.7230498986484477 - }, - { - "x": 4.714970653212224, - "y": 4.516061742260872, - "heading": -7.032849060398364e-17, - "angularVelocity": 3.669040576501961e-17, - "velocityX": 1.8246188950379656, - "velocityY": -2.3636632666873396, - "timestamp": 1.7625388787549958 - }, - { - "x": 4.790809657597791, - "y": 4.427490193568245, - "heading": -6.79306191406781e-17, - "angularVelocity": 6.072255395566305e-17, - "velocityX": 1.920510587538043, - "velocityY": -2.2429434351976525, - "timestamp": 1.802027858861544 - }, - { - "x": 4.870413047012439, - "y": 4.343703362363669, - "heading": -6.538034983995767e-17, - "angularVelocity": 6.458180589038589e-17, - "velocityX": 2.015838069253688, - "velocityY": -2.121777543469226, - "timestamp": 1.841516838968092 + "heading": 4.825304787734528e-17, + "angularVelocity": 4.592915652788645e-17, + "velocityX": 2.302361736529145, + "velocityY": -1.825148306820832, + "timestamp": 1.2801092786524113 + }, + { + "x": 4.11697825249087, + "y": 5.353217174122665, + "heading": 5.003311226824959e-17, + "angularVelocity": 4.404060645079727e-17, + "velocityX": 2.1963179706677165, + "velocityY": -1.9660406239470811, + "timestamp": 1.3205280076771686 + }, + { + "x": 4.201472100752748, + "y": 5.268051894541109, + "heading": 5.106612036315502e-17, + "angularVelocity": 2.555768903480938e-17, + "velocityX": 2.0904627706110244, + "velocityY": -2.1070746566406697, + "timestamp": 1.360946736701926 + }, + { + "x": 4.281698714286708, + "y": 5.177177737015351, + "heading": 5.2015291988095576e-17, + "angularVelocity": 2.3483498091432662e-17, + "velocityX": 1.9848870924323019, + "velocityY": -2.2483180376661425, + "timestamp": 1.4013654657266832 + }, + { + "x": 4.357676572309284, + "y": 5.080580936550585, + "heading": 5.3034148842228453e-17, + "angularVelocity": 2.5207570315775117e-17, + "velocityX": 1.8797686086529983, + "velocityY": -2.3899019785048825, + "timestamp": 1.4417841947514405 + }, + { + "x": 4.429441369664381, + "y": 4.978235165876808, + "heading": 5.4156772615538654e-17, + "angularVelocity": 2.7774872934374724e-17, + "velocityX": 1.7755332512056952, + "velocityY": -2.5321372824774984, + "timestamp": 1.4822029237761978 + }, + { + "x": 4.497091014290783, + "y": 4.870069946485337, + "heading": 5.562432201646814e-17, + "angularVelocity": 3.6308680200224936e-17, + "velocityX": 1.6737202346179283, + "velocityY": -2.6761162956262776, + "timestamp": 1.522621652800955 + }, + { + "x": 4.561997626649892, + "y": 4.755326331353034, + "heading": 5.3639840433255375e-17, + "angularVelocity": -4.9098048541470276e-17, + "velocityX": 1.6058548580143424, + "velocityY": -2.8388724212920464, + "timestamp": 1.5630403818257124 + }, + { + "x": 4.631511261933969, + "y": 4.646021074802578, + "heading": 5.11321781778172e-17, + "angularVelocity": -6.20420508556326e-17, + "velocityX": 1.7198372378713005, + "velocityY": -2.704321961318929, + "timestamp": 1.6034591108504697 + }, + { + "x": 4.705480207686577, + "y": 4.542279156484067, + "heading": 4.966571246690934e-17, + "angularVelocity": -3.628181222560111e-17, + "velocityX": 1.8300660989972783, + "velocityY": -2.566679379125897, + "timestamp": 1.643877839875227 + }, + { + "x": 4.783858959652748, + "y": 4.44413671743755, + "heading": 4.8920670981753807e-17, + "angularVelocity": -1.843305932153545e-17, + "velocityX": 1.939169139097368, + "velocityY": -2.4281426312595698, + "timestamp": 1.6842965688999842 + }, + { + "x": 4.86662561684807, + "y": 4.3516109378583465, + "heading": 4.74522093373318e-17, + "angularVelocity": -3.633120321770328e-17, + "velocityX": 2.0477303268199805, + "velocityY": -2.2891808280849295, + "timestamp": 1.7247152979247415 }, { "x": 4.9537672996521, "y": 4.264711856842041, - "heading": -6.195211405748592e-17, - "angularVelocity": 8.681501204664676e-17, - "velocityX": 2.110823131285873, - "velocityY": -2.000343014899152, - "timestamp": 1.8810058190746402 - }, - { - "x": 5.140611798119327, - "y": 4.122815195708918, - "heading": -5.763201257610311e-17, - "angularVelocity": 5.330138299303481e-17, - "velocityX": 2.305304898128395, - "velocityY": -1.7507342770944088, - "timestamp": 1.9620556180018687 - }, - { - "x": 5.342270775871548, - "y": 4.001853598441547, - "heading": -4.9592622237969717e-17, - "angularVelocity": 9.919017350845832e-17, - "velocityX": 2.488087329199462, - "velocityY": -1.492435501001034, - "timestamp": 2.043105416929097 - }, - { - "x": 5.557422233205499, - "y": 3.9027025181083355, - "heading": -4.2459537824589503e-17, - "angularVelocity": 8.80081167138009e-17, - "velocityX": 2.6545588042826695, - "velocityY": -1.2233353030661955, - "timestamp": 2.1241552158563257 - }, - { - "x": 5.784130404385857, - "y": 3.8264465892983552, - "heading": -3.761401308233599e-17, - "angularVelocity": 5.978405423676299e-17, - "velocityX": 2.7971466206678173, - "velocityY": -0.9408527821223643, - "timestamp": 2.2052050147835542 - }, - { - "x": 6.019396811829555, - "y": 3.7743671389893727, - "heading": -3.166083335314611e-17, - "angularVelocity": 7.345025936850683e-17, - "velocityX": 2.9027389401800168, - "velocityY": -0.6425611290798379, - "timestamp": 2.2862548137107828 - }, - { - "x": 6.258339960092501, - "y": 3.747669315728899, - "heading": -3.233315846876214e-17, - "angularVelocity": -8.295259833338512e-18, - "velocityX": 2.948102912326617, - "velocityY": -0.32940024053407974, - "timestamp": 2.3673046126380113 - }, - { - "x": 6.493116863691712, - "y": 3.7462773751063128, - "heading": -3.323952522012367e-17, - "angularVelocity": -1.118285892488459e-17, - "velocityX": 2.89669939600846, - "velocityY": -0.017173893603773912, - "timestamp": 2.44835441156524 - }, - { - "x": 6.713625196897221, - "y": 3.7661963030569305, - "heading": -3.285398522445789e-17, - "angularVelocity": 4.756788622971773e-18, - "velocityX": 2.72065243992446, - "velocityY": 0.24576159613832255, - "timestamp": 2.5294042104924683 - }, - { - "x": 6.912235405370678, - "y": 3.7994650405286214, - "heading": -3.0556799809969664e-17, - "angularVelocity": 2.834291294084942e-17, - "velocityX": 2.450471328784962, - "velocityY": 0.41047279466928444, - "timestamp": 2.610454009419697 - }, - { - "x": 7.0858734676149435, - "y": 3.8385764928806374, - "heading": -2.668740545037057e-17, - "angularVelocity": 4.7740713033428904e-17, - "velocityX": 2.1423626528855833, - "velocityY": 0.4825607573534232, - "timestamp": 2.6915038083469254 - }, - { - "x": 7.233873696789502, - "y": 3.8783565470591044, - "heading": -2.1696316515086995e-17, - "angularVelocity": 6.158022408908137e-17, - "velocityX": 1.8260406704834464, - "velocityY": 0.490810029223817, - "timestamp": 2.772553607274154 - }, - { - "x": 7.356370026927679, - "y": 3.9154353558580755, - "heading": -1.8133459969897106e-17, - "angularVelocity": 4.395875811080922e-17, - "velocityX": 1.5113711787120845, - "velocityY": 0.4574818110732585, - "timestamp": 2.8536034062013824 - }, - { - "x": 7.453704189310843, - "y": 3.9475532207908426, - "heading": -1.3708657227789814e-17, - "angularVelocity": 5.459339466075284e-17, - "velocityX": 1.2009179994537236, - "velocityY": 0.39627322163647194, - "timestamp": 2.934653205128611 - }, - { - "x": 7.526241371253178, - "y": 3.973124678040023, - "heading": -8.453544536238416e-18, - "angularVelocity": 6.483779098448022e-17, - "velocityX": 0.8949705354351214, - "velocityY": 0.3155030313273332, - "timestamp": 3.0157030040558395 - }, - { - "x": 7.574317422270549, - "y": 3.9909890736877442, - "heading": -3.967819924258361e-18, - "angularVelocity": 5.534514397033646e-17, - "velocityX": 0.593166814156423, - "velocityY": 0.22041258441994602, - "timestamp": 3.096752802983068 + "heading": 4.682424401401172e-17, + "angularVelocity": -1.5536488446652234e-17, + "velocityX": 2.1559728597813255, + "velocityY": -2.1499706475905587, + "timestamp": 1.7651340269494988 + }, + { + "x": 5.149371577077341, + "y": 4.111086251410353, + "heading": 4.245818870652888e-17, + "angularVelocity": -5.3034667663063016e-17, + "velocityX": 2.376013169452415, + "velocityY": -1.8660965214215293, + "timestamp": 1.847458603234601 + }, + { + "x": 5.361995748730517, + "y": 3.981639454553548, + "heading": 3.8311842606234285e-17, + "angularVelocity": -5.0365753698371845e-17, + "velocityX": 2.582754521741994, + "velocityY": -1.5723955433397911, + "timestamp": 1.9297831795197031 + }, + { + "x": 5.590089166249063, + "y": 3.8773917441987824, + "heading": 3.443521699897252e-17, + "angularVelocity": -4.708934879277126e-17, + "velocityX": 2.7706600848603538, + "velocityY": -1.2663012074886832, + "timestamp": 2.0121077558048053 + }, + { + "x": 5.831330212934442, + "y": 3.799628582145428, + "heading": 2.9264449434168703e-17, + "angularVelocity": -6.280941218977205e-17, + "velocityX": 2.9303648748534723, + "velocityY": -0.9445923145111385, + "timestamp": 2.0944323320899074 + }, + { + "x": 6.082008486894108, + "y": 3.749887377260942, + "heading": 2.126679829101086e-17, + "angularVelocity": -9.714783202978127e-17, + "velocityX": 3.044999261088646, + "velocityY": -0.6042084530518639, + "timestamp": 2.1767569083750096 + }, + { + "x": 6.335839306220575, + "y": 3.729546047320075, + "heading": 1.712845100944366e-17, + "angularVelocity": -5.02686298415513e-17, + "velocityX": 3.0832933587884717, + "velocityY": -0.2470869679456106, + "timestamp": 2.2590814846601117 + }, + { + "x": 6.582471637063415, + "y": 3.7378835035769398, + "heading": 1.3745368058462902e-17, + "angularVelocity": -4.109437406088766e-17, + "velocityX": 2.9958530243527477, + "velocityY": 0.10127542263348481, + "timestamp": 2.341406060945214 + }, + { + "x": 6.809467380291713, + "y": 3.768327415425299, + "heading": 1.1162328016061889e-17, + "angularVelocity": -3.137619156324536e-17, + "velocityX": 2.757326590330181, + "velocityY": 0.36980344415440275, + "timestamp": 2.423730637230316 + }, + { + "x": 7.009276028148806, + "y": 3.8103968563967796, + "heading": 8.26924402312721e-18, + "angularVelocity": -3.5142311707247735e-17, + "velocityX": 2.427083829324394, + "velocityY": 0.5110192225827506, + "timestamp": 2.506055213515418 + }, + { + "x": 7.179701211541178, + "y": 3.8557683077640763, + "heading": 5.91482335399215e-18, + "angularVelocity": -2.8599155256386034e-17, + "velocityX": 2.07016168297691, + "velocityY": 0.5511288781087693, + "timestamp": 2.5883797898005203 + }, + { + "x": 7.320624454413459, + "y": 3.899158119195988, + "heading": 3.995408556984211e-18, + "angularVelocity": -2.3315109304438273e-17, + "velocityX": 1.7118004031318457, + "velocityY": 0.5270578166492144, + "timestamp": 2.6707043660856224 + }, + { + "x": 7.432462903898932, + "y": 3.9372219303084806, + "heading": 2.438823399699254e-18, + "angularVelocity": -1.890781257081445e-17, + "velocityX": 1.35850622660075, + "velocityY": 0.4623626726238319, + "timestamp": 2.7530289423707246 + }, + { + "x": 7.515713681888788, + "y": 3.967733484062364, + "heading": 1.1936770855475268e-18, + "angularVelocity": -1.5124745408572555e-17, + "velocityX": 1.0112506100436387, + "velocityY": 0.37062509315884895, + "timestamp": 2.8353535186558267 + }, + { + "x": 7.570835923487974, + "y": 3.989130376969442, + "heading": 2.2208685009288135e-19, + "angularVelocity": -1.1801843204007997e-17, + "velocityX": 0.6695721264120479, + "velocityY": 0.25990893452889574, + "timestamp": 2.917678094940929 }, { "x": 7.598227500915527, "y": 4.000265598297119, - "heading": -3.691475085618823e-33, - "angularVelocity": 4.8955194718279566e-17, - "velocityX": 0.29500478670651564, - "velocityY": 0.114454628295526, - "timestamp": 3.1778026019102965 + "heading": -1.415925420811752e-32, + "angularVelocity": -2.6977014897899104e-18, + "velocityX": 0.3327266129215478, + "velocityY": 0.13525998955023022, + "timestamp": 3.000002671226031 }, { "x": 7.598227500915527, "y": 4.000265598297119, - "heading": -4.750113876264814e-34, - "angularVelocity": 3.9684907899163274e-32, - "velocityX": 8.530993042269065e-31, - "velocityY": -1.6956516929480016e-30, - "timestamp": 3.258852400837525 + "heading": -2.7014243414553336e-34, + "angularVelocity": 1.6871197561712535e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 3.082327247511133 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneSprint.traj b/src/main/deploy/choreo/CenterLaneSprint.traj index 054b36ac..b15ef766 100644 --- a/src/main/deploy/choreo/CenterLaneSprint.traj +++ b/src/main/deploy/choreo/CenterLaneSprint.traj @@ -3,434 +3,407 @@ { "x": 1.2955970764160156, "y": 5.586942195892334, - "heading": 4.584052603316453e-36, - "angularVelocity": 0, - "velocityX": -1.1087002227609576e-30, - "velocityY": 4.436024859486677e-30, + "heading": 0, + "angularVelocity": -1.1592130893720681e-31, + "velocityX": 0, + "velocityY": 0, "timestamp": 0 }, { - "x": 1.3181821407757048, - "y": 5.598273732258848, - "heading": -7.069763767214106e-20, - "angularVelocity": -8.787949104114132e-19, - "velocityX": 0.28073436408544955, - "velocityY": 0.14085200756269842, - "timestamp": 0.08044994574644204 - }, - { - "x": 1.3636517023728314, - "y": 5.620319213488495, - "heading": -2.0052541119733845e-19, - "angularVelocity": -1.6138052893300047e-18, - "velocityX": 0.5651907055441345, - "velocityY": 0.2740272976628797, - "timestamp": 0.16089989149288408 - }, - { - "x": 1.4323621310520749, - "y": 5.6522818969499085, - "heading": -2.5664724388774436e-18, - "angularVelocity": -2.9409059245655595e-17, - "velocityX": 0.8540767559525704, - "velocityY": 0.39729900580333494, - "timestamp": 0.24134983723932613 - }, - { - "x": 1.524738455066009, - "y": 5.693099757868794, - "heading": -4.957647208916547e-18, - "angularVelocity": -2.9722661390485903e-17, - "velocityX": 1.1482459454437492, - "velocityY": 0.507369651290102, - "timestamp": 0.32179978298576817 - }, - { - "x": 1.6412810261673692, - "y": 5.741298758765306, - "heading": -7.348054486157485e-18, - "angularVelocity": -2.9713141519150695e-17, - "velocityX": 1.4486345518419708, - "velocityY": 0.5991178794505583, - "timestamp": 0.4022497287322102 - }, - { - "x": 1.7825444409555524, - "y": 5.794729255821419, - "heading": -1.1630259555579544e-17, - "angularVelocity": -5.3228447997874313e-17, - "velocityX": 1.75591684342883, - "velocityY": 0.664145843251592, - "timestamp": 0.48269967447865225 - }, - { - "x": 1.9490035629897984, - "y": 5.8500741494423, - "heading": -1.6535392715689588e-17, - "angularVelocity": -6.097141104334343e-17, - "velocityX": 2.0691017314048668, - "velocityY": 0.6879419632751668, - "timestamp": 0.5631496202250943 - }, - { - "x": 2.140495465443084, - "y": 5.901976898384377, - "heading": -2.1721290723634077e-17, - "angularVelocity": -6.446138546893251e-17, - "velocityX": 2.380261424391423, - "velocityY": 0.6451557979517949, - "timestamp": 0.6435995659715363 - }, - { - "x": 2.3543799781199475, - "y": 5.942172413663534, - "heading": -2.6571028223887664e-17, - "angularVelocity": -6.02829067757922e-17, - "velocityX": 2.658603566399057, - "velocityY": 0.4996338394841148, - "timestamp": 0.7240495117179784 - }, - { - "x": 2.5827650177602375, - "y": 5.961674379906129, - "heading": -2.745308531544403e-17, - "angularVelocity": -1.0964182829837211e-17, - "velocityX": 2.8388464096705315, - "velocityY": 0.24241117953620092, - "timestamp": 0.8044994574644204 - }, - { - "x": 2.8152635583029264, - "y": 5.956245091375537, - "heading": -2.787857803251133e-17, - "angularVelocity": -5.289064001063628e-18, - "velocityX": 2.8899775927024023, - "velocityY": -0.06748654062169376, - "timestamp": 0.8849494032108625 - }, - { - "x": 3.0443318857824635, - "y": 5.925781383839179, - "heading": -2.6153565480502514e-17, - "angularVelocity": 2.144199289966682e-17, - "velocityX": 2.8473397384110632, - "velocityY": -0.3786666013850943, - "timestamp": 0.9653993489573045 - }, - { - "x": 3.265462860964513, - "y": 5.871328365222942, - "heading": -2.40553850185398e-17, - "angularVelocity": 2.608049948713598e-17, - "velocityX": 2.748677741502352, - "velocityY": -0.6768558774270661, - "timestamp": 1.0458492947037465 - }, - { - "x": 3.4759237911438947, - "y": 5.793970350971247, - "heading": -2.5315322230553355e-17, - "angularVelocity": -1.5661336184343358e-17, - "velocityX": 2.6160481305744674, - "velocityY": -0.9615670157972775, - "timestamp": 1.1262992404501886 - }, - { - "x": 3.6739512617428174, - "y": 5.694614659038136, - "heading": -2.7826462563542755e-17, - "angularVelocity": -3.1213956935036674e-17, - "velocityX": 2.461499119188376, - "velocityY": -1.2350001110858715, - "timestamp": 1.2067491861966306 - }, - { - "x": 3.858335998672662, - "y": 5.573990102564811, - "heading": -3.317033762126302e-17, - "angularVelocity": -6.642521120825162e-17, - "velocityX": 2.291918723041152, - "velocityY": -1.499373994035365, - "timestamp": 1.2871991319430727 + "x": 1.3207451593695585, + "y": 5.599086645701892, + "heading": 2.231825504271677e-18, + "angularVelocity": 2.789547782680952e-17, + "velocityX": 0.3143242018214309, + "velocityY": 0.15179266348424167, + "timestamp": 0.08000682991577572 + }, + { + "x": 1.3713809631464564, + "y": 5.62264555119355, + "heading": 4.51507105043022e-18, + "angularVelocity": 2.853816960464376e-17, + "velocityX": 0.6328935145938026, + "velocityY": 0.2944611793396252, + "timestamp": 0.16001365983155144 + }, + { + "x": 1.4479102774130088, + "y": 5.656665804231403, + "heading": 6.004653783100575e-18, + "angularVelocity": 1.8618332407574406e-17, + "velocityX": 0.9565347651837746, + "velocityY": 0.4252168605416788, + "timestamp": 0.24002048974732715 + }, + { + "x": 1.5508175668842004, + "y": 5.699857537973527, + "heading": 7.57580271600204e-18, + "angularVelocity": 1.963779951822175e-17, + "velocityX": 1.2862313077471652, + "velocityY": 0.5398505826146306, + "timestamp": 0.3200273196631029 + }, + { + "x": 1.6806682557862314, + "y": 5.750395481752386, + "heading": 9.251664351913254e-18, + "angularVelocity": 2.0946531612097715e-17, + "velocityX": 1.6229950497837156, + "velocityY": 0.6316703690584408, + "timestamp": 0.4000341495788786 + }, + { + "x": 1.8380612948775077, + "y": 5.805552130674172, + "heading": 1.2501573477685283e-17, + "angularVelocity": 4.062041901915404e-17, + "velocityX": 1.9672450371704346, + "velocityY": 0.6893992548031757, + "timestamp": 0.48004097949465435 + }, + { + "x": 2.0233797317677533, + "y": 5.861001231409725, + "heading": 1.6054120788359745e-17, + "angularVelocity": 4.440302445729553e-17, + "velocityX": 2.316282710935199, + "velocityY": 0.6930545904033714, + "timestamp": 0.5600478094104301 + }, + { + "x": 2.235784948485807, + "y": 5.909651835150332, + "heading": 2.213686509511287e-17, + "angularVelocity": 7.602767686101118e-17, + "velocityX": 2.6548385549342215, + "velocityY": 0.6080806325227727, + "timestamp": 0.6400546393262058 + }, + { + "x": 2.4702985640404234, + "y": 5.941239855854083, + "heading": 2.864477544319077e-17, + "angularVelocity": 8.134175236038742e-17, + "velocityX": 2.9311699488835434, + "velocityY": 0.39481655174808844, + "timestamp": 0.7200614692419814 + }, + { + "x": 2.715877888033922, + "y": 5.947186832463751, + "heading": 3.119004227784184e-17, + "angularVelocity": 3.1813081703857335e-17, + "velocityX": 3.0694794963261316, + "velocityY": 0.07433086169371976, + "timestamp": 0.8000682991577571 + }, + { + "x": 2.961603624456153, + "y": 5.925207266559582, + "heading": 3.402854212881078e-17, + "angularVelocity": 3.5478294907307905e-17, + "velocityX": 3.0713094954425415, + "velocityY": -0.2747211197853527, + "timestamp": 0.8800751290735328 + }, + { + "x": 3.2005883629277774, + "y": 5.876126550770939, + "heading": 3.9287160074963035e-17, + "angularVelocity": 6.572712324724996e-17, + "velocityX": 2.987054214261441, + "velocityY": -0.6134565741588879, + "timestamp": 0.9600819589893085 + }, + { + "x": 3.428817982052201, + "y": 5.801272963497273, + "heading": 4.077287726704947e-17, + "angularVelocity": 1.856989731461567e-17, + "velocityX": 2.8526266990269726, + "velocityY": -0.9355899659232499, + "timestamp": 1.0400887889050843 + }, + { + "x": 3.6438287356883494, + "y": 5.701819067812634, + "heading": 4.10496280483666e-17, + "angularVelocity": 3.459161663925654e-18, + "velocityX": 2.6874049860525546, + "velocityY": -1.243067570484904, + "timestamp": 1.12009561882086 + }, + { + "x": 3.8440012077207406, + "y": 5.578706367507132, + "heading": 4.4578411240497065e-17, + "angularVelocity": 4.41060783161696e-17, + "velocityX": 2.5019422996871006, + "velocityY": -1.538777382333539, + "timestamp": 1.2001024487366356 }, { "x": 4.028205871582031, "y": 5.432682037353516, - "heading": -4.136266271080991e-17, - "angularVelocity": -1.0183169434818875e-16, - "velocityX": 2.1114976688519658, - "velocityY": -1.7564718521622575, - "timestamp": 1.3676490776895147 - }, - { - "x": 4.107931819415156, - "y": 5.358451922494366, - "heading": -4.607311297857423e-17, - "angularVelocity": -1.1928519771528388e-16, - "velocityX": 2.018941679878885, - "velocityY": -1.8797678405182021, - "timestamp": 1.4071380577960628 - }, - { - "x": 4.184009699685142, - "y": 5.2793478225384876, - "heading": -5.07069292162639e-17, - "angularVelocity": -1.1734455595125913e-16, - "velocityX": 1.9265597659044507, - "velocityY": -2.0031943023709897, - "timestamp": 1.446627037902611 - }, - { - "x": 4.256449183576903, - "y": 5.195362514292594, - "heading": -5.599760363210485e-17, - "angularVelocity": -1.3397851916939406e-16, - "velocityX": 1.8344227603800494, - "velocityY": -2.126803680907542, - "timestamp": 1.486116018009159 - }, - { - "x": 4.325264882493694, - "y": 5.106485141313743, - "heading": -6.116421864840668e-17, - "angularVelocity": -1.3083689819279164e-16, - "velocityX": 1.7426557670265217, - "velocityY": -2.250687982800167, - "timestamp": 1.5256049981157072 - }, - { - "x": 4.390481430565823, - "y": 5.012697552943817, - "heading": -6.615883447942732e-17, - "angularVelocity": -1.2648127296403954e-16, - "velocityX": 1.6515125965785218, - "velocityY": -2.375031923257045, - "timestamp": 1.5650939782222553 - }, - { - "x": 4.452149171763237, - "y": 4.913963243889134, - "heading": -7.13027960351743e-17, - "angularVelocity": -1.3026323063398506e-16, - "velocityX": 1.5616443126888815, - "velocityY": -2.500300306269253, - "timestamp": 1.6045829583288034 - }, - { - "x": 4.510428130257496, - "y": 4.810171252999288, - "heading": -7.511831906490934e-17, - "angularVelocity": -9.662248553573966e-17, - "velocityX": 1.475828404199086, - "velocityY": -2.6283786162559313, - "timestamp": 1.6440719384353515 - }, - { - "x": 4.574696256850231, - "y": 4.707471171992527, - "heading": -7.369504337579853e-17, - "angularVelocity": 3.604235274007527e-17, - "velocityX": 1.6274952257440216, - "velocityY": -2.6007276138734374, - "timestamp": 1.6835609185418996 - }, - { - "x": 4.6429183139641825, - "y": 4.609400393977656, - "heading": -7.177735720537442e-17, - "angularVelocity": 4.8562568904755854e-17, - "velocityX": 1.7276226666283043, - "velocityY": -2.483497364335036, - "timestamp": 1.7230498986484477 - }, - { - "x": 4.714970653212224, - "y": 4.516061742260872, - "heading": -7.032849060398364e-17, - "angularVelocity": 3.669040576501961e-17, - "velocityX": 1.8246188950379656, - "velocityY": -2.3636632666873396, - "timestamp": 1.7625388787549958 - }, - { - "x": 4.790809657597791, - "y": 4.427490193568245, - "heading": -6.79306191406781e-17, - "angularVelocity": 6.072255395566305e-17, - "velocityX": 1.920510587538043, - "velocityY": -2.2429434351976525, - "timestamp": 1.802027858861544 - }, - { - "x": 4.870413047012439, - "y": 4.343703362363669, - "heading": -6.538034983995767e-17, - "angularVelocity": 6.458180589038589e-17, - "velocityX": 2.015838069253688, - "velocityY": -2.121777543469226, - "timestamp": 1.841516838968092 + "heading": 4.825304787734528e-17, + "angularVelocity": 4.592915652788645e-17, + "velocityX": 2.302361736529145, + "velocityY": -1.825148306820832, + "timestamp": 1.2801092786524113 + }, + { + "x": 4.11697825249087, + "y": 5.353217174122665, + "heading": 5.003311226824959e-17, + "angularVelocity": 4.404060645079727e-17, + "velocityX": 2.1963179706677165, + "velocityY": -1.9660406239470811, + "timestamp": 1.3205280076771686 + }, + { + "x": 4.201472100752748, + "y": 5.268051894541109, + "heading": 5.106612036315502e-17, + "angularVelocity": 2.555768903480938e-17, + "velocityX": 2.0904627706110244, + "velocityY": -2.1070746566406697, + "timestamp": 1.360946736701926 + }, + { + "x": 4.281698714286708, + "y": 5.177177737015351, + "heading": 5.2015291988095576e-17, + "angularVelocity": 2.3483498091432662e-17, + "velocityX": 1.9848870924323019, + "velocityY": -2.2483180376661425, + "timestamp": 1.4013654657266832 + }, + { + "x": 4.357676572309284, + "y": 5.080580936550585, + "heading": 5.3034148842228453e-17, + "angularVelocity": 2.5207570315775117e-17, + "velocityX": 1.8797686086529983, + "velocityY": -2.3899019785048825, + "timestamp": 1.4417841947514405 + }, + { + "x": 4.429441369664381, + "y": 4.978235165876808, + "heading": 5.4156772615538654e-17, + "angularVelocity": 2.7774872934374724e-17, + "velocityX": 1.7755332512056952, + "velocityY": -2.5321372824774984, + "timestamp": 1.4822029237761978 + }, + { + "x": 4.497091014290783, + "y": 4.870069946485337, + "heading": 5.562432201646814e-17, + "angularVelocity": 3.6308680200224936e-17, + "velocityX": 1.6737202346179283, + "velocityY": -2.6761162956262776, + "timestamp": 1.522621652800955 + }, + { + "x": 4.561997626649892, + "y": 4.755326331353034, + "heading": 5.3639840433255375e-17, + "angularVelocity": -4.9098048541470276e-17, + "velocityX": 1.6058548580143424, + "velocityY": -2.8388724212920464, + "timestamp": 1.5630403818257124 + }, + { + "x": 4.631511261933969, + "y": 4.646021074802578, + "heading": 5.11321781778172e-17, + "angularVelocity": -6.20420508556326e-17, + "velocityX": 1.7198372378713005, + "velocityY": -2.704321961318929, + "timestamp": 1.6034591108504697 + }, + { + "x": 4.705480207686577, + "y": 4.542279156484067, + "heading": 4.966571246690934e-17, + "angularVelocity": -3.628181222560111e-17, + "velocityX": 1.8300660989972783, + "velocityY": -2.566679379125897, + "timestamp": 1.643877839875227 + }, + { + "x": 4.783858959652748, + "y": 4.44413671743755, + "heading": 4.8920670981753807e-17, + "angularVelocity": -1.843305932153545e-17, + "velocityX": 1.939169139097368, + "velocityY": -2.4281426312595698, + "timestamp": 1.6842965688999842 + }, + { + "x": 4.86662561684807, + "y": 4.3516109378583465, + "heading": 4.74522093373318e-17, + "angularVelocity": -3.633120321770328e-17, + "velocityX": 2.0477303268199805, + "velocityY": -2.2891808280849295, + "timestamp": 1.7247152979247415 }, { "x": 4.9537672996521, "y": 4.264711856842041, - "heading": -6.195211405748592e-17, - "angularVelocity": 8.681501204664676e-17, - "velocityX": 2.110823131285873, - "velocityY": -2.000343014899152, - "timestamp": 1.8810058190746402 - }, - { - "x": 5.140611798119327, - "y": 4.122815195708918, - "heading": -5.763201257610311e-17, - "angularVelocity": 5.330138299303481e-17, - "velocityX": 2.305304898128395, - "velocityY": -1.7507342770944088, - "timestamp": 1.9620556180018687 - }, - { - "x": 5.342270775871548, - "y": 4.001853598441547, - "heading": -4.9592622237969717e-17, - "angularVelocity": 9.919017350845832e-17, - "velocityX": 2.488087329199462, - "velocityY": -1.492435501001034, - "timestamp": 2.043105416929097 - }, - { - "x": 5.557422233205499, - "y": 3.9027025181083355, - "heading": -4.2459537824589503e-17, - "angularVelocity": 8.80081167138009e-17, - "velocityX": 2.6545588042826695, - "velocityY": -1.2233353030661955, - "timestamp": 2.1241552158563257 - }, - { - "x": 5.784130404385857, - "y": 3.8264465892983552, - "heading": -3.761401308233599e-17, - "angularVelocity": 5.978405423676299e-17, - "velocityX": 2.7971466206678173, - "velocityY": -0.9408527821223643, - "timestamp": 2.2052050147835542 - }, - { - "x": 6.019396811829555, - "y": 3.7743671389893727, - "heading": -3.166083335314611e-17, - "angularVelocity": 7.345025936850683e-17, - "velocityX": 2.9027389401800168, - "velocityY": -0.6425611290798379, - "timestamp": 2.2862548137107828 - }, - { - "x": 6.258339960092501, - "y": 3.747669315728899, - "heading": -3.233315846876214e-17, - "angularVelocity": -8.295259833338512e-18, - "velocityX": 2.948102912326617, - "velocityY": -0.32940024053407974, - "timestamp": 2.3673046126380113 - }, - { - "x": 6.493116863691712, - "y": 3.7462773751063128, - "heading": -3.323952522012367e-17, - "angularVelocity": -1.118285892488459e-17, - "velocityX": 2.89669939600846, - "velocityY": -0.017173893603773912, - "timestamp": 2.44835441156524 - }, - { - "x": 6.713625196897221, - "y": 3.7661963030569305, - "heading": -3.285398522445789e-17, - "angularVelocity": 4.756788622971773e-18, - "velocityX": 2.72065243992446, - "velocityY": 0.24576159613832255, - "timestamp": 2.5294042104924683 - }, - { - "x": 6.912235405370678, - "y": 3.7994650405286214, - "heading": -3.0556799809969664e-17, - "angularVelocity": 2.834291294084942e-17, - "velocityX": 2.450471328784962, - "velocityY": 0.41047279466928444, - "timestamp": 2.610454009419697 - }, - { - "x": 7.0858734676149435, - "y": 3.8385764928806374, - "heading": -2.668740545037057e-17, - "angularVelocity": 4.7740713033428904e-17, - "velocityX": 2.1423626528855833, - "velocityY": 0.4825607573534232, - "timestamp": 2.6915038083469254 - }, - { - "x": 7.233873696789502, - "y": 3.8783565470591044, - "heading": -2.1696316515086995e-17, - "angularVelocity": 6.158022408908137e-17, - "velocityX": 1.8260406704834464, - "velocityY": 0.490810029223817, - "timestamp": 2.772553607274154 - }, - { - "x": 7.356370026927679, - "y": 3.9154353558580755, - "heading": -1.8133459969897106e-17, - "angularVelocity": 4.395875811080922e-17, - "velocityX": 1.5113711787120845, - "velocityY": 0.4574818110732585, - "timestamp": 2.8536034062013824 - }, - { - "x": 7.453704189310843, - "y": 3.9475532207908426, - "heading": -1.3708657227789814e-17, - "angularVelocity": 5.459339466075284e-17, - "velocityX": 1.2009179994537236, - "velocityY": 0.39627322163647194, - "timestamp": 2.934653205128611 - }, - { - "x": 7.526241371253178, - "y": 3.973124678040023, - "heading": -8.453544536238416e-18, - "angularVelocity": 6.483779098448022e-17, - "velocityX": 0.8949705354351214, - "velocityY": 0.3155030313273332, - "timestamp": 3.0157030040558395 - }, - { - "x": 7.574317422270549, - "y": 3.9909890736877442, - "heading": -3.967819924258361e-18, - "angularVelocity": 5.534514397033646e-17, - "velocityX": 0.593166814156423, - "velocityY": 0.22041258441994602, - "timestamp": 3.096752802983068 + "heading": 4.682424401401172e-17, + "angularVelocity": -1.5536488446652234e-17, + "velocityX": 2.1559728597813255, + "velocityY": -2.1499706475905587, + "timestamp": 1.7651340269494988 + }, + { + "x": 5.149371577077341, + "y": 4.111086251410353, + "heading": 4.245818870652888e-17, + "angularVelocity": -5.3034667663063016e-17, + "velocityX": 2.376013169452415, + "velocityY": -1.8660965214215293, + "timestamp": 1.847458603234601 + }, + { + "x": 5.361995748730517, + "y": 3.981639454553548, + "heading": 3.8311842606234285e-17, + "angularVelocity": -5.0365753698371845e-17, + "velocityX": 2.582754521741994, + "velocityY": -1.5723955433397911, + "timestamp": 1.9297831795197031 + }, + { + "x": 5.590089166249063, + "y": 3.8773917441987824, + "heading": 3.443521699897252e-17, + "angularVelocity": -4.708934879277126e-17, + "velocityX": 2.7706600848603538, + "velocityY": -1.2663012074886832, + "timestamp": 2.0121077558048053 + }, + { + "x": 5.831330212934442, + "y": 3.799628582145428, + "heading": 2.9264449434168703e-17, + "angularVelocity": -6.280941218977205e-17, + "velocityX": 2.9303648748534723, + "velocityY": -0.9445923145111385, + "timestamp": 2.0944323320899074 + }, + { + "x": 6.082008486894108, + "y": 3.749887377260942, + "heading": 2.126679829101086e-17, + "angularVelocity": -9.714783202978127e-17, + "velocityX": 3.044999261088646, + "velocityY": -0.6042084530518639, + "timestamp": 2.1767569083750096 + }, + { + "x": 6.335839306220575, + "y": 3.729546047320075, + "heading": 1.712845100944366e-17, + "angularVelocity": -5.02686298415513e-17, + "velocityX": 3.0832933587884717, + "velocityY": -0.2470869679456106, + "timestamp": 2.2590814846601117 + }, + { + "x": 6.582471637063415, + "y": 3.7378835035769398, + "heading": 1.3745368058462902e-17, + "angularVelocity": -4.109437406088766e-17, + "velocityX": 2.9958530243527477, + "velocityY": 0.10127542263348481, + "timestamp": 2.341406060945214 + }, + { + "x": 6.809467380291713, + "y": 3.768327415425299, + "heading": 1.1162328016061889e-17, + "angularVelocity": -3.137619156324536e-17, + "velocityX": 2.757326590330181, + "velocityY": 0.36980344415440275, + "timestamp": 2.423730637230316 + }, + { + "x": 7.009276028148806, + "y": 3.8103968563967796, + "heading": 8.26924402312721e-18, + "angularVelocity": -3.5142311707247735e-17, + "velocityX": 2.427083829324394, + "velocityY": 0.5110192225827506, + "timestamp": 2.506055213515418 + }, + { + "x": 7.179701211541178, + "y": 3.8557683077640763, + "heading": 5.91482335399215e-18, + "angularVelocity": -2.8599155256386034e-17, + "velocityX": 2.07016168297691, + "velocityY": 0.5511288781087693, + "timestamp": 2.5883797898005203 + }, + { + "x": 7.320624454413459, + "y": 3.899158119195988, + "heading": 3.995408556984211e-18, + "angularVelocity": -2.3315109304438273e-17, + "velocityX": 1.7118004031318457, + "velocityY": 0.5270578166492144, + "timestamp": 2.6707043660856224 + }, + { + "x": 7.432462903898932, + "y": 3.9372219303084806, + "heading": 2.438823399699254e-18, + "angularVelocity": -1.890781257081445e-17, + "velocityX": 1.35850622660075, + "velocityY": 0.4623626726238319, + "timestamp": 2.7530289423707246 + }, + { + "x": 7.515713681888788, + "y": 3.967733484062364, + "heading": 1.1936770855475268e-18, + "angularVelocity": -1.5124745408572555e-17, + "velocityX": 1.0112506100436387, + "velocityY": 0.37062509315884895, + "timestamp": 2.8353535186558267 + }, + { + "x": 7.570835923487974, + "y": 3.989130376969442, + "heading": 2.2208685009288135e-19, + "angularVelocity": -1.1801843204007997e-17, + "velocityX": 0.6695721264120479, + "velocityY": 0.25990893452889574, + "timestamp": 2.917678094940929 }, { "x": 7.598227500915527, "y": 4.000265598297119, - "heading": -3.691475085618823e-33, - "angularVelocity": 4.8955194718279566e-17, - "velocityX": 0.29500478670651564, - "velocityY": 0.114454628295526, - "timestamp": 3.1778026019102965 + "heading": -1.415925420811752e-32, + "angularVelocity": -2.6977014897899104e-18, + "velocityX": 0.3327266129215478, + "velocityY": 0.13525998955023022, + "timestamp": 3.000002671226031 }, { "x": 7.598227500915527, "y": 4.000265598297119, - "heading": -4.750113876264814e-34, - "angularVelocity": 3.9684907899163274e-32, - "velocityX": 8.530993042269065e-31, - "velocityY": -1.6956516929480016e-30, - "timestamp": 3.258852400837525 + "heading": -2.7014243414553336e-34, + "angularVelocity": 1.6871197561712535e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 3.082327247511133 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneSprintBonus.traj b/src/main/deploy/choreo/CenterLaneSprintBonus.traj deleted file mode 100644 index 9dbdd643..00000000 --- a/src/main/deploy/choreo/CenterLaneSprintBonus.traj +++ /dev/null @@ -1,877 +0,0 @@ -{ - "samples": [ - { - "x": 1.3954176902770996, - "y": 5.518980503082275, - "heading": -4.154669365501339e-31, - "angularVelocity": -1.673896254624468e-30, - "velocityX": -8.167537270046595e-18, - "velocityY": 1.3237751910388058e-17, - "timestamp": 0 - }, - { - "x": 1.4086689901208764, - "y": 5.531099909602429, - "heading": 5.4865363809901704e-18, - "angularVelocity": 8.089770653155719e-17, - "velocityX": 0.19538732001957015, - "velocityY": 0.1786978174305604, - "timestamp": 0.06782067455784145 - }, - { - "x": 1.435302968552215, - "y": 5.555193491205879, - "heading": 8.977091372122931e-18, - "angularVelocity": 5.146742470079397e-17, - "velocityX": 0.39271178891950187, - "velocityY": 0.35525423125810013, - "timestamp": 0.1356413491156829 - }, - { - "x": 1.4754682380563253, - "y": 5.591093050497596, - "heading": 1.4654590389523967e-17, - "angularVelocity": 8.37134055043427e-17, - "velocityX": 0.5922275141904324, - "velocityY": 0.5293306137953979, - "timestamp": 0.20346202367352434 - }, - { - "x": 1.5293341329115746, - "y": 5.638601649509276, - "heading": 2.0391035972700556e-17, - "angularVelocity": 8.458255853655089e-17, - "velocityX": 0.7942400338307883, - "velocityY": 0.7005031920047591, - "timestamp": 0.2712826982313658 - }, - { - "x": 1.597095145056665, - "y": 5.697485776780742, - "heading": 2.0873484642332193e-17, - "angularVelocity": 7.113597162801294e-18, - "velocityX": 0.9991202916641198, - "velocityY": 0.8682326982939436, - "timestamp": 0.33910337278920727 - }, - { - "x": 1.6789766146521206, - "y": 5.767464479151507, - "heading": 2.2177946405449375e-17, - "angularVelocity": 1.9233987436627017e-17, - "velocityX": 1.2073231375134688, - "velocityY": 1.031819615876666, - "timestamp": 0.40692404734704873 - }, - { - "x": 1.7752420754207336, - "y": 5.848193888395741, - "heading": 2.5659685154236302e-17, - "angularVelocity": 5.133742988390828e-17, - "velocityX": 1.4194117265305242, - "velocityY": 1.1903362768146415, - "timestamp": 0.4747447219048902 - }, - { - "x": 1.8862027378041668, - "y": 5.939244534569037, - "heading": 2.687644733119201e-17, - "angularVelocity": 1.7940873991424383e-17, - "velocityX": 1.6360890407953306, - "velocityY": 1.3425205037682877, - "timestamp": 0.5425653964627316 - }, - { - "x": 2.012229564399117, - "y": 6.040066954163349, - "heading": 2.5975996915224922e-17, - "angularVelocity": -1.3276929351019487e-17, - "velocityX": 1.8582361118727206, - "velocityY": 1.486603019678443, - "timestamp": 0.6103860710205731 - }, - { - "x": 2.1537679046461733, - "y": 6.149937565607546, - "heading": 2.3459157805274306e-17, - "angularVelocity": -3.711020537728529e-17, - "velocityX": 2.0869497563953514, - "velocityY": 1.6200164943876396, - "timestamp": 0.6782067455784145 - }, - { - "x": 2.311352553455493, - "y": 6.2678699868034595, - "heading": 2.1605195532052067e-17, - "angularVelocity": -2.7336238682566866e-17, - "velocityX": 2.3235488269129725, - "velocityY": 1.7388859955283122, - "timestamp": 0.746027420136256 - }, - { - "x": 2.485613520095621, - "y": 6.392464027187378, - "heading": 1.967921307517928e-17, - "angularVelocity": -2.839815790860674e-17, - "velocityX": 2.5694372368931395, - "velocityY": 1.8371100139627066, - "timestamp": 0.8138480946940975 - }, - { - "x": 2.6772369323815792, - "y": 6.5216430608077145, - "heading": 1.8045458271823762e-17, - "angularVelocity": -2.4089331717730728e-17, - "velocityX": 2.825442441191459, - "velocityY": 1.9047146679462823, - "timestamp": 0.8816687692519389 - }, - { - "x": 2.886763562184598, - "y": 6.652219019276088, - "heading": 1.683660040648187e-17, - "angularVelocity": -1.7824326418575357e-17, - "velocityX": 3.0894212003792894, - "velocityY": 1.9253119984379097, - "timestamp": 0.9494894438097804 - }, - { - "x": 3.113920279000272, - "y": 6.779380811677554, - "heading": 1.476992837613865e-17, - "angularVelocity": -3.0472596800531746e-17, - "velocityX": 3.3493727140967953, - "velocityY": 1.87497091750402, - "timestamp": 1.0173101183676219 - }, - { - "x": 3.356286094849282, - "y": 6.896994843265456, - "heading": 1.2661893348816599e-17, - "angularVelocity": -3.108248574533372e-17, - "velocityX": 3.5736273257250715, - "velocityY": 1.7341914151500553, - "timestamp": 1.0851307929254632 - }, - { - "x": 3.6089329789773132, - "y": 6.999885557033016, - "heading": 1.0426963344280672e-17, - "angularVelocity": -3.295352531869201e-17, - "velocityX": 3.725219275319372, - "velocityY": 1.5170995340042566, - "timestamp": 1.1529514674833046 - }, - { - "x": 3.866603136062622, - "y": 7.085535526275635, - "heading": 5.130524196392721e-18, - "angularVelocity": -7.809476623133778e-17, - "velocityX": 3.7992862613826874, - "velocityY": 1.262888784297902, - "timestamp": 1.220772142041146 - }, - { - "x": 4.03577288165141, - "y": 7.133931780153134, - "heading": 1.53240025945702e-18, - "angularVelocity": -8.102449650854585e-17, - "velocityX": 3.809456142072583, - "velocityY": 1.0898131101710078, - "timestamp": 1.2651799890617301 - }, - { - "x": 4.204491733807284, - "y": 7.174642072343406, - "heading": -1.8107357967918406e-18, - "angularVelocity": -7.528254097814744e-17, - "velocityX": 3.7993026790437296, - "velocityY": 0.9167364536138055, - "timestamp": 1.3095878360823143 - }, - { - "x": 4.372041951477231, - "y": 7.207742396297824, - "heading": -3.82115164020192e-18, - "angularVelocity": -4.5271625051994066e-17, - "velocityX": 3.7729867334545513, - "velocityY": 0.7453710588373391, - "timestamp": 1.3539956831028985 - }, - { - "x": 4.537853181541874, - "y": 7.2333425032674095, - "heading": -5.87160481310551e-18, - "angularVelocity": -4.617321158640688e-17, - "velocityX": 3.7338272667842607, - "velocityY": 0.5764771022950116, - "timestamp": 1.3984035301234827 - }, - { - "x": 4.701468659536594, - "y": 7.251563178434466, - "heading": -5.388451020675662e-18, - "angularVelocity": 1.0879921481471181e-17, - "velocityX": 3.684382130006832, - "velocityY": 0.410303052039688, - "timestamp": 1.442811377144067 - }, - { - "x": 4.8625183641051555, - "y": 7.262524779655412, - "heading": -5.872012470604977e-18, - "angularVelocity": -1.0889098042063212e-17, - "velocityX": 3.626604651513686, - "velocityY": 0.24683928531516408, - "timestamp": 1.4872192241646511 - }, - { - "x": 5.020698725587354, - "y": 7.266341856818374, - "heading": -7.150563317225696e-18, - "angularVelocity": -2.879110223357202e-17, - "velocityX": 3.561991226660372, - "velocityY": 0.08595501513940547, - "timestamp": 1.5316270711852353 - }, - { - "x": 5.175757540528759, - "y": 7.263120943781581, - "heading": -8.096150179100186e-18, - "angularVelocity": -2.1293241253900183e-17, - "velocityX": 3.4916985475456466, - "velocityY": -0.07253026779929446, - "timestamp": 1.5760349182058195 - }, - { - "x": 5.327482769552672, - "y": 7.252959960990395, - "heading": -8.592025684797133e-18, - "angularVelocity": -1.116639798884869e-17, - "velocityX": 3.41663105066957, - "velocityY": -0.22881052500644172, - "timestamp": 1.6204427652264037 - }, - { - "x": 5.475694166399093, - "y": 7.235948405191675, - "heading": -8.833226442519028e-18, - "angularVelocity": -5.431493574531005e-18, - "velocityX": 3.3375046706884697, - "velocityY": -0.38307544589665543, - "timestamp": 1.664850612246988 - }, - { - "x": 5.620236960766286, - "y": 7.212167896274621, - "heading": -9.572307077519199e-18, - "angularVelocity": -1.6643020598536764e-17, - "velocityX": 3.254893089056846, - "velocityY": -0.5355024058255178, - "timestamp": 1.7092584592675721 - }, - { - "x": 5.7609770366577315, - "y": 7.1816928593209, - "heading": -1.1397932886275862e-17, - "angularVelocity": -4.111043115967244e-17, - "velocityX": 3.169261410628764, - "velocityY": -0.6862534213738285, - "timestamp": 1.7536663062881563 - }, - { - "x": 5.897797208582785, - "y": 7.144591229805976, - "heading": -1.3242662306872499e-17, - "angularVelocity": -4.154061444960465e-17, - "velocityX": 3.080990885724137, - "velocityY": -0.8354746290160561, - "timestamp": 1.7980741533087405 - }, - { - "x": 6.030594311960715, - "y": 7.100925127890339, - "heading": -1.2969314838489408e-17, - "angularVelocity": 6.1553878370471014e-18, - "velocityX": 2.990397244801673, - "velocityY": -0.9832969811708759, - "timestamp": 1.8424820003293247 - }, - { - "x": 6.159276904080374, - "y": 7.050751478229476, - "heading": -1.2712775294894754e-17, - "angularVelocity": 5.77689752908346e-18, - "velocityX": 2.8977444472822667, - "velocityY": -1.1298374730395475, - "timestamp": 1.886889847349909 - }, - { - "x": 6.283763428080075, - "y": 6.994122567551955, - "heading": -1.1520483238842908e-17, - "angularVelocity": 2.6848678729359197e-17, - "velocityX": 2.8032551080892625, - "velocityY": -1.27520054397759, - "timestamp": 1.9312976943704931 - }, - { - "x": 6.403980731964111, - "y": 6.931086540222168, - "heading": -1.0278212817380173e-17, - "angularVelocity": 2.797411882250772e-17, - "velocityX": 2.707118492556311, - "velocityY": -1.4194794739895686, - "timestamp": 1.9757055413910773 - }, - { - "x": 6.60729389364524, - "y": 6.796210534013358, - "heading": -8.076224683373957e-18, - "angularVelocity": 2.7406592243829792e-17, - "velocityX": 2.5304956120267805, - "velocityY": -1.6787065778573356, - "timestamp": 2.0560507357138165 - }, - { - "x": 6.795704830022524, - "y": 6.641009825084134, - "heading": -5.924369506882037e-18, - "angularVelocity": 2.6782622499519642e-17, - "velocityX": 2.3450181179530984, - "velocityY": -1.9316738261382134, - "timestamp": 2.1363959300365556 - }, - { - "x": 6.968323808401379, - "y": 6.4661677340473895, - "heading": 1.813427886027723e-18, - "angularVelocity": 9.630691182046325e-17, - "velocityX": 2.148466748184869, - "velocityY": -2.176136264409528, - "timestamp": 2.2167411243592947 - }, - { - "x": 7.12401657856057, - "y": 6.272652848277024, - "heading": 5.514219590575789e-18, - "angularVelocity": 4.606114294192909e-17, - "velocityX": 1.9377981654234078, - "velocityY": -2.4085433783759767, - "timestamp": 2.297086318682034 - }, - { - "x": 7.261313462923536, - "y": 6.061911206598393, - "heading": 9.2916222876539e-18, - "angularVelocity": 4.7014669947754546e-17, - "velocityX": 1.7088375418131223, - "velocityY": -2.622952666367338, - "timestamp": 2.377431513004773 - }, - { - "x": 7.378307598032396, - "y": 5.836237450824618, - "heading": 1.3317359546362108e-17, - "angularVelocity": 5.010551054488734e-17, - "velocityX": 1.4561435328528385, - "velocityY": -2.8088021651582906, - "timestamp": 2.457776707327512 - }, - { - "x": 7.472656845707877, - "y": 5.59950120754036, - "heading": 1.7242988626530655e-17, - "angularVelocity": 4.8859538047431847e-17, - "velocityX": 1.1742985809019115, - "velocityY": -2.946489149473078, - "timestamp": 2.538121901650251 - }, - { - "x": 7.542192569709091, - "y": 5.358353787993265, - "heading": 1.7116315403461932e-17, - "angularVelocity": -1.5766120825832005e-18, - "velocityX": 0.8654621422893939, - "velocityY": -3.001391951065907, - "timestamp": 2.6184670959729903 - }, - { - "x": 7.5871412458956256, - "y": 5.1227422896934245, - "heading": 1.6858435017814408e-17, - "angularVelocity": -3.209652588562192e-18, - "velocityX": 0.5594444890627913, - "velocityY": -2.932490241462491, - "timestamp": 2.6988122902957294 - }, - { - "x": 7.612106015705454, - "y": 4.902486990914375, - "heading": 1.5879880365887054e-17, - "angularVelocity": -1.2179381518025204e-17, - "velocityX": 0.3107188926514608, - "velocityY": -2.741362450307933, - "timestamp": 2.7791574846184686 - }, - { - "x": 7.623350532741401, - "y": 4.703372273008907, - "heading": 1.7045873071267902e-17, - "angularVelocity": 1.45122904961207e-17, - "velocityX": 0.13995257750920653, - "velocityY": -2.4782405417509445, - "timestamp": 2.8595026789412077 - }, - { - "x": 7.625915530957838, - "y": 4.527918399643226, - "heading": 1.7155064370003262e-17, - "angularVelocity": 1.3590256508962654e-18, - "velocityX": 0.03192472478357754, - "velocityY": -2.1837506878246002, - "timestamp": 2.939847873263947 - }, - { - "x": 7.6233153599672425, - "y": 4.377132131065689, - "heading": 1.6703747348721444e-17, - "angularVelocity": -5.617224306630994e-18, - "velocityX": -0.032362495510961514, - "velocityY": -1.8767303987322825, - "timestamp": 3.020193067586686 - }, - { - "x": 7.6179855647139085, - "y": 4.251400181756933, - "heading": 1.5614701617946926e-17, - "angularVelocity": -1.355458489040667e-17, - "velocityX": -0.06633620465122089, - "velocityY": -1.5648969470868832, - "timestamp": 3.100538261909425 - }, - { - "x": 7.611666193625904, - "y": 4.150851372626415, - "heading": 1.448138270462699e-17, - "angularVelocity": -1.4105621493743314e-17, - "velocityX": -0.07865275753296536, - "velocityY": -1.251460152384776, - "timestamp": 3.180883456232164 - }, - { - "x": 7.605645305191298, - "y": 4.075503372077343, - "heading": 1.0097210956722963e-17, - "angularVelocity": -5.456669753373492e-17, - "velocityX": -0.07493775434062062, - "velocityY": -0.9378034515220308, - "timestamp": 3.2612286505549033 - }, - { - "x": 7.600907710154459, - "y": 4.0253252549341, - "heading": 5.681220340527481e-18, - "angularVelocity": -5.496272423373191e-17, - "velocityX": -0.058965505986702256, - "velocityY": -0.624531654521636, - "timestamp": 3.3415738448776424 - }, - { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -1.0462507114010796e-31, - "angularVelocity": -7.071014853118142e-17, - "velocityX": -0.03335867517061736, - "velocityY": -0.3118998821051884, - "timestamp": 3.4219190392003815 - }, - { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -7.6740532227992184e-31, - "angularVelocity": -3.211824120166074e-30, - "velocityX": 1.9649506717636078e-19, - "velocityY": 1.3301278158953467e-20, - "timestamp": 3.5022642335231207 - }, - { - "x": 7.601080260023373, - "y": 4.024833528540064, - "heading": -2.306467806437822e-19, - "angularVelocity": -2.8978205760100555e-18, - "velocityX": 0.035841753611820425, - "velocityY": 0.308668790189073, - "timestamp": 3.5818574124379605 - }, - { - "x": 7.6061588306375265, - "y": 4.074034103309106, - "heading": -6.659912056470629e-19, - "angularVelocity": -5.469619135125979e-18, - "velocityX": 0.06380660608606212, - "velocityY": 0.6181506435581036, - "timestamp": 3.6614505913528004 - }, - { - "x": 7.612681282533822, - "y": 4.147925498197174, - "heading": -1.328067367399e-18, - "angularVelocity": -8.318252856515268e-18, - "velocityX": 0.08194737269231386, - "velocityY": 0.9283634087178491, - "timestamp": 3.7410437702676402 - }, - { - "x": 7.619648669627935, - "y": 4.2465458921824055, - "heading": -2.2445697547086026e-18, - "angularVelocity": -1.151483703155595e-17, - "velocityX": 0.0875374898843103, - "velocityY": 1.2390558503857954, - "timestamp": 3.82063694918248 - }, - { - "x": 7.625747861911206, - "y": 4.369884044272492, - "heading": -3.6731106086224716e-18, - "angularVelocity": -1.7948034238446692e-17, - "velocityX": 0.07662958517843273, - "velocityY": 1.5496070614550481, - "timestamp": 3.90023012809732 - }, - { - "x": 7.629194369652072, - "y": 4.517812530748991, - "heading": -5.438708340419596e-18, - "angularVelocity": -2.2182783637629958e-17, - "velocityX": 0.04330154653780812, - "velocityY": 1.8585573348537896, - "timestamp": 3.97982330701216 - }, - { - "x": 7.627474126240345, - "y": 4.689928325847814, - "heading": -7.604279409389531e-18, - "angularVelocity": -2.7208006214780384e-17, - "velocityX": -0.021612950194643966, - "velocityY": 2.1624440366049984, - "timestamp": 4.059416485927 - }, - { - "x": 7.616940876905995, - "y": 4.885153687224004, - "heading": -1.141400124068985e-17, - "angularVelocity": -4.7864941206348986e-17, - "velocityX": -0.1323385933059981, - "velocityY": 2.452790101336185, - "timestamp": 4.139009664841839 - }, - { - "x": 7.592385072354184, - "y": 5.10075278062736, - "heading": -1.6698122492004076e-17, - "angularVelocity": -6.638915655253322e-17, - "velocityX": -0.3085164443310098, - "velocityY": 2.708763443586594, - "timestamp": 4.2186028437566785 - }, - { - "x": 7.547559201834021, - "y": 5.330523864977108, - "heading": -1.75098708974119e-17, - "angularVelocity": -1.0198737275031467e-17, - "velocityX": -0.5631873375496935, - "velocityY": 2.88681878877579, - "timestamp": 4.298196022671518 - }, - { - "x": 7.478361687755516, - "y": 5.564507297546682, - "heading": -1.8964828080994495e-17, - "angularVelocity": -1.827994481974364e-17, - "velocityX": -0.8693900032884319, - "velocityY": 2.93974227138131, - "timestamp": 4.377789201586357 - }, - { - "x": 7.384986158389392, - "y": 5.79328068583031, - "heading": -2.0967248801545294e-17, - "angularVelocity": -2.515821870147438e-17, - "velocityX": -1.173159944598191, - "velocityY": 2.8742838444534518, - "timestamp": 4.457382380501197 - }, - { - "x": 7.269611510863885, - "y": 6.010750981441168, - "heading": -2.341183721366507e-17, - "angularVelocity": -3.071356740399464e-17, - "velocityX": -1.4495544605518746, - "velocityY": 2.732273023590656, - "timestamp": 4.536975559416036 - }, - { - "x": 7.134451807687368, - "y": 6.213379687795212, - "heading": -1.8377494488650744e-17, - "angularVelocity": 6.325092910440118e-17, - "velocityX": -1.6981317371571643, - "velocityY": 2.545804918419267, - "timestamp": 4.6165687383308756 - }, - { - "x": 6.981289024355567, - "y": 6.399049314780469, - "heading": -1.3636102854748774e-17, - "angularVelocity": 5.957033241098293e-17, - "velocityX": -1.9243204684119384, - "velocityY": 2.332732898932091, - "timestamp": 4.696161917245715 - }, - { - "x": 6.8114962888064765, - "y": 6.566411402116164, - "heading": -1.0217857426728479e-17, - "angularVelocity": 4.294646070658522e-17, - "velocityX": -2.1332573703427147, - "velocityY": 2.1027189718699257, - "timestamp": 4.775755096160554 - }, - { - "x": 6.626136187340535, - "y": 6.714554267951228, - "heading": 1.5840075957918408e-18, - "angularVelocity": 1.4827738452159993e-16, - "velocityX": -2.328844054140172, - "velocityY": 1.8612507736817756, - "timestamp": 4.855348275075394 - }, - { - "x": 6.426044940948486, - "y": 6.842829704284668, - "heading": 1.3036775744196506e-17, - "angularVelocity": 1.4389137918114227e-16, - "velocityX": -2.513924548812549, - "velocityY": 1.6116385610211605, - "timestamp": 4.934941453990233 - }, - { - "x": 6.196280073025626, - "y": 6.956629849882612, - "heading": 2.0458443333295487e-17, - "angularVelocity": 8.729748510308141e-17, - "velocityX": -2.702613571061956, - "velocityY": 1.338576348343764, - "timestamp": 5.019957258833017 - }, - { - "x": 5.950838280027335, - "y": 7.046967618713607, - "heading": 2.793959259584985e-17, - "angularVelocity": 8.799713979563742e-17, - "velocityX": -2.887013696478903, - "velocityY": 1.0625997012916502, - "timestamp": 5.104973063675801 - }, - { - "x": 5.6901710071441105, - "y": 7.113547571189235, - "heading": 3.9506385392959274e-17, - "angularVelocity": 1.360545935414649e-16, - "velocityX": -3.0661036893700855, - "velocityY": 0.783147940536072, - "timestamp": 5.189988868518585 - }, - { - "x": 5.414850837059096, - "y": 7.156012349089107, - "heading": 4.700202282082566e-17, - "angularVelocity": 8.816755094375023e-17, - "velocityX": -3.238458667704845, - "velocityY": 0.4994927470064634, - "timestamp": 5.2750046733613685 - }, - { - "x": 5.12562598325124, - "y": 7.173922926375987, - "heading": 5.0881573025403224e-17, - "angularVelocity": 4.563326938276916e-17, - "velocityX": -3.402012770951351, - "velocityY": 0.21067350147440403, - "timestamp": 5.360020478204152 - }, - { - "x": 4.82351161445262, - "y": 7.166731524808041, - "heading": 5.499719430693057e-17, - "angularVelocity": 4.841005753815757e-17, - "velocityX": -3.5536259329345636, - "velocityY": -0.0845889959076519, - "timestamp": 5.445036283046936 - }, - { - "x": 4.5099531855637025, - "y": 7.1337470554057925, - "heading": 6.306907946999819e-17, - "angularVelocity": 9.494568681598964e-17, - "velocityX": -3.688236904523418, - "velocityY": -0.387980440380867, - "timestamp": 5.53005208788972 - }, - { - "x": 4.187141754472217, - "y": 7.074104928642276, - "heading": 6.708348504544881e-17, - "angularVelocity": 4.721950773543392e-17, - "velocityX": -3.797075516587084, - "velocityY": -0.7015416353912106, - "timestamp": 5.615067892732504 - }, - { - "x": 3.8586724966260957, - "y": 6.986817954786897, - "heading": 7.185623411631233e-17, - "angularVelocity": 5.613953431429229e-17, - "velocityX": -3.8636258099718415, - "velocityY": -1.0267146681348074, - "timestamp": 5.7000836975752875 - }, - { - "x": 3.5309501359202713, - "y": 6.871322995246275, - "heading": 7.621484395675117e-17, - "angularVelocity": 5.126820454622351e-17, - "velocityX": -3.8548404183416, - "velocityY": -1.3585116291517334, - "timestamp": 5.785099502418071 - }, - { - "x": 3.2151594653612365, - "y": 6.73025690445887, - "heading": 7.966221541391077e-17, - "angularVelocity": 4.054976338417916e-17, - "velocityX": -3.714493689061746, - "velocityY": -1.6592925403490721, - "timestamp": 5.870115307260855 - }, - { - "x": 2.9234333026279216, - "y": 6.574454892414151, - "heading": 7.863320324730858e-17, - "angularVelocity": -1.2103771141102572e-17, - "velocityX": -3.4314344641303904, - "velocityY": -1.83262409069529, - "timestamp": 5.955131112103639 - }, - { - "x": 2.659851278869089, - "y": 6.416614338182214, - "heading": 7.578110084515079e-17, - "angularVelocity": -3.354789840476959e-17, - "velocityX": -3.1003885012470134, - "velocityY": -1.856602481430881, - "timestamp": 6.040146916946423 - }, - { - "x": 2.4239206377137292, - "y": 6.264399576901125, - "heading": 7.251198491757422e-17, - "angularVelocity": -3.845302447267169e-17, - "velocityX": -2.775138594425478, - "velocityY": -1.7904289862642684, - "timestamp": 6.125162721789207 - }, - { - "x": 2.2144464260119667, - "y": 6.121998379809068, - "heading": 6.761005309686019e-17, - "angularVelocity": -5.765904670925165e-17, - "velocityX": -2.4639443464556736, - "velocityY": -1.6749967533143706, - "timestamp": 6.21017852663199 - }, - { - "x": 2.030377404257279, - "y": 5.9918782719805765, - "heading": 5.994279183003849e-17, - "angularVelocity": -9.018628912533282e-17, - "velocityX": -2.1651153229105353, - "velocityY": -1.530540210365924, - "timestamp": 6.295194331474774 - }, - { - "x": 1.8708849952976385, - "y": 5.875623192871665, - "heading": 4.994168872232487e-17, - "angularVelocity": -1.176381435863843e-16, - "velocityX": -1.8760324536664574, - "velocityY": -1.3674525498394738, - "timestamp": 6.380210136317558 - }, - { - "x": 1.735321954953519, - "y": 5.774322521108853, - "heading": 3.80276915684229e-17, - "angularVelocity": -1.4013859419532884e-16, - "velocityX": -1.5945628062312347, - "velocityY": -1.1915510527736544, - "timestamp": 6.465225941160342 - }, - { - "x": 1.6231760146266, - "y": 5.6887663231669565, - "heading": 2.9027956357036794e-17, - "angularVelocity": -1.0585954002347058e-16, - "velocityX": -1.3191187277977714, - "velocityY": -1.0063563839702132, - "timestamp": 6.550241746003126 - }, - { - "x": 1.5340345289934116, - "y": 5.619551610175038, - "heading": 2.339141109649791e-17, - "angularVelocity": -6.629995787223342e-17, - "velocityX": -1.048528397725959, - "velocityY": -0.8141393605567464, - "timestamp": 6.6352575508459095 - }, - { - "x": 1.467559291615285, - "y": 5.567144344510584, - "heading": 1.436014992750064e-17, - "angularVelocity": -1.0623036243813761e-16, - "velocityX": -0.7819162272362828, - "velocityY": -0.6164414459331455, - "timestamp": 6.720273355688693 - }, - { - "x": 1.4234687066815632, - "y": 5.531917778907625, - "heading": 5.2334973544914086e-18, - "angularVelocity": -1.0735240881555591e-16, - "velocityX": -0.5186163327543208, - "velocityY": -0.414353139020498, - "timestamp": 6.805289160531477 - }, - { - "x": 1.4015250205993652, - "y": 5.514177322387695, - "heading": 1.924648421034324e-30, - "angularVelocity": -6.15591051597521e-17, - "velocityX": -0.2581130193706609, - "velocityY": -0.20867245276024773, - "timestamp": 6.890304965374261 - }, - { - "x": 1.4015250205993652, - "y": 5.514177322387695, - "heading": 1.0779182942243083e-30, - "angularVelocity": 2.7193521636113197e-30, - "velocityX": -1.8832789283152474e-18, - "velocityY": 1.3879124381036463e-18, - "timestamp": 6.975320770217045 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneTaxi.1.traj b/src/main/deploy/choreo/CenterLaneTaxi.1.traj index ad7d85be..cf8ad366 100644 --- a/src/main/deploy/choreo/CenterLaneTaxi.1.traj +++ b/src/main/deploy/choreo/CenterLaneTaxi.1.traj @@ -6,134 +6,134 @@ "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": 4.665881846916086e-41, + "velocityY": -2.0318283346913183e-41, "timestamp": 0 }, { - "x": 1.48726957542182, + "x": 1.4872695756258638, "y": 5.489123821258545, - "heading": -7.108379598069269e-25, - "angularVelocity": -7.187577831060394e-24, - "velocityX": 0.3861138590221928, - "velocityY": -1.886047206290218e-34, - "timestamp": 0.09889862037628933 + "heading": 2.754950983259061e-24, + "angularVelocity": 2.9447241753048086e-23, + "velocityX": 0.40816653670481284, + "velocityY": 5.876249720718061e-34, + "timestamp": 0.09355526417669109 }, { - "x": 1.5636418305949276, + "x": 1.5636418311637772, "y": 5.489123821258545, - "heading": -4.643559244941043e-24, - "angularVelocity": -3.976545068254259e-23, - "velocityX": 0.7722277103818697, - "velocityY": -1.8726511377848079e-34, - "timestamp": 0.19779724075257865 + "heading": 4.052554305374892e-24, + "angularVelocity": 1.3869873668873725e-23, + "velocityX": 0.8163330648468328, + "velocityY": 2.351179297170128e-34, + "timestamp": 0.18711052835338218 }, { - "x": 1.6782002118810653, + "x": 1.6782002129129638, "y": 5.489123821258545, - "heading": -1.5517298527540587e-23, - "angularVelocity": -1.099488986748608e-22, - "velocityX": 1.1583415506734676, - "velocityY": -1.8670080146173677e-34, - "timestamp": 0.296695861128868 + "heading": -2.072394992605464e-24, + "angularVelocity": -6.546869835666046e-23, + "velocityX": 1.2244995806203742, + "velocityY": -1.9003523177361593e-34, + "timestamp": 0.2806657925300733 }, { - "x": 1.8309447175601195, + "x": 1.8309447190550667, "y": 5.489123821258545, - "heading": -3.870026243741996e-23, - "angularVelocity": -2.344121636116204e-22, - "velocityX": 1.5444553735723716, - "velocityY": -1.869661321607696e-34, - "timestamp": 0.3955944815051573 + "heading": -2.4578798145232008e-23, + "angularVelocity": -2.4056824153148397e-22, + "velocityX": 1.632666076957736, + "velocityY": 3.860759321784845e-34, + "timestamp": 0.37422105670676437 }, { - "x": 2.021875344535886, + "x": 2.0218753463170436, "y": 5.489123821258545, - "heading": -8.470852813064393e-23, - "angularVelocity": -4.652074531485588e-22, - "velocityX": 1.9305691651644288, - "velocityY": -1.8932805917377586e-34, - "timestamp": 0.49449310188144663 + "heading": -7.973182418935717e-23, + "angularVelocity": -5.895236107850817e-22, + "velocityX": 2.0408325383099775, + "velocityY": 1.2704796254278893e-34, + "timestamp": 0.46777632088345544 }, { - "x": 2.2509920855838907, + "x": 2.250992087061798, "y": 5.489123821258545, - "heading": -1.763106339588755e-22, - "angularVelocity": -9.262236889121556e-22, - "velocityX": 2.3166828837071916, - "velocityY": -1.9667253699171086e-34, - "timestamp": 0.593391722257736 + "heading": -2.0707507158653019e-22, + "angularVelocity": -1.3611552826942191e-21, + "velocityX": 2.4489989180302882, + "velocityY": -1.7750165055103215e-35, + "timestamp": 0.5613315850601466 }, { - "x": 2.5182949045818197, + "x": 2.5182949031039126, "y": 5.489123821258545, - "heading": -1.9581239719515217e-22, - "angularVelocity": -1.9718812851378736e-22, - "velocityX": 2.7027962370040624, - "velocityY": -1.941321300199066e-34, - "timestamp": 0.6922903426340252 + "heading": -2.160490651108523e-22, + "angularVelocity": -9.592193197806397e-23, + "velocityX": 2.8571648895916577, + "velocityY": 2.8715559113727715e-34, + "timestamp": 0.6548868492368377 }, { - "x": 2.7474116456298243, + "x": 2.747411643848667, "y": 5.489123821258545, - "heading": -1.6406643315943827e-22, - "angularVelocity": 3.2099681933784467e-22, - "velocityX": 2.3166828837071916, - "velocityY": -2.5239482095290997e-34, - "timestamp": 0.7911889630103146 + "heading": -1.6667699647956365e-22, + "angularVelocity": 5.277318832756414e-22, + "velocityX": 2.448998918030288, + "velocityY": 2.947986532686387e-35, + "timestamp": 0.7484421134135288 }, { - "x": 2.938342272605591, + "x": 2.938342271110644, "y": 5.489123821258545, - "heading": -1.2356782768830726e-22, - "angularVelocity": 4.0949693081452393e-22, - "velocityX": 1.930569165164429, - "velocityY": 4.651553097843434e-35, - "timestamp": 0.890087583386604 + "heading": -1.1298656729823459e-22, + "angularVelocity": 5.738899818074355e-22, + "velocityX": 2.0408325383099775, + "velocityY": -2.8596027354894936e-34, + "timestamp": 0.84199737759022 }, { - "x": 3.0910867782846454, + "x": 3.0910867772527473, "y": 5.489123821258545, - "heading": -8.304773508097308e-23, - "angularVelocity": 4.0971396195220896e-22, - "velocityX": 1.5444553735723718, - "velocityY": 3.44484956688297e-35, - "timestamp": 0.9889862037628934 + "heading": -6.615705102278205e-23, + "angularVelocity": 5.00554598927309e-22, + "velocityX": 1.632666076957736, + "velocityY": 3.1581487960600027e-34, + "timestamp": 0.9355526417669111 }, { - "x": 3.205645159570783, + "x": 3.2056451590019335, "y": 5.489123821258545, - "heading": -4.640632580637159e-23, - "angularVelocity": 3.704945408904745e-22, - "velocityX": 1.1583415506734678, - "velocityY": 2.4410393142900414e-35, - "timestamp": 1.0878848241391827 + "heading": -3.1148929262698523e-23, + "angularVelocity": 3.7419731471853073e-22, + "velocityX": 1.2244995806203742, + "velocityY": 5.822081879901906e-34, + "timestamp": 1.0291079059436021 }, { - "x": 3.2820174147438905, + "x": 3.2820174145398466, "y": 5.489123821258545, - "heading": -1.5379540807100434e-23, - "angularVelocity": 3.1372300226566687e-22, - "velocityX": 0.7722277103818695, - "velocityY": 1.5631443217200112e-35, - "timestamp": 1.1867834445154721 + "heading": -1.119480593141529e-23, + "angularVelocity": 2.1328705390671606e-22, + "velocityX": 0.8163330648468328, + "velocityY": 1.1078164340378654e-33, + "timestamp": 1.1226631701202932 }, { "x": 3.3202035427093506, "y": 5.489123821258545, - "heading": -2.9036533712144826e-40, - "angularVelocity": 1.555081061138629e-22, - "velocityX": 0.3861138590221928, - "velocityY": 7.652078130049953e-36, - "timestamp": 1.2856820648917615 + "heading": 3.079038293636101e-40, + "angularVelocity": 1.1965981728795564e-22, + "velocityX": 0.40816653670481284, + "velocityY": 4.557991704503881e-34, + "timestamp": 1.2162184342969842 }, { "x": 3.3202035427093506, "y": 5.489123821258545, - "heading": -5.67935929133569e-41, - "angularVelocity": 2.3616091621946912e-39, - "velocityX": 9.293663335236278e-40, - "velocityY": -6.396511271061404e-51, - "timestamp": 1.384580685268051 + "heading": 1.1605365063908143e-41, + "angularVelocity": -3.167017137026152e-39, + "velocityX": -4.508017624963005e-40, + "velocityY": -1.70250903211491e-50, + "timestamp": 1.3097736984736752 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneTaxi.traj b/src/main/deploy/choreo/CenterLaneTaxi.traj index ad7d85be..cf8ad366 100644 --- a/src/main/deploy/choreo/CenterLaneTaxi.traj +++ b/src/main/deploy/choreo/CenterLaneTaxi.traj @@ -6,134 +6,134 @@ "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": 4.665881846916086e-41, + "velocityY": -2.0318283346913183e-41, "timestamp": 0 }, { - "x": 1.48726957542182, + "x": 1.4872695756258638, "y": 5.489123821258545, - "heading": -7.108379598069269e-25, - "angularVelocity": -7.187577831060394e-24, - "velocityX": 0.3861138590221928, - "velocityY": -1.886047206290218e-34, - "timestamp": 0.09889862037628933 + "heading": 2.754950983259061e-24, + "angularVelocity": 2.9447241753048086e-23, + "velocityX": 0.40816653670481284, + "velocityY": 5.876249720718061e-34, + "timestamp": 0.09355526417669109 }, { - "x": 1.5636418305949276, + "x": 1.5636418311637772, "y": 5.489123821258545, - "heading": -4.643559244941043e-24, - "angularVelocity": -3.976545068254259e-23, - "velocityX": 0.7722277103818697, - "velocityY": -1.8726511377848079e-34, - "timestamp": 0.19779724075257865 + "heading": 4.052554305374892e-24, + "angularVelocity": 1.3869873668873725e-23, + "velocityX": 0.8163330648468328, + "velocityY": 2.351179297170128e-34, + "timestamp": 0.18711052835338218 }, { - "x": 1.6782002118810653, + "x": 1.6782002129129638, "y": 5.489123821258545, - "heading": -1.5517298527540587e-23, - "angularVelocity": -1.099488986748608e-22, - "velocityX": 1.1583415506734676, - "velocityY": -1.8670080146173677e-34, - "timestamp": 0.296695861128868 + "heading": -2.072394992605464e-24, + "angularVelocity": -6.546869835666046e-23, + "velocityX": 1.2244995806203742, + "velocityY": -1.9003523177361593e-34, + "timestamp": 0.2806657925300733 }, { - "x": 1.8309447175601195, + "x": 1.8309447190550667, "y": 5.489123821258545, - "heading": -3.870026243741996e-23, - "angularVelocity": -2.344121636116204e-22, - "velocityX": 1.5444553735723716, - "velocityY": -1.869661321607696e-34, - "timestamp": 0.3955944815051573 + "heading": -2.4578798145232008e-23, + "angularVelocity": -2.4056824153148397e-22, + "velocityX": 1.632666076957736, + "velocityY": 3.860759321784845e-34, + "timestamp": 0.37422105670676437 }, { - "x": 2.021875344535886, + "x": 2.0218753463170436, "y": 5.489123821258545, - "heading": -8.470852813064393e-23, - "angularVelocity": -4.652074531485588e-22, - "velocityX": 1.9305691651644288, - "velocityY": -1.8932805917377586e-34, - "timestamp": 0.49449310188144663 + "heading": -7.973182418935717e-23, + "angularVelocity": -5.895236107850817e-22, + "velocityX": 2.0408325383099775, + "velocityY": 1.2704796254278893e-34, + "timestamp": 0.46777632088345544 }, { - "x": 2.2509920855838907, + "x": 2.250992087061798, "y": 5.489123821258545, - "heading": -1.763106339588755e-22, - "angularVelocity": -9.262236889121556e-22, - "velocityX": 2.3166828837071916, - "velocityY": -1.9667253699171086e-34, - "timestamp": 0.593391722257736 + "heading": -2.0707507158653019e-22, + "angularVelocity": -1.3611552826942191e-21, + "velocityX": 2.4489989180302882, + "velocityY": -1.7750165055103215e-35, + "timestamp": 0.5613315850601466 }, { - "x": 2.5182949045818197, + "x": 2.5182949031039126, "y": 5.489123821258545, - "heading": -1.9581239719515217e-22, - "angularVelocity": -1.9718812851378736e-22, - "velocityX": 2.7027962370040624, - "velocityY": -1.941321300199066e-34, - "timestamp": 0.6922903426340252 + "heading": -2.160490651108523e-22, + "angularVelocity": -9.592193197806397e-23, + "velocityX": 2.8571648895916577, + "velocityY": 2.8715559113727715e-34, + "timestamp": 0.6548868492368377 }, { - "x": 2.7474116456298243, + "x": 2.747411643848667, "y": 5.489123821258545, - "heading": -1.6406643315943827e-22, - "angularVelocity": 3.2099681933784467e-22, - "velocityX": 2.3166828837071916, - "velocityY": -2.5239482095290997e-34, - "timestamp": 0.7911889630103146 + "heading": -1.6667699647956365e-22, + "angularVelocity": 5.277318832756414e-22, + "velocityX": 2.448998918030288, + "velocityY": 2.947986532686387e-35, + "timestamp": 0.7484421134135288 }, { - "x": 2.938342272605591, + "x": 2.938342271110644, "y": 5.489123821258545, - "heading": -1.2356782768830726e-22, - "angularVelocity": 4.0949693081452393e-22, - "velocityX": 1.930569165164429, - "velocityY": 4.651553097843434e-35, - "timestamp": 0.890087583386604 + "heading": -1.1298656729823459e-22, + "angularVelocity": 5.738899818074355e-22, + "velocityX": 2.0408325383099775, + "velocityY": -2.8596027354894936e-34, + "timestamp": 0.84199737759022 }, { - "x": 3.0910867782846454, + "x": 3.0910867772527473, "y": 5.489123821258545, - "heading": -8.304773508097308e-23, - "angularVelocity": 4.0971396195220896e-22, - "velocityX": 1.5444553735723718, - "velocityY": 3.44484956688297e-35, - "timestamp": 0.9889862037628934 + "heading": -6.615705102278205e-23, + "angularVelocity": 5.00554598927309e-22, + "velocityX": 1.632666076957736, + "velocityY": 3.1581487960600027e-34, + "timestamp": 0.9355526417669111 }, { - "x": 3.205645159570783, + "x": 3.2056451590019335, "y": 5.489123821258545, - "heading": -4.640632580637159e-23, - "angularVelocity": 3.704945408904745e-22, - "velocityX": 1.1583415506734678, - "velocityY": 2.4410393142900414e-35, - "timestamp": 1.0878848241391827 + "heading": -3.1148929262698523e-23, + "angularVelocity": 3.7419731471853073e-22, + "velocityX": 1.2244995806203742, + "velocityY": 5.822081879901906e-34, + "timestamp": 1.0291079059436021 }, { - "x": 3.2820174147438905, + "x": 3.2820174145398466, "y": 5.489123821258545, - "heading": -1.5379540807100434e-23, - "angularVelocity": 3.1372300226566687e-22, - "velocityX": 0.7722277103818695, - "velocityY": 1.5631443217200112e-35, - "timestamp": 1.1867834445154721 + "heading": -1.119480593141529e-23, + "angularVelocity": 2.1328705390671606e-22, + "velocityX": 0.8163330648468328, + "velocityY": 1.1078164340378654e-33, + "timestamp": 1.1226631701202932 }, { "x": 3.3202035427093506, "y": 5.489123821258545, - "heading": -2.9036533712144826e-40, - "angularVelocity": 1.555081061138629e-22, - "velocityX": 0.3861138590221928, - "velocityY": 7.652078130049953e-36, - "timestamp": 1.2856820648917615 + "heading": 3.079038293636101e-40, + "angularVelocity": 1.1965981728795564e-22, + "velocityX": 0.40816653670481284, + "velocityY": 4.557991704503881e-34, + "timestamp": 1.2162184342969842 }, { "x": 3.3202035427093506, "y": 5.489123821258545, - "heading": -5.67935929133569e-41, - "angularVelocity": 2.3616091621946912e-39, - "velocityX": 9.293663335236278e-40, - "velocityY": -6.396511271061404e-51, - "timestamp": 1.384580685268051 + "heading": 1.1605365063908143e-41, + "angularVelocity": -3.167017137026152e-39, + "velocityX": -4.508017624963005e-40, + "velocityY": -1.70250903211491e-50, + "timestamp": 1.3097736984736752 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SQUARFE (1).traj b/src/main/deploy/choreo/SQUARFE (1).traj deleted file mode 100644 index 2c5310f9..00000000 --- a/src/main/deploy/choreo/SQUARFE (1).traj +++ /dev/null @@ -1,805 +0,0 @@ -{ - "samples": [ - { - "x": 0, - "y": 2, - "heading": 6.834778036374476e-42, - "angularVelocity": 0, - "velocityX": 6.685663635549999e-41, - "velocityY": 1.8473219086701058e-41, - "timestamp": 0 - }, - { - "x": 0.008264463615126694, - "y": 2, - "heading": 1.7039918296957192e-26, - "angularVelocity": 1.739180661511921e-25, - "velocityX": 0.08434788783387884, - "velocityY": -2.7623927453648295e-18, - "timestamp": 0.09798068247308528 - }, - { - "x": 0.024793390752145244, - "y": 2, - "heading": 7.683593549129969e-26, - "angularVelocity": 6.102971583946312e-25, - "velocityX": 0.16869577471619418, - "velocityY": -5.524785459565961e-18, - "timestamp": 0.19596136494617056 - }, - { - "x": 0.04958678129588319, - "y": 2, - "heading": 5.293972470528129e-26, - "angularVelocity": -2.438680388678047e-25, - "velocityX": 0.2530436604230487, - "velocityY": -8.287178135270759e-18, - "timestamp": 0.29394204741925584 - }, - { - "x": 0.08264463510045543, - "y": 2, - "heading": -7.599657248983579e-26, - "angularVelocity": -1.3159126117680205e-24, - "velocityX": 0.33739154464098625, - "velocityY": -1.1049570762213534e-17, - "timestamp": 0.3919227298923411 - }, - { - "x": 0.12396695197508915, - "y": 2, - "heading": -6.356500212054052e-25, - "angularVelocity": -5.711849121394279e-24, - "velocityX": 0.42173942691187855, - "velocityY": -1.3811963325390594e-17, - "timestamp": 0.4899034123654264 - }, - { - "x": 0.1735537316596396, - "y": 2, - "heading": -1.7701677559872392e-24, - "angularVelocity": -1.1578935811088608e-23, - "velocityX": 0.5060873065277093, - "velocityY": -1.6574355801614406e-17, - "timestamp": 0.5878840948385117 - }, - { - "x": 0.23140497377834218, - "y": 2, - "heading": -2.69685519918364e-24, - "angularVelocity": -9.45777203938324e-24, - "velocityX": 0.5904351823084512, - "velocityY": -1.9336748152239084e-17, - "timestamp": 0.6858647773115969 - }, - { - "x": 0.29752067774070967, - "y": 2, - "heading": -3.3710373572859254e-24, - "angularVelocity": -6.88078025268394e-24, - "velocityX": 0.6747830520626255, - "velocityY": -2.2099140305493712e-17, - "timestamp": 0.7838454597846822 - }, - { - "x": 0.37190084248386523, - "y": 2, - "heading": -3.785377470906936e-24, - "angularVelocity": -4.228794191414106e-24, - "velocityX": 0.7591309109689794, - "velocityY": -2.4861532103482302e-17, - "timestamp": 0.8818261422577676 - }, - { - "x": 0.454545465527764, - "y": 2, - "heading": -3.6811470837306276e-24, - "angularVelocity": 1.0637753265934456e-24, - "velocityX": 0.8434787445637631, - "velocityY": -2.7623923072517135e-17, - "timestamp": 0.9798068247308529 - }, - { - "x": 0.5454545344722358, - "y": 2, - "heading": -3.223140769415779e-24, - "angularVelocity": 4.674291630935048e-24, - "velocityX": 0.9278264516012495, - "velocityY": -3.038630989680133e-17, - "timestamp": 1.0777875072039382 - }, - { - "x": 0.6280991575161347, - "y": 2, - "heading": -3.145189007341093e-24, - "angularVelocity": 7.953315368066196e-25, - "velocityX": 0.8434787445637631, - "velocityY": -2.7623923072517132e-17, - "timestamp": 1.1757681896770236 - }, - { - "x": 0.7024793222592902, - "y": 2, - "heading": -2.7590363290180994e-24, - "angularVelocity": 3.940934765433298e-24, - "velocityX": 0.7591309109689796, - "velocityY": -2.48615321034823e-17, - "timestamp": 1.273748872150109 - }, - { - "x": 0.7685950262216578, - "y": 2, - "heading": -2.36523859209739e-24, - "angularVelocity": 4.0191634044675645e-24, - "velocityX": 0.6747830520626255, - "velocityY": -2.209914030549371e-17, - "timestamp": 1.3717295546231942 - }, - { - "x": 0.8264462683403603, - "y": 2, - "heading": -1.912083934393757e-24, - "angularVelocity": 4.624982515888689e-24, - "velocityX": 0.5904351823084512, - "velocityY": -1.9336748152239084e-17, - "timestamp": 1.4697102370962796 - }, - { - "x": 0.8760330480249108, - "y": 2, - "heading": -1.4947650795710447e-24, - "angularVelocity": 4.259255952533442e-24, - "velocityX": 0.5060873065277093, - "velocityY": -1.6574355801614403e-17, - "timestamp": 1.567690919569365 - }, - { - "x": 0.9173553648995445, - "y": 2, - "heading": -1.0599672683839614e-24, - "angularVelocity": 4.4376648384860465e-24, - "velocityX": 0.42173942691187855, - "velocityY": -1.3811963325390594e-17, - "timestamp": 1.6656716020424502 - }, - { - "x": 0.9504132187041168, - "y": 2, - "heading": -6.474555283607888e-25, - "angularVelocity": 4.210197162464856e-24, - "velocityX": 0.33739154464098625, - "velocityY": -1.1049570762213536e-17, - "timestamp": 1.7636522845155356 - }, - { - "x": 0.9752066092478547, - "y": 2, - "heading": -3.2958736625896835e-25, - "angularVelocity": 3.2442420699516962e-24, - "velocityX": 0.2530436604230487, - "velocityY": -8.287178135270759e-18, - "timestamp": 1.861632966988621 - }, - { - "x": 0.9917355363848733, - "y": 2, - "heading": -1.1216781384407191e-25, - "angularVelocity": 2.219040288078826e-24, - "velocityX": 0.16869577471619418, - "velocityY": -5.524785459565961e-18, - "timestamp": 1.9596136494617062 - }, - { - "x": 1, - "y": 2, - "heading": 1.4025423606350947e-41, - "angularVelocity": 1.1448173504163706e-24, - "velocityX": 0.08434788783387884, - "velocityY": -2.7623927453648302e-18, - "timestamp": 2.0575943319347916 - }, - { - "x": 1, - "y": 2, - "heading": 4.353206627192277e-42, - "angularVelocity": -9.871558911369574e-41, - "velocityX": -6.752513577763152e-40, - "velocityY": -2.739881155941097e-39, - "timestamp": 2.155575014407877 - }, - { - "x": 1, - "y": 1.9917355363848732, - "heading": 2.678019676818953e-25, - "angularVelocity": 2.73325426399564e-24, - "velocityX": -7.221388058205676e-18, - "velocityY": -0.08434788783387888, - "timestamp": 2.2535556968809622 - }, - { - "x": 1, - "y": 1.9752066092478548, - "heading": 5.868915118029391e-25, - "angularVelocity": 3.2567428149199723e-24, - "velocityX": -1.4442776034943873e-17, - "velocityY": -0.16869577471619424, - "timestamp": 2.3515363793540476 - }, - { - "x": 1, - "y": 1.9504132187041168, - "heading": 7.292918744038093e-25, - "angularVelocity": 1.4534178819580077e-24, - "velocityX": -2.1664163911045784e-17, - "velocityY": -0.2530436604230488, - "timestamp": 2.449517061827133 - }, - { - "x": 1, - "y": 1.9173553648995445, - "heading": 6.385291298407452e-25, - "angularVelocity": -9.262537240715945e-25, - "velocityX": -2.888555165967506e-17, - "velocityY": -0.33739154464098636, - "timestamp": 2.5474977443002182 - }, - { - "x": 1, - "y": 1.8760330480249108, - "heading": 1.3873456572049932e-25, - "angularVelocity": -5.1008572041181364e-24, - "velocityX": -3.6106939241609364e-17, - "velocityY": -0.4217394269118787, - "timestamp": 2.6454784267733036 - }, - { - "x": 1, - "y": 1.8264462683403602, - "heading": -1.4606320867425958e-24, - "angularVelocity": -1.6323178027645798e-23, - "velocityX": -4.332832659623233e-17, - "velocityY": -0.5060873065277094, - "timestamp": 2.743459109246389 - }, - { - "x": 1, - "y": 1.7685950262216577, - "heading": -2.249822541344819e-24, - "angularVelocity": -8.054428449773172e-24, - "velocityX": -5.054971362251674e-17, - "velocityY": -0.5904351823084514, - "timestamp": 2.8414397917194743 - }, - { - "x": 1, - "y": 1.7024793222592902, - "heading": -1.6693794150187738e-24, - "angularVelocity": 5.924134836898355e-24, - "velocityX": -5.777110013284059e-17, - "velocityY": -0.6747830520626257, - "timestamp": 2.9394204741925596 - }, - { - "x": 1, - "y": 1.6280991575161345, - "heading": -4.17992965329782e-24, - "angularVelocity": -2.5622809982354645e-23, - "velocityX": -6.499248571443555e-17, - "velocityY": -0.7591309109689798, - "timestamp": 3.037401156665645 - }, - { - "x": 1, - "y": 1.545454534472236, - "heading": -1.2224446341565232e-23, - "angularVelocity": -8.210320576787122e-23, - "velocityX": -7.221386912899733e-17, - "velocityY": -0.8434787445637635, - "timestamp": 3.1353818391387303 - }, - { - "x": 1, - "y": 1.454545465527764, - "heading": -1.4447403666940808e-23, - "angularVelocity": -2.2688260164068862e-23, - "velocityX": -7.943524170844061e-17, - "velocityY": -0.9278264516012499, - "timestamp": 3.2333625216118156 - }, - { - "x": 1, - "y": 1.3719008424838655, - "heading": -1.4038421369615485e-23, - "angularVelocity": 4.174104041070465e-24, - "velocityX": -7.221386912899733e-17, - "velocityY": -0.8434787445637635, - "timestamp": 3.331343204084901 - }, - { - "x": 1, - "y": 1.2975206777407098, - "heading": -1.2608666390151249e-23, - "angularVelocity": 1.4592214374369727e-23, - "velocityX": -6.499248571443555e-17, - "velocityY": -0.7591309109689798, - "timestamp": 3.4293238865579863 - }, - { - "x": 1, - "y": 1.2314049737783423, - "heading": -9.223958699370151e-24, - "angularVelocity": 3.4544645258213173e-23, - "velocityX": -5.777110013284059e-17, - "velocityY": -0.6747830520626257, - "timestamp": 3.5273045690310716 - }, - { - "x": 1, - "y": 1.1735537316596398, - "heading": -6.510980866302344e-24, - "angularVelocity": 2.768890658272266e-23, - "velocityX": -5.054971362251675e-17, - "velocityY": -0.5904351823084515, - "timestamp": 3.625285251504157 - }, - { - "x": 1, - "y": 1.1239669519750892, - "heading": -4.570495528808751e-24, - "angularVelocity": 1.980477298357616e-23, - "velocityX": -4.332832659623233e-17, - "velocityY": -0.5060873065277095, - "timestamp": 3.7232659339772423 - }, - { - "x": 1, - "y": 1.0826446351004555, - "heading": -2.7074347601637494e-24, - "angularVelocity": 1.9014567030490677e-23, - "velocityX": -3.610693924160936e-17, - "velocityY": -0.42173942691187877, - "timestamp": 3.8212466164503276 - }, - { - "x": 1, - "y": 1.0495867812958832, - "heading": -1.1867771133422862e-24, - "angularVelocity": 1.551993357249157e-23, - "velocityX": -2.888555165967506e-17, - "velocityY": -0.33739154464098636, - "timestamp": 3.919227298923413 - }, - { - "x": 1, - "y": 1.0247933907521452, - "heading": -6.71373293397456e-25, - "angularVelocity": 5.260245478860946e-24, - "velocityX": -2.166416391104578e-17, - "velocityY": -0.2530436604230488, - "timestamp": 4.017207981396498 - }, - { - "x": 1, - "y": 1.0082644636151268, - "heading": -2.683389851754243e-25, - "angularVelocity": 4.1133864703848926e-24, - "velocityX": -1.444277603494387e-17, - "velocityY": -0.16869577471619424, - "timestamp": 4.115188663869583 - }, - { - "x": 1, - "y": 1, - "heading": 1.3844887180664726e-41, - "angularVelocity": 2.738667729952811e-24, - "velocityX": -7.221388058205674e-18, - "velocityY": -0.08434788783387888, - "timestamp": 4.213169346342668 - }, - { - "x": 1, - "y": 1, - "heading": -1.2548656701086555e-41, - "angularVelocity": -9.97631116732387e-41, - "velocityX": 4.048108670644253e-37, - "velocityY": -1.1182831125559368e-40, - "timestamp": 4.311150028815753 - }, - { - "x": 0.9917355363848733, - "y": 1, - "heading": -1.1243665350537136e-24, - "angularVelocity": -1.1475372005589736e-23, - "velocityX": -0.08434788783387885, - "velocityY": 1.1036137138901194e-18, - "timestamp": 4.409130711288838 - }, - { - "x": 0.9752066092478547, - "y": 1, - "heading": -3.378137956306648e-24, - "angularVelocity": -2.3002166920042362e-23, - "velocityX": -0.16869577471619418, - "velocityY": 2.207227415329915e-18, - "timestamp": 4.507111393761923 - }, - { - "x": 0.9504132187041168, - "y": 1, - "heading": -6.761085388842748e-24, - "angularVelocity": -3.4526627166894346e-23, - "velocityX": -0.2530436604230487, - "velocityY": 3.3108411013898984e-18, - "timestamp": 4.605092076235008 - }, - { - "x": 0.9173553648995445, - "y": 1, - "heading": -1.1133132425652307e-23, - "angularVelocity": -4.46214562056447e-23, - "velocityX": -0.33739154464098625, - "velocityY": 4.414454767968787e-18, - "timestamp": 4.7030727587080925 - }, - { - "x": 0.8760330480249108, - "y": 1, - "heading": -1.6100751630919242e-23, - "angularVelocity": -5.0699909707958593e-23, - "velocityX": -0.42173942691187855, - "velocityY": 5.518068409072398e-18, - "timestamp": 4.801053441181177 - }, - { - "x": 0.8264462683403604, - "y": 1, - "heading": -2.1677679346560742e-23, - "angularVelocity": -5.691865345905286e-23, - "velocityX": -0.5060873065277093, - "velocityY": 6.621682015436997e-18, - "timestamp": 4.899034123654262 - }, - { - "x": 0.7685950262216578, - "y": 1, - "heading": -2.70563073072413e-23, - "angularVelocity": -5.489487623439359e-23, - "velocityX": -0.5904351823084512, - "velocityY": 7.725295571623022e-18, - "timestamp": 4.997014806127347 - }, - { - "x": 0.7024793222592902, - "y": 1, - "heading": -3.2234788403103226e-23, - "angularVelocity": -5.285225361991524e-23, - "velocityX": -0.6747830520626255, - "velocityY": 8.828909048957011e-18, - "timestamp": 5.094995488600432 - }, - { - "x": 0.6280991575161347, - "y": 1, - "heading": -3.5244044696367074e-23, - "angularVelocity": -3.071298695388889e-23, - "velocityX": -0.7591309109689796, - "velocityY": 9.932522384357355e-18, - "timestamp": 5.192976171073517 - }, - { - "x": 0.5454545344722359, - "y": 1, - "heading": -3.185198887062514e-23, - "angularVelocity": 3.4619333983565953e-23, - "velocityX": -0.8434787445637631, - "velocityY": 1.1036135388579327e-17, - "timestamp": 5.290956853546602 - }, - { - "x": 0.4545454655277641, - "y": 1, - "heading": -1.1536119074148839e-23, - "angularVelocity": 2.07345235284118e-22, - "velocityX": -0.9278264516012495, - "velocityY": 1.2139746736916687e-17, - "timestamp": 5.388937536019687 - }, - { - "x": 0.3719008424838653, - "y": 1, - "heading": -1.7080978536028252e-24, - "angularVelocity": 1.0030590652108129e-22, - "velocityX": -0.8434787445637631, - "velocityY": 1.1036135388579327e-17, - "timestamp": 5.486918218492772 - }, - { - "x": 0.2975206777407097, - "y": 1, - "heading": 3.9485980786107975e-24, - "angularVelocity": 5.773286957268272e-23, - "velocityX": -0.7591309109689796, - "velocityY": 9.932522384357355e-18, - "timestamp": 5.5848989009658565 - }, - { - "x": 0.23140497377834224, - "y": 1, - "heading": 6.2504476669030164e-24, - "angularVelocity": 2.3493130652605514e-23, - "velocityX": -0.6747830520626255, - "velocityY": 8.828909048957011e-18, - "timestamp": 5.682879583438941 - }, - { - "x": 0.17355373165963964, - "y": 1, - "heading": 6.985240619947539e-24, - "angularVelocity": 7.499550312311534e-24, - "velocityX": -0.5904351823084512, - "velocityY": 7.725295571623022e-18, - "timestamp": 5.780860265912026 - }, - { - "x": 0.12396695197508918, - "y": 1, - "heading": 6.271694392407826e-24, - "angularVelocity": -7.282389940082428e-24, - "velocityX": -0.5060873065277093, - "velocityY": 6.621682015436996e-18, - "timestamp": 5.878840948385111 - }, - { - "x": 0.08264463510045544, - "y": 1, - "heading": 4.81506621376513e-24, - "angularVelocity": -1.486641000682138e-23, - "velocityX": -0.4217394269118786, - "velocityY": 5.5180684090723976e-18, - "timestamp": 5.976821630858196 - }, - { - "x": 0.04958678129588319, - "y": 1, - "heading": 3.1680135607204943e-24, - "angularVelocity": -1.6809957018793716e-23, - "velocityX": -0.33739154464098625, - "velocityY": 4.414454767968786e-18, - "timestamp": 6.074802313331281 - }, - { - "x": 0.02479339075214524, - "y": 1, - "heading": 1.7613266611845955e-24, - "angularVelocity": -1.4356757129038765e-23, - "velocityX": -0.2530436604230487, - "velocityY": 3.310841101389898e-18, - "timestamp": 6.172782995804366 - }, - { - "x": 0.008264463615126694, - "y": 1, - "heading": 6.558100798178395e-25, - "angularVelocity": -1.1282980364373314e-23, - "velocityX": -0.16869577471619418, - "velocityY": 2.207227415329915e-18, - "timestamp": 6.270763678277451 - }, - { - "x": -2.582873329443503e-41, - "y": 1, - "heading": 0, - "angularVelocity": -6.693229593886659e-24, - "velocityX": -0.08434788783387885, - "velocityY": 1.1036137138901196e-18, - "timestamp": 6.368744360750536 - }, - { - "x": 0, - "y": 1, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.5254761443447583e-40, - "velocityY": -1.330214233321333e-39, - "timestamp": 6.4667250432236205 - }, - { - "x": 1.117551892465105e-37, - "y": 1.0082644636151268, - "heading": 5.777226208013994e-25, - "angularVelocity": 5.8962777231441345e-24, - "velocityX": 1.1405739034082113e-36, - "velocityY": 0.08434788783387888, - "timestamp": 6.564705725696705 - }, - { - "x": 2.6897755866899145e-37, - "y": 1.0247933907521452, - "heading": 1.878956892361082e-24, - "angularVelocity": 1.3280490783242518e-23, - "velocityX": 1.604618665494541e-36, - "velocityY": 0.16869577471619424, - "timestamp": 6.66268640816979 - }, - { - "x": 4.617201760898304e-37, - "y": 1.0495867812958832, - "heading": 3.957176037517467e-24, - "angularVelocity": 2.121045567762179e-23, - "velocityX": 1.9672540809484087e-36, - "velocityY": 0.2530436604230488, - "timestamp": 6.760667090642875 - }, - { - "x": 7.822490165552593e-37, - "y": 1.0826446351004555, - "heading": 6.68318309457519e-24, - "angularVelocity": 2.7821884471994474e-23, - "velocityX": 3.2713410852659234e-36, - "velocityY": 0.33739154464098636, - "timestamp": 6.85864777311596 - }, - { - "x": 1.1548715422163086e-36, - "y": 1.1239669519750892, - "heading": 1.0156145042962959e-23, - "angularVelocity": 3.544538859114664e-23, - "velocityX": 3.802953813650495e-36, - "velocityY": 0.42173942691187877, - "timestamp": 6.956628455589045 - }, - { - "x": 1.590303807530914e-36, - "y": 1.1735537316596398, - "heading": 1.604994022058709e-23, - "angularVelocity": 6.015264831157614e-23, - "velocityX": 4.4441607272626095e-36, - "velocityY": 0.5060873065277095, - "timestamp": 7.05460913806213 - }, - { - "x": 2.0969602398345516e-36, - "y": 1.2314049737783423, - "heading": 2.689554282848633e-23, - "angularVelocity": 1.106911690454712e-22, - "velocityX": 5.170849268642284e-36, - "velocityY": 0.5904351823084514, - "timestamp": 7.152589820535215 - }, - { - "x": 2.6970018922938464e-36, - "y": 1.2975206777407098, - "heading": 4.114017114855317e-23, - "angularVelocity": 1.4538184971887797e-22, - "velocityX": 6.124058670873409e-36, - "velocityY": 0.6747830520626257, - "timestamp": 7.2505705030083 - }, - { - "x": 3.420493410463521e-36, - "y": 1.3719008424838655, - "heading": 5.947449866584252e-23, - "angularVelocity": 1.8712166065083759e-22, - "velocityX": 7.384122751685e-36, - "velocityY": 0.7591309109689798, - "timestamp": 7.3485511854813845 - }, - { - "x": 1.9831416217837412e-36, - "y": 1.454545465527764, - "heading": 8.166369448762052e-23, - "angularVelocity": 2.2646486737171235e-22, - "velocityX": -1.466983946444783e-35, - "velocityY": 0.8434787445637635, - "timestamp": 7.446531867954469 - }, - { - "x": 1.0861613719346016e-36, - "y": 1.545454534472236, - "heading": 9.281000307136208e-23, - "angularVelocity": 1.1376045389293476e-22, - "velocityX": -9.154736228879551e-36, - "velocityY": 0.9278264516012499, - "timestamp": 7.544512550427554 - }, - { - "x": 6.8567251290811176e-37, - "y": 1.6280991575161345, - "heading": 7.9681702937135e-23, - "angularVelocity": -1.3398862607068113e-22, - "velocityX": -4.087402783562853e-36, - "velocityY": 0.8434787445637635, - "timestamp": 7.642493232900639 - }, - { - "x": 4.416311613052699e-37, - "y": 1.7024793222592902, - "heading": 6.631726452847393e-23, - "angularVelocity": -1.3639872730018692e-22, - "velocityX": -2.490693450174919e-36, - "velocityY": 0.7591309109689798, - "timestamp": 7.740473915373724 - }, - { - "x": 2.8341582058058063e-37, - "y": 1.7685950262216577, - "heading": 5.520332634625129e-23, - "angularVelocity": -1.1342981734081153e-22, - "velocityX": -1.6148009261310582e-36, - "velocityY": 0.6747830520626257, - "timestamp": 7.838454597846809 - }, - { - "x": 1.7315008828738255e-37, - "y": 1.8264462683403602, - "heading": 4.288088447351943e-23, - "angularVelocity": -1.2576387583905343e-22, - "velocityX": -1.125317731608965e-36, - "velocityY": 0.5904351823084514, - "timestamp": 7.936435280319894 - }, - { - "x": 9.563231995227316e-38, - "y": 1.8760330480249108, - "heading": 3.053579758781607e-23, - "angularVelocity": -1.25995006403652e-22, - "velocityX": -7.911627994010943e-37, - "velocityY": 0.5060873065277095, - "timestamp": 8.034415962792979 - }, - { - "x": 4.825237858288704e-38, - "y": 1.9173553648995445, - "heading": 2.0171064793054303e-23, - "angularVelocity": -1.0578336973328714e-22, - "velocityX": -4.835712844569225e-37, - "velocityY": 0.4217394269118787, - "timestamp": 8.132396645266065 - }, - { - "x": 1.9830009986802493e-38, - "y": 1.9504132187041168, - "heading": 1.190680907263644e-23, - "angularVelocity": -8.434574430528315e-23, - "velocityX": -2.900681543335251e-37, - "velocityY": 0.33739154464098636, - "timestamp": 8.23037732773915 - }, - { - "x": 7.503247022033376e-39, - "y": 1.9752066092478548, - "heading": 5.867706563960283e-24, - "angularVelocity": -6.163563013249786e-23, - "velocityX": -1.2580315029942821e-37, - "velocityY": 0.2530436604230488, - "timestamp": 8.328358010212236 - }, - { - "x": 2.19006134392397e-39, - "y": 1.9917355363848732, - "heading": 1.9081673701003045e-24, - "angularVelocity": -4.041141844838844e-23, - "velocityX": -5.422599062203887e-38, - "velocityY": 0.16869577471619424, - "timestamp": 8.426338692685322 - }, - { - "x": 1.871946327660505e-43, - "y": 2, - "heading": 3.8918994248492545e-43, - "angularVelocity": -1.9474930664723247e-23, - "velocityX": -2.235046387745111e-38, - "velocityY": 0.08434788783387888, - "timestamp": 8.524319375158408 - }, - { - "x": 0, - "y": 2, - "heading": -2.5991331833497144e-43, - "angularVelocity": -6.625057575927064e-42, - "velocityX": -1.9101859645151015e-42, - "velocityY": 4.591774807899561e-41, - "timestamp": 8.622300057631493 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SQUARFE.traj b/src/main/deploy/choreo/SQUARFE.traj deleted file mode 100644 index c0295972..00000000 --- a/src/main/deploy/choreo/SQUARFE.traj +++ /dev/null @@ -1,805 +0,0 @@ -{ - "samples": [ - { - "x": 0, - "y": 2, - "heading": 1.066025235117479e-35, - "angularVelocity": 0, - "velocityX": -3.176387274976944e-34, - "velocityY": -5.951320349991925e-34, - "timestamp": 0 - }, - { - "x": 0.005019255586936127, - "y": 2.002260189779503, - "heading": -2.3828228381393305e-18, - "angularVelocity": -2.979840326637856e-17, - "velocityX": 0.0627683285032655, - "velocityY": 0.028264815788379395, - "timestamp": 0.07996478011478358 - }, - { - "x": 0.015089028014644117, - "y": 2.006709823197785, - "heading": -4.5974330969027984e-18, - "angularVelocity": -2.769481997390876e-17, - "velocityX": 0.12592759478927565, - "velocityY": 0.055644915322655746, - "timestamp": 0.15992956022956717 - }, - { - "x": 0.030245129831638854, - "y": 2.0132643642969383, - "heading": -6.642004718817398e-18, - "angularVelocity": -2.5568400908806152e-17, - "velocityX": 0.18953471509881312, - "velocityY": 0.08196784996775677, - "timestamp": 0.23989434034435075 - }, - { - "x": 0.050528852743670936, - "y": 2.0218211251492875, - "heading": -7.257064379564418e-18, - "angularVelocity": -7.691631736143833e-18, - "velocityX": 0.2536582090629937, - "velocityY": 0.10700662016536089, - "timestamp": 0.31985912045913434 - }, - { - "x": 0.07598806396033884, - "y": 2.032252906444552, - "heading": -7.528860856473167e-18, - "angularVelocity": -3.3989522858672543e-18, - "velocityX": 0.31838030668155515, - "velocityY": 0.13045469868472426, - "timestamp": 0.3998239005739179 - }, - { - "x": 0.1066783911012953, - "y": 2.044398398790724, - "heading": -7.622887589894659e-18, - "angularVelocity": -1.1758518205026792e-18, - "velocityX": 0.38379805580535253, - "velocityY": 0.151885521710158, - "timestamp": 0.4797886806887015 - }, - { - "x": 0.14266416316439484, - "y": 2.0580471012266317, - "heading": -7.535437739911399e-18, - "angularVelocity": 1.0936045537373189e-18, - "velocityX": 0.4500202715676151, - "velocityY": 0.17068392380140238, - "timestamp": 0.5597534608034851 - }, - { - "x": 0.18401799909562916, - "y": 2.0729145182854087, - "heading": -7.261962767216934e-18, - "angularVelocity": 3.4199427139841472e-18, - "velocityX": 0.5171506239606228, - "velocityY": 0.1859245662582475, - "timestamp": 0.6397182409182687 - }, - { - "x": 0.23081547811659442, - "y": 2.088599322388309, - "heading": -5.496441495113289e-18, - "angularVelocity": 2.2078735582384192e-17, - "velocityX": 0.5852261327273193, - "velocityY": 0.19614640445938436, - "timestamp": 0.7196830210330523 - }, - { - "x": 0.2831131435217446, - "y": 2.1045062001347263, - "heading": -3.615528657766951e-18, - "angularVelocity": 2.352176542156873e-17, - "velocityX": 0.6540087439755441, - "velocityY": 0.19892354763665143, - "timestamp": 0.7996478011478358 - }, - { - "x": 0.3408701135805606, - "y": 2.1197078453164817, - "heading": -1.7358822624727144e-18, - "angularVelocity": 2.350592756240704e-17, - "velocityX": 0.7222801085166501, - "velocityY": 0.19010425789872726, - "timestamp": 0.8796125812626194 - }, - { - "x": 0.403692097997754, - "y": 2.13275380476459, - "heading": -9.4361640863198e-19, - "angularVelocity": 9.907684705793237e-18, - "velocityX": 0.7856206735892604, - "velocityY": 0.16314631803378768, - "timestamp": 0.959577361377403 - }, - { - "x": 0.4702607204262372, - "y": 2.1417668933259137, - "heading": -6.362257579471842e-19, - "angularVelocity": 3.844075127130267e-18, - "velocityX": 0.8324742759616, - "velocityY": 0.11271322885382787, - "timestamp": 1.0395421414921866 - }, - { - "x": 0.5382796795158337, - "y": 2.145469815282364, - "heading": -2.2167383910431224e-19, - "angularVelocity": 5.184181008606903e-18, - "velocityX": 0.850611469098773, - "velocityY": 0.04630691100676233, - "timestamp": 1.1195069216069702 - }, - { - "x": 0.6057260502322735, - "y": 2.143697931570498, - "heading": 3.6199909161643833e-19, - "angularVelocity": 7.299124814607158e-18, - "velocityX": 0.8434509620313541, - "velocityY": -0.022158301558787858, - "timestamp": 1.1994717017217538 - }, - { - "x": 0.6713533698714674, - "y": 2.136730625262623, - "heading": 1.6921521064800653e-18, - "angularVelocity": 1.6634235500642562e-17, - "velocityX": 0.8207028087239234, - "velocityY": -0.08712968756837854, - "timestamp": 1.2794364818365374 - }, - { - "x": 0.734431350428031, - "y": 2.1248845651211443, - "heading": 2.0691864465641914e-18, - "angularVelocity": 4.7150049781463826e-18, - "velocityX": 0.7888220347260337, - "velocityY": -0.14814097061824272, - "timestamp": 1.359401261951321 - }, - { - "x": 0.7945085091672697, - "y": 2.10842369823011, - "heading": 1.8471511862263127e-18, - "angularVelocity": -2.7766630484638473e-18, - "velocityX": 0.7512952408923314, - "velocityY": -0.20585146194869802, - "timestamp": 1.4393660420661045 - }, - { - "x": 0.851286615700003, - "y": 2.0875562872881996, - "heading": 1.5362209056083268e-18, - "angularVelocity": -3.8883400349055174e-18, - "velocityX": 0.7100389252772585, - "velocityY": -0.260957522948938, - "timestamp": 1.519330822180888 - }, - { - "x": 0.9045570425926258, - "y": 2.0624465230223628, - "heading": 1.13056857408644e-18, - "angularVelocity": -5.0728873391071146e-18, - "velocityX": 0.6661736181373479, - "velocityY": -0.3140102959051894, - "timestamp": 1.5992956022956717 - }, - { - "x": 0.9541669202924247, - "y": 2.0332255838005793, - "heading": 6.201980366388928e-19, - "angularVelocity": -6.382441609832398e-18, - "velocityX": 0.6203965999604764, - "velocityY": -0.36542261705514717, - "timestamp": 1.6792603824104553 - }, - { - "x": 1, - "y": 2, - "heading": 0, - "angularVelocity": -7.755889945091827e-18, - "velocityX": 0.5731658317797576, - "velocityY": -0.41550272198442756, - "timestamp": 1.7592251625252389 - }, - { - "x": 1.0360682198258142, - "y": 1.9690252357457936, - "heading": -9.890855738376694e-19, - "angularVelocity": -1.4593472195699707e-17, - "velocityX": 0.5321688938798576, - "velocityY": -0.45701745500488516, - "timestamp": 1.8270010514218133 - }, - { - "x": 1.0692669632152427, - "y": 1.9353295083481672, - "heading": -2.0665277353015157e-18, - "angularVelocity": -1.5897129978073518e-17, - "velocityX": 0.4898311763950358, - "velocityY": -0.4971639316903078, - "timestamp": 1.8947769403183878 - }, - { - "x": 1.0995000427990536, - "y": 1.8990179878910807, - "heading": -2.6852884598983727e-18, - "angularVelocity": -9.129510523370085e-18, - "velocityX": 0.4460742614522743, - "velocityY": -0.5357586753675423, - "timestamp": 1.9625528292149623 - }, - { - "x": 1.1266661316570161, - "y": 1.860210249756036, - "heading": -3.1299706826939245e-18, - "angularVelocity": -6.561067790129639e-18, - "velocityX": 0.40082231749727, - "velocityY": -0.5725891429364366, - "timestamp": 2.030328718111537 - }, - { - "x": 1.1506592197210015, - "y": 1.8190425115321425, - "heading": -3.75029364893705e-18, - "angularVelocity": -9.152560722079088e-18, - "velocityX": 0.3540062469796368, - "velocityY": -0.6074097867859044, - "timestamp": 2.098104607008111 - }, - { - "x": 1.1713694688316625, - "y": 1.77567011857456, - "heading": -4.734849315417307e-18, - "angularVelocity": -1.452663543228679e-17, - "velocityX": 0.3055695682909384, - "velocityY": -0.6399383861090825, - "timestamp": 2.1658804959046853 - }, - { - "x": 1.1886846128926962, - "y": 1.7302702159613776, - "heading": -4.365253326644485e-18, - "angularVelocity": 5.453207120625985e-18, - "velocityX": 0.25547645841217265, - "velocityY": -0.6698532966858195, - "timestamp": 2.2336563848012596 - }, - { - "x": 1.2024920726171215, - "y": 1.6830444754865612, - "heading": -3.8567761493787476e-18, - "angularVelocity": 7.502330767109902e-18, - "velocityX": 0.20372229636848332, - "velocityY": -0.6967926388524772, - "timestamp": 2.301432273697834 - }, - { - "x": 1.2126819581312744, - "y": 1.6342216471211168, - "heading": -3.4963000828679734e-18, - "angularVelocity": 5.318647051824287e-18, - "velocityX": 0.1503467631343443, - "velocityY": -0.720356887387306, - "timestamp": 2.369208162594408 - }, - { - "x": 1.2191510957893172, - "y": 1.5840595772212502, - "heading": -3.2938741554653124e-18, - "angularVelocity": 2.986694853995746e-18, - "velocityX": 0.0954489533573625, - "velocityY": -0.7401167393970102, - "timestamp": 2.4369840514909824 - }, - { - "x": 1.2218081103679437, - "y": 1.5328461987368172, - "heading": -3.259573872067373e-18, - "angularVelocity": 5.060835243168827e-19, - "velocityX": 0.03920294697544978, - "velocityY": -0.7556282819482338, - "timestamp": 2.5047599403875567 - }, - { - "x": 1.220579396344816, - "y": 1.4808988958720275, - "heading": -3.3453058153274248e-18, - "angularVelocity": -1.2649331292104237e-18, - "velocityX": -0.018129072788743095, - "velocityY": -0.7664569762273552, - "timestamp": 2.572535829284131 - }, - { - "x": 1.2154155233457054, - "y": 1.4285616503210614, - "heading": -3.618440035746402e-18, - "angularVelocity": -4.029961768868618e-18, - "velocityX": -0.07619041348156665, - "velocityY": -0.7722103893145882, - "timestamp": 2.640311718180705 - }, - { - "x": 1.2062972964706975, - "y": 1.3761995720847502, - "heading": -4.087599432286243e-18, - "angularVelocity": -6.9222164854494395e-18, - "velocityX": -0.13453496550848654, - "velocityY": -0.7725767834076737, - "timestamp": 2.7080876070772795 - }, - { - "x": 1.1932404566060013, - "y": 1.3241908475189814, - "heading": -3.555359678860336e-18, - "angularVelocity": 7.852936361221584e-18, - "velocityX": -0.19264726847951472, - "velocityY": -0.7673632233010427, - "timestamp": 2.7758634959738537 - }, - { - "x": 1.1762980179515987, - "y": 1.2729167338053733, - "heading": -3.2328805268101095e-18, - "angularVelocity": 4.7580215665228305e-18, - "velocityX": -0.24997737293061145, - "velocityY": -0.7565244004671337, - "timestamp": 2.843639384870428 - }, - { - "x": 1.155559596070278, - "y": 1.2227507908644146, - "heading": -3.1247964605575482e-18, - "angularVelocity": 1.5947273141037222e-18, - "velocityX": -0.3059852437046957, - "velocityY": -0.7401738842187882, - "timestamp": 2.9114152737670023 - }, - { - "x": 1.131147710143389, - "y": 1.1740488128086175, - "heading": -3.234068776048195e-18, - "angularVelocity": -1.612259410180699e-18, - "velocityX": -0.36018540404746696, - "velocityY": -0.7185738003394676, - "timestamp": 2.9791911626635765 - }, - { - "x": 1.1032117091845473, - "y": 1.127140753581476, - "heading": -2.267931270616541e-18, - "angularVelocity": 1.4254885098319373e-17, - "velocityX": -0.4121819929425308, - "velocityY": -0.6921054078497251, - "timestamp": 3.046967051560151 - }, - { - "x": 1.0719204023306301, - "y": 1.0823254089032228, - "heading": -1.2961485271050903e-18, - "angularVelocity": 1.4338177791253964e-17, - "velocityX": -0.4616878858153752, - "velocityY": -0.6612284310522442, - "timestamp": 3.114742940456725 - }, - { - "x": 1.0374545343277675, - "y": 1.0398679644991287, - "heading": -5.410966813185065e-19, - "angularVelocity": 1.1140419716282834e-17, - "velocityX": -0.5085269786052852, - "velocityY": -0.6264387689386715, - "timestamp": 3.1825188293532993 - }, - { - "x": 1, - "y": 1, - "heading": 0, - "angularVelocity": 7.983616269770876e-18, - "velocityX": -0.5526232844385539, - "velocityY": -0.5882322629507188, - "timestamp": 3.2502947182498736 - }, - { - "x": 0.9601002931841937, - "y": 0.9632066842447741, - "heading": 3.2977160442109246e-19, - "angularVelocity": 4.906429958430405e-18, - "velocityX": -0.5936384674356385, - "velocityY": -0.547420753682207, - "timestamp": 3.3175068498830584 - }, - { - "x": 0.9176402744830485, - "y": 0.9293405687986124, - "heading": 1.3352918677177412e-18, - "angularVelocity": 1.4960397928012193e-17, - "velocityX": -0.6317314697423027, - "velocityY": -0.5038690876669165, - "timestamp": 3.3847189815162433 - }, - { - "x": 0.8728479847439143, - "y": 0.8985863996924396, - "heading": 2.1525838940809656e-18, - "angularVelocity": 1.2159888120347238e-17, - "velocityX": -0.6664316195711695, - "velocityY": -0.4575687209865014, - "timestamp": 3.451931113149428 - }, - { - "x": 0.8259849242635205, - "y": 0.8711239788151253, - "heading": 2.7942768738430748e-18, - "angularVelocity": 9.547278761162172e-18, - "velocityX": -0.6972410983206457, - "velocityY": -0.4085932139035895, - "timestamp": 3.519143244782613 - }, - { - "x": 0.7773460963851867, - "y": 0.8471213769660901, - "heading": 3.275531455850251e-18, - "angularVelocity": 7.160233562268939e-18, - "velocityX": -0.7236614387382273, - "velocityY": -0.35711710469221697, - "timestamp": 3.586355376415798 - }, - { - "x": 0.7272575825238243, - "y": 0.8267273909004392, - "heading": 3.656107749997039e-18, - "angularVelocity": 5.662315468533709e-18, - "velocityX": -0.7452302529954558, - "velocityY": -0.3034271577183785, - "timestamp": 3.6535675080489827 - }, - { - "x": 0.6760712290601476, - "y": 0.8100641481084726, - "heading": 3.9141657523662e-18, - "angularVelocity": 3.839455683091591e-18, - "velocityX": -0.7615642030672349, - "velocityY": -0.2479201654086435, - "timestamp": 3.7207796396821675 - }, - { - "x": 0.6241565565906713, - "y": 0.7972210163794645, - "heading": 3.9620361753801646e-18, - "angularVelocity": 7.122289175726647e-19, - "velocityX": -0.7724003272624098, - "velocityY": -0.19108353532216987, - "timestamp": 3.7879917713153524 - }, - { - "x": 0.5718906668220771, - "y": 0.7882509123495861, - "heading": 3.933061205617742e-18, - "angularVelocity": -4.310973365379806e-19, - "velocityX": -0.777625831804279, - "velocityY": -0.1334595974255566, - "timestamp": 3.855203902948537 - }, - { - "x": 0.5196474628149044, - "y": 0.7831696619611915, - "heading": 3.841828847804768e-18, - "angularVelocity": -1.3573793776640569e-18, - "velocityX": -0.7772883070022795, - "velocityY": -0.07560019694250741, - "timestamp": 3.922416034581722 - }, - { - "x": 0.46778766247295284, - "y": 0.7819583856222608, - "heading": 3.2741304171392934e-18, - "angularVelocity": -8.446368348915311e-18, - "velocityX": -0.7715839251309572, - "velocityY": -0.018021692059127074, - "timestamp": 3.989628166214907 - }, - { - "x": 0.41665080107502755, - "y": 0.784568242242575, - "heading": 2.3398005355380153e-18, - "angularVelocity": -1.390120864955836e-17, - "velocityX": -0.760827846928116, - "velocityY": 0.03883014207252925, - "timestamp": 4.056840297848092 - }, - { - "x": 0.3665498423457967, - "y": 0.7909265123693979, - "heading": 1.4204312415958456e-18, - "angularVelocity": -1.3678621115538104e-17, - "velocityX": -0.7454154110549615, - "velocityY": 0.09460003681364573, - "timestamp": 4.124052429481277 - }, - { - "x": 0.3177684131174127, - "y": 0.8009429979668188, - "heading": 2.911000402406795e-19, - "angularVelocity": -1.6802490157769785e-17, - "velocityX": -0.7257830996138033, - "velocityY": 0.14902794114738804, - "timestamp": 4.191264561114462 - }, - { - "x": 0.2705602477366799, - "y": 0.8145159621306034, - "heading": -8.90875593988358e-21, - "angularVelocity": -4.4636106745506625e-18, - "velocityX": -0.7023756609651165, - "velocityY": 0.201942176716847, - "timestamp": 4.258476692747647 - }, - { - "x": 0.22515023866998407, - "y": 0.8315371625385247, - "heading": 8.015184743221423e-21, - "angularVelocity": 2.5179924008277626e-19, - "velocityX": -0.6756222122893548, - "velocityY": 0.25324595418005347, - "timestamp": 4.325688824380832 - }, - { - "x": 0.1817364922151659, - "y": 0.8518958223545926, - "heading": -1.0221415362403761e-19, - "angularVelocity": -1.6400213737530966e-18, - "velocityX": -0.6459212853380655, - "velocityY": 0.30290156436603854, - "timestamp": 4.392900956014016 - }, - { - "x": 0.14049289628323774, - "y": 0.8754815778933299, - "heading": 1.279799371183032e-19, - "angularVelocity": 3.4248888768950546e-18, - "velocityX": -0.6136332077223501, - "velocityY": 0.35091515423820036, - "timestamp": 4.460113087647201 - }, - { - "x": 0.1015718469138536, - "y": 0.9021865445377336, - "heading": 2.2387831921490547e-19, - "angularVelocity": 1.4268016689731864e-18, - "velocityX": -0.5790777412297899, - "velocityY": 0.39732360803772554, - "timestamp": 4.527325219280386 - }, - { - "x": 0.06510690798404553, - "y": 0.931906676568605, - "heading": -1.120819147896167e-19, - "angularVelocity": -4.998505735207955e-18, - "velocityX": -0.542535075792835, - "velocityY": 0.4421840419088505, - "timestamp": 4.594537350913571 - }, - { - "x": 0.031215277251384486, - "y": 0.9645425908347474, - "heading": -3.927086503171987e-19, - "angularVelocity": -4.1752392884434696e-18, - "velocityX": -0.5042487108968212, - "velocityY": 0.4855658267804262, - "timestamp": 4.661749482546756 - }, - { - "x": 0, - "y": 1, - "heading": 0, - "angularVelocity": 5.84282387256885e-18, - "velocityX": -0.46442921081188193, - "velocityY": 0.527544779546109, - "timestamp": 4.728961614179941 - }, - { - "x": -0.034523228131504415, - "y": 1.0482920245716583, - "heading": 6.961793842243958e-19, - "angularVelocity": 8.333545907574826e-18, - "velocityX": -0.4132568741634686, - "velocityY": 0.5780748846976158, - "timestamp": 4.81250100648016 - }, - { - "x": -0.06466937577608543, - "y": 1.1006992615641917, - "heading": 2.448822269391661e-18, - "angularVelocity": 2.0979837004191778e-17, - "velocityX": -0.3608614668424137, - "velocityY": 0.6273356263377532, - "timestamp": 4.896040398780379 - }, - { - "x": -0.09031335806620472, - "y": 1.1570844769783872, - "heading": 3.582191703035754e-18, - "angularVelocity": 1.3566885979268263e-17, - "velocityX": -0.30696874353552017, - "velocityY": 0.6749536220177591, - "timestamp": 4.979579791080598 - }, - { - "x": -0.11129933776190112, - "y": 1.217264005888493, - "heading": 4.527037428957119e-18, - "angularVelocity": 1.1310181537234104e-17, - "velocityX": -0.2512105860224372, - "velocityY": 0.7203730749421273, - "timestamp": 5.063119183380817 - }, - { - "x": -0.12742973186409806, - "y": 1.2809814846007999, - "heading": 5.3660105917774544e-18, - "angularVelocity": 1.0042844899524535e-17, - "velocityX": -0.19308728083906038, - "velocityY": 0.7627237517280782, - "timestamp": 5.146658575681036 - }, - { - "x": -0.13845089687734538, - "y": 1.3478595921295984, - "heading": 5.804477769789897e-18, - "angularVelocity": 5.248628037181788e-18, - "velocityX": -0.13192776138040377, - "velocityY": 0.8005577451228699, - "timestamp": 5.230197967981255 - }, - { - "x": -0.14404059723021467, - "y": 1.41730538004403, - "heading": 5.60459161147457e-18, - "angularVelocity": -2.3927171307864518e-18, - "velocityX": -0.06691095301221847, - "velocityY": 0.8312939082063359, - "timestamp": 5.313737360281474 - }, - { - "x": -0.14383032525510003, - "y": 1.488317725972261, - "heading": 4.431739846980368e-18, - "angularVelocity": -1.40395049470865e-17, - "velocityX": 0.002517039797931534, - "velocityY": 0.8500462353500342, - "timestamp": 5.397276752581693 - }, - { - "x": -0.13761503207018486, - "y": 1.559147061402284, - "heading": 3.662869249400604e-18, - "angularVelocity": -9.203688693849832e-18, - "velocityX": 0.07439954988634583, - "velocityY": 0.8478555263543226, - "timestamp": 5.480816144881912 - }, - { - "x": -0.12607515499835087, - "y": 1.627193849249961, - "heading": 1.3243020605488167e-18, - "angularVelocity": -2.7993585531762966e-17, - "velocityX": 0.1381369525691848, - "velocityY": 0.8145473168290962, - "timestamp": 5.564355537182131 - }, - { - "x": -0.11116381180741976, - "y": 1.690268013573467, - "heading": 2.379879606606124e-19, - "angularVelocity": -1.3003614752838011e-17, - "velocityX": 0.17849475295850228, - "velocityY": 0.7550230207185785, - "timestamp": 5.64789492948235 - }, - { - "x": -0.09482890835601043, - "y": 1.747505461760242, - "heading": -2.2107636185603513e-19, - "angularVelocity": -5.4951837817255995e-18, - "velocityX": 0.1955353396958621, - "velocityY": 0.6851551897945106, - "timestamp": 5.731434321782569 - }, - { - "x": -0.07835937233604903, - "y": 1.7987366080747376, - "heading": -5.60981270871949e-19, - "angularVelocity": -4.068797903654375e-18, - "velocityX": 0.19714694548859107, - "velocityY": 0.6132573496630682, - "timestamp": 5.814973714082788 - }, - { - "x": -0.06255809380104226, - "y": 1.843997224564569, - "heading": -8.808563650504602e-19, - "angularVelocity": -3.8290331719286885e-18, - "velocityX": 0.18914763562345505, - "velocityY": 0.5417877152754039, - "timestamp": 5.898513106383007 - }, - { - "x": -0.04794693255938144, - "y": 1.8833690873915618, - "heading": -1.2134869024792098e-18, - "angularVelocity": -3.981720931889883e-18, - "velocityX": 0.17490145474307478, - "velocityY": 0.4712969743124334, - "timestamp": 5.982052498683226 - }, - { - "x": -0.034884553374127325, - "y": 1.9169362042125617, - "heading": -1.5815565915485662e-18, - "angularVelocity": -4.405941868057666e-18, - "velocityX": 0.1563619129321798, - "velocityY": 0.40181183866370773, - "timestamp": 6.065591890983445 - }, - { - "x": -0.0236298825386545, - "y": 1.9447739236006856, - "heading": -7.689111560877529e-19, - "angularVelocity": 9.727690984238907e-18, - "velocityX": 0.13472291963803598, - "velocityY": 0.3332286556273034, - "timestamp": 6.149131283283664 - }, - { - "x": -0.01437751052078003, - "y": 1.9669473155185608, - "heading": -4.414002390832506e-19, - "angularVelocity": 3.9204366617451644e-18, - "velocityX": 0.11075460047188025, - "velocityY": 0.26542438611702496, - "timestamp": 6.232670675583883 - }, - { - "x": -0.007278507844366605, - "y": 1.9835120705105598, - "heading": -6.316031204453377e-19, - "angularVelocity": -2.2768046075674146e-18, - "velocityX": 0.08497790660125235, - "velocityY": 0.19828675473804738, - "timestamp": 6.316210067884102 - }, - { - "x": -0.0024533007608353937, - "y": 1.9945159206791379, - "heading": -4.823898230322713e-19, - "angularVelocity": 1.786142874866039e-18, - "velocityX": 0.05775966224641234, - "velocityY": 0.13172049575166966, - "timestamp": 6.399749460184321 - }, - { - "x": -9.534714777782856e-35, - "y": 2, - "heading": -4.51332949370556e-37, - "angularVelocity": 5.774399435321978e-18, - "velocityX": 0.029366993142814022, - "velocityY": 0.06564662693683221, - "timestamp": 6.48328885248454 - }, - { - "x": 0, - "y": 2, - "heading": -6.22058063415872e-36, - "angularVelocity": -6.906021002654526e-35, - "velocityX": 1.1413436685370076e-33, - "velocityY": -9.106343514179098e-35, - "timestamp": 6.566828244784759 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGFSprint.traj b/src/main/deploy/choreo/SourceLanePHGFSprint.traj deleted file mode 100644 index fa18d9ca..00000000 --- a/src/main/deploy/choreo/SourceLanePHGFSprint.traj +++ /dev/null @@ -1,1336 +0,0 @@ -{ - "samples": [ - { - "x": 1.433172345161438, - "y": 2.136303424835205, - "heading": 2.8770383139324086e-29, - "angularVelocity": 0, - "velocityX": 1.701446192977867e-30, - "velocityY": -1.266914834653595e-29, - "timestamp": 0 - }, - { - "x": 1.4678872089966617, - "y": 2.127700564422531, - "heading": 0.004117315336523342, - "angularVelocity": 0.04300890670031375, - "velocityX": 0.3626266675757136, - "velocityY": -0.08986429034885272, - "timestamp": 0.09573169030115974 - }, - { - "x": 1.537317326749348, - "y": 2.110494823513309, - "heading": 0.0122988440120433, - "angularVelocity": 0.0854631172768069, - "velocityX": 0.7252574098965585, - "velocityY": -0.17972879049106513, - "timestamp": 0.1914633806023195 - }, - { - "x": 1.6414631627627712, - "y": 2.0846861763424815, - "heading": 0.024480626754227467, - "angularVelocity": 0.1272492181380779, - "velocityX": 1.0878930026830405, - "velocityY": -0.26959355977151667, - "timestamp": 0.28719507090347923 - }, - { - "x": 1.780325283643077, - "y": 2.0502745948695433, - "heading": 0.04058342242477801, - "angularVelocity": 0.16820757702525263, - "velocityX": 1.450534514158978, - "velocityY": -0.35945862195920303, - "timestamp": 0.382926761204639 - }, - { - "x": 1.9539044005410915, - "y": 2.0072600541327383, - "heading": 0.060505710064861905, - "angularVelocity": 0.20810546201361826, - "velocityX": 1.8131834542181169, - "velocityY": -0.44932394489394517, - "timestamp": 0.4786584515057987 - }, - { - "x": 2.1622014359513058, - "y": 1.9556425412505571, - "heading": 0.08411192731013951, - "angularVelocity": 0.2465872812881091, - "velocityX": 2.175842030527155, - "velocityY": -0.5391894023871501, - "timestamp": 0.5743901418069585 - }, - { - "x": 2.4052176349352066, - "y": 1.8954220721835344, - "heading": 0.11121111812581518, - "angularVelocity": 0.28307440022634617, - "velocityX": 2.5385136125763252, - "velocityY": -0.6290546931588193, - "timestamp": 0.6701218321081182 - }, - { - "x": 2.682954761527793, - "y": 1.8265987279247267, - "heading": 0.1415138814765405, - "angularVelocity": 0.3165384759644085, - "velocityX": 2.9012036214911956, - "velocityY": -0.7189191378882197, - "timestamp": 0.765853522409278 - }, - { - "x": 2.9954154621622653, - "y": 1.749172751537101, - "heading": 0.17453107861154096, - "angularVelocity": 0.3448930759444131, - "velocityX": 3.2639212746813424, - "velocityY": -0.8087810436154711, - "timestamp": 0.8615852127104378 - }, - { - "x": 3.3426038443567645, - "y": 1.6631449161907195, - "heading": 0.20926651570903568, - "angularVelocity": 0.36284157300443215, - "velocityX": 3.6266818344380534, - "velocityY": -0.8986348729151596, - "timestamp": 0.9573169030115976 - }, - { - "x": 3.724521896188089, - "y": 1.5685193860859707, - "heading": 0.24264187046069935, - "angularVelocity": 0.3486343408669774, - "velocityX": 3.989463161370044, - "velocityY": -0.9884452035533644, - "timestamp": 1.0530485933127574 - }, - { - "x": 4.117233745301501, - "y": 1.4711990601083857, - "heading": 0.24264187604727303, - "angularVelocity": 5.835657624563612e-8, - "velocityX": 4.102213675301073, - "velocityY": -1.0165946686272007, - "timestamp": 1.1487802836139172 - }, - { - "x": 4.509945594349689, - "y": 1.373878733867449, - "heading": 0.24264188163378278, - "angularVelocity": 5.835590832713292e-8, - "velocityX": 4.102213674619734, - "velocityY": -1.0165946713781402, - "timestamp": 1.244511973915077 - }, - { - "x": 4.902657443397878, - "y": 1.27655840762652, - "heading": 0.24264188722029242, - "angularVelocity": 5.835590717483892e-8, - "velocityX": 4.102213674619755, - "velocityY": -1.0165946713780565, - "timestamp": 1.3402436642162368 - }, - { - "x": 5.295369292588233, - "y": 1.1792380819592594, - "heading": 0.24264189280680207, - "angularVelocity": 5.8355907494596246e-8, - "velocityX": 4.102213676104793, - "velocityY": -1.0165946653856088, - "timestamp": 1.4359753545173966 - }, - { - "x": 5.688086680864775, - "y": 1.0819401068348615, - "heading": 0.242641898394392, - "angularVelocity": 5.83671917480604e-8, - "velocityX": 4.102271536518705, - "velocityY": -1.0163611951659655, - "timestamp": 1.5317070448185564 - }, - { - "x": 6.057380727938785, - "y": 1.0111000403294017, - "heading": 0.24430232120402645, - "angularVelocity": 0.017344547115249777, - "velocityX": 3.8575945532093883, - "velocityY": -0.7399855394762825, - "timestamp": 1.6274387351197162 - }, - { - "x": 6.391965385523026, - "y": 0.9488653299205841, - "heading": 0.24119901543014724, - "angularVelocity": -0.03241670299487622, - "velocityX": 3.495025069879339, - "velocityY": -0.6500951795986862, - "timestamp": 1.723170425420876 - }, - { - "x": 6.691840373882702, - "y": 0.895234975869734, - "heading": 0.2333347965847716, - "angularVelocity": -0.08214854265046012, - "velocityX": 3.1324526644833015, - "velocityY": -0.5602152628325792, - "timestamp": 1.8189021157220358 - }, - { - "x": 6.957005576380099, - "y": 0.8502086242600702, - "heading": 0.2207147600128324, - "angularVelocity": -0.1318271570356455, - "velocityX": 2.7698790407075005, - "velocityY": -0.4703390430254154, - "timestamp": 1.9146338060231956 - }, - { - "x": 7.18746090816696, - "y": 0.8137860741035156, - "heading": 0.20334602215443712, - "angularVelocity": -0.18143143407058088, - "velocityX": 2.4073045306193745, - "velocityY": -0.38046492270850996, - "timestamp": 2.010365496324355 - }, - { - "x": 7.383206290326008, - "y": 0.7859671775168647, - "heading": 0.18123751114726283, - "angularVelocity": -0.2309424490133121, - "velocityX": 2.0447291961727903, - "velocityY": -0.29059234715707755, - "timestamp": 2.1060971866255147 - }, - { - "x": 7.544241641769053, - "y": 0.7667518051986745, - "heading": 0.15439971280375697, - "angularVelocity": -0.28034393060874496, - "velocityX": 1.6821530146905204, - "velocityY": -0.2007211222692816, - "timestamp": 2.2018288769266743 - }, - { - "x": 7.670566876398333, - "y": 0.7561398304498085, - "heading": 0.12284435298340343, - "angularVelocity": -0.32962292549710004, - "velocityX": 1.31957593384315, - "velocityY": -0.11085122086217061, - "timestamp": 2.297560567227834 - }, - { - "x": 7.762181902442319, - "y": 0.7541311201311741, - "heading": 0.08658400694043104, - "angularVelocity": -0.37877056101837964, - "velocityX": 0.9569978943632882, - "velocityY": -0.020982710206882894, - "timestamp": 2.3932922575289934 - }, - { - "x": 7.81908662296772, - "y": 0.7607255287810747, - "heading": 0.045631629218029345, - "angularVelocity": -0.42778287517258756, - "velocityX": 0.5944188423317792, - "velocityY": 0.06888428097620071, - "timestamp": 2.489023947830153 - }, - { - "x": 7.841280937194824, - "y": 0.7759228944778442, - "heading": 0, - "angularVelocity": -0.4766616892938097, - "velocityX": 0.2318387375694728, - "velocityY": 0.15874958074840423, - "timestamp": 2.5847556381313126 - }, - { - "x": 7.835403168803218, - "y": 0.7947828328368709, - "heading": -0.04166817387070667, - "angularVelocity": -0.5176676769900772, - "velocityX": -0.0730228955314616, - "velocityY": 0.2343078558429211, - "timestamp": 2.6652477679723754 - }, - { - "x": 7.804986392936784, - "y": 0.8197245003639376, - "heading": -0.0866323351495184, - "angularVelocity": -0.5586156232617521, - "velocityX": -0.3778850916668647, - "velocityY": 0.30986467339008555, - "timestamp": 2.745739897813438 - }, - { - "x": 7.750030563561768, - "y": 0.8507477655512394, - "heading": -0.13488866933437027, - "angularVelocity": -0.5995161797814921, - "velocityX": -0.6827478597060769, - "velocityY": 0.38541985714018384, - "timestamp": 2.826232027654501 - }, - { - "x": 7.6705356351825085, - "y": 0.8878524818601358, - "heading": -0.18643456038885645, - "angularVelocity": -0.640384235777116, - "velocityX": -0.9876111929640158, - "velocityY": 0.4609732203565098, - "timestamp": 2.906724157495564 - }, - { - "x": 7.5665015641225395, - "y": 0.9310384864662067, - "heading": -0.24126895121607964, - "angularVelocity": -0.6812391588502915, - "velocityX": -1.292475068839824, - "velocityY": 0.5365245607114574, - "timestamp": 2.9872162873366266 - }, - { - "x": 7.437928309633229, - "y": 0.9803055976729939, - "heading": -0.29939271538639584, - "angularVelocity": -0.7221049347901444, - "velocityX": -1.5973394509593273, - "velocityY": 0.6120736437468451, - "timestamp": 3.0677084171776894 - }, - { - "x": 7.284815834214756, - "y": 1.0356536080910796, - "heading": -0.36080902567537027, - "angularVelocity": -0.7630101279446713, - "velocityX": -1.9022042989606411, - "velocityY": 0.6876201502642577, - "timestamp": 3.148200547018752 - }, - { - "x": 7.107164100687513, - "y": 1.0970822617404554, - "heading": -0.4255236836274993, - "angularVelocity": -0.8039873970214162, - "velocityX": -2.207069608865607, - "velocityY": 0.763163476594831, - "timestamp": 3.228692676859815 - }, - { - "x": 6.904973048833601, - "y": 1.1645911316931183, - "heading": -0.49354522243909305, - "angularVelocity": -0.8450706789009879, - "velocityX": -2.5119356668921826, - "velocityY": 0.8387014989532872, - "timestamp": 3.309184806700878 - }, - { - "x": 6.684120642089045, - "y": 1.2193164102467027, - "heading": -0.5235123583408667, - "angularVelocity": -0.3722989559393653, - "velocityX": -2.7437764062379517, - "velocityY": 0.6798835943938059, - "timestamp": 3.3896769365419406 - }, - { - "x": 6.487807289631828, - "y": 1.267960625172924, - "heading": -0.5501610284793903, - "angularVelocity": -0.3310717481395217, - "velocityX": -2.4389136285238937, - "velocityY": 0.6043350452407918, - "timestamp": 3.4701690663830034 - }, - { - "x": 6.316033066880454, - "y": 1.310524076323902, - "heading": -0.5734871955472854, - "angularVelocity": -0.2897943825495106, - "velocityX": -2.134049913842353, - "velocityY": 0.5287902213231489, - "timestamp": 3.550661196224066 - }, - { - "x": 6.168797996381023, - "y": 1.3470068919007985, - "heading": -0.5934869971532756, - "angularVelocity": -0.2484690322524527, - "velocityX": -1.8291859190576596, - "velocityY": 0.45324699012227004, - "timestamp": 3.631153326065129 - }, - { - "x": 6.046102088284196, - "y": 1.3774091505678683, - "heading": -0.6101571315963537, - "angularVelocity": -0.20710266302584004, - "velocityX": -1.5243217981654285, - "velocityY": 0.3777047362126793, - "timestamp": 3.7116454559061918 - }, - { - "x": 5.947945348853021, - "y": 1.401730906160479, - "heading": -0.6234950110327551, - "angularVelocity": -0.16570414352854168, - "velocityX": -1.219457599463783, - "velocityY": 0.30216315113081255, - "timestamp": 3.7921375857472546 - }, - { - "x": 5.874327783550542, - "y": 1.4199721961221465, - "heading": -0.6334988572334701, - "angularVelocity": -0.12428353207005839, - "velocityX": -0.914593332891598, - "velocityY": 0.2266220312312794, - "timestamp": 3.8726297155883174 - }, - { - "x": 5.825249398499615, - "y": 1.4321330452450582, - "heading": -0.6401677660826286, - "angularVelocity": -0.08285168826039573, - "velocityX": -0.6097289902516339, - "velocityY": 0.15108121933934396, - "timestamp": 3.95312184542938 - }, - { - "x": 5.800710201263428, - "y": 1.4382134675979614, - "heading": -0.6435017481374367, - "angularVelocity": -0.04141997560898668, - "velocityX": -0.30486455364984805, - "velocityY": 0.07554058223155366, - "timestamp": 4.033613975270443 - }, - { - "x": 5.800710201263428, - "y": 1.4382134675979614, - "heading": -0.6435017481374367, - "angularVelocity": -1.8014229783540224e-27, - "velocityX": -3.376915136198235e-29, - "velocityY": 5.736032048058663e-30, - "timestamp": 4.114106105111506 - }, - { - "x": 5.818547555700618, - "y": 1.4390540038628774, - "heading": -0.6376275065659812, - "angularVelocity": 0.08671691701318579, - "velocityX": 0.26331916480604495, - "velocityY": 0.012408191363030088, - "timestamp": 4.1818465388559645 - }, - { - "x": 5.854212795257727, - "y": 1.4409016567123936, - "heading": -0.6258375764635414, - "angularVelocity": 0.1740456836594175, - "velocityX": 0.5264985413534782, - "velocityY": 0.0272754800550328, - "timestamp": 4.249586972600423 - }, - { - "x": 5.907692159338857, - "y": 1.443961759263086, - "heading": -0.6080852705803181, - "angularVelocity": 0.2620636583195098, - "velocityX": 0.7894747807915273, - "velocityY": 0.04517394385510083, - "timestamp": 4.317327406344882 - }, - { - "x": 5.978964930251624, - "y": 1.4484936039085805, - "heading": -0.5843154945280162, - "angularVelocity": 0.3508949491225589, - "velocityX": 1.0521451808477307, - "velocityY": 0.06690014213062502, - "timestamp": 4.385067840089341 - }, - { - "x": 6.067998493941586, - "y": 1.4548346984588374, - "heading": -0.5544599081943509, - "angularVelocity": 0.44073509251935744, - "velocityX": 1.31433412466517, - "velocityY": 0.09360870900499038, - "timestamp": 4.452808273833799 - }, - { - "x": 6.174738434598293, - "y": 1.4634418593409828, - "heading": -0.5184292628909575, - "angularVelocity": 0.5318927457611795, - "velocityX": 1.575719769663251, - "velocityY": 0.12706090596665875, - "timestamp": 4.520548707578258 - }, - { - "x": 6.2990864932887405, - "y": 1.4749664049356752, - "heading": -0.47610067620246677, - "angularVelocity": 0.6248644177297312, - "velocityX": 1.8356548934943828, - "velocityY": 0.1701280159818153, - "timestamp": 4.588289141322717 - }, - { - "x": 6.440844047185778, - "y": 1.490406322856265, - "heading": -0.42729541631250667, - "angularVelocity": 0.7204745702407376, - "velocityX": 2.0926579010668633, - "velocityY": 0.22792765069729165, - "timestamp": 4.656029575067175 - }, - { - "x": 6.59953463470863, - "y": 1.5114574086941215, - "heading": -0.371739955153997, - "angularVelocity": 0.8201226075416767, - "velocityX": 2.3426272722358106, - "velocityY": 0.31076101338926715, - "timestamp": 4.723770008811634 - }, - { - "x": 6.773654182639981, - "y": 1.5414451590022467, - "heading": -0.3090282649921381, - "angularVelocity": 0.9257645204698558, - "velocityX": 2.570393165597269, - "velocityY": 0.4426861277748774, - "timestamp": 4.791510442556093 - }, - { - "x": 6.956422392745004, - "y": 1.5870356897360083, - "heading": -0.23940243254396396, - "angularVelocity": 1.027832693112474, - "velocityX": 2.698066723258537, - "velocityY": 0.6730179925588539, - "timestamp": 4.8592508763005515 - }, - { - "x": 7.13098273232138, - "y": 1.6485480325873274, - "heading": -0.1686125340995527, - "angularVelocity": 1.0450169054342962, - "velocityX": 2.576900234133126, - "velocityY": 0.9080594772003634, - "timestamp": 4.92699131004501 - }, - { - "x": 7.290659982684113, - "y": 1.7200152367861081, - "heading": -0.1002049626024793, - "angularVelocity": 1.0098484422926985, - "velocityX": 2.357192618002618, - "velocityY": 1.0550154501280693, - "timestamp": 4.994731743789469 - }, - { - "x": 7.433825813884328, - "y": 1.798376978611666, - "heading": -0.035376660914373755, - "angularVelocity": 0.9570104309142923, - "velocityX": 2.1134472173633743, - "velocityY": 1.1567942142379297, - "timestamp": 5.062472177533928 - }, - { - "x": 7.559908602192434, - "y": 1.882036919563645, - "heading": 0.025328755206427432, - "angularVelocity": 0.8961474375821659, - "velocityX": 1.8612633746004037, - "velocityY": 1.235007459024761, - "timestamp": 5.130212611278386 - }, - { - "x": 7.6686462166126725, - "y": 1.9700426296704405, - "heading": 0.08160707257360432, - "angularVelocity": 0.8307935786103601, - "velocityX": 1.6052098932586671, - "velocityY": 1.2991607115889543, - "timestamp": 5.197953045022845 - }, - { - "x": 7.759896555956456, - "y": 2.061767068594048, - "heading": 0.13326449164713097, - "angularVelocity": 0.762578805863526, - "velocityX": 1.347058681200838, - "velocityY": 1.354057449198286, - "timestamp": 5.265693478767304 - }, - { - "x": 7.833573521261895, - "y": 2.156767827393036, - "heading": 0.1801664809201526, - "angularVelocity": 0.6923780477986421, - "velocityX": 1.0876364562910028, - "velocityY": 1.4024232433669601, - "timestamp": 5.3334339125117625 - }, - { - "x": 7.8896206787548335, - "y": 2.2547166504175005, - "heading": 0.22221394940579448, - "angularVelocity": 0.6207144855945275, - "velocityX": 0.8273811429133712, - "velocityY": 1.4459432514701964, - "timestamp": 5.401174346256221 - }, - { - "x": 7.9279988186375325, - "y": 2.3553605545122482, - "heading": 0.2593308440003242, - "angularVelocity": 0.547928209827353, - "velocityX": 0.566547005404121, - "velocityY": 1.485728663539606, - "timestamp": 5.46891478000068 - }, - { - "x": 7.948679447174072, - "y": 2.45849871635437, - "heading": 0.2914571610744201, - "angularVelocity": 0.47425614656215387, - "velocityX": 0.30529223675411965, - "velocityY": 1.5225494751214026, - "timestamp": 5.536655213745139 - }, - { - "x": 7.946831051825066, - "y": 2.5911250937356964, - "heading": 0.3237506602012709, - "angularVelocity": 0.38120746378091935, - "velocityX": -0.02181931726540404, - "velocityY": 1.5655833625644262, - "timestamp": 5.621368925293583 - }, - { - "x": 7.917248965751211, - "y": 2.7272096797694707, - "heading": 0.34809099666884863, - "angularVelocity": 0.28732463756659415, - "velocityX": -0.34920068467238213, - "velocityY": 1.6064056638098445, - "timestamp": 5.7060826368420265 - }, - { - "x": 7.859906635586575, - "y": 2.8665210553288647, - "heading": 0.3644006813152481, - "angularVelocity": 0.19252709329200723, - "velocityX": -0.6768955003446485, - "velocityY": 1.6444961861897385, - "timestamp": 5.7907963483904705 - }, - { - "x": 7.774773151306567, - "y": 3.008766263060034, - "heading": 0.37259167904917256, - "angularVelocity": 0.09669034190811442, - "velocityX": -1.004955192304655, - "velocityY": 1.679128503888361, - "timestamp": 5.8755100599389145 - }, - { - "x": 7.6618127476379465, - "y": 3.1535627303797917, - "heading": 0.3725589270974435, - "angularVelocity": -0.00038661925124551516, - "velocityX": -1.333437074162696, - "velocityY": 1.7092447571129605, - "timestamp": 5.9602237714873585 - }, - { - "x": 7.520985197062152, - "y": 3.300390177076741, - "heading": 0.3641689647090754, - "angularVelocity": -0.09903901310675314, - "velocityX": -1.6623938203352226, - "velocityY": 1.7332193810559793, - "timestamp": 6.0449374830358025 - }, - { - "x": 7.352249696631812, - "y": 3.448501059595182, - "heading": 0.34723906580696273, - "angularVelocity": -0.19984839045130479, - "velocityX": -1.991832223451176, - "velocityY": 1.748369653639153, - "timestamp": 6.1296511945842465 - }, - { - "x": 7.155582548717868, - "y": 3.5967336644018815, - "heading": 0.32149579845629134, - "angularVelocity": -0.30388548536148036, - "velocityX": -2.3215503643879303, - "velocityY": 1.7498065200688502, - "timestamp": 6.21436490613269 - }, - { - "x": 6.931058620807543, - "y": 3.7430527877883204, - "heading": 0.28648334175757967, - "angularVelocity": -0.4133033018945168, - "velocityX": -2.6503847347300935, - "velocityY": 1.7272188965863609, - "timestamp": 6.299078617681134 - }, - { - "x": 6.679342452608786, - "y": 3.8831106231055927, - "heading": 0.24134794483523736, - "angularVelocity": -0.5327991903238861, - "velocityX": -2.9713745696859344, - "velocityY": 1.6533077438966797, - "timestamp": 6.383792329229578 - }, - { - "x": 6.406788011208051, - "y": 4.004645601729296, - "heading": 0.18499930261907258, - "angularVelocity": -0.6651655462403078, - "velocityX": -3.2173592257833326, - "velocityY": 1.4346553397581023, - "timestamp": 6.468506040778022 - }, - { - "x": 6.145744610358307, - "y": 4.100645232567065, - "heading": 0.1258988121859892, - "angularVelocity": -0.6976496408056244, - "velocityX": -3.0814775563276378, - "velocityY": 1.1332242335158502, - "timestamp": 6.553219752326466 - }, - { - "x": 5.907416819572644, - "y": 4.18026386145866, - "heading": 0.07010067718233673, - "angularVelocity": -0.6586671033973509, - "velocityX": -2.8133319439011144, - "velocityY": 0.9398552776909607, - "timestamp": 6.63793346387491 - }, - { - "x": 5.694140881693529, - "y": 4.247396241924432, - "heading": 0.01923390264073781, - "angularVelocity": -0.6004550339234085, - "velocityX": -2.517608235794895, - "velocityY": 0.7924618015040228, - "timestamp": 6.722647175423354 - }, - { - "x": 5.50676824724975, - "y": 4.303942898252954, - "heading": -0.02601095760308067, - "angularVelocity": -0.5340913462154806, - "velocityX": -2.2118336101544562, - "velocityY": 0.6675029967986442, - "timestamp": 6.807360886971798 - }, - { - "x": 5.345718150174197, - "y": 4.351008976671829, - "heading": -0.06525584873836304, - "angularVelocity": -0.4632649239177752, - "velocityX": -1.901110152439194, - "velocityY": 0.5555898514960005, - "timestamp": 6.892074598520242 - }, - { - "x": 5.21123496648668, - "y": 4.389313273102533, - "heading": -0.09826172817354481, - "angularVelocity": -0.38961673183575596, - "velocityX": -1.5875019666752757, - "velocityY": 0.45216170712576903, - "timestamp": 6.976788310068686 - }, - { - "x": 5.103477083411047, - "y": 4.419359601001064, - "heading": -0.1248629898593969, - "angularVelocity": -0.3140136490258714, - "velocityX": -1.2720241045513758, - "velocityY": 0.3546808108076878, - "timestamp": 7.06150202161713 - }, - { - "x": 5.022554847546882, - "y": 4.44152029736655, - "heading": -0.14493751186900491, - "angularVelocity": -0.23696898226597282, - "velocityX": -0.9552436599108176, - "velocityY": 0.26159515337505695, - "timestamp": 7.146215733165574 - }, - { - "x": 4.968549251002919, - "y": 4.456081570178744, - "heading": -0.15839111246886314, - "angularVelocity": -0.15881255057706564, - "velocityX": -0.6375071467985308, - "velocityY": 0.17188802787689772, - "timestamp": 7.230929444714018 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": -0.07976943545791065, - "velocityX": -0.3190407913838158, - "velocityY": 0.08485777647782772, - "timestamp": 7.315643156262462 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": -5.429647481375564e-36, - "velocityX": -2.0130702505219768e-38, - "velocityY": 9.290884594011316e-38, - "timestamp": 7.400356867810906 - }, - { - "x": 4.970877788406757, - "y": 4.459511277906968, - "heading": -0.16349815793624287, - "angularVelocity": 0.018956158928001787, - "velocityX": 0.33714881644675093, - "velocityY": -0.04317094482113791, - "timestamp": 7.487427222340686 - }, - { - "x": 5.029589121543177, - "y": 4.45199345668782, - "heading": -0.16019712382497864, - "angularVelocity": 0.03791226220556215, - "velocityX": 0.6742976234964045, - "velocityY": -0.0863419157961063, - "timestamp": 7.574497576870465 - }, - { - "x": 5.117656119781506, - "y": 4.440716720346877, - "heading": -0.15524559648557837, - "angularVelocity": 0.05686811965037711, - "velocityX": 1.0114464184042, - "velocityY": -0.12951292551686924, - "timestamp": 7.661567931400245 - }, - { - "x": 5.235078781735307, - "y": 4.425681064380957, - "heading": -0.14864360395133913, - "angularVelocity": 0.0758236551337492, - "velocityX": 1.3485951973888195, - "velocityY": -0.17268398695652012, - "timestamp": 7.748638285930024 - }, - { - "x": 5.381857105533103, - "y": 4.406886483098539, - "heading": -0.14039116676025787, - "angularVelocity": 0.09477895473893824, - "velocityX": 1.6857439548795636, - "velocityY": -0.21585511376308925, - "timestamp": 7.835708640459804 - }, - { - "x": 5.557991088523465, - "y": 4.3843329695031, - "heading": -0.13048827839881783, - "angularVelocity": 0.11373432915164189, - "velocityX": 2.0228926819187647, - "velocityY": -0.25902632092448025, - "timestamp": 7.922778994989583 - }, - { - "x": 5.763480726622518, - "y": 4.3580205150215345, - "heading": -0.11893487664373759, - "angularVelocity": 0.13269041819657243, - "velocityX": 2.360041362054773, - "velocityY": -0.30219762654769566, - "timestamp": 8.009849349519362 - }, - { - "x": 5.998326012444723, - "y": 4.327949108694172, - "heading": -0.10573079137043012, - "angularVelocity": 0.15164846111648134, - "velocityX": 2.697189957368156, - "velocityY": -0.34536905804233253, - "timestamp": 8.09691970404914 - }, - { - "x": 6.262526926435642, - "y": 4.2941187332410315, - "heading": -0.09087557162555815, - "angularVelocity": 0.17061168321981837, - "velocityX": 3.034338328099486, - "velocityY": -0.38854068799696395, - "timestamp": 8.18399005857892 - }, - { - "x": 6.556082683402125, - "y": 4.256529028141766, - "heading": -0.0743557876801526, - "angularVelocity": 0.18972914529428694, - "velocityX": 3.371477680914724, - "velocityY": -0.4317164585152746, - "timestamp": 8.271060413108698 - }, - { - "x": 6.820282808642692, - "y": 4.222698267435462, - "heading": -0.05948639298755931, - "angularVelocity": 0.17077448200245657, - "velocityX": 3.0343292693290502, - "velocityY": -0.3885451126161881, - "timestamp": 8.358130767638476 - }, - { - "x": 7.055127288351479, - "y": 4.192626473165152, - "heading": -0.04626808716200777, - "angularVelocity": 0.15181178366548068, - "velocityX": 2.6971806991835345, - "velocityY": -0.3453735135536204, - "timestamp": 8.445201122168255 - }, - { - "x": 7.260616119038252, - "y": 4.166313656843632, - "heading": -0.034701331761684315, - "angularVelocity": 0.13284378434875296, - "velocityX": 2.360032088952761, - "velocityY": -0.3022017822670142, - "timestamp": 8.532271476698034 - }, - { - "x": 7.436749299548261, - "y": 4.1437598266696405, - "heading": -0.02478650882076325, - "angularVelocity": 0.11387139738279192, - "velocityX": 2.022883465459762, - "velocityY": -0.25902995681816454, - "timestamp": 8.619341831227812 - }, - { - "x": 7.583526829501114, - "y": 4.124964988926465, - "heading": -0.016523936768266155, - "angularVelocity": 0.09489535327056917, - "velocityX": 1.68573483759795, - "velocityY": -0.2158580592059934, - "timestamp": 8.706412185757591 - }, - { - "x": 7.70094870876185, - "y": 4.10992914844881, - "heading": -0.009913866219354965, - "angularVelocity": 0.07591643085190908, - "velocityX": 1.3485862081861149, - "velocityY": -0.1726861060673939, - "timestamp": 8.79348254028737 - }, - { - "x": 7.789014937206106, - "y": 4.09865230882672, - "heading": -0.004956472404176458, - "angularVelocity": 0.05693549592109463, - "velocityX": 1.011437577345994, - "velocityY": -0.12951411169725838, - "timestamp": 8.880552894817148 - }, - { - "x": 7.847725514598055, - "y": 4.091134472510321, - "heading": -0.0016518483286155007, - "angularVelocity": 0.03795349282092027, - "velocityX": 0.6742889437973931, - "velocityY": -0.08634208918752465, - "timestamp": 8.967623249346927 - }, - { - "x": 7.87708044052124, - "y": 4.087375640869141, - "heading": 7.60993628191321e-37, - "angularVelocity": 0.01897142072679338, - "velocityX": 0.3371403054657971, - "velocityY": -0.043170050948797965, - "timestamp": 9.054693603876705 - }, - { - "x": 7.87366387419082, - "y": 4.087813222475328, - "heading": -0.0001932265358713677, - "angularVelocity": -0.002007825984855413, - "velocityX": -0.035501700769334996, - "velocityY": 0.004546930965948054, - "timestamp": 9.150930298355625 - }, - { - "x": 7.834385473616069, - "y": 4.092842926972847, - "heading": -0.0024054804575218995, - "angularVelocity": -0.02298763412052877, - "velocityX": -0.4081437001491553, - "velocityY": 0.05226389502209074, - "timestamp": 9.247166992834545 - }, - { - "x": 7.759245239147118, - "y": 4.102464751168547, - "heading": -0.006636690667772024, - "angularVelocity": -0.04396670348207906, - "velocityX": -0.780785695890768, - "velocityY": 0.09998082589804004, - "timestamp": 9.343403687313465 - }, - { - "x": 7.6482431709074055, - "y": 4.116678690374747, - "heading": -0.012886671317606013, - "angularVelocity": -0.06494384167780186, - "velocityX": -1.153427690349718, - "velocityY": 0.14769770806408986, - "timestamp": 9.439640381792385 - }, - { - "x": 7.501379268940608, - "y": 4.135484738377977, - "heading": -0.02115513791608259, - "angularVelocity": -0.08591802371482843, - "velocityX": -1.526069684354818, - "velocityY": 0.19541452566566683, - "timestamp": 9.535877076271305 - }, - { - "x": 7.318653533488854, - "y": 4.158882887388937, - "heading": -0.031441729230877964, - "angularVelocity": -0.10688845216985864, - "velocityX": -1.8987116758439624, - "velocityY": 0.24313126232826374, - "timestamp": 9.632113770750225 - }, - { - "x": 7.100065965603603, - "y": 4.1868731279492994, - "heading": -0.04374603503260887, - "angularVelocity": -0.12785461791215239, - "velocityX": -2.2713536564073333, - "velocityY": 0.2908479007089456, - "timestamp": 9.728350465229145 - }, - { - "x": 6.845616568893901, - "y": 4.219455448693913, - "heading": -0.05806762969505058, - "angularVelocity": -0.14881636095240902, - "velocityX": -2.643995599468943, - "velocityY": 0.3385644209938051, - "timestamp": 9.824587159708065 - }, - { - "x": 6.555305358042951, - "y": 4.256629835248091, - "heading": -0.07440611137609496, - "angularVelocity": -0.16977392843249786, - "velocityX": -3.016637389956695, - "velocityY": 0.38628079191062686, - "timestamp": 9.920823854186985 - }, - { - "x": 6.23254875309289, - "y": 4.297957965433844, - "heading": -0.09255419315316445, - "angularVelocity": -0.18857756779088714, - "velocityX": -3.353778999763519, - "velocityY": 0.4294425365451959, - "timestamp": 10.017060548665905 - }, - { - "x": 5.945653966420611, - "y": 4.334694055593217, - "heading": -0.10868589302504747, - "angularVelocity": -0.16762524896796518, - "velocityX": -2.981137166292844, - "velocityY": 0.3817264335426585, - "timestamp": 10.113297243144824 - }, - { - "x": 5.694621016414327, - "y": 4.366838114244224, - "heading": -0.12280128203638066, - "angularVelocity": -0.14667366837319035, - "velocityX": -2.6084951417493896, - "velocityY": 0.3340104190512136, - "timestamp": 10.209533937623744 - }, - { - "x": 5.479449909856628, - "y": 4.394390149838104, - "heading": -0.13490034942614496, - "angularVelocity": -0.12572197596016224, - "velocityX": -2.2358530467277298, - "velocityY": 0.28629449237696436, - "timestamp": 10.305770632102664 - }, - { - "x": 5.300140650438581, - "y": 4.417350169666037, - "heading": -0.1449830377404426, - "angularVelocity": -0.10476968654099145, - "velocityX": -1.863210913351998, - "velocityY": 0.23857864146571758, - "timestamp": 10.402007326581584 - }, - { - "x": 5.156693240503607, - "y": 4.435718179635196, - "heading": -0.1530492718346854, - "angularVelocity": -0.08381661629088484, - "velocityX": -1.4905687556256995, - "velocityY": 0.19086285193619654, - "timestamp": 10.498244021060504 - }, - { - "x": 5.04910768165416, - "y": 4.449494184192496, - "heading": -0.1590989820260221, - "angularVelocity": -0.06286282196300745, - "velocityX": -1.117926581248191, - "velocityY": 0.14314710861477242, - "timestamp": 10.594480715539424 - }, - { - "x": 4.97738397502577, - "y": 4.458678186291814, - "heading": -0.16313212141892855, - "angularVelocity": -0.0419085403415415, - "velocityX": -0.7452843950713719, - "velocityY": 0.09543139598720737, - "timestamp": 10.690717410018344 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": -0.020954127805584536, - "velocityX": -0.37264220046732904, - "velocityY": 0.047715698372427014, - "timestamp": 10.786954104497264 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": 0, - "velocityX": 6.569525640446563e-39, - "velocityY": 1.8902971795210484e-36, - "timestamp": 10.883190798976184 - }, - { - "x": 4.970161732141451, - "y": 4.445728451981636, - "heading": -0.16350561334854938, - "angularVelocity": 0.017714526683228992, - "velocityX": 0.3087750250459319, - "velocityY": -0.18912442074741204, - "timestamp": 10.975943153891373 - }, - { - "x": 5.027440952562264, - "y": 4.4106449800780645, - "heading": -0.1602193291584108, - "angularVelocity": 0.03543073588960003, - "velocityX": 0.6175500392759685, - "velocityY": -0.378248853472785, - "timestamp": 11.068695508806563 - }, - { - "x": 5.113359780805984, - "y": 4.358019770281013, - "heading": -0.1552893983395244, - "angularVelocity": 0.053151543412500564, - "velocityX": 0.9263250331733618, - "velocityY": -0.5673733011433544, - "timestamp": 11.161447863721753 - }, - { - "x": 5.227918214012291, - "y": 4.287852820966911, - "heading": -0.14871510376483507, - "angularVelocity": 0.07088008256718281, - "velocityX": 1.2350999962325, - "velocityY": -0.756497766318278, - "timestamp": 11.254200218636942 - }, - { - "x": 5.371116248190762, - "y": 4.200144130331831, - "heading": -0.1404954056786583, - "angularVelocity": 0.0886198317411194, - "velocityX": 1.5438749162693188, - "velocityY": -0.9456222509420716, - "timestamp": 11.346952573552132 - }, - { - "x": 5.5429538779355045, - "y": 4.094893696480259, - "heading": -0.1306288879973164, - "angularVelocity": 0.1063748482759657, - "velocityX": 1.8526497780230582, - "velocityY": -1.1347467560021554, - "timestamp": 11.439704928467322 - }, - { - "x": 5.7434310958415224, - "y": 3.9721015175775665, - "heading": -0.11911365753726154, - "angularVelocity": 0.12415027597503053, - "velocityX": 2.1614245599405897, - "velocityY": -1.3238712808420907, - "timestamp": 11.532457283382511 - }, - { - "x": 5.972547891098849, - "y": 3.83176759216481, - "heading": -0.10594711713280619, - "angularVelocity": 0.1419537047495384, - "velocityX": 2.470199225311589, - "velocityY": -1.5129958214114747, - "timestamp": 11.6252096382977 - }, - { - "x": 6.230304245042205, - "y": 3.673891920053355, - "heading": -0.091125277264113, - "angularVelocity": 0.15980014612292848, - "velocityX": 2.7789736894447628, - "velocityY": -1.702120364014608, - "timestamp": 11.71796199321289 - }, - { - "x": 6.5167001058009015, - "y": 3.498474507164359, - "heading": -0.07463891609789546, - "angularVelocity": 0.17774601174592225, - "velocityX": 3.087747594339454, - "velocityY": -1.8912448427793878, - "timestamp": 11.81071434812808 - }, - { - "x": 6.774456304633716, - "y": 3.340598685286401, - "heading": -0.059730001301582306, - "angularVelocity": 0.16073893552293816, - "velocityX": 2.7789720171363874, - "velocityY": -1.7021219787067958, - "timestamp": 11.90346670304327 - }, - { - "x": 7.003572909925525, - "y": 3.200264603321266, - "heading": -0.04646689079252882, - "angularVelocity": 0.14299486542612352, - "velocityX": 2.4701971772178397, - "velocityY": -1.512997509264889, - "timestamp": 11.99621905795846 - }, - { - "x": 7.204049930227038, - "y": 3.0774722725779653, - "heading": -0.03485583277229418, - "angularVelocity": 0.12518343098513846, - "velocityX": 2.161422429487887, - "velocityY": -1.3238729178960327, - "timestamp": 12.088971412873649 - }, - { - "x": 7.375887370453985, - "y": 2.9722216978386684, - "heading": -0.024899933451460868, - "angularVelocity": 0.107338507253393, - "velocityX": 1.8526477347563841, - "velocityY": -1.1347482749687028, - "timestamp": 12.181723767788839 - }, - { - "x": 7.519085234016694, - "y": 2.884512881951178, - "heading": -0.016601225810711995, - "angularVelocity": 0.0894716651489553, - "velocityX": 1.5438730767929685, - "velocityY": -0.9456236013380901, - "timestamp": 12.274476122704028 - }, - { - "x": 7.633643523301954, - "y": 2.814345826878192, - "heading": -0.009961123896120593, - "angularVelocity": 0.07158957765183391, - "velocityX": 1.235098444562514, - "velocityY": -0.7564989065468437, - "timestamp": 12.367228477619218 - }, - { - "x": 7.7195622398456, - "y": 2.761720534080977, - "heading": -0.004980577858606377, - "angularVelocity": 0.05369724620003789, - "velocityX": 0.92632382889046, - "velocityY": -0.5673741960011052, - "timestamp": 12.459980832534407 - }, - { - "x": 7.776841384411153, - "y": 2.726637004697944, - "heading": -0.0016601403967374856, - "angularVelocity": 0.035798955885325216, - "velocityX": 0.6175492214502578, - "velocityY": -0.37824947318170576, - "timestamp": 12.552733187449597 - }, - { - "x": 7.80548095703125, - "y": 2.7090952396392822, - "heading": 2.2544024955772584e-32, - "angularVelocity": 0.017898633390553583, - "velocityX": 0.3087746143619144, - "velocityY": -0.1891247405491907, - "timestamp": 12.645485542364787 - }, - { - "x": 7.80548095703125, - "y": 2.7090952396392822, - "heading": 1.2738822120559367e-32, - "angularVelocity": -1.0571378529733318e-31, - "velocityX": -6.060794460308241e-34, - "velocityY": -2.7880656703838183e-33, - "timestamp": 12.738237897279976 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubGSub.1.traj b/src/main/deploy/choreo/SourceLanePSubHSubGSub.1.traj index 9f900ddb..1d70bb19 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubGSub.1.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubGSub.1.traj @@ -5,945 +5,900 @@ "y": 4.409571170806885, "heading": -1.0475181007536924, "angularVelocity": 0, - "velocityX": -1.2693923486855146e-35, - "velocityY": 4.115755350063261e-34, + "velocityX": -1.7128958192175968e-32, + "velocityY": -9.692960229070848e-33, "timestamp": 0 }, { - "x": 0.6686935325209526, - "y": 4.399440636010588, - "heading": -1.042618245961612, - "angularVelocity": 0.09045080045353641, - "velocityX": 0.09617884019158171, - "velocityY": -0.1870085992810291, - "timestamp": 0.05417149176692242 - }, - { - "x": 0.6791748263382973, - "y": 4.379207943091365, - "heading": -1.0329282635016275, - "angularVelocity": 0.17887604981743244, - "velocityX": 0.19348357365607174, - "velocityY": -0.37349336817742057, - "timestamp": 0.10834298353384483 - }, - { - "x": 0.6949946113692655, - "y": 4.348905021041836, - "heading": -1.0185695679650135, - "angularVelocity": 0.26505999868701274, - "velocityX": 0.2920315559895287, - "velocityY": -0.5593887312520465, - "timestamp": 0.16251447530076724 - }, - { - "x": 0.7162276376127428, - "y": 4.308568047404174, - "heading": -0.9996771897134135, - "angularVelocity": 0.3487513013835064, - "velocityX": 0.3919594153846487, - "velocityY": -0.7446162607301866, - "timestamp": 0.21668596706768967 - }, - { - "x": 0.7429573380312586, - "y": 4.258238333670889, - "heading": -0.9764021455676966, - "angularVelocity": 0.4296548495629363, - "velocityX": 0.49342743843058046, - "velocityY": -0.9290811844324458, - "timestamp": 0.2708574588346121 - }, - { - "x": 0.775277417821335, - "y": 4.197963466887649, - "heading": -0.9489143945994782, - "angularVelocity": 0.5074209712829532, - "velocityX": 0.5966252494787523, - "velocityY": -1.1126676563122382, - "timestamp": 0.3250289506015345 - }, - { - "x": 0.8132938470478027, - "y": 4.127798804086614, - "heading": -0.91740659080495, - "angularVelocity": 0.5816307206398024, - "velocityX": 0.701779256699018, - "velocityY": -1.2952322432419845, - "timestamp": 0.37920044236845696 - }, - { - "x": 0.8571273890255799, - "y": 4.047809461983153, - "heading": -0.8820989485703652, - "angularVelocity": 0.651775335752228, - "velocityX": 0.809162541921033, - "velocityY": -1.4765947825033474, - "timestamp": 0.4333719341353794 - }, - { - "x": 0.9069168499797303, - "y": 3.95807301716377, - "heading": -0.843245694489665, - "angularVelocity": 0.7172269548689871, - "velocityX": 0.9191081753549234, - "velocityY": -1.6565252661950285, - "timestamp": 0.4875434259023018 - }, - { - "x": 0.9628233134219261, - "y": 3.8586832511299094, - "heading": -0.8011438194850887, - "angularVelocity": 0.7771961530194386, - "velocityX": 1.0320273933518045, - "velocityY": -1.8347245533036767, - "timestamp": 0.5417149176692242 - }, - { - "x": 1.025035737811962, - "y": 3.749755475919721, - "heading": -0.7561452166136415, - "angularVelocity": 0.8306694426112097, - "velocityX": 1.1484347645014206, - "velocityY": -2.0107951924023055, - "timestamp": 0.5958864094361466 - }, - { - "x": 1.0937784660640726, - "y": 3.631434328521385, - "heading": -0.7086738761047954, - "angularVelocity": 0.8763159174773277, - "velocityX": 1.268983482084674, - "velocityY": -2.1841958480195354, - "timestamp": 0.6500579012030689 - }, - { - "x": 1.1693214408917312, - "y": 3.5039055633996625, - "heading": -0.6592507614486313, - "angularVelocity": 0.9123454614986682, - "velocityX": 1.3945153135654589, - "velocityY": -2.3541674958929617, - "timestamp": 0.7042293929699913 - }, - { - "x": 1.2519942470920469, - "y": 3.3674145874122954, - "heading": -0.6085305941721398, - "angularVelocity": 0.9362889154819557, - "velocityX": 1.5261312454902076, - "velocityY": -2.519608959166787, - "timestamp": 0.7584008847369137 - }, - { - "x": 1.3422054245010013, - "y": 3.222296884123663, - "heading": -0.557357603065156, - "angularVelocity": 0.9446479954282978, - "velocityX": 1.665288779513328, - "velocityY": -2.678857431377627, - "timestamp": 0.8125723765038361 - }, - { - "x": 1.4404683178656266, - "y": 3.0690304198840344, - "heading": -0.506852671522414, - "angularVelocity": 0.9323156866353964, - "velocityX": 1.813922603191545, - "velocityY": -2.82928269539025, - "timestamp": 0.8667438682707584 - }, - { - "x": 1.5474318621807708, - "y": 2.9083305512073854, - "heading": -0.4585562938775888, - "angularVelocity": 0.8915460156169343, - "velocityX": 1.974535698137392, - "velocityY": -2.9665025539277043, - "timestamp": 0.9209153600376808 - }, - { - "x": 1.6638998489942354, - "y": 2.7413289476125455, - "heading": -0.41468556937766105, - "angularVelocity": 0.8098489273414394, - "velocityX": 2.1499866999154866, - "velocityY": -3.082831913017614, - "timestamp": 0.9750868518046032 - }, - { - "x": 1.7907578946957254, - "y": 2.5699019528024616, - "heading": -0.37863644859559736, - "angularVelocity": 0.6654629511989103, - "velocityX": 2.3417860864402265, - "velocityY": -3.164524166099463, - "timestamp": 1.0292583435715257 - }, - { - "x": 1.9285624590284545, - "y": 2.3970723087204746, - "heading": -0.35526123126492787, - "angularVelocity": 0.43150403594649805, - "velocityX": 2.543857660882686, - "velocityY": -3.1904169230857033, - "timestamp": 1.0834298353384482 - }, - { - "x": 2.076969770307468, - "y": 2.2268102881561207, - "heading": -0.3460628874086392, - "angularVelocity": 0.16980045326913482, - "velocityX": 2.7395832464342806, - "velocityY": -3.1430188649210824, - "timestamp": 1.1376013271053707 - }, - { - "x": 2.235228147006563, - "y": 2.062095599555062, - "heading": -0.34448101087817407, - "angularVelocity": 0.02920127319497364, - "velocityX": 2.9214328706326906, - "velocityY": -3.0406157044697584, - "timestamp": 1.1917728188722931 - }, - { - "x": 2.4019056025260537, - "y": 1.9051420100449306, - "heading": -0.3444808874915784, - "angularVelocity": 0.0000022777034862099955, - "velocityX": 3.076848173881473, - "velocityY": -2.897346637331644, - "timestamp": 1.2459443106392156 - }, - { - "x": 2.5762261949471483, - "y": 1.7567232022649006, - "heading": -0.34448083068467217, - "angularVelocity": 0.0000010486494725236109, - "velocityX": 3.217939671499618, - "velocityY": -2.7397954706253063, - "timestamp": 1.300115802406138 + "x": 0.6705320996855912, + "y": 4.398099342118358, + "heading": -1.0386114127976835, + "angularVelocity": 0.15927551181553445, + "velocityX": 0.1260500242755718, + "velocityY": -0.20514712032687513, + "timestamp": 0.05592000838348859 + }, + { + "x": 0.6846335258864488, + "y": 4.375148415401169, + "heading": -1.0210111049652701, + "angularVelocity": 0.31474079388031545, + "velocityX": 0.25217138924859694, + "velocityY": -0.41042423598715305, + "timestamp": 0.11184001676697718 + }, + { + "x": 0.7057920861320249, + "y": 4.340710150429397, + "heading": -0.99496347376924, + "angularVelocity": 0.4658016325283906, + "velocityX": 0.3783719076090773, + "velocityY": -0.6158487090273924, + "timestamp": 0.16776002515046576 + }, + { + "x": 0.7340126708459819, + "y": 4.29477510174039, + "heading": -0.9607567098648512, + "angularVelocity": 0.6117088479280167, + "velocityX": 0.5046598798845959, + "velocityY": -0.8214420923185946, + "timestamp": 0.22368003353395435 + }, + { + "x": 0.7693006604451206, + "y": 4.237332365746024, + "heading": -0.918731573784226, + "angularVelocity": 0.7515223494321578, + "velocityX": 0.6310440684690269, + "velocityY": -1.0272304610620717, + "timestamp": 0.2796000419174429 + }, + { + "x": 0.8116619611973517, + "y": 4.168369327121192, + "heading": -0.8692948358062459, + "angularVelocity": 0.8840617054087768, + "velocityX": 0.7575338769931039, + "velocityY": -1.2332444257142754, + "timestamp": 0.3355200503009315 + }, + { + "x": 0.8611030812908155, + "y": 4.087871426229515, + "heading": -0.8129373545696883, + "angularVelocity": 1.0078231900479873, + "velocityX": 0.8841400694078274, + "velocityY": -1.4395187557848361, + "timestamp": 0.3914400586844201 + }, + { + "x": 0.9176312901451578, + "y": 3.995821968070834, + "heading": -0.7502611156342615, + "angularVelocity": 1.120819555419329, + "velocityX": 1.0108762585778373, + "velocityY": -1.6460916373156267, + "timestamp": 0.4473600670679087 + }, + { + "x": 0.9812548770105831, + "y": 3.8922019971306105, + "heading": -0.6820246305048665, + "angularVelocity": 1.2202516970570252, + "velocityX": 1.1377606818136925, + "velocityY": -1.8530034943775027, + "timestamp": 0.5032800754513973 + }, + { + "x": 1.0519833618359966, + "y": 3.77699034023748, + "heading": -0.6092267013635411, + "angularVelocity": 1.3018225720227254, + "velocityX": 1.2648153473148886, + "velocityY": -2.060293984633039, + "timestamp": 0.5592000838348858 + }, + { + "x": 1.129826908295346, + "y": 3.6501643798944907, + "heading": -0.5332724156478236, + "angularVelocity": 1.3582667083101592, + "velocityX": 1.3920517666147918, + "velocityY": -2.26798893650447, + "timestamp": 0.6151200922183744 + }, + { + "x": 1.214791954900265, + "y": 3.511704401985981, + "heading": -0.45632702327380104, + "angularVelocity": 1.3759903583409006, + "velocityX": 1.5194033238021893, + "velocityY": -2.4760364297333126, + "timestamp": 0.6710401006018629 + }, + { + "x": 1.306860167939052, + "y": 3.361616595640326, + "heading": -0.38215505844470404, + "angularVelocity": 1.3263940219829728, + "velocityX": 1.6464270249639614, + "velocityY": -2.6839732447173734, + "timestamp": 0.7269601089853515 + }, + { + "x": 1.4058792704184802, + "y": 3.2000739494751453, + "heading": -0.3184323584737964, + "angularVelocity": 1.1395330904442935, + "velocityX": 1.770727604337509, + "velocityY": -2.888816558419535, + "timestamp": 0.78288011736884 + }, + { + "x": 1.5121691709784744, + "y": 3.0300749520186443, + "heading": -0.28662541962876287, + "angularVelocity": 0.5687935278354703, + "velocityX": 1.9007490097476158, + "velocityY": -3.040038840671852, + "timestamp": 0.8388001257523285 + }, + { + "x": 1.6244650109400063, + "y": 2.8508645326617055, + "heading": -0.2866253125429205, + "angularVelocity": 0.0000019149825875479786, + "velocityX": 2.00815134345883, + "velocityY": -3.2047638141959642, + "timestamp": 0.8947201341358171 + }, + { + "x": 1.735288205529194, + "y": 2.670739688820617, + "heading": -0.28662527835532886, + "angularVelocity": 6.113659969296124e-7, + "velocityX": 1.9818164873864772, + "velocityY": -3.2211161809173365, + "timestamp": 0.9506401425193056 + }, + { + "x": 1.8506362591615513, + "y": 2.493478394875359, + "heading": -0.2866252441530595, + "angularVelocity": 6.116284727574992e-7, + "velocityX": 2.062733124811472, + "velocityY": -3.1699082147776885, + "timestamp": 1.0065601509027942 + }, + { + "x": 1.9771720781105007, + "y": 2.324022128559085, + "heading": -0.2866252095630549, + "angularVelocity": 6.185622213406466e-7, + "velocityX": 2.2628004288052175, + "velocityY": -3.0303333496335663, + "timestamp": 1.0624801592862827 + }, + { + "x": 2.1143701177449694, + "y": 2.1630766803808767, + "heading": -0.28662517392184667, + "angularVelocity": 6.373605666632096e-7, + "velocityX": 2.4534695827223683, + "velocityY": -2.878137053815794, + "timestamp": 1.1184001676697712 + }, + { + "x": 2.261659606829455, + "y": 2.011311910787294, + "heading": -0.2866251364727607, + "angularVelocity": 6.696902775127292e-7, + "velocityX": 2.633931813357441, + "velocityY": -2.7139618533818717, + "timestamp": 1.1743201760532598 + }, + { + "x": 2.418427669547749, + "y": 1.8693594092228303, + "heading": -0.2866250962912339, + "angularVelocity": 7.185536621676126e-7, + "velocityX": 2.803434177677683, + "velocityY": -2.538492136678184, + "timestamp": 1.2302401844367483 + }, + { + "x": 2.5840219554137547, + "y": 1.7378099138847216, + "heading": -0.28662505217416817, + "angularVelocity": 7.889316720181228e-7, + "velocityX": 2.961270762521925, + "velocityY": -2.3524584337678864, + "timestamp": 1.2861601928202369 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.34448076255255106, - "angularVelocity": 0.000001257711738653551, - "velocityX": 3.3509724640087613, - "velocityY": -2.575382967934733, - "timestamp": 1.3542872941730606 - }, - { - "x": 2.9607343532453942, - "y": 1.47791150359439, - "heading": -0.34448070266933767, - "angularVelocity": 0.0000010280378855043595, - "velocityX": 3.484651653320559, - "velocityY": -2.391405084272886, - "timestamp": 1.4125373005573685 - }, - { - "x": 3.170914415879318, - "y": 1.3497321947844976, - "heading": -0.344480652877576, - "angularVelocity": 8.547940977947146e-7, - "velocityX": 3.608241023138232, - "velocityY": -2.2005029143554395, - "timestamp": 1.4707873069416764 - }, - { - "x": 3.387684985247893, - "y": 1.2330440779434721, - "heading": -0.34448061027247806, - "angularVelocity": 7.314179104628634e-7, - "velocityX": 3.721382757255318, - "velocityY": -2.003229254108037, - "timestamp": 1.5290373133259842 - }, - { - "x": 3.6104184041129233, - "y": 1.1281850184409496, - "heading": -0.3444805729289611, - "angularVelocity": 6.41090349411582e-7, - "velocityX": 3.8237492609963484, - "velocityY": -1.8001553306399407, - "timestamp": 1.5872873197102921 - }, - { - "x": 3.8384697501167597, - "y": 1.0354586289592775, - "heading": -0.34448053951161883, - "angularVelocity": 5.736882162731552e-7, - "velocityX": 3.915044137493387, - "velocityY": -1.5918691728530348, - "timestamp": 1.6455373260946 - }, - { - "x": 4.071178703094687, - "y": 0.9551333894958018, - "heading": -0.3444805090563753, - "angularVelocity": 5.228367414213026e-7, - "velocityX": 3.99500304673988, - "velocityY": -1.3789739169044062, - "timestamp": 1.703787332478908 - }, - { - "x": 4.307871457008001, - "y": 0.8874418681856828, - "heading": -0.3444804808411649, - "angularVelocity": 4.843812402300459e-7, - "velocityX": 4.063394471611187, - "velocityY": -1.1620860753820388, - "timestamp": 1.7620373388632158 - }, - { - "x": 4.547862670970626, - "y": 0.8325800440403511, - "heading": -0.3444804543055099, - "angularVelocity": 4.5554767570145907e-7, - "velocityX": 4.120020388998225, - "velocityY": -0.9418337876802539, - "timestamp": 1.8202873452475237 - }, - { - "x": 4.7904574537733415, - "y": 0.7907067300496627, - "heading": -0.34448042899819054, - "angularVelocity": 4.344603701427382e-7, - "velocityX": 4.1647168448735075, - "velocityY": -0.7188550970179598, - "timestamp": 1.8785373516318316 - }, - { - "x": 5.03495337639213, - "y": 0.7619430850125104, - "heading": -0.3444804045415982, - "angularVelocity": 4.1985561744498176e-7, - "velocityX": 4.1973544347053195, - "velocityY": -0.49379642720350075, - "timestamp": 1.9367873580161394 - }, - { - "x": 5.280642508082443, - "y": 0.7463721419648577, - "heading": -0.344480380606166, - "angularVelocity": 4.109086623403158e-7, - "velocityX": 4.217838708366216, - "velocityY": -0.2673122977004072, - "timestamp": 1.9950373644004473 - }, - { - "x": 5.5268134912780935, - "y": 0.7440374377079143, - "heading": -0.3444803568901529, - "angularVelocity": 4.071418107502121e-7, - "velocityX": 4.2261108363065745, - "velocityY": -0.04008075538292679, - "timestamp": 2.053287370784755 - }, - { - "x": 5.766738904442569, - "y": 0.7458615462006624, - "heading": -0.3246781449807404, - "angularVelocity": 0.3399520985245257, - "velocityX": 4.118890761685976, - "velocityY": 0.03131516382527713, - "timestamp": 2.111537377169063 - }, - { - "x": 5.994439434226097, - "y": 0.7477938306057788, - "heading": -0.2833891296763232, - "angularVelocity": 0.7088242193830927, - "velocityX": 3.909021542096679, - "velocityY": 0.033172260829777846, - "timestamp": 2.1697873835533708 - }, - { - "x": 6.2089250785606716, - "y": 0.7495883833677881, - "heading": -0.23844348738786494, - "angularVelocity": 0.7715989246752472, - "velocityX": 3.682156580713365, - "velocityY": 0.030807769361768905, - "timestamp": 2.2280373899376786 - }, - { - "x": 6.410164853990332, - "y": 0.7512394694030761, - "heading": -0.19387042094561185, - "angularVelocity": 0.7652027735100942, - "velocityX": 3.4547597145649904, - "velocityY": 0.02834482153349081, - "timestamp": 2.2862873963219865 - }, - { - "x": 6.598169970341291, - "y": 0.7527437994565946, - "heading": -0.15150652534345024, - "angularVelocity": 0.7272770980085961, - "velocityX": 3.227555291763987, - "velocityY": 0.025825405813585094, - "timestamp": 2.3445374027062944 - }, - { - "x": 6.772954131152115, - "y": 0.7540995948883421, - "heading": -0.11241236663435353, - "angularVelocity": 0.6711442819623177, - "velocityX": 3.000586122818176, - "velocityY": 0.02327545550471737, - "timestamp": 2.4027874090906023 - }, - { - "x": 6.934529146268428, - "y": 0.7553058862941308, - "heading": -0.07728196459640024, - "angularVelocity": 0.6030969645939325, - "velocityX": 2.773819697981016, - "velocityY": 0.020708863065698472, - "timestamp": 2.46103741547491 - }, - { - "x": 7.082904710855205, - "y": 0.7563621191254241, - "heading": -0.046606183781962664, - "angularVelocity": 0.5266227888809539, - "velocityX": 2.5472197137260486, - "velocityY": 0.018132750481171057, - "timestamp": 2.519287421859218 - }, - { - "x": 7.2180887691200715, - "y": 0.7572679503200676, - "heading": -0.02075102525586853, - "angularVelocity": 0.44386533377375426, - "velocityX": 2.3207561107029173, - "velocityY": 0.01555074841824514, - "timestamp": 2.577537428243526 + "heading": -0.2866250016379229, + "angularVelocity": 9.037238488587988e-7, + "velocityX": 3.1067845266977008, + "velocityY": -2.1566350283233997, + "timestamp": 1.3420802012037254 + }, + { + "x": 2.964172022537395, + "y": 1.4952512181839386, + "heading": -0.28662495601958665, + "angularVelocity": 7.195928192095116e-7, + "velocityX": 3.256089340971223, + "velocityY": -1.9238160187086888, + "timestamp": 1.4054748531095118 + }, + { + "x": 3.1789518158723995, + "y": 1.3887033110695066, + "heading": -0.2866249156918973, + "angularVelocity": 6.361370893313497e-7, + "velocityX": 3.3879796935268014, + "velocityY": -1.6807081340674839, + "timestamp": 1.4688695050152982 + }, + { + "x": 3.4009440542760783, + "y": 1.2981369637223683, + "heading": -0.28662487900587236, + "angularVelocity": 5.786927430417384e-7, + "velocityX": 3.501750253847766, + "velocityY": -1.4286117933375984, + "timestamp": 1.5322641569210846 + }, + { + "x": 3.6289614625982565, + "y": 1.2240364953903842, + "heading": -0.2866248447654741, + "angularVelocity": 5.401149339588049e-7, + "velocityX": 3.596792496960864, + "velocityY": -1.168875703302355, + "timestamp": 1.595658808826871 + }, + { + "x": 3.8617845298582507, + "y": 1.1667980667810436, + "heading": -0.2866248120410035, + "angularVelocity": 5.162023862741612e-7, + "velocityX": 3.672597928386812, + "velocityY": -0.9028904945231835, + "timestamp": 1.6590534607326575 + }, + { + "x": 4.0981679647792335, + "y": 1.1267270015307347, + "heading": -0.28662478005254766, + "angularVelocity": 5.045923416827531e-7, + "velocityX": 3.728759884544863, + "velocityY": -0.6320890492444092, + "timestamp": 1.7224481126384439 + }, + { + "x": 4.3358126512762265, + "y": 1.0949807955483843, + "heading": -0.28662474820722456, + "angularVelocity": 5.023345368252209e-7, + "velocityX": 3.7486551207847545, + "velocityY": -0.5007710434238176, + "timestamp": 1.7858427645442303 + }, + { + "x": 4.573457419312962, + "y": 1.06323519995946, + "heading": -0.28662471636190573, + "angularVelocity": 5.023344693474633e-7, + "velocityX": 3.7486564070090496, + "velocityY": -0.5007614149550472, + "timestamp": 1.8492374164500167 + }, + { + "x": 4.811102187354235, + "y": 1.0314896044045172, + "heading": -0.28662468451658685, + "angularVelocity": 5.02334469511756e-7, + "velocityX": 3.748656407080655, + "velocityY": -0.5007614144190154, + "timestamp": 1.9126320683558031 + }, + { + "x": 5.04874695539551, + "y": 0.9997440088495764, + "heading": -0.286624652671268, + "angularVelocity": 5.023344701827313e-7, + "velocityX": 3.748656407080659, + "velocityY": -0.5007614144189856, + "timestamp": 1.9760267202615895 + }, + { + "x": 5.286391723436784, + "y": 0.9679984132946354, + "heading": -0.2866246208259492, + "angularVelocity": 5.023344694262712e-7, + "velocityX": 3.7486564070806585, + "velocityY": -0.500761414418986, + "timestamp": 2.039421372167376 + }, + { + "x": 5.524036491478058, + "y": 0.9362528177396949, + "heading": -0.28662458898063026, + "angularVelocity": 5.023344704301764e-7, + "velocityX": 3.74865640708066, + "velocityY": -0.5007614144189806, + "timestamp": 2.1028160240731624 + }, + { + "x": 5.761681259520114, + "y": 0.9045072221906024, + "heading": -0.2866245571353113, + "angularVelocity": 5.02334470827004e-7, + "velocityX": 3.748656407092983, + "velocityY": -0.5007614143267314, + "timestamp": 2.166210675978949 + }, + { + "x": 5.99932604159473, + "y": 0.8727617316882137, + "heading": -0.28662452528999255, + "angularVelocity": 5.023344683092169e-7, + "velocityX": 3.748656628445413, + "velocityY": -0.5007597572988834, + "timestamp": 2.229605327884735 + }, + { + "x": 6.237212534637086, + "y": 0.8428813906316717, + "heading": -0.286624491558187, + "angularVelocity": 5.320922916664829e-7, + "velocityX": 3.7524694259050357, + "velocityY": -0.47133851450037906, + "timestamp": 2.2929999797905216 + }, + { + "x": 6.4638245505635705, + "y": 0.8220107849523657, + "heading": -0.24424436180847475, + "angularVelocity": 0.6685126974542769, + "velocityX": 3.5746235544169, + "velocityY": -0.32921713507194117, + "timestamp": 2.356394631696308 + }, + { + "x": 6.673647733933235, + "y": 0.8041360764949451, + "heading": -0.18514255243378552, + "angularVelocity": 0.9322838377994884, + "velocityX": 3.309793130207448, + "velocityY": -0.2819592492436893, + "timestamp": 2.4197892836020944 + }, + { + "x": 6.866141134149738, + "y": 0.7888964331390943, + "heading": -0.12766912670980685, + "angularVelocity": 0.9065973863125305, + "velocityX": 3.0364296424022634, + "velocityY": -0.24039320191392657, + "timestamp": 2.483183935507881 + }, + { + "x": 7.04134978279503, + "y": 0.7761674362974819, + "heading": -0.07636429138531158, + "angularVelocity": 0.8092927996630003, + "velocityX": 2.7637764918352543, + "velocityY": -0.20078975842520036, + "timestamp": 2.5465785874136673 + }, + { + "x": 7.19932046714385, + "y": 0.765888430525111, + "heading": -0.03338684229285238, + "angularVelocity": 0.6779349330023905, + "velocityX": 2.4918613731579065, + "velocityY": -0.16214310613530766, + "timestamp": 2.6099732393194537 }, { "x": 7.340087890625, "y": 0.7580231428146362, "heading": 0, - "angularVelocity": 0.35624073788013705, - "velocityX": 2.094405289847217, - "velocityY": 0.012964676597393893, - "timestamp": 2.635787434627834 - }, - { - "x": 7.452016076049052, - "y": 0.7586421586100276, - "heading": 0.015744593535721572, - "angularVelocity": 0.2617476388383187, - "velocityX": 1.8607618029473847, - "velocityY": 0.010290892710547365, - "timestamp": 2.6959392419396355 - }, - { - "x": 7.549878174356822, - "y": 0.7590935431078216, - "heading": 0.02631444920223928, - "angularVelocity": 0.17571966893244026, - "velocityX": 1.6269186693010698, - "velocityY": 0.007504088704338438, - "timestamp": 2.756091049251437 - }, - { - "x": 7.633665269428441, - "y": 0.7593717238160655, - "heading": 0.03212207455289435, - "angularVelocity": 0.09654947390941697, - "velocityX": 1.3929273086892113, - "velocityY": 0.004624644223938123, - "timestamp": 2.816242856563239 - }, - { - "x": 7.703370541137993, - "y": 0.7594720412293507, - "heading": 0.03350861659317102, - "angularVelocity": 0.023050712891957155, - "velocityX": 1.158822566182069, - "velocityY": 0.0016677373094563682, - "timestamp": 2.8763946638750406 - }, - { - "x": 7.758988643067764, - "y": 0.759390536759096, - "heading": 0.030761021907964082, - "angularVelocity": -0.04567767467009867, - "velocityX": 0.9246289415955516, - "velocityY": -0.001354979574134501, - "timestamp": 2.9365464711868423 - }, - { - "x": 7.800515297323124, - "y": 0.759123802858703, - "heading": 0.024124062565761484, - "angularVelocity": -0.11033682342742264, - "velocityX": 0.6903641987031875, - "velocityY": -0.004434345571870156, - "timestamp": 2.996698278498644 - }, - { - "x": 7.827947021203066, - "y": 0.7586688736183343, - "heading": 0.013809003674597087, - "angularVelocity": -0.17148377334192952, - "velocityX": 0.4560415572843554, - "velocityY": -0.007563018647310543, - "timestamp": 3.0568500858104457 + "angularVelocity": 0.5266507708326882, + "velocityX": 2.220493673351972, + "velocityY": -0.12406863156475333, + "timestamp": 2.67336789122524 + }, + { + "x": 7.464023522460179, + "y": 0.7525372143128384, + "heading": 0.023010119360387892, + "angularVelocity": 0.36178963616626764, + "velocityX": 1.9486481772395672, + "velocityY": -0.08625561847872552, + "timestamp": 2.736968716290925 + }, + { + "x": 7.57058231953554, + "y": 0.7491648354718851, + "heading": 0.03698370022131657, + "angularVelocity": 0.21970754068830903, + "velocityX": 1.6754310492876554, + "velocityY": -0.05302413667543652, + "timestamp": 2.8005695413566096 + }, + { + "x": 7.659708944521439, + "y": 0.7476928551810322, + "heading": 0.04295335113137195, + "angularVelocity": 0.09386121805637215, + "velocityX": 1.4013438488235506, + "velocityY": -0.02314404395434409, + "timestamp": 2.8641703664222944 + }, + { + "x": 7.731365798341117, + "y": 0.7479588271083956, + "heading": 0.041693436231508264, + "angularVelocity": -0.019809725716020507, + "velocityX": 1.1266654755763426, + "velocityY": 0.004181894292859409, + "timestamp": 2.927771191487979 + }, + { + "x": 7.785525986892143, + "y": 0.7498349370538775, + "heading": 0.03380693253041854, + "angularVelocity": -0.1240000219013676, + "velocityX": 0.8515642445690118, + "velocityY": 0.029498201376229376, + "timestamp": 2.991372016553664 + }, + { + "x": 7.822169492500932, + "y": 0.7532180353995639, + "heading": 0.01977708435408761, + "angularVelocity": -0.22059223542841888, + "velocityX": 0.5761482743493486, + "velocityY": 0.05319268016086978, + "timestamp": 3.0549728416193487 }, { "x": 7.841280937194824, "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": -0.22956922313268238, - "velocityX": 0.2216710783541506, - "velocityY": -0.010735019154968724, - "timestamp": 3.1170018931222474 - }, - { - "x": 7.835826711308599, - "y": 0.7569095792763894, - "heading": -0.022558535214212153, - "angularVelocity": -0.29904118534160484, - "velocityX": -0.07230248589501269, - "velocityY": -0.014761657052128235, - "timestamp": 3.19243810833268 - }, - { - "x": 7.808194016867113, - "y": 0.7554921769260273, - "heading": -0.05020545199265331, - "angularVelocity": -0.366493953883011, - "velocityX": -0.3663054192790896, - "velocityY": -0.018789414956835414, - "timestamp": 3.2678743235431122 - }, - { - "x": 7.758380379628707, - "y": 0.7537708581865793, - "heading": -0.082765542477478, - "angularVelocity": -0.4316241263429897, - "velocityX": -0.6603411517856365, - "velocityY": -0.022818201239900495, - "timestamp": 3.3433105387535447 - }, - { - "x": 7.686383023957851, - "y": 0.7517455702312359, - "heading": -0.1200350345811684, - "angularVelocity": -0.4940530486547548, - "velocityX": -0.9544136787617978, - "velocityY": -0.026847687807424973, - "timestamp": 3.418746753963977 - }, - { - "x": 7.5921988218888945, - "y": 0.7494163084029398, - "heading": -0.1617736418535849, - "angularVelocity": -0.5532966779415547, - "velocityX": -1.2485276707775705, - "velocityY": -0.030877236109982744, - "timestamp": 3.4941829691744095 - }, - { - "x": 7.475824234031671, - "y": 0.7467831468197799, - "heading": -0.2076930912246962, - "angularVelocity": -0.6087188924181447, - "velocityX": -1.5426885817719311, - "velocityY": -0.03490580188593299, - "timestamp": 3.569619184384842 - }, - { - "x": 7.337255246576761, - "y": 0.7438462777509637, - "heading": -0.25744001302414304, - "angularVelocity": -0.659456756422306, - "velocityX": -1.8369027007567336, - "velocityY": -0.03893181889658235, - "timestamp": 3.6450553995952744 - }, - { - "x": 7.17648732101356, - "y": 0.7406060606730164, - "heading": -0.3105694414767273, - "angularVelocity": -0.7042960507016083, - "velocityX": -2.1311769832928764, - "velocityY": -0.04295307060287463, - "timestamp": 3.720491614805707 - }, - { - "x": 6.9935154095251395, - "y": 0.7370630794965956, - "heading": -0.3665018755155811, - "angularVelocity": -0.7414533441640445, - "velocityX": -2.425518180863289, - "velocityY": -0.04696658185378862, - "timestamp": 3.795927830016139 - }, - { - "x": 6.788334200415596, - "y": 0.7332181989405221, - "heading": -0.4244496104660046, - "angularVelocity": -0.7681686413982437, - "velocityX": -2.719929791509048, - "velocityY": -0.050968630191055964, - "timestamp": 3.8713640452265716 - }, - { - "x": 6.5609391315868795, - "y": 0.7290725862923455, - "heading": -0.4832803161020974, - "angularVelocity": -0.7798735060074524, - "velocityX": -3.0144018783867756, - "velocityY": -0.05495520469329157, - "timestamp": 3.946800260437004 - }, - { - "x": 6.311330163631764, - "y": 0.7246275712093064, - "heading": -0.5412355988933977, - "angularVelocity": -0.7682686973310038, - "velocityX": -3.3088744876558285, - "velocityY": -0.05892415295013936, - "timestamp": 4.0222364756474365 - }, - { - "x": 6.039527438794169, - "y": 0.7198837950524889, - "heading": -0.5952481631436163, - "angularVelocity": -0.7160031040733982, - "velocityX": -3.603080086658513, - "velocityY": -0.06288459917540226, - "timestamp": 4.097672690857869 - }, - { - "x": 5.745659598268619, - "y": 0.7148372711288513, - "heading": -0.638783130826649, - "angularVelocity": -0.5771096490139386, - "velocityX": -3.8955803880906834, - "velocityY": -0.06689789393012663, - "timestamp": 4.173108906068301 - }, - { - "x": 5.431003009987887, - "y": 0.7096634209299225, - "heading": -0.6507371973275397, - "angularVelocity": -0.15846588362823302, - "velocityX": -4.171160859581617, - "velocityY": -0.06858576062566446, - "timestamp": 4.248545121278734 - }, - { - "x": 5.112561537797849, - "y": 0.7251155992021675, - "heading": -0.6507372121831845, - "angularVelocity": -1.969298784568896e-7, - "velocityX": -4.221334160280071, - "velocityY": 0.20483766622093208, - "timestamp": 4.323981336489166 - }, - { - "x": 4.795969323317984, - "y": 0.7627069594521951, - "heading": -0.6507372273638599, - "angularVelocity": -2.0123856138904834e-7, - "velocityX": -4.196819970311576, - "velocityY": 0.49831980760388084, - "timestamp": 4.399417551699599 - }, - { - "x": 4.48276376848833, - "y": 0.8222554301685094, - "heading": -0.6507372432084315, - "angularVelocity": -2.1003932248991623e-7, - "velocityX": -4.15192562293793, - "velocityY": 0.789388366717515, - "timestamp": 4.474853766910031 - }, - { - "x": 4.174465835671775, - "y": 0.9034719526361538, - "heading": -0.6507372601213973, - "angularVelocity": -2.242022056467609e-7, - "velocityX": -4.0868690450143115, - "velocityY": 1.076625096329228, - "timestamp": 4.550289982120463 - }, - { - "x": 3.872572656476166, - "y": 1.0059621755922359, - "heading": -0.650737278622818, - "angularVelocity": -2.4525913278926714e-7, - "velocityX": -4.001966142567798, - "velocityY": 1.3586342139538858, - "timestamp": 4.625726197330896 - }, - { - "x": 3.5785502607652493, - "y": 1.1292284170855964, - "heading": -0.6507372994260644, - "angularVelocity": -2.7577266985991995e-7, - "velocityX": -3.8976292075461214, - "velocityY": 1.6340459439740451, - "timestamp": 4.701162412541328 - }, - { - "x": 3.2938264573018237, - "y": 1.2726720943591394, - "heading": -0.6507373235725763, - "angularVelocity": -3.200917719298211e-7, - "velocityX": -3.7743649077459236, - "velocityY": 1.9015227218571442, - "timestamp": 4.776598627751761 - }, - { - "x": 3.0197839001768125, - "y": 1.4355966353904746, - "heading": -0.6507373526846939, - "angularVelocity": -3.859169970324026e-7, - "velocityX": -3.6327718239914044, - "velocityY": 2.1597655791299997, - "timestamp": 4.852034842962193 + "heading": 2.046055081284657e-30, + "angularVelocity": -0.310956411865142, + "velocityX": 0.30049051524338544, + "velocityY": 0.07555102327854848, + "timestamp": 3.1185736666850334 + }, + { + "x": 7.837626456422163, + "y": 0.7661832239405314, + "heading": -0.033360531548023535, + "angularVelocity": -0.41786111882102345, + "velocityX": -0.045774613098587845, + "velocityY": 0.10221002096529477, + "timestamp": 3.1984100765480727 + }, + { + "x": 7.806322741144082, + "y": 0.7764707746744537, + "heading": -0.07501113915617402, + "angularVelocity": -0.5216994060680163, + "velocityX": -0.3920982335225698, + "velocityY": 0.12885788265743264, + "timestamp": 3.278246486411112 + }, + { + "x": 7.74736425619444, + "y": 0.7888847549403224, + "heading": -0.12465356813987538, + "angularVelocity": -0.6218018704606533, + "velocityX": -0.738491185297363, + "velocityY": 0.15549271675874554, + "timestamp": 3.3580828962741514 + }, + { + "x": 7.660744366778727, + "y": 0.8034239689194149, + "heading": -0.18191677738707485, + "angularVelocity": -0.7172568173523243, + "velocityX": -1.084967241942757, + "velocityY": 0.18211257249711932, + "timestamp": 3.4379193061371907 + }, + { + "x": 7.5464550145644145, + "y": 0.8200870575241468, + "heading": -0.24632578527266594, + "angularVelocity": -0.8067623280666757, + "velocityX": -1.4315442341455176, + "velocityY": 0.20871540482992282, + "timestamp": 3.51775571600023 + }, + { + "x": 7.404486266909035, + "y": 0.8388724767291542, + "heading": -0.3172478637527482, + "angularVelocity": -0.8883425319569115, + "velocityX": -1.7782456387872365, + "velocityY": 0.23529889729803788, + "timestamp": 3.5975921258632693 + }, + { + "x": 7.234825726560123, + "y": 0.8597784078771571, + "heading": -0.3937913075224082, + "angularVelocity": -0.958753579989022, + "velocityX": -2.1251023266197273, + "velocityY": 0.2618596099682753, + "timestamp": 3.6774285357263086 + }, + { + "x": 7.037458048630302, + "y": 0.8828023770067691, + "heading": -0.474592846440749, + "angularVelocity": -1.0120888333650895, + "velocityX": -2.472151218578179, + "velocityY": 0.28838933475478196, + "timestamp": 3.757264945589348 + }, + { + "x": 6.812366694532574, + "y": 0.9079395818644894, + "heading": -0.5572950195818104, + "angularVelocity": -1.0358954427301792, + "velocityX": -2.8194072664825045, + "velocityY": 0.3148589083707988, + "timestamp": 3.837101355452387 + }, + { + "x": 6.559557151686243, + "y": 0.9351738572850871, + "heading": -0.6368741954392075, + "angularVelocity": -0.996777986308708, + "velocityX": -3.1665945810943685, + "velocityY": 0.3411260033776375, + "timestamp": 3.9169377653154265 + }, + { + "x": 6.2794552371906684, + "y": 0.9644007497338529, + "heading": -0.6963848347023115, + "angularVelocity": -0.7454072567290441, + "velocityX": -3.5084482753682624, + "velocityY": 0.3660847537972336, + "timestamp": 3.996774175178466 + }, + { + "x": 5.978414020607044, + "y": 0.9876514129078968, + "heading": -0.6963848502753711, + "angularVelocity": -1.950621226343756e-7, + "velocityX": -3.770725876828189, + "velocityY": 0.2912288167006853, + "timestamp": 4.0766105850415055 + }, + { + "x": 5.677372687056552, + "y": 1.010900561597095, + "heading": -0.6963848658464019, + "angularVelocity": -1.9503671222417687e-7, + "velocityX": -3.770727341909935, + "velocityY": 0.29120984684910667, + "timestamp": 4.156446994904545 + }, + { + "x": 5.376331353502739, + "y": 1.034149710243266, + "heading": -0.6963848814174328, + "angularVelocity": -1.9503671215554748e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.2912098463101618, + "timestamp": 4.236283404767585 + }, + { + "x": 5.075290019948924, + "y": 1.0573988588894354, + "heading": -0.6963848969884637, + "angularVelocity": -1.9503671305455908e-7, + "velocityX": -3.770727341951559, + "velocityY": 0.2912098463101465, + "timestamp": 4.316119814630625 + }, + { + "x": 4.774248686395111, + "y": 1.0806480075356062, + "heading": -0.6963849125594946, + "angularVelocity": -1.95036712978891e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.29120984631016333, + "timestamp": 4.3959562244936645 + }, + { + "x": 4.473207352844907, + "y": 1.1038971562285251, + "heading": -0.6963849281305255, + "angularVelocity": -1.9503671242042344e-7, + "velocityX": -3.770727341906336, + "velocityY": 0.29120984689571133, + "timestamp": 4.475792634356704 + }, + { + "x": 4.172166146375442, + "y": 1.1271479503662634, + "heading": -0.6963849437015556, + "angularVelocity": -1.9503670063864683e-7, + "velocityX": -3.770725750142127, + "velocityY": 0.2912304571012814, + "timestamp": 4.555629044219744 + }, + { + "x": 3.8735860999949274, + "y": 1.1720518831992321, + "heading": -0.696384959370356, + "angularVelocity": -1.9626133629188135e-7, + "velocityX": -3.7398982105123864, + "velocityY": 0.5624492998871312, + "timestamp": 4.635465454082784 + }, + { + "x": 3.580403442801602, + "y": 1.244234542261993, + "heading": -0.6963849756495779, + "angularVelocity": -2.0390723854783083e-7, + "velocityX": -3.672292600535076, + "velocityY": 0.9041320769131684, + "timestamp": 4.7153018639458235 + }, + { + "x": 3.295105029858622, + "y": 1.3430840304472003, + "heading": -0.6963849932202171, + "angularVelocity": -2.2008303364523734e-7, + "velocityX": -3.573537605616451, + "velocityY": 1.2381504673717785, + "timestamp": 4.795138273808863 + }, + { + "x": 3.020110821637561, + "y": 1.4677619739384067, + "heading": -0.6963850130039023, + "angularVelocity": -2.4780279138256264e-7, + "velocityX": -3.4444711215448884, + "velocityY": 1.5616677115753714, + "timestamp": 4.874974683671903 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.6507373865448278, - "angularVelocity": -4.4885780393316453e-7, - "velocityX": -3.473537574141072, - "velocityY": 2.407520434630195, - "timestamp": 4.9274710581726255 - }, - { - "x": 2.569555685248769, - "y": 1.762694432110511, - "heading": -0.6507374202071338, - "angularVelocity": -5.980782803410117e-7, - "velocityX": -3.3437088024696617, - "velocityY": 2.584806921874972, - "timestamp": 4.983755172029852 - }, - { - "x": 2.389174039276651, - "y": 1.9177631325165114, - "heading": -0.6507374491092287, - "angularVelocity": -5.135035974859511e-7, - "velocityX": -3.2048411818241203, - "velocityY": 2.755106010896001, - "timestamp": 5.040039285887079 - }, - { - "x": 2.2170960677982245, - "y": 2.081997759940487, - "heading": -0.6507374803842504, - "angularVelocity": -5.556633928977291e-7, - "velocityX": -3.0573097750979614, - "velocityY": 2.917957060505229, - "timestamp": 5.096323399744305 - }, - { - "x": 2.0538462988954582, - "y": 2.2548939722714945, - "heading": -0.6510332999359818, - "angularVelocity": -0.005255826759246614, - "velocityX": -2.9004590765499803, - "velocityY": 3.0718474624933285, - "timestamp": 5.152607513601532 - }, - { - "x": 1.9011340137811743, - "y": 2.4339552115414294, - "heading": -0.6596386590017012, - "angularVelocity": -0.15289143731654284, - "velocityX": -2.713239574165845, - "velocityY": 3.181381512448628, - "timestamp": 5.208891627458758 - }, - { - "x": 1.7604827039783657, - "y": 2.6146352839408076, - "heading": -0.6772772556341842, - "angularVelocity": -0.3133849930946056, - "velocityX": -2.498952193856903, - "velocityY": 3.2101433249477958, - "timestamp": 5.265175741315985 - }, - { - "x": 1.631754490870844, - "y": 2.792388921478468, - "heading": -0.7009679668217429, - "angularVelocity": -0.42091292842690614, - "velocityX": -2.2871145032870293, - "velocityY": 3.158149349007424, - "timestamp": 5.321459855173211 - }, - { - "x": 1.513917799766492, - "y": 2.964350328382096, - "heading": -0.7282264856039945, - "angularVelocity": -0.4843021754130653, - "velocityX": -2.093604803004671, - "velocityY": 3.0552387719887317, - "timestamp": 5.377743969030438 - }, - { - "x": 1.4059399900768588, - "y": 3.128858674851357, - "heading": -0.757359284956742, - "angularVelocity": -0.5176025232741039, - "velocityX": -1.9184420307928356, - "velocityY": 2.92282022750792, - "timestamp": 5.434028082887664 - }, - { - "x": 1.3069898193149363, - "y": 3.2849152261509276, - "heading": -0.787231438028103, - "angularVelocity": -0.5307386227512847, - "velocityX": -1.7580479460496543, - "velocityY": 2.77265716033891, - "timestamp": 5.490312196744891 - }, - { - "x": 1.2164244831734237, - "y": 3.431880035528415, - "heading": -0.817049536411403, - "angularVelocity": -0.5297782329653102, - "velocityX": -1.6090745671371067, - "velocityY": 2.611124157524907, - "timestamp": 5.546596310602117 - }, - { - "x": 1.133744535036132, - "y": 3.5693179916241067, - "heading": -0.8462342058218654, - "angularVelocity": -0.5185240987269294, - "velocityX": -1.4689748575774313, - "velocityY": 2.441860530030267, - "timestamp": 5.602880424459344 - }, - { - "x": 1.058555464808831, - "y": 3.696918257435487, - "heading": -0.8743465993903727, - "angularVelocity": -0.4994729710024167, - "velocityX": -1.3358844099070253, - "velocityY": 2.267074260688507, - "timestamp": 5.6591645383165705 - }, - { - "x": 0.9905398311066821, - "y": 3.8144496656298092, - "heading": -0.9010441658077244, - "angularVelocity": -0.47433573325990525, - "velocityX": -1.2084339441619472, - "velocityY": 2.088180840733473, - "timestamp": 5.715448652173797 - }, - { - "x": 0.9294376821542455, - "y": 3.921734581554844, - "heading": -0.9260527839372265, - "angularVelocity": -0.44432818455559475, - "velocityX": -1.0856020422997397, - "velocityY": 1.9061313854417152, - "timestamp": 5.771732766031024 - }, - { - "x": 0.8750327547294673, - "y": 4.018632790760157, - "heading": -0.9491484821020626, - "angularVelocity": -0.4103413304759842, - "velocityX": -0.9666124896766538, - "velocityY": 1.721590739637662, - "timestamp": 5.82801687988825 - }, - { - "x": 0.8271426027720992, - "y": 4.105031120160407, - "heading": -0.9701450202727028, - "angularVelocity": -0.37304554929835076, - "velocityX": -0.8508644566893013, - "velocityY": 1.5350393473265314, - "timestamp": 5.884300993745477 - }, - { - "x": 0.7856114088806271, - "y": 4.180836495357623, - "heading": -0.9888851979118991, - "angularVelocity": -0.33295678575901055, - "velocityX": -0.7378848318874176, - "velocityY": 1.3468343019401061, - "timestamp": 5.940585107602703 - }, - { - "x": 0.750304653010895, - "y": 4.2459711432493465, - "heading": -1.0052346087320225, - "angularVelocity": -0.2904800253513115, - "velocityX": -0.6272952250664012, - "velocityY": 1.1572474616362955, - "timestamp": 5.99686922145993 - }, - { - "x": 0.7211050890293871, - "y": 4.300369182693025, - "heading": -1.0190770490522025, - "angularVelocity": -0.24593867383776238, - "velocityX": -0.5187887306101521, - "velocityY": 0.9664901108981995, - "timestamp": 6.053153335317156 - }, - { - "x": 0.6979096587548851, - "y": 4.343974141765046, - "heading": -1.0303110716490886, - "angularVelocity": -0.19959490923820775, - "velocityX": -0.41211327113260987, - "velocityY": 0.7747294233437039, - "timestamp": 6.109437449174383 - }, - { - "x": 0.6806270896717198, - "y": 4.3767371107575626, - "heading": -1.0388473503636761, - "angularVelocity": -0.15166408653498495, - "velocityX": -0.3070594506827464, - "velocityY": 0.5820997568803311, - "timestamp": 6.165721563031609 - }, - { - "x": 0.6691759993701499, - "y": 4.398615342035701, - "heading": -1.0446066293483676, - "angularVelocity": -0.10232512497755435, - "velocityX": -0.20345155172234425, - "velocityY": 0.3887105930749016, - "timestamp": 6.222005676888836 + "heading": -0.6963850355061774, + "angularVelocity": -2.8185479639389566e-7, + "velocityX": -3.2861879673103584, + "velocityY": 1.8719390230438342, + "timestamp": 4.954811093534943 + }, + { + "x": 2.5741841985169076, + "y": 1.73878733799422, + "heading": -0.6963850589279512, + "angularVelocity": -4.0231077616908087e-7, + "velocityX": -3.153128257888759, + "velocityY": 2.0882929565548642, + "timestamp": 5.0130292054104135 + }, + { + "x": 2.3991894720533877, + "y": 1.872411165939459, + "heading": -0.6963850794155536, + "angularVelocity": -3.519111451084776e-7, + "velocityX": -3.0058468202788267, + "velocityY": 2.2952277846293376, + "timestamp": 5.071247317285884 + }, + { + "x": 2.2335584908852413, + "y": 2.017479635272907, + "heading": -0.6963850978369928, + "angularVelocity": -3.164211012595542e-7, + "velocityX": -2.845007779064253, + "velocityY": 2.491809930967045, + "timestamp": 5.129465429161355 + }, + { + "x": 2.0780383093475274, + "y": 2.173338406648706, + "heading": -0.6963851148088384, + "angularVelocity": -2.9152174806156023e-7, + "velocityX": -2.6713367460348776, + "velocityY": 2.6771526309404154, + "timestamp": 5.187683541036826 + }, + { + "x": 1.9333303562340614, + "y": 2.339284459115452, + "heading": -0.6963851307944149, + "angularVelocity": -2.745808111537906e-7, + "velocityX": -2.4856174213103777, + "velocityY": 2.850419691069807, + "timestamp": 5.245901652912297 + }, + { + "x": 1.8000872249814166, + "y": 2.514569234135199, + "heading": -0.6963851461633945, + "angularVelocity": -2.639896616028141e-7, + "velocityX": -2.288688639330214, + "velocityY": 3.0108289220145816, + "timestamp": 5.304119764787767 + }, + { + "x": 1.6789094155739503, + "y": 2.6984018293834224, + "heading": -0.6963851612308567, + "angularVelocity": -2.588105615203023e-7, + "velocityX": -2.0814451981312545, + "velocityY": 3.1576529936498847, + "timestamp": 5.362337876663238 + }, + { + "x": 1.5665736982883989, + "y": 2.8877670089616614, + "heading": -0.6963851762457395, + "angularVelocity": -2.5790741626665006e-7, + "velocityX": -1.9295664814042677, + "velocityY": 3.2526850060560912, + "timestamp": 5.420555988538709 + }, + { + "x": 1.4540456075762054, + "y": 3.0767463777174253, + "heading": -0.6971250426595835, + "angularVelocity": -0.012708526436355243, + "velocityX": -1.9328708384238367, + "velocityY": 3.246058016446739, + "timestamp": 5.47877410041418 + }, + { + "x": 1.3486317039814464, + "y": 3.2544802087721734, + "heading": -0.7253513181524348, + "angularVelocity": -0.48483666995638414, + "velocityX": -1.8106719747325546, + "velocityY": 3.052895831368142, + "timestamp": 5.53699221228965 + }, + { + "x": 1.2507109072486164, + "y": 3.4195717896549707, + "heading": -0.7601787682651484, + "angularVelocity": -0.598223628193407, + "velocityX": -1.6819644879978937, + "velocityY": 2.8357426162485435, + "timestamp": 5.595210324165121 + }, + { + "x": 1.160328320301069, + "y": 3.57195134860905, + "heading": -0.7969704460064531, + "angularVelocity": -0.6319627441714823, + "velocityX": -1.5524822780387875, + "velocityY": 2.617390946652855, + "timestamp": 5.653428436040592 + }, + { + "x": 1.0774888591394964, + "y": 3.71161238889488, + "heading": -0.8336344675120992, + "angularVelocity": -0.6297700204374731, + "velocityX": -1.422915626991949, + "velocityY": 2.398927683957996, + "timestamp": 5.711646547916063 + }, + { + "x": 1.0021908033919844, + "y": 3.838558158350536, + "heading": -0.8689739346571361, + "angularVelocity": -0.6070184347549549, + "velocityX": -1.2933785264038766, + "velocityY": 2.1805202086799973, + "timestamp": 5.7698646597915335 + }, + { + "x": 0.9344313462846833, + "y": 3.952793536595294, + "heading": -0.9022118432658017, + "angularVelocity": -0.5709204152783613, + "velocityX": -1.1638896371672067, + "velocityY": 1.9621965495739466, + "timestamp": 5.828082771667004 + }, + { + "x": 0.8742077702690397, + "y": 4.05432328906152, + "heading": -0.932802055793642, + "angularVelocity": -0.5254415085338586, + "velocityX": -1.0344474266781911, + "velocityY": 1.74395474527581, + "timestamp": 5.886300883542475 + }, + { + "x": 0.8215176865940929, + "y": 4.143151679881797, + "heading": -0.9603390876580661, + "angularVelocity": -0.47299768022924404, + "velocityX": -0.9050462472512313, + "velocityY": 1.5257861850670003, + "timestamp": 5.944518995417946 + }, + { + "x": 0.7763590444591775, + "y": 4.21928243186123, + "heading": -0.9845096003709082, + "angularVelocity": -0.4151717040316119, + "velocityX": -0.7756802939866984, + "velocityY": 1.307681570681621, + "timestamp": 6.002737107293417 + }, + { + "x": 0.7387300854642734, + "y": 4.282718776761074, + "heading": -1.0050639989905084, + "angularVelocity": -0.35305848914451915, + "velocityX": -0.6463445443815323, + "velocityY": 1.0896324675649904, + "timestamp": 6.060955219168887 + }, + { + "x": 0.7086292911290722, + "y": 4.333463522832413, + "heading": -1.0217986916878445, + "angularVelocity": -0.28744822115035373, + "velocityX": -0.5170348773863939, + "velocityY": 0.871631601173907, + "timestamp": 6.119173331044358 + }, + { + "x": 0.6860553360496396, + "y": 4.371519118466469, + "heading": -1.0345444282293665, + "angularVelocity": -0.21893077825652071, + "velocityX": -0.3877479765698806, + "velocityY": 0.6536727902728674, + "timestamp": 6.177391442919829 + }, + { + "x": 0.6710070489868055, + "y": 4.396887706764773, + "heading": -1.0431583071392123, + "angularVelocity": -0.14795874741302198, + "velocityX": -0.2584811938769611, + "velocityY": 0.43575079096636016, + "timestamp": 6.2356095547953 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -0.05172812017100072, - "velocityX": -0.10114076084111032, - "velocityY": 0.19465223880001714, - "timestamp": 6.278289790746062 + "angularVelocity": -0.07488723824993972, + "velocityX": -0.12923242394972312, + "velocityY": 0.2178611369128848, + "timestamp": 6.29382766667077 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": 6.011381134786383e-35, - "velocityY": 3.397969437810217e-36, - "timestamp": 6.334573904603289 + "angularVelocity": -7.364462273467936e-32, + "velocityX": -8.426727085890477e-35, + "velocityY": -7.327805300908389e-34, + "timestamp": 6.352045778546241 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubGSub.2.traj b/src/main/deploy/choreo/SourceLanePSubHSubGSub.2.traj index 4c083b8f..88eac2d5 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubGSub.2.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubGSub.2.traj @@ -4,973 +4,928 @@ "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": 6.011381134786383e-35, - "velocityY": 3.397969437810217e-36, + "angularVelocity": -7.364462273467936e-32, + "velocityX": -8.426727085890477e-35, + "velocityY": -7.327805300908389e-34, "timestamp": 0 }, { - "x": 0.6659136967977343, - "y": 4.396697343009468, - "heading": -1.044698875889832, - "angularVelocity": 0.048632422759591634, - "velocityX": 0.04192362717597145, - "velocityY": -0.22207715461221028, - "timestamp": 0.057970068194980406 - }, - { - "x": 0.6710310256277597, - "y": 4.371001166581999, - "heading": -1.0390353111823831, - "angularVelocity": 0.09769808599154256, - "velocityX": 0.08827536329288825, - "velocityY": -0.4432662790914817, - "timestamp": 0.11594013638996081 - }, - { - "x": 0.6791263180734669, - "y": 4.3325474770670445, - "heading": -1.0304995454316488, - "angularVelocity": 0.14724436276363576, - "velocityX": 0.13964607422021558, - "velocityY": -0.6633369722046352, - "timestamp": 0.17391020458494122 - }, - { - "x": 0.6905315555019766, - "y": 4.281418801002382, - "heading": -1.0190608629623887, - "angularVelocity": 0.1973204935827632, - "velocityX": 0.19674355721212186, - "velocityY": -0.8819840593026762, - "timestamp": 0.23188027277992163 - }, - { - "x": 0.7056282186927765, - "y": 4.217721440877341, - "heading": -1.0046856737535708, - "angularVelocity": 0.24797606172322137, - "velocityX": 0.26042169107034036, - "velocityY": -1.0987973985263735, - "timestamp": 0.28985034097490203 - }, - { - "x": 0.7248578813432716, - "y": 4.141594157346286, - "heading": -0.9873376775101346, - "angularVelocity": 0.2992578201751781, - "velocityX": 0.3317170955503569, - "velocityY": -1.313217077389022, - "timestamp": 0.34782040916988244 - }, - { - "x": 0.7487354042613769, - "y": 4.053220761716442, - "heading": -0.966978385263236, - "angularVelocity": 0.3512035241776958, - "velocityX": 0.41189399394518583, - "velocityY": -1.5244659594431005, - "timestamp": 0.40579047736486284 - }, - { - "x": 0.7778651202320954, - "y": 3.9528486632227207, - "heading": -0.9435683140016388, - "angularVelocity": 0.4038303212419584, - "velocityX": 0.5024958030537675, - "velocityY": -1.7314469625276876, - "timestamp": 0.46376054555984325 - }, - { - "x": 0.8129599622873139, - "y": 3.840816527291042, - "heading": -0.9170694264777192, - "angularVelocity": 0.4571132715385337, - "velocityX": 0.6053959077153058, - "velocityY": -1.932585891651215, - "timestamp": 0.5217306137548237 - }, - { - "x": 0.854862177504241, - "y": 3.717595744583205, - "heading": -0.8874498393217566, - "angularVelocity": 0.510946218940748, - "velocityX": 0.7228250116247967, - "velocityY": -2.1255931991210963, - "timestamp": 0.5797006819498041 - }, - { - "x": 0.904560986315067, - "y": 3.5838520361766597, - "heading": -0.8546925510341115, - "angularVelocity": 0.5650724469991482, - "velocityX": 0.8573184465415072, - "velocityY": -2.307116630546341, - "timestamp": 0.6376707501447845 - }, - { - "x": 0.9631953960515734, - "y": 3.4405334281831, - "heading": -0.8188108593180419, - "angularVelocity": 0.6189692859318806, - "velocityX": 1.0114600786614798, - "velocityY": -2.4722863445927272, - "timestamp": 0.6956408183397649 - }, - { - "x": 1.0320177450129453, - "y": 3.288983389093817, - "heading": -0.7798733754078085, - "angularVelocity": 0.671682561753563, - "velocityX": 1.18720489908499, - "velocityY": -2.6142808488606364, - "timestamp": 0.7536108865347453 - }, - { - "x": 1.1122806132492202, - "y": 3.131051679274452, - "heading": -0.7380383985495225, - "angularVelocity": 0.721665131004769, - "velocityX": 1.3845570780823164, - "velocityY": -2.724366465952162, - "timestamp": 0.8115809547297257 - }, - { - "x": 1.2050235127281292, - "y": 2.9691249632164602, - "heading": -0.6935876595861372, - "angularVelocity": 0.7667877638831885, - "velocityX": 1.5998411312363996, - "velocityY": -2.7932814485115998, - "timestamp": 0.8695510229247061 - }, - { - "x": 1.3108176746302633, - "y": 2.8059720420366694, - "heading": -0.6469378343676327, - "angularVelocity": 0.8047226210188198, - "velocityX": 1.8249790831071382, - "velocityY": -2.814433832146465, - "timestamp": 0.9275210911196865 - }, - { - "x": 1.42962965449819, - "y": 2.6444006527037383, - "heading": -0.5986101263227683, - "angularVelocity": 0.8336665722440691, - "velocityX": 2.0495401086696328, - "velocityY": -2.7871519624488035, - "timestamp": 0.9854911593146669 - }, - { - "x": 1.5609092304168717, - "y": 2.4869058396466244, - "heading": -0.5491623613282449, - "angularVelocity": 0.8529878700195282, - "velocityX": 2.2646096512622123, - "velocityY": -2.7168298737787384, - "timestamp": 1.0434612275096473 - }, - { - "x": 1.7038131780379508, - "y": 2.335492698782744, - "heading": -0.4991191344613761, - "angularVelocity": 0.863259754992006, - "velocityX": 2.4651333364043317, - "velocityY": -2.6119193159236365, - "timestamp": 1.1014312957046277 - }, - { - "x": 1.8574111825796424, - "y": 2.19168017622197, - "heading": -0.44893240803487616, - "angularVelocity": 0.8657351628033007, - "velocityX": 2.649608829595809, - "velocityY": -2.4808065099572545, - "timestamp": 1.1594013638996081 - }, - { - "x": 2.020808784771142, - "y": 2.056591090907756, - "heading": -0.3989735055068982, - "angularVelocity": 0.8618051364014737, - "velocityX": 2.8186546484975037, - "velocityY": -2.3303247610447158, - "timestamp": 1.2173714320945885 - }, - { - "x": 2.1931999690795068, - "y": 1.9310536028968317, - "heading": -0.3495416314047453, - "angularVelocity": 0.8527137476514646, - "velocityX": 2.973796472492188, - "velocityY": -2.165557017954213, - "timestamp": 1.275341500289569 - }, - { - "x": 2.3738798200885083, - "y": 1.815684583131589, - "heading": -0.3008771196490498, - "angularVelocity": 0.8394765310955621, - "velocityX": 3.1167783070616735, - "velocityY": -1.9901480774043983, - "timestamp": 1.3333115684845493 - }, - { - "x": 2.5622392362768553, - "y": 1.7109506397451384, - "heading": -0.253174013671076, - "angularVelocity": 0.8228920107101786, - "velocityX": 3.249252968873622, - "velocityY": -1.8066900151675016, - "timestamp": 1.3912816366795298 + "x": 0.6687542994396359, + "y": 4.396916362187497, + "heading": -1.0350125956919525, + "angularVelocity": 0.2204003026897336, + "velocityX": 0.09289604490221867, + "velocityY": -0.22303166776742134, + "timestamp": 0.05673996319027008 + }, + { + "x": 0.6793565646010321, + "y": 4.371607473302922, + "heading": -1.0104451675948027, + "angularVelocity": 0.4329827993501856, + "velocityX": 0.18685710327027932, + "velocityY": -0.4460504988292779, + "timestamp": 0.11347992638054016 + }, + { + "x": 0.6953596015342451, + "y": 4.333646306767502, + "heading": -0.974331857670356, + "angularVelocity": 0.6364704503481122, + "velocityX": 0.2820417221553139, + "velocityY": -0.6690375601429702, + "timestamp": 0.17021988957081025 + }, + { + "x": 0.7168434967725004, + "y": 4.283036502515157, + "heading": -0.9272707047547418, + "angularVelocity": 0.8294181079709282, + "velocityX": 0.3786378071168583, + "velocityY": -0.8919604703061096, + "timestamp": 0.22695985276108033 + }, + { + "x": 0.7439015517455688, + "y": 4.2197848874550585, + "heading": -0.8699519201955506, + "angularVelocity": 1.0102012996903078, + "velocityX": 0.4768782609592592, + "velocityY": -1.114763061230607, + "timestamp": 0.2836998159513504 + }, + { + "x": 0.7766442528059767, + "y": 4.143903696796417, + "heading": -0.8031709855452435, + "angularVelocity": 1.1769647157923862, + "velocityX": 0.5770659552705253, + "velocityY": -1.3373500156174554, + "timestamp": 0.3404397791416205 + }, + { + "x": 0.8152053931020986, + "y": 4.055414010177326, + "heading": -0.7278520306671347, + "angularVelocity": 1.3274410246890034, + "velocityX": 0.6796116551364678, + "velocityY": -1.559565456931146, + "timestamp": 0.3971797423318906 + }, + { + "x": 0.8597512927978146, + "y": 3.9543509740987406, + "heading": -0.6450962807712558, + "angularVelocity": 1.4585090515192565, + "velocityX": 0.7850886252135366, + "velocityY": -1.7811614670894944, + "timestamp": 0.45391970552216065 + }, + { + "x": 0.9104943772436287, + "y": 3.840772171273386, + "heading": -0.5562797497368643, + "angularVelocity": 1.5653258486713548, + "velocityX": 0.8943094354089278, + "velocityY": -2.0017426244088754, + "timestamp": 0.5106596687124307 + }, + { + "x": 0.9677131149809225, + "y": 3.7147727983516208, + "heading": -0.46323645073147074, + "angularVelocity": 1.6398195165087535, + "velocityX": 1.0084380482486026, + "velocityY": -2.2206460109824513, + "timestamp": 0.5673996319027008 + }, + { + "x": 1.0317820620609501, + "y": 3.576517638239885, + "heading": -0.3685846112186535, + "angularVelocity": 1.6681688564973882, + "velocityX": 1.1291679352201949, + "velocityY": -2.436645220373414, + "timestamp": 0.6241395950929709 + }, + { + "x": 1.1032183238971194, + "y": 3.4263179326907856, + "heading": -0.2763023679431359, + "angularVelocity": 1.6264064706221368, + "velocityX": 1.2590114236876868, + "velocityY": -2.64715902344566, + "timestamp": 0.680879558283241 + }, + { + "x": 1.1827463587467486, + "y": 3.2648378432524847, + "heading": -0.1927401719055838, + "angularVelocity": 1.4727220699339676, + "velocityX": 1.4016229545821517, + "velocityY": -2.8459674691151595, + "timestamp": 0.7376195214735111 + }, + { + "x": 1.2713975131080564, + "y": 3.0937718159398226, + "heading": -0.12815970051626668, + "angularVelocity": 1.1381831738726118, + "velocityX": 1.562411206789599, + "velocityY": -3.014912553591437, + "timestamp": 0.7943594846637811 + }, + { + "x": 1.3706976001434645, + "y": 2.9168916321975784, + "heading": -0.09351951840137682, + "angularVelocity": 0.610507659279375, + "velocityX": 1.7500907905494714, + "velocityY": -3.117382772158287, + "timestamp": 0.8510994478540512 + }, + { + "x": 1.4825331259415389, + "y": 2.737551336103052, + "heading": -0.084031813825935, + "angularVelocity": 0.1672137950394149, + "velocityX": 1.9710186526390192, + "velocityY": -3.1607404377956887, + "timestamp": 0.9078394110443213 + }, + { + "x": 1.6073609242840539, + "y": 2.563006200971128, + "heading": -0.08403157292142222, + "angularVelocity": 0.000004245764347455466, + "velocityX": 2.199997873173106, + "velocityY": -3.0762292627263235, + "timestamp": 0.9645793742345914 + }, + { + "x": 1.7433400400906298, + "y": 2.3970011200993575, + "heading": -0.08403150231700539, + "angularVelocity": 0.0000012443507689652163, + "velocityX": 2.3965316182984986, + "velocityY": -2.925717105509803, + "timestamp": 1.0213193374248615 + }, + { + "x": 1.889887864003564, + "y": 2.2402473630588187, + "heading": -0.08403142284388228, + "angularVelocity": 0.0000014006551755862827, + "velocityX": 2.5827973032253166, + "velocityY": -2.7626693467333516, + "timestamp": 1.0780593006151316 + }, + { + "x": 2.046376539489404, + "y": 2.0934165181715283, + "heading": -0.08403133019563887, + "angularVelocity": 0.0000016328569530158415, + "velocityX": 2.757997479855154, + "velocityY": -2.58778533914292, + "timestamp": 1.1347992638054016 + }, + { + "x": 2.2121356179273546, + "y": 1.9571376609651308, + "heading": -0.08403121735664319, + "angularVelocity": 0.00000198870406896459, + "velocityX": 2.921381494064391, + "velocityY": -2.401814339381988, + "timestamp": 1.1915392269956717 + }, + { + "x": 2.3864549305613383, + "y": 1.8319946594051495, + "heading": -0.08403107170612155, + "angularVelocity": 0.0000025669830126630976, + "velocityX": 3.0722493077661266, + "velocityY": -2.2055530973879893, + "timestamp": 1.2482791901859418 + }, + { + "x": 2.5685876291674634, + "y": 1.7185236734546225, + "heading": -0.08403086720089675, + "angularVelocity": 0.000003604253744603396, + "velocityX": 3.2099544723948052, + "velocityY": -1.9998424315154402, + "timestamp": 1.3050191533762119 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.20659037596839774, - "angularVelocity": 0.8035808677332508, - "velocityX": 3.3726738988458935, - "velocityY": -1.6170375099283227, - "timestamp": 1.4492517048745102 - }, - { - "x": 2.954766850408611, - "y": 1.5365275146757538, - "heading": -0.16235575982643646, - "angularVelocity": 0.7825616402901594, - "velocityX": 3.48539682535889, - "velocityY": -1.4273820029873086, - "timestamp": 1.5057771130526572 - }, - { - "x": 3.157939243604707, - "y": 1.4666859169715192, - "heading": -0.11962489482799478, - "angularVelocity": 0.7559585392779498, - "velocityX": 3.594355171320012, - "velocityY": -1.2355788300390564, - "timestamp": 1.5623025212308042 - }, - { - "x": 3.3670172328271493, - "y": 1.4078228373530197, - "heading": -0.07876775443844967, - "angularVelocity": 0.7228101787567607, - "velocityX": 3.698832011323254, - "velocityY": -1.0413561178184736, - "timestamp": 1.6188279294089511 - }, - { - "x": 3.5816948765446446, - "y": 1.3600926772574, - "heading": -0.04022391942526907, - "angularVelocity": 0.6818851248575686, - "velocityX": 3.797896390963022, - "velocityY": -0.8444018651787949, - "timestamp": 1.6753533375870973 - }, - { - "x": 3.8015965172009225, - "y": 1.323669626598405, - "heading": -0.004523858036694332, - "angularVelocity": 0.6315754726805621, - "velocityX": 3.890314952936408, - "velocityY": -0.6443659910283791, - "timestamp": 1.7318787457652434 - }, - { - "x": 4.026252074416622, - "y": 1.298748947880293, - "heading": 0.02768066779446809, - "angularVelocity": 0.5697353963312556, - "velocityX": 3.9744172480395035, - "velocityY": -0.44087569681180167, - "timestamp": 1.7884041539433895 - }, - { - "x": 4.255060536192153, - "y": 1.2855455255338215, - "heading": 0.05557184744847584, - "angularVelocity": 0.49342730203920493, - "velocityX": 4.0478869441227845, - "velocityY": -0.23358384790180253, - "timestamp": 1.8449295621215356 - }, - { - "x": 4.487235082115286, - "y": 1.284285135117098, - "heading": 0.07809760377772143, - "angularVelocity": 0.39850674334368213, - "velocityX": 4.107436874960846, - "velocityY": -0.02229776762957918, - "timestamp": 1.9014549702996817 - }, - { - "x": 4.721720600054119, - "y": 1.2951774473138997, - "heading": 0.0938646398689849, - "angularVelocity": 0.27893714701841205, - "velocityX": 4.148320649004834, - "velocityY": 0.19269763010774343, - "timestamp": 1.9579803784778278 - }, - { - "x": 4.957338422985755, - "y": 1.31837138535252, - "heading": 0.1023163328663219, - "angularVelocity": 0.14952024708429268, - "velocityX": 4.168352436997136, - "velocityY": 0.4103276523987602, - "timestamp": 2.014505786655974 - }, - { - "x": 5.192892039115628, - "y": 1.3539608804787848, - "heading": 0.10486380159502258, - "angularVelocity": 0.04506767506520281, - "velocityX": 4.16721654423966, - "velocityY": 0.6296194273219583, - "timestamp": 2.07103119483412 - }, - { - "x": 5.426922141496893, - "y": 1.4019164045228902, - "heading": 0.10486553946063551, - "angularVelocity": 0.0000307448573823463, - "velocityX": 4.140263819832846, - "velocityY": 0.8483888146896288, - "timestamp": 2.127556603012266 - }, - { - "x": 5.657546782205124, - "y": 1.4618615011178113, - "heading": 0.10700306943729049, - "angularVelocity": 0.037815383303704875, - "velocityX": 4.080017254919927, - "velocityY": 1.0604982525026048, - "timestamp": 2.1840820111904122 - }, - { - "x": 5.881407902963978, - "y": 1.532219235085926, - "heading": 0.11413761051896783, - "angularVelocity": 0.12621830273550538, - "velocityX": 3.9603627461358366, - "velocityY": 1.2447098788985973, - "timestamp": 2.2406074193685583 - }, - { - "x": 6.0948633682195945, - "y": 1.6093965656168685, - "heading": 0.12496970465048783, - "angularVelocity": 0.19163230272272128, - "velocityX": 3.7762746371133757, - "velocityY": 1.3653564479836866, - "timestamp": 2.2971328275467044 - }, - { - "x": 6.296160249007556, - "y": 1.6893086925404435, - "heading": 0.13798616586845183, - "angularVelocity": 0.23027628879637688, - "velocityX": 3.5611751825577227, - "velocityY": 1.41373816659089, - "timestamp": 2.3536582357248506 - }, - { - "x": 6.484990958449558, - "y": 1.7688506297276971, - "heading": 0.1520046167268183, - "angularVelocity": 0.24800264713145628, - "velocityX": 3.3406341595425215, - "velocityY": 1.4071890809981862, - "timestamp": 2.4101836439029967 - }, - { - "x": 6.661585058300773, - "y": 1.8459701168111895, - "heading": 0.1662218219427335, - "angularVelocity": 0.25151884213039066, - "velocityX": 3.124154350104949, - "velocityY": 1.364333130340974, - "timestamp": 2.4667090520811428 - }, - { - "x": 6.826289993460413, - "y": 1.9193152685714405, - "heading": 0.18009164346035805, - "angularVelocity": 0.24537322178925483, - "velocityX": 2.9138212437237407, - "velocityY": 1.2975607629244343, - "timestamp": 2.523234460259289 - }, - { - "x": 6.979441082007995, - "y": 1.9879614203209928, - "heading": 0.1932294988279346, - "angularVelocity": 0.23242389203402142, - "velocityX": 2.7094203029000408, - "velocityY": 1.2144300052324348, - "timestamp": 2.579759868437435 - }, - { - "x": 7.121330748513538, - "y": 2.0512482937052026, - "heading": 0.20535415779786798, - "angularVelocity": 0.21449927317147402, - "velocityX": 2.510192691724767, - "velocityY": 1.1196181579928337, - "timestamp": 2.636285276615581 - }, - { - "x": 7.25220694864691, - "y": 2.1086857599712796, - "heading": 0.21625288773093435, - "angularVelocity": 0.19281116730228098, - "velocityX": 2.3153517038019378, - "velocityY": 1.0161353649151172, - "timestamp": 2.692810684793727 - }, - { - "x": 7.372278984681028, - "y": 2.1598978527910453, - "heading": 0.22576002013784088, - "angularVelocity": 0.1681921937997094, - "velocityX": 2.124213515729, - "velocityY": 0.9060012916379937, - "timestamp": 2.7493360929718733 + "heading": -0.08403051576382121, + "angularVelocity": 0.000006193819237400721, + "velocityX": 3.3339066927233554, + "velocityY": -1.7855635206200389, + "timestamp": 1.361759116566482 + }, + { + "x": 2.9656624873675215, + "y": 1.523823440642391, + "heading": -0.08403020563552344, + "angularVelocity": 0.0000051460695851667095, + "velocityX": 3.449910187925088, + "velocityY": -1.5496108793286638, + "timestamp": 1.4220241970612824 + }, + { + "x": 3.179557704009846, + "y": 1.4451070785194118, + "heading": -0.08403001029754076, + "angularVelocity": 0.0000032413128973800796, + "velocityX": 3.5492397070767696, + "velocityY": -1.306168704607808, + "timestamp": 1.4822892775560828 + }, + { + "x": 3.3984052007772028, + "y": 1.3814422389869163, + "heading": -0.0840298717822531, + "angularVelocity": 0.0000022984336290020716, + "velocityX": 3.631414659542976, + "velocityY": -1.0564134156924954, + "timestamp": 1.5425543580508831 + }, + { + "x": 3.6211472313814954, + "y": 1.3331366287340103, + "heading": -0.08402976608446186, + "angularVelocity": 0.000001753881192375512, + "velocityX": 3.6960380501525916, + "velocityY": -0.8015522398094749, + "timestamp": 1.6028194385456835 + }, + { + "x": 3.846707230492903, + "y": 1.3004237167488812, + "heading": -0.08402968105378755, + "angularVelocity": 0.0000014109443416028125, + "velocityX": 3.7427976078264336, + "velocityY": -0.5428170296387713, + "timestamp": 1.6630845190404848 + }, + { + "x": 4.073995015295796, + "y": 1.2834616061839175, + "heading": -0.08402960975207145, + "angularVelocity": 0.0000011831348355128416, + "velocityX": 3.7714673727599504, + "velocityY": -0.2814583574052856, + "timestamp": 1.723349599535286 + }, + { + "x": 4.301912054355543, + "y": 1.282332269070522, + "heading": -0.08402954787131725, + "angularVelocity": 0.0000010268094506712622, + "velocityX": 3.78190881333695, + "velocityY": -0.018739493984294066, + "timestamp": 1.7836146800300874 + }, + { + "x": 4.529356777936335, + "y": 1.2970411469267544, + "heading": -0.08402949255215464, + "angularVelocity": 9.179306183548953e-7, + "velocityX": 3.7740715139410477, + "velocityY": 0.2440696624889034, + "timestamp": 1.8438797605248887 + }, + { + "x": 4.755229905146536, + "y": 1.32751711580047, + "heading": -0.08402944177914254, + "angularVelocity": 8.424947198098048e-7, + "velocityX": 3.7479934541809494, + "velocityY": 0.5056986338273413, + "timestamp": 1.90414484101969 + }, + { + "x": 4.978439767588294, + "y": 1.373612800948183, + "heading": -0.08402939404329389, + "angularVelocity": 7.920979819424461e-7, + "velocityX": 3.7038009508842378, + "velocityY": 0.7648821634228096, + "timestamp": 1.9644099215144912 + }, + { + "x": 5.197907642868858, + "y": 1.435105133457297, + "heading": -0.08402934813807361, + "angularVelocity": 7.617217119968608e-7, + "velocityX": 3.6417088217363065, + "velocityY": 1.0203642308985088, + "timestamp": 2.0246750020092925 + }, + { + "x": 5.41257465992254, + "y": 1.51169186312203, + "heading": -0.08402930302434115, + "angularVelocity": 7.485882719571497e-7, + "velocityX": 3.562046466895548, + "velocityY": 1.2708309527826855, + "timestamp": 2.084940082504094 + }, + { + "x": 5.626738902818376, + "y": 1.589673479175761, + "heading": -0.0840292579538456, + "angularVelocity": 7.478708262765028e-7, + "velocityX": 3.5537037557647206, + "velocityY": 1.2939768007189265, + "timestamp": 2.145205162998895 + }, + { + "x": 5.8409031120271635, + "y": 1.6676551877456105, + "heading": -0.0840292128833514, + "angularVelocity": 7.478708040156826e-7, + "velocityX": 3.5537031967834927, + "velocityY": 1.2939783358719235, + "timestamp": 2.2054702434936964 + }, + { + "x": 6.055067321252931, + "y": 1.745636896268829, + "heading": -0.08402916781285703, + "angularVelocity": 7.478708068018038e-7, + "velocityX": 3.5537031970652393, + "velocityY": 1.2939783350981522, + "timestamp": 2.2657353239884976 + }, + { + "x": 6.269231822230628, + "y": 1.8236178035375321, + "heading": -0.08402912274221092, + "angularVelocity": 7.478733249018271e-7, + "velocityX": 3.5537080382091872, + "velocityY": 1.2939650395958724, + "timestamp": 2.326000404483299 + }, + { + "x": 6.484552473362613, + "y": 1.8927933712391167, + "heading": -0.0785738032137305, + "angularVelocity": 0.09052206491205224, + "velocityX": 3.572892450555367, + "velocityY": 1.1478548959633867, + "timestamp": 2.3862654849781 + }, + { + "x": 6.6874523544155435, + "y": 1.9584473392764923, + "heading": -0.032553192185028265, + "angularVelocity": 0.7636364317587296, + "velocityX": 3.3667901774467204, + "velocityY": 1.0894197352485022, + "timestamp": 2.4465305654729015 + }, + { + "x": 6.875915672715089, + "y": 2.018652626020107, + "heading": 0.028144572860875648, + "angularVelocity": 1.0071796892586995, + "velocityX": 3.1272391366971615, + "velocityY": 0.9990078209355214, + "timestamp": 2.5067956459677028 + }, + { + "x": 7.049532373282227, + "y": 2.073323850727038, + "heading": 0.08948102802542592, + "angularVelocity": 1.0177777024597514, + "velocityX": 2.8808839072589665, + "velocityY": 0.9071791534676087, + "timestamp": 2.567060726462504 + }, + { + "x": 7.208331847820926, + "y": 2.1225088918275805, + "heading": 0.1460274817443076, + "angularVelocity": 0.9382954980664153, + "velocityX": 2.635016384859868, + "velocityY": 0.8161449498899447, + "timestamp": 2.6273258069573053 + }, + { + "x": 7.352376622997373, + "y": 2.1662517609495984, + "heading": 0.19476972653717045, + "angularVelocity": 0.8087974726436816, + "velocityX": 2.3901863897597297, + "velocityY": 0.72584104696901, + "timestamp": 2.6875908874521066 }, { "x": 7.481724262237549, "y": 2.2045881748199463, "heading": 0.2337432969576531, - "angularVelocity": 0.14123342187378718, - "velocityX": 1.9362138387677101, - "velocityY": 0.7906236057253049, - "timestamp": 2.8058615011500194 - }, - { - "x": 7.586144701324698, - "y": 2.244425533220676, - "heading": 0.24038146155956527, - "angularVelocity": 0.11057598681783708, - "velocityX": 1.739395418529896, - "velocityY": 0.6635953582873915, - "timestamp": 2.86589410671788 - }, - { - "x": 7.678582360185057, - "y": 2.2769030569359834, - "heading": 0.24515554508543944, - "angularVelocity": 0.07952484288688075, - "velocityX": 1.5397908850694146, - "velocityY": 0.5409980694340338, - "timestamp": 2.92592671228574 - }, - { - "x": 7.7589012203408085, - "y": 2.3022472705839294, - "heading": 0.2480460603725282, - "angularVelocity": 0.04814908931149622, - "velocityX": 1.3379206082427928, - "velocityY": 0.42217414033940376, - "timestamp": 2.9859593178536006 - }, - { - "x": 7.826988761652501, - "y": 2.320653171453341, - "heading": 0.24903661097421825, - "angularVelocity": 0.016500210049526117, - "velocityX": 1.1341760143115402, - "velocityY": 0.30659840090741436, - "timestamp": 3.045991923421461 - }, - { - "x": 7.882750526752601, - "y": 2.332290238336933, - "heading": 0.2481131524247365, - "angularVelocity": -0.015382616508921566, - "velocityX": 0.9288579859667665, - "velocityY": 0.19384577386762514, - "timestamp": 3.1060245289893214 - }, - { - "x": 7.926106207329853, - "y": 2.337307039185365, - "heading": 0.24526345998864965, - "angularVelocity": -0.047469077997382, - "velocityX": 0.7222022127332522, - "velocityY": 0.08356793447456733, - "timestamp": 3.1660571345571817 - }, - { - "x": 7.956986763254213, - "y": 2.3358348134459193, - "heading": 0.2404767337370615, - "angularVelocity": -0.07973544053784674, - "velocityX": 0.5143963956295903, - "velocityY": -0.02452376880063506, - "timestamp": 3.226089740125042 + "angularVelocity": 0.6467023706015796, + "velocityX": 2.146311565141514, + "velocityY": 0.6361298044504436, + "timestamp": 2.747855967946908 + }, + { + "x": 7.603777233505286, + "y": 2.239565945400217, + "heading": 0.2627306680277325, + "angularVelocity": 0.44772760939813167, + "velocityX": 1.8851825132237892, + "velocityY": 0.540252980034664, + "timestamp": 2.8125992861198625 + }, + { + "x": 7.70877923669531, + "y": 2.2685458571337813, + "heading": 0.28047750188422, + "angularVelocity": 0.2741106628035178, + "velocityX": 1.621819921393637, + "velocityY": 0.4476123954003698, + "timestamp": 2.877342604292817 + }, + { + "x": 7.796631047750002, + "y": 2.291686366419086, + "heading": 0.28817114093339696, + "angularVelocity": 0.11883294317143692, + "velocityX": 1.3569247535321998, + "velocityY": 0.3574192663942211, + "timestamp": 2.942085922465772 + }, + { + "x": 7.867260828377704, + "y": 2.309110963970804, + "heading": 0.2867085865097153, + "angularVelocity": -0.022590044269504023, + "velocityX": 1.0909199994819943, + "velocityY": 0.2691335267242617, + "timestamp": 3.0068292406387265 + }, + { + "x": 7.920614376230513, + "y": 2.320918360587503, + "heading": 0.2767910958513383, + "angularVelocity": -0.15318168636157245, + "velocityX": 0.824078057140675, + "velocityY": 0.1823724354867997, + "timestamp": 3.071572558811681 + }, + { + "x": 7.956649448780364, + "y": 2.327189146390105, + "heading": 0.25898192892634386, + "angularVelocity": -0.27507343502876075, + "velocityX": 0.5565836532132435, + "velocityY": 0.09685610777394495, + "timestamp": 3.1363158769846358 }, { "x": 7.975332260131836, "y": 2.3279902935028076, "heading": 0.2337432969576531, - "angularVelocity": -0.11216299402158979, - "velocityX": 0.3055922144989234, - "velocityY": -0.13067098902186064, - "timestamp": 3.2861223456929025 - }, - { - "x": 7.978374325239172, - "y": 2.3079982956389222, - "heading": 0.2220767476081581, - "angularVelocity": -0.15342778134763177, - "velocityX": 0.04000645659240227, - "velocityY": -0.2629164618495627, - "timestamp": 3.362161699498465 - }, - { - "x": 7.9611015630205095, - "y": 2.2781926707436404, - "heading": 0.20738275399488582, - "angularVelocity": -0.19324195798462207, - "velocityX": -0.2271555629316675, - "velocityY": -0.3919763044212194, - "timestamp": 3.4382010533040273 - }, - { - "x": 7.923380278914192, - "y": 2.238852677448683, - "heading": 0.18978782188047655, - "angularVelocity": -0.23139244659286162, - "velocityX": -0.49607581099088477, - "velocityY": -0.5173635930093835, - "timestamp": 3.5142404071095896 - }, - { - "x": 7.865060691710785, - "y": 2.19030356664395, - "heading": 0.16943867046954186, - "angularVelocity": -0.2676134184802368, - "velocityX": -0.7669658444564572, - "velocityY": -0.6384734795205669, - "timestamp": 3.590279760915152 - }, - { - "x": 7.785974313825651, - "y": 2.132928607276668, - "heading": 0.14650757608805504, - "angularVelocity": -0.30156876977312563, - "velocityX": -1.04007167245744, - "velocityY": -0.7545429635553378, - "timestamp": 3.6663191147207144 - }, - { - "x": 7.685930994427759, - "y": 2.067185547524282, - "heading": 0.12119962682948068, - "angularVelocity": -0.33282699013051165, - "velocityX": -1.3156781901870174, - "velocityY": -0.8645925624314937, - "timestamp": 3.7423584685262767 - }, - { - "x": 7.564715810849877, - "y": 1.9936296675417498, - "heading": 0.09376277438448252, - "angularVelocity": -0.3608243767451793, - "velocityX": -1.5941111741669691, - "velocityY": -0.9673396248292669, - "timestamp": 3.818397822331839 - }, - { - "x": 7.422086372418718, - "y": 1.912946891741318, - "heading": 0.06450207380614308, - "angularVelocity": -0.3848099584481087, - "velocityX": -1.8757318584778, - "velocityY": -1.0610660370252898, - "timestamp": 3.8944371761374015 - }, - { - "x": 7.2577720289721634, - "y": 1.8260026827906595, - "heading": 0.03380034441899183, - "angularVelocity": -0.4037610507008996, - "velocityX": -2.160911886057286, - "velocityY": -1.1434106761740845, - "timestamp": 3.970476529942964 - }, - { - "x": 7.0714787796722804, - "y": 1.7339163522481107, - "heading": 0.002148903485203126, - "angularVelocity": -0.41625078791073866, - "velocityX": -2.4499583436262102, - "velocityY": -1.2110351539548843, - "timestamp": 4.046515883748526 - }, - { - "x": 6.8629095324946725, - "y": 1.6381770650545604, - "heading": -0.029805628870875043, - "angularVelocity": -0.4202367689471438, - "velocityX": -2.7429118836403266, - "velocityY": -1.259075497121691, - "timestamp": 4.122555237554089 - }, - { - "x": 6.631824574679955, - "y": 1.5408277464126163, - "heading": -0.06118892010520367, - "angularVelocity": -0.4127243284389006, - "velocityX": -3.0390179065121625, - "velocityY": -1.2802491574411938, - "timestamp": 4.198594591359651 - }, - { - "x": 6.378206231452605, - "y": 1.444749972258303, - "heading": -0.09078814079587186, - "angularVelocity": -0.3892618651962165, - "velocityX": -3.335356371858044, - "velocityY": -1.26352696789074, - "timestamp": 4.274633945165213 - }, - { - "x": 6.102679405785972, - "y": 1.3540414004691264, - "heading": -0.11690114137793724, - "angularVelocity": -0.343414288459606, - "velocityX": -3.6234766851277205, - "velocityY": -1.1929161315747718, - "timestamp": 4.350673298970776 - }, - { - "x": 5.807422454316563, - "y": 1.2742067055061217, - "heading": -0.1372503142523278, - "angularVelocity": -0.26761370074796764, - "velocityX": -3.8829492452605625, - "velocityY": -1.0499128539038807, - "timestamp": 4.426712652776338 - }, - { - "x": 5.497293853879703, - "y": 1.211233933640027, - "heading": -0.14936074429275475, - "angularVelocity": -0.15926529401333597, - "velocityX": -4.078527563896464, - "velocityY": -0.8281602711553835, - "timestamp": 4.5027520065819004 - }, - { - "x": 5.179311523814037, - "y": 1.1692856386862889, - "heading": -0.15140362446925906, - "angularVelocity": -0.026866090705190943, - "velocityX": -4.181812629270475, - "velocityY": -0.5516655896498385, - "timestamp": 4.578791360387463 - }, - { - "x": 4.858542014963219, - "y": 1.1497329672294159, - "heading": -0.1514046806841633, - "angularVelocity": -0.000013890371910813042, - "velocityX": -4.21846705419206, - "velocityY": -0.2571388429584845, - "timestamp": 4.654830714193025 - }, - { - "x": 4.538318225528884, - "y": 1.1527274796290288, - "heading": -0.15538772135714082, - "angularVelocity": -0.052381306174200754, - "velocityX": -4.211290251797366, - "velocityY": 0.03938108689442748, - "timestamp": 4.730870067998588 - }, - { - "x": 4.221401569033501, - "y": 1.177956926117284, - "heading": -0.16806437438072017, - "angularVelocity": -0.1667117405546922, - "velocityX": -4.167797865638882, - "velocityY": 0.33179459353072166, - "timestamp": 4.80690942180415 - }, - { - "x": 3.910015091156467, - "y": 1.2249289706320923, - "heading": -0.19130842148638338, - "angularVelocity": -0.30568443762817654, - "velocityX": -4.095070016945054, - "velocityY": 0.6177333467998585, - "timestamp": 4.882948775609712 - }, - { - "x": 3.606469436454312, - "y": 1.2929813536267714, - "heading": -0.22267226856626002, - "angularVelocity": -0.41246861671228957, - "velocityX": -3.9919546854427725, - "velocityY": 0.8949626685241646, - "timestamp": 4.958988129415275 - }, - { - "x": 3.3124526239021637, - "y": 1.3814436970961244, - "heading": -0.26028249186707797, - "angularVelocity": -0.49461524090525394, - "velocityX": -3.8666400730333566, - "velocityY": 1.163375792166213, - "timestamp": 5.035027483220837 - }, - { - "x": 3.0292255135853545, - "y": 1.48970128529521, - "heading": -0.302685251705683, - "angularVelocity": -0.5576422959489076, - "velocityX": -3.724743782555543, - "velocityY": 1.4237047368380822, - "timestamp": 5.111066837026399 + "angularVelocity": -0.38982604971324913, + "velocityX": 0.2885674055438917, + "velocityY": 0.012374205328225838, + "timestamp": 3.2010591951575904 + }, + { + "x": 7.972208451820267, + "y": 2.320885188130861, + "heading": 0.19238106068409147, + "angularVelocity": -0.5227816803364386, + "velocityX": -0.039482143745094074, + "velocityY": -0.08980217850764118, + "timestamp": 3.2801787164211174 + }, + { + "x": 7.9431197679460075, + "y": 2.3056970040477323, + "heading": 0.14088004342725263, + "angularVelocity": -0.6509268058549238, + "velocityX": -0.3676549530345614, + "velocityY": -0.19196506551829257, + "timestamp": 3.3592982376846443 + }, + { + "x": 7.888054782175393, + "y": 2.282427289475609, + "heading": 0.07969868788669623, + "angularVelocity": -0.7732776255909873, + "velocityX": -0.695972180964121, + "velocityY": -0.2941083843849025, + "timestamp": 3.4384177589481713 + }, + { + "x": 7.806999942498959, + "y": 2.251078456184475, + "heading": 0.009399898258955654, + "angularVelocity": -0.8885138396324784, + "velocityX": -1.0244606941750847, + "velocityY": -0.39622122063553056, + "timestamp": 3.517537280211698 + }, + { + "x": 7.699938927127902, + "y": 2.2116544317191673, + "heading": -0.06930512925958275, + "angularVelocity": -0.9947611697041394, + "velocityX": -1.353155500201559, + "velocityY": -0.49828441623143704, + "timestamp": 3.596656801475225 + }, + { + "x": 7.56685172715319, + "y": 2.1641617607796966, + "heading": -0.15547855671843705, + "angularVelocity": -1.089155066697531, + "velocityX": -1.682103200946216, + "velocityY": -0.6002648926714905, + "timestamp": 3.675776322738752 + }, + { + "x": 7.407713494064462, + "y": 2.1086115226344657, + "heading": -0.24780538645569025, + "angularVelocity": -1.1669285691167814, + "velocityX": -2.011364964642267, + "velocityY": -0.702105337066011, + "timestamp": 3.754895844002279 + }, + { + "x": 7.222493943520295, + "y": 2.045023086580113, + "heading": -0.3442810817864959, + "angularVelocity": -1.2193665202986854, + "velocityX": -2.3410094953335943, + "velocityY": -0.8037009708710834, + "timestamp": 3.834015365265806 + }, + { + "x": 7.0111623875747595, + "y": 1.9734335261753837, + "heading": -0.4414827777687788, + "angularVelocity": -1.2285425193427022, + "velocityX": -2.6710418942202843, + "velocityY": -0.9048280280448349, + "timestamp": 3.913134886529333 + }, + { + "x": 6.773734962048751, + "y": 1.893931948844204, + "heading": -0.5324651154460041, + "angularVelocity": -1.1499353917244444, + "velocityX": -3.0008703507595227, + "velocityY": -1.0048288470601288, + "timestamp": 3.99225440779286 + }, + { + "x": 6.510837595414292, + "y": 1.8068437015263887, + "heading": -0.5976366951472135, + "angularVelocity": -0.8237104909184032, + "velocityX": -3.3227876311184024, + "velocityY": -1.1007175716817783, + "timestamp": 4.071373929056387 + }, + { + "x": 6.227373100797986, + "y": 1.7110085398670838, + "heading": -0.597636758184297, + "angularVelocity": -7.967323658608173e-7, + "velocityX": -3.5827377376583796, + "velocityY": -1.2112707474568936, + "timestamp": 4.150493450319914 + }, + { + "x": 5.941831354312318, + "y": 1.6215523108704284, + "heading": -0.5976367779578059, + "angularVelocity": -2.499194721356179e-7, + "velocityX": -3.6089923438060088, + "velocityY": -1.1306467426502582, + "timestamp": 4.229612971583441 + }, + { + "x": 5.656289545112644, + "y": 1.532096282055343, + "heading": -0.5976367977313131, + "angularVelocity": -2.499194483006569e-7, + "velocityX": -3.6089931364549517, + "velocityY": -1.130644212534215, + "timestamp": 4.308732492846968 + }, + { + "x": 5.370747722448539, + "y": 1.442640296218458, + "heading": -0.5976368175048198, + "angularVelocity": -2.499194443736624e-7, + "velocityX": -3.6089933066333257, + "velocityY": -1.130643669328191, + "timestamp": 4.387852014110495 + }, + { + "x": 5.084751687474933, + "y": 1.3546472631644115, + "heading": -0.5976368372766676, + "angularVelocity": -2.4989847693362694e-7, + "velocityX": -3.6147341440682292, + "velocityY": -1.112153254327249, + "timestamp": 4.466971535374022 + }, + { + "x": 4.791925527807106, + "y": 1.2930896001142946, + "heading": -0.5976368572220272, + "angularVelocity": -2.52091508667939e-7, + "velocityX": -3.7010608126974813, + "velocityY": -0.7780338160172059, + "timestamp": 4.5460910566375485 + }, + { + "x": 4.494706489260304, + "y": 1.2584868813594625, + "heading": -0.5976368780658494, + "angularVelocity": -2.63447274854176e-7, + "velocityX": -3.756582873610133, + "velocityY": -0.4373474232683845, + "timestamp": 4.6252105779010755 + }, + { + "x": 4.195570491935266, + "y": 1.2511276025347224, + "heading": -0.597636900694303, + "angularVelocity": -2.860034177860552e-7, + "velocityX": -3.780811518420208, + "velocityY": -0.09301470366874595, + "timestamp": 4.704330099164602 + }, + { + "x": 3.897009479956395, + "y": 1.2710731343763437, + "heading": -0.597636926337917, + "angularVelocity": -3.24112350243062e-7, + "velocityX": -3.7735442177972423, + "velocityY": 0.2520936871595517, + "timestamp": 4.783449620428129 + }, + { + "x": 3.6015106196507847, + "y": 1.3181573465478027, + "heading": -0.5976369569136154, + "angularVelocity": -3.8644948741127607e-7, + "velocityX": -3.7348413588269285, + "velocityY": 0.5951023390881358, + "timestamp": 4.862569141691656 + }, + { + "x": 3.311535572783971, + "y": 1.3919880157013045, + "heading": -0.5976369957684174, + "angularVelocity": -4.910899536256082e-7, + "velocityX": -3.6650252963611574, + "velocityY": 0.933153638627185, + "timestamp": 4.941688662955183 + }, + { + "x": 3.029499988052695, + "y": 1.4919500996437052, + "heading": -0.5976370495734, + "angularVelocity": -6.80046867328716e-7, + "velocityX": -3.5646775944445412, + "velocityY": 1.263431354816361, + "timestamp": 5.02080818421871 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.34872944068091694, - "angularVelocity": -0.6055310397951567, - "velocityX": -3.5701531878761683, - "velocityY": 1.676889312494585, - "timestamp": 5.187106190831962 - }, - { - "x": 2.5541569719261092, - "y": 1.727757046354288, - "heading": -0.38618529156646786, - "angularVelocity": -0.6332589444448175, - "velocityX": -3.442165602360439, - "velocityY": 1.8689832549148022, - "timestamp": 5.246253955287763 - }, - { - "x": 2.3587219086882643, - "y": 1.8492493966793417, - "heading": -0.4251400494897362, - "angularVelocity": -0.658600680544372, - "velocityX": -3.304183430024454, - "velocityY": 2.0540480513991684, - "timestamp": 5.305401719743564 - }, - { - "x": 2.172129075786119, - "y": 1.9811469849343935, - "heading": -0.4654182614046572, - "angularVelocity": -0.6809760653763983, - "velocityX": -3.1546895240914963, - "velocityY": 2.2299674293457925, - "timestamp": 5.364549484199365 - }, - { - "x": 1.995157304922952, - "y": 2.1227357753454896, - "heading": -0.5067996859573998, - "angularVelocity": -0.6996278715430715, - "velocityX": -2.9920280587343844, - "velocityY": 2.3938147403170578, - "timestamp": 5.4236972486551664 - }, - { - "x": 1.8286811439598407, - "y": 2.273062802113803, - "heading": -0.549006464745395, - "angularVelocity": -0.7135819785637939, - "velocityX": -2.8145807790844684, - "velocityY": 2.5415504398420463, - "timestamp": 5.482845013110968 - }, - { - "x": 1.673644229855571, - "y": 2.430852006094566, - "heading": -0.5916896444353141, - "angularVelocity": -0.7216363979709688, - "velocityX": -2.6211796089119246, - "velocityY": 2.6677120501937903, - "timestamp": 5.541992777566769 - }, - { - "x": 1.5309859321329615, - "y": 2.594413834176532, - "heading": -0.634420502020555, - "angularVelocity": -0.7224424790758148, - "velocityX": -2.411896696944723, - "velocityY": 2.7653087075538, - "timestamp": 5.60114054202257 - }, - { - "x": 1.4014991059285755, - "y": 2.7615886598812227, - "heading": -0.6766978032526942, - "angularVelocity": -0.7147742881090955, - "velocityX": -2.189209134034951, - "velocityY": 2.826392970940027, - "timestamp": 5.660288306478371 - }, - { - "x": 1.2856293975453275, - "y": 2.9297974775958746, - "heading": -0.7179836021581544, - "angularVelocity": -0.6980111469185274, - "velocityX": -1.9589871138719785, - "velocityY": 2.8438744771216182, - "timestamp": 5.719436070934172 - }, - { - "x": 1.1833007632826107, - "y": 3.0962581686871435, - "heading": -0.7577647191568426, - "angularVelocity": -0.6725717762066157, - "velocityX": -1.7300507500868296, - "velocityY": 2.814319232904572, - "timestamp": 5.7785838353899734 - }, - { - "x": 1.0938911269423066, - "y": 3.2583116792220825, - "heading": -0.7956106235538323, - "angularVelocity": -0.6398535049498092, - "velocityX": -1.5116317102249486, - "velocityY": 2.7398078697638226, - "timestamp": 5.837731599845775 - }, - { - "x": 1.016385611128122, - "y": 3.4136921446071793, - "heading": -0.8311990080013264, - "angularVelocity": -0.601686044687091, - "velocityX": -1.3103710094081817, - "velocityY": 2.62698796505164, - "timestamp": 5.896879364301576 - }, - { - "x": 0.9495982361573998, - "y": 3.5606291449139458, - "heading": -0.864309152764572, - "angularVelocity": -0.5597869178637827, - "velocityX": -1.1291614414375832, - "velocityY": 2.4842359074546003, - "timestamp": 5.956027128757377 - }, - { - "x": 0.8923465849102798, - "y": 3.697809371688421, - "heading": -0.8947997322814718, - "angularVelocity": -0.5154984266511812, - "velocityX": -0.9679427747417668, - "velocityY": 2.319279993700968, - "timestamp": 6.015174893213178 - }, - { - "x": 0.8435455769066035, - "y": 3.8242806095123454, - "heading": -0.9225850102577191, - "angularVelocity": -0.46976040822321335, - "velocityX": -0.8250693572729036, - "velocityY": 2.138225155042558, - "timestamp": 6.074322657668979 - }, - { - "x": 0.8022404458446034, - "y": 3.939354876295257, - "heading": -0.947615738236735, - "angularVelocity": -0.42318975551004434, - "velocityX": -0.698337992010946, - "velocityY": 1.9455387340785029, - "timestamp": 6.13347042212478 - }, - { - "x": 0.7676071594331643, - "y": 4.0425313748152725, - "heading": -0.9698657760492436, - "angularVelocity": -0.3761771559284466, - "velocityX": -0.5855383839116994, - "velocityY": 1.7443854297674437, - "timestamp": 6.192618186580582 - }, - { - "x": 0.7389393229135627, - "y": 4.133440839855761, - "heading": -0.9893233232577529, - "angularVelocity": -0.32896504859535863, - "velocityX": -0.4846816575971237, - "velocityY": 1.5369890286964785, - "timestamp": 6.251765951036383 - }, - { - "x": 0.7156313779093327, - "y": 4.21180687471968, - "heading": -1.0059853372092005, - "angularVelocity": -0.28170149970585456, - "velocityX": -0.39406299153786883, - "velocityY": 1.324919641256768, - "timestamp": 6.310913715492184 - }, - { - "x": 0.6971623458837157, - "y": 4.277419392368828, - "heading": -1.0198540109194538, - "angularVelocity": -0.23447502771836987, - "velocityX": -0.31225241047644936, - "velocityY": 1.1092983522340676, - "timestamp": 6.370061479947985 - }, - { - "x": 0.6830816107540019, - "y": 4.330116323499302, - "heading": -1.0309345469547266, - "angularVelocity": -0.1873365145279998, - "velocityX": -0.23806030978966433, - "velocityY": 0.8909369883261355, - "timestamp": 6.429209244403786 - }, - { - "x": 0.6729970259294025, - "y": 4.3697708967031526, - "heading": -1.039233740627151, - "angularVelocity": -0.14031288838694966, - "velocityX": -0.17049815690219822, - "velocityY": 0.6704323243439457, - "timestamp": 6.488357008859587 - }, - { - "x": 0.6665651633052443, - "y": 4.396282678408227, - "heading": -1.0447590694290692, - "angularVelocity": -0.09341568278623903, - "velocityX": -0.10874227763865159, - "velocityY": 0.448229649066216, - "timestamp": 6.547504773315389 + "heading": -0.5976371219829064, + "angularVelocity": -9.151914125801383e-7, + "velocityX": -3.434634228323893, + "velocityY": 1.583184066038948, + "timestamp": 5.099927705482237 + }, + { + "x": 2.562615182382705, + "y": 1.7237517111402614, + "heading": -0.5976372044989885, + "angularVelocity": -0.000001403654433041047, + "velocityX": -3.319432744203681, + "velocityY": 1.8123319353797018, + "timestamp": 5.158714312968238 + }, + { + "x": 2.3751466995867516, + "y": 1.8432734201994623, + "heading": -0.5976372636960788, + "angularVelocity": -0.000001006982590926024, + "velocityX": -3.188965834447897, + "velocityY": 2.033145203823246, + "timestamp": 5.217500920454238 + }, + { + "x": 2.1962100914661193, + "y": 1.9752263102479175, + "heading": -0.5976373093137471, + "angularVelocity": -7.759874278787006e-7, + "velocityX": -3.04383286896162, + "velocityY": 2.2446080100791628, + "timestamp": 5.276287527940239 + }, + { + "x": 2.026628285101079, + "y": 2.1190035289778706, + "heading": -0.5976373463774368, + "angularVelocity": -6.304784584021761e-7, + "velocityX": -2.884701356604495, + "velocityY": 2.4457478476571914, + "timestamp": 5.33507413542624 + }, + { + "x": 1.8671811829950287, + "y": 2.2739438442752706, + "heading": -0.5976373777665476, + "angularVelocity": -5.339500279584693e-7, + "velocityX": -2.7123031745627153, + "velocityY": 2.6356396792296386, + "timestamp": 5.39386074291224 + }, + { + "x": 1.7186020763048488, + "y": 2.439334685025019, + "heading": -0.5976374052703787, + "angularVelocity": -4.678587919148875e-7, + "velocityX": -2.5274312134028762, + "velocityY": 2.813410193625064, + "timestamp": 5.452647350398241 + }, + { + "x": 1.5815742720495027, + "y": 2.6144154181152923, + "heading": -0.597637430079118, + "angularVelocity": -4.220134537614776e-7, + "velocityX": -2.330935737157126, + "velocityY": 2.978241823734581, + "timestamp": 5.5114339578842415 + }, + { + "x": 1.456727954325337, + "y": 2.7983808405830617, + "heading": -0.5976374777093088, + "angularVelocity": -8.102217978593512e-7, + "velocityX": -2.123720402710731, + "velocityY": 3.129376406209191, + "timestamp": 5.570220565370242 + }, + { + "x": 1.3455561973341574, + "y": 2.9885105140912174, + "heading": -0.6039795556256176, + "angularVelocity": -0.10788303982023785, + "velocityX": -1.8911068650739042, + "velocityY": 3.234234490456602, + "timestamp": 5.629007172856243 + }, + { + "x": 1.246887053690835, + "y": 3.1733277865183314, + "heading": -0.6383468335999772, + "angularVelocity": -0.584610669743856, + "velocityX": -1.6784289460285728, + "velocityY": 3.1438669508378774, + "timestamp": 5.687793780342243 + }, + { + "x": 1.158565022655154, + "y": 3.3474027501759167, + "heading": -0.6816254514815488, + "angularVelocity": -0.736198595775039, + "velocityX": -1.5024175541464038, + "velocityY": 2.9611330046395477, + "timestamp": 5.746580387828244 + }, + { + "x": 1.0790333486785466, + "y": 3.5092497722088574, + "heading": -0.7278331515121171, + "angularVelocity": -0.7860242665231638, + "velocityX": -1.3528876282841755, + "velocityY": 2.7531274375968073, + "timestamp": 5.805366995314245 + }, + { + "x": 1.0074427197106477, + "y": 3.6582801720807683, + "heading": -0.7741804064083878, + "angularVelocity": -0.7883981892867038, + "velocityX": -1.217805075500369, + "velocityY": 2.5351080160120225, + "timestamp": 5.864153602800245 + }, + { + "x": 0.9432732996874974, + "y": 3.7941912605500616, + "heading": -0.8190469180938782, + "angularVelocity": -0.7632097446033984, + "velocityX": -1.0915652861654275, + "velocityY": 2.3119396454653414, + "timestamp": 5.922940210286246 + }, + { + "x": 0.8861770840921692, + "y": 3.9168021213870996, + "heading": -0.8613760150853045, + "angularVelocity": -0.7200466024767106, + "velocityX": -0.971245289310589, + "velocityY": 2.085693767347223, + "timestamp": 5.9817268177722465 + }, + { + "x": 0.8359056858624994, + "y": 4.025993630774074, + "heading": -0.9004245404660403, + "angularVelocity": -0.6642418579781981, + "velocityX": -0.8551505245755496, + "velocityY": 1.857421512424882, + "timestamp": 6.040513425258247 + }, + { + "x": 0.7922732558571624, + "y": 4.121681912181887, + "heading": -0.9356409495537121, + "angularVelocity": -0.5990549649604897, + "velocityX": -0.7422171795800264, + "velocityY": 1.627722460946589, + "timestamp": 6.099300032744248 + }, + { + "x": 0.7551356458246514, + "y": 4.203804939460416, + "heading": -0.9665991512691937, + "angularVelocity": -0.5266199741642557, + "velocityX": -0.6317358939509348, + "velocityY": 1.3969683026544202, + "timestamp": 6.158086640230248 + }, + { + "x": 0.7243778479811152, + "y": 4.272315098350931, + "heading": -0.9929596716344966, + "angularVelocity": -0.44841030113160957, + "velocityX": -0.5232109686013307, + "velocityY": 1.1654041935798078, + "timestamp": 6.216873247716249 + }, + { + "x": 0.6999059919554825, + "y": 4.327174746467568, + "heading": -1.0144454156380707, + "angularVelocity": -0.3654870543208486, + "velocityX": -0.41628284182686576, + "velocityY": 0.9331997620325617, + "timestamp": 6.27565985520225 + }, + { + "x": 0.6816420121384978, + "y": 4.368353408748977, + "heading": -1.0308257653496888, + "angularVelocity": -0.27864084035669345, + "velocityX": -0.31068266392705546, + "velocityY": 0.7004769290559099, + "timestamp": 6.33444646268825 + }, + { + "x": 0.6695199611090151, + "y": 4.395825923599214, + "heading": -1.0419057024338247, + "angularVelocity": -0.1884772324508513, + "velocityX": -0.2062042963164626, + "velocityY": 0.46732608029439393, + "timestamp": 6.393233070174251 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -0.046646417662750905, - "velocityX": -0.052103102496543485, - "velocityY": 0.22466601267048775, - "timestamp": 6.60665253777119 + "angularVelocity": -0.09547069579077468, + "velocityX": -0.1026863106378516, + "velocityY": 0.23381596243572259, + "timestamp": 6.452019677660251 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -2.054852244630119e-40, - "velocityX": -3.6573844759845184e-41, - "velocityY": 1.4664679602509001e-41, - "timestamp": 6.665800302226991 + "angularVelocity": -7.821518056781921e-37, + "velocityX": -6.302822662985753e-37, + "velocityY": 5.041148502478119e-38, + "timestamp": 6.510806285146252 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubGSub.traj b/src/main/deploy/choreo/SourceLanePSubHSubGSub.traj index ec4e67ac..09e98b3d 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubGSub.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubGSub.traj @@ -5,1908 +5,1818 @@ "y": 4.409571170806885, "heading": -1.0475181007536924, "angularVelocity": 0, - "velocityX": -1.2693923486855146e-35, - "velocityY": 4.115755350063261e-34, + "velocityX": -1.7128958192175968e-32, + "velocityY": -9.692960229070848e-33, "timestamp": 0 }, { - "x": 0.6686935325209526, - "y": 4.399440636010588, - "heading": -1.042618245961612, - "angularVelocity": 0.09045080045353641, - "velocityX": 0.09617884019158171, - "velocityY": -0.1870085992810291, - "timestamp": 0.05417149176692242 - }, - { - "x": 0.6791748263382973, - "y": 4.379207943091365, - "heading": -1.0329282635016275, - "angularVelocity": 0.17887604981743244, - "velocityX": 0.19348357365607174, - "velocityY": -0.37349336817742057, - "timestamp": 0.10834298353384483 - }, - { - "x": 0.6949946113692655, - "y": 4.348905021041836, - "heading": -1.0185695679650135, - "angularVelocity": 0.26505999868701274, - "velocityX": 0.2920315559895287, - "velocityY": -0.5593887312520465, - "timestamp": 0.16251447530076724 - }, - { - "x": 0.7162276376127428, - "y": 4.308568047404174, - "heading": -0.9996771897134135, - "angularVelocity": 0.3487513013835064, - "velocityX": 0.3919594153846487, - "velocityY": -0.7446162607301866, - "timestamp": 0.21668596706768967 - }, - { - "x": 0.7429573380312586, - "y": 4.258238333670889, - "heading": -0.9764021455676966, - "angularVelocity": 0.4296548495629363, - "velocityX": 0.49342743843058046, - "velocityY": -0.9290811844324458, - "timestamp": 0.2708574588346121 - }, - { - "x": 0.775277417821335, - "y": 4.197963466887649, - "heading": -0.9489143945994782, - "angularVelocity": 0.5074209712829532, - "velocityX": 0.5966252494787523, - "velocityY": -1.1126676563122382, - "timestamp": 0.3250289506015345 - }, - { - "x": 0.8132938470478027, - "y": 4.127798804086614, - "heading": -0.91740659080495, - "angularVelocity": 0.5816307206398024, - "velocityX": 0.701779256699018, - "velocityY": -1.2952322432419845, - "timestamp": 0.37920044236845696 - }, - { - "x": 0.8571273890255799, - "y": 4.047809461983153, - "heading": -0.8820989485703652, - "angularVelocity": 0.651775335752228, - "velocityX": 0.809162541921033, - "velocityY": -1.4765947825033474, - "timestamp": 0.4333719341353794 - }, - { - "x": 0.9069168499797303, - "y": 3.95807301716377, - "heading": -0.843245694489665, - "angularVelocity": 0.7172269548689871, - "velocityX": 0.9191081753549234, - "velocityY": -1.6565252661950285, - "timestamp": 0.4875434259023018 - }, - { - "x": 0.9628233134219261, - "y": 3.8586832511299094, - "heading": -0.8011438194850887, - "angularVelocity": 0.7771961530194386, - "velocityX": 1.0320273933518045, - "velocityY": -1.8347245533036767, - "timestamp": 0.5417149176692242 - }, - { - "x": 1.025035737811962, - "y": 3.749755475919721, - "heading": -0.7561452166136415, - "angularVelocity": 0.8306694426112097, - "velocityX": 1.1484347645014206, - "velocityY": -2.0107951924023055, - "timestamp": 0.5958864094361466 - }, - { - "x": 1.0937784660640726, - "y": 3.631434328521385, - "heading": -0.7086738761047954, - "angularVelocity": 0.8763159174773277, - "velocityX": 1.268983482084674, - "velocityY": -2.1841958480195354, - "timestamp": 0.6500579012030689 - }, - { - "x": 1.1693214408917312, - "y": 3.5039055633996625, - "heading": -0.6592507614486313, - "angularVelocity": 0.9123454614986682, - "velocityX": 1.3945153135654589, - "velocityY": -2.3541674958929617, - "timestamp": 0.7042293929699913 - }, - { - "x": 1.2519942470920469, - "y": 3.3674145874122954, - "heading": -0.6085305941721398, - "angularVelocity": 0.9362889154819557, - "velocityX": 1.5261312454902076, - "velocityY": -2.519608959166787, - "timestamp": 0.7584008847369137 - }, - { - "x": 1.3422054245010013, - "y": 3.222296884123663, - "heading": -0.557357603065156, - "angularVelocity": 0.9446479954282978, - "velocityX": 1.665288779513328, - "velocityY": -2.678857431377627, - "timestamp": 0.8125723765038361 - }, - { - "x": 1.4404683178656266, - "y": 3.0690304198840344, - "heading": -0.506852671522414, - "angularVelocity": 0.9323156866353964, - "velocityX": 1.813922603191545, - "velocityY": -2.82928269539025, - "timestamp": 0.8667438682707584 - }, - { - "x": 1.5474318621807708, - "y": 2.9083305512073854, - "heading": -0.4585562938775888, - "angularVelocity": 0.8915460156169343, - "velocityX": 1.974535698137392, - "velocityY": -2.9665025539277043, - "timestamp": 0.9209153600376808 - }, - { - "x": 1.6638998489942354, - "y": 2.7413289476125455, - "heading": -0.41468556937766105, - "angularVelocity": 0.8098489273414394, - "velocityX": 2.1499866999154866, - "velocityY": -3.082831913017614, - "timestamp": 0.9750868518046032 - }, - { - "x": 1.7907578946957254, - "y": 2.5699019528024616, - "heading": -0.37863644859559736, - "angularVelocity": 0.6654629511989103, - "velocityX": 2.3417860864402265, - "velocityY": -3.164524166099463, - "timestamp": 1.0292583435715257 - }, - { - "x": 1.9285624590284545, - "y": 2.3970723087204746, - "heading": -0.35526123126492787, - "angularVelocity": 0.43150403594649805, - "velocityX": 2.543857660882686, - "velocityY": -3.1904169230857033, - "timestamp": 1.0834298353384482 - }, - { - "x": 2.076969770307468, - "y": 2.2268102881561207, - "heading": -0.3460628874086392, - "angularVelocity": 0.16980045326913482, - "velocityX": 2.7395832464342806, - "velocityY": -3.1430188649210824, - "timestamp": 1.1376013271053707 - }, - { - "x": 2.235228147006563, - "y": 2.062095599555062, - "heading": -0.34448101087817407, - "angularVelocity": 0.02920127319497364, - "velocityX": 2.9214328706326906, - "velocityY": -3.0406157044697584, - "timestamp": 1.1917728188722931 - }, - { - "x": 2.4019056025260537, - "y": 1.9051420100449306, - "heading": -0.3444808874915784, - "angularVelocity": 0.0000022777034862099955, - "velocityX": 3.076848173881473, - "velocityY": -2.897346637331644, - "timestamp": 1.2459443106392156 - }, - { - "x": 2.5762261949471483, - "y": 1.7567232022649006, - "heading": -0.34448083068467217, - "angularVelocity": 0.0000010486494725236109, - "velocityX": 3.217939671499618, - "velocityY": -2.7397954706253063, - "timestamp": 1.300115802406138 + "x": 0.6705320996855912, + "y": 4.398099342118358, + "heading": -1.0386114127976835, + "angularVelocity": 0.15927551181553445, + "velocityX": 0.1260500242755718, + "velocityY": -0.20514712032687513, + "timestamp": 0.05592000838348859 + }, + { + "x": 0.6846335258864488, + "y": 4.375148415401169, + "heading": -1.0210111049652701, + "angularVelocity": 0.31474079388031545, + "velocityX": 0.25217138924859694, + "velocityY": -0.41042423598715305, + "timestamp": 0.11184001676697718 + }, + { + "x": 0.7057920861320249, + "y": 4.340710150429397, + "heading": -0.99496347376924, + "angularVelocity": 0.4658016325283906, + "velocityX": 0.3783719076090773, + "velocityY": -0.6158487090273924, + "timestamp": 0.16776002515046576 + }, + { + "x": 0.7340126708459819, + "y": 4.29477510174039, + "heading": -0.9607567098648512, + "angularVelocity": 0.6117088479280167, + "velocityX": 0.5046598798845959, + "velocityY": -0.8214420923185946, + "timestamp": 0.22368003353395435 + }, + { + "x": 0.7693006604451206, + "y": 4.237332365746024, + "heading": -0.918731573784226, + "angularVelocity": 0.7515223494321578, + "velocityX": 0.6310440684690269, + "velocityY": -1.0272304610620717, + "timestamp": 0.2796000419174429 + }, + { + "x": 0.8116619611973517, + "y": 4.168369327121192, + "heading": -0.8692948358062459, + "angularVelocity": 0.8840617054087768, + "velocityX": 0.7575338769931039, + "velocityY": -1.2332444257142754, + "timestamp": 0.3355200503009315 + }, + { + "x": 0.8611030812908155, + "y": 4.087871426229515, + "heading": -0.8129373545696883, + "angularVelocity": 1.0078231900479873, + "velocityX": 0.8841400694078274, + "velocityY": -1.4395187557848361, + "timestamp": 0.3914400586844201 + }, + { + "x": 0.9176312901451578, + "y": 3.995821968070834, + "heading": -0.7502611156342615, + "angularVelocity": 1.120819555419329, + "velocityX": 1.0108762585778373, + "velocityY": -1.6460916373156267, + "timestamp": 0.4473600670679087 + }, + { + "x": 0.9812548770105831, + "y": 3.8922019971306105, + "heading": -0.6820246305048665, + "angularVelocity": 1.2202516970570252, + "velocityX": 1.1377606818136925, + "velocityY": -1.8530034943775027, + "timestamp": 0.5032800754513973 + }, + { + "x": 1.0519833618359966, + "y": 3.77699034023748, + "heading": -0.6092267013635411, + "angularVelocity": 1.3018225720227254, + "velocityX": 1.2648153473148886, + "velocityY": -2.060293984633039, + "timestamp": 0.5592000838348858 + }, + { + "x": 1.129826908295346, + "y": 3.6501643798944907, + "heading": -0.5332724156478236, + "angularVelocity": 1.3582667083101592, + "velocityX": 1.3920517666147918, + "velocityY": -2.26798893650447, + "timestamp": 0.6151200922183744 + }, + { + "x": 1.214791954900265, + "y": 3.511704401985981, + "heading": -0.45632702327380104, + "angularVelocity": 1.3759903583409006, + "velocityX": 1.5194033238021893, + "velocityY": -2.4760364297333126, + "timestamp": 0.6710401006018629 + }, + { + "x": 1.306860167939052, + "y": 3.361616595640326, + "heading": -0.38215505844470404, + "angularVelocity": 1.3263940219829728, + "velocityX": 1.6464270249639614, + "velocityY": -2.6839732447173734, + "timestamp": 0.7269601089853515 + }, + { + "x": 1.4058792704184802, + "y": 3.2000739494751453, + "heading": -0.3184323584737964, + "angularVelocity": 1.1395330904442935, + "velocityX": 1.770727604337509, + "velocityY": -2.888816558419535, + "timestamp": 0.78288011736884 + }, + { + "x": 1.5121691709784744, + "y": 3.0300749520186443, + "heading": -0.28662541962876287, + "angularVelocity": 0.5687935278354703, + "velocityX": 1.9007490097476158, + "velocityY": -3.040038840671852, + "timestamp": 0.8388001257523285 + }, + { + "x": 1.6244650109400063, + "y": 2.8508645326617055, + "heading": -0.2866253125429205, + "angularVelocity": 0.0000019149825875479786, + "velocityX": 2.00815134345883, + "velocityY": -3.2047638141959642, + "timestamp": 0.8947201341358171 + }, + { + "x": 1.735288205529194, + "y": 2.670739688820617, + "heading": -0.28662527835532886, + "angularVelocity": 6.113659969296124e-7, + "velocityX": 1.9818164873864772, + "velocityY": -3.2211161809173365, + "timestamp": 0.9506401425193056 + }, + { + "x": 1.8506362591615513, + "y": 2.493478394875359, + "heading": -0.2866252441530595, + "angularVelocity": 6.116284727574992e-7, + "velocityX": 2.062733124811472, + "velocityY": -3.1699082147776885, + "timestamp": 1.0065601509027942 + }, + { + "x": 1.9771720781105007, + "y": 2.324022128559085, + "heading": -0.2866252095630549, + "angularVelocity": 6.185622213406466e-7, + "velocityX": 2.2628004288052175, + "velocityY": -3.0303333496335663, + "timestamp": 1.0624801592862827 + }, + { + "x": 2.1143701177449694, + "y": 2.1630766803808767, + "heading": -0.28662517392184667, + "angularVelocity": 6.373605666632096e-7, + "velocityX": 2.4534695827223683, + "velocityY": -2.878137053815794, + "timestamp": 1.1184001676697712 + }, + { + "x": 2.261659606829455, + "y": 2.011311910787294, + "heading": -0.2866251364727607, + "angularVelocity": 6.696902775127292e-7, + "velocityX": 2.633931813357441, + "velocityY": -2.7139618533818717, + "timestamp": 1.1743201760532598 + }, + { + "x": 2.418427669547749, + "y": 1.8693594092228303, + "heading": -0.2866250962912339, + "angularVelocity": 7.185536621676126e-7, + "velocityX": 2.803434177677683, + "velocityY": -2.538492136678184, + "timestamp": 1.2302401844367483 + }, + { + "x": 2.5840219554137547, + "y": 1.7378099138847216, + "heading": -0.28662505217416817, + "angularVelocity": 7.889316720181228e-7, + "velocityX": 2.961270762521925, + "velocityY": -2.3524584337678864, + "timestamp": 1.2861601928202369 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.34448076255255106, - "angularVelocity": 0.000001257711738653551, - "velocityX": 3.3509724640087613, - "velocityY": -2.575382967934733, - "timestamp": 1.3542872941730606 - }, - { - "x": 2.9607343532453942, - "y": 1.47791150359439, - "heading": -0.34448070266933767, - "angularVelocity": 0.0000010280378855043595, - "velocityX": 3.484651653320559, - "velocityY": -2.391405084272886, - "timestamp": 1.4125373005573685 - }, - { - "x": 3.170914415879318, - "y": 1.3497321947844976, - "heading": -0.344480652877576, - "angularVelocity": 8.547940977947146e-7, - "velocityX": 3.608241023138232, - "velocityY": -2.2005029143554395, - "timestamp": 1.4707873069416764 - }, - { - "x": 3.387684985247893, - "y": 1.2330440779434721, - "heading": -0.34448061027247806, - "angularVelocity": 7.314179104628634e-7, - "velocityX": 3.721382757255318, - "velocityY": -2.003229254108037, - "timestamp": 1.5290373133259842 - }, - { - "x": 3.6104184041129233, - "y": 1.1281850184409496, - "heading": -0.3444805729289611, - "angularVelocity": 6.41090349411582e-7, - "velocityX": 3.8237492609963484, - "velocityY": -1.8001553306399407, - "timestamp": 1.5872873197102921 - }, - { - "x": 3.8384697501167597, - "y": 1.0354586289592775, - "heading": -0.34448053951161883, - "angularVelocity": 5.736882162731552e-7, - "velocityX": 3.915044137493387, - "velocityY": -1.5918691728530348, - "timestamp": 1.6455373260946 - }, - { - "x": 4.071178703094687, - "y": 0.9551333894958018, - "heading": -0.3444805090563753, - "angularVelocity": 5.228367414213026e-7, - "velocityX": 3.99500304673988, - "velocityY": -1.3789739169044062, - "timestamp": 1.703787332478908 - }, - { - "x": 4.307871457008001, - "y": 0.8874418681856828, - "heading": -0.3444804808411649, - "angularVelocity": 4.843812402300459e-7, - "velocityX": 4.063394471611187, - "velocityY": -1.1620860753820388, - "timestamp": 1.7620373388632158 - }, - { - "x": 4.547862670970626, - "y": 0.8325800440403511, - "heading": -0.3444804543055099, - "angularVelocity": 4.5554767570145907e-7, - "velocityX": 4.120020388998225, - "velocityY": -0.9418337876802539, - "timestamp": 1.8202873452475237 - }, - { - "x": 4.7904574537733415, - "y": 0.7907067300496627, - "heading": -0.34448042899819054, - "angularVelocity": 4.344603701427382e-7, - "velocityX": 4.1647168448735075, - "velocityY": -0.7188550970179598, - "timestamp": 1.8785373516318316 - }, - { - "x": 5.03495337639213, - "y": 0.7619430850125104, - "heading": -0.3444804045415982, - "angularVelocity": 4.1985561744498176e-7, - "velocityX": 4.1973544347053195, - "velocityY": -0.49379642720350075, - "timestamp": 1.9367873580161394 - }, - { - "x": 5.280642508082443, - "y": 0.7463721419648577, - "heading": -0.344480380606166, - "angularVelocity": 4.109086623403158e-7, - "velocityX": 4.217838708366216, - "velocityY": -0.2673122977004072, - "timestamp": 1.9950373644004473 - }, - { - "x": 5.5268134912780935, - "y": 0.7440374377079143, - "heading": -0.3444803568901529, - "angularVelocity": 4.071418107502121e-7, - "velocityX": 4.2261108363065745, - "velocityY": -0.04008075538292679, - "timestamp": 2.053287370784755 - }, - { - "x": 5.766738904442569, - "y": 0.7458615462006624, - "heading": -0.3246781449807404, - "angularVelocity": 0.3399520985245257, - "velocityX": 4.118890761685976, - "velocityY": 0.03131516382527713, - "timestamp": 2.111537377169063 - }, - { - "x": 5.994439434226097, - "y": 0.7477938306057788, - "heading": -0.2833891296763232, - "angularVelocity": 0.7088242193830927, - "velocityX": 3.909021542096679, - "velocityY": 0.033172260829777846, - "timestamp": 2.1697873835533708 - }, - { - "x": 6.2089250785606716, - "y": 0.7495883833677881, - "heading": -0.23844348738786494, - "angularVelocity": 0.7715989246752472, - "velocityX": 3.682156580713365, - "velocityY": 0.030807769361768905, - "timestamp": 2.2280373899376786 - }, - { - "x": 6.410164853990332, - "y": 0.7512394694030761, - "heading": -0.19387042094561185, - "angularVelocity": 0.7652027735100942, - "velocityX": 3.4547597145649904, - "velocityY": 0.02834482153349081, - "timestamp": 2.2862873963219865 - }, - { - "x": 6.598169970341291, - "y": 0.7527437994565946, - "heading": -0.15150652534345024, - "angularVelocity": 0.7272770980085961, - "velocityX": 3.227555291763987, - "velocityY": 0.025825405813585094, - "timestamp": 2.3445374027062944 - }, - { - "x": 6.772954131152115, - "y": 0.7540995948883421, - "heading": -0.11241236663435353, - "angularVelocity": 0.6711442819623177, - "velocityX": 3.000586122818176, - "velocityY": 0.02327545550471737, - "timestamp": 2.4027874090906023 - }, - { - "x": 6.934529146268428, - "y": 0.7553058862941308, - "heading": -0.07728196459640024, - "angularVelocity": 0.6030969645939325, - "velocityX": 2.773819697981016, - "velocityY": 0.020708863065698472, - "timestamp": 2.46103741547491 - }, - { - "x": 7.082904710855205, - "y": 0.7563621191254241, - "heading": -0.046606183781962664, - "angularVelocity": 0.5266227888809539, - "velocityX": 2.5472197137260486, - "velocityY": 0.018132750481171057, - "timestamp": 2.519287421859218 - }, - { - "x": 7.2180887691200715, - "y": 0.7572679503200676, - "heading": -0.02075102525586853, - "angularVelocity": 0.44386533377375426, - "velocityX": 2.3207561107029173, - "velocityY": 0.01555074841824514, - "timestamp": 2.577537428243526 + "heading": -0.2866250016379229, + "angularVelocity": 9.037238488587988e-7, + "velocityX": 3.1067845266977008, + "velocityY": -2.1566350283233997, + "timestamp": 1.3420802012037254 + }, + { + "x": 2.964172022537395, + "y": 1.4952512181839386, + "heading": -0.28662495601958665, + "angularVelocity": 7.195928192095116e-7, + "velocityX": 3.256089340971223, + "velocityY": -1.9238160187086888, + "timestamp": 1.4054748531095118 + }, + { + "x": 3.1789518158723995, + "y": 1.3887033110695066, + "heading": -0.2866249156918973, + "angularVelocity": 6.361370893313497e-7, + "velocityX": 3.3879796935268014, + "velocityY": -1.6807081340674839, + "timestamp": 1.4688695050152982 + }, + { + "x": 3.4009440542760783, + "y": 1.2981369637223683, + "heading": -0.28662487900587236, + "angularVelocity": 5.786927430417384e-7, + "velocityX": 3.501750253847766, + "velocityY": -1.4286117933375984, + "timestamp": 1.5322641569210846 + }, + { + "x": 3.6289614625982565, + "y": 1.2240364953903842, + "heading": -0.2866248447654741, + "angularVelocity": 5.401149339588049e-7, + "velocityX": 3.596792496960864, + "velocityY": -1.168875703302355, + "timestamp": 1.595658808826871 + }, + { + "x": 3.8617845298582507, + "y": 1.1667980667810436, + "heading": -0.2866248120410035, + "angularVelocity": 5.162023862741612e-7, + "velocityX": 3.672597928386812, + "velocityY": -0.9028904945231835, + "timestamp": 1.6590534607326575 + }, + { + "x": 4.0981679647792335, + "y": 1.1267270015307347, + "heading": -0.28662478005254766, + "angularVelocity": 5.045923416827531e-7, + "velocityX": 3.728759884544863, + "velocityY": -0.6320890492444092, + "timestamp": 1.7224481126384439 + }, + { + "x": 4.3358126512762265, + "y": 1.0949807955483843, + "heading": -0.28662474820722456, + "angularVelocity": 5.023345368252209e-7, + "velocityX": 3.7486551207847545, + "velocityY": -0.5007710434238176, + "timestamp": 1.7858427645442303 + }, + { + "x": 4.573457419312962, + "y": 1.06323519995946, + "heading": -0.28662471636190573, + "angularVelocity": 5.023344693474633e-7, + "velocityX": 3.7486564070090496, + "velocityY": -0.5007614149550472, + "timestamp": 1.8492374164500167 + }, + { + "x": 4.811102187354235, + "y": 1.0314896044045172, + "heading": -0.28662468451658685, + "angularVelocity": 5.02334469511756e-7, + "velocityX": 3.748656407080655, + "velocityY": -0.5007614144190154, + "timestamp": 1.9126320683558031 + }, + { + "x": 5.04874695539551, + "y": 0.9997440088495764, + "heading": -0.286624652671268, + "angularVelocity": 5.023344701827313e-7, + "velocityX": 3.748656407080659, + "velocityY": -0.5007614144189856, + "timestamp": 1.9760267202615895 + }, + { + "x": 5.286391723436784, + "y": 0.9679984132946354, + "heading": -0.2866246208259492, + "angularVelocity": 5.023344694262712e-7, + "velocityX": 3.7486564070806585, + "velocityY": -0.500761414418986, + "timestamp": 2.039421372167376 + }, + { + "x": 5.524036491478058, + "y": 0.9362528177396949, + "heading": -0.28662458898063026, + "angularVelocity": 5.023344704301764e-7, + "velocityX": 3.74865640708066, + "velocityY": -0.5007614144189806, + "timestamp": 2.1028160240731624 + }, + { + "x": 5.761681259520114, + "y": 0.9045072221906024, + "heading": -0.2866245571353113, + "angularVelocity": 5.02334470827004e-7, + "velocityX": 3.748656407092983, + "velocityY": -0.5007614143267314, + "timestamp": 2.166210675978949 + }, + { + "x": 5.99932604159473, + "y": 0.8727617316882137, + "heading": -0.28662452528999255, + "angularVelocity": 5.023344683092169e-7, + "velocityX": 3.748656628445413, + "velocityY": -0.5007597572988834, + "timestamp": 2.229605327884735 + }, + { + "x": 6.237212534637086, + "y": 0.8428813906316717, + "heading": -0.286624491558187, + "angularVelocity": 5.320922916664829e-7, + "velocityX": 3.7524694259050357, + "velocityY": -0.47133851450037906, + "timestamp": 2.2929999797905216 + }, + { + "x": 6.4638245505635705, + "y": 0.8220107849523657, + "heading": -0.24424436180847475, + "angularVelocity": 0.6685126974542769, + "velocityX": 3.5746235544169, + "velocityY": -0.32921713507194117, + "timestamp": 2.356394631696308 + }, + { + "x": 6.673647733933235, + "y": 0.8041360764949451, + "heading": -0.18514255243378552, + "angularVelocity": 0.9322838377994884, + "velocityX": 3.309793130207448, + "velocityY": -0.2819592492436893, + "timestamp": 2.4197892836020944 + }, + { + "x": 6.866141134149738, + "y": 0.7888964331390943, + "heading": -0.12766912670980685, + "angularVelocity": 0.9065973863125305, + "velocityX": 3.0364296424022634, + "velocityY": -0.24039320191392657, + "timestamp": 2.483183935507881 + }, + { + "x": 7.04134978279503, + "y": 0.7761674362974819, + "heading": -0.07636429138531158, + "angularVelocity": 0.8092927996630003, + "velocityX": 2.7637764918352543, + "velocityY": -0.20078975842520036, + "timestamp": 2.5465785874136673 + }, + { + "x": 7.19932046714385, + "y": 0.765888430525111, + "heading": -0.03338684229285238, + "angularVelocity": 0.6779349330023905, + "velocityX": 2.4918613731579065, + "velocityY": -0.16214310613530766, + "timestamp": 2.6099732393194537 }, { "x": 7.340087890625, "y": 0.7580231428146362, "heading": 0, - "angularVelocity": 0.35624073788013705, - "velocityX": 2.094405289847217, - "velocityY": 0.012964676597393893, - "timestamp": 2.635787434627834 - }, - { - "x": 7.452016076049052, - "y": 0.7586421586100276, - "heading": 0.015744593535721572, - "angularVelocity": 0.2617476388383187, - "velocityX": 1.8607618029473847, - "velocityY": 0.010290892710547365, - "timestamp": 2.6959392419396355 - }, - { - "x": 7.549878174356822, - "y": 0.7590935431078216, - "heading": 0.02631444920223928, - "angularVelocity": 0.17571966893244026, - "velocityX": 1.6269186693010698, - "velocityY": 0.007504088704338438, - "timestamp": 2.756091049251437 - }, - { - "x": 7.633665269428441, - "y": 0.7593717238160655, - "heading": 0.03212207455289435, - "angularVelocity": 0.09654947390941697, - "velocityX": 1.3929273086892113, - "velocityY": 0.004624644223938123, - "timestamp": 2.816242856563239 - }, - { - "x": 7.703370541137993, - "y": 0.7594720412293507, - "heading": 0.03350861659317102, - "angularVelocity": 0.023050712891957155, - "velocityX": 1.158822566182069, - "velocityY": 0.0016677373094563682, - "timestamp": 2.8763946638750406 - }, - { - "x": 7.758988643067764, - "y": 0.759390536759096, - "heading": 0.030761021907964082, - "angularVelocity": -0.04567767467009867, - "velocityX": 0.9246289415955516, - "velocityY": -0.001354979574134501, - "timestamp": 2.9365464711868423 - }, - { - "x": 7.800515297323124, - "y": 0.759123802858703, - "heading": 0.024124062565761484, - "angularVelocity": -0.11033682342742264, - "velocityX": 0.6903641987031875, - "velocityY": -0.004434345571870156, - "timestamp": 2.996698278498644 - }, - { - "x": 7.827947021203066, - "y": 0.7586688736183343, - "heading": 0.013809003674597087, - "angularVelocity": -0.17148377334192952, - "velocityX": 0.4560415572843554, - "velocityY": -0.007563018647310543, - "timestamp": 3.0568500858104457 + "angularVelocity": 0.5266507708326882, + "velocityX": 2.220493673351972, + "velocityY": -0.12406863156475333, + "timestamp": 2.67336789122524 + }, + { + "x": 7.464023522460179, + "y": 0.7525372143128384, + "heading": 0.023010119360387892, + "angularVelocity": 0.36178963616626764, + "velocityX": 1.9486481772395672, + "velocityY": -0.08625561847872552, + "timestamp": 2.736968716290925 + }, + { + "x": 7.57058231953554, + "y": 0.7491648354718851, + "heading": 0.03698370022131657, + "angularVelocity": 0.21970754068830903, + "velocityX": 1.6754310492876554, + "velocityY": -0.05302413667543652, + "timestamp": 2.8005695413566096 + }, + { + "x": 7.659708944521439, + "y": 0.7476928551810322, + "heading": 0.04295335113137195, + "angularVelocity": 0.09386121805637215, + "velocityX": 1.4013438488235506, + "velocityY": -0.02314404395434409, + "timestamp": 2.8641703664222944 + }, + { + "x": 7.731365798341117, + "y": 0.7479588271083956, + "heading": 0.041693436231508264, + "angularVelocity": -0.019809725716020507, + "velocityX": 1.1266654755763426, + "velocityY": 0.004181894292859409, + "timestamp": 2.927771191487979 + }, + { + "x": 7.785525986892143, + "y": 0.7498349370538775, + "heading": 0.03380693253041854, + "angularVelocity": -0.1240000219013676, + "velocityX": 0.8515642445690118, + "velocityY": 0.029498201376229376, + "timestamp": 2.991372016553664 + }, + { + "x": 7.822169492500932, + "y": 0.7532180353995639, + "heading": 0.01977708435408761, + "angularVelocity": -0.22059223542841888, + "velocityX": 0.5761482743493486, + "velocityY": 0.05319268016086978, + "timestamp": 3.0549728416193487 }, { "x": 7.841280937194824, "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": -0.22956922313268238, - "velocityX": 0.2216710783541506, - "velocityY": -0.010735019154968724, - "timestamp": 3.1170018931222474 - }, - { - "x": 7.835826711308599, - "y": 0.7569095792763894, - "heading": -0.022558535214212153, - "angularVelocity": -0.29904118534160484, - "velocityX": -0.07230248589501269, - "velocityY": -0.014761657052128235, - "timestamp": 3.19243810833268 - }, - { - "x": 7.808194016867113, - "y": 0.7554921769260273, - "heading": -0.05020545199265331, - "angularVelocity": -0.366493953883011, - "velocityX": -0.3663054192790896, - "velocityY": -0.018789414956835414, - "timestamp": 3.2678743235431122 - }, - { - "x": 7.758380379628707, - "y": 0.7537708581865793, - "heading": -0.082765542477478, - "angularVelocity": -0.4316241263429897, - "velocityX": -0.6603411517856365, - "velocityY": -0.022818201239900495, - "timestamp": 3.3433105387535447 - }, - { - "x": 7.686383023957851, - "y": 0.7517455702312359, - "heading": -0.1200350345811684, - "angularVelocity": -0.4940530486547548, - "velocityX": -0.9544136787617978, - "velocityY": -0.026847687807424973, - "timestamp": 3.418746753963977 - }, - { - "x": 7.5921988218888945, - "y": 0.7494163084029398, - "heading": -0.1617736418535849, - "angularVelocity": -0.5532966779415547, - "velocityX": -1.2485276707775705, - "velocityY": -0.030877236109982744, - "timestamp": 3.4941829691744095 - }, - { - "x": 7.475824234031671, - "y": 0.7467831468197799, - "heading": -0.2076930912246962, - "angularVelocity": -0.6087188924181447, - "velocityX": -1.5426885817719311, - "velocityY": -0.03490580188593299, - "timestamp": 3.569619184384842 - }, - { - "x": 7.337255246576761, - "y": 0.7438462777509637, - "heading": -0.25744001302414304, - "angularVelocity": -0.659456756422306, - "velocityX": -1.8369027007567336, - "velocityY": -0.03893181889658235, - "timestamp": 3.6450553995952744 - }, - { - "x": 7.17648732101356, - "y": 0.7406060606730164, - "heading": -0.3105694414767273, - "angularVelocity": -0.7042960507016083, - "velocityX": -2.1311769832928764, - "velocityY": -0.04295307060287463, - "timestamp": 3.720491614805707 - }, - { - "x": 6.9935154095251395, - "y": 0.7370630794965956, - "heading": -0.3665018755155811, - "angularVelocity": -0.7414533441640445, - "velocityX": -2.425518180863289, - "velocityY": -0.04696658185378862, - "timestamp": 3.795927830016139 - }, - { - "x": 6.788334200415596, - "y": 0.7332181989405221, - "heading": -0.4244496104660046, - "angularVelocity": -0.7681686413982437, - "velocityX": -2.719929791509048, - "velocityY": -0.050968630191055964, - "timestamp": 3.8713640452265716 - }, - { - "x": 6.5609391315868795, - "y": 0.7290725862923455, - "heading": -0.4832803161020974, - "angularVelocity": -0.7798735060074524, - "velocityX": -3.0144018783867756, - "velocityY": -0.05495520469329157, - "timestamp": 3.946800260437004 - }, - { - "x": 6.311330163631764, - "y": 0.7246275712093064, - "heading": -0.5412355988933977, - "angularVelocity": -0.7682686973310038, - "velocityX": -3.3088744876558285, - "velocityY": -0.05892415295013936, - "timestamp": 4.0222364756474365 - }, - { - "x": 6.039527438794169, - "y": 0.7198837950524889, - "heading": -0.5952481631436163, - "angularVelocity": -0.7160031040733982, - "velocityX": -3.603080086658513, - "velocityY": -0.06288459917540226, - "timestamp": 4.097672690857869 - }, - { - "x": 5.745659598268619, - "y": 0.7148372711288513, - "heading": -0.638783130826649, - "angularVelocity": -0.5771096490139386, - "velocityX": -3.8955803880906834, - "velocityY": -0.06689789393012663, - "timestamp": 4.173108906068301 - }, - { - "x": 5.431003009987887, - "y": 0.7096634209299225, - "heading": -0.6507371973275397, - "angularVelocity": -0.15846588362823302, - "velocityX": -4.171160859581617, - "velocityY": -0.06858576062566446, - "timestamp": 4.248545121278734 - }, - { - "x": 5.112561537797849, - "y": 0.7251155992021675, - "heading": -0.6507372121831845, - "angularVelocity": -1.969298784568896e-7, - "velocityX": -4.221334160280071, - "velocityY": 0.20483766622093208, - "timestamp": 4.323981336489166 - }, - { - "x": 4.795969323317984, - "y": 0.7627069594521951, - "heading": -0.6507372273638599, - "angularVelocity": -2.0123856138904834e-7, - "velocityX": -4.196819970311576, - "velocityY": 0.49831980760388084, - "timestamp": 4.399417551699599 - }, - { - "x": 4.48276376848833, - "y": 0.8222554301685094, - "heading": -0.6507372432084315, - "angularVelocity": -2.1003932248991623e-7, - "velocityX": -4.15192562293793, - "velocityY": 0.789388366717515, - "timestamp": 4.474853766910031 - }, - { - "x": 4.174465835671775, - "y": 0.9034719526361538, - "heading": -0.6507372601213973, - "angularVelocity": -2.242022056467609e-7, - "velocityX": -4.0868690450143115, - "velocityY": 1.076625096329228, - "timestamp": 4.550289982120463 - }, - { - "x": 3.872572656476166, - "y": 1.0059621755922359, - "heading": -0.650737278622818, - "angularVelocity": -2.4525913278926714e-7, - "velocityX": -4.001966142567798, - "velocityY": 1.3586342139538858, - "timestamp": 4.625726197330896 - }, - { - "x": 3.5785502607652493, - "y": 1.1292284170855964, - "heading": -0.6507372994260644, - "angularVelocity": -2.7577266985991995e-7, - "velocityX": -3.8976292075461214, - "velocityY": 1.6340459439740451, - "timestamp": 4.701162412541328 - }, - { - "x": 3.2938264573018237, - "y": 1.2726720943591394, - "heading": -0.6507373235725763, - "angularVelocity": -3.200917719298211e-7, - "velocityX": -3.7743649077459236, - "velocityY": 1.9015227218571442, - "timestamp": 4.776598627751761 - }, - { - "x": 3.0197839001768125, - "y": 1.4355966353904746, - "heading": -0.6507373526846939, - "angularVelocity": -3.859169970324026e-7, - "velocityX": -3.6327718239914044, - "velocityY": 2.1597655791299997, - "timestamp": 4.852034842962193 + "heading": 2.046055081284657e-30, + "angularVelocity": -0.310956411865142, + "velocityX": 0.30049051524338544, + "velocityY": 0.07555102327854848, + "timestamp": 3.1185736666850334 + }, + { + "x": 7.837626456422163, + "y": 0.7661832239405314, + "heading": -0.033360531548023535, + "angularVelocity": -0.41786111882102345, + "velocityX": -0.045774613098587845, + "velocityY": 0.10221002096529477, + "timestamp": 3.1984100765480727 + }, + { + "x": 7.806322741144082, + "y": 0.7764707746744537, + "heading": -0.07501113915617402, + "angularVelocity": -0.5216994060680163, + "velocityX": -0.3920982335225698, + "velocityY": 0.12885788265743264, + "timestamp": 3.278246486411112 + }, + { + "x": 7.74736425619444, + "y": 0.7888847549403224, + "heading": -0.12465356813987538, + "angularVelocity": -0.6218018704606533, + "velocityX": -0.738491185297363, + "velocityY": 0.15549271675874554, + "timestamp": 3.3580828962741514 + }, + { + "x": 7.660744366778727, + "y": 0.8034239689194149, + "heading": -0.18191677738707485, + "angularVelocity": -0.7172568173523243, + "velocityX": -1.084967241942757, + "velocityY": 0.18211257249711932, + "timestamp": 3.4379193061371907 + }, + { + "x": 7.5464550145644145, + "y": 0.8200870575241468, + "heading": -0.24632578527266594, + "angularVelocity": -0.8067623280666757, + "velocityX": -1.4315442341455176, + "velocityY": 0.20871540482992282, + "timestamp": 3.51775571600023 + }, + { + "x": 7.404486266909035, + "y": 0.8388724767291542, + "heading": -0.3172478637527482, + "angularVelocity": -0.8883425319569115, + "velocityX": -1.7782456387872365, + "velocityY": 0.23529889729803788, + "timestamp": 3.5975921258632693 + }, + { + "x": 7.234825726560123, + "y": 0.8597784078771571, + "heading": -0.3937913075224082, + "angularVelocity": -0.958753579989022, + "velocityX": -2.1251023266197273, + "velocityY": 0.2618596099682753, + "timestamp": 3.6774285357263086 + }, + { + "x": 7.037458048630302, + "y": 0.8828023770067691, + "heading": -0.474592846440749, + "angularVelocity": -1.0120888333650895, + "velocityX": -2.472151218578179, + "velocityY": 0.28838933475478196, + "timestamp": 3.757264945589348 + }, + { + "x": 6.812366694532574, + "y": 0.9079395818644894, + "heading": -0.5572950195818104, + "angularVelocity": -1.0358954427301792, + "velocityX": -2.8194072664825045, + "velocityY": 0.3148589083707988, + "timestamp": 3.837101355452387 + }, + { + "x": 6.559557151686243, + "y": 0.9351738572850871, + "heading": -0.6368741954392075, + "angularVelocity": -0.996777986308708, + "velocityX": -3.1665945810943685, + "velocityY": 0.3411260033776375, + "timestamp": 3.9169377653154265 + }, + { + "x": 6.2794552371906684, + "y": 0.9644007497338529, + "heading": -0.6963848347023115, + "angularVelocity": -0.7454072567290441, + "velocityX": -3.5084482753682624, + "velocityY": 0.3660847537972336, + "timestamp": 3.996774175178466 + }, + { + "x": 5.978414020607044, + "y": 0.9876514129078968, + "heading": -0.6963848502753711, + "angularVelocity": -1.950621226343756e-7, + "velocityX": -3.770725876828189, + "velocityY": 0.2912288167006853, + "timestamp": 4.0766105850415055 + }, + { + "x": 5.677372687056552, + "y": 1.010900561597095, + "heading": -0.6963848658464019, + "angularVelocity": -1.9503671222417687e-7, + "velocityX": -3.770727341909935, + "velocityY": 0.29120984684910667, + "timestamp": 4.156446994904545 + }, + { + "x": 5.376331353502739, + "y": 1.034149710243266, + "heading": -0.6963848814174328, + "angularVelocity": -1.9503671215554748e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.2912098463101618, + "timestamp": 4.236283404767585 + }, + { + "x": 5.075290019948924, + "y": 1.0573988588894354, + "heading": -0.6963848969884637, + "angularVelocity": -1.9503671305455908e-7, + "velocityX": -3.770727341951559, + "velocityY": 0.2912098463101465, + "timestamp": 4.316119814630625 + }, + { + "x": 4.774248686395111, + "y": 1.0806480075356062, + "heading": -0.6963849125594946, + "angularVelocity": -1.95036712978891e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.29120984631016333, + "timestamp": 4.3959562244936645 + }, + { + "x": 4.473207352844907, + "y": 1.1038971562285251, + "heading": -0.6963849281305255, + "angularVelocity": -1.9503671242042344e-7, + "velocityX": -3.770727341906336, + "velocityY": 0.29120984689571133, + "timestamp": 4.475792634356704 + }, + { + "x": 4.172166146375442, + "y": 1.1271479503662634, + "heading": -0.6963849437015556, + "angularVelocity": -1.9503670063864683e-7, + "velocityX": -3.770725750142127, + "velocityY": 0.2912304571012814, + "timestamp": 4.555629044219744 + }, + { + "x": 3.8735860999949274, + "y": 1.1720518831992321, + "heading": -0.696384959370356, + "angularVelocity": -1.9626133629188135e-7, + "velocityX": -3.7398982105123864, + "velocityY": 0.5624492998871312, + "timestamp": 4.635465454082784 + }, + { + "x": 3.580403442801602, + "y": 1.244234542261993, + "heading": -0.6963849756495779, + "angularVelocity": -2.0390723854783083e-7, + "velocityX": -3.672292600535076, + "velocityY": 0.9041320769131684, + "timestamp": 4.7153018639458235 + }, + { + "x": 3.295105029858622, + "y": 1.3430840304472003, + "heading": -0.6963849932202171, + "angularVelocity": -2.2008303364523734e-7, + "velocityX": -3.573537605616451, + "velocityY": 1.2381504673717785, + "timestamp": 4.795138273808863 + }, + { + "x": 3.020110821637561, + "y": 1.4677619739384067, + "heading": -0.6963850130039023, + "angularVelocity": -2.4780279138256264e-7, + "velocityX": -3.4444711215448884, + "velocityY": 1.5616677115753714, + "timestamp": 4.874974683671903 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.6507373865448278, - "angularVelocity": -4.4885780393316453e-7, - "velocityX": -3.473537574141072, - "velocityY": 2.407520434630195, - "timestamp": 4.9274710581726255 - }, - { - "x": 2.569555685248769, - "y": 1.762694432110511, - "heading": -0.6507374202071338, - "angularVelocity": -5.980782803410117e-7, - "velocityX": -3.3437088024696617, - "velocityY": 2.584806921874972, - "timestamp": 4.983755172029852 - }, - { - "x": 2.389174039276651, - "y": 1.9177631325165114, - "heading": -0.6507374491092287, - "angularVelocity": -5.135035974859511e-7, - "velocityX": -3.2048411818241203, - "velocityY": 2.755106010896001, - "timestamp": 5.040039285887079 - }, - { - "x": 2.2170960677982245, - "y": 2.081997759940487, - "heading": -0.6507374803842504, - "angularVelocity": -5.556633928977291e-7, - "velocityX": -3.0573097750979614, - "velocityY": 2.917957060505229, - "timestamp": 5.096323399744305 - }, - { - "x": 2.0538462988954582, - "y": 2.2548939722714945, - "heading": -0.6510332999359818, - "angularVelocity": -0.005255826759246614, - "velocityX": -2.9004590765499803, - "velocityY": 3.0718474624933285, - "timestamp": 5.152607513601532 - }, - { - "x": 1.9011340137811743, - "y": 2.4339552115414294, - "heading": -0.6596386590017012, - "angularVelocity": -0.15289143731654284, - "velocityX": -2.713239574165845, - "velocityY": 3.181381512448628, - "timestamp": 5.208891627458758 - }, - { - "x": 1.7604827039783657, - "y": 2.6146352839408076, - "heading": -0.6772772556341842, - "angularVelocity": -0.3133849930946056, - "velocityX": -2.498952193856903, - "velocityY": 3.2101433249477958, - "timestamp": 5.265175741315985 - }, - { - "x": 1.631754490870844, - "y": 2.792388921478468, - "heading": -0.7009679668217429, - "angularVelocity": -0.42091292842690614, - "velocityX": -2.2871145032870293, - "velocityY": 3.158149349007424, - "timestamp": 5.321459855173211 - }, - { - "x": 1.513917799766492, - "y": 2.964350328382096, - "heading": -0.7282264856039945, - "angularVelocity": -0.4843021754130653, - "velocityX": -2.093604803004671, - "velocityY": 3.0552387719887317, - "timestamp": 5.377743969030438 - }, - { - "x": 1.4059399900768588, - "y": 3.128858674851357, - "heading": -0.757359284956742, - "angularVelocity": -0.5176025232741039, - "velocityX": -1.9184420307928356, - "velocityY": 2.92282022750792, - "timestamp": 5.434028082887664 - }, - { - "x": 1.3069898193149363, - "y": 3.2849152261509276, - "heading": -0.787231438028103, - "angularVelocity": -0.5307386227512847, - "velocityX": -1.7580479460496543, - "velocityY": 2.77265716033891, - "timestamp": 5.490312196744891 - }, - { - "x": 1.2164244831734237, - "y": 3.431880035528415, - "heading": -0.817049536411403, - "angularVelocity": -0.5297782329653102, - "velocityX": -1.6090745671371067, - "velocityY": 2.611124157524907, - "timestamp": 5.546596310602117 - }, - { - "x": 1.133744535036132, - "y": 3.5693179916241067, - "heading": -0.8462342058218654, - "angularVelocity": -0.5185240987269294, - "velocityX": -1.4689748575774313, - "velocityY": 2.441860530030267, - "timestamp": 5.602880424459344 - }, - { - "x": 1.058555464808831, - "y": 3.696918257435487, - "heading": -0.8743465993903727, - "angularVelocity": -0.4994729710024167, - "velocityX": -1.3358844099070253, - "velocityY": 2.267074260688507, - "timestamp": 5.6591645383165705 - }, - { - "x": 0.9905398311066821, - "y": 3.8144496656298092, - "heading": -0.9010441658077244, - "angularVelocity": -0.47433573325990525, - "velocityX": -1.2084339441619472, - "velocityY": 2.088180840733473, - "timestamp": 5.715448652173797 - }, - { - "x": 0.9294376821542455, - "y": 3.921734581554844, - "heading": -0.9260527839372265, - "angularVelocity": -0.44432818455559475, - "velocityX": -1.0856020422997397, - "velocityY": 1.9061313854417152, - "timestamp": 5.771732766031024 - }, - { - "x": 0.8750327547294673, - "y": 4.018632790760157, - "heading": -0.9491484821020626, - "angularVelocity": -0.4103413304759842, - "velocityX": -0.9666124896766538, - "velocityY": 1.721590739637662, - "timestamp": 5.82801687988825 - }, - { - "x": 0.8271426027720992, - "y": 4.105031120160407, - "heading": -0.9701450202727028, - "angularVelocity": -0.37304554929835076, - "velocityX": -0.8508644566893013, - "velocityY": 1.5350393473265314, - "timestamp": 5.884300993745477 - }, - { - "x": 0.7856114088806271, - "y": 4.180836495357623, - "heading": -0.9888851979118991, - "angularVelocity": -0.33295678575901055, - "velocityX": -0.7378848318874176, - "velocityY": 1.3468343019401061, - "timestamp": 5.940585107602703 - }, - { - "x": 0.750304653010895, - "y": 4.2459711432493465, - "heading": -1.0052346087320225, - "angularVelocity": -0.2904800253513115, - "velocityX": -0.6272952250664012, - "velocityY": 1.1572474616362955, - "timestamp": 5.99686922145993 - }, - { - "x": 0.7211050890293871, - "y": 4.300369182693025, - "heading": -1.0190770490522025, - "angularVelocity": -0.24593867383776238, - "velocityX": -0.5187887306101521, - "velocityY": 0.9664901108981995, - "timestamp": 6.053153335317156 - }, - { - "x": 0.6979096587548851, - "y": 4.343974141765046, - "heading": -1.0303110716490886, - "angularVelocity": -0.19959490923820775, - "velocityX": -0.41211327113260987, - "velocityY": 0.7747294233437039, - "timestamp": 6.109437449174383 - }, - { - "x": 0.6806270896717198, - "y": 4.3767371107575626, - "heading": -1.0388473503636761, - "angularVelocity": -0.15166408653498495, - "velocityX": -0.3070594506827464, - "velocityY": 0.5820997568803311, - "timestamp": 6.165721563031609 - }, - { - "x": 0.6691759993701499, - "y": 4.398615342035701, - "heading": -1.0446066293483676, - "angularVelocity": -0.10232512497755435, - "velocityX": -0.20345155172234425, - "velocityY": 0.3887105930749016, - "timestamp": 6.222005676888836 + "heading": -0.6963850355061774, + "angularVelocity": -2.8185479639389566e-7, + "velocityX": -3.2861879673103584, + "velocityY": 1.8719390230438342, + "timestamp": 4.954811093534943 + }, + { + "x": 2.5741841985169076, + "y": 1.73878733799422, + "heading": -0.6963850589279512, + "angularVelocity": -4.0231077616908087e-7, + "velocityX": -3.153128257888759, + "velocityY": 2.0882929565548642, + "timestamp": 5.0130292054104135 + }, + { + "x": 2.3991894720533877, + "y": 1.872411165939459, + "heading": -0.6963850794155536, + "angularVelocity": -3.519111451084776e-7, + "velocityX": -3.0058468202788267, + "velocityY": 2.2952277846293376, + "timestamp": 5.071247317285884 + }, + { + "x": 2.2335584908852413, + "y": 2.017479635272907, + "heading": -0.6963850978369928, + "angularVelocity": -3.164211012595542e-7, + "velocityX": -2.845007779064253, + "velocityY": 2.491809930967045, + "timestamp": 5.129465429161355 + }, + { + "x": 2.0780383093475274, + "y": 2.173338406648706, + "heading": -0.6963851148088384, + "angularVelocity": -2.9152174806156023e-7, + "velocityX": -2.6713367460348776, + "velocityY": 2.6771526309404154, + "timestamp": 5.187683541036826 + }, + { + "x": 1.9333303562340614, + "y": 2.339284459115452, + "heading": -0.6963851307944149, + "angularVelocity": -2.745808111537906e-7, + "velocityX": -2.4856174213103777, + "velocityY": 2.850419691069807, + "timestamp": 5.245901652912297 + }, + { + "x": 1.8000872249814166, + "y": 2.514569234135199, + "heading": -0.6963851461633945, + "angularVelocity": -2.639896616028141e-7, + "velocityX": -2.288688639330214, + "velocityY": 3.0108289220145816, + "timestamp": 5.304119764787767 + }, + { + "x": 1.6789094155739503, + "y": 2.6984018293834224, + "heading": -0.6963851612308567, + "angularVelocity": -2.588105615203023e-7, + "velocityX": -2.0814451981312545, + "velocityY": 3.1576529936498847, + "timestamp": 5.362337876663238 + }, + { + "x": 1.5665736982883989, + "y": 2.8877670089616614, + "heading": -0.6963851762457395, + "angularVelocity": -2.5790741626665006e-7, + "velocityX": -1.9295664814042677, + "velocityY": 3.2526850060560912, + "timestamp": 5.420555988538709 + }, + { + "x": 1.4540456075762054, + "y": 3.0767463777174253, + "heading": -0.6971250426595835, + "angularVelocity": -0.012708526436355243, + "velocityX": -1.9328708384238367, + "velocityY": 3.246058016446739, + "timestamp": 5.47877410041418 + }, + { + "x": 1.3486317039814464, + "y": 3.2544802087721734, + "heading": -0.7253513181524348, + "angularVelocity": -0.48483666995638414, + "velocityX": -1.8106719747325546, + "velocityY": 3.052895831368142, + "timestamp": 5.53699221228965 + }, + { + "x": 1.2507109072486164, + "y": 3.4195717896549707, + "heading": -0.7601787682651484, + "angularVelocity": -0.598223628193407, + "velocityX": -1.6819644879978937, + "velocityY": 2.8357426162485435, + "timestamp": 5.595210324165121 + }, + { + "x": 1.160328320301069, + "y": 3.57195134860905, + "heading": -0.7969704460064531, + "angularVelocity": -0.6319627441714823, + "velocityX": -1.5524822780387875, + "velocityY": 2.617390946652855, + "timestamp": 5.653428436040592 + }, + { + "x": 1.0774888591394964, + "y": 3.71161238889488, + "heading": -0.8336344675120992, + "angularVelocity": -0.6297700204374731, + "velocityX": -1.422915626991949, + "velocityY": 2.398927683957996, + "timestamp": 5.711646547916063 + }, + { + "x": 1.0021908033919844, + "y": 3.838558158350536, + "heading": -0.8689739346571361, + "angularVelocity": -0.6070184347549549, + "velocityX": -1.2933785264038766, + "velocityY": 2.1805202086799973, + "timestamp": 5.7698646597915335 + }, + { + "x": 0.9344313462846833, + "y": 3.952793536595294, + "heading": -0.9022118432658017, + "angularVelocity": -0.5709204152783613, + "velocityX": -1.1638896371672067, + "velocityY": 1.9621965495739466, + "timestamp": 5.828082771667004 + }, + { + "x": 0.8742077702690397, + "y": 4.05432328906152, + "heading": -0.932802055793642, + "angularVelocity": -0.5254415085338586, + "velocityX": -1.0344474266781911, + "velocityY": 1.74395474527581, + "timestamp": 5.886300883542475 + }, + { + "x": 0.8215176865940929, + "y": 4.143151679881797, + "heading": -0.9603390876580661, + "angularVelocity": -0.47299768022924404, + "velocityX": -0.9050462472512313, + "velocityY": 1.5257861850670003, + "timestamp": 5.944518995417946 + }, + { + "x": 0.7763590444591775, + "y": 4.21928243186123, + "heading": -0.9845096003709082, + "angularVelocity": -0.4151717040316119, + "velocityX": -0.7756802939866984, + "velocityY": 1.307681570681621, + "timestamp": 6.002737107293417 + }, + { + "x": 0.7387300854642734, + "y": 4.282718776761074, + "heading": -1.0050639989905084, + "angularVelocity": -0.35305848914451915, + "velocityX": -0.6463445443815323, + "velocityY": 1.0896324675649904, + "timestamp": 6.060955219168887 + }, + { + "x": 0.7086292911290722, + "y": 4.333463522832413, + "heading": -1.0217986916878445, + "angularVelocity": -0.28744822115035373, + "velocityX": -0.5170348773863939, + "velocityY": 0.871631601173907, + "timestamp": 6.119173331044358 + }, + { + "x": 0.6860553360496396, + "y": 4.371519118466469, + "heading": -1.0345444282293665, + "angularVelocity": -0.21893077825652071, + "velocityX": -0.3877479765698806, + "velocityY": 0.6536727902728674, + "timestamp": 6.177391442919829 + }, + { + "x": 0.6710070489868055, + "y": 4.396887706764773, + "heading": -1.0431583071392123, + "angularVelocity": -0.14795874741302198, + "velocityX": -0.2584811938769611, + "velocityY": 0.43575079096636016, + "timestamp": 6.2356095547953 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -0.05172812017100072, - "velocityX": -0.10114076084111032, - "velocityY": 0.19465223880001714, - "timestamp": 6.278289790746062 + "angularVelocity": -0.07488723824993972, + "velocityX": -0.12923242394972312, + "velocityY": 0.2178611369128848, + "timestamp": 6.29382766667077 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": 6.011381134786383e-35, - "velocityY": 3.397969437810217e-36, - "timestamp": 6.334573904603289 - }, - { - "x": 0.6659136967977343, - "y": 4.396697343009468, - "heading": -1.044698875889832, - "angularVelocity": 0.048632422759591634, - "velocityX": 0.04192362717597145, - "velocityY": -0.22207715461221028, - "timestamp": 6.392543972798269 - }, - { - "x": 0.6710310256277597, - "y": 4.371001166581999, - "heading": -1.0390353111823831, - "angularVelocity": 0.09769808599154256, - "velocityX": 0.08827536329288825, - "velocityY": -0.4432662790914817, - "timestamp": 6.45051404099325 - }, - { - "x": 0.6791263180734669, - "y": 4.3325474770670445, - "heading": -1.0304995454316488, - "angularVelocity": 0.14724436276363576, - "velocityX": 0.13964607422021558, - "velocityY": -0.6633369722046352, - "timestamp": 6.50848410918823 - }, - { - "x": 0.6905315555019766, - "y": 4.281418801002382, - "heading": -1.0190608629623887, - "angularVelocity": 0.1973204935827632, - "velocityX": 0.19674355721212186, - "velocityY": -0.8819840593026762, - "timestamp": 6.566454177383211 - }, - { - "x": 0.7056282186927765, - "y": 4.217721440877341, - "heading": -1.0046856737535708, - "angularVelocity": 0.24797606172322137, - "velocityX": 0.26042169107034036, - "velocityY": -1.0987973985263735, - "timestamp": 6.624424245578191 - }, - { - "x": 0.7248578813432716, - "y": 4.141594157346286, - "heading": -0.9873376775101346, - "angularVelocity": 0.2992578201751781, - "velocityX": 0.3317170955503569, - "velocityY": -1.313217077389022, - "timestamp": 6.682394313773171 - }, - { - "x": 0.7487354042613769, - "y": 4.053220761716442, - "heading": -0.966978385263236, - "angularVelocity": 0.3512035241776958, - "velocityX": 0.41189399394518583, - "velocityY": -1.5244659594431005, - "timestamp": 6.740364381968152 - }, - { - "x": 0.7778651202320954, - "y": 3.9528486632227207, - "heading": -0.9435683140016388, - "angularVelocity": 0.4038303212419584, - "velocityX": 0.5024958030537675, - "velocityY": -1.7314469625276876, - "timestamp": 6.798334450163132 - }, - { - "x": 0.8129599622873139, - "y": 3.840816527291042, - "heading": -0.9170694264777192, - "angularVelocity": 0.4571132715385337, - "velocityX": 0.6053959077153058, - "velocityY": -1.932585891651215, - "timestamp": 6.856304518358113 - }, - { - "x": 0.854862177504241, - "y": 3.717595744583205, - "heading": -0.8874498393217566, - "angularVelocity": 0.510946218940748, - "velocityX": 0.7228250116247967, - "velocityY": -2.1255931991210963, - "timestamp": 6.914274586553093 - }, - { - "x": 0.904560986315067, - "y": 3.5838520361766597, - "heading": -0.8546925510341115, - "angularVelocity": 0.5650724469991482, - "velocityX": 0.8573184465415072, - "velocityY": -2.307116630546341, - "timestamp": 6.972244654748073 - }, - { - "x": 0.9631953960515734, - "y": 3.4405334281831, - "heading": -0.8188108593180419, - "angularVelocity": 0.6189692859318806, - "velocityX": 1.0114600786614798, - "velocityY": -2.4722863445927272, - "timestamp": 7.030214722943054 - }, - { - "x": 1.0320177450129453, - "y": 3.288983389093817, - "heading": -0.7798733754078085, - "angularVelocity": 0.671682561753563, - "velocityX": 1.18720489908499, - "velocityY": -2.6142808488606364, - "timestamp": 7.088184791138034 - }, - { - "x": 1.1122806132492202, - "y": 3.131051679274452, - "heading": -0.7380383985495225, - "angularVelocity": 0.721665131004769, - "velocityX": 1.3845570780823164, - "velocityY": -2.724366465952162, - "timestamp": 7.146154859333015 - }, - { - "x": 1.2050235127281292, - "y": 2.9691249632164602, - "heading": -0.6935876595861372, - "angularVelocity": 0.7667877638831885, - "velocityX": 1.5998411312363996, - "velocityY": -2.7932814485115998, - "timestamp": 7.204124927527995 - }, - { - "x": 1.3108176746302633, - "y": 2.8059720420366694, - "heading": -0.6469378343676327, - "angularVelocity": 0.8047226210188198, - "velocityX": 1.8249790831071382, - "velocityY": -2.814433832146465, - "timestamp": 7.2620949957229755 - }, - { - "x": 1.42962965449819, - "y": 2.6444006527037383, - "heading": -0.5986101263227683, - "angularVelocity": 0.8336665722440691, - "velocityX": 2.0495401086696328, - "velocityY": -2.7871519624488035, - "timestamp": 7.320065063917956 - }, - { - "x": 1.5609092304168717, - "y": 2.4869058396466244, - "heading": -0.5491623613282449, - "angularVelocity": 0.8529878700195282, - "velocityX": 2.2646096512622123, - "velocityY": -2.7168298737787384, - "timestamp": 7.378035132112936 - }, - { - "x": 1.7038131780379508, - "y": 2.335492698782744, - "heading": -0.4991191344613761, - "angularVelocity": 0.863259754992006, - "velocityX": 2.4651333364043317, - "velocityY": -2.6119193159236365, - "timestamp": 7.436005200307917 - }, - { - "x": 1.8574111825796424, - "y": 2.19168017622197, - "heading": -0.44893240803487616, - "angularVelocity": 0.8657351628033007, - "velocityX": 2.649608829595809, - "velocityY": -2.4808065099572545, - "timestamp": 7.493975268502897 - }, - { - "x": 2.020808784771142, - "y": 2.056591090907756, - "heading": -0.3989735055068982, - "angularVelocity": 0.8618051364014737, - "velocityX": 2.8186546484975037, - "velocityY": -2.3303247610447158, - "timestamp": 7.5519453366978775 - }, - { - "x": 2.1931999690795068, - "y": 1.9310536028968317, - "heading": -0.3495416314047453, - "angularVelocity": 0.8527137476514646, - "velocityX": 2.973796472492188, - "velocityY": -2.165557017954213, - "timestamp": 7.609915404892858 - }, - { - "x": 2.3738798200885083, - "y": 1.815684583131589, - "heading": -0.3008771196490498, - "angularVelocity": 0.8394765310955621, - "velocityX": 3.1167783070616735, - "velocityY": -1.9901480774043983, - "timestamp": 7.667885473087838 - }, - { - "x": 2.5622392362768553, - "y": 1.7109506397451384, - "heading": -0.253174013671076, - "angularVelocity": 0.8228920107101786, - "velocityX": 3.249252968873622, - "velocityY": -1.8066900151675016, - "timestamp": 7.725855541282819 + "angularVelocity": -7.364462273467936e-32, + "velocityX": -8.426727085890477e-35, + "velocityY": -7.327805300908389e-34, + "timestamp": 6.352045778546241 + }, + { + "x": 0.6687542994396359, + "y": 4.396916362187497, + "heading": -1.0350125956919525, + "angularVelocity": 0.2204003026897336, + "velocityX": 0.09289604490221867, + "velocityY": -0.22303166776742134, + "timestamp": 6.408785741736511 + }, + { + "x": 0.6793565646010321, + "y": 4.371607473302922, + "heading": -1.0104451675948027, + "angularVelocity": 0.4329827993501856, + "velocityX": 0.18685710327027932, + "velocityY": -0.4460504988292779, + "timestamp": 6.465525704926781 + }, + { + "x": 0.6953596015342451, + "y": 4.333646306767502, + "heading": -0.974331857670356, + "angularVelocity": 0.6364704503481122, + "velocityX": 0.2820417221553139, + "velocityY": -0.6690375601429702, + "timestamp": 6.522265668117051 + }, + { + "x": 0.7168434967725004, + "y": 4.283036502515157, + "heading": -0.9272707047547418, + "angularVelocity": 0.8294181079709282, + "velocityX": 0.3786378071168583, + "velocityY": -0.8919604703061096, + "timestamp": 6.5790056313073215 + }, + { + "x": 0.7439015517455688, + "y": 4.2197848874550585, + "heading": -0.8699519201955506, + "angularVelocity": 1.0102012996903078, + "velocityX": 0.4768782609592592, + "velocityY": -1.114763061230607, + "timestamp": 6.635745594497592 + }, + { + "x": 0.7766442528059767, + "y": 4.143903696796417, + "heading": -0.8031709855452435, + "angularVelocity": 1.1769647157923862, + "velocityX": 0.5770659552705253, + "velocityY": -1.3373500156174554, + "timestamp": 6.692485557687862 + }, + { + "x": 0.8152053931020986, + "y": 4.055414010177326, + "heading": -0.7278520306671347, + "angularVelocity": 1.3274410246890034, + "velocityX": 0.6796116551364678, + "velocityY": -1.559565456931146, + "timestamp": 6.749225520878132 + }, + { + "x": 0.8597512927978146, + "y": 3.9543509740987406, + "heading": -0.6450962807712558, + "angularVelocity": 1.4585090515192565, + "velocityX": 0.7850886252135366, + "velocityY": -1.7811614670894944, + "timestamp": 6.805965484068402 + }, + { + "x": 0.9104943772436287, + "y": 3.840772171273386, + "heading": -0.5562797497368643, + "angularVelocity": 1.5653258486713548, + "velocityX": 0.8943094354089278, + "velocityY": -2.0017426244088754, + "timestamp": 6.862705447258672 + }, + { + "x": 0.9677131149809225, + "y": 3.7147727983516208, + "heading": -0.46323645073147074, + "angularVelocity": 1.6398195165087535, + "velocityX": 1.0084380482486026, + "velocityY": -2.2206460109824513, + "timestamp": 6.919445410448942 + }, + { + "x": 1.0317820620609501, + "y": 3.576517638239885, + "heading": -0.3685846112186535, + "angularVelocity": 1.6681688564973882, + "velocityX": 1.1291679352201949, + "velocityY": -2.436645220373414, + "timestamp": 6.976185373639212 + }, + { + "x": 1.1032183238971194, + "y": 3.4263179326907856, + "heading": -0.2763023679431359, + "angularVelocity": 1.6264064706221368, + "velocityX": 1.2590114236876868, + "velocityY": -2.64715902344566, + "timestamp": 7.032925336829482 + }, + { + "x": 1.1827463587467486, + "y": 3.2648378432524847, + "heading": -0.1927401719055838, + "angularVelocity": 1.4727220699339676, + "velocityX": 1.4016229545821517, + "velocityY": -2.8459674691151595, + "timestamp": 7.089665300019752 + }, + { + "x": 1.2713975131080564, + "y": 3.0937718159398226, + "heading": -0.12815970051626668, + "angularVelocity": 1.1381831738726118, + "velocityX": 1.562411206789599, + "velocityY": -3.014912553591437, + "timestamp": 7.146405263210022 + }, + { + "x": 1.3706976001434645, + "y": 2.9168916321975784, + "heading": -0.09351951840137682, + "angularVelocity": 0.610507659279375, + "velocityX": 1.7500907905494714, + "velocityY": -3.117382772158287, + "timestamp": 7.203145226400292 + }, + { + "x": 1.4825331259415389, + "y": 2.737551336103052, + "heading": -0.084031813825935, + "angularVelocity": 0.1672137950394149, + "velocityX": 1.9710186526390192, + "velocityY": -3.1607404377956887, + "timestamp": 7.2598851895905625 + }, + { + "x": 1.6073609242840539, + "y": 2.563006200971128, + "heading": -0.08403157292142222, + "angularVelocity": 0.000004245764347455466, + "velocityX": 2.199997873173106, + "velocityY": -3.0762292627263235, + "timestamp": 7.316625152780833 + }, + { + "x": 1.7433400400906298, + "y": 2.3970011200993575, + "heading": -0.08403150231700539, + "angularVelocity": 0.0000012443507689652163, + "velocityX": 2.3965316182984986, + "velocityY": -2.925717105509803, + "timestamp": 7.373365115971103 + }, + { + "x": 1.889887864003564, + "y": 2.2402473630588187, + "heading": -0.08403142284388228, + "angularVelocity": 0.0000014006551755862827, + "velocityX": 2.5827973032253166, + "velocityY": -2.7626693467333516, + "timestamp": 7.430105079161373 + }, + { + "x": 2.046376539489404, + "y": 2.0934165181715283, + "heading": -0.08403133019563887, + "angularVelocity": 0.0000016328569530158415, + "velocityX": 2.757997479855154, + "velocityY": -2.58778533914292, + "timestamp": 7.486845042351643 + }, + { + "x": 2.2121356179273546, + "y": 1.9571376609651308, + "heading": -0.08403121735664319, + "angularVelocity": 0.00000198870406896459, + "velocityX": 2.921381494064391, + "velocityY": -2.401814339381988, + "timestamp": 7.543585005541913 + }, + { + "x": 2.3864549305613383, + "y": 1.8319946594051495, + "heading": -0.08403107170612155, + "angularVelocity": 0.0000025669830126630976, + "velocityX": 3.0722493077661266, + "velocityY": -2.2055530973879893, + "timestamp": 7.600324968732183 + }, + { + "x": 2.5685876291674634, + "y": 1.7185236734546225, + "heading": -0.08403086720089675, + "angularVelocity": 0.000003604253744603396, + "velocityX": 3.2099544723948052, + "velocityY": -1.9998424315154402, + "timestamp": 7.657064931922453 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.20659037596839774, - "angularVelocity": 0.8035808677332508, - "velocityX": 3.3726738988458935, - "velocityY": -1.6170375099283227, - "timestamp": 7.783825609477799 - }, - { - "x": 2.954766850408611, - "y": 1.5365275146757538, - "heading": -0.16235575982643646, - "angularVelocity": 0.7825616402901594, - "velocityX": 3.48539682535889, - "velocityY": -1.4273820029873086, - "timestamp": 7.840351017655946 - }, - { - "x": 3.157939243604707, - "y": 1.4666859169715192, - "heading": -0.11962489482799478, - "angularVelocity": 0.7559585392779498, - "velocityX": 3.594355171320012, - "velocityY": -1.2355788300390564, - "timestamp": 7.896876425834093 - }, - { - "x": 3.3670172328271493, - "y": 1.4078228373530197, - "heading": -0.07876775443844967, - "angularVelocity": 0.7228101787567607, - "velocityX": 3.698832011323254, - "velocityY": -1.0413561178184736, - "timestamp": 7.95340183401224 - }, - { - "x": 3.5816948765446446, - "y": 1.3600926772574, - "heading": -0.04022391942526907, - "angularVelocity": 0.6818851248575686, - "velocityX": 3.797896390963022, - "velocityY": -0.8444018651787949, - "timestamp": 8.009927242190386 - }, - { - "x": 3.8015965172009225, - "y": 1.323669626598405, - "heading": -0.004523858036694332, - "angularVelocity": 0.6315754726805621, - "velocityX": 3.890314952936408, - "velocityY": -0.6443659910283791, - "timestamp": 8.066452650368532 - }, - { - "x": 4.026252074416622, - "y": 1.298748947880293, - "heading": 0.02768066779446809, - "angularVelocity": 0.5697353963312556, - "velocityX": 3.9744172480395035, - "velocityY": -0.44087569681180167, - "timestamp": 8.122978058546678 - }, - { - "x": 4.255060536192153, - "y": 1.2855455255338215, - "heading": 0.05557184744847584, - "angularVelocity": 0.49342730203920493, - "velocityX": 4.0478869441227845, - "velocityY": -0.23358384790180253, - "timestamp": 8.179503466724825 - }, - { - "x": 4.487235082115286, - "y": 1.284285135117098, - "heading": 0.07809760377772143, - "angularVelocity": 0.39850674334368213, - "velocityX": 4.107436874960846, - "velocityY": -0.02229776762957918, - "timestamp": 8.23602887490297 - }, - { - "x": 4.721720600054119, - "y": 1.2951774473138997, - "heading": 0.0938646398689849, - "angularVelocity": 0.27893714701841205, - "velocityX": 4.148320649004834, - "velocityY": 0.19269763010774343, - "timestamp": 8.292554283081117 - }, - { - "x": 4.957338422985755, - "y": 1.31837138535252, - "heading": 0.1023163328663219, - "angularVelocity": 0.14952024708429268, - "velocityX": 4.168352436997136, - "velocityY": 0.4103276523987602, - "timestamp": 8.349079691259263 - }, - { - "x": 5.192892039115628, - "y": 1.3539608804787848, - "heading": 0.10486380159502258, - "angularVelocity": 0.04506767506520281, - "velocityX": 4.16721654423966, - "velocityY": 0.6296194273219583, - "timestamp": 8.405605099437409 - }, - { - "x": 5.426922141496893, - "y": 1.4019164045228902, - "heading": 0.10486553946063551, - "angularVelocity": 0.0000307448573823463, - "velocityX": 4.140263819832846, - "velocityY": 0.8483888146896288, - "timestamp": 8.462130507615555 - }, - { - "x": 5.657546782205124, - "y": 1.4618615011178113, - "heading": 0.10700306943729049, - "angularVelocity": 0.037815383303704875, - "velocityX": 4.080017254919927, - "velocityY": 1.0604982525026048, - "timestamp": 8.518655915793701 - }, - { - "x": 5.881407902963978, - "y": 1.532219235085926, - "heading": 0.11413761051896783, - "angularVelocity": 0.12621830273550538, - "velocityX": 3.9603627461358366, - "velocityY": 1.2447098788985973, - "timestamp": 8.575181323971847 - }, - { - "x": 6.0948633682195945, - "y": 1.6093965656168685, - "heading": 0.12496970465048783, - "angularVelocity": 0.19163230272272128, - "velocityX": 3.7762746371133757, - "velocityY": 1.3653564479836866, - "timestamp": 8.631706732149993 - }, - { - "x": 6.296160249007556, - "y": 1.6893086925404435, - "heading": 0.13798616586845183, - "angularVelocity": 0.23027628879637688, - "velocityX": 3.5611751825577227, - "velocityY": 1.41373816659089, - "timestamp": 8.68823214032814 - }, - { - "x": 6.484990958449558, - "y": 1.7688506297276971, - "heading": 0.1520046167268183, - "angularVelocity": 0.24800264713145628, - "velocityX": 3.3406341595425215, - "velocityY": 1.4071890809981862, - "timestamp": 8.744757548506286 - }, - { - "x": 6.661585058300773, - "y": 1.8459701168111895, - "heading": 0.1662218219427335, - "angularVelocity": 0.25151884213039066, - "velocityX": 3.124154350104949, - "velocityY": 1.364333130340974, - "timestamp": 8.801282956684432 - }, - { - "x": 6.826289993460413, - "y": 1.9193152685714405, - "heading": 0.18009164346035805, - "angularVelocity": 0.24537322178925483, - "velocityX": 2.9138212437237407, - "velocityY": 1.2975607629244343, - "timestamp": 8.857808364862578 - }, - { - "x": 6.979441082007995, - "y": 1.9879614203209928, - "heading": 0.1932294988279346, - "angularVelocity": 0.23242389203402142, - "velocityX": 2.7094203029000408, - "velocityY": 1.2144300052324348, - "timestamp": 8.914333773040724 - }, - { - "x": 7.121330748513538, - "y": 2.0512482937052026, - "heading": 0.20535415779786798, - "angularVelocity": 0.21449927317147402, - "velocityX": 2.510192691724767, - "velocityY": 1.1196181579928337, - "timestamp": 8.97085918121887 - }, - { - "x": 7.25220694864691, - "y": 2.1086857599712796, - "heading": 0.21625288773093435, - "angularVelocity": 0.19281116730228098, - "velocityX": 2.3153517038019378, - "velocityY": 1.0161353649151172, - "timestamp": 9.027384589397016 - }, - { - "x": 7.372278984681028, - "y": 2.1598978527910453, - "heading": 0.22576002013784088, - "angularVelocity": 0.1681921937997094, - "velocityX": 2.124213515729, - "velocityY": 0.9060012916379937, - "timestamp": 9.083909997575162 + "heading": -0.08403051576382121, + "angularVelocity": 0.000006193819237400721, + "velocityX": 3.3339066927233554, + "velocityY": -1.7855635206200389, + "timestamp": 7.713804895112723 + }, + { + "x": 2.9656624873675215, + "y": 1.523823440642391, + "heading": -0.08403020563552344, + "angularVelocity": 0.0000051460695851667095, + "velocityX": 3.449910187925088, + "velocityY": -1.5496108793286638, + "timestamp": 7.7740699756075236 + }, + { + "x": 3.179557704009846, + "y": 1.4451070785194118, + "heading": -0.08403001029754076, + "angularVelocity": 0.0000032413128973800796, + "velocityX": 3.5492397070767696, + "velocityY": -1.306168704607808, + "timestamp": 7.834335056102324 + }, + { + "x": 3.3984052007772028, + "y": 1.3814422389869163, + "heading": -0.0840298717822531, + "angularVelocity": 0.0000022984336290020716, + "velocityX": 3.631414659542976, + "velocityY": -1.0564134156924954, + "timestamp": 7.894600136597124 + }, + { + "x": 3.6211472313814954, + "y": 1.3331366287340103, + "heading": -0.08402976608446186, + "angularVelocity": 0.000001753881192375512, + "velocityX": 3.6960380501525916, + "velocityY": -0.8015522398094749, + "timestamp": 7.954865217091925 + }, + { + "x": 3.846707230492903, + "y": 1.3004237167488812, + "heading": -0.08402968105378755, + "angularVelocity": 0.0000014109443416028125, + "velocityX": 3.7427976078264336, + "velocityY": -0.5428170296387713, + "timestamp": 8.015130297586726 + }, + { + "x": 4.073995015295796, + "y": 1.2834616061839175, + "heading": -0.08402960975207145, + "angularVelocity": 0.0000011831348355128416, + "velocityX": 3.7714673727599504, + "velocityY": -0.2814583574052856, + "timestamp": 8.075395378081527 + }, + { + "x": 4.301912054355543, + "y": 1.282332269070522, + "heading": -0.08402954787131725, + "angularVelocity": 0.0000010268094506712622, + "velocityX": 3.78190881333695, + "velocityY": -0.018739493984294066, + "timestamp": 8.135660458576329 + }, + { + "x": 4.529356777936335, + "y": 1.2970411469267544, + "heading": -0.08402949255215464, + "angularVelocity": 9.179306183548953e-7, + "velocityX": 3.7740715139410477, + "velocityY": 0.2440696624889034, + "timestamp": 8.19592553907113 + }, + { + "x": 4.755229905146536, + "y": 1.32751711580047, + "heading": -0.08402944177914254, + "angularVelocity": 8.424947198098048e-7, + "velocityX": 3.7479934541809494, + "velocityY": 0.5056986338273413, + "timestamp": 8.256190619565931 + }, + { + "x": 4.978439767588294, + "y": 1.373612800948183, + "heading": -0.08402939404329389, + "angularVelocity": 7.920979819424461e-7, + "velocityX": 3.7038009508842378, + "velocityY": 0.7648821634228096, + "timestamp": 8.316455700060732 + }, + { + "x": 5.197907642868858, + "y": 1.435105133457297, + "heading": -0.08402934813807361, + "angularVelocity": 7.617217119968608e-7, + "velocityX": 3.6417088217363065, + "velocityY": 1.0203642308985088, + "timestamp": 8.376720780555534 + }, + { + "x": 5.41257465992254, + "y": 1.51169186312203, + "heading": -0.08402930302434115, + "angularVelocity": 7.485882719571497e-7, + "velocityX": 3.562046466895548, + "velocityY": 1.2708309527826855, + "timestamp": 8.436985861050335 + }, + { + "x": 5.626738902818376, + "y": 1.589673479175761, + "heading": -0.0840292579538456, + "angularVelocity": 7.478708262765028e-7, + "velocityX": 3.5537037557647206, + "velocityY": 1.2939768007189265, + "timestamp": 8.497250941545136 + }, + { + "x": 5.8409031120271635, + "y": 1.6676551877456105, + "heading": -0.0840292128833514, + "angularVelocity": 7.478708040156826e-7, + "velocityX": 3.5537031967834927, + "velocityY": 1.2939783358719235, + "timestamp": 8.557516022039938 + }, + { + "x": 6.055067321252931, + "y": 1.745636896268829, + "heading": -0.08402916781285703, + "angularVelocity": 7.478708068018038e-7, + "velocityX": 3.5537031970652393, + "velocityY": 1.2939783350981522, + "timestamp": 8.617781102534739 + }, + { + "x": 6.269231822230628, + "y": 1.8236178035375321, + "heading": -0.08402912274221092, + "angularVelocity": 7.478733249018271e-7, + "velocityX": 3.5537080382091872, + "velocityY": 1.2939650395958724, + "timestamp": 8.67804618302954 + }, + { + "x": 6.484552473362613, + "y": 1.8927933712391167, + "heading": -0.0785738032137305, + "angularVelocity": 0.09052206491205224, + "velocityX": 3.572892450555367, + "velocityY": 1.1478548959633867, + "timestamp": 8.738311263524341 + }, + { + "x": 6.6874523544155435, + "y": 1.9584473392764923, + "heading": -0.032553192185028265, + "angularVelocity": 0.7636364317587296, + "velocityX": 3.3667901774467204, + "velocityY": 1.0894197352485022, + "timestamp": 8.798576344019143 + }, + { + "x": 6.875915672715089, + "y": 2.018652626020107, + "heading": 0.028144572860875648, + "angularVelocity": 1.0071796892586995, + "velocityX": 3.1272391366971615, + "velocityY": 0.9990078209355214, + "timestamp": 8.858841424513944 + }, + { + "x": 7.049532373282227, + "y": 2.073323850727038, + "heading": 0.08948102802542592, + "angularVelocity": 1.0177777024597514, + "velocityX": 2.8808839072589665, + "velocityY": 0.9071791534676087, + "timestamp": 8.919106505008745 + }, + { + "x": 7.208331847820926, + "y": 2.1225088918275805, + "heading": 0.1460274817443076, + "angularVelocity": 0.9382954980664153, + "velocityX": 2.635016384859868, + "velocityY": 0.8161449498899447, + "timestamp": 8.979371585503547 + }, + { + "x": 7.352376622997373, + "y": 2.1662517609495984, + "heading": 0.19476972653717045, + "angularVelocity": 0.8087974726436816, + "velocityX": 2.3901863897597297, + "velocityY": 0.72584104696901, + "timestamp": 9.039636665998348 }, { "x": 7.481724262237549, "y": 2.2045881748199463, "heading": 0.2337432969576531, - "angularVelocity": 0.14123342187378718, - "velocityX": 1.9362138387677101, - "velocityY": 0.7906236057253049, - "timestamp": 9.140435405753308 - }, - { - "x": 7.586144701324698, - "y": 2.244425533220676, - "heading": 0.24038146155956527, - "angularVelocity": 0.11057598681783708, - "velocityX": 1.739395418529896, - "velocityY": 0.6635953582873915, - "timestamp": 9.200468011321169 - }, - { - "x": 7.678582360185057, - "y": 2.2769030569359834, - "heading": 0.24515554508543944, - "angularVelocity": 0.07952484288688075, - "velocityX": 1.5397908850694146, - "velocityY": 0.5409980694340338, - "timestamp": 9.26050061688903 - }, - { - "x": 7.7589012203408085, - "y": 2.3022472705839294, - "heading": 0.2480460603725282, - "angularVelocity": 0.04814908931149622, - "velocityX": 1.3379206082427928, - "velocityY": 0.42217414033940376, - "timestamp": 9.32053322245689 - }, - { - "x": 7.826988761652501, - "y": 2.320653171453341, - "heading": 0.24903661097421825, - "angularVelocity": 0.016500210049526117, - "velocityX": 1.1341760143115402, - "velocityY": 0.30659840090741436, - "timestamp": 9.38056582802475 - }, - { - "x": 7.882750526752601, - "y": 2.332290238336933, - "heading": 0.2481131524247365, - "angularVelocity": -0.015382616508921566, - "velocityX": 0.9288579859667665, - "velocityY": 0.19384577386762514, - "timestamp": 9.44059843359261 - }, - { - "x": 7.926106207329853, - "y": 2.337307039185365, - "heading": 0.24526345998864965, - "angularVelocity": -0.047469077997382, - "velocityX": 0.7222022127332522, - "velocityY": 0.08356793447456733, - "timestamp": 9.50063103916047 - }, - { - "x": 7.956986763254213, - "y": 2.3358348134459193, - "heading": 0.2404767337370615, - "angularVelocity": -0.07973544053784674, - "velocityX": 0.5143963956295903, - "velocityY": -0.02452376880063506, - "timestamp": 9.560663644728331 + "angularVelocity": 0.6467023706015796, + "velocityX": 2.146311565141514, + "velocityY": 0.6361298044504436, + "timestamp": 9.099901746493149 + }, + { + "x": 7.603777233505286, + "y": 2.239565945400217, + "heading": 0.2627306680277325, + "angularVelocity": 0.44772760939813167, + "velocityX": 1.8851825132237892, + "velocityY": 0.540252980034664, + "timestamp": 9.164645064666104 + }, + { + "x": 7.70877923669531, + "y": 2.2685458571337813, + "heading": 0.28047750188422, + "angularVelocity": 0.2741106628035178, + "velocityX": 1.621819921393637, + "velocityY": 0.4476123954003698, + "timestamp": 9.229388382839058 + }, + { + "x": 7.796631047750002, + "y": 2.291686366419086, + "heading": 0.28817114093339696, + "angularVelocity": 0.11883294317143692, + "velocityX": 1.3569247535321998, + "velocityY": 0.3574192663942211, + "timestamp": 9.294131701012013 + }, + { + "x": 7.867260828377704, + "y": 2.309110963970804, + "heading": 0.2867085865097153, + "angularVelocity": -0.022590044269504023, + "velocityX": 1.0909199994819943, + "velocityY": 0.2691335267242617, + "timestamp": 9.358875019184968 + }, + { + "x": 7.920614376230513, + "y": 2.320918360587503, + "heading": 0.2767910958513383, + "angularVelocity": -0.15318168636157245, + "velocityX": 0.824078057140675, + "velocityY": 0.1823724354867997, + "timestamp": 9.423618337357922 + }, + { + "x": 7.956649448780364, + "y": 2.327189146390105, + "heading": 0.25898192892634386, + "angularVelocity": -0.27507343502876075, + "velocityX": 0.5565836532132435, + "velocityY": 0.09685610777394495, + "timestamp": 9.488361655530877 }, { "x": 7.975332260131836, "y": 2.3279902935028076, "heading": 0.2337432969576531, - "angularVelocity": -0.11216299402158979, - "velocityX": 0.3055922144989234, - "velocityY": -0.13067098902186064, - "timestamp": 9.620696250296191 - }, - { - "x": 7.978374325239172, - "y": 2.3079982956389222, - "heading": 0.2220767476081581, - "angularVelocity": -0.15342778134763177, - "velocityX": 0.04000645659240227, - "velocityY": -0.2629164618495627, - "timestamp": 9.696735604101754 - }, - { - "x": 7.9611015630205095, - "y": 2.2781926707436404, - "heading": 0.20738275399488582, - "angularVelocity": -0.19324195798462207, - "velocityX": -0.2271555629316675, - "velocityY": -0.3919763044212194, - "timestamp": 9.772774957907316 - }, - { - "x": 7.923380278914192, - "y": 2.238852677448683, - "heading": 0.18978782188047655, - "angularVelocity": -0.23139244659286162, - "velocityX": -0.49607581099088477, - "velocityY": -0.5173635930093835, - "timestamp": 9.848814311712879 - }, - { - "x": 7.865060691710785, - "y": 2.19030356664395, - "heading": 0.16943867046954186, - "angularVelocity": -0.2676134184802368, - "velocityX": -0.7669658444564572, - "velocityY": -0.6384734795205669, - "timestamp": 9.924853665518441 - }, - { - "x": 7.785974313825651, - "y": 2.132928607276668, - "heading": 0.14650757608805504, - "angularVelocity": -0.30156876977312563, - "velocityX": -1.04007167245744, - "velocityY": -0.7545429635553378, - "timestamp": 10.000893019324003 - }, - { - "x": 7.685930994427759, - "y": 2.067185547524282, - "heading": 0.12119962682948068, - "angularVelocity": -0.33282699013051165, - "velocityX": -1.3156781901870174, - "velocityY": -0.8645925624314937, - "timestamp": 10.076932373129566 - }, - { - "x": 7.564715810849877, - "y": 1.9936296675417498, - "heading": 0.09376277438448252, - "angularVelocity": -0.3608243767451793, - "velocityX": -1.5941111741669691, - "velocityY": -0.9673396248292669, - "timestamp": 10.152971726935128 - }, - { - "x": 7.422086372418718, - "y": 1.912946891741318, - "heading": 0.06450207380614308, - "angularVelocity": -0.3848099584481087, - "velocityX": -1.8757318584778, - "velocityY": -1.0610660370252898, - "timestamp": 10.22901108074069 - }, - { - "x": 7.2577720289721634, - "y": 1.8260026827906595, - "heading": 0.03380034441899183, - "angularVelocity": -0.4037610507008996, - "velocityX": -2.160911886057286, - "velocityY": -1.1434106761740845, - "timestamp": 10.305050434546253 - }, - { - "x": 7.0714787796722804, - "y": 1.7339163522481107, - "heading": 0.002148903485203126, - "angularVelocity": -0.41625078791073866, - "velocityX": -2.4499583436262102, - "velocityY": -1.2110351539548843, - "timestamp": 10.381089788351815 - }, - { - "x": 6.8629095324946725, - "y": 1.6381770650545604, - "heading": -0.029805628870875043, - "angularVelocity": -0.4202367689471438, - "velocityX": -2.7429118836403266, - "velocityY": -1.259075497121691, - "timestamp": 10.457129142157378 - }, - { - "x": 6.631824574679955, - "y": 1.5408277464126163, - "heading": -0.06118892010520367, - "angularVelocity": -0.4127243284389006, - "velocityX": -3.0390179065121625, - "velocityY": -1.2802491574411938, - "timestamp": 10.53316849596294 - }, - { - "x": 6.378206231452605, - "y": 1.444749972258303, - "heading": -0.09078814079587186, - "angularVelocity": -0.3892618651962165, - "velocityX": -3.335356371858044, - "velocityY": -1.26352696789074, - "timestamp": 10.609207849768502 - }, - { - "x": 6.102679405785972, - "y": 1.3540414004691264, - "heading": -0.11690114137793724, - "angularVelocity": -0.343414288459606, - "velocityX": -3.6234766851277205, - "velocityY": -1.1929161315747718, - "timestamp": 10.685247203574065 - }, - { - "x": 5.807422454316563, - "y": 1.2742067055061217, - "heading": -0.1372503142523278, - "angularVelocity": -0.26761370074796764, - "velocityX": -3.8829492452605625, - "velocityY": -1.0499128539038807, - "timestamp": 10.761286557379627 - }, - { - "x": 5.497293853879703, - "y": 1.211233933640027, - "heading": -0.14936074429275475, - "angularVelocity": -0.15926529401333597, - "velocityX": -4.078527563896464, - "velocityY": -0.8281602711553835, - "timestamp": 10.83732591118519 - }, - { - "x": 5.179311523814037, - "y": 1.1692856386862889, - "heading": -0.15140362446925906, - "angularVelocity": -0.026866090705190943, - "velocityX": -4.181812629270475, - "velocityY": -0.5516655896498385, - "timestamp": 10.913365264990752 - }, - { - "x": 4.858542014963219, - "y": 1.1497329672294159, - "heading": -0.1514046806841633, - "angularVelocity": -0.000013890371910813042, - "velocityX": -4.21846705419206, - "velocityY": -0.2571388429584845, - "timestamp": 10.989404618796314 - }, - { - "x": 4.538318225528884, - "y": 1.1527274796290288, - "heading": -0.15538772135714082, - "angularVelocity": -0.052381306174200754, - "velocityX": -4.211290251797366, - "velocityY": 0.03938108689442748, - "timestamp": 11.065443972601877 - }, - { - "x": 4.221401569033501, - "y": 1.177956926117284, - "heading": -0.16806437438072017, - "angularVelocity": -0.1667117405546922, - "velocityX": -4.167797865638882, - "velocityY": 0.33179459353072166, - "timestamp": 11.141483326407439 - }, - { - "x": 3.910015091156467, - "y": 1.2249289706320923, - "heading": -0.19130842148638338, - "angularVelocity": -0.30568443762817654, - "velocityX": -4.095070016945054, - "velocityY": 0.6177333467998585, - "timestamp": 11.217522680213001 - }, - { - "x": 3.606469436454312, - "y": 1.2929813536267714, - "heading": -0.22267226856626002, - "angularVelocity": -0.41246861671228957, - "velocityX": -3.9919546854427725, - "velocityY": 0.8949626685241646, - "timestamp": 11.293562034018564 - }, - { - "x": 3.3124526239021637, - "y": 1.3814436970961244, - "heading": -0.26028249186707797, - "angularVelocity": -0.49461524090525394, - "velocityX": -3.8666400730333566, - "velocityY": 1.163375792166213, - "timestamp": 11.369601387824126 - }, - { - "x": 3.0292255135853545, - "y": 1.48970128529521, - "heading": -0.302685251705683, - "angularVelocity": -0.5576422959489076, - "velocityX": -3.724743782555543, - "velocityY": 1.4237047368380822, - "timestamp": 11.445640741629688 + "angularVelocity": -0.38982604971324913, + "velocityX": 0.2885674055438917, + "velocityY": 0.012374205328225838, + "timestamp": 9.553104973703832 + }, + { + "x": 7.972208451820267, + "y": 2.320885188130861, + "heading": 0.19238106068409147, + "angularVelocity": -0.5227816803364386, + "velocityX": -0.039482143745094074, + "velocityY": -0.08980217850764118, + "timestamp": 9.632224494967359 + }, + { + "x": 7.9431197679460075, + "y": 2.3056970040477323, + "heading": 0.14088004342725263, + "angularVelocity": -0.6509268058549238, + "velocityX": -0.3676549530345614, + "velocityY": -0.19196506551829257, + "timestamp": 9.711344016230886 + }, + { + "x": 7.888054782175393, + "y": 2.282427289475609, + "heading": 0.07969868788669623, + "angularVelocity": -0.7732776255909873, + "velocityX": -0.695972180964121, + "velocityY": -0.2941083843849025, + "timestamp": 9.790463537494412 + }, + { + "x": 7.806999942498959, + "y": 2.251078456184475, + "heading": 0.009399898258955654, + "angularVelocity": -0.8885138396324784, + "velocityX": -1.0244606941750847, + "velocityY": -0.39622122063553056, + "timestamp": 9.86958305875794 + }, + { + "x": 7.699938927127902, + "y": 2.2116544317191673, + "heading": -0.06930512925958275, + "angularVelocity": -0.9947611697041394, + "velocityX": -1.353155500201559, + "velocityY": -0.49828441623143704, + "timestamp": 9.948702580021466 + }, + { + "x": 7.56685172715319, + "y": 2.1641617607796966, + "heading": -0.15547855671843705, + "angularVelocity": -1.089155066697531, + "velocityX": -1.682103200946216, + "velocityY": -0.6002648926714905, + "timestamp": 10.027822101284993 + }, + { + "x": 7.407713494064462, + "y": 2.1086115226344657, + "heading": -0.24780538645569025, + "angularVelocity": -1.1669285691167814, + "velocityX": -2.011364964642267, + "velocityY": -0.702105337066011, + "timestamp": 10.10694162254852 + }, + { + "x": 7.222493943520295, + "y": 2.045023086580113, + "heading": -0.3442810817864959, + "angularVelocity": -1.2193665202986854, + "velocityX": -2.3410094953335943, + "velocityY": -0.8037009708710834, + "timestamp": 10.186061143812047 + }, + { + "x": 7.0111623875747595, + "y": 1.9734335261753837, + "heading": -0.4414827777687788, + "angularVelocity": -1.2285425193427022, + "velocityX": -2.6710418942202843, + "velocityY": -0.9048280280448349, + "timestamp": 10.265180665075574 + }, + { + "x": 6.773734962048751, + "y": 1.893931948844204, + "heading": -0.5324651154460041, + "angularVelocity": -1.1499353917244444, + "velocityX": -3.0008703507595227, + "velocityY": -1.0048288470601288, + "timestamp": 10.344300186339101 + }, + { + "x": 6.510837595414292, + "y": 1.8068437015263887, + "heading": -0.5976366951472135, + "angularVelocity": -0.8237104909184032, + "velocityX": -3.3227876311184024, + "velocityY": -1.1007175716817783, + "timestamp": 10.423419707602628 + }, + { + "x": 6.227373100797986, + "y": 1.7110085398670838, + "heading": -0.597636758184297, + "angularVelocity": -7.967323658608173e-7, + "velocityX": -3.5827377376583796, + "velocityY": -1.2112707474568936, + "timestamp": 10.502539228866155 + }, + { + "x": 5.941831354312318, + "y": 1.6215523108704284, + "heading": -0.5976367779578059, + "angularVelocity": -2.499194721356179e-7, + "velocityX": -3.6089923438060088, + "velocityY": -1.1306467426502582, + "timestamp": 10.581658750129682 + }, + { + "x": 5.656289545112644, + "y": 1.532096282055343, + "heading": -0.5976367977313131, + "angularVelocity": -2.499194483006569e-7, + "velocityX": -3.6089931364549517, + "velocityY": -1.130644212534215, + "timestamp": 10.660778271393209 + }, + { + "x": 5.370747722448539, + "y": 1.442640296218458, + "heading": -0.5976368175048198, + "angularVelocity": -2.499194443736624e-7, + "velocityX": -3.6089933066333257, + "velocityY": -1.130643669328191, + "timestamp": 10.739897792656736 + }, + { + "x": 5.084751687474933, + "y": 1.3546472631644115, + "heading": -0.5976368372766676, + "angularVelocity": -2.4989847693362694e-7, + "velocityX": -3.6147341440682292, + "velocityY": -1.112153254327249, + "timestamp": 10.819017313920263 + }, + { + "x": 4.791925527807106, + "y": 1.2930896001142946, + "heading": -0.5976368572220272, + "angularVelocity": -2.52091508667939e-7, + "velocityX": -3.7010608126974813, + "velocityY": -0.7780338160172059, + "timestamp": 10.89813683518379 + }, + { + "x": 4.494706489260304, + "y": 1.2584868813594625, + "heading": -0.5976368780658494, + "angularVelocity": -2.63447274854176e-7, + "velocityX": -3.756582873610133, + "velocityY": -0.4373474232683845, + "timestamp": 10.977256356447317 + }, + { + "x": 4.195570491935266, + "y": 1.2511276025347224, + "heading": -0.597636900694303, + "angularVelocity": -2.860034177860552e-7, + "velocityX": -3.780811518420208, + "velocityY": -0.09301470366874595, + "timestamp": 11.056375877710844 + }, + { + "x": 3.897009479956395, + "y": 1.2710731343763437, + "heading": -0.597636926337917, + "angularVelocity": -3.24112350243062e-7, + "velocityX": -3.7735442177972423, + "velocityY": 0.2520936871595517, + "timestamp": 11.13549539897437 + }, + { + "x": 3.6015106196507847, + "y": 1.3181573465478027, + "heading": -0.5976369569136154, + "angularVelocity": -3.8644948741127607e-7, + "velocityX": -3.7348413588269285, + "velocityY": 0.5951023390881358, + "timestamp": 11.214614920237898 + }, + { + "x": 3.311535572783971, + "y": 1.3919880157013045, + "heading": -0.5976369957684174, + "angularVelocity": -4.910899536256082e-7, + "velocityX": -3.6650252963611574, + "velocityY": 0.933153638627185, + "timestamp": 11.293734441501424 + }, + { + "x": 3.029499988052695, + "y": 1.4919500996437052, + "heading": -0.5976370495734, + "angularVelocity": -6.80046867328716e-7, + "velocityX": -3.5646775944445412, + "velocityY": 1.263431354816361, + "timestamp": 11.372853962764951 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.34872944068091694, - "angularVelocity": -0.6055310397951567, - "velocityX": -3.5701531878761683, - "velocityY": 1.676889312494585, - "timestamp": 11.52168009543525 - }, - { - "x": 2.5541569719261092, - "y": 1.727757046354288, - "heading": -0.38618529156646786, - "angularVelocity": -0.6332589444448175, - "velocityX": -3.442165602360439, - "velocityY": 1.8689832549148022, - "timestamp": 11.580827859891052 - }, - { - "x": 2.3587219086882643, - "y": 1.8492493966793417, - "heading": -0.4251400494897362, - "angularVelocity": -0.658600680544372, - "velocityX": -3.304183430024454, - "velocityY": 2.0540480513991684, - "timestamp": 11.639975624346853 - }, - { - "x": 2.172129075786119, - "y": 1.9811469849343935, - "heading": -0.4654182614046572, - "angularVelocity": -0.6809760653763983, - "velocityX": -3.1546895240914963, - "velocityY": 2.2299674293457925, - "timestamp": 11.699123388802654 - }, - { - "x": 1.995157304922952, - "y": 2.1227357753454896, - "heading": -0.5067996859573998, - "angularVelocity": -0.6996278715430715, - "velocityX": -2.9920280587343844, - "velocityY": 2.3938147403170578, - "timestamp": 11.758271153258455 - }, - { - "x": 1.8286811439598407, - "y": 2.273062802113803, - "heading": -0.549006464745395, - "angularVelocity": -0.7135819785637939, - "velocityX": -2.8145807790844684, - "velocityY": 2.5415504398420463, - "timestamp": 11.817418917714257 - }, - { - "x": 1.673644229855571, - "y": 2.430852006094566, - "heading": -0.5916896444353141, - "angularVelocity": -0.7216363979709688, - "velocityX": -2.6211796089119246, - "velocityY": 2.6677120501937903, - "timestamp": 11.876566682170058 - }, - { - "x": 1.5309859321329615, - "y": 2.594413834176532, - "heading": -0.634420502020555, - "angularVelocity": -0.7224424790758148, - "velocityX": -2.411896696944723, - "velocityY": 2.7653087075538, - "timestamp": 11.935714446625859 - }, - { - "x": 1.4014991059285755, - "y": 2.7615886598812227, - "heading": -0.6766978032526942, - "angularVelocity": -0.7147742881090955, - "velocityX": -2.189209134034951, - "velocityY": 2.826392970940027, - "timestamp": 11.99486221108166 - }, - { - "x": 1.2856293975453275, - "y": 2.9297974775958746, - "heading": -0.7179836021581544, - "angularVelocity": -0.6980111469185274, - "velocityX": -1.9589871138719785, - "velocityY": 2.8438744771216182, - "timestamp": 12.054009975537461 - }, - { - "x": 1.1833007632826107, - "y": 3.0962581686871435, - "heading": -0.7577647191568426, - "angularVelocity": -0.6725717762066157, - "velocityX": -1.7300507500868296, - "velocityY": 2.814319232904572, - "timestamp": 12.113157739993262 - }, - { - "x": 1.0938911269423066, - "y": 3.2583116792220825, - "heading": -0.7956106235538323, - "angularVelocity": -0.6398535049498092, - "velocityX": -1.5116317102249486, - "velocityY": 2.7398078697638226, - "timestamp": 12.172305504449064 - }, - { - "x": 1.016385611128122, - "y": 3.4136921446071793, - "heading": -0.8311990080013264, - "angularVelocity": -0.601686044687091, - "velocityX": -1.3103710094081817, - "velocityY": 2.62698796505164, - "timestamp": 12.231453268904865 - }, - { - "x": 0.9495982361573998, - "y": 3.5606291449139458, - "heading": -0.864309152764572, - "angularVelocity": -0.5597869178637827, - "velocityX": -1.1291614414375832, - "velocityY": 2.4842359074546003, - "timestamp": 12.290601033360666 - }, - { - "x": 0.8923465849102798, - "y": 3.697809371688421, - "heading": -0.8947997322814718, - "angularVelocity": -0.5154984266511812, - "velocityX": -0.9679427747417668, - "velocityY": 2.319279993700968, - "timestamp": 12.349748797816467 - }, - { - "x": 0.8435455769066035, - "y": 3.8242806095123454, - "heading": -0.9225850102577191, - "angularVelocity": -0.46976040822321335, - "velocityX": -0.8250693572729036, - "velocityY": 2.138225155042558, - "timestamp": 12.408896562272268 - }, - { - "x": 0.8022404458446034, - "y": 3.939354876295257, - "heading": -0.947615738236735, - "angularVelocity": -0.42318975551004434, - "velocityX": -0.698337992010946, - "velocityY": 1.9455387340785029, - "timestamp": 12.46804432672807 - }, - { - "x": 0.7676071594331643, - "y": 4.0425313748152725, - "heading": -0.9698657760492436, - "angularVelocity": -0.3761771559284466, - "velocityX": -0.5855383839116994, - "velocityY": 1.7443854297674437, - "timestamp": 12.52719209118387 - }, - { - "x": 0.7389393229135627, - "y": 4.133440839855761, - "heading": -0.9893233232577529, - "angularVelocity": -0.32896504859535863, - "velocityX": -0.4846816575971237, - "velocityY": 1.5369890286964785, - "timestamp": 12.586339855639672 - }, - { - "x": 0.7156313779093327, - "y": 4.21180687471968, - "heading": -1.0059853372092005, - "angularVelocity": -0.28170149970585456, - "velocityX": -0.39406299153786883, - "velocityY": 1.324919641256768, - "timestamp": 12.645487620095473 - }, - { - "x": 0.6971623458837157, - "y": 4.277419392368828, - "heading": -1.0198540109194538, - "angularVelocity": -0.23447502771836987, - "velocityX": -0.31225241047644936, - "velocityY": 1.1092983522340676, - "timestamp": 12.704635384551274 - }, - { - "x": 0.6830816107540019, - "y": 4.330116323499302, - "heading": -1.0309345469547266, - "angularVelocity": -0.1873365145279998, - "velocityX": -0.23806030978966433, - "velocityY": 0.8909369883261355, - "timestamp": 12.763783149007075 - }, - { - "x": 0.6729970259294025, - "y": 4.3697708967031526, - "heading": -1.039233740627151, - "angularVelocity": -0.14031288838694966, - "velocityX": -0.17049815690219822, - "velocityY": 0.6704323243439457, - "timestamp": 12.822930913462876 - }, - { - "x": 0.6665651633052443, - "y": 4.396282678408227, - "heading": -1.0447590694290692, - "angularVelocity": -0.09341568278623903, - "velocityX": -0.10874227763865159, - "velocityY": 0.448229649066216, - "timestamp": 12.882078677918678 + "heading": -0.5976371219829064, + "angularVelocity": -9.151914125801383e-7, + "velocityX": -3.434634228323893, + "velocityY": 1.583184066038948, + "timestamp": 11.451973484028478 + }, + { + "x": 2.562615182382705, + "y": 1.7237517111402614, + "heading": -0.5976372044989885, + "angularVelocity": -0.000001403654433041047, + "velocityX": -3.319432744203681, + "velocityY": 1.8123319353797018, + "timestamp": 11.510760091514479 + }, + { + "x": 2.3751466995867516, + "y": 1.8432734201994623, + "heading": -0.5976372636960788, + "angularVelocity": -0.000001006982590926024, + "velocityX": -3.188965834447897, + "velocityY": 2.033145203823246, + "timestamp": 11.56954669900048 + }, + { + "x": 2.1962100914661193, + "y": 1.9752263102479175, + "heading": -0.5976373093137471, + "angularVelocity": -7.759874278787006e-7, + "velocityX": -3.04383286896162, + "velocityY": 2.2446080100791628, + "timestamp": 11.62833330648648 + }, + { + "x": 2.026628285101079, + "y": 2.1190035289778706, + "heading": -0.5976373463774368, + "angularVelocity": -6.304784584021761e-7, + "velocityX": -2.884701356604495, + "velocityY": 2.4457478476571914, + "timestamp": 11.68711991397248 + }, + { + "x": 1.8671811829950287, + "y": 2.2739438442752706, + "heading": -0.5976373777665476, + "angularVelocity": -5.339500279584693e-7, + "velocityX": -2.7123031745627153, + "velocityY": 2.6356396792296386, + "timestamp": 11.745906521458481 + }, + { + "x": 1.7186020763048488, + "y": 2.439334685025019, + "heading": -0.5976374052703787, + "angularVelocity": -4.678587919148875e-7, + "velocityX": -2.5274312134028762, + "velocityY": 2.813410193625064, + "timestamp": 11.804693128944482 + }, + { + "x": 1.5815742720495027, + "y": 2.6144154181152923, + "heading": -0.597637430079118, + "angularVelocity": -4.220134537614776e-7, + "velocityX": -2.330935737157126, + "velocityY": 2.978241823734581, + "timestamp": 11.863479736430483 + }, + { + "x": 1.456727954325337, + "y": 2.7983808405830617, + "heading": -0.5976374777093088, + "angularVelocity": -8.102217978593512e-7, + "velocityX": -2.123720402710731, + "velocityY": 3.129376406209191, + "timestamp": 11.922266343916483 + }, + { + "x": 1.3455561973341574, + "y": 2.9885105140912174, + "heading": -0.6039795556256176, + "angularVelocity": -0.10788303982023785, + "velocityX": -1.8911068650739042, + "velocityY": 3.234234490456602, + "timestamp": 11.981052951402484 + }, + { + "x": 1.246887053690835, + "y": 3.1733277865183314, + "heading": -0.6383468335999772, + "angularVelocity": -0.584610669743856, + "velocityX": -1.6784289460285728, + "velocityY": 3.1438669508378774, + "timestamp": 12.039839558888485 + }, + { + "x": 1.158565022655154, + "y": 3.3474027501759167, + "heading": -0.6816254514815488, + "angularVelocity": -0.736198595775039, + "velocityX": -1.5024175541464038, + "velocityY": 2.9611330046395477, + "timestamp": 12.098626166374485 + }, + { + "x": 1.0790333486785466, + "y": 3.5092497722088574, + "heading": -0.7278331515121171, + "angularVelocity": -0.7860242665231638, + "velocityX": -1.3528876282841755, + "velocityY": 2.7531274375968073, + "timestamp": 12.157412773860486 + }, + { + "x": 1.0074427197106477, + "y": 3.6582801720807683, + "heading": -0.7741804064083878, + "angularVelocity": -0.7883981892867038, + "velocityX": -1.217805075500369, + "velocityY": 2.5351080160120225, + "timestamp": 12.216199381346486 + }, + { + "x": 0.9432732996874974, + "y": 3.7941912605500616, + "heading": -0.8190469180938782, + "angularVelocity": -0.7632097446033984, + "velocityX": -1.0915652861654275, + "velocityY": 2.3119396454653414, + "timestamp": 12.274985988832487 + }, + { + "x": 0.8861770840921692, + "y": 3.9168021213870996, + "heading": -0.8613760150853045, + "angularVelocity": -0.7200466024767106, + "velocityX": -0.971245289310589, + "velocityY": 2.085693767347223, + "timestamp": 12.333772596318488 + }, + { + "x": 0.8359056858624994, + "y": 4.025993630774074, + "heading": -0.9004245404660403, + "angularVelocity": -0.6642418579781981, + "velocityX": -0.8551505245755496, + "velocityY": 1.857421512424882, + "timestamp": 12.392559203804488 + }, + { + "x": 0.7922732558571624, + "y": 4.121681912181887, + "heading": -0.9356409495537121, + "angularVelocity": -0.5990549649604897, + "velocityX": -0.7422171795800264, + "velocityY": 1.627722460946589, + "timestamp": 12.451345811290489 + }, + { + "x": 0.7551356458246514, + "y": 4.203804939460416, + "heading": -0.9665991512691937, + "angularVelocity": -0.5266199741642557, + "velocityX": -0.6317358939509348, + "velocityY": 1.3969683026544202, + "timestamp": 12.51013241877649 + }, + { + "x": 0.7243778479811152, + "y": 4.272315098350931, + "heading": -0.9929596716344966, + "angularVelocity": -0.44841030113160957, + "velocityX": -0.5232109686013307, + "velocityY": 1.1654041935798078, + "timestamp": 12.56891902626249 + }, + { + "x": 0.6999059919554825, + "y": 4.327174746467568, + "heading": -1.0144454156380707, + "angularVelocity": -0.3654870543208486, + "velocityX": -0.41628284182686576, + "velocityY": 0.9331997620325617, + "timestamp": 12.62770563374849 + }, + { + "x": 0.6816420121384978, + "y": 4.368353408748977, + "heading": -1.0308257653496888, + "angularVelocity": -0.27864084035669345, + "velocityX": -0.31068266392705546, + "velocityY": 0.7004769290559099, + "timestamp": 12.686492241234491 + }, + { + "x": 0.6695199611090151, + "y": 4.395825923599214, + "heading": -1.0419057024338247, + "angularVelocity": -0.1884772324508513, + "velocityX": -0.2062042963164626, + "velocityY": 0.46732608029439393, + "timestamp": 12.745278848720492 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -0.046646417662750905, - "velocityX": -0.052103102496543485, - "velocityY": 0.22466601267048775, - "timestamp": 12.941226442374479 + "angularVelocity": -0.09547069579077468, + "velocityX": -0.1026863106378516, + "velocityY": 0.23381596243572259, + "timestamp": 12.804065456206493 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -2.054852244630119e-40, - "velocityX": -3.6573844759845184e-41, - "velocityY": 1.4664679602509001e-41, - "timestamp": 13.00037420683028 + "angularVelocity": -7.821518056781921e-37, + "velocityX": -6.302822662985753e-37, + "velocityY": 5.041148502478119e-38, + "timestamp": 12.862852063692493 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubSprint.1.traj b/src/main/deploy/choreo/SourceLanePSubHSubSprint.1.traj index 82c21a47..e72c3131 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubSprint.1.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubSprint.1.traj @@ -5,945 +5,900 @@ "y": 4.409571170806885, "heading": -1.0475181007536924, "angularVelocity": 0, - "velocityX": 6.43793008966589e-32, - "velocityY": 2.60583916914808e-32, + "velocityX": 2.1342037757525677e-32, + "velocityY": 1.1024409975331184e-32, "timestamp": 0 }, { - "x": 0.6686935342644011, - "y": 4.399440637290449, - "heading": -1.042618248425991, - "angularVelocity": 0.09045075655350086, - "velocityX": 0.09617887406842093, - "velocityY": -0.18700857894671152, - "timestamp": 0.05417149081338269 - }, - { - "x": 0.6791748314486301, - "y": 4.379207946886432, - "heading": -1.0329282708117224, - "angularVelocity": 0.17887596351464607, - "velocityX": 0.19348363921414355, - "velocityY": -0.37349332832129434, - "timestamp": 0.10834298162676538 - }, - { - "x": 0.6949946213377666, - "y": 4.348905028536486, - "heading": -1.0185695824101737, - "angularVelocity": 0.2650598716401143, - "velocityX": 0.29203165081121235, - "velocityY": -0.5593886728046312, - "timestamp": 0.16251447244014808 - }, - { - "x": 0.7162276537845403, - "y": 4.3085680597239815, - "heading": -0.9996772134807529, - "angularVelocity": 0.34875113543586445, - "velocityX": 0.39195953679621004, - "velocityY": -0.7446161847651872, - "timestamp": 0.21668596325353076 - }, - { - "x": 0.742957361589069, - "y": 4.258238351873032, - "heading": -0.9764021807302357, - "angularVelocity": 0.42965464677164156, - "velocityX": 0.4934275834610263, - "velocityY": -0.9290810921990941, - "timestamp": 0.2708574540669135 - }, - { - "x": 0.775277449766474, - "y": 4.197963491949002, - "heading": -0.9489144431022858, - "angularVelocity": 0.507420733954753, - "velocityX": 0.5966254148098966, - "velocityY": -1.1126675492774045, - "timestamp": 0.32502894488029616 - }, - { - "x": 0.8132938881778625, - "y": 4.1277988368888145, - "heading": -0.9174066544487739, - "angularVelocity": 0.5816304513762527, - "velocityX": 0.7017794386045728, - "velocityY": -1.295232123145776, - "timestamp": 0.37920043569367884 - }, - { - "x": 0.8571274399080208, - "y": 4.047809503293585, - "heading": -0.8820990289920404, - "angularVelocity": 0.6517750375075625, - "velocityX": 0.8091627361920356, - "velocityY": -1.4765946514336408, - "timestamp": 0.4333719265070615 - }, - { - "x": 0.9069169109204649, - "y": 3.958073067611011, - "heading": -0.8432457931383238, - "angularVelocity": 0.7172266310255936, - "velocityX": 0.919108377208329, - "velocityY": -1.6565251266890444, - "timestamp": 0.4875434173204442 - }, - { - "x": 0.9628233844277805, - "y": 3.8586833111714633, - "heading": -0.8011439375928472, - "angularVelocity": 0.7771958074869153, - "velocityX": 1.0320275973188553, - "velocityY": -1.8347244084889383, - "timestamp": 0.541714908133827 - }, - { - "x": 1.0250358185457515, - "y": 3.7497555457988567, - "heading": -0.7561453551591197, - "angularVelocity": 0.8306690799547085, - "velocityX": 1.148434964293097, - "velocityY": -2.010795046196093, - "timestamp": 0.5958863989472096 - }, - { - "x": 1.0937785557910409, - "y": 3.6314344082082353, - "heading": -0.7086740357677194, - "angularVelocity": 0.876315543076626, - "velocityX": 1.2689836704347575, - "velocityY": -2.184195705416893, - "timestamp": 0.6500578897605923 - }, - { - "x": 1.1693215384166689, - "y": 3.503905652509146, - "heading": -0.65925094255443, - "angularVelocity": 0.9123450817247932, - "velocityX": 1.39451548206175, - "velocityY": -2.354167363390805, - "timestamp": 0.704229380573975 - }, - { - "x": 1.2519943506884508, - "y": 3.3674146850873483, - "heading": -0.6085307966283009, - "angularVelocity": 0.9362885378373057, - "velocityX": 1.5261313844321562, - "velocityY": -2.519608845398026, - "timestamp": 0.7584008713873577 - }, - { - "x": 1.3422055318396924, - "y": 3.2222969888664816, - "heading": -0.5573578262997513, - "angularVelocity": 0.9446476284885243, - "velocityX": 1.6652888779083848, - "velocityY": -2.6788573480613262, - "timestamp": 0.8125723622007404 - }, - { - "x": 1.440468425966884, - "y": 3.0690305293119637, - "heading": -0.5068529144587157, - "angularVelocity": 0.9323153393548251, - "velocityX": 1.8139226491975393, - "velocityY": -2.8292826587053166, - "timestamp": 0.866743853014123 - }, - { - "x": 1.5474319674576553, - "y": 2.908330661716264, - "heading": -0.4585565550407312, - "angularVelocity": 0.8915456948445868, - "velocityX": 1.9745356807560286, - "velocityY": -2.9665025861905914, - "timestamp": 0.9209153438275057 - }, - { - "x": 1.66389994755801, - "y": 2.7413290540117057, - "heading": -0.41468584741790443, - "angularVelocity": 0.8098486300470928, - "velocityX": 2.1499866138367625, - "velocityY": -3.0828320431473393, - "timestamp": 0.9750868346408884 - }, - { - "x": 1.7907579832157325, - "y": 2.5699020484114805, - "heading": -0.3786367444287472, - "angularVelocity": 0.6654626344573777, - "velocityX": 2.3417859422540332, - "velocityY": -3.1645244209870533, - "timestamp": 1.029258325454271 - }, - { - "x": 1.9285625356473743, - "y": 2.397072387394727, - "heading": -0.3552615550516524, - "angularVelocity": 0.4315035275218978, - "velocityX": 2.5438574859674787, - "velocityY": -3.1904172918581897, - "timestamp": 1.083429816267654 - }, - { - "x": 2.0769698348954466, - "y": 2.226810347381072, - "heading": -0.3460632427651378, - "angularVelocity": 0.16979987348331044, - "velocityX": 2.739583072567186, - "velocityY": -3.1430192792772944, - "timestamp": 1.1376013070810367 - }, - { - "x": 2.2352281984049327, - "y": 2.0620956413219225, - "heading": -0.3444813887227629, - "angularVelocity": 0.029200858581211304, - "velocityX": 2.9214326785776583, - "velocityY": -3.0406160802659317, - "timestamp": 1.1917727978944195 - }, - { - "x": 2.401905636235413, - "y": 1.90514203890439, - "heading": -0.3444812653381511, - "angularVelocity": 0.0000022776669023009663, - "velocityX": 3.076847901503659, - "velocityY": -2.8973469266007075, - "timestamp": 1.2459442887078023 - }, - { - "x": 2.5762262115041326, - "y": 1.7567232171949336, - "heading": -0.34448120853135994, - "angularVelocity": 0.0000010486473662298194, - "velocityX": 3.217939411511546, - "velocityY": -2.739795775987586, - "timestamp": 1.300115779521185 + "x": 0.6705320996855912, + "y": 4.398099342118358, + "heading": -1.0386114127976833, + "angularVelocity": 0.15927551181553457, + "velocityX": 0.12605002427557185, + "velocityY": -0.20514712032687507, + "timestamp": 0.05592000838348859 + }, + { + "x": 0.6846335258864488, + "y": 4.375148415401169, + "heading": -1.0210111049652704, + "angularVelocity": 0.3147407938803157, + "velocityX": 0.25217138924859706, + "velocityY": -0.41042423598715294, + "timestamp": 0.11184001676697718 + }, + { + "x": 0.7057920861320249, + "y": 4.340710150429397, + "heading": -0.9949634737692402, + "angularVelocity": 0.4658016325283909, + "velocityX": 0.3783719076090775, + "velocityY": -0.6158487090273922, + "timestamp": 0.16776002515046576 + }, + { + "x": 0.7340126708459819, + "y": 4.294775101740391, + "heading": -0.9607567098648513, + "angularVelocity": 0.6117088479280168, + "velocityX": 0.5046598798845962, + "velocityY": -0.8214420923185943, + "timestamp": 0.22368003353395435 + }, + { + "x": 0.7693006604451206, + "y": 4.237332365746024, + "heading": -0.9187315737842261, + "angularVelocity": 0.7515223494321577, + "velocityX": 0.6310440684690272, + "velocityY": -1.0272304610620715, + "timestamp": 0.2796000419174429 + }, + { + "x": 0.8116619611973516, + "y": 4.168369327121192, + "heading": -0.8692948358062461, + "angularVelocity": 0.8840617054087766, + "velocityX": 0.7575338769931041, + "velocityY": -1.2332444257142752, + "timestamp": 0.3355200503009315 + }, + { + "x": 0.8611030812908155, + "y": 4.087871426229515, + "heading": -0.8129373545696884, + "angularVelocity": 1.0078231900479868, + "velocityX": 0.8841400694078276, + "velocityY": -1.4395187557848361, + "timestamp": 0.3914400586844201 + }, + { + "x": 0.9176312901451578, + "y": 3.995821968070834, + "heading": -0.7502611156342618, + "angularVelocity": 1.120819555419328, + "velocityX": 1.0108762585778377, + "velocityY": -1.6460916373156267, + "timestamp": 0.4473600670679087 + }, + { + "x": 0.9812548770105831, + "y": 3.8922019971306105, + "heading": -0.6820246305048668, + "angularVelocity": 1.220251697057024, + "velocityX": 1.1377606818136927, + "velocityY": -1.8530034943775027, + "timestamp": 0.5032800754513973 + }, + { + "x": 1.0519833618359966, + "y": 3.7769903402374796, + "heading": -0.6092267013635415, + "angularVelocity": 1.3018225720227237, + "velocityX": 1.2648153473148889, + "velocityY": -2.0602939846330393, + "timestamp": 0.5592000838348858 + }, + { + "x": 1.1298269082953458, + "y": 3.6501643798944903, + "heading": -0.5332724156478241, + "angularVelocity": 1.3582667083101572, + "velocityX": 1.392051766614792, + "velocityY": -2.26798893650447, + "timestamp": 0.6151200922183744 + }, + { + "x": 1.2147919549002646, + "y": 3.511704401985981, + "heading": -0.45632702327380176, + "angularVelocity": 1.375990358340898, + "velocityX": 1.5194033238021896, + "velocityY": -2.4760364297333126, + "timestamp": 0.6710401006018629 + }, + { + "x": 1.3068601679390515, + "y": 3.3616165956403266, + "heading": -0.38215505844470493, + "angularVelocity": 1.3263940219829704, + "velocityX": 1.6464270249639619, + "velocityY": -2.683973244717373, + "timestamp": 0.7269601089853515 + }, + { + "x": 1.4058792704184795, + "y": 3.2000739494751453, + "heading": -0.3184323584737973, + "angularVelocity": 1.139533090444293, + "velocityX": 1.7707276043375095, + "velocityY": -2.8888165584195344, + "timestamp": 0.78288011736884 + }, + { + "x": 1.512169170978474, + "y": 3.0300749520186443, + "heading": -0.28662541962876376, + "angularVelocity": 0.5687935278354703, + "velocityX": 1.900749009747616, + "velocityY": -3.0400388406718526, + "timestamp": 0.8388001257523285 + }, + { + "x": 1.6244650109400058, + "y": 2.850864532661705, + "heading": -0.2866253125429214, + "angularVelocity": 0.0000019149825876044375, + "velocityX": 2.008151343458836, + "velocityY": -3.2047638141959607, + "timestamp": 0.8947201341358171 + }, + { + "x": 1.7352882055291936, + "y": 2.6707396888206167, + "heading": -0.2866252783553297, + "angularVelocity": 6.113659974508405e-7, + "velocityX": 1.9818164873864779, + "velocityY": -3.221116180917336, + "timestamp": 0.9506401425193056 + }, + { + "x": 1.8506362591615508, + "y": 2.493478394875359, + "heading": -0.2866252441530604, + "angularVelocity": 6.116284725625113e-7, + "velocityX": 2.0627331248114724, + "velocityY": -3.169908214777688, + "timestamp": 1.0065601509027942 + }, + { + "x": 1.9771720781105002, + "y": 2.324022128559085, + "heading": -0.2866252095630558, + "angularVelocity": 6.185622200419991e-7, + "velocityX": 2.2628004288052184, + "velocityY": -3.0303333496335663, + "timestamp": 1.0624801592862827 + }, + { + "x": 2.114370117744969, + "y": 2.1630766803808767, + "heading": -0.28662517392184755, + "angularVelocity": 6.373605669269748e-7, + "velocityX": 2.4534695827223696, + "velocityY": -2.8781370538157938, + "timestamp": 1.1184001676697712 + }, + { + "x": 2.2616596068294545, + "y": 2.011311910787294, + "heading": -0.28662513647276167, + "angularVelocity": 6.696902771759525e-7, + "velocityX": 2.6339318133574414, + "velocityY": -2.713961853381871, + "timestamp": 1.1743201760532598 + }, + { + "x": 2.418427669547749, + "y": 1.8693594092228303, + "heading": -0.2866250962912349, + "angularVelocity": 7.185536621043778e-7, + "velocityX": 2.8034341776776834, + "velocityY": -2.538492136678183, + "timestamp": 1.2302401844367483 + }, + { + "x": 2.5840219554137547, + "y": 1.7378099138847216, + "heading": -0.28662505217416917, + "angularVelocity": 7.889316725786441e-7, + "velocityX": 2.961270762521926, + "velocityY": -2.3524584337678855, + "timestamp": 1.2861601928202369 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.3444811403993986, - "angularVelocity": 0.0000012577088124740217, - "velocityX": 3.3509722173532026, - "velocityY": -2.575383288874084, - "timestamp": 1.3542872703345679 - }, - { - "x": 2.960734343821119, - "y": 1.4779114817648484, - "heading": -0.34448108051633547, - "angularVelocity": 0.0000010280352851970435, - "velocityX": 3.4846514266808644, - "velocityY": -2.391405414524714, - "timestamp": 1.4125372778029122 - }, - { - "x": 3.170914398347007, - "y": 1.349732150860621, - "heading": -0.34448103072468966, - "angularVelocity": 8.54792093887579e-7, - "velocityX": 3.6082408167949036, - "velocityY": -2.200503252705787, - "timestamp": 1.4707872852712565 - }, - { - "x": 3.387684960924611, - "y": 1.2330440117382415, - "heading": -0.3444809881196843, - "angularVelocity": 7.31416307943616e-7, - "velocityX": 3.7213825714169735, - "velocityY": -2.00322959934028, - "timestamp": 1.5290372927396008 - }, - { - "x": 3.610418374312036, - "y": 1.1281849298444195, - "heading": -0.3444809507762438, - "angularVelocity": 6.410890256196379e-7, - "velocityX": 3.8237490957999993, - "velocityY": -1.80015568153888, - "timestamp": 1.587287300207945 - }, - { - "x": 3.8384697161434858, - "y": 1.0354585179376077, - "heading": -0.34448091735896624, - "angularVelocity": 5.736870952656925e-7, - "velocityX": 3.9150439930051975, - "velocityY": -1.591869528209129, - "timestamp": 1.6455373076762894 - }, - { - "x": 4.071178666241736, - "y": 0.9551332560900274, - "heading": -0.3444808869037787, - "angularVelocity": 5.228357697616013e-7, - "velocityX": 3.995002922956148, - "velocityY": -1.3789742755180425, - "timestamp": 1.7037873151446337 - }, - { - "x": 4.307871418551335, - "y": 0.8874417125102056, - "heading": -0.34448085868861794, - "angularVelocity": 4.843803799821142e-7, - "velocityX": 4.063394368459522, - "velocityY": -1.1620864360679888, - "timestamp": 1.762037322612978 - }, - { - "x": 4.547862632165302, - "y": 0.8325798662811171, - "heading": -0.3444808321530075, - "angularVelocity": 4.555469028757283e-7, - "velocityX": 4.120020306338885, - "velocityY": -0.9418341492729086, - "timestamp": 1.8202873300813223 - }, - { - "x": 4.7904574158495095, - "y": 0.7907065304618335, - "heading": -0.3444808068457288, - "angularVelocity": 4.3445966396412105e-7, - "velocityX": 4.164716782500731, - "velocityY": -0.7188554583798012, - "timestamp": 1.8785373375496666 - }, - { - "x": 5.03495334055107, - "y": 0.7619428639166234, - "heading": -0.34448078238917407, - "angularVelocity": 4.198549626958455e-7, - "velocityX": 4.197354392348067, - "velocityY": -0.4937967872509155, - "timestamp": 1.9367873450180109 - }, - { - "x": 5.280642475492497, - "y": 0.7463718997323922, - "heading": -0.3444807584537773, - "angularVelocity": 4.109080464550885e-7, - "velocityX": 4.217838685685062, - "velocityY": -0.26731265558537776, - "timestamp": 1.9950373524863552 - }, - { - "x": 5.526813463070627, - "y": 0.7440371744430943, - "heading": -0.34448073473848534, - "angularVelocity": 4.0712942424247657e-7, - "velocityX": 4.226110832894084, - "velocityY": -0.040081115707442425, - "timestamp": 2.0532873599546995 - }, - { - "x": 5.766738873929576, - "y": 0.7458613898059425, - "heading": -0.32467851072076565, - "angularVelocity": 0.33995230006590477, - "velocityX": 4.118890645453325, - "velocityY": 0.03131699792210607, - "timestamp": 2.1115373674230438 - }, - { - "x": 5.994439408520209, - "y": 0.7477937318314798, - "heading": -0.2833894042686398, - "angularVelocity": 0.7088257709591617, - "velocityX": 3.9090215518749, - "velocityY": 0.033173249404078294, - "timestamp": 2.169787374891388 - }, - { - "x": 6.208925057122028, - "y": 0.7495883201901966, - "heading": -0.23844368677979322, - "angularVelocity": 0.7716002013093647, - "velocityX": 3.6821565854456084, - "velocityY": 0.030808379890630424, - "timestamp": 2.2280373823597324 - }, - { - "x": 6.410164836489075, - "y": 0.7512394294870858, - "heading": -0.1938705620031709, - "angularVelocity": 0.7652037607178938, - "velocityX": 3.454759717866313, - "velocityY": 0.028345220346736016, - "timestamp": 2.2862873898280767 - }, - { - "x": 6.598169956514198, - "y": 0.752743774939267, - "heading": -0.15150662184462524, - "angularVelocity": 0.7272778493902874, - "velocityX": 3.227555294774738, - "velocityY": 0.02582566968766568, - "timestamp": 2.344537397296421 - }, - { - "x": 6.772954120733039, - "y": 0.754099580476696, - "heading": -0.11241242955567449, - "angularVelocity": 0.6711448459503878, - "velocityX": 3.000586125483803, - "velocityY": 0.023275628559629353, - "timestamp": 2.4027874047647653 - }, - { - "x": 6.934529138966309, - "y": 0.7553058783616067, - "heading": -0.07728200268237817, - "angularVelocity": 0.6030973797280247, - "velocityX": 2.77381969987003, - "velocityY": 0.020708973909851786, - "timestamp": 2.4610374122331096 - }, - { - "x": 7.082904706348492, - "y": 0.7563621152126468, - "heading": -0.046606204008256526, - "angularVelocity": 0.52662308568445, - "velocityX": 2.547219714311956, - "velocityY": 0.018132819152241828, - "timestamp": 2.519287419701454 - }, - { - "x": 7.218088767056845, - "y": 0.7572679488303273, - "heading": -0.020751033175153387, - "angularVelocity": 0.4438655367924897, - "velocityX": 2.3207561094617386, - "velocityY": 0.015550789726041245, - "timestamp": 2.577537427169798 + "heading": -0.28662500163792387, + "angularVelocity": 9.037238500487202e-7, + "velocityX": 3.1067845266977017, + "velocityY": -2.1566350283233993, + "timestamp": 1.3420802012037254 + }, + { + "x": 2.964172022537395, + "y": 1.4952512181839386, + "heading": -0.2866249560195876, + "angularVelocity": 7.195928187708238e-7, + "velocityX": 3.256089340971223, + "velocityY": -1.923816018708688, + "timestamp": 1.4054748531095118 + }, + { + "x": 3.1789518158724, + "y": 1.3887033110695066, + "heading": -0.28662491569189824, + "angularVelocity": 6.361370890269852e-7, + "velocityX": 3.387979693526802, + "velocityY": -1.6807081340674828, + "timestamp": 1.4688695050152982 + }, + { + "x": 3.4009440542760783, + "y": 1.2981369637223683, + "heading": -0.2866248790058733, + "angularVelocity": 5.786927429417107e-7, + "velocityX": 3.5017502538477667, + "velocityY": -1.4286117933375972, + "timestamp": 1.5322641569210846 + }, + { + "x": 3.6289614625982565, + "y": 1.2240364953903844, + "heading": -0.286624844765475, + "angularVelocity": 5.401149349395126e-7, + "velocityX": 3.5967924969608642, + "velocityY": -1.1688757033023536, + "timestamp": 1.595658808826871 + }, + { + "x": 3.861784529858251, + "y": 1.1667980667810438, + "heading": -0.2866248120410044, + "angularVelocity": 5.162023864828723e-7, + "velocityX": 3.672597928386812, + "velocityY": -0.9028904945231823, + "timestamp": 1.6590534607326575 + }, + { + "x": 4.0981679647792335, + "y": 1.1267270015307351, + "heading": -0.2866247800525485, + "angularVelocity": 5.045923424053189e-7, + "velocityX": 3.728759884544863, + "velocityY": -0.6320890492444079, + "timestamp": 1.7224481126384439 + }, + { + "x": 4.3358126512762265, + "y": 1.0949807955483848, + "heading": -0.2866247482072254, + "angularVelocity": 5.023345364444787e-7, + "velocityX": 3.7486551207847545, + "velocityY": -0.5007710434238178, + "timestamp": 1.7858427645442303 + }, + { + "x": 4.573457419312962, + "y": 1.0632351999594603, + "heading": -0.28662471636190656, + "angularVelocity": 5.023344691113163e-7, + "velocityX": 3.7486564070090496, + "velocityY": -0.5007614149550478, + "timestamp": 1.8492374164500167 + }, + { + "x": 4.811102187354235, + "y": 1.0314896044045174, + "heading": -0.28662468451658774, + "angularVelocity": 5.023344695763415e-7, + "velocityX": 3.748656407080655, + "velocityY": -0.5007614144190159, + "timestamp": 1.9126320683558031 + }, + { + "x": 5.04874695539551, + "y": 0.9997440088495765, + "heading": -0.28662465267126885, + "angularVelocity": 5.023344699297441e-7, + "velocityX": 3.7486564070806594, + "velocityY": -0.5007614144189861, + "timestamp": 1.9760267202615895 + }, + { + "x": 5.286391723436784, + "y": 0.9679984132946355, + "heading": -0.28662462082595, + "angularVelocity": 5.023344691743294e-7, + "velocityX": 3.748656407080659, + "velocityY": -0.5007614144189861, + "timestamp": 2.039421372167376 + }, + { + "x": 5.524036491478059, + "y": 0.936252817739695, + "heading": -0.28662458898063115, + "angularVelocity": 5.023344697631327e-7, + "velocityX": 3.74865640708066, + "velocityY": -0.5007614144189809, + "timestamp": 2.1028160240731624 + }, + { + "x": 5.761681259520114, + "y": 0.9045072221906025, + "heading": -0.2866245571353122, + "angularVelocity": 5.023344707942377e-7, + "velocityX": 3.748656407092983, + "velocityY": -0.5007614143267316, + "timestamp": 2.166210675978949 + }, + { + "x": 5.99932604159473, + "y": 0.8727617316882137, + "heading": -0.2866245252899934, + "angularVelocity": 5.023344694954779e-7, + "velocityX": 3.748656628445413, + "velocityY": -0.500759757298884, + "timestamp": 2.229605327884735 + }, + { + "x": 6.237212534637086, + "y": 0.8428813906316718, + "heading": -0.2866244915581877, + "angularVelocity": 5.320922919551869e-7, + "velocityX": 3.752469425905036, + "velocityY": -0.47133851450037834, + "timestamp": 2.2929999797905216 + }, + { + "x": 6.4638245505635705, + "y": 0.8220107849523658, + "heading": -0.24424436180847547, + "angularVelocity": 0.6685126974542774, + "velocityX": 3.574623554416899, + "velocityY": -0.3292171350719418, + "timestamp": 2.356394631696308 + }, + { + "x": 6.673647733933235, + "y": 0.8041360764949451, + "heading": -0.18514255243378605, + "angularVelocity": 0.9322838377994912, + "velocityX": 3.309793130207447, + "velocityY": -0.2819592492436898, + "timestamp": 2.4197892836020944 + }, + { + "x": 6.866141134149738, + "y": 0.7888964331390943, + "heading": -0.1276691267098072, + "angularVelocity": 0.9065973863125333, + "velocityX": 3.0364296424022625, + "velocityY": -0.240393201913927, + "timestamp": 2.483183935507881 + }, + { + "x": 7.04134978279503, + "y": 0.7761674362974819, + "heading": -0.07636429138531178, + "angularVelocity": 0.8092927996630026, + "velocityX": 2.7637764918352534, + "velocityY": -0.2007897584252007, + "timestamp": 2.5465785874136673 + }, + { + "x": 7.19932046714385, + "y": 0.765888430525111, + "heading": -0.03338684229285246, + "angularVelocity": 0.6779349330023924, + "velocityX": 2.4918613731579056, + "velocityY": -0.16214310613530794, + "timestamp": 2.6099732393194537 }, { "x": 7.340087890625, "y": 0.7580231428146362, "heading": 0, - "angularVelocity": 0.35624086720384496, - "velocityX": 2.0944052862903964, - "velocityY": 0.012964701931058227, - "timestamp": 2.6357874346381425 - }, - { - "x": 7.452016074368774, - "y": 0.7586421596982608, - "heading": 0.015744597720939148, - "angularVelocity": 0.26174771257837953, - "velocityX": 1.8607618046043786, - "velocityY": 0.010290910965644757, - "timestamp": 2.6959392409933733 - }, - { - "x": 7.549878171503854, - "y": 0.7590935449079357, - "heading": 0.02631445528043402, - "angularVelocity": 0.17571970319684052, - "velocityX": 1.626918675677823, - "velocityY": 0.007504100658407957, - "timestamp": 2.756091047348604 - }, - { - "x": 7.633665265890791, - "y": 0.7593717259908211, - "heading": 0.032122080946348684, - "angularVelocity": 0.09654948068587346, - "velocityX": 1.3929273194578256, - "velocityY": 0.004624650525748951, - "timestamp": 2.816242853703835 - }, - { - "x": 7.703370537390185, - "y": 0.7594720434745997, - "heading": 0.03350862227223397, - "angularVelocity": 0.023050701382050013, - "velocityX": 1.1588225811166206, - "velocityY": 0.001667738507906577, - "timestamp": 2.876394660059066 - }, - { - "x": 7.758988639574762, - "y": 0.7593905387990408, - "heading": 0.0307610262741512, - "angularVelocity": -0.04567769722253501, - "velocityX": 0.9246289605355835, - "velocityY": -0.0013549830087877977, - "timestamp": 2.936546466414297 - }, - { - "x": 7.800515294542927, - "y": 0.7591238044421035, - "heading": 0.024124065366989432, - "angularVelocity": -0.11033685119889487, - "velocityX": 0.6903642215318662, - "velocityY": -0.004434353232251388, - "timestamp": 2.9966982727695277 - }, - { - "x": 7.827947019588505, - "y": 0.7586688745154345, - "heading": 0.01380900494204492, - "angularVelocity": -0.17148380156745927, - "velocityX": 0.4560415839148344, - "velocityY": -0.007563030177051137, - "timestamp": 3.0568500791247586 + "angularVelocity": 0.5266507708326894, + "velocityX": 2.220493673351971, + "velocityY": -0.12406863156475356, + "timestamp": 2.67336789122524 + }, + { + "x": 7.464023522460178, + "y": 0.7525372143128384, + "heading": 0.02301011936038792, + "angularVelocity": 0.3617896361662683, + "velocityX": 1.9486481772395665, + "velocityY": -0.08625561847872572, + "timestamp": 2.736968716290925 + }, + { + "x": 7.570582319535539, + "y": 0.7491648354718851, + "heading": 0.036983700221316605, + "angularVelocity": 0.21970754068830917, + "velocityX": 1.675431049287655, + "velocityY": -0.053024136675436706, + "timestamp": 2.8005695413566096 + }, + { + "x": 7.659708944521439, + "y": 0.7476928551810322, + "heading": 0.04295335113137197, + "angularVelocity": 0.09386121805637201, + "velocityX": 1.4013438488235506, + "velocityY": -0.02314404395434427, + "timestamp": 2.8641703664222944 + }, + { + "x": 7.731365798341117, + "y": 0.7479588271083956, + "heading": 0.041693436231508264, + "angularVelocity": -0.019809725716020715, + "velocityX": 1.1266654755763426, + "velocityY": 0.0041818942928592215, + "timestamp": 2.927771191487979 + }, + { + "x": 7.785525986892142, + "y": 0.7498349370538775, + "heading": 0.03380693253041853, + "angularVelocity": -0.12400002190136777, + "velocityX": 0.8515642445690119, + "velocityY": 0.029498201376229192, + "timestamp": 2.991372016553664 + }, + { + "x": 7.822169492500932, + "y": 0.7532180353995639, + "heading": 0.0197770843540876, + "angularVelocity": -0.220592235428419, + "velocityX": 0.5761482743493489, + "velocityY": 0.053192680160869574, + "timestamp": 3.0549728416193487 }, { "x": 7.841280937194824, "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": -0.2295692478542545, - "velocityX": 0.22167110872074697, - "velocityY": -0.010735034239617657, - "timestamp": 3.1170018854799895 - }, - { - "x": 7.835826713549701, - "y": 0.7569095778214322, - "heading": -0.022558536561860222, - "angularVelocity": -0.299041202712131, - "velocityX": -0.07230245606695226, - "velocityY": -0.014761676314980546, - "timestamp": 3.1924381008150915 - }, - { - "x": 7.80819402127163, - "y": 0.7554921737702587, - "heading": -0.05020545416339097, - "angularVelocity": -0.3664939641883925, - "velocityX": -0.36630538999500056, - "velocityY": -0.018789437472137644, - "timestamp": 3.2678743161501935 - }, - { - "x": 7.758380386118448, - "y": 0.7537708531649631, - "heading": -0.08276554497145149, - "angularVelocity": -0.43162412991455607, - "velocityX": -0.6603411230520984, - "velocityY": -0.02281822593630055, - "timestamp": 3.3433105314852956 - }, - { - "x": 7.686383032454013, - "y": 0.7517455632733434, - "heading": -0.12003503692740472, - "angularVelocity": -0.494053045879821, - "velocityX": -0.9544136505869183, - "velocityY": -0.026847713430781565, - "timestamp": 3.4187467468203976 - }, - { - "x": 7.592198832311897, - "y": 0.7494162995505907, - "heading": -0.1617736436152976, - "angularVelocity": -0.5532966692785698, - "velocityX": -1.2485276431715386, - "velocityY": -0.030877261172313774, - "timestamp": 3.4941829621554996 - }, - { - "x": 7.475824246300941, - "y": 0.7467831362501152, - "heading": -0.2076930920063058, - "angularVelocity": -0.6087188784196724, - "velocityX": -1.542688554747834, - "velocityY": -0.0349058245933778, - "timestamp": 3.5696191774906016 - }, - { - "x": 7.337255260610424, - "y": 0.7438462658074466, - "heading": -0.257440012481047, - "angularVelocity": -0.6594567377718513, - "velocityX": -1.8369026743318102, - "velocityY": -0.03893183704435113, - "timestamp": 3.6450553928257037 - }, - { - "x": 7.1764873367279565, - "y": 0.7406060479084567, - "heading": -0.31056943932971864, - "angularVelocity": -0.7042960282758145, - "velocityX": -2.131176957490571, - "velocityY": -0.04295308141581948, - "timestamp": 3.7204916081608057 - }, - { - "x": 6.9935154268341, - "y": 0.7370630667353528, - "heading": -0.3665018715735503, - "angularVelocity": -0.741453319143453, - "velocityX": -2.425518155716889, - "velocityY": -0.04696658173219917, - "timestamp": 3.7959278234959077 - }, - { - "x": 6.788334219229139, - "y": 0.7332181873732633, - "heading": -0.42444960466432774, - "angularVelocity": -0.768168615476831, - "velocityX": -2.719929767068861, - "velocityY": -0.050968614279096394, - "timestamp": 3.8713640388310098 - }, - { - "x": 6.560939151808715, - "y": 0.7290725776308253, - "heading": -0.48328030857318144, - "angularVelocity": -0.7798734818219146, - "velocityX": -3.0144018547363567, - "velocityY": -0.05495516608332171, - "timestamp": 3.946800254166112 - }, - { - "x": 6.311330185153471, - "y": 0.724627567965158, - "heading": -0.5412355901112249, - "angularVelocity": -0.7682686794478624, - "velocityX": -3.3088744649560358, - "velocityY": -0.05892408103881697, - "timestamp": 4.022236469501213 - }, - { - "x": 6.039527461477683, - "y": 0.7198838011192613, - "heading": -0.5952481542708341, - "angularVelocity": -0.7160031016889598, - "velocityX": -3.6030800653027115, - "velocityY": -0.06288447564374298, - "timestamp": 4.097672684836315 - }, - { - "x": 5.745659621866784, - "y": 0.7148372933474686, - "heading": -0.638783124761932, - "angularVelocity": -0.5771096852845462, - "velocityX": -3.895580369527838, - "velocityY": -0.06689767970695078, - "timestamp": 4.173108900171417 - }, - { - "x": 5.431003034025849, - "y": 0.7096634777046316, - "heading": -0.6507371958403388, - "angularVelocity": -0.15846594404696066, - "velocityX": -4.17116084685812, - "velocityY": -0.0685853024287286, - "timestamp": 4.248545115506518 - }, - { - "x": 5.1125615609654815, - "y": 0.7251156489118818, - "heading": -0.6507372106959876, - "angularVelocity": -1.9692993052752515e-7, - "velocityX": -4.221334164840962, - "velocityY": 0.2048375722271958, - "timestamp": 4.32398133084162 - }, - { - "x": 4.795969345130646, - "y": 0.7627070022190265, - "heading": -0.6507372258766643, - "angularVelocity": -2.012385798908336e-7, - "velocityX": -4.196819981337514, - "velocityY": 0.49831971474386, - "timestamp": 4.399417546176721 - }, - { - "x": 4.482763788472813, - "y": 0.8222554661406114, - "heading": -0.6507372417212374, - "angularVelocity": -2.1003934401896774e-7, - "velocityX": -4.151925640311049, - "velocityY": 0.7893882753404204, - "timestamp": 4.474853761511823 - }, - { - "x": 4.174465853368726, - "y": 0.9034719819930799, - "heading": -0.6507372586342053, - "angularVelocity": -2.2420223152003646e-7, - "velocityX": -4.0868690685842415, - "velocityY": 1.0766250068576386, - "timestamp": 4.5502899768469245 - }, - { - "x": 3.8725726714422697, - "y": 1.0059621985442242, - "heading": -0.6507372771356286, - "angularVelocity": -2.452591658573351e-7, - "velocityX": -4.0019661721546855, - "velocityY": 1.3586341268031932, - "timestamp": 4.625726192182026 - }, - { - "x": 3.5785502725752965, - "y": 1.1292284338722989, - "heading": -0.6507372979388782, - "angularVelocity": -2.757727119617825e-7, - "velocityX": -3.897629242942143, - "velocityY": 1.6340458595450806, - "timestamp": 4.701162407517128 - }, - { - "x": 3.293826465550643, - "y": 1.2726721052481516, - "heading": -0.6507373220853944, - "angularVelocity": -3.200918277895427e-7, - "velocityX": -3.7743649487166713, - "velocityY": 1.9015226405334464, - "timestamp": 4.776598622852229 - }, - { - "x": 3.019783904481099, - "y": 1.4355966406757221, - "heading": -0.6507373511975181, - "angularVelocity": -3.859170793282021e-7, - "velocityX": -3.632771870277356, - "velocityY": 2.1597655012758525, - "timestamp": 4.852034838187331 + "heading": -3.3329541710174495e-31, + "angularVelocity": -0.3109564118651419, + "velocityX": 0.3004905152433859, + "velocityY": 0.07555102327854826, + "timestamp": 3.1185736666850334 + }, + { + "x": 7.837626456422163, + "y": 0.7661832239405314, + "heading": -0.033360531548023514, + "angularVelocity": -0.4178611188210231, + "velocityX": -0.04577461309858743, + "velocityY": 0.10221002096529454, + "timestamp": 3.1984100765480727 + }, + { + "x": 7.806322741144082, + "y": 0.7764707746744536, + "heading": -0.07501113915617397, + "angularVelocity": -0.521699406068016, + "velocityX": -0.3920982335225694, + "velocityY": 0.12885788265743237, + "timestamp": 3.278246486411112 + }, + { + "x": 7.74736425619444, + "y": 0.7888847549403223, + "heading": -0.12465356813987527, + "angularVelocity": -0.6218018704606526, + "velocityX": -0.7384911852973625, + "velocityY": 0.15549271675874526, + "timestamp": 3.3580828962741514 + }, + { + "x": 7.660744366778727, + "y": 0.8034239689194147, + "heading": -0.18191677738707465, + "angularVelocity": -0.7172568173523234, + "velocityX": -1.0849672419427567, + "velocityY": 0.182112572497119, + "timestamp": 3.4379193061371907 + }, + { + "x": 7.5464550145644145, + "y": 0.8200870575241466, + "heading": -0.24632578527266566, + "angularVelocity": -0.8067623280666746, + "velocityX": -1.4315442341455173, + "velocityY": 0.20871540482992249, + "timestamp": 3.51775571600023 + }, + { + "x": 7.404486266909035, + "y": 0.8388724767291539, + "heading": -0.3172478637527479, + "angularVelocity": -0.8883425319569104, + "velocityX": -1.7782456387872363, + "velocityY": 0.23529889729803757, + "timestamp": 3.5975921258632693 + }, + { + "x": 7.234825726560123, + "y": 0.8597784078771568, + "heading": -0.39379130752240776, + "angularVelocity": -0.958753579989021, + "velocityX": -2.125102326619727, + "velocityY": 0.26185960996827495, + "timestamp": 3.6774285357263086 + }, + { + "x": 7.037458048630302, + "y": 0.8828023770067689, + "heading": -0.47459284644074845, + "angularVelocity": -1.0120888333650886, + "velocityX": -2.4721512185781784, + "velocityY": 0.28838933475478157, + "timestamp": 3.757264945589348 + }, + { + "x": 6.812366694532574, + "y": 0.9079395818644891, + "heading": -0.5572950195818098, + "angularVelocity": -1.0358954427301785, + "velocityX": -2.8194072664825045, + "velocityY": 0.31485890837079844, + "timestamp": 3.837101355452387 + }, + { + "x": 6.559557151686244, + "y": 0.9351738572850867, + "heading": -0.6368741954392069, + "angularVelocity": -0.9967779863087077, + "velocityX": -3.1665945810943685, + "velocityY": 0.3411260033776371, + "timestamp": 3.9169377653154265 + }, + { + "x": 6.2794552371906684, + "y": 0.9644007497338526, + "heading": -0.6963848347023109, + "angularVelocity": -0.7454072567290455, + "velocityX": -3.5084482753682624, + "velocityY": 0.36608475379723326, + "timestamp": 3.996774175178466 + }, + { + "x": 5.978414020607044, + "y": 0.9876514129078965, + "heading": -0.6963848502753706, + "angularVelocity": -1.9506212382871808e-7, + "velocityX": -3.7707258768281893, + "velocityY": 0.2912288167006853, + "timestamp": 4.0766105850415055 + }, + { + "x": 5.677372687056552, + "y": 1.0109005615970947, + "heading": -0.6963848658464015, + "angularVelocity": -1.950367123738679e-7, + "velocityX": -3.770727341909936, + "velocityY": 0.29120984684910617, + "timestamp": 4.156446994904545 + }, + { + "x": 5.376331353502739, + "y": 1.0341497102432655, + "heading": -0.6963848814174324, + "angularVelocity": -1.950367127895845e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.29120984631016156, + "timestamp": 4.236283404767585 + }, + { + "x": 5.075290019948924, + "y": 1.0573988588894352, + "heading": -0.6963848969884634, + "angularVelocity": -1.950367129001825e-7, + "velocityX": -3.770727341951559, + "velocityY": 0.2912098463101467, + "timestamp": 4.316119814630625 + }, + { + "x": 4.774248686395111, + "y": 1.0806480075356062, + "heading": -0.6963849125594943, + "angularVelocity": -1.9503671289084429e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.2912098463101629, + "timestamp": 4.3959562244936645 + }, + { + "x": 4.473207352844907, + "y": 1.1038971562285251, + "heading": -0.6963849281305252, + "angularVelocity": -1.950367124712723e-7, + "velocityX": -3.770727341906337, + "velocityY": 0.2912098468957114, + "timestamp": 4.475792634356704 + }, + { + "x": 4.172166146375442, + "y": 1.1271479503662634, + "heading": -0.6963849437015552, + "angularVelocity": -1.9503670107200537e-7, + "velocityX": -3.770725750142127, + "velocityY": 0.29123045710128126, + "timestamp": 4.555629044219744 + }, + { + "x": 3.8735860999949274, + "y": 1.1720518831992321, + "heading": -0.6963849593703558, + "angularVelocity": -1.9626133737167217e-7, + "velocityX": -3.7398982105123864, + "velocityY": 0.5624492998871315, + "timestamp": 4.635465454082784 + }, + { + "x": 3.580403442801602, + "y": 1.244234542261993, + "heading": -0.6963849756495777, + "angularVelocity": -2.0390723857242194e-7, + "velocityX": -3.672292600535076, + "velocityY": 0.9041320769131688, + "timestamp": 4.7153018639458235 + }, + { + "x": 3.295105029858622, + "y": 1.3430840304472003, + "heading": -0.6963849932202171, + "angularVelocity": -2.20083034395915e-7, + "velocityX": -3.5735376056164516, + "velocityY": 1.238150467371779, + "timestamp": 4.795138273808863 + }, + { + "x": 3.0201108216375614, + "y": 1.4677619739384065, + "heading": -0.6963850130039021, + "angularVelocity": -2.4780278968053093e-7, + "velocityX": -3.444471121544888, + "velocityY": 1.5616677115753714, + "timestamp": 4.874974683671903 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.6507373850576595, - "angularVelocity": -4.4885790275828266e-7, - "velocityX": -3.473537625459172, - "velocityY": 2.40752036058895, - "timestamp": 4.927471053522432 - }, - { - "x": 2.56955568155037, - "y": 1.762694428612382, - "heading": -0.6507374187199763, - "angularVelocity": -5.980784656519904e-7, - "velocityX": -3.34370885712208, - "velocityY": 2.5848068511762112, - "timestamp": 4.98375516756578 - }, - { - "x": 2.389174031729705, - "y": 1.917763125748239, - "heading": -0.6507374476220785, - "angularVelocity": -5.135037275394257e-7, - "velocityX": -3.2048412396034367, - "velocityY": 2.7551059436847196, - "timestamp": 5.040039281609128 - }, - { - "x": 2.21709605626624, - "y": 2.0819977501361024, - "heading": -0.6507374788972222, - "angularVelocity": -5.556655576645871e-7, - "velocityX": -3.0573098357902078, - "velocityY": 2.917956996913463, - "timestamp": 5.096323395652476 - }, - { - "x": 2.053846284333076, - "y": 2.2548939585643604, - "heading": -0.6510333038480676, - "angularVelocity": -0.005255922667952023, - "velocityX": -2.900459120799771, - "velocityY": 3.071847382995126, - "timestamp": 5.152607509695824 - }, - { - "x": 1.9011339963459661, - "y": 2.4339551936894854, - "heading": -0.6596386701119549, - "angularVelocity": -0.15289156470082665, - "velocityX": -2.7132396162351555, - "velocityY": 3.1813814282875046, - "timestamp": 5.208891623739172 - }, - { - "x": 1.7604826838741388, - "y": 2.6146352624702294, - "heading": -0.6772772697494215, - "angularVelocity": -0.3133850454478603, - "velocityX": -2.4989522330137985, - "velocityY": 3.2101432500401494, - "timestamp": 5.26517573778252 - }, - { - "x": 1.6317544686478245, - "y": 2.792388897989417, - "heading": -0.7009679816171792, - "angularVelocity": -0.4209129391201239, - "velocityX": -2.287114533368532, - "velocityY": 3.158149302701766, - "timestamp": 5.321459851825868 - }, - { - "x": 1.5139177763229132, - "y": 2.964350304348354, - "heading": -0.7282265000249424, - "angularVelocity": -0.48430216715802665, - "velocityX": -2.0936048177671953, - "velocityY": 3.0552387522080946, - "timestamp": 5.377743965869215 - }, - { - "x": 1.405939966332392, - "y": 3.1288586513458805, - "heading": -0.7573592985064115, - "angularVelocity": -0.5176025060824792, - "velocityX": -1.9184420297947733, - "velocityY": 2.922820227228369, - "timestamp": 5.434028079912563 - }, - { - "x": 1.3069897960630168, - "y": 3.2849152039001472, - "heading": -0.7872314504581, - "angularVelocity": -0.5307386011030019, - "velocityX": -1.7580479314850108, - "velocityY": 2.7726571734624392, - "timestamp": 5.490312193955911 - }, - { - "x": 1.2164244610459272, - "y": 3.431880015003849, - "heading": -0.81704954760011, - "angularVelocity": -0.5297782091594296, - "velocityX": -1.609074541838559, - "velocityY": 2.6111241795600484, - "timestamp": 5.546596307999259 - }, - { - "x": 1.133744514509131, - "y": 3.569317973113813, - "heading": -0.8462342157208796, - "angularVelocity": -0.5185240740982935, - "velocityX": -1.468974824283782, - "velocityY": 2.4418605577430528, - "timestamp": 5.602880422042607 - }, - { - "x": 1.0585554462199285, - "y": 3.696918241093708, - "heading": -0.8743466079980574, - "angularVelocity": -0.4994729464076927, - "velocityX": -1.3358843710553012, - "velocityY": 2.2670742917197226, - "timestamp": 5.659164536085955 - }, - { - "x": 0.990539814673681, - "y": 3.8144496515109747, - "heading": -0.9010441731548517, - "angularVelocity": -0.4743357092950377, - "velocityX": -1.2084339018619632, - "velocityY": 2.088180873323288, - "timestamp": 5.715448650129303 - }, - { - "x": 0.9294376679920158, - "y": 3.9217345696371626, - "heading": -0.9260527900785384, - "angularVelocity": -0.4443281616625532, - "velocityX": -1.0856019983650476, - "velocityY": 1.9061314182463815, - "timestamp": 5.771732764172651 - }, - { - "x": 0.8750327428643173, - "y": 4.018632780962349, - "heading": -0.949148487110952, - "angularVelocity": -0.4103413089993085, - "velocityX": -0.9666124456680172, - "velocityY": 1.7215907716084669, - "timestamp": 5.8280168782159985 - }, - { - "x": 0.8271425931537745, - "y": 4.105031112353838, - "heading": -0.9701450242376531, - "angularVelocity": -0.37304552951709063, - "velocityX": -0.8508644139562936, - "velocityY": 1.5350393776287605, - "timestamp": 5.884300992259346 - }, - { - "x": 0.7856114013922731, - "y": 4.180836489375294, - "heading": -0.9888852009339675, - "angularVelocity": -0.3329567679057974, - "velocityX": -0.7378847916041681, - "velocityY": 1.346834329897688, - "timestamp": 5.940585106302694 - }, - { - "x": 0.7503046474773418, - "y": 4.245971138892709, - "heading": -1.0052346109229755, - "angularVelocity": -0.2904800096243145, - "velocityX": -0.6272951882611009, - "velocityY": 1.157247486693155, - "timestamp": 5.996869220346042 - }, - { - "x": 0.7211050852240694, - "y": 4.300369179737215, - "heading": -1.0190770505330764, - "angularVelocity": -0.24593866040851983, - "velocityX": -0.5187886981890432, - "velocityY": 0.9664901325906992, - "timestamp": 6.05315333438939 - }, - { - "x": 0.6979096564056603, - "y": 4.343974139962992, - "heading": -1.0303110725490423, - "angularVelocity": -0.1995948982569619, - "velocityX": -0.4121132438994187, - "velocityY": 0.7747294412805966, - "timestamp": 6.109437448432738 - }, - { - "x": 0.6806270884657889, - "y": 4.376737109843248, - "heading": -1.038847350819041, - "angularVelocity": -0.15166407813444996, - "velocityX": -0.30705942935444774, - "velocityY": 0.5820997707279092, - "timestamp": 6.165721562476086 - }, - { - "x": 0.6691759989582504, - "y": 4.398615341726796, - "heading": -1.044606629501852, - "angularVelocity": -0.1023251192756673, - "velocityX": -0.20345153694200915, - "velocityY": 0.3887106025458067, - "timestamp": 6.222005676519434 + "heading": -0.696385035506177, + "angularVelocity": -2.8185479518318006e-7, + "velocityX": -3.286187967310359, + "velocityY": 1.8719390230438346, + "timestamp": 4.954811093534943 + }, + { + "x": 2.5741841985169076, + "y": 1.7387873379942203, + "heading": -0.6963850589279509, + "angularVelocity": -4.023107769663099e-7, + "velocityX": -3.153128257888759, + "velocityY": 2.0882929565548642, + "timestamp": 5.0130292054104135 + }, + { + "x": 2.3991894720533877, + "y": 1.872411165939459, + "heading": -0.6963850794155533, + "angularVelocity": -3.519111438487621e-7, + "velocityX": -3.0058468202788267, + "velocityY": 2.2952277846293376, + "timestamp": 5.071247317285884 + }, + { + "x": 2.2335584908852413, + "y": 2.017479635272907, + "heading": -0.6963850978369923, + "angularVelocity": -3.1642110134470593e-7, + "velocityX": -2.845007779064253, + "velocityY": 2.4918099309670456, + "timestamp": 5.129465429161355 + }, + { + "x": 2.0780383093475274, + "y": 2.173338406648706, + "heading": -0.696385114808838, + "angularVelocity": -2.9152174806319146e-7, + "velocityX": -2.671336746034877, + "velocityY": 2.6771526309404154, + "timestamp": 5.187683541036826 + }, + { + "x": 1.9333303562340614, + "y": 2.339284459115452, + "heading": -0.6963851307944144, + "angularVelocity": -2.7458081036886754e-7, + "velocityX": -2.485617421310377, + "velocityY": 2.8504196910698067, + "timestamp": 5.245901652912297 + }, + { + "x": 1.8000872249814166, + "y": 2.5145692341351995, + "heading": -0.6963851461633941, + "angularVelocity": -2.6398966215373687e-7, + "velocityX": -2.2886886393302137, + "velocityY": 3.0108289220145825, + "timestamp": 5.304119764787767 + }, + { + "x": 1.6789094155739503, + "y": 2.698401829383423, + "heading": -0.6963851612308563, + "angularVelocity": -2.588105610313673e-7, + "velocityX": -2.0814451981312536, + "velocityY": 3.157652993649885, + "timestamp": 5.362337876663238 + }, + { + "x": 1.5665736982883989, + "y": 2.8877670089616614, + "heading": -0.6963851762457391, + "angularVelocity": -2.5790741716981964e-7, + "velocityX": -1.929566481404267, + "velocityY": 3.2526850060560912, + "timestamp": 5.420555988538709 + }, + { + "x": 1.4540456075762054, + "y": 3.0767463777174253, + "heading": -0.6971250426595832, + "angularVelocity": -0.012708526436353156, + "velocityX": -1.9328708384238364, + "velocityY": 3.24605801644674, + "timestamp": 5.47877410041418 + }, + { + "x": 1.3486317039814464, + "y": 3.254480208772174, + "heading": -0.7253513181524345, + "angularVelocity": -0.48483666995638336, + "velocityX": -1.8106719747325544, + "velocityY": 3.0528958313681422, + "timestamp": 5.53699221228965 + }, + { + "x": 1.2507109072486164, + "y": 3.4195717896549707, + "heading": -0.7601787682651481, + "angularVelocity": -0.5982236281934066, + "velocityX": -1.6819644879978934, + "velocityY": 2.835742616248544, + "timestamp": 5.595210324165121 + }, + { + "x": 1.160328320301069, + "y": 3.5719513486090504, + "heading": -0.7969704460064527, + "angularVelocity": -0.631962744171482, + "velocityX": -1.5524822780387872, + "velocityY": 2.6173909466528555, + "timestamp": 5.653428436040592 + }, + { + "x": 1.0774888591394964, + "y": 3.71161238889488, + "heading": -0.8336344675120989, + "angularVelocity": -0.6297700204374729, + "velocityX": -1.422915626991949, + "velocityY": 2.3989276839579965, + "timestamp": 5.711646547916063 + }, + { + "x": 1.0021908033919844, + "y": 3.838558158350536, + "heading": -0.8689739346571358, + "angularVelocity": -0.6070184347549548, + "velocityX": -1.2933785264038764, + "velocityY": 2.1805202086799977, + "timestamp": 5.7698646597915335 + }, + { + "x": 0.9344313462846832, + "y": 3.9527935365952946, + "heading": -0.9022118432658016, + "angularVelocity": -0.5709204152783613, + "velocityX": -1.1638896371672067, + "velocityY": 1.962196549573947, + "timestamp": 5.828082771667004 + }, + { + "x": 0.8742077702690397, + "y": 4.054323289061519, + "heading": -0.9328020557936417, + "angularVelocity": -0.5254415085338586, + "velocityX": -1.034447426678191, + "velocityY": 1.7439547452758104, + "timestamp": 5.886300883542475 + }, + { + "x": 0.8215176865940929, + "y": 4.143151679881797, + "heading": -0.9603390876580659, + "angularVelocity": -0.4729976802292441, + "velocityX": -0.9050462472512312, + "velocityY": 1.5257861850670007, + "timestamp": 5.944518995417946 + }, + { + "x": 0.7763590444591775, + "y": 4.21928243186123, + "heading": -0.984509600370908, + "angularVelocity": -0.41517170403161197, + "velocityX": -0.7756802939866982, + "velocityY": 1.3076815706816212, + "timestamp": 6.002737107293417 + }, + { + "x": 0.7387300854642734, + "y": 4.282718776761074, + "heading": -1.0050639989905081, + "angularVelocity": -0.3530584891445192, + "velocityX": -0.6463445443815322, + "velocityY": 1.0896324675649907, + "timestamp": 6.060955219168887 + }, + { + "x": 0.7086292911290721, + "y": 4.333463522832413, + "heading": -1.0217986916878445, + "angularVelocity": -0.28744822115035373, + "velocityX": -0.5170348773863939, + "velocityY": 0.8716316011739073, + "timestamp": 6.119173331044358 + }, + { + "x": 0.6860553360496396, + "y": 4.371519118466469, + "heading": -1.0345444282293665, + "angularVelocity": -0.21893077825652074, + "velocityX": -0.3877479765698806, + "velocityY": 0.6536727902728675, + "timestamp": 6.177391442919829 + }, + { + "x": 0.6710070489868055, + "y": 4.396887706764773, + "heading": -1.0431583071392123, + "angularVelocity": -0.14795874741302203, + "velocityX": -0.25848119387696106, + "velocityY": 0.4357507909663603, + "timestamp": 6.2356095547953 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -0.05172811727299292, - "velocityX": -0.10114075318843706, - "velocityY": 0.19465224364467987, - "timestamp": 6.2782897905627815 + "angularVelocity": -0.07488723824993983, + "velocityX": -0.12923242394972312, + "velocityY": 0.2178611369128849, + "timestamp": 6.29382766667077 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": 3.206426664477851e-31, - "velocityX": -6.202690310891061e-31, - "velocityY": -1.8966043908205797e-31, - "timestamp": 6.334573904606129 + "angularVelocity": -4.5881817726943837e-32, + "velocityX": 7.1951222626888875e-34, + "velocityY": 0, + "timestamp": 6.352045778546241 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubSprint.2.traj b/src/main/deploy/choreo/SourceLanePSubHSubSprint.2.traj index 87ec505e..739cb370 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubSprint.2.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubSprint.2.traj @@ -4,451 +4,442 @@ "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": 3.206426664477851e-31, - "velocityX": -6.202690310891061e-31, - "velocityY": -1.8966043908205797e-31, + "angularVelocity": -4.5881817726943837e-32, + "velocityX": 7.1951222626888875e-34, + "velocityY": 0, "timestamp": 0 }, { - "x": 0.6687462669994619, - "y": 4.399468321543485, - "heading": -1.0440928198775505, - "angularVelocity": 0.06332262739330675, - "velocityX": 0.09729413850238322, - "velocityY": -0.18676978112239445, - "timestamp": 0.05409252611801296 - }, - { - "x": 0.6793295244169436, - "y": 4.379291243512293, - "heading": -1.0373158700882419, - "angularVelocity": 0.1252844020356336, - "velocityX": 0.19565101090554332, - "velocityY": -0.3730104596559098, - "timestamp": 0.10818505223602592 - }, - { - "x": 0.69529661974622, - "y": 4.349072044732176, - "heading": -1.0272687237370097, - "angularVelocity": 0.1857400101686365, - "velocityX": 0.2951811733539195, - "velocityY": -0.5586575623080092, - "timestamp": 0.1622775783540389 - }, - { - "x": 0.716717978867713, - "y": 4.308846987227399, - "heading": -1.0140420515401747, - "angularVelocity": 0.24451940306772088, - "velocityX": 0.3960132879476041, - "velocityY": -0.7436342946359814, - "timestamp": 0.21637010447205185 - }, - { - "x": 0.7436721888450865, - "y": 4.258657330572967, - "heading": -0.9977373757966207, - "angularVelocity": 0.30142196923729686, - "velocityX": 0.49829822919416444, - "velocityY": -0.9278482677074609, - "timestamp": 0.2704626305900648 - }, - { - "x": 0.7762474872180782, - "y": 4.1985504141499295, - "heading": -0.9784691435647803, - "angularVelocity": 0.35620877068584267, - "velocityX": 0.6022144039229431, - "velocityY": -1.1111870851067975, - "timestamp": 0.3245551567080778 - }, - { - "x": 0.8145436288745613, - "y": 4.128581066891007, - "heading": -0.9563673669616457, - "angularVelocity": 0.4085920586303395, - "velocityX": 0.7079747315326336, - "velocityY": -1.2935122886733275, - "timestamp": 0.37864768282609074 - }, - { - "x": 0.8586742551972165, - "y": 4.0488134751688944, - "heading": -0.9315810408217653, - "angularVelocity": 0.4582209025648986, - "velocityX": 0.8158359294628605, - "velocityY": -1.4746508888900018, - "timestamp": 0.4327402089441037 - }, - { - "x": 0.9087699412940919, - "y": 3.9593237057932655, - "heading": -0.9042826435219333, - "angularVelocity": 0.5046611659473402, - "velocityX": 0.926111048824076, - "velocityY": -1.6543832539918422, - "timestamp": 0.48683273506211666 - }, - { - "x": 0.9649821731331605, - "y": 3.8602031891787973, - "heading": -0.8746741747762036, - "angularVelocity": 0.5473670924712444, - "velocityX": 1.0391866653860837, - "velocityY": -1.8324253594360185, - "timestamp": 0.5409252611801296 - }, - { - "x": 1.0274886208992116, - "y": 3.751563649709137, - "heading": -0.8429954192038807, - "angularVelocity": 0.5856401585518797, - "velocityX": 1.1555468426359812, - "velocityY": -2.008402033818445, - "timestamp": 0.5950177872981426 - }, - { - "x": 1.0965002487203657, - "y": 3.6335442878114614, - "heading": -0.8095355084845481, - "angularVelocity": 0.6185680928678664, - "velocityX": 1.275807080453678, - "velocityY": -2.181805331855407, - "timestamp": 0.6491103134161555 - }, - { - "x": 1.1722710636909373, - "y": 3.5063225947671968, - "heading": -0.7746495035161819, - "angularVelocity": 0.6449320723557629, - "velocityX": 1.4007631073674283, - "velocityY": -2.351927376562676, - "timestamp": 0.7032028395341685 - }, - { - "x": 1.2551116907799804, - "y": 3.370131275203577, - "heading": -0.738782856357727, - "angularVelocity": 0.6630610498773233, - "velocityX": 1.531461609098938, - "velocityY": -2.5177474475235027, - "timestamp": 0.7572953656521815 - }, - { - "x": 1.3454084529571924, - "y": 3.2252859282148103, - "heading": -0.7025087034544132, - "angularVelocity": 0.6705945443190738, - "velocityX": 1.669302002648637, - "velocityY": -2.6777330877992154, - "timestamp": 0.8113878917701944 - }, - { - "x": 1.4436499618435756, - "y": 3.0722326748820543, - "heading": -0.6665870109674555, - "angularVelocity": 0.6640786641844795, - "velocityX": 1.8161752821823995, - "velocityY": -2.829471358000162, - "timestamp": 0.8654804178882074 - }, - { - "x": 1.5504618658475082, - "y": 2.9116347206115476, - "heading": -0.6320630523320445, - "angularVelocity": 0.6382389788923385, - "velocityX": 1.9746148251778084, - "velocityY": -2.96894905444256, - "timestamp": 0.9195729440062204 - }, - { - "x": 1.666640324199563, - "y": 2.7445376431595645, - "heading": -0.6004402859276634, - "angularVelocity": 0.5846050956330832, - "velocityX": 2.147772838315358, - "velocityY": -3.0890973197909535, - "timestamp": 0.9736654701242333 - }, - { - "x": 1.7931265785030042, - "y": 2.572686348472304, - "heading": -0.5739736412013764, - "angularVelocity": 0.48928468728727015, - "velocityX": 2.338331436539341, - "velocityY": -3.1769877840815965, - "timestamp": 1.0277579962422463 - }, - { - "x": 1.9307039385810119, - "y": 2.3990237133183436, - "heading": -0.5558132076436828, - "angularVelocity": 0.33572907129927604, - "velocityX": 2.5433709599366927, - "velocityY": -3.2104737496406606, - "timestamp": 1.0818505223602592 - }, - { - "x": 2.079064797054882, - "y": 2.227910687938415, - "heading": -0.5489525363882142, - "angularVelocity": 0.12683214758631672, - "velocityX": 2.742723794233161, - "velocityY": -3.16333951581913, - "timestamp": 1.1359430484782722 - }, - { - "x": 2.237064005369136, - "y": 2.0627082520134143, - "heading": -0.548898243902152, - "angularVelocity": 0.0010036966332050488, - "velocityX": 2.920906447773384, - "velocityY": -3.0540713806611905, - "timestamp": 1.1900355745962852 - }, - { - "x": 2.40313061190331, - "y": 1.9055936598291874, - "heading": -0.5488982087618529, - "angularVelocity": 6.496331729658335e-7, - "velocityX": 3.0700471664443327, - "velocityY": -2.904552688872191, - "timestamp": 1.2441281007142981 - }, - { - "x": 2.5768383133172232, - "y": 1.7569708294797155, - "heading": -0.5488981704735678, - "angularVelocity": 7.078294853357702e-7, - "velocityX": 3.211306882491981, - "velocityY": -2.7475668269761684, - "timestamp": 1.298220626832311 + "x": 0.6704852067504524, + "y": 4.397985362145517, + "heading": -1.0417684453451177, + "angularVelocity": 0.10293302663784343, + "velocityX": 0.12534996227388928, + "velocityY": -0.20741457823419143, + "timestamp": 0.055858217681718614 + }, + { + "x": 0.684490344241533, + "y": 4.374811471822251, + "heading": -1.0303744644017594, + "angularVelocity": 0.20398038849505432, + "velocityX": 0.25072653715666804, + "velocityY": -0.4148698487895252, + "timestamp": 0.11171643536343723 + }, + { + "x": 0.7055004573054668, + "y": 4.340046908929331, + "heading": -1.0134578875635165, + "angularVelocity": 0.3028484892703545, + "velocityX": 0.3761328938859112, + "velocityY": -0.6223715029901059, + "timestamp": 0.16757465304515584 + }, + { + "x": 0.7335174330880717, + "y": 4.293688705050122, + "heading": -0.9911609198549822, + "angularVelocity": 0.3991707690278106, + "velocityX": 0.5015730351843025, + "velocityY": -0.8299262991769599, + "timestamp": 0.22343287072687446 + }, + { + "x": 0.768543442750825, + "y": 4.235733440885856, + "heading": -0.963651742055662, + "angularVelocity": 0.49248219762521406, + "velocityX": 0.6270520456333297, + "velocityY": -1.0375423092533531, + "timestamp": 0.27929108840859307 + }, + { + "x": 0.8105810218384888, + "y": 4.16617715590748, + "heading": -0.9311321668224927, + "angularVelocity": 0.5821806814257259, + "velocityX": 0.7525764485933851, + "velocityY": -1.24522922257758, + "timestamp": 0.3351493060903117 + }, + { + "x": 0.8596331801158593, + "y": 4.085015237354172, + "heading": -0.8938487217946981, + "angularVelocity": 0.667465711853477, + "velocityX": 0.8781547337738402, + "velocityY": -1.4529987157085942, + "timestamp": 0.3910075237720303 + }, + { + "x": 0.9157035549190764, + "y": 3.992242285598939, + "heading": -0.8521094876337724, + "angularVelocity": 0.7472353378469229, + "velocityX": 1.0037981362510981, + "velocityY": -1.6608648754934532, + "timestamp": 0.4468657414537489 + }, + { + "x": 0.9787966256099634, + "y": 3.88785195918136, + "heading": -0.8063112110880574, + "angularVelocity": 0.8199022175514963, + "velocityX": 1.1295217303637053, + "velocityY": -1.868844563075711, + "timestamp": 0.5027239591354675 + }, + { + "x": 1.0489179988671087, + "y": 3.7718368324625953, + "heading": -0.756986124262755, + "angularVelocity": 0.8830408285913816, + "velocityX": 1.2553456978648867, + "velocityY": -2.0769571879257267, + "timestamp": 0.5585821768171861 + }, + { + "x": 1.1260747058581024, + "y": 3.644188427808813, + "heading": -0.7048901245652565, + "angularVelocity": 0.9326470098695786, + "velocityX": 1.381295540624565, + "velocityY": -2.2852215833510225, + "timestamp": 0.6144403944989048 + }, + { + "x": 1.210275065586603, + "y": 3.504898185530833, + "heading": -0.6511892097412871, + "angularVelocity": 0.9613789528688276, + "velocityX": 1.5073943140877994, + "velocityY": -2.4936392183449136, + "timestamp": 0.6702986121806234 + }, + { + "x": 1.3015253174439423, + "y": 3.3539635614800978, + "heading": -0.5979268038301901, + "angularVelocity": 0.95352856073188, + "velocityX": 1.6336047880597582, + "velocityY": -2.7021023998790135, + "timestamp": 0.726156829862342 + }, + { + "x": 1.3998011054306767, + "y": 3.1914330516367446, + "heading": -0.5495841694257507, + "angularVelocity": 0.865452504766571, + "velocityX": 1.7593792295112722, + "velocityY": -2.909697383641142, + "timestamp": 0.7820150475440606 + }, + { + "x": 1.504860441802437, + "y": 3.018388823031747, + "heading": -0.5232691895419826, + "angularVelocity": 0.4711031066854229, + "velocityX": 1.8808214929160718, + "velocityY": -3.0979189058807544, + "timestamp": 0.8378732652257792 + }, + { + "x": 1.6140923684634172, + "y": 2.8375675393930413, + "heading": -0.5232691688026271, + "angularVelocity": 3.712856650457212e-7, + "velocityX": 1.9555211604385054, + "velocityY": -3.237147391079152, + "timestamp": 0.8937314829074978 + }, + { + "x": 1.7233256383679056, + "y": 2.6567470671976134, + "heading": -0.5232691480644303, + "angularVelocity": 3.7126492252401944e-7, + "velocityX": 1.9555452078135154, + "velocityY": -3.2371328642411785, + "timestamp": 0.9495897005892164 + }, + { + "x": 1.84078732146222, + "y": 2.4811603268646896, + "heading": -0.5232691272677354, + "angularVelocity": 3.72312181556421e-7, + "velocityX": 2.102854118325333, + "velocityY": -3.143436142796787, + "timestamp": 1.005447918270935 + }, + { + "x": 1.9693130123787856, + "y": 2.313502648911294, + "heading": -0.5232691060970931, + "angularVelocity": 3.790067651022537e-7, + "velocityX": 2.3009271733106282, + "velocityY": -3.0014863508304788, + "timestamp": 1.0613061359526537 + }, + { + "x": 2.1083695489461487, + "y": 2.154470483882732, + "heading": -0.5232690841533676, + "angularVelocity": 3.928468612451328e-7, + "velocityX": 2.4894553091491622, + "velocityY": -2.847068374697022, + "timestamp": 1.1171643536343723 + }, + { + "x": 2.2573796500994945, + "y": 2.0047242347352134, + "heading": -0.5232690609733034, + "angularVelocity": 4.1498037571099624e-7, + "velocityX": 2.6676486887284834, + "velocityY": -2.6808275552359513, + "timestamp": 1.173022571316091 + }, + { + "x": 2.4157246359676448, + "y": 1.8648857059384452, + "heading": -0.5232690359846437, + "angularVelocity": 4.47358702857604e-7, + "velocityX": 2.8347661712087695, + "velocityY": -2.5034549006481424, + "timestamp": 1.2288807889978095 + }, + { + "x": 2.582747044375024, + "y": 1.7355355500002163, + "heading": -0.523269008439205, + "angularVelocity": 4.931313565110266e-7, + "velocityX": 2.9901134575950428, + "velocityY": -2.3156871326484083, + "timestamp": 1.2847390066795281 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.54889811849075, - "angularVelocity": 9.609981531028035e-7, - "velocityX": 3.344548163283859, - "velocityY": -2.583720422924227, - "timestamp": 1.352313152950324 - }, - { - "x": 3.040083158490555, - "y": 1.4307079955900162, - "heading": -0.5488980816236001, - "angularVelocity": 4.6047885659418846e-7, - "velocityX": 3.5263614649855297, - "velocityY": -2.3294620822502163, - "timestamp": 1.4323757866596898 - }, - { - "x": 3.3354250115679163, - "y": 1.2655819134364221, - "heading": -0.5488980521717318, - "angularVelocity": 3.678603468591226e-7, - "velocityX": 3.688885056533252, - "velocityY": -2.062461281917701, - "timestamp": 1.5124384203690555 - }, - { - "x": 3.642163402363816, - "y": 1.1227358560558156, - "heading": -0.5488980275036914, - "angularVelocity": 3.08109279752253e-7, - "velocityX": 3.8312303328569812, - "velocityY": -1.7841788455210303, - "timestamp": 1.5925010540784212 - }, - { - "x": 3.958620462965578, - "y": 1.0029511838587135, - "heading": -0.548898006032631, - "angularVelocity": 2.6817829186367307e-7, - "velocityX": 3.9526186679093493, - "velocityY": -1.496137044807694, - "timestamp": 1.6725636877877879 - }, - { - "x": 4.283065164425698, - "y": 0.9068831020971607, - "heading": -0.5488979867253696, - "angularVelocity": 2.4115196404363913e-7, - "velocityX": 4.052386068609446, - "velocityY": -1.199911585604899, - "timestamp": 1.7526263214971536 - }, - { - "x": 4.613722785487234, - "y": 0.8350570656894738, - "heading": -0.5488979688602151, - "angularVelocity": 2.2313973197090786e-7, - "velocityX": 4.129986808350196, - "velocityY": -0.8971230782737135, - "timestamp": 1.8326889552065193 - }, - { - "x": 4.948784620508865, - "y": 0.7878658651941135, - "heading": -0.5488979518957929, - "angularVelocity": 2.118893824822783e-7, - "velocityX": 4.184996414655578, - "velocityY": -0.5894285300060232, - "timestamp": 1.912751588915885 - }, - { - "x": 5.286417874607001, - "y": 0.7655672355617388, - "heading": -0.5488979353932385, - "angularVelocity": 2.0612055273986908e-7, - "velocityX": 4.217114007562478, - "velocityY": -0.2785148152093341, - "timestamp": 1.9928142226252508 - }, - { - "x": 5.624784390347261, - "y": 0.7668102763452317, - "heading": -0.5488979189757273, - "angularVelocity": 2.0505834761632137e-7, - "velocityX": 4.226272607625093, - "velocityY": 0.015525854397220307, - "timestamp": 2.0728768563346165 - }, - { - "x": 5.963150845273002, - "y": 0.7680694375278687, - "heading": -0.5488979025584118, - "angularVelocity": 2.050559016505279e-7, - "velocityX": 4.226271848038887, - "velocityY": 0.01572720160901255, - "timestamp": 2.152939490043982 - }, - { - "x": 6.287499241199154, - "y": 0.7691154701328422, - "heading": -0.5071298401601563, - "angularVelocity": 0.5216923359002789, - "velocityX": 4.051183191195162, - "velocityY": 0.013065178543461153, - "timestamp": 2.233002123753348 - }, - { - "x": 6.587273814834465, - "y": 0.7701306302032382, - "heading": -0.44617628297839135, - "angularVelocity": 0.7613234084075665, - "velocityX": 3.7442507165521994, - "velocityY": 0.012679573760330002, - "timestamp": 2.3130647574627137 - }, - { - "x": 6.86203616618243, - "y": 0.7710774941037043, - "heading": -0.38194080023757476, - "angularVelocity": 0.8023153844019356, - "velocityX": 3.431842529005624, - "velocityY": 0.011826539495826505, - "timestamp": 2.3931273911720794 - }, - { - "x": 7.11177505601951, - "y": 0.7719477065280066, - "heading": -0.31898594689669185, - "angularVelocity": 0.7863200399997604, - "velocityX": 3.1192939610715853, - "velocityY": 0.010869145608779591, - "timestamp": 2.473190024881445 - }, - { - "x": 7.336503129467627, - "y": 0.7727363298825438, - "heading": -0.2594660169270826, - "angularVelocity": 0.7434170874971441, - "velocityX": 2.80690333350678, - "velocityY": 0.009850080085754406, - "timestamp": 2.553252658590811 - }, - { - "x": 7.536233129593461, - "y": 0.7734405408906285, - "heading": -0.20463677394358387, - "angularVelocity": 0.6848293697469102, - "velocityX": 2.494671869662653, - "velocityY": 0.008795751212569192, - "timestamp": 2.6333152923001766 - }, - { - "x": 7.7109755860070575, - "y": 0.7740586905401403, - "heading": -0.15532295555151873, - "angularVelocity": 0.6159404969245411, - "velocityX": 2.1825719229765794, - "velocityY": 0.007720825813338177, - "timestamp": 2.7133779260095423 - }, - { - "x": 7.860738998574353, - "y": 0.774589785610018, - "heading": -0.1121095095684322, - "angularVelocity": 0.5397454965062622, - "velocityX": 1.8705781414957077, - "velocityY": 0.006633494863411078, - "timestamp": 2.793440559718908 - }, - { - "x": 7.985530277012728, - "y": 0.7750332018658471, - "heading": -0.07543399366174408, - "angularVelocity": 0.4580853040608456, - "velocityX": 1.558670663912856, - "velocityY": 0.005538367087607409, - "timestamp": 2.8735031934282738 - }, - { - "x": 8.085355113462763, - "y": 0.7753885191044545, - "heading": -0.04563658013807198, - "angularVelocity": 0.3721762842804108, - "velocityX": 1.2468342824247154, - "velocityY": 0.004437990881245155, - "timestamp": 2.9535658271376395 - }, - { - "x": 8.160218261803882, - "y": 0.7756554219775593, - "heading": -0.022989482111851405, - "angularVelocity": 0.2828672625037418, - "velocityX": 0.9350572779417692, - "velocityY": 0.0033336759051113586, - "timestamp": 3.033628460847005 - }, - { - "x": 8.210123742972717, - "y": 0.7758336375934914, - "heading": -0.007715443356935036, - "angularVelocity": 0.19077612173431838, - "velocityX": 0.623330495846574, - "velocityY": 0.0022259524516894435, - "timestamp": 3.113691094556371 + "heading": -0.5232689753097743, + "angularVelocity": 5.930985989560848e-7, + "velocityX": 3.133045325838173, + "velocityY": -2.1183039826598433, + "timestamp": 1.3405972243612467 + }, + { + "x": 3.027691141146945, + "y": 1.4695236072310565, + "heading": -0.5232689479285146, + "angularVelocity": 3.365468040910692e-7, + "velocityX": 3.3178420048715034, + "velocityY": -1.8152442667680868, + "timestamp": 1.4219566647921713 + }, + { + "x": 3.3102859882061892, + "y": 1.3477940719025636, + "heading": -0.523268924510295, + "angularVelocity": 2.8783653775450794e-7, + "velocityX": 3.473411881429686, + "velocityY": -1.4961943529078627, + "timestamp": 1.5033161052230959 + }, + { + "x": 3.6030485594101127, + "y": 1.2530945172715204, + "heading": -0.5232689034957836, + "angularVelocity": 2.582922326531371e-7, + "velocityX": 3.5983847682984376, + "velocityY": -1.1639651665432065, + "timestamp": 1.5846755456540205 + }, + { + "x": 3.9033999272897986, + "y": 1.1862590073495238, + "heading": -0.5232688838271395, + "angularVelocity": 2.4174999152857024e-7, + "velocityX": 3.6916597052396023, + "velocityY": -0.8214843854382324, + "timestamp": 1.666034986084945 + }, + { + "x": 4.2086942600952995, + "y": 1.1478754132776305, + "heading": -0.5232688646920325, + "angularVelocity": 2.3519221562308788e-7, + "velocityX": 3.752414362592638, + "velocityY": -0.471777999806691, + "timestamp": 1.7473944265158696 + }, + { + "x": 4.515097551396115, + "y": 1.1196805012022473, + "heading": -0.5232688456111896, + "angularVelocity": 2.345252478429259e-7, + "velocityX": 3.7660447229963117, + "velocityY": -0.3465475171172218, + "timestamp": 1.8287538669467942 + }, + { + "x": 4.821500870663513, + "y": 1.0914858930504614, + "heading": -0.5232688265303463, + "angularVelocity": 2.3452525108540152e-7, + "velocityX": 3.7660450667373904, + "velocityY": -0.3465437815507533, + "timestamp": 1.9101133073777188 + }, + { + "x": 5.127904189931636, + "y": 1.0632912849065406, + "heading": -0.5232688074495031, + "angularVelocity": 2.3452525107290264e-7, + "velocityX": 3.7660450667462855, + "velocityY": -0.34654378145408427, + "timestamp": 1.9914727478086434 + }, + { + "x": 5.4343075091997575, + "y": 1.0350966767626197, + "heading": -0.5232687883686599, + "angularVelocity": 2.3452525075472094e-7, + "velocityX": 3.7660450667462855, + "velocityY": -0.3465437814540822, + "timestamp": 2.072832188239568 + }, + { + "x": 5.740710828467879, + "y": 1.0069020686186991, + "heading": -0.5232687692878167, + "angularVelocity": 2.3452525040262446e-7, + "velocityX": 3.7660450667462855, + "velocityY": -0.3465437814540818, + "timestamp": 2.1541916286704925 + }, + { + "x": 6.047114147736001, + "y": 0.9787074604747784, + "heading": -0.5232687502069736, + "angularVelocity": 2.345252500146494e-7, + "velocityX": 3.766045066746286, + "velocityY": -0.34654378145408327, + "timestamp": 2.235551069101417 + }, + { + "x": 6.353517467003837, + "y": 0.9505128523277552, + "heading": -0.5232687311261306, + "angularVelocity": 2.3452524994005725e-7, + "velocityX": 3.7660450667427767, + "velocityY": -0.3465437814922157, + "timestamp": 2.3169105095323417 + }, + { + "x": 6.659920775239498, + "y": 0.9223181242925758, + "heading": -0.5232687120445318, + "angularVelocity": 2.3453453739742123e-7, + "velocityX": 3.7660449311448136, + "velocityY": -0.3465452550539253, + "timestamp": 2.3982699499632663 + }, + { + "x": 6.946350295372631, + "y": 0.8955257235290481, + "heading": -0.4609901004295649, + "angularVelocity": 0.7654749256522044, + "velocityX": 3.5205443721840264, + "velocityY": -0.3293090589318255, + "timestamp": 2.479629390394191 + }, + { + "x": 7.204305307998309, + "y": 0.8715129005451068, + "heading": -0.38283013166037505, + "angularVelocity": 0.9606748566019065, + "velocityX": 3.170560309404857, + "velocityY": -0.2951448886171808, + "timestamp": 2.5609888308251154 + }, + { + "x": 7.433510095635826, + "y": 0.8502195253793108, + "heading": -0.30543025852913497, + "angularVelocity": 0.9513324172497697, + "velocityX": 2.8171873653938824, + "velocityY": -0.2617197838752883, + "timestamp": 2.64234827125604 + }, + { + "x": 7.633994056446923, + "y": 0.8316157363547472, + "heading": -0.2335287634090791, + "angularVelocity": 0.8837511017679792, + "velocityX": 2.4641757582060952, + "velocityY": -0.22866171308489489, + "timestamp": 2.7237077116869646 + }, + { + "x": 7.805788503499911, + "y": 0.8156861724274476, + "heading": -0.16939985967365537, + "angularVelocity": 0.7882171189447914, + "velocityX": 2.1115490242198023, + "velocityY": -0.19579244698498122, + "timestamp": 2.805067152117889 + }, + { + "x": 7.948917170139319, + "y": 0.802421883536533, + "heading": -0.1143881084380515, + "angularVelocity": 0.6761569517222741, + "velocityX": 1.759214000014262, + "velocityY": -0.16303318730634864, + "timestamp": 2.8864265925488137 + }, + { + "x": 8.063397872753223, + "y": 0.7918170647898031, + "heading": -0.0693839808587397, + "angularVelocity": 0.5531518818338115, + "velocityX": 1.407097959469132, + "velocityY": -0.13034527635097967, + "timestamp": 2.9677860329797383 + }, + { + "x": 8.149244274991434, + "y": 0.7838676118817843, + "heading": -0.0350206058237791, + "angularVelocity": 0.4223649382659615, + "velocityX": 1.0551498607109575, + "velocityY": -0.09770781197503392, + "timestamp": 3.049145473410663 + }, + { + "x": 8.20646710981356, + "y": 0.7785704017832406, + "heading": -0.011770656016753542, + "angularVelocity": 0.2857683101540642, + "velocityX": 0.703333682225969, + "velocityY": -0.06510873317818848, + "timestamp": 3.1305049138415875 }, { "x": 8.235074996948242, "y": 0.7759228944778442, - "heading": -1.0986669650236063e-32, - "angularVelocity": 0.09636759371342492, - "velocityX": 0.3116466798494725, - "velocityY": 0.0011148382228791801, - "timestamp": 3.1937537282657367 + "heading": -3.5413188929792264e-38, + "angularVelocity": 0.1446747415470122, + "velocityX": 0.35162345000356726, + "velocityY": -0.03254087406911382, + "timestamp": 3.211864354272512 }, { "x": 8.235074996948242, "y": 0.7759228944778442, - "heading": 1.3508572737206773e-32, - "angularVelocity": 3.0595153521375356e-31, - "velocityX": -1.0447785999071724e-32, - "velocityY": -1.7698938774257706e-30, - "timestamp": 3.2738163619751024 + "heading": -7.544724392466369e-38, + "angularVelocity": -4.920640404901405e-37, + "velocityX": -6.041520920103385e-38, + "velocityY": 3.1783876166730987e-37, + "timestamp": 3.2932237947034366 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubSprint.traj b/src/main/deploy/choreo/SourceLanePSubHSubSprint.traj index d4b51594..5d530312 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubSprint.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubSprint.traj @@ -5,1386 +5,1332 @@ "y": 4.409571170806885, "heading": -1.0475181007536924, "angularVelocity": 0, - "velocityX": 6.43793008966589e-32, - "velocityY": 2.60583916914808e-32, + "velocityX": 2.1342037757525677e-32, + "velocityY": 1.1024409975331184e-32, "timestamp": 0 }, { - "x": 0.6686935342644011, - "y": 4.399440637290449, - "heading": -1.042618248425991, - "angularVelocity": 0.09045075655350086, - "velocityX": 0.09617887406842093, - "velocityY": -0.18700857894671152, - "timestamp": 0.05417149081338269 - }, - { - "x": 0.6791748314486301, - "y": 4.379207946886432, - "heading": -1.0329282708117224, - "angularVelocity": 0.17887596351464607, - "velocityX": 0.19348363921414355, - "velocityY": -0.37349332832129434, - "timestamp": 0.10834298162676538 - }, - { - "x": 0.6949946213377666, - "y": 4.348905028536486, - "heading": -1.0185695824101737, - "angularVelocity": 0.2650598716401143, - "velocityX": 0.29203165081121235, - "velocityY": -0.5593886728046312, - "timestamp": 0.16251447244014808 - }, - { - "x": 0.7162276537845403, - "y": 4.3085680597239815, - "heading": -0.9996772134807529, - "angularVelocity": 0.34875113543586445, - "velocityX": 0.39195953679621004, - "velocityY": -0.7446161847651872, - "timestamp": 0.21668596325353076 - }, - { - "x": 0.742957361589069, - "y": 4.258238351873032, - "heading": -0.9764021807302357, - "angularVelocity": 0.42965464677164156, - "velocityX": 0.4934275834610263, - "velocityY": -0.9290810921990941, - "timestamp": 0.2708574540669135 - }, - { - "x": 0.775277449766474, - "y": 4.197963491949002, - "heading": -0.9489144431022858, - "angularVelocity": 0.507420733954753, - "velocityX": 0.5966254148098966, - "velocityY": -1.1126675492774045, - "timestamp": 0.32502894488029616 - }, - { - "x": 0.8132938881778625, - "y": 4.1277988368888145, - "heading": -0.9174066544487739, - "angularVelocity": 0.5816304513762527, - "velocityX": 0.7017794386045728, - "velocityY": -1.295232123145776, - "timestamp": 0.37920043569367884 - }, - { - "x": 0.8571274399080208, - "y": 4.047809503293585, - "heading": -0.8820990289920404, - "angularVelocity": 0.6517750375075625, - "velocityX": 0.8091627361920356, - "velocityY": -1.4765946514336408, - "timestamp": 0.4333719265070615 - }, - { - "x": 0.9069169109204649, - "y": 3.958073067611011, - "heading": -0.8432457931383238, - "angularVelocity": 0.7172266310255936, - "velocityX": 0.919108377208329, - "velocityY": -1.6565251266890444, - "timestamp": 0.4875434173204442 - }, - { - "x": 0.9628233844277805, - "y": 3.8586833111714633, - "heading": -0.8011439375928472, - "angularVelocity": 0.7771958074869153, - "velocityX": 1.0320275973188553, - "velocityY": -1.8347244084889383, - "timestamp": 0.541714908133827 - }, - { - "x": 1.0250358185457515, - "y": 3.7497555457988567, - "heading": -0.7561453551591197, - "angularVelocity": 0.8306690799547085, - "velocityX": 1.148434964293097, - "velocityY": -2.010795046196093, - "timestamp": 0.5958863989472096 - }, - { - "x": 1.0937785557910409, - "y": 3.6314344082082353, - "heading": -0.7086740357677194, - "angularVelocity": 0.876315543076626, - "velocityX": 1.2689836704347575, - "velocityY": -2.184195705416893, - "timestamp": 0.6500578897605923 - }, - { - "x": 1.1693215384166689, - "y": 3.503905652509146, - "heading": -0.65925094255443, - "angularVelocity": 0.9123450817247932, - "velocityX": 1.39451548206175, - "velocityY": -2.354167363390805, - "timestamp": 0.704229380573975 - }, - { - "x": 1.2519943506884508, - "y": 3.3674146850873483, - "heading": -0.6085307966283009, - "angularVelocity": 0.9362885378373057, - "velocityX": 1.5261313844321562, - "velocityY": -2.519608845398026, - "timestamp": 0.7584008713873577 - }, - { - "x": 1.3422055318396924, - "y": 3.2222969888664816, - "heading": -0.5573578262997513, - "angularVelocity": 0.9446476284885243, - "velocityX": 1.6652888779083848, - "velocityY": -2.6788573480613262, - "timestamp": 0.8125723622007404 - }, - { - "x": 1.440468425966884, - "y": 3.0690305293119637, - "heading": -0.5068529144587157, - "angularVelocity": 0.9323153393548251, - "velocityX": 1.8139226491975393, - "velocityY": -2.8292826587053166, - "timestamp": 0.866743853014123 - }, - { - "x": 1.5474319674576553, - "y": 2.908330661716264, - "heading": -0.4585565550407312, - "angularVelocity": 0.8915456948445868, - "velocityX": 1.9745356807560286, - "velocityY": -2.9665025861905914, - "timestamp": 0.9209153438275057 - }, - { - "x": 1.66389994755801, - "y": 2.7413290540117057, - "heading": -0.41468584741790443, - "angularVelocity": 0.8098486300470928, - "velocityX": 2.1499866138367625, - "velocityY": -3.0828320431473393, - "timestamp": 0.9750868346408884 - }, - { - "x": 1.7907579832157325, - "y": 2.5699020484114805, - "heading": -0.3786367444287472, - "angularVelocity": 0.6654626344573777, - "velocityX": 2.3417859422540332, - "velocityY": -3.1645244209870533, - "timestamp": 1.029258325454271 - }, - { - "x": 1.9285625356473743, - "y": 2.397072387394727, - "heading": -0.3552615550516524, - "angularVelocity": 0.4315035275218978, - "velocityX": 2.5438574859674787, - "velocityY": -3.1904172918581897, - "timestamp": 1.083429816267654 - }, - { - "x": 2.0769698348954466, - "y": 2.226810347381072, - "heading": -0.3460632427651378, - "angularVelocity": 0.16979987348331044, - "velocityX": 2.739583072567186, - "velocityY": -3.1430192792772944, - "timestamp": 1.1376013070810367 - }, - { - "x": 2.2352281984049327, - "y": 2.0620956413219225, - "heading": -0.3444813887227629, - "angularVelocity": 0.029200858581211304, - "velocityX": 2.9214326785776583, - "velocityY": -3.0406160802659317, - "timestamp": 1.1917727978944195 - }, - { - "x": 2.401905636235413, - "y": 1.90514203890439, - "heading": -0.3444812653381511, - "angularVelocity": 0.0000022776669023009663, - "velocityX": 3.076847901503659, - "velocityY": -2.8973469266007075, - "timestamp": 1.2459442887078023 - }, - { - "x": 2.5762262115041326, - "y": 1.7567232171949336, - "heading": -0.34448120853135994, - "angularVelocity": 0.0000010486473662298194, - "velocityX": 3.217939411511546, - "velocityY": -2.739795775987586, - "timestamp": 1.300115779521185 + "x": 0.6705320996855912, + "y": 4.398099342118358, + "heading": -1.0386114127976833, + "angularVelocity": 0.15927551181553457, + "velocityX": 0.12605002427557185, + "velocityY": -0.20514712032687507, + "timestamp": 0.05592000838348859 + }, + { + "x": 0.6846335258864488, + "y": 4.375148415401169, + "heading": -1.0210111049652704, + "angularVelocity": 0.3147407938803157, + "velocityX": 0.25217138924859706, + "velocityY": -0.41042423598715294, + "timestamp": 0.11184001676697718 + }, + { + "x": 0.7057920861320249, + "y": 4.340710150429397, + "heading": -0.9949634737692402, + "angularVelocity": 0.4658016325283909, + "velocityX": 0.3783719076090775, + "velocityY": -0.6158487090273922, + "timestamp": 0.16776002515046576 + }, + { + "x": 0.7340126708459819, + "y": 4.294775101740391, + "heading": -0.9607567098648513, + "angularVelocity": 0.6117088479280168, + "velocityX": 0.5046598798845962, + "velocityY": -0.8214420923185943, + "timestamp": 0.22368003353395435 + }, + { + "x": 0.7693006604451206, + "y": 4.237332365746024, + "heading": -0.9187315737842261, + "angularVelocity": 0.7515223494321577, + "velocityX": 0.6310440684690272, + "velocityY": -1.0272304610620715, + "timestamp": 0.2796000419174429 + }, + { + "x": 0.8116619611973516, + "y": 4.168369327121192, + "heading": -0.8692948358062461, + "angularVelocity": 0.8840617054087766, + "velocityX": 0.7575338769931041, + "velocityY": -1.2332444257142752, + "timestamp": 0.3355200503009315 + }, + { + "x": 0.8611030812908155, + "y": 4.087871426229515, + "heading": -0.8129373545696884, + "angularVelocity": 1.0078231900479868, + "velocityX": 0.8841400694078276, + "velocityY": -1.4395187557848361, + "timestamp": 0.3914400586844201 + }, + { + "x": 0.9176312901451578, + "y": 3.995821968070834, + "heading": -0.7502611156342618, + "angularVelocity": 1.120819555419328, + "velocityX": 1.0108762585778377, + "velocityY": -1.6460916373156267, + "timestamp": 0.4473600670679087 + }, + { + "x": 0.9812548770105831, + "y": 3.8922019971306105, + "heading": -0.6820246305048668, + "angularVelocity": 1.220251697057024, + "velocityX": 1.1377606818136927, + "velocityY": -1.8530034943775027, + "timestamp": 0.5032800754513973 + }, + { + "x": 1.0519833618359966, + "y": 3.7769903402374796, + "heading": -0.6092267013635415, + "angularVelocity": 1.3018225720227237, + "velocityX": 1.2648153473148889, + "velocityY": -2.0602939846330393, + "timestamp": 0.5592000838348858 + }, + { + "x": 1.1298269082953458, + "y": 3.6501643798944903, + "heading": -0.5332724156478241, + "angularVelocity": 1.3582667083101572, + "velocityX": 1.392051766614792, + "velocityY": -2.26798893650447, + "timestamp": 0.6151200922183744 + }, + { + "x": 1.2147919549002646, + "y": 3.511704401985981, + "heading": -0.45632702327380176, + "angularVelocity": 1.375990358340898, + "velocityX": 1.5194033238021896, + "velocityY": -2.4760364297333126, + "timestamp": 0.6710401006018629 + }, + { + "x": 1.3068601679390515, + "y": 3.3616165956403266, + "heading": -0.38215505844470493, + "angularVelocity": 1.3263940219829704, + "velocityX": 1.6464270249639619, + "velocityY": -2.683973244717373, + "timestamp": 0.7269601089853515 + }, + { + "x": 1.4058792704184795, + "y": 3.2000739494751453, + "heading": -0.3184323584737973, + "angularVelocity": 1.139533090444293, + "velocityX": 1.7707276043375095, + "velocityY": -2.8888165584195344, + "timestamp": 0.78288011736884 + }, + { + "x": 1.512169170978474, + "y": 3.0300749520186443, + "heading": -0.28662541962876376, + "angularVelocity": 0.5687935278354703, + "velocityX": 1.900749009747616, + "velocityY": -3.0400388406718526, + "timestamp": 0.8388001257523285 + }, + { + "x": 1.6244650109400058, + "y": 2.850864532661705, + "heading": -0.2866253125429214, + "angularVelocity": 0.0000019149825876044375, + "velocityX": 2.008151343458836, + "velocityY": -3.2047638141959607, + "timestamp": 0.8947201341358171 + }, + { + "x": 1.7352882055291936, + "y": 2.6707396888206167, + "heading": -0.2866252783553297, + "angularVelocity": 6.113659974508405e-7, + "velocityX": 1.9818164873864779, + "velocityY": -3.221116180917336, + "timestamp": 0.9506401425193056 + }, + { + "x": 1.8506362591615508, + "y": 2.493478394875359, + "heading": -0.2866252441530604, + "angularVelocity": 6.116284725625113e-7, + "velocityX": 2.0627331248114724, + "velocityY": -3.169908214777688, + "timestamp": 1.0065601509027942 + }, + { + "x": 1.9771720781105002, + "y": 2.324022128559085, + "heading": -0.2866252095630558, + "angularVelocity": 6.185622200419991e-7, + "velocityX": 2.2628004288052184, + "velocityY": -3.0303333496335663, + "timestamp": 1.0624801592862827 + }, + { + "x": 2.114370117744969, + "y": 2.1630766803808767, + "heading": -0.28662517392184755, + "angularVelocity": 6.373605669269748e-7, + "velocityX": 2.4534695827223696, + "velocityY": -2.8781370538157938, + "timestamp": 1.1184001676697712 + }, + { + "x": 2.2616596068294545, + "y": 2.011311910787294, + "heading": -0.28662513647276167, + "angularVelocity": 6.696902771759525e-7, + "velocityX": 2.6339318133574414, + "velocityY": -2.713961853381871, + "timestamp": 1.1743201760532598 + }, + { + "x": 2.418427669547749, + "y": 1.8693594092228303, + "heading": -0.2866250962912349, + "angularVelocity": 7.185536621043778e-7, + "velocityX": 2.8034341776776834, + "velocityY": -2.538492136678183, + "timestamp": 1.2302401844367483 + }, + { + "x": 2.5840219554137547, + "y": 1.7378099138847216, + "heading": -0.28662505217416917, + "angularVelocity": 7.889316725786441e-7, + "velocityX": 2.961270762521926, + "velocityY": -2.3524584337678855, + "timestamp": 1.2861601928202369 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.3444811403993986, - "angularVelocity": 0.0000012577088124740217, - "velocityX": 3.3509722173532026, - "velocityY": -2.575383288874084, - "timestamp": 1.3542872703345679 - }, - { - "x": 2.960734343821119, - "y": 1.4779114817648484, - "heading": -0.34448108051633547, - "angularVelocity": 0.0000010280352851970435, - "velocityX": 3.4846514266808644, - "velocityY": -2.391405414524714, - "timestamp": 1.4125372778029122 - }, - { - "x": 3.170914398347007, - "y": 1.349732150860621, - "heading": -0.34448103072468966, - "angularVelocity": 8.54792093887579e-7, - "velocityX": 3.6082408167949036, - "velocityY": -2.200503252705787, - "timestamp": 1.4707872852712565 - }, - { - "x": 3.387684960924611, - "y": 1.2330440117382415, - "heading": -0.3444809881196843, - "angularVelocity": 7.31416307943616e-7, - "velocityX": 3.7213825714169735, - "velocityY": -2.00322959934028, - "timestamp": 1.5290372927396008 - }, - { - "x": 3.610418374312036, - "y": 1.1281849298444195, - "heading": -0.3444809507762438, - "angularVelocity": 6.410890256196379e-7, - "velocityX": 3.8237490957999993, - "velocityY": -1.80015568153888, - "timestamp": 1.587287300207945 - }, - { - "x": 3.8384697161434858, - "y": 1.0354585179376077, - "heading": -0.34448091735896624, - "angularVelocity": 5.736870952656925e-7, - "velocityX": 3.9150439930051975, - "velocityY": -1.591869528209129, - "timestamp": 1.6455373076762894 - }, - { - "x": 4.071178666241736, - "y": 0.9551332560900274, - "heading": -0.3444808869037787, - "angularVelocity": 5.228357697616013e-7, - "velocityX": 3.995002922956148, - "velocityY": -1.3789742755180425, - "timestamp": 1.7037873151446337 - }, - { - "x": 4.307871418551335, - "y": 0.8874417125102056, - "heading": -0.34448085868861794, - "angularVelocity": 4.843803799821142e-7, - "velocityX": 4.063394368459522, - "velocityY": -1.1620864360679888, - "timestamp": 1.762037322612978 - }, - { - "x": 4.547862632165302, - "y": 0.8325798662811171, - "heading": -0.3444808321530075, - "angularVelocity": 4.555469028757283e-7, - "velocityX": 4.120020306338885, - "velocityY": -0.9418341492729086, - "timestamp": 1.8202873300813223 - }, - { - "x": 4.7904574158495095, - "y": 0.7907065304618335, - "heading": -0.3444808068457288, - "angularVelocity": 4.3445966396412105e-7, - "velocityX": 4.164716782500731, - "velocityY": -0.7188554583798012, - "timestamp": 1.8785373375496666 - }, - { - "x": 5.03495334055107, - "y": 0.7619428639166234, - "heading": -0.34448078238917407, - "angularVelocity": 4.198549626958455e-7, - "velocityX": 4.197354392348067, - "velocityY": -0.4937967872509155, - "timestamp": 1.9367873450180109 - }, - { - "x": 5.280642475492497, - "y": 0.7463718997323922, - "heading": -0.3444807584537773, - "angularVelocity": 4.109080464550885e-7, - "velocityX": 4.217838685685062, - "velocityY": -0.26731265558537776, - "timestamp": 1.9950373524863552 - }, - { - "x": 5.526813463070627, - "y": 0.7440371744430943, - "heading": -0.34448073473848534, - "angularVelocity": 4.0712942424247657e-7, - "velocityX": 4.226110832894084, - "velocityY": -0.040081115707442425, - "timestamp": 2.0532873599546995 - }, - { - "x": 5.766738873929576, - "y": 0.7458613898059425, - "heading": -0.32467851072076565, - "angularVelocity": 0.33995230006590477, - "velocityX": 4.118890645453325, - "velocityY": 0.03131699792210607, - "timestamp": 2.1115373674230438 - }, - { - "x": 5.994439408520209, - "y": 0.7477937318314798, - "heading": -0.2833894042686398, - "angularVelocity": 0.7088257709591617, - "velocityX": 3.9090215518749, - "velocityY": 0.033173249404078294, - "timestamp": 2.169787374891388 - }, - { - "x": 6.208925057122028, - "y": 0.7495883201901966, - "heading": -0.23844368677979322, - "angularVelocity": 0.7716002013093647, - "velocityX": 3.6821565854456084, - "velocityY": 0.030808379890630424, - "timestamp": 2.2280373823597324 - }, - { - "x": 6.410164836489075, - "y": 0.7512394294870858, - "heading": -0.1938705620031709, - "angularVelocity": 0.7652037607178938, - "velocityX": 3.454759717866313, - "velocityY": 0.028345220346736016, - "timestamp": 2.2862873898280767 - }, - { - "x": 6.598169956514198, - "y": 0.752743774939267, - "heading": -0.15150662184462524, - "angularVelocity": 0.7272778493902874, - "velocityX": 3.227555294774738, - "velocityY": 0.02582566968766568, - "timestamp": 2.344537397296421 - }, - { - "x": 6.772954120733039, - "y": 0.754099580476696, - "heading": -0.11241242955567449, - "angularVelocity": 0.6711448459503878, - "velocityX": 3.000586125483803, - "velocityY": 0.023275628559629353, - "timestamp": 2.4027874047647653 - }, - { - "x": 6.934529138966309, - "y": 0.7553058783616067, - "heading": -0.07728200268237817, - "angularVelocity": 0.6030973797280247, - "velocityX": 2.77381969987003, - "velocityY": 0.020708973909851786, - "timestamp": 2.4610374122331096 - }, - { - "x": 7.082904706348492, - "y": 0.7563621152126468, - "heading": -0.046606204008256526, - "angularVelocity": 0.52662308568445, - "velocityX": 2.547219714311956, - "velocityY": 0.018132819152241828, - "timestamp": 2.519287419701454 - }, - { - "x": 7.218088767056845, - "y": 0.7572679488303273, - "heading": -0.020751033175153387, - "angularVelocity": 0.4438655367924897, - "velocityX": 2.3207561094617386, - "velocityY": 0.015550789726041245, - "timestamp": 2.577537427169798 + "heading": -0.28662500163792387, + "angularVelocity": 9.037238500487202e-7, + "velocityX": 3.1067845266977017, + "velocityY": -2.1566350283233993, + "timestamp": 1.3420802012037254 + }, + { + "x": 2.964172022537395, + "y": 1.4952512181839386, + "heading": -0.2866249560195876, + "angularVelocity": 7.195928187708238e-7, + "velocityX": 3.256089340971223, + "velocityY": -1.923816018708688, + "timestamp": 1.4054748531095118 + }, + { + "x": 3.1789518158724, + "y": 1.3887033110695066, + "heading": -0.28662491569189824, + "angularVelocity": 6.361370890269852e-7, + "velocityX": 3.387979693526802, + "velocityY": -1.6807081340674828, + "timestamp": 1.4688695050152982 + }, + { + "x": 3.4009440542760783, + "y": 1.2981369637223683, + "heading": -0.2866248790058733, + "angularVelocity": 5.786927429417107e-7, + "velocityX": 3.5017502538477667, + "velocityY": -1.4286117933375972, + "timestamp": 1.5322641569210846 + }, + { + "x": 3.6289614625982565, + "y": 1.2240364953903844, + "heading": -0.286624844765475, + "angularVelocity": 5.401149349395126e-7, + "velocityX": 3.5967924969608642, + "velocityY": -1.1688757033023536, + "timestamp": 1.595658808826871 + }, + { + "x": 3.861784529858251, + "y": 1.1667980667810438, + "heading": -0.2866248120410044, + "angularVelocity": 5.162023864828723e-7, + "velocityX": 3.672597928386812, + "velocityY": -0.9028904945231823, + "timestamp": 1.6590534607326575 + }, + { + "x": 4.0981679647792335, + "y": 1.1267270015307351, + "heading": -0.2866247800525485, + "angularVelocity": 5.045923424053189e-7, + "velocityX": 3.728759884544863, + "velocityY": -0.6320890492444079, + "timestamp": 1.7224481126384439 + }, + { + "x": 4.3358126512762265, + "y": 1.0949807955483848, + "heading": -0.2866247482072254, + "angularVelocity": 5.023345364444787e-7, + "velocityX": 3.7486551207847545, + "velocityY": -0.5007710434238178, + "timestamp": 1.7858427645442303 + }, + { + "x": 4.573457419312962, + "y": 1.0632351999594603, + "heading": -0.28662471636190656, + "angularVelocity": 5.023344691113163e-7, + "velocityX": 3.7486564070090496, + "velocityY": -0.5007614149550478, + "timestamp": 1.8492374164500167 + }, + { + "x": 4.811102187354235, + "y": 1.0314896044045174, + "heading": -0.28662468451658774, + "angularVelocity": 5.023344695763415e-7, + "velocityX": 3.748656407080655, + "velocityY": -0.5007614144190159, + "timestamp": 1.9126320683558031 + }, + { + "x": 5.04874695539551, + "y": 0.9997440088495765, + "heading": -0.28662465267126885, + "angularVelocity": 5.023344699297441e-7, + "velocityX": 3.7486564070806594, + "velocityY": -0.5007614144189861, + "timestamp": 1.9760267202615895 + }, + { + "x": 5.286391723436784, + "y": 0.9679984132946355, + "heading": -0.28662462082595, + "angularVelocity": 5.023344691743294e-7, + "velocityX": 3.748656407080659, + "velocityY": -0.5007614144189861, + "timestamp": 2.039421372167376 + }, + { + "x": 5.524036491478059, + "y": 0.936252817739695, + "heading": -0.28662458898063115, + "angularVelocity": 5.023344697631327e-7, + "velocityX": 3.74865640708066, + "velocityY": -0.5007614144189809, + "timestamp": 2.1028160240731624 + }, + { + "x": 5.761681259520114, + "y": 0.9045072221906025, + "heading": -0.2866245571353122, + "angularVelocity": 5.023344707942377e-7, + "velocityX": 3.748656407092983, + "velocityY": -0.5007614143267316, + "timestamp": 2.166210675978949 + }, + { + "x": 5.99932604159473, + "y": 0.8727617316882137, + "heading": -0.2866245252899934, + "angularVelocity": 5.023344694954779e-7, + "velocityX": 3.748656628445413, + "velocityY": -0.500759757298884, + "timestamp": 2.229605327884735 + }, + { + "x": 6.237212534637086, + "y": 0.8428813906316718, + "heading": -0.2866244915581877, + "angularVelocity": 5.320922919551869e-7, + "velocityX": 3.752469425905036, + "velocityY": -0.47133851450037834, + "timestamp": 2.2929999797905216 + }, + { + "x": 6.4638245505635705, + "y": 0.8220107849523658, + "heading": -0.24424436180847547, + "angularVelocity": 0.6685126974542774, + "velocityX": 3.574623554416899, + "velocityY": -0.3292171350719418, + "timestamp": 2.356394631696308 + }, + { + "x": 6.673647733933235, + "y": 0.8041360764949451, + "heading": -0.18514255243378605, + "angularVelocity": 0.9322838377994912, + "velocityX": 3.309793130207447, + "velocityY": -0.2819592492436898, + "timestamp": 2.4197892836020944 + }, + { + "x": 6.866141134149738, + "y": 0.7888964331390943, + "heading": -0.1276691267098072, + "angularVelocity": 0.9065973863125333, + "velocityX": 3.0364296424022625, + "velocityY": -0.240393201913927, + "timestamp": 2.483183935507881 + }, + { + "x": 7.04134978279503, + "y": 0.7761674362974819, + "heading": -0.07636429138531178, + "angularVelocity": 0.8092927996630026, + "velocityX": 2.7637764918352534, + "velocityY": -0.2007897584252007, + "timestamp": 2.5465785874136673 + }, + { + "x": 7.19932046714385, + "y": 0.765888430525111, + "heading": -0.03338684229285246, + "angularVelocity": 0.6779349330023924, + "velocityX": 2.4918613731579056, + "velocityY": -0.16214310613530794, + "timestamp": 2.6099732393194537 }, { "x": 7.340087890625, "y": 0.7580231428146362, "heading": 0, - "angularVelocity": 0.35624086720384496, - "velocityX": 2.0944052862903964, - "velocityY": 0.012964701931058227, - "timestamp": 2.6357874346381425 - }, - { - "x": 7.452016074368774, - "y": 0.7586421596982608, - "heading": 0.015744597720939148, - "angularVelocity": 0.26174771257837953, - "velocityX": 1.8607618046043786, - "velocityY": 0.010290910965644757, - "timestamp": 2.6959392409933733 - }, - { - "x": 7.549878171503854, - "y": 0.7590935449079357, - "heading": 0.02631445528043402, - "angularVelocity": 0.17571970319684052, - "velocityX": 1.626918675677823, - "velocityY": 0.007504100658407957, - "timestamp": 2.756091047348604 - }, - { - "x": 7.633665265890791, - "y": 0.7593717259908211, - "heading": 0.032122080946348684, - "angularVelocity": 0.09654948068587346, - "velocityX": 1.3929273194578256, - "velocityY": 0.004624650525748951, - "timestamp": 2.816242853703835 - }, - { - "x": 7.703370537390185, - "y": 0.7594720434745997, - "heading": 0.03350862227223397, - "angularVelocity": 0.023050701382050013, - "velocityX": 1.1588225811166206, - "velocityY": 0.001667738507906577, - "timestamp": 2.876394660059066 - }, - { - "x": 7.758988639574762, - "y": 0.7593905387990408, - "heading": 0.0307610262741512, - "angularVelocity": -0.04567769722253501, - "velocityX": 0.9246289605355835, - "velocityY": -0.0013549830087877977, - "timestamp": 2.936546466414297 - }, - { - "x": 7.800515294542927, - "y": 0.7591238044421035, - "heading": 0.024124065366989432, - "angularVelocity": -0.11033685119889487, - "velocityX": 0.6903642215318662, - "velocityY": -0.004434353232251388, - "timestamp": 2.9966982727695277 - }, - { - "x": 7.827947019588505, - "y": 0.7586688745154345, - "heading": 0.01380900494204492, - "angularVelocity": -0.17148380156745927, - "velocityX": 0.4560415839148344, - "velocityY": -0.007563030177051137, - "timestamp": 3.0568500791247586 + "angularVelocity": 0.5266507708326894, + "velocityX": 2.220493673351971, + "velocityY": -0.12406863156475356, + "timestamp": 2.67336789122524 + }, + { + "x": 7.464023522460178, + "y": 0.7525372143128384, + "heading": 0.02301011936038792, + "angularVelocity": 0.3617896361662683, + "velocityX": 1.9486481772395665, + "velocityY": -0.08625561847872572, + "timestamp": 2.736968716290925 + }, + { + "x": 7.570582319535539, + "y": 0.7491648354718851, + "heading": 0.036983700221316605, + "angularVelocity": 0.21970754068830917, + "velocityX": 1.675431049287655, + "velocityY": -0.053024136675436706, + "timestamp": 2.8005695413566096 + }, + { + "x": 7.659708944521439, + "y": 0.7476928551810322, + "heading": 0.04295335113137197, + "angularVelocity": 0.09386121805637201, + "velocityX": 1.4013438488235506, + "velocityY": -0.02314404395434427, + "timestamp": 2.8641703664222944 + }, + { + "x": 7.731365798341117, + "y": 0.7479588271083956, + "heading": 0.041693436231508264, + "angularVelocity": -0.019809725716020715, + "velocityX": 1.1266654755763426, + "velocityY": 0.0041818942928592215, + "timestamp": 2.927771191487979 + }, + { + "x": 7.785525986892142, + "y": 0.7498349370538775, + "heading": 0.03380693253041853, + "angularVelocity": -0.12400002190136777, + "velocityX": 0.8515642445690119, + "velocityY": 0.029498201376229192, + "timestamp": 2.991372016553664 + }, + { + "x": 7.822169492500932, + "y": 0.7532180353995639, + "heading": 0.0197770843540876, + "angularVelocity": -0.220592235428419, + "velocityX": 0.5761482743493489, + "velocityY": 0.053192680160869574, + "timestamp": 3.0549728416193487 }, { "x": 7.841280937194824, "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": -0.2295692478542545, - "velocityX": 0.22167110872074697, - "velocityY": -0.010735034239617657, - "timestamp": 3.1170018854799895 - }, - { - "x": 7.835826713549701, - "y": 0.7569095778214322, - "heading": -0.022558536561860222, - "angularVelocity": -0.299041202712131, - "velocityX": -0.07230245606695226, - "velocityY": -0.014761676314980546, - "timestamp": 3.1924381008150915 - }, - { - "x": 7.80819402127163, - "y": 0.7554921737702587, - "heading": -0.05020545416339097, - "angularVelocity": -0.3664939641883925, - "velocityX": -0.36630538999500056, - "velocityY": -0.018789437472137644, - "timestamp": 3.2678743161501935 - }, - { - "x": 7.758380386118448, - "y": 0.7537708531649631, - "heading": -0.08276554497145149, - "angularVelocity": -0.43162412991455607, - "velocityX": -0.6603411230520984, - "velocityY": -0.02281822593630055, - "timestamp": 3.3433105314852956 - }, - { - "x": 7.686383032454013, - "y": 0.7517455632733434, - "heading": -0.12003503692740472, - "angularVelocity": -0.494053045879821, - "velocityX": -0.9544136505869183, - "velocityY": -0.026847713430781565, - "timestamp": 3.4187467468203976 - }, - { - "x": 7.592198832311897, - "y": 0.7494162995505907, - "heading": -0.1617736436152976, - "angularVelocity": -0.5532966692785698, - "velocityX": -1.2485276431715386, - "velocityY": -0.030877261172313774, - "timestamp": 3.4941829621554996 - }, - { - "x": 7.475824246300941, - "y": 0.7467831362501152, - "heading": -0.2076930920063058, - "angularVelocity": -0.6087188784196724, - "velocityX": -1.542688554747834, - "velocityY": -0.0349058245933778, - "timestamp": 3.5696191774906016 - }, - { - "x": 7.337255260610424, - "y": 0.7438462658074466, - "heading": -0.257440012481047, - "angularVelocity": -0.6594567377718513, - "velocityX": -1.8369026743318102, - "velocityY": -0.03893183704435113, - "timestamp": 3.6450553928257037 - }, - { - "x": 7.1764873367279565, - "y": 0.7406060479084567, - "heading": -0.31056943932971864, - "angularVelocity": -0.7042960282758145, - "velocityX": -2.131176957490571, - "velocityY": -0.04295308141581948, - "timestamp": 3.7204916081608057 - }, - { - "x": 6.9935154268341, - "y": 0.7370630667353528, - "heading": -0.3665018715735503, - "angularVelocity": -0.741453319143453, - "velocityX": -2.425518155716889, - "velocityY": -0.04696658173219917, - "timestamp": 3.7959278234959077 - }, - { - "x": 6.788334219229139, - "y": 0.7332181873732633, - "heading": -0.42444960466432774, - "angularVelocity": -0.768168615476831, - "velocityX": -2.719929767068861, - "velocityY": -0.050968614279096394, - "timestamp": 3.8713640388310098 - }, - { - "x": 6.560939151808715, - "y": 0.7290725776308253, - "heading": -0.48328030857318144, - "angularVelocity": -0.7798734818219146, - "velocityX": -3.0144018547363567, - "velocityY": -0.05495516608332171, - "timestamp": 3.946800254166112 - }, - { - "x": 6.311330185153471, - "y": 0.724627567965158, - "heading": -0.5412355901112249, - "angularVelocity": -0.7682686794478624, - "velocityX": -3.3088744649560358, - "velocityY": -0.05892408103881697, - "timestamp": 4.022236469501213 - }, - { - "x": 6.039527461477683, - "y": 0.7198838011192613, - "heading": -0.5952481542708341, - "angularVelocity": -0.7160031016889598, - "velocityX": -3.6030800653027115, - "velocityY": -0.06288447564374298, - "timestamp": 4.097672684836315 - }, - { - "x": 5.745659621866784, - "y": 0.7148372933474686, - "heading": -0.638783124761932, - "angularVelocity": -0.5771096852845462, - "velocityX": -3.895580369527838, - "velocityY": -0.06689767970695078, - "timestamp": 4.173108900171417 - }, - { - "x": 5.431003034025849, - "y": 0.7096634777046316, - "heading": -0.6507371958403388, - "angularVelocity": -0.15846594404696066, - "velocityX": -4.17116084685812, - "velocityY": -0.0685853024287286, - "timestamp": 4.248545115506518 - }, - { - "x": 5.1125615609654815, - "y": 0.7251156489118818, - "heading": -0.6507372106959876, - "angularVelocity": -1.9692993052752515e-7, - "velocityX": -4.221334164840962, - "velocityY": 0.2048375722271958, - "timestamp": 4.32398133084162 - }, - { - "x": 4.795969345130646, - "y": 0.7627070022190265, - "heading": -0.6507372258766643, - "angularVelocity": -2.012385798908336e-7, - "velocityX": -4.196819981337514, - "velocityY": 0.49831971474386, - "timestamp": 4.399417546176721 - }, - { - "x": 4.482763788472813, - "y": 0.8222554661406114, - "heading": -0.6507372417212374, - "angularVelocity": -2.1003934401896774e-7, - "velocityX": -4.151925640311049, - "velocityY": 0.7893882753404204, - "timestamp": 4.474853761511823 - }, - { - "x": 4.174465853368726, - "y": 0.9034719819930799, - "heading": -0.6507372586342053, - "angularVelocity": -2.2420223152003646e-7, - "velocityX": -4.0868690685842415, - "velocityY": 1.0766250068576386, - "timestamp": 4.5502899768469245 - }, - { - "x": 3.8725726714422697, - "y": 1.0059621985442242, - "heading": -0.6507372771356286, - "angularVelocity": -2.452591658573351e-7, - "velocityX": -4.0019661721546855, - "velocityY": 1.3586341268031932, - "timestamp": 4.625726192182026 - }, - { - "x": 3.5785502725752965, - "y": 1.1292284338722989, - "heading": -0.6507372979388782, - "angularVelocity": -2.757727119617825e-7, - "velocityX": -3.897629242942143, - "velocityY": 1.6340458595450806, - "timestamp": 4.701162407517128 - }, - { - "x": 3.293826465550643, - "y": 1.2726721052481516, - "heading": -0.6507373220853944, - "angularVelocity": -3.200918277895427e-7, - "velocityX": -3.7743649487166713, - "velocityY": 1.9015226405334464, - "timestamp": 4.776598622852229 - }, - { - "x": 3.019783904481099, - "y": 1.4355966406757221, - "heading": -0.6507373511975181, - "angularVelocity": -3.859170793282021e-7, - "velocityX": -3.632771870277356, - "velocityY": 2.1597655012758525, - "timestamp": 4.852034838187331 + "heading": -3.3329541710174495e-31, + "angularVelocity": -0.3109564118651419, + "velocityX": 0.3004905152433859, + "velocityY": 0.07555102327854826, + "timestamp": 3.1185736666850334 + }, + { + "x": 7.837626456422163, + "y": 0.7661832239405314, + "heading": -0.033360531548023514, + "angularVelocity": -0.4178611188210231, + "velocityX": -0.04577461309858743, + "velocityY": 0.10221002096529454, + "timestamp": 3.1984100765480727 + }, + { + "x": 7.806322741144082, + "y": 0.7764707746744536, + "heading": -0.07501113915617397, + "angularVelocity": -0.521699406068016, + "velocityX": -0.3920982335225694, + "velocityY": 0.12885788265743237, + "timestamp": 3.278246486411112 + }, + { + "x": 7.74736425619444, + "y": 0.7888847549403223, + "heading": -0.12465356813987527, + "angularVelocity": -0.6218018704606526, + "velocityX": -0.7384911852973625, + "velocityY": 0.15549271675874526, + "timestamp": 3.3580828962741514 + }, + { + "x": 7.660744366778727, + "y": 0.8034239689194147, + "heading": -0.18191677738707465, + "angularVelocity": -0.7172568173523234, + "velocityX": -1.0849672419427567, + "velocityY": 0.182112572497119, + "timestamp": 3.4379193061371907 + }, + { + "x": 7.5464550145644145, + "y": 0.8200870575241466, + "heading": -0.24632578527266566, + "angularVelocity": -0.8067623280666746, + "velocityX": -1.4315442341455173, + "velocityY": 0.20871540482992249, + "timestamp": 3.51775571600023 + }, + { + "x": 7.404486266909035, + "y": 0.8388724767291539, + "heading": -0.3172478637527479, + "angularVelocity": -0.8883425319569104, + "velocityX": -1.7782456387872363, + "velocityY": 0.23529889729803757, + "timestamp": 3.5975921258632693 + }, + { + "x": 7.234825726560123, + "y": 0.8597784078771568, + "heading": -0.39379130752240776, + "angularVelocity": -0.958753579989021, + "velocityX": -2.125102326619727, + "velocityY": 0.26185960996827495, + "timestamp": 3.6774285357263086 + }, + { + "x": 7.037458048630302, + "y": 0.8828023770067689, + "heading": -0.47459284644074845, + "angularVelocity": -1.0120888333650886, + "velocityX": -2.4721512185781784, + "velocityY": 0.28838933475478157, + "timestamp": 3.757264945589348 + }, + { + "x": 6.812366694532574, + "y": 0.9079395818644891, + "heading": -0.5572950195818098, + "angularVelocity": -1.0358954427301785, + "velocityX": -2.8194072664825045, + "velocityY": 0.31485890837079844, + "timestamp": 3.837101355452387 + }, + { + "x": 6.559557151686244, + "y": 0.9351738572850867, + "heading": -0.6368741954392069, + "angularVelocity": -0.9967779863087077, + "velocityX": -3.1665945810943685, + "velocityY": 0.3411260033776371, + "timestamp": 3.9169377653154265 + }, + { + "x": 6.2794552371906684, + "y": 0.9644007497338526, + "heading": -0.6963848347023109, + "angularVelocity": -0.7454072567290455, + "velocityX": -3.5084482753682624, + "velocityY": 0.36608475379723326, + "timestamp": 3.996774175178466 + }, + { + "x": 5.978414020607044, + "y": 0.9876514129078965, + "heading": -0.6963848502753706, + "angularVelocity": -1.9506212382871808e-7, + "velocityX": -3.7707258768281893, + "velocityY": 0.2912288167006853, + "timestamp": 4.0766105850415055 + }, + { + "x": 5.677372687056552, + "y": 1.0109005615970947, + "heading": -0.6963848658464015, + "angularVelocity": -1.950367123738679e-7, + "velocityX": -3.770727341909936, + "velocityY": 0.29120984684910617, + "timestamp": 4.156446994904545 + }, + { + "x": 5.376331353502739, + "y": 1.0341497102432655, + "heading": -0.6963848814174324, + "angularVelocity": -1.950367127895845e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.29120984631016156, + "timestamp": 4.236283404767585 + }, + { + "x": 5.075290019948924, + "y": 1.0573988588894352, + "heading": -0.6963848969884634, + "angularVelocity": -1.950367129001825e-7, + "velocityX": -3.770727341951559, + "velocityY": 0.2912098463101467, + "timestamp": 4.316119814630625 + }, + { + "x": 4.774248686395111, + "y": 1.0806480075356062, + "heading": -0.6963849125594943, + "angularVelocity": -1.9503671289084429e-7, + "velocityX": -3.7707273419515577, + "velocityY": 0.2912098463101629, + "timestamp": 4.3959562244936645 + }, + { + "x": 4.473207352844907, + "y": 1.1038971562285251, + "heading": -0.6963849281305252, + "angularVelocity": -1.950367124712723e-7, + "velocityX": -3.770727341906337, + "velocityY": 0.2912098468957114, + "timestamp": 4.475792634356704 + }, + { + "x": 4.172166146375442, + "y": 1.1271479503662634, + "heading": -0.6963849437015552, + "angularVelocity": -1.9503670107200537e-7, + "velocityX": -3.770725750142127, + "velocityY": 0.29123045710128126, + "timestamp": 4.555629044219744 + }, + { + "x": 3.8735860999949274, + "y": 1.1720518831992321, + "heading": -0.6963849593703558, + "angularVelocity": -1.9626133737167217e-7, + "velocityX": -3.7398982105123864, + "velocityY": 0.5624492998871315, + "timestamp": 4.635465454082784 + }, + { + "x": 3.580403442801602, + "y": 1.244234542261993, + "heading": -0.6963849756495777, + "angularVelocity": -2.0390723857242194e-7, + "velocityX": -3.672292600535076, + "velocityY": 0.9041320769131688, + "timestamp": 4.7153018639458235 + }, + { + "x": 3.295105029858622, + "y": 1.3430840304472003, + "heading": -0.6963849932202171, + "angularVelocity": -2.20083034395915e-7, + "velocityX": -3.5735376056164516, + "velocityY": 1.238150467371779, + "timestamp": 4.795138273808863 + }, + { + "x": 3.0201108216375614, + "y": 1.4677619739384065, + "heading": -0.6963850130039021, + "angularVelocity": -2.4780278968053093e-7, + "velocityX": -3.444471121544888, + "velocityY": 1.5616677115753714, + "timestamp": 4.874974683671903 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.6507373850576595, - "angularVelocity": -4.4885790275828266e-7, - "velocityX": -3.473537625459172, - "velocityY": 2.40752036058895, - "timestamp": 4.927471053522432 - }, - { - "x": 2.56955568155037, - "y": 1.762694428612382, - "heading": -0.6507374187199763, - "angularVelocity": -5.980784656519904e-7, - "velocityX": -3.34370885712208, - "velocityY": 2.5848068511762112, - "timestamp": 4.98375516756578 - }, - { - "x": 2.389174031729705, - "y": 1.917763125748239, - "heading": -0.6507374476220785, - "angularVelocity": -5.135037275394257e-7, - "velocityX": -3.2048412396034367, - "velocityY": 2.7551059436847196, - "timestamp": 5.040039281609128 - }, - { - "x": 2.21709605626624, - "y": 2.0819977501361024, - "heading": -0.6507374788972222, - "angularVelocity": -5.556655576645871e-7, - "velocityX": -3.0573098357902078, - "velocityY": 2.917956996913463, - "timestamp": 5.096323395652476 - }, - { - "x": 2.053846284333076, - "y": 2.2548939585643604, - "heading": -0.6510333038480676, - "angularVelocity": -0.005255922667952023, - "velocityX": -2.900459120799771, - "velocityY": 3.071847382995126, - "timestamp": 5.152607509695824 - }, - { - "x": 1.9011339963459661, - "y": 2.4339551936894854, - "heading": -0.6596386701119549, - "angularVelocity": -0.15289156470082665, - "velocityX": -2.7132396162351555, - "velocityY": 3.1813814282875046, - "timestamp": 5.208891623739172 - }, - { - "x": 1.7604826838741388, - "y": 2.6146352624702294, - "heading": -0.6772772697494215, - "angularVelocity": -0.3133850454478603, - "velocityX": -2.4989522330137985, - "velocityY": 3.2101432500401494, - "timestamp": 5.26517573778252 - }, - { - "x": 1.6317544686478245, - "y": 2.792388897989417, - "heading": -0.7009679816171792, - "angularVelocity": -0.4209129391201239, - "velocityX": -2.287114533368532, - "velocityY": 3.158149302701766, - "timestamp": 5.321459851825868 - }, - { - "x": 1.5139177763229132, - "y": 2.964350304348354, - "heading": -0.7282265000249424, - "angularVelocity": -0.48430216715802665, - "velocityX": -2.0936048177671953, - "velocityY": 3.0552387522080946, - "timestamp": 5.377743965869215 - }, - { - "x": 1.405939966332392, - "y": 3.1288586513458805, - "heading": -0.7573592985064115, - "angularVelocity": -0.5176025060824792, - "velocityX": -1.9184420297947733, - "velocityY": 2.922820227228369, - "timestamp": 5.434028079912563 - }, - { - "x": 1.3069897960630168, - "y": 3.2849152039001472, - "heading": -0.7872314504581, - "angularVelocity": -0.5307386011030019, - "velocityX": -1.7580479314850108, - "velocityY": 2.7726571734624392, - "timestamp": 5.490312193955911 - }, - { - "x": 1.2164244610459272, - "y": 3.431880015003849, - "heading": -0.81704954760011, - "angularVelocity": -0.5297782091594296, - "velocityX": -1.609074541838559, - "velocityY": 2.6111241795600484, - "timestamp": 5.546596307999259 - }, - { - "x": 1.133744514509131, - "y": 3.569317973113813, - "heading": -0.8462342157208796, - "angularVelocity": -0.5185240740982935, - "velocityX": -1.468974824283782, - "velocityY": 2.4418605577430528, - "timestamp": 5.602880422042607 - }, - { - "x": 1.0585554462199285, - "y": 3.696918241093708, - "heading": -0.8743466079980574, - "angularVelocity": -0.4994729464076927, - "velocityX": -1.3358843710553012, - "velocityY": 2.2670742917197226, - "timestamp": 5.659164536085955 - }, - { - "x": 0.990539814673681, - "y": 3.8144496515109747, - "heading": -0.9010441731548517, - "angularVelocity": -0.4743357092950377, - "velocityX": -1.2084339018619632, - "velocityY": 2.088180873323288, - "timestamp": 5.715448650129303 - }, - { - "x": 0.9294376679920158, - "y": 3.9217345696371626, - "heading": -0.9260527900785384, - "angularVelocity": -0.4443281616625532, - "velocityX": -1.0856019983650476, - "velocityY": 1.9061314182463815, - "timestamp": 5.771732764172651 - }, - { - "x": 0.8750327428643173, - "y": 4.018632780962349, - "heading": -0.949148487110952, - "angularVelocity": -0.4103413089993085, - "velocityX": -0.9666124456680172, - "velocityY": 1.7215907716084669, - "timestamp": 5.8280168782159985 - }, - { - "x": 0.8271425931537745, - "y": 4.105031112353838, - "heading": -0.9701450242376531, - "angularVelocity": -0.37304552951709063, - "velocityX": -0.8508644139562936, - "velocityY": 1.5350393776287605, - "timestamp": 5.884300992259346 - }, - { - "x": 0.7856114013922731, - "y": 4.180836489375294, - "heading": -0.9888852009339675, - "angularVelocity": -0.3329567679057974, - "velocityX": -0.7378847916041681, - "velocityY": 1.346834329897688, - "timestamp": 5.940585106302694 - }, - { - "x": 0.7503046474773418, - "y": 4.245971138892709, - "heading": -1.0052346109229755, - "angularVelocity": -0.2904800096243145, - "velocityX": -0.6272951882611009, - "velocityY": 1.157247486693155, - "timestamp": 5.996869220346042 - }, - { - "x": 0.7211050852240694, - "y": 4.300369179737215, - "heading": -1.0190770505330764, - "angularVelocity": -0.24593866040851983, - "velocityX": -0.5187886981890432, - "velocityY": 0.9664901325906992, - "timestamp": 6.05315333438939 - }, - { - "x": 0.6979096564056603, - "y": 4.343974139962992, - "heading": -1.0303110725490423, - "angularVelocity": -0.1995948982569619, - "velocityX": -0.4121132438994187, - "velocityY": 0.7747294412805966, - "timestamp": 6.109437448432738 - }, - { - "x": 0.6806270884657889, - "y": 4.376737109843248, - "heading": -1.038847350819041, - "angularVelocity": -0.15166407813444996, - "velocityX": -0.30705942935444774, - "velocityY": 0.5820997707279092, - "timestamp": 6.165721562476086 - }, - { - "x": 0.6691759989582504, - "y": 4.398615341726796, - "heading": -1.044606629501852, - "angularVelocity": -0.1023251192756673, - "velocityX": -0.20345153694200915, - "velocityY": 0.3887106025458067, - "timestamp": 6.222005676519434 + "heading": -0.696385035506177, + "angularVelocity": -2.8185479518318006e-7, + "velocityX": -3.286187967310359, + "velocityY": 1.8719390230438346, + "timestamp": 4.954811093534943 + }, + { + "x": 2.5741841985169076, + "y": 1.7387873379942203, + "heading": -0.6963850589279509, + "angularVelocity": -4.023107769663099e-7, + "velocityX": -3.153128257888759, + "velocityY": 2.0882929565548642, + "timestamp": 5.0130292054104135 + }, + { + "x": 2.3991894720533877, + "y": 1.872411165939459, + "heading": -0.6963850794155533, + "angularVelocity": -3.519111438487621e-7, + "velocityX": -3.0058468202788267, + "velocityY": 2.2952277846293376, + "timestamp": 5.071247317285884 + }, + { + "x": 2.2335584908852413, + "y": 2.017479635272907, + "heading": -0.6963850978369923, + "angularVelocity": -3.1642110134470593e-7, + "velocityX": -2.845007779064253, + "velocityY": 2.4918099309670456, + "timestamp": 5.129465429161355 + }, + { + "x": 2.0780383093475274, + "y": 2.173338406648706, + "heading": -0.696385114808838, + "angularVelocity": -2.9152174806319146e-7, + "velocityX": -2.671336746034877, + "velocityY": 2.6771526309404154, + "timestamp": 5.187683541036826 + }, + { + "x": 1.9333303562340614, + "y": 2.339284459115452, + "heading": -0.6963851307944144, + "angularVelocity": -2.7458081036886754e-7, + "velocityX": -2.485617421310377, + "velocityY": 2.8504196910698067, + "timestamp": 5.245901652912297 + }, + { + "x": 1.8000872249814166, + "y": 2.5145692341351995, + "heading": -0.6963851461633941, + "angularVelocity": -2.6398966215373687e-7, + "velocityX": -2.2886886393302137, + "velocityY": 3.0108289220145825, + "timestamp": 5.304119764787767 + }, + { + "x": 1.6789094155739503, + "y": 2.698401829383423, + "heading": -0.6963851612308563, + "angularVelocity": -2.588105610313673e-7, + "velocityX": -2.0814451981312536, + "velocityY": 3.157652993649885, + "timestamp": 5.362337876663238 + }, + { + "x": 1.5665736982883989, + "y": 2.8877670089616614, + "heading": -0.6963851762457391, + "angularVelocity": -2.5790741716981964e-7, + "velocityX": -1.929566481404267, + "velocityY": 3.2526850060560912, + "timestamp": 5.420555988538709 + }, + { + "x": 1.4540456075762054, + "y": 3.0767463777174253, + "heading": -0.6971250426595832, + "angularVelocity": -0.012708526436353156, + "velocityX": -1.9328708384238364, + "velocityY": 3.24605801644674, + "timestamp": 5.47877410041418 + }, + { + "x": 1.3486317039814464, + "y": 3.254480208772174, + "heading": -0.7253513181524345, + "angularVelocity": -0.48483666995638336, + "velocityX": -1.8106719747325544, + "velocityY": 3.0528958313681422, + "timestamp": 5.53699221228965 + }, + { + "x": 1.2507109072486164, + "y": 3.4195717896549707, + "heading": -0.7601787682651481, + "angularVelocity": -0.5982236281934066, + "velocityX": -1.6819644879978934, + "velocityY": 2.835742616248544, + "timestamp": 5.595210324165121 + }, + { + "x": 1.160328320301069, + "y": 3.5719513486090504, + "heading": -0.7969704460064527, + "angularVelocity": -0.631962744171482, + "velocityX": -1.5524822780387872, + "velocityY": 2.6173909466528555, + "timestamp": 5.653428436040592 + }, + { + "x": 1.0774888591394964, + "y": 3.71161238889488, + "heading": -0.8336344675120989, + "angularVelocity": -0.6297700204374729, + "velocityX": -1.422915626991949, + "velocityY": 2.3989276839579965, + "timestamp": 5.711646547916063 + }, + { + "x": 1.0021908033919844, + "y": 3.838558158350536, + "heading": -0.8689739346571358, + "angularVelocity": -0.6070184347549548, + "velocityX": -1.2933785264038764, + "velocityY": 2.1805202086799977, + "timestamp": 5.7698646597915335 + }, + { + "x": 0.9344313462846832, + "y": 3.9527935365952946, + "heading": -0.9022118432658016, + "angularVelocity": -0.5709204152783613, + "velocityX": -1.1638896371672067, + "velocityY": 1.962196549573947, + "timestamp": 5.828082771667004 + }, + { + "x": 0.8742077702690397, + "y": 4.054323289061519, + "heading": -0.9328020557936417, + "angularVelocity": -0.5254415085338586, + "velocityX": -1.034447426678191, + "velocityY": 1.7439547452758104, + "timestamp": 5.886300883542475 + }, + { + "x": 0.8215176865940929, + "y": 4.143151679881797, + "heading": -0.9603390876580659, + "angularVelocity": -0.4729976802292441, + "velocityX": -0.9050462472512312, + "velocityY": 1.5257861850670007, + "timestamp": 5.944518995417946 + }, + { + "x": 0.7763590444591775, + "y": 4.21928243186123, + "heading": -0.984509600370908, + "angularVelocity": -0.41517170403161197, + "velocityX": -0.7756802939866982, + "velocityY": 1.3076815706816212, + "timestamp": 6.002737107293417 + }, + { + "x": 0.7387300854642734, + "y": 4.282718776761074, + "heading": -1.0050639989905081, + "angularVelocity": -0.3530584891445192, + "velocityX": -0.6463445443815322, + "velocityY": 1.0896324675649907, + "timestamp": 6.060955219168887 + }, + { + "x": 0.7086292911290721, + "y": 4.333463522832413, + "heading": -1.0217986916878445, + "angularVelocity": -0.28744822115035373, + "velocityX": -0.5170348773863939, + "velocityY": 0.8716316011739073, + "timestamp": 6.119173331044358 + }, + { + "x": 0.6860553360496396, + "y": 4.371519118466469, + "heading": -1.0345444282293665, + "angularVelocity": -0.21893077825652074, + "velocityX": -0.3877479765698806, + "velocityY": 0.6536727902728675, + "timestamp": 6.177391442919829 + }, + { + "x": 0.6710070489868055, + "y": 4.396887706764773, + "heading": -1.0431583071392123, + "angularVelocity": -0.14795874741302203, + "velocityX": -0.25848119387696106, + "velocityY": 0.4357507909663603, + "timestamp": 6.2356095547953 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -0.05172811727299292, - "velocityX": -0.10114075318843706, - "velocityY": 0.19465224364467987, - "timestamp": 6.2782897905627815 + "angularVelocity": -0.07488723824993983, + "velocityX": -0.12923242394972312, + "velocityY": 0.2178611369128849, + "timestamp": 6.29382766667077 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": 3.206426664477851e-31, - "velocityX": -6.202690310891061e-31, - "velocityY": -1.8966043908205797e-31, - "timestamp": 6.334573904606129 - }, - { - "x": 0.6687462669994619, - "y": 4.399468321543485, - "heading": -1.0440928198775505, - "angularVelocity": 0.06332262739330675, - "velocityX": 0.09729413850238322, - "velocityY": -0.18676978112239445, - "timestamp": 6.388666430724142 - }, - { - "x": 0.6793295244169436, - "y": 4.379291243512293, - "heading": -1.0373158700882419, - "angularVelocity": 0.1252844020356336, - "velocityX": 0.19565101090554332, - "velocityY": -0.3730104596559098, - "timestamp": 6.442758956842155 - }, - { - "x": 0.69529661974622, - "y": 4.349072044732176, - "heading": -1.0272687237370097, - "angularVelocity": 0.1857400101686365, - "velocityX": 0.2951811733539195, - "velocityY": -0.5586575623080092, - "timestamp": 6.496851482960168 - }, - { - "x": 0.716717978867713, - "y": 4.308846987227399, - "heading": -1.0140420515401747, - "angularVelocity": 0.24451940306772088, - "velocityX": 0.3960132879476041, - "velocityY": -0.7436342946359814, - "timestamp": 6.550944009078181 - }, - { - "x": 0.7436721888450865, - "y": 4.258657330572967, - "heading": -0.9977373757966207, - "angularVelocity": 0.30142196923729686, - "velocityX": 0.49829822919416444, - "velocityY": -0.9278482677074609, - "timestamp": 6.605036535196194 - }, - { - "x": 0.7762474872180782, - "y": 4.1985504141499295, - "heading": -0.9784691435647803, - "angularVelocity": 0.35620877068584267, - "velocityX": 0.6022144039229431, - "velocityY": -1.1111870851067975, - "timestamp": 6.659129061314207 - }, - { - "x": 0.8145436288745613, - "y": 4.128581066891007, - "heading": -0.9563673669616457, - "angularVelocity": 0.4085920586303395, - "velocityX": 0.7079747315326336, - "velocityY": -1.2935122886733275, - "timestamp": 6.71322158743222 - }, - { - "x": 0.8586742551972165, - "y": 4.0488134751688944, - "heading": -0.9315810408217653, - "angularVelocity": 0.4582209025648986, - "velocityX": 0.8158359294628605, - "velocityY": -1.4746508888900018, - "timestamp": 6.767314113550233 - }, - { - "x": 0.9087699412940919, - "y": 3.9593237057932655, - "heading": -0.9042826435219333, - "angularVelocity": 0.5046611659473402, - "velocityX": 0.926111048824076, - "velocityY": -1.6543832539918422, - "timestamp": 6.821406639668246 - }, - { - "x": 0.9649821731331605, - "y": 3.8602031891787973, - "heading": -0.8746741747762036, - "angularVelocity": 0.5473670924712444, - "velocityX": 1.0391866653860837, - "velocityY": -1.8324253594360185, - "timestamp": 6.875499165786259 - }, - { - "x": 1.0274886208992116, - "y": 3.751563649709137, - "heading": -0.8429954192038807, - "angularVelocity": 0.5856401585518797, - "velocityX": 1.1555468426359812, - "velocityY": -2.008402033818445, - "timestamp": 6.929591691904272 - }, - { - "x": 1.0965002487203657, - "y": 3.6335442878114614, - "heading": -0.8095355084845481, - "angularVelocity": 0.6185680928678664, - "velocityX": 1.275807080453678, - "velocityY": -2.181805331855407, - "timestamp": 6.983684218022285 - }, - { - "x": 1.1722710636909373, - "y": 3.5063225947671968, - "heading": -0.7746495035161819, - "angularVelocity": 0.6449320723557629, - "velocityX": 1.4007631073674283, - "velocityY": -2.351927376562676, - "timestamp": 7.037776744140298 - }, - { - "x": 1.2551116907799804, - "y": 3.370131275203577, - "heading": -0.738782856357727, - "angularVelocity": 0.6630610498773233, - "velocityX": 1.531461609098938, - "velocityY": -2.5177474475235027, - "timestamp": 7.091869270258311 - }, - { - "x": 1.3454084529571924, - "y": 3.2252859282148103, - "heading": -0.7025087034544132, - "angularVelocity": 0.6705945443190738, - "velocityX": 1.669302002648637, - "velocityY": -2.6777330877992154, - "timestamp": 7.145961796376324 - }, - { - "x": 1.4436499618435756, - "y": 3.0722326748820543, - "heading": -0.6665870109674555, - "angularVelocity": 0.6640786641844795, - "velocityX": 1.8161752821823995, - "velocityY": -2.829471358000162, - "timestamp": 7.200054322494337 - }, - { - "x": 1.5504618658475082, - "y": 2.9116347206115476, - "heading": -0.6320630523320445, - "angularVelocity": 0.6382389788923385, - "velocityX": 1.9746148251778084, - "velocityY": -2.96894905444256, - "timestamp": 7.25414684861235 - }, - { - "x": 1.666640324199563, - "y": 2.7445376431595645, - "heading": -0.6004402859276634, - "angularVelocity": 0.5846050956330832, - "velocityX": 2.147772838315358, - "velocityY": -3.0890973197909535, - "timestamp": 7.308239374730363 - }, - { - "x": 1.7931265785030042, - "y": 2.572686348472304, - "heading": -0.5739736412013764, - "angularVelocity": 0.48928468728727015, - "velocityX": 2.338331436539341, - "velocityY": -3.1769877840815965, - "timestamp": 7.362331900848376 - }, - { - "x": 1.9307039385810119, - "y": 2.3990237133183436, - "heading": -0.5558132076436828, - "angularVelocity": 0.33572907129927604, - "velocityX": 2.5433709599366927, - "velocityY": -3.2104737496406606, - "timestamp": 7.416424426966389 - }, - { - "x": 2.079064797054882, - "y": 2.227910687938415, - "heading": -0.5489525363882142, - "angularVelocity": 0.12683214758631672, - "velocityX": 2.742723794233161, - "velocityY": -3.16333951581913, - "timestamp": 7.470516953084402 - }, - { - "x": 2.237064005369136, - "y": 2.0627082520134143, - "heading": -0.548898243902152, - "angularVelocity": 0.0010036966332050488, - "velocityX": 2.920906447773384, - "velocityY": -3.0540713806611905, - "timestamp": 7.5246094792024145 - }, - { - "x": 2.40313061190331, - "y": 1.9055936598291874, - "heading": -0.5488982087618529, - "angularVelocity": 6.496331729658335e-7, - "velocityX": 3.0700471664443327, - "velocityY": -2.904552688872191, - "timestamp": 7.5787020053204275 - }, - { - "x": 2.5768383133172232, - "y": 1.7569708294797155, - "heading": -0.5488981704735678, - "angularVelocity": 7.078294853357702e-7, - "velocityX": 3.211306882491981, - "velocityY": -2.7475668269761684, - "timestamp": 7.63279453143844 + "angularVelocity": -4.5881817726943837e-32, + "velocityX": 7.1951222626888875e-34, + "velocityY": 0, + "timestamp": 6.352045778546241 + }, + { + "x": 0.6704852067504524, + "y": 4.397985362145517, + "heading": -1.0417684453451177, + "angularVelocity": 0.10293302663784343, + "velocityX": 0.12534996227388928, + "velocityY": -0.20741457823419143, + "timestamp": 6.40790399622796 + }, + { + "x": 0.684490344241533, + "y": 4.374811471822251, + "heading": -1.0303744644017594, + "angularVelocity": 0.20398038849505432, + "velocityX": 0.25072653715666804, + "velocityY": -0.4148698487895252, + "timestamp": 6.463762213909678 + }, + { + "x": 0.7055004573054668, + "y": 4.340046908929331, + "heading": -1.0134578875635165, + "angularVelocity": 0.3028484892703545, + "velocityX": 0.3761328938859112, + "velocityY": -0.6223715029901059, + "timestamp": 6.519620431591397 + }, + { + "x": 0.7335174330880717, + "y": 4.293688705050122, + "heading": -0.9911609198549822, + "angularVelocity": 0.3991707690278106, + "velocityX": 0.5015730351843025, + "velocityY": -0.8299262991769599, + "timestamp": 6.575478649273116 + }, + { + "x": 0.768543442750825, + "y": 4.235733440885856, + "heading": -0.963651742055662, + "angularVelocity": 0.49248219762521406, + "velocityX": 0.6270520456333297, + "velocityY": -1.0375423092533531, + "timestamp": 6.631336866954834 + }, + { + "x": 0.8105810218384888, + "y": 4.16617715590748, + "heading": -0.9311321668224927, + "angularVelocity": 0.5821806814257259, + "velocityX": 0.7525764485933851, + "velocityY": -1.24522922257758, + "timestamp": 6.687195084636553 + }, + { + "x": 0.8596331801158593, + "y": 4.085015237354172, + "heading": -0.8938487217946981, + "angularVelocity": 0.667465711853477, + "velocityX": 0.8781547337738402, + "velocityY": -1.4529987157085942, + "timestamp": 6.7430533023182715 + }, + { + "x": 0.9157035549190764, + "y": 3.992242285598939, + "heading": -0.8521094876337724, + "angularVelocity": 0.7472353378469229, + "velocityX": 1.0037981362510981, + "velocityY": -1.6608648754934532, + "timestamp": 6.79891151999999 + }, + { + "x": 0.9787966256099634, + "y": 3.88785195918136, + "heading": -0.8063112110880574, + "angularVelocity": 0.8199022175514963, + "velocityX": 1.1295217303637053, + "velocityY": -1.868844563075711, + "timestamp": 6.854769737681709 + }, + { + "x": 1.0489179988671087, + "y": 3.7718368324625953, + "heading": -0.756986124262755, + "angularVelocity": 0.8830408285913816, + "velocityX": 1.2553456978648867, + "velocityY": -2.0769571879257267, + "timestamp": 6.910627955363427 + }, + { + "x": 1.1260747058581024, + "y": 3.644188427808813, + "heading": -0.7048901245652565, + "angularVelocity": 0.9326470098695786, + "velocityX": 1.381295540624565, + "velocityY": -2.2852215833510225, + "timestamp": 6.966486173045146 + }, + { + "x": 1.210275065586603, + "y": 3.504898185530833, + "heading": -0.6511892097412871, + "angularVelocity": 0.9613789528688276, + "velocityX": 1.5073943140877994, + "velocityY": -2.4936392183449136, + "timestamp": 7.022344390726865 + }, + { + "x": 1.3015253174439423, + "y": 3.3539635614800978, + "heading": -0.5979268038301901, + "angularVelocity": 0.95352856073188, + "velocityX": 1.6336047880597582, + "velocityY": -2.7021023998790135, + "timestamp": 7.078202608408583 + }, + { + "x": 1.3998011054306767, + "y": 3.1914330516367446, + "heading": -0.5495841694257507, + "angularVelocity": 0.865452504766571, + "velocityX": 1.7593792295112722, + "velocityY": -2.909697383641142, + "timestamp": 7.134060826090302 + }, + { + "x": 1.504860441802437, + "y": 3.018388823031747, + "heading": -0.5232691895419826, + "angularVelocity": 0.4711031066854229, + "velocityX": 1.8808214929160718, + "velocityY": -3.0979189058807544, + "timestamp": 7.18991904377202 + }, + { + "x": 1.6140923684634172, + "y": 2.8375675393930413, + "heading": -0.5232691688026271, + "angularVelocity": 3.712856650457212e-7, + "velocityX": 1.9555211604385054, + "velocityY": -3.237147391079152, + "timestamp": 7.245777261453739 + }, + { + "x": 1.7233256383679056, + "y": 2.6567470671976134, + "heading": -0.5232691480644303, + "angularVelocity": 3.7126492252401944e-7, + "velocityX": 1.9555452078135154, + "velocityY": -3.2371328642411785, + "timestamp": 7.301635479135458 + }, + { + "x": 1.84078732146222, + "y": 2.4811603268646896, + "heading": -0.5232691272677354, + "angularVelocity": 3.72312181556421e-7, + "velocityX": 2.102854118325333, + "velocityY": -3.143436142796787, + "timestamp": 7.357493696817176 + }, + { + "x": 1.9693130123787856, + "y": 2.313502648911294, + "heading": -0.5232691060970931, + "angularVelocity": 3.790067651022537e-7, + "velocityX": 2.3009271733106282, + "velocityY": -3.0014863508304788, + "timestamp": 7.413351914498895 + }, + { + "x": 2.1083695489461487, + "y": 2.154470483882732, + "heading": -0.5232690841533676, + "angularVelocity": 3.928468612451328e-7, + "velocityX": 2.4894553091491622, + "velocityY": -2.847068374697022, + "timestamp": 7.4692101321806135 + }, + { + "x": 2.2573796500994945, + "y": 2.0047242347352134, + "heading": -0.5232690609733034, + "angularVelocity": 4.1498037571099624e-7, + "velocityX": 2.6676486887284834, + "velocityY": -2.6808275552359513, + "timestamp": 7.525068349862332 + }, + { + "x": 2.4157246359676448, + "y": 1.8648857059384452, + "heading": -0.5232690359846437, + "angularVelocity": 4.47358702857604e-7, + "velocityX": 2.8347661712087695, + "velocityY": -2.5034549006481424, + "timestamp": 7.580926567544051 + }, + { + "x": 2.582747044375024, + "y": 1.7355355500002163, + "heading": -0.523269008439205, + "angularVelocity": 4.931313565110266e-7, + "velocityX": 2.9901134575950428, + "velocityY": -2.3156871326484083, + "timestamp": 7.636784785225769 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.54889811849075, - "angularVelocity": 9.609981531028035e-7, - "velocityX": 3.344548163283859, - "velocityY": -2.583720422924227, - "timestamp": 7.686887057556453 - }, - { - "x": 3.040083158490555, - "y": 1.4307079955900162, - "heading": -0.5488980816236001, - "angularVelocity": 4.6047885659418846e-7, - "velocityX": 3.5263614649855297, - "velocityY": -2.3294620822502163, - "timestamp": 7.766949691265819 - }, - { - "x": 3.3354250115679163, - "y": 1.2655819134364221, - "heading": -0.5488980521717318, - "angularVelocity": 3.678603468591226e-7, - "velocityX": 3.688885056533252, - "velocityY": -2.062461281917701, - "timestamp": 7.847012324975185 - }, - { - "x": 3.642163402363816, - "y": 1.1227358560558156, - "heading": -0.5488980275036914, - "angularVelocity": 3.08109279752253e-7, - "velocityX": 3.8312303328569812, - "velocityY": -1.7841788455210303, - "timestamp": 7.927074958684551 - }, - { - "x": 3.958620462965578, - "y": 1.0029511838587135, - "heading": -0.548898006032631, - "angularVelocity": 2.6817829186367307e-7, - "velocityX": 3.9526186679093493, - "velocityY": -1.496137044807694, - "timestamp": 8.007137592393917 - }, - { - "x": 4.283065164425698, - "y": 0.9068831020971607, - "heading": -0.5488979867253696, - "angularVelocity": 2.4115196404363913e-7, - "velocityX": 4.052386068609446, - "velocityY": -1.199911585604899, - "timestamp": 8.087200226103283 - }, - { - "x": 4.613722785487234, - "y": 0.8350570656894738, - "heading": -0.5488979688602151, - "angularVelocity": 2.2313973197090786e-7, - "velocityX": 4.129986808350196, - "velocityY": -0.8971230782737135, - "timestamp": 8.167262859812649 - }, - { - "x": 4.948784620508865, - "y": 0.7878658651941135, - "heading": -0.5488979518957929, - "angularVelocity": 2.118893824822783e-7, - "velocityX": 4.184996414655578, - "velocityY": -0.5894285300060232, - "timestamp": 8.247325493522014 - }, - { - "x": 5.286417874607001, - "y": 0.7655672355617388, - "heading": -0.5488979353932385, - "angularVelocity": 2.0612055273986908e-7, - "velocityX": 4.217114007562478, - "velocityY": -0.2785148152093341, - "timestamp": 8.32738812723138 - }, - { - "x": 5.624784390347261, - "y": 0.7668102763452317, - "heading": -0.5488979189757273, - "angularVelocity": 2.0505834761632137e-7, - "velocityX": 4.226272607625093, - "velocityY": 0.015525854397220307, - "timestamp": 8.407450760940746 - }, - { - "x": 5.963150845273002, - "y": 0.7680694375278687, - "heading": -0.5488979025584118, - "angularVelocity": 2.050559016505279e-7, - "velocityX": 4.226271848038887, - "velocityY": 0.01572720160901255, - "timestamp": 8.487513394650112 - }, - { - "x": 6.287499241199154, - "y": 0.7691154701328422, - "heading": -0.5071298401601563, - "angularVelocity": 0.5216923359002789, - "velocityX": 4.051183191195162, - "velocityY": 0.013065178543461153, - "timestamp": 8.567576028359477 - }, - { - "x": 6.587273814834465, - "y": 0.7701306302032382, - "heading": -0.44617628297839135, - "angularVelocity": 0.7613234084075665, - "velocityX": 3.7442507165521994, - "velocityY": 0.012679573760330002, - "timestamp": 8.647638662068843 - }, - { - "x": 6.86203616618243, - "y": 0.7710774941037043, - "heading": -0.38194080023757476, - "angularVelocity": 0.8023153844019356, - "velocityX": 3.431842529005624, - "velocityY": 0.011826539495826505, - "timestamp": 8.727701295778209 - }, - { - "x": 7.11177505601951, - "y": 0.7719477065280066, - "heading": -0.31898594689669185, - "angularVelocity": 0.7863200399997604, - "velocityX": 3.1192939610715853, - "velocityY": 0.010869145608779591, - "timestamp": 8.807763929487574 - }, - { - "x": 7.336503129467627, - "y": 0.7727363298825438, - "heading": -0.2594660169270826, - "angularVelocity": 0.7434170874971441, - "velocityX": 2.80690333350678, - "velocityY": 0.009850080085754406, - "timestamp": 8.88782656319694 - }, - { - "x": 7.536233129593461, - "y": 0.7734405408906285, - "heading": -0.20463677394358387, - "angularVelocity": 0.6848293697469102, - "velocityX": 2.494671869662653, - "velocityY": 0.008795751212569192, - "timestamp": 8.967889196906306 - }, - { - "x": 7.7109755860070575, - "y": 0.7740586905401403, - "heading": -0.15532295555151873, - "angularVelocity": 0.6159404969245411, - "velocityX": 2.1825719229765794, - "velocityY": 0.007720825813338177, - "timestamp": 9.047951830615672 - }, - { - "x": 7.860738998574353, - "y": 0.774589785610018, - "heading": -0.1121095095684322, - "angularVelocity": 0.5397454965062622, - "velocityX": 1.8705781414957077, - "velocityY": 0.006633494863411078, - "timestamp": 9.128014464325037 - }, - { - "x": 7.985530277012728, - "y": 0.7750332018658471, - "heading": -0.07543399366174408, - "angularVelocity": 0.4580853040608456, - "velocityX": 1.558670663912856, - "velocityY": 0.005538367087607409, - "timestamp": 9.208077098034403 - }, - { - "x": 8.085355113462763, - "y": 0.7753885191044545, - "heading": -0.04563658013807198, - "angularVelocity": 0.3721762842804108, - "velocityX": 1.2468342824247154, - "velocityY": 0.004437990881245155, - "timestamp": 9.288139731743769 - }, - { - "x": 8.160218261803882, - "y": 0.7756554219775593, - "heading": -0.022989482111851405, - "angularVelocity": 0.2828672625037418, - "velocityX": 0.9350572779417692, - "velocityY": 0.0033336759051113586, - "timestamp": 9.368202365453135 - }, - { - "x": 8.210123742972717, - "y": 0.7758336375934914, - "heading": -0.007715443356935036, - "angularVelocity": 0.19077612173431838, - "velocityX": 0.623330495846574, - "velocityY": 0.0022259524516894435, - "timestamp": 9.4482649991625 + "heading": -0.5232689753097743, + "angularVelocity": 5.930985989560848e-7, + "velocityX": 3.133045325838173, + "velocityY": -2.1183039826598433, + "timestamp": 7.692643002907488 + }, + { + "x": 3.027691141146945, + "y": 1.4695236072310565, + "heading": -0.5232689479285146, + "angularVelocity": 3.365468040910692e-7, + "velocityX": 3.3178420048715034, + "velocityY": -1.8152442667680868, + "timestamp": 7.7740024433384125 + }, + { + "x": 3.3102859882061892, + "y": 1.3477940719025636, + "heading": -0.523268924510295, + "angularVelocity": 2.8783653775450794e-7, + "velocityX": 3.473411881429686, + "velocityY": -1.4961943529078627, + "timestamp": 7.855361883769337 + }, + { + "x": 3.6030485594101127, + "y": 1.2530945172715204, + "heading": -0.5232689034957836, + "angularVelocity": 2.582922326531371e-7, + "velocityX": 3.5983847682984376, + "velocityY": -1.1639651665432065, + "timestamp": 7.936721324200262 + }, + { + "x": 3.9033999272897986, + "y": 1.1862590073495238, + "heading": -0.5232688838271395, + "angularVelocity": 2.4174999152857024e-7, + "velocityX": 3.6916597052396023, + "velocityY": -0.8214843854382324, + "timestamp": 8.018080764631186 + }, + { + "x": 4.2086942600952995, + "y": 1.1478754132776305, + "heading": -0.5232688646920325, + "angularVelocity": 2.3519221562308788e-7, + "velocityX": 3.752414362592638, + "velocityY": -0.471777999806691, + "timestamp": 8.09944020506211 + }, + { + "x": 4.515097551396115, + "y": 1.1196805012022473, + "heading": -0.5232688456111896, + "angularVelocity": 2.345252478429259e-7, + "velocityX": 3.7660447229963117, + "velocityY": -0.3465475171172218, + "timestamp": 8.180799645493035 + }, + { + "x": 4.821500870663513, + "y": 1.0914858930504614, + "heading": -0.5232688265303463, + "angularVelocity": 2.3452525108540152e-7, + "velocityX": 3.7660450667373904, + "velocityY": -0.3465437815507533, + "timestamp": 8.26215908592396 + }, + { + "x": 5.127904189931636, + "y": 1.0632912849065406, + "heading": -0.5232688074495031, + "angularVelocity": 2.3452525107290264e-7, + "velocityX": 3.7660450667462855, + "velocityY": -0.34654378145408427, + "timestamp": 8.343518526354885 + }, + { + "x": 5.4343075091997575, + "y": 1.0350966767626197, + "heading": -0.5232687883686599, + "angularVelocity": 2.3452525075472094e-7, + "velocityX": 3.7660450667462855, + "velocityY": -0.3465437814540822, + "timestamp": 8.42487796678581 + }, + { + "x": 5.740710828467879, + "y": 1.0069020686186991, + "heading": -0.5232687692878167, + "angularVelocity": 2.3452525040262446e-7, + "velocityX": 3.7660450667462855, + "velocityY": -0.3465437814540818, + "timestamp": 8.506237407216734 + }, + { + "x": 6.047114147736001, + "y": 0.9787074604747784, + "heading": -0.5232687502069736, + "angularVelocity": 2.345252500146494e-7, + "velocityX": 3.766045066746286, + "velocityY": -0.34654378145408327, + "timestamp": 8.587596847647658 + }, + { + "x": 6.353517467003837, + "y": 0.9505128523277552, + "heading": -0.5232687311261306, + "angularVelocity": 2.3452524994005725e-7, + "velocityX": 3.7660450667427767, + "velocityY": -0.3465437814922157, + "timestamp": 8.668956288078583 + }, + { + "x": 6.659920775239498, + "y": 0.9223181242925758, + "heading": -0.5232687120445318, + "angularVelocity": 2.3453453739742123e-7, + "velocityX": 3.7660449311448136, + "velocityY": -0.3465452550539253, + "timestamp": 8.750315728509507 + }, + { + "x": 6.946350295372631, + "y": 0.8955257235290481, + "heading": -0.4609901004295649, + "angularVelocity": 0.7654749256522044, + "velocityX": 3.5205443721840264, + "velocityY": -0.3293090589318255, + "timestamp": 8.831675168940432 + }, + { + "x": 7.204305307998309, + "y": 0.8715129005451068, + "heading": -0.38283013166037505, + "angularVelocity": 0.9606748566019065, + "velocityX": 3.170560309404857, + "velocityY": -0.2951448886171808, + "timestamp": 8.913034609371357 + }, + { + "x": 7.433510095635826, + "y": 0.8502195253793108, + "heading": -0.30543025852913497, + "angularVelocity": 0.9513324172497697, + "velocityX": 2.8171873653938824, + "velocityY": -0.2617197838752883, + "timestamp": 8.994394049802281 + }, + { + "x": 7.633994056446923, + "y": 0.8316157363547472, + "heading": -0.2335287634090791, + "angularVelocity": 0.8837511017679792, + "velocityX": 2.4641757582060952, + "velocityY": -0.22866171308489489, + "timestamp": 9.075753490233206 + }, + { + "x": 7.805788503499911, + "y": 0.8156861724274476, + "heading": -0.16939985967365537, + "angularVelocity": 0.7882171189447914, + "velocityX": 2.1115490242198023, + "velocityY": -0.19579244698498122, + "timestamp": 9.15711293066413 + }, + { + "x": 7.948917170139319, + "y": 0.802421883536533, + "heading": -0.1143881084380515, + "angularVelocity": 0.6761569517222741, + "velocityX": 1.759214000014262, + "velocityY": -0.16303318730634864, + "timestamp": 9.238472371095055 + }, + { + "x": 8.063397872753223, + "y": 0.7918170647898031, + "heading": -0.0693839808587397, + "angularVelocity": 0.5531518818338115, + "velocityX": 1.407097959469132, + "velocityY": -0.13034527635097967, + "timestamp": 9.31983181152598 + }, + { + "x": 8.149244274991434, + "y": 0.7838676118817843, + "heading": -0.0350206058237791, + "angularVelocity": 0.4223649382659615, + "velocityX": 1.0551498607109575, + "velocityY": -0.09770781197503392, + "timestamp": 9.401191251956904 + }, + { + "x": 8.20646710981356, + "y": 0.7785704017832406, + "heading": -0.011770656016753542, + "angularVelocity": 0.2857683101540642, + "velocityX": 0.703333682225969, + "velocityY": -0.06510873317818848, + "timestamp": 9.482550692387829 }, { "x": 8.235074996948242, "y": 0.7759228944778442, - "heading": -1.0986669650236063e-32, - "angularVelocity": 0.09636759371342492, - "velocityX": 0.3116466798494725, - "velocityY": 0.0011148382228791801, - "timestamp": 9.528327632871866 + "heading": -3.5413188929792264e-38, + "angularVelocity": 0.1446747415470122, + "velocityX": 0.35162345000356726, + "velocityY": -0.03254087406911382, + "timestamp": 9.563910132818753 }, { "x": 8.235074996948242, "y": 0.7759228944778442, - "heading": 1.3508572737206773e-32, - "angularVelocity": 3.0595153521375356e-31, - "velocityX": -1.0447785999071724e-32, - "velocityY": -1.7698938774257706e-30, - "timestamp": 9.608390266581232 + "heading": -7.544724392466369e-38, + "angularVelocity": -4.920640404901405e-37, + "velocityX": -6.041520920103385e-38, + "velocityY": 3.1783876166730987e-37, + "timestamp": 9.645269573249678 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLaneSprint.1.traj b/src/main/deploy/choreo/SourceLaneSprint.1.traj index a59928d0..a119bd81 100644 --- a/src/main/deploy/choreo/SourceLaneSprint.1.traj +++ b/src/main/deploy/choreo/SourceLaneSprint.1.traj @@ -5,243 +5,234 @@ "y": 1.5100655555725098, "heading": 0, "angularVelocity": 0, - "velocityX": 0, + "velocityX": 2.82118644197349e-37, "velocityY": 0, "timestamp": 0 }, { - "x": 1.530136363451793, + "x": 1.5360008147282442, "y": 1.5100655555725098, - "heading": 1.63546146728146e-19, - "angularVelocity": 1.69832040317376e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.2961741231159756e-22, - "timestamp": 0.096298758216477 + "heading": 3.2333963174943666e-19, + "angularVelocity": 3.292766805519721e-18, + "velocityX": 0.4284174372625368, + "velocityY": 1.8392808793575992e-26, + "timestamp": 0.09819694336189172 }, { - "x": 1.602546026150824, + "x": 1.6201393797142145, "y": 1.5100655555725098, - "heading": 5.000550672728244e-19, - "angularVelocity": 3.494426359967847e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.641598990275319e-22, - "timestamp": 0.192597516432954 + "heading": 7.027380416999147e-19, + "angularVelocity": 3.863647922469736e-18, + "velocityX": 0.8568348678215871, + "velocityY": 3.963342552313322e-26, + "timestamp": 0.19639388672378344 }, { - "x": 1.7111605195237438, + "x": 1.746347225986537, "y": 1.5100655555725098, - "heading": 1.021045803758983e-18, - "angularVelocity": 5.410150084422451e-18, - "velocityX": 1.1278909031075715, - "velocityY": 4.0472192553510183e-22, - "timestamp": 0.288896274649431 + "heading": 8.361393183209098e-19, + "angularVelocity": 1.3585074996311424e-18, + "velocityX": 1.2852522894444904, + "velocityY": 6.468173032965746e-26, + "timestamp": 0.29459083008567516 }, { - "x": 1.8559798429712064, + "x": 1.9146243523169562, "y": 1.5100655555725098, - "heading": 1.7409021964949573e-18, - "angularVelocity": 7.475240644111669e-18, - "velocityX": 1.5038545265756458, - "velocityY": 5.528083834903345e-22, - "timestamp": 0.385195032865908 + "heading": 3.80235401897157e-19, + "angularVelocity": -4.642750516315599e-18, + "velocityX": 1.7136696985593167, + "velocityY": 9.506575448004094e-26, + "timestamp": 0.3927877734475669 }, { - "x": 2.0370039957226234, + "x": 2.12497075686256, "y": 1.5100655555725098, - "heading": 2.678153616617339e-18, - "angularVelocity": 9.732746683078845e-18, - "velocityX": 1.8798181420416615, - "velocityY": 7.105691212840138e-22, - "timestamp": 0.481493791082385 + "heading": -1.0623970199278788e-18, + "angularVelocity": -1.4691215100259113e-17, + "velocityX": 2.1420870889066284, + "velocityY": 1.3343481989110454e-25, + "timestamp": 0.4909847168094586 }, { - "x": 2.254232976750543, + "x": 2.3773864365383615, "y": 1.5100655555725098, - "heading": 3.8575512078730386e-18, - "angularVelocity": 1.2247277267116202e-17, - "velocityX": 2.25578174683826, - "velocityY": 8.812289177822076e-22, - "timestamp": 0.577792549298862 + "heading": -3.9564889545725385e-18, + "angularVelocity": -2.947232200024047e-17, + "velocityX": 2.5705044478376236, + "velocityY": 1.8500903675379378e-25, + "timestamp": 0.5891816601713503 }, { - "x": 2.5076667846165317, + "x": 2.6718713848686027, "y": 1.5100655555725098, - "heading": 5.3137068489139784e-18, - "angularVelocity": 1.512122972072063e-17, - "velocityX": 2.6317453366976604, - "velocityY": 1.0699474323795816e-21, - "timestamp": 0.6740913075153391 + "heading": -8.853646546888228e-18, + "angularVelocity": -4.987077429157769e-17, + "velocityX": 2.9989217408219764, + "velocityY": 2.625793984498987e-25, + "timestamp": 0.6873786035332421 }, { - "x": 2.7973054171629355, + "x": 3.0084255847706767, "y": 1.5100655555725098, - "heading": 7.098464960583512e-18, - "angularVelocity": 1.8533552797092878e-17, - "velocityX": 3.007708904151223, - "velocityY": 1.2857541020722067e-21, - "timestamp": 0.7703900657318161 + "heading": -1.6413613329692498e-17, + "angularVelocity": -7.698780165331962e-17, + "velocityX": 3.4273388598436147, + "velocityY": 4.161152605292971e-25, + "timestamp": 0.7855755468951338 }, { - "x": 3.1231488707936497, + "x": 3.3798020724552167, "y": 1.5100655555725098, - "heading": 9.299048469122608e-18, - "angularVelocity": 2.285162928757611e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.5467079012007584e-21, - "timestamp": 0.8666888239482932 + "heading": -2.58741769005412e-17, + "angularVelocity": -9.634275005875749e-17, + "velocityX": 3.781955679779987, + "velocityY": -5.487818039619291e-25, + "timestamp": 0.8837724902570254 }, { - "x": 3.4851971383163978, + "x": 3.7511785589388484, "y": 1.5100655555725098, - "heading": 1.2088326363522958e-17, - "angularVelocity": 2.896483790183752e-17, - "velocityX": 3.7596358896848248, - "velocityY": 1.8979567079276025e-21, - "timestamp": 0.9629875821647702 + "heading": 5.146033898598045e-18, + "angularVelocity": 3.158979265252523e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.295977316525143e-24, + "timestamp": 0.9819694336189171 }, { - "x": 3.883450198153611, + "x": 4.122555045422488, "y": 1.5100655555725098, - "heading": 1.5983950666179298e-17, - "angularVelocity": 4.0453526414309363e-17, - "velocityX": 4.1355991210390295, - "velocityY": 2.520086451941594e-21, - "timestamp": 1.0592863403812471 + "heading": 4.0038826563950476e-17, + "angularVelocity": 3.553348146159671e-16, + "velocityX": 3.7819556675504797, + "velocityY": 7.623936784532972e-25, + "timestamp": 1.0801663769808088 }, { - "x": 4.290437753988616, + "x": 4.493931531906129, "y": 1.5100655555725098, - "heading": -1.0811293578876384e-19, - "angularVelocity": -1.671056190093009e-16, - "velocityX": 4.226301183657094, - "velocityY": -2.0251172410052617e-20, - "timestamp": 1.155585098597724 + "heading": 6.570668459860429e-17, + "angularVelocity": 2.613916192895976e-16, + "velocityX": 3.7819556675504797, + "velocityY": 3.4278643163770276e-26, + "timestamp": 1.1783633203427004 }, { - "x": 4.697425309823639, + "x": 4.865308018389769, "y": 1.5100655555725098, - "heading": -8.423356676535005e-18, - "angularVelocity": -8.634840048564003e-17, - "velocityX": 4.226301183657286, - "velocityY": 2.1724036271938957e-21, - "timestamp": 1.251883856814201 + "heading": 6.663782988380241e-17, + "angularVelocity": 9.482426370100795e-18, + "velocityX": 3.7819556675504797, + "velocityY": 2.693381196266241e-25, + "timestamp": 1.276560263704592 }, { - "x": 5.104412865658644, + "x": 5.236684504873409, "y": 1.5100655555725098, - "heading": -8.042445280935507e-18, - "angularVelocity": 3.955517211854611e-18, - "velocityX": 4.226301183657094, - "velocityY": -6.018603432474332e-22, - "timestamp": 1.348182615030678 + "heading": 5.2326006182614025e-17, + "angularVelocity": -1.4574612214194992e-16, + "velocityX": 3.7819556675504797, + "velocityY": 5.124149697811417e-24, + "timestamp": 1.3747572070664837 }, { - "x": 5.502665925495857, + "x": 5.60806099135704, "y": 1.5100655555725098, - "heading": -6.3335804348522236e-18, - "angularVelocity": 1.7745450464376603e-17, - "velocityX": 4.13559912103903, - "velocityY": 1.2361742339090949e-21, - "timestamp": 1.4444813732471549 + "heading": 2.884064553605338e-17, + "angularVelocity": -2.3916590315859936e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.1014556301472372e-23, + "timestamp": 1.4729541504283754 }, { - "x": 5.864714193018606, + "x": 5.9794374790415805, "y": 1.5100655555725098, - "heading": -5.002513564509544e-18, - "angularVelocity": 1.3822263999404139e-17, - "velocityX": 3.759635889684825, - "velocityY": 1.1237973471200677e-21, - "timestamp": 1.5407801314636318 + "heading": 1.6502190572837753e-17, + "angularVelocity": -1.2565009195595738e-16, + "velocityX": 3.7819556797799856, + "velocityY": 6.524035237986936e-24, + "timestamp": 1.571151093790267 }, { - "x": 6.19055764664932, + "x": 6.315991678943653, "y": 1.5100655555725098, - "heading": -3.8974854517760974e-18, - "angularVelocity": 1.1474998421478588e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.0114186808354653e-21, - "timestamp": 1.6370788896801087 + "heading": 9.967918627860597e-18, + "angularVelocity": -6.65425189408073e-17, + "velocityX": 3.427338859843601, + "velocityY": -3.262675265981889e-25, + "timestamp": 1.6693480371521587 }, { - "x": 6.480196279195724, + "x": 6.610476627273894, "y": 1.5100655555725098, - "heading": -2.9674142808273095e-18, - "angularVelocity": 9.658184315185454e-18, - "velocityX": 3.0077089041512233, - "velocityY": 8.990394180618527e-22, - "timestamp": 1.7333776478965857 + "heading": 5.2731830670839714e-18, + "angularVelocity": -4.78093860085579e-17, + "velocityX": 2.9989217408219764, + "velocityY": -2.379324733264266e-25, + "timestamp": 1.7675449805140504 }, { - "x": 6.733630087061712, + "x": 6.862892306949696, "y": 1.5100655555725098, - "heading": -2.1863127731116462e-18, - "angularVelocity": 8.111231213379657e-18, - "velocityX": 2.631745336697661, - "velocityY": 7.866598563444379e-22, - "timestamp": 1.8296764061130626 + "heading": 2.2289001785398693e-18, + "angularVelocity": -3.10018090849907e-17, + "velocityX": 2.5705044478376236, + "velocityY": -1.7314309405461854e-25, + "timestamp": 1.865741923875942 }, { - "x": 6.950859068089632, + "x": 7.0732387114953, "y": 1.5100655555725098, - "heading": -1.5381923338445708e-18, - "angularVelocity": 6.730309280881326e-18, - "velocityX": 2.2557817468382604, - "velocityY": 6.742801150833687e-22, - "timestamp": 1.9259751643295395 + "heading": 5.002812433628901e-19, + "angularVelocity": -1.760359206249271e-17, + "velocityX": 2.1420870889066284, + "velocityY": -1.2661949502166272e-25, + "timestamp": 1.9639388672378337 }, { - "x": 7.13188322084105, + "x": 7.24151583782572, "y": 1.5100655555725098, - "heading": -1.0119189398601327e-18, - "angularVelocity": 5.4650069455448464e-18, - "velocityX": 1.8798181420416618, - "velocityY": 5.619002540306745e-22, - "timestamp": 2.0222739225460167 + "heading": -2.663585350994398e-19, + "angularVelocity": -7.807165345874594e-18, + "velocityX": 1.7136696985593165, + "velocityY": -9.096401200333518e-26, + "timestamp": 2.0621358105997256 }, { - "x": 7.276702544288512, + "x": 7.367723684098041, "y": 1.5100655555725098, - "heading": -6.000129613332895e-19, - "angularVelocity": 4.277375706520152e-18, - "velocityX": 1.5038545265756458, - "velocityY": 4.495203072508259e-22, - "timestamp": 2.118572680762494 + "heading": -4.029353027704155e-19, + "angularVelocity": -1.390845344762092e-18, + "velocityX": 1.2852522894444902, + "velocityY": -6.226126404979919e-26, + "timestamp": 2.1603327539616175 }, { - "x": 7.385317037661432, + "x": 7.451862249084011, "y": 1.5100655555725098, - "heading": -2.9682943520502703e-19, - "angularVelocity": 3.1483637414904354e-18, - "velocityX": 1.1278909031075715, - "velocityY": 3.3714029617848224e-22, - "timestamp": 2.214871438978971 - }, - { - "x": 7.457726700360463, - "y": 1.5100655555725098, - "heading": -9.799024750037137e-20, - "angularVelocity": 2.064815645768494e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.2476023506664887e-22, - "timestamp": 2.311170197195448 + "heading": -2.1995708883691042e-19, + "angularVelocity": 1.8633799906775463e-18, + "velocityX": 0.8568348678215871, + "velocityY": -3.833262935804246e-26, + "timestamp": 2.2585296973235094 }, { "x": 7.493931531906128, "y": 1.5100655555725098, "heading": 0, - "angularVelocity": 1.017564989623581e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.1238013392767656e-22, - "timestamp": 2.4074689554119253 + "angularVelocity": 2.2399586452459634e-18, + "velocityX": 0.42841743726253684, + "velocityY": -1.7862050890834192e-26, + "timestamp": 2.3567266406854013 }, { "x": 7.493931531906128, "y": 1.5100655555725098, "heading": 0, "angularVelocity": 0, - "velocityX": -2.537301157014256e-39, + "velocityX": -2.1810930337522913e-39, "velocityY": 0, - "timestamp": 2.5037677136284024 + "timestamp": 2.454923584047293 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLaneSprint.traj b/src/main/deploy/choreo/SourceLaneSprint.traj index a59928d0..a119bd81 100644 --- a/src/main/deploy/choreo/SourceLaneSprint.traj +++ b/src/main/deploy/choreo/SourceLaneSprint.traj @@ -5,243 +5,234 @@ "y": 1.5100655555725098, "heading": 0, "angularVelocity": 0, - "velocityX": 0, + "velocityX": 2.82118644197349e-37, "velocityY": 0, "timestamp": 0 }, { - "x": 1.530136363451793, + "x": 1.5360008147282442, "y": 1.5100655555725098, - "heading": 1.63546146728146e-19, - "angularVelocity": 1.69832040317376e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.2961741231159756e-22, - "timestamp": 0.096298758216477 + "heading": 3.2333963174943666e-19, + "angularVelocity": 3.292766805519721e-18, + "velocityX": 0.4284174372625368, + "velocityY": 1.8392808793575992e-26, + "timestamp": 0.09819694336189172 }, { - "x": 1.602546026150824, + "x": 1.6201393797142145, "y": 1.5100655555725098, - "heading": 5.000550672728244e-19, - "angularVelocity": 3.494426359967847e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.641598990275319e-22, - "timestamp": 0.192597516432954 + "heading": 7.027380416999147e-19, + "angularVelocity": 3.863647922469736e-18, + "velocityX": 0.8568348678215871, + "velocityY": 3.963342552313322e-26, + "timestamp": 0.19639388672378344 }, { - "x": 1.7111605195237438, + "x": 1.746347225986537, "y": 1.5100655555725098, - "heading": 1.021045803758983e-18, - "angularVelocity": 5.410150084422451e-18, - "velocityX": 1.1278909031075715, - "velocityY": 4.0472192553510183e-22, - "timestamp": 0.288896274649431 + "heading": 8.361393183209098e-19, + "angularVelocity": 1.3585074996311424e-18, + "velocityX": 1.2852522894444904, + "velocityY": 6.468173032965746e-26, + "timestamp": 0.29459083008567516 }, { - "x": 1.8559798429712064, + "x": 1.9146243523169562, "y": 1.5100655555725098, - "heading": 1.7409021964949573e-18, - "angularVelocity": 7.475240644111669e-18, - "velocityX": 1.5038545265756458, - "velocityY": 5.528083834903345e-22, - "timestamp": 0.385195032865908 + "heading": 3.80235401897157e-19, + "angularVelocity": -4.642750516315599e-18, + "velocityX": 1.7136696985593167, + "velocityY": 9.506575448004094e-26, + "timestamp": 0.3927877734475669 }, { - "x": 2.0370039957226234, + "x": 2.12497075686256, "y": 1.5100655555725098, - "heading": 2.678153616617339e-18, - "angularVelocity": 9.732746683078845e-18, - "velocityX": 1.8798181420416615, - "velocityY": 7.105691212840138e-22, - "timestamp": 0.481493791082385 + "heading": -1.0623970199278788e-18, + "angularVelocity": -1.4691215100259113e-17, + "velocityX": 2.1420870889066284, + "velocityY": 1.3343481989110454e-25, + "timestamp": 0.4909847168094586 }, { - "x": 2.254232976750543, + "x": 2.3773864365383615, "y": 1.5100655555725098, - "heading": 3.8575512078730386e-18, - "angularVelocity": 1.2247277267116202e-17, - "velocityX": 2.25578174683826, - "velocityY": 8.812289177822076e-22, - "timestamp": 0.577792549298862 + "heading": -3.9564889545725385e-18, + "angularVelocity": -2.947232200024047e-17, + "velocityX": 2.5705044478376236, + "velocityY": 1.8500903675379378e-25, + "timestamp": 0.5891816601713503 }, { - "x": 2.5076667846165317, + "x": 2.6718713848686027, "y": 1.5100655555725098, - "heading": 5.3137068489139784e-18, - "angularVelocity": 1.512122972072063e-17, - "velocityX": 2.6317453366976604, - "velocityY": 1.0699474323795816e-21, - "timestamp": 0.6740913075153391 + "heading": -8.853646546888228e-18, + "angularVelocity": -4.987077429157769e-17, + "velocityX": 2.9989217408219764, + "velocityY": 2.625793984498987e-25, + "timestamp": 0.6873786035332421 }, { - "x": 2.7973054171629355, + "x": 3.0084255847706767, "y": 1.5100655555725098, - "heading": 7.098464960583512e-18, - "angularVelocity": 1.8533552797092878e-17, - "velocityX": 3.007708904151223, - "velocityY": 1.2857541020722067e-21, - "timestamp": 0.7703900657318161 + "heading": -1.6413613329692498e-17, + "angularVelocity": -7.698780165331962e-17, + "velocityX": 3.4273388598436147, + "velocityY": 4.161152605292971e-25, + "timestamp": 0.7855755468951338 }, { - "x": 3.1231488707936497, + "x": 3.3798020724552167, "y": 1.5100655555725098, - "heading": 9.299048469122608e-18, - "angularVelocity": 2.285162928757611e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.5467079012007584e-21, - "timestamp": 0.8666888239482932 + "heading": -2.58741769005412e-17, + "angularVelocity": -9.634275005875749e-17, + "velocityX": 3.781955679779987, + "velocityY": -5.487818039619291e-25, + "timestamp": 0.8837724902570254 }, { - "x": 3.4851971383163978, + "x": 3.7511785589388484, "y": 1.5100655555725098, - "heading": 1.2088326363522958e-17, - "angularVelocity": 2.896483790183752e-17, - "velocityX": 3.7596358896848248, - "velocityY": 1.8979567079276025e-21, - "timestamp": 0.9629875821647702 + "heading": 5.146033898598045e-18, + "angularVelocity": 3.158979265252523e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.295977316525143e-24, + "timestamp": 0.9819694336189171 }, { - "x": 3.883450198153611, + "x": 4.122555045422488, "y": 1.5100655555725098, - "heading": 1.5983950666179298e-17, - "angularVelocity": 4.0453526414309363e-17, - "velocityX": 4.1355991210390295, - "velocityY": 2.520086451941594e-21, - "timestamp": 1.0592863403812471 + "heading": 4.0038826563950476e-17, + "angularVelocity": 3.553348146159671e-16, + "velocityX": 3.7819556675504797, + "velocityY": 7.623936784532972e-25, + "timestamp": 1.0801663769808088 }, { - "x": 4.290437753988616, + "x": 4.493931531906129, "y": 1.5100655555725098, - "heading": -1.0811293578876384e-19, - "angularVelocity": -1.671056190093009e-16, - "velocityX": 4.226301183657094, - "velocityY": -2.0251172410052617e-20, - "timestamp": 1.155585098597724 + "heading": 6.570668459860429e-17, + "angularVelocity": 2.613916192895976e-16, + "velocityX": 3.7819556675504797, + "velocityY": 3.4278643163770276e-26, + "timestamp": 1.1783633203427004 }, { - "x": 4.697425309823639, + "x": 4.865308018389769, "y": 1.5100655555725098, - "heading": -8.423356676535005e-18, - "angularVelocity": -8.634840048564003e-17, - "velocityX": 4.226301183657286, - "velocityY": 2.1724036271938957e-21, - "timestamp": 1.251883856814201 + "heading": 6.663782988380241e-17, + "angularVelocity": 9.482426370100795e-18, + "velocityX": 3.7819556675504797, + "velocityY": 2.693381196266241e-25, + "timestamp": 1.276560263704592 }, { - "x": 5.104412865658644, + "x": 5.236684504873409, "y": 1.5100655555725098, - "heading": -8.042445280935507e-18, - "angularVelocity": 3.955517211854611e-18, - "velocityX": 4.226301183657094, - "velocityY": -6.018603432474332e-22, - "timestamp": 1.348182615030678 + "heading": 5.2326006182614025e-17, + "angularVelocity": -1.4574612214194992e-16, + "velocityX": 3.7819556675504797, + "velocityY": 5.124149697811417e-24, + "timestamp": 1.3747572070664837 }, { - "x": 5.502665925495857, + "x": 5.60806099135704, "y": 1.5100655555725098, - "heading": -6.3335804348522236e-18, - "angularVelocity": 1.7745450464376603e-17, - "velocityX": 4.13559912103903, - "velocityY": 1.2361742339090949e-21, - "timestamp": 1.4444813732471549 + "heading": 2.884064553605338e-17, + "angularVelocity": -2.3916590315859936e-16, + "velocityX": 3.7819556675503945, + "velocityY": -1.1014556301472372e-23, + "timestamp": 1.4729541504283754 }, { - "x": 5.864714193018606, + "x": 5.9794374790415805, "y": 1.5100655555725098, - "heading": -5.002513564509544e-18, - "angularVelocity": 1.3822263999404139e-17, - "velocityX": 3.759635889684825, - "velocityY": 1.1237973471200677e-21, - "timestamp": 1.5407801314636318 + "heading": 1.6502190572837753e-17, + "angularVelocity": -1.2565009195595738e-16, + "velocityX": 3.7819556797799856, + "velocityY": 6.524035237986936e-24, + "timestamp": 1.571151093790267 }, { - "x": 6.19055764664932, + "x": 6.315991678943653, "y": 1.5100655555725098, - "heading": -3.8974854517760974e-18, - "angularVelocity": 1.1474998421478588e-17, - "velocityX": 3.383672434261581, - "velocityY": 1.0114186808354653e-21, - "timestamp": 1.6370788896801087 + "heading": 9.967918627860597e-18, + "angularVelocity": -6.65425189408073e-17, + "velocityX": 3.427338859843601, + "velocityY": -3.262675265981889e-25, + "timestamp": 1.6693480371521587 }, { - "x": 6.480196279195724, + "x": 6.610476627273894, "y": 1.5100655555725098, - "heading": -2.9674142808273095e-18, - "angularVelocity": 9.658184315185454e-18, - "velocityX": 3.0077089041512233, - "velocityY": 8.990394180618527e-22, - "timestamp": 1.7333776478965857 + "heading": 5.2731830670839714e-18, + "angularVelocity": -4.78093860085579e-17, + "velocityX": 2.9989217408219764, + "velocityY": -2.379324733264266e-25, + "timestamp": 1.7675449805140504 }, { - "x": 6.733630087061712, + "x": 6.862892306949696, "y": 1.5100655555725098, - "heading": -2.1863127731116462e-18, - "angularVelocity": 8.111231213379657e-18, - "velocityX": 2.631745336697661, - "velocityY": 7.866598563444379e-22, - "timestamp": 1.8296764061130626 + "heading": 2.2289001785398693e-18, + "angularVelocity": -3.10018090849907e-17, + "velocityX": 2.5705044478376236, + "velocityY": -1.7314309405461854e-25, + "timestamp": 1.865741923875942 }, { - "x": 6.950859068089632, + "x": 7.0732387114953, "y": 1.5100655555725098, - "heading": -1.5381923338445708e-18, - "angularVelocity": 6.730309280881326e-18, - "velocityX": 2.2557817468382604, - "velocityY": 6.742801150833687e-22, - "timestamp": 1.9259751643295395 + "heading": 5.002812433628901e-19, + "angularVelocity": -1.760359206249271e-17, + "velocityX": 2.1420870889066284, + "velocityY": -1.2661949502166272e-25, + "timestamp": 1.9639388672378337 }, { - "x": 7.13188322084105, + "x": 7.24151583782572, "y": 1.5100655555725098, - "heading": -1.0119189398601327e-18, - "angularVelocity": 5.4650069455448464e-18, - "velocityX": 1.8798181420416618, - "velocityY": 5.619002540306745e-22, - "timestamp": 2.0222739225460167 + "heading": -2.663585350994398e-19, + "angularVelocity": -7.807165345874594e-18, + "velocityX": 1.7136696985593165, + "velocityY": -9.096401200333518e-26, + "timestamp": 2.0621358105997256 }, { - "x": 7.276702544288512, + "x": 7.367723684098041, "y": 1.5100655555725098, - "heading": -6.000129613332895e-19, - "angularVelocity": 4.277375706520152e-18, - "velocityX": 1.5038545265756458, - "velocityY": 4.495203072508259e-22, - "timestamp": 2.118572680762494 + "heading": -4.029353027704155e-19, + "angularVelocity": -1.390845344762092e-18, + "velocityX": 1.2852522894444902, + "velocityY": -6.226126404979919e-26, + "timestamp": 2.1603327539616175 }, { - "x": 7.385317037661432, + "x": 7.451862249084011, "y": 1.5100655555725098, - "heading": -2.9682943520502703e-19, - "angularVelocity": 3.1483637414904354e-18, - "velocityX": 1.1278909031075715, - "velocityY": 3.3714029617848224e-22, - "timestamp": 2.214871438978971 - }, - { - "x": 7.457726700360463, - "y": 1.5100655555725098, - "heading": -9.799024750037137e-20, - "angularVelocity": 2.064815645768494e-18, - "velocityX": 0.751927273415676, - "velocityY": 2.2476023506664887e-22, - "timestamp": 2.311170197195448 + "heading": -2.1995708883691042e-19, + "angularVelocity": 1.8633799906775463e-18, + "velocityX": 0.8568348678215871, + "velocityY": -3.833262935804246e-26, + "timestamp": 2.2585296973235094 }, { "x": 7.493931531906128, "y": 1.5100655555725098, "heading": 0, - "angularVelocity": 1.017564989623581e-18, - "velocityX": 0.3759636387447243, - "velocityY": 1.1238013392767656e-22, - "timestamp": 2.4074689554119253 + "angularVelocity": 2.2399586452459634e-18, + "velocityX": 0.42841743726253684, + "velocityY": -1.7862050890834192e-26, + "timestamp": 2.3567266406854013 }, { "x": 7.493931531906128, "y": 1.5100655555725098, "heading": 0, "angularVelocity": 0, - "velocityX": -2.537301157014256e-39, + "velocityX": -2.1810930337522913e-39, "velocityY": 0, - "timestamp": 2.5037677136284024 + "timestamp": 2.454923584047293 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLaneTaxi.1.traj b/src/main/deploy/choreo/SourceLaneTaxi.1.traj index 960b6bbb..2a7cdf97 100644 --- a/src/main/deploy/choreo/SourceLaneTaxi.1.traj +++ b/src/main/deploy/choreo/SourceLaneTaxi.1.traj @@ -6,143 +6,134 @@ "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": 9.261974312050868e-43, + "velocityY": -6.616666962933087e-41, "timestamp": 0 }, { - "x": 1.5046861371964564, + "x": 1.5097881789229493, "y": 1.9215065240859983, - "heading": -2.0425159024477705e-24, - "angularVelocity": -2.1355387753237974e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.2842385813221e-33, - "timestamp": 0.09564415279842821 + "heading": 2.3748244992157107e-19, + "angularVelocity": 2.455567735557075e-18, + "velocityX": 0.42198942031848213, + "velocityY": 1.4827190365964012e-17, + "timestamp": 0.09672358706123588 }, { - "x": 1.5761147139482576, - "y": 1.9215065240859983, - "heading": -7.39456542057925e-24, - "angularVelocity": -5.595811575686501e-23, - "velocityX": 0.7468159282286507, - "velocityY": -1.2474076386660863e-32, - "timestamp": 0.19128830559685642 + "x": 1.5914208389649156, + "y": 1.9215065240859985, + "heading": 6.823321419293863e-18, + "angularVelocity": 6.808983664986858e-17, + "velocityX": 0.843978832074171, + "velocityY": 2.965467782674968e-17, + "timestamp": 0.19344717412247175 }, { - "x": 1.6832575778116918, - "y": 1.9215065240859983, - "heading": -1.797881654717191e-23, - "angularVelocity": -1.1066309979696055e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.866390782935744e-32, - "timestamp": 0.28693245839528464 + "x": 1.7138698274174293, + "y": 1.9215065240859985, + "heading": 1.3350406511427553e-17, + "angularVelocity": 6.748261205621051e-17, + "velocityX": 1.2659682314613812, + "velocityY": 4.448259121719168e-17, + "timestamp": 0.29017076118370766 }, { - "x": 1.8261147273894114, - "y": 1.9215065240859983, - "heading": -3.609466520226707e-23, - "angularVelocity": -1.8940924580147868e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4777348604111533e-32, - "timestamp": 0.38257661119371283 + "x": 1.8771351424005533, + "y": 1.9215065240859985, + "heading": 2.3094752603641018e-17, + "angularVelocity": 1.0074517896422799e-16, + "velocityX": 1.6879576114124106, + "velocityY": 5.931116840042e-17, + "timestamp": 0.3868943482449435 }, { - "x": 2.004686160352503, - "y": 1.9215065240859983, - "heading": -6.568172939485326e-23, - "angularVelocity": -3.093458575185046e-22, - "velocityX": 1.867039727346783, - "velocityY": -3.0967151425129867e-32, - "timestamp": 0.47822076399214103 - }, - { - "x": 2.21897187204314, - "y": 1.9215065240859983, - "heading": -1.1554278307693229e-22, - "angularVelocity": -5.213187002510948e-22, - "velocityX": 2.2404475905834893, - "velocityY": -3.7086764301179e-32, - "timestamp": 0.5738649167905693 + "x": 2.081216780530401, + "y": 1.9215065240859985, + "heading": 2.7994689255823954e-17, + "angularVelocity": 5.0660149601833395e-17, + "velocityX": 2.1099469563783173, + "velocityY": 7.414094299456716e-17, + "timestamp": 0.48361793530617936 }, { - "x": 2.468971848487854, - "y": 1.9215065240859983, - "heading": -1.644337522360258e-22, - "angularVelocity": -5.111750826050956e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.332586658245059e-32, - "timestamp": 0.6695090695889975 + "x": 2.326114733911239, + "y": 1.9215065240859985, + "heading": 3.194288143588596e-17, + "angularVelocity": 4.082043898429076e-17, + "velocityX": 2.531936219712287, + "velocityY": 8.89740008515513e-17, + "timestamp": 0.5803415223674152 }, { - "x": 2.7189718249325674, - "y": 1.9215065240859983, - "heading": -1.1817502956999641e-22, - "angularVelocity": 4.836559085941933e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.2931003905648025e-32, - "timestamp": 0.7651532223874257 + "x": 2.6118289630644647, + "y": 1.9215065240859985, + "heading": 2.800063510694937e-17, + "angularVelocity": -4.0776590509092634e-17, + "velocityX": 2.953925074887259, + "velocityY": 1.036106690795216e-16, + "timestamp": 0.6770651094286511 }, { - "x": 2.933257536623205, - "y": 1.9215065240859983, - "heading": -8.188453918715617e-23, - "angularVelocity": 3.79432828279643e-22, - "velocityX": 2.2404475905834897, - "velocityY": -3.6782254895688684e-32, - "timestamp": 0.8607973751858539 + "x": 2.8567269164453046, + "y": 1.9215065240859985, + "heading": 2.2632732501440496e-17, + "angularVelocity": -5.54925246223202e-17, + "velocityX": 2.531936219712306, + "velocityY": 8.894540117079268e-17, + "timestamp": 0.7737886964898869 }, { - "x": 3.1118289695862966, - "y": 1.9215065240859983, - "heading": -5.2607955008850217e-23, - "angularVelocity": 3.0609884687768117e-22, - "velocityX": 1.8670397273467831, - "velocityY": -3.06686637514316e-32, - "timestamp": 0.9564415279842821 + "x": 3.0608085545751536, + "y": 1.9215065240859985, + "heading": 1.7892029047767983e-17, + "angularVelocity": -4.9009641288663106e-17, + "velocityX": 2.109946956378327, + "velocityY": 7.412152260849424e-17, + "timestamp": 0.8705122835511228 }, { - "x": 3.2546861191640164, - "y": 1.9215065240859983, - "heading": -2.971367837778933e-23, - "angularVelocity": 2.3936923829343535e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4588855517580793e-32, - "timestamp": 1.0520856807827104 + "x": 3.224073869558278, + "y": 1.9215065240859985, + "heading": 2.083425694040062e-17, + "angularVelocity": 3.042138426747003e-17, + "velocityX": 1.687957611412416, + "velocityY": 5.929781651761968e-17, + "timestamp": 0.9672358706123586 }, { - "x": 3.3618289830274506, - "y": 1.9215065240859983, - "heading": -1.3562846674979416e-23, - "angularVelocity": 1.6886357418178234e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.8475316069845286e-32, - "timestamp": 1.1477298335811386 + "x": 3.3465228580107924, + "y": 1.9215065240859985, + "heading": 1.131382345905398e-17, + "angularVelocity": -9.842749204538218e-17, + "velocityX": 1.265968231461384, + "velocityY": 4.4473700308316656e-17, + "timestamp": 1.0639594576735945 }, { - "x": 3.4332575597792516, + "x": 3.4281555180527588, "y": 1.9215065240859983, - "heading": -3.7629345495763975e-24, - "angularVelocity": 1.0246209621359342e-22, - "velocityX": 0.7468159282286508, - "velocityY": -1.2285406220691536e-32, - "timestamp": 1.2433739863795668 + "heading": -4.743251660877572e-19, + "angularVelocity": -1.2187342421598578e-16, + "velocityX": 0.8439788320741727, + "velocityY": 2.964930533250184e-17, + "timestamp": 1.1606830447348304 }, { "x": 3.468971848487854, "y": 1.9215065240859983, - "heading": -4.805430418918825e-41, - "angularVelocity": 3.9342997042908397e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.171880734353278e-33, - "timestamp": 1.339018139177995 + "heading": -3.025400677588882e-41, + "angularVelocity": 4.904504421960316e-18, + "velocityX": 0.42198942031848286, + "velocityY": 1.4824720643168207e-17, + "timestamp": 1.2574066317960664 }, { "x": 3.468971848487854, "y": 1.9215065240859983, - "heading": -6.447284134666629e-41, - "angularVelocity": -1.716658280508069e-40, - "velocityX": 0, - "velocityY": 0, - "timestamp": 1.4346622919764231 + "heading": 4.753747394144705e-41, + "angularVelocity": 8.043397133285877e-40, + "velocityX": -3.5738155514754405e-40, + "velocityY": 2.012790081694559e-42, + "timestamp": 1.3541302188573023 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLaneTaxi.traj b/src/main/deploy/choreo/SourceLaneTaxi.traj index 960b6bbb..2a7cdf97 100644 --- a/src/main/deploy/choreo/SourceLaneTaxi.traj +++ b/src/main/deploy/choreo/SourceLaneTaxi.traj @@ -6,143 +6,134 @@ "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": 9.261974312050868e-43, + "velocityY": -6.616666962933087e-41, "timestamp": 0 }, { - "x": 1.5046861371964564, + "x": 1.5097881789229493, "y": 1.9215065240859983, - "heading": -2.0425159024477705e-24, - "angularVelocity": -2.1355387753237974e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.2842385813221e-33, - "timestamp": 0.09564415279842821 + "heading": 2.3748244992157107e-19, + "angularVelocity": 2.455567735557075e-18, + "velocityX": 0.42198942031848213, + "velocityY": 1.4827190365964012e-17, + "timestamp": 0.09672358706123588 }, { - "x": 1.5761147139482576, - "y": 1.9215065240859983, - "heading": -7.39456542057925e-24, - "angularVelocity": -5.595811575686501e-23, - "velocityX": 0.7468159282286507, - "velocityY": -1.2474076386660863e-32, - "timestamp": 0.19128830559685642 + "x": 1.5914208389649156, + "y": 1.9215065240859985, + "heading": 6.823321419293863e-18, + "angularVelocity": 6.808983664986858e-17, + "velocityX": 0.843978832074171, + "velocityY": 2.965467782674968e-17, + "timestamp": 0.19344717412247175 }, { - "x": 1.6832575778116918, - "y": 1.9215065240859983, - "heading": -1.797881654717191e-23, - "angularVelocity": -1.1066309979696055e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.866390782935744e-32, - "timestamp": 0.28693245839528464 + "x": 1.7138698274174293, + "y": 1.9215065240859985, + "heading": 1.3350406511427553e-17, + "angularVelocity": 6.748261205621051e-17, + "velocityX": 1.2659682314613812, + "velocityY": 4.448259121719168e-17, + "timestamp": 0.29017076118370766 }, { - "x": 1.8261147273894114, - "y": 1.9215065240859983, - "heading": -3.609466520226707e-23, - "angularVelocity": -1.8940924580147868e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4777348604111533e-32, - "timestamp": 0.38257661119371283 + "x": 1.8771351424005533, + "y": 1.9215065240859985, + "heading": 2.3094752603641018e-17, + "angularVelocity": 1.0074517896422799e-16, + "velocityX": 1.6879576114124106, + "velocityY": 5.931116840042e-17, + "timestamp": 0.3868943482449435 }, { - "x": 2.004686160352503, - "y": 1.9215065240859983, - "heading": -6.568172939485326e-23, - "angularVelocity": -3.093458575185046e-22, - "velocityX": 1.867039727346783, - "velocityY": -3.0967151425129867e-32, - "timestamp": 0.47822076399214103 - }, - { - "x": 2.21897187204314, - "y": 1.9215065240859983, - "heading": -1.1554278307693229e-22, - "angularVelocity": -5.213187002510948e-22, - "velocityX": 2.2404475905834893, - "velocityY": -3.7086764301179e-32, - "timestamp": 0.5738649167905693 + "x": 2.081216780530401, + "y": 1.9215065240859985, + "heading": 2.7994689255823954e-17, + "angularVelocity": 5.0660149601833395e-17, + "velocityX": 2.1099469563783173, + "velocityY": 7.414094299456716e-17, + "timestamp": 0.48361793530617936 }, { - "x": 2.468971848487854, - "y": 1.9215065240859983, - "heading": -1.644337522360258e-22, - "angularVelocity": -5.111750826050956e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.332586658245059e-32, - "timestamp": 0.6695090695889975 + "x": 2.326114733911239, + "y": 1.9215065240859985, + "heading": 3.194288143588596e-17, + "angularVelocity": 4.082043898429076e-17, + "velocityX": 2.531936219712287, + "velocityY": 8.89740008515513e-17, + "timestamp": 0.5803415223674152 }, { - "x": 2.7189718249325674, - "y": 1.9215065240859983, - "heading": -1.1817502956999641e-22, - "angularVelocity": 4.836559085941933e-22, - "velocityX": 2.6138553077216615, - "velocityY": -4.2931003905648025e-32, - "timestamp": 0.7651532223874257 + "x": 2.6118289630644647, + "y": 1.9215065240859985, + "heading": 2.800063510694937e-17, + "angularVelocity": -4.0776590509092634e-17, + "velocityX": 2.953925074887259, + "velocityY": 1.036106690795216e-16, + "timestamp": 0.6770651094286511 }, { - "x": 2.933257536623205, - "y": 1.9215065240859983, - "heading": -8.188453918715617e-23, - "angularVelocity": 3.79432828279643e-22, - "velocityX": 2.2404475905834897, - "velocityY": -3.6782254895688684e-32, - "timestamp": 0.8607973751858539 + "x": 2.8567269164453046, + "y": 1.9215065240859985, + "heading": 2.2632732501440496e-17, + "angularVelocity": -5.54925246223202e-17, + "velocityX": 2.531936219712306, + "velocityY": 8.894540117079268e-17, + "timestamp": 0.7737886964898869 }, { - "x": 3.1118289695862966, - "y": 1.9215065240859983, - "heading": -5.2607955008850217e-23, - "angularVelocity": 3.0609884687768117e-22, - "velocityX": 1.8670397273467831, - "velocityY": -3.06686637514316e-32, - "timestamp": 0.9564415279842821 + "x": 3.0608085545751536, + "y": 1.9215065240859985, + "heading": 1.7892029047767983e-17, + "angularVelocity": -4.9009641288663106e-17, + "velocityX": 2.109946956378327, + "velocityY": 7.412152260849424e-17, + "timestamp": 0.8705122835511228 }, { - "x": 3.2546861191640164, - "y": 1.9215065240859983, - "heading": -2.971367837778933e-23, - "angularVelocity": 2.3936923829343535e-22, - "velocityX": 1.493631815410541, - "velocityY": -2.4588855517580793e-32, - "timestamp": 1.0520856807827104 + "x": 3.224073869558278, + "y": 1.9215065240859985, + "heading": 2.083425694040062e-17, + "angularVelocity": 3.042138426747003e-17, + "velocityX": 1.687957611412416, + "velocityY": 5.929781651761968e-17, + "timestamp": 0.9672358706123586 }, { - "x": 3.3618289830274506, - "y": 1.9215065240859983, - "heading": -1.3562846674979416e-23, - "angularVelocity": 1.6886357418178234e-22, - "velocityX": 1.1202238791245276, - "velocityY": -1.8475316069845286e-32, - "timestamp": 1.1477298335811386 + "x": 3.3465228580107924, + "y": 1.9215065240859985, + "heading": 1.131382345905398e-17, + "angularVelocity": -9.842749204538218e-17, + "velocityX": 1.265968231461384, + "velocityY": 4.4473700308316656e-17, + "timestamp": 1.0639594576735945 }, { - "x": 3.4332575597792516, + "x": 3.4281555180527588, "y": 1.9215065240859983, - "heading": -3.7629345495763975e-24, - "angularVelocity": 1.0246209621359342e-22, - "velocityX": 0.7468159282286508, - "velocityY": -1.2285406220691536e-32, - "timestamp": 1.2433739863795668 + "heading": -4.743251660877572e-19, + "angularVelocity": -1.2187342421598578e-16, + "velocityX": 0.8439788320741727, + "velocityY": 2.964930533250184e-17, + "timestamp": 1.1606830447348304 }, { "x": 3.468971848487854, "y": 1.9215065240859983, - "heading": -4.805430418918825e-41, - "angularVelocity": 3.9342997042908397e-23, - "velocityX": 0.37340796759286454, - "velocityY": -6.171880734353278e-33, - "timestamp": 1.339018139177995 + "heading": -3.025400677588882e-41, + "angularVelocity": 4.904504421960316e-18, + "velocityX": 0.42198942031848286, + "velocityY": 1.4824720643168207e-17, + "timestamp": 1.2574066317960664 }, { "x": 3.468971848487854, "y": 1.9215065240859983, - "heading": -6.447284134666629e-41, - "angularVelocity": -1.716658280508069e-40, - "velocityX": 0, - "velocityY": 0, - "timestamp": 1.4346622919764231 + "heading": 4.753747394144705e-41, + "angularVelocity": 8.043397133285877e-40, + "velocityX": -3.5738155514754405e-40, + "velocityY": 2.012790081694559e-42, + "timestamp": 1.3541302188573023 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/StraightForward.traj b/src/main/deploy/choreo/StraightForward.traj deleted file mode 100644 index 892fcac1..00000000 --- a/src/main/deploy/choreo/StraightForward.traj +++ /dev/null @@ -1,3181 +0,0 @@ -{ - "samples": [ - { - "x": -1.1376170994794325e-18, - "y": -7.908497077335199e-28, - "heading": -7.532778409384681e-27, - "angularVelocity": -8.199365209469061e-27, - "velocityX": -6.394191701786484e-18, - "velocityY": 1.7031182398553385e-25, - "timestamp": 0 - }, - { - "x": 0.0033613448640062186, - "y": -3.207353843920882e-24, - "heading": 1.8540534582969372e-19, - "angularVelocity": 1.8765611031481612e-18, - "velocityX": 0.03402149614781158, - "velocityY": -3.300241746816345e-23, - "timestamp": 0.09880061856781212 - }, - { - "x": 0.010084034581535006, - "y": -9.734755015964764e-24, - "heading": 6.933840485443198e-19, - "angularVelocity": 5.141453424525524e-18, - "velocityX": 0.06804299218951397, - "velocityY": -6.718410081091478e-23, - "timestamp": 0.19760123713562425 - }, - { - "x": 0.02016806914144748, - "y": -1.9685611056691934e-23, - "heading": 1.6696757328056013e-18, - "angularVelocity": 9.881433945283818e-18, - "velocityX": 0.10206448811847535, - "velocityY": -1.0242470180298394e-22, - "timestamp": 0.29640185570343636 - }, - { - "x": 0.033613448531886116, - "y": -3.317040886768565e-23, - "heading": 2.0919453510145205e-18, - "angularVelocity": 4.27395869496419e-18, - "velocityX": 0.1360859839274221, - "velocityY": -1.388002418111676e-22, - "timestamp": 0.3952024742712485 - }, - { - "x": 0.050420172740202905, - "y": -5.030578939773999e-23, - "heading": 2.1256769428547757e-18, - "angularVelocity": 3.4141256618882077e-19, - "velocityX": 0.17010747960835326, - "velocityY": -1.7638794972644125e-22, - "timestamp": 0.49400309283906063 - }, - { - "x": 0.07058824175287753, - "y": -7.121786937435362e-23, - "heading": 1.9477652127659873e-18, - "angularVelocity": -1.800712610315598e-18, - "velocityX": 0.20412897515244016, - "velocityY": -2.1525580928579397e-22, - "timestamp": 0.5928037114068727 - }, - { - "x": 0.09411765555542399, - "y": -9.604275159196487e-23, - "heading": 1.7477397651697942e-18, - "angularVelocity": -2.0245340371444115e-18, - "velocityX": 0.23815047054990815, - "velocityY": -2.555345814319257e-22, - "timestamp": 0.6916043299746848 - }, - { - "x": 0.12100841413228318, - "y": -1.2492405496996858e-22, - "heading": 5.516927538791495e-19, - "angularVelocity": -1.210566046231088e-17, - "velocityX": 0.27217196578989666, - "velocityY": -2.972647097568963e-22, - "timestamp": 0.7904049485424969 - }, - { - "x": 0.1512605174666992, - "y": -1.5800976588220565e-22, - "heading": -2.4367433435029e-19, - "angularVelocity": -8.050220986716945e-18, - "velocityX": 0.30619346086029187, - "velocityY": -3.4057685817351433e-22, - "timestamp": 0.889205567110309 - }, - { - "x": 0.18487396554057559, - "y": -1.9547245627985932e-22, - "heading": -4.015998529188928e-19, - "angularVelocity": -1.598423665927227e-18, - "velocityX": 0.34021495574752625, - "velocityY": -3.856342021902462e-22, - "timestamp": 0.988006185678121 - }, - { - "x": 0.22184875833430798, - "y": -2.3748820340521924e-22, - "heading": -8.43069725371826e-19, - "angularVelocity": -4.468287727535479e-18, - "velocityX": 0.37423645043633647, - "velocityY": -4.325560385300792e-22, - "timestamp": 1.0868068042459331 - }, - { - "x": 0.2621848958265872, - "y": -2.842690964569586e-22, - "heading": -1.2892913052717086e-18, - "angularVelocity": -4.516381645740685e-18, - "velocityX": 0.4082579449094682, - "velocityY": -4.815024197737192e-22, - "timestamp": 1.1856074228137452 - }, - { - "x": 0.30588237799416684, - "y": -3.3601955996258784e-22, - "heading": -1.4361286105818133e-18, - "angularVelocity": -1.4861955425735956e-18, - "velocityX": 0.44227943914731443, - "velocityY": -5.325974076598438e-22, - "timestamp": 1.2844080413815573 - }, - { - "x": 0.35294120481158675, - "y": -3.9297468573778265e-22, - "heading": -2.1279601842953803e-18, - "angularVelocity": -7.002297360670298e-18, - "velocityX": 0.47630093312746685, - "velocityY": -5.861971021361979e-22, - "timestamp": 1.3832086599493694 - }, - { - "x": 0.4033613762508404, - "y": -4.553879204439828e-22, - "heading": -1.820906029131428e-18, - "angularVelocity": 3.10781837556822e-18, - "velocityX": 0.5103224268241561, - "velocityY": -6.423935446828857e-22, - "timestamp": 1.4820092785171814 - }, - { - "x": 0.4571428922809735, - "y": -5.235567689200076e-22, - "heading": -1.2874752262796756e-18, - "angularVelocity": 5.399065310541327e-18, - "velocityX": 0.5443439202075441, - "velocityY": -7.01517753563082e-22, - "timestamp": 1.5808098970849935 - }, - { - "x": 0.5142857528675923, - "y": -5.977742188885252e-22, - "heading": -1.2551861792254937e-18, - "angularVelocity": 3.268118073802405e-19, - "velocityX": 0.5783654132428195, - "velocityY": -7.638949232743287e-22, - "timestamp": 1.6796105156528056 - }, - { - "x": 0.5747899579722567, - "y": -6.783964811322378e-22, - "heading": -1.2211517140311946e-18, - "angularVelocity": 3.4447739303320164e-19, - "velocityX": 0.6123869058890279, - "velocityY": -8.298034991350254e-22, - "timestamp": 1.7784111342206177 - }, - { - "x": 0.6386555075517218, - "y": -7.658133530250519e-22, - "heading": -6.19635872521098e-19, - "angularVelocity": 6.0881793581750656e-18, - "velocityX": 0.6464083980975359, - "velocityY": -8.997961755349937e-22, - "timestamp": 1.8772117527884298 - }, - { - "x": 0.7058824015569765, - "y": -8.604791608615287e-22, - "heading": 1.304515627908251e-20, - "angularVelocity": 6.403613779661501e-18, - "velocityX": 0.6804298898099835, - "velocityY": -9.743947632554384e-22, - "timestamp": 1.9760123713562419 - }, - { - "x": 0.7764706399320078, - "y": -9.628899194740571e-22, - "heading": 1.4096290346844686e-18, - "angularVelocity": 1.4135374581608934e-17, - "velocityX": 0.714451380955505, - "velocityY": -1.0541932211126647e-21, - "timestamp": 2.074812989924054 - }, - { - "x": 0.8504202226121862, - "y": -1.0736383554782084e-21, - "heading": 2.060605710264541e-18, - "angularVelocity": 6.5887893423673715e-18, - "velocityX": 0.7484728714468809, - "velocityY": -1.1398796868710084e-21, - "timestamp": 2.1736136084918662 - }, - { - "x": 0.9277311495221099, - "y": -1.1933843266651194e-21, - "heading": 2.952299975389915e-18, - "angularVelocity": 9.025185671520631e-18, - "velocityX": 0.782494361175087, - "velocityY": -1.2326251827346421e-21, - "timestamp": 2.2724142270596785 - }, - { - "x": 1.0084034205726677, - "y": -1.3228608726632435e-21, - "heading": 4.072843680930929e-18, - "angularVelocity": 1.134145955927035e-17, - "velocityX": 0.8165158500013651, - "velocityY": -1.3332101260360877e-21, - "timestamp": 2.371214845627491 - }, - { - "x": 1.0924370356569266, - "y": -1.463114094622031e-21, - "heading": 5.6434310876510846e-18, - "angularVelocity": 1.5896527419660045e-17, - "velocityX": 0.8505373377453298, - "velocityY": -1.4438898247438702e-21, - "timestamp": 2.470015464195303 - }, - { - "x": 1.17983199464419, - "y": -1.6151196326348776e-21, - "heading": 7.018518492787492e-18, - "angularVelocity": 1.3917792832553574e-17, - "velocityX": 0.8845588241664668, - "velocityY": -1.5652186056968467e-21, - "timestamp": 2.5688160827631155 - }, - { - "x": 1.270588297371088, - "y": -1.7802259481782284e-21, - "heading": 7.979758659218145e-18, - "angularVelocity": 9.729079426406841e-18, - "velocityX": 0.9185803089340697, - "velocityY": -1.7002423732627545e-21, - "timestamp": 2.6676167013309278 - }, - { - "x": 1.3647059436275732, - "y": -1.960029507975355e-21, - "heading": 8.919068700069961e-18, - "angularVelocity": 9.507112968894936e-18, - "velocityX": 0.9526017915757001, - "velocityY": -1.851885594364366e-21, - "timestamp": 2.76641731989874 - }, - { - "x": 1.4621849331335823, - "y": -2.156646942783595e-21, - "heading": 9.966114158544211e-18, - "angularVelocity": 1.0597541833276519e-17, - "velocityX": 0.9866232713827009, - "velocityY": -2.0254826724633154e-21, - "timestamp": 2.8652179384665524 - }, - { - "x": 1.5630252654970274, - "y": -2.372489218605291e-21, - "heading": 1.1537140907678108e-17, - "angularVelocity": 1.5900958141426103e-17, - "velocityX": 1.0206447472212212, - "velocityY": -2.2237643073670865e-21, - "timestamp": 2.9640185570343647 - }, - { - "x": 1.6672269401297757, - "y": -2.6101775109831374e-21, - "heading": 1.3077485198475941e-17, - "angularVelocity": 1.5590402638929695e-17, - "velocityX": 1.054666217107023, - "velocityY": -2.4507675580642354e-21, - "timestamp": 3.062819175602177 - }, - { - "x": 1.7747899560516078, - "y": -2.8712245936259582e-21, - "heading": 1.4443190701622367e-17, - "angularVelocity": 1.3822805901452837e-17, - "velocityX": 1.0886876770716345, - "velocityY": -2.6936515218666954e-21, - "timestamp": 3.1616197941699893 - }, - { - "x": 1.8857143113020887, - "y": -3.1425844237227276e-21, - "heading": 1.6507544936655253e-17, - "angularVelocity": 2.0894091122735124e-17, - "velocityX": 1.122709117193911, - "velocityY": -2.8091008420292473e-21, - "timestamp": 3.2604204127378016 - }, - { - "x": 2, - "y": -3.164655249070787e-21, - "heading": 1.8228167907282414e-17, - "angularVelocity": 1.7415022670654974e-17, - "velocityX": 1.156730497790058, - "velocityY": -3.0799511941989887e-22, - "timestamp": 3.359221031305614 - }, - { - "x": 2.1142856886979113, - "y": -3.147499024006811e-21, - "heading": 1.9005172299965157e-17, - "angularVelocity": 7.864349059131218e-18, - "velocityX": 1.156730497790058, - "velocityY": 2.5626451006413984e-22, - "timestamp": 3.458021649873426 - }, - { - "x": 2.225210043948392, - "y": -2.8708197340377503e-21, - "heading": 1.976235400694375e-17, - "angularVelocity": 7.66372977455926e-18, - "velocityX": 1.122709117193911, - "velocityY": 2.8612962139149132e-21, - "timestamp": 3.5568222684412385 - }, - { - "x": 2.3327730598702243, - "y": -2.607451498389937e-21, - "heading": 2.027561255801402e-17, - "angularVelocity": 5.194893740089378e-18, - "velocityX": 1.0886876770716345, - "velocityY": 2.7166086454579756e-21, - "timestamp": 3.655622887009051 - }, - { - "x": 2.4369747345029724, - "y": -2.368709002669498e-21, - "heading": 2.0492359746230776e-17, - "angularVelocity": 2.1937892093060286e-18, - "velocityX": 1.054666217107023, - "velocityY": 2.4606042161289694e-21, - "timestamp": 3.754423505576863 - }, - { - "x": 2.5378150668664174, - "y": -2.1523875052863635e-21, - "heading": 2.0979083463355325e-17, - "angularVelocity": 4.926330516473677e-18, - "velocityX": 1.0206447472212212, - "velocityY": 2.228247008672144e-21, - "timestamp": 3.8532241241446754 - }, - { - "x": 2.6352940563724263, - "y": -1.9555762507063697e-21, - "heading": 2.089955236742629e-17, - "angularVelocity": -8.049557622963381e-19, - "velocityX": 0.9866232713827009, - "velocityY": 2.0274855500697654e-21, - "timestamp": 3.9520247427124877 - }, - { - "x": 2.7294117026289118, - "y": -1.7756973118970903e-21, - "heading": 2.048181439249147e-17, - "angularVelocity": -4.228079574039095e-18, - "velocityX": 0.9526017915757001, - "velocityY": 1.8530533449651495e-21, - "timestamp": 4.0508253612803 - }, - { - "x": 2.82016800535581, - "y": -1.610696178925322e-21, - "heading": 2.0393670363820926e-17, - "angularVelocity": -8.921285320598318e-19, - "velocityX": 0.9185803089340697, - "velocityY": 1.6996791445230123e-21, - "timestamp": 4.149625979848111 - }, - { - "x": 2.907562964343073, - "y": -1.458877869554151e-21, - "heading": 2.084174445721108e-17, - "angularVelocity": 4.53514691585319e-18, - "velocityX": 0.8845588241664668, - "velocityY": 1.5641372351679697e-21, - "timestamp": 4.248426598415923 - }, - { - "x": 2.991596579427332, - "y": -1.3188231303239282e-21, - "heading": 2.0547699398998952e-17, - "angularVelocity": -2.9761329639527713e-18, - "velocityX": 0.8505373377453298, - "velocityY": 1.4429357708430696e-21, - "timestamp": 4.347227216983735 - }, - { - "x": 3.0722688504778892, - "y": -1.1894052177930912e-21, - "heading": 2.0372841992969962e-17, - "angularVelocity": -1.769787561100399e-18, - "velocityX": 0.8165158500013651, - "velocityY": 1.3327558109827944e-21, - "timestamp": 4.446027835551547 - }, - { - "x": 3.149579777387813, - "y": -1.0697504752144254e-21, - "heading": 1.984446413733655e-17, - "angularVelocity": -5.347907206549522e-18, - "velocityX": 0.782494361175087, - "velocityY": 1.2321009132712108e-21, - "timestamp": 4.544828454119359 - }, - { - "x": 3.2235293600679915, - "y": -9.590738101698616e-22, - "heading": 1.9549867930164684e-17, - "angularVelocity": -2.981711079527443e-18, - "velocityX": 0.7484728714468809, - "velocityY": 1.139418959561847e-21, - "timestamp": 4.643629072687171 - }, - { - "x": 3.2941175984430227, - "y": -8.567435506711912e-22, - "heading": 1.880855841586028e-17, - "angularVelocity": -7.503072559944813e-18, - "velocityX": 0.714451380955505, - "velocityY": 1.053479684788194e-21, - "timestamp": 4.742429691254983 - }, - { - "x": 3.361344492448277, - "y": -7.621603632941542e-22, - "heading": 1.804651402975059e-17, - "angularVelocity": -7.71293857627177e-18, - "velocityX": 0.6804298898099835, - "velocityY": 9.736267688058442e-22, - "timestamp": 4.8412303098227945 - }, - { - "x": 3.4252100420277425, - "y": -6.74848663914946e-22, - "heading": 1.7632923835461265e-17, - "angularVelocity": -4.186096719140597e-18, - "velocityX": 0.6464083980975359, - "velocityY": 8.98840146425094e-22, - "timestamp": 4.940030928390606 - }, - { - "x": 3.4857142471324067, - "y": -5.943339267652739e-22, - "heading": 1.6713354403567037e-17, - "angularVelocity": -9.30731224773011e-18, - "velocityX": 0.6123869058890279, - "velocityY": 8.286419249206994e-22, - "timestamp": 5.038831546958418 - }, - { - "x": 3.5428571077190254, - "y": -5.202415794373983e-22, - "heading": 1.5572837882186598e-17, - "angularVelocity": -1.1543605179767327e-17, - "velocityX": 0.5783654132428195, - "velocityY": 7.624593234334322e-22, - "timestamp": 5.13763216552623 - }, - { - "x": 3.596638623749159, - "y": -4.522125741089791e-22, - "heading": 1.4464732933422248e-17, - "angularVelocity": -1.1215555338005426e-17, - "velocityX": 0.5443439202075441, - "velocityY": 6.999916566838223e-22, - "timestamp": 5.236432784094042 - }, - { - "x": 3.6470587951884124, - "y": -3.899342272220007e-22, - "heading": 1.3615733384952931e-17, - "angularVelocity": -8.593048010242285e-18, - "velocityX": 0.5103224268241561, - "velocityY": 6.4066863404930395e-22, - "timestamp": 5.335233402661854 - }, - { - "x": 3.694117622005832, - "y": -3.3313328500098465e-22, - "heading": 1.2052388332633744e-17, - "angularVelocity": -1.5823220790511242e-17, - "velocityX": 0.4763009331274668, - "velocityY": 5.842312496177269e-22, - "timestamp": 5.434034021229666 - }, - { - "x": 3.737815104173412, - "y": -2.8156433257179758e-22, - "heading": 1.1136769768708063e-17, - "angularVelocity": -9.267326418690942e-18, - "velocityX": 0.4422794391473144, - "velocityY": 5.304122695590917e-22, - "timestamp": 5.5328346397974775 - }, - { - "x": 3.7781512416656913, - "y": -2.3498113666642085e-22, - "heading": 9.859209548379765e-18, - "angularVelocity": -1.2930681107294836e-17, - "velocityX": 0.40825794490946815, - "velocityY": 4.790511156155067e-22, - "timestamp": 5.631635258365289 - }, - { - "x": 3.815126034459424, - "y": -1.9316819160355648e-22, - "heading": 8.372929242622771e-18, - "angularVelocity": -1.50432199482529e-17, - "velocityX": 0.3742364504363364, - "velocityY": 4.29918013017057e-22, - "timestamp": 5.730435876933101 - }, - { - "x": 3.8487394825333006, - "y": -1.5593072898156008e-22, - "heading": 6.818382919293722e-18, - "angularVelocity": -1.5734167867569838e-17, - "velocityX": 0.3402149557475262, - "velocityY": 3.8283016928136736e-22, - "timestamp": 5.829236495500913 - }, - { - "x": 3.8789915858677166, - "y": -1.2308917074135604e-22, - "heading": 5.324791099236139e-18, - "angularVelocity": -1.5117224017561152e-17, - "velocityX": 0.3061934608602918, - "velocityY": 3.37574987627076e-22, - "timestamp": 5.928037114068725 - }, - { - "x": 3.905882344444576, - "y": -9.448393998536425e-23, - "heading": 4.011436152983029e-18, - "angularVelocity": -1.3292976330431885e-17, - "velocityX": 0.2721719657898966, - "velocityY": 2.9401710775658265e-22, - "timestamp": 6.026837732636537 - }, - { - "x": 3.9294117582471224, - "y": -6.995384807456761e-23, - "heading": 2.9887621240859383e-18, - "angularVelocity": -1.0350880993468448e-17, - "velocityX": 0.23815047054990812, - "velocityY": 2.5206100605913985e-22, - "timestamp": 6.125638351204349 - }, - { - "x": 3.949579827259797, - "y": -4.935913599830647e-23, - "heading": 2.359326118608253e-18, - "angularVelocity": -6.3707648062754864e-18, - "velocityX": 0.20412897515244013, - "velocityY": 2.1159029061948054e-22, - "timestamp": 6.2244389697721605 - }, - { - "x": 3.966386551468114, - "y": -3.25642428787695e-23, - "heading": 2.218613212934694e-18, - "angularVelocity": -1.4242065657400621e-18, - "velocityX": 0.17010747960835323, - "velocityY": 1.725072970802276e-22, - "timestamp": 6.323239588339972 - }, - { - "x": 3.979831930858553, - "y": -1.944612833644722e-23, - "heading": 1.4782676158808526e-18, - "angularVelocity": -7.493326036397043e-18, - "velocityX": 0.1360859839274221, - "velocityY": 1.3470636183720975e-22, - "timestamp": 6.422040206907784 - }, - { - "x": 3.989915965418465, - "y": -9.887317530316752e-24, - "heading": 2.216610663986935e-19, - "angularVelocity": -1.2718607311216777e-17, - "velocityX": 0.10206448811847536, - "velocityY": 9.811604675618584e-23, - "timestamp": 6.520840825475596 - }, - { - "x": 3.996638655135994, - "y": -3.7771668643641844e-24, - "heading": -2.955699978047428e-19, - "angularVelocity": -5.2350975881402005e-18, - "velocityX": 0.06804299218951398, - "velocityY": 6.266873493281363e-23, - "timestamp": 6.619641444043408 - }, - { - "x": 4, - "y": -1.0128885732606577e-24, - "heading": 1.6809468171434698e-26, - "angularVelocity": 2.991581476965099e-18, - "velocityX": 0.03402149614781159, - "velocityY": 2.828535780318287e-23, - "timestamp": 6.71844206261122 - }, - { - "x": 4, - "y": -1.4975010105831042e-24, - "heading": 7.569049534824136e-27, - "angularVelocity": -1.729958968573892e-26, - "velocityX": -1.9187470113508293e-19, - "velocityY": -5.092260525072701e-24, - "timestamp": 6.817242681179032 - }, - { - "x": 3.9966386551359934, - "y": 1.622382271786415e-20, - "heading": 3.3801616172272874e-20, - "angularVelocity": 3.421193955466234e-19, - "velocityX": -0.034021496147811595, - "velocityY": 1.6425392695916164e-19, - "timestamp": 6.9160432997468435 - }, - { - "x": 3.989915965418465, - "y": 4.867644146454126e-20, - "heading": 1.0140943353587003e-19, - "angularVelocity": 6.842853514451412e-19, - "velocityX": -0.06804299218951398, - "velocityY": 3.285129050412422e-19, - "timestamp": 7.014843918314655 - }, - { - "x": 3.9798319308585524, - "y": 9.735635325747916e-20, - "heading": 2.0282865518075705e-19, - "angularVelocity": 1.0265038932822472e-18, - "velocityX": -0.10206448811847536, - "velocityY": 4.927718248471109e-19, - "timestamp": 7.113644536882467 - }, - { - "x": 3.966386551468114, - "y": 1.6226355072631345e-19, - "heading": 3.3806475714931607e-19, - "angularVelocity": 1.368777860282825e-18, - "velocityX": -0.1360859839274221, - "velocityY": 6.570306670773842e-19, - "timestamp": 7.212445155450279 - }, - { - "x": 3.949579827259797, - "y": 2.4339802438242795e-19, - "heading": 5.071235363962782e-19, - "angularVelocity": 1.7111105005542439e-18, - "velocityX": -0.17010747960835323, - "velocityY": 8.212894100973546e-19, - "timestamp": 7.311245774018091 - }, - { - "x": 3.9294117582471224, - "y": 3.4075976233216677e-19, - "heading": 7.100112314845174e-19, - "angularVelocity": 2.0535062837733624e-18, - "velocityX": -0.2041289751524401, - "velocityY": 9.855480293458379e-19, - "timestamp": 7.410046392585903 - }, - { - "x": 3.9058823444445765, - "y": 4.543487499464003e-19, - "heading": 9.467344638444388e-19, - "angularVelocity": 2.3959690850953575e-18, - "velocityX": -0.23815047054990812, - "velocityY": 1.1498064968884804e-18, - "timestamp": 7.508847011153715 - }, - { - "x": 3.8789915858677175, - "y": 5.84164969472011e-19, - "heading": 1.2173004091506322e-18, - "angularVelocity": 2.7385045142480053e-18, - "velocityX": -0.2721719657898966, - "velocityY": 1.3140647808442577e-18, - "timestamp": 7.6076476297215265 - }, - { - "x": 3.8487394825333014, - "y": 7.302083995695225e-19, - "heading": 1.5217167130383014e-18, - "angularVelocity": 3.0811173278628614e-18, - "velocityX": -0.30619346086029176, - "velocityY": 1.478322844607657e-18, - "timestamp": 7.706448248289338 - }, - { - "x": 3.815126034459425, - "y": 8.9247901475715e-19, - "heading": 1.859991687922629e-18, - "angularVelocity": 3.42381427473618e-18, - "velocityX": -0.3402149557475262, - "velocityY": 1.6425806459233442e-18, - "timestamp": 7.80524886685715 - }, - { - "x": 3.7781512416656926, - "y": 1.0709767847438179e-18, - "heading": 2.232134281072569e-18, - "angularVelocity": 3.7666017804132156e-18, - "velocityX": -0.3742364504363364, - "velocityY": 1.8068381357194375e-18, - "timestamp": 7.904049485424962 - }, - { - "x": 3.7378151041734133, - "y": 1.2657016736040118e-18, - "heading": 2.638154283334348e-18, - "angularVelocity": 4.1094883831216016e-18, - "velocityX": -0.40825794490946815, - "velocityY": 1.9710952565763296e-18, - "timestamp": 8.002850103992774 - }, - { - "x": 3.694117622005834, - "y": 1.476653638799985e-18, - "heading": 3.078062355478974e-18, - "angularVelocity": 4.452482887685102e-18, - "velocityX": -0.4422794391473143, - "velocityY": 2.135351941018014e-18, - "timestamp": 8.101650722560587 - }, - { - "x": 3.6470587951884137, - "y": 1.7038326299290477e-18, - "heading": 3.551870199581173e-18, - "angularVelocity": 4.795595833539573e-18, - "velocityX": -0.47630093312746674, - "velocityY": 2.299608108984575e-18, - "timestamp": 8.2004513411284 - }, - { - "x": 3.5966386237491603, - "y": 1.9472385872002766e-18, - "heading": 4.059590654442284e-18, - "angularVelocity": 5.138838726011681e-18, - "velocityX": -0.5103224268241561, - "velocityY": 2.4638636646972548e-18, - "timestamp": 8.299251959696212 - }, - { - "x": 3.542857107719027, - "y": 2.206871439463151e-18, - "heading": 3.423761751446479e-18, - "angularVelocity": -6.435474798217317e-18, - "velocityX": -0.5443439202075441, - "velocityY": 2.62811849254691e-18, - "timestamp": 8.398052578264025 - }, - { - "x": 3.4857142471324085, - "y": 2.4827311017573516e-18, - "heading": 2.8218754348898888e-18, - "angularVelocity": -6.091928510129713e-18, - "velocityX": -0.5783654132428195, - "velocityY": 2.7923724518212657e-18, - "timestamp": 8.496853196831838 - }, - { - "x": 3.425210042027744, - "y": 2.7748174720594974e-18, - "heading": 2.2539492010975555e-18, - "angularVelocity": -5.748205134856798e-18, - "velocityX": -0.6123869058890279, - "velocityY": 2.956625368836816e-18, - "timestamp": 8.59565381539965 - }, - { - "x": 3.3613444924482785, - "y": 3.0831304270690512e-18, - "heading": 1.7200028141341935e-18, - "angularVelocity": -5.4042817196730335e-18, - "velocityX": -0.6464083980975359, - "velocityY": 3.1208770270543982e-18, - "timestamp": 8.694454433967463 - }, - { - "x": 3.2941175984430235, - "y": 3.407669816567964e-18, - "heading": 1.2200587363363886e-18, - "angularVelocity": -5.0601309543877636e-18, - "velocityX": -0.6804298898099835, - "velocityY": 3.2851271525026197e-18, - "timestamp": 8.793255052535276 - }, - { - "x": 3.2235293600679924, - "y": 3.748435455691416e-18, - "heading": 7.541425169569603e-19, - "angularVelocity": -4.715721595037053e-18, - "velocityX": -0.714451380955505, - "velocityY": 3.4493753928478014e-18, - "timestamp": 8.892055671103089 - }, - { - "x": 3.149579777387814, - "y": 4.105427114281349e-18, - "heading": 3.2228393306074877e-19, - "angularVelocity": -4.371010850288281e-18, - "velocityX": -0.7484728714468808, - "velocityY": 3.61362128746775e-18, - "timestamp": 8.990856289670901 - }, - { - "x": 3.0722688504778906, - "y": 4.478644501633054e-18, - "heading": -7.548243860660323e-20, - "angularVelocity": -4.025950140734063e-18, - "velocityX": -0.782494361175087, - "velocityY": 3.7778642208016786e-18, - "timestamp": 9.089656908238714 - }, - { - "x": 2.9915965794273327, - "y": 4.868087244242979e-18, - "heading": -4.391158104077786e-19, - "angularVelocity": -3.680476603421153e-18, - "velocityX": -0.8165158500013651, - "velocityY": 3.942103351400059e-18, - "timestamp": 9.188457526806527 - }, - { - "x": 2.907562964343074, - "y": 5.2737548521275295e-18, - "heading": -7.685671750620224e-19, - "angularVelocity": -3.334507044128586e-18, - "velocityX": -0.8505373377453298, - "velocityY": 4.1063374959021146e-18, - "timestamp": 9.28725814537434 - }, - { - "x": 2.8201680053558102, - "y": 5.695646665833697e-18, - "heading": -1.0637766148592117e-18, - "angularVelocity": -2.9879310340772823e-18, - "velocityX": -0.8845588241664668, - "velocityY": 4.270564932988105e-18, - "timestamp": 9.386058763942152 - }, - { - "x": 2.729411702628912, - "y": 6.1337617692699354e-18, - "heading": -1.3246696022153147e-18, - "angularVelocity": -2.6406007010458607e-18, - "velocityX": -0.9185803089340697, - "velocityY": 4.434783057031529e-18, - "timestamp": 9.484859382509965 - }, - { - "x": 2.6352940563724268, - "y": 6.588098837620084e-18, - "heading": -1.5511502459600477e-18, - "angularVelocity": -2.2922998157391772e-18, - "velocityX": -0.9526017915757001, - "velocityY": 4.59898771937502e-18, - "timestamp": 9.583660001077778 - }, - { - "x": 2.5378150668664174, - "y": 7.058655852182776e-18, - "heading": -1.7430903085127116e-18, - "angularVelocity": -1.9427009874027487e-18, - "velocityX": -0.9866232713827009, - "velocityY": 4.763171879327714e-18, - "timestamp": 9.68246061964559 - }, - { - "x": 2.4369747345029724, - "y": 7.545429512244077e-18, - "heading": -1.9003111994243256e-18, - "angularVelocity": -1.591294574936401e-18, - "velocityX": -1.0206447472212212, - "velocityY": 4.927322533296983e-18, - "timestamp": 9.781261238213403 - }, - { - "x": 2.3327730598702243, - "y": 8.04841385541656e-18, - "heading": -2.022543272012013e-18, - "angularVelocity": -1.2371589623518452e-18, - "velocityX": -1.054666217107023, - "velocityY": 5.091412692060407e-18, - "timestamp": 9.880061856781216 - }, - { - "x": 2.225210043948392, - "y": 8.567596345936824e-18, - "heading": -2.1093375884193087e-18, - "angularVelocity": -8.784794728630001e-19, - "velocityX": -1.0886876770716345, - "velocityY": 5.255375725743804e-18, - "timestamp": 9.978862475349029 - }, - { - "x": 2.1142856886979113, - "y": 9.102942798044225e-18, - "heading": -2.159797685075532e-18, - "angularVelocity": -5.107265229524464e-19, - "velocityX": -1.122709117193911, - "velocityY": 5.418992260996229e-18, - "timestamp": 10.077663093916842 - }, - { - "x": 2, - "y": 9.654290125645511e-18, - "heading": -2.171249723825065e-18, - "angularVelocity": -1.1591060473957685e-19, - "velocityX": -1.156730497790058, - "velocityY": 5.580956667058148e-18, - "timestamp": 10.176463712484654 - }, - { - "x": 1.8857143113020887, - "y": 9.61185739696908e-18, - "heading": -2.2270648319021466e-18, - "angularVelocity": -5.649266874918011e-19, - "velocityX": -1.1567304977900583, - "velocityY": -4.2949032487006095e-19, - "timestamp": 10.275264331052467 - }, - { - "x": 1.7747899560516078, - "y": 9.553513369992968e-18, - "heading": -2.3111613029527945e-18, - "angularVelocity": -8.511735047427319e-19, - "velocityX": -1.1227091171939112, - "velocityY": -5.905538912325038e-19, - "timestamp": 10.37406494962028 - }, - { - "x": 1.6672269401297757, - "y": 9.479013543281256e-18, - "heading": -2.4262130872250256e-18, - "angularVelocity": -1.1644844250684416e-18, - "velocityX": -1.0886876770716345, - "velocityY": -7.540907241750166e-19, - "timestamp": 10.472865568188093 - }, - { - "x": 1.5630252654970274, - "y": 9.388316488623372e-18, - "heading": -2.5731140577432874e-18, - "angularVelocity": -1.4868425859374024e-18, - "velocityX": -1.054666217107023, - "velocityY": -9.180463912283675e-19, - "timestamp": 10.571666186755905 - }, - { - "x": 1.4621849331335826, - "y": 9.281408255613739e-18, - "heading": -2.7523119274054557e-18, - "angularVelocity": -1.8137322254046327e-18, - "velocityX": -1.0206447472212212, - "velocityY": -1.082143017049038e-18, - "timestamp": 10.670466805323718 - }, - { - "x": 1.3647059436275732, - "y": 9.158282543727594e-18, - "heading": -2.964075868359904e-18, - "angularVelocity": -2.143346262102346e-18, - "velocityX": -0.986623271382701, - "velocityY": -1.246303269806804e-18, - "timestamp": 10.769267423891531 - }, - { - "x": 1.2705882973710878, - "y": 9.018935994247637e-18, - "heading": -3.2085845705889515e-18, - "angularVelocity": -2.4747688902482226e-18, - "velocityX": -0.9526017915757001, - "velocityY": -1.4104974218850865e-18, - "timestamp": 10.868068042459344 - }, - { - "x": 1.17983199464419, - "y": 8.86336661456802e-18, - "heading": -3.485966591694146e-18, - "angularVelocity": -2.8074927005328834e-18, - "velocityX": -0.9185803089340697, - "velocityY": -1.5747116736319965e-18, - "timestamp": 10.966868661027156 - }, - { - "x": 1.0924370356569264, - "y": 8.69157313096237e-18, - "heading": -3.796317507759278e-18, - "angularVelocity": -3.141183873969326e-18, - "velocityX": -0.8845588241664669, - "velocityY": -1.7389387660362588e-18, - "timestamp": 11.06566927959497 - }, - { - "x": 1.0084034205726677, - "y": 8.503554683319867e-18, - "heading": -4.139711774475018e-18, - "angularVelocity": -3.475628642739219e-18, - "velocityX": -0.8505373377453298, - "velocityY": -1.903174524206938e-18, - "timestamp": 11.164469898162782 - }, - { - "x": 0.9277311495221099, - "y": 8.299310665997281e-18, - "heading": -4.516208836106112e-18, - "angularVelocity": -3.810675070331667e-18, - "velocityX": -0.8165158500013652, - "velocityY": -2.067416380452275e-18, - "timestamp": 11.263270516730595 - }, - { - "x": 0.8504202226121863, - "y": 8.0788406383874e-18, - "heading": -4.9258577031240064e-18, - "angularVelocity": -4.1462175521934945e-18, - "velocityX": -0.782494361175087, - "velocityY": -2.2316626702442053e-18, - "timestamp": 11.362071135298407 - }, - { - "x": 0.7764706399320079, - "y": 7.842144271446874e-18, - "heading": -5.368698751428868e-18, - "angularVelocity": -4.482168694424708e-18, - "velocityX": -0.7484728714468809, - "velocityY": -2.395912268619746e-18, - "timestamp": 11.46087175386622 - }, - { - "x": 0.7058824015569765, - "y": 7.589221314273757e-18, - "heading": -5.844766537688045e-18, - "angularVelocity": -4.8184695983098995e-18, - "velocityX": -0.714451380955505, - "velocityY": -2.5601643880906105e-18, - "timestamp": 11.559672372434033 - }, - { - "x": 0.6386555075517218, - "y": 7.320071572246599e-18, - "heading": -6.3540907090448196e-18, - "angularVelocity": -5.155070572582827e-18, - "velocityX": -0.6804298898099835, - "velocityY": -2.7244184616393568e-18, - "timestamp": 11.658472991001846 - }, - { - "x": 0.5747899579722568, - "y": 7.034694892326384e-18, - "heading": -5.719220693211024e-18, - "angularVelocity": 6.4257695231037726e-18, - "velocityX": -0.6464083980975359, - "velocityY": -2.888674070537039e-18, - "timestamp": 11.757273609569658 - }, - { - "x": 0.5142857528675924, - "y": 6.733091152832895e-18, - "heading": -5.1176550171087195e-18, - "angularVelocity": 6.0886831811172465e-18, - "velocityX": -0.6123869058890279, - "velocityY": -3.0529308990878334e-18, - "timestamp": 11.856074228137471 - }, - { - "x": 0.4571428922809735, - "y": 6.415260256155639e-18, - "heading": -4.549413483741888e-18, - "angularVelocity": 5.751396405133998e-18, - "velocityX": -0.5783654132428195, - "velocityY": -3.2171887049931132e-18, - "timestamp": 11.954874846705284 - }, - { - "x": 0.4033613762508404, - "y": 6.081202123490567e-18, - "heading": -4.014513636196958e-18, - "angularVelocity": 5.413932068682268e-18, - "velocityX": -0.5443439202075441, - "velocityY": -3.381447299080878e-18, - "timestamp": 12.053675465273097 - }, - { - "x": 0.35294120481158675, - "y": 5.730916690946256e-18, - "heading": -3.512971170353614e-18, - "angularVelocity": 5.076308868051237e-18, - "velocityX": -0.5103224268241561, - "velocityY": -3.545706531606395e-18, - "timestamp": 12.15247608384091 - }, - { - "x": 0.3058823779941669, - "y": 5.364403906543085e-18, - "heading": -3.0448003053157668e-18, - "angularVelocity": 4.738541750260109e-18, - "velocityX": -0.47630093312746674, - "velocityY": -3.709966282517656e-18, - "timestamp": 12.251276702408722 - }, - { - "x": 0.2621848958265872, - "y": 4.981663728010729e-18, - "heading": -2.6100139125080014e-18, - "angularVelocity": 4.400644355727739e-18, - "velocityX": -0.4422794391473143, - "velocityY": -3.874226454538646e-18, - "timestamp": 12.350077320976535 - }, - { - "x": 0.22184875833430798, - "y": 4.582696121005342e-18, - "heading": -2.208623621378623e-18, - "angularVelocity": 4.062629254983044e-18, - "velocityX": -0.4082579449094682, - "velocityY": -4.038486967950237e-18, - "timestamp": 12.448877939544348 - }, - { - "x": 0.18487396554057556, - "y": 4.167501057752707e-18, - "heading": -1.8406400335009087e-18, - "angularVelocity": 3.724506851511534e-18, - "velocityX": -0.3742364504363364, - "velocityY": -4.202747756784724e-18, - "timestamp": 12.54767855811216 - }, - { - "x": 0.15126051746669916, - "y": 3.736078515959616e-18, - "heading": -1.506072956037892e-18, - "angularVelocity": 3.3862851859073145e-18, - "velocityX": -0.3402149557475262, - "velocityY": -4.367008766061658e-18, - "timestamp": 12.646479176679973 - }, - { - "x": 0.12100841413228317, - "y": 3.2884284779303413e-18, - "heading": -1.2049313957041987e-18, - "angularVelocity": 3.0479723597881684e-18, - "velocityX": -0.3061934608602918, - "velocityY": -4.531269949657919e-18, - "timestamp": 12.745279795247786 - }, - { - "x": 0.09411765555542398, - "y": 2.824550929875594e-18, - "heading": -9.372236288478572e-19, - "angularVelocity": 2.7095757655060014e-18, - "velocityX": -0.2721719657898966, - "velocityY": -4.695531268401831e-18, - "timestamp": 12.844080413815599 - }, - { - "x": 0.07058824175287753, - "y": 2.3444458613366558e-18, - "heading": -7.029573657694057e-19, - "angularVelocity": 2.371101132177252e-18, - "velocityX": -0.23815047054990812, - "velocityY": -4.859792688975063e-18, - "timestamp": 12.942881032383411 - }, - { - "x": 0.050420172740202905, - "y": 1.8481132646899755e-18, - "heading": -5.021397823279256e-19, - "angularVelocity": 2.032553869215878e-18, - "velocityX": -0.2041289751524401, - "velocityY": -5.024054182861873e-18, - "timestamp": 13.041681650951224 - }, - { - "x": 0.03361344853188612, - "y": 1.3355531347764025e-18, - "heading": -3.3477751504246894e-19, - "angularVelocity": 1.6939394354207508e-18, - "velocityX": -0.17010747960835323, - "velocityY": -5.188315725440333e-18, - "timestamp": 13.140482269519037 - }, - { - "x": 0.020168069141447482, - "y": 8.067654685706554e-19, - "heading": -2.008767863019259e-19, - "angularVelocity": 1.355262022365565e-18, - "velocityX": -0.1360859839274221, - "velocityY": -5.352577295354402e-18, - "timestamp": 13.23928288808685 - }, - { - "x": 0.010084034581535006, - "y": 2.6175026490285327e-19, - "heading": -1.0044337813369068e-19, - "angularVelocity": 1.016526087149893e-18, - "velocityX": -0.10206448811847536, - "velocityY": -5.516838874018515e-18, - "timestamp": 13.338083506654662 - }, - { - "x": 0.0033613448640062178, - "y": -2.9949247577400035e-19, - "heading": -3.3482745638057493e-20, - "angularVelocity": 6.777349387241571e-19, - "velocityX": -0.06804299218951398, - "velocityY": -5.6811004451979164e-18, - "timestamp": 13.436884125222475 - }, - { - "x": -2.9258798480024608e-18, - "y": -8.769627515656533e-19, - "heading": -1.2196216569957611e-26, - "angularVelocity": 3.388919345415541e-19, - "velocityX": -0.03402149614781159, - "velocityY": -5.845361994494316e-18, - "timestamp": 13.535684743790288 - }, - { - "x": -1.1376141000811762e-18, - "y": -1.4706605593137037e-18, - "heading": -9.263715163460755e-27, - "angularVelocity": 2.929800407564323e-26, - "velocityX": 6.5860729270801154e-18, - "velocityY": -6.0096235092764735e-18, - "timestamp": 13.6344853623581 - }, - { - "x": -5.135794547435122e-19, - "y": 0.0033333336521360404, - "heading": -2.1647079011876355e-19, - "angularVelocity": -2.2001741500814918e-18, - "velocityX": 6.3431154311892524e-18, - "velocityY": 0.03387944331015331, - "timestamp": 13.732873450427336 - }, - { - "x": 8.655817806035818e-20, - "y": 0.010000000941341826, - "heading": -5.697678805519529e-19, - "angularVelocity": -3.590855508881197e-18, - "velocityX": 6.100208278091576e-18, - "velocityY": 0.06775888646717527, - "timestamp": 13.831261538496571 - }, - { - "x": 6.628042064573855e-19, - "y": 0.020000001851181393, - "heading": -9.256562256435773e-19, - "angularVelocity": -3.617194481532914e-18, - "velocityX": 5.857356086283696e-18, - "velocityY": 0.10163832945714488, - "timestamp": 13.929649626565807 - }, - { - "x": 1.2151645581634748e-18, - "y": 0.033333336363653446, - "heading": -1.117382623336341e-18, - "angularVelocity": -1.9486820947092033e-18, - "velocityX": 5.614564136214341e-18, - "velocityY": 0.13551777226415238, - "timestamp": 14.028037714635042 - }, - { - "x": 1.7436457586956917e-18, - "y": 0.05000000445895656, - "heading": -2.3967423989417355e-19, - "angularVelocity": 8.920870961137473e-18, - "velocityX": 5.371838503153778e-18, - "velocityY": 0.16939721486990153, - "timestamp": 14.126425802704278 - }, - { - "x": 2.2482550261017366e-18, - "y": 0.07000000611520496, - "heading": 3.6125235146058906e-19, - "angularVelocity": 6.107704969814862e-18, - "velocityX": 5.129186225139142e-18, - "velocityY": 0.20327665725320723, - "timestamp": 14.224813890773513 - }, - { - "x": 2.7290003868346387e-18, - "y": 0.09333334130808112, - "heading": 2.3536802574991945e-18, - "angularVelocity": 2.0250687712038572e-17, - "velocityX": 4.886615517778109e-18, - "velocityY": 0.23715609938935361, - "timestamp": 14.323201978842748 - }, - { - "x": 3.185890819000824e-18, - "y": 0.12000001001040657, - "heading": 3.40725878345287e-18, - "angularVelocity": 1.070837766681137e-17, - "velocityX": 4.64413605242649e-18, - "velocityY": 0.27103554124926305, - "timestamp": 14.421590066911984 - }, - { - "x": 3.618936431576692e-18, - "y": 0.15000001219160564, - "heading": 4.731756190632852e-18, - "angularVelocity": 1.3461948548984505e-17, - "velocityX": 4.4017593218063314e-18, - "velocityY": 0.30491498279840595, - "timestamp": 14.519978154981219 - }, - { - "x": 4.028148691625798e-18, - "y": 0.18333334781702582, - "heading": 5.109272897761843e-18, - "angularVelocity": 3.836992583636047e-18, - "velocityX": 4.1594991279037615e-18, - "velocityY": 0.3387944239953468, - "timestamp": 14.618366243050454 - }, - { - "x": 4.413540716810058e-18, - "y": 0.22000001684706427, - "heading": 6.997707395181324e-18, - "angularVelocity": 1.919370316508542e-17, - "velocityX": 3.91737224524324e-18, - "velocityY": 0.37267386478977105, - "timestamp": 14.71675433111969 - }, - { - "x": 4.775127658373826e-18, - "y": 0.2600000192360254, - "heading": 7.882590747581164e-18, - "angularVelocity": 8.993773931256457e-18, - "velocityX": 3.6753993405321354e-18, - "velocityY": 0.406553305119753, - "timestamp": 14.815142419188925 - }, - { - "x": 5.112927212518244e-18, - "y": 0.3033333549305977, - "heading": 8.987118747688736e-18, - "angularVelocity": 1.1226200578912163e-17, - "velocityX": 3.433606277613366e-18, - "velocityY": 0.4404327449078857, - "timestamp": 14.91353050725816 - }, - { - "x": 5.426960319003839e-18, - "y": 0.3500000238677766, - "heading": 1.0530500974465385e-17, - "angularVelocity": 1.5686635916726856e-17, - "velocityX": 3.1920260198981374e-18, - "velocityY": 0.47431218405565095, - "timestamp": 15.011918595327396 - }, - { - "x": 5.7172521411776675e-18, - "y": 0.4000000259719568, - "heading": 1.2181097094013063e-17, - "angularVelocity": 1.6776333979129697e-17, - "velocityX": 2.9507014905610136e-18, - "velocityY": 0.5081916224349754, - "timestamp": 15.110306683396631 - }, - { - "x": 5.983833485064665e-18, - "y": 0.45333336115073153, - "heading": 1.4802873414250385e-17, - "angularVelocity": 2.6647239353050688e-17, - "velocityX": 2.7096900347694933e-18, - "velocityY": 0.5420710598750945, - "timestamp": 15.208694771465867 - }, - { - "x": 6.226742934460706e-18, - "y": 0.5100000292885927, - "heading": 1.7753206373705575e-17, - "angularVelocity": 2.9986626187410924e-17, - "velocityX": 2.469070695949251e-18, - "velocityY": 0.575950496141207, - "timestamp": 15.307082859535102 - }, - { - "x": 6.446030218838556e-18, - "y": 0.5700000302370294, - "heading": 2.0426163520318335e-17, - "angularVelocity": 2.7167415779249895e-17, - "velocityX": 2.22895674498737e-18, - "velocityY": 0.6098299308978823, - "timestamp": 15.405470947604337 - }, - { - "x": 6.6417618540851514e-18, - "y": 0.6333333637980277, - "heading": 2.266086393224807e-17, - "angularVelocity": 2.2713036643856925e-17, - "velocityX": 1.9895187779423867e-18, - "velocityY": 0.643709363641975, - "timestamp": 15.503859035673573 - }, - { - "x": 6.814031362443908e-18, - "y": 0.7000000296943679, - "heading": 2.4278538293185775e-17, - "angularVelocity": 1.6441673304388865e-17, - "velocityX": 1.7510312541171544e-18, - "velocityY": 0.6775887935684523, - "timestamp": 15.602247123742808 - }, - { - "x": 6.962979904521674e-18, - "y": 0.7700000275102201, - "heading": 2.5576668981263964e-17, - "angularVelocity": 1.3193866152308666e-17, - "velocityX": 1.5139783111446211e-18, - "velocityY": 0.7114682192685073, - "timestamp": 15.700635211812044 - }, - { - "x": 7.08884511898635e-18, - "y": 0.8433333565525355, - "heading": 2.687208646774119e-17, - "angularVelocity": 1.3166263419656072e-17, - "velocityX": 1.2793404346458061e-18, - "velocityY": 0.7453476379245281, - "timestamp": 15.799023299881279 - }, - { - "x": 7.192111469900735e-18, - "y": 0.9200000154352173, - "heading": 2.792648282176678e-17, - "angularVelocity": 1.07165211280523e-17, - "velocityX": 1.0496260639054993e-18, - "velocityY": 0.7792270424924943, - "timestamp": 15.897411387950514 - }, - { - "x": 7.274312354830099e-18, - "y": 1, - "heading": 3.000827595607593e-17, - "angularVelocity": 2.1158704872256568e-17, - "velocityX": 8.354953731965381e-19, - "velocityY": 0.813106404796549, - "timestamp": 15.99579947601975 - }, - { - "x": 6.687149573241866e-18, - "y": 1.0799999845647827, - "heading": 2.930546078718981e-17, - "angularVelocity": -7.143039017142222e-18, - "velocityX": -5.968361601177678e-18, - "velocityY": 0.813106404796549, - "timestamp": 16.094187564088987 - }, - { - "x": 6.126769558515328e-18, - "y": 1.1566666434474644, - "heading": 2.90297401967108e-17, - "angularVelocity": -2.8021955879817803e-18, - "velocityX": -5.696128244278018e-18, - "velocityY": 0.7792270424924943, - "timestamp": 16.192575652158222 - }, - { - "x": 5.591823986937553e-18, - "y": 1.2299999724897799, - "heading": 2.857601134904138e-17, - "angularVelocity": -4.611479301985519e-18, - "velocityX": -5.437597031717557e-18, - "velocityY": 0.745347637924528, - "timestamp": 16.290963740227458 - }, - { - "x": 5.081849281816435e-18, - "y": 1.299999970305632, - "heading": 2.647194627599618e-17, - "angularVelocity": -2.1385244054143357e-17, - "velocityX": -5.183776870340226e-18, - "velocityY": 0.7114682192685073, - "timestamp": 16.389351828296693 - }, - { - "x": 4.596610451039836e-18, - "y": 1.3666666362019722, - "heading": 2.5047603581040707e-17, - "angularVelocity": -1.4476678683571795e-17, - "velocityX": -4.932344796891426e-18, - "velocityY": 0.6775887935684523, - "timestamp": 16.48773991636593 - }, - { - "x": 4.135965398247791e-18, - "y": 1.4299999697629706, - "heading": 2.302762325152366e-17, - "angularVelocity": -2.0530653971935406e-17, - "velocityX": -4.682356767449444e-18, - "velocityY": 0.6437093636419748, - "timestamp": 16.586128004435164 - }, - { - "x": 3.699818913378491e-18, - "y": 1.4899999707114076, - "heading": 2.142223093747466e-17, - "angularVelocity": -1.6316863078396375e-17, - "velocityX": -4.433336304057429e-18, - "velocityY": 0.6098299308978822, - "timestamp": 16.6845160925044 - }, - { - "x": 3.2881027461238596e-18, - "y": 1.5466666388492685, - "heading": 2.064122419774981e-17, - "angularVelocity": -7.937956363543176e-18, - "velocityX": -4.185009430290064e-18, - "velocityY": 0.5759504961412069, - "timestamp": 16.782904180573635 - }, - { - "x": 2.9007655712244052e-18, - "y": 1.5999999740280433, - "heading": 1.9969619559651016e-17, - "angularVelocity": -6.826019925002285e-18, - "velocityX": -3.937204146623221e-18, - "velocityY": 0.5420710598750945, - "timestamp": 16.88129226864287 - }, - { - "x": 2.5377673846695666e-18, - "y": 1.6499999761322235, - "heading": 1.9374646420177126e-17, - "angularVelocity": -6.047157286094339e-18, - "velocityX": -3.689805401521427e-18, - "velocityY": 0.5081916224349754, - "timestamp": 16.979680356712105 - }, - { - "x": 2.1990761279741485e-18, - "y": 1.6966666450694023, - "heading": 1.8916317262571595e-17, - "angularVelocity": -4.65833693251108e-18, - "velocityX": -3.4427324491167103e-18, - "velocityY": 0.4743121840556509, - "timestamp": 17.07806844478134 - }, - { - "x": 1.8846655332659204e-18, - "y": 1.7399999807639748, - "heading": 1.7548485280288527e-17, - "angularVelocity": -1.3902376050525728e-17, - "velocityX": -3.1959264426763933e-18, - "velocityY": 0.44043274490788564, - "timestamp": 17.176456532850576 - }, - { - "x": 1.5945136831032818e-18, - "y": 1.7799999831529358, - "heading": 1.5621235661695857e-17, - "angularVelocity": -1.9588207602447508e-17, - "velocityX": -2.9493431711978625e-18, - "velocityY": 0.40655330511975296, - "timestamp": 17.27484462091981 - }, - { - "x": 1.3286020112835303e-18, - "y": 1.8166666521829742, - "heading": 1.3232319504175871e-17, - "angularVelocity": -2.4280513008707978e-17, - "velocityX": -2.7029485778613663e-18, - "velocityY": 0.37267386478977105, - "timestamp": 17.373232708989047 - }, - { - "x": 1.086914587899011e-18, - "y": 1.8499999878083944, - "heading": 1.2132419497554097e-17, - "angularVelocity": -1.1179173608616076e-17, - "velocityX": -2.4567158715211472e-18, - "velocityY": 0.3387944239953468, - "timestamp": 17.471620797058282 - }, - { - "x": 8.694375942744545e-19, - "y": 1.8799999899895934, - "heading": 1.0540845375160962e-17, - "angularVelocity": -1.617647020627435e-17, - "velocityX": -2.21062359685979e-18, - "velocityY": 0.304914982798406, - "timestamp": 17.570008885127518 - }, - { - "x": 6.761589286442026e-19, - "y": 1.9066666586919188, - "heading": 8.883338993825269e-18, - "angularVelocity": -1.6846597854180293e-17, - "velocityX": -1.964654305865992e-18, - "velocityY": 0.27103554124926305, - "timestamp": 17.668396973196753 - }, - { - "x": 5.070679042760459e-19, - "y": 1.929999993884795, - "heading": 6.529061416277828e-18, - "angularVelocity": -2.392846609653831e-17, - "velocityX": -1.7187936183501359e-18, - "velocityY": 0.2371560993893536, - "timestamp": 17.76678506126599 - }, - { - "x": 3.6215501445535984e-19, - "y": 1.9499999955410434, - "heading": 5.175466168401875e-18, - "angularVelocity": -1.3757701954247557e-17, - "velocityX": -1.4730295424471154e-18, - "velocityY": 0.20327665725320723, - "timestamp": 17.865173149335224 - }, - { - "x": 2.414117467704845e-19, - "y": 1.9666666636363466, - "heading": 2.4887675690357e-18, - "angularVelocity": -2.7307142780637332e-17, - "velocityX": -1.2273519735815864e-18, - "velocityY": 0.1693972148699015, - "timestamp": 17.96356123740446 - }, - { - "x": 1.4483043443847653e-19, - "y": 1.9799999981488186, - "heading": 8.601988500829106e-19, - "angularVelocity": -1.655249097200501e-17, - "velocityX": -9.817523181071624e-19, - "velocityY": 0.13551777226415235, - "timestamp": 18.061949325473694 - }, - { - "x": 7.240413585822682e-20, - "y": 1.989999999058658, - "heading": 3.266565799202321e-19, - "angularVelocity": -5.4228284055470716e-18, - "velocityX": -7.3622320639297e-19, - "velocityY": 0.10163832945714488, - "timestamp": 18.16033741354293 - }, - { - "x": 2.4126536001171076e-20, - "y": 1.9966666663478638, - "heading": 2.6734077366946317e-19, - "angularVelocity": -6.028723729689646e-19, - "velocityX": -4.907582709261484e-19, - "velocityY": 0.06775888646717529, - "timestamp": 18.258725501612165 - }, - { - "x": -8.135093947234693e-24, - "y": 1.9999999999999998, - "heading": -1.6954777581594528e-26, - "angularVelocity": -2.7172052030925932e-18, - "velocityX": -2.4535197248634874e-19, - "velocityY": 0.033879443310153315, - "timestamp": 18.3571135896814 - }, - { - "x": -5.008838848500651e-24, - "y": 2, - "heading": -9.913062195911202e-27, - "angularVelocity": -2.918386915613901e-26, - "velocityX": 2.1783781061156283e-24, - "velocityY": -2.5283992697715324e-19, - "timestamp": 18.455501677750636 - }, - { - "x": 2.0753539310053916e-20, - "y": 1.9966666663478638, - "heading": -3.3924267442427026e-25, - "angularVelocity": -3.3472516337821726e-24, - "velocityX": 2.1102334506622982e-19, - "velocityY": -0.03387944331015331, - "timestamp": 18.55388976581987 - }, - { - "x": 6.227260339405634e-20, - "y": 1.989999999058658, - "heading": -9.415127970314296e-25, - "angularVelocity": -6.1213762440798274e-24, - "velocityX": 4.2204537503841897e-19, - "velocityY": -0.06775888646717527, - "timestamp": 18.652277853889107 - }, - { - "x": 1.2455227342885217e-19, - "y": 1.9799999981488186, - "heading": -1.7962723582885153e-24, - "angularVelocity": -8.687657753919161e-24, - "velocityX": 6.330683169707924e-19, - "velocityY": -0.10163832945714488, - "timestamp": 18.750665941958342 - }, - { - "x": 2.075926447807237e-19, - "y": 1.9666666636363466, - "heading": -2.8366172681319197e-24, - "angularVelocity": -1.0573910042856395e-23, - "velocityX": 8.440922249651074e-19, - "velocityY": -0.13551777226415238, - "timestamp": 18.849054030027578 - }, - { - "x": 3.1139381878486925e-19, - "y": 1.9499999955410434, - "heading": -3.972041686262479e-24, - "angularVelocity": -1.1540279609110701e-23, - "velocityX": 1.0551171593656732e-18, - "velocityY": -0.1693972148699015, - "timestamp": 18.947442118096813 - }, - { - "x": 4.359559034453201e-19, - "y": 1.929999993884795, - "heading": -5.159478281485765e-24, - "angularVelocity": -1.20689097210531e-23, - "velocityX": 1.2661431875802598e-18, - "velocityY": -0.20327665725320723, - "timestamp": 19.04583020616605 - }, - { - "x": 5.812790142432268e-19, - "y": 1.9066666586919188, - "heading": -6.324264281794437e-24, - "angularVelocity": -1.1838680158880217e-23, - "velocityX": 1.4771703851722756e-18, - "velocityY": -0.2371560993893536, - "timestamp": 19.144218294235284 - }, - { - "x": 7.473632750557841e-19, - "y": 1.8799999899895934, - "heading": -7.433822752952983e-24, - "angularVelocity": -1.1277360292414862e-23, - "velocityX": 1.6881988369545231e-18, - "velocityY": -0.27103554124926305, - "timestamp": 19.24260638230452 - }, - { - "x": 9.342088191957516e-19, - "y": 1.8499999878083944, - "heading": -8.289463274563344e-24, - "angularVelocity": -8.696581833507738e-24, - "velocityX": 1.899228638237177e-18, - "velocityY": -0.304914982798406, - "timestamp": 19.340994470373754 - }, - { - "x": 1.1418157905706857e-18, - "y": 1.8166666521829742, - "heading": -8.791094438531766e-24, - "angularVelocity": -5.098477457853364e-24, - "velocityX": 2.1102598959558546e-18, - "velocityY": -0.33879442399534687, - "timestamp": 19.43938255844299 - }, - { - "x": 1.3701843449632601e-18, - "y": 1.7799999831529358, - "heading": -8.958538336930905e-24, - "angularVelocity": -1.7018687746695977e-24, - "velocityX": 2.3212927299416833e-18, - "velocityY": -0.37267386478977105, - "timestamp": 19.537770646512225 - }, - { - "x": 1.6193146513766686e-18, - "y": 1.7399999807639748, - "heading": -8.662387950504875e-24, - "angularVelocity": 3.010025947478922e-24, - "velocityX": 2.532327273455358e-18, - "velocityY": -0.406553305119753, - "timestamp": 19.63615873458146 - }, - { - "x": 1.8892068933023243e-18, - "y": 1.6966666450694023, - "heading": -7.763746676879998e-24, - "angularVelocity": 9.133641008650935e-24, - "velocityX": 2.743363672401436e-18, - "velocityY": -0.44043274490788564, - "timestamp": 19.734546822650696 - }, - { - "x": 2.179861269683059e-18, - "y": 1.6499999761322235, - "heading": -5.979891084403687e-24, - "angularVelocity": 1.8130823090544997e-23, - "velocityX": 2.9544020820640314e-18, - "velocityY": -0.47431218405565095, - "timestamp": 19.83293491071993 - }, - { - "x": 2.491277994923378e-18, - "y": 1.5999999740280433, - "heading": -3.1318625511206987e-24, - "angularVelocity": 2.8946883296905834e-23, - "velocityX": 3.1654426573706926e-18, - "velocityY": -0.5081916224349754, - "timestamp": 19.931322998789167 - }, - { - "x": 2.8234572966317407e-18, - "y": 1.5466666388492685, - "heading": 1.2746740644139744e-24, - "angularVelocity": 4.47872963412253e-23, - "velocityX": 3.376485529499522e-18, - "velocityY": -0.5420710598750945, - "timestamp": 20.029711086858402 - }, - { - "x": 3.1763994081437363e-18, - "y": 1.4899999707114076, - "heading": 7.137353173597414e-24, - "angularVelocity": 5.958725558661365e-23, - "velocityX": 3.587530752531668e-18, - "velocityY": -0.575950496141207, - "timestamp": 20.128099174927637 - }, - { - "x": 3.550104548721676e-18, - "y": 1.4299999697629708, - "heading": 1.4784368588518567e-23, - "angularVelocity": 7.772294744741366e-23, - "velocityX": 3.798578177501792e-18, - "velocityY": -0.6098299308978822, - "timestamp": 20.226487262996873 - }, - { - "x": 3.944572873439836e-18, - "y": 1.3666666362019724, - "heading": 2.4685522368597076e-23, - "angularVelocity": 1.0063362932558388e-22, - "velocityX": 4.009627143145663e-18, - "velocityY": -0.643709363641975, - "timestamp": 20.324875351066108 - }, - { - "x": 4.35980434250334e-18, - "y": 1.2999999703056322, - "heading": 3.740510491747365e-23, - "angularVelocity": 1.2927969167310106e-22, - "velocityX": 4.220675654736974e-18, - "velocityY": -0.6775887935684523, - "timestamp": 20.423263439135344 - }, - { - "x": 4.7957983473533305e-18, - "y": 1.22999997248978, - "heading": 5.318650036063457e-23, - "angularVelocity": 1.6039947437582512e-22, - "velocityX": 4.431717906486655e-18, - "velocityY": -0.7114682192685073, - "timestamp": 20.52165152720458 - }, - { - "x": 5.252552446679876e-18, - "y": 1.1566666434474648, - "heading": 7.27694765189339e-23, - "angularVelocity": 1.9903820911278035e-22, - "velocityX": 4.642735221235561e-18, - "velocityY": -0.7453476379245281, - "timestamp": 20.620039615273814 - }, - { - "x": 5.730056595973869e-18, - "y": 1.079999984564783, - "heading": 1.0096630665085409e-22, - "angularVelocity": 2.865878607555212e-22, - "velocityX": 4.853650197261181e-18, - "velocityY": -0.7792270424924945, - "timestamp": 20.71842770334305 - }, - { - "x": 6.2282427168358e-18, - "y": 1, - "heading": 1.3642695798435716e-22, - "angularVelocity": 3.6041629217015064e-22, - "velocityX": 5.063872492830655e-18, - "velocityY": -0.8131064047965492, - "timestamp": 20.816815791412285 - }, - { - "x": 6.183163776953066e-18, - "y": 0.9200000154352174, - "heading": 1.0949873427318893e-22, - "angularVelocity": -2.7369386218952554e-22, - "velocityX": -4.58180081175704e-19, - "velocityY": -0.8131064047965493, - "timestamp": 20.91520387948152 - }, - { - "x": 6.117523635853445e-18, - "y": 0.8433333565525356, - "heading": 9.207244155443994e-23, - "angularVelocity": -1.771178319990209e-22, - "velocityX": -6.671778045636001e-19, - "velocityY": -0.7792270424924946, - "timestamp": 21.013591967550756 - }, - { - "x": 6.031184241871937e-18, - "y": 0.7700000275102202, - "heading": 7.870338506115835e-23, - "angularVelocity": -1.3588087365283406e-22, - "velocityX": -8.775779476826745e-19, - "velocityY": -0.7453476379245282, - "timestamp": 21.11198005561999 - }, - { - "x": 5.924117435021876e-18, - "y": 0.7000000296943679, - "heading": 6.771187556227619e-23, - "angularVelocity": -1.1171597807921007e-22, - "velocityX": -1.0882640657264913e-18, - "velocityY": -0.7114682192685075, - "timestamp": 21.210368143689227 - }, - { - "x": 5.796312293836418e-18, - "y": 0.6333333637980278, - "heading": 5.866324558507834e-23, - "angularVelocity": -9.196882191601766e-23, - "velocityX": -1.2990610684817378e-18, - "velocityY": -0.6775887935684524, - "timestamp": 21.308756231758462 - }, - { - "x": 5.647763255268458e-18, - "y": 0.5700000302370295, - "heading": 5.1400871049015666e-23, - "angularVelocity": -7.381359399345974e-23, - "velocityX": -1.5099145415853494e-18, - "velocityY": -0.6437093636419751, - "timestamp": 21.407144319827697 - }, - { - "x": 5.4784670203584905e-18, - "y": 0.5100000292885928, - "heading": 4.54530402227702e-23, - "angularVelocity": -6.045276480742016e-23, - "velocityX": -1.7208014970199695e-18, - "velocityY": -0.6098299308978823, - "timestamp": 21.505532407896933 - }, - { - "x": 5.288421433894345e-18, - "y": 0.45333336115073164, - "heading": 4.030799828317159e-23, - "angularVelocity": -5.2293407257414914e-23, - "velocityX": -1.9317103236247645e-18, - "velocityY": -0.5759504961412071, - "timestamp": 21.603920495966168 - }, - { - "x": 5.077624990656839e-18, - "y": 0.4000000259719569, - "heading": 3.58579938101029e-23, - "angularVelocity": -4.5229107826418413e-23, - "velocityX": -2.1426344234041997e-18, - "velocityY": -0.5420710598750946, - "timestamp": 21.702308584035404 - }, - { - "x": 4.846076586985391e-18, - "y": 0.3500000238677767, - "heading": 3.180712014523294e-23, - "angularVelocity": -4.1172381156419403e-23, - "velocityX": -2.353569720709197e-18, - "velocityY": -0.5081916224349755, - "timestamp": 21.80069667210464 - }, - { - "x": 4.593775383282716e-18, - "y": 0.3033333549305977, - "heading": 2.770876605483693e-23, - "angularVelocity": -4.165496316836929e-23, - "velocityX": -2.564513535686176e-18, - "velocityY": -0.474312184055651, - "timestamp": 21.899084760173874 - }, - { - "x": 4.320720722178402e-18, - "y": 0.26000001923602545, - "heading": 2.3786765227246354e-23, - "angularVelocity": -3.986256457760449e-23, - "velocityX": -2.7754640190164052e-18, - "velocityY": -0.4404327449078857, - "timestamp": 21.99747284824311 - }, - { - "x": 4.02691207701313e-18, - "y": 0.22000001684706427, - "heading": 2.020229881065505e-23, - "angularVelocity": -3.6431920742617227e-23, - "velocityX": -2.9864198443486014e-18, - "velocityY": -0.4065533051197531, - "timestamp": 22.095860936312345 - }, - { - "x": 3.712349017892417e-18, - "y": 0.18333334781702584, - "heading": 1.6882521656893888e-23, - "angularVelocity": -3.374167614218178e-23, - "velocityX": -3.197380029916969e-18, - "velocityY": -0.37267386478977116, - "timestamp": 22.19424902438158 - }, - { - "x": 3.377031188434149e-18, - "y": 0.15000001219160564, - "heading": 1.3838247028470944e-23, - "angularVelocity": -3.0941513829444673e-23, - "velocityX": -3.4083438298267012e-18, - "velocityY": -0.33879442399534687, - "timestamp": 22.292637112450816 - }, - { - "x": 3.0209582893555436e-18, - "y": 0.12000001001040657, - "heading": 1.1063576902336286e-23, - "angularVelocity": -2.8201330931181093e-23, - "velocityX": -3.619310664732038e-18, - "velocityY": -0.304914982798406, - "timestamp": 22.39102520052005 - }, - { - "x": 2.6441300665755467e-18, - "y": 0.09333334130808112, - "heading": 8.606488657282892e-24, - "angularVelocity": -2.4973453877481673e-23, - "velocityX": -3.83028007615491e-18, - "velocityY": -0.27103554124926305, - "timestamp": 22.489413288589287 - }, - { - "x": 2.2465463023701263e-18, - "y": 0.07000000611520496, - "heading": 6.4325429496626686e-24, - "angularVelocity": -2.2095612525455652e-23, - "velocityX": -4.041251695195559e-18, - "velocityY": -0.23715609938935361, - "timestamp": 22.587801376658522 - }, - { - "x": 1.8282068086937753e-18, - "y": 0.05000000445895656, - "heading": 4.560020147095621e-24, - "angularVelocity": -1.9031993197120398e-23, - "velocityX": -4.252225220837285e-18, - "velocityY": -0.20327665725320723, - "timestamp": 22.686189464727757 - }, - { - "x": 1.3891114220101059e-18, - "y": 0.033333336363653446, - "heading": 3.049320875422628e-24, - "angularVelocity": -1.5354485508607965e-23, - "velocityX": -4.463200404415354e-18, - "velocityY": -0.16939721486990153, - "timestamp": 22.784577552796993 - }, - { - "x": 9.292599992741813e-19, - "y": 0.02000000185118139, - "heading": 1.8599640085541418e-24, - "angularVelocity": -1.2088421222550768e-23, - "velocityX": -4.674177038146959e-18, - "velocityY": -0.13551777226415238, - "timestamp": 22.882965640866228 - }, - { - "x": 4.48652414721063e-19, - "y": 0.010000000941341822, - "heading": 9.448880778978016e-25, - "angularVelocity": -9.300681800302624e-24, - "velocityX": -4.885154946696719e-18, - "velocityY": -0.10163832945714489, - "timestamp": 22.981353728935463 - }, - { - "x": -5.271144269665744e-20, - "y": 0.0033333336521360374, - "heading": 3.2591541895178886e-25, - "angularVelocity": -6.291130188677765e-24, - "velocityX": -5.096133980824773e-18, - "velocityY": -0.06775888646717527, - "timestamp": 23.0797418170047 - }, - { - "x": -5.748316713988906e-19, - "y": -4.132250866556525e-18, - "heading": -1.2567862526727601e-29, - "angularVelocity": -3.3126782172732724e-24, - "velocityX": -5.307114012445354e-18, - "velocityY": -0.03387944331015331, - "timestamp": 23.178129905073934 - }, - { - "x": -1.1177083588923526e-18, - "y": -2.588370317815696e-18, - "heading": -1.4472783150300173e-29, - "angularVelocity": -1.9354755320059638e-29, - "velocityX": -5.51809493087181e-18, - "velocityY": 7.443696375437466e-19, - "timestamp": 23.27651799314317 - }, - { - "x": 0.0023781215211549377, - "y": 0.0023781215211549364, - "heading": -5.418034508365842e-21, - "angularVelocity": -5.482324599836796e-20, - "velocityX": 0.0240633670172817, - "velocityY": 0.024063367017281703, - "timestamp": 23.375345456458966 - }, - { - "x": 0.007134364554539963, - "y": 0.007134364554539962, - "heading": 2.784928879289317e-19, - "angularVelocity": 2.872793614299252e-18, - "velocityX": 0.04812673394425598, - "velocityY": 0.04812673394425598, - "timestamp": 23.474172919774762 - }, - { - "x": 0.01426872909055665, - "y": 0.014268729090556648, - "heading": 2.6290919679734843e-19, - "angularVelocity": -1.5768594792595913e-19, - "velocityX": 0.07219010077410719, - "velocityY": 0.07219010077410717, - "timestamp": 23.57300038309056 - }, - { - "x": 0.02378121511885375, - "y": 0.023781215118853744, - "heading": 2.4261266154684275e-19, - "angularVelocity": -2.053734849348351e-19, - "velocityX": 0.09625346749921782, - "velocityY": 0.0962534674992178, - "timestamp": 23.671827846406355 - }, - { - "x": 0.03567182262823502, - "y": 0.03567182262823501, - "heading": -1.6278307756301597e-19, - "angularVelocity": -4.102055402193371e-18, - "velocityX": 0.12031683411103763, - "velocityY": 0.12031683411103761, - "timestamp": 23.77065530972215 - }, - { - "x": 0.049940551606551344, - "y": 0.04994055160655134, - "heading": -8.668505353601748e-19, - "angularVelocity": -7.124208314248421e-18, - "velocityX": 0.14438020059992485, - "velocityY": 0.14438020059992482, - "timestamp": 23.869482773037948 - }, - { - "x": 0.06658740204057369, - "y": 0.06658740204057367, - "heading": -1.2802085811795093e-18, - "angularVelocity": -4.182623038241345e-18, - "velocityX": 0.16844356695495213, - "velocityY": 0.1684435669549521, - "timestamp": 23.968310236353744 - }, - { - "x": 0.0856123739158424, - "y": 0.08561237391584238, - "heading": -1.394541841050903e-18, - "angularVelocity": -1.1568975558424374e-18, - "velocityX": 0.1925069331636674, - "velocityY": 0.19250693316366735, - "timestamp": 24.06713769966954 - }, - { - "x": 0.10701546721648714, - "y": 0.10701546721648711, - "heading": -1.5119540621698801e-18, - "angularVelocity": -1.1880524815792804e-18, - "velocityX": 0.21657029921179627, - "velocityY": 0.21657029921179627, - "timestamp": 24.165965162985337 - }, - { - "x": 0.13079668192500984, - "y": 0.1307966819250098, - "heading": -1.9265469652509166e-18, - "angularVelocity": -4.195118191853065e-18, - "velocityX": 0.2406336650828684, - "velocityY": 0.2406336650828684, - "timestamp": 24.264792626301134 - }, - { - "x": 0.15695601802202072, - "y": 0.15695601802202067, - "heading": -2.1496546222771247e-18, - "angularVelocity": -2.2575471033153812e-18, - "velocityX": 0.26469703075774254, - "velocityY": 0.2646970307577425, - "timestamp": 24.36362008961693 - }, - { - "x": 0.1854934754859141, - "y": 0.18549347548591405, - "heading": -2.374412701432123e-18, - "angularVelocity": -2.274247218263183e-18, - "velocityX": 0.28876039621399596, - "velocityY": 0.2887603962139959, - "timestamp": 24.462447552932726 - }, - { - "x": 0.21640905429246535, - "y": 0.2164090542924653, - "heading": -2.8947586701185075e-18, - "angularVelocity": -5.265196027705901e-18, - "velocityX": 0.3128237614251293, - "velocityY": 0.31282376142512924, - "timestamp": 24.561275016248523 - }, - { - "x": 0.24970275441432413, - "y": 0.24970275441432405, - "heading": -3.4154953875764575e-18, - "angularVelocity": -5.2691499554024646e-18, - "velocityX": 0.33688712635951407, - "velocityY": 0.33688712635951407, - "timestamp": 24.66010247956432 - }, - { - "x": 0.2853745758203676, - "y": 0.28537457582036757, - "heading": -3.908799891042607e-18, - "angularVelocity": -4.9915731422981935e-18, - "velocityX": 0.36095049097897813, - "velocityY": 0.3609504909789781, - "timestamp": 24.758929942880116 - }, - { - "x": 0.3234245184748622, - "y": 0.32342451847486214, - "heading": -4.106333508592265e-18, - "angularVelocity": -1.998772747832259e-18, - "velocityX": 0.38501385523686654, - "velocityY": 0.38501385523686654, - "timestamp": 24.857757406195912 - }, - { - "x": 0.3638525823363569, - "y": 0.36385258233635687, - "heading": -4.301612868352148e-18, - "angularVelocity": -1.9759627937070713e-18, - "velocityX": 0.4090772190753272, - "velocityY": 0.4090772190753271, - "timestamp": 24.95658486951171 - }, - { - "x": 0.4066587673561919, - "y": 0.40665876735619183, - "heading": -4.4934499230572835e-18, - "angularVelocity": -1.9411313422842306e-18, - "velocityX": 0.4331405824214162, - "velocityY": 0.4331405824214161, - "timestamp": 25.055412332827505 - }, - { - "x": 0.45184307347643876, - "y": 0.4518430734764387, - "heading": -4.38590045106494e-18, - "angularVelocity": 1.0882545061626265e-18, - "velocityX": 0.4572039451813483, - "velocityY": 0.45720394518134827, - "timestamp": 25.1542397961433 - }, - { - "x": 0.49940550062697375, - "y": 0.49940550062697364, - "heading": -4.075459454626922e-18, - "angularVelocity": 3.1412417917185753e-18, - "velocityX": 0.48126730723172234, - "velocityY": 0.48126730723172234, - "timestamp": 25.253067259459097 - }, - { - "x": 0.5493460487211731, - "y": 0.549346048721173, - "heading": -3.461804526195924e-18, - "angularVelocity": 6.209355725483015e-18, - "velocityX": 0.5053306684055835, - "velocityY": 0.5053306684055834, - "timestamp": 25.351894722774894 - }, - { - "x": 0.6016647176493138, - "y": 0.6016647176493137, - "heading": -3.1313136201207807e-18, - "angularVelocity": 3.3441196837755998e-18, - "velocityX": 0.5293940284691947, - "velocityY": 0.5293940284691947, - "timestamp": 25.45072218609069 - }, - { - "x": 0.6563615072679119, - "y": 0.6563615072679118, - "heading": -2.785953977666397e-18, - "angularVelocity": 3.494571203940914e-18, - "velocityX": 0.5534573870809412, - "velocityY": 0.5534573870809412, - "timestamp": 25.549549649406487 - }, - { - "x": 0.7134364173813069, - "y": 0.7134364173813067, - "heading": -2.715478020612081e-18, - "angularVelocity": 7.131208814821819e-19, - "velocityX": 0.5775207437128721, - "velocityY": 0.577520743712872, - "timestamp": 25.648377112722283 - }, - { - "x": 0.772889447706879, - "y": 0.7728894477068786, - "heading": -2.308002641013774e-18, - "angularVelocity": 4.123098255203255e-18, - "velocityX": 0.60158409748507, - "velocityY": 0.60158409748507, - "timestamp": 25.74720457603808 - }, - { - "x": 0.8347205978005107, - "y": 0.8347205978005103, - "heading": -2.1882520138105614e-18, - "angularVelocity": 1.2117138211518718e-18, - "velocityX": 0.6256474467634036, - "velocityY": 0.6256474467634034, - "timestamp": 25.846032039353876 - }, - { - "x": 0.8989298668627916, - "y": 0.8989298668627914, - "heading": -2.600558892907419e-18, - "angularVelocity": -4.171986778366762e-18, - "velocityX": 0.6497107879527877, - "velocityY": 0.6497107879527876, - "timestamp": 25.944859502669672 - }, - { - "x": 0.9655172530284372, - "y": 0.9655172530284369, - "heading": -2.9104158106151282e-18, - "angularVelocity": -3.1353319319610485e-18, - "velocityX": 0.6737741102680166, - "velocityY": 0.6737741102680165, - "timestamp": 26.04368696598547 - }, - { - "x": 1.034482746971563, - "y": 1.0344827469715627, - "heading": -2.8870355286032125e-18, - "angularVelocity": 2.365768387844758e-19, - "velocityX": 0.6978373382179309, - "velocityY": 0.6978373382179308, - "timestamp": 26.142514429301265 - }, - { - "x": 1.1010701331372088, - "y": 1.1010701331372084, - "heading": -2.5044778062473253e-18, - "angularVelocity": 3.870965844879766e-18, - "velocityX": 0.6737741102680164, - "velocityY": 0.6737741102680164, - "timestamp": 26.24134189261706 - }, - { - "x": 1.1652794021994897, - "y": 1.1652794021994894, - "heading": -1.9025969173015906e-18, - "angularVelocity": 6.090218909244497e-18, - "velocityX": 0.6497107879527875, - "velocityY": 0.6497107879527875, - "timestamp": 26.340169355932858 - }, - { - "x": 1.2271105522931214, - "y": 1.2271105522931212, - "heading": -9.264679815140483e-19, - "angularVelocity": 9.877101951914267e-18, - "velocityX": 0.6256474467634033, - "velocityY": 0.6256474467634033, - "timestamp": 26.438996819248654 - }, - { - "x": 1.2865635826186934, - "y": 1.2865635826186932, - "heading": -1.846677135454343e-19, - "angularVelocity": 7.506013398090202e-18, - "velocityX": 0.60158409748507, - "velocityY": 0.6015840974850699, - "timestamp": 26.53782428256445 - }, - { - "x": 1.3436384927320884, - "y": 1.3436384927320884, - "heading": 2.985033438459339e-19, - "angularVelocity": 4.889036286936694e-18, - "velocityX": 0.5775207437128721, - "velocityY": 0.577520743712872, - "timestamp": 26.636651745880247 - }, - { - "x": 1.3983352823506863, - "y": 1.3983352823506863, - "heading": 5.055652462787692e-19, - "angularVelocity": 2.095185898822513e-18, - "velocityX": 0.5534573870809412, - "velocityY": 0.553457387080941, - "timestamp": 26.735479209196043 - }, - { - "x": 1.450653951278827, - "y": 1.450653951278827, - "heading": 5.279093823877594e-19, - "angularVelocity": 2.260923516848411e-19, - "velocityX": 0.5293940284691947, - "velocityY": 0.5293940284691946, - "timestamp": 26.83430667251184 - }, - { - "x": 1.5005944993730265, - "y": 1.5005944993730265, - "heading": 5.844300351339039e-19, - "angularVelocity": 5.719124628116133e-19, - "velocityX": 0.5053306684055834, - "velocityY": 0.5053306684055834, - "timestamp": 26.933134135827636 - }, - { - "x": 1.5481569265235613, - "y": 1.5481569265235613, - "heading": 3.779197885193655e-19, - "angularVelocity": -2.0896037166931965e-18, - "velocityX": 0.4812673072317223, - "velocityY": 0.4812673072317223, - "timestamp": 27.031961599143433 - }, - { - "x": 1.5933412326438083, - "y": 1.5933412326438083, - "heading": 2.039493487749352e-20, - "angularVelocity": -3.6176667234042734e-18, - "velocityX": 0.45720394518134827, - "velocityY": 0.45720394518134827, - "timestamp": 27.13078906245923 - }, - { - "x": 1.6361474176636432, - "y": 1.6361474176636432, - "heading": -6.968095113053582e-21, - "angularVelocity": -2.7687653315843575e-19, - "velocityX": 0.43314058242141606, - "velocityY": 0.43314058242141606, - "timestamp": 27.229616525775025 - }, - { - "x": 1.676575481525138, - "y": 1.676575481525138, - "heading": 1.028814691598036e-19, - "angularVelocity": 1.111528889365859e-18, - "velocityX": 0.4090772190753271, - "velocityY": 0.4090772190753271, - "timestamp": 27.32844398909082 - }, - { - "x": 1.7146254241796324, - "y": 1.7146254241796324, - "heading": -5.672999077177864e-20, - "angularVelocity": -1.6150514296064684e-18, - "velocityX": 0.3850138552368665, - "velocityY": 0.3850138552368665, - "timestamp": 27.427271452406618 - }, - { - "x": 1.7502972455856758, - "y": 1.750297245585676, - "heading": -1.922855653141426e-19, - "angularVelocity": -1.371638460144624e-18, - "velocityX": 0.36095049097897813, - "velocityY": 0.3609504909789781, - "timestamp": 27.526098915722415 - }, - { - "x": 1.7835909457075345, - "y": 1.7835909457075347, - "heading": -3.046581816575936e-19, - "angularVelocity": -1.1370583307666246e-18, - "velocityX": 0.33688712635951407, - "velocityY": 0.336887126359514, - "timestamp": 27.62492637903821 - }, - { - "x": 1.814506524514086, - "y": 1.8145065245140861, - "heading": -1.0007879634751174e-19, - "angularVelocity": 2.0700663400412595e-18, - "velocityX": 0.3128237614251293, - "velocityY": 0.3128237614251292, - "timestamp": 27.723753842354007 - }, - { - "x": 1.8430439819779791, - "y": 1.8430439819779794, - "heading": 4.207844233499018e-19, - "angularVelocity": 5.2704300636110826e-18, - "velocityX": 0.28876039621399596, - "velocityY": 0.2887603962139959, - "timestamp": 27.822581305669804 - }, - { - "x": 1.86920331807499, - "y": 1.8692033180749903, - "heading": 1.25733988172306e-18, - "angularVelocity": 8.46480757118372e-18, - "velocityX": 0.2646970307577425, - "velocityY": 0.2646970307577425, - "timestamp": 27.9214087689856 - }, - { - "x": 1.8929845327835126, - "y": 1.8929845327835129, - "heading": 1.52547262188251e-18, - "angularVelocity": 2.713140055177162e-18, - "velocityX": 0.24063366508286838, - "velocityY": 0.24063366508286835, - "timestamp": 28.020236232301396 - }, - { - "x": 1.9143876260841575, - "y": 1.9143876260841575, - "heading": 1.5351626707181141e-18, - "angularVelocity": 9.805025681151685e-20, - "velocityX": 0.21657029921179624, - "velocityY": 0.21657029921179624, - "timestamp": 28.119063695617193 - }, - { - "x": 1.9334125979594263, - "y": 1.9334125979594263, - "heading": 1.5661943887692378e-18, - "angularVelocity": 3.139989977967844e-19, - "velocityX": 0.19250693316366735, - "velocityY": 0.19250693316366735, - "timestamp": 28.21789115893299 - }, - { - "x": 1.9500594483934486, - "y": 1.9500594483934488, - "heading": 1.9111016259033982e-18, - "angularVelocity": 3.48999383435869e-18, - "velocityX": 0.1684435669549521, - "velocityY": 0.1684435669549521, - "timestamp": 28.316718622248786 - }, - { - "x": 1.9643281773717647, - "y": 1.9643281773717651, - "heading": 1.8525436426975768e-18, - "angularVelocity": -5.925273851352638e-19, - "velocityX": 0.14438020059992485, - "velocityY": 0.14438020059992482, - "timestamp": 28.415546085564582 - }, - { - "x": 1.976218784881146, - "y": 1.9762187848811463, - "heading": 1.5181268192693785e-18, - "angularVelocity": -3.383844909664733e-18, - "velocityX": 0.12031683411103762, - "velocityY": 0.12031683411103762, - "timestamp": 28.51437354888038 - }, - { - "x": 1.9857312709094432, - "y": 1.9857312709094435, - "heading": 1.218007859467631e-18, - "angularVelocity": -3.03679700352538e-18, - "velocityX": 0.09625346749921782, - "velocityY": 0.09625346749921782, - "timestamp": 28.613201012196175 - }, - { - "x": 1.9928656354454601, - "y": 1.99286563544546, - "heading": 6.414597240846549e-19, - "angularVelocity": -5.833885501885998e-18, - "velocityX": 0.07219010077410719, - "velocityY": 0.07219010077410719, - "timestamp": 28.71202847551197 - }, - { - "x": 1.997621878478845, - "y": 1.9976218784788449, - "heading": 3.1192356934373525e-19, - "angularVelocity": -3.3344590469642965e-18, - "velocityX": 0.04812673394425598, - "velocityY": 0.04812673394425598, - "timestamp": 28.810855938827768 - }, - { - "x": 2, - "y": 2, - "heading": 3.095704809390742e-29, - "angularVelocity": -3.1562436810261706e-18, - "velocityX": 0.024063367017281703, - "velocityY": 0.0240633670172817, - "timestamp": 28.909683402143564 - }, - { - "x": 2, - "y": 2, - "heading": 9.24554742891244e-30, - "angularVelocity": -7.339590862305549e-29, - "velocityX": -1.9532349903527946e-19, - "velocityY": -1.953180778215209e-19, - "timestamp": 29.00851086545936 - }, - { - "x": 1.997621878478845, - "y": 1.997621878478845, - "heading": 2.9176550698701047e-19, - "angularVelocity": 2.952271403840556e-18, - "velocityX": -0.024063367017281706, - "velocityY": -0.0240633670172817, - "timestamp": 29.107338328775157 - }, - { - "x": 1.99286563544546, - "y": 1.99286563544546, - "heading": 5.967411588481832e-19, - "angularVelocity": 3.0859402387521106e-18, - "velocityX": -0.04812673394425599, - "velocityY": -0.04812673394425598, - "timestamp": 29.206165792090953 - }, - { - "x": 1.9857312709094432, - "y": 1.9857312709094432, - "heading": 8.990553363945518e-19, - "angularVelocity": 3.0590097309406617e-18, - "velocityX": -0.07219010077410719, - "velocityY": -0.07219010077410717, - "timestamp": 29.30499325540675 - }, - { - "x": 1.9762187848811459, - "y": 1.9762187848811459, - "heading": 1.4932940433223978e-18, - "angularVelocity": 6.01289029503833e-18, - "velocityX": -0.09625346749921782, - "velocityY": -0.0962534674992178, - "timestamp": 29.403820718722546 - }, - { - "x": 1.9643281773717647, - "y": 1.9643281773717647, - "heading": 2.301350529247808e-18, - "angularVelocity": 8.176436550349542e-18, - "velocityX": -0.12031683411103763, - "velocityY": -0.1203168341110376, - "timestamp": 29.502648182038342 - }, - { - "x": 1.9500594483934484, - "y": 1.9500594483934484, - "heading": 3.1069311821375085e-18, - "angularVelocity": 8.151384411515975e-18, - "velocityX": -0.14438020059992485, - "velocityY": -0.1443802005999248, - "timestamp": 29.60147564535414 - }, - { - "x": 1.933412597959426, - "y": 1.933412597959426, - "heading": 3.615579822445298e-18, - "angularVelocity": 5.146834914364682e-18, - "velocityX": -0.1684435669549521, - "velocityY": -0.16844356695495205, - "timestamp": 29.700303108669935 - }, - { - "x": 1.9143876260841575, - "y": 1.9143876260841575, - "heading": 4.121905172237148e-18, - "angularVelocity": 5.123326299163994e-18, - "velocityX": -0.19250693316366735, - "velocityY": -0.1925069331636673, - "timestamp": 29.79913057198573 - }, - { - "x": 1.8929845327835126, - "y": 1.8929845327835126, - "heading": 4.625994654512789e-18, - "angularVelocity": 5.100702286243361e-18, - "velocityX": -0.2165702992117962, - "velocityY": -0.21657029921179619, - "timestamp": 29.897958035301528 - }, - { - "x": 1.8692033180749899, - "y": 1.8692033180749899, - "heading": 5.127944634100554e-18, - "angularVelocity": 5.079053493036733e-18, - "velocityX": -0.24063366508286835, - "velocityY": -0.24063366508286832, - "timestamp": 29.996785498617324 - }, - { - "x": 1.8430439819779791, - "y": 1.8430439819779791, - "heading": 5.333332886477307e-18, - "angularVelocity": 2.0782507129575468e-18, - "velocityX": -0.2646970307577425, - "velocityY": -0.2646970307577424, - "timestamp": 30.09561296193312 - }, - { - "x": 1.8145065245140857, - "y": 1.8145065245140857, - "heading": 5.2582030051247135e-18, - "angularVelocity": -7.602127045758556e-19, - "velocityX": -0.2887603962139959, - "velocityY": -0.28876039621399585, - "timestamp": 30.194440425248917 - }, - { - "x": 1.7835909457075345, - "y": 1.7835909457075345, - "heading": 5.181292957950482e-18, - "angularVelocity": -7.782256292200617e-19, - "velocityX": -0.3128237614251292, - "velocityY": -0.3128237614251292, - "timestamp": 30.293267888564714 - }, - { - "x": 1.7502972455856758, - "y": 1.7502972455856758, - "heading": 5.1027534114594344e-18, - "angularVelocity": -7.947139485337703e-19, - "velocityX": -0.336887126359514, - "velocityY": -0.33688712635951396, - "timestamp": 30.39209535188051 - }, - { - "x": 1.7146254241796322, - "y": 1.7146254241796322, - "heading": 5.022755773866078e-18, - "angularVelocity": -8.094678491276822e-19, - "velocityX": -0.360950490978978, - "velocityY": -0.360950490978978, - "timestamp": 30.490922815196306 - }, - { - "x": 1.6765754815251377, - "y": 1.6765754815251377, - "heading": 4.646967759716646e-18, - "angularVelocity": -3.802465605838998e-18, - "velocityX": -0.38501385523686643, - "velocityY": -0.38501385523686643, - "timestamp": 30.589750278512103 - }, - { - "x": 1.636147417663643, - "y": 1.6361474176636428, - "heading": 4.5646761078242135e-18, - "angularVelocity": -8.326802921818095e-19, - "velocityX": -0.40907721907532707, - "velocityY": -0.40907721907532707, - "timestamp": 30.6885777418279 - }, - { - "x": 1.5933412326438081, - "y": 1.593341232643808, - "heading": 4.481619773950942e-18, - "angularVelocity": -8.404179011597725e-19, - "velocityX": -0.433140582421416, - "velocityY": -0.433140582421416, - "timestamp": 30.787405205143695 - }, - { - "x": 1.548156926523561, - "y": 1.548156926523561, - "heading": 4.69264716566721e-18, - "angularVelocity": 2.135310765625931e-18, - "velocityX": -0.4572039451813482, - "velocityY": -0.4572039451813482, - "timestamp": 30.886232668459492 - }, - { - "x": 1.5005944993730262, - "y": 1.5005944993730262, - "heading": 5.1981444287811744e-18, - "angularVelocity": 5.114946741660759e-18, - "velocityX": -0.48126730723172223, - "velocityY": -0.48126730723172223, - "timestamp": 30.98506013177529 - }, - { - "x": 1.450653951278827, - "y": 1.4506539512788268, - "heading": 5.998588984822106e-18, - "angularVelocity": 8.099413645686185e-18, - "velocityX": -0.5053306684055833, - "velocityY": -0.5053306684055833, - "timestamp": 31.083887595091085 - }, - { - "x": 1.3983352823506863, - "y": 1.398335282350686, - "heading": 6.800057219972362e-18, - "angularVelocity": 8.109771959924199e-18, - "velocityX": -0.5293940284691946, - "velocityY": -0.5293940284691946, - "timestamp": 31.18271505840688 - }, - { - "x": 1.3436384927320881, - "y": 1.3436384927320881, - "heading": 7.308809795922512e-18, - "angularVelocity": 5.147886314223004e-18, - "velocityX": -0.553457387080941, - "velocityY": -0.553457387080941, - "timestamp": 31.281542521722677 - }, - { - "x": 1.2865635826186932, - "y": 1.286563582618693, - "heading": 7.52592489681066e-18, - "angularVelocity": 2.1969102927676096e-18, - "velocityX": -0.5775207437128719, - "velocityY": -0.577520743712872, - "timestamp": 31.380369985038474 - }, - { - "x": 1.2271105522931212, - "y": 1.2271105522931212, - "heading": 8.042018199779756e-18, - "angularVelocity": 5.222164534781162e-18, - "velocityX": -0.6015840974850699, - "velocityY": -0.6015840974850699, - "timestamp": 31.47919744835427 - }, - { - "x": 1.1652794021994897, - "y": 1.1652794021994894, - "heading": 8.270479544084847e-18, - "angularVelocity": 2.3117189802704113e-18, - "velocityX": -0.6256474467634033, - "velocityY": -0.6256474467634034, - "timestamp": 31.578024911670067 - }, - { - "x": 1.1010701331372086, - "y": 1.1010701331372084, - "heading": 8.510243075879087e-18, - "angularVelocity": 2.4260818071067678e-18, - "velocityX": -0.6497107879527876, - "velocityY": -0.6497107879527876, - "timestamp": 31.676852374985863 - }, - { - "x": 1.0344827469715632, - "y": 1.034482746971563, - "heading": 8.487156635946621e-18, - "angularVelocity": -2.3360355366177763e-19, - "velocityX": -0.6737741102680165, - "velocityY": -0.6737741102680165, - "timestamp": 31.77567983830166 - }, - { - "x": 0.9655172530284372, - "y": 0.965517253028437, - "heading": 8.5480561716338e-18, - "angularVelocity": 6.162207686069967e-19, - "velocityX": -0.6978373382179308, - "velocityY": -0.6978373382179308, - "timestamp": 31.874507301617456 - }, - { - "x": 0.8989298668627916, - "y": 0.8989298668627914, - "heading": 8.406703773978591e-18, - "angularVelocity": -1.4302945801324534e-18, - "velocityX": -0.6737741102680165, - "velocityY": -0.6737741102680164, - "timestamp": 31.973334764933252 - }, - { - "x": 0.8347205978005106, - "y": 0.8347205978005104, - "heading": 8.29516939176197e-18, - "angularVelocity": -1.1285766777045508e-18, - "velocityX": -0.6497107879527876, - "velocityY": -0.6497107879527875, - "timestamp": 32.07216222824905 - }, - { - "x": 0.7728894477068788, - "y": 0.7728894477068787, - "heading": 8.497700879239251e-18, - "angularVelocity": 2.0493442633946093e-18, - "velocityX": -0.6256474467634034, - "velocityY": -0.6256474467634033, - "timestamp": 32.17098969156484 - }, - { - "x": 0.7134364173813069, - "y": 0.7134364173813067, - "heading": 8.715363483429085e-18, - "angularVelocity": 2.2024506538239383e-18, - "velocityX": -0.6015840974850699, - "velocityY": -0.6015840974850699, - "timestamp": 32.269817154880634 - }, - { - "x": 0.6563615072679118, - "y": 0.6563615072679116, - "heading": 8.945709750816389e-18, - "angularVelocity": 2.330792062318582e-18, - "velocityX": -0.577520743712872, - "velocityY": -0.577520743712872, - "timestamp": 32.36864461819643 - }, - { - "x": 0.6016647176493137, - "y": 0.6016647176493136, - "heading": 9.481711280544972e-18, - "angularVelocity": 5.4236090257494665e-18, - "velocityX": -0.553457387080941, - "velocityY": -0.553457387080941, - "timestamp": 32.46747208151222 - }, - { - "x": 0.549346048721173, - "y": 0.5493460487211729, - "heading": 9.733232170684809e-18, - "angularVelocity": 2.5450506077548204e-18, - "velocityX": -0.5293940284691947, - "velocityY": -0.5293940284691946, - "timestamp": 32.56629954482801 - }, - { - "x": 0.4994055006269736, - "y": 0.4994055006269735, - "heading": 9.699481252660535e-18, - "angularVelocity": -3.415135021970597e-19, - "velocityX": -0.5053306684055833, - "velocityY": -0.5053306684055833, - "timestamp": 32.665127008143806 - }, - { - "x": 0.45184307347643865, - "y": 0.45184307347643854, - "heading": 9.37985428465038e-18, - "angularVelocity": -3.234191651445345e-18, - "velocityX": -0.4812673072317223, - "velocityY": -0.48126730723172223, - "timestamp": 32.7639544714596 - }, - { - "x": 0.40665876735619183, - "y": 0.4066587673561917, - "heading": 9.362931683815324e-18, - "angularVelocity": -1.712336792194834e-19, - "velocityX": -0.45720394518134827, - "velocityY": -0.4572039451813482, - "timestamp": 32.86278193477539 - }, - { - "x": 0.3638525823363568, - "y": 0.36385258233635676, - "heading": 9.059269008897951e-18, - "angularVelocity": -3.072654581500084e-18, - "velocityX": -0.43314058242141606, - "velocityY": -0.433140582421416, - "timestamp": 32.961609398091184 - }, - { - "x": 0.32342451847486214, - "y": 0.32342451847486203, - "heading": 8.763075814779251e-18, - "angularVelocity": -2.9970734945597385e-18, - "velocityX": -0.40907721907532707, - "velocityY": -0.409077219075327, - "timestamp": 33.06043686140698 - }, - { - "x": 0.28537457582036757, - "y": 0.28537457582036746, - "heading": 8.179554689955981e-18, - "angularVelocity": -5.9044427782956105e-18, - "velocityX": -0.3850138552368665, - "velocityY": -0.38501385523686643, - "timestamp": 33.15926432472277 - }, - { - "x": 0.2497027544143241, - "y": 0.24970275441432402, - "heading": 7.603006050284837e-18, - "angularVelocity": -5.8338906138842846e-18, - "velocityX": -0.3609504909789781, - "velocityY": -0.360950490978978, - "timestamp": 33.25809178803856 - }, - { - "x": 0.21640905429246532, - "y": 0.21640905429246526, - "heading": 7.033233234769523e-18, - "angularVelocity": -5.765328461225564e-18, - "velocityX": -0.336887126359514, - "velocityY": -0.33688712635951396, - "timestamp": 33.356919251354356 - }, - { - "x": 0.18549347548591408, - "y": 0.18549347548591402, - "heading": 6.175535578468071e-18, - "angularVelocity": -8.67873752787532e-18, - "velocityX": -0.31282376142512924, - "velocityY": -0.3128237614251292, - "timestamp": 33.45574671467015 - }, - { - "x": 0.15695601802202072, - "y": 0.15695601802202067, - "heading": 5.3242912654674056e-18, - "angularVelocity": -8.613438444393486e-18, - "velocityX": -0.2887603962139959, - "velocityY": -0.2887603962139959, - "timestamp": 33.55457417798594 - }, - { - "x": 0.13079668192500982, - "y": 0.13079668192500976, - "heading": 4.479366707374499e-18, - "angularVelocity": -8.549491094513284e-18, - "velocityX": -0.26469703075774254, - "velocityY": -0.2646970307577425, - "timestamp": 33.653401641301734 - }, - { - "x": 0.10701546721648711, - "y": 0.10701546721648708, - "heading": 3.64064272114218e-18, - "angularVelocity": -8.486749715566848e-18, - "velocityX": -0.2406336650828684, - "velocityY": -0.24063366508286835, - "timestamp": 33.75222910461753 - }, - { - "x": 0.08561237391584237, - "y": 0.08561237391584235, - "heading": 3.1105036447238866e-18, - "angularVelocity": -5.364288719895572e-18, - "velocityX": -0.2165702992117963, - "velocityY": -0.21657029921179624, - "timestamp": 33.85105656793332 - }, - { - "x": 0.06658740204057367, - "y": 0.06658740204057366, - "heading": 2.5863618381776226e-18, - "angularVelocity": -5.303604412403694e-18, - "velocityX": -0.19250693316366738, - "velocityY": -0.19250693316366735, - "timestamp": 33.94988403124911 - }, - { - "x": 0.04994055160655133, - "y": 0.04994055160655132, - "heading": 2.06812977982204e-18, - "angularVelocity": -5.2438058439965976e-18, - "velocityX": -0.1684435669549521, - "velocityY": -0.16844356695495208, - "timestamp": 34.048711494564905 - }, - { - "x": 0.03567182262823501, - "y": 0.035671822628235005, - "heading": 1.5557277971478494e-18, - "angularVelocity": -5.184813384293766e-18, - "velocityX": -0.14438020059992485, - "velocityY": -0.1443802005999248, - "timestamp": 34.1475389578807 - }, - { - "x": 0.023781215118853744, - "y": 0.02378121511885374, - "heading": 1.2333207349325143e-18, - "angularVelocity": -3.2623223165160492e-18, - "velocityX": -0.12031683411103762, - "velocityY": -0.12031683411103758, - "timestamp": 34.24636642119649 - }, - { - "x": 0.014268729090556646, - "y": 0.014268729090556643, - "heading": 9.166038261970496e-19, - "angularVelocity": -3.2047456798497867e-18, - "velocityX": -0.09625346749921782, - "velocityY": -0.09625346749921779, - "timestamp": 34.345193884512284 - }, - { - "x": 0.0071343645545399615, - "y": 0.00713436455453996, - "heading": 6.055153925645065e-19, - "angularVelocity": -3.1477932139121737e-18, - "velocityX": -0.07219010077410719, - "velocityY": -0.07219010077410716, - "timestamp": 34.44402134782808 - }, - { - "x": 0.0023781215211549364, - "y": 0.0023781215211549356, - "heading": 2.99998430529326e-19, - "angularVelocity": -3.0914174770892124e-18, - "velocityX": -0.04812673394425598, - "velocityY": -0.04812673394425597, - "timestamp": 34.54284881114387 - }, - { - "x": -2.5177140124632005e-18, - "y": -2.5177139656237954e-18, - "heading": 1.1418576549630119e-29, - "angularVelocity": -3.0355774220989457e-18, - "velocityX": -0.024063367017281703, - "velocityY": -0.024063367017281696, - "timestamp": 34.64167627445966 - }, - { - "x": -1.1177094777187861e-18, - "y": -1.1177094535843756e-18, - "heading": 5.214297458751026e-30, - "angularVelocity": -1.0017271530534466e-29, - "velocityX": 2.856704072481815e-18, - "velocityY": 2.856704086919547e-18, - "timestamp": 34.740503737775455 - } - ] -} \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 2e6724d6..dfb8a171 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -109,8 +109,8 @@ public static class Vision { public static final String FRONT_CAMERA_NAME = "limelight-front"; public static final String REAR_CAMERA_NAME = "limelight-rear"; - public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.066606, 0, 0.626963, new Rotation3d(0, Math.toRadians(-16.393), 0)); - public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.005553, 0, 0.540533, new Rotation3d(0, Math.toRadians(10), Math.PI)); + public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.086018, 0, 0.627079, new Rotation3d(0, Math.toRadians(-40.843), 0)); + public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(0, Math.toRadians(10), Math.PI)); public static final boolean TAKE_SNAPSHOTS = true; public static final double SNAPSHOT_PERIOD_SECS = 1; diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index ad9f694b..cd90221b 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -250,6 +250,8 @@ private void configureAutoRoutines() { ) ) ); + + autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.COUNTER_CLOCKWISE))); } autos.addDefault(new AutoCommand("Dwayne :skull:")); autos.add(new AutoCommand("SubwooferShot", new ShootSequence(shooter, indexer))); @@ -261,6 +263,7 @@ private void configureAutoRoutines() { autos.add(new CenterLanePSubSprint(drivetrain, shooter, indexer)); autos.add(new CenterLanePCBADSprint(drivetrain, shooter, indexer)); autos.add(new CenterLanePCBAFSprint(drivetrain, shooter, indexer)); +// autos.add(new CenterLanePCBAFE(drivetrain, shooter, indexer)); autos.add(new CenterLanePCBA(drivetrain, shooter, indexer)); autos.add(new CenterLanePBDA(drivetrain, shooter, indexer)); autos.add(new CenterLanePSubCSubBSubASubFSub(drivetrain, shooter, indexer)); @@ -270,8 +273,6 @@ private void configureAutoRoutines() { autos.add(new DriveSinglePath("SourceLaneSprint", drivetrain)); autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer)); autos.add(new SourceLanePHG(drivetrain, shooter, indexer)); - autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.COUNTER_CLOCKWISE))); - } diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java index 5664c357..3012a89b 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADESprint.java @@ -32,8 +32,7 @@ public AmpLanePADESprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer createSegmentSequence(drivetrain, shooter, indexer, 2), // new WaitCommand(5), // drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, shooter, indexer, 3), - getPath(4).getCommand(drivetrain) + createSegmentSequence(drivetrain, shooter, indexer, 3) // drivetrain.commandCopyVisionPose() ) ), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFE.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFE.java new file mode 100644 index 00000000..1e3b3b54 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFE.java @@ -0,0 +1,42 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.commands.shooter.ShootSequence; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class CenterLanePCBAFE extends AutoCommand { + + public CenterLanePCBAFE(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("!CenterLanePCBAFE"); + addPath( + PathHelper.fromChoreoPath("CenterLanePCBAFE.1", false, true), + PathHelper.fromChoreoPath("CenterLanePCBAFE.2"), + PathHelper.fromChoreoPath("CenterLanePCBAFE.3"), + PathHelper.fromChoreoPath("CenterLanePCBAFE.4"), + PathHelper.fromChoreoPath("CenterLanePCBAFE.5") + + ); + + addCommands( + ShootSequence.forAutoSubwoofer(shooter, indexer), + Commands.parallel( + new AutoShootPrepare(drivetrain, shooter), + Commands.sequence( + drivetrain.commandCopyVisionPose(), + createSegmentSequence(drivetrain, shooter, indexer, 0, false, false, false), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, false, true), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, false, true), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, false, true), + createSegmentSequence(drivetrain, shooter, indexer, 4, false, false, true) + ) + ) + ); + + addRequirements(drivetrain, shooter, indexer); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java index 9a1bc077..a528a978 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java @@ -14,7 +14,7 @@ public class CenterLanePCBAFSprint extends AutoCommand { public CenterLanePCBAFSprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { super("!CenterLanePCBAFSprint"); addPath( - PathHelper.fromChoreoPath("CenterLanePCBAFSprint.1", true, true), + PathHelper.fromChoreoPath("CenterLanePCBAFSprint.1", false, true), PathHelper.fromChoreoPath("CenterLanePCBAFSprint.2"), PathHelper.fromChoreoPath("CenterLanePCBAFSprint.3"), PathHelper.fromChoreoPath("CenterLanePCBAFSprint.4"), @@ -27,6 +27,7 @@ public CenterLanePCBAFSprint(Drivetrain drivetrain, Shooter shooter, Indexer ind Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( + drivetrain.commandCopyVisionPose(), createSegmentSequence(drivetrain, shooter, indexer, 0, false, false, false), createSegmentSequence(drivetrain, shooter, indexer, 1, false, false, true), createSegmentSequence(drivetrain, shooter, indexer, 2, false, false, true), diff --git a/src/main/java/org/team1540/robot2024/commands/autos/PathVisualising.java b/src/main/java/org/team1540/robot2024/commands/autos/PathVisualising.java new file mode 100644 index 00000000..5c074e6b --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/PathVisualising.java @@ -0,0 +1,21 @@ +package org.team1540.robot2024.commands.autos; + +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class PathVisualising extends AutoCommand { + + public PathVisualising(Drivetrain drivetrain){ + super("PathVisualising"); + + addPath( + PathHelper.fromChoreoPath("AmpLanePADESprint", true, true) + ); + addCommands( + getPath(0).getCommand(drivetrain) + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java index ed971ee4..5bf93935 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.commands.shooter.ShootSequence; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; @@ -18,6 +19,7 @@ public SourceLanePHG(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ ); addCommands( + ShootSequence.forAutoSubwoofer(shooter, indexer), Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java index dbb592bf..ac32b2ba 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java @@ -51,7 +51,7 @@ public static Command forAutoSubwoofer(Shooter shooter, Indexer indexer) { return Commands.race( new PrepareShooterCommand(shooter, ()-> shooter.lerp.get(Units.feetToMeters(3))), Commands.sequence( - Commands.waitSeconds(1.5), + Commands.waitSeconds(1), Commands.runOnce(shooter::zeroPivotToCancoder), IntakeAndFeed.withDefaults(indexer).withTimeout(0.5) ) diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index 1503c9f0..f82343b1 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -99,7 +99,7 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Commands.parallel( new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4).onlyIf(()->shouldRealignYaw), Commands.sequence( - Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10).onlyIf(()->shouldRealignYaw), +// Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10).onlyIf(()->shouldRealignYaw), new ParallelDeadlineGroup( Commands.sequence( Commands.waitUntil(()->!indexer.isNoteStaged()), From 1a9a3c637b895d9e92d1bc433e5d10334a394f8e Mon Sep 17 00:00:00 2001 From: Zach R Date: Fri, 22 Mar 2024 22:58:02 -0700 Subject: [PATCH 55/75] event: eod day 1 --- .../choreo/CenterLanePCBADSprint.1.traj | 18 -- .../choreo/CenterLanePCBADSprint.2.traj | 61 +++--- .../choreo/CenterLanePCBADSprint.3.traj | 99 ++++----- .../choreo/CenterLanePCBADSprint.4.traj | 194 ++++++++---------- .../choreo/CenterLanePCBADSprint.5.traj | 147 +++++++------ .../org/team1540/robot2024/Constants.java | 6 +- .../java/org/team1540/robot2024/Robot.java | 2 + .../team1540/robot2024/RobotContainer.java | 10 +- .../DriveWithSpeakerTargetingCommand.java | 67 ------ .../drivetrain/DriveWithTargetingCommand.java | 15 +- .../commands/drivetrain/HoldRotation.java | 57 ----- .../drivetrain/SwerveDriveCommand.java | 11 +- .../indexer/ContinuousIntakeCommand.java | 33 +++ .../commands/indexer/IntakeCommand.java | 2 +- .../OverStageShootPrepareWithTargeting.java | 3 +- .../commands/shooter/ShootSequence.java | 10 - .../subsystems/drive/Drivetrain.java | 4 +- .../robot2024/subsystems/indexer/Indexer.java | 4 + .../subsystems/indexer/IndexerIO.java | 2 + .../subsystems/indexer/IndexerIOSparkMax.java | 5 + .../subsystems/indexer/IndexerIOTalonFX.java | 5 + .../robot2024/subsystems/led/Leds.java | 2 +- 22 files changed, 313 insertions(+), 444 deletions(-) delete mode 100644 src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerTargetingCommand.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/drivetrain/HoldRotation.java create mode 100644 src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.1.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.1.traj index 7d68d234..96a07c42 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.1.traj @@ -305,24 +305,6 @@ "velocityX": -0.3726737311878495, "velocityY": 0.4676017284248481, "timestamp": 2.8194091225871976 - }, - { - "x": 1.8129411935806854, - "y": 5.542999267578127, - "heading": 2.3632850028431835e-17, - "angularVelocity": 0.14297150447865262, - "velocityX": -0.1946760191950624, - "velocityY": 0.24372755600475027, - "timestamp": 2.905157968293726 - }, - { - "x": 1.812941193580624, - "y": 5.542999267578126, - "heading": -4.920156603511903e-17, - "angularVelocity": -5.733397110540358e-17, - "velocityX": -7.214388151371406e-13, - "velocityY": -9.03683027618772e-16, - "timestamp": 2.9909068140002546 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.2.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.2.traj index c49e7767..4ff3aa49 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.2.traj @@ -1,5 +1,23 @@ { "samples": [ + { + "x": 1.8296344375134035, + "y": 5.522099910983847, + "heading": -0.012259641477970167, + "angularVelocity": 0.2754641349951199, + "velocityX": -0.3726737311878495, + "velocityY": 0.4676017284248481, + "timestamp": 0 + }, + { + "x": 1.8129411935806854, + "y": 5.542999267578127, + "heading": 2.3632850028431835e-17, + "angularVelocity": 0.14297150447865262, + "velocityX": -0.1946760191950624, + "velocityY": 0.24372755600475027, + "timestamp": 0.08574884570652852 + }, { "x": 1.812941193580624, "y": 5.542999267578126, @@ -7,7 +25,7 @@ "angularVelocity": -5.733397110540358e-17, "velocityX": -7.214388151371406e-13, "velocityY": -9.03683027618772e-16, - "timestamp": 0 + "timestamp": 0.17149769141305704 }, { "x": 1.8513325137310372, @@ -16,7 +34,7 @@ "angularVelocity": -0.00016991086246254008, "velocityX": 0.3871147468977978, "velocityY": 0.0006118348336243464, - "timestamp": 0.09917297250510426 + "timestamp": 0.2706706639181613 }, { "x": 1.9275402570059652, @@ -25,7 +43,7 @@ "angularVelocity": 0.00001113074602288469, "velocityX": 0.7684325814778618, "velocityY": 0.0012145076227106227, - "timestamp": 0.19834594501020852 + "timestamp": 0.36984363642326556 }, { "x": 2.0338963277323163, @@ -34,7 +52,7 @@ "angularVelocity": 0.00025124822289482134, "velocityX": 1.0724299982122594, "velocityY": 0.0016949754373461064, - "timestamp": 0.2975189175153128 + "timestamp": 0.4690166089283698 }, { "x": 2.163383514243291, @@ -43,7 +61,7 @@ "angularVelocity": 0.0002468830606297171, "velocityX": 1.305670115961409, "velocityY": 0.00206361144256778, - "timestamp": 0.39669189002041705 + "timestamp": 0.5681895814334741 }, { "x": 2.3079360710490797, @@ -52,7 +70,7 @@ "angularVelocity": -0.000004807060894986283, "velocityX": 1.457580156714077, "velocityY": 0.0023037062351123753, - "timestamp": 0.4958648625255213 + "timestamp": 0.6673625539385784 }, { "x": 2.437398077457075, @@ -61,7 +79,7 @@ "angularVelocity": -0.00011123604437218979, "velocityX": 1.3054162151016944, "velocityY": 0.002063210056106694, - "timestamp": 0.5950378350306256 + "timestamp": 0.7665355264436826 }, { "x": 2.5437229473484053, @@ -70,34 +88,7 @@ "angularVelocity": -0.00006795896080977589, "velocityX": 1.0721153879486738, "velocityY": 0.0016944781198031775, - "timestamp": 0.6942108075357298 - }, - { - "x": 2.619913870706316, - "y": 5.544274687859312, - "heading": 0.000009213432317016853, - "angularVelocity": -0.00006244644733859944, - "velocityX": 0.7682629796539725, - "velocityY": 0.0012142394582550931, - "timestamp": 0.7933837800408341 - }, - { - "x": 2.6583051681519314, - "y": 5.5443353652954785, - "heading": 6.370510606485668e-14, - "angularVelocity": -0.00009290265301587805, - "velocityX": 0.38711451795641516, - "velocityY": 0.0006118344003786568, - "timestamp": 0.8925567525459384 - }, - { - "x": 2.658305168152, - "y": 5.544335365295488, - "heading": 6.625668653050138e-14, - "angularVelocity": 2.6906565131836242e-14, - "velocityX": 7.155954565396242e-13, - "velocityY": 8.671180221391495e-14, - "timestamp": 0.9917297250510426 + "timestamp": 0.8657084989487869 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.3.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.3.traj index c0121fc6..16980d6c 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.3.traj @@ -1,5 +1,32 @@ { "samples": [ + { + "x": 2.5437229473484053, + "y": 5.544154268122903, + "heading": 0.000015406432122086, + "angularVelocity": -0.00006795896080977589, + "velocityX": 1.0721153879486738, + "velocityY": 0.0016944781198031775, + "timestamp": 0 + }, + { + "x": 2.619913870706316, + "y": 5.544274687859312, + "heading": 0.000009213432317016853, + "angularVelocity": -0.00006244644733859944, + "velocityX": 0.7682629796539725, + "velocityY": 0.0012142394582550931, + "timestamp": 0.09917297250510426 + }, + { + "x": 2.6583051681519314, + "y": 5.5443353652954785, + "heading": 6.370510606485668e-14, + "angularVelocity": -0.00009290265301587805, + "velocityX": 0.38711451795641516, + "velocityY": 0.0006118344003786568, + "timestamp": 0.19834594501020852 + }, { "x": 2.658305168152, "y": 5.544335365295488, @@ -7,7 +34,7 @@ "angularVelocity": 2.6906565131836242e-14, "velocityX": 7.155954565396242e-13, "velocityY": 8.671180221391495e-14, - "timestamp": 0 + "timestamp": 0.2975189175153128 }, { "x": 2.6398879819889713, @@ -16,7 +43,7 @@ "angularVelocity": 0.0943787022324955, "velocityX": -0.20721108536177554, "velocityY": 0.26663254150593735, - "timestamp": 0.0888812783882651 + "timestamp": 0.3864001959035779 }, { "x": 2.6055876283476542, @@ -25,7 +52,7 @@ "angularVelocity": 0.19023499118162804, "velocityX": -0.3859120195327329, "velocityY": 0.5085239675934979, - "timestamp": 0.1777625567765302 + "timestamp": 0.475281474291843 }, { "x": 2.5578825329127857, @@ -34,7 +61,7 @@ "angularVelocity": 0.27862992225075944, "velocityX": -0.5367282773164671, "velocityY": 0.7284533903912082, - "timestamp": 0.2666438351647953 + "timestamp": 0.5641627526801081 }, { "x": 2.4993543830957403, @@ -43,7 +70,7 @@ "angularVelocity": 0.36099011222914734, "velocityX": -0.6584980648176972, "velocityY": 0.9291098855468343, - "timestamp": 0.3555251135530604 + "timestamp": 0.6530440310683732 }, { "x": 2.433098392034208, @@ -52,7 +79,7 @@ "angularVelocity": 0.43875592188117873, "velocityX": -0.7454437229429913, "velocityY": 1.1115906563809108, - "timestamp": 0.4444063919413255 + "timestamp": 0.7419253094566383 }, { "x": 2.3637465533640976, @@ -61,7 +88,7 @@ "angularVelocity": 0.5135279461940396, "velocityX": -0.7802749907275444, "velocityY": 1.2748541102325668, - "timestamp": 0.5332876703295906 + "timestamp": 0.8308065878449034 }, { "x": 2.300988842467506, @@ -70,7 +97,7 @@ "angularVelocity": 0.5735817090404455, "velocityX": -0.7060847012414084, "velocityY": 1.385745889998316, - "timestamp": 0.6221689487178557 + "timestamp": 0.9196878662331684 }, { "x": 2.2515752134889366, @@ -79,7 +106,7 @@ "angularVelocity": 0.582478263071738, "velocityX": -0.5559509254902292, "velocityY": 1.3658139481175915, - "timestamp": 0.7110502271061208 + "timestamp": 1.0085691446214335 }, { "x": 2.217045589242532, @@ -88,7 +115,7 @@ "angularVelocity": 0.5655475357136119, "velocityX": -0.3884915346931451, "velocityY": 1.2850808686903799, - "timestamp": 0.7999315054943859 + "timestamp": 1.0974504230096986 }, { "x": 2.19907919431313, @@ -97,7 +124,7 @@ "angularVelocity": 0.5348199283510537, "velocityX": -0.20213924974232386, "velocityY": 1.1677658147606698, - "timestamp": 0.888812783882651 + "timestamp": 1.1863317013979637 }, { "x": 2.1993959276283994, @@ -106,7 +133,7 @@ "angularVelocity": 0.4976218206191788, "velocityX": 0.0035635549001480525, "velocityY": 1.021701577951703, - "timestamp": 0.977694062270916 + "timestamp": 1.2752129797862288 }, { "x": 2.219770279567901, @@ -115,7 +142,7 @@ "angularVelocity": 0.4495548605556116, "velocityX": 0.2292310856552352, "velocityY": 0.8500699075051399, - "timestamp": 1.0665753406591811 + "timestamp": 1.364094258174494 }, { "x": 2.2626490848637184, @@ -124,7 +151,7 @@ "angularVelocity": 0.36945922212429394, "velocityX": 0.4824278641490618, "velocityY": 0.648489016846146, - "timestamp": 1.1554566190474462 + "timestamp": 1.452975536562759 }, { "x": 2.329372405994089, @@ -133,7 +160,7 @@ "angularVelocity": 0.31710501792120865, "velocityX": 0.7507016363871187, "velocityY": 0.4289730049288785, - "timestamp": 1.2443378974357113 + "timestamp": 1.5418568149510241 }, { "x": 2.399251093648798, @@ -142,7 +169,7 @@ "angularVelocity": 0.21745079806450165, "velocityX": 0.902277239279105, "velocityY": 0.5155877648973247, - "timestamp": 1.321784922168539 + "timestamp": 1.6193038396838517 }, { "x": 2.4725481079431484, @@ -151,7 +178,7 @@ "angularVelocity": -0.28366617987293774, "velocityX": 0.9464148501358945, "velocityY": 0.5408092945202858, - "timestamp": 1.3992319469013665 + "timestamp": 1.6967508644166793 }, { "x": 2.537702213604298, @@ -160,43 +187,7 @@ "angularVelocity": -0.07424082257084566, "velocityX": 0.8412731915467285, "velocityY": 0.48072825693890747, - "timestamp": 1.4766789716341941 - }, - { - "x": 2.5904059035391898, - "y": 6.846537042294488, - "heading": 0.5036538467311155, - "angularVelocity": 0.025583194098508056, - "velocityX": 0.680512777970975, - "velocityY": 0.38886502613926416, - "timestamp": 1.5541259963670218 - }, - { - "x": 2.6280877308244723, - "y": 6.868069547224279, - "heading": 0.508503710225543, - "angularVelocity": 0.06262168896947905, - "velocityX": 0.4865497083575037, - "velocityY": 0.27802882066934376, - "timestamp": 1.6315730210998494 - }, - { - "x": 2.648344516781087, - "y": 6.8796448707953655, - "heading": 0.5125504196001363, - "angularVelocity": 0.052251321320124576, - "velocityX": 0.26155667100350394, - "velocityY": 0.14946117855453953, - "timestamp": 1.709020045832677 - }, - { - "x": 2.6483445167657806, - "y": 6.879644870779064, - "heading": 0.5125504196000689, - "angularVelocity": 1.8669291195039983e-14, - "velocityX": -4.749030905139852e-11, - "velocityY": 6.098431731550748e-11, - "timestamp": 1.7864670705655046 + "timestamp": 1.774197889149507 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.4.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.4.traj index 29278195..81537aca 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.4.traj @@ -1,5 +1,41 @@ { "samples": [ + { + "x": 2.537702213604298, + "y": 6.816420603018375, + "heading": 0.5016725044650684, + "angularVelocity": -0.07424082257084566, + "velocityX": 0.8412731915467285, + "velocityY": 0.48072825693890747, + "timestamp": 0 + }, + { + "x": 2.5904059035391898, + "y": 6.846537042294488, + "heading": 0.5036538467311155, + "angularVelocity": 0.025583194098508056, + "velocityX": 0.680512777970975, + "velocityY": 0.38886502613926416, + "timestamp": 0.07744702473282761 + }, + { + "x": 2.6280877308244723, + "y": 6.868069547224279, + "heading": 0.508503710225543, + "angularVelocity": 0.06262168896947905, + "velocityX": 0.4865497083575037, + "velocityY": 0.27802882066934376, + "timestamp": 0.15489404946565521 + }, + { + "x": 2.648344516781087, + "y": 6.8796448707953655, + "heading": 0.5125504196001363, + "angularVelocity": 0.052251321320124576, + "velocityX": 0.26155667100350394, + "velocityY": 0.14946117855453953, + "timestamp": 0.23234107419848282 + }, { "x": 2.6483445167657806, "y": 6.879644870779064, @@ -7,7 +43,7 @@ "angularVelocity": 1.8669291195039983e-14, "velocityX": -4.749030905139852e-11, "velocityY": 6.098431731550748e-11, - "timestamp": 0 + "timestamp": 0.30978809893131043 }, { "x": 2.6747339306487334, @@ -16,7 +52,7 @@ "angularVelocity": -0.03296702980330207, "velocityX": 0.3189128531205414, "velocityY": 0.046360649216292196, - "timestamp": 0.08274804111760403 + "timestamp": 0.39253614004891446 }, { "x": 2.7264961186821273, @@ -25,7 +61,7 @@ "angularVelocity": -0.07142543798202369, "velocityX": 0.6255397388782451, "velocityY": 0.09076159355636307, - "timestamp": 0.16549608223520806 + "timestamp": 0.4752841811665185 }, { "x": 2.8018050306182753, @@ -34,7 +70,7 @@ "angularVelocity": -0.10834773625231511, "velocityX": 0.9100990297657242, "velocityY": 0.13177893803804336, - "timestamp": 0.2482441233528121 + "timestamp": 0.5580322222841225 }, { "x": 2.899101313212433, @@ -43,7 +79,7 @@ "angularVelocity": -0.14299973603485935, "velocityX": 1.1758137265856894, "velocityY": 0.16987621248329202, - "timestamp": 0.33099216447041613 + "timestamp": 0.6407802634017266 }, { "x": 3.017081432389269, @@ -52,7 +88,7 @@ "angularVelocity": -0.1754594403754914, "velocityX": 1.42577537284728, "velocityY": 0.20549148936151598, - "timestamp": 0.41374020558802016 + "timestamp": 0.7235283045193306 }, { "x": 3.1546294358748312, @@ -61,7 +97,7 @@ "angularVelocity": -0.20581401948312003, "velocityX": 1.6622508717768, "velocityY": 0.23893590563959213, - "timestamp": 0.4964882467056242 + "timestamp": 0.8062763456369346 }, { "x": 3.3107722570719647, @@ -70,7 +106,7 @@ "angularVelocity": -0.2341283632545328, "velocityX": 1.8869669793779915, "velocityY": 0.2704332848357982, - "timestamp": 0.5792362878232282 + "timestamp": 0.8890243867545387 }, { "x": 3.484600968601402, @@ -79,7 +115,7 @@ "angularVelocity": -0.2603795999490034, "velocityX": 2.1006988102882644, "velocityY": 0.3000610372485155, - "timestamp": 0.6619843289408323 + "timestamp": 0.9717724278721427 }, { "x": 3.675234170530874, @@ -88,7 +124,7 @@ "angularVelocity": -0.28448128265230255, "velocityX": 2.3037790303523367, "velocityY": 0.32781977728352046, - "timestamp": 0.7447323700584363 + "timestamp": 1.0545204689897467 }, { "x": 3.8817881153060045, @@ -97,7 +133,7 @@ "angularVelocity": -0.3062656862983269, "velocityX": 2.4961792688429623, "velocityY": 0.3536391340419632, - "timestamp": 0.8274804111760403 + "timestamp": 1.1372685101073507 }, { "x": 4.1033458782194, @@ -106,7 +142,7 @@ "angularVelocity": -0.32545025414364576, "velocityX": 2.677498583906162, "velocityY": 0.3773656090873216, - "timestamp": 0.9102284522936444 + "timestamp": 1.2200165512249548 }, { "x": 4.338922324182521, @@ -115,7 +151,7 @@ "angularVelocity": -0.34157841544560996, "velocityX": 2.846912661392294, "velocityY": 0.39873465138360303, - "timestamp": 0.9929764934112484 + "timestamp": 1.3027645923425588 }, { "x": 4.587417768628414, @@ -124,7 +160,7 @@ "angularVelocity": -0.3538974315929522, "velocityX": 3.00303718480442, "velocityY": 0.4173060068834074, - "timestamp": 1.0757245345288524 + "timestamp": 1.3855126334601628 }, { "x": 4.847543704393879, @@ -133,7 +169,7 @@ "angularVelocity": -0.36103140944690143, "velocityX": 3.1435902560613274, "velocityY": 0.4322997406709641, - "timestamp": 1.1584725756464564 + "timestamp": 1.4682606745777669 }, { "x": 5.117670126803491, @@ -142,7 +178,7 @@ "angularVelocity": -0.35992408723308145, "velocityX": 3.264444919314729, "velocityY": 0.4420723558891399, - "timestamp": 1.2412206167640605 + "timestamp": 1.551008715695371 }, { "x": 5.395335267694769, @@ -151,7 +187,7 @@ "angularVelocity": -0.33958275322536735, "velocityX": 3.3555494141142153, "velocityY": 0.44127506759904395, - "timestamp": 1.3239686578816645 + "timestamp": 1.633756756812975 }, { "x": 5.667910261992729, @@ -160,7 +196,7 @@ "angularVelocity": -0.27330048016738406, "velocityX": 3.294035612402776, "velocityY": 0.3969564071048189, - "timestamp": 1.4067166989992685 + "timestamp": 1.716504797930579 }, { "x": 5.931901746285159, @@ -169,7 +205,7 @@ "angularVelocity": -0.23112783503586568, "velocityX": 3.1903049392702143, "velocityY": 0.3700769541063461, - "timestamp": 1.4894647401168726 + "timestamp": 1.799252839048183 }, { "x": 6.1801220482371395, @@ -178,7 +214,7 @@ "angularVelocity": -0.38015707459464126, "velocityX": 2.999712121271843, "velocityY": 0.33107935620263895, - "timestamp": 1.5722127812344766 + "timestamp": 1.882000880165787 }, { "x": 6.415697256681246, @@ -187,7 +223,7 @@ "angularVelocity": -0.3302398748429845, "velocityX": 2.84689770612573, "velocityY": 0.3017535622589267, - "timestamp": 1.6549608223520806 + "timestamp": 1.964748921283391 }, { "x": 6.637987945996846, @@ -196,7 +232,7 @@ "angularVelocity": -0.2930892166194916, "velocityX": 2.686355910222349, "velocityY": 0.2721399578655483, - "timestamp": 1.7377088634696847 + "timestamp": 2.047496962400995 }, { "x": 6.845920803268571, @@ -205,7 +241,7 @@ "angularVelocity": -0.2552238760840361, "velocityX": 2.5128432584428544, "velocityY": 0.24114492931132497, - "timestamp": 1.8204569045872887 + "timestamp": 2.130245003518599 }, { "x": 7.038548371158306, @@ -214,7 +250,7 @@ "angularVelocity": -0.2162243988212622, "velocityX": 2.3278807001118484, "velocityY": 0.2088324250667052, - "timestamp": 1.9032049457048927 + "timestamp": 2.212993044636203 }, { "x": 7.2149794023417675, @@ -223,7 +259,7 @@ "angularVelocity": -0.17615603807074215, "velocityX": 2.1321475264013814, "velocityY": 0.1751952781490545, - "timestamp": 1.9859529868224968 + "timestamp": 2.295741085753807 }, { "x": 7.374384880065902, @@ -232,7 +268,7 @@ "angularVelocity": -0.134857925997875, "velocityX": 1.9263957861864134, "velocityY": 0.14027874932231896, - "timestamp": 2.068701027940101 + "timestamp": 2.3784891268714112 }, { "x": 7.500924464216497, @@ -241,7 +277,7 @@ "angularVelocity": -0.09816609550664941, "velocityX": 1.7511003938959169, "velocityY": 0.11081557912801578, - "timestamp": 2.1409639230840387 + "timestamp": 2.450752022015349 }, { "x": 7.613899532464792, @@ -250,7 +286,7 @@ "angularVelocity": -0.06390193386252535, "velocityX": 1.5633897316635108, "velocityY": 0.08131119317636289, - "timestamp": 2.2132268182279766 + "timestamp": 2.523014917159287 }, { "x": 7.712046212988902, @@ -259,7 +295,7 @@ "angularVelocity": -0.032459027470621904, "velocityX": 1.3581891554251948, "velocityY": 0.05102319751389575, - "timestamp": 2.2854897133719154 + "timestamp": 2.595277812303226 }, { "x": 7.79365194754084, @@ -268,7 +304,7 @@ "angularVelocity": -0.003984930117620908, "velocityX": 1.129289580626482, "velocityY": 0.019180164253482204, - "timestamp": 2.3577526085158533 + "timestamp": 2.6675407074471638 }, { "x": 7.856538953365895, @@ -277,7 +313,7 @@ "angularVelocity": 0.02143014432730538, "velocityX": 0.8702530627897808, "velocityY": -0.0148964629220964, - "timestamp": 2.4300155036597912 + "timestamp": 2.7398036025911017 }, { "x": 7.899249386445817, @@ -286,7 +322,7 @@ "angularVelocity": 0.04198793896480952, "velocityX": 0.5910423737502203, "velocityY": -0.04974532887286408, - "timestamp": 2.502278398803729 + "timestamp": 2.8120664977350396 }, { "x": 7.921731862807926, @@ -295,7 +331,7 @@ "angularVelocity": 0.0598199119499377, "velocityX": 0.31112061476836195, "velocityY": -0.08298006744976984, - "timestamp": 2.574541293947667 + "timestamp": 2.8843293928789775 }, { "x": 7.923961639404313, @@ -304,7 +340,7 @@ "angularVelocity": 0.07527399171536123, "velocityX": 0.030856452567537763, "velocityY": -0.11470827482980998, - "timestamp": 2.646804189091605 + "timestamp": 2.9565922880229154 }, { "x": 7.888861809774048, @@ -313,7 +349,7 @@ "angularVelocity": 0.09629823646465466, "velocityX": -0.3535799196587898, "velocityY": -0.15629824551531185, - "timestamp": 2.7460740505253023 + "timestamp": 3.0558621494566127 }, { "x": 7.817152523236329, @@ -322,7 +358,7 @@ "angularVelocity": 0.12167117579976462, "velocityX": -0.72236714650414, "velocityY": -0.19600081775371833, - "timestamp": 2.8453439119589996 + "timestamp": 3.15513201089031 }, { "x": 7.711448594725456, @@ -331,7 +367,7 @@ "angularVelocity": 0.14503824333134957, "velocityX": -1.064813902067085, "velocityY": -0.23264016737978235, - "timestamp": 2.944613773392697 + "timestamp": 3.2544018723240073 }, { "x": 7.574048351882708, @@ -340,7 +376,7 @@ "angularVelocity": 0.16611675869298972, "velocityX": -1.3841083371967766, "velocityY": -0.2665404504210516, - "timestamp": 3.0438836348263942 + "timestamp": 3.3536717337577047 }, { "x": 7.407019371926434, @@ -349,7 +385,7 @@ "angularVelocity": 0.18508773050978647, "velocityX": -1.682574928019174, "velocityY": -0.2979209522500118, - "timestamp": 3.1431534962600916 + "timestamp": 3.452941595191402 }, { "x": 7.212241390572204, @@ -358,7 +394,7 @@ "angularVelocity": 0.20193563564714437, "velocityX": -1.96210590546985, "velocityY": -0.3269362847056093, - "timestamp": 3.242423357693789 + "timestamp": 3.5522114566250993 }, { "x": 6.990210935369373, @@ -367,7 +403,7 @@ "angularVelocity": 0.216427391579795, "velocityX": -2.236635087388812, "velocityY": -0.35494313919580667, - "timestamp": 3.341693219127486 + "timestamp": 3.6514813180587966 }, { "x": 6.744250267253016, @@ -376,7 +412,7 @@ "angularVelocity": 0.2294640783075409, "velocityX": -2.4776973047417434, "velocityY": -0.3789168637892277, - "timestamp": 3.4409630805611835 + "timestamp": 3.750751179492494 }, { "x": 6.474537469267562, @@ -385,7 +421,7 @@ "angularVelocity": 0.24078036998495714, "velocityX": -2.7169655934857664, "velocityY": -0.401829660482739, - "timestamp": 3.540232941994881 + "timestamp": 3.8500210409261912 }, { "x": 6.185093329086759, @@ -394,7 +430,7 @@ "angularVelocity": 0.24911475902781344, "velocityX": -2.915730273020737, "velocityY": -0.4194814316536095, - "timestamp": 3.639502803428578 + "timestamp": 3.9492909023598886 }, { "x": 5.8780260788239564, @@ -403,7 +439,7 @@ "angularVelocity": 0.28544185589002363, "velocityX": -3.0932575690950848, "velocityY": -0.43272064472210836, - "timestamp": 3.7387726648622754 + "timestamp": 4.048560763793586 }, { "x": 5.557966335758099, @@ -412,7 +448,7 @@ "angularVelocity": 0.283179219968629, "velocityX": -3.224138106404308, "velocityY": -0.4357153838585266, - "timestamp": 3.8380425262959728 + "timestamp": 4.147830625227283 }, { "x": 5.245908943265439, @@ -421,7 +457,7 @@ "angularVelocity": 0.25598634216619676, "velocityX": -3.1435260207458486, "velocityY": -0.40250762031696863, - "timestamp": 3.93731238772967 + "timestamp": 4.2471004866609805 }, { "x": 4.948601311707987, @@ -430,7 +466,7 @@ "angularVelocity": 0.23625249005411966, "velocityX": -2.9949435534976283, "velocityY": -0.37708963138260865, - "timestamp": 4.036582249163367 + "timestamp": 4.346370348094678 }, { "x": 4.669544920179785, @@ -439,7 +475,7 @@ "angularVelocity": 0.21637288647809105, "velocityX": -2.811088758440401, "velocityY": -0.35030685059909655, - "timestamp": 4.135852110597065 + "timestamp": 4.445640209528375 }, { "x": 4.411446504016625, @@ -448,7 +484,7 @@ "angularVelocity": 0.19537592694947853, "velocityX": -2.599967527259491, "velocityY": -0.3215995394142002, - "timestamp": 4.235121972030762 + "timestamp": 4.5449100709620724 }, { "x": 4.176717541970206, @@ -457,7 +493,7 @@ "angularVelocity": 0.17288975707028345, "velocityX": -2.364554142177353, "velocityY": -0.2907945401778102, - "timestamp": 4.334391833464459 + "timestamp": 4.64417993239577 }, { "x": 3.9677270788540704, @@ -466,61 +502,7 @@ "angularVelocity": 0.14871602710307644, "velocityX": -2.1052760636290624, "velocityY": -0.2576878866065069, - "timestamp": 4.433661694898157 - }, - { - "x": 3.7869910923072463, - "y": 6.857920125982094, - "heading": 0.37415675707125545, - "angularVelocity": 0.1229382444164791, - "velocityX": -1.8206531563211636, - "velocityY": -0.22195804012459744, - "timestamp": 4.532931556331854 - }, - { - "x": 3.6375222511124736, - "y": 6.83976261955988, - "heading": 0.3836657545916857, - "angularVelocity": 0.09578937033957077, - "velocityX": -1.5056819767458316, - "velocityY": -0.18291056479757375, - "timestamp": 4.632201417765551 - }, - { - "x": 3.5230866973435258, - "y": 6.825906628204701, - "heading": 0.39040681994356247, - "angularVelocity": 0.06790646480733925, - "velocityX": -1.1527723733691484, - "velocityY": -0.1395790339088257, - "timestamp": 4.731471279199249 - }, - { - "x": 3.4467104911360567, - "y": 6.816686862116541, - "heading": 0.39477159352312596, - "angularVelocity": 0.04396876873328538, - "velocityX": -0.7693795992500979, - "velocityY": -0.09287578279051523, - "timestamp": 4.830741140632946 - }, - { - "x": 3.4085204601287793, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": 0.021852252491790684, - "velocityX": -0.3847092204594867, - "velocityY": -0.046321940190690435, - "timestamp": 4.930011002066643 - }, - { - "x": 3.408520460128781, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": -9.258859562419926e-18, - "velocityX": 7.880837628913899e-16, - "velocityY": -2.4194099006901583e-16, - "timestamp": 5.0292808635003405 + "timestamp": 4.743449793829467 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.5.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.5.traj index cb5a691c..53f46346 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.5.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.5.traj @@ -1,5 +1,59 @@ { "samples": [ + { + "x": 3.9677270788540704, + "y": 6.879953869869358, + "heading": 0.3619526945831296, + "angularVelocity": 0.14871602710307644, + "velocityX": -2.1052760636290624, + "velocityY": -0.2576878866065069, + "timestamp": 0 + }, + { + "x": 3.7869910923072463, + "y": 6.857920125982094, + "heading": 0.37415675707125545, + "angularVelocity": 0.1229382444164791, + "velocityX": -1.8206531563211636, + "velocityY": -0.22195804012459744, + "timestamp": 0.09926986143369732 + }, + { + "x": 3.6375222511124736, + "y": 6.83976261955988, + "heading": 0.3836657545916857, + "angularVelocity": 0.09578937033957077, + "velocityX": -1.5056819767458316, + "velocityY": -0.18291056479757375, + "timestamp": 0.19853972286739463 + }, + { + "x": 3.5230866973435258, + "y": 6.825906628204701, + "heading": 0.39040681994356247, + "angularVelocity": 0.06790646480733925, + "velocityX": -1.1527723733691484, + "velocityY": -0.1395790339088257, + "timestamp": 0.29780958430109195 + }, + { + "x": 3.4467104911360567, + "y": 6.816686862116541, + "heading": 0.39477159352312596, + "angularVelocity": 0.04396876873328538, + "velocityX": -0.7693795992500979, + "velocityY": -0.09287578279051523, + "timestamp": 0.39707944573478926 + }, + { + "x": 3.4085204601287793, + "y": 6.812088489532471, + "heading": 0.39694086360000014, + "angularVelocity": 0.021852252491790684, + "velocityX": -0.3847092204594867, + "velocityY": -0.046321940190690435, + "timestamp": 0.4963493071684866 + }, { "x": 3.408520460128781, "y": 6.812088489532471, @@ -7,7 +61,7 @@ "angularVelocity": -9.258859562419926e-18, "velocityX": 7.880837628913899e-16, "velocityY": -2.4194099006901583e-16, - "timestamp": 0 + "timestamp": 0.5956191686021839 }, { "x": 3.4451800140480633, @@ -16,7 +70,7 @@ "angularVelocity": -0.032542531675579224, "velocityX": 0.37821978715244237, "velocityY": -0.00728463273656313, - "timestamp": 0.09692658915412089 + "timestamp": 0.6925457577563048 }, { "x": 3.518496947445585, @@ -25,7 +79,7 @@ "angularVelocity": -0.06518770266528738, "velocityX": 0.756417140408642, "velocityY": -0.014568380179178244, - "timestamp": 0.19385317830824178 + "timestamp": 0.7894723469104257 }, { "x": 3.6283397734394334, @@ -34,7 +88,7 @@ "angularVelocity": -0.1028144765290181, "velocityX": 1.1332579321365746, "velocityY": -0.021822826471036318, - "timestamp": 0.29077976746236267 + "timestamp": 0.8863989360645466 }, { "x": 3.7707535238798844, @@ -43,7 +97,7 @@ "angularVelocity": -0.16015244726939834, "velocityX": 1.469294975540766, "velocityY": -0.028283854860319504, - "timestamp": 0.38770635661648356 + "timestamp": 0.9833255252186675 }, { "x": 3.9413941020263046, @@ -52,7 +106,7 @@ "angularVelocity": -0.20640661246244818, "velocityX": 1.7605135973070385, "velocityY": -0.03388542757086655, - "timestamp": 0.48463294577060445 + "timestamp": 1.0802521143727883 }, { "x": 4.137139185749371, @@ -61,7 +115,7 @@ "angularVelocity": -0.24433707640817395, "velocityX": 2.0195189517276724, "velocityY": -0.038869046827304486, - "timestamp": 0.5815595349247253 + "timestamp": 1.1771787035269092 }, { "x": 4.3554217491364975, @@ -70,7 +124,7 @@ "angularVelocity": -0.2760533970258723, "velocityX": 2.2520400778783602, "velocityY": -0.043344020960483176, - "timestamp": 0.6784861240788462 + "timestamp": 1.2741052926810301 }, { "x": 4.593770534553052, @@ -79,7 +133,7 @@ "angularVelocity": -0.30281003978695553, "velocityX": 2.459065025362269, "velocityY": -0.04732871870534226, - "timestamp": 0.7754127132329671 + "timestamp": 1.371031881835151 }, { "x": 4.849438377602218, @@ -88,7 +142,7 @@ "angularVelocity": -0.32507743822988416, "velocityX": 2.6377472402607576, "velocityY": -0.05076797373402456, - "timestamp": 0.872339302387088 + "timestamp": 1.467958470989272 }, { "x": 5.118739979240287, @@ -97,7 +151,7 @@ "angularVelocity": -0.3422017100921637, "velocityX": 2.7784079063161986, "velocityY": -0.053475353490631775, - "timestamp": 0.9692658915412089 + "timestamp": 1.5648850601433928 }, { "x": 5.388165163887155, @@ -106,7 +160,7 @@ "angularVelocity": -0.33969888042458984, "velocityX": 2.779682922902244, "velocityY": -0.053496510556234986, - "timestamp": 1.0661924806953298 + "timestamp": 1.6618116492975137 }, { "x": 5.643920085833601, @@ -115,7 +169,7 @@ "angularVelocity": -0.32257120434452946, "velocityX": 2.6386456407723093, "velocityY": -0.05078175178849002, - "timestamp": 1.1631190698494507 + "timestamp": 1.7587382384516346 }, { "x": 5.882330711464664, @@ -124,7 +178,7 @@ "angularVelocity": -0.3003343525465651, "velocityX": 2.4597030362017005, "velocityY": -0.04733756008947892, - "timestamp": 1.2600456590035716 + "timestamp": 1.8556648276057555 }, { "x": 6.100655045491889, @@ -133,70 +187,7 @@ "angularVelocity": -0.27364395679871373, "velocityX": 2.252471029183505, "velocityY": -0.04334908323253468, - "timestamp": 1.3569722481576925 - }, - { - "x": 6.29642574105686, - "y": 6.756504387895065, - "heading": 0.054221932212767415, - "angularVelocity": -0.2420290183052161, - "velocityX": 2.019783191314849, - "velocityY": -0.038870840861772844, - "timestamp": 1.4538988373118134 - }, - { - "x": 6.467079365349485, - "y": 6.7532201591852425, - "heading": 0.03442675668029697, - "angularVelocity": -0.20422853734174792, - "velocityX": 1.7606481955253344, - "velocityY": -0.03388367153413224, - "timestamp": 1.5508254264659342 - }, - { - "x": 6.60949406591357, - "y": 6.750479406444339, - "heading": 0.019100936295010484, - "angularVelocity": -0.15811781389436272, - "velocityX": 1.4693047780484314, - "velocityY": -0.028276582977095337, - "timestamp": 1.6477520156200551 - }, - { - "x": 6.719322188320328, - "y": 6.748365796127614, - "heading": 0.009318208717894495, - "angularVelocity": -0.10092924616960162, - "velocityX": 1.1331062339573879, - "velocityY": -0.021806300367839485, - "timestamp": 1.744678604774176 - }, - { - "x": 6.792644320795612, - "y": 6.746954731447967, - "heading": 0.0031061365662838365, - "angularVelocity": -0.06409048544701255, - "velocityX": 0.756470779743405, - "velocityY": -0.014558076292195965, - "timestamp": 1.841605193928297 - }, - { - "x": 6.829305171966555, - "y": 6.746249198913574, - "heading": -6.996174209709525e-17, - "angularVelocity": -0.03204627949246174, - "velocityX": 0.3782331710099824, - "velocityY": -0.007279040153471663, - "timestamp": 1.9385317830824178 - }, - { - "x": 6.8293051719665545, - "y": 6.746249198913574, - "heading": -3.495236266448189e-17, - "angularVelocity": 5.858001175456844e-19, - "velocityX": -7.463612085764823e-16, - "velocityY": 1.4437890171198592e-17, - "timestamp": 2.0354583722365387 + "timestamp": 1.9525914167598764 } ] } \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index dfb8a171..431f79b8 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -20,7 +20,7 @@ public final class Constants { public static final boolean IS_COMPETITION_ROBOT = true; // Whether to pull PID constants from SmartDashboard - private static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP + private static final boolean tuningMode = false; // TODO: DO NOT SET TO TRUE FOR COMP private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; @@ -169,7 +169,7 @@ public static class Pivot { public static final Rotation2d MAX_ANGLE = Rotation2d.fromRotations(0.174); public static final Rotation2d MIN_ANGLE = Rotation2d.fromRotations(0.044); - public static final Rotation2d REAL_ZEROED_ANGLE = Rotation2d.fromDegrees(8.5); + public static final Rotation2d REAL_ZEROED_ANGLE = Rotation2d.fromDegrees(8.5).minus(Rotation2d.fromRadians(0.3153)); public static final double PIVOT_HEIGHT = Units.inchesToMeters(10.5); public static final double KP = 400.0; @@ -191,7 +191,7 @@ public static class Pivot { public static final double JERK_RPS3 = 2000; - public static final ShooterSetpoint HUB_SHOOT = new ShooterSetpoint(0.125, 4800,4000); + public static final ShooterSetpoint HUB_SHOOT = new ShooterSetpoint(Rotation2d.fromRadians(1.06184)); public static final ShooterSetpoint PODIUM_SHOOT = new ShooterSetpoint(0.07, 6000,5000); diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 5e6e7f0a..a2297b87 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -132,6 +132,7 @@ public void robotPeriodic() { public void disabledInit() { // robotContainer.elevator.setBrakeMode(false); robotContainer.shooter.setPivotBrakeMode(false); + robotContainer.indexer.setIntakeNeutralMode(false); robotContainer.drivetrain.unblockTags(); robotContainer.shooter.setPivotPosition(new Rotation2d()); robotContainer.leds.setPattern(Leds.Zone.MAIN, new LedPatternRainbow(1)); @@ -150,6 +151,7 @@ public void enabledInit() { robotContainer.elevator.setBrakeMode(true); robotContainer.shooter.setPivotBrakeMode(true); robotContainer.drivetrain.setBrakeMode(true); + robotContainer.indexer.setIntakeNeutralMode(true); } /** diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index cd90221b..72106a61 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -1,5 +1,9 @@ package org.team1540.robot2024; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; @@ -12,6 +16,7 @@ import org.team1540.robot2024.commands.drivetrain.SwerveDriveCommand; import org.team1540.robot2024.commands.drivetrain.WheelRadiusCharacterization; import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; +import org.team1540.robot2024.commands.indexer.ContinuousIntakeCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.commands.indexer.StageTrampCommand; @@ -140,6 +145,7 @@ private void configureLedBindings() { private void configureButtonBindings() { Command manualPivotCommand = new ManualPivotCommand(shooter, copilot); drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); +// drivetrain.setDefaultCommand(new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter)); // drivetrain.setDefaultCommand(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter)); elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); shooter.setDefaultCommand(manualPivotCommand); @@ -172,7 +178,9 @@ private void configureButtonBindings() { if (isTuningMode()) { driver.leftTrigger().whileTrue(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter).alongWith(leds.commandShowPattern(new LedPatternWave("#00ff00"), Leds.PatternLevel.DRIVER_LOCK))); driver.a().whileTrue(new TuneShooterCommand(shooter, indexer, drivetrain::getPose)); + } + driver.povDown().and(() -> !DriverStation.isFMSAttached()).onTrue(Commands.runOnce(() -> drivetrain.setPose(new Pose2d(Units.inchesToMeters(260), Units.inchesToMeters(161.62), Rotation2d.fromRadians(0)))).ignoringDisable(true)); driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); @@ -181,7 +189,7 @@ private void configureButtonBindings() { copilot.back().onTrue(Commands.runOnce(shooter::zeroPivotToCancoder).andThen(Commands.print("BACK IS PRESSED"))); copilot.leftBumper().whileTrue(new AmpScoreSequence(tramp, indexer, elevator)); - Command intakeCommand = new IntakeCommand(indexer, tramp::isNoteStaged, 1) + Command intakeCommand = new ContinuousIntakeCommand(indexer, 1) .deadlineWith(CommandUtils.rumbleCommand(driver, 0.5), CommandUtils.rumbleCommand(copilot, 0.5)); copilot.rightBumper().whileTrue(intakeCommand); diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerTargetingCommand.java deleted file mode 100644 index ccdd8415..00000000 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithSpeakerTargetingCommand.java +++ /dev/null @@ -1,67 +0,0 @@ -package org.team1540.robot2024.commands.drivetrain; - -import com.pathplanner.lib.util.GeometryUtil; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import org.littletonrobotics.junction.Logger; -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.util.LoggedTunableNumber; - -import static org.team1540.robot2024.Constants.Targeting.*; - -public class DriveWithSpeakerTargetingCommand extends Command { - private final Drivetrain drivetrain; - private final XboxController controller; - - private final PIDController rotController = new PIDController(ROT_KP, ROT_KI, ROT_KD); - - private final LoggedTunableNumber kP = new LoggedTunableNumber("Targeting/ROT_KP", ROT_KP); - private final LoggedTunableNumber kI = new LoggedTunableNumber("Targeting/ROT_KI", ROT_KI); - private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/ROT_KD", ROT_KD); - - private boolean isFlipped; - private Pose2d speakerPose; - - public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, XboxController controller) { - this.drivetrain = drivetrain; - this.controller = controller; - rotController.enableContinuousInput(-Math.PI, Math.PI); - addRequirements(drivetrain); - } - - @Override - public void initialize() { - rotController.reset(); - isFlipped = DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red; - speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; - } - - @Override - public void execute() { - if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { - rotController.setPID(kP.get(), kI.get(), kD.get()); - } - - Rotation2d targetRot = - drivetrain.getPose().minus(speakerPose).getTranslation().getAngle() - .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0)); - Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); - Logger.recordOutput("Targeting/rotErrorDegrees", Math.abs(targetRot.minus(drivetrain.getRotation()).getDegrees())); - - double xPercent = MathUtil.applyDeadband((-controller.getLeftY()), 0.1); - double yPercent = MathUtil.applyDeadband((-controller.getLeftX()), 0.1); - double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians()); - drivetrain.drivePercent(xPercent, yPercent, rotPercent, true); - } - - @Override - public void end(boolean interrupted) { - drivetrain.stop(); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java index 9560fe16..6d54a8eb 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java @@ -12,6 +12,7 @@ import org.team1540.robot2024.Constants; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.util.LoggedTunableNumber; +import org.team1540.robot2024.util.math.JoystickUtils; import org.team1540.robot2024.util.vision.AprilTagsCrescendo; import java.util.function.Supplier; @@ -32,6 +33,8 @@ public class DriveWithTargetingCommand extends Command { private Supplier target; + private final double deadzone = 0.03; + public DriveWithTargetingCommand(Drivetrain drivetrain, XboxController controller){ this(drivetrain, controller, ()-> AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d()); } @@ -60,14 +63,16 @@ public void execute() { Logger.recordOutput("Targeting/rotErrorDegrees", Math.abs(targetRot.minus(drivetrain.getRotation()).getDegrees())); Logger.recordOutput("Targeting/target", target.get()); - double xPercent = 0; - double yPercent = 0; + double linearMagnitude = 0; + Rotation2d linearDirection = new Rotation2d(); if(controller != null){ - xPercent = MathUtil.applyDeadband((-controller.getLeftY()), 0.1); - yPercent = MathUtil.applyDeadband((-controller.getLeftX()), 0.1); + double xPercent = -controller.getLeftY() * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); + double yPercent = -controller.getLeftX() * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); + linearMagnitude = JoystickUtils.smartDeadzone(Math.hypot(xPercent, yPercent), deadzone); + linearDirection = new Rotation2d(xPercent, yPercent); } double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians()); - drivetrain.drivePercent(xPercent, yPercent, rotPercent, true); + drivetrain.drivePercent(linearMagnitude, linearDirection, rotPercent, true); } @Override diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/HoldRotation.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/HoldRotation.java deleted file mode 100644 index 03b7fae3..00000000 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/HoldRotation.java +++ /dev/null @@ -1,57 +0,0 @@ -package org.team1540.robot2024.commands.drivetrain; - -import com.pathplanner.lib.util.GeometryUtil; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import org.littletonrobotics.junction.Logger; -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.util.LoggedTunableNumber; - -import static org.team1540.robot2024.Constants.Targeting.*; - -public class HoldRotation extends Command { - private final Drivetrain drivetrain; - private final CommandXboxController controller; - - private final PIDController rotController = new PIDController(ROT_KP, ROT_KI, ROT_KD); - - private boolean isFlipped; - private Rotation2d targetRotation; - - public HoldRotation(Drivetrain drivetrain, CommandXboxController controller, Rotation2d targetRotation) { - this.drivetrain = drivetrain; - this.controller = controller; - this.targetRotation = targetRotation; - rotController.enableContinuousInput(-Math.PI, Math.PI); - addRequirements(drivetrain); - } - - @Override - public void initialize() { - rotController.reset(); - - isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; - } - - @Override - public void execute() { - Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRotation)); - double xPercent = MathUtil.applyDeadband((-controller.getLeftY()), 0.1); - double yPercent = MathUtil.applyDeadband((-controller.getLeftX()), 0.1); - double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRotation.getRadians()); - drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped); - } - - @Override - public void end(boolean interrupted) { - drivetrain.stop(); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/SwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/SwerveDriveCommand.java index 76038cac..f725e7d2 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/SwerveDriveCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/SwerveDriveCommand.java @@ -1,5 +1,6 @@ package org.team1540.robot2024.commands.drivetrain; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.team1540.robot2024.Constants; @@ -24,11 +25,13 @@ public void initialize() { @Override public void execute() { - double xPercent = JoystickUtils.squaredSmartDeadzone(-controller.getLeftY(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); - double yPercent = JoystickUtils.squaredSmartDeadzone(-controller.getLeftX(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); - double rotPercent = JoystickUtils.squaredSmartDeadzone(-controller.getRightX(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); + double xPercent = -controller.getLeftY() * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); + double yPercent = -controller.getLeftX() * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); + double linearMagnitude = JoystickUtils.smartDeadzone(Math.hypot(xPercent, yPercent), deadzone); + Rotation2d linearDirection = new Rotation2d(xPercent, yPercent); + double rotPercent = JoystickUtils.smartDeadzone(-controller.getRightX(), deadzone) * (Constants.IS_COMPETITION_ROBOT ? 1 : -1); - drivetrain.drivePercent(xPercent, yPercent, rotPercent, true); + drivetrain.drivePercent(linearMagnitude, linearDirection, rotPercent, true); } @Override diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java new file mode 100644 index 00000000..a9369a94 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java @@ -0,0 +1,33 @@ +package org.team1540.robot2024.commands.indexer; + +import edu.wpi.first.wpilibj2.command.Command; +import org.team1540.robot2024.subsystems.indexer.Indexer; + +import java.util.function.BooleanSupplier; + +public class ContinuousIntakeCommand extends Command { + private final Indexer indexer; + private final double percent; + + + + public ContinuousIntakeCommand(Indexer indexer, double percent) { + this.indexer = indexer; + this.percent = percent; + addRequirements(indexer); + } + + @Override + public void execute() { + if (indexer.isNoteStaged()) { + indexer.stopIntake(); + } else { + indexer.setIntakePercent(percent); + } + } + + @Override + public void end(boolean interrupted) { + indexer.stopIntake(); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java index 65c73b56..4726ffc8 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java @@ -31,7 +31,7 @@ public void initialize() { @Override public boolean isFinished() { - return shouldUseBeambreak && (noteInTramp.getAsBoolean() || indexer.isNoteStaged()); + return shouldUseBeambreak && (indexer.isNoteStaged()); } @Override diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java index 94901017..5d4c888f 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; @@ -12,7 +13,7 @@ public class OverStageShootPrepareWithTargeting extends ParallelCommandGroup { public OverStageShootPrepareWithTargeting(XboxController controller, Drivetrain drivetrain, Shooter shooter) { addCommands( - new DriveWithTargetingCommand(drivetrain,controller, ()-> AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().plus(new Transform2d(0,-2,new Rotation2d()))), + new DriveWithTargetingCommand(drivetrain,controller, ()-> AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().plus(new Transform2d(0,-2 * (DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Blue ? -1 : 1),new Rotation2d()))), new OverStageShootPrepare(drivetrain, shooter) ); } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java index ac32b2ba..2e0bab12 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java @@ -19,16 +19,6 @@ public class ShootSequence extends ParallelRaceGroup { - public ShootSequence(Supplier positionSupplier, Shooter shooter, Indexer indexer) { - this(shooter, indexer, () -> new ShooterSetpoint( - Rotation2d.fromRadians( - Math.atan2(Constants.Targeting.SPEAKER_CENTER_HEIGHT - Constants.Shooter.Pivot.PIVOT_HEIGHT, positionSupplier.get().getTranslation().getDistance( - AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation() - ))).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), - 8000, 6000) - ); - } - public ShootSequence(Shooter shooter, Indexer indexer) { this(shooter, indexer, () -> HUB_SHOOT); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 7d8ee2ed..8bbd7fb2 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -211,9 +211,7 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); } - public void drivePercent(double xPercent, double yPercent, double rotPercent, boolean fieldRelative) { - Rotation2d linearDirection = new Rotation2d(xPercent, yPercent); - double linearMagnitude = Math.hypot(xPercent, yPercent); + public void drivePercent(double linearMagnitude, Rotation2d linearDirection, double rotPercent, boolean fieldRelative) { Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection) diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index 66a00f20..84976249 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -141,4 +141,8 @@ public Command setIntakeAndFeeder(double intakePercent, double feederVelocity) { ); } + public void setIntakeNeutralMode(boolean isBrake) { + io.setIntakeNeutralMode(isBrake); + } + } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java index c7fcdcaf..56d0842c 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -30,4 +30,6 @@ default void setFeederVelocity(double velocityRPM) {} default void configureFeederPID(double p, double i, double d) {} + default void setIntakeNeutralMode(boolean isBrakeMode) {} + } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java index d69271ab..d00087c5 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -73,4 +73,9 @@ public void configureFeederPID(double p, double i, double d) { feederPID.setI(i); feederPID.setD(d); } + + public void setIntakeNeutralMode(boolean isBrakeMode) { + intakeMotor.setIdleMode(isBrakeMode ? CANSparkBase.IdleMode.kBrake : CANSparkBase.IdleMode.kCoast); + } } + diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java index f2668afb..cabb1293 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java @@ -114,4 +114,9 @@ public void configureFeederPID(double p, double i, double d) { pidConfigs.kD = d; feederMotor.getConfigurator().apply(pidConfigs); } + + @Override + public void setIntakeNeutralMode(boolean isBrakeMode) { + intakeMotor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index 91706287..323a4381 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -25,7 +25,7 @@ public Leds() { strip.start(); buffers[Zone.MAIN.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 41, false); - buffers[Zone.TOP.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 35, 41, false); + buffers[Zone.TOP.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 32, 41, false); for (int i = 0; i < ZONE_COUNT;i++) { patterns[i] = new LedTriager(); } From e4f055aed593d16b5cf718d68d957af47702f382 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Sat, 23 Mar 2024 08:17:19 -0700 Subject: [PATCH 56/75] feat: amp lock on amp stage --- .../org/team1540/robot2024/RobotContainer.java | 7 ++++++- .../drivetrain/DriveWithAmpSideLock.java | 17 +++++++++++++++++ 2 files changed, 23 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithAmpSideLock.java diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 72106a61..1d14b117 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -13,6 +13,7 @@ import org.team1540.robot2024.commands.FeedForwardCharacterization; import org.team1540.robot2024.commands.autos.*; import org.team1540.robot2024.commands.climb.ClimbAlignment; +import org.team1540.robot2024.commands.drivetrain.DriveWithAmpSideLock; import org.team1540.robot2024.commands.drivetrain.SwerveDriveCommand; import org.team1540.robot2024.commands.drivetrain.WheelRadiusCharacterization; import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; @@ -147,6 +148,7 @@ private void configureButtonBindings() { drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); // drivetrain.setDefaultCommand(new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter)); // drivetrain.setDefaultCommand(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter)); +// drivetrain.setDefaultCommand(new DriveWithAmpSideLock(drivetrain, driver.getHID())); elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); shooter.setDefaultCommand(manualPivotCommand); driver.b().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); @@ -163,9 +165,12 @@ private void configureButtonBindings() { .alongWith(leds.commandShowPattern(new LedPatternWave("#f700ff"), Leds.PatternLevel.DRIVER_LOCK)); Command autoShooterCommand = new AutoShootPrepare(drivetrain, shooter) .alongWith(leds.commandShowPattern(new LedPatternWave("#00ffbc"), Leds.PatternLevel.DRIVER_LOCK)); + Command ampLock = new DriveWithAmpSideLock(drivetrain, driver.getHID()) + .alongWith(leds.commandShowPattern(new LedPatternWave("#ffffff"), Leds.PatternLevel.DRIVER_LOCK)); Command cancelAlignment = Commands.runOnce(() -> { targetDrive.cancel(); overstageTargetDrive.cancel(); + ampLock.cancel(); }); driver.x() .whileTrue(IntakeAndFeed.withDefaults(indexer)) @@ -203,7 +208,7 @@ private void configureButtonBindings() { copilot.x().whileTrue(new ShootSequence(shooter, indexer)); - copilot.a().whileTrue(new AmpScoreStageSequence(indexer, tramp, elevator)); + copilot.a().whileTrue(new AmpScoreStageSequence(indexer, tramp, elevator).alongWith(ampLock)); copilot.b().whileTrue(IntakeAndFeed.withDefaults(indexer)) .onFalse(cancelAlignment); copilot.y().whileTrue(new StageTrampCommand(tramp, indexer)); diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithAmpSideLock.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithAmpSideLock.java new file mode 100644 index 00000000..ee0d26d4 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithAmpSideLock.java @@ -0,0 +1,17 @@ +package org.team1540.robot2024.commands.drivetrain; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.team1540.robot2024.subsystems.drive.Drivetrain; + +public class DriveWithAmpSideLock extends SequentialCommandGroup { + public DriveWithAmpSideLock(Drivetrain drivetrain, XboxController controller){ + addCommands( + new DriveWithTargetingCommand(drivetrain, controller, ()->drivetrain.getPose().plus(new Transform2d(0,DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Blue ? -1 : 1, new Rotation2d()))) + ); + } +} From 034ae5e2054739e785df9c47294cec33864766e8 Mon Sep 17 00:00:00 2001 From: Zach R Date: Sun, 24 Mar 2024 16:47:08 -0700 Subject: [PATCH 57/75] event: eod day 2 --- paths.chor | 22161 +++++++++------- .../choreo/CenterLanePCBAFSprint.1.traj | 570 +- .../choreo/CenterLanePCBAFSprint.2.traj | 102 +- .../choreo/CenterLanePCBAFSprint.3.traj | 168 +- .../choreo/CenterLanePCBAFSprint.4.traj | 1636 +- .../choreo/CenterLanePCBAFSprint.5.traj | 354 +- .../deploy/choreo/CenterLanePCBAFSprint.traj | 2926 +- .../deploy/choreo/CenterLanePCBFSprint.1.traj | 564 +- .../deploy/choreo/CenterLanePCBFSprint.2.traj | 133 +- .../deploy/choreo/CenterLanePCBFSprint.3.traj | 1202 +- .../deploy/choreo/CenterLanePCBFSprint.4.traj | 732 +- .../deploy/choreo/CenterLanePCBFSprint.5.traj | 382 - .../deploy/choreo/CenterLanePCBFSprint.traj | 2982 +-- .../deploy/choreo/SourceLanePGCSprint.1.traj | 184 + .../deploy/choreo/SourceLanePGCSprint.2.traj | 841 + .../deploy/choreo/SourceLanePGCSprint.3.traj | 319 + .../deploy/choreo/SourceLanePGCSprint.4.traj | 391 + .../deploy/choreo/SourceLanePGCSprint.traj | 1696 ++ .../deploy/choreo/SourceLanePGHSprint.1.traj | 315 +- .../deploy/choreo/SourceLanePGHSprint.2.traj | 1503 +- .../deploy/choreo/SourceLanePGHSprint.3.traj | 1428 +- .../deploy/choreo/SourceLanePGHSprint.4.traj | 644 +- .../deploy/choreo/SourceLanePGHSprint.traj | 3884 ++- .../choreo/SourceLanePHGSprint (1).1.traj | 184 + .../choreo/SourceLanePHGSprint (1).2.traj | 814 + .../choreo/SourceLanePHGSprint (1).3.traj | 787 + .../choreo/SourceLanePHGSprint (1).4.traj | 355 + .../choreo/SourceLanePHGSprint (1).traj | 2101 ++ .../deploy/choreo/SourceLanePHGSprint.1.traj | 350 +- .../deploy/choreo/SourceLanePHGSprint.2.traj | 1322 +- .../deploy/choreo/SourceLanePHGSprint.3.traj | 962 +- .../deploy/choreo/SourceLanePHGSprint.4.traj | 638 +- .../deploy/choreo/SourceLanePHGSprint.traj | 1570 -- .../commands/autos/CenterLanePCBAFSprint.java | 6 +- .../commands/autos/CenterLanePCBFSprint.java | 25 +- .../commands/autos/SourceLanePGHSprint.java | 9 +- .../robot2024/util/auto/AutoCommand.java | 13 +- 37 files changed, 32161 insertions(+), 22092 deletions(-) delete mode 100644 src/main/deploy/choreo/CenterLanePCBFSprint.5.traj create mode 100644 src/main/deploy/choreo/SourceLanePGCSprint.1.traj create mode 100644 src/main/deploy/choreo/SourceLanePGCSprint.2.traj create mode 100644 src/main/deploy/choreo/SourceLanePGCSprint.3.traj create mode 100644 src/main/deploy/choreo/SourceLanePGCSprint.4.traj create mode 100644 src/main/deploy/choreo/SourceLanePGCSprint.traj create mode 100644 src/main/deploy/choreo/SourceLanePHGSprint (1).1.traj create mode 100644 src/main/deploy/choreo/SourceLanePHGSprint (1).2.traj create mode 100644 src/main/deploy/choreo/SourceLanePHGSprint (1).3.traj create mode 100644 src/main/deploy/choreo/SourceLanePHGSprint (1).4.traj create mode 100644 src/main/deploy/choreo/SourceLanePHGSprint (1).traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGSprint.traj diff --git a/paths.chor b/paths.chor index 16258b2d..2db800e9 100644 --- a/paths.chor +++ b/paths.chor @@ -3,7 +3,7 @@ "robotConfiguration": { "mass": 61.73998083590995, "rotationalInertia": 3.6816072891155396, - "motorMaxTorque": 0.5, + "motorMaxTorque": 0.6, "motorMaxVelocity": 4864, "gearing": 6.746, "wheelbase": 0.4762497428251389, @@ -11209,16 +11209,16 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { - "x": 8.0634765625, + "x": 8.1634765625, "y": 4.073246955871582, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { "x": 5.665951728820801, @@ -11280,1801 +11280,1819 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": -8.928261679081423e-33, - "velocityX": -3.922322521208882e-33, - "velocityY": -1.0855099176840522e-33, + "angularVelocity": -2.75917641303777e-30, + "velocityX": -8.847327306636262e-30, + "velocityY": 1.4420464788285476e-29, "timestamp": 0 }, { - "x": 1.306057392567738, - "y": 5.572382489557579, - "heading": -0.01069490392439116, - "angularVelocity": -0.14212387666683293, - "velocityX": 0.21372515459002697, - "velocityY": -0.2467996203807809, - "timestamp": 0.07525057840535954 - }, - { - "x": 1.33822347033388, - "y": 5.535239164732594, - "heading": -0.0320761792409761, - "angularVelocity": -0.28413436507302803, - "velocityX": 0.4274528973434579, - "velocityY": -0.49359520700160897, - "timestamp": 0.15050115681071907 - }, - { - "x": 1.3864731960688501, - "y": 5.4795246417936365, - "heading": -0.06412173122646007, - "angularVelocity": -0.42585123815077036, - "velocityX": 0.6411874401158554, - "velocityY": -0.7403866404698544, - "timestamp": 0.2257517352160786 - }, - { - "x": 1.4508074313630048, - "y": 5.405239333647035, - "heading": -0.1067984096254206, - "angularVelocity": -0.567127579658856, - "velocityX": 0.8549334325059841, - "velocityY": -0.9871725868529632, - "timestamp": 0.30100231362143814 - }, - { - "x": 1.5312274241077561, - "y": 5.312383875814834, - "heading": -0.16006570142282744, - "angularVelocity": -0.707865546367854, - "velocityX": 1.0686960080432237, - "velocityY": -1.233950087825352, - "timestamp": 0.3762528920267977 - }, - { - "x": 1.6277348707066417, - "y": 5.200959293585713, - "heading": -0.2238805012390217, - "angularVelocity": -0.848030688514272, - "velocityX": 1.2824811269757883, - "velocityY": -1.4807139638036912, - "timestamp": 0.4515034704321572 - }, - { - "x": 1.7403321345867286, - "y": 5.070967309799712, - "heading": -0.29820239377845176, - "angularVelocity": -0.9876587544493436, - "velocityX": 1.4962976533356125, - "velocityY": -1.727454945073796, - "timestamp": 0.5267540488375168 - }, - { - "x": 1.8690235919905007, - "y": 4.922411591690569, - "heading": -0.3829978316122351, - "angularVelocity": -1.126841010802703, - "velocityX": 1.7101723352947207, - "velocityY": -1.9741471927152945, - "timestamp": 0.6020046272428763 - }, - { - "x": 2.021840076397225, - "y": 4.778612689187088, - "heading": -0.46243678907347296, - "angularVelocity": -1.055659094516408, - "velocityX": 2.030768236538101, - "velocityY": -1.9109341821781907, - "timestamp": 0.6772552056482358 - }, - { - "x": 2.158571900473102, - "y": 4.653385130963448, - "heading": -0.5314200615226269, - "angularVelocity": -0.916714182282495, - "velocityX": 1.8170202405532427, - "velocityY": -1.6641408063212961, - "timestamp": 0.7525057840535954 - }, - { - "x": 2.2792154502461437, - "y": 4.546725640046215, - "heading": -0.5899528620106769, - "angularVelocity": -0.7778385459410783, - "velocityX": 1.6032242187317085, - "velocityY": -1.4173909779494496, - "timestamp": 0.8277563624589549 - }, - { - "x": 2.383769251108642, - "y": 4.458632746099449, - "heading": -0.6380359031198586, - "angularVelocity": -0.6389723790582461, - "velocityX": 1.3894086009450728, - "velocityY": -1.1706606887754087, - "timestamp": 0.9030069408643144 - }, - { - "x": 2.472232460585404, - "y": 4.389105547578634, - "heading": -0.6756681828246474, - "angularVelocity": -0.5000928963239504, - "velocityX": 1.175581787560874, - "velocityY": -0.9239423801673171, - "timestamp": 0.978257519269674 - }, - { - "x": 2.544604550352729, - "y": 4.338143448939913, - "heading": -0.7028480663779157, - "angularVelocity": -0.3611916895423175, - "velocityX": 0.9617479533176606, - "velocityY": -0.677231985702458, - "timestamp": 1.0535080976750335 - }, - { - "x": 2.6008851962155197, - "y": 4.305746063716667, - "heading": -0.7195737337551226, - "angularVelocity": -0.22226629657394859, - "velocityX": 0.7479098108670842, - "velocityY": -0.4305267269671826, - "timestamp": 1.128758676080393 - }, - { - "x": 2.641074231196452, - "y": 4.291913169677742, - "heading": -0.7258433495685629, - "angularVelocity": -0.08331651325877153, - "velocityX": 0.5340694494657862, - "velocityY": -0.18382442144709177, - "timestamp": 1.2040092544857526 + "x": 1.3060574436245773, + "y": 5.572382537817046, + "heading": -0.010694867587036427, + "angularVelocity": -0.14212340511596666, + "velocityX": 0.21372585011902087, + "velocityY": -0.24679899874441508, + "timestamp": 0.07525057240542533 + }, + { + "x": 1.3382236275279018, + "y": 5.53523931290218, + "heading": -0.032076067517629925, + "angularVelocity": -0.2841333859323569, + "velocityX": 0.42745434186951237, + "velocityY": -0.4935939186635594, + "timestamp": 0.15050114481085067 + }, + { + "x": 1.386473519841097, + "y": 5.479524946048311, + "heading": -0.0641215016247012, + "angularVelocity": -0.42584970563284796, + "velocityX": 0.6411897048771923, + "velocityY": -0.7403846253066342, + "timestamp": 0.22575171721627602 + }, + { + "x": 1.450807989654895, + "y": 5.405239856496701, + "heading": -0.10679801494850981, + "angularVelocity": -0.5671254312134459, + "velocityX": 0.8549366171751026, + "velocityY": -0.9871697606799944, + "timestamp": 0.30100228962170134 + }, + { + "x": 1.5312282961066404, + "y": 5.312384689270129, + "heading": -0.16006508743981135, + "angularVelocity": -0.7078626884752101, + "velocityX": 1.0687002620672836, + "velocityY": -1.23394632438782, + "timestamp": 0.37625286202712666 + }, + { + "x": 1.627736154340151, + "y": 5.200960485487107, + "heading": -0.22387960186124028, + "angularVelocity": -0.8480269635530943, + "velocityX": 1.2824866993939017, + "velocityY": -1.4807090527398261, + "timestamp": 0.451503434432552 + }, + { + "x": 1.7403339652666383, + "y": 5.0709689996393825, + "heading": -0.2982011188077779, + "angularVelocity": -0.9876538419890185, + "velocityX": 1.4963050422725384, + "velocityY": -1.7274484657660032, + "timestamp": 0.5267540068379774 + }, + { + "x": 1.8690262176186487, + "y": 4.922413993905078, + "heading": -0.382996017565801, + "angularVelocity": -1.1268339369356364, + "velocityX": 1.710183035621927, + "velocityY": -1.9741378834553056, + "timestamp": 0.6020045792434027 + }, + { + "x": 2.0218420905066052, + "y": 4.778614575286612, + "heading": -0.4624353832107095, + "angularVelocity": -1.055664602990153, + "velocityX": 2.030760272057439, + "velocityY": -1.9109411931313167, + "timestamp": 0.677255151648828 + }, + { + "x": 2.15857350503456, + "y": 4.653386666317764, + "heading": -0.5314189322497749, + "angularVelocity": -0.9167179309443209, + "velocityX": 1.8170149429960494, + "velocityY": -1.6641456000181232, + "timestamp": 0.7525057240542533 + }, + { + "x": 2.2792167348207513, + "y": 4.5467268950736175, + "heading": -0.589951951408673, + "angularVelocity": -0.7778415138506264, + "velocityX": 1.6032200942942694, + "velocityY": -1.4173948161921597, + "timestamp": 0.8277562964596786 + }, + { + "x": 2.38377026778444, + "y": 4.458633759569044, + "heading": -0.6380351777892487, + "angularVelocity": -0.6389748920543931, + "velocityX": 1.3894051516512624, + "velocityY": -1.1706639921478266, + "timestamp": 0.903006868865104 + }, + { + "x": 2.472233242714231, + "y": 4.389106342435906, + "heading": -0.6756676216200074, + "angularVelocity": -0.5000951172479533, + "velocityX": 1.1755787644266529, + "velocityY": -0.9239453589485842, + "timestamp": 0.9782574412705293 + }, + { + "x": 2.5446051200448103, + "y": 4.3381440386367105, + "heading": -0.7028476555095471, + "angularVelocity": -0.3611937161417901, + "velocityX": 0.9617452069540623, + "velocityY": -0.6772347660530061, + "timestamp": 1.0535080136759547 + }, + { + "x": 2.600885568086537, + "y": 4.305746455375921, + "heading": -0.7195734643408855, + "angularVelocity": -0.22226819406292112, + "velocityX": 0.7479072416796508, + "velocityY": -0.4305293929915994, + "timestamp": 1.12875858608138 + }, + { + "x": 2.641074414508583, + "y": 4.291913365901662, + "heading": -0.7258432162336076, + "angularVelocity": -0.08331832824324738, + "velocityX": 0.5340669863123017, + "velocityY": -0.18382703322097846, + "timestamp": 1.2040091584868053 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.055657173156648594, - "velocityX": 0.320228661947563, - "velocityY": 0.06287683198072233, - "timestamp": 1.2792598328911122 - }, - { - "x": 2.6731094248331484, - "y": 4.3202884971859605, - "heading": -0.7067986870475855, - "angularVelocity": 0.19580509628602769, - "velocityX": 0.1046188223798005, - "velocityY": 0.3116212313704759, - "timestamp": 1.355133384347763 - }, - { - "x": 2.6646882165450285, - "y": 4.362805712309731, - "heading": -0.6813129128724049, - "angularVelocity": 0.3358979998417677, - "velocityX": -0.11099003706094854, - "velocityY": 0.5603693817873046, - "timestamp": 1.431006935804414 - }, - { - "x": 2.6399081500889086, - "y": 4.424196738034775, - "heading": -0.6452019314674031, - "angularVelocity": 0.47593635346874824, - "velocityX": -0.3265968968155938, - "velocityY": 0.8091228701758959, - "timestamp": 1.5068804872610648 - }, - { - "x": 2.5987694540060504, - "y": 4.504462102027486, - "heading": -0.5984691810773222, - "angularVelocity": 0.6159293916376241, - "velocityX": -0.542200744436769, - "velocityY": 1.0578833131143173, - "timestamp": 1.5827540387177157 - }, - { - "x": 2.5412724297463463, - "y": 4.603602434972891, - "heading": -0.5411158632576627, - "angularVelocity": 0.7559065935173384, - "velocityX": -0.757800619001658, - "velocityY": 1.3066520683699112, - "timestamp": 1.6586275901743666 - }, - { - "x": 2.4674174122154677, - "y": 4.7216183838541035, - "heading": -0.4731379535005828, - "angularVelocity": 0.8959368376992063, - "velocityX": -0.9733960795689668, - "velocityY": 1.5554293507486547, - "timestamp": 1.7345011416310174 - }, - { - "x": 2.37720454589188, - "y": 4.858510309507254, - "heading": -0.39452114170963426, - "angularVelocity": 1.0361556864234158, - "velocityX": -1.188989635935645, - "velocityY": 1.8042113888838065, - "timestamp": 1.8103746930876683 - }, - { - "x": 2.270632337422942, - "y": 5.01427685954172, - "heading": -0.305233667532112, - "angularVelocity": 1.1767931309836617, - "velocityX": -1.4046028744262238, - "velocityY": 2.0529756027495263, - "timestamp": 1.8862482445443192 - }, - { - "x": 2.156211319422429, - "y": 5.146461372898718, - "heading": -0.2291032339501207, - "angularVelocity": 1.003385661016371, - "velocityX": -1.508048797028907, - "velocityY": 1.7421685267035296, - "timestamp": 1.96212179600097 - }, - { - "x": 2.058134463277846, - "y": 5.259759383515319, - "heading": -0.16374337088202898, - "angularVelocity": 0.8614314449935028, - "velocityX": -1.2926356320702503, - "velocityY": 1.493247758164214, - "timestamp": 2.037995347457621 - }, - { - "x": 1.976403644982925, - "y": 5.354173277081842, - "heading": -0.10920518207341517, - "angularVelocity": 0.7188036906348487, - "velocityX": -1.0771977418457426, - "velocityY": 1.2443584326016996, - "timestamp": 2.113868898914272 - }, - { - "x": 1.9110189705231253, - "y": 5.429703862947542, - "heading": -0.06553401357556585, - "angularVelocity": 0.5755782833336871, - "velocityX": -0.861758454751598, - "velocityY": 0.9954797741193555, - "timestamp": 2.189742450370923 - }, - { - "x": 1.8619803278189464, - "y": 5.486351581956828, - "heading": -0.032764600292336035, - "angularVelocity": 0.43189507613798817, - "velocityX": -0.6463206448454762, - "velocityY": 0.7466069258884014, - "timestamp": 2.265616001827574 - }, - { - "x": 1.829287687812507, - "y": 5.524116686023993, - "heading": -0.010918035230124941, - "angularVelocity": 0.28793386684545474, - "velocityX": -0.43088321791708567, - "velocityY": 0.4977373978433378, - "timestamp": 2.3414895532842253 + "angularVelocity": 0.05565540572186624, + "velocityX": 0.32022625146750794, + "velocityY": 0.06287422939745346, + "timestamp": 1.2792597308922307 + }, + { + "x": 2.6731092407857906, + "y": 4.320288302812448, + "heading": -0.7067988166038234, + "angularVelocity": 0.1958033723255861, + "velocityX": 0.10461638788158735, + "velocityY": 0.31161864341117435, + "timestamp": 1.3551332887134768 + }, + { + "x": 2.664687841052256, + "y": 4.3628053226693035, + "heading": -0.6813131694887641, + "angularVelocity": 0.33589629702821755, + "velocityX": -0.11099255097832775, + "velocityY": 0.5603667611869495, + "timestamp": 1.431006846534723 + }, + { + "x": 2.6399075701479586, + "y": 4.42419614745244, + "heading": -0.6452023149092349, + "angularVelocity": 0.47593464200079133, + "velocityX": -0.32659956402163576, + "velocityY": 0.8091201539132412, + "timestamp": 1.5068804043559691 + }, + { + "x": 2.598768648766779, + "y": 4.504461298133711, + "heading": -0.5984696942887661, + "angularVelocity": 0.6159276296230222, + "velocityX": -0.5422036683592814, + "velocityY": 1.057880412955572, + "timestamp": 1.5827539621772153 + }, + { + "x": 2.5412713665853555, + "y": 4.603601395355974, + "heading": -0.5411165139934166, + "angularVelocity": 0.7559047175548449, + "velocityX": -0.7578039548110096, + "velocityY": 1.3066488519598491, + "timestamp": 1.6586275199984615 + }, + { + "x": 2.4674160388824338, + "y": 4.721617069365857, + "heading": -0.47313875765168273, + "angularVelocity": 0.8959347405484323, + "velocityX": -0.9734000859460189, + "velocityY": 1.5554255975016502, + "timestamp": 1.7345010778197076 + }, + { + "x": 2.377202770872684, + "y": 4.858508647531332, + "heading": -0.39452213175531803, + "angularVelocity": 1.0361531494380563, + "velocityX": -1.1889948303725923, + "velocityY": 1.8042066576932412, + "timestamp": 1.8103746356409538 + }, + { + "x": 2.270629951391599, + "y": 5.014274677090094, + "heading": -0.305234926659776, + "angularVelocity": 1.1767894858012566, + "velocityX": -1.4046108096674554, + "velocityY": 2.0529685707303664, + "timestamp": 1.8862481934622 + }, + { + "x": 2.1562097285316435, + "y": 5.146459905330176, + "heading": -0.22910409225153178, + "angularVelocity": 1.0033908596890408, + "velocityX": -1.5080381906707807, + "velocityY": 1.7421778026344412, + "timestamp": 1.9621217512834461 + }, + { + "x": 2.0581334055736438, + "y": 5.259758402247803, + "heading": -0.16374394944584372, + "angularVelocity": 0.8614350596401947, + "velocityX": -1.2926284963034982, + "velocityY": 1.4932540422920335, + "timestamp": 2.0379953091046925 + }, + { + "x": 1.9764029763301112, + "y": 5.354172653968883, + "heading": -0.10920555144834718, + "angularVelocity": 0.7188063874223503, + "velocityX": -1.0771925238352613, + "velocityY": 1.2443630486526756, + "timestamp": 2.1138688669259387 + }, + { + "x": 1.9110185860607423, + "y": 5.429703503317112, + "heading": -0.06553422750381226, + "angularVelocity": 0.5755802838199697, + "velocityX": -0.8617546368678625, + "velocityY": 0.9954831632816045, + "timestamp": 2.189742424747185 + }, + { + "x": 1.8619801423257207, + "y": 5.486351407872208, + "heading": -0.032764704050183134, + "angularVelocity": 0.43189649194112756, + "velocityX": -0.6463179682397073, + "velocityY": 0.7466093087307788, + "timestamp": 2.265615982568431 + }, + { + "x": 1.8292876278534882, + "y": 5.524116629586748, + "heading": -0.010918068889347814, + "angularVelocity": 0.28793476658359446, + "velocityX": -0.43088152724695716, + "velocityY": 0.4977389066689233, + "timestamp": 2.341489540389677 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -6.884486248240525e-32, - "angularVelocity": 0.14389777492309389, - "velocityX": -0.21544390526149312, - "velocityY": 0.2488690879972792, - "timestamp": 2.4173631047408763 + "heading": 2.394848489426687e-27, + "angularVelocity": 0.14389820647675833, + "velocityX": -0.21544309693644778, + "velocityY": 0.24886981095702526, + "timestamp": 2.4173630982109233 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -2.79027732569672e-32, - "angularVelocity": 5.39778462149558e-31, - "velocityX": -1.372141826224318e-33, - "velocityY": 4.2150961560958763e-32, - "timestamp": 2.4932366561975274 + "heading": 2.3925172511788446e-27, + "angularVelocity": -3.168744293663733e-29, + "velocityX": 8.48223659243716e-32, + "velocityY": -3.031681211954363e-28, + "timestamp": 2.4932366560321695 }, { "x": 1.8507557561824508, "y": 5.5430527114919315, - "heading": -1.045695782000071e-18, - "angularVelocity": -1.1232079825061295e-17, - "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395097007, - "timestamp": 2.586335690239901 + "heading": -1.9710234547097696e-21, + "angularVelocity": -2.1171281249858964e-20, + "velocityX": 0.40617567078743816, + "velocityY": 0.0005740544395108319, + "timestamp": 2.586335690074543 }, { "x": 1.9263848800936068, - "y": 5.543159599317717, - "heading": -3.434421194050575e-18, - "angularVelocity": -2.5657896847386088e-17, - "velocityX": 0.812351327691905, - "velocityY": 0.0011481088593983807, - "timestamp": 2.6794347242822742 + "y": 5.5431595993177165, + "heading": -2.0389636321585498e-20, + "angularVelocity": -1.9783892557349584e-19, + "velocityX": 0.8123513276919052, + "velocityY": 0.001148108859400643, + "timestamp": 2.6794347241169163 }, { "x": 2.039828562987611, "y": 5.543319931052194, - "heading": -7.689792330773154e-18, - "angularVelocity": -4.570800524939683e-17, - "velocityX": 1.2185269596070252, - "velocityY": 0.0017221632439692253, - "timestamp": 2.7725337583246477 + "heading": -2.352458064017618e-20, + "angularVelocity": -3.3673220682050347e-20, + "velocityX": 1.2185269596070256, + "velocityY": 0.001722163243972619, + "timestamp": 2.7725337581592897 }, { "x": 2.1910867994360017, - "y": 5.543533706687691, - "heading": -1.506248986539688e-17, - "angularVelocity": -7.919198744068695e-17, - "velocityX": 1.6247025332136764, - "velocityY": 0.0022962175461317975, - "timestamp": 2.865632792367021 + "y": 5.54353370668769, + "heading": -3.364930009404432e-20, + "angularVelocity": -1.087521428982912e-19, + "velocityX": 1.6247025332136769, + "velocityY": 0.0022962175461363225, + "timestamp": 2.865632792201663 }, { - "x": 2.380159562296481, - "y": 5.5438009261858445, - "heading": -1.506353706937017e-17, - "angularVelocity": -1.1248279684723702e-20, - "velocityX": 2.0308778152781284, - "velocityY": 0.0028702714362532117, - "timestamp": 2.9587318264093945 + "x": 2.3801595622964817, + "y": 5.543800926185845, + "heading": -1.7618072508363382e-20, + "angularVelocity": 1.721954234066794e-19, + "velocityX": 2.030877815278129, + "velocityY": 0.0028702714362588678, + "timestamp": 2.9587318262440365 }, { - "x": 2.5314177987448714, + "x": 2.5314177987448723, "y": 5.544014701821341, - "heading": -7.704513323430137e-18, - "angularVelocity": 7.904511385790122e-17, - "velocityX": 1.6247025332136762, - "velocityY": 0.002296217546131797, - "timestamp": 3.051830860451768 + "heading": -7.829457975228232e-21, + "angularVelocity": 1.0514195591633778e-19, + "velocityX": 1.6247025332136766, + "velocityY": 0.0022962175461363225, + "timestamp": 3.05183086028641 }, { - "x": 2.6448614816388756, - "y": 5.544175033555818, - "heading": -3.433430554917341e-18, - "angularVelocity": 4.587676781446345e-17, - "velocityX": 1.218526959607025, - "velocityY": 0.0017221632439692251, - "timestamp": 3.1449298944941413 + "x": 2.6448614816388765, + "y": 5.544175033555819, + "heading": 1.3151205701559364e-20, + "angularVelocity": 2.253585538517852e-19, + "velocityX": 1.2185269596070254, + "velocityY": 0.001722163243972619, + "timestamp": 3.1449298943287833 }, { - "x": 2.7204906055500317, + "x": 2.720490605550032, "y": 5.544281921381604, - "heading": -1.0420071252828256e-18, - "angularVelocity": 2.5686876928777523e-17, - "velocityX": 0.812351327691905, - "velocityY": 0.0011481088593983805, - "timestamp": 3.2380289285365147 + "heading": 9.829941746315052e-21, + "angularVelocity": -3.567452648039799e-20, + "velocityX": 0.8123513276919051, + "velocityY": 0.0011481088594006432, + "timestamp": 3.2380289283711567 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -1.1539777836664478e-35, - "angularVelocity": 1.1192459041074064e-17, - "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395097005, - "timestamp": 3.331127962578888 + "heading": -1.6124701211579614e-34, + "angularVelocity": -1.0558586184512984e-19, + "velocityX": 0.40617567078743816, + "velocityY": 0.000574054439510832, + "timestamp": 3.33112796241353 }, { "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, - "angularVelocity": 7.156872912951633e-35, - "velocityX": -1.751436869795613e-36, - "velocityY": -4.841667244220732e-36, - "timestamp": 3.4242269966212615 + "angularVelocity": 1.724633516740329e-33, + "velocityX": 6.381177903729197e-38, + "velocityY": -6.411016173939856e-36, + "timestamp": 3.4242269964559036 }, { "x": 2.7396220511907092, "y": 5.562601673573336, - "heading": 0.0037078860807813565, - "angularVelocity": 0.0478983711971462, - "velocityX": -0.2413479950107084, - "velocityY": 0.23596367181626482, - "timestamp": 3.5016385241556733 + "heading": 0.003707886080781361, + "angularVelocity": 0.04789837119714626, + "velocityX": -0.24134799501070836, + "velocityY": 0.2359636718162648, + "timestamp": 3.5016385239903154 }, { "x": 2.702380969511178, "y": 5.599259386918254, - "heading": 0.011292704027819494, - "angularVelocity": 0.09798047123752286, - "velocityX": -0.4810792767650385, - "velocityY": 0.47354334053959213, - "timestamp": 3.579050051690085 + "heading": 0.011292704027819503, + "angularVelocity": 0.09798047123752293, + "velocityX": -0.4810792767650384, + "velocityY": 0.473543340539592, + "timestamp": 3.579050051524727 }, { "x": 2.6467699433072402, "y": 5.6544931206396525, "heading": 0.023007945503191744, - "angularVelocity": 0.15133716965040475, + "angularVelocity": 0.1513371696504047, "velocityX": -0.7183817187849313, - "velocityY": 0.7135078647924301, - "timestamp": 3.656461579224497 + "velocityY": 0.7135078647924299, + "timestamp": 3.656461579059139 }, { "x": 2.573102880230356, "y": 5.72860253774289, - "heading": 0.03927230490424403, - "angularVelocity": 0.21010255086133378, - "velocityX": -0.9516291103303323, - "velocityY": 0.9573434275701879, - "timestamp": 3.733873106758909 + "heading": 0.03927230490424404, + "angularVelocity": 0.21010255086133403, + "velocityX": -0.9516291103303324, + "velocityY": 0.9573434275701875, + "timestamp": 3.733873106593551 }, { "x": 2.4820081686264115, "y": 5.82215722498829, - "heading": 0.060903903359814396, - "angularVelocity": 0.27943639848670454, - "velocityX": -1.1767589983733413, - "velocityY": 1.2085368965728291, - "timestamp": 3.8112846342933207 + "heading": 0.060903903359814444, + "angularVelocity": 0.2794363984867048, + "velocityX": -1.1767589983733415, + "velocityY": 1.208536896572829, + "timestamp": 3.8112846341279627 }, { "x": 2.3753503305689, "y": 5.936638406081269, - "heading": 0.09019255741494601, - "angularVelocity": 0.3783500337480349, - "velocityX": -1.3778030411568694, - "velocityY": 1.478864772976981, - "timestamp": 3.8886961618277325 + "heading": 0.09019255741494606, + "angularVelocity": 0.378350033748035, + "velocityX": -1.3778030411568696, + "velocityY": 1.4788647729769806, + "timestamp": 3.8886961616623745 }, { "x": 2.2779168004384966, - "y": 6.074876724828902, - "heading": 0.14378193450398408, + "y": 6.0748767248289015, + "heading": 0.1437819345039841, "angularVelocity": 0.6922661106928292, "velocityX": -1.25864368310122, - "velocityY": 1.7857588288279347, - "timestamp": 3.9661076893621443 + "velocityY": 1.7857588288279342, + "timestamp": 3.9661076891967864 }, { "x": 2.2032159820535764, "y": 6.200217981312059, - "heading": 0.2000311193060264, + "heading": 0.20003111930602646, "angularVelocity": 0.7266254341388357, - "velocityX": -0.9649831331866354, - "velocityY": 1.6191549304777415, - "timestamp": 4.043519216896556 + "velocityX": -0.9649831331866353, + "velocityY": 1.6191549304777408, + "timestamp": 4.043519216731198 }, { "x": 2.149623841782867, "y": 6.310133396102646, - "heading": 0.2561023628726984, + "heading": 0.25610236287269844, "angularVelocity": 0.7243267941166335, "velocityX": -0.6923018053982481, - "velocityY": 1.4198843284900602, - "timestamp": 4.120930744430967 + "velocityY": 1.4198843284900597, + "timestamp": 4.12093074426561 }, { "x": 2.1165539042639407, "y": 6.4038530923195935, - "heading": 0.3110558787207687, + "heading": 0.31105587872076873, "angularVelocity": 0.7098880179523891, "velocityX": -0.42719655033581183, - "velocityY": 1.2106684779639116, - "timestamp": 4.1983422719653785 + "velocityY": 1.2106684779639114, + "timestamp": 4.198342271800021 }, { "x": 2.1037067095365876, "y": 6.481006876335726, - "heading": 0.36442032903480814, - "angularVelocity": 0.6893605127520233, - "velocityX": -0.16595971086660646, - "velocityY": 0.9966704762651166, - "timestamp": 4.27575379949979 + "heading": 0.3644203290348082, + "angularVelocity": 0.6893605127520235, + "velocityX": -0.1659597108666065, + "velocityY": 0.9966704762651164, + "timestamp": 4.275753799334432 }, { "x": 2.1109008821512623, "y": 6.541377447734871, - "heading": 0.4159102858737852, - "angularVelocity": 0.6651458571992167, + "heading": 0.41591028587378526, + "angularVelocity": 0.6651458571992168, "velocityX": 0.09293412549541542, "velocityY": 0.7798653937207066, - "timestamp": 4.353165327034201 + "timestamp": 4.353165326868844 }, { "x": 2.1380148682221702, "y": 6.5848219723004595, "heading": 0.465333297367931, - "angularVelocity": 0.6384451136450677, + "angularVelocity": 0.6384451136450678, "velocityX": 0.3502577320781457, "velocityY": 0.5612151826648336, - "timestamp": 4.430576854568613 + "timestamp": 4.430576854403255 }, { "x": 2.1849615573883057, "y": 6.611239433288574, "heading": 0.5125504196, "angularVelocity": 0.6099494963599527, - "velocityX": 0.6064560493947925, - "velocityY": 0.34126004006795546, - "timestamp": 4.507988382103024 + "velocityX": 0.6064560493947926, + "velocityY": 0.3412600400679556, + "timestamp": 4.5079883819376665 }, { "x": 2.250707976446258, "y": 6.648678224064634, "heading": 0.55208552781375, - "angularVelocity": 0.5325541984016288, - "velocityX": 0.8856313560565373, + "angularVelocity": 0.5325541984016284, + "velocityX": 0.8856313560565378, "velocityY": 0.5043159387113171, - "timestamp": 4.582225160880485 + "timestamp": 4.582225160715128 }, { "x": 2.3371670619589073, "y": 6.698226995892447, - "heading": 0.5853560107794161, - "angularVelocity": 0.44816711492022887, - "velocityX": 1.1646395080237382, - "velocityY": 0.6674423734944788, - "timestamp": 4.656461939657946 + "heading": 0.585356010779416, + "angularVelocity": 0.44816711492022837, + "velocityX": 1.1646395080237388, + "velocityY": 0.6674423734944787, + "timestamp": 4.656461939492589 }, { "x": 2.4409073566894506, "y": 6.758690881227722, - "heading": 0.5617681472290976, - "angularVelocity": -0.31773824159353153, - "velocityX": 1.3974245170513666, - "velocityY": 0.8144734501011613, - "timestamp": 4.7306987184354075 + "heading": 0.5617681472290975, + "angularVelocity": -0.31773824159353026, + "velocityX": 1.3974245170513677, + "velocityY": 0.8144734501011609, + "timestamp": 4.73069871827005 }, { "x": 2.523886913207481, "y": 6.807069780587539, "heading": 0.5422957792802172, - "angularVelocity": -0.2623008200187737, - "velocityX": 1.1177688186980357, - "velocityY": 0.6516837092951017, - "timestamp": 4.804935497212869 + "angularVelocity": -0.262300820018773, + "velocityX": 1.1177688186980366, + "velocityY": 0.6516837092951012, + "timestamp": 4.804935497047511 }, { "x": 2.5861173110961473, "y": 6.843356427328165, "heading": 0.5274956073850987, - "angularVelocity": -0.1993644139582728, - "velocityX": 0.8382691021011789, - "velocityY": 0.4887960838037212, - "timestamp": 4.87917227599033 + "angularVelocity": -0.1993644139582724, + "velocityX": 0.8382691021011796, + "velocityY": 0.4887960838037209, + "timestamp": 4.879172275824972 }, { "x": 2.627602512984248, "y": 6.8675485030782895, "heading": 0.5175502299027057, - "angularVelocity": -0.13396833276139558, - "velocityX": 0.5588227637470666, - "velocityY": 0.3258772288954636, - "timestamp": 4.953409054767791 + "angularVelocity": -0.1339683327613954, + "velocityX": 0.558822763747067, + "velocityY": 0.3258772288954634, + "timestamp": 4.953409054602433 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.06734950498988643, - "velocityX": 0.2794033376916902, - "velocityY": 0.16294305705300594, - "timestamp": 5.027645833545252 + "angularVelocity": -0.06734950498988629, + "velocityX": 0.27940333769169035, + "velocityY": 0.16294305705300582, + "timestamp": 5.027645833379895 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -4.084231073354503e-32, - "velocityX": -1.1079933009257942e-33, - "velocityY": -1.0209152810451872e-34, - "timestamp": 5.101882612322713 - }, - { - "x": 2.6570277255317225, - "y": 6.867726793502589, - "heading": 0.5066730395869481, - "angularVelocity": -0.10087167460547103, - "velocityX": 0.14902725506901718, - "velocityY": -0.20454631284120778, - "timestamp": 5.160148523303567 - }, - { - "x": 2.6744037356282937, - "y": 6.843893366933042, - "heading": 0.49507655406382295, - "angularVelocity": -0.1990269323504748, - "velocityX": 0.2982191439910878, - "velocityY": -0.4090458068591583, - "timestamp": 5.218414434284421 - }, - { - "x": 2.7004835818665893, - "y": 6.808147821587349, - "heading": 0.4779456476925187, - "angularVelocity": -0.29401250375942084, - "velocityX": 0.44760042019879576, - "velocityY": -0.6134898561431401, - "timestamp": 5.276680345265275 - }, - { - "x": 2.735280123191829, - "y": 6.760494081762779, - "heading": 0.4554977531812837, - "angularVelocity": -0.38526634413407634, - "velocityX": 0.5972023905482879, - "velocityY": -0.8178665539139939, - "timestamp": 5.334946256246129 - }, - { - "x": 2.77880855180183, - "y": 6.700937036637074, - "heading": 0.4279915994110024, - "angularVelocity": -0.4720796999006835, - "velocityX": 0.7470650999399128, - "velocityY": -1.022159340223411, - "timestamp": 5.393212167226983 - }, - { - "x": 2.8310870878518544, - "y": 6.629482937316093, - "heading": 0.39573903851012515, - "angularVelocity": -0.5535408330177088, - "velocityX": 0.8972405162806606, - "velocityY": -1.2263448407158972, - "timestamp": 5.4514780782078365 - }, - { - "x": 2.8921379557757043, - "y": 6.546140000901142, - "heading": 0.3591220542576228, - "angularVelocity": -0.6284460954285072, - "velocityX": 1.0477973637777922, - "velocityY": -1.4303893136132022, - "timestamp": 5.50974398918869 - }, - { - "x": 2.9619888074879523, - "y": 6.4509193752842275, - "heading": 0.3186184742438743, - "angularVelocity": -0.6951505491273999, - "velocityX": 1.1988287926228676, - "velocityY": -1.6342424586513933, - "timestamp": 5.568009900169544 - }, - { - "x": 3.0406748905144103, - "y": 6.3438367809482505, - "heading": 0.27484324456730397, - "angularVelocity": -0.7513008711209391, - "velocityX": 1.3504651639672256, - "velocityY": -1.8378257978522827, - "timestamp": 5.626275811150398 - }, - { - "x": 3.12824252650645, - "y": 6.224915546783436, - "heading": 0.22861956489914778, - "angularVelocity": -0.7933228690673962, - "velocityX": 1.502896539639008, - "velocityY": -2.0410087504491825, - "timestamp": 5.684541722131252 - }, - { - "x": 3.224755029145573, - "y": 6.094192875417537, - "heading": 0.18111264418965425, - "angularVelocity": -0.815346742370587, - "velocityX": 1.6564145486515685, - "velocityY": -2.243553205730433, - "timestamp": 5.742807633112106 - }, - { - "x": 3.3303033780389395, - "y": 5.951734868373249, - "heading": 0.13411173965139422, - "angularVelocity": -0.8066621416715647, - "velocityX": 1.8114940128208097, - "velocityY": -2.444963180805693, - "timestamp": 5.80107354409296 - }, - { - "x": 3.4450256229008205, - "y": 5.797681352790493, - "heading": 0.09073046456880836, - "angularVelocity": -0.7445395489798686, - "velocityX": 1.9689427819909868, - "velocityY": -2.643973345467458, - "timestamp": 5.859339455073814 - }, - { - "x": 3.5691217392378216, - "y": 5.632434411388871, - "heading": 0.05763038975845234, - "angularVelocity": -0.5680864548952619, - "velocityX": 2.1298236695857957, - "velocityY": -2.8360826874554848, - "timestamp": 5.9176053660546675 - }, - { - "x": 3.7017085484583103, - "y": 5.4576517803314095, - "heading": 0.0547360803841417, - "angularVelocity": -0.049674146093104596, - "velocityX": 2.2755468332771343, - "velocityY": -2.999740810967421, - "timestamp": 5.975871277035521 - }, - { - "x": 3.8463662629346644, - "y": 5.2914222205797, - "heading": 0.05473605711060691, - "angularVelocity": -3.9943655557036433e-7, - "velocityX": 2.4827160863217212, - "velocityY": -2.8529470655034146, - "timestamp": 6.034137188016375 - }, - { - "x": 4.001863921193704, - "y": 5.135285744872655, - "heading": 0.05473603479828713, - "angularVelocity": -3.8293951656527263e-7, - "velocityX": 2.668758724293121, - "velocityY": -2.679722552666384, - "timestamp": 6.092403098997229 + "angularVelocity": -1.618919283202532e-33, + "velocityX": 2.4430876435253525e-35, + "velocityY": 8.017390657472539e-35, + "timestamp": 5.101882612157356 + }, + { + "x": 2.6570338601157935, + "y": 6.867732812321398, + "heading": 0.5065324038818937, + "angularVelocity": -0.10327871439636148, + "velocityX": 0.1491229424075764, + "velocityY": -0.20442985508511316, + "timestamp": 5.160152273561258 + }, + { + "x": 2.674422013328184, + "y": 6.84391114835803, + "heading": 0.49465795152131636, + "angularVelocity": -0.2037844750507175, + "velocityX": 0.2984083448136593, + "velocityY": -0.40881761433700353, + "timestamp": 5.218421934965161 + }, + { + "x": 2.7005198630420675, + "y": 6.808182791545169, + "heading": 0.47711568998903314, + "angularVelocity": -0.30105308851354295, + "velocityX": 0.4478805794491053, + "velocityY": -0.6131553874185988, + "timestamp": 5.276691596369063 + }, + { + "x": 2.7353400941220496, + "y": 6.760551298473928, + "heading": 0.4541277672109497, + "angularVelocity": -0.39450929049922157, + "velocityX": 0.5975705065217742, + "velocityY": -0.8174321237441035, + "timestamp": 5.3349612577729655 + }, + { + "x": 2.7788976924779565, + "y": 6.701021131245012, + "heading": 0.42595856862842546, + "angularVelocity": -0.4834282181127908, + "velocityX": 0.7475176156247499, + "velocityY": -1.021632283329671, + "timestamp": 5.393230919176868 + }, + { + "x": 2.831210629197317, + "y": 6.629598041190472, + "heading": 0.39292676107093794, + "angularVelocity": -0.5668783164625577, + "velocityX": 0.8977731371519051, + "velocityY": -1.2257337409164428, + "timestamp": 5.45150058058077 + }, + { + "x": 2.8923008190981245, + "y": 6.546289653631685, + "heading": 0.3554225990889349, + "angularVelocity": -0.6436310264794379, + "velocityX": 1.0484047517859123, + "velocityY": -1.4297043358691544, + "timestamp": 5.509770241984673 + }, + { + "x": 2.962195513842206, + "y": 6.451106403793388, + "heading": 0.31393409063739947, + "angularVelocity": -0.7120087443781998, + "velocityX": 1.1995040482490311, + "velocityY": -1.6334958457802513, + "timestamp": 5.568039903388575 + }, + { + "x": 3.0409294178085977, + "y": 6.344063134427028, + "heading": 0.26908904007938106, + "angularVelocity": -0.7696123416123889, + "velocityX": 1.351198927013472, + "velocityY": -1.8370326304863622, + "timestamp": 5.626309564792478 + }, + { + "x": 3.128548070700225, + "y": 6.22518206352159, + "heading": 0.22172760975049532, + "angularVelocity": -0.8127974178637293, + "velocityX": 1.5036753394582005, + "velocityY": -2.0401881191895193, + "timestamp": 5.68457922619638 + }, + { + "x": 3.2251135709999605, + "y": 6.094498947078054, + "heading": 0.17303894760412505, + "angularVelocity": -0.8355748252745036, + "velocityX": 1.6572174605646188, + "velocityY": -2.2427299780874135, + "timestamp": 5.742848887600283 + }, + { + "x": 3.3307147862388833, + "y": 5.952077971116604, + "heading": 0.12484956119030122, + "angularVelocity": -0.8270064601850674, + "velocityX": 1.8122846897450882, + "velocityY": -2.444170302865565, + "timestamp": 5.801118549004185 + }, + { + "x": 3.4454853336507822, + "y": 5.798056568785059, + "heading": 0.08033941158282583, + "angularVelocity": -0.7638649090295626, + "velocityX": 1.9696450030206056, + "velocityY": -2.643252056399113, + "timestamp": 5.859388210408087 + }, + { + "x": 3.5696119500976344, + "y": 5.632835617985738, + "heading": 0.04630921837828673, + "angularVelocity": -0.5840122009402995, + "velocityX": 2.1302100176360224, + "velocityY": -2.8354541079975273, + "timestamp": 5.91765787181199 + }, + { + "x": 3.7020235307305986, + "y": 5.458025004439994, + "heading": 0.04313298905641362, + "angularVelocity": -0.05450914327194612, + "velocityX": 2.272393170695454, + "velocityY": -3.00002796196164, + "timestamp": 5.975927533215892 + }, + { + "x": 3.84656727081631, + "y": 5.291677538591243, + "heading": 0.04313296518805293, + "angularVelocity": -4.0961900430587924e-7, + "velocityX": 2.480600309032019, + "velocityY": -2.8547868966613104, + "timestamp": 6.034197194619795 + }, + { + "x": 4.001959806149131, + "y": 5.135416436419877, + "heading": 0.04313294238925072, + "angularVelocity": -3.912636809189277e-7, + "velocityX": 2.666782877897643, + "velocityY": -2.6816888652951767, + "timestamp": 6.092466856023697 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.0547360118053875, - "angularVelocity": -3.946201001676767e-7, - "velocityX": 2.842745293382521, - "velocityY": -2.4943907433712678, - "timestamp": 6.150669009978083 - }, - { - "x": 4.269399861624288, - "y": 4.9075045981351995, - "heading": 0.054735989995402, - "angularVelocity": -6.29291522756053e-7, - "velocityX": 2.940181103085103, - "velocityY": -2.3787638667331756, - "timestamp": 6.185327009826526 - }, - { - "x": 4.374514707093794, - "y": 4.829200558555379, - "heading": 0.05473596913891511, - "angularVelocity": -6.017798771949806e-7, - "velocityX": 3.0329172464990677, - "velocityY": -2.2593352161762894, - "timestamp": 6.21998500967497 - }, - { - "x": 4.482675566428291, - "y": 4.7551608256734985, - "heading": 0.0547359490445404, - "angularVelocity": -5.797903745228634e-7, - "velocityX": 3.120805003389626, - "velocityY": -2.1362956086805514, - "timestamp": 6.254643009523413 - }, - { - "x": 4.593709527898866, - "y": 4.685503714898617, - "heading": 0.05473592954464412, - "angularVelocity": -5.62637669848401e-7, - "velocityX": 3.203703674652828, - "velocityY": -2.0098422032282808, - "timestamp": 6.289301009371856 - }, - { - "x": 4.707439069036232, - "y": 4.620340494746092, - "heading": 0.05473591048922828, - "angularVelocity": -5.498129124049459e-7, - "velocityX": 3.2814802249032198, - "velocityY": -1.8801783264319567, - "timestamp": 6.3239590092202995 - }, - { - "x": 4.823682298536361, - "y": 4.559775113095544, - "heading": 0.054735891740807854, - "angularVelocity": -5.409550611531198e-7, - "velocityX": 3.3540085985473036, - "velocityY": -1.747515203283397, - "timestamp": 6.358617009068743 - }, - { - "x": 4.942252940190184, - "y": 4.503903323846341, - "heading": 0.05473587317010921, - "angularVelocity": -5.358271892357539e-7, - "velocityX": 3.4211622763091856, - "velocityY": -1.6120892577045927, - "timestamp": 6.393275008917186 - }, - { - "x": 5.062124990174475, - "y": 4.450881200757823, - "heading": 0.05473585465308408, - "angularVelocity": -5.342785275702722e-7, - "velocityX": 3.4587122888938238, - "velocityY": -1.52986679324772, - "timestamp": 6.4279330087656295 - }, - { - "x": 5.1819977334562, - "y": 4.397860645099044, - "heading": 0.05473583613607813, - "angularVelocity": -5.3427797428813e-7, - "velocityX": 3.4587322928593447, - "velocityY": -1.529821567621728, - "timestamp": 6.462591008614073 - }, - { - "x": 5.301870477238367, - "y": 4.344840090571707, - "heading": 0.05473581761907218, - "angularVelocity": -5.342779742919752e-7, - "velocityX": 3.458732307298825, - "velocityY": -1.5298215349758078, - "timestamp": 6.497249008462516 - }, - { - "x": 5.421743975168144, - "y": 4.291821241109097, - "heading": 0.0547357991020742, - "angularVelocity": -5.342777443988636e-7, - "velocityX": 3.4587540669967396, - "velocityY": -1.5297723381170152, - "timestamp": 6.531907008310959 - }, - { - "x": 5.5428966637680075, - "y": 4.241794448876403, - "heading": 0.05473578057951323, - "angularVelocity": -5.34438255269497e-7, - "velocityX": 3.4956630252656034, - "velocityY": -1.4434414118372951, - "timestamp": 6.566565008159403 + "heading": 0.04313291889737663, + "angularVelocity": -4.031578960087653e-7, + "velocityX": 2.8409167869122447, + "velocityY": -2.496473071014189, + "timestamp": 6.1507365174276 + }, + { + "x": 4.2693517733895705, + "y": 4.907420739393082, + "heading": 0.043132896624531875, + "angularVelocity": -6.4257109962904e-7, + "velocityX": 2.9384484685182977, + "velocityY": -2.3809038315668807, + "timestamp": 6.185398587932365 + }, + { + "x": 4.374422288805421, + "y": 4.829031478695207, + "heading": 0.0431328753158102, + "angularVelocity": -6.147561689330783e-7, + "velocityX": 3.0312821445967915, + "velocityY": -2.261528511030489, + "timestamp": 6.220060658437131 + }, + { + "x": 4.482542607371009, + "y": 4.7549053274480135, + "heading": 0.043132854776618594, + "angularVelocity": -5.925552428867745e-7, + "velocityX": 3.1192689008789145, + "velocityY": -2.138537893661084, + "timestamp": 6.254722728941896 + }, + { + "x": 4.593539840912748, + "y": 4.685160765838758, + "heading": 0.04313283483631883, + "angularVelocity": -5.752772258439431e-7, + "velocityX": 3.2022678370145012, + "velocityY": -2.012129125398518, + "timestamp": 6.289384799446662 + }, + { + "x": 4.707236482283354, + "y": 4.619909225059653, + "heading": 0.04313281534202855, + "angularVelocity": -5.624098616258592e-7, + "velocityX": 3.280145695710103, + "velocityY": -1.882505569600442, + "timestamp": 6.324046869951427 + }, + { + "x": 4.823450643664921, + "y": 4.559254805625301, + "heading": 0.043132796153422026, + "angularVelocity": -5.53590891933908e-7, + "velocityX": 3.352776094711051, + "velocityY": -1.7498787161607805, + "timestamp": 6.358708940456193 + }, + { + "x": 4.9419959983331205, + "y": 4.5032933115020395, + "heading": 0.04313277713835443, + "angularVelocity": -5.485842974798401e-7, + "velocityX": 3.4200309716611494, + "velocityY": -1.6144879203210547, + "timestamp": 6.3933710109609585 + }, + { + "x": 5.061736542982073, + "y": 4.449937038090927, + "heading": 0.04313275817159016, + "angularVelocity": -5.471907475389428e-7, + "velocityX": 3.454512177294491, + "velocityY": -1.5393273579481606, + "timestamp": 6.428033081465724 + }, + { + "x": 5.1814776709209625, + "y": 4.396582073700789, + "heading": 0.04313273920484137, + "angularVelocity": -5.471903010583792e-7, + "velocityX": 3.45452900519684, + "velocityY": -1.5392895927207566, + "timestamp": 6.46269515197049 + }, + { + "x": 5.3012188001688525, + "y": 4.3432271122483534, + "heading": 0.043132720238092614, + "angularVelocity": -5.471902999394187e-7, + "velocityX": 3.454529042961442, + "velocityY": -1.5392895079680768, + "timestamp": 6.497357222475255 + }, + { + "x": 5.420963133587213, + "y": 4.289879342291843, + "heading": 0.04313270127136276, + "angularVelocity": -5.471897545932471e-7, + "velocityX": 3.4546214832116693, + "velocityY": -1.5390820334629507, + "timestamp": 6.532019292980021 + }, + { + "x": 5.5425254594597755, + "y": 4.240815191837089, + "heading": 0.04313268228677855, + "angularVelocity": -5.47704852461645e-7, + "velocityX": 3.5070705270145455, + "velocityY": -1.415499701554448, + "timestamp": 6.566681363484786 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.054735761954551196, - "angularVelocity": -5.373928706734811e-7, - "velocityX": 3.550552991831721, - "velocityY": -1.3025960678451716, - "timestamp": 6.601223008007846 - }, - { - "x": 5.798180192000891, - "y": 4.154397155999577, - "heading": 0.05473574376724012, - "angularVelocity": -4.955056404583076e-7, - "velocityX": 3.6025088632415367, - "velocityY": -1.15113574961205, - "timestamp": 6.637927557031803 - }, - { - "x": 5.93207840031246, - "y": 4.117779387908136, - "heading": 0.054735725716885786, - "angularVelocity": -4.917743118523635e-7, - "velocityX": 3.6480003670436316, - "velocityY": -0.9976356899941846, - "timestamp": 6.6746321060557605 - }, - { - "x": 6.067406110251661, - "y": 4.086860105171276, - "heading": 0.0547357076680171, - "angularVelocity": -4.917338358563753e-7, - "velocityX": 3.686946537631365, - "velocityY": -0.8423828533264233, - "timestamp": 6.711336655079718 - }, - { - "x": 6.203921243902459, - "y": 4.061696453595828, - "heading": 0.05473568948478555, - "angularVelocity": -4.953944957116945e-7, - "velocityX": 3.719297397216157, - "velocityY": -0.6855731031873736, - "timestamp": 6.748041204103675 - }, - { - "x": 6.341379198553944, - "y": 4.042334078664949, - "heading": 0.054735669406532635, - "angularVelocity": -5.47023555566929e-7, - "velocityX": 3.7449841588235726, - "velocityY": -0.5275197610585458, - "timestamp": 6.784745753127632 - }, - { - "x": 6.479285910845981, - "y": 4.028828408214569, - "heading": 0.05386070036293022, - "angularVelocity": -0.023838163575618188, - "velocityX": 3.7572103719902135, - "velocityY": -0.3679563108530375, - "timestamp": 6.8214503021515895 - }, - { - "x": 6.612620100679574, - "y": 4.0185345658565055, - "heading": 0.045620785713098924, - "angularVelocity": -0.22449300887617385, - "velocityX": 3.6326339208408243, - "velocityY": -0.2804514053923919, - "timestamp": 6.858154851175547 - }, - { - "x": 6.740581673709006, - "y": 4.010579483858113, - "heading": 0.035519685699448925, - "angularVelocity": -0.27520022128747124, - "velocityX": 3.4862592357669087, - "velocityY": -0.2167328630900888, - "timestamp": 6.894859400199504 - }, - { - "x": 6.863028396494948, - "y": 4.00465723612987, - "heading": 0.025264871332947036, - "angularVelocity": -0.27938810417772464, - "velocityX": 3.3360094604627837, - "velocityY": -0.16134914842236953, - "timestamp": 6.931563949223461 - }, - { - "x": 6.979909878212025, - "y": 4.000619227079583, - "heading": 0.015648348920582984, - "angularVelocity": -0.26199810835673765, - "velocityX": 3.184386808315938, - "velocityY": -0.1100138581637957, - "timestamp": 6.968268498247419 - }, - { - "x": 7.091201711972647, - "y": 3.998378156691166, - "heading": 0.00712732553533738, - "angularVelocity": -0.23215169813648398, - "velocityX": 3.0320992007824574, - "velocityY": -0.06105702012450316, - "timestamp": 7.004973047271376 + "heading": 0.043132663161499156, + "angularVelocity": -5.517639056314665e-7, + "velocityX": 3.560845257182681, + "velocityY": -1.274191548268104, + "timestamp": 6.601343433989552 + }, + { + "x": 5.795862627864808, + "y": 4.156159506730396, + "heading": 0.04313264445875193, + "angularVelocity": -5.198107758201059e-7, + "velocityX": 3.6106505851434836, + "velocityY": -1.1253380804221886, + "timestamp": 6.637323348406788 + }, + { + "x": 5.927341530201614, + "y": 4.121094637119033, + "heading": 0.043132625947930554, + "angularVelocity": -5.1447652596799e-7, + "velocityX": 3.654230546858154, + "velocityY": -0.9745678993212189, + "timestamp": 6.6733032628240245 + }, + { + "x": 6.059882410873037, + "y": 4.090286167435021, + "heading": 0.04313260749907939, + "angularVelocity": -5.127541702089576e-7, + "velocityX": 3.683746412913316, + "velocityY": -0.8562685649205634, + "timestamp": 6.709283177241261 + }, + { + "x": 6.193269302687495, + "y": 4.06337550162166, + "heading": 0.043132589025398085, + "angularVelocity": -5.134442814470016e-7, + "velocityX": 3.7072598413563593, + "velocityY": -0.7479357927674594, + "timestamp": 6.745263091658497 + }, + { + "x": 6.327657815035056, + "y": 4.042021941101089, + "heading": 0.043132570411953335, + "angularVelocity": -5.173287667087509e-7, + "velocityX": 3.7350981658583504, + "velocityY": -0.5934855840107985, + "timestamp": 6.781243006075734 + }, + { + "x": 6.462816636912207, + "y": 4.026263156605077, + "heading": 0.043132547717955015, + "angularVelocity": -6.30740753253067e-7, + "velocityX": 3.7565075978169236, + "velocityY": -0.4379883818862704, + "timestamp": 6.81722292049297 + }, + { + "x": 6.597942834210822, + "y": 4.016131027814117, + "heading": 0.04107885634805962, + "angularVelocity": -0.0570788286508975, + "velocityX": 3.755600853621886, + "velocityY": -0.2816051387300244, + "timestamp": 6.853202834910206 + }, + { + "x": 6.728400824069692, + "y": 4.008818936530008, + "heading": 0.03307687475358465, + "angularVelocity": -0.2224013515341097, + "velocityX": 3.625856035843536, + "velocityY": -0.20322703381989088, + "timestamp": 6.889182749327443 + }, + { + "x": 6.853599659736003, + "y": 4.0035443033890195, + "heading": 0.023873611715651655, + "angularVelocity": -0.25578890853403846, + "velocityX": 3.47968686680181, + "velocityY": -0.14659937986014848, + "timestamp": 6.925162663744679 + }, + { + "x": 6.973435503233387, + "y": 4.000040041798795, + "heading": 0.014893418694113961, + "angularVelocity": -0.24958906009058449, + "velocityX": 3.3306316993343295, + "velocityY": -0.09739493956513012, + "timestamp": 6.961142578161915 + }, + { + "x": 7.08787150697828, + "y": 3.998176312998215, + "heading": 0.006805585628585594, + "angularVelocity": -0.22478744589936686, + "velocityX": 3.1805524164913392, + "velocityY": -0.05179914490532842, + "timestamp": 6.997122492579152 }, { "x": 7.196889877319336, "y": 3.9978766441345215, - "heading": -3.597785008712915e-31, - "angularVelocity": -0.19418098641357107, - "velocityX": 2.87942961178202, - "velocityY": -0.013663498666533047, - "timestamp": 7.041677596295333 - }, - { - "x": 7.37119660747943, - "y": 4.00262802828422, - "heading": -0.0076676970964569885, - "angularVelocity": -0.11437965193876058, - "velocityX": 2.6001474595947878, - "velocityY": 0.07087677805125822, - "timestamp": 7.10871484432405 - }, - { - "x": 7.526209738417895, - "y": 4.010745052233057, - "heading": -0.011651875074249012, - "angularVelocity": -0.059432302114867865, - "velocityX": 2.3123432941647923, - "velocityY": 0.1210822965966538, - "timestamp": 7.1757520923527665 - }, - { - "x": 7.661711723306385, - "y": 4.020683594487604, - "heading": -0.013062756224948863, - "angularVelocity": -0.021046227167551262, - "velocityX": 2.0212939652660666, - "velocityY": 0.1482540311065431, - "timestamp": 7.242789340381483 - }, - { - "x": 7.777626027630173, - "y": 4.031355931329743, - "heading": -0.012680116865926582, - "angularVelocity": 0.005707861976350362, - "velocityX": 1.729102964879078, - "velocityY": 0.15920010376271715, - "timestamp": 7.3098265884102 - }, - { - "x": 7.873936260933145, - "y": 4.041961506483741, - "heading": -0.011077902998110952, - "angularVelocity": 0.023900352638719452, - "velocityX": 1.4366674667449708, - "velocityY": 0.15820421431164344, - "timestamp": 7.3768638364389165 - }, - { - "x": 7.950652622834928, - "y": 4.051889363613367, - "heading": -0.008694806000859931, - "angularVelocity": 0.035548848846393165, - "velocityX": 1.1443841171541105, - "velocityY": 0.14809464024199548, - "timestamp": 7.443901084467633 - }, - { - "x": 8.007796949441587, - "y": 4.060659248403223, - "heading": -0.005876390393834335, - "angularVelocity": 0.04204253142698025, - "velocityX": 0.8524264985069137, - "velocityY": 0.1308210740706325, - "timestamp": 7.51093833249635 - }, - { - "x": 8.045395699194648, - "y": 4.067884383860418, - "heading": -0.00290156854092843, - "angularVelocity": 0.04437565592835165, - "velocityX": 0.5608635625518943, - "velocityY": 0.1077779245069823, - "timestamp": 7.577975580525067 - }, - { - "x": 8.0634765625, + "heading": 3.395025904591232e-32, + "angularVelocity": -0.18914957800247972, + "velocityX": 3.0299785896330556, + "velocityY": -0.008328782003720711, + "timestamp": 7.033102406996388 + }, + { + "x": 7.373371840772393, + "y": 4.002118083585038, + "heading": -0.0072355781339062996, + "angularVelocity": -0.11324248534202788, + "velocityX": 2.7620814521804613, + "velocityY": 0.06638186139590158, + "timestamp": 7.096996961341894 + }, + { + "x": 7.532286444106961, + "y": 4.009181217967762, + "heading": -0.011195509678912387, + "angularVelocity": -0.06197604139459259, + "velocityX": 2.4871384574536473, + "velocityY": 0.11054360508613674, + "timestamp": 7.1608915156874 + }, + { + "x": 7.6734693827744955, + "y": 4.017827175121804, + "heading": -0.012870706885804134, + "angularVelocity": -0.02621815308129755, + "velocityX": 2.209623967390062, + "velocityY": 0.13531602563950743, + "timestamp": 7.224786070032906 + }, + { + "x": 7.7968608797168075, + "y": 4.027211909710403, + "heading": -0.012933711131137412, + "angularVelocity": -0.0009860659641287687, + "velocityX": 1.9311739193778674, + "velocityY": 0.14687847320841513, + "timestamp": 7.2886806243784115 + }, + { + "x": 7.9024437482359025, + "y": 4.036727334915007, + "heading": -0.011868630755415614, + "angularVelocity": 0.01666934508944925, + "velocityX": 1.6524548860324264, + "velocityY": 0.14892388407859264, + "timestamp": 7.352575178723917 + }, + { + "x": 7.99021929218393, + "y": 4.0459160326905685, + "heading": -0.010039813807728299, + "angularVelocity": 0.028622422777974377, + "velocityX": 1.3737562589980292, + "velocityY": 0.143810342989083, + "timestamp": 7.416469733069423 + }, + { + "x": 8.060196978947376, + "y": 4.0544220986291215, + "heading": -0.00773105502412215, + "angularVelocity": 0.03613388976972427, + "velocityX": 1.0952058040039734, + "velocityY": 0.13312661815523763, + "timestamp": 7.480364287414929 + }, + { + "x": 8.112389704520517, + "y": 4.061961050008052, + "heading": -0.005169479155646227, + "angularVelocity": 0.04009067587551167, + "velocityX": 0.816857181457298, + "velocityY": 0.11799051509403427, + "timestamp": 7.544258841760435 + }, + { + "x": 8.146811527228987, + "y": 4.068300458431988, + "heading": -0.0025408789062790695, + "angularVelocity": 0.04113966012116123, + "velocityX": 0.5387285827574291, + "velocityY": 0.09921672494429698, + "timestamp": 7.608153396105941 + }, + { + "x": 8.1634765625, "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 0.043282930404384794, - "velocityX": 0.26971368660010536, - "velocityY": 0.07999391635030323, - "timestamp": 7.645012828553783 - }, - { - "x": 8.059946431342839, - "y": 4.076566093038108, - "heading": 0.0028549561404960684, - "angularVelocity": 0.03895223157485018, - "velocityX": -0.048164132671898556, - "velocityY": 0.045285389048645555, - "timestamp": 7.718306604675698 - }, - { - "x": 8.033099887642729, - "y": 4.077511417867968, - "heading": 0.00539411397159862, - "angularVelocity": 0.03464356682727019, - "velocityX": -0.3662868134321987, - "velocityY": 0.012897750394093884, - "timestamp": 7.791600380797613 - }, - { - "x": 7.9829172655770035, - "y": 4.07628474252501, - "heading": 0.007619394466085314, - "angularVelocity": 0.03036111130070867, - "velocityX": -0.6846778092351761, - "velocityY": -0.01673641894119354, - "timestamp": 7.864894156919528 - }, - { - "x": 7.909377202284024, - "y": 4.0731292983210805, - "heading": 0.00953312535115696, - "angularVelocity": 0.026110414639960534, - "velocityX": -1.0033602740109082, - "velocityY": -0.04305200756310189, - "timestamp": 7.938187933041442 - }, - { - "x": 7.81245694795258, - "y": 4.068343833996595, - "heading": 0.011138189892816102, - "angularVelocity": 0.021899056462711293, - "velocityX": -1.3223531309156478, - "velocityY": -0.06529155103873027, - "timestamp": 8.011481709163357 - }, - { - "x": 7.692133347423482, - "y": 4.062303858835979, - "heading": 0.012438255979270885, - "angularVelocity": 0.01773774193721827, - "velocityX": -1.641661910404963, - "velocityY": -0.0824077497462929, - "timestamp": 8.084775485285272 - }, - { - "x": 7.548385409224001, - "y": 4.055495143379703, - "heading": 0.013438147372137587, - "angularVelocity": 0.013642241480415802, - "velocityX": -1.9612570917396126, - "velocityY": -0.0928962296191558, - "timestamp": 8.158069261407187 - }, - { - "x": 7.381200809360299, - "y": 4.048569614878887, - "heading": 0.014144485416621113, - "angularVelocity": 0.009637080825370731, - "velocityX": -2.2810204182359235, - "velocityY": -0.09448999447506434, - "timestamp": 8.231363037529102 - }, - { - "x": 7.190592956257051, - "y": 4.04244548860493, - "heading": 0.014566894857523213, - "angularVelocity": 0.00576323752509999, - "velocityX": -2.6006008038962976, - "velocityY": -0.08355588425094046, - "timestamp": 8.304656813651016 - }, - { - "x": 6.9766499613051005, - "y": 4.038503705706744, - "heading": 0.01472053802802316, - "angularVelocity": 0.002096264903098756, - "velocityX": -2.918979022121646, - "velocityY": -0.05378059511668448, - "timestamp": 8.377950589772931 - }, - { - "x": 6.739697889009689, - "y": 4.039020009641981, - "heading": 0.014632337527092197, - "angularVelocity": -0.0012033832283965633, - "velocityX": -3.232908506464046, - "velocityY": 0.007044308023879432, - "timestamp": 8.451244365894846 - }, - { - "x": 6.480981935824527, - "y": 4.048232688232657, - "heading": 0.01435998143770215, - "angularVelocity": -0.003715951118919239, - "velocityX": -3.529848874955213, - "velocityY": 0.12569523741486752, - "timestamp": 8.52453814201676 - }, - { - "x": 6.206211729235204, - "y": 4.074520375352005, - "heading": 0.014063196474504363, - "angularVelocity": -0.004049251913342814, - "velocityX": -3.7488886659663403, - "velocityY": 0.3586619288876869, - "timestamp": 8.597831918138676 - }, - { - "x": 5.93349789020665, - "y": 4.124154607710869, - "heading": 0.014063194566661911, - "angularVelocity": -2.6030074482550523e-8, - "velocityX": -3.7208321559927344, - "velocityY": 0.6771957318218086, - "timestamp": 8.67112569426059 + "heading": 2.9433151473989685e-31, + "angularVelocity": 0.03976675214822599, + "velocityX": 0.2608209015888442, + "velocityY": 0.07741657313776515, + "timestamp": 7.6720479504514465 + }, + { + "x": 8.160341731543628, + "y": 4.076804852758406, + "heading": 0.0025302388406713986, + "angularVelocity": 0.03600436134361471, + "velocityX": -0.0446074831711938, + "velocityY": 0.05062755463137832, + "timestamp": 7.7423238488772945 + }, + { + "x": 8.135735418270036, + "y": 4.078564432167465, + "heading": 0.004797198596772283, + "angularVelocity": 0.032257997505259615, + "velocityX": -0.3501387221617275, + "velocityY": 0.025038163132351486, + "timestamp": 7.8125997473031425 + }, + { + "x": 8.089649517071235, + "y": 4.078625238037987, + "heading": 0.006802207301300417, + "angularVelocity": 0.028530531084475955, + "velocityX": -0.6557853009510525, + "velocityY": 0.0008652450112215468, + "timestamp": 7.882875645728991 + }, + { + "x": 8.02207492391753, + "y": 4.077106587290107, + "heading": 0.008546856784408466, + "angularVelocity": 0.024825715817051033, + "velocityX": -0.9615614267102518, + "velocityY": -0.02160983753886332, + "timestamp": 7.953151544154839 + }, + { + "x": 7.933001430575272, + "y": 4.074154095631599, + "heading": 0.010033092526327095, + "angularVelocity": 0.021148584012580587, + "velocityX": -1.2674828118525576, + "velocityY": -0.042012862512522016, + "timestamp": 8.023427442580687 + }, + { + "x": 7.822417690835061, + "y": 4.069949420479082, + "heading": 0.011263348791088614, + "angularVelocity": 0.017506090883485915, + "velocityX": -1.5735656493512467, + "velocityY": -0.059830969744960986, + "timestamp": 8.093703341006535 + }, + { + "x": 7.690311409008121, + "y": 4.0647254734139295, + "heading": 0.012240761619989172, + "angularVelocity": 0.013908222460249003, + "velocityX": -1.879823449946105, + "velocityY": -0.07433483145952353, + "timestamp": 8.163979239432383 + }, + { + "x": 7.536670161450006, + "y": 4.058791543485583, + "heading": 0.012969524954057786, + "angularVelocity": 0.01037003226415625, + "velocityX": -2.186258034398947, + "velocityY": -0.08443762458060709, + "timestamp": 8.23425513785823 + }, + { + "x": 7.361484060419744, + "y": 4.0525779000493936, + "heading": 0.01345553350071711, + "angularVelocity": 0.006915721570918652, + "velocityX": -2.492833317742797, + "velocityY": -0.08841784417378167, + "timestamp": 8.304531036284079 + }, + { + "x": 7.164754272980972, + "y": 4.046723019235429, + "heading": 0.013707670109403618, + "angularVelocity": 0.0035878105343975153, + "velocityX": -2.799391994203425, + "velocityY": -0.08331278496768466, + "timestamp": 8.374806934709927 + }, + { + "x": 6.9465233889455575, + "y": 4.0422689491096495, + "heading": 0.013740797003253829, + "angularVelocity": 0.0004713834272096328, + "velocityX": -3.1053446334191217, + "velocityY": -0.06337976782296549, + "timestamp": 8.445082833135775 + }, + { + "x": 6.707012866607414, + "y": 4.041196732301159, + "heading": 0.013584512522768889, + "angularVelocity": -0.002223870259728453, + "velocityX": -3.408146003154458, + "velocityY": -0.015257247968487717, + "timestamp": 8.515358731561623 + }, + { + "x": 6.4476145619873, + "y": 4.048415292748879, + "heading": 0.013321412464940579, + "angularVelocity": -0.0037438163541362824, + "velocityX": -3.691141777345175, + "velocityY": 0.10271744096358496, + "timestamp": 8.58563462998747 + }, + { + "x": 6.1833350728544865, + "y": 4.076620284873478, + "heading": 0.013321410992019302, + "angularVelocity": -2.0959124115556836e-8, + "velocityX": -3.76059922466405, + "velocityY": 0.4013465890352113, + "timestamp": 8.655910528413319 + }, + { + "x": 5.922208724930001, + "y": 4.126140027657945, + "heading": 0.013321410307177713, + "angularVelocity": -9.745042100450887e-9, + "velocityX": -3.7157311933907815, + "velocityY": 0.7046475946048248, + "timestamp": 8.726186426839167 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.01406319364211473, - "angularVelocity": -1.2614265905768763e-8, - "velocityX": -3.6503257922039465, - "velocityY": 0.9890944453863133, - "timestamp": 8.744419470382505 - }, - { - "x": 5.5408846553597755, - "y": 4.23596798413579, - "heading": 0.01406319265930284, - "angularVelocity": -2.835157538408451e-8, - "velocityX": -3.6078608681964144, - "velocityY": 1.1342486182256648, - "timestamp": 8.779084630162778 - }, - { - "x": 5.417489605980159, - "y": 4.2802558166388724, - "heading": 0.014063191785023112, - "angularVelocity": -2.5220703836459854e-8, - "velocityX": -3.5596273076991327, - "velocityY": 1.2775891639848806, - "timestamp": 8.813749789943051 - }, - { - "x": 5.295963908009713, - "y": 4.329441746794644, - "heading": 0.014063190996401449, - "angularVelocity": -2.2749690676604448e-8, - "velocityX": -3.5057013653114657, - "velocityY": 1.4188865843266973, - "timestamp": 8.848414949723324 - }, - { - "x": 5.176501898681268, - "y": 4.383447115848144, - "heading": 0.014063190276173353, - "angularVelocity": -2.077671352365433e-8, - "velocityX": -3.446169297521196, - "velocityY": 1.5579149034885322, - "timestamp": 8.883080109503597 - }, - { - "x": 5.05929461402676, - "y": 4.442185556946357, - "heading": 0.01406318961105745, - "angularVelocity": -1.9186869677954374e-8, - "velocityX": -3.381126335416613, - "velocityY": 1.6944517628226294, - "timestamp": 8.91774526928387 - }, - { - "x": 4.944529482696556, - "y": 4.505563132440428, - "heading": 0.014063188990615918, - "angularVelocity": -1.7898129854823097e-8, - "velocityX": -3.310676542604923, - "velocityY": 1.828278764494154, - "timestamp": 8.952410429064143 - }, - { - "x": 4.832390024623587, - "y": 4.573478482189006, - "heading": 0.014063188406478968, - "angularVelocity": -1.685083682180617e-8, - "velocityX": -3.2349326754519327, - "velocityY": 1.9591817888352978, - "timestamp": 8.987075588844416 - }, - { - "x": 4.723055552753515, - "y": 4.64582297983034, - "heading": 0.014063187851803343, - "angularVelocity": -1.600095394099115e-8, - "velocityX": -3.1540160946348355, - "velocityY": 2.086951224223198, - "timestamp": 9.021740748624689 - }, - { - "x": 4.616700862639107, - "y": 4.722480877082754, - "heading": 0.014063187320883878, - "angularVelocity": -1.5315650301874817e-8, - "velocityX": -3.068057115228709, - "velocityY": 2.2113816217294997, - "timestamp": 9.056405908404962 - }, - { - "x": 4.508256477944758, - "y": 4.796152911545187, - "heading": 0.014063186782201262, - "angularVelocity": -1.5539597107094908e-8, - "velocityX": -3.128339386915466, - "velocityY": 2.125247220246655, - "timestamp": 9.091071068185235 - }, - { - "x": 4.396953351691581, - "y": 4.865430375769311, - "heading": 0.014063186216514915, - "angularVelocity": -1.6318584724554417e-8, - "velocityX": -3.2108066704055713, - "velocityY": 1.998475260556751, - "timestamp": 9.125736227965508 - }, - { - "x": 4.28296944450211, - "y": 4.930202424249945, - "heading": 0.014063179833248412, - "angularVelocity": -1.8414069176204727e-7, - "velocityX": -3.288140251248267, - "velocityY": 1.8685056953781265, - "timestamp": 9.16040138774578 + "heading": 0.013321409576784942, + "angularVelocity": -1.0393218545701755e-8, + "velocityX": -3.646442120972535, + "velocityY": 1.003317616364548, + "timestamp": 8.796462325265015 + }, + { + "x": 5.541094088534591, + "y": 4.236435199254689, + "heading": 0.013321408817775413, + "angularVelocity": -2.1905263169095435e-8, + "velocityX": -3.6034323243164974, + "velocityY": 1.148240568022246, + "timestamp": 8.831111967342002 + }, + { + "x": 5.41792618310833, + "y": 4.281179290629024, + "heading": 0.01332140812506875, + "angularVelocity": -1.9991740821363056e-8, + "velocityX": -3.5546660237528647, + "velocityY": 1.2913291073806714, + "timestamp": 8.865761609418989 + }, + { + "x": 5.2966447994258585, + "y": 4.330809856867523, + "heading": 0.01332140748575771, + "angularVelocity": -1.8450725633992864e-8, + "velocityX": -3.500220389376678, + "velocityY": 1.432354369728484, + "timestamp": 8.900411251495976 + }, + { + "x": 5.177443709175996, + "y": 4.385247597760623, + "heading": 0.013321406889674104, + "angularVelocity": -1.7203167808757175e-8, + "velocityX": -3.4401824407021615, + "velocityY": 1.5710910020988935, + "timestamp": 8.935060893572963 + }, + { + "x": 5.0605133585968645, + "y": 4.444405530588187, + "heading": 0.01332140632867156, + "angularVelocity": -1.6190716838073352e-8, + "velocityX": -3.3746481513237594, + "velocityY": 1.7073172847247369, + "timestamp": 8.96971053564995 + }, + { + "x": 4.946040562745207, + "y": 4.508189127302759, + "heading": 0.013321405796093607, + "angularVelocity": -1.5370373829355437e-8, + "velocityX": -3.3037223183234317, + "velocityY": 1.840815456992452, + "timestamp": 9.004360177726937 + }, + { + "x": 4.834208203167093, + "y": 4.576496460724576, + "heading": 0.013321405286393582, + "angularVelocity": -1.4710109443101504e-8, + "velocityX": -3.2275184641053225, + "velocityY": 1.9713719775242866, + "timestamp": 9.039009819803924 + }, + { + "x": 4.725194921038476, + "y": 4.649218348571895, + "heading": 0.013321404794856252, + "angularVelocity": -1.4185928002634242e-8, + "velocityX": -3.146158967137548, + "velocityY": 2.098777461704854, + "timestamp": 9.073659461880911 + }, + { + "x": 4.619174674827694, + "y": 4.72623832110653, + "heading": 0.013321404313366907, + "angularVelocity": -1.3895939972772152e-8, + "velocityX": -3.05977897189302, + "velocityY": 2.2228215911583726, + "timestamp": 9.108309103957899 + }, + { + "x": 4.510161328966249, + "y": 4.798960113344169, + "heading": 0.013321403820821752, + "angularVelocity": -1.421501423878401e-8, + "velocityX": -3.146160806487812, + "velocityY": 2.0987747023782037, + "timestamp": 9.142958746034886 + }, + { + "x": 4.398328904529435, + "y": 4.867267340500532, + "heading": 0.013321403310039242, + "angularVelocity": -1.4741350266093223e-8, + "velocityX": -3.2275203359486127, + "velocityY": 1.9713689106685317, + "timestamp": 9.177608388111873 + }, + { + "x": 4.283856051297854, + "y": 4.931050823285602, + "heading": 0.013321399241018467, + "angularVelocity": -1.1743327000811157e-7, + "velocityX": -3.3037239743267564, + "velocityY": 1.8408121689497896, + "timestamp": 9.21225803018886 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.013676554706226966, - "angularVelocity": -0.011153132697846535, - "velocityX": -3.331021112692202, - "velocityY": 1.7234990981340637, - "timestamp": 9.195066547526054 - }, - { - "x": 3.942209538282738, - "y": 5.088292732030669, - "heading": 0.012204358572730108, - "angularVelocity": -0.02138935040268071, - "velocityX": -3.2732028891433576, - "velocityY": 1.4288410708686299, - "timestamp": 9.263895011494382 - }, - { - "x": 3.7316526106102703, - "y": 5.172141912516415, - "heading": 0.010528895655226616, - "angularVelocity": -0.024342587657839698, - "velocityX": -3.05915482538382, - "velocityY": 1.2182340800795661, - "timestamp": 9.33272347546271 - }, - { - "x": 3.53847572791013, - "y": 5.244805689896845, - "heading": 0.00883705460181792, - "angularVelocity": -0.024580543511579803, - "velocityX": -2.806642362221417, - "velocityY": 1.0557227808231526, - "timestamp": 9.40155193943104 - }, - { - "x": 3.3636567049982276, - "y": 5.307973779066906, - "heading": 0.0072129294254924, - "angularVelocity": -0.0235967081449452, - "velocityX": -2.5399233519484916, - "velocityY": 0.9177611343924164, - "timestamp": 9.470380403399368 - }, - { - "x": 3.2076759026568156, - "y": 5.362638919945403, - "heading": 0.005703178674060931, - "angularVelocity": -0.02193497667078823, - "velocityX": -2.2662252409582635, - "velocityY": 0.7942228799941261, - "timestamp": 9.539208867367696 - }, - { - "x": 3.070812323282922, - "y": 5.409448485662979, - "heading": 0.00433725990535669, - "angularVelocity": -0.01984526008502491, - "velocityX": -1.9884735396226798, - "velocityY": 0.6800902274837387, - "timestamp": 9.608037331336025 - }, - { - "x": 2.953246241272587, - "y": 5.448856394346129, - "heading": 0.0031354038045633293, - "angularVelocity": -0.01746161444698809, - "velocityX": -1.7081026545127194, - "velocityY": 0.572552493707886, - "timestamp": 9.676865795304353 - }, - { - "x": 2.855102952665587, - "y": 5.48119799246298, - "heading": 0.002112343078266857, - "angularVelocity": -0.014863919188538589, - "velocityX": -1.4259113591749097, - "velocityY": 0.4698869661210702, - "timestamp": 9.745694259272682 - }, - { - "x": 2.7764742584987756, - "y": 5.506730918230691, - "heading": 0.0012792785012801142, - "angularVelocity": -0.012103489297248894, - "velocityX": -1.1423862982470774, - "velocityY": 0.3709646314271869, - "timestamp": 9.81452272324101 - }, - { - "x": 2.717430148167725, - "y": 5.5256592019268735, - "heading": 0.0006450106297669135, - "angularVelocity": -0.009215197244632127, - "velocityX": -0.8578443702916283, - "velocityY": 0.275006626689982, - "timestamp": 9.883351187209339 - }, - { - "x": 2.6780256614742735, - "y": 5.538148367640626, - "heading": 0.00021663624249770164, - "angularVelocity": -0.006223796995765153, - "velocityX": -0.5725027760547413, - "velocityY": 0.18145350039337144, - "timestamp": 9.952179651177667 + "heading": 0.012938020716139158, + "angularVelocity": -0.011064429584221965, + "velocityX": -3.358100659168625, + "velocityY": 1.6997858867187727, + "timestamp": 9.246907672265847 + }, + { + "x": 3.941212684187486, + "y": 5.0868100262217055, + "heading": 0.011498233293039487, + "angularVelocity": -0.020924944231025543, + "velocityX": -3.2887007006223063, + "velocityY": 1.4077333471091193, + "timestamp": 9.315714900959192 + }, + { + "x": 3.7303193403637978, + "y": 5.1698989262671065, + "heading": 0.009891075368403834, + "angularVelocity": -0.023357399435433027, + "velocityX": -3.0649881971497734, + "velocityY": 1.2075606244178938, + "timestamp": 9.384522129652536 + }, + { + "x": 3.5371027460588107, + "y": 5.242302097421373, + "heading": 0.008284033873001792, + "angularVelocity": -0.023355707327847468, + "velocityX": -2.808085690619801, + "velocityY": 1.0522611145545084, + "timestamp": 9.453329358345881 + }, + { + "x": 3.362389747323273, + "y": 5.305525449861525, + "heading": 0.006750320632579214, + "angularVelocity": -0.022290001640058982, + "velocityX": -2.5391663354759597, + "velocityY": 0.9188475344926457, + "timestamp": 9.522136587039226 + }, + { + "x": 3.2065870657576463, + "y": 5.360439069495437, + "heading": 0.00533021576404821, + "angularVelocity": -0.0206388906441795, + "velocityX": -2.2643359502240057, + "velocityY": 0.7980792233131078, + "timestamp": 9.59094381573257 + }, + { + "x": 3.0699321768781336, + "y": 5.407605853881433, + "heading": 0.00404906921437406, + "angularVelocity": -0.018619359826041904, + "velocityX": -1.9860542485811465, + "velocityY": 0.6854917031494734, + "timestamp": 9.659751044425915 + }, + { + "x": 2.952579395626419, + "y": 5.447418742231964, + "heading": 0.002924266712171353, + "angularVelocity": -0.01634715601198879, + "velocityX": -1.705529832842474, + "velocityY": 0.578614908732415, + "timestamp": 9.72855827311926 + }, + { + "x": 2.8546365398725873, + "y": 5.480167232965782, + "heading": 0.001968461119345437, + "angularVelocity": -0.013891063642247188, + "velocityX": -1.4234384615362952, + "velocityY": 0.47594549810702536, + "timestamp": 9.797365501812605 + }, + { + "x": 2.776182973714466, + "y": 5.506073331672824, + "heading": 0.0011912691725219405, + "angularVelocity": -0.01129520780857525, + "velocityX": -1.140193663485079, + "velocityY": 0.376502573915577, + "timestamp": 9.86617273050595 + }, + { + "x": 2.7172794523584742, + "y": 5.525312622061272, + "heading": 0.0006002459449936435, + "angularVelocity": -0.008589551399640344, + "velocityX": -0.8560658883459488, + "velocityY": 0.27961147039059914, + "timestamp": 9.934979959199294 + }, + { + "x": 2.6779739252366173, + "y": 5.5380274141109735, + "heading": 0.00020148371013726726, + "angularVelocity": -0.005795353808442843, + "velocityX": -0.5712412470066293, + "velocityY": 0.18478860856854942, + "timestamp": 10.00378718789264 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -3.0114656219279604e-34, - "angularVelocity": -0.003147480417366101, - "velocityX": -0.28651653960332735, - "velocityY": 0.08989010211866776, - "timestamp": 10.021008115145996 + "heading": -3.5000485154255244e-36, + "angularVelocity": -0.0029282346341141605, + "velocityX": -0.285853063090511, + "velocityY": 0.09167570478022308, + "timestamp": 10.072594416585984 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -4.388516089708894e-34, - "angularVelocity": -2.000698896485297e-33, - "velocityX": 9.851967688482278e-36, - "velocityY": -1.337407020908214e-34, - "timestamp": 10.089836579114325 + "heading": 7.813037085898689e-36, + "angularVelocity": 1.6441710918960667e-34, + "velocityX": -8.361910991338164e-36, + "velocityY": -1.503965302427871e-35, + "timestamp": 10.141401645279329 }, { "x": 2.6750735555579293, "y": 5.538716972759925, - "heading": 4.633169711684461e-19, - "angularVelocity": 7.277213797999712e-18, + "heading": 1.350818917618456e-19, + "angularVelocity": 2.121700407628309e-18, "velocityX": 0.2633772294028884, - "velocityY": -0.0882468077495622, - "timestamp": 10.15350338677302 + "velocityY": -0.088246807749562, + "timestamp": 10.205068452938024 }, { "x": 2.7085920650608806, "y": 5.52742596598787, - "heading": -4.184429803904338e-20, - "angularVelocity": -7.934452625857426e-18, + "heading": 3.9875003320592027e-19, + "angularVelocity": 4.14137524936932e-18, "velocityX": 0.5264675697678677, - "velocityY": -0.1773452633683705, - "timestamp": 10.217170194431715 + "velocityY": -0.17734526336837014, + "timestamp": 10.268735260596719 }, { "x": 2.7588390755328, "y": 5.51039889888726, - "heading": 3.955178903113731e-20, - "angularVelocity": 1.278469740567629e-18, - "velocityX": 0.7892183120172118, - "velocityY": -0.2674402522565472, - "timestamp": 10.28083700209041 + "heading": 7.83389944204561e-19, + "angularVelocity": 6.041451191657363e-18, + "velocityX": 0.7892183120172117, + "velocityY": -0.2674402522565467, + "timestamp": 10.332402068255414 }, { "x": 2.8257885982701296, "y": 5.487560532002736, - "heading": 8.970064045939625e-19, - "angularVelocity": 1.3467843717867433e-17, - "velocityX": 1.0515608556382, - "velocityY": -0.3587170100777762, - "timestamp": 10.344503809749105 + "heading": 1.2799519248933233e-18, + "angularVelocity": 7.799385566035206e-18, + "velocityX": 1.0515608556381997, + "velocityY": -0.35871701007777557, + "timestamp": 10.396068875914109 }, { "x": 2.9094088162269203, "y": 5.4588202139112365, - "heading": 2.18904914594727e-18, - "angularVelocity": 2.0293820106069983e-17, + "heading": 9.407556731948414e-19, + "angularVelocity": -5.3276780189270054e-18, "velocityX": 1.313403656188678, - "velocityY": -0.4514176090871547, - "timestamp": 10.4081706174078 + "velocityY": -0.45141760908715395, + "timestamp": 10.459735683572804 }, { "x": 3.0096598962036025, - "y": 5.424066617313393, - "heading": 3.904306808877207e-18, - "angularVelocity": 2.694116017446798e-17, + "y": 5.424066617313392, + "heading": 6.890803629951045e-19, + "angularVelocity": -3.9530065893820216e-18, "velocityX": 1.5746208057754196, - "velocityY": -0.5458668005493709, - "timestamp": 10.471837425066495 + "velocityY": -0.5458668005493699, + "timestamp": 10.523402491231499 }, { "x": 3.126490562172417, - "y": 5.383159777949491, - "heading": 4.8450172295989666e-18, - "angularVelocity": 1.4775523625508697e-17, - "velocityX": 1.8350325745107399, - "velocityY": -0.6425143786570184, - "timestamp": 10.53550423272519 - }, - { - "x": 3.2598324200617776, - "y": 5.335918462965805, - "heading": 4.235614277481238e-18, - "angularVelocity": -9.5717529200556e-18, + "y": 5.38315977794949, + "heading": 5.07923477935949e-19, + "angularVelocity": -2.8453897991917994e-18, + "velocityX": 1.8350325745107396, + "velocityY": -0.6425143786570174, + "timestamp": 10.587069298890194 + }, + { + "x": 3.259832420061777, + "y": 5.335918462965804, + "heading": 3.751534220684201e-19, + "angularVelocity": -2.0853889294918685e-18, "velocityX": 2.0943700931917317, - "velocityY": -0.7420085397863493, - "timestamp": 10.599171040383885 + "velocityY": -0.7420085397863481, + "timestamp": 10.650736106548889 }, { - "x": 3.4095898607657253, - "y": 5.282098877479993, - "heading": 2.3874123899446895e-18, - "angularVelocity": -2.902928473254694e-17, + "x": 3.409589860765725, + "y": 5.282098877479992, + "heading": 4.172337353071023e-18, + "angularVelocity": 5.964150034596624e-17, "velocityX": 2.3522059014921575, - "velocityY": -0.8453319314253612, - "timestamp": 10.66283784804258 + "velocityY": -0.8453319314253599, + "timestamp": 10.714402914207584 }, { - "x": 3.5756203018815462, - "y": 5.22135582426949, - "heading": 2.5233707069051096e-18, - "angularVelocity": 2.135466217958737e-18, - "velocityX": 2.607802200573299, - "velocityY": -0.9540772569615117, - "timestamp": 10.726504655701275 + "x": 3.575620301881546, + "y": 5.221355824269489, + "heading": 4.9261514224901164e-18, + "angularVelocity": 1.1839985341500765e-17, + "velocityX": 2.6078022005732984, + "velocityY": -0.9540772569615101, + "timestamp": 10.778069721866279 }, { - "x": 3.757690030648967, + "x": 3.7576900306489667, "y": 5.153163870724839, - "heading": 3.693577308512614e-18, - "angularVelocity": 1.8380167698697197e-17, + "heading": 5.589206376297959e-18, + "angularVelocity": 1.041445265109494e-17, "velocityX": 2.8597276267323055, - "velocityY": -1.0710754324327665, - "timestamp": 10.79017146335997 + "velocityY": -1.071075432432765, + "timestamp": 10.841736529524974 }, { - "x": 3.955353841452516, + "x": 3.9553538414525153, "y": 5.076631094268888, - "heading": 4.677490606421779e-18, - "angularVelocity": 1.5454101345613767e-17, - "velocityX": 3.1046603100187773, - "velocityY": -1.2020828320186725, - "timestamp": 10.853838271018665 + "heading": 6.855394728324636e-18, + "angularVelocity": 1.9887731120656582e-17, + "velocityX": 3.104660310018777, + "velocityY": -1.2020828320186703, + "timestamp": 10.90540333718367 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 5.777516683198087e-18, - "angularVelocity": 1.7277858231456074e-17, + "heading": 8.923179883172485e-18, + "angularVelocity": 3.2478228937327026e-17, "velocityX": 3.332116557247906, - "velocityY": -1.36151476081006, - "timestamp": 10.91750507867736 + "velocityY": -1.3615147608100575, + "timestamp": 10.969070144842364 }, { "x": 4.285701077560922, "y": 4.938388492372474, - "heading": 6.5114072312405955e-18, - "angularVelocity": 2.119278869815072e-17, - "velocityX": 3.413356765694629, - "velocityY": -1.4888942599373958, - "timestamp": 10.952134336699762 + "heading": 1.0772342870449973e-17, + "angularVelocity": 5.3398862490245696e-17, + "velocityX": 3.4133567656946298, + "velocityY": -1.4888942599373933, + "timestamp": 11.003699402864767 }, { "x": 4.403715149774036, "y": 4.881600772971698, - "heading": 6.4908057437571035e-18, - "angularVelocity": -5.949156482118572e-19, - "velocityX": 3.40792956455403, - "velocityY": -1.6398768741750078, - "timestamp": 10.986763594722165 + "heading": 9.945107603157448e-18, + "angularVelocity": -2.3888333580736016e-17, + "velocityX": 3.4079295645540313, + "velocityY": -1.6398768741750054, + "timestamp": 11.038328660887169 }, { "x": 4.519367006147829, "y": 4.8201449517293415, - "heading": 6.136184222547225e-18, - "angularVelocity": -1.0240517454358069e-17, - "velocityX": 3.3397151131270637, - "velocityY": -1.7746791225673975, - "timestamp": 11.021392852744567 + "heading": 1.0140262228054944e-17, + "angularVelocity": 5.6355416212281845e-18, + "velocityX": 3.3397151131270646, + "velocityY": -1.7746791225673952, + "timestamp": 11.072957918909571 }, { "x": 4.6324721141561005, - "y": 4.754119165100519, - "heading": 6.684498529619524e-18, - "angularVelocity": 1.5833845088958816e-17, - "velocityX": 3.2661718577713303, - "velocityY": -1.906647453610145, - "timestamp": 11.05602211076697 + "y": 4.75411916510052, + "heading": 1.0958701743566334e-17, + "angularVelocity": 2.3634335883891303e-17, + "velocityX": 3.2661718577713317, + "velocityY": -1.906647453610143, + "timestamp": 11.107587176931974 }, { "x": 4.742850082889288, - "y": 4.683628924272633, - "heading": 1.0669160659402455e-17, - "angularVelocity": 1.1506634439597858e-16, - "velocityX": 3.187419397256002, - "velocityY": -2.03556890483432, - "timestamp": 11.090651368789372 + "y": 4.683628924272634, + "heading": 1.1951109976942982e-17, + "angularVelocity": 2.8658085389373404e-17, + "velocityX": 3.1874193972560034, + "velocityY": -2.0355689048343186, + "timestamp": 11.142216434954376 }, { "x": 4.850325220314879, - "y": 4.608787325463071, - "heading": 1.658478005292643e-17, - "angularVelocity": 1.708272059914504e-16, - "velocityX": 3.1035934225348787, - "velocityY": -2.161224440937961, - "timestamp": 11.125280626811774 + "y": 4.608787325463072, + "heading": 1.1732443750037932e-17, + "angularVelocity": -6.3144935638988695e-18, + "velocityX": 3.1035934225348796, + "velocityY": -2.1612244409379597, + "timestamp": 11.176845692976778 }, { "x": 4.959315849281407, - "y": 4.536170434444089, - "heading": 2.4155748869411193e-17, - "angularVelocity": 2.1862925308959841e-16, - "velocityX": 3.1473567494868284, - "velocityY": -2.0969808527807587, - "timestamp": 11.159909884834176 + "y": 4.53617043444409, + "heading": 1.1207275250284524e-17, + "angularVelocity": -1.516545631482102e-17, + "velocityX": 3.147356749486827, + "velocityY": -2.096980852780761, + "timestamp": 11.21147495099918 }, { "x": 5.0711194796065495, - "y": 4.467963995966004, - "heading": 2.3436163975709297e-17, - "angularVelocity": -2.0779679808224236e-17, - "velocityX": 3.228588676454313, - "velocityY": -1.969618824462286, - "timestamp": 11.194539142856579 + "y": 4.467963995966005, + "heading": 1.0229768329797992e-17, + "angularVelocity": -2.822777547974504e-17, + "velocityX": 3.228588676454311, + "velocityY": -1.969618824462288, + "timestamp": 11.246104209021583 }, { "x": 5.1855579490393096, - "y": 4.4042772106470105, - "heading": 2.204718666214906e-17, - "angularVelocity": -4.0109935727230666e-17, - "velocityX": 3.3046757559381796, - "velocityY": -1.8391033754691897, - "timestamp": 11.229168400878981 + "y": 4.404277210647011, + "heading": 8.869711544920542e-18, + "angularVelocity": -3.9274788503918017e-17, + "velocityX": 3.3046757559381783, + "velocityY": -1.8391033754691917, + "timestamp": 11.280733467043985 }, { "x": 5.302448702578612, "y": 4.345211810292891, - "heading": 2.1117777344899902e-17, - "angularVelocity": -2.6838845829382515e-17, - "velocityX": 3.375491137109673, - "velocityY": -1.7056501850518673, - "timestamp": 11.263797658901384 + "heading": 8.222829686597657e-18, + "angularVelocity": -1.8680211337603908e-17, + "velocityX": 3.3754911371096723, + "velocityY": -1.705650185051869, + "timestamp": 11.315362725066388 }, { "x": 5.421605230513571, "y": 4.290862095789443, - "heading": 1.9153982489912048e-17, - "angularVelocity": -5.670912306921066e-17, - "velocityX": 3.440920618566971, - "velocityY": -1.569473838229202, - "timestamp": 11.298426916923786 + "heading": 7.194520598849692e-18, + "angularVelocity": -2.969480567798286e-17, + "velocityX": 3.4409206185669707, + "velocityY": -1.5694738382292037, + "timestamp": 11.34999198308879 }, { "x": 5.542837392691082, "y": 4.241314822773421, - "heading": 1.5300631909411777e-17, - "angularVelocity": -1.112744193943597e-16, - "velocityX": 3.5008593628856843, - "velocityY": -1.430792221536174, - "timestamp": 11.333056174946188 + "heading": 5.567604077424851e-18, + "angularVelocity": -4.698098123766793e-17, + "velocityX": 3.500859362885684, + "velocityY": -1.4307922215361755, + "timestamp": 11.384621241111192 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 9.183754863330666e-18, - "angularVelocity": -1.766389866662468e-16, - "velocityX": 3.555211493416193, - "velocityY": -1.2898268911821011, - "timestamp": 11.36768543296859 + "heading": 4.441119305733273e-18, + "angularVelocity": -3.252985585087723e-17, + "velocityX": 3.5552114934161922, + "velocityY": -1.2898268911821025, + "timestamp": 11.419250499133595 }, { "x": 5.934272395491136, - "y": 4.124337934678792, - "heading": -1.4789774171207362e-18, - "angularVelocity": -1.4511299214964166e-16, - "velocityX": 3.651673302114514, - "velocityY": -0.9841085377661543, - "timestamp": 11.441164258830412 + "y": 4.124337934678793, + "heading": 1.9097100466976255e-18, + "angularVelocity": -3.445086702659097e-17, + "velocityX": 3.6516733021145136, + "velocityY": -0.9841085377661556, + "timestamp": 11.492729324995416 }, { "x": 6.207753017493673, "y": 4.075010140740357, - "heading": -1.5184517487424443e-17, - "angularVelocity": -1.8652366732257328e-16, - "velocityX": 3.7218970063133985, - "velocityY": -0.6713198443208211, - "timestamp": 11.514643084692233 + "heading": -4.66846161540337e-18, + "angularVelocity": -8.952472477542546e-17, + "velocityX": 3.721897006313398, + "velocityY": -0.6713198443208224, + "timestamp": 11.566208150857237 }, { "x": 6.4844286250858945, "y": 4.049019985828209, - "heading": -2.210208594102609e-17, - "angularVelocity": -9.414369884747846e-17, + "heading": -6.502926410881733e-18, + "angularVelocity": -2.4965896963679208e-17, "velocityX": 3.7653787243758563, - "velocityY": -0.35370944768528717, - "timestamp": 11.588121910554054 + "velocityY": -0.3537094476852885, + "timestamp": 11.639686976719059 }, { "x": 6.743277363747609, "y": 4.038426394199003, - "heading": -1.998169117841033e-17, - "angularVelocity": 2.885722162467868e-17, - "velocityX": 3.522766397335807, - "velocityY": -0.14417203194192602, - "timestamp": 11.661600736415876 + "heading": -5.737032029312171e-18, + "angularVelocity": 1.0423334512854795e-17, + "velocityX": 3.5227663973358068, + "velocityY": -0.1441720319419282, + "timestamp": 11.71316580258088 }, { "x": 6.978672822837657, - "y": 4.030025299692623, - "heading": -1.7754526303263836e-17, - "angularVelocity": 3.031029482336482e-17, - "velocityX": 3.2035822065626665, - "velocityY": -0.1143335431376982, - "timestamp": 11.735079562277697 + "y": 4.030025299692622, + "heading": -4.2593802240854646e-18, + "angularVelocity": 2.010989952405427e-17, + "velocityX": 3.2035822065626656, + "velocityY": -0.11433354313770064, + "timestamp": 11.786644628442701 }, { "x": 7.19055269256627, "y": 4.022995759457438, - "heading": -1.4995601737739213e-17, - "angularVelocity": 3.754720537740805e-17, - "velocityX": 2.8835500192539434, - "velocityY": -0.095667563447506, - "timestamp": 11.808558388139518 + "heading": -3.134299993682012e-18, + "angularVelocity": 1.5311625045821916e-17, + "velocityX": 2.883550019253943, + "velocityY": -0.09566756344750842, + "timestamp": 11.860123454304523 }, { "x": 7.3789017577018114, "y": 4.017045849522277, - "heading": -1.2218093470845859e-17, - "angularVelocity": 3.780011771168623e-17, - "velocityX": 2.5633107623376494, - "velocityY": -0.08097448299391073, - "timestamp": 11.88203721400134 + "heading": -2.330563766445784e-18, + "angularVelocity": 1.0938337919929083e-17, + "velocityX": 2.5633107623376485, + "velocityY": -0.080974482993913, + "timestamp": 11.933602280166344 }, { "x": 7.54371362899692, "y": 4.012025997027571, - "heading": -9.378231648873842e-18, - "angularVelocity": 3.864870986523972e-17, - "velocityX": 2.2429845518359373, - "velocityY": -0.06831699385270533, - "timestamp": 11.955516039863161 + "heading": -1.6314906834703783e-18, + "angularVelocity": 9.513939216857223e-18, + "velocityX": 2.242984551835937, + "velocityY": -0.06831699385270738, + "timestamp": 12.007081106028165 }, { "x": 7.684984887810538, "y": 4.007845278835319, - "heading": -6.610589160747635e-18, - "angularVelocity": 3.7665850749041794e-17, - "velocityX": 1.9226118158077508, - "velocityY": -0.05689691068436128, - "timestamp": 12.028994865724982 + "heading": -1.3889434062297666e-18, + "angularVelocity": 3.300913894524217e-18, + "velocityX": 1.9226118158077503, + "velocityY": -0.05689691068436309, + "timestamp": 12.080559931889987 }, { "x": 7.8027134339680275, "y": 4.0044425825275765, - "heading": -4.4053913927362146e-18, - "angularVelocity": 3.0011336492479294e-17, - "velocityX": 1.6022104977409417, - "velocityY": -0.046308528584020826, - "timestamp": 12.102473691586804 + "heading": -1.0305851069387887e-18, + "angularVelocity": 4.877028110994567e-18, + "velocityX": 1.6022104977409415, + "velocityY": -0.04630852858402237, + "timestamp": 12.154038757751808 }, { "x": 7.896897856926731, "y": 4.001774011196325, - "heading": -2.659547727350374e-18, - "angularVelocity": 2.375981985162547e-17, - "velocityX": 1.281789983087365, - "velocityY": -0.03631755543113615, - "timestamp": 12.175952517448625 + "heading": -6.085636422043956e-19, + "angularVelocity": 5.743443227141563e-18, + "velocityX": 1.2817899830873647, + "velocityY": -0.036317555431137405, + "timestamp": 12.22751758361363 }, { "x": 7.967537148527336, "y": 3.999806507404438, - "heading": -1.3934136908294963e-18, - "angularVelocity": 1.723127746899314e-17, - "velocityX": 0.9613557480279207, - "velocityY": -0.026776472933672534, - "timestamp": 12.249431343310446 + "heading": -1.6215491216433702e-19, + "angularVelocity": 6.07533836862799e-18, + "velocityX": 0.9613557480279205, + "velocityY": -0.026776472933673492, + "timestamp": 12.30099640947545 }, { "x": 8.014630554462236, "y": 3.9985142796609896, - "heading": -4.425448526284405e-19, - "angularVelocity": 1.2940719003719293e-17, - "velocityX": 0.6409112473226126, - "velocityY": -0.017586396193627105, - "timestamp": 12.322910169172268 + "heading": 9.722147651876556e-20, + "angularVelocity": 3.5299473779135435e-18, + "velocityX": 0.6409112473226125, + "velocityY": -0.017586396193627746, + "timestamp": 12.374475235337272 }, { "x": 8.038177490234375, "y": 3.9978766441345215, - "heading": 3.3992773241588707e-43, - "angularVelocity": 6.022753459080235e-18, - "velocityX": 0.320458791984776, - "velocityY": -0.008677813220192955, - "timestamp": 12.396388995034089 + "heading": 9.32148398835055e-44, + "angularVelocity": -1.3231223468593855e-18, + "velocityX": 0.32045879198477595, + "velocityY": -0.00867781322019328, + "timestamp": 12.447954061199093 }, { "x": 8.038177490234375, "y": 3.9978766441345215, - "heading": -7.143469642081013e-44, - "angularVelocity": -5.598380381353948e-42, - "velocityX": -5.65394748040853e-41, - "velocityY": -2.108446218115532e-39, - "timestamp": 12.46986782089591 + "heading": -1.3950188036384555e-43, + "angularVelocity": -3.1671262842054474e-42, + "velocityX": -2.982658307493247e-43, + "velocityY": -2.0280554768402976e-41, + "timestamp": 12.521432887060914 } ], "constraints": [ @@ -14586,7 +14604,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 18 }, { "x": 2.3639590740203857, @@ -14595,7 +14613,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 21 }, { "x": 5.5030035972595215, @@ -14604,7 +14622,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { "x": 7.626483917236328, @@ -14613,7 +14631,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 8 }, { "x": 8.306674003601074, @@ -14622,7 +14640,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 18 }, { "x": 5.5030035972595215, @@ -14631,7 +14649,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 22 + "controlIntervalCount": 21 }, { "x": 2.3639590740203857, @@ -14640,7 +14658,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 19 }, { "x": 5.5030035972595215, @@ -14649,7 +14667,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 13 }, { "x": 7.536985397338867, @@ -14661,13 +14679,13 @@ "controlIntervalCount": 9 }, { - "x": 8.288774490356445, - "y": 0.7401233315467834, + "x": 8.385421752929688, + "y": 0.6878466606140137, "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 17 }, { "x": 5.5030035972595215, @@ -14676,7 +14694,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 22 + "controlIntervalCount": 21 }, { "x": 2.37211537361145, @@ -14685,7 +14703,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 21 }, { "x": 5.5030035972595215, @@ -14694,7 +14712,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 16 + "controlIntervalCount": 15 }, { "x": 8.063037872314453, @@ -14710,6530 +14728,7350 @@ { "x": 0.433241993188858, "y": 4.121134281158447, - "heading": -6.349582694240188e-38, + "heading": 8.991694014476798e-34, "angularVelocity": 0, - "velocityX": -2.609211774957871e-36, - "velocityY": -5.016830702674688e-35, + "velocityX": -4.6784915292932193e-35, + "velocityY": -2.119592179507727e-34, "timestamp": 0 }, { - "x": 0.45469188032137553, - "y": 4.109218526997661, - "heading": -0.008937300145995164, - "angularVelocity": -0.11893237262991438, - "velocityX": 0.2854425752342375, - "velocityY": -0.1585678997982562, - "timestamp": 0.07514606787342622 - }, - { - "x": 0.4975916635116251, - "y": 4.0853873108981436, - "heading": -0.026812413296636577, - "angularVelocity": -0.2378715700833438, - "velocityX": 0.570885269240018, - "velocityY": -0.31713191087600345, - "timestamp": 0.15029213574685243 - }, - { - "x": 0.5619414537480725, - "y": 4.049641027140498, - "heading": -0.05362171709802473, - "angularVelocity": -0.35676256336585627, - "velocityX": 0.856329440215504, - "velocityY": -0.4756906750976657, - "timestamp": 0.22543820362027867 - }, - { - "x": 0.6477414523773106, - "y": 4.001980177860296, - "heading": -0.08935853104911506, - "angularVelocity": -0.4755646564406316, - "velocityX": 1.141776290593902, - "velocityY": -0.6342427571923069, - "timestamp": 0.30058427149370487 - }, - { - "x": 0.7549919358777414, - "y": 3.942405375853288, - "heading": -0.13401442200653152, - "angularVelocity": -0.5942545261667384, - "velocityX": 1.4272268201854585, - "velocityY": -0.7927866845588475, - "timestamp": 0.37573033936713107 - }, - { - "x": 0.8836932393269182, - "y": 3.8709173416225418, - "heading": -0.18758057158075755, - "angularVelocity": -0.7128270459134503, - "velocityX": 1.7126818087934743, - "velocityY": -0.9513210238912078, - "timestamp": 0.4508764072405573 - }, - { - "x": 1.0338457421402325, - "y": 3.7875168932975933, - "heading": -0.2500489995023069, - "angularVelocity": -0.8312933688928248, - "velocityX": 1.998141846440012, - "velocityY": -1.109844476033343, - "timestamp": 0.5260224751139835 - }, - { - "x": 1.2054498603190866, - "y": 3.692204931169301, - "heading": -0.32141344961465557, - "angularVelocity": -0.9496764385936038, - "velocityX": 2.283607419990343, - "velocityY": -1.2683559476303248, - "timestamp": 0.6011685429874097 - }, - { - "x": 1.3985060453231253, - "y": 3.5849824249792595, - "heading": -0.40166985798593763, - "angularVelocity": -1.0680054278616893, - "velocityX": 2.5690790013020615, - "velocityY": -1.4268545144723213, - "timestamp": 0.676314610860836 + "x": 0.4570749914043126, + "y": 4.107894224868064, + "heading": -0.009932712149027413, + "angularVelocity": -0.1373640763265399, + "velocityX": 0.3295975698116412, + "velocityY": -0.18310287014791135, + "timestamp": 0.07230938695656872 + }, + { + "x": 0.5047410210186508, + "y": 4.0814144720383245, + "heading": -0.029797898632204542, + "angularVelocity": -0.27472486380099975, + "velocityX": 0.6591955985322896, + "velocityY": -0.36620076513225003, + "timestamp": 0.14461877391313743 + }, + { + "x": 0.576240240509544, + "y": 4.041695512105355, + "heading": -0.059590394556189516, + "angularVelocity": -0.41201422357348, + "velocityX": 0.9887958189140486, + "velocityY": -0.5492918914777362, + "timestamp": 0.21692816086970615 + }, + { + "x": 0.6715729183897807, + "y": 3.9887379727274364, + "heading": -0.09930156463679939, + "angularVelocity": -0.5491841620018386, + "velocityX": 1.3183997526835154, + "velocityY": -0.7323743376461849, + "timestamp": 0.28923754782627487 + }, + { + "x": 0.7907394144382421, + "y": 3.9225426248725555, + "heading": -0.148921029894937, + "angularVelocity": -0.686210564721571, + "velocityX": 1.648008662001748, + "velocityY": -0.9154461217412811, + "timestamp": 0.3615469347828436 + }, + { + "x": 0.9337401624459083, + "y": 3.8431103831358566, + "heading": -0.20843840181811515, + "angularVelocity": -0.8230932998910107, + "velocityX": 1.9776235704163385, + "velocityY": -1.0985052574765801, + "timestamp": 0.43385632173941235 + }, + { + "x": 1.100575665552893, + "y": 3.7504423098204516, + "heading": -0.2778446966612386, + "angularVelocity": -0.9598517946889951, + "velocityX": 2.307245436988583, + "velocityY": -1.2815497021301627, + "timestamp": 0.5061657086959811 + }, + { + "x": 1.2912465471408558, + "y": 3.6445396941775194, + "heading": -0.3571330045970933, + "angularVelocity": -1.0965147302863703, + "velocityX": 2.636875924594487, + "velocityY": -1.464576317132112, + "timestamp": 0.5784750956525498 + }, + { + "x": 1.505785276048115, + "y": 3.525464237325283, + "heading": -0.4461366707355329, + "angularVelocity": -1.2308729182270346, + "velocityX": 2.966955438802405, + "velocityY": -1.6467496387952614, + "timestamp": 0.6507844826091186 + }, + { + "x": 1.6964880033537257, + "y": 3.4196219693446848, + "heading": -0.5252603846379951, + "angularVelocity": -1.094238483172127, + "velocityX": 2.637316333772171, + "velocityY": -1.463741741361608, + "timestamp": 0.7230938695656873 + }, + { + "x": 1.8633540969095699, + "y": 3.3270118414974243, + "heading": -0.5945029830335408, + "angularVelocity": -0.9575879607046729, + "velocityX": 2.3076684864728434, + "velocityY": -1.280748347415596, + "timestamp": 0.7954032565222561 + }, + { + "x": 2.0063830653266717, + "y": 3.247633047534605, + "heading": -0.6538614517041024, + "angularVelocity": -0.8208957532196227, + "velocityX": 1.9780138435278007, + "velocityY": -1.0977661034589712, + "timestamp": 0.8677126434788248 + }, + { + "x": 2.125574496423577, + "y": 3.1814849244321213, + "heading": -0.7033318821965574, + "angularVelocity": -0.6841494939262661, + "velocityX": 1.648353500334547, + "velocityY": -0.9147930287697538, + "timestamp": 0.9400220304353936 + }, + { + "x": 2.220928049802523, + "y": 3.128566939134693, + "heading": -0.7429103365332542, + "angularVelocity": -0.5473487745162161, + "velocityX": 1.318688449622979, + "velocityY": -0.7318273259488806, + "timestamp": 1.0123314173919622 + }, + { + "x": 2.292443455008997, + "y": 3.0888786919590703, + "heading": -0.7725937492005902, + "angularVelocity": -0.4105056606988637, + "velocityX": 0.989019658670723, + "velocityY": -0.5488671505327598, + "timestamp": 1.084640804348531 + }, + { + "x": 2.34012050914709, + "y": 3.0624199236271457, + "heading": -0.7923807693651485, + "angularVelocity": -0.273643865580592, + "velocityX": 0.6593480617769434, + "velocityY": -0.3659105607936737, + "timestamp": 1.1569501913050997 }, { - "x": 1.5915937569821006, - "y": 3.4778198154599647, - "heading": -0.48176859267038563, - "angularVelocity": -1.065907198489266, - "velocityX": 2.5694985396202825, - "velocityY": -1.4260574445464824, - "timestamp": 0.7514606787342623 - }, - { - "x": 1.7632284858900336, - "y": 3.382566036520044, - "heading": -0.5529765476804537, - "angularVelocity": -0.9475938931363465, - "velocityX": 2.2840147697019786, - "velocityY": -1.2675816797275796, - "timestamp": 0.8266067466076885 - }, - { - "x": 1.9134098098562766, - "y": 3.2992204007390877, - "heading": -0.6152918418017524, - "angularVelocity": -0.8292555536806094, - "velocityX": 1.9985253815170194, - "velocityY": -1.1091150626981792, - "timestamp": 0.9017528144811148 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.13679610671265519, + "velocityX": 0.3296745537009426, + "velocityY": -0.18295553238277804, + "timestamp": 1.2292595782616684 }, { - "x": 2.0421373577646693, - "y": 3.2277823059512354, - "heading": -0.6687115003742364, - "angularVelocity": -0.7108776291856339, - "velocityX": 1.7130310547348626, - "velocityY": -0.9506564589404816, - "timestamp": 0.976898882354541 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 2.834394108236562e-32, + "velocityX": -3.2904683025944325e-34, + "velocityY": 0, + "timestamp": 1.3015689652182372 + }, + { + "x": 2.379804145625828, + "y": 3.040219511725313, + "heading": -0.7770894823147659, + "angularVelocity": 0.41593022428853316, + "velocityX": 0.2617028389642093, + "velocityY": -0.14816838427057621, + "timestamp": 1.3621150092281344 + }, + { + "x": 2.411575732183553, + "y": 3.0222750861201635, + "heading": -0.7276303412852089, + "angularVelocity": 0.8168847665996504, + "velocityX": 0.524750825215457, + "velocityY": -0.2963765163949562, + "timestamp": 1.4226610532380317 + }, + { + "x": 2.459369710281825, + "y": 2.995353279300503, + "heading": -0.6550403019178791, + "angularVelocity": 1.1989229115524662, + "velocityX": 0.789382343303212, + "velocityY": -0.4446501379224719, + "timestamp": 1.483207097247929 + }, + { + "x": 2.5233031088128075, + "y": 2.959437775047564, + "heading": -0.5609434444834801, + "angularVelocity": 1.5541371690447268, + "velocityX": 1.0559467522028614, + "velocityY": -0.5931932439230606, + "timestamp": 1.5437531412578263 + }, + { + "x": 2.6035132843463873, + "y": 2.9144863047853526, + "heading": -0.44765744633251, + "angularVelocity": 1.8710718429836914, + "velocityX": 1.3247797910705466, + "velocityY": -0.7424344727603186, + "timestamp": 1.6042991852677235 + }, + { + "x": 2.700148811858614, + "y": 2.8604246981568315, + "heading": -0.31830060076747974, + "angularVelocity": 2.136503675514867, + "velocityX": 1.5960667470930034, + "velocityY": -0.8929007255979282, + "timestamp": 1.6648452292776208 + }, + { + "x": 2.8133605147902916, + "y": 2.7971622097455233, + "heading": -0.1768724119476308, + "angularVelocity": 2.3358782746686186, + "velocityX": 1.869844756713935, + "velocityY": -1.0448657620135586, + "timestamp": 1.725391273287518 + }, + { + "x": 2.943296329434224, + "y": 2.7246257074465063, + "heading": -0.02888673141070084, + "angularVelocity": 2.4441841404657145, + "velocityX": 2.1460661347699634, + "velocityY": -1.1980386742882738, + "timestamp": 1.7859373172974153 + }, + { + "x": 3.0900160520490236, + "y": 2.6428091950168, + "heading": 0.11617462075416056, + "angularVelocity": 2.3958848928453333, + "velocityX": 2.4232751291036574, + "velocityY": -1.3513106226450158, + "timestamp": 1.8464833613073126 + }, + { + "x": 3.252842131476166, + "y": 2.55207108446101, + "heading": 0.23929724530787766, + "angularVelocity": 2.0335370636864525, + "velocityX": 2.6892934474880965, + "velocityY": -1.4986629108411686, + "timestamp": 1.9070294053172099 + }, + { + "x": 3.4280030327225406, + "y": 2.451410973839388, + "heading": 0.3174619447233944, + "angularVelocity": 1.2909959798981987, + "velocityX": 2.8930197523349617, + "velocityY": -1.662538193332126, + "timestamp": 1.9675754493271072 + }, + { + "x": 3.6156107870917813, + "y": 2.340964520507575, + "heading": 0.3503075182755731, + "angularVelocity": 0.5424891764490772, + "velocityX": 3.098596406043856, + "velocityY": -1.8241729106819762, + "timestamp": 2.028121493337004 + }, + { + "x": 3.8155697764375462, + "y": 2.229387834544172, + "heading": 0.3503075829776931, + "angularVelocity": 0.0000010686432293648565, + "velocityX": 3.302593796434966, + "velocityY": -1.8428402348659505, + "timestamp": 2.0886675373469012 + }, + { + "x": 4.015529033815858, + "y": 2.117811628928195, + "heading": 0.35030764767858236, + "angularVelocity": 0.000001068622902276308, + "velocityX": 3.3025982233558384, + "velocityY": -1.8428323012769912, + "timestamp": 2.1492135813567983 + }, + { + "x": 4.215488291220884, + "y": 2.0062354233600974, + "heading": 0.35030771237947195, + "angularVelocity": 0.0000010686229082180607, + "velocityX": 3.302598223797092, + "velocityY": -1.8428323004862048, + "timestamp": 2.2097596253666953 + }, + { + "x": 4.415447878546615, + "y": 1.8946598090554148, + "heading": 0.350307777080342, + "angularVelocity": 0.0000010686225853054014, + "velocityX": 3.3026036728847936, + "velocityY": -1.842822534969312, + "timestamp": 2.2703056693765924 + }, + { + "x": 4.620289311400001, + "y": 1.7923224180578192, + "heading": 0.35030784179343544, + "angularVelocity": 0.00000106882447086681, + "velocityX": 3.383233970164951, + "velocityY": -1.6902407526553997, + "timestamp": 2.3308517133864894 + }, + { + "x": 4.832980772388526, + "y": 1.7074973259478416, + "heading": 0.35030790812662205, + "angularVelocity": 0.0000010955825050486418, + "velocityX": 3.512887827217186, + "velocityY": -1.401001394841118, + "timestamp": 2.3913977573963865 + }, + { + "x": 5.052028438396142, + "y": 1.6407809635052637, + "heading": 0.3503079787205065, + "angularVelocity": 0.0000011659537069164618, + "velocityX": 3.617869170309609, + "velocityY": -1.101911174108616, + "timestamp": 2.4519438014062835 + }, + { + "x": 5.275893582425239, + "y": 1.5926421132936275, + "heading": 0.3503080570287918, + "angularVelocity": 0.0000012933674954586782, + "velocityX": 3.697436350961301, + "velocityY": -0.7950783738037029, + "timestamp": 2.5124898454161806 }, { - "x": 2.149410808485792, - "y": 3.1682512394298143, - "heading": -0.713232041811307, - "angularVelocity": -0.5924533737687984, - "velocityX": 1.4275324545498613, - "velocityY": -0.792204678250007, - "timestamp": 1.0520449502279672 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.35030814446785413, + "angularVelocity": 0.0000014441746575542642, + "velocityX": 3.751029791428781, + "velocityY": -0.4826595916936126, + "timestamp": 2.5730358894260776 + }, + { + "x": 5.645916996346935, + "y": 1.5526078041590061, + "heading": 0.35030823517092025, + "angularVelocity": 0.0000023934601751232044, + "velocityX": 3.7711793420471578, + "velocityY": -0.28528395446410554, + "timestamp": 2.6109320980454838 + }, + { + "x": 5.78920070535826, + "y": 1.5493061559809091, + "heading": 0.350308318678533, + "angularVelocity": 0.000002203587530623072, + "velocityX": 3.7809510299653186, + "velocityY": -0.08712344317225716, + "timestamp": 2.64882830666489 + }, + { + "x": 5.932460399917018, + "y": 1.5535231156733713, + "heading": 0.3503083968024594, + "angularVelocity": 0.0000020615235464554193, + "velocityX": 3.7803173398564307, + "velocityY": 0.11127655895113732, + "timestamp": 2.686724515284296 + }, + { + "x": 6.075301824148574, + "y": 1.5652470604158988, + "heading": 0.3503084709306375, + "angularVelocity": 0.0000019560842841988308, + "velocityX": 3.769280079337772, + "velocityY": 0.3093698596678062, + "timestamp": 2.724620723903702 + }, + { + "x": 6.217331878166829, + "y": 1.5844456932271112, + "heading": 0.3503085421651287, + "angularVelocity": 0.0000018797260658699983, + "velocityX": 3.747869752477631, + "velocityY": 0.5066109120314745, + "timestamp": 2.7625169325231083 + }, + { + "x": 6.358159707247505, + "y": 1.6110661098029424, + "heading": 0.3503086114142786, + "angularVelocity": 0.000001827337150563986, + "velocityX": 3.716145604300894, + "velocityY": 0.7024559328132687, + "timestamp": 2.8004131411425144 + }, + { + "x": 6.497397807591943, + "y": 1.6450348544134032, + "heading": 0.35030867945721406, + "angularVelocity": 0.0000017955077283623068, + "velocityX": 3.6741960585771425, + "velocityY": 0.8963626137804634, + "timestamp": 2.8383093497619205 + }, + { + "x": 6.634663343210931, + "y": 1.6862573848132372, + "heading": 0.3503087469924593, + "angularVelocity": 0.000001782110868887289, + "velocityX": 3.622144288827235, + "velocityY": 1.0877745268355006, + "timestamp": 2.8762055583813266 + }, + { + "x": 6.770832471288569, + "y": 1.7309686010151983, + "heading": 0.350308814465375, + "angularVelocity": 0.0000017804661250723641, + "velocityX": 3.593212435713354, + "velocityY": 1.179833493397679, + "timestamp": 2.9141017670007328 + }, + { + "x": 6.907000934216078, + "y": 1.775681842897667, + "heading": 0.35030888193834014, + "angularVelocity": 0.0000017804674286764754, + "velocityX": 3.5931948838222003, + "velocityY": 1.1798869467794602, + "timestamp": 2.951997975620139 + }, + { + "x": 7.041712141501968, + "y": 1.8246105114710787, + "heading": 0.3503089535974655, + "angularVelocity": 0.0000018909312562539825, + "velocityX": 3.5547410201057947, + "velocityY": 1.2911230530949709, + "timestamp": 2.989894184239545 + }, + { + "x": 7.171770756302622, + "y": 1.8790616059317657, + "heading": 0.3592109050640362, + "angularVelocity": 0.2349034848307258, + "velocityX": 3.43196904225537, + "velocityY": 1.43684807648023, + "timestamp": 3.027790392858951 + }, + { + "x": 7.296132052444026, + "y": 1.9323306109051834, + "heading": 0.3900794027691138, + "angularVelocity": 0.8145537200064432, + "velocityX": 3.281628972184848, + "velocityY": 1.4056552598282668, + "timestamp": 3.0656866014783573 + }, + { + "x": 7.41350002407911, + "y": 1.9843017273758428, + "heading": 0.4310938047294686, + "angularVelocity": 1.0822824618754054, + "velocityX": 3.097090076050037, + "velocityY": 1.371406754501705, + "timestamp": 3.1035828100977634 + }, + { + "x": 7.523578248846203, + "y": 2.0344435812093704, + "heading": 0.471270328487765, + "angularVelocity": 1.0601726458124487, + "velocityX": 2.904729226942307, + "velocityY": 1.3231364207724636, + "timestamp": 3.1414790187171695 }, { - "x": 2.2352298936713018, - "y": 3.120626784362015, - "heading": -0.7488501306819736, - "angularVelocity": -0.4739847323836121, - "velocityX": 1.1420302833417786, - "velocityY": -0.6337584442610682, - "timestamp": 1.1271910181013933 + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.9454290339278092, + "velocityX": 2.715460784576445, + "velocityY": 1.2708550641540455, + "timestamp": 3.1793752273365756 + }, + { + "x": 7.782116606596641, + "y": 2.1591418247265515, + "heading": 0.5502904833862925, + "angularVelocity": 0.6648323287723413, + "velocityX": 2.395575421885603, + "velocityY": 1.1781054843634051, + "timestamp": 3.244341952215483 + }, + { + "x": 7.917274495842872, + "y": 2.2282082387650113, + "heading": 0.5787513876779874, + "angularVelocity": 0.4380843323215672, + "velocityX": 2.0804171596791274, + "velocityY": 1.0631044456557224, + "timestamp": 3.30930867709439 + }, + { + "x": 8.032169166662662, + "y": 2.2890615062609574, + "heading": 0.5943003276772478, + "angularVelocity": 0.2393369840982844, + "velocityX": 1.768515667581298, + "velocityY": 0.9366836270316962, + "timestamp": 3.3742754019732972 + }, + { + "x": 8.12694730000964, + "y": 2.3412528312827354, + "heading": 0.5980433212943994, + "angularVelocity": 0.05761401123618554, + "velocityX": 1.458871961356148, + "velocityY": 0.8033547191898265, + "timestamp": 3.4392421268522044 + }, + { + "x": 8.201715109739398, + "y": 2.3844820330172567, + "heading": 0.590722775552934, + "angularVelocity": -0.11268146509017382, + "velocityX": 1.1508631513920338, + "velocityY": 0.6654052796273325, + "timestamp": 3.5042088517311116 + }, + { + "x": 8.256552657549896, + "y": 2.418534329374767, + "heading": 0.572871769156374, + "angularVelocity": -0.27477153003838295, + "velocityX": 0.8440866907283819, + "velocityY": 0.5241498077820845, + "timestamp": 3.569175576610019 + }, + { + "x": 8.291522317600203, + "y": 2.443248428942356, + "heading": 0.5448922111974887, + "angularVelocity": -0.4306752112106213, + "velocityX": 0.538270324007952, + "velocityY": 0.3804116586399298, + "timestamp": 3.634142301488926 }, { - "x": 2.2995943987852443, - "y": 3.0849086278583613, - "heading": -0.7755632624607879, - "angularVelocity": -0.3554827622359307, - "velocityX": 0.8565252572144717, - "velocityY": -0.4753163740226096, - "timestamp": 1.2023370859748195 + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.5817394500892022, + "velocityX": 0.23322225384012837, + "velocityY": 0.2347399755865949, + "timestamp": 3.6991090263678332 + }, + { + "x": 8.301908465997242, + "y": 2.464162051150854, + "heading": 0.4594182049840147, + "angularVelocity": -0.7297490572088752, + "velocityX": -0.07293676039464714, + "velocityY": 0.08667758549500083, + "timestamp": 3.7644469653994896 + }, + { + "x": 8.277129357589926, + "y": 2.4601442119160213, + "heading": 0.4025234718451046, + "angularVelocity": -0.8707763664131557, + "velocityX": -0.3792453324141557, + "velocityY": -0.061493204321711674, + "timestamp": 3.829784904431146 + }, + { + "x": 8.232325554908503, + "y": 2.44643699593842, + "heading": 0.33696231020008877, + "angularVelocity": -1.0034164318107932, + "velocityX": -0.6857241496355521, + "velocityY": -0.20978953699413314, + "timestamp": 3.8951228434628025 + }, + { + "x": 8.167484390379387, + "y": 2.42303097033958, + "heading": 0.26340235513425153, + "angularVelocity": -1.1258383131766259, + "velocityX": -0.992396844621899, + "velocityY": -0.35823024028199546, + "timestamp": 3.960460782494459 + }, + { + "x": 8.082591447673355, + "y": 2.3899154191246006, + "heading": 0.18267464694130434, + "angularVelocity": -1.235541086685248, + "velocityX": -1.2992901821543714, + "velocityY": -0.5068349523381112, + "timestamp": 4.025798721526115 + }, + { + "x": 7.9776304347285665, + "y": 2.347078349358033, + "heading": 0.09584952732096214, + "angularVelocity": -1.328862233904795, + "velocityX": -1.6064328704022224, + "velocityY": -0.6556232167931427, + "timestamp": 4.091136660557772 + }, + { + "x": 7.852583598052917, + "y": 2.2945065473278667, + "heading": 0.004378357289613106, + "angularVelocity": -1.3999702376138785, + "velocityX": -1.9138472766192256, + "velocityY": -0.8046137176854637, + "timestamp": 4.156474599589428 + }, + { + "x": 7.707434523289432, + "y": 2.232185897198679, + "heading": -0.08962534429630348, + "angularVelocity": -1.4387307432573238, + "velocityX": -2.221512905283988, + "velocityY": -0.9538202620531743, + "timestamp": 4.2218125386210845 + }, + { + "x": 7.542180705960218, + "y": 2.1601037497785995, + "heading": -0.1827828763211059, + "angularVelocity": -1.42578008130418, + "velocityX": -2.5292168650919318, + "velocityY": -1.1032204028528767, + "timestamp": 4.287150477652741 + }, + { + "x": 7.35689136818444, + "y": 2.078264678726032, + "heading": -0.2689709068831343, + "angularVelocity": -1.3191115581449608, + "velocityX": -2.835861377353881, + "velocityY": -1.252550543611691, + "timestamp": 4.352488416684397 + }, + { + "x": 7.152059282385439, + "y": 1.9867952525471149, + "heading": -0.3348300100346558, + "angularVelocity": -1.007976439532523, + "velocityX": -3.134963986234081, + "velocityY": -1.3999435478765458, + "timestamp": 4.417826355716054 + }, + { + "x": 6.932416862636984, + "y": 1.8843357350106482, + "heading": -0.3485403575636041, + "angularVelocity": -0.20983746552376023, + "velocityX": -3.3616367917885555, + "velocityY": -1.5681473743275887, + "timestamp": 4.48316429474771 + }, + { + "x": 6.70958980281502, + "y": 1.7775224962078198, + "heading": -0.34854038203919246, + "angularVelocity": -3.7459994478515855e-7, + "velocityX": -3.410377846690442, + "velocityY": -1.6347812677571678, + "timestamp": 4.548502233779367 + }, + { + "x": 6.478027967608038, + "y": 1.6912665812341474, + "heading": -0.3485404067374628, + "angularVelocity": -3.780081020278718e-7, + "velocityX": -3.5440639640437577, + "velocityY": -1.3201505320191125, + "timestamp": 4.613840172811023 + }, + { + "x": 6.239619726654668, + "y": 1.6262859956866853, + "heading": -0.34854043255776096, + "angularVelocity": -3.951807866461482e-7, + "velocityX": -3.6488485019072954, + "velocityY": -0.9945306893745133, + "timestamp": 4.679178111842679 + }, + { + "x": 5.996315346781838, + "y": 1.5831125822372847, + "heading": -0.3485404605967219, + "angularVelocity": -4.291375179720768e-7, + "velocityX": -3.7237841211206324, + "velocityY": -0.6607709714945885, + "timestamp": 4.744516050874336 + }, + { + "x": 5.7501052372367845, + "y": 1.5620996014320112, + "heading": -0.34854049237288, + "angularVelocity": -4.863354823397662e-7, + "velocityX": -3.7682564401941674, + "velocityY": -0.3216045855853207, + "timestamp": 4.809853989905992 }, { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080857, - "angularVelocity": -0.23696688264902363, - "velocityX": 0.5710180813549187, - "velocityY": -0.3168769856057537, - "timestamp": 1.2774831538482456 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.348540530032263, + "angularVelocity": -5.763784949174497e-7, + "velocityX": -3.7819013522532754, + "velocityY": 0.02019321393800792, + "timestamp": 4.875191928937649 + }, + { + "x": 5.265773365103949, + "y": 1.585443267719361, + "heading": -0.3485405619317938, + "angularVelocity": -5.063689324990554e-7, + "velocityX": -3.7657613238076486, + "velocityY": 0.34961055977354083, + "timestamp": 4.9381885481486885 + }, + { + "x": 5.031364041570911, + "y": 1.6280522215500413, + "heading": -0.3485405890146467, + "angularVelocity": -4.299096234955573e-7, + "velocityX": -3.720982593490641, + "velocityY": 0.6763688966853497, + "timestamp": 5.001185167359728 + }, + { + "x": 4.801558310439742, + "y": 1.6909217775230576, + "heading": -0.3485406130254673, + "angularVelocity": -3.811445901784285e-7, + "velocityX": -3.6479057766785146, + "velocityY": 0.9979830149678419, + "timestamp": 5.064181786570768 + }, + { + "x": 4.57810382416931, + "y": 1.7735737496127562, + "heading": -0.34854063513060685, + "angularVelocity": -3.5089406173482316e-7, + "velocityX": -3.5470869559183034, + "velocityY": 1.3120064715347763, + "timestamp": 5.127178405781808 + }, + { + "x": 4.3626998372206165, + "y": 1.8753793427019887, + "heading": -0.34854065619045194, + "angularVelocity": -3.3430119484907394e-7, + "velocityX": -3.4192943946259384, + "velocityY": 1.6160485175907584, + "timestamp": 5.190175024992848 + }, + { + "x": 4.156979523974483, + "y": 1.9955559179970916, + "heading": -0.34854067691382595, + "angularVelocity": -3.2896009785914497e-7, + "velocityX": -3.265577039253029, + "velocityY": 1.907667058965506, + "timestamp": 5.253171644203888 + }, + { + "x": 3.9516956402233028, + "y": 2.116476485340398, + "heading": -0.3485406976311522, + "angularVelocity": -3.288640962267947e-7, + "velocityX": -3.258649215182055, + "velocityY": 1.9194770903216498, + "timestamp": 5.316168263414927 + }, + { + "x": 3.746411773108691, + "y": 2.2373970809271935, + "heading": -0.34854071834847844, + "angularVelocity": -3.288640958736373e-7, + "velocityX": -3.2586489510953776, + "velocityY": 1.9194775386550607, + "timestamp": 5.379164882625967 + }, + { + "x": 3.5411278789507334, + "y": 2.3583176306028646, + "heading": -0.34854073906621164, + "angularVelocity": -3.2887055643589624e-7, + "velocityX": -3.2586493803778747, + "velocityY": 1.919476809867947, + "timestamp": 5.442161501837007 + }, + { + "x": 3.345073520020233, + "y": 2.47307412374708, + "heading": -0.3873766723080469, + "angularVelocity": -0.6164764669629881, + "velocityX": -3.1121409590840567, + "velocityY": 1.821629391884994, + "timestamp": 5.505158121048047 + }, + { + "x": 3.1664636949700515, + "y": 2.5778338770119946, + "heading": -0.44780580157610866, + "angularVelocity": -0.9592440042793773, + "velocityX": -2.835228735876023, + "velocityY": 1.6629424654356146, + "timestamp": 5.568154740259087 + }, + { + "x": 3.0057996639738764, + "y": 2.672135804524931, + "heading": -0.5103083460228613, + "angularVelocity": -0.9921571225491842, + "velocityX": -2.5503595749788768, + "velocityY": 1.496936322837019, + "timestamp": 5.631151359470127 + }, + { + "x": 2.8630598597209174, + "y": 2.7559513152021253, + "heading": -0.5701467016897592, + "angularVelocity": -0.9498661422835671, + "velocityX": -2.265832770720234, + "velocityY": 1.3304763291568347, + "timestamp": 5.694147978681166 + }, + { + "x": 2.738214483951598, + "y": 2.829279302871158, + "heading": -0.6251012943842897, + "angularVelocity": -0.8723419348970285, + "velocityX": -1.9817789800923042, + "velocityY": 1.1639987762419943, + "timestamp": 5.757144597892206 + }, + { + "x": 2.6312405711995455, + "y": 2.892122971419519, + "heading": -0.6738749327170909, + "angularVelocity": -0.7742262829916016, + "velocityX": -1.698089740239665, + "velocityY": 0.9975720814133604, + "timestamp": 5.820141217103246 + }, + { + "x": 2.5421208919645824, + "y": 2.9444859859038597, + "heading": -0.7156160888836572, + "angularVelocity": -0.6625935913597552, + "velocityX": -1.414673999193671, + "velocityY": 0.8312035652091746, + "timestamp": 5.883137836314286 + }, + { + "x": 2.4708422721118457, + "y": 2.98637166116498, + "heading": -0.749723844710458, + "angularVelocity": -0.5414220041322991, + "velocityX": -1.1314673826217794, + "velocityY": 0.6648876683493641, + "timestamp": 5.946134455525326 + }, + { + "x": 2.4173943837507443, + "y": 3.0177828179574866, + "heading": -0.775752501403198, + "angularVelocity": -0.4131754532023893, + "velocityX": -0.84842470961893, + "velocityY": 0.4986165477750299, + "timestamp": 6.0091310747363655 + }, + { + "x": 2.3817689275168203, + "y": 3.038721799552771, + "heading": -0.7933591587514436, + "angularVelocity": -0.2794857496917854, + "velocityX": -0.565513779629629, + "velocityY": 0.3323826239808009, + "timestamp": 6.072127693947405 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550474, - "velocityX": 0.2855094397331478, - "velocityY": -0.15843872456938407, - "timestamp": 1.3526292217216718 + "angularVelocity": -0.14148780267529462, + "velocityX": -0.28271125846882317, + "velocityY": 0.16617910323716242, + "timestamp": 6.135124313158445 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 3.467177167133168e-32, - "velocityX": -1.439882477346126e-32, - "velocityY": -1.3815367235364851e-32, - "timestamp": 1.427775289595098 + "angularVelocity": 1.2572572851920672e-28, + "velocityX": 2.1335040537014396e-30, + "velocityY": 1.8634721866519574e-29, + "timestamp": 6.198120932369485 + }, + { + "x": 2.384240228496088, + "y": 3.0394763007756747, + "heading": -0.7921549333865273, + "angularVelocity": 0.15389773911589558, + "velocityX": 0.30849818872977186, + "velocityY": -0.14776374894381028, + "timestamp": 6.263862499797187 + }, + { + "x": 2.4248103079231944, + "y": 3.020044385403153, + "heading": -0.7722377725963315, + "angularVelocity": 0.3029614529969845, + "velocityX": 0.6171145747585456, + "velocityY": -0.2955803479114183, + "timestamp": 6.329604067224889 + }, + { + "x": 2.485678594891513, + "y": 2.9908905714242224, + "heading": -0.7429157303412026, + "angularVelocity": 0.4460198228065572, + "velocityX": 0.9258721589693997, + "velocityY": -0.4434608896569423, + "timestamp": 6.395345634652592 + }, + { + "x": 2.5668563689761195, + "y": 2.952009793968192, + "heading": -0.7046902372675247, + "angularVelocity": 0.581450892781281, + "velocityX": 1.2348013176576613, + "velocityY": -0.5914184735371368, + "timestamp": 6.461087202080294 + }, + { + "x": 2.668357540158173, + "y": 2.903395952391573, + "heading": -0.6582160423239786, + "angularVelocity": 0.706922526522589, + "velocityX": 1.5439420621310505, + "velocityY": -0.7394688547710901, + "timestamp": 6.526828769507996 + }, + { + "x": 2.790199449693154, + "y": 2.8450417291439147, + "heading": -0.604379415056612, + "angularVelocity": 0.8189130465526614, + "velocityX": 1.8533465857651596, + "velocityY": -0.8876305438234711, + "timestamp": 6.592570336935698 + }, + { + "x": 2.9324037360004773, + "y": 2.7769385796688133, + "heading": -0.5444410242237507, + "angularVelocity": 0.9117274378767651, + "velocityX": 2.1630802530485744, + "velocityY": -1.0359222047754857, + "timestamp": 6.6583119043634005 + }, + { + "x": 3.094996415116772, + "y": 2.6990776165764148, + "heading": -0.4803328988113282, + "angularVelocity": 0.975153588829843, + "velocityX": 2.473209652250889, + "velocityY": -1.1843490524929314, + "timestamp": 6.724053471791103 + }, + { + "x": 3.2780012401054357, + "y": 2.6114549065561117, + "heading": -0.4154109268291881, + "angularVelocity": 0.9875330711202905, + "velocityX": 2.783700361113535, + "velocityY": -1.3328357300982285, + "timestamp": 6.789795039218805 + }, + { + "x": 3.48136962382921, + "y": 2.514109658078373, + "heading": -0.3571766392011983, + "angularVelocity": 0.8858061939583661, + "velocityX": 3.0934520073228393, + "velocityY": -1.4807260046665736, + "timestamp": 6.855536606646507 + }, + { + "x": 3.702896130222067, + "y": 2.408427187890042, + "heading": -0.3455558213194451, + "angularVelocity": 0.17676514778162136, + "velocityX": 3.3696565971974572, + "velocityY": -1.6075441204615863, + "timestamp": 6.921278174074209 + }, + { + "x": 3.927079400169571, + "y": 2.300911734169414, + "heading": -0.34555579773751244, + "angularVelocity": 3.5870657763866696e-7, + "velocityX": 3.4100688304100117, + "velocityY": -1.6354257728775006, + "timestamp": 6.9870197415019115 + }, + { + "x": 4.15126266204063, + "y": 2.1933962636083315, + "heading": -0.3455557741556376, + "angularVelocity": 3.587056984423895e-7, + "velocityX": 3.4100687075585983, + "velocityY": -1.6354260290389449, + "timestamp": 7.052761308929614 + }, + { + "x": 4.375445923911402, + "y": 2.0858807930466483, + "heading": -0.3455557505737627, + "angularVelocity": 3.5870569914452407e-7, + "velocityX": 3.4100687075542147, + "velocityY": -1.6354260290480847, + "timestamp": 7.118502876357316 + }, + { + "x": 4.599629185782173, + "y": 1.978365322484965, + "heading": -0.34555572699188775, + "angularVelocity": 3.5870569932883336e-7, + "velocityX": 3.4100687075542147, + "velocityY": -1.6354260290480853, + "timestamp": 7.184244443785018 + }, + { + "x": 4.823812447652946, + "y": 1.8708498519232832, + "heading": -0.3455557034100129, + "angularVelocity": 3.5870569853758494e-7, + "velocityX": 3.4100687075542258, + "velocityY": -1.6354260290480622, + "timestamp": 7.24998601121272 + }, + { + "x": 5.047995709543858, + "y": 1.7633343814035949, + "heading": -0.34555567982813806, + "angularVelocity": 3.5870569820699746e-7, + "velocityX": 3.410068707860571, + "velocityY": -1.6354260284092945, + "timestamp": 7.315727578640423 + }, + { + "x": 5.27217953587615, + "y": 1.65582008782303, + "heading": -0.34555565624625534, + "angularVelocity": 3.587058178727361e-7, + "velocityX": 3.410077293621499, + "velocityY": -1.6354081258984543, + "timestamp": 7.381469146068125 }, { - "x": 2.3773546785315864, - "y": 3.0407954088560687, - "heading": -0.7860770412156382, - "angularVelocity": 0.2650004205849919, - "velocityX": 0.21918861143707746, - "velocityY": -0.13736692695017716, - "timestamp": 1.4888897992558612 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 3.617497277100986e-7, + "velocityX": 3.511082415812739, + "velocityY": -1.405520236667608, + "timestamp": 7.447210713495827 + }, + { + "x": 5.6599765905642645, + "y": 1.5113593672660246, + "heading": -0.34555563704903824, + "angularVelocity": -1.0484559207434358e-7, + "velocityX": 3.589689630249378, + "velocityY": -1.1905096787253004, + "timestamp": 7.490939561801255 + }, + { + "x": 5.819811381479902, + "y": 1.4688909784273936, + "heading": -0.3455556415836398, + "angularVelocity": -1.0369817036745698e-7, + "velocityX": 3.6551337871799947, + "velocityY": -0.9711755622285476, + "timestamp": 7.534668410106684 + }, + { + "x": 5.98037051193366, + "y": 1.4292489543347602, + "heading": -0.34555564611341977, + "angularVelocity": -1.0358790889899858e-7, + "velocityX": 3.6716981277969567, + "velocityY": -0.9065416910994355, + "timestamp": 7.578397258412112 + }, + { + "x": 6.140929731813359, + "y": 1.389607292439096, + "heading": -0.3455556506431997, + "angularVelocity": -1.0358790900922708e-7, + "velocityX": 3.6717001728071876, + "velocityY": -0.9065334083071072, + "timestamp": 7.62212610671754 + }, + { + "x": 6.30148895170346, + "y": 1.3499656305855623, + "heading": -0.34555565517297965, + "angularVelocity": -1.0358790856487398e-7, + "velocityX": 3.6717001730450596, + "velocityY": -0.9065334073436624, + "timestamp": 7.665854955022969 + }, + { + "x": 6.462048171593562, + "y": 1.3103239687320334, + "heading": -0.34555565970275964, + "angularVelocity": -1.0358791005302326e-7, + "velocityX": 3.671700173045087, + "velocityY": -0.9065334073435504, + "timestamp": 7.709583803328397 + }, + { + "x": 6.622607391483664, + "y": 1.2706823068785016, + "heading": -0.34555566423253964, + "angularVelocity": -1.0358790893463982e-7, + "velocityX": 3.6717001730450707, + "velocityY": -0.9065334073436169, + "timestamp": 7.753312651633825 + }, + { + "x": 6.783166611367605, + "y": 1.2310406450000155, + "heading": -0.3455556687623196, + "angularVelocity": -1.0358790885289903e-7, + "velocityX": 3.6717001729041776, + "velocityY": -0.9065334079142718, + "timestamp": 7.797041499939254 + }, + { + "x": 6.943725778284007, + "y": 1.1913987685896004, + "heading": -0.34555567329209946, + "angularVelocity": -1.0358790829702175e-7, + "velocityX": 3.6716989616319724, + "velocityY": -0.9065383138730976, + "timestamp": 7.840770348244682 + }, + { + "x": 7.103836002524736, + "y": 1.1499808922348331, + "heading": -0.34555567791225333, + "angularVelocity": -1.0565459632350647e-7, + "velocityX": 3.661432451237353, + "velocityY": -0.9471522338178334, + "timestamp": 7.88449919655011 + }, + { + "x": 7.257955684496521, + "y": 1.1005509138662795, + "heading": -0.346747423550466, + "angularVelocity": -0.02725307627333064, + "velocityX": 3.524439539210384, + "velocityY": -1.1303745761449258, + "timestamp": 7.928228044855539 + }, + { + "x": 7.402334567868656, + "y": 1.0534126604700325, + "heading": -0.34675046354447403, + "angularVelocity": -0.00006951918758018307, + "velocityX": 3.3016850195483918, + "velocityY": -1.07796695369166, + "timestamp": 7.971956893160967 }, { - "x": 2.4041715283150715, - "y": 3.0239975454455843, - "heading": -0.75411979281041, - "angularVelocity": 0.5229077118120985, - "velocityX": 0.438796775632191, - "velocityY": -0.2748588429118795, - "timestamp": 1.5500043089166244 + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.027323634774631837, + "velocityX": 3.079221948168643, + "velocityY": -1.024340556060407, + "timestamp": 8.015685741466395 + }, + { + "x": 7.709533523799992, + "y": 0.9494390432841444, + "heading": -0.3413965784412158, + "angularVelocity": 0.06656216442856697, + "velocityX": 2.7614877570011993, + "velocityY": -0.9471323179117597, + "timestamp": 8.078169497196857 + }, + { + "x": 7.862380855125707, + "y": 0.8957055369328039, + "heading": -0.33682515317576994, + "angularVelocity": 0.07316181961221624, + "velocityX": 2.4461930871290236, + "velocityY": -0.8599596122731851, + "timestamp": 8.140653252927319 + }, + { + "x": 7.99561550345175, + "y": 0.8477270543434197, + "heading": -0.33285887044966567, + "angularVelocity": 0.06347702182330028, + "velocityX": 2.1323085779411386, + "velocityY": -0.7678552934037809, + "timestamp": 8.20313700865778 + }, + { + "x": 8.109294083490143, + "y": 0.8056872786590918, + "heading": -0.33010810532148027, + "angularVelocity": 0.04402368417243478, + "velocityX": 1.8193301396409471, + "velocityY": -0.6728112801937814, + "timestamp": 8.265620764388242 + }, + { + "x": 8.20345588526635, + "y": 0.7697081195865146, + "heading": -0.32897987990878186, + "angularVelocity": 0.018056299585531694, + "velocityX": 1.5069805051795295, + "velocityY": -0.5758162045793389, + "timestamp": 8.328104520118703 + }, + { + "x": 8.278129725528439, + "y": 0.7398763726381793, + "heading": -0.32976503867911666, + "angularVelocity": -0.012565806282865027, + "velocityX": 1.1950920585537632, + "velocityY": -0.4774320397291953, + "timestamp": 8.390588275849165 + }, + { + "x": 8.333337625588607, + "y": 0.7162569714576368, + "heading": -0.33268181216540116, + "angularVelocity": -0.04668050843272986, + "velocityX": 0.883556044523325, + "velocityY": -0.37800866648333437, + "timestamp": 8.453072031579627 + }, + { + "x": 8.369096953381794, + "y": 0.698900317224484, + "heading": -0.33790002427933163, + "angularVelocity": -0.08351309957167831, + "velocityX": 0.572297989695798, + "velocityY": -0.2777786647144699, + "timestamp": 8.515555787310088 + }, + { + "x": 8.385421752929688, + "y": 0.6878466606140137, + "heading": -0.34555563246426124, + "angularVelocity": -0.12252157533477633, + "velocityX": 0.26126469763300175, + "velocityY": -0.1769044847136382, + "timestamp": 8.57803954304055 + }, + { + "x": 8.379560991493735, + "y": 0.6833892008568203, + "heading": -0.35721868503670856, + "angularVelocity": -0.1678763417451177, + "velocityX": -0.08435897751441682, + "velocityY": -0.06416005011256522, + "timestamp": 8.64751361217505 + }, + { + "x": 8.349687812290387, + "y": 0.6867654591427064, + "heading": -0.37193021085772177, + "angularVelocity": -0.2117556378126066, + "velocityX": -0.42999034856465757, + "velocityY": 0.048597387887987537, + "timestamp": 8.71698768130955 + }, + { + "x": 8.295801597591089, + "y": 0.6979765367761911, + "heading": -0.38956490915625874, + "angularVelocity": -0.253831372168465, + "velocityX": -0.7756306111129838, + "velocityY": 0.1613706779112114, + "timestamp": 8.786461750444051 + }, + { + "x": 8.217901630671626, + "y": 0.7170238074049998, + "heading": -0.40996601054380316, + "angularVelocity": -0.2936505899495876, + "velocityX": -1.121281190089058, + "velocityY": 0.2741637400269978, + "timestamp": 8.855935819578551 + }, + { + "x": 8.115987079646073, + "y": 0.7439090305870278, + "heading": -0.4329315185089462, + "angularVelocity": -0.3305622982969707, + "velocityX": -1.4669437431144206, + "velocityY": 0.3869821289721611, + "timestamp": 8.925409888713052 + }, + { + "x": 7.990056990056933, + "y": 0.778634533730576, + "heading": -0.45819105339643357, + "angularVelocity": -0.36358219983610734, + "velocityX": -1.812620034466978, + "velocityY": 0.4998340183057432, + "timestamp": 8.994883957847552 + }, + { + "x": 7.840110319883288, + "y": 0.8212035218059903, + "heading": -0.4853637226253089, + "angularVelocity": -0.3911195870256229, + "velocityX": -2.1583113245224226, + "velocityY": 0.6127320395326444, + "timestamp": 9.064358026982053 + }, + { + "x": 7.6661461501938755, + "y": 0.8716206459377261, + "heading": -0.5138727598830971, + "angularVelocity": -0.4103550808661484, + "velocityX": -2.504015841545443, + "velocityY": 0.7256970083921437, + "timestamp": 9.133832096116553 + }, + { + "x": 7.468164698581641, + "y": 0.9298931437973682, + "heading": -0.5427426487462225, + "angularVelocity": -0.41554912822558204, + "velocityX": -2.8497172265661783, + "velocityY": 0.8387661552805812, + "timestamp": 9.203306165251053 + }, + { + "x": 7.24617329762414, + "y": 0.9960332266921984, + "heading": -0.5699703376985038, + "angularVelocity": -0.391911533201964, + "velocityX": -3.1953130674947268, + "velocityY": 0.9520110700121011, + "timestamp": 9.272780234385554 + }, + { + "x": 7.00025853984304, + "y": 1.0700564595014088, + "heading": -0.589089272180988, + "angularVelocity": -0.2751952594783323, + "velocityX": -3.53966250782022, + "velocityY": 1.065480023430083, + "timestamp": 9.342254303520054 + }, + { + "x": 6.750442302486931, + "y": 1.1514706650489683, + "heading": -0.5890892788822688, + "angularVelocity": -9.645729514768673e-8, + "velocityX": -3.5958198572257243, + "velocityY": 1.171864647656428, + "timestamp": 9.411728372654554 + }, + { + "x": 6.500626129709104, + "y": 1.232885068752249, + "heading": -0.5890892855834843, + "angularVelocity": -9.645635490096592e-8, + "velocityX": -3.5958189276949932, + "velocityY": 1.1718674998820673, + "timestamp": 9.481202441789055 + }, + { + "x": 6.250809956933241, + "y": 1.3142994724615544, + "heading": -0.5890892922846998, + "angularVelocity": -9.645635424551899e-8, + "velocityX": -3.595818927666732, + "velocityY": 1.1718674999687875, + "timestamp": 9.550676510923555 + }, + { + "x": 6.000993784158921, + "y": 1.395713876175591, + "heading": -0.589089298985915, + "angularVelocity": -9.645635348778811e-8, + "velocityX": -3.595818927644538, + "velocityY": 1.1718675000368866, + "timestamp": 9.620150580058056 + }, + { + "x": 5.751177662095486, + "y": 1.477128435493372, + "heading": -0.5890893056871302, + "angularVelocity": -9.64563521650664e-8, + "velocityX": -3.5958181977191597, + "velocityY": 1.1718697397753421, + "timestamp": 9.689624649192556 }, { - "x": 2.4444388170293236, - "y": 2.998788446329585, - "heading": -0.7069088657797715, - "angularVelocity": 0.772499481591177, - "velocityX": 0.6588826276733534, - "velocityY": -0.41248959135778174, - "timestamp": 1.6111188185773877 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.5890893123960679, + "angularVelocity": -9.65675052838285e-8, + "velocityX": -3.5721826564600034, + "velocityY": 1.242054049730676, + "timestamp": 9.759098718327056 + }, + { + "x": 5.287967104202961, + "y": 1.6596756394253018, + "heading": -0.5890893181956361, + "angularVelocity": -9.309834476919773e-8, + "velocityX": -3.4519020760085812, + "velocityY": 1.5451728334255468, + "timestamp": 9.821393791687077 + }, + { + "x": 5.074932733784246, + "y": 1.7602857483579184, + "heading": -0.5890893239791856, + "angularVelocity": -9.284120212486628e-8, + "velocityX": -3.419762734486735, + "velocityY": 1.6150572349624415, + "timestamp": 9.883688865047098 + }, + { + "x": 4.861898448797989, + "y": 1.860896038187167, + "heading": -0.5890893297627348, + "angularVelocity": -9.284119839047264e-8, + "velocityX": -3.4197613630707817, + "velocityY": 1.615060138829829, + "timestamp": 9.94598393840712 + }, + { + "x": 4.648864163815087, + "y": 1.9615063280235205, + "heading": -0.5890893355462841, + "angularVelocity": -9.28411976539737e-8, + "velocityX": -3.419761363016917, + "velocityY": 1.6150601389438841, + "timestamp": 10.00827901176714 + }, + { + "x": 4.435829878832186, + "y": 2.0621166178598744, + "heading": -0.5890893413298333, + "angularVelocity": -9.28411982771023e-8, + "velocityX": -3.419761363016914, + "velocityY": 1.6150601389438883, + "timestamp": 10.070574085127161 + }, + { + "x": 4.222795593849285, + "y": 2.162726907696228, + "heading": -0.5890893471133825, + "angularVelocity": -9.284119843074624e-8, + "velocityX": -3.4197613630169146, + "velocityY": 1.6150601389438883, + "timestamp": 10.132869158487182 + }, + { + "x": 4.009761308866383, + "y": 2.2633371975325822, + "heading": -0.5890893528969318, + "angularVelocity": -9.284119832193768e-8, + "velocityX": -3.419761363016914, + "velocityY": 1.6150601389438886, + "timestamp": 10.195164231847203 + }, + { + "x": 3.7967270238835944, + "y": 2.363947487369175, + "heading": -0.589089358680481, + "angularVelocity": -9.284119741513914e-8, + "velocityX": -3.4197613630151045, + "velocityY": 1.615060138947721, + "timestamp": 10.257459305207224 + }, + { + "x": 3.583692741771847, + "y": 2.464557783284562, + "heading": -0.5890893644643718, + "angularVelocity": -9.284668184194445e-8, + "velocityX": -3.419761316927328, + "velocityY": 1.615060236528384, + "timestamp": 10.319754378567245 + }, + { + "x": 3.381763406617466, + "y": 2.559912098840239, + "heading": -0.6245815423960026, + "angularVelocity": -0.5697429349912143, + "velocityX": -3.241497670086629, + "velocityY": 1.530687908570203, + "timestamp": 10.382049451927266 + }, + { + "x": 3.1981911985495857, + "y": 2.646597580557341, + "heading": -0.6568615726262876, + "angularVelocity": -0.5181795042399261, + "velocityX": -2.9468174314037126, + "velocityY": 1.3915302935133123, + "timestamp": 10.444344525287287 + }, + { + "x": 3.032976151851517, + "y": 2.7246143065734305, + "heading": -0.6859252496642307, + "angularVelocity": -0.46654856428174046, + "velocityX": -2.652136642383354, + "velocityY": 1.2523739327700791, + "timestamp": 10.506639598647308 + }, + { + "x": 2.8861182875087947, + "y": 2.793962348685928, + "heading": -0.7117686923384959, + "angularVelocity": -0.414855321301394, + "velocityX": -2.3574555164898965, + "velocityY": 1.1132187245645664, + "timestamp": 10.568934672007329 + }, + { + "x": 2.7576176211174075, + "y": 2.854641769350887, + "heading": -0.7343883425051251, + "angularVelocity": -0.3631049607390947, + "velocityX": -2.0627741402397475, + "velocityY": 0.9740645189431933, + "timestamp": 10.63122974536735 + }, + { + "x": 2.6474741650844584, + "y": 2.906652621199817, + "heading": -0.7537810666899133, + "angularVelocity": -0.31130429966287193, + "velocityX": -1.7680925648228973, + "velocityY": 0.8349111581961731, + "timestamp": 10.69352481872737 + }, + { + "x": 2.5556879298075064, + "y": 2.9499949470036255, + "heading": -0.7699442582183806, + "angularVelocity": -0.2594617945957906, + "velocityX": -1.473410822497863, + "velocityY": 0.6957584840348696, + "timestamp": 10.755819892087391 + }, + { + "x": 2.482258924484474, + "y": 2.9846687797700127, + "heading": -0.7828759248842103, + "angularVelocity": -0.20758730937026382, + "velocityX": -1.1787289325219326, + "velocityY": 0.556606339734085, + "timestamp": 10.818114965447412 + }, + { + "x": 2.4271871577101587, + "y": 3.010674142880251, + "heading": -0.7925747589157716, + "angularVelocity": -0.15569183096565328, + "velocityX": -0.8840469045768797, + "velocityY": 0.4174545707643066, + "timestamp": 10.880410038807433 + }, + { + "x": 2.3904726379075623, + "y": 3.0280110502230517, + "heading": -0.7990401886427835, + "angularVelocity": -0.10378717574737188, + "velocityX": -0.5893647414204509, + "velocityY": 0.27830302474492646, + "timestamp": 10.942705112167454 }, { - "x": 2.498190798561039, - "y": 2.965157152573787, - "heading": -0.6450804787000075, - "angularVelocity": 1.0116809808826641, - "velocityX": 0.8795289666903056, - "velocityY": -0.5502996578468837, - "timestamp": 1.672233328238151 + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": -0.05188569757551244, + "velocityX": -0.29468244125856385, + "velocityY": 0.13915155101802326, + "timestamp": 11.005000185527475 }, { - "x": 2.5654677170302604, - "y": 2.923088008476279, - "heading": -0.5694581806203519, - "angularVelocity": 1.237386972413308, - "velocityX": 1.1008338092322851, - "velocityY": -0.6883658943027919, - "timestamp": 1.7333478378989142 + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": -7.343237406242101e-30, + "velocityX": 6.07048254686404e-33, + "velocityY": -1.403576741314563e-31, + "timestamp": 11.067295258887496 + }, + { + "x": 2.3887780433959134, + "y": 3.0288255570872233, + "heading": -0.7958553483876907, + "angularVelocity": 0.1079808929405059, + "velocityX": 0.2803852472906352, + "velocityY": -0.13215958314273293, + "timestamp": 11.126723028163784 + }, + { + "x": 2.422106198393558, + "y": 3.0131153644204134, + "heading": -0.7831901230540868, + "angularVelocity": 0.2131196490779077, + "velocityX": 0.5608178702232047, + "velocityY": -0.26435777176444086, + "timestamp": 11.186150797440073 + }, + { + "x": 2.47210314120033, + "y": 2.9895461805025576, + "heading": -0.7644823111559433, + "angularVelocity": 0.3147991608305137, + "velocityX": 0.8413060664338303, + "velocityY": -0.3966021980108423, + "timestamp": 11.245578566716361 + }, + { + "x": 2.5387727913774722, + "y": 2.9581147004806385, + "heading": -0.7399860035384287, + "angularVelocity": 0.41220304776557914, + "velocityX": 1.1218602176902186, + "velocityY": -0.5289022355153943, + "timestamp": 11.30500633599265 + }, + { + "x": 2.6221198536798296, + "y": 2.9188169151709924, + "heading": -0.7100217609714737, + "angularVelocity": 0.5042128104749645, + "velocityX": 1.4024935365630693, + "velocityY": -0.6612697361556471, + "timestamp": 11.364434105268938 + }, + { + "x": 2.7221500309650937, + "y": 2.871647898493915, + "heading": -0.6750052177423963, + "angularVelocity": 0.5892286326010091, + "velocityX": 1.6832228182788898, + "velocityY": -0.7937201286790226, + "timestamp": 11.423861874545226 + }, + { + "x": 2.8388702680559588, + "y": 2.816601499667555, + "heading": -0.6354955831852485, + "angularVelocity": 0.6648345552665271, + "velocityX": 1.9640689615693319, + "velocityY": -0.9262740213325141, + "timestamp": 11.483289643821514 + }, + { + "x": 2.9722889163933544, + "y": 2.753669910841413, + "heading": -0.5922850618953597, + "angularVelocity": 0.7271099322127873, + "velocityX": 2.2450556358107243, + "velocityY": -1.0589592978591336, + "timestamp": 11.542717413097803 + }, + { + "x": 3.122415239721002, + "y": 2.6828431673634756, + "heading": -0.5465844996458821, + "angularVelocity": 0.7690102254582669, + "velocityX": 2.5261981924591717, + "velocityY": -1.191812251081979, + "timestamp": 11.602145182374091 + }, + { + "x": 3.289254937095535, + "y": 2.6041094136623544, + "heading": -0.5004858972625192, + "angularVelocity": 0.7757081065791619, + "velocityX": 2.8074366479896216, + "velocityY": -1.3248646997853857, + "timestamp": 11.66157295165038 + }, + { + "x": 3.4727741175032825, + "y": 2.5174653392644726, + "heading": -0.45856185133391086, + "angularVelocity": 0.7054622180728715, + "velocityX": 3.0881048143423393, + "velocityY": -1.4579728543249981, + "timestamp": 11.721000720926668 + }, + { + "x": 3.672087548521264, + "y": 2.4232619622496343, + "heading": -0.44156909021440377, + "angularVelocity": 0.2859397437680458, + "velocityX": 3.353877041746403, + "velocityY": -1.5851743749110756, + "timestamp": 11.780428490202956 + }, + { + "x": 3.87540622002593, + "y": 2.327473100540026, + "heading": -0.4415690736830568, + "angularVelocity": 2.781754584465502e-7, + "velocityX": 3.4212738250264785, + "velocityY": -1.6118535640173974, + "timestamp": 11.839856259479244 + }, + { + "x": 4.078724897772698, + "y": 2.231684252079618, + "heading": -0.4415690571518241, + "angularVelocity": 2.781735362772021e-7, + "velocityX": 3.4212739300632746, + "velocityY": -1.6118533410711124, + "timestamp": 11.899284028755533 + }, + { + "x": 4.282043575519764, + "y": 2.135895403619844, + "heading": -0.4415690406205913, + "angularVelocity": 2.7817353604412086e-7, + "velocityX": 3.4212739300683004, + "velocityY": -1.611853341060444, + "timestamp": 11.958711798031821 + }, + { + "x": 4.485362253266831, + "y": 2.0401065551600697, + "heading": -0.44156902408935866, + "angularVelocity": 2.781735361125895e-7, + "velocityX": 3.421273930068301, + "velocityY": -1.611853341060444, + "timestamp": 12.01813956730811 + }, + { + "x": 4.688680931013898, + "y": 1.9443177067002955, + "heading": -0.4415690075581259, + "angularVelocity": 2.781735368678374e-7, + "velocityX": 3.421273930068301, + "velocityY": -1.611853341060444, + "timestamp": 12.077567336584398 + }, + { + "x": 4.8919996087609645, + "y": 1.8485288582405217, + "heading": -0.4415689910268932, + "angularVelocity": 2.7817353639391735e-7, + "velocityX": 3.4212739300683026, + "velocityY": -1.6118533410604396, + "timestamp": 12.136995105860686 + }, + { + "x": 5.0953182865105076, + "y": 1.7527400097860029, + "heading": -0.4415689744956604, + "angularVelocity": 2.781735367341045e-7, + "velocityX": 3.4212739301099617, + "velocityY": -1.611853340972016, + "timestamp": 12.196422875136975 + }, + { + "x": 5.298637015997747, + "y": 1.656951271148522, + "heading": -0.4415689579644276, + "angularVelocity": 2.7817353744823303e-7, + "velocityX": 3.4212748007079674, + "velocityY": -1.6118514930642196, + "timestamp": 12.255850644413263 }, { - "x": 2.6463157759141644, - "y": 2.8725583813996622, - "heading": -0.4811095285927534, - "angularVelocity": 1.445624819997864, - "velocityX": 1.3228946666295571, - "velocityY": -0.8268024624119354, - "timestamp": 1.7944623475596775 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.44156894141194364, + "angularVelocity": 2.7853113329051353e-7, + "velocityX": 3.4389071599112104, + "velocityY": -1.5738818379758603, + "timestamp": 12.315278413689551 + }, + { + "x": 5.76887189670182, + "y": 1.4734081866468696, + "heading": -0.441568927295876, + "angularVelocity": 1.9019557057481577e-7, + "velocityX": 3.5822280134028937, + "velocityY": -1.2127779127617127, + "timestamp": 12.389497111786811 + }, + { + "x": 6.03546847764148, + "y": 1.3855778991398175, + "heading": -0.4415689131909676, + "angularVelocity": 1.9004521532739413e-7, + "velocityX": 3.5920406551769672, + "velocityY": -1.1833983855651649, + "timestamp": 12.463715809884071 + }, + { + "x": 6.302065075021444, + "y": 1.2977476615350296, + "heading": -0.441568899086059, + "angularVelocity": 1.900452165326357e-7, + "velocityX": 3.592040876688587, + "velocityY": -1.1833977131973445, + "timestamp": 12.537934507981332 + }, + { + "x": 6.568661672398624, + "y": 1.2099174239217916, + "heading": -0.44156888498115054, + "angularVelocity": 1.9004521568233082e-7, + "velocityX": 3.5920408766510765, + "velocityY": -1.1833977133112008, + "timestamp": 12.612153206078592 + }, + { + "x": 6.835258130689826, + "y": 1.1220867641390964, + "heading": -0.44156887087145064, + "angularVelocity": 1.9010977386064987e-7, + "velocityX": 3.5920390026492015, + "velocityY": -1.1834034014931274, + "timestamp": 12.686371904175852 + }, + { + "x": 7.080885784636739, + "y": 1.0408339813622511, + "heading": -0.35950242791659653, + "angularVelocity": 1.105738109919598, + "velocityX": 3.3095117570646315, + "velocityY": -1.0947751019611969, + "timestamp": 12.760590602273112 + }, + { + "x": 7.299180381748869, + "y": 0.9686950138524031, + "heading": -0.28235870642192235, + "angularVelocity": 1.03941086912835, + "velocityX": 2.9412345232203254, + "velocityY": -0.9719783472261578, + "timestamp": 12.834809300370372 + }, + { + "x": 7.490166378274822, + "y": 0.9056069788063349, + "heading": -0.2132225753720476, + "angularVelocity": 0.9315190487378978, + "velocityX": 2.5732868053771534, + "velocityY": -0.8500288561164988, + "timestamp": 12.909027998467632 + }, + { + "x": 7.653856153896351, + "y": 0.8515488890315335, + "heading": -0.15310137838681676, + "angularVelocity": 0.8100545890297152, + "velocityX": 2.2055058875732163, + "velocityY": -0.7283621400089914, + "timestamp": 12.983246696564892 + }, + { + "x": 7.790256705792751, + "y": 0.8065104939114175, + "heading": -0.10249736767748478, + "angularVelocity": 0.6818229369001992, + "velocityX": 1.8378192476193804, + "velocityY": -0.6068335375688586, + "timestamp": 13.057465394662152 + }, + { + "x": 7.899372493751606, + "y": 0.7704858089539702, + "heading": -0.06171206576746063, + "angularVelocity": 0.5495286626665922, + "velocityX": 1.470192697477151, + "velocityY": -0.48538556834071367, + "timestamp": 13.131684092759413 + }, + { + "x": 7.981206582030799, + "y": 0.7434709210905415, + "heading": -0.030946027709333555, + "angularVelocity": 0.4145321710976964, + "velocityX": 1.1026074342068202, + "velocityY": -0.3639903226007307, + "timestamp": 13.205902790856673 + }, + { + "x": 8.035761170601306, + "y": 0.7254630347979746, + "heading": -0.010340878696429398, + "angularVelocity": 0.2776274650631065, + "velocityX": 0.7350518126714517, + "velocityY": -0.24263274288330566, + "timestamp": 13.280121488953933 }, { - "x": 2.7407862870930626, - "y": 2.813537598490058, - "heading": -0.38139476987658943, - "angularVelocity": 1.6316053138553275, - "velocityX": 1.5457951262848801, - "velocityY": -0.9657409220366661, - "timestamp": 1.8555768572204407 + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -7.30119772009808e-38, + "angularVelocity": 0.13932983145219902, + "velocityX": 0.3675179222006851, + "velocityY": -0.12130427346551735, + "timestamp": 13.354340187051193 }, { - "x": 2.84893500974016, - "y": 2.7459886359527177, - "heading": -0.27202745092011676, - "angularVelocity": 1.7895475160244771, - "velocityX": 1.7696079580350594, - "velocityY": -1.105285191884772, - "timestamp": 1.916691366881204 + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": 1.121139660303252e-31, + "angularVelocity": 1.5105915295246559e-30, + "velocityX": -7.247567295016868e-32, + "velocityY": -3.7967728194239397e-31, + "timestamp": 13.428558885148453 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 2.970822636317052, - "y": 2.669873093439706, - "heading": -0.15523119808259794, - "angularVelocity": 1.9111051284848073, - "velocityX": 1.9944138839282306, - "velocityY": -1.245457796119401, - "timestamp": 1.9778058765419673 + "scope": [ + "last" + ], + "type": "StopPoint" }, { - "x": 3.106512263881207, - "y": 2.5851587340080315, - "heading": -0.03419676462946541, - "angularVelocity": 1.980453318286855, - "velocityX": 2.220252249708728, - "velocityY": -1.3861578846318265, - "timestamp": 2.0389203862027303 + "scope": [ + 6 + ], + "type": "StopPoint" }, { - "x": 3.256035657144846, - "y": 2.4918371927335334, - "heading": 0.08572826850512172, - "angularVelocity": 1.962300504418209, - "velocityX": 2.4466103727841246, - "velocityY": -1.5269948461095477, - "timestamp": 2.1000348958634936 + "scope": [ + 1 + ], + "type": "StopPoint" }, { - "x": 3.419180860242083, - "y": 2.390029027608911, - "heading": 0.19415018142326176, - "angularVelocity": 1.7740780956923756, - "velocityX": 2.6695003200194134, - "velocityY": -1.6658591501387068, - "timestamp": 2.161149405524257 - }, - { - "x": 3.5929644641848295, - "y": 2.279455345020229, - "heading": 0.26705766200125053, - "angularVelocity": 1.1929651564364412, - "velocityX": 2.8435735622750062, - "velocityY": -1.8092869140644157, - "timestamp": 2.22226391518502 - }, + "scope": [ + 11 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "AmpLanePADESprint": { + "waypoints": [ { - "x": 3.7768655464749616, - "y": 2.1600020280366277, - "heading": 0.30152237044830693, - "angularVelocity": 0.5639365944088304, - "velocityX": 3.0091230922237155, - "velocityY": -1.9545819421061743, - "timestamp": 2.2833784248457833 + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 3.9728967066647076, - "y": 2.0375522576737963, - "heading": 0.30152241765502197, - "angularVelocity": 7.724305624896446e-7, - "velocityX": 3.2076042379769354, - "velocityY": -2.003612088889046, - "timestamp": 2.3444929345065466 + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 }, { - "x": 4.174386727718979, - "y": 1.9243094504352285, - "heading": 0.30152246485493484, - "angularVelocity": 7.723192600121745e-7, - "velocityX": 3.2969260847008353, - "velocityY": -1.85296107040964, - "timestamp": 2.40560744416731 + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 }, { - "x": 4.383354452565466, - "y": 1.8255438101811146, - "heading": 0.30152251288342946, - "angularVelocity": 7.858771165145409e-7, - "velocityX": 3.419281705873656, - "velocityY": -1.6160751481496927, - "timestamp": 2.466721953828073 + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 }, { - "x": 4.598761540028333, - "y": 1.7417467464566943, - "heading": 0.30152256294147634, - "angularVelocity": 8.190861247953373e-7, - "velocityX": 3.5246472344874826, - "velocityY": -1.3711484259558735, - "timestamp": 2.5278364634888364 + "x": 8.017494201660156, + "y": 7.457698345184326, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 }, { - "x": 4.819537390717514, - "y": 1.6733348748555603, - "heading": 0.30152261646908723, - "angularVelocity": 8.758576510857166e-7, - "velocityX": 3.612494838208991, - "velocityY": -1.1194047367945392, - "timestamp": 2.5889509731495997 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 5.04458467733647, - "y": 1.620648272623701, - "heading": 0.30152267534909943, - "angularVelocity": 9.634375304093619e-7, - "velocityX": 3.682387175617667, - "velocityY": -0.8620964567058491, - "timestamp": 2.650065482810363 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 14 }, { - "x": 5.27278482830379, - "y": 1.5839488347993986, - "heading": 0.301522742248914, - "angularVelocity": 0.000001094663361600596, - "velocityX": 3.7339766322927477, - "velocityY": -0.6005028597630109, - "timestamp": 2.711179992471126 + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.3015228161300743, - "angularVelocity": 0.0000012088972110609183, - "velocityX": 3.7670067261218194, - "velocityY": -0.33592432468507777, - "timestamp": 2.7722945021318894 + "x": 7.985864639282227, + "y": 5.863699913024902, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 5.642161233224445, - "y": 1.5569512204704454, - "heading": 0.30152289610635313, - "angularVelocity": 0.0000021712102282875206, - "velocityX": 3.7778762267369546, - "velocityY": -0.17558800471026415, - "timestamp": 2.809129387028348 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 }, { - "x": 5.781468008961295, - "y": 1.556401100893367, - "heading": 0.30152296986307525, - "angularVelocity": 0.0000020023605969229892, - "velocityX": 3.781925099764402, - "velocityY": -0.014934744023907952, - "timestamp": 2.8459642719248066 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 }, { - "x": 5.920672391336217, - "y": 1.5617696165699877, - "heading": 0.3015230386365787, - "angularVelocity": 0.0000018670752912406278, - "velocityX": 3.779145306581512, - "velocityY": 0.14574541746802047, - "timestamp": 2.882799156821265 - }, + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ { - "x": 6.0595230325638285, - "y": 1.5730470711005764, - "heading": 0.3015231034083751, - "angularVelocity": 0.0000017584362380693717, - "velocityX": 3.769541879061509, - "velocityY": 0.30616233937716997, - "timestamp": 2.919634041717724 + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": 0, + "angularVelocity": 2.735053895521923e-38, + "velocityX": -3.8673469053295564e-39, + "velocityY": 1.858643532249755e-38, + "timestamp": 0 }, { - "x": 6.197769224123459, - "y": 1.5902130982406153, - "heading": 0.3015231649742209, - "angularVelocity": 0.000001671400517937078, - "velocityX": 3.753132172076425, - "velocityY": 0.46602635486148125, - "timestamp": 2.9564689266141824 + "x": 0.46193105617219093, + "y": 6.976128411049919, + "heading": 0.007924686694784022, + "angularVelocity": 0.0965568963773332, + "velocityX": 0.35283526252830383, + "velocityY": -0.05604467125266167, + "timestamp": 0.08207271559159549 }, { - "x": 6.335161349542959, - "y": 1.6132366986444322, - "heading": 0.3015232239953199, - "angularVelocity": 0.0000016023152822250254, - "velocityX": 3.72994583275345, - "velocityY": 0.6250487946015117, - "timestamp": 2.993303811510641 + "x": 0.5198473628641517, + "y": 6.966928755942856, + "heading": 0.023773564700110772, + "angularVelocity": 0.19310775683593598, + "velocityX": 0.7056706516226394, + "velocityY": -0.11209151592888836, + "timestamp": 0.16414543118319097 }, { - "x": 6.471451335210211, - "y": 1.6420762959087598, - "heading": 0.30152328103508963, - "angularVelocity": 0.000001548525804202316, - "velocityX": 3.7000247469309935, - "velocityY": 0.7829425107583317, - "timestamp": 3.0301386964070995 + "x": 0.6067218129336875, + "y": 6.9531289220308485, + "heading": 0.04754806846022335, + "angularVelocity": 0.2896760950181983, + "velocityX": 1.0585058559757474, + "velocityY": -0.16814155365198188, + "timestamp": 0.24621814677478646 }, { - "x": 6.606393098338155, - "y": 1.6766798119201127, - "heading": 0.301523336612691, - "angularVelocity": 0.000001508830595721816, - "velocityX": 3.663422961881362, - "velocityY": 0.9394224010369989, - "timestamp": 3.066973581303558 + "x": 0.7225543617111448, + "y": 6.934728565336742, + "heading": 0.07925217561750111, + "angularVelocity": 0.38629289805690764, + "velocityX": 1.4113405160596262, + "velocityY": -0.22419578250180797, + "timestamp": 0.32829086236638194 }, { - "x": 6.739742463422568, - "y": 1.716984614087568, - "heading": 0.3015255656708468, - "angularVelocity": 0.00006051486687697733, - "velocityX": 3.6201922568579414, - "velocityY": 1.094201930608725, - "timestamp": 3.1038084662000167 + "x": 0.8673449286095066, + "y": 6.9117272629835975, + "heading": 0.11889328943914418, + "angularVelocity": 0.48299990485147826, + "velocityX": 1.764174194245729, + "velocityY": -0.2802551638184133, + "timestamp": 0.4103635779579774 }, { - "x": 6.8694207904091344, - "y": 1.7614703171513673, - "heading": 0.3101205256410531, - "angularVelocity": 0.23333750042565204, - "velocityX": 3.520530262306672, - "velocityY": 1.207705771006116, - "timestamp": 3.1406433510964753 + "x": 1.0410933872613453, + "y": 6.884124518124938, + "heading": 0.16648343863275697, + "angularVelocity": 0.5798534732349732, + "velocityX": 2.117006332731008, + "velocityY": -0.33632059887008814, + "timestamp": 0.49243629354957286 }, { - "x": 6.994764102779472, - "y": 1.808693107878132, - "heading": 0.3306180633638746, - "angularVelocity": 0.5564707961064413, - "velocityX": 3.4028425152588873, - "velocityY": 1.282012713206686, - "timestamp": 3.177478235992934 + "x": 1.243799549589456, + "y": 6.851919767180746, + "heading": 0.22204089753091144, + "angularVelocity": 0.6769297018830912, + "velocityX": 2.4698361796239694, + "velocityY": -0.39239290075946437, + "timestamp": 0.5745090091411683 }, { - "x": 7.114597005425528, - "y": 1.856512920318317, - "heading": 0.35962771339750194, - "angularVelocity": 0.7875591335542964, - "velocityX": 3.253244932973218, - "velocityY": 1.2982207647615676, - "timestamp": 3.2143131208893925 + "x": 1.4754631289031934, + "y": 6.815112383244328, + "heading": 0.28559273385120976, + "angularVelocity": 0.7743357321882771, + "velocityX": 2.8226625333871667, + "velocityY": -0.44847284107882956, + "timestamp": 0.6565817247327638 }, { - "x": 7.228563598795877, - "y": 1.9039637614742513, - "heading": 0.39146295222083616, - "angularVelocity": 0.8642687200685354, - "velocityX": 3.093985326429113, - "velocityY": 1.2882038667778264, - "timestamp": 3.251148005785851 + "x": 1.6781593090979035, + "y": 6.782884452250807, + "heading": 0.3422497740441209, + "angularVelocity": 0.6903273491624664, + "velocityX": 2.4697145541443093, + "velocityY": -0.3926753338331338, + "timestamp": 0.7386544403243592 }, { - "x": 7.336676614220082, - "y": 1.9505339221399116, - "heading": 0.42341393247502157, - "angularVelocity": 0.8674108889982484, - "velocityX": 2.935071352282109, - "velocityY": 1.2642949963483496, - "timestamp": 3.2879828906823096 + "x": 1.8518984587746443, + "y": 6.755259929387126, + "heading": 0.39086385083963315, + "angularVelocity": 0.5923293319234401, + "velocityX": 2.1168929092256374, + "velocityY": -0.33658594899118865, + "timestamp": 0.8207271559159547 }, { - "x": 7.438996464546078, - "y": 1.995939510716552, - "heading": 0.45395278545537515, - "angularVelocity": 0.8290742068611583, - "velocityX": 2.77779747686502, - "velocityY": 1.2326789863542051, - "timestamp": 3.3248177755787682 + "x": 1.9966808291576696, + "y": 6.732239139219163, + "heading": 0.43140805286474204, + "angularVelocity": 0.4940034179795163, + "velocityX": 1.7640743253029585, + "velocityY": -0.28049260953564537, + "timestamp": 0.9027998715075501 }, { - "x": 7.535582073954796, - "y": 2.0400031624519186, - "heading": 0.48208291467910164, - "angularVelocity": 0.7636817463336474, - "velocityX": 2.6221232855814667, - "velocityY": 1.196247846551653, - "timestamp": 3.361652660475227 + "x": 2.1125066103123933, + "y": 6.7138223061958735, + "heading": 0.463861007608982, + "angularVelocity": 0.39541709458878044, + "velocityX": 1.4112580571975715, + "velocityY": -0.22439653532283038, + "timestamp": 0.9848725870991456 }, { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.6791276743107281, - "velocityX": 2.4678193928677437, - "velocityY": 1.1565397180800703, - "timestamp": 3.3984875453716854 + "x": 2.199375929341993, + "y": 6.700009583150749, + "heading": 0.4882072246931022, + "angularVelocity": 0.29664203150374724, + "velocityX": 1.0584433377576143, + "velocityY": -0.16829859893826818, + "timestamp": 1.0669453026907412 }, { - "x": 7.766333855095899, - "y": 2.1513911601167894, - "heading": 0.5393931714770351, - "angularVelocity": 0.5089507306836832, - "velocityX": 2.2039777611896034, - "velocityY": 1.0840548028640598, - "timestamp": 3.461940970502476 + "x": 2.257288845375976, + "y": 6.690801066111525, + "heading": 0.5044377142261048, + "angularVelocity": 0.19775743273523133, + "velocityX": 0.7056293387703308, + "velocityY": -0.11219949252132488, + "timestamp": 1.1490180182823366 }, { - "x": 7.8897168213730975, - "y": 2.214535022901093, - "heading": 0.5621690879226864, - "angularVelocity": 0.3589391179231937, - "velocityX": 1.9444650312080196, - "velocityY": 0.9951214241650669, - "timestamp": 3.5253943956332665 + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.0988477756026137, + "velocityX": 0.3528151893675758, + "velocityY": -0.0560997894575031, + "timestamp": 1.231090733873932 }, { - "x": 7.996856912991746, - "y": 2.271342778200504, - "heading": 0.5763089063837279, - "angularVelocity": 0.2228377496076287, - "velocityX": 1.6884839769927567, - "velocityY": 0.8952669644281369, - "timestamp": 3.588847820764057 + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0, + "velocityX": 3.087311543739499e-36, + "velocityY": 0, + "timestamp": 1.3131634494655275 }, { - "x": 8.08793407853681, - "y": 2.3213235528188796, - "heading": 0.5824533246990756, - "angularVelocity": 0.09683351690917834, - "velocityX": 1.4353388387992245, - "velocityY": 0.7876765441007412, - "timestamp": 3.6523012458948476 + "x": 2.3177158256290964, + "y": 6.704097470773247, + "heading": 0.5125504196, + "angularVelocity": -9.1973743139294e-17, + "velocityX": 0.34546318060283326, + "velocityY": 0.1965022887733569, + "timestamp": 1.4042599289943862 }, { - "x": 8.16309393881241, - "y": 2.364112462024999, - "heading": 0.5810872026138336, - "angularVelocity": -0.021529524725041504, - "velocityX": 1.1844886248564517, - "velocityY": 0.6743356898059124, - "timestamp": 3.715754671025638 + "x": 2.380656782769881, + "y": 6.7398988031009575, + "heading": 0.5125504196, + "angularVelocity": -1.3336684994607708e-16, + "velocityX": 0.6909263394843395, + "velocityY": 0.3930045651914449, + "timestamp": 1.495356408523245 }, { - "x": 8.222455939995415, - "y": 2.399428067130867, - "heading": 0.5725898810376202, - "angularVelocity": -0.13391430893917436, - "velocityX": 0.9355208337555891, - "velocityY": 0.5565594770191685, - "timestamp": 3.7792080961564287 + "x": 2.475068211555481, + "y": 6.793600797653198, + "heading": 0.5125504196, + "angularVelocity": 6.653839443641591e-19, + "velocityX": 1.0363894332018742, + "velocityY": 0.5895068045437317, + "timestamp": 1.5864528880521036 }, { - "x": 8.266119462815807, - "y": 2.4270468912932266, - "heading": 0.5572661381126698, - "angularVelocity": -0.24149591441856846, - "velocityX": 0.6881192422693254, - "velocityY": 0.43526136068197446, - "timestamp": 3.8426615212872193 + "x": 2.569479640341081, + "y": 6.847302792205439, + "heading": 0.5125504196, + "angularVelocity": 5.174999729369457e-17, + "velocityX": 1.0363894332018742, + "velocityY": 0.5895068045437317, + "timestamp": 1.6775493675809623 }, { - "x": 8.294168308263885, - "y": 2.4467872814650136, - "heading": 0.5353660980042331, - "angularVelocity": -0.3451356654632318, - "velocityX": 0.4420383200790668, - "velocityY": 0.3111004666981787, - "timestamp": 3.90611494641801 + "x": 2.6324205974818655, + "y": 6.8831041245331495, + "heading": 0.5125504196, + "angularVelocity": -1.3901806543706914e-17, + "velocityX": 0.6909263394843393, + "velocityY": 0.39300456519144494, + "timestamp": 1.768645847109821 }, { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.44548570157766576, - "velocityX": 0.19708463824313602, - "velocityY": 0.18456741878341876, - "timestamp": 3.9695683715488004 + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "angularVelocity": -1.8725484396545262e-17, + "velocityX": 0.3454631806028332, + "velocityY": 0.19650228877335693, + "timestamp": 1.8597423266386797 }, { - "x": 8.303024530407173, - "y": 2.461881127582199, - "heading": 0.47119103232326837, - "angularVelocity": -0.5465136547157349, - "velocityX": -0.05554517814981655, - "velocityY": 0.05148048067312068, - "timestamp": 4.035271161880262 + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "angularVelocity": 1.431796971368011e-33, + "velocityX": 3.743360269099248e-35, + "velocityY": -8.057233109044706e-35, + "timestamp": 1.9508388061675384 }, { - "x": 8.282750419132642, - "y": 2.456558519757899, - "heading": 0.4288756816904664, - "angularVelocity": -0.6440419108431604, - "velocityX": -0.3085730632177365, - "velocityY": -0.08101037714605593, - "timestamp": 4.1009739522117235 + "x": 2.6835271230542426, + "y": 6.903699548014665, + "heading": 0.5052880772246282, + "angularVelocity": -0.10752621184517612, + "velocityX": 0.2907312178663514, + "velocityY": 0.039898557617057115, + "timestamp": 2.018379011023157 }, { - "x": 8.245821395629305, - "y": 2.442576926285189, - "heading": 0.3804195474569323, - "angularVelocity": -0.7375049672788585, - "velocityX": -0.5620617224479442, - "velocityY": -0.21280060408660287, - "timestamp": 4.166676742543185 + "x": 2.7228033814945674, + "y": 6.909089957964811, + "heading": 0.49095749727137794, + "angularVelocity": -0.21217850884350908, + "velocityX": 0.5815241236576919, + "velocityY": 0.0798103879262595, + "timestamp": 2.0859192158787754 }, { - "x": 8.19220201387014, - "y": 2.4199912479232255, - "heading": 0.32613641295591117, - "angularVelocity": -0.8261922245184762, - "velocityX": -0.8160898721144456, - "velocityY": -0.34375523852216117, - "timestamp": 4.2323795328746465 + "x": 2.7817246923505206, + "y": 6.91717710303389, + "heading": 0.46979167660622734, + "angularVelocity": -0.31338105518627096, + "velocityX": 0.8723886902906202, + "velocityY": 0.11973823719319023, + "timestamp": 2.153459420734394 }, { - "x": 8.121850214183777, - "y": 2.388868090056124, - "heading": 0.2664003971864745, - "angularVelocity": -0.909185370485436, - "velocityX": -1.070758172239708, - "velocityY": -0.4736961354318384, - "timestamp": 4.298082323206108 + "x": 2.860296723174767, + "y": 6.927962260865171, + "heading": 0.4420737054643263, + "angularVelocity": -0.41039216865205086, + "velocityX": 1.1633371706853932, + "velocityY": 0.15968500324119947, + "timestamp": 2.220999625590012 }, { - "x": 8.034715295558529, - "y": 2.349289919990928, - "heading": 0.20166644793914118, - "angularVelocity": -0.9852541866298153, - "velocityX": -1.3261981444876794, - "velocityY": -0.6023818754961501, - "timestamp": 4.36378511353757 + "x": 2.9585261643123166, + "y": 6.941446920815512, + "heading": 0.40815310467896726, + "angularVelocity": -0.502228278073356, + "velocityX": 1.454384708301312, + "velocityY": 0.19965382070083557, + "timestamp": 2.2885398304456306 }, { - "x": 7.9307349853362235, - "y": 2.301361396168203, - "heading": 0.1325025874209066, - "angularVelocity": -1.052677674255729, - "velocityX": -1.5825859099398853, - "velocityY": -0.7294747084702512, - "timestamp": 4.429487903869031 + "x": 3.0764209718394073, + "y": 6.9576328058659405, + "heading": 0.3684706356327604, + "angularVelocity": -0.5875384762459123, + "velocityX": 1.7455500435498434, + "velocityY": 0.23964814861057657, + "timestamp": 2.356080035301249 }, { - "x": 7.809831081747403, - "y": 2.2452194829459002, - "heading": 0.0596429970770519, - "angularVelocity": -1.1089268808263426, - "velocityX": -1.8401639105261376, - "velocityY": -0.8544829365552749, - "timestamp": 4.495190694200493 + "x": 3.2139906503876703, + "y": 6.9765218958360915, + "heading": 0.323597871318007, + "angularVelocity": -0.6643859670055544, + "velocityX": 2.0368561043358877, + "velocityY": 0.27967178972185397, + "timestamp": 2.4236202401568674 }, { - "x": 7.671902780233127, - "y": 2.1810506699963903, - "heading": -0.015920256793080897, - "angularVelocity": -1.1500767850029918, - "velocityX": -2.099276161917122, - "velocityY": -0.9766527818040505, - "timestamp": 4.560893484531954 + "x": 3.3712465090237984, + "y": 6.998116434011635, + "heading": 0.27430452270447037, + "angularVelocity": -0.7298371202591344, + "velocityX": 2.3283296071176522, + "velocityY": 0.3197286449116718, + "timestamp": 2.491160445012486 }, { - "x": 7.516816185598606, - "y": 2.109122861945235, - "heading": -0.09276870443286889, - "angularVelocity": -1.1696375032490751, - "velocityX": -2.3604263053689314, - "velocityY": -1.0947451042534069, - "timestamp": 4.626596274863416 + "x": 3.5482015820439243, + "y": 7.022418860702876, + "heading": 0.22168358710657873, + "angularVelocity": -0.7791053596947181, + "velocityX": 2.619996095635254, + "velocityY": 0.3598216313260006, + "timestamp": 2.558700649868104 }, { - "x": 7.344387868807497, - "y": 2.02985165560778, - "heading": -0.16871929267657412, - "angularVelocity": -1.1559720349858067, - "velocityX": -2.6243682486121434, - "velocityY": -1.206512020837212, - "timestamp": 4.692299065194877 + "x": 3.7448688058303015, + "y": 7.04943148246024, + "heading": 0.16741520227188003, + "angularVelocity": -0.8034974864335817, + "velocityX": 2.911854120176202, + "velocityY": 0.3999487685166161, + "timestamp": 2.6262408547237226 }, { - "x": 7.154363408340053, - "y": 1.9439629823466762, - "heading": -0.2400485240661188, - "angularVelocity": -1.0856347352935642, - "velocityX": -2.8921825010596516, - "velocityY": -1.3072302230667405, - "timestamp": 4.758001855526339 + "x": 3.961249724160237, + "y": 7.079155012062387, + "heading": 0.1144423105156959, + "angularVelocity": -0.7843164211512963, + "velocityX": 3.2037350018777047, + "velocityY": 0.4400864591051642, + "timestamp": 2.693781059579341 }, { - "x": 6.9464441838510815, - "y": 1.8530162365161489, - "heading": -0.2992772419214471, - "angularVelocity": -0.9014642689682996, - "velocityX": -3.1645417712101533, - "velocityY": -1.3842143594162994, - "timestamp": 4.8237046458578 + "x": 4.197248092224393, + "y": 7.111577952654102, + "heading": 0.06945280731873603, + "angularVelocity": -0.6661144024234814, + "velocityX": 3.494190883321295, + "velocityY": 0.4800539273019009, + "timestamp": 2.7613212644349594 }, { - "x": 6.72212326982476, - "y": 1.7626751908782685, - "heading": -0.31855321839260475, - "angularVelocity": -0.2933813978662556, - "velocityX": -3.4141763674670904, - "velocityY": -1.3749955699312393, - "timestamp": 4.889407436189262 - }, - { - "x": 6.485759745047901, - "y": 1.6860130581467425, - "heading": -0.3185532921212082, - "angularVelocity": -0.0000011221533066716862, - "velocityX": -3.597465550312819, - "velocityY": -1.1668017803319484, - "timestamp": 4.955110226520723 + "x": 4.450279816565845, + "y": 7.14615999986413, + "heading": 0.06926458523360347, + "angularVelocity": -0.0027868154314146247, + "velocityX": 3.746386687490236, + "velocityY": 0.5120216511625231, + "timestamp": 2.828861469290578 }, { - "x": 6.24426876944993, - "y": 1.6274732096370486, - "heading": -0.3185533177664736, - "angularVelocity": -3.9032231775904613e-7, - "velocityX": -3.675505627381763, - "velocityY": -0.8909796405048926, - "timestamp": 5.020813016852185 + "x": 4.703335353284152, + "y": 7.180937109425349, + "heading": 0.06926456657564138, + "angularVelocity": -2.762497114544702e-7, + "velocityX": 3.746739253445653, + "velocityY": 0.5149097435455242, + "timestamp": 2.896401674146196 }, { - "x": 5.999037658914315, - "y": 1.5873919634602989, - "heading": -0.3185533462426243, - "angularVelocity": -4.334085446884588e-7, - "velocityX": -3.732430682143934, - "velocityY": -0.6100387209515062, - "timestamp": 5.0865158071836465 + "x": 4.956390888818978, + "y": 7.215714227598158, + "heading": 0.06926454791768001, + "angularVelocity": -2.7624970048363726e-7, + "velocityX": 3.746739235923052, + "velocityY": 0.5149098710487069, + "timestamp": 2.9639418790018146 }, { - "x": 5.751475205523387, - "y": 1.5659995878977724, - "heading": -0.3185533789943251, - "angularVelocity": -4.984826463431063e-7, - "velocityX": -3.7679138456983283, - "velocityY": -0.325593105781428, - "timestamp": 5.152218597515108 + "x": 5.209446424353711, + "y": 7.250491345771652, + "heading": 0.0692645292597187, + "angularVelocity": -2.7624969990656975e-7, + "velocityX": 3.7467392359216607, + "velocityY": 0.5149098710588286, + "timestamp": 3.031482083857433 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.3185534180019772, - "angularVelocity": -5.936985611348973e-7, - "velocityX": -3.781751231726404, - "velocityY": -0.039276923728911305, - "timestamp": 5.21792138784657 + "x": 5.462501959888314, + "y": 7.285268463946087, + "heading": 0.0692645106017574, + "angularVelocity": -2.7624969939846597e-7, + "velocityX": 3.7467392359197462, + "velocityY": 0.5149098710727624, + "timestamp": 3.0990222887130514 }, { - "x": 5.26265149067387, - "y": 1.5786031053003822, - "heading": -0.31855345112044986, - "angularVelocity": -5.20084412877405e-7, - "velocityX": -3.7744308273915435, - "velocityY": 0.23844772894968475, - "timestamp": 5.28160042121006 + "x": 5.715557495424461, + "y": 7.320045582109288, + "heading": 0.06926449194379608, + "angularVelocity": -2.762496999067462e-7, + "velocityX": 3.746739235942603, + "velocityY": 0.5149098709064452, + "timestamp": 3.16656249356867 }, { - "x": 5.0240625547753925, - "y": 1.61139052009726, - "heading": -0.3185534786660252, - "angularVelocity": -4.3256899255575315e-7, - "velocityX": -3.7467424251962935, - "velocityY": 0.5148855606793222, - "timestamp": 5.34527945457355 + "x": 5.968613025759916, + "y": 7.354822738115272, + "heading": 0.06926447328583468, + "angularVelocity": -2.762497009102298e-7, + "velocityX": 3.7467391589411925, + "velocityY": 0.5149104312065347, + "timestamp": 3.234102698424288 }, { - "x": 4.788524290519247, - "y": 1.661604289022676, - "heading": -0.31855350247111613, - "angularVelocity": -3.7382933947955595e-7, - "velocityX": -3.698835422197071, - "velocityY": 0.7885447732660761, - "timestamp": 5.40895848793704 + "x": 6.221690653746151, + "y": 7.389438720345797, + "heading": 0.06926445462738323, + "angularVelocity": -2.7625695670270355e-7, + "velocityX": 3.74706633666926, + "velocityY": 0.5125240929387752, + "timestamp": 3.3016429032799066 }, { - "x": 4.55730773239592, - "y": 1.7289734284079028, - "heading": -0.3185535237219762, - "angularVelocity": -3.337183198598428e-7, - "velocityX": -3.6309684037367047, - "velocityY": 1.0579485244487485, - "timestamp": 5.4726375213005305 + "x": 6.476102895472122, + "y": 7.412263085598601, + "heading": 0.06926437909315673, + "angularVelocity": -0.0000011183594520291744, + "velocityX": 3.766826622303432, + "velocityY": 0.33793745964490024, + "timestamp": 3.369183108135525 }, { - "x": 4.33166058516164, - "y": 1.8131343663413284, - "heading": -0.3185535432404584, - "angularVelocity": -3.0651348259884106e-7, - "velocityX": -3.543507734268667, - "velocityY": 1.321642831055895, - "timestamp": 5.536316554664021 + "x": 6.712136074986816, + "y": 7.430641724558031, + "heading": 0.04341074490415746, + "angularVelocity": -0.38278880326565307, + "velocityX": 3.4947063015172035, + "velocityY": 0.27211405412107026, + "timestamp": 3.4367233129911434 }, { - "x": 4.112800476187319, - "y": 1.9136328827222209, - "heading": -0.318553561635836, - "angularVelocity": -2.888765193967595e-7, - "velocityX": -3.436925741712075, - "velocityY": 1.5782041760469432, - "timestamp": 5.599995588027511 + "x": 6.9285315703702866, + "y": 7.445814297629393, + "heading": 0.019185726579151432, + "angularVelocity": -0.3586755233685199, + "velocityX": 3.203950829673405, + "velocityY": 0.2246450555457597, + "timestamp": 3.504263517846762 }, { - "x": 3.9019083093617053, - "y": 2.0299264483320134, - "heading": -0.31855357939397577, - "angularVelocity": -2.7886949297236075e-7, - "velocityX": -3.311799122668965, - "velocityY": 1.826245774585971, - "timestamp": 5.663674621391001 + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, + "angularVelocity": -0.2840637901552857, + "velocityX": 2.913217404608591, + "velocityY": 0.18042146002750595, + "timestamp": 3.5718037227023802 }, { - "x": 3.7001203074329077, - "y": 2.161384813738153, - "heading": -0.3185535969362317, - "angularVelocity": -2.75479306959021e-7, - "velocityX": -3.1688295388681516, - "velocityY": 2.0643900898393674, - "timestamp": 5.727353654754491 + "x": 7.284551146493795, + "y": 7.4665356658331685, + "heading": -0.011983560395956015, + "angularVelocity": -0.19978893791899283, + "velocityX": 2.6551742811725556, + "velocityY": 0.1423059220209307, + "timestamp": 3.6317848233617918 }, { - "x": 3.509285404964888, - "y": 2.2879634464070207, - "heading": -0.3579155843505842, - "angularVelocity": -0.6181310446354924, - "velocityX": -2.996824737880426, - "velocityY": 1.9877599577609337, - "timestamp": 5.791032688117982 + "x": 7.428286998104642, + "y": 7.47296871812108, + "heading": -0.020071347748117502, + "angularVelocity": -0.13483892864997732, + "velocityX": 2.396352351501819, + "velocityY": 0.10725132111930623, + "timestamp": 3.6917659240212033 }, { - "x": 3.332909755538667, - "y": 2.4050971261724468, - "heading": -0.4129449232759771, - "angularVelocity": -0.8641673093760197, - "velocityX": -2.769760156682042, - "velocityY": 1.8394387222684185, - "timestamp": 5.854711721481472 + "x": 7.556472866857842, + "y": 7.4774155432867, + "heading": -0.02498803078182015, + "angularVelocity": -0.08197053704667534, + "velocityX": 2.137104310257201, + "velocityY": 0.0741371051336659, + "timestamp": 3.751747024680615 }, { - "x": 3.171287726021972, - "y": 2.512479984884094, - "heading": -0.4699204658498491, - "angularVelocity": -0.8947300165290918, - "velocityX": -2.5380729100288124, - "velocityY": 1.6863142079856834, - "timestamp": 5.918390754844962 + "x": 7.669092898448606, + "y": 7.47995645101358, + "heading": -0.027230015787281982, + "angularVelocity": -0.037378190476904, + "velocityX": 1.877591947341067, + "velocityY": 0.04236180561785803, + "timestamp": 3.8117281253400264 }, { - "x": 3.024408806786741, - "y": 2.6100923699912792, - "heading": -0.5252799185719804, - "angularVelocity": -0.8693513358805374, - "velocityX": -2.306550704009953, - "velocityY": 1.5328810748429236, - "timestamp": 5.982069788208452 + "x": 7.766136403587462, + "y": 7.480650177988498, + "heading": -0.027158837231048453, + "angularVelocity": 0.001186683062681687, + "velocityX": 1.6179013734658474, + "velocityY": 0.011565759335732523, + "timestamp": 3.871709225999438 }, { - "x": 2.892255079193046, - "y": 2.6979336450739244, - "heading": -0.5773440254447586, - "angularVelocity": -0.8176020288434336, - "velocityX": -2.0753098879397367, - "velocityY": 1.3794379475145813, - "timestamp": 6.045748821571943 + "x": 7.847595735447885, + "y": 7.479541543526011, + "heading": -0.025049657080079103, + "angularVelocity": 0.03516407881452247, + "velocityX": 1.3580833123248393, + "velocityY": -0.018483063003166142, + "timestamp": 3.9316903266588494 }, { - "x": 2.7748123815012584, - "y": 2.7760062309054065, - "heading": -0.625128837055519, - "angularVelocity": -0.7504010203483636, - "velocityX": -1.8442914643726742, - "velocityY": 1.2260328354205916, - "timestamp": 6.109427854935433 + "x": 7.913465176456017, + "y": 7.4766658686921605, + "heading": -0.021118970688138776, + "angularVelocity": 0.06553208175121362, + "velocityX": 1.0981699282605077, + "velocityY": -0.04794301542047442, + "timestamp": 3.991671427318261 }, { - "x": 2.6720699818921556, - "y": 2.844312916696855, - "heading": -0.6679860381773631, - "angularVelocity": -0.6730190277419643, - "velocityX": -1.6134415706757543, - "velocityY": 1.0726715244175122, - "timestamp": 6.173106888298923 + "x": 7.963740302847386, + "y": 7.472051704673601, + "heading": -0.015541591171907823, + "angularVelocity": 0.09298561471722216, + "velocityX": 0.8381827915570343, + "velocityY": -0.07692696479112789, + "timestamp": 4.051652527977672 }, { - "x": 2.5840196243973645, - "y": 2.9028562717726505, - "heading": -0.7054558016839698, - "angularVelocity": -0.5884160221579265, - "velocityX": -1.382721326691407, - "velocityY": 0.9193505614575078, - "timestamp": 6.236785921662413 + "x": 7.998417597522527, + "y": 7.465722607991469, + "heading": -0.008461644038298986, + "angularVelocity": 0.11803629903043378, + "velocityX": 0.5781370180592035, + "velocityY": -0.10551818176978627, + "timestamp": 4.111633628637083 }, { - "x": 2.510654811151598, - "y": 2.95163853061359, - "heading": -0.7371951288142529, - "angularVelocity": -0.4984266477336408, - "velocityX": -1.1521031235978156, - "velocityY": 0.7660646882386306, - "timestamp": 6.3004649550259035 + "x": 8.017494201660156, + "y": 7.457698345184326, + "heading": 0, + "angularVelocity": 0.14107183671647575, + "velocityX": 0.3180435825269393, + "velocityY": -0.1337798526356988, + "timestamp": 4.171614729296494 }, { - "x": 2.4519703126340358, - "y": 2.9906615990271557, - "heading": -0.7629386573341134, - "angularVelocity": -0.4042700895428552, - "velocityX": -0.9215670436230066, - "velocityY": 0.6128087433553882, - "timestamp": 6.364143988389394 + "x": 8.008975668857596, + "y": 7.440959395735479, + "heading": 0.016440344425850486, + "angularVelocity": 0.17454816573497248, + "velocityX": -0.09044179592138195, + "velocityY": -0.17771847395317164, + "timestamp": 4.265802739560738 }, { - "x": 2.4079618327799786, - "y": 3.0199270886206313, - "heading": -0.7824753669479414, - "angularVelocity": -0.3067997201262361, - "velocityX": -0.6910984279998401, - "velocityY": 0.4595781067596215, - "timestamp": 6.427823021752884 + "x": 7.961981832760565, + "y": 7.4200887947149115, + "heading": 0.0360050890089686, + "angularVelocity": 0.20772011775415367, + "velocityX": -0.4989364990850843, + "velocityY": -0.22158447728129313, + "timestamp": 4.3599907498249815 }, { - "x": 2.378625774904473, - "y": 3.039436353320747, - "heading": -0.795633833561549, - "angularVelocity": -0.20663734856804464, - "velocityX": -0.4606862938394533, - "velocityY": 0.3063687318988399, - "timestamp": 6.491502055116374 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.10425061542851642, - "velocityX": -0.2303222914890624, - "velocityY": 0.15317707264507022, - "timestamp": 6.5551810884798645 + "x": 7.876511570645417, + "y": 7.39509531765153, + "heading": 0.058657775076795544, + "angularVelocity": 0.24050498576490828, + "velocityX": -0.9074431222759898, + "velocityY": -0.2653573102697685, + "timestamp": 4.454178760089225 }, { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -1.4092752497411054e-30, - "velocityX": 2.9428006004895336e-31, - "velocityY": 5.314710200495422e-31, - "timestamp": 6.618860121843355 + "x": 7.752563396875145, + "y": 7.365990633137338, + "heading": 0.08435039467917142, + "angularVelocity": 0.2727801503641011, + "velocityX": -1.315965518567977, + "velocityY": -0.3090062570866462, + "timestamp": 4.548366770353469 }, { - "x": 2.3794842314620843, - "y": 3.041727640405621, - "heading": -0.7962432666233688, - "angularVelocity": 0.0957711014436812, - "velocityX": 0.24661230413774035, - "velocityY": -0.11854554422657079, - "timestamp": 6.681813823589832 - }, - { - "x": 2.410536447869941, - "y": 3.0268006675090624, - "heading": -0.7842917338243374, - "angularVelocity": 0.18984638658997072, - "velocityX": 0.4932548134009415, - "velocityY": -0.23711032842312177, - "timestamp": 6.744767525336309 - }, - { - "x": 2.4571179241280796, - "y": 3.0044081651489933, - "heading": -0.7665449064601504, - "angularVelocity": 0.28190284084732253, - "velocityX": 0.7399322830248891, - "velocityY": -0.3556979452971155, - "timestamp": 6.807721227082786 - }, - { - "x": 2.5192312369840457, - "y": 2.9745484345249116, - "heading": -0.743155503226628, - "angularVelocity": 0.37153340605314533, - "velocityX": 0.9866506834833091, - "velocityY": -0.4743125470894598, - "timestamp": 6.8706749288292635 - }, - { - "x": 2.596879439737626, - "y": 2.937219467270397, - "heading": -0.714309410311632, - "angularVelocity": 0.4582112268975565, - "velocityX": 1.233417584660547, - "velocityY": -0.5929590511586358, - "timestamp": 6.933628630575741 - }, - { - "x": 2.690066196626848, - "y": 2.892418878795792, - "heading": -0.6802368283616581, - "angularVelocity": 0.5412323819683986, - "velocityX": 1.4802426911208155, - "velocityY": -0.7116434336939077, - "timestamp": 6.996582332322218 - }, - { - "x": 2.798795963694134, - "y": 2.8401438158547943, - "heading": -0.6412294413317386, - "angularVelocity": 0.6196202280051365, - "velocityX": 1.7271385804309867, - "velocityY": -0.8303731391605256, - "timestamp": 7.059536034068695 - }, - { - "x": 2.9230742308720847, - "y": 2.7803908299690017, - "heading": -0.5976682545562845, - "angularVelocity": 0.6919559226378926, - "velocityX": 1.9741216756154527, - "velocityY": -0.9491576226355342, - "timestamp": 7.122489735815172 - }, - { - "x": 3.062907829622796, - "y": 2.713155712528932, - "heading": -0.5500718966234338, - "angularVelocity": 0.7560533632244142, - "velocityX": 2.22121328645359, - "velocityY": -1.0680089585650514, - "timestamp": 7.185443437561649 - }, - { - "x": 3.218305230273769, - "y": 2.638433320090989, - "heading": -0.4991885737635272, - "angularVelocity": 0.8082657802208455, - "velocityX": 2.4684394458133507, - "velocityY": -1.1869419964986359, - "timestamp": 7.248397139308127 - }, - { - "x": 3.389276272924326, - "y": 2.556217612467452, - "heading": -0.4461960441931904, - "angularVelocity": 0.8417698737358584, - "velocityX": 2.715821912094713, - "velocityY": -1.3059709809381974, - "timestamp": 7.311350841054604 - }, - { - "x": 3.575827436112715, - "y": 2.4665034787202997, - "heading": -0.3932374103043789, - "angularVelocity": 0.841231451362194, - "velocityX": 2.9633072879440143, - "velocityY": -1.4250811510408536, - "timestamp": 7.374304542801081 - }, - { - "x": 3.777911234478158, - "y": 2.369308059755886, - "heading": -0.34555593776982274, - "angularVelocity": 0.7574053822375035, - "velocityX": 3.210038373585424, - "velocityY": -1.5439190431697394, - "timestamp": 7.437258244547558 - }, - { - "x": 3.9926293770053456, - "y": 2.266439040708753, - "heading": -0.3455557491739834, - "angularVelocity": 0.000002995786333953476, - "velocityX": 3.4107310066036556, - "velocityY": -1.6340424183696203, - "timestamp": 7.500211946294035 - }, - { - "x": 4.207200483864901, - "y": 2.1632635581376745, - "heading": -0.34555573252846533, - "angularVelocity": 2.644088848166298e-7, - "velocityX": 3.4083953906898548, - "velocityY": -1.6389104962656516, - "timestamp": 7.5631656480405125 - }, - { - "x": 4.421771581956281, - "y": 2.0600880573316713, - "heading": -0.3455557158829472, - "angularVelocity": 2.6440888572481287e-7, - "velocityX": 3.408395251410094, - "velocityY": -1.638910785921765, - "timestamp": 7.62611934978699 - }, - { - "x": 4.636342680047139, - "y": 1.9569125565245837, - "heading": -0.3455556992374292, - "angularVelocity": 2.6440888404451857e-7, - "velocityX": 3.4083952514018088, - "velocityY": -1.6389107859389946, - "timestamp": 7.689073051533467 - }, - { - "x": 4.850913778140846, - "y": 1.853737055723424, - "heading": -0.3455556825919111, - "angularVelocity": 2.64408884846605e-7, - "velocityX": 3.408395251447087, - "velocityY": -1.63891078584483, - "timestamp": 7.752026753279944 - }, - { - "x": 5.065484924140348, - "y": 1.7505616545506137, - "heading": -0.34555566594639303, - "angularVelocity": 2.644088848456774e-7, - "velocityX": 3.4083960124157384, - "velocityY": -1.6389092032794423, - "timestamp": 7.814980455026421 - }, - { - "x": 5.280845302209934, - "y": 1.6490439336616942, - "heading": -0.34555564929194366, - "angularVelocity": 2.6455075541297124e-7, - "velocityX": 3.4209327187283005, - "velocityY": -1.6125774668143436, - "timestamp": 7.877934156772898 + "x": 7.590135252234061, + "y": 7.332791018782474, + "heading": 0.11301654484596554, + "angularVelocity": 0.3043503104733978, + "velocityX": -1.7245097776818146, + "velocityY": -0.35248238349789773, + "timestamp": 4.642554780617712 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, - "angularVelocity": 2.6730250915052296e-7, - "velocityX": 3.52891551864971, - "velocityY": -1.360125725304771, - "timestamp": 7.940887858519376 - }, - { - "x": 5.655413318775525, - "y": 1.5130951799644636, - "heading": -0.34555563491945995, - "angularVelocity": -5.785220393859716e-8, - "velocityX": 3.5912523855702734, - "velocityY": -1.18578710714545, - "timestamp": 7.983327014878419 - }, - { - "x": 5.810103118724588, - "y": 1.4702901058122384, - "heading": -0.3455556373457815, - "angularVelocity": -5.7171766263130695e-8, - "velocityX": 3.6449782045702985, - "velocityY": -1.0086221740622332, - "timestamp": 8.025766171237462 - }, - { - "x": 5.966313456902953, - "y": 1.4334181808859694, - "heading": -0.34555563976573417, - "angularVelocity": -5.702169541040829e-8, - "velocityX": 3.680806867525754, - "velocityY": -0.8688185178405917, - "timestamp": 8.068205327596505 - }, - { - "x": 6.1225244823663525, - "y": 1.3965491678093302, - "heading": -0.3455556421856862, - "angularVelocity": -5.7021679533312126e-8, - "velocityX": 3.680823062122731, - "velocityY": -0.868749905505203, - "timestamp": 8.110644483955548 - }, - { - "x": 6.278735507957822, - "y": 1.3596801552753135, - "heading": -0.3455556446056381, - "angularVelocity": -5.702167859922657e-8, - "velocityX": 3.680823065140465, - "velocityY": -0.8687498927193055, - "timestamp": 8.153083640314591 - }, - { - "x": 6.434946533537413, - "y": 1.3228111426909752, - "heading": -0.34555564702559005, - "angularVelocity": -5.7021678932866206e-8, - "velocityX": 3.680823064860606, - "velocityY": -0.868749893905046, - "timestamp": 8.195522796673634 - }, - { - "x": 6.591157495254782, - "y": 1.285941859528607, - "heading": -0.3455556494455419, - "angularVelocity": -5.7021677677252914e-8, - "velocityX": 3.6808215600657594, - "velocityY": -0.8687562695744264, - "timestamp": 8.237961953032677 - }, - { - "x": 6.747031505398479, - "y": 1.2476730535838276, - "heading": -0.34555565194208476, - "angularVelocity": -5.8826401859372655e-8, - "velocityX": 3.6728819212374164, - "velocityY": -0.9017334279932018, - "timestamp": 8.28040110939172 - }, - { - "x": 6.897830261276489, - "y": 1.2034176695315386, - "heading": -0.3468060133183351, - "angularVelocity": -0.029462446559305244, - "velocityX": 3.5532929684610997, - "velocityY": -1.0427960367044076, - "timestamp": 8.322840265750763 - }, - { - "x": 7.040966542913821, - "y": 1.160898981796106, - "heading": -0.34756195184747823, - "angularVelocity": -0.017812289262957588, - "velocityX": 3.3727409759603195, - "velocityY": -1.001874009363348, - "timestamp": 8.365279422109806 - }, - { - "x": 7.176448137174031, - "y": 1.12015095875213, - "heading": -0.34781665101790943, - "angularVelocity": -0.006001513514462224, - "velocityX": 3.1923724664555144, - "velocityY": -0.9601515802821338, - "timestamp": 8.407718578468849 - }, - { - "x": 7.30427768133993, - "y": 1.0811849503221362, - "heading": -0.3475678387477372, - "angularVelocity": 0.005862799629361019, - "velocityX": 3.0120660996283153, - "velocityY": -0.918161711329369, - "timestamp": 8.450157734827892 - }, - { - "x": 7.424456501916377, - "y": 1.044006636667021, - "heading": -0.3468143875407301, - "angularVelocity": 0.017753680130510166, - "velocityX": 2.8317909894275823, - "velocityY": -0.8760379999211017, - "timestamp": 8.492596891186935 + "x": 7.3892240901384, + "y": 7.295520773303632, + "heading": 0.14455765680612448, + "angularVelocity": 0.33487395977121315, + "velocityX": -2.13308638256617, + "velocityY": -0.39570052891318314, + "timestamp": 4.736742790881956 }, { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, - "angularVelocity": 0.02966022853563592, - "velocityX": 2.65153469287831, - "velocityY": -0.8338339406813241, - "timestamp": 8.535036047545978 - }, - { - "x": 7.689789531297121, - "y": 0.9591642629851495, - "heading": -0.34249163023542684, - "angularVelocity": 0.04769743470803589, - "velocityX": 2.378707539440784, - "velocityY": -0.769870356769085, - "timestamp": 8.599274349827635 - }, - { - "x": 7.82516177952376, - "y": 0.9142093996063804, - "heading": -0.3392101704632614, - "angularVelocity": 0.05108260423473965, - "velocityX": 2.1073447369933547, - "velocityY": -0.6998140016475143, - "timestamp": 8.663512652109292 - }, - { - "x": 7.943169958712402, - "y": 0.8740090692537413, - "heading": -0.33632582750820933, - "angularVelocity": 0.044900672225200826, - "velocityX": 1.837037639494695, - "velocityY": -0.6258000122166694, - "timestamp": 8.727750954390949 - }, - { - "x": 8.043864833075729, - "y": 0.8387415563520763, - "heading": -0.3342715201633078, - "angularVelocity": 0.03197947753809357, - "velocityX": 1.567520790350633, - "velocityY": -0.5490106626266708, - "timestamp": 8.791989256672606 - }, - { - "x": 8.12728568078066, - "y": 0.8085387560486261, - "heading": -0.33336873570137193, - "angularVelocity": 0.014053678722353937, - "velocityX": 1.2986153858669556, - "velocityY": -0.4701680964578417, - "timestamp": 8.856227558954263 - }, - { - "x": 8.193463737140812, - "y": 0.7835021726798679, - "heading": -0.33386568134146205, - "angularVelocity": -0.007735970946293819, - "velocityX": 1.030196222652151, - "velocityY": -0.38974540857234485, - "timestamp": 8.92046586123592 - }, - { - "x": 8.242424408463135, - "y": 0.7637123264900073, - "heading": -0.33595980135953535, - "angularVelocity": -0.03259924287680334, - "velocityX": 0.7621725603464993, - "velocityY": -0.3080692591016908, - "timestamp": 8.984704163517577 - }, - { - "x": 8.274188750584008, - "y": 0.7492346436854774, - "heading": -0.3398119288829893, - "angularVelocity": -0.05996620998114242, - "velocityX": 0.4944766750154696, - "velocityY": -0.2253746174836856, - "timestamp": 9.048942465799234 - }, - { - "x": 8.288774490356445, - "y": 0.7401233315467834, - "heading": -0.34555563246426124, - "angularVelocity": -0.08941244362418393, - "velocityX": 0.2270567442533848, - "velocityY": -0.14183612914838375, - "timestamp": 9.113180768080891 - }, - { - "x": 8.286058974983396, - "y": 0.7364361738247144, - "heading": -0.3533713967772531, - "angularVelocity": -0.1208300500196775, - "velocityX": -0.04198128874091145, - "velocityY": -0.05700267231030528, - "timestamp": 9.177864713107141 - }, - { - "x": 8.26594084231072, - "y": 0.7382366486349878, - "heading": -0.3631834148956511, - "angularVelocity": -0.15169170826572306, - "velocityX": -0.3110220420927156, - "velocityY": 0.027834956719829915, - "timestamp": 9.24254865813339 - }, - { - "x": 8.22841989282902, - "y": 0.7455250700569162, - "heading": -0.3749497746847549, - "angularVelocity": -0.18190541384463682, - "velocityX": -0.5800658798172126, - "velocityY": 0.11267744134917045, - "timestamp": 9.30723260315964 - }, - { - "x": 8.173495898954863, - "y": 0.7583018091084519, - "heading": -0.38862097650887273, - "angularVelocity": -0.21135386560868902, - "velocityX": -0.8491132359330754, - "velocityY": 0.1975256618369646, - "timestamp": 9.37191654818589 - }, - { - "x": 8.101168599327213, - "y": 0.7765673113174031, - "heading": -0.4041376276128682, - "angularVelocity": -0.23988411804039605, - "velocityX": -1.11816463263485, - "velocityY": 0.2823807700896826, - "timestamp": 9.43660049321214 - }, - { - "x": 8.011437691690773, - "y": 0.8003221216608263, - "heading": -0.42142708913788907, - "angularVelocity": -0.26729138920027884, - "velocityX": -1.3872207021390834, - "velocityY": 0.36724430357151194, - "timestamp": 9.50128443823839 - }, - { - "x": 7.904302824322884, - "y": 0.8295669211666158, - "heading": -0.44039841224325665, - "angularVelocity": -0.29329261067284323, - "velocityX": -1.6562822092006166, - "velocityY": 0.45211836559939883, - "timestamp": 9.56596838326464 - }, - { - "x": 7.779763586783285, - "y": 0.8643025828479174, - "heading": -0.46093432615418145, - "angularVelocity": -0.317480850968364, - "velocityX": -1.9253500615811048, - "velocityY": 0.5370059242243979, - "timestamp": 9.63065232829089 - }, - { - "x": 7.63781950374517, - "y": 0.9045302615405436, - "heading": -0.48287779045070706, - "angularVelocity": -0.33924127985113656, - "velocityX": -2.194425262412658, - "velocityY": 0.6219113363648586, - "timestamp": 9.695336273317139 - }, - { - "x": 7.478470046612931, - "y": 0.9502515474489494, - "heading": -0.5060075980261466, - "angularVelocity": -0.3575818940241346, - "velocityX": -2.4635086352196294, - "velocityY": 0.7068413327271726, - "timestamp": 9.760020218343389 - }, - { - "x": 7.301714722310279, - "y": 1.0014687500113455, - "heading": -0.5299890975728885, - "angularVelocity": -0.37074887032647375, - "velocityX": -2.732599630880914, - "velocityY": 0.7918070325118697, - "timestamp": 9.824704163369638 - }, - { - "x": 7.107553521342319, - "y": 1.0581854755780744, - "heading": -0.5542575516052335, - "angularVelocity": -0.37518512549746846, - "velocityX": -3.0016907733312475, - "velocityY": 0.8768284857040194, - "timestamp": 9.889388108395888 - }, - { - "x": 6.895989597473325, - "y": 1.1204078946209601, - "heading": -0.5776591781660143, - "angularVelocity": -0.3617841575878507, - "velocityX": -3.2707331592582385, - "velocityY": 0.961945333075069, - "timestamp": 9.954072053422138 - }, - { - "x": 6.667061067423103, - "y": 1.188145012147935, - "heading": -0.5965312726664429, - "angularVelocity": -0.2917585575952417, - "velocityX": -3.5391862688232427, - "velocityY": 1.0472013959489617, - "timestamp": 10.018755998448388 - }, - { - "x": 6.433742027972299, - "y": 1.2616769248691375, - "heading": -0.5965312781733252, - "angularVelocity": -8.513522502240808e-8, - "velocityX": -3.607062608134345, - "velocityY": 1.136787694247182, - "timestamp": 10.083439943474637 - }, - { - "x": 6.200423078644191, - "y": 1.3352091235520722, - "heading": -0.5965312836800714, - "angularVelocity": -8.513312137357674e-8, - "velocityX": -3.6070612148566714, - "velocityY": 1.1367921151546025, - "timestamp": 10.148123888500887 - }, - { - "x": 5.967104129330135, - "y": 1.4087413222795935, - "heading": -0.5965312891868175, - "angularVelocity": -8.513312220445073e-8, - "velocityX": -3.607061214639434, - "velocityY": 1.136792115843899, - "timestamp": 10.212807833527137 - }, - { - "x": 5.733785345709214, - "y": 1.4822740467522528, - "heading": -0.5965312946935636, - "angularVelocity": -8.513311865132804e-8, - "velocityX": -3.6070586530588855, - "velocityY": 1.1368002437516491, - "timestamp": 10.277491778553387 + "x": 7.149824938411661, + "y": 7.254220108341429, + "heading": 0.1788109128374065, + "angularVelocity": 0.36366896312157765, + "velocityX": -2.54171577735964, + "velocityY": -0.4384917448232911, + "timestamp": 4.8309308011461995 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.5965313002103377, - "angularVelocity": -8.528815258110653e-8, - "velocityX": -3.567836630187545, - "velocityY": 1.254483436778072, - "timestamp": 10.342175723579636 - }, - { - "x": 5.285431093647359, - "y": 1.6581112689784743, - "heading": -0.5965313049795081, - "angularVelocity": -7.601302145092289e-8, - "velocityX": -3.4677610362467894, - "velocityY": 1.509245007513348, - "timestamp": 10.404917216242477 - }, - { - "x": 5.070948144949037, - "y": 1.7596075143791599, - "heading": -0.5965313097283005, - "angularVelocity": -7.568822719486902e-8, - "velocityX": -3.4185184252932688, - "velocityY": 1.6176893646140194, - "timestamp": 10.467658708905319 - }, - { - "x": 4.856465419222307, - "y": 1.861104230964457, - "heading": -0.5965313144770923, - "angularVelocity": -7.568821760685447e-8, - "velocityX": -3.4185148714793154, - "velocityY": 1.6176968745502942, - "timestamp": 10.53040020156816 - }, - { - "x": 4.641982693508075, - "y": 1.9626009475761665, - "heading": -0.5965313192258841, - "angularVelocity": -7.568821790279648e-8, - "velocityX": -3.418514871280104, - "velocityY": 1.6176968749712675, - "timestamp": 10.593141694231 - }, - { - "x": 4.427499967793844, - "y": 2.064097664187878, - "heading": -0.5965313239746759, - "angularVelocity": -7.568821822441872e-8, - "velocityX": -3.4185148712800926, - "velocityY": 1.617696874971291, - "timestamp": 10.655883186893842 - }, - { - "x": 4.213017242079613, - "y": 2.165594380799589, - "heading": -0.5965313287234677, - "angularVelocity": -7.568821811615467e-8, - "velocityX": -3.4185148712800917, - "velocityY": 1.617696874971293, - "timestamp": 10.718624679556683 - }, - { - "x": 3.998534516366738, - "y": 2.267091097414165, - "heading": -0.5965313334722594, - "angularVelocity": -7.56882168459371e-8, - "velocityX": -3.418514871258486, - "velocityY": 1.6176968750169503, - "timestamp": 10.781366172219524 - }, - { - "x": 3.7840518148370346, - "y": 2.3685878651306878, - "heading": -0.5965313382224369, - "angularVelocity": -7.571030356184493e-8, - "velocityX": -3.418514485817036, - "velocityY": 1.6176976895010247, - "timestamp": 10.844107664882365 - }, - { - "x": 3.5823467745214304, - "y": 2.46403001529431, - "heading": -0.6258886394381601, - "angularVelocity": -0.46790887449049295, - "velocityX": -3.214858808022383, - "velocityY": 1.5211966772373215, - "timestamp": 10.906849157545206 - }, - { - "x": 3.3961574491843427, - "y": 2.552130310684733, - "heading": -0.6529986278149837, - "angularVelocity": -0.43209026795882993, - "velocityX": -2.967562890759225, - "velocityY": 1.40417914288166, - "timestamp": 10.969590650208048 - }, - { - "x": 3.225483861981861, - "y": 2.6328887858865286, - "heading": -0.6778585687680168, - "angularVelocity": -0.3962280764760425, - "velocityX": -2.7202666044246855, - "velocityY": 1.2871621597492864, - "timestamp": 11.032332142870889 - }, - { - "x": 3.0703260256030673, - "y": 2.7063054766361296, - "heading": -0.7004659993691003, - "angularVelocity": -0.36032662982009556, - "velocityX": -2.4729701158463, - "velocityY": 1.17014574619904, - "timestamp": 11.09507363553373 - }, - { - "x": 2.9306839491479293, - "y": 2.7723804156875507, - "heading": -0.7208186227896802, - "angularVelocity": -0.32438857535555815, - "velocityX": -2.2256734822288196, - "velocityY": 1.0531298546958971, - "timestamp": 11.157815128196571 - }, - { - "x": 2.8065576397395007, - "y": 2.8311136320269066, - "heading": -0.7389143343573537, - "angularVelocity": -0.28841697574707026, - "velocityX": -1.9783767350811556, - "velocityY": 0.9361144251855119, - "timestamp": 11.220556620859412 - }, - { - "x": 2.697947103240289, - "y": 2.8825051506567756, - "heading": -0.7547512635952348, - "angularVelocity": -0.25241556370017226, - "velocityX": -1.7310798944944172, - "velocityY": 0.8190993941766099, - "timestamp": 11.283298113522253 - }, - { - "x": 2.6048523446905505, - "y": 2.9265549925489487, - "heading": -0.7683278153992709, - "angularVelocity": -0.21638872822158642, - "velocityX": -1.4837829735739574, - "velocityY": 0.702084697424841, - "timestamp": 11.346039606185094 - }, - { - "x": 2.527273368620795, - "y": 2.9632631746604594, - "heading": -0.779642706302846, - "angularVelocity": -0.18034143631837077, - "velocityX": -1.2364859804443729, - "velocityY": 0.5850702709413225, - "timestamp": 11.408781098847935 - }, - { - "x": 2.4652101792880994, - "y": 2.9926297099741825, - "heading": -0.7886949946743065, - "angularVelocity": -0.1442791362982944, - "velocityX": -0.9891889194637066, - "velocityY": 0.46805605138424683, - "timestamp": 11.471522591510777 - }, - { - "x": 2.4186627808554446, - "y": 3.014654607545873, - "heading": -0.7954841045559687, - "angularVelocity": -0.10820765642515946, - "velocityX": -0.7418917921317392, - "velocityY": 0.3510419761615724, - "timestamp": 11.534264084173618 - }, - { - "x": 2.3876311775224326, - "y": 3.0293378725476474, - "heading": -0.8000098431420335, - "angularVelocity": -0.07213310353301693, - "velocityX": -0.49459459786475296, - "velocityY": 0.23402798337424993, - "timestamp": 11.597005576836459 + "x": 6.871928226350113, + "y": 7.208968463075644, + "heading": 0.21545413209734332, + "angularVelocity": 0.3890433523028524, + "velocityX": -2.9504467849136042, + "velocityY": -0.4804395499897761, + "timestamp": 4.925118811410443 }, { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -0.036061762982130895, - "velocityX": -0.24729733470577522, - "velocityY": 0.11701401166345779, - "timestamp": 11.6597470694993 + "x": 6.555508023323144, + "y": 7.15999689291417, + "heading": 0.25354713749374036, + "angularVelocity": 0.40443582245264, + "velocityX": -3.3594530995957568, + "velocityY": -0.5199342254293745, + "timestamp": 5.019306821674687 }, { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": 2.8180807610025885e-36, - "velocityX": -1.7033240054088048e-37, - "velocityY": 8.364918339692578e-38, - "timestamp": 11.722488562162141 + "x": 6.201946981022424, + "y": 7.116596127430346, + "heading": 0.25354714311943394, + "angularVelocity": 5.972834101491316e-8, + "velocityX": -3.753779714730222, + "velocityY": -0.46078864350212084, + "timestamp": 5.11349483193893 }, { - "x": 2.3862784269967885, - "y": 3.0299921206795677, - "heading": -0.7979445022798209, - "angularVelocity": 0.07214760590413402, - "velocityX": 0.2361025217555023, - "velocityY": -0.11148080618080956, - "timestamp": 11.782475441082498 + "x": 5.847829680383322, + "y": 7.077995922009697, + "heading": 0.25354714640605686, + "angularVelocity": 3.4894280958051e-8, + "velocityX": -3.7596855443238373, + "velocityY": -0.4098207968546854, + "timestamp": 5.207682842203174 }, { - "x": 2.414605712310544, - "y": 3.016617241283919, - "heading": -0.7893555783917804, - "angularVelocity": 0.1431800427464117, - "velocityX": 0.4722246901920792, - "velocityY": -0.22296341527296182, - "timestamp": 11.842462320002856 + "x": 5.493712379149646, + "y": 7.039395722043647, + "heading": 0.25354714969267966, + "angularVelocity": 3.4894280048226024e-8, + "velocityX": -3.759685550636459, + "velocityY": -0.40982073894285953, + "timestamp": 5.301870852467418 }, { - "x": 2.457098595113841, - "y": 2.996554734272641, - "heading": -0.7765849369819304, - "angularVelocity": 0.2128905794016223, - "velocityX": 0.7083696229589314, - "velocityY": -0.33444825555792773, - "timestamp": 11.902449198923213 + "x": 5.13959507794107, + "y": 7.0007955218474756, + "heading": 0.25354715297933, + "angularVelocity": 3.489457219723442e-8, + "velocityX": -3.7596855503699804, + "velocityY": -0.4098207413860709, + "timestamp": 5.396058862731661 }, { - "x": 2.5137586742709717, - "y": 2.9698044458506936, - "heading": -0.759727315329062, - "angularVelocity": 0.281021816041631, - "velocityX": 0.944541209292721, - "velocityY": -0.44593565965421617, - "timestamp": 11.96243607784357 + "x": 4.793375065328026, + "y": 6.963049240971564, + "heading": 0.2809926873119857, + "angularVelocity": 0.2913909557666365, + "velocityX": -3.675839543076952, + "velocityY": -0.4007546265178986, + "timestamp": 5.490246872995905 }, { - "x": 2.5845878442638655, - "y": 2.9363662114763, - "heading": -0.7388971783301493, - "angularVelocity": 0.34724488711220036, - "velocityX": 1.180744377231747, - "velocityY": -0.5574258067133014, - "timestamp": 12.022422956763927 + "x": 4.4856271372174605, + "y": 6.929500155423233, + "heading": 0.3062072463057812, + "angularVelocity": 0.2677045509620205, + "velocityX": -3.2673790140292933, + "velocityY": -0.3561927410315718, + "timestamp": 5.5844348832601485 }, { - "x": 2.669588381015972, - "y": 2.8962398712988433, - "heading": -0.7142349782126004, - "angularVelocity": 0.41112657570152883, - "velocityX": 1.4169854855252446, - "velocityY": -0.6689186185320778, - "timestamp": 12.082409835684285 + "x": 4.216349125910067, + "y": 6.900146018877801, + "heading": 0.32859640421091696, + "angularVelocity": 0.23770709076795637, + "velocityX": -2.858941499580855, + "velocityY": -0.31165470491497754, + "timestamp": 5.678622893524392 }, { - "x": 2.768763062860025, - "y": 2.849425297465716, - "heading": -0.6859166617272396, - "angularVelocity": 0.4720751770225952, - "velocityX": 1.653272909492815, - "velocityY": -0.7804135616937369, - "timestamp": 12.142396714604642 + "x": 3.9855401854111148, + "y": 6.874986045056756, + "heading": 0.34796083888612495, + "angularVelocity": 0.2055934149249067, + "velocityX": -2.450512967111424, + "velocityY": -0.26712501676655553, + "timestamp": 5.772810903788636 }, { - "x": 2.8821153449579997, - "y": 2.795922445610523, - "heading": -0.6541688123580982, - "angularVelocity": 0.5292465609236296, - "velocityX": 1.8896179320892514, - "velocityY": -0.8919092444570627, - "timestamp": 12.202383593525 + "x": 3.7931998687162816, + "y": 6.854019825581219, + "heading": 0.36419988308525797, + "angularVelocity": 0.17241094862896578, + "velocityX": -2.042089180514842, + "velocityY": -0.22259966440226955, + "timestamp": 5.866998914052879 }, { - "x": 3.0096496153353156, - "y": 2.73573145985777, - "heading": -0.6192942663212868, - "angularVelocity": 0.5813695705541383, - "velocityX": 2.1260361044394105, - "velocityY": -1.0034025246198655, - "timestamp": 12.262370472445356 + "x": 3.639327899901621, + "y": 6.837247108195556, + "heading": 0.37725261237367164, + "angularVelocity": 0.13858164379727655, + "velocityX": -1.6336683234200817, + "velocityY": -0.17807699025180046, + "timestamp": 5.961186924317123 }, { - "x": 3.151371569227902, - "y": 2.6688529097388005, - "heading": -0.5817190571900679, - "angularVelocity": 0.6263904675071726, - "velocityX": 2.3625492181506123, - "velocityY": -1.1148863105173612, - "timestamp": 12.322357351365714 + "x": 3.5239240923326007, + "y": 6.824667722043819, + "heading": 0.38707830352299477, + "angularVelocity": 0.10431997790119188, + "velocityX": -1.2252494478358333, + "velocityY": -0.13355613008965372, + "timestamp": 6.0553749345813666 }, { - "x": 3.30728870131474, - "y": 2.5952884046594, - "heading": -0.5420886244414685, - "angularVelocity": 0.6606516868666429, - "velocityX": 2.5991872705003334, - "velocityY": -1.2263432671179575, - "timestamp": 12.382344230286071 + "x": 3.4469883122501845, + "y": 6.8162815454148475, + "heading": 0.39364810050149374, + "angularVelocity": 0.06975194571015403, + "velocityX": -0.8168319923796448, + "velocityY": -0.08903656214250527, + "timestamp": 6.14956294484561 }, { - "x": 3.477410464899007, - "y": 2.5150425607670805, - "heading": -0.501501110267189, - "angularVelocity": 0.6766065330414338, - "velocityX": 2.8359829123654157, - "velocityY": -1.3377232710983202, - "timestamp": 12.442331109206428 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.03495947190378589, + "velocityX": -0.4084155935928461, + "velocityY": -0.04451793673752315, + "timestamp": 6.243750955109854 }, { - "x": 3.661742704544347, - "y": 2.428132084038069, - "heading": -0.46226256646132613, - "angularVelocity": 0.6541187758402722, - "velocityX": 3.0728759849311573, - "velocityY": -1.4488247812392314, - "timestamp": 12.502317988126785 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0, + "velocityX": -1.9952146739162206e-34, + "velocityY": -3.951795765453016e-33, + "timestamp": 6.337938965374097 }, { - "x": 3.860146403975295, - "y": 2.3346891355318427, - "heading": -0.4327535611710109, - "angularVelocity": 0.4919243311440368, - "velocityX": 3.307451612782871, - "velocityY": -1.5577231252568988, - "timestamp": 12.562304867047143 + "x": 3.425127749537185, + "y": 6.810478633677757, + "heading": 0.3886892234850989, + "angularVelocity": -0.13293807639169805, + "velocityX": 0.2675517930120232, + "velocityY": -0.025935588272518423, + "timestamp": 6.400010272097332 }, { - "x": 4.065242173401202, - "y": 2.237711412138144, - "heading": -0.4327535477418668, - "angularVelocity": 2.238680243649846e-7, - "velocityX": 3.41901050891823, - "velocityY": -1.6166489262168846, - "timestamp": 12.6222917459675 + "x": 3.458347138585669, + "y": 6.807258412874914, + "heading": 0.3723514064813247, + "angularVelocity": -0.26321045691257755, + "velocityX": 0.535181081278085, + "velocityY": -0.05187937829625554, + "timestamp": 6.462081578820567 }, { - "x": 4.270337924993822, - "y": 2.140733651028875, - "heading": -0.4327535343131544, - "angularVelocity": 2.2386082909736944e-7, - "velocityX": 3.419010211631728, - "velocityY": -1.6166495549472233, - "timestamp": 12.682278624887857 + "x": 3.508184213080087, + "y": 6.802427294600358, + "heading": 0.34812438720722594, + "angularVelocity": -0.3903094771652888, + "velocityX": 0.8029003596884762, + "velocityY": -0.07783174754300737, + "timestamp": 6.524152885543802 }, { - "x": 4.475433676585232, - "y": 2.0437558899170503, - "heading": -0.43275352088444197, - "angularVelocity": 2.2386082765717578e-7, - "velocityX": 3.419010211611586, - "velocityY": -1.6166495549898217, - "timestamp": 12.742265503808214 + "x": 3.5746455076736363, + "y": 6.795984642746885, + "heading": 0.31624502791908643, + "angularVelocity": -0.5135925272251962, + "velocityX": 1.070724914651617, + "velocityY": -0.10379436479708294, + "timestamp": 6.586224192267037 }, { - "x": 4.6805294281766425, - "y": 1.9467781288052255, - "heading": -0.43275350745572955, - "angularVelocity": 2.2386082872977313e-7, - "velocityX": 3.4190102116115844, - "velocityY": -1.616649554989824, - "timestamp": 12.802252382728572 + "x": 3.6577387353786484, + "y": 6.787929625128416, + "heading": 0.2770021560106679, + "angularVelocity": -0.6322224225666108, + "velocityX": 1.3386737301264124, + "velocityY": -0.12977038898779852, + "timestamp": 6.648295498990271 }, { - "x": 4.885625179768054, - "y": 1.8498003676934025, - "heading": -0.4327534940270171, - "angularVelocity": 2.2386082836686737e-7, - "velocityX": 3.419010211611597, - "velocityY": -1.6166495549897981, - "timestamp": 12.862239261648929 + "x": 3.7574730901724314, + "y": 6.7782611086258875, + "heading": 0.23075444743671972, + "angularVelocity": -0.7450738677080124, + "velocityX": 1.6067706652041787, + "velocityY": -0.1557646682971112, + "timestamp": 6.710366805713506 }, { - "x": 5.090720931370458, - "y": 1.7528226066048298, - "heading": -0.4327534805983047, - "angularVelocity": 2.2386082909228912e-7, - "velocityX": 3.4190102117948658, - "velocityY": -1.616649554602208, - "timestamp": 12.922226140569286 + "x": 3.873859634378807, + "y": 6.766977536857145, + "heading": 0.17795757602156642, + "angularVelocity": -0.8505841781386996, + "velocityX": 1.875045819887845, + "velocityY": -0.18178402170643457, + "timestamp": 6.772438112436741 }, { - "x": 5.295816845235169, - "y": 1.6558451886813434, - "heading": -0.4327534671695918, - "angularVelocity": 2.2386083672590555e-7, - "velocityX": 3.4190129167581613, - "velocityY": -1.6166438339330798, - "timestamp": 12.982213019489643 + "x": 4.006911762109537, + "y": 6.754076769706622, + "heading": 0.11920696294593584, + "angularVelocity": -0.9465019535937051, + "velocityX": 2.1435367604550115, + "velocityY": -0.2078378534549514, + "timestamp": 6.834509419159976 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.4327534537085892, - "angularVelocity": 2.2439911588435075e-7, - "velocityX": 3.4538678416562716, - "velocityY": -1.5407736813730113, - "timestamp": 13.04219989841 + "x": 4.156645657189765, + "y": 6.73955582918857, + "heading": 0.055309270826440785, + "angularVelocity": -1.029423988195197, + "velocityX": 2.4122884305927053, + "velocityY": -0.23393966205310818, + "timestamp": 6.896580725883211 }, { - "x": 5.767637732978587, - "y": 1.4715486447521433, - "heading": -0.43275344189521775, - "angularVelocity": 1.5949042736003556e-7, - "velocityX": 3.5727828839624562, - "velocityY": -1.2403266728714895, - "timestamp": 13.116269368711977 + "x": 4.323080375842652, + "y": 6.723410435666096, + "heading": -0.012586184407094544, + "angularVelocity": -1.0938299645646508, + "velocityX": 2.681347106079046, + "velocityY": -0.2601104177560936, + "timestamp": 6.958652032606445 }, { - "x": 6.033774192023134, - "y": 1.384125705645109, - "heading": -0.43275343009903516, - "angularVelocity": 1.5925836231633141e-7, - "velocityX": 3.5930655094403474, - "velocityY": -1.1802830336252825, - "timestamp": 13.190338839013954 + "x": 4.506235840968647, + "y": 6.705634179985004, + "heading": -0.08270962096165607, + "angularVelocity": -1.1297238652836274, + "velocityX": 2.9507267495214404, + "velocityY": -0.2863844281597597, + "timestamp": 7.02072333932968 }, { - "x": 6.2999107025526015, - "y": 1.2967029232707166, - "heading": -0.43275341830285263, - "angularVelocity": 1.5925836222768275e-7, - "velocityX": 3.593066204529984, - "velocityY": -1.1802809176031077, - "timestamp": 13.26440830931593 + "x": 4.706118949857515, + "y": 6.686217722826257, + "heading": -0.15194437984101536, + "angularVelocity": -1.1154068205470853, + "velocityX": 3.2202175117742615, + "velocityY": -0.3128088996947802, + "timestamp": 7.082794646052915 }, { - "x": 6.566047160708243, - "y": 1.2092799814598572, - "heading": -0.4327534065048956, - "angularVelocity": 1.5928231960994732e-7, - "velocityX": 3.5930654974393894, - "velocityY": -1.1802830701291929, - "timestamp": 13.338477779617907 + "x": 4.922607059053276, + "y": 6.66515425622058, + "heading": -0.21301282950448638, + "angularVelocity": -0.9838434678965439, + "velocityX": 3.4877324262085807, + "velocityY": -0.3393430510421988, + "timestamp": 7.14486595277615 }, { - "x": 6.815604168443372, - "y": 1.1270295237235333, - "heading": -0.36800659105834677, - "angularVelocity": 0.874136336908857, - "velocityX": 3.369229005117782, - "velocityY": -1.1104501949452927, - "timestamp": 13.412547249919884 + "x": 5.152321807662402, + "y": 6.642265811014449, + "heading": -0.22692060137612477, + "angularVelocity": -0.22406120647088962, + "velocityX": 3.7008202458728654, + "velocityY": -0.3687443750482909, + "timestamp": 7.206937259499385 }, { - "x": 7.0424486777806585, - "y": 1.0523256982782683, - "heading": -0.30458898646479193, - "angularVelocity": 0.8561908750664163, - "velocityX": 3.0625912189253857, - "velocityY": -1.0085643267152118, - "timestamp": 13.48661672022186 + "x": 5.385997622338085, + "y": 6.619824669792801, + "heading": -0.22692062699124488, + "angularVelocity": -4.12672480414244e-7, + "velocityX": 3.7646350143327725, + "velocityY": -0.36153808267225507, + "timestamp": 7.269008566222619 }, { - "x": 7.2465905745815595, - "y": 0.9851213052520007, - "heading": -0.2456798424413219, - "angularVelocity": 0.7953228743678266, - "velocityX": 2.756086900157761, - "velocityY": -0.9073156963628867, - "timestamp": 13.560686190523837 + "x": 5.6195399174914655, + "y": 6.5960341766076915, + "heading": -0.2269206525768778, + "angularVelocity": -4.121974272362167e-7, + "velocityX": 3.7624839476103156, + "velocityY": -0.3832768221124699, + "timestamp": 7.331079872945854 }, { - "x": 7.428038548794458, - "y": 0.9253999088853845, - "heading": -0.1923064776337199, - "angularVelocity": 0.7205852099387569, - "velocityX": 2.449699902984963, - "velocityY": -0.8062889625528059, - "timestamp": 13.634755660825814 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.22692067835854435, + "angularVelocity": -4.1535562716932914e-7, + "velocityX": 3.729650183615213, + "velocityY": -0.6268142199732243, + "timestamp": 7.393151179669089 }, { - "x": 7.586797949690042, - "y": 0.8731533407207944, - "heading": -0.14497999296920738, - "angularVelocity": 0.6389472541327177, - "velocityX": 2.1433851254549703, - "velocityY": -0.7053725097747339, - "timestamp": 13.70882513112779 + "x": 5.959383441341322, + "y": 6.535142573313356, + "heading": -0.22692070161190955, + "angularVelocity": -7.95525127627382e-7, + "velocityX": 3.706414340926548, + "velocityY": -0.7521132022680976, + "timestamp": 7.422381387996315 }, { - "x": 7.722872327084394, - "y": 0.8283767866857153, - "heading": -0.10400779573923787, - "angularVelocity": 0.5531590419497899, - "velocityX": 1.837118273420693, - "velocityY": -0.6045210510150557, - "timestamp": 13.782894601429767 + "x": 6.066940887967902, + "y": 6.509606194124557, + "heading": -0.22692072465630755, + "angularVelocity": -7.883761123678933e-7, + "velocityX": 3.6796674667011553, + "velocityY": -0.8736297361581465, + "timestamp": 7.4516115963235405 }, { - "x": 7.836264194581955, - "y": 0.7910670998170554, - "heading": -0.06959570799723021, - "angularVelocity": 0.46459205934255987, - "velocityX": 1.5308853571555237, - "velocityY": -0.5037120788977025, - "timestamp": 13.856964071731744 + "x": 6.17368571603366, + "y": 6.480861582677863, + "heading": -0.2269207475995078, + "angularVelocity": -7.849140177187675e-7, + "velocityX": 3.651866824580282, + "velocityY": -0.9833871563580519, + "timestamp": 7.480841804650766 }, { - "x": 7.926975414267803, - "y": 0.7612220655404804, - "heading": -0.04189104317473832, - "angularVelocity": 0.3740362218001813, - "velocityX": 1.2246775806013535, - "velocityY": -0.4029330053920843, - "timestamp": 13.93103354203372 + "x": 6.2799139038286755, + "y": 6.450262838777624, + "heading": -0.2269207705194487, + "angularVelocity": -7.841182881547448e-7, + "velocityX": 3.63419194984251, + "velocityY": -1.0468192206387272, + "timestamp": 7.510072012977992 }, { - "x": 7.99500740651227, - "y": 0.7388400306235591, - "heading": -0.021003806792755153, - "angularVelocity": 0.28199521741990785, - "velocityX": 0.9184889802384881, - "velocityY": -0.30217625191150127, - "timestamp": 14.005103012335697 + "x": 6.38600768248991, + "y": 6.419201268635774, + "heading": -0.22692079344039778, + "angularVelocity": -7.841527793264923e-7, + "velocityX": 3.629593654398156, + "velocityY": -1.0626530537902528, + "timestamp": 7.539302221305218 }, { - "x": 8.040361272813582, - "y": 0.7239196956147146, - "heading": -0.007018297206773477, - "angularVelocity": 0.18881611450660732, - "velocityX": 0.6123152510259342, - "velocityY": -0.20143704211755917, - "timestamp": 14.079172482637674 + "x": 6.4915378820851375, + "y": 6.386275784732387, + "heading": -0.22692082347226308, + "angularVelocity": -0.0000010274256324387564, + "velocityX": 3.6103129479556895, + "velocityY": -1.1264197481865303, + "timestamp": 7.568532429632444 }, { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": 2.0779165225884802e-41, - "angularVelocity": 0.09475290127174345, - "velocityX": 0.3061531209608937, - "velocityY": -0.10071229126619018, - "timestamp": 14.15324195293965 + "x": 6.594254502115419, + "y": 6.3529873750963075, + "heading": -0.23642872835410836, + "angularVelocity": -0.32527667183951403, + "velocityX": 3.514057063171563, + "velocityY": -1.1388358667671954, + "timestamp": 7.597762637959669 }, { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": 1.169210048317657e-41, - "angularVelocity": -1.2268302369298258e-40, - "velocityX": -9.944733020002764e-42, - "velocityY": 5.460591898189625e-42, - "timestamp": 14.227311423241627 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 6.69366972769076, + "y": 6.320452523543764, + "heading": -0.25267195567048195, + "angularVelocity": -0.5557000187824542, + "velocityX": 3.4011124540202067, + "velocityY": -1.1130557534298289, + "timestamp": 7.626992846286895 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 6.789488054498416, + "y": 6.288864279277486, + "heading": -0.26990654049679197, + "angularVelocity": -0.5896155317621415, + "velocityX": 3.278058292810801, + "velocityY": -1.0806711985311153, + "timestamp": 7.656223054614121 }, { - "scope": [ - 6 - ], - "type": "StopPoint" + "x": 6.881710669103943, + "y": 6.258257632428043, + "heading": -0.28694313831911705, + "angularVelocity": -0.5828421621787924, + "velocityX": 3.1550447254125045, + "velocityY": -1.047089589879878, + "timestamp": 7.685453262941347 }, { - "scope": [ - 1 - ], - "type": "StopPoint" + "x": 6.970345859458022, + "y": 6.228646491338886, + "heading": -0.3032367291527766, + "angularVelocity": -0.5574230142767849, + "velocityX": 3.0323146986098455, + "velocityY": -1.0130321603485573, + "timestamp": 7.714683471268573 }, { - "scope": [ - 11 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "AmpLanePADESprint": { - "waypoints": [ - { - "x": 0.43297290802001953, - "y": 6.9807281494140625, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 7.055400617247705, + "y": 6.200038168598195, + "heading": -0.3184704659597927, + "angularVelocity": -0.5211641544429054, + "velocityX": 2.909823865690951, + "velocityY": -0.978724558526454, + "timestamp": 7.743913679595798 }, { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 7 + "x": 7.136880378911089, + "y": 6.172437128670844, + "heading": -0.33243641899697507, + "angularVelocity": -0.477791772157844, + "velocityX": 2.7875190197494866, + "velocityY": -0.9442642220804569, + "timestamp": 7.773143887923024 }, { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 24 + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.42938659336036833, + "velocityX": 2.665359438451487, + "velocityY": -0.9097014120066562, + "timestamp": 7.80237409625025 }, { - "x": 7.125290870666504, - "y": 7.458, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "x": 7.370577859130934, + "y": 6.0916577592619054, + "heading": -0.3653758531922844, + "angularVelocity": -0.3132570133567013, + "velocityX": 2.393610638018096, + "velocityY": -0.8325804140238011, + "timestamp": 7.867459230019526 }, { - "x": 8.017494201660156, - "y": 7.457698345184326, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 23 + "x": 7.5087533836626745, + "y": 6.042843250581257, + "heading": -0.37947223160146565, + "angularVelocity": -0.21658368958955537, + "velocityX": 2.1229967049244562, + "velocityY": -0.7500101152698444, + "timestamp": 7.932544363788803 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 7.629367562165718, + "y": 5.999619792415774, + "heading": -0.38804895009153, + "angularVelocity": -0.1317769203712161, + "velocityX": 1.85317554897581, + "velocityY": -0.6641064658284028, + "timestamp": 7.997629497558079 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 14 + "x": 7.7324579600950285, + "y": 5.962133690972344, + "heading": -0.39162657356882036, + "angularVelocity": -0.05496836635495089, + "velocityX": 1.5839315671495955, + "velocityY": -0.5759548958801737, + "timestamp": 8.062714631327355 }, { - "x": 7.214789390563965, - "y": 6.145846366882324, - "heading": -0.344987478573796, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "x": 7.81805300550911, + "y": 5.930490256491027, + "heading": -0.39057997729782273, + "angularVelocity": 0.01608041975771331, + "velocityX": 1.3151243679933469, + "velocityY": -0.4861852876186942, + "timestamp": 8.127799765096631 }, { - "x": 7.985864639282227, - "y": 5.863699913024902, - "heading": -0.344987478573796, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 7.886174910867723, + "y": 5.904768911877698, + "heading": -0.38519213768662347, + "angularVelocity": 0.08278141718657105, + "velocityX": 1.0466584519915365, + "velocityY": -0.39519538677620675, + "timestamp": 8.192884898865907 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 17 + "x": 7.936841490598973, + "y": 5.885031694896893, + "heading": -0.3756842946663964, + "angularVelocity": 0.1460831755210139, + "velocityX": 0.7784662456231665, + "velocityY": -0.3032523072130836, + "timestamp": 8.257970032635184 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 20 + "x": 7.970067341476633, + "y": 5.8713284070268905, + "heading": -0.36223415085714655, + "angularVelocity": 0.206654623418748, + "velocityX": 0.510498311264806, + "velocityY": -0.21054405324847428, + "timestamp": 8.32305516640446 }, { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ + "x": 7.985864639282227, + "y": 5.863699913024902, + "heading": -0.344987478573796, + "angularVelocity": 0.2649863537883773, + "velocityX": 0.24271745160106833, + "velocityY": -0.1172079330593501, + "timestamp": 8.388140300173736 + }, { - "x": 0.43297290802001953, - "y": 6.9807281494140625, - "heading": 0, - "angularVelocity": 2.735053895521923e-38, - "velocityX": -3.8673469053295564e-39, - "velocityY": 1.858643532249755e-38, - "timestamp": 0 + "x": 7.985045656249217, + "y": 5.861990715165632, + "heading": -0.3251381624172612, + "angularVelocity": 0.31897227576671205, + "velocityX": -0.01316080008970204, + "velocityY": -0.02746627272232015, + "timestamp": 8.450369270101682 }, { - "x": 0.46193105617219093, - "y": 6.976128411049919, - "heading": 0.007924686694784022, - "angularVelocity": 0.0965568963773332, - "velocityX": 0.35283526252830383, - "velocityY": -0.05604467125266167, - "timestamp": 0.08207271559159549 + "x": 7.968302412952749, + "y": 5.865866788787815, + "heading": -0.3020436028872691, + "angularVelocity": 0.37112231741475454, + "velocityX": -0.26905866055398503, + "velocityY": 0.062287285594966935, + "timestamp": 8.512598240029627 }, { - "x": 0.5198473628641517, - "y": 6.966928755942856, - "heading": 0.023773564700110772, - "angularVelocity": 0.19310775683593598, - "velocityX": 0.7056706516226394, - "velocityY": -0.11209151592888836, - "timestamp": 0.16414543118319097 + "x": 7.935633552055529, + "y": 5.875328955639361, + "heading": -0.2758355383990043, + "angularVelocity": 0.4211553641111372, + "velocityX": -0.5249783330022537, + "velocityY": 0.1520540491430483, + "timestamp": 8.574827209957572 }, { - "x": 0.6067218129336875, - "y": 6.9531289220308485, - "heading": 0.04754806846022335, - "angularVelocity": 0.2896760950181983, - "velocityX": 1.0585058559757474, - "velocityY": -0.16814155365198188, - "timestamp": 0.24621814677478646 + "x": 7.887037558008595, + "y": 5.8903781286161365, + "heading": -0.24666763452179902, + "angularVelocity": 0.468719053376244, + "velocityX": -0.7809223598462517, + "velocityY": 0.24183548264097016, + "timestamp": 8.637056179885517 }, { - "x": 0.7225543617111448, - "y": 6.934728565336742, - "heading": 0.07925217561750111, - "angularVelocity": 0.38629289805690764, - "velocityX": 1.4113405160596262, - "velocityY": -0.22419578250180797, - "timestamp": 0.32829086236638194 + "x": 7.822512734192608, + "y": 5.911015324029987, + "heading": -0.21472164060862464, + "angularVelocity": 0.5133620876283883, + "velocityX": -1.0368936508301385, + "velocityY": 0.33163324795100674, + "timestamp": 8.699285149813463 }, { - "x": 0.8673449286095066, - "y": 6.9117272629835975, - "heading": 0.11889328943914418, - "angularVelocity": 0.48299990485147826, - "velocityX": 1.764174194245729, - "velocityY": -0.2802551638184133, - "timestamp": 0.4103635779579774 + "x": 7.742057181096897, + "y": 5.937241677003945, + "heading": -0.1802161756247138, + "angularVelocity": 0.5544919837796514, + "velocityX": -1.29289546635384, + "velocityY": 0.4214492543316279, + "timestamp": 8.761514119741408 }, { - "x": 1.0410933872613453, - "y": 6.884124518124938, - "heading": 0.16648343863275697, - "angularVelocity": 0.5798534732349732, - "velocityX": 2.117006332731008, - "velocityY": -0.33632059887008814, - "timestamp": 0.49243629354957286 + "x": 7.645668783226514, + "y": 5.9690584605532635, + "heading": -0.14341967249459378, + "angularVelocity": 0.5913082471512265, + "velocityX": -1.5489312772168387, + "velocityY": 0.5112857176674774, + "timestamp": 8.823743089669353 }, { - "x": 1.243799549589456, - "y": 6.851919767180746, - "heading": 0.22204089753091144, - "angularVelocity": 0.6769297018830912, - "velocityX": 2.4698361796239694, - "velocityY": -0.39239290075946437, - "timestamp": 0.5745090091411683 + "x": 7.533345224831275, + "y": 6.006467108033043, + "heading": -0.10467019490543185, + "angularVelocity": 0.6226919332592115, + "velocityX": -1.8050043014579413, + "velocityY": 0.6011452145695896, + "timestamp": 8.885972059597298 }, { - "x": 1.4754631289031934, - "y": 6.815112383244328, - "heading": 0.28559273385120976, - "angularVelocity": 0.7743357321882771, - "velocityX": 2.8226625333871667, - "velocityY": -0.44847284107882956, - "timestamp": 0.6565817247327638 + "x": 7.405084087505747, + "y": 6.04946923502926, + "heading": -0.06440725119965417, + "angularVelocity": 0.6470128583583754, + "velocityX": -2.0611161887146876, + "velocityY": 0.6910306734308482, + "timestamp": 8.948201029525244 }, { - "x": 1.6781593090979035, - "y": 6.782884452250807, - "heading": 0.3422497740441209, - "angularVelocity": 0.6903273491624664, - "velocityX": 2.4697145541443093, - "velocityY": -0.3926753338331338, - "timestamp": 0.7386544403243592 + "x": 7.260883177292953, + "y": 6.098066643466027, + "heading": -0.023226072117979224, + "angularVelocity": 0.6617686124221249, + "velocityX": -2.3172633321708815, + "velocityY": 0.7809450886466088, + "timestamp": 9.010429999453189 }, { - "x": 1.8518984587746443, - "y": 6.755259929387126, - "heading": 0.39086385083963315, - "angularVelocity": 0.5923293319234401, - "velocityX": 2.1168929092256374, - "velocityY": -0.33658594899118865, - "timestamp": 0.8207271559159547 + "x": 7.100741532540205, + "y": 6.152261239581916, + "heading": 0.018021968863737303, + "angularVelocity": 0.6628430621538008, + "velocityX": -2.5734259290837285, + "velocityY": 0.8708901365174432, + "timestamp": 9.072658969381134 }, { - "x": 1.9966808291576696, - "y": 6.732239139219163, - "heading": 0.43140805286474204, - "angularVelocity": 0.4940034179795163, - "velocityX": 1.7640743253029585, - "velocityY": -0.28049260953564537, - "timestamp": 0.9027998715075501 + "x": 6.924662708731195, + "y": 6.212054571383467, + "heading": 0.05802124542072086, + "angularVelocity": 0.6427758100977419, + "velocityX": -2.829531390490462, + "velocityY": 0.9608600603671181, + "timestamp": 9.13488793930908 }, { - "x": 2.1125066103123933, - "y": 6.7138223061958735, - "heading": 0.463861007608982, - "angularVelocity": 0.39541709458878044, - "velocityX": 1.4112580571975715, - "velocityY": -0.22439653532283038, - "timestamp": 0.9848725870991456 + "x": 6.732667433541327, + "y": 6.277445378277029, + "heading": 0.09448040335619008, + "angularVelocity": 0.5858872158366861, + "velocityX": -3.0853037646642187, + "velocityY": 1.050809726872821, + "timestamp": 9.197116909237025 }, { - "x": 2.199375929341993, - "y": 6.700009583150749, - "heading": 0.4882072246931022, - "angularVelocity": 0.29664203150374724, - "velocityX": 1.0584433377576143, - "velocityY": -0.16829859893826818, - "timestamp": 1.0669453026907412 + "x": 6.524862060286313, + "y": 6.348410321942265, + "heading": 0.12245319566137218, + "angularVelocity": 0.44951398581032764, + "velocityX": -3.33936707446111, + "velocityY": 1.1403843539014786, + "timestamp": 9.25934587916497 }, { - "x": 2.257288845375976, - "y": 6.690801066111525, - "heading": 0.5044377142261048, - "angularVelocity": 0.19775743273523133, - "velocityX": 0.7056293387703308, - "velocityY": -0.11219949252132488, - "timestamp": 1.1490180182823366 + "x": 6.30228026524581, + "y": 6.424202518084616, + "heading": 0.1231057628389814, + "angularVelocity": 0.010486549566300301, + "velocityX": -3.5768195311320152, + "velocityY": 1.2179567849204198, + "timestamp": 9.321574849092915 }, { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": 0.0988477756026137, - "velocityX": 0.3528151893675758, - "velocityY": -0.0560997894575031, - "timestamp": 1.231090733873932 + "x": 6.078984969036473, + "y": 6.498549743391548, + "heading": 0.1231057748040032, + "angularVelocity": 1.9227414178241924e-7, + "velocityX": -3.5882852707972814, + "velocityY": 1.194736557475109, + "timestamp": 9.38380381902086 }, { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": 0, - "velocityX": 3.087311543739499e-36, - "velocityY": 0, - "timestamp": 1.3131634494655275 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0.12310578689683861, + "angularVelocity": 1.9432806685756468e-7, + "velocityX": -3.662936913327793, + "velocityY": 0.941318096340742, + "timestamp": 9.446032788948806 }, { - "x": 2.3177158256290964, - "y": 6.704097470773247, - "heading": 0.5125504196, - "angularVelocity": -9.1973743139294e-17, - "velocityX": 0.34546318060283326, - "velocityY": 0.1965022887733569, - "timestamp": 1.4042599289943862 + "x": 5.606427257807288, + "y": 6.600564902613298, + "heading": 0.1231057972666647, + "angularVelocity": 1.578555424299444e-7, + "velocityX": -3.7237014740499577, + "velocityY": 0.6612371129043467, + "timestamp": 9.511724660841988 }, { - "x": 2.380656782769881, - "y": 6.7398988031009575, - "heading": 0.5125504196, - "angularVelocity": -1.3336684994607708e-16, - "velocityX": 0.6909263394843395, - "velocityY": 0.3930045651914449, - "timestamp": 1.495356408523245 + "x": 5.35931577124569, + "y": 6.62625930787543, + "heading": 0.12310580746861746, + "angularVelocity": 1.5530007712924032e-7, + "velocityX": -3.7616752185629396, + "velocityY": 0.39113522756534835, + "timestamp": 9.577416532735171 }, { - "x": 2.475068211555481, - "y": 6.793600797653198, - "heading": 0.5125504196, - "angularVelocity": 6.653839443641591e-19, - "velocityX": 1.0363894332018742, - "velocityY": 0.5895068045437317, - "timestamp": 1.5864528880521036 + "x": 5.111994168115865, + "y": 6.649845615003531, + "heading": 0.1231058176602883, + "angularVelocity": 1.551435596748186e-7, + "velocityX": -3.764873735550995, + "velocityY": 0.35904452785959023, + "timestamp": 9.643108404628354 }, { - "x": 2.569479640341081, - "y": 6.847302792205439, - "heading": 0.5125504196, - "angularVelocity": 5.174999729369457e-17, - "velocityX": 1.0363894332018742, - "velocityY": 0.5895068045437317, - "timestamp": 1.6775493675809623 + "x": 4.868859374012009, + "y": 6.67294055859981, + "heading": 0.13716866748145323, + "angularVelocity": 0.21407290454489739, + "velocityX": -3.701139685883833, + "velocityY": 0.35156470550620644, + "timestamp": 9.708800276521536 }, { - "x": 2.6324205974818655, - "y": 6.8831041245331495, - "heading": 0.5125504196, - "angularVelocity": -1.3901806543706914e-17, - "velocityX": 0.6909263394843393, - "velocityY": 0.39300456519144494, - "timestamp": 1.768645847109821 + "x": 4.644141674488456, + "y": 6.6943246178055915, + "heading": 0.16550217221046287, + "angularVelocity": 0.43130913935716303, + "velocityX": -3.4207839272559015, + "velocityY": 0.3255206251475534, + "timestamp": 9.77449214841472 }, { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "angularVelocity": -1.8725484396545262e-17, - "velocityX": 0.3454631806028332, - "velocityY": 0.19650228877335693, - "timestamp": 1.8597423266386797 + "x": 4.438149971299598, + "y": 6.713938607045374, + "heading": 0.1971398735951766, + "angularVelocity": 0.48160754858923427, + "velocityX": -3.1357258859027066, + "velocityY": 0.2985755874284469, + "timestamp": 9.840184020307902 }, { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "angularVelocity": 1.431796971368011e-33, - "velocityX": 3.743360269099248e-35, - "velocityY": -8.057233109044706e-35, - "timestamp": 1.9508388061675384 + "x": 4.2509016025753406, + "y": 6.731775563046276, + "heading": 0.22901708759854217, + "angularVelocity": 0.48525354940713283, + "velocityX": -2.8504039134206605, + "velocityY": 0.27152455070706877, + "timestamp": 9.905875892201085 }, { - "x": 2.6835271230542426, - "y": 6.903699548014665, - "heading": 0.5052880772246282, - "angularVelocity": -0.10752621184517612, - "velocityX": 0.2907312178663514, - "velocityY": 0.039898557617057115, - "timestamp": 2.018379011023157 + "x": 4.082393919350406, + "y": 6.747832566507683, + "heading": 0.25967107343501483, + "angularVelocity": 0.46663285659323517, + "velocityX": -2.565122265033534, + "velocityY": 0.24442907468850525, + "timestamp": 9.971567764094267 }, { - "x": 2.7228033814945674, - "y": 6.909089957964811, - "heading": 0.49095749727137794, - "angularVelocity": -0.21217850884350908, - "velocityX": 0.5815241236576919, - "velocityY": 0.0798103879262595, - "timestamp": 2.0859192158787754 + "x": 3.9326220842038664, + "y": 6.762107971713798, + "heading": 0.28824128245235414, + "angularVelocity": 0.43491239013245725, + "velocityX": -2.279914254690053, + "velocityY": 0.21730854662395976, + "timestamp": 10.03725963598745 }, { - "x": 2.7817246923505206, - "y": 6.91717710303389, - "heading": 0.46979167660622734, - "angularVelocity": -0.31338105518627096, - "velocityX": 0.8723886902906202, - "velocityY": 0.11973823719319023, - "timestamp": 2.153459420734394 - }, - { - "x": 2.860296723174767, - "y": 6.927962260865171, - "heading": 0.4420737054643263, - "angularVelocity": -0.41039216865205086, - "velocityX": 1.1633371706853932, - "velocityY": 0.15968500324119947, - "timestamp": 2.220999625590012 + "x": 3.801581586235111, + "y": 6.77460070477556, + "heading": 0.314159509734349, + "angularVelocity": 0.3945423769342238, + "velocityX": -1.9947749119073828, + "velocityY": 0.19017167119358822, + "timestamp": 10.102951507880633 }, { - "x": 2.9585261643123166, - "y": 6.941446920815512, - "heading": 0.40815310467896726, - "angularVelocity": -0.502228278073356, - "velocityX": 1.454384708301312, - "velocityY": 0.19965382070083557, - "timestamp": 2.2885398304456306 + "x": 3.6892685916749244, + "y": 6.785310000165767, + "heading": 0.3370219943875935, + "angularVelocity": 0.34802607985383205, + "velocityX": -1.7096939289964497, + "velocityY": 0.16302314246152652, + "timestamp": 10.168643379773815 }, { - "x": 3.0764209718394073, - "y": 6.9576328058659405, - "heading": 0.3684706356327604, - "angularVelocity": -0.5875384762459123, - "velocityX": 1.7455500435498434, - "velocityY": 0.23964814861057657, - "timestamp": 2.356080035301249 + "x": 3.595679898494151, + "y": 6.794235280663889, + "heading": 0.35652698961990326, + "angularVelocity": 0.2969164170572818, + "velocityX": -1.4246616892414212, + "velocityY": 0.135865826941808, + "timestamp": 10.234335251666998 }, { - "x": 3.2139906503876703, - "y": 6.9765218958360915, - "heading": 0.323597871318007, - "angularVelocity": -0.6643859670055544, - "velocityX": 2.0368561043358877, - "velocityY": 0.27967178972185397, - "timestamp": 2.4236202401568674 + "x": 3.520812828526041, + "y": 6.801376095406927, + "heading": 0.37244068726433127, + "angularVelocity": 0.2422475899347831, + "velocityX": -1.1396702181031793, + "velocityY": 0.10870164812244498, + "timestamp": 10.30002712356018 }, { - "x": 3.3712465090237984, - "y": 6.998116434011635, - "heading": 0.27430452270447037, - "angularVelocity": -0.7298371202591344, - "velocityX": 2.3283296071176522, - "velocityY": 0.3197286449116718, - "timestamp": 2.491160445012486 + "x": 3.464665127058438, + "y": 6.806732084957018, + "heading": 0.38457706026543664, + "angularVelocity": 0.18474695044220302, + "velocityX": -0.8547130695088268, + "velocityY": 0.08153199773025213, + "timestamp": 10.365718995453364 }, { - "x": 3.5482015820439243, - "y": 7.022418860702876, - "heading": 0.22168358710657873, - "angularVelocity": -0.7791053596947181, - "velocityX": 2.619996095635254, - "velocityY": 0.3598216313260006, - "timestamp": 2.558700649868104 + "x": 3.4272348818352074, + "y": 6.810302960282004, + "heading": 0.39278518447772715, + "angularVelocity": 0.12494885555456782, + "velocityX": -0.5697850303930024, + "velocityY": 0.05435794752192765, + "timestamp": 10.431410867346546 }, { - "x": 3.7448688058303015, - "y": 7.04943148246024, - "heading": 0.16741520227188003, - "angularVelocity": -0.8034974864335817, - "velocityX": 2.911854120176202, - "velocityY": 0.3999487685166161, - "timestamp": 2.6262408547237226 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.0632601721112495, + "velocityX": -0.28488184560874424, + "velocityY": 0.027180367966521624, + "timestamp": 10.497102739239729 }, { - "x": 3.961249724160237, - "y": 7.079155012062387, - "heading": 0.1144423105156959, - "angularVelocity": -0.7843164211512963, - "velocityX": 3.2037350018777047, - "velocityY": 0.4400864591051642, - "timestamp": 2.693781059579341 + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0, + "velocityX": 1.9034366224257262e-34, + "velocityY": -2.547324229323533e-33, + "timestamp": 10.562794611132912 }, { - "x": 4.197248092224393, - "y": 7.111577952654102, - "heading": 0.06945280731873603, - "angularVelocity": -0.6661144024234814, - "velocityX": 3.494190883321295, - "velocityY": 0.4800539273019009, - "timestamp": 2.7613212644349594 + "x": 3.4428034429775143, + "y": 6.811427919070607, + "heading": 0.39249813327278055, + "angularVelocity": -0.050100574262489476, + "velocityX": 0.38660845958378076, + "velocityY": -0.007449238878501296, + "timestamp": 10.651470846737375 }, { - "x": 4.450279816565845, - "y": 7.14615999986413, - "heading": 0.06926458523360347, - "angularVelocity": -0.0027868154314146247, - "velocityX": 3.746386687490236, - "velocityY": 0.5120216511625231, - "timestamp": 2.828861469290578 + "x": 3.5113694006298597, + "y": 6.810106800279428, + "heading": 0.3836131655700934, + "angularVelocity": -0.10019558951868686, + "velocityX": 0.7732168284429818, + "velocityY": -0.014898228168725063, + "timestamp": 10.740147082341839 }, { - "x": 4.703335353284152, - "y": 7.180937109425349, - "heading": 0.06926456657564138, - "angularVelocity": -2.762497114544702e-7, - "velocityX": 3.746739253445653, - "velocityY": 0.5149097435455242, - "timestamp": 2.896401674146196 + "x": 3.614218332551768, + "y": 6.808125167746075, + "heading": 0.37028807906085554, + "angularVelocity": -0.1502667137188131, + "velocityX": 1.1598251912796755, + "velocityY": -0.022346827420505266, + "timestamp": 10.828823317946302 }, { - "x": 4.956390888818978, - "y": 7.215714227598158, - "heading": 0.06926454791768001, - "angularVelocity": -2.7624970048363726e-7, - "velocityX": 3.746739235923052, - "velocityY": 0.5149098710487069, - "timestamp": 2.9639418790018146 + "x": 3.7513502451988723, + "y": 6.8054830693306245, + "heading": 0.3525264976102127, + "angularVelocity": -0.20029697166969943, + "velocityX": 1.5464336269164098, + "velocityY": -0.02979488695523812, + "timestamp": 10.917499553550766 }, { - "x": 5.209446424353711, - "y": 7.250491345771652, - "heading": 0.0692645292597187, - "angularVelocity": -2.7624969990656975e-7, - "velocityX": 3.7467392359216607, - "velocityY": 0.5149098710588286, - "timestamp": 3.031482083857433 + "x": 3.9227651512282486, + "y": 6.802180567172425, + "heading": 0.3303333796886753, + "angularVelocity": -0.2502713130553812, + "velocityX": 1.9330422052867193, + "velocityY": -0.03724224574585722, + "timestamp": 11.006175789155229 }, { - "x": 5.462501959888314, - "y": 7.285268463946087, - "heading": 0.0692645106017574, - "angularVelocity": -2.7624969939846597e-7, - "velocityX": 3.7467392359197462, - "velocityY": 0.5149098710727624, - "timestamp": 3.0990222887130514 + "x": 4.128463068361194, + "y": 6.79821773882544, + "heading": 0.3037147939289921, + "angularVelocity": -0.3001772186001935, + "velocityX": 2.319650983499712, + "velocityY": -0.04468872996211646, + "timestamp": 11.094852024759692 }, { - "x": 5.715557495424461, - "y": 7.320045582109288, - "heading": 0.06926449194379608, - "angularVelocity": -2.762496999067462e-7, - "velocityX": 3.746739235942603, - "velocityY": 0.5149098709064452, - "timestamp": 3.16656249356867 + "x": 4.368444017678072, + "y": 6.793594678448866, + "heading": 0.2726776385751228, + "angularVelocity": -0.3500053327963678, + "velocityX": 2.706259999435507, + "velocityY": -0.05213415234714548, + "timestamp": 11.183528260364156 }, { - "x": 5.968613025759916, - "y": 7.354822738115272, - "heading": 0.06926447328583468, - "angularVelocity": -2.762497009102298e-7, - "velocityX": 3.7467391589411925, - "velocityY": 0.5149104312065347, - "timestamp": 3.234102698424288 + "x": 4.642708020512335, + "y": 6.788311497970363, + "heading": 0.23722930806330966, + "angularVelocity": -0.3997500600942169, + "velocityX": 3.092869255948188, + "velocityY": -0.05957831252634942, + "timestamp": 11.27220449596862 }, { - "x": 6.221690653746151, - "y": 7.389438720345797, - "heading": 0.06926445462738323, - "angularVelocity": -2.7625695670270355e-7, - "velocityX": 3.74706633666926, - "velocityY": 0.5125240929387752, - "timestamp": 3.3016429032799066 + "x": 4.951255088173746, + "y": 6.782368328182758, + "heading": 0.19737735072702745, + "angularVelocity": -0.44940966499796203, + "velocityX": 3.479478640000833, + "velocityY": -0.0670209977576627, + "timestamp": 11.360880731573083 }, { - "x": 6.476102895472122, - "y": 7.412263085598601, - "heading": 0.06926437909315673, - "angularVelocity": -0.0000011183594520291744, - "velocityX": 3.766826622303432, - "velocityY": 0.33793745964490024, - "timestamp": 3.369183108135525 + "x": 5.286562692788269, + "y": 6.775920770205836, + "heading": 0.19737734601955828, + "angularVelocity": -5.3086028753514235e-8, + "velocityX": 3.781256639153567, + "velocityY": -0.07270897251075666, + "timestamp": 11.449556967177546 }, { - "x": 6.712136074986816, - "y": 7.430641724558031, - "heading": 0.04341074490415746, - "angularVelocity": -0.38278880326565307, - "velocityX": 3.4947063015172035, - "velocityY": 0.27211405412107026, - "timestamp": 3.4367233129911434 + "x": 5.595111114675473, + "y": 6.769985794431735, + "heading": 0.15789301075905496, + "angularVelocity": -0.4452639987631135, + "velocityX": 3.4794939115759345, + "velocityY": -0.06692859404378032, + "timestamp": 11.53823320278201 + }, + { + "x": 5.8693764275565705, + "y": 6.76471055259859, + "heading": 0.12280143262174693, + "angularVelocity": -0.3957269712466434, + "velocityX": 3.092884029318143, + "velocityY": -0.059488788593539996, + "timestamp": 11.626909438386473 + }, + { + "x": 6.109358599544783, + "y": 6.760094936608861, + "heading": 0.09209943623091729, + "angularVelocity": -0.3462257523850543, + "velocityX": 2.7062737874738274, + "velocityY": -0.05205020215694004, + "timestamp": 11.715585673990937 + }, + { + "x": 6.315057615509001, + "y": 6.756138852616793, + "heading": 0.06578495581515914, + "angularVelocity": -0.29674782918315123, + "velocityX": 2.3196633749963063, + "velocityY": -0.04461267401690494, + "timestamp": 11.8042619095954 + }, + { + "x": 6.4864734670806214, + "y": 6.752842221795132, + "heading": 0.043856783005957245, + "angularVelocity": -0.24728353272698234, + "velocityX": 1.9330528681462538, + "velocityY": -0.037176034810112814, + "timestamp": 11.892938145199864 + }, + { + "x": 6.623606149810205, + "y": 6.750204980767773, + "heading": 0.026314311963007715, + "angularVelocity": -0.19782606831887856, + "velocityX": 1.5464423111199512, + "velocityY": -0.029740110294281013, + "timestamp": 11.981614380804327 + }, + { + "x": 6.726455661694478, + "y": 6.74822708185799, + "heading": 0.013157327563715553, + "angularVelocity": -0.14837102984364775, + "velocityX": 1.1598317315027789, + "velocityY": -0.022304723427886322, + "timestamp": 12.07029061640879 + }, + { + "x": 6.795022002255089, + "y": 6.7469084932218335, + "heading": 0.004385844550104781, + "angularVelocity": -0.09891582512292933, + "velocityX": 0.7732211464912397, + "velocityY": -0.014869695664985829, + "timestamp": 12.158966852013254 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -4.086871260037018e-38, + "angularVelocity": -0.04945907457853377, + "velocityX": 0.3866105668308079, + "velocityY": -0.007434847721776456, + "timestamp": 12.247643087617718 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -5.480350490542979e-38, + "angularVelocity": -1.571392014077605e-37, + "velocityX": 4.425248303324229e-37, + "velocityY": 1.8999126670467387e-37, + "timestamp": 12.336319323222181 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 10 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "CenterLanePBDA": { + "waypoints": [ + { + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 4.897946357727051, + "y": 6.407341003417969, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 20 + }, + { + "x": 8.008931159973145, + "y": 7.328619956970215, + "heading": 0.4756948301053548, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 1.8804243803024292, + "y": 6.046840667724609, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": 2.2921355473422064e-38, + "velocityX": 1.1360383232068741e-38, + "velocityY": -2.144062321628808e-38, + "timestamp": 0 + }, + { + "x": 1.32825806778149, + "y": 5.589895062873393, + "heading": -2.8062774074031584e-19, + "angularVelocity": -2.995198561755265e-18, + "velocityX": 0.40860904707084805, + "velocityY": -0.011305499292899219, + "timestamp": 0.09369253321938388 + }, + { + "x": 1.4048253002214361, + "y": 5.587776581164742, + "heading": -1.768868342127426e-19, + "angularVelocity": 1.1072483896415275e-18, + "velocityX": 0.8172180835441991, + "velocityY": -0.022610998292584186, + "timestamp": 0.18738506643876776 + }, + { + "x": 1.519676146824621, + "y": 5.584598858658671, + "heading": -2.386873521248768e-19, + "angularVelocity": -6.596098514057519e-19, + "velocityX": 1.225827103364341, + "velocityY": -0.03391649683150392, + "timestamp": 0.28107759965815166 + }, + { + "x": 1.6728106047825377, + "y": 5.580361895432887, + "heading": -7.795008789281329e-19, + "angularVelocity": -5.772215866345043e-18, + "velocityX": 1.6344360932087083, + "velocityY": -0.04522199454104627, + "timestamp": 0.3747701328775355 + }, + { + "x": 1.8642286675420066, + "y": 5.575065691668706, + "heading": -1.0638718286937245e-18, + "angularVelocity": -3.0351506143991226e-18, + "velocityX": 2.0430450131096105, + "velocityY": -0.056527490315375004, + "timestamp": 0.4684626660969194 + }, + { + "x": 2.0939303023371445, + "y": 5.5687102482727004, + "heading": -1.0113200205360793e-18, + "angularVelocity": 5.608964381023318e-19, + "velocityX": 2.451653583293393, + "velocityY": -0.0678329764136413, + "timestamp": 0.5621551993163033 + }, + { + "x": 2.285348365096613, + "y": 5.563414044508519, + "heading": -7.732946400687193e-19, + "angularVelocity": 2.5404946615264457e-18, + "velocityX": 2.04304501310961, + "velocityY": -0.056527490315375004, + "timestamp": 0.6558477325356872 + }, + { + "x": 2.43848282305453, + "y": 5.559177081282735, + "heading": -4.312009437616493e-19, + "angularVelocity": 3.651237559230762e-18, + "velocityX": 1.634436093208708, + "velocityY": -0.04522199454104628, + "timestamp": 0.749540265755071 + }, + { + "x": 2.553333669657715, + "y": 5.555999358776664, + "heading": -7.015602955490743e-20, + "angularVelocity": 3.853507870950435e-18, + "velocityX": 1.2258271033643409, + "velocityY": -0.033916496831503926, + "timestamp": 0.8432327989744549 + }, + { + "x": 2.6299009020976607, + "y": 5.553880877068013, + "heading": -2.7026882274997263e-20, + "angularVelocity": 4.603264080766157e-19, + "velocityX": 0.817218083544199, + "velocityY": -0.022610998292584186, + "timestamp": 0.9369253321938388 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": 0, + "angularVelocity": 2.884635663770645e-19, + "velocityX": 0.40860904707084805, + "velocityY": -0.011305499292899219, + "timestamp": 1.0306178654132228 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -1.1410946614987667e-37, + "timestamp": 1.1243103986326066 + }, + { + "x": 2.687410064097537, + "y": 5.560258515634833, + "heading": 0.0028176822125938034, + "angularVelocity": 0.04098002734374148, + "velocityX": 0.2796139919160097, + "velocityY": 0.1081610698436357, + "timestamp": 1.1930678494014684 + }, + { + "x": 2.7258611531960137, + "y": 5.575132263104083, + "heading": 0.008453116109374614, + "angularVelocity": 0.08196106507388369, + "velocityX": 0.5592279624754555, + "velocityY": 0.2163219738796251, + "timestamp": 1.2618253001703301 + }, + { + "x": 2.783537789303066, + "y": 5.597442863001631, + "heading": 0.016905769734324238, + "angularVelocity": 0.12293436610040698, + "velocityX": 0.8388419794814749, + "velocityY": 0.3244826509427688, + "timestamp": 1.3305827509391919 + }, + { + "x": 2.8604399802132514, + "y": 5.627190295278175, + "heading": 0.028174540960364546, + "angularVelocity": 0.1638916379247738, + "velocityX": 1.118456109850604, + "velocityY": 0.4326430364113507, + "timestamp": 1.3993402017080536 + }, + { + "x": 2.9565677382168056, + "y": 5.664374535134068, + "heading": 0.042257801458542396, + "angularVelocity": 0.20482522753092394, + "velocityX": 1.3980704189674162, + "velocityY": 0.5408030611968617, + "timestamp": 1.4680976524769154 + }, + { + "x": 3.0719210799387406, + "y": 5.708995552649304, + "heading": 0.05915345386800317, + "angularVelocity": 0.2457283133759373, + "velocityX": 1.6776849698763825, + "velocityY": 0.648962650829455, + "timestamp": 1.5368551032457771 + }, + { + "x": 3.2065000261043313, + "y": 5.761053312357379, + "heading": 0.07885900262196403, + "angularVelocity": 0.2865951040012339, + "velocityX": 1.9572998222112317, + "velocityY": 0.757121724641519, + "timestamp": 1.6056125540146389 + }, + { + "x": 3.360304601198153, + "y": 5.820547772766793, + "heading": 0.1013716386695389, + "angularVelocity": 0.32742104013214957, + "velocityX": 2.2369150306467707, + "velocityY": 0.8652801950062525, + "timestamp": 1.6743700047835006 + }, + { + "x": 3.5333348329448953, + "y": 5.8874788858214675, + "heading": 0.12668833767209217, + "angularVelocity": 0.36820299064984424, + "velocityX": 2.5165306423068015, + "velocityY": 0.973437966449241, + "timestamp": 1.7431274555523624 + }, + { + "x": 3.7255907514118967, + "y": 5.961846596243587, + "heading": 0.15480596913918515, + "angularVelocity": 0.4089394116954055, + "velocityX": 2.796146691262561, + "velocityY": 1.0815949339384094, + "timestamp": 1.8118849063212241 + }, + { + "x": 3.937072386955544, + "y": 6.04365084047756, + "heading": 0.18572140510730675, + "angularVelocity": 0.44963034002016317, + "velocityX": 3.075763181717921, + "velocityY": 1.1897509770827353, + "timestamp": 1.8806423570900859 + }, + { + "x": 4.167779761570079, + "y": 6.132891543122838, + "heading": 0.21943155266112502, + "angularVelocity": 0.4902762853605699, + "velocityX": 3.355379992054849, + "velocityY": 1.2979059236107295, + "timestamp": 1.9493998078589476 + }, + { + "x": 4.410308649457132, + "y": 6.2266961328710915, + "heading": 0.21943155790785696, + "angularVelocity": 7.630783127279499e-8, + "velocityX": 3.5273106430654275, + "velocityY": 1.3642825424635276, + "timestamp": 2.0181572586278094 + }, + { + "x": 4.652837676050608, + "y": 6.320500363997887, + "heading": 0.21943156315423873, + "angularVelocity": 7.630273806741925e-8, + "velocityX": 3.5273126603947684, + "velocityY": 1.3642773267166644, + "timestamp": 2.0869147093966713 + }, + { + "x": 4.897946357727051, + "y": 6.407341003417969, + "heading": 0.21943156841217684, + "angularVelocity": 7.647081243446701e-8, + "velocityX": 3.5648308501199604, + "velocityY": 1.2629996960185401, + "timestamp": 2.1556721601655333 + }, + { + "x": 5.1308682473529785, + "y": 6.471009903235158, + "heading": 0.21943157303192085, + "angularVelocity": 7.23563158909772e-8, + "velocityX": 3.6481177055177683, + "velocityY": 0.9972082962532426, + "timestamp": 2.2195193022886155 + }, + { + "x": 5.365529567427774, + "y": 6.527934417793533, + "heading": 0.21943157763716262, + "angularVelocity": 7.21291764013049e-8, + "velocityX": 3.6753613751798433, + "velocityY": 0.8915749815181846, + "timestamp": 2.2833664444116977 + }, + { + "x": 5.600190991292763, + "y": 6.584858504493345, + "heading": 0.21943158224240422, + "angularVelocity": 7.21291734995501e-8, + "velocityX": 3.6753630007842206, + "velocityY": 0.8915682802227483, + "timestamp": 2.34721358653478 + }, + { + "x": 5.8348524151632635, + "y": 6.641782591170481, + "heading": 0.2194315868476458, + "angularVelocity": 7.212917336838375e-8, + "velocityX": 3.6753630008705414, + "velocityY": 0.8915682798675781, + "timestamp": 2.411060728657862 + }, + { + "x": 6.069513839029734, + "y": 6.698706677864191, + "heading": 0.21943159145288735, + "angularVelocity": 7.212917280605725e-8, + "velocityX": 3.6753630008074327, + "velocityY": 0.891568280127171, + "timestamp": 2.474907870780944 + }, + { + "x": 6.3041751870414915, + "y": 6.75563107725722, + "heading": 0.21943159605812906, + "angularVelocity": 7.212917561549481e-8, + "velocityX": 3.6753618127399528, + "velocityY": 0.8915731777514998, + "timestamp": 2.5387550129040264 + }, + { + "x": 6.537489271202827, + "y": 6.817847413489264, + "heading": 0.21943160514124108, + "angularVelocity": 1.4226340838423697e-7, + "velocityX": 3.6542604164106804, + "velocityY": 0.9744576524998471, + "timestamp": 2.6026021550271086 + }, + { + "x": 6.7545327794045615, + "y": 6.881509207420976, + "heading": 0.24890077922126197, + "angularVelocity": 0.46155823268064305, + "velocityX": 3.399424014677561, + "velocityY": 0.9970970009742466, + "timestamp": 2.6664492971501907 + }, + { + "x": 6.954212307545749, + "y": 6.941327014961267, + "heading": 0.2778274290995393, + "angularVelocity": 0.45306099719431336, + "velocityX": 3.1274622716276532, + "velocityY": 0.9368909171372027, + "timestamp": 2.730296439273273 + }, + { + "x": 7.1365689728963595, + "y": 6.997132012930522, + "heading": 0.305376589845945, + "angularVelocity": 0.43148620017020184, + "velocityX": 2.856144524042602, + "velocityY": 0.8740406557535503, + "timestamp": 2.794143581396355 + }, + { + "x": 7.301617943292834, + "y": 7.048866027505986, + "heading": 0.33125825392103403, + "angularVelocity": 0.40536918669284566, + "velocityX": 2.585064341302851, + "velocityY": 0.8102792522134878, + "timestamp": 2.8579907235194373 + }, + { + "x": 7.449367094221676, + "y": 7.096499619737108, + "heading": 0.35532461486989275, + "angularVelocity": 0.37693716818945566, + "velocityX": 2.3141075076472033, + "velocityY": 0.7460567638139751, + "timestamp": 2.9218378656425195 + }, + { + "x": 7.579821246213137, + "y": 7.140015020198173, + "heading": 0.3774857294479878, + "angularVelocity": 0.347096421878647, + "velocityX": 2.04322617510391, + "velocityY": 0.681555963416076, + "timestamp": 2.9856850077656016 + }, + { + "x": 7.69298365301254, + "y": 7.179400342359022, + "heading": 0.3976808814927073, + "angularVelocity": 0.3163047142468991, + "velocityX": 1.7723958040479335, + "velocityY": 0.6168689913312067, + "timestamp": 3.049532149888684 + }, + { + "x": 7.788856658439436, + "y": 7.214647078189013, + "heading": 0.41586620005922853, + "angularVelocity": 0.2848258819708253, + "velocityX": 1.5016021428504343, + "velocityY": 0.5520487629977263, + "timestamp": 3.113379292011766 + }, + { + "x": 7.867442031134741, + "y": 7.2457488378850705, + "heading": 0.43200844627235874, + "angularVelocity": 0.2528264488645923, + "velocityX": 1.2308361828288252, + "velocityY": 0.48712845496042256, + "timestamp": 3.177226434134848 + }, + { + "x": 7.928741153018892, + "y": 7.27270064652237, + "heading": 0.4460815614823235, + "angularVelocity": 0.22041887454940703, + "velocityX": 0.9600918670092122, + "velocityY": 0.4221302276198955, + "timestamp": 3.2410735762579304 + }, + { + "x": 7.9727551334716695, + "y": 7.29549852060884, + "heading": 0.45806460123681797, + "angularVelocity": 0.1876832596734667, + "velocityX": 0.6893649267484526, + "velocityY": 0.35706960920065395, + "timestamp": 3.3049207183810125 + }, + { + "x": 7.999484882530383, + "y": 7.314139197912392, + "heading": 0.4679404256802237, + "angularVelocity": 0.1546791933829663, + "velocityX": 0.4186522398635212, + "velocityY": 0.2919578963709263, + "timestamp": 3.3687678605040947 + }, + { + "x": 8.008931159973145, + "y": 7.328619956970215, + "heading": 0.4756948301053548, + "angularVelocity": 0.12145264717068183, + "velocityX": 0.1479514529335022, + "velocityY": 0.22680355887981873, + "timestamp": 3.432615002627177 + }, + { + "x": 7.9838028545378235, + "y": 7.340994975747599, + "heading": 0.4825391441548679, + "angularVelocity": 0.07100818067139564, + "velocityX": -0.26070037689812714, + "velocityY": 0.12838796741348574, + "timestamp": 3.5290026861982695 + }, + { + "x": 7.919285560188609, + "y": 7.343883953710174, + "heading": 0.4845188810802152, + "angularVelocity": 0.02053931427751041, + "velocityX": -0.6693520578449088, + "velocityY": 0.029972480461632738, + "timestamp": 3.625390369769362 + }, + { + "x": 7.815379289936097, + "y": 7.337286883355744, + "heading": 0.4816330205236378, + "angularVelocity": -0.029940138092932247, + "velocityX": -1.0780036038097405, + "velocityY": -0.06844308432376209, + "timestamp": 3.7217780533404548 + }, + { + "x": 7.6720840556149295, + "y": 7.321203739342901, + "heading": 0.47388185121605153, + "angularVelocity": -0.08041659494667612, + "velocityX": -1.4866550269929244, + "velocityY": -0.16685891202042144, + "timestamp": 3.8181657369115474 + }, + { + "x": 7.489399868443907, + "y": 7.295634478019173, + "heading": 0.46126687890088164, + "angularVelocity": -0.13087743005945188, + "velocityX": -1.895306333783606, + "velocityY": -0.2652751926014197, + "timestamp": 3.91455342048264 + }, + { + "x": 7.267326740077932, + "y": 7.2605790366684495, + "heading": 0.44379066346613116, + "angularVelocity": -0.1813117069242423, + "velocityX": -2.3039575196573705, + "velocityY": -0.36369212384761535, + "timestamp": 4.010941104053733 + }, + { + "x": 7.00586468444221, + "y": 7.216037332217007, + "heading": 0.4214565844390286, + "angularVelocity": -0.23171092197300747, + "velocityX": -2.7126085610603474, + "velocityY": -0.4621099169645314, + "timestamp": 4.107328787624826 + }, + { + "x": 6.7050137215236445, + "y": 7.162009258221821, + "heading": 0.3942685380548319, + "angularVelocity": -0.2820697144790415, + "velocityX": -3.1212593951037992, + "velocityY": -0.5605288144032944, + "timestamp": 4.203716471195919 + }, + { + "x": 6.364773891396992, + "y": 7.098494671595923, + "heading": 0.3622306012435557, + "angularVelocity": -0.3323862097758952, + "velocityX": -3.529909813380894, + "velocityY": -0.658949196336373, + "timestamp": 4.300104154767012 + }, + { + "x": 6.010374312951181, + "y": 7.013136980735367, + "heading": 0.36223059672201996, + "angularVelocity": -4.690989126354215e-8, + "velocityX": -3.6768139384158545, + "velocityY": -0.8855663680059631, + "timestamp": 4.396491838338105 + }, + { + "x": 5.655974859532265, + "y": 6.927778770772817, + "heading": 0.36223059220056536, + "angularVelocity": -4.690904952415849e-8, + "velocityX": -3.676812641290635, + "velocityY": -0.885571753569461, + "timestamp": 4.492879521909198 + }, + { + "x": 5.301574928644714, + "y": 6.842422689744232, + "heading": 0.3622305506125328, + "angularVelocity": -4.314662515297724e-7, + "velocityX": -3.6768175949176816, + "velocityY": -0.8855496663702899, + "timestamp": 4.589267205480291 + }, + { + "x": 4.986369114322999, + "y": 6.766504438890574, + "heading": 0.34532942408176365, + "angularVelocity": -0.1753452921016411, + "velocityX": -3.270187669664538, + "velocityY": -0.7876343536948325, + "timestamp": 4.685654889051384 + }, + { + "x": 4.710566832932512, + "y": 6.70007668327783, + "heading": 0.3264669766789321, + "angularVelocity": -0.1956935440711402, + "velocityX": -2.861385097890258, + "velocityY": -0.6891726530988762, + "timestamp": 4.782042572622477 + }, + { + "x": 4.474170501345758, + "y": 6.643140019477641, + "heading": 0.30840376738356945, + "angularVelocity": -0.18740163292794487, + "velocityX": -2.4525574516208333, + "velocityY": -0.5907047632096585, + "timestamp": 4.87843025619357 + }, + { + "x": 4.277177710352041, + "y": 6.5956938222588235, + "heading": 0.2923060351860776, + "angularVelocity": -0.16701026107365813, + "velocityX": -2.0437548003570405, + "velocityY": -0.4922433599498598, + "timestamp": 4.974817939764663 + }, + { + "x": 4.1195862527073634, + "y": 6.557737539015946, + "heading": 0.27881764120910085, + "angularVelocity": -0.1399389784798502, + "velocityX": -1.6349750487409798, + "velocityY": -0.3937876898440308, + "timestamp": 5.071205623335756 + }, + { + "x": 4.0013944038572795, + "y": 6.529270748726535, + "heading": 0.26834692670698207, + "angularVelocity": -0.10863124949357753, + "velocityX": -1.226213188979785, + "velocityY": -0.2953363877493204, + "timestamp": 5.167593306906849 + }, + { + "x": 3.9226008250519366, + "y": 6.510293128915594, + "heading": 0.26117603215990287, + "angularVelocity": -0.07439637805789222, + "velocityX": -0.817465218439728, + "velocityY": -0.19688843125838204, + "timestamp": 5.263980990477942 + }, + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": -0.038017450198398375, + "velocityX": -0.40872820518438957, + "velocityY": -0.09844312341738799, + "timestamp": 5.3603686740490355 + }, + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -1.0638409913121086e-32, + "timestamp": 5.456756357620129 + }, + { + "x": 3.856532695000848, + "y": 6.488812456981272, + "heading": 0.25799867286334044, + "angularVelocity": 0.005949010116156059, + "velocityX": -0.32577575495196986, + "velocityY": -0.14647295299154703, + "timestamp": 5.53862790495835 + }, + { + "x": 3.8031475649180457, + "y": 6.464921556304023, + "heading": 0.25900179360543873, + "angularVelocity": 0.01225237307357005, + "velocityX": -0.6520596204474033, + "velocityY": -0.29180956576467965, + "timestamp": 5.620499452296571 + }, + { + "x": 3.7229939182765985, + "y": 6.429256431279799, + "heading": 0.2605597691266567, + "angularVelocity": 0.019029511128961094, + "velocityX": -0.9790171219107733, + "velocityY": -0.43562295063126366, + "timestamp": 5.702370999634793 + }, + { + "x": 3.6159951636151835, + "y": 6.38199294529669, + "heading": 0.26272711898900253, + "angularVelocity": 0.02647256504622092, + "velocityX": -1.3069101310543174, + "velocityY": -0.5772882951369854, + "timestamp": 5.784242546973014 + }, + { + "x": 3.4820378233980955, + "y": 6.3233976602765205, + "heading": 0.2655861022623468, + "angularVelocity": 0.03492035226271501, + "velocityX": -1.6361891838161302, + "velocityY": -0.7156977842143984, + "timestamp": 5.866114094311236 + }, + { + "x": 3.3209367433633594, + "y": 6.253922161955361, + "heading": 0.26927508652336807, + "angularVelocity": 0.04505819641812337, + "velocityX": -1.9677297580465591, + "velocityY": -0.8485914897167804, + "timestamp": 5.947985641649457 + }, + { + "x": 3.132337992654885, + "y": 6.1744965280297155, + "heading": 0.27407536868463933, + "angularVelocity": 0.0586318729440976, + "velocityX": -2.303593334193982, + "velocityY": -0.9701249885693223, + "timestamp": 6.029857188987679 + }, + { + "x": 2.9153402053678152, + "y": 6.088110219485557, + "heading": 0.2808618012562985, + "angularVelocity": 0.08289122158182176, + "velocityX": -2.650466423831284, + "velocityY": -1.0551444470358684, + "timestamp": 6.1117287363259 + }, + { + "x": 2.6980153975230565, + "y": 6.030900368674856, + "heading": 0.29585281339155545, + "angularVelocity": 0.1831040529053057, + "velocityX": -2.6544607364871555, + "velocityY": -0.6987757367569974, + "timestamp": 6.1936002836641215 + }, + { + "x": 2.5044902704926533, + "y": 5.990681526275272, + "heading": 0.31201148477172486, + "angularVelocity": 0.1973661412971212, + "velocityX": -2.3637653534374596, + "velocityY": -0.4912432182750236, + "timestamp": 6.275471831002343 + }, + { + "x": 2.3360827419423367, + "y": 5.965439222430923, + "heading": 0.328645437097879, + "angularVelocity": 0.20317134422093217, + "velocityX": -2.0569725872482194, + "velocityY": -0.30831594937456525, + "timestamp": 6.357343378340564 + }, + { + "x": 2.1932220965526215, + "y": 5.95442921664246, + "heading": 0.34550838612461254, + "angularVelocity": 0.2059683684378234, + "velocityX": -1.744936428275136, + "velocityY": -0.13447902411053952, + "timestamp": 6.439214925678786 + }, + { + "x": 2.076119719939017, + "y": 5.957265361506873, + "heading": 0.3624743968160351, + "angularVelocity": 0.20722718994590164, + "velocityX": -1.4303183513784072, + "velocityY": 0.03464139810008477, + "timestamp": 6.521086473017007 + }, + { + "x": 1.984901205969126, + "y": 5.973711453340982, + "heading": 0.37946704953669896, + "angularVelocity": 0.20755260249895005, + "velocityX": -1.1141662388895142, + "velocityY": 0.20087676816670857, + "timestamp": 6.602958020355229 + }, + { + "x": 1.9196497213527057, + "y": 6.003608138127345, + "heading": 0.39643505242128974, + "angularVelocity": 0.207251523102328, + "velocityX": -0.7969983069558729, + "velocityY": 0.3651657475442049, + "timestamp": 6.68482956769345 + }, + { + "x": 1.8804243803024292, + "y": 6.046840667724609, + "heading": 0.41334160220463595, + "angularVelocity": 0.20650091922048514, + "velocityX": -0.4791083389230676, + "velocityY": 0.5280531638014023, + "timestamp": 6.766701115031672 + }, + { + "x": 1.8671378435769306, + "y": 6.1025986442210955, + "heading": 0.4299850055686104, + "angularVelocity": 0.20542336557580967, + "velocityX": -0.16399080352197462, + "velocityY": 0.6882000597544763, + "timestamp": 6.84772112615515 + }, + { + "x": 1.8794728316218305, + "y": 6.171151657888672, + "heading": 0.4464831819952248, + "angularVelocity": 0.20363088325759873, + "velocityX": 0.1522461904639937, + "velocityY": 0.8461244662518951, + "timestamp": 6.928741137278629 + }, + { + "x": 1.9175617070085949, + "y": 6.252231071793212, + "heading": 0.46275009292634184, + "angularVelocity": 0.2007764588716919, + "velocityX": 0.4701168866629144, + "velocityY": 1.0007331865330475, + "timestamp": 7.009761148402108 + }, + { + "x": 1.9816154560716552, + "y": 6.34539325484771, + "heading": 0.47864458300524776, + "angularVelocity": 0.19617980618000597, + "velocityX": 0.7905917090709768, + "velocityY": 1.1498663325596743, + "timestamp": 7.090781159525586 + }, + { + "x": 2.0720214338634078, + "y": 6.449767500665198, + "heading": 0.4938927703980073, + "angularVelocity": 0.18820273141558316, + "velocityX": 1.115847511474288, + "velocityY": 1.2882526720271141, + "timestamp": 7.171801170649065 + }, + { + "x": 2.1896944819931505, + "y": 6.562891621237937, + "heading": 0.5077401370806992, + "angularVelocity": 0.17091292003882794, + "velocityX": 1.4523948651451515, + "velocityY": 1.3962491365291643, + "timestamp": 7.252821181772544 + }, + { + "x": 2.3309802432224416, + "y": 6.659921406601519, + "heading": 0.5136983140040886, + "angularVelocity": 0.07353957177701248, + "velocityX": 1.7438378404313606, + "velocityY": 1.1976027158982174, + "timestamp": 7.333841192896022 + }, + { + "x": 2.4512963274996546, + "y": 6.737453803425126, + "heading": 0.5174619765612319, + "angularVelocity": 0.04645349346357551, + "velocityX": 1.4850168817410538, + "velocityY": 0.9569536679703065, + "timestamp": 7.414861204019501 + }, + { + "x": 2.5483430242698013, + "y": 6.798294060832995, + "heading": 0.5200641230005525, + "angularVelocity": 0.03211733006744314, + "velocityX": 1.197811447128081, + "velocityY": 0.7509287713518723, + "timestamp": 7.49588121514298 + }, + { + "x": 2.62147124688145, + "y": 6.843385043467462, + "heading": 0.5218324598046723, + "angularVelocity": 0.02182592645444906, + "velocityX": 0.9025945762979182, + "velocityY": 0.556541303922395, + "timestamp": 7.576901226266458 + }, + { + "x": 2.6703780246763387, + "y": 6.8731963037812145, + "heading": 0.5229271765337971, + "angularVelocity": 0.013511683273610298, + "velocityX": 0.6036382508063606, + "velocityY": 0.367949348566719, + "timestamp": 7.657921237389937 + }, + { + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": 0.006368644721814939, + "velocityX": 0.30252191195191025, + "velocityY": 0.18282262031723026, + "timestamp": 7.738941248513416 + }, + { + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": -2.018007055520513e-33, + "velocityX": -1.0144673376047062e-35, + "velocityY": 0, + "timestamp": 7.819961259636894 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "CenterLanePCBA": { + "waypoints": [ + { + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 6.9285315703702866, - "y": 7.445814297629393, - "heading": 0.019185726579151432, - "angularVelocity": -0.3586755233685199, - "velocityX": 3.203950829673405, - "velocityY": 0.2246450555457597, - "timestamp": 3.504263517846762 + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 7.125290870666504, - "y": 7.458, + "x": 2.6583051681518555, + "y": 5.54433536529541, "heading": 0, - "angularVelocity": -0.2840637901552857, - "velocityX": 2.913217404608591, - "velocityY": 0.18042146002750595, - "timestamp": 3.5718037227023802 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 7.284551146493795, - "y": 7.4665356658331685, - "heading": -0.011983560395956015, - "angularVelocity": -0.19978893791899283, - "velocityX": 2.6551742811725556, - "velocityY": 0.1423059220209307, - "timestamp": 3.6317848233617918 + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { - "x": 7.428286998104642, - "y": 7.47296871812108, - "heading": -0.020071347748117502, - "angularVelocity": -0.13483892864997732, - "velocityX": 2.396352351501819, - "velocityY": 0.10725132111930623, - "timestamp": 3.6917659240212033 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -1.7297728380351187e-19, + "angularVelocity": 4.2448351957919954e-19, + "velocityX": -3.109246931729259e-17, + "velocityY": 1.4154954258989747e-18, + "timestamp": 0 }, { - "x": 7.556472866857842, - "y": 7.4774155432867, - "heading": -0.02498803078182015, - "angularVelocity": -0.08197053704667534, - "velocityX": 2.137104310257201, - "velocityY": 0.0741371051336659, - "timestamp": 3.751747024680615 + "x": 1.3060810448259774, + "y": 5.572403683390414, + "heading": -0.01067910748684314, + "angularVelocity": -0.14191654637088458, + "velocityX": 0.21404337049401068, + "velocityY": -0.246522471704611, + "timestamp": 0.07524920638169826 }, { - "x": 7.669092898448606, - "y": 7.47995645101358, - "heading": -0.027230015787281982, - "angularVelocity": -0.037378190476904, - "velocityX": 1.877591947341067, - "velocityY": 0.04236180561785803, - "timestamp": 3.8117281253400264 + "x": 1.338296279992689, + "y": 5.535304313687478, + "heading": -0.032027558921844806, + "angularVelocity": -0.28370334335098873, + "velocityX": 0.4281139525027307, + "velocityY": -0.4930200793709309, + "timestamp": 0.1504984127633965 }, { - "x": 7.766136403587462, - "y": 7.480650177988498, - "heading": -0.027158837231048453, - "angularVelocity": 0.001186683062681687, - "velocityX": 1.6179013734658474, - "velocityY": 0.011565759335732523, - "timestamp": 3.871709225999438 + "x": 1.3866231363779933, + "y": 5.479658598357654, + "heading": -0.06402169471627948, + "angularVelocity": -0.42517572387591435, + "velocityX": 0.6422241337702286, + "velocityY": -0.7394857435109029, + "timestamp": 0.22574761914509478 }, { - "x": 7.847595735447885, - "y": 7.479541543526011, - "heading": -0.025049657080079103, - "angularVelocity": 0.03516407881452247, - "velocityX": 1.3580833123248393, - "velocityY": -0.018483063003166142, - "timestamp": 3.9316903266588494 + "x": 1.4510659275520246, + "y": 5.405469878379882, + "heading": -0.1066262273585151, + "angularVelocity": -0.5661791624235272, + "velocityX": 0.8563916388334698, + "velocityY": -0.9859070087913582, + "timestamp": 0.300996825526793 }, { - "x": 7.913465176456017, - "y": 7.4766658686921605, - "heading": -0.021118970688138776, - "angularVelocity": 0.06553208175121362, - "velocityX": 1.0981699282605077, - "velocityY": -0.04794301542047442, - "timestamp": 3.991671427318261 + "x": 1.5316310742105061, + "y": 5.312743180460654, + "heading": -0.15979745091713102, + "angularVelocity": -0.7066017851257455, + "velocityX": 1.0706444696567097, + "velocityY": -1.2322614732810149, + "timestamp": 0.37624603190849126 }, { - "x": 7.963740302847386, - "y": 7.472051704673601, - "heading": -0.015541591171907823, - "angularVelocity": 0.09298561471722216, - "velocityX": 0.8381827915570343, - "velocityY": -0.07692696479112789, - "timestamp": 4.051652527977672 + "x": 1.6283288838862067, + "y": 5.201486845299536, + "heading": -0.22348688396132305, + "angularVelocity": -0.8463801295274643, + "velocityX": 1.2850342791041873, + "velocityY": -1.4785050967404112, + "timestamp": 0.4514952382901895 }, { - "x": 7.998417597522527, - "y": 7.465722607991469, - "heading": -0.008461644038298986, - "angularVelocity": 0.11803629903043378, - "velocityX": 0.5781370180592035, - "velocityY": -0.10551818176978627, - "timestamp": 4.111633628637083 + "x": 1.7411789077586277, + "y": 5.071717216156576, + "heading": -0.2976431936929222, + "angularVelocity": -0.9854763033040427, + "velocityX": 1.4996839076304158, + "velocityY": -1.7245315317294676, + "timestamp": 0.5267444446718877 }, { - "x": 8.017494201660156, - "y": 7.457698345184326, - "heading": 0, - "angularVelocity": 0.14107183671647575, - "velocityX": 0.3180435825269393, - "velocityY": -0.1337798526356988, - "timestamp": 4.171614729296494 + "x": 1.8702368010806378, + "y": 4.923481749892843, + "heading": -0.3821997162379228, + "angularVelocity": -1.1236865690804896, + "velocityX": 1.7150731486446302, + "velocityY": -1.9699273041075376, + "timestamp": 0.601993651053586 }, { - "x": 8.008975668857596, - "y": 7.440959395735479, - "heading": 0.016440344425850486, - "angularVelocity": 0.17454816573497248, - "velocityX": -0.09044179592138195, - "velocityY": -0.17771847395317164, - "timestamp": 4.265802739560738 + "x": 2.022770759979851, + "y": 4.779444476824575, + "heading": -0.46182371324885607, + "angularVelocity": -1.0581373656886817, + "velocityX": 2.0270507322765163, + "velocityY": -1.914136772919067, + "timestamp": 0.6772428574352843 }, { - "x": 7.961981832760565, - "y": 7.4200887947149115, - "heading": 0.0360050890089686, - "angularVelocity": 0.20772011775415367, - "velocityX": -0.4989364990850843, - "velocityY": -0.22158447728129313, - "timestamp": 4.3599907498249815 + "x": 2.159313623811303, + "y": 4.6540559405768205, + "heading": -0.5309316944889129, + "angularVelocity": -0.9183881739497737, + "velocityX": 1.8145422443221764, + "velocityY": -1.666310414116386, + "timestamp": 0.7524920638169826 }, { - "x": 7.876511570645417, - "y": 7.39509531765153, - "heading": 0.058657775076795544, - "angularVelocity": 0.24050498576490828, - "velocityX": -0.9074431222759898, - "velocityY": -0.2653573102697685, - "timestamp": 4.454178760089225 + "x": 2.2798095691663756, + "y": 4.547269113259306, + "heading": -0.5895622742606058, + "angularVelocity": -0.7791521344995035, + "velocityX": 1.601291909229005, + "velocityY": -1.4191090172553904, + "timestamp": 0.8277412701986809 }, { - "x": 7.752563396875145, - "y": 7.365990633137338, - "heading": 0.08435039467917142, - "angularVelocity": 0.2727801503641011, - "velocityX": -1.315965518567977, - "velocityY": -0.3090062570866462, - "timestamp": 4.548366770353469 + "x": 2.3842397515062683, + "y": 4.459067906121371, + "heading": -0.6377272819831894, + "angularVelocity": -0.6400732982918503, + "velocityX": 1.3877911457302048, + "velocityY": -1.172121426644562, + "timestamp": 0.9029904765803792 }, { - "x": 7.590135252234061, - "y": 7.332791018782474, - "heading": 0.11301654484596554, - "angularVelocity": 0.3043503104733978, - "velocityX": -1.7245097776818146, - "velocityY": -0.35248238349789773, - "timestamp": 4.642554780617712 + "x": 2.47259465372947, + "y": 4.389444105411809, + "heading": -0.675431270833758, + "angularVelocity": -0.5010549700591305, + "velocityX": 1.1741639077877584, + "velocityY": -0.9252429900248669, + "timestamp": 0.9782396829620775 + }, + { + "x": 2.5448685468329297, + "y": 4.338392728081247, + "heading": -0.7026759403224371, + "angularVelocity": -0.36205922691677456, + "velocityX": 0.9604605361114344, + "velocityY": -0.6784307740288329, + "timestamp": 1.0534888893437757 }, { - "x": 7.3892240901384, - "y": 7.295520773303632, - "heading": 0.14455765680612448, - "angularVelocity": 0.33487395977121315, - "velocityX": -2.13308638256617, - "velocityY": -0.39570052891318314, - "timestamp": 4.736742790881956 + "x": 2.601057641331632, + "y": 4.305910462744002, + "heading": -0.7194616948028688, + "angularVelocity": -0.2230688573014308, + "velocityX": 0.7467068052991092, + "velocityY": -0.43166256362226185, + "timestamp": 1.128738095725474 }, { - "x": 7.149824938411661, - "y": 7.254220108341429, - "heading": 0.1788109128374065, - "angularVelocity": 0.36366896312157765, - "velocityX": -2.54171577735964, - "velocityY": -0.4384917448232911, - "timestamp": 4.8309308011461995 + "x": 2.641159295921209, + "y": 4.291994997972796, + "heading": -0.7257882885399572, + "angularVelocity": -0.08407522206898749, + "velocityX": 0.5329179737262918, + "velocityY": -0.18492507018124238, + "timestamp": 1.2039873021071723 }, { - "x": 6.871928226350113, - "y": 7.208968463075644, - "heading": 0.21545413209734332, - "angularVelocity": 0.3890433523028524, - "velocityX": -2.9504467849136042, - "velocityY": -0.4804395499897761, - "timestamp": 4.925118811410443 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.054926472219667455, + "velocityX": 0.31910406053771284, + "velocityY": 0.061790547746105144, + "timestamp": 1.2792365084888706 }, { - "x": 6.555508023323144, - "y": 7.15999689291417, - "heading": 0.25354713749374036, - "angularVelocity": 0.40443582245264, - "velocityX": -3.3594530995957568, - "velocityY": -0.5199342254293745, - "timestamp": 5.019306821674687 + "x": 2.6730242117812852, + "y": 4.320207160810404, + "heading": -0.7068522062263088, + "angularVelocity": 0.19509567726207716, + "velocityX": 0.10349358326673892, + "velocityY": 0.31054279257798323, + "timestamp": 1.3551116333379198 }, { - "x": 6.201946981022424, - "y": 7.116596127430346, - "heading": 0.25354714311943394, - "angularVelocity": 5.972834101491316e-8, - "velocityX": -3.753779714730222, - "velocityY": -0.46078864350212084, - "timestamp": 5.11349483193893 + "x": 2.6645156104831624, + "y": 4.362642495661916, + "heading": -0.6814188000083412, + "angularVelocity": 0.33520084834820957, + "velocityX": -0.1121395360458166, + "velocityY": 0.5592786164892918, + "timestamp": 1.430986758186969 }, { - "x": 5.847829680383322, - "y": 7.077995922009697, - "heading": 0.25354714640605686, - "angularVelocity": 3.4894280958051e-8, - "velocityX": -3.7596855443238373, - "velocityY": -0.4098207968546854, - "timestamp": 5.207682842203174 + "x": 2.6396435810539995, + "y": 4.423949055779942, + "heading": -0.6453599487845141, + "angularVelocity": 0.47523943183649325, + "velocityX": -0.32780215490271986, + "velocityY": 0.807992873026772, + "timestamp": 1.5068618830360183 }, { - "x": 5.493712379149646, - "y": 7.039395722043647, - "heading": 0.25354714969267966, - "angularVelocity": 3.4894280048226024e-8, - "velocityX": -3.759685550636459, - "velocityY": -0.40982073894285953, - "timestamp": 5.301870852467418 + "x": 2.5984050053739445, + "y": 4.5041245094115245, + "heading": -0.5986803757122448, + "angularVelocity": 0.615215766235525, + "velocityX": -0.5435058691760214, + "velocityY": 1.0566763981096883, + "timestamp": 1.5827370078850675 }, { - "x": 5.13959507794107, - "y": 7.0007955218474756, - "heading": 0.25354715297933, - "angularVelocity": 3.489457219723442e-8, - "velocityX": -3.7596855503699804, - "velocityY": -0.4098207413860709, - "timestamp": 5.396058862731661 + "x": 2.540795163397632, + "y": 4.603165195262531, + "heading": -0.541383229045499, + "angularVelocity": 0.755150608064052, + "velocityX": -0.7592717915230448, + "velocityY": 1.3053116689836102, + "timestamp": 1.6586121327341168 }, { - "x": 4.793375065328026, - "y": 6.963049240971564, - "heading": 0.2809926873119857, - "angularVelocity": 0.2913909557666365, - "velocityX": -3.675839543076952, - "velocityY": -0.4007546265178986, - "timestamp": 5.490246872995905 + "x": 2.4668060178101583, + "y": 4.721064600465768, + "heading": -0.4734677826566921, + "angularVelocity": 0.8950950199273465, + "velocityX": -0.9751436420635095, + "velocityY": 1.5538611031988796, + "timestamp": 1.734487257583166 }, { - "x": 4.4856271372174605, - "y": 6.929500155423233, - "heading": 0.3062072463057812, - "angularVelocity": 0.2677045509620205, - "velocityX": -3.2673790140292933, - "velocityY": -0.3561927410315718, - "timestamp": 5.5844348832601485 + "x": 2.3764209643089336, + "y": 4.857808746061696, + "heading": -0.3949264705539342, + "angularVelocity": 1.0351391481588907, + "velocityX": -1.1912343298399446, + "velocityY": 1.8022263010181736, + "timestamp": 1.8103623824322153 }, { - "x": 4.216349125910067, - "y": 6.900146018877801, - "heading": 0.32859640421091696, - "angularVelocity": 0.23770709076795637, - "velocityX": -2.858941499580855, - "velocityY": -0.31165470491497754, - "timestamp": 5.678622893524392 + "x": 2.269588305263905, + "y": 5.013353141552007, + "heading": -0.3057482687442009, + "angularVelocity": 1.175328567657553, + "velocityX": -1.4080063691172056, + "velocityY": 2.0500051340898513, + "timestamp": 1.8862375072812645 }, { - "x": 3.9855401854111148, - "y": 6.874986045056756, - "heading": 0.34796083888612495, - "angularVelocity": 0.2055934149249067, - "velocityX": -2.450512967111424, - "velocityY": -0.26712501676655553, - "timestamp": 5.772810903788636 + "x": 2.155512806030656, + "y": 5.145840094887286, + "heading": -0.22945454409592414, + "angularVelocity": 1.0055169569743476, + "velocityX": -1.5034637433586158, + "velocityY": 1.7461184228535829, + "timestamp": 1.9621126321303137 }, { - "x": 3.7931998687162816, - "y": 6.854019825581219, - "heading": 0.36419988308525797, - "angularVelocity": 0.17241094862896578, - "velocityX": -2.042089180514842, - "velocityY": -0.22259966440226955, - "timestamp": 5.866998914052879 + "x": 2.0576690248190195, + "y": 5.259344023095087, + "heading": -0.1639803843714035, + "angularVelocity": 0.8629199603276121, + "velocityX": -1.289537004472555, + "velocityY": 1.4959306944600357, + "timestamp": 2.037987756979363 }, { - "x": 3.639327899901621, - "y": 6.837247108195556, - "heading": 0.37725261237367164, - "angularVelocity": 0.13858164379727655, - "velocityX": -1.6336683234200817, - "velocityY": -0.17807699025180046, - "timestamp": 5.961186924317123 + "x": 1.9761088976814785, + "y": 5.353909559223238, + "heading": -0.10935658983885492, + "angularVelocity": 0.7199170300053718, + "velocityX": -1.0749257717858158, + "velocityY": 1.2463312095522094, + "timestamp": 2.113862881828412 }, { - "x": 3.5239240923326007, - "y": 6.824667722043819, - "heading": 0.38707830352299477, - "angularVelocity": 0.10431997790119188, - "velocityX": -1.2252494478358333, - "velocityY": -0.13355613008965372, - "timestamp": 6.0553749345813666 + "x": 1.9108492511031485, + "y": 5.429551680381221, + "heading": -0.06562174275393094, + "angularVelocity": 0.5764056029164016, + "velocityX": -0.8600927735973349, + "velocityY": 0.9969291162074897, + "timestamp": 2.1897380066774614 }, { - "x": 3.4469883122501845, - "y": 6.8162815454148475, - "heading": 0.39364810050149374, - "angularVelocity": 0.06975194571015403, - "velocityX": -0.8168319923796448, - "velocityY": -0.08903656214250527, - "timestamp": 6.14956294484561 + "x": 1.8618983395529738, + "y": 5.486277925937523, + "heading": -0.03280716495777854, + "angularVelocity": 0.4324813680566584, + "velocityX": -0.6451509852228532, + "velocityY": 0.7476263883465671, + "timestamp": 2.2656131315265107 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0.03495947190378589, - "velocityX": -0.4084155935928461, - "velocityY": -0.04451793673752315, - "timestamp": 6.243750955109854 + "x": 1.8292611562031649, + "y": 5.524092810268815, + "heading": -0.010931847108471086, + "angularVelocity": 0.2883068448697581, + "velocityX": -0.430143389085703, + "velocityY": 0.4983831579385639, + "timestamp": 2.34148825637556 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": -1.9952146739162206e-34, - "velocityY": -3.951795765453016e-33, - "timestamp": 6.337938965374097 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -1.6776667047563746e-20, + "angularVelocity": 0.1440768253130289, + "velocityX": -0.2150897630156809, + "velocityY": 0.24917859900600148, + "timestamp": 2.417363381224609 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.1170600847702852e-20, + "angularVelocity": 5.938902420193292e-19, + "velocityX": -2.686779016810018e-17, + "velocityY": -1.2969153357483792e-17, + "timestamp": 2.4932385060736584 }, { - "x": 3.425127749537185, - "y": 6.810478633677757, - "heading": 0.3886892234850989, - "angularVelocity": -0.13293807639169805, - "velocityX": 0.2675517930120232, - "velocityY": -0.025935588272518423, - "timestamp": 6.400010272097332 + "x": 1.8552093951788071, + "y": 5.543066072468525, + "heading": -1.0867013680305264e-17, + "angularVelocity": -1.1051820926914384e-16, + "velocityX": 0.42942883009919236, + "velocityY": 0.0006787122433536914, + "timestamp": 2.5916673926214395 }, { - "x": 3.458347138585669, - "y": 6.807258412874914, - "heading": 0.3723514064813247, - "angularVelocity": -0.26321045691257755, - "velocityX": 0.535181081278085, - "velocityY": -0.05187937829625554, - "timestamp": 6.462081578820567 + "x": 1.9397457967809342, + "y": 5.543199682246804, + "heading": -3.2635799657096195e-17, + "angularVelocity": -2.2116257473548814e-16, + "velocityX": 0.8588576440015888, + "velocityY": 0.001357424461108283, + "timestamp": 2.6900962791692207 }, { - "x": 3.508184213080087, - "y": 6.802427294600358, - "heading": 0.34812438720722594, - "angularVelocity": -0.3903094771652888, - "velocityX": 0.8029003596884762, - "velocityY": -0.07783174754300737, - "timestamp": 6.524152885543802 + "x": 2.0665503951985436, + "y": 5.5434000969079245, + "heading": -4.603800029490119e-17, + "angularVelocity": -1.3616125416355767e-16, + "velocityX": 1.2882864255103954, + "velocityY": 0.002036136627664645, + "timestamp": 2.788525165717002 }, { - "x": 3.5746455076736363, - "y": 6.795984642746885, - "heading": 0.31624502791908643, - "angularVelocity": -0.5135925272251962, - "velocityX": 1.070724914651617, - "velocityY": -0.10379436479708294, - "timestamp": 6.586224192267037 + "x": 2.235623180866242, + "y": 5.543667316436768, + "heading": -4.798133527402523e-17, + "angularVelocity": -1.9743543259972466e-17, + "velocityX": 1.7177151098384498, + "velocityY": 0.002714848640626296, + "timestamp": 2.886954052264783 }, { - "x": 3.6577387353786484, - "y": 6.787929625128416, - "heading": 0.2770021560106679, - "angularVelocity": -0.6322224225666108, - "velocityX": 1.3386737301264124, - "velocityY": -0.12977038898779852, - "timestamp": 6.648295498990271 + "x": 2.40469596653394, + "y": 5.54393453596561, + "heading": -3.9330353852399465e-17, + "angularVelocity": 8.789067645262313e-17, + "velocityX": 1.7177151098384498, + "velocityY": 0.0027148486406302456, + "timestamp": 2.985382938812564 }, { - "x": 3.7574730901724314, - "y": 6.7782611086258875, - "heading": 0.23075444743671972, - "angularVelocity": -0.7450738677080124, - "velocityX": 1.6067706652041787, - "velocityY": -0.1557646682971112, - "timestamp": 6.710366805713506 + "x": 2.531500564951549, + "y": 5.544134950626731, + "heading": -2.0009563266152006e-17, + "angularVelocity": 1.96291873894211e-16, + "velocityX": 1.2882864255103954, + "velocityY": 0.002036136627666603, + "timestamp": 3.0838118253603453 }, { - "x": 3.873859634378807, - "y": 6.766977536857145, - "heading": 0.17795757602156642, - "angularVelocity": -0.8505841781386996, - "velocityX": 1.875045819887845, - "velocityY": -0.18178402170643457, - "timestamp": 6.772438112436741 + "x": 2.616036966553676, + "y": 5.54426856040501, + "heading": -7.02269200923652e-18, + "angularVelocity": 1.3194166582538958e-16, + "velocityX": 0.8588576440015888, + "velocityY": 0.0013574244611093242, + "timestamp": 3.1822407119081264 }, { - "x": 4.006911762109537, - "y": 6.754076769706622, - "heading": 0.11920696294593584, - "angularVelocity": -0.9465019535937051, - "velocityX": 2.1435367604550115, - "velocityY": -0.2078378534549514, - "timestamp": 6.834509419159976 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -3.8911537114769476e-19, + "angularVelocity": 6.739461219451495e-17, + "velocityX": 0.4294288300991924, + "velocityY": 0.0006787122433541438, + "timestamp": 3.2806695984559076 }, { - "x": 4.156645657189765, - "y": 6.73955582918857, - "heading": 0.055309270826440785, - "angularVelocity": -1.029423988195197, - "velocityX": 2.4122884305927053, - "velocityY": -0.23393966205310818, - "timestamp": 6.896580725883211 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -4.27312910681112e-19, + "angularVelocity": -3.880724585667238e-19, + "velocityX": -2.3908482729518656e-18, + "velocityY": 6.838225330043828e-18, + "timestamp": 3.3790984850036887 }, { - "x": 4.323080375842652, - "y": 6.723410435666096, - "heading": -0.012586184407094544, - "angularVelocity": -1.0938299645646508, - "velocityX": 2.681347106079046, - "velocityY": -0.2601104177560936, - "timestamp": 6.958652032606445 + "x": 2.6438893629989613, + "y": 5.563919104052164, + "heading": 0.003139586303789949, + "angularVelocity": 0.042042846518139325, + "velocityX": -0.19304501448070868, + "velocityY": 0.26224987725536203, + "timestamp": 3.453774358875246 }, { - "x": 4.506235840968647, - "y": 6.705634179985004, - "heading": -0.08270962096165607, - "angularVelocity": -1.1297238652836274, - "velocityX": 2.9507267495214404, - "velocityY": -0.2863844281597597, - "timestamp": 7.02072333932968 + "x": 2.6152669032432256, + "y": 5.6032366820496495, + "heading": 0.009644303445155527, + "angularVelocity": 0.0871060063194363, + "velocityX": -0.38328925088933385, + "velocityY": 0.5265097809918172, + "timestamp": 3.5284502327468035 }, { - "x": 4.706118949857515, - "y": 6.686217722826257, - "heading": -0.15194437984101536, - "angularVelocity": -1.1154068205470853, - "velocityX": 3.2202175117742615, - "velocityY": -0.3128088996947802, - "timestamp": 7.082794646052915 + "x": 2.5727487902328847, + "y": 5.662504910556293, + "heading": 0.019847811829768587, + "angularVelocity": 0.13663728130135686, + "velocityX": -0.5693688042201159, + "velocityY": 0.7936730490571009, + "timestamp": 3.603126106618361 }, { - "x": 4.922607059053276, - "y": 6.66515425622058, - "heading": -0.21301282950448638, - "angularVelocity": -0.9838434678965439, - "velocityX": 3.4877324262085807, - "velocityY": -0.3393430510421988, - "timestamp": 7.14486595277615 + "x": 2.5168455054942007, + "y": 5.742063733198033, + "heading": 0.03429127316793741, + "angularVelocity": 0.19341536415109115, + "velocityX": -0.7486123943435556, + "velocityY": 1.0653885721991296, + "timestamp": 3.6778019804899182 }, { - "x": 5.152321807662402, - "y": 6.642265811014449, - "heading": -0.22692060137612477, - "angularVelocity": -0.22406120647088962, - "velocityX": 3.7008202458728654, - "velocityY": -0.3687443750482909, - "timestamp": 7.206937259499385 + "x": 2.4485453653591587, + "y": 5.842518348175502, + "heading": 0.05399784068706335, + "angularVelocity": 0.2638947024981649, + "velocityX": -0.9146212370077963, + "velocityY": 1.3452084290335973, + "timestamp": 3.7524778543614756 }, { - "x": 5.385997622338085, - "y": 6.619824669792801, - "heading": -0.22692062699124488, - "angularVelocity": -4.12672480414244e-7, - "velocityX": 3.7646350143327725, - "velocityY": -0.36153808267225507, - "timestamp": 7.269008566222619 + "x": 2.370517685528427, + "y": 5.96519344251776, + "heading": 0.08158842838664396, + "angularVelocity": 0.369471239761256, + "velocityX": -1.0448847236115244, + "velocityY": 1.642767442578038, + "timestamp": 3.827153728233033 }, { - "x": 5.6195399174914655, - "y": 6.5960341766076915, - "heading": -0.2269206525768778, - "angularVelocity": -4.121974272362167e-7, - "velocityX": 3.7624839476103156, - "velocityY": -0.3832768221124699, - "timestamp": 7.331079872945854 + "x": 2.3003293154277267, + "y": 6.110432122884094, + "heading": 0.1284873855211158, + "angularVelocity": 0.6280335897392839, + "velocityX": -0.9399069131942741, + "velocityY": 1.94492106802986, + "timestamp": 3.9018296021045904 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": -0.22692067835854435, - "angularVelocity": -4.1535562716932914e-7, - "velocityX": 3.729650183615213, - "velocityY": -0.6268142199732243, - "timestamp": 7.393151179669089 + "x": 2.2517603849650345, + "y": 6.244622565856429, + "heading": 0.18176076028404833, + "angularVelocity": 0.7133947284575884, + "velocityX": -0.6503965463628967, + "velocityY": 1.796971846665519, + "timestamp": 3.976505475976148 }, { - "x": 5.959383441341322, - "y": 6.535142573313356, - "heading": -0.22692070161190955, - "angularVelocity": -7.95525127627382e-7, - "velocityX": 3.706414340926548, - "velocityY": -0.7521132022680976, - "timestamp": 7.422381387996315 + "x": 2.22192407028561, + "y": 6.363296472335711, + "heading": 0.23706248397325824, + "angularVelocity": 0.7405567664909981, + "velocityX": -0.39954423206005096, + "velocityY": 1.5891867122091021, + "timestamp": 4.051181349847705 }, { - "x": 6.066940887967902, - "y": 6.509606194124557, - "heading": -0.22692072465630755, - "angularVelocity": -7.883761123678933e-7, - "velocityX": 3.6796674667011553, - "velocityY": -0.8736297361581465, - "timestamp": 7.4516115963235405 + "x": 2.2097448289139274, + "y": 6.465233576489708, + "heading": 0.2929845885143906, + "angularVelocity": 0.7488644141924427, + "velocityX": -0.16309472846117115, + "velocityY": 1.3650607467858884, + "timestamp": 4.125857223719263 }, { - "x": 6.17368571603366, - "y": 6.480861582677863, - "heading": -0.2269207475995078, - "angularVelocity": -7.849140177187675e-7, - "velocityX": 3.651866824580282, - "velocityY": -0.9833871563580519, - "timestamp": 7.480841804650766 + "x": 2.2146777318065234, + "y": 6.549876478070479, + "heading": 0.348839415183568, + "angularVelocity": 0.747963482359082, + "velocityX": 0.06605751813605938, + "velocityY": 1.1334705198944957, + "timestamp": 4.20053309759082 }, { - "x": 6.2799139038286755, - "y": 6.450262838777624, - "heading": -0.2269207705194487, - "angularVelocity": -7.841182881547448e-7, - "velocityX": 3.63419194984251, - "velocityY": -1.0468192206387272, - "timestamp": 7.510072012977992 + "x": 2.236395128424509, + "y": 6.61690736376859, + "heading": 0.4042192057980219, + "angularVelocity": 0.7416021767580177, + "velocityX": 0.2908221289159501, + "velocityY": 0.8976243895505626, + "timestamp": 4.275208971462377 }, { - "x": 6.38600768248991, - "y": 6.419201268635774, - "heading": -0.22692079344039778, - "angularVelocity": -7.841527793264923e-7, - "velocityX": 3.629593654398156, - "velocityY": -1.0626530537902528, - "timestamp": 7.539302221305218 + "x": 2.274678638341779, + "y": 6.666121192465354, + "heading": 0.45885367997336884, + "angularVelocity": 0.7316214908889929, + "velocityX": 0.5126623624534666, + "velocityY": 0.6590325113759182, + "timestamp": 4.349884845333935 }, { - "x": 6.4915378820851375, - "y": 6.386275784732387, - "heading": -0.22692082347226308, - "angularVelocity": -0.0000010274256324387564, - "velocityX": 3.6103129479556895, - "velocityY": -1.1264197481865303, - "timestamp": 7.568532429632444 + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.719064094502462, + "velocityX": 0.7324155022029422, + "velocityY": 0.41852376977375944, + "timestamp": 4.424560719205492 }, { - "x": 6.594254502115419, - "y": 6.3529873750963075, - "heading": -0.23642872835410836, - "angularVelocity": -0.32527667183951403, - "velocityX": 3.514057063171563, - "velocityY": -1.1388358667671954, - "timestamp": 7.597762637959669 + "x": 2.4110936768539344, + "y": 6.744072759574757, + "heading": 0.5643107226604884, + "angularVelocity": 0.653664165942785, + "velocityX": 1.0320315606781885, + "velocityY": 0.5897332019890127, + "timestamp": 4.503745575523619 }, { - "x": 6.69366972769076, - "y": 6.320452523543764, - "heading": -0.25267195567048195, - "angularVelocity": -0.5557000187824542, - "velocityX": 3.4011124540202067, - "velocityY": -1.1130557534298289, - "timestamp": 7.626992846286895 + "x": 2.50599401367539, + "y": 6.798301604540289, + "heading": 0.5436072588792712, + "angularVelocity": -0.2614573637418783, + "velocityX": 1.198465732389427, + "velocityY": 0.6848385851414024, + "timestamp": 4.5829304318417465 }, { - "x": 6.789488054498416, - "y": 6.288864279277486, - "heading": -0.26990654049679197, - "angularVelocity": -0.5896155317621415, - "velocityX": 3.278058292810801, - "velocityY": -1.0806711985311153, - "timestamp": 7.656223054614121 + "x": 2.577169265515479, + "y": 6.838973237821007, + "heading": 0.528079022912807, + "angularVelocity": -0.19610108155123726, + "velocityX": 0.8988492894921869, + "velocityY": 0.51362893325611, + "timestamp": 4.662115288159874 }, { - "x": 6.881710669103943, - "y": 6.258257632428043, - "heading": -0.28694313831911705, - "angularVelocity": -0.5828421621787924, - "velocityX": 3.1550447254125045, - "velocityY": -1.047089589879878, - "timestamp": 7.685453262941347 + "x": 2.6246194330616937, + "y": 6.8660876598097635, + "heading": 0.5177266590936774, + "angularVelocity": -0.13073666229232678, + "velocityX": 0.5992328552770498, + "velocityY": 0.3424192863320269, + "timestamp": 4.741300144478001 }, { - "x": 6.970345859458022, - "y": 6.228646491338886, - "heading": -0.3032367291527766, - "angularVelocity": -0.5574230142767849, - "velocityX": 3.0323146986098455, - "velocityY": -1.0130321603485573, - "timestamp": 7.714683471268573 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06536905835734146, + "velocityX": 0.2996164266200211, + "velocityY": 0.1712096425840104, + "timestamp": 4.820485000796128 }, { - "x": 7.055400617247705, - "y": 6.200038168598195, - "heading": -0.3184704659597927, - "angularVelocity": -0.5211641544429054, - "velocityX": 2.909823865690951, - "velocityY": -0.978724558526454, - "timestamp": 7.743913679595798 - }, + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -8.381816905081638e-20, + "velocityX": -7.007327358505968e-19, + "velocityY": 8.954594613208896e-19, + "timestamp": 4.899669857114255 + } + ], + "constraints": [ { - "x": 7.136880378911089, - "y": 6.172437128670844, - "heading": -0.33243641899697507, - "angularVelocity": -0.477791772157844, - "velocityX": 2.7875190197494866, - "velocityY": -0.9442642220804569, - "timestamp": 7.773143887923024 + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 7.214789390563965, - "y": 6.145846366882324, - "heading": -0.344987478573796, - "angularVelocity": -0.42938659336036833, - "velocityX": 2.665359438451487, - "velocityY": -0.9097014120066562, - "timestamp": 7.80237409625025 + "scope": [ + "last" + ], + "type": "StopPoint" }, { - "x": 7.370577859130934, - "y": 6.0916577592619054, - "heading": -0.3653758531922844, - "angularVelocity": -0.3132570133567013, - "velocityX": 2.393610638018096, - "velocityY": -0.8325804140238011, - "timestamp": 7.867459230019526 + "scope": [ + 3 + ], + "type": "StopPoint" }, { - "x": 7.5087533836626745, - "y": 6.042843250581257, - "heading": -0.37947223160146565, - "angularVelocity": -0.21658368958955537, - "velocityX": 2.1229967049244562, - "velocityY": -0.7500101152698444, - "timestamp": 7.932544363788803 + "scope": [ + 5 + ], + "type": "StopPoint" }, { - "x": 7.629367562165718, - "y": 5.999619792415774, - "heading": -0.38804895009153, - "angularVelocity": -0.1317769203712161, - "velocityX": 1.85317554897581, - "velocityY": -0.6641064658284028, - "timestamp": 7.997629497558079 + "scope": [ + 4, + 5 + ], + "type": "StraightLine" }, { - "x": 7.7324579600950285, - "y": 5.962133690972344, - "heading": -0.39162657356882036, - "angularVelocity": -0.05496836635495089, - "velocityX": 1.5839315671495955, - "velocityY": -0.5759548958801737, - "timestamp": 8.062714631327355 + "scope": [ + 2 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "CenterLanePCBAFE": { + "waypoints": [ + { + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 7.81805300550911, - "y": 5.930490256491027, - "heading": -0.39057997729782273, - "angularVelocity": 0.01608041975771331, - "velocityX": 1.3151243679933469, - "velocityY": -0.4861852876186942, - "timestamp": 8.127799765096631 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 7.886174910867723, - "y": 5.904768911877698, - "heading": -0.38519213768662347, - "angularVelocity": 0.08278141718657105, - "velocityX": 1.0466584519915365, - "velocityY": -0.39519538677620675, - "timestamp": 8.192884898865907 + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 7.936841490598973, - "y": 5.885031694896893, - "heading": -0.3756842946663964, - "angularVelocity": 0.1460831755210139, - "velocityX": 0.7784662456231665, - "velocityY": -0.3032523072130836, - "timestamp": 8.257970032635184 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 }, { - "x": 7.970067341476633, - "y": 5.8713284070268905, - "heading": -0.36223415085714655, - "angularVelocity": 0.206654623418748, - "velocityX": 0.510498311264806, - "velocityY": -0.21054405324847428, - "timestamp": 8.32305516640446 + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { - "x": 7.985864639282227, - "y": 5.863699913024902, - "heading": -0.344987478573796, - "angularVelocity": 0.2649863537883773, - "velocityX": 0.24271745160106833, - "velocityY": -0.1172079330593501, - "timestamp": 8.388140300173736 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 7.985045656249217, - "y": 5.861990715165632, - "heading": -0.3251381624172612, - "angularVelocity": 0.31897227576671205, - "velocityX": -0.01316080008970204, - "velocityY": -0.02746627272232015, - "timestamp": 8.450369270101682 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 7.968302412952749, - "y": 5.865866788787815, - "heading": -0.3020436028872691, - "angularVelocity": 0.37112231741475454, - "velocityX": -0.26905866055398503, - "velocityY": 0.062287285594966935, - "timestamp": 8.512598240029627 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 }, { - "x": 7.935633552055529, - "y": 5.875328955639361, - "heading": -0.2758355383990043, - "angularVelocity": 0.4211553641111372, - "velocityX": -0.5249783330022537, - "velocityY": 0.1520540491430483, - "timestamp": 8.574827209957572 + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 7.887037558008595, - "y": 5.8903781286161365, - "heading": -0.24666763452179902, - "angularVelocity": 0.468719053376244, - "velocityX": -0.7809223598462517, - "velocityY": 0.24183548264097016, - "timestamp": 8.637056179885517 + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 7.822512734192608, - "y": 5.911015324029987, - "heading": -0.21472164060862464, - "angularVelocity": 0.5133620876283883, - "velocityX": -1.0368936508301385, - "velocityY": 0.33163324795100674, - "timestamp": 8.699285149813463 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 7.742057181096897, - "y": 5.937241677003945, - "heading": -0.1802161756247138, - "angularVelocity": 0.5544919837796514, - "velocityX": -1.29289546635384, - "velocityY": 0.4214492543316279, - "timestamp": 8.761514119741408 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 7.645668783226514, - "y": 5.9690584605532635, - "heading": -0.14341967249459378, - "angularVelocity": 0.5913082471512265, - "velocityX": -1.5489312772168387, - "velocityY": 0.5112857176674774, - "timestamp": 8.823743089669353 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 }, { - "x": 7.533345224831275, - "y": 6.006467108033043, - "heading": -0.10467019490543185, - "angularVelocity": 0.6226919332592115, - "velocityX": -1.8050043014579413, - "velocityY": 0.6011452145695896, - "timestamp": 8.885972059597298 + "x": 7.984478950500488, + "y": 5.483555793762207, + "heading": 0.7044936776046891, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 }, { - "x": 7.405084087505747, - "y": 6.04946923502926, - "heading": -0.06440725119965417, - "angularVelocity": 0.6470128583583754, - "velocityX": -2.0611161887146876, - "velocityY": 0.6910306734308482, - "timestamp": 8.948201029525244 + "x": 8.217175483703613, + "y": 5.698352336883545, + "heading": 0.7044936776046891, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 7.260883177292953, - "y": 6.098066643466027, - "heading": -0.023226072117979224, - "angularVelocity": 0.6617686124221249, - "velocityX": -2.3172633321708815, - "velocityY": 0.7809450886466088, - "timestamp": 9.010429999453189 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 7.100741532540205, - "y": 6.152261239581916, - "heading": 0.018021968863737303, - "angularVelocity": 0.6628430621538008, - "velocityX": -2.5734259290837285, - "velocityY": 0.8708901365174432, - "timestamp": 9.072658969381134 - }, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ { - "x": 6.924662708731195, - "y": 6.212054571383467, - "heading": 0.05802124542072086, - "angularVelocity": 0.6427758100977419, - "velocityX": -2.829531390490462, - "velocityY": 0.9608600603671181, - "timestamp": 9.13488793930908 + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 0, + "angularVelocity": -8.364664059409844e-30, + "velocityX": -6.773677606947568e-30, + "velocityY": -2.308132540143278e-30, + "timestamp": 0 }, { - "x": 6.732667433541327, - "y": 6.277445378277029, - "heading": 0.09448040335619008, - "angularVelocity": 0.5858872158366861, - "velocityX": -3.0853037646642187, - "velocityY": 1.050809726872821, - "timestamp": 9.197116909237025 + "x": 1.3079899492454155, + "y": 5.5701428985236205, + "heading": -0.011966271801548406, + "angularVelocity": -0.16457159783011938, + "velocityX": 0.24776633609486393, + "velocityY": -0.2862183198381729, + "timestamp": 0.07271164623703327 }, { - "x": 6.524862060286313, - "y": 6.348410321942265, - "heading": 0.12245319566137218, - "angularVelocity": 0.44951398581032764, - "velocityX": -3.33936707446111, - "velocityY": 1.1403843539014786, - "timestamp": 9.25934587916497 + "x": 1.3440211933652915, + "y": 5.528520469164234, + "heading": -0.03588810704483216, + "angularVelocity": -0.328995924069994, + "velocityX": 0.49553607960947427, + "velocityY": -0.5724313987522216, + "timestamp": 0.14542329247406655 }, { - "x": 6.30228026524581, - "y": 6.424202518084616, - "heading": 0.1231057628389814, - "angularVelocity": 0.010486549566300301, - "velocityX": -3.5768195311320152, - "velocityY": 1.2179567849204198, - "timestamp": 9.321574849092915 + "x": 1.398068831056723, + "y": 5.466087418740183, + "heading": -0.07173813692543105, + "angularVelocity": -0.49304384837450815, + "velocityX": 0.7433147299769343, + "velocityY": -0.8586389341691013, + "timestamp": 0.21813493871109982 }, { - "x": 6.078984969036473, - "y": 6.498549743391548, - "heading": 0.1231057748040032, - "angularVelocity": 1.9227414178241924e-7, - "velocityX": -3.5882852707972814, - "velocityY": 1.194736557475109, - "timestamp": 9.38380381902086 + "x": 1.4701339496615213, + "y": 5.382844295736326, + "heading": -0.11947590052159912, + "angularVelocity": -0.6565353154432494, + "velocityX": 0.9911083345055308, + "velocityY": -1.1448389262999967, + "timestamp": 0.2908465849481331 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": 0.12310578689683861, - "angularVelocity": 1.9432806685756468e-7, - "velocityX": -3.662936913327793, - "velocityY": 0.941318096340742, - "timestamp": 9.446032788948806 + "x": 1.5602181142824867, + "y": 5.2787919544216955, + "heading": -0.17905308310612672, + "angularVelocity": -0.819362312243441, + "velocityX": 1.2389234638197153, + "velocityY": -1.431027169709696, + "timestamp": 0.3635582311851664 }, { - "x": 5.606427257807288, - "y": 6.600564902613298, - "heading": 0.1231057972666647, - "angularVelocity": 1.578555424299444e-7, - "velocityX": -3.7237014740499577, - "velocityY": 0.6612371129043467, - "timestamp": 9.511724660841988 + "x": 1.668323429942649, + "y": 5.153931763984036, + "heading": -0.25042016061718175, + "angularVelocity": -0.9815082068514337, + "velocityX": 1.4867675434162453, + "velocityY": -1.717196582745939, + "timestamp": 0.4362698774221997 }, { - "x": 5.35931577124569, - "y": 6.62625930787543, - "heading": 0.12310580746861746, - "angularVelocity": 1.5530007712924032e-7, - "velocityX": -3.7616752185629396, - "velocityY": 0.39113522756534835, - "timestamp": 9.577416532735171 + "x": 1.7944527928954905, + "y": 5.008265979195592, + "heading": -0.33353344500312104, + "angularVelocity": -1.1430532616554276, + "velocityX": 1.7346514550472283, + "velocityY": -2.003334986008902, + "timestamp": 0.508981523659233 }, { - "x": 5.111994168115865, - "y": 6.649845615003531, - "heading": 0.1231058176602883, - "angularVelocity": 1.551435596748186e-7, - "velocityX": -3.764873735550995, - "velocityY": 0.35904452785959023, - "timestamp": 9.643108404628354 + "x": 1.938611553409707, + "y": 4.8417992681651185, + "heading": -0.42836015839962444, + "angularVelocity": -1.3041475239812899, + "velocityX": 1.98260894865471, + "velocityY": -2.2894091890060286, + "timestamp": 0.5816931698962663 }, { - "x": 4.868859374012009, - "y": 6.67294055859981, - "heading": 0.13716866748145323, - "angularVelocity": 0.21407290454489739, - "velocityX": -3.701139685883833, - "velocityY": 0.35156470550620644, - "timestamp": 9.708800276521536 + "x": 2.09250704020051, + "y": 4.700832043022275, + "heading": -0.5059805202146961, + "angularVelocity": -1.0675093444479318, + "velocityX": 2.116517707491784, + "velocityY": -1.9387159062918606, + "timestamp": 0.6544048161332996 }, { - "x": 4.644141674488456, - "y": 6.6943246178055915, - "heading": 0.16550217221046287, - "angularVelocity": 0.43130913935716303, - "velocityX": -3.4207839272559015, - "velocityY": 0.3255206251475534, - "timestamp": 9.77449214841472 + "x": 2.228385575629257, + "y": 4.5806756631537535, + "heading": -0.5718968876072178, + "angularVelocity": -0.906544835648882, + "velocityX": 1.8687313857908063, + "velocityY": -1.652505287385456, + "timestamp": 0.7271164623703329 }, { - "x": 4.438149971299598, - "y": 6.713938607045374, - "heading": 0.1971398735951766, - "angularVelocity": 0.48160754858923427, - "velocityX": -3.1357258859027066, - "velocityY": 0.2985755874284469, - "timestamp": 9.840184020307902 + "x": 2.346242849494598, + "y": 4.481326236786965, + "heading": -0.6261128487399352, + "angularVelocity": -0.7456296746751315, + "velocityX": 1.6208857861127495, + "velocityY": -1.3663481918284743, + "timestamp": 0.7998281086073662 }, { - "x": 4.2509016025753406, - "y": 6.731775563046276, - "heading": 0.22901708759854217, - "angularVelocity": 0.48525354940713283, - "velocityX": -2.8504039134206605, - "velocityY": 0.27152455070706877, - "timestamp": 9.905875892201085 + "x": 2.446077177803767, + "y": 4.402782122456306, + "heading": -0.6686283268670905, + "angularVelocity": -0.5847134582152551, + "velocityX": 1.3730170265673516, + "velocityY": -1.080213671266156, + "timestamp": 0.8725397548443995 }, { - "x": 4.082393919350406, - "y": 6.747832566507683, - "heading": 0.25967107343501483, - "angularVelocity": 0.46663285659323517, - "velocityX": -2.565122265033534, - "velocityY": 0.24442907468850525, - "timestamp": 9.971567764094267 + "x": 2.5278876403134056, + "y": 4.3450423723526415, + "heading": -0.6994420265312672, + "angularVelocity": -0.423779425385222, + "velocityX": 1.12513561096113, + "velocityY": -0.7940921858762265, + "timestamp": 0.9452514010814328 }, { - "x": 3.9326220842038664, - "y": 6.762107971713798, - "heading": 0.28824128245235414, - "angularVelocity": 0.43491239013245725, - "velocityX": -2.279914254690053, - "velocityY": 0.21730854662395976, - "timestamp": 10.03725963598745 + "x": 2.591673696422735, + "y": 4.308106404528037, + "heading": -0.718552253950641, + "angularVelocity": -0.2628220980484028, + "velocityX": 0.8772467604995492, + "velocityY": -0.5079787039876672, + "timestamp": 1.017963047318466 }, { - "x": 3.801581586235111, - "y": 6.77460070477556, - "heading": 0.314159509734349, - "angularVelocity": 0.3945423769342238, - "velocityX": -1.9947749119073828, - "velocityY": 0.19017167119358822, - "timestamp": 10.102951507880633 + "x": 2.637435055651834, + "y": 4.2919738867991235, + "heading": -0.7259572155509334, + "angularVelocity": -0.10184010376417504, + "velocityX": 0.6293539150825374, + "velocityY": -0.22186979060047113, + "timestamp": 1.0906746935554992 }, { - "x": 3.6892685916749244, - "y": 6.785310000165767, - "heading": 0.3370219943875935, - "angularVelocity": 0.34802607985383205, - "velocityX": -1.7096939289964497, - "velocityY": 0.16302314246152652, - "timestamp": 10.168643379773815 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05916659407406203, + "velocityX": 0.38145976628941564, + "velocityY": 0.06423731409121712, + "timestamp": 1.1633863397925324 }, { - "x": 3.595679898494151, - "y": 6.794235280663889, - "heading": 0.35652698961990326, - "angularVelocity": 0.2969164170572818, - "velocityX": -1.4246616892414212, - "velocityY": 0.135865826941808, - "timestamp": 10.234335251666998 + "x": 2.6747076115982096, + "y": 4.323065260999168, + "heading": -0.7050775877835139, + "angularVelocity": 0.2234613634683759, + "velocityX": 0.12854299211755085, + "velocityY": 0.35614342430781004, + "timestamp": 1.237571545918339 }, { - "x": 3.520812828526041, - "y": 6.801376095406927, - "heading": 0.37244068726433127, - "angularVelocity": 0.2422475899347831, - "velocityX": -1.1396702181031793, - "velocityY": 0.10870164812244498, - "timestamp": 10.30002712356018 + "x": 2.6654811031724366, + "y": 4.37114137531458, + "heading": -0.6763165724415564, + "angularVelocity": 0.38769205939503093, + "velocityX": -0.1243712717061632, + "velocityY": 0.6480552770879449, + "timestamp": 1.3117567520441455 }, { - "x": 3.464665127058438, - "y": 6.806732084957018, - "heading": 0.38457706026543664, - "angularVelocity": 0.18474695044220302, - "velocityX": -0.8547130695088268, - "velocityY": 0.08153199773025213, - "timestamp": 10.365718995453364 + "x": 2.6374924128586628, + "y": 4.440873637695316, + "heading": -0.6353766207501428, + "angularVelocity": 0.5518613996935385, + "velocityX": -0.3772812906679928, + "velocityY": 0.9399753133925751, + "timestamp": 1.3859419581699521 }, { - "x": 3.4272348818352074, - "y": 6.810302960282004, - "heading": 0.39278518447772715, - "angularVelocity": 0.12494885555456782, - "velocityX": -0.5697850303930024, - "velocityY": 0.05435794752192765, - "timestamp": 10.431410867346546 + "x": 2.590742000006679, + "y": 4.532262849422754, + "heading": -0.5822609370446827, + "angularVelocity": 0.7159875462663353, + "velocityX": -0.6301851176982644, + "velocityY": 1.231906150790299, + "timestamp": 1.4601271642957587 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0.0632601721112495, - "velocityX": -0.28488184560874424, - "velocityY": 0.027180367966521624, - "timestamp": 10.497102739239729 + "x": 2.525230488767617, + "y": 4.645309988591984, + "heading": -0.5169687243847326, + "angularVelocity": 0.8801244354174206, + "velocityX": -0.8830805313123746, + "velocityY": 1.5238501726898892, + "timestamp": 1.5343123704215653 }, { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": 1.9034366224257262e-34, - "velocityY": -2.547324229323533e-33, - "timestamp": 10.562794611132912 + "x": 2.4409586345992826, + "y": 4.780016081558229, + "heading": -0.4394900359726193, + "angularVelocity": 1.0443954051766633, + "velocityX": -1.1359657615273402, + "velocityY": 1.8158080296257462, + "timestamp": 1.608497576547372 }, { - "x": 3.4428034429775143, - "y": 6.811427919070607, - "heading": 0.39249813327278055, - "angularVelocity": -0.050100574262489476, - "velocityX": 0.38660845958378076, - "velocityY": -0.007449238878501296, - "timestamp": 10.651470846737375 + "x": 2.337926784653071, + "y": 4.936381539754754, + "heading": -0.3497973159677186, + "angularVelocity": 1.2090378215045976, + "velocityX": -1.3888463122287895, + "velocityY": 2.107771432581833, + "timestamp": 1.6826827826731785 }, { - "x": 3.5113694006298597, - "y": 6.810106800279428, - "heading": 0.3836131655700934, - "angularVelocity": -0.10019558951868686, - "velocityX": 0.7732168284429818, - "velocityY": -0.014898228168725063, - "timestamp": 10.740147082341839 + "x": 2.2066781223966365, + "y": 5.088037063238321, + "heading": -0.26257787440108193, + "angularVelocity": 1.1756985809634768, + "velocityX": -1.7692026362756765, + "velocityY": 2.044282565348452, + "timestamp": 1.756867988798985 }, { - "x": 3.614218332551768, - "y": 6.808125167746075, - "heading": 0.37028807906085554, - "angularVelocity": -0.1502667137188131, - "velocityX": 1.1598251912796755, - "velocityY": -0.022346827420505266, - "timestamp": 10.828823317946302 + "x": 2.094180452686652, + "y": 5.218026595083209, + "heading": -0.18768356705291486, + "angularVelocity": 1.009558526050363, + "velocityX": -1.5164434471606913, + "velocityY": 1.7522298398549971, + "timestamp": 1.8310531949247917 }, { - "x": 3.7513502451988723, - "y": 6.8054830693306245, - "heading": 0.3525264976102127, - "angularVelocity": -0.20029697166969943, - "velocityX": 1.5464336269164098, - "velocityY": -0.02979488695523812, - "timestamp": 10.917499553550766 + "x": 2.0004335399095625, + "y": 5.326350877049268, + "heading": -0.12517882612598913, + "angularVelocity": 0.8425499394477148, + "velocityX": -1.2636874340540831, + "velocityY": 1.460187113126797, + "timestamp": 1.9052384010505983 }, { - "x": 3.9227651512282486, - "y": 6.802180567172425, - "heading": 0.3303333796886753, - "angularVelocity": -0.2502713130553812, - "velocityX": 1.9330422052867193, - "velocityY": -0.03724224574585722, - "timestamp": 11.006175789155229 + "x": 1.9254366143080366, + "y": 5.4130101547737155, + "heading": -0.07512185913342963, + "angularVelocity": 0.6747567285776663, + "velocityX": -1.0109417971010437, + "velocityY": 1.1681476975361738, + "timestamp": 1.9794236071764049 }, { - "x": 4.128463068361194, - "y": 6.79821773882544, - "heading": 0.3037147939289921, - "angularVelocity": -0.3001772186001935, - "velocityX": 2.319650983499712, - "velocityY": -0.04468872996211646, - "timestamp": 11.094852024759692 + "x": 1.869189070291752, + "y": 5.478004602849777, + "heading": -0.037557912625022415, + "angularVelocity": 0.5063536043287745, + "velocityX": -0.7582043233686032, + "velocityY": 0.876110635424555, + "timestamp": 2.0536088133022115 }, { - "x": 4.368444017678072, - "y": 6.793594678448866, - "heading": 0.2726776385751228, - "angularVelocity": -0.3500053327963678, - "velocityX": 2.706259999435507, - "velocityY": -0.05213415234714548, - "timestamp": 11.183528260364156 + "x": 1.8316905948823166, + "y": 5.5213343174096465, + "heading": -0.012514751718529437, + "angularVelocity": 0.3375762124977515, + "velocityX": -0.5054710685139713, + "velocityY": 0.5840748691542834, + "timestamp": 2.127794019428018 }, { - "x": 4.642708020512335, - "y": 6.788311497970363, - "heading": 0.23722930806330966, - "angularVelocity": -0.3997500600942169, - "velocityX": 3.092869255948188, - "velocityY": -0.05957831252634942, - "timestamp": 11.27220449596862 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 2.368719711095728e-29, + "angularVelocity": 0.1686960564308085, + "velocityX": -0.2527377395078362, + "velocityY": 0.2920386866937519, + "timestamp": 2.2019792255538246 }, { - "x": 4.951255088173746, - "y": 6.782368328182758, - "heading": 0.19737735072702745, - "angularVelocity": -0.44940966499796203, - "velocityX": 3.479478640000833, - "velocityY": -0.0670209977576627, - "timestamp": 11.360880731573083 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.1956060311838367e-29, + "angularVelocity": -1.025164181333131e-28, + "velocityX": 1.21885672142624e-30, + "velocityY": -5.1343833235782786e-30, + "timestamp": 2.2761644316796312 }, { - "x": 5.286562692788269, - "y": 6.775920770205836, - "heading": 0.19737734601955828, - "angularVelocity": -5.3086028753514235e-8, - "velocityX": 3.781256639153567, - "velocityY": -0.07270897251075666, - "timestamp": 11.449556967177546 + "x": 1.8602093956334294, + "y": 5.5430660724686875, + "heading": -1.6999094202020734e-20, + "angularVelocity": -1.7890242544799916e-19, + "velocityX": 0.49746156397034486, + "velocityY": 0.0007030702226192673, + "timestamp": 2.3711832340882935 }, { - "x": 5.595111114675473, - "y": 6.769985794431735, - "heading": 0.15789301075905496, - "angularVelocity": -0.4452639987631135, - "velocityX": 3.4794939115759345, - "velocityY": -0.06692859404378032, - "timestamp": 11.53823320278201 + "x": 1.9547457978922331, + "y": 5.543199682247202, + "heading": -3.210902716840507e-20, + "angularVelocity": -1.5902045262092918e-19, + "velocityX": 0.9949231085045297, + "velocityY": 0.001406140417769105, + "timestamp": 2.4662020364969557 }, { - "x": 5.8693764275565705, - "y": 6.76471055259859, - "heading": 0.12280143262174693, - "angularVelocity": -0.3957269712466434, - "velocityX": 3.092884029318143, - "velocityY": -0.059488788593539996, - "timestamp": 11.626909438386473 + "x": 2.0965503966634373, + "y": 5.543400096908449, + "heading": -4.532979164623425e-20, + "angularVelocity": -1.3913840358532023e-19, + "velocityX": 1.492384614166397, + "velocityY": 0.0021092105579800877, + "timestamp": 2.561220838905618 }, { - "x": 6.109358599544783, - "y": 6.760094936608861, - "heading": 0.09209943623091729, - "angularVelocity": -0.3462257523850543, - "velocityX": 2.7062737874738274, - "velocityY": -0.05205020215694004, - "timestamp": 11.715585673990937 + "x": 2.2856231808662413, + "y": 5.543667316436768, + "heading": -7.554638691710094e-20, + "angularVelocity": -3.1800648403053284e-19, + "velocityX": 1.9898460032113345, + "velocityY": 0.0028122805333745363, + "timestamp": 2.65623964131428 }, { - "x": 6.315057615509001, - "y": 6.756138852616793, - "heading": 0.06578495581515914, - "angularVelocity": -0.29674782918315123, - "velocityX": 2.3196633749963063, - "velocityY": -0.04461267401690494, - "timestamp": 11.8042619095954 + "x": 2.474695965069045, + "y": 5.543934535965086, + "heading": -6.169486717931722e-20, + "angularVelocity": 1.457766188026957e-19, + "velocityX": 1.9898460032113345, + "velocityY": 0.002812280533374536, + "timestamp": 2.7512584437229424 }, { - "x": 6.4864734670806214, - "y": 6.752842221795132, - "heading": 0.043856783005957245, - "angularVelocity": -0.24728353272698234, - "velocityX": 1.9330528681462538, - "velocityY": -0.037176034810112814, - "timestamp": 11.892938145199864 + "x": 2.6165005638402494, + "y": 5.544134950626333, + "heading": -3.084743805748469e-20, + "angularVelocity": 3.2464552635862006e-19, + "velocityX": 1.492384614166397, + "velocityY": 0.0021092105579800877, + "timestamp": 2.8462772461316046 }, { - "x": 6.623606149810205, - "y": 6.750204980767773, - "heading": 0.026314311963007715, - "angularVelocity": -0.19782606831887856, - "velocityX": 1.5464423111199512, - "velocityY": -0.029740110294281013, - "timestamp": 11.981614380804327 + "x": 2.711036966099053, + "y": 5.544268560404848, + "heading": -1.8891361886639493e-21, + "angularVelocity": 3.047639112965043e-19, + "velocityX": 0.9949231085045297, + "velocityY": 0.001406140417769105, + "timestamp": 2.941296048540267 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -8.137968740525874e-34, + "angularVelocity": 1.9881709101649526e-20, + "velocityX": 0.49746156397034486, + "velocityY": 0.0007030702226192673, + "timestamp": 3.036314850948929 }, { - "x": 6.726455661694478, - "y": 6.74822708185799, - "heading": 0.013157327563715553, - "angularVelocity": -0.14837102984364775, - "velocityX": 1.1598317315027789, - "velocityY": -0.022304723427886322, - "timestamp": 12.07029061640879 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -8.137968740525874e-34, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -9.422453604168856e-36, + "timestamp": 3.1313336533575913 }, { - "x": 6.795022002255089, - "y": 6.7469084932218335, - "heading": 0.004385844550104781, - "angularVelocity": -0.09891582512292933, - "velocityX": 0.7732211464912397, - "velocityY": -0.014869695664985829, - "timestamp": 12.158966852013254 + "x": 2.744670053824415, + "y": 5.560420833392183, + "heading": 0.008590361313936763, + "angularVelocity": 0.13502733079351698, + "velocityX": -0.21432312628245512, + "velocityY": 0.2528389368381776, + "timestamp": 3.1949530796133994 }, { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": -4.086871260037018e-38, - "angularVelocity": -0.04945907457853377, - "velocityX": 0.3866105668308079, - "velocityY": -0.007434847721776456, - "timestamp": 12.247643087617718 + "x": 2.717653230778774, + "y": 5.592802243922747, + "heading": 0.025816975580253556, + "angularVelocity": 0.27077600789812356, + "velocityX": -0.4246631042695725, + "velocityY": 0.5089862080862002, + "timestamp": 3.2585725058692074 }, { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": -5.480350490542979e-38, - "angularVelocity": -1.571392014077605e-37, - "velocityX": 4.425248303324229e-37, - "velocityY": 1.8999126670467387e-37, - "timestamp": 12.336319323222181 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 2.6776153836811907, + "y": 5.641767759246714, + "heading": 0.051750317714318324, + "angularVelocity": 0.4076324427980389, + "velocityX": -0.629333671394567, + "velocityY": 0.7696629505440931, + "timestamp": 3.3221919321250155 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 2.6251092372323575, + "y": 5.707734732807458, + "heading": 0.0864931852991208, + "angularVelocity": 0.5461046983527398, + "velocityX": -0.8253162522672026, + "velocityY": 1.0368998502988076, + "timestamp": 3.3858113583808236 }, { - "scope": [ - 1 - ], - "type": "StopPoint" + "x": 2.5610825168108655, + "y": 5.791356317104455, + "heading": 0.1301885817245676, + "angularVelocity": 0.686824748304888, + "velocityX": -1.006402040849065, + "velocityY": 1.314403307580339, + "timestamp": 3.4494307846366317 }, { - "scope": [ - 5 - ], - "type": "StopPoint" + "x": 2.48749773492398, + "y": 5.89376509224382, + "heading": 0.18299253621322714, + "angularVelocity": 0.8299973388055953, + "velocityX": -1.1566401367878931, + "velocityY": 1.6097091905165586, + "timestamp": 3.5130502108924397 }, { - "scope": [ - 10 - ], - "type": "StopPoint" + "x": 2.410156329475762, + "y": 6.016926517894053, + "heading": 0.24447965431113647, + "angularVelocity": 0.9664833796311684, + "velocityX": -1.215688508997792, + "velocityY": 1.9359090909592849, + "timestamp": 3.576669637148248 }, { - "scope": [ - 2 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "CenterLanePCBFSprint": { - "waypoints": [ + "x": 2.3496693828620137, + "y": 6.152928215304285, + "heading": 0.3070071930810449, + "angularVelocity": 0.9828372000478389, + "velocityX": -0.9507622148388409, + "velocityY": 2.137738508728141, + "timestamp": 3.640289063404056 + }, { - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 2.309701308910896, + "y": 6.283843034176789, + "heading": 0.36362933631377437, + "angularVelocity": 0.8900134214517557, + "velocityX": -0.6282369443322133, + "velocityY": 2.0577805644789557, + "timestamp": 3.703908489659864 }, { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 2.288494901023514, + "y": 6.40507103844379, + "heading": 0.41290495871744193, + "angularVelocity": 0.7745373591634508, + "velocityX": -0.3333322718459105, + "velocityY": 1.905518666256975, + "timestamp": 3.767527915915672 }, { - "x": 1.8129411935806274, - "y": 5.542999267578125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "x": 2.285060268530139, + "y": 6.514915678699493, + "heading": 0.45431002431284384, + "angularVelocity": 0.6508242534114624, + "velocityX": -0.05398716548566081, + "velocityY": 1.7265896082436825, + "timestamp": 3.83114734217148 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 + "x": 2.2988127872129502, + "y": 6.612520892931977, + "heading": 0.48757772897770557, + "angularVelocity": 0.522917395248037, + "velocityX": 0.2161685430408261, + "velocityY": 1.5342045657567287, + "timestamp": 3.894766768427288 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.3925324714794108, + "velocityX": 0.48035043054352317, + "velocityY": 1.3337738607711032, + "timestamp": 3.9583861946830963 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 + "x": 2.3906193814869496, + "y": 6.780920533993851, + "heading": 0.5304788719493444, + "angularVelocity": 0.23275934723222522, + "velocityX": 0.7951498408867412, + "velocityY": 1.0846472026287595, + "timestamp": 4.035411897678633 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "x": 2.471491792404757, + "y": 6.8405967325057775, + "heading": 0.5351323155604377, + "angularVelocity": 0.06041416605263601, + "velocityX": 1.0499405753231936, + "velocityY": 0.7747569472411664, + "timestamp": 4.112437600674169 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 + "x": 2.556148280048404, + "y": 6.869576729136414, + "heading": 0.5277899567279947, + "angularVelocity": -0.09532349004160981, + "velocityX": 1.0990680299088356, + "velocityY": 0.37623800242778493, + "timestamp": 4.189463303669705 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.6173499244947225, + "y": 6.878199709364964, + "heading": 0.518346888239368, + "angularVelocity": -0.12259632981439711, + "velocityX": 0.794561322599876, + "velocityY": 0.11194938693450328, + "timestamp": 4.266489006665242 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.0752536934288536, + "velocityX": 0.4023928513995403, + "velocityY": 0.0187620669061233, + "timestamp": 4.343514709660778 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -4.4995568742972564e-34, + "velocityX": 2.8230892463303505e-33, + "velocityY": 2.0807279166194578e-33, + "timestamp": 4.420540412656314 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.658706958910465, + "y": 6.86593478207681, + "heading": 0.5043163629464208, + "angularVelocity": -0.14325761626715308, + "velocityX": 0.1802876546124509, + "velocityY": -0.23853061812890558, + "timestamp": 4.4780176824038715 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 15 + "x": 2.679434758469343, + "y": 6.838509950906704, + "heading": 0.48805940847309726, + "angularVelocity": -0.2828414527120879, + "velocityX": 0.36062602920974224, + "velocityY": -0.4771422040496659, + "timestamp": 4.535494952151429 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 2.3560143253921187e-30, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 + "x": 2.7105313297582017, + "y": 6.797364836772264, + "heading": 0.464037646160856, + "angularVelocity": -0.4179349926979131, + "velocityX": 0.5410238068968174, + "velocityY": -0.7158501841710775, + "timestamp": 4.592972221898986 }, { - "x": 1.3060574270397207, - "y": 5.572382522338357, - "heading": -0.010694879272561806, - "angularVelocity": -0.14212355715817127, - "velocityX": 0.21372562484439114, - "velocityY": -0.24679919880328743, - "timestamp": 0.07525057412377358 - }, - { - "x": 1.3382235764672503, - "y": 5.535239265365289, - "heading": -0.0320761034546414, - "angularVelocity": -0.2841337017193578, - "velocityX": 0.42745387396382534, - "velocityY": -0.49359433341007053, - "timestamp": 0.15050114824754715 - }, - { - "x": 1.3864734146735522, - "y": 5.47952484840506, - "heading": -0.06412157549775398, - "angularVelocity": -0.42585020003622126, - "velocityX": 0.6411889712155109, - "velocityY": -0.7403852742575773, - "timestamp": 0.22575172237132074 - }, - { - "x": 1.4508078083146234, - "y": 5.405239688642535, - "heading": -0.10679814197023921, - "angularVelocity": -0.5671261245497303, - "velocityX": 0.8549355854022526, - "velocityY": -0.9871706711626929, - "timestamp": 0.3010022964950943 - }, - { - "x": 1.5312280128772082, - "y": 5.3123844280164345, - "heading": -0.16006528510741636, - "angularVelocity": -0.7078636111159992, - "velocityX": 1.0686988836704838, - "velocityY": -1.2339475373861764, - "timestamp": 0.37625287061886786 - }, - { - "x": 1.6277357374217547, - "y": 5.200960102506698, - "heading": -0.22387989152223717, - "angularVelocity": -0.8480281666778853, - "velocityX": 1.2824848935314492, - "velocityY": -1.4807106365400295, - "timestamp": 0.4515034447426414 - }, - { - "x": 1.7403333706916029, - "y": 5.070968456330507, - "heading": -0.29820152963685287, - "angularVelocity": -0.9876554296244249, - "velocityX": 1.496302647246288, - "velocityY": -1.7274505569061047, - "timestamp": 0.526754018866415 - }, - { - "x": 1.8690253649035526, - "y": 4.922413220845796, - "heading": -0.38299660252732504, - "angularVelocity": -1.1268362252301867, - "velocityX": 1.7101795661770514, - "velocityY": -1.9741408915022984, - "timestamp": 0.6020045929901886 - }, - { - "x": 2.021841436297539, - "y": 4.7786139697645345, - "heading": -0.46243583568438273, - "angularVelocity": -1.055662818268618, - "velocityX": 2.0307628636084356, - "velocityY": -1.9109389231128335, - "timestamp": 0.6772551671139622 - }, - { - "x": 2.158572983780592, - "y": 4.6533861744648695, - "heading": -0.5314192950572162, - "angularVelocity": -0.9167167184470773, - "velocityX": 1.8170166683292757, - "velocityY": -1.6641440514810064, - "timestamp": 0.7525057412377357 - }, - { - "x": 2.2792163174605955, - "y": 4.5467264938443535, - "heading": -0.5899522434559231, - "angularVelocity": -0.777840555764763, - "velocityX": 1.6032214383174732, - "velocityY": -1.4173935795391805, - "timestamp": 0.8277563153615093 - }, - { - "x": 2.3837699374210346, - "y": 4.458633436191598, - "heading": -0.6380354100231267, - "angularVelocity": -0.6389740826105205, - "velocityX": 1.3894062760138433, - "velocityY": -1.1706629308520584, - "timestamp": 0.9030068894852828 - }, - { - "x": 2.472232988532522, - "y": 4.389106089275013, - "heading": -0.6756678010096412, - "angularVelocity": -0.5000944035873048, - "velocityX": 1.1755797499519767, - "velocityY": -0.9239444047507308, - "timestamp": 0.9782574636090564 - }, - { - "x": 2.5446049348789113, - "y": 4.338143851138861, - "heading": -0.7028477866363936, - "angularVelocity": -0.36119306653537947, - "velocityX": 0.9617461021356389, - "velocityY": -0.6772338779998112, - "timestamp": 1.05350803773283 - }, - { - "x": 2.6008854472033804, - "y": 4.305746331042048, - "heading": -0.7195735501934102, - "angularVelocity": -0.222267587342223, - "velocityX": 0.7479080788470353, - "velocityY": -0.43052854378169836, - "timestamp": 1.1287586118566038 - }, - { - "x": 2.6410743549129108, - "y": 4.291913303700176, - "heading": -0.7258432586618085, - "angularVelocity": -0.0833177492798597, - "velocityX": 0.5340677885590666, - "velocityY": -0.18382620335314903, - "timestamp": 1.2040091859803774 + "x": 2.7520007950829926, + "y": 6.742492873007072, + "heading": 0.4325704761896421, + "angularVelocity": -0.5474715502218408, + "velocityX": 0.721493305213118, + "velocityY": -0.954672412350015, + "timestamp": 4.650449491646543 }, { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.05565596827444861, - "velocityX": 0.3202270361150376, - "velocityY": 0.06287505455011895, - "timestamp": 1.279259760104151 - }, - { - "x": 2.6731093005797457, - "y": 4.320288364477002, - "heading": -0.7067987753568107, - "angularVelocity": 0.19580392065911026, - "velocityX": 0.10461717847139641, - "velocityY": 0.3116194636271141, - "timestamp": 1.3551333161029784 - }, - { - "x": 2.664687962821916, - "y": 4.362805446311989, - "heading": -0.6813130877895294, - "angularVelocity": 0.3358968382514228, - "velocityX": -0.11099173681211887, - "velocityY": 0.5603675915105346, - "timestamp": 1.4310068721018057 - }, - { - "x": 2.6399077578590466, - "y": 4.424196334908807, - "heading": -0.645202192832585, - "angularVelocity": 0.47593518560166614, - "velocityX": -0.32659870276641295, - "velocityY": 0.8091210144040187, - "timestamp": 1.506880428100633 - }, - { - "x": 2.5987689088828363, - "y": 4.504461553370171, - "heading": -0.5984695308975294, - "angularVelocity": 0.6159281889382251, - "velocityX": -0.5422027270940676, - "velocityY": 1.0578813316982019, - "timestamp": 1.5827539840994602 - }, - { - "x": 2.541271709317337, - "y": 4.60360172553535, - "heading": -0.5411163068190713, - "angularVelocity": 0.7559053127670017, - "velocityX": -0.7578028841452508, - "velocityY": 1.3066498710827958, - "timestamp": 1.6586275400982875 - }, - { - "x": 2.4674164806885255, - "y": 4.721617486978178, - "heading": -0.47313850163583826, - "angularVelocity": 0.8959354057929544, - "velocityX": -0.9733988035419044, - "velocityY": 1.5554267872170555, - "timestamp": 1.7345010960971148 - }, - { - "x": 2.377203340708699, - "y": 4.858509175719519, - "heading": -0.39452181655964974, - "angularVelocity": 1.0361539543083473, - "velocityX": -1.188993171512905, - "velocityY": 1.804208158404987, - "timestamp": 1.810374652095942 - }, - { - "x": 2.270630715712783, - "y": 5.014275370945325, - "heading": -0.30523452580590593, - "angularVelocity": 1.1767906430312367, - "velocityX": -1.4046082801148865, - "velocityY": 2.052970803512055, - "timestamp": 1.8862482080947693 - }, - { - "x": 2.156210238547871, - "y": 5.1464603718429265, - "heading": -0.22910381899713708, - "angularVelocity": 1.0033892020445663, - "velocityX": -1.508041578600413, - "velocityY": 1.7421748481347223, - "timestamp": 1.9621217640935966 - }, - { - "x": 2.0581337448378676, - "y": 5.259758714146455, - "heading": -0.16374376524716946, - "angularVelocity": 0.8614339065879271, - "velocityX": -1.2926307778416481, - "velocityY": 1.4932520403643688, - "timestamp": 2.0379953200924237 - }, - { - "x": 1.9764031908936424, - "y": 5.354172852013204, - "heading": -0.10920543384793971, - "angularVelocity": 0.7188055269306859, - "velocityX": -1.077194193248253, - "velocityY": 1.24436157795548, - "timestamp": 2.1138688760912507 - }, - { - "x": 1.9110187094739324, - "y": 5.42970361761196, - "heading": -0.0655341593932664, - "angularVelocity": 0.5755796453747686, - "velocityX": -0.8617558589167408, - "velocityY": 0.995482083384748, - "timestamp": 2.189742432090078 - }, - { - "x": 1.8619802018876241, - "y": 5.486351463195586, - "heading": -0.03276467101544991, - "angularVelocity": 0.4318960400201182, - "velocityX": -0.646318825315813, - "velocityY": 0.7466085494271616, - "timestamp": 2.265615988088905 - }, - { - "x": 1.8292876471115809, - "y": 5.524116647521472, - "heading": -0.010918058172762177, - "angularVelocity": 0.2879344793491674, - "velocityX": -0.43088206879563057, - "velocityY": 0.4977384258464482, - "timestamp": 2.341489544087732 + "x": 2.803848241216662, + "y": 6.673886273182379, + "heading": 0.39405966194845793, + "angularVelocity": -0.6700181551824853, + "velocityX": 0.9020513041309295, + "velocityY": -1.1936301102334148, + "timestamp": 4.7079267613941 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.8937989389363637e-30, - "angularVelocity": 0.14389806868982188, - "velocityX": -0.21544335593050407, - "velocityY": 0.24886958055725772, - "timestamp": 2.417363100086559 + "x": 2.866080023237396, + "y": 6.591535755483362, + "heading": 0.3490218715149645, + "angularVelocity": -0.7835756748937736, + "velocityX": 1.0827198698556684, + "velocityY": -1.4327492948204654, + "timestamp": 4.7654040311416574 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 1.8511737998863575e-30, - "angularVelocity": -5.603397953128688e-29, - "velocityX": 0, - "velocityY": 2.843834189498754e-31, - "timestamp": 2.493236656085386 + "x": 2.938704098816793, + "y": 6.495430149531881, + "heading": 0.29814315673038183, + "angularVelocity": -0.8851971398092591, + "velocityX": 1.2635268845991694, + "velocityY": -1.6720628236793682, + "timestamp": 4.822881300889215 }, { - "x": 1.855209395178807, - "y": 5.543066072468525, - "heading": -4.900273115413656e-27, - "angularVelocity": -4.980371579437602e-26, - "velocityX": 0.42942883009919236, - "velocityY": 0.0006787122433542538, - "timestamp": 2.591665542633167 + "x": 3.0217302964606527, + "y": 6.385555893825644, + "heading": 0.2423799369377359, + "angularVelocity": -0.9701786469253116, + "velocityX": 1.4445048974753851, + "velocityY": -1.9116122980233827, + "timestamp": 4.880358570636772 }, { - "x": 1.939745796780934, - "y": 5.543199682246804, - "heading": 4.528417643864408e-20, - "angularVelocity": 4.600700356082414e-19, - "velocityX": 0.8588576440015887, - "velocityY": 0.0013574244611094727, - "timestamp": 2.6900944291809483 + "x": 3.1151700095083186, + "y": 6.261896797084101, + "heading": 0.1831716018579807, + "angularVelocity": -1.0301173897055516, + "velocityX": 1.6256811337430896, + "velocityY": -2.151443471213232, + "timestamp": 4.937835840384329 }, { - "x": 2.066550395198543, - "y": 5.5434000969079245, - "heading": 6.339784356887693e-20, - "angularVelocity": 1.8402795932716144e-19, - "velocityX": 1.2882864255103954, - "velocityY": 0.0020361366276666244, - "timestamp": 2.7885233157287295 + "x": 3.2190322842248182, + "y": 6.124436959606287, + "heading": 0.12297302537717429, + "angularVelocity": -1.0473457898261622, + "velocityX": 1.8070147585761873, + "velocityY": -2.39155127029422, + "timestamp": 4.995313110131886 }, { - "x": 2.2356231808662415, - "y": 5.543667316436768, - "heading": 5.434100687876101e-20, - "angularVelocity": -9.20140114123854e-20, - "velocityX": 1.7177151098384495, - "velocityY": 0.002714848640629602, - "timestamp": 2.8869522022765106 + "x": 3.3332908123673026, + "y": 5.973192452697109, + "heading": 0.06701518728601086, + "angularVelocity": -0.9735646515036827, + "velocityX": 1.9878906678120518, + "velocityY": -2.6313794578874745, + "timestamp": 5.052790379879443 }, { - "x": 2.4046959665339394, - "y": 5.543934535965611, - "heading": 4.338082196797977e-27, - "angularVelocity": -5.5208388966484585e-19, - "velocityX": 1.7177151098384498, - "velocityY": 0.0027148486406296025, - "timestamp": 2.9853810888242918 + "x": 3.457219648102922, + "y": 5.80867115561654, + "heading": 0.033419490428721374, + "angularVelocity": -0.5845040483802287, + "velocityX": 2.1561364393249893, + "velocityY": -2.8623714696810656, + "timestamp": 5.1102676496270005 }, { - "x": 2.5315005649515485, - "y": 5.544134950626731, - "heading": -2.717050695881145e-20, - "angularVelocity": -2.760420466983953e-19, - "velocityX": 1.2882864255103954, - "velocityY": 0.0020361366276666244, - "timestamp": 3.083809975372073 + "x": 3.588371868131788, + "y": 5.635317112607293, + "heading": 0.03341946780631197, + "angularVelocity": -3.935887960456646e-7, + "velocityX": 2.2818101939234974, + "velocityY": -3.0160451909185437, + "timestamp": 5.167744919374558 }, { - "x": 2.6160369665536756, - "y": 5.54426856040501, - "heading": 3.0657579714860263e-27, - "angularVelocity": 2.760420337720658e-19, - "velocityX": 0.8588576440015887, - "velocityY": 0.0013574244611094727, - "timestamp": 3.182238861919854 + "x": 3.719524526285539, + "y": 5.461963401064903, + "heading": 0.033419445184933325, + "angularVelocity": -3.9357086276096175e-7, + "velocityX": 2.281817816499982, + "velocityY": -3.0160394239978356, + "timestamp": 5.225222189122115 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 1.737489718984482e-38, - "angularVelocity": -3.114693341043594e-26, - "velocityX": 0.42942883009919236, - "velocityY": 0.0006787122433542539, - "timestamp": 3.280667748467635 + "x": 3.8561567314585825, + "y": 5.292894866069507, + "heading": 0.0334194225694706, + "angularVelocity": -3.9346793646231113e-7, + "velocityX": 2.3771519728257604, + "velocityY": -2.9414851425259596, + "timestamp": 5.282699458869672 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 0, - "angularVelocity": -1.1323659927897137e-37, - "velocityX": 0, - "velocityY": 3.593395183653217e-38, - "timestamp": 3.3790966350154164 - }, - { - "x": 2.6753320649100156, - "y": 5.539342065750356, - "heading": 1.5795430701564016e-18, - "angularVelocity": 2.476798764658572e-17, - "velocityX": 0.2669898507573085, - "velocityY": -0.07829731508072835, - "timestamp": 3.442870208092684 - }, - { - "x": 2.7093452100309583, - "y": 5.5292188917484815, - "heading": 4.421175983497276e-18, - "angularVelocity": 4.455815749727496e-17, - "velocityX": 0.5333423153150382, - "velocityY": -0.15873618982597607, - "timestamp": 3.506643781169952 - }, - { - "x": 2.7602957743928864, - "y": 5.513806855363461, - "heading": 7.811064020414863e-18, - "angularVelocity": 5.3155058958518925e-17, - "velocityX": 0.7989291159865928, - "velocityY": -0.24166807097898507, - "timestamp": 3.5704173542472195 - }, - { - "x": 2.8281241133670565, - "y": 5.492918602334042, - "heading": 1.1328926559518176e-17, - "angularVelocity": 5.516176010462958e-17, - "velocityX": 1.0635806604718465, - "velocityY": -0.3275377561817081, - "timestamp": 3.6341909273244872 - }, - { - "x": 2.9127559105445995, - "y": 5.466330173559929, - "heading": 1.5973697872969206e-17, - "angularVelocity": 7.283222641176841e-17, - "velocityX": 1.3270668882705299, - "velocityY": -0.41691922674457305, - "timestamp": 3.697964500401755 - }, - { - "x": 3.0140963269470453, - "y": 5.433769288149837, - "heading": 1.634340357557127e-17, - "angularVelocity": 5.797161500644316e-18, - "velocityX": 1.5890659957167883, - "velocityY": -0.5105701913650192, - "timestamp": 3.7617380734790227 - }, - { - "x": 3.132020733935084, - "y": 5.394898138842126, - "heading": 1.753294263521122e-17, - "angularVelocity": 1.8652539010143133e-17, - "velocityX": 1.8491108667403897, - "velocityY": -0.6095181347392088, - "timestamp": 3.8255116465562904 - }, - { - "x": 3.2663592482856187, - "y": 5.34928716553499, - "heading": 1.3361291372353952e-17, - "angularVelocity": -6.54134786803134e-17, - "velocityX": 2.1064918879136747, - "velocityY": -0.7152017851010761, - "timestamp": 3.889285219633558 - }, - { - "x": 3.4168692069208895, - "y": 5.296373281622491, - "heading": 9.91837272236996e-18, - "angularVelocity": -5.398660422887962e-17, - "velocityX": 2.36006783645184, - "velocityY": -0.8297149016315647, - "timestamp": 3.953058792710826 - }, - { - "x": 3.5831820501397047, - "y": 5.235389893074956, - "heading": 8.197617273877545e-18, - "angularVelocity": -2.6982264995683312e-17, - "velocityX": 2.6078645933372964, - "velocityY": -0.9562485776615884, - "timestamp": 4.016832365788094 - }, - { - "x": 3.7646897201959026, - "y": 5.165243249909899, - "heading": 6.703480606914857e-18, - "angularVelocity": -2.342877456705142e-17, - "velocityX": 2.8461267151565104, - "velocityY": -1.0999327743494527, - "timestamp": 4.080605938865362 - }, - { - "x": 3.960268086544922, - "y": 5.08428626917176, - "heading": 4.5447273483145725e-18, - "angularVelocity": -3.385027926825969e-17, - "velocityX": 3.066761934634853, - "velocityY": -1.2694440162550131, - "timestamp": 4.144379511942629 + "x": 4.00579710900262, + "y": 5.135223415824853, + "heading": 0.03341939963642931, + "angularVelocity": -3.9899322628139097e-7, + "velocityX": 2.6034705232392743, + "velocityY": -2.743196587749476, + "timestamp": 5.340176728617229 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 2.916776997028848e-18, - "angularVelocity": -2.5527036870167268e-17, - "velocityX": 3.2494804486361053, - "velocityY": -1.4792721930373927, - "timestamp": 4.208153085019897 - }, - { - "x": 4.281965617918933, - "y": 4.933633775459018, - "heading": 2.5007263674842835e-18, - "angularVelocity": -1.1988032936813997e-17, - "velocityX": 3.298225514699012, - "velocityY": -1.6226254295188278, - "timestamp": 4.242858581053939 - }, - { - "x": 4.397154085355019, - "y": 4.872114684924303, - "heading": 2.80280710947709e-18, - "angularVelocity": 8.704118266930975e-18, - "velocityX": 3.31902668450838, - "velocityY": -1.772603695805773, - "timestamp": 4.277564077087981 - }, - { - "x": 4.510378349293925, - "y": 4.805721616464948, - "heading": 5.6088052918749416e-18, - "angularVelocity": 8.085169506424825e-17, - "velocityX": 3.2624303605355687, - "velocityY": -1.9130419111206762, - "timestamp": 4.312269573122023 - }, - { - "x": 4.620854325513756, - "y": 4.734849659733664, - "heading": 8.920577651651403e-18, - "angularVelocity": 9.542501154652829e-17, - "velocityX": 3.183241527839498, - "velocityY": -2.0420960605711302, - "timestamp": 4.346975069156065 - }, - { - "x": 4.728404969374526, - "y": 4.659612469130667, - "heading": 1.0355720889195964e-17, - "angularVelocity": 4.1352045109421545e-17, - "velocityX": 3.0989513521223016, - "velocityY": -2.167875385765943, - "timestamp": 4.381680565190107 - }, - { - "x": 4.836822070451743, - "y": 4.585629308888239, - "heading": 8.659234104719362e-18, - "angularVelocity": -4.888236672406423e-17, - "velocityX": 3.1239173464304306, - "velocityY": -2.131741905370206, - "timestamp": 4.4163860612241495 - }, - { - "x": 4.9481136166787945, - "y": 4.516045063115851, - "heading": 6.20990752321007e-18, - "angularVelocity": -7.057460233695506e-17, - "velocityX": 3.2067412642045734, - "velocityY": -2.0049921114550315, - "timestamp": 4.4510915572581915 - }, - { - "x": 5.062101259723399, - "y": 4.450971317120919, - "heading": 3.721439651007284e-18, - "angularVelocity": -7.170241479222451e-17, - "velocityX": 3.2844262745242743, - "velocityY": -1.8750271118759503, - "timestamp": 4.4857970532922335 - }, - { - "x": 5.178602303777092, - "y": 4.39051239286923, - "heading": 1.522860892330953e-18, - "angularVelocity": -6.334958464560708e-17, - "velocityX": 3.356847109732077, - "velocityY": -1.7420561916875086, - "timestamp": 4.520502549326276 - }, - { - "x": 5.2974300176780025, - "y": 4.334765207042833, - "heading": -2.608193001732958e-19, - "angularVelocity": -5.139474712462447e-17, - "velocityX": 3.42388749563912, - "velocityY": -1.606292725847098, - "timestamp": 4.555208045360318 - }, - { - "x": 5.4183939381282835, - "y": 4.283819120698816, - "heading": -2.822662772810701e-18, - "angularVelocity": -7.38166505421658e-17, - "velocityX": 3.485439894926986, - "velocityY": -1.4679544212261044, - "timestamp": 4.58991354139436 - }, - { - "x": 5.5413001762936585, - "y": 4.23775579768525, - "heading": -5.4175777354584686e-18, - "angularVelocity": -7.476956848858907e-17, - "velocityX": 3.5414056045998694, - "velocityY": -1.3272630642818801, - "timestamp": 4.624619037428402 + "heading": 0.03341937596859772, + "angularVelocity": -4.117772415638071e-7, + "velocityX": 2.813320067337111, + "velocityY": -2.527531676347718, + "timestamp": 5.397653998364786 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -7.665872207711091e-18, - "angularVelocity": -6.478208725348017e-17, - "velocityX": 3.591694883279384, - "velocityY": -1.1844441897757751, - "timestamp": 4.659324533462444 - }, - { - "x": 5.800468403278387, - "y": 4.158565232862714, - "heading": -9.714096811795654e-18, - "angularVelocity": -5.540829464294432e-17, - "velocityX": 3.6389268627432543, - "velocityY": -1.0302389285738007, - "timestamp": 4.6962905620558795 - }, - { - "x": 5.936486438230336, - "y": 4.1262510009162545, - "heading": -1.1401685441264777e-17, - "angularVelocity": -4.5652419090776246e-17, - "velocityX": 3.67954146353996, - "velocityY": -0.8741602269982254, - "timestamp": 4.733256590649315 - }, - { - "x": 6.0737584862384155, - "y": 4.099765139631346, - "heading": -1.2627280609090703e-17, - "angularVelocity": -3.3154634524185157e-17, - "velocityX": 3.713464854930551, - "velocityY": -0.7164919330720612, - "timestamp": 4.770222619242751 - }, - { - "x": 6.212034919724313, - "y": 4.079155810434451, - "heading": -1.3669283004490658e-17, - "angularVelocity": -2.818810770451562e-17, - "velocityX": 3.7406353548742786, - "velocityY": -0.5575207827587797, - "timestamp": 4.8071886478361865 - }, - { - "x": 6.351064277842407, - "y": 4.06446048840337, - "heading": -1.3375402013759826e-17, - "angularVelocity": 7.950028767305027e-18, - "velocityX": 3.761003370072177, - "velocityY": -0.3975358617152357, - "timestamp": 4.844154676429622 - }, - { - "x": 6.490086441164715, - "y": 4.055726900176193, - "heading": -1.4576849056839345e-17, - "angularVelocity": -3.2501382723403574e-17, - "velocityX": 3.7608087374308066, - "velocityY": -0.23625984612066567, - "timestamp": 4.881120705023058 - }, - { - "x": 6.625430002884342, - "y": 4.051684818750479, - "heading": -1.3372180037518883e-17, - "angularVelocity": 3.2588543188391143e-17, - "velocityX": 3.661295704988517, - "velocityY": -0.10934583939674931, - "timestamp": 4.9180867336164935 - }, - { - "x": 6.755501460439252, - "y": 4.050426142633095, - "heading": -1.1630354881514603e-17, - "angularVelocity": 4.7119618262525755e-17, - "velocityX": 3.518675457011608, - "velocityY": -0.03404953589216864, - "timestamp": 4.955052762209929 - }, - { - "x": 6.8798297740402194, - "y": 4.050767085715321, - "heading": -9.335447591441833e-18, - "angularVelocity": 6.208152126139743e-17, - "velocityX": 3.3633127044392923, - "velocityY": 0.009223146093816154, - "timestamp": 4.992018790803365 - }, - { - "x": 6.998265582982919, - "y": 4.052014068285868, - "heading": -6.4179877740703305e-18, - "angularVelocity": 7.892272793106084e-17, - "velocityX": 3.203909466318293, - "velocityY": 0.03373320364655125, - "timestamp": 5.0289848193968005 - }, - { - "x": 7.110758098746119, - "y": 4.053729886887297, - "heading": -3.1976006784105176e-18, - "angularVelocity": 8.71174756444278e-17, - "velocityX": 3.0431323039981164, - "velocityY": 0.046416092469654044, - "timestamp": 5.065950847990236 + "x": 4.277933740289957, + "y": 4.900652577427371, + "heading": 0.03341935253431191, + "angularVelocity": -6.240523322284166e-7, + "velocityX": 2.940862673655656, + "velocityY": -2.37792138300258, + "timestamp": 5.435205794264698 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -8.646914705534956e-42, - "angularVelocity": 8.650106057047145e-17, - "velocityX": 2.8819232262850263, - "velocityY": 0.051084720684648116, - "timestamp": 5.102916876583672 - }, - { - "x": 7.405338431031862, - "y": 4.059210715077462, - "heading": 6.035426461355953e-18, - "angularVelocity": 8.221730826644903e-17, - "velocityX": 2.561662296238626, - "velocityY": 0.04893769145541128, - "timestamp": 5.176325099117501 - }, - { - "x": 7.569876923635766, - "y": 4.062517669134614, - "heading": 1.1021958875984658e-17, - "angularVelocity": 6.792879928853616e-17, - "velocityX": 2.2414177448320305, - "velocityY": 0.04504882345608618, - "timestamp": 5.24973332165133 - }, - { - "x": 7.710907923869345, - "y": 4.065460118931574, - "heading": 1.4466516010520595e-17, - "angularVelocity": 4.692331479554e-17, - "velocityX": 1.9211880544932918, - "velocityY": 0.04008338160760226, - "timestamp": 5.32314154418516 - }, - { - "x": 7.828432324355203, - "y": 4.067984380811152, - "heading": 1.5902740281761073e-17, - "angularVelocity": 1.956489643348339e-17, - "velocityX": 1.600970523863292, - "velocityY": 0.03438663670702133, - "timestamp": 5.396549766718989 - }, - { - "x": 7.922450847470263, - "y": 4.070051609380546, - "heading": 1.5219645547347743e-17, - "angularVelocity": -9.30542534385069e-18, - "velocityX": 1.2807628337783643, - "velocityY": 0.02816072230112734, - "timestamp": 5.469957989252818 - }, - { - "x": 7.992964082884089, - "y": 4.071632392724107, - "heading": 1.2631622975679932e-17, - "angularVelocity": -3.5255213685022036e-17, - "velocityX": 0.9605631764388524, - "velocityY": 0.021534145481204726, - "timestamp": 5.543366211786648 - }, - { - "x": 8.039972518291798, - "y": 4.072703688242487, - "heading": 7.596971431802736e-18, - "angularVelocity": -6.858429982495519e-17, - "velocityX": 0.6403701627027665, - "velocityY": 0.014593671953933919, - "timestamp": 5.616774434320477 + "x": 4.3928594316694705, + "y": 4.81721673895127, + "heading": 0.033419330076064284, + "angularVelocity": -5.980605477839765e-7, + "velocityX": 3.060457925523134, + "velocityY": -2.2218867693700792, + "timestamp": 5.47275759016461 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 8.652482395157843e-45, - "angularVelocity": -1.0348937993018065e-16, - "velocityX": 0.32018271791516006, - "velocityY": 0.007400637290262982, - "timestamp": 5.690182656854306 + "x": 4.5119655190903245, + "y": 4.739865631807423, + "heading": 0.03341930829497473, + "angularVelocity": -5.800279064172658e-7, + "velocityX": 3.1717813906506853, + "velocityY": -2.0598510747665353, + "timestamp": 5.510309386064522 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 5.394624259127602e-43, - "velocityX": 0, - "velocityY": -7.823862347152674e-43, - "timestamp": 5.763590879388135 - }, - { - "x": 8.042595807906332, - "y": 4.071940125417329, - "heading": 2.4014031773112573e-19, - "angularVelocity": 3.467784621738616e-18, - "velocityX": -0.30153187250835495, - "velocityY": -0.01887149394694188, - "timestamp": 5.832839793010322 - }, - { - "x": 8.000826969409328, - "y": 4.069449378892138, - "heading": 7.295832460611546e-19, - "angularVelocity": 7.067878797353639e-18, - "velocityX": -0.6031695850838968, - "velocityY": -0.03596802310545437, - "timestamp": 5.902088706632509 - }, - { - "x": 7.938162262991824, - "y": 4.0659211372128246, - "heading": 1.4792287784714074e-18, - "angularVelocity": 1.0825376070160765e-17, - "velocityX": -0.9049197040027859, - "velocityY": -0.0509501376232871, - "timestamp": 5.971337620254696 - }, - { - "x": 7.8545936374958405, - "y": 4.061532754567933, - "heading": 2.6959287825754293e-18, - "angularVelocity": 1.756995078279752e-17, - "velocityX": -1.206786086954711, - "velocityY": -0.06337114064826968, - "timestamp": 6.0405865338768825 - }, - { - "x": 7.7501132248715185, - "y": 4.056503424652652, - "heading": 4.202275116563444e-18, - "angularVelocity": 2.17526348818473e-17, - "velocityX": -1.5087660897375712, - "velocityY": -0.07262684210066948, - "timestamp": 6.109835447499069 - }, - { - "x": 7.624714358960079, - "y": 4.051110859560637, - "heading": 6.0188244090330196e-18, - "angularVelocity": 2.6232170260178098e-17, - "velocityX": -1.8108423562512328, - "velocityY": -0.07787219769881376, - "timestamp": 6.179084361121256 - }, - { - "x": 7.478393887159083, - "y": 4.045718105040055, - "heading": 8.17237550228445e-18, - "angularVelocity": 3.1098698601987103e-17, - "velocityX": -2.1129641484240804, - "velocityY": -0.07787493317230812, - "timestamp": 6.248333274743443 - }, - { - "x": 7.311157646174121, - "y": 4.040819461213868, - "heading": 1.0532118174804612e-17, - "angularVelocity": 3.4076241042489266e-17, - "velocityX": -2.4150016547173685, - "velocityY": -0.07073964875337486, - "timestamp": 6.31758218836563 - }, - { - "x": 7.123034533164843, - "y": 4.037125830040512, - "heading": 8.283291482043014e-18, - "angularVelocity": -3.247454111743766e-17, - "velocityX": -2.716621866960299, - "velocityY": -0.053338471033761534, - "timestamp": 6.386831101987816 - }, - { - "x": 6.914117681129227, - "y": 4.035741699141579, - "heading": 5.798379315469736e-18, - "angularVelocity": -3.588377111777714e-17, - "velocityX": -3.016897177267489, - "velocityY": -0.019987763367446825, - "timestamp": 6.456080015610003 - }, - { - "x": 6.684712125750405, - "y": 4.03859150608828, - "heading": 3.611288298800787e-18, - "angularVelocity": -3.158303722425788e-17, - "velocityX": -3.312767571061532, - "velocityY": 0.041153092483867636, - "timestamp": 6.52532892923219 - }, - { - "x": 6.43606725460267, - "y": 4.049660896865006, - "heading": 2.3554139869933522e-18, - "angularVelocity": -1.8135653631468075e-17, - "velocityX": -3.5905959839934547, - "velocityY": 0.15984930589841134, - "timestamp": 6.594577842854377 - }, - { - "x": 6.175716481433539, - "y": 4.078070527288863, - "heading": 2.6764824042330446e-18, - "angularVelocity": 4.636439771335605e-18, - "velocityX": -3.759636932206204, - "velocityY": 0.41025380670743106, - "timestamp": 6.663826756476563 - }, - { - "x": 5.918464051979435, - "y": 4.12717103366876, - "heading": 2.4806366122903936e-18, - "angularVelocity": -2.8281424458318553e-18, - "velocityX": -3.7148948048144055, - "velocityY": 0.7090437064151481, - "timestamp": 6.73307567009875 + "x": 4.634929904279705, + "y": 4.6688078109694935, + "heading": 0.033419286924895195, + "angularVelocity": -5.690827567994812e-7, + "velocityX": 3.2745274158690463, + "velocityY": -1.8922615852334157, + "timestamp": 5.547861181964434 + }, + { + "x": 4.761195179582567, + "y": 4.6037960104372555, + "heading": 0.03341926571852252, + "angularVelocity": -5.647232617468814e-7, + "velocityX": 3.362429739429795, + "velocityY": -1.7312567608088656, + "timestamp": 5.585412977864346 + }, + { + "x": 4.8874652266060234, + "y": 4.538793478329322, + "heading": 0.03341924451230877, + "angularVelocity": -5.647190300189245e-7, + "velocityX": 3.3625568097996403, + "velocityY": -1.7310099437370914, + "timestamp": 5.622964773764258 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 3.0661477431883128e-18, - "angularVelocity": 8.455167023881293e-18, - "velocityX": -3.6464445426004524, - "velocityY": 1.0033087488526664, - "timestamp": 6.802324583720937 - }, - { - "x": 5.541093538436437, - "y": 4.236435061250764, - "heading": 3.354859743531915e-18, - "angularVelocity": 8.33229202553877e-18, - "velocityX": -3.6034349206984246, - "velocityY": 1.1482323536375627, - "timestamp": 6.83697435349121 - }, - { - "x": 5.417925084630127, - "y": 4.281179055760442, - "heading": 3.9245083052882345e-18, - "angularVelocity": 1.6440183168115343e-17, - "velocityX": -3.554668750266214, - "velocityY": 1.2913215529664346, - "timestamp": 6.871624123261483 - }, - { - "x": 5.296643156826778, - "y": 4.330809566180666, - "heading": 4.682534245858068e-18, - "angularVelocity": 2.1876795880478242e-17, - "velocityX": -3.500223193615562, - "velocityY": 1.4323474802075717, - "timestamp": 6.906273893031757 - }, - { - "x": 5.177441529342695, - "y": 4.385247292254255, - "heading": 5.748631283020555e-18, - "angularVelocity": 3.076779569476711e-17, - "velocityX": -3.4401852674457762, - "velocityY": 1.5710847845312972, - "timestamp": 6.94092366280203 - }, - { - "x": 5.060510651244131, - "y": 4.444405251423305, - "heading": 6.298107237796981e-18, - "angularVelocity": 1.5857997280196432e-17, - "velocityX": -3.374650939207116, - "velocityY": 1.7073117530438153, - "timestamp": 6.975573432572303 - }, - { - "x": 4.946037340921014, - "y": 4.5081889164365, - "heading": 6.498139126492573e-18, - "angularVelocity": 5.772964438776829e-18, - "velocityX": -3.303724991019314, - "velocityY": 1.840810644228753, - "timestamp": 7.010223202342576 - }, - { - "x": 4.834204484779198, - "y": 4.576496362942444, - "heading": 6.284318947548799e-18, - "angularVelocity": -6.170897537311036e-18, - "velocityX": -3.227520900810133, - "velocityY": 1.971367976145888, - "timestamp": 7.04487297211285 - }, - { - "x": 4.725190735681975, - "y": 4.649218420709879, - "heading": 4.218601510181254e-18, - "angularVelocity": -5.961706098087163e-17, - "velocityX": -3.146160849551947, - "velocityY": 2.098774631103723, - "timestamp": 7.079522741883123 - }, - { - "x": 4.619170166207779, - "y": 4.726238769851069, - "heading": 2.3861247317221027e-18, - "angularVelocity": -5.2885684107236655e-17, - "velocityX": -3.059777025276331, - "velocityY": 2.22282426843907, - "timestamp": 7.114172511653396 - }, - { - "x": 4.5101566662173465, - "y": 4.798961201296667, - "heading": 8.546946946648484e-19, - "angularVelocity": -4.419740873346574e-17, - "velocityX": -3.146153660275008, - "velocityY": 2.0987854155379675, - "timestamp": 7.148822281423669 - }, - { - "x": 4.398324060290128, - "y": 4.867269057737162, - "heading": -1.0977695137154222e-18, - "angularVelocity": -5.634854780637897e-17, - "velocityX": -3.2275136795616595, - "velocityY": 1.9713798069474653, - "timestamp": 7.1834720511939425 - }, - { - "x": 4.283850992630547, - "y": 4.931053147799285, - "heading": -2.453093242554037e-18, - "angularVelocity": -3.911494182571376e-17, - "velocityX": -3.3037179876961247, - "velocityY": 1.8408229112346155, - "timestamp": 7.218121820964216 + "x": 5.0137352745443575, + "y": 4.473790947998577, + "heading": 0.03341922330609504, + "angularVelocity": -5.647190293196981e-7, + "velocityX": 3.3625568341627456, + "velocityY": -1.731009896410775, + "timestamp": 5.6605165696641695 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -1.5610631193350687e-18, - "angularVelocity": 2.5744186155726097e-17, - "velocityX": -3.3579422894520032, - "velocityY": 1.6997125365941679, - "timestamp": 7.252771590734489 - }, - { - "x": 3.941206853693096, - "y": 5.086799964367495, - "heading": 3.827307300721309e-18, - "angularVelocity": 7.831135017939884e-17, - "velocityX": -3.2887955452772286, - "velocityY": 1.4075914407764616, - "timestamp": 7.321578607951182 - }, - { - "x": 3.730310577999757, - "y": 5.1698821724733195, - "heading": 7.528976478562542e-18, - "angularVelocity": 5.37978439929115e-17, - "velocityX": -3.0650402273530597, - "velocityY": 1.2074670791813904, - "timestamp": 7.390385625167876 - }, - { - "x": 3.5370931608840936, - "y": 5.242282386882321, - "heading": 5.4493611354862e-18, - "angularVelocity": -3.022388452804216e-17, - "velocityX": -2.808106279439004, - "velocityY": 1.0522213770870446, - "timestamp": 7.459192642384569 - }, - { - "x": 3.3623805585841375, - "y": 5.305505508408588, - "heading": 3.90044719086442e-18, - "angularVelocity": -2.2510988083436363e-17, - "velocityX": -2.5391683779829535, - "velocityY": 0.9188470025834666, - "timestamp": 7.527999659601263 - }, - { - "x": 3.2065789543659307, - "y": 5.3604207146722205, - "heading": 2.734702057316729e-18, - "angularVelocity": -1.6942241950067733e-17, - "velocityX": -2.2643272520816113, - "velocityY": 0.7981047353162919, - "timestamp": 7.596806676817956 - }, - { - "x": 3.0699254867036734, - "y": 5.407590197311926, - "heading": 1.8584817371846494e-18, - "angularVelocity": -1.2734461622898273e-17, - "velocityX": -1.9860396975485235, - "velocityY": 0.6855330247953545, - "timestamp": 7.66561369403465 - }, - { - "x": 2.952574245770292, - "y": 5.4474063504067525, - "heading": 1.2072006114656177e-18, - "angularVelocity": -9.465330021034912e-18, - "velocityX": -1.7055126886812788, - "velocityY": 0.5786641349302285, - "timestamp": 7.734420711251343 - }, - { - "x": 2.8546328911186802, - "y": 5.480158244239293, - "heading": 7.336690244970979e-19, - "angularVelocity": -6.882024626605053e-18, - "velocityX": -1.4234210203178261, - "velocityY": 0.4759964195133704, - "timestamp": 7.803227728468037 - }, - { - "x": 2.7761806703190945, - "y": 5.5060675412854945, - "heading": 4.019181330238403e-19, - "angularVelocity": -4.821468868916063e-18, - "velocityX": -1.1401776152062642, - "velocityY": 0.37655021383364773, - "timestamp": 7.87203474568473 - }, - { - "x": 2.7172782497291417, - "y": 5.525309545004062, - "heading": 1.8363150875881817e-19, - "angularVelocity": -3.1724471295939874e-18, - "velocityX": -0.856052521568438, - "velocityY": 0.2796517636852234, - "timestamp": 7.940841762901424 - }, - { - "x": 2.6779735090707537, - "y": 5.538026332588319, - "heading": 5.594227372773024e-20, - "angularVelocity": -1.8557589065219566e-18, - "velocityX": -0.5712315727130863, - "velocityY": 0.18481817841642284, - "timestamp": 8.009648780118116 + "x": 5.140005505333594, + "y": 4.408788772864344, + "heading": 0.03341920209988365, + "angularVelocity": -5.64718967252867e-7, + "velocityX": 3.362561703461214, + "velocityY": -1.7310004375685482, + "timestamp": 5.698068365564081 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -4.379352373324341e-45, - "angularVelocity": -8.130315190317274e-19, - "velocityX": -0.2858478933472287, - "velocityY": 0.09169170474607218, - "timestamp": 8.07845579733481 + "x": 5.267176463182282, + "y": 4.34556692987905, + "heading": 0.033419180889344834, + "angularVelocity": -5.648342058496427e-7, + "velocityX": 3.3865479613183083, + "velocityY": -1.6835903974819586, + "timestamp": 5.735620161463993 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -7.265203918119814e-45, - "angularVelocity": -4.19412391684243e-44, - "velocityX": 0, - "velocityY": -6.607621745560535e-42, - "timestamp": 8.147262814551503 - }, - { - "x": 2.6751665057839835, - "y": 5.538944468629318, - "heading": 4.149201789004472e-19, - "angularVelocity": 6.513823637389646e-18, - "velocityX": 0.2647058041795455, - "velocityY": -0.08463157955674916, - "timestamp": 8.210961219973287 - }, - { - "x": 2.7088624712986666, - "y": 5.5280798374619815, - "heading": 1.747454129766702e-18, - "angularVelocity": 2.0919423995667872e-17, - "velocityX": 0.5289922925316967, - "velocityY": -0.1705636286402431, - "timestamp": 8.27465962539507 - }, - { - "x": 2.7593613029745305, - "y": 5.511644735540107, - "heading": 3.0764256336987273e-18, - "angularVelocity": 2.086349721209079e-17, - "velocityX": 0.79278015425161, - "velocityY": -0.2580143382404696, - "timestamp": 8.338358030816854 - }, - { - "x": 2.8266246280660527, - "y": 5.489524723354374, - "heading": 4.563957990922327e-18, - "angularVelocity": 2.3352740894748934e-17, - "velocityX": 1.0559656030026554, - "velocityY": -0.3472616314217525, - "timestamp": 8.402056436238638 - }, - { - "x": 2.9106051995536757, - "y": 5.461582331644454, - "heading": 6.988237520643839e-18, - "angularVelocity": 3.8058716127490844e-17, - "velocityX": 1.31840932173323, - "velocityY": -0.43866705178720633, - "timestamp": 8.465754841660422 - }, - { - "x": 3.0112434875257597, - "y": 5.427649372766819, - "heading": 9.422486137058152e-18, - "angularVelocity": 3.821522062123431e-17, - "velocityX": 1.5799184815647955, - "velocityY": -0.5327128466237975, - "timestamp": 8.529453247082206 - }, - { - "x": 3.128462311204818, - "y": 5.387515427846556, - "heading": 1.2002270824368104e-17, - "angularVelocity": 4.0499988504071535e-17, - "velocityX": 1.8402159819054258, - "velocityY": -0.6300620031932208, - "timestamp": 8.59315165250399 - }, - { - "x": 3.2621579049769087, - "y": 5.340909820689257, - "heading": 1.4607724763792015e-17, - "angularVelocity": 4.0902969582545765e-17, - "velocityX": 2.0988844679362657, - "velocityY": -0.7316604999559294, - "timestamp": 8.656850057925773 - }, - { - "x": 3.4121839694430376, - "y": 5.287471786804592, - "heading": 1.2983801342724242e-17, - "angularVelocity": -2.5493941493744293e-17, - "velocityX": 2.3552562026116504, - "velocityY": -0.8389226312781332, - "timestamp": 8.720548463347557 - }, - { - "x": 3.5783204640757695, - "y": 5.226697519809262, - "heading": 1.1414351740343512e-17, - "angularVelocity": -2.4638758097451353e-17, - "velocityX": 2.608173525422576, - "velocityY": -0.9540940089929807, - "timestamp": 8.78424686876934 - }, - { - "x": 3.7602044322338806, - "y": 5.157837260218168, - "heading": 1.0662286375073935e-17, - "angularVelocity": -1.1806659213675993e-17, - "velocityX": 2.855392799140749, - "velocityY": -1.081035845954548, - "timestamp": 8.847945274191124 - }, - { - "x": 3.9571467965572817, - "y": 5.079670509622147, - "heading": 1.0033604735305209e-17, - "angularVelocity": -9.869660560666429e-18, - "velocityX": 3.091794261085985, - "velocityY": -1.2271382631705257, - "timestamp": 8.911643679612908 + "x": 5.39745989861742, + "y": 4.289036580908955, + "heading": 0.03341915955151481, + "angularVelocity": -5.682239561025287e-7, + "velocityX": 3.4694328809835504, + "velocityY": -1.5053966825119904, + "timestamp": 5.773171957363905 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 9.614004929327239e-18, - "angularVelocity": -6.58728900981987e-18, - "velocityX": 3.3023160854502263, - "velocityY": -1.4085550989874989, - "timestamp": 8.975342085034692 - }, - { - "x": 4.2842075611264425, - "y": 4.936417577245955, - "heading": 8.433110849470374e-18, - "angularVelocity": -3.4077840923717525e-17, - "velocityX": 3.3679341946759727, - "velocityY": -1.5447568972758872, - "timestamp": 9.009994928285538 - }, - { - "x": 4.401356584826297, - "y": 4.877666966713146, - "heading": 7.494016874909782e-18, - "angularVelocity": -2.7100055477775347e-17, - "velocityX": 3.3806468015288584, - "velocityY": -1.695405196841212, - "timestamp": 9.044647771536384 - }, - { - "x": 4.516063976001338, - "y": 4.814281195629901, - "heading": 5.449898069000543e-18, - "angularVelocity": -5.898848735476586e-17, - "velocityX": 3.310186998068025, - "velocityY": -1.8291650882556743, - "timestamp": 9.07930061478723 - }, - { - "x": 4.628146410125642, - "y": 4.746361612927903, - "heading": 3.873819251911276e-18, - "angularVelocity": -4.5481948066433736e-17, - "velocityX": 3.234436877602133, - "velocityY": -1.9600002865663548, - "timestamp": 9.113953458038077 - }, - { - "x": 4.737424855337868, - "y": 4.674016859733605, - "heading": 3.562150006954802e-18, - "angularVelocity": -8.994045386127452e-18, - "velocityX": 3.1535203163901007, - "velocityY": -2.0877003560892837, - "timestamp": 9.148606301288924 - }, - { - "x": 4.844483096398323, - "y": 4.598425359059355, - "heading": 4.67240858769656e-18, - "angularVelocity": 3.2039465642249735e-17, - "velocityX": 3.0894504178337554, - "velocityY": -2.181393893916723, - "timestamp": 9.18325914453977 - }, - { - "x": 4.954476829355994, - "y": 4.5271728465414585, - "heading": 6.725892368510384e-18, - "angularVelocity": 5.925873862496592e-17, - "velocityX": 3.174161847599147, - "velocityY": -2.0561808450207577, - "timestamp": 9.217911987790616 - }, - { - "x": 5.06723038345343, - "y": 4.460373312794278, - "heading": 8.84315284493419e-18, - "angularVelocity": 6.109918488065446e-17, - "velocityX": 3.2538038302147596, - "velocityY": -1.9276782936288406, - "timestamp": 9.252564831041463 - }, - { - "x": 5.18256360847748, - "y": 4.398133545147612, - "heading": 1.1174088851587773e-17, - "angularVelocity": 6.726536087617125e-17, - "velocityX": 3.32824709906689, - "velocityY": -1.796094109684512, - "timestamp": 9.28721767429231 - }, - { - "x": 5.300292214676822, - "y": 4.340553023175947, - "heading": 1.351328882085302e-17, - "angularVelocity": 6.750383950696769e-17, - "velocityX": 3.3973721967667427, - "velocityY": -1.6616391779124182, - "timestamp": 9.321870517543156 - }, - { - "x": 5.420228077668345, - "y": 4.287723772710641, - "heading": 1.5740193529975693e-17, - "angularVelocity": 6.4263260968298e-17, - "velocityX": 3.461068464810413, - "velocityY": -1.5245285959043573, - "timestamp": 9.356523360794002 - }, - { - "x": 5.542179542504676, - "y": 4.239730223009142, - "heading": 1.83851008704834e-17, - "angularVelocity": 7.632583916308507e-17, - "velocityX": 3.5192340193716003, - "velocityY": -1.3849815830141698, - "timestamp": 9.391176204044848 + "x": 5.530504784906373, + "y": 4.239352458442723, + "heading": 0.03341913783750815, + "angularVelocity": -5.782414966353427e-7, + "velocityX": 3.542969999186219, + "velocityY": -1.323082459189313, + "timestamp": 5.810723753263817 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 2.1337719863838943e-17, - "angularVelocity": 8.520567769813293e-17, - "velocityX": 3.571775782441806, - "velocityY": -1.2432211735943428, - "timestamp": 9.425829047295695 - }, - { - "x": 5.936838928321791, - "y": 4.127579524293423, - "heading": 2.7128761029914745e-17, - "angularVelocity": 7.834427123009397e-17, - "velocityX": 3.6647054686449687, - "velocityY": -0.9344094480089555, - "timestamp": 9.499746911041823 - }, - { - "x": 6.212625572925942, - "y": 4.081838891930782, - "heading": 3.048119806681363e-17, - "angularVelocity": 4.535354333849354e-17, - "velocityX": 3.730987756239054, - "velocityY": -0.6188034941017404, - "timestamp": 9.573664774787952 - }, - { - "x": 6.491306353239061, - "y": 4.059759739930169, - "heading": 2.990728340154358e-17, - "angularVelocity": -7.764221477519524e-18, - "velocityX": 3.770141156544387, - "velocityY": -0.2986984590956799, - "timestamp": 9.64758263853408 - }, - { - "x": 6.752875972710039, - "y": 4.054277278014817, - "heading": 2.289191300833474e-17, - "angularVelocity": -9.490764529266178e-17, - "velocityX": 3.538652312373937, - "velocityY": -0.07416964773468503, - "timestamp": 9.72150050228021 - }, - { - "x": 6.991092096287726, - "y": 4.05357587432536, - "heading": 1.9322766501808003e-17, - "angularVelocity": -4.8285303790501985e-17, - "velocityX": 3.2227138543376115, - "velocityY": -0.009488960501710962, - "timestamp": 9.795418366026338 - }, - { - "x": 7.205567187241114, - "y": 4.0550211409111485, - "heading": 1.4185957310010018e-17, - "angularVelocity": -6.949347466859182e-17, - "velocityX": 2.9015325942048964, - "velocityY": 0.01955233163599203, - "timestamp": 9.869336229772466 - }, - { - "x": 7.396227777452996, - "y": 4.057522153925314, - "heading": 7.96211441465592e-18, - "angularVelocity": -8.419944219072616e-17, - "velocityX": 2.579357418481523, - "velocityY": 0.03383502833300578, - "timestamp": 9.943254093518595 - }, - { - "x": 7.5630549716186115, - "y": 4.0604862007272615, - "heading": 9.124881669040973e-19, - "angularVelocity": -9.537107663127069e-17, - "velocityX": 2.2569266170703433, - "velocityY": 0.040099194588843756, - "timestamp": 10.017171957264724 - }, - { - "x": 7.70604444565388, - "y": 4.0635415850381555, - "heading": -6.759455196513469e-18, - "angularVelocity": -1.0379011208666613e-16, - "velocityX": 1.934437317160139, - "velocityY": 0.04133485677276264, - "timestamp": 10.091089821010852 - }, - { - "x": 7.825196583277639, - "y": 4.066433602906457, - "heading": -4.535875150648457e-18, - "angularVelocity": 3.0081768238080076e-17, - "velocityX": 1.6119532084015247, - "velocityY": 0.03912474903542123, - "timestamp": 10.165007684756981 - }, - { - "x": 7.920513375065405, - "y": 4.068976876297905, - "heading": -2.8949605954376388e-18, - "angularVelocity": 2.219916096123338e-17, - "velocityX": 1.2894960292025357, - "velocityY": 0.03440674909358656, - "timestamp": 10.23892554850311 - }, - { - "x": 7.991997299424134, - "y": 4.071030463180751, - "heading": -1.5603341535814708e-18, - "angularVelocity": 1.8055533185319825e-17, - "velocityX": 0.9670723792051263, - "velocityY": 0.02778201071799316, - "timestamp": 10.312843412249238 - }, - { - "x": 8.03965088919926, - "y": 4.07248360094205, - "heading": -5.821232156931401e-19, - "angularVelocity": 1.3233755526918469e-17, - "velocityX": 0.6446829948819025, - "velocityY": 0.019658817066051913, - "timestamp": 10.386761275995367 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 8.014500942600024e-44, - "angularVelocity": 7.875271094040935e-18, - "velocityX": 0.322326324020543, - "velocityY": 0.0103270696803873, - "timestamp": 10.460679139741496 + "heading": 0.033419115463141504, + "angularVelocity": -5.958268068735077e-7, + "velocityX": 3.6069365171092738, + "velocityY": -1.1371861948253605, + "timestamp": 5.848275549163729 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": -7.373298274013602e-44, - "angularVelocity": -1.7505506389116263e-42, - "velocityX": -2.259320082617454e-45, - "velocityY": 0, - "timestamp": 10.534597003487624 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 5.808220957669321, + "y": 4.160073254115033, + "heading": 0.03341909344161441, + "angularVelocity": -5.669632847998976e-7, + "velocityX": 3.6628444951256323, + "velocityY": -0.941676170142482, + "timestamp": 5.887116736398908 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 5.952250404538064, + "y": 4.131196918833519, + "heading": 0.033419072126499465, + "angularVelocity": -5.487760919877281e-7, + "velocityX": 3.708162832321777, + "velocityY": -0.7434462573626127, + "timestamp": 5.925957923634087 }, { - "scope": [ - 3 - ], - "type": "StopPoint" + "x": 6.09762364792411, + "y": 4.110103210641541, + "heading": 0.03341905123842176, + "angularVelocity": -5.377816485869475e-7, + "velocityX": 3.7427600373239653, + "velocityY": -0.5430757835556856, + "timestamp": 5.964799110869266 }, { - "scope": [ - 2 - ], - "type": "StopPoint" + "x": 6.24392007770515, + "y": 4.096848811559285, + "heading": 0.0334190305197359, + "angularVelocity": -5.334205087871439e-7, + "velocityX": 3.7665282704987906, + "velocityY": -0.3412459820551187, + "timestamp": 6.003640298104445 }, { - "scope": [ - 10 - ], - "type": "StopPoint" + "x": 6.390314919287249, + "y": 4.0847296192319185, + "heading": 0.03341900981240726, + "angularVelocity": -5.331281075500129e-7, + "velocityX": 3.7690619675370143, + "velocityY": -0.31201910111516795, + "timestamp": 6.042481485339624 }, { - "scope": [ - 7 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "CenterLanePBDA": { - "waypoints": [ - { - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 + "x": 6.536709800200056, + "y": 4.07261090201266, + "heading": 0.033418989105078956, + "angularVelocity": -5.331280988936517e-7, + "velocityX": 3.769062980140168, + "velocityY": -0.31200686904551156, + "timestamp": 6.081322672574803 }, { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 + "x": 6.68323027882942, + "y": 4.062119297351164, + "heading": 0.03341896797261103, + "angularVelocity": -5.440736864773728e-7, + "velocityX": 3.772296602114613, + "velocityY": -0.27011544724343456, + "timestamp": 6.120163859809982 }, { - "x": 4.897946357727051, - "y": 6.407341003417969, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 20 + "x": 6.828478445747191, + "y": 4.059283961769526, + "heading": 0.02725441210059553, + "angularVelocity": -0.15871182913873783, + "velocityX": 3.739539835337918, + "velocityY": -0.07299816981572388, + "timestamp": 6.159005047045161 }, { - "x": 8.008931159973145, - "y": 7.328619956970215, - "heading": 0.4756948301053548, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 21 + "x": 6.965940131033012, + "y": 4.0573223410737915, + "heading": 0.017080975304275284, + "angularVelocity": -0.2619239400361572, + "velocityX": 3.5390701229986914, + "velocityY": -0.05050362348240824, + "timestamp": 6.19784623428034 }, { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 7.095541107200079, + "y": 4.056112825219225, + "heading": 0.007563330070755733, + "angularVelocity": -0.2450400183673897, + "velocityX": 3.336689359734222, + "velocityY": -0.03114003305929124, + "timestamp": 6.236687421515519 }, { - "x": 1.8804243803024292, - "y": 6.046840667724609, + "x": 7.217291355133057, + "y": 4.0556182861328125, "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "angularVelocity": -0.194724482157576, + "velocityX": 3.1345655629883886, + "velocityY": -0.012732337027157752, + "timestamp": 6.275528608750698 }, { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ + "x": 7.3857970557158446, + "y": 4.056503208949951, + "heading": -0.005539516561442828, + "angularVelocity": -0.09285314353970942, + "velocityX": 2.8244854636552765, + "velocityY": 0.014833039029661928, + "timestamp": 6.335187510434235 + }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 2.2921355473422064e-38, - "velocityX": 1.1360383232068741e-38, - "velocityY": -2.144062321628808e-38, - "timestamp": 0 + "x": 7.5357095386456, + "y": 4.058265721996549, + "heading": -0.007640794437617486, + "angularVelocity": -0.03522153135371121, + "velocityX": 2.5128267316246156, + "velocityY": 0.029543169533142722, + "timestamp": 6.394846412117771 }, { - "x": 1.32825806778149, - "y": 5.589895062873393, - "heading": -2.8062774074031584e-19, - "angularVelocity": -2.995198561755265e-18, - "velocityX": 0.40860904707084805, - "velocityY": -0.011305499292899219, - "timestamp": 0.09369253321938388 + "x": 7.667000937583321, + "y": 4.060490426598279, + "heading": -0.007708640614146542, + "angularVelocity": -0.0011372347564986972, + "velocityX": 2.200700905192029, + "velocityY": 0.037290404934565556, + "timestamp": 6.454505313801308 }, { - "x": 1.4048253002214361, - "y": 5.587776581164742, - "heading": -1.768868342127426e-19, - "angularVelocity": 1.1072483896415275e-18, - "velocityX": 0.8172180835441991, - "velocityY": -0.022610998292584186, - "timestamp": 0.18738506643876776 + "x": 7.779661923967275, + "y": 4.062917456014214, + "heading": -0.0066168114481254744, + "angularVelocity": 0.018301194544491067, + "velocityX": 1.888418713800162, + "velocityY": 0.0406817649578728, + "timestamp": 6.514164215484844 }, { - "x": 1.519676146824621, - "y": 5.584598858658671, - "heading": -2.386873521248768e-19, - "angularVelocity": -6.596098514057519e-19, - "velocityX": 1.225827103364341, - "velocityY": -0.03391649683150392, - "timestamp": 0.28107759965815166 + "x": 7.873689685619936, + "y": 4.06536905162046, + "heading": -0.004961765552273349, + "angularVelocity": 0.027741809673791765, + "velocityX": 1.5760893848068933, + "velocityY": 0.04109354240631638, + "timestamp": 6.573823117168381 }, { - "x": 1.6728106047825377, - "y": 5.580361895432887, - "heading": -7.795008789281329e-19, - "angularVelocity": -5.772215866345043e-18, - "velocityX": 1.6344360932087083, - "velocityY": -0.04522199454104627, - "timestamp": 0.3747701328775355 + "x": 7.949083994059634, + "y": 4.067716008149933, + "heading": -0.003176764942269326, + "angularVelocity": 0.029920105124841948, + "velocityX": 1.263756226013528, + "velocityY": 0.03933958660390468, + "timestamp": 6.633482018851917 }, { - "x": 1.8642286675420066, - "y": 5.575065691668706, - "heading": -1.0638718286937245e-18, - "angularVelocity": -3.0351506143991226e-18, - "velocityX": 2.0430450131096105, - "velocityY": -0.056527490315375004, - "timestamp": 0.4684626660969194 + "x": 8.005845696502893, + "y": 4.069860191657625, + "heading": -0.0015908607397525072, + "angularVelocity": 0.02658285951909277, + "velocityX": 0.951437268227904, + "velocityY": 0.035940713744034375, + "timestamp": 6.693140920535454 }, { - "x": 2.0939303023371445, - "y": 5.5687102482727004, - "heading": -1.0113200205360793e-18, - "angularVelocity": 5.608964381023318e-19, - "velocityX": 2.451653583293393, - "velocityY": -0.0678329764136413, - "timestamp": 0.5621551993163033 + "x": 8.043976076014772, + "y": 4.071724543898811, + "heading": -0.00046248549538235704, + "angularVelocity": 0.018913778372181077, + "velocityX": 0.6391398171247351, + "velocityY": 0.03125019382816773, + "timestamp": 6.75279982221899 }, { - "x": 2.285348365096613, - "y": 5.563414044508519, - "heading": -7.732946400687193e-19, - "angularVelocity": 2.5404946615264457e-18, - "velocityX": 2.04304501310961, - "velocityY": -0.056527490315375004, - "timestamp": 0.6558477325356872 + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 3.4463826664070866e-33, + "angularVelocity": 0.00775216241552071, + "velocityX": 0.32686633402451676, + "velocityY": 0.02551860543538527, + "timestamp": 6.812458723902527 }, { - "x": 2.43848282305453, - "y": 5.559177081282735, - "heading": -4.312009437616493e-19, - "angularVelocity": 3.651237559230762e-18, - "velocityX": 1.634436093208708, - "velocityY": -0.04522199454104628, - "timestamp": 0.749540265755071 + "x": 8.059339450711764, + "y": 4.074523581107775, + "heading": -0.0006944740465081056, + "angularVelocity": -0.009481698382290156, + "velocityX": -0.0564842507032717, + "velocityY": 0.017429845647464255, + "timestamp": 6.88570235795497 }, { - "x": 2.553333669657715, - "y": 5.555999358776664, - "heading": -7.015602955490743e-20, - "angularVelocity": 3.853507870950435e-18, - "velocityX": 1.2258271033643409, - "velocityY": -0.033916496831503926, - "timestamp": 0.8432327989744549 + "x": 8.027124349390371, + "y": 4.075207755344852, + "heading": -0.002651211191279339, + "angularVelocity": -0.026715456845985846, + "velocityX": -0.43983482985464195, + "velocityY": 0.009341074428224473, + "timestamp": 6.958945992007413 }, { - "x": 2.6299009020976607, - "y": 5.553880877068013, - "heading": -2.7026882274997263e-20, - "angularVelocity": 4.603264080766157e-19, - "velocityX": 0.817218083544199, - "velocityY": -0.022610998292584186, - "timestamp": 0.9369253321938388 + "x": 7.96683125907587, + "y": 4.075299477238389, + "heading": -0.0058702138612145086, + "angularVelocity": -0.043949248444312826, + "velocityX": -0.8231854016327222, + "velocityY": 0.0012522848534625944, + "timestamp": 7.032189626059856 }, { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, - "angularVelocity": 2.884635663770645e-19, - "velocityX": 0.40860904707084805, - "velocityY": -0.011305499292899219, - "timestamp": 1.0306178654132228 + "x": 7.878460180490193, + "y": 4.074798744928973, + "heading": -0.0103514975430217, + "angularVelocity": -0.06118325148365151, + "velocityX": -1.2065359635542152, + "velocityY": -0.006836530107951673, + "timestamp": 7.105433260112299 }, { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.1410946614987667e-37, - "timestamp": 1.1243103986326066 + "x": 7.7620111146201785, + "y": 4.073705556029158, + "heading": -0.01609509527793809, + "angularVelocity": -0.07841770563710548, + "velocityX": -1.5898865120023296, + "velocityY": -0.01492537766534737, + "timestamp": 7.1786768941647425 }, { - "x": 2.687410064097537, - "y": 5.560258515634833, - "heading": 0.0028176822125938034, - "angularVelocity": 0.04098002734374148, - "velocityX": 0.2796139919160097, - "velocityY": 0.1081610698436357, - "timestamp": 1.1930678494014684 + "x": 7.617484062863899, + "y": 4.072019907599753, + "heading": -0.023101063438175296, + "angularVelocity": -0.0956529294439546, + "velocityX": -1.9732370413624691, + "velocityY": -0.02301426535169652, + "timestamp": 7.251920528217186 }, { - "x": 2.7258611531960137, - "y": 5.575132263104083, - "heading": 0.008453116109374614, - "angularVelocity": 0.08196106507388369, - "velocityX": 0.5592279624754555, - "velocityY": 0.2163219738796251, - "timestamp": 1.2618253001703301 + "x": 7.444879027316714, + "y": 4.06974179610174, + "heading": -0.031369488604002356, + "angularVelocity": -0.1128893353367305, + "velocityX": -2.356587542114559, + "velocityY": -0.031103201356480925, + "timestamp": 7.325164162269629 }, { - "x": 2.783537789303066, - "y": 5.597442863001631, - "heading": 0.016905769734324238, - "angularVelocity": 0.12293436610040698, - "velocityX": 0.8388419794814749, - "velocityY": 0.3244826509427688, - "timestamp": 1.3305827509391919 + "x": 7.244196011426359, + "y": 4.066871217282435, + "heading": -0.040900495023888005, + "angularVelocity": -0.13012743760176224, + "velocityX": -2.7399379957944494, + "velocityY": -0.0391921954234215, + "timestamp": 7.398407796322072 }, { - "x": 2.8604399802132514, - "y": 5.627190295278175, - "heading": 0.028174540960364546, - "angularVelocity": 0.1638916379247738, - "velocityX": 1.118456109850604, - "velocityY": 0.4326430364113507, - "timestamp": 1.3993402017080536 + "x": 7.015435021934675, + "y": 4.063408165830539, + "heading": -0.05169425070794916, + "angularVelocity": -0.14736783371961967, + "velocityX": -3.123288357427606, + "velocityY": -0.047281262005875575, + "timestamp": 7.471651430374515 }, { - "x": 2.9565677382168056, - "y": 5.664374535134068, - "heading": 0.042257801458542396, - "angularVelocity": 0.20482522753092394, - "velocityX": 1.3980704189674162, - "velocityY": 0.5408030611968617, - "timestamp": 1.4680976524769154 + "x": 6.758596078532163, + "y": 4.059352633648335, + "heading": -0.06375095974291681, + "angularVelocity": -0.1646110162466115, + "velocityX": -3.5066384502250503, + "velocityY": -0.05537043914696755, + "timestamp": 7.544895064426958 }, { - "x": 3.0719210799387406, - "y": 5.708995552649304, - "heading": 0.05915345386800317, - "angularVelocity": 0.2457283133759373, - "velocityX": 1.6776849698763825, - "velocityY": 0.648962650829455, - "timestamp": 1.5368551032457771 + "x": 6.481653456514826, + "y": 4.053513800761945, + "heading": -0.06375096147352038, + "angularVelocity": -2.3628040945559003e-8, + "velocityX": -3.7811152545904956, + "velocityY": -0.07971795722490971, + "timestamp": 7.6181386984794015 }, { - "x": 3.2065000261043313, - "y": 5.761053312357379, - "heading": 0.07885900262196403, - "angularVelocity": 0.2865951040012339, - "velocityX": 1.9572998222112317, - "velocityY": 0.757121724641519, - "timestamp": 1.6056125540146389 + "x": 6.205372409181029, + "y": 4.073516064315128, + "heading": -0.06375096323470678, + "angularVelocity": -2.4045589953178915e-8, + "velocityX": -3.7720827333058655, + "velocityY": 0.2730921780705308, + "timestamp": 7.691382332531845 }, { - "x": 3.360304601198153, - "y": 5.820547772766793, - "heading": 0.1013716386695389, - "angularVelocity": 0.32742104013214957, - "velocityX": 2.2369150306467707, - "velocityY": 0.8652801950062525, - "timestamp": 1.6743700047835006 + "x": 5.9325368870621995, + "y": 4.121391787593611, + "heading": -0.0637509651063581, + "angularVelocity": -2.555377459766275e-8, + "velocityX": -3.725040758128897, + "velocityY": 0.6536502987304462, + "timestamp": 7.764625966584288 }, { - "x": 3.5333348329448953, - "y": 5.8874788858214675, - "heading": 0.12668833767209217, - "angularVelocity": 0.36820299064984424, - "velocityX": 2.5165306423068015, - "velocityY": 0.973437966449241, - "timestamp": 1.7431274555523624 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.063750967187294, + "angularVelocity": -2.84111505129003e-8, + "velocityX": -3.639704142076304, + "velocityY": 1.0274925313911556, + "timestamp": 7.837869600636731 }, { - "x": 3.7255907514118967, - "y": 5.961846596243587, - "heading": 0.15480596913918515, - "angularVelocity": 0.4089394116954055, - "velocityX": 2.796146691262561, - "velocityY": 1.0815949339384094, - "timestamp": 1.8118849063212241 + "x": 5.417504232216337, + "y": 4.293821782165803, + "heading": -0.06375097411514968, + "angularVelocity": -9.821339780633191e-8, + "velocityX": -3.5221393138546646, + "velocityY": 1.3775780331429743, + "timestamp": 7.90840840517157 }, { - "x": 3.937072386955544, - "y": 6.04365084047756, - "heading": 0.18572140510730675, - "angularVelocity": 0.44963034002016317, - "velocityX": 3.075763181717921, - "velocityY": 1.1897509770827353, - "timestamp": 1.8806423570900859 + "x": 5.186014168311204, + "y": 4.410724788288498, + "heading": -0.06922328693791815, + "angularVelocity": -0.07757875766190163, + "velocityX": -3.281740673543775, + "velocityY": 1.6572864665569722, + "timestamp": 7.9789472097064085 }, { - "x": 4.167779761570079, - "y": 6.132891543122838, - "heading": 0.21943155266112502, - "angularVelocity": 0.4902762853605699, - "velocityX": 3.355379992054849, - "velocityY": 1.2979059236107295, - "timestamp": 1.9493998078589476 + "x": 4.979961148964822, + "y": 4.522014233902487, + "heading": -0.07551597396386121, + "angularVelocity": -0.08920886974821321, + "velocityX": -2.9211300177991313, + "velocityY": 1.5777052977843462, + "timestamp": 8.049486014241246 }, { - "x": 4.410308649457132, - "y": 6.2266961328710915, - "heading": 0.21943155790785696, - "angularVelocity": 7.630783127279499e-8, - "velocityX": 3.5273106430654275, - "velocityY": 1.3642825424635276, - "timestamp": 2.0181572586278094 + "x": 4.7980291404215, + "y": 4.623466282585381, + "heading": -0.08170492719079357, + "angularVelocity": -0.08773827778546625, + "velocityX": -2.579176238427287, + "velocityY": 1.4382445145180842, + "timestamp": 8.120024818776084 }, { - "x": 4.652837676050608, - "y": 6.320500363997887, - "heading": 0.21943156315423873, - "angularVelocity": 7.630273806741925e-8, - "velocityX": 3.5273126603947684, - "velocityY": 1.3642773267166644, - "timestamp": 2.0869147093966713 + "x": 4.639563192627529, + "y": 4.713607705102715, + "heading": -0.0874491132160183, + "angularVelocity": -0.08143299369906003, + "velocityX": -2.246507420120879, + "velocityY": 1.27789835838249, + "timestamp": 8.190563623310922 }, { - "x": 4.897946357727051, - "y": 6.407341003417969, - "heading": 0.21943156841217684, - "angularVelocity": 7.647081243446701e-8, - "velocityX": 3.5648308501199604, - "velocityY": 1.2629996960185401, - "timestamp": 2.1556721601655333 + "x": 4.504192007690659, + "y": 4.791698947450768, + "heading": -0.09257307344830198, + "angularVelocity": -0.07264030438385177, + "velocityX": -1.919102341322099, + "velocityY": 1.1070678453231335, + "timestamp": 8.26110242784576 }, { - "x": 5.1308682473529785, - "y": 6.471009903235158, - "heading": 0.21943157303192085, - "angularVelocity": 7.23563158909772e-8, - "velocityX": 3.6481177055177683, - "velocityY": 0.9972082962532426, - "timestamp": 2.2195193022886155 + "x": 4.391678654940531, + "y": 4.857296763925047, + "heading": -0.09697023729098278, + "angularVelocity": -0.0623368069770602, + "velocityX": -1.5950561324662005, + "velocityY": 0.9299536178257779, + "timestamp": 8.331641232380598 }, { - "x": 5.365529567427774, - "y": 6.527934417793533, - "heading": 0.21943157763716262, - "angularVelocity": 7.21291764013049e-8, - "velocityX": 3.6753613751798433, - "velocityY": 0.8915749815181846, - "timestamp": 2.2833664444116977 + "x": 4.301859289367306, + "y": 4.910106206970951, + "heading": -0.10056909340764136, + "angularVelocity": -0.051019522380495494, + "velocityX": -1.2733326878096392, + "velocityY": 0.7486580385668955, + "timestamp": 8.402180036915436 }, { - "x": 5.600190991292763, - "y": 6.584858504493345, - "heading": 0.21943158224240422, - "angularVelocity": 7.21291734995501e-8, - "velocityX": 3.6753630007842206, - "velocityY": 0.8915682802227483, - "timestamp": 2.34721358653478 + "x": 4.234614001747884, + "y": 4.949916980366573, + "heading": -0.10331834169263278, + "angularVelocity": -0.03897497700905296, + "velocityX": -0.9533091475375062, + "velocityY": 0.5643811751297677, + "timestamp": 8.472718841450273 }, { - "x": 5.8348524151632635, - "y": 6.641782591170481, - "heading": 0.2194315868476458, - "angularVelocity": 7.212917336838375e-8, - "velocityX": 3.6753630008705414, - "velocityY": 0.8915682798675781, - "timestamp": 2.411060728657862 + "x": 4.18985129168719, + "y": 4.976571613170809, + "heading": -0.10517936365869324, + "angularVelocity": -0.026382953019019773, + "velocityX": -0.6345827712261082, + "velocityY": 0.37787191007854753, + "timestamp": 8.543257645985111 }, { - "x": 6.069513839029734, - "y": 6.698706677864191, - "heading": 0.21943159145288735, - "angularVelocity": 7.212917280605725e-8, - "velocityX": 3.6753630008074327, - "velocityY": 0.891568280127171, - "timestamp": 2.474907870780944 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.013363372792080578, + "velocityX": -0.31687843925651377, + "velocityY": 0.18962871266842563, + "timestamp": 8.61379645051995 }, { - "x": 6.3041751870414915, - "y": 6.75563107725722, - "heading": 0.21943159605812906, - "angularVelocity": 7.212917561549481e-8, - "velocityX": 3.6753618127399528, - "velocityY": 0.8915731777514998, - "timestamp": 2.5387550129040264 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": 0, + "velocityX": 8.237561081237056e-34, + "velocityY": 0, + "timestamp": 8.684335255054787 }, { - "x": 6.537489271202827, - "y": 6.817847413489264, - "heading": 0.21943160514124108, - "angularVelocity": 1.4226340838423697e-7, - "velocityX": 3.6542604164106804, - "velocityY": 0.9744576524998471, - "timestamp": 2.6026021550271086 + "x": 4.18294196758914, + "y": 4.969364336221789, + "heading": -0.10045524830547124, + "angularVelocity": 0.08077125535022475, + "velocityX": 0.22011597884885625, + "velocityY": -0.29338710511594707, + "timestamp": 8.754493279341492 }, { - "x": 6.7545327794045615, - "y": 6.881509207420976, - "heading": 0.24890077922126197, - "angularVelocity": 0.46155823268064305, - "velocityX": 3.399424014677561, - "velocityY": 0.9970970009742466, - "timestamp": 2.6664492971501907 + "x": 4.2148538086473515, + "y": 4.929011600999615, + "heading": -0.08896968745487453, + "angularVelocity": 0.16370986736542367, + "velocityX": 0.4548566095276789, + "velocityY": -0.5751692074062589, + "timestamp": 8.824651303628197 }, { - "x": 6.954212307545749, - "y": 6.941327014961267, - "heading": 0.2778274290995393, - "angularVelocity": 0.45306099719431336, - "velocityX": 3.1274622716276532, - "velocityY": 0.9368909171372027, - "timestamp": 2.730296439273273 + "x": 4.264519080629496, + "y": 4.870038214957966, + "heading": -0.07148588735689271, + "angularVelocity": 0.24920599283886813, + "velocityX": 0.7079057953397331, + "velocityY": -0.8405793441481644, + "timestamp": 8.894809327914903 }, { - "x": 7.1365689728963595, - "y": 6.997132012930522, - "heading": 0.305376589845945, - "angularVelocity": 0.43148620017020184, - "velocityX": 2.856144524042602, - "velocityY": 0.8740406557535503, - "timestamp": 2.794143581396355 + "x": 4.333555645892384, + "y": 4.794138441539978, + "heading": -0.04780159185128592, + "angularVelocity": 0.3375849839901328, + "velocityX": 0.9840152422303832, + "velocityY": -1.081840234095224, + "timestamp": 8.964967352201608 + }, + { + "x": 4.423967613250648, + "y": 4.703932489290492, + "heading": -0.017716503350327845, + "angularVelocity": 0.4288189242332906, + "velocityX": 1.2886903284047762, + "velocityY": -1.2857538844146224, + "timestamp": 9.035125376488313 }, { - "x": 7.301617943292834, - "y": 7.048866027505986, - "heading": 0.33125825392103403, - "angularVelocity": 0.40536918669284566, - "velocityX": 2.585064341302851, - "velocityY": 0.8102792522134878, - "timestamp": 2.8579907235194373 + "x": 4.53803034325044, + "y": 4.603622363293204, + "heading": 0.0188848190363467, + "angularVelocity": 0.5216983054867799, + "velocityX": 1.6257973504736565, + "velocityY": -1.429774099500926, + "timestamp": 9.105283400775019 }, { - "x": 7.449367094221676, - "y": 7.096499619737108, - "heading": 0.35532461486989275, - "angularVelocity": 0.37693716818945566, - "velocityX": 2.3141075076472033, - "velocityY": 0.7460567638139751, - "timestamp": 2.9218378656425195 + "x": 4.677578748555051, + "y": 4.49984021599331, + "heading": 0.06181064388576721, + "angularVelocity": 0.6118448357952799, + "velocityX": 1.9890583682108496, + "velocityY": -1.4792626838489913, + "timestamp": 9.175441425061724 }, { - "x": 7.579821246213137, - "y": 7.140015020198173, - "heading": 0.3774857294479878, - "angularVelocity": 0.347096421878647, - "velocityX": 2.04322617510391, - "velocityY": 0.681555963416076, - "timestamp": 2.9856850077656016 + "x": 4.842258553230926, + "y": 4.40159118744757, + "heading": 0.11019595374155315, + "angularVelocity": 0.6896618077221814, + "velocityX": 2.3472697007957355, + "velocityY": -1.4003961705682948, + "timestamp": 9.245599449348429 }, { - "x": 7.69298365301254, - "y": 7.179400342359022, - "heading": 0.3976808814927073, - "angularVelocity": 0.3163047142468991, - "velocityX": 1.7723958040479335, - "velocityY": 0.6168689913312067, - "timestamp": 3.049532149888684 + "x": 5.028363428376419, + "y": 4.3176283558190685, + "heading": 0.16247927063581383, + "angularVelocity": 0.7452221955481739, + "velocityX": 2.6526527369836272, + "velocityY": -1.1967673332045712, + "timestamp": 9.315757473635134 }, { - "x": 7.788856658439436, - "y": 7.214647078189013, - "heading": 0.41586620005922853, - "angularVelocity": 0.2848258819708253, - "velocityX": 1.5016021428504343, - "velocityY": 0.5520487629977263, - "timestamp": 3.113379292011766 + "x": 5.230618576677741, + "y": 4.253738965485636, + "heading": 0.2169576127744663, + "angularVelocity": 0.7765090692409344, + "velocityX": 2.8828512541173206, + "velocityY": -0.9106497935623799, + "timestamp": 9.38591549792184 }, { - "x": 7.867442031134741, - "y": 7.2457488378850705, - "heading": 0.43200844627235874, - "angularVelocity": 0.2528264488645923, - "velocityX": 1.2308361828288252, - "velocityY": 0.48712845496042256, - "timestamp": 3.177226434134848 + "x": 5.444331250681613, + "y": 4.2129309832647035, + "heading": 0.2722364606827335, + "angularVelocity": 0.7879191079037022, + "velocityX": 3.0461615214607765, + "velocityY": -0.5816580873795356, + "timestamp": 9.456073522208545 }, { - "x": 7.928741153018892, - "y": 7.27270064652237, - "heading": 0.4460815614823235, - "angularVelocity": 0.22041887454940703, - "velocityX": 0.9600918670092122, - "velocityY": 0.4221302276198955, - "timestamp": 3.2410735762579304 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.3273042274639755, + "angularVelocity": 0.7849104552346565, + "velocityX": 3.1588757008538453, + "velocityY": -0.23207478938864265, + "timestamp": 9.52623154649525 }, { - "x": 7.9727551334716695, - "y": 7.29549852060884, - "heading": 0.45806460123681797, - "angularVelocity": 0.1876832596734667, - "velocityX": 0.6893649267484526, - "velocityY": 0.35706960920065395, - "timestamp": 3.3049207183810125 + "x": 5.81887557997068, + "y": 4.1972183908996925, + "heading": 0.36426543181084436, + "angularVelocity": 0.7759151841130933, + "velocityX": 3.210283328613804, + "velocityY": 0.011951482768166844, + "timestamp": 9.573867170985725 }, { - "x": 7.999484882530383, - "y": 7.314139197912392, - "heading": 0.4679404256802237, - "angularVelocity": 0.1546791933829663, - "velocityX": 0.4186522398635212, - "velocityY": 0.2919578963709263, - "timestamp": 3.3687678605040947 + "x": 5.973953798857342, + "y": 4.209469324927334, + "heading": 0.4004931510026103, + "angularVelocity": 0.7605173560600648, + "velocityX": 3.255509307276357, + "velocityY": 0.257180086514691, + "timestamp": 9.6215027954762 }, { - "x": 8.008931159973145, - "y": 7.328619956970215, - "heading": 0.4756948301053548, - "angularVelocity": 0.12145264717068183, - "velocityX": 0.1479514529335022, - "velocityY": 0.22680355887981873, - "timestamp": 3.432615002627177 + "x": 6.130819634438986, + "y": 4.233461866975936, + "heading": 0.4356102472173979, + "angularVelocity": 0.7372023898166727, + "velocityX": 3.2930361942250057, + "velocityY": 0.5036680489703445, + "timestamp": 9.669138419966675 }, { - "x": 7.9838028545378235, - "y": 7.340994975747599, - "heading": 0.4825391441548679, - "angularVelocity": 0.07100818067139564, - "velocityX": -0.26070037689812714, - "velocityY": 0.12838796741348574, - "timestamp": 3.5290026861982695 + "x": 6.289005438456121, + "y": 4.269254559307002, + "heading": 0.4691390733673604, + "angularVelocity": 0.7038603253047925, + "velocityX": 3.32074588523062, + "velocityY": 0.7513849710991581, + "timestamp": 9.71677404445715 }, { - "x": 7.919285560188609, - "y": 7.343883953710174, - "heading": 0.4845188810802152, - "angularVelocity": 0.02053931427751041, - "velocityX": -0.6693520578449088, - "velocityY": 0.029972480461632738, - "timestamp": 3.625390369769362 + "x": 6.447898093638549, + "y": 4.316894151635714, + "heading": 0.5004573197150548, + "angularVelocity": 0.6574543040567628, + "velocityX": 3.335584594135005, + "velocityY": 1.0000832956066072, + "timestamp": 9.764409668947625 }, { - "x": 7.815379289936097, - "y": 7.337286883355744, - "heading": 0.4816330205236378, - "angularVelocity": -0.029940138092932247, - "velocityX": -1.0780036038097405, - "velocityY": -0.06844308432376209, - "timestamp": 3.7217780533404548 + "x": 6.606667388474461, + "y": 4.376390356038531, + "heading": 0.5287266543286071, + "angularVelocity": 0.5934494386486953, + "velocityX": 3.33299492835787, + "velocityY": 1.248985502745193, + "timestamp": 9.8120452934381 }, { - "x": 7.6720840556149295, - "y": 7.321203739342901, - "heading": 0.47388185121605153, - "angularVelocity": -0.08041659494667612, - "velocityX": -1.4866550269929244, - "velocityY": -0.16685891202042144, - "timestamp": 3.8181657369115474 + "x": 6.764149093497097, + "y": 4.447653029340476, + "heading": 0.5527726659639904, + "angularVelocity": 0.5047905195447082, + "velocityX": 3.3059649518004255, + "velocityY": 1.4959953619626436, + "timestamp": 9.859680917928575 }, { - "x": 7.489399868443907, - "y": 7.295634478019173, - "heading": 0.46126687890088164, - "angularVelocity": -0.13087743005945188, - "velocityX": -1.895306333783606, - "velocityY": -0.2652751926014197, - "timestamp": 3.91455342048264 + "x": 6.918662738022944, + "y": 4.530331648811429, + "heading": 0.5708777234010636, + "angularVelocity": 0.3800738970199359, + "velocityX": 3.243657371527556, + "velocityY": 1.735646805417357, + "timestamp": 9.90731654241905 }, { - "x": 7.267326740077932, - "y": 7.2605790366684495, - "heading": 0.44379066346613116, - "angularVelocity": -0.1813117069242423, - "velocityX": -2.3039575196573705, - "velocityY": -0.36369212384761535, - "timestamp": 4.010941104053733 + "x": 7.06846751913844, + "y": 4.623866769358016, + "heading": 0.5845734852773292, + "angularVelocity": 0.2875109127414552, + "velocityX": 3.144805651607445, + "velocityY": 1.9635539902555645, + "timestamp": 9.954952166909525 }, { - "x": 7.00586468444221, - "y": 7.216037332217007, - "heading": 0.4214565844390286, - "angularVelocity": -0.23171092197300747, - "velocityX": -2.7126085610603474, - "velocityY": -0.4621099169645314, - "timestamp": 4.107328787624826 + "x": 7.211529043645413, + "y": 4.727181705822193, + "heading": 0.5988053175713824, + "angularVelocity": 0.2987644739893997, + "velocityX": 3.0032465415789793, + "velocityY": 2.168858655035301, + "timestamp": 10.0025877914 }, { - "x": 6.7050137215236445, - "y": 7.162009258221821, - "heading": 0.3942685380548319, - "angularVelocity": -0.2820697144790415, - "velocityX": -3.1212593951037992, - "velocityY": -0.5605288144032944, - "timestamp": 4.203716471195919 + "x": 7.344594975677938, + "y": 4.83690117801315, + "heading": 0.6149173466892092, + "angularVelocity": 0.3382348670803817, + "velocityX": 2.7934121459693166, + "velocityY": 2.303307101870708, + "timestamp": 10.050223415890475 }, { - "x": 6.364773891396992, - "y": 7.098494671595923, - "heading": 0.3622306012435557, - "angularVelocity": -0.3323862097758952, - "velocityX": -3.529909813380894, - "velocityY": -0.658949196336373, - "timestamp": 4.300104154767012 + "x": 7.465817541114002, + "y": 4.947544737637474, + "heading": 0.6314993734499258, + "angularVelocity": 0.34810138290582165, + "velocityX": 2.5447879970651583, + "velocityY": 2.322706184873219, + "timestamp": 10.09785904038095 }, { - "x": 6.010374312951181, - "y": 7.013136980735367, - "heading": 0.36223059672201996, - "angularVelocity": -4.690989126354215e-8, - "velocityX": -3.6768139384158545, - "velocityY": -0.8855663680059631, - "timestamp": 4.396491838338105 + "x": 7.575645879536293, + "y": 5.054829291458012, + "heading": 0.6474951019450449, + "angularVelocity": 0.33579340391176205, + "velocityX": 2.305592497149964, + "velocityY": 2.252191610125547, + "timestamp": 10.145494664871425 }, { - "x": 5.655974859532265, - "y": 6.927778770772817, - "heading": 0.36223059220056536, - "angularVelocity": -4.690904952415849e-8, - "velocityX": -3.676812641290635, - "velocityY": -0.885571753569461, - "timestamp": 4.492879521909198 + "x": 7.675112095346534, + "y": 5.156311134408354, + "heading": 0.6623068665871409, + "angularVelocity": 0.31093881523601674, + "velocityX": 2.0880636472002183, + "velocityY": 2.1303770872287977, + "timestamp": 10.1931302893619 }, { - "x": 5.301574928644714, - "y": 6.842422689744232, - "heading": 0.3622305506125328, - "angularVelocity": -4.314662515297724e-7, - "velocityX": -3.6768175949176816, - "velocityY": -0.8855496663702899, - "timestamp": 4.589267205480291 + "x": 7.76511711692712, + "y": 5.250618542879285, + "heading": 0.6755917653702976, + "angularVelocity": 0.27888579031462285, + "velocityX": 1.8894477094255981, + "velocityY": 1.979766392897571, + "timestamp": 10.240765913852375 }, { - "x": 4.986369114322999, - "y": 6.766504438890574, - "heading": 0.34532942408176365, - "angularVelocity": -0.1753452921016411, - "velocityX": -3.270187669664538, - "velocityY": -0.7876343536948325, - "timestamp": 4.685654889051384 + "x": 7.846351174360072, + "y": 5.336925095447112, + "heading": 0.6871379054751507, + "angularVelocity": 0.2423845646688635, + "velocityX": 1.7053215592711606, + "velocityY": 1.8118068880378715, + "timestamp": 10.28840153834285 }, { - "x": 4.710566832932512, - "y": 6.70007668327783, - "heading": 0.3264669766789321, - "angularVelocity": -0.1956935440711402, - "velocityX": -2.861385097890258, - "velocityY": -0.6891726530988762, - "timestamp": 4.782042572622477 + "x": 7.919337423332426, + "y": 5.414694341101313, + "heading": 0.6968047845571967, + "angularVelocity": 0.2029338165594727, + "velocityX": 1.5321778553978798, + "velocityY": 1.6325858322640792, + "timestamp": 10.336037162833325 }, { - "x": 4.474170501345758, - "y": 6.643140019477641, - "heading": 0.30840376738356945, - "angularVelocity": -0.18740163292794487, - "velocityX": -2.4525574516208333, - "velocityY": -0.5907047632096585, - "timestamp": 4.87843025619357 + "x": 7.984478950500488, + "y": 5.483555793762207, + "heading": 0.7044936776046891, + "angularVelocity": 0.16141056467161, + "velocityX": 1.3674960256076552, + "velocityY": 1.445587276276031, + "timestamp": 10.3836727873238 }, { - "x": 4.277177710352041, - "y": 6.5956938222588235, - "heading": 0.2923060351860776, - "angularVelocity": -0.16701026107365813, - "velocityX": -2.0437548003570405, - "velocityY": -0.4922433599498598, - "timestamp": 4.974817939764663 + "x": 8.05911422318913, + "y": 5.560200385763573, + "heading": 0.7111561296070518, + "angularVelocity": 0.10289890039880417, + "velocityX": 1.1527118676281318, + "velocityY": 1.1837449989371336, + "timestamp": 10.44842034027666 }, { - "x": 4.1195862527073634, - "y": 6.557737539015946, - "heading": 0.27881764120910085, - "angularVelocity": -0.1399389784798502, - "velocityX": -1.6349750487409798, - "velocityY": -0.3937876898440308, - "timestamp": 5.071205623335756 + "x": 8.119756898561732, + "y": 5.61995838055026, + "heading": 0.7143078455831314, + "angularVelocity": 0.04867698982190534, + "velocityX": 0.9366018112955389, + "velocityY": 0.9229382742880431, + "timestamp": 10.513167893229522 }, { - "x": 4.0013944038572795, - "y": 6.529270748726535, - "heading": 0.26834692670698207, - "angularVelocity": -0.10863124949357753, - "velocityX": -1.226213188979785, - "velocityY": -0.2953363877493204, - "timestamp": 5.167593306906849 + "x": 8.166344572702991, + "y": 5.662879129934428, + "heading": 0.7141510891264902, + "angularVelocity": -0.002421040633850211, + "velocityX": 0.7195279515069205, + "velocityY": 0.6628937685941038, + "timestamp": 10.577915446182383 }, { - "x": 3.9226008250519366, - "y": 6.510293128915594, - "heading": 0.26117603215990287, - "angularVelocity": -0.07439637805789222, - "velocityX": -0.817465218439728, - "velocityY": -0.19688843125838204, - "timestamp": 5.263980990477942 + "x": 8.198829842984775, + "y": 5.689000472186179, + "heading": 0.7108395294312745, + "angularVelocity": -0.0511457119873956, + "velocityX": 0.5017219771291601, + "velocityY": 0.4034336598136588, + "timestamp": 10.642662999135243 }, { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "angularVelocity": -0.038017450198398375, - "velocityX": -0.40872820518438957, - "velocityY": -0.09844312341738799, - "timestamp": 5.3603686740490355 + "x": 8.217175483703613, + "y": 5.698352336883545, + "heading": 0.7044936776046891, + "angularVelocity": -0.09800913759946452, + "velocityX": 0.28334106668393205, + "velocityY": 0.1444358013680284, + "timestamp": 10.707410552088104 }, { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.0638409913121086e-32, - "timestamp": 5.456756357620129 + "x": 8.221318658765867, + "y": 5.6908798741594095, + "heading": 0.6951703689604403, + "angularVelocity": -0.14352542875947455, + "velocityX": 0.06378111032529457, + "velocityY": -0.11503302714668015, + "timestamp": 10.77236983299625 }, { - "x": 3.856532695000848, - "y": 6.488812456981272, - "heading": 0.25799867286334044, - "angularVelocity": 0.005949010116156059, - "velocityX": -0.32577575495196986, - "velocityY": -0.14647295299154703, - "timestamp": 5.53862790495835 + "x": 8.210964366460544, + "y": 5.666753242665305, + "heading": 0.6829793287804198, + "angularVelocity": -0.18767203099521718, + "velocityX": -0.15939665834608732, + "velocityY": -0.3714116159663335, + "timestamp": 10.837329113904394 }, { - "x": 3.8031475649180457, - "y": 6.464921556304023, - "heading": 0.25900179360543873, - "angularVelocity": 0.01225237307357005, - "velocityX": -0.6520596204474033, - "velocityY": -0.29180956576467965, - "timestamp": 5.620499452296571 + "x": 8.185835060378174, + "y": 5.6262170290983935, + "heading": 0.6680275277365855, + "angularVelocity": -0.2301718989927961, + "velocityX": -0.3868470483517696, + "velocityY": -0.6240249738021217, + "timestamp": 10.902288394812539 }, { - "x": 3.7229939182765985, - "y": 6.429256431279799, - "heading": 0.2605597691266567, - "angularVelocity": 0.019029511128961094, - "velocityX": -0.9790171219107733, - "velocityY": -0.43562295063126366, - "timestamp": 5.702370999634793 + "x": 8.14559839699319, + "y": 5.569575312921383, + "heading": 0.6504460796726778, + "angularVelocity": -0.2706533665107588, + "velocityX": -0.6194136206938888, + "velocityY": -0.8719572536078877, + "timestamp": 10.967247675720683 }, { - "x": 3.6159951636151835, - "y": 6.38199294529669, - "heading": 0.26272711898900253, - "angularVelocity": 0.02647256504622092, - "velocityX": -1.3069101310543174, - "velocityY": -0.5772882951369854, - "timestamp": 5.784242546973014 + "x": 8.08985004858283, + "y": 5.497215477528086, + "heading": 0.6303993620580922, + "angularVelocity": -0.30860436467780866, + "velocityX": -0.858204518753663, + "velocityY": -1.11392605308573, + "timestamp": 11.032206956628828 }, { - "x": 3.4820378233980955, - "y": 6.3233976602765205, - "heading": 0.2655861022623468, - "angularVelocity": 0.03492035226271501, - "velocityX": -1.6361891838161302, - "velocityY": -0.7156977842143984, - "timestamp": 5.866114094311236 + "x": 8.018089118716903, + "y": 5.40964606854142, + "heading": 0.6080990792438148, + "angularVelocity": -0.3432963312172538, + "velocityX": -1.1047063462324833, + "velocityY": -1.348066169489957, + "timestamp": 11.097166237536973 }, { - "x": 3.3209367433633594, - "y": 6.253922161955361, - "heading": 0.26927508652336807, - "angularVelocity": 0.04505819641812337, - "velocityX": -1.9677297580465591, - "velocityY": -0.8485914897167804, - "timestamp": 5.947985641649457 + "x": 7.9296824410081745, + "y": 5.307560214485479, + "heading": 0.5838269457597958, + "angularVelocity": -0.3736515112958315, + "velocityX": -1.3609553011176063, + "velocityY": -1.5715360858180332, + "timestamp": 11.162125518445118 }, { - "x": 3.132337992654885, - "y": 6.1744965280297155, - "heading": 0.27407536868463933, - "angularVelocity": 0.0586318729440976, - "velocityX": -2.303593334193982, - "velocityY": -0.9701249885693223, - "timestamp": 6.029857188987679 + "x": 7.823812920396748, + "y": 5.191948689740781, + "heading": 0.5579732566194568, + "angularVelocity": -0.3979983888198644, + "velocityX": -1.6297828290483853, + "velocityY": -1.779753764641825, + "timestamp": 11.227084799353262 }, { - "x": 2.9153402053678152, - "y": 6.088110219485557, - "heading": 0.2808618012562985, - "angularVelocity": 0.08289122158182176, - "velocityX": -2.650466423831284, - "velocityY": -1.0551444470358684, - "timestamp": 6.1117287363259 + "x": 7.699409898040676, + "y": 5.064316508380435, + "heading": 0.5311065320658849, + "angularVelocity": -0.41359331842916214, + "velocityX": -1.9150923565792266, + "velocityY": -1.9648028669039912, + "timestamp": 11.292044080261407 }, { - "x": 2.6980153975230565, - "y": 6.030900368674856, - "heading": 0.29585281339155545, - "angularVelocity": 0.1831040529053057, - "velocityX": -2.6544607364871555, - "velocityY": -0.6987757367569974, - "timestamp": 6.1936002836641215 + "x": 7.555088273925506, + "y": 4.927129260587564, + "heading": 0.5041069990932381, + "angularVelocity": -0.4156378056405048, + "velocityX": -2.221724472585296, + "velocityY": -2.111896035100176, + "timestamp": 11.357003361169552 }, { - "x": 2.5044902704926533, - "y": 5.990681526275272, - "heading": 0.31201148477172486, - "angularVelocity": 0.1973661412971212, - "velocityX": -2.3637653534374596, - "velocityY": -0.4912432182750236, - "timestamp": 6.275471831002343 + "x": 7.389294037285908, + "y": 4.784758435393721, + "heading": 0.47842631175838873, + "angularVelocity": -0.3953351542047192, + "velocityX": -2.5522794329271568, + "velocityY": -2.1916933685759186, + "timestamp": 11.421962642077697 }, { - "x": 2.3360827419423367, - "y": 5.965439222430923, - "heading": 0.328645437097879, - "angularVelocity": 0.20317134422093217, - "velocityX": -2.0569725872482194, - "velocityY": -0.30831594937456525, - "timestamp": 6.357343378340564 + "x": 7.201584646398764, + "y": 4.645022669025462, + "heading": 0.45644938425338794, + "angularVelocity": -0.33831851581111005, + "velocityX": -2.8896469952087562, + "velocityY": -2.1511285903218442, + "timestamp": 11.486921922985841 }, { - "x": 2.1932220965526215, - "y": 5.95442921664246, - "heading": 0.34550838612461254, - "angularVelocity": 0.2059683684378234, - "velocityX": -1.744936428275136, - "velocityY": -0.13447902411053952, - "timestamp": 6.439214925678786 + "x": 6.996114167384252, + "y": 4.5183305598699945, + "heading": 0.4409940125357126, + "angularVelocity": -0.23792399641137907, + "velocityX": -3.16306578739771, + "velocityY": -1.9503311518274693, + "timestamp": 11.551881203893986 }, { - "x": 2.076119719939017, - "y": 5.957265361506873, - "heading": 0.3624743968160351, - "angularVelocity": 0.20722718994590164, - "velocityX": -1.4303183513784072, - "velocityY": 0.03464139810008477, - "timestamp": 6.521086473017007 + "x": 6.77954298581377, + "y": 4.410738153252425, + "heading": 0.4262239025498049, + "angularVelocity": -0.2273748997744145, + "velocityX": -3.3339528785227577, + "velocityY": -1.656305382593562, + "timestamp": 11.61684048480213 }, { - "x": 1.984901205969126, - "y": 5.973711453340982, - "heading": 0.37946704953669896, - "angularVelocity": 0.20755260249895005, - "velocityX": -1.1141662388895142, - "velocityY": 0.20087676816670857, - "timestamp": 6.602958020355229 + "x": 6.556274698008425, + "y": 4.324091773722086, + "heading": 0.4025359766205174, + "angularVelocity": -0.3646580688413556, + "velocityX": -3.43704986699365, + "velocityY": -1.3338568149000936, + "timestamp": 11.681799765710275 }, { - "x": 1.9196497213527057, - "y": 6.003608138127345, - "heading": 0.39643505242128974, - "angularVelocity": 0.207251523102328, - "velocityX": -0.7969983069558729, - "velocityY": 0.3651657475442049, - "timestamp": 6.68482956769345 + "x": 6.330620508913663, + "y": 4.259333422241355, + "heading": 0.3712479640794565, + "angularVelocity": -0.4816557711792279, + "velocityX": -3.4737790495840706, + "velocityY": -0.996906840337434, + "timestamp": 11.74675904661842 }, { - "x": 1.8804243803024292, - "y": 6.046840667724609, - "heading": 0.41334160220463595, - "angularVelocity": 0.20650091922048514, - "velocityX": -0.4791083389230676, - "velocityY": 0.5280531638014023, - "timestamp": 6.766701115031672 + "x": 6.105727463248204, + "y": 4.216621857922548, + "heading": 0.33499279771425994, + "angularVelocity": -0.5581214240419701, + "velocityX": -3.462061810435755, + "velocityY": -0.6575128868683483, + "timestamp": 11.811718327526565 }, { - "x": 1.8671378435769306, - "y": 6.1025986442210955, - "heading": 0.4299850055686104, - "angularVelocity": 0.20542336557580967, - "velocityX": -0.16399080352197462, - "velocityY": 0.6882000597544763, - "timestamp": 6.84772112615515 + "x": 5.88368699546657, + "y": 4.195803829122771, + "heading": 0.29554108787827377, + "angularVelocity": -0.6073298423942238, + "velocityX": -3.4181484874441033, + "velocityY": -0.3204781288945418, + "timestamp": 11.87667760843471 }, { - "x": 1.8794728316218305, - "y": 6.171151657888672, - "heading": 0.4464831819952248, - "angularVelocity": 0.20363088325759873, - "velocityX": 0.1522461904639937, - "velocityY": 0.8461244662518951, - "timestamp": 6.928741137278629 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.2541387867771501, + "angularVelocity": -0.6373577496904229, + "velocityX": -3.351873105763798, + "velocityY": 0.013011927161991938, + "timestamp": 11.941636889342854 }, { - "x": 1.9175617070085949, - "y": 6.252231071793212, - "heading": 0.46275009292634184, - "angularVelocity": 0.2007764588716919, - "velocityX": 0.4701168866629144, - "velocityY": 1.0007331865330475, - "timestamp": 7.009761148402108 + "x": 5.421845873128357, + "y": 4.226158090162152, + "heading": 0.20498652451135702, + "angularVelocity": -0.6557520326399592, + "velocityX": -3.256674335436179, + "velocityY": 0.3936868024775833, + "timestamp": 12.01659245153595 }, { - "x": 1.9816154560716552, - "y": 6.34539325484771, - "heading": 0.47864458300524776, - "angularVelocity": 0.19617980618000597, - "velocityX": 0.7905917090709768, - "velocityY": 1.1498663325596743, - "timestamp": 7.090781159525586 + "x": 5.1888462982312005, + "y": 4.282903811630084, + "heading": 0.15538584116955914, + "angularVelocity": -0.6617345249712095, + "velocityX": -3.1085027992574967, + "velocityY": 0.7570581796417871, + "timestamp": 12.091548013729044 }, { - "x": 2.0720214338634078, - "y": 6.449767500665198, - "heading": 0.4938927703980073, - "angularVelocity": 0.18820273141558316, - "velocityX": 1.115847511474288, - "velocityY": 1.2882526720271141, - "timestamp": 7.171801170649065 + "x": 4.97232882862877, + "y": 4.364011234145266, + "heading": 0.10667736071804554, + "angularVelocity": -0.6498314338038612, + "velocityX": -2.8886111086013, + "velocityY": 1.08207343313948, + "timestamp": 12.166503575922139 }, { - "x": 2.1896944819931505, - "y": 6.562891621237937, - "heading": 0.5077401370806992, - "angularVelocity": 0.17091292003882794, - "velocityX": 1.4523948651451515, - "velocityY": 1.3962491365291643, - "timestamp": 7.252821181772544 + "x": 4.778685496187651, + "y": 4.463599711895904, + "heading": 0.06061512765816293, + "angularVelocity": -0.6145272173560689, + "velocityX": -2.5834417990524554, + "velocityY": 1.3286335908479392, + "timestamp": 12.241459138115234 }, { - "x": 2.3309802432224416, - "y": 6.659921406601519, - "heading": 0.5136983140040886, - "angularVelocity": 0.07353957177701248, - "velocityX": 1.7438378404313606, - "velocityY": 1.1976027158982174, - "timestamp": 7.333841192896022 + "x": 4.613047571450946, + "y": 4.572114931147961, + "heading": 0.018966273025947666, + "angularVelocity": -0.5556472850530078, + "velocityX": -2.2098149875789206, + "velocityY": 1.4477273744209653, + "timestamp": 12.316414700308329 + }, + { + "x": 4.476747715125104, + "y": 4.67904248790438, + "heading": -0.017090034062622824, + "angularVelocity": -0.4810357768471503, + "velocityX": -1.8184088323521106, + "velocityY": 1.4265459910894789, + "timestamp": 12.391370262501423 }, { - "x": 2.4512963274996546, - "y": 6.737453803425126, - "heading": 0.5174619765612319, - "angularVelocity": 0.04645349346357551, - "velocityX": 1.4850168817410538, - "velocityY": 0.9569536679703065, - "timestamp": 7.414861204019501 + "x": 4.368193539344783, + "y": 4.776321878924991, + "heading": -0.04707218407807139, + "angularVelocity": -0.3999990012510431, + "velocityX": -1.4482471027389752, + "velocityY": 1.2978275150549403, + "timestamp": 12.466325824694518 }, { - "x": 2.5483430242698013, - "y": 6.798294060832995, - "heading": 0.5200641230005525, - "angularVelocity": 0.03211733006744314, - "velocityX": 1.197811447128081, - "velocityY": 0.7509287713518723, - "timestamp": 7.49588121514298 + "x": 4.2849948544017495, + "y": 4.858769971637843, + "heading": -0.07089061350138144, + "angularVelocity": -0.3177673374252157, + "velocityX": -1.1099734630593825, + "velocityY": 1.0999596334219341, + "timestamp": 12.541281386887613 }, { - "x": 2.62147124688145, - "y": 6.843385043467462, - "heading": 0.5218324598046723, - "angularVelocity": 0.02182592645444906, - "velocityX": 0.9025945762979182, - "velocityY": 0.556541303922395, - "timestamp": 7.576901226266458 + "x": 4.224952920077694, + "y": 4.923143218233113, + "heading": -0.08860420951463882, + "angularVelocity": -0.23632130151495423, + "velocityX": -0.8010337400896302, + "velocityY": 0.8588188082618416, + "timestamp": 12.616236949080708 }, { - "x": 2.6703780246763387, - "y": 6.8731963037812145, - "heading": 0.5229271765337971, - "angularVelocity": 0.013511683273610298, - "velocityX": 0.6036382508063606, - "velocityY": 0.367949348566719, - "timestamp": 7.657921237389937 + "x": 4.186266834046781, + "y": 4.967343359425017, + "heading": -0.10031361199761639, + "angularVelocity": -0.1562179262002281, + "velocityX": -0.5161202837923176, + "velocityY": 0.5896846064343946, + "timestamp": 12.691192511273803 }, { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, - "angularVelocity": 0.006368644721814939, - "velocityX": 0.30252191195191025, - "velocityY": 0.18282262031723026, - "timestamp": 7.738941248513416 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.07749108715135035, + "velocityX": -0.250385269598314, + "velocityY": 0.3015711680564945, + "timestamp": 12.766148073466898 }, { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, - "angularVelocity": -2.018007055520513e-33, - "velocityX": -1.0144673376047062e-35, - "velocityY": 0, - "timestamp": 7.819961259636894 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -7.372861805121809e-39, + "velocityX": 4.014609310130765e-40, + "velocityY": -1.5891469244205483e-39, + "timestamp": 12.841103635659993 } ], "constraints": [ @@ -21251,13 +22089,25 @@ }, { "scope": [ - 4 + 3 ], "type": "StopPoint" }, { "scope": [ - 1 + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 11 ], "type": "StopPoint" } @@ -21267,7 +22117,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "CenterLanePCBA": { + "CenterLanePCBFSprint": { "waypoints": [ { "x": 1.2899744510650635, @@ -21294,30 +22144,102 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 12 }, { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 10 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -21328,569 +22250,1568 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.7297728380351187e-19, - "angularVelocity": 4.2448351957919954e-19, - "velocityX": -3.109246931729259e-17, - "velocityY": 1.4154954258989747e-18, + "heading": 0, + "angularVelocity": -1.254857983919809e-31, + "velocityX": 0, + "velocityY": 0, "timestamp": 0 }, { - "x": 1.3060810448259774, - "y": 5.572403683390414, - "heading": -0.01067910748684314, - "angularVelocity": -0.14191654637088458, - "velocityX": 0.21404337049401068, - "velocityY": -0.246522471704611, - "timestamp": 0.07524920638169826 - }, - { - "x": 1.338296279992689, - "y": 5.535304313687478, - "heading": -0.032027558921844806, - "angularVelocity": -0.28370334335098873, - "velocityX": 0.4281139525027307, - "velocityY": -0.4930200793709309, - "timestamp": 0.1504984127633965 - }, - { - "x": 1.3866231363779933, - "y": 5.479658598357654, - "heading": -0.06402169471627948, - "angularVelocity": -0.42517572387591435, - "velocityX": 0.6422241337702286, - "velocityY": -0.7394857435109029, - "timestamp": 0.22574761914509478 - }, - { - "x": 1.4510659275520246, - "y": 5.405469878379882, - "heading": -0.1066262273585151, - "angularVelocity": -0.5661791624235272, - "velocityX": 0.8563916388334698, - "velocityY": -0.9859070087913582, - "timestamp": 0.300996825526793 + "x": 1.3060574276601378, + "y": 5.572382522924401, + "heading": -0.010694878831235951, + "angularVelocity": -0.1421235514303064, + "velocityX": 0.21372563329485256, + "velocityY": -0.24679919125306551, + "timestamp": 0.07525057405130896 + }, + { + "x": 1.3382235783773917, + "y": 5.535239267164631, + "heading": -0.032076102097718266, + "angularVelocity": -0.2841336898257091, + "velocityX": 0.4274538915144429, + "velocityY": -0.4935943177620345, + "timestamp": 0.15050114810261792 + }, + { + "x": 1.3864734186078633, + "y": 5.479524852099925, + "heading": -0.06412157270911513, + "angularVelocity": -0.42585018142040665, + "velocityX": 0.6411889987319064, + "velocityY": -0.7403852497811617, + "timestamp": 0.22575172215392686 + }, + { + "x": 1.4508078150986874, + "y": 5.405239694992128, + "heading": -0.1067981371766019, + "angularVelocity": -0.567126098451675, + "velocityX": 0.8549356240955547, + "velocityY": -0.9871706368349642, + "timestamp": 0.30100229620523583 + }, + { + "x": 1.5312280234732587, + "y": 5.3123844378954015, + "heading": -0.16006527765002243, + "angularVelocity": -0.7078635763993126, + "velocityX": 1.068698935356638, + "velocityY": -1.233947491673002, + "timestamp": 0.3762528702565448 + }, + { + "x": 1.6277357530197398, + "y": 5.200960116982037, + "heading": -0.2238798805982425, + "angularVelocity": -0.8480281214272752, + "velocityX": 1.2824849612365503, + "velocityY": -1.4807105768852864, + "timestamp": 0.4515034443078538 + }, + { + "x": 1.7403333929369436, + "y": 5.0709684768538175, + "heading": -0.2982015141504559, + "angularVelocity": -0.9876553699462992, + "velocityX": 1.4963027370230912, + "velocityY": -1.727450478198834, + "timestamp": 0.5267540183591627 + }, + { + "x": 1.8690253968085462, + "y": 4.922413250022354, + "heading": -0.3829965804922361, + "angularVelocity": -1.1268361392905244, + "velocityX": 1.7101796961898603, + "velocityY": -1.9741407784113794, + "timestamp": 0.6020045924104717 + }, + { + "x": 2.02184146077192, + "y": 4.778613992669738, + "heading": -0.4624358186091319, + "angularVelocity": -1.0556628851958798, + "velocityX": 2.0307627668195027, + "velocityY": -1.9109390082922875, + "timestamp": 0.6772551664617806 + }, + { + "x": 2.158573003278503, + "y": 4.653386193108499, + "heading": -0.5314192813425986, + "angularVelocity": -0.9167167639888999, + "velocityX": 1.817016603947309, + "velocityY": -1.664144109715068, + "timestamp": 0.7525057405130896 + }, + { + "x": 2.279216333070278, + "y": 4.546726509082428, + "heading": -0.5899522323979595, + "angularVelocity": -0.7778405918177544, + "velocityX": 1.6032213881911423, + "velocityY": -1.417393626160111, + "timestamp": 0.8277563145643986 + }, + { + "x": 2.3837699497753926, + "y": 4.458633448495562, + "heading": -0.6380354012157764, + "angularVelocity": -0.6389741131339646, + "velocityX": 1.3894062340921998, + "velocityY": -1.1706629709704055, + "timestamp": 0.9030068886157075 + }, + { + "x": 2.4722329980367945, + "y": 4.3891060989240405, + "heading": -0.6756677941957627, + "angularVelocity": -0.5000944305598923, + "velocityX": 1.1755797132095953, + "velocityY": -0.9239444409216138, + "timestamp": 0.9782574626670165 + }, + { + "x": 2.5446049418017402, + "y": 4.338143858296765, + "heading": -0.7028477816482255, + "angularVelocity": -0.36119309114484527, + "velocityX": 0.9617460687572824, + "velocityY": -0.6772339117562157, + "timestamp": 1.0535080367183254 + }, + { + "x": 2.6008854517223408, + "y": 4.305746335795738, + "heading": -0.7195735469228228, + "angularVelocity": -0.22226761038098528, + "velocityX": 0.7479080476225236, + "velocityY": -0.430528576145586, + "timestamp": 1.1287586107696344 + }, + { + "x": 2.641074357140525, + "y": 4.291913306081633, + "heading": -0.7258432570432893, + "angularVelocity": -0.08331777131422959, + "velocityX": 0.5340677586239448, + "velocityY": -0.1838262350545031, + "timestamp": 1.2040091848209433 }, { - "x": 1.5316310742105061, - "y": 5.312743180460654, - "heading": -0.15979745091713102, - "angularVelocity": -0.7066017851257455, - "velocityX": 1.0706444696567097, - "velocityY": -1.2322614732810149, - "timestamp": 0.37624603190849126 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05565594681973357, + "velocityX": 0.3202270068209183, + "velocityY": 0.06287502296377193, + "timestamp": 1.2792597588722523 + }, + { + "x": 2.6731092983432756, + "y": 4.320288362117908, + "heading": -0.7067987769295019, + "angularVelocity": 0.19580389973285955, + "velocityX": 0.10461714888899723, + "velocityY": 0.3116194322188291, + "timestamp": 1.3551333149479476 + }, + { + "x": 2.664687958259504, + "y": 4.362805441582899, + "heading": -0.6813130909046112, + "angularVelocity": 0.3358968175826016, + "velocityX": -0.11099176735530734, + "velocityY": 0.5603675597065643, + "timestamp": 1.431006871023643 + }, + { + "x": 2.6399077508131867, + "y": 4.424196327740775, + "heading": -0.6452021974872086, + "angularVelocity": 0.4759351648285144, + "velocityX": -0.32659873516708793, + "velocityY": 0.8091209814393522, + "timestamp": 1.5068804270993383 + }, + { + "x": 2.5987688991007616, + "y": 4.504461543612991, + "heading": -0.5984695371274339, + "angularVelocity": 0.6159281675522111, + "velocityX": -0.5422027626077326, + "velocityY": 1.057881296501775, + "timestamp": 1.5827539831750337 + }, + { + "x": 2.541271696403367, + "y": 4.60360171291691, + "heading": -0.5411163147183906, + "angularVelocity": 0.7559052899984986, + "velocityX": -0.7578029246555089, + "velocityY": 1.3066498320479574, + "timestamp": 1.658627539250729 + }, + { + "x": 2.46741646400873, + "y": 4.721617471023204, + "heading": -0.47313851139747326, + "angularVelocity": 0.8959353803401775, + "velocityX": -0.9733988521888661, + "velocityY": 1.55542674166613, + "timestamp": 1.7345010953264244 + }, + { + "x": 2.3772033191525277, + "y": 4.85850915554647, + "heading": -0.39452182857786383, + "angularVelocity": 1.036153923517168, + "velocityX": -1.1889932345783751, + "velocityY": 1.8042081009834092, + "timestamp": 1.8103746514021197 + }, + { + "x": 2.2706306867395725, + "y": 5.014275344454255, + "heading": -0.30523454109050485, + "angularVelocity": 1.1767905987884617, + "velocityX": -1.404608376447531, + "velocityY": 2.0529707181614056, + "timestamp": 1.886248207477815 + }, + { + "x": 2.1562102192291532, + "y": 5.146460354029385, + "heading": -0.2291038294161038, + "angularVelocity": 1.0033892651564627, + "velocityX": -1.5080414498275487, + "velocityY": 1.7421749607384658, + "timestamp": 1.9621217635535104 + }, + { + "x": 2.0581337319934665, + "y": 5.259758702235753, + "heading": -0.16374377227039283, + "angularVelocity": 0.8614339504706835, + "velocityX": -1.292630691201352, + "velocityY": 1.4932521166502593, + "timestamp": 2.0379953196292058 + }, + { + "x": 1.976403182773577, + "y": 5.354172844449837, + "heading": -0.1092054383318089, + "angularVelocity": 0.7188055596708355, + "velocityX": -1.077194129890758, + "velocityY": 1.244361633992147, + "timestamp": 2.113868875704901 + }, + { + "x": 1.9110187048049692, + "y": 5.429703613246766, + "heading": -0.0655341619901584, + "angularVelocity": 0.575579669661773, + "velocityX": -0.8617558125585749, + "velocityY": 0.9954821245277466, + "timestamp": 2.1897424317805965 + }, + { + "x": 1.861980199634934, + "y": 5.486351461082553, + "heading": -0.03276467227497517, + "angularVelocity": 0.4318960572088904, + "velocityX": -0.6463187928148273, + "velocityY": 0.7466085783539637, + "timestamp": 2.265615987856292 + }, + { + "x": 1.8292876463834091, + "y": 5.5241166468364415, + "heading": -0.01091805858135444, + "angularVelocity": 0.2879344902726512, + "velocityX": -0.4308820482661345, + "velocityY": 0.4977384441630725, + "timestamp": 2.341489543931987 }, { - "x": 1.6283288838862067, - "y": 5.201486845299536, - "heading": -0.22348688396132305, - "angularVelocity": -0.8463801295274643, - "velocityX": 1.2850342791041873, - "velocityY": -1.4785050967404112, - "timestamp": 0.4514952382901895 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 2.72551505430432e-32, + "angularVelocity": 0.1438980739292335, + "velocityX": -0.21544334611501834, + "velocityY": 0.2488695893337634, + "timestamp": 2.4173631000076825 }, { - "x": 1.7411789077586277, - "y": 5.071717216156576, - "heading": -0.2976431936929222, - "angularVelocity": -0.9854763033040427, - "velocityX": 1.4996839076304158, - "velocityY": -1.7245315317294676, - "timestamp": 0.5267444446718877 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -2.2798037235511704e-30, + "angularVelocity": -3.1967802998211905e-29, + "velocityX": 0, + "velocityY": 3.158110370432563e-31, + "timestamp": 2.493236656083378 }, { - "x": 1.8702368010806378, - "y": 4.923481749892843, - "heading": -0.3821997162379228, - "angularVelocity": -1.1236865690804896, - "velocityX": 1.7150731486446302, - "velocityY": -1.9699273041075376, - "timestamp": 0.601993651053586 + "x": 1.8507557561824508, + "y": 5.5430527114919315, + "heading": 1.2021210509026438e-21, + "angularVelocity": 1.2912282770144636e-20, + "velocityX": 0.40617567078743805, + "velocityY": 0.0005740544395093852, + "timestamp": 2.5863356901257513 }, { - "x": 2.022770759979851, - "y": 4.779444476824575, - "heading": -0.46182371324885607, - "angularVelocity": -1.0581373656886817, - "velocityX": 2.0270507322765163, - "velocityY": -1.914136772919067, - "timestamp": 0.6772428574352843 + "x": 1.9263848800936068, + "y": 5.543159599317717, + "heading": 3.6194046492049965e-21, + "angularVelocity": 2.5964647465645482e-20, + "velocityX": 0.8123513276919048, + "velocityY": 0.00114810885939775, + "timestamp": 2.6794347241681247 }, { - "x": 2.159313623811303, - "y": 4.6540559405768205, - "heading": -0.5309316944889129, - "angularVelocity": -0.9183881739497737, - "velocityX": 1.8145422443221764, - "velocityY": -1.666310414116386, - "timestamp": 0.7524920638169826 + "x": 2.039828562987611, + "y": 5.543319931052194, + "heading": -1.5854010106883968e-20, + "angularVelocity": -2.0916881637274291e-19, + "velocityX": 1.2185269596070247, + "velocityY": 0.0017221632439682793, + "timestamp": 2.772533758210498 }, { - "x": 2.2798095691663756, - "y": 4.547269113259306, - "heading": -0.5895622742606058, - "angularVelocity": -0.7791521344995035, - "velocityX": 1.601291909229005, - "velocityY": -1.4191090172553904, - "timestamp": 0.8277412701986809 + "x": 2.1910867994360017, + "y": 5.543533706687691, + "heading": 4.514872599726518e-21, + "angularVelocity": 2.1878726150197974e-19, + "velocityX": 1.624702533213676, + "velocityY": 0.0022962175461305355, + "timestamp": 2.8656327922528715 }, { - "x": 2.3842397515062683, - "y": 4.459067906121371, - "heading": -0.6377272819831894, - "angularVelocity": -0.6400732982918503, - "velocityX": 1.3877911457302048, - "velocityY": -1.172121426644562, - "timestamp": 0.9029904765803792 + "x": 2.380159562296481, + "y": 5.5438009261858445, + "heading": -2.4606104769465887e-21, + "angularVelocity": -7.492540764169747e-20, + "velocityX": 2.0308778152781284, + "velocityY": 0.002870271436251635, + "timestamp": 2.958731826295245 }, { - "x": 2.47259465372947, - "y": 4.389444105411809, - "heading": -0.675431270833758, - "angularVelocity": -0.5010549700591305, - "velocityX": 1.1741639077877584, - "velocityY": -0.9252429900248669, - "timestamp": 0.9782396829620775 + "x": 2.5314177987448714, + "y": 5.544014701821341, + "heading": 1.8729877149138444e-20, + "angularVelocity": 2.2761232534851326e-19, + "velocityX": 1.6247025332136762, + "velocityY": 0.002296217546130536, + "timestamp": 3.0518308603376183 }, { - "x": 2.5448685468329297, - "y": 4.338392728081247, - "heading": -0.7026759403224371, - "angularVelocity": -0.36205922691677456, - "velocityX": 0.9604605361114344, - "velocityY": -0.6784307740288329, - "timestamp": 1.0534888893437757 + "x": 2.6448614816388756, + "y": 5.544175033555818, + "heading": 3.8900824798152035e-20, + "angularVelocity": 2.1666119156330769e-19, + "velocityX": 1.218526959607025, + "velocityY": 0.0017221632439682793, + "timestamp": 3.1449298943799917 }, { - "x": 2.601057641331632, - "y": 4.305910462744002, - "heading": -0.7194616948028688, - "angularVelocity": -0.2230688573014308, - "velocityX": 0.7467068052991092, - "velocityY": -0.43166256362226185, - "timestamp": 1.128738095725474 + "x": 2.7204906055500317, + "y": 5.544281921381604, + "heading": 1.1738818455912713e-20, + "angularVelocity": -2.9175390079640015e-19, + "velocityX": 0.8123513276919049, + "velocityY": 0.00114810885939775, + "timestamp": 3.238028928422365 }, { - "x": 2.641159295921209, - "y": 4.291994997972796, - "heading": -0.7257882885399572, - "angularVelocity": -0.08407522206898749, - "velocityX": 0.5329179737262918, - "velocityY": -0.18492507018124238, - "timestamp": 1.2039873021071723 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.0197992544535218e-37, + "angularVelocity": -1.260895838142623e-19, + "velocityX": 0.40617567078743805, + "velocityY": 0.0005740544395093853, + "timestamp": 3.3311279624647385 }, { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.054926472219667455, - "velocityX": 0.31910406053771284, - "velocityY": 0.061790547746105144, - "timestamp": 1.2792365084888706 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "angularVelocity": 9.592877809620058e-37, + "velocityX": 0, + "velocityY": 7.039095899612751e-38, + "timestamp": 3.424226996507112 + }, + { + "x": 2.7765323823959327, + "y": 5.537624192256726, + "heading": 9.00740418544712e-19, + "angularVelocity": 1.3499597384555195e-17, + "velocityX": 0.2731753218474804, + "velocityY": -0.10058184592921769, + "timestamp": 3.490950498911381 + }, + { + "x": 2.812967100056819, + "y": 5.524148553837151, + "heading": 1.885084819596924e-20, + "angularVelocity": -1.3217075521281405e-17, + "velocityX": 0.5460552331340873, + "velocityY": -0.2019623960674071, + "timestamp": 3.55767400131565 + }, + { + "x": 2.8675858546495747, + "y": 5.503845620088409, + "heading": 3.43159270927708e-20, + "angularVelocity": 2.317785843530086e-19, + "velocityX": 0.8185834469813632, + "velocityY": -0.30428459264216856, + "timestamp": 3.6243975037199188 + }, + { + "x": 2.9403602428294473, + "y": 5.476640214649627, + "heading": 3.5333541604123114e-18, + "angularVelocity": 5.244086577062937e-17, + "velocityX": 1.0906859735710903, + "velocityY": -0.40773347409056915, + "timestamp": 3.6911210061241877 + }, + { + "x": 3.031255192543775, + "y": 5.4424407847147105, + "heading": 5.715508292123115e-18, + "angularVelocity": 3.2704430269696984e-17, + "velocityX": 1.3622628674916857, + "velocityY": -0.5125544778465974, + "timestamp": 3.7578445085284566 + }, + { + "x": 3.140226319352468, + "y": 5.40113340648718, + "heading": 6.027086577670902e-18, + "angularVelocity": 4.669693201563416e-18, + "velocityX": 1.6331745619175013, + "velocityY": -0.619082882928634, + "timestamp": 3.8245680109327256 + }, + { + "x": 3.267215684627595, + "y": 5.352572466235242, + "heading": 7.223223650198719e-18, + "angularVelocity": 1.7926772867621648e-17, + "velocityX": 1.9032179172147636, + "velocityY": -0.7277936334593704, + "timestamp": 3.8912915133369945 + }, + { + "x": 3.41214454573205, + "y": 5.296565350322459, + "heading": 8.483175715581024e-18, + "angularVelocity": 1.888318238685755e-17, + "velocityX": 2.172081139061778, + "velocityY": -0.8393911274837239, + "timestamp": 3.9580150157412635 + }, + { + "x": 3.5748998961264853, + "y": 5.232845435249424, + "heading": 1.0703776873779667e-17, + "angularVelocity": 3.3280644423357047e-17, + "velocityX": 2.4392507067198372, + "velocityY": -0.9549845673113, + "timestamp": 4.024738518145532 + }, + { + "x": 3.755306479840233, + "y": 5.161019665655948, + "heading": 1.3781541182824059e-17, + "angularVelocity": 4.612713958550321e-17, + "velocityX": 2.7037936740893525, + "velocityY": -1.0764688154152113, + "timestamp": 4.091462020549801 + }, + { + "x": 3.953058178777003, + "y": 5.080452170332065, + "heading": 1.6552203248378283e-17, + "angularVelocity": 4.15245298240439e-17, + "velocityX": 2.9637487813307404, + "velocityY": -1.2074830070480274, + "timestamp": 4.15818552295407 }, { - "x": 2.6730242117812852, - "y": 4.320207160810404, - "heading": -0.7068522062263088, - "angularVelocity": 0.19509567726207716, - "velocityX": 0.10349358326673892, - "velocityY": 0.31054279257798323, - "timestamp": 1.3551116333379198 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 2.0019563257216408e-17, + "angularVelocity": 5.196609716079013e-17, + "velocityX": 3.2138733563909483, + "velocityY": -1.356409229176722, + "timestamp": 4.224909025358339 + }, + { + "x": 4.282917561329041, + "y": 4.939413083786088, + "heading": 2.2898103599715735e-17, + "angularVelocity": 8.297955477827177e-17, + "velocityX": 3.327163862876717, + "velocityY": -1.456761903756842, + "timestamp": 4.25959877936104 + }, + { + "x": 4.401833285665519, + "y": 4.884962621939577, + "heading": 2.513067280390373e-17, + "angularVelocity": 6.43581734252565e-17, + "velocityX": 3.4279783110372324, + "velocityY": -1.5696410485433538, + "timestamp": 4.2942885333637415 + }, + { + "x": 4.518840762702163, + "y": 4.825621230712266, + "heading": 2.8188632508696854e-17, + "angularVelocity": 8.815166877675102e-17, + "velocityX": 3.372969350763695, + "velocityY": -1.7106316528704353, + "timestamp": 4.328978287366443 + }, + { + "x": 4.633380439466489, + "y": 4.761646132811981, + "heading": 3.13421659224765e-17, + "angularVelocity": 9.090676784661175e-17, + "velocityX": 3.3018301817708395, + "velocityY": -1.8442073096080187, + "timestamp": 4.363668041369144 + }, + { + "x": 4.745269125938669, + "y": 4.693140128704652, + "heading": 3.2482743151706595e-17, + "angularVelocity": 3.287936919763874e-17, + "velocityX": 3.2254102022016937, + "velocityY": -1.9748195418737586, + "timestamp": 4.398357795371846 + }, + { + "x": 4.854330043574812, + "y": 4.6202164218202615, + "heading": 3.23593373316049e-17, + "angularVelocity": -3.55741410250663e-18, + "velocityX": 3.1438942353886365, + "velocityY": -2.1021684638845084, + "timestamp": 4.433047549374547 + }, + { + "x": 4.962829604926255, + "y": 4.546460093483164, + "heading": 3.1455405916033935e-17, + "angularVelocity": -2.605759082352001e-17, + "velocityX": 3.1277120426680747, + "velocityY": -2.1261704055714574, + "timestamp": 4.467737303377248 + }, + { + "x": 5.07386693975175, + "y": 4.476582621829349, + "heading": 2.90082292000263e-17, + "angularVelocity": -7.054465464981545e-17, + "velocityX": 3.200868326043711, + "velocityY": -2.014354776005967, + "timestamp": 4.50242705737995 + }, + { + "x": 5.187610618884458, + "y": 4.411202761148161, + "heading": 2.619388391387083e-17, + "angularVelocity": -8.112900673600335e-17, + "velocityX": 3.2788839933500147, + "velocityY": -1.884702343985982, + "timestamp": 4.537116811382651 + }, + { + "x": 5.303878902064225, + "y": 4.350425816718677, + "heading": 2.3840017411584327e-17, + "angularVelocity": -6.785480525757533e-17, + "velocityX": 3.35166064223783, + "velocityY": -1.7520142813566866, + "timestamp": 4.571806565385352 + }, + { + "x": 5.422485692712026, + "y": 4.294349264150295, + "heading": 2.0445926478566092e-17, + "angularVelocity": -9.784130878396876e-17, + "velocityX": 3.419072693296267, + "velocityY": -1.6165162936588033, + "timestamp": 4.606496319388054 + }, + { + "x": 5.5432410890609845, + "y": 4.243062963447586, + "heading": 1.6758899100249362e-17, + "angularVelocity": -1.0628577469963028e-16, + "velocityX": 3.4810104545438403, + "velocityY": -1.478427915594755, + "timestamp": 4.641186073390755 }, { - "x": 2.6645156104831624, - "y": 4.362642495661916, - "heading": -0.6814188000083412, - "angularVelocity": 0.33520084834820957, - "velocityX": -0.1121395360458166, - "velocityY": 0.5592786164892918, - "timestamp": 1.430986758186969 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 1.3081934400390577e-17, + "angularVelocity": -1.0599569831404499e-16, + "velocityX": 3.537374169625408, + "velocityY": -1.337971116472263, + "timestamp": 4.675875827393456 + }, + { + "x": 5.795129656789146, + "y": 4.153827522905326, + "heading": 9.653855416146634e-18, + "angularVelocity": -9.526631807314967e-17, + "velocityX": 3.589854735093561, + "velocityY": -1.1900109590649786, + "timestamp": 4.711859993188672 + }, + { + "x": 5.925973457628177, + "y": 4.116403970594212, + "heading": 6.773802428340066e-18, + "angularVelocity": -8.003667513641558e-17, + "velocityX": 3.636149343676849, + "velocityY": -1.0400005525788714, + "timestamp": 4.747844158983888 + }, + { + "x": 6.058257666523593, + "y": 4.0844428903853265, + "heading": 3.7266948743050956e-18, + "angularVelocity": -8.467912168301893e-17, + "velocityX": 3.6761782848667472, + "velocityY": -0.8881984479166671, + "timestamp": 4.783828324779104 + }, + { + "x": 6.1917543385226965, + "y": 4.0579993432791355, + "heading": 2.3240409715281496e-19, + "angularVelocity": -9.710634385796704e-17, + "velocityX": 3.7098726356150347, + "velocityY": -0.7348661980016377, + "timestamp": 4.81981249057432 + }, + { + "x": 6.326233441808095, + "y": 4.037118888120514, + "heading": -2.9507025724087755e-18, + "angularVelocity": -8.845853722652732e-17, + "velocityX": 3.7371744019498516, + "velocityY": -0.5802678677464012, + "timestamp": 4.855796656369535 + }, + { + "x": 6.461463252337996, + "y": 4.021837506709874, + "heading": -5.178975495681446e-18, + "angularVelocity": -6.192370655342397e-17, + "velocityX": 3.7580365569536736, + "velocityY": -0.4246696032250841, + "timestamp": 4.891780822164751 + }, + { + "x": 6.596863602394611, + "y": 4.012202736620321, + "heading": -4.3543144284076125e-18, + "angularVelocity": 2.2917331805219353e-17, + "velocityX": 3.7627758505553097, + "velocityY": -0.26775026950420705, + "timestamp": 4.927764987959967 + }, + { + "x": 6.727539043403903, + "y": 4.005664590276287, + "heading": -3.5766746235771005e-18, + "angularVelocity": 2.161061087932747e-17, + "velocityX": 3.6314706238561003, + "velocityY": -0.1816950928150608, + "timestamp": 4.963749153755183 + }, + { + "x": 6.85298501008117, + "y": 4.001263406795894, + "heading": -2.645512635943814e-18, + "angularVelocity": 2.5876992478309148e-17, + "velocityX": 3.4861435274385992, + "velocityY": -0.122308892901394, + "timestamp": 4.999733319550399 + }, + { + "x": 6.973061817653349, + "y": 3.9986189838302026, + "heading": -1.6196069084437975e-18, + "angularVelocity": 2.850992109492732e-17, + "velocityX": 3.336934591051298, + "velocityY": -0.07348851660866845, + "timestamp": 5.035717485345614 + }, + { + "x": 7.0877080332373295, + "y": 3.99753108954472, + "heading": -5.829248205352589e-19, + "angularVelocity": 2.880939616045139e-17, + "velocityX": 3.186018434786799, + "velocityY": -0.030232583177662708, + "timestamp": 5.07170165114083 }, { - "x": 2.6396435810539995, - "y": 4.423949055779942, - "heading": -0.6453599487845141, - "angularVelocity": 0.47523943183649325, - "velocityX": -0.32780215490271986, - "velocityY": 0.807992873026772, - "timestamp": 1.5068618830360183 + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "angularVelocity": 1.619948128975407e-17, + "velocityX": 3.034163545803906, + "velocityY": 0.009602962363163213, + "timestamp": 5.107685816936046 + }, + { + "x": 7.37338840046626, + "y": 4.002743213470309, + "heading": 8.093787297850302e-19, + "angularVelocity": 1.2673214059219633e-17, + "velocityX": 2.7636055688801218, + "velocityY": 0.07620051362427777, + "timestamp": 5.171551125768248 + }, + { + "x": 7.532268457404618, + "y": 4.010110422409507, + "heading": 7.012678022232893e-19, + "angularVelocity": -1.6927958157108362e-18, + "velocityX": 2.487736454164719, + "velocityY": 0.11535541084684194, + "timestamp": 5.235416434600451 + }, + { + "x": 7.673406960294269, + "y": 4.018856350887051, + "heading": 5.093320205791772e-19, + "angularVelocity": -3.0053214359033797e-18, + "velocityX": 2.2099400358412575, + "velocityY": 0.13694333649152024, + "timestamp": 5.299281743432653 + }, + { + "x": 7.796760795915294, + "y": 4.028209945243461, + "heading": 2.737170340594828e-19, + "angularVelocity": -3.689248370192142e-18, + "velocityX": 1.931468552749354, + "velocityY": 0.14645814022421355, + "timestamp": 5.363147052264855 + }, + { + "x": 7.902319650014668, + "y": 4.037611597874035, + "heading": 3.811839387951723e-20, + "angularVelocity": -3.688992419979718e-18, + "velocityX": 1.65283556956901, + "velocityY": 0.14721063441931925, + "timestamp": 5.4270123610970575 + }, + { + "x": 7.990087417936277, + "y": 4.046637814965993, + "heading": -1.8999762440941744e-19, + "angularVelocity": -3.5718298785688795e-18, + "velocityX": 1.3742635794998819, + "velocityY": 0.14133208242480425, + "timestamp": 5.49087766992926 + }, + { + "x": 8.06007417147421, + "y": 4.0549574599073335, + "heading": -2.1124739162594107e-19, + "angularVelocity": -3.327278549958625e-19, + "velocityX": 1.0958492931086379, + "velocityY": 0.1302686089438499, + "timestamp": 5.554742978761462 + }, + { + "x": 8.112292461632407, + "y": 4.062304767499659, + "heading": -1.9392441616154913e-19, + "angularVelocity": 2.712423345539538e-19, + "velocityX": 0.8176315297463507, + "velocityY": 0.11504379649410336, + "timestamp": 5.6186082875936645 + }, + { + "x": 8.146755551617066, + "y": 4.068461861174775, + "heading": -1.2666965805994144e-19, + "angularVelocity": 1.0530718371509655e-18, + "velocityX": 0.5396214410425377, + "velocityY": 0.0964074829935333, + "timestamp": 5.682473596425867 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "angularVelocity": 1.9833875444262685e-18, + "velocityX": 0.26181680146361314, + "velocityY": 0.07492478756156581, + "timestamp": 5.746338905258069 + }, + { + "x": 8.160404735397709, + "y": 4.0766649695322, + "heading": 2.2110153580287816e-19, + "angularVelocity": 3.145700398787454e-18, + "velocityX": -0.04370411858778341, + "velocityY": 0.04862945386701764, + "timestamp": 5.816625807306458 + }, + { + "x": 8.135851784135758, + "y": 4.07831805033267, + "heading": 5.233120115827785e-19, + "angularVelocity": 4.299669881203419e-18, + "velocityX": -0.34932470412548816, + "velocityY": 0.02351904483329859, + "timestamp": 5.8869127093548475 + }, + { + "x": 8.089809855784104, + "y": 4.078304537638241, + "heading": 9.059382457941767e-19, + "angularVelocity": 5.443777191210811e-18, + "velocityX": -0.6550570164545902, + "velocityY": -0.00019225053366650092, + "timestamp": 5.957199611403237 + }, + { + "x": 8.022270134044854, + "y": 4.076742304755474, + "heading": 1.3681480133267195e-18, + "angularVelocity": 6.5760441001468545e-18, + "velocityX": -0.9609147617966158, + "velocityY": -0.022226515001204008, + "timestamp": 6.027486513451626 + }, + { + "x": 7.9332227395210335, + "y": 4.0737752063023285, + "heading": 1.9089237482918726e-18, + "angularVelocity": 7.693833690281903e-18, + "velocityX": -1.2669130652893827, + "velocityY": -0.04221410201153685, + "timestamp": 6.097773415500015 + }, + { + "x": 7.822656701940191, + "y": 4.069582704513375, + "heading": 2.0768842809054915e-18, + "angularVelocity": 2.3896419918926418e-18, + "velocityX": -1.5730674472567026, + "velocityY": -0.05964840769432409, + "timestamp": 6.168060317548404 + }, + { + "x": 7.690560152144021, + "y": 4.064394900776391, + "heading": 2.063501960546536e-18, + "angularVelocity": -1.9039564936380514e-19, + "velocityX": -1.8793906965088152, + "velocityY": -0.07380896846772396, + "timestamp": 6.238347219596793 + }, + { + "x": 7.536921137087825, + "y": 4.058517363178713, + "heading": 1.8242393677495476e-18, + "angularVelocity": -3.404085054560583e-18, + "velocityX": -2.185884006531148, + "velocityY": -0.08362208927108938, + "timestamp": 6.3086341216451824 + }, + { + "x": 7.361730243526304, + "y": 4.0523752131799995, + "heading": 1.7449161631907716e-18, + "angularVelocity": -1.128563106982712e-18, + "velocityX": -2.492511242577032, + "velocityY": -0.08738683623422486, + "timestamp": 6.378921023693572 + }, + { + "x": 7.164988961998875, + "y": 4.046599373609111, + "heading": 1.6203310877610785e-18, + "angularVelocity": -1.7725219321907188e-18, + "velocityX": -2.799117272119643, + "velocityY": -0.0821751905769253, + "timestamp": 6.449207925741961 + }, + { + "x": 6.946739461316107, + "y": 4.042219879396985, + "heading": 9.714507263424963e-19, + "angularVelocity": -9.231881652246047e-18, + "velocityX": -3.105123349048912, + "velocityY": -0.062308824041090796, + "timestamp": 6.51949482779035 + }, + { + "x": 6.707199372936614, + "y": 4.041196558011354, + "heading": 4.304213175776067e-18, + "angularVelocity": 4.741655062759286e-17, + "velocityX": -3.408033095761992, + "velocityY": -0.014559204571673643, + "timestamp": 6.589781729838739 + }, + { + "x": 6.4477381349567215, + "y": 4.048401250319157, + "heading": 3.981085161265745e-18, + "angularVelocity": -4.597272110228882e-18, + "velocityX": -3.691459296374506, + "velocityY": 0.102504052644734, + "timestamp": 6.660068631887128 + }, + { + "x": 6.183416579926013, + "y": 4.076604236120931, + "heading": 3.783709688486887e-18, + "angularVelocity": -2.808140165901377e-18, + "velocityX": -3.7606089801587, + "velocityY": 0.4012552122777795, + "timestamp": 6.730355533935517 + }, + { + "x": 5.922248771081443, + "y": 4.126128703964576, + "heading": 4.4649221139499255e-18, + "angularVelocity": 9.691882920007657e-18, + "velocityX": -3.7157393658460984, + "velocityY": 0.7046045052540438, + "timestamp": 6.800642435983907 }, { - "x": 2.5984050053739445, - "y": 4.5041245094115245, - "heading": -0.5986803757122448, - "angularVelocity": 0.615215766235525, - "velocityX": -0.5435058691760214, - "velocityY": 1.0566763981096883, - "timestamp": 1.5827370078850675 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 6.738429572739451e-18, + "angularVelocity": 3.2346104217601064e-17, + "velocityX": -3.646441012355221, + "velocityY": 1.003321650758162, + "timestamp": 6.870929338032296 + }, + { + "x": 5.5410942529337, + "y": 4.236435293525444, + "heading": 7.176696434761227e-18, + "angularVelocity": 1.2648537308401216e-17, + "velocityX": -3.603431102925481, + "velocityY": 1.1482444113922432, + "timestamp": 6.90557894623092 + }, + { + "x": 5.417926513758212, + "y": 4.281179467560016, + "heading": 6.902122193005105e-18, + "angularVelocity": -7.924310144639137e-18, + "velocityX": -3.554664701241286, + "velocityY": 1.2913327555706813, + "timestamp": 6.9402285544295435 + }, + { + "x": 5.296645297491296, + "y": 4.3308101048073215, + "heading": 7.073122455788382e-18, + "angularVelocity": 4.9351283224632194e-18, + "velocityX": -3.5002189800153154, + "velocityY": 1.432357819540261, + "timestamp": 6.974878162628167 + }, + { + "x": 5.177444375118266, + "y": 4.38524790502043, + "heading": 7.030277898528261e-18, + "angularVelocity": -1.2365091407245627e-18, + "velocityX": -3.44018095932656, + "velocityY": 1.5710942502164025, + "timestamp": 7.009527770826791 + }, + { + "x": 5.06051419213652, + "y": 4.444405885414455, + "heading": 6.93623096110632e-18, + "angularVelocity": -2.7142280190538443e-18, + "velocityX": -3.3746466139374856, + "velocityY": 1.7073203268248038, + "timestamp": 7.044177379025415 + }, + { + "x": 4.9460415627669985, + "y": 4.508189517767328, + "heading": 6.830736149305672e-18, + "angularVelocity": -3.0446177398576756e-18, + "velocityX": -3.303720743776551, + "velocityY": 1.8408182853682935, + "timestamp": 7.078826987224039 + }, + { + "x": 4.834209367418348, + "y": 4.576496874327604, + "heading": 6.686796334021e-18, + "angularVelocity": -4.154154195902009e-18, + "velocityX": -3.2275168800637117, + "velocityY": 1.971374572800783, + "timestamp": 7.113476595422663 + }, + { + "x": 4.725196244628599, + "y": 4.649218770197504, + "heading": 6.8822518039480765e-18, + "angularVelocity": 5.6409142870147405e-18, + "velocityX": -3.1461574446916942, + "velocityY": 2.0987797453014108, + "timestamp": 7.1481262036212865 + }, + { + "x": 4.61917611476972, + "y": 4.726238684910051, + "heading": 7.643017507811422e-18, + "angularVelocity": 2.195596843411265e-17, + "velocityX": -3.0597786056088934, + "velocityY": 2.2228220957374782, + "timestamp": 7.18277581181991 + }, + { + "x": 4.510162876116158, + "y": 4.798960406973094, + "heading": 8.156998265512583e-18, + "angularVelocity": 1.483366723094954e-17, + "velocityX": -3.14616078856249, + "velocityY": 2.098774729173764, + "timestamp": 7.217425420018534 + }, + { + "x": 4.3983305631985266, + "y": 4.867267570914994, + "heading": 9.169956130720048e-18, + "angularVelocity": 2.9234323788045264e-17, + "velocityX": -3.227520273146238, + "velocityY": 1.9713690137660609, + "timestamp": 7.252075028217158 + }, + { + "x": 4.28385782617867, + "y": 4.93105099911187, + "heading": 9.858535854282379e-18, + "angularVelocity": 1.9872655402484013e-17, + "velocityX": -3.3037238506034297, + "velocityY": 1.840812393353667, + "timestamp": 7.286724636415782 }, { - "x": 2.540795163397632, - "y": 4.603165195262531, - "heading": -0.541383229045499, - "angularVelocity": 0.755150608064052, - "velocityX": -0.7592717915230448, - "velocityY": 1.3053116689836102, - "timestamp": 1.6586121327341168 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 9.428613355674913e-18, + "angularVelocity": -1.2407716016383212e-17, + "velocityX": -3.3581551662140523, + "velocityY": 1.6997824742615957, + "timestamp": 7.321374244614406 + }, + { + "x": 3.9412126137988053, + "y": 5.086809869447978, + "heading": 6.455008109104801e-18, + "angularVelocity": -4.321651467897992e-17, + "velocityX": -3.28870544216361, + "velocityY": 1.4077326603917448, + "timestamp": 7.390181395507035 + }, + { + "x": 3.7303194478817963, + "y": 5.1698988995186355, + "heading": 5.431152098522906e-18, + "angularVelocity": -1.488008146391029e-17, + "velocityX": -3.0649890771686916, + "velocityY": 1.2075638795204993, + "timestamp": 7.458988546399665 + }, + { + "x": 3.537102982396402, + "y": 5.24230220978644, + "heading": 5.612600329775147e-18, + "angularVelocity": 2.6370548540132515e-18, + "velocityX": -2.8080869935582533, + "velocityY": 1.052264326142308, + "timestamp": 7.527795697292294 + }, + { + "x": 3.3623900460315483, + "y": 5.305525654536907, + "heading": 4.225050788166454e-18, + "angularVelocity": -2.0165775266205907e-17, + "velocityX": -2.5391683000722396, + "velocityY": 0.918849915020073, + "timestamp": 7.596602848184924 + }, + { + "x": 3.2065873746476083, + "y": 5.360439315991953, + "heading": 1.940037667439135e-18, + "angularVelocity": -3.3208948359058894e-17, + "velocityX": -2.2643383625498803, + "velocityY": 0.7980807335088809, + "timestamp": 7.6654099990775535 + }, + { + "x": 3.069932459985333, + "y": 5.407606100907469, + "heading": -1.0877040544793261e-18, + "angularVelocity": -4.4003300276785115e-17, + "velocityX": -1.986056868936749, + "velocityY": 0.6854924859353159, + "timestamp": 7.734217149970183 + }, + { + "x": 2.9525796309081422, + "y": 5.44741896036095, + "heading": -3.706402695175446e-18, + "angularVelocity": -3.805852453885881e-17, + "velocityX": -1.705532456362192, + "velocityY": 0.5786151430046301, + "timestamp": 7.803024300862813 + }, + { + "x": 2.854636716681842, + "y": 5.480167404381309, + "heading": -5.982131114439951e-18, + "angularVelocity": -3.307401032802041e-17, + "velocityX": -1.4234409208300893, + "velocityY": 0.4759453573577233, + "timestamp": 7.871831451755442 + }, + { + "x": 2.776183090706554, + "y": 5.506073449109121, + "heading": -4.835349945984594e-18, + "angularVelocity": 1.666659865403876e-17, + "velocityX": -1.1401958220550505, + "velocityY": 0.3765022151293121, + "timestamp": 7.940638602648072 + }, + { + "x": 2.7172795158509544, + "y": 5.525312687608081, + "heading": -3.43564479511919e-18, + "angularVelocity": 2.034243727152086e-17, + "velocityX": -0.8560676338352585, + "velocityY": 0.2796110324199007, + "timestamp": 8.009445753540701 + }, + { + "x": 2.6779739479348046, + "y": 5.538027438094956, + "heading": -1.81505607662002e-18, + "angularVelocity": 2.355262058485764e-17, + "velocityX": -0.5712424857916886, + "velocityY": 0.18478821346223093, + "timestamp": 8.07825290443333 }, { - "x": 2.4668060178101583, - "y": 4.721064600465768, - "heading": -0.4734677826566921, - "angularVelocity": 0.8950950199273465, - "velocityX": -0.9751436420635095, - "velocityY": 1.5538611031988796, - "timestamp": 1.734487257583166 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -7.816373438340897e-46, + "angularVelocity": 2.6378887267870092e-17, + "velocityX": -0.2858537161877381, + "velocityY": 0.09167545987040067, + "timestamp": 8.14706005532596 }, { - "x": 2.3764209643089336, - "y": 4.857808746061696, - "heading": -0.3949264705539342, - "angularVelocity": 1.0351391481588907, - "velocityX": -1.1912343298399446, - "velocityY": 1.8022263010181736, - "timestamp": 1.8103623824322153 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 1.7878647482773802e-40, + "angularVelocity": 2.5983819147247646e-39, + "velocityX": 0, + "velocityY": -2.2530838665454385e-40, + "timestamp": 8.21586720621859 }, { - "x": 2.269588305263905, - "y": 5.013353141552007, - "heading": -0.3057482687442009, - "angularVelocity": 1.175328567657553, - "velocityX": -1.4080063691172056, - "velocityY": 2.0500051340898513, - "timestamp": 1.8862375072812645 + "x": 2.6750735555579293, + "y": 5.538716972759925, + "heading": 1.5037626920568013e-19, + "angularVelocity": 2.3619256993662716e-18, + "velocityX": 0.26337722940288844, + "velocityY": -0.08824680774956185, + "timestamp": 8.279534013877285 }, { - "x": 2.155512806030656, - "y": 5.145840094887286, - "heading": -0.22945454409592414, - "angularVelocity": 1.0055169569743476, - "velocityX": -1.5034637433586158, - "velocityY": 1.7461184228535829, - "timestamp": 1.9621126321303137 + "x": 2.7085920650608806, + "y": 5.52742596598787, + "heading": 4.62580789697953e-19, + "angularVelocity": 4.9037250644942105e-18, + "velocityX": 0.5264675697678678, + "velocityY": -0.17734526336836984, + "timestamp": 8.34320082153598 }, { - "x": 2.0576690248190195, - "y": 5.259344023095087, - "heading": -0.1639803843714035, - "angularVelocity": 0.8629199603276121, - "velocityX": -1.289537004472555, - "velocityY": 1.4959306944600357, - "timestamp": 2.037987756979363 + "x": 2.7588390755328, + "y": 5.51039889888726, + "heading": 7.215027779377387e-19, + "angularVelocity": 4.066828505487763e-18, + "velocityX": 0.789218312017212, + "velocityY": -0.2674402522565463, + "timestamp": 8.406867629194675 }, { - "x": 1.9761088976814785, - "y": 5.353909559223238, - "heading": -0.10935658983885492, - "angularVelocity": 0.7199170300053718, - "velocityX": -1.0749257717858158, - "velocityY": 1.2463312095522094, - "timestamp": 2.113862881828412 + "x": 2.8257885982701296, + "y": 5.487560532002736, + "heading": -1.3111959461224426e-18, + "angularVelocity": -3.192713438621083e-17, + "velocityX": 1.0515608556382001, + "velocityY": -0.35871701007777496, + "timestamp": 8.47053443685337 }, { - "x": 1.9108492511031485, - "y": 5.429551680381221, - "heading": -0.06562174275393094, - "angularVelocity": 0.5764056029164016, - "velocityX": -0.8600927735973349, - "velocityY": 0.9969291162074897, - "timestamp": 2.1897380066774614 + "x": 2.9094088162269207, + "y": 5.458820213911237, + "heading": -3.133450423089841e-18, + "angularVelocity": -2.8621734683732664e-17, + "velocityX": 1.3134036561886782, + "velocityY": -0.4514176090871532, + "timestamp": 8.534201244512065 }, { - "x": 1.8618983395529738, - "y": 5.486277925937523, - "heading": -0.03280716495777854, - "angularVelocity": 0.4324813680566584, - "velocityX": -0.6451509852228532, - "velocityY": 0.7476263883465671, - "timestamp": 2.2656131315265107 + "x": 3.009659896203603, + "y": 5.424066617313393, + "heading": -4.721555014357318e-18, + "angularVelocity": -2.494399593240158e-17, + "velocityX": 1.5746208057754199, + "velocityY": -0.5458668005493692, + "timestamp": 8.59786805217076 }, { - "x": 1.8292611562031649, - "y": 5.524092810268815, - "heading": -0.010931847108471086, - "angularVelocity": 0.2883068448697581, - "velocityX": -0.430143389085703, - "velocityY": 0.4983831579385639, - "timestamp": 2.34148825637556 + "x": 3.1264905621724175, + "y": 5.383159777949491, + "heading": -8.056338252897749e-18, + "angularVelocity": -5.237867832823588e-17, + "velocityX": 1.8350325745107399, + "velocityY": -0.6425143786570167, + "timestamp": 8.661534859829455 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": -1.6776667047563746e-20, - "angularVelocity": 0.1440768253130289, - "velocityX": -0.2150897630156809, - "velocityY": 0.24917859900600148, - "timestamp": 2.417363381224609 + "x": 3.259832420061778, + "y": 5.335918462965804, + "heading": -1.1096621419747018e-17, + "angularVelocity": -4.775303299558587e-17, + "velocityX": 2.094370093191732, + "velocityY": -0.7420085397863474, + "timestamp": 8.72520166748815 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 1.1170600847702852e-20, - "angularVelocity": 5.938902420193292e-19, - "velocityX": -2.686779016810018e-17, - "velocityY": -1.2969153357483792e-17, - "timestamp": 2.4932385060736584 + "x": 3.409589860765726, + "y": 5.282098877479992, + "heading": -1.3592361718623002e-17, + "angularVelocity": -3.9200022596627705e-17, + "velocityX": 2.352205901492158, + "velocityY": -0.8453319314253591, + "timestamp": 8.788868475146845 }, { - "x": 1.8552093951788071, - "y": 5.543066072468525, - "heading": -1.0867013680305264e-17, - "angularVelocity": -1.1051820926914384e-16, - "velocityX": 0.42942883009919236, - "velocityY": 0.0006787122433536914, - "timestamp": 2.5916673926214395 + "x": 3.5756203018815467, + "y": 5.221355824269489, + "heading": -1.4313494756802784e-17, + "angularVelocity": -1.132667185145573e-17, + "velocityX": 2.607802200573299, + "velocityY": -0.9540772569615096, + "timestamp": 8.85253528280554 }, { - "x": 1.9397457967809342, - "y": 5.543199682246804, - "heading": -3.2635799657096195e-17, - "angularVelocity": -2.2116257473548814e-16, - "velocityX": 0.8588576440015888, - "velocityY": 0.001357424461108283, - "timestamp": 2.6900962791692207 + "x": 3.7576900306489676, + "y": 5.153163870724839, + "heading": -1.1221400900164693e-17, + "angularVelocity": 4.856681166133233e-17, + "velocityX": 2.8597276267323055, + "velocityY": -1.0710754324327645, + "timestamp": 8.916202090464235 }, { - "x": 2.0665503951985436, - "y": 5.5434000969079245, - "heading": -4.603800029490119e-17, - "angularVelocity": -1.3616125416355767e-16, - "velocityX": 1.2882864255103954, - "velocityY": 0.002036136627664645, - "timestamp": 2.788525165717002 + "x": 3.955353841452516, + "y": 5.076631094268888, + "heading": -7.381711218130638e-18, + "angularVelocity": 6.030912846483351e-17, + "velocityX": 3.104660310018777, + "velocityY": -1.2020828320186705, + "timestamp": 8.97986889812293 }, { - "x": 2.235623180866242, - "y": 5.543667316436768, - "heading": -4.798133527402523e-17, - "angularVelocity": -1.9743543259972466e-17, - "velocityX": 1.7177151098384498, - "velocityY": 0.002714848640626296, - "timestamp": 2.886954052264783 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -4.354722543751261e-18, + "angularVelocity": 4.754421944015892e-17, + "velocityX": 3.332116557247906, + "velocityY": -1.3615147608100584, + "timestamp": 9.043535705781625 }, { - "x": 2.40469596653394, - "y": 5.54393453596561, - "heading": -3.9330353852399465e-17, - "angularVelocity": 8.789067645262313e-17, - "velocityX": 1.7177151098384498, - "velocityY": 0.0027148486406302456, - "timestamp": 2.985382938812564 + "x": 4.285701077560922, + "y": 4.938388492372474, + "heading": -4.775397572984342e-18, + "angularVelocity": -1.2147965427412122e-17, + "velocityX": 3.4133567656946284, + "velocityY": -1.4888942599373947, + "timestamp": 9.078164963804028 }, { - "x": 2.531500564951549, - "y": 5.544134950626731, - "heading": -2.0009563266152006e-17, - "angularVelocity": 1.96291873894211e-16, - "velocityX": 1.2882864255103954, - "velocityY": 0.002036136627666603, - "timestamp": 3.0838118253603453 + "x": 4.403715149774036, + "y": 4.881600772971697, + "heading": -7.574216488503896e-18, + "angularVelocity": -8.082237608755476e-17, + "velocityX": 3.4079295645540304, + "velocityY": -1.6398768741750067, + "timestamp": 9.11279422182643 }, { - "x": 2.616036966553676, - "y": 5.54426856040501, - "heading": -7.02269200923652e-18, - "angularVelocity": 1.3194166582538958e-16, - "velocityX": 0.8588576440015888, - "velocityY": 0.0013574244611093242, - "timestamp": 3.1822407119081264 + "x": 4.519367006147829, + "y": 4.820144951729341, + "heading": -1.1070492199966305e-17, + "angularVelocity": -1.009630558414108e-16, + "velocityX": 3.3397151131270637, + "velocityY": -1.7746791225673963, + "timestamp": 9.147423479848833 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -3.8911537114769476e-19, - "angularVelocity": 6.739461219451495e-17, - "velocityX": 0.4294288300991924, - "velocityY": 0.0006787122433541438, - "timestamp": 3.2806695984559076 + "x": 4.6324721141561005, + "y": 4.754119165100519, + "heading": -1.4960169278634737e-17, + "angularVelocity": -1.123234311330653e-16, + "velocityX": 3.266171857771331, + "velocityY": -1.9066474536101443, + "timestamp": 9.182052737871235 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -4.27312910681112e-19, - "angularVelocity": -3.880724585667238e-19, - "velocityX": -2.3908482729518656e-18, - "velocityY": 6.838225330043828e-18, - "timestamp": 3.3790984850036887 + "x": 4.742850082889288, + "y": 4.683628924272633, + "heading": -1.7795441089169654e-17, + "angularVelocity": -8.18750378278602e-17, + "velocityX": 3.187419397256003, + "velocityY": -2.0355689048343195, + "timestamp": 9.216681995893637 }, { - "x": 2.6438893629989613, - "y": 5.563919104052164, - "heading": 0.003139586303789949, - "angularVelocity": 0.042042846518139325, - "velocityX": -0.19304501448070868, - "velocityY": 0.26224987725536203, - "timestamp": 3.453774358875246 + "x": 4.850325220314879, + "y": 4.608787325463071, + "heading": -1.8976395039793544e-17, + "angularVelocity": -3.410277950107739e-17, + "velocityX": 3.1035934225348787, + "velocityY": -2.1612244409379606, + "timestamp": 9.25131125391604 }, { - "x": 2.6152669032432256, - "y": 5.6032366820496495, - "heading": 0.009644303445155527, - "angularVelocity": 0.0871060063194363, - "velocityX": -0.38328925088933385, - "velocityY": 0.5265097809918172, - "timestamp": 3.5284502327468035 + "x": 4.959315849281407, + "y": 4.536170434444089, + "heading": -1.7824338333790707e-17, + "angularVelocity": 3.3268304658955216e-17, + "velocityX": 3.1473567494868275, + "velocityY": -2.09698085278076, + "timestamp": 9.285940511938442 }, { - "x": 2.5727487902328847, - "y": 5.662504910556293, - "heading": 0.019847811829768587, - "angularVelocity": 0.13663728130135686, - "velocityX": -0.5693688042201159, - "velocityY": 0.7936730490571009, - "timestamp": 3.603126106618361 + "x": 5.0711194796065495, + "y": 4.467963995966004, + "heading": -1.5727172733760973e-17, + "angularVelocity": 6.056051211588329e-17, + "velocityX": 3.228588676454312, + "velocityY": -1.9696188244622874, + "timestamp": 9.320569769960844 }, { - "x": 2.5168455054942007, - "y": 5.742063733198033, - "heading": 0.03429127316793741, - "angularVelocity": 0.19341536415109115, - "velocityX": -0.7486123943435556, - "velocityY": 1.0653885721991296, - "timestamp": 3.6778019804899182 + "x": 5.1855579490393096, + "y": 4.4042772106470105, + "heading": -1.2316155333836473e-17, + "angularVelocity": 9.850102470338397e-17, + "velocityX": 3.3046757559381787, + "velocityY": -1.839103375469191, + "timestamp": 9.355199027983247 }, { - "x": 2.4485453653591587, - "y": 5.842518348175502, - "heading": 0.05399784068706335, - "angularVelocity": 0.2638947024981649, - "velocityX": -0.9146212370077963, - "velocityY": 1.3452084290335973, - "timestamp": 3.7524778543614756 + "x": 5.302448702578612, + "y": 4.345211810292891, + "heading": -8.765491689722714e-18, + "angularVelocity": 1.0253363331714457e-16, + "velocityX": 3.3754911371096727, + "velocityY": -1.7056501850518684, + "timestamp": 9.389828286005649 }, { - "x": 2.370517685528427, - "y": 5.96519344251776, - "heading": 0.08158842838664396, - "angularVelocity": 0.369471239761256, - "velocityX": -1.0448847236115244, - "velocityY": 1.642767442578038, - "timestamp": 3.827153728233033 + "x": 5.421605230513571, + "y": 4.290862095789443, + "heading": -5.5119830878393785e-18, + "angularVelocity": 9.395259349127813e-17, + "velocityX": 3.4409206185669707, + "velocityY": -1.5694738382292028, + "timestamp": 9.424457544028051 }, { - "x": 2.3003293154277267, - "y": 6.110432122884094, - "heading": 0.1284873855211158, - "angularVelocity": 0.6280335897392839, - "velocityX": -0.9399069131942741, - "velocityY": 1.94492106802986, - "timestamp": 3.9018296021045904 + "x": 5.542837392691082, + "y": 4.241314822773421, + "heading": -2.184892589584252e-18, + "angularVelocity": 9.60774411078281e-17, + "velocityX": 3.500859362885684, + "velocityY": -1.4307922215361746, + "timestamp": 9.459086802050454 }, { - "x": 2.2517603849650345, - "y": 6.244622565856429, - "heading": 0.18176076028404833, - "angularVelocity": 0.7133947284575884, - "velocityX": -0.6503965463628967, - "velocityY": 1.796971846665519, - "timestamp": 3.976505475976148 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 1.1869648608997422e-18, + "angularVelocity": 9.737019049910581e-17, + "velocityX": 3.5552114934161927, + "velocityY": -1.2898268911821018, + "timestamp": 9.493716060072856 }, { - "x": 2.22192407028561, - "y": 6.363296472335711, - "heading": 0.23706248397325824, - "angularVelocity": 0.7405567664909981, - "velocityX": -0.39954423206005096, - "velocityY": 1.5891867122091021, - "timestamp": 4.051181349847705 + "x": 5.934272395491136, + "y": 4.124337934678792, + "heading": 7.497166766936204e-18, + "angularVelocity": 8.587782714305931e-17, + "velocityX": 3.6516733021145136, + "velocityY": -0.9841085377661548, + "timestamp": 9.567194885934677 }, { - "x": 2.2097448289139274, - "y": 6.465233576489708, - "heading": 0.2929845885143906, - "angularVelocity": 0.7488644141924427, - "velocityX": -0.16309472846117115, - "velocityY": 1.3650607467858884, - "timestamp": 4.125857223719263 + "x": 6.2077530174936735, + "y": 4.075010140740357, + "heading": 1.3006105833327096e-17, + "angularVelocity": 7.497315045222091e-17, + "velocityX": 3.7218970063133985, + "velocityY": -0.6713198443208217, + "timestamp": 9.640673711796499 }, { - "x": 2.2146777318065234, - "y": 6.549876478070479, - "heading": 0.348839415183568, - "angularVelocity": 0.747963482359082, - "velocityX": 0.06605751813605938, - "velocityY": 1.1334705198944957, - "timestamp": 4.20053309759082 + "x": 6.4844286250858945, + "y": 4.049019985828209, + "heading": 1.5986683133968542e-17, + "angularVelocity": 4.0563757867422685e-17, + "velocityX": 3.7653787243758563, + "velocityY": -0.3537094476852878, + "timestamp": 9.71415253765832 }, { - "x": 2.236395128424509, - "y": 6.61690736376859, - "heading": 0.4042192057980219, - "angularVelocity": 0.7416021767580177, - "velocityX": 0.2908221289159501, - "velocityY": 0.8976243895505626, - "timestamp": 4.275208971462377 + "x": 6.743277363747608, + "y": 4.038426394199003, + "heading": 1.2719246482061777e-17, + "angularVelocity": -4.446773085420891e-17, + "velocityX": 3.522766397335807, + "velocityY": -0.1441720319419269, + "timestamp": 9.787631363520141 }, { - "x": 2.274678638341779, - "y": 6.666121192465354, - "heading": 0.45885367997336884, - "angularVelocity": 0.7316214908889929, - "velocityX": 0.5126623624534666, - "velocityY": 0.6590325113759182, - "timestamp": 4.349884845333935 + "x": 6.978672822837657, + "y": 4.030025299692623, + "heading": 8.975092196512461e-18, + "angularVelocity": -5.095555408833389e-17, + "velocityX": 3.2035822065626665, + "velocityY": -0.11433354313769856, + "timestamp": 9.861110189381963 }, { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "angularVelocity": 0.719064094502462, - "velocityX": 0.7324155022029422, - "velocityY": 0.41852376977375944, - "timestamp": 4.424560719205492 + "x": 7.1905526925662695, + "y": 4.022995759457438, + "heading": 5.368720420708233e-18, + "angularVelocity": -4.9080421924352456e-17, + "velocityX": 2.8835500192539434, + "velocityY": -0.09566756344750614, + "timestamp": 9.934589015243784 }, { - "x": 2.4110936768539344, - "y": 6.744072759574757, - "heading": 0.5643107226604884, - "angularVelocity": 0.653664165942785, - "velocityX": 1.0320315606781885, - "velocityY": 0.5897332019890127, - "timestamp": 4.503745575523619 + "x": 7.3789017577018114, + "y": 4.017045849522277, + "heading": 1.5887833889908863e-18, + "angularVelocity": -5.144253446326957e-17, + "velocityX": 2.563310762337649, + "velocityY": -0.08097448299391075, + "timestamp": 10.008067841105605 }, { - "x": 2.50599401367539, - "y": 6.798301604540289, - "heading": 0.5436072588792712, - "angularVelocity": -0.2614573637418783, - "velocityX": 1.198465732389427, - "velocityY": 0.6848385851414024, - "timestamp": 4.5829304318417465 + "x": 7.54371362899692, + "y": 4.012025997027571, + "heading": -2.7250521222406746e-18, + "angularVelocity": -5.870855257463982e-17, + "velocityX": 2.2429845518359373, + "velocityY": -0.06831699385270525, + "timestamp": 10.081546666967427 }, { - "x": 2.577169265515479, - "y": 6.838973237821007, - "heading": 0.528079022912807, - "angularVelocity": -0.19610108155123726, - "velocityX": 0.8988492894921869, - "velocityY": 0.51362893325611, - "timestamp": 4.662115288159874 + "x": 7.684984887810538, + "y": 4.007845278835319, + "heading": -2.10421445779977e-18, + "angularVelocity": 8.449205021435769e-18, + "velocityX": 1.9226118158077505, + "velocityY": -0.056896910684361174, + "timestamp": 10.155025492829248 }, { - "x": 2.6246194330616937, - "y": 6.8660876598097635, - "heading": 0.5177266590936774, - "angularVelocity": -0.13073666229232678, - "velocityX": 0.5992328552770498, - "velocityY": 0.3424192863320269, - "timestamp": 4.741300144478001 + "x": 7.8027134339680275, + "y": 4.0044425825275765, + "heading": -1.435402908569355e-18, + "angularVelocity": 9.102099024937298e-18, + "velocityX": 1.6022104977409413, + "velocityY": -0.0463085285840207, + "timestamp": 10.22850431869107 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.06536905835734146, - "velocityX": 0.2996164266200211, - "velocityY": 0.1712096425840104, - "timestamp": 4.820485000796128 + "x": 7.896897856926731, + "y": 4.001774011196325, + "heading": -8.461914935171916e-19, + "angularVelocity": 8.01879191918765e-18, + "velocityX": 1.2817899830873647, + "velocityY": -0.03631755543113603, + "timestamp": 10.30198314455289 + }, + { + "x": 7.967537148527336, + "y": 3.999806507404438, + "heading": -3.275924545852618e-19, + "angularVelocity": 7.057802473697343e-18, + "velocityX": 0.9613557480279206, + "velocityY": -0.026776472933672434, + "timestamp": 10.375461970414712 + }, + { + "x": 8.014630554462236, + "y": 3.9985142796609896, + "heading": -5.3367647403100726e-20, + "angularVelocity": 3.732024892420789e-18, + "velocityX": 0.6409112473226126, + "velocityY": -0.01758639619362703, + "timestamp": 10.448940796276533 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -8.381816905081638e-20, - "velocityX": -7.007327358505968e-19, - "velocityY": 8.954594613208896e-19, - "timestamp": 4.899669857114255 + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 7.342940745159904e-44, + "angularVelocity": 7.262996758203436e-19, + "velocityX": 0.320458791984776, + "velocityY": -0.008677813220192914, + "timestamp": 10.522419622138354 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 1.3038141825006824e-44, + "angularVelocity": -8.034632634038792e-43, + "velocityX": -5.7118786505402475e-43, + "velocityY": 0, + "timestamp": 10.595898448000176 } ], "constraints": [ @@ -21914,20 +23835,13 @@ }, { "scope": [ - 5 + 2 ], "type": "StopPoint" }, { "scope": [ - 4, - 5 - ], - "type": "StraightLine" - }, - { - "scope": [ - 2 + 10 ], "type": "StopPoint" } @@ -21937,1971 +23851,4090 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "CenterLanePCBAFE": { + "SourceLanePGCSprint": { "waypoints": [ { - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 1.8129411935806274, - "y": 5.542999267578125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 6 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 11 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, + "x": 0.43324199318885803, + "y": 4.121134281158447, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 18 - }, - { - "x": 7.984478950500488, - "y": 5.483555793762207, - "heading": 0.7044936776046891, - "isInitialGuess": false, - "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 19 }, { - "x": 8.217175483703613, - "y": 5.698352336883545, - "heading": 0.7044936776046891, + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 22 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, + "x": 5.5030035972595215, + "y": 1.563418984413147, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 12 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -8.364664059409844e-30, - "velocityX": -6.773677606947568e-30, - "velocityY": -2.308132540143278e-30, - "timestamp": 0 - }, - { - "x": 1.3079899492454155, - "y": 5.5701428985236205, - "heading": -0.011966271801548406, - "angularVelocity": -0.16457159783011938, - "velocityX": 0.24776633609486393, - "velocityY": -0.2862183198381729, - "timestamp": 0.07271164623703327 - }, - { - "x": 1.3440211933652915, - "y": 5.528520469164234, - "heading": -0.03588810704483216, - "angularVelocity": -0.328995924069994, - "velocityX": 0.49553607960947427, - "velocityY": -0.5724313987522216, - "timestamp": 0.14542329247406655 - }, - { - "x": 1.398068831056723, - "y": 5.466087418740183, - "heading": -0.07173813692543105, - "angularVelocity": -0.49304384837450815, - "velocityX": 0.7433147299769343, - "velocityY": -0.8586389341691013, - "timestamp": 0.21813493871109982 - }, - { - "x": 1.4701339496615213, - "y": 5.382844295736326, - "heading": -0.11947590052159912, - "angularVelocity": -0.6565353154432494, - "velocityX": 0.9911083345055308, - "velocityY": -1.1448389262999967, - "timestamp": 0.2908465849481331 - }, - { - "x": 1.5602181142824867, - "y": 5.2787919544216955, - "heading": -0.17905308310612672, - "angularVelocity": -0.819362312243441, - "velocityX": 1.2389234638197153, - "velocityY": -1.431027169709696, - "timestamp": 0.3635582311851664 - }, - { - "x": 1.668323429942649, - "y": 5.153931763984036, - "heading": -0.25042016061718175, - "angularVelocity": -0.9815082068514337, - "velocityX": 1.4867675434162453, - "velocityY": -1.717196582745939, - "timestamp": 0.4362698774221997 - }, - { - "x": 1.7944527928954905, - "y": 5.008265979195592, - "heading": -0.33353344500312104, - "angularVelocity": -1.1430532616554276, - "velocityX": 1.7346514550472283, - "velocityY": -2.003334986008902, - "timestamp": 0.508981523659233 - }, - { - "x": 1.938611553409707, - "y": 4.8417992681651185, - "heading": -0.42836015839962444, - "angularVelocity": -1.3041475239812899, - "velocityX": 1.98260894865471, - "velocityY": -2.2894091890060286, - "timestamp": 0.5816931698962663 + "controlIntervalCount": 17 }, { - "x": 2.09250704020051, - "y": 4.700832043022275, - "heading": -0.5059805202146961, - "angularVelocity": -1.0675093444479318, - "velocityX": 2.116517707491784, - "velocityY": -1.9387159062918606, - "timestamp": 0.6544048161332996 + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 2.228385575629257, - "y": 4.5806756631537535, - "heading": -0.5718968876072178, - "angularVelocity": -0.906544835648882, - "velocityX": 1.8687313857908063, - "velocityY": -1.652505287385456, - "timestamp": 0.7271164623703329 + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 2.346242849494598, - "y": 4.481326236786965, - "heading": -0.6261128487399352, - "angularVelocity": -0.7456296746751315, - "velocityX": 1.6208857861127495, - "velocityY": -1.3663481918284743, - "timestamp": 0.7998281086073662 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 25 }, { - "x": 2.446077177803767, - "y": 4.402782122456306, - "heading": -0.6686283268670905, - "angularVelocity": -0.5847134582152551, - "velocityX": 1.3730170265673516, - "velocityY": -1.080213671266156, - "timestamp": 0.8725397548443995 + "x": 1.664683222770691, + "y": 3.8699920177459717, + "heading": -0.7701709445795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 2.5278876403134056, - "y": 4.3450423723526415, - "heading": -0.6994420265312672, - "angularVelocity": -0.423779425385222, - "velocityX": 1.12513561096113, - "velocityY": -0.7940921858762265, - "timestamp": 0.9452514010814328 + "x": 1.848745584487915, + "y": 4.087520122528076, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 }, { - "x": 2.591673696422735, - "y": 4.308106404528037, - "heading": -0.718552253950641, - "angularVelocity": -0.2628220980484028, - "velocityX": 0.8772467604995492, - "velocityY": -0.5079787039876672, - "timestamp": 1.017963047318466 + "x": 2.819255828857422, + "y": 4.10425329208374, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 2.637435055651834, - "y": 4.2919738867991235, - "heading": -0.7259572155509334, - "angularVelocity": -0.10184010376417504, - "velocityX": 0.6293539150825374, - "velocityY": -0.22186979060047113, - "timestamp": 1.0906746935554992 + "x": 1.664683222770691, + "y": 3.8699920177459717, + "heading": -0.7701709445795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 }, { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.05916659407406203, - "velocityX": 0.38145976628941564, - "velocityY": 0.06423731409121712, - "timestamp": 1.1633863397925324 + "x": 4.7937421798706055, + "y": 1.3935176134109497, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 }, { - "x": 2.6747076115982096, - "y": 4.323065260999168, - "heading": -0.7050775877835139, - "angularVelocity": 0.2234613634683759, - "velocityX": 0.12854299211755085, - "velocityY": 0.35614342430781004, - "timestamp": 1.237571545918339 - }, + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ { - "x": 2.6654811031724366, - "y": 4.37114137531458, - "heading": -0.6763165724415564, - "angularVelocity": 0.38769205939503093, - "velocityX": -0.1243712717061632, - "velocityY": 0.6480552770879449, - "timestamp": 1.3117567520441455 + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": 9.759657159056985e-29, + "angularVelocity": 1.5485227916478622e-29, + "velocityX": -1.3617494152189479e-27, + "velocityY": -5.3557690609839536e-27, + "timestamp": 0 }, { - "x": 2.6374924128586628, - "y": 4.440873637695316, - "heading": -0.6353766207501428, - "angularVelocity": 0.5518613996935385, - "velocityX": -0.3772812906679928, - "velocityY": 0.9399753133925751, - "timestamp": 1.3859419581699521 + "x": 0.4546918803213756, + "y": 4.109218526997661, + "heading": -0.008937300145995024, + "angularVelocity": -0.11893237262991248, + "velocityX": 0.28544257523423766, + "velocityY": -0.15856789979825608, + "timestamp": 0.07514606787342623 }, { - "x": 2.590742000006679, - "y": 4.532262849422754, - "heading": -0.5822609370446827, - "angularVelocity": 0.7159875462663353, - "velocityX": -0.6301851176982644, - "velocityY": 1.231906150790299, - "timestamp": 1.4601271642957587 + "x": 0.4975916635116252, + "y": 4.0853873108981436, + "heading": -0.02681241329663613, + "angularVelocity": -0.23787157008333967, + "velocityX": 0.5708852692400183, + "velocityY": -0.3171319108760033, + "timestamp": 0.15029213574685246 }, { - "x": 2.525230488767617, - "y": 4.645309988591984, - "heading": -0.5169687243847326, - "angularVelocity": 0.8801244354174206, - "velocityX": -0.8830805313123746, - "velocityY": 1.5238501726898892, - "timestamp": 1.5343123704215653 + "x": 0.5619414537480726, + "y": 4.049641027140498, + "heading": -0.05362171709802378, + "angularVelocity": -0.3567625633658495, + "velocityX": 0.8563294402155045, + "velocityY": -0.4756906750976654, + "timestamp": 0.2254382036202787 }, { - "x": 2.4409586345992826, - "y": 4.780016081558229, - "heading": -0.4394900359726193, - "angularVelocity": 1.0443954051766633, - "velocityX": -1.1359657615273402, - "velocityY": 1.8158080296257462, - "timestamp": 1.608497576547372 + "x": 0.6477414523773108, + "y": 4.001980177860296, + "heading": -0.08935853104911338, + "angularVelocity": -0.4755646564406219, + "velocityX": 1.1417762905939028, + "velocityY": -0.6342427571923064, + "timestamp": 0.3005842714937049 + }, + { + "x": 0.7549919358777417, + "y": 3.9424053758532884, + "heading": -0.13401442200652888, + "angularVelocity": -0.5942545261667251, + "velocityX": 1.4272268201854599, + "velocityY": -0.7927866845588466, + "timestamp": 0.3757303393671312 + }, + { + "x": 0.8836932393269187, + "y": 3.8709173416225426, + "heading": -0.18758057158075359, + "angularVelocity": -0.7128270459134325, + "velocityX": 1.7126818087934759, + "velocityY": -0.9513210238912064, + "timestamp": 0.45087640724055744 + }, + { + "x": 1.0338457421402332, + "y": 3.7875168932975942, + "heading": -0.2500489995023012, + "angularVelocity": -0.8312933688928013, + "velocityX": 1.9981418464400145, + "velocityY": -1.1098444760333412, + "timestamp": 0.5260224751139837 + }, + { + "x": 1.2054498603190875, + "y": 3.692204931169302, + "heading": -0.3214134496146474, + "angularVelocity": -0.9496764385935714, + "velocityX": 2.283607419990346, + "velocityY": -1.268355947630322, + "timestamp": 0.60116854298741 + }, + { + "x": 1.3985060453231266, + "y": 3.584982424979261, + "heading": -0.40166985798592575, + "angularVelocity": -1.0680054278616389, + "velocityX": 2.569079001302066, + "velocityY": -1.4268545144723166, + "timestamp": 0.6763146108608362 + }, + { + "x": 1.5915937569821017, + "y": 3.4778198154599655, + "heading": -0.48176859267037747, + "angularVelocity": -1.0659071984893158, + "velocityX": 2.5694985396202785, + "velocityY": -1.4260574445464886, + "timestamp": 0.7514606787342625 + }, + { + "x": 1.7632284858900344, + "y": 3.382566036520045, + "heading": -0.552976547680448, + "angularVelocity": -0.9475938931363788, + "velocityX": 2.2840147697019764, + "velocityY": -1.2675816797275836, + "timestamp": 0.8266067466076887 + }, + { + "x": 1.9134098098562773, + "y": 3.299220400739088, + "heading": -0.6152918418017485, + "angularVelocity": -0.8292555536806328, + "velocityX": 1.9985253815170176, + "velocityY": -1.1091150626981823, + "timestamp": 0.901752814481115 + }, + { + "x": 2.04213735776467, + "y": 3.2277823059512354, + "heading": -0.6687115003742338, + "angularVelocity": -0.7108776291856513, + "velocityX": 1.7130310547348613, + "velocityY": -0.9506564589404838, + "timestamp": 0.9768988823545413 }, { - "x": 2.337926784653071, - "y": 4.936381539754754, - "heading": -0.3497973159677186, - "angularVelocity": 1.2090378215045976, - "velocityX": -1.3888463122287895, - "velocityY": 2.107771432581833, - "timestamp": 1.6826827826731785 + "x": 2.1494108084857926, + "y": 3.1682512394298143, + "heading": -0.7132320418113052, + "angularVelocity": -0.5924533737688112, + "velocityX": 1.4275324545498602, + "velocityY": -0.7922046782500088, + "timestamp": 1.0520449502279674 }, { - "x": 2.2066781223966365, - "y": 5.088037063238321, - "heading": -0.26257787440108193, - "angularVelocity": 1.1756985809634768, - "velocityX": -1.7692026362756765, - "velocityY": 2.044282565348452, - "timestamp": 1.756867988798985 + "x": 2.235229893671302, + "y": 3.120626784362015, + "heading": -0.7488501306819726, + "angularVelocity": -0.4739847323836216, + "velocityX": 1.142030283341778, + "velocityY": -0.6337584442610695, + "timestamp": 1.1271910181013935 }, { - "x": 2.094180452686652, - "y": 5.218026595083209, - "heading": -0.18768356705291486, - "angularVelocity": 1.009558526050363, - "velocityX": -1.5164434471606913, - "velocityY": 1.7522298398549971, - "timestamp": 1.8310531949247917 + "x": 2.2995943987852447, + "y": 3.0849086278583613, + "heading": -0.7755632624607874, + "angularVelocity": -0.35548276223593744, + "velocityX": 0.8565252572144713, + "velocityY": -0.4753163740226105, + "timestamp": 1.2023370859748197 }, { - "x": 2.0004335399095625, - "y": 5.326350877049268, - "heading": -0.12517882612598913, - "angularVelocity": 0.8425499394477148, - "velocityX": -1.2636874340540831, - "velocityY": 1.460187113126797, - "timestamp": 1.9052384010505983 + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080856, + "angularVelocity": -0.2369668826490278, + "velocityX": 0.5710180813549185, + "velocityY": -0.31687698560575434, + "timestamp": 1.2774831538482458 }, { - "x": 1.9254366143080366, - "y": 5.4130101547737155, - "heading": -0.07512185913342963, - "angularVelocity": 0.6747567285776663, - "velocityX": -1.0109417971010437, - "velocityY": 1.1681476975361738, - "timestamp": 1.9794236071764049 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288599550668, + "velocityX": 0.28550943973314763, + "velocityY": -0.15843872456938438, + "timestamp": 1.352629221721672 }, { - "x": 1.869189070291752, - "y": 5.478004602849777, - "heading": -0.037557912625022415, - "angularVelocity": 0.5063536043287745, - "velocityX": -0.7582043233686032, - "velocityY": 0.876110635424555, - "timestamp": 2.0536088133022115 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.820153695476826e-18, + "velocityX": -5.7963239407122954e-18, + "velocityY": -1.1530643297922074e-18, + "timestamp": 1.4277752895950981 + }, + { + "x": 2.37735740952237, + "y": 3.0408026970784268, + "heading": -0.7860869247798871, + "angularVelocity": 0.2648554274741457, + "velocityX": 0.21924714610297322, + "velocityY": -0.1372563412236913, + "timestamp": 1.4888859391070042 + }, + { + "x": 2.404179793787175, + "y": 3.024019444447085, + "heading": -0.7541504516362038, + "angularVelocity": 0.5226007806947081, + "velocityX": 0.4389150578342217, + "velocityY": -0.2746371175137346, + "timestamp": 1.5499965886189102 + }, + { + "x": 2.4444554974169383, + "y": 2.998832316814256, + "heading": -0.7069723408476191, + "angularVelocity": 0.7720112806098135, + "velocityX": 0.6590619466729163, + "velocityY": -0.41215611082519754, + "timestamp": 1.6111072381308162 + }, + { + "x": 2.4982188560395806, + "y": 2.96523040511413, + "heading": -0.645190074686331, + "angularVelocity": 1.0109901736398696, + "velocityX": 0.8797706954852063, + "velocityY": -0.5498536174710421, + "timestamp": 1.6722178876427223 + }, + { + "x": 2.5655102024042766, + "y": 2.923198127539445, + "heading": -0.5696285601591381, + "angularVelocity": 1.2364704864161304, + "velocityX": 1.101139439723772, + "velocityY": -0.6878061010707456, + "timestamp": 1.7333285371546283 + }, + { + "x": 2.646375841205288, + "y": 2.8727129664624207, + "heading": -0.48135684674506, + "angularVelocity": 1.4444571301255718, + "velocityX": 1.3232659028645477, + "velocityY": -0.8261270577264651, + "timestamp": 1.7944391866665343 + }, + { + "x": 2.740867207337065, + "y": 2.813744425609022, + "heading": -0.38173698900245784, + "angularVelocity": 1.6301554399809315, + "velocityX": 1.5462340342720005, + "velocityY": -0.9649470480903679, + "timestamp": 1.8555498361784404 + }, + { + "x": 2.849040211493812, + "y": 2.746255741762665, + "heading": -0.2724850390795677, + "angularVelocity": 1.7877726843928354, + "velocityX": 1.7701170748589676, + "velocityY": -1.1043686229060403, + "timestamp": 1.9166604856903464 + }, + { + "x": 2.9709557136972373, + "y": 2.6702088910620962, + "heading": -0.155828372393994, + "angularVelocity": 1.9089416921161393, + "velocityX": 1.994996014232719, + "velocityY": -1.2444124110602532, + "timestamp": 1.9777711352022525 + }, + { + "x": 3.1066769296559538, + "y": 2.585572206493352, + "heading": -0.03496320783780511, + "angularVelocity": 1.977808541089754, + "velocityX": 2.2209094002882996, + "velocityY": -1.3849743906298195, + "timestamp": 2.0388817847141585 + }, + { + "x": 3.2562355055408387, + "y": 2.4923383100856262, + "heading": 0.08475597327951304, + "angularVelocity": 1.959055943170691, + "velocityX": 2.4473406366879895, + "velocityY": -1.5256571015427038, + "timestamp": 2.0999924342260647 + }, + { + "x": 3.4194188611023764, + "y": 2.3906298265628587, + "heading": 0.19293288100296513, + "angularVelocity": 1.7701809518876104, + "velocityX": 2.6702932609110017, + "velocityY": -1.6643332109070865, + "timestamp": 2.161103083737971 + }, + { + "x": 3.5932872633720114, + "y": 2.2801851899216916, + "heading": 0.26578166446591744, + "angularVelocity": 1.192080006427692, + "velocityX": 2.845140800471438, + "velocityY": -1.8072895235657866, + "timestamp": 2.2222137332498773 + }, + { + "x": 3.7772765002319786, + "y": 2.1608722925529302, + "heading": 0.30020319888903974, + "angularVelocity": 0.5632657269731189, + "velocityX": 3.0107557083666587, + "velocityY": -1.9524076134310657, + "timestamp": 2.2833243827617835 + }, + { + "x": 3.9732507395108834, + "y": 2.038358982543431, + "heading": 0.30020324669419357, + "angularVelocity": 7.822720631834703e-7, + "velocityX": 3.2068754111462736, + "velocityY": -2.0047783976771947, + "timestamp": 2.3444350322736898 + }, + { + "x": 4.174667645417682, + "y": 1.9250159473015482, + "heading": 0.3002032939445946, + "angularVelocity": 7.731942209741845e-7, + "velocityX": 3.2959379014218597, + "velocityY": -1.8547182225546543, + "timestamp": 2.405545681785596 + }, + { + "x": 4.383569049898422, + "y": 1.8261442295125712, + "heading": 0.30020334201253857, + "angularVelocity": 7.865722968351813e-7, + "velocityX": 3.4184124395542623, + "velocityY": -1.6179130573585891, + "timestamp": 2.4666563312975023 + }, + { + "x": 4.59891708073721, + "y": 1.7422357169202292, + "heading": 0.3002033920988207, + "angularVelocity": 8.195998987503142e-7, + "velocityX": 3.52390348587004, + "velocityY": -1.3730587591937538, + "timestamp": 2.5277669808094085 + }, + { + "x": 4.819641568305972, + "y": 1.6737075280608862, + "heading": 0.30020344564176327, + "angularVelocity": 8.761638607213102e-7, + "velocityX": 3.6118825332687328, + "velocityY": -1.1213788334223447, + "timestamp": 2.588877630321315 + }, + { + "x": 5.044645579459654, + "y": 1.6209002757550093, + "heading": 0.30020350452029126, + "angularVelocity": 9.634740995599556e-7, + "velocityX": 3.681911629982681, + "velocityY": -0.8641252012153561, + "timestamp": 2.649988279833221 + }, + { + "x": 5.272810899090047, + "y": 1.584076421741111, + "heading": 0.3002035713939988, + "angularVelocity": 0.0000010943052979537505, + "velocityX": 3.733642523075129, + "velocityY": -0.6025767081190017, + "timestamp": 2.7110989293451273 }, { - "x": 1.8316905948823166, - "y": 5.5213343174096465, - "heading": -0.012514751718529437, - "angularVelocity": 0.3375762124977515, - "velocityX": -0.5054710685139713, - "velocityY": 0.5840748691542834, - "timestamp": 2.127794019428018 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3002036452391599, + "angularVelocity": 0.0000012083844913604929, + "velocityX": 3.7668180588495743, + "velocityY": -0.33803334595452383, + "timestamp": 2.7722095788570336 + }, + { + "x": 5.642203177619129, + "y": 1.5568731039887571, + "heading": 0.3002037251666547, + "angularVelocity": 0.0000021691766198010436, + "velocityX": 3.7777797970827858, + "velocityY": -0.177650642031358, + "timestamp": 2.8090565069037923 + }, + { + "x": 5.781555187145968, + "y": 1.5562486567630798, + "heading": 0.30020379895395954, + "angularVelocity": 0.0000020025361337225055, + "velocityX": 3.78191661866635, + "velocityY": -0.016947063399286564, + "timestamp": 2.845903434950551 + }, + { + "x": 5.9208078464820115, + "y": 1.5615467685843438, + "heading": 0.30020386782175307, + "angularVelocity": 0.0000018690240183201108, + "velocityX": 3.7792203235866277, + "velocityY": 0.14378706997068585, + "timestamp": 2.8827503629973097 + }, + { + "x": 6.059709556234372, + "y": 1.5727578639536914, + "heading": 0.3002039327408759, + "angularVelocity": 0.0000017618598429234572, + "velocityX": 3.7696957959723445, + "velocityY": 0.30426133096145186, + "timestamp": 2.9195972910440684 + }, + { + "x": 6.198009351638548, + "y": 1.5898616832743162, + "heading": 0.30020399449987484, + "angularVelocity": 0.0000016760962775678482, + "velocityX": 3.753360259196494, + "velocityY": 0.46418576058553174, + "timestamp": 2.956444219090827 + }, + { + "x": 6.335457356092555, + "y": 1.6128273194464042, + "heading": 0.30020405375523124, + "angularVelocity": 0.0000016081491612309129, + "velocityX": 3.730243245233006, + "velocityY": 0.6232713930166671, + "timestamp": 2.993291147137586 + }, + { + "x": 6.471805232718414, + "y": 1.6416132738069629, + "heading": 0.3002041110675489, + "angularVelocity": 0.0000015554164405322294, + "velocityX": 3.700386541120455, + "velocityY": 0.7812307805966762, + "timestamp": 3.0301380751843445 + }, + { + "x": 6.60680663253268, + "y": 1.676167531284266, + "heading": 0.3002041691027596, + "angularVelocity": 0.0000015750352547385845, + "velocityX": 3.6638440969339445, + "velocityY": 0.937778515306732, + "timestamp": 3.0669850032311032 + }, + { + "x": 6.740176057879373, + "y": 1.7164157557118485, + "heading": 0.3003755003099619, + "angularVelocity": 0.004649809801927131, + "velocityX": 3.6195534449289073, + "velocityY": 1.0923088181600296, + "timestamp": 3.103831931277862 + }, + { + "x": 6.869837525657242, + "y": 1.7607930294165437, + "heading": 0.3093430143729343, + "angularVelocity": 0.2433720947264084, + "velocityX": 3.5189220554106844, + "velocityY": 1.2043683437702197, + "timestamp": 3.1406788593246207 + }, + { + "x": 6.995165288352465, + "y": 1.807928452248917, + "heading": 0.3301850856624388, + "angularVelocity": 0.5656393190514163, + "velocityX": 3.4013083135772337, + "velocityY": 1.2792225927914125, + "timestamp": 3.1775257873713794 + }, + { + "x": 7.114963240628345, + "y": 1.8557254140007795, + "heading": 0.35927322814233364, + "angularVelocity": 0.7894319559824341, + "velocityX": 3.2512331048020355, + "velocityY": 1.2971762989633377, + "timestamp": 3.214372715418138 + }, + { + "x": 7.228886146559884, + "y": 1.9032191557267804, + "heading": 0.3911508310575271, + "angularVelocity": 0.8651359721153189, + "velocityX": 3.091788433135363, + "velocityY": 1.2889471183522156, + "timestamp": 3.251219643464897 + }, + { + "x": 7.336942556890349, + "y": 1.949894399055616, + "heading": 0.423151819771819, + "angularVelocity": 0.8684845768875223, + "velocityX": 2.9325758226938556, + "velocityY": 1.2667336411221237, + "timestamp": 3.2880665715116555 + }, + { + "x": 7.43919043499382, + "y": 1.9954619855368936, + "heading": 0.45375826572504047, + "angularVelocity": 0.8306376562622595, + "velocityX": 2.7749362979111796, + "velocityY": 1.2366726046592738, + "timestamp": 3.324913499558414 + }, + { + "x": 7.535687560612936, + "y": 2.0397400223416313, + "heading": 0.48197582868140687, + "angularVelocity": 0.7658050332054913, + "velocityX": 2.618864875157601, + "velocityY": 1.201675123325048, + "timestamp": 3.361760427605173 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.368719711095728e-29, - "angularVelocity": 0.1686960564308085, - "velocityX": -0.2527377395078362, - "velocityY": 0.2920386866937519, - "timestamp": 2.2019792255538246 + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6818119458710672, + "velocityX": 2.464149969521805, + "velocityY": 1.163303151067446, + "timestamp": 3.3986073556519316 + }, + { + "x": 7.766065583230135, + "y": 2.1519617180113952, + "heading": 0.5396264377138885, + "angularVelocity": 0.5126079654708053, + "velocityX": 2.1996686083558368, + "velocityY": 1.0930061650047849, + "timestamp": 3.4620631259727794 + }, + { + "x": 7.889178097615864, + "y": 2.215679412144434, + "heading": 0.5626262771579723, + "angularVelocity": 0.3624546566496878, + "velocityX": 1.9401311143689866, + "velocityY": 1.004127659484183, + "timestamp": 3.5255188962936272 + }, + { + "x": 7.996082408378713, + "y": 2.2729729622929304, + "heading": 0.5769453477724344, + "angularVelocity": 0.22565435014123394, + "velocityX": 1.6847059018008188, + "velocityY": 0.90288952224213, + "timestamp": 3.588974666614475 + }, + { + "x": 8.086990111713977, + "y": 2.3232868849538337, + "heading": 0.5832040110121259, + "angularVelocity": 0.09863032483958853, + "velocityX": 1.432615235393288, + "velocityY": 0.7928975159627525, + "timestamp": 3.652430436935323 + }, + { + "x": 8.16207340516984, + "y": 2.3662086171292933, + "heading": 0.581874089552229, + "angularVelocity": -0.020958243090781536, + "velocityX": 1.183238231546559, + "velocityY": 0.676403926048603, + "timestamp": 3.7158862072561707 + }, + { + "x": 8.221474033046453, + "y": 2.401420292157025, + "heading": 0.5733258497363348, + "angularVelocity": -0.1347117807044565, + "velocityX": 0.9360949772773745, + "velocityY": 0.5549010728211649, + "timestamp": 3.7793419775770185 + }, + { + "x": 8.265310204045273, + "y": 2.4286697945211704, + "heading": 0.5578573342178689, + "angularVelocity": -0.24376846172150818, + "velocityX": 0.6908145748948221, + "velocityY": 0.4294251291311274, + "timestamp": 3.8427977478978663 + }, + { + "x": 8.293681756385627, + "y": 2.447752418352113, + "heading": 0.5357134031725193, + "angularVelocity": -0.34896638924064666, + "velocityX": 0.44710752382171043, + "velocityY": 0.30072322397878726, + "timestamp": 3.906253518218714 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 1.1956060311838367e-29, - "angularVelocity": -1.025164181333131e-28, - "velocityX": 1.21885672142624e-30, - "velocityY": -5.1343833235782786e-30, - "timestamp": 2.2761644316796312 + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.45094242234392296, + "velocityX": 0.20474492941705758, + "velocityY": 0.16935099752034305, + "timestamp": 3.969709288539562 + }, + { + "x": 8.303680612059921, + "y": 2.4605220907329786, + "heading": 0.4706360870687623, + "angularVelocity": -0.5539055745477068, + "velocityX": -0.04547302080206286, + "velocityY": 0.030737357256443772, + "timestamp": 4.035537146208892 + }, + { + "x": 8.28417079095848, + "y": 2.4534909416021047, + "heading": 0.42765554839737563, + "angularVelocity": -0.6529232485020114, + "velocityX": -0.2963763639315537, + "velocityY": -0.1068111492583101, + "timestamp": 4.101365003878222 + }, + { + "x": 8.248092493080108, + "y": 2.437487576077448, + "heading": 0.3784580606750378, + "angularVelocity": -0.7473657728536394, + "velocityX": -0.5480703634561933, + "velocityY": -0.24310931710774963, + "timestamp": 4.1671928615475515 + }, + { + "x": 8.195385029545998, + "y": 2.412609981570135, + "heading": 0.3233968459430461, + "angularVelocity": -0.8364424528074197, + "velocityX": -0.8006862960492162, + "velocityY": -0.37791894477683524, + "timestamp": 4.233020719216881 + }, + { + "x": 8.125976752501963, + "y": 2.378976757579925, + "heading": 0.2628923564512202, + "angularVelocity": -0.9191319850595123, + "velocityX": -1.0543906410063166, + "velocityY": -0.5109269112046505, + "timestamp": 4.298848576886211 + }, + { + "x": 8.039781850826214, + "y": 2.336734306251694, + "heading": 0.19745501482460134, + "angularVelocity": -0.9940676173198235, + "velocityX": -1.3093985544650038, + "velocityY": -0.641710862602064, + "timestamp": 4.364676434555541 + }, + { + "x": 7.936695821305385, + "y": 2.286067717588345, + "heading": 0.12772072746128918, + "angularVelocity": -1.0593431084086873, + "velocityX": -1.565993990548153, + "velocityY": -0.7696830864200519, + "timestamp": 4.430504292224871 + }, + { + "x": 7.816588930914964, + "y": 2.2272180395232652, + "heading": 0.05450854772010129, + "angularVelocity": -1.1121762477665882, + "velocityX": -1.8245602187716414, + "velocityY": -0.8939935180740173, + "timestamp": 4.496332149894201 + }, + { + "x": 7.679296617994528, + "y": 2.1605113660425403, + "heading": -0.02108172308249024, + "angularVelocity": -1.1483021547245422, + "velocityX": -2.085626325712903, + "velocityY": -1.01335021133165, + "timestamp": 4.562160007563531 + }, + { + "x": 7.52460540494712, + "y": 2.086411774056454, + "heading": -0.09749530920005847, + "angularVelocity": -1.16080925041513, + "velocityX": -2.34993540006212, + "velocityY": -1.1256570486967377, + "timestamp": 4.6279878652328605 + }, + { + "x": 7.352233710537335, + "y": 2.0056280451301003, + "heading": -0.17238692358324137, + "angularVelocity": -1.1376887693867086, + "velocityX": -2.6185220135167313, + "velocityY": -1.2271966882493952, + "timestamp": 4.69381572290219 + }, + { + "x": 7.161817946345765, + "y": 1.9193611743859953, + "heading": -0.24189742475427997, + "angularVelocity": -1.055943541717672, + "velocityX": -2.8926319484385736, + "velocityY": -1.3104918464375106, + "timestamp": 4.75964358057152 + }, + { + "x": 6.953014973362115, + "y": 1.8300106922230066, + "heading": -0.298719434373675, + "angularVelocity": -0.8631909290566133, + "velocityX": -3.1719545550536123, + "velocityY": -1.3573354097564532, + "timestamp": 4.82547143824085 + }, + { + "x": 6.727431268978415, + "y": 1.7436413956339059, + "heading": -0.3201876383615551, + "angularVelocity": -0.32612642653085444, + "velocityX": -3.4268729436231227, + "velocityY": -1.3120478114745338, + "timestamp": 4.89129929591018 + }, + { + "x": 6.489449879790782, + "y": 1.6705324422161696, + "heading": -0.3201878815696182, + "angularVelocity": -0.0000036946069898928346, + "velocityX": -3.61520787115807, + "velocityY": -1.1106081225517261, + "timestamp": 4.95712715357951 + }, + { + "x": 6.246606826324383, + "y": 1.6156931769447658, + "heading": -0.3201879117478121, + "angularVelocity": -4.584410758613646e-7, + "velocityX": -3.689062078949349, + "velocityY": -0.8330707881589553, + "timestamp": 5.02295501124884 + }, + { + "x": 6.0003025647982255, + "y": 1.5794398654466013, + "heading": -0.32018794628515007, + "angularVelocity": -5.246614307457641e-7, + "velocityX": -3.741641764546064, + "velocityY": -0.5507290193199733, + "timestamp": 5.0887828689181696 + }, + { + "x": 5.7519574472803, + "y": 1.5619815744715873, + "heading": -0.32018798749762734, + "angularVelocity": -6.260643854910128e-7, + "velocityX": -3.772644687381865, + "velocityY": -0.26521128885450773, + "timestamp": 5.154610726587499 }, { - "x": 1.8602093956334294, - "y": 5.5430660724686875, - "heading": -1.6999094202020734e-20, - "angularVelocity": -1.7890242544799916e-19, - "velocityX": 0.49746156397034486, - "velocityY": 0.0007030702226192673, - "timestamp": 2.3711832340882935 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.32018803965163717, + "angularVelocity": -7.922787054710825e-7, + "velocityX": -3.7818920261895626, + "velocityY": 0.021835891254127007, + "timestamp": 5.220438584256829 + }, + { + "x": 5.250083628410863, + "y": 1.584512642850435, + "heading": -0.3201880864656727, + "angularVelocity": -6.975962896264183e-7, + "velocityX": -3.768870382202017, + "velocityY": 0.31432577229262343, + "timestamp": 5.287546216836352 + }, + { + "x": 4.999553273526972, + "y": 1.6251081857259102, + "heading": -0.32018812296137045, + "angularVelocity": -5.438382542161688e-7, + "velocityX": -3.7332617059171653, + "velocityY": 0.6049318283932773, + "timestamp": 5.354653849415875 + }, + { + "x": 4.752913977063861, + "y": 1.6849623155206188, + "heading": -0.3201881529794151, + "angularVelocity": -4.473119302496649e-7, + "velocityX": -3.675279353221748, + "velocityY": 0.8919124024198194, + "timestamp": 5.421761481995398 + }, + { + "x": 4.511643860653374, + "y": 1.7637163133941143, + "heading": -0.3201881787520496, + "angularVelocity": -3.8404922716455346e-7, + "velocityX": -3.595270867655492, + "velocityY": 1.1735475510952014, + "timestamp": 5.488869114574921 + }, + { + "x": 4.277188862383835, + "y": 1.86089818593956, + "heading": -0.3201882016908444, + "angularVelocity": -3.41820951724987e-7, + "velocityX": -3.4937158301883047, + "velocityY": 1.4481493208732221, + "timestamp": 5.555976747154444 + }, + { + "x": 4.050954064298834, + "y": 1.9759254853688604, + "heading": -0.32018822275897413, + "angularVelocity": -3.1394535808439923e-7, + "velocityX": -3.3712230544999895, + "velocityY": 1.7140717830120589, + "timestamp": 5.623084379733967 + }, + { + "x": 3.834295246075004, + "y": 2.1081087693328064, + "heading": -0.32018824266587215, + "angularVelocity": -2.966413400864343e-7, + "velocityX": -3.2285272165887804, + "velocityY": 1.969720565053586, + "timestamp": 5.69019201231349 + }, + { + "x": 3.6285105963453295, + "y": 2.256655537498403, + "heading": -0.32018826197928, + "angularVelocity": -2.8779748460931035e-7, + "velocityX": -3.066486505626901, + "velocityY": 2.213559955189428, + "timestamp": 5.757299644893013 + }, + { + "x": 3.4323361610843617, + "y": 2.417680552436319, + "heading": -0.3201882811600919, + "angularVelocity": -2.858216149634079e-7, + "velocityX": -2.9232805229494647, + "velocityY": 2.399503733753405, + "timestamp": 5.824407277472536 + }, + { + "x": 3.2362856931678534, + "y": 2.578856476556199, + "heading": -0.32018830034052725, + "angularVelocity": -2.858160039511642e-7, + "velocityX": -2.921433231669861, + "velocityY": 2.401752497063368, + "timestamp": 5.891514910052059 + }, + { + "x": 3.0402351711109183, + "y": 2.7400323348209366, + "heading": -0.3201883195209977, + "angularVelocity": -2.858165259418001e-7, + "velocityX": -2.9214340384398776, + "velocityY": 2.401751515727269, + "timestamp": 5.958622542631582 + }, + { + "x": 2.8448193652926044, + "y": 2.899781496680513, + "heading": -0.32456406822795664, + "angularVelocity": -0.06520493331028634, + "velocityX": -2.911975855902028, + "velocityY": 2.380491692510139, + "timestamp": 6.025730175211105 + }, + { + "x": 2.66318575993582, + "y": 3.04911523833719, + "heading": -0.3692876119349906, + "angularVelocity": -0.6664449629337832, + "velocityX": -2.706601296678858, + "velocityY": 2.2252869892216163, + "timestamp": 6.0928378077906284 + }, + { + "x": 2.496643428698106, + "y": 3.186035937240345, + "heading": -0.4222622822262224, + "angularVelocity": -0.7893985863449552, + "velocityX": -2.4817196619231154, + "velocityY": 2.040314844081317, + "timestamp": 6.1599454403701515 + }, + { + "x": 2.3452748100493945, + "y": 3.3104804771184444, + "heading": -0.4767233164983405, + "angularVelocity": -0.8115475420412368, + "velocityX": -2.255609575696765, + "velocityY": 1.8544021759467144, + "timestamp": 6.2270530729496745 + }, + { + "x": 2.209079907655855, + "y": 3.4224492823331634, + "heading": -0.5296433021172221, + "angularVelocity": -0.7885837062747133, + "velocityX": -2.0294994348392206, + "velocityY": 1.6684958314098723, + "timestamp": 6.2941607055291975 + }, + { + "x": 2.088048432415951, + "y": 3.5219507247047837, + "heading": -0.5792907495482946, + "angularVelocity": -0.7398181923977142, + "velocityX": -1.803542616355617, + "velocityY": 1.4827142390056927, + "timestamp": 6.3612683381087205 + }, + { + "x": 1.9821698412647601, + "y": 3.6089936285690154, + "heading": -0.624538681260696, + "angularVelocity": -0.6742590965160707, + "velocityX": -1.5777429046647484, + "velocityY": 1.297064141863228, + "timestamp": 6.4283759706882435 + }, + { + "x": 1.8914349570352331, + "y": 3.6835858822586136, + "heading": -0.6645923745497577, + "angularVelocity": -0.5968574922025148, + "velocityX": -1.3520799459287431, + "velocityY": 1.1115315921956592, + "timestamp": 6.4954836032677665 + }, + { + "x": 1.815836039628641, + "y": 3.7457342610555044, + "heading": -0.698860244587487, + "angularVelocity": -0.5106404252470358, + "velocityX": -1.1265323257679698, + "velocityY": 0.926100003948804, + "timestamp": 6.5625912358472895 + }, + { + "x": 1.7553665690239355, + "y": 3.7954445247907986, + "heading": -0.7268845476309118, + "angularVelocity": -0.41760231982876106, + "velocityX": -0.9010818632746272, + "velocityY": 0.7407542454487245, + "timestamp": 6.6296988684268126 + }, + { + "x": 1.7100210087875185, + "y": 3.8327215653417337, + "heading": -0.7483006908429537, + "angularVelocity": -0.31913125808251036, + "velocityX": -0.6757139015846285, + "velocityY": 0.5554813829374979, + "timestamp": 6.696806501006336 + }, + { + "x": 1.6797946046172731, + "y": 3.8575695457400823, + "heading": -0.7628116421295846, + "angularVelocity": -0.21623399200434476, + "velocityX": -0.4504167858168276, + "velocityY": 0.37027055557210875, + "timestamp": 6.763914133585859 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.10966416437475777, + "velocityX": -0.22518126874279432, + "velocityY": 0.1851126545280661, + "timestamp": 6.831021766165382 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -1.211900873027185e-19, + "velocityX": -5.462077711506373e-19, + "velocityY": -3.512888080115249e-19, + "timestamp": 6.898129398744905 + }, + { + "x": 1.659892349205253, + "y": 3.8800826826438297, + "heading": -0.7264777443526473, + "angularVelocity": 0.7466474211356967, + "velocityX": -0.08186842286768115, + "velocityY": 0.1724334423754288, + "timestamp": 6.95664858596781 + }, + { + "x": 1.6562452238185605, + "y": 3.90221669421699, + "heading": -0.6460681306884017, + "angularVelocity": 1.3740726329290587, + "velocityX": -0.06232358239700476, + "velocityY": 0.3782351160970439, + "timestamp": 7.015167773190715 + }, + { + "x": 1.6553319250883793, + "y": 3.9350953760295733, + "heading": -0.5183992679116819, + "angularVelocity": 2.1816581677803772, + "velocityX": -0.01560682527428515, + "velocityY": 0.5618444714097892, + "timestamp": 7.07368696041362 + }, + { + "x": 1.6623538133314129, + "y": 3.9788519148323376, + "heading": -0.3621106955703112, + "angularVelocity": 2.670723565350507, + "velocityX": 0.11999292157436166, + "velocityY": 0.7477297768352748, + "timestamp": 7.132206147636525 + }, + { + "x": 1.6832034751641156, + "y": 4.019420947962781, + "heading": -0.22540240918746318, + "angularVelocity": 2.3361275655130602, + "velocityX": 0.35628761816674753, + "velocityY": 0.6932603656286723, + "timestamp": 7.19072533485943 + }, + { + "x": 1.7146647081648767, + "y": 4.052413452071288, + "heading": -0.11878752772286896, + "angularVelocity": 1.8218790541038834, + "velocityX": 0.5376225216683612, + "velocityY": 0.5637895137339223, + "timestamp": 7.249244522082336 + }, + { + "x": 1.7541026264736177, + "y": 4.074977116543675, + "heading": -0.0408151972626467, + "angularVelocity": 1.332423332593777, + "velocityX": 0.6739314091721454, + "velocityY": 0.38557720199428364, + "timestamp": 7.307763709305241 + }, + { + "x": 1.799090355046403, + "y": 4.0866639858943925, + "heading": 1.7934233852716553e-24, + "angularVelocity": 0.6974669198185859, + "velocityX": 0.7687688552717376, + "velocityY": 0.199710042215758, + "timestamp": 7.366282896528146 + }, + { + "x": 1.848745584487915, + "y": 4.087520122528076, + "heading": 7.573339964499894e-25, + "angularVelocity": -5.2358045608260825e-25, + "velocityX": 0.848529034628775, + "velocityY": 0.014630015800164336, + "timestamp": 7.424802083751051 + }, + { + "x": 1.9382598731728313, + "y": 4.089063493944204, + "heading": 6.819777064270339e-25, + "angularVelocity": -2.006050179968467e-26, + "velocityX": 1.1795649340806893, + "velocityY": 0.02033761122914328, + "timestamp": 7.500689630166358 + }, + { + "x": 2.0528956622357724, + "y": 4.091040000742217, + "heading": 6.067470023829544e-25, + "angularVelocity": -1.8954686232133377e-26, + "velocityX": 1.510600809724148, + "velocityY": 0.026045206247625866, + "timestamp": 7.576577176581664 + }, + { + "x": 2.1926529480236727, + "y": 4.09344964285913, + "heading": 5.320081952060173e-25, + "angularVelocity": -2.1711320260805291e-26, + "velocityX": 1.8416366372297248, + "velocityY": 0.03175280043613335, + "timestamp": 7.652464722996971 + }, + { + "x": 2.3575317192504395, + "y": 4.096292420100353, + "heading": 4.5695624647021355e-25, + "angularVelocity": -2.0004263459154622e-26, + "velocityX": 2.1726723160140353, + "velocityY": 0.03746039206044517, + "timestamp": 7.7283522694122775 + }, + { + "x": 2.4972894766259857, + "y": 4.098702070348201, + "heading": 3.822235688125773e-25, + "angularVelocity": -1.9197661326309743e-26, + "velocityX": 1.8416428515253338, + "velocityY": 0.0317529075806614, + "timestamp": 7.804239815827584 + }, + { + "x": 2.611925746748375, + "y": 4.100678585440459, + "heading": 3.063572284762821e-25, + "angularVelocity": -2.4686338901390002e-26, + "velocityX": 1.5106071488334032, + "velocityY": 0.026045315544143462, + "timestamp": 7.880127362242891 + }, + { + "x": 2.701440519692856, + "y": 4.102221965206007, + "heading": 2.3114445870748474e-25, + "angularVelocity": -1.5210807516831905e-26, + "velocityX": 1.1795713153591425, + "velocityY": 0.020337721252726194, + "timestamp": 7.956014908658197 + }, + { + "x": 2.7658337920580536, + "y": 4.1033322095862, + "heading": 1.561656112047754e-25, + "angularVelocity": -1.9040979941775662e-26, + "velocityX": 0.8485354370636105, + "velocityY": 0.01463012618851756, + "timestamp": 8.031902455073505 + }, + { + "x": 2.805105562125262, + "y": 4.104009318551404, + "heading": 8.077058611592956e-26, + "angularVelocity": -3.263981497829794e-26, + "velocityX": 0.5174995361200375, + "velocityY": 0.008922530733819996, + "timestamp": 8.107790001488812 + }, + { + "x": 2.819255828857422, + "y": 4.10425329208374, + "heading": 3.4975515409752656e-27, + "angularVelocity": -3.9242842268499737e-26, + "velocityX": 0.18646362151070262, + "velocityY": 0.0032149350435025517, + "timestamp": 8.18367754790412 + }, + { + "x": 2.810470941680652, + "y": 4.100290874791341, + "heading": -0.013665728423960572, + "angularVelocity": -0.18841669161843644, + "velocityX": -0.12112192828199068, + "velocityY": -0.05463196207942443, + "timestamp": 8.256206835551495 + }, + { + "x": 2.779376565530904, + "y": 4.092132450200435, + "heading": -0.040981035402949714, + "angularVelocity": -0.3766107163742046, + "velocityX": -0.4287147600418107, + "velocityY": -0.11248455424752818, + "timestamp": 8.32873612319887 + }, + { + "x": 2.7259718800290944, + "y": 4.079777209778572, + "heading": -0.08192049181497908, + "angularVelocity": -0.5644541362527821, + "velocityX": -0.7363189028058957, + "velocityY": -0.17034829408406268, + "timestamp": 8.401265410846246 + }, + { + "x": 2.650255783412156, + "y": 4.06322370149856, + "heading": -0.13644943849416394, + "angularVelocity": -0.7518196917125987, + "velocityX": -1.0439382361654468, + "velocityY": -0.22823205379447506, + "timestamp": 8.473794698493622 + }, + { + "x": 2.5522270104067304, + "y": 4.04246950295157, + "heading": -0.20452759410435262, + "angularVelocity": -0.9386298668914527, + "velocityX": -1.3515750145241086, + "velocityY": -0.2861492125483705, + "timestamp": 8.546323986140997 + }, + { + "x": 2.4318844389859766, + "y": 4.01751088765649, + "heading": -0.2861194089559698, + "angularVelocity": -1.1249498995261227, + "velocityX": -1.6592272628656817, + "velocityY": -0.34411775028624336, + "timestamp": 8.618853273788373 + }, + { + "x": 2.2892276526666033, + "y": 3.9883426531177015, + "heading": -0.3812142224775482, + "angularVelocity": -1.311122949172083, + "velocityX": -1.9668852534846832, + "velocityY": -0.40215801760798864, + "timestamp": 8.691382561435748 + }, + { + "x": 2.1330829846519728, + "y": 3.9587634030266514, + "heading": -0.4780632841326763, + "angularVelocity": -1.3353097044877917, + "velocityX": -2.1528498773319837, + "velocityY": -0.40782490839919366, + "timestamp": 8.763911849083124 + }, + { + "x": 1.9992497997330843, + "y": 3.9334042162180256, + "heading": -0.5612966415288853, + "angularVelocity": -1.1475827227322806, + "velocityX": -1.8452295515373094, + "velocityY": -0.349640643541367, + "timestamp": 8.8364411367305 + }, + { + "x": 1.8877256900718635, + "y": 3.9122685100016006, + "heading": -0.6308164435957174, + "angularVelocity": -0.9585066160421192, + "velocityX": -1.537642423891296, + "velocityY": -0.29140926240973475, + "timestamp": 8.908970424377875 + }, + { + "x": 1.7985083705984093, + "y": 3.895358481595443, + "heading": -0.6865270710591681, + "angularVelocity": -0.768112155386183, + "velocityX": -1.2300868017236275, + "velocityY": -0.23314758705988012, + "timestamp": 8.98149971202525 + }, + { + "x": 1.731596065924516, + "y": 3.88267537699245, + "heading": -0.7283506709725182, + "angularVelocity": -0.5766442946012179, + "velocityX": -0.9225556577807338, + "velocityY": -0.17486873254092689, + "timestamp": 9.054028999672626 + }, + { + "x": 1.6869877424298634, + "y": 3.8742198088507074, + "heading": -0.7562373778990242, + "angularVelocity": -0.3844889124250871, + "velocityX": -0.6150387649128701, + "velocityY": -0.11658143097795469, + "timestamp": 9.126558287320002 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.19210952061606446, + "velocityX": -0.3075243171780863, + "velocityY": -0.05829081246861152, + "timestamp": 9.199087574967377 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -5.999453085593573e-20, + "velocityX": -1.6973677435282043e-18, + "velocityY": -2.503873223513922e-18, + "timestamp": 9.271616862614753 + }, + { + "x": 1.6770247995246623, + "y": 3.8595873316999896, + "heading": -0.7649878221642326, + "angularVelocity": 0.08507451391469989, + "velocityX": 0.20257164680788678, + "velocityY": -0.1707799926111949, + "timestamp": 9.332541364875802 + }, + { + "x": 1.70170978110981, + "y": 3.8387765844705157, + "heading": -0.7547381408402699, + "angularVelocity": 0.1682357827076709, + "velocityX": 0.405173299231529, + "velocityY": -0.3415825563958536, + "timestamp": 9.39346586713685 + }, + { + "x": 1.7387402457793653, + "y": 3.80755819428449, + "heading": -0.7395583157876386, + "angularVelocity": 0.24915796583103744, + "velocityX": 0.607809063599532, + "velocityY": -0.5124110830197925, + "timestamp": 9.454390369397899 + }, + { + "x": 1.7881185850906471, + "y": 3.7659303409499723, + "heading": -0.7196099497583256, + "angularVelocity": 0.3274276405876679, + "velocityX": 0.8104840824091846, + "velocityY": -0.6832694858326719, + "timestamp": 9.515314871658948 + }, + { + "x": 1.8498475856412335, + "y": 3.713890927230462, + "heading": -0.6950873259687543, + "angularVelocity": 0.4025083977624826, + "velocityX": 1.0132048397554583, + "velocityY": -0.8541623121766772, + "timestamp": 9.576239373919996 + }, + { + "x": 1.9239305360498986, + "y": 3.6514375329357844, + "heading": -0.6662283152580175, + "angularVelocity": 0.47368480069122665, + "velocityX": 1.2159795756924812, + "velocityY": -1.025094862935087, + "timestamp": 9.637163876181045 + }, + { + "x": 2.010371367313072, + "y": 3.5785673636651287, + "heading": -0.6333309419257536, + "angularVelocity": 0.539969505065564, + "velocityX": 1.4188188340512586, + "velocityY": -1.1960732803104883, + "timestamp": 9.698088378442094 + }, + { + "x": 2.1091748328759063, + "y": 3.4952772039974476, + "heading": -0.5967798759520212, + "angularVelocity": 0.5999403297070707, + "velocityX": 1.6217361143055882, + "velocityY": -1.3671044748269063, + "timestamp": 9.759012880703143 + }, + { + "x": 2.220346716936752, + "y": 3.40156341112366, + "heading": -0.5570916262340567, + "angularVelocity": 0.6514333026129457, + "velocityX": 1.82474833498841, + "velocityY": -1.5381954615278515, + "timestamp": 9.819937382964191 + }, + { + "x": 2.3438939700884585, + "y": 3.297422079154605, + "heading": -0.5149984011781104, + "angularVelocity": 0.6909079843703304, + "velocityX": 2.027874641016087, + "velocityY": -1.7093505585459128, + "timestamp": 9.88086188522524 + }, + { + "x": 2.4798242349395756, + "y": 3.182849894813871, + "heading": -0.4716228537300689, + "angularVelocity": 0.7119557130263738, + "velocityX": 2.2311263909664074, + "velocityY": -1.8805600388792227, + "timestamp": 9.941786387486289 + }, + { + "x": 2.628141721858615, + "y": 3.057848265310819, + "heading": -0.42891150330345956, + "angularVelocity": 0.7010537442489162, + "velocityX": 2.4344472488840574, + "velocityY": -2.0517464216194523, + "timestamp": 10.002710889747338 + }, + { + "x": 2.788815804109156, + "y": 2.922450520332472, + "heading": -0.3910899170501554, + "angularVelocity": 0.6207943413513182, + "velocityX": 2.6372654069800667, + "velocityY": -2.2223857389625743, + "timestamp": 10.063635392008386 + }, + { + "x": 2.9613127738384817, + "y": 2.777334038124512, + "heading": -0.375202514467059, + "angularVelocity": 0.2607719717597751, + "velocityX": 2.831323413857593, + "velocityY": -2.3819067341111335, + "timestamp": 10.124559894269435 + }, + { + "x": 3.137446263833113, + "y": 2.628782733735228, + "heading": -0.3752024981016849, + "angularVelocity": 2.6861728103084975e-7, + "velocityX": 2.891012375282727, + "velocityY": -2.438285072117188, + "timestamp": 10.185484396530484 + }, + { + "x": 3.313579787334416, + "y": 2.480231469074176, + "heading": -0.37520248173599324, + "angularVelocity": 2.686224924493493e-7, + "velocityX": 2.8910129252531243, + "velocityY": -2.4382844200276295, + "timestamp": 10.246408898791532 + }, + { + "x": 3.489713311593346, + "y": 2.3316802053114225, + "heading": -0.37520246537030155, + "angularVelocity": 2.6862249292188543e-7, + "velocityX": 2.8910129376886133, + "velocityY": -2.4382844052831705, + "timestamp": 10.307333401052581 + }, + { + "x": 3.6658468653792484, + "y": 2.183128976558075, + "heading": -0.37520244900461003, + "angularVelocity": 2.6862249035440315e-7, + "velocityX": 2.89101342233718, + "velocityY": -2.4382838306472756, + "timestamp": 10.36825790331363 + }, + { + "x": 3.8419803666744587, + "y": 2.034577685567575, + "heading": -0.37520243263891834, + "angularVelocity": 2.686224937536833e-7, + "velocityX": 2.8910125607676815, + "velocityY": -2.4382848521928207, + "timestamp": 10.429182405574679 + }, + { + "x": 4.017984969789346, + "y": 1.8858736976718657, + "heading": -0.37520241627306933, + "angularVelocity": 2.6862507525069377e-7, + "velocityX": 2.8888968573061926, + "velocityY": -2.440791182151078, + "timestamp": 10.490106907835727 + }, + { + "x": 4.198449630537114, + "y": 1.742615181247194, + "heading": -0.37520239987870785, + "angularVelocity": 2.6909307205451447e-7, + "velocityX": 2.9621031612948867, + "velocityY": -2.3514105344815093, + "timestamp": 10.551031410096776 + }, + { + "x": 4.38853011399539, + "y": 1.6123849775845955, + "heading": -0.3752023832167796, + "angularVelocity": 2.734848475989746e-7, + "velocityX": 3.1199349424936167, + "velocityY": -2.137566969436863, + "timestamp": 10.611955912357825 + }, + { + "x": 4.587288186636938, + "y": 1.495827162172114, + "heading": -0.3752023659192684, + "angularVelocity": 2.8391715306138947e-7, + "velocityX": 3.2623667861891117, + "velocityY": -1.9131517055823732, + "timestamp": 10.672880414618874 + }, + { + "x": 4.7937421798706055, + "y": 1.3935176134109497, + "heading": -0.3752023473605566, + "angularVelocity": 3.046181942517293e-7, + "velocityX": 3.3886857597794955, + "velocityY": -1.6792841133570497, + "timestamp": 10.733804916879922 + }, + { + "x": 5.0647852667187605, + "y": 1.2879733857720441, + "heading": -0.3752023309051681, + "angularVelocity": 2.139583629256744e-7, + "velocityX": 3.524191190248399, + "velocityY": -1.3723206946612971, + "timestamp": 10.81071421953017 + }, + { + "x": 5.34411641028699, + "y": 1.2068681953777092, + "heading": -0.37520231550400196, + "angularVelocity": 2.0025101753440068e-7, + "velocityX": 3.6319552244350577, + "velocityY": -1.0545563098285724, + "timestamp": 10.88762352218042 + }, + { + "x": 5.629536746845857, + "y": 1.1508398853670567, + "heading": -0.375202300582326, + "angularVelocity": 1.940165285663064e-7, + "velocityX": 3.711128910592765, + "velocityY": -0.7284984791169633, + "timestamp": 10.964532824830668 + }, + { + "x": 5.917306487549604, + "y": 1.108501764833761, + "heading": -0.37520228573814113, + "angularVelocity": 1.9300896478374464e-7, + "velocityX": 3.741676634521091, + "velocityY": -0.5504941414672958, + "timestamp": 11.041442127480916 + }, + { + "x": 6.2050577552774975, + "y": 1.0660382742134262, + "heading": -0.37520227089383906, + "angularVelocity": 1.9301048840600142e-7, + "velocityX": 3.7414364428249387, + "velocityY": -0.5521242444940722, + "timestamp": 11.118351430131165 + }, + { + "x": 6.492809022919495, + "y": 1.0235747830116217, + "heading": -0.3752022560493111, + "angularVelocity": 1.9301342582848665e-7, + "velocityX": 3.7414364417080774, + "velocityY": -0.5521242520545259, + "timestamp": 11.195260732781414 + }, + { + "x": 6.772658036083171, + "y": 0.9819575118333774, + "heading": -0.34826612342726787, + "angularVelocity": 0.3502324386496837, + "velocityX": 3.638688734915609, + "velocityY": -0.541121421520385, + "timestamp": 11.272170035431662 + }, + { + "x": 7.027520568845826, + "y": 0.9441505058911935, + "heading": -0.3001058813779335, + "angularVelocity": 0.6261952766409414, + "velocityX": 3.313806314454086, + "velocityY": -0.49157910212908845, + "timestamp": 11.34907933808191 + }, + { + "x": 7.256873768560855, + "y": 0.9101570740557017, + "heading": -0.24860478708038097, + "angularVelocity": 0.6696341342705694, + "velocityX": 2.9821255922450605, + "velocityY": -0.44199375971564203, + "timestamp": 11.42598864073216 + }, + { + "x": 7.460704020800595, + "y": 0.8799617502558256, + "heading": -0.1984971865955797, + "angularVelocity": 0.6515154702763284, + "velocityX": 2.65026785077839, + "velocityY": -0.3926095122353607, + "timestamp": 11.502897943382408 + }, + { + "x": 7.6390235862814775, + "y": 0.8535543484261158, + "heading": -0.15201891065632164, + "angularVelocity": 0.6043258011403606, + "velocityX": 2.318569527171569, + "velocityY": -0.3433577073218239, + "timestamp": 11.579807246032656 + }, + { + "x": 7.7918452312093995, + "y": 0.8309283282540864, + "heading": -0.11047967273973482, + "angularVelocity": 0.5401068074364143, + "velocityX": 1.9870371939645617, + "velocityY": -0.29419094169820903, + "timestamp": 11.656716548682905 + }, + { + "x": 7.9191796079863, + "y": 0.812079256076005, + "heading": -0.07474204501348639, + "angularVelocity": 0.46467236725273836, + "velocityX": 1.6556433667844261, + "velocityY": -0.2450818240258775, + "timestamp": 11.733625851333153 + }, + { + "x": 8.021035341477521, + "y": 0.7970039548993032, + "heading": -0.04541767666824434, + "angularVelocity": 0.3812850635065168, + "velocityX": 1.3243616829347478, + "velocityY": -0.1960140146538302, + "timestamp": 11.810535153983402 + }, + { + "x": 8.097419437322872, + "y": 0.7857000289910334, + "heading": -0.022962750583734995, + "angularVelocity": 0.29196632020738633, + "velocityX": 0.9931710887135993, + "velocityY": -0.14697735538800996, + "timestamp": 11.88744445663365 + }, + { + "x": 8.148337646767295, + "y": 0.7781655823556067, + "heading": -0.0077300757248004864, + "angularVelocity": 0.19806023893112515, + "velocityX": 0.662055274067148, + "velocityY": -0.09796534847923602, + "timestamp": 11.964353759283899 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": 1.1088837243155707e-19, + "angularVelocity": 0.10050898211824409, + "velocityX": 0.3310015661876223, + "velocityY": -0.04897379245809494, + "timestamp": 12.041263061934147 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": 5.633915553103528e-20, + "angularVelocity": 2.8365160942051004e-20, + "velocityX": -1.2324505157130932e-18, + "velocityY": 3.669165812220002e-18, + "timestamp": 12.118172364584396 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 1.9547457978922331, - "y": 5.543199682247202, - "heading": -3.210902716840507e-20, - "angularVelocity": -1.5902045262092918e-19, - "velocityX": 0.9949231085045297, - "velocityY": 0.001406140417769105, - "timestamp": 2.4662020364969557 + "scope": [ + "last" + ], + "type": "StopPoint" }, { - "x": 2.0965503966634373, - "y": 5.543400096908449, - "heading": -4.532979164623425e-20, - "angularVelocity": -1.3913840358532023e-19, - "velocityX": 1.492384614166397, - "velocityY": 0.0021092105579800877, - "timestamp": 2.561220838905618 + "scope": [ + 6 + ], + "type": "StopPoint", + "direction": 0 }, { - "x": 2.2856231808662413, - "y": 5.543667316436768, - "heading": -7.554638691710094e-20, - "angularVelocity": -3.1800648403053284e-19, - "velocityX": 1.9898460032113345, - "velocityY": 0.0028122805333745363, - "timestamp": 2.65623964131428 + "scope": [ + 1 + ], + "type": "StopPoint", + "direction": 0 }, { - "x": 2.474695965069045, - "y": 5.543934535965086, - "heading": -6.169486717931722e-20, - "angularVelocity": 1.457766188026957e-19, - "velocityX": 1.9898460032113345, - "velocityY": 0.002812280533374536, - "timestamp": 2.7512584437229424 + "scope": [ + 9 + ], + "type": "StopPoint" }, { - "x": 2.6165005638402494, - "y": 5.544134950626333, - "heading": -3.084743805748469e-20, - "angularVelocity": 3.2464552635862006e-19, - "velocityX": 1.492384614166397, - "velocityY": 0.0021092105579800877, - "timestamp": 2.8462772461316046 + "scope": [ + 7, + 8 + ], + "type": "ZeroAngularVelocity" }, { - "x": 2.711036966099053, - "y": 5.544268560404848, - "heading": -1.8891361886639493e-21, - "angularVelocity": 3.047639112965043e-19, - "velocityX": 0.9949231085045297, - "velocityY": 0.001406140417769105, - "timestamp": 2.941296048540267 - }, + "scope": [ + 7, + 8 + ], + "type": "StraightLine" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "SourceLanePHGSprint (1)": { + "waypoints": [ { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -8.137968740525874e-34, - "angularVelocity": 1.9881709101649526e-20, - "velocityX": 0.49746156397034486, - "velocityY": 0.0007030702226192673, - "timestamp": 3.036314850948929 + "x": 0.43324199318885803, + "y": 4.121134281158447, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -8.137968740525874e-34, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -9.422453604168856e-36, - "timestamp": 3.1313336533575913 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 }, { - "x": 2.744670053824415, - "y": 5.560420833392183, - "heading": 0.008590361313936763, - "angularVelocity": 0.13502733079351698, - "velocityX": -0.21432312628245512, - "velocityY": 0.2528389368381776, - "timestamp": 3.1949530796133994 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 }, { - "x": 2.717653230778774, - "y": 5.592802243922747, - "heading": 0.025816975580253556, - "angularVelocity": 0.27077600789812356, - "velocityX": -0.4246631042695725, - "velocityY": 0.5089862080862002, - "timestamp": 3.2585725058692074 + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 2.6776153836811907, - "y": 5.641767759246714, - "heading": 0.051750317714318324, - "angularVelocity": 0.4076324427980389, - "velocityX": -0.629333671394567, - "velocityY": 0.7696629505440931, - "timestamp": 3.3221919321250155 + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 2.6251092372323575, - "y": 5.707734732807458, - "heading": 0.0864931852991208, - "angularVelocity": 0.5461046983527398, - "velocityX": -0.8253162522672026, - "velocityY": 1.0368998502988076, - "timestamp": 3.3858113583808236 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 }, { - "x": 2.5610825168108655, - "y": 5.791356317104455, - "heading": 0.1301885817245676, - "angularVelocity": 0.686824748304888, - "velocityX": -1.006402040849065, - "velocityY": 1.314403307580339, - "timestamp": 3.4494307846366317 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 }, { - "x": 2.48749773492398, - "y": 5.89376509224382, - "heading": 0.18299253621322714, - "angularVelocity": 0.8299973388055953, - "velocityX": -1.1566401367878931, - "velocityY": 1.6097091905165586, - "timestamp": 3.5130502108924397 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 2.410156329475762, - "y": 6.016926517894053, - "heading": 0.24447965431113647, - "angularVelocity": 0.9664833796311684, - "velocityX": -1.215688508997792, - "velocityY": 1.9359090909592849, - "timestamp": 3.576669637148248 + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 }, { - "x": 2.3496693828620137, - "y": 6.152928215304285, - "heading": 0.3070071930810449, - "angularVelocity": 0.9828372000478389, - "velocityX": -0.9507622148388409, - "velocityY": 2.137738508728141, - "timestamp": 3.640289063404056 + "x": 8.385421752929688, + "y": 0.6878466606140137, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 2.309701308910896, - "y": 6.283843034176789, - "heading": 0.36362933631377437, - "angularVelocity": 0.8900134214517557, - "velocityX": -0.6282369443322133, - "velocityY": 2.0577805644789557, - "timestamp": 3.703908489659864 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 }, { - "x": 2.288494901023514, - "y": 6.40507103844379, - "heading": 0.41290495871744193, - "angularVelocity": 0.7745373591634508, - "velocityX": -0.3333322718459105, - "velocityY": 1.905518666256975, - "timestamp": 3.767527915915672 + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 }, { - "x": 2.285060268530139, - "y": 6.514915678699493, - "heading": 0.45431002431284384, - "angularVelocity": 0.6508242534114624, - "velocityX": -0.05398716548566081, - "velocityY": 1.7265896082436825, - "timestamp": 3.83114734217148 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 16 }, { - "x": 2.2988127872129502, - "y": 6.612520892931977, - "heading": 0.48757772897770557, - "angularVelocity": 0.522917395248037, - "velocityX": 0.2161685430408261, - "velocityY": 1.5342045657567287, - "timestamp": 3.894766768427288 + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": -3.528930169589175e-36, + "angularVelocity": 0, + "velocityX": -3.4629926489463697e-37, + "velocityY": -7.866244288467422e-37, + "timestamp": 0 }, { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "angularVelocity": 0.3925324714794108, - "velocityX": 0.48035043054352317, - "velocityY": 1.3337738607711032, - "timestamp": 3.9583861946830963 + "x": 0.45469188032137553, + "y": 4.109218526997661, + "heading": -0.008937300145995173, + "angularVelocity": -0.1189323726299145, + "velocityX": 0.2854425752342375, + "velocityY": -0.15856789979825622, + "timestamp": 0.07514606787342622 }, { - "x": 2.3906193814869496, - "y": 6.780920533993851, - "heading": 0.5304788719493444, - "angularVelocity": 0.23275934723222522, - "velocityX": 0.7951498408867412, - "velocityY": 1.0846472026287595, - "timestamp": 4.035411897678633 + "x": 0.49759166351162515, + "y": 4.0853873108981436, + "heading": -0.026812413296636604, + "angularVelocity": -0.237871570083344, + "velocityX": 0.5708852692400179, + "velocityY": -0.3171319108760035, + "timestamp": 0.15029213574685243 }, { - "x": 2.471491792404757, - "y": 6.8405967325057775, - "heading": 0.5351323155604377, - "angularVelocity": 0.06041416605263601, - "velocityX": 1.0499405753231936, - "velocityY": 0.7747569472411664, - "timestamp": 4.112437600674169 + "x": 0.5619414537480725, + "y": 4.049641027140498, + "heading": -0.053621717098024775, + "angularVelocity": -0.3567625633658565, + "velocityX": 0.856329440215504, + "velocityY": -0.4756906750976659, + "timestamp": 0.22543820362027867 }, { - "x": 2.556148280048404, - "y": 6.869576729136414, - "heading": 0.5277899567279947, - "angularVelocity": -0.09532349004160981, - "velocityX": 1.0990680299088356, - "velocityY": 0.37623800242778493, - "timestamp": 4.189463303669705 + "x": 0.6477414523773106, + "y": 4.001980177860296, + "heading": -0.08935853104911512, + "angularVelocity": -0.47556465644063195, + "velocityX": 1.141776290593902, + "velocityY": -0.6342427571923072, + "timestamp": 0.30058427149370487 }, { - "x": 2.6173499244947225, - "y": 6.878199709364964, - "heading": 0.518346888239368, - "angularVelocity": -0.12259632981439711, - "velocityX": 0.794561322599876, - "velocityY": 0.11194938693450328, - "timestamp": 4.266489006665242 + "x": 0.7549919358777414, + "y": 3.942405375853288, + "heading": -0.1340144220065316, + "angularVelocity": -0.5942545261667388, + "velocityX": 1.4272268201854585, + "velocityY": -0.7927866845588477, + "timestamp": 0.37573033936713107 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.0752536934288536, - "velocityX": 0.4023928513995403, - "velocityY": 0.0187620669061233, - "timestamp": 4.343514709660778 + "x": 0.8836932393269182, + "y": 3.8709173416225418, + "heading": -0.18758057158075767, + "angularVelocity": -0.7128270459134506, + "velocityX": 1.712681808793474, + "velocityY": -0.951321023891208, + "timestamp": 0.4508764072405573 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -4.4995568742972564e-34, - "velocityX": 2.8230892463303505e-33, - "velocityY": 2.0807279166194578e-33, - "timestamp": 4.420540412656314 + "x": 1.0338457421402325, + "y": 3.787516893297593, + "heading": -0.2500489995023071, + "angularVelocity": -0.8312933688928251, + "velocityX": 1.9981418464400118, + "velocityY": -1.1098444760333435, + "timestamp": 0.5260224751139835 }, { - "x": 2.658706958910465, - "y": 6.86593478207681, - "heading": 0.5043163629464208, - "angularVelocity": -0.14325761626715308, - "velocityX": 0.1802876546124509, - "velocityY": -0.23853061812890558, - "timestamp": 4.4780176824038715 + "x": 1.2054498603190866, + "y": 3.6922049311693006, + "heading": -0.32141344961465573, + "angularVelocity": -0.9496764385936043, + "velocityX": 2.283607419990343, + "velocityY": -1.268355947630325, + "timestamp": 0.6011685429874097 }, { - "x": 2.679434758469343, - "y": 6.838509950906704, - "heading": 0.48805940847309726, - "angularVelocity": -0.2828414527120879, - "velocityX": 0.36062602920974224, - "velocityY": -0.4771422040496659, - "timestamp": 4.535494952151429 + "x": 1.3985060453231255, + "y": 3.584982424979259, + "heading": -0.4016698579859379, + "angularVelocity": -1.0680054278616897, + "velocityX": 2.5690790013020615, + "velocityY": -1.4268545144723217, + "timestamp": 0.676314610860836 }, { - "x": 2.7105313297582017, - "y": 6.797364836772264, - "heading": 0.464037646160856, - "angularVelocity": -0.4179349926979131, - "velocityX": 0.5410238068968174, - "velocityY": -0.7158501841710775, - "timestamp": 4.592972221898986 + "x": 1.5915937569821008, + "y": 3.4778198154599647, + "heading": -0.48176859267038585, + "angularVelocity": -1.065907198489266, + "velocityX": 2.5694985396202825, + "velocityY": -1.4260574445464829, + "timestamp": 0.7514606787342623 }, { - "x": 2.7520007950829926, - "y": 6.742492873007072, - "heading": 0.4325704761896421, - "angularVelocity": -0.5474715502218408, - "velocityX": 0.721493305213118, - "velocityY": -0.954672412350015, - "timestamp": 4.650449491646543 + "x": 1.7632284858900336, + "y": 3.3825660365200445, + "heading": -0.5529765476804539, + "angularVelocity": -0.9475938931363466, + "velocityX": 2.2840147697019786, + "velocityY": -1.2675816797275798, + "timestamp": 0.8266067466076885 }, { - "x": 2.803848241216662, - "y": 6.673886273182379, - "heading": 0.39405966194845793, - "angularVelocity": -0.6700181551824853, - "velocityX": 0.9020513041309295, - "velocityY": -1.1936301102334148, - "timestamp": 4.7079267613941 + "x": 1.9134098098562764, + "y": 3.2992204007390877, + "heading": -0.6152918418017526, + "angularVelocity": -0.8292555536806094, + "velocityX": 1.9985253815170194, + "velocityY": -1.1091150626981794, + "timestamp": 0.9017528144811148 }, { - "x": 2.866080023237396, - "y": 6.591535755483362, - "heading": 0.3490218715149645, - "angularVelocity": -0.7835756748937736, - "velocityX": 1.0827198698556684, - "velocityY": -1.4327492948204654, - "timestamp": 4.7654040311416574 + "x": 2.0421373577646693, + "y": 3.2277823059512354, + "heading": -0.6687115003742365, + "angularVelocity": -0.7108776291856339, + "velocityX": 1.7130310547348626, + "velocityY": -0.9506564589404817, + "timestamp": 0.976898882354541 }, { - "x": 2.938704098816793, - "y": 6.495430149531881, - "heading": 0.29814315673038183, - "angularVelocity": -0.8851971398092591, - "velocityX": 1.2635268845991694, - "velocityY": -1.6720628236793682, - "timestamp": 4.822881300889215 + "x": 2.149410808485792, + "y": 3.1682512394298143, + "heading": -0.7132320418113071, + "angularVelocity": -0.5924533737687983, + "velocityX": 1.4275324545498613, + "velocityY": -0.7922046782500071, + "timestamp": 1.0520449502279672 }, { - "x": 3.0217302964606527, - "y": 6.385555893825644, - "heading": 0.2423799369377359, - "angularVelocity": -0.9701786469253116, - "velocityX": 1.4445048974753851, - "velocityY": -1.9116122980233827, - "timestamp": 4.880358570636772 + "x": 2.2352298936713018, + "y": 3.120626784362015, + "heading": -0.7488501306819737, + "angularVelocity": -0.4739847323836121, + "velocityX": 1.1420302833417786, + "velocityY": -0.6337584442610683, + "timestamp": 1.1271910181013933 }, { - "x": 3.1151700095083186, - "y": 6.261896797084101, - "heading": 0.1831716018579807, - "angularVelocity": -1.0301173897055516, - "velocityX": 1.6256811337430896, - "velocityY": -2.151443471213232, - "timestamp": 4.937835840384329 + "x": 2.2995943987852443, + "y": 3.0849086278583613, + "heading": -0.775563262460788, + "angularVelocity": -0.3554827622359309, + "velocityX": 0.8565252572144717, + "velocityY": -0.4753163740226096, + "timestamp": 1.2023370859748195 }, { - "x": 3.2190322842248182, - "y": 6.124436959606287, - "heading": 0.12297302537717429, - "angularVelocity": -1.0473457898261622, - "velocityX": 1.8070147585761873, - "velocityY": -2.39155127029422, - "timestamp": 4.995313110131886 + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080858, + "angularVelocity": -0.23696688264902366, + "velocityX": 0.5710180813549187, + "velocityY": -0.3168769856057537, + "timestamp": 1.2774831538482456 }, { - "x": 3.3332908123673026, - "y": 5.973192452697109, - "heading": 0.06701518728601086, - "angularVelocity": -0.9735646515036827, - "velocityX": 1.9878906678120518, - "velocityY": -2.6313794578874745, - "timestamp": 5.052790379879443 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288599550472, + "velocityX": 0.28550943973314774, + "velocityY": -0.15843872456938407, + "timestamp": 1.3526292217216718 }, { - "x": 3.457219648102922, - "y": 5.80867115561654, - "heading": 0.033419490428721374, - "angularVelocity": -0.5845040483802287, - "velocityX": 2.1561364393249893, - "velocityY": -2.8623714696810656, - "timestamp": 5.1102676496270005 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 9.576319315825878e-33, + "velocityX": -3.340074147697442e-33, + "velocityY": 1.4972964833121403e-33, + "timestamp": 1.427775289595098 }, { - "x": 3.588371868131788, - "y": 5.635317112607293, - "heading": 0.03341946780631197, - "angularVelocity": -3.935887960456646e-7, - "velocityX": 2.2818101939234974, - "velocityY": -3.0160451909185437, - "timestamp": 5.167744919374558 + "x": 2.3773546785315864, + "y": 3.0407954088560687, + "heading": -0.7860770412156382, + "angularVelocity": 0.26500042058499174, + "velocityX": 0.21918861143707746, + "velocityY": -0.13736692695017713, + "timestamp": 1.4888897992558612 }, { - "x": 3.719524526285539, - "y": 5.461963401064903, - "heading": 0.033419445184933325, - "angularVelocity": -3.9357086276096175e-7, - "velocityX": 2.281817816499982, - "velocityY": -3.0160394239978356, - "timestamp": 5.225222189122115 + "x": 2.4041715283150715, + "y": 3.0239975454455843, + "heading": -0.7541197928104101, + "angularVelocity": 0.5229077118120982, + "velocityX": 0.438796775632191, + "velocityY": -0.27485884291187945, + "timestamp": 1.5500043089166244 }, { - "x": 3.8561567314585825, - "y": 5.292894866069507, - "heading": 0.0334194225694706, - "angularVelocity": -3.9346793646231113e-7, - "velocityX": 2.3771519728257604, - "velocityY": -2.9414851425259596, - "timestamp": 5.282699458869672 + "x": 2.4444388170293236, + "y": 2.998788446329585, + "heading": -0.7069088657797715, + "angularVelocity": 0.7724994815911764, + "velocityX": 0.6588826276733535, + "velocityY": -0.41248959135778174, + "timestamp": 1.6111188185773877 }, { - "x": 4.00579710900262, - "y": 5.135223415824853, - "heading": 0.03341939963642931, - "angularVelocity": -3.9899322628139097e-7, - "velocityX": 2.6034705232392743, - "velocityY": -2.743196587749476, - "timestamp": 5.340176728617229 + "x": 2.498190798561039, + "y": 2.965157152573787, + "heading": -0.6450804787000076, + "angularVelocity": 1.0116809808826634, + "velocityX": 0.8795289666903056, + "velocityY": -0.5502996578468836, + "timestamp": 1.672233328238151 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.03341937596859772, - "angularVelocity": -4.117772415638071e-7, - "velocityX": 2.813320067337111, - "velocityY": -2.527531676347718, - "timestamp": 5.397653998364786 + "x": 2.5654677170302604, + "y": 2.923088008476279, + "heading": -0.5694581806203519, + "angularVelocity": 1.2373869724133075, + "velocityX": 1.1008338092322851, + "velocityY": -0.6883658943027918, + "timestamp": 1.7333478378989142 }, { - "x": 4.277933740289957, - "y": 4.900652577427371, - "heading": 0.03341935253431191, - "angularVelocity": -6.240523322284166e-7, - "velocityX": 2.940862673655656, - "velocityY": -2.37792138300258, - "timestamp": 5.435205794264698 + "x": 2.646315775914165, + "y": 2.8725583813996622, + "heading": -0.48110952859275347, + "angularVelocity": 1.4456248199978636, + "velocityX": 1.3228946666295571, + "velocityY": -0.8268024624119353, + "timestamp": 1.7944623475596775 }, { - "x": 4.3928594316694705, - "y": 4.81721673895127, - "heading": 0.033419330076064284, - "angularVelocity": -5.980605477839765e-7, - "velocityX": 3.060457925523134, - "velocityY": -2.2218867693700792, - "timestamp": 5.47275759016461 + "x": 2.7407862870930626, + "y": 2.813537598490058, + "heading": -0.3813947698765895, + "angularVelocity": 1.631605313855327, + "velocityX": 1.5457951262848801, + "velocityY": -0.965740922036666, + "timestamp": 1.8555768572204407 }, { - "x": 4.5119655190903245, - "y": 4.739865631807423, - "heading": 0.03341930829497473, - "angularVelocity": -5.800279064172658e-7, - "velocityX": 3.1717813906506853, - "velocityY": -2.0598510747665353, - "timestamp": 5.510309386064522 + "x": 2.84893500974016, + "y": 2.7459886359527177, + "heading": -0.2720274509201168, + "angularVelocity": 1.7895475160244767, + "velocityX": 1.7696079580350594, + "velocityY": -1.105285191884772, + "timestamp": 1.916691366881204 }, { - "x": 4.634929904279705, - "y": 4.6688078109694935, - "heading": 0.033419286924895195, - "angularVelocity": -5.690827567994812e-7, - "velocityX": 3.2745274158690463, - "velocityY": -1.8922615852334157, - "timestamp": 5.547861181964434 + "x": 2.9708226363170516, + "y": 2.669873093439706, + "heading": -0.15523119808259803, + "angularVelocity": 1.9111051284848068, + "velocityX": 1.9944138839282306, + "velocityY": -1.2454577961194009, + "timestamp": 1.9778058765419673 }, { - "x": 4.761195179582567, - "y": 4.6037960104372555, - "heading": 0.03341926571852252, - "angularVelocity": -5.647232617468814e-7, - "velocityX": 3.362429739429795, - "velocityY": -1.7312567608088656, - "timestamp": 5.585412977864346 + "x": 3.1065122638812066, + "y": 2.5851587340080315, + "heading": -0.03419676462946552, + "angularVelocity": 1.9804533182868547, + "velocityX": 2.220252249708728, + "velocityY": -1.3861578846318265, + "timestamp": 2.0389203862027303 }, { - "x": 4.8874652266060234, - "y": 4.538793478329322, - "heading": 0.03341924451230877, - "angularVelocity": -5.647190300189245e-7, - "velocityX": 3.3625568097996403, - "velocityY": -1.7310099437370914, - "timestamp": 5.622964773764258 + "x": 3.2560356571448454, + "y": 2.4918371927335334, + "heading": 0.08572826850512158, + "angularVelocity": 1.9623005044182085, + "velocityX": 2.446610372784124, + "velocityY": -1.5269948461095475, + "timestamp": 2.1000348958634936 }, { - "x": 5.0137352745443575, - "y": 4.473790947998577, - "heading": 0.03341922330609504, - "angularVelocity": -5.647190293196981e-7, - "velocityX": 3.3625568341627456, - "velocityY": -1.731009896410775, - "timestamp": 5.6605165696641695 + "x": 3.419180860242083, + "y": 2.390029027608911, + "heading": 0.19415018142326163, + "angularVelocity": 1.7740780956923754, + "velocityX": 2.6695003200194134, + "velocityY": -1.6658591501387063, + "timestamp": 2.161149405524257 }, { - "x": 5.140005505333594, - "y": 4.408788772864344, - "heading": 0.03341920209988365, - "angularVelocity": -5.64718967252867e-7, - "velocityX": 3.362561703461214, - "velocityY": -1.7310004375685482, - "timestamp": 5.698068365564081 + "x": 3.5929644641848295, + "y": 2.279455345020229, + "heading": 0.2670576620012504, + "angularVelocity": 1.1929651564364419, + "velocityX": 2.8435735622750062, + "velocityY": -1.809286914064415, + "timestamp": 2.22226391518502 }, { - "x": 5.267176463182282, - "y": 4.34556692987905, - "heading": 0.033419180889344834, - "angularVelocity": -5.648342058496427e-7, - "velocityX": 3.3865479613183083, - "velocityY": -1.6835903974819586, - "timestamp": 5.735620161463993 + "x": 3.776865546474961, + "y": 2.1600020280366277, + "heading": 0.3015223704483068, + "angularVelocity": 0.5639365944088309, + "velocityX": 3.0091230922237155, + "velocityY": -1.9545819421061736, + "timestamp": 2.2833784248457833 }, { - "x": 5.39745989861742, - "y": 4.289036580908955, - "heading": 0.03341915955151481, - "angularVelocity": -5.682239561025287e-7, - "velocityX": 3.4694328809835504, - "velocityY": -1.5053966825119904, - "timestamp": 5.773171957363905 + "x": 3.9728967066647076, + "y": 2.0375522576737963, + "heading": 0.30152241765502197, + "angularVelocity": 7.724305629887772e-7, + "velocityX": 3.2076042379769354, + "velocityY": -2.003612088889046, + "timestamp": 2.3444929345065466 }, { - "x": 5.530504784906373, - "y": 4.239352458442723, - "heading": 0.03341913783750815, - "angularVelocity": -5.782414966353427e-7, - "velocityX": 3.542969999186219, - "velocityY": -1.323082459189313, - "timestamp": 5.810723753263817 + "x": 4.174386727718979, + "y": 1.9243094504352285, + "heading": 0.30152246485493484, + "angularVelocity": 7.72319260587173e-7, + "velocityX": 3.2969260847008353, + "velocityY": -1.8529610704096402, + "timestamp": 2.40560744416731 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.033419115463141504, - "angularVelocity": -5.958268068735077e-7, - "velocityX": 3.6069365171092738, - "velocityY": -1.1371861948253605, - "timestamp": 5.848275549163729 + "x": 4.383354452565465, + "y": 1.8255438101811148, + "heading": 0.3015225128834295, + "angularVelocity": 7.858771167686507e-7, + "velocityX": 3.4192817058736558, + "velocityY": -1.6160751481496929, + "timestamp": 2.466721953828073 }, { - "x": 5.808220957669321, - "y": 4.160073254115033, - "heading": 0.03341909344161441, - "angularVelocity": -5.669632847998976e-7, - "velocityX": 3.6628444951256323, - "velocityY": -0.941676170142482, - "timestamp": 5.887116736398908 + "x": 4.598761540028333, + "y": 1.7417467464566943, + "heading": 0.30152256294147645, + "angularVelocity": 8.190861254028443e-7, + "velocityX": 3.524647234487482, + "velocityY": -1.3711484259558735, + "timestamp": 2.5278364634888364 }, { - "x": 5.952250404538064, - "y": 4.131196918833519, - "heading": 0.033419072126499465, - "angularVelocity": -5.487760919877281e-7, - "velocityX": 3.708162832321777, - "velocityY": -0.7434462573626127, - "timestamp": 5.925957923634087 + "x": 4.819537390717514, + "y": 1.6733348748555603, + "heading": 0.3015226164690873, + "angularVelocity": 8.758576513463982e-7, + "velocityX": 3.612494838208991, + "velocityY": -1.1194047367945394, + "timestamp": 2.5889509731495997 }, { - "x": 6.09762364792411, - "y": 4.110103210641541, - "heading": 0.03341905123842176, - "angularVelocity": -5.377816485869475e-7, - "velocityX": 3.7427600373239653, - "velocityY": -0.5430757835556856, - "timestamp": 5.964799110869266 + "x": 5.04458467733647, + "y": 1.620648272623701, + "heading": 0.30152267534909954, + "angularVelocity": 9.634375302003616e-7, + "velocityX": 3.682387175617667, + "velocityY": -0.8620964567058493, + "timestamp": 2.650065482810363 }, { - "x": 6.24392007770515, - "y": 4.096848811559285, - "heading": 0.0334190305197359, - "angularVelocity": -5.334205087871439e-7, - "velocityX": 3.7665282704987906, - "velocityY": -0.3412459820551187, - "timestamp": 6.003640298104445 + "x": 5.27278482830379, + "y": 1.5839488347993986, + "heading": 0.3015227422489141, + "angularVelocity": 0.0000010946633609592033, + "velocityX": 3.7339766322927472, + "velocityY": -0.6005028597630111, + "timestamp": 2.711179992471126 }, { - "x": 6.390314919287249, - "y": 4.0847296192319185, - "heading": 0.03341900981240726, - "angularVelocity": -5.331281075500129e-7, - "velocityX": 3.7690619675370143, - "velocityY": -0.31201910111516795, - "timestamp": 6.042481485339624 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3015228161300745, + "angularVelocity": 0.0000012088972118666571, + "velocityX": 3.7670067261218194, + "velocityY": -0.33592432468507805, + "timestamp": 2.7722945021318894 }, { - "x": 6.536709800200056, - "y": 4.07261090201266, - "heading": 0.033418989105078956, - "angularVelocity": -5.331280988936517e-7, - "velocityX": 3.769062980140168, - "velocityY": -0.31200686904551156, - "timestamp": 6.081322672574803 + "x": 5.642161233224445, + "y": 1.5569512204704454, + "heading": 0.30152289610635336, + "angularVelocity": 0.0000021712102287950276, + "velocityX": 3.777876226736954, + "velocityY": -0.1755880047102645, + "timestamp": 2.809129387028348 }, { - "x": 6.68323027882942, - "y": 4.062119297351164, - "heading": 0.03341896797261103, - "angularVelocity": -5.440736864773728e-7, - "velocityX": 3.772296602114613, - "velocityY": -0.27011544724343456, - "timestamp": 6.120163859809982 + "x": 5.781468008961295, + "y": 1.556401100893367, + "heading": 0.3015229698630755, + "angularVelocity": 0.0000020023605971612715, + "velocityX": 3.7819250997644014, + "velocityY": -0.01493474402390838, + "timestamp": 2.8459642719248066 }, { - "x": 6.828478445747191, - "y": 4.059283961769526, - "heading": 0.02725441210059553, - "angularVelocity": -0.15871182913873783, - "velocityX": 3.739539835337918, - "velocityY": -0.07299816981572388, - "timestamp": 6.159005047045161 + "x": 5.920672391336217, + "y": 1.5617696165699877, + "heading": 0.3015230386365789, + "angularVelocity": 0.0000018670752917486593, + "velocityX": 3.779145306581512, + "velocityY": 0.14574541746801997, + "timestamp": 2.882799156821265 }, { - "x": 6.965940131033012, - "y": 4.0573223410737915, - "heading": 0.017080975304275284, - "angularVelocity": -0.2619239400361572, - "velocityX": 3.5390701229986914, - "velocityY": -0.05050362348240824, - "timestamp": 6.19784623428034 + "x": 6.0595230325638285, + "y": 1.5730470711005764, + "heading": 0.30152310340837535, + "angularVelocity": 0.000001758436237719359, + "velocityX": 3.769541879061509, + "velocityY": 0.3061623393771694, + "timestamp": 2.919634041717724 + }, + { + "x": 6.197769224123459, + "y": 1.5902130982406153, + "heading": 0.3015231649742211, + "angularVelocity": 0.0000016714005183062998, + "velocityX": 3.753132172076425, + "velocityY": 0.4660263548614807, + "timestamp": 2.9564689266141824 }, { - "x": 7.095541107200079, - "y": 4.056112825219225, - "heading": 0.007563330070755733, - "angularVelocity": -0.2450400183673897, - "velocityX": 3.336689359734222, - "velocityY": -0.03114003305929124, - "timestamp": 6.236687421515519 + "x": 6.335161349542959, + "y": 1.6132366986444322, + "heading": 0.3015232239953201, + "angularVelocity": 0.0000016023152826544925, + "velocityX": 3.7299458327534505, + "velocityY": 0.6250487946015111, + "timestamp": 2.993303811510641 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, - "angularVelocity": -0.194724482157576, - "velocityX": 3.1345655629883886, - "velocityY": -0.012732337027157752, - "timestamp": 6.275528608750698 + "x": 6.471451335210211, + "y": 1.6420762959087596, + "heading": 0.3015232810350899, + "angularVelocity": 0.000001548525804727782, + "velocityX": 3.7000247469309935, + "velocityY": 0.7829425107583309, + "timestamp": 3.0301386964070995 }, { - "x": 7.3857970557158446, - "y": 4.056503208949951, - "heading": -0.005539516561442828, - "angularVelocity": -0.09285314353970942, - "velocityX": 2.8244854636552765, - "velocityY": 0.014833039029661928, - "timestamp": 6.335187510434235 + "x": 6.606393098338155, + "y": 1.6766798119201123, + "heading": 0.3015233366126912, + "angularVelocity": 0.0000015088305949550033, + "velocityX": 3.6634229618813627, + "velocityY": 0.9394224010369983, + "timestamp": 3.066973581303558 }, { - "x": 7.5357095386456, - "y": 4.058265721996549, - "heading": -0.007640794437617486, - "angularVelocity": -0.03522153135371121, - "velocityX": 2.5128267316246156, - "velocityY": 0.029543169533142722, - "timestamp": 6.394846412117771 + "x": 6.739742463422568, + "y": 1.7169846140875678, + "heading": 0.30152556567084704, + "angularVelocity": 0.00006051486687687375, + "velocityX": 3.6201922568579414, + "velocityY": 1.0942019306087243, + "timestamp": 3.1038084662000167 }, { - "x": 7.667000937583321, - "y": 4.060490426598279, - "heading": -0.007708640614146542, - "angularVelocity": -0.0011372347564986972, - "velocityX": 2.200700905192029, - "velocityY": 0.037290404934565556, - "timestamp": 6.454505313801308 + "x": 6.8694207904091344, + "y": 1.7614703171513668, + "heading": 0.3101205256410533, + "angularVelocity": 0.23333750042565127, + "velocityX": 3.520530262306672, + "velocityY": 1.2077057710061154, + "timestamp": 3.1406433510964753 }, { - "x": 7.779661923967275, - "y": 4.062917456014214, - "heading": -0.0066168114481254744, - "angularVelocity": 0.018301194544491067, - "velocityX": 1.888418713800162, - "velocityY": 0.0406817649578728, - "timestamp": 6.514164215484844 + "x": 6.994764102779472, + "y": 1.8086931078781319, + "heading": 0.3306180633638748, + "angularVelocity": 0.5564707961064402, + "velocityX": 3.4028425152588877, + "velocityY": 1.2820127132066854, + "timestamp": 3.177478235992934 }, { - "x": 7.873689685619936, - "y": 4.06536905162046, - "heading": -0.004961765552273349, - "angularVelocity": 0.027741809673791765, - "velocityX": 1.5760893848068933, - "velocityY": 0.04109354240631638, - "timestamp": 6.573823117168381 + "x": 7.114597005425528, + "y": 1.8565129203183168, + "heading": 0.359627713397502, + "angularVelocity": 0.7875591335542955, + "velocityX": 3.2532449329732183, + "velocityY": 1.298220764761567, + "timestamp": 3.2143131208893925 }, { - "x": 7.949083994059634, - "y": 4.067716008149933, - "heading": -0.003176764942269326, - "angularVelocity": 0.029920105124841948, - "velocityX": 1.263756226013528, - "velocityY": 0.03933958660390468, - "timestamp": 6.633482018851917 + "x": 7.228563598795877, + "y": 1.903963761474251, + "heading": 0.39146295222083616, + "angularVelocity": 0.8642687200685345, + "velocityX": 3.0939853264291135, + "velocityY": 1.288203866777826, + "timestamp": 3.251148005785851 }, { - "x": 8.005845696502893, - "y": 4.069860191657625, - "heading": -0.0015908607397525072, - "angularVelocity": 0.02658285951909277, - "velocityX": 0.951437268227904, - "velocityY": 0.035940713744034375, - "timestamp": 6.693140920535454 + "x": 7.336676614220082, + "y": 1.9505339221399112, + "heading": 0.42341393247502157, + "angularVelocity": 0.8674108889982476, + "velocityX": 2.9350713522821095, + "velocityY": 1.2642949963483492, + "timestamp": 3.2879828906823096 }, { - "x": 8.043976076014772, - "y": 4.071724543898811, - "heading": -0.00046248549538235704, - "angularVelocity": 0.018913778372181077, - "velocityX": 0.6391398171247351, - "velocityY": 0.03125019382816773, - "timestamp": 6.75279982221899 + "x": 7.438996464546078, + "y": 1.9959395107165516, + "heading": 0.45395278545537515, + "angularVelocity": 0.8290742068611577, + "velocityX": 2.777797476865021, + "velocityY": 1.2326789863542045, + "timestamp": 3.3248177755787682 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 3.4463826664070866e-33, - "angularVelocity": 0.00775216241552071, - "velocityX": 0.32686633402451676, - "velocityY": 0.02551860543538527, - "timestamp": 6.812458723902527 + "x": 7.535582073954796, + "y": 2.0400031624519186, + "heading": 0.4820829146791016, + "angularVelocity": 0.7636817463336469, + "velocityX": 2.622123285581467, + "velocityY": 1.1962478465516526, + "timestamp": 3.361652660475227 }, { - "x": 8.059339450711764, - "y": 4.074523581107775, - "heading": -0.0006944740465081056, - "angularVelocity": -0.009481698382290156, - "velocityX": -0.0564842507032717, - "velocityY": 0.017429845647464255, - "timestamp": 6.88570235795497 + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6791276743107278, + "velocityX": 2.4678193928677445, + "velocityY": 1.1565397180800696, + "timestamp": 3.3984875453716854 }, { - "x": 8.027124349390371, - "y": 4.075207755344852, - "heading": -0.002651211191279339, - "angularVelocity": -0.026715456845985846, - "velocityX": -0.43983482985464195, - "velocityY": 0.009341074428224473, - "timestamp": 6.958945992007413 + "x": 7.766333855095899, + "y": 2.151391160116789, + "heading": 0.5393931714770351, + "angularVelocity": 0.508950730683683, + "velocityX": 2.2039777611896034, + "velocityY": 1.0840548028640593, + "timestamp": 3.461940970502476 }, { - "x": 7.96683125907587, - "y": 4.075299477238389, - "heading": -0.0058702138612145086, - "angularVelocity": -0.043949248444312826, - "velocityX": -0.8231854016327222, - "velocityY": 0.0012522848534625944, - "timestamp": 7.032189626059856 + "x": 7.8897168213730975, + "y": 2.2145350229010927, + "heading": 0.5621690879226864, + "angularVelocity": 0.3589391179231936, + "velocityX": 1.9444650312080196, + "velocityY": 0.9951214241650663, + "timestamp": 3.5253943956332665 }, { - "x": 7.878460180490193, - "y": 4.074798744928973, - "heading": -0.0103514975430217, - "angularVelocity": -0.06118325148365151, - "velocityX": -1.2065359635542152, - "velocityY": -0.006836530107951673, - "timestamp": 7.105433260112299 + "x": 7.996856912991746, + "y": 2.2713427782005033, + "heading": 0.5763089063837279, + "angularVelocity": 0.22283774960762856, + "velocityX": 1.6884839769927564, + "velocityY": 0.8952669644281364, + "timestamp": 3.588847820764057 }, { - "x": 7.7620111146201785, - "y": 4.073705556029158, - "heading": -0.01609509527793809, - "angularVelocity": -0.07841770563710548, - "velocityX": -1.5898865120023296, - "velocityY": -0.01492537766534737, - "timestamp": 7.1786768941647425 + "x": 8.08793407853681, + "y": 2.3213235528188796, + "heading": 0.5824533246990756, + "angularVelocity": 0.09683351690917823, + "velocityX": 1.4353388387992243, + "velocityY": 0.7876765441007405, + "timestamp": 3.6523012458948476 }, { - "x": 7.617484062863899, - "y": 4.072019907599753, - "heading": -0.023101063438175296, - "angularVelocity": -0.0956529294439546, - "velocityX": -1.9732370413624691, - "velocityY": -0.02301426535169652, - "timestamp": 7.251920528217186 + "x": 8.16309393881241, + "y": 2.364112462024999, + "heading": 0.5810872026138336, + "angularVelocity": -0.021529524725041688, + "velocityX": 1.1844886248564515, + "velocityY": 0.6743356898059116, + "timestamp": 3.715754671025638 }, { - "x": 7.444879027316714, - "y": 4.06974179610174, - "heading": -0.031369488604002356, - "angularVelocity": -0.1128893353367305, - "velocityX": -2.356587542114559, - "velocityY": -0.031103201356480925, - "timestamp": 7.325164162269629 + "x": 8.222455939995415, + "y": 2.3994280671308665, + "heading": 0.5725898810376202, + "angularVelocity": -0.13391430893917458, + "velocityX": 0.9355208337555886, + "velocityY": 0.5565594770191679, + "timestamp": 3.7792080961564287 }, { - "x": 7.244196011426359, - "y": 4.066871217282435, - "heading": -0.040900495023888005, - "angularVelocity": -0.13012743760176224, - "velocityX": -2.7399379957944494, - "velocityY": -0.0391921954234215, - "timestamp": 7.398407796322072 + "x": 8.266119462815809, + "y": 2.427046891293226, + "heading": 0.5572661381126698, + "angularVelocity": -0.24149591441856877, + "velocityX": 0.6881192422693246, + "velocityY": 0.4352613606819736, + "timestamp": 3.8426615212872193 }, { - "x": 7.015435021934675, - "y": 4.063408165830539, - "heading": -0.05169425070794916, - "angularVelocity": -0.14736783371961967, - "velocityX": -3.123288357427606, - "velocityY": -0.047281262005875575, - "timestamp": 7.471651430374515 + "x": 8.294168308263885, + "y": 2.446787281465013, + "heading": 0.5353660980042331, + "angularVelocity": -0.3451356654632322, + "velocityX": 0.4420383200790658, + "velocityY": 0.31110046669817776, + "timestamp": 3.90611494641801 }, { - "x": 6.758596078532163, - "y": 4.059352633648335, - "heading": -0.06375095974291681, - "angularVelocity": -0.1646110162466115, - "velocityX": -3.5066384502250503, - "velocityY": -0.05537043914696755, - "timestamp": 7.544895064426958 + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.4454857015776663, + "velocityX": 0.19708463824313482, + "velocityY": 0.18456741878341767, + "timestamp": 3.9695683715488004 }, { - "x": 6.481653456514826, - "y": 4.053513800761945, - "heading": -0.06375096147352038, - "angularVelocity": -2.3628040945559003e-8, - "velocityX": -3.7811152545904956, - "velocityY": -0.07971795722490971, - "timestamp": 7.6181386984794015 + "x": 8.303024530407173, + "y": 2.4618811275821986, + "heading": 0.4711910323232683, + "angularVelocity": -0.5465136547157353, + "velocityX": -0.055545178149817624, + "velocityY": 0.05148048067311967, + "timestamp": 4.035271161880262 }, { - "x": 6.205372409181029, - "y": 4.073516064315128, - "heading": -0.06375096323470678, - "angularVelocity": -2.4045589953178915e-8, - "velocityX": -3.7720827333058655, - "velocityY": 0.2730921780705308, - "timestamp": 7.691382332531845 + "x": 8.282750419132642, + "y": 2.456558519757899, + "heading": 0.42887568169046636, + "angularVelocity": -0.6440419108431609, + "velocityX": -0.30857306321773753, + "velocityY": -0.08101037714605691, + "timestamp": 4.1009739522117235 }, { - "x": 5.9325368870621995, - "y": 4.121391787593611, - "heading": -0.0637509651063581, - "angularVelocity": -2.555377459766275e-8, - "velocityX": -3.725040758128897, - "velocityY": 0.6536502987304462, - "timestamp": 7.764625966584288 + "x": 8.245821395629305, + "y": 2.4425769262851884, + "heading": 0.3804195474569322, + "angularVelocity": -0.7375049672788591, + "velocityX": -0.5620617224479451, + "velocityY": -0.2128006040866038, + "timestamp": 4.166676742543185 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -0.063750967187294, - "angularVelocity": -2.84111505129003e-8, - "velocityX": -3.639704142076304, - "velocityY": 1.0274925313911556, - "timestamp": 7.837869600636731 + "x": 8.19220201387014, + "y": 2.4199912479232255, + "heading": 0.32613641295591106, + "angularVelocity": -0.8261922245184766, + "velocityX": -0.8160898721144464, + "velocityY": -0.3437552385221621, + "timestamp": 4.2323795328746465 + }, + { + "x": 8.121850214183777, + "y": 2.388868090056124, + "heading": 0.2664003971864744, + "angularVelocity": -0.9091853704854365, + "velocityX": -1.0707581722397086, + "velocityY": -0.4736961354318394, + "timestamp": 4.298082323206108 + }, + { + "x": 8.034715295558527, + "y": 2.3492899199909276, + "heading": 0.20166644793914107, + "angularVelocity": -0.9852541866298157, + "velocityX": -1.32619814448768, + "velocityY": -0.6023818754961511, + "timestamp": 4.36378511353757 }, { - "x": 5.417504232216337, - "y": 4.293821782165803, - "heading": -0.06375097411514968, - "angularVelocity": -9.821339780633191e-8, - "velocityX": -3.5221393138546646, - "velocityY": 1.3775780331429743, - "timestamp": 7.90840840517157 + "x": 7.930734985336223, + "y": 2.3013613961682027, + "heading": 0.13250258742090648, + "angularVelocity": -1.0526776742557293, + "velocityX": -1.582585909939886, + "velocityY": -0.7294747084702521, + "timestamp": 4.429487903869031 }, { - "x": 5.186014168311204, - "y": 4.410724788288498, - "heading": -0.06922328693791815, - "angularVelocity": -0.07757875766190163, - "velocityX": -3.281740673543775, - "velocityY": 1.6572864665569722, - "timestamp": 7.9789472097064085 + "x": 7.809831081747402, + "y": 2.2452194829459002, + "heading": 0.05964299707705179, + "angularVelocity": -1.1089268808263428, + "velocityX": -1.8401639105261383, + "velocityY": -0.8544829365552755, + "timestamp": 4.495190694200493 }, { - "x": 4.979961148964822, - "y": 4.522014233902487, - "heading": -0.07551597396386121, - "angularVelocity": -0.08920886974821321, - "velocityX": -2.9211300177991313, - "velocityY": 1.5777052977843462, - "timestamp": 8.049486014241246 + "x": 7.671902780233126, + "y": 2.1810506699963903, + "heading": -0.015920256793081004, + "angularVelocity": -1.1500767850029923, + "velocityX": -2.099276161917122, + "velocityY": -0.976652781804051, + "timestamp": 4.560893484531954 }, { - "x": 4.7980291404215, - "y": 4.623466282585381, - "heading": -0.08170492719079357, - "angularVelocity": -0.08773827778546625, - "velocityX": -2.579176238427287, - "velocityY": 1.4382445145180842, - "timestamp": 8.120024818776084 + "x": 7.516816185598605, + "y": 2.109122861945235, + "heading": -0.09276870443286901, + "angularVelocity": -1.1696375032490753, + "velocityX": -2.360426305368932, + "velocityY": -1.0947451042534073, + "timestamp": 4.626596274863416 }, { - "x": 4.639563192627529, - "y": 4.713607705102715, - "heading": -0.0874491132160183, - "angularVelocity": -0.08143299369906003, - "velocityX": -2.246507420120879, - "velocityY": 1.27789835838249, - "timestamp": 8.190563623310922 + "x": 7.344387868807497, + "y": 2.0298516556077795, + "heading": -0.16871929267657423, + "angularVelocity": -1.155972034985807, + "velocityX": -2.624368248612144, + "velocityY": -1.2065120208372122, + "timestamp": 4.692299065194877 }, { - "x": 4.504192007690659, - "y": 4.791698947450768, - "heading": -0.09257307344830198, - "angularVelocity": -0.07264030438385177, - "velocityX": -1.919102341322099, - "velocityY": 1.1070678453231335, - "timestamp": 8.26110242784576 + "x": 7.154363408340053, + "y": 1.943962982346676, + "heading": -0.2400485240661189, + "angularVelocity": -1.0856347352935647, + "velocityX": -2.892182501059652, + "velocityY": -1.3072302230667407, + "timestamp": 4.758001855526339 }, { - "x": 4.391678654940531, - "y": 4.857296763925047, - "heading": -0.09697023729098278, - "angularVelocity": -0.0623368069770602, - "velocityX": -1.5950561324662005, - "velocityY": 0.9299536178257779, - "timestamp": 8.331641232380598 + "x": 6.946444183851081, + "y": 1.8530162365161487, + "heading": -0.2992772419214472, + "angularVelocity": -0.9014642689682999, + "velocityX": -3.164541771210153, + "velocityY": -1.3842143594162997, + "timestamp": 4.8237046458578 }, { - "x": 4.301859289367306, - "y": 4.910106206970951, - "heading": -0.10056909340764136, - "angularVelocity": -0.051019522380495494, - "velocityX": -1.2733326878096392, - "velocityY": 0.7486580385668955, - "timestamp": 8.402180036915436 + "x": 6.72212326982476, + "y": 1.762675190878268, + "heading": -0.31855321839260486, + "angularVelocity": -0.29338139786625583, + "velocityX": -3.4141763674670904, + "velocityY": -1.3749955699312393, + "timestamp": 4.889407436189262 }, { - "x": 4.234614001747884, - "y": 4.949916980366573, - "heading": -0.10331834169263278, - "angularVelocity": -0.03897497700905296, - "velocityX": -0.9533091475375062, - "velocityY": 0.5643811751297677, - "timestamp": 8.472718841450273 + "x": 6.4857597450479005, + "y": 1.6860130581467423, + "heading": -0.3185532921212083, + "angularVelocity": -0.000001122153306519187, + "velocityX": -3.5974655503128186, + "velocityY": -1.1668017803319486, + "timestamp": 4.955110226520723 }, { - "x": 4.18985129168719, - "y": 4.976571613170809, - "heading": -0.10517936365869324, - "angularVelocity": -0.026382953019019773, - "velocityX": -0.6345827712261082, - "velocityY": 0.37787191007854753, - "timestamp": 8.543257645985111 + "x": 6.24426876944993, + "y": 1.6274732096370486, + "heading": -0.3185533177664737, + "angularVelocity": -3.903223175651228e-7, + "velocityX": -3.6755056273817623, + "velocityY": -0.8909796405048931, + "timestamp": 5.020813016852185 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.013363372792080578, - "velocityX": -0.31687843925651377, - "velocityY": 0.18962871266842563, - "timestamp": 8.61379645051995 + "x": 5.999037658914315, + "y": 1.5873919634602989, + "heading": -0.3185533462426245, + "angularVelocity": -4.3340854500246066e-7, + "velocityX": -3.732430682143934, + "velocityY": -0.6100387209515066, + "timestamp": 5.0865158071836465 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": 0, - "velocityX": 8.237561081237056e-34, - "velocityY": 0, - "timestamp": 8.684335255054787 + "x": 5.751475205523387, + "y": 1.5659995878977724, + "heading": -0.3185533789943253, + "angularVelocity": -4.984826460154038e-7, + "velocityX": -3.7679138456983283, + "velocityY": -0.32559310578142847, + "timestamp": 5.152218597515108 }, { - "x": 4.18294196758914, - "y": 4.969364336221789, - "heading": -0.10045524830547124, - "angularVelocity": 0.08077125535022475, - "velocityX": 0.22011597884885625, - "velocityY": -0.29338710511594707, - "timestamp": 8.754493279341492 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.3185534180019773, + "angularVelocity": -5.936985598886166e-7, + "velocityX": -3.781751231726404, + "velocityY": -0.03927692372891183, + "timestamp": 5.21792138784657 }, { - "x": 4.2148538086473515, - "y": 4.929011600999615, - "heading": -0.08896968745487453, - "angularVelocity": 0.16370986736542367, - "velocityX": 0.4548566095276789, - "velocityY": -0.5751692074062589, - "timestamp": 8.824651303628197 + "x": 5.26265149067387, + "y": 1.5786031053003824, + "heading": -0.31855345112044986, + "angularVelocity": -5.200844119271939e-7, + "velocityX": -3.774430827391544, + "velocityY": 0.23844772894968422, + "timestamp": 5.28160042121006 }, { - "x": 4.264519080629496, - "y": 4.870038214957966, - "heading": -0.07148588735689271, - "angularVelocity": 0.24920599283886813, - "velocityX": 0.7079057953397331, - "velocityY": -0.8405793441481644, - "timestamp": 8.894809327914903 + "x": 5.0240625547753925, + "y": 1.61139052009726, + "heading": -0.3185534786660252, + "angularVelocity": -4.325689925822067e-7, + "velocityX": -3.746742425196293, + "velocityY": 0.5148855606793217, + "timestamp": 5.34527945457355 }, { - "x": 4.333555645892384, - "y": 4.794138441539978, - "heading": -0.04780159185128592, - "angularVelocity": 0.3375849839901328, - "velocityX": 0.9840152422303832, - "velocityY": -1.081840234095224, - "timestamp": 8.964967352201608 + "x": 4.788524290519247, + "y": 1.6616042890226763, + "heading": -0.31855350247111613, + "angularVelocity": -3.7382933976434193e-7, + "velocityX": -3.698835422197071, + "velocityY": 0.7885447732660757, + "timestamp": 5.40895848793704 }, { - "x": 4.423967613250648, - "y": 4.703932489290492, - "heading": -0.017716503350327845, - "angularVelocity": 0.4288189242332906, - "velocityX": 1.2886903284047762, - "velocityY": -1.2857538844146224, - "timestamp": 9.035125376488313 + "x": 4.55730773239592, + "y": 1.7289734284079032, + "heading": -0.3185535237219762, + "angularVelocity": -3.3371831981217076e-7, + "velocityX": -3.6309684037367047, + "velocityY": 1.0579485244487483, + "timestamp": 5.4726375213005305 }, { - "x": 4.53803034325044, - "y": 4.603622363293204, - "heading": 0.0188848190363467, - "angularVelocity": 0.5216983054867799, - "velocityX": 1.6257973504736565, - "velocityY": -1.429774099500926, - "timestamp": 9.105283400775019 + "x": 4.33166058516164, + "y": 1.8131343663413289, + "heading": -0.31855354324045854, + "angularVelocity": -3.06513484100539e-7, + "velocityX": -3.5435077342686676, + "velocityY": 1.3216428310558945, + "timestamp": 5.536316554664021 }, { - "x": 4.677578748555051, - "y": 4.49984021599331, - "heading": 0.06181064388576721, - "angularVelocity": 0.6118448357952799, - "velocityX": 1.9890583682108496, - "velocityY": -1.4792626838489913, - "timestamp": 9.175441425061724 + "x": 4.112800476187319, + "y": 1.9136328827222213, + "heading": -0.31855356163583604, + "angularVelocity": -2.8887651909639526e-7, + "velocityX": -3.4369257417120753, + "velocityY": 1.5782041760469425, + "timestamp": 5.599995588027511 }, { - "x": 4.842258553230926, - "y": 4.40159118744757, - "heading": 0.11019595374155315, - "angularVelocity": 0.6896618077221814, - "velocityX": 2.3472697007957355, - "velocityY": -1.4003961705682948, - "timestamp": 9.245599449348429 + "x": 3.9019083093617053, + "y": 2.029926448332014, + "heading": -0.3185535793939757, + "angularVelocity": -2.788694925452677e-7, + "velocityX": -3.3117991226689654, + "velocityY": 1.8262457745859704, + "timestamp": 5.663674621391001 }, { - "x": 5.028363428376419, - "y": 4.3176283558190685, - "heading": 0.16247927063581383, - "angularVelocity": 0.7452221955481739, - "velocityX": 2.6526527369836272, - "velocityY": -1.1967673332045712, - "timestamp": 9.315757473635134 + "x": 3.700120307432907, + "y": 2.161384813738153, + "heading": -0.3185535969362317, + "angularVelocity": -2.7547930719470604e-7, + "velocityX": -3.1688295388681516, + "velocityY": 2.0643900898393666, + "timestamp": 5.727353654754491 }, { - "x": 5.230618576677741, - "y": 4.253738965485636, - "heading": 0.2169576127744663, - "angularVelocity": 0.7765090692409344, - "velocityX": 2.8828512541173206, - "velocityY": -0.9106497935623799, - "timestamp": 9.38591549792184 + "x": 3.5092854049648876, + "y": 2.2879634464070207, + "heading": -0.3579155843505842, + "angularVelocity": -0.618131044635493, + "velocityX": -2.996824737880426, + "velocityY": 1.9877599577609333, + "timestamp": 5.791032688117982 }, { - "x": 5.444331250681613, - "y": 4.2129309832647035, - "heading": 0.2722364606827335, - "angularVelocity": 0.7879191079037022, - "velocityX": 3.0461615214607765, - "velocityY": -0.5816580873795356, - "timestamp": 9.456073522208545 + "x": 3.3329097555386666, + "y": 2.4050971261724468, + "heading": -0.41294492327597715, + "angularVelocity": -0.8641673093760206, + "velocityX": -2.7697601566820413, + "velocityY": 1.8394387222684179, + "timestamp": 5.854711721481472 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.3273042274639755, - "angularVelocity": 0.7849104552346565, - "velocityX": 3.1588757008538453, - "velocityY": -0.23207478938864265, - "timestamp": 9.52623154649525 + "x": 3.171287726021972, + "y": 2.512479984884094, + "heading": -0.46992046584984914, + "angularVelocity": -0.8947300165290927, + "velocityX": -2.538072910028812, + "velocityY": 1.686314207985683, + "timestamp": 5.918390754844962 }, { - "x": 5.81887557997068, - "y": 4.1972183908996925, - "heading": 0.36426543181084436, - "angularVelocity": 0.7759151841130933, - "velocityX": 3.210283328613804, - "velocityY": 0.011951482768166844, - "timestamp": 9.573867170985725 + "x": 3.024408806786741, + "y": 2.6100923699912792, + "heading": -0.5252799185719805, + "angularVelocity": -0.8693513358805383, + "velocityX": -2.306550704009953, + "velocityY": 1.532881074842923, + "timestamp": 5.982069788208452 }, { - "x": 5.973953798857342, - "y": 4.209469324927334, - "heading": 0.4004931510026103, - "angularVelocity": 0.7605173560600648, - "velocityX": 3.255509307276357, - "velocityY": 0.257180086514691, - "timestamp": 9.6215027954762 + "x": 2.892255079193046, + "y": 2.6979336450739244, + "heading": -0.5773440254447586, + "angularVelocity": -0.8176020288434342, + "velocityX": -2.0753098879397363, + "velocityY": 1.3794379475145806, + "timestamp": 6.045748821571943 }, { - "x": 6.130819634438986, - "y": 4.233461866975936, - "heading": 0.4356102472173979, - "angularVelocity": 0.7372023898166727, - "velocityX": 3.2930361942250057, - "velocityY": 0.5036680489703445, - "timestamp": 9.669138419966675 + "x": 2.7748123815012584, + "y": 2.776006230905406, + "heading": -0.625128837055519, + "angularVelocity": -0.7504010203483642, + "velocityX": -1.8442914643726742, + "velocityY": 1.226032835420591, + "timestamp": 6.109427854935433 }, { - "x": 6.289005438456121, - "y": 4.269254559307002, - "heading": 0.4691390733673604, - "angularVelocity": 0.7038603253047925, - "velocityX": 3.32074588523062, - "velocityY": 0.7513849710991581, - "timestamp": 9.71677404445715 + "x": 2.6720699818921556, + "y": 2.8443129166968544, + "heading": -0.6679860381773632, + "angularVelocity": -0.6730190277419648, + "velocityX": -1.6134415706757543, + "velocityY": 1.0726715244175118, + "timestamp": 6.173106888298923 }, { - "x": 6.447898093638549, - "y": 4.316894151635714, - "heading": 0.5004573197150548, - "angularVelocity": 0.6574543040567628, - "velocityX": 3.335584594135005, - "velocityY": 1.0000832956066072, - "timestamp": 9.764409668947625 + "x": 2.5840196243973645, + "y": 2.9028562717726505, + "heading": -0.7054558016839699, + "angularVelocity": -0.5884160221579269, + "velocityX": -1.382721326691407, + "velocityY": 0.9193505614575074, + "timestamp": 6.236785921662413 }, { - "x": 6.606667388474461, - "y": 4.376390356038531, - "heading": 0.5287266543286071, - "angularVelocity": 0.5934494386486953, - "velocityX": 3.33299492835787, - "velocityY": 1.248985502745193, - "timestamp": 9.8120452934381 + "x": 2.510654811151598, + "y": 2.95163853061359, + "heading": -0.7371951288142529, + "angularVelocity": -0.4984266477336411, + "velocityX": -1.1521031235978156, + "velocityY": 0.7660646882386302, + "timestamp": 6.3004649550259035 }, { - "x": 6.764149093497097, - "y": 4.447653029340476, - "heading": 0.5527726659639904, - "angularVelocity": 0.5047905195447082, - "velocityX": 3.3059649518004255, - "velocityY": 1.4959953619626436, - "timestamp": 9.859680917928575 + "x": 2.4519703126340358, + "y": 2.9906615990271557, + "heading": -0.7629386573341135, + "angularVelocity": -0.4042700895428554, + "velocityX": -0.9215670436230067, + "velocityY": 0.6128087433553877, + "timestamp": 6.364143988389394 }, { - "x": 6.918662738022944, - "y": 4.530331648811429, - "heading": 0.5708777234010636, - "angularVelocity": 0.3800738970199359, - "velocityX": 3.243657371527556, - "velocityY": 1.735646805417357, - "timestamp": 9.90731654241905 + "x": 2.4079618327799786, + "y": 3.0199270886206313, + "heading": -0.7824753669479414, + "angularVelocity": -0.30679972012623624, + "velocityX": -0.6910984279998401, + "velocityY": 0.45957810675962124, + "timestamp": 6.427823021752884 }, { - "x": 7.06846751913844, - "y": 4.623866769358016, - "heading": 0.5845734852773292, - "angularVelocity": 0.2875109127414552, - "velocityX": 3.144805651607445, - "velocityY": 1.9635539902555645, - "timestamp": 9.954952166909525 + "x": 2.378625774904473, + "y": 3.039436353320747, + "heading": -0.795633833561549, + "angularVelocity": -0.2066373485680448, + "velocityX": -0.4606862938394533, + "velocityY": 0.3063687318988397, + "timestamp": 6.491502055116374 }, { - "x": 7.211529043645413, - "y": 4.727181705822193, - "heading": 0.5988053175713824, - "angularVelocity": 0.2987644739893997, - "velocityX": 3.0032465415789793, - "velocityY": 2.168858655035301, - "timestamp": 10.0025877914 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.10425061542851646, + "velocityX": -0.2303222914890624, + "velocityY": 0.15317707264507013, + "timestamp": 6.5551810884798645 }, { - "x": 7.344594975677938, - "y": 4.83690117801315, - "heading": 0.6149173466892092, - "angularVelocity": 0.3382348670803817, - "velocityX": 2.7934121459693166, - "velocityY": 2.303307101870708, - "timestamp": 10.050223415890475 + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -7.869331036874185e-31, + "timestamp": 6.618860121843355 }, { - "x": 7.465817541114002, - "y": 4.947544737637474, - "heading": 0.6314993734499258, - "angularVelocity": 0.34810138290582165, - "velocityX": 2.5447879970651583, - "velocityY": 2.322706184873219, - "timestamp": 10.09785904038095 + "x": 2.3794831364602356, + "y": 3.04172499089676, + "heading": -0.7962438371233982, + "angularVelocity": 0.09576162685715454, + "velocityX": 0.24659384848580665, + "velocityY": -0.11858712018957836, + "timestamp": 6.6818140946823945 + }, + { + "x": 2.4105331611816494, + "y": 3.0267927192788293, + "heading": -0.7842933789401895, + "angularVelocity": 0.1898284992078828, + "velocityX": 0.49321787523723476, + "velocityY": -0.23719347555886855, + "timestamp": 6.744768067521434 + }, + { + "x": 2.457111347137164, + "y": 3.0043922693157348, + "heading": -0.7665480516906409, + "angularVelocity": 0.2818777981640571, + "velocityX": 0.7398768315163422, + "velocityY": -0.3558226582517257, + "timestamp": 6.807722040360474 + }, + { + "x": 2.5192202688476826, + "y": 2.9745219425624034, + "heading": -0.7431604801963628, + "angularVelocity": 0.37150270967132, + "velocityX": 0.9865766830842898, + "velocityY": -0.4744788201008906, + "timestamp": 6.870676013199514 + }, + { + "x": 2.5968629770363694, + "y": 2.9371797310172574, + "heading": -0.7143164372650311, + "angularVelocity": 0.45817669053357035, + "velocityX": 1.2333249942335256, + "velocityY": -0.5931668782941151, + "timestamp": 6.933629986038554 + }, + { + "x": 2.690043132952377, + "y": 2.892363250439385, + "heading": -0.6802459845187668, + "angularVelocity": 0.5411962297181008, + "velocityX": 1.4801314629379994, + "velocityY": -0.711892809250637, + "timestamp": 6.9965839588775935 + }, + { + "x": 2.798765189175504, + "y": 2.840069647867451, + "heading": -0.6412406321012497, + "angularVelocity": 0.6195852407479587, + "velocityX": 1.7270086591851181, + "velocityY": -0.8306640584167341, + "timestamp": 7.059537931716633 + }, + { + "x": 2.9230346316689584, + "y": 2.7802954749466373, + "heading": -0.5976811625941267, + "angularVelocity": 0.6919256647788576, + "velocityX": 1.9739729978787044, + "velocityY": -0.9494900833922507, + "timestamp": 7.122491904555673 + }, + { + "x": 3.0628582875020656, + "y": 2.7130365228135442, + "heading": -0.550085909804202, + "angularVelocity": 0.7560325527288918, + "velocityX": 2.2210457819811675, + "velocityY": -1.0683829645677803, + "timestamp": 7.185445877394713 + }, + { + "x": 3.2182446226798445, + "y": 2.6382876468732976, + "heading": -0.4992026727158649, + "angularVelocity": 0.8082609372157451, + "velocityX": 2.4682530453648863, + "velocityY": -1.1873575656831603, + "timestamp": 7.248399850233753 + }, + { + "x": 3.3892034750687743, + "y": 2.5560428034877645, + "heading": -0.4462086118156311, + "angularVelocity": 0.8417905734992818, + "velocityX": 2.7156165795292284, + "velocityY": -1.306428167699852, + "timestamp": 7.311353823072793 + }, + { + "x": 3.57574133188106, + "y": 2.4662968714042055, + "heading": -0.3932458693377865, + "angularVelocity": 0.8412930922288103, + "velocityX": 2.963083160600891, + "velocityY": -1.4255801188754254, + "timestamp": 7.374307795911832 + }, + { + "x": 3.777810779911522, + "y": 2.369066952878771, + "heading": -0.3455559373144148, + "angularVelocity": 0.7575364964702819, + "velocityX": 3.209796600877132, + "velocityY": -1.5444604071935277, + "timestamp": 7.437261768750872 + }, + { + "x": 3.9925130304181535, + "y": 2.2661623966169606, + "heading": -0.3455557491568548, + "angularVelocity": 0.0000029888115329808544, + "velocityX": 3.4104638805811245, + "velocityY": -1.63459987703898, + "timestamp": 7.500215741589912 + }, + { + "x": 4.207067940824861, + "y": 2.162950872410893, + "heading": -0.34555573251479127, + "angularVelocity": 2.643528717349897e-7, + "velocityX": 3.408123438933375, + "velocityY": -1.6394759464975768, + "timestamp": 7.563169714428952 + }, + { + "x": 4.421622842445454, + "y": 2.0597393299403546, + "heading": -0.34555571587272765, + "angularVelocity": 2.6435287281597733e-7, + "velocityX": 3.408123299369276, + "velocityY": -1.6394762366217663, + "timestamp": 7.626123687267992 + }, + { + "x": 4.636177744065525, + "y": 1.9565277874687297, + "heading": -0.3455556992306641, + "angularVelocity": 2.6435287314542613e-7, + "velocityX": 3.4081232993609745, + "velocityY": -1.6394762366390223, + "timestamp": 7.689077660107031 + }, + { + "x": 4.850732645688907, + "y": 1.8533162450039902, + "heading": -0.34555568258860053, + "angularVelocity": 2.643528725209065e-7, + "velocityX": 3.408123299413585, + "velocityY": -1.6394762365296556, + "timestamp": 7.752031632946071 + }, + { + "x": 5.065287602977393, + "y": 1.7501048182552732, + "heading": -0.345555665946537, + "angularVelocity": 2.6435287138683083e-7, + "velocityX": 3.4081241836326663, + "velocityY": -1.63947439842449, + "timestamp": 7.814985605785111 + }, + { + "x": 5.280754789180012, + "y": 1.6488115807144432, + "heading": -0.34555564929395255, + "angularVelocity": 2.6451999265849417e-7, + "velocityX": 3.4226145942134942, + "velocityY": -1.6090046898202166, + "timestamp": 7.877939578624151 }, { - "x": 7.575645879536293, - "y": 5.054829291458012, - "heading": 0.6474951019450449, - "angularVelocity": 0.33579340391176205, - "velocityX": 2.305592497149964, - "velocityY": 2.252191610125547, - "timestamp": 10.145494664871425 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 2.673332686258823e-7, + "velocityX": 3.530338087601756, + "velocityY": -1.3564290298187671, + "timestamp": 7.940893551463191 + }, + { + "x": 5.652794664229654, + "y": 1.5139935049725783, + "heading": -0.34555563561361163, + "angularVelocity": -7.551097078688917e-8, + "velocityX": 3.591492706822241, + "velocityY": -1.185058979367509, + "timestamp": 7.982600740979571 + }, + { + "x": 5.804789442560329, + "y": 1.4718291930788845, + "heading": -0.345555638724863, + "angularVelocity": -7.459748339352162e-8, + "velocityX": 3.644330392269195, + "velocityY": -1.010960277655107, + "timestamp": 8.02430793049595 + }, + { + "x": 5.958396699024851, + "y": 1.435981562464388, + "heading": -0.3455556418269712, + "angularVelocity": -7.437825995471752e-8, + "velocityX": 3.6829922669374113, + "velocityY": -0.8595072223799174, + "timestamp": 8.066015120012331 + }, + { + "x": 6.112005102436535, + "y": 1.4001388468787674, + "heading": -0.3455556449290776, + "angularVelocity": -7.437821592181375e-8, + "velocityX": 3.6830197669242586, + "velocityY": -0.8593893762979032, + "timestamp": 8.107722309528711 + }, + { + "x": 6.2656135060730795, + "y": 1.3642961322568121, + "heading": -0.3455556480311839, + "angularVelocity": -7.437821455888137e-8, + "velocityX": 3.683019772315654, + "velocityY": -0.8593893531924107, + "timestamp": 8.149429499045091 + }, + { + "x": 6.419221909709655, + "y": 1.3284534176349936, + "heading": -0.3455556511332902, + "angularVelocity": -7.437821520024811e-8, + "velocityX": 3.6830197723164173, + "velocityY": -0.8593893531891351, + "timestamp": 8.191136688561471 + }, + { + "x": 6.572830313283965, + "y": 1.292610702746327, + "heading": -0.3455556542353965, + "angularVelocity": -7.437821564310945e-8, + "velocityX": 3.6830197708234897, + "velocityY": -0.859389359587264, + "timestamp": 8.232843878077851 + }, + { + "x": 6.72643839927336, + "y": 1.2567666268353574, + "heading": -0.34555565733750304, + "angularVelocity": -7.43782201062097e-8, + "velocityX": 3.6830121561911366, + "velocityY": -0.8594219923855618, + "timestamp": 8.274551067594231 + }, + { + "x": 6.87895592150965, + "y": 1.216534214678565, + "heading": -0.3455556607967647, + "angularVelocity": -8.294161584911855e-8, + "velocityX": 3.656864056409029, + "velocityY": -0.9646397329408027, + "timestamp": 8.316258257110611 + }, + { + "x": 7.025398163354816, + "y": 1.1717644512232448, + "heading": -0.34674583940477344, + "angularVelocity": -0.02853653343247435, + "velocityX": 3.5111989933450576, + "velocityY": -1.073430359955979, + "timestamp": 8.357965446626991 + }, + { + "x": 7.164406770259058, + "y": 1.1285160108597303, + "heading": -0.3473601506358553, + "angularVelocity": -0.014729144739916763, + "velocityX": 3.3329650958535093, + "velocityY": -1.0369540806994204, + "timestamp": 8.399672636143372 + }, + { + "x": 7.296003099222487, + "y": 1.086889230520204, + "heading": -0.3473718226301098, + "angularVelocity": -0.0002798556889061875, + "velocityX": 3.155243268351752, + "velocityY": -0.9980720547755352, + "timestamp": 8.441379825659752 + }, + { + "x": 7.420194691021883, + "y": 1.0469180216341771, + "heading": -0.34677182369168125, + "angularVelocity": 0.014385983457189529, + "velocityX": 2.977702243653219, + "velocityY": -0.9583769453064839, + "timestamp": 8.483087015176132 }, { - "x": 7.675112095346534, - "y": 5.156311134408354, - "heading": 0.6623068665871409, - "angularVelocity": 0.31093881523601674, - "velocityX": 2.0880636472002183, - "velocityY": 2.1303770872287977, - "timestamp": 10.1931302893619 + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.02916022972351966, + "velocityX": 2.8002535694982806, + "velocityY": -0.9182731897618387, + "timestamp": 8.524794204692512 + }, + { + "x": 7.694109552956691, + "y": 0.9554552181085981, + "heading": -0.3423846619503678, + "angularVelocity": 0.05119624293404516, + "velocityX": 2.536815276765165, + "velocityY": -0.8583516550337287, + "timestamp": 8.586731767532397 + }, + { + "x": 7.835030504559327, + "y": 0.9064832871477253, + "heading": -0.33886448301682054, + "angularVelocity": 0.05683431462499228, + "velocityX": 2.2752098264978664, + "velocityY": -0.7906660952654867, + "timestamp": 8.648669330372282 + }, + { + "x": 7.959827163561028, + "y": 0.861997905644216, + "heading": -0.3356203872211221, + "angularVelocity": 0.05237687191671972, + "velocityX": 2.014878424007598, + "velocityY": -0.7182294469433418, + "timestamp": 8.710606893212168 + }, + { + "x": 8.068556860689503, + "y": 0.8221974775860657, + "heading": -0.3330761096963967, + "angularVelocity": 0.04107810201222393, + "velocityX": 1.7554726428218221, + "velocityY": -0.6425895084221868, + "timestamp": 8.772544456052053 + }, + { + "x": 8.161262929678218, + "y": 0.7872247496182815, + "heading": -0.33153778712366017, + "angularVelocity": 0.02483666618774324, + "velocityX": 1.4967664973897774, + "velocityY": -0.5646448837225355, + "timestamp": 8.834482018891938 + }, + { + "x": 8.237979200485006, + "y": 0.7571873196552872, + "heading": -0.33123696700010097, + "angularVelocity": 0.0048568285506627, + "velocityX": 1.2386065464846836, + "velocityY": -0.48496305934161354, + "timestamp": 8.896419581731823 + }, + { + "x": 8.298732784455282, + "y": 0.7321691813322279, + "heading": -0.3323549200927614, + "angularVelocity": -0.018049678440697953, + "velocityX": 0.9808843161512628, + "velocityY": -0.4039251332464228, + "timestamp": 8.958357144571709 + }, + { + "x": 8.34354587924, + "y": 0.712237715373984, + "heading": -0.3350374219820423, + "angularVelocity": -0.04330977465508917, + "velocityX": 0.7235204733606144, + "velocityY": -0.3217993257140046, + "timestamp": 9.020294707411594 + }, + { + "x": 8.372436985155346, + "y": 0.6974481694465224, + "heading": -0.33940426042561933, + "angularVelocity": -0.07050387912204041, + "velocityX": 0.46645532356563796, + "velocityY": -0.23878152851596665, + "timestamp": 9.08223227025148 + }, + { + "x": 8.385421752929688, + "y": 0.6878466606140137, + "heading": -0.34555563246426124, + "angularVelocity": -0.09931569400855825, + "velocityX": 0.20964285934061647, + "velocityY": -0.15501915787887396, + "timestamp": 9.144169833091365 + }, + { + "x": 8.381221613866826, + "y": 0.6835499753387894, + "heading": -0.35422874908994917, + "angularVelocity": -0.13146724375807764, + "velocityX": -0.06366577665514053, + "velocityY": -0.06512922572222897, + "timestamp": 9.210141525946263 + }, + { + "x": 8.358990442151228, + "y": 0.6851839721097847, + "heading": -0.36494544812778174, + "angularVelocity": -0.16244389940702994, + "velocityX": -0.33698046470466403, + "velocityY": 0.024768149797052455, + "timestamp": 9.27611321880116 + }, + { + "x": 8.318727796191062, + "y": 0.692749219284977, + "heading": -0.37761522689458643, + "angularVelocity": -0.1920487139032685, + "velocityX": -0.6103018464104298, + "velocityY": 0.1146741404958606, + "timestamp": 9.342084911656059 + }, + { + "x": 8.260433187957593, + "y": 0.7062463828943455, + "heading": -0.3921309909220783, + "angularVelocity": -0.22003018869651583, + "velocityX": -0.8836306256637128, + "velocityY": 0.20459022688799033, + "timestamp": 9.408056604510957 + }, + { + "x": 8.184106080259626, + "y": 0.7256762539187386, + "heading": -0.40836397752669645, + "angularVelocity": -0.24605987662499226, + "velocityX": -1.1569675476699668, + "velocityY": 0.2945183029807682, + "timestamp": 9.474028297365855 + }, + { + "x": 8.089745888152331, + "y": 0.7510397855554056, + "heading": -0.4261563635075344, + "angularVelocity": -0.26969727789116266, + "velocityX": -1.4303133362794993, + "velocityY": 0.3844608276530502, + "timestamp": 9.539999990220753 + }, + { + "x": 7.977351991056692, + "y": 0.7823381451863918, + "heading": -0.445310079113529, + "angularVelocity": -0.2903323346290704, + "velocityX": -1.7036685316358853, + "velocityY": 0.47442104752147907, + "timestamp": 9.605971683075651 + }, + { + "x": 7.846923771917719, + "y": 0.8195727882757919, + "heading": -0.4655690572275494, + "angularVelocity": -0.30708592181466465, + "velocityX": -1.9770330803219518, + "velocityY": 0.5644033293384785, + "timestamp": 9.67194337593055 + }, + { + "x": 7.6984607267480145, + "y": 0.8627455646922209, + "heading": -0.48658930450770715, + "angularVelocity": -0.31862525229405353, + "velocityX": -2.2504052684572193, + "velocityY": 0.6544136514942158, + "timestamp": 9.737915068785448 + }, + { + "x": 7.531962771855591, + "y": 0.9118588688249081, + "heading": -0.5078842268520795, + "angularVelocity": -0.32278878141280287, + "velocityX": -2.5237787251969923, + "velocityY": 0.7444602678410827, + "timestamp": 9.803886761640346 + }, + { + "x": 7.347431182646945, + "y": 0.9669158200795512, + "heading": -0.5287129812318903, + "angularVelocity": -0.31572259977658373, + "velocityX": -2.7971328493042473, + "velocityY": 0.8345541681905657, + "timestamp": 9.869858454495244 + }, + { + "x": 7.144872022891467, + "y": 1.0279202386913098, + "heading": -0.5478111966657127, + "angularVelocity": -0.28949106211095804, + "velocityX": -3.0703950586958326, + "velocityY": 0.9247059757271185, + "timestamp": 9.935830147350142 + }, + { + "x": 6.92431385893063, + "y": 1.0948740954299652, + "heading": -0.56252983495188, + "angularVelocity": -0.2231053600297971, + "velocityX": -3.343224259015806, + "velocityY": 1.0148876562242728, + "timestamp": 10.00180184020504 + }, + { + "x": 6.686027050772059, + "y": 1.1677363127465568, + "heading": -0.5635938882487441, + "angularVelocity": -0.016128937288368943, + "velocityX": -3.611955337915506, + "velocityY": 1.1044466825619976, + "timestamp": 10.067773533059938 + }, + { + "x": 6.448996512762522, + "y": 1.2456322264917639, + "heading": -0.5635938947986299, + "angularVelocity": -9.928327608034937e-8, + "velocityX": -3.592912774436053, + "velocityY": 1.180747535409401, + "timestamp": 10.133745225914836 + }, + { + "x": 6.2119660667132655, + "y": 1.3235284200636117, + "heading": -0.5635939013485092, + "angularVelocity": -9.928317804375559e-8, + "velocityX": -3.5929113805006425, + "velocityY": 1.180751777026201, + "timestamp": 10.199716918769735 + }, + { + "x": 5.97493562067527, + "y": 1.4014246136697293, + "heading": -0.5635939078983885, + "angularVelocity": -9.928317794724392e-8, + "velocityX": -3.5929113803299297, + "velocityY": 1.180751777545662, + "timestamp": 10.265688611624633 + }, + { + "x": 5.737905297280568, + "y": 1.4793211804663164, + "heading": -0.5635939144482677, + "angularVelocity": -9.928317524609207e-8, + "velocityX": -3.5929095213008893, + "velocityY": 1.180757434372905, + "timestamp": 10.33166030447953 }, { - "x": 7.76511711692712, - "y": 5.250618542879285, - "heading": 0.6755917653702976, - "angularVelocity": 0.27888579031462285, - "velocityX": 1.8894477094255981, - "velocityY": 1.979766392897571, - "timestamp": 10.240765913852375 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.5635939210079286, + "angularVelocity": -9.943144757674115e-8, + "velocityX": -3.5606438133655525, + "velocityY": 1.2747558885869388, + "timestamp": 10.397631997334429 + }, + { + "x": 5.2859625551559315, + "y": 1.6593525606269757, + "heading": -0.5635939265318385, + "angularVelocity": -8.803796196510374e-8, + "velocityX": -3.459117077662582, + "velocityY": 1.5289526284347386, + "timestamp": 10.4603766333877 + }, + { + "x": 5.071426966449666, + "y": 1.7607653123039275, + "heading": -0.5635939320385204, + "angularVelocity": -8.776338927441567e-8, + "velocityX": -3.4191861201350386, + "velocityY": 1.6162776303436093, + "timestamp": 10.52312126944097 + }, + { + "x": 4.856891543272159, + "y": 1.8621784141511948, + "heading": -0.5635939375452019, + "angularVelocity": -8.776338338833754e-8, + "velocityX": -3.419183482000995, + "velocityY": 1.6162832112241017, + "timestamp": 10.585865905494241 + }, + { + "x": 4.642356120103932, + "y": 1.9635915160180906, + "heading": -0.5635939430518834, + "angularVelocity": -8.776338232016452e-8, + "velocityX": -3.419183481853115, + "velocityY": 1.6162832115369357, + "timestamp": 10.648610541547512 + }, + { + "x": 4.427820696935705, + "y": 2.0650046178849877, + "heading": -0.5635939485585649, + "angularVelocity": -8.776338320403535e-8, + "velocityX": -3.419183481853107, + "velocityY": 1.616283211536953, + "timestamp": 10.711355177600783 + }, + { + "x": 4.213285273767479, + "y": 2.1664177197518852, + "heading": -0.5635939540652464, + "angularVelocity": -8.776338205784925e-8, + "velocityX": -3.419183481853105, + "velocityY": 1.616283211536959, + "timestamp": 10.774099813654054 + }, + { + "x": 3.99874985060226, + "y": 2.2678308216251453, + "heading": -0.5635939595719279, + "angularVelocity": -8.776338204240658e-8, + "velocityX": -3.4191834818051703, + "velocityY": 1.616283211638361, + "timestamp": 10.836844449707325 + }, + { + "x": 3.7842144810928517, + "y": 2.369244037002313, + "heading": -0.5635939650808715, + "angularVelocity": -8.779943360299573e-8, + "velocityX": -3.4191826266593917, + "velocityY": 1.6162850206202093, + "timestamp": 10.899589085760596 + }, + { + "x": 3.582486282432141, + "y": 2.464592725052892, + "heading": -0.5976475269249973, + "angularVelocity": -0.5427326379774409, + "velocityX": -3.21506683837393, + "velocityY": 1.5196309046980254, + "timestamp": 10.962333721813867 + }, + { + "x": 3.3962755581787456, + "y": 2.5526066599990593, + "heading": -0.6290952664889575, + "angularVelocity": -0.5012020396016188, + "velocityX": -2.9677552690767404, + "velocityY": 1.4027324163844592, + "timestamp": 11.025078357867137 + }, + { + "x": 3.2255823358404876, + "y": 2.63328589857046, + "heading": -0.6579338206032609, + "angularVelocity": -0.4596178403173682, + "velocityX": -2.7204432613704155, + "velocityY": 1.2858348322062758, + "timestamp": 11.087822993920408 + }, + { + "x": 3.0704066332901094, + "y": 2.7066304971231805, + "heading": -0.684160167831089, + "angularVelocity": -0.4179854865292692, + "velocityX": -2.4731309688151453, + "velocityY": 1.168938146209836, + "timestamp": 11.15056762997368 + }, + { + "x": 2.930748464239733, + "y": 2.7726405069094633, + "heading": -0.7077714539525848, + "angularVelocity": -0.3763076432772636, + "velocityX": -2.22581845772133, + "velocityY": 1.05204227705202, + "timestamp": 11.21331226602695 + }, + { + "x": 2.8066078397359373, + "y": 2.83131597316776, + "heading": -0.7287650298436226, + "angularVelocity": -0.3345875792986393, + "velocityX": -1.978505770571369, + "velocityY": 0.9351471288873412, + "timestamp": 11.27605690208022 + }, + { + "x": 2.697984768963403, + "y": 2.882656934878273, + "heading": -0.7471385155745514, + "angularVelocity": -0.2928295849119221, + "velocityX": -1.7311929370394774, + "velocityY": 0.8182526019742015, + "timestamp": 11.338801538133492 + }, + { + "x": 2.604879259815539, + "y": 2.926663424723888, + "heading": -0.7628898637084475, + "angularVelocity": -0.25103895925897446, + "velocityX": -1.4838799777054614, + "velocityY": 0.7013585959483968, + "timestamp": 11.401546174186763 + }, + { + "x": 2.5272913193421016, + "y": 2.963335469132253, + "heading": -0.7760174152523261, + "angularVelocity": -0.20922189321065382, + "velocityX": -1.236566906015123, + "velocityY": 0.584465011116332, + "timestamp": 11.464290810240033 + }, + { + "x": 2.465220954107753, + "y": 2.9926730883504185, + "heading": -0.7865199463491176, + "angularVelocity": -0.1673853217966667, + "velocityX": -0.9892537296997038, + "velocityY": 0.46757174897401693, + "timestamp": 11.527035446293304 + }, + { + "x": 2.418668170473455, + "y": 3.014676296527397, + "heading": -0.7943967051912195, + "angularVelocity": -0.12553676836079783, + "velocityX": -0.7419404520057251, + "velocityY": 0.3506787123332304, + "timestamp": 11.589780082346575 + }, + { + "x": 2.38763297480514, + "y": 3.0293451017899056, + "heading": -0.7996474391139023, + "angularVelocity": -0.08368418805115034, + "velocityX": -0.49462707285394797, + "velocityY": 0.23378580521296682, + "timestamp": 11.652524718399846 }, { - "x": 7.846351174360072, - "y": 5.336925095447112, - "heading": 0.6871379054751507, - "angularVelocity": 0.2423845646688635, - "velocityX": 1.7053215592711606, - "velocityY": 1.8118068880378715, - "timestamp": 10.28840153834285 + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": -0.0418358130797821, + "velocityX": -0.24731358996990807, + "velocityY": 0.11689293258068652, + "timestamp": 11.715269354453117 }, { - "x": 7.919337423332426, - "y": 5.414694341101313, - "heading": 0.6968047845571967, - "angularVelocity": 0.2029338165594727, - "velocityX": 1.5321778553978798, - "velocityY": 1.6325858322640792, - "timestamp": 10.336037162833325 + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": 1.2905382194087526e-36, + "velocityX": 1.2188676912146853e-38, + "velocityY": -1.9456328527917923e-39, + "timestamp": 11.778013990506388 }, { - "x": 7.984478950500488, - "y": 5.483555793762207, - "heading": 0.7044936776046891, - "angularVelocity": 0.16141056467161, - "velocityX": 1.3674960256076552, - "velocityY": 1.445587276276031, - "timestamp": 10.3836727873238 + "x": 2.3862784269967885, + "y": 3.0299921206795677, + "heading": -0.7979445022798209, + "angularVelocity": 0.07214760590413372, + "velocityX": 0.23610252175550234, + "velocityY": -0.11148080618080955, + "timestamp": 11.838000869426745 }, { - "x": 8.05911422318913, - "y": 5.560200385763573, - "heading": 0.7111561296070518, - "angularVelocity": 0.10289890039880417, - "velocityX": 1.1527118676281318, - "velocityY": 1.1837449989371336, - "timestamp": 10.44842034027666 + "x": 2.414605712310544, + "y": 3.016617241283919, + "heading": -0.7893555783917804, + "angularVelocity": 0.14318004274641105, + "velocityX": 0.4722246901920793, + "velocityY": -0.22296341527296173, + "timestamp": 11.897987748347102 }, { - "x": 8.119756898561732, - "y": 5.61995838055026, - "heading": 0.7143078455831314, - "angularVelocity": 0.04867698982190534, - "velocityX": 0.9366018112955389, - "velocityY": 0.9229382742880431, - "timestamp": 10.513167893229522 + "x": 2.457098595113841, + "y": 2.996554734272641, + "heading": -0.7765849369819305, + "angularVelocity": 0.21289057940162145, + "velocityX": 0.7083696229589316, + "velocityY": -0.3344482555579276, + "timestamp": 11.95797462726746 }, { - "x": 8.166344572702991, - "y": 5.662879129934428, - "heading": 0.7141510891264902, - "angularVelocity": -0.002421040633850211, - "velocityX": 0.7195279515069205, - "velocityY": 0.6628937685941038, - "timestamp": 10.577915446182383 + "x": 2.5137586742709717, + "y": 2.9698044458506936, + "heading": -0.7597273153290622, + "angularVelocity": 0.28102181604163, + "velocityX": 0.9445412092927212, + "velocityY": -0.44593565965421605, + "timestamp": 12.017961506187817 }, { - "x": 8.198829842984775, - "y": 5.689000472186179, - "heading": 0.7108395294312745, - "angularVelocity": -0.0511457119873956, - "velocityX": 0.5017219771291601, - "velocityY": 0.4034336598136588, - "timestamp": 10.642662999135243 + "x": 2.5845878442638655, + "y": 2.9363662114763, + "heading": -0.7388971783301495, + "angularVelocity": 0.34724488711219914, + "velocityX": 1.180744377231747, + "velocityY": -0.5574258067133014, + "timestamp": 12.077948385108174 }, { - "x": 8.217175483703613, - "y": 5.698352336883545, - "heading": 0.7044936776046891, - "angularVelocity": -0.09800913759946452, - "velocityX": 0.28334106668393205, - "velocityY": 0.1444358013680284, - "timestamp": 10.707410552088104 + "x": 2.6695883810159726, + "y": 2.8962398712988433, + "heading": -0.7142349782126007, + "angularVelocity": 0.4111265757015274, + "velocityX": 1.4169854855252446, + "velocityY": -0.6689186185320777, + "timestamp": 12.137935264028531 }, { - "x": 8.221318658765867, - "y": 5.6908798741594095, - "heading": 0.6951703689604403, - "angularVelocity": -0.14352542875947455, - "velocityX": 0.06378111032529457, - "velocityY": -0.11503302714668015, - "timestamp": 10.77236983299625 + "x": 2.7687630628600255, + "y": 2.849425297465716, + "heading": -0.6859166617272401, + "angularVelocity": 0.4720751770225938, + "velocityX": 1.653272909492815, + "velocityY": -0.7804135616937368, + "timestamp": 12.197922142948888 }, { - "x": 8.210964366460544, - "y": 5.666753242665305, - "heading": 0.6829793287804198, - "angularVelocity": -0.18767203099521718, - "velocityX": -0.15939665834608732, - "velocityY": -0.3714116159663335, - "timestamp": 10.837329113904394 + "x": 2.882115344958, + "y": 2.795922445610523, + "heading": -0.6541688123580988, + "angularVelocity": 0.529246560923628, + "velocityX": 1.8896179320892517, + "velocityY": -0.8919092444570627, + "timestamp": 12.257909021869246 }, { - "x": 8.185835060378174, - "y": 5.6262170290983935, - "heading": 0.6680275277365855, - "angularVelocity": -0.2301718989927961, - "velocityX": -0.3868470483517696, - "velocityY": -0.6240249738021217, - "timestamp": 10.902288394812539 + "x": 3.009649615335316, + "y": 2.73573145985777, + "heading": -0.6192942663212875, + "angularVelocity": 0.5813695705541366, + "velocityX": 2.1260361044394105, + "velocityY": -1.0034025246198652, + "timestamp": 12.317895900789603 }, { - "x": 8.14559839699319, - "y": 5.569575312921383, - "heading": 0.6504460796726778, - "angularVelocity": -0.2706533665107588, - "velocityX": -0.6194136206938888, - "velocityY": -0.8719572536078877, - "timestamp": 10.967247675720683 + "x": 3.1513715692279023, + "y": 2.6688529097388005, + "heading": -0.5817190571900686, + "angularVelocity": 0.626390467507171, + "velocityX": 2.3625492181506127, + "velocityY": -1.114886310517361, + "timestamp": 12.37788277970996 }, { - "x": 8.08985004858283, - "y": 5.497215477528086, - "heading": 0.6303993620580922, - "angularVelocity": -0.30860436467780866, - "velocityX": -0.858204518753663, - "velocityY": -1.11392605308573, - "timestamp": 11.032206956628828 + "x": 3.3072887013147403, + "y": 2.5952884046594, + "heading": -0.5420886244414693, + "angularVelocity": 0.6606516868666416, + "velocityX": 2.5991872705003334, + "velocityY": -1.2263432671179573, + "timestamp": 12.437869658630317 }, { - "x": 8.018089118716903, - "y": 5.40964606854142, - "heading": 0.6080990792438148, - "angularVelocity": -0.3432963312172538, - "velocityX": -1.1047063462324833, - "velocityY": -1.348066169489957, - "timestamp": 11.097166237536973 + "x": 3.4774104648990076, + "y": 2.5150425607670805, + "heading": -0.50150111026719, + "angularVelocity": 0.6766065330414328, + "velocityX": 2.8359829123654157, + "velocityY": -1.33772327109832, + "timestamp": 12.497856537550675 }, { - "x": 7.9296824410081745, - "y": 5.307560214485479, - "heading": 0.5838269457597958, - "angularVelocity": -0.3736515112958315, - "velocityX": -1.3609553011176063, - "velocityY": -1.5715360858180332, - "timestamp": 11.162125518445118 + "x": 3.6617427045443476, + "y": 2.428132084038069, + "heading": -0.4622625664613271, + "angularVelocity": 0.6541187758402723, + "velocityX": 3.0728759849311564, + "velocityY": -1.4488247812392312, + "timestamp": 12.557843416471032 }, { - "x": 7.823812920396748, - "y": 5.191948689740781, - "heading": 0.5579732566194568, - "angularVelocity": -0.3979983888198644, - "velocityX": -1.6297828290483853, - "velocityY": -1.779753764641825, - "timestamp": 11.227084799353262 + "x": 3.8601464039752953, + "y": 2.3346891355318427, + "heading": -0.4327535611710116, + "angularVelocity": 0.49192433114404066, + "velocityX": 3.3074516127828706, + "velocityY": -1.5577231252568986, + "timestamp": 12.61783029539139 }, { - "x": 7.699409898040676, - "y": 5.064316508380435, - "heading": 0.5311065320658849, - "angularVelocity": -0.41359331842916214, - "velocityX": -1.9150923565792266, - "velocityY": -1.9648028669039912, - "timestamp": 11.292044080261407 + "x": 4.065242173401204, + "y": 2.237711412138144, + "heading": -0.4327535477418676, + "angularVelocity": 2.2386802351026997e-7, + "velocityX": 3.4190105089182294, + "velocityY": -1.6166489262168848, + "timestamp": 12.677817174311746 }, { - "x": 7.555088273925506, - "y": 4.927129260587564, - "heading": 0.5041069990932381, - "angularVelocity": -0.4156378056405048, - "velocityX": -2.221724472585296, - "velocityY": -2.111896035100176, - "timestamp": 11.357003361169552 + "x": 4.2703379249938225, + "y": 2.140733651028875, + "heading": -0.43275353431315516, + "angularVelocity": 2.2386082807599726e-7, + "velocityX": 3.419010211631728, + "velocityY": -1.6166495549472233, + "timestamp": 12.737804053232104 }, { - "x": 7.389294037285908, - "y": 4.784758435393721, - "heading": 0.47842631175838873, - "angularVelocity": -0.3953351542047192, - "velocityX": -2.5522794329271568, - "velocityY": -2.1916933685759186, - "timestamp": 11.421962642077697 + "x": 4.475433676585232, + "y": 2.0437558899170503, + "heading": -0.43275352088444274, + "angularVelocity": 2.2386082918741598e-7, + "velocityX": 3.4190102116115857, + "velocityY": -1.616649554989821, + "timestamp": 12.797790932152461 }, { - "x": 7.201584646398764, - "y": 4.645022669025462, - "heading": 0.45644938425338794, - "angularVelocity": -0.33831851581111005, - "velocityX": -2.8896469952087562, - "velocityY": -2.1511285903218442, - "timestamp": 11.486921922985841 + "x": 4.6805294281766425, + "y": 1.9467781288052255, + "heading": -0.4327535074557304, + "angularVelocity": 2.2386082819075723e-7, + "velocityX": 3.4190102116115852, + "velocityY": -1.616649554989824, + "timestamp": 12.857777811072818 }, { - "x": 6.996114167384252, - "y": 4.5183305598699945, - "heading": 0.4409940125357126, - "angularVelocity": -0.23792399641137907, - "velocityX": -3.16306578739771, - "velocityY": -1.9503311518274693, - "timestamp": 11.551881203893986 + "x": 4.885625179768054, + "y": 1.8498003676934025, + "heading": -0.43275349402701796, + "angularVelocity": 2.2386082903608912e-7, + "velocityX": 3.4190102116115972, + "velocityY": -1.6166495549897977, + "timestamp": 12.917764689993176 }, { - "x": 6.77954298581377, - "y": 4.410738153252425, - "heading": 0.4262239025498049, - "angularVelocity": -0.2273748997744145, - "velocityX": -3.3339528785227577, - "velocityY": -1.656305382593562, - "timestamp": 11.61684048480213 + "x": 5.090720931370459, + "y": 1.7528226066048298, + "heading": -0.43275348059830554, + "angularVelocity": 2.2386082830289002e-7, + "velocityX": 3.4190102117948658, + "velocityY": -1.6166495546022077, + "timestamp": 12.977751568913533 }, { - "x": 6.556274698008425, - "y": 4.324091773722086, - "heading": 0.4025359766205174, - "angularVelocity": -0.3646580688413556, - "velocityX": -3.43704986699365, - "velocityY": -1.3338568149000936, - "timestamp": 11.681799765710275 + "x": 5.29581684523517, + "y": 1.6558451886813434, + "heading": -0.43275346716959256, + "angularVelocity": 2.2386083719980442e-7, + "velocityX": 3.4190129167581613, + "velocityY": -1.6166438339330798, + "timestamp": 13.03773844783389 }, { - "x": 6.330620508913663, - "y": 4.259333422241355, - "heading": 0.3712479640794565, - "angularVelocity": -0.4816557711792279, - "velocityX": -3.4737790495840706, - "velocityY": -0.996906840337434, - "timestamp": 11.74675904661842 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.43275345370859003, + "angularVelocity": 2.243991161112281e-7, + "velocityX": 3.453867841656271, + "velocityY": -1.5407736813730113, + "timestamp": 13.097725326754247 }, { - "x": 6.105727463248204, - "y": 4.216621857922548, - "heading": 0.33499279771425994, - "angularVelocity": -0.5581214240419701, - "velocityX": -3.462061810435755, - "velocityY": -0.6575128868683483, - "timestamp": 11.811718327526565 + "x": 5.767637732978587, + "y": 1.4715486447521433, + "heading": -0.4327534418952186, + "angularVelocity": 1.5949042666645356e-7, + "velocityX": 3.572782883962456, + "velocityY": -1.2403266728714897, + "timestamp": 13.171794797056224 }, { - "x": 5.88368699546657, - "y": 4.195803829122771, - "heading": 0.29554108787827377, - "angularVelocity": -0.6073298423942238, - "velocityX": -3.4181484874441033, - "velocityY": -0.3204781288945418, - "timestamp": 11.87667760843471 + "x": 6.033774192023134, + "y": 1.384125705645109, + "heading": -0.432753430099036, + "angularVelocity": 1.5925836332865082e-7, + "velocityX": 3.593065509440347, + "velocityY": -1.1802830336252828, + "timestamp": 13.2458642673582 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.2541387867771501, - "angularVelocity": -0.6373577496904229, - "velocityX": -3.351873105763798, - "velocityY": 0.013011927161991938, - "timestamp": 11.941636889342854 + "x": 6.2999107025526015, + "y": 1.2967029232707166, + "heading": -0.43275341830285335, + "angularVelocity": 1.5925836332521055e-7, + "velocityX": 3.5930662045299844, + "velocityY": -1.1802809176031086, + "timestamp": 13.319933737660177 }, { - "x": 5.421845873128357, - "y": 4.226158090162152, - "heading": 0.20498652451135702, - "angularVelocity": -0.6557520326399592, - "velocityX": -3.256674335436179, - "velocityY": 0.3936868024775833, - "timestamp": 12.01659245153595 + "x": 6.566047160708243, + "y": 1.2092799814598572, + "heading": -0.43275340650489624, + "angularVelocity": 1.5928232033441407e-7, + "velocityX": 3.593065497439389, + "velocityY": -1.1802830701291938, + "timestamp": 13.394003207962154 }, { - "x": 5.1888462982312005, - "y": 4.282903811630084, - "heading": 0.15538584116955914, - "angularVelocity": -0.6617345249712095, - "velocityX": -3.1085027992574967, - "velocityY": 0.7570581796417871, - "timestamp": 12.091548013729044 + "x": 6.815604168443372, + "y": 1.1270295237235333, + "heading": -0.36800659105834704, + "angularVelocity": 0.8741363369088632, + "velocityX": 3.3692290051177802, + "velocityY": -1.110450194945293, + "timestamp": 13.46807267826413 }, { - "x": 4.97232882862877, - "y": 4.364011234145266, - "heading": 0.10667736071804554, - "angularVelocity": -0.6498314338038612, - "velocityX": -2.8886111086013, - "velocityY": 1.08207343313948, - "timestamp": 12.166503575922139 + "x": 7.0424486777806585, + "y": 1.0523256982782683, + "heading": -0.304588986464792, + "angularVelocity": 0.8561908750664192, + "velocityX": 3.0625912189253843, + "velocityY": -1.0085643267152118, + "timestamp": 13.542142148566107 }, { - "x": 4.778685496187651, - "y": 4.463599711895904, - "heading": 0.06061512765816293, - "angularVelocity": -0.6145272173560689, - "velocityX": -2.5834417990524554, - "velocityY": 1.3286335908479392, - "timestamp": 12.241459138115234 + "x": 7.246590574581559, + "y": 0.9851213052520007, + "heading": -0.24567984244132185, + "angularVelocity": 0.7953228743678282, + "velocityX": 2.7560869001577597, + "velocityY": -0.9073156963628868, + "timestamp": 13.616211618868084 }, { - "x": 4.613047571450946, - "y": 4.572114931147961, - "heading": 0.018966273025947666, - "angularVelocity": -0.5556472850530078, - "velocityX": -2.2098149875789206, - "velocityY": 1.4477273744209653, - "timestamp": 12.316414700308329 + "x": 7.428038548794457, + "y": 0.9253999088853845, + "heading": -0.19230647763371986, + "angularVelocity": 0.7205852099387576, + "velocityX": 2.4496999029849618, + "velocityY": -0.8062889625528061, + "timestamp": 13.69028108917006 }, { - "x": 4.476747715125104, - "y": 4.67904248790438, - "heading": -0.017090034062622824, - "angularVelocity": -0.4810357768471503, - "velocityX": -1.8184088323521106, - "velocityY": 1.4265459910894789, - "timestamp": 12.391370262501423 + "x": 7.586797949690041, + "y": 0.8731533407207944, + "heading": -0.1449799929692073, + "angularVelocity": 0.6389472541327181, + "velocityX": 2.1433851254549694, + "velocityY": -0.705372509774734, + "timestamp": 13.764350559472037 }, { - "x": 4.368193539344783, - "y": 4.776321878924991, - "heading": -0.04707218407807139, - "angularVelocity": -0.3999990012510431, - "velocityX": -1.4482471027389752, - "velocityY": 1.2978275150549403, - "timestamp": 12.466325824694518 + "x": 7.722872327084393, + "y": 0.8283767866857153, + "heading": -0.10400779573923778, + "angularVelocity": 0.5531590419497899, + "velocityX": 1.8371182734206921, + "velocityY": -0.6045210510150558, + "timestamp": 13.838420029774014 }, { - "x": 4.2849948544017495, - "y": 4.858769971637843, - "heading": -0.07089061350138144, - "angularVelocity": -0.3177673374252157, - "velocityX": -1.1099734630593825, - "velocityY": 1.0999596334219341, - "timestamp": 12.541281386887613 + "x": 7.836264194581954, + "y": 0.7910670998170554, + "heading": -0.06959570799723012, + "angularVelocity": 0.46459205934255987, + "velocityX": 1.530885357155523, + "velocityY": -0.5037120788977025, + "timestamp": 13.91248950007599 }, { - "x": 4.224952920077694, - "y": 4.923143218233113, - "heading": -0.08860420951463882, - "angularVelocity": -0.23632130151495423, - "velocityX": -0.8010337400896302, - "velocityY": 0.8588188082618416, - "timestamp": 12.616236949080708 + "x": 7.926975414267802, + "y": 0.7612220655404804, + "heading": -0.04189104317473826, + "angularVelocity": 0.3740362218001811, + "velocityX": 1.224677580601353, + "velocityY": -0.4029330053920843, + "timestamp": 13.986558970377967 }, { - "x": 4.186266834046781, - "y": 4.967343359425017, - "heading": -0.10031361199761639, - "angularVelocity": -0.1562179262002281, - "velocityX": -0.5161202837923176, - "velocityY": 0.5896846064343946, - "timestamp": 12.691192511273803 + "x": 7.995007406512269, + "y": 0.7388400306235591, + "heading": -0.021003806792755122, + "angularVelocity": 0.28199521741990763, + "velocityX": 0.9184889802384877, + "velocityY": -0.3021762519115013, + "timestamp": 14.060628440679944 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.07749108715135035, - "velocityX": -0.250385269598314, - "velocityY": 0.3015711680564945, - "timestamp": 12.766148073466898 + "x": 8.040361272813582, + "y": 0.7239196956147146, + "heading": -0.0070182972067734635, + "angularVelocity": 0.18881611450660718, + "velocityX": 0.612315251025934, + "velocityY": -0.20143704211755914, + "timestamp": 14.13469791098192 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -7.372861805121809e-39, - "velocityX": 4.014609310130765e-40, - "velocityY": -1.5891469244205483e-39, - "timestamp": 12.841103635659993 + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -5.172058523180806e-42, + "angularVelocity": 0.09475290127174331, + "velocityX": 0.30615312096089364, + "velocityY": -0.10071229126619016, + "timestamp": 14.208767381283897 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -2.2197990868657932e-42, + "angularVelocity": 3.9858008032330975e-41, + "velocityX": -4.443522904196121e-42, + "velocityY": 7.940802598890531e-42, + "timestamp": 14.282836851585873 } ], "constraints": [ @@ -23919,19 +27952,13 @@ }, { "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 5 + 6 ], "type": "StopPoint" }, { "scope": [ - 2 + 1 ], "type": "StopPoint" }, diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj index 3efa5d70..2bcc94b3 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj @@ -4,307 +4,307 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": -8.928261679081423e-33, - "velocityX": -3.922322521208882e-33, - "velocityY": -1.0855099176840522e-33, + "angularVelocity": -2.75917641303777e-30, + "velocityX": -8.847327306636262e-30, + "velocityY": 1.4420464788285476e-29, "timestamp": 0 }, { - "x": 1.306057392567738, - "y": 5.572382489557579, - "heading": -0.01069490392439116, - "angularVelocity": -0.14212387666683293, - "velocityX": 0.21372515459002697, - "velocityY": -0.2467996203807809, - "timestamp": 0.07525057840535954 - }, - { - "x": 1.33822347033388, - "y": 5.535239164732594, - "heading": -0.0320761792409761, - "angularVelocity": -0.28413436507302803, - "velocityX": 0.4274528973434579, - "velocityY": -0.49359520700160897, - "timestamp": 0.15050115681071907 - }, - { - "x": 1.3864731960688501, - "y": 5.4795246417936365, - "heading": -0.06412173122646007, - "angularVelocity": -0.42585123815077036, - "velocityX": 0.6411874401158554, - "velocityY": -0.7403866404698544, - "timestamp": 0.2257517352160786 - }, - { - "x": 1.4508074313630048, - "y": 5.405239333647035, - "heading": -0.1067984096254206, - "angularVelocity": -0.567127579658856, - "velocityX": 0.8549334325059841, - "velocityY": -0.9871725868529632, - "timestamp": 0.30100231362143814 - }, - { - "x": 1.5312274241077561, - "y": 5.312383875814834, - "heading": -0.16006570142282744, - "angularVelocity": -0.707865546367854, - "velocityX": 1.0686960080432237, - "velocityY": -1.233950087825352, - "timestamp": 0.3762528920267977 - }, - { - "x": 1.6277348707066417, - "y": 5.200959293585713, - "heading": -0.2238805012390217, - "angularVelocity": -0.848030688514272, - "velocityX": 1.2824811269757883, - "velocityY": -1.4807139638036912, - "timestamp": 0.4515034704321572 - }, - { - "x": 1.7403321345867286, - "y": 5.070967309799712, - "heading": -0.29820239377845176, - "angularVelocity": -0.9876587544493436, - "velocityX": 1.4962976533356125, - "velocityY": -1.727454945073796, - "timestamp": 0.5267540488375168 - }, - { - "x": 1.8690235919905007, - "y": 4.922411591690569, - "heading": -0.3829978316122351, - "angularVelocity": -1.126841010802703, - "velocityX": 1.7101723352947207, - "velocityY": -1.9741471927152945, - "timestamp": 0.6020046272428763 - }, - { - "x": 2.021840076397225, - "y": 4.778612689187088, - "heading": -0.46243678907347296, - "angularVelocity": -1.055659094516408, - "velocityX": 2.030768236538101, - "velocityY": -1.9109341821781907, - "timestamp": 0.6772552056482358 - }, - { - "x": 2.158571900473102, - "y": 4.653385130963448, - "heading": -0.5314200615226269, - "angularVelocity": -0.916714182282495, - "velocityX": 1.8170202405532427, - "velocityY": -1.6641408063212961, - "timestamp": 0.7525057840535954 - }, - { - "x": 2.2792154502461437, - "y": 4.546725640046215, - "heading": -0.5899528620106769, - "angularVelocity": -0.7778385459410783, - "velocityX": 1.6032242187317085, - "velocityY": -1.4173909779494496, - "timestamp": 0.8277563624589549 - }, - { - "x": 2.383769251108642, - "y": 4.458632746099449, - "heading": -0.6380359031198586, - "angularVelocity": -0.6389723790582461, - "velocityX": 1.3894086009450728, - "velocityY": -1.1706606887754087, - "timestamp": 0.9030069408643144 - }, - { - "x": 2.472232460585404, - "y": 4.389105547578634, - "heading": -0.6756681828246474, - "angularVelocity": -0.5000928963239504, - "velocityX": 1.175581787560874, - "velocityY": -0.9239423801673171, - "timestamp": 0.978257519269674 - }, - { - "x": 2.544604550352729, - "y": 4.338143448939913, - "heading": -0.7028480663779157, - "angularVelocity": -0.3611916895423175, - "velocityX": 0.9617479533176606, - "velocityY": -0.677231985702458, - "timestamp": 1.0535080976750335 - }, - { - "x": 2.6008851962155197, - "y": 4.305746063716667, - "heading": -0.7195737337551226, - "angularVelocity": -0.22226629657394859, - "velocityX": 0.7479098108670842, - "velocityY": -0.4305267269671826, - "timestamp": 1.128758676080393 - }, - { - "x": 2.641074231196452, - "y": 4.291913169677742, - "heading": -0.7258433495685629, - "angularVelocity": -0.08331651325877153, - "velocityX": 0.5340694494657862, - "velocityY": -0.18382442144709177, - "timestamp": 1.2040092544857526 + "x": 1.3060574436245773, + "y": 5.572382537817046, + "heading": -0.010694867587036427, + "angularVelocity": -0.14212340511596666, + "velocityX": 0.21372585011902087, + "velocityY": -0.24679899874441508, + "timestamp": 0.07525057240542533 + }, + { + "x": 1.3382236275279018, + "y": 5.53523931290218, + "heading": -0.032076067517629925, + "angularVelocity": -0.2841333859323569, + "velocityX": 0.42745434186951237, + "velocityY": -0.4935939186635594, + "timestamp": 0.15050114481085067 + }, + { + "x": 1.386473519841097, + "y": 5.479524946048311, + "heading": -0.0641215016247012, + "angularVelocity": -0.42584970563284796, + "velocityX": 0.6411897048771923, + "velocityY": -0.7403846253066342, + "timestamp": 0.22575171721627602 + }, + { + "x": 1.450807989654895, + "y": 5.405239856496701, + "heading": -0.10679801494850981, + "angularVelocity": -0.5671254312134459, + "velocityX": 0.8549366171751026, + "velocityY": -0.9871697606799944, + "timestamp": 0.30100228962170134 + }, + { + "x": 1.5312282961066404, + "y": 5.312384689270129, + "heading": -0.16006508743981135, + "angularVelocity": -0.7078626884752101, + "velocityX": 1.0687002620672836, + "velocityY": -1.23394632438782, + "timestamp": 0.37625286202712666 + }, + { + "x": 1.627736154340151, + "y": 5.200960485487107, + "heading": -0.22387960186124028, + "angularVelocity": -0.8480269635530943, + "velocityX": 1.2824866993939017, + "velocityY": -1.4807090527398261, + "timestamp": 0.451503434432552 + }, + { + "x": 1.7403339652666383, + "y": 5.0709689996393825, + "heading": -0.2982011188077779, + "angularVelocity": -0.9876538419890185, + "velocityX": 1.4963050422725384, + "velocityY": -1.7274484657660032, + "timestamp": 0.5267540068379774 + }, + { + "x": 1.8690262176186487, + "y": 4.922413993905078, + "heading": -0.382996017565801, + "angularVelocity": -1.1268339369356364, + "velocityX": 1.710183035621927, + "velocityY": -1.9741378834553056, + "timestamp": 0.6020045792434027 + }, + { + "x": 2.0218420905066052, + "y": 4.778614575286612, + "heading": -0.4624353832107095, + "angularVelocity": -1.055664602990153, + "velocityX": 2.030760272057439, + "velocityY": -1.9109411931313167, + "timestamp": 0.677255151648828 + }, + { + "x": 2.15857350503456, + "y": 4.653386666317764, + "heading": -0.5314189322497749, + "angularVelocity": -0.9167179309443209, + "velocityX": 1.8170149429960494, + "velocityY": -1.6641456000181232, + "timestamp": 0.7525057240542533 + }, + { + "x": 2.2792167348207513, + "y": 4.5467268950736175, + "heading": -0.589951951408673, + "angularVelocity": -0.7778415138506264, + "velocityX": 1.6032200942942694, + "velocityY": -1.4173948161921597, + "timestamp": 0.8277562964596786 + }, + { + "x": 2.38377026778444, + "y": 4.458633759569044, + "heading": -0.6380351777892487, + "angularVelocity": -0.6389748920543931, + "velocityX": 1.3894051516512624, + "velocityY": -1.1706639921478266, + "timestamp": 0.903006868865104 + }, + { + "x": 2.472233242714231, + "y": 4.389106342435906, + "heading": -0.6756676216200074, + "angularVelocity": -0.5000951172479533, + "velocityX": 1.1755787644266529, + "velocityY": -0.9239453589485842, + "timestamp": 0.9782574412705293 + }, + { + "x": 2.5446051200448103, + "y": 4.3381440386367105, + "heading": -0.7028476555095471, + "angularVelocity": -0.3611937161417901, + "velocityX": 0.9617452069540623, + "velocityY": -0.6772347660530061, + "timestamp": 1.0535080136759547 + }, + { + "x": 2.600885568086537, + "y": 4.305746455375921, + "heading": -0.7195734643408855, + "angularVelocity": -0.22226819406292112, + "velocityX": 0.7479072416796508, + "velocityY": -0.4305293929915994, + "timestamp": 1.12875858608138 + }, + { + "x": 2.641074414508583, + "y": 4.291913365901662, + "heading": -0.7258432162336076, + "angularVelocity": -0.08331832824324738, + "velocityX": 0.5340669863123017, + "velocityY": -0.18382703322097846, + "timestamp": 1.2040091584868053 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.055657173156648594, - "velocityX": 0.320228661947563, - "velocityY": 0.06287683198072233, - "timestamp": 1.2792598328911122 - }, - { - "x": 2.6731094248331484, - "y": 4.3202884971859605, - "heading": -0.7067986870475855, - "angularVelocity": 0.19580509628602769, - "velocityX": 0.1046188223798005, - "velocityY": 0.3116212313704759, - "timestamp": 1.355133384347763 - }, - { - "x": 2.6646882165450285, - "y": 4.362805712309731, - "heading": -0.6813129128724049, - "angularVelocity": 0.3358979998417677, - "velocityX": -0.11099003706094854, - "velocityY": 0.5603693817873046, - "timestamp": 1.431006935804414 - }, - { - "x": 2.6399081500889086, - "y": 4.424196738034775, - "heading": -0.6452019314674031, - "angularVelocity": 0.47593635346874824, - "velocityX": -0.3265968968155938, - "velocityY": 0.8091228701758959, - "timestamp": 1.5068804872610648 - }, - { - "x": 2.5987694540060504, - "y": 4.504462102027486, - "heading": -0.5984691810773222, - "angularVelocity": 0.6159293916376241, - "velocityX": -0.542200744436769, - "velocityY": 1.0578833131143173, - "timestamp": 1.5827540387177157 - }, - { - "x": 2.5412724297463463, - "y": 4.603602434972891, - "heading": -0.5411158632576627, - "angularVelocity": 0.7559065935173384, - "velocityX": -0.757800619001658, - "velocityY": 1.3066520683699112, - "timestamp": 1.6586275901743666 - }, - { - "x": 2.4674174122154677, - "y": 4.7216183838541035, - "heading": -0.4731379535005828, - "angularVelocity": 0.8959368376992063, - "velocityX": -0.9733960795689668, - "velocityY": 1.5554293507486547, - "timestamp": 1.7345011416310174 - }, - { - "x": 2.37720454589188, - "y": 4.858510309507254, - "heading": -0.39452114170963426, - "angularVelocity": 1.0361556864234158, - "velocityX": -1.188989635935645, - "velocityY": 1.8042113888838065, - "timestamp": 1.8103746930876683 - }, - { - "x": 2.270632337422942, - "y": 5.01427685954172, - "heading": -0.305233667532112, - "angularVelocity": 1.1767931309836617, - "velocityX": -1.4046028744262238, - "velocityY": 2.0529756027495263, - "timestamp": 1.8862482445443192 - }, - { - "x": 2.156211319422429, - "y": 5.146461372898718, - "heading": -0.2291032339501207, - "angularVelocity": 1.003385661016371, - "velocityX": -1.508048797028907, - "velocityY": 1.7421685267035296, - "timestamp": 1.96212179600097 - }, - { - "x": 2.058134463277846, - "y": 5.259759383515319, - "heading": -0.16374337088202898, - "angularVelocity": 0.8614314449935028, - "velocityX": -1.2926356320702503, - "velocityY": 1.493247758164214, - "timestamp": 2.037995347457621 - }, - { - "x": 1.976403644982925, - "y": 5.354173277081842, - "heading": -0.10920518207341517, - "angularVelocity": 0.7188036906348487, - "velocityX": -1.0771977418457426, - "velocityY": 1.2443584326016996, - "timestamp": 2.113868898914272 - }, - { - "x": 1.9110189705231253, - "y": 5.429703862947542, - "heading": -0.06553401357556585, - "angularVelocity": 0.5755782833336871, - "velocityX": -0.861758454751598, - "velocityY": 0.9954797741193555, - "timestamp": 2.189742450370923 - }, - { - "x": 1.8619803278189464, - "y": 5.486351581956828, - "heading": -0.032764600292336035, - "angularVelocity": 0.43189507613798817, - "velocityX": -0.6463206448454762, - "velocityY": 0.7466069258884014, - "timestamp": 2.265616001827574 - }, - { - "x": 1.829287687812507, - "y": 5.524116686023993, - "heading": -0.010918035230124941, - "angularVelocity": 0.28793386684545474, - "velocityX": -0.43088321791708567, - "velocityY": 0.4977373978433378, - "timestamp": 2.3414895532842253 + "angularVelocity": 0.05565540572186624, + "velocityX": 0.32022625146750794, + "velocityY": 0.06287422939745346, + "timestamp": 1.2792597308922307 + }, + { + "x": 2.6731092407857906, + "y": 4.320288302812448, + "heading": -0.7067988166038234, + "angularVelocity": 0.1958033723255861, + "velocityX": 0.10461638788158735, + "velocityY": 0.31161864341117435, + "timestamp": 1.3551332887134768 + }, + { + "x": 2.664687841052256, + "y": 4.3628053226693035, + "heading": -0.6813131694887641, + "angularVelocity": 0.33589629702821755, + "velocityX": -0.11099255097832775, + "velocityY": 0.5603667611869495, + "timestamp": 1.431006846534723 + }, + { + "x": 2.6399075701479586, + "y": 4.42419614745244, + "heading": -0.6452023149092349, + "angularVelocity": 0.47593464200079133, + "velocityX": -0.32659956402163576, + "velocityY": 0.8091201539132412, + "timestamp": 1.5068804043559691 + }, + { + "x": 2.598768648766779, + "y": 4.504461298133711, + "heading": -0.5984696942887661, + "angularVelocity": 0.6159276296230222, + "velocityX": -0.5422036683592814, + "velocityY": 1.057880412955572, + "timestamp": 1.5827539621772153 + }, + { + "x": 2.5412713665853555, + "y": 4.603601395355974, + "heading": -0.5411165139934166, + "angularVelocity": 0.7559047175548449, + "velocityX": -0.7578039548110096, + "velocityY": 1.3066488519598491, + "timestamp": 1.6586275199984615 + }, + { + "x": 2.4674160388824338, + "y": 4.721617069365857, + "heading": -0.47313875765168273, + "angularVelocity": 0.8959347405484323, + "velocityX": -0.9734000859460189, + "velocityY": 1.5554255975016502, + "timestamp": 1.7345010778197076 + }, + { + "x": 2.377202770872684, + "y": 4.858508647531332, + "heading": -0.39452213175531803, + "angularVelocity": 1.0361531494380563, + "velocityX": -1.1889948303725923, + "velocityY": 1.8042066576932412, + "timestamp": 1.8103746356409538 + }, + { + "x": 2.270629951391599, + "y": 5.014274677090094, + "heading": -0.305234926659776, + "angularVelocity": 1.1767894858012566, + "velocityX": -1.4046108096674554, + "velocityY": 2.0529685707303664, + "timestamp": 1.8862481934622 + }, + { + "x": 2.1562097285316435, + "y": 5.146459905330176, + "heading": -0.22910409225153178, + "angularVelocity": 1.0033908596890408, + "velocityX": -1.5080381906707807, + "velocityY": 1.7421778026344412, + "timestamp": 1.9621217512834461 + }, + { + "x": 2.0581334055736438, + "y": 5.259758402247803, + "heading": -0.16374394944584372, + "angularVelocity": 0.8614350596401947, + "velocityX": -1.2926284963034982, + "velocityY": 1.4932540422920335, + "timestamp": 2.0379953091046925 + }, + { + "x": 1.9764029763301112, + "y": 5.354172653968883, + "heading": -0.10920555144834718, + "angularVelocity": 0.7188063874223503, + "velocityX": -1.0771925238352613, + "velocityY": 1.2443630486526756, + "timestamp": 2.1138688669259387 + }, + { + "x": 1.9110185860607423, + "y": 5.429703503317112, + "heading": -0.06553422750381226, + "angularVelocity": 0.5755802838199697, + "velocityX": -0.8617546368678625, + "velocityY": 0.9954831632816045, + "timestamp": 2.189742424747185 + }, + { + "x": 1.8619801423257207, + "y": 5.486351407872208, + "heading": -0.032764704050183134, + "angularVelocity": 0.43189649194112756, + "velocityX": -0.6463179682397073, + "velocityY": 0.7466093087307788, + "timestamp": 2.265615982568431 + }, + { + "x": 1.8292876278534882, + "y": 5.524116629586748, + "heading": -0.010918068889347814, + "angularVelocity": 0.28793476658359446, + "velocityX": -0.43088152724695716, + "velocityY": 0.4977389066689233, + "timestamp": 2.341489540389677 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -6.884486248240525e-32, - "angularVelocity": 0.14389777492309389, - "velocityX": -0.21544390526149312, - "velocityY": 0.2488690879972792, - "timestamp": 2.4173631047408763 + "heading": 2.394848489426687e-27, + "angularVelocity": 0.14389820647675833, + "velocityX": -0.21544309693644778, + "velocityY": 0.24886981095702526, + "timestamp": 2.4173630982109233 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -2.79027732569672e-32, - "angularVelocity": 5.39778462149558e-31, - "velocityX": -1.372141826224318e-33, - "velocityY": 4.2150961560958763e-32, - "timestamp": 2.4932366561975274 + "heading": 2.3925172511788446e-27, + "angularVelocity": -3.168744293663733e-29, + "velocityX": 8.48223659243716e-32, + "velocityY": -3.031681211954363e-28, + "timestamp": 2.4932366560321695 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj index 8d6dd6ad..d4ca8cc4 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj @@ -3,100 +3,100 @@ { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -2.79027732569672e-32, - "angularVelocity": 5.39778462149558e-31, - "velocityX": -1.372141826224318e-33, - "velocityY": 4.2150961560958763e-32, + "heading": 2.3925172511788446e-27, + "angularVelocity": -3.168744293663733e-29, + "velocityX": 8.48223659243716e-32, + "velocityY": -3.031681211954363e-28, "timestamp": 0 }, { "x": 1.8507557561824508, "y": 5.5430527114919315, - "heading": -1.045695782000071e-18, - "angularVelocity": -1.1232079825061295e-17, - "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395097007, + "heading": -1.9710234547097696e-21, + "angularVelocity": -2.1171281249858964e-20, + "velocityX": 0.40617567078743816, + "velocityY": 0.0005740544395108319, "timestamp": 0.09309903404237341 }, { "x": 1.9263848800936068, - "y": 5.543159599317717, - "heading": -3.434421194050575e-18, - "angularVelocity": -2.5657896847386088e-17, - "velocityX": 0.812351327691905, - "velocityY": 0.0011481088593983807, + "y": 5.5431595993177165, + "heading": -2.0389636321585498e-20, + "angularVelocity": -1.9783892557349584e-19, + "velocityX": 0.8123513276919052, + "velocityY": 0.001148108859400643, "timestamp": 0.18619806808474682 }, { "x": 2.039828562987611, "y": 5.543319931052194, - "heading": -7.689792330773154e-18, - "angularVelocity": -4.570800524939683e-17, - "velocityX": 1.2185269596070252, - "velocityY": 0.0017221632439692253, + "heading": -2.352458064017618e-20, + "angularVelocity": -3.3673220682050347e-20, + "velocityX": 1.2185269596070256, + "velocityY": 0.001722163243972619, "timestamp": 0.2792971021271202 }, { "x": 2.1910867994360017, - "y": 5.543533706687691, - "heading": -1.506248986539688e-17, - "angularVelocity": -7.919198744068695e-17, - "velocityX": 1.6247025332136764, - "velocityY": 0.0022962175461317975, + "y": 5.54353370668769, + "heading": -3.364930009404432e-20, + "angularVelocity": -1.087521428982912e-19, + "velocityX": 1.6247025332136769, + "velocityY": 0.0022962175461363225, "timestamp": 0.37239613616949363 }, { - "x": 2.380159562296481, - "y": 5.5438009261858445, - "heading": -1.506353706937017e-17, - "angularVelocity": -1.1248279684723702e-20, - "velocityX": 2.0308778152781284, - "velocityY": 0.0028702714362532117, + "x": 2.3801595622964817, + "y": 5.543800926185845, + "heading": -1.7618072508363382e-20, + "angularVelocity": 1.721954234066794e-19, + "velocityX": 2.030877815278129, + "velocityY": 0.0028702714362588678, "timestamp": 0.46549517021186704 }, { - "x": 2.5314177987448714, + "x": 2.5314177987448723, "y": 5.544014701821341, - "heading": -7.704513323430137e-18, - "angularVelocity": 7.904511385790122e-17, - "velocityX": 1.6247025332136762, - "velocityY": 0.002296217546131797, + "heading": -7.829457975228232e-21, + "angularVelocity": 1.0514195591633778e-19, + "velocityX": 1.6247025332136766, + "velocityY": 0.0022962175461363225, "timestamp": 0.5585942042542404 }, { - "x": 2.6448614816388756, - "y": 5.544175033555818, - "heading": -3.433430554917341e-18, - "angularVelocity": 4.587676781446345e-17, - "velocityX": 1.218526959607025, - "velocityY": 0.0017221632439692251, + "x": 2.6448614816388765, + "y": 5.544175033555819, + "heading": 1.3151205701559364e-20, + "angularVelocity": 2.253585538517852e-19, + "velocityX": 1.2185269596070254, + "velocityY": 0.001722163243972619, "timestamp": 0.6516932382966139 }, { - "x": 2.7204906055500317, + "x": 2.720490605550032, "y": 5.544281921381604, - "heading": -1.0420071252828256e-18, - "angularVelocity": 2.5686876928777523e-17, - "velocityX": 0.812351327691905, - "velocityY": 0.0011481088593983805, + "heading": 9.829941746315052e-21, + "angularVelocity": -3.567452648039799e-20, + "velocityX": 0.8123513276919051, + "velocityY": 0.0011481088594006432, "timestamp": 0.7447922723389873 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -1.1539777836664478e-35, - "angularVelocity": 1.1192459041074064e-17, - "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395097005, + "heading": -1.6124701211579614e-34, + "angularVelocity": -1.0558586184512984e-19, + "velocityX": 0.40617567078743816, + "velocityY": 0.000574054439510832, "timestamp": 0.8378913063813607 }, { "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, - "angularVelocity": 7.156872912951633e-35, - "velocityX": -1.751436869795613e-36, - "velocityY": -4.841667244220732e-36, + "angularVelocity": 1.724633516740329e-33, + "velocityX": 6.381177903729197e-38, + "velocityY": -6.411016173939856e-36, "timestamp": 0.9309903404237341 } ] diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj index 565f82b6..8f1e23cf 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj @@ -4,208 +4,208 @@ "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, - "angularVelocity": 7.156872912951633e-35, - "velocityX": -1.751436869795613e-36, - "velocityY": -4.841667244220732e-36, + "angularVelocity": 1.724633516740329e-33, + "velocityX": 6.381177903729197e-38, + "velocityY": -6.411016173939856e-36, "timestamp": 0 }, { "x": 2.7396220511907092, "y": 5.562601673573336, - "heading": 0.0037078860807813565, - "angularVelocity": 0.0478983711971462, - "velocityX": -0.2413479950107084, - "velocityY": 0.23596367181626482, + "heading": 0.003707886080781361, + "angularVelocity": 0.04789837119714626, + "velocityX": -0.24134799501070836, + "velocityY": 0.2359636718162648, "timestamp": 0.07741152753441183 }, { "x": 2.702380969511178, "y": 5.599259386918254, - "heading": 0.011292704027819494, - "angularVelocity": 0.09798047123752286, - "velocityX": -0.4810792767650385, - "velocityY": 0.47354334053959213, + "heading": 0.011292704027819503, + "angularVelocity": 0.09798047123752293, + "velocityX": -0.4810792767650384, + "velocityY": 0.473543340539592, "timestamp": 0.15482305506882366 }, { "x": 2.6467699433072402, "y": 5.6544931206396525, "heading": 0.023007945503191744, - "angularVelocity": 0.15133716965040475, + "angularVelocity": 0.1513371696504047, "velocityX": -0.7183817187849313, - "velocityY": 0.7135078647924301, + "velocityY": 0.7135078647924299, "timestamp": 0.23223458260323548 }, { "x": 2.573102880230356, "y": 5.72860253774289, - "heading": 0.03927230490424403, - "angularVelocity": 0.21010255086133378, - "velocityX": -0.9516291103303323, - "velocityY": 0.9573434275701879, + "heading": 0.03927230490424404, + "angularVelocity": 0.21010255086133403, + "velocityX": -0.9516291103303324, + "velocityY": 0.9573434275701875, "timestamp": 0.3096461101376473 }, { "x": 2.4820081686264115, "y": 5.82215722498829, - "heading": 0.060903903359814396, - "angularVelocity": 0.27943639848670454, - "velocityX": -1.1767589983733413, - "velocityY": 1.2085368965728291, + "heading": 0.060903903359814444, + "angularVelocity": 0.2794363984867048, + "velocityX": -1.1767589983733415, + "velocityY": 1.208536896572829, "timestamp": 0.38705763767205914 }, { "x": 2.3753503305689, "y": 5.936638406081269, - "heading": 0.09019255741494601, - "angularVelocity": 0.3783500337480349, - "velocityX": -1.3778030411568694, - "velocityY": 1.478864772976981, + "heading": 0.09019255741494606, + "angularVelocity": 0.378350033748035, + "velocityX": -1.3778030411568696, + "velocityY": 1.4788647729769806, "timestamp": 0.46446916520647097 }, { "x": 2.2779168004384966, - "y": 6.074876724828902, - "heading": 0.14378193450398408, + "y": 6.0748767248289015, + "heading": 0.1437819345039841, "angularVelocity": 0.6922661106928292, "velocityX": -1.25864368310122, - "velocityY": 1.7857588288279347, + "velocityY": 1.7857588288279342, "timestamp": 0.5418806927408828 }, { "x": 2.2032159820535764, "y": 6.200217981312059, - "heading": 0.2000311193060264, + "heading": 0.20003111930602646, "angularVelocity": 0.7266254341388357, - "velocityX": -0.9649831331866354, - "velocityY": 1.6191549304777415, - "timestamp": 0.6192922202752942 + "velocityX": -0.9649831331866353, + "velocityY": 1.6191549304777408, + "timestamp": 0.6192922202752946 }, { "x": 2.149623841782867, "y": 6.310133396102646, - "heading": 0.2561023628726984, + "heading": 0.25610236287269844, "angularVelocity": 0.7243267941166335, "velocityX": -0.6923018053982481, - "velocityY": 1.4198843284900602, - "timestamp": 0.6967037478097056 + "velocityY": 1.4198843284900597, + "timestamp": 0.696703747809706 }, { "x": 2.1165539042639407, "y": 6.4038530923195935, - "heading": 0.3110558787207687, + "heading": 0.31105587872076873, "angularVelocity": 0.7098880179523891, "velocityX": -0.42719655033581183, - "velocityY": 1.2106684779639116, - "timestamp": 0.774115275344117 + "velocityY": 1.2106684779639114, + "timestamp": 0.7741152753441174 }, { "x": 2.1037067095365876, "y": 6.481006876335726, - "heading": 0.36442032903480814, - "angularVelocity": 0.6893605127520233, - "velocityX": -0.16595971086660646, - "velocityY": 0.9966704762651166, - "timestamp": 0.8515268028785283 + "heading": 0.3644203290348082, + "angularVelocity": 0.6893605127520235, + "velocityX": -0.1659597108666065, + "velocityY": 0.9966704762651164, + "timestamp": 0.8515268028785288 }, { "x": 2.1109008821512623, "y": 6.541377447734871, - "heading": 0.4159102858737852, - "angularVelocity": 0.6651458571992167, + "heading": 0.41591028587378526, + "angularVelocity": 0.6651458571992168, "velocityX": 0.09293412549541542, "velocityY": 0.7798653937207066, - "timestamp": 0.9289383304129397 + "timestamp": 0.9289383304129402 }, { "x": 2.1380148682221702, "y": 6.5848219723004595, "heading": 0.465333297367931, - "angularVelocity": 0.6384451136450677, + "angularVelocity": 0.6384451136450678, "velocityX": 0.3502577320781457, "velocityY": 0.5612151826648336, - "timestamp": 1.006349857947351 + "timestamp": 1.0063498579473515 }, { "x": 2.1849615573883057, "y": 6.611239433288574, "heading": 0.5125504196, "angularVelocity": 0.6099494963599527, - "velocityX": 0.6064560493947925, - "velocityY": 0.34126004006795546, - "timestamp": 1.0837613854817625 + "velocityX": 0.6064560493947926, + "velocityY": 0.3412600400679556, + "timestamp": 1.083761385481763 }, { "x": 2.250707976446258, "y": 6.648678224064634, "heading": 0.55208552781375, - "angularVelocity": 0.5325541984016288, - "velocityX": 0.8856313560565373, + "angularVelocity": 0.5325541984016284, + "velocityX": 0.8856313560565378, "velocityY": 0.5043159387113171, - "timestamp": 1.1579981642592236 + "timestamp": 1.157998164259224 }, { "x": 2.3371670619589073, "y": 6.698226995892447, - "heading": 0.5853560107794161, - "angularVelocity": 0.44816711492022887, - "velocityX": 1.1646395080237382, - "velocityY": 0.6674423734944788, - "timestamp": 1.2322349430366848 + "heading": 0.585356010779416, + "angularVelocity": 0.44816711492022837, + "velocityX": 1.1646395080237388, + "velocityY": 0.6674423734944787, + "timestamp": 1.2322349430366852 }, { "x": 2.4409073566894506, "y": 6.758690881227722, - "heading": 0.5617681472290976, - "angularVelocity": -0.31773824159353153, - "velocityX": 1.3974245170513666, - "velocityY": 0.8144734501011613, - "timestamp": 1.306471721814146 + "heading": 0.5617681472290975, + "angularVelocity": -0.31773824159353026, + "velocityX": 1.3974245170513677, + "velocityY": 0.8144734501011609, + "timestamp": 1.3064717218141464 }, { "x": 2.523886913207481, "y": 6.807069780587539, "heading": 0.5422957792802172, - "angularVelocity": -0.2623008200187737, - "velocityX": 1.1177688186980357, - "velocityY": 0.6516837092951017, - "timestamp": 1.380708500591607 + "angularVelocity": -0.262300820018773, + "velocityX": 1.1177688186980366, + "velocityY": 0.6516837092951012, + "timestamp": 1.3807085005916075 }, { "x": 2.5861173110961473, "y": 6.843356427328165, "heading": 0.5274956073850987, - "angularVelocity": -0.1993644139582728, - "velocityX": 0.8382691021011789, - "velocityY": 0.4887960838037212, - "timestamp": 1.4549452793690683 + "angularVelocity": -0.1993644139582724, + "velocityX": 0.8382691021011796, + "velocityY": 0.4887960838037209, + "timestamp": 1.4549452793690687 }, { "x": 2.627602512984248, "y": 6.8675485030782895, "heading": 0.5175502299027057, - "angularVelocity": -0.13396833276139558, - "velocityX": 0.5588227637470666, - "velocityY": 0.3258772288954636, - "timestamp": 1.5291820581465294 + "angularVelocity": -0.1339683327613954, + "velocityX": 0.558822763747067, + "velocityY": 0.3258772288954634, + "timestamp": 1.5291820581465299 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.06734950498988643, - "velocityX": 0.2794033376916902, - "velocityY": 0.16294305705300594, - "timestamp": 1.6034188369239906 + "angularVelocity": -0.06734950498988629, + "velocityX": 0.27940333769169035, + "velocityY": 0.16294305705300582, + "timestamp": 1.603418836923991 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -4.084231073354503e-32, - "velocityX": -1.1079933009257942e-33, - "velocityY": -1.0209152810451872e-34, - "timestamp": 1.6776556157014517 + "angularVelocity": -1.618919283202532e-33, + "velocityX": 2.4430876435253525e-35, + "velocityY": 8.017390657472539e-35, + "timestamp": 1.6776556157014522 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj index d8cf383e..2c5b2ae4 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj @@ -4,847 +4,865 @@ "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -4.084231073354503e-32, - "velocityX": -1.1079933009257942e-33, - "velocityY": -1.0209152810451872e-34, + "angularVelocity": -1.618919283202532e-33, + "velocityX": 2.4430876435253525e-35, + "velocityY": 8.017390657472539e-35, "timestamp": 0 }, { - "x": 2.6570277255317225, - "y": 6.867726793502589, - "heading": 0.5066730395869481, - "angularVelocity": -0.10087167460547103, - "velocityX": 0.14902725506901718, - "velocityY": -0.20454631284120778, - "timestamp": 0.058265910980853874 - }, - { - "x": 2.6744037356282937, - "y": 6.843893366933042, - "heading": 0.49507655406382295, - "angularVelocity": -0.1990269323504748, - "velocityX": 0.2982191439910878, - "velocityY": -0.4090458068591583, - "timestamp": 0.11653182196170775 - }, - { - "x": 2.7004835818665893, - "y": 6.808147821587349, - "heading": 0.4779456476925187, - "angularVelocity": -0.29401250375942084, - "velocityX": 0.44760042019879576, - "velocityY": -0.6134898561431401, - "timestamp": 0.17479773294256162 - }, - { - "x": 2.735280123191829, - "y": 6.760494081762779, - "heading": 0.4554977531812837, - "angularVelocity": -0.38526634413407634, - "velocityX": 0.5972023905482879, - "velocityY": -0.8178665539139939, - "timestamp": 0.2330636439234155 - }, - { - "x": 2.77880855180183, - "y": 6.700937036637074, - "heading": 0.4279915994110024, - "angularVelocity": -0.4720796999006835, - "velocityX": 0.7470650999399128, - "velocityY": -1.022159340223411, - "timestamp": 0.29132955490426937 - }, - { - "x": 2.8310870878518544, - "y": 6.629482937316093, - "heading": 0.39573903851012515, - "angularVelocity": -0.5535408330177088, - "velocityX": 0.8972405162806606, - "velocityY": -1.2263448407158972, - "timestamp": 0.34959546588512325 - }, - { - "x": 2.8921379557757043, - "y": 6.546140000901142, - "heading": 0.3591220542576228, - "angularVelocity": -0.6284460954285072, - "velocityX": 1.0477973637777922, - "velocityY": -1.4303893136132022, - "timestamp": 0.4078613768659771 - }, - { - "x": 2.9619888074879523, - "y": 6.4509193752842275, - "heading": 0.3186184742438743, - "angularVelocity": -0.6951505491273999, - "velocityX": 1.1988287926228676, - "velocityY": -1.6342424586513933, - "timestamp": 0.466127287846831 - }, - { - "x": 3.0406748905144103, - "y": 6.3438367809482505, - "heading": 0.27484324456730397, - "angularVelocity": -0.7513008711209391, - "velocityX": 1.3504651639672256, - "velocityY": -1.8378257978522827, - "timestamp": 0.5243931988276849 - }, - { - "x": 3.12824252650645, - "y": 6.224915546783436, - "heading": 0.22861956489914778, - "angularVelocity": -0.7933228690673962, - "velocityX": 1.502896539639008, - "velocityY": -2.0410087504491825, - "timestamp": 0.5826591098085387 - }, - { - "x": 3.224755029145573, - "y": 6.094192875417537, - "heading": 0.18111264418965425, - "angularVelocity": -0.815346742370587, - "velocityX": 1.6564145486515685, - "velocityY": -2.243553205730433, - "timestamp": 0.6409250207893926 - }, - { - "x": 3.3303033780389395, - "y": 5.951734868373249, - "heading": 0.13411173965139422, - "angularVelocity": -0.8066621416715647, - "velocityX": 1.8114940128208097, - "velocityY": -2.444963180805693, - "timestamp": 0.6991909317702465 - }, - { - "x": 3.4450256229008205, - "y": 5.797681352790493, - "heading": 0.09073046456880836, - "angularVelocity": -0.7445395489798686, - "velocityX": 1.9689427819909868, - "velocityY": -2.643973345467458, - "timestamp": 0.7574568427511004 - }, - { - "x": 3.5691217392378216, - "y": 5.632434411388871, - "heading": 0.05763038975845234, - "angularVelocity": -0.5680864548952619, - "velocityX": 2.1298236695857957, - "velocityY": -2.8360826874554848, - "timestamp": 0.8157227537319542 - }, - { - "x": 3.7017085484583103, - "y": 5.4576517803314095, - "heading": 0.0547360803841417, - "angularVelocity": -0.049674146093104596, - "velocityX": 2.2755468332771343, - "velocityY": -2.999740810967421, - "timestamp": 0.8739886647128081 - }, - { - "x": 3.8463662629346644, - "y": 5.2914222205797, - "heading": 0.05473605711060691, - "angularVelocity": -3.9943655557036433e-7, - "velocityX": 2.4827160863217212, - "velocityY": -2.8529470655034146, - "timestamp": 0.932254575693662 - }, - { - "x": 4.001863921193704, - "y": 5.135285744872655, - "heading": 0.05473603479828713, - "angularVelocity": -3.8293951656527263e-7, - "velocityX": 2.668758724293121, - "velocityY": -2.679722552666384, - "timestamp": 0.9905204866745159 + "x": 2.6570338601157935, + "y": 6.867732812321398, + "heading": 0.5065324038818937, + "angularVelocity": -0.10327871439636148, + "velocityX": 0.1491229424075764, + "velocityY": -0.20442985508511316, + "timestamp": 0.05826966140390244 + }, + { + "x": 2.674422013328184, + "y": 6.84391114835803, + "heading": 0.49465795152131636, + "angularVelocity": -0.2037844750507175, + "velocityX": 0.2984083448136593, + "velocityY": -0.40881761433700353, + "timestamp": 0.11653932280780488 + }, + { + "x": 2.7005198630420675, + "y": 6.808182791545169, + "heading": 0.47711568998903314, + "angularVelocity": -0.30105308851354295, + "velocityX": 0.4478805794491053, + "velocityY": -0.6131553874185988, + "timestamp": 0.17480898421170732 + }, + { + "x": 2.7353400941220496, + "y": 6.760551298473928, + "heading": 0.4541277672109497, + "angularVelocity": -0.39450929049922157, + "velocityX": 0.5975705065217742, + "velocityY": -0.8174321237441035, + "timestamp": 0.23307864561560976 + }, + { + "x": 2.7788976924779565, + "y": 6.701021131245012, + "heading": 0.42595856862842546, + "angularVelocity": -0.4834282181127908, + "velocityX": 0.7475176156247499, + "velocityY": -1.021632283329671, + "timestamp": 0.2913483070195122 + }, + { + "x": 2.831210629197317, + "y": 6.629598041190472, + "heading": 0.39292676107093794, + "angularVelocity": -0.5668783164625577, + "velocityX": 0.8977731371519051, + "velocityY": -1.2257337409164428, + "timestamp": 0.34961796842341464 + }, + { + "x": 2.8923008190981245, + "y": 6.546289653631685, + "heading": 0.3554225990889349, + "angularVelocity": -0.6436310264794379, + "velocityX": 1.0484047517859123, + "velocityY": -1.4297043358691544, + "timestamp": 0.4078876298273171 + }, + { + "x": 2.962195513842206, + "y": 6.451106403793388, + "heading": 0.31393409063739947, + "angularVelocity": -0.7120087443781998, + "velocityX": 1.1995040482490311, + "velocityY": -1.6334958457802513, + "timestamp": 0.4661572912312195 + }, + { + "x": 3.0409294178085977, + "y": 6.344063134427028, + "heading": 0.26908904007938106, + "angularVelocity": -0.7696123416123889, + "velocityX": 1.351198927013472, + "velocityY": -1.8370326304863622, + "timestamp": 0.524426952635122 + }, + { + "x": 3.128548070700225, + "y": 6.22518206352159, + "heading": 0.22172760975049532, + "angularVelocity": -0.8127974178637293, + "velocityX": 1.5036753394582005, + "velocityY": -2.0401881191895193, + "timestamp": 0.5826966140390244 + }, + { + "x": 3.2251135709999605, + "y": 6.094498947078054, + "heading": 0.17303894760412505, + "angularVelocity": -0.8355748252745036, + "velocityX": 1.6572174605646188, + "velocityY": -2.2427299780874135, + "timestamp": 0.6409662754429268 + }, + { + "x": 3.3307147862388833, + "y": 5.952077971116604, + "heading": 0.12484956119030122, + "angularVelocity": -0.8270064601850674, + "velocityX": 1.8122846897450882, + "velocityY": -2.444170302865565, + "timestamp": 0.6992359368468293 + }, + { + "x": 3.4454853336507822, + "y": 5.798056568785059, + "heading": 0.08033941158282583, + "angularVelocity": -0.7638649090295626, + "velocityX": 1.9696450030206056, + "velocityY": -2.643252056399113, + "timestamp": 0.7575055982507317 + }, + { + "x": 3.5696119500976344, + "y": 5.632835617985738, + "heading": 0.04630921837828673, + "angularVelocity": -0.5840122009402995, + "velocityX": 2.1302100176360224, + "velocityY": -2.8354541079975273, + "timestamp": 0.8157752596546342 + }, + { + "x": 3.7020235307305986, + "y": 5.458025004439994, + "heading": 0.04313298905641362, + "angularVelocity": -0.05450914327194612, + "velocityX": 2.272393170695454, + "velocityY": -3.00002796196164, + "timestamp": 0.8740449210585366 + }, + { + "x": 3.84656727081631, + "y": 5.291677538591243, + "heading": 0.04313296518805293, + "angularVelocity": -4.0961900430587924e-7, + "velocityX": 2.480600309032019, + "velocityY": -2.8547868966613104, + "timestamp": 0.932314582462439 + }, + { + "x": 4.001959806149131, + "y": 5.135416436419877, + "heading": 0.04313294238925072, + "angularVelocity": -3.912636809189277e-7, + "velocityX": 2.666782877897643, + "velocityY": -2.6816888652951767, + "timestamp": 0.9905842438663415 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.0547360118053875, - "angularVelocity": -3.946201001676767e-7, - "velocityX": 2.842745293382521, - "velocityY": -2.4943907433712678, - "timestamp": 1.0487863976553697 - }, - { - "x": 4.269399861624288, - "y": 4.9075045981351995, - "heading": 0.054735989995402, - "angularVelocity": -6.29291522756053e-7, - "velocityX": 2.940181103085103, - "velocityY": -2.3787638667331756, - "timestamp": 1.083444397503813 - }, - { - "x": 4.374514707093794, - "y": 4.829200558555379, - "heading": 0.05473596913891511, - "angularVelocity": -6.017798771949806e-7, - "velocityX": 3.0329172464990677, - "velocityY": -2.2593352161762894, - "timestamp": 1.1181023973522564 - }, - { - "x": 4.482675566428291, - "y": 4.7551608256734985, - "heading": 0.0547359490445404, - "angularVelocity": -5.797903745228634e-7, - "velocityX": 3.120805003389626, - "velocityY": -2.1362956086805514, - "timestamp": 1.1527603972006997 - }, - { - "x": 4.593709527898866, - "y": 4.685503714898617, - "heading": 0.05473592954464412, - "angularVelocity": -5.62637669848401e-7, - "velocityX": 3.203703674652828, - "velocityY": -2.0098422032282808, - "timestamp": 1.187418397049143 - }, - { - "x": 4.707439069036232, - "y": 4.620340494746092, - "heading": 0.05473591048922828, - "angularVelocity": -5.498129124049459e-7, - "velocityX": 3.2814802249032198, - "velocityY": -1.8801783264319567, - "timestamp": 1.2220763968975863 - }, - { - "x": 4.823682298536361, - "y": 4.559775113095544, - "heading": 0.054735891740807854, - "angularVelocity": -5.409550611531198e-7, - "velocityX": 3.3540085985473036, - "velocityY": -1.747515203283397, - "timestamp": 1.2567343967460296 - }, - { - "x": 4.942252940190184, - "y": 4.503903323846341, - "heading": 0.05473587317010921, - "angularVelocity": -5.358271892357539e-7, - "velocityX": 3.4211622763091856, - "velocityY": -1.6120892577045927, - "timestamp": 1.291392396594473 - }, - { - "x": 5.062124990174475, - "y": 4.450881200757823, - "heading": 0.05473585465308408, - "angularVelocity": -5.342785275702722e-7, - "velocityX": 3.4587122888938238, - "velocityY": -1.52986679324772, - "timestamp": 1.3260503964429162 - }, - { - "x": 5.1819977334562, - "y": 4.397860645099044, - "heading": 0.05473583613607813, - "angularVelocity": -5.3427797428813e-7, - "velocityX": 3.4587322928593447, - "velocityY": -1.529821567621728, - "timestamp": 1.3607083962913595 - }, - { - "x": 5.301870477238367, - "y": 4.344840090571707, - "heading": 0.05473581761907218, - "angularVelocity": -5.342779742919752e-7, - "velocityX": 3.458732307298825, - "velocityY": -1.5298215349758078, - "timestamp": 1.3953663961398028 - }, - { - "x": 5.421743975168144, - "y": 4.291821241109097, - "heading": 0.0547357991020742, - "angularVelocity": -5.342777443988636e-7, - "velocityX": 3.4587540669967396, - "velocityY": -1.5297723381170152, - "timestamp": 1.4300243959882462 - }, - { - "x": 5.5428966637680075, - "y": 4.241794448876403, - "heading": 0.05473578057951323, - "angularVelocity": -5.34438255269497e-7, - "velocityX": 3.4956630252656034, - "velocityY": -1.4434414118372951, - "timestamp": 1.4646823958366895 + "heading": 0.04313291889737663, + "angularVelocity": -4.031578960087653e-7, + "velocityX": 2.8409167869122447, + "velocityY": -2.496473071014189, + "timestamp": 1.048853905270244 + }, + { + "x": 4.2693517733895705, + "y": 4.907420739393082, + "heading": 0.043132896624531875, + "angularVelocity": -6.4257109962904e-7, + "velocityX": 2.9384484685182977, + "velocityY": -2.3809038315668807, + "timestamp": 1.0835159757750095 + }, + { + "x": 4.374422288805421, + "y": 4.829031478695207, + "heading": 0.0431328753158102, + "angularVelocity": -6.147561689330783e-7, + "velocityX": 3.0312821445967915, + "velocityY": -2.261528511030489, + "timestamp": 1.118178046279775 + }, + { + "x": 4.482542607371009, + "y": 4.7549053274480135, + "heading": 0.043132854776618594, + "angularVelocity": -5.925552428867745e-7, + "velocityX": 3.1192689008789145, + "velocityY": -2.138537893661084, + "timestamp": 1.1528401167845406 + }, + { + "x": 4.593539840912748, + "y": 4.685160765838758, + "heading": 0.04313283483631883, + "angularVelocity": -5.752772258439431e-7, + "velocityX": 3.2022678370145012, + "velocityY": -2.012129125398518, + "timestamp": 1.1875021872893061 + }, + { + "x": 4.707236482283354, + "y": 4.619909225059653, + "heading": 0.04313281534202855, + "angularVelocity": -5.624098616258592e-7, + "velocityX": 3.280145695710103, + "velocityY": -1.882505569600442, + "timestamp": 1.2221642577940717 + }, + { + "x": 4.823450643664921, + "y": 4.559254805625301, + "heading": 0.043132796153422026, + "angularVelocity": -5.53590891933908e-7, + "velocityX": 3.352776094711051, + "velocityY": -1.7498787161607805, + "timestamp": 1.2568263282988372 + }, + { + "x": 4.9419959983331205, + "y": 4.5032933115020395, + "heading": 0.04313277713835443, + "angularVelocity": -5.485842974798401e-7, + "velocityX": 3.4200309716611494, + "velocityY": -1.6144879203210547, + "timestamp": 1.2914883988036028 + }, + { + "x": 5.061736542982073, + "y": 4.449937038090927, + "heading": 0.04313275817159016, + "angularVelocity": -5.471907475389428e-7, + "velocityX": 3.454512177294491, + "velocityY": -1.5393273579481606, + "timestamp": 1.3261504693083683 + }, + { + "x": 5.1814776709209625, + "y": 4.396582073700789, + "heading": 0.04313273920484137, + "angularVelocity": -5.471903010583792e-7, + "velocityX": 3.45452900519684, + "velocityY": -1.5392895927207566, + "timestamp": 1.3608125398131339 + }, + { + "x": 5.3012188001688525, + "y": 4.3432271122483534, + "heading": 0.043132720238092614, + "angularVelocity": -5.471902999394187e-7, + "velocityX": 3.454529042961442, + "velocityY": -1.5392895079680768, + "timestamp": 1.3954746103178994 + }, + { + "x": 5.420963133587213, + "y": 4.289879342291843, + "heading": 0.04313270127136276, + "angularVelocity": -5.471897545932471e-7, + "velocityX": 3.4546214832116693, + "velocityY": -1.5390820334629507, + "timestamp": 1.430136680822665 + }, + { + "x": 5.5425254594597755, + "y": 4.240815191837089, + "heading": 0.04313268228677855, + "angularVelocity": -5.47704852461645e-7, + "velocityX": 3.5070705270145455, + "velocityY": -1.415499701554448, + "timestamp": 1.4647987513274305 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.054735761954551196, - "angularVelocity": -5.373928706734811e-7, - "velocityX": 3.550552991831721, - "velocityY": -1.3025960678451716, - "timestamp": 1.4993403956851328 - }, - { - "x": 5.798180192000891, - "y": 4.154397155999577, - "heading": 0.05473574376724012, - "angularVelocity": -4.955056404583076e-7, - "velocityX": 3.6025088632415367, - "velocityY": -1.15113574961205, - "timestamp": 1.53604494470909 - }, - { - "x": 5.93207840031246, - "y": 4.117779387908136, - "heading": 0.054735725716885786, - "angularVelocity": -4.917743118523635e-7, - "velocityX": 3.6480003670436316, - "velocityY": -0.9976356899941846, - "timestamp": 1.5727494937330473 - }, - { - "x": 6.067406110251661, - "y": 4.086860105171276, - "heading": 0.0547357076680171, - "angularVelocity": -4.917338358563753e-7, - "velocityX": 3.686946537631365, - "velocityY": -0.8423828533264233, - "timestamp": 1.6094540427570045 - }, - { - "x": 6.203921243902459, - "y": 4.061696453595828, - "heading": 0.05473568948478555, - "angularVelocity": -4.953944957116945e-7, - "velocityX": 3.719297397216157, - "velocityY": -0.6855731031873736, - "timestamp": 1.6461585917809618 - }, - { - "x": 6.341379198553944, - "y": 4.042334078664949, - "heading": 0.054735669406532635, - "angularVelocity": -5.47023555566929e-7, - "velocityX": 3.7449841588235726, - "velocityY": -0.5275197610585458, - "timestamp": 1.682863140804919 - }, - { - "x": 6.479285910845981, - "y": 4.028828408214569, - "heading": 0.05386070036293022, - "angularVelocity": -0.023838163575618188, - "velocityX": 3.7572103719902135, - "velocityY": -0.3679563108530375, - "timestamp": 1.7195676898288763 - }, - { - "x": 6.612620100679574, - "y": 4.0185345658565055, - "heading": 0.045620785713098924, - "angularVelocity": -0.22449300887617385, - "velocityX": 3.6326339208408243, - "velocityY": -0.2804514053923919, - "timestamp": 1.7562722388528336 - }, - { - "x": 6.740581673709006, - "y": 4.010579483858113, - "heading": 0.035519685699448925, - "angularVelocity": -0.27520022128747124, - "velocityX": 3.4862592357669087, - "velocityY": -0.2167328630900888, - "timestamp": 1.7929767878767908 - }, - { - "x": 6.863028396494948, - "y": 4.00465723612987, - "heading": 0.025264871332947036, - "angularVelocity": -0.27938810417772464, - "velocityX": 3.3360094604627837, - "velocityY": -0.16134914842236953, - "timestamp": 1.829681336900748 - }, - { - "x": 6.979909878212025, - "y": 4.000619227079583, - "heading": 0.015648348920582984, - "angularVelocity": -0.26199810835673765, - "velocityX": 3.184386808315938, - "velocityY": -0.1100138581637957, - "timestamp": 1.8663858859247053 - }, - { - "x": 7.091201711972647, - "y": 3.998378156691166, - "heading": 0.00712732553533738, - "angularVelocity": -0.23215169813648398, - "velocityX": 3.0320992007824574, - "velocityY": -0.06105702012450316, - "timestamp": 1.9030904349486626 + "heading": 0.043132663161499156, + "angularVelocity": -5.517639056314665e-7, + "velocityX": 3.560845257182681, + "velocityY": -1.274191548268104, + "timestamp": 1.499460821832196 + }, + { + "x": 5.795862627864808, + "y": 4.156159506730396, + "heading": 0.04313264445875193, + "angularVelocity": -5.198107758201059e-7, + "velocityX": 3.6106505851434836, + "velocityY": -1.1253380804221886, + "timestamp": 1.5354407362494324 + }, + { + "x": 5.927341530201614, + "y": 4.121094637119033, + "heading": 0.043132625947930554, + "angularVelocity": -5.1447652596799e-7, + "velocityX": 3.654230546858154, + "velocityY": -0.9745678993212189, + "timestamp": 1.5714206506666688 + }, + { + "x": 6.059882410873037, + "y": 4.090286167435021, + "heading": 0.04313260749907939, + "angularVelocity": -5.127541702089576e-7, + "velocityX": 3.683746412913316, + "velocityY": -0.8562685649205634, + "timestamp": 1.6074005650839052 + }, + { + "x": 6.193269302687495, + "y": 4.06337550162166, + "heading": 0.043132589025398085, + "angularVelocity": -5.134442814470016e-7, + "velocityX": 3.7072598413563593, + "velocityY": -0.7479357927674594, + "timestamp": 1.6433804795011415 + }, + { + "x": 6.327657815035056, + "y": 4.042021941101089, + "heading": 0.043132570411953335, + "angularVelocity": -5.173287667087509e-7, + "velocityX": 3.7350981658583504, + "velocityY": -0.5934855840107985, + "timestamp": 1.6793603939183779 + }, + { + "x": 6.462816636912207, + "y": 4.026263156605077, + "heading": 0.043132547717955015, + "angularVelocity": -6.30740753253067e-7, + "velocityX": 3.7565075978169236, + "velocityY": -0.4379883818862704, + "timestamp": 1.7153403083356142 + }, + { + "x": 6.597942834210822, + "y": 4.016131027814117, + "heading": 0.04107885634805962, + "angularVelocity": -0.0570788286508975, + "velocityX": 3.755600853621886, + "velocityY": -0.2816051387300244, + "timestamp": 1.7513202227528506 + }, + { + "x": 6.728400824069692, + "y": 4.008818936530008, + "heading": 0.03307687475358465, + "angularVelocity": -0.2224013515341097, + "velocityX": 3.625856035843536, + "velocityY": -0.20322703381989088, + "timestamp": 1.787300137170087 + }, + { + "x": 6.853599659736003, + "y": 4.0035443033890195, + "heading": 0.023873611715651655, + "angularVelocity": -0.25578890853403846, + "velocityX": 3.47968686680181, + "velocityY": -0.14659937986014848, + "timestamp": 1.8232800515873233 + }, + { + "x": 6.973435503233387, + "y": 4.000040041798795, + "heading": 0.014893418694113961, + "angularVelocity": -0.24958906009058449, + "velocityX": 3.3306316993343295, + "velocityY": -0.09739493956513012, + "timestamp": 1.8592599660045597 + }, + { + "x": 7.08787150697828, + "y": 3.998176312998215, + "heading": 0.006805585628585594, + "angularVelocity": -0.22478744589936686, + "velocityX": 3.1805524164913392, + "velocityY": -0.05179914490532842, + "timestamp": 1.895239880421796 }, { "x": 7.196889877319336, "y": 3.9978766441345215, - "heading": -3.597785008712915e-31, - "angularVelocity": -0.19418098641357107, - "velocityX": 2.87942961178202, - "velocityY": -0.013663498666533047, - "timestamp": 1.9397949839726198 - }, - { - "x": 7.37119660747943, - "y": 4.00262802828422, - "heading": -0.0076676970964569885, - "angularVelocity": -0.11437965193876058, - "velocityX": 2.6001474595947878, - "velocityY": 0.07087677805125822, - "timestamp": 2.0068322320013365 - }, - { - "x": 7.526209738417895, - "y": 4.010745052233057, - "heading": -0.011651875074249012, - "angularVelocity": -0.059432302114867865, - "velocityX": 2.3123432941647923, - "velocityY": 0.1210822965966538, - "timestamp": 2.0738694800300532 - }, - { - "x": 7.661711723306385, - "y": 4.020683594487604, - "heading": -0.013062756224948863, - "angularVelocity": -0.021046227167551262, - "velocityX": 2.0212939652660666, - "velocityY": 0.1482540311065431, - "timestamp": 2.14090672805877 - }, - { - "x": 7.777626027630173, - "y": 4.031355931329743, - "heading": -0.012680116865926582, - "angularVelocity": 0.005707861976350362, - "velocityX": 1.729102964879078, - "velocityY": 0.15920010376271715, - "timestamp": 2.2079439760874866 - }, - { - "x": 7.873936260933145, - "y": 4.041961506483741, - "heading": -0.011077902998110952, - "angularVelocity": 0.023900352638719452, - "velocityX": 1.4366674667449708, - "velocityY": 0.15820421431164344, - "timestamp": 2.2749812241162033 - }, - { - "x": 7.950652622834928, - "y": 4.051889363613367, - "heading": -0.008694806000859931, - "angularVelocity": 0.035548848846393165, - "velocityX": 1.1443841171541105, - "velocityY": 0.14809464024199548, - "timestamp": 2.34201847214492 - }, - { - "x": 8.007796949441587, - "y": 4.060659248403223, - "heading": -0.005876390393834335, - "angularVelocity": 0.04204253142698025, - "velocityX": 0.8524264985069137, - "velocityY": 0.1308210740706325, - "timestamp": 2.4090557201736367 - }, - { - "x": 8.045395699194648, - "y": 4.067884383860418, - "heading": -0.00290156854092843, - "angularVelocity": 0.04437565592835165, - "velocityX": 0.5608635625518943, - "velocityY": 0.1077779245069823, - "timestamp": 2.4760929682023534 - }, - { - "x": 8.0634765625, + "heading": 3.395025904591232e-32, + "angularVelocity": -0.18914957800247972, + "velocityX": 3.0299785896330556, + "velocityY": -0.008328782003720711, + "timestamp": 1.9312197948390324 + }, + { + "x": 7.373371840772393, + "y": 4.002118083585038, + "heading": -0.0072355781339062996, + "angularVelocity": -0.11324248534202788, + "velocityX": 2.7620814521804613, + "velocityY": 0.06638186139590158, + "timestamp": 1.9951143491845382 + }, + { + "x": 7.532286444106961, + "y": 4.009181217967762, + "heading": -0.011195509678912387, + "angularVelocity": -0.06197604139459259, + "velocityX": 2.4871384574536473, + "velocityY": 0.11054360508613674, + "timestamp": 2.059008903530044 + }, + { + "x": 7.6734693827744955, + "y": 4.017827175121804, + "heading": -0.012870706885804134, + "angularVelocity": -0.02621815308129755, + "velocityX": 2.209623967390062, + "velocityY": 0.13531602563950743, + "timestamp": 2.12290345787555 + }, + { + "x": 7.7968608797168075, + "y": 4.027211909710403, + "heading": -0.012933711131137412, + "angularVelocity": -0.0009860659641287687, + "velocityX": 1.9311739193778674, + "velocityY": 0.14687847320841513, + "timestamp": 2.1867980122210557 + }, + { + "x": 7.9024437482359025, + "y": 4.036727334915007, + "heading": -0.011868630755415614, + "angularVelocity": 0.01666934508944925, + "velocityX": 1.6524548860324264, + "velocityY": 0.14892388407859264, + "timestamp": 2.2506925665665616 + }, + { + "x": 7.99021929218393, + "y": 4.0459160326905685, + "heading": -0.010039813807728299, + "angularVelocity": 0.028622422777974377, + "velocityX": 1.3737562589980292, + "velocityY": 0.143810342989083, + "timestamp": 2.3145871209120674 + }, + { + "x": 8.060196978947376, + "y": 4.0544220986291215, + "heading": -0.00773105502412215, + "angularVelocity": 0.03613388976972427, + "velocityX": 1.0952058040039734, + "velocityY": 0.13312661815523763, + "timestamp": 2.3784816752575733 + }, + { + "x": 8.112389704520517, + "y": 4.061961050008052, + "heading": -0.005169479155646227, + "angularVelocity": 0.04009067587551167, + "velocityX": 0.816857181457298, + "velocityY": 0.11799051509403427, + "timestamp": 2.442376229603079 + }, + { + "x": 8.146811527228987, + "y": 4.068300458431988, + "heading": -0.0025408789062790695, + "angularVelocity": 0.04113966012116123, + "velocityX": 0.5387285827574291, + "velocityY": 0.09921672494429698, + "timestamp": 2.506270783948585 + }, + { + "x": 8.1634765625, "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 0.043282930404384794, - "velocityX": 0.26971368660010536, - "velocityY": 0.07999391635030323, - "timestamp": 2.54313021623107 - }, - { - "x": 8.059946431342839, - "y": 4.076566093038108, - "heading": 0.0028549561404960684, - "angularVelocity": 0.03895223157485018, - "velocityX": -0.048164132671898556, - "velocityY": 0.045285389048645555, - "timestamp": 2.616423992352985 - }, - { - "x": 8.033099887642729, - "y": 4.077511417867968, - "heading": 0.00539411397159862, - "angularVelocity": 0.03464356682727019, - "velocityX": -0.3662868134321987, - "velocityY": 0.012897750394093884, - "timestamp": 2.6897177684748996 - }, - { - "x": 7.9829172655770035, - "y": 4.07628474252501, - "heading": 0.007619394466085314, - "angularVelocity": 0.03036111130070867, - "velocityX": -0.6846778092351761, - "velocityY": -0.01673641894119354, - "timestamp": 2.7630115445968144 - }, - { - "x": 7.909377202284024, - "y": 4.0731292983210805, - "heading": 0.00953312535115696, - "angularVelocity": 0.026110414639960534, - "velocityX": -1.0033602740109082, - "velocityY": -0.04305200756310189, - "timestamp": 2.836305320718729 - }, - { - "x": 7.81245694795258, - "y": 4.068343833996595, - "heading": 0.011138189892816102, - "angularVelocity": 0.021899056462711293, - "velocityX": -1.3223531309156478, - "velocityY": -0.06529155103873027, - "timestamp": 2.909599096840644 - }, - { - "x": 7.692133347423482, - "y": 4.062303858835979, - "heading": 0.012438255979270885, - "angularVelocity": 0.01773774193721827, - "velocityX": -1.641661910404963, - "velocityY": -0.0824077497462929, - "timestamp": 2.982892872962559 - }, - { - "x": 7.548385409224001, - "y": 4.055495143379703, - "heading": 0.013438147372137587, - "angularVelocity": 0.013642241480415802, - "velocityX": -1.9612570917396126, - "velocityY": -0.0928962296191558, - "timestamp": 3.0561866490844736 - }, - { - "x": 7.381200809360299, - "y": 4.048569614878887, - "heading": 0.014144485416621113, - "angularVelocity": 0.009637080825370731, - "velocityX": -2.2810204182359235, - "velocityY": -0.09448999447506434, - "timestamp": 3.1294804252063884 - }, - { - "x": 7.190592956257051, - "y": 4.04244548860493, - "heading": 0.014566894857523213, - "angularVelocity": 0.00576323752509999, - "velocityX": -2.6006008038962976, - "velocityY": -0.08355588425094046, - "timestamp": 3.202774201328303 - }, - { - "x": 6.9766499613051005, - "y": 4.038503705706744, - "heading": 0.01472053802802316, - "angularVelocity": 0.002096264903098756, - "velocityX": -2.918979022121646, - "velocityY": -0.05378059511668448, - "timestamp": 3.276067977450218 - }, - { - "x": 6.739697889009689, - "y": 4.039020009641981, - "heading": 0.014632337527092197, - "angularVelocity": -0.0012033832283965633, - "velocityX": -3.232908506464046, - "velocityY": 0.007044308023879432, - "timestamp": 3.3493617535721327 - }, - { - "x": 6.480981935824527, - "y": 4.048232688232657, - "heading": 0.01435998143770215, - "angularVelocity": -0.003715951118919239, - "velocityX": -3.529848874955213, - "velocityY": 0.12569523741486752, - "timestamp": 3.4226555296940475 - }, - { - "x": 6.206211729235204, - "y": 4.074520375352005, - "heading": 0.014063196474504363, - "angularVelocity": -0.004049251913342814, - "velocityX": -3.7488886659663403, - "velocityY": 0.3586619288876869, - "timestamp": 3.4959493058159623 - }, - { - "x": 5.93349789020665, - "y": 4.124154607710869, - "heading": 0.014063194566661911, - "angularVelocity": -2.6030074482550523e-8, - "velocityX": -3.7208321559927344, - "velocityY": 0.6771957318218086, - "timestamp": 3.569243081937877 + "heading": 2.9433151473989685e-31, + "angularVelocity": 0.03976675214822599, + "velocityX": 0.2608209015888442, + "velocityY": 0.07741657313776515, + "timestamp": 2.5701653382940908 + }, + { + "x": 8.160341731543628, + "y": 4.076804852758406, + "heading": 0.0025302388406713986, + "angularVelocity": 0.03600436134361471, + "velocityX": -0.0446074831711938, + "velocityY": 0.05062755463137832, + "timestamp": 2.640441236719939 + }, + { + "x": 8.135735418270036, + "y": 4.078564432167465, + "heading": 0.004797198596772283, + "angularVelocity": 0.032257997505259615, + "velocityX": -0.3501387221617275, + "velocityY": 0.025038163132351486, + "timestamp": 2.710717135145787 + }, + { + "x": 8.089649517071235, + "y": 4.078625238037987, + "heading": 0.006802207301300417, + "angularVelocity": 0.028530531084475955, + "velocityX": -0.6557853009510525, + "velocityY": 0.0008652450112215468, + "timestamp": 2.780993033571635 + }, + { + "x": 8.02207492391753, + "y": 4.077106587290107, + "heading": 0.008546856784408466, + "angularVelocity": 0.024825715817051033, + "velocityX": -0.9615614267102518, + "velocityY": -0.02160983753886332, + "timestamp": 2.851268931997483 + }, + { + "x": 7.933001430575272, + "y": 4.074154095631599, + "heading": 0.010033092526327095, + "angularVelocity": 0.021148584012580587, + "velocityX": -1.2674828118525576, + "velocityY": -0.042012862512522016, + "timestamp": 2.921544830423331 + }, + { + "x": 7.822417690835061, + "y": 4.069949420479082, + "heading": 0.011263348791088614, + "angularVelocity": 0.017506090883485915, + "velocityX": -1.5735656493512467, + "velocityY": -0.059830969744960986, + "timestamp": 2.991820728849179 + }, + { + "x": 7.690311409008121, + "y": 4.0647254734139295, + "heading": 0.012240761619989172, + "angularVelocity": 0.013908222460249003, + "velocityX": -1.879823449946105, + "velocityY": -0.07433483145952353, + "timestamp": 3.062096627275027 + }, + { + "x": 7.536670161450006, + "y": 4.058791543485583, + "heading": 0.012969524954057786, + "angularVelocity": 0.01037003226415625, + "velocityX": -2.186258034398947, + "velocityY": -0.08443762458060709, + "timestamp": 3.132372525700875 + }, + { + "x": 7.361484060419744, + "y": 4.0525779000493936, + "heading": 0.01345553350071711, + "angularVelocity": 0.006915721570918652, + "velocityX": -2.492833317742797, + "velocityY": -0.08841784417378167, + "timestamp": 3.202648424126723 + }, + { + "x": 7.164754272980972, + "y": 4.046723019235429, + "heading": 0.013707670109403618, + "angularVelocity": 0.0035878105343975153, + "velocityX": -2.799391994203425, + "velocityY": -0.08331278496768466, + "timestamp": 3.272924322552571 + }, + { + "x": 6.9465233889455575, + "y": 4.0422689491096495, + "heading": 0.013740797003253829, + "angularVelocity": 0.0004713834272096328, + "velocityX": -3.1053446334191217, + "velocityY": -0.06337976782296549, + "timestamp": 3.343200220978419 + }, + { + "x": 6.707012866607414, + "y": 4.041196732301159, + "heading": 0.013584512522768889, + "angularVelocity": -0.002223870259728453, + "velocityX": -3.408146003154458, + "velocityY": -0.015257247968487717, + "timestamp": 3.413476119404267 + }, + { + "x": 6.4476145619873, + "y": 4.048415292748879, + "heading": 0.013321412464940579, + "angularVelocity": -0.0037438163541362824, + "velocityX": -3.691141777345175, + "velocityY": 0.10271744096358496, + "timestamp": 3.483752017830115 + }, + { + "x": 6.1833350728544865, + "y": 4.076620284873478, + "heading": 0.013321410992019302, + "angularVelocity": -2.0959124115556836e-8, + "velocityX": -3.76059922466405, + "velocityY": 0.4013465890352113, + "timestamp": 3.554027916255963 + }, + { + "x": 5.922208724930001, + "y": 4.126140027657945, + "heading": 0.013321410307177713, + "angularVelocity": -9.745042100450887e-9, + "velocityX": -3.7157311933907815, + "velocityY": 0.7046475946048248, + "timestamp": 3.624303814681811 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.01406319364211473, - "angularVelocity": -1.2614265905768763e-8, - "velocityX": -3.6503257922039465, - "velocityY": 0.9890944453863133, - "timestamp": 3.642536858059792 - }, - { - "x": 5.5408846553597755, - "y": 4.23596798413579, - "heading": 0.01406319265930284, - "angularVelocity": -2.835157538408451e-8, - "velocityX": -3.6078608681964144, - "velocityY": 1.1342486182256648, - "timestamp": 3.677202017840065 - }, - { - "x": 5.417489605980159, - "y": 4.2802558166388724, - "heading": 0.014063191785023112, - "angularVelocity": -2.5220703836459854e-8, - "velocityX": -3.5596273076991327, - "velocityY": 1.2775891639848806, - "timestamp": 3.711867177620338 - }, - { - "x": 5.295963908009713, - "y": 4.329441746794644, - "heading": 0.014063190996401449, - "angularVelocity": -2.2749690676604448e-8, - "velocityX": -3.5057013653114657, - "velocityY": 1.4188865843266973, - "timestamp": 3.7465323374006108 - }, - { - "x": 5.176501898681268, - "y": 4.383447115848144, - "heading": 0.014063190276173353, - "angularVelocity": -2.077671352365433e-8, - "velocityX": -3.446169297521196, - "velocityY": 1.5579149034885322, - "timestamp": 3.7811974971808837 - }, - { - "x": 5.05929461402676, - "y": 4.442185556946357, - "heading": 0.01406318961105745, - "angularVelocity": -1.9186869677954374e-8, - "velocityX": -3.381126335416613, - "velocityY": 1.6944517628226294, - "timestamp": 3.8158626569611567 - }, - { - "x": 4.944529482696556, - "y": 4.505563132440428, - "heading": 0.014063188990615918, - "angularVelocity": -1.7898129854823097e-8, - "velocityX": -3.310676542604923, - "velocityY": 1.828278764494154, - "timestamp": 3.8505278167414296 - }, - { - "x": 4.832390024623587, - "y": 4.573478482189006, - "heading": 0.014063188406478968, - "angularVelocity": -1.685083682180617e-8, - "velocityX": -3.2349326754519327, - "velocityY": 1.9591817888352978, - "timestamp": 3.8851929765217026 - }, - { - "x": 4.723055552753515, - "y": 4.64582297983034, - "heading": 0.014063187851803343, - "angularVelocity": -1.600095394099115e-8, - "velocityX": -3.1540160946348355, - "velocityY": 2.086951224223198, - "timestamp": 3.9198581363019755 - }, - { - "x": 4.616700862639107, - "y": 4.722480877082754, - "heading": 0.014063187320883878, - "angularVelocity": -1.5315650301874817e-8, - "velocityX": -3.068057115228709, - "velocityY": 2.2113816217294997, - "timestamp": 3.9545232960822485 - }, - { - "x": 4.508256477944758, - "y": 4.796152911545187, - "heading": 0.014063186782201262, - "angularVelocity": -1.5539597107094908e-8, - "velocityX": -3.128339386915466, - "velocityY": 2.125247220246655, - "timestamp": 3.9891884558625215 - }, - { - "x": 4.396953351691581, - "y": 4.865430375769311, - "heading": 0.014063186216514915, - "angularVelocity": -1.6318584724554417e-8, - "velocityX": -3.2108066704055713, - "velocityY": 1.998475260556751, - "timestamp": 4.023853615642794 - }, - { - "x": 4.28296944450211, - "y": 4.930202424249945, - "heading": 0.014063179833248412, - "angularVelocity": -1.8414069176204727e-7, - "velocityX": -3.288140251248267, - "velocityY": 1.8685056953781265, - "timestamp": 4.058518775423067 + "heading": 0.013321409576784942, + "angularVelocity": -1.0393218545701755e-8, + "velocityX": -3.646442120972535, + "velocityY": 1.003317616364548, + "timestamp": 3.694579713107659 + }, + { + "x": 5.541094088534591, + "y": 4.236435199254689, + "heading": 0.013321408817775413, + "angularVelocity": -2.1905263169095435e-8, + "velocityX": -3.6034323243164974, + "velocityY": 1.148240568022246, + "timestamp": 3.7292293551846463 + }, + { + "x": 5.41792618310833, + "y": 4.281179290629024, + "heading": 0.01332140812506875, + "angularVelocity": -1.9991740821363056e-8, + "velocityX": -3.5546660237528647, + "velocityY": 1.2913291073806714, + "timestamp": 3.7638789972616333 + }, + { + "x": 5.2966447994258585, + "y": 4.330809856867523, + "heading": 0.01332140748575771, + "angularVelocity": -1.8450725633992864e-8, + "velocityX": -3.500220389376678, + "velocityY": 1.432354369728484, + "timestamp": 3.7985286393386204 + }, + { + "x": 5.177443709175996, + "y": 4.385247597760623, + "heading": 0.013321406889674104, + "angularVelocity": -1.7203167808757175e-8, + "velocityX": -3.4401824407021615, + "velocityY": 1.5710910020988935, + "timestamp": 3.8331782814156075 + }, + { + "x": 5.0605133585968645, + "y": 4.444405530588187, + "heading": 0.01332140632867156, + "angularVelocity": -1.6190716838073352e-8, + "velocityX": -3.3746481513237594, + "velocityY": 1.7073172847247369, + "timestamp": 3.8678279234925945 + }, + { + "x": 4.946040562745207, + "y": 4.508189127302759, + "heading": 0.013321405796093607, + "angularVelocity": -1.5370373829355437e-8, + "velocityX": -3.3037223183234317, + "velocityY": 1.840815456992452, + "timestamp": 3.9024775655695816 + }, + { + "x": 4.834208203167093, + "y": 4.576496460724576, + "heading": 0.013321405286393582, + "angularVelocity": -1.4710109443101504e-8, + "velocityX": -3.2275184641053225, + "velocityY": 1.9713719775242866, + "timestamp": 3.9371272076465686 + }, + { + "x": 4.725194921038476, + "y": 4.649218348571895, + "heading": 0.013321404794856252, + "angularVelocity": -1.4185928002634242e-8, + "velocityX": -3.146158967137548, + "velocityY": 2.098777461704854, + "timestamp": 3.9717768497235557 + }, + { + "x": 4.619174674827694, + "y": 4.72623832110653, + "heading": 0.013321404313366907, + "angularVelocity": -1.3895939972772152e-8, + "velocityX": -3.05977897189302, + "velocityY": 2.2228215911583726, + "timestamp": 4.006426491800543 + }, + { + "x": 4.510161328966249, + "y": 4.798960113344169, + "heading": 0.013321403820821752, + "angularVelocity": -1.421501423878401e-8, + "velocityX": -3.146160806487812, + "velocityY": 2.0987747023782037, + "timestamp": 4.04107613387753 + }, + { + "x": 4.398328904529435, + "y": 4.867267340500532, + "heading": 0.013321403310039242, + "angularVelocity": -1.4741350266093223e-8, + "velocityX": -3.2275203359486127, + "velocityY": 1.9713689106685317, + "timestamp": 4.075725775954517 + }, + { + "x": 4.283856051297854, + "y": 4.931050823285602, + "heading": 0.013321399241018467, + "angularVelocity": -1.1743327000811157e-7, + "velocityX": -3.3037239743267564, + "velocityY": 1.8408121689497896, + "timestamp": 4.110375418031504 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.013676554706226966, - "angularVelocity": -0.011153132697846535, - "velocityX": -3.331021112692202, - "velocityY": 1.7234990981340637, - "timestamp": 4.09318393520334 - }, - { - "x": 3.942209538282738, - "y": 5.088292732030669, - "heading": 0.012204358572730108, - "angularVelocity": -0.02138935040268071, - "velocityX": -3.2732028891433576, - "velocityY": 1.4288410708686299, - "timestamp": 4.162012399171669 - }, - { - "x": 3.7316526106102703, - "y": 5.172141912516415, - "heading": 0.010528895655226616, - "angularVelocity": -0.024342587657839698, - "velocityX": -3.05915482538382, - "velocityY": 1.2182340800795661, - "timestamp": 4.230840863139997 - }, - { - "x": 3.53847572791013, - "y": 5.244805689896845, - "heading": 0.00883705460181792, - "angularVelocity": -0.024580543511579803, - "velocityX": -2.806642362221417, - "velocityY": 1.0557227808231526, - "timestamp": 4.299669327108326 - }, - { - "x": 3.3636567049982276, - "y": 5.307973779066906, - "heading": 0.0072129294254924, - "angularVelocity": -0.0235967081449452, - "velocityX": -2.5399233519484916, - "velocityY": 0.9177611343924164, - "timestamp": 4.3684977910766545 - }, - { - "x": 3.2076759026568156, - "y": 5.362638919945403, - "heading": 0.005703178674060931, - "angularVelocity": -0.02193497667078823, - "velocityX": -2.2662252409582635, - "velocityY": 0.7942228799941261, - "timestamp": 4.437326255044983 - }, - { - "x": 3.070812323282922, - "y": 5.409448485662979, - "heading": 0.00433725990535669, - "angularVelocity": -0.01984526008502491, - "velocityX": -1.9884735396226798, - "velocityY": 0.6800902274837387, - "timestamp": 4.5061547190133115 - }, - { - "x": 2.953246241272587, - "y": 5.448856394346129, - "heading": 0.0031354038045633293, - "angularVelocity": -0.01746161444698809, - "velocityX": -1.7081026545127194, - "velocityY": 0.572552493707886, - "timestamp": 4.57498318298164 - }, - { - "x": 2.855102952665587, - "y": 5.48119799246298, - "heading": 0.002112343078266857, - "angularVelocity": -0.014863919188538589, - "velocityX": -1.4259113591749097, - "velocityY": 0.4698869661210702, - "timestamp": 4.643811646949969 - }, - { - "x": 2.7764742584987756, - "y": 5.506730918230691, - "heading": 0.0012792785012801142, - "angularVelocity": -0.012103489297248894, - "velocityX": -1.1423862982470774, - "velocityY": 0.3709646314271869, - "timestamp": 4.712640110918297 - }, - { - "x": 2.717430148167725, - "y": 5.5256592019268735, - "heading": 0.0006450106297669135, - "angularVelocity": -0.009215197244632127, - "velocityX": -0.8578443702916283, - "velocityY": 0.275006626689982, - "timestamp": 4.781468574886626 - }, - { - "x": 2.6780256614742735, - "y": 5.538148367640626, - "heading": 0.00021663624249770164, - "angularVelocity": -0.006223796995765153, - "velocityX": -0.5725027760547413, - "velocityY": 0.18145350039337144, - "timestamp": 4.850297038854954 + "heading": 0.012938020716139158, + "angularVelocity": -0.011064429584221965, + "velocityX": -3.358100659168625, + "velocityY": 1.6997858867187727, + "timestamp": 4.145025060108491 + }, + { + "x": 3.941212684187486, + "y": 5.0868100262217055, + "heading": 0.011498233293039487, + "angularVelocity": -0.020924944231025543, + "velocityX": -3.2887007006223063, + "velocityY": 1.4077333471091193, + "timestamp": 4.213832288801836 + }, + { + "x": 3.7303193403637978, + "y": 5.1698989262671065, + "heading": 0.009891075368403834, + "angularVelocity": -0.023357399435433027, + "velocityX": -3.0649881971497734, + "velocityY": 1.2075606244178938, + "timestamp": 4.282639517495181 + }, + { + "x": 3.5371027460588107, + "y": 5.242302097421373, + "heading": 0.008284033873001792, + "angularVelocity": -0.023355707327847468, + "velocityX": -2.808085690619801, + "velocityY": 1.0522611145545084, + "timestamp": 4.351446746188525 + }, + { + "x": 3.362389747323273, + "y": 5.305525449861525, + "heading": 0.006750320632579214, + "angularVelocity": -0.022290001640058982, + "velocityX": -2.5391663354759597, + "velocityY": 0.9188475344926457, + "timestamp": 4.42025397488187 + }, + { + "x": 3.2065870657576463, + "y": 5.360439069495437, + "heading": 0.00533021576404821, + "angularVelocity": -0.0206388906441795, + "velocityX": -2.2643359502240057, + "velocityY": 0.7980792233131078, + "timestamp": 4.489061203575215 + }, + { + "x": 3.0699321768781336, + "y": 5.407605853881433, + "heading": 0.00404906921437406, + "angularVelocity": -0.018619359826041904, + "velocityX": -1.9860542485811465, + "velocityY": 0.6854917031494734, + "timestamp": 4.55786843226856 + }, + { + "x": 2.952579395626419, + "y": 5.447418742231964, + "heading": 0.002924266712171353, + "angularVelocity": -0.01634715601198879, + "velocityX": -1.705529832842474, + "velocityY": 0.578614908732415, + "timestamp": 4.626675660961904 + }, + { + "x": 2.8546365398725873, + "y": 5.480167232965782, + "heading": 0.001968461119345437, + "angularVelocity": -0.013891063642247188, + "velocityX": -1.4234384615362952, + "velocityY": 0.47594549810702536, + "timestamp": 4.695482889655249 + }, + { + "x": 2.776182973714466, + "y": 5.506073331672824, + "heading": 0.0011912691725219405, + "angularVelocity": -0.01129520780857525, + "velocityX": -1.140193663485079, + "velocityY": 0.376502573915577, + "timestamp": 4.764290118348594 + }, + { + "x": 2.7172794523584742, + "y": 5.525312622061272, + "heading": 0.0006002459449936435, + "angularVelocity": -0.008589551399640344, + "velocityX": -0.8560658883459488, + "velocityY": 0.27961147039059914, + "timestamp": 4.833097347041939 + }, + { + "x": 2.6779739252366173, + "y": 5.5380274141109735, + "heading": 0.00020148371013726726, + "angularVelocity": -0.005795353808442843, + "velocityX": -0.5712412470066293, + "velocityY": 0.18478860856854942, + "timestamp": 4.9019045757352835 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -3.0114656219279604e-34, - "angularVelocity": -0.003147480417366101, - "velocityX": -0.28651653960332735, - "velocityY": 0.08989010211866776, - "timestamp": 4.919125502823283 + "heading": -3.5000485154255244e-36, + "angularVelocity": -0.0029282346341141605, + "velocityX": -0.285853063090511, + "velocityY": 0.09167570478022308, + "timestamp": 4.970711804428628 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -4.388516089708894e-34, - "angularVelocity": -2.000698896485297e-33, - "velocityX": 9.851967688482278e-36, - "velocityY": -1.337407020908214e-34, - "timestamp": 4.987953966791611 + "heading": 7.813037085898689e-36, + "angularVelocity": 1.6441710918960667e-34, + "velocityX": -8.361910991338164e-36, + "velocityY": -1.503965302427871e-35, + "timestamp": 5.039519033121973 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj index 60152d9f..e8bddc0e 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj @@ -3,379 +3,379 @@ { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -4.388516089708894e-34, - "angularVelocity": -2.000698896485297e-33, - "velocityX": 9.851967688482278e-36, - "velocityY": -1.337407020908214e-34, + "heading": 7.813037085898689e-36, + "angularVelocity": 1.6441710918960667e-34, + "velocityX": -8.361910991338164e-36, + "velocityY": -1.503965302427871e-35, "timestamp": 0 }, { "x": 2.6750735555579293, "y": 5.538716972759925, - "heading": 4.633169711684461e-19, - "angularVelocity": 7.277213797999712e-18, + "heading": 1.350818917618456e-19, + "angularVelocity": 2.121700407628309e-18, "velocityX": 0.2633772294028884, - "velocityY": -0.0882468077495622, + "velocityY": -0.088246807749562, "timestamp": 0.06366680765869503 }, { "x": 2.7085920650608806, "y": 5.52742596598787, - "heading": -4.184429803904338e-20, - "angularVelocity": -7.934452625857426e-18, + "heading": 3.9875003320592027e-19, + "angularVelocity": 4.14137524936932e-18, "velocityX": 0.5264675697678677, - "velocityY": -0.1773452633683705, + "velocityY": -0.17734526336837014, "timestamp": 0.12733361531739007 }, { "x": 2.7588390755328, "y": 5.51039889888726, - "heading": 3.955178903113731e-20, - "angularVelocity": 1.278469740567629e-18, - "velocityX": 0.7892183120172118, - "velocityY": -0.2674402522565472, + "heading": 7.83389944204561e-19, + "angularVelocity": 6.041451191657363e-18, + "velocityX": 0.7892183120172117, + "velocityY": -0.2674402522565467, "timestamp": 0.1910004229760851 }, { "x": 2.8257885982701296, "y": 5.487560532002736, - "heading": 8.970064045939625e-19, - "angularVelocity": 1.3467843717867433e-17, - "velocityX": 1.0515608556382, - "velocityY": -0.3587170100777762, + "heading": 1.2799519248933233e-18, + "angularVelocity": 7.799385566035206e-18, + "velocityX": 1.0515608556381997, + "velocityY": -0.35871701007777557, "timestamp": 0.25466723063478014 }, { "x": 2.9094088162269203, "y": 5.4588202139112365, - "heading": 2.18904914594727e-18, - "angularVelocity": 2.0293820106069983e-17, + "heading": 9.407556731948414e-19, + "angularVelocity": -5.3276780189270054e-18, "velocityX": 1.313403656188678, - "velocityY": -0.4514176090871547, + "velocityY": -0.45141760908715395, "timestamp": 0.3183340382934752 }, { "x": 3.0096598962036025, - "y": 5.424066617313393, - "heading": 3.904306808877207e-18, - "angularVelocity": 2.694116017446798e-17, + "y": 5.424066617313392, + "heading": 6.890803629951045e-19, + "angularVelocity": -3.9530065893820216e-18, "velocityX": 1.5746208057754196, - "velocityY": -0.5458668005493709, + "velocityY": -0.5458668005493699, "timestamp": 0.3820008459521702 }, { "x": 3.126490562172417, - "y": 5.383159777949491, - "heading": 4.8450172295989666e-18, - "angularVelocity": 1.4775523625508697e-17, - "velocityX": 1.8350325745107399, - "velocityY": -0.6425143786570184, + "y": 5.38315977794949, + "heading": 5.07923477935949e-19, + "angularVelocity": -2.8453897991917994e-18, + "velocityX": 1.8350325745107396, + "velocityY": -0.6425143786570174, "timestamp": 0.44566765361086524 }, { - "x": 3.2598324200617776, - "y": 5.335918462965805, - "heading": 4.235614277481238e-18, - "angularVelocity": -9.5717529200556e-18, + "x": 3.259832420061777, + "y": 5.335918462965804, + "heading": 3.751534220684201e-19, + "angularVelocity": -2.0853889294918685e-18, "velocityX": 2.0943700931917317, - "velocityY": -0.7420085397863493, + "velocityY": -0.7420085397863481, "timestamp": 0.5093344612695603 }, { - "x": 3.4095898607657253, - "y": 5.282098877479993, - "heading": 2.3874123899446895e-18, - "angularVelocity": -2.902928473254694e-17, + "x": 3.409589860765725, + "y": 5.282098877479992, + "heading": 4.172337353071023e-18, + "angularVelocity": 5.964150034596624e-17, "velocityX": 2.3522059014921575, - "velocityY": -0.8453319314253612, + "velocityY": -0.8453319314253599, "timestamp": 0.5730012689282553 }, { - "x": 3.5756203018815462, - "y": 5.22135582426949, - "heading": 2.5233707069051096e-18, - "angularVelocity": 2.135466217958737e-18, - "velocityX": 2.607802200573299, - "velocityY": -0.9540772569615117, + "x": 3.575620301881546, + "y": 5.221355824269489, + "heading": 4.9261514224901164e-18, + "angularVelocity": 1.1839985341500765e-17, + "velocityX": 2.6078022005732984, + "velocityY": -0.9540772569615101, "timestamp": 0.6366680765869503 }, { - "x": 3.757690030648967, + "x": 3.7576900306489667, "y": 5.153163870724839, - "heading": 3.693577308512614e-18, - "angularVelocity": 1.8380167698697197e-17, + "heading": 5.589206376297959e-18, + "angularVelocity": 1.041445265109494e-17, "velocityX": 2.8597276267323055, - "velocityY": -1.0710754324327665, + "velocityY": -1.071075432432765, "timestamp": 0.7003348842456454 }, { - "x": 3.955353841452516, + "x": 3.9553538414525153, "y": 5.076631094268888, - "heading": 4.677490606421779e-18, - "angularVelocity": 1.5454101345613767e-17, - "velocityX": 3.1046603100187773, - "velocityY": -1.2020828320186725, + "heading": 6.855394728324636e-18, + "angularVelocity": 1.9887731120656582e-17, + "velocityX": 3.104660310018777, + "velocityY": -1.2020828320186703, "timestamp": 0.7640016919043404 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 5.777516683198087e-18, - "angularVelocity": 1.7277858231456074e-17, + "heading": 8.923179883172485e-18, + "angularVelocity": 3.2478228937327026e-17, "velocityX": 3.332116557247906, - "velocityY": -1.36151476081006, + "velocityY": -1.3615147608100575, "timestamp": 0.8276684995630355 }, { "x": 4.285701077560922, "y": 4.938388492372474, - "heading": 6.5114072312405955e-18, - "angularVelocity": 2.119278869815072e-17, - "velocityX": 3.413356765694629, - "velocityY": -1.4888942599373958, + "heading": 1.0772342870449973e-17, + "angularVelocity": 5.3398862490245696e-17, + "velocityX": 3.4133567656946298, + "velocityY": -1.4888942599373933, "timestamp": 0.8622977575854378 }, { "x": 4.403715149774036, "y": 4.881600772971698, - "heading": 6.4908057437571035e-18, - "angularVelocity": -5.949156482118572e-19, - "velocityX": 3.40792956455403, - "velocityY": -1.6398768741750078, + "heading": 9.945107603157448e-18, + "angularVelocity": -2.3888333580736016e-17, + "velocityX": 3.4079295645540313, + "velocityY": -1.6398768741750054, "timestamp": 0.8969270156078402 }, { "x": 4.519367006147829, "y": 4.8201449517293415, - "heading": 6.136184222547225e-18, - "angularVelocity": -1.0240517454358069e-17, - "velocityX": 3.3397151131270637, - "velocityY": -1.7746791225673975, + "heading": 1.0140262228054944e-17, + "angularVelocity": 5.6355416212281845e-18, + "velocityX": 3.3397151131270646, + "velocityY": -1.7746791225673952, "timestamp": 0.9315562736302425 }, { "x": 4.6324721141561005, - "y": 4.754119165100519, - "heading": 6.684498529619524e-18, - "angularVelocity": 1.5833845088958816e-17, - "velocityX": 3.2661718577713303, - "velocityY": -1.906647453610145, + "y": 4.75411916510052, + "heading": 1.0958701743566334e-17, + "angularVelocity": 2.3634335883891303e-17, + "velocityX": 3.2661718577713317, + "velocityY": -1.906647453610143, "timestamp": 0.9661855316526449 }, { "x": 4.742850082889288, - "y": 4.683628924272633, - "heading": 1.0669160659402455e-17, - "angularVelocity": 1.1506634439597858e-16, - "velocityX": 3.187419397256002, - "velocityY": -2.03556890483432, + "y": 4.683628924272634, + "heading": 1.1951109976942982e-17, + "angularVelocity": 2.8658085389373404e-17, + "velocityX": 3.1874193972560034, + "velocityY": -2.0355689048343186, "timestamp": 1.0008147896750472 }, { "x": 4.850325220314879, - "y": 4.608787325463071, - "heading": 1.658478005292643e-17, - "angularVelocity": 1.708272059914504e-16, - "velocityX": 3.1035934225348787, - "velocityY": -2.161224440937961, + "y": 4.608787325463072, + "heading": 1.1732443750037932e-17, + "angularVelocity": -6.3144935638988695e-18, + "velocityX": 3.1035934225348796, + "velocityY": -2.1612244409379597, "timestamp": 1.0354440476974496 }, { "x": 4.959315849281407, - "y": 4.536170434444089, - "heading": 2.4155748869411193e-17, - "angularVelocity": 2.1862925308959841e-16, - "velocityX": 3.1473567494868284, - "velocityY": -2.0969808527807587, + "y": 4.53617043444409, + "heading": 1.1207275250284524e-17, + "angularVelocity": -1.516545631482102e-17, + "velocityX": 3.147356749486827, + "velocityY": -2.096980852780761, "timestamp": 1.070073305719852 }, { "x": 5.0711194796065495, - "y": 4.467963995966004, - "heading": 2.3436163975709297e-17, - "angularVelocity": -2.0779679808224236e-17, - "velocityX": 3.228588676454313, - "velocityY": -1.969618824462286, + "y": 4.467963995966005, + "heading": 1.0229768329797992e-17, + "angularVelocity": -2.822777547974504e-17, + "velocityX": 3.228588676454311, + "velocityY": -1.969618824462288, "timestamp": 1.1047025637422543 }, { "x": 5.1855579490393096, - "y": 4.4042772106470105, - "heading": 2.204718666214906e-17, - "angularVelocity": -4.0109935727230666e-17, - "velocityX": 3.3046757559381796, - "velocityY": -1.8391033754691897, + "y": 4.404277210647011, + "heading": 8.869711544920542e-18, + "angularVelocity": -3.9274788503918017e-17, + "velocityX": 3.3046757559381783, + "velocityY": -1.8391033754691917, "timestamp": 1.1393318217646566 }, { "x": 5.302448702578612, "y": 4.345211810292891, - "heading": 2.1117777344899902e-17, - "angularVelocity": -2.6838845829382515e-17, - "velocityX": 3.375491137109673, - "velocityY": -1.7056501850518673, + "heading": 8.222829686597657e-18, + "angularVelocity": -1.8680211337603908e-17, + "velocityX": 3.3754911371096723, + "velocityY": -1.705650185051869, "timestamp": 1.173961079787059 }, { "x": 5.421605230513571, "y": 4.290862095789443, - "heading": 1.9153982489912048e-17, - "angularVelocity": -5.670912306921066e-17, - "velocityX": 3.440920618566971, - "velocityY": -1.569473838229202, + "heading": 7.194520598849692e-18, + "angularVelocity": -2.969480567798286e-17, + "velocityX": 3.4409206185669707, + "velocityY": -1.5694738382292037, "timestamp": 1.2085903378094613 }, { "x": 5.542837392691082, "y": 4.241314822773421, - "heading": 1.5300631909411777e-17, - "angularVelocity": -1.112744193943597e-16, - "velocityX": 3.5008593628856843, - "velocityY": -1.430792221536174, + "heading": 5.567604077424851e-18, + "angularVelocity": -4.698098123766793e-17, + "velocityX": 3.500859362885684, + "velocityY": -1.4307922215361755, "timestamp": 1.2432195958318637 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 9.183754863330666e-18, - "angularVelocity": -1.766389866662468e-16, - "velocityX": 3.555211493416193, - "velocityY": -1.2898268911821011, + "heading": 4.441119305733273e-18, + "angularVelocity": -3.252985585087723e-17, + "velocityX": 3.5552114934161922, + "velocityY": -1.2898268911821025, "timestamp": 1.277848853854266 }, { "x": 5.934272395491136, - "y": 4.124337934678792, - "heading": -1.4789774171207362e-18, - "angularVelocity": -1.4511299214964166e-16, - "velocityX": 3.651673302114514, - "velocityY": -0.9841085377661543, + "y": 4.124337934678793, + "heading": 1.9097100466976255e-18, + "angularVelocity": -3.445086702659097e-17, + "velocityX": 3.6516733021145136, + "velocityY": -0.9841085377661556, "timestamp": 1.3513276797160874 }, { "x": 6.207753017493673, "y": 4.075010140740357, - "heading": -1.5184517487424443e-17, - "angularVelocity": -1.8652366732257328e-16, - "velocityX": 3.7218970063133985, - "velocityY": -0.6713198443208211, + "heading": -4.66846161540337e-18, + "angularVelocity": -8.952472477542546e-17, + "velocityX": 3.721897006313398, + "velocityY": -0.6713198443208224, "timestamp": 1.4248065055779087 }, { "x": 6.4844286250858945, "y": 4.049019985828209, - "heading": -2.210208594102609e-17, - "angularVelocity": -9.414369884747846e-17, + "heading": -6.502926410881733e-18, + "angularVelocity": -2.4965896963679208e-17, "velocityX": 3.7653787243758563, - "velocityY": -0.35370944768528717, + "velocityY": -0.3537094476852885, "timestamp": 1.49828533143973 }, { "x": 6.743277363747609, "y": 4.038426394199003, - "heading": -1.998169117841033e-17, - "angularVelocity": 2.885722162467868e-17, - "velocityX": 3.522766397335807, - "velocityY": -0.14417203194192602, + "heading": -5.737032029312171e-18, + "angularVelocity": 1.0423334512854795e-17, + "velocityX": 3.5227663973358068, + "velocityY": -0.1441720319419282, "timestamp": 1.5717641573015513 }, { "x": 6.978672822837657, - "y": 4.030025299692623, - "heading": -1.7754526303263836e-17, - "angularVelocity": 3.031029482336482e-17, - "velocityX": 3.2035822065626665, - "velocityY": -0.1143335431376982, + "y": 4.030025299692622, + "heading": -4.2593802240854646e-18, + "angularVelocity": 2.010989952405427e-17, + "velocityX": 3.2035822065626656, + "velocityY": -0.11433354313770064, "timestamp": 1.6452429831633726 }, { "x": 7.19055269256627, "y": 4.022995759457438, - "heading": -1.4995601737739213e-17, - "angularVelocity": 3.754720537740805e-17, - "velocityX": 2.8835500192539434, - "velocityY": -0.095667563447506, + "heading": -3.134299993682012e-18, + "angularVelocity": 1.5311625045821916e-17, + "velocityX": 2.883550019253943, + "velocityY": -0.09566756344750842, "timestamp": 1.718721809025194 }, { "x": 7.3789017577018114, "y": 4.017045849522277, - "heading": -1.2218093470845859e-17, - "angularVelocity": 3.780011771168623e-17, - "velocityX": 2.5633107623376494, - "velocityY": -0.08097448299391073, + "heading": -2.330563766445784e-18, + "angularVelocity": 1.0938337919929083e-17, + "velocityX": 2.5633107623376485, + "velocityY": -0.080974482993913, "timestamp": 1.7922006348870152 }, { "x": 7.54371362899692, "y": 4.012025997027571, - "heading": -9.378231648873842e-18, - "angularVelocity": 3.864870986523972e-17, - "velocityX": 2.2429845518359373, - "velocityY": -0.06831699385270533, + "heading": -1.6314906834703783e-18, + "angularVelocity": 9.513939216857223e-18, + "velocityX": 2.242984551835937, + "velocityY": -0.06831699385270738, "timestamp": 1.8656794607488365 }, { "x": 7.684984887810538, "y": 4.007845278835319, - "heading": -6.610589160747635e-18, - "angularVelocity": 3.7665850749041794e-17, - "velocityX": 1.9226118158077508, - "velocityY": -0.05689691068436128, + "heading": -1.3889434062297666e-18, + "angularVelocity": 3.300913894524217e-18, + "velocityX": 1.9226118158077503, + "velocityY": -0.05689691068436309, "timestamp": 1.9391582866106578 }, { "x": 7.8027134339680275, "y": 4.0044425825275765, - "heading": -4.4053913927362146e-18, - "angularVelocity": 3.0011336492479294e-17, - "velocityX": 1.6022104977409417, - "velocityY": -0.046308528584020826, + "heading": -1.0305851069387887e-18, + "angularVelocity": 4.877028110994567e-18, + "velocityX": 1.6022104977409415, + "velocityY": -0.04630852858402237, "timestamp": 2.012637112472479 }, { "x": 7.896897856926731, "y": 4.001774011196325, - "heading": -2.659547727350374e-18, - "angularVelocity": 2.375981985162547e-17, - "velocityX": 1.281789983087365, - "velocityY": -0.03631755543113615, + "heading": -6.085636422043956e-19, + "angularVelocity": 5.743443227141563e-18, + "velocityX": 1.2817899830873647, + "velocityY": -0.036317555431137405, "timestamp": 2.0861159383343004 }, { "x": 7.967537148527336, "y": 3.999806507404438, - "heading": -1.3934136908294963e-18, - "angularVelocity": 1.723127746899314e-17, - "velocityX": 0.9613557480279207, - "velocityY": -0.026776472933672534, + "heading": -1.6215491216433702e-19, + "angularVelocity": 6.07533836862799e-18, + "velocityX": 0.9613557480279205, + "velocityY": -0.026776472933673492, "timestamp": 2.1595947641961217 }, { "x": 8.014630554462236, "y": 3.9985142796609896, - "heading": -4.425448526284405e-19, - "angularVelocity": 1.2940719003719293e-17, - "velocityX": 0.6409112473226126, - "velocityY": -0.017586396193627105, + "heading": 9.722147651876556e-20, + "angularVelocity": 3.5299473779135435e-18, + "velocityX": 0.6409112473226125, + "velocityY": -0.017586396193627746, "timestamp": 2.233073590057943 }, { "x": 8.038177490234375, "y": 3.9978766441345215, - "heading": 3.3992773241588707e-43, - "angularVelocity": 6.022753459080235e-18, - "velocityX": 0.320458791984776, - "velocityY": -0.008677813220192955, + "heading": 9.32148398835055e-44, + "angularVelocity": -1.3231223468593855e-18, + "velocityX": 0.32045879198477595, + "velocityY": -0.00867781322019328, "timestamp": 2.3065524159197643 }, { "x": 8.038177490234375, "y": 3.9978766441345215, - "heading": -7.143469642081013e-44, - "angularVelocity": -5.598380381353948e-42, - "velocityX": -5.65394748040853e-41, - "velocityY": -2.108446218115532e-39, + "heading": -1.3950188036384555e-43, + "angularVelocity": -3.1671262842054474e-42, + "velocityX": -2.982658307493247e-43, + "velocityY": -2.0280554768402976e-41, "timestamp": 2.3800312417815856 } ] diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.traj index 8f5ce45b..c863df6d 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.traj @@ -4,1801 +4,1819 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": -8.928261679081423e-33, - "velocityX": -3.922322521208882e-33, - "velocityY": -1.0855099176840522e-33, + "angularVelocity": -2.75917641303777e-30, + "velocityX": -8.847327306636262e-30, + "velocityY": 1.4420464788285476e-29, "timestamp": 0 }, { - "x": 1.306057392567738, - "y": 5.572382489557579, - "heading": -0.01069490392439116, - "angularVelocity": -0.14212387666683293, - "velocityX": 0.21372515459002697, - "velocityY": -0.2467996203807809, - "timestamp": 0.07525057840535954 - }, - { - "x": 1.33822347033388, - "y": 5.535239164732594, - "heading": -0.0320761792409761, - "angularVelocity": -0.28413436507302803, - "velocityX": 0.4274528973434579, - "velocityY": -0.49359520700160897, - "timestamp": 0.15050115681071907 - }, - { - "x": 1.3864731960688501, - "y": 5.4795246417936365, - "heading": -0.06412173122646007, - "angularVelocity": -0.42585123815077036, - "velocityX": 0.6411874401158554, - "velocityY": -0.7403866404698544, - "timestamp": 0.2257517352160786 - }, - { - "x": 1.4508074313630048, - "y": 5.405239333647035, - "heading": -0.1067984096254206, - "angularVelocity": -0.567127579658856, - "velocityX": 0.8549334325059841, - "velocityY": -0.9871725868529632, - "timestamp": 0.30100231362143814 - }, - { - "x": 1.5312274241077561, - "y": 5.312383875814834, - "heading": -0.16006570142282744, - "angularVelocity": -0.707865546367854, - "velocityX": 1.0686960080432237, - "velocityY": -1.233950087825352, - "timestamp": 0.3762528920267977 - }, - { - "x": 1.6277348707066417, - "y": 5.200959293585713, - "heading": -0.2238805012390217, - "angularVelocity": -0.848030688514272, - "velocityX": 1.2824811269757883, - "velocityY": -1.4807139638036912, - "timestamp": 0.4515034704321572 - }, - { - "x": 1.7403321345867286, - "y": 5.070967309799712, - "heading": -0.29820239377845176, - "angularVelocity": -0.9876587544493436, - "velocityX": 1.4962976533356125, - "velocityY": -1.727454945073796, - "timestamp": 0.5267540488375168 - }, - { - "x": 1.8690235919905007, - "y": 4.922411591690569, - "heading": -0.3829978316122351, - "angularVelocity": -1.126841010802703, - "velocityX": 1.7101723352947207, - "velocityY": -1.9741471927152945, - "timestamp": 0.6020046272428763 - }, - { - "x": 2.021840076397225, - "y": 4.778612689187088, - "heading": -0.46243678907347296, - "angularVelocity": -1.055659094516408, - "velocityX": 2.030768236538101, - "velocityY": -1.9109341821781907, - "timestamp": 0.6772552056482358 - }, - { - "x": 2.158571900473102, - "y": 4.653385130963448, - "heading": -0.5314200615226269, - "angularVelocity": -0.916714182282495, - "velocityX": 1.8170202405532427, - "velocityY": -1.6641408063212961, - "timestamp": 0.7525057840535954 - }, - { - "x": 2.2792154502461437, - "y": 4.546725640046215, - "heading": -0.5899528620106769, - "angularVelocity": -0.7778385459410783, - "velocityX": 1.6032242187317085, - "velocityY": -1.4173909779494496, - "timestamp": 0.8277563624589549 - }, - { - "x": 2.383769251108642, - "y": 4.458632746099449, - "heading": -0.6380359031198586, - "angularVelocity": -0.6389723790582461, - "velocityX": 1.3894086009450728, - "velocityY": -1.1706606887754087, - "timestamp": 0.9030069408643144 - }, - { - "x": 2.472232460585404, - "y": 4.389105547578634, - "heading": -0.6756681828246474, - "angularVelocity": -0.5000928963239504, - "velocityX": 1.175581787560874, - "velocityY": -0.9239423801673171, - "timestamp": 0.978257519269674 - }, - { - "x": 2.544604550352729, - "y": 4.338143448939913, - "heading": -0.7028480663779157, - "angularVelocity": -0.3611916895423175, - "velocityX": 0.9617479533176606, - "velocityY": -0.677231985702458, - "timestamp": 1.0535080976750335 - }, - { - "x": 2.6008851962155197, - "y": 4.305746063716667, - "heading": -0.7195737337551226, - "angularVelocity": -0.22226629657394859, - "velocityX": 0.7479098108670842, - "velocityY": -0.4305267269671826, - "timestamp": 1.128758676080393 - }, - { - "x": 2.641074231196452, - "y": 4.291913169677742, - "heading": -0.7258433495685629, - "angularVelocity": -0.08331651325877153, - "velocityX": 0.5340694494657862, - "velocityY": -0.18382442144709177, - "timestamp": 1.2040092544857526 + "x": 1.3060574436245773, + "y": 5.572382537817046, + "heading": -0.010694867587036427, + "angularVelocity": -0.14212340511596666, + "velocityX": 0.21372585011902087, + "velocityY": -0.24679899874441508, + "timestamp": 0.07525057240542533 + }, + { + "x": 1.3382236275279018, + "y": 5.53523931290218, + "heading": -0.032076067517629925, + "angularVelocity": -0.2841333859323569, + "velocityX": 0.42745434186951237, + "velocityY": -0.4935939186635594, + "timestamp": 0.15050114481085067 + }, + { + "x": 1.386473519841097, + "y": 5.479524946048311, + "heading": -0.0641215016247012, + "angularVelocity": -0.42584970563284796, + "velocityX": 0.6411897048771923, + "velocityY": -0.7403846253066342, + "timestamp": 0.22575171721627602 + }, + { + "x": 1.450807989654895, + "y": 5.405239856496701, + "heading": -0.10679801494850981, + "angularVelocity": -0.5671254312134459, + "velocityX": 0.8549366171751026, + "velocityY": -0.9871697606799944, + "timestamp": 0.30100228962170134 + }, + { + "x": 1.5312282961066404, + "y": 5.312384689270129, + "heading": -0.16006508743981135, + "angularVelocity": -0.7078626884752101, + "velocityX": 1.0687002620672836, + "velocityY": -1.23394632438782, + "timestamp": 0.37625286202712666 + }, + { + "x": 1.627736154340151, + "y": 5.200960485487107, + "heading": -0.22387960186124028, + "angularVelocity": -0.8480269635530943, + "velocityX": 1.2824866993939017, + "velocityY": -1.4807090527398261, + "timestamp": 0.451503434432552 + }, + { + "x": 1.7403339652666383, + "y": 5.0709689996393825, + "heading": -0.2982011188077779, + "angularVelocity": -0.9876538419890185, + "velocityX": 1.4963050422725384, + "velocityY": -1.7274484657660032, + "timestamp": 0.5267540068379774 + }, + { + "x": 1.8690262176186487, + "y": 4.922413993905078, + "heading": -0.382996017565801, + "angularVelocity": -1.1268339369356364, + "velocityX": 1.710183035621927, + "velocityY": -1.9741378834553056, + "timestamp": 0.6020045792434027 + }, + { + "x": 2.0218420905066052, + "y": 4.778614575286612, + "heading": -0.4624353832107095, + "angularVelocity": -1.055664602990153, + "velocityX": 2.030760272057439, + "velocityY": -1.9109411931313167, + "timestamp": 0.677255151648828 + }, + { + "x": 2.15857350503456, + "y": 4.653386666317764, + "heading": -0.5314189322497749, + "angularVelocity": -0.9167179309443209, + "velocityX": 1.8170149429960494, + "velocityY": -1.6641456000181232, + "timestamp": 0.7525057240542533 + }, + { + "x": 2.2792167348207513, + "y": 4.5467268950736175, + "heading": -0.589951951408673, + "angularVelocity": -0.7778415138506264, + "velocityX": 1.6032200942942694, + "velocityY": -1.4173948161921597, + "timestamp": 0.8277562964596786 + }, + { + "x": 2.38377026778444, + "y": 4.458633759569044, + "heading": -0.6380351777892487, + "angularVelocity": -0.6389748920543931, + "velocityX": 1.3894051516512624, + "velocityY": -1.1706639921478266, + "timestamp": 0.903006868865104 + }, + { + "x": 2.472233242714231, + "y": 4.389106342435906, + "heading": -0.6756676216200074, + "angularVelocity": -0.5000951172479533, + "velocityX": 1.1755787644266529, + "velocityY": -0.9239453589485842, + "timestamp": 0.9782574412705293 + }, + { + "x": 2.5446051200448103, + "y": 4.3381440386367105, + "heading": -0.7028476555095471, + "angularVelocity": -0.3611937161417901, + "velocityX": 0.9617452069540623, + "velocityY": -0.6772347660530061, + "timestamp": 1.0535080136759547 + }, + { + "x": 2.600885568086537, + "y": 4.305746455375921, + "heading": -0.7195734643408855, + "angularVelocity": -0.22226819406292112, + "velocityX": 0.7479072416796508, + "velocityY": -0.4305293929915994, + "timestamp": 1.12875858608138 + }, + { + "x": 2.641074414508583, + "y": 4.291913365901662, + "heading": -0.7258432162336076, + "angularVelocity": -0.08331832824324738, + "velocityX": 0.5340669863123017, + "velocityY": -0.18382703322097846, + "timestamp": 1.2040091584868053 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.055657173156648594, - "velocityX": 0.320228661947563, - "velocityY": 0.06287683198072233, - "timestamp": 1.2792598328911122 - }, - { - "x": 2.6731094248331484, - "y": 4.3202884971859605, - "heading": -0.7067986870475855, - "angularVelocity": 0.19580509628602769, - "velocityX": 0.1046188223798005, - "velocityY": 0.3116212313704759, - "timestamp": 1.355133384347763 - }, - { - "x": 2.6646882165450285, - "y": 4.362805712309731, - "heading": -0.6813129128724049, - "angularVelocity": 0.3358979998417677, - "velocityX": -0.11099003706094854, - "velocityY": 0.5603693817873046, - "timestamp": 1.431006935804414 - }, - { - "x": 2.6399081500889086, - "y": 4.424196738034775, - "heading": -0.6452019314674031, - "angularVelocity": 0.47593635346874824, - "velocityX": -0.3265968968155938, - "velocityY": 0.8091228701758959, - "timestamp": 1.5068804872610648 - }, - { - "x": 2.5987694540060504, - "y": 4.504462102027486, - "heading": -0.5984691810773222, - "angularVelocity": 0.6159293916376241, - "velocityX": -0.542200744436769, - "velocityY": 1.0578833131143173, - "timestamp": 1.5827540387177157 - }, - { - "x": 2.5412724297463463, - "y": 4.603602434972891, - "heading": -0.5411158632576627, - "angularVelocity": 0.7559065935173384, - "velocityX": -0.757800619001658, - "velocityY": 1.3066520683699112, - "timestamp": 1.6586275901743666 - }, - { - "x": 2.4674174122154677, - "y": 4.7216183838541035, - "heading": -0.4731379535005828, - "angularVelocity": 0.8959368376992063, - "velocityX": -0.9733960795689668, - "velocityY": 1.5554293507486547, - "timestamp": 1.7345011416310174 - }, - { - "x": 2.37720454589188, - "y": 4.858510309507254, - "heading": -0.39452114170963426, - "angularVelocity": 1.0361556864234158, - "velocityX": -1.188989635935645, - "velocityY": 1.8042113888838065, - "timestamp": 1.8103746930876683 - }, - { - "x": 2.270632337422942, - "y": 5.01427685954172, - "heading": -0.305233667532112, - "angularVelocity": 1.1767931309836617, - "velocityX": -1.4046028744262238, - "velocityY": 2.0529756027495263, - "timestamp": 1.8862482445443192 - }, - { - "x": 2.156211319422429, - "y": 5.146461372898718, - "heading": -0.2291032339501207, - "angularVelocity": 1.003385661016371, - "velocityX": -1.508048797028907, - "velocityY": 1.7421685267035296, - "timestamp": 1.96212179600097 - }, - { - "x": 2.058134463277846, - "y": 5.259759383515319, - "heading": -0.16374337088202898, - "angularVelocity": 0.8614314449935028, - "velocityX": -1.2926356320702503, - "velocityY": 1.493247758164214, - "timestamp": 2.037995347457621 - }, - { - "x": 1.976403644982925, - "y": 5.354173277081842, - "heading": -0.10920518207341517, - "angularVelocity": 0.7188036906348487, - "velocityX": -1.0771977418457426, - "velocityY": 1.2443584326016996, - "timestamp": 2.113868898914272 - }, - { - "x": 1.9110189705231253, - "y": 5.429703862947542, - "heading": -0.06553401357556585, - "angularVelocity": 0.5755782833336871, - "velocityX": -0.861758454751598, - "velocityY": 0.9954797741193555, - "timestamp": 2.189742450370923 - }, - { - "x": 1.8619803278189464, - "y": 5.486351581956828, - "heading": -0.032764600292336035, - "angularVelocity": 0.43189507613798817, - "velocityX": -0.6463206448454762, - "velocityY": 0.7466069258884014, - "timestamp": 2.265616001827574 - }, - { - "x": 1.829287687812507, - "y": 5.524116686023993, - "heading": -0.010918035230124941, - "angularVelocity": 0.28793386684545474, - "velocityX": -0.43088321791708567, - "velocityY": 0.4977373978433378, - "timestamp": 2.3414895532842253 + "angularVelocity": 0.05565540572186624, + "velocityX": 0.32022625146750794, + "velocityY": 0.06287422939745346, + "timestamp": 1.2792597308922307 + }, + { + "x": 2.6731092407857906, + "y": 4.320288302812448, + "heading": -0.7067988166038234, + "angularVelocity": 0.1958033723255861, + "velocityX": 0.10461638788158735, + "velocityY": 0.31161864341117435, + "timestamp": 1.3551332887134768 + }, + { + "x": 2.664687841052256, + "y": 4.3628053226693035, + "heading": -0.6813131694887641, + "angularVelocity": 0.33589629702821755, + "velocityX": -0.11099255097832775, + "velocityY": 0.5603667611869495, + "timestamp": 1.431006846534723 + }, + { + "x": 2.6399075701479586, + "y": 4.42419614745244, + "heading": -0.6452023149092349, + "angularVelocity": 0.47593464200079133, + "velocityX": -0.32659956402163576, + "velocityY": 0.8091201539132412, + "timestamp": 1.5068804043559691 + }, + { + "x": 2.598768648766779, + "y": 4.504461298133711, + "heading": -0.5984696942887661, + "angularVelocity": 0.6159276296230222, + "velocityX": -0.5422036683592814, + "velocityY": 1.057880412955572, + "timestamp": 1.5827539621772153 + }, + { + "x": 2.5412713665853555, + "y": 4.603601395355974, + "heading": -0.5411165139934166, + "angularVelocity": 0.7559047175548449, + "velocityX": -0.7578039548110096, + "velocityY": 1.3066488519598491, + "timestamp": 1.6586275199984615 + }, + { + "x": 2.4674160388824338, + "y": 4.721617069365857, + "heading": -0.47313875765168273, + "angularVelocity": 0.8959347405484323, + "velocityX": -0.9734000859460189, + "velocityY": 1.5554255975016502, + "timestamp": 1.7345010778197076 + }, + { + "x": 2.377202770872684, + "y": 4.858508647531332, + "heading": -0.39452213175531803, + "angularVelocity": 1.0361531494380563, + "velocityX": -1.1889948303725923, + "velocityY": 1.8042066576932412, + "timestamp": 1.8103746356409538 + }, + { + "x": 2.270629951391599, + "y": 5.014274677090094, + "heading": -0.305234926659776, + "angularVelocity": 1.1767894858012566, + "velocityX": -1.4046108096674554, + "velocityY": 2.0529685707303664, + "timestamp": 1.8862481934622 + }, + { + "x": 2.1562097285316435, + "y": 5.146459905330176, + "heading": -0.22910409225153178, + "angularVelocity": 1.0033908596890408, + "velocityX": -1.5080381906707807, + "velocityY": 1.7421778026344412, + "timestamp": 1.9621217512834461 + }, + { + "x": 2.0581334055736438, + "y": 5.259758402247803, + "heading": -0.16374394944584372, + "angularVelocity": 0.8614350596401947, + "velocityX": -1.2926284963034982, + "velocityY": 1.4932540422920335, + "timestamp": 2.0379953091046925 + }, + { + "x": 1.9764029763301112, + "y": 5.354172653968883, + "heading": -0.10920555144834718, + "angularVelocity": 0.7188063874223503, + "velocityX": -1.0771925238352613, + "velocityY": 1.2443630486526756, + "timestamp": 2.1138688669259387 + }, + { + "x": 1.9110185860607423, + "y": 5.429703503317112, + "heading": -0.06553422750381226, + "angularVelocity": 0.5755802838199697, + "velocityX": -0.8617546368678625, + "velocityY": 0.9954831632816045, + "timestamp": 2.189742424747185 + }, + { + "x": 1.8619801423257207, + "y": 5.486351407872208, + "heading": -0.032764704050183134, + "angularVelocity": 0.43189649194112756, + "velocityX": -0.6463179682397073, + "velocityY": 0.7466093087307788, + "timestamp": 2.265615982568431 + }, + { + "x": 1.8292876278534882, + "y": 5.524116629586748, + "heading": -0.010918068889347814, + "angularVelocity": 0.28793476658359446, + "velocityX": -0.43088152724695716, + "velocityY": 0.4977389066689233, + "timestamp": 2.341489540389677 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -6.884486248240525e-32, - "angularVelocity": 0.14389777492309389, - "velocityX": -0.21544390526149312, - "velocityY": 0.2488690879972792, - "timestamp": 2.4173631047408763 + "heading": 2.394848489426687e-27, + "angularVelocity": 0.14389820647675833, + "velocityX": -0.21544309693644778, + "velocityY": 0.24886981095702526, + "timestamp": 2.4173630982109233 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -2.79027732569672e-32, - "angularVelocity": 5.39778462149558e-31, - "velocityX": -1.372141826224318e-33, - "velocityY": 4.2150961560958763e-32, - "timestamp": 2.4932366561975274 + "heading": 2.3925172511788446e-27, + "angularVelocity": -3.168744293663733e-29, + "velocityX": 8.48223659243716e-32, + "velocityY": -3.031681211954363e-28, + "timestamp": 2.4932366560321695 }, { "x": 1.8507557561824508, "y": 5.5430527114919315, - "heading": -1.045695782000071e-18, - "angularVelocity": -1.1232079825061295e-17, - "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395097007, - "timestamp": 2.586335690239901 + "heading": -1.9710234547097696e-21, + "angularVelocity": -2.1171281249858964e-20, + "velocityX": 0.40617567078743816, + "velocityY": 0.0005740544395108319, + "timestamp": 2.586335690074543 }, { "x": 1.9263848800936068, - "y": 5.543159599317717, - "heading": -3.434421194050575e-18, - "angularVelocity": -2.5657896847386088e-17, - "velocityX": 0.812351327691905, - "velocityY": 0.0011481088593983807, - "timestamp": 2.6794347242822742 + "y": 5.5431595993177165, + "heading": -2.0389636321585498e-20, + "angularVelocity": -1.9783892557349584e-19, + "velocityX": 0.8123513276919052, + "velocityY": 0.001148108859400643, + "timestamp": 2.6794347241169163 }, { "x": 2.039828562987611, "y": 5.543319931052194, - "heading": -7.689792330773154e-18, - "angularVelocity": -4.570800524939683e-17, - "velocityX": 1.2185269596070252, - "velocityY": 0.0017221632439692253, - "timestamp": 2.7725337583246477 + "heading": -2.352458064017618e-20, + "angularVelocity": -3.3673220682050347e-20, + "velocityX": 1.2185269596070256, + "velocityY": 0.001722163243972619, + "timestamp": 2.7725337581592897 }, { "x": 2.1910867994360017, - "y": 5.543533706687691, - "heading": -1.506248986539688e-17, - "angularVelocity": -7.919198744068695e-17, - "velocityX": 1.6247025332136764, - "velocityY": 0.0022962175461317975, - "timestamp": 2.865632792367021 + "y": 5.54353370668769, + "heading": -3.364930009404432e-20, + "angularVelocity": -1.087521428982912e-19, + "velocityX": 1.6247025332136769, + "velocityY": 0.0022962175461363225, + "timestamp": 2.865632792201663 }, { - "x": 2.380159562296481, - "y": 5.5438009261858445, - "heading": -1.506353706937017e-17, - "angularVelocity": -1.1248279684723702e-20, - "velocityX": 2.0308778152781284, - "velocityY": 0.0028702714362532117, - "timestamp": 2.9587318264093945 + "x": 2.3801595622964817, + "y": 5.543800926185845, + "heading": -1.7618072508363382e-20, + "angularVelocity": 1.721954234066794e-19, + "velocityX": 2.030877815278129, + "velocityY": 0.0028702714362588678, + "timestamp": 2.9587318262440365 }, { - "x": 2.5314177987448714, + "x": 2.5314177987448723, "y": 5.544014701821341, - "heading": -7.704513323430137e-18, - "angularVelocity": 7.904511385790122e-17, - "velocityX": 1.6247025332136762, - "velocityY": 0.002296217546131797, - "timestamp": 3.051830860451768 + "heading": -7.829457975228232e-21, + "angularVelocity": 1.0514195591633778e-19, + "velocityX": 1.6247025332136766, + "velocityY": 0.0022962175461363225, + "timestamp": 3.05183086028641 }, { - "x": 2.6448614816388756, - "y": 5.544175033555818, - "heading": -3.433430554917341e-18, - "angularVelocity": 4.587676781446345e-17, - "velocityX": 1.218526959607025, - "velocityY": 0.0017221632439692251, - "timestamp": 3.1449298944941413 + "x": 2.6448614816388765, + "y": 5.544175033555819, + "heading": 1.3151205701559364e-20, + "angularVelocity": 2.253585538517852e-19, + "velocityX": 1.2185269596070254, + "velocityY": 0.001722163243972619, + "timestamp": 3.1449298943287833 }, { - "x": 2.7204906055500317, + "x": 2.720490605550032, "y": 5.544281921381604, - "heading": -1.0420071252828256e-18, - "angularVelocity": 2.5686876928777523e-17, - "velocityX": 0.812351327691905, - "velocityY": 0.0011481088593983805, - "timestamp": 3.2380289285365147 + "heading": 9.829941746315052e-21, + "angularVelocity": -3.567452648039799e-20, + "velocityX": 0.8123513276919051, + "velocityY": 0.0011481088594006432, + "timestamp": 3.2380289283711567 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -1.1539777836664478e-35, - "angularVelocity": 1.1192459041074064e-17, - "velocityX": 0.4061756707874381, - "velocityY": 0.0005740544395097005, - "timestamp": 3.331127962578888 + "heading": -1.6124701211579614e-34, + "angularVelocity": -1.0558586184512984e-19, + "velocityX": 0.40617567078743816, + "velocityY": 0.000574054439510832, + "timestamp": 3.33112796241353 }, { "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, - "angularVelocity": 7.156872912951633e-35, - "velocityX": -1.751436869795613e-36, - "velocityY": -4.841667244220732e-36, - "timestamp": 3.4242269966212615 + "angularVelocity": 1.724633516740329e-33, + "velocityX": 6.381177903729197e-38, + "velocityY": -6.411016173939856e-36, + "timestamp": 3.4242269964559036 }, { "x": 2.7396220511907092, "y": 5.562601673573336, - "heading": 0.0037078860807813565, - "angularVelocity": 0.0478983711971462, - "velocityX": -0.2413479950107084, - "velocityY": 0.23596367181626482, - "timestamp": 3.5016385241556733 + "heading": 0.003707886080781361, + "angularVelocity": 0.04789837119714626, + "velocityX": -0.24134799501070836, + "velocityY": 0.2359636718162648, + "timestamp": 3.5016385239903154 }, { "x": 2.702380969511178, "y": 5.599259386918254, - "heading": 0.011292704027819494, - "angularVelocity": 0.09798047123752286, - "velocityX": -0.4810792767650385, - "velocityY": 0.47354334053959213, - "timestamp": 3.579050051690085 + "heading": 0.011292704027819503, + "angularVelocity": 0.09798047123752293, + "velocityX": -0.4810792767650384, + "velocityY": 0.473543340539592, + "timestamp": 3.579050051524727 }, { "x": 2.6467699433072402, "y": 5.6544931206396525, "heading": 0.023007945503191744, - "angularVelocity": 0.15133716965040475, + "angularVelocity": 0.1513371696504047, "velocityX": -0.7183817187849313, - "velocityY": 0.7135078647924301, - "timestamp": 3.656461579224497 + "velocityY": 0.7135078647924299, + "timestamp": 3.656461579059139 }, { "x": 2.573102880230356, "y": 5.72860253774289, - "heading": 0.03927230490424403, - "angularVelocity": 0.21010255086133378, - "velocityX": -0.9516291103303323, - "velocityY": 0.9573434275701879, - "timestamp": 3.733873106758909 + "heading": 0.03927230490424404, + "angularVelocity": 0.21010255086133403, + "velocityX": -0.9516291103303324, + "velocityY": 0.9573434275701875, + "timestamp": 3.733873106593551 }, { "x": 2.4820081686264115, "y": 5.82215722498829, - "heading": 0.060903903359814396, - "angularVelocity": 0.27943639848670454, - "velocityX": -1.1767589983733413, - "velocityY": 1.2085368965728291, - "timestamp": 3.8112846342933207 + "heading": 0.060903903359814444, + "angularVelocity": 0.2794363984867048, + "velocityX": -1.1767589983733415, + "velocityY": 1.208536896572829, + "timestamp": 3.8112846341279627 }, { "x": 2.3753503305689, "y": 5.936638406081269, - "heading": 0.09019255741494601, - "angularVelocity": 0.3783500337480349, - "velocityX": -1.3778030411568694, - "velocityY": 1.478864772976981, - "timestamp": 3.8886961618277325 + "heading": 0.09019255741494606, + "angularVelocity": 0.378350033748035, + "velocityX": -1.3778030411568696, + "velocityY": 1.4788647729769806, + "timestamp": 3.8886961616623745 }, { "x": 2.2779168004384966, - "y": 6.074876724828902, - "heading": 0.14378193450398408, + "y": 6.0748767248289015, + "heading": 0.1437819345039841, "angularVelocity": 0.6922661106928292, "velocityX": -1.25864368310122, - "velocityY": 1.7857588288279347, - "timestamp": 3.9661076893621443 + "velocityY": 1.7857588288279342, + "timestamp": 3.9661076891967864 }, { "x": 2.2032159820535764, "y": 6.200217981312059, - "heading": 0.2000311193060264, + "heading": 0.20003111930602646, "angularVelocity": 0.7266254341388357, - "velocityX": -0.9649831331866354, - "velocityY": 1.6191549304777415, - "timestamp": 4.043519216896556 + "velocityX": -0.9649831331866353, + "velocityY": 1.6191549304777408, + "timestamp": 4.043519216731198 }, { "x": 2.149623841782867, "y": 6.310133396102646, - "heading": 0.2561023628726984, + "heading": 0.25610236287269844, "angularVelocity": 0.7243267941166335, "velocityX": -0.6923018053982481, - "velocityY": 1.4198843284900602, - "timestamp": 4.120930744430967 + "velocityY": 1.4198843284900597, + "timestamp": 4.12093074426561 }, { "x": 2.1165539042639407, "y": 6.4038530923195935, - "heading": 0.3110558787207687, + "heading": 0.31105587872076873, "angularVelocity": 0.7098880179523891, "velocityX": -0.42719655033581183, - "velocityY": 1.2106684779639116, - "timestamp": 4.1983422719653785 + "velocityY": 1.2106684779639114, + "timestamp": 4.198342271800021 }, { "x": 2.1037067095365876, "y": 6.481006876335726, - "heading": 0.36442032903480814, - "angularVelocity": 0.6893605127520233, - "velocityX": -0.16595971086660646, - "velocityY": 0.9966704762651166, - "timestamp": 4.27575379949979 + "heading": 0.3644203290348082, + "angularVelocity": 0.6893605127520235, + "velocityX": -0.1659597108666065, + "velocityY": 0.9966704762651164, + "timestamp": 4.275753799334432 }, { "x": 2.1109008821512623, "y": 6.541377447734871, - "heading": 0.4159102858737852, - "angularVelocity": 0.6651458571992167, + "heading": 0.41591028587378526, + "angularVelocity": 0.6651458571992168, "velocityX": 0.09293412549541542, "velocityY": 0.7798653937207066, - "timestamp": 4.353165327034201 + "timestamp": 4.353165326868844 }, { "x": 2.1380148682221702, "y": 6.5848219723004595, "heading": 0.465333297367931, - "angularVelocity": 0.6384451136450677, + "angularVelocity": 0.6384451136450678, "velocityX": 0.3502577320781457, "velocityY": 0.5612151826648336, - "timestamp": 4.430576854568613 + "timestamp": 4.430576854403255 }, { "x": 2.1849615573883057, "y": 6.611239433288574, "heading": 0.5125504196, "angularVelocity": 0.6099494963599527, - "velocityX": 0.6064560493947925, - "velocityY": 0.34126004006795546, - "timestamp": 4.507988382103024 + "velocityX": 0.6064560493947926, + "velocityY": 0.3412600400679556, + "timestamp": 4.5079883819376665 }, { "x": 2.250707976446258, "y": 6.648678224064634, "heading": 0.55208552781375, - "angularVelocity": 0.5325541984016288, - "velocityX": 0.8856313560565373, + "angularVelocity": 0.5325541984016284, + "velocityX": 0.8856313560565378, "velocityY": 0.5043159387113171, - "timestamp": 4.582225160880485 + "timestamp": 4.582225160715128 }, { "x": 2.3371670619589073, "y": 6.698226995892447, - "heading": 0.5853560107794161, - "angularVelocity": 0.44816711492022887, - "velocityX": 1.1646395080237382, - "velocityY": 0.6674423734944788, - "timestamp": 4.656461939657946 + "heading": 0.585356010779416, + "angularVelocity": 0.44816711492022837, + "velocityX": 1.1646395080237388, + "velocityY": 0.6674423734944787, + "timestamp": 4.656461939492589 }, { "x": 2.4409073566894506, "y": 6.758690881227722, - "heading": 0.5617681472290976, - "angularVelocity": -0.31773824159353153, - "velocityX": 1.3974245170513666, - "velocityY": 0.8144734501011613, - "timestamp": 4.7306987184354075 + "heading": 0.5617681472290975, + "angularVelocity": -0.31773824159353026, + "velocityX": 1.3974245170513677, + "velocityY": 0.8144734501011609, + "timestamp": 4.73069871827005 }, { "x": 2.523886913207481, "y": 6.807069780587539, "heading": 0.5422957792802172, - "angularVelocity": -0.2623008200187737, - "velocityX": 1.1177688186980357, - "velocityY": 0.6516837092951017, - "timestamp": 4.804935497212869 + "angularVelocity": -0.262300820018773, + "velocityX": 1.1177688186980366, + "velocityY": 0.6516837092951012, + "timestamp": 4.804935497047511 }, { "x": 2.5861173110961473, "y": 6.843356427328165, "heading": 0.5274956073850987, - "angularVelocity": -0.1993644139582728, - "velocityX": 0.8382691021011789, - "velocityY": 0.4887960838037212, - "timestamp": 4.87917227599033 + "angularVelocity": -0.1993644139582724, + "velocityX": 0.8382691021011796, + "velocityY": 0.4887960838037209, + "timestamp": 4.879172275824972 }, { "x": 2.627602512984248, "y": 6.8675485030782895, "heading": 0.5175502299027057, - "angularVelocity": -0.13396833276139558, - "velocityX": 0.5588227637470666, - "velocityY": 0.3258772288954636, - "timestamp": 4.953409054767791 + "angularVelocity": -0.1339683327613954, + "velocityX": 0.558822763747067, + "velocityY": 0.3258772288954634, + "timestamp": 4.953409054602433 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.06734950498988643, - "velocityX": 0.2794033376916902, - "velocityY": 0.16294305705300594, - "timestamp": 5.027645833545252 + "angularVelocity": -0.06734950498988629, + "velocityX": 0.27940333769169035, + "velocityY": 0.16294305705300582, + "timestamp": 5.027645833379895 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -4.084231073354503e-32, - "velocityX": -1.1079933009257942e-33, - "velocityY": -1.0209152810451872e-34, - "timestamp": 5.101882612322713 - }, - { - "x": 2.6570277255317225, - "y": 6.867726793502589, - "heading": 0.5066730395869481, - "angularVelocity": -0.10087167460547103, - "velocityX": 0.14902725506901718, - "velocityY": -0.20454631284120778, - "timestamp": 5.160148523303567 - }, - { - "x": 2.6744037356282937, - "y": 6.843893366933042, - "heading": 0.49507655406382295, - "angularVelocity": -0.1990269323504748, - "velocityX": 0.2982191439910878, - "velocityY": -0.4090458068591583, - "timestamp": 5.218414434284421 - }, - { - "x": 2.7004835818665893, - "y": 6.808147821587349, - "heading": 0.4779456476925187, - "angularVelocity": -0.29401250375942084, - "velocityX": 0.44760042019879576, - "velocityY": -0.6134898561431401, - "timestamp": 5.276680345265275 - }, - { - "x": 2.735280123191829, - "y": 6.760494081762779, - "heading": 0.4554977531812837, - "angularVelocity": -0.38526634413407634, - "velocityX": 0.5972023905482879, - "velocityY": -0.8178665539139939, - "timestamp": 5.334946256246129 - }, - { - "x": 2.77880855180183, - "y": 6.700937036637074, - "heading": 0.4279915994110024, - "angularVelocity": -0.4720796999006835, - "velocityX": 0.7470650999399128, - "velocityY": -1.022159340223411, - "timestamp": 5.393212167226983 - }, - { - "x": 2.8310870878518544, - "y": 6.629482937316093, - "heading": 0.39573903851012515, - "angularVelocity": -0.5535408330177088, - "velocityX": 0.8972405162806606, - "velocityY": -1.2263448407158972, - "timestamp": 5.4514780782078365 - }, - { - "x": 2.8921379557757043, - "y": 6.546140000901142, - "heading": 0.3591220542576228, - "angularVelocity": -0.6284460954285072, - "velocityX": 1.0477973637777922, - "velocityY": -1.4303893136132022, - "timestamp": 5.50974398918869 - }, - { - "x": 2.9619888074879523, - "y": 6.4509193752842275, - "heading": 0.3186184742438743, - "angularVelocity": -0.6951505491273999, - "velocityX": 1.1988287926228676, - "velocityY": -1.6342424586513933, - "timestamp": 5.568009900169544 - }, - { - "x": 3.0406748905144103, - "y": 6.3438367809482505, - "heading": 0.27484324456730397, - "angularVelocity": -0.7513008711209391, - "velocityX": 1.3504651639672256, - "velocityY": -1.8378257978522827, - "timestamp": 5.626275811150398 - }, - { - "x": 3.12824252650645, - "y": 6.224915546783436, - "heading": 0.22861956489914778, - "angularVelocity": -0.7933228690673962, - "velocityX": 1.502896539639008, - "velocityY": -2.0410087504491825, - "timestamp": 5.684541722131252 - }, - { - "x": 3.224755029145573, - "y": 6.094192875417537, - "heading": 0.18111264418965425, - "angularVelocity": -0.815346742370587, - "velocityX": 1.6564145486515685, - "velocityY": -2.243553205730433, - "timestamp": 5.742807633112106 - }, - { - "x": 3.3303033780389395, - "y": 5.951734868373249, - "heading": 0.13411173965139422, - "angularVelocity": -0.8066621416715647, - "velocityX": 1.8114940128208097, - "velocityY": -2.444963180805693, - "timestamp": 5.80107354409296 - }, - { - "x": 3.4450256229008205, - "y": 5.797681352790493, - "heading": 0.09073046456880836, - "angularVelocity": -0.7445395489798686, - "velocityX": 1.9689427819909868, - "velocityY": -2.643973345467458, - "timestamp": 5.859339455073814 - }, - { - "x": 3.5691217392378216, - "y": 5.632434411388871, - "heading": 0.05763038975845234, - "angularVelocity": -0.5680864548952619, - "velocityX": 2.1298236695857957, - "velocityY": -2.8360826874554848, - "timestamp": 5.9176053660546675 - }, - { - "x": 3.7017085484583103, - "y": 5.4576517803314095, - "heading": 0.0547360803841417, - "angularVelocity": -0.049674146093104596, - "velocityX": 2.2755468332771343, - "velocityY": -2.999740810967421, - "timestamp": 5.975871277035521 - }, - { - "x": 3.8463662629346644, - "y": 5.2914222205797, - "heading": 0.05473605711060691, - "angularVelocity": -3.9943655557036433e-7, - "velocityX": 2.4827160863217212, - "velocityY": -2.8529470655034146, - "timestamp": 6.034137188016375 - }, - { - "x": 4.001863921193704, - "y": 5.135285744872655, - "heading": 0.05473603479828713, - "angularVelocity": -3.8293951656527263e-7, - "velocityX": 2.668758724293121, - "velocityY": -2.679722552666384, - "timestamp": 6.092403098997229 + "angularVelocity": -1.618919283202532e-33, + "velocityX": 2.4430876435253525e-35, + "velocityY": 8.017390657472539e-35, + "timestamp": 5.101882612157356 + }, + { + "x": 2.6570338601157935, + "y": 6.867732812321398, + "heading": 0.5065324038818937, + "angularVelocity": -0.10327871439636148, + "velocityX": 0.1491229424075764, + "velocityY": -0.20442985508511316, + "timestamp": 5.160152273561258 + }, + { + "x": 2.674422013328184, + "y": 6.84391114835803, + "heading": 0.49465795152131636, + "angularVelocity": -0.2037844750507175, + "velocityX": 0.2984083448136593, + "velocityY": -0.40881761433700353, + "timestamp": 5.218421934965161 + }, + { + "x": 2.7005198630420675, + "y": 6.808182791545169, + "heading": 0.47711568998903314, + "angularVelocity": -0.30105308851354295, + "velocityX": 0.4478805794491053, + "velocityY": -0.6131553874185988, + "timestamp": 5.276691596369063 + }, + { + "x": 2.7353400941220496, + "y": 6.760551298473928, + "heading": 0.4541277672109497, + "angularVelocity": -0.39450929049922157, + "velocityX": 0.5975705065217742, + "velocityY": -0.8174321237441035, + "timestamp": 5.3349612577729655 + }, + { + "x": 2.7788976924779565, + "y": 6.701021131245012, + "heading": 0.42595856862842546, + "angularVelocity": -0.4834282181127908, + "velocityX": 0.7475176156247499, + "velocityY": -1.021632283329671, + "timestamp": 5.393230919176868 + }, + { + "x": 2.831210629197317, + "y": 6.629598041190472, + "heading": 0.39292676107093794, + "angularVelocity": -0.5668783164625577, + "velocityX": 0.8977731371519051, + "velocityY": -1.2257337409164428, + "timestamp": 5.45150058058077 + }, + { + "x": 2.8923008190981245, + "y": 6.546289653631685, + "heading": 0.3554225990889349, + "angularVelocity": -0.6436310264794379, + "velocityX": 1.0484047517859123, + "velocityY": -1.4297043358691544, + "timestamp": 5.509770241984673 + }, + { + "x": 2.962195513842206, + "y": 6.451106403793388, + "heading": 0.31393409063739947, + "angularVelocity": -0.7120087443781998, + "velocityX": 1.1995040482490311, + "velocityY": -1.6334958457802513, + "timestamp": 5.568039903388575 + }, + { + "x": 3.0409294178085977, + "y": 6.344063134427028, + "heading": 0.26908904007938106, + "angularVelocity": -0.7696123416123889, + "velocityX": 1.351198927013472, + "velocityY": -1.8370326304863622, + "timestamp": 5.626309564792478 + }, + { + "x": 3.128548070700225, + "y": 6.22518206352159, + "heading": 0.22172760975049532, + "angularVelocity": -0.8127974178637293, + "velocityX": 1.5036753394582005, + "velocityY": -2.0401881191895193, + "timestamp": 5.68457922619638 + }, + { + "x": 3.2251135709999605, + "y": 6.094498947078054, + "heading": 0.17303894760412505, + "angularVelocity": -0.8355748252745036, + "velocityX": 1.6572174605646188, + "velocityY": -2.2427299780874135, + "timestamp": 5.742848887600283 + }, + { + "x": 3.3307147862388833, + "y": 5.952077971116604, + "heading": 0.12484956119030122, + "angularVelocity": -0.8270064601850674, + "velocityX": 1.8122846897450882, + "velocityY": -2.444170302865565, + "timestamp": 5.801118549004185 + }, + { + "x": 3.4454853336507822, + "y": 5.798056568785059, + "heading": 0.08033941158282583, + "angularVelocity": -0.7638649090295626, + "velocityX": 1.9696450030206056, + "velocityY": -2.643252056399113, + "timestamp": 5.859388210408087 + }, + { + "x": 3.5696119500976344, + "y": 5.632835617985738, + "heading": 0.04630921837828673, + "angularVelocity": -0.5840122009402995, + "velocityX": 2.1302100176360224, + "velocityY": -2.8354541079975273, + "timestamp": 5.91765787181199 + }, + { + "x": 3.7020235307305986, + "y": 5.458025004439994, + "heading": 0.04313298905641362, + "angularVelocity": -0.05450914327194612, + "velocityX": 2.272393170695454, + "velocityY": -3.00002796196164, + "timestamp": 5.975927533215892 + }, + { + "x": 3.84656727081631, + "y": 5.291677538591243, + "heading": 0.04313296518805293, + "angularVelocity": -4.0961900430587924e-7, + "velocityX": 2.480600309032019, + "velocityY": -2.8547868966613104, + "timestamp": 6.034197194619795 + }, + { + "x": 4.001959806149131, + "y": 5.135416436419877, + "heading": 0.04313294238925072, + "angularVelocity": -3.912636809189277e-7, + "velocityX": 2.666782877897643, + "velocityY": -2.6816888652951767, + "timestamp": 6.092466856023697 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.0547360118053875, - "angularVelocity": -3.946201001676767e-7, - "velocityX": 2.842745293382521, - "velocityY": -2.4943907433712678, - "timestamp": 6.150669009978083 - }, - { - "x": 4.269399861624288, - "y": 4.9075045981351995, - "heading": 0.054735989995402, - "angularVelocity": -6.29291522756053e-7, - "velocityX": 2.940181103085103, - "velocityY": -2.3787638667331756, - "timestamp": 6.185327009826526 - }, - { - "x": 4.374514707093794, - "y": 4.829200558555379, - "heading": 0.05473596913891511, - "angularVelocity": -6.017798771949806e-7, - "velocityX": 3.0329172464990677, - "velocityY": -2.2593352161762894, - "timestamp": 6.21998500967497 - }, - { - "x": 4.482675566428291, - "y": 4.7551608256734985, - "heading": 0.0547359490445404, - "angularVelocity": -5.797903745228634e-7, - "velocityX": 3.120805003389626, - "velocityY": -2.1362956086805514, - "timestamp": 6.254643009523413 - }, - { - "x": 4.593709527898866, - "y": 4.685503714898617, - "heading": 0.05473592954464412, - "angularVelocity": -5.62637669848401e-7, - "velocityX": 3.203703674652828, - "velocityY": -2.0098422032282808, - "timestamp": 6.289301009371856 - }, - { - "x": 4.707439069036232, - "y": 4.620340494746092, - "heading": 0.05473591048922828, - "angularVelocity": -5.498129124049459e-7, - "velocityX": 3.2814802249032198, - "velocityY": -1.8801783264319567, - "timestamp": 6.3239590092202995 - }, - { - "x": 4.823682298536361, - "y": 4.559775113095544, - "heading": 0.054735891740807854, - "angularVelocity": -5.409550611531198e-7, - "velocityX": 3.3540085985473036, - "velocityY": -1.747515203283397, - "timestamp": 6.358617009068743 - }, - { - "x": 4.942252940190184, - "y": 4.503903323846341, - "heading": 0.05473587317010921, - "angularVelocity": -5.358271892357539e-7, - "velocityX": 3.4211622763091856, - "velocityY": -1.6120892577045927, - "timestamp": 6.393275008917186 - }, - { - "x": 5.062124990174475, - "y": 4.450881200757823, - "heading": 0.05473585465308408, - "angularVelocity": -5.342785275702722e-7, - "velocityX": 3.4587122888938238, - "velocityY": -1.52986679324772, - "timestamp": 6.4279330087656295 - }, - { - "x": 5.1819977334562, - "y": 4.397860645099044, - "heading": 0.05473583613607813, - "angularVelocity": -5.3427797428813e-7, - "velocityX": 3.4587322928593447, - "velocityY": -1.529821567621728, - "timestamp": 6.462591008614073 - }, - { - "x": 5.301870477238367, - "y": 4.344840090571707, - "heading": 0.05473581761907218, - "angularVelocity": -5.342779742919752e-7, - "velocityX": 3.458732307298825, - "velocityY": -1.5298215349758078, - "timestamp": 6.497249008462516 - }, - { - "x": 5.421743975168144, - "y": 4.291821241109097, - "heading": 0.0547357991020742, - "angularVelocity": -5.342777443988636e-7, - "velocityX": 3.4587540669967396, - "velocityY": -1.5297723381170152, - "timestamp": 6.531907008310959 - }, - { - "x": 5.5428966637680075, - "y": 4.241794448876403, - "heading": 0.05473578057951323, - "angularVelocity": -5.34438255269497e-7, - "velocityX": 3.4956630252656034, - "velocityY": -1.4434414118372951, - "timestamp": 6.566565008159403 + "heading": 0.04313291889737663, + "angularVelocity": -4.031578960087653e-7, + "velocityX": 2.8409167869122447, + "velocityY": -2.496473071014189, + "timestamp": 6.1507365174276 + }, + { + "x": 4.2693517733895705, + "y": 4.907420739393082, + "heading": 0.043132896624531875, + "angularVelocity": -6.4257109962904e-7, + "velocityX": 2.9384484685182977, + "velocityY": -2.3809038315668807, + "timestamp": 6.185398587932365 + }, + { + "x": 4.374422288805421, + "y": 4.829031478695207, + "heading": 0.0431328753158102, + "angularVelocity": -6.147561689330783e-7, + "velocityX": 3.0312821445967915, + "velocityY": -2.261528511030489, + "timestamp": 6.220060658437131 + }, + { + "x": 4.482542607371009, + "y": 4.7549053274480135, + "heading": 0.043132854776618594, + "angularVelocity": -5.925552428867745e-7, + "velocityX": 3.1192689008789145, + "velocityY": -2.138537893661084, + "timestamp": 6.254722728941896 + }, + { + "x": 4.593539840912748, + "y": 4.685160765838758, + "heading": 0.04313283483631883, + "angularVelocity": -5.752772258439431e-7, + "velocityX": 3.2022678370145012, + "velocityY": -2.012129125398518, + "timestamp": 6.289384799446662 + }, + { + "x": 4.707236482283354, + "y": 4.619909225059653, + "heading": 0.04313281534202855, + "angularVelocity": -5.624098616258592e-7, + "velocityX": 3.280145695710103, + "velocityY": -1.882505569600442, + "timestamp": 6.324046869951427 + }, + { + "x": 4.823450643664921, + "y": 4.559254805625301, + "heading": 0.043132796153422026, + "angularVelocity": -5.53590891933908e-7, + "velocityX": 3.352776094711051, + "velocityY": -1.7498787161607805, + "timestamp": 6.358708940456193 + }, + { + "x": 4.9419959983331205, + "y": 4.5032933115020395, + "heading": 0.04313277713835443, + "angularVelocity": -5.485842974798401e-7, + "velocityX": 3.4200309716611494, + "velocityY": -1.6144879203210547, + "timestamp": 6.3933710109609585 + }, + { + "x": 5.061736542982073, + "y": 4.449937038090927, + "heading": 0.04313275817159016, + "angularVelocity": -5.471907475389428e-7, + "velocityX": 3.454512177294491, + "velocityY": -1.5393273579481606, + "timestamp": 6.428033081465724 + }, + { + "x": 5.1814776709209625, + "y": 4.396582073700789, + "heading": 0.04313273920484137, + "angularVelocity": -5.471903010583792e-7, + "velocityX": 3.45452900519684, + "velocityY": -1.5392895927207566, + "timestamp": 6.46269515197049 + }, + { + "x": 5.3012188001688525, + "y": 4.3432271122483534, + "heading": 0.043132720238092614, + "angularVelocity": -5.471902999394187e-7, + "velocityX": 3.454529042961442, + "velocityY": -1.5392895079680768, + "timestamp": 6.497357222475255 + }, + { + "x": 5.420963133587213, + "y": 4.289879342291843, + "heading": 0.04313270127136276, + "angularVelocity": -5.471897545932471e-7, + "velocityX": 3.4546214832116693, + "velocityY": -1.5390820334629507, + "timestamp": 6.532019292980021 + }, + { + "x": 5.5425254594597755, + "y": 4.240815191837089, + "heading": 0.04313268228677855, + "angularVelocity": -5.47704852461645e-7, + "velocityX": 3.5070705270145455, + "velocityY": -1.415499701554448, + "timestamp": 6.566681363484786 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.054735761954551196, - "angularVelocity": -5.373928706734811e-7, - "velocityX": 3.550552991831721, - "velocityY": -1.3025960678451716, - "timestamp": 6.601223008007846 - }, - { - "x": 5.798180192000891, - "y": 4.154397155999577, - "heading": 0.05473574376724012, - "angularVelocity": -4.955056404583076e-7, - "velocityX": 3.6025088632415367, - "velocityY": -1.15113574961205, - "timestamp": 6.637927557031803 - }, - { - "x": 5.93207840031246, - "y": 4.117779387908136, - "heading": 0.054735725716885786, - "angularVelocity": -4.917743118523635e-7, - "velocityX": 3.6480003670436316, - "velocityY": -0.9976356899941846, - "timestamp": 6.6746321060557605 - }, - { - "x": 6.067406110251661, - "y": 4.086860105171276, - "heading": 0.0547357076680171, - "angularVelocity": -4.917338358563753e-7, - "velocityX": 3.686946537631365, - "velocityY": -0.8423828533264233, - "timestamp": 6.711336655079718 - }, - { - "x": 6.203921243902459, - "y": 4.061696453595828, - "heading": 0.05473568948478555, - "angularVelocity": -4.953944957116945e-7, - "velocityX": 3.719297397216157, - "velocityY": -0.6855731031873736, - "timestamp": 6.748041204103675 - }, - { - "x": 6.341379198553944, - "y": 4.042334078664949, - "heading": 0.054735669406532635, - "angularVelocity": -5.47023555566929e-7, - "velocityX": 3.7449841588235726, - "velocityY": -0.5275197610585458, - "timestamp": 6.784745753127632 - }, - { - "x": 6.479285910845981, - "y": 4.028828408214569, - "heading": 0.05386070036293022, - "angularVelocity": -0.023838163575618188, - "velocityX": 3.7572103719902135, - "velocityY": -0.3679563108530375, - "timestamp": 6.8214503021515895 - }, - { - "x": 6.612620100679574, - "y": 4.0185345658565055, - "heading": 0.045620785713098924, - "angularVelocity": -0.22449300887617385, - "velocityX": 3.6326339208408243, - "velocityY": -0.2804514053923919, - "timestamp": 6.858154851175547 - }, - { - "x": 6.740581673709006, - "y": 4.010579483858113, - "heading": 0.035519685699448925, - "angularVelocity": -0.27520022128747124, - "velocityX": 3.4862592357669087, - "velocityY": -0.2167328630900888, - "timestamp": 6.894859400199504 - }, - { - "x": 6.863028396494948, - "y": 4.00465723612987, - "heading": 0.025264871332947036, - "angularVelocity": -0.27938810417772464, - "velocityX": 3.3360094604627837, - "velocityY": -0.16134914842236953, - "timestamp": 6.931563949223461 - }, - { - "x": 6.979909878212025, - "y": 4.000619227079583, - "heading": 0.015648348920582984, - "angularVelocity": -0.26199810835673765, - "velocityX": 3.184386808315938, - "velocityY": -0.1100138581637957, - "timestamp": 6.968268498247419 - }, - { - "x": 7.091201711972647, - "y": 3.998378156691166, - "heading": 0.00712732553533738, - "angularVelocity": -0.23215169813648398, - "velocityX": 3.0320992007824574, - "velocityY": -0.06105702012450316, - "timestamp": 7.004973047271376 + "heading": 0.043132663161499156, + "angularVelocity": -5.517639056314665e-7, + "velocityX": 3.560845257182681, + "velocityY": -1.274191548268104, + "timestamp": 6.601343433989552 + }, + { + "x": 5.795862627864808, + "y": 4.156159506730396, + "heading": 0.04313264445875193, + "angularVelocity": -5.198107758201059e-7, + "velocityX": 3.6106505851434836, + "velocityY": -1.1253380804221886, + "timestamp": 6.637323348406788 + }, + { + "x": 5.927341530201614, + "y": 4.121094637119033, + "heading": 0.043132625947930554, + "angularVelocity": -5.1447652596799e-7, + "velocityX": 3.654230546858154, + "velocityY": -0.9745678993212189, + "timestamp": 6.6733032628240245 + }, + { + "x": 6.059882410873037, + "y": 4.090286167435021, + "heading": 0.04313260749907939, + "angularVelocity": -5.127541702089576e-7, + "velocityX": 3.683746412913316, + "velocityY": -0.8562685649205634, + "timestamp": 6.709283177241261 + }, + { + "x": 6.193269302687495, + "y": 4.06337550162166, + "heading": 0.043132589025398085, + "angularVelocity": -5.134442814470016e-7, + "velocityX": 3.7072598413563593, + "velocityY": -0.7479357927674594, + "timestamp": 6.745263091658497 + }, + { + "x": 6.327657815035056, + "y": 4.042021941101089, + "heading": 0.043132570411953335, + "angularVelocity": -5.173287667087509e-7, + "velocityX": 3.7350981658583504, + "velocityY": -0.5934855840107985, + "timestamp": 6.781243006075734 + }, + { + "x": 6.462816636912207, + "y": 4.026263156605077, + "heading": 0.043132547717955015, + "angularVelocity": -6.30740753253067e-7, + "velocityX": 3.7565075978169236, + "velocityY": -0.4379883818862704, + "timestamp": 6.81722292049297 + }, + { + "x": 6.597942834210822, + "y": 4.016131027814117, + "heading": 0.04107885634805962, + "angularVelocity": -0.0570788286508975, + "velocityX": 3.755600853621886, + "velocityY": -0.2816051387300244, + "timestamp": 6.853202834910206 + }, + { + "x": 6.728400824069692, + "y": 4.008818936530008, + "heading": 0.03307687475358465, + "angularVelocity": -0.2224013515341097, + "velocityX": 3.625856035843536, + "velocityY": -0.20322703381989088, + "timestamp": 6.889182749327443 + }, + { + "x": 6.853599659736003, + "y": 4.0035443033890195, + "heading": 0.023873611715651655, + "angularVelocity": -0.25578890853403846, + "velocityX": 3.47968686680181, + "velocityY": -0.14659937986014848, + "timestamp": 6.925162663744679 + }, + { + "x": 6.973435503233387, + "y": 4.000040041798795, + "heading": 0.014893418694113961, + "angularVelocity": -0.24958906009058449, + "velocityX": 3.3306316993343295, + "velocityY": -0.09739493956513012, + "timestamp": 6.961142578161915 + }, + { + "x": 7.08787150697828, + "y": 3.998176312998215, + "heading": 0.006805585628585594, + "angularVelocity": -0.22478744589936686, + "velocityX": 3.1805524164913392, + "velocityY": -0.05179914490532842, + "timestamp": 6.997122492579152 }, { "x": 7.196889877319336, "y": 3.9978766441345215, - "heading": -3.597785008712915e-31, - "angularVelocity": -0.19418098641357107, - "velocityX": 2.87942961178202, - "velocityY": -0.013663498666533047, - "timestamp": 7.041677596295333 - }, - { - "x": 7.37119660747943, - "y": 4.00262802828422, - "heading": -0.0076676970964569885, - "angularVelocity": -0.11437965193876058, - "velocityX": 2.6001474595947878, - "velocityY": 0.07087677805125822, - "timestamp": 7.10871484432405 - }, - { - "x": 7.526209738417895, - "y": 4.010745052233057, - "heading": -0.011651875074249012, - "angularVelocity": -0.059432302114867865, - "velocityX": 2.3123432941647923, - "velocityY": 0.1210822965966538, - "timestamp": 7.1757520923527665 - }, - { - "x": 7.661711723306385, - "y": 4.020683594487604, - "heading": -0.013062756224948863, - "angularVelocity": -0.021046227167551262, - "velocityX": 2.0212939652660666, - "velocityY": 0.1482540311065431, - "timestamp": 7.242789340381483 - }, - { - "x": 7.777626027630173, - "y": 4.031355931329743, - "heading": -0.012680116865926582, - "angularVelocity": 0.005707861976350362, - "velocityX": 1.729102964879078, - "velocityY": 0.15920010376271715, - "timestamp": 7.3098265884102 - }, - { - "x": 7.873936260933145, - "y": 4.041961506483741, - "heading": -0.011077902998110952, - "angularVelocity": 0.023900352638719452, - "velocityX": 1.4366674667449708, - "velocityY": 0.15820421431164344, - "timestamp": 7.3768638364389165 - }, - { - "x": 7.950652622834928, - "y": 4.051889363613367, - "heading": -0.008694806000859931, - "angularVelocity": 0.035548848846393165, - "velocityX": 1.1443841171541105, - "velocityY": 0.14809464024199548, - "timestamp": 7.443901084467633 - }, - { - "x": 8.007796949441587, - "y": 4.060659248403223, - "heading": -0.005876390393834335, - "angularVelocity": 0.04204253142698025, - "velocityX": 0.8524264985069137, - "velocityY": 0.1308210740706325, - "timestamp": 7.51093833249635 - }, - { - "x": 8.045395699194648, - "y": 4.067884383860418, - "heading": -0.00290156854092843, - "angularVelocity": 0.04437565592835165, - "velocityX": 0.5608635625518943, - "velocityY": 0.1077779245069823, - "timestamp": 7.577975580525067 - }, - { - "x": 8.0634765625, + "heading": 3.395025904591232e-32, + "angularVelocity": -0.18914957800247972, + "velocityX": 3.0299785896330556, + "velocityY": -0.008328782003720711, + "timestamp": 7.033102406996388 + }, + { + "x": 7.373371840772393, + "y": 4.002118083585038, + "heading": -0.0072355781339062996, + "angularVelocity": -0.11324248534202788, + "velocityX": 2.7620814521804613, + "velocityY": 0.06638186139590158, + "timestamp": 7.096996961341894 + }, + { + "x": 7.532286444106961, + "y": 4.009181217967762, + "heading": -0.011195509678912387, + "angularVelocity": -0.06197604139459259, + "velocityX": 2.4871384574536473, + "velocityY": 0.11054360508613674, + "timestamp": 7.1608915156874 + }, + { + "x": 7.6734693827744955, + "y": 4.017827175121804, + "heading": -0.012870706885804134, + "angularVelocity": -0.02621815308129755, + "velocityX": 2.209623967390062, + "velocityY": 0.13531602563950743, + "timestamp": 7.224786070032906 + }, + { + "x": 7.7968608797168075, + "y": 4.027211909710403, + "heading": -0.012933711131137412, + "angularVelocity": -0.0009860659641287687, + "velocityX": 1.9311739193778674, + "velocityY": 0.14687847320841513, + "timestamp": 7.2886806243784115 + }, + { + "x": 7.9024437482359025, + "y": 4.036727334915007, + "heading": -0.011868630755415614, + "angularVelocity": 0.01666934508944925, + "velocityX": 1.6524548860324264, + "velocityY": 0.14892388407859264, + "timestamp": 7.352575178723917 + }, + { + "x": 7.99021929218393, + "y": 4.0459160326905685, + "heading": -0.010039813807728299, + "angularVelocity": 0.028622422777974377, + "velocityX": 1.3737562589980292, + "velocityY": 0.143810342989083, + "timestamp": 7.416469733069423 + }, + { + "x": 8.060196978947376, + "y": 4.0544220986291215, + "heading": -0.00773105502412215, + "angularVelocity": 0.03613388976972427, + "velocityX": 1.0952058040039734, + "velocityY": 0.13312661815523763, + "timestamp": 7.480364287414929 + }, + { + "x": 8.112389704520517, + "y": 4.061961050008052, + "heading": -0.005169479155646227, + "angularVelocity": 0.04009067587551167, + "velocityX": 0.816857181457298, + "velocityY": 0.11799051509403427, + "timestamp": 7.544258841760435 + }, + { + "x": 8.146811527228987, + "y": 4.068300458431988, + "heading": -0.0025408789062790695, + "angularVelocity": 0.04113966012116123, + "velocityX": 0.5387285827574291, + "velocityY": 0.09921672494429698, + "timestamp": 7.608153396105941 + }, + { + "x": 8.1634765625, "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 0.043282930404384794, - "velocityX": 0.26971368660010536, - "velocityY": 0.07999391635030323, - "timestamp": 7.645012828553783 - }, - { - "x": 8.059946431342839, - "y": 4.076566093038108, - "heading": 0.0028549561404960684, - "angularVelocity": 0.03895223157485018, - "velocityX": -0.048164132671898556, - "velocityY": 0.045285389048645555, - "timestamp": 7.718306604675698 - }, - { - "x": 8.033099887642729, - "y": 4.077511417867968, - "heading": 0.00539411397159862, - "angularVelocity": 0.03464356682727019, - "velocityX": -0.3662868134321987, - "velocityY": 0.012897750394093884, - "timestamp": 7.791600380797613 - }, - { - "x": 7.9829172655770035, - "y": 4.07628474252501, - "heading": 0.007619394466085314, - "angularVelocity": 0.03036111130070867, - "velocityX": -0.6846778092351761, - "velocityY": -0.01673641894119354, - "timestamp": 7.864894156919528 - }, - { - "x": 7.909377202284024, - "y": 4.0731292983210805, - "heading": 0.00953312535115696, - "angularVelocity": 0.026110414639960534, - "velocityX": -1.0033602740109082, - "velocityY": -0.04305200756310189, - "timestamp": 7.938187933041442 - }, - { - "x": 7.81245694795258, - "y": 4.068343833996595, - "heading": 0.011138189892816102, - "angularVelocity": 0.021899056462711293, - "velocityX": -1.3223531309156478, - "velocityY": -0.06529155103873027, - "timestamp": 8.011481709163357 - }, - { - "x": 7.692133347423482, - "y": 4.062303858835979, - "heading": 0.012438255979270885, - "angularVelocity": 0.01773774193721827, - "velocityX": -1.641661910404963, - "velocityY": -0.0824077497462929, - "timestamp": 8.084775485285272 - }, - { - "x": 7.548385409224001, - "y": 4.055495143379703, - "heading": 0.013438147372137587, - "angularVelocity": 0.013642241480415802, - "velocityX": -1.9612570917396126, - "velocityY": -0.0928962296191558, - "timestamp": 8.158069261407187 - }, - { - "x": 7.381200809360299, - "y": 4.048569614878887, - "heading": 0.014144485416621113, - "angularVelocity": 0.009637080825370731, - "velocityX": -2.2810204182359235, - "velocityY": -0.09448999447506434, - "timestamp": 8.231363037529102 - }, - { - "x": 7.190592956257051, - "y": 4.04244548860493, - "heading": 0.014566894857523213, - "angularVelocity": 0.00576323752509999, - "velocityX": -2.6006008038962976, - "velocityY": -0.08355588425094046, - "timestamp": 8.304656813651016 - }, - { - "x": 6.9766499613051005, - "y": 4.038503705706744, - "heading": 0.01472053802802316, - "angularVelocity": 0.002096264903098756, - "velocityX": -2.918979022121646, - "velocityY": -0.05378059511668448, - "timestamp": 8.377950589772931 - }, - { - "x": 6.739697889009689, - "y": 4.039020009641981, - "heading": 0.014632337527092197, - "angularVelocity": -0.0012033832283965633, - "velocityX": -3.232908506464046, - "velocityY": 0.007044308023879432, - "timestamp": 8.451244365894846 - }, - { - "x": 6.480981935824527, - "y": 4.048232688232657, - "heading": 0.01435998143770215, - "angularVelocity": -0.003715951118919239, - "velocityX": -3.529848874955213, - "velocityY": 0.12569523741486752, - "timestamp": 8.52453814201676 - }, - { - "x": 6.206211729235204, - "y": 4.074520375352005, - "heading": 0.014063196474504363, - "angularVelocity": -0.004049251913342814, - "velocityX": -3.7488886659663403, - "velocityY": 0.3586619288876869, - "timestamp": 8.597831918138676 - }, - { - "x": 5.93349789020665, - "y": 4.124154607710869, - "heading": 0.014063194566661911, - "angularVelocity": -2.6030074482550523e-8, - "velocityX": -3.7208321559927344, - "velocityY": 0.6771957318218086, - "timestamp": 8.67112569426059 + "heading": 2.9433151473989685e-31, + "angularVelocity": 0.03976675214822599, + "velocityX": 0.2608209015888442, + "velocityY": 0.07741657313776515, + "timestamp": 7.6720479504514465 + }, + { + "x": 8.160341731543628, + "y": 4.076804852758406, + "heading": 0.0025302388406713986, + "angularVelocity": 0.03600436134361471, + "velocityX": -0.0446074831711938, + "velocityY": 0.05062755463137832, + "timestamp": 7.7423238488772945 + }, + { + "x": 8.135735418270036, + "y": 4.078564432167465, + "heading": 0.004797198596772283, + "angularVelocity": 0.032257997505259615, + "velocityX": -0.3501387221617275, + "velocityY": 0.025038163132351486, + "timestamp": 7.8125997473031425 + }, + { + "x": 8.089649517071235, + "y": 4.078625238037987, + "heading": 0.006802207301300417, + "angularVelocity": 0.028530531084475955, + "velocityX": -0.6557853009510525, + "velocityY": 0.0008652450112215468, + "timestamp": 7.882875645728991 + }, + { + "x": 8.02207492391753, + "y": 4.077106587290107, + "heading": 0.008546856784408466, + "angularVelocity": 0.024825715817051033, + "velocityX": -0.9615614267102518, + "velocityY": -0.02160983753886332, + "timestamp": 7.953151544154839 + }, + { + "x": 7.933001430575272, + "y": 4.074154095631599, + "heading": 0.010033092526327095, + "angularVelocity": 0.021148584012580587, + "velocityX": -1.2674828118525576, + "velocityY": -0.042012862512522016, + "timestamp": 8.023427442580687 + }, + { + "x": 7.822417690835061, + "y": 4.069949420479082, + "heading": 0.011263348791088614, + "angularVelocity": 0.017506090883485915, + "velocityX": -1.5735656493512467, + "velocityY": -0.059830969744960986, + "timestamp": 8.093703341006535 + }, + { + "x": 7.690311409008121, + "y": 4.0647254734139295, + "heading": 0.012240761619989172, + "angularVelocity": 0.013908222460249003, + "velocityX": -1.879823449946105, + "velocityY": -0.07433483145952353, + "timestamp": 8.163979239432383 + }, + { + "x": 7.536670161450006, + "y": 4.058791543485583, + "heading": 0.012969524954057786, + "angularVelocity": 0.01037003226415625, + "velocityX": -2.186258034398947, + "velocityY": -0.08443762458060709, + "timestamp": 8.23425513785823 + }, + { + "x": 7.361484060419744, + "y": 4.0525779000493936, + "heading": 0.01345553350071711, + "angularVelocity": 0.006915721570918652, + "velocityX": -2.492833317742797, + "velocityY": -0.08841784417378167, + "timestamp": 8.304531036284079 + }, + { + "x": 7.164754272980972, + "y": 4.046723019235429, + "heading": 0.013707670109403618, + "angularVelocity": 0.0035878105343975153, + "velocityX": -2.799391994203425, + "velocityY": -0.08331278496768466, + "timestamp": 8.374806934709927 + }, + { + "x": 6.9465233889455575, + "y": 4.0422689491096495, + "heading": 0.013740797003253829, + "angularVelocity": 0.0004713834272096328, + "velocityX": -3.1053446334191217, + "velocityY": -0.06337976782296549, + "timestamp": 8.445082833135775 + }, + { + "x": 6.707012866607414, + "y": 4.041196732301159, + "heading": 0.013584512522768889, + "angularVelocity": -0.002223870259728453, + "velocityX": -3.408146003154458, + "velocityY": -0.015257247968487717, + "timestamp": 8.515358731561623 + }, + { + "x": 6.4476145619873, + "y": 4.048415292748879, + "heading": 0.013321412464940579, + "angularVelocity": -0.0037438163541362824, + "velocityX": -3.691141777345175, + "velocityY": 0.10271744096358496, + "timestamp": 8.58563462998747 + }, + { + "x": 6.1833350728544865, + "y": 4.076620284873478, + "heading": 0.013321410992019302, + "angularVelocity": -2.0959124115556836e-8, + "velocityX": -3.76059922466405, + "velocityY": 0.4013465890352113, + "timestamp": 8.655910528413319 + }, + { + "x": 5.922208724930001, + "y": 4.126140027657945, + "heading": 0.013321410307177713, + "angularVelocity": -9.745042100450887e-9, + "velocityX": -3.7157311933907815, + "velocityY": 0.7046475946048248, + "timestamp": 8.726186426839167 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.01406319364211473, - "angularVelocity": -1.2614265905768763e-8, - "velocityX": -3.6503257922039465, - "velocityY": 0.9890944453863133, - "timestamp": 8.744419470382505 - }, - { - "x": 5.5408846553597755, - "y": 4.23596798413579, - "heading": 0.01406319265930284, - "angularVelocity": -2.835157538408451e-8, - "velocityX": -3.6078608681964144, - "velocityY": 1.1342486182256648, - "timestamp": 8.779084630162778 - }, - { - "x": 5.417489605980159, - "y": 4.2802558166388724, - "heading": 0.014063191785023112, - "angularVelocity": -2.5220703836459854e-8, - "velocityX": -3.5596273076991327, - "velocityY": 1.2775891639848806, - "timestamp": 8.813749789943051 - }, - { - "x": 5.295963908009713, - "y": 4.329441746794644, - "heading": 0.014063190996401449, - "angularVelocity": -2.2749690676604448e-8, - "velocityX": -3.5057013653114657, - "velocityY": 1.4188865843266973, - "timestamp": 8.848414949723324 - }, - { - "x": 5.176501898681268, - "y": 4.383447115848144, - "heading": 0.014063190276173353, - "angularVelocity": -2.077671352365433e-8, - "velocityX": -3.446169297521196, - "velocityY": 1.5579149034885322, - "timestamp": 8.883080109503597 - }, - { - "x": 5.05929461402676, - "y": 4.442185556946357, - "heading": 0.01406318961105745, - "angularVelocity": -1.9186869677954374e-8, - "velocityX": -3.381126335416613, - "velocityY": 1.6944517628226294, - "timestamp": 8.91774526928387 - }, - { - "x": 4.944529482696556, - "y": 4.505563132440428, - "heading": 0.014063188990615918, - "angularVelocity": -1.7898129854823097e-8, - "velocityX": -3.310676542604923, - "velocityY": 1.828278764494154, - "timestamp": 8.952410429064143 - }, - { - "x": 4.832390024623587, - "y": 4.573478482189006, - "heading": 0.014063188406478968, - "angularVelocity": -1.685083682180617e-8, - "velocityX": -3.2349326754519327, - "velocityY": 1.9591817888352978, - "timestamp": 8.987075588844416 - }, - { - "x": 4.723055552753515, - "y": 4.64582297983034, - "heading": 0.014063187851803343, - "angularVelocity": -1.600095394099115e-8, - "velocityX": -3.1540160946348355, - "velocityY": 2.086951224223198, - "timestamp": 9.021740748624689 - }, - { - "x": 4.616700862639107, - "y": 4.722480877082754, - "heading": 0.014063187320883878, - "angularVelocity": -1.5315650301874817e-8, - "velocityX": -3.068057115228709, - "velocityY": 2.2113816217294997, - "timestamp": 9.056405908404962 - }, - { - "x": 4.508256477944758, - "y": 4.796152911545187, - "heading": 0.014063186782201262, - "angularVelocity": -1.5539597107094908e-8, - "velocityX": -3.128339386915466, - "velocityY": 2.125247220246655, - "timestamp": 9.091071068185235 - }, - { - "x": 4.396953351691581, - "y": 4.865430375769311, - "heading": 0.014063186216514915, - "angularVelocity": -1.6318584724554417e-8, - "velocityX": -3.2108066704055713, - "velocityY": 1.998475260556751, - "timestamp": 9.125736227965508 - }, - { - "x": 4.28296944450211, - "y": 4.930202424249945, - "heading": 0.014063179833248412, - "angularVelocity": -1.8414069176204727e-7, - "velocityX": -3.288140251248267, - "velocityY": 1.8685056953781265, - "timestamp": 9.16040138774578 + "heading": 0.013321409576784942, + "angularVelocity": -1.0393218545701755e-8, + "velocityX": -3.646442120972535, + "velocityY": 1.003317616364548, + "timestamp": 8.796462325265015 + }, + { + "x": 5.541094088534591, + "y": 4.236435199254689, + "heading": 0.013321408817775413, + "angularVelocity": -2.1905263169095435e-8, + "velocityX": -3.6034323243164974, + "velocityY": 1.148240568022246, + "timestamp": 8.831111967342002 + }, + { + "x": 5.41792618310833, + "y": 4.281179290629024, + "heading": 0.01332140812506875, + "angularVelocity": -1.9991740821363056e-8, + "velocityX": -3.5546660237528647, + "velocityY": 1.2913291073806714, + "timestamp": 8.865761609418989 + }, + { + "x": 5.2966447994258585, + "y": 4.330809856867523, + "heading": 0.01332140748575771, + "angularVelocity": -1.8450725633992864e-8, + "velocityX": -3.500220389376678, + "velocityY": 1.432354369728484, + "timestamp": 8.900411251495976 + }, + { + "x": 5.177443709175996, + "y": 4.385247597760623, + "heading": 0.013321406889674104, + "angularVelocity": -1.7203167808757175e-8, + "velocityX": -3.4401824407021615, + "velocityY": 1.5710910020988935, + "timestamp": 8.935060893572963 + }, + { + "x": 5.0605133585968645, + "y": 4.444405530588187, + "heading": 0.01332140632867156, + "angularVelocity": -1.6190716838073352e-8, + "velocityX": -3.3746481513237594, + "velocityY": 1.7073172847247369, + "timestamp": 8.96971053564995 + }, + { + "x": 4.946040562745207, + "y": 4.508189127302759, + "heading": 0.013321405796093607, + "angularVelocity": -1.5370373829355437e-8, + "velocityX": -3.3037223183234317, + "velocityY": 1.840815456992452, + "timestamp": 9.004360177726937 + }, + { + "x": 4.834208203167093, + "y": 4.576496460724576, + "heading": 0.013321405286393582, + "angularVelocity": -1.4710109443101504e-8, + "velocityX": -3.2275184641053225, + "velocityY": 1.9713719775242866, + "timestamp": 9.039009819803924 + }, + { + "x": 4.725194921038476, + "y": 4.649218348571895, + "heading": 0.013321404794856252, + "angularVelocity": -1.4185928002634242e-8, + "velocityX": -3.146158967137548, + "velocityY": 2.098777461704854, + "timestamp": 9.073659461880911 + }, + { + "x": 4.619174674827694, + "y": 4.72623832110653, + "heading": 0.013321404313366907, + "angularVelocity": -1.3895939972772152e-8, + "velocityX": -3.05977897189302, + "velocityY": 2.2228215911583726, + "timestamp": 9.108309103957899 + }, + { + "x": 4.510161328966249, + "y": 4.798960113344169, + "heading": 0.013321403820821752, + "angularVelocity": -1.421501423878401e-8, + "velocityX": -3.146160806487812, + "velocityY": 2.0987747023782037, + "timestamp": 9.142958746034886 + }, + { + "x": 4.398328904529435, + "y": 4.867267340500532, + "heading": 0.013321403310039242, + "angularVelocity": -1.4741350266093223e-8, + "velocityX": -3.2275203359486127, + "velocityY": 1.9713689106685317, + "timestamp": 9.177608388111873 + }, + { + "x": 4.283856051297854, + "y": 4.931050823285602, + "heading": 0.013321399241018467, + "angularVelocity": -1.1743327000811157e-7, + "velocityX": -3.3037239743267564, + "velocityY": 1.8408121689497896, + "timestamp": 9.21225803018886 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.013676554706226966, - "angularVelocity": -0.011153132697846535, - "velocityX": -3.331021112692202, - "velocityY": 1.7234990981340637, - "timestamp": 9.195066547526054 - }, - { - "x": 3.942209538282738, - "y": 5.088292732030669, - "heading": 0.012204358572730108, - "angularVelocity": -0.02138935040268071, - "velocityX": -3.2732028891433576, - "velocityY": 1.4288410708686299, - "timestamp": 9.263895011494382 - }, - { - "x": 3.7316526106102703, - "y": 5.172141912516415, - "heading": 0.010528895655226616, - "angularVelocity": -0.024342587657839698, - "velocityX": -3.05915482538382, - "velocityY": 1.2182340800795661, - "timestamp": 9.33272347546271 - }, - { - "x": 3.53847572791013, - "y": 5.244805689896845, - "heading": 0.00883705460181792, - "angularVelocity": -0.024580543511579803, - "velocityX": -2.806642362221417, - "velocityY": 1.0557227808231526, - "timestamp": 9.40155193943104 - }, - { - "x": 3.3636567049982276, - "y": 5.307973779066906, - "heading": 0.0072129294254924, - "angularVelocity": -0.0235967081449452, - "velocityX": -2.5399233519484916, - "velocityY": 0.9177611343924164, - "timestamp": 9.470380403399368 - }, - { - "x": 3.2076759026568156, - "y": 5.362638919945403, - "heading": 0.005703178674060931, - "angularVelocity": -0.02193497667078823, - "velocityX": -2.2662252409582635, - "velocityY": 0.7942228799941261, - "timestamp": 9.539208867367696 - }, - { - "x": 3.070812323282922, - "y": 5.409448485662979, - "heading": 0.00433725990535669, - "angularVelocity": -0.01984526008502491, - "velocityX": -1.9884735396226798, - "velocityY": 0.6800902274837387, - "timestamp": 9.608037331336025 - }, - { - "x": 2.953246241272587, - "y": 5.448856394346129, - "heading": 0.0031354038045633293, - "angularVelocity": -0.01746161444698809, - "velocityX": -1.7081026545127194, - "velocityY": 0.572552493707886, - "timestamp": 9.676865795304353 - }, - { - "x": 2.855102952665587, - "y": 5.48119799246298, - "heading": 0.002112343078266857, - "angularVelocity": -0.014863919188538589, - "velocityX": -1.4259113591749097, - "velocityY": 0.4698869661210702, - "timestamp": 9.745694259272682 - }, - { - "x": 2.7764742584987756, - "y": 5.506730918230691, - "heading": 0.0012792785012801142, - "angularVelocity": -0.012103489297248894, - "velocityX": -1.1423862982470774, - "velocityY": 0.3709646314271869, - "timestamp": 9.81452272324101 - }, - { - "x": 2.717430148167725, - "y": 5.5256592019268735, - "heading": 0.0006450106297669135, - "angularVelocity": -0.009215197244632127, - "velocityX": -0.8578443702916283, - "velocityY": 0.275006626689982, - "timestamp": 9.883351187209339 - }, - { - "x": 2.6780256614742735, - "y": 5.538148367640626, - "heading": 0.00021663624249770164, - "angularVelocity": -0.006223796995765153, - "velocityX": -0.5725027760547413, - "velocityY": 0.18145350039337144, - "timestamp": 9.952179651177667 + "heading": 0.012938020716139158, + "angularVelocity": -0.011064429584221965, + "velocityX": -3.358100659168625, + "velocityY": 1.6997858867187727, + "timestamp": 9.246907672265847 + }, + { + "x": 3.941212684187486, + "y": 5.0868100262217055, + "heading": 0.011498233293039487, + "angularVelocity": -0.020924944231025543, + "velocityX": -3.2887007006223063, + "velocityY": 1.4077333471091193, + "timestamp": 9.315714900959192 + }, + { + "x": 3.7303193403637978, + "y": 5.1698989262671065, + "heading": 0.009891075368403834, + "angularVelocity": -0.023357399435433027, + "velocityX": -3.0649881971497734, + "velocityY": 1.2075606244178938, + "timestamp": 9.384522129652536 + }, + { + "x": 3.5371027460588107, + "y": 5.242302097421373, + "heading": 0.008284033873001792, + "angularVelocity": -0.023355707327847468, + "velocityX": -2.808085690619801, + "velocityY": 1.0522611145545084, + "timestamp": 9.453329358345881 + }, + { + "x": 3.362389747323273, + "y": 5.305525449861525, + "heading": 0.006750320632579214, + "angularVelocity": -0.022290001640058982, + "velocityX": -2.5391663354759597, + "velocityY": 0.9188475344926457, + "timestamp": 9.522136587039226 + }, + { + "x": 3.2065870657576463, + "y": 5.360439069495437, + "heading": 0.00533021576404821, + "angularVelocity": -0.0206388906441795, + "velocityX": -2.2643359502240057, + "velocityY": 0.7980792233131078, + "timestamp": 9.59094381573257 + }, + { + "x": 3.0699321768781336, + "y": 5.407605853881433, + "heading": 0.00404906921437406, + "angularVelocity": -0.018619359826041904, + "velocityX": -1.9860542485811465, + "velocityY": 0.6854917031494734, + "timestamp": 9.659751044425915 + }, + { + "x": 2.952579395626419, + "y": 5.447418742231964, + "heading": 0.002924266712171353, + "angularVelocity": -0.01634715601198879, + "velocityX": -1.705529832842474, + "velocityY": 0.578614908732415, + "timestamp": 9.72855827311926 + }, + { + "x": 2.8546365398725873, + "y": 5.480167232965782, + "heading": 0.001968461119345437, + "angularVelocity": -0.013891063642247188, + "velocityX": -1.4234384615362952, + "velocityY": 0.47594549810702536, + "timestamp": 9.797365501812605 + }, + { + "x": 2.776182973714466, + "y": 5.506073331672824, + "heading": 0.0011912691725219405, + "angularVelocity": -0.01129520780857525, + "velocityX": -1.140193663485079, + "velocityY": 0.376502573915577, + "timestamp": 9.86617273050595 + }, + { + "x": 2.7172794523584742, + "y": 5.525312622061272, + "heading": 0.0006002459449936435, + "angularVelocity": -0.008589551399640344, + "velocityX": -0.8560658883459488, + "velocityY": 0.27961147039059914, + "timestamp": 9.934979959199294 + }, + { + "x": 2.6779739252366173, + "y": 5.5380274141109735, + "heading": 0.00020148371013726726, + "angularVelocity": -0.005795353808442843, + "velocityX": -0.5712412470066293, + "velocityY": 0.18478860856854942, + "timestamp": 10.00378718789264 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -3.0114656219279604e-34, - "angularVelocity": -0.003147480417366101, - "velocityX": -0.28651653960332735, - "velocityY": 0.08989010211866776, - "timestamp": 10.021008115145996 + "heading": -3.5000485154255244e-36, + "angularVelocity": -0.0029282346341141605, + "velocityX": -0.285853063090511, + "velocityY": 0.09167570478022308, + "timestamp": 10.072594416585984 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -4.388516089708894e-34, - "angularVelocity": -2.000698896485297e-33, - "velocityX": 9.851967688482278e-36, - "velocityY": -1.337407020908214e-34, - "timestamp": 10.089836579114325 + "heading": 7.813037085898689e-36, + "angularVelocity": 1.6441710918960667e-34, + "velocityX": -8.361910991338164e-36, + "velocityY": -1.503965302427871e-35, + "timestamp": 10.141401645279329 }, { "x": 2.6750735555579293, "y": 5.538716972759925, - "heading": 4.633169711684461e-19, - "angularVelocity": 7.277213797999712e-18, + "heading": 1.350818917618456e-19, + "angularVelocity": 2.121700407628309e-18, "velocityX": 0.2633772294028884, - "velocityY": -0.0882468077495622, - "timestamp": 10.15350338677302 + "velocityY": -0.088246807749562, + "timestamp": 10.205068452938024 }, { "x": 2.7085920650608806, "y": 5.52742596598787, - "heading": -4.184429803904338e-20, - "angularVelocity": -7.934452625857426e-18, + "heading": 3.9875003320592027e-19, + "angularVelocity": 4.14137524936932e-18, "velocityX": 0.5264675697678677, - "velocityY": -0.1773452633683705, - "timestamp": 10.217170194431715 + "velocityY": -0.17734526336837014, + "timestamp": 10.268735260596719 }, { "x": 2.7588390755328, "y": 5.51039889888726, - "heading": 3.955178903113731e-20, - "angularVelocity": 1.278469740567629e-18, - "velocityX": 0.7892183120172118, - "velocityY": -0.2674402522565472, - "timestamp": 10.28083700209041 + "heading": 7.83389944204561e-19, + "angularVelocity": 6.041451191657363e-18, + "velocityX": 0.7892183120172117, + "velocityY": -0.2674402522565467, + "timestamp": 10.332402068255414 }, { "x": 2.8257885982701296, "y": 5.487560532002736, - "heading": 8.970064045939625e-19, - "angularVelocity": 1.3467843717867433e-17, - "velocityX": 1.0515608556382, - "velocityY": -0.3587170100777762, - "timestamp": 10.344503809749105 + "heading": 1.2799519248933233e-18, + "angularVelocity": 7.799385566035206e-18, + "velocityX": 1.0515608556381997, + "velocityY": -0.35871701007777557, + "timestamp": 10.396068875914109 }, { "x": 2.9094088162269203, "y": 5.4588202139112365, - "heading": 2.18904914594727e-18, - "angularVelocity": 2.0293820106069983e-17, + "heading": 9.407556731948414e-19, + "angularVelocity": -5.3276780189270054e-18, "velocityX": 1.313403656188678, - "velocityY": -0.4514176090871547, - "timestamp": 10.4081706174078 + "velocityY": -0.45141760908715395, + "timestamp": 10.459735683572804 }, { "x": 3.0096598962036025, - "y": 5.424066617313393, - "heading": 3.904306808877207e-18, - "angularVelocity": 2.694116017446798e-17, + "y": 5.424066617313392, + "heading": 6.890803629951045e-19, + "angularVelocity": -3.9530065893820216e-18, "velocityX": 1.5746208057754196, - "velocityY": -0.5458668005493709, - "timestamp": 10.471837425066495 + "velocityY": -0.5458668005493699, + "timestamp": 10.523402491231499 }, { "x": 3.126490562172417, - "y": 5.383159777949491, - "heading": 4.8450172295989666e-18, - "angularVelocity": 1.4775523625508697e-17, - "velocityX": 1.8350325745107399, - "velocityY": -0.6425143786570184, - "timestamp": 10.53550423272519 - }, - { - "x": 3.2598324200617776, - "y": 5.335918462965805, - "heading": 4.235614277481238e-18, - "angularVelocity": -9.5717529200556e-18, + "y": 5.38315977794949, + "heading": 5.07923477935949e-19, + "angularVelocity": -2.8453897991917994e-18, + "velocityX": 1.8350325745107396, + "velocityY": -0.6425143786570174, + "timestamp": 10.587069298890194 + }, + { + "x": 3.259832420061777, + "y": 5.335918462965804, + "heading": 3.751534220684201e-19, + "angularVelocity": -2.0853889294918685e-18, "velocityX": 2.0943700931917317, - "velocityY": -0.7420085397863493, - "timestamp": 10.599171040383885 + "velocityY": -0.7420085397863481, + "timestamp": 10.650736106548889 }, { - "x": 3.4095898607657253, - "y": 5.282098877479993, - "heading": 2.3874123899446895e-18, - "angularVelocity": -2.902928473254694e-17, + "x": 3.409589860765725, + "y": 5.282098877479992, + "heading": 4.172337353071023e-18, + "angularVelocity": 5.964150034596624e-17, "velocityX": 2.3522059014921575, - "velocityY": -0.8453319314253612, - "timestamp": 10.66283784804258 + "velocityY": -0.8453319314253599, + "timestamp": 10.714402914207584 }, { - "x": 3.5756203018815462, - "y": 5.22135582426949, - "heading": 2.5233707069051096e-18, - "angularVelocity": 2.135466217958737e-18, - "velocityX": 2.607802200573299, - "velocityY": -0.9540772569615117, - "timestamp": 10.726504655701275 + "x": 3.575620301881546, + "y": 5.221355824269489, + "heading": 4.9261514224901164e-18, + "angularVelocity": 1.1839985341500765e-17, + "velocityX": 2.6078022005732984, + "velocityY": -0.9540772569615101, + "timestamp": 10.778069721866279 }, { - "x": 3.757690030648967, + "x": 3.7576900306489667, "y": 5.153163870724839, - "heading": 3.693577308512614e-18, - "angularVelocity": 1.8380167698697197e-17, + "heading": 5.589206376297959e-18, + "angularVelocity": 1.041445265109494e-17, "velocityX": 2.8597276267323055, - "velocityY": -1.0710754324327665, - "timestamp": 10.79017146335997 + "velocityY": -1.071075432432765, + "timestamp": 10.841736529524974 }, { - "x": 3.955353841452516, + "x": 3.9553538414525153, "y": 5.076631094268888, - "heading": 4.677490606421779e-18, - "angularVelocity": 1.5454101345613767e-17, - "velocityX": 3.1046603100187773, - "velocityY": -1.2020828320186725, - "timestamp": 10.853838271018665 + "heading": 6.855394728324636e-18, + "angularVelocity": 1.9887731120656582e-17, + "velocityX": 3.104660310018777, + "velocityY": -1.2020828320186703, + "timestamp": 10.90540333718367 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 5.777516683198087e-18, - "angularVelocity": 1.7277858231456074e-17, + "heading": 8.923179883172485e-18, + "angularVelocity": 3.2478228937327026e-17, "velocityX": 3.332116557247906, - "velocityY": -1.36151476081006, - "timestamp": 10.91750507867736 + "velocityY": -1.3615147608100575, + "timestamp": 10.969070144842364 }, { "x": 4.285701077560922, "y": 4.938388492372474, - "heading": 6.5114072312405955e-18, - "angularVelocity": 2.119278869815072e-17, - "velocityX": 3.413356765694629, - "velocityY": -1.4888942599373958, - "timestamp": 10.952134336699762 + "heading": 1.0772342870449973e-17, + "angularVelocity": 5.3398862490245696e-17, + "velocityX": 3.4133567656946298, + "velocityY": -1.4888942599373933, + "timestamp": 11.003699402864767 }, { "x": 4.403715149774036, "y": 4.881600772971698, - "heading": 6.4908057437571035e-18, - "angularVelocity": -5.949156482118572e-19, - "velocityX": 3.40792956455403, - "velocityY": -1.6398768741750078, - "timestamp": 10.986763594722165 + "heading": 9.945107603157448e-18, + "angularVelocity": -2.3888333580736016e-17, + "velocityX": 3.4079295645540313, + "velocityY": -1.6398768741750054, + "timestamp": 11.038328660887169 }, { "x": 4.519367006147829, "y": 4.8201449517293415, - "heading": 6.136184222547225e-18, - "angularVelocity": -1.0240517454358069e-17, - "velocityX": 3.3397151131270637, - "velocityY": -1.7746791225673975, - "timestamp": 11.021392852744567 + "heading": 1.0140262228054944e-17, + "angularVelocity": 5.6355416212281845e-18, + "velocityX": 3.3397151131270646, + "velocityY": -1.7746791225673952, + "timestamp": 11.072957918909571 }, { "x": 4.6324721141561005, - "y": 4.754119165100519, - "heading": 6.684498529619524e-18, - "angularVelocity": 1.5833845088958816e-17, - "velocityX": 3.2661718577713303, - "velocityY": -1.906647453610145, - "timestamp": 11.05602211076697 + "y": 4.75411916510052, + "heading": 1.0958701743566334e-17, + "angularVelocity": 2.3634335883891303e-17, + "velocityX": 3.2661718577713317, + "velocityY": -1.906647453610143, + "timestamp": 11.107587176931974 }, { "x": 4.742850082889288, - "y": 4.683628924272633, - "heading": 1.0669160659402455e-17, - "angularVelocity": 1.1506634439597858e-16, - "velocityX": 3.187419397256002, - "velocityY": -2.03556890483432, - "timestamp": 11.090651368789372 + "y": 4.683628924272634, + "heading": 1.1951109976942982e-17, + "angularVelocity": 2.8658085389373404e-17, + "velocityX": 3.1874193972560034, + "velocityY": -2.0355689048343186, + "timestamp": 11.142216434954376 }, { "x": 4.850325220314879, - "y": 4.608787325463071, - "heading": 1.658478005292643e-17, - "angularVelocity": 1.708272059914504e-16, - "velocityX": 3.1035934225348787, - "velocityY": -2.161224440937961, - "timestamp": 11.125280626811774 + "y": 4.608787325463072, + "heading": 1.1732443750037932e-17, + "angularVelocity": -6.3144935638988695e-18, + "velocityX": 3.1035934225348796, + "velocityY": -2.1612244409379597, + "timestamp": 11.176845692976778 }, { "x": 4.959315849281407, - "y": 4.536170434444089, - "heading": 2.4155748869411193e-17, - "angularVelocity": 2.1862925308959841e-16, - "velocityX": 3.1473567494868284, - "velocityY": -2.0969808527807587, - "timestamp": 11.159909884834176 + "y": 4.53617043444409, + "heading": 1.1207275250284524e-17, + "angularVelocity": -1.516545631482102e-17, + "velocityX": 3.147356749486827, + "velocityY": -2.096980852780761, + "timestamp": 11.21147495099918 }, { "x": 5.0711194796065495, - "y": 4.467963995966004, - "heading": 2.3436163975709297e-17, - "angularVelocity": -2.0779679808224236e-17, - "velocityX": 3.228588676454313, - "velocityY": -1.969618824462286, - "timestamp": 11.194539142856579 + "y": 4.467963995966005, + "heading": 1.0229768329797992e-17, + "angularVelocity": -2.822777547974504e-17, + "velocityX": 3.228588676454311, + "velocityY": -1.969618824462288, + "timestamp": 11.246104209021583 }, { "x": 5.1855579490393096, - "y": 4.4042772106470105, - "heading": 2.204718666214906e-17, - "angularVelocity": -4.0109935727230666e-17, - "velocityX": 3.3046757559381796, - "velocityY": -1.8391033754691897, - "timestamp": 11.229168400878981 + "y": 4.404277210647011, + "heading": 8.869711544920542e-18, + "angularVelocity": -3.9274788503918017e-17, + "velocityX": 3.3046757559381783, + "velocityY": -1.8391033754691917, + "timestamp": 11.280733467043985 }, { "x": 5.302448702578612, "y": 4.345211810292891, - "heading": 2.1117777344899902e-17, - "angularVelocity": -2.6838845829382515e-17, - "velocityX": 3.375491137109673, - "velocityY": -1.7056501850518673, - "timestamp": 11.263797658901384 + "heading": 8.222829686597657e-18, + "angularVelocity": -1.8680211337603908e-17, + "velocityX": 3.3754911371096723, + "velocityY": -1.705650185051869, + "timestamp": 11.315362725066388 }, { "x": 5.421605230513571, "y": 4.290862095789443, - "heading": 1.9153982489912048e-17, - "angularVelocity": -5.670912306921066e-17, - "velocityX": 3.440920618566971, - "velocityY": -1.569473838229202, - "timestamp": 11.298426916923786 + "heading": 7.194520598849692e-18, + "angularVelocity": -2.969480567798286e-17, + "velocityX": 3.4409206185669707, + "velocityY": -1.5694738382292037, + "timestamp": 11.34999198308879 }, { "x": 5.542837392691082, "y": 4.241314822773421, - "heading": 1.5300631909411777e-17, - "angularVelocity": -1.112744193943597e-16, - "velocityX": 3.5008593628856843, - "velocityY": -1.430792221536174, - "timestamp": 11.333056174946188 + "heading": 5.567604077424851e-18, + "angularVelocity": -4.698098123766793e-17, + "velocityX": 3.500859362885684, + "velocityY": -1.4307922215361755, + "timestamp": 11.384621241111192 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 9.183754863330666e-18, - "angularVelocity": -1.766389866662468e-16, - "velocityX": 3.555211493416193, - "velocityY": -1.2898268911821011, - "timestamp": 11.36768543296859 + "heading": 4.441119305733273e-18, + "angularVelocity": -3.252985585087723e-17, + "velocityX": 3.5552114934161922, + "velocityY": -1.2898268911821025, + "timestamp": 11.419250499133595 }, { "x": 5.934272395491136, - "y": 4.124337934678792, - "heading": -1.4789774171207362e-18, - "angularVelocity": -1.4511299214964166e-16, - "velocityX": 3.651673302114514, - "velocityY": -0.9841085377661543, - "timestamp": 11.441164258830412 + "y": 4.124337934678793, + "heading": 1.9097100466976255e-18, + "angularVelocity": -3.445086702659097e-17, + "velocityX": 3.6516733021145136, + "velocityY": -0.9841085377661556, + "timestamp": 11.492729324995416 }, { "x": 6.207753017493673, "y": 4.075010140740357, - "heading": -1.5184517487424443e-17, - "angularVelocity": -1.8652366732257328e-16, - "velocityX": 3.7218970063133985, - "velocityY": -0.6713198443208211, - "timestamp": 11.514643084692233 + "heading": -4.66846161540337e-18, + "angularVelocity": -8.952472477542546e-17, + "velocityX": 3.721897006313398, + "velocityY": -0.6713198443208224, + "timestamp": 11.566208150857237 }, { "x": 6.4844286250858945, "y": 4.049019985828209, - "heading": -2.210208594102609e-17, - "angularVelocity": -9.414369884747846e-17, + "heading": -6.502926410881733e-18, + "angularVelocity": -2.4965896963679208e-17, "velocityX": 3.7653787243758563, - "velocityY": -0.35370944768528717, - "timestamp": 11.588121910554054 + "velocityY": -0.3537094476852885, + "timestamp": 11.639686976719059 }, { "x": 6.743277363747609, "y": 4.038426394199003, - "heading": -1.998169117841033e-17, - "angularVelocity": 2.885722162467868e-17, - "velocityX": 3.522766397335807, - "velocityY": -0.14417203194192602, - "timestamp": 11.661600736415876 + "heading": -5.737032029312171e-18, + "angularVelocity": 1.0423334512854795e-17, + "velocityX": 3.5227663973358068, + "velocityY": -0.1441720319419282, + "timestamp": 11.71316580258088 }, { "x": 6.978672822837657, - "y": 4.030025299692623, - "heading": -1.7754526303263836e-17, - "angularVelocity": 3.031029482336482e-17, - "velocityX": 3.2035822065626665, - "velocityY": -0.1143335431376982, - "timestamp": 11.735079562277697 + "y": 4.030025299692622, + "heading": -4.2593802240854646e-18, + "angularVelocity": 2.010989952405427e-17, + "velocityX": 3.2035822065626656, + "velocityY": -0.11433354313770064, + "timestamp": 11.786644628442701 }, { "x": 7.19055269256627, "y": 4.022995759457438, - "heading": -1.4995601737739213e-17, - "angularVelocity": 3.754720537740805e-17, - "velocityX": 2.8835500192539434, - "velocityY": -0.095667563447506, - "timestamp": 11.808558388139518 + "heading": -3.134299993682012e-18, + "angularVelocity": 1.5311625045821916e-17, + "velocityX": 2.883550019253943, + "velocityY": -0.09566756344750842, + "timestamp": 11.860123454304523 }, { "x": 7.3789017577018114, "y": 4.017045849522277, - "heading": -1.2218093470845859e-17, - "angularVelocity": 3.780011771168623e-17, - "velocityX": 2.5633107623376494, - "velocityY": -0.08097448299391073, - "timestamp": 11.88203721400134 + "heading": -2.330563766445784e-18, + "angularVelocity": 1.0938337919929083e-17, + "velocityX": 2.5633107623376485, + "velocityY": -0.080974482993913, + "timestamp": 11.933602280166344 }, { "x": 7.54371362899692, "y": 4.012025997027571, - "heading": -9.378231648873842e-18, - "angularVelocity": 3.864870986523972e-17, - "velocityX": 2.2429845518359373, - "velocityY": -0.06831699385270533, - "timestamp": 11.955516039863161 + "heading": -1.6314906834703783e-18, + "angularVelocity": 9.513939216857223e-18, + "velocityX": 2.242984551835937, + "velocityY": -0.06831699385270738, + "timestamp": 12.007081106028165 }, { "x": 7.684984887810538, "y": 4.007845278835319, - "heading": -6.610589160747635e-18, - "angularVelocity": 3.7665850749041794e-17, - "velocityX": 1.9226118158077508, - "velocityY": -0.05689691068436128, - "timestamp": 12.028994865724982 + "heading": -1.3889434062297666e-18, + "angularVelocity": 3.300913894524217e-18, + "velocityX": 1.9226118158077503, + "velocityY": -0.05689691068436309, + "timestamp": 12.080559931889987 }, { "x": 7.8027134339680275, "y": 4.0044425825275765, - "heading": -4.4053913927362146e-18, - "angularVelocity": 3.0011336492479294e-17, - "velocityX": 1.6022104977409417, - "velocityY": -0.046308528584020826, - "timestamp": 12.102473691586804 + "heading": -1.0305851069387887e-18, + "angularVelocity": 4.877028110994567e-18, + "velocityX": 1.6022104977409415, + "velocityY": -0.04630852858402237, + "timestamp": 12.154038757751808 }, { "x": 7.896897856926731, "y": 4.001774011196325, - "heading": -2.659547727350374e-18, - "angularVelocity": 2.375981985162547e-17, - "velocityX": 1.281789983087365, - "velocityY": -0.03631755543113615, - "timestamp": 12.175952517448625 + "heading": -6.085636422043956e-19, + "angularVelocity": 5.743443227141563e-18, + "velocityX": 1.2817899830873647, + "velocityY": -0.036317555431137405, + "timestamp": 12.22751758361363 }, { "x": 7.967537148527336, "y": 3.999806507404438, - "heading": -1.3934136908294963e-18, - "angularVelocity": 1.723127746899314e-17, - "velocityX": 0.9613557480279207, - "velocityY": -0.026776472933672534, - "timestamp": 12.249431343310446 + "heading": -1.6215491216433702e-19, + "angularVelocity": 6.07533836862799e-18, + "velocityX": 0.9613557480279205, + "velocityY": -0.026776472933673492, + "timestamp": 12.30099640947545 }, { "x": 8.014630554462236, "y": 3.9985142796609896, - "heading": -4.425448526284405e-19, - "angularVelocity": 1.2940719003719293e-17, - "velocityX": 0.6409112473226126, - "velocityY": -0.017586396193627105, - "timestamp": 12.322910169172268 + "heading": 9.722147651876556e-20, + "angularVelocity": 3.5299473779135435e-18, + "velocityX": 0.6409112473226125, + "velocityY": -0.017586396193627746, + "timestamp": 12.374475235337272 }, { "x": 8.038177490234375, "y": 3.9978766441345215, - "heading": 3.3992773241588707e-43, - "angularVelocity": 6.022753459080235e-18, - "velocityX": 0.320458791984776, - "velocityY": -0.008677813220192955, - "timestamp": 12.396388995034089 + "heading": 9.32148398835055e-44, + "angularVelocity": -1.3231223468593855e-18, + "velocityX": 0.32045879198477595, + "velocityY": -0.00867781322019328, + "timestamp": 12.447954061199093 }, { "x": 8.038177490234375, "y": 3.9978766441345215, - "heading": -7.143469642081013e-44, - "angularVelocity": -5.598380381353948e-42, - "velocityX": -5.65394748040853e-41, - "velocityY": -2.108446218115532e-39, - "timestamp": 12.46986782089591 + "heading": -1.3950188036384555e-43, + "angularVelocity": -3.1671262842054474e-42, + "velocityX": -2.982658307493247e-43, + "velocityY": -2.0280554768402976e-41, + "timestamp": 12.521432887060914 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj index 2c19ea6b..602aa0e1 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj @@ -4,307 +4,307 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": 2.3560143253921187e-30, + "angularVelocity": -1.254857983919809e-31, "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 1.3060574270397207, - "y": 5.572382522338357, - "heading": -0.010694879272561806, - "angularVelocity": -0.14212355715817127, - "velocityX": 0.21372562484439114, - "velocityY": -0.24679919880328743, - "timestamp": 0.07525057412377358 - }, - { - "x": 1.3382235764672503, - "y": 5.535239265365289, - "heading": -0.0320761034546414, - "angularVelocity": -0.2841337017193578, - "velocityX": 0.42745387396382534, - "velocityY": -0.49359433341007053, - "timestamp": 0.15050114824754715 - }, - { - "x": 1.3864734146735522, - "y": 5.47952484840506, - "heading": -0.06412157549775398, - "angularVelocity": -0.42585020003622126, - "velocityX": 0.6411889712155109, - "velocityY": -0.7403852742575773, - "timestamp": 0.22575172237132074 - }, - { - "x": 1.4508078083146234, - "y": 5.405239688642535, - "heading": -0.10679814197023921, - "angularVelocity": -0.5671261245497303, - "velocityX": 0.8549355854022526, - "velocityY": -0.9871706711626929, - "timestamp": 0.3010022964950943 - }, - { - "x": 1.5312280128772082, - "y": 5.3123844280164345, - "heading": -0.16006528510741636, - "angularVelocity": -0.7078636111159992, - "velocityX": 1.0686988836704838, - "velocityY": -1.2339475373861764, - "timestamp": 0.37625287061886786 - }, - { - "x": 1.6277357374217547, - "y": 5.200960102506698, - "heading": -0.22387989152223717, - "angularVelocity": -0.8480281666778853, - "velocityX": 1.2824848935314492, - "velocityY": -1.4807106365400295, - "timestamp": 0.4515034447426414 - }, - { - "x": 1.7403333706916029, - "y": 5.070968456330507, - "heading": -0.29820152963685287, - "angularVelocity": -0.9876554296244249, - "velocityX": 1.496302647246288, - "velocityY": -1.7274505569061047, - "timestamp": 0.526754018866415 - }, - { - "x": 1.8690253649035526, - "y": 4.922413220845796, - "heading": -0.38299660252732504, - "angularVelocity": -1.1268362252301867, - "velocityX": 1.7101795661770514, - "velocityY": -1.9741408915022984, - "timestamp": 0.6020045929901886 - }, - { - "x": 2.021841436297539, - "y": 4.7786139697645345, - "heading": -0.46243583568438273, - "angularVelocity": -1.055662818268618, - "velocityX": 2.0307628636084356, - "velocityY": -1.9109389231128335, - "timestamp": 0.6772551671139622 - }, - { - "x": 2.158572983780592, - "y": 4.6533861744648695, - "heading": -0.5314192950572162, - "angularVelocity": -0.9167167184470773, - "velocityX": 1.8170166683292757, - "velocityY": -1.6641440514810064, - "timestamp": 0.7525057412377357 - }, - { - "x": 2.2792163174605955, - "y": 4.5467264938443535, - "heading": -0.5899522434559231, - "angularVelocity": -0.777840555764763, - "velocityX": 1.6032214383174732, - "velocityY": -1.4173935795391805, - "timestamp": 0.8277563153615093 - }, - { - "x": 2.3837699374210346, - "y": 4.458633436191598, - "heading": -0.6380354100231267, - "angularVelocity": -0.6389740826105205, - "velocityX": 1.3894062760138433, - "velocityY": -1.1706629308520584, - "timestamp": 0.9030068894852828 - }, - { - "x": 2.472232988532522, - "y": 4.389106089275013, - "heading": -0.6756678010096412, - "angularVelocity": -0.5000944035873048, - "velocityX": 1.1755797499519767, - "velocityY": -0.9239444047507308, - "timestamp": 0.9782574636090564 - }, - { - "x": 2.5446049348789113, - "y": 4.338143851138861, - "heading": -0.7028477866363936, - "angularVelocity": -0.36119306653537947, - "velocityX": 0.9617461021356389, - "velocityY": -0.6772338779998112, - "timestamp": 1.05350803773283 - }, - { - "x": 2.6008854472033804, - "y": 4.305746331042048, - "heading": -0.7195735501934102, - "angularVelocity": -0.222267587342223, - "velocityX": 0.7479080788470353, - "velocityY": -0.43052854378169836, - "timestamp": 1.1287586118566038 - }, - { - "x": 2.6410743549129108, - "y": 4.291913303700176, - "heading": -0.7258432586618085, - "angularVelocity": -0.0833177492798597, - "velocityX": 0.5340677885590666, - "velocityY": -0.18382620335314903, - "timestamp": 1.2040091859803774 + "x": 1.3060574276601378, + "y": 5.572382522924401, + "heading": -0.010694878831235951, + "angularVelocity": -0.1421235514303064, + "velocityX": 0.21372563329485256, + "velocityY": -0.24679919125306551, + "timestamp": 0.07525057405130896 + }, + { + "x": 1.3382235783773917, + "y": 5.535239267164631, + "heading": -0.032076102097718266, + "angularVelocity": -0.2841336898257091, + "velocityX": 0.4274538915144429, + "velocityY": -0.4935943177620345, + "timestamp": 0.15050114810261792 + }, + { + "x": 1.3864734186078633, + "y": 5.479524852099925, + "heading": -0.06412157270911513, + "angularVelocity": -0.42585018142040665, + "velocityX": 0.6411889987319064, + "velocityY": -0.7403852497811617, + "timestamp": 0.22575172215392686 + }, + { + "x": 1.4508078150986874, + "y": 5.405239694992128, + "heading": -0.1067981371766019, + "angularVelocity": -0.567126098451675, + "velocityX": 0.8549356240955547, + "velocityY": -0.9871706368349642, + "timestamp": 0.30100229620523583 + }, + { + "x": 1.5312280234732587, + "y": 5.3123844378954015, + "heading": -0.16006527765002243, + "angularVelocity": -0.7078635763993126, + "velocityX": 1.068698935356638, + "velocityY": -1.233947491673002, + "timestamp": 0.3762528702565448 + }, + { + "x": 1.6277357530197398, + "y": 5.200960116982037, + "heading": -0.2238798805982425, + "angularVelocity": -0.8480281214272752, + "velocityX": 1.2824849612365503, + "velocityY": -1.4807105768852864, + "timestamp": 0.4515034443078538 + }, + { + "x": 1.7403333929369436, + "y": 5.0709684768538175, + "heading": -0.2982015141504559, + "angularVelocity": -0.9876553699462992, + "velocityX": 1.4963027370230912, + "velocityY": -1.727450478198834, + "timestamp": 0.5267540183591627 + }, + { + "x": 1.8690253968085462, + "y": 4.922413250022354, + "heading": -0.3829965804922361, + "angularVelocity": -1.1268361392905244, + "velocityX": 1.7101796961898603, + "velocityY": -1.9741407784113794, + "timestamp": 0.6020045924104717 + }, + { + "x": 2.02184146077192, + "y": 4.778613992669738, + "heading": -0.4624358186091319, + "angularVelocity": -1.0556628851958798, + "velocityX": 2.0307627668195027, + "velocityY": -1.9109390082922875, + "timestamp": 0.6772551664617806 + }, + { + "x": 2.158573003278503, + "y": 4.653386193108499, + "heading": -0.5314192813425986, + "angularVelocity": -0.9167167639888999, + "velocityX": 1.817016603947309, + "velocityY": -1.664144109715068, + "timestamp": 0.7525057405130896 + }, + { + "x": 2.279216333070278, + "y": 4.546726509082428, + "heading": -0.5899522323979595, + "angularVelocity": -0.7778405918177544, + "velocityX": 1.6032213881911423, + "velocityY": -1.417393626160111, + "timestamp": 0.8277563145643986 + }, + { + "x": 2.3837699497753926, + "y": 4.458633448495562, + "heading": -0.6380354012157764, + "angularVelocity": -0.6389741131339646, + "velocityX": 1.3894062340921998, + "velocityY": -1.1706629709704055, + "timestamp": 0.9030068886157075 + }, + { + "x": 2.4722329980367945, + "y": 4.3891060989240405, + "heading": -0.6756677941957627, + "angularVelocity": -0.5000944305598923, + "velocityX": 1.1755797132095953, + "velocityY": -0.9239444409216138, + "timestamp": 0.9782574626670165 + }, + { + "x": 2.5446049418017402, + "y": 4.338143858296765, + "heading": -0.7028477816482255, + "angularVelocity": -0.36119309114484527, + "velocityX": 0.9617460687572824, + "velocityY": -0.6772339117562157, + "timestamp": 1.0535080367183254 + }, + { + "x": 2.6008854517223408, + "y": 4.305746335795738, + "heading": -0.7195735469228228, + "angularVelocity": -0.22226761038098528, + "velocityX": 0.7479080476225236, + "velocityY": -0.430528576145586, + "timestamp": 1.1287586107696344 + }, + { + "x": 2.641074357140525, + "y": 4.291913306081633, + "heading": -0.7258432570432893, + "angularVelocity": -0.08331777131422959, + "velocityX": 0.5340677586239448, + "velocityY": -0.1838262350545031, + "timestamp": 1.2040091848209433 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.05565596827444861, - "velocityX": 0.3202270361150376, - "velocityY": 0.06287505455011895, - "timestamp": 1.279259760104151 - }, - { - "x": 2.6731093005797457, - "y": 4.320288364477002, - "heading": -0.7067987753568107, - "angularVelocity": 0.19580392065911026, - "velocityX": 0.10461717847139641, - "velocityY": 0.3116194636271141, - "timestamp": 1.3551333161029784 - }, - { - "x": 2.664687962821916, - "y": 4.362805446311989, - "heading": -0.6813130877895294, - "angularVelocity": 0.3358968382514228, - "velocityX": -0.11099173681211887, - "velocityY": 0.5603675915105346, - "timestamp": 1.4310068721018057 - }, - { - "x": 2.6399077578590466, - "y": 4.424196334908807, - "heading": -0.645202192832585, - "angularVelocity": 0.47593518560166614, - "velocityX": -0.32659870276641295, - "velocityY": 0.8091210144040187, - "timestamp": 1.506880428100633 - }, - { - "x": 2.5987689088828363, - "y": 4.504461553370171, - "heading": -0.5984695308975294, - "angularVelocity": 0.6159281889382251, - "velocityX": -0.5422027270940676, - "velocityY": 1.0578813316982019, - "timestamp": 1.5827539840994602 - }, - { - "x": 2.541271709317337, - "y": 4.60360172553535, - "heading": -0.5411163068190713, - "angularVelocity": 0.7559053127670017, - "velocityX": -0.7578028841452508, - "velocityY": 1.3066498710827958, - "timestamp": 1.6586275400982875 - }, - { - "x": 2.4674164806885255, - "y": 4.721617486978178, - "heading": -0.47313850163583826, - "angularVelocity": 0.8959354057929544, - "velocityX": -0.9733988035419044, - "velocityY": 1.5554267872170555, - "timestamp": 1.7345010960971148 - }, - { - "x": 2.377203340708699, - "y": 4.858509175719519, - "heading": -0.39452181655964974, - "angularVelocity": 1.0361539543083473, - "velocityX": -1.188993171512905, - "velocityY": 1.804208158404987, - "timestamp": 1.810374652095942 - }, - { - "x": 2.270630715712783, - "y": 5.014275370945325, - "heading": -0.30523452580590593, - "angularVelocity": 1.1767906430312367, - "velocityX": -1.4046082801148865, - "velocityY": 2.052970803512055, - "timestamp": 1.8862482080947693 - }, - { - "x": 2.156210238547871, - "y": 5.1464603718429265, - "heading": -0.22910381899713708, - "angularVelocity": 1.0033892020445663, - "velocityX": -1.508041578600413, - "velocityY": 1.7421748481347223, - "timestamp": 1.9621217640935966 - }, - { - "x": 2.0581337448378676, - "y": 5.259758714146455, - "heading": -0.16374376524716946, - "angularVelocity": 0.8614339065879271, - "velocityX": -1.2926307778416481, - "velocityY": 1.4932520403643688, - "timestamp": 2.0379953200924237 - }, - { - "x": 1.9764031908936424, - "y": 5.354172852013204, - "heading": -0.10920543384793971, - "angularVelocity": 0.7188055269306859, - "velocityX": -1.077194193248253, - "velocityY": 1.24436157795548, - "timestamp": 2.1138688760912507 - }, - { - "x": 1.9110187094739324, - "y": 5.42970361761196, - "heading": -0.0655341593932664, - "angularVelocity": 0.5755796453747686, - "velocityX": -0.8617558589167408, - "velocityY": 0.995482083384748, - "timestamp": 2.189742432090078 - }, - { - "x": 1.8619802018876241, - "y": 5.486351463195586, - "heading": -0.03276467101544991, - "angularVelocity": 0.4318960400201182, - "velocityX": -0.646318825315813, - "velocityY": 0.7466085494271616, - "timestamp": 2.265615988088905 - }, - { - "x": 1.8292876471115809, - "y": 5.524116647521472, - "heading": -0.010918058172762177, - "angularVelocity": 0.2879344793491674, - "velocityX": -0.43088206879563057, - "velocityY": 0.4977384258464482, - "timestamp": 2.341489544087732 + "angularVelocity": 0.05565594681973357, + "velocityX": 0.3202270068209183, + "velocityY": 0.06287502296377193, + "timestamp": 1.2792597588722523 + }, + { + "x": 2.6731092983432756, + "y": 4.320288362117908, + "heading": -0.7067987769295019, + "angularVelocity": 0.19580389973285955, + "velocityX": 0.10461714888899723, + "velocityY": 0.3116194322188291, + "timestamp": 1.3551333149479476 + }, + { + "x": 2.664687958259504, + "y": 4.362805441582899, + "heading": -0.6813130909046112, + "angularVelocity": 0.3358968175826016, + "velocityX": -0.11099176735530734, + "velocityY": 0.5603675597065643, + "timestamp": 1.431006871023643 + }, + { + "x": 2.6399077508131867, + "y": 4.424196327740775, + "heading": -0.6452021974872086, + "angularVelocity": 0.4759351648285144, + "velocityX": -0.32659873516708793, + "velocityY": 0.8091209814393522, + "timestamp": 1.5068804270993383 + }, + { + "x": 2.5987688991007616, + "y": 4.504461543612991, + "heading": -0.5984695371274339, + "angularVelocity": 0.6159281675522111, + "velocityX": -0.5422027626077326, + "velocityY": 1.057881296501775, + "timestamp": 1.5827539831750337 + }, + { + "x": 2.541271696403367, + "y": 4.60360171291691, + "heading": -0.5411163147183906, + "angularVelocity": 0.7559052899984986, + "velocityX": -0.7578029246555089, + "velocityY": 1.3066498320479574, + "timestamp": 1.658627539250729 + }, + { + "x": 2.46741646400873, + "y": 4.721617471023204, + "heading": -0.47313851139747326, + "angularVelocity": 0.8959353803401775, + "velocityX": -0.9733988521888661, + "velocityY": 1.55542674166613, + "timestamp": 1.7345010953264244 + }, + { + "x": 2.3772033191525277, + "y": 4.85850915554647, + "heading": -0.39452182857786383, + "angularVelocity": 1.036153923517168, + "velocityX": -1.1889932345783751, + "velocityY": 1.8042081009834092, + "timestamp": 1.8103746514021197 + }, + { + "x": 2.2706306867395725, + "y": 5.014275344454255, + "heading": -0.30523454109050485, + "angularVelocity": 1.1767905987884617, + "velocityX": -1.404608376447531, + "velocityY": 2.0529707181614056, + "timestamp": 1.886248207477815 + }, + { + "x": 2.1562102192291532, + "y": 5.146460354029385, + "heading": -0.2291038294161038, + "angularVelocity": 1.0033892651564627, + "velocityX": -1.5080414498275487, + "velocityY": 1.7421749607384658, + "timestamp": 1.9621217635535104 + }, + { + "x": 2.0581337319934665, + "y": 5.259758702235753, + "heading": -0.16374377227039283, + "angularVelocity": 0.8614339504706835, + "velocityX": -1.292630691201352, + "velocityY": 1.4932521166502593, + "timestamp": 2.0379953196292058 + }, + { + "x": 1.976403182773577, + "y": 5.354172844449837, + "heading": -0.1092054383318089, + "angularVelocity": 0.7188055596708355, + "velocityX": -1.077194129890758, + "velocityY": 1.244361633992147, + "timestamp": 2.113868875704901 + }, + { + "x": 1.9110187048049692, + "y": 5.429703613246766, + "heading": -0.0655341619901584, + "angularVelocity": 0.575579669661773, + "velocityX": -0.8617558125585749, + "velocityY": 0.9954821245277466, + "timestamp": 2.1897424317805965 + }, + { + "x": 1.861980199634934, + "y": 5.486351461082553, + "heading": -0.03276467227497517, + "angularVelocity": 0.4318960572088904, + "velocityX": -0.6463187928148273, + "velocityY": 0.7466085783539637, + "timestamp": 2.265615987856292 + }, + { + "x": 1.8292876463834091, + "y": 5.5241166468364415, + "heading": -0.01091805858135444, + "angularVelocity": 0.2879344902726512, + "velocityX": -0.4308820482661345, + "velocityY": 0.4977384441630725, + "timestamp": 2.341489543931987 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 2.8937989389363637e-30, - "angularVelocity": 0.14389806868982188, - "velocityX": -0.21544335593050407, - "velocityY": 0.24886958055725772, - "timestamp": 2.417363100086559 + "heading": 2.72551505430432e-32, + "angularVelocity": 0.1438980739292335, + "velocityX": -0.21544334611501834, + "velocityY": 0.2488695893337634, + "timestamp": 2.4173631000076825 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.8511737998863575e-30, - "angularVelocity": -5.603397953128688e-29, + "heading": -2.2798037235511704e-30, + "angularVelocity": -3.1967802998211905e-29, "velocityX": 0, - "velocityY": 2.843834189498754e-31, - "timestamp": 2.493236656085386 + "velocityY": 3.158110370432563e-31, + "timestamp": 2.493236656083378 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj index e3057310..c607bce4 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj @@ -3,92 +3,101 @@ { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.8511737998863575e-30, - "angularVelocity": -5.603397953128688e-29, + "heading": -2.2798037235511704e-30, + "angularVelocity": -3.1967802998211905e-29, "velocityX": 0, - "velocityY": 2.843834189498754e-31, + "velocityY": 3.158110370432563e-31, "timestamp": 0 }, { - "x": 1.855209395178807, - "y": 5.543066072468525, - "heading": -4.900273115413656e-27, - "angularVelocity": -4.980371579437602e-26, - "velocityX": 0.42942883009919236, - "velocityY": 0.0006787122433542538, - "timestamp": 0.09842888654778115 + "x": 1.8507557561824508, + "y": 5.5430527114919315, + "heading": 1.2021210509026438e-21, + "angularVelocity": 1.2912282770144636e-20, + "velocityX": 0.40617567078743805, + "velocityY": 0.0005740544395093852, + "timestamp": 0.09309903404237341 }, { - "x": 1.939745796780934, - "y": 5.543199682246804, - "heading": 4.528417643864408e-20, - "angularVelocity": 4.600700356082414e-19, - "velocityX": 0.8588576440015887, - "velocityY": 0.0013574244611094727, - "timestamp": 0.1968577730955623 + "x": 1.9263848800936068, + "y": 5.543159599317717, + "heading": 3.6194046492049965e-21, + "angularVelocity": 2.5964647465645482e-20, + "velocityX": 0.8123513276919048, + "velocityY": 0.00114810885939775, + "timestamp": 0.18619806808474682 }, { - "x": 2.066550395198543, - "y": 5.5434000969079245, - "heading": 6.339784356887693e-20, - "angularVelocity": 1.8402795932716144e-19, - "velocityX": 1.2882864255103954, - "velocityY": 0.0020361366276666244, - "timestamp": 0.29528665964334344 + "x": 2.039828562987611, + "y": 5.543319931052194, + "heading": -1.5854010106883968e-20, + "angularVelocity": -2.0916881637274291e-19, + "velocityX": 1.2185269596070247, + "velocityY": 0.0017221632439682793, + "timestamp": 0.2792971021271202 }, { - "x": 2.2356231808662415, - "y": 5.543667316436768, - "heading": 5.434100687876101e-20, - "angularVelocity": -9.20140114123854e-20, - "velocityX": 1.7177151098384495, - "velocityY": 0.002714848640629602, - "timestamp": 0.3937155461911246 + "x": 2.1910867994360017, + "y": 5.543533706687691, + "heading": 4.514872599726518e-21, + "angularVelocity": 2.1878726150197974e-19, + "velocityX": 1.624702533213676, + "velocityY": 0.0022962175461305355, + "timestamp": 0.37239613616949363 }, { - "x": 2.4046959665339394, - "y": 5.543934535965611, - "heading": 4.338082196797977e-27, - "angularVelocity": -5.5208388966484585e-19, - "velocityX": 1.7177151098384498, - "velocityY": 0.0027148486406296025, - "timestamp": 0.49214443273890573 + "x": 2.380159562296481, + "y": 5.5438009261858445, + "heading": -2.4606104769465887e-21, + "angularVelocity": -7.492540764169747e-20, + "velocityX": 2.0308778152781284, + "velocityY": 0.002870271436251635, + "timestamp": 0.46549517021186704 }, { - "x": 2.5315005649515485, - "y": 5.544134950626731, - "heading": -2.717050695881145e-20, - "angularVelocity": -2.760420466983953e-19, - "velocityX": 1.2882864255103954, - "velocityY": 0.0020361366276666244, - "timestamp": 0.5905733192866869 + "x": 2.5314177987448714, + "y": 5.544014701821341, + "heading": 1.8729877149138444e-20, + "angularVelocity": 2.2761232534851326e-19, + "velocityX": 1.6247025332136762, + "velocityY": 0.002296217546130536, + "timestamp": 0.5585942042542404 }, { - "x": 2.6160369665536756, - "y": 5.54426856040501, - "heading": 3.0657579714860263e-27, - "angularVelocity": 2.760420337720658e-19, - "velocityX": 0.8588576440015887, - "velocityY": 0.0013574244611094727, - "timestamp": 0.689002205834468 + "x": 2.6448614816388756, + "y": 5.544175033555818, + "heading": 3.8900824798152035e-20, + "angularVelocity": 2.1666119156330769e-19, + "velocityX": 1.218526959607025, + "velocityY": 0.0017221632439682793, + "timestamp": 0.6516932382966139 }, { - "x": 2.6583051681518555, + "x": 2.7204906055500317, + "y": 5.544281921381604, + "heading": 1.1738818455912713e-20, + "angularVelocity": -2.9175390079640015e-19, + "velocityX": 0.8123513276919049, + "velocityY": 0.00114810885939775, + "timestamp": 0.7447922723389873 + }, + { + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": 1.737489718984482e-38, - "angularVelocity": -3.114693341043594e-26, - "velocityX": 0.42942883009919236, - "velocityY": 0.0006787122433542539, - "timestamp": 0.7874310923822492 + "heading": -1.0197992544535218e-37, + "angularVelocity": -1.260895838142623e-19, + "velocityX": 0.40617567078743805, + "velocityY": 0.0005740544395093853, + "timestamp": 0.8378913063813607 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, - "angularVelocity": -1.1323659927897137e-37, + "angularVelocity": 9.592877809620058e-37, "velocityX": 0, - "velocityY": 3.593395183653217e-38, - "timestamp": 0.8858599789300303 + "velocityY": 7.039095899612751e-38, + "timestamp": 0.9309903404237341 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj index 6561888b..820a29f1 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj @@ -1,436 +1,814 @@ { "samples": [ { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, - "angularVelocity": -1.1323659927897137e-37, + "angularVelocity": 9.592877809620058e-37, "velocityX": 0, - "velocityY": 3.593395183653217e-38, + "velocityY": 7.039095899612751e-38, "timestamp": 0 }, { - "x": 2.6753320649100156, - "y": 5.539342065750356, - "heading": 1.5795430701564016e-18, - "angularVelocity": 2.476798764658572e-17, - "velocityX": 0.2669898507573085, - "velocityY": -0.07829731508072835, - "timestamp": 0.06377357307726772 - }, - { - "x": 2.7093452100309583, - "y": 5.5292188917484815, - "heading": 4.421175983497276e-18, - "angularVelocity": 4.455815749727496e-17, - "velocityX": 0.5333423153150382, - "velocityY": -0.15873618982597607, - "timestamp": 0.12754714615453544 - }, - { - "x": 2.7602957743928864, - "y": 5.513806855363461, - "heading": 7.811064020414863e-18, - "angularVelocity": 5.3155058958518925e-17, - "velocityX": 0.7989291159865928, - "velocityY": -0.24166807097898507, - "timestamp": 0.19132071923180316 - }, - { - "x": 2.8281241133670565, - "y": 5.492918602334042, - "heading": 1.1328926559518176e-17, - "angularVelocity": 5.516176010462958e-17, - "velocityX": 1.0635806604718465, - "velocityY": -0.3275377561817081, - "timestamp": 0.2550942923090709 - }, - { - "x": 2.9127559105445995, - "y": 5.466330173559929, - "heading": 1.5973697872969206e-17, - "angularVelocity": 7.283222641176841e-17, - "velocityX": 1.3270668882705299, - "velocityY": -0.41691922674457305, - "timestamp": 0.3188678653863386 - }, - { - "x": 3.0140963269470453, - "y": 5.433769288149837, - "heading": 1.634340357557127e-17, - "angularVelocity": 5.797161500644316e-18, - "velocityX": 1.5890659957167883, - "velocityY": -0.5105701913650192, - "timestamp": 0.3826414384636063 - }, - { - "x": 3.132020733935084, - "y": 5.394898138842126, - "heading": 1.753294263521122e-17, - "angularVelocity": 1.8652539010143133e-17, - "velocityX": 1.8491108667403897, - "velocityY": -0.6095181347392088, - "timestamp": 0.44641501154087404 - }, - { - "x": 3.2663592482856187, - "y": 5.34928716553499, - "heading": 1.3361291372353952e-17, - "angularVelocity": -6.54134786803134e-17, - "velocityX": 2.1064918879136747, - "velocityY": -0.7152017851010761, - "timestamp": 0.5101885846181418 - }, - { - "x": 3.4168692069208895, - "y": 5.296373281622491, - "heading": 9.91837272236996e-18, - "angularVelocity": -5.398660422887962e-17, - "velocityX": 2.36006783645184, - "velocityY": -0.8297149016315647, - "timestamp": 0.5739621576954095 - }, - { - "x": 3.5831820501397047, - "y": 5.235389893074956, - "heading": 8.197617273877545e-18, - "angularVelocity": -2.6982264995683312e-17, - "velocityX": 2.6078645933372964, - "velocityY": -0.9562485776615884, - "timestamp": 0.6377357307726776 - }, - { - "x": 3.7646897201959026, - "y": 5.165243249909899, - "heading": 6.703480606914857e-18, - "angularVelocity": -2.342877456705142e-17, - "velocityX": 2.8461267151565104, - "velocityY": -1.0999327743494527, - "timestamp": 0.7015093038499454 - }, - { - "x": 3.960268086544922, - "y": 5.08428626917176, - "heading": 4.5447273483145725e-18, - "angularVelocity": -3.385027926825969e-17, - "velocityX": 3.066761934634853, - "velocityY": -1.2694440162550131, - "timestamp": 0.7652828769272131 + "x": 2.7765323823959327, + "y": 5.537624192256726, + "heading": 9.00740418544712e-19, + "angularVelocity": 1.3499597384555195e-17, + "velocityX": 0.2731753218474804, + "velocityY": -0.10058184592921769, + "timestamp": 0.06672350240426894 + }, + { + "x": 2.812967100056819, + "y": 5.524148553837151, + "heading": 1.885084819596924e-20, + "angularVelocity": -1.3217075521281405e-17, + "velocityX": 0.5460552331340873, + "velocityY": -0.2019623960674071, + "timestamp": 0.13344700480853788 + }, + { + "x": 2.8675858546495747, + "y": 5.503845620088409, + "heading": 3.43159270927708e-20, + "angularVelocity": 2.317785843530086e-19, + "velocityX": 0.8185834469813632, + "velocityY": -0.30428459264216856, + "timestamp": 0.20017050721280683 + }, + { + "x": 2.9403602428294473, + "y": 5.476640214649627, + "heading": 3.5333541604123114e-18, + "angularVelocity": 5.244086577062937e-17, + "velocityX": 1.0906859735710903, + "velocityY": -0.40773347409056915, + "timestamp": 0.26689400961707577 + }, + { + "x": 3.031255192543775, + "y": 5.4424407847147105, + "heading": 5.715508292123115e-18, + "angularVelocity": 3.2704430269696984e-17, + "velocityX": 1.3622628674916857, + "velocityY": -0.5125544778465974, + "timestamp": 0.3336175120213447 + }, + { + "x": 3.140226319352468, + "y": 5.40113340648718, + "heading": 6.027086577670902e-18, + "angularVelocity": 4.669693201563416e-18, + "velocityX": 1.6331745619175013, + "velocityY": -0.619082882928634, + "timestamp": 0.40034101442561365 + }, + { + "x": 3.267215684627595, + "y": 5.352572466235242, + "heading": 7.223223650198719e-18, + "angularVelocity": 1.7926772867621648e-17, + "velocityX": 1.9032179172147636, + "velocityY": -0.7277936334593704, + "timestamp": 0.4670645168298826 + }, + { + "x": 3.41214454573205, + "y": 5.296565350322459, + "heading": 8.483175715581024e-18, + "angularVelocity": 1.888318238685755e-17, + "velocityX": 2.172081139061778, + "velocityY": -0.8393911274837239, + "timestamp": 0.5337880192341515 + }, + { + "x": 3.5748998961264853, + "y": 5.232845435249424, + "heading": 1.0703776873779667e-17, + "angularVelocity": 3.3280644423357047e-17, + "velocityX": 2.4392507067198372, + "velocityY": -0.9549845673113, + "timestamp": 0.60051152163842 + }, + { + "x": 3.755306479840233, + "y": 5.161019665655948, + "heading": 1.3781541182824059e-17, + "angularVelocity": 4.612713958550321e-17, + "velocityX": 2.7037936740893525, + "velocityY": -1.0764688154152113, + "timestamp": 0.667235024042689 + }, + { + "x": 3.953058178777003, + "y": 5.080452170332065, + "heading": 1.6552203248378283e-17, + "angularVelocity": 4.15245298240439e-17, + "velocityX": 2.9637487813307404, + "velocityY": -1.2074830070480274, + "timestamp": 0.7339585264469579 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 2.916776997028848e-18, - "angularVelocity": -2.5527036870167268e-17, - "velocityX": 3.2494804486361053, - "velocityY": -1.4792721930373927, - "timestamp": 0.8290564500044808 - }, - { - "x": 4.281965617918933, - "y": 4.933633775459018, - "heading": 2.5007263674842835e-18, - "angularVelocity": -1.1988032936813997e-17, - "velocityX": 3.298225514699012, - "velocityY": -1.6226254295188278, - "timestamp": 0.8637619460385229 - }, - { - "x": 4.397154085355019, - "y": 4.872114684924303, - "heading": 2.80280710947709e-18, - "angularVelocity": 8.704118266930975e-18, - "velocityX": 3.31902668450838, - "velocityY": -1.772603695805773, - "timestamp": 0.8984674420725649 - }, - { - "x": 4.510378349293925, - "y": 4.805721616464948, - "heading": 5.6088052918749416e-18, - "angularVelocity": 8.085169506424825e-17, - "velocityX": 3.2624303605355687, - "velocityY": -1.9130419111206762, - "timestamp": 0.933172938106607 - }, - { - "x": 4.620854325513756, - "y": 4.734849659733664, - "heading": 8.920577651651403e-18, - "angularVelocity": 9.542501154652829e-17, - "velocityX": 3.183241527839498, - "velocityY": -2.0420960605711302, - "timestamp": 0.967878434140649 - }, - { - "x": 4.728404969374526, - "y": 4.659612469130667, - "heading": 1.0355720889195964e-17, - "angularVelocity": 4.1352045109421545e-17, - "velocityX": 3.0989513521223016, - "velocityY": -2.167875385765943, - "timestamp": 1.002583930174691 - }, - { - "x": 4.836822070451743, - "y": 4.585629308888239, - "heading": 8.659234104719362e-18, - "angularVelocity": -4.888236672406423e-17, - "velocityX": 3.1239173464304306, - "velocityY": -2.131741905370206, - "timestamp": 1.037289426208733 - }, - { - "x": 4.9481136166787945, - "y": 4.516045063115851, - "heading": 6.20990752321007e-18, - "angularVelocity": -7.057460233695506e-17, - "velocityX": 3.2067412642045734, - "velocityY": -2.0049921114550315, - "timestamp": 1.0719949222427752 - }, - { - "x": 5.062101259723399, - "y": 4.450971317120919, - "heading": 3.721439651007284e-18, - "angularVelocity": -7.170241479222451e-17, - "velocityX": 3.2844262745242743, - "velocityY": -1.8750271118759503, - "timestamp": 1.1067004182768172 - }, - { - "x": 5.178602303777092, - "y": 4.39051239286923, - "heading": 1.522860892330953e-18, - "angularVelocity": -6.334958464560708e-17, - "velocityX": 3.356847109732077, - "velocityY": -1.7420561916875086, - "timestamp": 1.1414059143108592 - }, - { - "x": 5.2974300176780025, - "y": 4.334765207042833, - "heading": -2.608193001732958e-19, - "angularVelocity": -5.139474712462447e-17, - "velocityX": 3.42388749563912, - "velocityY": -1.606292725847098, - "timestamp": 1.1761114103449013 - }, - { - "x": 5.4183939381282835, - "y": 4.283819120698816, - "heading": -2.822662772810701e-18, - "angularVelocity": -7.38166505421658e-17, - "velocityX": 3.485439894926986, - "velocityY": -1.4679544212261044, - "timestamp": 1.2108169063789433 - }, - { - "x": 5.5413001762936585, - "y": 4.23775579768525, - "heading": -5.4175777354584686e-18, - "angularVelocity": -7.476956848858907e-17, - "velocityX": 3.5414056045998694, - "velocityY": -1.3272630642818801, - "timestamp": 1.2455224024129854 + "heading": 2.0019563257216408e-17, + "angularVelocity": 5.196609716079013e-17, + "velocityX": 3.2138733563909483, + "velocityY": -1.356409229176722, + "timestamp": 0.8006820288512269 + }, + { + "x": 4.282917561329041, + "y": 4.939413083786088, + "heading": 2.2898103599715735e-17, + "angularVelocity": 8.297955477827177e-17, + "velocityX": 3.327163862876717, + "velocityY": -1.456761903756842, + "timestamp": 0.8353717828539282 + }, + { + "x": 4.401833285665519, + "y": 4.884962621939577, + "heading": 2.513067280390373e-17, + "angularVelocity": 6.43581734252565e-17, + "velocityX": 3.4279783110372324, + "velocityY": -1.5696410485433538, + "timestamp": 0.8700615368566296 + }, + { + "x": 4.518840762702163, + "y": 4.825621230712266, + "heading": 2.8188632508696854e-17, + "angularVelocity": 8.815166877675102e-17, + "velocityX": 3.372969350763695, + "velocityY": -1.7106316528704353, + "timestamp": 0.9047512908593309 + }, + { + "x": 4.633380439466489, + "y": 4.761646132811981, + "heading": 3.13421659224765e-17, + "angularVelocity": 9.090676784661175e-17, + "velocityX": 3.3018301817708395, + "velocityY": -1.8442073096080187, + "timestamp": 0.9394410448620323 + }, + { + "x": 4.745269125938669, + "y": 4.693140128704652, + "heading": 3.2482743151706595e-17, + "angularVelocity": 3.287936919763874e-17, + "velocityX": 3.2254102022016937, + "velocityY": -1.9748195418737586, + "timestamp": 0.9741307988647336 + }, + { + "x": 4.854330043574812, + "y": 4.6202164218202615, + "heading": 3.23593373316049e-17, + "angularVelocity": -3.55741410250663e-18, + "velocityX": 3.1438942353886365, + "velocityY": -2.1021684638845084, + "timestamp": 1.008820552867435 + }, + { + "x": 4.962829604926255, + "y": 4.546460093483164, + "heading": 3.1455405916033935e-17, + "angularVelocity": -2.605759082352001e-17, + "velocityX": 3.1277120426680747, + "velocityY": -2.1261704055714574, + "timestamp": 1.0435103068701364 + }, + { + "x": 5.07386693975175, + "y": 4.476582621829349, + "heading": 2.90082292000263e-17, + "angularVelocity": -7.054465464981545e-17, + "velocityX": 3.200868326043711, + "velocityY": -2.014354776005967, + "timestamp": 1.0782000608728377 + }, + { + "x": 5.187610618884458, + "y": 4.411202761148161, + "heading": 2.619388391387083e-17, + "angularVelocity": -8.112900673600335e-17, + "velocityX": 3.2788839933500147, + "velocityY": -1.884702343985982, + "timestamp": 1.112889814875539 + }, + { + "x": 5.303878902064225, + "y": 4.350425816718677, + "heading": 2.3840017411584327e-17, + "angularVelocity": -6.785480525757533e-17, + "velocityX": 3.35166064223783, + "velocityY": -1.7520142813566866, + "timestamp": 1.1475795688782404 + }, + { + "x": 5.422485692712026, + "y": 4.294349264150295, + "heading": 2.0445926478566092e-17, + "angularVelocity": -9.784130878396876e-17, + "velocityX": 3.419072693296267, + "velocityY": -1.6165162936588033, + "timestamp": 1.1822693228809418 + }, + { + "x": 5.5432410890609845, + "y": 4.243062963447586, + "heading": 1.6758899100249362e-17, + "angularVelocity": -1.0628577469963028e-16, + "velocityX": 3.4810104545438403, + "velocityY": -1.478427915594755, + "timestamp": 1.2169590768836431 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -7.665872207711091e-18, - "angularVelocity": -6.478208725348017e-17, - "velocityX": 3.591694883279384, - "velocityY": -1.1844441897757751, - "timestamp": 1.2802278984470274 - }, - { - "x": 5.800468403278387, - "y": 4.158565232862714, - "heading": -9.714096811795654e-18, - "angularVelocity": -5.540829464294432e-17, - "velocityX": 3.6389268627432543, - "velocityY": -1.0302389285738007, - "timestamp": 1.3171939270404631 - }, - { - "x": 5.936486438230336, - "y": 4.1262510009162545, - "heading": -1.1401685441264777e-17, - "angularVelocity": -4.5652419090776246e-17, - "velocityX": 3.67954146353996, - "velocityY": -0.8741602269982254, - "timestamp": 1.3541599556338988 - }, - { - "x": 6.0737584862384155, - "y": 4.099765139631346, - "heading": -1.2627280609090703e-17, - "angularVelocity": -3.3154634524185157e-17, - "velocityX": 3.713464854930551, - "velocityY": -0.7164919330720612, - "timestamp": 1.3911259842273345 - }, - { - "x": 6.212034919724313, - "y": 4.079155810434451, - "heading": -1.3669283004490658e-17, - "angularVelocity": -2.818810770451562e-17, - "velocityX": 3.7406353548742786, - "velocityY": -0.5575207827587797, - "timestamp": 1.4280920128207701 - }, - { - "x": 6.351064277842407, - "y": 4.06446048840337, - "heading": -1.3375402013759826e-17, - "angularVelocity": 7.950028767305027e-18, - "velocityX": 3.761003370072177, - "velocityY": -0.3975358617152357, - "timestamp": 1.4650580414142058 - }, - { - "x": 6.490086441164715, - "y": 4.055726900176193, - "heading": -1.4576849056839345e-17, - "angularVelocity": -3.2501382723403574e-17, - "velocityX": 3.7608087374308066, - "velocityY": -0.23625984612066567, - "timestamp": 1.5020240700076415 - }, - { - "x": 6.625430002884342, - "y": 4.051684818750479, - "heading": -1.3372180037518883e-17, - "angularVelocity": 3.2588543188391143e-17, - "velocityX": 3.661295704988517, - "velocityY": -0.10934583939674931, - "timestamp": 1.5389900986010772 - }, - { - "x": 6.755501460439252, - "y": 4.050426142633095, - "heading": -1.1630354881514603e-17, - "angularVelocity": 4.7119618262525755e-17, - "velocityX": 3.518675457011608, - "velocityY": -0.03404953589216864, - "timestamp": 1.5759561271945128 - }, - { - "x": 6.8798297740402194, - "y": 4.050767085715321, - "heading": -9.335447591441833e-18, - "angularVelocity": 6.208152126139743e-17, - "velocityX": 3.3633127044392923, - "velocityY": 0.009223146093816154, - "timestamp": 1.6129221557879485 - }, - { - "x": 6.998265582982919, - "y": 4.052014068285868, - "heading": -6.4179877740703305e-18, - "angularVelocity": 7.892272793106084e-17, - "velocityX": 3.203909466318293, - "velocityY": 0.03373320364655125, - "timestamp": 1.6498881843813842 - }, - { - "x": 7.110758098746119, - "y": 4.053729886887297, - "heading": -3.1976006784105176e-18, - "angularVelocity": 8.71174756444278e-17, - "velocityX": 3.0431323039981164, - "velocityY": 0.046416092469654044, - "timestamp": 1.6868542129748199 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -8.646914705534956e-42, - "angularVelocity": 8.650106057047145e-17, - "velocityX": 2.8819232262850263, - "velocityY": 0.051084720684648116, - "timestamp": 1.7238202415682555 - }, - { - "x": 7.405338431031862, - "y": 4.059210715077462, - "heading": 6.035426461355953e-18, - "angularVelocity": 8.221730826644903e-17, - "velocityX": 2.561662296238626, - "velocityY": 0.04893769145541128, - "timestamp": 1.7972284641020848 - }, - { - "x": 7.569876923635766, - "y": 4.062517669134614, - "heading": 1.1021958875984658e-17, - "angularVelocity": 6.792879928853616e-17, - "velocityX": 2.2414177448320305, - "velocityY": 0.04504882345608618, - "timestamp": 1.870636686635914 - }, - { - "x": 7.710907923869345, - "y": 4.065460118931574, - "heading": 1.4466516010520595e-17, - "angularVelocity": 4.692331479554e-17, - "velocityX": 1.9211880544932918, - "velocityY": 0.04008338160760226, - "timestamp": 1.9440449091697434 - }, - { - "x": 7.828432324355203, - "y": 4.067984380811152, - "heading": 1.5902740281761073e-17, - "angularVelocity": 1.956489643348339e-17, - "velocityX": 1.600970523863292, - "velocityY": 0.03438663670702133, - "timestamp": 2.0174531317035727 - }, - { - "x": 7.922450847470263, - "y": 4.070051609380546, - "heading": 1.5219645547347743e-17, - "angularVelocity": -9.30542534385069e-18, - "velocityX": 1.2807628337783643, - "velocityY": 0.02816072230112734, - "timestamp": 2.090861354237402 - }, - { - "x": 7.992964082884089, - "y": 4.071632392724107, - "heading": 1.2631622975679932e-17, - "angularVelocity": -3.5255213685022036e-17, - "velocityX": 0.9605631764388524, - "velocityY": 0.021534145481204726, - "timestamp": 2.164269576771231 - }, - { - "x": 8.039972518291798, - "y": 4.072703688242487, - "heading": 7.596971431802736e-18, - "angularVelocity": -6.858429982495519e-17, - "velocityX": 0.6403701627027665, - "velocityY": 0.014593671953933919, - "timestamp": 2.2376777993050605 - }, - { - "x": 8.0634765625, + "heading": 1.3081934400390577e-17, + "angularVelocity": -1.0599569831404499e-16, + "velocityX": 3.537374169625408, + "velocityY": -1.337971116472263, + "timestamp": 1.2516488308863445 + }, + { + "x": 5.795129656789146, + "y": 4.153827522905326, + "heading": 9.653855416146634e-18, + "angularVelocity": -9.526631807314967e-17, + "velocityX": 3.589854735093561, + "velocityY": -1.1900109590649786, + "timestamp": 1.2876329966815603 + }, + { + "x": 5.925973457628177, + "y": 4.116403970594212, + "heading": 6.773802428340066e-18, + "angularVelocity": -8.003667513641558e-17, + "velocityX": 3.636149343676849, + "velocityY": -1.0400005525788714, + "timestamp": 1.323617162476776 + }, + { + "x": 6.058257666523593, + "y": 4.0844428903853265, + "heading": 3.7266948743050956e-18, + "angularVelocity": -8.467912168301893e-17, + "velocityX": 3.6761782848667472, + "velocityY": -0.8881984479166671, + "timestamp": 1.3596013282719919 + }, + { + "x": 6.1917543385226965, + "y": 4.0579993432791355, + "heading": 2.3240409715281496e-19, + "angularVelocity": -9.710634385796704e-17, + "velocityX": 3.7098726356150347, + "velocityY": -0.7348661980016377, + "timestamp": 1.3955854940672077 + }, + { + "x": 6.326233441808095, + "y": 4.037118888120514, + "heading": -2.9507025724087755e-18, + "angularVelocity": -8.845853722652732e-17, + "velocityX": 3.7371744019498516, + "velocityY": -0.5802678677464012, + "timestamp": 1.4315696598624235 + }, + { + "x": 6.461463252337996, + "y": 4.021837506709874, + "heading": -5.178975495681446e-18, + "angularVelocity": -6.192370655342397e-17, + "velocityX": 3.7580365569536736, + "velocityY": -0.4246696032250841, + "timestamp": 1.4675538256576393 + }, + { + "x": 6.596863602394611, + "y": 4.012202736620321, + "heading": -4.3543144284076125e-18, + "angularVelocity": 2.2917331805219353e-17, + "velocityX": 3.7627758505553097, + "velocityY": -0.26775026950420705, + "timestamp": 1.503537991452855 + }, + { + "x": 6.727539043403903, + "y": 4.005664590276287, + "heading": -3.5766746235771005e-18, + "angularVelocity": 2.161061087932747e-17, + "velocityX": 3.6314706238561003, + "velocityY": -0.1816950928150608, + "timestamp": 1.5395221572480708 + }, + { + "x": 6.85298501008117, + "y": 4.001263406795894, + "heading": -2.645512635943814e-18, + "angularVelocity": 2.5876992478309148e-17, + "velocityX": 3.4861435274385992, + "velocityY": -0.122308892901394, + "timestamp": 1.5755063230432866 + }, + { + "x": 6.973061817653349, + "y": 3.9986189838302026, + "heading": -1.6196069084437975e-18, + "angularVelocity": 2.850992109492732e-17, + "velocityX": 3.336934591051298, + "velocityY": -0.07348851660866845, + "timestamp": 1.6114904888385024 + }, + { + "x": 7.0877080332373295, + "y": 3.99753108954472, + "heading": -5.829248205352589e-19, + "angularVelocity": 2.880939616045139e-17, + "velocityX": 3.186018434786799, + "velocityY": -0.030232583177662708, + "timestamp": 1.6474746546337182 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "angularVelocity": 1.619948128975407e-17, + "velocityX": 3.034163545803906, + "velocityY": 0.009602962363163213, + "timestamp": 1.683458820428934 + }, + { + "x": 7.37338840046626, + "y": 4.002743213470309, + "heading": 8.093787297850302e-19, + "angularVelocity": 1.2673214059219633e-17, + "velocityX": 2.7636055688801218, + "velocityY": 0.07620051362427777, + "timestamp": 1.7473241292611363 + }, + { + "x": 7.532268457404618, + "y": 4.010110422409507, + "heading": 7.012678022232893e-19, + "angularVelocity": -1.6927958157108362e-18, + "velocityX": 2.487736454164719, + "velocityY": 0.11535541084684194, + "timestamp": 1.8111894380933387 + }, + { + "x": 7.673406960294269, + "y": 4.018856350887051, + "heading": 5.093320205791772e-19, + "angularVelocity": -3.0053214359033797e-18, + "velocityX": 2.2099400358412575, + "velocityY": 0.13694333649152024, + "timestamp": 1.875054746925541 + }, + { + "x": 7.796760795915294, + "y": 4.028209945243461, + "heading": 2.737170340594828e-19, + "angularVelocity": -3.689248370192142e-18, + "velocityX": 1.931468552749354, + "velocityY": 0.14645814022421355, + "timestamp": 1.9389200557577433 + }, + { + "x": 7.902319650014668, + "y": 4.037611597874035, + "heading": 3.811839387951723e-20, + "angularVelocity": -3.688992419979718e-18, + "velocityX": 1.65283556956901, + "velocityY": 0.14721063441931925, + "timestamp": 2.0027853645899456 + }, + { + "x": 7.990087417936277, + "y": 4.046637814965993, + "heading": -1.8999762440941744e-19, + "angularVelocity": -3.5718298785688795e-18, + "velocityX": 1.3742635794998819, + "velocityY": 0.14133208242480425, + "timestamp": 2.066650673422148 + }, + { + "x": 8.06007417147421, + "y": 4.0549574599073335, + "heading": -2.1124739162594107e-19, + "angularVelocity": -3.327278549958625e-19, + "velocityX": 1.0958492931086379, + "velocityY": 0.1302686089438499, + "timestamp": 2.1305159822543502 + }, + { + "x": 8.112292461632407, + "y": 4.062304767499659, + "heading": -1.9392441616154913e-19, + "angularVelocity": 2.712423345539538e-19, + "velocityX": 0.8176315297463507, + "velocityY": 0.11504379649410336, + "timestamp": 2.1943812910865526 + }, + { + "x": 8.146755551617066, + "y": 4.068461861174775, + "heading": -1.2666965805994144e-19, + "angularVelocity": 1.0530718371509655e-18, + "velocityX": 0.5396214410425377, + "velocityY": 0.0964074829935333, + "timestamp": 2.258246599918755 + }, + { + "x": 8.1634765625, "y": 4.073246955871582, - "heading": 8.652482395157843e-45, - "angularVelocity": -1.0348937993018065e-16, - "velocityX": 0.32018271791516006, - "velocityY": 0.007400637290262982, - "timestamp": 2.31108602183889 + "heading": 0, + "angularVelocity": 1.9833875444262685e-18, + "velocityX": 0.26181680146361314, + "velocityY": 0.07492478756156581, + "timestamp": 2.322111908750957 + }, + { + "x": 8.160404735397709, + "y": 4.0766649695322, + "heading": 2.2110153580287816e-19, + "angularVelocity": 3.145700398787454e-18, + "velocityX": -0.04370411858778341, + "velocityY": 0.04862945386701764, + "timestamp": 2.3923988107993464 + }, + { + "x": 8.135851784135758, + "y": 4.07831805033267, + "heading": 5.233120115827785e-19, + "angularVelocity": 4.299669881203419e-18, + "velocityX": -0.34932470412548816, + "velocityY": 0.02351904483329859, + "timestamp": 2.4626857128477355 + }, + { + "x": 8.089809855784104, + "y": 4.078304537638241, + "heading": 9.059382457941767e-19, + "angularVelocity": 5.443777191210811e-18, + "velocityX": -0.6550570164545902, + "velocityY": -0.00019225053366650092, + "timestamp": 2.5329726148961247 + }, + { + "x": 8.022270134044854, + "y": 4.076742304755474, + "heading": 1.3681480133267195e-18, + "angularVelocity": 6.5760441001468545e-18, + "velocityX": -0.9609147617966158, + "velocityY": -0.022226515001204008, + "timestamp": 2.603259516944514 + }, + { + "x": 7.9332227395210335, + "y": 4.0737752063023285, + "heading": 1.9089237482918726e-18, + "angularVelocity": 7.693833690281903e-18, + "velocityX": -1.2669130652893827, + "velocityY": -0.04221410201153685, + "timestamp": 2.673546418992903 + }, + { + "x": 7.822656701940191, + "y": 4.069582704513375, + "heading": 2.0768842809054915e-18, + "angularVelocity": 2.3896419918926418e-18, + "velocityX": -1.5730674472567026, + "velocityY": -0.05964840769432409, + "timestamp": 2.743833321041292 + }, + { + "x": 7.690560152144021, + "y": 4.064394900776391, + "heading": 2.063501960546536e-18, + "angularVelocity": -1.9039564936380514e-19, + "velocityX": -1.8793906965088152, + "velocityY": -0.07380896846772396, + "timestamp": 2.8141202230896813 + }, + { + "x": 7.536921137087825, + "y": 4.058517363178713, + "heading": 1.8242393677495476e-18, + "angularVelocity": -3.404085054560583e-18, + "velocityX": -2.185884006531148, + "velocityY": -0.08362208927108938, + "timestamp": 2.8844071251380705 + }, + { + "x": 7.361730243526304, + "y": 4.0523752131799995, + "heading": 1.7449161631907716e-18, + "angularVelocity": -1.128563106982712e-18, + "velocityX": -2.492511242577032, + "velocityY": -0.08738683623422486, + "timestamp": 2.9546940271864597 + }, + { + "x": 7.164988961998875, + "y": 4.046599373609111, + "heading": 1.6203310877610785e-18, + "angularVelocity": -1.7725219321907188e-18, + "velocityX": -2.799117272119643, + "velocityY": -0.0821751905769253, + "timestamp": 3.024980929234849 + }, + { + "x": 6.946739461316107, + "y": 4.042219879396985, + "heading": 9.714507263424963e-19, + "angularVelocity": -9.231881652246047e-18, + "velocityX": -3.105123349048912, + "velocityY": -0.062308824041090796, + "timestamp": 3.095267831283238 + }, + { + "x": 6.707199372936614, + "y": 4.041196558011354, + "heading": 4.304213175776067e-18, + "angularVelocity": 4.741655062759286e-17, + "velocityX": -3.408033095761992, + "velocityY": -0.014559204571673643, + "timestamp": 3.165554733331627 + }, + { + "x": 6.4477381349567215, + "y": 4.048401250319157, + "heading": 3.981085161265745e-18, + "angularVelocity": -4.597272110228882e-18, + "velocityX": -3.691459296374506, + "velocityY": 0.102504052644734, + "timestamp": 3.2358416353800163 + }, + { + "x": 6.183416579926013, + "y": 4.076604236120931, + "heading": 3.783709688486887e-18, + "angularVelocity": -2.808140165901377e-18, + "velocityX": -3.7606089801587, + "velocityY": 0.4012552122777795, + "timestamp": 3.3061285374284055 + }, + { + "x": 5.922248771081443, + "y": 4.126128703964576, + "heading": 4.4649221139499255e-18, + "angularVelocity": 9.691882920007657e-18, + "velocityX": -3.7157393658460984, + "velocityY": 0.7046045052540438, + "timestamp": 3.3764154394767947 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 5.394624259127602e-43, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 6.738429572739451e-18, + "angularVelocity": 3.2346104217601064e-17, + "velocityX": -3.646441012355221, + "velocityY": 1.003321650758162, + "timestamp": 3.446702341525184 + }, + { + "x": 5.5410942529337, + "y": 4.236435293525444, + "heading": 7.176696434761227e-18, + "angularVelocity": 1.2648537308401216e-17, + "velocityX": -3.603431102925481, + "velocityY": 1.1482444113922432, + "timestamp": 3.4813519497238077 + }, + { + "x": 5.417926513758212, + "y": 4.281179467560016, + "heading": 6.902122193005105e-18, + "angularVelocity": -7.924310144639137e-18, + "velocityX": -3.554664701241286, + "velocityY": 1.2913327555706813, + "timestamp": 3.5160015579224315 + }, + { + "x": 5.296645297491296, + "y": 4.3308101048073215, + "heading": 7.073122455788382e-18, + "angularVelocity": 4.9351283224632194e-18, + "velocityX": -3.5002189800153154, + "velocityY": 1.432357819540261, + "timestamp": 3.5506511661210554 + }, + { + "x": 5.177444375118266, + "y": 4.38524790502043, + "heading": 7.030277898528261e-18, + "angularVelocity": -1.2365091407245627e-18, + "velocityX": -3.44018095932656, + "velocityY": 1.5710942502164025, + "timestamp": 3.585300774319679 + }, + { + "x": 5.06051419213652, + "y": 4.444405885414455, + "heading": 6.93623096110632e-18, + "angularVelocity": -2.7142280190538443e-18, + "velocityX": -3.3746466139374856, + "velocityY": 1.7073203268248038, + "timestamp": 3.619950382518303 + }, + { + "x": 4.9460415627669985, + "y": 4.508189517767328, + "heading": 6.830736149305672e-18, + "angularVelocity": -3.0446177398576756e-18, + "velocityX": -3.303720743776551, + "velocityY": 1.8408182853682935, + "timestamp": 3.654599990716927 + }, + { + "x": 4.834209367418348, + "y": 4.576496874327604, + "heading": 6.686796334021e-18, + "angularVelocity": -4.154154195902009e-18, + "velocityX": -3.2275168800637117, + "velocityY": 1.971374572800783, + "timestamp": 3.6892495989155507 + }, + { + "x": 4.725196244628599, + "y": 4.649218770197504, + "heading": 6.8822518039480765e-18, + "angularVelocity": 5.6409142870147405e-18, + "velocityX": -3.1461574446916942, + "velocityY": 2.0987797453014108, + "timestamp": 3.7238992071141745 + }, + { + "x": 4.61917611476972, + "y": 4.726238684910051, + "heading": 7.643017507811422e-18, + "angularVelocity": 2.195596843411265e-17, + "velocityX": -3.0597786056088934, + "velocityY": 2.2228220957374782, + "timestamp": 3.7585488153127984 + }, + { + "x": 4.510162876116158, + "y": 4.798960406973094, + "heading": 8.156998265512583e-18, + "angularVelocity": 1.483366723094954e-17, + "velocityX": -3.14616078856249, + "velocityY": 2.098774729173764, + "timestamp": 3.7931984235114222 + }, + { + "x": 4.3983305631985266, + "y": 4.867267570914994, + "heading": 9.169956130720048e-18, + "angularVelocity": 2.9234323788045264e-17, + "velocityX": -3.227520273146238, + "velocityY": 1.9713690137660609, + "timestamp": 3.827848031710046 + }, + { + "x": 4.28385782617867, + "y": 4.93105099911187, + "heading": 9.858535854282379e-18, + "angularVelocity": 1.9872655402484013e-17, + "velocityX": -3.3037238506034297, + "velocityY": 1.840812393353667, + "timestamp": 3.86249763990867 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 9.428613355674913e-18, + "angularVelocity": -1.2407716016383212e-17, + "velocityX": -3.3581551662140523, + "velocityY": 1.6997824742615957, + "timestamp": 3.8971472481072937 + }, + { + "x": 3.9412126137988053, + "y": 5.086809869447978, + "heading": 6.455008109104801e-18, + "angularVelocity": -4.321651467897992e-17, + "velocityX": -3.28870544216361, + "velocityY": 1.4077326603917448, + "timestamp": 3.9659543989999233 + }, + { + "x": 3.7303194478817963, + "y": 5.1698988995186355, + "heading": 5.431152098522906e-18, + "angularVelocity": -1.488008146391029e-17, + "velocityX": -3.0649890771686916, + "velocityY": 1.2075638795204993, + "timestamp": 4.034761549892552 + }, + { + "x": 3.537102982396402, + "y": 5.24230220978644, + "heading": 5.612600329775147e-18, + "angularVelocity": 2.6370548540132515e-18, + "velocityX": -2.8080869935582533, + "velocityY": 1.052264326142308, + "timestamp": 4.103568700785182 + }, + { + "x": 3.3623900460315483, + "y": 5.305525654536907, + "heading": 4.225050788166454e-18, + "angularVelocity": -2.0165775266205907e-17, + "velocityX": -2.5391683000722396, + "velocityY": 0.918849915020073, + "timestamp": 4.1723758516778116 + }, + { + "x": 3.2065873746476083, + "y": 5.360439315991953, + "heading": 1.940037667439135e-18, + "angularVelocity": -3.3208948359058894e-17, + "velocityX": -2.2643383625498803, + "velocityY": 0.7980807335088809, + "timestamp": 4.241183002570441 + }, + { + "x": 3.069932459985333, + "y": 5.407606100907469, + "heading": -1.0877040544793261e-18, + "angularVelocity": -4.4003300276785115e-17, + "velocityX": -1.986056868936749, + "velocityY": 0.6854924859353159, + "timestamp": 4.309990153463071 + }, + { + "x": 2.9525796309081422, + "y": 5.44741896036095, + "heading": -3.706402695175446e-18, + "angularVelocity": -3.805852453885881e-17, + "velocityX": -1.705532456362192, + "velocityY": 0.5786151430046301, + "timestamp": 4.3787973043557 + }, + { + "x": 2.854636716681842, + "y": 5.480167404381309, + "heading": -5.982131114439951e-18, + "angularVelocity": -3.307401032802041e-17, + "velocityX": -1.4234409208300893, + "velocityY": 0.4759453573577233, + "timestamp": 4.44760445524833 + }, + { + "x": 2.776183090706554, + "y": 5.506073449109121, + "heading": -4.835349945984594e-18, + "angularVelocity": 1.666659865403876e-17, + "velocityX": -1.1401958220550505, + "velocityY": 0.3765022151293121, + "timestamp": 4.516411606140959 + }, + { + "x": 2.7172795158509544, + "y": 5.525312687608081, + "heading": -3.43564479511919e-18, + "angularVelocity": 2.034243727152086e-17, + "velocityX": -0.8560676338352585, + "velocityY": 0.2796110324199007, + "timestamp": 4.585218757033589 + }, + { + "x": 2.6779739479348046, + "y": 5.538027438094956, + "heading": -1.81505607662002e-18, + "angularVelocity": 2.355262058485764e-17, + "velocityX": -0.5712424857916886, + "velocityY": 0.18478821346223093, + "timestamp": 4.6540259079262185 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -7.816373438340897e-46, + "angularVelocity": 2.6378887267870092e-17, + "velocityX": -0.2858537161877381, + "velocityY": 0.09167545987040067, + "timestamp": 4.722833058818848 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 1.7878647482773802e-40, + "angularVelocity": 2.5983819147247646e-39, "velocityX": 0, - "velocityY": -7.823862347152674e-43, - "timestamp": 2.384494244372719 + "velocityY": -2.2530838665454385e-40, + "timestamp": 4.791640209711478 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj index f61fce96..2593024c 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj @@ -1,382 +1,382 @@ { "samples": [ { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 5.394624259127602e-43, + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 1.7878647482773802e-40, + "angularVelocity": 2.5983819147247646e-39, "velocityX": 0, - "velocityY": -7.823862347152674e-43, + "velocityY": -2.2530838665454385e-40, "timestamp": 0 }, { - "x": 8.042595807906332, - "y": 4.071940125417329, - "heading": 2.4014031773112573e-19, - "angularVelocity": 3.467784621738616e-18, - "velocityX": -0.30153187250835495, - "velocityY": -0.01887149394694188, - "timestamp": 0.06924891362218677 - }, - { - "x": 8.000826969409328, - "y": 4.069449378892138, - "heading": 7.295832460611546e-19, - "angularVelocity": 7.067878797353639e-18, - "velocityX": -0.6031695850838968, - "velocityY": -0.03596802310545437, - "timestamp": 0.13849782724437354 - }, - { - "x": 7.938162262991824, - "y": 4.0659211372128246, - "heading": 1.4792287784714074e-18, - "angularVelocity": 1.0825376070160765e-17, - "velocityX": -0.9049197040027859, - "velocityY": -0.0509501376232871, - "timestamp": 0.2077467408665603 - }, - { - "x": 7.8545936374958405, - "y": 4.061532754567933, - "heading": 2.6959287825754293e-18, - "angularVelocity": 1.756995078279752e-17, - "velocityX": -1.206786086954711, - "velocityY": -0.06337114064826968, - "timestamp": 0.2769956544887471 - }, - { - "x": 7.7501132248715185, - "y": 4.056503424652652, - "heading": 4.202275116563444e-18, - "angularVelocity": 2.17526348818473e-17, - "velocityX": -1.5087660897375712, - "velocityY": -0.07262684210066948, - "timestamp": 0.34624456811093385 - }, - { - "x": 7.624714358960079, - "y": 4.051110859560637, - "heading": 6.0188244090330196e-18, - "angularVelocity": 2.6232170260178098e-17, - "velocityX": -1.8108423562512328, - "velocityY": -0.07787219769881376, - "timestamp": 0.4154934817331206 - }, - { - "x": 7.478393887159083, - "y": 4.045718105040055, - "heading": 8.17237550228445e-18, - "angularVelocity": 3.1098698601987103e-17, - "velocityX": -2.1129641484240804, - "velocityY": -0.07787493317230812, - "timestamp": 0.4847423953553074 - }, - { - "x": 7.311157646174121, - "y": 4.040819461213868, - "heading": 1.0532118174804612e-17, - "angularVelocity": 3.4076241042489266e-17, - "velocityX": -2.4150016547173685, - "velocityY": -0.07073964875337486, - "timestamp": 0.5539913089774942 - }, - { - "x": 7.123034533164843, - "y": 4.037125830040512, - "heading": 8.283291482043014e-18, - "angularVelocity": -3.247454111743766e-17, - "velocityX": -2.716621866960299, - "velocityY": -0.053338471033761534, - "timestamp": 0.6232402225996809 - }, - { - "x": 6.914117681129227, - "y": 4.035741699141579, - "heading": 5.798379315469736e-18, - "angularVelocity": -3.588377111777714e-17, - "velocityX": -3.016897177267489, - "velocityY": -0.019987763367446825, - "timestamp": 0.6924891362218677 - }, - { - "x": 6.684712125750405, - "y": 4.03859150608828, - "heading": 3.611288298800787e-18, - "angularVelocity": -3.158303722425788e-17, - "velocityX": -3.312767571061532, - "velocityY": 0.041153092483867636, - "timestamp": 0.7617380498440545 - }, - { - "x": 6.43606725460267, - "y": 4.049660896865006, - "heading": 2.3554139869933522e-18, - "angularVelocity": -1.8135653631468075e-17, - "velocityX": -3.5905959839934547, - "velocityY": 0.15984930589841134, - "timestamp": 0.8309869634662412 - }, - { - "x": 6.175716481433539, - "y": 4.078070527288863, - "heading": 2.6764824042330446e-18, - "angularVelocity": 4.636439771335605e-18, - "velocityX": -3.759636932206204, - "velocityY": 0.41025380670743106, - "timestamp": 0.900235877088428 - }, - { - "x": 5.918464051979435, - "y": 4.12717103366876, - "heading": 2.4806366122903936e-18, - "angularVelocity": -2.8281424458318553e-18, - "velocityX": -3.7148948048144055, - "velocityY": 0.7090437064151481, - "timestamp": 0.9694847907106148 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 3.0661477431883128e-18, - "angularVelocity": 8.455167023881293e-18, - "velocityX": -3.6464445426004524, - "velocityY": 1.0033087488526664, - "timestamp": 1.0387337043328015 - }, - { - "x": 5.541093538436437, - "y": 4.236435061250764, - "heading": 3.354859743531915e-18, - "angularVelocity": 8.33229202553877e-18, - "velocityX": -3.6034349206984246, - "velocityY": 1.1482323536375627, - "timestamp": 1.0733834741030748 - }, - { - "x": 5.417925084630127, - "y": 4.281179055760442, - "heading": 3.9245083052882345e-18, - "angularVelocity": 1.6440183168115343e-17, - "velocityX": -3.554668750266214, - "velocityY": 1.2913215529664346, - "timestamp": 1.108033243873348 - }, - { - "x": 5.296643156826778, - "y": 4.330809566180666, - "heading": 4.682534245858068e-18, - "angularVelocity": 2.1876795880478242e-17, - "velocityX": -3.500223193615562, - "velocityY": 1.4323474802075717, - "timestamp": 1.1426830136436212 - }, - { - "x": 5.177441529342695, - "y": 4.385247292254255, - "heading": 5.748631283020555e-18, - "angularVelocity": 3.076779569476711e-17, - "velocityX": -3.4401852674457762, - "velocityY": 1.5710847845312972, - "timestamp": 1.1773327834138945 - }, - { - "x": 5.060510651244131, - "y": 4.444405251423305, - "heading": 6.298107237796981e-18, - "angularVelocity": 1.5857997280196432e-17, - "velocityX": -3.374650939207116, - "velocityY": 1.7073117530438153, - "timestamp": 1.2119825531841677 - }, - { - "x": 4.946037340921014, - "y": 4.5081889164365, - "heading": 6.498139126492573e-18, - "angularVelocity": 5.772964438776829e-18, - "velocityX": -3.303724991019314, - "velocityY": 1.840810644228753, - "timestamp": 1.246632322954441 - }, - { - "x": 4.834204484779198, - "y": 4.576496362942444, - "heading": 6.284318947548799e-18, - "angularVelocity": -6.170897537311036e-18, - "velocityX": -3.227520900810133, - "velocityY": 1.971367976145888, - "timestamp": 1.2812820927247142 - }, - { - "x": 4.725190735681975, - "y": 4.649218420709879, - "heading": 4.218601510181254e-18, - "angularVelocity": -5.961706098087163e-17, - "velocityX": -3.146160849551947, - "velocityY": 2.098774631103723, - "timestamp": 1.3159318624949874 - }, - { - "x": 4.619170166207779, - "y": 4.726238769851069, - "heading": 2.3861247317221027e-18, - "angularVelocity": -5.2885684107236655e-17, - "velocityX": -3.059777025276331, - "velocityY": 2.22282426843907, - "timestamp": 1.3505816322652606 - }, - { - "x": 4.5101566662173465, - "y": 4.798961201296667, - "heading": 8.546946946648484e-19, - "angularVelocity": -4.419740873346574e-17, - "velocityX": -3.146153660275008, - "velocityY": 2.0987854155379675, - "timestamp": 1.3852314020355339 - }, - { - "x": 4.398324060290128, - "y": 4.867269057737162, - "heading": -1.0977695137154222e-18, - "angularVelocity": -5.634854780637897e-17, - "velocityX": -3.2275136795616595, - "velocityY": 1.9713798069474653, - "timestamp": 1.419881171805807 - }, - { - "x": 4.283850992630547, - "y": 4.931053147799285, - "heading": -2.453093242554037e-18, - "angularVelocity": -3.911494182571376e-17, - "velocityX": -3.3037179876961247, - "velocityY": 1.8408229112346155, - "timestamp": 1.4545309415760803 + "x": 2.6750735555579293, + "y": 5.538716972759925, + "heading": 1.5037626920568013e-19, + "angularVelocity": 2.3619256993662716e-18, + "velocityX": 0.26337722940288844, + "velocityY": -0.08824680774956185, + "timestamp": 0.06366680765869503 + }, + { + "x": 2.7085920650608806, + "y": 5.52742596598787, + "heading": 4.62580789697953e-19, + "angularVelocity": 4.9037250644942105e-18, + "velocityX": 0.5264675697678678, + "velocityY": -0.17734526336836984, + "timestamp": 0.12733361531739007 + }, + { + "x": 2.7588390755328, + "y": 5.51039889888726, + "heading": 7.215027779377387e-19, + "angularVelocity": 4.066828505487763e-18, + "velocityX": 0.789218312017212, + "velocityY": -0.2674402522565463, + "timestamp": 0.1910004229760851 + }, + { + "x": 2.8257885982701296, + "y": 5.487560532002736, + "heading": -1.3111959461224426e-18, + "angularVelocity": -3.192713438621083e-17, + "velocityX": 1.0515608556382001, + "velocityY": -0.35871701007777496, + "timestamp": 0.25466723063478014 + }, + { + "x": 2.9094088162269207, + "y": 5.458820213911237, + "heading": -3.133450423089841e-18, + "angularVelocity": -2.8621734683732664e-17, + "velocityX": 1.3134036561886782, + "velocityY": -0.4514176090871532, + "timestamp": 0.3183340382934752 + }, + { + "x": 3.009659896203603, + "y": 5.424066617313393, + "heading": -4.721555014357318e-18, + "angularVelocity": -2.494399593240158e-17, + "velocityX": 1.5746208057754199, + "velocityY": -0.5458668005493692, + "timestamp": 0.3820008459521702 + }, + { + "x": 3.1264905621724175, + "y": 5.383159777949491, + "heading": -8.056338252897749e-18, + "angularVelocity": -5.237867832823588e-17, + "velocityX": 1.8350325745107399, + "velocityY": -0.6425143786570167, + "timestamp": 0.44566765361086524 + }, + { + "x": 3.259832420061778, + "y": 5.335918462965804, + "heading": -1.1096621419747018e-17, + "angularVelocity": -4.775303299558587e-17, + "velocityX": 2.094370093191732, + "velocityY": -0.7420085397863474, + "timestamp": 0.5093344612695603 + }, + { + "x": 3.409589860765726, + "y": 5.282098877479992, + "heading": -1.3592361718623002e-17, + "angularVelocity": -3.9200022596627705e-17, + "velocityX": 2.352205901492158, + "velocityY": -0.8453319314253591, + "timestamp": 0.5730012689282553 + }, + { + "x": 3.5756203018815467, + "y": 5.221355824269489, + "heading": -1.4313494756802784e-17, + "angularVelocity": -1.132667185145573e-17, + "velocityX": 2.607802200573299, + "velocityY": -0.9540772569615096, + "timestamp": 0.6366680765869503 + }, + { + "x": 3.7576900306489676, + "y": 5.153163870724839, + "heading": -1.1221400900164693e-17, + "angularVelocity": 4.856681166133233e-17, + "velocityX": 2.8597276267323055, + "velocityY": -1.0710754324327645, + "timestamp": 0.7003348842456454 + }, + { + "x": 3.955353841452516, + "y": 5.076631094268888, + "heading": -7.381711218130638e-18, + "angularVelocity": 6.030912846483351e-17, + "velocityX": 3.104660310018777, + "velocityY": -1.2020828320186705, + "timestamp": 0.7640016919043404 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -1.5610631193350687e-18, - "angularVelocity": 2.5744186155726097e-17, - "velocityX": -3.3579422894520032, - "velocityY": 1.6997125365941679, - "timestamp": 1.4891807113463535 - }, - { - "x": 3.941206853693096, - "y": 5.086799964367495, - "heading": 3.827307300721309e-18, - "angularVelocity": 7.831135017939884e-17, - "velocityX": -3.2887955452772286, - "velocityY": 1.4075914407764616, - "timestamp": 1.557987728563047 - }, - { - "x": 3.730310577999757, - "y": 5.1698821724733195, - "heading": 7.528976478562542e-18, - "angularVelocity": 5.37978439929115e-17, - "velocityX": -3.0650402273530597, - "velocityY": 1.2074670791813904, - "timestamp": 1.6267947457797405 - }, - { - "x": 3.5370931608840936, - "y": 5.242282386882321, - "heading": 5.4493611354862e-18, - "angularVelocity": -3.022388452804216e-17, - "velocityX": -2.808106279439004, - "velocityY": 1.0522213770870446, - "timestamp": 1.695601762996434 - }, - { - "x": 3.3623805585841375, - "y": 5.305505508408588, - "heading": 3.90044719086442e-18, - "angularVelocity": -2.2510988083436363e-17, - "velocityX": -2.5391683779829535, - "velocityY": 0.9188470025834666, - "timestamp": 1.7644087802131274 - }, - { - "x": 3.2065789543659307, - "y": 5.3604207146722205, - "heading": 2.734702057316729e-18, - "angularVelocity": -1.6942241950067733e-17, - "velocityX": -2.2643272520816113, - "velocityY": 0.7981047353162919, - "timestamp": 1.8332157974298209 - }, - { - "x": 3.0699254867036734, - "y": 5.407590197311926, - "heading": 1.8584817371846494e-18, - "angularVelocity": -1.2734461622898273e-17, - "velocityX": -1.9860396975485235, - "velocityY": 0.6855330247953545, - "timestamp": 1.9020228146465143 - }, - { - "x": 2.952574245770292, - "y": 5.4474063504067525, - "heading": 1.2072006114656177e-18, - "angularVelocity": -9.465330021034912e-18, - "velocityX": -1.7055126886812788, - "velocityY": 0.5786641349302285, - "timestamp": 1.9708298318632078 - }, - { - "x": 2.8546328911186802, - "y": 5.480158244239293, - "heading": 7.336690244970979e-19, - "angularVelocity": -6.882024626605053e-18, - "velocityX": -1.4234210203178261, - "velocityY": 0.4759964195133704, - "timestamp": 2.0396368490799013 - }, - { - "x": 2.7761806703190945, - "y": 5.5060675412854945, - "heading": 4.019181330238403e-19, - "angularVelocity": -4.821468868916063e-18, - "velocityX": -1.1401776152062642, - "velocityY": 0.37655021383364773, - "timestamp": 2.1084438662965947 - }, - { - "x": 2.7172782497291417, - "y": 5.525309545004062, - "heading": 1.8363150875881817e-19, - "angularVelocity": -3.1724471295939874e-18, - "velocityX": -0.856052521568438, - "velocityY": 0.2796517636852234, - "timestamp": 2.177250883513288 - }, - { - "x": 2.6779735090707537, - "y": 5.538026332588319, - "heading": 5.594227372773024e-20, - "angularVelocity": -1.8557589065219566e-18, - "velocityX": -0.5712315727130863, - "velocityY": 0.18481817841642284, - "timestamp": 2.246057900729981 + "heading": -4.354722543751261e-18, + "angularVelocity": 4.754421944015892e-17, + "velocityX": 3.332116557247906, + "velocityY": -1.3615147608100584, + "timestamp": 0.8276684995630355 + }, + { + "x": 4.285701077560922, + "y": 4.938388492372474, + "heading": -4.775397572984342e-18, + "angularVelocity": -1.2147965427412122e-17, + "velocityX": 3.4133567656946284, + "velocityY": -1.4888942599373947, + "timestamp": 0.8622977575854378 + }, + { + "x": 4.403715149774036, + "y": 4.881600772971697, + "heading": -7.574216488503896e-18, + "angularVelocity": -8.082237608755476e-17, + "velocityX": 3.4079295645540304, + "velocityY": -1.6398768741750067, + "timestamp": 0.8969270156078402 + }, + { + "x": 4.519367006147829, + "y": 4.820144951729341, + "heading": -1.1070492199966305e-17, + "angularVelocity": -1.009630558414108e-16, + "velocityX": 3.3397151131270637, + "velocityY": -1.7746791225673963, + "timestamp": 0.9315562736302425 + }, + { + "x": 4.6324721141561005, + "y": 4.754119165100519, + "heading": -1.4960169278634737e-17, + "angularVelocity": -1.123234311330653e-16, + "velocityX": 3.266171857771331, + "velocityY": -1.9066474536101443, + "timestamp": 0.9661855316526449 + }, + { + "x": 4.742850082889288, + "y": 4.683628924272633, + "heading": -1.7795441089169654e-17, + "angularVelocity": -8.18750378278602e-17, + "velocityX": 3.187419397256003, + "velocityY": -2.0355689048343195, + "timestamp": 1.0008147896750472 + }, + { + "x": 4.850325220314879, + "y": 4.608787325463071, + "heading": -1.8976395039793544e-17, + "angularVelocity": -3.410277950107739e-17, + "velocityX": 3.1035934225348787, + "velocityY": -2.1612244409379606, + "timestamp": 1.0354440476974496 + }, + { + "x": 4.959315849281407, + "y": 4.536170434444089, + "heading": -1.7824338333790707e-17, + "angularVelocity": 3.3268304658955216e-17, + "velocityX": 3.1473567494868275, + "velocityY": -2.09698085278076, + "timestamp": 1.070073305719852 + }, + { + "x": 5.0711194796065495, + "y": 4.467963995966004, + "heading": -1.5727172733760973e-17, + "angularVelocity": 6.056051211588329e-17, + "velocityX": 3.228588676454312, + "velocityY": -1.9696188244622874, + "timestamp": 1.1047025637422543 + }, + { + "x": 5.1855579490393096, + "y": 4.4042772106470105, + "heading": -1.2316155333836473e-17, + "angularVelocity": 9.850102470338397e-17, + "velocityX": 3.3046757559381787, + "velocityY": -1.839103375469191, + "timestamp": 1.1393318217646566 + }, + { + "x": 5.302448702578612, + "y": 4.345211810292891, + "heading": -8.765491689722714e-18, + "angularVelocity": 1.0253363331714457e-16, + "velocityX": 3.3754911371096727, + "velocityY": -1.7056501850518684, + "timestamp": 1.173961079787059 + }, + { + "x": 5.421605230513571, + "y": 4.290862095789443, + "heading": -5.5119830878393785e-18, + "angularVelocity": 9.395259349127813e-17, + "velocityX": 3.4409206185669707, + "velocityY": -1.5694738382292028, + "timestamp": 1.2085903378094613 + }, + { + "x": 5.542837392691082, + "y": 4.241314822773421, + "heading": -2.184892589584252e-18, + "angularVelocity": 9.60774411078281e-17, + "velocityX": 3.500859362885684, + "velocityY": -1.4307922215361746, + "timestamp": 1.2432195958318637 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -4.379352373324341e-45, - "angularVelocity": -8.130315190317274e-19, - "velocityX": -0.2858478933472287, - "velocityY": 0.09169170474607218, - "timestamp": 2.3148649179466743 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -7.265203918119814e-45, - "angularVelocity": -4.19412391684243e-44, - "velocityX": 0, - "velocityY": -6.607621745560535e-42, - "timestamp": 2.3836719351633677 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 1.1869648608997422e-18, + "angularVelocity": 9.737019049910581e-17, + "velocityX": 3.5552114934161927, + "velocityY": -1.2898268911821018, + "timestamp": 1.277848853854266 + }, + { + "x": 5.934272395491136, + "y": 4.124337934678792, + "heading": 7.497166766936204e-18, + "angularVelocity": 8.587782714305931e-17, + "velocityX": 3.6516733021145136, + "velocityY": -0.9841085377661548, + "timestamp": 1.3513276797160874 + }, + { + "x": 6.2077530174936735, + "y": 4.075010140740357, + "heading": 1.3006105833327096e-17, + "angularVelocity": 7.497315045222091e-17, + "velocityX": 3.7218970063133985, + "velocityY": -0.6713198443208217, + "timestamp": 1.4248065055779087 + }, + { + "x": 6.4844286250858945, + "y": 4.049019985828209, + "heading": 1.5986683133968542e-17, + "angularVelocity": 4.0563757867422685e-17, + "velocityX": 3.7653787243758563, + "velocityY": -0.3537094476852878, + "timestamp": 1.49828533143973 + }, + { + "x": 6.743277363747608, + "y": 4.038426394199003, + "heading": 1.2719246482061777e-17, + "angularVelocity": -4.446773085420891e-17, + "velocityX": 3.522766397335807, + "velocityY": -0.1441720319419269, + "timestamp": 1.5717641573015513 + }, + { + "x": 6.978672822837657, + "y": 4.030025299692623, + "heading": 8.975092196512461e-18, + "angularVelocity": -5.095555408833389e-17, + "velocityX": 3.2035822065626665, + "velocityY": -0.11433354313769856, + "timestamp": 1.6452429831633726 + }, + { + "x": 7.1905526925662695, + "y": 4.022995759457438, + "heading": 5.368720420708233e-18, + "angularVelocity": -4.9080421924352456e-17, + "velocityX": 2.8835500192539434, + "velocityY": -0.09566756344750614, + "timestamp": 1.718721809025194 + }, + { + "x": 7.3789017577018114, + "y": 4.017045849522277, + "heading": 1.5887833889908863e-18, + "angularVelocity": -5.144253446326957e-17, + "velocityX": 2.563310762337649, + "velocityY": -0.08097448299391075, + "timestamp": 1.7922006348870152 + }, + { + "x": 7.54371362899692, + "y": 4.012025997027571, + "heading": -2.7250521222406746e-18, + "angularVelocity": -5.870855257463982e-17, + "velocityX": 2.2429845518359373, + "velocityY": -0.06831699385270525, + "timestamp": 1.8656794607488365 + }, + { + "x": 7.684984887810538, + "y": 4.007845278835319, + "heading": -2.10421445779977e-18, + "angularVelocity": 8.449205021435769e-18, + "velocityX": 1.9226118158077505, + "velocityY": -0.056896910684361174, + "timestamp": 1.9391582866106578 + }, + { + "x": 7.8027134339680275, + "y": 4.0044425825275765, + "heading": -1.435402908569355e-18, + "angularVelocity": 9.102099024937298e-18, + "velocityX": 1.6022104977409413, + "velocityY": -0.0463085285840207, + "timestamp": 2.012637112472479 + }, + { + "x": 7.896897856926731, + "y": 4.001774011196325, + "heading": -8.461914935171916e-19, + "angularVelocity": 8.01879191918765e-18, + "velocityX": 1.2817899830873647, + "velocityY": -0.03631755543113603, + "timestamp": 2.0861159383343004 + }, + { + "x": 7.967537148527336, + "y": 3.999806507404438, + "heading": -3.275924545852618e-19, + "angularVelocity": 7.057802473697343e-18, + "velocityX": 0.9613557480279206, + "velocityY": -0.026776472933672434, + "timestamp": 2.1595947641961217 + }, + { + "x": 8.014630554462236, + "y": 3.9985142796609896, + "heading": -5.3367647403100726e-20, + "angularVelocity": 3.732024892420789e-18, + "velocityX": 0.6409112473226126, + "velocityY": -0.01758639619362703, + "timestamp": 2.233073590057943 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 7.342940745159904e-44, + "angularVelocity": 7.262996758203436e-19, + "velocityX": 0.320458791984776, + "velocityY": -0.008677813220192914, + "timestamp": 2.3065524159197643 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 1.3038141825006824e-44, + "angularVelocity": -8.034632634038792e-43, + "velocityX": -5.7118786505402475e-43, + "velocityY": 0, + "timestamp": 2.3800312417815856 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.5.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.5.traj deleted file mode 100644 index 5c65b031..00000000 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.5.traj +++ /dev/null @@ -1,382 +0,0 @@ -{ - "samples": [ - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -7.265203918119814e-45, - "angularVelocity": -4.19412391684243e-44, - "velocityX": 0, - "velocityY": -6.607621745560535e-42, - "timestamp": 0 - }, - { - "x": 2.6751665057839835, - "y": 5.538944468629318, - "heading": 4.149201789004472e-19, - "angularVelocity": 6.513823637389646e-18, - "velocityX": 0.2647058041795455, - "velocityY": -0.08463157955674916, - "timestamp": 0.06369840542178373 - }, - { - "x": 2.7088624712986666, - "y": 5.5280798374619815, - "heading": 1.747454129766702e-18, - "angularVelocity": 2.0919423995667872e-17, - "velocityX": 0.5289922925316967, - "velocityY": -0.1705636286402431, - "timestamp": 0.12739681084356747 - }, - { - "x": 2.7593613029745305, - "y": 5.511644735540107, - "heading": 3.0764256336987273e-18, - "angularVelocity": 2.086349721209079e-17, - "velocityX": 0.79278015425161, - "velocityY": -0.2580143382404696, - "timestamp": 0.1910952162653512 - }, - { - "x": 2.8266246280660527, - "y": 5.489524723354374, - "heading": 4.563957990922327e-18, - "angularVelocity": 2.3352740894748934e-17, - "velocityX": 1.0559656030026554, - "velocityY": -0.3472616314217525, - "timestamp": 0.25479362168713493 - }, - { - "x": 2.9106051995536757, - "y": 5.461582331644454, - "heading": 6.988237520643839e-18, - "angularVelocity": 3.8058716127490844e-17, - "velocityX": 1.31840932173323, - "velocityY": -0.43866705178720633, - "timestamp": 0.31849202710891866 - }, - { - "x": 3.0112434875257597, - "y": 5.427649372766819, - "heading": 9.422486137058152e-18, - "angularVelocity": 3.821522062123431e-17, - "velocityX": 1.5799184815647955, - "velocityY": -0.5327128466237975, - "timestamp": 0.3821904325307024 - }, - { - "x": 3.128462311204818, - "y": 5.387515427846556, - "heading": 1.2002270824368104e-17, - "angularVelocity": 4.0499988504071535e-17, - "velocityX": 1.8402159819054258, - "velocityY": -0.6300620031932208, - "timestamp": 0.44588883795248613 - }, - { - "x": 3.2621579049769087, - "y": 5.340909820689257, - "heading": 1.4607724763792015e-17, - "angularVelocity": 4.0902969582545765e-17, - "velocityX": 2.0988844679362657, - "velocityY": -0.7316604999559294, - "timestamp": 0.5095872433742699 - }, - { - "x": 3.4121839694430376, - "y": 5.287471786804592, - "heading": 1.2983801342724242e-17, - "angularVelocity": -2.5493941493744293e-17, - "velocityX": 2.3552562026116504, - "velocityY": -0.8389226312781332, - "timestamp": 0.5732856487960536 - }, - { - "x": 3.5783204640757695, - "y": 5.226697519809262, - "heading": 1.1414351740343512e-17, - "angularVelocity": -2.4638758097451353e-17, - "velocityX": 2.608173525422576, - "velocityY": -0.9540940089929807, - "timestamp": 0.6369840542178373 - }, - { - "x": 3.7602044322338806, - "y": 5.157837260218168, - "heading": 1.0662286375073935e-17, - "angularVelocity": -1.1806659213675993e-17, - "velocityX": 2.855392799140749, - "velocityY": -1.081035845954548, - "timestamp": 0.7006824596396211 - }, - { - "x": 3.9571467965572817, - "y": 5.079670509622147, - "heading": 1.0033604735305209e-17, - "angularVelocity": -9.869660560666429e-18, - "velocityX": 3.091794261085985, - "velocityY": -1.2271382631705257, - "timestamp": 0.7643808650614048 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 9.614004929327239e-18, - "angularVelocity": -6.58728900981987e-18, - "velocityX": 3.3023160854502263, - "velocityY": -1.4085550989874989, - "timestamp": 0.8280792704831885 - }, - { - "x": 4.2842075611264425, - "y": 4.936417577245955, - "heading": 8.433110849470374e-18, - "angularVelocity": -3.4077840923717525e-17, - "velocityX": 3.3679341946759727, - "velocityY": -1.5447568972758872, - "timestamp": 0.8627321137340349 - }, - { - "x": 4.401356584826297, - "y": 4.877666966713146, - "heading": 7.494016874909782e-18, - "angularVelocity": -2.7100055477775347e-17, - "velocityX": 3.3806468015288584, - "velocityY": -1.695405196841212, - "timestamp": 0.8973849569848813 - }, - { - "x": 4.516063976001338, - "y": 4.814281195629901, - "heading": 5.449898069000543e-18, - "angularVelocity": -5.898848735476586e-17, - "velocityX": 3.310186998068025, - "velocityY": -1.8291650882556743, - "timestamp": 0.9320378002357277 - }, - { - "x": 4.628146410125642, - "y": 4.746361612927903, - "heading": 3.873819251911276e-18, - "angularVelocity": -4.5481948066433736e-17, - "velocityX": 3.234436877602133, - "velocityY": -1.9600002865663548, - "timestamp": 0.9666906434865741 - }, - { - "x": 4.737424855337868, - "y": 4.674016859733605, - "heading": 3.562150006954802e-18, - "angularVelocity": -8.994045386127452e-18, - "velocityX": 3.1535203163901007, - "velocityY": -2.0877003560892837, - "timestamp": 1.0013434867374205 - }, - { - "x": 4.844483096398323, - "y": 4.598425359059355, - "heading": 4.67240858769656e-18, - "angularVelocity": 3.2039465642249735e-17, - "velocityX": 3.0894504178337554, - "velocityY": -2.181393893916723, - "timestamp": 1.0359963299882669 - }, - { - "x": 4.954476829355994, - "y": 4.5271728465414585, - "heading": 6.725892368510384e-18, - "angularVelocity": 5.925873862496592e-17, - "velocityX": 3.174161847599147, - "velocityY": -2.0561808450207577, - "timestamp": 1.0706491732391132 - }, - { - "x": 5.06723038345343, - "y": 4.460373312794278, - "heading": 8.84315284493419e-18, - "angularVelocity": 6.109918488065446e-17, - "velocityX": 3.2538038302147596, - "velocityY": -1.9276782936288406, - "timestamp": 1.1053020164899596 - }, - { - "x": 5.18256360847748, - "y": 4.398133545147612, - "heading": 1.1174088851587773e-17, - "angularVelocity": 6.726536087617125e-17, - "velocityX": 3.32824709906689, - "velocityY": -1.796094109684512, - "timestamp": 1.139954859740806 - }, - { - "x": 5.300292214676822, - "y": 4.340553023175947, - "heading": 1.351328882085302e-17, - "angularVelocity": 6.750383950696769e-17, - "velocityX": 3.3973721967667427, - "velocityY": -1.6616391779124182, - "timestamp": 1.1746077029916524 - }, - { - "x": 5.420228077668345, - "y": 4.287723772710641, - "heading": 1.5740193529975693e-17, - "angularVelocity": 6.4263260968298e-17, - "velocityX": 3.461068464810413, - "velocityY": -1.5245285959043573, - "timestamp": 1.2092605462424988 - }, - { - "x": 5.542179542504676, - "y": 4.239730223009142, - "heading": 1.83851008704834e-17, - "angularVelocity": 7.632583916308507e-17, - "velocityX": 3.5192340193716003, - "velocityY": -1.3849815830141698, - "timestamp": 1.2439133894933452 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 2.1337719863838943e-17, - "angularVelocity": 8.520567769813293e-17, - "velocityX": 3.571775782441806, - "velocityY": -1.2432211735943428, - "timestamp": 1.2785662327441916 - }, - { - "x": 5.936838928321791, - "y": 4.127579524293423, - "heading": 2.7128761029914745e-17, - "angularVelocity": 7.834427123009397e-17, - "velocityX": 3.6647054686449687, - "velocityY": -0.9344094480089555, - "timestamp": 1.3524840964903202 - }, - { - "x": 6.212625572925942, - "y": 4.081838891930782, - "heading": 3.048119806681363e-17, - "angularVelocity": 4.535354333849354e-17, - "velocityX": 3.730987756239054, - "velocityY": -0.6188034941017404, - "timestamp": 1.4264019602364488 - }, - { - "x": 6.491306353239061, - "y": 4.059759739930169, - "heading": 2.990728340154358e-17, - "angularVelocity": -7.764221477519524e-18, - "velocityX": 3.770141156544387, - "velocityY": -0.2986984590956799, - "timestamp": 1.5003198239825775 - }, - { - "x": 6.752875972710039, - "y": 4.054277278014817, - "heading": 2.289191300833474e-17, - "angularVelocity": -9.490764529266178e-17, - "velocityX": 3.538652312373937, - "velocityY": -0.07416964773468503, - "timestamp": 1.574237687728706 - }, - { - "x": 6.991092096287726, - "y": 4.05357587432536, - "heading": 1.9322766501808003e-17, - "angularVelocity": -4.8285303790501985e-17, - "velocityX": 3.2227138543376115, - "velocityY": -0.009488960501710962, - "timestamp": 1.6481555514748347 - }, - { - "x": 7.205567187241114, - "y": 4.0550211409111485, - "heading": 1.4185957310010018e-17, - "angularVelocity": -6.949347466859182e-17, - "velocityX": 2.9015325942048964, - "velocityY": 0.01955233163599203, - "timestamp": 1.7220734152209634 - }, - { - "x": 7.396227777452996, - "y": 4.057522153925314, - "heading": 7.96211441465592e-18, - "angularVelocity": -8.419944219072616e-17, - "velocityX": 2.579357418481523, - "velocityY": 0.03383502833300578, - "timestamp": 1.795991278967092 - }, - { - "x": 7.5630549716186115, - "y": 4.0604862007272615, - "heading": 9.124881669040973e-19, - "angularVelocity": -9.537107663127069e-17, - "velocityX": 2.2569266170703433, - "velocityY": 0.040099194588843756, - "timestamp": 1.8699091427132206 - }, - { - "x": 7.70604444565388, - "y": 4.0635415850381555, - "heading": -6.759455196513469e-18, - "angularVelocity": -1.0379011208666613e-16, - "velocityX": 1.934437317160139, - "velocityY": 0.04133485677276264, - "timestamp": 1.9438270064593492 - }, - { - "x": 7.825196583277639, - "y": 4.066433602906457, - "heading": -4.535875150648457e-18, - "angularVelocity": 3.0081768238080076e-17, - "velocityX": 1.6119532084015247, - "velocityY": 0.03912474903542123, - "timestamp": 2.017744870205478 - }, - { - "x": 7.920513375065405, - "y": 4.068976876297905, - "heading": -2.8949605954376388e-18, - "angularVelocity": 2.219916096123338e-17, - "velocityX": 1.2894960292025357, - "velocityY": 0.03440674909358656, - "timestamp": 2.0916627339516065 - }, - { - "x": 7.991997299424134, - "y": 4.071030463180751, - "heading": -1.5603341535814708e-18, - "angularVelocity": 1.8055533185319825e-17, - "velocityX": 0.9670723792051263, - "velocityY": 0.02778201071799316, - "timestamp": 2.165580597697735 - }, - { - "x": 8.03965088919926, - "y": 4.07248360094205, - "heading": -5.821232156931401e-19, - "angularVelocity": 1.3233755526918469e-17, - "velocityX": 0.6446829948819025, - "velocityY": 0.019658817066051913, - "timestamp": 2.2394984614438638 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 8.014500942600024e-44, - "angularVelocity": 7.875271094040935e-18, - "velocityX": 0.322326324020543, - "velocityY": 0.0103270696803873, - "timestamp": 2.3134163251899924 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": -7.373298274013602e-44, - "angularVelocity": -1.7505506389116263e-42, - "velocityX": -2.259320082617454e-45, - "velocityY": 0, - "timestamp": 2.387334188936121 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.traj index 19f29638..5b76f9bb 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.traj @@ -4,1549 +4,1567 @@ "x": 1.2899744510650637, "y": 5.590954303741455, "heading": 0, - "angularVelocity": 2.3560143253921187e-30, + "angularVelocity": -1.254857983919809e-31, "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 1.3060574270397207, - "y": 5.572382522338357, - "heading": -0.010694879272561806, - "angularVelocity": -0.14212355715817127, - "velocityX": 0.21372562484439114, - "velocityY": -0.24679919880328743, - "timestamp": 0.07525057412377358 - }, - { - "x": 1.3382235764672503, - "y": 5.535239265365289, - "heading": -0.0320761034546414, - "angularVelocity": -0.2841337017193578, - "velocityX": 0.42745387396382534, - "velocityY": -0.49359433341007053, - "timestamp": 0.15050114824754715 - }, - { - "x": 1.3864734146735522, - "y": 5.47952484840506, - "heading": -0.06412157549775398, - "angularVelocity": -0.42585020003622126, - "velocityX": 0.6411889712155109, - "velocityY": -0.7403852742575773, - "timestamp": 0.22575172237132074 - }, - { - "x": 1.4508078083146234, - "y": 5.405239688642535, - "heading": -0.10679814197023921, - "angularVelocity": -0.5671261245497303, - "velocityX": 0.8549355854022526, - "velocityY": -0.9871706711626929, - "timestamp": 0.3010022964950943 - }, - { - "x": 1.5312280128772082, - "y": 5.3123844280164345, - "heading": -0.16006528510741636, - "angularVelocity": -0.7078636111159992, - "velocityX": 1.0686988836704838, - "velocityY": -1.2339475373861764, - "timestamp": 0.37625287061886786 - }, - { - "x": 1.6277357374217547, - "y": 5.200960102506698, - "heading": -0.22387989152223717, - "angularVelocity": -0.8480281666778853, - "velocityX": 1.2824848935314492, - "velocityY": -1.4807106365400295, - "timestamp": 0.4515034447426414 - }, - { - "x": 1.7403333706916029, - "y": 5.070968456330507, - "heading": -0.29820152963685287, - "angularVelocity": -0.9876554296244249, - "velocityX": 1.496302647246288, - "velocityY": -1.7274505569061047, - "timestamp": 0.526754018866415 - }, - { - "x": 1.8690253649035526, - "y": 4.922413220845796, - "heading": -0.38299660252732504, - "angularVelocity": -1.1268362252301867, - "velocityX": 1.7101795661770514, - "velocityY": -1.9741408915022984, - "timestamp": 0.6020045929901886 - }, - { - "x": 2.021841436297539, - "y": 4.7786139697645345, - "heading": -0.46243583568438273, - "angularVelocity": -1.055662818268618, - "velocityX": 2.0307628636084356, - "velocityY": -1.9109389231128335, - "timestamp": 0.6772551671139622 - }, - { - "x": 2.158572983780592, - "y": 4.6533861744648695, - "heading": -0.5314192950572162, - "angularVelocity": -0.9167167184470773, - "velocityX": 1.8170166683292757, - "velocityY": -1.6641440514810064, - "timestamp": 0.7525057412377357 - }, - { - "x": 2.2792163174605955, - "y": 4.5467264938443535, - "heading": -0.5899522434559231, - "angularVelocity": -0.777840555764763, - "velocityX": 1.6032214383174732, - "velocityY": -1.4173935795391805, - "timestamp": 0.8277563153615093 - }, - { - "x": 2.3837699374210346, - "y": 4.458633436191598, - "heading": -0.6380354100231267, - "angularVelocity": -0.6389740826105205, - "velocityX": 1.3894062760138433, - "velocityY": -1.1706629308520584, - "timestamp": 0.9030068894852828 - }, - { - "x": 2.472232988532522, - "y": 4.389106089275013, - "heading": -0.6756678010096412, - "angularVelocity": -0.5000944035873048, - "velocityX": 1.1755797499519767, - "velocityY": -0.9239444047507308, - "timestamp": 0.9782574636090564 - }, - { - "x": 2.5446049348789113, - "y": 4.338143851138861, - "heading": -0.7028477866363936, - "angularVelocity": -0.36119306653537947, - "velocityX": 0.9617461021356389, - "velocityY": -0.6772338779998112, - "timestamp": 1.05350803773283 - }, - { - "x": 2.6008854472033804, - "y": 4.305746331042048, - "heading": -0.7195735501934102, - "angularVelocity": -0.222267587342223, - "velocityX": 0.7479080788470353, - "velocityY": -0.43052854378169836, - "timestamp": 1.1287586118566038 - }, - { - "x": 2.6410743549129108, - "y": 4.291913303700176, - "heading": -0.7258432586618085, - "angularVelocity": -0.0833177492798597, - "velocityX": 0.5340677885590666, - "velocityY": -0.18382620335314903, - "timestamp": 1.2040091859803774 + "x": 1.3060574276601378, + "y": 5.572382522924401, + "heading": -0.010694878831235951, + "angularVelocity": -0.1421235514303064, + "velocityX": 0.21372563329485256, + "velocityY": -0.24679919125306551, + "timestamp": 0.07525057405130896 + }, + { + "x": 1.3382235783773917, + "y": 5.535239267164631, + "heading": -0.032076102097718266, + "angularVelocity": -0.2841336898257091, + "velocityX": 0.4274538915144429, + "velocityY": -0.4935943177620345, + "timestamp": 0.15050114810261792 + }, + { + "x": 1.3864734186078633, + "y": 5.479524852099925, + "heading": -0.06412157270911513, + "angularVelocity": -0.42585018142040665, + "velocityX": 0.6411889987319064, + "velocityY": -0.7403852497811617, + "timestamp": 0.22575172215392686 + }, + { + "x": 1.4508078150986874, + "y": 5.405239694992128, + "heading": -0.1067981371766019, + "angularVelocity": -0.567126098451675, + "velocityX": 0.8549356240955547, + "velocityY": -0.9871706368349642, + "timestamp": 0.30100229620523583 + }, + { + "x": 1.5312280234732587, + "y": 5.3123844378954015, + "heading": -0.16006527765002243, + "angularVelocity": -0.7078635763993126, + "velocityX": 1.068698935356638, + "velocityY": -1.233947491673002, + "timestamp": 0.3762528702565448 + }, + { + "x": 1.6277357530197398, + "y": 5.200960116982037, + "heading": -0.2238798805982425, + "angularVelocity": -0.8480281214272752, + "velocityX": 1.2824849612365503, + "velocityY": -1.4807105768852864, + "timestamp": 0.4515034443078538 + }, + { + "x": 1.7403333929369436, + "y": 5.0709684768538175, + "heading": -0.2982015141504559, + "angularVelocity": -0.9876553699462992, + "velocityX": 1.4963027370230912, + "velocityY": -1.727450478198834, + "timestamp": 0.5267540183591627 + }, + { + "x": 1.8690253968085462, + "y": 4.922413250022354, + "heading": -0.3829965804922361, + "angularVelocity": -1.1268361392905244, + "velocityX": 1.7101796961898603, + "velocityY": -1.9741407784113794, + "timestamp": 0.6020045924104717 + }, + { + "x": 2.02184146077192, + "y": 4.778613992669738, + "heading": -0.4624358186091319, + "angularVelocity": -1.0556628851958798, + "velocityX": 2.0307627668195027, + "velocityY": -1.9109390082922875, + "timestamp": 0.6772551664617806 + }, + { + "x": 2.158573003278503, + "y": 4.653386193108499, + "heading": -0.5314192813425986, + "angularVelocity": -0.9167167639888999, + "velocityX": 1.817016603947309, + "velocityY": -1.664144109715068, + "timestamp": 0.7525057405130896 + }, + { + "x": 2.279216333070278, + "y": 4.546726509082428, + "heading": -0.5899522323979595, + "angularVelocity": -0.7778405918177544, + "velocityX": 1.6032213881911423, + "velocityY": -1.417393626160111, + "timestamp": 0.8277563145643986 + }, + { + "x": 2.3837699497753926, + "y": 4.458633448495562, + "heading": -0.6380354012157764, + "angularVelocity": -0.6389741131339646, + "velocityX": 1.3894062340921998, + "velocityY": -1.1706629709704055, + "timestamp": 0.9030068886157075 + }, + { + "x": 2.4722329980367945, + "y": 4.3891060989240405, + "heading": -0.6756677941957627, + "angularVelocity": -0.5000944305598923, + "velocityX": 1.1755797132095953, + "velocityY": -0.9239444409216138, + "timestamp": 0.9782574626670165 + }, + { + "x": 2.5446049418017402, + "y": 4.338143858296765, + "heading": -0.7028477816482255, + "angularVelocity": -0.36119309114484527, + "velocityX": 0.9617460687572824, + "velocityY": -0.6772339117562157, + "timestamp": 1.0535080367183254 + }, + { + "x": 2.6008854517223408, + "y": 4.305746335795738, + "heading": -0.7195735469228228, + "angularVelocity": -0.22226761038098528, + "velocityX": 0.7479080476225236, + "velocityY": -0.430528576145586, + "timestamp": 1.1287586107696344 + }, + { + "x": 2.641074357140525, + "y": 4.291913306081633, + "heading": -0.7258432570432893, + "angularVelocity": -0.08331777131422959, + "velocityX": 0.5340677586239448, + "velocityY": -0.1838262350545031, + "timestamp": 1.2040091848209433 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.05565596827444861, - "velocityX": 0.3202270361150376, - "velocityY": 0.06287505455011895, - "timestamp": 1.279259760104151 - }, - { - "x": 2.6731093005797457, - "y": 4.320288364477002, - "heading": -0.7067987753568107, - "angularVelocity": 0.19580392065911026, - "velocityX": 0.10461717847139641, - "velocityY": 0.3116194636271141, - "timestamp": 1.3551333161029784 - }, - { - "x": 2.664687962821916, - "y": 4.362805446311989, - "heading": -0.6813130877895294, - "angularVelocity": 0.3358968382514228, - "velocityX": -0.11099173681211887, - "velocityY": 0.5603675915105346, - "timestamp": 1.4310068721018057 - }, - { - "x": 2.6399077578590466, - "y": 4.424196334908807, - "heading": -0.645202192832585, - "angularVelocity": 0.47593518560166614, - "velocityX": -0.32659870276641295, - "velocityY": 0.8091210144040187, - "timestamp": 1.506880428100633 - }, - { - "x": 2.5987689088828363, - "y": 4.504461553370171, - "heading": -0.5984695308975294, - "angularVelocity": 0.6159281889382251, - "velocityX": -0.5422027270940676, - "velocityY": 1.0578813316982019, - "timestamp": 1.5827539840994602 - }, - { - "x": 2.541271709317337, - "y": 4.60360172553535, - "heading": -0.5411163068190713, - "angularVelocity": 0.7559053127670017, - "velocityX": -0.7578028841452508, - "velocityY": 1.3066498710827958, - "timestamp": 1.6586275400982875 - }, - { - "x": 2.4674164806885255, - "y": 4.721617486978178, - "heading": -0.47313850163583826, - "angularVelocity": 0.8959354057929544, - "velocityX": -0.9733988035419044, - "velocityY": 1.5554267872170555, - "timestamp": 1.7345010960971148 - }, - { - "x": 2.377203340708699, - "y": 4.858509175719519, - "heading": -0.39452181655964974, - "angularVelocity": 1.0361539543083473, - "velocityX": -1.188993171512905, - "velocityY": 1.804208158404987, - "timestamp": 1.810374652095942 - }, - { - "x": 2.270630715712783, - "y": 5.014275370945325, - "heading": -0.30523452580590593, - "angularVelocity": 1.1767906430312367, - "velocityX": -1.4046082801148865, - "velocityY": 2.052970803512055, - "timestamp": 1.8862482080947693 - }, - { - "x": 2.156210238547871, - "y": 5.1464603718429265, - "heading": -0.22910381899713708, - "angularVelocity": 1.0033892020445663, - "velocityX": -1.508041578600413, - "velocityY": 1.7421748481347223, - "timestamp": 1.9621217640935966 - }, - { - "x": 2.0581337448378676, - "y": 5.259758714146455, - "heading": -0.16374376524716946, - "angularVelocity": 0.8614339065879271, - "velocityX": -1.2926307778416481, - "velocityY": 1.4932520403643688, - "timestamp": 2.0379953200924237 - }, - { - "x": 1.9764031908936424, - "y": 5.354172852013204, - "heading": -0.10920543384793971, - "angularVelocity": 0.7188055269306859, - "velocityX": -1.077194193248253, - "velocityY": 1.24436157795548, - "timestamp": 2.1138688760912507 - }, - { - "x": 1.9110187094739324, - "y": 5.42970361761196, - "heading": -0.0655341593932664, - "angularVelocity": 0.5755796453747686, - "velocityX": -0.8617558589167408, - "velocityY": 0.995482083384748, - "timestamp": 2.189742432090078 - }, - { - "x": 1.8619802018876241, - "y": 5.486351463195586, - "heading": -0.03276467101544991, - "angularVelocity": 0.4318960400201182, - "velocityX": -0.646318825315813, - "velocityY": 0.7466085494271616, - "timestamp": 2.265615988088905 - }, - { - "x": 1.8292876471115809, - "y": 5.524116647521472, - "heading": -0.010918058172762177, - "angularVelocity": 0.2879344793491674, - "velocityX": -0.43088206879563057, - "velocityY": 0.4977384258464482, - "timestamp": 2.341489544087732 + "angularVelocity": 0.05565594681973357, + "velocityX": 0.3202270068209183, + "velocityY": 0.06287502296377193, + "timestamp": 1.2792597588722523 + }, + { + "x": 2.6731092983432756, + "y": 4.320288362117908, + "heading": -0.7067987769295019, + "angularVelocity": 0.19580389973285955, + "velocityX": 0.10461714888899723, + "velocityY": 0.3116194322188291, + "timestamp": 1.3551333149479476 + }, + { + "x": 2.664687958259504, + "y": 4.362805441582899, + "heading": -0.6813130909046112, + "angularVelocity": 0.3358968175826016, + "velocityX": -0.11099176735530734, + "velocityY": 0.5603675597065643, + "timestamp": 1.431006871023643 + }, + { + "x": 2.6399077508131867, + "y": 4.424196327740775, + "heading": -0.6452021974872086, + "angularVelocity": 0.4759351648285144, + "velocityX": -0.32659873516708793, + "velocityY": 0.8091209814393522, + "timestamp": 1.5068804270993383 + }, + { + "x": 2.5987688991007616, + "y": 4.504461543612991, + "heading": -0.5984695371274339, + "angularVelocity": 0.6159281675522111, + "velocityX": -0.5422027626077326, + "velocityY": 1.057881296501775, + "timestamp": 1.5827539831750337 + }, + { + "x": 2.541271696403367, + "y": 4.60360171291691, + "heading": -0.5411163147183906, + "angularVelocity": 0.7559052899984986, + "velocityX": -0.7578029246555089, + "velocityY": 1.3066498320479574, + "timestamp": 1.658627539250729 + }, + { + "x": 2.46741646400873, + "y": 4.721617471023204, + "heading": -0.47313851139747326, + "angularVelocity": 0.8959353803401775, + "velocityX": -0.9733988521888661, + "velocityY": 1.55542674166613, + "timestamp": 1.7345010953264244 + }, + { + "x": 2.3772033191525277, + "y": 4.85850915554647, + "heading": -0.39452182857786383, + "angularVelocity": 1.036153923517168, + "velocityX": -1.1889932345783751, + "velocityY": 1.8042081009834092, + "timestamp": 1.8103746514021197 + }, + { + "x": 2.2706306867395725, + "y": 5.014275344454255, + "heading": -0.30523454109050485, + "angularVelocity": 1.1767905987884617, + "velocityX": -1.404608376447531, + "velocityY": 2.0529707181614056, + "timestamp": 1.886248207477815 + }, + { + "x": 2.1562102192291532, + "y": 5.146460354029385, + "heading": -0.2291038294161038, + "angularVelocity": 1.0033892651564627, + "velocityX": -1.5080414498275487, + "velocityY": 1.7421749607384658, + "timestamp": 1.9621217635535104 + }, + { + "x": 2.0581337319934665, + "y": 5.259758702235753, + "heading": -0.16374377227039283, + "angularVelocity": 0.8614339504706835, + "velocityX": -1.292630691201352, + "velocityY": 1.4932521166502593, + "timestamp": 2.0379953196292058 + }, + { + "x": 1.976403182773577, + "y": 5.354172844449837, + "heading": -0.1092054383318089, + "angularVelocity": 0.7188055596708355, + "velocityX": -1.077194129890758, + "velocityY": 1.244361633992147, + "timestamp": 2.113868875704901 + }, + { + "x": 1.9110187048049692, + "y": 5.429703613246766, + "heading": -0.0655341619901584, + "angularVelocity": 0.575579669661773, + "velocityX": -0.8617558125585749, + "velocityY": 0.9954821245277466, + "timestamp": 2.1897424317805965 + }, + { + "x": 1.861980199634934, + "y": 5.486351461082553, + "heading": -0.03276467227497517, + "angularVelocity": 0.4318960572088904, + "velocityX": -0.6463187928148273, + "velocityY": 0.7466085783539637, + "timestamp": 2.265615987856292 + }, + { + "x": 1.8292876463834091, + "y": 5.5241166468364415, + "heading": -0.01091805858135444, + "angularVelocity": 0.2879344902726512, + "velocityX": -0.4308820482661345, + "velocityY": 0.4977384441630725, + "timestamp": 2.341489543931987 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 2.8937989389363637e-30, - "angularVelocity": 0.14389806868982188, - "velocityX": -0.21544335593050407, - "velocityY": 0.24886958055725772, - "timestamp": 2.417363100086559 + "heading": 2.72551505430432e-32, + "angularVelocity": 0.1438980739292335, + "velocityX": -0.21544334611501834, + "velocityY": 0.2488695893337634, + "timestamp": 2.4173631000076825 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.8511737998863575e-30, - "angularVelocity": -5.603397953128688e-29, + "heading": -2.2798037235511704e-30, + "angularVelocity": -3.1967802998211905e-29, "velocityX": 0, - "velocityY": 2.843834189498754e-31, - "timestamp": 2.493236656085386 - }, - { - "x": 1.855209395178807, - "y": 5.543066072468525, - "heading": -4.900273115413656e-27, - "angularVelocity": -4.980371579437602e-26, - "velocityX": 0.42942883009919236, - "velocityY": 0.0006787122433542538, - "timestamp": 2.591665542633167 - }, - { - "x": 1.939745796780934, - "y": 5.543199682246804, - "heading": 4.528417643864408e-20, - "angularVelocity": 4.600700356082414e-19, - "velocityX": 0.8588576440015887, - "velocityY": 0.0013574244611094727, - "timestamp": 2.6900944291809483 - }, - { - "x": 2.066550395198543, - "y": 5.5434000969079245, - "heading": 6.339784356887693e-20, - "angularVelocity": 1.8402795932716144e-19, - "velocityX": 1.2882864255103954, - "velocityY": 0.0020361366276666244, - "timestamp": 2.7885233157287295 - }, - { - "x": 2.2356231808662415, - "y": 5.543667316436768, - "heading": 5.434100687876101e-20, - "angularVelocity": -9.20140114123854e-20, - "velocityX": 1.7177151098384495, - "velocityY": 0.002714848640629602, - "timestamp": 2.8869522022765106 - }, - { - "x": 2.4046959665339394, - "y": 5.543934535965611, - "heading": 4.338082196797977e-27, - "angularVelocity": -5.5208388966484585e-19, - "velocityX": 1.7177151098384498, - "velocityY": 0.0027148486406296025, - "timestamp": 2.9853810888242918 - }, - { - "x": 2.5315005649515485, - "y": 5.544134950626731, - "heading": -2.717050695881145e-20, - "angularVelocity": -2.760420466983953e-19, - "velocityX": 1.2882864255103954, - "velocityY": 0.0020361366276666244, - "timestamp": 3.083809975372073 - }, - { - "x": 2.6160369665536756, - "y": 5.54426856040501, - "heading": 3.0657579714860263e-27, - "angularVelocity": 2.760420337720658e-19, - "velocityX": 0.8588576440015887, - "velocityY": 0.0013574244611094727, - "timestamp": 3.182238861919854 - }, - { - "x": 2.6583051681518555, + "velocityY": 3.158110370432563e-31, + "timestamp": 2.493236656083378 + }, + { + "x": 1.8507557561824508, + "y": 5.5430527114919315, + "heading": 1.2021210509026438e-21, + "angularVelocity": 1.2912282770144636e-20, + "velocityX": 0.40617567078743805, + "velocityY": 0.0005740544395093852, + "timestamp": 2.5863356901257513 + }, + { + "x": 1.9263848800936068, + "y": 5.543159599317717, + "heading": 3.6194046492049965e-21, + "angularVelocity": 2.5964647465645482e-20, + "velocityX": 0.8123513276919048, + "velocityY": 0.00114810885939775, + "timestamp": 2.6794347241681247 + }, + { + "x": 2.039828562987611, + "y": 5.543319931052194, + "heading": -1.5854010106883968e-20, + "angularVelocity": -2.0916881637274291e-19, + "velocityX": 1.2185269596070247, + "velocityY": 0.0017221632439682793, + "timestamp": 2.772533758210498 + }, + { + "x": 2.1910867994360017, + "y": 5.543533706687691, + "heading": 4.514872599726518e-21, + "angularVelocity": 2.1878726150197974e-19, + "velocityX": 1.624702533213676, + "velocityY": 0.0022962175461305355, + "timestamp": 2.8656327922528715 + }, + { + "x": 2.380159562296481, + "y": 5.5438009261858445, + "heading": -2.4606104769465887e-21, + "angularVelocity": -7.492540764169747e-20, + "velocityX": 2.0308778152781284, + "velocityY": 0.002870271436251635, + "timestamp": 2.958731826295245 + }, + { + "x": 2.5314177987448714, + "y": 5.544014701821341, + "heading": 1.8729877149138444e-20, + "angularVelocity": 2.2761232534851326e-19, + "velocityX": 1.6247025332136762, + "velocityY": 0.002296217546130536, + "timestamp": 3.0518308603376183 + }, + { + "x": 2.6448614816388756, + "y": 5.544175033555818, + "heading": 3.8900824798152035e-20, + "angularVelocity": 2.1666119156330769e-19, + "velocityX": 1.218526959607025, + "velocityY": 0.0017221632439682793, + "timestamp": 3.1449298943799917 + }, + { + "x": 2.7204906055500317, + "y": 5.544281921381604, + "heading": 1.1738818455912713e-20, + "angularVelocity": -2.9175390079640015e-19, + "velocityX": 0.8123513276919049, + "velocityY": 0.00114810885939775, + "timestamp": 3.238028928422365 + }, + { + "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": 1.737489718984482e-38, - "angularVelocity": -3.114693341043594e-26, - "velocityX": 0.42942883009919236, - "velocityY": 0.0006787122433542539, - "timestamp": 3.280667748467635 + "heading": -1.0197992544535218e-37, + "angularVelocity": -1.260895838142623e-19, + "velocityX": 0.40617567078743805, + "velocityY": 0.0005740544395093853, + "timestamp": 3.3311279624647385 }, { - "x": 2.6583051681518555, + "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, - "angularVelocity": -1.1323659927897137e-37, + "angularVelocity": 9.592877809620058e-37, "velocityX": 0, - "velocityY": 3.593395183653217e-38, - "timestamp": 3.3790966350154164 - }, - { - "x": 2.6753320649100156, - "y": 5.539342065750356, - "heading": 1.5795430701564016e-18, - "angularVelocity": 2.476798764658572e-17, - "velocityX": 0.2669898507573085, - "velocityY": -0.07829731508072835, - "timestamp": 3.442870208092684 - }, - { - "x": 2.7093452100309583, - "y": 5.5292188917484815, - "heading": 4.421175983497276e-18, - "angularVelocity": 4.455815749727496e-17, - "velocityX": 0.5333423153150382, - "velocityY": -0.15873618982597607, - "timestamp": 3.506643781169952 - }, - { - "x": 2.7602957743928864, - "y": 5.513806855363461, - "heading": 7.811064020414863e-18, - "angularVelocity": 5.3155058958518925e-17, - "velocityX": 0.7989291159865928, - "velocityY": -0.24166807097898507, - "timestamp": 3.5704173542472195 - }, - { - "x": 2.8281241133670565, - "y": 5.492918602334042, - "heading": 1.1328926559518176e-17, - "angularVelocity": 5.516176010462958e-17, - "velocityX": 1.0635806604718465, - "velocityY": -0.3275377561817081, - "timestamp": 3.6341909273244872 - }, - { - "x": 2.9127559105445995, - "y": 5.466330173559929, - "heading": 1.5973697872969206e-17, - "angularVelocity": 7.283222641176841e-17, - "velocityX": 1.3270668882705299, - "velocityY": -0.41691922674457305, - "timestamp": 3.697964500401755 - }, - { - "x": 3.0140963269470453, - "y": 5.433769288149837, - "heading": 1.634340357557127e-17, - "angularVelocity": 5.797161500644316e-18, - "velocityX": 1.5890659957167883, - "velocityY": -0.5105701913650192, - "timestamp": 3.7617380734790227 - }, - { - "x": 3.132020733935084, - "y": 5.394898138842126, - "heading": 1.753294263521122e-17, - "angularVelocity": 1.8652539010143133e-17, - "velocityX": 1.8491108667403897, - "velocityY": -0.6095181347392088, - "timestamp": 3.8255116465562904 - }, - { - "x": 3.2663592482856187, - "y": 5.34928716553499, - "heading": 1.3361291372353952e-17, - "angularVelocity": -6.54134786803134e-17, - "velocityX": 2.1064918879136747, - "velocityY": -0.7152017851010761, - "timestamp": 3.889285219633558 - }, - { - "x": 3.4168692069208895, - "y": 5.296373281622491, - "heading": 9.91837272236996e-18, - "angularVelocity": -5.398660422887962e-17, - "velocityX": 2.36006783645184, - "velocityY": -0.8297149016315647, - "timestamp": 3.953058792710826 - }, - { - "x": 3.5831820501397047, - "y": 5.235389893074956, - "heading": 8.197617273877545e-18, - "angularVelocity": -2.6982264995683312e-17, - "velocityX": 2.6078645933372964, - "velocityY": -0.9562485776615884, - "timestamp": 4.016832365788094 - }, - { - "x": 3.7646897201959026, - "y": 5.165243249909899, - "heading": 6.703480606914857e-18, - "angularVelocity": -2.342877456705142e-17, - "velocityX": 2.8461267151565104, - "velocityY": -1.0999327743494527, - "timestamp": 4.080605938865362 - }, - { - "x": 3.960268086544922, - "y": 5.08428626917176, - "heading": 4.5447273483145725e-18, - "angularVelocity": -3.385027926825969e-17, - "velocityX": 3.066761934634853, - "velocityY": -1.2694440162550131, - "timestamp": 4.144379511942629 + "velocityY": 7.039095899612751e-38, + "timestamp": 3.424226996507112 + }, + { + "x": 2.7765323823959327, + "y": 5.537624192256726, + "heading": 9.00740418544712e-19, + "angularVelocity": 1.3499597384555195e-17, + "velocityX": 0.2731753218474804, + "velocityY": -0.10058184592921769, + "timestamp": 3.490950498911381 + }, + { + "x": 2.812967100056819, + "y": 5.524148553837151, + "heading": 1.885084819596924e-20, + "angularVelocity": -1.3217075521281405e-17, + "velocityX": 0.5460552331340873, + "velocityY": -0.2019623960674071, + "timestamp": 3.55767400131565 + }, + { + "x": 2.8675858546495747, + "y": 5.503845620088409, + "heading": 3.43159270927708e-20, + "angularVelocity": 2.317785843530086e-19, + "velocityX": 0.8185834469813632, + "velocityY": -0.30428459264216856, + "timestamp": 3.6243975037199188 + }, + { + "x": 2.9403602428294473, + "y": 5.476640214649627, + "heading": 3.5333541604123114e-18, + "angularVelocity": 5.244086577062937e-17, + "velocityX": 1.0906859735710903, + "velocityY": -0.40773347409056915, + "timestamp": 3.6911210061241877 + }, + { + "x": 3.031255192543775, + "y": 5.4424407847147105, + "heading": 5.715508292123115e-18, + "angularVelocity": 3.2704430269696984e-17, + "velocityX": 1.3622628674916857, + "velocityY": -0.5125544778465974, + "timestamp": 3.7578445085284566 + }, + { + "x": 3.140226319352468, + "y": 5.40113340648718, + "heading": 6.027086577670902e-18, + "angularVelocity": 4.669693201563416e-18, + "velocityX": 1.6331745619175013, + "velocityY": -0.619082882928634, + "timestamp": 3.8245680109327256 + }, + { + "x": 3.267215684627595, + "y": 5.352572466235242, + "heading": 7.223223650198719e-18, + "angularVelocity": 1.7926772867621648e-17, + "velocityX": 1.9032179172147636, + "velocityY": -0.7277936334593704, + "timestamp": 3.8912915133369945 + }, + { + "x": 3.41214454573205, + "y": 5.296565350322459, + "heading": 8.483175715581024e-18, + "angularVelocity": 1.888318238685755e-17, + "velocityX": 2.172081139061778, + "velocityY": -0.8393911274837239, + "timestamp": 3.9580150157412635 + }, + { + "x": 3.5748998961264853, + "y": 5.232845435249424, + "heading": 1.0703776873779667e-17, + "angularVelocity": 3.3280644423357047e-17, + "velocityX": 2.4392507067198372, + "velocityY": -0.9549845673113, + "timestamp": 4.024738518145532 + }, + { + "x": 3.755306479840233, + "y": 5.161019665655948, + "heading": 1.3781541182824059e-17, + "angularVelocity": 4.612713958550321e-17, + "velocityX": 2.7037936740893525, + "velocityY": -1.0764688154152113, + "timestamp": 4.091462020549801 + }, + { + "x": 3.953058178777003, + "y": 5.080452170332065, + "heading": 1.6552203248378283e-17, + "angularVelocity": 4.15245298240439e-17, + "velocityX": 2.9637487813307404, + "velocityY": -1.2074830070480274, + "timestamp": 4.15818552295407 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 2.916776997028848e-18, - "angularVelocity": -2.5527036870167268e-17, - "velocityX": 3.2494804486361053, - "velocityY": -1.4792721930373927, - "timestamp": 4.208153085019897 - }, - { - "x": 4.281965617918933, - "y": 4.933633775459018, - "heading": 2.5007263674842835e-18, - "angularVelocity": -1.1988032936813997e-17, - "velocityX": 3.298225514699012, - "velocityY": -1.6226254295188278, - "timestamp": 4.242858581053939 - }, - { - "x": 4.397154085355019, - "y": 4.872114684924303, - "heading": 2.80280710947709e-18, - "angularVelocity": 8.704118266930975e-18, - "velocityX": 3.31902668450838, - "velocityY": -1.772603695805773, - "timestamp": 4.277564077087981 - }, - { - "x": 4.510378349293925, - "y": 4.805721616464948, - "heading": 5.6088052918749416e-18, - "angularVelocity": 8.085169506424825e-17, - "velocityX": 3.2624303605355687, - "velocityY": -1.9130419111206762, - "timestamp": 4.312269573122023 - }, - { - "x": 4.620854325513756, - "y": 4.734849659733664, - "heading": 8.920577651651403e-18, - "angularVelocity": 9.542501154652829e-17, - "velocityX": 3.183241527839498, - "velocityY": -2.0420960605711302, - "timestamp": 4.346975069156065 - }, - { - "x": 4.728404969374526, - "y": 4.659612469130667, - "heading": 1.0355720889195964e-17, - "angularVelocity": 4.1352045109421545e-17, - "velocityX": 3.0989513521223016, - "velocityY": -2.167875385765943, - "timestamp": 4.381680565190107 - }, - { - "x": 4.836822070451743, - "y": 4.585629308888239, - "heading": 8.659234104719362e-18, - "angularVelocity": -4.888236672406423e-17, - "velocityX": 3.1239173464304306, - "velocityY": -2.131741905370206, - "timestamp": 4.4163860612241495 - }, - { - "x": 4.9481136166787945, - "y": 4.516045063115851, - "heading": 6.20990752321007e-18, - "angularVelocity": -7.057460233695506e-17, - "velocityX": 3.2067412642045734, - "velocityY": -2.0049921114550315, - "timestamp": 4.4510915572581915 - }, - { - "x": 5.062101259723399, - "y": 4.450971317120919, - "heading": 3.721439651007284e-18, - "angularVelocity": -7.170241479222451e-17, - "velocityX": 3.2844262745242743, - "velocityY": -1.8750271118759503, - "timestamp": 4.4857970532922335 - }, - { - "x": 5.178602303777092, - "y": 4.39051239286923, - "heading": 1.522860892330953e-18, - "angularVelocity": -6.334958464560708e-17, - "velocityX": 3.356847109732077, - "velocityY": -1.7420561916875086, - "timestamp": 4.520502549326276 - }, - { - "x": 5.2974300176780025, - "y": 4.334765207042833, - "heading": -2.608193001732958e-19, - "angularVelocity": -5.139474712462447e-17, - "velocityX": 3.42388749563912, - "velocityY": -1.606292725847098, - "timestamp": 4.555208045360318 - }, - { - "x": 5.4183939381282835, - "y": 4.283819120698816, - "heading": -2.822662772810701e-18, - "angularVelocity": -7.38166505421658e-17, - "velocityX": 3.485439894926986, - "velocityY": -1.4679544212261044, - "timestamp": 4.58991354139436 - }, - { - "x": 5.5413001762936585, - "y": 4.23775579768525, - "heading": -5.4175777354584686e-18, - "angularVelocity": -7.476956848858907e-17, - "velocityX": 3.5414056045998694, - "velocityY": -1.3272630642818801, - "timestamp": 4.624619037428402 + "heading": 2.0019563257216408e-17, + "angularVelocity": 5.196609716079013e-17, + "velocityX": 3.2138733563909483, + "velocityY": -1.356409229176722, + "timestamp": 4.224909025358339 + }, + { + "x": 4.282917561329041, + "y": 4.939413083786088, + "heading": 2.2898103599715735e-17, + "angularVelocity": 8.297955477827177e-17, + "velocityX": 3.327163862876717, + "velocityY": -1.456761903756842, + "timestamp": 4.25959877936104 + }, + { + "x": 4.401833285665519, + "y": 4.884962621939577, + "heading": 2.513067280390373e-17, + "angularVelocity": 6.43581734252565e-17, + "velocityX": 3.4279783110372324, + "velocityY": -1.5696410485433538, + "timestamp": 4.2942885333637415 + }, + { + "x": 4.518840762702163, + "y": 4.825621230712266, + "heading": 2.8188632508696854e-17, + "angularVelocity": 8.815166877675102e-17, + "velocityX": 3.372969350763695, + "velocityY": -1.7106316528704353, + "timestamp": 4.328978287366443 + }, + { + "x": 4.633380439466489, + "y": 4.761646132811981, + "heading": 3.13421659224765e-17, + "angularVelocity": 9.090676784661175e-17, + "velocityX": 3.3018301817708395, + "velocityY": -1.8442073096080187, + "timestamp": 4.363668041369144 + }, + { + "x": 4.745269125938669, + "y": 4.693140128704652, + "heading": 3.2482743151706595e-17, + "angularVelocity": 3.287936919763874e-17, + "velocityX": 3.2254102022016937, + "velocityY": -1.9748195418737586, + "timestamp": 4.398357795371846 + }, + { + "x": 4.854330043574812, + "y": 4.6202164218202615, + "heading": 3.23593373316049e-17, + "angularVelocity": -3.55741410250663e-18, + "velocityX": 3.1438942353886365, + "velocityY": -2.1021684638845084, + "timestamp": 4.433047549374547 + }, + { + "x": 4.962829604926255, + "y": 4.546460093483164, + "heading": 3.1455405916033935e-17, + "angularVelocity": -2.605759082352001e-17, + "velocityX": 3.1277120426680747, + "velocityY": -2.1261704055714574, + "timestamp": 4.467737303377248 + }, + { + "x": 5.07386693975175, + "y": 4.476582621829349, + "heading": 2.90082292000263e-17, + "angularVelocity": -7.054465464981545e-17, + "velocityX": 3.200868326043711, + "velocityY": -2.014354776005967, + "timestamp": 4.50242705737995 + }, + { + "x": 5.187610618884458, + "y": 4.411202761148161, + "heading": 2.619388391387083e-17, + "angularVelocity": -8.112900673600335e-17, + "velocityX": 3.2788839933500147, + "velocityY": -1.884702343985982, + "timestamp": 4.537116811382651 + }, + { + "x": 5.303878902064225, + "y": 4.350425816718677, + "heading": 2.3840017411584327e-17, + "angularVelocity": -6.785480525757533e-17, + "velocityX": 3.35166064223783, + "velocityY": -1.7520142813566866, + "timestamp": 4.571806565385352 + }, + { + "x": 5.422485692712026, + "y": 4.294349264150295, + "heading": 2.0445926478566092e-17, + "angularVelocity": -9.784130878396876e-17, + "velocityX": 3.419072693296267, + "velocityY": -1.6165162936588033, + "timestamp": 4.606496319388054 + }, + { + "x": 5.5432410890609845, + "y": 4.243062963447586, + "heading": 1.6758899100249362e-17, + "angularVelocity": -1.0628577469963028e-16, + "velocityX": 3.4810104545438403, + "velocityY": -1.478427915594755, + "timestamp": 4.641186073390755 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -7.665872207711091e-18, - "angularVelocity": -6.478208725348017e-17, - "velocityX": 3.591694883279384, - "velocityY": -1.1844441897757751, - "timestamp": 4.659324533462444 - }, - { - "x": 5.800468403278387, - "y": 4.158565232862714, - "heading": -9.714096811795654e-18, - "angularVelocity": -5.540829464294432e-17, - "velocityX": 3.6389268627432543, - "velocityY": -1.0302389285738007, - "timestamp": 4.6962905620558795 - }, - { - "x": 5.936486438230336, - "y": 4.1262510009162545, - "heading": -1.1401685441264777e-17, - "angularVelocity": -4.5652419090776246e-17, - "velocityX": 3.67954146353996, - "velocityY": -0.8741602269982254, - "timestamp": 4.733256590649315 - }, - { - "x": 6.0737584862384155, - "y": 4.099765139631346, - "heading": -1.2627280609090703e-17, - "angularVelocity": -3.3154634524185157e-17, - "velocityX": 3.713464854930551, - "velocityY": -0.7164919330720612, - "timestamp": 4.770222619242751 - }, - { - "x": 6.212034919724313, - "y": 4.079155810434451, - "heading": -1.3669283004490658e-17, - "angularVelocity": -2.818810770451562e-17, - "velocityX": 3.7406353548742786, - "velocityY": -0.5575207827587797, - "timestamp": 4.8071886478361865 - }, - { - "x": 6.351064277842407, - "y": 4.06446048840337, - "heading": -1.3375402013759826e-17, - "angularVelocity": 7.950028767305027e-18, - "velocityX": 3.761003370072177, - "velocityY": -0.3975358617152357, - "timestamp": 4.844154676429622 - }, - { - "x": 6.490086441164715, - "y": 4.055726900176193, - "heading": -1.4576849056839345e-17, - "angularVelocity": -3.2501382723403574e-17, - "velocityX": 3.7608087374308066, - "velocityY": -0.23625984612066567, - "timestamp": 4.881120705023058 - }, - { - "x": 6.625430002884342, - "y": 4.051684818750479, - "heading": -1.3372180037518883e-17, - "angularVelocity": 3.2588543188391143e-17, - "velocityX": 3.661295704988517, - "velocityY": -0.10934583939674931, - "timestamp": 4.9180867336164935 - }, - { - "x": 6.755501460439252, - "y": 4.050426142633095, - "heading": -1.1630354881514603e-17, - "angularVelocity": 4.7119618262525755e-17, - "velocityX": 3.518675457011608, - "velocityY": -0.03404953589216864, - "timestamp": 4.955052762209929 - }, - { - "x": 6.8798297740402194, - "y": 4.050767085715321, - "heading": -9.335447591441833e-18, - "angularVelocity": 6.208152126139743e-17, - "velocityX": 3.3633127044392923, - "velocityY": 0.009223146093816154, - "timestamp": 4.992018790803365 - }, - { - "x": 6.998265582982919, - "y": 4.052014068285868, - "heading": -6.4179877740703305e-18, - "angularVelocity": 7.892272793106084e-17, - "velocityX": 3.203909466318293, - "velocityY": 0.03373320364655125, - "timestamp": 5.0289848193968005 - }, - { - "x": 7.110758098746119, - "y": 4.053729886887297, - "heading": -3.1976006784105176e-18, - "angularVelocity": 8.71174756444278e-17, - "velocityX": 3.0431323039981164, - "velocityY": 0.046416092469654044, - "timestamp": 5.065950847990236 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -8.646914705534956e-42, - "angularVelocity": 8.650106057047145e-17, - "velocityX": 2.8819232262850263, - "velocityY": 0.051084720684648116, - "timestamp": 5.102916876583672 - }, - { - "x": 7.405338431031862, - "y": 4.059210715077462, - "heading": 6.035426461355953e-18, - "angularVelocity": 8.221730826644903e-17, - "velocityX": 2.561662296238626, - "velocityY": 0.04893769145541128, - "timestamp": 5.176325099117501 - }, - { - "x": 7.569876923635766, - "y": 4.062517669134614, - "heading": 1.1021958875984658e-17, - "angularVelocity": 6.792879928853616e-17, - "velocityX": 2.2414177448320305, - "velocityY": 0.04504882345608618, - "timestamp": 5.24973332165133 - }, - { - "x": 7.710907923869345, - "y": 4.065460118931574, - "heading": 1.4466516010520595e-17, - "angularVelocity": 4.692331479554e-17, - "velocityX": 1.9211880544932918, - "velocityY": 0.04008338160760226, - "timestamp": 5.32314154418516 - }, - { - "x": 7.828432324355203, - "y": 4.067984380811152, - "heading": 1.5902740281761073e-17, - "angularVelocity": 1.956489643348339e-17, - "velocityX": 1.600970523863292, - "velocityY": 0.03438663670702133, - "timestamp": 5.396549766718989 - }, - { - "x": 7.922450847470263, - "y": 4.070051609380546, - "heading": 1.5219645547347743e-17, - "angularVelocity": -9.30542534385069e-18, - "velocityX": 1.2807628337783643, - "velocityY": 0.02816072230112734, - "timestamp": 5.469957989252818 - }, - { - "x": 7.992964082884089, - "y": 4.071632392724107, - "heading": 1.2631622975679932e-17, - "angularVelocity": -3.5255213685022036e-17, - "velocityX": 0.9605631764388524, - "velocityY": 0.021534145481204726, - "timestamp": 5.543366211786648 - }, - { - "x": 8.039972518291798, - "y": 4.072703688242487, - "heading": 7.596971431802736e-18, - "angularVelocity": -6.858429982495519e-17, - "velocityX": 0.6403701627027665, - "velocityY": 0.014593671953933919, - "timestamp": 5.616774434320477 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 8.652482395157843e-45, - "angularVelocity": -1.0348937993018065e-16, - "velocityX": 0.32018271791516006, - "velocityY": 0.007400637290262982, - "timestamp": 5.690182656854306 - }, - { - "x": 8.0634765625, + "heading": 1.3081934400390577e-17, + "angularVelocity": -1.0599569831404499e-16, + "velocityX": 3.537374169625408, + "velocityY": -1.337971116472263, + "timestamp": 4.675875827393456 + }, + { + "x": 5.795129656789146, + "y": 4.153827522905326, + "heading": 9.653855416146634e-18, + "angularVelocity": -9.526631807314967e-17, + "velocityX": 3.589854735093561, + "velocityY": -1.1900109590649786, + "timestamp": 4.711859993188672 + }, + { + "x": 5.925973457628177, + "y": 4.116403970594212, + "heading": 6.773802428340066e-18, + "angularVelocity": -8.003667513641558e-17, + "velocityX": 3.636149343676849, + "velocityY": -1.0400005525788714, + "timestamp": 4.747844158983888 + }, + { + "x": 6.058257666523593, + "y": 4.0844428903853265, + "heading": 3.7266948743050956e-18, + "angularVelocity": -8.467912168301893e-17, + "velocityX": 3.6761782848667472, + "velocityY": -0.8881984479166671, + "timestamp": 4.783828324779104 + }, + { + "x": 6.1917543385226965, + "y": 4.0579993432791355, + "heading": 2.3240409715281496e-19, + "angularVelocity": -9.710634385796704e-17, + "velocityX": 3.7098726356150347, + "velocityY": -0.7348661980016377, + "timestamp": 4.81981249057432 + }, + { + "x": 6.326233441808095, + "y": 4.037118888120514, + "heading": -2.9507025724087755e-18, + "angularVelocity": -8.845853722652732e-17, + "velocityX": 3.7371744019498516, + "velocityY": -0.5802678677464012, + "timestamp": 4.855796656369535 + }, + { + "x": 6.461463252337996, + "y": 4.021837506709874, + "heading": -5.178975495681446e-18, + "angularVelocity": -6.192370655342397e-17, + "velocityX": 3.7580365569536736, + "velocityY": -0.4246696032250841, + "timestamp": 4.891780822164751 + }, + { + "x": 6.596863602394611, + "y": 4.012202736620321, + "heading": -4.3543144284076125e-18, + "angularVelocity": 2.2917331805219353e-17, + "velocityX": 3.7627758505553097, + "velocityY": -0.26775026950420705, + "timestamp": 4.927764987959967 + }, + { + "x": 6.727539043403903, + "y": 4.005664590276287, + "heading": -3.5766746235771005e-18, + "angularVelocity": 2.161061087932747e-17, + "velocityX": 3.6314706238561003, + "velocityY": -0.1816950928150608, + "timestamp": 4.963749153755183 + }, + { + "x": 6.85298501008117, + "y": 4.001263406795894, + "heading": -2.645512635943814e-18, + "angularVelocity": 2.5876992478309148e-17, + "velocityX": 3.4861435274385992, + "velocityY": -0.122308892901394, + "timestamp": 4.999733319550399 + }, + { + "x": 6.973061817653349, + "y": 3.9986189838302026, + "heading": -1.6196069084437975e-18, + "angularVelocity": 2.850992109492732e-17, + "velocityX": 3.336934591051298, + "velocityY": -0.07348851660866845, + "timestamp": 5.035717485345614 + }, + { + "x": 7.0877080332373295, + "y": 3.99753108954472, + "heading": -5.829248205352589e-19, + "angularVelocity": 2.880939616045139e-17, + "velocityX": 3.186018434786799, + "velocityY": -0.030232583177662708, + "timestamp": 5.07170165114083 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "angularVelocity": 1.619948128975407e-17, + "velocityX": 3.034163545803906, + "velocityY": 0.009602962363163213, + "timestamp": 5.107685816936046 + }, + { + "x": 7.37338840046626, + "y": 4.002743213470309, + "heading": 8.093787297850302e-19, + "angularVelocity": 1.2673214059219633e-17, + "velocityX": 2.7636055688801218, + "velocityY": 0.07620051362427777, + "timestamp": 5.171551125768248 + }, + { + "x": 7.532268457404618, + "y": 4.010110422409507, + "heading": 7.012678022232893e-19, + "angularVelocity": -1.6927958157108362e-18, + "velocityX": 2.487736454164719, + "velocityY": 0.11535541084684194, + "timestamp": 5.235416434600451 + }, + { + "x": 7.673406960294269, + "y": 4.018856350887051, + "heading": 5.093320205791772e-19, + "angularVelocity": -3.0053214359033797e-18, + "velocityX": 2.2099400358412575, + "velocityY": 0.13694333649152024, + "timestamp": 5.299281743432653 + }, + { + "x": 7.796760795915294, + "y": 4.028209945243461, + "heading": 2.737170340594828e-19, + "angularVelocity": -3.689248370192142e-18, + "velocityX": 1.931468552749354, + "velocityY": 0.14645814022421355, + "timestamp": 5.363147052264855 + }, + { + "x": 7.902319650014668, + "y": 4.037611597874035, + "heading": 3.811839387951723e-20, + "angularVelocity": -3.688992419979718e-18, + "velocityX": 1.65283556956901, + "velocityY": 0.14721063441931925, + "timestamp": 5.4270123610970575 + }, + { + "x": 7.990087417936277, + "y": 4.046637814965993, + "heading": -1.8999762440941744e-19, + "angularVelocity": -3.5718298785688795e-18, + "velocityX": 1.3742635794998819, + "velocityY": 0.14133208242480425, + "timestamp": 5.49087766992926 + }, + { + "x": 8.06007417147421, + "y": 4.0549574599073335, + "heading": -2.1124739162594107e-19, + "angularVelocity": -3.327278549958625e-19, + "velocityX": 1.0958492931086379, + "velocityY": 0.1302686089438499, + "timestamp": 5.554742978761462 + }, + { + "x": 8.112292461632407, + "y": 4.062304767499659, + "heading": -1.9392441616154913e-19, + "angularVelocity": 2.712423345539538e-19, + "velocityX": 0.8176315297463507, + "velocityY": 0.11504379649410336, + "timestamp": 5.6186082875936645 + }, + { + "x": 8.146755551617066, + "y": 4.068461861174775, + "heading": -1.2666965805994144e-19, + "angularVelocity": 1.0530718371509655e-18, + "velocityX": 0.5396214410425377, + "velocityY": 0.0964074829935333, + "timestamp": 5.682473596425867 + }, + { + "x": 8.1634765625, "y": 4.073246955871582, "heading": 0, - "angularVelocity": 5.394624259127602e-43, - "velocityX": 0, - "velocityY": -7.823862347152674e-43, - "timestamp": 5.763590879388135 - }, - { - "x": 8.042595807906332, - "y": 4.071940125417329, - "heading": 2.4014031773112573e-19, - "angularVelocity": 3.467784621738616e-18, - "velocityX": -0.30153187250835495, - "velocityY": -0.01887149394694188, - "timestamp": 5.832839793010322 - }, - { - "x": 8.000826969409328, - "y": 4.069449378892138, - "heading": 7.295832460611546e-19, - "angularVelocity": 7.067878797353639e-18, - "velocityX": -0.6031695850838968, - "velocityY": -0.03596802310545437, - "timestamp": 5.902088706632509 - }, - { - "x": 7.938162262991824, - "y": 4.0659211372128246, - "heading": 1.4792287784714074e-18, - "angularVelocity": 1.0825376070160765e-17, - "velocityX": -0.9049197040027859, - "velocityY": -0.0509501376232871, - "timestamp": 5.971337620254696 - }, - { - "x": 7.8545936374958405, - "y": 4.061532754567933, - "heading": 2.6959287825754293e-18, - "angularVelocity": 1.756995078279752e-17, - "velocityX": -1.206786086954711, - "velocityY": -0.06337114064826968, - "timestamp": 6.0405865338768825 - }, - { - "x": 7.7501132248715185, - "y": 4.056503424652652, - "heading": 4.202275116563444e-18, - "angularVelocity": 2.17526348818473e-17, - "velocityX": -1.5087660897375712, - "velocityY": -0.07262684210066948, - "timestamp": 6.109835447499069 - }, - { - "x": 7.624714358960079, - "y": 4.051110859560637, - "heading": 6.0188244090330196e-18, - "angularVelocity": 2.6232170260178098e-17, - "velocityX": -1.8108423562512328, - "velocityY": -0.07787219769881376, - "timestamp": 6.179084361121256 - }, - { - "x": 7.478393887159083, - "y": 4.045718105040055, - "heading": 8.17237550228445e-18, - "angularVelocity": 3.1098698601987103e-17, - "velocityX": -2.1129641484240804, - "velocityY": -0.07787493317230812, - "timestamp": 6.248333274743443 - }, - { - "x": 7.311157646174121, - "y": 4.040819461213868, - "heading": 1.0532118174804612e-17, - "angularVelocity": 3.4076241042489266e-17, - "velocityX": -2.4150016547173685, - "velocityY": -0.07073964875337486, - "timestamp": 6.31758218836563 - }, - { - "x": 7.123034533164843, - "y": 4.037125830040512, - "heading": 8.283291482043014e-18, - "angularVelocity": -3.247454111743766e-17, - "velocityX": -2.716621866960299, - "velocityY": -0.053338471033761534, - "timestamp": 6.386831101987816 - }, - { - "x": 6.914117681129227, - "y": 4.035741699141579, - "heading": 5.798379315469736e-18, - "angularVelocity": -3.588377111777714e-17, - "velocityX": -3.016897177267489, - "velocityY": -0.019987763367446825, - "timestamp": 6.456080015610003 - }, - { - "x": 6.684712125750405, - "y": 4.03859150608828, - "heading": 3.611288298800787e-18, - "angularVelocity": -3.158303722425788e-17, - "velocityX": -3.312767571061532, - "velocityY": 0.041153092483867636, - "timestamp": 6.52532892923219 - }, - { - "x": 6.43606725460267, - "y": 4.049660896865006, - "heading": 2.3554139869933522e-18, - "angularVelocity": -1.8135653631468075e-17, - "velocityX": -3.5905959839934547, - "velocityY": 0.15984930589841134, - "timestamp": 6.594577842854377 - }, - { - "x": 6.175716481433539, - "y": 4.078070527288863, - "heading": 2.6764824042330446e-18, - "angularVelocity": 4.636439771335605e-18, - "velocityX": -3.759636932206204, - "velocityY": 0.41025380670743106, - "timestamp": 6.663826756476563 - }, - { - "x": 5.918464051979435, - "y": 4.12717103366876, - "heading": 2.4806366122903936e-18, - "angularVelocity": -2.8281424458318553e-18, - "velocityX": -3.7148948048144055, - "velocityY": 0.7090437064151481, - "timestamp": 6.73307567009875 + "angularVelocity": 1.9833875444262685e-18, + "velocityX": 0.26181680146361314, + "velocityY": 0.07492478756156581, + "timestamp": 5.746338905258069 + }, + { + "x": 8.160404735397709, + "y": 4.0766649695322, + "heading": 2.2110153580287816e-19, + "angularVelocity": 3.145700398787454e-18, + "velocityX": -0.04370411858778341, + "velocityY": 0.04862945386701764, + "timestamp": 5.816625807306458 + }, + { + "x": 8.135851784135758, + "y": 4.07831805033267, + "heading": 5.233120115827785e-19, + "angularVelocity": 4.299669881203419e-18, + "velocityX": -0.34932470412548816, + "velocityY": 0.02351904483329859, + "timestamp": 5.8869127093548475 + }, + { + "x": 8.089809855784104, + "y": 4.078304537638241, + "heading": 9.059382457941767e-19, + "angularVelocity": 5.443777191210811e-18, + "velocityX": -0.6550570164545902, + "velocityY": -0.00019225053366650092, + "timestamp": 5.957199611403237 + }, + { + "x": 8.022270134044854, + "y": 4.076742304755474, + "heading": 1.3681480133267195e-18, + "angularVelocity": 6.5760441001468545e-18, + "velocityX": -0.9609147617966158, + "velocityY": -0.022226515001204008, + "timestamp": 6.027486513451626 + }, + { + "x": 7.9332227395210335, + "y": 4.0737752063023285, + "heading": 1.9089237482918726e-18, + "angularVelocity": 7.693833690281903e-18, + "velocityX": -1.2669130652893827, + "velocityY": -0.04221410201153685, + "timestamp": 6.097773415500015 + }, + { + "x": 7.822656701940191, + "y": 4.069582704513375, + "heading": 2.0768842809054915e-18, + "angularVelocity": 2.3896419918926418e-18, + "velocityX": -1.5730674472567026, + "velocityY": -0.05964840769432409, + "timestamp": 6.168060317548404 + }, + { + "x": 7.690560152144021, + "y": 4.064394900776391, + "heading": 2.063501960546536e-18, + "angularVelocity": -1.9039564936380514e-19, + "velocityX": -1.8793906965088152, + "velocityY": -0.07380896846772396, + "timestamp": 6.238347219596793 + }, + { + "x": 7.536921137087825, + "y": 4.058517363178713, + "heading": 1.8242393677495476e-18, + "angularVelocity": -3.404085054560583e-18, + "velocityX": -2.185884006531148, + "velocityY": -0.08362208927108938, + "timestamp": 6.3086341216451824 + }, + { + "x": 7.361730243526304, + "y": 4.0523752131799995, + "heading": 1.7449161631907716e-18, + "angularVelocity": -1.128563106982712e-18, + "velocityX": -2.492511242577032, + "velocityY": -0.08738683623422486, + "timestamp": 6.378921023693572 + }, + { + "x": 7.164988961998875, + "y": 4.046599373609111, + "heading": 1.6203310877610785e-18, + "angularVelocity": -1.7725219321907188e-18, + "velocityX": -2.799117272119643, + "velocityY": -0.0821751905769253, + "timestamp": 6.449207925741961 + }, + { + "x": 6.946739461316107, + "y": 4.042219879396985, + "heading": 9.714507263424963e-19, + "angularVelocity": -9.231881652246047e-18, + "velocityX": -3.105123349048912, + "velocityY": -0.062308824041090796, + "timestamp": 6.51949482779035 + }, + { + "x": 6.707199372936614, + "y": 4.041196558011354, + "heading": 4.304213175776067e-18, + "angularVelocity": 4.741655062759286e-17, + "velocityX": -3.408033095761992, + "velocityY": -0.014559204571673643, + "timestamp": 6.589781729838739 + }, + { + "x": 6.4477381349567215, + "y": 4.048401250319157, + "heading": 3.981085161265745e-18, + "angularVelocity": -4.597272110228882e-18, + "velocityX": -3.691459296374506, + "velocityY": 0.102504052644734, + "timestamp": 6.660068631887128 + }, + { + "x": 6.183416579926013, + "y": 4.076604236120931, + "heading": 3.783709688486887e-18, + "angularVelocity": -2.808140165901377e-18, + "velocityX": -3.7606089801587, + "velocityY": 0.4012552122777795, + "timestamp": 6.730355533935517 + }, + { + "x": 5.922248771081443, + "y": 4.126128703964576, + "heading": 4.4649221139499255e-18, + "angularVelocity": 9.691882920007657e-18, + "velocityX": -3.7157393658460984, + "velocityY": 0.7046045052540438, + "timestamp": 6.800642435983907 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 3.0661477431883128e-18, - "angularVelocity": 8.455167023881293e-18, - "velocityX": -3.6464445426004524, - "velocityY": 1.0033087488526664, - "timestamp": 6.802324583720937 - }, - { - "x": 5.541093538436437, - "y": 4.236435061250764, - "heading": 3.354859743531915e-18, - "angularVelocity": 8.33229202553877e-18, - "velocityX": -3.6034349206984246, - "velocityY": 1.1482323536375627, - "timestamp": 6.83697435349121 - }, - { - "x": 5.417925084630127, - "y": 4.281179055760442, - "heading": 3.9245083052882345e-18, - "angularVelocity": 1.6440183168115343e-17, - "velocityX": -3.554668750266214, - "velocityY": 1.2913215529664346, - "timestamp": 6.871624123261483 - }, - { - "x": 5.296643156826778, - "y": 4.330809566180666, - "heading": 4.682534245858068e-18, - "angularVelocity": 2.1876795880478242e-17, - "velocityX": -3.500223193615562, - "velocityY": 1.4323474802075717, - "timestamp": 6.906273893031757 - }, - { - "x": 5.177441529342695, - "y": 4.385247292254255, - "heading": 5.748631283020555e-18, - "angularVelocity": 3.076779569476711e-17, - "velocityX": -3.4401852674457762, - "velocityY": 1.5710847845312972, - "timestamp": 6.94092366280203 - }, - { - "x": 5.060510651244131, - "y": 4.444405251423305, - "heading": 6.298107237796981e-18, - "angularVelocity": 1.5857997280196432e-17, - "velocityX": -3.374650939207116, - "velocityY": 1.7073117530438153, - "timestamp": 6.975573432572303 - }, - { - "x": 4.946037340921014, - "y": 4.5081889164365, - "heading": 6.498139126492573e-18, - "angularVelocity": 5.772964438776829e-18, - "velocityX": -3.303724991019314, - "velocityY": 1.840810644228753, - "timestamp": 7.010223202342576 - }, - { - "x": 4.834204484779198, - "y": 4.576496362942444, - "heading": 6.284318947548799e-18, - "angularVelocity": -6.170897537311036e-18, - "velocityX": -3.227520900810133, - "velocityY": 1.971367976145888, - "timestamp": 7.04487297211285 - }, - { - "x": 4.725190735681975, - "y": 4.649218420709879, - "heading": 4.218601510181254e-18, - "angularVelocity": -5.961706098087163e-17, - "velocityX": -3.146160849551947, - "velocityY": 2.098774631103723, - "timestamp": 7.079522741883123 - }, - { - "x": 4.619170166207779, - "y": 4.726238769851069, - "heading": 2.3861247317221027e-18, - "angularVelocity": -5.2885684107236655e-17, - "velocityX": -3.059777025276331, - "velocityY": 2.22282426843907, - "timestamp": 7.114172511653396 - }, - { - "x": 4.5101566662173465, - "y": 4.798961201296667, - "heading": 8.546946946648484e-19, - "angularVelocity": -4.419740873346574e-17, - "velocityX": -3.146153660275008, - "velocityY": 2.0987854155379675, - "timestamp": 7.148822281423669 - }, - { - "x": 4.398324060290128, - "y": 4.867269057737162, - "heading": -1.0977695137154222e-18, - "angularVelocity": -5.634854780637897e-17, - "velocityX": -3.2275136795616595, - "velocityY": 1.9713798069474653, - "timestamp": 7.1834720511939425 - }, - { - "x": 4.283850992630547, - "y": 4.931053147799285, - "heading": -2.453093242554037e-18, - "angularVelocity": -3.911494182571376e-17, - "velocityX": -3.3037179876961247, - "velocityY": 1.8408229112346155, - "timestamp": 7.218121820964216 + "heading": 6.738429572739451e-18, + "angularVelocity": 3.2346104217601064e-17, + "velocityX": -3.646441012355221, + "velocityY": 1.003321650758162, + "timestamp": 6.870929338032296 + }, + { + "x": 5.5410942529337, + "y": 4.236435293525444, + "heading": 7.176696434761227e-18, + "angularVelocity": 1.2648537308401216e-17, + "velocityX": -3.603431102925481, + "velocityY": 1.1482444113922432, + "timestamp": 6.90557894623092 + }, + { + "x": 5.417926513758212, + "y": 4.281179467560016, + "heading": 6.902122193005105e-18, + "angularVelocity": -7.924310144639137e-18, + "velocityX": -3.554664701241286, + "velocityY": 1.2913327555706813, + "timestamp": 6.9402285544295435 + }, + { + "x": 5.296645297491296, + "y": 4.3308101048073215, + "heading": 7.073122455788382e-18, + "angularVelocity": 4.9351283224632194e-18, + "velocityX": -3.5002189800153154, + "velocityY": 1.432357819540261, + "timestamp": 6.974878162628167 + }, + { + "x": 5.177444375118266, + "y": 4.38524790502043, + "heading": 7.030277898528261e-18, + "angularVelocity": -1.2365091407245627e-18, + "velocityX": -3.44018095932656, + "velocityY": 1.5710942502164025, + "timestamp": 7.009527770826791 + }, + { + "x": 5.06051419213652, + "y": 4.444405885414455, + "heading": 6.93623096110632e-18, + "angularVelocity": -2.7142280190538443e-18, + "velocityX": -3.3746466139374856, + "velocityY": 1.7073203268248038, + "timestamp": 7.044177379025415 + }, + { + "x": 4.9460415627669985, + "y": 4.508189517767328, + "heading": 6.830736149305672e-18, + "angularVelocity": -3.0446177398576756e-18, + "velocityX": -3.303720743776551, + "velocityY": 1.8408182853682935, + "timestamp": 7.078826987224039 + }, + { + "x": 4.834209367418348, + "y": 4.576496874327604, + "heading": 6.686796334021e-18, + "angularVelocity": -4.154154195902009e-18, + "velocityX": -3.2275168800637117, + "velocityY": 1.971374572800783, + "timestamp": 7.113476595422663 + }, + { + "x": 4.725196244628599, + "y": 4.649218770197504, + "heading": 6.8822518039480765e-18, + "angularVelocity": 5.6409142870147405e-18, + "velocityX": -3.1461574446916942, + "velocityY": 2.0987797453014108, + "timestamp": 7.1481262036212865 + }, + { + "x": 4.61917611476972, + "y": 4.726238684910051, + "heading": 7.643017507811422e-18, + "angularVelocity": 2.195596843411265e-17, + "velocityX": -3.0597786056088934, + "velocityY": 2.2228220957374782, + "timestamp": 7.18277581181991 + }, + { + "x": 4.510162876116158, + "y": 4.798960406973094, + "heading": 8.156998265512583e-18, + "angularVelocity": 1.483366723094954e-17, + "velocityX": -3.14616078856249, + "velocityY": 2.098774729173764, + "timestamp": 7.217425420018534 + }, + { + "x": 4.3983305631985266, + "y": 4.867267570914994, + "heading": 9.169956130720048e-18, + "angularVelocity": 2.9234323788045264e-17, + "velocityX": -3.227520273146238, + "velocityY": 1.9713690137660609, + "timestamp": 7.252075028217158 + }, + { + "x": 4.28385782617867, + "y": 4.93105099911187, + "heading": 9.858535854282379e-18, + "angularVelocity": 1.9872655402484013e-17, + "velocityX": -3.3037238506034297, + "velocityY": 1.840812393353667, + "timestamp": 7.286724636415782 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -1.5610631193350687e-18, - "angularVelocity": 2.5744186155726097e-17, - "velocityX": -3.3579422894520032, - "velocityY": 1.6997125365941679, - "timestamp": 7.252771590734489 - }, - { - "x": 3.941206853693096, - "y": 5.086799964367495, - "heading": 3.827307300721309e-18, - "angularVelocity": 7.831135017939884e-17, - "velocityX": -3.2887955452772286, - "velocityY": 1.4075914407764616, - "timestamp": 7.321578607951182 - }, - { - "x": 3.730310577999757, - "y": 5.1698821724733195, - "heading": 7.528976478562542e-18, - "angularVelocity": 5.37978439929115e-17, - "velocityX": -3.0650402273530597, - "velocityY": 1.2074670791813904, - "timestamp": 7.390385625167876 - }, - { - "x": 3.5370931608840936, - "y": 5.242282386882321, - "heading": 5.4493611354862e-18, - "angularVelocity": -3.022388452804216e-17, - "velocityX": -2.808106279439004, - "velocityY": 1.0522213770870446, - "timestamp": 7.459192642384569 - }, - { - "x": 3.3623805585841375, - "y": 5.305505508408588, - "heading": 3.90044719086442e-18, - "angularVelocity": -2.2510988083436363e-17, - "velocityX": -2.5391683779829535, - "velocityY": 0.9188470025834666, - "timestamp": 7.527999659601263 - }, - { - "x": 3.2065789543659307, - "y": 5.3604207146722205, - "heading": 2.734702057316729e-18, - "angularVelocity": -1.6942241950067733e-17, - "velocityX": -2.2643272520816113, - "velocityY": 0.7981047353162919, - "timestamp": 7.596806676817956 - }, - { - "x": 3.0699254867036734, - "y": 5.407590197311926, - "heading": 1.8584817371846494e-18, - "angularVelocity": -1.2734461622898273e-17, - "velocityX": -1.9860396975485235, - "velocityY": 0.6855330247953545, - "timestamp": 7.66561369403465 - }, - { - "x": 2.952574245770292, - "y": 5.4474063504067525, - "heading": 1.2072006114656177e-18, - "angularVelocity": -9.465330021034912e-18, - "velocityX": -1.7055126886812788, - "velocityY": 0.5786641349302285, - "timestamp": 7.734420711251343 - }, - { - "x": 2.8546328911186802, - "y": 5.480158244239293, - "heading": 7.336690244970979e-19, - "angularVelocity": -6.882024626605053e-18, - "velocityX": -1.4234210203178261, - "velocityY": 0.4759964195133704, - "timestamp": 7.803227728468037 - }, - { - "x": 2.7761806703190945, - "y": 5.5060675412854945, - "heading": 4.019181330238403e-19, - "angularVelocity": -4.821468868916063e-18, - "velocityX": -1.1401776152062642, - "velocityY": 0.37655021383364773, - "timestamp": 7.87203474568473 - }, - { - "x": 2.7172782497291417, - "y": 5.525309545004062, - "heading": 1.8363150875881817e-19, - "angularVelocity": -3.1724471295939874e-18, - "velocityX": -0.856052521568438, - "velocityY": 0.2796517636852234, - "timestamp": 7.940841762901424 - }, - { - "x": 2.6779735090707537, - "y": 5.538026332588319, - "heading": 5.594227372773024e-20, - "angularVelocity": -1.8557589065219566e-18, - "velocityX": -0.5712315727130863, - "velocityY": 0.18481817841642284, - "timestamp": 8.009648780118116 + "heading": 9.428613355674913e-18, + "angularVelocity": -1.2407716016383212e-17, + "velocityX": -3.3581551662140523, + "velocityY": 1.6997824742615957, + "timestamp": 7.321374244614406 + }, + { + "x": 3.9412126137988053, + "y": 5.086809869447978, + "heading": 6.455008109104801e-18, + "angularVelocity": -4.321651467897992e-17, + "velocityX": -3.28870544216361, + "velocityY": 1.4077326603917448, + "timestamp": 7.390181395507035 + }, + { + "x": 3.7303194478817963, + "y": 5.1698988995186355, + "heading": 5.431152098522906e-18, + "angularVelocity": -1.488008146391029e-17, + "velocityX": -3.0649890771686916, + "velocityY": 1.2075638795204993, + "timestamp": 7.458988546399665 + }, + { + "x": 3.537102982396402, + "y": 5.24230220978644, + "heading": 5.612600329775147e-18, + "angularVelocity": 2.6370548540132515e-18, + "velocityX": -2.8080869935582533, + "velocityY": 1.052264326142308, + "timestamp": 7.527795697292294 + }, + { + "x": 3.3623900460315483, + "y": 5.305525654536907, + "heading": 4.225050788166454e-18, + "angularVelocity": -2.0165775266205907e-17, + "velocityX": -2.5391683000722396, + "velocityY": 0.918849915020073, + "timestamp": 7.596602848184924 + }, + { + "x": 3.2065873746476083, + "y": 5.360439315991953, + "heading": 1.940037667439135e-18, + "angularVelocity": -3.3208948359058894e-17, + "velocityX": -2.2643383625498803, + "velocityY": 0.7980807335088809, + "timestamp": 7.6654099990775535 + }, + { + "x": 3.069932459985333, + "y": 5.407606100907469, + "heading": -1.0877040544793261e-18, + "angularVelocity": -4.4003300276785115e-17, + "velocityX": -1.986056868936749, + "velocityY": 0.6854924859353159, + "timestamp": 7.734217149970183 + }, + { + "x": 2.9525796309081422, + "y": 5.44741896036095, + "heading": -3.706402695175446e-18, + "angularVelocity": -3.805852453885881e-17, + "velocityX": -1.705532456362192, + "velocityY": 0.5786151430046301, + "timestamp": 7.803024300862813 + }, + { + "x": 2.854636716681842, + "y": 5.480167404381309, + "heading": -5.982131114439951e-18, + "angularVelocity": -3.307401032802041e-17, + "velocityX": -1.4234409208300893, + "velocityY": 0.4759453573577233, + "timestamp": 7.871831451755442 + }, + { + "x": 2.776183090706554, + "y": 5.506073449109121, + "heading": -4.835349945984594e-18, + "angularVelocity": 1.666659865403876e-17, + "velocityX": -1.1401958220550505, + "velocityY": 0.3765022151293121, + "timestamp": 7.940638602648072 + }, + { + "x": 2.7172795158509544, + "y": 5.525312687608081, + "heading": -3.43564479511919e-18, + "angularVelocity": 2.034243727152086e-17, + "velocityX": -0.8560676338352585, + "velocityY": 0.2796110324199007, + "timestamp": 8.009445753540701 + }, + { + "x": 2.6779739479348046, + "y": 5.538027438094956, + "heading": -1.81505607662002e-18, + "angularVelocity": 2.355262058485764e-17, + "velocityX": -0.5712424857916886, + "velocityY": 0.18478821346223093, + "timestamp": 8.07825290443333 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -4.379352373324341e-45, - "angularVelocity": -8.130315190317274e-19, - "velocityX": -0.2858478933472287, - "velocityY": 0.09169170474607218, - "timestamp": 8.07845579733481 + "heading": -7.816373438340897e-46, + "angularVelocity": 2.6378887267870092e-17, + "velocityX": -0.2858537161877381, + "velocityY": 0.09167545987040067, + "timestamp": 8.14706005532596 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -7.265203918119814e-45, - "angularVelocity": -4.19412391684243e-44, + "heading": 1.7878647482773802e-40, + "angularVelocity": 2.5983819147247646e-39, "velocityX": 0, - "velocityY": -6.607621745560535e-42, - "timestamp": 8.147262814551503 - }, - { - "x": 2.6751665057839835, - "y": 5.538944468629318, - "heading": 4.149201789004472e-19, - "angularVelocity": 6.513823637389646e-18, - "velocityX": 0.2647058041795455, - "velocityY": -0.08463157955674916, - "timestamp": 8.210961219973287 - }, - { - "x": 2.7088624712986666, - "y": 5.5280798374619815, - "heading": 1.747454129766702e-18, - "angularVelocity": 2.0919423995667872e-17, - "velocityX": 0.5289922925316967, - "velocityY": -0.1705636286402431, - "timestamp": 8.27465962539507 - }, - { - "x": 2.7593613029745305, - "y": 5.511644735540107, - "heading": 3.0764256336987273e-18, - "angularVelocity": 2.086349721209079e-17, - "velocityX": 0.79278015425161, - "velocityY": -0.2580143382404696, - "timestamp": 8.338358030816854 - }, - { - "x": 2.8266246280660527, - "y": 5.489524723354374, - "heading": 4.563957990922327e-18, - "angularVelocity": 2.3352740894748934e-17, - "velocityX": 1.0559656030026554, - "velocityY": -0.3472616314217525, - "timestamp": 8.402056436238638 - }, - { - "x": 2.9106051995536757, - "y": 5.461582331644454, - "heading": 6.988237520643839e-18, - "angularVelocity": 3.8058716127490844e-17, - "velocityX": 1.31840932173323, - "velocityY": -0.43866705178720633, - "timestamp": 8.465754841660422 - }, - { - "x": 3.0112434875257597, - "y": 5.427649372766819, - "heading": 9.422486137058152e-18, - "angularVelocity": 3.821522062123431e-17, - "velocityX": 1.5799184815647955, - "velocityY": -0.5327128466237975, - "timestamp": 8.529453247082206 - }, - { - "x": 3.128462311204818, - "y": 5.387515427846556, - "heading": 1.2002270824368104e-17, - "angularVelocity": 4.0499988504071535e-17, - "velocityX": 1.8402159819054258, - "velocityY": -0.6300620031932208, - "timestamp": 8.59315165250399 - }, - { - "x": 3.2621579049769087, - "y": 5.340909820689257, - "heading": 1.4607724763792015e-17, - "angularVelocity": 4.0902969582545765e-17, - "velocityX": 2.0988844679362657, - "velocityY": -0.7316604999559294, - "timestamp": 8.656850057925773 - }, - { - "x": 3.4121839694430376, - "y": 5.287471786804592, - "heading": 1.2983801342724242e-17, - "angularVelocity": -2.5493941493744293e-17, - "velocityX": 2.3552562026116504, - "velocityY": -0.8389226312781332, - "timestamp": 8.720548463347557 - }, - { - "x": 3.5783204640757695, - "y": 5.226697519809262, - "heading": 1.1414351740343512e-17, - "angularVelocity": -2.4638758097451353e-17, - "velocityX": 2.608173525422576, - "velocityY": -0.9540940089929807, - "timestamp": 8.78424686876934 - }, - { - "x": 3.7602044322338806, - "y": 5.157837260218168, - "heading": 1.0662286375073935e-17, - "angularVelocity": -1.1806659213675993e-17, - "velocityX": 2.855392799140749, - "velocityY": -1.081035845954548, - "timestamp": 8.847945274191124 - }, - { - "x": 3.9571467965572817, - "y": 5.079670509622147, - "heading": 1.0033604735305209e-17, - "angularVelocity": -9.869660560666429e-18, - "velocityX": 3.091794261085985, - "velocityY": -1.2271382631705257, - "timestamp": 8.911643679612908 + "velocityY": -2.2530838665454385e-40, + "timestamp": 8.21586720621859 + }, + { + "x": 2.6750735555579293, + "y": 5.538716972759925, + "heading": 1.5037626920568013e-19, + "angularVelocity": 2.3619256993662716e-18, + "velocityX": 0.26337722940288844, + "velocityY": -0.08824680774956185, + "timestamp": 8.279534013877285 + }, + { + "x": 2.7085920650608806, + "y": 5.52742596598787, + "heading": 4.62580789697953e-19, + "angularVelocity": 4.9037250644942105e-18, + "velocityX": 0.5264675697678678, + "velocityY": -0.17734526336836984, + "timestamp": 8.34320082153598 + }, + { + "x": 2.7588390755328, + "y": 5.51039889888726, + "heading": 7.215027779377387e-19, + "angularVelocity": 4.066828505487763e-18, + "velocityX": 0.789218312017212, + "velocityY": -0.2674402522565463, + "timestamp": 8.406867629194675 + }, + { + "x": 2.8257885982701296, + "y": 5.487560532002736, + "heading": -1.3111959461224426e-18, + "angularVelocity": -3.192713438621083e-17, + "velocityX": 1.0515608556382001, + "velocityY": -0.35871701007777496, + "timestamp": 8.47053443685337 + }, + { + "x": 2.9094088162269207, + "y": 5.458820213911237, + "heading": -3.133450423089841e-18, + "angularVelocity": -2.8621734683732664e-17, + "velocityX": 1.3134036561886782, + "velocityY": -0.4514176090871532, + "timestamp": 8.534201244512065 + }, + { + "x": 3.009659896203603, + "y": 5.424066617313393, + "heading": -4.721555014357318e-18, + "angularVelocity": -2.494399593240158e-17, + "velocityX": 1.5746208057754199, + "velocityY": -0.5458668005493692, + "timestamp": 8.59786805217076 + }, + { + "x": 3.1264905621724175, + "y": 5.383159777949491, + "heading": -8.056338252897749e-18, + "angularVelocity": -5.237867832823588e-17, + "velocityX": 1.8350325745107399, + "velocityY": -0.6425143786570167, + "timestamp": 8.661534859829455 + }, + { + "x": 3.259832420061778, + "y": 5.335918462965804, + "heading": -1.1096621419747018e-17, + "angularVelocity": -4.775303299558587e-17, + "velocityX": 2.094370093191732, + "velocityY": -0.7420085397863474, + "timestamp": 8.72520166748815 + }, + { + "x": 3.409589860765726, + "y": 5.282098877479992, + "heading": -1.3592361718623002e-17, + "angularVelocity": -3.9200022596627705e-17, + "velocityX": 2.352205901492158, + "velocityY": -0.8453319314253591, + "timestamp": 8.788868475146845 + }, + { + "x": 3.5756203018815467, + "y": 5.221355824269489, + "heading": -1.4313494756802784e-17, + "angularVelocity": -1.132667185145573e-17, + "velocityX": 2.607802200573299, + "velocityY": -0.9540772569615096, + "timestamp": 8.85253528280554 + }, + { + "x": 3.7576900306489676, + "y": 5.153163870724839, + "heading": -1.1221400900164693e-17, + "angularVelocity": 4.856681166133233e-17, + "velocityX": 2.8597276267323055, + "velocityY": -1.0710754324327645, + "timestamp": 8.916202090464235 + }, + { + "x": 3.955353841452516, + "y": 5.076631094268888, + "heading": -7.381711218130638e-18, + "angularVelocity": 6.030912846483351e-17, + "velocityX": 3.104660310018777, + "velocityY": -1.2020828320186705, + "timestamp": 8.97986889812293 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 9.614004929327239e-18, - "angularVelocity": -6.58728900981987e-18, - "velocityX": 3.3023160854502263, - "velocityY": -1.4085550989874989, - "timestamp": 8.975342085034692 - }, - { - "x": 4.2842075611264425, - "y": 4.936417577245955, - "heading": 8.433110849470374e-18, - "angularVelocity": -3.4077840923717525e-17, - "velocityX": 3.3679341946759727, - "velocityY": -1.5447568972758872, - "timestamp": 9.009994928285538 - }, - { - "x": 4.401356584826297, - "y": 4.877666966713146, - "heading": 7.494016874909782e-18, - "angularVelocity": -2.7100055477775347e-17, - "velocityX": 3.3806468015288584, - "velocityY": -1.695405196841212, - "timestamp": 9.044647771536384 - }, - { - "x": 4.516063976001338, - "y": 4.814281195629901, - "heading": 5.449898069000543e-18, - "angularVelocity": -5.898848735476586e-17, - "velocityX": 3.310186998068025, - "velocityY": -1.8291650882556743, - "timestamp": 9.07930061478723 - }, - { - "x": 4.628146410125642, - "y": 4.746361612927903, - "heading": 3.873819251911276e-18, - "angularVelocity": -4.5481948066433736e-17, - "velocityX": 3.234436877602133, - "velocityY": -1.9600002865663548, - "timestamp": 9.113953458038077 - }, - { - "x": 4.737424855337868, - "y": 4.674016859733605, - "heading": 3.562150006954802e-18, - "angularVelocity": -8.994045386127452e-18, - "velocityX": 3.1535203163901007, - "velocityY": -2.0877003560892837, - "timestamp": 9.148606301288924 - }, - { - "x": 4.844483096398323, - "y": 4.598425359059355, - "heading": 4.67240858769656e-18, - "angularVelocity": 3.2039465642249735e-17, - "velocityX": 3.0894504178337554, - "velocityY": -2.181393893916723, - "timestamp": 9.18325914453977 - }, - { - "x": 4.954476829355994, - "y": 4.5271728465414585, - "heading": 6.725892368510384e-18, - "angularVelocity": 5.925873862496592e-17, - "velocityX": 3.174161847599147, - "velocityY": -2.0561808450207577, - "timestamp": 9.217911987790616 - }, - { - "x": 5.06723038345343, - "y": 4.460373312794278, - "heading": 8.84315284493419e-18, - "angularVelocity": 6.109918488065446e-17, - "velocityX": 3.2538038302147596, - "velocityY": -1.9276782936288406, - "timestamp": 9.252564831041463 - }, - { - "x": 5.18256360847748, - "y": 4.398133545147612, - "heading": 1.1174088851587773e-17, - "angularVelocity": 6.726536087617125e-17, - "velocityX": 3.32824709906689, - "velocityY": -1.796094109684512, - "timestamp": 9.28721767429231 - }, - { - "x": 5.300292214676822, - "y": 4.340553023175947, - "heading": 1.351328882085302e-17, - "angularVelocity": 6.750383950696769e-17, - "velocityX": 3.3973721967667427, - "velocityY": -1.6616391779124182, - "timestamp": 9.321870517543156 - }, - { - "x": 5.420228077668345, - "y": 4.287723772710641, - "heading": 1.5740193529975693e-17, - "angularVelocity": 6.4263260968298e-17, - "velocityX": 3.461068464810413, - "velocityY": -1.5245285959043573, - "timestamp": 9.356523360794002 - }, - { - "x": 5.542179542504676, - "y": 4.239730223009142, - "heading": 1.83851008704834e-17, - "angularVelocity": 7.632583916308507e-17, - "velocityX": 3.5192340193716003, - "velocityY": -1.3849815830141698, - "timestamp": 9.391176204044848 + "heading": -4.354722543751261e-18, + "angularVelocity": 4.754421944015892e-17, + "velocityX": 3.332116557247906, + "velocityY": -1.3615147608100584, + "timestamp": 9.043535705781625 + }, + { + "x": 4.285701077560922, + "y": 4.938388492372474, + "heading": -4.775397572984342e-18, + "angularVelocity": -1.2147965427412122e-17, + "velocityX": 3.4133567656946284, + "velocityY": -1.4888942599373947, + "timestamp": 9.078164963804028 + }, + { + "x": 4.403715149774036, + "y": 4.881600772971697, + "heading": -7.574216488503896e-18, + "angularVelocity": -8.082237608755476e-17, + "velocityX": 3.4079295645540304, + "velocityY": -1.6398768741750067, + "timestamp": 9.11279422182643 + }, + { + "x": 4.519367006147829, + "y": 4.820144951729341, + "heading": -1.1070492199966305e-17, + "angularVelocity": -1.009630558414108e-16, + "velocityX": 3.3397151131270637, + "velocityY": -1.7746791225673963, + "timestamp": 9.147423479848833 + }, + { + "x": 4.6324721141561005, + "y": 4.754119165100519, + "heading": -1.4960169278634737e-17, + "angularVelocity": -1.123234311330653e-16, + "velocityX": 3.266171857771331, + "velocityY": -1.9066474536101443, + "timestamp": 9.182052737871235 + }, + { + "x": 4.742850082889288, + "y": 4.683628924272633, + "heading": -1.7795441089169654e-17, + "angularVelocity": -8.18750378278602e-17, + "velocityX": 3.187419397256003, + "velocityY": -2.0355689048343195, + "timestamp": 9.216681995893637 + }, + { + "x": 4.850325220314879, + "y": 4.608787325463071, + "heading": -1.8976395039793544e-17, + "angularVelocity": -3.410277950107739e-17, + "velocityX": 3.1035934225348787, + "velocityY": -2.1612244409379606, + "timestamp": 9.25131125391604 + }, + { + "x": 4.959315849281407, + "y": 4.536170434444089, + "heading": -1.7824338333790707e-17, + "angularVelocity": 3.3268304658955216e-17, + "velocityX": 3.1473567494868275, + "velocityY": -2.09698085278076, + "timestamp": 9.285940511938442 + }, + { + "x": 5.0711194796065495, + "y": 4.467963995966004, + "heading": -1.5727172733760973e-17, + "angularVelocity": 6.056051211588329e-17, + "velocityX": 3.228588676454312, + "velocityY": -1.9696188244622874, + "timestamp": 9.320569769960844 + }, + { + "x": 5.1855579490393096, + "y": 4.4042772106470105, + "heading": -1.2316155333836473e-17, + "angularVelocity": 9.850102470338397e-17, + "velocityX": 3.3046757559381787, + "velocityY": -1.839103375469191, + "timestamp": 9.355199027983247 + }, + { + "x": 5.302448702578612, + "y": 4.345211810292891, + "heading": -8.765491689722714e-18, + "angularVelocity": 1.0253363331714457e-16, + "velocityX": 3.3754911371096727, + "velocityY": -1.7056501850518684, + "timestamp": 9.389828286005649 + }, + { + "x": 5.421605230513571, + "y": 4.290862095789443, + "heading": -5.5119830878393785e-18, + "angularVelocity": 9.395259349127813e-17, + "velocityX": 3.4409206185669707, + "velocityY": -1.5694738382292028, + "timestamp": 9.424457544028051 + }, + { + "x": 5.542837392691082, + "y": 4.241314822773421, + "heading": -2.184892589584252e-18, + "angularVelocity": 9.60774411078281e-17, + "velocityX": 3.500859362885684, + "velocityY": -1.4307922215361746, + "timestamp": 9.459086802050454 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 2.1337719863838943e-17, - "angularVelocity": 8.520567769813293e-17, - "velocityX": 3.571775782441806, - "velocityY": -1.2432211735943428, - "timestamp": 9.425829047295695 - }, - { - "x": 5.936838928321791, - "y": 4.127579524293423, - "heading": 2.7128761029914745e-17, - "angularVelocity": 7.834427123009397e-17, - "velocityX": 3.6647054686449687, - "velocityY": -0.9344094480089555, - "timestamp": 9.499746911041823 - }, - { - "x": 6.212625572925942, - "y": 4.081838891930782, - "heading": 3.048119806681363e-17, - "angularVelocity": 4.535354333849354e-17, - "velocityX": 3.730987756239054, - "velocityY": -0.6188034941017404, - "timestamp": 9.573664774787952 - }, - { - "x": 6.491306353239061, - "y": 4.059759739930169, - "heading": 2.990728340154358e-17, - "angularVelocity": -7.764221477519524e-18, - "velocityX": 3.770141156544387, - "velocityY": -0.2986984590956799, - "timestamp": 9.64758263853408 - }, - { - "x": 6.752875972710039, - "y": 4.054277278014817, - "heading": 2.289191300833474e-17, - "angularVelocity": -9.490764529266178e-17, - "velocityX": 3.538652312373937, - "velocityY": -0.07416964773468503, - "timestamp": 9.72150050228021 - }, - { - "x": 6.991092096287726, - "y": 4.05357587432536, - "heading": 1.9322766501808003e-17, - "angularVelocity": -4.8285303790501985e-17, - "velocityX": 3.2227138543376115, - "velocityY": -0.009488960501710962, - "timestamp": 9.795418366026338 - }, - { - "x": 7.205567187241114, - "y": 4.0550211409111485, - "heading": 1.4185957310010018e-17, - "angularVelocity": -6.949347466859182e-17, - "velocityX": 2.9015325942048964, - "velocityY": 0.01955233163599203, - "timestamp": 9.869336229772466 - }, - { - "x": 7.396227777452996, - "y": 4.057522153925314, - "heading": 7.96211441465592e-18, - "angularVelocity": -8.419944219072616e-17, - "velocityX": 2.579357418481523, - "velocityY": 0.03383502833300578, - "timestamp": 9.943254093518595 - }, - { - "x": 7.5630549716186115, - "y": 4.0604862007272615, - "heading": 9.124881669040973e-19, - "angularVelocity": -9.537107663127069e-17, - "velocityX": 2.2569266170703433, - "velocityY": 0.040099194588843756, - "timestamp": 10.017171957264724 - }, - { - "x": 7.70604444565388, - "y": 4.0635415850381555, - "heading": -6.759455196513469e-18, - "angularVelocity": -1.0379011208666613e-16, - "velocityX": 1.934437317160139, - "velocityY": 0.04133485677276264, - "timestamp": 10.091089821010852 - }, - { - "x": 7.825196583277639, - "y": 4.066433602906457, - "heading": -4.535875150648457e-18, - "angularVelocity": 3.0081768238080076e-17, - "velocityX": 1.6119532084015247, - "velocityY": 0.03912474903542123, - "timestamp": 10.165007684756981 - }, - { - "x": 7.920513375065405, - "y": 4.068976876297905, - "heading": -2.8949605954376388e-18, - "angularVelocity": 2.219916096123338e-17, - "velocityX": 1.2894960292025357, - "velocityY": 0.03440674909358656, - "timestamp": 10.23892554850311 - }, - { - "x": 7.991997299424134, - "y": 4.071030463180751, - "heading": -1.5603341535814708e-18, - "angularVelocity": 1.8055533185319825e-17, - "velocityX": 0.9670723792051263, - "velocityY": 0.02778201071799316, - "timestamp": 10.312843412249238 - }, - { - "x": 8.03965088919926, - "y": 4.07248360094205, - "heading": -5.821232156931401e-19, - "angularVelocity": 1.3233755526918469e-17, - "velocityX": 0.6446829948819025, - "velocityY": 0.019658817066051913, - "timestamp": 10.386761275995367 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 8.014500942600024e-44, - "angularVelocity": 7.875271094040935e-18, - "velocityX": 0.322326324020543, - "velocityY": 0.0103270696803873, - "timestamp": 10.460679139741496 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": -7.373298274013602e-44, - "angularVelocity": -1.7505506389116263e-42, - "velocityX": -2.259320082617454e-45, + "heading": 1.1869648608997422e-18, + "angularVelocity": 9.737019049910581e-17, + "velocityX": 3.5552114934161927, + "velocityY": -1.2898268911821018, + "timestamp": 9.493716060072856 + }, + { + "x": 5.934272395491136, + "y": 4.124337934678792, + "heading": 7.497166766936204e-18, + "angularVelocity": 8.587782714305931e-17, + "velocityX": 3.6516733021145136, + "velocityY": -0.9841085377661548, + "timestamp": 9.567194885934677 + }, + { + "x": 6.2077530174936735, + "y": 4.075010140740357, + "heading": 1.3006105833327096e-17, + "angularVelocity": 7.497315045222091e-17, + "velocityX": 3.7218970063133985, + "velocityY": -0.6713198443208217, + "timestamp": 9.640673711796499 + }, + { + "x": 6.4844286250858945, + "y": 4.049019985828209, + "heading": 1.5986683133968542e-17, + "angularVelocity": 4.0563757867422685e-17, + "velocityX": 3.7653787243758563, + "velocityY": -0.3537094476852878, + "timestamp": 9.71415253765832 + }, + { + "x": 6.743277363747608, + "y": 4.038426394199003, + "heading": 1.2719246482061777e-17, + "angularVelocity": -4.446773085420891e-17, + "velocityX": 3.522766397335807, + "velocityY": -0.1441720319419269, + "timestamp": 9.787631363520141 + }, + { + "x": 6.978672822837657, + "y": 4.030025299692623, + "heading": 8.975092196512461e-18, + "angularVelocity": -5.095555408833389e-17, + "velocityX": 3.2035822065626665, + "velocityY": -0.11433354313769856, + "timestamp": 9.861110189381963 + }, + { + "x": 7.1905526925662695, + "y": 4.022995759457438, + "heading": 5.368720420708233e-18, + "angularVelocity": -4.9080421924352456e-17, + "velocityX": 2.8835500192539434, + "velocityY": -0.09566756344750614, + "timestamp": 9.934589015243784 + }, + { + "x": 7.3789017577018114, + "y": 4.017045849522277, + "heading": 1.5887833889908863e-18, + "angularVelocity": -5.144253446326957e-17, + "velocityX": 2.563310762337649, + "velocityY": -0.08097448299391075, + "timestamp": 10.008067841105605 + }, + { + "x": 7.54371362899692, + "y": 4.012025997027571, + "heading": -2.7250521222406746e-18, + "angularVelocity": -5.870855257463982e-17, + "velocityX": 2.2429845518359373, + "velocityY": -0.06831699385270525, + "timestamp": 10.081546666967427 + }, + { + "x": 7.684984887810538, + "y": 4.007845278835319, + "heading": -2.10421445779977e-18, + "angularVelocity": 8.449205021435769e-18, + "velocityX": 1.9226118158077505, + "velocityY": -0.056896910684361174, + "timestamp": 10.155025492829248 + }, + { + "x": 7.8027134339680275, + "y": 4.0044425825275765, + "heading": -1.435402908569355e-18, + "angularVelocity": 9.102099024937298e-18, + "velocityX": 1.6022104977409413, + "velocityY": -0.0463085285840207, + "timestamp": 10.22850431869107 + }, + { + "x": 7.896897856926731, + "y": 4.001774011196325, + "heading": -8.461914935171916e-19, + "angularVelocity": 8.01879191918765e-18, + "velocityX": 1.2817899830873647, + "velocityY": -0.03631755543113603, + "timestamp": 10.30198314455289 + }, + { + "x": 7.967537148527336, + "y": 3.999806507404438, + "heading": -3.275924545852618e-19, + "angularVelocity": 7.057802473697343e-18, + "velocityX": 0.9613557480279206, + "velocityY": -0.026776472933672434, + "timestamp": 10.375461970414712 + }, + { + "x": 8.014630554462236, + "y": 3.9985142796609896, + "heading": -5.3367647403100726e-20, + "angularVelocity": 3.732024892420789e-18, + "velocityX": 0.6409112473226126, + "velocityY": -0.01758639619362703, + "timestamp": 10.448940796276533 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 7.342940745159904e-44, + "angularVelocity": 7.262996758203436e-19, + "velocityX": 0.320458791984776, + "velocityY": -0.008677813220192914, + "timestamp": 10.522419622138354 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 1.3038141825006824e-44, + "angularVelocity": -8.034632634038792e-43, + "velocityX": -5.7118786505402475e-43, "velocityY": 0, - "timestamp": 10.534597003487624 + "timestamp": 10.595898448000176 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGCSprint.1.traj b/src/main/deploy/choreo/SourceLanePGCSprint.1.traj new file mode 100644 index 00000000..87faabce --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGCSprint.1.traj @@ -0,0 +1,184 @@ +{ + "samples": [ + { + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": 9.759657159056985e-29, + "angularVelocity": 1.5485227916478622e-29, + "velocityX": -1.3617494152189479e-27, + "velocityY": -5.3557690609839536e-27, + "timestamp": 0 + }, + { + "x": 0.4546918803213756, + "y": 4.109218526997661, + "heading": -0.008937300145995024, + "angularVelocity": -0.11893237262991248, + "velocityX": 0.28544257523423766, + "velocityY": -0.15856789979825608, + "timestamp": 0.07514606787342623 + }, + { + "x": 0.4975916635116252, + "y": 4.0853873108981436, + "heading": -0.02681241329663613, + "angularVelocity": -0.23787157008333967, + "velocityX": 0.5708852692400183, + "velocityY": -0.3171319108760033, + "timestamp": 0.15029213574685246 + }, + { + "x": 0.5619414537480726, + "y": 4.049641027140498, + "heading": -0.05362171709802378, + "angularVelocity": -0.3567625633658495, + "velocityX": 0.8563294402155045, + "velocityY": -0.4756906750976654, + "timestamp": 0.2254382036202787 + }, + { + "x": 0.6477414523773108, + "y": 4.001980177860296, + "heading": -0.08935853104911338, + "angularVelocity": -0.4755646564406219, + "velocityX": 1.1417762905939028, + "velocityY": -0.6342427571923064, + "timestamp": 0.3005842714937049 + }, + { + "x": 0.7549919358777417, + "y": 3.9424053758532884, + "heading": -0.13401442200652888, + "angularVelocity": -0.5942545261667251, + "velocityX": 1.4272268201854599, + "velocityY": -0.7927866845588466, + "timestamp": 0.3757303393671312 + }, + { + "x": 0.8836932393269187, + "y": 3.8709173416225426, + "heading": -0.18758057158075359, + "angularVelocity": -0.7128270459134325, + "velocityX": 1.7126818087934759, + "velocityY": -0.9513210238912064, + "timestamp": 0.45087640724055744 + }, + { + "x": 1.0338457421402332, + "y": 3.7875168932975942, + "heading": -0.2500489995023012, + "angularVelocity": -0.8312933688928013, + "velocityX": 1.9981418464400145, + "velocityY": -1.1098444760333412, + "timestamp": 0.5260224751139837 + }, + { + "x": 1.2054498603190875, + "y": 3.692204931169302, + "heading": -0.3214134496146474, + "angularVelocity": -0.9496764385935714, + "velocityX": 2.283607419990346, + "velocityY": -1.268355947630322, + "timestamp": 0.60116854298741 + }, + { + "x": 1.3985060453231266, + "y": 3.584982424979261, + "heading": -0.40166985798592575, + "angularVelocity": -1.0680054278616389, + "velocityX": 2.569079001302066, + "velocityY": -1.4268545144723166, + "timestamp": 0.6763146108608362 + }, + { + "x": 1.5915937569821017, + "y": 3.4778198154599655, + "heading": -0.48176859267037747, + "angularVelocity": -1.0659071984893158, + "velocityX": 2.5694985396202785, + "velocityY": -1.4260574445464886, + "timestamp": 0.7514606787342625 + }, + { + "x": 1.7632284858900344, + "y": 3.382566036520045, + "heading": -0.552976547680448, + "angularVelocity": -0.9475938931363788, + "velocityX": 2.2840147697019764, + "velocityY": -1.2675816797275836, + "timestamp": 0.8266067466076887 + }, + { + "x": 1.9134098098562773, + "y": 3.299220400739088, + "heading": -0.6152918418017485, + "angularVelocity": -0.8292555536806328, + "velocityX": 1.9985253815170176, + "velocityY": -1.1091150626981823, + "timestamp": 0.901752814481115 + }, + { + "x": 2.04213735776467, + "y": 3.2277823059512354, + "heading": -0.6687115003742338, + "angularVelocity": -0.7108776291856513, + "velocityX": 1.7130310547348613, + "velocityY": -0.9506564589404838, + "timestamp": 0.9768988823545413 + }, + { + "x": 2.1494108084857926, + "y": 3.1682512394298143, + "heading": -0.7132320418113052, + "angularVelocity": -0.5924533737688112, + "velocityX": 1.4275324545498602, + "velocityY": -0.7922046782500088, + "timestamp": 1.0520449502279674 + }, + { + "x": 2.235229893671302, + "y": 3.120626784362015, + "heading": -0.7488501306819726, + "angularVelocity": -0.4739847323836216, + "velocityX": 1.142030283341778, + "velocityY": -0.6337584442610695, + "timestamp": 1.1271910181013935 + }, + { + "x": 2.2995943987852447, + "y": 3.0849086278583613, + "heading": -0.7755632624607874, + "angularVelocity": -0.35548276223593744, + "velocityX": 0.8565252572144713, + "velocityY": -0.4753163740226105, + "timestamp": 1.2023370859748197 + }, + { + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080856, + "angularVelocity": -0.2369668826490278, + "velocityX": 0.5710180813549185, + "velocityY": -0.31687698560575434, + "timestamp": 1.2774831538482458 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288599550668, + "velocityX": 0.28550943973314763, + "velocityY": -0.15843872456938438, + "timestamp": 1.352629221721672 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.820153695476826e-18, + "velocityX": -5.7963239407122954e-18, + "velocityY": -1.1530643297922074e-18, + "timestamp": 1.4277752895950981 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGCSprint.2.traj b/src/main/deploy/choreo/SourceLanePGCSprint.2.traj new file mode 100644 index 00000000..38e969e1 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGCSprint.2.traj @@ -0,0 +1,841 @@ +{ + "samples": [ + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.820153695476826e-18, + "velocityX": -5.7963239407122954e-18, + "velocityY": -1.1530643297922074e-18, + "timestamp": 0 + }, + { + "x": 2.37735740952237, + "y": 3.0408026970784268, + "heading": -0.7860869247798871, + "angularVelocity": 0.2648554274741457, + "velocityX": 0.21924714610297322, + "velocityY": -0.1372563412236913, + "timestamp": 0.061110649511906034 + }, + { + "x": 2.404179793787175, + "y": 3.024019444447085, + "heading": -0.7541504516362038, + "angularVelocity": 0.5226007806947081, + "velocityX": 0.4389150578342217, + "velocityY": -0.2746371175137346, + "timestamp": 0.12222129902381207 + }, + { + "x": 2.4444554974169383, + "y": 2.998832316814256, + "heading": -0.7069723408476191, + "angularVelocity": 0.7720112806098135, + "velocityX": 0.6590619466729163, + "velocityY": -0.41215611082519754, + "timestamp": 0.1833319485357181 + }, + { + "x": 2.4982188560395806, + "y": 2.96523040511413, + "heading": -0.645190074686331, + "angularVelocity": 1.0109901736398696, + "velocityX": 0.8797706954852063, + "velocityY": -0.5498536174710421, + "timestamp": 0.24444259804762414 + }, + { + "x": 2.5655102024042766, + "y": 2.923198127539445, + "heading": -0.5696285601591381, + "angularVelocity": 1.2364704864161304, + "velocityX": 1.101139439723772, + "velocityY": -0.6878061010707456, + "timestamp": 0.30555324755953017 + }, + { + "x": 2.646375841205288, + "y": 2.8727129664624207, + "heading": -0.48135684674506, + "angularVelocity": 1.4444571301255718, + "velocityX": 1.3232659028645477, + "velocityY": -0.8261270577264651, + "timestamp": 0.3666638970714362 + }, + { + "x": 2.740867207337065, + "y": 2.813744425609022, + "heading": -0.38173698900245784, + "angularVelocity": 1.6301554399809315, + "velocityX": 1.5462340342720005, + "velocityY": -0.9649470480903679, + "timestamp": 0.42777454658334224 + }, + { + "x": 2.849040211493812, + "y": 2.746255741762665, + "heading": -0.2724850390795677, + "angularVelocity": 1.7877726843928354, + "velocityX": 1.7701170748589676, + "velocityY": -1.1043686229060403, + "timestamp": 0.4888851960952483 + }, + { + "x": 2.9709557136972373, + "y": 2.6702088910620962, + "heading": -0.155828372393994, + "angularVelocity": 1.9089416921161393, + "velocityX": 1.994996014232719, + "velocityY": -1.2444124110602532, + "timestamp": 0.5499958456071543 + }, + { + "x": 3.1066769296559538, + "y": 2.585572206493352, + "heading": -0.03496320783780511, + "angularVelocity": 1.977808541089754, + "velocityX": 2.2209094002882996, + "velocityY": -1.3849743906298195, + "timestamp": 0.6111064951190603 + }, + { + "x": 3.2562355055408387, + "y": 2.4923383100856262, + "heading": 0.08475597327951304, + "angularVelocity": 1.959055943170691, + "velocityX": 2.4473406366879895, + "velocityY": -1.5256571015427038, + "timestamp": 0.6722171446309666 + }, + { + "x": 3.4194188611023764, + "y": 2.3906298265628587, + "heading": 0.19293288100296513, + "angularVelocity": 1.7701809518876104, + "velocityX": 2.6702932609110017, + "velocityY": -1.6643332109070865, + "timestamp": 0.7333277941428729 + }, + { + "x": 3.5932872633720114, + "y": 2.2801851899216916, + "heading": 0.26578166446591744, + "angularVelocity": 1.192080006427692, + "velocityX": 2.845140800471438, + "velocityY": -1.8072895235657866, + "timestamp": 0.7944384436547791 + }, + { + "x": 3.7772765002319786, + "y": 2.1608722925529302, + "heading": 0.30020319888903974, + "angularVelocity": 0.5632657269731189, + "velocityX": 3.0107557083666587, + "velocityY": -1.9524076134310657, + "timestamp": 0.8555490931666854 + }, + { + "x": 3.9732507395108834, + "y": 2.038358982543431, + "heading": 0.30020324669419357, + "angularVelocity": 7.822720631834703e-7, + "velocityX": 3.2068754111462736, + "velocityY": -2.0047783976771947, + "timestamp": 0.9166597426785916 + }, + { + "x": 4.174667645417682, + "y": 1.9250159473015482, + "heading": 0.3002032939445946, + "angularVelocity": 7.731942209741845e-7, + "velocityX": 3.2959379014218597, + "velocityY": -1.8547182225546543, + "timestamp": 0.9777703921904979 + }, + { + "x": 4.383569049898422, + "y": 1.8261442295125712, + "heading": 0.30020334201253857, + "angularVelocity": 7.865722968351813e-7, + "velocityX": 3.4184124395542623, + "velocityY": -1.6179130573585891, + "timestamp": 1.0388810417024041 + }, + { + "x": 4.59891708073721, + "y": 1.7422357169202292, + "heading": 0.3002033920988207, + "angularVelocity": 8.195998987503142e-7, + "velocityX": 3.52390348587004, + "velocityY": -1.3730587591937538, + "timestamp": 1.0999916912143104 + }, + { + "x": 4.819641568305972, + "y": 1.6737075280608862, + "heading": 0.30020344564176327, + "angularVelocity": 8.761638607213102e-7, + "velocityX": 3.6118825332687328, + "velocityY": -1.1213788334223447, + "timestamp": 1.1611023407262167 + }, + { + "x": 5.044645579459654, + "y": 1.6209002757550093, + "heading": 0.30020350452029126, + "angularVelocity": 9.634740995599556e-7, + "velocityX": 3.681911629982681, + "velocityY": -0.8641252012153561, + "timestamp": 1.222212990238123 + }, + { + "x": 5.272810899090047, + "y": 1.584076421741111, + "heading": 0.3002035713939988, + "angularVelocity": 0.0000010943052979537505, + "velocityX": 3.733642523075129, + "velocityY": -0.6025767081190017, + "timestamp": 1.2833236397500292 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3002036452391599, + "angularVelocity": 0.0000012083844913604929, + "velocityX": 3.7668180588495743, + "velocityY": -0.33803334595452383, + "timestamp": 1.3444342892619354 + }, + { + "x": 5.642203177619129, + "y": 1.5568731039887571, + "heading": 0.3002037251666547, + "angularVelocity": 0.0000021691766198010436, + "velocityX": 3.7777797970827858, + "velocityY": -0.177650642031358, + "timestamp": 1.3812812173086941 + }, + { + "x": 5.781555187145968, + "y": 1.5562486567630798, + "heading": 0.30020379895395954, + "angularVelocity": 0.0000020025361337225055, + "velocityX": 3.78191661866635, + "velocityY": -0.016947063399286564, + "timestamp": 1.4181281453554528 + }, + { + "x": 5.9208078464820115, + "y": 1.5615467685843438, + "heading": 0.30020386782175307, + "angularVelocity": 0.0000018690240183201108, + "velocityX": 3.7792203235866277, + "velocityY": 0.14378706997068585, + "timestamp": 1.4549750734022115 + }, + { + "x": 6.059709556234372, + "y": 1.5727578639536914, + "heading": 0.3002039327408759, + "angularVelocity": 0.0000017618598429234572, + "velocityX": 3.7696957959723445, + "velocityY": 0.30426133096145186, + "timestamp": 1.4918220014489703 + }, + { + "x": 6.198009351638548, + "y": 1.5898616832743162, + "heading": 0.30020399449987484, + "angularVelocity": 0.0000016760962775678482, + "velocityX": 3.753360259196494, + "velocityY": 0.46418576058553174, + "timestamp": 1.528668929495729 + }, + { + "x": 6.335457356092555, + "y": 1.6128273194464042, + "heading": 0.30020405375523124, + "angularVelocity": 0.0000016081491612309129, + "velocityX": 3.730243245233006, + "velocityY": 0.6232713930166671, + "timestamp": 1.5655158575424877 + }, + { + "x": 6.471805232718414, + "y": 1.6416132738069629, + "heading": 0.3002041110675489, + "angularVelocity": 0.0000015554164405322294, + "velocityX": 3.700386541120455, + "velocityY": 0.7812307805966762, + "timestamp": 1.6023627855892464 + }, + { + "x": 6.60680663253268, + "y": 1.676167531284266, + "heading": 0.3002041691027596, + "angularVelocity": 0.0000015750352547385845, + "velocityX": 3.6638440969339445, + "velocityY": 0.937778515306732, + "timestamp": 1.639209713636005 + }, + { + "x": 6.740176057879373, + "y": 1.7164157557118485, + "heading": 0.3003755003099619, + "angularVelocity": 0.004649809801927131, + "velocityX": 3.6195534449289073, + "velocityY": 1.0923088181600296, + "timestamp": 1.6760566416827638 + }, + { + "x": 6.869837525657242, + "y": 1.7607930294165437, + "heading": 0.3093430143729343, + "angularVelocity": 0.2433720947264084, + "velocityX": 3.5189220554106844, + "velocityY": 1.2043683437702197, + "timestamp": 1.7129035697295225 + }, + { + "x": 6.995165288352465, + "y": 1.807928452248917, + "heading": 0.3301850856624388, + "angularVelocity": 0.5656393190514163, + "velocityX": 3.4013083135772337, + "velocityY": 1.2792225927914125, + "timestamp": 1.7497504977762812 + }, + { + "x": 7.114963240628345, + "y": 1.8557254140007795, + "heading": 0.35927322814233364, + "angularVelocity": 0.7894319559824341, + "velocityX": 3.2512331048020355, + "velocityY": 1.2971762989633377, + "timestamp": 1.78659742582304 + }, + { + "x": 7.228886146559884, + "y": 1.9032191557267804, + "heading": 0.3911508310575271, + "angularVelocity": 0.8651359721153189, + "velocityX": 3.091788433135363, + "velocityY": 1.2889471183522156, + "timestamp": 1.8234443538697986 + }, + { + "x": 7.336942556890349, + "y": 1.949894399055616, + "heading": 0.423151819771819, + "angularVelocity": 0.8684845768875223, + "velocityX": 2.9325758226938556, + "velocityY": 1.2667336411221237, + "timestamp": 1.8602912819165573 + }, + { + "x": 7.43919043499382, + "y": 1.9954619855368936, + "heading": 0.45375826572504047, + "angularVelocity": 0.8306376562622595, + "velocityX": 2.7749362979111796, + "velocityY": 1.2366726046592738, + "timestamp": 1.897138209963316 + }, + { + "x": 7.535687560612936, + "y": 2.0397400223416313, + "heading": 0.48197582868140687, + "angularVelocity": 0.7658050332054913, + "velocityX": 2.618864875157601, + "velocityY": 1.201675123325048, + "timestamp": 1.9339851380100748 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6818119458710672, + "velocityX": 2.464149969521805, + "velocityY": 1.163303151067446, + "timestamp": 1.9708320660568335 + }, + { + "x": 7.766065583230135, + "y": 2.1519617180113952, + "heading": 0.5396264377138885, + "angularVelocity": 0.5126079654708053, + "velocityX": 2.1996686083558368, + "velocityY": 1.0930061650047849, + "timestamp": 2.0342878363776813 + }, + { + "x": 7.889178097615864, + "y": 2.215679412144434, + "heading": 0.5626262771579723, + "angularVelocity": 0.3624546566496878, + "velocityX": 1.9401311143689866, + "velocityY": 1.004127659484183, + "timestamp": 2.097743606698529 + }, + { + "x": 7.996082408378713, + "y": 2.2729729622929304, + "heading": 0.5769453477724344, + "angularVelocity": 0.22565435014123394, + "velocityX": 1.6847059018008188, + "velocityY": 0.90288952224213, + "timestamp": 2.161199377019377 + }, + { + "x": 8.086990111713977, + "y": 2.3232868849538337, + "heading": 0.5832040110121259, + "angularVelocity": 0.09863032483958853, + "velocityX": 1.432615235393288, + "velocityY": 0.7928975159627525, + "timestamp": 2.2246551473402247 + }, + { + "x": 8.16207340516984, + "y": 2.3662086171292933, + "heading": 0.581874089552229, + "angularVelocity": -0.020958243090781536, + "velocityX": 1.183238231546559, + "velocityY": 0.676403926048603, + "timestamp": 2.2881109176610726 + }, + { + "x": 8.221474033046453, + "y": 2.401420292157025, + "heading": 0.5733258497363348, + "angularVelocity": -0.1347117807044565, + "velocityX": 0.9360949772773745, + "velocityY": 0.5549010728211649, + "timestamp": 2.3515666879819204 + }, + { + "x": 8.265310204045273, + "y": 2.4286697945211704, + "heading": 0.5578573342178689, + "angularVelocity": -0.24376846172150818, + "velocityX": 0.6908145748948221, + "velocityY": 0.4294251291311274, + "timestamp": 2.415022458302768 + }, + { + "x": 8.293681756385627, + "y": 2.447752418352113, + "heading": 0.5357134031725193, + "angularVelocity": -0.34896638924064666, + "velocityX": 0.44710752382171043, + "velocityY": 0.30072322397878726, + "timestamp": 2.478478228623616 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.45094242234392296, + "velocityX": 0.20474492941705758, + "velocityY": 0.16935099752034305, + "timestamp": 2.541933998944464 + }, + { + "x": 8.303680612059921, + "y": 2.4605220907329786, + "heading": 0.4706360870687623, + "angularVelocity": -0.5539055745477068, + "velocityX": -0.04547302080206286, + "velocityY": 0.030737357256443772, + "timestamp": 2.6077618566137937 + }, + { + "x": 8.28417079095848, + "y": 2.4534909416021047, + "heading": 0.42765554839737563, + "angularVelocity": -0.6529232485020114, + "velocityX": -0.2963763639315537, + "velocityY": -0.1068111492583101, + "timestamp": 2.6735897142831235 + }, + { + "x": 8.248092493080108, + "y": 2.437487576077448, + "heading": 0.3784580606750378, + "angularVelocity": -0.7473657728536394, + "velocityX": -0.5480703634561933, + "velocityY": -0.24310931710774963, + "timestamp": 2.7394175719524534 + }, + { + "x": 8.195385029545998, + "y": 2.412609981570135, + "heading": 0.3233968459430461, + "angularVelocity": -0.8364424528074197, + "velocityX": -0.8006862960492162, + "velocityY": -0.37791894477683524, + "timestamp": 2.8052454296217832 + }, + { + "x": 8.125976752501963, + "y": 2.378976757579925, + "heading": 0.2628923564512202, + "angularVelocity": -0.9191319850595123, + "velocityX": -1.0543906410063166, + "velocityY": -0.5109269112046505, + "timestamp": 2.871073287291113 + }, + { + "x": 8.039781850826214, + "y": 2.336734306251694, + "heading": 0.19745501482460134, + "angularVelocity": -0.9940676173198235, + "velocityX": -1.3093985544650038, + "velocityY": -0.641710862602064, + "timestamp": 2.936901144960443 + }, + { + "x": 7.936695821305385, + "y": 2.286067717588345, + "heading": 0.12772072746128918, + "angularVelocity": -1.0593431084086873, + "velocityX": -1.565993990548153, + "velocityY": -0.7696830864200519, + "timestamp": 3.002729002629773 + }, + { + "x": 7.816588930914964, + "y": 2.2272180395232652, + "heading": 0.05450854772010129, + "angularVelocity": -1.1121762477665882, + "velocityX": -1.8245602187716414, + "velocityY": -0.8939935180740173, + "timestamp": 3.0685568602991027 + }, + { + "x": 7.679296617994528, + "y": 2.1605113660425403, + "heading": -0.02108172308249024, + "angularVelocity": -1.1483021547245422, + "velocityX": -2.085626325712903, + "velocityY": -1.01335021133165, + "timestamp": 3.1343847179684325 + }, + { + "x": 7.52460540494712, + "y": 2.086411774056454, + "heading": -0.09749530920005847, + "angularVelocity": -1.16080925041513, + "velocityX": -2.34993540006212, + "velocityY": -1.1256570486967377, + "timestamp": 3.2002125756377624 + }, + { + "x": 7.352233710537335, + "y": 2.0056280451301003, + "heading": -0.17238692358324137, + "angularVelocity": -1.1376887693867086, + "velocityX": -2.6185220135167313, + "velocityY": -1.2271966882493952, + "timestamp": 3.2660404333070923 + }, + { + "x": 7.161817946345765, + "y": 1.9193611743859953, + "heading": -0.24189742475427997, + "angularVelocity": -1.055943541717672, + "velocityX": -2.8926319484385736, + "velocityY": -1.3104918464375106, + "timestamp": 3.331868290976422 + }, + { + "x": 6.953014973362115, + "y": 1.8300106922230066, + "heading": -0.298719434373675, + "angularVelocity": -0.8631909290566133, + "velocityX": -3.1719545550536123, + "velocityY": -1.3573354097564532, + "timestamp": 3.397696148645752 + }, + { + "x": 6.727431268978415, + "y": 1.7436413956339059, + "heading": -0.3201876383615551, + "angularVelocity": -0.32612642653085444, + "velocityX": -3.4268729436231227, + "velocityY": -1.3120478114745338, + "timestamp": 3.463524006315082 + }, + { + "x": 6.489449879790782, + "y": 1.6705324422161696, + "heading": -0.3201878815696182, + "angularVelocity": -0.0000036946069898928346, + "velocityX": -3.61520787115807, + "velocityY": -1.1106081225517261, + "timestamp": 3.5293518639844117 + }, + { + "x": 6.246606826324383, + "y": 1.6156931769447658, + "heading": -0.3201879117478121, + "angularVelocity": -4.584410758613646e-7, + "velocityX": -3.689062078949349, + "velocityY": -0.8330707881589553, + "timestamp": 3.5951797216537416 + }, + { + "x": 6.0003025647982255, + "y": 1.5794398654466013, + "heading": -0.32018794628515007, + "angularVelocity": -5.246614307457641e-7, + "velocityX": -3.741641764546064, + "velocityY": -0.5507290193199733, + "timestamp": 3.6610075793230714 + }, + { + "x": 5.7519574472803, + "y": 1.5619815744715873, + "heading": -0.32018798749762734, + "angularVelocity": -6.260643854910128e-7, + "velocityX": -3.772644687381865, + "velocityY": -0.26521128885450773, + "timestamp": 3.7268354369924013 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.32018803965163717, + "angularVelocity": -7.922787054710825e-7, + "velocityX": -3.7818920261895626, + "velocityY": 0.021835891254127007, + "timestamp": 3.792663294661731 + }, + { + "x": 5.250083628410863, + "y": 1.584512642850435, + "heading": -0.3201880864656727, + "angularVelocity": -6.975962896264183e-7, + "velocityX": -3.768870382202017, + "velocityY": 0.31432577229262343, + "timestamp": 3.859770927241254 + }, + { + "x": 4.999553273526972, + "y": 1.6251081857259102, + "heading": -0.32018812296137045, + "angularVelocity": -5.438382542161688e-7, + "velocityX": -3.7332617059171653, + "velocityY": 0.6049318283932773, + "timestamp": 3.926878559820777 + }, + { + "x": 4.752913977063861, + "y": 1.6849623155206188, + "heading": -0.3201881529794151, + "angularVelocity": -4.473119302496649e-7, + "velocityX": -3.675279353221748, + "velocityY": 0.8919124024198194, + "timestamp": 3.9939861924003 + }, + { + "x": 4.511643860653374, + "y": 1.7637163133941143, + "heading": -0.3201881787520496, + "angularVelocity": -3.8404922716455346e-7, + "velocityX": -3.595270867655492, + "velocityY": 1.1735475510952014, + "timestamp": 4.061093824979823 + }, + { + "x": 4.277188862383835, + "y": 1.86089818593956, + "heading": -0.3201882016908444, + "angularVelocity": -3.41820951724987e-7, + "velocityX": -3.4937158301883047, + "velocityY": 1.4481493208732221, + "timestamp": 4.128201457559346 + }, + { + "x": 4.050954064298834, + "y": 1.9759254853688604, + "heading": -0.32018822275897413, + "angularVelocity": -3.1394535808439923e-7, + "velocityX": -3.3712230544999895, + "velocityY": 1.7140717830120589, + "timestamp": 4.195309090138869 + }, + { + "x": 3.834295246075004, + "y": 2.1081087693328064, + "heading": -0.32018824266587215, + "angularVelocity": -2.966413400864343e-7, + "velocityX": -3.2285272165887804, + "velocityY": 1.969720565053586, + "timestamp": 4.262416722718392 + }, + { + "x": 3.6285105963453295, + "y": 2.256655537498403, + "heading": -0.32018826197928, + "angularVelocity": -2.8779748460931035e-7, + "velocityX": -3.066486505626901, + "velocityY": 2.213559955189428, + "timestamp": 4.329524355297915 + }, + { + "x": 3.4323361610843617, + "y": 2.417680552436319, + "heading": -0.3201882811600919, + "angularVelocity": -2.858216149634079e-7, + "velocityX": -2.9232805229494647, + "velocityY": 2.399503733753405, + "timestamp": 4.396631987877438 + }, + { + "x": 3.2362856931678534, + "y": 2.578856476556199, + "heading": -0.32018830034052725, + "angularVelocity": -2.858160039511642e-7, + "velocityX": -2.921433231669861, + "velocityY": 2.401752497063368, + "timestamp": 4.463739620456961 + }, + { + "x": 3.0402351711109183, + "y": 2.7400323348209366, + "heading": -0.3201883195209977, + "angularVelocity": -2.858165259418001e-7, + "velocityX": -2.9214340384398776, + "velocityY": 2.401751515727269, + "timestamp": 4.530847253036484 + }, + { + "x": 2.8448193652926044, + "y": 2.899781496680513, + "heading": -0.32456406822795664, + "angularVelocity": -0.06520493331028634, + "velocityX": -2.911975855902028, + "velocityY": 2.380491692510139, + "timestamp": 4.597954885616007 + }, + { + "x": 2.66318575993582, + "y": 3.04911523833719, + "heading": -0.3692876119349906, + "angularVelocity": -0.6664449629337832, + "velocityX": -2.706601296678858, + "velocityY": 2.2252869892216163, + "timestamp": 4.66506251819553 + }, + { + "x": 2.496643428698106, + "y": 3.186035937240345, + "heading": -0.4222622822262224, + "angularVelocity": -0.7893985863449552, + "velocityX": -2.4817196619231154, + "velocityY": 2.040314844081317, + "timestamp": 4.732170150775053 + }, + { + "x": 2.3452748100493945, + "y": 3.3104804771184444, + "heading": -0.4767233164983405, + "angularVelocity": -0.8115475420412368, + "velocityX": -2.255609575696765, + "velocityY": 1.8544021759467144, + "timestamp": 4.799277783354576 + }, + { + "x": 2.209079907655855, + "y": 3.4224492823331634, + "heading": -0.5296433021172221, + "angularVelocity": -0.7885837062747133, + "velocityX": -2.0294994348392206, + "velocityY": 1.6684958314098723, + "timestamp": 4.866385415934099 + }, + { + "x": 2.088048432415951, + "y": 3.5219507247047837, + "heading": -0.5792907495482946, + "angularVelocity": -0.7398181923977142, + "velocityX": -1.803542616355617, + "velocityY": 1.4827142390056927, + "timestamp": 4.933493048513622 + }, + { + "x": 1.9821698412647601, + "y": 3.6089936285690154, + "heading": -0.624538681260696, + "angularVelocity": -0.6742590965160707, + "velocityX": -1.5777429046647484, + "velocityY": 1.297064141863228, + "timestamp": 5.000600681093145 + }, + { + "x": 1.8914349570352331, + "y": 3.6835858822586136, + "heading": -0.6645923745497577, + "angularVelocity": -0.5968574922025148, + "velocityX": -1.3520799459287431, + "velocityY": 1.1115315921956592, + "timestamp": 5.067708313672668 + }, + { + "x": 1.815836039628641, + "y": 3.7457342610555044, + "heading": -0.698860244587487, + "angularVelocity": -0.5106404252470358, + "velocityX": -1.1265323257679698, + "velocityY": 0.926100003948804, + "timestamp": 5.134815946252191 + }, + { + "x": 1.7553665690239355, + "y": 3.7954445247907986, + "heading": -0.7268845476309118, + "angularVelocity": -0.41760231982876106, + "velocityX": -0.9010818632746272, + "velocityY": 0.7407542454487245, + "timestamp": 5.201923578831714 + }, + { + "x": 1.7100210087875185, + "y": 3.8327215653417337, + "heading": -0.7483006908429537, + "angularVelocity": -0.31913125808251036, + "velocityX": -0.6757139015846285, + "velocityY": 0.5554813829374979, + "timestamp": 5.269031211411237 + }, + { + "x": 1.6797946046172731, + "y": 3.8575695457400823, + "heading": -0.7628116421295846, + "angularVelocity": -0.21623399200434476, + "velocityX": -0.4504167858168276, + "velocityY": 0.37027055557210875, + "timestamp": 5.33613884399076 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.10966416437475777, + "velocityX": -0.22518126874279432, + "velocityY": 0.1851126545280661, + "timestamp": 5.403246476570283 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -1.211900873027185e-19, + "velocityX": -5.462077711506373e-19, + "velocityY": -3.512888080115249e-19, + "timestamp": 5.470354109149806 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGCSprint.3.traj b/src/main/deploy/choreo/SourceLanePGCSprint.3.traj new file mode 100644 index 00000000..d9e2ca2a --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGCSprint.3.traj @@ -0,0 +1,319 @@ +{ + "samples": [ + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -1.211900873027185e-19, + "velocityX": -5.462077711506373e-19, + "velocityY": -3.512888080115249e-19, + "timestamp": 0 + }, + { + "x": 1.659892349205253, + "y": 3.8800826826438297, + "heading": -0.7264777443526473, + "angularVelocity": 0.7466474211356967, + "velocityX": -0.08186842286768115, + "velocityY": 0.1724334423754288, + "timestamp": 0.05851918722290517 + }, + { + "x": 1.6562452238185605, + "y": 3.90221669421699, + "heading": -0.6460681306884017, + "angularVelocity": 1.3740726329290587, + "velocityX": -0.06232358239700476, + "velocityY": 0.3782351160970439, + "timestamp": 0.11703837444581033 + }, + { + "x": 1.6553319250883793, + "y": 3.9350953760295733, + "heading": -0.5183992679116819, + "angularVelocity": 2.1816581677803772, + "velocityX": -0.01560682527428515, + "velocityY": 0.5618444714097892, + "timestamp": 0.1755575616687155 + }, + { + "x": 1.6623538133314129, + "y": 3.9788519148323376, + "heading": -0.3621106955703112, + "angularVelocity": 2.670723565350507, + "velocityX": 0.11999292157436166, + "velocityY": 0.7477297768352748, + "timestamp": 0.23407674889162067 + }, + { + "x": 1.6832034751641156, + "y": 4.019420947962781, + "heading": -0.22540240918746318, + "angularVelocity": 2.3361275655130602, + "velocityX": 0.35628761816674753, + "velocityY": 0.6932603656286723, + "timestamp": 0.29259593611452583 + }, + { + "x": 1.7146647081648767, + "y": 4.052413452071288, + "heading": -0.11878752772286896, + "angularVelocity": 1.8218790541038834, + "velocityX": 0.5376225216683612, + "velocityY": 0.5637895137339223, + "timestamp": 0.351115123337431 + }, + { + "x": 1.7541026264736177, + "y": 4.074977116543675, + "heading": -0.0408151972626467, + "angularVelocity": 1.332423332593777, + "velocityX": 0.6739314091721454, + "velocityY": 0.38557720199428364, + "timestamp": 0.40963431056033617 + }, + { + "x": 1.799090355046403, + "y": 4.0866639858943925, + "heading": 1.7934233852716553e-24, + "angularVelocity": 0.6974669198185859, + "velocityX": 0.7687688552717376, + "velocityY": 0.199710042215758, + "timestamp": 0.46815349778324133 + }, + { + "x": 1.848745584487915, + "y": 4.087520122528076, + "heading": 7.573339964499894e-25, + "angularVelocity": -5.2358045608260825e-25, + "velocityX": 0.848529034628775, + "velocityY": 0.014630015800164336, + "timestamp": 0.5266726850061465 + }, + { + "x": 1.9382598731728313, + "y": 4.089063493944204, + "heading": 6.819777064270339e-25, + "angularVelocity": -2.006050179968467e-26, + "velocityX": 1.1795649340806893, + "velocityY": 0.02033761122914328, + "timestamp": 0.6025602314214531 + }, + { + "x": 2.0528956622357724, + "y": 4.091040000742217, + "heading": 6.067470023829544e-25, + "angularVelocity": -1.8954686232133377e-26, + "velocityX": 1.510600809724148, + "velocityY": 0.026045206247625866, + "timestamp": 0.6784477778367597 + }, + { + "x": 2.1926529480236727, + "y": 4.09344964285913, + "heading": 5.320081952060173e-25, + "angularVelocity": -2.1711320260805291e-26, + "velocityX": 1.8416366372297248, + "velocityY": 0.03175280043613335, + "timestamp": 0.7543353242520663 + }, + { + "x": 2.3575317192504395, + "y": 4.096292420100353, + "heading": 4.5695624647021355e-25, + "angularVelocity": -2.0004263459154622e-26, + "velocityX": 2.1726723160140353, + "velocityY": 0.03746039206044517, + "timestamp": 0.8302228706673729 + }, + { + "x": 2.4972894766259857, + "y": 4.098702070348201, + "heading": 3.822235688125773e-25, + "angularVelocity": -1.9197661326309743e-26, + "velocityX": 1.8416428515253338, + "velocityY": 0.0317529075806614, + "timestamp": 0.9061104170826795 + }, + { + "x": 2.611925746748375, + "y": 4.100678585440459, + "heading": 3.063572284762821e-25, + "angularVelocity": -2.4686338901390002e-26, + "velocityX": 1.5106071488334032, + "velocityY": 0.026045315544143462, + "timestamp": 0.9819979634979861 + }, + { + "x": 2.701440519692856, + "y": 4.102221965206007, + "heading": 2.3114445870748474e-25, + "angularVelocity": -1.5210807516831905e-26, + "velocityX": 1.1795713153591425, + "velocityY": 0.020337721252726194, + "timestamp": 1.0578855099132927 + }, + { + "x": 2.7658337920580536, + "y": 4.1033322095862, + "heading": 1.561656112047754e-25, + "angularVelocity": -1.9040979941775662e-26, + "velocityX": 0.8485354370636105, + "velocityY": 0.01463012618851756, + "timestamp": 1.1337730563286001 + }, + { + "x": 2.805105562125262, + "y": 4.104009318551404, + "heading": 8.077058611592956e-26, + "angularVelocity": -3.263981497829794e-26, + "velocityX": 0.5174995361200375, + "velocityY": 0.008922530733819996, + "timestamp": 1.2096606027439076 + }, + { + "x": 2.819255828857422, + "y": 4.10425329208374, + "heading": 3.4975515409752656e-27, + "angularVelocity": -3.9242842268499737e-26, + "velocityX": 0.18646362151070262, + "velocityY": 0.0032149350435025517, + "timestamp": 1.2855481491592151 + }, + { + "x": 2.810470941680652, + "y": 4.100290874791341, + "heading": -0.013665728423960572, + "angularVelocity": -0.18841669161843644, + "velocityX": -0.12112192828199068, + "velocityY": -0.05463196207942443, + "timestamp": 1.3580774368065907 + }, + { + "x": 2.779376565530904, + "y": 4.092132450200435, + "heading": -0.040981035402949714, + "angularVelocity": -0.3766107163742046, + "velocityX": -0.4287147600418107, + "velocityY": -0.11248455424752818, + "timestamp": 1.4306067244539662 + }, + { + "x": 2.7259718800290944, + "y": 4.079777209778572, + "heading": -0.08192049181497908, + "angularVelocity": -0.5644541362527821, + "velocityX": -0.7363189028058957, + "velocityY": -0.17034829408406268, + "timestamp": 1.5031360121013417 + }, + { + "x": 2.650255783412156, + "y": 4.06322370149856, + "heading": -0.13644943849416394, + "angularVelocity": -0.7518196917125987, + "velocityX": -1.0439382361654468, + "velocityY": -0.22823205379447506, + "timestamp": 1.5756652997487173 + }, + { + "x": 2.5522270104067304, + "y": 4.04246950295157, + "heading": -0.20452759410435262, + "angularVelocity": -0.9386298668914527, + "velocityX": -1.3515750145241086, + "velocityY": -0.2861492125483705, + "timestamp": 1.6481945873960928 + }, + { + "x": 2.4318844389859766, + "y": 4.01751088765649, + "heading": -0.2861194089559698, + "angularVelocity": -1.1249498995261227, + "velocityX": -1.6592272628656817, + "velocityY": -0.34411775028624336, + "timestamp": 1.7207238750434684 + }, + { + "x": 2.2892276526666033, + "y": 3.9883426531177015, + "heading": -0.3812142224775482, + "angularVelocity": -1.311122949172083, + "velocityX": -1.9668852534846832, + "velocityY": -0.40215801760798864, + "timestamp": 1.793253162690844 + }, + { + "x": 2.1330829846519728, + "y": 3.9587634030266514, + "heading": -0.4780632841326763, + "angularVelocity": -1.3353097044877917, + "velocityX": -2.1528498773319837, + "velocityY": -0.40782490839919366, + "timestamp": 1.8657824503382194 + }, + { + "x": 1.9992497997330843, + "y": 3.9334042162180256, + "heading": -0.5612966415288853, + "angularVelocity": -1.1475827227322806, + "velocityX": -1.8452295515373094, + "velocityY": -0.349640643541367, + "timestamp": 1.938311737985595 + }, + { + "x": 1.8877256900718635, + "y": 3.9122685100016006, + "heading": -0.6308164435957174, + "angularVelocity": -0.9585066160421192, + "velocityX": -1.537642423891296, + "velocityY": -0.29140926240973475, + "timestamp": 2.0108410256329705 + }, + { + "x": 1.7985083705984093, + "y": 3.895358481595443, + "heading": -0.6865270710591681, + "angularVelocity": -0.768112155386183, + "velocityX": -1.2300868017236275, + "velocityY": -0.23314758705988012, + "timestamp": 2.083370313280346 + }, + { + "x": 1.731596065924516, + "y": 3.88267537699245, + "heading": -0.7283506709725182, + "angularVelocity": -0.5766442946012179, + "velocityX": -0.9225556577807338, + "velocityY": -0.17486873254092689, + "timestamp": 2.1558996009277216 + }, + { + "x": 1.6869877424298634, + "y": 3.8742198088507074, + "heading": -0.7562373778990242, + "angularVelocity": -0.3844889124250871, + "velocityX": -0.6150387649128701, + "velocityY": -0.11658143097795469, + "timestamp": 2.228428888575097 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.19210952061606446, + "velocityX": -0.3075243171780863, + "velocityY": -0.05829081246861152, + "timestamp": 2.3009581762224727 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -5.999453085593573e-20, + "velocityX": -1.6973677435282043e-18, + "velocityY": -2.503873223513922e-18, + "timestamp": 2.373487463869848 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGCSprint.4.traj b/src/main/deploy/choreo/SourceLanePGCSprint.4.traj new file mode 100644 index 00000000..0cd0adae --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGCSprint.4.traj @@ -0,0 +1,391 @@ +{ + "samples": [ + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -5.999453085593573e-20, + "velocityX": -1.6973677435282043e-18, + "velocityY": -2.503873223513922e-18, + "timestamp": 0 + }, + { + "x": 1.6770247995246623, + "y": 3.8595873316999896, + "heading": -0.7649878221642326, + "angularVelocity": 0.08507451391469989, + "velocityX": 0.20257164680788678, + "velocityY": -0.1707799926111949, + "timestamp": 0.06092450226104873 + }, + { + "x": 1.70170978110981, + "y": 3.8387765844705157, + "heading": -0.7547381408402699, + "angularVelocity": 0.1682357827076709, + "velocityX": 0.405173299231529, + "velocityY": -0.3415825563958536, + "timestamp": 0.12184900452209746 + }, + { + "x": 1.7387402457793653, + "y": 3.80755819428449, + "heading": -0.7395583157876386, + "angularVelocity": 0.24915796583103744, + "velocityX": 0.607809063599532, + "velocityY": -0.5124110830197925, + "timestamp": 0.1827735067831462 + }, + { + "x": 1.7881185850906471, + "y": 3.7659303409499723, + "heading": -0.7196099497583256, + "angularVelocity": 0.3274276405876679, + "velocityX": 0.8104840824091846, + "velocityY": -0.6832694858326719, + "timestamp": 0.24369800904419492 + }, + { + "x": 1.8498475856412335, + "y": 3.713890927230462, + "heading": -0.6950873259687543, + "angularVelocity": 0.4025083977624826, + "velocityX": 1.0132048397554583, + "velocityY": -0.8541623121766772, + "timestamp": 0.30462251130524365 + }, + { + "x": 1.9239305360498986, + "y": 3.6514375329357844, + "heading": -0.6662283152580175, + "angularVelocity": 0.47368480069122665, + "velocityX": 1.2159795756924812, + "velocityY": -1.025094862935087, + "timestamp": 0.3655470135662924 + }, + { + "x": 2.010371367313072, + "y": 3.5785673636651287, + "heading": -0.6333309419257536, + "angularVelocity": 0.539969505065564, + "velocityX": 1.4188188340512586, + "velocityY": -1.1960732803104883, + "timestamp": 0.4264715158273411 + }, + { + "x": 2.1091748328759063, + "y": 3.4952772039974476, + "heading": -0.5967798759520212, + "angularVelocity": 0.5999403297070707, + "velocityX": 1.6217361143055882, + "velocityY": -1.3671044748269063, + "timestamp": 0.48739601808838984 + }, + { + "x": 2.220346716936752, + "y": 3.40156341112366, + "heading": -0.5570916262340567, + "angularVelocity": 0.6514333026129457, + "velocityX": 1.82474833498841, + "velocityY": -1.5381954615278515, + "timestamp": 0.5483205203494386 + }, + { + "x": 2.3438939700884585, + "y": 3.297422079154605, + "heading": -0.5149984011781104, + "angularVelocity": 0.6909079843703304, + "velocityX": 2.027874641016087, + "velocityY": -1.7093505585459128, + "timestamp": 0.6092450226104873 + }, + { + "x": 2.4798242349395756, + "y": 3.182849894813871, + "heading": -0.4716228537300689, + "angularVelocity": 0.7119557130263738, + "velocityX": 2.2311263909664074, + "velocityY": -1.8805600388792227, + "timestamp": 0.670169524871536 + }, + { + "x": 2.628141721858615, + "y": 3.057848265310819, + "heading": -0.42891150330345956, + "angularVelocity": 0.7010537442489162, + "velocityX": 2.4344472488840574, + "velocityY": -2.0517464216194523, + "timestamp": 0.7310940271325848 + }, + { + "x": 2.788815804109156, + "y": 2.922450520332472, + "heading": -0.3910899170501554, + "angularVelocity": 0.6207943413513182, + "velocityX": 2.6372654069800667, + "velocityY": -2.2223857389625743, + "timestamp": 0.7920185293936335 + }, + { + "x": 2.9613127738384817, + "y": 2.777334038124512, + "heading": -0.375202514467059, + "angularVelocity": 0.2607719717597751, + "velocityX": 2.831323413857593, + "velocityY": -2.3819067341111335, + "timestamp": 0.8529430316546822 + }, + { + "x": 3.137446263833113, + "y": 2.628782733735228, + "heading": -0.3752024981016849, + "angularVelocity": 2.6861728103084975e-7, + "velocityX": 2.891012375282727, + "velocityY": -2.438285072117188, + "timestamp": 0.913867533915731 + }, + { + "x": 3.313579787334416, + "y": 2.480231469074176, + "heading": -0.37520248173599324, + "angularVelocity": 2.686224924493493e-7, + "velocityX": 2.8910129252531243, + "velocityY": -2.4382844200276295, + "timestamp": 0.9747920361767797 + }, + { + "x": 3.489713311593346, + "y": 2.3316802053114225, + "heading": -0.37520246537030155, + "angularVelocity": 2.6862249292188543e-7, + "velocityX": 2.8910129376886133, + "velocityY": -2.4382844052831705, + "timestamp": 1.0357165384378284 + }, + { + "x": 3.6658468653792484, + "y": 2.183128976558075, + "heading": -0.37520244900461003, + "angularVelocity": 2.6862249035440315e-7, + "velocityX": 2.89101342233718, + "velocityY": -2.4382838306472756, + "timestamp": 1.0966410406988771 + }, + { + "x": 3.8419803666744587, + "y": 2.034577685567575, + "heading": -0.37520243263891834, + "angularVelocity": 2.686224937536833e-7, + "velocityX": 2.8910125607676815, + "velocityY": -2.4382848521928207, + "timestamp": 1.1575655429599259 + }, + { + "x": 4.017984969789346, + "y": 1.8858736976718657, + "heading": -0.37520241627306933, + "angularVelocity": 2.6862507525069377e-7, + "velocityX": 2.8888968573061926, + "velocityY": -2.440791182151078, + "timestamp": 1.2184900452209746 + }, + { + "x": 4.198449630537114, + "y": 1.742615181247194, + "heading": -0.37520239987870785, + "angularVelocity": 2.6909307205451447e-7, + "velocityX": 2.9621031612948867, + "velocityY": -2.3514105344815093, + "timestamp": 1.2794145474820233 + }, + { + "x": 4.38853011399539, + "y": 1.6123849775845955, + "heading": -0.3752023832167796, + "angularVelocity": 2.734848475989746e-7, + "velocityX": 3.1199349424936167, + "velocityY": -2.137566969436863, + "timestamp": 1.340339049743072 + }, + { + "x": 4.587288186636938, + "y": 1.495827162172114, + "heading": -0.3752023659192684, + "angularVelocity": 2.8391715306138947e-7, + "velocityX": 3.2623667861891117, + "velocityY": -1.9131517055823732, + "timestamp": 1.4012635520041208 + }, + { + "x": 4.7937421798706055, + "y": 1.3935176134109497, + "heading": -0.3752023473605566, + "angularVelocity": 3.046181942517293e-7, + "velocityX": 3.3886857597794955, + "velocityY": -1.6792841133570497, + "timestamp": 1.4621880542651695 + }, + { + "x": 5.0647852667187605, + "y": 1.2879733857720441, + "heading": -0.3752023309051681, + "angularVelocity": 2.139583629256744e-7, + "velocityX": 3.524191190248399, + "velocityY": -1.3723206946612971, + "timestamp": 1.539097356915418 + }, + { + "x": 5.34411641028699, + "y": 1.2068681953777092, + "heading": -0.37520231550400196, + "angularVelocity": 2.0025101753440068e-7, + "velocityX": 3.6319552244350577, + "velocityY": -1.0545563098285724, + "timestamp": 1.6160066595656666 + }, + { + "x": 5.629536746845857, + "y": 1.1508398853670567, + "heading": -0.375202300582326, + "angularVelocity": 1.940165285663064e-7, + "velocityX": 3.711128910592765, + "velocityY": -0.7284984791169633, + "timestamp": 1.6929159622159151 + }, + { + "x": 5.917306487549604, + "y": 1.108501764833761, + "heading": -0.37520228573814113, + "angularVelocity": 1.9300896478374464e-7, + "velocityX": 3.741676634521091, + "velocityY": -0.5504941414672958, + "timestamp": 1.7698252648661636 + }, + { + "x": 6.2050577552774975, + "y": 1.0660382742134262, + "heading": -0.37520227089383906, + "angularVelocity": 1.9301048840600142e-7, + "velocityX": 3.7414364428249387, + "velocityY": -0.5521242444940722, + "timestamp": 1.8467345675164122 + }, + { + "x": 6.492809022919495, + "y": 1.0235747830116217, + "heading": -0.3752022560493111, + "angularVelocity": 1.9301342582848665e-7, + "velocityX": 3.7414364417080774, + "velocityY": -0.5521242520545259, + "timestamp": 1.9236438701666607 + }, + { + "x": 6.772658036083171, + "y": 0.9819575118333774, + "heading": -0.34826612342726787, + "angularVelocity": 0.3502324386496837, + "velocityX": 3.638688734915609, + "velocityY": -0.541121421520385, + "timestamp": 2.0005531728169093 + }, + { + "x": 7.027520568845826, + "y": 0.9441505058911935, + "heading": -0.3001058813779335, + "angularVelocity": 0.6261952766409414, + "velocityX": 3.313806314454086, + "velocityY": -0.49157910212908845, + "timestamp": 2.077462475467158 + }, + { + "x": 7.256873768560855, + "y": 0.9101570740557017, + "heading": -0.24860478708038097, + "angularVelocity": 0.6696341342705694, + "velocityX": 2.9821255922450605, + "velocityY": -0.44199375971564203, + "timestamp": 2.1543717781174063 + }, + { + "x": 7.460704020800595, + "y": 0.8799617502558256, + "heading": -0.1984971865955797, + "angularVelocity": 0.6515154702763284, + "velocityX": 2.65026785077839, + "velocityY": -0.3926095122353607, + "timestamp": 2.231281080767655 + }, + { + "x": 7.6390235862814775, + "y": 0.8535543484261158, + "heading": -0.15201891065632164, + "angularVelocity": 0.6043258011403606, + "velocityX": 2.318569527171569, + "velocityY": -0.3433577073218239, + "timestamp": 2.3081903834179034 + }, + { + "x": 7.7918452312093995, + "y": 0.8309283282540864, + "heading": -0.11047967273973482, + "angularVelocity": 0.5401068074364143, + "velocityX": 1.9870371939645617, + "velocityY": -0.29419094169820903, + "timestamp": 2.385099686068152 + }, + { + "x": 7.9191796079863, + "y": 0.812079256076005, + "heading": -0.07474204501348639, + "angularVelocity": 0.46467236725273836, + "velocityX": 1.6556433667844261, + "velocityY": -0.2450818240258775, + "timestamp": 2.4620089887184005 + }, + { + "x": 8.021035341477521, + "y": 0.7970039548993032, + "heading": -0.04541767666824434, + "angularVelocity": 0.3812850635065168, + "velocityX": 1.3243616829347478, + "velocityY": -0.1960140146538302, + "timestamp": 2.538918291368649 + }, + { + "x": 8.097419437322872, + "y": 0.7857000289910334, + "heading": -0.022962750583734995, + "angularVelocity": 0.29196632020738633, + "velocityX": 0.9931710887135993, + "velocityY": -0.14697735538800996, + "timestamp": 2.6158275940188975 + }, + { + "x": 8.148337646767295, + "y": 0.7781655823556067, + "heading": -0.0077300757248004864, + "angularVelocity": 0.19806023893112515, + "velocityX": 0.662055274067148, + "velocityY": -0.09796534847923602, + "timestamp": 2.692736896669146 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": 1.1088837243155707e-19, + "angularVelocity": 0.10050898211824409, + "velocityX": 0.3310015661876223, + "velocityY": -0.04897379245809494, + "timestamp": 2.7696461993193946 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": 5.633915553103528e-20, + "angularVelocity": 2.8365160942051004e-20, + "velocityX": -1.2324505157130932e-18, + "velocityY": 3.669165812220002e-18, + "timestamp": 2.846555501969643 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGCSprint.traj b/src/main/deploy/choreo/SourceLanePGCSprint.traj new file mode 100644 index 00000000..32aa2f01 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGCSprint.traj @@ -0,0 +1,1696 @@ +{ + "samples": [ + { + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": 9.759657159056985e-29, + "angularVelocity": 1.5485227916478622e-29, + "velocityX": -1.3617494152189479e-27, + "velocityY": -5.3557690609839536e-27, + "timestamp": 0 + }, + { + "x": 0.4546918803213756, + "y": 4.109218526997661, + "heading": -0.008937300145995024, + "angularVelocity": -0.11893237262991248, + "velocityX": 0.28544257523423766, + "velocityY": -0.15856789979825608, + "timestamp": 0.07514606787342623 + }, + { + "x": 0.4975916635116252, + "y": 4.0853873108981436, + "heading": -0.02681241329663613, + "angularVelocity": -0.23787157008333967, + "velocityX": 0.5708852692400183, + "velocityY": -0.3171319108760033, + "timestamp": 0.15029213574685246 + }, + { + "x": 0.5619414537480726, + "y": 4.049641027140498, + "heading": -0.05362171709802378, + "angularVelocity": -0.3567625633658495, + "velocityX": 0.8563294402155045, + "velocityY": -0.4756906750976654, + "timestamp": 0.2254382036202787 + }, + { + "x": 0.6477414523773108, + "y": 4.001980177860296, + "heading": -0.08935853104911338, + "angularVelocity": -0.4755646564406219, + "velocityX": 1.1417762905939028, + "velocityY": -0.6342427571923064, + "timestamp": 0.3005842714937049 + }, + { + "x": 0.7549919358777417, + "y": 3.9424053758532884, + "heading": -0.13401442200652888, + "angularVelocity": -0.5942545261667251, + "velocityX": 1.4272268201854599, + "velocityY": -0.7927866845588466, + "timestamp": 0.3757303393671312 + }, + { + "x": 0.8836932393269187, + "y": 3.8709173416225426, + "heading": -0.18758057158075359, + "angularVelocity": -0.7128270459134325, + "velocityX": 1.7126818087934759, + "velocityY": -0.9513210238912064, + "timestamp": 0.45087640724055744 + }, + { + "x": 1.0338457421402332, + "y": 3.7875168932975942, + "heading": -0.2500489995023012, + "angularVelocity": -0.8312933688928013, + "velocityX": 1.9981418464400145, + "velocityY": -1.1098444760333412, + "timestamp": 0.5260224751139837 + }, + { + "x": 1.2054498603190875, + "y": 3.692204931169302, + "heading": -0.3214134496146474, + "angularVelocity": -0.9496764385935714, + "velocityX": 2.283607419990346, + "velocityY": -1.268355947630322, + "timestamp": 0.60116854298741 + }, + { + "x": 1.3985060453231266, + "y": 3.584982424979261, + "heading": -0.40166985798592575, + "angularVelocity": -1.0680054278616389, + "velocityX": 2.569079001302066, + "velocityY": -1.4268545144723166, + "timestamp": 0.6763146108608362 + }, + { + "x": 1.5915937569821017, + "y": 3.4778198154599655, + "heading": -0.48176859267037747, + "angularVelocity": -1.0659071984893158, + "velocityX": 2.5694985396202785, + "velocityY": -1.4260574445464886, + "timestamp": 0.7514606787342625 + }, + { + "x": 1.7632284858900344, + "y": 3.382566036520045, + "heading": -0.552976547680448, + "angularVelocity": -0.9475938931363788, + "velocityX": 2.2840147697019764, + "velocityY": -1.2675816797275836, + "timestamp": 0.8266067466076887 + }, + { + "x": 1.9134098098562773, + "y": 3.299220400739088, + "heading": -0.6152918418017485, + "angularVelocity": -0.8292555536806328, + "velocityX": 1.9985253815170176, + "velocityY": -1.1091150626981823, + "timestamp": 0.901752814481115 + }, + { + "x": 2.04213735776467, + "y": 3.2277823059512354, + "heading": -0.6687115003742338, + "angularVelocity": -0.7108776291856513, + "velocityX": 1.7130310547348613, + "velocityY": -0.9506564589404838, + "timestamp": 0.9768988823545413 + }, + { + "x": 2.1494108084857926, + "y": 3.1682512394298143, + "heading": -0.7132320418113052, + "angularVelocity": -0.5924533737688112, + "velocityX": 1.4275324545498602, + "velocityY": -0.7922046782500088, + "timestamp": 1.0520449502279674 + }, + { + "x": 2.235229893671302, + "y": 3.120626784362015, + "heading": -0.7488501306819726, + "angularVelocity": -0.4739847323836216, + "velocityX": 1.142030283341778, + "velocityY": -0.6337584442610695, + "timestamp": 1.1271910181013935 + }, + { + "x": 2.2995943987852447, + "y": 3.0849086278583613, + "heading": -0.7755632624607874, + "angularVelocity": -0.35548276223593744, + "velocityX": 0.8565252572144713, + "velocityY": -0.4753163740226105, + "timestamp": 1.2023370859748197 + }, + { + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080856, + "angularVelocity": -0.2369668826490278, + "velocityX": 0.5710180813549185, + "velocityY": -0.31687698560575434, + "timestamp": 1.2774831538482458 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288599550668, + "velocityX": 0.28550943973314763, + "velocityY": -0.15843872456938438, + "timestamp": 1.352629221721672 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.820153695476826e-18, + "velocityX": -5.7963239407122954e-18, + "velocityY": -1.1530643297922074e-18, + "timestamp": 1.4277752895950981 + }, + { + "x": 2.37735740952237, + "y": 3.0408026970784268, + "heading": -0.7860869247798871, + "angularVelocity": 0.2648554274741457, + "velocityX": 0.21924714610297322, + "velocityY": -0.1372563412236913, + "timestamp": 1.4888859391070042 + }, + { + "x": 2.404179793787175, + "y": 3.024019444447085, + "heading": -0.7541504516362038, + "angularVelocity": 0.5226007806947081, + "velocityX": 0.4389150578342217, + "velocityY": -0.2746371175137346, + "timestamp": 1.5499965886189102 + }, + { + "x": 2.4444554974169383, + "y": 2.998832316814256, + "heading": -0.7069723408476191, + "angularVelocity": 0.7720112806098135, + "velocityX": 0.6590619466729163, + "velocityY": -0.41215611082519754, + "timestamp": 1.6111072381308162 + }, + { + "x": 2.4982188560395806, + "y": 2.96523040511413, + "heading": -0.645190074686331, + "angularVelocity": 1.0109901736398696, + "velocityX": 0.8797706954852063, + "velocityY": -0.5498536174710421, + "timestamp": 1.6722178876427223 + }, + { + "x": 2.5655102024042766, + "y": 2.923198127539445, + "heading": -0.5696285601591381, + "angularVelocity": 1.2364704864161304, + "velocityX": 1.101139439723772, + "velocityY": -0.6878061010707456, + "timestamp": 1.7333285371546283 + }, + { + "x": 2.646375841205288, + "y": 2.8727129664624207, + "heading": -0.48135684674506, + "angularVelocity": 1.4444571301255718, + "velocityX": 1.3232659028645477, + "velocityY": -0.8261270577264651, + "timestamp": 1.7944391866665343 + }, + { + "x": 2.740867207337065, + "y": 2.813744425609022, + "heading": -0.38173698900245784, + "angularVelocity": 1.6301554399809315, + "velocityX": 1.5462340342720005, + "velocityY": -0.9649470480903679, + "timestamp": 1.8555498361784404 + }, + { + "x": 2.849040211493812, + "y": 2.746255741762665, + "heading": -0.2724850390795677, + "angularVelocity": 1.7877726843928354, + "velocityX": 1.7701170748589676, + "velocityY": -1.1043686229060403, + "timestamp": 1.9166604856903464 + }, + { + "x": 2.9709557136972373, + "y": 2.6702088910620962, + "heading": -0.155828372393994, + "angularVelocity": 1.9089416921161393, + "velocityX": 1.994996014232719, + "velocityY": -1.2444124110602532, + "timestamp": 1.9777711352022525 + }, + { + "x": 3.1066769296559538, + "y": 2.585572206493352, + "heading": -0.03496320783780511, + "angularVelocity": 1.977808541089754, + "velocityX": 2.2209094002882996, + "velocityY": -1.3849743906298195, + "timestamp": 2.0388817847141585 + }, + { + "x": 3.2562355055408387, + "y": 2.4923383100856262, + "heading": 0.08475597327951304, + "angularVelocity": 1.959055943170691, + "velocityX": 2.4473406366879895, + "velocityY": -1.5256571015427038, + "timestamp": 2.0999924342260647 + }, + { + "x": 3.4194188611023764, + "y": 2.3906298265628587, + "heading": 0.19293288100296513, + "angularVelocity": 1.7701809518876104, + "velocityX": 2.6702932609110017, + "velocityY": -1.6643332109070865, + "timestamp": 2.161103083737971 + }, + { + "x": 3.5932872633720114, + "y": 2.2801851899216916, + "heading": 0.26578166446591744, + "angularVelocity": 1.192080006427692, + "velocityX": 2.845140800471438, + "velocityY": -1.8072895235657866, + "timestamp": 2.2222137332498773 + }, + { + "x": 3.7772765002319786, + "y": 2.1608722925529302, + "heading": 0.30020319888903974, + "angularVelocity": 0.5632657269731189, + "velocityX": 3.0107557083666587, + "velocityY": -1.9524076134310657, + "timestamp": 2.2833243827617835 + }, + { + "x": 3.9732507395108834, + "y": 2.038358982543431, + "heading": 0.30020324669419357, + "angularVelocity": 7.822720631834703e-7, + "velocityX": 3.2068754111462736, + "velocityY": -2.0047783976771947, + "timestamp": 2.3444350322736898 + }, + { + "x": 4.174667645417682, + "y": 1.9250159473015482, + "heading": 0.3002032939445946, + "angularVelocity": 7.731942209741845e-7, + "velocityX": 3.2959379014218597, + "velocityY": -1.8547182225546543, + "timestamp": 2.405545681785596 + }, + { + "x": 4.383569049898422, + "y": 1.8261442295125712, + "heading": 0.30020334201253857, + "angularVelocity": 7.865722968351813e-7, + "velocityX": 3.4184124395542623, + "velocityY": -1.6179130573585891, + "timestamp": 2.4666563312975023 + }, + { + "x": 4.59891708073721, + "y": 1.7422357169202292, + "heading": 0.3002033920988207, + "angularVelocity": 8.195998987503142e-7, + "velocityX": 3.52390348587004, + "velocityY": -1.3730587591937538, + "timestamp": 2.5277669808094085 + }, + { + "x": 4.819641568305972, + "y": 1.6737075280608862, + "heading": 0.30020344564176327, + "angularVelocity": 8.761638607213102e-7, + "velocityX": 3.6118825332687328, + "velocityY": -1.1213788334223447, + "timestamp": 2.588877630321315 + }, + { + "x": 5.044645579459654, + "y": 1.6209002757550093, + "heading": 0.30020350452029126, + "angularVelocity": 9.634740995599556e-7, + "velocityX": 3.681911629982681, + "velocityY": -0.8641252012153561, + "timestamp": 2.649988279833221 + }, + { + "x": 5.272810899090047, + "y": 1.584076421741111, + "heading": 0.3002035713939988, + "angularVelocity": 0.0000010943052979537505, + "velocityX": 3.733642523075129, + "velocityY": -0.6025767081190017, + "timestamp": 2.7110989293451273 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3002036452391599, + "angularVelocity": 0.0000012083844913604929, + "velocityX": 3.7668180588495743, + "velocityY": -0.33803334595452383, + "timestamp": 2.7722095788570336 + }, + { + "x": 5.642203177619129, + "y": 1.5568731039887571, + "heading": 0.3002037251666547, + "angularVelocity": 0.0000021691766198010436, + "velocityX": 3.7777797970827858, + "velocityY": -0.177650642031358, + "timestamp": 2.8090565069037923 + }, + { + "x": 5.781555187145968, + "y": 1.5562486567630798, + "heading": 0.30020379895395954, + "angularVelocity": 0.0000020025361337225055, + "velocityX": 3.78191661866635, + "velocityY": -0.016947063399286564, + "timestamp": 2.845903434950551 + }, + { + "x": 5.9208078464820115, + "y": 1.5615467685843438, + "heading": 0.30020386782175307, + "angularVelocity": 0.0000018690240183201108, + "velocityX": 3.7792203235866277, + "velocityY": 0.14378706997068585, + "timestamp": 2.8827503629973097 + }, + { + "x": 6.059709556234372, + "y": 1.5727578639536914, + "heading": 0.3002039327408759, + "angularVelocity": 0.0000017618598429234572, + "velocityX": 3.7696957959723445, + "velocityY": 0.30426133096145186, + "timestamp": 2.9195972910440684 + }, + { + "x": 6.198009351638548, + "y": 1.5898616832743162, + "heading": 0.30020399449987484, + "angularVelocity": 0.0000016760962775678482, + "velocityX": 3.753360259196494, + "velocityY": 0.46418576058553174, + "timestamp": 2.956444219090827 + }, + { + "x": 6.335457356092555, + "y": 1.6128273194464042, + "heading": 0.30020405375523124, + "angularVelocity": 0.0000016081491612309129, + "velocityX": 3.730243245233006, + "velocityY": 0.6232713930166671, + "timestamp": 2.993291147137586 + }, + { + "x": 6.471805232718414, + "y": 1.6416132738069629, + "heading": 0.3002041110675489, + "angularVelocity": 0.0000015554164405322294, + "velocityX": 3.700386541120455, + "velocityY": 0.7812307805966762, + "timestamp": 3.0301380751843445 + }, + { + "x": 6.60680663253268, + "y": 1.676167531284266, + "heading": 0.3002041691027596, + "angularVelocity": 0.0000015750352547385845, + "velocityX": 3.6638440969339445, + "velocityY": 0.937778515306732, + "timestamp": 3.0669850032311032 + }, + { + "x": 6.740176057879373, + "y": 1.7164157557118485, + "heading": 0.3003755003099619, + "angularVelocity": 0.004649809801927131, + "velocityX": 3.6195534449289073, + "velocityY": 1.0923088181600296, + "timestamp": 3.103831931277862 + }, + { + "x": 6.869837525657242, + "y": 1.7607930294165437, + "heading": 0.3093430143729343, + "angularVelocity": 0.2433720947264084, + "velocityX": 3.5189220554106844, + "velocityY": 1.2043683437702197, + "timestamp": 3.1406788593246207 + }, + { + "x": 6.995165288352465, + "y": 1.807928452248917, + "heading": 0.3301850856624388, + "angularVelocity": 0.5656393190514163, + "velocityX": 3.4013083135772337, + "velocityY": 1.2792225927914125, + "timestamp": 3.1775257873713794 + }, + { + "x": 7.114963240628345, + "y": 1.8557254140007795, + "heading": 0.35927322814233364, + "angularVelocity": 0.7894319559824341, + "velocityX": 3.2512331048020355, + "velocityY": 1.2971762989633377, + "timestamp": 3.214372715418138 + }, + { + "x": 7.228886146559884, + "y": 1.9032191557267804, + "heading": 0.3911508310575271, + "angularVelocity": 0.8651359721153189, + "velocityX": 3.091788433135363, + "velocityY": 1.2889471183522156, + "timestamp": 3.251219643464897 + }, + { + "x": 7.336942556890349, + "y": 1.949894399055616, + "heading": 0.423151819771819, + "angularVelocity": 0.8684845768875223, + "velocityX": 2.9325758226938556, + "velocityY": 1.2667336411221237, + "timestamp": 3.2880665715116555 + }, + { + "x": 7.43919043499382, + "y": 1.9954619855368936, + "heading": 0.45375826572504047, + "angularVelocity": 0.8306376562622595, + "velocityX": 2.7749362979111796, + "velocityY": 1.2366726046592738, + "timestamp": 3.324913499558414 + }, + { + "x": 7.535687560612936, + "y": 2.0397400223416313, + "heading": 0.48197582868140687, + "angularVelocity": 0.7658050332054913, + "velocityX": 2.618864875157601, + "velocityY": 1.201675123325048, + "timestamp": 3.361760427605173 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6818119458710672, + "velocityX": 2.464149969521805, + "velocityY": 1.163303151067446, + "timestamp": 3.3986073556519316 + }, + { + "x": 7.766065583230135, + "y": 2.1519617180113952, + "heading": 0.5396264377138885, + "angularVelocity": 0.5126079654708053, + "velocityX": 2.1996686083558368, + "velocityY": 1.0930061650047849, + "timestamp": 3.4620631259727794 + }, + { + "x": 7.889178097615864, + "y": 2.215679412144434, + "heading": 0.5626262771579723, + "angularVelocity": 0.3624546566496878, + "velocityX": 1.9401311143689866, + "velocityY": 1.004127659484183, + "timestamp": 3.5255188962936272 + }, + { + "x": 7.996082408378713, + "y": 2.2729729622929304, + "heading": 0.5769453477724344, + "angularVelocity": 0.22565435014123394, + "velocityX": 1.6847059018008188, + "velocityY": 0.90288952224213, + "timestamp": 3.588974666614475 + }, + { + "x": 8.086990111713977, + "y": 2.3232868849538337, + "heading": 0.5832040110121259, + "angularVelocity": 0.09863032483958853, + "velocityX": 1.432615235393288, + "velocityY": 0.7928975159627525, + "timestamp": 3.652430436935323 + }, + { + "x": 8.16207340516984, + "y": 2.3662086171292933, + "heading": 0.581874089552229, + "angularVelocity": -0.020958243090781536, + "velocityX": 1.183238231546559, + "velocityY": 0.676403926048603, + "timestamp": 3.7158862072561707 + }, + { + "x": 8.221474033046453, + "y": 2.401420292157025, + "heading": 0.5733258497363348, + "angularVelocity": -0.1347117807044565, + "velocityX": 0.9360949772773745, + "velocityY": 0.5549010728211649, + "timestamp": 3.7793419775770185 + }, + { + "x": 8.265310204045273, + "y": 2.4286697945211704, + "heading": 0.5578573342178689, + "angularVelocity": -0.24376846172150818, + "velocityX": 0.6908145748948221, + "velocityY": 0.4294251291311274, + "timestamp": 3.8427977478978663 + }, + { + "x": 8.293681756385627, + "y": 2.447752418352113, + "heading": 0.5357134031725193, + "angularVelocity": -0.34896638924064666, + "velocityX": 0.44710752382171043, + "velocityY": 0.30072322397878726, + "timestamp": 3.906253518218714 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.45094242234392296, + "velocityX": 0.20474492941705758, + "velocityY": 0.16935099752034305, + "timestamp": 3.969709288539562 + }, + { + "x": 8.303680612059921, + "y": 2.4605220907329786, + "heading": 0.4706360870687623, + "angularVelocity": -0.5539055745477068, + "velocityX": -0.04547302080206286, + "velocityY": 0.030737357256443772, + "timestamp": 4.035537146208892 + }, + { + "x": 8.28417079095848, + "y": 2.4534909416021047, + "heading": 0.42765554839737563, + "angularVelocity": -0.6529232485020114, + "velocityX": -0.2963763639315537, + "velocityY": -0.1068111492583101, + "timestamp": 4.101365003878222 + }, + { + "x": 8.248092493080108, + "y": 2.437487576077448, + "heading": 0.3784580606750378, + "angularVelocity": -0.7473657728536394, + "velocityX": -0.5480703634561933, + "velocityY": -0.24310931710774963, + "timestamp": 4.1671928615475515 + }, + { + "x": 8.195385029545998, + "y": 2.412609981570135, + "heading": 0.3233968459430461, + "angularVelocity": -0.8364424528074197, + "velocityX": -0.8006862960492162, + "velocityY": -0.37791894477683524, + "timestamp": 4.233020719216881 + }, + { + "x": 8.125976752501963, + "y": 2.378976757579925, + "heading": 0.2628923564512202, + "angularVelocity": -0.9191319850595123, + "velocityX": -1.0543906410063166, + "velocityY": -0.5109269112046505, + "timestamp": 4.298848576886211 + }, + { + "x": 8.039781850826214, + "y": 2.336734306251694, + "heading": 0.19745501482460134, + "angularVelocity": -0.9940676173198235, + "velocityX": -1.3093985544650038, + "velocityY": -0.641710862602064, + "timestamp": 4.364676434555541 + }, + { + "x": 7.936695821305385, + "y": 2.286067717588345, + "heading": 0.12772072746128918, + "angularVelocity": -1.0593431084086873, + "velocityX": -1.565993990548153, + "velocityY": -0.7696830864200519, + "timestamp": 4.430504292224871 + }, + { + "x": 7.816588930914964, + "y": 2.2272180395232652, + "heading": 0.05450854772010129, + "angularVelocity": -1.1121762477665882, + "velocityX": -1.8245602187716414, + "velocityY": -0.8939935180740173, + "timestamp": 4.496332149894201 + }, + { + "x": 7.679296617994528, + "y": 2.1605113660425403, + "heading": -0.02108172308249024, + "angularVelocity": -1.1483021547245422, + "velocityX": -2.085626325712903, + "velocityY": -1.01335021133165, + "timestamp": 4.562160007563531 + }, + { + "x": 7.52460540494712, + "y": 2.086411774056454, + "heading": -0.09749530920005847, + "angularVelocity": -1.16080925041513, + "velocityX": -2.34993540006212, + "velocityY": -1.1256570486967377, + "timestamp": 4.6279878652328605 + }, + { + "x": 7.352233710537335, + "y": 2.0056280451301003, + "heading": -0.17238692358324137, + "angularVelocity": -1.1376887693867086, + "velocityX": -2.6185220135167313, + "velocityY": -1.2271966882493952, + "timestamp": 4.69381572290219 + }, + { + "x": 7.161817946345765, + "y": 1.9193611743859953, + "heading": -0.24189742475427997, + "angularVelocity": -1.055943541717672, + "velocityX": -2.8926319484385736, + "velocityY": -1.3104918464375106, + "timestamp": 4.75964358057152 + }, + { + "x": 6.953014973362115, + "y": 1.8300106922230066, + "heading": -0.298719434373675, + "angularVelocity": -0.8631909290566133, + "velocityX": -3.1719545550536123, + "velocityY": -1.3573354097564532, + "timestamp": 4.82547143824085 + }, + { + "x": 6.727431268978415, + "y": 1.7436413956339059, + "heading": -0.3201876383615551, + "angularVelocity": -0.32612642653085444, + "velocityX": -3.4268729436231227, + "velocityY": -1.3120478114745338, + "timestamp": 4.89129929591018 + }, + { + "x": 6.489449879790782, + "y": 1.6705324422161696, + "heading": -0.3201878815696182, + "angularVelocity": -0.0000036946069898928346, + "velocityX": -3.61520787115807, + "velocityY": -1.1106081225517261, + "timestamp": 4.95712715357951 + }, + { + "x": 6.246606826324383, + "y": 1.6156931769447658, + "heading": -0.3201879117478121, + "angularVelocity": -4.584410758613646e-7, + "velocityX": -3.689062078949349, + "velocityY": -0.8330707881589553, + "timestamp": 5.02295501124884 + }, + { + "x": 6.0003025647982255, + "y": 1.5794398654466013, + "heading": -0.32018794628515007, + "angularVelocity": -5.246614307457641e-7, + "velocityX": -3.741641764546064, + "velocityY": -0.5507290193199733, + "timestamp": 5.0887828689181696 + }, + { + "x": 5.7519574472803, + "y": 1.5619815744715873, + "heading": -0.32018798749762734, + "angularVelocity": -6.260643854910128e-7, + "velocityX": -3.772644687381865, + "velocityY": -0.26521128885450773, + "timestamp": 5.154610726587499 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.32018803965163717, + "angularVelocity": -7.922787054710825e-7, + "velocityX": -3.7818920261895626, + "velocityY": 0.021835891254127007, + "timestamp": 5.220438584256829 + }, + { + "x": 5.250083628410863, + "y": 1.584512642850435, + "heading": -0.3201880864656727, + "angularVelocity": -6.975962896264183e-7, + "velocityX": -3.768870382202017, + "velocityY": 0.31432577229262343, + "timestamp": 5.287546216836352 + }, + { + "x": 4.999553273526972, + "y": 1.6251081857259102, + "heading": -0.32018812296137045, + "angularVelocity": -5.438382542161688e-7, + "velocityX": -3.7332617059171653, + "velocityY": 0.6049318283932773, + "timestamp": 5.354653849415875 + }, + { + "x": 4.752913977063861, + "y": 1.6849623155206188, + "heading": -0.3201881529794151, + "angularVelocity": -4.473119302496649e-7, + "velocityX": -3.675279353221748, + "velocityY": 0.8919124024198194, + "timestamp": 5.421761481995398 + }, + { + "x": 4.511643860653374, + "y": 1.7637163133941143, + "heading": -0.3201881787520496, + "angularVelocity": -3.8404922716455346e-7, + "velocityX": -3.595270867655492, + "velocityY": 1.1735475510952014, + "timestamp": 5.488869114574921 + }, + { + "x": 4.277188862383835, + "y": 1.86089818593956, + "heading": -0.3201882016908444, + "angularVelocity": -3.41820951724987e-7, + "velocityX": -3.4937158301883047, + "velocityY": 1.4481493208732221, + "timestamp": 5.555976747154444 + }, + { + "x": 4.050954064298834, + "y": 1.9759254853688604, + "heading": -0.32018822275897413, + "angularVelocity": -3.1394535808439923e-7, + "velocityX": -3.3712230544999895, + "velocityY": 1.7140717830120589, + "timestamp": 5.623084379733967 + }, + { + "x": 3.834295246075004, + "y": 2.1081087693328064, + "heading": -0.32018824266587215, + "angularVelocity": -2.966413400864343e-7, + "velocityX": -3.2285272165887804, + "velocityY": 1.969720565053586, + "timestamp": 5.69019201231349 + }, + { + "x": 3.6285105963453295, + "y": 2.256655537498403, + "heading": -0.32018826197928, + "angularVelocity": -2.8779748460931035e-7, + "velocityX": -3.066486505626901, + "velocityY": 2.213559955189428, + "timestamp": 5.757299644893013 + }, + { + "x": 3.4323361610843617, + "y": 2.417680552436319, + "heading": -0.3201882811600919, + "angularVelocity": -2.858216149634079e-7, + "velocityX": -2.9232805229494647, + "velocityY": 2.399503733753405, + "timestamp": 5.824407277472536 + }, + { + "x": 3.2362856931678534, + "y": 2.578856476556199, + "heading": -0.32018830034052725, + "angularVelocity": -2.858160039511642e-7, + "velocityX": -2.921433231669861, + "velocityY": 2.401752497063368, + "timestamp": 5.891514910052059 + }, + { + "x": 3.0402351711109183, + "y": 2.7400323348209366, + "heading": -0.3201883195209977, + "angularVelocity": -2.858165259418001e-7, + "velocityX": -2.9214340384398776, + "velocityY": 2.401751515727269, + "timestamp": 5.958622542631582 + }, + { + "x": 2.8448193652926044, + "y": 2.899781496680513, + "heading": -0.32456406822795664, + "angularVelocity": -0.06520493331028634, + "velocityX": -2.911975855902028, + "velocityY": 2.380491692510139, + "timestamp": 6.025730175211105 + }, + { + "x": 2.66318575993582, + "y": 3.04911523833719, + "heading": -0.3692876119349906, + "angularVelocity": -0.6664449629337832, + "velocityX": -2.706601296678858, + "velocityY": 2.2252869892216163, + "timestamp": 6.0928378077906284 + }, + { + "x": 2.496643428698106, + "y": 3.186035937240345, + "heading": -0.4222622822262224, + "angularVelocity": -0.7893985863449552, + "velocityX": -2.4817196619231154, + "velocityY": 2.040314844081317, + "timestamp": 6.1599454403701515 + }, + { + "x": 2.3452748100493945, + "y": 3.3104804771184444, + "heading": -0.4767233164983405, + "angularVelocity": -0.8115475420412368, + "velocityX": -2.255609575696765, + "velocityY": 1.8544021759467144, + "timestamp": 6.2270530729496745 + }, + { + "x": 2.209079907655855, + "y": 3.4224492823331634, + "heading": -0.5296433021172221, + "angularVelocity": -0.7885837062747133, + "velocityX": -2.0294994348392206, + "velocityY": 1.6684958314098723, + "timestamp": 6.2941607055291975 + }, + { + "x": 2.088048432415951, + "y": 3.5219507247047837, + "heading": -0.5792907495482946, + "angularVelocity": -0.7398181923977142, + "velocityX": -1.803542616355617, + "velocityY": 1.4827142390056927, + "timestamp": 6.3612683381087205 + }, + { + "x": 1.9821698412647601, + "y": 3.6089936285690154, + "heading": -0.624538681260696, + "angularVelocity": -0.6742590965160707, + "velocityX": -1.5777429046647484, + "velocityY": 1.297064141863228, + "timestamp": 6.4283759706882435 + }, + { + "x": 1.8914349570352331, + "y": 3.6835858822586136, + "heading": -0.6645923745497577, + "angularVelocity": -0.5968574922025148, + "velocityX": -1.3520799459287431, + "velocityY": 1.1115315921956592, + "timestamp": 6.4954836032677665 + }, + { + "x": 1.815836039628641, + "y": 3.7457342610555044, + "heading": -0.698860244587487, + "angularVelocity": -0.5106404252470358, + "velocityX": -1.1265323257679698, + "velocityY": 0.926100003948804, + "timestamp": 6.5625912358472895 + }, + { + "x": 1.7553665690239355, + "y": 3.7954445247907986, + "heading": -0.7268845476309118, + "angularVelocity": -0.41760231982876106, + "velocityX": -0.9010818632746272, + "velocityY": 0.7407542454487245, + "timestamp": 6.6296988684268126 + }, + { + "x": 1.7100210087875185, + "y": 3.8327215653417337, + "heading": -0.7483006908429537, + "angularVelocity": -0.31913125808251036, + "velocityX": -0.6757139015846285, + "velocityY": 0.5554813829374979, + "timestamp": 6.696806501006336 + }, + { + "x": 1.6797946046172731, + "y": 3.8575695457400823, + "heading": -0.7628116421295846, + "angularVelocity": -0.21623399200434476, + "velocityX": -0.4504167858168276, + "velocityY": 0.37027055557210875, + "timestamp": 6.763914133585859 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.10966416437475777, + "velocityX": -0.22518126874279432, + "velocityY": 0.1851126545280661, + "timestamp": 6.831021766165382 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -1.211900873027185e-19, + "velocityX": -5.462077711506373e-19, + "velocityY": -3.512888080115249e-19, + "timestamp": 6.898129398744905 + }, + { + "x": 1.659892349205253, + "y": 3.8800826826438297, + "heading": -0.7264777443526473, + "angularVelocity": 0.7466474211356967, + "velocityX": -0.08186842286768115, + "velocityY": 0.1724334423754288, + "timestamp": 6.95664858596781 + }, + { + "x": 1.6562452238185605, + "y": 3.90221669421699, + "heading": -0.6460681306884017, + "angularVelocity": 1.3740726329290587, + "velocityX": -0.06232358239700476, + "velocityY": 0.3782351160970439, + "timestamp": 7.015167773190715 + }, + { + "x": 1.6553319250883793, + "y": 3.9350953760295733, + "heading": -0.5183992679116819, + "angularVelocity": 2.1816581677803772, + "velocityX": -0.01560682527428515, + "velocityY": 0.5618444714097892, + "timestamp": 7.07368696041362 + }, + { + "x": 1.6623538133314129, + "y": 3.9788519148323376, + "heading": -0.3621106955703112, + "angularVelocity": 2.670723565350507, + "velocityX": 0.11999292157436166, + "velocityY": 0.7477297768352748, + "timestamp": 7.132206147636525 + }, + { + "x": 1.6832034751641156, + "y": 4.019420947962781, + "heading": -0.22540240918746318, + "angularVelocity": 2.3361275655130602, + "velocityX": 0.35628761816674753, + "velocityY": 0.6932603656286723, + "timestamp": 7.19072533485943 + }, + { + "x": 1.7146647081648767, + "y": 4.052413452071288, + "heading": -0.11878752772286896, + "angularVelocity": 1.8218790541038834, + "velocityX": 0.5376225216683612, + "velocityY": 0.5637895137339223, + "timestamp": 7.249244522082336 + }, + { + "x": 1.7541026264736177, + "y": 4.074977116543675, + "heading": -0.0408151972626467, + "angularVelocity": 1.332423332593777, + "velocityX": 0.6739314091721454, + "velocityY": 0.38557720199428364, + "timestamp": 7.307763709305241 + }, + { + "x": 1.799090355046403, + "y": 4.0866639858943925, + "heading": 1.7934233852716553e-24, + "angularVelocity": 0.6974669198185859, + "velocityX": 0.7687688552717376, + "velocityY": 0.199710042215758, + "timestamp": 7.366282896528146 + }, + { + "x": 1.848745584487915, + "y": 4.087520122528076, + "heading": 7.573339964499894e-25, + "angularVelocity": -5.2358045608260825e-25, + "velocityX": 0.848529034628775, + "velocityY": 0.014630015800164336, + "timestamp": 7.424802083751051 + }, + { + "x": 1.9382598731728313, + "y": 4.089063493944204, + "heading": 6.819777064270339e-25, + "angularVelocity": -2.006050179968467e-26, + "velocityX": 1.1795649340806893, + "velocityY": 0.02033761122914328, + "timestamp": 7.500689630166358 + }, + { + "x": 2.0528956622357724, + "y": 4.091040000742217, + "heading": 6.067470023829544e-25, + "angularVelocity": -1.8954686232133377e-26, + "velocityX": 1.510600809724148, + "velocityY": 0.026045206247625866, + "timestamp": 7.576577176581664 + }, + { + "x": 2.1926529480236727, + "y": 4.09344964285913, + "heading": 5.320081952060173e-25, + "angularVelocity": -2.1711320260805291e-26, + "velocityX": 1.8416366372297248, + "velocityY": 0.03175280043613335, + "timestamp": 7.652464722996971 + }, + { + "x": 2.3575317192504395, + "y": 4.096292420100353, + "heading": 4.5695624647021355e-25, + "angularVelocity": -2.0004263459154622e-26, + "velocityX": 2.1726723160140353, + "velocityY": 0.03746039206044517, + "timestamp": 7.7283522694122775 + }, + { + "x": 2.4972894766259857, + "y": 4.098702070348201, + "heading": 3.822235688125773e-25, + "angularVelocity": -1.9197661326309743e-26, + "velocityX": 1.8416428515253338, + "velocityY": 0.0317529075806614, + "timestamp": 7.804239815827584 + }, + { + "x": 2.611925746748375, + "y": 4.100678585440459, + "heading": 3.063572284762821e-25, + "angularVelocity": -2.4686338901390002e-26, + "velocityX": 1.5106071488334032, + "velocityY": 0.026045315544143462, + "timestamp": 7.880127362242891 + }, + { + "x": 2.701440519692856, + "y": 4.102221965206007, + "heading": 2.3114445870748474e-25, + "angularVelocity": -1.5210807516831905e-26, + "velocityX": 1.1795713153591425, + "velocityY": 0.020337721252726194, + "timestamp": 7.956014908658197 + }, + { + "x": 2.7658337920580536, + "y": 4.1033322095862, + "heading": 1.561656112047754e-25, + "angularVelocity": -1.9040979941775662e-26, + "velocityX": 0.8485354370636105, + "velocityY": 0.01463012618851756, + "timestamp": 8.031902455073505 + }, + { + "x": 2.805105562125262, + "y": 4.104009318551404, + "heading": 8.077058611592956e-26, + "angularVelocity": -3.263981497829794e-26, + "velocityX": 0.5174995361200375, + "velocityY": 0.008922530733819996, + "timestamp": 8.107790001488812 + }, + { + "x": 2.819255828857422, + "y": 4.10425329208374, + "heading": 3.4975515409752656e-27, + "angularVelocity": -3.9242842268499737e-26, + "velocityX": 0.18646362151070262, + "velocityY": 0.0032149350435025517, + "timestamp": 8.18367754790412 + }, + { + "x": 2.810470941680652, + "y": 4.100290874791341, + "heading": -0.013665728423960572, + "angularVelocity": -0.18841669161843644, + "velocityX": -0.12112192828199068, + "velocityY": -0.05463196207942443, + "timestamp": 8.256206835551495 + }, + { + "x": 2.779376565530904, + "y": 4.092132450200435, + "heading": -0.040981035402949714, + "angularVelocity": -0.3766107163742046, + "velocityX": -0.4287147600418107, + "velocityY": -0.11248455424752818, + "timestamp": 8.32873612319887 + }, + { + "x": 2.7259718800290944, + "y": 4.079777209778572, + "heading": -0.08192049181497908, + "angularVelocity": -0.5644541362527821, + "velocityX": -0.7363189028058957, + "velocityY": -0.17034829408406268, + "timestamp": 8.401265410846246 + }, + { + "x": 2.650255783412156, + "y": 4.06322370149856, + "heading": -0.13644943849416394, + "angularVelocity": -0.7518196917125987, + "velocityX": -1.0439382361654468, + "velocityY": -0.22823205379447506, + "timestamp": 8.473794698493622 + }, + { + "x": 2.5522270104067304, + "y": 4.04246950295157, + "heading": -0.20452759410435262, + "angularVelocity": -0.9386298668914527, + "velocityX": -1.3515750145241086, + "velocityY": -0.2861492125483705, + "timestamp": 8.546323986140997 + }, + { + "x": 2.4318844389859766, + "y": 4.01751088765649, + "heading": -0.2861194089559698, + "angularVelocity": -1.1249498995261227, + "velocityX": -1.6592272628656817, + "velocityY": -0.34411775028624336, + "timestamp": 8.618853273788373 + }, + { + "x": 2.2892276526666033, + "y": 3.9883426531177015, + "heading": -0.3812142224775482, + "angularVelocity": -1.311122949172083, + "velocityX": -1.9668852534846832, + "velocityY": -0.40215801760798864, + "timestamp": 8.691382561435748 + }, + { + "x": 2.1330829846519728, + "y": 3.9587634030266514, + "heading": -0.4780632841326763, + "angularVelocity": -1.3353097044877917, + "velocityX": -2.1528498773319837, + "velocityY": -0.40782490839919366, + "timestamp": 8.763911849083124 + }, + { + "x": 1.9992497997330843, + "y": 3.9334042162180256, + "heading": -0.5612966415288853, + "angularVelocity": -1.1475827227322806, + "velocityX": -1.8452295515373094, + "velocityY": -0.349640643541367, + "timestamp": 8.8364411367305 + }, + { + "x": 1.8877256900718635, + "y": 3.9122685100016006, + "heading": -0.6308164435957174, + "angularVelocity": -0.9585066160421192, + "velocityX": -1.537642423891296, + "velocityY": -0.29140926240973475, + "timestamp": 8.908970424377875 + }, + { + "x": 1.7985083705984093, + "y": 3.895358481595443, + "heading": -0.6865270710591681, + "angularVelocity": -0.768112155386183, + "velocityX": -1.2300868017236275, + "velocityY": -0.23314758705988012, + "timestamp": 8.98149971202525 + }, + { + "x": 1.731596065924516, + "y": 3.88267537699245, + "heading": -0.7283506709725182, + "angularVelocity": -0.5766442946012179, + "velocityX": -0.9225556577807338, + "velocityY": -0.17486873254092689, + "timestamp": 9.054028999672626 + }, + { + "x": 1.6869877424298634, + "y": 3.8742198088507074, + "heading": -0.7562373778990242, + "angularVelocity": -0.3844889124250871, + "velocityX": -0.6150387649128701, + "velocityY": -0.11658143097795469, + "timestamp": 9.126558287320002 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.19210952061606446, + "velocityX": -0.3075243171780863, + "velocityY": -0.05829081246861152, + "timestamp": 9.199087574967377 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -5.999453085593573e-20, + "velocityX": -1.6973677435282043e-18, + "velocityY": -2.503873223513922e-18, + "timestamp": 9.271616862614753 + }, + { + "x": 1.6770247995246623, + "y": 3.8595873316999896, + "heading": -0.7649878221642326, + "angularVelocity": 0.08507451391469989, + "velocityX": 0.20257164680788678, + "velocityY": -0.1707799926111949, + "timestamp": 9.332541364875802 + }, + { + "x": 1.70170978110981, + "y": 3.8387765844705157, + "heading": -0.7547381408402699, + "angularVelocity": 0.1682357827076709, + "velocityX": 0.405173299231529, + "velocityY": -0.3415825563958536, + "timestamp": 9.39346586713685 + }, + { + "x": 1.7387402457793653, + "y": 3.80755819428449, + "heading": -0.7395583157876386, + "angularVelocity": 0.24915796583103744, + "velocityX": 0.607809063599532, + "velocityY": -0.5124110830197925, + "timestamp": 9.454390369397899 + }, + { + "x": 1.7881185850906471, + "y": 3.7659303409499723, + "heading": -0.7196099497583256, + "angularVelocity": 0.3274276405876679, + "velocityX": 0.8104840824091846, + "velocityY": -0.6832694858326719, + "timestamp": 9.515314871658948 + }, + { + "x": 1.8498475856412335, + "y": 3.713890927230462, + "heading": -0.6950873259687543, + "angularVelocity": 0.4025083977624826, + "velocityX": 1.0132048397554583, + "velocityY": -0.8541623121766772, + "timestamp": 9.576239373919996 + }, + { + "x": 1.9239305360498986, + "y": 3.6514375329357844, + "heading": -0.6662283152580175, + "angularVelocity": 0.47368480069122665, + "velocityX": 1.2159795756924812, + "velocityY": -1.025094862935087, + "timestamp": 9.637163876181045 + }, + { + "x": 2.010371367313072, + "y": 3.5785673636651287, + "heading": -0.6333309419257536, + "angularVelocity": 0.539969505065564, + "velocityX": 1.4188188340512586, + "velocityY": -1.1960732803104883, + "timestamp": 9.698088378442094 + }, + { + "x": 2.1091748328759063, + "y": 3.4952772039974476, + "heading": -0.5967798759520212, + "angularVelocity": 0.5999403297070707, + "velocityX": 1.6217361143055882, + "velocityY": -1.3671044748269063, + "timestamp": 9.759012880703143 + }, + { + "x": 2.220346716936752, + "y": 3.40156341112366, + "heading": -0.5570916262340567, + "angularVelocity": 0.6514333026129457, + "velocityX": 1.82474833498841, + "velocityY": -1.5381954615278515, + "timestamp": 9.819937382964191 + }, + { + "x": 2.3438939700884585, + "y": 3.297422079154605, + "heading": -0.5149984011781104, + "angularVelocity": 0.6909079843703304, + "velocityX": 2.027874641016087, + "velocityY": -1.7093505585459128, + "timestamp": 9.88086188522524 + }, + { + "x": 2.4798242349395756, + "y": 3.182849894813871, + "heading": -0.4716228537300689, + "angularVelocity": 0.7119557130263738, + "velocityX": 2.2311263909664074, + "velocityY": -1.8805600388792227, + "timestamp": 9.941786387486289 + }, + { + "x": 2.628141721858615, + "y": 3.057848265310819, + "heading": -0.42891150330345956, + "angularVelocity": 0.7010537442489162, + "velocityX": 2.4344472488840574, + "velocityY": -2.0517464216194523, + "timestamp": 10.002710889747338 + }, + { + "x": 2.788815804109156, + "y": 2.922450520332472, + "heading": -0.3910899170501554, + "angularVelocity": 0.6207943413513182, + "velocityX": 2.6372654069800667, + "velocityY": -2.2223857389625743, + "timestamp": 10.063635392008386 + }, + { + "x": 2.9613127738384817, + "y": 2.777334038124512, + "heading": -0.375202514467059, + "angularVelocity": 0.2607719717597751, + "velocityX": 2.831323413857593, + "velocityY": -2.3819067341111335, + "timestamp": 10.124559894269435 + }, + { + "x": 3.137446263833113, + "y": 2.628782733735228, + "heading": -0.3752024981016849, + "angularVelocity": 2.6861728103084975e-7, + "velocityX": 2.891012375282727, + "velocityY": -2.438285072117188, + "timestamp": 10.185484396530484 + }, + { + "x": 3.313579787334416, + "y": 2.480231469074176, + "heading": -0.37520248173599324, + "angularVelocity": 2.686224924493493e-7, + "velocityX": 2.8910129252531243, + "velocityY": -2.4382844200276295, + "timestamp": 10.246408898791532 + }, + { + "x": 3.489713311593346, + "y": 2.3316802053114225, + "heading": -0.37520246537030155, + "angularVelocity": 2.6862249292188543e-7, + "velocityX": 2.8910129376886133, + "velocityY": -2.4382844052831705, + "timestamp": 10.307333401052581 + }, + { + "x": 3.6658468653792484, + "y": 2.183128976558075, + "heading": -0.37520244900461003, + "angularVelocity": 2.6862249035440315e-7, + "velocityX": 2.89101342233718, + "velocityY": -2.4382838306472756, + "timestamp": 10.36825790331363 + }, + { + "x": 3.8419803666744587, + "y": 2.034577685567575, + "heading": -0.37520243263891834, + "angularVelocity": 2.686224937536833e-7, + "velocityX": 2.8910125607676815, + "velocityY": -2.4382848521928207, + "timestamp": 10.429182405574679 + }, + { + "x": 4.017984969789346, + "y": 1.8858736976718657, + "heading": -0.37520241627306933, + "angularVelocity": 2.6862507525069377e-7, + "velocityX": 2.8888968573061926, + "velocityY": -2.440791182151078, + "timestamp": 10.490106907835727 + }, + { + "x": 4.198449630537114, + "y": 1.742615181247194, + "heading": -0.37520239987870785, + "angularVelocity": 2.6909307205451447e-7, + "velocityX": 2.9621031612948867, + "velocityY": -2.3514105344815093, + "timestamp": 10.551031410096776 + }, + { + "x": 4.38853011399539, + "y": 1.6123849775845955, + "heading": -0.3752023832167796, + "angularVelocity": 2.734848475989746e-7, + "velocityX": 3.1199349424936167, + "velocityY": -2.137566969436863, + "timestamp": 10.611955912357825 + }, + { + "x": 4.587288186636938, + "y": 1.495827162172114, + "heading": -0.3752023659192684, + "angularVelocity": 2.8391715306138947e-7, + "velocityX": 3.2623667861891117, + "velocityY": -1.9131517055823732, + "timestamp": 10.672880414618874 + }, + { + "x": 4.7937421798706055, + "y": 1.3935176134109497, + "heading": -0.3752023473605566, + "angularVelocity": 3.046181942517293e-7, + "velocityX": 3.3886857597794955, + "velocityY": -1.6792841133570497, + "timestamp": 10.733804916879922 + }, + { + "x": 5.0647852667187605, + "y": 1.2879733857720441, + "heading": -0.3752023309051681, + "angularVelocity": 2.139583629256744e-7, + "velocityX": 3.524191190248399, + "velocityY": -1.3723206946612971, + "timestamp": 10.81071421953017 + }, + { + "x": 5.34411641028699, + "y": 1.2068681953777092, + "heading": -0.37520231550400196, + "angularVelocity": 2.0025101753440068e-7, + "velocityX": 3.6319552244350577, + "velocityY": -1.0545563098285724, + "timestamp": 10.88762352218042 + }, + { + "x": 5.629536746845857, + "y": 1.1508398853670567, + "heading": -0.375202300582326, + "angularVelocity": 1.940165285663064e-7, + "velocityX": 3.711128910592765, + "velocityY": -0.7284984791169633, + "timestamp": 10.964532824830668 + }, + { + "x": 5.917306487549604, + "y": 1.108501764833761, + "heading": -0.37520228573814113, + "angularVelocity": 1.9300896478374464e-7, + "velocityX": 3.741676634521091, + "velocityY": -0.5504941414672958, + "timestamp": 11.041442127480916 + }, + { + "x": 6.2050577552774975, + "y": 1.0660382742134262, + "heading": -0.37520227089383906, + "angularVelocity": 1.9301048840600142e-7, + "velocityX": 3.7414364428249387, + "velocityY": -0.5521242444940722, + "timestamp": 11.118351430131165 + }, + { + "x": 6.492809022919495, + "y": 1.0235747830116217, + "heading": -0.3752022560493111, + "angularVelocity": 1.9301342582848665e-7, + "velocityX": 3.7414364417080774, + "velocityY": -0.5521242520545259, + "timestamp": 11.195260732781414 + }, + { + "x": 6.772658036083171, + "y": 0.9819575118333774, + "heading": -0.34826612342726787, + "angularVelocity": 0.3502324386496837, + "velocityX": 3.638688734915609, + "velocityY": -0.541121421520385, + "timestamp": 11.272170035431662 + }, + { + "x": 7.027520568845826, + "y": 0.9441505058911935, + "heading": -0.3001058813779335, + "angularVelocity": 0.6261952766409414, + "velocityX": 3.313806314454086, + "velocityY": -0.49157910212908845, + "timestamp": 11.34907933808191 + }, + { + "x": 7.256873768560855, + "y": 0.9101570740557017, + "heading": -0.24860478708038097, + "angularVelocity": 0.6696341342705694, + "velocityX": 2.9821255922450605, + "velocityY": -0.44199375971564203, + "timestamp": 11.42598864073216 + }, + { + "x": 7.460704020800595, + "y": 0.8799617502558256, + "heading": -0.1984971865955797, + "angularVelocity": 0.6515154702763284, + "velocityX": 2.65026785077839, + "velocityY": -0.3926095122353607, + "timestamp": 11.502897943382408 + }, + { + "x": 7.6390235862814775, + "y": 0.8535543484261158, + "heading": -0.15201891065632164, + "angularVelocity": 0.6043258011403606, + "velocityX": 2.318569527171569, + "velocityY": -0.3433577073218239, + "timestamp": 11.579807246032656 + }, + { + "x": 7.7918452312093995, + "y": 0.8309283282540864, + "heading": -0.11047967273973482, + "angularVelocity": 0.5401068074364143, + "velocityX": 1.9870371939645617, + "velocityY": -0.29419094169820903, + "timestamp": 11.656716548682905 + }, + { + "x": 7.9191796079863, + "y": 0.812079256076005, + "heading": -0.07474204501348639, + "angularVelocity": 0.46467236725273836, + "velocityX": 1.6556433667844261, + "velocityY": -0.2450818240258775, + "timestamp": 11.733625851333153 + }, + { + "x": 8.021035341477521, + "y": 0.7970039548993032, + "heading": -0.04541767666824434, + "angularVelocity": 0.3812850635065168, + "velocityX": 1.3243616829347478, + "velocityY": -0.1960140146538302, + "timestamp": 11.810535153983402 + }, + { + "x": 8.097419437322872, + "y": 0.7857000289910334, + "heading": -0.022962750583734995, + "angularVelocity": 0.29196632020738633, + "velocityX": 0.9931710887135993, + "velocityY": -0.14697735538800996, + "timestamp": 11.88744445663365 + }, + { + "x": 8.148337646767295, + "y": 0.7781655823556067, + "heading": -0.0077300757248004864, + "angularVelocity": 0.19806023893112515, + "velocityX": 0.662055274067148, + "velocityY": -0.09796534847923602, + "timestamp": 11.964353759283899 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": 1.1088837243155707e-19, + "angularVelocity": 0.10050898211824409, + "velocityX": 0.3310015661876223, + "velocityY": -0.04897379245809494, + "timestamp": 12.041263061934147 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": 5.633915553103528e-20, + "angularVelocity": 2.8365160942051004e-20, + "velocityX": -1.2324505157130932e-18, + "velocityY": 3.669165812220002e-18, + "timestamp": 12.118172364584396 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.1.traj b/src/main/deploy/choreo/SourceLanePGHSprint.1.traj index 3b1771e0..161fc9d2 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.1.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.1.traj @@ -3,182 +3,173 @@ { "x": 0.433241993188858, "y": 4.121134281158447, - "heading": -6.349582694240188e-38, + "heading": 8.991694014476798e-34, "angularVelocity": 0, - "velocityX": -2.609211774957871e-36, - "velocityY": -5.016830702674688e-35, + "velocityX": -4.6784915292932193e-35, + "velocityY": -2.119592179507727e-34, "timestamp": 0 }, { - "x": 0.45469188032137553, - "y": 4.109218526997661, - "heading": -0.008937300145995164, - "angularVelocity": -0.11893237262991438, - "velocityX": 0.2854425752342375, - "velocityY": -0.1585678997982562, - "timestamp": 0.07514606787342622 - }, - { - "x": 0.4975916635116251, - "y": 4.0853873108981436, - "heading": -0.026812413296636577, - "angularVelocity": -0.2378715700833438, - "velocityX": 0.570885269240018, - "velocityY": -0.31713191087600345, - "timestamp": 0.15029213574685243 - }, - { - "x": 0.5619414537480725, - "y": 4.049641027140498, - "heading": -0.05362171709802473, - "angularVelocity": -0.35676256336585627, - "velocityX": 0.856329440215504, - "velocityY": -0.4756906750976657, - "timestamp": 0.22543820362027867 - }, - { - "x": 0.6477414523773106, - "y": 4.001980177860296, - "heading": -0.08935853104911506, - "angularVelocity": -0.4755646564406316, - "velocityX": 1.141776290593902, - "velocityY": -0.6342427571923069, - "timestamp": 0.30058427149370487 - }, - { - "x": 0.7549919358777414, - "y": 3.942405375853288, - "heading": -0.13401442200653152, - "angularVelocity": -0.5942545261667384, - "velocityX": 1.4272268201854585, - "velocityY": -0.7927866845588475, - "timestamp": 0.37573033936713107 - }, - { - "x": 0.8836932393269182, - "y": 3.8709173416225418, - "heading": -0.18758057158075755, - "angularVelocity": -0.7128270459134503, - "velocityX": 1.7126818087934743, - "velocityY": -0.9513210238912078, - "timestamp": 0.4508764072405573 - }, - { - "x": 1.0338457421402325, - "y": 3.7875168932975933, - "heading": -0.2500489995023069, - "angularVelocity": -0.8312933688928248, - "velocityX": 1.998141846440012, - "velocityY": -1.109844476033343, - "timestamp": 0.5260224751139835 - }, - { - "x": 1.2054498603190866, - "y": 3.692204931169301, - "heading": -0.32141344961465557, - "angularVelocity": -0.9496764385936038, - "velocityX": 2.283607419990343, - "velocityY": -1.2683559476303248, - "timestamp": 0.6011685429874097 - }, - { - "x": 1.3985060453231253, - "y": 3.5849824249792595, - "heading": -0.40166985798593763, - "angularVelocity": -1.0680054278616893, - "velocityX": 2.5690790013020615, - "velocityY": -1.4268545144723213, - "timestamp": 0.676314610860836 - }, - { - "x": 1.5915937569821006, - "y": 3.4778198154599647, - "heading": -0.48176859267038563, - "angularVelocity": -1.065907198489266, - "velocityX": 2.5694985396202825, - "velocityY": -1.4260574445464824, - "timestamp": 0.7514606787342623 - }, - { - "x": 1.7632284858900336, - "y": 3.382566036520044, - "heading": -0.5529765476804537, - "angularVelocity": -0.9475938931363465, - "velocityX": 2.2840147697019786, - "velocityY": -1.2675816797275796, - "timestamp": 0.8266067466076885 - }, - { - "x": 1.9134098098562766, - "y": 3.2992204007390877, - "heading": -0.6152918418017524, - "angularVelocity": -0.8292555536806094, - "velocityX": 1.9985253815170194, - "velocityY": -1.1091150626981792, - "timestamp": 0.9017528144811148 - }, - { - "x": 2.0421373577646693, - "y": 3.2277823059512354, - "heading": -0.6687115003742364, - "angularVelocity": -0.7108776291856339, - "velocityX": 1.7130310547348626, - "velocityY": -0.9506564589404816, - "timestamp": 0.976898882354541 - }, - { - "x": 2.149410808485792, - "y": 3.1682512394298143, - "heading": -0.713232041811307, - "angularVelocity": -0.5924533737687984, - "velocityX": 1.4275324545498613, - "velocityY": -0.792204678250007, - "timestamp": 1.0520449502279672 - }, - { - "x": 2.2352298936713018, - "y": 3.120626784362015, - "heading": -0.7488501306819736, - "angularVelocity": -0.4739847323836121, - "velocityX": 1.1420302833417786, - "velocityY": -0.6337584442610682, - "timestamp": 1.1271910181013933 - }, - { - "x": 2.2995943987852443, - "y": 3.0849086278583613, - "heading": -0.7755632624607879, - "angularVelocity": -0.3554827622359307, - "velocityX": 0.8565252572144717, - "velocityY": -0.4753163740226096, - "timestamp": 1.2023370859748195 - }, - { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080857, - "angularVelocity": -0.23696688264902363, - "velocityX": 0.5710180813549187, - "velocityY": -0.3168769856057537, - "timestamp": 1.2774831538482456 + "x": 0.4570749914043126, + "y": 4.107894224868064, + "heading": -0.009932712149027413, + "angularVelocity": -0.1373640763265399, + "velocityX": 0.3295975698116412, + "velocityY": -0.18310287014791135, + "timestamp": 0.07230938695656872 + }, + { + "x": 0.5047410210186508, + "y": 4.0814144720383245, + "heading": -0.029797898632204542, + "angularVelocity": -0.27472486380099975, + "velocityX": 0.6591955985322896, + "velocityY": -0.36620076513225003, + "timestamp": 0.14461877391313743 + }, + { + "x": 0.576240240509544, + "y": 4.041695512105355, + "heading": -0.059590394556189516, + "angularVelocity": -0.41201422357348, + "velocityX": 0.9887958189140486, + "velocityY": -0.5492918914777362, + "timestamp": 0.21692816086970615 + }, + { + "x": 0.6715729183897807, + "y": 3.9887379727274364, + "heading": -0.09930156463679939, + "angularVelocity": -0.5491841620018386, + "velocityX": 1.3183997526835154, + "velocityY": -0.7323743376461849, + "timestamp": 0.28923754782627487 + }, + { + "x": 0.7907394144382421, + "y": 3.9225426248725555, + "heading": -0.148921029894937, + "angularVelocity": -0.686210564721571, + "velocityX": 1.648008662001748, + "velocityY": -0.9154461217412811, + "timestamp": 0.3615469347828436 + }, + { + "x": 0.9337401624459083, + "y": 3.8431103831358566, + "heading": -0.20843840181811515, + "angularVelocity": -0.8230932998910107, + "velocityX": 1.9776235704163385, + "velocityY": -1.0985052574765801, + "timestamp": 0.43385632173941235 + }, + { + "x": 1.100575665552893, + "y": 3.7504423098204516, + "heading": -0.2778446966612386, + "angularVelocity": -0.9598517946889951, + "velocityX": 2.307245436988583, + "velocityY": -1.2815497021301627, + "timestamp": 0.5061657086959811 + }, + { + "x": 1.2912465471408558, + "y": 3.6445396941775194, + "heading": -0.3571330045970933, + "angularVelocity": -1.0965147302863703, + "velocityX": 2.636875924594487, + "velocityY": -1.464576317132112, + "timestamp": 0.5784750956525498 + }, + { + "x": 1.505785276048115, + "y": 3.525464237325283, + "heading": -0.4461366707355329, + "angularVelocity": -1.2308729182270346, + "velocityX": 2.966955438802405, + "velocityY": -1.6467496387952614, + "timestamp": 0.6507844826091186 + }, + { + "x": 1.6964880033537257, + "y": 3.4196219693446848, + "heading": -0.5252603846379951, + "angularVelocity": -1.094238483172127, + "velocityX": 2.637316333772171, + "velocityY": -1.463741741361608, + "timestamp": 0.7230938695656873 + }, + { + "x": 1.8633540969095699, + "y": 3.3270118414974243, + "heading": -0.5945029830335408, + "angularVelocity": -0.9575879607046729, + "velocityX": 2.3076684864728434, + "velocityY": -1.280748347415596, + "timestamp": 0.7954032565222561 + }, + { + "x": 2.0063830653266717, + "y": 3.247633047534605, + "heading": -0.6538614517041024, + "angularVelocity": -0.8208957532196227, + "velocityX": 1.9780138435278007, + "velocityY": -1.0977661034589712, + "timestamp": 0.8677126434788248 + }, + { + "x": 2.125574496423577, + "y": 3.1814849244321213, + "heading": -0.7033318821965574, + "angularVelocity": -0.6841494939262661, + "velocityX": 1.648353500334547, + "velocityY": -0.9147930287697538, + "timestamp": 0.9400220304353936 + }, + { + "x": 2.220928049802523, + "y": 3.128566939134693, + "heading": -0.7429103365332542, + "angularVelocity": -0.5473487745162161, + "velocityX": 1.318688449622979, + "velocityY": -0.7318273259488806, + "timestamp": 1.0123314173919622 + }, + { + "x": 2.292443455008997, + "y": 3.0888786919590703, + "heading": -0.7725937492005902, + "angularVelocity": -0.4105056606988637, + "velocityX": 0.989019658670723, + "velocityY": -0.5488671505327598, + "timestamp": 1.084640804348531 + }, + { + "x": 2.34012050914709, + "y": 3.0624199236271457, + "heading": -0.7923807693651485, + "angularVelocity": -0.273643865580592, + "velocityX": 0.6593480617769434, + "velocityY": -0.3659105607936737, + "timestamp": 1.1569501913050997 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550474, - "velocityX": 0.2855094397331478, - "velocityY": -0.15843872456938407, - "timestamp": 1.3526292217216718 + "angularVelocity": -0.13679610671265519, + "velocityX": 0.3296745537009426, + "velocityY": -0.18295553238277804, + "timestamp": 1.2292595782616684 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 3.467177167133168e-32, - "velocityX": -1.439882477346126e-32, - "velocityY": -1.3815367235364851e-32, - "timestamp": 1.427775289595098 + "angularVelocity": 2.834394108236562e-32, + "velocityX": -3.2904683025944325e-34, + "velocityY": 0, + "timestamp": 1.3015689652182372 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.2.traj b/src/main/deploy/choreo/SourceLanePGHSprint.2.traj index 3cd03d5b..0c672467 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.2.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.2.traj @@ -4,811 +4,766 @@ "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 3.467177167133168e-32, - "velocityX": -1.439882477346126e-32, - "velocityY": -1.3815367235364851e-32, + "angularVelocity": 2.834394108236562e-32, + "velocityX": -3.2904683025944325e-34, + "velocityY": 0, "timestamp": 0 }, { - "x": 2.3773546785315864, - "y": 3.0407954088560687, - "heading": -0.7860770412156382, - "angularVelocity": 0.2650004205849919, - "velocityX": 0.21918861143707746, - "velocityY": -0.13736692695017716, - "timestamp": 0.06111450966076326 - }, - { - "x": 2.4041715283150715, - "y": 3.0239975454455843, - "heading": -0.75411979281041, - "angularVelocity": 0.5229077118120985, - "velocityX": 0.438796775632191, - "velocityY": -0.2748588429118795, - "timestamp": 0.12222901932152652 - }, - { - "x": 2.4444388170293236, - "y": 2.998788446329585, - "heading": -0.7069088657797715, - "angularVelocity": 0.772499481591177, - "velocityX": 0.6588826276733534, - "velocityY": -0.41248959135778174, - "timestamp": 0.18334352898228978 - }, - { - "x": 2.498190798561039, - "y": 2.965157152573787, - "heading": -0.6450804787000075, - "angularVelocity": 1.0116809808826641, - "velocityX": 0.8795289666903056, - "velocityY": -0.5502996578468837, - "timestamp": 0.24445803864305304 - }, - { - "x": 2.5654677170302604, - "y": 2.923088008476279, - "heading": -0.5694581806203519, - "angularVelocity": 1.237386972413308, - "velocityX": 1.1008338092322851, - "velocityY": -0.6883658943027919, - "timestamp": 0.3055725483038163 - }, - { - "x": 2.6463157759141644, - "y": 2.8725583813996622, - "heading": -0.4811095285927534, - "angularVelocity": 1.445624819997864, - "velocityX": 1.3228946666295571, - "velocityY": -0.8268024624119354, - "timestamp": 0.36668705796457957 - }, - { - "x": 2.7407862870930626, - "y": 2.813537598490058, - "heading": -0.38139476987658943, - "angularVelocity": 1.6316053138553275, - "velocityX": 1.5457951262848801, - "velocityY": -0.9657409220366661, - "timestamp": 0.4278015676253428 - }, - { - "x": 2.84893500974016, - "y": 2.7459886359527177, - "heading": -0.27202745092011676, - "angularVelocity": 1.7895475160244771, - "velocityX": 1.7696079580350594, - "velocityY": -1.105285191884772, - "timestamp": 0.4889160772861061 - }, - { - "x": 2.970822636317052, - "y": 2.669873093439706, - "heading": -0.15523119808259794, - "angularVelocity": 1.9111051284848073, - "velocityX": 1.9944138839282306, - "velocityY": -1.245457796119401, - "timestamp": 0.5500305869468693 - }, - { - "x": 3.106512263881207, - "y": 2.5851587340080315, - "heading": -0.03419676462946541, - "angularVelocity": 1.980453318286855, - "velocityX": 2.220252249708728, - "velocityY": -1.3861578846318265, - "timestamp": 0.6111450966076324 - }, - { - "x": 3.256035657144846, - "y": 2.4918371927335334, - "heading": 0.08572826850512172, - "angularVelocity": 1.962300504418209, - "velocityX": 2.4466103727841246, - "velocityY": -1.5269948461095477, - "timestamp": 0.6722596062683956 - }, - { - "x": 3.419180860242083, - "y": 2.390029027608911, - "heading": 0.19415018142326176, - "angularVelocity": 1.7740780956923756, - "velocityX": 2.6695003200194134, - "velocityY": -1.6658591501387068, - "timestamp": 0.7333741159291589 - }, - { - "x": 3.5929644641848295, - "y": 2.279455345020229, - "heading": 0.26705766200125053, - "angularVelocity": 1.1929651564364412, - "velocityX": 2.8435735622750062, - "velocityY": -1.8092869140644157, - "timestamp": 0.7944886255899222 - }, - { - "x": 3.7768655464749616, - "y": 2.1600020280366277, - "heading": 0.30152237044830693, - "angularVelocity": 0.5639365944088304, - "velocityX": 3.0091230922237155, - "velocityY": -1.9545819421061743, - "timestamp": 0.8556031352506854 - }, - { - "x": 3.9728967066647076, - "y": 2.0375522576737963, - "heading": 0.30152241765502197, - "angularVelocity": 7.724305624896446e-7, - "velocityX": 3.2076042379769354, - "velocityY": -2.003612088889046, - "timestamp": 0.9167176449114487 - }, - { - "x": 4.174386727718979, - "y": 1.9243094504352285, - "heading": 0.30152246485493484, - "angularVelocity": 7.723192600121745e-7, - "velocityX": 3.2969260847008353, - "velocityY": -1.85296107040964, - "timestamp": 0.977832154572212 - }, - { - "x": 4.383354452565466, - "y": 1.8255438101811146, - "heading": 0.30152251288342946, - "angularVelocity": 7.858771165145409e-7, - "velocityX": 3.419281705873656, - "velocityY": -1.6160751481496927, - "timestamp": 1.0389466642329752 - }, - { - "x": 4.598761540028333, - "y": 1.7417467464566943, - "heading": 0.30152256294147634, - "angularVelocity": 8.190861247953373e-7, - "velocityX": 3.5246472344874826, - "velocityY": -1.3711484259558735, - "timestamp": 1.1000611738937385 - }, - { - "x": 4.819537390717514, - "y": 1.6733348748555603, - "heading": 0.30152261646908723, - "angularVelocity": 8.758576510857166e-7, - "velocityX": 3.612494838208991, - "velocityY": -1.1194047367945392, - "timestamp": 1.1611756835545017 - }, - { - "x": 5.04458467733647, - "y": 1.620648272623701, - "heading": 0.30152267534909943, - "angularVelocity": 9.634375304093619e-7, - "velocityX": 3.682387175617667, - "velocityY": -0.8620964567058491, - "timestamp": 1.222290193215265 - }, - { - "x": 5.27278482830379, - "y": 1.5839488347993986, - "heading": 0.301522742248914, - "angularVelocity": 0.000001094663361600596, - "velocityX": 3.7339766322927477, - "velocityY": -0.6005028597630109, - "timestamp": 1.2834047028760283 + "x": 2.379804145625828, + "y": 3.040219511725313, + "heading": -0.7770894823147659, + "angularVelocity": 0.41593022428853316, + "velocityX": 0.2617028389642093, + "velocityY": -0.14816838427057621, + "timestamp": 0.06054604400989727 + }, + { + "x": 2.411575732183553, + "y": 3.0222750861201635, + "heading": -0.7276303412852089, + "angularVelocity": 0.8168847665996504, + "velocityX": 0.524750825215457, + "velocityY": -0.2963765163949562, + "timestamp": 0.12109208801979454 + }, + { + "x": 2.459369710281825, + "y": 2.995353279300503, + "heading": -0.6550403019178791, + "angularVelocity": 1.1989229115524662, + "velocityX": 0.789382343303212, + "velocityY": -0.4446501379224719, + "timestamp": 0.1816381320296918 + }, + { + "x": 2.5233031088128075, + "y": 2.959437775047564, + "heading": -0.5609434444834801, + "angularVelocity": 1.5541371690447268, + "velocityX": 1.0559467522028614, + "velocityY": -0.5931932439230606, + "timestamp": 0.24218417603958908 + }, + { + "x": 2.6035132843463873, + "y": 2.9144863047853526, + "heading": -0.44765744633251, + "angularVelocity": 1.8710718429836914, + "velocityX": 1.3247797910705466, + "velocityY": -0.7424344727603186, + "timestamp": 0.30273022004948635 + }, + { + "x": 2.700148811858614, + "y": 2.8604246981568315, + "heading": -0.31830060076747974, + "angularVelocity": 2.136503675514867, + "velocityX": 1.5960667470930034, + "velocityY": -0.8929007255979282, + "timestamp": 0.3632762640593836 + }, + { + "x": 2.8133605147902916, + "y": 2.7971622097455233, + "heading": -0.1768724119476308, + "angularVelocity": 2.3358782746686186, + "velocityX": 1.869844756713935, + "velocityY": -1.0448657620135586, + "timestamp": 0.4238223080692809 + }, + { + "x": 2.943296329434224, + "y": 2.7246257074465063, + "heading": -0.02888673141070084, + "angularVelocity": 2.4441841404657145, + "velocityX": 2.1460661347699634, + "velocityY": -1.1980386742882738, + "timestamp": 0.48436835207917817 + }, + { + "x": 3.0900160520490236, + "y": 2.6428091950168, + "heading": 0.11617462075416056, + "angularVelocity": 2.3958848928453333, + "velocityX": 2.4232751291036574, + "velocityY": -1.3513106226450158, + "timestamp": 0.5449143960890754 + }, + { + "x": 3.252842131476166, + "y": 2.55207108446101, + "heading": 0.23929724530787766, + "angularVelocity": 2.0335370636864525, + "velocityX": 2.6892934474880965, + "velocityY": -1.4986629108411686, + "timestamp": 0.6054604400989727 + }, + { + "x": 3.4280030327225406, + "y": 2.451410973839388, + "heading": 0.3174619447233944, + "angularVelocity": 1.2909959798981987, + "velocityX": 2.8930197523349617, + "velocityY": -1.662538193332126, + "timestamp": 0.66600648410887 + }, + { + "x": 3.6156107870917813, + "y": 2.340964520507575, + "heading": 0.3503075182755731, + "angularVelocity": 0.5424891764490772, + "velocityX": 3.098596406043856, + "velocityY": -1.8241729106819762, + "timestamp": 0.726552528118767 + }, + { + "x": 3.8155697764375462, + "y": 2.229387834544172, + "heading": 0.3503075829776931, + "angularVelocity": 0.0000010686432293648565, + "velocityX": 3.302593796434966, + "velocityY": -1.8428402348659505, + "timestamp": 0.7870985721286641 + }, + { + "x": 4.015529033815858, + "y": 2.117811628928195, + "heading": 0.35030764767858236, + "angularVelocity": 0.000001068622902276308, + "velocityX": 3.3025982233558384, + "velocityY": -1.8428323012769912, + "timestamp": 0.8476446161385611 + }, + { + "x": 4.215488291220884, + "y": 2.0062354233600974, + "heading": 0.35030771237947195, + "angularVelocity": 0.0000010686229082180607, + "velocityX": 3.302598223797092, + "velocityY": -1.8428323004862048, + "timestamp": 0.9081906601484582 + }, + { + "x": 4.415447878546615, + "y": 1.8946598090554148, + "heading": 0.350307777080342, + "angularVelocity": 0.0000010686225853054014, + "velocityX": 3.3026036728847936, + "velocityY": -1.842822534969312, + "timestamp": 0.9687367041583552 + }, + { + "x": 4.620289311400001, + "y": 1.7923224180578192, + "heading": 0.35030784179343544, + "angularVelocity": 0.00000106882447086681, + "velocityX": 3.383233970164951, + "velocityY": -1.6902407526553997, + "timestamp": 1.0292827481682523 + }, + { + "x": 4.832980772388526, + "y": 1.7074973259478416, + "heading": 0.35030790812662205, + "angularVelocity": 0.0000010955825050486418, + "velocityX": 3.512887827217186, + "velocityY": -1.401001394841118, + "timestamp": 1.0898287921781493 + }, + { + "x": 5.052028438396142, + "y": 1.6407809635052637, + "heading": 0.3503079787205065, + "angularVelocity": 0.0000011659537069164618, + "velocityX": 3.617869170309609, + "velocityY": -1.101911174108616, + "timestamp": 1.1503748361880464 + }, + { + "x": 5.275893582425239, + "y": 1.5926421132936275, + "heading": 0.3503080570287918, + "angularVelocity": 0.0000012933674954586782, + "velocityX": 3.697436350961301, + "velocityY": -0.7950783738037029, + "timestamp": 1.2109208801979434 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": 0.3015228161300743, - "angularVelocity": 0.0000012088972110609183, - "velocityX": 3.7670067261218194, - "velocityY": -0.33592432468507777, - "timestamp": 1.3445192125367915 - }, - { - "x": 5.642161233224445, - "y": 1.5569512204704454, - "heading": 0.30152289610635313, - "angularVelocity": 0.0000021712102282875206, - "velocityX": 3.7778762267369546, - "velocityY": -0.17558800471026415, - "timestamp": 1.38135409743325 - }, - { - "x": 5.781468008961295, - "y": 1.556401100893367, - "heading": 0.30152296986307525, - "angularVelocity": 0.0000020023605969229892, - "velocityX": 3.781925099764402, - "velocityY": -0.014934744023907952, - "timestamp": 1.4181889823297087 - }, - { - "x": 5.920672391336217, - "y": 1.5617696165699877, - "heading": 0.3015230386365787, - "angularVelocity": 0.0000018670752912406278, - "velocityX": 3.779145306581512, - "velocityY": 0.14574541746802047, - "timestamp": 1.4550238672261673 - }, - { - "x": 6.0595230325638285, - "y": 1.5730470711005764, - "heading": 0.3015231034083751, - "angularVelocity": 0.0000017584362380693717, - "velocityX": 3.769541879061509, - "velocityY": 0.30616233937716997, - "timestamp": 1.4918587521226259 - }, - { - "x": 6.197769224123459, - "y": 1.5902130982406153, - "heading": 0.3015231649742209, - "angularVelocity": 0.000001671400517937078, - "velocityX": 3.753132172076425, - "velocityY": 0.46602635486148125, - "timestamp": 1.5286936370190845 - }, - { - "x": 6.335161349542959, - "y": 1.6132366986444322, - "heading": 0.3015232239953199, - "angularVelocity": 0.0000016023152822250254, - "velocityX": 3.72994583275345, - "velocityY": 0.6250487946015117, - "timestamp": 1.565528521915543 - }, - { - "x": 6.471451335210211, - "y": 1.6420762959087598, - "heading": 0.30152328103508963, - "angularVelocity": 0.000001548525804202316, - "velocityX": 3.7000247469309935, - "velocityY": 0.7829425107583317, - "timestamp": 1.6023634068120016 - }, - { - "x": 6.606393098338155, - "y": 1.6766798119201127, - "heading": 0.301523336612691, - "angularVelocity": 0.000001508830595721816, - "velocityX": 3.663422961881362, - "velocityY": 0.9394224010369989, - "timestamp": 1.6391982917084602 - }, - { - "x": 6.739742463422568, - "y": 1.716984614087568, - "heading": 0.3015255656708468, - "angularVelocity": 0.00006051486687697733, - "velocityX": 3.6201922568579414, - "velocityY": 1.094201930608725, - "timestamp": 1.6760331766049188 - }, - { - "x": 6.8694207904091344, - "y": 1.7614703171513673, - "heading": 0.3101205256410531, - "angularVelocity": 0.23333750042565204, - "velocityX": 3.520530262306672, - "velocityY": 1.207705771006116, - "timestamp": 1.7128680615013774 - }, - { - "x": 6.994764102779472, - "y": 1.808693107878132, - "heading": 0.3306180633638746, - "angularVelocity": 0.5564707961064413, - "velocityX": 3.4028425152588873, - "velocityY": 1.282012713206686, - "timestamp": 1.749702946397836 - }, - { - "x": 7.114597005425528, - "y": 1.856512920318317, - "heading": 0.35962771339750194, - "angularVelocity": 0.7875591335542964, - "velocityX": 3.253244932973218, - "velocityY": 1.2982207647615676, - "timestamp": 1.7865378312942946 - }, - { - "x": 7.228563598795877, - "y": 1.9039637614742513, - "heading": 0.39146295222083616, - "angularVelocity": 0.8642687200685354, - "velocityX": 3.093985326429113, - "velocityY": 1.2882038667778264, - "timestamp": 1.8233727161907531 - }, - { - "x": 7.336676614220082, - "y": 1.9505339221399116, - "heading": 0.42341393247502157, - "angularVelocity": 0.8674108889982484, - "velocityX": 2.935071352282109, - "velocityY": 1.2642949963483496, - "timestamp": 1.8602076010872117 - }, - { - "x": 7.438996464546078, - "y": 1.995939510716552, - "heading": 0.45395278545537515, - "angularVelocity": 0.8290742068611583, - "velocityX": 2.77779747686502, - "velocityY": 1.2326789863542051, - "timestamp": 1.8970424859836703 - }, - { - "x": 7.535582073954796, - "y": 2.0400031624519186, - "heading": 0.48208291467910164, - "angularVelocity": 0.7636817463336474, - "velocityX": 2.6221232855814667, - "velocityY": 1.196247846551653, - "timestamp": 1.933877370880129 + "heading": 0.35030814446785413, + "angularVelocity": 0.0000014441746575542642, + "velocityX": 3.751029791428781, + "velocityY": -0.4826595916936126, + "timestamp": 1.2714669242078405 + }, + { + "x": 5.645916996346935, + "y": 1.5526078041590061, + "heading": 0.35030823517092025, + "angularVelocity": 0.0000023934601751232044, + "velocityX": 3.7711793420471578, + "velocityY": -0.28528395446410554, + "timestamp": 1.3093631328272466 + }, + { + "x": 5.78920070535826, + "y": 1.5493061559809091, + "heading": 0.350308318678533, + "angularVelocity": 0.000002203587530623072, + "velocityX": 3.7809510299653186, + "velocityY": -0.08712344317225716, + "timestamp": 1.3472593414466527 + }, + { + "x": 5.932460399917018, + "y": 1.5535231156733713, + "heading": 0.3503083968024594, + "angularVelocity": 0.0000020615235464554193, + "velocityX": 3.7803173398564307, + "velocityY": 0.11127655895113732, + "timestamp": 1.3851555500660588 + }, + { + "x": 6.075301824148574, + "y": 1.5652470604158988, + "heading": 0.3503084709306375, + "angularVelocity": 0.0000019560842841988308, + "velocityX": 3.769280079337772, + "velocityY": 0.3093698596678062, + "timestamp": 1.423051758685465 + }, + { + "x": 6.217331878166829, + "y": 1.5844456932271112, + "heading": 0.3503085421651287, + "angularVelocity": 0.0000018797260658699983, + "velocityX": 3.747869752477631, + "velocityY": 0.5066109120314745, + "timestamp": 1.460947967304871 + }, + { + "x": 6.358159707247505, + "y": 1.6110661098029424, + "heading": 0.3503086114142786, + "angularVelocity": 0.000001827337150563986, + "velocityX": 3.716145604300894, + "velocityY": 0.7024559328132687, + "timestamp": 1.4988441759242772 + }, + { + "x": 6.497397807591943, + "y": 1.6450348544134032, + "heading": 0.35030867945721406, + "angularVelocity": 0.0000017955077283623068, + "velocityX": 3.6741960585771425, + "velocityY": 0.8963626137804634, + "timestamp": 1.5367403845436833 + }, + { + "x": 6.634663343210931, + "y": 1.6862573848132372, + "heading": 0.3503087469924593, + "angularVelocity": 0.000001782110868887289, + "velocityX": 3.622144288827235, + "velocityY": 1.0877745268355006, + "timestamp": 1.5746365931630895 + }, + { + "x": 6.770832471288569, + "y": 1.7309686010151983, + "heading": 0.350308814465375, + "angularVelocity": 0.0000017804661250723641, + "velocityX": 3.593212435713354, + "velocityY": 1.179833493397679, + "timestamp": 1.6125328017824956 + }, + { + "x": 6.907000934216078, + "y": 1.775681842897667, + "heading": 0.35030888193834014, + "angularVelocity": 0.0000017804674286764754, + "velocityX": 3.5931948838222003, + "velocityY": 1.1798869467794602, + "timestamp": 1.6504290104019017 + }, + { + "x": 7.041712141501968, + "y": 1.8246105114710787, + "heading": 0.3503089535974655, + "angularVelocity": 0.0000018909312562539825, + "velocityX": 3.5547410201057947, + "velocityY": 1.2911230530949709, + "timestamp": 1.6883252190213078 + }, + { + "x": 7.171770756302622, + "y": 1.8790616059317657, + "heading": 0.3592109050640362, + "angularVelocity": 0.2349034848307258, + "velocityX": 3.43196904225537, + "velocityY": 1.43684807648023, + "timestamp": 1.726221427640714 + }, + { + "x": 7.296132052444026, + "y": 1.9323306109051834, + "heading": 0.3900794027691138, + "angularVelocity": 0.8145537200064432, + "velocityX": 3.281628972184848, + "velocityY": 1.4056552598282668, + "timestamp": 1.76411763626012 + }, + { + "x": 7.41350002407911, + "y": 1.9843017273758428, + "heading": 0.4310938047294686, + "angularVelocity": 1.0822824618754054, + "velocityX": 3.097090076050037, + "velocityY": 1.371406754501705, + "timestamp": 1.8020138448795262 + }, + { + "x": 7.523578248846203, + "y": 2.0344435812093704, + "heading": 0.471270328487765, + "angularVelocity": 1.0601726458124487, + "velocityX": 2.904729226942307, + "velocityY": 1.3231364207724636, + "timestamp": 1.8399100534989323 }, { "x": 7.626483917236328, "y": 2.082604169845581, "heading": 0.507098504392337, - "angularVelocity": 0.6791276743107281, - "velocityX": 2.4678193928677437, - "velocityY": 1.1565397180800703, - "timestamp": 1.9707122557765875 - }, - { - "x": 7.766333855095899, - "y": 2.1513911601167894, - "heading": 0.5393931714770351, - "angularVelocity": 0.5089507306836832, - "velocityX": 2.2039777611896034, - "velocityY": 1.0840548028640598, - "timestamp": 2.0341656809073783 - }, - { - "x": 7.8897168213730975, - "y": 2.214535022901093, - "heading": 0.5621690879226864, - "angularVelocity": 0.3589391179231937, - "velocityX": 1.9444650312080196, - "velocityY": 0.9951214241650669, - "timestamp": 2.0976191060381684 - }, - { - "x": 7.996856912991746, - "y": 2.271342778200504, - "heading": 0.5763089063837279, - "angularVelocity": 0.2228377496076287, - "velocityX": 1.6884839769927567, - "velocityY": 0.8952669644281369, - "timestamp": 2.1610725311689594 - }, - { - "x": 8.08793407853681, - "y": 2.3213235528188796, - "heading": 0.5824533246990756, - "angularVelocity": 0.09683351690917834, - "velocityX": 1.4353388387992245, - "velocityY": 0.7876765441007412, - "timestamp": 2.2245259562997495 - }, - { - "x": 8.16309393881241, - "y": 2.364112462024999, - "heading": 0.5810872026138336, - "angularVelocity": -0.021529524725041504, - "velocityX": 1.1844886248564517, - "velocityY": 0.6743356898059124, - "timestamp": 2.2879793814305405 - }, - { - "x": 8.222455939995415, - "y": 2.399428067130867, - "heading": 0.5725898810376202, - "angularVelocity": -0.13391430893917436, - "velocityX": 0.9355208337555891, - "velocityY": 0.5565594770191685, - "timestamp": 2.3514328065613306 - }, - { - "x": 8.266119462815807, - "y": 2.4270468912932266, - "heading": 0.5572661381126698, - "angularVelocity": -0.24149591441856846, - "velocityX": 0.6881192422693254, - "velocityY": 0.43526136068197446, - "timestamp": 2.4148862316921216 - }, - { - "x": 8.294168308263885, - "y": 2.4467872814650136, - "heading": 0.5353660980042331, - "angularVelocity": -0.3451356654632318, - "velocityX": 0.4420383200790668, - "velocityY": 0.3111004666981787, - "timestamp": 2.4783396568229117 + "angularVelocity": 0.9454290339278092, + "velocityX": 2.715460784576445, + "velocityY": 1.2708550641540455, + "timestamp": 1.8778062621183385 + }, + { + "x": 7.782116606596641, + "y": 2.1591418247265515, + "heading": 0.5502904833862925, + "angularVelocity": 0.6648323287723413, + "velocityX": 2.395575421885603, + "velocityY": 1.1781054843634051, + "timestamp": 1.9427729869972457 + }, + { + "x": 7.917274495842872, + "y": 2.2282082387650113, + "heading": 0.5787513876779874, + "angularVelocity": 0.4380843323215672, + "velocityX": 2.0804171596791274, + "velocityY": 1.0631044456557224, + "timestamp": 2.0077397118761526 + }, + { + "x": 8.032169166662662, + "y": 2.2890615062609574, + "heading": 0.5943003276772478, + "angularVelocity": 0.2393369840982844, + "velocityX": 1.768515667581298, + "velocityY": 0.9366836270316962, + "timestamp": 2.07270643675506 + }, + { + "x": 8.12694730000964, + "y": 2.3412528312827354, + "heading": 0.5980433212943994, + "angularVelocity": 0.05761401123618554, + "velocityX": 1.458871961356148, + "velocityY": 0.8033547191898265, + "timestamp": 2.137673161633967 + }, + { + "x": 8.201715109739398, + "y": 2.3844820330172567, + "heading": 0.590722775552934, + "angularVelocity": -0.11268146509017382, + "velocityX": 1.1508631513920338, + "velocityY": 0.6654052796273325, + "timestamp": 2.2026398865128742 + }, + { + "x": 8.256552657549896, + "y": 2.418534329374767, + "heading": 0.572871769156374, + "angularVelocity": -0.27477153003838295, + "velocityX": 0.8440866907283819, + "velocityY": 0.5241498077820845, + "timestamp": 2.2676066113917814 + }, + { + "x": 8.291522317600203, + "y": 2.443248428942356, + "heading": 0.5448922111974887, + "angularVelocity": -0.4306752112106213, + "velocityX": 0.538270324007952, + "velocityY": 0.3804116586399298, + "timestamp": 2.3325733362706886 }, { "x": 8.306674003601074, "y": 2.45849871635437, "heading": 0.507098504392337, - "angularVelocity": -0.44548570157766576, - "velocityX": 0.19708463824313602, - "velocityY": 0.18456741878341876, - "timestamp": 2.5417930819537027 - }, - { - "x": 8.303024530407173, - "y": 2.461881127582199, - "heading": 0.47119103232326837, - "angularVelocity": -0.5465136547157349, - "velocityX": -0.05554517814981655, - "velocityY": 0.05148048067312068, - "timestamp": 2.607495872285164 - }, - { - "x": 8.282750419132642, - "y": 2.456558519757899, - "heading": 0.4288756816904664, - "angularVelocity": -0.6440419108431604, - "velocityX": -0.3085730632177365, - "velocityY": -0.08101037714605593, - "timestamp": 2.6731986626166258 - }, - { - "x": 8.245821395629305, - "y": 2.442576926285189, - "heading": 0.3804195474569323, - "angularVelocity": -0.7375049672788585, - "velocityX": -0.5620617224479442, - "velocityY": -0.21280060408660287, - "timestamp": 2.7389014529480873 - }, - { - "x": 8.19220201387014, - "y": 2.4199912479232255, - "heading": 0.32613641295591117, - "angularVelocity": -0.8261922245184762, - "velocityX": -0.8160898721144456, - "velocityY": -0.34375523852216117, - "timestamp": 2.804604243279549 - }, - { - "x": 8.121850214183777, - "y": 2.388868090056124, - "heading": 0.2664003971864745, - "angularVelocity": -0.909185370485436, - "velocityX": -1.070758172239708, - "velocityY": -0.4736961354318384, - "timestamp": 2.8703070336110104 - }, - { - "x": 8.034715295558529, - "y": 2.349289919990928, - "heading": 0.20166644793914118, - "angularVelocity": -0.9852541866298153, - "velocityX": -1.3261981444876794, - "velocityY": -0.6023818754961501, - "timestamp": 2.936009823942472 - }, - { - "x": 7.9307349853362235, - "y": 2.301361396168203, - "heading": 0.1325025874209066, - "angularVelocity": -1.052677674255729, - "velocityX": -1.5825859099398853, - "velocityY": -0.7294747084702512, - "timestamp": 3.0017126142739334 - }, - { - "x": 7.809831081747403, - "y": 2.2452194829459002, - "heading": 0.0596429970770519, - "angularVelocity": -1.1089268808263426, - "velocityX": -1.8401639105261376, - "velocityY": -0.8544829365552749, - "timestamp": 3.067415404605395 - }, - { - "x": 7.671902780233127, - "y": 2.1810506699963903, - "heading": -0.015920256793080897, - "angularVelocity": -1.1500767850029918, - "velocityX": -2.099276161917122, - "velocityY": -0.9766527818040505, - "timestamp": 3.1331181949368565 - }, - { - "x": 7.516816185598606, - "y": 2.109122861945235, - "heading": -0.09276870443286889, - "angularVelocity": -1.1696375032490751, - "velocityX": -2.3604263053689314, - "velocityY": -1.0947451042534069, - "timestamp": 3.198820985268318 - }, - { - "x": 7.344387868807497, - "y": 2.02985165560778, - "heading": -0.16871929267657412, - "angularVelocity": -1.1559720349858067, - "velocityX": -2.6243682486121434, - "velocityY": -1.206512020837212, - "timestamp": 3.2645237755997796 - }, - { - "x": 7.154363408340053, - "y": 1.9439629823466762, - "heading": -0.2400485240661188, - "angularVelocity": -1.0856347352935642, - "velocityX": -2.8921825010596516, - "velocityY": -1.3072302230667405, - "timestamp": 3.330226565931241 - }, - { - "x": 6.9464441838510815, - "y": 1.8530162365161489, - "heading": -0.2992772419214471, - "angularVelocity": -0.9014642689682996, - "velocityX": -3.1645417712101533, - "velocityY": -1.3842143594162994, - "timestamp": 3.3959293562627026 - }, - { - "x": 6.72212326982476, - "y": 1.7626751908782685, - "heading": -0.31855321839260475, - "angularVelocity": -0.2933813978662556, - "velocityX": -3.4141763674670904, - "velocityY": -1.3749955699312393, - "timestamp": 3.461632146594164 - }, - { - "x": 6.485759745047901, - "y": 1.6860130581467425, - "heading": -0.3185532921212082, - "angularVelocity": -0.0000011221533066716862, - "velocityX": -3.597465550312819, - "velocityY": -1.1668017803319484, - "timestamp": 3.5273349369256257 - }, - { - "x": 6.24426876944993, - "y": 1.6274732096370486, - "heading": -0.3185533177664736, - "angularVelocity": -3.9032231775904613e-7, - "velocityX": -3.675505627381763, - "velocityY": -0.8909796405048926, - "timestamp": 3.5930377272570873 - }, - { - "x": 5.999037658914315, - "y": 1.5873919634602989, - "heading": -0.3185533462426243, - "angularVelocity": -4.334085446884588e-7, - "velocityX": -3.732430682143934, - "velocityY": -0.6100387209515062, - "timestamp": 3.658740517588549 - }, - { - "x": 5.751475205523387, - "y": 1.5659995878977724, - "heading": -0.3185533789943251, - "angularVelocity": -4.984826463431063e-7, - "velocityX": -3.7679138456983283, - "velocityY": -0.325593105781428, - "timestamp": 3.7244433079200103 + "angularVelocity": -0.5817394500892022, + "velocityX": 0.23322225384012837, + "velocityY": 0.2347399755865949, + "timestamp": 2.397540061149596 + }, + { + "x": 8.301908465997242, + "y": 2.464162051150854, + "heading": 0.4594182049840147, + "angularVelocity": -0.7297490572088752, + "velocityX": -0.07293676039464714, + "velocityY": 0.08667758549500083, + "timestamp": 2.4628780001812522 + }, + { + "x": 8.277129357589926, + "y": 2.4601442119160213, + "heading": 0.4025234718451046, + "angularVelocity": -0.8707763664131557, + "velocityX": -0.3792453324141557, + "velocityY": -0.061493204321711674, + "timestamp": 2.5282159392129087 + }, + { + "x": 8.232325554908503, + "y": 2.44643699593842, + "heading": 0.33696231020008877, + "angularVelocity": -1.0034164318107932, + "velocityX": -0.6857241496355521, + "velocityY": -0.20978953699413314, + "timestamp": 2.593553878244565 + }, + { + "x": 8.167484390379387, + "y": 2.42303097033958, + "heading": 0.26340235513425153, + "angularVelocity": -1.1258383131766259, + "velocityX": -0.992396844621899, + "velocityY": -0.35823024028199546, + "timestamp": 2.6588918172762215 + }, + { + "x": 8.082591447673355, + "y": 2.3899154191246006, + "heading": 0.18267464694130434, + "angularVelocity": -1.235541086685248, + "velocityX": -1.2992901821543714, + "velocityY": -0.5068349523381112, + "timestamp": 2.724229756307878 + }, + { + "x": 7.9776304347285665, + "y": 2.347078349358033, + "heading": 0.09584952732096214, + "angularVelocity": -1.328862233904795, + "velocityX": -1.6064328704022224, + "velocityY": -0.6556232167931427, + "timestamp": 2.7895676953395343 + }, + { + "x": 7.852583598052917, + "y": 2.2945065473278667, + "heading": 0.004378357289613106, + "angularVelocity": -1.3999702376138785, + "velocityX": -1.9138472766192256, + "velocityY": -0.8046137176854637, + "timestamp": 2.8549056343711907 + }, + { + "x": 7.707434523289432, + "y": 2.232185897198679, + "heading": -0.08962534429630348, + "angularVelocity": -1.4387307432573238, + "velocityX": -2.221512905283988, + "velocityY": -0.9538202620531743, + "timestamp": 2.920243573402847 + }, + { + "x": 7.542180705960218, + "y": 2.1601037497785995, + "heading": -0.1827828763211059, + "angularVelocity": -1.42578008130418, + "velocityX": -2.5292168650919318, + "velocityY": -1.1032204028528767, + "timestamp": 2.9855815124345035 + }, + { + "x": 7.35689136818444, + "y": 2.078264678726032, + "heading": -0.2689709068831343, + "angularVelocity": -1.3191115581449608, + "velocityX": -2.835861377353881, + "velocityY": -1.252550543611691, + "timestamp": 3.05091945146616 + }, + { + "x": 7.152059282385439, + "y": 1.9867952525471149, + "heading": -0.3348300100346558, + "angularVelocity": -1.007976439532523, + "velocityX": -3.134963986234081, + "velocityY": -1.3999435478765458, + "timestamp": 3.1162573904978164 + }, + { + "x": 6.932416862636984, + "y": 1.8843357350106482, + "heading": -0.3485403575636041, + "angularVelocity": -0.20983746552376023, + "velocityX": -3.3616367917885555, + "velocityY": -1.5681473743275887, + "timestamp": 3.1815953295294728 + }, + { + "x": 6.70958980281502, + "y": 1.7775224962078198, + "heading": -0.34854038203919246, + "angularVelocity": -3.7459994478515855e-7, + "velocityX": -3.410377846690442, + "velocityY": -1.6347812677571678, + "timestamp": 3.246933268561129 + }, + { + "x": 6.478027967608038, + "y": 1.6912665812341474, + "heading": -0.3485404067374628, + "angularVelocity": -3.780081020278718e-7, + "velocityX": -3.5440639640437577, + "velocityY": -1.3201505320191125, + "timestamp": 3.3122712075927856 + }, + { + "x": 6.239619726654668, + "y": 1.6262859956866853, + "heading": -0.34854043255776096, + "angularVelocity": -3.951807866461482e-7, + "velocityX": -3.6488485019072954, + "velocityY": -0.9945306893745133, + "timestamp": 3.377609146624442 + }, + { + "x": 5.996315346781838, + "y": 1.5831125822372847, + "heading": -0.3485404605967219, + "angularVelocity": -4.291375179720768e-7, + "velocityX": -3.7237841211206324, + "velocityY": -0.6607709714945885, + "timestamp": 3.4429470856560984 + }, + { + "x": 5.7501052372367845, + "y": 1.5620996014320112, + "heading": -0.34854049237288, + "angularVelocity": -4.863354823397662e-7, + "velocityX": -3.7682564401941674, + "velocityY": -0.3216045855853207, + "timestamp": 3.508285024687755 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.3185534180019772, - "angularVelocity": -5.936985611348973e-7, - "velocityX": -3.781751231726404, - "velocityY": -0.039276923728911305, - "timestamp": 3.790146098251472 - }, - { - "x": 5.26265149067387, - "y": 1.5786031053003822, - "heading": -0.31855345112044986, - "angularVelocity": -5.20084412877405e-7, - "velocityX": -3.7744308273915435, - "velocityY": 0.23844772894968475, - "timestamp": 3.853825131614962 - }, - { - "x": 5.0240625547753925, - "y": 1.61139052009726, - "heading": -0.3185534786660252, - "angularVelocity": -4.3256899255575315e-7, - "velocityX": -3.7467424251962935, - "velocityY": 0.5148855606793222, - "timestamp": 3.9175041649784523 - }, - { - "x": 4.788524290519247, - "y": 1.661604289022676, - "heading": -0.31855350247111613, - "angularVelocity": -3.7382933947955595e-7, - "velocityX": -3.698835422197071, - "velocityY": 0.7885447732660761, - "timestamp": 3.9811831983419426 - }, - { - "x": 4.55730773239592, - "y": 1.7289734284079028, - "heading": -0.3185535237219762, - "angularVelocity": -3.337183198598428e-7, - "velocityX": -3.6309684037367047, - "velocityY": 1.0579485244487485, - "timestamp": 4.044862231705433 - }, - { - "x": 4.33166058516164, - "y": 1.8131343663413284, - "heading": -0.3185535432404584, - "angularVelocity": -3.0651348259884106e-7, - "velocityX": -3.543507734268667, - "velocityY": 1.321642831055895, - "timestamp": 4.108541265068923 - }, - { - "x": 4.112800476187319, - "y": 1.9136328827222209, - "heading": -0.318553561635836, - "angularVelocity": -2.888765193967595e-7, - "velocityX": -3.436925741712075, - "velocityY": 1.5782041760469432, - "timestamp": 4.172220298432413 - }, - { - "x": 3.9019083093617053, - "y": 2.0299264483320134, - "heading": -0.31855357939397577, - "angularVelocity": -2.7886949297236075e-7, - "velocityX": -3.311799122668965, - "velocityY": 1.826245774585971, - "timestamp": 4.2358993317959035 - }, - { - "x": 3.7001203074329077, - "y": 2.161384813738153, - "heading": -0.3185535969362317, - "angularVelocity": -2.75479306959021e-7, - "velocityX": -3.1688295388681516, - "velocityY": 2.0643900898393674, - "timestamp": 4.299578365159394 - }, - { - "x": 3.509285404964888, - "y": 2.2879634464070207, - "heading": -0.3579155843505842, - "angularVelocity": -0.6181310446354924, - "velocityX": -2.996824737880426, - "velocityY": 1.9877599577609337, - "timestamp": 4.363257398522884 - }, - { - "x": 3.332909755538667, - "y": 2.4050971261724468, - "heading": -0.4129449232759771, - "angularVelocity": -0.8641673093760197, - "velocityX": -2.769760156682042, - "velocityY": 1.8394387222684185, - "timestamp": 4.426936431886374 - }, - { - "x": 3.171287726021972, - "y": 2.512479984884094, - "heading": -0.4699204658498491, - "angularVelocity": -0.8947300165290918, - "velocityX": -2.5380729100288124, - "velocityY": 1.6863142079856834, - "timestamp": 4.490615465249864 - }, - { - "x": 3.024408806786741, - "y": 2.6100923699912792, - "heading": -0.5252799185719804, - "angularVelocity": -0.8693513358805374, - "velocityX": -2.306550704009953, - "velocityY": 1.5328810748429236, - "timestamp": 4.554294498613355 - }, - { - "x": 2.892255079193046, - "y": 2.6979336450739244, - "heading": -0.5773440254447586, - "angularVelocity": -0.8176020288434336, - "velocityX": -2.0753098879397367, - "velocityY": 1.3794379475145813, - "timestamp": 4.617973531976845 - }, - { - "x": 2.7748123815012584, - "y": 2.7760062309054065, - "heading": -0.625128837055519, - "angularVelocity": -0.7504010203483636, - "velocityX": -1.8442914643726742, - "velocityY": 1.2260328354205916, - "timestamp": 4.681652565340335 - }, - { - "x": 2.6720699818921556, - "y": 2.844312916696855, - "heading": -0.6679860381773631, - "angularVelocity": -0.6730190277419643, - "velocityX": -1.6134415706757543, - "velocityY": 1.0726715244175122, - "timestamp": 4.745331598703825 - }, - { - "x": 2.5840196243973645, - "y": 2.9028562717726505, - "heading": -0.7054558016839698, - "angularVelocity": -0.5884160221579265, - "velocityX": -1.382721326691407, - "velocityY": 0.9193505614575078, - "timestamp": 4.809010632067316 - }, - { - "x": 2.510654811151598, - "y": 2.95163853061359, - "heading": -0.7371951288142529, - "angularVelocity": -0.4984266477336408, - "velocityX": -1.1521031235978156, - "velocityY": 0.7660646882386306, - "timestamp": 4.872689665430806 - }, - { - "x": 2.4519703126340358, - "y": 2.9906615990271557, - "heading": -0.7629386573341134, - "angularVelocity": -0.4042700895428552, - "velocityX": -0.9215670436230066, - "velocityY": 0.6128087433553882, - "timestamp": 4.936368698794296 - }, - { - "x": 2.4079618327799786, - "y": 3.0199270886206313, - "heading": -0.7824753669479414, - "angularVelocity": -0.3067997201262361, - "velocityX": -0.6910984279998401, - "velocityY": 0.4595781067596215, - "timestamp": 5.000047732157786 - }, - { - "x": 2.378625774904473, - "y": 3.039436353320747, - "heading": -0.795633833561549, - "angularVelocity": -0.20663734856804464, - "velocityX": -0.4606862938394533, - "velocityY": 0.3063687318988399, - "timestamp": 5.0637267655212765 + "heading": -0.348540530032263, + "angularVelocity": -5.763784949174497e-7, + "velocityX": -3.7819013522532754, + "velocityY": 0.02019321393800792, + "timestamp": 3.5736229637194112 + }, + { + "x": 5.265773365103949, + "y": 1.585443267719361, + "heading": -0.3485405619317938, + "angularVelocity": -5.063689324990554e-7, + "velocityX": -3.7657613238076486, + "velocityY": 0.34961055977354083, + "timestamp": 3.636619582930451 + }, + { + "x": 5.031364041570911, + "y": 1.6280522215500413, + "heading": -0.3485405890146467, + "angularVelocity": -4.299096234955573e-7, + "velocityX": -3.720982593490641, + "velocityY": 0.6763688966853497, + "timestamp": 3.699616202141491 + }, + { + "x": 4.801558310439742, + "y": 1.6909217775230576, + "heading": -0.3485406130254673, + "angularVelocity": -3.811445901784285e-7, + "velocityX": -3.6479057766785146, + "velocityY": 0.9979830149678419, + "timestamp": 3.7626128213525307 + }, + { + "x": 4.57810382416931, + "y": 1.7735737496127562, + "heading": -0.34854063513060685, + "angularVelocity": -3.5089406173482316e-7, + "velocityX": -3.5470869559183034, + "velocityY": 1.3120064715347763, + "timestamp": 3.8256094405635706 + }, + { + "x": 4.3626998372206165, + "y": 1.8753793427019887, + "heading": -0.34854065619045194, + "angularVelocity": -3.3430119484907394e-7, + "velocityX": -3.4192943946259384, + "velocityY": 1.6160485175907584, + "timestamp": 3.8886060597746104 + }, + { + "x": 4.156979523974483, + "y": 1.9955559179970916, + "heading": -0.34854067691382595, + "angularVelocity": -3.2896009785914497e-7, + "velocityX": -3.265577039253029, + "velocityY": 1.907667058965506, + "timestamp": 3.95160267898565 + }, + { + "x": 3.9516956402233028, + "y": 2.116476485340398, + "heading": -0.3485406976311522, + "angularVelocity": -3.288640962267947e-7, + "velocityX": -3.258649215182055, + "velocityY": 1.9194770903216498, + "timestamp": 4.01459929819669 + }, + { + "x": 3.746411773108691, + "y": 2.2373970809271935, + "heading": -0.34854071834847844, + "angularVelocity": -3.288640958736373e-7, + "velocityX": -3.2586489510953776, + "velocityY": 1.9194775386550607, + "timestamp": 4.07759591740773 + }, + { + "x": 3.5411278789507334, + "y": 2.3583176306028646, + "heading": -0.34854073906621164, + "angularVelocity": -3.2887055643589624e-7, + "velocityX": -3.2586493803778747, + "velocityY": 1.919476809867947, + "timestamp": 4.14059253661877 + }, + { + "x": 3.345073520020233, + "y": 2.47307412374708, + "heading": -0.3873766723080469, + "angularVelocity": -0.6164764669629881, + "velocityX": -3.1121409590840567, + "velocityY": 1.821629391884994, + "timestamp": 4.2035891558298095 + }, + { + "x": 3.1664636949700515, + "y": 2.5778338770119946, + "heading": -0.44780580157610866, + "angularVelocity": -0.9592440042793773, + "velocityX": -2.835228735876023, + "velocityY": 1.6629424654356146, + "timestamp": 4.266585775040849 + }, + { + "x": 3.0057996639738764, + "y": 2.672135804524931, + "heading": -0.5103083460228613, + "angularVelocity": -0.9921571225491842, + "velocityX": -2.5503595749788768, + "velocityY": 1.496936322837019, + "timestamp": 4.329582394251889 + }, + { + "x": 2.8630598597209174, + "y": 2.7559513152021253, + "heading": -0.5701467016897592, + "angularVelocity": -0.9498661422835671, + "velocityX": -2.265832770720234, + "velocityY": 1.3304763291568347, + "timestamp": 4.392579013462929 + }, + { + "x": 2.738214483951598, + "y": 2.829279302871158, + "heading": -0.6251012943842897, + "angularVelocity": -0.8723419348970285, + "velocityX": -1.9817789800923042, + "velocityY": 1.1639987762419943, + "timestamp": 4.455575632673969 + }, + { + "x": 2.6312405711995455, + "y": 2.892122971419519, + "heading": -0.6738749327170909, + "angularVelocity": -0.7742262829916016, + "velocityX": -1.698089740239665, + "velocityY": 0.9975720814133604, + "timestamp": 4.518572251885009 + }, + { + "x": 2.5421208919645824, + "y": 2.9444859859038597, + "heading": -0.7156160888836572, + "angularVelocity": -0.6625935913597552, + "velocityX": -1.414673999193671, + "velocityY": 0.8312035652091746, + "timestamp": 4.5815688710960485 + }, + { + "x": 2.4708422721118457, + "y": 2.98637166116498, + "heading": -0.749723844710458, + "angularVelocity": -0.5414220041322991, + "velocityX": -1.1314673826217794, + "velocityY": 0.6648876683493641, + "timestamp": 4.644565490307088 + }, + { + "x": 2.4173943837507443, + "y": 3.0177828179574866, + "heading": -0.775752501403198, + "angularVelocity": -0.4131754532023893, + "velocityX": -0.84842470961893, + "velocityY": 0.4986165477750299, + "timestamp": 4.707562109518128 + }, + { + "x": 2.3817689275168203, + "y": 3.038721799552771, + "heading": -0.7933591587514436, + "angularVelocity": -0.2794857496917854, + "velocityX": -0.565513779629629, + "velocityY": 0.3323826239808009, + "timestamp": 4.770558728729168 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.10425061542851642, - "velocityX": -0.2303222914890624, - "velocityY": 0.15317707264507022, - "timestamp": 5.127405798884767 + "angularVelocity": -0.14148780267529462, + "velocityX": -0.28271125846882317, + "velocityY": 0.16617910323716242, + "timestamp": 4.833555347940208 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -1.4092752497411054e-30, - "velocityX": 2.9428006004895336e-31, - "velocityY": 5.314710200495422e-31, - "timestamp": 5.191084832248257 + "angularVelocity": 1.2572572851920672e-28, + "velocityX": 2.1335040537014396e-30, + "velocityY": 1.8634721866519574e-29, + "timestamp": 4.896551967151248 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.3.traj b/src/main/deploy/choreo/SourceLanePGHSprint.3.traj index 2bed8424..81315e26 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.3.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.3.traj @@ -4,775 +4,721 @@ "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -1.4092752497411054e-30, - "velocityX": 2.9428006004895336e-31, - "velocityY": 5.314710200495422e-31, + "angularVelocity": 1.2572572851920672e-28, + "velocityX": 2.1335040537014396e-30, + "velocityY": 1.8634721866519574e-29, "timestamp": 0 }, { - "x": 2.3794842314620843, - "y": 3.041727640405621, - "heading": -0.7962432666233688, - "angularVelocity": 0.0957711014436812, - "velocityX": 0.24661230413774035, - "velocityY": -0.11854554422657079, - "timestamp": 0.06295370174647719 - }, - { - "x": 2.410536447869941, - "y": 3.0268006675090624, - "heading": -0.7842917338243374, - "angularVelocity": 0.18984638658997072, - "velocityX": 0.4932548134009415, - "velocityY": -0.23711032842312177, - "timestamp": 0.12590740349295437 - }, - { - "x": 2.4571179241280796, - "y": 3.0044081651489933, - "heading": -0.7665449064601504, - "angularVelocity": 0.28190284084732253, - "velocityX": 0.7399322830248891, - "velocityY": -0.3556979452971155, - "timestamp": 0.18886110523943156 - }, - { - "x": 2.5192312369840457, - "y": 2.9745484345249116, - "heading": -0.743155503226628, - "angularVelocity": 0.37153340605314533, - "velocityX": 0.9866506834833091, - "velocityY": -0.4743125470894598, - "timestamp": 0.25181480698590875 - }, - { - "x": 2.596879439737626, - "y": 2.937219467270397, - "heading": -0.714309410311632, - "angularVelocity": 0.4582112268975565, - "velocityX": 1.233417584660547, - "velocityY": -0.5929590511586358, - "timestamp": 0.31476850873238593 - }, - { - "x": 2.690066196626848, - "y": 2.892418878795792, - "heading": -0.6802368283616581, - "angularVelocity": 0.5412323819683986, - "velocityX": 1.4802426911208155, - "velocityY": -0.7116434336939077, - "timestamp": 0.3777222104788631 - }, - { - "x": 2.798795963694134, - "y": 2.8401438158547943, - "heading": -0.6412294413317386, - "angularVelocity": 0.6196202280051365, - "velocityX": 1.7271385804309867, - "velocityY": -0.8303731391605256, - "timestamp": 0.4406759122253403 - }, - { - "x": 2.9230742308720847, - "y": 2.7803908299690017, - "heading": -0.5976682545562845, - "angularVelocity": 0.6919559226378926, - "velocityX": 1.9741216756154527, - "velocityY": -0.9491576226355342, - "timestamp": 0.5036296139718175 - }, - { - "x": 3.062907829622796, - "y": 2.713155712528932, - "heading": -0.5500718966234338, - "angularVelocity": 0.7560533632244142, - "velocityX": 2.22121328645359, - "velocityY": -1.0680089585650514, - "timestamp": 0.5665833157182947 - }, - { - "x": 3.218305230273769, - "y": 2.638433320090989, - "heading": -0.4991885737635272, - "angularVelocity": 0.8082657802208455, - "velocityX": 2.4684394458133507, - "velocityY": -1.1869419964986359, - "timestamp": 0.6295370174647719 - }, - { - "x": 3.389276272924326, - "y": 2.556217612467452, - "heading": -0.4461960441931904, - "angularVelocity": 0.8417698737358584, - "velocityX": 2.715821912094713, - "velocityY": -1.3059709809381974, - "timestamp": 0.692490719211249 - }, - { - "x": 3.575827436112715, - "y": 2.4665034787202997, - "heading": -0.3932374103043789, - "angularVelocity": 0.841231451362194, - "velocityX": 2.9633072879440143, - "velocityY": -1.4250811510408536, - "timestamp": 0.7554444209577262 - }, - { - "x": 3.777911234478158, - "y": 2.369308059755886, - "heading": -0.34555593776982274, - "angularVelocity": 0.7574053822375035, - "velocityX": 3.210038373585424, - "velocityY": -1.5439190431697394, - "timestamp": 0.8183981227042034 - }, - { - "x": 3.9926293770053456, - "y": 2.266439040708753, - "heading": -0.3455557491739834, - "angularVelocity": 0.000002995786333953476, - "velocityX": 3.4107310066036556, - "velocityY": -1.6340424183696203, - "timestamp": 0.8813518244506806 - }, - { - "x": 4.207200483864901, - "y": 2.1632635581376745, - "heading": -0.34555573252846533, - "angularVelocity": 2.644088848166298e-7, - "velocityX": 3.4083953906898548, - "velocityY": -1.6389104962656516, - "timestamp": 0.9443055261971578 - }, - { - "x": 4.421771581956281, - "y": 2.0600880573316713, - "heading": -0.3455557158829472, - "angularVelocity": 2.6440888572481287e-7, - "velocityX": 3.408395251410094, - "velocityY": -1.638910785921765, - "timestamp": 1.007259227943635 - }, - { - "x": 4.636342680047139, - "y": 1.9569125565245837, - "heading": -0.3455556992374292, - "angularVelocity": 2.6440888404451857e-7, - "velocityX": 3.4083952514018088, - "velocityY": -1.6389107859389946, - "timestamp": 1.0702129296901122 - }, - { - "x": 4.850913778140846, - "y": 1.853737055723424, - "heading": -0.3455556825919111, - "angularVelocity": 2.64408884846605e-7, - "velocityX": 3.408395251447087, - "velocityY": -1.63891078584483, - "timestamp": 1.1331666314365894 - }, - { - "x": 5.065484924140348, - "y": 1.7505616545506137, - "heading": -0.34555566594639303, - "angularVelocity": 2.644088848456774e-7, - "velocityX": 3.4083960124157384, - "velocityY": -1.6389092032794423, - "timestamp": 1.1961203331830665 - }, - { - "x": 5.280845302209934, - "y": 1.6490439336616942, - "heading": -0.34555564929194366, - "angularVelocity": 2.6455075541297124e-7, - "velocityX": 3.4209327187283005, - "velocityY": -1.6125774668143436, - "timestamp": 1.2590740349295437 + "x": 2.384240228496088, + "y": 3.0394763007756747, + "heading": -0.7921549333865273, + "angularVelocity": 0.15389773911589558, + "velocityX": 0.30849818872977186, + "velocityY": -0.14776374894381028, + "timestamp": 0.06574156742770221 + }, + { + "x": 2.4248103079231944, + "y": 3.020044385403153, + "heading": -0.7722377725963315, + "angularVelocity": 0.3029614529969845, + "velocityX": 0.6171145747585456, + "velocityY": -0.2955803479114183, + "timestamp": 0.13148313485540442 + }, + { + "x": 2.485678594891513, + "y": 2.9908905714242224, + "heading": -0.7429157303412026, + "angularVelocity": 0.4460198228065572, + "velocityX": 0.9258721589693997, + "velocityY": -0.4434608896569423, + "timestamp": 0.19722470228310662 + }, + { + "x": 2.5668563689761195, + "y": 2.952009793968192, + "heading": -0.7046902372675247, + "angularVelocity": 0.581450892781281, + "velocityX": 1.2348013176576613, + "velocityY": -0.5914184735371368, + "timestamp": 0.26296626971080883 + }, + { + "x": 2.668357540158173, + "y": 2.903395952391573, + "heading": -0.6582160423239786, + "angularVelocity": 0.706922526522589, + "velocityX": 1.5439420621310505, + "velocityY": -0.7394688547710901, + "timestamp": 0.32870783713851104 + }, + { + "x": 2.790199449693154, + "y": 2.8450417291439147, + "heading": -0.604379415056612, + "angularVelocity": 0.8189130465526614, + "velocityX": 1.8533465857651596, + "velocityY": -0.8876305438234711, + "timestamp": 0.39444940456621325 + }, + { + "x": 2.9324037360004773, + "y": 2.7769385796688133, + "heading": -0.5444410242237507, + "angularVelocity": 0.9117274378767651, + "velocityX": 2.1630802530485744, + "velocityY": -1.0359222047754857, + "timestamp": 0.46019097199391545 + }, + { + "x": 3.094996415116772, + "y": 2.6990776165764148, + "heading": -0.4803328988113282, + "angularVelocity": 0.975153588829843, + "velocityX": 2.473209652250889, + "velocityY": -1.1843490524929314, + "timestamp": 0.5259325394216177 + }, + { + "x": 3.2780012401054357, + "y": 2.6114549065561117, + "heading": -0.4154109268291881, + "angularVelocity": 0.9875330711202905, + "velocityX": 2.783700361113535, + "velocityY": -1.3328357300982285, + "timestamp": 0.5916741068493199 + }, + { + "x": 3.48136962382921, + "y": 2.514109658078373, + "heading": -0.3571766392011983, + "angularVelocity": 0.8858061939583661, + "velocityX": 3.0934520073228393, + "velocityY": -1.4807260046665736, + "timestamp": 0.6574156742770221 + }, + { + "x": 3.702896130222067, + "y": 2.408427187890042, + "heading": -0.3455558213194451, + "angularVelocity": 0.17676514778162136, + "velocityX": 3.3696565971974572, + "velocityY": -1.6075441204615863, + "timestamp": 0.7231572417047243 + }, + { + "x": 3.927079400169571, + "y": 2.300911734169414, + "heading": -0.34555579773751244, + "angularVelocity": 3.5870657763866696e-7, + "velocityX": 3.4100688304100117, + "velocityY": -1.6354257728775006, + "timestamp": 0.7888988091324265 + }, + { + "x": 4.15126266204063, + "y": 2.1933962636083315, + "heading": -0.3455557741556376, + "angularVelocity": 3.587056984423895e-7, + "velocityX": 3.4100687075585983, + "velocityY": -1.6354260290389449, + "timestamp": 0.8546403765601287 + }, + { + "x": 4.375445923911402, + "y": 2.0858807930466483, + "heading": -0.3455557505737627, + "angularVelocity": 3.5870569914452407e-7, + "velocityX": 3.4100687075542147, + "velocityY": -1.6354260290480847, + "timestamp": 0.9203819439878309 + }, + { + "x": 4.599629185782173, + "y": 1.978365322484965, + "heading": -0.34555572699188775, + "angularVelocity": 3.5870569932883336e-7, + "velocityX": 3.4100687075542147, + "velocityY": -1.6354260290480853, + "timestamp": 0.9861235114155331 + }, + { + "x": 4.823812447652946, + "y": 1.8708498519232832, + "heading": -0.3455557034100129, + "angularVelocity": 3.5870569853758494e-7, + "velocityX": 3.4100687075542258, + "velocityY": -1.6354260290480622, + "timestamp": 1.0518650788432353 + }, + { + "x": 5.047995709543858, + "y": 1.7633343814035949, + "heading": -0.34555567982813806, + "angularVelocity": 3.5870569820699746e-7, + "velocityX": 3.410068707860571, + "velocityY": -1.6354260284092945, + "timestamp": 1.1176066462709375 + }, + { + "x": 5.27217953587615, + "y": 1.65582008782303, + "heading": -0.34555565624625534, + "angularVelocity": 3.587058178727361e-7, + "velocityX": 3.410077293621499, + "velocityY": -1.6354081258984543, + "timestamp": 1.1833482136986397 }, { "x": 5.5030035972595215, "y": 1.563418984413147, "heading": -0.34555563246426124, - "angularVelocity": 2.6730250915052296e-7, - "velocityX": 3.52891551864971, - "velocityY": -1.360125725304771, - "timestamp": 1.322027736676021 - }, - { - "x": 5.655413318775525, - "y": 1.5130951799644636, - "heading": -0.34555563491945995, - "angularVelocity": -5.785220393859716e-8, - "velocityX": 3.5912523855702734, - "velocityY": -1.18578710714545, - "timestamp": 1.3644668930350639 - }, - { - "x": 5.810103118724588, - "y": 1.4702901058122384, - "heading": -0.3455556373457815, - "angularVelocity": -5.7171766263130695e-8, - "velocityX": 3.6449782045702985, - "velocityY": -1.0086221740622332, - "timestamp": 1.4069060493941077 - }, - { - "x": 5.966313456902953, - "y": 1.4334181808859694, - "heading": -0.34555563976573417, - "angularVelocity": -5.702169541040829e-8, - "velocityX": 3.680806867525754, - "velocityY": -0.8688185178405917, - "timestamp": 1.4493452057531506 - }, - { - "x": 6.1225244823663525, - "y": 1.3965491678093302, - "heading": -0.3455556421856862, - "angularVelocity": -5.7021679533312126e-8, - "velocityX": 3.680823062122731, - "velocityY": -0.868749905505203, - "timestamp": 1.4917843621121936 - }, - { - "x": 6.278735507957822, - "y": 1.3596801552753135, - "heading": -0.3455556446056381, - "angularVelocity": -5.702167859922657e-8, - "velocityX": 3.680823065140465, - "velocityY": -0.8687498927193055, - "timestamp": 1.5342235184712365 - }, - { - "x": 6.434946533537413, - "y": 1.3228111426909752, - "heading": -0.34555564702559005, - "angularVelocity": -5.7021678932866206e-8, - "velocityX": 3.680823064860606, - "velocityY": -0.868749893905046, - "timestamp": 1.5766626748302794 - }, - { - "x": 6.591157495254782, - "y": 1.285941859528607, - "heading": -0.3455556494455419, - "angularVelocity": -5.7021677677252914e-8, - "velocityX": 3.6808215600657594, - "velocityY": -0.8687562695744264, - "timestamp": 1.6191018311893224 - }, - { - "x": 6.747031505398479, - "y": 1.2476730535838276, - "heading": -0.34555565194208476, - "angularVelocity": -5.8826401859372655e-8, - "velocityX": 3.6728819212374164, - "velocityY": -0.9017334279932018, - "timestamp": 1.6615409875483653 - }, - { - "x": 6.897830261276489, - "y": 1.2034176695315386, - "heading": -0.3468060133183351, - "angularVelocity": -0.029462446559305244, - "velocityX": 3.5532929684610997, - "velocityY": -1.0427960367044076, - "timestamp": 1.7039801439074083 - }, - { - "x": 7.040966542913821, - "y": 1.160898981796106, - "heading": -0.34756195184747823, - "angularVelocity": -0.017812289262957588, - "velocityX": 3.3727409759603195, - "velocityY": -1.001874009363348, - "timestamp": 1.7464193002664512 - }, - { - "x": 7.176448137174031, - "y": 1.12015095875213, - "heading": -0.34781665101790943, - "angularVelocity": -0.006001513514462224, - "velocityX": 3.1923724664555144, - "velocityY": -0.9601515802821338, - "timestamp": 1.7888584566254941 - }, - { - "x": 7.30427768133993, - "y": 1.0811849503221362, - "heading": -0.3475678387477372, - "angularVelocity": 0.005862799629361019, - "velocityX": 3.0120660996283153, - "velocityY": -0.918161711329369, - "timestamp": 1.831297612984537 - }, - { - "x": 7.424456501916377, - "y": 1.044006636667021, - "heading": -0.3468143875407301, - "angularVelocity": 0.017753680130510166, - "velocityX": 2.8317909894275823, - "velocityY": -0.8760379999211017, - "timestamp": 1.87373676934358 + "angularVelocity": 3.617497277100986e-7, + "velocityX": 3.511082415812739, + "velocityY": -1.405520236667608, + "timestamp": 1.249089781126342 + }, + { + "x": 5.6599765905642645, + "y": 1.5113593672660246, + "heading": -0.34555563704903824, + "angularVelocity": -1.0484559207434358e-7, + "velocityX": 3.589689630249378, + "velocityY": -1.1905096787253004, + "timestamp": 1.2928186294317703 + }, + { + "x": 5.819811381479902, + "y": 1.4688909784273936, + "heading": -0.3455556415836398, + "angularVelocity": -1.0369817036745698e-7, + "velocityX": 3.6551337871799947, + "velocityY": -0.9711755622285476, + "timestamp": 1.3365474777371986 + }, + { + "x": 5.98037051193366, + "y": 1.4292489543347602, + "heading": -0.34555564611341977, + "angularVelocity": -1.0358790889899858e-7, + "velocityX": 3.6716981277969567, + "velocityY": -0.9065416910994355, + "timestamp": 1.380276326042627 + }, + { + "x": 6.140929731813359, + "y": 1.389607292439096, + "heading": -0.3455556506431997, + "angularVelocity": -1.0358790900922708e-7, + "velocityX": 3.6717001728071876, + "velocityY": -0.9065334083071072, + "timestamp": 1.4240051743480553 + }, + { + "x": 6.30148895170346, + "y": 1.3499656305855623, + "heading": -0.34555565517297965, + "angularVelocity": -1.0358790856487398e-7, + "velocityX": 3.6717001730450596, + "velocityY": -0.9065334073436624, + "timestamp": 1.4677340226534836 + }, + { + "x": 6.462048171593562, + "y": 1.3103239687320334, + "heading": -0.34555565970275964, + "angularVelocity": -1.0358791005302326e-7, + "velocityX": 3.671700173045087, + "velocityY": -0.9065334073435504, + "timestamp": 1.511462870958912 + }, + { + "x": 6.622607391483664, + "y": 1.2706823068785016, + "heading": -0.34555566423253964, + "angularVelocity": -1.0358790893463982e-7, + "velocityX": 3.6717001730450707, + "velocityY": -0.9065334073436169, + "timestamp": 1.5551917192643403 + }, + { + "x": 6.783166611367605, + "y": 1.2310406450000155, + "heading": -0.3455556687623196, + "angularVelocity": -1.0358790885289903e-7, + "velocityX": 3.6717001729041776, + "velocityY": -0.9065334079142718, + "timestamp": 1.5989205675697686 + }, + { + "x": 6.943725778284007, + "y": 1.1913987685896004, + "heading": -0.34555567329209946, + "angularVelocity": -1.0358790829702175e-7, + "velocityX": 3.6716989616319724, + "velocityY": -0.9065383138730976, + "timestamp": 1.642649415875197 + }, + { + "x": 7.103836002524736, + "y": 1.1499808922348331, + "heading": -0.34555567791225333, + "angularVelocity": -1.0565459632350647e-7, + "velocityX": 3.661432451237353, + "velocityY": -0.9471522338178334, + "timestamp": 1.6863782641806253 + }, + { + "x": 7.257955684496521, + "y": 1.1005509138662795, + "heading": -0.346747423550466, + "angularVelocity": -0.02725307627333064, + "velocityX": 3.524439539210384, + "velocityY": -1.1303745761449258, + "timestamp": 1.7301071124860536 + }, + { + "x": 7.402334567868656, + "y": 1.0534126604700325, + "heading": -0.34675046354447403, + "angularVelocity": -0.00006951918758018307, + "velocityX": 3.3016850195483918, + "velocityY": -1.07796695369166, + "timestamp": 1.773835960791482 }, { "x": 7.536985397338867, "y": 1.0086194276809692, "heading": -0.34555563246426124, - "angularVelocity": 0.02966022853563592, - "velocityX": 2.65153469287831, - "velocityY": -0.8338339406813241, - "timestamp": 1.916175925702623 - }, - { - "x": 7.689789531297121, - "y": 0.9591642629851495, - "heading": -0.34249163023542684, - "angularVelocity": 0.04769743470803589, - "velocityX": 2.378707539440784, - "velocityY": -0.769870356769085, - "timestamp": 1.98041422798428 - }, - { - "x": 7.82516177952376, - "y": 0.9142093996063804, - "heading": -0.3392101704632614, - "angularVelocity": 0.05108260423473965, - "velocityX": 2.1073447369933547, - "velocityY": -0.6998140016475143, - "timestamp": 2.044652530265937 - }, - { - "x": 7.943169958712402, - "y": 0.8740090692537413, - "heading": -0.33632582750820933, - "angularVelocity": 0.044900672225200826, - "velocityX": 1.837037639494695, - "velocityY": -0.6258000122166694, - "timestamp": 2.108890832547594 - }, - { - "x": 8.043864833075729, - "y": 0.8387415563520763, - "heading": -0.3342715201633078, - "angularVelocity": 0.03197947753809357, - "velocityX": 1.567520790350633, - "velocityY": -0.5490106626266708, - "timestamp": 2.173129134829251 - }, - { - "x": 8.12728568078066, - "y": 0.8085387560486261, - "heading": -0.33336873570137193, - "angularVelocity": 0.014053678722353937, - "velocityX": 1.2986153858669556, - "velocityY": -0.4701680964578417, - "timestamp": 2.2373674371109082 - }, - { - "x": 8.193463737140812, - "y": 0.7835021726798679, - "heading": -0.33386568134146205, - "angularVelocity": -0.007735970946293819, - "velocityX": 1.030196222652151, - "velocityY": -0.38974540857234485, - "timestamp": 2.3016057393925653 - }, - { - "x": 8.242424408463135, - "y": 0.7637123264900073, - "heading": -0.33595980135953535, - "angularVelocity": -0.03259924287680334, - "velocityX": 0.7621725603464993, - "velocityY": -0.3080692591016908, - "timestamp": 2.3658440416742224 - }, - { - "x": 8.274188750584008, - "y": 0.7492346436854774, - "heading": -0.3398119288829893, - "angularVelocity": -0.05996620998114242, - "velocityX": 0.4944766750154696, - "velocityY": -0.2253746174836856, - "timestamp": 2.4300823439558794 - }, - { - "x": 8.288774490356445, - "y": 0.7401233315467834, + "angularVelocity": 0.027323634774631837, + "velocityX": 3.079221948168643, + "velocityY": -1.024340556060407, + "timestamp": 1.8175648090969103 + }, + { + "x": 7.709533523799992, + "y": 0.9494390432841444, + "heading": -0.3413965784412158, + "angularVelocity": 0.06656216442856697, + "velocityX": 2.7614877570011993, + "velocityY": -0.9471323179117597, + "timestamp": 1.880048564827372 + }, + { + "x": 7.862380855125707, + "y": 0.8957055369328039, + "heading": -0.33682515317576994, + "angularVelocity": 0.07316181961221624, + "velocityX": 2.4461930871290236, + "velocityY": -0.8599596122731851, + "timestamp": 1.9425323205578335 + }, + { + "x": 7.99561550345175, + "y": 0.8477270543434197, + "heading": -0.33285887044966567, + "angularVelocity": 0.06347702182330028, + "velocityX": 2.1323085779411386, + "velocityY": -0.7678552934037809, + "timestamp": 2.005016076288295 + }, + { + "x": 8.109294083490143, + "y": 0.8056872786590918, + "heading": -0.33010810532148027, + "angularVelocity": 0.04402368417243478, + "velocityX": 1.8193301396409471, + "velocityY": -0.6728112801937814, + "timestamp": 2.0674998320187568 + }, + { + "x": 8.20345588526635, + "y": 0.7697081195865146, + "heading": -0.32897987990878186, + "angularVelocity": 0.018056299585531694, + "velocityX": 1.5069805051795295, + "velocityY": -0.5758162045793389, + "timestamp": 2.1299835877492184 + }, + { + "x": 8.278129725528439, + "y": 0.7398763726381793, + "heading": -0.32976503867911666, + "angularVelocity": -0.012565806282865027, + "velocityX": 1.1950920585537632, + "velocityY": -0.4774320397291953, + "timestamp": 2.19246734347968 + }, + { + "x": 8.333337625588607, + "y": 0.7162569714576368, + "heading": -0.33268181216540116, + "angularVelocity": -0.04668050843272986, + "velocityX": 0.883556044523325, + "velocityY": -0.37800866648333437, + "timestamp": 2.2549510992101416 + }, + { + "x": 8.369096953381794, + "y": 0.698900317224484, + "heading": -0.33790002427933163, + "angularVelocity": -0.08351309957167831, + "velocityX": 0.572297989695798, + "velocityY": -0.2777786647144699, + "timestamp": 2.317434854940603 + }, + { + "x": 8.385421752929688, + "y": 0.6878466606140137, "heading": -0.34555563246426124, - "angularVelocity": -0.08941244362418393, - "velocityX": 0.2270567442533848, - "velocityY": -0.14183612914838375, - "timestamp": 2.4943206462375365 - }, - { - "x": 8.286058974983396, - "y": 0.7364361738247144, - "heading": -0.3533713967772531, - "angularVelocity": -0.1208300500196775, - "velocityX": -0.04198128874091145, - "velocityY": -0.05700267231030528, - "timestamp": 2.559004591263786 - }, - { - "x": 8.26594084231072, - "y": 0.7382366486349878, - "heading": -0.3631834148956511, - "angularVelocity": -0.15169170826572306, - "velocityX": -0.3110220420927156, - "velocityY": 0.027834956719829915, - "timestamp": 2.623688536290036 - }, - { - "x": 8.22841989282902, - "y": 0.7455250700569162, - "heading": -0.3749497746847549, - "angularVelocity": -0.18190541384463682, - "velocityX": -0.5800658798172126, - "velocityY": 0.11267744134917045, - "timestamp": 2.6883724813162857 - }, - { - "x": 8.173495898954863, - "y": 0.7583018091084519, - "heading": -0.38862097650887273, - "angularVelocity": -0.21135386560868902, - "velocityX": -0.8491132359330754, - "velocityY": 0.1975256618369646, - "timestamp": 2.7530564263425354 - }, - { - "x": 8.101168599327213, - "y": 0.7765673113174031, - "heading": -0.4041376276128682, - "angularVelocity": -0.23988411804039605, - "velocityX": -1.11816463263485, - "velocityY": 0.2823807700896826, - "timestamp": 2.817740371368785 - }, - { - "x": 8.011437691690773, - "y": 0.8003221216608263, - "heading": -0.42142708913788907, - "angularVelocity": -0.26729138920027884, - "velocityX": -1.3872207021390834, - "velocityY": 0.36724430357151194, - "timestamp": 2.882424316395035 - }, - { - "x": 7.904302824322884, - "y": 0.8295669211666158, - "heading": -0.44039841224325665, - "angularVelocity": -0.29329261067284323, - "velocityX": -1.6562822092006166, - "velocityY": 0.45211836559939883, - "timestamp": 2.9471082614212847 - }, - { - "x": 7.779763586783285, - "y": 0.8643025828479174, - "heading": -0.46093432615418145, - "angularVelocity": -0.317480850968364, - "velocityX": -1.9253500615811048, - "velocityY": 0.5370059242243979, - "timestamp": 3.0117922064475344 - }, - { - "x": 7.63781950374517, - "y": 0.9045302615405436, - "heading": -0.48287779045070706, - "angularVelocity": -0.33924127985113656, - "velocityX": -2.194425262412658, - "velocityY": 0.6219113363648586, - "timestamp": 3.076476151473784 - }, - { - "x": 7.478470046612931, - "y": 0.9502515474489494, - "heading": -0.5060075980261466, - "angularVelocity": -0.3575818940241346, - "velocityX": -2.4635086352196294, - "velocityY": 0.7068413327271726, - "timestamp": 3.141160096500034 - }, - { - "x": 7.301714722310279, - "y": 1.0014687500113455, - "heading": -0.5299890975728885, - "angularVelocity": -0.37074887032647375, - "velocityX": -2.732599630880914, - "velocityY": 0.7918070325118697, - "timestamp": 3.2058440415262837 - }, - { - "x": 7.107553521342319, - "y": 1.0581854755780744, - "heading": -0.5542575516052335, - "angularVelocity": -0.37518512549746846, - "velocityX": -3.0016907733312475, - "velocityY": 0.8768284857040194, - "timestamp": 3.2705279865525334 - }, - { - "x": 6.895989597473325, - "y": 1.1204078946209601, - "heading": -0.5776591781660143, - "angularVelocity": -0.3617841575878507, - "velocityX": -3.2707331592582385, - "velocityY": 0.961945333075069, - "timestamp": 3.335211931578783 - }, - { - "x": 6.667061067423103, - "y": 1.188145012147935, - "heading": -0.5965312726664429, - "angularVelocity": -0.2917585575952417, - "velocityX": -3.5391862688232427, - "velocityY": 1.0472013959489617, - "timestamp": 3.399895876605033 - }, - { - "x": 6.433742027972299, - "y": 1.2616769248691375, - "heading": -0.5965312781733252, - "angularVelocity": -8.513522502240808e-8, - "velocityX": -3.607062608134345, - "velocityY": 1.136787694247182, - "timestamp": 3.4645798216312826 - }, - { - "x": 6.200423078644191, - "y": 1.3352091235520722, - "heading": -0.5965312836800714, - "angularVelocity": -8.513312137357674e-8, - "velocityX": -3.6070612148566714, - "velocityY": 1.1367921151546025, - "timestamp": 3.5292637666575324 - }, - { - "x": 5.967104129330135, - "y": 1.4087413222795935, - "heading": -0.5965312891868175, - "angularVelocity": -8.513312220445073e-8, - "velocityX": -3.607061214639434, - "velocityY": 1.136792115843899, - "timestamp": 3.593947711683782 - }, - { - "x": 5.733785345709214, - "y": 1.4822740467522528, - "heading": -0.5965312946935636, - "angularVelocity": -8.513311865132804e-8, - "velocityX": -3.6070586530588855, - "velocityY": 1.1368002437516491, - "timestamp": 3.658631656710032 + "angularVelocity": -0.12252157533477633, + "velocityX": 0.26126469763300175, + "velocityY": -0.1769044847136382, + "timestamp": 2.379918610671065 + }, + { + "x": 8.379560991493735, + "y": 0.6833892008568203, + "heading": -0.35721868503670856, + "angularVelocity": -0.1678763417451177, + "velocityX": -0.08435897751441682, + "velocityY": -0.06416005011256522, + "timestamp": 2.449392679805565 + }, + { + "x": 8.349687812290387, + "y": 0.6867654591427064, + "heading": -0.37193021085772177, + "angularVelocity": -0.2117556378126066, + "velocityX": -0.42999034856465757, + "velocityY": 0.048597387887987537, + "timestamp": 2.5188667489400656 + }, + { + "x": 8.295801597591089, + "y": 0.6979765367761911, + "heading": -0.38956490915625874, + "angularVelocity": -0.253831372168465, + "velocityX": -0.7756306111129838, + "velocityY": 0.1613706779112114, + "timestamp": 2.588340818074566 + }, + { + "x": 8.217901630671626, + "y": 0.7170238074049998, + "heading": -0.40996601054380316, + "angularVelocity": -0.2936505899495876, + "velocityX": -1.121281190089058, + "velocityY": 0.2741637400269978, + "timestamp": 2.6578148872090663 + }, + { + "x": 8.115987079646073, + "y": 0.7439090305870278, + "heading": -0.4329315185089462, + "angularVelocity": -0.3305622982969707, + "velocityX": -1.4669437431144206, + "velocityY": 0.3869821289721611, + "timestamp": 2.7272889563435667 + }, + { + "x": 7.990056990056933, + "y": 0.778634533730576, + "heading": -0.45819105339643357, + "angularVelocity": -0.36358219983610734, + "velocityX": -1.812620034466978, + "velocityY": 0.4998340183057432, + "timestamp": 2.796763025478067 + }, + { + "x": 7.840110319883288, + "y": 0.8212035218059903, + "heading": -0.4853637226253089, + "angularVelocity": -0.3911195870256229, + "velocityX": -2.1583113245224226, + "velocityY": 0.6127320395326444, + "timestamp": 2.8662370946125675 + }, + { + "x": 7.6661461501938755, + "y": 0.8716206459377261, + "heading": -0.5138727598830971, + "angularVelocity": -0.4103550808661484, + "velocityX": -2.504015841545443, + "velocityY": 0.7256970083921437, + "timestamp": 2.935711163747068 + }, + { + "x": 7.468164698581641, + "y": 0.9298931437973682, + "heading": -0.5427426487462225, + "angularVelocity": -0.41554912822558204, + "velocityX": -2.8497172265661783, + "velocityY": 0.8387661552805812, + "timestamp": 3.0051852328815682 + }, + { + "x": 7.24617329762414, + "y": 0.9960332266921984, + "heading": -0.5699703376985038, + "angularVelocity": -0.391911533201964, + "velocityX": -3.1953130674947268, + "velocityY": 0.9520110700121011, + "timestamp": 3.0746593020160686 + }, + { + "x": 7.00025853984304, + "y": 1.0700564595014088, + "heading": -0.589089272180988, + "angularVelocity": -0.2751952594783323, + "velocityX": -3.53966250782022, + "velocityY": 1.065480023430083, + "timestamp": 3.144133371150569 + }, + { + "x": 6.750442302486931, + "y": 1.1514706650489683, + "heading": -0.5890892788822688, + "angularVelocity": -9.645729514768673e-8, + "velocityX": -3.5958198572257243, + "velocityY": 1.171864647656428, + "timestamp": 3.2136074402850694 + }, + { + "x": 6.500626129709104, + "y": 1.232885068752249, + "heading": -0.5890892855834843, + "angularVelocity": -9.645635490096592e-8, + "velocityX": -3.5958189276949932, + "velocityY": 1.1718674998820673, + "timestamp": 3.2830815094195698 + }, + { + "x": 6.250809956933241, + "y": 1.3142994724615544, + "heading": -0.5890892922846998, + "angularVelocity": -9.645635424551899e-8, + "velocityX": -3.595818927666732, + "velocityY": 1.1718674999687875, + "timestamp": 3.35255557855407 + }, + { + "x": 6.000993784158921, + "y": 1.395713876175591, + "heading": -0.589089298985915, + "angularVelocity": -9.645635348778811e-8, + "velocityX": -3.595818927644538, + "velocityY": 1.1718675000368866, + "timestamp": 3.4220296476885705 + }, + { + "x": 5.751177662095486, + "y": 1.477128435493372, + "heading": -0.5890893056871302, + "angularVelocity": -9.64563521650664e-8, + "velocityX": -3.5958181977191597, + "velocityY": 1.1718697397753421, + "timestamp": 3.491503716823071 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.5965313002103377, - "angularVelocity": -8.528815258110653e-8, - "velocityX": -3.567836630187545, - "velocityY": 1.254483436778072, - "timestamp": 3.7233156017362816 - }, - { - "x": 5.285431093647359, - "y": 1.6581112689784743, - "heading": -0.5965313049795081, - "angularVelocity": -7.601302145092289e-8, - "velocityX": -3.4677610362467894, - "velocityY": 1.509245007513348, - "timestamp": 3.7860570943991227 - }, - { - "x": 5.070948144949037, - "y": 1.7596075143791599, - "heading": -0.5965313097283005, - "angularVelocity": -7.568822719486902e-8, - "velocityX": -3.4185184252932688, - "velocityY": 1.6176893646140194, - "timestamp": 3.848798587061964 - }, - { - "x": 4.856465419222307, - "y": 1.861104230964457, - "heading": -0.5965313144770923, - "angularVelocity": -7.568821760685447e-8, - "velocityX": -3.4185148714793154, - "velocityY": 1.6176968745502942, - "timestamp": 3.911540079724805 - }, - { - "x": 4.641982693508075, - "y": 1.9626009475761665, - "heading": -0.5965313192258841, - "angularVelocity": -7.568821790279648e-8, - "velocityX": -3.418514871280104, - "velocityY": 1.6176968749712675, - "timestamp": 3.974281572387646 - }, - { - "x": 4.427499967793844, - "y": 2.064097664187878, - "heading": -0.5965313239746759, - "angularVelocity": -7.568821822441872e-8, - "velocityX": -3.4185148712800926, - "velocityY": 1.617696874971291, - "timestamp": 4.037023065050487 - }, - { - "x": 4.213017242079613, - "y": 2.165594380799589, - "heading": -0.5965313287234677, - "angularVelocity": -7.568821811615467e-8, - "velocityX": -3.4185148712800917, - "velocityY": 1.617696874971293, - "timestamp": 4.099764557713328 - }, - { - "x": 3.998534516366738, - "y": 2.267091097414165, - "heading": -0.5965313334722594, - "angularVelocity": -7.56882168459371e-8, - "velocityX": -3.418514871258486, - "velocityY": 1.6176968750169503, - "timestamp": 4.1625060503761695 - }, - { - "x": 3.7840518148370346, - "y": 2.3685878651306878, - "heading": -0.5965313382224369, - "angularVelocity": -7.571030356184493e-8, - "velocityX": -3.418514485817036, - "velocityY": 1.6176976895010247, - "timestamp": 4.225247543039011 - }, - { - "x": 3.5823467745214304, - "y": 2.46403001529431, - "heading": -0.6258886394381601, - "angularVelocity": -0.46790887449049295, - "velocityX": -3.214858808022383, - "velocityY": 1.5211966772373215, - "timestamp": 4.287989035701852 - }, - { - "x": 3.3961574491843427, - "y": 2.552130310684733, - "heading": -0.6529986278149837, - "angularVelocity": -0.43209026795882993, - "velocityX": -2.967562890759225, - "velocityY": 1.40417914288166, - "timestamp": 4.350730528364693 - }, - { - "x": 3.225483861981861, - "y": 2.6328887858865286, - "heading": -0.6778585687680168, - "angularVelocity": -0.3962280764760425, - "velocityX": -2.7202666044246855, - "velocityY": 1.2871621597492864, - "timestamp": 4.413472021027534 - }, - { - "x": 3.0703260256030673, - "y": 2.7063054766361296, - "heading": -0.7004659993691003, - "angularVelocity": -0.36032662982009556, - "velocityX": -2.4729701158463, - "velocityY": 1.17014574619904, - "timestamp": 4.476213513690375 - }, - { - "x": 2.9306839491479293, - "y": 2.7723804156875507, - "heading": -0.7208186227896802, - "angularVelocity": -0.32438857535555815, - "velocityX": -2.2256734822288196, - "velocityY": 1.0531298546958971, - "timestamp": 4.538955006353216 - }, - { - "x": 2.8065576397395007, - "y": 2.8311136320269066, - "heading": -0.7389143343573537, - "angularVelocity": -0.28841697574707026, - "velocityX": -1.9783767350811556, - "velocityY": 0.9361144251855119, - "timestamp": 4.601696499016057 - }, - { - "x": 2.697947103240289, - "y": 2.8825051506567756, - "heading": -0.7547512635952348, - "angularVelocity": -0.25241556370017226, - "velocityX": -1.7310798944944172, - "velocityY": 0.8190993941766099, - "timestamp": 4.6644379916788985 - }, - { - "x": 2.6048523446905505, - "y": 2.9265549925489487, - "heading": -0.7683278153992709, - "angularVelocity": -0.21638872822158642, - "velocityX": -1.4837829735739574, - "velocityY": 0.702084697424841, - "timestamp": 4.72717948434174 - }, - { - "x": 2.527273368620795, - "y": 2.9632631746604594, - "heading": -0.779642706302846, - "angularVelocity": -0.18034143631837077, - "velocityX": -1.2364859804443729, - "velocityY": 0.5850702709413225, - "timestamp": 4.789920977004581 - }, - { - "x": 2.4652101792880994, - "y": 2.9926297099741825, - "heading": -0.7886949946743065, - "angularVelocity": -0.1442791362982944, - "velocityX": -0.9891889194637066, - "velocityY": 0.46805605138424683, - "timestamp": 4.852662469667422 - }, - { - "x": 2.4186627808554446, - "y": 3.014654607545873, - "heading": -0.7954841045559687, - "angularVelocity": -0.10820765642515946, - "velocityX": -0.7418917921317392, - "velocityY": 0.3510419761615724, - "timestamp": 4.915403962330263 - }, - { - "x": 2.3876311775224326, - "y": 3.0293378725476474, - "heading": -0.8000098431420335, - "angularVelocity": -0.07213310353301693, - "velocityX": -0.49459459786475296, - "velocityY": 0.23402798337424993, - "timestamp": 4.978145454993104 + "heading": -0.5890893123960679, + "angularVelocity": -9.65675052838285e-8, + "velocityX": -3.5721826564600034, + "velocityY": 1.242054049730676, + "timestamp": 3.5609777859575713 + }, + { + "x": 5.287967104202961, + "y": 1.6596756394253018, + "heading": -0.5890893181956361, + "angularVelocity": -9.309834476919773e-8, + "velocityX": -3.4519020760085812, + "velocityY": 1.5451728334255468, + "timestamp": 3.6232728593175922 + }, + { + "x": 5.074932733784246, + "y": 1.7602857483579184, + "heading": -0.5890893239791856, + "angularVelocity": -9.284120212486628e-8, + "velocityX": -3.419762734486735, + "velocityY": 1.6150572349624415, + "timestamp": 3.685567932677613 + }, + { + "x": 4.861898448797989, + "y": 1.860896038187167, + "heading": -0.5890893297627348, + "angularVelocity": -9.284119839047264e-8, + "velocityX": -3.4197613630707817, + "velocityY": 1.615060138829829, + "timestamp": 3.747863006037634 + }, + { + "x": 4.648864163815087, + "y": 1.9615063280235205, + "heading": -0.5890893355462841, + "angularVelocity": -9.28411976539737e-8, + "velocityX": -3.419761363016917, + "velocityY": 1.6150601389438841, + "timestamp": 3.810158079397655 + }, + { + "x": 4.435829878832186, + "y": 2.0621166178598744, + "heading": -0.5890893413298333, + "angularVelocity": -9.28411982771023e-8, + "velocityX": -3.419761363016914, + "velocityY": 1.6150601389438883, + "timestamp": 3.872453152757676 + }, + { + "x": 4.222795593849285, + "y": 2.162726907696228, + "heading": -0.5890893471133825, + "angularVelocity": -9.284119843074624e-8, + "velocityX": -3.4197613630169146, + "velocityY": 1.6150601389438883, + "timestamp": 3.934748226117697 + }, + { + "x": 4.009761308866383, + "y": 2.2633371975325822, + "heading": -0.5890893528969318, + "angularVelocity": -9.284119832193768e-8, + "velocityX": -3.419761363016914, + "velocityY": 1.6150601389438886, + "timestamp": 3.997043299477718 + }, + { + "x": 3.7967270238835944, + "y": 2.363947487369175, + "heading": -0.589089358680481, + "angularVelocity": -9.284119741513914e-8, + "velocityX": -3.4197613630151045, + "velocityY": 1.615060138947721, + "timestamp": 4.059338372837739 + }, + { + "x": 3.583692741771847, + "y": 2.464557783284562, + "heading": -0.5890893644643718, + "angularVelocity": -9.284668184194445e-8, + "velocityX": -3.419761316927328, + "velocityY": 1.615060236528384, + "timestamp": 4.12163344619776 + }, + { + "x": 3.381763406617466, + "y": 2.559912098840239, + "heading": -0.6245815423960026, + "angularVelocity": -0.5697429349912143, + "velocityX": -3.241497670086629, + "velocityY": 1.530687908570203, + "timestamp": 4.183928519557781 + }, + { + "x": 3.1981911985495857, + "y": 2.646597580557341, + "heading": -0.6568615726262876, + "angularVelocity": -0.5181795042399261, + "velocityX": -2.9468174314037126, + "velocityY": 1.3915302935133123, + "timestamp": 4.246223592917802 + }, + { + "x": 3.032976151851517, + "y": 2.7246143065734305, + "heading": -0.6859252496642307, + "angularVelocity": -0.46654856428174046, + "velocityX": -2.652136642383354, + "velocityY": 1.2523739327700791, + "timestamp": 4.3085186662778225 + }, + { + "x": 2.8861182875087947, + "y": 2.793962348685928, + "heading": -0.7117686923384959, + "angularVelocity": -0.414855321301394, + "velocityX": -2.3574555164898965, + "velocityY": 1.1132187245645664, + "timestamp": 4.3708137396378435 + }, + { + "x": 2.7576176211174075, + "y": 2.854641769350887, + "heading": -0.7343883425051251, + "angularVelocity": -0.3631049607390947, + "velocityX": -2.0627741402397475, + "velocityY": 0.9740645189431933, + "timestamp": 4.433108812997864 + }, + { + "x": 2.6474741650844584, + "y": 2.906652621199817, + "heading": -0.7537810666899133, + "angularVelocity": -0.31130429966287193, + "velocityX": -1.7680925648228973, + "velocityY": 0.8349111581961731, + "timestamp": 4.495403886357885 + }, + { + "x": 2.5556879298075064, + "y": 2.9499949470036255, + "heading": -0.7699442582183806, + "angularVelocity": -0.2594617945957906, + "velocityX": -1.473410822497863, + "velocityY": 0.6957584840348696, + "timestamp": 4.557698959717906 + }, + { + "x": 2.482258924484474, + "y": 2.9846687797700127, + "heading": -0.7828759248842103, + "angularVelocity": -0.20758730937026382, + "velocityX": -1.1787289325219326, + "velocityY": 0.556606339734085, + "timestamp": 4.619994033077927 + }, + { + "x": 2.4271871577101587, + "y": 3.010674142880251, + "heading": -0.7925747589157716, + "angularVelocity": -0.15569183096565328, + "velocityX": -0.8840469045768797, + "velocityY": 0.4174545707643066, + "timestamp": 4.682289106437948 + }, + { + "x": 2.3904726379075623, + "y": 3.0280110502230517, + "heading": -0.7990401886427835, + "angularVelocity": -0.10378717574737188, + "velocityX": -0.5893647414204509, + "velocityY": 0.27830302474492646, + "timestamp": 4.744584179797969 }, { "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": -0.036061762982130895, - "velocityX": -0.24729733470577522, - "velocityY": 0.11701401166345779, - "timestamp": 5.040886947655945 + "angularVelocity": -0.05188569757551244, + "velocityX": -0.29468244125856385, + "velocityY": 0.13915155101802326, + "timestamp": 4.80687925315799 }, { "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": 2.8180807610025885e-36, - "velocityX": -1.7033240054088048e-37, - "velocityY": 8.364918339692578e-38, - "timestamp": 5.103628440318786 + "angularVelocity": -7.343237406242101e-30, + "velocityX": 6.07048254686404e-33, + "velocityY": -1.403576741314563e-31, + "timestamp": 4.869174326518011 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.4.traj b/src/main/deploy/choreo/SourceLanePGHSprint.4.traj index 5cd380c3..f736ac99 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.4.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.4.traj @@ -4,352 +4,334 @@ "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": 2.8180807610025885e-36, - "velocityX": -1.7033240054088048e-37, - "velocityY": 8.364918339692578e-38, + "angularVelocity": -7.343237406242101e-30, + "velocityX": 6.07048254686404e-33, + "velocityY": -1.403576741314563e-31, "timestamp": 0 }, { - "x": 2.3862784269967885, - "y": 3.0299921206795677, - "heading": -0.7979445022798209, - "angularVelocity": 0.07214760590413402, - "velocityX": 0.2361025217555023, - "velocityY": -0.11148080618080956, - "timestamp": 0.059986878920357256 - }, - { - "x": 2.414605712310544, - "y": 3.016617241283919, - "heading": -0.7893555783917804, - "angularVelocity": 0.1431800427464117, - "velocityX": 0.4722246901920792, - "velocityY": -0.22296341527296182, - "timestamp": 0.11997375784071451 - }, - { - "x": 2.457098595113841, - "y": 2.996554734272641, - "heading": -0.7765849369819304, - "angularVelocity": 0.2128905794016223, - "velocityX": 0.7083696229589314, - "velocityY": -0.33444825555792773, - "timestamp": 0.17996063676107177 - }, - { - "x": 2.5137586742709717, - "y": 2.9698044458506936, - "heading": -0.759727315329062, - "angularVelocity": 0.281021816041631, - "velocityX": 0.944541209292721, - "velocityY": -0.44593565965421617, - "timestamp": 0.23994751568142902 - }, - { - "x": 2.5845878442638655, - "y": 2.9363662114763, - "heading": -0.7388971783301493, - "angularVelocity": 0.34724488711220036, - "velocityX": 1.180744377231747, - "velocityY": -0.5574258067133014, - "timestamp": 0.2999343946017863 - }, - { - "x": 2.669588381015972, - "y": 2.8962398712988433, - "heading": -0.7142349782126004, - "angularVelocity": 0.41112657570152883, - "velocityX": 1.4169854855252446, - "velocityY": -0.6689186185320778, - "timestamp": 0.35992127352214354 - }, - { - "x": 2.768763062860025, - "y": 2.849425297465716, - "heading": -0.6859166617272396, - "angularVelocity": 0.4720751770225952, - "velocityX": 1.653272909492815, - "velocityY": -0.7804135616937369, - "timestamp": 0.4199081524425008 - }, - { - "x": 2.8821153449579997, - "y": 2.795922445610523, - "heading": -0.6541688123580982, - "angularVelocity": 0.5292465609236296, - "velocityX": 1.8896179320892514, - "velocityY": -0.8919092444570627, - "timestamp": 0.47989503136285805 - }, - { - "x": 3.0096496153353156, - "y": 2.73573145985777, - "heading": -0.6192942663212868, - "angularVelocity": 0.5813695705541383, - "velocityX": 2.1260361044394105, - "velocityY": -1.0034025246198655, - "timestamp": 0.5398819102832153 - }, - { - "x": 3.151371569227902, - "y": 2.6688529097388005, - "heading": -0.5817190571900679, - "angularVelocity": 0.6263904675071726, - "velocityX": 2.3625492181506123, - "velocityY": -1.1148863105173612, - "timestamp": 0.5998687892035726 - }, - { - "x": 3.30728870131474, - "y": 2.5952884046594, - "heading": -0.5420886244414685, - "angularVelocity": 0.6606516868666429, - "velocityX": 2.5991872705003334, - "velocityY": -1.2263432671179575, - "timestamp": 0.6598556681239298 - }, - { - "x": 3.477410464899007, - "y": 2.5150425607670805, - "heading": -0.501501110267189, - "angularVelocity": 0.6766065330414338, - "velocityX": 2.8359829123654157, - "velocityY": -1.3377232710983202, - "timestamp": 0.7198425470442871 - }, - { - "x": 3.661742704544347, - "y": 2.428132084038069, - "heading": -0.46226256646132613, - "angularVelocity": 0.6541187758402722, - "velocityX": 3.0728759849311573, - "velocityY": -1.4488247812392314, - "timestamp": 0.7798294259646443 - }, - { - "x": 3.860146403975295, - "y": 2.3346891355318427, - "heading": -0.4327535611710109, - "angularVelocity": 0.4919243311440368, - "velocityX": 3.307451612782871, - "velocityY": -1.5577231252568988, - "timestamp": 0.8398163048850016 - }, - { - "x": 4.065242173401202, - "y": 2.237711412138144, - "heading": -0.4327535477418668, - "angularVelocity": 2.238680243649846e-7, - "velocityX": 3.41901050891823, - "velocityY": -1.6166489262168846, - "timestamp": 0.8998031838053588 - }, - { - "x": 4.270337924993822, - "y": 2.140733651028875, - "heading": -0.4327535343131544, - "angularVelocity": 2.2386082909736944e-7, - "velocityX": 3.419010211631728, - "velocityY": -1.6166495549472233, - "timestamp": 0.9597900627257161 - }, - { - "x": 4.475433676585232, - "y": 2.0437558899170503, - "heading": -0.43275352088444197, - "angularVelocity": 2.2386082765717578e-7, - "velocityX": 3.419010211611586, - "velocityY": -1.6166495549898217, - "timestamp": 1.0197769416460734 - }, - { - "x": 4.6805294281766425, - "y": 1.9467781288052255, - "heading": -0.43275350745572955, - "angularVelocity": 2.2386082872977313e-7, - "velocityX": 3.4190102116115844, - "velocityY": -1.616649554989824, - "timestamp": 1.0797638205664306 - }, - { - "x": 4.885625179768054, - "y": 1.8498003676934025, - "heading": -0.4327534940270171, - "angularVelocity": 2.2386082836686737e-7, - "velocityX": 3.419010211611597, - "velocityY": -1.6166495549897981, - "timestamp": 1.1397506994867879 - }, - { - "x": 5.090720931370458, - "y": 1.7528226066048298, - "heading": -0.4327534805983047, - "angularVelocity": 2.2386082909228912e-7, - "velocityX": 3.4190102117948658, - "velocityY": -1.616649554602208, - "timestamp": 1.1997375784071451 - }, - { - "x": 5.295816845235169, - "y": 1.6558451886813434, - "heading": -0.4327534671695918, - "angularVelocity": 2.2386083672590555e-7, - "velocityX": 3.4190129167581613, - "velocityY": -1.6166438339330798, - "timestamp": 1.2597244573275024 + "x": 2.3887780433959134, + "y": 3.0288255570872233, + "heading": -0.7958553483876907, + "angularVelocity": 0.1079808929405059, + "velocityX": 0.2803852472906352, + "velocityY": -0.13215958314273293, + "timestamp": 0.059427769276288345 + }, + { + "x": 2.422106198393558, + "y": 3.0131153644204134, + "heading": -0.7831901230540868, + "angularVelocity": 0.2131196490779077, + "velocityX": 0.5608178702232047, + "velocityY": -0.26435777176444086, + "timestamp": 0.11885553855257669 + }, + { + "x": 2.47210314120033, + "y": 2.9895461805025576, + "heading": -0.7644823111559433, + "angularVelocity": 0.3147991608305137, + "velocityX": 0.8413060664338303, + "velocityY": -0.3966021980108423, + "timestamp": 0.17828330782886503 + }, + { + "x": 2.5387727913774722, + "y": 2.9581147004806385, + "heading": -0.7399860035384287, + "angularVelocity": 0.41220304776557914, + "velocityX": 1.1218602176902186, + "velocityY": -0.5289022355153943, + "timestamp": 0.23771107710515338 + }, + { + "x": 2.6221198536798296, + "y": 2.9188169151709924, + "heading": -0.7100217609714737, + "angularVelocity": 0.5042128104749645, + "velocityX": 1.4024935365630693, + "velocityY": -0.6612697361556471, + "timestamp": 0.2971388463814417 + }, + { + "x": 2.7221500309650937, + "y": 2.871647898493915, + "heading": -0.6750052177423963, + "angularVelocity": 0.5892286326010091, + "velocityX": 1.6832228182788898, + "velocityY": -0.7937201286790226, + "timestamp": 0.35656661565773007 + }, + { + "x": 2.8388702680559588, + "y": 2.816601499667555, + "heading": -0.6354955831852485, + "angularVelocity": 0.6648345552665271, + "velocityX": 1.9640689615693319, + "velocityY": -0.9262740213325141, + "timestamp": 0.4159943849340184 + }, + { + "x": 2.9722889163933544, + "y": 2.753669910841413, + "heading": -0.5922850618953597, + "angularVelocity": 0.7271099322127873, + "velocityX": 2.2450556358107243, + "velocityY": -1.0589592978591336, + "timestamp": 0.47542215421030676 + }, + { + "x": 3.122415239721002, + "y": 2.6828431673634756, + "heading": -0.5465844996458821, + "angularVelocity": 0.7690102254582669, + "velocityX": 2.5261981924591717, + "velocityY": -1.191812251081979, + "timestamp": 0.5348499234865951 + }, + { + "x": 3.289254937095535, + "y": 2.6041094136623544, + "heading": -0.5004858972625192, + "angularVelocity": 0.7757081065791619, + "velocityX": 2.8074366479896216, + "velocityY": -1.3248646997853857, + "timestamp": 0.5942776927628834 + }, + { + "x": 3.4727741175032825, + "y": 2.5174653392644726, + "heading": -0.45856185133391086, + "angularVelocity": 0.7054622180728715, + "velocityX": 3.0881048143423393, + "velocityY": -1.4579728543249981, + "timestamp": 0.6537054620391718 + }, + { + "x": 3.672087548521264, + "y": 2.4232619622496343, + "heading": -0.44156909021440377, + "angularVelocity": 0.2859397437680458, + "velocityX": 3.353877041746403, + "velocityY": -1.5851743749110756, + "timestamp": 0.7131332313154601 + }, + { + "x": 3.87540622002593, + "y": 2.327473100540026, + "heading": -0.4415690736830568, + "angularVelocity": 2.781754584465502e-7, + "velocityX": 3.4212738250264785, + "velocityY": -1.6118535640173974, + "timestamp": 0.7725610005917485 + }, + { + "x": 4.078724897772698, + "y": 2.231684252079618, + "heading": -0.4415690571518241, + "angularVelocity": 2.781735362772021e-7, + "velocityX": 3.4212739300632746, + "velocityY": -1.6118533410711124, + "timestamp": 0.8319887698680368 + }, + { + "x": 4.282043575519764, + "y": 2.135895403619844, + "heading": -0.4415690406205913, + "angularVelocity": 2.7817353604412086e-7, + "velocityX": 3.4212739300683004, + "velocityY": -1.611853341060444, + "timestamp": 0.8914165391443252 + }, + { + "x": 4.485362253266831, + "y": 2.0401065551600697, + "heading": -0.44156902408935866, + "angularVelocity": 2.781735361125895e-7, + "velocityX": 3.421273930068301, + "velocityY": -1.611853341060444, + "timestamp": 0.9508443084206135 + }, + { + "x": 4.688680931013898, + "y": 1.9443177067002955, + "heading": -0.4415690075581259, + "angularVelocity": 2.781735368678374e-7, + "velocityX": 3.421273930068301, + "velocityY": -1.611853341060444, + "timestamp": 1.0102720776969019 + }, + { + "x": 4.8919996087609645, + "y": 1.8485288582405217, + "heading": -0.4415689910268932, + "angularVelocity": 2.7817353639391735e-7, + "velocityX": 3.4212739300683026, + "velocityY": -1.6118533410604396, + "timestamp": 1.0696998469731902 + }, + { + "x": 5.0953182865105076, + "y": 1.7527400097860029, + "heading": -0.4415689744956604, + "angularVelocity": 2.781735367341045e-7, + "velocityX": 3.4212739301099617, + "velocityY": -1.611853340972016, + "timestamp": 1.1291276162494785 + }, + { + "x": 5.298637015997747, + "y": 1.656951271148522, + "heading": -0.4415689579644276, + "angularVelocity": 2.7817353744823303e-7, + "velocityX": 3.4212748007079674, + "velocityY": -1.6118514930642196, + "timestamp": 1.188555385525767 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.4327534537085892, - "angularVelocity": 2.2439911588435075e-7, - "velocityX": 3.4538678416562716, - "velocityY": -1.5407736813730113, - "timestamp": 1.3197113362478596 - }, - { - "x": 5.767637732978587, - "y": 1.4715486447521433, - "heading": -0.43275344189521775, - "angularVelocity": 1.5949042736003556e-7, - "velocityX": 3.5727828839624562, - "velocityY": -1.2403266728714895, - "timestamp": 1.3937808065498363 - }, - { - "x": 6.033774192023134, - "y": 1.384125705645109, - "heading": -0.43275343009903516, - "angularVelocity": 1.5925836231633141e-7, - "velocityX": 3.5930655094403474, - "velocityY": -1.1802830336252825, - "timestamp": 1.467850276851813 - }, - { - "x": 6.2999107025526015, - "y": 1.2967029232707166, - "heading": -0.43275341830285263, - "angularVelocity": 1.5925836222768275e-7, - "velocityX": 3.593066204529984, - "velocityY": -1.1802809176031077, - "timestamp": 1.5419197471537895 - }, - { - "x": 6.566047160708243, - "y": 1.2092799814598572, - "heading": -0.4327534065048956, - "angularVelocity": 1.5928231960994732e-7, - "velocityX": 3.5930654974393894, - "velocityY": -1.1802830701291929, - "timestamp": 1.6159892174557662 - }, - { - "x": 6.815604168443372, - "y": 1.1270295237235333, - "heading": -0.36800659105834677, - "angularVelocity": 0.874136336908857, - "velocityX": 3.369229005117782, - "velocityY": -1.1104501949452927, - "timestamp": 1.6900586877577428 - }, - { - "x": 7.0424486777806585, - "y": 1.0523256982782683, - "heading": -0.30458898646479193, - "angularVelocity": 0.8561908750664163, - "velocityX": 3.0625912189253857, - "velocityY": -1.0085643267152118, - "timestamp": 1.7641281580597195 - }, - { - "x": 7.2465905745815595, - "y": 0.9851213052520007, - "heading": -0.2456798424413219, - "angularVelocity": 0.7953228743678266, - "velocityX": 2.756086900157761, - "velocityY": -0.9073156963628867, - "timestamp": 1.838197628361696 - }, - { - "x": 7.428038548794458, - "y": 0.9253999088853845, - "heading": -0.1923064776337199, - "angularVelocity": 0.7205852099387569, - "velocityX": 2.449699902984963, - "velocityY": -0.8062889625528059, - "timestamp": 1.9122670986636727 - }, - { - "x": 7.586797949690042, - "y": 0.8731533407207944, - "heading": -0.14497999296920738, - "angularVelocity": 0.6389472541327177, - "velocityX": 2.1433851254549703, - "velocityY": -0.7053725097747339, - "timestamp": 1.9863365689656494 - }, - { - "x": 7.722872327084394, - "y": 0.8283767866857153, - "heading": -0.10400779573923787, - "angularVelocity": 0.5531590419497899, - "velocityX": 1.837118273420693, - "velocityY": -0.6045210510150557, - "timestamp": 2.060406039267626 - }, - { - "x": 7.836264194581955, - "y": 0.7910670998170554, - "heading": -0.06959570799723021, - "angularVelocity": 0.46459205934255987, - "velocityX": 1.5308853571555237, - "velocityY": -0.5037120788977025, - "timestamp": 2.1344755095696026 - }, - { - "x": 7.926975414267803, - "y": 0.7612220655404804, - "heading": -0.04189104317473832, - "angularVelocity": 0.3740362218001813, - "velocityX": 1.2246775806013535, - "velocityY": -0.4029330053920843, - "timestamp": 2.2085449798715793 - }, - { - "x": 7.99500740651227, - "y": 0.7388400306235591, - "heading": -0.021003806792755153, - "angularVelocity": 0.28199521741990785, - "velocityX": 0.9184889802384881, - "velocityY": -0.30217625191150127, - "timestamp": 2.282614450173556 - }, - { - "x": 8.040361272813582, - "y": 0.7239196956147146, - "heading": -0.007018297206773477, - "angularVelocity": 0.18881611450660732, - "velocityX": 0.6123152510259342, - "velocityY": -0.20143704211755917, - "timestamp": 2.3566839204755325 + "heading": -0.44156894141194364, + "angularVelocity": 2.7853113329051353e-7, + "velocityX": 3.4389071599112104, + "velocityY": -1.5738818379758603, + "timestamp": 1.2479831548020552 + }, + { + "x": 5.76887189670182, + "y": 1.4734081866468696, + "heading": -0.441568927295876, + "angularVelocity": 1.9019557057481577e-7, + "velocityX": 3.5822280134028937, + "velocityY": -1.2127779127617127, + "timestamp": 1.3222018528993154 + }, + { + "x": 6.03546847764148, + "y": 1.3855778991398175, + "heading": -0.4415689131909676, + "angularVelocity": 1.9004521532739413e-7, + "velocityX": 3.5920406551769672, + "velocityY": -1.1833983855651649, + "timestamp": 1.3964205509965755 + }, + { + "x": 6.302065075021444, + "y": 1.2977476615350296, + "heading": -0.441568899086059, + "angularVelocity": 1.900452165326357e-7, + "velocityX": 3.592040876688587, + "velocityY": -1.1833977131973445, + "timestamp": 1.4706392490938356 + }, + { + "x": 6.568661672398624, + "y": 1.2099174239217916, + "heading": -0.44156888498115054, + "angularVelocity": 1.9004521568233082e-7, + "velocityX": 3.5920408766510765, + "velocityY": -1.1833977133112008, + "timestamp": 1.5448579471910957 + }, + { + "x": 6.835258130689826, + "y": 1.1220867641390964, + "heading": -0.44156887087145064, + "angularVelocity": 1.9010977386064987e-7, + "velocityX": 3.5920390026492015, + "velocityY": -1.1834034014931274, + "timestamp": 1.6190766452883558 + }, + { + "x": 7.080885784636739, + "y": 1.0408339813622511, + "heading": -0.35950242791659653, + "angularVelocity": 1.105738109919598, + "velocityX": 3.3095117570646315, + "velocityY": -1.0947751019611969, + "timestamp": 1.693295343385616 + }, + { + "x": 7.299180381748869, + "y": 0.9686950138524031, + "heading": -0.28235870642192235, + "angularVelocity": 1.03941086912835, + "velocityX": 2.9412345232203254, + "velocityY": -0.9719783472261578, + "timestamp": 1.767514041482876 + }, + { + "x": 7.490166378274822, + "y": 0.9056069788063349, + "heading": -0.2132225753720476, + "angularVelocity": 0.9315190487378978, + "velocityX": 2.5732868053771534, + "velocityY": -0.8500288561164988, + "timestamp": 1.8417327395801362 + }, + { + "x": 7.653856153896351, + "y": 0.8515488890315335, + "heading": -0.15310137838681676, + "angularVelocity": 0.8100545890297152, + "velocityX": 2.2055058875732163, + "velocityY": -0.7283621400089914, + "timestamp": 1.9159514376773963 + }, + { + "x": 7.790256705792751, + "y": 0.8065104939114175, + "heading": -0.10249736767748478, + "angularVelocity": 0.6818229369001992, + "velocityX": 1.8378192476193804, + "velocityY": -0.6068335375688586, + "timestamp": 1.9901701357746564 + }, + { + "x": 7.899372493751606, + "y": 0.7704858089539702, + "heading": -0.06171206576746063, + "angularVelocity": 0.5495286626665922, + "velocityX": 1.470192697477151, + "velocityY": -0.48538556834071367, + "timestamp": 2.0643888338719165 + }, + { + "x": 7.981206582030799, + "y": 0.7434709210905415, + "heading": -0.030946027709333555, + "angularVelocity": 0.4145321710976964, + "velocityX": 1.1026074342068202, + "velocityY": -0.3639903226007307, + "timestamp": 2.1386075319691766 + }, + { + "x": 8.035761170601306, + "y": 0.7254630347979746, + "heading": -0.010340878696429398, + "angularVelocity": 0.2776274650631065, + "velocityX": 0.7350518126714517, + "velocityY": -0.24263274288330566, + "timestamp": 2.2128262300664368 }, { "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": 2.0779165225884802e-41, - "angularVelocity": 0.09475290127174345, - "velocityX": 0.3061531209608937, - "velocityY": -0.10071229126619018, - "timestamp": 2.430753390777509 + "heading": -7.30119772009808e-38, + "angularVelocity": 0.13932983145219902, + "velocityX": 0.3675179222006851, + "velocityY": -0.12130427346551735, + "timestamp": 2.287044928163697 }, { "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": 1.169210048317657e-41, - "angularVelocity": -1.2268302369298258e-40, - "velocityX": -9.944733020002764e-42, - "velocityY": 5.460591898189625e-42, - "timestamp": 2.504822861079486 + "heading": 1.121139660303252e-31, + "angularVelocity": 1.5105915295246559e-30, + "velocityX": -7.247567295016868e-32, + "velocityY": -3.7967728194239397e-31, + "timestamp": 2.361263626260957 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.traj b/src/main/deploy/choreo/SourceLanePGHSprint.traj index 1c871ae2..1540ab71 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.traj @@ -3,2090 +3,1964 @@ { "x": 0.433241993188858, "y": 4.121134281158447, - "heading": -6.349582694240188e-38, + "heading": 8.991694014476798e-34, "angularVelocity": 0, - "velocityX": -2.609211774957871e-36, - "velocityY": -5.016830702674688e-35, + "velocityX": -4.6784915292932193e-35, + "velocityY": -2.119592179507727e-34, "timestamp": 0 }, { - "x": 0.45469188032137553, - "y": 4.109218526997661, - "heading": -0.008937300145995164, - "angularVelocity": -0.11893237262991438, - "velocityX": 0.2854425752342375, - "velocityY": -0.1585678997982562, - "timestamp": 0.07514606787342622 - }, - { - "x": 0.4975916635116251, - "y": 4.0853873108981436, - "heading": -0.026812413296636577, - "angularVelocity": -0.2378715700833438, - "velocityX": 0.570885269240018, - "velocityY": -0.31713191087600345, - "timestamp": 0.15029213574685243 - }, - { - "x": 0.5619414537480725, - "y": 4.049641027140498, - "heading": -0.05362171709802473, - "angularVelocity": -0.35676256336585627, - "velocityX": 0.856329440215504, - "velocityY": -0.4756906750976657, - "timestamp": 0.22543820362027867 - }, - { - "x": 0.6477414523773106, - "y": 4.001980177860296, - "heading": -0.08935853104911506, - "angularVelocity": -0.4755646564406316, - "velocityX": 1.141776290593902, - "velocityY": -0.6342427571923069, - "timestamp": 0.30058427149370487 - }, - { - "x": 0.7549919358777414, - "y": 3.942405375853288, - "heading": -0.13401442200653152, - "angularVelocity": -0.5942545261667384, - "velocityX": 1.4272268201854585, - "velocityY": -0.7927866845588475, - "timestamp": 0.37573033936713107 - }, - { - "x": 0.8836932393269182, - "y": 3.8709173416225418, - "heading": -0.18758057158075755, - "angularVelocity": -0.7128270459134503, - "velocityX": 1.7126818087934743, - "velocityY": -0.9513210238912078, - "timestamp": 0.4508764072405573 - }, - { - "x": 1.0338457421402325, - "y": 3.7875168932975933, - "heading": -0.2500489995023069, - "angularVelocity": -0.8312933688928248, - "velocityX": 1.998141846440012, - "velocityY": -1.109844476033343, - "timestamp": 0.5260224751139835 - }, - { - "x": 1.2054498603190866, - "y": 3.692204931169301, - "heading": -0.32141344961465557, - "angularVelocity": -0.9496764385936038, - "velocityX": 2.283607419990343, - "velocityY": -1.2683559476303248, - "timestamp": 0.6011685429874097 - }, - { - "x": 1.3985060453231253, - "y": 3.5849824249792595, - "heading": -0.40166985798593763, - "angularVelocity": -1.0680054278616893, - "velocityX": 2.5690790013020615, - "velocityY": -1.4268545144723213, - "timestamp": 0.676314610860836 - }, - { - "x": 1.5915937569821006, - "y": 3.4778198154599647, - "heading": -0.48176859267038563, - "angularVelocity": -1.065907198489266, - "velocityX": 2.5694985396202825, - "velocityY": -1.4260574445464824, - "timestamp": 0.7514606787342623 - }, - { - "x": 1.7632284858900336, - "y": 3.382566036520044, - "heading": -0.5529765476804537, - "angularVelocity": -0.9475938931363465, - "velocityX": 2.2840147697019786, - "velocityY": -1.2675816797275796, - "timestamp": 0.8266067466076885 - }, - { - "x": 1.9134098098562766, - "y": 3.2992204007390877, - "heading": -0.6152918418017524, - "angularVelocity": -0.8292555536806094, - "velocityX": 1.9985253815170194, - "velocityY": -1.1091150626981792, - "timestamp": 0.9017528144811148 - }, - { - "x": 2.0421373577646693, - "y": 3.2277823059512354, - "heading": -0.6687115003742364, - "angularVelocity": -0.7108776291856339, - "velocityX": 1.7130310547348626, - "velocityY": -0.9506564589404816, - "timestamp": 0.976898882354541 - }, - { - "x": 2.149410808485792, - "y": 3.1682512394298143, - "heading": -0.713232041811307, - "angularVelocity": -0.5924533737687984, - "velocityX": 1.4275324545498613, - "velocityY": -0.792204678250007, - "timestamp": 1.0520449502279672 - }, - { - "x": 2.2352298936713018, - "y": 3.120626784362015, - "heading": -0.7488501306819736, - "angularVelocity": -0.4739847323836121, - "velocityX": 1.1420302833417786, - "velocityY": -0.6337584442610682, - "timestamp": 1.1271910181013933 - }, - { - "x": 2.2995943987852443, - "y": 3.0849086278583613, - "heading": -0.7755632624607879, - "angularVelocity": -0.3554827622359307, - "velocityX": 0.8565252572144717, - "velocityY": -0.4753163740226096, - "timestamp": 1.2023370859748195 - }, - { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080857, - "angularVelocity": -0.23696688264902363, - "velocityX": 0.5710180813549187, - "velocityY": -0.3168769856057537, - "timestamp": 1.2774831538482456 + "x": 0.4570749914043126, + "y": 4.107894224868064, + "heading": -0.009932712149027413, + "angularVelocity": -0.1373640763265399, + "velocityX": 0.3295975698116412, + "velocityY": -0.18310287014791135, + "timestamp": 0.07230938695656872 + }, + { + "x": 0.5047410210186508, + "y": 4.0814144720383245, + "heading": -0.029797898632204542, + "angularVelocity": -0.27472486380099975, + "velocityX": 0.6591955985322896, + "velocityY": -0.36620076513225003, + "timestamp": 0.14461877391313743 + }, + { + "x": 0.576240240509544, + "y": 4.041695512105355, + "heading": -0.059590394556189516, + "angularVelocity": -0.41201422357348, + "velocityX": 0.9887958189140486, + "velocityY": -0.5492918914777362, + "timestamp": 0.21692816086970615 + }, + { + "x": 0.6715729183897807, + "y": 3.9887379727274364, + "heading": -0.09930156463679939, + "angularVelocity": -0.5491841620018386, + "velocityX": 1.3183997526835154, + "velocityY": -0.7323743376461849, + "timestamp": 0.28923754782627487 + }, + { + "x": 0.7907394144382421, + "y": 3.9225426248725555, + "heading": -0.148921029894937, + "angularVelocity": -0.686210564721571, + "velocityX": 1.648008662001748, + "velocityY": -0.9154461217412811, + "timestamp": 0.3615469347828436 + }, + { + "x": 0.9337401624459083, + "y": 3.8431103831358566, + "heading": -0.20843840181811515, + "angularVelocity": -0.8230932998910107, + "velocityX": 1.9776235704163385, + "velocityY": -1.0985052574765801, + "timestamp": 0.43385632173941235 + }, + { + "x": 1.100575665552893, + "y": 3.7504423098204516, + "heading": -0.2778446966612386, + "angularVelocity": -0.9598517946889951, + "velocityX": 2.307245436988583, + "velocityY": -1.2815497021301627, + "timestamp": 0.5061657086959811 + }, + { + "x": 1.2912465471408558, + "y": 3.6445396941775194, + "heading": -0.3571330045970933, + "angularVelocity": -1.0965147302863703, + "velocityX": 2.636875924594487, + "velocityY": -1.464576317132112, + "timestamp": 0.5784750956525498 + }, + { + "x": 1.505785276048115, + "y": 3.525464237325283, + "heading": -0.4461366707355329, + "angularVelocity": -1.2308729182270346, + "velocityX": 2.966955438802405, + "velocityY": -1.6467496387952614, + "timestamp": 0.6507844826091186 + }, + { + "x": 1.6964880033537257, + "y": 3.4196219693446848, + "heading": -0.5252603846379951, + "angularVelocity": -1.094238483172127, + "velocityX": 2.637316333772171, + "velocityY": -1.463741741361608, + "timestamp": 0.7230938695656873 + }, + { + "x": 1.8633540969095699, + "y": 3.3270118414974243, + "heading": -0.5945029830335408, + "angularVelocity": -0.9575879607046729, + "velocityX": 2.3076684864728434, + "velocityY": -1.280748347415596, + "timestamp": 0.7954032565222561 + }, + { + "x": 2.0063830653266717, + "y": 3.247633047534605, + "heading": -0.6538614517041024, + "angularVelocity": -0.8208957532196227, + "velocityX": 1.9780138435278007, + "velocityY": -1.0977661034589712, + "timestamp": 0.8677126434788248 + }, + { + "x": 2.125574496423577, + "y": 3.1814849244321213, + "heading": -0.7033318821965574, + "angularVelocity": -0.6841494939262661, + "velocityX": 1.648353500334547, + "velocityY": -0.9147930287697538, + "timestamp": 0.9400220304353936 + }, + { + "x": 2.220928049802523, + "y": 3.128566939134693, + "heading": -0.7429103365332542, + "angularVelocity": -0.5473487745162161, + "velocityX": 1.318688449622979, + "velocityY": -0.7318273259488806, + "timestamp": 1.0123314173919622 + }, + { + "x": 2.292443455008997, + "y": 3.0888786919590703, + "heading": -0.7725937492005902, + "angularVelocity": -0.4105056606988637, + "velocityX": 0.989019658670723, + "velocityY": -0.5488671505327598, + "timestamp": 1.084640804348531 + }, + { + "x": 2.34012050914709, + "y": 3.0624199236271457, + "heading": -0.7923807693651485, + "angularVelocity": -0.273643865580592, + "velocityX": 0.6593480617769434, + "velocityY": -0.3659105607936737, + "timestamp": 1.1569501913050997 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550474, - "velocityX": 0.2855094397331478, - "velocityY": -0.15843872456938407, - "timestamp": 1.3526292217216718 + "angularVelocity": -0.13679610671265519, + "velocityX": 0.3296745537009426, + "velocityY": -0.18295553238277804, + "timestamp": 1.2292595782616684 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 3.467177167133168e-32, - "velocityX": -1.439882477346126e-32, - "velocityY": -1.3815367235364851e-32, - "timestamp": 1.427775289595098 - }, - { - "x": 2.3773546785315864, - "y": 3.0407954088560687, - "heading": -0.7860770412156382, - "angularVelocity": 0.2650004205849919, - "velocityX": 0.21918861143707746, - "velocityY": -0.13736692695017716, - "timestamp": 1.4888897992558612 - }, - { - "x": 2.4041715283150715, - "y": 3.0239975454455843, - "heading": -0.75411979281041, - "angularVelocity": 0.5229077118120985, - "velocityX": 0.438796775632191, - "velocityY": -0.2748588429118795, - "timestamp": 1.5500043089166244 - }, - { - "x": 2.4444388170293236, - "y": 2.998788446329585, - "heading": -0.7069088657797715, - "angularVelocity": 0.772499481591177, - "velocityX": 0.6588826276733534, - "velocityY": -0.41248959135778174, - "timestamp": 1.6111188185773877 - }, - { - "x": 2.498190798561039, - "y": 2.965157152573787, - "heading": -0.6450804787000075, - "angularVelocity": 1.0116809808826641, - "velocityX": 0.8795289666903056, - "velocityY": -0.5502996578468837, - "timestamp": 1.672233328238151 - }, - { - "x": 2.5654677170302604, - "y": 2.923088008476279, - "heading": -0.5694581806203519, - "angularVelocity": 1.237386972413308, - "velocityX": 1.1008338092322851, - "velocityY": -0.6883658943027919, - "timestamp": 1.7333478378989142 - }, - { - "x": 2.6463157759141644, - "y": 2.8725583813996622, - "heading": -0.4811095285927534, - "angularVelocity": 1.445624819997864, - "velocityX": 1.3228946666295571, - "velocityY": -0.8268024624119354, - "timestamp": 1.7944623475596775 - }, - { - "x": 2.7407862870930626, - "y": 2.813537598490058, - "heading": -0.38139476987658943, - "angularVelocity": 1.6316053138553275, - "velocityX": 1.5457951262848801, - "velocityY": -0.9657409220366661, - "timestamp": 1.8555768572204407 - }, - { - "x": 2.84893500974016, - "y": 2.7459886359527177, - "heading": -0.27202745092011676, - "angularVelocity": 1.7895475160244771, - "velocityX": 1.7696079580350594, - "velocityY": -1.105285191884772, - "timestamp": 1.916691366881204 - }, - { - "x": 2.970822636317052, - "y": 2.669873093439706, - "heading": -0.15523119808259794, - "angularVelocity": 1.9111051284848073, - "velocityX": 1.9944138839282306, - "velocityY": -1.245457796119401, - "timestamp": 1.9778058765419673 - }, - { - "x": 3.106512263881207, - "y": 2.5851587340080315, - "heading": -0.03419676462946541, - "angularVelocity": 1.980453318286855, - "velocityX": 2.220252249708728, - "velocityY": -1.3861578846318265, - "timestamp": 2.0389203862027303 - }, - { - "x": 3.256035657144846, - "y": 2.4918371927335334, - "heading": 0.08572826850512172, - "angularVelocity": 1.962300504418209, - "velocityX": 2.4466103727841246, - "velocityY": -1.5269948461095477, - "timestamp": 2.1000348958634936 - }, - { - "x": 3.419180860242083, - "y": 2.390029027608911, - "heading": 0.19415018142326176, - "angularVelocity": 1.7740780956923756, - "velocityX": 2.6695003200194134, - "velocityY": -1.6658591501387068, - "timestamp": 2.161149405524257 - }, - { - "x": 3.5929644641848295, - "y": 2.279455345020229, - "heading": 0.26705766200125053, - "angularVelocity": 1.1929651564364412, - "velocityX": 2.8435735622750062, - "velocityY": -1.8092869140644157, - "timestamp": 2.22226391518502 - }, - { - "x": 3.7768655464749616, - "y": 2.1600020280366277, - "heading": 0.30152237044830693, - "angularVelocity": 0.5639365944088304, - "velocityX": 3.0091230922237155, - "velocityY": -1.9545819421061743, - "timestamp": 2.2833784248457833 - }, - { - "x": 3.9728967066647076, - "y": 2.0375522576737963, - "heading": 0.30152241765502197, - "angularVelocity": 7.724305624896446e-7, - "velocityX": 3.2076042379769354, - "velocityY": -2.003612088889046, - "timestamp": 2.3444929345065466 - }, - { - "x": 4.174386727718979, - "y": 1.9243094504352285, - "heading": 0.30152246485493484, - "angularVelocity": 7.723192600121745e-7, - "velocityX": 3.2969260847008353, - "velocityY": -1.85296107040964, - "timestamp": 2.40560744416731 - }, - { - "x": 4.383354452565466, - "y": 1.8255438101811146, - "heading": 0.30152251288342946, - "angularVelocity": 7.858771165145409e-7, - "velocityX": 3.419281705873656, - "velocityY": -1.6160751481496927, - "timestamp": 2.466721953828073 - }, - { - "x": 4.598761540028333, - "y": 1.7417467464566943, - "heading": 0.30152256294147634, - "angularVelocity": 8.190861247953373e-7, - "velocityX": 3.5246472344874826, - "velocityY": -1.3711484259558735, - "timestamp": 2.5278364634888364 - }, - { - "x": 4.819537390717514, - "y": 1.6733348748555603, - "heading": 0.30152261646908723, - "angularVelocity": 8.758576510857166e-7, - "velocityX": 3.612494838208991, - "velocityY": -1.1194047367945392, - "timestamp": 2.5889509731495997 - }, - { - "x": 5.04458467733647, - "y": 1.620648272623701, - "heading": 0.30152267534909943, - "angularVelocity": 9.634375304093619e-7, - "velocityX": 3.682387175617667, - "velocityY": -0.8620964567058491, - "timestamp": 2.650065482810363 - }, - { - "x": 5.27278482830379, - "y": 1.5839488347993986, - "heading": 0.301522742248914, - "angularVelocity": 0.000001094663361600596, - "velocityX": 3.7339766322927477, - "velocityY": -0.6005028597630109, - "timestamp": 2.711179992471126 + "angularVelocity": 2.834394108236562e-32, + "velocityX": -3.2904683025944325e-34, + "velocityY": 0, + "timestamp": 1.3015689652182372 + }, + { + "x": 2.379804145625828, + "y": 3.040219511725313, + "heading": -0.7770894823147659, + "angularVelocity": 0.41593022428853316, + "velocityX": 0.2617028389642093, + "velocityY": -0.14816838427057621, + "timestamp": 1.3621150092281344 + }, + { + "x": 2.411575732183553, + "y": 3.0222750861201635, + "heading": -0.7276303412852089, + "angularVelocity": 0.8168847665996504, + "velocityX": 0.524750825215457, + "velocityY": -0.2963765163949562, + "timestamp": 1.4226610532380317 + }, + { + "x": 2.459369710281825, + "y": 2.995353279300503, + "heading": -0.6550403019178791, + "angularVelocity": 1.1989229115524662, + "velocityX": 0.789382343303212, + "velocityY": -0.4446501379224719, + "timestamp": 1.483207097247929 + }, + { + "x": 2.5233031088128075, + "y": 2.959437775047564, + "heading": -0.5609434444834801, + "angularVelocity": 1.5541371690447268, + "velocityX": 1.0559467522028614, + "velocityY": -0.5931932439230606, + "timestamp": 1.5437531412578263 + }, + { + "x": 2.6035132843463873, + "y": 2.9144863047853526, + "heading": -0.44765744633251, + "angularVelocity": 1.8710718429836914, + "velocityX": 1.3247797910705466, + "velocityY": -0.7424344727603186, + "timestamp": 1.6042991852677235 + }, + { + "x": 2.700148811858614, + "y": 2.8604246981568315, + "heading": -0.31830060076747974, + "angularVelocity": 2.136503675514867, + "velocityX": 1.5960667470930034, + "velocityY": -0.8929007255979282, + "timestamp": 1.6648452292776208 + }, + { + "x": 2.8133605147902916, + "y": 2.7971622097455233, + "heading": -0.1768724119476308, + "angularVelocity": 2.3358782746686186, + "velocityX": 1.869844756713935, + "velocityY": -1.0448657620135586, + "timestamp": 1.725391273287518 + }, + { + "x": 2.943296329434224, + "y": 2.7246257074465063, + "heading": -0.02888673141070084, + "angularVelocity": 2.4441841404657145, + "velocityX": 2.1460661347699634, + "velocityY": -1.1980386742882738, + "timestamp": 1.7859373172974153 + }, + { + "x": 3.0900160520490236, + "y": 2.6428091950168, + "heading": 0.11617462075416056, + "angularVelocity": 2.3958848928453333, + "velocityX": 2.4232751291036574, + "velocityY": -1.3513106226450158, + "timestamp": 1.8464833613073126 + }, + { + "x": 3.252842131476166, + "y": 2.55207108446101, + "heading": 0.23929724530787766, + "angularVelocity": 2.0335370636864525, + "velocityX": 2.6892934474880965, + "velocityY": -1.4986629108411686, + "timestamp": 1.9070294053172099 + }, + { + "x": 3.4280030327225406, + "y": 2.451410973839388, + "heading": 0.3174619447233944, + "angularVelocity": 1.2909959798981987, + "velocityX": 2.8930197523349617, + "velocityY": -1.662538193332126, + "timestamp": 1.9675754493271072 + }, + { + "x": 3.6156107870917813, + "y": 2.340964520507575, + "heading": 0.3503075182755731, + "angularVelocity": 0.5424891764490772, + "velocityX": 3.098596406043856, + "velocityY": -1.8241729106819762, + "timestamp": 2.028121493337004 + }, + { + "x": 3.8155697764375462, + "y": 2.229387834544172, + "heading": 0.3503075829776931, + "angularVelocity": 0.0000010686432293648565, + "velocityX": 3.302593796434966, + "velocityY": -1.8428402348659505, + "timestamp": 2.0886675373469012 + }, + { + "x": 4.015529033815858, + "y": 2.117811628928195, + "heading": 0.35030764767858236, + "angularVelocity": 0.000001068622902276308, + "velocityX": 3.3025982233558384, + "velocityY": -1.8428323012769912, + "timestamp": 2.1492135813567983 + }, + { + "x": 4.215488291220884, + "y": 2.0062354233600974, + "heading": 0.35030771237947195, + "angularVelocity": 0.0000010686229082180607, + "velocityX": 3.302598223797092, + "velocityY": -1.8428323004862048, + "timestamp": 2.2097596253666953 + }, + { + "x": 4.415447878546615, + "y": 1.8946598090554148, + "heading": 0.350307777080342, + "angularVelocity": 0.0000010686225853054014, + "velocityX": 3.3026036728847936, + "velocityY": -1.842822534969312, + "timestamp": 2.2703056693765924 + }, + { + "x": 4.620289311400001, + "y": 1.7923224180578192, + "heading": 0.35030784179343544, + "angularVelocity": 0.00000106882447086681, + "velocityX": 3.383233970164951, + "velocityY": -1.6902407526553997, + "timestamp": 2.3308517133864894 + }, + { + "x": 4.832980772388526, + "y": 1.7074973259478416, + "heading": 0.35030790812662205, + "angularVelocity": 0.0000010955825050486418, + "velocityX": 3.512887827217186, + "velocityY": -1.401001394841118, + "timestamp": 2.3913977573963865 + }, + { + "x": 5.052028438396142, + "y": 1.6407809635052637, + "heading": 0.3503079787205065, + "angularVelocity": 0.0000011659537069164618, + "velocityX": 3.617869170309609, + "velocityY": -1.101911174108616, + "timestamp": 2.4519438014062835 + }, + { + "x": 5.275893582425239, + "y": 1.5926421132936275, + "heading": 0.3503080570287918, + "angularVelocity": 0.0000012933674954586782, + "velocityX": 3.697436350961301, + "velocityY": -0.7950783738037029, + "timestamp": 2.5124898454161806 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": 0.3015228161300743, - "angularVelocity": 0.0000012088972110609183, - "velocityX": 3.7670067261218194, - "velocityY": -0.33592432468507777, - "timestamp": 2.7722945021318894 - }, - { - "x": 5.642161233224445, - "y": 1.5569512204704454, - "heading": 0.30152289610635313, - "angularVelocity": 0.0000021712102282875206, - "velocityX": 3.7778762267369546, - "velocityY": -0.17558800471026415, - "timestamp": 2.809129387028348 - }, - { - "x": 5.781468008961295, - "y": 1.556401100893367, - "heading": 0.30152296986307525, - "angularVelocity": 0.0000020023605969229892, - "velocityX": 3.781925099764402, - "velocityY": -0.014934744023907952, - "timestamp": 2.8459642719248066 - }, - { - "x": 5.920672391336217, - "y": 1.5617696165699877, - "heading": 0.3015230386365787, - "angularVelocity": 0.0000018670752912406278, - "velocityX": 3.779145306581512, - "velocityY": 0.14574541746802047, - "timestamp": 2.882799156821265 - }, - { - "x": 6.0595230325638285, - "y": 1.5730470711005764, - "heading": 0.3015231034083751, - "angularVelocity": 0.0000017584362380693717, - "velocityX": 3.769541879061509, - "velocityY": 0.30616233937716997, - "timestamp": 2.919634041717724 - }, - { - "x": 6.197769224123459, - "y": 1.5902130982406153, - "heading": 0.3015231649742209, - "angularVelocity": 0.000001671400517937078, - "velocityX": 3.753132172076425, - "velocityY": 0.46602635486148125, - "timestamp": 2.9564689266141824 - }, - { - "x": 6.335161349542959, - "y": 1.6132366986444322, - "heading": 0.3015232239953199, - "angularVelocity": 0.0000016023152822250254, - "velocityX": 3.72994583275345, - "velocityY": 0.6250487946015117, - "timestamp": 2.993303811510641 - }, - { - "x": 6.471451335210211, - "y": 1.6420762959087598, - "heading": 0.30152328103508963, - "angularVelocity": 0.000001548525804202316, - "velocityX": 3.7000247469309935, - "velocityY": 0.7829425107583317, - "timestamp": 3.0301386964070995 - }, - { - "x": 6.606393098338155, - "y": 1.6766798119201127, - "heading": 0.301523336612691, - "angularVelocity": 0.000001508830595721816, - "velocityX": 3.663422961881362, - "velocityY": 0.9394224010369989, - "timestamp": 3.066973581303558 - }, - { - "x": 6.739742463422568, - "y": 1.716984614087568, - "heading": 0.3015255656708468, - "angularVelocity": 0.00006051486687697733, - "velocityX": 3.6201922568579414, - "velocityY": 1.094201930608725, - "timestamp": 3.1038084662000167 - }, - { - "x": 6.8694207904091344, - "y": 1.7614703171513673, - "heading": 0.3101205256410531, - "angularVelocity": 0.23333750042565204, - "velocityX": 3.520530262306672, - "velocityY": 1.207705771006116, - "timestamp": 3.1406433510964753 - }, - { - "x": 6.994764102779472, - "y": 1.808693107878132, - "heading": 0.3306180633638746, - "angularVelocity": 0.5564707961064413, - "velocityX": 3.4028425152588873, - "velocityY": 1.282012713206686, - "timestamp": 3.177478235992934 - }, - { - "x": 7.114597005425528, - "y": 1.856512920318317, - "heading": 0.35962771339750194, - "angularVelocity": 0.7875591335542964, - "velocityX": 3.253244932973218, - "velocityY": 1.2982207647615676, - "timestamp": 3.2143131208893925 - }, - { - "x": 7.228563598795877, - "y": 1.9039637614742513, - "heading": 0.39146295222083616, - "angularVelocity": 0.8642687200685354, - "velocityX": 3.093985326429113, - "velocityY": 1.2882038667778264, - "timestamp": 3.251148005785851 - }, - { - "x": 7.336676614220082, - "y": 1.9505339221399116, - "heading": 0.42341393247502157, - "angularVelocity": 0.8674108889982484, - "velocityX": 2.935071352282109, - "velocityY": 1.2642949963483496, - "timestamp": 3.2879828906823096 - }, - { - "x": 7.438996464546078, - "y": 1.995939510716552, - "heading": 0.45395278545537515, - "angularVelocity": 0.8290742068611583, - "velocityX": 2.77779747686502, - "velocityY": 1.2326789863542051, - "timestamp": 3.3248177755787682 - }, - { - "x": 7.535582073954796, - "y": 2.0400031624519186, - "heading": 0.48208291467910164, - "angularVelocity": 0.7636817463336474, - "velocityX": 2.6221232855814667, - "velocityY": 1.196247846551653, - "timestamp": 3.361652660475227 + "heading": 0.35030814446785413, + "angularVelocity": 0.0000014441746575542642, + "velocityX": 3.751029791428781, + "velocityY": -0.4826595916936126, + "timestamp": 2.5730358894260776 + }, + { + "x": 5.645916996346935, + "y": 1.5526078041590061, + "heading": 0.35030823517092025, + "angularVelocity": 0.0000023934601751232044, + "velocityX": 3.7711793420471578, + "velocityY": -0.28528395446410554, + "timestamp": 2.6109320980454838 + }, + { + "x": 5.78920070535826, + "y": 1.5493061559809091, + "heading": 0.350308318678533, + "angularVelocity": 0.000002203587530623072, + "velocityX": 3.7809510299653186, + "velocityY": -0.08712344317225716, + "timestamp": 2.64882830666489 + }, + { + "x": 5.932460399917018, + "y": 1.5535231156733713, + "heading": 0.3503083968024594, + "angularVelocity": 0.0000020615235464554193, + "velocityX": 3.7803173398564307, + "velocityY": 0.11127655895113732, + "timestamp": 2.686724515284296 + }, + { + "x": 6.075301824148574, + "y": 1.5652470604158988, + "heading": 0.3503084709306375, + "angularVelocity": 0.0000019560842841988308, + "velocityX": 3.769280079337772, + "velocityY": 0.3093698596678062, + "timestamp": 2.724620723903702 + }, + { + "x": 6.217331878166829, + "y": 1.5844456932271112, + "heading": 0.3503085421651287, + "angularVelocity": 0.0000018797260658699983, + "velocityX": 3.747869752477631, + "velocityY": 0.5066109120314745, + "timestamp": 2.7625169325231083 + }, + { + "x": 6.358159707247505, + "y": 1.6110661098029424, + "heading": 0.3503086114142786, + "angularVelocity": 0.000001827337150563986, + "velocityX": 3.716145604300894, + "velocityY": 0.7024559328132687, + "timestamp": 2.8004131411425144 + }, + { + "x": 6.497397807591943, + "y": 1.6450348544134032, + "heading": 0.35030867945721406, + "angularVelocity": 0.0000017955077283623068, + "velocityX": 3.6741960585771425, + "velocityY": 0.8963626137804634, + "timestamp": 2.8383093497619205 + }, + { + "x": 6.634663343210931, + "y": 1.6862573848132372, + "heading": 0.3503087469924593, + "angularVelocity": 0.000001782110868887289, + "velocityX": 3.622144288827235, + "velocityY": 1.0877745268355006, + "timestamp": 2.8762055583813266 + }, + { + "x": 6.770832471288569, + "y": 1.7309686010151983, + "heading": 0.350308814465375, + "angularVelocity": 0.0000017804661250723641, + "velocityX": 3.593212435713354, + "velocityY": 1.179833493397679, + "timestamp": 2.9141017670007328 + }, + { + "x": 6.907000934216078, + "y": 1.775681842897667, + "heading": 0.35030888193834014, + "angularVelocity": 0.0000017804674286764754, + "velocityX": 3.5931948838222003, + "velocityY": 1.1798869467794602, + "timestamp": 2.951997975620139 + }, + { + "x": 7.041712141501968, + "y": 1.8246105114710787, + "heading": 0.3503089535974655, + "angularVelocity": 0.0000018909312562539825, + "velocityX": 3.5547410201057947, + "velocityY": 1.2911230530949709, + "timestamp": 2.989894184239545 + }, + { + "x": 7.171770756302622, + "y": 1.8790616059317657, + "heading": 0.3592109050640362, + "angularVelocity": 0.2349034848307258, + "velocityX": 3.43196904225537, + "velocityY": 1.43684807648023, + "timestamp": 3.027790392858951 + }, + { + "x": 7.296132052444026, + "y": 1.9323306109051834, + "heading": 0.3900794027691138, + "angularVelocity": 0.8145537200064432, + "velocityX": 3.281628972184848, + "velocityY": 1.4056552598282668, + "timestamp": 3.0656866014783573 + }, + { + "x": 7.41350002407911, + "y": 1.9843017273758428, + "heading": 0.4310938047294686, + "angularVelocity": 1.0822824618754054, + "velocityX": 3.097090076050037, + "velocityY": 1.371406754501705, + "timestamp": 3.1035828100977634 + }, + { + "x": 7.523578248846203, + "y": 2.0344435812093704, + "heading": 0.471270328487765, + "angularVelocity": 1.0601726458124487, + "velocityX": 2.904729226942307, + "velocityY": 1.3231364207724636, + "timestamp": 3.1414790187171695 }, { "x": 7.626483917236328, "y": 2.082604169845581, "heading": 0.507098504392337, - "angularVelocity": 0.6791276743107281, - "velocityX": 2.4678193928677437, - "velocityY": 1.1565397180800703, - "timestamp": 3.3984875453716854 - }, - { - "x": 7.766333855095899, - "y": 2.1513911601167894, - "heading": 0.5393931714770351, - "angularVelocity": 0.5089507306836832, - "velocityX": 2.2039777611896034, - "velocityY": 1.0840548028640598, - "timestamp": 3.461940970502476 - }, - { - "x": 7.8897168213730975, - "y": 2.214535022901093, - "heading": 0.5621690879226864, - "angularVelocity": 0.3589391179231937, - "velocityX": 1.9444650312080196, - "velocityY": 0.9951214241650669, - "timestamp": 3.5253943956332665 - }, - { - "x": 7.996856912991746, - "y": 2.271342778200504, - "heading": 0.5763089063837279, - "angularVelocity": 0.2228377496076287, - "velocityX": 1.6884839769927567, - "velocityY": 0.8952669644281369, - "timestamp": 3.588847820764057 - }, - { - "x": 8.08793407853681, - "y": 2.3213235528188796, - "heading": 0.5824533246990756, - "angularVelocity": 0.09683351690917834, - "velocityX": 1.4353388387992245, - "velocityY": 0.7876765441007412, - "timestamp": 3.6523012458948476 - }, - { - "x": 8.16309393881241, - "y": 2.364112462024999, - "heading": 0.5810872026138336, - "angularVelocity": -0.021529524725041504, - "velocityX": 1.1844886248564517, - "velocityY": 0.6743356898059124, - "timestamp": 3.715754671025638 - }, - { - "x": 8.222455939995415, - "y": 2.399428067130867, - "heading": 0.5725898810376202, - "angularVelocity": -0.13391430893917436, - "velocityX": 0.9355208337555891, - "velocityY": 0.5565594770191685, - "timestamp": 3.7792080961564287 - }, - { - "x": 8.266119462815807, - "y": 2.4270468912932266, - "heading": 0.5572661381126698, - "angularVelocity": -0.24149591441856846, - "velocityX": 0.6881192422693254, - "velocityY": 0.43526136068197446, - "timestamp": 3.8426615212872193 - }, - { - "x": 8.294168308263885, - "y": 2.4467872814650136, - "heading": 0.5353660980042331, - "angularVelocity": -0.3451356654632318, - "velocityX": 0.4420383200790668, - "velocityY": 0.3111004666981787, - "timestamp": 3.90611494641801 + "angularVelocity": 0.9454290339278092, + "velocityX": 2.715460784576445, + "velocityY": 1.2708550641540455, + "timestamp": 3.1793752273365756 + }, + { + "x": 7.782116606596641, + "y": 2.1591418247265515, + "heading": 0.5502904833862925, + "angularVelocity": 0.6648323287723413, + "velocityX": 2.395575421885603, + "velocityY": 1.1781054843634051, + "timestamp": 3.244341952215483 + }, + { + "x": 7.917274495842872, + "y": 2.2282082387650113, + "heading": 0.5787513876779874, + "angularVelocity": 0.4380843323215672, + "velocityX": 2.0804171596791274, + "velocityY": 1.0631044456557224, + "timestamp": 3.30930867709439 + }, + { + "x": 8.032169166662662, + "y": 2.2890615062609574, + "heading": 0.5943003276772478, + "angularVelocity": 0.2393369840982844, + "velocityX": 1.768515667581298, + "velocityY": 0.9366836270316962, + "timestamp": 3.3742754019732972 + }, + { + "x": 8.12694730000964, + "y": 2.3412528312827354, + "heading": 0.5980433212943994, + "angularVelocity": 0.05761401123618554, + "velocityX": 1.458871961356148, + "velocityY": 0.8033547191898265, + "timestamp": 3.4392421268522044 + }, + { + "x": 8.201715109739398, + "y": 2.3844820330172567, + "heading": 0.590722775552934, + "angularVelocity": -0.11268146509017382, + "velocityX": 1.1508631513920338, + "velocityY": 0.6654052796273325, + "timestamp": 3.5042088517311116 + }, + { + "x": 8.256552657549896, + "y": 2.418534329374767, + "heading": 0.572871769156374, + "angularVelocity": -0.27477153003838295, + "velocityX": 0.8440866907283819, + "velocityY": 0.5241498077820845, + "timestamp": 3.569175576610019 + }, + { + "x": 8.291522317600203, + "y": 2.443248428942356, + "heading": 0.5448922111974887, + "angularVelocity": -0.4306752112106213, + "velocityX": 0.538270324007952, + "velocityY": 0.3804116586399298, + "timestamp": 3.634142301488926 }, { "x": 8.306674003601074, "y": 2.45849871635437, "heading": 0.507098504392337, - "angularVelocity": -0.44548570157766576, - "velocityX": 0.19708463824313602, - "velocityY": 0.18456741878341876, - "timestamp": 3.9695683715488004 - }, - { - "x": 8.303024530407173, - "y": 2.461881127582199, - "heading": 0.47119103232326837, - "angularVelocity": -0.5465136547157349, - "velocityX": -0.05554517814981655, - "velocityY": 0.05148048067312068, - "timestamp": 4.035271161880262 - }, - { - "x": 8.282750419132642, - "y": 2.456558519757899, - "heading": 0.4288756816904664, - "angularVelocity": -0.6440419108431604, - "velocityX": -0.3085730632177365, - "velocityY": -0.08101037714605593, - "timestamp": 4.1009739522117235 - }, - { - "x": 8.245821395629305, - "y": 2.442576926285189, - "heading": 0.3804195474569323, - "angularVelocity": -0.7375049672788585, - "velocityX": -0.5620617224479442, - "velocityY": -0.21280060408660287, - "timestamp": 4.166676742543185 - }, - { - "x": 8.19220201387014, - "y": 2.4199912479232255, - "heading": 0.32613641295591117, - "angularVelocity": -0.8261922245184762, - "velocityX": -0.8160898721144456, - "velocityY": -0.34375523852216117, - "timestamp": 4.2323795328746465 - }, - { - "x": 8.121850214183777, - "y": 2.388868090056124, - "heading": 0.2664003971864745, - "angularVelocity": -0.909185370485436, - "velocityX": -1.070758172239708, - "velocityY": -0.4736961354318384, - "timestamp": 4.298082323206108 - }, - { - "x": 8.034715295558529, - "y": 2.349289919990928, - "heading": 0.20166644793914118, - "angularVelocity": -0.9852541866298153, - "velocityX": -1.3261981444876794, - "velocityY": -0.6023818754961501, - "timestamp": 4.36378511353757 - }, - { - "x": 7.9307349853362235, - "y": 2.301361396168203, - "heading": 0.1325025874209066, - "angularVelocity": -1.052677674255729, - "velocityX": -1.5825859099398853, - "velocityY": -0.7294747084702512, - "timestamp": 4.429487903869031 - }, - { - "x": 7.809831081747403, - "y": 2.2452194829459002, - "heading": 0.0596429970770519, - "angularVelocity": -1.1089268808263426, - "velocityX": -1.8401639105261376, - "velocityY": -0.8544829365552749, - "timestamp": 4.495190694200493 - }, - { - "x": 7.671902780233127, - "y": 2.1810506699963903, - "heading": -0.015920256793080897, - "angularVelocity": -1.1500767850029918, - "velocityX": -2.099276161917122, - "velocityY": -0.9766527818040505, - "timestamp": 4.560893484531954 - }, - { - "x": 7.516816185598606, - "y": 2.109122861945235, - "heading": -0.09276870443286889, - "angularVelocity": -1.1696375032490751, - "velocityX": -2.3604263053689314, - "velocityY": -1.0947451042534069, - "timestamp": 4.626596274863416 - }, - { - "x": 7.344387868807497, - "y": 2.02985165560778, - "heading": -0.16871929267657412, - "angularVelocity": -1.1559720349858067, - "velocityX": -2.6243682486121434, - "velocityY": -1.206512020837212, - "timestamp": 4.692299065194877 - }, - { - "x": 7.154363408340053, - "y": 1.9439629823466762, - "heading": -0.2400485240661188, - "angularVelocity": -1.0856347352935642, - "velocityX": -2.8921825010596516, - "velocityY": -1.3072302230667405, - "timestamp": 4.758001855526339 - }, - { - "x": 6.9464441838510815, - "y": 1.8530162365161489, - "heading": -0.2992772419214471, - "angularVelocity": -0.9014642689682996, - "velocityX": -3.1645417712101533, - "velocityY": -1.3842143594162994, - "timestamp": 4.8237046458578 - }, - { - "x": 6.72212326982476, - "y": 1.7626751908782685, - "heading": -0.31855321839260475, - "angularVelocity": -0.2933813978662556, - "velocityX": -3.4141763674670904, - "velocityY": -1.3749955699312393, - "timestamp": 4.889407436189262 - }, - { - "x": 6.485759745047901, - "y": 1.6860130581467425, - "heading": -0.3185532921212082, - "angularVelocity": -0.0000011221533066716862, - "velocityX": -3.597465550312819, - "velocityY": -1.1668017803319484, - "timestamp": 4.955110226520723 - }, - { - "x": 6.24426876944993, - "y": 1.6274732096370486, - "heading": -0.3185533177664736, - "angularVelocity": -3.9032231775904613e-7, - "velocityX": -3.675505627381763, - "velocityY": -0.8909796405048926, - "timestamp": 5.020813016852185 - }, - { - "x": 5.999037658914315, - "y": 1.5873919634602989, - "heading": -0.3185533462426243, - "angularVelocity": -4.334085446884588e-7, - "velocityX": -3.732430682143934, - "velocityY": -0.6100387209515062, - "timestamp": 5.0865158071836465 - }, - { - "x": 5.751475205523387, - "y": 1.5659995878977724, - "heading": -0.3185533789943251, - "angularVelocity": -4.984826463431063e-7, - "velocityX": -3.7679138456983283, - "velocityY": -0.325593105781428, - "timestamp": 5.152218597515108 + "angularVelocity": -0.5817394500892022, + "velocityX": 0.23322225384012837, + "velocityY": 0.2347399755865949, + "timestamp": 3.6991090263678332 + }, + { + "x": 8.301908465997242, + "y": 2.464162051150854, + "heading": 0.4594182049840147, + "angularVelocity": -0.7297490572088752, + "velocityX": -0.07293676039464714, + "velocityY": 0.08667758549500083, + "timestamp": 3.7644469653994896 + }, + { + "x": 8.277129357589926, + "y": 2.4601442119160213, + "heading": 0.4025234718451046, + "angularVelocity": -0.8707763664131557, + "velocityX": -0.3792453324141557, + "velocityY": -0.061493204321711674, + "timestamp": 3.829784904431146 + }, + { + "x": 8.232325554908503, + "y": 2.44643699593842, + "heading": 0.33696231020008877, + "angularVelocity": -1.0034164318107932, + "velocityX": -0.6857241496355521, + "velocityY": -0.20978953699413314, + "timestamp": 3.8951228434628025 + }, + { + "x": 8.167484390379387, + "y": 2.42303097033958, + "heading": 0.26340235513425153, + "angularVelocity": -1.1258383131766259, + "velocityX": -0.992396844621899, + "velocityY": -0.35823024028199546, + "timestamp": 3.960460782494459 + }, + { + "x": 8.082591447673355, + "y": 2.3899154191246006, + "heading": 0.18267464694130434, + "angularVelocity": -1.235541086685248, + "velocityX": -1.2992901821543714, + "velocityY": -0.5068349523381112, + "timestamp": 4.025798721526115 + }, + { + "x": 7.9776304347285665, + "y": 2.347078349358033, + "heading": 0.09584952732096214, + "angularVelocity": -1.328862233904795, + "velocityX": -1.6064328704022224, + "velocityY": -0.6556232167931427, + "timestamp": 4.091136660557772 + }, + { + "x": 7.852583598052917, + "y": 2.2945065473278667, + "heading": 0.004378357289613106, + "angularVelocity": -1.3999702376138785, + "velocityX": -1.9138472766192256, + "velocityY": -0.8046137176854637, + "timestamp": 4.156474599589428 + }, + { + "x": 7.707434523289432, + "y": 2.232185897198679, + "heading": -0.08962534429630348, + "angularVelocity": -1.4387307432573238, + "velocityX": -2.221512905283988, + "velocityY": -0.9538202620531743, + "timestamp": 4.2218125386210845 + }, + { + "x": 7.542180705960218, + "y": 2.1601037497785995, + "heading": -0.1827828763211059, + "angularVelocity": -1.42578008130418, + "velocityX": -2.5292168650919318, + "velocityY": -1.1032204028528767, + "timestamp": 4.287150477652741 + }, + { + "x": 7.35689136818444, + "y": 2.078264678726032, + "heading": -0.2689709068831343, + "angularVelocity": -1.3191115581449608, + "velocityX": -2.835861377353881, + "velocityY": -1.252550543611691, + "timestamp": 4.352488416684397 + }, + { + "x": 7.152059282385439, + "y": 1.9867952525471149, + "heading": -0.3348300100346558, + "angularVelocity": -1.007976439532523, + "velocityX": -3.134963986234081, + "velocityY": -1.3999435478765458, + "timestamp": 4.417826355716054 + }, + { + "x": 6.932416862636984, + "y": 1.8843357350106482, + "heading": -0.3485403575636041, + "angularVelocity": -0.20983746552376023, + "velocityX": -3.3616367917885555, + "velocityY": -1.5681473743275887, + "timestamp": 4.48316429474771 + }, + { + "x": 6.70958980281502, + "y": 1.7775224962078198, + "heading": -0.34854038203919246, + "angularVelocity": -3.7459994478515855e-7, + "velocityX": -3.410377846690442, + "velocityY": -1.6347812677571678, + "timestamp": 4.548502233779367 + }, + { + "x": 6.478027967608038, + "y": 1.6912665812341474, + "heading": -0.3485404067374628, + "angularVelocity": -3.780081020278718e-7, + "velocityX": -3.5440639640437577, + "velocityY": -1.3201505320191125, + "timestamp": 4.613840172811023 + }, + { + "x": 6.239619726654668, + "y": 1.6262859956866853, + "heading": -0.34854043255776096, + "angularVelocity": -3.951807866461482e-7, + "velocityX": -3.6488485019072954, + "velocityY": -0.9945306893745133, + "timestamp": 4.679178111842679 + }, + { + "x": 5.996315346781838, + "y": 1.5831125822372847, + "heading": -0.3485404605967219, + "angularVelocity": -4.291375179720768e-7, + "velocityX": -3.7237841211206324, + "velocityY": -0.6607709714945885, + "timestamp": 4.744516050874336 + }, + { + "x": 5.7501052372367845, + "y": 1.5620996014320112, + "heading": -0.34854049237288, + "angularVelocity": -4.863354823397662e-7, + "velocityX": -3.7682564401941674, + "velocityY": -0.3216045855853207, + "timestamp": 4.809853989905992 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.3185534180019772, - "angularVelocity": -5.936985611348973e-7, - "velocityX": -3.781751231726404, - "velocityY": -0.039276923728911305, - "timestamp": 5.21792138784657 - }, - { - "x": 5.26265149067387, - "y": 1.5786031053003822, - "heading": -0.31855345112044986, - "angularVelocity": -5.20084412877405e-7, - "velocityX": -3.7744308273915435, - "velocityY": 0.23844772894968475, - "timestamp": 5.28160042121006 - }, - { - "x": 5.0240625547753925, - "y": 1.61139052009726, - "heading": -0.3185534786660252, - "angularVelocity": -4.3256899255575315e-7, - "velocityX": -3.7467424251962935, - "velocityY": 0.5148855606793222, - "timestamp": 5.34527945457355 - }, - { - "x": 4.788524290519247, - "y": 1.661604289022676, - "heading": -0.31855350247111613, - "angularVelocity": -3.7382933947955595e-7, - "velocityX": -3.698835422197071, - "velocityY": 0.7885447732660761, - "timestamp": 5.40895848793704 - }, - { - "x": 4.55730773239592, - "y": 1.7289734284079028, - "heading": -0.3185535237219762, - "angularVelocity": -3.337183198598428e-7, - "velocityX": -3.6309684037367047, - "velocityY": 1.0579485244487485, - "timestamp": 5.4726375213005305 - }, - { - "x": 4.33166058516164, - "y": 1.8131343663413284, - "heading": -0.3185535432404584, - "angularVelocity": -3.0651348259884106e-7, - "velocityX": -3.543507734268667, - "velocityY": 1.321642831055895, - "timestamp": 5.536316554664021 - }, - { - "x": 4.112800476187319, - "y": 1.9136328827222209, - "heading": -0.318553561635836, - "angularVelocity": -2.888765193967595e-7, - "velocityX": -3.436925741712075, - "velocityY": 1.5782041760469432, - "timestamp": 5.599995588027511 - }, - { - "x": 3.9019083093617053, - "y": 2.0299264483320134, - "heading": -0.31855357939397577, - "angularVelocity": -2.7886949297236075e-7, - "velocityX": -3.311799122668965, - "velocityY": 1.826245774585971, - "timestamp": 5.663674621391001 - }, - { - "x": 3.7001203074329077, - "y": 2.161384813738153, - "heading": -0.3185535969362317, - "angularVelocity": -2.75479306959021e-7, - "velocityX": -3.1688295388681516, - "velocityY": 2.0643900898393674, - "timestamp": 5.727353654754491 - }, - { - "x": 3.509285404964888, - "y": 2.2879634464070207, - "heading": -0.3579155843505842, - "angularVelocity": -0.6181310446354924, - "velocityX": -2.996824737880426, - "velocityY": 1.9877599577609337, - "timestamp": 5.791032688117982 - }, - { - "x": 3.332909755538667, - "y": 2.4050971261724468, - "heading": -0.4129449232759771, - "angularVelocity": -0.8641673093760197, - "velocityX": -2.769760156682042, - "velocityY": 1.8394387222684185, - "timestamp": 5.854711721481472 - }, - { - "x": 3.171287726021972, - "y": 2.512479984884094, - "heading": -0.4699204658498491, - "angularVelocity": -0.8947300165290918, - "velocityX": -2.5380729100288124, - "velocityY": 1.6863142079856834, - "timestamp": 5.918390754844962 - }, - { - "x": 3.024408806786741, - "y": 2.6100923699912792, - "heading": -0.5252799185719804, - "angularVelocity": -0.8693513358805374, - "velocityX": -2.306550704009953, - "velocityY": 1.5328810748429236, - "timestamp": 5.982069788208452 - }, - { - "x": 2.892255079193046, - "y": 2.6979336450739244, - "heading": -0.5773440254447586, - "angularVelocity": -0.8176020288434336, - "velocityX": -2.0753098879397367, - "velocityY": 1.3794379475145813, - "timestamp": 6.045748821571943 - }, - { - "x": 2.7748123815012584, - "y": 2.7760062309054065, - "heading": -0.625128837055519, - "angularVelocity": -0.7504010203483636, - "velocityX": -1.8442914643726742, - "velocityY": 1.2260328354205916, - "timestamp": 6.109427854935433 - }, - { - "x": 2.6720699818921556, - "y": 2.844312916696855, - "heading": -0.6679860381773631, - "angularVelocity": -0.6730190277419643, - "velocityX": -1.6134415706757543, - "velocityY": 1.0726715244175122, - "timestamp": 6.173106888298923 - }, - { - "x": 2.5840196243973645, - "y": 2.9028562717726505, - "heading": -0.7054558016839698, - "angularVelocity": -0.5884160221579265, - "velocityX": -1.382721326691407, - "velocityY": 0.9193505614575078, - "timestamp": 6.236785921662413 - }, - { - "x": 2.510654811151598, - "y": 2.95163853061359, - "heading": -0.7371951288142529, - "angularVelocity": -0.4984266477336408, - "velocityX": -1.1521031235978156, - "velocityY": 0.7660646882386306, - "timestamp": 6.3004649550259035 - }, - { - "x": 2.4519703126340358, - "y": 2.9906615990271557, - "heading": -0.7629386573341134, - "angularVelocity": -0.4042700895428552, - "velocityX": -0.9215670436230066, - "velocityY": 0.6128087433553882, - "timestamp": 6.364143988389394 - }, - { - "x": 2.4079618327799786, - "y": 3.0199270886206313, - "heading": -0.7824753669479414, - "angularVelocity": -0.3067997201262361, - "velocityX": -0.6910984279998401, - "velocityY": 0.4595781067596215, - "timestamp": 6.427823021752884 - }, - { - "x": 2.378625774904473, - "y": 3.039436353320747, - "heading": -0.795633833561549, - "angularVelocity": -0.20663734856804464, - "velocityX": -0.4606862938394533, - "velocityY": 0.3063687318988399, - "timestamp": 6.491502055116374 + "heading": -0.348540530032263, + "angularVelocity": -5.763784949174497e-7, + "velocityX": -3.7819013522532754, + "velocityY": 0.02019321393800792, + "timestamp": 4.875191928937649 + }, + { + "x": 5.265773365103949, + "y": 1.585443267719361, + "heading": -0.3485405619317938, + "angularVelocity": -5.063689324990554e-7, + "velocityX": -3.7657613238076486, + "velocityY": 0.34961055977354083, + "timestamp": 4.9381885481486885 + }, + { + "x": 5.031364041570911, + "y": 1.6280522215500413, + "heading": -0.3485405890146467, + "angularVelocity": -4.299096234955573e-7, + "velocityX": -3.720982593490641, + "velocityY": 0.6763688966853497, + "timestamp": 5.001185167359728 + }, + { + "x": 4.801558310439742, + "y": 1.6909217775230576, + "heading": -0.3485406130254673, + "angularVelocity": -3.811445901784285e-7, + "velocityX": -3.6479057766785146, + "velocityY": 0.9979830149678419, + "timestamp": 5.064181786570768 + }, + { + "x": 4.57810382416931, + "y": 1.7735737496127562, + "heading": -0.34854063513060685, + "angularVelocity": -3.5089406173482316e-7, + "velocityX": -3.5470869559183034, + "velocityY": 1.3120064715347763, + "timestamp": 5.127178405781808 + }, + { + "x": 4.3626998372206165, + "y": 1.8753793427019887, + "heading": -0.34854065619045194, + "angularVelocity": -3.3430119484907394e-7, + "velocityX": -3.4192943946259384, + "velocityY": 1.6160485175907584, + "timestamp": 5.190175024992848 + }, + { + "x": 4.156979523974483, + "y": 1.9955559179970916, + "heading": -0.34854067691382595, + "angularVelocity": -3.2896009785914497e-7, + "velocityX": -3.265577039253029, + "velocityY": 1.907667058965506, + "timestamp": 5.253171644203888 + }, + { + "x": 3.9516956402233028, + "y": 2.116476485340398, + "heading": -0.3485406976311522, + "angularVelocity": -3.288640962267947e-7, + "velocityX": -3.258649215182055, + "velocityY": 1.9194770903216498, + "timestamp": 5.316168263414927 + }, + { + "x": 3.746411773108691, + "y": 2.2373970809271935, + "heading": -0.34854071834847844, + "angularVelocity": -3.288640958736373e-7, + "velocityX": -3.2586489510953776, + "velocityY": 1.9194775386550607, + "timestamp": 5.379164882625967 + }, + { + "x": 3.5411278789507334, + "y": 2.3583176306028646, + "heading": -0.34854073906621164, + "angularVelocity": -3.2887055643589624e-7, + "velocityX": -3.2586493803778747, + "velocityY": 1.919476809867947, + "timestamp": 5.442161501837007 + }, + { + "x": 3.345073520020233, + "y": 2.47307412374708, + "heading": -0.3873766723080469, + "angularVelocity": -0.6164764669629881, + "velocityX": -3.1121409590840567, + "velocityY": 1.821629391884994, + "timestamp": 5.505158121048047 + }, + { + "x": 3.1664636949700515, + "y": 2.5778338770119946, + "heading": -0.44780580157610866, + "angularVelocity": -0.9592440042793773, + "velocityX": -2.835228735876023, + "velocityY": 1.6629424654356146, + "timestamp": 5.568154740259087 + }, + { + "x": 3.0057996639738764, + "y": 2.672135804524931, + "heading": -0.5103083460228613, + "angularVelocity": -0.9921571225491842, + "velocityX": -2.5503595749788768, + "velocityY": 1.496936322837019, + "timestamp": 5.631151359470127 + }, + { + "x": 2.8630598597209174, + "y": 2.7559513152021253, + "heading": -0.5701467016897592, + "angularVelocity": -0.9498661422835671, + "velocityX": -2.265832770720234, + "velocityY": 1.3304763291568347, + "timestamp": 5.694147978681166 + }, + { + "x": 2.738214483951598, + "y": 2.829279302871158, + "heading": -0.6251012943842897, + "angularVelocity": -0.8723419348970285, + "velocityX": -1.9817789800923042, + "velocityY": 1.1639987762419943, + "timestamp": 5.757144597892206 + }, + { + "x": 2.6312405711995455, + "y": 2.892122971419519, + "heading": -0.6738749327170909, + "angularVelocity": -0.7742262829916016, + "velocityX": -1.698089740239665, + "velocityY": 0.9975720814133604, + "timestamp": 5.820141217103246 + }, + { + "x": 2.5421208919645824, + "y": 2.9444859859038597, + "heading": -0.7156160888836572, + "angularVelocity": -0.6625935913597552, + "velocityX": -1.414673999193671, + "velocityY": 0.8312035652091746, + "timestamp": 5.883137836314286 + }, + { + "x": 2.4708422721118457, + "y": 2.98637166116498, + "heading": -0.749723844710458, + "angularVelocity": -0.5414220041322991, + "velocityX": -1.1314673826217794, + "velocityY": 0.6648876683493641, + "timestamp": 5.946134455525326 + }, + { + "x": 2.4173943837507443, + "y": 3.0177828179574866, + "heading": -0.775752501403198, + "angularVelocity": -0.4131754532023893, + "velocityX": -0.84842470961893, + "velocityY": 0.4986165477750299, + "timestamp": 6.0091310747363655 + }, + { + "x": 2.3817689275168203, + "y": 3.038721799552771, + "heading": -0.7933591587514436, + "angularVelocity": -0.2794857496917854, + "velocityX": -0.565513779629629, + "velocityY": 0.3323826239808009, + "timestamp": 6.072127693947405 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.10425061542851642, - "velocityX": -0.2303222914890624, - "velocityY": 0.15317707264507022, - "timestamp": 6.5551810884798645 + "angularVelocity": -0.14148780267529462, + "velocityX": -0.28271125846882317, + "velocityY": 0.16617910323716242, + "timestamp": 6.135124313158445 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -1.4092752497411054e-30, - "velocityX": 2.9428006004895336e-31, - "velocityY": 5.314710200495422e-31, - "timestamp": 6.618860121843355 - }, - { - "x": 2.3794842314620843, - "y": 3.041727640405621, - "heading": -0.7962432666233688, - "angularVelocity": 0.0957711014436812, - "velocityX": 0.24661230413774035, - "velocityY": -0.11854554422657079, - "timestamp": 6.681813823589832 - }, - { - "x": 2.410536447869941, - "y": 3.0268006675090624, - "heading": -0.7842917338243374, - "angularVelocity": 0.18984638658997072, - "velocityX": 0.4932548134009415, - "velocityY": -0.23711032842312177, - "timestamp": 6.744767525336309 - }, - { - "x": 2.4571179241280796, - "y": 3.0044081651489933, - "heading": -0.7665449064601504, - "angularVelocity": 0.28190284084732253, - "velocityX": 0.7399322830248891, - "velocityY": -0.3556979452971155, - "timestamp": 6.807721227082786 - }, - { - "x": 2.5192312369840457, - "y": 2.9745484345249116, - "heading": -0.743155503226628, - "angularVelocity": 0.37153340605314533, - "velocityX": 0.9866506834833091, - "velocityY": -0.4743125470894598, - "timestamp": 6.8706749288292635 - }, - { - "x": 2.596879439737626, - "y": 2.937219467270397, - "heading": -0.714309410311632, - "angularVelocity": 0.4582112268975565, - "velocityX": 1.233417584660547, - "velocityY": -0.5929590511586358, - "timestamp": 6.933628630575741 - }, - { - "x": 2.690066196626848, - "y": 2.892418878795792, - "heading": -0.6802368283616581, - "angularVelocity": 0.5412323819683986, - "velocityX": 1.4802426911208155, - "velocityY": -0.7116434336939077, - "timestamp": 6.996582332322218 - }, - { - "x": 2.798795963694134, - "y": 2.8401438158547943, - "heading": -0.6412294413317386, - "angularVelocity": 0.6196202280051365, - "velocityX": 1.7271385804309867, - "velocityY": -0.8303731391605256, - "timestamp": 7.059536034068695 - }, - { - "x": 2.9230742308720847, - "y": 2.7803908299690017, - "heading": -0.5976682545562845, - "angularVelocity": 0.6919559226378926, - "velocityX": 1.9741216756154527, - "velocityY": -0.9491576226355342, - "timestamp": 7.122489735815172 - }, - { - "x": 3.062907829622796, - "y": 2.713155712528932, - "heading": -0.5500718966234338, - "angularVelocity": 0.7560533632244142, - "velocityX": 2.22121328645359, - "velocityY": -1.0680089585650514, - "timestamp": 7.185443437561649 - }, - { - "x": 3.218305230273769, - "y": 2.638433320090989, - "heading": -0.4991885737635272, - "angularVelocity": 0.8082657802208455, - "velocityX": 2.4684394458133507, - "velocityY": -1.1869419964986359, - "timestamp": 7.248397139308127 - }, - { - "x": 3.389276272924326, - "y": 2.556217612467452, - "heading": -0.4461960441931904, - "angularVelocity": 0.8417698737358584, - "velocityX": 2.715821912094713, - "velocityY": -1.3059709809381974, - "timestamp": 7.311350841054604 - }, - { - "x": 3.575827436112715, - "y": 2.4665034787202997, - "heading": -0.3932374103043789, - "angularVelocity": 0.841231451362194, - "velocityX": 2.9633072879440143, - "velocityY": -1.4250811510408536, - "timestamp": 7.374304542801081 - }, - { - "x": 3.777911234478158, - "y": 2.369308059755886, - "heading": -0.34555593776982274, - "angularVelocity": 0.7574053822375035, - "velocityX": 3.210038373585424, - "velocityY": -1.5439190431697394, - "timestamp": 7.437258244547558 - }, - { - "x": 3.9926293770053456, - "y": 2.266439040708753, - "heading": -0.3455557491739834, - "angularVelocity": 0.000002995786333953476, - "velocityX": 3.4107310066036556, - "velocityY": -1.6340424183696203, - "timestamp": 7.500211946294035 - }, - { - "x": 4.207200483864901, - "y": 2.1632635581376745, - "heading": -0.34555573252846533, - "angularVelocity": 2.644088848166298e-7, - "velocityX": 3.4083953906898548, - "velocityY": -1.6389104962656516, - "timestamp": 7.5631656480405125 - }, - { - "x": 4.421771581956281, - "y": 2.0600880573316713, - "heading": -0.3455557158829472, - "angularVelocity": 2.6440888572481287e-7, - "velocityX": 3.408395251410094, - "velocityY": -1.638910785921765, - "timestamp": 7.62611934978699 - }, - { - "x": 4.636342680047139, - "y": 1.9569125565245837, - "heading": -0.3455556992374292, - "angularVelocity": 2.6440888404451857e-7, - "velocityX": 3.4083952514018088, - "velocityY": -1.6389107859389946, - "timestamp": 7.689073051533467 - }, - { - "x": 4.850913778140846, - "y": 1.853737055723424, - "heading": -0.3455556825919111, - "angularVelocity": 2.64408884846605e-7, - "velocityX": 3.408395251447087, - "velocityY": -1.63891078584483, - "timestamp": 7.752026753279944 - }, - { - "x": 5.065484924140348, - "y": 1.7505616545506137, - "heading": -0.34555566594639303, - "angularVelocity": 2.644088848456774e-7, - "velocityX": 3.4083960124157384, - "velocityY": -1.6389092032794423, - "timestamp": 7.814980455026421 - }, - { - "x": 5.280845302209934, - "y": 1.6490439336616942, - "heading": -0.34555564929194366, - "angularVelocity": 2.6455075541297124e-7, - "velocityX": 3.4209327187283005, - "velocityY": -1.6125774668143436, - "timestamp": 7.877934156772898 + "angularVelocity": 1.2572572851920672e-28, + "velocityX": 2.1335040537014396e-30, + "velocityY": 1.8634721866519574e-29, + "timestamp": 6.198120932369485 + }, + { + "x": 2.384240228496088, + "y": 3.0394763007756747, + "heading": -0.7921549333865273, + "angularVelocity": 0.15389773911589558, + "velocityX": 0.30849818872977186, + "velocityY": -0.14776374894381028, + "timestamp": 6.263862499797187 + }, + { + "x": 2.4248103079231944, + "y": 3.020044385403153, + "heading": -0.7722377725963315, + "angularVelocity": 0.3029614529969845, + "velocityX": 0.6171145747585456, + "velocityY": -0.2955803479114183, + "timestamp": 6.329604067224889 + }, + { + "x": 2.485678594891513, + "y": 2.9908905714242224, + "heading": -0.7429157303412026, + "angularVelocity": 0.4460198228065572, + "velocityX": 0.9258721589693997, + "velocityY": -0.4434608896569423, + "timestamp": 6.395345634652592 + }, + { + "x": 2.5668563689761195, + "y": 2.952009793968192, + "heading": -0.7046902372675247, + "angularVelocity": 0.581450892781281, + "velocityX": 1.2348013176576613, + "velocityY": -0.5914184735371368, + "timestamp": 6.461087202080294 + }, + { + "x": 2.668357540158173, + "y": 2.903395952391573, + "heading": -0.6582160423239786, + "angularVelocity": 0.706922526522589, + "velocityX": 1.5439420621310505, + "velocityY": -0.7394688547710901, + "timestamp": 6.526828769507996 + }, + { + "x": 2.790199449693154, + "y": 2.8450417291439147, + "heading": -0.604379415056612, + "angularVelocity": 0.8189130465526614, + "velocityX": 1.8533465857651596, + "velocityY": -0.8876305438234711, + "timestamp": 6.592570336935698 + }, + { + "x": 2.9324037360004773, + "y": 2.7769385796688133, + "heading": -0.5444410242237507, + "angularVelocity": 0.9117274378767651, + "velocityX": 2.1630802530485744, + "velocityY": -1.0359222047754857, + "timestamp": 6.6583119043634005 + }, + { + "x": 3.094996415116772, + "y": 2.6990776165764148, + "heading": -0.4803328988113282, + "angularVelocity": 0.975153588829843, + "velocityX": 2.473209652250889, + "velocityY": -1.1843490524929314, + "timestamp": 6.724053471791103 + }, + { + "x": 3.2780012401054357, + "y": 2.6114549065561117, + "heading": -0.4154109268291881, + "angularVelocity": 0.9875330711202905, + "velocityX": 2.783700361113535, + "velocityY": -1.3328357300982285, + "timestamp": 6.789795039218805 + }, + { + "x": 3.48136962382921, + "y": 2.514109658078373, + "heading": -0.3571766392011983, + "angularVelocity": 0.8858061939583661, + "velocityX": 3.0934520073228393, + "velocityY": -1.4807260046665736, + "timestamp": 6.855536606646507 + }, + { + "x": 3.702896130222067, + "y": 2.408427187890042, + "heading": -0.3455558213194451, + "angularVelocity": 0.17676514778162136, + "velocityX": 3.3696565971974572, + "velocityY": -1.6075441204615863, + "timestamp": 6.921278174074209 + }, + { + "x": 3.927079400169571, + "y": 2.300911734169414, + "heading": -0.34555579773751244, + "angularVelocity": 3.5870657763866696e-7, + "velocityX": 3.4100688304100117, + "velocityY": -1.6354257728775006, + "timestamp": 6.9870197415019115 + }, + { + "x": 4.15126266204063, + "y": 2.1933962636083315, + "heading": -0.3455557741556376, + "angularVelocity": 3.587056984423895e-7, + "velocityX": 3.4100687075585983, + "velocityY": -1.6354260290389449, + "timestamp": 7.052761308929614 + }, + { + "x": 4.375445923911402, + "y": 2.0858807930466483, + "heading": -0.3455557505737627, + "angularVelocity": 3.5870569914452407e-7, + "velocityX": 3.4100687075542147, + "velocityY": -1.6354260290480847, + "timestamp": 7.118502876357316 + }, + { + "x": 4.599629185782173, + "y": 1.978365322484965, + "heading": -0.34555572699188775, + "angularVelocity": 3.5870569932883336e-7, + "velocityX": 3.4100687075542147, + "velocityY": -1.6354260290480853, + "timestamp": 7.184244443785018 + }, + { + "x": 4.823812447652946, + "y": 1.8708498519232832, + "heading": -0.3455557034100129, + "angularVelocity": 3.5870569853758494e-7, + "velocityX": 3.4100687075542258, + "velocityY": -1.6354260290480622, + "timestamp": 7.24998601121272 + }, + { + "x": 5.047995709543858, + "y": 1.7633343814035949, + "heading": -0.34555567982813806, + "angularVelocity": 3.5870569820699746e-7, + "velocityX": 3.410068707860571, + "velocityY": -1.6354260284092945, + "timestamp": 7.315727578640423 + }, + { + "x": 5.27217953587615, + "y": 1.65582008782303, + "heading": -0.34555565624625534, + "angularVelocity": 3.587058178727361e-7, + "velocityX": 3.410077293621499, + "velocityY": -1.6354081258984543, + "timestamp": 7.381469146068125 }, { "x": 5.5030035972595215, "y": 1.563418984413147, "heading": -0.34555563246426124, - "angularVelocity": 2.6730250915052296e-7, - "velocityX": 3.52891551864971, - "velocityY": -1.360125725304771, - "timestamp": 7.940887858519376 - }, - { - "x": 5.655413318775525, - "y": 1.5130951799644636, - "heading": -0.34555563491945995, - "angularVelocity": -5.785220393859716e-8, - "velocityX": 3.5912523855702734, - "velocityY": -1.18578710714545, - "timestamp": 7.983327014878419 - }, - { - "x": 5.810103118724588, - "y": 1.4702901058122384, - "heading": -0.3455556373457815, - "angularVelocity": -5.7171766263130695e-8, - "velocityX": 3.6449782045702985, - "velocityY": -1.0086221740622332, - "timestamp": 8.025766171237462 - }, - { - "x": 5.966313456902953, - "y": 1.4334181808859694, - "heading": -0.34555563976573417, - "angularVelocity": -5.702169541040829e-8, - "velocityX": 3.680806867525754, - "velocityY": -0.8688185178405917, - "timestamp": 8.068205327596505 - }, - { - "x": 6.1225244823663525, - "y": 1.3965491678093302, - "heading": -0.3455556421856862, - "angularVelocity": -5.7021679533312126e-8, - "velocityX": 3.680823062122731, - "velocityY": -0.868749905505203, - "timestamp": 8.110644483955548 - }, - { - "x": 6.278735507957822, - "y": 1.3596801552753135, - "heading": -0.3455556446056381, - "angularVelocity": -5.702167859922657e-8, - "velocityX": 3.680823065140465, - "velocityY": -0.8687498927193055, - "timestamp": 8.153083640314591 - }, - { - "x": 6.434946533537413, - "y": 1.3228111426909752, - "heading": -0.34555564702559005, - "angularVelocity": -5.7021678932866206e-8, - "velocityX": 3.680823064860606, - "velocityY": -0.868749893905046, - "timestamp": 8.195522796673634 - }, - { - "x": 6.591157495254782, - "y": 1.285941859528607, - "heading": -0.3455556494455419, - "angularVelocity": -5.7021677677252914e-8, - "velocityX": 3.6808215600657594, - "velocityY": -0.8687562695744264, - "timestamp": 8.237961953032677 - }, - { - "x": 6.747031505398479, - "y": 1.2476730535838276, - "heading": -0.34555565194208476, - "angularVelocity": -5.8826401859372655e-8, - "velocityX": 3.6728819212374164, - "velocityY": -0.9017334279932018, - "timestamp": 8.28040110939172 - }, - { - "x": 6.897830261276489, - "y": 1.2034176695315386, - "heading": -0.3468060133183351, - "angularVelocity": -0.029462446559305244, - "velocityX": 3.5532929684610997, - "velocityY": -1.0427960367044076, - "timestamp": 8.322840265750763 - }, - { - "x": 7.040966542913821, - "y": 1.160898981796106, - "heading": -0.34756195184747823, - "angularVelocity": -0.017812289262957588, - "velocityX": 3.3727409759603195, - "velocityY": -1.001874009363348, - "timestamp": 8.365279422109806 - }, - { - "x": 7.176448137174031, - "y": 1.12015095875213, - "heading": -0.34781665101790943, - "angularVelocity": -0.006001513514462224, - "velocityX": 3.1923724664555144, - "velocityY": -0.9601515802821338, - "timestamp": 8.407718578468849 - }, - { - "x": 7.30427768133993, - "y": 1.0811849503221362, - "heading": -0.3475678387477372, - "angularVelocity": 0.005862799629361019, - "velocityX": 3.0120660996283153, - "velocityY": -0.918161711329369, - "timestamp": 8.450157734827892 - }, - { - "x": 7.424456501916377, - "y": 1.044006636667021, - "heading": -0.3468143875407301, - "angularVelocity": 0.017753680130510166, - "velocityX": 2.8317909894275823, - "velocityY": -0.8760379999211017, - "timestamp": 8.492596891186935 + "angularVelocity": 3.617497277100986e-7, + "velocityX": 3.511082415812739, + "velocityY": -1.405520236667608, + "timestamp": 7.447210713495827 + }, + { + "x": 5.6599765905642645, + "y": 1.5113593672660246, + "heading": -0.34555563704903824, + "angularVelocity": -1.0484559207434358e-7, + "velocityX": 3.589689630249378, + "velocityY": -1.1905096787253004, + "timestamp": 7.490939561801255 + }, + { + "x": 5.819811381479902, + "y": 1.4688909784273936, + "heading": -0.3455556415836398, + "angularVelocity": -1.0369817036745698e-7, + "velocityX": 3.6551337871799947, + "velocityY": -0.9711755622285476, + "timestamp": 7.534668410106684 + }, + { + "x": 5.98037051193366, + "y": 1.4292489543347602, + "heading": -0.34555564611341977, + "angularVelocity": -1.0358790889899858e-7, + "velocityX": 3.6716981277969567, + "velocityY": -0.9065416910994355, + "timestamp": 7.578397258412112 + }, + { + "x": 6.140929731813359, + "y": 1.389607292439096, + "heading": -0.3455556506431997, + "angularVelocity": -1.0358790900922708e-7, + "velocityX": 3.6717001728071876, + "velocityY": -0.9065334083071072, + "timestamp": 7.62212610671754 + }, + { + "x": 6.30148895170346, + "y": 1.3499656305855623, + "heading": -0.34555565517297965, + "angularVelocity": -1.0358790856487398e-7, + "velocityX": 3.6717001730450596, + "velocityY": -0.9065334073436624, + "timestamp": 7.665854955022969 + }, + { + "x": 6.462048171593562, + "y": 1.3103239687320334, + "heading": -0.34555565970275964, + "angularVelocity": -1.0358791005302326e-7, + "velocityX": 3.671700173045087, + "velocityY": -0.9065334073435504, + "timestamp": 7.709583803328397 + }, + { + "x": 6.622607391483664, + "y": 1.2706823068785016, + "heading": -0.34555566423253964, + "angularVelocity": -1.0358790893463982e-7, + "velocityX": 3.6717001730450707, + "velocityY": -0.9065334073436169, + "timestamp": 7.753312651633825 + }, + { + "x": 6.783166611367605, + "y": 1.2310406450000155, + "heading": -0.3455556687623196, + "angularVelocity": -1.0358790885289903e-7, + "velocityX": 3.6717001729041776, + "velocityY": -0.9065334079142718, + "timestamp": 7.797041499939254 + }, + { + "x": 6.943725778284007, + "y": 1.1913987685896004, + "heading": -0.34555567329209946, + "angularVelocity": -1.0358790829702175e-7, + "velocityX": 3.6716989616319724, + "velocityY": -0.9065383138730976, + "timestamp": 7.840770348244682 + }, + { + "x": 7.103836002524736, + "y": 1.1499808922348331, + "heading": -0.34555567791225333, + "angularVelocity": -1.0565459632350647e-7, + "velocityX": 3.661432451237353, + "velocityY": -0.9471522338178334, + "timestamp": 7.88449919655011 + }, + { + "x": 7.257955684496521, + "y": 1.1005509138662795, + "heading": -0.346747423550466, + "angularVelocity": -0.02725307627333064, + "velocityX": 3.524439539210384, + "velocityY": -1.1303745761449258, + "timestamp": 7.928228044855539 + }, + { + "x": 7.402334567868656, + "y": 1.0534126604700325, + "heading": -0.34675046354447403, + "angularVelocity": -0.00006951918758018307, + "velocityX": 3.3016850195483918, + "velocityY": -1.07796695369166, + "timestamp": 7.971956893160967 }, { "x": 7.536985397338867, "y": 1.0086194276809692, "heading": -0.34555563246426124, - "angularVelocity": 0.02966022853563592, - "velocityX": 2.65153469287831, - "velocityY": -0.8338339406813241, - "timestamp": 8.535036047545978 - }, - { - "x": 7.689789531297121, - "y": 0.9591642629851495, - "heading": -0.34249163023542684, - "angularVelocity": 0.04769743470803589, - "velocityX": 2.378707539440784, - "velocityY": -0.769870356769085, - "timestamp": 8.599274349827635 - }, - { - "x": 7.82516177952376, - "y": 0.9142093996063804, - "heading": -0.3392101704632614, - "angularVelocity": 0.05108260423473965, - "velocityX": 2.1073447369933547, - "velocityY": -0.6998140016475143, - "timestamp": 8.663512652109292 - }, - { - "x": 7.943169958712402, - "y": 0.8740090692537413, - "heading": -0.33632582750820933, - "angularVelocity": 0.044900672225200826, - "velocityX": 1.837037639494695, - "velocityY": -0.6258000122166694, - "timestamp": 8.727750954390949 - }, - { - "x": 8.043864833075729, - "y": 0.8387415563520763, - "heading": -0.3342715201633078, - "angularVelocity": 0.03197947753809357, - "velocityX": 1.567520790350633, - "velocityY": -0.5490106626266708, - "timestamp": 8.791989256672606 - }, - { - "x": 8.12728568078066, - "y": 0.8085387560486261, - "heading": -0.33336873570137193, - "angularVelocity": 0.014053678722353937, - "velocityX": 1.2986153858669556, - "velocityY": -0.4701680964578417, - "timestamp": 8.856227558954263 - }, - { - "x": 8.193463737140812, - "y": 0.7835021726798679, - "heading": -0.33386568134146205, - "angularVelocity": -0.007735970946293819, - "velocityX": 1.030196222652151, - "velocityY": -0.38974540857234485, - "timestamp": 8.92046586123592 - }, - { - "x": 8.242424408463135, - "y": 0.7637123264900073, - "heading": -0.33595980135953535, - "angularVelocity": -0.03259924287680334, - "velocityX": 0.7621725603464993, - "velocityY": -0.3080692591016908, - "timestamp": 8.984704163517577 - }, - { - "x": 8.274188750584008, - "y": 0.7492346436854774, - "heading": -0.3398119288829893, - "angularVelocity": -0.05996620998114242, - "velocityX": 0.4944766750154696, - "velocityY": -0.2253746174836856, - "timestamp": 9.048942465799234 - }, - { - "x": 8.288774490356445, - "y": 0.7401233315467834, + "angularVelocity": 0.027323634774631837, + "velocityX": 3.079221948168643, + "velocityY": -1.024340556060407, + "timestamp": 8.015685741466395 + }, + { + "x": 7.709533523799992, + "y": 0.9494390432841444, + "heading": -0.3413965784412158, + "angularVelocity": 0.06656216442856697, + "velocityX": 2.7614877570011993, + "velocityY": -0.9471323179117597, + "timestamp": 8.078169497196857 + }, + { + "x": 7.862380855125707, + "y": 0.8957055369328039, + "heading": -0.33682515317576994, + "angularVelocity": 0.07316181961221624, + "velocityX": 2.4461930871290236, + "velocityY": -0.8599596122731851, + "timestamp": 8.140653252927319 + }, + { + "x": 7.99561550345175, + "y": 0.8477270543434197, + "heading": -0.33285887044966567, + "angularVelocity": 0.06347702182330028, + "velocityX": 2.1323085779411386, + "velocityY": -0.7678552934037809, + "timestamp": 8.20313700865778 + }, + { + "x": 8.109294083490143, + "y": 0.8056872786590918, + "heading": -0.33010810532148027, + "angularVelocity": 0.04402368417243478, + "velocityX": 1.8193301396409471, + "velocityY": -0.6728112801937814, + "timestamp": 8.265620764388242 + }, + { + "x": 8.20345588526635, + "y": 0.7697081195865146, + "heading": -0.32897987990878186, + "angularVelocity": 0.018056299585531694, + "velocityX": 1.5069805051795295, + "velocityY": -0.5758162045793389, + "timestamp": 8.328104520118703 + }, + { + "x": 8.278129725528439, + "y": 0.7398763726381793, + "heading": -0.32976503867911666, + "angularVelocity": -0.012565806282865027, + "velocityX": 1.1950920585537632, + "velocityY": -0.4774320397291953, + "timestamp": 8.390588275849165 + }, + { + "x": 8.333337625588607, + "y": 0.7162569714576368, + "heading": -0.33268181216540116, + "angularVelocity": -0.04668050843272986, + "velocityX": 0.883556044523325, + "velocityY": -0.37800866648333437, + "timestamp": 8.453072031579627 + }, + { + "x": 8.369096953381794, + "y": 0.698900317224484, + "heading": -0.33790002427933163, + "angularVelocity": -0.08351309957167831, + "velocityX": 0.572297989695798, + "velocityY": -0.2777786647144699, + "timestamp": 8.515555787310088 + }, + { + "x": 8.385421752929688, + "y": 0.6878466606140137, "heading": -0.34555563246426124, - "angularVelocity": -0.08941244362418393, - "velocityX": 0.2270567442533848, - "velocityY": -0.14183612914838375, - "timestamp": 9.113180768080891 - }, - { - "x": 8.286058974983396, - "y": 0.7364361738247144, - "heading": -0.3533713967772531, - "angularVelocity": -0.1208300500196775, - "velocityX": -0.04198128874091145, - "velocityY": -0.05700267231030528, - "timestamp": 9.177864713107141 - }, - { - "x": 8.26594084231072, - "y": 0.7382366486349878, - "heading": -0.3631834148956511, - "angularVelocity": -0.15169170826572306, - "velocityX": -0.3110220420927156, - "velocityY": 0.027834956719829915, - "timestamp": 9.24254865813339 - }, - { - "x": 8.22841989282902, - "y": 0.7455250700569162, - "heading": -0.3749497746847549, - "angularVelocity": -0.18190541384463682, - "velocityX": -0.5800658798172126, - "velocityY": 0.11267744134917045, - "timestamp": 9.30723260315964 - }, - { - "x": 8.173495898954863, - "y": 0.7583018091084519, - "heading": -0.38862097650887273, - "angularVelocity": -0.21135386560868902, - "velocityX": -0.8491132359330754, - "velocityY": 0.1975256618369646, - "timestamp": 9.37191654818589 - }, - { - "x": 8.101168599327213, - "y": 0.7765673113174031, - "heading": -0.4041376276128682, - "angularVelocity": -0.23988411804039605, - "velocityX": -1.11816463263485, - "velocityY": 0.2823807700896826, - "timestamp": 9.43660049321214 - }, - { - "x": 8.011437691690773, - "y": 0.8003221216608263, - "heading": -0.42142708913788907, - "angularVelocity": -0.26729138920027884, - "velocityX": -1.3872207021390834, - "velocityY": 0.36724430357151194, - "timestamp": 9.50128443823839 - }, - { - "x": 7.904302824322884, - "y": 0.8295669211666158, - "heading": -0.44039841224325665, - "angularVelocity": -0.29329261067284323, - "velocityX": -1.6562822092006166, - "velocityY": 0.45211836559939883, - "timestamp": 9.56596838326464 - }, - { - "x": 7.779763586783285, - "y": 0.8643025828479174, - "heading": -0.46093432615418145, - "angularVelocity": -0.317480850968364, - "velocityX": -1.9253500615811048, - "velocityY": 0.5370059242243979, - "timestamp": 9.63065232829089 - }, - { - "x": 7.63781950374517, - "y": 0.9045302615405436, - "heading": -0.48287779045070706, - "angularVelocity": -0.33924127985113656, - "velocityX": -2.194425262412658, - "velocityY": 0.6219113363648586, - "timestamp": 9.695336273317139 - }, - { - "x": 7.478470046612931, - "y": 0.9502515474489494, - "heading": -0.5060075980261466, - "angularVelocity": -0.3575818940241346, - "velocityX": -2.4635086352196294, - "velocityY": 0.7068413327271726, - "timestamp": 9.760020218343389 - }, - { - "x": 7.301714722310279, - "y": 1.0014687500113455, - "heading": -0.5299890975728885, - "angularVelocity": -0.37074887032647375, - "velocityX": -2.732599630880914, - "velocityY": 0.7918070325118697, - "timestamp": 9.824704163369638 - }, - { - "x": 7.107553521342319, - "y": 1.0581854755780744, - "heading": -0.5542575516052335, - "angularVelocity": -0.37518512549746846, - "velocityX": -3.0016907733312475, - "velocityY": 0.8768284857040194, - "timestamp": 9.889388108395888 - }, - { - "x": 6.895989597473325, - "y": 1.1204078946209601, - "heading": -0.5776591781660143, - "angularVelocity": -0.3617841575878507, - "velocityX": -3.2707331592582385, - "velocityY": 0.961945333075069, - "timestamp": 9.954072053422138 - }, - { - "x": 6.667061067423103, - "y": 1.188145012147935, - "heading": -0.5965312726664429, - "angularVelocity": -0.2917585575952417, - "velocityX": -3.5391862688232427, - "velocityY": 1.0472013959489617, - "timestamp": 10.018755998448388 - }, - { - "x": 6.433742027972299, - "y": 1.2616769248691375, - "heading": -0.5965312781733252, - "angularVelocity": -8.513522502240808e-8, - "velocityX": -3.607062608134345, - "velocityY": 1.136787694247182, - "timestamp": 10.083439943474637 - }, - { - "x": 6.200423078644191, - "y": 1.3352091235520722, - "heading": -0.5965312836800714, - "angularVelocity": -8.513312137357674e-8, - "velocityX": -3.6070612148566714, - "velocityY": 1.1367921151546025, - "timestamp": 10.148123888500887 - }, - { - "x": 5.967104129330135, - "y": 1.4087413222795935, - "heading": -0.5965312891868175, - "angularVelocity": -8.513312220445073e-8, - "velocityX": -3.607061214639434, - "velocityY": 1.136792115843899, - "timestamp": 10.212807833527137 - }, - { - "x": 5.733785345709214, - "y": 1.4822740467522528, - "heading": -0.5965312946935636, - "angularVelocity": -8.513311865132804e-8, - "velocityX": -3.6070586530588855, - "velocityY": 1.1368002437516491, - "timestamp": 10.277491778553387 + "angularVelocity": -0.12252157533477633, + "velocityX": 0.26126469763300175, + "velocityY": -0.1769044847136382, + "timestamp": 8.57803954304055 + }, + { + "x": 8.379560991493735, + "y": 0.6833892008568203, + "heading": -0.35721868503670856, + "angularVelocity": -0.1678763417451177, + "velocityX": -0.08435897751441682, + "velocityY": -0.06416005011256522, + "timestamp": 8.64751361217505 + }, + { + "x": 8.349687812290387, + "y": 0.6867654591427064, + "heading": -0.37193021085772177, + "angularVelocity": -0.2117556378126066, + "velocityX": -0.42999034856465757, + "velocityY": 0.048597387887987537, + "timestamp": 8.71698768130955 + }, + { + "x": 8.295801597591089, + "y": 0.6979765367761911, + "heading": -0.38956490915625874, + "angularVelocity": -0.253831372168465, + "velocityX": -0.7756306111129838, + "velocityY": 0.1613706779112114, + "timestamp": 8.786461750444051 + }, + { + "x": 8.217901630671626, + "y": 0.7170238074049998, + "heading": -0.40996601054380316, + "angularVelocity": -0.2936505899495876, + "velocityX": -1.121281190089058, + "velocityY": 0.2741637400269978, + "timestamp": 8.855935819578551 + }, + { + "x": 8.115987079646073, + "y": 0.7439090305870278, + "heading": -0.4329315185089462, + "angularVelocity": -0.3305622982969707, + "velocityX": -1.4669437431144206, + "velocityY": 0.3869821289721611, + "timestamp": 8.925409888713052 + }, + { + "x": 7.990056990056933, + "y": 0.778634533730576, + "heading": -0.45819105339643357, + "angularVelocity": -0.36358219983610734, + "velocityX": -1.812620034466978, + "velocityY": 0.4998340183057432, + "timestamp": 8.994883957847552 + }, + { + "x": 7.840110319883288, + "y": 0.8212035218059903, + "heading": -0.4853637226253089, + "angularVelocity": -0.3911195870256229, + "velocityX": -2.1583113245224226, + "velocityY": 0.6127320395326444, + "timestamp": 9.064358026982053 + }, + { + "x": 7.6661461501938755, + "y": 0.8716206459377261, + "heading": -0.5138727598830971, + "angularVelocity": -0.4103550808661484, + "velocityX": -2.504015841545443, + "velocityY": 0.7256970083921437, + "timestamp": 9.133832096116553 + }, + { + "x": 7.468164698581641, + "y": 0.9298931437973682, + "heading": -0.5427426487462225, + "angularVelocity": -0.41554912822558204, + "velocityX": -2.8497172265661783, + "velocityY": 0.8387661552805812, + "timestamp": 9.203306165251053 + }, + { + "x": 7.24617329762414, + "y": 0.9960332266921984, + "heading": -0.5699703376985038, + "angularVelocity": -0.391911533201964, + "velocityX": -3.1953130674947268, + "velocityY": 0.9520110700121011, + "timestamp": 9.272780234385554 + }, + { + "x": 7.00025853984304, + "y": 1.0700564595014088, + "heading": -0.589089272180988, + "angularVelocity": -0.2751952594783323, + "velocityX": -3.53966250782022, + "velocityY": 1.065480023430083, + "timestamp": 9.342254303520054 + }, + { + "x": 6.750442302486931, + "y": 1.1514706650489683, + "heading": -0.5890892788822688, + "angularVelocity": -9.645729514768673e-8, + "velocityX": -3.5958198572257243, + "velocityY": 1.171864647656428, + "timestamp": 9.411728372654554 + }, + { + "x": 6.500626129709104, + "y": 1.232885068752249, + "heading": -0.5890892855834843, + "angularVelocity": -9.645635490096592e-8, + "velocityX": -3.5958189276949932, + "velocityY": 1.1718674998820673, + "timestamp": 9.481202441789055 + }, + { + "x": 6.250809956933241, + "y": 1.3142994724615544, + "heading": -0.5890892922846998, + "angularVelocity": -9.645635424551899e-8, + "velocityX": -3.595818927666732, + "velocityY": 1.1718674999687875, + "timestamp": 9.550676510923555 + }, + { + "x": 6.000993784158921, + "y": 1.395713876175591, + "heading": -0.589089298985915, + "angularVelocity": -9.645635348778811e-8, + "velocityX": -3.595818927644538, + "velocityY": 1.1718675000368866, + "timestamp": 9.620150580058056 + }, + { + "x": 5.751177662095486, + "y": 1.477128435493372, + "heading": -0.5890893056871302, + "angularVelocity": -9.64563521650664e-8, + "velocityX": -3.5958181977191597, + "velocityY": 1.1718697397753421, + "timestamp": 9.689624649192556 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.5965313002103377, - "angularVelocity": -8.528815258110653e-8, - "velocityX": -3.567836630187545, - "velocityY": 1.254483436778072, - "timestamp": 10.342175723579636 - }, - { - "x": 5.285431093647359, - "y": 1.6581112689784743, - "heading": -0.5965313049795081, - "angularVelocity": -7.601302145092289e-8, - "velocityX": -3.4677610362467894, - "velocityY": 1.509245007513348, - "timestamp": 10.404917216242477 - }, - { - "x": 5.070948144949037, - "y": 1.7596075143791599, - "heading": -0.5965313097283005, - "angularVelocity": -7.568822719486902e-8, - "velocityX": -3.4185184252932688, - "velocityY": 1.6176893646140194, - "timestamp": 10.467658708905319 - }, - { - "x": 4.856465419222307, - "y": 1.861104230964457, - "heading": -0.5965313144770923, - "angularVelocity": -7.568821760685447e-8, - "velocityX": -3.4185148714793154, - "velocityY": 1.6176968745502942, - "timestamp": 10.53040020156816 - }, - { - "x": 4.641982693508075, - "y": 1.9626009475761665, - "heading": -0.5965313192258841, - "angularVelocity": -7.568821790279648e-8, - "velocityX": -3.418514871280104, - "velocityY": 1.6176968749712675, - "timestamp": 10.593141694231 - }, - { - "x": 4.427499967793844, - "y": 2.064097664187878, - "heading": -0.5965313239746759, - "angularVelocity": -7.568821822441872e-8, - "velocityX": -3.4185148712800926, - "velocityY": 1.617696874971291, - "timestamp": 10.655883186893842 - }, - { - "x": 4.213017242079613, - "y": 2.165594380799589, - "heading": -0.5965313287234677, - "angularVelocity": -7.568821811615467e-8, - "velocityX": -3.4185148712800917, - "velocityY": 1.617696874971293, - "timestamp": 10.718624679556683 - }, - { - "x": 3.998534516366738, - "y": 2.267091097414165, - "heading": -0.5965313334722594, - "angularVelocity": -7.56882168459371e-8, - "velocityX": -3.418514871258486, - "velocityY": 1.6176968750169503, - "timestamp": 10.781366172219524 - }, - { - "x": 3.7840518148370346, - "y": 2.3685878651306878, - "heading": -0.5965313382224369, - "angularVelocity": -7.571030356184493e-8, - "velocityX": -3.418514485817036, - "velocityY": 1.6176976895010247, - "timestamp": 10.844107664882365 - }, - { - "x": 3.5823467745214304, - "y": 2.46403001529431, - "heading": -0.6258886394381601, - "angularVelocity": -0.46790887449049295, - "velocityX": -3.214858808022383, - "velocityY": 1.5211966772373215, - "timestamp": 10.906849157545206 - }, - { - "x": 3.3961574491843427, - "y": 2.552130310684733, - "heading": -0.6529986278149837, - "angularVelocity": -0.43209026795882993, - "velocityX": -2.967562890759225, - "velocityY": 1.40417914288166, - "timestamp": 10.969590650208048 - }, - { - "x": 3.225483861981861, - "y": 2.6328887858865286, - "heading": -0.6778585687680168, - "angularVelocity": -0.3962280764760425, - "velocityX": -2.7202666044246855, - "velocityY": 1.2871621597492864, - "timestamp": 11.032332142870889 - }, - { - "x": 3.0703260256030673, - "y": 2.7063054766361296, - "heading": -0.7004659993691003, - "angularVelocity": -0.36032662982009556, - "velocityX": -2.4729701158463, - "velocityY": 1.17014574619904, - "timestamp": 11.09507363553373 - }, - { - "x": 2.9306839491479293, - "y": 2.7723804156875507, - "heading": -0.7208186227896802, - "angularVelocity": -0.32438857535555815, - "velocityX": -2.2256734822288196, - "velocityY": 1.0531298546958971, - "timestamp": 11.157815128196571 - }, - { - "x": 2.8065576397395007, - "y": 2.8311136320269066, - "heading": -0.7389143343573537, - "angularVelocity": -0.28841697574707026, - "velocityX": -1.9783767350811556, - "velocityY": 0.9361144251855119, - "timestamp": 11.220556620859412 - }, - { - "x": 2.697947103240289, - "y": 2.8825051506567756, - "heading": -0.7547512635952348, - "angularVelocity": -0.25241556370017226, - "velocityX": -1.7310798944944172, - "velocityY": 0.8190993941766099, - "timestamp": 11.283298113522253 - }, - { - "x": 2.6048523446905505, - "y": 2.9265549925489487, - "heading": -0.7683278153992709, - "angularVelocity": -0.21638872822158642, - "velocityX": -1.4837829735739574, - "velocityY": 0.702084697424841, - "timestamp": 11.346039606185094 - }, - { - "x": 2.527273368620795, - "y": 2.9632631746604594, - "heading": -0.779642706302846, - "angularVelocity": -0.18034143631837077, - "velocityX": -1.2364859804443729, - "velocityY": 0.5850702709413225, - "timestamp": 11.408781098847935 - }, - { - "x": 2.4652101792880994, - "y": 2.9926297099741825, - "heading": -0.7886949946743065, - "angularVelocity": -0.1442791362982944, - "velocityX": -0.9891889194637066, - "velocityY": 0.46805605138424683, - "timestamp": 11.471522591510777 - }, - { - "x": 2.4186627808554446, - "y": 3.014654607545873, - "heading": -0.7954841045559687, - "angularVelocity": -0.10820765642515946, - "velocityX": -0.7418917921317392, - "velocityY": 0.3510419761615724, - "timestamp": 11.534264084173618 - }, - { - "x": 2.3876311775224326, - "y": 3.0293378725476474, - "heading": -0.8000098431420335, - "angularVelocity": -0.07213310353301693, - "velocityX": -0.49459459786475296, - "velocityY": 0.23402798337424993, - "timestamp": 11.597005576836459 + "heading": -0.5890893123960679, + "angularVelocity": -9.65675052838285e-8, + "velocityX": -3.5721826564600034, + "velocityY": 1.242054049730676, + "timestamp": 9.759098718327056 + }, + { + "x": 5.287967104202961, + "y": 1.6596756394253018, + "heading": -0.5890893181956361, + "angularVelocity": -9.309834476919773e-8, + "velocityX": -3.4519020760085812, + "velocityY": 1.5451728334255468, + "timestamp": 9.821393791687077 + }, + { + "x": 5.074932733784246, + "y": 1.7602857483579184, + "heading": -0.5890893239791856, + "angularVelocity": -9.284120212486628e-8, + "velocityX": -3.419762734486735, + "velocityY": 1.6150572349624415, + "timestamp": 9.883688865047098 + }, + { + "x": 4.861898448797989, + "y": 1.860896038187167, + "heading": -0.5890893297627348, + "angularVelocity": -9.284119839047264e-8, + "velocityX": -3.4197613630707817, + "velocityY": 1.615060138829829, + "timestamp": 9.94598393840712 + }, + { + "x": 4.648864163815087, + "y": 1.9615063280235205, + "heading": -0.5890893355462841, + "angularVelocity": -9.28411976539737e-8, + "velocityX": -3.419761363016917, + "velocityY": 1.6150601389438841, + "timestamp": 10.00827901176714 + }, + { + "x": 4.435829878832186, + "y": 2.0621166178598744, + "heading": -0.5890893413298333, + "angularVelocity": -9.28411982771023e-8, + "velocityX": -3.419761363016914, + "velocityY": 1.6150601389438883, + "timestamp": 10.070574085127161 + }, + { + "x": 4.222795593849285, + "y": 2.162726907696228, + "heading": -0.5890893471133825, + "angularVelocity": -9.284119843074624e-8, + "velocityX": -3.4197613630169146, + "velocityY": 1.6150601389438883, + "timestamp": 10.132869158487182 + }, + { + "x": 4.009761308866383, + "y": 2.2633371975325822, + "heading": -0.5890893528969318, + "angularVelocity": -9.284119832193768e-8, + "velocityX": -3.419761363016914, + "velocityY": 1.6150601389438886, + "timestamp": 10.195164231847203 + }, + { + "x": 3.7967270238835944, + "y": 2.363947487369175, + "heading": -0.589089358680481, + "angularVelocity": -9.284119741513914e-8, + "velocityX": -3.4197613630151045, + "velocityY": 1.615060138947721, + "timestamp": 10.257459305207224 + }, + { + "x": 3.583692741771847, + "y": 2.464557783284562, + "heading": -0.5890893644643718, + "angularVelocity": -9.284668184194445e-8, + "velocityX": -3.419761316927328, + "velocityY": 1.615060236528384, + "timestamp": 10.319754378567245 + }, + { + "x": 3.381763406617466, + "y": 2.559912098840239, + "heading": -0.6245815423960026, + "angularVelocity": -0.5697429349912143, + "velocityX": -3.241497670086629, + "velocityY": 1.530687908570203, + "timestamp": 10.382049451927266 + }, + { + "x": 3.1981911985495857, + "y": 2.646597580557341, + "heading": -0.6568615726262876, + "angularVelocity": -0.5181795042399261, + "velocityX": -2.9468174314037126, + "velocityY": 1.3915302935133123, + "timestamp": 10.444344525287287 + }, + { + "x": 3.032976151851517, + "y": 2.7246143065734305, + "heading": -0.6859252496642307, + "angularVelocity": -0.46654856428174046, + "velocityX": -2.652136642383354, + "velocityY": 1.2523739327700791, + "timestamp": 10.506639598647308 + }, + { + "x": 2.8861182875087947, + "y": 2.793962348685928, + "heading": -0.7117686923384959, + "angularVelocity": -0.414855321301394, + "velocityX": -2.3574555164898965, + "velocityY": 1.1132187245645664, + "timestamp": 10.568934672007329 + }, + { + "x": 2.7576176211174075, + "y": 2.854641769350887, + "heading": -0.7343883425051251, + "angularVelocity": -0.3631049607390947, + "velocityX": -2.0627741402397475, + "velocityY": 0.9740645189431933, + "timestamp": 10.63122974536735 + }, + { + "x": 2.6474741650844584, + "y": 2.906652621199817, + "heading": -0.7537810666899133, + "angularVelocity": -0.31130429966287193, + "velocityX": -1.7680925648228973, + "velocityY": 0.8349111581961731, + "timestamp": 10.69352481872737 + }, + { + "x": 2.5556879298075064, + "y": 2.9499949470036255, + "heading": -0.7699442582183806, + "angularVelocity": -0.2594617945957906, + "velocityX": -1.473410822497863, + "velocityY": 0.6957584840348696, + "timestamp": 10.755819892087391 + }, + { + "x": 2.482258924484474, + "y": 2.9846687797700127, + "heading": -0.7828759248842103, + "angularVelocity": -0.20758730937026382, + "velocityX": -1.1787289325219326, + "velocityY": 0.556606339734085, + "timestamp": 10.818114965447412 + }, + { + "x": 2.4271871577101587, + "y": 3.010674142880251, + "heading": -0.7925747589157716, + "angularVelocity": -0.15569183096565328, + "velocityX": -0.8840469045768797, + "velocityY": 0.4174545707643066, + "timestamp": 10.880410038807433 + }, + { + "x": 2.3904726379075623, + "y": 3.0280110502230517, + "heading": -0.7990401886427835, + "angularVelocity": -0.10378717574737188, + "velocityX": -0.5893647414204509, + "velocityY": 0.27830302474492646, + "timestamp": 10.942705112167454 }, { "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": -0.036061762982130895, - "velocityX": -0.24729733470577522, - "velocityY": 0.11701401166345779, - "timestamp": 11.6597470694993 + "angularVelocity": -0.05188569757551244, + "velocityX": -0.29468244125856385, + "velocityY": 0.13915155101802326, + "timestamp": 11.005000185527475 }, { "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": 2.8180807610025885e-36, - "velocityX": -1.7033240054088048e-37, - "velocityY": 8.364918339692578e-38, - "timestamp": 11.722488562162141 - }, - { - "x": 2.3862784269967885, - "y": 3.0299921206795677, - "heading": -0.7979445022798209, - "angularVelocity": 0.07214760590413402, - "velocityX": 0.2361025217555023, - "velocityY": -0.11148080618080956, - "timestamp": 11.782475441082498 - }, - { - "x": 2.414605712310544, - "y": 3.016617241283919, - "heading": -0.7893555783917804, - "angularVelocity": 0.1431800427464117, - "velocityX": 0.4722246901920792, - "velocityY": -0.22296341527296182, - "timestamp": 11.842462320002856 - }, - { - "x": 2.457098595113841, - "y": 2.996554734272641, - "heading": -0.7765849369819304, - "angularVelocity": 0.2128905794016223, - "velocityX": 0.7083696229589314, - "velocityY": -0.33444825555792773, - "timestamp": 11.902449198923213 - }, - { - "x": 2.5137586742709717, - "y": 2.9698044458506936, - "heading": -0.759727315329062, - "angularVelocity": 0.281021816041631, - "velocityX": 0.944541209292721, - "velocityY": -0.44593565965421617, - "timestamp": 11.96243607784357 - }, - { - "x": 2.5845878442638655, - "y": 2.9363662114763, - "heading": -0.7388971783301493, - "angularVelocity": 0.34724488711220036, - "velocityX": 1.180744377231747, - "velocityY": -0.5574258067133014, - "timestamp": 12.022422956763927 - }, - { - "x": 2.669588381015972, - "y": 2.8962398712988433, - "heading": -0.7142349782126004, - "angularVelocity": 0.41112657570152883, - "velocityX": 1.4169854855252446, - "velocityY": -0.6689186185320778, - "timestamp": 12.082409835684285 - }, - { - "x": 2.768763062860025, - "y": 2.849425297465716, - "heading": -0.6859166617272396, - "angularVelocity": 0.4720751770225952, - "velocityX": 1.653272909492815, - "velocityY": -0.7804135616937369, - "timestamp": 12.142396714604642 - }, - { - "x": 2.8821153449579997, - "y": 2.795922445610523, - "heading": -0.6541688123580982, - "angularVelocity": 0.5292465609236296, - "velocityX": 1.8896179320892514, - "velocityY": -0.8919092444570627, - "timestamp": 12.202383593525 - }, - { - "x": 3.0096496153353156, - "y": 2.73573145985777, - "heading": -0.6192942663212868, - "angularVelocity": 0.5813695705541383, - "velocityX": 2.1260361044394105, - "velocityY": -1.0034025246198655, - "timestamp": 12.262370472445356 - }, - { - "x": 3.151371569227902, - "y": 2.6688529097388005, - "heading": -0.5817190571900679, - "angularVelocity": 0.6263904675071726, - "velocityX": 2.3625492181506123, - "velocityY": -1.1148863105173612, - "timestamp": 12.322357351365714 - }, - { - "x": 3.30728870131474, - "y": 2.5952884046594, - "heading": -0.5420886244414685, - "angularVelocity": 0.6606516868666429, - "velocityX": 2.5991872705003334, - "velocityY": -1.2263432671179575, - "timestamp": 12.382344230286071 - }, - { - "x": 3.477410464899007, - "y": 2.5150425607670805, - "heading": -0.501501110267189, - "angularVelocity": 0.6766065330414338, - "velocityX": 2.8359829123654157, - "velocityY": -1.3377232710983202, - "timestamp": 12.442331109206428 - }, - { - "x": 3.661742704544347, - "y": 2.428132084038069, - "heading": -0.46226256646132613, - "angularVelocity": 0.6541187758402722, - "velocityX": 3.0728759849311573, - "velocityY": -1.4488247812392314, - "timestamp": 12.502317988126785 - }, - { - "x": 3.860146403975295, - "y": 2.3346891355318427, - "heading": -0.4327535611710109, - "angularVelocity": 0.4919243311440368, - "velocityX": 3.307451612782871, - "velocityY": -1.5577231252568988, - "timestamp": 12.562304867047143 - }, - { - "x": 4.065242173401202, - "y": 2.237711412138144, - "heading": -0.4327535477418668, - "angularVelocity": 2.238680243649846e-7, - "velocityX": 3.41901050891823, - "velocityY": -1.6166489262168846, - "timestamp": 12.6222917459675 - }, - { - "x": 4.270337924993822, - "y": 2.140733651028875, - "heading": -0.4327535343131544, - "angularVelocity": 2.2386082909736944e-7, - "velocityX": 3.419010211631728, - "velocityY": -1.6166495549472233, - "timestamp": 12.682278624887857 - }, - { - "x": 4.475433676585232, - "y": 2.0437558899170503, - "heading": -0.43275352088444197, - "angularVelocity": 2.2386082765717578e-7, - "velocityX": 3.419010211611586, - "velocityY": -1.6166495549898217, - "timestamp": 12.742265503808214 - }, - { - "x": 4.6805294281766425, - "y": 1.9467781288052255, - "heading": -0.43275350745572955, - "angularVelocity": 2.2386082872977313e-7, - "velocityX": 3.4190102116115844, - "velocityY": -1.616649554989824, - "timestamp": 12.802252382728572 - }, - { - "x": 4.885625179768054, - "y": 1.8498003676934025, - "heading": -0.4327534940270171, - "angularVelocity": 2.2386082836686737e-7, - "velocityX": 3.419010211611597, - "velocityY": -1.6166495549897981, - "timestamp": 12.862239261648929 - }, - { - "x": 5.090720931370458, - "y": 1.7528226066048298, - "heading": -0.4327534805983047, - "angularVelocity": 2.2386082909228912e-7, - "velocityX": 3.4190102117948658, - "velocityY": -1.616649554602208, - "timestamp": 12.922226140569286 - }, - { - "x": 5.295816845235169, - "y": 1.6558451886813434, - "heading": -0.4327534671695918, - "angularVelocity": 2.2386083672590555e-7, - "velocityX": 3.4190129167581613, - "velocityY": -1.6166438339330798, - "timestamp": 12.982213019489643 + "angularVelocity": -7.343237406242101e-30, + "velocityX": 6.07048254686404e-33, + "velocityY": -1.403576741314563e-31, + "timestamp": 11.067295258887496 + }, + { + "x": 2.3887780433959134, + "y": 3.0288255570872233, + "heading": -0.7958553483876907, + "angularVelocity": 0.1079808929405059, + "velocityX": 0.2803852472906352, + "velocityY": -0.13215958314273293, + "timestamp": 11.126723028163784 + }, + { + "x": 2.422106198393558, + "y": 3.0131153644204134, + "heading": -0.7831901230540868, + "angularVelocity": 0.2131196490779077, + "velocityX": 0.5608178702232047, + "velocityY": -0.26435777176444086, + "timestamp": 11.186150797440073 + }, + { + "x": 2.47210314120033, + "y": 2.9895461805025576, + "heading": -0.7644823111559433, + "angularVelocity": 0.3147991608305137, + "velocityX": 0.8413060664338303, + "velocityY": -0.3966021980108423, + "timestamp": 11.245578566716361 + }, + { + "x": 2.5387727913774722, + "y": 2.9581147004806385, + "heading": -0.7399860035384287, + "angularVelocity": 0.41220304776557914, + "velocityX": 1.1218602176902186, + "velocityY": -0.5289022355153943, + "timestamp": 11.30500633599265 + }, + { + "x": 2.6221198536798296, + "y": 2.9188169151709924, + "heading": -0.7100217609714737, + "angularVelocity": 0.5042128104749645, + "velocityX": 1.4024935365630693, + "velocityY": -0.6612697361556471, + "timestamp": 11.364434105268938 + }, + { + "x": 2.7221500309650937, + "y": 2.871647898493915, + "heading": -0.6750052177423963, + "angularVelocity": 0.5892286326010091, + "velocityX": 1.6832228182788898, + "velocityY": -0.7937201286790226, + "timestamp": 11.423861874545226 + }, + { + "x": 2.8388702680559588, + "y": 2.816601499667555, + "heading": -0.6354955831852485, + "angularVelocity": 0.6648345552665271, + "velocityX": 1.9640689615693319, + "velocityY": -0.9262740213325141, + "timestamp": 11.483289643821514 + }, + { + "x": 2.9722889163933544, + "y": 2.753669910841413, + "heading": -0.5922850618953597, + "angularVelocity": 0.7271099322127873, + "velocityX": 2.2450556358107243, + "velocityY": -1.0589592978591336, + "timestamp": 11.542717413097803 + }, + { + "x": 3.122415239721002, + "y": 2.6828431673634756, + "heading": -0.5465844996458821, + "angularVelocity": 0.7690102254582669, + "velocityX": 2.5261981924591717, + "velocityY": -1.191812251081979, + "timestamp": 11.602145182374091 + }, + { + "x": 3.289254937095535, + "y": 2.6041094136623544, + "heading": -0.5004858972625192, + "angularVelocity": 0.7757081065791619, + "velocityX": 2.8074366479896216, + "velocityY": -1.3248646997853857, + "timestamp": 11.66157295165038 + }, + { + "x": 3.4727741175032825, + "y": 2.5174653392644726, + "heading": -0.45856185133391086, + "angularVelocity": 0.7054622180728715, + "velocityX": 3.0881048143423393, + "velocityY": -1.4579728543249981, + "timestamp": 11.721000720926668 + }, + { + "x": 3.672087548521264, + "y": 2.4232619622496343, + "heading": -0.44156909021440377, + "angularVelocity": 0.2859397437680458, + "velocityX": 3.353877041746403, + "velocityY": -1.5851743749110756, + "timestamp": 11.780428490202956 + }, + { + "x": 3.87540622002593, + "y": 2.327473100540026, + "heading": -0.4415690736830568, + "angularVelocity": 2.781754584465502e-7, + "velocityX": 3.4212738250264785, + "velocityY": -1.6118535640173974, + "timestamp": 11.839856259479244 + }, + { + "x": 4.078724897772698, + "y": 2.231684252079618, + "heading": -0.4415690571518241, + "angularVelocity": 2.781735362772021e-7, + "velocityX": 3.4212739300632746, + "velocityY": -1.6118533410711124, + "timestamp": 11.899284028755533 + }, + { + "x": 4.282043575519764, + "y": 2.135895403619844, + "heading": -0.4415690406205913, + "angularVelocity": 2.7817353604412086e-7, + "velocityX": 3.4212739300683004, + "velocityY": -1.611853341060444, + "timestamp": 11.958711798031821 + }, + { + "x": 4.485362253266831, + "y": 2.0401065551600697, + "heading": -0.44156902408935866, + "angularVelocity": 2.781735361125895e-7, + "velocityX": 3.421273930068301, + "velocityY": -1.611853341060444, + "timestamp": 12.01813956730811 + }, + { + "x": 4.688680931013898, + "y": 1.9443177067002955, + "heading": -0.4415690075581259, + "angularVelocity": 2.781735368678374e-7, + "velocityX": 3.421273930068301, + "velocityY": -1.611853341060444, + "timestamp": 12.077567336584398 + }, + { + "x": 4.8919996087609645, + "y": 1.8485288582405217, + "heading": -0.4415689910268932, + "angularVelocity": 2.7817353639391735e-7, + "velocityX": 3.4212739300683026, + "velocityY": -1.6118533410604396, + "timestamp": 12.136995105860686 + }, + { + "x": 5.0953182865105076, + "y": 1.7527400097860029, + "heading": -0.4415689744956604, + "angularVelocity": 2.781735367341045e-7, + "velocityX": 3.4212739301099617, + "velocityY": -1.611853340972016, + "timestamp": 12.196422875136975 + }, + { + "x": 5.298637015997747, + "y": 1.656951271148522, + "heading": -0.4415689579644276, + "angularVelocity": 2.7817353744823303e-7, + "velocityX": 3.4212748007079674, + "velocityY": -1.6118514930642196, + "timestamp": 12.255850644413263 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.4327534537085892, - "angularVelocity": 2.2439911588435075e-7, - "velocityX": 3.4538678416562716, - "velocityY": -1.5407736813730113, - "timestamp": 13.04219989841 - }, - { - "x": 5.767637732978587, - "y": 1.4715486447521433, - "heading": -0.43275344189521775, - "angularVelocity": 1.5949042736003556e-7, - "velocityX": 3.5727828839624562, - "velocityY": -1.2403266728714895, - "timestamp": 13.116269368711977 - }, - { - "x": 6.033774192023134, - "y": 1.384125705645109, - "heading": -0.43275343009903516, - "angularVelocity": 1.5925836231633141e-7, - "velocityX": 3.5930655094403474, - "velocityY": -1.1802830336252825, - "timestamp": 13.190338839013954 - }, - { - "x": 6.2999107025526015, - "y": 1.2967029232707166, - "heading": -0.43275341830285263, - "angularVelocity": 1.5925836222768275e-7, - "velocityX": 3.593066204529984, - "velocityY": -1.1802809176031077, - "timestamp": 13.26440830931593 - }, - { - "x": 6.566047160708243, - "y": 1.2092799814598572, - "heading": -0.4327534065048956, - "angularVelocity": 1.5928231960994732e-7, - "velocityX": 3.5930654974393894, - "velocityY": -1.1802830701291929, - "timestamp": 13.338477779617907 - }, - { - "x": 6.815604168443372, - "y": 1.1270295237235333, - "heading": -0.36800659105834677, - "angularVelocity": 0.874136336908857, - "velocityX": 3.369229005117782, - "velocityY": -1.1104501949452927, - "timestamp": 13.412547249919884 - }, - { - "x": 7.0424486777806585, - "y": 1.0523256982782683, - "heading": -0.30458898646479193, - "angularVelocity": 0.8561908750664163, - "velocityX": 3.0625912189253857, - "velocityY": -1.0085643267152118, - "timestamp": 13.48661672022186 - }, - { - "x": 7.2465905745815595, - "y": 0.9851213052520007, - "heading": -0.2456798424413219, - "angularVelocity": 0.7953228743678266, - "velocityX": 2.756086900157761, - "velocityY": -0.9073156963628867, - "timestamp": 13.560686190523837 - }, - { - "x": 7.428038548794458, - "y": 0.9253999088853845, - "heading": -0.1923064776337199, - "angularVelocity": 0.7205852099387569, - "velocityX": 2.449699902984963, - "velocityY": -0.8062889625528059, - "timestamp": 13.634755660825814 - }, - { - "x": 7.586797949690042, - "y": 0.8731533407207944, - "heading": -0.14497999296920738, - "angularVelocity": 0.6389472541327177, - "velocityX": 2.1433851254549703, - "velocityY": -0.7053725097747339, - "timestamp": 13.70882513112779 - }, - { - "x": 7.722872327084394, - "y": 0.8283767866857153, - "heading": -0.10400779573923787, - "angularVelocity": 0.5531590419497899, - "velocityX": 1.837118273420693, - "velocityY": -0.6045210510150557, - "timestamp": 13.782894601429767 - }, - { - "x": 7.836264194581955, - "y": 0.7910670998170554, - "heading": -0.06959570799723021, - "angularVelocity": 0.46459205934255987, - "velocityX": 1.5308853571555237, - "velocityY": -0.5037120788977025, - "timestamp": 13.856964071731744 - }, - { - "x": 7.926975414267803, - "y": 0.7612220655404804, - "heading": -0.04189104317473832, - "angularVelocity": 0.3740362218001813, - "velocityX": 1.2246775806013535, - "velocityY": -0.4029330053920843, - "timestamp": 13.93103354203372 - }, - { - "x": 7.99500740651227, - "y": 0.7388400306235591, - "heading": -0.021003806792755153, - "angularVelocity": 0.28199521741990785, - "velocityX": 0.9184889802384881, - "velocityY": -0.30217625191150127, - "timestamp": 14.005103012335697 - }, - { - "x": 8.040361272813582, - "y": 0.7239196956147146, - "heading": -0.007018297206773477, - "angularVelocity": 0.18881611450660732, - "velocityX": 0.6123152510259342, - "velocityY": -0.20143704211755917, - "timestamp": 14.079172482637674 + "heading": -0.44156894141194364, + "angularVelocity": 2.7853113329051353e-7, + "velocityX": 3.4389071599112104, + "velocityY": -1.5738818379758603, + "timestamp": 12.315278413689551 + }, + { + "x": 5.76887189670182, + "y": 1.4734081866468696, + "heading": -0.441568927295876, + "angularVelocity": 1.9019557057481577e-7, + "velocityX": 3.5822280134028937, + "velocityY": -1.2127779127617127, + "timestamp": 12.389497111786811 + }, + { + "x": 6.03546847764148, + "y": 1.3855778991398175, + "heading": -0.4415689131909676, + "angularVelocity": 1.9004521532739413e-7, + "velocityX": 3.5920406551769672, + "velocityY": -1.1833983855651649, + "timestamp": 12.463715809884071 + }, + { + "x": 6.302065075021444, + "y": 1.2977476615350296, + "heading": -0.441568899086059, + "angularVelocity": 1.900452165326357e-7, + "velocityX": 3.592040876688587, + "velocityY": -1.1833977131973445, + "timestamp": 12.537934507981332 + }, + { + "x": 6.568661672398624, + "y": 1.2099174239217916, + "heading": -0.44156888498115054, + "angularVelocity": 1.9004521568233082e-7, + "velocityX": 3.5920408766510765, + "velocityY": -1.1833977133112008, + "timestamp": 12.612153206078592 + }, + { + "x": 6.835258130689826, + "y": 1.1220867641390964, + "heading": -0.44156887087145064, + "angularVelocity": 1.9010977386064987e-7, + "velocityX": 3.5920390026492015, + "velocityY": -1.1834034014931274, + "timestamp": 12.686371904175852 + }, + { + "x": 7.080885784636739, + "y": 1.0408339813622511, + "heading": -0.35950242791659653, + "angularVelocity": 1.105738109919598, + "velocityX": 3.3095117570646315, + "velocityY": -1.0947751019611969, + "timestamp": 12.760590602273112 + }, + { + "x": 7.299180381748869, + "y": 0.9686950138524031, + "heading": -0.28235870642192235, + "angularVelocity": 1.03941086912835, + "velocityX": 2.9412345232203254, + "velocityY": -0.9719783472261578, + "timestamp": 12.834809300370372 + }, + { + "x": 7.490166378274822, + "y": 0.9056069788063349, + "heading": -0.2132225753720476, + "angularVelocity": 0.9315190487378978, + "velocityX": 2.5732868053771534, + "velocityY": -0.8500288561164988, + "timestamp": 12.909027998467632 + }, + { + "x": 7.653856153896351, + "y": 0.8515488890315335, + "heading": -0.15310137838681676, + "angularVelocity": 0.8100545890297152, + "velocityX": 2.2055058875732163, + "velocityY": -0.7283621400089914, + "timestamp": 12.983246696564892 + }, + { + "x": 7.790256705792751, + "y": 0.8065104939114175, + "heading": -0.10249736767748478, + "angularVelocity": 0.6818229369001992, + "velocityX": 1.8378192476193804, + "velocityY": -0.6068335375688586, + "timestamp": 13.057465394662152 + }, + { + "x": 7.899372493751606, + "y": 0.7704858089539702, + "heading": -0.06171206576746063, + "angularVelocity": 0.5495286626665922, + "velocityX": 1.470192697477151, + "velocityY": -0.48538556834071367, + "timestamp": 13.131684092759413 + }, + { + "x": 7.981206582030799, + "y": 0.7434709210905415, + "heading": -0.030946027709333555, + "angularVelocity": 0.4145321710976964, + "velocityX": 1.1026074342068202, + "velocityY": -0.3639903226007307, + "timestamp": 13.205902790856673 + }, + { + "x": 8.035761170601306, + "y": 0.7254630347979746, + "heading": -0.010340878696429398, + "angularVelocity": 0.2776274650631065, + "velocityX": 0.7350518126714517, + "velocityY": -0.24263274288330566, + "timestamp": 13.280121488953933 }, { "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": 2.0779165225884802e-41, - "angularVelocity": 0.09475290127174345, - "velocityX": 0.3061531209608937, - "velocityY": -0.10071229126619018, - "timestamp": 14.15324195293965 + "heading": -7.30119772009808e-38, + "angularVelocity": 0.13932983145219902, + "velocityX": 0.3675179222006851, + "velocityY": -0.12130427346551735, + "timestamp": 13.354340187051193 }, { "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": 1.169210048317657e-41, - "angularVelocity": -1.2268302369298258e-40, - "velocityX": -9.944733020002764e-42, - "velocityY": 5.460591898189625e-42, - "timestamp": 14.227311423241627 + "heading": 1.121139660303252e-31, + "angularVelocity": 1.5105915295246559e-30, + "velocityX": -7.247567295016868e-32, + "velocityY": -3.7967728194239397e-31, + "timestamp": 13.428558885148453 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint (1).1.traj b/src/main/deploy/choreo/SourceLanePHGSprint (1).1.traj new file mode 100644 index 00000000..b65ea667 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePHGSprint (1).1.traj @@ -0,0 +1,184 @@ +{ + "samples": [ + { + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": -3.528930169589175e-36, + "angularVelocity": 0, + "velocityX": -3.4629926489463697e-37, + "velocityY": -7.866244288467422e-37, + "timestamp": 0 + }, + { + "x": 0.45469188032137553, + "y": 4.109218526997661, + "heading": -0.008937300145995173, + "angularVelocity": -0.1189323726299145, + "velocityX": 0.2854425752342375, + "velocityY": -0.15856789979825622, + "timestamp": 0.07514606787342622 + }, + { + "x": 0.49759166351162515, + "y": 4.0853873108981436, + "heading": -0.026812413296636604, + "angularVelocity": -0.237871570083344, + "velocityX": 0.5708852692400179, + "velocityY": -0.3171319108760035, + "timestamp": 0.15029213574685243 + }, + { + "x": 0.5619414537480725, + "y": 4.049641027140498, + "heading": -0.053621717098024775, + "angularVelocity": -0.3567625633658565, + "velocityX": 0.856329440215504, + "velocityY": -0.4756906750976659, + "timestamp": 0.22543820362027867 + }, + { + "x": 0.6477414523773106, + "y": 4.001980177860296, + "heading": -0.08935853104911512, + "angularVelocity": -0.47556465644063195, + "velocityX": 1.141776290593902, + "velocityY": -0.6342427571923072, + "timestamp": 0.30058427149370487 + }, + { + "x": 0.7549919358777414, + "y": 3.942405375853288, + "heading": -0.1340144220065316, + "angularVelocity": -0.5942545261667388, + "velocityX": 1.4272268201854585, + "velocityY": -0.7927866845588477, + "timestamp": 0.37573033936713107 + }, + { + "x": 0.8836932393269182, + "y": 3.8709173416225418, + "heading": -0.18758057158075767, + "angularVelocity": -0.7128270459134506, + "velocityX": 1.712681808793474, + "velocityY": -0.951321023891208, + "timestamp": 0.4508764072405573 + }, + { + "x": 1.0338457421402325, + "y": 3.787516893297593, + "heading": -0.2500489995023071, + "angularVelocity": -0.8312933688928251, + "velocityX": 1.9981418464400118, + "velocityY": -1.1098444760333435, + "timestamp": 0.5260224751139835 + }, + { + "x": 1.2054498603190866, + "y": 3.6922049311693006, + "heading": -0.32141344961465573, + "angularVelocity": -0.9496764385936043, + "velocityX": 2.283607419990343, + "velocityY": -1.268355947630325, + "timestamp": 0.6011685429874097 + }, + { + "x": 1.3985060453231255, + "y": 3.584982424979259, + "heading": -0.4016698579859379, + "angularVelocity": -1.0680054278616897, + "velocityX": 2.5690790013020615, + "velocityY": -1.4268545144723217, + "timestamp": 0.676314610860836 + }, + { + "x": 1.5915937569821008, + "y": 3.4778198154599647, + "heading": -0.48176859267038585, + "angularVelocity": -1.065907198489266, + "velocityX": 2.5694985396202825, + "velocityY": -1.4260574445464829, + "timestamp": 0.7514606787342623 + }, + { + "x": 1.7632284858900336, + "y": 3.3825660365200445, + "heading": -0.5529765476804539, + "angularVelocity": -0.9475938931363466, + "velocityX": 2.2840147697019786, + "velocityY": -1.2675816797275798, + "timestamp": 0.8266067466076885 + }, + { + "x": 1.9134098098562764, + "y": 3.2992204007390877, + "heading": -0.6152918418017526, + "angularVelocity": -0.8292555536806094, + "velocityX": 1.9985253815170194, + "velocityY": -1.1091150626981794, + "timestamp": 0.9017528144811148 + }, + { + "x": 2.0421373577646693, + "y": 3.2277823059512354, + "heading": -0.6687115003742365, + "angularVelocity": -0.7108776291856339, + "velocityX": 1.7130310547348626, + "velocityY": -0.9506564589404817, + "timestamp": 0.976898882354541 + }, + { + "x": 2.149410808485792, + "y": 3.1682512394298143, + "heading": -0.7132320418113071, + "angularVelocity": -0.5924533737687983, + "velocityX": 1.4275324545498613, + "velocityY": -0.7922046782500071, + "timestamp": 1.0520449502279672 + }, + { + "x": 2.2352298936713018, + "y": 3.120626784362015, + "heading": -0.7488501306819737, + "angularVelocity": -0.4739847323836121, + "velocityX": 1.1420302833417786, + "velocityY": -0.6337584442610683, + "timestamp": 1.1271910181013933 + }, + { + "x": 2.2995943987852443, + "y": 3.0849086278583613, + "heading": -0.775563262460788, + "angularVelocity": -0.3554827622359309, + "velocityX": 0.8565252572144717, + "velocityY": -0.4753163740226096, + "timestamp": 1.2023370859748195 + }, + { + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080858, + "angularVelocity": -0.23696688264902366, + "velocityX": 0.5710180813549187, + "velocityY": -0.3168769856057537, + "timestamp": 1.2774831538482456 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288599550472, + "velocityX": 0.28550943973314774, + "velocityY": -0.15843872456938407, + "timestamp": 1.3526292217216718 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 9.576319315825878e-33, + "velocityX": -3.340074147697442e-33, + "velocityY": 1.4972964833121403e-33, + "timestamp": 1.427775289595098 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint (1).2.traj b/src/main/deploy/choreo/SourceLanePHGSprint (1).2.traj new file mode 100644 index 00000000..a125ab60 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePHGSprint (1).2.traj @@ -0,0 +1,814 @@ +{ + "samples": [ + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 9.576319315825878e-33, + "velocityX": -3.340074147697442e-33, + "velocityY": 1.4972964833121403e-33, + "timestamp": 0 + }, + { + "x": 2.3773546785315864, + "y": 3.0407954088560687, + "heading": -0.7860770412156382, + "angularVelocity": 0.26500042058499174, + "velocityX": 0.21918861143707746, + "velocityY": -0.13736692695017713, + "timestamp": 0.06111450966076326 + }, + { + "x": 2.4041715283150715, + "y": 3.0239975454455843, + "heading": -0.7541197928104101, + "angularVelocity": 0.5229077118120982, + "velocityX": 0.438796775632191, + "velocityY": -0.27485884291187945, + "timestamp": 0.12222901932152652 + }, + { + "x": 2.4444388170293236, + "y": 2.998788446329585, + "heading": -0.7069088657797715, + "angularVelocity": 0.7724994815911764, + "velocityX": 0.6588826276733535, + "velocityY": -0.41248959135778174, + "timestamp": 0.18334352898228978 + }, + { + "x": 2.498190798561039, + "y": 2.965157152573787, + "heading": -0.6450804787000076, + "angularVelocity": 1.0116809808826634, + "velocityX": 0.8795289666903056, + "velocityY": -0.5502996578468836, + "timestamp": 0.24445803864305304 + }, + { + "x": 2.5654677170302604, + "y": 2.923088008476279, + "heading": -0.5694581806203519, + "angularVelocity": 1.2373869724133075, + "velocityX": 1.1008338092322851, + "velocityY": -0.6883658943027918, + "timestamp": 0.3055725483038163 + }, + { + "x": 2.646315775914165, + "y": 2.8725583813996622, + "heading": -0.48110952859275347, + "angularVelocity": 1.4456248199978636, + "velocityX": 1.3228946666295571, + "velocityY": -0.8268024624119353, + "timestamp": 0.36668705796457957 + }, + { + "x": 2.7407862870930626, + "y": 2.813537598490058, + "heading": -0.3813947698765895, + "angularVelocity": 1.631605313855327, + "velocityX": 1.5457951262848801, + "velocityY": -0.965740922036666, + "timestamp": 0.4278015676253428 + }, + { + "x": 2.84893500974016, + "y": 2.7459886359527177, + "heading": -0.2720274509201168, + "angularVelocity": 1.7895475160244767, + "velocityX": 1.7696079580350594, + "velocityY": -1.105285191884772, + "timestamp": 0.4889160772861061 + }, + { + "x": 2.9708226363170516, + "y": 2.669873093439706, + "heading": -0.15523119808259803, + "angularVelocity": 1.9111051284848068, + "velocityX": 1.9944138839282306, + "velocityY": -1.2454577961194009, + "timestamp": 0.5500305869468693 + }, + { + "x": 3.1065122638812066, + "y": 2.5851587340080315, + "heading": -0.03419676462946552, + "angularVelocity": 1.9804533182868547, + "velocityX": 2.220252249708728, + "velocityY": -1.3861578846318265, + "timestamp": 0.6111450966076324 + }, + { + "x": 3.2560356571448454, + "y": 2.4918371927335334, + "heading": 0.08572826850512158, + "angularVelocity": 1.9623005044182085, + "velocityX": 2.446610372784124, + "velocityY": -1.5269948461095475, + "timestamp": 0.6722596062683956 + }, + { + "x": 3.419180860242083, + "y": 2.390029027608911, + "heading": 0.19415018142326163, + "angularVelocity": 1.7740780956923754, + "velocityX": 2.6695003200194134, + "velocityY": -1.6658591501387063, + "timestamp": 0.7333741159291589 + }, + { + "x": 3.5929644641848295, + "y": 2.279455345020229, + "heading": 0.2670576620012504, + "angularVelocity": 1.1929651564364419, + "velocityX": 2.8435735622750062, + "velocityY": -1.809286914064415, + "timestamp": 0.7944886255899222 + }, + { + "x": 3.776865546474961, + "y": 2.1600020280366277, + "heading": 0.3015223704483068, + "angularVelocity": 0.5639365944088309, + "velocityX": 3.0091230922237155, + "velocityY": -1.9545819421061736, + "timestamp": 0.8556031352506854 + }, + { + "x": 3.9728967066647076, + "y": 2.0375522576737963, + "heading": 0.30152241765502197, + "angularVelocity": 7.724305629887772e-7, + "velocityX": 3.2076042379769354, + "velocityY": -2.003612088889046, + "timestamp": 0.9167176449114487 + }, + { + "x": 4.174386727718979, + "y": 1.9243094504352285, + "heading": 0.30152246485493484, + "angularVelocity": 7.72319260587173e-7, + "velocityX": 3.2969260847008353, + "velocityY": -1.8529610704096402, + "timestamp": 0.977832154572212 + }, + { + "x": 4.383354452565465, + "y": 1.8255438101811148, + "heading": 0.3015225128834295, + "angularVelocity": 7.858771167686507e-7, + "velocityX": 3.4192817058736558, + "velocityY": -1.6160751481496929, + "timestamp": 1.0389466642329752 + }, + { + "x": 4.598761540028333, + "y": 1.7417467464566943, + "heading": 0.30152256294147645, + "angularVelocity": 8.190861254028443e-7, + "velocityX": 3.524647234487482, + "velocityY": -1.3711484259558735, + "timestamp": 1.1000611738937385 + }, + { + "x": 4.819537390717514, + "y": 1.6733348748555603, + "heading": 0.3015226164690873, + "angularVelocity": 8.758576513463982e-7, + "velocityX": 3.612494838208991, + "velocityY": -1.1194047367945394, + "timestamp": 1.1611756835545017 + }, + { + "x": 5.04458467733647, + "y": 1.620648272623701, + "heading": 0.30152267534909954, + "angularVelocity": 9.634375302003616e-7, + "velocityX": 3.682387175617667, + "velocityY": -0.8620964567058493, + "timestamp": 1.222290193215265 + }, + { + "x": 5.27278482830379, + "y": 1.5839488347993986, + "heading": 0.3015227422489141, + "angularVelocity": 0.0000010946633609592033, + "velocityX": 3.7339766322927472, + "velocityY": -0.6005028597630111, + "timestamp": 1.2834047028760283 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3015228161300745, + "angularVelocity": 0.0000012088972118666571, + "velocityX": 3.7670067261218194, + "velocityY": -0.33592432468507805, + "timestamp": 1.3445192125367915 + }, + { + "x": 5.642161233224445, + "y": 1.5569512204704454, + "heading": 0.30152289610635336, + "angularVelocity": 0.0000021712102287950276, + "velocityX": 3.777876226736954, + "velocityY": -0.1755880047102645, + "timestamp": 1.38135409743325 + }, + { + "x": 5.781468008961295, + "y": 1.556401100893367, + "heading": 0.3015229698630755, + "angularVelocity": 0.0000020023605971612715, + "velocityX": 3.7819250997644014, + "velocityY": -0.01493474402390838, + "timestamp": 1.4181889823297087 + }, + { + "x": 5.920672391336217, + "y": 1.5617696165699877, + "heading": 0.3015230386365789, + "angularVelocity": 0.0000018670752917486593, + "velocityX": 3.779145306581512, + "velocityY": 0.14574541746801997, + "timestamp": 1.4550238672261673 + }, + { + "x": 6.0595230325638285, + "y": 1.5730470711005764, + "heading": 0.30152310340837535, + "angularVelocity": 0.000001758436237719359, + "velocityX": 3.769541879061509, + "velocityY": 0.3061623393771694, + "timestamp": 1.4918587521226259 + }, + { + "x": 6.197769224123459, + "y": 1.5902130982406153, + "heading": 0.3015231649742211, + "angularVelocity": 0.0000016714005183062998, + "velocityX": 3.753132172076425, + "velocityY": 0.4660263548614807, + "timestamp": 1.5286936370190845 + }, + { + "x": 6.335161349542959, + "y": 1.6132366986444322, + "heading": 0.3015232239953201, + "angularVelocity": 0.0000016023152826544925, + "velocityX": 3.7299458327534505, + "velocityY": 0.6250487946015111, + "timestamp": 1.565528521915543 + }, + { + "x": 6.471451335210211, + "y": 1.6420762959087596, + "heading": 0.3015232810350899, + "angularVelocity": 0.000001548525804727782, + "velocityX": 3.7000247469309935, + "velocityY": 0.7829425107583309, + "timestamp": 1.6023634068120016 + }, + { + "x": 6.606393098338155, + "y": 1.6766798119201123, + "heading": 0.3015233366126912, + "angularVelocity": 0.0000015088305949550033, + "velocityX": 3.6634229618813627, + "velocityY": 0.9394224010369983, + "timestamp": 1.6391982917084602 + }, + { + "x": 6.739742463422568, + "y": 1.7169846140875678, + "heading": 0.30152556567084704, + "angularVelocity": 0.00006051486687687375, + "velocityX": 3.6201922568579414, + "velocityY": 1.0942019306087243, + "timestamp": 1.6760331766049188 + }, + { + "x": 6.8694207904091344, + "y": 1.7614703171513668, + "heading": 0.3101205256410533, + "angularVelocity": 0.23333750042565127, + "velocityX": 3.520530262306672, + "velocityY": 1.2077057710061154, + "timestamp": 1.7128680615013774 + }, + { + "x": 6.994764102779472, + "y": 1.8086931078781319, + "heading": 0.3306180633638748, + "angularVelocity": 0.5564707961064402, + "velocityX": 3.4028425152588877, + "velocityY": 1.2820127132066854, + "timestamp": 1.749702946397836 + }, + { + "x": 7.114597005425528, + "y": 1.8565129203183168, + "heading": 0.359627713397502, + "angularVelocity": 0.7875591335542955, + "velocityX": 3.2532449329732183, + "velocityY": 1.298220764761567, + "timestamp": 1.7865378312942946 + }, + { + "x": 7.228563598795877, + "y": 1.903963761474251, + "heading": 0.39146295222083616, + "angularVelocity": 0.8642687200685345, + "velocityX": 3.0939853264291135, + "velocityY": 1.288203866777826, + "timestamp": 1.8233727161907531 + }, + { + "x": 7.336676614220082, + "y": 1.9505339221399112, + "heading": 0.42341393247502157, + "angularVelocity": 0.8674108889982476, + "velocityX": 2.9350713522821095, + "velocityY": 1.2642949963483492, + "timestamp": 1.8602076010872117 + }, + { + "x": 7.438996464546078, + "y": 1.9959395107165516, + "heading": 0.45395278545537515, + "angularVelocity": 0.8290742068611577, + "velocityX": 2.777797476865021, + "velocityY": 1.2326789863542045, + "timestamp": 1.8970424859836703 + }, + { + "x": 7.535582073954796, + "y": 2.0400031624519186, + "heading": 0.4820829146791016, + "angularVelocity": 0.7636817463336469, + "velocityX": 2.622123285581467, + "velocityY": 1.1962478465516526, + "timestamp": 1.933877370880129 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6791276743107278, + "velocityX": 2.4678193928677445, + "velocityY": 1.1565397180800696, + "timestamp": 1.9707122557765875 + }, + { + "x": 7.766333855095899, + "y": 2.151391160116789, + "heading": 0.5393931714770351, + "angularVelocity": 0.508950730683683, + "velocityX": 2.2039777611896034, + "velocityY": 1.0840548028640593, + "timestamp": 2.0341656809073783 + }, + { + "x": 7.8897168213730975, + "y": 2.2145350229010927, + "heading": 0.5621690879226864, + "angularVelocity": 0.3589391179231936, + "velocityX": 1.9444650312080196, + "velocityY": 0.9951214241650663, + "timestamp": 2.0976191060381684 + }, + { + "x": 7.996856912991746, + "y": 2.2713427782005033, + "heading": 0.5763089063837279, + "angularVelocity": 0.22283774960762856, + "velocityX": 1.6884839769927564, + "velocityY": 0.8952669644281364, + "timestamp": 2.1610725311689594 + }, + { + "x": 8.08793407853681, + "y": 2.3213235528188796, + "heading": 0.5824533246990756, + "angularVelocity": 0.09683351690917823, + "velocityX": 1.4353388387992243, + "velocityY": 0.7876765441007405, + "timestamp": 2.2245259562997495 + }, + { + "x": 8.16309393881241, + "y": 2.364112462024999, + "heading": 0.5810872026138336, + "angularVelocity": -0.021529524725041688, + "velocityX": 1.1844886248564515, + "velocityY": 0.6743356898059116, + "timestamp": 2.2879793814305405 + }, + { + "x": 8.222455939995415, + "y": 2.3994280671308665, + "heading": 0.5725898810376202, + "angularVelocity": -0.13391430893917458, + "velocityX": 0.9355208337555886, + "velocityY": 0.5565594770191679, + "timestamp": 2.3514328065613306 + }, + { + "x": 8.266119462815809, + "y": 2.427046891293226, + "heading": 0.5572661381126698, + "angularVelocity": -0.24149591441856877, + "velocityX": 0.6881192422693246, + "velocityY": 0.4352613606819736, + "timestamp": 2.4148862316921216 + }, + { + "x": 8.294168308263885, + "y": 2.446787281465013, + "heading": 0.5353660980042331, + "angularVelocity": -0.3451356654632322, + "velocityX": 0.4420383200790658, + "velocityY": 0.31110046669817776, + "timestamp": 2.4783396568229117 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.4454857015776663, + "velocityX": 0.19708463824313482, + "velocityY": 0.18456741878341767, + "timestamp": 2.5417930819537027 + }, + { + "x": 8.303024530407173, + "y": 2.4618811275821986, + "heading": 0.4711910323232683, + "angularVelocity": -0.5465136547157353, + "velocityX": -0.055545178149817624, + "velocityY": 0.05148048067311967, + "timestamp": 2.607495872285164 + }, + { + "x": 8.282750419132642, + "y": 2.456558519757899, + "heading": 0.42887568169046636, + "angularVelocity": -0.6440419108431609, + "velocityX": -0.30857306321773753, + "velocityY": -0.08101037714605691, + "timestamp": 2.6731986626166258 + }, + { + "x": 8.245821395629305, + "y": 2.4425769262851884, + "heading": 0.3804195474569322, + "angularVelocity": -0.7375049672788591, + "velocityX": -0.5620617224479451, + "velocityY": -0.2128006040866038, + "timestamp": 2.7389014529480873 + }, + { + "x": 8.19220201387014, + "y": 2.4199912479232255, + "heading": 0.32613641295591106, + "angularVelocity": -0.8261922245184766, + "velocityX": -0.8160898721144464, + "velocityY": -0.3437552385221621, + "timestamp": 2.804604243279549 + }, + { + "x": 8.121850214183777, + "y": 2.388868090056124, + "heading": 0.2664003971864744, + "angularVelocity": -0.9091853704854365, + "velocityX": -1.0707581722397086, + "velocityY": -0.4736961354318394, + "timestamp": 2.8703070336110104 + }, + { + "x": 8.034715295558527, + "y": 2.3492899199909276, + "heading": 0.20166644793914107, + "angularVelocity": -0.9852541866298157, + "velocityX": -1.32619814448768, + "velocityY": -0.6023818754961511, + "timestamp": 2.936009823942472 + }, + { + "x": 7.930734985336223, + "y": 2.3013613961682027, + "heading": 0.13250258742090648, + "angularVelocity": -1.0526776742557293, + "velocityX": -1.582585909939886, + "velocityY": -0.7294747084702521, + "timestamp": 3.0017126142739334 + }, + { + "x": 7.809831081747402, + "y": 2.2452194829459002, + "heading": 0.05964299707705179, + "angularVelocity": -1.1089268808263428, + "velocityX": -1.8401639105261383, + "velocityY": -0.8544829365552755, + "timestamp": 3.067415404605395 + }, + { + "x": 7.671902780233126, + "y": 2.1810506699963903, + "heading": -0.015920256793081004, + "angularVelocity": -1.1500767850029923, + "velocityX": -2.099276161917122, + "velocityY": -0.976652781804051, + "timestamp": 3.1331181949368565 + }, + { + "x": 7.516816185598605, + "y": 2.109122861945235, + "heading": -0.09276870443286901, + "angularVelocity": -1.1696375032490753, + "velocityX": -2.360426305368932, + "velocityY": -1.0947451042534073, + "timestamp": 3.198820985268318 + }, + { + "x": 7.344387868807497, + "y": 2.0298516556077795, + "heading": -0.16871929267657423, + "angularVelocity": -1.155972034985807, + "velocityX": -2.624368248612144, + "velocityY": -1.2065120208372122, + "timestamp": 3.2645237755997796 + }, + { + "x": 7.154363408340053, + "y": 1.943962982346676, + "heading": -0.2400485240661189, + "angularVelocity": -1.0856347352935647, + "velocityX": -2.892182501059652, + "velocityY": -1.3072302230667407, + "timestamp": 3.330226565931241 + }, + { + "x": 6.946444183851081, + "y": 1.8530162365161487, + "heading": -0.2992772419214472, + "angularVelocity": -0.9014642689682999, + "velocityX": -3.164541771210153, + "velocityY": -1.3842143594162997, + "timestamp": 3.3959293562627026 + }, + { + "x": 6.72212326982476, + "y": 1.762675190878268, + "heading": -0.31855321839260486, + "angularVelocity": -0.29338139786625583, + "velocityX": -3.4141763674670904, + "velocityY": -1.3749955699312393, + "timestamp": 3.461632146594164 + }, + { + "x": 6.4857597450479005, + "y": 1.6860130581467423, + "heading": -0.3185532921212083, + "angularVelocity": -0.000001122153306519187, + "velocityX": -3.5974655503128186, + "velocityY": -1.1668017803319486, + "timestamp": 3.5273349369256257 + }, + { + "x": 6.24426876944993, + "y": 1.6274732096370486, + "heading": -0.3185533177664737, + "angularVelocity": -3.903223175651228e-7, + "velocityX": -3.6755056273817623, + "velocityY": -0.8909796405048931, + "timestamp": 3.5930377272570873 + }, + { + "x": 5.999037658914315, + "y": 1.5873919634602989, + "heading": -0.3185533462426245, + "angularVelocity": -4.3340854500246066e-7, + "velocityX": -3.732430682143934, + "velocityY": -0.6100387209515066, + "timestamp": 3.658740517588549 + }, + { + "x": 5.751475205523387, + "y": 1.5659995878977724, + "heading": -0.3185533789943253, + "angularVelocity": -4.984826460154038e-7, + "velocityX": -3.7679138456983283, + "velocityY": -0.32559310578142847, + "timestamp": 3.7244433079200103 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.3185534180019773, + "angularVelocity": -5.936985598886166e-7, + "velocityX": -3.781751231726404, + "velocityY": -0.03927692372891183, + "timestamp": 3.790146098251472 + }, + { + "x": 5.26265149067387, + "y": 1.5786031053003824, + "heading": -0.31855345112044986, + "angularVelocity": -5.200844119271939e-7, + "velocityX": -3.774430827391544, + "velocityY": 0.23844772894968422, + "timestamp": 3.853825131614962 + }, + { + "x": 5.0240625547753925, + "y": 1.61139052009726, + "heading": -0.3185534786660252, + "angularVelocity": -4.325689925822067e-7, + "velocityX": -3.746742425196293, + "velocityY": 0.5148855606793217, + "timestamp": 3.9175041649784523 + }, + { + "x": 4.788524290519247, + "y": 1.6616042890226763, + "heading": -0.31855350247111613, + "angularVelocity": -3.7382933976434193e-7, + "velocityX": -3.698835422197071, + "velocityY": 0.7885447732660757, + "timestamp": 3.9811831983419426 + }, + { + "x": 4.55730773239592, + "y": 1.7289734284079032, + "heading": -0.3185535237219762, + "angularVelocity": -3.3371831981217076e-7, + "velocityX": -3.6309684037367047, + "velocityY": 1.0579485244487483, + "timestamp": 4.044862231705433 + }, + { + "x": 4.33166058516164, + "y": 1.8131343663413289, + "heading": -0.31855354324045854, + "angularVelocity": -3.06513484100539e-7, + "velocityX": -3.5435077342686676, + "velocityY": 1.3216428310558945, + "timestamp": 4.108541265068923 + }, + { + "x": 4.112800476187319, + "y": 1.9136328827222213, + "heading": -0.31855356163583604, + "angularVelocity": -2.8887651909639526e-7, + "velocityX": -3.4369257417120753, + "velocityY": 1.5782041760469425, + "timestamp": 4.172220298432413 + }, + { + "x": 3.9019083093617053, + "y": 2.029926448332014, + "heading": -0.3185535793939757, + "angularVelocity": -2.788694925452677e-7, + "velocityX": -3.3117991226689654, + "velocityY": 1.8262457745859704, + "timestamp": 4.2358993317959035 + }, + { + "x": 3.700120307432907, + "y": 2.161384813738153, + "heading": -0.3185535969362317, + "angularVelocity": -2.7547930719470604e-7, + "velocityX": -3.1688295388681516, + "velocityY": 2.0643900898393666, + "timestamp": 4.299578365159394 + }, + { + "x": 3.5092854049648876, + "y": 2.2879634464070207, + "heading": -0.3579155843505842, + "angularVelocity": -0.618131044635493, + "velocityX": -2.996824737880426, + "velocityY": 1.9877599577609333, + "timestamp": 4.363257398522884 + }, + { + "x": 3.3329097555386666, + "y": 2.4050971261724468, + "heading": -0.41294492327597715, + "angularVelocity": -0.8641673093760206, + "velocityX": -2.7697601566820413, + "velocityY": 1.8394387222684179, + "timestamp": 4.426936431886374 + }, + { + "x": 3.171287726021972, + "y": 2.512479984884094, + "heading": -0.46992046584984914, + "angularVelocity": -0.8947300165290927, + "velocityX": -2.538072910028812, + "velocityY": 1.686314207985683, + "timestamp": 4.490615465249864 + }, + { + "x": 3.024408806786741, + "y": 2.6100923699912792, + "heading": -0.5252799185719805, + "angularVelocity": -0.8693513358805383, + "velocityX": -2.306550704009953, + "velocityY": 1.532881074842923, + "timestamp": 4.554294498613355 + }, + { + "x": 2.892255079193046, + "y": 2.6979336450739244, + "heading": -0.5773440254447586, + "angularVelocity": -0.8176020288434342, + "velocityX": -2.0753098879397363, + "velocityY": 1.3794379475145806, + "timestamp": 4.617973531976845 + }, + { + "x": 2.7748123815012584, + "y": 2.776006230905406, + "heading": -0.625128837055519, + "angularVelocity": -0.7504010203483642, + "velocityX": -1.8442914643726742, + "velocityY": 1.226032835420591, + "timestamp": 4.681652565340335 + }, + { + "x": 2.6720699818921556, + "y": 2.8443129166968544, + "heading": -0.6679860381773632, + "angularVelocity": -0.6730190277419648, + "velocityX": -1.6134415706757543, + "velocityY": 1.0726715244175118, + "timestamp": 4.745331598703825 + }, + { + "x": 2.5840196243973645, + "y": 2.9028562717726505, + "heading": -0.7054558016839699, + "angularVelocity": -0.5884160221579269, + "velocityX": -1.382721326691407, + "velocityY": 0.9193505614575074, + "timestamp": 4.809010632067316 + }, + { + "x": 2.510654811151598, + "y": 2.95163853061359, + "heading": -0.7371951288142529, + "angularVelocity": -0.4984266477336411, + "velocityX": -1.1521031235978156, + "velocityY": 0.7660646882386302, + "timestamp": 4.872689665430806 + }, + { + "x": 2.4519703126340358, + "y": 2.9906615990271557, + "heading": -0.7629386573341135, + "angularVelocity": -0.4042700895428554, + "velocityX": -0.9215670436230067, + "velocityY": 0.6128087433553877, + "timestamp": 4.936368698794296 + }, + { + "x": 2.4079618327799786, + "y": 3.0199270886206313, + "heading": -0.7824753669479414, + "angularVelocity": -0.30679972012623624, + "velocityX": -0.6910984279998401, + "velocityY": 0.45957810675962124, + "timestamp": 5.000047732157786 + }, + { + "x": 2.378625774904473, + "y": 3.039436353320747, + "heading": -0.795633833561549, + "angularVelocity": -0.2066373485680448, + "velocityX": -0.4606862938394533, + "velocityY": 0.3063687318988397, + "timestamp": 5.0637267655212765 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.10425061542851646, + "velocityX": -0.2303222914890624, + "velocityY": 0.15317707264507013, + "timestamp": 5.127405798884767 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -7.869331036874185e-31, + "timestamp": 5.191084832248257 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint (1).3.traj b/src/main/deploy/choreo/SourceLanePHGSprint (1).3.traj new file mode 100644 index 00000000..e900750d --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePHGSprint (1).3.traj @@ -0,0 +1,787 @@ +{ + "samples": [ + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -7.869331036874185e-31, + "timestamp": 0 + }, + { + "x": 2.3794831364602356, + "y": 3.04172499089676, + "heading": -0.7962438371233982, + "angularVelocity": 0.09576162685715454, + "velocityX": 0.24659384848580665, + "velocityY": -0.11858712018957836, + "timestamp": 0.0629539728390398 + }, + { + "x": 2.4105331611816494, + "y": 3.0267927192788293, + "heading": -0.7842933789401895, + "angularVelocity": 0.1898284992078828, + "velocityX": 0.49321787523723476, + "velocityY": -0.23719347555886855, + "timestamp": 0.1259079456780796 + }, + { + "x": 2.457111347137164, + "y": 3.0043922693157348, + "heading": -0.7665480516906409, + "angularVelocity": 0.2818777981640571, + "velocityX": 0.7398768315163422, + "velocityY": -0.3558226582517257, + "timestamp": 0.18886191851711942 + }, + { + "x": 2.5192202688476826, + "y": 2.9745219425624034, + "heading": -0.7431604801963628, + "angularVelocity": 0.37150270967132, + "velocityX": 0.9865766830842898, + "velocityY": -0.4744788201008906, + "timestamp": 0.2518158913561592 + }, + { + "x": 2.5968629770363694, + "y": 2.9371797310172574, + "heading": -0.7143164372650311, + "angularVelocity": 0.45817669053357035, + "velocityX": 1.2333249942335256, + "velocityY": -0.5931668782941151, + "timestamp": 0.31476986419519903 + }, + { + "x": 2.690043132952377, + "y": 2.892363250439385, + "heading": -0.6802459845187668, + "angularVelocity": 0.5411962297181008, + "velocityX": 1.4801314629379994, + "velocityY": -0.711892809250637, + "timestamp": 0.37772383703423884 + }, + { + "x": 2.798765189175504, + "y": 2.840069647867451, + "heading": -0.6412406321012497, + "angularVelocity": 0.6195852407479587, + "velocityX": 1.7270086591851181, + "velocityY": -0.8306640584167341, + "timestamp": 0.44067780987327865 + }, + { + "x": 2.9230346316689584, + "y": 2.7802954749466373, + "heading": -0.5976811625941267, + "angularVelocity": 0.6919256647788576, + "velocityX": 1.9739729978787044, + "velocityY": -0.9494900833922507, + "timestamp": 0.5036317827123185 + }, + { + "x": 3.0628582875020656, + "y": 2.7130365228135442, + "heading": -0.550085909804202, + "angularVelocity": 0.7560325527288918, + "velocityX": 2.2210457819811675, + "velocityY": -1.0683829645677803, + "timestamp": 0.5665857555513583 + }, + { + "x": 3.2182446226798445, + "y": 2.6382876468732976, + "heading": -0.4992026727158649, + "angularVelocity": 0.8082609372157451, + "velocityX": 2.4682530453648863, + "velocityY": -1.1873575656831603, + "timestamp": 0.6295397283903981 + }, + { + "x": 3.3892034750687743, + "y": 2.5560428034877645, + "heading": -0.4462086118156311, + "angularVelocity": 0.8417905734992818, + "velocityX": 2.7156165795292284, + "velocityY": -1.306428167699852, + "timestamp": 0.6924937012294379 + }, + { + "x": 3.57574133188106, + "y": 2.4662968714042055, + "heading": -0.3932458693377865, + "angularVelocity": 0.8412930922288103, + "velocityX": 2.963083160600891, + "velocityY": -1.4255801188754254, + "timestamp": 0.7554476740684777 + }, + { + "x": 3.777810779911522, + "y": 2.369066952878771, + "heading": -0.3455559373144148, + "angularVelocity": 0.7575364964702819, + "velocityX": 3.209796600877132, + "velocityY": -1.5444604071935277, + "timestamp": 0.8184016469075175 + }, + { + "x": 3.9925130304181535, + "y": 2.2661623966169606, + "heading": -0.3455557491568548, + "angularVelocity": 0.0000029888115329808544, + "velocityX": 3.4104638805811245, + "velocityY": -1.63459987703898, + "timestamp": 0.8813556197465573 + }, + { + "x": 4.207067940824861, + "y": 2.162950872410893, + "heading": -0.34555573251479127, + "angularVelocity": 2.643528717349897e-7, + "velocityX": 3.408123438933375, + "velocityY": -1.6394759464975768, + "timestamp": 0.9443095925855971 + }, + { + "x": 4.421622842445454, + "y": 2.0597393299403546, + "heading": -0.34555571587272765, + "angularVelocity": 2.6435287281597733e-7, + "velocityX": 3.408123299369276, + "velocityY": -1.6394762366217663, + "timestamp": 1.007263565424637 + }, + { + "x": 4.636177744065525, + "y": 1.9565277874687297, + "heading": -0.3455556992306641, + "angularVelocity": 2.6435287314542613e-7, + "velocityX": 3.4081232993609745, + "velocityY": -1.6394762366390223, + "timestamp": 1.0702175382636767 + }, + { + "x": 4.850732645688907, + "y": 1.8533162450039902, + "heading": -0.34555568258860053, + "angularVelocity": 2.643528725209065e-7, + "velocityX": 3.408123299413585, + "velocityY": -1.6394762365296556, + "timestamp": 1.1331715111027165 + }, + { + "x": 5.065287602977393, + "y": 1.7501048182552732, + "heading": -0.345555665946537, + "angularVelocity": 2.6435287138683083e-7, + "velocityX": 3.4081241836326663, + "velocityY": -1.63947439842449, + "timestamp": 1.1961254839417563 + }, + { + "x": 5.280754789180012, + "y": 1.6488115807144432, + "heading": -0.34555564929395255, + "angularVelocity": 2.6451999265849417e-7, + "velocityX": 3.4226145942134942, + "velocityY": -1.6090046898202166, + "timestamp": 1.2590794567807961 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 2.673332686258823e-7, + "velocityX": 3.530338087601756, + "velocityY": -1.3564290298187671, + "timestamp": 1.322033429619836 + }, + { + "x": 5.652794664229654, + "y": 1.5139935049725783, + "heading": -0.34555563561361163, + "angularVelocity": -7.551097078688917e-8, + "velocityX": 3.591492706822241, + "velocityY": -1.185058979367509, + "timestamp": 1.363740619136216 + }, + { + "x": 5.804789442560329, + "y": 1.4718291930788845, + "heading": -0.345555638724863, + "angularVelocity": -7.459748339352162e-8, + "velocityX": 3.644330392269195, + "velocityY": -1.010960277655107, + "timestamp": 1.405447808652596 + }, + { + "x": 5.958396699024851, + "y": 1.435981562464388, + "heading": -0.3455556418269712, + "angularVelocity": -7.437825995471752e-8, + "velocityX": 3.6829922669374113, + "velocityY": -0.8595072223799174, + "timestamp": 1.4471549981689762 + }, + { + "x": 6.112005102436535, + "y": 1.4001388468787674, + "heading": -0.3455556449290776, + "angularVelocity": -7.437821592181375e-8, + "velocityX": 3.6830197669242586, + "velocityY": -0.8593893762979032, + "timestamp": 1.4888621876853563 + }, + { + "x": 6.2656135060730795, + "y": 1.3642961322568121, + "heading": -0.3455556480311839, + "angularVelocity": -7.437821455888137e-8, + "velocityX": 3.683019772315654, + "velocityY": -0.8593893531924107, + "timestamp": 1.5305693772017364 + }, + { + "x": 6.419221909709655, + "y": 1.3284534176349936, + "heading": -0.3455556511332902, + "angularVelocity": -7.437821520024811e-8, + "velocityX": 3.6830197723164173, + "velocityY": -0.8593893531891351, + "timestamp": 1.5722765667181164 + }, + { + "x": 6.572830313283965, + "y": 1.292610702746327, + "heading": -0.3455556542353965, + "angularVelocity": -7.437821564310945e-8, + "velocityX": 3.6830197708234897, + "velocityY": -0.859389359587264, + "timestamp": 1.6139837562344965 + }, + { + "x": 6.72643839927336, + "y": 1.2567666268353574, + "heading": -0.34555565733750304, + "angularVelocity": -7.43782201062097e-8, + "velocityX": 3.6830121561911366, + "velocityY": -0.8594219923855618, + "timestamp": 1.6556909457508766 + }, + { + "x": 6.87895592150965, + "y": 1.216534214678565, + "heading": -0.3455556607967647, + "angularVelocity": -8.294161584911855e-8, + "velocityX": 3.656864056409029, + "velocityY": -0.9646397329408027, + "timestamp": 1.6973981352672567 + }, + { + "x": 7.025398163354816, + "y": 1.1717644512232448, + "heading": -0.34674583940477344, + "angularVelocity": -0.02853653343247435, + "velocityX": 3.5111989933450576, + "velocityY": -1.073430359955979, + "timestamp": 1.7391053247836368 + }, + { + "x": 7.164406770259058, + "y": 1.1285160108597303, + "heading": -0.3473601506358553, + "angularVelocity": -0.014729144739916763, + "velocityX": 3.3329650958535093, + "velocityY": -1.0369540806994204, + "timestamp": 1.7808125143000169 + }, + { + "x": 7.296003099222487, + "y": 1.086889230520204, + "heading": -0.3473718226301098, + "angularVelocity": -0.0002798556889061875, + "velocityX": 3.155243268351752, + "velocityY": -0.9980720547755352, + "timestamp": 1.822519703816397 + }, + { + "x": 7.420194691021883, + "y": 1.0469180216341771, + "heading": -0.34677182369168125, + "angularVelocity": 0.014385983457189529, + "velocityX": 2.977702243653219, + "velocityY": -0.9583769453064839, + "timestamp": 1.864226893332777 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.02916022972351966, + "velocityX": 2.8002535694982806, + "velocityY": -0.9182731897618387, + "timestamp": 1.905934082849157 + }, + { + "x": 7.694109552956691, + "y": 0.9554552181085981, + "heading": -0.3423846619503678, + "angularVelocity": 0.05119624293404516, + "velocityX": 2.536815276765165, + "velocityY": -0.8583516550337287, + "timestamp": 1.9678716456890424 + }, + { + "x": 7.835030504559327, + "y": 0.9064832871477253, + "heading": -0.33886448301682054, + "angularVelocity": 0.05683431462499228, + "velocityX": 2.2752098264978664, + "velocityY": -0.7906660952654867, + "timestamp": 2.0298092085289277 + }, + { + "x": 7.959827163561028, + "y": 0.861997905644216, + "heading": -0.3356203872211221, + "angularVelocity": 0.05237687191671972, + "velocityX": 2.014878424007598, + "velocityY": -0.7182294469433418, + "timestamp": 2.091746771368813 + }, + { + "x": 8.068556860689503, + "y": 0.8221974775860657, + "heading": -0.3330761096963967, + "angularVelocity": 0.04107810201222393, + "velocityX": 1.7554726428218221, + "velocityY": -0.6425895084221868, + "timestamp": 2.153684334208698 + }, + { + "x": 8.161262929678218, + "y": 0.7872247496182815, + "heading": -0.33153778712366017, + "angularVelocity": 0.02483666618774324, + "velocityX": 1.4967664973897774, + "velocityY": -0.5646448837225355, + "timestamp": 2.2156218970485835 + }, + { + "x": 8.237979200485006, + "y": 0.7571873196552872, + "heading": -0.33123696700010097, + "angularVelocity": 0.0048568285506627, + "velocityX": 1.2386065464846836, + "velocityY": -0.48496305934161354, + "timestamp": 2.2775594598884688 + }, + { + "x": 8.298732784455282, + "y": 0.7321691813322279, + "heading": -0.3323549200927614, + "angularVelocity": -0.018049678440697953, + "velocityX": 0.9808843161512628, + "velocityY": -0.4039251332464228, + "timestamp": 2.339497022728354 + }, + { + "x": 8.34354587924, + "y": 0.712237715373984, + "heading": -0.3350374219820423, + "angularVelocity": -0.04330977465508917, + "velocityX": 0.7235204733606144, + "velocityY": -0.3217993257140046, + "timestamp": 2.4014345855682393 + }, + { + "x": 8.372436985155346, + "y": 0.6974481694465224, + "heading": -0.33940426042561933, + "angularVelocity": -0.07050387912204041, + "velocityX": 0.46645532356563796, + "velocityY": -0.23878152851596665, + "timestamp": 2.4633721484081246 + }, + { + "x": 8.385421752929688, + "y": 0.6878466606140137, + "heading": -0.34555563246426124, + "angularVelocity": -0.09931569400855825, + "velocityX": 0.20964285934061647, + "velocityY": -0.15501915787887396, + "timestamp": 2.52530971124801 + }, + { + "x": 8.381221613866826, + "y": 0.6835499753387894, + "heading": -0.35422874908994917, + "angularVelocity": -0.13146724375807764, + "velocityX": -0.06366577665514053, + "velocityY": -0.06512922572222897, + "timestamp": 2.591281404102908 + }, + { + "x": 8.358990442151228, + "y": 0.6851839721097847, + "heading": -0.36494544812778174, + "angularVelocity": -0.16244389940702994, + "velocityX": -0.33698046470466403, + "velocityY": 0.024768149797052455, + "timestamp": 2.657253096957806 + }, + { + "x": 8.318727796191062, + "y": 0.692749219284977, + "heading": -0.37761522689458643, + "angularVelocity": -0.1920487139032685, + "velocityX": -0.6103018464104298, + "velocityY": 0.1146741404958606, + "timestamp": 2.723224789812704 + }, + { + "x": 8.260433187957593, + "y": 0.7062463828943455, + "heading": -0.3921309909220783, + "angularVelocity": -0.22003018869651583, + "velocityX": -0.8836306256637128, + "velocityY": 0.20459022688799033, + "timestamp": 2.7891964826676023 + }, + { + "x": 8.184106080259626, + "y": 0.7256762539187386, + "heading": -0.40836397752669645, + "angularVelocity": -0.24605987662499226, + "velocityX": -1.1569675476699668, + "velocityY": 0.2945183029807682, + "timestamp": 2.8551681755225005 + }, + { + "x": 8.089745888152331, + "y": 0.7510397855554056, + "heading": -0.4261563635075344, + "angularVelocity": -0.26969727789116266, + "velocityX": -1.4303133362794993, + "velocityY": 0.3844608276530502, + "timestamp": 2.9211398683773986 + }, + { + "x": 7.977351991056692, + "y": 0.7823381451863918, + "heading": -0.445310079113529, + "angularVelocity": -0.2903323346290704, + "velocityX": -1.7036685316358853, + "velocityY": 0.47442104752147907, + "timestamp": 2.9871115612322967 + }, + { + "x": 7.846923771917719, + "y": 0.8195727882757919, + "heading": -0.4655690572275494, + "angularVelocity": -0.30708592181466465, + "velocityX": -1.9770330803219518, + "velocityY": 0.5644033293384785, + "timestamp": 3.053083254087195 + }, + { + "x": 7.6984607267480145, + "y": 0.8627455646922209, + "heading": -0.48658930450770715, + "angularVelocity": -0.31862525229405353, + "velocityX": -2.2504052684572193, + "velocityY": 0.6544136514942158, + "timestamp": 3.119054946942093 + }, + { + "x": 7.531962771855591, + "y": 0.9118588688249081, + "heading": -0.5078842268520795, + "angularVelocity": -0.32278878141280287, + "velocityX": -2.5237787251969923, + "velocityY": 0.7444602678410827, + "timestamp": 3.185026639796991 + }, + { + "x": 7.347431182646945, + "y": 0.9669158200795512, + "heading": -0.5287129812318903, + "angularVelocity": -0.31572259977658373, + "velocityX": -2.7971328493042473, + "velocityY": 0.8345541681905657, + "timestamp": 3.250998332651889 + }, + { + "x": 7.144872022891467, + "y": 1.0279202386913098, + "heading": -0.5478111966657127, + "angularVelocity": -0.28949106211095804, + "velocityX": -3.0703950586958326, + "velocityY": 0.9247059757271185, + "timestamp": 3.3169700255067873 + }, + { + "x": 6.92431385893063, + "y": 1.0948740954299652, + "heading": -0.56252983495188, + "angularVelocity": -0.2231053600297971, + "velocityX": -3.343224259015806, + "velocityY": 1.0148876562242728, + "timestamp": 3.3829417183616854 + }, + { + "x": 6.686027050772059, + "y": 1.1677363127465568, + "heading": -0.5635938882487441, + "angularVelocity": -0.016128937288368943, + "velocityX": -3.611955337915506, + "velocityY": 1.1044466825619976, + "timestamp": 3.4489134112165836 + }, + { + "x": 6.448996512762522, + "y": 1.2456322264917639, + "heading": -0.5635938947986299, + "angularVelocity": -9.928327608034937e-8, + "velocityX": -3.592912774436053, + "velocityY": 1.180747535409401, + "timestamp": 3.5148851040714817 + }, + { + "x": 6.2119660667132655, + "y": 1.3235284200636117, + "heading": -0.5635939013485092, + "angularVelocity": -9.928317804375559e-8, + "velocityX": -3.5929113805006425, + "velocityY": 1.180751777026201, + "timestamp": 3.58085679692638 + }, + { + "x": 5.97493562067527, + "y": 1.4014246136697293, + "heading": -0.5635939078983885, + "angularVelocity": -9.928317794724392e-8, + "velocityX": -3.5929113803299297, + "velocityY": 1.180751777545662, + "timestamp": 3.646828489781278 + }, + { + "x": 5.737905297280568, + "y": 1.4793211804663164, + "heading": -0.5635939144482677, + "angularVelocity": -9.928317524609207e-8, + "velocityX": -3.5929095213008893, + "velocityY": 1.180757434372905, + "timestamp": 3.712800182636176 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.5635939210079286, + "angularVelocity": -9.943144757674115e-8, + "velocityX": -3.5606438133655525, + "velocityY": 1.2747558885869388, + "timestamp": 3.778771875491074 + }, + { + "x": 5.2859625551559315, + "y": 1.6593525606269757, + "heading": -0.5635939265318385, + "angularVelocity": -8.803796196510374e-8, + "velocityX": -3.459117077662582, + "velocityY": 1.5289526284347386, + "timestamp": 3.841516511544345 + }, + { + "x": 5.071426966449666, + "y": 1.7607653123039275, + "heading": -0.5635939320385204, + "angularVelocity": -8.776338927441567e-8, + "velocityX": -3.4191861201350386, + "velocityY": 1.6162776303436093, + "timestamp": 3.904261147597616 + }, + { + "x": 4.856891543272159, + "y": 1.8621784141511948, + "heading": -0.5635939375452019, + "angularVelocity": -8.776338338833754e-8, + "velocityX": -3.419183482000995, + "velocityY": 1.6162832112241017, + "timestamp": 3.9670057836508867 + }, + { + "x": 4.642356120103932, + "y": 1.9635915160180906, + "heading": -0.5635939430518834, + "angularVelocity": -8.776338232016452e-8, + "velocityX": -3.419183481853115, + "velocityY": 1.6162832115369357, + "timestamp": 4.029750419704158 + }, + { + "x": 4.427820696935705, + "y": 2.0650046178849877, + "heading": -0.5635939485585649, + "angularVelocity": -8.776338320403535e-8, + "velocityX": -3.419183481853107, + "velocityY": 1.616283211536953, + "timestamp": 4.0924950557574284 + }, + { + "x": 4.213285273767479, + "y": 2.1664177197518852, + "heading": -0.5635939540652464, + "angularVelocity": -8.776338205784925e-8, + "velocityX": -3.419183481853105, + "velocityY": 1.616283211536959, + "timestamp": 4.155239691810699 + }, + { + "x": 3.99874985060226, + "y": 2.2678308216251453, + "heading": -0.5635939595719279, + "angularVelocity": -8.776338204240658e-8, + "velocityX": -3.4191834818051703, + "velocityY": 1.616283211638361, + "timestamp": 4.21798432786397 + }, + { + "x": 3.7842144810928517, + "y": 2.369244037002313, + "heading": -0.5635939650808715, + "angularVelocity": -8.779943360299573e-8, + "velocityX": -3.4191826266593917, + "velocityY": 1.6162850206202093, + "timestamp": 4.280728963917241 + }, + { + "x": 3.582486282432141, + "y": 2.464592725052892, + "heading": -0.5976475269249973, + "angularVelocity": -0.5427326379774409, + "velocityX": -3.21506683837393, + "velocityY": 1.5196309046980254, + "timestamp": 4.343473599970512 + }, + { + "x": 3.3962755581787456, + "y": 2.5526066599990593, + "heading": -0.6290952664889575, + "angularVelocity": -0.5012020396016188, + "velocityX": -2.9677552690767404, + "velocityY": 1.4027324163844592, + "timestamp": 4.406218236023783 + }, + { + "x": 3.2255823358404876, + "y": 2.63328589857046, + "heading": -0.6579338206032609, + "angularVelocity": -0.4596178403173682, + "velocityX": -2.7204432613704155, + "velocityY": 1.2858348322062758, + "timestamp": 4.468962872077054 + }, + { + "x": 3.0704066332901094, + "y": 2.7066304971231805, + "heading": -0.684160167831089, + "angularVelocity": -0.4179854865292692, + "velocityX": -2.4731309688151453, + "velocityY": 1.168938146209836, + "timestamp": 4.531707508130324 + }, + { + "x": 2.930748464239733, + "y": 2.7726405069094633, + "heading": -0.7077714539525848, + "angularVelocity": -0.3763076432772636, + "velocityX": -2.22581845772133, + "velocityY": 1.05204227705202, + "timestamp": 4.594452144183595 + }, + { + "x": 2.8066078397359373, + "y": 2.83131597316776, + "heading": -0.7287650298436226, + "angularVelocity": -0.3345875792986393, + "velocityX": -1.978505770571369, + "velocityY": 0.9351471288873412, + "timestamp": 4.657196780236866 + }, + { + "x": 2.697984768963403, + "y": 2.882656934878273, + "heading": -0.7471385155745514, + "angularVelocity": -0.2928295849119221, + "velocityX": -1.7311929370394774, + "velocityY": 0.8182526019742015, + "timestamp": 4.719941416290137 + }, + { + "x": 2.604879259815539, + "y": 2.926663424723888, + "heading": -0.7628898637084475, + "angularVelocity": -0.25103895925897446, + "velocityX": -1.4838799777054614, + "velocityY": 0.7013585959483968, + "timestamp": 4.782686052343408 + }, + { + "x": 2.5272913193421016, + "y": 2.963335469132253, + "heading": -0.7760174152523261, + "angularVelocity": -0.20922189321065382, + "velocityX": -1.236566906015123, + "velocityY": 0.584465011116332, + "timestamp": 4.845430688396679 + }, + { + "x": 2.465220954107753, + "y": 2.9926730883504185, + "heading": -0.7865199463491176, + "angularVelocity": -0.1673853217966667, + "velocityX": -0.9892537296997038, + "velocityY": 0.46757174897401693, + "timestamp": 4.9081753244499495 + }, + { + "x": 2.418668170473455, + "y": 3.014676296527397, + "heading": -0.7943967051912195, + "angularVelocity": -0.12553676836079783, + "velocityX": -0.7419404520057251, + "velocityY": 0.3506787123332304, + "timestamp": 4.97091996050322 + }, + { + "x": 2.38763297480514, + "y": 3.0293451017899056, + "heading": -0.7996474391139023, + "angularVelocity": -0.08368418805115034, + "velocityX": -0.49462707285394797, + "velocityY": 0.23378580521296682, + "timestamp": 5.033664596556491 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": -0.0418358130797821, + "velocityX": -0.24731358996990807, + "velocityY": 0.11689293258068652, + "timestamp": 5.096409232609762 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": 1.2905382194087526e-36, + "velocityX": 1.2188676912146853e-38, + "velocityY": -1.9456328527917923e-39, + "timestamp": 5.159153868663033 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint (1).4.traj b/src/main/deploy/choreo/SourceLanePHGSprint (1).4.traj new file mode 100644 index 00000000..274b8c90 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePHGSprint (1).4.traj @@ -0,0 +1,355 @@ +{ + "samples": [ + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": 1.2905382194087526e-36, + "velocityX": 1.2188676912146853e-38, + "velocityY": -1.9456328527917923e-39, + "timestamp": 0 + }, + { + "x": 2.3862784269967885, + "y": 3.0299921206795677, + "heading": -0.7979445022798209, + "angularVelocity": 0.07214760590413372, + "velocityX": 0.23610252175550234, + "velocityY": -0.11148080618080955, + "timestamp": 0.059986878920357256 + }, + { + "x": 2.414605712310544, + "y": 3.016617241283919, + "heading": -0.7893555783917804, + "angularVelocity": 0.14318004274641105, + "velocityX": 0.4722246901920793, + "velocityY": -0.22296341527296173, + "timestamp": 0.11997375784071451 + }, + { + "x": 2.457098595113841, + "y": 2.996554734272641, + "heading": -0.7765849369819305, + "angularVelocity": 0.21289057940162145, + "velocityX": 0.7083696229589316, + "velocityY": -0.3344482555579276, + "timestamp": 0.17996063676107177 + }, + { + "x": 2.5137586742709717, + "y": 2.9698044458506936, + "heading": -0.7597273153290622, + "angularVelocity": 0.28102181604163, + "velocityX": 0.9445412092927212, + "velocityY": -0.44593565965421605, + "timestamp": 0.23994751568142902 + }, + { + "x": 2.5845878442638655, + "y": 2.9363662114763, + "heading": -0.7388971783301495, + "angularVelocity": 0.34724488711219914, + "velocityX": 1.180744377231747, + "velocityY": -0.5574258067133014, + "timestamp": 0.2999343946017863 + }, + { + "x": 2.6695883810159726, + "y": 2.8962398712988433, + "heading": -0.7142349782126007, + "angularVelocity": 0.4111265757015274, + "velocityX": 1.4169854855252446, + "velocityY": -0.6689186185320777, + "timestamp": 0.35992127352214354 + }, + { + "x": 2.7687630628600255, + "y": 2.849425297465716, + "heading": -0.6859166617272401, + "angularVelocity": 0.4720751770225938, + "velocityX": 1.653272909492815, + "velocityY": -0.7804135616937368, + "timestamp": 0.4199081524425008 + }, + { + "x": 2.882115344958, + "y": 2.795922445610523, + "heading": -0.6541688123580988, + "angularVelocity": 0.529246560923628, + "velocityX": 1.8896179320892517, + "velocityY": -0.8919092444570627, + "timestamp": 0.47989503136285805 + }, + { + "x": 3.009649615335316, + "y": 2.73573145985777, + "heading": -0.6192942663212875, + "angularVelocity": 0.5813695705541366, + "velocityX": 2.1260361044394105, + "velocityY": -1.0034025246198652, + "timestamp": 0.5398819102832153 + }, + { + "x": 3.1513715692279023, + "y": 2.6688529097388005, + "heading": -0.5817190571900686, + "angularVelocity": 0.626390467507171, + "velocityX": 2.3625492181506127, + "velocityY": -1.114886310517361, + "timestamp": 0.5998687892035726 + }, + { + "x": 3.3072887013147403, + "y": 2.5952884046594, + "heading": -0.5420886244414693, + "angularVelocity": 0.6606516868666416, + "velocityX": 2.5991872705003334, + "velocityY": -1.2263432671179573, + "timestamp": 0.6598556681239298 + }, + { + "x": 3.4774104648990076, + "y": 2.5150425607670805, + "heading": -0.50150111026719, + "angularVelocity": 0.6766065330414328, + "velocityX": 2.8359829123654157, + "velocityY": -1.33772327109832, + "timestamp": 0.7198425470442871 + }, + { + "x": 3.6617427045443476, + "y": 2.428132084038069, + "heading": -0.4622625664613271, + "angularVelocity": 0.6541187758402723, + "velocityX": 3.0728759849311564, + "velocityY": -1.4488247812392312, + "timestamp": 0.7798294259646443 + }, + { + "x": 3.8601464039752953, + "y": 2.3346891355318427, + "heading": -0.4327535611710116, + "angularVelocity": 0.49192433114404066, + "velocityX": 3.3074516127828706, + "velocityY": -1.5577231252568986, + "timestamp": 0.8398163048850016 + }, + { + "x": 4.065242173401204, + "y": 2.237711412138144, + "heading": -0.4327535477418676, + "angularVelocity": 2.2386802351026997e-7, + "velocityX": 3.4190105089182294, + "velocityY": -1.6166489262168848, + "timestamp": 0.8998031838053588 + }, + { + "x": 4.2703379249938225, + "y": 2.140733651028875, + "heading": -0.43275353431315516, + "angularVelocity": 2.2386082807599726e-7, + "velocityX": 3.419010211631728, + "velocityY": -1.6166495549472233, + "timestamp": 0.9597900627257161 + }, + { + "x": 4.475433676585232, + "y": 2.0437558899170503, + "heading": -0.43275352088444274, + "angularVelocity": 2.2386082918741598e-7, + "velocityX": 3.4190102116115857, + "velocityY": -1.616649554989821, + "timestamp": 1.0197769416460734 + }, + { + "x": 4.6805294281766425, + "y": 1.9467781288052255, + "heading": -0.4327535074557304, + "angularVelocity": 2.2386082819075723e-7, + "velocityX": 3.4190102116115852, + "velocityY": -1.616649554989824, + "timestamp": 1.0797638205664306 + }, + { + "x": 4.885625179768054, + "y": 1.8498003676934025, + "heading": -0.43275349402701796, + "angularVelocity": 2.2386082903608912e-7, + "velocityX": 3.4190102116115972, + "velocityY": -1.6166495549897977, + "timestamp": 1.1397506994867879 + }, + { + "x": 5.090720931370459, + "y": 1.7528226066048298, + "heading": -0.43275348059830554, + "angularVelocity": 2.2386082830289002e-7, + "velocityX": 3.4190102117948658, + "velocityY": -1.6166495546022077, + "timestamp": 1.1997375784071451 + }, + { + "x": 5.29581684523517, + "y": 1.6558451886813434, + "heading": -0.43275346716959256, + "angularVelocity": 2.2386083719980442e-7, + "velocityX": 3.4190129167581613, + "velocityY": -1.6166438339330798, + "timestamp": 1.2597244573275024 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.43275345370859003, + "angularVelocity": 2.243991161112281e-7, + "velocityX": 3.453867841656271, + "velocityY": -1.5407736813730113, + "timestamp": 1.3197113362478596 + }, + { + "x": 5.767637732978587, + "y": 1.4715486447521433, + "heading": -0.4327534418952186, + "angularVelocity": 1.5949042666645356e-7, + "velocityX": 3.572782883962456, + "velocityY": -1.2403266728714897, + "timestamp": 1.3937808065498363 + }, + { + "x": 6.033774192023134, + "y": 1.384125705645109, + "heading": -0.432753430099036, + "angularVelocity": 1.5925836332865082e-7, + "velocityX": 3.593065509440347, + "velocityY": -1.1802830336252828, + "timestamp": 1.467850276851813 + }, + { + "x": 6.2999107025526015, + "y": 1.2967029232707166, + "heading": -0.43275341830285335, + "angularVelocity": 1.5925836332521055e-7, + "velocityX": 3.5930662045299844, + "velocityY": -1.1802809176031086, + "timestamp": 1.5419197471537895 + }, + { + "x": 6.566047160708243, + "y": 1.2092799814598572, + "heading": -0.43275340650489624, + "angularVelocity": 1.5928232033441407e-7, + "velocityX": 3.593065497439389, + "velocityY": -1.1802830701291938, + "timestamp": 1.6159892174557662 + }, + { + "x": 6.815604168443372, + "y": 1.1270295237235333, + "heading": -0.36800659105834704, + "angularVelocity": 0.8741363369088632, + "velocityX": 3.3692290051177802, + "velocityY": -1.110450194945293, + "timestamp": 1.6900586877577428 + }, + { + "x": 7.0424486777806585, + "y": 1.0523256982782683, + "heading": -0.304588986464792, + "angularVelocity": 0.8561908750664192, + "velocityX": 3.0625912189253843, + "velocityY": -1.0085643267152118, + "timestamp": 1.7641281580597195 + }, + { + "x": 7.246590574581559, + "y": 0.9851213052520007, + "heading": -0.24567984244132185, + "angularVelocity": 0.7953228743678282, + "velocityX": 2.7560869001577597, + "velocityY": -0.9073156963628868, + "timestamp": 1.838197628361696 + }, + { + "x": 7.428038548794457, + "y": 0.9253999088853845, + "heading": -0.19230647763371986, + "angularVelocity": 0.7205852099387576, + "velocityX": 2.4496999029849618, + "velocityY": -0.8062889625528061, + "timestamp": 1.9122670986636727 + }, + { + "x": 7.586797949690041, + "y": 0.8731533407207944, + "heading": -0.1449799929692073, + "angularVelocity": 0.6389472541327181, + "velocityX": 2.1433851254549694, + "velocityY": -0.705372509774734, + "timestamp": 1.9863365689656494 + }, + { + "x": 7.722872327084393, + "y": 0.8283767866857153, + "heading": -0.10400779573923778, + "angularVelocity": 0.5531590419497899, + "velocityX": 1.8371182734206921, + "velocityY": -0.6045210510150558, + "timestamp": 2.060406039267626 + }, + { + "x": 7.836264194581954, + "y": 0.7910670998170554, + "heading": -0.06959570799723012, + "angularVelocity": 0.46459205934255987, + "velocityX": 1.530885357155523, + "velocityY": -0.5037120788977025, + "timestamp": 2.1344755095696026 + }, + { + "x": 7.926975414267802, + "y": 0.7612220655404804, + "heading": -0.04189104317473826, + "angularVelocity": 0.3740362218001811, + "velocityX": 1.224677580601353, + "velocityY": -0.4029330053920843, + "timestamp": 2.2085449798715793 + }, + { + "x": 7.995007406512269, + "y": 0.7388400306235591, + "heading": -0.021003806792755122, + "angularVelocity": 0.28199521741990763, + "velocityX": 0.9184889802384877, + "velocityY": -0.3021762519115013, + "timestamp": 2.282614450173556 + }, + { + "x": 8.040361272813582, + "y": 0.7239196956147146, + "heading": -0.0070182972067734635, + "angularVelocity": 0.18881611450660718, + "velocityX": 0.612315251025934, + "velocityY": -0.20143704211755914, + "timestamp": 2.3566839204755325 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -5.172058523180806e-42, + "angularVelocity": 0.09475290127174331, + "velocityX": 0.30615312096089364, + "velocityY": -0.10071229126619016, + "timestamp": 2.430753390777509 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -2.2197990868657932e-42, + "angularVelocity": 3.9858008032330975e-41, + "velocityX": -4.443522904196121e-42, + "velocityY": 7.940802598890531e-42, + "timestamp": 2.504822861079486 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint (1).traj b/src/main/deploy/choreo/SourceLanePHGSprint (1).traj new file mode 100644 index 00000000..6d244255 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePHGSprint (1).traj @@ -0,0 +1,2101 @@ +{ + "samples": [ + { + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": -3.528930169589175e-36, + "angularVelocity": 0, + "velocityX": -3.4629926489463697e-37, + "velocityY": -7.866244288467422e-37, + "timestamp": 0 + }, + { + "x": 0.45469188032137553, + "y": 4.109218526997661, + "heading": -0.008937300145995173, + "angularVelocity": -0.1189323726299145, + "velocityX": 0.2854425752342375, + "velocityY": -0.15856789979825622, + "timestamp": 0.07514606787342622 + }, + { + "x": 0.49759166351162515, + "y": 4.0853873108981436, + "heading": -0.026812413296636604, + "angularVelocity": -0.237871570083344, + "velocityX": 0.5708852692400179, + "velocityY": -0.3171319108760035, + "timestamp": 0.15029213574685243 + }, + { + "x": 0.5619414537480725, + "y": 4.049641027140498, + "heading": -0.053621717098024775, + "angularVelocity": -0.3567625633658565, + "velocityX": 0.856329440215504, + "velocityY": -0.4756906750976659, + "timestamp": 0.22543820362027867 + }, + { + "x": 0.6477414523773106, + "y": 4.001980177860296, + "heading": -0.08935853104911512, + "angularVelocity": -0.47556465644063195, + "velocityX": 1.141776290593902, + "velocityY": -0.6342427571923072, + "timestamp": 0.30058427149370487 + }, + { + "x": 0.7549919358777414, + "y": 3.942405375853288, + "heading": -0.1340144220065316, + "angularVelocity": -0.5942545261667388, + "velocityX": 1.4272268201854585, + "velocityY": -0.7927866845588477, + "timestamp": 0.37573033936713107 + }, + { + "x": 0.8836932393269182, + "y": 3.8709173416225418, + "heading": -0.18758057158075767, + "angularVelocity": -0.7128270459134506, + "velocityX": 1.712681808793474, + "velocityY": -0.951321023891208, + "timestamp": 0.4508764072405573 + }, + { + "x": 1.0338457421402325, + "y": 3.787516893297593, + "heading": -0.2500489995023071, + "angularVelocity": -0.8312933688928251, + "velocityX": 1.9981418464400118, + "velocityY": -1.1098444760333435, + "timestamp": 0.5260224751139835 + }, + { + "x": 1.2054498603190866, + "y": 3.6922049311693006, + "heading": -0.32141344961465573, + "angularVelocity": -0.9496764385936043, + "velocityX": 2.283607419990343, + "velocityY": -1.268355947630325, + "timestamp": 0.6011685429874097 + }, + { + "x": 1.3985060453231255, + "y": 3.584982424979259, + "heading": -0.4016698579859379, + "angularVelocity": -1.0680054278616897, + "velocityX": 2.5690790013020615, + "velocityY": -1.4268545144723217, + "timestamp": 0.676314610860836 + }, + { + "x": 1.5915937569821008, + "y": 3.4778198154599647, + "heading": -0.48176859267038585, + "angularVelocity": -1.065907198489266, + "velocityX": 2.5694985396202825, + "velocityY": -1.4260574445464829, + "timestamp": 0.7514606787342623 + }, + { + "x": 1.7632284858900336, + "y": 3.3825660365200445, + "heading": -0.5529765476804539, + "angularVelocity": -0.9475938931363466, + "velocityX": 2.2840147697019786, + "velocityY": -1.2675816797275798, + "timestamp": 0.8266067466076885 + }, + { + "x": 1.9134098098562764, + "y": 3.2992204007390877, + "heading": -0.6152918418017526, + "angularVelocity": -0.8292555536806094, + "velocityX": 1.9985253815170194, + "velocityY": -1.1091150626981794, + "timestamp": 0.9017528144811148 + }, + { + "x": 2.0421373577646693, + "y": 3.2277823059512354, + "heading": -0.6687115003742365, + "angularVelocity": -0.7108776291856339, + "velocityX": 1.7130310547348626, + "velocityY": -0.9506564589404817, + "timestamp": 0.976898882354541 + }, + { + "x": 2.149410808485792, + "y": 3.1682512394298143, + "heading": -0.7132320418113071, + "angularVelocity": -0.5924533737687983, + "velocityX": 1.4275324545498613, + "velocityY": -0.7922046782500071, + "timestamp": 1.0520449502279672 + }, + { + "x": 2.2352298936713018, + "y": 3.120626784362015, + "heading": -0.7488501306819737, + "angularVelocity": -0.4739847323836121, + "velocityX": 1.1420302833417786, + "velocityY": -0.6337584442610683, + "timestamp": 1.1271910181013933 + }, + { + "x": 2.2995943987852443, + "y": 3.0849086278583613, + "heading": -0.775563262460788, + "angularVelocity": -0.3554827622359309, + "velocityX": 0.8565252572144717, + "velocityY": -0.4753163740226096, + "timestamp": 1.2023370859748195 + }, + { + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080858, + "angularVelocity": -0.23696688264902366, + "velocityX": 0.5710180813549187, + "velocityY": -0.3168769856057537, + "timestamp": 1.2774831538482456 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288599550472, + "velocityX": 0.28550943973314774, + "velocityY": -0.15843872456938407, + "timestamp": 1.3526292217216718 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 9.576319315825878e-33, + "velocityX": -3.340074147697442e-33, + "velocityY": 1.4972964833121403e-33, + "timestamp": 1.427775289595098 + }, + { + "x": 2.3773546785315864, + "y": 3.0407954088560687, + "heading": -0.7860770412156382, + "angularVelocity": 0.26500042058499174, + "velocityX": 0.21918861143707746, + "velocityY": -0.13736692695017713, + "timestamp": 1.4888897992558612 + }, + { + "x": 2.4041715283150715, + "y": 3.0239975454455843, + "heading": -0.7541197928104101, + "angularVelocity": 0.5229077118120982, + "velocityX": 0.438796775632191, + "velocityY": -0.27485884291187945, + "timestamp": 1.5500043089166244 + }, + { + "x": 2.4444388170293236, + "y": 2.998788446329585, + "heading": -0.7069088657797715, + "angularVelocity": 0.7724994815911764, + "velocityX": 0.6588826276733535, + "velocityY": -0.41248959135778174, + "timestamp": 1.6111188185773877 + }, + { + "x": 2.498190798561039, + "y": 2.965157152573787, + "heading": -0.6450804787000076, + "angularVelocity": 1.0116809808826634, + "velocityX": 0.8795289666903056, + "velocityY": -0.5502996578468836, + "timestamp": 1.672233328238151 + }, + { + "x": 2.5654677170302604, + "y": 2.923088008476279, + "heading": -0.5694581806203519, + "angularVelocity": 1.2373869724133075, + "velocityX": 1.1008338092322851, + "velocityY": -0.6883658943027918, + "timestamp": 1.7333478378989142 + }, + { + "x": 2.646315775914165, + "y": 2.8725583813996622, + "heading": -0.48110952859275347, + "angularVelocity": 1.4456248199978636, + "velocityX": 1.3228946666295571, + "velocityY": -0.8268024624119353, + "timestamp": 1.7944623475596775 + }, + { + "x": 2.7407862870930626, + "y": 2.813537598490058, + "heading": -0.3813947698765895, + "angularVelocity": 1.631605313855327, + "velocityX": 1.5457951262848801, + "velocityY": -0.965740922036666, + "timestamp": 1.8555768572204407 + }, + { + "x": 2.84893500974016, + "y": 2.7459886359527177, + "heading": -0.2720274509201168, + "angularVelocity": 1.7895475160244767, + "velocityX": 1.7696079580350594, + "velocityY": -1.105285191884772, + "timestamp": 1.916691366881204 + }, + { + "x": 2.9708226363170516, + "y": 2.669873093439706, + "heading": -0.15523119808259803, + "angularVelocity": 1.9111051284848068, + "velocityX": 1.9944138839282306, + "velocityY": -1.2454577961194009, + "timestamp": 1.9778058765419673 + }, + { + "x": 3.1065122638812066, + "y": 2.5851587340080315, + "heading": -0.03419676462946552, + "angularVelocity": 1.9804533182868547, + "velocityX": 2.220252249708728, + "velocityY": -1.3861578846318265, + "timestamp": 2.0389203862027303 + }, + { + "x": 3.2560356571448454, + "y": 2.4918371927335334, + "heading": 0.08572826850512158, + "angularVelocity": 1.9623005044182085, + "velocityX": 2.446610372784124, + "velocityY": -1.5269948461095475, + "timestamp": 2.1000348958634936 + }, + { + "x": 3.419180860242083, + "y": 2.390029027608911, + "heading": 0.19415018142326163, + "angularVelocity": 1.7740780956923754, + "velocityX": 2.6695003200194134, + "velocityY": -1.6658591501387063, + "timestamp": 2.161149405524257 + }, + { + "x": 3.5929644641848295, + "y": 2.279455345020229, + "heading": 0.2670576620012504, + "angularVelocity": 1.1929651564364419, + "velocityX": 2.8435735622750062, + "velocityY": -1.809286914064415, + "timestamp": 2.22226391518502 + }, + { + "x": 3.776865546474961, + "y": 2.1600020280366277, + "heading": 0.3015223704483068, + "angularVelocity": 0.5639365944088309, + "velocityX": 3.0091230922237155, + "velocityY": -1.9545819421061736, + "timestamp": 2.2833784248457833 + }, + { + "x": 3.9728967066647076, + "y": 2.0375522576737963, + "heading": 0.30152241765502197, + "angularVelocity": 7.724305629887772e-7, + "velocityX": 3.2076042379769354, + "velocityY": -2.003612088889046, + "timestamp": 2.3444929345065466 + }, + { + "x": 4.174386727718979, + "y": 1.9243094504352285, + "heading": 0.30152246485493484, + "angularVelocity": 7.72319260587173e-7, + "velocityX": 3.2969260847008353, + "velocityY": -1.8529610704096402, + "timestamp": 2.40560744416731 + }, + { + "x": 4.383354452565465, + "y": 1.8255438101811148, + "heading": 0.3015225128834295, + "angularVelocity": 7.858771167686507e-7, + "velocityX": 3.4192817058736558, + "velocityY": -1.6160751481496929, + "timestamp": 2.466721953828073 + }, + { + "x": 4.598761540028333, + "y": 1.7417467464566943, + "heading": 0.30152256294147645, + "angularVelocity": 8.190861254028443e-7, + "velocityX": 3.524647234487482, + "velocityY": -1.3711484259558735, + "timestamp": 2.5278364634888364 + }, + { + "x": 4.819537390717514, + "y": 1.6733348748555603, + "heading": 0.3015226164690873, + "angularVelocity": 8.758576513463982e-7, + "velocityX": 3.612494838208991, + "velocityY": -1.1194047367945394, + "timestamp": 2.5889509731495997 + }, + { + "x": 5.04458467733647, + "y": 1.620648272623701, + "heading": 0.30152267534909954, + "angularVelocity": 9.634375302003616e-7, + "velocityX": 3.682387175617667, + "velocityY": -0.8620964567058493, + "timestamp": 2.650065482810363 + }, + { + "x": 5.27278482830379, + "y": 1.5839488347993986, + "heading": 0.3015227422489141, + "angularVelocity": 0.0000010946633609592033, + "velocityX": 3.7339766322927472, + "velocityY": -0.6005028597630111, + "timestamp": 2.711179992471126 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3015228161300745, + "angularVelocity": 0.0000012088972118666571, + "velocityX": 3.7670067261218194, + "velocityY": -0.33592432468507805, + "timestamp": 2.7722945021318894 + }, + { + "x": 5.642161233224445, + "y": 1.5569512204704454, + "heading": 0.30152289610635336, + "angularVelocity": 0.0000021712102287950276, + "velocityX": 3.777876226736954, + "velocityY": -0.1755880047102645, + "timestamp": 2.809129387028348 + }, + { + "x": 5.781468008961295, + "y": 1.556401100893367, + "heading": 0.3015229698630755, + "angularVelocity": 0.0000020023605971612715, + "velocityX": 3.7819250997644014, + "velocityY": -0.01493474402390838, + "timestamp": 2.8459642719248066 + }, + { + "x": 5.920672391336217, + "y": 1.5617696165699877, + "heading": 0.3015230386365789, + "angularVelocity": 0.0000018670752917486593, + "velocityX": 3.779145306581512, + "velocityY": 0.14574541746801997, + "timestamp": 2.882799156821265 + }, + { + "x": 6.0595230325638285, + "y": 1.5730470711005764, + "heading": 0.30152310340837535, + "angularVelocity": 0.000001758436237719359, + "velocityX": 3.769541879061509, + "velocityY": 0.3061623393771694, + "timestamp": 2.919634041717724 + }, + { + "x": 6.197769224123459, + "y": 1.5902130982406153, + "heading": 0.3015231649742211, + "angularVelocity": 0.0000016714005183062998, + "velocityX": 3.753132172076425, + "velocityY": 0.4660263548614807, + "timestamp": 2.9564689266141824 + }, + { + "x": 6.335161349542959, + "y": 1.6132366986444322, + "heading": 0.3015232239953201, + "angularVelocity": 0.0000016023152826544925, + "velocityX": 3.7299458327534505, + "velocityY": 0.6250487946015111, + "timestamp": 2.993303811510641 + }, + { + "x": 6.471451335210211, + "y": 1.6420762959087596, + "heading": 0.3015232810350899, + "angularVelocity": 0.000001548525804727782, + "velocityX": 3.7000247469309935, + "velocityY": 0.7829425107583309, + "timestamp": 3.0301386964070995 + }, + { + "x": 6.606393098338155, + "y": 1.6766798119201123, + "heading": 0.3015233366126912, + "angularVelocity": 0.0000015088305949550033, + "velocityX": 3.6634229618813627, + "velocityY": 0.9394224010369983, + "timestamp": 3.066973581303558 + }, + { + "x": 6.739742463422568, + "y": 1.7169846140875678, + "heading": 0.30152556567084704, + "angularVelocity": 0.00006051486687687375, + "velocityX": 3.6201922568579414, + "velocityY": 1.0942019306087243, + "timestamp": 3.1038084662000167 + }, + { + "x": 6.8694207904091344, + "y": 1.7614703171513668, + "heading": 0.3101205256410533, + "angularVelocity": 0.23333750042565127, + "velocityX": 3.520530262306672, + "velocityY": 1.2077057710061154, + "timestamp": 3.1406433510964753 + }, + { + "x": 6.994764102779472, + "y": 1.8086931078781319, + "heading": 0.3306180633638748, + "angularVelocity": 0.5564707961064402, + "velocityX": 3.4028425152588877, + "velocityY": 1.2820127132066854, + "timestamp": 3.177478235992934 + }, + { + "x": 7.114597005425528, + "y": 1.8565129203183168, + "heading": 0.359627713397502, + "angularVelocity": 0.7875591335542955, + "velocityX": 3.2532449329732183, + "velocityY": 1.298220764761567, + "timestamp": 3.2143131208893925 + }, + { + "x": 7.228563598795877, + "y": 1.903963761474251, + "heading": 0.39146295222083616, + "angularVelocity": 0.8642687200685345, + "velocityX": 3.0939853264291135, + "velocityY": 1.288203866777826, + "timestamp": 3.251148005785851 + }, + { + "x": 7.336676614220082, + "y": 1.9505339221399112, + "heading": 0.42341393247502157, + "angularVelocity": 0.8674108889982476, + "velocityX": 2.9350713522821095, + "velocityY": 1.2642949963483492, + "timestamp": 3.2879828906823096 + }, + { + "x": 7.438996464546078, + "y": 1.9959395107165516, + "heading": 0.45395278545537515, + "angularVelocity": 0.8290742068611577, + "velocityX": 2.777797476865021, + "velocityY": 1.2326789863542045, + "timestamp": 3.3248177755787682 + }, + { + "x": 7.535582073954796, + "y": 2.0400031624519186, + "heading": 0.4820829146791016, + "angularVelocity": 0.7636817463336469, + "velocityX": 2.622123285581467, + "velocityY": 1.1962478465516526, + "timestamp": 3.361652660475227 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6791276743107278, + "velocityX": 2.4678193928677445, + "velocityY": 1.1565397180800696, + "timestamp": 3.3984875453716854 + }, + { + "x": 7.766333855095899, + "y": 2.151391160116789, + "heading": 0.5393931714770351, + "angularVelocity": 0.508950730683683, + "velocityX": 2.2039777611896034, + "velocityY": 1.0840548028640593, + "timestamp": 3.461940970502476 + }, + { + "x": 7.8897168213730975, + "y": 2.2145350229010927, + "heading": 0.5621690879226864, + "angularVelocity": 0.3589391179231936, + "velocityX": 1.9444650312080196, + "velocityY": 0.9951214241650663, + "timestamp": 3.5253943956332665 + }, + { + "x": 7.996856912991746, + "y": 2.2713427782005033, + "heading": 0.5763089063837279, + "angularVelocity": 0.22283774960762856, + "velocityX": 1.6884839769927564, + "velocityY": 0.8952669644281364, + "timestamp": 3.588847820764057 + }, + { + "x": 8.08793407853681, + "y": 2.3213235528188796, + "heading": 0.5824533246990756, + "angularVelocity": 0.09683351690917823, + "velocityX": 1.4353388387992243, + "velocityY": 0.7876765441007405, + "timestamp": 3.6523012458948476 + }, + { + "x": 8.16309393881241, + "y": 2.364112462024999, + "heading": 0.5810872026138336, + "angularVelocity": -0.021529524725041688, + "velocityX": 1.1844886248564515, + "velocityY": 0.6743356898059116, + "timestamp": 3.715754671025638 + }, + { + "x": 8.222455939995415, + "y": 2.3994280671308665, + "heading": 0.5725898810376202, + "angularVelocity": -0.13391430893917458, + "velocityX": 0.9355208337555886, + "velocityY": 0.5565594770191679, + "timestamp": 3.7792080961564287 + }, + { + "x": 8.266119462815809, + "y": 2.427046891293226, + "heading": 0.5572661381126698, + "angularVelocity": -0.24149591441856877, + "velocityX": 0.6881192422693246, + "velocityY": 0.4352613606819736, + "timestamp": 3.8426615212872193 + }, + { + "x": 8.294168308263885, + "y": 2.446787281465013, + "heading": 0.5353660980042331, + "angularVelocity": -0.3451356654632322, + "velocityX": 0.4420383200790658, + "velocityY": 0.31110046669817776, + "timestamp": 3.90611494641801 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.4454857015776663, + "velocityX": 0.19708463824313482, + "velocityY": 0.18456741878341767, + "timestamp": 3.9695683715488004 + }, + { + "x": 8.303024530407173, + "y": 2.4618811275821986, + "heading": 0.4711910323232683, + "angularVelocity": -0.5465136547157353, + "velocityX": -0.055545178149817624, + "velocityY": 0.05148048067311967, + "timestamp": 4.035271161880262 + }, + { + "x": 8.282750419132642, + "y": 2.456558519757899, + "heading": 0.42887568169046636, + "angularVelocity": -0.6440419108431609, + "velocityX": -0.30857306321773753, + "velocityY": -0.08101037714605691, + "timestamp": 4.1009739522117235 + }, + { + "x": 8.245821395629305, + "y": 2.4425769262851884, + "heading": 0.3804195474569322, + "angularVelocity": -0.7375049672788591, + "velocityX": -0.5620617224479451, + "velocityY": -0.2128006040866038, + "timestamp": 4.166676742543185 + }, + { + "x": 8.19220201387014, + "y": 2.4199912479232255, + "heading": 0.32613641295591106, + "angularVelocity": -0.8261922245184766, + "velocityX": -0.8160898721144464, + "velocityY": -0.3437552385221621, + "timestamp": 4.2323795328746465 + }, + { + "x": 8.121850214183777, + "y": 2.388868090056124, + "heading": 0.2664003971864744, + "angularVelocity": -0.9091853704854365, + "velocityX": -1.0707581722397086, + "velocityY": -0.4736961354318394, + "timestamp": 4.298082323206108 + }, + { + "x": 8.034715295558527, + "y": 2.3492899199909276, + "heading": 0.20166644793914107, + "angularVelocity": -0.9852541866298157, + "velocityX": -1.32619814448768, + "velocityY": -0.6023818754961511, + "timestamp": 4.36378511353757 + }, + { + "x": 7.930734985336223, + "y": 2.3013613961682027, + "heading": 0.13250258742090648, + "angularVelocity": -1.0526776742557293, + "velocityX": -1.582585909939886, + "velocityY": -0.7294747084702521, + "timestamp": 4.429487903869031 + }, + { + "x": 7.809831081747402, + "y": 2.2452194829459002, + "heading": 0.05964299707705179, + "angularVelocity": -1.1089268808263428, + "velocityX": -1.8401639105261383, + "velocityY": -0.8544829365552755, + "timestamp": 4.495190694200493 + }, + { + "x": 7.671902780233126, + "y": 2.1810506699963903, + "heading": -0.015920256793081004, + "angularVelocity": -1.1500767850029923, + "velocityX": -2.099276161917122, + "velocityY": -0.976652781804051, + "timestamp": 4.560893484531954 + }, + { + "x": 7.516816185598605, + "y": 2.109122861945235, + "heading": -0.09276870443286901, + "angularVelocity": -1.1696375032490753, + "velocityX": -2.360426305368932, + "velocityY": -1.0947451042534073, + "timestamp": 4.626596274863416 + }, + { + "x": 7.344387868807497, + "y": 2.0298516556077795, + "heading": -0.16871929267657423, + "angularVelocity": -1.155972034985807, + "velocityX": -2.624368248612144, + "velocityY": -1.2065120208372122, + "timestamp": 4.692299065194877 + }, + { + "x": 7.154363408340053, + "y": 1.943962982346676, + "heading": -0.2400485240661189, + "angularVelocity": -1.0856347352935647, + "velocityX": -2.892182501059652, + "velocityY": -1.3072302230667407, + "timestamp": 4.758001855526339 + }, + { + "x": 6.946444183851081, + "y": 1.8530162365161487, + "heading": -0.2992772419214472, + "angularVelocity": -0.9014642689682999, + "velocityX": -3.164541771210153, + "velocityY": -1.3842143594162997, + "timestamp": 4.8237046458578 + }, + { + "x": 6.72212326982476, + "y": 1.762675190878268, + "heading": -0.31855321839260486, + "angularVelocity": -0.29338139786625583, + "velocityX": -3.4141763674670904, + "velocityY": -1.3749955699312393, + "timestamp": 4.889407436189262 + }, + { + "x": 6.4857597450479005, + "y": 1.6860130581467423, + "heading": -0.3185532921212083, + "angularVelocity": -0.000001122153306519187, + "velocityX": -3.5974655503128186, + "velocityY": -1.1668017803319486, + "timestamp": 4.955110226520723 + }, + { + "x": 6.24426876944993, + "y": 1.6274732096370486, + "heading": -0.3185533177664737, + "angularVelocity": -3.903223175651228e-7, + "velocityX": -3.6755056273817623, + "velocityY": -0.8909796405048931, + "timestamp": 5.020813016852185 + }, + { + "x": 5.999037658914315, + "y": 1.5873919634602989, + "heading": -0.3185533462426245, + "angularVelocity": -4.3340854500246066e-7, + "velocityX": -3.732430682143934, + "velocityY": -0.6100387209515066, + "timestamp": 5.0865158071836465 + }, + { + "x": 5.751475205523387, + "y": 1.5659995878977724, + "heading": -0.3185533789943253, + "angularVelocity": -4.984826460154038e-7, + "velocityX": -3.7679138456983283, + "velocityY": -0.32559310578142847, + "timestamp": 5.152218597515108 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.3185534180019773, + "angularVelocity": -5.936985598886166e-7, + "velocityX": -3.781751231726404, + "velocityY": -0.03927692372891183, + "timestamp": 5.21792138784657 + }, + { + "x": 5.26265149067387, + "y": 1.5786031053003824, + "heading": -0.31855345112044986, + "angularVelocity": -5.200844119271939e-7, + "velocityX": -3.774430827391544, + "velocityY": 0.23844772894968422, + "timestamp": 5.28160042121006 + }, + { + "x": 5.0240625547753925, + "y": 1.61139052009726, + "heading": -0.3185534786660252, + "angularVelocity": -4.325689925822067e-7, + "velocityX": -3.746742425196293, + "velocityY": 0.5148855606793217, + "timestamp": 5.34527945457355 + }, + { + "x": 4.788524290519247, + "y": 1.6616042890226763, + "heading": -0.31855350247111613, + "angularVelocity": -3.7382933976434193e-7, + "velocityX": -3.698835422197071, + "velocityY": 0.7885447732660757, + "timestamp": 5.40895848793704 + }, + { + "x": 4.55730773239592, + "y": 1.7289734284079032, + "heading": -0.3185535237219762, + "angularVelocity": -3.3371831981217076e-7, + "velocityX": -3.6309684037367047, + "velocityY": 1.0579485244487483, + "timestamp": 5.4726375213005305 + }, + { + "x": 4.33166058516164, + "y": 1.8131343663413289, + "heading": -0.31855354324045854, + "angularVelocity": -3.06513484100539e-7, + "velocityX": -3.5435077342686676, + "velocityY": 1.3216428310558945, + "timestamp": 5.536316554664021 + }, + { + "x": 4.112800476187319, + "y": 1.9136328827222213, + "heading": -0.31855356163583604, + "angularVelocity": -2.8887651909639526e-7, + "velocityX": -3.4369257417120753, + "velocityY": 1.5782041760469425, + "timestamp": 5.599995588027511 + }, + { + "x": 3.9019083093617053, + "y": 2.029926448332014, + "heading": -0.3185535793939757, + "angularVelocity": -2.788694925452677e-7, + "velocityX": -3.3117991226689654, + "velocityY": 1.8262457745859704, + "timestamp": 5.663674621391001 + }, + { + "x": 3.700120307432907, + "y": 2.161384813738153, + "heading": -0.3185535969362317, + "angularVelocity": -2.7547930719470604e-7, + "velocityX": -3.1688295388681516, + "velocityY": 2.0643900898393666, + "timestamp": 5.727353654754491 + }, + { + "x": 3.5092854049648876, + "y": 2.2879634464070207, + "heading": -0.3579155843505842, + "angularVelocity": -0.618131044635493, + "velocityX": -2.996824737880426, + "velocityY": 1.9877599577609333, + "timestamp": 5.791032688117982 + }, + { + "x": 3.3329097555386666, + "y": 2.4050971261724468, + "heading": -0.41294492327597715, + "angularVelocity": -0.8641673093760206, + "velocityX": -2.7697601566820413, + "velocityY": 1.8394387222684179, + "timestamp": 5.854711721481472 + }, + { + "x": 3.171287726021972, + "y": 2.512479984884094, + "heading": -0.46992046584984914, + "angularVelocity": -0.8947300165290927, + "velocityX": -2.538072910028812, + "velocityY": 1.686314207985683, + "timestamp": 5.918390754844962 + }, + { + "x": 3.024408806786741, + "y": 2.6100923699912792, + "heading": -0.5252799185719805, + "angularVelocity": -0.8693513358805383, + "velocityX": -2.306550704009953, + "velocityY": 1.532881074842923, + "timestamp": 5.982069788208452 + }, + { + "x": 2.892255079193046, + "y": 2.6979336450739244, + "heading": -0.5773440254447586, + "angularVelocity": -0.8176020288434342, + "velocityX": -2.0753098879397363, + "velocityY": 1.3794379475145806, + "timestamp": 6.045748821571943 + }, + { + "x": 2.7748123815012584, + "y": 2.776006230905406, + "heading": -0.625128837055519, + "angularVelocity": -0.7504010203483642, + "velocityX": -1.8442914643726742, + "velocityY": 1.226032835420591, + "timestamp": 6.109427854935433 + }, + { + "x": 2.6720699818921556, + "y": 2.8443129166968544, + "heading": -0.6679860381773632, + "angularVelocity": -0.6730190277419648, + "velocityX": -1.6134415706757543, + "velocityY": 1.0726715244175118, + "timestamp": 6.173106888298923 + }, + { + "x": 2.5840196243973645, + "y": 2.9028562717726505, + "heading": -0.7054558016839699, + "angularVelocity": -0.5884160221579269, + "velocityX": -1.382721326691407, + "velocityY": 0.9193505614575074, + "timestamp": 6.236785921662413 + }, + { + "x": 2.510654811151598, + "y": 2.95163853061359, + "heading": -0.7371951288142529, + "angularVelocity": -0.4984266477336411, + "velocityX": -1.1521031235978156, + "velocityY": 0.7660646882386302, + "timestamp": 6.3004649550259035 + }, + { + "x": 2.4519703126340358, + "y": 2.9906615990271557, + "heading": -0.7629386573341135, + "angularVelocity": -0.4042700895428554, + "velocityX": -0.9215670436230067, + "velocityY": 0.6128087433553877, + "timestamp": 6.364143988389394 + }, + { + "x": 2.4079618327799786, + "y": 3.0199270886206313, + "heading": -0.7824753669479414, + "angularVelocity": -0.30679972012623624, + "velocityX": -0.6910984279998401, + "velocityY": 0.45957810675962124, + "timestamp": 6.427823021752884 + }, + { + "x": 2.378625774904473, + "y": 3.039436353320747, + "heading": -0.795633833561549, + "angularVelocity": -0.2066373485680448, + "velocityX": -0.4606862938394533, + "velocityY": 0.3063687318988397, + "timestamp": 6.491502055116374 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.10425061542851646, + "velocityX": -0.2303222914890624, + "velocityY": 0.15317707264507013, + "timestamp": 6.5551810884798645 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -7.869331036874185e-31, + "timestamp": 6.618860121843355 + }, + { + "x": 2.3794831364602356, + "y": 3.04172499089676, + "heading": -0.7962438371233982, + "angularVelocity": 0.09576162685715454, + "velocityX": 0.24659384848580665, + "velocityY": -0.11858712018957836, + "timestamp": 6.6818140946823945 + }, + { + "x": 2.4105331611816494, + "y": 3.0267927192788293, + "heading": -0.7842933789401895, + "angularVelocity": 0.1898284992078828, + "velocityX": 0.49321787523723476, + "velocityY": -0.23719347555886855, + "timestamp": 6.744768067521434 + }, + { + "x": 2.457111347137164, + "y": 3.0043922693157348, + "heading": -0.7665480516906409, + "angularVelocity": 0.2818777981640571, + "velocityX": 0.7398768315163422, + "velocityY": -0.3558226582517257, + "timestamp": 6.807722040360474 + }, + { + "x": 2.5192202688476826, + "y": 2.9745219425624034, + "heading": -0.7431604801963628, + "angularVelocity": 0.37150270967132, + "velocityX": 0.9865766830842898, + "velocityY": -0.4744788201008906, + "timestamp": 6.870676013199514 + }, + { + "x": 2.5968629770363694, + "y": 2.9371797310172574, + "heading": -0.7143164372650311, + "angularVelocity": 0.45817669053357035, + "velocityX": 1.2333249942335256, + "velocityY": -0.5931668782941151, + "timestamp": 6.933629986038554 + }, + { + "x": 2.690043132952377, + "y": 2.892363250439385, + "heading": -0.6802459845187668, + "angularVelocity": 0.5411962297181008, + "velocityX": 1.4801314629379994, + "velocityY": -0.711892809250637, + "timestamp": 6.9965839588775935 + }, + { + "x": 2.798765189175504, + "y": 2.840069647867451, + "heading": -0.6412406321012497, + "angularVelocity": 0.6195852407479587, + "velocityX": 1.7270086591851181, + "velocityY": -0.8306640584167341, + "timestamp": 7.059537931716633 + }, + { + "x": 2.9230346316689584, + "y": 2.7802954749466373, + "heading": -0.5976811625941267, + "angularVelocity": 0.6919256647788576, + "velocityX": 1.9739729978787044, + "velocityY": -0.9494900833922507, + "timestamp": 7.122491904555673 + }, + { + "x": 3.0628582875020656, + "y": 2.7130365228135442, + "heading": -0.550085909804202, + "angularVelocity": 0.7560325527288918, + "velocityX": 2.2210457819811675, + "velocityY": -1.0683829645677803, + "timestamp": 7.185445877394713 + }, + { + "x": 3.2182446226798445, + "y": 2.6382876468732976, + "heading": -0.4992026727158649, + "angularVelocity": 0.8082609372157451, + "velocityX": 2.4682530453648863, + "velocityY": -1.1873575656831603, + "timestamp": 7.248399850233753 + }, + { + "x": 3.3892034750687743, + "y": 2.5560428034877645, + "heading": -0.4462086118156311, + "angularVelocity": 0.8417905734992818, + "velocityX": 2.7156165795292284, + "velocityY": -1.306428167699852, + "timestamp": 7.311353823072793 + }, + { + "x": 3.57574133188106, + "y": 2.4662968714042055, + "heading": -0.3932458693377865, + "angularVelocity": 0.8412930922288103, + "velocityX": 2.963083160600891, + "velocityY": -1.4255801188754254, + "timestamp": 7.374307795911832 + }, + { + "x": 3.777810779911522, + "y": 2.369066952878771, + "heading": -0.3455559373144148, + "angularVelocity": 0.7575364964702819, + "velocityX": 3.209796600877132, + "velocityY": -1.5444604071935277, + "timestamp": 7.437261768750872 + }, + { + "x": 3.9925130304181535, + "y": 2.2661623966169606, + "heading": -0.3455557491568548, + "angularVelocity": 0.0000029888115329808544, + "velocityX": 3.4104638805811245, + "velocityY": -1.63459987703898, + "timestamp": 7.500215741589912 + }, + { + "x": 4.207067940824861, + "y": 2.162950872410893, + "heading": -0.34555573251479127, + "angularVelocity": 2.643528717349897e-7, + "velocityX": 3.408123438933375, + "velocityY": -1.6394759464975768, + "timestamp": 7.563169714428952 + }, + { + "x": 4.421622842445454, + "y": 2.0597393299403546, + "heading": -0.34555571587272765, + "angularVelocity": 2.6435287281597733e-7, + "velocityX": 3.408123299369276, + "velocityY": -1.6394762366217663, + "timestamp": 7.626123687267992 + }, + { + "x": 4.636177744065525, + "y": 1.9565277874687297, + "heading": -0.3455556992306641, + "angularVelocity": 2.6435287314542613e-7, + "velocityX": 3.4081232993609745, + "velocityY": -1.6394762366390223, + "timestamp": 7.689077660107031 + }, + { + "x": 4.850732645688907, + "y": 1.8533162450039902, + "heading": -0.34555568258860053, + "angularVelocity": 2.643528725209065e-7, + "velocityX": 3.408123299413585, + "velocityY": -1.6394762365296556, + "timestamp": 7.752031632946071 + }, + { + "x": 5.065287602977393, + "y": 1.7501048182552732, + "heading": -0.345555665946537, + "angularVelocity": 2.6435287138683083e-7, + "velocityX": 3.4081241836326663, + "velocityY": -1.63947439842449, + "timestamp": 7.814985605785111 + }, + { + "x": 5.280754789180012, + "y": 1.6488115807144432, + "heading": -0.34555564929395255, + "angularVelocity": 2.6451999265849417e-7, + "velocityX": 3.4226145942134942, + "velocityY": -1.6090046898202166, + "timestamp": 7.877939578624151 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 2.673332686258823e-7, + "velocityX": 3.530338087601756, + "velocityY": -1.3564290298187671, + "timestamp": 7.940893551463191 + }, + { + "x": 5.652794664229654, + "y": 1.5139935049725783, + "heading": -0.34555563561361163, + "angularVelocity": -7.551097078688917e-8, + "velocityX": 3.591492706822241, + "velocityY": -1.185058979367509, + "timestamp": 7.982600740979571 + }, + { + "x": 5.804789442560329, + "y": 1.4718291930788845, + "heading": -0.345555638724863, + "angularVelocity": -7.459748339352162e-8, + "velocityX": 3.644330392269195, + "velocityY": -1.010960277655107, + "timestamp": 8.02430793049595 + }, + { + "x": 5.958396699024851, + "y": 1.435981562464388, + "heading": -0.3455556418269712, + "angularVelocity": -7.437825995471752e-8, + "velocityX": 3.6829922669374113, + "velocityY": -0.8595072223799174, + "timestamp": 8.066015120012331 + }, + { + "x": 6.112005102436535, + "y": 1.4001388468787674, + "heading": -0.3455556449290776, + "angularVelocity": -7.437821592181375e-8, + "velocityX": 3.6830197669242586, + "velocityY": -0.8593893762979032, + "timestamp": 8.107722309528711 + }, + { + "x": 6.2656135060730795, + "y": 1.3642961322568121, + "heading": -0.3455556480311839, + "angularVelocity": -7.437821455888137e-8, + "velocityX": 3.683019772315654, + "velocityY": -0.8593893531924107, + "timestamp": 8.149429499045091 + }, + { + "x": 6.419221909709655, + "y": 1.3284534176349936, + "heading": -0.3455556511332902, + "angularVelocity": -7.437821520024811e-8, + "velocityX": 3.6830197723164173, + "velocityY": -0.8593893531891351, + "timestamp": 8.191136688561471 + }, + { + "x": 6.572830313283965, + "y": 1.292610702746327, + "heading": -0.3455556542353965, + "angularVelocity": -7.437821564310945e-8, + "velocityX": 3.6830197708234897, + "velocityY": -0.859389359587264, + "timestamp": 8.232843878077851 + }, + { + "x": 6.72643839927336, + "y": 1.2567666268353574, + "heading": -0.34555565733750304, + "angularVelocity": -7.43782201062097e-8, + "velocityX": 3.6830121561911366, + "velocityY": -0.8594219923855618, + "timestamp": 8.274551067594231 + }, + { + "x": 6.87895592150965, + "y": 1.216534214678565, + "heading": -0.3455556607967647, + "angularVelocity": -8.294161584911855e-8, + "velocityX": 3.656864056409029, + "velocityY": -0.9646397329408027, + "timestamp": 8.316258257110611 + }, + { + "x": 7.025398163354816, + "y": 1.1717644512232448, + "heading": -0.34674583940477344, + "angularVelocity": -0.02853653343247435, + "velocityX": 3.5111989933450576, + "velocityY": -1.073430359955979, + "timestamp": 8.357965446626991 + }, + { + "x": 7.164406770259058, + "y": 1.1285160108597303, + "heading": -0.3473601506358553, + "angularVelocity": -0.014729144739916763, + "velocityX": 3.3329650958535093, + "velocityY": -1.0369540806994204, + "timestamp": 8.399672636143372 + }, + { + "x": 7.296003099222487, + "y": 1.086889230520204, + "heading": -0.3473718226301098, + "angularVelocity": -0.0002798556889061875, + "velocityX": 3.155243268351752, + "velocityY": -0.9980720547755352, + "timestamp": 8.441379825659752 + }, + { + "x": 7.420194691021883, + "y": 1.0469180216341771, + "heading": -0.34677182369168125, + "angularVelocity": 0.014385983457189529, + "velocityX": 2.977702243653219, + "velocityY": -0.9583769453064839, + "timestamp": 8.483087015176132 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.02916022972351966, + "velocityX": 2.8002535694982806, + "velocityY": -0.9182731897618387, + "timestamp": 8.524794204692512 + }, + { + "x": 7.694109552956691, + "y": 0.9554552181085981, + "heading": -0.3423846619503678, + "angularVelocity": 0.05119624293404516, + "velocityX": 2.536815276765165, + "velocityY": -0.8583516550337287, + "timestamp": 8.586731767532397 + }, + { + "x": 7.835030504559327, + "y": 0.9064832871477253, + "heading": -0.33886448301682054, + "angularVelocity": 0.05683431462499228, + "velocityX": 2.2752098264978664, + "velocityY": -0.7906660952654867, + "timestamp": 8.648669330372282 + }, + { + "x": 7.959827163561028, + "y": 0.861997905644216, + "heading": -0.3356203872211221, + "angularVelocity": 0.05237687191671972, + "velocityX": 2.014878424007598, + "velocityY": -0.7182294469433418, + "timestamp": 8.710606893212168 + }, + { + "x": 8.068556860689503, + "y": 0.8221974775860657, + "heading": -0.3330761096963967, + "angularVelocity": 0.04107810201222393, + "velocityX": 1.7554726428218221, + "velocityY": -0.6425895084221868, + "timestamp": 8.772544456052053 + }, + { + "x": 8.161262929678218, + "y": 0.7872247496182815, + "heading": -0.33153778712366017, + "angularVelocity": 0.02483666618774324, + "velocityX": 1.4967664973897774, + "velocityY": -0.5646448837225355, + "timestamp": 8.834482018891938 + }, + { + "x": 8.237979200485006, + "y": 0.7571873196552872, + "heading": -0.33123696700010097, + "angularVelocity": 0.0048568285506627, + "velocityX": 1.2386065464846836, + "velocityY": -0.48496305934161354, + "timestamp": 8.896419581731823 + }, + { + "x": 8.298732784455282, + "y": 0.7321691813322279, + "heading": -0.3323549200927614, + "angularVelocity": -0.018049678440697953, + "velocityX": 0.9808843161512628, + "velocityY": -0.4039251332464228, + "timestamp": 8.958357144571709 + }, + { + "x": 8.34354587924, + "y": 0.712237715373984, + "heading": -0.3350374219820423, + "angularVelocity": -0.04330977465508917, + "velocityX": 0.7235204733606144, + "velocityY": -0.3217993257140046, + "timestamp": 9.020294707411594 + }, + { + "x": 8.372436985155346, + "y": 0.6974481694465224, + "heading": -0.33940426042561933, + "angularVelocity": -0.07050387912204041, + "velocityX": 0.46645532356563796, + "velocityY": -0.23878152851596665, + "timestamp": 9.08223227025148 + }, + { + "x": 8.385421752929688, + "y": 0.6878466606140137, + "heading": -0.34555563246426124, + "angularVelocity": -0.09931569400855825, + "velocityX": 0.20964285934061647, + "velocityY": -0.15501915787887396, + "timestamp": 9.144169833091365 + }, + { + "x": 8.381221613866826, + "y": 0.6835499753387894, + "heading": -0.35422874908994917, + "angularVelocity": -0.13146724375807764, + "velocityX": -0.06366577665514053, + "velocityY": -0.06512922572222897, + "timestamp": 9.210141525946263 + }, + { + "x": 8.358990442151228, + "y": 0.6851839721097847, + "heading": -0.36494544812778174, + "angularVelocity": -0.16244389940702994, + "velocityX": -0.33698046470466403, + "velocityY": 0.024768149797052455, + "timestamp": 9.27611321880116 + }, + { + "x": 8.318727796191062, + "y": 0.692749219284977, + "heading": -0.37761522689458643, + "angularVelocity": -0.1920487139032685, + "velocityX": -0.6103018464104298, + "velocityY": 0.1146741404958606, + "timestamp": 9.342084911656059 + }, + { + "x": 8.260433187957593, + "y": 0.7062463828943455, + "heading": -0.3921309909220783, + "angularVelocity": -0.22003018869651583, + "velocityX": -0.8836306256637128, + "velocityY": 0.20459022688799033, + "timestamp": 9.408056604510957 + }, + { + "x": 8.184106080259626, + "y": 0.7256762539187386, + "heading": -0.40836397752669645, + "angularVelocity": -0.24605987662499226, + "velocityX": -1.1569675476699668, + "velocityY": 0.2945183029807682, + "timestamp": 9.474028297365855 + }, + { + "x": 8.089745888152331, + "y": 0.7510397855554056, + "heading": -0.4261563635075344, + "angularVelocity": -0.26969727789116266, + "velocityX": -1.4303133362794993, + "velocityY": 0.3844608276530502, + "timestamp": 9.539999990220753 + }, + { + "x": 7.977351991056692, + "y": 0.7823381451863918, + "heading": -0.445310079113529, + "angularVelocity": -0.2903323346290704, + "velocityX": -1.7036685316358853, + "velocityY": 0.47442104752147907, + "timestamp": 9.605971683075651 + }, + { + "x": 7.846923771917719, + "y": 0.8195727882757919, + "heading": -0.4655690572275494, + "angularVelocity": -0.30708592181466465, + "velocityX": -1.9770330803219518, + "velocityY": 0.5644033293384785, + "timestamp": 9.67194337593055 + }, + { + "x": 7.6984607267480145, + "y": 0.8627455646922209, + "heading": -0.48658930450770715, + "angularVelocity": -0.31862525229405353, + "velocityX": -2.2504052684572193, + "velocityY": 0.6544136514942158, + "timestamp": 9.737915068785448 + }, + { + "x": 7.531962771855591, + "y": 0.9118588688249081, + "heading": -0.5078842268520795, + "angularVelocity": -0.32278878141280287, + "velocityX": -2.5237787251969923, + "velocityY": 0.7444602678410827, + "timestamp": 9.803886761640346 + }, + { + "x": 7.347431182646945, + "y": 0.9669158200795512, + "heading": -0.5287129812318903, + "angularVelocity": -0.31572259977658373, + "velocityX": -2.7971328493042473, + "velocityY": 0.8345541681905657, + "timestamp": 9.869858454495244 + }, + { + "x": 7.144872022891467, + "y": 1.0279202386913098, + "heading": -0.5478111966657127, + "angularVelocity": -0.28949106211095804, + "velocityX": -3.0703950586958326, + "velocityY": 0.9247059757271185, + "timestamp": 9.935830147350142 + }, + { + "x": 6.92431385893063, + "y": 1.0948740954299652, + "heading": -0.56252983495188, + "angularVelocity": -0.2231053600297971, + "velocityX": -3.343224259015806, + "velocityY": 1.0148876562242728, + "timestamp": 10.00180184020504 + }, + { + "x": 6.686027050772059, + "y": 1.1677363127465568, + "heading": -0.5635938882487441, + "angularVelocity": -0.016128937288368943, + "velocityX": -3.611955337915506, + "velocityY": 1.1044466825619976, + "timestamp": 10.067773533059938 + }, + { + "x": 6.448996512762522, + "y": 1.2456322264917639, + "heading": -0.5635938947986299, + "angularVelocity": -9.928327608034937e-8, + "velocityX": -3.592912774436053, + "velocityY": 1.180747535409401, + "timestamp": 10.133745225914836 + }, + { + "x": 6.2119660667132655, + "y": 1.3235284200636117, + "heading": -0.5635939013485092, + "angularVelocity": -9.928317804375559e-8, + "velocityX": -3.5929113805006425, + "velocityY": 1.180751777026201, + "timestamp": 10.199716918769735 + }, + { + "x": 5.97493562067527, + "y": 1.4014246136697293, + "heading": -0.5635939078983885, + "angularVelocity": -9.928317794724392e-8, + "velocityX": -3.5929113803299297, + "velocityY": 1.180751777545662, + "timestamp": 10.265688611624633 + }, + { + "x": 5.737905297280568, + "y": 1.4793211804663164, + "heading": -0.5635939144482677, + "angularVelocity": -9.928317524609207e-8, + "velocityX": -3.5929095213008893, + "velocityY": 1.180757434372905, + "timestamp": 10.33166030447953 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.5635939210079286, + "angularVelocity": -9.943144757674115e-8, + "velocityX": -3.5606438133655525, + "velocityY": 1.2747558885869388, + "timestamp": 10.397631997334429 + }, + { + "x": 5.2859625551559315, + "y": 1.6593525606269757, + "heading": -0.5635939265318385, + "angularVelocity": -8.803796196510374e-8, + "velocityX": -3.459117077662582, + "velocityY": 1.5289526284347386, + "timestamp": 10.4603766333877 + }, + { + "x": 5.071426966449666, + "y": 1.7607653123039275, + "heading": -0.5635939320385204, + "angularVelocity": -8.776338927441567e-8, + "velocityX": -3.4191861201350386, + "velocityY": 1.6162776303436093, + "timestamp": 10.52312126944097 + }, + { + "x": 4.856891543272159, + "y": 1.8621784141511948, + "heading": -0.5635939375452019, + "angularVelocity": -8.776338338833754e-8, + "velocityX": -3.419183482000995, + "velocityY": 1.6162832112241017, + "timestamp": 10.585865905494241 + }, + { + "x": 4.642356120103932, + "y": 1.9635915160180906, + "heading": -0.5635939430518834, + "angularVelocity": -8.776338232016452e-8, + "velocityX": -3.419183481853115, + "velocityY": 1.6162832115369357, + "timestamp": 10.648610541547512 + }, + { + "x": 4.427820696935705, + "y": 2.0650046178849877, + "heading": -0.5635939485585649, + "angularVelocity": -8.776338320403535e-8, + "velocityX": -3.419183481853107, + "velocityY": 1.616283211536953, + "timestamp": 10.711355177600783 + }, + { + "x": 4.213285273767479, + "y": 2.1664177197518852, + "heading": -0.5635939540652464, + "angularVelocity": -8.776338205784925e-8, + "velocityX": -3.419183481853105, + "velocityY": 1.616283211536959, + "timestamp": 10.774099813654054 + }, + { + "x": 3.99874985060226, + "y": 2.2678308216251453, + "heading": -0.5635939595719279, + "angularVelocity": -8.776338204240658e-8, + "velocityX": -3.4191834818051703, + "velocityY": 1.616283211638361, + "timestamp": 10.836844449707325 + }, + { + "x": 3.7842144810928517, + "y": 2.369244037002313, + "heading": -0.5635939650808715, + "angularVelocity": -8.779943360299573e-8, + "velocityX": -3.4191826266593917, + "velocityY": 1.6162850206202093, + "timestamp": 10.899589085760596 + }, + { + "x": 3.582486282432141, + "y": 2.464592725052892, + "heading": -0.5976475269249973, + "angularVelocity": -0.5427326379774409, + "velocityX": -3.21506683837393, + "velocityY": 1.5196309046980254, + "timestamp": 10.962333721813867 + }, + { + "x": 3.3962755581787456, + "y": 2.5526066599990593, + "heading": -0.6290952664889575, + "angularVelocity": -0.5012020396016188, + "velocityX": -2.9677552690767404, + "velocityY": 1.4027324163844592, + "timestamp": 11.025078357867137 + }, + { + "x": 3.2255823358404876, + "y": 2.63328589857046, + "heading": -0.6579338206032609, + "angularVelocity": -0.4596178403173682, + "velocityX": -2.7204432613704155, + "velocityY": 1.2858348322062758, + "timestamp": 11.087822993920408 + }, + { + "x": 3.0704066332901094, + "y": 2.7066304971231805, + "heading": -0.684160167831089, + "angularVelocity": -0.4179854865292692, + "velocityX": -2.4731309688151453, + "velocityY": 1.168938146209836, + "timestamp": 11.15056762997368 + }, + { + "x": 2.930748464239733, + "y": 2.7726405069094633, + "heading": -0.7077714539525848, + "angularVelocity": -0.3763076432772636, + "velocityX": -2.22581845772133, + "velocityY": 1.05204227705202, + "timestamp": 11.21331226602695 + }, + { + "x": 2.8066078397359373, + "y": 2.83131597316776, + "heading": -0.7287650298436226, + "angularVelocity": -0.3345875792986393, + "velocityX": -1.978505770571369, + "velocityY": 0.9351471288873412, + "timestamp": 11.27605690208022 + }, + { + "x": 2.697984768963403, + "y": 2.882656934878273, + "heading": -0.7471385155745514, + "angularVelocity": -0.2928295849119221, + "velocityX": -1.7311929370394774, + "velocityY": 0.8182526019742015, + "timestamp": 11.338801538133492 + }, + { + "x": 2.604879259815539, + "y": 2.926663424723888, + "heading": -0.7628898637084475, + "angularVelocity": -0.25103895925897446, + "velocityX": -1.4838799777054614, + "velocityY": 0.7013585959483968, + "timestamp": 11.401546174186763 + }, + { + "x": 2.5272913193421016, + "y": 2.963335469132253, + "heading": -0.7760174152523261, + "angularVelocity": -0.20922189321065382, + "velocityX": -1.236566906015123, + "velocityY": 0.584465011116332, + "timestamp": 11.464290810240033 + }, + { + "x": 2.465220954107753, + "y": 2.9926730883504185, + "heading": -0.7865199463491176, + "angularVelocity": -0.1673853217966667, + "velocityX": -0.9892537296997038, + "velocityY": 0.46757174897401693, + "timestamp": 11.527035446293304 + }, + { + "x": 2.418668170473455, + "y": 3.014676296527397, + "heading": -0.7943967051912195, + "angularVelocity": -0.12553676836079783, + "velocityX": -0.7419404520057251, + "velocityY": 0.3506787123332304, + "timestamp": 11.589780082346575 + }, + { + "x": 2.38763297480514, + "y": 3.0293451017899056, + "heading": -0.7996474391139023, + "angularVelocity": -0.08368418805115034, + "velocityX": -0.49462707285394797, + "velocityY": 0.23378580521296682, + "timestamp": 11.652524718399846 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": -0.0418358130797821, + "velocityX": -0.24731358996990807, + "velocityY": 0.11689293258068652, + "timestamp": 11.715269354453117 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": 1.2905382194087526e-36, + "velocityX": 1.2188676912146853e-38, + "velocityY": -1.9456328527917923e-39, + "timestamp": 11.778013990506388 + }, + { + "x": 2.3862784269967885, + "y": 3.0299921206795677, + "heading": -0.7979445022798209, + "angularVelocity": 0.07214760590413372, + "velocityX": 0.23610252175550234, + "velocityY": -0.11148080618080955, + "timestamp": 11.838000869426745 + }, + { + "x": 2.414605712310544, + "y": 3.016617241283919, + "heading": -0.7893555783917804, + "angularVelocity": 0.14318004274641105, + "velocityX": 0.4722246901920793, + "velocityY": -0.22296341527296173, + "timestamp": 11.897987748347102 + }, + { + "x": 2.457098595113841, + "y": 2.996554734272641, + "heading": -0.7765849369819305, + "angularVelocity": 0.21289057940162145, + "velocityX": 0.7083696229589316, + "velocityY": -0.3344482555579276, + "timestamp": 11.95797462726746 + }, + { + "x": 2.5137586742709717, + "y": 2.9698044458506936, + "heading": -0.7597273153290622, + "angularVelocity": 0.28102181604163, + "velocityX": 0.9445412092927212, + "velocityY": -0.44593565965421605, + "timestamp": 12.017961506187817 + }, + { + "x": 2.5845878442638655, + "y": 2.9363662114763, + "heading": -0.7388971783301495, + "angularVelocity": 0.34724488711219914, + "velocityX": 1.180744377231747, + "velocityY": -0.5574258067133014, + "timestamp": 12.077948385108174 + }, + { + "x": 2.6695883810159726, + "y": 2.8962398712988433, + "heading": -0.7142349782126007, + "angularVelocity": 0.4111265757015274, + "velocityX": 1.4169854855252446, + "velocityY": -0.6689186185320777, + "timestamp": 12.137935264028531 + }, + { + "x": 2.7687630628600255, + "y": 2.849425297465716, + "heading": -0.6859166617272401, + "angularVelocity": 0.4720751770225938, + "velocityX": 1.653272909492815, + "velocityY": -0.7804135616937368, + "timestamp": 12.197922142948888 + }, + { + "x": 2.882115344958, + "y": 2.795922445610523, + "heading": -0.6541688123580988, + "angularVelocity": 0.529246560923628, + "velocityX": 1.8896179320892517, + "velocityY": -0.8919092444570627, + "timestamp": 12.257909021869246 + }, + { + "x": 3.009649615335316, + "y": 2.73573145985777, + "heading": -0.6192942663212875, + "angularVelocity": 0.5813695705541366, + "velocityX": 2.1260361044394105, + "velocityY": -1.0034025246198652, + "timestamp": 12.317895900789603 + }, + { + "x": 3.1513715692279023, + "y": 2.6688529097388005, + "heading": -0.5817190571900686, + "angularVelocity": 0.626390467507171, + "velocityX": 2.3625492181506127, + "velocityY": -1.114886310517361, + "timestamp": 12.37788277970996 + }, + { + "x": 3.3072887013147403, + "y": 2.5952884046594, + "heading": -0.5420886244414693, + "angularVelocity": 0.6606516868666416, + "velocityX": 2.5991872705003334, + "velocityY": -1.2263432671179573, + "timestamp": 12.437869658630317 + }, + { + "x": 3.4774104648990076, + "y": 2.5150425607670805, + "heading": -0.50150111026719, + "angularVelocity": 0.6766065330414328, + "velocityX": 2.8359829123654157, + "velocityY": -1.33772327109832, + "timestamp": 12.497856537550675 + }, + { + "x": 3.6617427045443476, + "y": 2.428132084038069, + "heading": -0.4622625664613271, + "angularVelocity": 0.6541187758402723, + "velocityX": 3.0728759849311564, + "velocityY": -1.4488247812392312, + "timestamp": 12.557843416471032 + }, + { + "x": 3.8601464039752953, + "y": 2.3346891355318427, + "heading": -0.4327535611710116, + "angularVelocity": 0.49192433114404066, + "velocityX": 3.3074516127828706, + "velocityY": -1.5577231252568986, + "timestamp": 12.61783029539139 + }, + { + "x": 4.065242173401204, + "y": 2.237711412138144, + "heading": -0.4327535477418676, + "angularVelocity": 2.2386802351026997e-7, + "velocityX": 3.4190105089182294, + "velocityY": -1.6166489262168848, + "timestamp": 12.677817174311746 + }, + { + "x": 4.2703379249938225, + "y": 2.140733651028875, + "heading": -0.43275353431315516, + "angularVelocity": 2.2386082807599726e-7, + "velocityX": 3.419010211631728, + "velocityY": -1.6166495549472233, + "timestamp": 12.737804053232104 + }, + { + "x": 4.475433676585232, + "y": 2.0437558899170503, + "heading": -0.43275352088444274, + "angularVelocity": 2.2386082918741598e-7, + "velocityX": 3.4190102116115857, + "velocityY": -1.616649554989821, + "timestamp": 12.797790932152461 + }, + { + "x": 4.6805294281766425, + "y": 1.9467781288052255, + "heading": -0.4327535074557304, + "angularVelocity": 2.2386082819075723e-7, + "velocityX": 3.4190102116115852, + "velocityY": -1.616649554989824, + "timestamp": 12.857777811072818 + }, + { + "x": 4.885625179768054, + "y": 1.8498003676934025, + "heading": -0.43275349402701796, + "angularVelocity": 2.2386082903608912e-7, + "velocityX": 3.4190102116115972, + "velocityY": -1.6166495549897977, + "timestamp": 12.917764689993176 + }, + { + "x": 5.090720931370459, + "y": 1.7528226066048298, + "heading": -0.43275348059830554, + "angularVelocity": 2.2386082830289002e-7, + "velocityX": 3.4190102117948658, + "velocityY": -1.6166495546022077, + "timestamp": 12.977751568913533 + }, + { + "x": 5.29581684523517, + "y": 1.6558451886813434, + "heading": -0.43275346716959256, + "angularVelocity": 2.2386083719980442e-7, + "velocityX": 3.4190129167581613, + "velocityY": -1.6166438339330798, + "timestamp": 13.03773844783389 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.43275345370859003, + "angularVelocity": 2.243991161112281e-7, + "velocityX": 3.453867841656271, + "velocityY": -1.5407736813730113, + "timestamp": 13.097725326754247 + }, + { + "x": 5.767637732978587, + "y": 1.4715486447521433, + "heading": -0.4327534418952186, + "angularVelocity": 1.5949042666645356e-7, + "velocityX": 3.572782883962456, + "velocityY": -1.2403266728714897, + "timestamp": 13.171794797056224 + }, + { + "x": 6.033774192023134, + "y": 1.384125705645109, + "heading": -0.432753430099036, + "angularVelocity": 1.5925836332865082e-7, + "velocityX": 3.593065509440347, + "velocityY": -1.1802830336252828, + "timestamp": 13.2458642673582 + }, + { + "x": 6.2999107025526015, + "y": 1.2967029232707166, + "heading": -0.43275341830285335, + "angularVelocity": 1.5925836332521055e-7, + "velocityX": 3.5930662045299844, + "velocityY": -1.1802809176031086, + "timestamp": 13.319933737660177 + }, + { + "x": 6.566047160708243, + "y": 1.2092799814598572, + "heading": -0.43275340650489624, + "angularVelocity": 1.5928232033441407e-7, + "velocityX": 3.593065497439389, + "velocityY": -1.1802830701291938, + "timestamp": 13.394003207962154 + }, + { + "x": 6.815604168443372, + "y": 1.1270295237235333, + "heading": -0.36800659105834704, + "angularVelocity": 0.8741363369088632, + "velocityX": 3.3692290051177802, + "velocityY": -1.110450194945293, + "timestamp": 13.46807267826413 + }, + { + "x": 7.0424486777806585, + "y": 1.0523256982782683, + "heading": -0.304588986464792, + "angularVelocity": 0.8561908750664192, + "velocityX": 3.0625912189253843, + "velocityY": -1.0085643267152118, + "timestamp": 13.542142148566107 + }, + { + "x": 7.246590574581559, + "y": 0.9851213052520007, + "heading": -0.24567984244132185, + "angularVelocity": 0.7953228743678282, + "velocityX": 2.7560869001577597, + "velocityY": -0.9073156963628868, + "timestamp": 13.616211618868084 + }, + { + "x": 7.428038548794457, + "y": 0.9253999088853845, + "heading": -0.19230647763371986, + "angularVelocity": 0.7205852099387576, + "velocityX": 2.4496999029849618, + "velocityY": -0.8062889625528061, + "timestamp": 13.69028108917006 + }, + { + "x": 7.586797949690041, + "y": 0.8731533407207944, + "heading": -0.1449799929692073, + "angularVelocity": 0.6389472541327181, + "velocityX": 2.1433851254549694, + "velocityY": -0.705372509774734, + "timestamp": 13.764350559472037 + }, + { + "x": 7.722872327084393, + "y": 0.8283767866857153, + "heading": -0.10400779573923778, + "angularVelocity": 0.5531590419497899, + "velocityX": 1.8371182734206921, + "velocityY": -0.6045210510150558, + "timestamp": 13.838420029774014 + }, + { + "x": 7.836264194581954, + "y": 0.7910670998170554, + "heading": -0.06959570799723012, + "angularVelocity": 0.46459205934255987, + "velocityX": 1.530885357155523, + "velocityY": -0.5037120788977025, + "timestamp": 13.91248950007599 + }, + { + "x": 7.926975414267802, + "y": 0.7612220655404804, + "heading": -0.04189104317473826, + "angularVelocity": 0.3740362218001811, + "velocityX": 1.224677580601353, + "velocityY": -0.4029330053920843, + "timestamp": 13.986558970377967 + }, + { + "x": 7.995007406512269, + "y": 0.7388400306235591, + "heading": -0.021003806792755122, + "angularVelocity": 0.28199521741990763, + "velocityX": 0.9184889802384877, + "velocityY": -0.3021762519115013, + "timestamp": 14.060628440679944 + }, + { + "x": 8.040361272813582, + "y": 0.7239196956147146, + "heading": -0.0070182972067734635, + "angularVelocity": 0.18881611450660718, + "velocityX": 0.612315251025934, + "velocityY": -0.20143704211755914, + "timestamp": 14.13469791098192 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -5.172058523180806e-42, + "angularVelocity": 0.09475290127174331, + "velocityX": 0.30615312096089364, + "velocityY": -0.10071229126619016, + "timestamp": 14.208767381283897 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -2.2197990868657932e-42, + "angularVelocity": 3.9858008032330975e-41, + "velocityX": -4.443522904196121e-42, + "velocityY": 7.940802598890531e-42, + "timestamp": 14.282836851585873 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint.1.traj b/src/main/deploy/choreo/SourceLanePHGSprint.1.traj index 58ab85b9..87faabce 100644 --- a/src/main/deploy/choreo/SourceLanePHGSprint.1.traj +++ b/src/main/deploy/choreo/SourceLanePHGSprint.1.traj @@ -1,184 +1,184 @@ { "samples": [ { - "x": 1.468971848487854, - "y": 1.4024139642715454, - "heading": 2.7984066036335196e-36, - "angularVelocity": 0, - "velocityX": -5.8999770086815376e-36, - "velocityY": -1.737717144475295e-35, + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": 9.759657159056985e-29, + "angularVelocity": 1.5485227916478622e-29, + "velocityX": -1.3617494152189479e-27, + "velocityY": -5.3557690609839536e-27, "timestamp": 0 }, { - "x": 1.4884655154496769, - "y": 1.4133531216091884, - "heading": -0.008129236612255402, - "angularVelocity": -0.10721835741229961, - "velocityX": 0.2571064235525047, - "velocityY": 0.14427904330507066, - "timestamp": 0.07581944741976482 - }, - { - "x": 1.527452868051014, - "y": 1.4352316834976648, - "heading": -0.024386425737641767, - "angularVelocity": -0.2144197785481142, - "velocityX": 0.514213093449344, - "velocityY": 0.2885613471613503, - "timestamp": 0.15163889483952964 - }, - { - "x": 1.5859338286155955, - "y": 1.4680499620319052, - "heading": -0.04877538927616455, - "angularVelocity": -0.32167160759556, - "velocityX": 0.7713187388561265, - "velocityY": 0.4328477673089076, - "timestamp": 0.22745834225929445 - }, - { - "x": 1.6639082150870297, - "y": 1.5118083204789063, - "heading": -0.08130602266355597, - "angularVelocity": -0.4290539498037971, - "velocityX": 1.028421983079603, - "velocityY": 0.5771389786678144, - "timestamp": 0.3032777896790593 - }, - { - "x": 1.7613757308728617, - "y": 1.5665071508132398, - "heading": -0.12199563276545015, - "angularVelocity": -0.5366645562136756, - "velocityX": 1.2855213154774845, - "velocityY": 0.7214353598688232, - "timestamp": 0.3790972370988241 - }, - { - "x": 1.8783359544327143, - "y": 1.6321468388152505, - "heading": -0.17087061798873185, - "angularVelocity": -0.6446233372381559, - "velocityX": 1.5426150880831033, - "velocityY": 0.8657368292148704, - "timestamp": 0.4549166845185889 - }, - { - "x": 2.014788331993746, - "y": 1.708727712438889, - "heading": -0.22796838283483978, - "angularVelocity": -0.7530754547707706, - "velocityX": 1.7997015568523023, - "velocityY": 1.0100426240204334, - "timestamp": 0.5307361319383537 - }, - { - "x": 2.1707321785420235, - "y": 1.7962499692148692, - "heading": -0.2933392420298012, - "angularVelocity": -0.8621911847107495, - "velocityX": 2.056778990816348, - "velocityY": 1.1543510241036752, - "timestamp": 0.6065555793581185 - }, - { - "x": 2.346166691242211, - "y": 1.8947135784311677, - "heading": -0.3670478679076374, - "angularVelocity": -0.9721598928274647, - "velocityX": 2.3138458360018936, - "velocityY": 1.2986590191190295, - "timestamp": 0.6823750267778834 - }, - { - "x": 2.5215633369919264, - "y": 1.9931565040407253, - "heading": -0.44471119009190996, - "angularVelocity": -1.0243192851868113, - "velocityX": 2.3133464001478927, - "velocityY": 1.2983862183080896, - "timestamp": 0.7581944741976483 - }, - { - "x": 2.6774707734274146, - "y": 2.080658978979359, - "heading": -0.5138920707628942, - "angularVelocity": -0.9124424277055597, - "velocityX": 2.056298769527115, - "velocityY": 1.1540901169351336, - "timestamp": 0.8340139216174132 - }, - { - "x": 2.8138894851599456, - "y": 2.157221711134291, - "heading": -0.5745281398733623, - "angularVelocity": -0.7997429574336522, - "velocityX": 1.799257530555003, - "velocityY": 1.009803352048346, - "timestamp": 0.909833369037178 - }, - { - "x": 2.9308197777643024, - "y": 2.222845447108769, - "heading": -0.6265685355305138, - "angularVelocity": -0.6863726580469048, - "velocityX": 1.5422203218784643, - "velocityY": 0.8655264342822331, - "timestamp": 0.9856528164569429 - }, - { - "x": 3.028261803981169, - "y": 2.277530893632237, - "heading": -0.6699741130308978, - "angularVelocity": -0.5724860702304326, - "velocityX": 1.285185127733667, - "velocityY": 0.7212588377320857, - "timestamp": 1.0614722638767078 - }, - { - "x": 3.1062155998459384, - "y": 2.321278659025598, - "heading": -0.70471685674301, - "angularVelocity": -0.4582299778546705, - "velocityX": 1.028150408867906, - "velocityY": 0.576999264465178, - "timestamp": 1.1372917112964727 - }, - { - "x": 3.164681120107137, - "y": 2.3540892120142103, - "heading": -0.7307788978147941, - "angularVelocity": -0.3437382091100568, - "velocityX": 0.7711150931701107, - "velocityY": 0.4327458733240421, - "timestamp": 1.2131111587162375 - }, - { - "x": 3.2036582678448955, - "y": 2.375962855138776, - "heading": -0.7481514784912885, - "angularVelocity": -0.22913093233604348, - "velocityX": 0.5140784991740481, - "velocityY": 0.2884964724612806, - "timestamp": 1.2889306061360024 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": -0.11451726762925943, - "velocityX": 0.25704022192353, - "velocityY": 0.14424867148149015, - "timestamp": 1.3647500535557673 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": -9.65574327504051e-30, - "velocityX": -6.322792420178433e-32, - "velocityY": 5.37688279123967e-31, - "timestamp": 1.4405695009755322 + "x": 0.4546918803213756, + "y": 4.109218526997661, + "heading": -0.008937300145995024, + "angularVelocity": -0.11893237262991248, + "velocityX": 0.28544257523423766, + "velocityY": -0.15856789979825608, + "timestamp": 0.07514606787342623 + }, + { + "x": 0.4975916635116252, + "y": 4.0853873108981436, + "heading": -0.02681241329663613, + "angularVelocity": -0.23787157008333967, + "velocityX": 0.5708852692400183, + "velocityY": -0.3171319108760033, + "timestamp": 0.15029213574685246 + }, + { + "x": 0.5619414537480726, + "y": 4.049641027140498, + "heading": -0.05362171709802378, + "angularVelocity": -0.3567625633658495, + "velocityX": 0.8563294402155045, + "velocityY": -0.4756906750976654, + "timestamp": 0.2254382036202787 + }, + { + "x": 0.6477414523773108, + "y": 4.001980177860296, + "heading": -0.08935853104911338, + "angularVelocity": -0.4755646564406219, + "velocityX": 1.1417762905939028, + "velocityY": -0.6342427571923064, + "timestamp": 0.3005842714937049 + }, + { + "x": 0.7549919358777417, + "y": 3.9424053758532884, + "heading": -0.13401442200652888, + "angularVelocity": -0.5942545261667251, + "velocityX": 1.4272268201854599, + "velocityY": -0.7927866845588466, + "timestamp": 0.3757303393671312 + }, + { + "x": 0.8836932393269187, + "y": 3.8709173416225426, + "heading": -0.18758057158075359, + "angularVelocity": -0.7128270459134325, + "velocityX": 1.7126818087934759, + "velocityY": -0.9513210238912064, + "timestamp": 0.45087640724055744 + }, + { + "x": 1.0338457421402332, + "y": 3.7875168932975942, + "heading": -0.2500489995023012, + "angularVelocity": -0.8312933688928013, + "velocityX": 1.9981418464400145, + "velocityY": -1.1098444760333412, + "timestamp": 0.5260224751139837 + }, + { + "x": 1.2054498603190875, + "y": 3.692204931169302, + "heading": -0.3214134496146474, + "angularVelocity": -0.9496764385935714, + "velocityX": 2.283607419990346, + "velocityY": -1.268355947630322, + "timestamp": 0.60116854298741 + }, + { + "x": 1.3985060453231266, + "y": 3.584982424979261, + "heading": -0.40166985798592575, + "angularVelocity": -1.0680054278616389, + "velocityX": 2.569079001302066, + "velocityY": -1.4268545144723166, + "timestamp": 0.6763146108608362 + }, + { + "x": 1.5915937569821017, + "y": 3.4778198154599655, + "heading": -0.48176859267037747, + "angularVelocity": -1.0659071984893158, + "velocityX": 2.5694985396202785, + "velocityY": -1.4260574445464886, + "timestamp": 0.7514606787342625 + }, + { + "x": 1.7632284858900344, + "y": 3.382566036520045, + "heading": -0.552976547680448, + "angularVelocity": -0.9475938931363788, + "velocityX": 2.2840147697019764, + "velocityY": -1.2675816797275836, + "timestamp": 0.8266067466076887 + }, + { + "x": 1.9134098098562773, + "y": 3.299220400739088, + "heading": -0.6152918418017485, + "angularVelocity": -0.8292555536806328, + "velocityX": 1.9985253815170176, + "velocityY": -1.1091150626981823, + "timestamp": 0.901752814481115 + }, + { + "x": 2.04213735776467, + "y": 3.2277823059512354, + "heading": -0.6687115003742338, + "angularVelocity": -0.7108776291856513, + "velocityX": 1.7130310547348613, + "velocityY": -0.9506564589404838, + "timestamp": 0.9768988823545413 + }, + { + "x": 2.1494108084857926, + "y": 3.1682512394298143, + "heading": -0.7132320418113052, + "angularVelocity": -0.5924533737688112, + "velocityX": 1.4275324545498602, + "velocityY": -0.7922046782500088, + "timestamp": 1.0520449502279674 + }, + { + "x": 2.235229893671302, + "y": 3.120626784362015, + "heading": -0.7488501306819726, + "angularVelocity": -0.4739847323836216, + "velocityX": 1.142030283341778, + "velocityY": -0.6337584442610695, + "timestamp": 1.1271910181013935 + }, + { + "x": 2.2995943987852447, + "y": 3.0849086278583613, + "heading": -0.7755632624607874, + "angularVelocity": -0.35548276223593744, + "velocityX": 0.8565252572144713, + "velocityY": -0.4753163740226105, + "timestamp": 1.2023370859748197 + }, + { + "x": 2.342504162283695, + "y": 3.0610965683905045, + "heading": -0.7933703919080856, + "angularVelocity": -0.2369668826490278, + "velocityX": 0.5710180813549185, + "velocityY": -0.31687698560575434, + "timestamp": 1.2774831538482458 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288599550668, + "velocityX": 0.28550943973314763, + "velocityY": -0.15843872456938438, + "timestamp": 1.352629221721672 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.820153695476826e-18, + "velocityX": -5.7963239407122954e-18, + "velocityY": -1.1530643297922074e-18, + "timestamp": 1.4277752895950981 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint.2.traj b/src/main/deploy/choreo/SourceLanePHGSprint.2.traj index 29242775..38e969e1 100644 --- a/src/main/deploy/choreo/SourceLanePHGSprint.2.traj +++ b/src/main/deploy/choreo/SourceLanePHGSprint.2.traj @@ -1,499 +1,841 @@ { "samples": [ { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": -9.65574327504051e-30, - "velocityX": -6.322792420178433e-32, - "velocityY": 5.37688279123967e-31, + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.820153695476826e-18, + "velocityX": -5.7963239407122954e-18, + "velocityY": -1.1530643297922074e-18, "timestamp": 0 }, { - "x": 3.2476092863283554, - "y": 2.3785223403683062, - "heading": -0.7517504460683161, - "angularVelocity": 0.062429840741884866, - "velocityX": 0.3004094300502235, - "velocityY": -0.10287803899994612, - "timestamp": 0.08143010320439914 - }, - { - "x": 3.2965349345854977, - "y": 2.361766694915971, - "heading": -0.7416811367035665, - "angularVelocity": 0.12365585905593313, - "velocityX": 0.6008299920034862, - "velocityY": -0.20576721375725762, - "timestamp": 0.16286020640879828 - }, - { - "x": 3.369924920019678, - "y": 2.336631694642855, - "heading": -0.7267441449116069, - "angularVelocity": 0.18343329068945555, - "velocityX": 0.9012635689526604, - "velocityY": -0.30866963547896215, - "timestamp": 0.24429030961319742 - }, - { - "x": 3.467780489895946, - "y": 2.3031160513517954, - "heading": -0.7070825285982575, - "angularVelocity": 0.24145390389600055, - "velocityX": 1.2017124629037867, - "velocityY": -0.41158787686822623, - "timestamp": 0.32572041281759656 - }, - { - "x": 3.59010312419892, - "y": 2.2612182145129647, - "heading": -0.682871988862579, - "angularVelocity": 0.2973168248960161, - "velocityX": 1.5021795317627178, - "velocityY": -0.5145251594936876, - "timestamp": 0.4071505160219957 - }, - { - "x": 3.7368945961709503, - "y": 2.2109362946882256, - "heading": -0.6543323433324523, - "angularVelocity": 0.35048028194805075, - "velocityX": 1.8026683768724503, - "velocityY": -0.6174856453089018, - "timestamp": 0.48858061922639484 - }, - { - "x": 3.9081570523847278, - "y": 2.1522679483317453, - "heading": -0.621745834041024, - "angularVelocity": 0.4001776739695827, - "velocityX": 2.103183582905316, - "velocityY": -0.7204749109701621, - "timestamp": 0.5700107224307938 - }, - { - "x": 4.10389311229091, - "y": 2.085210196605494, - "heading": -0.5854881361653628, - "angularVelocity": 0.44526159797025444, - "velocityX": 2.4037309570253367, - "velocityY": -0.8235007581646905, - "timestamp": 0.6514408256351927 - }, - { - "x": 4.324105961894927, - "y": 2.009759125465929, - "heading": -0.5460851529882595, - "angularVelocity": 0.4838871820929687, - "velocityX": 2.704317456791832, - "velocityY": -0.9265746716564273, - "timestamp": 0.7328709288395916 - }, - { - "x": 4.568799288247487, - "y": 1.9259093624648695, - "heading": -0.5043289871971579, - "angularVelocity": 0.5127853723370845, - "velocityX": 3.0049492352766722, - "velocityY": -1.0297145613408714, - "timestamp": 0.8143010320439905 - }, - { - "x": 4.837976169390964, - "y": 1.8336531477869393, - "heading": -0.4615566059930767, - "angularVelocity": 0.5252649759847486, - "velocityX": 3.3056188135708484, - "velocityY": -1.1329497452110122, - "timestamp": 0.8957311352483894 - }, - { - "x": 5.131630158633593, - "y": 1.7329792749804975, - "heading": -0.42053192360161584, - "angularVelocity": 0.5038024118487874, - "velocityX": 3.606209223455392, - "velocityY": -1.2363225495826649, - "timestamp": 0.9771612384527883 - }, - { - "x": 5.449610743156184, - "y": 1.6238957636954845, - "heading": -0.39063702231560526, - "angularVelocity": 0.36712346060735523, - "velocityX": 3.9049512650674507, - "velocityY": -1.3395968688778515, - "timestamp": 1.0585913416571873 - }, - { - "x": 5.775207914535934, - "y": 1.5124309100983953, - "heading": -0.39063698838615835, - "angularVelocity": 4.1666957830653565e-7, - "velocityX": 3.998486537128211, - "velocityY": -1.3688408734703337, - "timestamp": 1.1400214448615862 - }, - { - "x": 6.0786617783907655, - "y": 1.4085746056244475, - "heading": -0.3427587122754328, - "angularVelocity": 0.5879677690019331, - "velocityX": 3.7265562969154904, - "velocityY": -1.2754043085670228, - "timestamp": 1.221451548065985 - }, - { - "x": 6.3576257280922785, - "y": 1.3131018062400224, - "heading": -0.2933999650299559, - "angularVelocity": 0.6061486514582699, - "velocityX": 3.4258086226574016, - "velocityY": -1.1724509195914545, - "timestamp": 1.302881651270384 - }, - { - "x": 6.612101112923588, - "y": 1.226012176425091, - "heading": -0.2458719495426286, - "angularVelocity": 0.583666403664149, - "velocityX": 3.1250775182311497, - "velocityY": -1.069501650959794, - "timestamp": 1.384311754474783 - }, - { - "x": 6.84209496743372, - "y": 1.1473035772965767, - "heading": -0.20141693465093127, - "angularVelocity": 0.5459285097563893, - "velocityX": 2.8244328013783875, - "velocityY": -0.9665786488191752, - "timestamp": 1.4657418576791819 - }, - { - "x": 7.047612440347946, - "y": 1.0769745409038134, - "heading": -0.1606891956044553, - "angularVelocity": 0.5001558077881458, - "velocityX": 2.523851313295684, - "velocityY": -0.8636736738037645, - "timestamp": 1.5471719608835808 - }, - { - "x": 7.228657225078741, - "y": 1.0150240865596738, - "heading": -0.12409433294508956, - "angularVelocity": 0.44940214023188485, - "velocityX": 2.223315182056817, - "velocityY": -0.7607807420879281, - "timestamp": 1.6286020640879797 - }, - { - "x": 7.385232062561463, - "y": 0.961451535253283, - "heading": -0.09190931635870993, - "angularVelocity": 0.3952471545515241, - "velocityX": 1.9228127107944446, - "velocityY": -0.6578961489452781, - "timestamp": 1.7100321672923786 - }, - { - "x": 7.517339056055513, - "y": 0.9162563961012582, - "heading": -0.06433562135119408, - "angularVelocity": 0.33861795481579837, - "velocityX": 1.6223360685475974, - "velocityY": -0.5550175840816443, - "timestamp": 1.7914622704967775 - }, - { - "x": 7.624979863197351, - "y": 0.8794382979964872, - "heading": -0.04152628624846568, - "angularVelocity": 0.2801093723960078, - "velocityX": 1.321879782856784, - "velocityY": -0.45214357658804644, - "timestamp": 1.8728923737011764 - }, - { - "x": 7.708155816822711, - "y": 0.8509969471259123, - "heading": -0.02360109386059999, - "angularVelocity": 0.22012980068157206, - "velocityX": 1.0214398650162453, - "velocityY": -0.34927317725710716, - "timestamp": 1.9543224769055754 - }, - { - "x": 7.766868003937176, - "y": 0.8309320995113674, - "heading": -0.010655731264137824, - "angularVelocity": 0.1589751466231924, - "velocityX": 0.7210132961159337, - "velocityY": -0.24640577409290096, - "timestamp": 2.0357525801099743 - }, - { - "x": 7.801117319270969, - "y": 0.8192435425103285, - "heading": -0.0027676483891359333, - "angularVelocity": 0.09686937096470247, - "velocityX": 0.4205977149239743, - "velocityY": -0.1435409822789855, - "timestamp": 2.117182683314373 - }, - { - "x": 7.810904502868652, - "y": 0.8159310817718506, - "heading": 3.324097175751255e-30, - "angularVelocity": 0.03398802506964661, - "velocityX": 0.12019122182758048, - "velocityY": -0.04067857718616754, - "timestamp": 2.198612786518772 - }, - { - "x": 7.7950494488648365, - "y": 0.8213999709085323, - "heading": -0.0026333253300755664, - "angularVelocity": -0.031408125409593986, - "velocityX": -0.18910596379430453, - "velocityY": 0.06522838401102503, - "timestamp": 2.282454954611688 - }, - { - "x": 7.753261271011051, - "y": 0.8357490695116764, - "heading": -0.010653353631792484, - "angularVelocity": -0.09565626085465573, - "velocityX": -0.4984148049162354, - "velocityY": 0.17114417398225837, - "timestamp": 2.3662971227046037 - }, - { - "x": 7.685538815396101, - "y": 0.8589792353835662, - "heading": -0.023944188886482875, - "angularVelocity": -0.15852208449523947, - "velocityX": -0.8077374089360129, - "velocityY": 0.2770701951092858, - "timestamp": 2.4501392907975195 - }, - { - "x": 7.591880716426718, - "y": 0.8910914866950913, - "heading": -0.04236515594390405, - "angularVelocity": -0.21971005135513041, - "velocityX": -1.1170763006222482, - "velocityY": 0.3830083601361243, - "timestamp": 2.5339814588904352 - }, - { - "x": 7.472285350218667, - "y": 0.9320870626307362, - "heading": -0.06574332589364143, - "angularVelocity": -0.27883546527360753, - "velocityX": -1.426434560655829, - "velocityY": 0.4889613051300446, - "timestamp": 2.617823626983351 - }, - { - "x": 7.326750771760018, - "y": 0.9819675124503678, - "heading": -0.09386215907940265, - "angularVelocity": -0.33537817336257436, - "velocityX": -1.7358160191821825, - "velocityY": 0.5949327284136178, - "timestamp": 2.7016657950762673 - }, - { - "x": 7.155274630305055, - "y": 1.0407348314785088, - "heading": -0.12644338322972526, - "angularVelocity": -0.38860188007321295, - "velocityX": -2.045225515458149, - "velocityY": 0.7009279502769267, - "timestamp": 2.7855079631691835 - }, - { - "x": 6.9578540596820755, - "y": 1.1083916783743404, - "heading": -0.16311628951040621, - "angularVelocity": -0.43740407857819336, - "velocityX": -2.3546691970583673, - "velocityY": 0.8069548824268558, - "timestamp": 2.8693501312620997 - }, - { - "x": 6.734485559178827, - "y": 1.1849417416525785, - "heading": -0.20336148011715094, - "angularVelocity": -0.48001132988525586, - "velocityX": -2.664154632257422, - "velocityY": 0.9130258081279993, - "timestamp": 2.953192299355016 - }, - { - "x": 6.485164986270469, - "y": 1.2703903985697897, - "heading": -0.2463960185234521, - "angularVelocity": -0.5132803621992265, - "velocityX": -2.973689476064775, - "velocityY": 1.0191608692956917, - "timestamp": 3.037034467447932 - }, - { - "x": 6.2098884128498435, - "y": 1.3647459693546886, - "heading": -0.2908977372042754, - "angularVelocity": -0.5307796743937351, - "velocityX": -3.2832711710837126, - "velocityY": 1.125395167266338, - "timestamp": 3.1208766355408484 - }, - { - "x": 5.908659770542328, - "y": 1.4680218269782972, - "heading": -0.3341353968865912, - "angularVelocity": -0.5157030246927623, - "velocityX": -3.592805972928633, - "velocityY": 1.231788966968944, - "timestamp": 3.2047188036337646 - }, - { - "x": 5.581609353432503, - "y": 1.5802212470917454, - "heading": -0.3669186513644775, - "angularVelocity": -0.39101153063642635, - "velocityX": -3.9007867347535576, - "velocityY": 1.3382218359275648, - "timestamp": 3.288560971726681 - }, - { - "x": 5.24637103251191, - "y": 1.69499790168712, - "heading": -0.36691872086414007, - "angularVelocity": -8.28934451080165e-7, - "velocityX": -3.9984452757599787, - "velocityY": 1.3689609561166869, - "timestamp": 3.372403139819597 - }, - { - "x": 4.935041308656135, - "y": 1.8014679656257433, - "heading": -0.4122700447901933, - "angularVelocity": -0.540913062693946, - "velocityX": -3.713283314796349, - "velocityY": 1.2698868166270474, - "timestamp": 3.4562453079125133 - }, - { - "x": 4.649661722331028, - "y": 1.8990649018297885, - "heading": -0.4616985909169464, - "angularVelocity": -0.5895427951239265, - "velocityX": -3.4037715485702025, - "velocityY": 1.16405549169346, - "timestamp": 3.5400874760054295 - }, - { - "x": 4.390249017075816, - "y": 1.987780874059184, - "heading": -0.5104183331347605, - "angularVelocity": -0.5810887686470702, - "velocityX": -3.0940600792637536, - "velocityY": 1.058130702572937, - "timestamp": 3.6239296440983457 - }, - { - "x": 4.156796279326317, - "y": 2.0676179686564895, - "heading": -0.5565184993786366, - "angularVelocity": -0.5498446341796518, - "velocityX": -2.784431069229766, - "velocityY": 0.9522307976199715, - "timestamp": 3.707771812191262 - }, - { - "x": 3.949296604382671, - "y": 2.1385786696874747, - "heading": -0.5989640106497574, - "angularVelocity": -0.5062549339621698, - "velocityX": -2.4748844127419294, - "velocityY": 0.8463605205478963, - "timestamp": 3.791613980284178 - }, - { - "x": 3.7677445474124176, - "y": 2.200665074641089, - "heading": -0.6371029281396509, - "angularVelocity": -0.4548894471291752, - "velocityX": -2.1654026977099767, - "velocityY": 0.7405152605883109, - "timestamp": 3.8754561483770944 - }, - { - "x": 3.612135883138456, - "y": 2.25387888808437, - "heading": -0.6704857144975235, - "angularVelocity": -0.3981622507767433, - "velocityX": -1.8559713782867908, - "velocityY": 0.6346903312937655, - "timestamp": 3.9592983164700106 - }, - { - "x": 3.482467285243419, - "y": 2.298221496264914, - "heading": -0.6987833643307629, - "angularVelocity": -0.3375109503595359, - "velocityX": -1.5465797324245634, - "velocityY": 0.5288819360134136, - "timestamp": 4.043140484562927 - }, - { - "x": 3.3787360861374416, - "y": 2.3336940331656435, - "heading": -0.7217450303198044, - "angularVelocity": -0.2738677506955549, - "velocityX": -1.2372199033667683, - "velocityY": 0.42308706594297585, - "timestamp": 4.126982652655843 - }, - { - "x": 3.3009401097656865, - "y": 2.3602974295620016, - "heading": -0.739173949966493, - "angularVelocity": -0.2078777307783457, - "velocityX": -0.927886028490337, - "velocityY": 0.3173032973917799, - "timestamp": 4.210824820748759 - }, - { - "x": 3.249077555413445, - "y": 2.378032448311797, - "heading": -0.7509127758202662, - "angularVelocity": -0.14001100067887515, - "velocityX": -0.6185736310488396, - "velocityY": 0.2115286275772482, - "timestamp": 4.2946669888416755 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": -0.07062482706951323, - "velocityX": -0.3092792155483943, - "velocityY": 0.10576135602688908, - "timestamp": 4.378509156934592 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": 0, - "velocityX": 2.4942198398372333e-31, - "velocityY": -2.7810761994582423e-31, - "timestamp": 4.462351325027508 + "x": 2.37735740952237, + "y": 3.0408026970784268, + "heading": -0.7860869247798871, + "angularVelocity": 0.2648554274741457, + "velocityX": 0.21924714610297322, + "velocityY": -0.1372563412236913, + "timestamp": 0.061110649511906034 + }, + { + "x": 2.404179793787175, + "y": 3.024019444447085, + "heading": -0.7541504516362038, + "angularVelocity": 0.5226007806947081, + "velocityX": 0.4389150578342217, + "velocityY": -0.2746371175137346, + "timestamp": 0.12222129902381207 + }, + { + "x": 2.4444554974169383, + "y": 2.998832316814256, + "heading": -0.7069723408476191, + "angularVelocity": 0.7720112806098135, + "velocityX": 0.6590619466729163, + "velocityY": -0.41215611082519754, + "timestamp": 0.1833319485357181 + }, + { + "x": 2.4982188560395806, + "y": 2.96523040511413, + "heading": -0.645190074686331, + "angularVelocity": 1.0109901736398696, + "velocityX": 0.8797706954852063, + "velocityY": -0.5498536174710421, + "timestamp": 0.24444259804762414 + }, + { + "x": 2.5655102024042766, + "y": 2.923198127539445, + "heading": -0.5696285601591381, + "angularVelocity": 1.2364704864161304, + "velocityX": 1.101139439723772, + "velocityY": -0.6878061010707456, + "timestamp": 0.30555324755953017 + }, + { + "x": 2.646375841205288, + "y": 2.8727129664624207, + "heading": -0.48135684674506, + "angularVelocity": 1.4444571301255718, + "velocityX": 1.3232659028645477, + "velocityY": -0.8261270577264651, + "timestamp": 0.3666638970714362 + }, + { + "x": 2.740867207337065, + "y": 2.813744425609022, + "heading": -0.38173698900245784, + "angularVelocity": 1.6301554399809315, + "velocityX": 1.5462340342720005, + "velocityY": -0.9649470480903679, + "timestamp": 0.42777454658334224 + }, + { + "x": 2.849040211493812, + "y": 2.746255741762665, + "heading": -0.2724850390795677, + "angularVelocity": 1.7877726843928354, + "velocityX": 1.7701170748589676, + "velocityY": -1.1043686229060403, + "timestamp": 0.4888851960952483 + }, + { + "x": 2.9709557136972373, + "y": 2.6702088910620962, + "heading": -0.155828372393994, + "angularVelocity": 1.9089416921161393, + "velocityX": 1.994996014232719, + "velocityY": -1.2444124110602532, + "timestamp": 0.5499958456071543 + }, + { + "x": 3.1066769296559538, + "y": 2.585572206493352, + "heading": -0.03496320783780511, + "angularVelocity": 1.977808541089754, + "velocityX": 2.2209094002882996, + "velocityY": -1.3849743906298195, + "timestamp": 0.6111064951190603 + }, + { + "x": 3.2562355055408387, + "y": 2.4923383100856262, + "heading": 0.08475597327951304, + "angularVelocity": 1.959055943170691, + "velocityX": 2.4473406366879895, + "velocityY": -1.5256571015427038, + "timestamp": 0.6722171446309666 + }, + { + "x": 3.4194188611023764, + "y": 2.3906298265628587, + "heading": 0.19293288100296513, + "angularVelocity": 1.7701809518876104, + "velocityX": 2.6702932609110017, + "velocityY": -1.6643332109070865, + "timestamp": 0.7333277941428729 + }, + { + "x": 3.5932872633720114, + "y": 2.2801851899216916, + "heading": 0.26578166446591744, + "angularVelocity": 1.192080006427692, + "velocityX": 2.845140800471438, + "velocityY": -1.8072895235657866, + "timestamp": 0.7944384436547791 + }, + { + "x": 3.7772765002319786, + "y": 2.1608722925529302, + "heading": 0.30020319888903974, + "angularVelocity": 0.5632657269731189, + "velocityX": 3.0107557083666587, + "velocityY": -1.9524076134310657, + "timestamp": 0.8555490931666854 + }, + { + "x": 3.9732507395108834, + "y": 2.038358982543431, + "heading": 0.30020324669419357, + "angularVelocity": 7.822720631834703e-7, + "velocityX": 3.2068754111462736, + "velocityY": -2.0047783976771947, + "timestamp": 0.9166597426785916 + }, + { + "x": 4.174667645417682, + "y": 1.9250159473015482, + "heading": 0.3002032939445946, + "angularVelocity": 7.731942209741845e-7, + "velocityX": 3.2959379014218597, + "velocityY": -1.8547182225546543, + "timestamp": 0.9777703921904979 + }, + { + "x": 4.383569049898422, + "y": 1.8261442295125712, + "heading": 0.30020334201253857, + "angularVelocity": 7.865722968351813e-7, + "velocityX": 3.4184124395542623, + "velocityY": -1.6179130573585891, + "timestamp": 1.0388810417024041 + }, + { + "x": 4.59891708073721, + "y": 1.7422357169202292, + "heading": 0.3002033920988207, + "angularVelocity": 8.195998987503142e-7, + "velocityX": 3.52390348587004, + "velocityY": -1.3730587591937538, + "timestamp": 1.0999916912143104 + }, + { + "x": 4.819641568305972, + "y": 1.6737075280608862, + "heading": 0.30020344564176327, + "angularVelocity": 8.761638607213102e-7, + "velocityX": 3.6118825332687328, + "velocityY": -1.1213788334223447, + "timestamp": 1.1611023407262167 + }, + { + "x": 5.044645579459654, + "y": 1.6209002757550093, + "heading": 0.30020350452029126, + "angularVelocity": 9.634740995599556e-7, + "velocityX": 3.681911629982681, + "velocityY": -0.8641252012153561, + "timestamp": 1.222212990238123 + }, + { + "x": 5.272810899090047, + "y": 1.584076421741111, + "heading": 0.3002035713939988, + "angularVelocity": 0.0000010943052979537505, + "velocityX": 3.733642523075129, + "velocityY": -0.6025767081190017, + "timestamp": 1.2833236397500292 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3002036452391599, + "angularVelocity": 0.0000012083844913604929, + "velocityX": 3.7668180588495743, + "velocityY": -0.33803334595452383, + "timestamp": 1.3444342892619354 + }, + { + "x": 5.642203177619129, + "y": 1.5568731039887571, + "heading": 0.3002037251666547, + "angularVelocity": 0.0000021691766198010436, + "velocityX": 3.7777797970827858, + "velocityY": -0.177650642031358, + "timestamp": 1.3812812173086941 + }, + { + "x": 5.781555187145968, + "y": 1.5562486567630798, + "heading": 0.30020379895395954, + "angularVelocity": 0.0000020025361337225055, + "velocityX": 3.78191661866635, + "velocityY": -0.016947063399286564, + "timestamp": 1.4181281453554528 + }, + { + "x": 5.9208078464820115, + "y": 1.5615467685843438, + "heading": 0.30020386782175307, + "angularVelocity": 0.0000018690240183201108, + "velocityX": 3.7792203235866277, + "velocityY": 0.14378706997068585, + "timestamp": 1.4549750734022115 + }, + { + "x": 6.059709556234372, + "y": 1.5727578639536914, + "heading": 0.3002039327408759, + "angularVelocity": 0.0000017618598429234572, + "velocityX": 3.7696957959723445, + "velocityY": 0.30426133096145186, + "timestamp": 1.4918220014489703 + }, + { + "x": 6.198009351638548, + "y": 1.5898616832743162, + "heading": 0.30020399449987484, + "angularVelocity": 0.0000016760962775678482, + "velocityX": 3.753360259196494, + "velocityY": 0.46418576058553174, + "timestamp": 1.528668929495729 + }, + { + "x": 6.335457356092555, + "y": 1.6128273194464042, + "heading": 0.30020405375523124, + "angularVelocity": 0.0000016081491612309129, + "velocityX": 3.730243245233006, + "velocityY": 0.6232713930166671, + "timestamp": 1.5655158575424877 + }, + { + "x": 6.471805232718414, + "y": 1.6416132738069629, + "heading": 0.3002041110675489, + "angularVelocity": 0.0000015554164405322294, + "velocityX": 3.700386541120455, + "velocityY": 0.7812307805966762, + "timestamp": 1.6023627855892464 + }, + { + "x": 6.60680663253268, + "y": 1.676167531284266, + "heading": 0.3002041691027596, + "angularVelocity": 0.0000015750352547385845, + "velocityX": 3.6638440969339445, + "velocityY": 0.937778515306732, + "timestamp": 1.639209713636005 + }, + { + "x": 6.740176057879373, + "y": 1.7164157557118485, + "heading": 0.3003755003099619, + "angularVelocity": 0.004649809801927131, + "velocityX": 3.6195534449289073, + "velocityY": 1.0923088181600296, + "timestamp": 1.6760566416827638 + }, + { + "x": 6.869837525657242, + "y": 1.7607930294165437, + "heading": 0.3093430143729343, + "angularVelocity": 0.2433720947264084, + "velocityX": 3.5189220554106844, + "velocityY": 1.2043683437702197, + "timestamp": 1.7129035697295225 + }, + { + "x": 6.995165288352465, + "y": 1.807928452248917, + "heading": 0.3301850856624388, + "angularVelocity": 0.5656393190514163, + "velocityX": 3.4013083135772337, + "velocityY": 1.2792225927914125, + "timestamp": 1.7497504977762812 + }, + { + "x": 7.114963240628345, + "y": 1.8557254140007795, + "heading": 0.35927322814233364, + "angularVelocity": 0.7894319559824341, + "velocityX": 3.2512331048020355, + "velocityY": 1.2971762989633377, + "timestamp": 1.78659742582304 + }, + { + "x": 7.228886146559884, + "y": 1.9032191557267804, + "heading": 0.3911508310575271, + "angularVelocity": 0.8651359721153189, + "velocityX": 3.091788433135363, + "velocityY": 1.2889471183522156, + "timestamp": 1.8234443538697986 + }, + { + "x": 7.336942556890349, + "y": 1.949894399055616, + "heading": 0.423151819771819, + "angularVelocity": 0.8684845768875223, + "velocityX": 2.9325758226938556, + "velocityY": 1.2667336411221237, + "timestamp": 1.8602912819165573 + }, + { + "x": 7.43919043499382, + "y": 1.9954619855368936, + "heading": 0.45375826572504047, + "angularVelocity": 0.8306376562622595, + "velocityX": 2.7749362979111796, + "velocityY": 1.2366726046592738, + "timestamp": 1.897138209963316 + }, + { + "x": 7.535687560612936, + "y": 2.0397400223416313, + "heading": 0.48197582868140687, + "angularVelocity": 0.7658050332054913, + "velocityX": 2.618864875157601, + "velocityY": 1.201675123325048, + "timestamp": 1.9339851380100748 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6818119458710672, + "velocityX": 2.464149969521805, + "velocityY": 1.163303151067446, + "timestamp": 1.9708320660568335 + }, + { + "x": 7.766065583230135, + "y": 2.1519617180113952, + "heading": 0.5396264377138885, + "angularVelocity": 0.5126079654708053, + "velocityX": 2.1996686083558368, + "velocityY": 1.0930061650047849, + "timestamp": 2.0342878363776813 + }, + { + "x": 7.889178097615864, + "y": 2.215679412144434, + "heading": 0.5626262771579723, + "angularVelocity": 0.3624546566496878, + "velocityX": 1.9401311143689866, + "velocityY": 1.004127659484183, + "timestamp": 2.097743606698529 + }, + { + "x": 7.996082408378713, + "y": 2.2729729622929304, + "heading": 0.5769453477724344, + "angularVelocity": 0.22565435014123394, + "velocityX": 1.6847059018008188, + "velocityY": 0.90288952224213, + "timestamp": 2.161199377019377 + }, + { + "x": 8.086990111713977, + "y": 2.3232868849538337, + "heading": 0.5832040110121259, + "angularVelocity": 0.09863032483958853, + "velocityX": 1.432615235393288, + "velocityY": 0.7928975159627525, + "timestamp": 2.2246551473402247 + }, + { + "x": 8.16207340516984, + "y": 2.3662086171292933, + "heading": 0.581874089552229, + "angularVelocity": -0.020958243090781536, + "velocityX": 1.183238231546559, + "velocityY": 0.676403926048603, + "timestamp": 2.2881109176610726 + }, + { + "x": 8.221474033046453, + "y": 2.401420292157025, + "heading": 0.5733258497363348, + "angularVelocity": -0.1347117807044565, + "velocityX": 0.9360949772773745, + "velocityY": 0.5549010728211649, + "timestamp": 2.3515666879819204 + }, + { + "x": 8.265310204045273, + "y": 2.4286697945211704, + "heading": 0.5578573342178689, + "angularVelocity": -0.24376846172150818, + "velocityX": 0.6908145748948221, + "velocityY": 0.4294251291311274, + "timestamp": 2.415022458302768 + }, + { + "x": 8.293681756385627, + "y": 2.447752418352113, + "heading": 0.5357134031725193, + "angularVelocity": -0.34896638924064666, + "velocityX": 0.44710752382171043, + "velocityY": 0.30072322397878726, + "timestamp": 2.478478228623616 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.45094242234392296, + "velocityX": 0.20474492941705758, + "velocityY": 0.16935099752034305, + "timestamp": 2.541933998944464 + }, + { + "x": 8.303680612059921, + "y": 2.4605220907329786, + "heading": 0.4706360870687623, + "angularVelocity": -0.5539055745477068, + "velocityX": -0.04547302080206286, + "velocityY": 0.030737357256443772, + "timestamp": 2.6077618566137937 + }, + { + "x": 8.28417079095848, + "y": 2.4534909416021047, + "heading": 0.42765554839737563, + "angularVelocity": -0.6529232485020114, + "velocityX": -0.2963763639315537, + "velocityY": -0.1068111492583101, + "timestamp": 2.6735897142831235 + }, + { + "x": 8.248092493080108, + "y": 2.437487576077448, + "heading": 0.3784580606750378, + "angularVelocity": -0.7473657728536394, + "velocityX": -0.5480703634561933, + "velocityY": -0.24310931710774963, + "timestamp": 2.7394175719524534 + }, + { + "x": 8.195385029545998, + "y": 2.412609981570135, + "heading": 0.3233968459430461, + "angularVelocity": -0.8364424528074197, + "velocityX": -0.8006862960492162, + "velocityY": -0.37791894477683524, + "timestamp": 2.8052454296217832 + }, + { + "x": 8.125976752501963, + "y": 2.378976757579925, + "heading": 0.2628923564512202, + "angularVelocity": -0.9191319850595123, + "velocityX": -1.0543906410063166, + "velocityY": -0.5109269112046505, + "timestamp": 2.871073287291113 + }, + { + "x": 8.039781850826214, + "y": 2.336734306251694, + "heading": 0.19745501482460134, + "angularVelocity": -0.9940676173198235, + "velocityX": -1.3093985544650038, + "velocityY": -0.641710862602064, + "timestamp": 2.936901144960443 + }, + { + "x": 7.936695821305385, + "y": 2.286067717588345, + "heading": 0.12772072746128918, + "angularVelocity": -1.0593431084086873, + "velocityX": -1.565993990548153, + "velocityY": -0.7696830864200519, + "timestamp": 3.002729002629773 + }, + { + "x": 7.816588930914964, + "y": 2.2272180395232652, + "heading": 0.05450854772010129, + "angularVelocity": -1.1121762477665882, + "velocityX": -1.8245602187716414, + "velocityY": -0.8939935180740173, + "timestamp": 3.0685568602991027 + }, + { + "x": 7.679296617994528, + "y": 2.1605113660425403, + "heading": -0.02108172308249024, + "angularVelocity": -1.1483021547245422, + "velocityX": -2.085626325712903, + "velocityY": -1.01335021133165, + "timestamp": 3.1343847179684325 + }, + { + "x": 7.52460540494712, + "y": 2.086411774056454, + "heading": -0.09749530920005847, + "angularVelocity": -1.16080925041513, + "velocityX": -2.34993540006212, + "velocityY": -1.1256570486967377, + "timestamp": 3.2002125756377624 + }, + { + "x": 7.352233710537335, + "y": 2.0056280451301003, + "heading": -0.17238692358324137, + "angularVelocity": -1.1376887693867086, + "velocityX": -2.6185220135167313, + "velocityY": -1.2271966882493952, + "timestamp": 3.2660404333070923 + }, + { + "x": 7.161817946345765, + "y": 1.9193611743859953, + "heading": -0.24189742475427997, + "angularVelocity": -1.055943541717672, + "velocityX": -2.8926319484385736, + "velocityY": -1.3104918464375106, + "timestamp": 3.331868290976422 + }, + { + "x": 6.953014973362115, + "y": 1.8300106922230066, + "heading": -0.298719434373675, + "angularVelocity": -0.8631909290566133, + "velocityX": -3.1719545550536123, + "velocityY": -1.3573354097564532, + "timestamp": 3.397696148645752 + }, + { + "x": 6.727431268978415, + "y": 1.7436413956339059, + "heading": -0.3201876383615551, + "angularVelocity": -0.32612642653085444, + "velocityX": -3.4268729436231227, + "velocityY": -1.3120478114745338, + "timestamp": 3.463524006315082 + }, + { + "x": 6.489449879790782, + "y": 1.6705324422161696, + "heading": -0.3201878815696182, + "angularVelocity": -0.0000036946069898928346, + "velocityX": -3.61520787115807, + "velocityY": -1.1106081225517261, + "timestamp": 3.5293518639844117 + }, + { + "x": 6.246606826324383, + "y": 1.6156931769447658, + "heading": -0.3201879117478121, + "angularVelocity": -4.584410758613646e-7, + "velocityX": -3.689062078949349, + "velocityY": -0.8330707881589553, + "timestamp": 3.5951797216537416 + }, + { + "x": 6.0003025647982255, + "y": 1.5794398654466013, + "heading": -0.32018794628515007, + "angularVelocity": -5.246614307457641e-7, + "velocityX": -3.741641764546064, + "velocityY": -0.5507290193199733, + "timestamp": 3.6610075793230714 + }, + { + "x": 5.7519574472803, + "y": 1.5619815744715873, + "heading": -0.32018798749762734, + "angularVelocity": -6.260643854910128e-7, + "velocityX": -3.772644687381865, + "velocityY": -0.26521128885450773, + "timestamp": 3.7268354369924013 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.32018803965163717, + "angularVelocity": -7.922787054710825e-7, + "velocityX": -3.7818920261895626, + "velocityY": 0.021835891254127007, + "timestamp": 3.792663294661731 + }, + { + "x": 5.250083628410863, + "y": 1.584512642850435, + "heading": -0.3201880864656727, + "angularVelocity": -6.975962896264183e-7, + "velocityX": -3.768870382202017, + "velocityY": 0.31432577229262343, + "timestamp": 3.859770927241254 + }, + { + "x": 4.999553273526972, + "y": 1.6251081857259102, + "heading": -0.32018812296137045, + "angularVelocity": -5.438382542161688e-7, + "velocityX": -3.7332617059171653, + "velocityY": 0.6049318283932773, + "timestamp": 3.926878559820777 + }, + { + "x": 4.752913977063861, + "y": 1.6849623155206188, + "heading": -0.3201881529794151, + "angularVelocity": -4.473119302496649e-7, + "velocityX": -3.675279353221748, + "velocityY": 0.8919124024198194, + "timestamp": 3.9939861924003 + }, + { + "x": 4.511643860653374, + "y": 1.7637163133941143, + "heading": -0.3201881787520496, + "angularVelocity": -3.8404922716455346e-7, + "velocityX": -3.595270867655492, + "velocityY": 1.1735475510952014, + "timestamp": 4.061093824979823 + }, + { + "x": 4.277188862383835, + "y": 1.86089818593956, + "heading": -0.3201882016908444, + "angularVelocity": -3.41820951724987e-7, + "velocityX": -3.4937158301883047, + "velocityY": 1.4481493208732221, + "timestamp": 4.128201457559346 + }, + { + "x": 4.050954064298834, + "y": 1.9759254853688604, + "heading": -0.32018822275897413, + "angularVelocity": -3.1394535808439923e-7, + "velocityX": -3.3712230544999895, + "velocityY": 1.7140717830120589, + "timestamp": 4.195309090138869 + }, + { + "x": 3.834295246075004, + "y": 2.1081087693328064, + "heading": -0.32018824266587215, + "angularVelocity": -2.966413400864343e-7, + "velocityX": -3.2285272165887804, + "velocityY": 1.969720565053586, + "timestamp": 4.262416722718392 + }, + { + "x": 3.6285105963453295, + "y": 2.256655537498403, + "heading": -0.32018826197928, + "angularVelocity": -2.8779748460931035e-7, + "velocityX": -3.066486505626901, + "velocityY": 2.213559955189428, + "timestamp": 4.329524355297915 + }, + { + "x": 3.4323361610843617, + "y": 2.417680552436319, + "heading": -0.3201882811600919, + "angularVelocity": -2.858216149634079e-7, + "velocityX": -2.9232805229494647, + "velocityY": 2.399503733753405, + "timestamp": 4.396631987877438 + }, + { + "x": 3.2362856931678534, + "y": 2.578856476556199, + "heading": -0.32018830034052725, + "angularVelocity": -2.858160039511642e-7, + "velocityX": -2.921433231669861, + "velocityY": 2.401752497063368, + "timestamp": 4.463739620456961 + }, + { + "x": 3.0402351711109183, + "y": 2.7400323348209366, + "heading": -0.3201883195209977, + "angularVelocity": -2.858165259418001e-7, + "velocityX": -2.9214340384398776, + "velocityY": 2.401751515727269, + "timestamp": 4.530847253036484 + }, + { + "x": 2.8448193652926044, + "y": 2.899781496680513, + "heading": -0.32456406822795664, + "angularVelocity": -0.06520493331028634, + "velocityX": -2.911975855902028, + "velocityY": 2.380491692510139, + "timestamp": 4.597954885616007 + }, + { + "x": 2.66318575993582, + "y": 3.04911523833719, + "heading": -0.3692876119349906, + "angularVelocity": -0.6664449629337832, + "velocityX": -2.706601296678858, + "velocityY": 2.2252869892216163, + "timestamp": 4.66506251819553 + }, + { + "x": 2.496643428698106, + "y": 3.186035937240345, + "heading": -0.4222622822262224, + "angularVelocity": -0.7893985863449552, + "velocityX": -2.4817196619231154, + "velocityY": 2.040314844081317, + "timestamp": 4.732170150775053 + }, + { + "x": 2.3452748100493945, + "y": 3.3104804771184444, + "heading": -0.4767233164983405, + "angularVelocity": -0.8115475420412368, + "velocityX": -2.255609575696765, + "velocityY": 1.8544021759467144, + "timestamp": 4.799277783354576 + }, + { + "x": 2.209079907655855, + "y": 3.4224492823331634, + "heading": -0.5296433021172221, + "angularVelocity": -0.7885837062747133, + "velocityX": -2.0294994348392206, + "velocityY": 1.6684958314098723, + "timestamp": 4.866385415934099 + }, + { + "x": 2.088048432415951, + "y": 3.5219507247047837, + "heading": -0.5792907495482946, + "angularVelocity": -0.7398181923977142, + "velocityX": -1.803542616355617, + "velocityY": 1.4827142390056927, + "timestamp": 4.933493048513622 + }, + { + "x": 1.9821698412647601, + "y": 3.6089936285690154, + "heading": -0.624538681260696, + "angularVelocity": -0.6742590965160707, + "velocityX": -1.5777429046647484, + "velocityY": 1.297064141863228, + "timestamp": 5.000600681093145 + }, + { + "x": 1.8914349570352331, + "y": 3.6835858822586136, + "heading": -0.6645923745497577, + "angularVelocity": -0.5968574922025148, + "velocityX": -1.3520799459287431, + "velocityY": 1.1115315921956592, + "timestamp": 5.067708313672668 + }, + { + "x": 1.815836039628641, + "y": 3.7457342610555044, + "heading": -0.698860244587487, + "angularVelocity": -0.5106404252470358, + "velocityX": -1.1265323257679698, + "velocityY": 0.926100003948804, + "timestamp": 5.134815946252191 + }, + { + "x": 1.7553665690239355, + "y": 3.7954445247907986, + "heading": -0.7268845476309118, + "angularVelocity": -0.41760231982876106, + "velocityX": -0.9010818632746272, + "velocityY": 0.7407542454487245, + "timestamp": 5.201923578831714 + }, + { + "x": 1.7100210087875185, + "y": 3.8327215653417337, + "heading": -0.7483006908429537, + "angularVelocity": -0.31913125808251036, + "velocityX": -0.6757139015846285, + "velocityY": 0.5554813829374979, + "timestamp": 5.269031211411237 + }, + { + "x": 1.6797946046172731, + "y": 3.8575695457400823, + "heading": -0.7628116421295846, + "angularVelocity": -0.21623399200434476, + "velocityX": -0.4504167858168276, + "velocityY": 0.37027055557210875, + "timestamp": 5.33613884399076 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.10966416437475777, + "velocityX": -0.22518126874279432, + "velocityY": 0.1851126545280661, + "timestamp": 5.403246476570283 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -1.211900873027185e-19, + "velocityX": -5.462077711506373e-19, + "velocityY": -3.512888080115249e-19, + "timestamp": 5.470354109149806 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint.3.traj b/src/main/deploy/choreo/SourceLanePHGSprint.3.traj index d30025ca..d9e2ca2a 100644 --- a/src/main/deploy/choreo/SourceLanePHGSprint.3.traj +++ b/src/main/deploy/choreo/SourceLanePHGSprint.3.traj @@ -1,661 +1,319 @@ { "samples": [ { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": 0, - "velocityX": 2.4942198398372333e-31, - "velocityY": -2.7810761994582423e-31, + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -1.211900873027185e-19, + "velocityX": -5.462077711506373e-19, + "velocityY": -3.512888080115249e-19, "timestamp": 0 }, { - "x": 3.2340252732371284, - "y": 2.3791227322097765, - "heading": -0.751823485237952, - "angularVelocity": 0.08542971221571066, - "velocityX": 0.1854727097784258, - "velocityY": -0.13259511367658153, - "timestamp": 0.05865206700400005 - }, - { - "x": 3.255906850263932, - "y": 2.3637444378026573, - "heading": -0.7418521789042297, - "angularVelocity": 0.1700077566412493, - "velocityX": 0.37307426906730357, - "velocityY": -0.2621952676632906, - "timestamp": 0.1173041340080001 - }, - { - "x": 3.2889308491117024, - "y": 2.3409679566825354, - "heading": -0.7269791464813684, - "angularVelocity": 0.25358070367489455, - "velocityX": 0.5630491905002851, - "velocityY": -0.38833211314732513, - "timestamp": 0.17595620101200016 - }, - { - "x": 3.333253092651543, - "y": 2.311030569251179, - "heading": -0.7072747038712209, - "angularVelocity": 0.33595478585270977, - "velocityX": 0.7556808447487132, - "velocityY": -0.5104233995592209, - "timestamp": 0.2346082680160002 - }, - { - "x": 3.3890486465390515, - "y": 2.274212618473126, - "heading": -0.6828237317423621, - "angularVelocity": 0.4168816783079693, - "velocityX": 0.9512973154672166, - "velocityY": -0.6277349232302787, - "timestamp": 0.29326033502000026 - }, - { - "x": 3.456514730281209, - "y": 2.2308496047648436, - "heading": -0.6537300972010771, - "angularVelocity": 0.4960376680211611, - "velocityX": 1.1502763191202974, - "velocityY": -0.7393262662904144, - "timestamp": 0.3519124020240003 - }, - { - "x": 3.5358737017178647, - "y": 2.1813488976810533, - "heading": -0.6201228984071238, - "angularVelocity": 0.5729925731630496, - "velocityX": 1.3530464566789135, - "velocityY": -0.843972081673003, - "timestamp": 0.41056446902800037 - }, - { - "x": 3.6273755302567405, - "y": 2.1262132491994077, - "heading": -0.5821654777138521, - "angularVelocity": 0.6471625405918474, - "velocityX": 1.5600784970227222, - "velocityY": -0.9400461279205379, - "timestamp": 0.4692165360320004 - }, - { - "x": 3.731298337786468, - "y": 2.066074432465498, - "heading": -0.540068702761689, - "angularVelocity": 0.7177372785394325, - "velocityX": 1.7718524314350428, - "velocityY": -1.0253486331489114, - "timestamp": 0.5278686030360005 - }, - { - "x": 3.8479437090237303, - "y": 2.0017419289175256, - "heading": -0.49411087881678867, - "angularVelocity": 0.783566995887226, - "velocityX": 1.9887682940365567, - "velocityY": -1.09684972472642, - "timestamp": 0.5865206700400005 - }, - { - "x": 3.977619323731122, - "y": 1.934273302196501, - "heading": -0.44466788468288826, - "angularVelocity": 0.8429880933357167, - "velocityX": 2.2109300035163186, - "velocityY": -1.1503196761407186, - "timestamp": 0.6451727370440006 - }, - { - "x": 4.120592730506154, - "y": 1.8650726101269166, - "heading": -0.3922584382465056, - "angularVelocity": 0.8935652077327264, - "velocityX": 2.437653335649394, - "velocityY": -1.1798508663789342, - "timestamp": 0.7038248040480006 - }, - { - "x": 4.276984319445326, - "y": 1.7960128941742703, - "heading": -0.3376091877980351, - "angularVelocity": 0.9317531885916955, - "velocityX": 2.6664292825094678, - "velocityY": -1.1774472662308222, - "timestamp": 0.7624768710520007 - }, - { - "x": 4.446552051157268, - "y": 1.729536835320209, - "heading": -0.28173622459333697, - "angularVelocity": 0.9526171209087647, - "velocityX": 2.891078530964293, - "velocityY": -1.1333966942636178, - "timestamp": 0.8211289380560007 - }, - { - "x": 4.628358146968442, - "y": 1.6685924186219296, - "heading": -0.2260024396425045, - "angularVelocity": 0.9502441737821766, - "velocityX": 3.0997389367159993, - "velocityY": -1.0390838688451245, - "timestamp": 0.8797810050600008 - }, - { - "x": 4.820505608377407, - "y": 1.6162066866444724, - "heading": -0.17202923007966578, - "angularVelocity": 0.9202268959959776, - "velocityX": 3.2760560918656307, - "velocityY": -0.8931608833817366, - "timestamp": 0.9384330720640008 - }, - { - "x": 5.0203266835475935, - "y": 1.5748141105227507, - "heading": -0.12139543853917772, - "angularVelocity": 0.863290828898412, - "velocityX": 3.4068888852043435, - "velocityY": -0.7057309015025682, - "timestamp": 0.9970851390680009 - }, - { - "x": 5.225003672109784, - "y": 1.5458956549300438, - "heading": -0.07534747020156207, - "angularVelocity": 0.7851039305140859, - "velocityX": 3.4896807396102973, - "velocityY": -0.4930509199400402, - "timestamp": 1.055737206072001 - }, - { - "x": 5.432079779027424, - "y": 1.5301290764345634, - "heading": -0.034717164940788754, - "angularVelocity": 0.6927344139125162, - "velocityX": 3.5305849818305606, - "velocityY": -0.2688153939128066, - "timestamp": 1.114389273076001 - }, - { - "x": 5.639612197875977, - "y": 1.527712106704712, - "heading": 0, - "angularVelocity": 0.5919171601986558, - "velocityX": 3.538364962216936, - "velocityY": -0.04120860275370209, - "timestamp": 1.173041340080001 - }, - { - "x": 5.885430007343772, - "y": 1.5437296393309712, - "heading": 0.03257763753195304, - "angularVelocity": 0.4661620557103504, - "velocityX": 3.5174722316598084, - "velocityY": 0.22919912253124494, - "timestamp": 1.24292613042895 - }, - { - "x": 6.1267293814658395, - "y": 1.5781470725007365, - "heading": 0.05611463103184265, - "angularVelocity": 0.3367970824891158, - "velocityX": 3.4528167419149254, - "velocityY": 0.49248817944379436, - "timestamp": 1.312810920777899 - }, - { - "x": 6.3596238960316915, - "y": 1.6295476131104372, - "heading": 0.07077632892605286, - "angularVelocity": 0.20979812375484447, - "velocityX": 3.332549377381865, - "velocityY": 0.7355039680744148, - "timestamp": 1.382695711126848 - }, - { - "x": 6.579809345763393, - "y": 1.6950287619001128, - "heading": 0.07741889217082244, - "angularVelocity": 0.09505019921504924, - "velocityX": 3.1506919979622094, - "velocityY": 0.9369871249912134, - "timestamp": 1.452580501475797 - }, - { - "x": 6.7836458698285345, - "y": 1.770189303236, - "heading": 0.07752756675122702, - "angularVelocity": 0.0015550533937633648, - "velocityX": 2.916750884524989, - "velocityY": 1.0754921201107606, - "timestamp": 1.522465291824746 - }, - { - "x": 6.969066872375604, - "y": 1.8501496814117884, - "heading": 0.07288957023575314, - "angularVelocity": -0.06636632223285495, - "velocityX": 2.653238302944389, - "velocityY": 1.1441742584692527, - "timestamp": 1.5923500821736951 - }, - { - "x": 7.135444568197587, - "y": 1.930676718523136, - "heading": 0.06516656217506993, - "angularVelocity": -0.11051057064234768, - "velocityX": 2.3807425763349213, - "velocityY": 1.1522827314678832, - "timestamp": 1.6622348725226441 - }, - { - "x": 7.282944965396222, - "y": 2.00855476747338, - "heading": 0.05568193868283516, - "angularVelocity": -0.1357179930693943, - "velocityX": 2.110622303682033, - "velocityY": 1.114377657304012, - "timestamp": 1.7321196628715931 - }, - { - "x": 7.412041998803859, - "y": 2.081448389680507, - "heading": 0.04544169764196664, - "angularVelocity": -0.14653032497825166, - "velocityX": 1.8472836902425835, - "velocityY": 1.0430541730633198, - "timestamp": 1.8020044532205421 - }, - { - "x": 7.52328085623725, - "y": 2.1476599835831536, - "heading": 0.035211647982849995, - "angularVelocity": -0.1463844937937982, - "velocityX": 1.5917463138681878, - "velocityY": 0.9474392578419155, - "timestamp": 1.8718892435694912 - }, - { - "x": 7.6171846756036246, - "y": 2.2059319251882075, - "heading": 0.025583765782598092, - "angularVelocity": -0.13776791991759432, - "velocityX": 1.343694656555366, - "velocityY": 0.8338286673550865, - "timestamp": 1.9417740339184402 - }, - { - "x": 7.6942246354285295, - "y": 2.2553098100503624, - "heading": 0.01702415874138481, - "angularVelocity": -0.12248168733816604, - "velocityX": 1.1023852177309095, - "velocityY": 0.7065612505324912, - "timestamp": 2.011658824267389 - }, - { - "x": 7.7548150787245245, - "y": 2.295051799999363, - "heading": 0.009906531689475896, - "angularVelocity": -0.10184801322818779, - "velocityX": 0.8670047229655302, - "velocityY": 0.5686786745808462, - "timestamp": 2.081543614616338 - }, - { - "x": 7.799317576321477, - "y": 2.3245685027375944, - "heading": 0.0045355005230709265, - "angularVelocity": -0.0768555094690318, - "velocityX": 0.6367980411008246, - "velocityY": 0.42236232792355316, - "timestamp": 2.151428404965287 - }, - { - "x": 7.828047535553298, - "y": 2.3433824520970585, - "heading": 0.0011629993208823611, - "angularVelocity": -0.04825801415943252, - "velocityX": 0.41110460642961777, - "velocityY": 0.26921379123442685, - "timestamp": 2.221313195314236 - }, - { - "x": 7.841280937194824, - "y": 2.351100206375122, - "heading": 0, - "angularVelocity": -0.016641665734064087, - "velocityX": 0.18936025386137842, - "velocityY": 0.11043539287343317, - "timestamp": 2.291197985663185 - }, - { - "x": 7.838534697779776, - "y": 2.3467601761094876, - "heading": 0.0013754462067047074, - "angularVelocity": 0.018914520887371087, - "velocityX": -0.03776505582293692, - "velocityY": -0.059682154569921285, - "timestamp": 2.363917047113114 - }, - { - "x": 7.819004816606, - "y": 2.3304120813281917, - "heading": 0.005139122834024266, - "angularVelocity": 0.05175639718495326, - "velocityX": -0.2685661886219886, - "velocityY": -0.22481168562045675, - "timestamp": 2.436636108563043 - }, - { - "x": 7.782387572934137, - "y": 2.3024894711129518, - "heading": 0.011058252379099645, - "angularVelocity": 0.08139722140323555, - "velocityX": -0.5035439531501099, - "velocityY": -0.3839792436604268, - "timestamp": 2.509355170012972 - }, - { - "x": 7.728336383406839, - "y": 2.2635182406954444, - "heading": 0.018855442211434054, - "angularVelocity": 0.10722346626686281, - "velocityX": -0.7432877769539888, - "velocityY": -0.5359149257494328, - "timestamp": 2.582074231462901 - }, - { - "x": 7.656454719773965, - "y": 2.2141471165839395, - "heading": 0.028196004270951525, - "angularVelocity": 0.12844723066109603, - "velocityX": -0.9884844798549632, - "velocityY": -0.6789296111240317, - "timestamp": 2.6547932929128297 - }, - { - "x": 7.566289368711756, - "y": 2.155191783566264, - "heading": 0.0386702307264049, - "angularVelocity": 0.14403687625513997, - "velocityX": -1.2399135696256622, - "velocityY": -0.8107273642175508, - "timestamp": 2.7275123543627586 - }, - { - "x": 7.457326803867402, - "y": 2.087700059950414, - "heading": 0.049768261525896085, - "angularVelocity": 0.15261515451671143, - "velocityX": -1.4984044440587245, - "velocityY": -0.9281159887125598, - "timestamp": 2.8002314158126875 - }, - { - "x": 7.328999818599222, - "y": 2.0130495362466543, - "heading": 0.06084411215879332, - "angularVelocity": 0.1523101427886762, - "velocityX": -1.7646952904712614, - "velocityY": -1.0265606048169413, - "timestamp": 2.8729504772626164 - }, - { - "x": 7.180722027662032, - "y": 1.933093466427668, - "heading": 0.07106435780754797, - "angularVelocity": 0.14054424582737238, - "velocityX": -2.039049844438464, - "velocityY": -1.0995201013979004, - "timestamp": 2.9456695387125453 - }, - { - "x": 7.011991212253484, - "y": 1.8503687292260445, - "heading": 0.07933791177942008, - "angularVelocity": 0.11377421279795905, - "velocityX": -2.320310686692896, - "velocityY": -1.1375935765972949, - "timestamp": 3.0183886001624742 - }, - { - "x": 6.822644207407782, - "y": 1.768344037494235, - "heading": 0.0842369889796069, - "angularVelocity": 0.06736991790742633, - "velocityX": -2.6038153005601905, - "velocityY": -1.1279668644828114, - "timestamp": 3.091107661612403 - }, - { - "x": 6.613366078180803, - "y": 1.6915478876016088, - "heading": 0.08398293037214401, - "angularVelocity": -0.0034937003090699014, - "velocityX": -2.8778992062635105, - "velocityY": -1.0560662962558207, - "timestamp": 3.163826723062332 - }, - { - "x": 6.386348019322392, - "y": 1.6251685070860846, - "heading": 0.07667502896821522, - "angularVelocity": -0.10049499069732412, - "velocityX": -3.121850782063855, - "velocityY": -0.9128195440370198, - "timestamp": 3.236545784512261 - }, - { - "x": 6.145418125434476, - "y": 1.573898737537472, - "heading": 0.060738243657818394, - "angularVelocity": -0.21915554178831262, - "velocityX": -3.313160113511754, - "velocityY": -0.7050389337589908, - "timestamp": 3.30926484596219 - }, - { - "x": 5.895122985100364, - "y": 1.5408852116427318, - "heading": 0.035252939658974705, - "angularVelocity": -0.35046249897479254, - "velocityX": -3.441946792815155, - "velocityY": -0.45398723851065725, - "timestamp": 3.3819839074121187 - }, - { - "x": 5.639612197875977, - "y": 1.527712106704712, - "heading": 0, - "angularVelocity": -0.48478265472730503, - "velocityX": -3.513670035473711, - "velocityY": -0.18115064572292816, - "timestamp": 3.4547029688620476 - }, - { - "x": 5.421755886877325, - "y": 1.5312175249525746, - "heading": -0.03672524163627121, - "angularVelocity": -0.5960811255744302, - "velocityX": -3.5359885813607668, - "velocityY": 0.05689584497468608, - "timestamp": 3.516314115500654 - }, - { - "x": 5.204290292939256, - "y": 1.5494561200267603, - "heading": -0.08000897664374254, - "angularVelocity": -0.7025309115144727, - "velocityX": -3.5296469194716105, - "velocityY": 0.2960275221165476, - "timestamp": 3.57792526213926 - }, - { - "x": 4.9895755761204486, - "y": 1.5821879429902566, - "heading": -0.1292741669916279, - "angularVelocity": -0.7996148917153227, - "velocityX": -3.4849979027052926, - "velocityY": 0.531264629036892, - "timestamp": 3.6395364087778663 - }, - { - "x": 4.780630223860834, - "y": 1.6285196439150298, - "heading": -0.18348985013811628, - "angularVelocity": -0.8799654949534231, - "velocityX": -3.3913563317565196, - "velocityY": 0.7520019258291494, - "timestamp": 3.7011475554164726 - }, - { - "x": 4.58085176131032, - "y": 1.6864679206086615, - "heading": -0.24107165456516222, - "angularVelocity": -0.9346004346389692, - "velocityX": -3.2425701102815387, - "velocityY": 0.9405485834169061, - "timestamp": 3.762758702055079 - }, - { - "x": 4.39322892066763, - "y": 1.7528867750124326, - "heading": -0.3000698023851603, - "angularVelocity": -0.9575888623866302, - "velocityX": -3.045274286862978, - "velocityY": 1.0780330837432093, - "timestamp": 3.824369848693685 - }, - { - "x": 4.21961035389735, - "y": 1.8241528988520266, - "heading": -0.35862216993070006, - "angularVelocity": -0.9503534788760283, - "velocityX": -2.81797330909422, - "velocityY": 1.1567082862070746, - "timestamp": 3.8859809953322912 - }, - { - "x": 4.060717549232027, - "y": 1.89701942348135, - "heading": -0.4152832433765806, - "angularVelocity": -0.9196562073132535, - "velocityX": -2.578961979028652, - "velocityY": 1.1826841181310743, - "timestamp": 3.9475921419708975 - }, - { - "x": 3.916597920233781, - "y": 1.9689586974543731, - "heading": -0.46903556717701983, - "angularVelocity": -0.8724447885337365, - "velocityX": -2.3391810875329626, - "velocityY": 1.1676340710715827, - "timestamp": 4.009203288609504 - }, - { - "x": 3.7870044894605353, - "y": 2.0381033443744836, - "heading": -0.5191754657650203, - "angularVelocity": -0.8138121317901729, - "velocityX": -2.103408844724257, - "velocityY": 1.122274956603138, - "timestamp": 4.07081443524811 - }, - { - "x": 3.6715959698515057, - "y": 2.1030801530689605, - "heading": -0.5652089663039379, - "angularVelocity": -0.747161886288821, - "velocityX": -1.8731759739188905, - "velocityY": 1.0546274860880838, - "timestamp": 4.132425581886716 - }, - { - "x": 3.5700229782202233, - "y": 2.1628635944770185, - "heading": -0.6067804926332262, - "angularVelocity": -0.6747403448459703, - "velocityX": -1.6486138819503326, - "velocityY": 0.9703348285129628, - "timestamp": 4.194036728525322 - }, - { - "x": 3.4819602854664806, - "y": 2.2166708649366824, - "heading": -0.6436270740346884, - "angularVelocity": -0.5980505705825362, - "velocityX": -1.4293305279691468, - "velocityY": 0.873336618376596, - "timestamp": 4.255647875163929 - }, - { - "x": 3.407116174486021, - "y": 2.2638908403054034, - "heading": -0.6755491614396836, - "angularVelocity": -0.518121949462191, - "velocityX": -1.2147819844917866, - "velocityY": 0.7664193566417493, - "timestamp": 4.317259021802535 - }, - { - "x": 3.3452327250842697, - "y": 2.3040363154035894, - "heading": -0.7023917849131657, - "angularVelocity": -0.43567803778971587, - "velocityX": -1.0044196996485568, - "velocityY": 0.6515943508350659, - "timestamp": 4.378870168441141 - }, - { - "x": 3.296082852481604, - "y": 2.336711509393393, - "heading": -0.7240321240208134, - "angularVelocity": -0.35124064862133164, - "velocityX": -0.7977431890850113, - "velocityY": 0.5303454938351937, - "timestamp": 4.440481315079747 - }, - { - "x": 3.259466484246681, - "y": 2.3615895348218507, - "heading": -0.740371103456648, - "angularVelocity": -0.26519518508030326, - "velocityX": -0.5943140199890253, - "velocityY": 0.4037909824075443, - "timestamp": 4.5020924617183535 - }, - { - "x": 3.235206808114262, - "y": 2.3783964459205325, - "heading": -0.7513275636357691, - "angularVelocity": -0.1778324341760547, - "velocityX": -0.393754660576598, - "velocityY": 0.2727901039931329, - "timestamp": 4.56370360835696 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": -0.08937588581967223, - "velocityX": -0.19574205864421412, - "velocityY": 0.1380150223608686, - "timestamp": 4.625314754995566 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": 0, - "velocityX": 8.242872749735147e-35, - "velocityY": 1.4597202684890146e-35, - "timestamp": 4.686925901634172 + "x": 1.659892349205253, + "y": 3.8800826826438297, + "heading": -0.7264777443526473, + "angularVelocity": 0.7466474211356967, + "velocityX": -0.08186842286768115, + "velocityY": 0.1724334423754288, + "timestamp": 0.05851918722290517 + }, + { + "x": 1.6562452238185605, + "y": 3.90221669421699, + "heading": -0.6460681306884017, + "angularVelocity": 1.3740726329290587, + "velocityX": -0.06232358239700476, + "velocityY": 0.3782351160970439, + "timestamp": 0.11703837444581033 + }, + { + "x": 1.6553319250883793, + "y": 3.9350953760295733, + "heading": -0.5183992679116819, + "angularVelocity": 2.1816581677803772, + "velocityX": -0.01560682527428515, + "velocityY": 0.5618444714097892, + "timestamp": 0.1755575616687155 + }, + { + "x": 1.6623538133314129, + "y": 3.9788519148323376, + "heading": -0.3621106955703112, + "angularVelocity": 2.670723565350507, + "velocityX": 0.11999292157436166, + "velocityY": 0.7477297768352748, + "timestamp": 0.23407674889162067 + }, + { + "x": 1.6832034751641156, + "y": 4.019420947962781, + "heading": -0.22540240918746318, + "angularVelocity": 2.3361275655130602, + "velocityX": 0.35628761816674753, + "velocityY": 0.6932603656286723, + "timestamp": 0.29259593611452583 + }, + { + "x": 1.7146647081648767, + "y": 4.052413452071288, + "heading": -0.11878752772286896, + "angularVelocity": 1.8218790541038834, + "velocityX": 0.5376225216683612, + "velocityY": 0.5637895137339223, + "timestamp": 0.351115123337431 + }, + { + "x": 1.7541026264736177, + "y": 4.074977116543675, + "heading": -0.0408151972626467, + "angularVelocity": 1.332423332593777, + "velocityX": 0.6739314091721454, + "velocityY": 0.38557720199428364, + "timestamp": 0.40963431056033617 + }, + { + "x": 1.799090355046403, + "y": 4.0866639858943925, + "heading": 1.7934233852716553e-24, + "angularVelocity": 0.6974669198185859, + "velocityX": 0.7687688552717376, + "velocityY": 0.199710042215758, + "timestamp": 0.46815349778324133 + }, + { + "x": 1.848745584487915, + "y": 4.087520122528076, + "heading": 7.573339964499894e-25, + "angularVelocity": -5.2358045608260825e-25, + "velocityX": 0.848529034628775, + "velocityY": 0.014630015800164336, + "timestamp": 0.5266726850061465 + }, + { + "x": 1.9382598731728313, + "y": 4.089063493944204, + "heading": 6.819777064270339e-25, + "angularVelocity": -2.006050179968467e-26, + "velocityX": 1.1795649340806893, + "velocityY": 0.02033761122914328, + "timestamp": 0.6025602314214531 + }, + { + "x": 2.0528956622357724, + "y": 4.091040000742217, + "heading": 6.067470023829544e-25, + "angularVelocity": -1.8954686232133377e-26, + "velocityX": 1.510600809724148, + "velocityY": 0.026045206247625866, + "timestamp": 0.6784477778367597 + }, + { + "x": 2.1926529480236727, + "y": 4.09344964285913, + "heading": 5.320081952060173e-25, + "angularVelocity": -2.1711320260805291e-26, + "velocityX": 1.8416366372297248, + "velocityY": 0.03175280043613335, + "timestamp": 0.7543353242520663 + }, + { + "x": 2.3575317192504395, + "y": 4.096292420100353, + "heading": 4.5695624647021355e-25, + "angularVelocity": -2.0004263459154622e-26, + "velocityX": 2.1726723160140353, + "velocityY": 0.03746039206044517, + "timestamp": 0.8302228706673729 + }, + { + "x": 2.4972894766259857, + "y": 4.098702070348201, + "heading": 3.822235688125773e-25, + "angularVelocity": -1.9197661326309743e-26, + "velocityX": 1.8416428515253338, + "velocityY": 0.0317529075806614, + "timestamp": 0.9061104170826795 + }, + { + "x": 2.611925746748375, + "y": 4.100678585440459, + "heading": 3.063572284762821e-25, + "angularVelocity": -2.4686338901390002e-26, + "velocityX": 1.5106071488334032, + "velocityY": 0.026045315544143462, + "timestamp": 0.9819979634979861 + }, + { + "x": 2.701440519692856, + "y": 4.102221965206007, + "heading": 2.3114445870748474e-25, + "angularVelocity": -1.5210807516831905e-26, + "velocityX": 1.1795713153591425, + "velocityY": 0.020337721252726194, + "timestamp": 1.0578855099132927 + }, + { + "x": 2.7658337920580536, + "y": 4.1033322095862, + "heading": 1.561656112047754e-25, + "angularVelocity": -1.9040979941775662e-26, + "velocityX": 0.8485354370636105, + "velocityY": 0.01463012618851756, + "timestamp": 1.1337730563286001 + }, + { + "x": 2.805105562125262, + "y": 4.104009318551404, + "heading": 8.077058611592956e-26, + "angularVelocity": -3.263981497829794e-26, + "velocityX": 0.5174995361200375, + "velocityY": 0.008922530733819996, + "timestamp": 1.2096606027439076 + }, + { + "x": 2.819255828857422, + "y": 4.10425329208374, + "heading": 3.4975515409752656e-27, + "angularVelocity": -3.9242842268499737e-26, + "velocityX": 0.18646362151070262, + "velocityY": 0.0032149350435025517, + "timestamp": 1.2855481491592151 + }, + { + "x": 2.810470941680652, + "y": 4.100290874791341, + "heading": -0.013665728423960572, + "angularVelocity": -0.18841669161843644, + "velocityX": -0.12112192828199068, + "velocityY": -0.05463196207942443, + "timestamp": 1.3580774368065907 + }, + { + "x": 2.779376565530904, + "y": 4.092132450200435, + "heading": -0.040981035402949714, + "angularVelocity": -0.3766107163742046, + "velocityX": -0.4287147600418107, + "velocityY": -0.11248455424752818, + "timestamp": 1.4306067244539662 + }, + { + "x": 2.7259718800290944, + "y": 4.079777209778572, + "heading": -0.08192049181497908, + "angularVelocity": -0.5644541362527821, + "velocityX": -0.7363189028058957, + "velocityY": -0.17034829408406268, + "timestamp": 1.5031360121013417 + }, + { + "x": 2.650255783412156, + "y": 4.06322370149856, + "heading": -0.13644943849416394, + "angularVelocity": -0.7518196917125987, + "velocityX": -1.0439382361654468, + "velocityY": -0.22823205379447506, + "timestamp": 1.5756652997487173 + }, + { + "x": 2.5522270104067304, + "y": 4.04246950295157, + "heading": -0.20452759410435262, + "angularVelocity": -0.9386298668914527, + "velocityX": -1.3515750145241086, + "velocityY": -0.2861492125483705, + "timestamp": 1.6481945873960928 + }, + { + "x": 2.4318844389859766, + "y": 4.01751088765649, + "heading": -0.2861194089559698, + "angularVelocity": -1.1249498995261227, + "velocityX": -1.6592272628656817, + "velocityY": -0.34411775028624336, + "timestamp": 1.7207238750434684 + }, + { + "x": 2.2892276526666033, + "y": 3.9883426531177015, + "heading": -0.3812142224775482, + "angularVelocity": -1.311122949172083, + "velocityX": -1.9668852534846832, + "velocityY": -0.40215801760798864, + "timestamp": 1.793253162690844 + }, + { + "x": 2.1330829846519728, + "y": 3.9587634030266514, + "heading": -0.4780632841326763, + "angularVelocity": -1.3353097044877917, + "velocityX": -2.1528498773319837, + "velocityY": -0.40782490839919366, + "timestamp": 1.8657824503382194 + }, + { + "x": 1.9992497997330843, + "y": 3.9334042162180256, + "heading": -0.5612966415288853, + "angularVelocity": -1.1475827227322806, + "velocityX": -1.8452295515373094, + "velocityY": -0.349640643541367, + "timestamp": 1.938311737985595 + }, + { + "x": 1.8877256900718635, + "y": 3.9122685100016006, + "heading": -0.6308164435957174, + "angularVelocity": -0.9585066160421192, + "velocityX": -1.537642423891296, + "velocityY": -0.29140926240973475, + "timestamp": 2.0108410256329705 + }, + { + "x": 1.7985083705984093, + "y": 3.895358481595443, + "heading": -0.6865270710591681, + "angularVelocity": -0.768112155386183, + "velocityX": -1.2300868017236275, + "velocityY": -0.23314758705988012, + "timestamp": 2.083370313280346 + }, + { + "x": 1.731596065924516, + "y": 3.88267537699245, + "heading": -0.7283506709725182, + "angularVelocity": -0.5766442946012179, + "velocityX": -0.9225556577807338, + "velocityY": -0.17486873254092689, + "timestamp": 2.1558996009277216 + }, + { + "x": 1.6869877424298634, + "y": 3.8742198088507074, + "heading": -0.7562373778990242, + "angularVelocity": -0.3844889124250871, + "velocityX": -0.6150387649128701, + "velocityY": -0.11658143097795469, + "timestamp": 2.228428888575097 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.19210952061606446, + "velocityX": -0.3075243171780863, + "velocityY": -0.05829081246861152, + "timestamp": 2.3009581762224727 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -5.999453085593573e-20, + "velocityX": -1.6973677435282043e-18, + "velocityY": -2.503873223513922e-18, + "timestamp": 2.373487463869848 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint.4.traj b/src/main/deploy/choreo/SourceLanePHGSprint.4.traj index b7ee408d..0cd0adae 100644 --- a/src/main/deploy/choreo/SourceLanePHGSprint.4.traj +++ b/src/main/deploy/choreo/SourceLanePHGSprint.4.traj @@ -1,265 +1,391 @@ { "samples": [ { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": 0, - "velocityX": 8.242872749735147e-35, - "velocityY": 1.4597202684890146e-35, + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -5.999453085593573e-20, + "velocityX": -1.6973677435282043e-18, + "velocityY": -2.503873223513922e-18, "timestamp": 0 }, { - "x": 3.2500504466823896, - "y": 2.378701613137869, - "heading": -0.7519553497418766, - "angularVelocity": 0.05745486103192854, - "velocityX": 0.3168299239145798, - "velocityY": -0.09654503294396014, - "timestamp": 0.08491474199846039 - }, - { - "x": 3.3038574781039927, - "y": 2.3623053962285474, - "heading": -0.7421975985160749, - "angularVelocity": 0.1149123343739134, - "velocityX": 0.6336594819139709, - "velocityY": -0.19309034595687619, - "timestamp": 0.16982948399692077 - }, - { - "x": 3.3845679847892804, - "y": 2.337711013412665, - "heading": -0.7275626808140259, - "angularVelocity": 0.17234837388205654, - "velocityX": 0.9504887465447538, - "velocityY": -0.2896361955186583, - "timestamp": 0.25474422599538116 - }, - { - "x": 3.4921819468417152, - "y": 2.3049183962425888, - "heading": -0.7080543108308168, - "angularVelocity": 0.22974067310435634, - "velocityX": 1.2673177768636017, - "velocityY": -0.3861828511552315, - "timestamp": 0.33965896799384154 - }, - { - "x": 3.626699347800246, - "y": 2.2639274521002806, - "heading": -0.6836778860236516, - "angularVelocity": 0.28706940907395306, - "velocityX": 1.5841466133285746, - "velocityY": -0.48273059750981107, - "timestamp": 0.42457370999230193 - }, - { - "x": 3.7881201726436093, - "y": 2.214738062849279, - "heading": -0.6544402090024873, - "angularVelocity": 0.3443180339839501, - "velocityX": 1.9009752728953775, - "velocityY": -0.5792797351005696, - "timestamp": 0.5094884519907623 - }, - { - "x": 3.976444405401589, - "y": 2.157350083559159, - "heading": -0.6203491385118067, - "angularVelocity": 0.401474110246943, - "velocityX": 2.217803744388632, - "velocityY": -0.6758305794671143, - "timestamp": 0.5944031939892227 - }, - { - "x": 4.191672026395985, - "y": 2.091763341463472, - "heading": -0.581413167044626, - "angularVelocity": 0.45853017451182726, - "velocityX": 2.5346319841411913, - "velocityY": -0.7723834584208757, - "timestamp": 0.6793179359876831 - }, - { - "x": 4.433803009109946, - "y": 2.017977635340623, - "heading": -0.5376409250533941, - "angularVelocity": 0.5154846020968357, - "velocityX": 2.851459911617613, - "velocityY": -0.868938707064401, - "timestamp": 0.7642326779861435 - }, - { - "x": 4.702837316573546, - "y": 1.9359927355638575, - "heading": -0.48904061693477097, - "angularVelocity": 0.5723424104557058, - "velocityX": 3.168287403716993, - "velocityY": -0.9654966599115613, - "timestamp": 0.8491474199846039 - }, - { - "x": 4.998774896616477, - "y": 1.8458083852834486, - "heading": -0.4356194112597129, - "angularVelocity": 0.6291157980086282, - "velocityX": 3.4851142814318106, - "velocityY": -1.0620576375541941, - "timestamp": 0.9340621619830642 - }, - { - "x": 5.321615670956704, - "y": 1.7474243048973186, - "heading": -0.37738292770821397, - "angularVelocity": 0.6858230052981278, - "velocityX": 3.8019402372568085, - "velocityY": -1.1586219079357767, - "timestamp": 1.0189769039815246 - }, - { - "x": 5.664898737663787, - "y": 1.6427906762580982, - "heading": -0.37738292062184065, - "angularVelocity": 8.345280395968993e-8, - "velocityX": 4.0426792642590454, - "velocityY": -1.2322198263420197, - "timestamp": 1.103891645979985 - }, - { - "x": 6.008181814361773, - "y": 1.5381570803955327, - "heading": -0.37738291353643977, - "angularVelocity": 8.344135212048987e-8, - "velocityX": 4.042679381917106, - "velocityY": -1.232219440347153, - "timestamp": 1.1888063879784454 - }, - { - "x": 6.351464887209067, - "y": 1.4335234719011998, - "heading": -0.3773829064500845, - "angularVelocity": 8.345259143210343e-8, - "velocityX": 4.042679336569362, - "velocityY": -1.2322195891053875, - "timestamp": 1.2737211299769058 - }, - { - "x": 6.674297686591828, - "y": 1.3351103504378636, - "heading": -0.3193930007642967, - "angularVelocity": 0.6829191765882003, - "velocityX": 3.8018463200255006, - "velocityY": -1.1589639107084677, - "timestamp": 1.3586358719753662 - }, - { - "x": 6.970227447718613, - "y": 1.244897503452253, - "heading": -0.2662161162246808, - "angularVelocity": 0.6262385457236648, - "velocityX": 3.4850222018239303, - "velocityY": -1.0623932295201048, - "timestamp": 1.4435506139738266 - }, - { - "x": 7.239254236839848, - "y": 1.1628851677059724, - "heading": -0.21785490809513916, - "angularVelocity": 0.5695266450956005, - "velocityX": 3.1681988638217, - "velocityY": -0.9658197601044064, - "timestamp": 1.528465355972287 - }, - { - "x": 7.48137812927984, - "y": 1.0890735490452839, - "heading": -0.1743132115865013, - "angularVelocity": 0.512769579037611, - "velocityX": 2.8513764128775443, - "velocityY": -0.8692438665364745, - "timestamp": 1.6133800979707473 - }, - { - "x": 7.696599198563337, - "y": 1.0234628255753193, - "heading": -0.13559564838663762, - "angularVelocity": 0.45595808558855033, - "velocityX": 2.53455482779892, - "velocityY": -0.7726658755102153, - "timestamp": 1.6982948399692077 - }, - { - "x": 7.884917512273281, - "y": 0.9660531491893245, - "heading": -0.10170713796449406, - "angularVelocity": 0.3990886579241798, - "velocityX": 2.217734038612032, - "velocityY": -0.6760860957103977, - "timestamp": 1.783209581967668 - }, - { - "x": 8.046333129383763, - "y": 0.9168446470274297, - "heading": -0.07265245047866593, - "angularVelocity": 0.3421630544005527, - "velocityX": 1.9009139439346003, - "velocityY": -0.5795048186425281, - "timestamp": 1.8681243239661285 - }, - { - "x": 8.18084609829272, - "y": 0.8758374229594964, - "heading": -0.048435821985695085, - "angularVelocity": 0.28518756488019376, - "velocityX": 1.5840944192163566, - "velocityY": -0.4829223183493492, - "timestamp": 1.9530390659645889 - }, - { - "x": 8.288456455335494, - "y": 0.8430315590188601, - "heading": -0.029060636546388994, - "angularVelocity": 0.2281722229063284, - "velocityX": 1.2672753224019029, - "velocityY": -0.38633885198910756, - "timestamp": 2.0379538079630493 - }, - { - "x": 8.369164223685814, - "y": 0.8184271166917905, - "heading": -0.014529175658252375, - "angularVelocity": 0.17113001283569912, - "velocityX": 0.9504564984933311, - "velocityY": -0.289754661534691, - "timestamp": 2.1228685499615096 - }, - { - "x": 8.422969412591065, - "y": 0.8020241379761622, - "heading": -0.004842433994812591, - "angularVelocity": 0.11407608897422551, - "velocityX": 0.6336377834867251, - "velocityY": -0.19316997649154619, - "timestamp": 2.20778329195997 - }, - { - "x": 8.449872016906738, - "y": 0.7938226461410522, - "heading": 5.797004207427939e-34, - "angularVelocity": 0.05702701181027418, - "velocityX": 0.3168190078957048, - "velocityY": -0.09658501741969248, - "timestamp": 2.2926980339584304 - }, - { - "x": 8.449872016906738, - "y": 0.7938226461410522, - "heading": -2.2932518796347515e-34, - "angularVelocity": -9.527505988231194e-33, - "velocityX": -1.2891294779020923e-34, - "velocityY": 4.6042246741965494e-35, - "timestamp": 2.377612775956891 + "x": 1.6770247995246623, + "y": 3.8595873316999896, + "heading": -0.7649878221642326, + "angularVelocity": 0.08507451391469989, + "velocityX": 0.20257164680788678, + "velocityY": -0.1707799926111949, + "timestamp": 0.06092450226104873 + }, + { + "x": 1.70170978110981, + "y": 3.8387765844705157, + "heading": -0.7547381408402699, + "angularVelocity": 0.1682357827076709, + "velocityX": 0.405173299231529, + "velocityY": -0.3415825563958536, + "timestamp": 0.12184900452209746 + }, + { + "x": 1.7387402457793653, + "y": 3.80755819428449, + "heading": -0.7395583157876386, + "angularVelocity": 0.24915796583103744, + "velocityX": 0.607809063599532, + "velocityY": -0.5124110830197925, + "timestamp": 0.1827735067831462 + }, + { + "x": 1.7881185850906471, + "y": 3.7659303409499723, + "heading": -0.7196099497583256, + "angularVelocity": 0.3274276405876679, + "velocityX": 0.8104840824091846, + "velocityY": -0.6832694858326719, + "timestamp": 0.24369800904419492 + }, + { + "x": 1.8498475856412335, + "y": 3.713890927230462, + "heading": -0.6950873259687543, + "angularVelocity": 0.4025083977624826, + "velocityX": 1.0132048397554583, + "velocityY": -0.8541623121766772, + "timestamp": 0.30462251130524365 + }, + { + "x": 1.9239305360498986, + "y": 3.6514375329357844, + "heading": -0.6662283152580175, + "angularVelocity": 0.47368480069122665, + "velocityX": 1.2159795756924812, + "velocityY": -1.025094862935087, + "timestamp": 0.3655470135662924 + }, + { + "x": 2.010371367313072, + "y": 3.5785673636651287, + "heading": -0.6333309419257536, + "angularVelocity": 0.539969505065564, + "velocityX": 1.4188188340512586, + "velocityY": -1.1960732803104883, + "timestamp": 0.4264715158273411 + }, + { + "x": 2.1091748328759063, + "y": 3.4952772039974476, + "heading": -0.5967798759520212, + "angularVelocity": 0.5999403297070707, + "velocityX": 1.6217361143055882, + "velocityY": -1.3671044748269063, + "timestamp": 0.48739601808838984 + }, + { + "x": 2.220346716936752, + "y": 3.40156341112366, + "heading": -0.5570916262340567, + "angularVelocity": 0.6514333026129457, + "velocityX": 1.82474833498841, + "velocityY": -1.5381954615278515, + "timestamp": 0.5483205203494386 + }, + { + "x": 2.3438939700884585, + "y": 3.297422079154605, + "heading": -0.5149984011781104, + "angularVelocity": 0.6909079843703304, + "velocityX": 2.027874641016087, + "velocityY": -1.7093505585459128, + "timestamp": 0.6092450226104873 + }, + { + "x": 2.4798242349395756, + "y": 3.182849894813871, + "heading": -0.4716228537300689, + "angularVelocity": 0.7119557130263738, + "velocityX": 2.2311263909664074, + "velocityY": -1.8805600388792227, + "timestamp": 0.670169524871536 + }, + { + "x": 2.628141721858615, + "y": 3.057848265310819, + "heading": -0.42891150330345956, + "angularVelocity": 0.7010537442489162, + "velocityX": 2.4344472488840574, + "velocityY": -2.0517464216194523, + "timestamp": 0.7310940271325848 + }, + { + "x": 2.788815804109156, + "y": 2.922450520332472, + "heading": -0.3910899170501554, + "angularVelocity": 0.6207943413513182, + "velocityX": 2.6372654069800667, + "velocityY": -2.2223857389625743, + "timestamp": 0.7920185293936335 + }, + { + "x": 2.9613127738384817, + "y": 2.777334038124512, + "heading": -0.375202514467059, + "angularVelocity": 0.2607719717597751, + "velocityX": 2.831323413857593, + "velocityY": -2.3819067341111335, + "timestamp": 0.8529430316546822 + }, + { + "x": 3.137446263833113, + "y": 2.628782733735228, + "heading": -0.3752024981016849, + "angularVelocity": 2.6861728103084975e-7, + "velocityX": 2.891012375282727, + "velocityY": -2.438285072117188, + "timestamp": 0.913867533915731 + }, + { + "x": 3.313579787334416, + "y": 2.480231469074176, + "heading": -0.37520248173599324, + "angularVelocity": 2.686224924493493e-7, + "velocityX": 2.8910129252531243, + "velocityY": -2.4382844200276295, + "timestamp": 0.9747920361767797 + }, + { + "x": 3.489713311593346, + "y": 2.3316802053114225, + "heading": -0.37520246537030155, + "angularVelocity": 2.6862249292188543e-7, + "velocityX": 2.8910129376886133, + "velocityY": -2.4382844052831705, + "timestamp": 1.0357165384378284 + }, + { + "x": 3.6658468653792484, + "y": 2.183128976558075, + "heading": -0.37520244900461003, + "angularVelocity": 2.6862249035440315e-7, + "velocityX": 2.89101342233718, + "velocityY": -2.4382838306472756, + "timestamp": 1.0966410406988771 + }, + { + "x": 3.8419803666744587, + "y": 2.034577685567575, + "heading": -0.37520243263891834, + "angularVelocity": 2.686224937536833e-7, + "velocityX": 2.8910125607676815, + "velocityY": -2.4382848521928207, + "timestamp": 1.1575655429599259 + }, + { + "x": 4.017984969789346, + "y": 1.8858736976718657, + "heading": -0.37520241627306933, + "angularVelocity": 2.6862507525069377e-7, + "velocityX": 2.8888968573061926, + "velocityY": -2.440791182151078, + "timestamp": 1.2184900452209746 + }, + { + "x": 4.198449630537114, + "y": 1.742615181247194, + "heading": -0.37520239987870785, + "angularVelocity": 2.6909307205451447e-7, + "velocityX": 2.9621031612948867, + "velocityY": -2.3514105344815093, + "timestamp": 1.2794145474820233 + }, + { + "x": 4.38853011399539, + "y": 1.6123849775845955, + "heading": -0.3752023832167796, + "angularVelocity": 2.734848475989746e-7, + "velocityX": 3.1199349424936167, + "velocityY": -2.137566969436863, + "timestamp": 1.340339049743072 + }, + { + "x": 4.587288186636938, + "y": 1.495827162172114, + "heading": -0.3752023659192684, + "angularVelocity": 2.8391715306138947e-7, + "velocityX": 3.2623667861891117, + "velocityY": -1.9131517055823732, + "timestamp": 1.4012635520041208 + }, + { + "x": 4.7937421798706055, + "y": 1.3935176134109497, + "heading": -0.3752023473605566, + "angularVelocity": 3.046181942517293e-7, + "velocityX": 3.3886857597794955, + "velocityY": -1.6792841133570497, + "timestamp": 1.4621880542651695 + }, + { + "x": 5.0647852667187605, + "y": 1.2879733857720441, + "heading": -0.3752023309051681, + "angularVelocity": 2.139583629256744e-7, + "velocityX": 3.524191190248399, + "velocityY": -1.3723206946612971, + "timestamp": 1.539097356915418 + }, + { + "x": 5.34411641028699, + "y": 1.2068681953777092, + "heading": -0.37520231550400196, + "angularVelocity": 2.0025101753440068e-7, + "velocityX": 3.6319552244350577, + "velocityY": -1.0545563098285724, + "timestamp": 1.6160066595656666 + }, + { + "x": 5.629536746845857, + "y": 1.1508398853670567, + "heading": -0.375202300582326, + "angularVelocity": 1.940165285663064e-7, + "velocityX": 3.711128910592765, + "velocityY": -0.7284984791169633, + "timestamp": 1.6929159622159151 + }, + { + "x": 5.917306487549604, + "y": 1.108501764833761, + "heading": -0.37520228573814113, + "angularVelocity": 1.9300896478374464e-7, + "velocityX": 3.741676634521091, + "velocityY": -0.5504941414672958, + "timestamp": 1.7698252648661636 + }, + { + "x": 6.2050577552774975, + "y": 1.0660382742134262, + "heading": -0.37520227089383906, + "angularVelocity": 1.9301048840600142e-7, + "velocityX": 3.7414364428249387, + "velocityY": -0.5521242444940722, + "timestamp": 1.8467345675164122 + }, + { + "x": 6.492809022919495, + "y": 1.0235747830116217, + "heading": -0.3752022560493111, + "angularVelocity": 1.9301342582848665e-7, + "velocityX": 3.7414364417080774, + "velocityY": -0.5521242520545259, + "timestamp": 1.9236438701666607 + }, + { + "x": 6.772658036083171, + "y": 0.9819575118333774, + "heading": -0.34826612342726787, + "angularVelocity": 0.3502324386496837, + "velocityX": 3.638688734915609, + "velocityY": -0.541121421520385, + "timestamp": 2.0005531728169093 + }, + { + "x": 7.027520568845826, + "y": 0.9441505058911935, + "heading": -0.3001058813779335, + "angularVelocity": 0.6261952766409414, + "velocityX": 3.313806314454086, + "velocityY": -0.49157910212908845, + "timestamp": 2.077462475467158 + }, + { + "x": 7.256873768560855, + "y": 0.9101570740557017, + "heading": -0.24860478708038097, + "angularVelocity": 0.6696341342705694, + "velocityX": 2.9821255922450605, + "velocityY": -0.44199375971564203, + "timestamp": 2.1543717781174063 + }, + { + "x": 7.460704020800595, + "y": 0.8799617502558256, + "heading": -0.1984971865955797, + "angularVelocity": 0.6515154702763284, + "velocityX": 2.65026785077839, + "velocityY": -0.3926095122353607, + "timestamp": 2.231281080767655 + }, + { + "x": 7.6390235862814775, + "y": 0.8535543484261158, + "heading": -0.15201891065632164, + "angularVelocity": 0.6043258011403606, + "velocityX": 2.318569527171569, + "velocityY": -0.3433577073218239, + "timestamp": 2.3081903834179034 + }, + { + "x": 7.7918452312093995, + "y": 0.8309283282540864, + "heading": -0.11047967273973482, + "angularVelocity": 0.5401068074364143, + "velocityX": 1.9870371939645617, + "velocityY": -0.29419094169820903, + "timestamp": 2.385099686068152 + }, + { + "x": 7.9191796079863, + "y": 0.812079256076005, + "heading": -0.07474204501348639, + "angularVelocity": 0.46467236725273836, + "velocityX": 1.6556433667844261, + "velocityY": -0.2450818240258775, + "timestamp": 2.4620089887184005 + }, + { + "x": 8.021035341477521, + "y": 0.7970039548993032, + "heading": -0.04541767666824434, + "angularVelocity": 0.3812850635065168, + "velocityX": 1.3243616829347478, + "velocityY": -0.1960140146538302, + "timestamp": 2.538918291368649 + }, + { + "x": 8.097419437322872, + "y": 0.7857000289910334, + "heading": -0.022962750583734995, + "angularVelocity": 0.29196632020738633, + "velocityX": 0.9931710887135993, + "velocityY": -0.14697735538800996, + "timestamp": 2.6158275940188975 + }, + { + "x": 8.148337646767295, + "y": 0.7781655823556067, + "heading": -0.0077300757248004864, + "angularVelocity": 0.19806023893112515, + "velocityX": 0.662055274067148, + "velocityY": -0.09796534847923602, + "timestamp": 2.692736896669146 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": 1.1088837243155707e-19, + "angularVelocity": 0.10050898211824409, + "velocityX": 0.3310015661876223, + "velocityY": -0.04897379245809494, + "timestamp": 2.7696461993193946 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": 5.633915553103528e-20, + "angularVelocity": 2.8365160942051004e-20, + "velocityX": -1.2324505157130932e-18, + "velocityY": 3.669165812220002e-18, + "timestamp": 2.846555501969643 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint.traj b/src/main/deploy/choreo/SourceLanePHGSprint.traj deleted file mode 100644 index adbaebe3..00000000 --- a/src/main/deploy/choreo/SourceLanePHGSprint.traj +++ /dev/null @@ -1,1570 +0,0 @@ -{ - "samples": [ - { - "x": 1.468971848487854, - "y": 1.4024139642715454, - "heading": 2.7984066036335196e-36, - "angularVelocity": 0, - "velocityX": -5.8999770086815376e-36, - "velocityY": -1.737717144475295e-35, - "timestamp": 0 - }, - { - "x": 1.4884655154496769, - "y": 1.4133531216091884, - "heading": -0.008129236612255402, - "angularVelocity": -0.10721835741229961, - "velocityX": 0.2571064235525047, - "velocityY": 0.14427904330507066, - "timestamp": 0.07581944741976482 - }, - { - "x": 1.527452868051014, - "y": 1.4352316834976648, - "heading": -0.024386425737641767, - "angularVelocity": -0.2144197785481142, - "velocityX": 0.514213093449344, - "velocityY": 0.2885613471613503, - "timestamp": 0.15163889483952964 - }, - { - "x": 1.5859338286155955, - "y": 1.4680499620319052, - "heading": -0.04877538927616455, - "angularVelocity": -0.32167160759556, - "velocityX": 0.7713187388561265, - "velocityY": 0.4328477673089076, - "timestamp": 0.22745834225929445 - }, - { - "x": 1.6639082150870297, - "y": 1.5118083204789063, - "heading": -0.08130602266355597, - "angularVelocity": -0.4290539498037971, - "velocityX": 1.028421983079603, - "velocityY": 0.5771389786678144, - "timestamp": 0.3032777896790593 - }, - { - "x": 1.7613757308728617, - "y": 1.5665071508132398, - "heading": -0.12199563276545015, - "angularVelocity": -0.5366645562136756, - "velocityX": 1.2855213154774845, - "velocityY": 0.7214353598688232, - "timestamp": 0.3790972370988241 - }, - { - "x": 1.8783359544327143, - "y": 1.6321468388152505, - "heading": -0.17087061798873185, - "angularVelocity": -0.6446233372381559, - "velocityX": 1.5426150880831033, - "velocityY": 0.8657368292148704, - "timestamp": 0.4549166845185889 - }, - { - "x": 2.014788331993746, - "y": 1.708727712438889, - "heading": -0.22796838283483978, - "angularVelocity": -0.7530754547707706, - "velocityX": 1.7997015568523023, - "velocityY": 1.0100426240204334, - "timestamp": 0.5307361319383537 - }, - { - "x": 2.1707321785420235, - "y": 1.7962499692148692, - "heading": -0.2933392420298012, - "angularVelocity": -0.8621911847107495, - "velocityX": 2.056778990816348, - "velocityY": 1.1543510241036752, - "timestamp": 0.6065555793581185 - }, - { - "x": 2.346166691242211, - "y": 1.8947135784311677, - "heading": -0.3670478679076374, - "angularVelocity": -0.9721598928274647, - "velocityX": 2.3138458360018936, - "velocityY": 1.2986590191190295, - "timestamp": 0.6823750267778834 - }, - { - "x": 2.5215633369919264, - "y": 1.9931565040407253, - "heading": -0.44471119009190996, - "angularVelocity": -1.0243192851868113, - "velocityX": 2.3133464001478927, - "velocityY": 1.2983862183080896, - "timestamp": 0.7581944741976483 - }, - { - "x": 2.6774707734274146, - "y": 2.080658978979359, - "heading": -0.5138920707628942, - "angularVelocity": -0.9124424277055597, - "velocityX": 2.056298769527115, - "velocityY": 1.1540901169351336, - "timestamp": 0.8340139216174132 - }, - { - "x": 2.8138894851599456, - "y": 2.157221711134291, - "heading": -0.5745281398733623, - "angularVelocity": -0.7997429574336522, - "velocityX": 1.799257530555003, - "velocityY": 1.009803352048346, - "timestamp": 0.909833369037178 - }, - { - "x": 2.9308197777643024, - "y": 2.222845447108769, - "heading": -0.6265685355305138, - "angularVelocity": -0.6863726580469048, - "velocityX": 1.5422203218784643, - "velocityY": 0.8655264342822331, - "timestamp": 0.9856528164569429 - }, - { - "x": 3.028261803981169, - "y": 2.277530893632237, - "heading": -0.6699741130308978, - "angularVelocity": -0.5724860702304326, - "velocityX": 1.285185127733667, - "velocityY": 0.7212588377320857, - "timestamp": 1.0614722638767078 - }, - { - "x": 3.1062155998459384, - "y": 2.321278659025598, - "heading": -0.70471685674301, - "angularVelocity": -0.4582299778546705, - "velocityX": 1.028150408867906, - "velocityY": 0.576999264465178, - "timestamp": 1.1372917112964727 - }, - { - "x": 3.164681120107137, - "y": 2.3540892120142103, - "heading": -0.7307788978147941, - "angularVelocity": -0.3437382091100568, - "velocityX": 0.7711150931701107, - "velocityY": 0.4327458733240421, - "timestamp": 1.2131111587162375 - }, - { - "x": 3.2036582678448955, - "y": 2.375962855138776, - "heading": -0.7481514784912885, - "angularVelocity": -0.22913093233604348, - "velocityX": 0.5140784991740481, - "velocityY": 0.2884964724612806, - "timestamp": 1.2889306061360024 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": -0.11451726762925943, - "velocityX": 0.25704022192353, - "velocityY": 0.14424867148149015, - "timestamp": 1.3647500535557673 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": -9.65574327504051e-30, - "velocityX": -6.322792420178433e-32, - "velocityY": 5.37688279123967e-31, - "timestamp": 1.4405695009755322 - }, - { - "x": 3.2476092863283554, - "y": 2.3785223403683062, - "heading": -0.7517504460683161, - "angularVelocity": 0.062429840741884866, - "velocityX": 0.3004094300502235, - "velocityY": -0.10287803899994612, - "timestamp": 1.5219996041799313 - }, - { - "x": 3.2965349345854977, - "y": 2.361766694915971, - "heading": -0.7416811367035665, - "angularVelocity": 0.12365585905593313, - "velocityX": 0.6008299920034862, - "velocityY": -0.20576721375725762, - "timestamp": 1.6034297073843304 - }, - { - "x": 3.369924920019678, - "y": 2.336631694642855, - "heading": -0.7267441449116069, - "angularVelocity": 0.18343329068945555, - "velocityX": 0.9012635689526604, - "velocityY": -0.30866963547896215, - "timestamp": 1.6848598105887296 - }, - { - "x": 3.467780489895946, - "y": 2.3031160513517954, - "heading": -0.7070825285982575, - "angularVelocity": 0.24145390389600055, - "velocityX": 1.2017124629037867, - "velocityY": -0.41158787686822623, - "timestamp": 1.7662899137931287 - }, - { - "x": 3.59010312419892, - "y": 2.2612182145129647, - "heading": -0.682871988862579, - "angularVelocity": 0.2973168248960161, - "velocityX": 1.5021795317627178, - "velocityY": -0.5145251594936876, - "timestamp": 1.8477200169975279 - }, - { - "x": 3.7368945961709503, - "y": 2.2109362946882256, - "heading": -0.6543323433324523, - "angularVelocity": 0.35048028194805075, - "velocityX": 1.8026683768724503, - "velocityY": -0.6174856453089018, - "timestamp": 1.929150120201927 - }, - { - "x": 3.9081570523847278, - "y": 2.1522679483317453, - "heading": -0.621745834041024, - "angularVelocity": 0.4001776739695827, - "velocityX": 2.103183582905316, - "velocityY": -0.7204749109701621, - "timestamp": 2.010580223406326 - }, - { - "x": 4.10389311229091, - "y": 2.085210196605494, - "heading": -0.5854881361653628, - "angularVelocity": 0.44526159797025444, - "velocityX": 2.4037309570253367, - "velocityY": -0.8235007581646905, - "timestamp": 2.092010326610725 - }, - { - "x": 4.324105961894927, - "y": 2.009759125465929, - "heading": -0.5460851529882595, - "angularVelocity": 0.4838871820929687, - "velocityX": 2.704317456791832, - "velocityY": -0.9265746716564273, - "timestamp": 2.1734404298151238 - }, - { - "x": 4.568799288247487, - "y": 1.9259093624648695, - "heading": -0.5043289871971579, - "angularVelocity": 0.5127853723370845, - "velocityX": 3.0049492352766722, - "velocityY": -1.0297145613408714, - "timestamp": 2.2548705330195227 - }, - { - "x": 4.837976169390964, - "y": 1.8336531477869393, - "heading": -0.4615566059930767, - "angularVelocity": 0.5252649759847486, - "velocityX": 3.3056188135708484, - "velocityY": -1.1329497452110122, - "timestamp": 2.3363006362239216 - }, - { - "x": 5.131630158633593, - "y": 1.7329792749804975, - "heading": -0.42053192360161584, - "angularVelocity": 0.5038024118487874, - "velocityX": 3.606209223455392, - "velocityY": -1.2363225495826649, - "timestamp": 2.4177307394283205 - }, - { - "x": 5.449610743156184, - "y": 1.6238957636954845, - "heading": -0.39063702231560526, - "angularVelocity": 0.36712346060735523, - "velocityX": 3.9049512650674507, - "velocityY": -1.3395968688778515, - "timestamp": 2.4991608426327194 - }, - { - "x": 5.775207914535934, - "y": 1.5124309100983953, - "heading": -0.39063698838615835, - "angularVelocity": 4.1666957830653565e-7, - "velocityX": 3.998486537128211, - "velocityY": -1.3688408734703337, - "timestamp": 2.5805909458371183 - }, - { - "x": 6.0786617783907655, - "y": 1.4085746056244475, - "heading": -0.3427587122754328, - "angularVelocity": 0.5879677690019331, - "velocityX": 3.7265562969154904, - "velocityY": -1.2754043085670228, - "timestamp": 2.6620210490415173 - }, - { - "x": 6.3576257280922785, - "y": 1.3131018062400224, - "heading": -0.2933999650299559, - "angularVelocity": 0.6061486514582699, - "velocityX": 3.4258086226574016, - "velocityY": -1.1724509195914545, - "timestamp": 2.743451152245916 - }, - { - "x": 6.612101112923588, - "y": 1.226012176425091, - "heading": -0.2458719495426286, - "angularVelocity": 0.583666403664149, - "velocityX": 3.1250775182311497, - "velocityY": -1.069501650959794, - "timestamp": 2.824881255450315 - }, - { - "x": 6.84209496743372, - "y": 1.1473035772965767, - "heading": -0.20141693465093127, - "angularVelocity": 0.5459285097563893, - "velocityX": 2.8244328013783875, - "velocityY": -0.9665786488191752, - "timestamp": 2.906311358654714 - }, - { - "x": 7.047612440347946, - "y": 1.0769745409038134, - "heading": -0.1606891956044553, - "angularVelocity": 0.5001558077881458, - "velocityX": 2.523851313295684, - "velocityY": -0.8636736738037645, - "timestamp": 2.987741461859113 - }, - { - "x": 7.228657225078741, - "y": 1.0150240865596738, - "heading": -0.12409433294508956, - "angularVelocity": 0.44940214023188485, - "velocityX": 2.223315182056817, - "velocityY": -0.7607807420879281, - "timestamp": 3.069171565063512 - }, - { - "x": 7.385232062561463, - "y": 0.961451535253283, - "heading": -0.09190931635870993, - "angularVelocity": 0.3952471545515241, - "velocityX": 1.9228127107944446, - "velocityY": -0.6578961489452781, - "timestamp": 3.1506016682679108 - }, - { - "x": 7.517339056055513, - "y": 0.9162563961012582, - "heading": -0.06433562135119408, - "angularVelocity": 0.33861795481579837, - "velocityX": 1.6223360685475974, - "velocityY": -0.5550175840816443, - "timestamp": 3.2320317714723097 - }, - { - "x": 7.624979863197351, - "y": 0.8794382979964872, - "heading": -0.04152628624846568, - "angularVelocity": 0.2801093723960078, - "velocityX": 1.321879782856784, - "velocityY": -0.45214357658804644, - "timestamp": 3.3134618746767086 - }, - { - "x": 7.708155816822711, - "y": 0.8509969471259123, - "heading": -0.02360109386059999, - "angularVelocity": 0.22012980068157206, - "velocityX": 1.0214398650162453, - "velocityY": -0.34927317725710716, - "timestamp": 3.3948919778811075 - }, - { - "x": 7.766868003937176, - "y": 0.8309320995113674, - "heading": -0.010655731264137824, - "angularVelocity": 0.1589751466231924, - "velocityX": 0.7210132961159337, - "velocityY": -0.24640577409290096, - "timestamp": 3.4763220810855064 - }, - { - "x": 7.801117319270969, - "y": 0.8192435425103285, - "heading": -0.0027676483891359333, - "angularVelocity": 0.09686937096470247, - "velocityX": 0.4205977149239743, - "velocityY": -0.1435409822789855, - "timestamp": 3.5577521842899054 - }, - { - "x": 7.810904502868652, - "y": 0.8159310817718506, - "heading": 3.324097175751255e-30, - "angularVelocity": 0.03398802506964661, - "velocityX": 0.12019122182758048, - "velocityY": -0.04067857718616754, - "timestamp": 3.6391822874943043 - }, - { - "x": 7.7950494488648365, - "y": 0.8213999709085323, - "heading": -0.0026333253300755664, - "angularVelocity": -0.031408125409593986, - "velocityX": -0.18910596379430453, - "velocityY": 0.06522838401102503, - "timestamp": 3.72302445558722 - }, - { - "x": 7.753261271011051, - "y": 0.8357490695116764, - "heading": -0.010653353631792484, - "angularVelocity": -0.09565626085465573, - "velocityX": -0.4984148049162354, - "velocityY": 0.17114417398225837, - "timestamp": 3.806866623680136 - }, - { - "x": 7.685538815396101, - "y": 0.8589792353835662, - "heading": -0.023944188886482875, - "angularVelocity": -0.15852208449523947, - "velocityX": -0.8077374089360129, - "velocityY": 0.2770701951092858, - "timestamp": 3.8907087917730516 - }, - { - "x": 7.591880716426718, - "y": 0.8910914866950913, - "heading": -0.04236515594390405, - "angularVelocity": -0.21971005135513041, - "velocityX": -1.1170763006222482, - "velocityY": 0.3830083601361243, - "timestamp": 3.9745509598659674 - }, - { - "x": 7.472285350218667, - "y": 0.9320870626307362, - "heading": -0.06574332589364143, - "angularVelocity": -0.27883546527360753, - "velocityX": -1.426434560655829, - "velocityY": 0.4889613051300446, - "timestamp": 4.058393127958883 - }, - { - "x": 7.326750771760018, - "y": 0.9819675124503678, - "heading": -0.09386215907940265, - "angularVelocity": -0.33537817336257436, - "velocityX": -1.7358160191821825, - "velocityY": 0.5949327284136178, - "timestamp": 4.142235296051799 - }, - { - "x": 7.155274630305055, - "y": 1.0407348314785088, - "heading": -0.12644338322972526, - "angularVelocity": -0.38860188007321295, - "velocityX": -2.045225515458149, - "velocityY": 0.7009279502769267, - "timestamp": 4.226077464144716 - }, - { - "x": 6.9578540596820755, - "y": 1.1083916783743404, - "heading": -0.16311628951040621, - "angularVelocity": -0.43740407857819336, - "velocityX": -2.3546691970583673, - "velocityY": 0.8069548824268558, - "timestamp": 4.309919632237632 - }, - { - "x": 6.734485559178827, - "y": 1.1849417416525785, - "heading": -0.20336148011715094, - "angularVelocity": -0.48001132988525586, - "velocityX": -2.664154632257422, - "velocityY": 0.9130258081279993, - "timestamp": 4.393761800330548 - }, - { - "x": 6.485164986270469, - "y": 1.2703903985697897, - "heading": -0.2463960185234521, - "angularVelocity": -0.5132803621992265, - "velocityX": -2.973689476064775, - "velocityY": 1.0191608692956917, - "timestamp": 4.477603968423464 - }, - { - "x": 6.2098884128498435, - "y": 1.3647459693546886, - "heading": -0.2908977372042754, - "angularVelocity": -0.5307796743937351, - "velocityX": -3.2832711710837126, - "velocityY": 1.125395167266338, - "timestamp": 4.5614461365163805 - }, - { - "x": 5.908659770542328, - "y": 1.4680218269782972, - "heading": -0.3341353968865912, - "angularVelocity": -0.5157030246927623, - "velocityX": -3.592805972928633, - "velocityY": 1.231788966968944, - "timestamp": 4.645288304609297 - }, - { - "x": 5.581609353432503, - "y": 1.5802212470917454, - "heading": -0.3669186513644775, - "angularVelocity": -0.39101153063642635, - "velocityX": -3.9007867347535576, - "velocityY": 1.3382218359275648, - "timestamp": 4.729130472702213 - }, - { - "x": 5.24637103251191, - "y": 1.69499790168712, - "heading": -0.36691872086414007, - "angularVelocity": -8.28934451080165e-7, - "velocityX": -3.9984452757599787, - "velocityY": 1.3689609561166869, - "timestamp": 4.812972640795129 - }, - { - "x": 4.935041308656135, - "y": 1.8014679656257433, - "heading": -0.4122700447901933, - "angularVelocity": -0.540913062693946, - "velocityX": -3.713283314796349, - "velocityY": 1.2698868166270474, - "timestamp": 4.896814808888045 - }, - { - "x": 4.649661722331028, - "y": 1.8990649018297885, - "heading": -0.4616985909169464, - "angularVelocity": -0.5895427951239265, - "velocityX": -3.4037715485702025, - "velocityY": 1.16405549169346, - "timestamp": 4.980656976980962 - }, - { - "x": 4.390249017075816, - "y": 1.987780874059184, - "heading": -0.5104183331347605, - "angularVelocity": -0.5810887686470702, - "velocityX": -3.0940600792637536, - "velocityY": 1.058130702572937, - "timestamp": 5.064499145073878 - }, - { - "x": 4.156796279326317, - "y": 2.0676179686564895, - "heading": -0.5565184993786366, - "angularVelocity": -0.5498446341796518, - "velocityX": -2.784431069229766, - "velocityY": 0.9522307976199715, - "timestamp": 5.148341313166794 - }, - { - "x": 3.949296604382671, - "y": 2.1385786696874747, - "heading": -0.5989640106497574, - "angularVelocity": -0.5062549339621698, - "velocityX": -2.4748844127419294, - "velocityY": 0.8463605205478963, - "timestamp": 5.23218348125971 - }, - { - "x": 3.7677445474124176, - "y": 2.200665074641089, - "heading": -0.6371029281396509, - "angularVelocity": -0.4548894471291752, - "velocityX": -2.1654026977099767, - "velocityY": 0.7405152605883109, - "timestamp": 5.316025649352627 - }, - { - "x": 3.612135883138456, - "y": 2.25387888808437, - "heading": -0.6704857144975235, - "angularVelocity": -0.3981622507767433, - "velocityX": -1.8559713782867908, - "velocityY": 0.6346903312937655, - "timestamp": 5.399867817445543 - }, - { - "x": 3.482467285243419, - "y": 2.298221496264914, - "heading": -0.6987833643307629, - "angularVelocity": -0.3375109503595359, - "velocityX": -1.5465797324245634, - "velocityY": 0.5288819360134136, - "timestamp": 5.483709985538459 - }, - { - "x": 3.3787360861374416, - "y": 2.3336940331656435, - "heading": -0.7217450303198044, - "angularVelocity": -0.2738677506955549, - "velocityX": -1.2372199033667683, - "velocityY": 0.42308706594297585, - "timestamp": 5.567552153631375 - }, - { - "x": 3.3009401097656865, - "y": 2.3602974295620016, - "heading": -0.739173949966493, - "angularVelocity": -0.2078777307783457, - "velocityX": -0.927886028490337, - "velocityY": 0.3173032973917799, - "timestamp": 5.6513943217242915 - }, - { - "x": 3.249077555413445, - "y": 2.378032448311797, - "heading": -0.7509127758202662, - "angularVelocity": -0.14001100067887515, - "velocityX": -0.6185736310488396, - "velocityY": 0.2115286275772482, - "timestamp": 5.735236489817208 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": -0.07062482706951323, - "velocityX": -0.3092792155483943, - "velocityY": 0.10576135602688908, - "timestamp": 5.819078657910124 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": 0, - "velocityX": 2.4942198398372333e-31, - "velocityY": -2.7810761994582423e-31, - "timestamp": 5.90292082600304 - }, - { - "x": 3.2340252732371284, - "y": 2.3791227322097765, - "heading": -0.751823485237952, - "angularVelocity": 0.08542971221571066, - "velocityX": 0.1854727097784258, - "velocityY": -0.13259511367658153, - "timestamp": 5.96157289300704 - }, - { - "x": 3.255906850263932, - "y": 2.3637444378026573, - "heading": -0.7418521789042297, - "angularVelocity": 0.1700077566412493, - "velocityX": 0.37307426906730357, - "velocityY": -0.2621952676632906, - "timestamp": 6.02022496001104 - }, - { - "x": 3.2889308491117024, - "y": 2.3409679566825354, - "heading": -0.7269791464813684, - "angularVelocity": 0.25358070367489455, - "velocityX": 0.5630491905002851, - "velocityY": -0.38833211314732513, - "timestamp": 6.07887702701504 - }, - { - "x": 3.333253092651543, - "y": 2.311030569251179, - "heading": -0.7072747038712209, - "angularVelocity": 0.33595478585270977, - "velocityX": 0.7556808447487132, - "velocityY": -0.5104233995592209, - "timestamp": 6.13752909401904 - }, - { - "x": 3.3890486465390515, - "y": 2.274212618473126, - "heading": -0.6828237317423621, - "angularVelocity": 0.4168816783079693, - "velocityX": 0.9512973154672166, - "velocityY": -0.6277349232302787, - "timestamp": 6.19618116102304 - }, - { - "x": 3.456514730281209, - "y": 2.2308496047648436, - "heading": -0.6537300972010771, - "angularVelocity": 0.4960376680211611, - "velocityX": 1.1502763191202974, - "velocityY": -0.7393262662904144, - "timestamp": 6.2548332280270404 - }, - { - "x": 3.5358737017178647, - "y": 2.1813488976810533, - "heading": -0.6201228984071238, - "angularVelocity": 0.5729925731630496, - "velocityX": 1.3530464566789135, - "velocityY": -0.843972081673003, - "timestamp": 6.3134852950310405 - }, - { - "x": 3.6273755302567405, - "y": 2.1262132491994077, - "heading": -0.5821654777138521, - "angularVelocity": 0.6471625405918474, - "velocityX": 1.5600784970227222, - "velocityY": -0.9400461279205379, - "timestamp": 6.3721373620350406 - }, - { - "x": 3.731298337786468, - "y": 2.066074432465498, - "heading": -0.540068702761689, - "angularVelocity": 0.7177372785394325, - "velocityX": 1.7718524314350428, - "velocityY": -1.0253486331489114, - "timestamp": 6.430789429039041 - }, - { - "x": 3.8479437090237303, - "y": 2.0017419289175256, - "heading": -0.49411087881678867, - "angularVelocity": 0.783566995887226, - "velocityX": 1.9887682940365567, - "velocityY": -1.09684972472642, - "timestamp": 6.489441496043041 - }, - { - "x": 3.977619323731122, - "y": 1.934273302196501, - "heading": -0.44466788468288826, - "angularVelocity": 0.8429880933357167, - "velocityX": 2.2109300035163186, - "velocityY": -1.1503196761407186, - "timestamp": 6.548093563047041 - }, - { - "x": 4.120592730506154, - "y": 1.8650726101269166, - "heading": -0.3922584382465056, - "angularVelocity": 0.8935652077327264, - "velocityX": 2.437653335649394, - "velocityY": -1.1798508663789342, - "timestamp": 6.606745630051041 - }, - { - "x": 4.276984319445326, - "y": 1.7960128941742703, - "heading": -0.3376091877980351, - "angularVelocity": 0.9317531885916955, - "velocityX": 2.6664292825094678, - "velocityY": -1.1774472662308222, - "timestamp": 6.665397697055041 - }, - { - "x": 4.446552051157268, - "y": 1.729536835320209, - "heading": -0.28173622459333697, - "angularVelocity": 0.9526171209087647, - "velocityX": 2.891078530964293, - "velocityY": -1.1333966942636178, - "timestamp": 6.724049764059041 - }, - { - "x": 4.628358146968442, - "y": 1.6685924186219296, - "heading": -0.2260024396425045, - "angularVelocity": 0.9502441737821766, - "velocityX": 3.0997389367159993, - "velocityY": -1.0390838688451245, - "timestamp": 6.782701831063041 - }, - { - "x": 4.820505608377407, - "y": 1.6162066866444724, - "heading": -0.17202923007966578, - "angularVelocity": 0.9202268959959776, - "velocityX": 3.2760560918656307, - "velocityY": -0.8931608833817366, - "timestamp": 6.841353898067041 - }, - { - "x": 5.0203266835475935, - "y": 1.5748141105227507, - "heading": -0.12139543853917772, - "angularVelocity": 0.863290828898412, - "velocityX": 3.4068888852043435, - "velocityY": -0.7057309015025682, - "timestamp": 6.900005965071041 - }, - { - "x": 5.225003672109784, - "y": 1.5458956549300438, - "heading": -0.07534747020156207, - "angularVelocity": 0.7851039305140859, - "velocityX": 3.4896807396102973, - "velocityY": -0.4930509199400402, - "timestamp": 6.958658032075041 - }, - { - "x": 5.432079779027424, - "y": 1.5301290764345634, - "heading": -0.034717164940788754, - "angularVelocity": 0.6927344139125162, - "velocityX": 3.5305849818305606, - "velocityY": -0.2688153939128066, - "timestamp": 7.017310099079041 - }, - { - "x": 5.639612197875977, - "y": 1.527712106704712, - "heading": 0, - "angularVelocity": 0.5919171601986558, - "velocityX": 3.538364962216936, - "velocityY": -0.04120860275370209, - "timestamp": 7.075962166083041 - }, - { - "x": 5.885430007343772, - "y": 1.5437296393309712, - "heading": 0.03257763753195304, - "angularVelocity": 0.4661620557103504, - "velocityX": 3.5174722316598084, - "velocityY": 0.22919912253124494, - "timestamp": 7.14584695643199 - }, - { - "x": 6.1267293814658395, - "y": 1.5781470725007365, - "heading": 0.05611463103184265, - "angularVelocity": 0.3367970824891158, - "velocityX": 3.4528167419149254, - "velocityY": 0.49248817944379436, - "timestamp": 7.215731746780939 - }, - { - "x": 6.3596238960316915, - "y": 1.6295476131104372, - "heading": 0.07077632892605286, - "angularVelocity": 0.20979812375484447, - "velocityX": 3.332549377381865, - "velocityY": 0.7355039680744148, - "timestamp": 7.285616537129888 - }, - { - "x": 6.579809345763393, - "y": 1.6950287619001128, - "heading": 0.07741889217082244, - "angularVelocity": 0.09505019921504924, - "velocityX": 3.1506919979622094, - "velocityY": 0.9369871249912134, - "timestamp": 7.355501327478837 - }, - { - "x": 6.7836458698285345, - "y": 1.770189303236, - "heading": 0.07752756675122702, - "angularVelocity": 0.0015550533937633648, - "velocityX": 2.916750884524989, - "velocityY": 1.0754921201107606, - "timestamp": 7.425386117827786 - }, - { - "x": 6.969066872375604, - "y": 1.8501496814117884, - "heading": 0.07288957023575314, - "angularVelocity": -0.06636632223285495, - "velocityX": 2.653238302944389, - "velocityY": 1.1441742584692527, - "timestamp": 7.495270908176735 - }, - { - "x": 7.135444568197587, - "y": 1.930676718523136, - "heading": 0.06516656217506993, - "angularVelocity": -0.11051057064234768, - "velocityX": 2.3807425763349213, - "velocityY": 1.1522827314678832, - "timestamp": 7.565155698525684 - }, - { - "x": 7.282944965396222, - "y": 2.00855476747338, - "heading": 0.05568193868283516, - "angularVelocity": -0.1357179930693943, - "velocityX": 2.110622303682033, - "velocityY": 1.114377657304012, - "timestamp": 7.635040488874633 - }, - { - "x": 7.412041998803859, - "y": 2.081448389680507, - "heading": 0.04544169764196664, - "angularVelocity": -0.14653032497825166, - "velocityX": 1.8472836902425835, - "velocityY": 1.0430541730633198, - "timestamp": 7.704925279223582 - }, - { - "x": 7.52328085623725, - "y": 2.1476599835831536, - "heading": 0.035211647982849995, - "angularVelocity": -0.1463844937937982, - "velocityX": 1.5917463138681878, - "velocityY": 0.9474392578419155, - "timestamp": 7.774810069572531 - }, - { - "x": 7.6171846756036246, - "y": 2.2059319251882075, - "heading": 0.025583765782598092, - "angularVelocity": -0.13776791991759432, - "velocityX": 1.343694656555366, - "velocityY": 0.8338286673550865, - "timestamp": 7.84469485992148 - }, - { - "x": 7.6942246354285295, - "y": 2.2553098100503624, - "heading": 0.01702415874138481, - "angularVelocity": -0.12248168733816604, - "velocityX": 1.1023852177309095, - "velocityY": 0.7065612505324912, - "timestamp": 7.914579650270429 - }, - { - "x": 7.7548150787245245, - "y": 2.295051799999363, - "heading": 0.009906531689475896, - "angularVelocity": -0.10184801322818779, - "velocityX": 0.8670047229655302, - "velocityY": 0.5686786745808462, - "timestamp": 7.984464440619378 - }, - { - "x": 7.799317576321477, - "y": 2.3245685027375944, - "heading": 0.0045355005230709265, - "angularVelocity": -0.0768555094690318, - "velocityX": 0.6367980411008246, - "velocityY": 0.42236232792355316, - "timestamp": 8.054349230968327 - }, - { - "x": 7.828047535553298, - "y": 2.3433824520970585, - "heading": 0.0011629993208823611, - "angularVelocity": -0.04825801415943252, - "velocityX": 0.41110460642961777, - "velocityY": 0.26921379123442685, - "timestamp": 8.124234021317276 - }, - { - "x": 7.841280937194824, - "y": 2.351100206375122, - "heading": 0, - "angularVelocity": -0.016641665734064087, - "velocityX": 0.18936025386137842, - "velocityY": 0.11043539287343317, - "timestamp": 8.194118811666225 - }, - { - "x": 7.838534697779776, - "y": 2.3467601761094876, - "heading": 0.0013754462067047074, - "angularVelocity": 0.018914520887371087, - "velocityX": -0.03776505582293692, - "velocityY": -0.059682154569921285, - "timestamp": 8.266837873116154 - }, - { - "x": 7.819004816606, - "y": 2.3304120813281917, - "heading": 0.005139122834024266, - "angularVelocity": 0.05175639718495326, - "velocityX": -0.2685661886219886, - "velocityY": -0.22481168562045675, - "timestamp": 8.339556934566083 - }, - { - "x": 7.782387572934137, - "y": 2.3024894711129518, - "heading": 0.011058252379099645, - "angularVelocity": 0.08139722140323555, - "velocityX": -0.5035439531501099, - "velocityY": -0.3839792436604268, - "timestamp": 8.412275996016012 - }, - { - "x": 7.728336383406839, - "y": 2.2635182406954444, - "heading": 0.018855442211434054, - "angularVelocity": 0.10722346626686281, - "velocityX": -0.7432877769539888, - "velocityY": -0.5359149257494328, - "timestamp": 8.484995057465941 - }, - { - "x": 7.656454719773965, - "y": 2.2141471165839395, - "heading": 0.028196004270951525, - "angularVelocity": 0.12844723066109603, - "velocityX": -0.9884844798549632, - "velocityY": -0.6789296111240317, - "timestamp": 8.55771411891587 - }, - { - "x": 7.566289368711756, - "y": 2.155191783566264, - "heading": 0.0386702307264049, - "angularVelocity": 0.14403687625513997, - "velocityX": -1.2399135696256622, - "velocityY": -0.8107273642175508, - "timestamp": 8.630433180365799 - }, - { - "x": 7.457326803867402, - "y": 2.087700059950414, - "heading": 0.049768261525896085, - "angularVelocity": 0.15261515451671143, - "velocityX": -1.4984044440587245, - "velocityY": -0.9281159887125598, - "timestamp": 8.703152241815728 - }, - { - "x": 7.328999818599222, - "y": 2.0130495362466543, - "heading": 0.06084411215879332, - "angularVelocity": 0.1523101427886762, - "velocityX": -1.7646952904712614, - "velocityY": -1.0265606048169413, - "timestamp": 8.775871303265657 - }, - { - "x": 7.180722027662032, - "y": 1.933093466427668, - "heading": 0.07106435780754797, - "angularVelocity": 0.14054424582737238, - "velocityX": -2.039049844438464, - "velocityY": -1.0995201013979004, - "timestamp": 8.848590364715585 - }, - { - "x": 7.011991212253484, - "y": 1.8503687292260445, - "heading": 0.07933791177942008, - "angularVelocity": 0.11377421279795905, - "velocityX": -2.320310686692896, - "velocityY": -1.1375935765972949, - "timestamp": 8.921309426165514 - }, - { - "x": 6.822644207407782, - "y": 1.768344037494235, - "heading": 0.0842369889796069, - "angularVelocity": 0.06736991790742633, - "velocityX": -2.6038153005601905, - "velocityY": -1.1279668644828114, - "timestamp": 8.994028487615443 - }, - { - "x": 6.613366078180803, - "y": 1.6915478876016088, - "heading": 0.08398293037214401, - "angularVelocity": -0.0034937003090699014, - "velocityX": -2.8778992062635105, - "velocityY": -1.0560662962558207, - "timestamp": 9.066747549065372 - }, - { - "x": 6.386348019322392, - "y": 1.6251685070860846, - "heading": 0.07667502896821522, - "angularVelocity": -0.10049499069732412, - "velocityX": -3.121850782063855, - "velocityY": -0.9128195440370198, - "timestamp": 9.139466610515301 - }, - { - "x": 6.145418125434476, - "y": 1.573898737537472, - "heading": 0.060738243657818394, - "angularVelocity": -0.21915554178831262, - "velocityX": -3.313160113511754, - "velocityY": -0.7050389337589908, - "timestamp": 9.21218567196523 - }, - { - "x": 5.895122985100364, - "y": 1.5408852116427318, - "heading": 0.035252939658974705, - "angularVelocity": -0.35046249897479254, - "velocityX": -3.441946792815155, - "velocityY": -0.45398723851065725, - "timestamp": 9.284904733415159 - }, - { - "x": 5.639612197875977, - "y": 1.527712106704712, - "heading": 0, - "angularVelocity": -0.48478265472730503, - "velocityX": -3.513670035473711, - "velocityY": -0.18115064572292816, - "timestamp": 9.357623794865088 - }, - { - "x": 5.421755886877325, - "y": 1.5312175249525746, - "heading": -0.03672524163627121, - "angularVelocity": -0.5960811255744302, - "velocityX": -3.5359885813607668, - "velocityY": 0.05689584497468608, - "timestamp": 9.419234941503694 - }, - { - "x": 5.204290292939256, - "y": 1.5494561200267603, - "heading": -0.08000897664374254, - "angularVelocity": -0.7025309115144727, - "velocityX": -3.5296469194716105, - "velocityY": 0.2960275221165476, - "timestamp": 9.4808460881423 - }, - { - "x": 4.9895755761204486, - "y": 1.5821879429902566, - "heading": -0.1292741669916279, - "angularVelocity": -0.7996148917153227, - "velocityX": -3.4849979027052926, - "velocityY": 0.531264629036892, - "timestamp": 9.542457234780906 - }, - { - "x": 4.780630223860834, - "y": 1.6285196439150298, - "heading": -0.18348985013811628, - "angularVelocity": -0.8799654949534231, - "velocityX": -3.3913563317565196, - "velocityY": 0.7520019258291494, - "timestamp": 9.604068381419513 - }, - { - "x": 4.58085176131032, - "y": 1.6864679206086615, - "heading": -0.24107165456516222, - "angularVelocity": -0.9346004346389692, - "velocityX": -3.2425701102815387, - "velocityY": 0.9405485834169061, - "timestamp": 9.665679528058119 - }, - { - "x": 4.39322892066763, - "y": 1.7528867750124326, - "heading": -0.3000698023851603, - "angularVelocity": -0.9575888623866302, - "velocityX": -3.045274286862978, - "velocityY": 1.0780330837432093, - "timestamp": 9.727290674696725 - }, - { - "x": 4.21961035389735, - "y": 1.8241528988520266, - "heading": -0.35862216993070006, - "angularVelocity": -0.9503534788760283, - "velocityX": -2.81797330909422, - "velocityY": 1.1567082862070746, - "timestamp": 9.788901821335331 - }, - { - "x": 4.060717549232027, - "y": 1.89701942348135, - "heading": -0.4152832433765806, - "angularVelocity": -0.9196562073132535, - "velocityX": -2.578961979028652, - "velocityY": 1.1826841181310743, - "timestamp": 9.850512967973938 - }, - { - "x": 3.916597920233781, - "y": 1.9689586974543731, - "heading": -0.46903556717701983, - "angularVelocity": -0.8724447885337365, - "velocityX": -2.3391810875329626, - "velocityY": 1.1676340710715827, - "timestamp": 9.912124114612544 - }, - { - "x": 3.7870044894605353, - "y": 2.0381033443744836, - "heading": -0.5191754657650203, - "angularVelocity": -0.8138121317901729, - "velocityX": -2.103408844724257, - "velocityY": 1.122274956603138, - "timestamp": 9.97373526125115 - }, - { - "x": 3.6715959698515057, - "y": 2.1030801530689605, - "heading": -0.5652089663039379, - "angularVelocity": -0.747161886288821, - "velocityX": -1.8731759739188905, - "velocityY": 1.0546274860880838, - "timestamp": 10.035346407889756 - }, - { - "x": 3.5700229782202233, - "y": 2.1628635944770185, - "heading": -0.6067804926332262, - "angularVelocity": -0.6747403448459703, - "velocityX": -1.6486138819503326, - "velocityY": 0.9703348285129628, - "timestamp": 10.096957554528363 - }, - { - "x": 3.4819602854664806, - "y": 2.2166708649366824, - "heading": -0.6436270740346884, - "angularVelocity": -0.5980505705825362, - "velocityX": -1.4293305279691468, - "velocityY": 0.873336618376596, - "timestamp": 10.158568701166969 - }, - { - "x": 3.407116174486021, - "y": 2.2638908403054034, - "heading": -0.6755491614396836, - "angularVelocity": -0.518121949462191, - "velocityX": -1.2147819844917866, - "velocityY": 0.7664193566417493, - "timestamp": 10.220179847805575 - }, - { - "x": 3.3452327250842697, - "y": 2.3040363154035894, - "heading": -0.7023917849131657, - "angularVelocity": -0.43567803778971587, - "velocityX": -1.0044196996485568, - "velocityY": 0.6515943508350659, - "timestamp": 10.281790994444181 - }, - { - "x": 3.296082852481604, - "y": 2.336711509393393, - "heading": -0.7240321240208134, - "angularVelocity": -0.35124064862133164, - "velocityX": -0.7977431890850113, - "velocityY": 0.5303454938351937, - "timestamp": 10.343402141082787 - }, - { - "x": 3.259466484246681, - "y": 2.3615895348218507, - "heading": -0.740371103456648, - "angularVelocity": -0.26519518508030326, - "velocityX": -0.5943140199890253, - "velocityY": 0.4037909824075443, - "timestamp": 10.405013287721394 - }, - { - "x": 3.235206808114262, - "y": 2.3783964459205325, - "heading": -0.7513275636357691, - "angularVelocity": -0.1778324341760547, - "velocityX": -0.393754660576598, - "velocityY": 0.2727901039931329, - "timestamp": 10.46662443436 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": -0.08937588581967223, - "velocityX": -0.19574205864421412, - "velocityY": 0.1380150223608686, - "timestamp": 10.528235580998606 - }, - { - "x": 3.223146915435791, - "y": 2.386899709701538, - "heading": -0.7568341144429602, - "angularVelocity": 0, - "velocityX": 8.242872749735147e-35, - "velocityY": 1.4597202684890146e-35, - "timestamp": 10.589846727637212 - }, - { - "x": 3.2500504466823896, - "y": 2.378701613137869, - "heading": -0.7519553497418766, - "angularVelocity": 0.05745486103192854, - "velocityX": 0.3168299239145798, - "velocityY": -0.09654503294396014, - "timestamp": 10.674761469635673 - }, - { - "x": 3.3038574781039927, - "y": 2.3623053962285474, - "heading": -0.7421975985160749, - "angularVelocity": 0.1149123343739134, - "velocityX": 0.6336594819139709, - "velocityY": -0.19309034595687619, - "timestamp": 10.759676211634133 - }, - { - "x": 3.3845679847892804, - "y": 2.337711013412665, - "heading": -0.7275626808140259, - "angularVelocity": 0.17234837388205654, - "velocityX": 0.9504887465447538, - "velocityY": -0.2896361955186583, - "timestamp": 10.844590953632594 - }, - { - "x": 3.4921819468417152, - "y": 2.3049183962425888, - "heading": -0.7080543108308168, - "angularVelocity": 0.22974067310435634, - "velocityX": 1.2673177768636017, - "velocityY": -0.3861828511552315, - "timestamp": 10.929505695631054 - }, - { - "x": 3.626699347800246, - "y": 2.2639274521002806, - "heading": -0.6836778860236516, - "angularVelocity": 0.28706940907395306, - "velocityX": 1.5841466133285746, - "velocityY": -0.48273059750981107, - "timestamp": 11.014420437629514 - }, - { - "x": 3.7881201726436093, - "y": 2.214738062849279, - "heading": -0.6544402090024873, - "angularVelocity": 0.3443180339839501, - "velocityX": 1.9009752728953775, - "velocityY": -0.5792797351005696, - "timestamp": 11.099335179627975 - }, - { - "x": 3.976444405401589, - "y": 2.157350083559159, - "heading": -0.6203491385118067, - "angularVelocity": 0.401474110246943, - "velocityX": 2.217803744388632, - "velocityY": -0.6758305794671143, - "timestamp": 11.184249921626435 - }, - { - "x": 4.191672026395985, - "y": 2.091763341463472, - "heading": -0.581413167044626, - "angularVelocity": 0.45853017451182726, - "velocityX": 2.5346319841411913, - "velocityY": -0.7723834584208757, - "timestamp": 11.269164663624895 - }, - { - "x": 4.433803009109946, - "y": 2.017977635340623, - "heading": -0.5376409250533941, - "angularVelocity": 0.5154846020968357, - "velocityX": 2.851459911617613, - "velocityY": -0.868938707064401, - "timestamp": 11.354079405623356 - }, - { - "x": 4.702837316573546, - "y": 1.9359927355638575, - "heading": -0.48904061693477097, - "angularVelocity": 0.5723424104557058, - "velocityX": 3.168287403716993, - "velocityY": -0.9654966599115613, - "timestamp": 11.438994147621816 - }, - { - "x": 4.998774896616477, - "y": 1.8458083852834486, - "heading": -0.4356194112597129, - "angularVelocity": 0.6291157980086282, - "velocityX": 3.4851142814318106, - "velocityY": -1.0620576375541941, - "timestamp": 11.523908889620277 - }, - { - "x": 5.321615670956704, - "y": 1.7474243048973186, - "heading": -0.37738292770821397, - "angularVelocity": 0.6858230052981278, - "velocityX": 3.8019402372568085, - "velocityY": -1.1586219079357767, - "timestamp": 11.608823631618737 - }, - { - "x": 5.664898737663787, - "y": 1.6427906762580982, - "heading": -0.37738292062184065, - "angularVelocity": 8.345280395968993e-8, - "velocityX": 4.0426792642590454, - "velocityY": -1.2322198263420197, - "timestamp": 11.693738373617197 - }, - { - "x": 6.008181814361773, - "y": 1.5381570803955327, - "heading": -0.37738291353643977, - "angularVelocity": 8.344135212048987e-8, - "velocityX": 4.042679381917106, - "velocityY": -1.232219440347153, - "timestamp": 11.778653115615658 - }, - { - "x": 6.351464887209067, - "y": 1.4335234719011998, - "heading": -0.3773829064500845, - "angularVelocity": 8.345259143210343e-8, - "velocityX": 4.042679336569362, - "velocityY": -1.2322195891053875, - "timestamp": 11.863567857614118 - }, - { - "x": 6.674297686591828, - "y": 1.3351103504378636, - "heading": -0.3193930007642967, - "angularVelocity": 0.6829191765882003, - "velocityX": 3.8018463200255006, - "velocityY": -1.1589639107084677, - "timestamp": 11.948482599612579 - }, - { - "x": 6.970227447718613, - "y": 1.244897503452253, - "heading": -0.2662161162246808, - "angularVelocity": 0.6262385457236648, - "velocityX": 3.4850222018239303, - "velocityY": -1.0623932295201048, - "timestamp": 12.033397341611039 - }, - { - "x": 7.239254236839848, - "y": 1.1628851677059724, - "heading": -0.21785490809513916, - "angularVelocity": 0.5695266450956005, - "velocityX": 3.1681988638217, - "velocityY": -0.9658197601044064, - "timestamp": 12.1183120836095 - }, - { - "x": 7.48137812927984, - "y": 1.0890735490452839, - "heading": -0.1743132115865013, - "angularVelocity": 0.512769579037611, - "velocityX": 2.8513764128775443, - "velocityY": -0.8692438665364745, - "timestamp": 12.20322682560796 - }, - { - "x": 7.696599198563337, - "y": 1.0234628255753193, - "heading": -0.13559564838663762, - "angularVelocity": 0.45595808558855033, - "velocityX": 2.53455482779892, - "velocityY": -0.7726658755102153, - "timestamp": 12.28814156760642 - }, - { - "x": 7.884917512273281, - "y": 0.9660531491893245, - "heading": -0.10170713796449406, - "angularVelocity": 0.3990886579241798, - "velocityX": 2.217734038612032, - "velocityY": -0.6760860957103977, - "timestamp": 12.37305630960488 - }, - { - "x": 8.046333129383763, - "y": 0.9168446470274297, - "heading": -0.07265245047866593, - "angularVelocity": 0.3421630544005527, - "velocityX": 1.9009139439346003, - "velocityY": -0.5795048186425281, - "timestamp": 12.45797105160334 - }, - { - "x": 8.18084609829272, - "y": 0.8758374229594964, - "heading": -0.048435821985695085, - "angularVelocity": 0.28518756488019376, - "velocityX": 1.5840944192163566, - "velocityY": -0.4829223183493492, - "timestamp": 12.542885793601801 - }, - { - "x": 8.288456455335494, - "y": 0.8430315590188601, - "heading": -0.029060636546388994, - "angularVelocity": 0.2281722229063284, - "velocityX": 1.2672753224019029, - "velocityY": -0.38633885198910756, - "timestamp": 12.627800535600262 - }, - { - "x": 8.369164223685814, - "y": 0.8184271166917905, - "heading": -0.014529175658252375, - "angularVelocity": 0.17113001283569912, - "velocityX": 0.9504564984933311, - "velocityY": -0.289754661534691, - "timestamp": 12.712715277598722 - }, - { - "x": 8.422969412591065, - "y": 0.8020241379761622, - "heading": -0.004842433994812591, - "angularVelocity": 0.11407608897422551, - "velocityX": 0.6336377834867251, - "velocityY": -0.19316997649154619, - "timestamp": 12.797630019597182 - }, - { - "x": 8.449872016906738, - "y": 0.7938226461410522, - "heading": 5.797004207427939e-34, - "angularVelocity": 0.05702701181027418, - "velocityX": 0.3168190078957048, - "velocityY": -0.09658501741969248, - "timestamp": 12.882544761595643 - }, - { - "x": 8.449872016906738, - "y": 0.7938226461410522, - "heading": -2.2932518796347515e-34, - "angularVelocity": -9.527505988231194e-33, - "velocityX": -1.2891294779020923e-34, - "velocityY": 4.6042246741965494e-35, - "timestamp": 12.967459503594103 - } - ] -} \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java index a528a978..a3843902 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFSprint.java @@ -31,10 +31,10 @@ public CenterLanePCBAFSprint(Drivetrain drivetrain, Shooter shooter, Indexer ind createSegmentSequence(drivetrain, shooter, indexer, 0, false, false, false), createSegmentSequence(drivetrain, shooter, indexer, 1, false, false, true), createSegmentSequence(drivetrain, shooter, indexer, 2, false, false, true), - createSegmentSequence(drivetrain, shooter, indexer, 3, false, false, true) + createSegmentSequence(drivetrain, shooter, indexer, 3, false, false, true), + getPath(4).getCommand(drivetrain) ) - ), - getPath(4).getCommand(drivetrain) + ) ); addRequirements(drivetrain, shooter, indexer); diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java index f71b6978..3f1bc86e 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBFSprint.java @@ -10,15 +10,15 @@ import org.team1540.robot2024.util.auto.PathHelper; public class CenterLanePCBFSprint extends AutoCommand { - public CenterLanePCBFSprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ - super("!!CenterLanePCBFSprint"); + public CenterLanePCBFSprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("!CenterLanePCBFSprint"); addPath( - PathHelper.fromChoreoPath("CenterLanePCBFSprint.1", true, true), + PathHelper.fromChoreoPath("CenterLanePCBAFSprint.1", false, true), PathHelper.fromChoreoPath("CenterLanePCBFSprint.2"), PathHelper.fromChoreoPath("CenterLanePCBFSprint.3"), - PathHelper.fromChoreoPath("CenterLanePCBFSprint.4"), - PathHelper.fromChoreoPath("CenterLanePCBFSprint.5") + PathHelper.fromChoreoPath("CenterLanePCBFSprint.4") + ); addCommands( @@ -26,14 +26,15 @@ public CenterLanePCBFSprint(Drivetrain drivetrain, Shooter shooter, Indexer inde Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, shooter, indexer, 0), - createSegmentSequence(drivetrain, shooter, indexer, 1), - getPath(2).getCommand(drivetrain), - Commands.runOnce(drivetrain::copyVisionPose), - createSegmentSequence(drivetrain, shooter, indexer, 3) + drivetrain.commandCopyVisionPose(), + createSegmentSequence(drivetrain, shooter, indexer, 0, false, false, false), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, false, true), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, false, true), + getPath(3).getCommand(drivetrain) ) - ), - getPath(4).getCommand(drivetrain) + ) ); + + addRequirements(drivetrain, shooter, indexer); } } diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java index 198ec0da..0a265843 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHSprint.java @@ -9,8 +9,9 @@ import org.team1540.robot2024.util.auto.PathHelper; public class SourceLanePGHSprint extends AutoCommand { + private static final double SHOT_WAIT = 0.2; public SourceLanePGHSprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ - super("!SourceLanePGHSprint"); + super("!!SourceLanePGHSprint"); addPath( PathHelper.fromChoreoPath("SourceLanePGHSprint.1", true, true), PathHelper.fromChoreoPath("SourceLanePGHSprint.2"), @@ -22,11 +23,11 @@ public SourceLanePGHSprint(Drivetrain drivetrain, Shooter shooter, Indexer index Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, SHOT_WAIT), drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, shooter, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, SHOT_WAIT), drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, shooter, indexer, 2), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, SHOT_WAIT), drivetrain.commandCopyVisionPose(), getPath(3).getCommand(drivetrain) ) diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index f82343b1..3948e432 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -84,7 +84,7 @@ public void setPathIndex(int index){ * @param pathIndex * @return the commands to run in the segment */ - protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry) { + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry, double extraPreShotWait) { return Commands.sequence( Commands.deadline( getPath(pathIndex).getCommand(drivetrain), @@ -102,9 +102,10 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, // Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10).onlyIf(()->shouldRealignYaw), new ParallelDeadlineGroup( Commands.sequence( + Commands.waitSeconds(extraPreShotWait), Commands.waitUntil(()->!indexer.isNoteStaged()), Commands.waitSeconds(0.2) - ).withTimeout(1), + ).withTimeout(1+extraPreShotWait), IntakeAndFeed.withDefaults(indexer) ) ) @@ -112,11 +113,15 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, ); } protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex){ - return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, false, true, true); + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, false, true, true,0); + } + + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry){ + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, shouldZeroCancoder, shouldRealignYaw, shouldResetOdometry,0); } protected Command createCancoderSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex) { - return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, true, true, true); + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, true, true, true,0 ); } public List toTrajectory() { From e7f52f15724d2f4d4bfa808dcdda89e1fba399c6 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Tue, 26 Mar 2024 13:06:54 -0700 Subject: [PATCH 58/75] feat: default shooter behavior and auto pruning --- paths.chor | 48215 ++++++++-------- .../deploy/choreo/AmpLanePABCSprint.1.traj | 139 - .../deploy/choreo/AmpLanePABCSprint.2.traj | 148 - .../deploy/choreo/AmpLanePABCSprint.3.traj | 148 - .../deploy/choreo/AmpLanePABCSprint.4.traj | 454 - .../deploy/choreo/AmpLanePADESprint.1.traj | 311 +- .../deploy/choreo/AmpLanePADESprint.2.traj | 149 +- .../deploy/choreo/AmpLanePADESprint.3.traj | 1049 +- .../deploy/choreo/AmpLanePADESprint.4.traj | 1355 +- .../deploy/choreo/AmpLanePADESprint.5.traj | 383 +- src/main/deploy/choreo/AmpLanePADESprint.traj | 3155 +- src/main/deploy/choreo/AmpLanePADEf.1.traj | 130 - src/main/deploy/choreo/AmpLanePADEf.2.traj | 103 - src/main/deploy/choreo/AmpLanePADEf.3.traj | 463 - src/main/deploy/choreo/AmpLanePADEf.4.traj | 661 - src/main/deploy/choreo/AmpLanePADEf.5.traj | 373 - src/main/deploy/choreo/AmpLanePADEf.traj | 1678 - .../choreo/AmpLanePAmpAAmpSprint.1.traj | 391 - .../deploy/choreo/AmpLanePAmpAAmpSprint.traj | 391 - src/main/deploy/choreo/AmpLanePSprint.1.traj | 148 - src/main/deploy/choreo/AmpLanePSprint.2.traj | 301 - .../choreo/AmpLanePSubASubDSubESub.1.traj | 707 +- .../choreo/AmpLanePSubASubDSubESub.2.traj | 1319 +- .../choreo/AmpLanePSubASubDSubESub.3.traj | 1715 +- .../choreo/AmpLanePSubASubDSubESub.traj | 3695 +- src/main/deploy/choreo/AmpLaneSprint.1.traj | 238 - src/main/deploy/choreo/AmpLaneSprint.traj | 238 - src/main/deploy/choreo/AmpLaneTaxi.1.traj | 139 - src/main/deploy/choreo/AmpLaneTaxi.traj | 139 - src/main/deploy/choreo/CenterLanePBDA.1.traj | 239 +- src/main/deploy/choreo/CenterLanePBDA.2.traj | 1031 +- src/main/deploy/choreo/CenterLanePBDA.3.traj | 545 +- src/main/deploy/choreo/CenterLanePBDA.traj | 1769 +- src/main/deploy/choreo/CenterLanePBc.1.traj | 256 - src/main/deploy/choreo/CenterLanePBc.2.traj | 220 - src/main/deploy/choreo/CenterLanePBc.3.traj | 220 - src/main/deploy/choreo/CenterLanePBc.4.traj | 283 - src/main/deploy/choreo/CenterLanePCBA.1.traj | 617 +- src/main/deploy/choreo/CenterLanePCBA.2.traj | 185 +- src/main/deploy/choreo/CenterLanePCBA.3.traj | 383 +- src/main/deploy/choreo/CenterLanePCBA.traj | 1139 +- src/main/deploy/choreo/CenterLanePCBAD.1.traj | 328 - src/main/deploy/choreo/CenterLanePCBAD.2.traj | 103 - src/main/deploy/choreo/CenterLanePCBAD.3.traj | 202 - src/main/deploy/choreo/CenterLanePCBAD.4.traj | 526 - src/main/deploy/choreo/CenterLanePCBAD.5.traj | 202 - src/main/deploy/choreo/CenterLanePCBAD.traj | 1309 - .../deploy/choreo/CenterLanePCBADEF.1.traj | 157 - .../deploy/choreo/CenterLanePCBADEF.2.traj | 211 - .../deploy/choreo/CenterLanePCBADEF.3.traj | 157 - .../deploy/choreo/CenterLanePCBADEF.4.traj | 427 - .../deploy/choreo/CenterLanePCBADEF.5.traj | 436 - .../deploy/choreo/CenterLanePCBADEF.6.traj | 589 - .../choreo/CenterLanePCBADSprint.1.traj | 617 +- .../choreo/CenterLanePCBADSprint.2.traj | 185 +- .../choreo/CenterLanePCBADSprint.3.traj | 383 +- .../choreo/CenterLanePCBADSprint.4.traj | 1013 +- .../choreo/CenterLanePCBADSprint.5.traj | 383 +- .../deploy/choreo/CenterLanePCBADSprint.traj | 2552 +- .../deploy/choreo/CenterLanePCBAFE.1.traj | 599 +- .../deploy/choreo/CenterLanePCBAFE.2.traj | 194 +- .../deploy/choreo/CenterLanePCBAFE.3.traj | 374 +- .../deploy/choreo/CenterLanePCBAFE.4.traj | 1418 +- .../deploy/choreo/CenterLanePCBAFE.5.traj | 1265 +- src/main/deploy/choreo/CenterLanePCBAFE.traj | 3758 +- .../choreo/CenterLanePCBAFSprint.1.traj | 617 +- .../choreo/CenterLanePCBAFSprint.2.traj | 203 +- .../choreo/CenterLanePCBAFSprint.3.traj | 419 +- .../choreo/CenterLanePCBAFSprint.4.traj | 1733 +- .../choreo/CenterLanePCBAFSprint.5.traj | 761 +- .../deploy/choreo/CenterLanePCBAFSprint.traj | 3641 +- .../deploy/choreo/CenterLanePCBFSprint.1.traj | 617 +- .../deploy/choreo/CenterLanePCBFSprint.2.traj | 203 +- .../deploy/choreo/CenterLanePCBFSprint.3.traj | 1625 +- .../deploy/choreo/CenterLanePCBFSprint.4.traj | 761 +- .../deploy/choreo/CenterLanePCBFSprint.traj | 3137 +- .../choreo/CenterLanePSubCSubBSubAS.1.traj | 337 - .../choreo/CenterLanePSubCSubBSubAS.2.traj | 229 - .../choreo/CenterLanePSubCSubBSubAS.3.traj | 337 - .../choreo/CenterLanePSubCSubBSubAS.4.traj | 508 - .../CenterLanePSubCSubBSubASubFSub.1.traj | 635 +- .../CenterLanePSubCSubBSubASubFSub.2.traj | 455 +- .../CenterLanePSubCSubBSubASubFSub.3.traj | 671 +- .../CenterLanePSubCSubBSubASubFSub.4.traj | 1859 +- .../CenterLanePSubCSubBSubASubFSub.traj | 3551 +- .../CenterLanePSubCSubBSubASubSprint.1.traj | 635 +- .../CenterLanePSubCSubBSubASubSprint.2.traj | 419 +- .../CenterLanePSubCSubBSubASubSprint.3.traj | 635 +- .../CenterLanePSubCSubBSubASubSprint.4.traj | 941 +- .../CenterLanePSubCSubBSubASubSprint.traj | 2561 +- .../deploy/choreo/CenterLaneSprint.1.traj | 815 +- src/main/deploy/choreo/CenterLaneSprint.traj | 815 +- .../choreo/CenterLaneSprintBonus.1.traj | 499 - .../choreo/CenterLaneSprintBonus.2.traj | 391 - src/main/deploy/choreo/CenterLaneTaxi.1.traj | 139 - src/main/deploy/choreo/CenterLaneTaxi.traj | 139 - src/main/deploy/choreo/New Path (1).1.traj | 373 - src/main/deploy/choreo/New Path (1).2.traj | 679 - src/main/deploy/choreo/SQUARFE (1).1.traj | 211 - src/main/deploy/choreo/SQUARFE (1).2.traj | 211 - src/main/deploy/choreo/SQUARFE (1).3.traj | 211 - src/main/deploy/choreo/SQUARFE (1).4.traj | 211 - src/main/deploy/choreo/SQUARFE.1.traj | 805 - .../deploy/choreo/SourceLanePGCSprint.1.traj | 365 +- .../deploy/choreo/SourceLanePGCSprint.2.traj | 1679 +- .../deploy/choreo/SourceLanePGCSprint.3.traj | 635 +- .../deploy/choreo/SourceLanePGCSprint.4.traj | 779 +- .../deploy/choreo/SourceLanePGCSprint.traj | 3389 +- .../deploy/choreo/SourceLanePGHSprint.1.traj | 356 +- .../deploy/choreo/SourceLanePGHSprint.2.traj | 1580 +- .../deploy/choreo/SourceLanePGHSprint.3.traj | 1508 +- .../deploy/choreo/SourceLanePGHSprint.4.traj | 689 +- .../deploy/choreo/SourceLanePGHSprint.traj | 4064 +- src/main/deploy/choreo/SourceLanePHG.1.traj | 760 - src/main/deploy/choreo/SourceLanePHG.2.traj | 751 - src/main/deploy/choreo/SourceLanePHG.traj | 1498 - .../deploy/choreo/SourceLanePHGFSprint.1.traj | 427 - .../deploy/choreo/SourceLanePHGFSprint.2.traj | 400 - .../deploy/choreo/SourceLanePHGFSprint.3.traj | 355 - .../deploy/choreo/SourceLanePHGFSprint.4.traj | 193 - .../choreo/SourceLanePHGSprint (1).1.traj | 184 - .../choreo/SourceLanePHGSprint (1).2.traj | 814 - .../choreo/SourceLanePHGSprint (1).3.traj | 787 - .../choreo/SourceLanePHGSprint (1).4.traj | 355 - .../choreo/SourceLanePHGSprint (1).traj | 2101 - .../deploy/choreo/SourceLanePHGSprint.1.traj | 184 - .../deploy/choreo/SourceLanePHGSprint.2.traj | 841 - .../deploy/choreo/SourceLanePHGSprint.3.traj | 319 - .../deploy/choreo/SourceLanePHGSprint.4.traj | 391 - .../choreo/SourceLanePSubHSubGSub.1.traj | 1805 +- .../choreo/SourceLanePSubHSubGSub.2.traj | 1859 +- .../deploy/choreo/SourceLanePSubHSubGSub.traj | 3641 +- .../choreo/SourceLanePSubHSubSprint.1.traj | 1805 +- .../choreo/SourceLanePSubHSubSprint.2.traj | 887 +- .../choreo/SourceLanePSubHSubSprint.traj | 2669 +- .../deploy/choreo/SourceLaneSprint.1.traj | 238 - src/main/deploy/choreo/SourceLaneSprint.traj | 238 - src/main/deploy/choreo/SourceLaneTaxi.1.traj | 139 - src/main/deploy/choreo/SourceLaneTaxi.traj | 139 - src/main/deploy/choreo/Sprint.1.traj | 239 + src/main/deploy/choreo/Sprint.traj | 239 + src/main/deploy/choreo/StraightForward.1.traj | 634 - src/main/deploy/choreo/StraightForward.2.traj | 634 - src/main/deploy/choreo/StraightForward.3.traj | 454 - src/main/deploy/choreo/StraightForward.4.traj | 454 - src/main/deploy/choreo/StraightForward.5.traj | 535 - src/main/deploy/choreo/StraightForward.6.traj | 535 - src/main/deploy/choreo/Tag14.1.traj | 230 +- src/main/deploy/choreo/Tag14.2.traj | 104 +- src/main/deploy/choreo/Tag14.traj | 311 +- src/main/deploy/choreo/Tag15.1.traj | 230 +- src/main/deploy/choreo/Tag15.2.traj | 104 +- src/main/deploy/choreo/Tag15.traj | 311 +- src/main/deploy/choreo/Tag16.1.traj | 230 +- src/main/deploy/choreo/Tag16.2.traj | 104 +- src/main/deploy/choreo/Tag16.traj | 311 +- src/main/deploy/choreo/Taxi.1.traj | 140 + src/main/deploy/choreo/Taxi.traj | 140 + src/main/deploy/pathplanner/navgrid.json | 2 +- .../team1540/robot2024/RobotContainer.java | 49 +- .../robot2024/commands/autos/ATestAuto.java | 58 + .../commands/autos/AmpLanePABCSprint.java | 37 - .../commands/autos/AmpLanePSprint.java | 24 - .../commands/autos/CenterLaneSprintBonus.java | 26 - .../commands/autos/PathVisualising.java | 21 - .../commands/autos/SourceLanePHG.java | 32 - .../commands/autos/SourceLanePHGFSprint.java | 26 - .../commands/autos/StraightForward.java | 32 - .../commands/shooter/AutoShootPrepare.java | 10 +- .../commands/shooter/LeadingShootPrepare.java | 43 + .../shooter/OverStageShootPrepare.java | 8 +- 171 files changed, 69269 insertions(+), 101461 deletions(-) delete mode 100644 src/main/deploy/choreo/AmpLanePABCSprint.1.traj delete mode 100644 src/main/deploy/choreo/AmpLanePABCSprint.2.traj delete mode 100644 src/main/deploy/choreo/AmpLanePABCSprint.3.traj delete mode 100644 src/main/deploy/choreo/AmpLanePABCSprint.4.traj delete mode 100644 src/main/deploy/choreo/AmpLanePADEf.1.traj delete mode 100644 src/main/deploy/choreo/AmpLanePADEf.2.traj delete mode 100644 src/main/deploy/choreo/AmpLanePADEf.3.traj delete mode 100644 src/main/deploy/choreo/AmpLanePADEf.4.traj delete mode 100644 src/main/deploy/choreo/AmpLanePADEf.5.traj delete mode 100644 src/main/deploy/choreo/AmpLanePADEf.traj delete mode 100644 src/main/deploy/choreo/AmpLanePAmpAAmpSprint.1.traj delete mode 100644 src/main/deploy/choreo/AmpLanePAmpAAmpSprint.traj delete mode 100644 src/main/deploy/choreo/AmpLanePSprint.1.traj delete mode 100644 src/main/deploy/choreo/AmpLanePSprint.2.traj delete mode 100644 src/main/deploy/choreo/AmpLaneSprint.1.traj delete mode 100644 src/main/deploy/choreo/AmpLaneSprint.traj delete mode 100644 src/main/deploy/choreo/AmpLaneTaxi.1.traj delete mode 100644 src/main/deploy/choreo/AmpLaneTaxi.traj delete mode 100644 src/main/deploy/choreo/CenterLanePBc.1.traj delete mode 100644 src/main/deploy/choreo/CenterLanePBc.2.traj delete mode 100644 src/main/deploy/choreo/CenterLanePBc.3.traj delete mode 100644 src/main/deploy/choreo/CenterLanePBc.4.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBAD.1.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBAD.2.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBAD.3.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBAD.4.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBAD.5.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBAD.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBADEF.1.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBADEF.2.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBADEF.3.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBADEF.4.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBADEF.5.traj delete mode 100644 src/main/deploy/choreo/CenterLanePCBADEF.6.traj delete mode 100644 src/main/deploy/choreo/CenterLanePSubCSubBSubAS.1.traj delete mode 100644 src/main/deploy/choreo/CenterLanePSubCSubBSubAS.2.traj delete mode 100644 src/main/deploy/choreo/CenterLanePSubCSubBSubAS.3.traj delete mode 100644 src/main/deploy/choreo/CenterLanePSubCSubBSubAS.4.traj delete mode 100644 src/main/deploy/choreo/CenterLaneSprintBonus.1.traj delete mode 100644 src/main/deploy/choreo/CenterLaneSprintBonus.2.traj delete mode 100644 src/main/deploy/choreo/CenterLaneTaxi.1.traj delete mode 100644 src/main/deploy/choreo/CenterLaneTaxi.traj delete mode 100644 src/main/deploy/choreo/New Path (1).1.traj delete mode 100644 src/main/deploy/choreo/New Path (1).2.traj delete mode 100644 src/main/deploy/choreo/SQUARFE (1).1.traj delete mode 100644 src/main/deploy/choreo/SQUARFE (1).2.traj delete mode 100644 src/main/deploy/choreo/SQUARFE (1).3.traj delete mode 100644 src/main/deploy/choreo/SQUARFE (1).4.traj delete mode 100644 src/main/deploy/choreo/SQUARFE.1.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHG.1.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHG.2.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHG.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGFSprint.1.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGFSprint.2.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGFSprint.3.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGFSprint.4.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGSprint (1).1.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGSprint (1).2.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGSprint (1).3.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGSprint (1).4.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGSprint (1).traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGSprint.1.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGSprint.2.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGSprint.3.traj delete mode 100644 src/main/deploy/choreo/SourceLanePHGSprint.4.traj delete mode 100644 src/main/deploy/choreo/SourceLaneSprint.1.traj delete mode 100644 src/main/deploy/choreo/SourceLaneSprint.traj delete mode 100644 src/main/deploy/choreo/SourceLaneTaxi.1.traj delete mode 100644 src/main/deploy/choreo/SourceLaneTaxi.traj create mode 100644 src/main/deploy/choreo/Sprint.1.traj create mode 100644 src/main/deploy/choreo/Sprint.traj delete mode 100644 src/main/deploy/choreo/StraightForward.1.traj delete mode 100644 src/main/deploy/choreo/StraightForward.2.traj delete mode 100644 src/main/deploy/choreo/StraightForward.3.traj delete mode 100644 src/main/deploy/choreo/StraightForward.4.traj delete mode 100644 src/main/deploy/choreo/StraightForward.5.traj delete mode 100644 src/main/deploy/choreo/StraightForward.6.traj create mode 100644 src/main/deploy/choreo/Taxi.1.traj create mode 100644 src/main/deploy/choreo/Taxi.traj create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/ATestAuto.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/autos/AmpLanePSprint.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/autos/CenterLaneSprintBonus.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/autos/PathVisualising.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGFSprint.java delete mode 100644 src/main/java/org/team1540/robot2024/commands/autos/StraightForward.java create mode 100644 src/main/java/org/team1540/robot2024/commands/shooter/LeadingShootPrepare.java diff --git a/paths.chor b/paths.chor index 2db800e9..f4fa6b27 100644 --- a/paths.chor +++ b/paths.chor @@ -1,9 +1,9 @@ { - "version": "v0.2.2", + "version": "v0.3", "robotConfiguration": { "mass": 61.73998083590995, "rotationalInertia": 3.6816072891155396, - "motorMaxTorque": 0.6, + "motorMaxTorque": 0.5, "motorMaxVelocity": 4864, "gearing": 6.746, "wheelbase": 0.4762497428251389, @@ -13,7 +13,7 @@ "wheelRadius": 0.0500888 }, "paths": { - "SourceLaneTaxi": { + "Taxi": { "waypoints": [ { "x": 1.468971848487854, @@ -40,338 +40,143 @@ "y": 1.9215065240859983, "heading": 0, "angularVelocity": 0, - "velocityX": 0, - "velocityY": -6.616666962933087e-41, + "velocityX": -2.4295253407291496e-19, + "velocityY": 0, "timestamp": 0 }, { "x": 1.5097881789229493, "y": 1.9215065240859983, - "heading": 2.3748244992157107e-19, - "angularVelocity": 2.455567735557075e-18, - "velocityX": 0.42198942031848213, - "velocityY": 1.4827190365964012e-17, - "timestamp": 0.09672358706123588 - }, - { - "x": 1.5914208389649156, - "y": 1.9215065240859985, - "heading": 6.823321419293863e-18, - "angularVelocity": 6.808983664986858e-17, - "velocityX": 0.843978832074171, - "velocityY": 2.965467782674968e-17, - "timestamp": 0.19344717412247175 + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.4219894192635095, + "velocityY": 0, + "timestamp": 0.09672358730304476 }, { - "x": 1.7138698274174293, - "y": 1.9215065240859985, - "heading": 1.3350406511427553e-17, - "angularVelocity": 6.748261205621051e-17, - "velocityX": 1.2659682314613812, - "velocityY": 4.448259121719168e-17, - "timestamp": 0.29017076118370766 + "x": 1.5914208389651578, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.8439788299642234, + "velocityY": 0, + "timestamp": 0.1934471746060895 }, { - "x": 1.8771351424005533, - "y": 1.9215065240859985, - "heading": 2.3094752603641018e-17, - "angularVelocity": 1.0074517896422799e-16, - "velocityX": 1.6879576114124106, - "velocityY": 5.931116840042e-17, - "timestamp": 0.3868943482449435 + "x": 1.7138698274174076, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.2659682282964586, + "velocityY": 0, + "timestamp": 0.29017076190913427 }, { - "x": 2.081216780530401, - "y": 1.9215065240859985, - "heading": 2.7994689255823954e-17, - "angularVelocity": 5.0660149601833395e-17, - "velocityX": 2.1099469563783173, - "velocityY": 7.414094299456716e-17, - "timestamp": 0.48361793530617936 + "x": 1.877135142400635, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.6879576071925144, + "velocityY": 0, + "timestamp": 0.386894349212179 }, { - "x": 2.326114733911239, - "y": 1.9215065240859985, - "heading": 3.194288143588596e-17, - "angularVelocity": 4.082043898429076e-17, - "velocityX": 2.531936219712287, - "velocityY": 8.89740008515513e-17, - "timestamp": 0.5803415223674152 + "x": 2.0812167805304824, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.109946951103447, + "velocityY": 0, + "timestamp": 0.4836179365152238 }, { - "x": 2.6118289630644647, - "y": 1.9215065240859985, - "heading": 2.800063510694937e-17, - "angularVelocity": -4.0776590509092634e-17, - "velocityX": 2.953925074887259, - "velocityY": 1.036106690795216e-16, - "timestamp": 0.6770651094286511 + "x": 2.326114733911371, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.53193621338244, + "velocityY": 0, + "timestamp": 0.5803415238182685 }, { - "x": 2.8567269164453046, - "y": 1.9215065240859985, - "heading": 2.2632732501440496e-17, - "angularVelocity": -5.54925246223202e-17, - "velocityX": 2.531936219712306, - "velocityY": 8.894540117079268e-17, - "timestamp": 0.7737886964898869 + "x": 2.611828963064364, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.9539250675023654, + "velocityY": 0, + "timestamp": 0.6770651111213133 }, { - "x": 3.0608085545751536, - "y": 1.9215065240859985, - "heading": 1.7892029047767983e-17, - "angularVelocity": -4.9009641288663106e-17, - "velocityX": 2.109946956378327, - "velocityY": 7.412152260849424e-17, - "timestamp": 0.8705122835511228 + "x": 2.856726916445444, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.5319362133824383, + "velocityY": 0, + "timestamp": 0.773788698424358 }, { - "x": 3.224073869558278, - "y": 1.9215065240859985, - "heading": 2.083425694040062e-17, - "angularVelocity": 3.042138426747003e-17, - "velocityX": 1.687957611412416, - "velocityY": 5.929781651761968e-17, - "timestamp": 0.9672358706123586 + "x": 3.0608085545750643, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.109946951103445, + "velocityY": 0, + "timestamp": 0.8705122857274028 }, { - "x": 3.3465228580107924, - "y": 1.9215065240859985, - "heading": 1.131382345905398e-17, - "angularVelocity": -9.842749204538218e-17, - "velocityX": 1.265968231461384, - "velocityY": 4.4473700308316656e-17, - "timestamp": 1.0639594576735945 + "x": 3.2240738695580573, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.6879576071925126, + "velocityY": 0, + "timestamp": 0.9672358730304476 }, { - "x": 3.4281555180527588, + "x": 3.3465228580110096, "y": 1.9215065240859983, - "heading": -4.743251660877572e-19, - "angularVelocity": -1.2187342421598578e-16, - "velocityX": 0.8439788320741727, - "velocityY": 2.964930533250184e-17, - "timestamp": 1.1606830447348304 + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.265968228296457, + "velocityY": 0, + "timestamp": 1.0639594603334923 }, { - "x": 3.468971848487854, + "x": 3.428155518052592, "y": 1.9215065240859983, - "heading": -3.025400677588882e-41, - "angularVelocity": 4.904504421960316e-18, - "velocityX": 0.42198942031848286, - "velocityY": 1.4824720643168207e-17, - "timestamp": 1.2574066317960664 + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.8439788299642216, + "velocityY": 0, + "timestamp": 1.160683047636537 }, { "x": 3.468971848487854, "y": 1.9215065240859983, - "heading": 4.753747394144705e-41, - "angularVelocity": 8.043397133285877e-40, - "velocityX": -3.5738155514754405e-40, - "velocityY": 2.012790081694559e-42, - "timestamp": 1.3541302188573023 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "CenterLaneTaxi": { - "waypoints": [ - { - "x": 1.4490834474563599, - "y": 5.489123821258545, "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 + "angularVelocity": 0, + "velocityX": 0.42198941926350725, + "velocityY": 0, + "timestamp": 1.2574066349395818 }, { - "x": 3.3202035427093506, - "y": 5.489123821258545, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.44908344745636, - "y": 5.489123821258545, + "x": 3.468971848487854, + "y": 1.9215065240859983, "heading": 0, "angularVelocity": 0, - "velocityX": 0, - "velocityY": -2.0318283346913183e-41, - "timestamp": 0 - }, - { - "x": 1.4872695756258638, - "y": 5.489123821258545, - "heading": 2.754950983259061e-24, - "angularVelocity": 2.9447241753048086e-23, - "velocityX": 0.40816653670481284, - "velocityY": 5.876249720718061e-34, - "timestamp": 0.09355526417669109 - }, - { - "x": 1.5636418311637772, - "y": 5.489123821258545, - "heading": 4.052554305374892e-24, - "angularVelocity": 1.3869873668873725e-23, - "velocityX": 0.8163330648468328, - "velocityY": 2.351179297170128e-34, - "timestamp": 0.18711052835338218 - }, - { - "x": 1.6782002129129638, - "y": 5.489123821258545, - "heading": -2.072394992605464e-24, - "angularVelocity": -6.546869835666046e-23, - "velocityX": 1.2244995806203742, - "velocityY": -1.9003523177361593e-34, - "timestamp": 0.2806657925300733 - }, - { - "x": 1.8309447190550667, - "y": 5.489123821258545, - "heading": -2.4578798145232008e-23, - "angularVelocity": -2.4056824153148397e-22, - "velocityX": 1.632666076957736, - "velocityY": 3.860759321784845e-34, - "timestamp": 0.37422105670676437 - }, - { - "x": 2.0218753463170436, - "y": 5.489123821258545, - "heading": -7.973182418935717e-23, - "angularVelocity": -5.895236107850817e-22, - "velocityX": 2.0408325383099775, - "velocityY": 1.2704796254278893e-34, - "timestamp": 0.46777632088345544 - }, - { - "x": 2.250992087061798, - "y": 5.489123821258545, - "heading": -2.0707507158653019e-22, - "angularVelocity": -1.3611552826942191e-21, - "velocityX": 2.4489989180302882, - "velocityY": -1.7750165055103215e-35, - "timestamp": 0.5613315850601466 - }, - { - "x": 2.5182949031039126, - "y": 5.489123821258545, - "heading": -2.160490651108523e-22, - "angularVelocity": -9.592193197806397e-23, - "velocityX": 2.8571648895916577, - "velocityY": 2.8715559113727715e-34, - "timestamp": 0.6548868492368377 - }, - { - "x": 2.747411643848667, - "y": 5.489123821258545, - "heading": -1.6667699647956365e-22, - "angularVelocity": 5.277318832756414e-22, - "velocityX": 2.448998918030288, - "velocityY": 2.947986532686387e-35, - "timestamp": 0.7484421134135288 - }, - { - "x": 2.938342271110644, - "y": 5.489123821258545, - "heading": -1.1298656729823459e-22, - "angularVelocity": 5.738899818074355e-22, - "velocityX": 2.0408325383099775, - "velocityY": -2.8596027354894936e-34, - "timestamp": 0.84199737759022 - }, - { - "x": 3.0910867772527473, - "y": 5.489123821258545, - "heading": -6.615705102278205e-23, - "angularVelocity": 5.00554598927309e-22, - "velocityX": 1.632666076957736, - "velocityY": 3.1581487960600027e-34, - "timestamp": 0.9355526417669111 - }, - { - "x": 3.2056451590019335, - "y": 5.489123821258545, - "heading": -3.1148929262698523e-23, - "angularVelocity": 3.7419731471853073e-22, - "velocityX": 1.2244995806203742, - "velocityY": 5.822081879901906e-34, - "timestamp": 1.0291079059436021 - }, - { - "x": 3.2820174145398466, - "y": 5.489123821258545, - "heading": -1.119480593141529e-23, - "angularVelocity": 2.1328705390671606e-22, - "velocityX": 0.8163330648468328, - "velocityY": 1.1078164340378654e-33, - "timestamp": 1.1226631701202932 - }, - { - "x": 3.3202035427093506, - "y": 5.489123821258545, - "heading": 3.079038293636101e-40, - "angularVelocity": 1.1965981728795564e-22, - "velocityX": 0.40816653670481284, - "velocityY": 4.557991704503881e-34, - "timestamp": 1.2162184342969842 - }, - { - "x": 3.3202035427093506, - "y": 5.489123821258545, - "heading": 1.1605365063908143e-41, - "angularVelocity": -3.167017137026152e-39, - "velocityX": -4.508017624963005e-40, - "velocityY": -1.70250903211491e-50, - "timestamp": 1.3097736984736752 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" + "velocityX": -3.3284599459506804e-19, + "velocityY": 0, + "timestamp": 1.3541302222426266 } ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "AmpLaneTaxi": { - "waypoints": [ + "trajectoryWaypoints": [ { + "timestamp": 0, + "isStopPoint": true, "x": 1.468971848487854, - "y": 7.309329509735107, + "y": 1.9215065240859985, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -379,8 +184,10 @@ "controlIntervalCount": 14 }, { + "timestamp": 1.3541302222426266, + "isStopPoint": true, "x": 3.468971848487854, - "y": 7.309329509735107, + "y": 1.9215065240859985, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -388,163 +195,30 @@ "controlIntervalCount": 40 } ], - "trajectory": [ - { - "x": 1.468971848487854, - "y": 7.309329509735107, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -5.968004408679696e-41, - "timestamp": 0 - }, - { - "x": 1.5097881789229493, - "y": 7.309329509735107, - "heading": 1.867923413309381e-18, - "angularVelocity": 1.9312421328909037e-17, - "velocityX": 0.42198942031848213, - "velocityY": -8.175734433695576e-17, - "timestamp": 0.09672358706123589 - }, - { - "x": 1.5914208389649154, - "y": 7.309329509735107, - "heading": -2.7784523382293792e-18, - "angularVelocity": -4.803678645072435e-17, - "velocityX": 0.843978832074171, - "velocityY": -1.635143003079991e-16, - "timestamp": 0.19344717412247178 - }, - { - "x": 1.713869827417429, - "y": 7.309329509735108, - "heading": -6.149022858043543e-18, - "angularVelocity": -3.4846149955338706e-17, - "velocityX": 1.2659682314613812, - "velocityY": -2.4527069236460427e-16, - "timestamp": 0.29017076118370766 - }, - { - "x": 1.8771351424005531, - "y": 7.309329509735107, - "heading": 9.334684137308459e-20, - "angularVelocity": 6.453994057032915e-17, - "velocityX": 1.6879576114124106, - "velocityY": -3.2702618744361184e-16, - "timestamp": 0.38689434824494356 - }, - { - "x": 2.081216780530401, - "y": 7.309329509735108, - "heading": 7.900764051063292e-18, - "angularVelocity": 8.072096604598862e-17, - "velocityX": 2.1099469563783173, - "velocityY": -4.087800036020465e-16, - "timestamp": 0.48361793530617947 - }, - { - "x": 2.326114733911239, - "y": 7.309329509735108, - "heading": -4.0292085158169106e-19, - "angularVelocity": -8.584682780302335e-17, - "velocityX": 2.5319362197122866, - "velocityY": -4.905289237309452e-16, - "timestamp": 0.5803415223674153 - }, - { - "x": 2.611828963064465, - "y": 7.309329509735108, - "heading": 4.895920849005717e-18, - "angularVelocity": 5.476420787705161e-17, - "velocityX": 2.9539250748872585, - "velocityY": -5.724892251787591e-16, - "timestamp": 0.6770651094286512 - }, - { - "x": 2.856726916445305, - "y": 7.309329509735108, - "heading": 1.0580912487609066e-17, - "angularVelocity": 5.877889823861001e-17, - "velocityX": 2.531936219712306, - "velocityY": -4.90569760600763e-16, - "timestamp": 0.773788696489887 - }, - { - "x": 3.0608085545751536, - "y": 7.309329509735108, - "heading": 1.2560262621019845e-17, - "angularVelocity": 2.0466206900583654e-17, - "velocityX": 2.109946956378327, - "velocityY": -4.088065981186001e-16, - "timestamp": 0.8705122835511229 - }, - { - "x": 3.224073869558278, - "y": 7.309329509735107, - "heading": 1.163556067743937e-17, - "angularVelocity": -9.558506562343336e-18, - "velocityX": 1.687957611412416, - "velocityY": -3.270442505924889e-16, - "timestamp": 0.9672358706123587 - }, - { - "x": 3.3465228580107924, - "y": 7.309329509735107, - "heading": 1.1661599473339483e-17, - "angularVelocity": 2.705282706656373e-19, - "velocityX": 1.2659682314613843, - "velocityY": -2.4528264910978726e-16, - "timestamp": 1.0639594576735947 - }, - { - "x": 3.4281555180527588, - "y": 7.309329509735107, - "heading": 1.4643178138706357e-18, - "angularVelocity": -1.0542614870618343e-16, - "velocityX": 0.8439788320741728, - "velocityY": -1.6352149946538548e-16, - "timestamp": 1.1606830447348306 - }, - { - "x": 3.468971848487854, - "y": 7.309329509735107, - "heading": -3.620479666148997e-40, - "angularVelocity": -1.5138749754669695e-17, - "velocityX": 0.42198942031848286, - "velocityY": -8.176064575101043e-17, - "timestamp": 1.2574066317960666 - }, - { - "x": 3.468971848487854, - "y": 7.309329509735107, - "heading": -2.966209034098039e-40, - "angularVelocity": 6.774689529655474e-40, - "velocityX": 3.003387584304273e-39, - "velocityY": 2.5761438166078746e-41, - "timestamp": 1.3541302188573026 - } - ], "constraints": [ { "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, - "SourceLaneSprint": { + "Sprint": { "waypoints": [ { "x": 1.493931531906128, @@ -569,236 +243,260 @@ { "x": 1.493931531906128, "y": 1.5100655555725098, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.82118644197349e-37, - "velocityY": 0, + "heading": -2.3489391793091186e-30, + "angularVelocity": -1.8438286181159006e-30, + "velocityX": -6.15674840421599e-23, + "velocityY": -1.1532892177192752e-40, "timestamp": 0 }, { - "x": 1.5360008147282442, + "x": 1.536000814827162, + "y": 1.5100655555725098, + "heading": -8.185103677237685e-20, + "angularVelocity": -8.335395549473075e-19, + "velocityX": 0.42841743669638827, + "velocityY": -2.1388205270259975e-34, + "timestamp": 0.09819694372254935 + }, + { + "x": 1.62013938001091, "y": 1.5100655555725098, - "heading": 3.2333963174943666e-19, - "angularVelocity": 3.292766805519721e-18, - "velocityX": 0.4284174372625368, - "velocityY": 1.8392808793575992e-26, - "timestamp": 0.09819694336189172 + "heading": -2.4554391273005915e-19, + "angularVelocity": -1.6669854452684527e-18, + "velocityX": 0.8568348666886964, + "velocityY": 1.6301517969798743e-35, + "timestamp": 0.1963938874450987 }, { - "x": 1.6201393797142145, + "x": 1.7463472265796118, "y": 1.5100655555725098, - "heading": 7.027380416999147e-19, - "angularVelocity": 3.863647922469736e-18, - "velocityX": 0.8568348678215871, - "velocityY": 3.963342552313322e-26, - "timestamp": 0.19639388672378344 + "heading": -4.910663826735234e-19, + "angularVelocity": -2.5003066351518114e-18, + "velocityX": 1.2852522877422325, + "velocityY": -6.448425500517587e-35, + "timestamp": 0.29459083116764806 }, { - "x": 1.746347225986537, + "x": 1.9146243533044036, "y": 1.5100655555725098, - "heading": 8.361393183209098e-19, - "angularVelocity": 1.3585074996311424e-18, - "velocityX": 1.2852522894444904, - "velocityY": 6.468173032965746e-26, - "timestamp": 0.29459083008567516 + "heading": -8.184016386536964e-19, + "angularVelocity": -3.3334566593304657e-18, + "velocityX": 1.7136696962814904, + "velocityY": 1.7221012737740448e-34, + "timestamp": 0.3927877748901974 }, { - "x": 1.9146243523169562, + "x": 2.12497075834199, "y": 1.5100655555725098, - "heading": 3.80235401897157e-19, - "angularVelocity": -4.642750516315599e-18, - "velocityX": 1.7136696985593167, - "velocityY": 9.506575448004094e-26, - "timestamp": 0.3927877734475669 + "heading": -1.2275245556703786e-18, + "angularVelocity": -4.1663508201467245e-18, + "velocityX": 2.1420870860493375, + "velocityY": 1.0129663973735025e-34, + "timestamp": 0.49098471861274673 }, { - "x": 2.12497075686256, + "x": 2.3773864386202144, "y": 1.5100655555725098, - "heading": -1.0623970199278788e-18, - "angularVelocity": -1.4691215100259113e-17, - "velocityX": 2.1420870889066284, - "velocityY": 1.3343481989110454e-25, - "timestamp": 0.4909847168094586 + "heading": -1.7183931876154327e-18, + "angularVelocity": -4.9988178178910575e-18, + "velocityX": 2.570504444531517, + "velocityY": 3.8560644096510287e-35, + "timestamp": 0.5891816623352961 }, { - "x": 2.3773864365383615, + "x": 2.6718713879947695, "y": 1.5100655555725098, - "heading": -3.9564889545725385e-18, - "angularVelocity": -2.947232200024047e-17, - "velocityX": 2.5705044478376236, - "velocityY": 1.8500903675379378e-25, - "timestamp": 0.5891816601713503 + "heading": -2.290925793320508e-18, + "angularVelocity": -5.830452394938298e-18, + "velocityX": 2.9989217404424307, + "velocityY": -1.57638284621387e-34, + "timestamp": 0.6873786060578455 }, { - "x": 2.6718713848686027, + "x": 3.008425588032791, "y": 1.5100655555725098, - "heading": -8.853646546888228e-18, - "angularVelocity": -4.987077429157769e-17, - "velocityX": 2.9989217408219764, - "velocityY": 2.625793984498987e-25, - "timestamp": 0.6873786035332421 + "heading": -2.9448687714922995e-18, + "angularVelocity": -6.6595043937130054e-18, + "velocityX": 3.4273388486401246, + "velocityY": -2.5130241601160745e-34, + "timestamp": 0.7855755497803949 }, { - "x": 3.0084255847706767, + "x": 3.379802074000998, "y": 1.5100655555725098, - "heading": -1.6413613329692498e-17, - "angularVelocity": -7.698780165331962e-17, - "velocityX": 3.4273388598436147, - "velocityY": 4.161152605292971e-25, - "timestamp": 0.7855755468951338 + "heading": -4.375619805389422e-18, + "angularVelocity": -1.4570219598048688e-17, + "velocityX": 3.781955648411144, + "velocityY": 2.8959618409076868e-34, + "timestamp": 0.8837724935029443 }, { - "x": 3.3798020724552167, + "x": 3.7511785599693743, "y": 1.5100655555725098, - "heading": -2.58741769005412e-17, - "angularVelocity": -9.634275005875749e-17, - "velocityX": 3.781955679779987, - "velocityY": -5.487818039619291e-25, - "timestamp": 0.8837724902570254 + "heading": -4.530753712175346e-18, + "angularVelocity": -1.579824187012377e-18, + "velocityX": 3.781955648412874, + "velocityY": 1.4582574743238952e-34, + "timestamp": 0.9819694372254937 }, { - "x": 3.7511785589388484, + "x": 4.122555045937751, "y": 1.5100655555725098, - "heading": 5.146033898598045e-18, - "angularVelocity": 3.158979265252523e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.295977316525143e-24, - "timestamp": 0.9819694336189171 + "heading": -2.578867570214759e-18, + "angularVelocity": 1.9877259596570474e-17, + "velocityX": 3.781955648412874, + "velocityY": 2.230882066085756e-34, + "timestamp": 1.080166380948043 }, { - "x": 4.122555045422488, + "x": 4.493931531906128, "y": 1.5100655555725098, - "heading": 4.0038826563950476e-17, - "angularVelocity": 3.553348146159671e-16, - "velocityX": 3.7819556675504797, - "velocityY": 7.623936784532972e-25, - "timestamp": 1.0801663769808088 + "heading": 2.246937505394666e-18, + "angularVelocity": 4.9144147390644355e-17, + "velocityX": 3.781955648412874, + "velocityY": 2.057712754500175e-33, + "timestamp": 1.1783633246705925 }, { - "x": 4.493931531906129, + "x": 4.865308017874504, "y": 1.5100655555725098, - "heading": 6.570668459860429e-17, - "angularVelocity": 2.613916192895976e-16, - "velocityX": 3.7819556675504797, - "velocityY": 3.4278643163770276e-26, - "timestamp": 1.1783633203427004 + "heading": 5.0874648823324006e-18, + "angularVelocity": 2.8926840991771126e-17, + "velocityX": 3.781955648412874, + "velocityY": -7.440450670385348e-34, + "timestamp": 1.2765602683931419 }, { - "x": 4.865308018389769, + "x": 5.2366845038428815, "y": 1.5100655555725098, - "heading": 6.663782988380241e-17, - "angularVelocity": 9.482426370100795e-18, - "velocityX": 3.7819556675504797, - "velocityY": 2.693381196266241e-25, - "timestamp": 1.276560263704592 + "heading": 4.705898936719953e-18, + "angularVelocity": -3.885721196050614e-18, + "velocityX": 3.781955648412874, + "velocityY": -9.318220497232172e-35, + "timestamp": 1.3747572121156912 }, { - "x": 5.236684504873409, + "x": 5.608060989811258, "y": 1.5100655555725098, - "heading": 5.2326006182614025e-17, - "angularVelocity": -1.4574612214194992e-16, - "velocityX": 3.7819556675504797, - "velocityY": 5.124149697811417e-24, - "timestamp": 1.3747572070664837 + "heading": 7.44292639457605e-18, + "angularVelocity": 2.7872837525287766e-17, + "velocityX": 3.781955648412874, + "velocityY": 9.982777944892104e-32, + "timestamp": 1.4729541558382406 }, { - "x": 5.60806099135704, + "x": 5.979437475779465, "y": 1.5100655555725098, - "heading": 2.884064553605338e-17, - "angularVelocity": -2.3916590315859936e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.1014556301472372e-23, - "timestamp": 1.4729541504283754 + "heading": 2.983432162684063e-18, + "angularVelocity": -4.541377829935033e-17, + "velocityX": 3.781955648411144, + "velocityY": -1.946339553545498e-31, + "timestamp": 1.57115109956079 }, { - "x": 5.9794374790415805, + "x": 6.315991675817486, "y": 1.5100655555725098, - "heading": 1.6502190572837753e-17, - "angularVelocity": -1.2565009195595738e-16, - "velocityX": 3.7819556797799856, - "velocityY": 6.524035237986936e-24, - "timestamp": 1.571151093790267 + "heading": 2.316345923938171e-18, + "angularVelocity": -6.793350316770868e-18, + "velocityX": 3.4273388486401246, + "velocityY": 3.102303151666472e-32, + "timestamp": 1.6693480432833394 }, { - "x": 6.315991678943653, + "x": 6.610476625192041, "y": 1.5100655555725098, - "heading": 9.967918627860597e-18, - "angularVelocity": -6.65425189408073e-17, - "velocityX": 3.427338859843601, - "velocityY": -3.262675265981889e-25, - "timestamp": 1.6693480371521587 + "heading": 1.7355272119353566e-18, + "angularVelocity": -5.9148349223730734e-18, + "velocityX": 2.99892174044243, + "velocityY": 1.9710968904355283e-32, + "timestamp": 1.7675449870058888 }, { - "x": 6.610476627273894, + "x": 6.8628923054702655, "y": 1.5100655555725098, - "heading": 5.2731830670839714e-18, - "angularVelocity": -4.78093860085579e-17, - "velocityX": 2.9989217408219764, - "velocityY": -2.379324733264266e-25, - "timestamp": 1.7675449805140504 + "heading": 1.23879193875581e-18, + "angularVelocity": -5.058561441389415e-18, + "velocityX": 2.570504444531517, + "velocityY": 1.4085581304064316e-32, + "timestamp": 1.8657419307284382 }, { - "x": 6.862892306949696, + "x": 7.073238710507852, "y": 1.5100655555725098, - "heading": 2.2289001785398693e-18, - "angularVelocity": -3.10018090849907e-17, - "velocityX": 2.5705044478376236, - "velocityY": -1.7314309405461854e-25, - "timestamp": 1.865741923875942 + "heading": 8.254145878831398e-19, + "angularVelocity": -4.20967634225236e-18, + "velocityX": 2.1420870860493375, + "velocityY": 1.0374368292665007e-32, + "timestamp": 1.9639388744509876 }, { - "x": 7.0732387114953, + "x": 7.241515837232644, "y": 1.5100655555725098, - "heading": 5.002812433628901e-19, - "angularVelocity": -1.760359206249271e-17, - "velocityX": 2.1420870889066284, - "velocityY": -1.2661949502166272e-25, - "timestamp": 1.9639388672378337 + "heading": 4.95031593430639e-19, + "angularVelocity": -3.364493658617499e-18, + "velocityX": 1.7136696962814904, + "velocityY": 7.695161857259128e-33, + "timestamp": 2.062135818173537 }, { - "x": 7.24151583782572, + "x": 7.367723683801346, "y": 1.5100655555725098, - "heading": -2.663585350994398e-19, - "angularVelocity": -7.807165345874594e-18, - "velocityX": 1.7136696985593165, - "velocityY": -9.096401200333518e-26, - "timestamp": 2.0621358105997256 + "heading": 2.474248388890608e-19, + "angularVelocity": -2.521532189828829e-18, + "velocityX": 1.2852522877422325, + "velocityY": 5.4450246921373414e-33, + "timestamp": 2.160332761896086 }, { - "x": 7.367723684098041, + "x": 7.451862248985094, "y": 1.5100655555725098, - "heading": -4.029353027704155e-19, - "angularVelocity": -1.390845344762092e-18, - "velocityX": 1.2852522894444902, - "velocityY": -6.226126404979919e-26, - "timestamp": 2.1603327539616175 + "heading": 8.244895136962396e-20, + "angularVelocity": -1.6800511427648084e-18, + "velocityX": 0.8568348666886965, + "velocityY": 3.3251578108171654e-33, + "timestamp": 2.2585297056186353 }, { - "x": 7.451862249084011, + "x": 7.493931531906128, "y": 1.5100655555725098, - "heading": -2.1995708883691042e-19, - "angularVelocity": 1.8633799906775463e-18, - "velocityX": 0.8568348678215871, - "velocityY": -3.833262935804246e-26, - "timestamp": 2.2585296973235094 + "heading": 4.9021992998797275e-30, + "angularVelocity": -8.396284877798725e-19, + "velocityX": 0.4284174366963883, + "velocityY": 1.5649544358297725e-33, + "timestamp": 2.3567266493411845 }, { "x": 7.493931531906128, "y": 1.5100655555725098, + "heading": 2.348585897046796e-30, + "angularVelocity": -2.0879214567555354e-30, + "velocityX": -3.9015027280463263e-23, + "velocityY": -1.2968680331891934e-40, + "timestamp": 2.4549235930637336 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.493931531906128, + "y": 1.5100655555725098, "heading": 0, - "angularVelocity": 2.2399586452459634e-18, - "velocityX": 0.42841743726253684, - "velocityY": -1.7862050890834192e-26, - "timestamp": 2.3567266406854013 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 }, { + "timestamp": 2.4549235930637336, + "isStopPoint": true, "x": 7.493931531906128, "y": 1.5100655555725098, "heading": 0, - "angularVelocity": 0, - "velocityX": -2.1810930337522913e-39, - "velocityY": 0, - "timestamp": 2.454923584047293 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -806,19 +504,23 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "CenterLaneSprint": { "waypoints": [ @@ -863,683 +565,453 @@ { "x": 1.2955970764160156, "y": 5.586942195892334, - "heading": 0, - "angularVelocity": -1.1592130893720681e-31, - "velocityX": 0, - "velocityY": 0, + "heading": -1.15998018681278e-25, + "angularVelocity": 7.739922918144294e-25, + "velocityX": 9.282187758357099e-21, + "velocityY": -2.0893158073718473e-20, "timestamp": 0 }, { - "x": 1.3207451593695585, - "y": 5.599086645701892, - "heading": 2.231825504271677e-18, - "angularVelocity": 2.789547782680952e-17, - "velocityX": 0.3143242018214309, - "velocityY": 0.15179266348424167, - "timestamp": 0.08000682991577572 - }, - { - "x": 1.3713809631464564, - "y": 5.62264555119355, - "heading": 4.51507105043022e-18, - "angularVelocity": 2.853816960464376e-17, - "velocityX": 0.6328935145938026, - "velocityY": 0.2944611793396252, - "timestamp": 0.16001365983155144 - }, - { - "x": 1.4479102774130088, - "y": 5.656665804231403, - "heading": 6.004653783100575e-18, - "angularVelocity": 1.8618332407574406e-17, - "velocityX": 0.9565347651837746, - "velocityY": 0.4252168605416788, - "timestamp": 0.24002048974732715 - }, - { - "x": 1.5508175668842004, - "y": 5.699857537973527, - "heading": 7.57580271600204e-18, - "angularVelocity": 1.963779951822175e-17, - "velocityX": 1.2862313077471652, - "velocityY": 0.5398505826146306, - "timestamp": 0.3200273196631029 - }, - { - "x": 1.6806682557862314, - "y": 5.750395481752386, - "heading": 9.251664351913254e-18, - "angularVelocity": 2.0946531612097715e-17, - "velocityX": 1.6229950497837156, - "velocityY": 0.6316703690584408, - "timestamp": 0.4000341495788786 - }, - { - "x": 1.8380612948775077, - "y": 5.805552130674172, - "heading": 1.2501573477685283e-17, - "angularVelocity": 4.062041901915404e-17, - "velocityX": 1.9672450371704346, - "velocityY": 0.6893992548031757, - "timestamp": 0.48004097949465435 - }, - { - "x": 2.0233797317677533, - "y": 5.861001231409725, - "heading": 1.6054120788359745e-17, - "angularVelocity": 4.440302445729553e-17, - "velocityX": 2.316282710935199, - "velocityY": 0.6930545904033714, - "timestamp": 0.5600478094104301 - }, - { - "x": 2.235784948485807, - "y": 5.909651835150332, - "heading": 2.213686509511287e-17, - "angularVelocity": 7.602767686101118e-17, - "velocityX": 2.6548385549342215, - "velocityY": 0.6080806325227727, - "timestamp": 0.6400546393262058 - }, - { - "x": 2.4702985640404234, - "y": 5.941239855854083, - "heading": 2.864477544319077e-17, - "angularVelocity": 8.134175236038742e-17, - "velocityX": 2.9311699488835434, - "velocityY": 0.39481655174808844, - "timestamp": 0.7200614692419814 - }, - { - "x": 2.715877888033922, - "y": 5.947186832463751, - "heading": 3.119004227784184e-17, - "angularVelocity": 3.1813081703857335e-17, - "velocityX": 3.0694794963261316, - "velocityY": 0.07433086169371976, - "timestamp": 0.8000682991577571 - }, - { - "x": 2.961603624456153, - "y": 5.925207266559582, - "heading": 3.402854212881078e-17, - "angularVelocity": 3.5478294907307905e-17, - "velocityX": 3.0713094954425415, - "velocityY": -0.2747211197853527, - "timestamp": 0.8800751290735328 - }, - { - "x": 3.2005883629277774, - "y": 5.876126550770939, - "heading": 3.9287160074963035e-17, - "angularVelocity": 6.572712324724996e-17, - "velocityX": 2.987054214261441, - "velocityY": -0.6134565741588879, - "timestamp": 0.9600819589893085 - }, - { - "x": 3.428817982052201, - "y": 5.801272963497273, - "heading": 4.077287726704947e-17, - "angularVelocity": 1.856989731461567e-17, - "velocityX": 2.8526266990269726, - "velocityY": -0.9355899659232499, - "timestamp": 1.0400887889050843 - }, - { - "x": 3.6438287356883494, - "y": 5.701819067812634, - "heading": 4.10496280483666e-17, - "angularVelocity": 3.459161663925654e-18, - "velocityX": 2.6874049860525546, - "velocityY": -1.243067570484904, - "timestamp": 1.12009561882086 - }, - { - "x": 3.8440012077207406, - "y": 5.578706367507132, - "heading": 4.4578411240497065e-17, - "angularVelocity": 4.41060783161696e-17, - "velocityX": 2.5019422996871006, - "velocityY": -1.538777382333539, - "timestamp": 1.2001024487366356 + "x": 1.320745159351232, + "y": 5.599086645641333, + "heading": -1.049092437408357e-17, + "angularVelocity": -1.311255590411248e-16, + "velocityX": 0.3143242010468075, + "velocityY": 0.15179266246022666, + "timestamp": 0.08000683005465262 + }, + { + "x": 1.371380963088836, + "y": 5.622645551014504, + "heading": -4.3101779756853274e-17, + "angularVelocity": -4.0760132341640306e-16, + "velocityX": 0.6328935130040201, + "velocityY": 0.2944611773403811, + "timestamp": 0.16001366010930523 + }, + { + "x": 1.4479102772917398, + "y": 5.656665803879934, + "heading": -1.0786579481334514e-16, + "angularVelocity": -8.094817731725291e-16, + "velocityX": 0.9565347627274442, + "velocityY": 0.4252168576383191, + "timestamp": 0.24002049016395785 + }, + { + "x": 1.5508175666705484, + "y": 5.699857537402172, + "heading": -2.0334854319560128e-16, + "angularVelocity": -1.1934334700391475e-15, + "velocityX": 1.286231304358806, + "velocityY": 0.539850578916034, + "timestamp": 0.32002732021861047 + }, + { + "x": 1.680668255445947, + "y": 5.7503954809246025, + "heading": -3.285516720695047e-16, + "angularVelocity": -1.564906883941498e-15, + "velocityX": 1.6229950453817308, + "velocityY": 0.6316703647414047, + "timestamp": 0.40003415027326306 + }, + { + "x": 1.838061294370242, + "y": 5.80555212957311, + "heading": -4.764293851393569e-16, + "angularVelocity": -1.8483154501568934e-15, + "velocityX": 1.9672450316651602, + "velocityY": 0.689399250174457, + "timestamp": 0.48004098032791565 + }, + { + "x": 2.0233797310506665, + "y": 5.8610012300559005, + "heading": -6.647867232817025e-16, + "angularVelocity": -2.354268154652633e-15, + "velocityX": 2.316282704287029, + "velocityY": 0.6930545860260591, + "timestamp": 0.5600478103825682 + }, + { + "x": 2.2357849475361156, + "y": 5.909651833631349, + "heading": -8.83507324298293e-16, + "angularVelocity": -2.7337773116326468e-15, + "velocityX": 2.6548385474134464, + "velocityY": 0.6080806293941452, + "timestamp": 0.6400546404372208 + }, + { + "x": 2.470298562920597, + "y": 5.9412398543265486, + "heading": -1.1401611802382058e-15, + "angularVelocity": -3.2079035041983383e-15, + "velocityX": 2.931169941669824, + "velocityY": 0.39481655095687995, + "timestamp": 0.7200614704918734 + }, + { + "x": 2.715877886892817, + "y": 5.947186831043097, + "heading": -1.4273357911268484e-15, + "angularVelocity": -3.589381409909839e-15, + "velocityX": 3.069479490744277, + "velocityY": 0.07433086290896584, + "timestamp": 0.800068300546526 + }, + { + "x": 2.961603623411278, + "y": 5.925207265290794, + "heading": -1.7286845121883707e-15, + "angularVelocity": -3.766543528450414e-15, + "velocityX": 3.0713094913347376, + "velocityY": -0.2747211174005281, + "timestamp": 0.8800751306011786 + }, + { + "x": 3.200588362045183, + "y": 5.876126549681864, + "heading": -2.0448770370213645e-15, + "angularVelocity": -3.952075909922286e-15, + "velocityX": 2.9870542111299168, + "velocityY": -0.6134565708379317, + "timestamp": 0.9600819606558312 + }, + { + "x": 3.4288179813677098, + "y": 5.801272962619079, + "heading": -2.3674444528927447e-15, + "angularVelocity": -4.031755779497343e-15, + "velocityX": 2.8526266965785654, + "velocityY": -0.9355899616526571, + "timestamp": 1.0400887907104839 + }, + { + "x": 3.6438287352220136, + "y": 5.701819067182799, + "heading": -2.6977753991710968e-15, + "angularVelocity": -4.128792062826012e-15, + "velocityX": 2.687404984142338, + "velocityY": -1.2430675652106578, + "timestamp": 1.1200956207651365 + }, + { + "x": 3.844001207484155, + "y": 5.578706367168648, + "heading": -3.0187544149547513e-15, + "angularVelocity": -4.0119032807509685e-15, + "velocityX": 2.5019422982438493, + "velocityY": -1.538777376007428, + "timestamp": 1.200102450819789 }, { "x": 4.028205871582031, "y": 5.432682037353516, - "heading": 4.825304787734528e-17, - "angularVelocity": 4.592915652788645e-17, - "velocityX": 2.302361736529145, - "velocityY": -1.825148306820832, - "timestamp": 1.2801092786524113 - }, - { - "x": 4.11697825249087, - "y": 5.353217174122665, - "heading": 5.003311226824959e-17, - "angularVelocity": 4.404060645079727e-17, - "velocityX": 2.1963179706677165, - "velocityY": -1.9660406239470811, - "timestamp": 1.3205280076771686 - }, - { - "x": 4.201472100752748, - "y": 5.268051894541109, - "heading": 5.106612036315502e-17, - "angularVelocity": 2.555768903480938e-17, - "velocityX": 2.0904627706110244, - "velocityY": -2.1070746566406697, - "timestamp": 1.360946736701926 - }, - { - "x": 4.281698714286708, - "y": 5.177177737015351, - "heading": 5.2015291988095576e-17, - "angularVelocity": 2.3483498091432662e-17, - "velocityX": 1.9848870924323019, - "velocityY": -2.2483180376661425, - "timestamp": 1.4013654657266832 - }, - { - "x": 4.357676572309284, - "y": 5.080580936550585, - "heading": 5.3034148842228453e-17, - "angularVelocity": 2.5207570315775117e-17, - "velocityX": 1.8797686086529983, - "velocityY": -2.3899019785048825, - "timestamp": 1.4417841947514405 - }, - { - "x": 4.429441369664381, - "y": 4.978235165876808, - "heading": 5.4156772615538654e-17, - "angularVelocity": 2.7774872934374724e-17, - "velocityX": 1.7755332512056952, - "velocityY": -2.5321372824774984, - "timestamp": 1.4822029237761978 - }, - { - "x": 4.497091014290783, - "y": 4.870069946485337, - "heading": 5.562432201646814e-17, - "angularVelocity": 3.6308680200224936e-17, - "velocityX": 1.6737202346179283, - "velocityY": -2.6761162956262776, - "timestamp": 1.522621652800955 - }, - { - "x": 4.561997626649892, - "y": 4.755326331353034, - "heading": 5.3639840433255375e-17, - "angularVelocity": -4.9098048541470276e-17, - "velocityX": 1.6058548580143424, - "velocityY": -2.8388724212920464, - "timestamp": 1.5630403818257124 - }, - { - "x": 4.631511261933969, - "y": 4.646021074802578, - "heading": 5.11321781778172e-17, - "angularVelocity": -6.20420508556326e-17, - "velocityX": 1.7198372378713005, - "velocityY": -2.704321961318929, - "timestamp": 1.6034591108504697 - }, - { - "x": 4.705480207686577, - "y": 4.542279156484067, - "heading": 4.966571246690934e-17, - "angularVelocity": -3.628181222560111e-17, - "velocityX": 1.8300660989972783, - "velocityY": -2.566679379125897, - "timestamp": 1.643877839875227 - }, - { - "x": 4.783858959652748, - "y": 4.44413671743755, - "heading": 4.8920670981753807e-17, - "angularVelocity": -1.843305932153545e-17, - "velocityX": 1.939169139097368, - "velocityY": -2.4281426312595698, - "timestamp": 1.6842965688999842 - }, - { - "x": 4.86662561684807, - "y": 4.3516109378583465, - "heading": 4.74522093373318e-17, - "angularVelocity": -3.633120321770328e-17, - "velocityX": 2.0477303268199805, - "velocityY": -2.2891808280849295, - "timestamp": 1.7247152979247415 + "heading": -3.3335153284372285e-15, + "angularVelocity": -3.934183957133696e-15, + "velocityX": 2.302361735517395, + "velocityY": -1.8251482994061918, + "timestamp": 1.2801092808744416 + }, + { + "x": 4.116978252667536, + "y": 5.353217174251384, + "heading": -3.489150469085252e-15, + "angularVelocity": -3.850570993843687e-15, + "velocityX": 2.1963179697950594, + "velocityY": -1.9660406160607158, + "timestamp": 1.3205280099957935 + }, + { + "x": 4.201472101100227, + "y": 5.268051894804914, + "heading": -3.639541652943779e-15, + "angularVelocity": -3.720830252304448e-15, + "velocityX": 2.090462769846413, + "velocityY": -2.107074648259462, + "timestamp": 1.3609467391171453 + }, + { + "x": 4.281698714797313, + "y": 5.17717773742197, + "heading": -3.7839469401200426e-15, + "angularVelocity": -3.572733177528651e-15, + "velocityX": 1.9848870917295764, + "velocityY": -2.248318028756004, + "timestamp": 1.401365468238497 + }, + { + "x": 4.357676572972308, + "y": 5.080580937109945, + "heading": -3.920017225673807e-15, + "angularVelocity": -3.366516893435145e-15, + "velocityX": 1.8797686079362514, + "velocityY": -2.3899019690105905, + "timestamp": 1.4417841973598489 + }, + { + "x": 4.429441370463213, + "y": 4.978235166602986, + "heading": -4.042506986372987e-15, + "angularVelocity": -3.0305211076940933e-15, + "velocityX": 1.7755332503265975, + "velocityY": -2.5321372722947273, + "timestamp": 1.4822029264812007 + }, + { + "x": 4.4970910151920265, + "y": 4.870069947402846, + "heading": -4.139505406670724e-15, + "angularVelocity": -2.3998398161117503e-15, + "velocityX": 1.6737202331548666, + "velocityY": -2.6761162844925224, + "timestamp": 1.5226216556025525 + }, + { + "x": 4.561997627464952, + "y": 4.755326332071665, + "heading": -4.1031671192864916e-15, + "angularVelocity": 8.99043679267757e-16, + "velocityX": 1.6058548520428109, + "velocityY": -2.8388724194339274, + "timestamp": 1.5630403847239043 + }, + { + "x": 4.6315112626192185, + "y": 4.646021075355723, + "heading": -3.9748504414902894e-15, + "angularVelocity": 3.174685126320571e-15, + "velocityX": 1.71983723054629, + "velocityY": -2.7043219589554015, + "timestamp": 1.603459113845256 + }, + { + "x": 4.705480208220003, + "y": 4.542279156886889, + "heading": -3.831832674451909e-15, + "angularVelocity": 3.5384047912600144e-15, + "velocityX": 1.8300660908635011, + "velocityY": -2.566679376715867, + "timestamp": 1.6438778429666079 + }, + { + "x": 4.783858960019311, + "y": 4.444136717699662, + "heading": -3.678608964011542e-15, + "angularVelocity": 3.7909100509561506e-15, + "velocityX": 1.9391691303303216, + "velocityY": -2.4281426289422314, + "timestamp": 1.6842965720879597 + }, + { + "x": 4.86662561703615, + "y": 4.351610937986713, + "heading": -3.518654034373506e-15, + "angularVelocity": 3.957447150613264e-15, + "velocityX": 2.047730317505612, + "velocityY": -2.2891808259273896, + "timestamp": 1.7247153012093115 }, { "x": 4.9537672996521, "y": 4.264711856842041, - "heading": 4.682424401401172e-17, - "angularVelocity": -1.5536488446652234e-17, - "velocityX": 2.1559728597813255, - "velocityY": -2.1499706475905587, - "timestamp": 1.7651340269494988 - }, - { - "x": 5.149371577077341, - "y": 4.111086251410353, - "heading": 4.245818870652888e-17, - "angularVelocity": -5.3034667663063016e-17, - "velocityX": 2.376013169452415, - "velocityY": -1.8660965214215293, - "timestamp": 1.847458603234601 - }, - { - "x": 5.361995748730517, - "y": 3.981639454553548, - "heading": 3.8311842606234285e-17, - "angularVelocity": -5.0365753698371845e-17, - "velocityX": 2.582754521741994, - "velocityY": -1.5723955433397911, - "timestamp": 1.9297831795197031 - }, - { - "x": 5.590089166249063, - "y": 3.8773917441987824, - "heading": 3.443521699897252e-17, - "angularVelocity": -4.708934879277126e-17, - "velocityX": 2.7706600848603538, - "velocityY": -1.2663012074886832, - "timestamp": 2.0121077558048053 - }, - { - "x": 5.831330212934442, - "y": 3.799628582145428, - "heading": 2.9264449434168703e-17, - "angularVelocity": -6.280941218977205e-17, - "velocityX": 2.9303648748534723, - "velocityY": -0.9445923145111385, - "timestamp": 2.0944323320899074 - }, - { - "x": 6.082008486894108, - "y": 3.749887377260942, - "heading": 2.126679829101086e-17, - "angularVelocity": -9.714783202978127e-17, - "velocityX": 3.044999261088646, - "velocityY": -0.6042084530518639, - "timestamp": 2.1767569083750096 - }, - { - "x": 6.335839306220575, - "y": 3.729546047320075, - "heading": 1.712845100944366e-17, - "angularVelocity": -5.02686298415513e-17, - "velocityX": 3.0832933587884717, - "velocityY": -0.2470869679456106, - "timestamp": 2.2590814846601117 - }, - { - "x": 6.582471637063415, - "y": 3.7378835035769398, - "heading": 1.3745368058462902e-17, - "angularVelocity": -4.109437406088766e-17, - "velocityX": 2.9958530243527477, - "velocityY": 0.10127542263348481, - "timestamp": 2.341406060945214 - }, - { - "x": 6.809467380291713, - "y": 3.768327415425299, - "heading": 1.1162328016061889e-17, - "angularVelocity": -3.137619156324536e-17, - "velocityX": 2.757326590330181, - "velocityY": 0.36980344415440275, - "timestamp": 2.423730637230316 - }, - { - "x": 7.009276028148806, - "y": 3.8103968563967796, - "heading": 8.26924402312721e-18, - "angularVelocity": -3.5142311707247735e-17, - "velocityX": 2.427083829324394, - "velocityY": 0.5110192225827506, - "timestamp": 2.506055213515418 - }, - { - "x": 7.179701211541178, - "y": 3.8557683077640763, - "heading": 5.91482335399215e-18, - "angularVelocity": -2.8599155256386034e-17, - "velocityX": 2.07016168297691, - "velocityY": 0.5511288781087693, - "timestamp": 2.5883797898005203 - }, - { - "x": 7.320624454413459, - "y": 3.899158119195988, - "heading": 3.995408556984211e-18, - "angularVelocity": -2.3315109304438273e-17, - "velocityX": 1.7118004031318457, - "velocityY": 0.5270578166492144, - "timestamp": 2.6707043660856224 - }, - { - "x": 7.432462903898932, - "y": 3.9372219303084806, - "heading": 2.438823399699254e-18, - "angularVelocity": -1.890781257081445e-17, - "velocityX": 1.35850622660075, - "velocityY": 0.4623626726238319, - "timestamp": 2.7530289423707246 - }, - { - "x": 7.515713681888788, - "y": 3.967733484062364, - "heading": 1.1936770855475268e-18, - "angularVelocity": -1.5124745408572555e-17, - "velocityX": 1.0112506100436387, - "velocityY": 0.37062509315884895, - "timestamp": 2.8353535186558267 - }, - { - "x": 7.570835923487974, - "y": 3.989130376969442, - "heading": 2.2208685009288135e-19, - "angularVelocity": -1.1801843204007997e-17, - "velocityX": 0.6695721264120479, - "velocityY": 0.25990893452889574, - "timestamp": 2.917678094940929 + "heading": -3.353665589188468e-15, + "angularVelocity": 4.081981361358232e-15, + "velocityX": 2.155972849970579, + "velocityY": -2.149970645632314, + "timestamp": 1.7651340303306633 + }, + { + "x": 5.149371576843732, + "y": 4.1110862510516375, + "heading": -3.00177635471192e-15, + "angularVelocity": 4.274403041302851e-15, + "velocityX": 2.3760131588886937, + "velocityY": -1.8660965196719834, + "timestamp": 1.8474586068844063 + }, + { + "x": 5.361995748264174, + "y": 3.9816394538906854, + "heading": -2.6353623927870383e-15, + "angularVelocity": 4.450835884022828e-15, + "velocityX": 2.582754510515282, + "velocityY": -1.5723955418881683, + "timestamp": 1.9297831834381494 + }, + { + "x": 5.590089165557832, + "y": 3.8773917432800733, + "heading": -2.261979917940497e-15, + "angularVelocity": 4.535483155952485e-15, + "velocityX": 2.7706600731162854, + "velocityY": -1.2663012064517525, + "timestamp": 2.0121077599918924 + }, + { + "x": 5.831330212039324, + "y": 3.799628581011004, + "heading": -1.9000074489203716e-15, + "angularVelocity": 4.396885915313072e-15, + "velocityX": 2.9303648628426733, + "velocityY": -0.9445923140375594, + "timestamp": 2.0944323365456357 + }, + { + "x": 6.082008485841778, + "y": 3.749887375942779, + "heading": -1.5532288164739933e-15, + "angularVelocity": 4.21232648197744e-15, + "velocityX": 3.044999249268032, + "velocityY": -0.6042084533014725, + "timestamp": 2.176756913099379 + }, + { + "x": 6.335839305106152, + "y": 3.7295460458524454, + "heading": -1.2319901327901992e-15, + "angularVelocity": 3.9020926184765074e-15, + "velocityX": 3.0832933479915057, + "velocityY": -0.2470869689447997, + "timestamp": 2.259081489653122 + }, + { + "x": 6.582471636042754, + "y": 3.7378835020415453, + "heading": -9.36500814934657e-16, + "angularVelocity": 3.589315196126135e-15, + "velocityX": 2.995853015722435, + "velocityY": 0.10127542148557588, + "timestamp": 2.3414060662068654 + }, + { + "x": 6.8094673794894955, + "y": 3.7683274139859995, + "heading": -6.953816130946725e-16, + "angularVelocity": 2.928880396529573e-15, + "velocityX": 2.757326583982553, + "velocityY": 0.3698034441094732, + "timestamp": 2.4237306427606087 + }, + { + "x": 7.00927602757565, + "y": 3.81039685518788, + "heading": -4.96986443618591e-16, + "angularVelocity": 2.4099108474923954e-15, + "velocityX": 2.427083824181171, + "velocityY": 0.5110192236997284, + "timestamp": 2.506055219314352 + }, + { + "x": 7.179701211157642, + "y": 3.855768306837493, + "heading": -3.462577830221023e-16, + "angularVelocity": 1.8309046086233712e-15, + "velocityX": 2.070161678520555, + "velocityY": 0.5511288797213748, + "timestamp": 2.588379795868095 + }, + { + "x": 7.320624454174356, + "y": 3.899158118548539, + "heading": -2.3018244299305313e-16, + "angularVelocity": 1.409969937133298e-15, + "velocityX": 1.711800399297762, + "velocityY": 0.5270578183020111, + "timestamp": 2.6707043724218384 + }, + { + "x": 7.432462903764418, + "y": 3.93722192990686, + "heading": -1.4070305310406466e-16, + "angularVelocity": 1.0869083608873546e-15, + "velocityX": 1.358506223436839, + "velocityY": 0.46236267408521664, + "timestamp": 2.7530289489755817 + }, + { + "x": 7.515713681825484, + "y": 3.9677334838565543, + "heading": -7.487411399750651e-17, + "angularVelocity": 7.996258056579044e-16, + "velocityX": 1.0112506076082748, + "velocityY": 0.3706250943148165, + "timestamp": 2.835353525529325 + }, + { + "x": 7.570835923468037, + "y": 3.9891303768994915, + "heading": -2.4456304849391743e-17, + "angularVelocity": 6.12426558799873e-16, + "velocityX": 0.6695721247538555, + "velocityY": 0.25990893532273207, + "timestamp": 2.917678102083068 }, { "x": 7.598227500915527, "y": 4.000265598297119, - "heading": -1.415925420811752e-32, - "angularVelocity": -2.6977014897899104e-18, - "velocityX": 0.3327266129215478, - "velocityY": 0.13525998955023022, - "timestamp": 3.000002671226031 + "heading": 3.2179463374057155e-25, + "angularVelocity": 2.9707144199274434e-16, + "velocityX": 0.33272661207810256, + "velocityY": 0.13525998995380084, + "timestamp": 3.0000026786368115 }, { "x": 7.598227500915527, "y": 4.000265598297119, - "heading": -2.7014243414553336e-34, - "angularVelocity": 1.6871197561712535e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 3.082327247511133 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" + "heading": 1.144163224349134e-25, + "angularVelocity": -1.1292130842830807e-24, + "velocityX": -1.2177734406642243e-20, + "velocityY": 2.8518350164068296e-20, + "timestamp": 3.0823272551905547 } ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "AmpLaneSprint": { - "waypoints": [ + "trajectoryWaypoints": [ { - "x": 1.493931531906128, - "y": 7.3939900398254395, + "timestamp": 0, + "isStopPoint": true, + "x": 1.2955970764160156, + "y": 5.586942195892334, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 25 + "controlIntervalCount": 16 }, { - "x": 7.493931531906128, - "y": 7.3939900398254395, + "timestamp": 1.2801092808744416, + "isStopPoint": false, + "x": 4.028205871582031, + "y": 5.432682037353516, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.493931531906128, - "y": 7.39399003982544, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.82118644197349e-37, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.5360008147282442, - "y": 7.39399003982544, - "heading": 3.2333963174943666e-19, - "angularVelocity": 3.292766805519721e-18, - "velocityX": 0.4284174372625368, - "velocityY": 1.8392808793575992e-26, - "timestamp": 0.09819694336189172 - }, - { - "x": 1.6201393797142145, - "y": 7.39399003982544, - "heading": 7.027380416999147e-19, - "angularVelocity": 3.863647922469736e-18, - "velocityX": 0.8568348678215871, - "velocityY": 3.963342552313322e-26, - "timestamp": 0.19639388672378344 - }, - { - "x": 1.746347225986537, - "y": 7.39399003982544, - "heading": 8.361393183209098e-19, - "angularVelocity": 1.3585074996311424e-18, - "velocityX": 1.2852522894444904, - "velocityY": 6.468173032965746e-26, - "timestamp": 0.29459083008567516 - }, - { - "x": 1.9146243523169562, - "y": 7.39399003982544, - "heading": 3.80235401897157e-19, - "angularVelocity": -4.642750516315599e-18, - "velocityX": 1.7136696985593167, - "velocityY": 9.506575448004094e-26, - "timestamp": 0.3927877734475669 - }, - { - "x": 2.12497075686256, - "y": 7.39399003982544, - "heading": -1.0623970199278788e-18, - "angularVelocity": -1.4691215100259113e-17, - "velocityX": 2.1420870889066284, - "velocityY": 1.3343481989110454e-25, - "timestamp": 0.4909847168094586 - }, - { - "x": 2.3773864365383615, - "y": 7.39399003982544, - "heading": -3.9564889545725385e-18, - "angularVelocity": -2.947232200024047e-17, - "velocityX": 2.5705044478376236, - "velocityY": 1.8500903675379378e-25, - "timestamp": 0.5891816601713503 - }, - { - "x": 2.6718713848686027, - "y": 7.39399003982544, - "heading": -8.853646546888228e-18, - "angularVelocity": -4.987077429157769e-17, - "velocityX": 2.9989217408219764, - "velocityY": 2.625793984498987e-25, - "timestamp": 0.6873786035332421 - }, - { - "x": 3.0084255847706767, - "y": 7.39399003982544, - "heading": -1.6413613329692498e-17, - "angularVelocity": -7.698780165331962e-17, - "velocityX": 3.4273388598436147, - "velocityY": 4.161152605292971e-25, - "timestamp": 0.7855755468951338 - }, - { - "x": 3.3798020724552167, - "y": 7.39399003982544, - "heading": -2.58741769005412e-17, - "angularVelocity": -9.634275005875749e-17, - "velocityX": 3.781955679779987, - "velocityY": -5.487818039619291e-25, - "timestamp": 0.8837724902570254 - }, - { - "x": 3.7511785589388484, - "y": 7.39399003982544, - "heading": 5.146033898598045e-18, - "angularVelocity": 3.158979265252523e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.295977316525143e-24, - "timestamp": 0.9819694336189171 - }, - { - "x": 4.122555045422488, - "y": 7.39399003982544, - "heading": 4.0038826563950476e-17, - "angularVelocity": 3.553348146159671e-16, - "velocityX": 3.7819556675504797, - "velocityY": 7.623936784532972e-25, - "timestamp": 1.0801663769808088 - }, - { - "x": 4.493931531906129, - "y": 7.39399003982544, - "heading": 6.570668459860429e-17, - "angularVelocity": 2.613916192895976e-16, - "velocityX": 3.7819556675504797, - "velocityY": 3.4278643163770276e-26, - "timestamp": 1.1783633203427004 - }, - { - "x": 4.865308018389769, - "y": 7.39399003982544, - "heading": 6.663782988380241e-17, - "angularVelocity": 9.482426370100795e-18, - "velocityX": 3.7819556675504797, - "velocityY": 2.693381196266241e-25, - "timestamp": 1.276560263704592 - }, - { - "x": 5.236684504873409, - "y": 7.39399003982544, - "heading": 5.2326006182614025e-17, - "angularVelocity": -1.4574612214194992e-16, - "velocityX": 3.7819556675504797, - "velocityY": 5.124149697811417e-24, - "timestamp": 1.3747572070664837 - }, - { - "x": 5.60806099135704, - "y": 7.39399003982544, - "heading": 2.884064553605338e-17, - "angularVelocity": -2.3916590315859936e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.1014556301472372e-23, - "timestamp": 1.4729541504283754 - }, - { - "x": 5.9794374790415805, - "y": 7.39399003982544, - "heading": 1.6502190572837753e-17, - "angularVelocity": -1.2565009195595738e-16, - "velocityX": 3.7819556797799856, - "velocityY": 6.524035237986936e-24, - "timestamp": 1.571151093790267 - }, - { - "x": 6.315991678943653, - "y": 7.39399003982544, - "heading": 9.967918627860597e-18, - "angularVelocity": -6.65425189408073e-17, - "velocityX": 3.427338859843601, - "velocityY": -3.262675265981889e-25, - "timestamp": 1.6693480371521587 - }, - { - "x": 6.610476627273894, - "y": 7.39399003982544, - "heading": 5.2731830670839714e-18, - "angularVelocity": -4.78093860085579e-17, - "velocityX": 2.9989217408219764, - "velocityY": -2.379324733264266e-25, - "timestamp": 1.7675449805140504 - }, - { - "x": 6.862892306949696, - "y": 7.39399003982544, - "heading": 2.2289001785398693e-18, - "angularVelocity": -3.10018090849907e-17, - "velocityX": 2.5705044478376236, - "velocityY": -1.7314309405461854e-25, - "timestamp": 1.865741923875942 - }, - { - "x": 7.0732387114953, - "y": 7.39399003982544, - "heading": 5.002812433628901e-19, - "angularVelocity": -1.760359206249271e-17, - "velocityX": 2.1420870889066284, - "velocityY": -1.2661949502166272e-25, - "timestamp": 1.9639388672378337 - }, - { - "x": 7.24151583782572, - "y": 7.39399003982544, - "heading": -2.663585350994398e-19, - "angularVelocity": -7.807165345874594e-18, - "velocityX": 1.7136696985593165, - "velocityY": -9.096401200333518e-26, - "timestamp": 2.0621358105997256 - }, - { - "x": 7.367723684098041, - "y": 7.39399003982544, - "heading": -4.029353027704155e-19, - "angularVelocity": -1.390845344762092e-18, - "velocityX": 1.2852522894444902, - "velocityY": -6.226126404979919e-26, - "timestamp": 2.1603327539616175 - }, - { - "x": 7.451862249084011, - "y": 7.39399003982544, - "heading": -2.1995708883691042e-19, - "angularVelocity": 1.8633799906775463e-18, - "velocityX": 0.8568348678215871, - "velocityY": -3.833262935804246e-26, - "timestamp": 2.2585296973235094 + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 7.493931531906128, - "y": 7.39399003982544, + "timestamp": 1.7651340303306633, + "isStopPoint": false, + "x": 4.9537672996521, + "y": 4.264711856842041, "heading": 0, - "angularVelocity": 2.2399586452459634e-18, - "velocityX": 0.42841743726253684, - "velocityY": -1.7862050890834192e-26, - "timestamp": 2.3567266406854013 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 16 }, { - "x": 7.493931531906128, - "y": 7.39399003982544, + "timestamp": 3.0823272551905547, + "isStopPoint": true, + "x": 7.598227500915527, + "y": 4.000265598297119, "heading": 0, - "angularVelocity": 0, - "velocityX": -2.1810930337522913e-39, - "velocityY": 0, - "timestamp": 2.454923584047293 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -1547,19 +1019,23 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "Tag14": { "waypoints": [ @@ -1570,7 +1046,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 7 }, { "x": 5.77, @@ -1579,7 +1055,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 3 }, { "x": 5.826, @@ -1596,208 +1072,135 @@ "x": 6.224338054656982, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.4108248462198856e-52, + "angularVelocity": -1.9390670879884598e-35, + "velocityX": 1.7766873980807548e-19, + "velocityY": 8.904182726292149e-48, "timestamp": 0 }, { - "x": 6.21723902185644, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 2.4668199846059922e-26, - "velocityX": -0.0744521368751352, - "velocityY": -9.854529719176074e-36, - "timestamp": 0.09535028944096227 - }, - { - "x": 6.203040956377823, + "x": 6.186476547774305, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 4.073373060497981e-25, - "velocityX": -0.14890427246588678, - "velocityY": -3.4838944147190658e-37, - "timestamp": 0.19070057888192454 + "angularVelocity": -2.4996122271018033e-24, + "velocityX": -0.40642791397744565, + "velocityY": 1.1300565278043194e-37, + "timestamp": 0.0931567581373812 }, { - "x": 6.181743858388128, + "x": 6.110753536336873, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 4.6563923109407e-25, - "velocityX": -0.22335640630520623, - "velocityY": 9.60043551478312e-36, - "timestamp": 0.28605086832288684 + "angularVelocity": -7.642010441615522e-24, + "velocityX": -0.8128558029655188, + "velocityY": -2.827834467819816e-37, + "timestamp": 0.1863135162747624 }, { - "x": 6.153347728128578, + "x": 5.99716902732849, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.4742520302800545e-24, - "velocityX": -0.29780853761467935, - "velocityY": -1.788338902588745e-35, - "timestamp": 0.3814011577638491 + "angularVelocity": -1.6489729233438956e-24, + "velocityX": -1.2192836169854868, + "velocityY": -1.22969459333099e-36, + "timestamp": 0.27947027441214356 }, { - "x": 6.117852565978237, + "x": 5.88358451832009, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 6.442632745673608e-25, - "velocityX": -0.3722606649486799, - "velocityY": -6.290456308064992e-36, - "timestamp": 0.47675144720481133 + "angularVelocity": 1.0379573373635505e-23, + "velocityX": -1.2192836169854868, + "velocityY": -2.8845016149269677e-37, + "timestamp": 0.3726270325495248 }, { - "x": 6.075258372619414, + "x": 5.807861506882689, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 4.4423067176817646e-24, - "velocityX": -0.4467127851268304, - "velocityY": 5.242130261899161e-35, - "timestamp": 0.5721017366457736 + "angularVelocity": -6.829975200213146e-25, + "velocityX": -0.8128558029655188, + "velocityY": 9.593634209674488e-37, + "timestamp": 0.465783790686906 }, { - "x": 6.025565149644175, + "x": 5.769999999999999, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 8.105662994909866e-24, - "velocityX": -0.5211648886080018, - "velocityY": 7.004285681194871e-35, - "timestamp": 0.6674520260867358 + "angularVelocity": 2.101401640366878e-24, + "velocityX": -0.4064279139774457, + "velocityY": 7.2855825720271415e-37, + "timestamp": 0.5589405488242872 }, { - "x": 5.968772905012807, + "x": 5.77, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -2.0725270792413912e-24, - "velocityX": -0.5956169086044655, - "velocityY": -4.3523966027072635e-35, - "timestamp": 0.7628023155276981 + "angularVelocity": -1.8956839170817245e-35, + "velocityX": 1.3726397507122414e-19, + "velocityY": 1.1843965226752523e-47, + "timestamp": 0.6520973069616685 }, { - "x": 5.919079682037568, + "x": 5.797999999999997, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -8.006174965180316e-24, - "velocityX": -0.5211648886080017, - "velocityY": -1.8945235189214832e-35, - "timestamp": 0.8581526049686603 + "angularVelocity": -4.600113928387426e-28, + "velocityX": 0.3495132037100882, + "velocityY": 8.714533783663232e-44, + "timestamp": 0.7322087296569475 }, { - "x": 5.876485488678745, + "x": 5.826, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.383666380682825e-24, - "velocityX": -0.4467127851268303, - "velocityY": -1.5466989905142068e-35, - "timestamp": 0.9535028944096225 + "angularVelocity": -4.607782485444709e-28, + "velocityX": 0.34951320371008815, + "velocityY": 8.023846057264112e-44, + "timestamp": 0.8123201523522265 }, { - "x": 5.840990326528404, + "x": 5.826, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -2.1969937395424362e-24, - "velocityX": -0.3722606649486798, - "velocityY": -1.620842864899481e-35, - "timestamp": 1.0488531838505848 - }, + "angularVelocity": 9.888109688615658e-44, + "velocityX": -2.1136492085418514e-19, + "velocityY": -4.40493861881828e-55, + "timestamp": 0.8924315750475056 + } + ], + "trajectoryWaypoints": [ { - "x": 5.812594196268854, + "timestamp": 0, + "isStopPoint": true, + "x": 6.224338054656982, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.3032170138675753e-24, - "velocityX": -0.29780853761467935, - "velocityY": 2.776053837220692e-37, - "timestamp": 1.1442034732915471 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 }, { - "x": 5.791297098279159, + "timestamp": 0.6520973069616685, + "isStopPoint": true, + "x": 5.77, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -5.244503893696962e-25, - "velocityX": -0.22335640630520623, - "velocityY": -3.2771501301056225e-36, - "timestamp": 1.2395537627325095 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 3 }, { - "x": 5.777099032800542, + "timestamp": 0.8924315750475056, + "isStopPoint": true, + "x": 5.826, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.4115566589891501e-25, - "velocityX": -0.14890427246588678, - "velocityY": 2.0466110465352638e-36, - "timestamp": 1.3349040521734719 - }, - { - "x": 5.77, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 6.406778527596553e-26, - "velocityX": -0.0744521368751352, - "velocityY": -2.5902780242718716e-36, - "timestamp": 1.4302543416144342 - }, - { - "x": 5.77, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -7.3977584585344e-40, - "velocityX": -2.3244728289151072e-40, - "velocityY": 1.0431484672638798e-42, - "timestamp": 1.5256046310553966 - }, - { - "x": 5.776222222781161, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 7.967001188899933e-26, - "velocityX": 0.06970282002598219, - "velocityY": 6.68920621834722e-35, - "timestamp": 1.6148725082925262 - }, - { - "x": 5.788666667784544, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 1.2807830939495395e-24, - "velocityX": 0.13940563379059512, - "velocityY": -9.825179674996197e-36, - "timestamp": 1.7041403855296557 - }, - { - "x": 5.807333332215455, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.0883251323550264e-24, - "velocityX": 0.209108416248375, - "velocityY": -9.687396006109793e-36, - "timestamp": 1.7934082627667853 - }, - { - "x": 5.819777777218838, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -2.360522661586125e-25, - "velocityX": 0.13940563379059512, - "velocityY": -9.74036123267109e-36, - "timestamp": 1.882676140003915 - }, - { - "x": 5.826, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -3.6074338636642e-26, - "velocityX": 0.06970282002598219, - "velocityY": -4.2689399061332323e-35, - "timestamp": 1.9719440172410445 - }, - { - "x": 5.826, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 3.7086661645988916e-41, - "velocityX": -1.1448525627762707e-40, - "velocityY": 6.561918971393292e-42, - "timestamp": 2.0612118944781743 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -1805,25 +1208,30 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 1 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "Tag15": { "waypoints": [ @@ -1834,7 +1242,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 7 }, { "x": 4.416737995, @@ -1843,7 +1251,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 3 }, { "x": 4.388737995, @@ -1860,208 +1268,135 @@ "x": 4.189568967671509, "y": 5.279922480109792, "heading": -1.0471975512, - "angularVelocity": 8.220687424023247e-35, - "velocityX": -1.8495013172516143e-33, - "velocityY": -1.5997840520692296e-33, + "angularVelocity": -8.682191393976642e-30, + "velocityX": -8.883974143721003e-20, + "velocityY": 1.5386233886481345e-19, "timestamp": 0 }, { - "x": 4.19311848407178, - "y": 5.273774537362223, - "heading": -1.0471975512, - "angularVelocity": 7.45114074725589e-17, - "velocityX": 0.03722606843756754, - "velocityY": -0.06447744189990307, - "timestamp": 0.09535028944096205 - }, - { - "x": 4.200217516811089, - "y": 5.261478651973145, - "heading": -1.0471975512, - "angularVelocity": 1.61376783975804e-16, - "velocityX": 0.07445213623294329, - "velocityY": -0.1289548826874973, - "timestamp": 0.1907005788819241 - }, - { - "x": 4.210866065805936, - "y": 5.243034824087183, - "heading": -1.0471975512, - "angularVelocity": 2.4319200253214066e-16, - "velocityX": 0.11167820315260298, - "velocityY": -0.1934323219583068, - "timestamp": 0.2860508683228862 - }, - { - "x": 4.225064130935711, - "y": 5.218443053913242, - "heading": -1.0471975512, - "angularVelocity": 3.169122315246911e-16, - "velocityX": 0.14890426880733948, - "velocityY": -0.2579097590382051, - "timestamp": 0.3814011577638482 - }, - { - "x": 4.242811712010882, - "y": 5.1877033417795975, - "heading": -1.0471975512, - "angularVelocity": 3.710180313583571e-16, - "velocityX": 0.1861303324743397, - "velocityY": -0.3223871926752432, - "timestamp": 0.4767514472048102 - }, - { - "x": 4.264108808690293, - "y": 5.150815688277151, - "heading": -1.0471975512, - "angularVelocity": 4.524900551124174e-16, - "velocityX": 0.22335639256341489, - "velocityY": -0.38686462011513334, - "timestamp": 0.5721017366457722 - }, - { - "x": 4.2889554201779125, - "y": 5.10778009478467, + "x": 4.208499721112847, + "y": 5.247133453323834, "heading": -1.0471975512, - "angularVelocity": 5.323391559336802e-16, - "velocityX": 0.2605824443040005, - "velocityY": -0.4513420330950154, - "timestamp": 0.6674520260867343 + "angularVelocity": -9.594003781408746e-18, + "velocityX": 0.2032139569887226, + "velocityY": -0.35197689831158363, + "timestamp": 0.09315675813738096 }, { - "x": 4.317351542493596, - "y": 5.0585965681959655, + "x": 4.246361226831558, + "y": 5.181555401767972, "heading": -1.0471975512, - "angularVelocity": 5.803718444019021e-16, - "velocityX": 0.2978084543022323, - "velocityY": -0.5158193737750199, - "timestamp": 0.7628023155276963 + "angularVelocity": -1.757511495393616e-18, + "velocityX": 0.4064279014827589, + "velocityY": -0.7039537749817357, + "timestamp": 0.18631351627476192 }, { - "x": 4.342198153981216, - "y": 5.015560974703484, + "x": 4.303153481335755, + "y": 5.083188331490316, "heading": -1.0471975512, - "angularVelocity": 5.035003607050494e-16, - "velocityX": 0.2605824443040005, - "velocityY": -0.45134203309501547, - "timestamp": 0.8581526049686583 + "angularVelocity": -4.4708927110588736e-18, + "velocityX": 0.6096418084927427, + "velocityY": -1.0559305867276043, + "timestamp": 0.2794702744121429 }, { - "x": 4.363495250660627, - "y": 4.978673321201038, + "x": 4.35994573583996, + "y": 4.984821261212667, "heading": -1.0471975512, - "angularVelocity": 4.229927708554212e-16, - "velocityX": 0.22335639256341489, - "velocityY": -0.3868646201151334, - "timestamp": 0.9535028944096203 + "angularVelocity": -2.3203957666119342e-17, + "velocityX": 0.6096418084927429, + "velocityY": -1.055930586727604, + "timestamp": 0.37262703254952384 }, { - "x": 4.381242831735798, - "y": 4.947933609067394, + "x": 4.397807241558656, + "y": 4.919243209656798, "heading": -1.0471975512, - "angularVelocity": 3.364084361912933e-16, - "velocityX": 0.1861303324743397, - "velocityY": -0.32238719267524324, - "timestamp": 1.0488531838505823 + "angularVelocity": 3.512195891842984e-17, + "velocityX": 0.406427901482759, + "velocityY": -0.7039537749817355, + "timestamp": 0.4657837906869048 }, { - "x": 4.395440896865573, - "y": 4.923341838893452, + "x": 4.416737995000001, + "y": 4.8864541828708425, "heading": -1.0471975512, - "angularVelocity": 2.5077961884006393e-16, - "velocityX": 0.14890426880733948, - "velocityY": -0.2579097590382052, - "timestamp": 1.1442034732915445 - }, - { - "x": 4.406089445860419, - "y": 4.90489801100749, - "heading": -1.0471975512, - "angularVelocity": 1.7002215965852864e-16, - "velocityX": 0.11167820315260296, - "velocityY": -0.19343232195830684, - "timestamp": 1.2395537627325066 - }, - { - "x": 4.413188478599729, - "y": 4.892602125618413, - "heading": -1.0471975512, - "angularVelocity": 1.1674982705623592e-16, - "velocityX": 0.07445213623294329, - "velocityY": -0.12895488268749733, - "timestamp": 1.3349040521734687 - }, - { - "x": 4.416737995, - "y": 4.886454182870843, - "heading": -1.0471975512, - "angularVelocity": 6.969332401174355e-17, - "velocityX": 0.037226068437567546, - "velocityY": -0.06447744189990308, - "timestamp": 1.4302543416144309 + "angularVelocity": 3.9044172518056536e-18, + "velocityX": 0.20321395698872258, + "velocityY": -0.35197689831158346, + "timestamp": 0.5589405488242858 }, { "x": 4.416737995, "y": 4.886454182870843, "heading": -1.0471975512, - "angularVelocity": 7.023845548835228e-34, - "velocityX": 1.047712734399832e-33, - "velocityY": 1.3991585734206156e-34, - "timestamp": 1.525604631055393 + "angularVelocity": -6.644908206829601e-29, + "velocityX": -6.863436538799463e-20, + "velocityY": 1.1889252306500327e-19, + "timestamp": 0.6520973069616668 }, { - "x": 4.413626883609419, - "y": 4.891842785867335, + "x": 4.402737994999999, + "y": 4.910702894176807, "heading": -1.0471975512, - "angularVelocity": 3.2593553049819007e-18, - "velocityX": -0.03485141001299148, - "velocityY": 0.060364412857915745, - "timestamp": 1.6148725082925235 + "angularVelocity": 2.5156332234400082e-17, + "velocityX": -0.1747566018550504, + "velocityY": 0.30268731337101645, + "timestamp": 0.7322087296569455 }, { - "x": 4.407404661107727, - "y": 4.902619991376263, + "x": 4.388737995, + "y": 4.934951605482771, "heading": -1.0471975512, - "angularVelocity": -2.840644301428943e-19, - "velocityX": -0.06970281689529834, - "velocityY": 0.12072882029332666, - "timestamp": 1.704140385529654 + "angularVelocity": -2.5156340257881864e-17, + "velocityX": -0.1747566018550504, + "velocityY": 0.3026873133710164, + "timestamp": 0.8123201523522241 }, { - "x": 4.398071328892272, - "y": 4.918785796977351, + "x": 4.388737995, + "y": 4.934951605482771, "heading": -1.0471975512, - "angularVelocity": 7.063784329206859e-19, - "velocityX": -0.10455420812418867, - "velocityY": 0.1810932006162248, - "timestamp": 1.7934082627667844 - }, + "angularVelocity": -1.0978097374367874e-28, + "velocityX": 1.0569070815560555e-19, + "velocityY": -1.830330405016386e-19, + "timestamp": 0.8924315750475027 + } + ], + "trajectoryWaypoints": [ { - "x": 4.3918491063905805, - "y": 4.9295630024862795, + "timestamp": 0, + "isStopPoint": true, + "x": 4.189568967671509, + "y": 5.279922480109792, "heading": -1.0471975512, - "angularVelocity": -2.9031033307640906e-18, - "velocityX": -0.06970281689529835, - "velocityY": 0.12072882029332664, - "timestamp": 1.882676140003915 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 }, { - "x": 4.388737995, - "y": 4.934951605482771, + "timestamp": 0.6520973069616668, + "isStopPoint": true, + "x": 4.416737995, + "y": 4.886454182870843, "heading": -1.0471975512, - "angularVelocity": -7.785659769863438e-19, - "velocityX": -0.03485141001299149, - "velocityY": 0.06036441285791574, - "timestamp": 1.9719440172410454 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 3 }, { + "timestamp": 0.8924315750475027, + "isStopPoint": true, "x": 4.388737995, "y": 4.934951605482771, "heading": -1.0471975512, - "angularVelocity": 0, - "velocityX": 2.2438461971651043e-34, - "velocityY": -1.8871387146521994e-34, - "timestamp": 2.0612118944781757 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -2069,25 +1404,30 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 1 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "Tag16": { "waypoints": [ @@ -2098,7 +1438,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 7 }, { "x": 4.416737995, @@ -2107,7 +1447,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 3 }, { "x": 4.388737995, @@ -2124,208 +1464,135 @@ "x": 4.189568967671509, "y": 2.930373519890208, "heading": 1.0471975512, - "angularVelocity": 1.1450146397244414e-36, - "velocityX": 3.551739543404917e-34, - "velocityY": -2.349922304718694e-34, + "angularVelocity": -2.002499149483344e-30, + "velocityX": -8.884646650147337e-20, + "velocityY": -1.5387579641415467e-19, "timestamp": 0 }, { - "x": 4.19311848407178, - "y": 2.9365214626377765, - "heading": 1.0471975512, - "angularVelocity": 2.0807829607167273e-17, - "velocityX": 0.03722606843756739, - "velocityY": 0.0644774418999032, - "timestamp": 0.0953502894409621 - }, - { - "x": 4.200217516811089, - "y": 2.948817348026855, - "heading": 1.0471975512, - "angularVelocity": 6.456177548720404e-17, - "velocityX": 0.07445213623294297, - "velocityY": 0.1289548826874976, - "timestamp": 0.1907005788819242 - }, - { - "x": 4.210866065805936, - "y": 2.967261175912817, - "heading": 1.0471975512, - "angularVelocity": 1.2599339745541e-16, - "velocityX": 0.11167820315260249, - "velocityY": 0.19343232195830726, - "timestamp": 0.2860508683228863 - }, - { - "x": 4.225064130935711, - "y": 2.991852946086759, - "heading": 1.0471975512, - "angularVelocity": 1.4686476867569292e-16, - "velocityX": 0.1489042688073388, - "velocityY": 0.2579097590382057, - "timestamp": 0.3814011577638484 - }, - { - "x": 4.242811712010882, - "y": 3.0225926582204026, - "heading": 1.0471975512, - "angularVelocity": 1.7718661382769725e-16, - "velocityX": 0.18613033247433888, - "velocityY": 0.32238719267524385, - "timestamp": 0.47675144720481055 - }, - { - "x": 4.264108808690293, - "y": 3.059480311722848, - "heading": 1.0471975512, - "angularVelocity": 2.0209928205397637e-16, - "velocityX": 0.2233563925634139, - "velocityY": 0.3868646201151341, - "timestamp": 0.5721017366457727 - }, - { - "x": 4.2889554201779125, - "y": 3.1025159052153306, - "heading": 1.0471975512, - "angularVelocity": 2.386366328775348e-16, - "velocityX": 0.2605824443039994, - "velocityY": 0.45134203309501636, - "timestamp": 0.6674520260867348 - }, - { - "x": 4.317351542493596, - "y": 3.1516994318040346, - "heading": 1.0471975512, - "angularVelocity": 2.693541370963743e-16, - "velocityX": 0.297808454302231, - "velocityY": 0.515819373775021, - "timestamp": 0.762802315527697 - }, - { - "x": 4.342198153981216, - "y": 3.194735025296517, - "heading": 1.0471975512, - "angularVelocity": 2.483620297451855e-16, - "velocityX": 0.2605824443039994, - "velocityY": 0.45134203309501647, - "timestamp": 0.8581526049686591 - }, - { - "x": 4.363495250660627, - "y": 3.2316226787989626, + "x": 4.208499721112847, + "y": 2.9631625466761657, "heading": 1.0471975512, - "angularVelocity": 2.1567920172697482e-16, - "velocityX": 0.22335639256341389, - "velocityY": 0.38686462011513423, - "timestamp": 0.9535028944096212 + "angularVelocity": -7.928469907031793e-18, + "velocityX": 0.20321395698872408, + "velocityY": 0.35197689831158385, + "timestamp": 0.0931567581373812 }, { - "x": 4.381242831735798, - "y": 3.2623623909326063, + "x": 4.2463612268315565, + "y": 3.0287405982320337, "heading": 1.0471975512, - "angularVelocity": 1.7766713056448412e-16, - "velocityX": 0.18613033247433886, - "velocityY": 0.32238719267524396, - "timestamp": 1.0488531838505832 + "angularVelocity": -2.0524140454745182e-17, + "velocityX": 0.40642790148276187, + "velocityY": 0.7039537749817363, + "timestamp": 0.1863135162747624 }, { - "x": 4.395440896865573, - "y": 3.2869541611065483, + "x": 4.303153481335761, + "y": 3.127107668509687, "heading": 1.0471975512, - "angularVelocity": 1.4244354178358303e-16, - "velocityX": 0.1489042688073388, - "velocityY": 0.25790975903820573, - "timestamp": 1.1442034732915454 + "angularVelocity": 5.366441184028338e-17, + "velocityX": 0.6096418084927471, + "velocityY": 1.055930586727605, + "timestamp": 0.27947027441214356 }, { - "x": 4.406089445860419, - "y": 3.3053979889925102, + "x": 4.3599457358399505, + "y": 3.225474738787313, "heading": 1.0471975512, - "angularVelocity": 1.228103737524146e-16, - "velocityX": 0.11167820315260248, - "velocityY": 0.19343232195830726, - "timestamp": 1.2395537627325075 + "angularVelocity": -4.609538572640182e-17, + "velocityX": 0.6096418084927472, + "velocityY": 1.0559305867276048, + "timestamp": 0.3726270325495248 }, { - "x": 4.413188478599729, - "y": 3.3176938743815887, + "x": 4.397807241558657, + "y": 3.291052790343219, "heading": 1.0471975512, - "angularVelocity": 1.00554048636381e-16, - "velocityX": 0.07445213623294296, - "velocityY": 0.1289548826874976, - "timestamp": 1.3349040521734696 + "angularVelocity": 9.988825598390611e-18, + "velocityX": 0.4064279014827619, + "velocityY": 0.7039537749817361, + "timestamp": 0.465783790686906 }, { - "x": 4.416737995, - "y": 3.323841817129157, + "x": 4.416737994999997, + "y": 3.323841817129139, "heading": 1.0471975512, - "angularVelocity": 4.815824152485883e-17, - "velocityX": 0.03722606843756737, - "velocityY": 0.06447744189990322, - "timestamp": 1.4302543416144318 + "angularVelocity": 1.0894764403980658e-17, + "velocityX": 0.20321395698872408, + "velocityY": 0.35197689831158374, + "timestamp": 0.5589405488242872 }, { "x": 4.416737995, "y": 3.323841817129157, "heading": 1.0471975512, - "angularVelocity": 5.809967250206149e-34, - "velocityX": -3.837623152452288e-34, - "velocityY": -8.025233082712604e-35, - "timestamp": 1.525604631055394 + "angularVelocity": -2.786574070680907e-28, + "velocityX": -6.865066631576706e-20, + "velocityY": -1.1890733865081739e-19, + "timestamp": 0.6520973069616685 }, { - "x": 4.413626883609419, - "y": 3.3184532141326653, + "x": 4.402737994999996, + "y": 3.299593105823197, "heading": 1.0471975512, - "angularVelocity": -1.5491376688497814e-18, - "velocityX": -0.03485141001299182, - "velocityY": -0.06036441285791556, - "timestamp": 1.6148725082925244 + "angularVelocity": 1.2203521575276725e-17, + "velocityX": -0.17475660185504371, + "velocityY": -0.30268731337102, + "timestamp": 0.732208729656947 }, { - "x": 4.407404661107727, - "y": 3.307676008623737, + "x": 4.388737995, + "y": 3.275344394517229, "heading": 1.0471975512, - "angularVelocity": -2.100497225592737e-18, - "velocityX": -0.06970281689529902, - "velocityY": -0.12072882029332627, - "timestamp": 1.7041403855296549 + "angularVelocity": -1.2203535086762994e-17, + "velocityX": -0.17475660185504374, + "velocityY": -0.30268731337102006, + "timestamp": 0.8123201523522257 }, { - "x": 4.398071328892272, - "y": 3.291510203022649, + "x": 4.388737995, + "y": 3.275344394517229, "heading": 1.0471975512, - "angularVelocity": 2.337039351447479e-18, - "velocityX": -0.10455420812418968, - "velocityY": -0.18109320061622425, - "timestamp": 1.7934082627667853 - }, + "angularVelocity": -5.52100607399031e-28, + "velocityX": 1.0568999281101467e-19, + "velocityY": 1.8303406915772078e-19, + "timestamp": 0.8924315750475043 + } + ], + "trajectoryWaypoints": [ { - "x": 4.3918491063905805, - "y": 3.280732997513721, + "timestamp": 0, + "isStopPoint": true, + "x": 4.189568967671509, + "y": 2.930373519890208, "heading": 1.0471975512, - "angularVelocity": 5.855066236692621e-18, - "velocityX": -0.06970281689529902, - "velocityY": -0.12072882029332627, - "timestamp": 1.8826761400039158 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 }, { - "x": 4.388737995, - "y": 3.275344394517229, + "timestamp": 0.6520973069616685, + "isStopPoint": true, + "x": 4.416737995, + "y": 3.323841817129157, "heading": 1.0471975512, - "angularVelocity": -4.542470693700315e-18, - "velocityX": -0.03485141001299182, - "velocityY": -0.06036441285791555, - "timestamp": 1.9719440172410463 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 3 }, { + "timestamp": 0.8924315750475043, + "isStopPoint": true, "x": 4.388737995, "y": 3.275344394517229, "heading": 1.0471975512, - "angularVelocity": 0, - "velocityX": 8.236649016847915e-35, - "velocityY": -4.795525817613878e-35, - "timestamp": 2.0612118944781765 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -2333,25 +1600,30 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 1 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "SourceLanePSubHSubSprint": { "waypoints": [ @@ -2433,1333 +1705,1423 @@ "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": 2.1342037757525677e-32, - "velocityY": 1.1024409975331184e-32, + "angularVelocity": -3.050132919883851e-26, + "velocityX": -1.6533006332521264e-25, + "velocityY": 1.9877794824032622e-25, "timestamp": 0 }, { - "x": 0.6705320996855912, - "y": 4.398099342118358, - "heading": -1.0386114127976833, - "angularVelocity": 0.15927551181553457, - "velocityX": 0.12605002427557185, - "velocityY": -0.20514712032687507, - "timestamp": 0.05592000838348859 - }, - { - "x": 0.6846335258864488, - "y": 4.375148415401169, - "heading": -1.0210111049652704, - "angularVelocity": 0.3147407938803157, - "velocityX": 0.25217138924859706, - "velocityY": -0.41042423598715294, - "timestamp": 0.11184001676697718 - }, - { - "x": 0.7057920861320249, - "y": 4.340710150429397, - "heading": -0.9949634737692402, - "angularVelocity": 0.4658016325283909, - "velocityX": 0.3783719076090775, - "velocityY": -0.6158487090273922, - "timestamp": 0.16776002515046576 - }, - { - "x": 0.7340126708459819, - "y": 4.294775101740391, - "heading": -0.9607567098648513, - "angularVelocity": 0.6117088479280168, - "velocityX": 0.5046598798845962, - "velocityY": -0.8214420923185943, - "timestamp": 0.22368003353395435 - }, - { - "x": 0.7693006604451206, - "y": 4.237332365746024, - "heading": -0.9187315737842261, - "angularVelocity": 0.7515223494321577, - "velocityX": 0.6310440684690272, - "velocityY": -1.0272304610620715, - "timestamp": 0.2796000419174429 - }, - { - "x": 0.8116619611973516, - "y": 4.168369327121192, - "heading": -0.8692948358062461, - "angularVelocity": 0.8840617054087766, - "velocityX": 0.7575338769931041, - "velocityY": -1.2332444257142752, - "timestamp": 0.3355200503009315 - }, - { - "x": 0.8611030812908155, - "y": 4.087871426229515, - "heading": -0.8129373545696884, - "angularVelocity": 1.0078231900479868, - "velocityX": 0.8841400694078276, - "velocityY": -1.4395187557848361, - "timestamp": 0.3914400586844201 - }, - { - "x": 0.9176312901451578, - "y": 3.995821968070834, - "heading": -0.7502611156342618, - "angularVelocity": 1.120819555419328, - "velocityX": 1.0108762585778377, - "velocityY": -1.6460916373156267, - "timestamp": 0.4473600670679087 - }, - { - "x": 0.9812548770105831, - "y": 3.8922019971306105, - "heading": -0.6820246305048668, - "angularVelocity": 1.220251697057024, - "velocityX": 1.1377606818136927, - "velocityY": -1.8530034943775027, - "timestamp": 0.5032800754513973 - }, - { - "x": 1.0519833618359966, - "y": 3.7769903402374796, - "heading": -0.6092267013635415, - "angularVelocity": 1.3018225720227237, - "velocityX": 1.2648153473148889, - "velocityY": -2.0602939846330393, - "timestamp": 0.5592000838348858 - }, - { - "x": 1.1298269082953458, - "y": 3.6501643798944903, - "heading": -0.5332724156478241, - "angularVelocity": 1.3582667083101572, - "velocityX": 1.392051766614792, - "velocityY": -2.26798893650447, - "timestamp": 0.6151200922183744 - }, - { - "x": 1.2147919549002646, - "y": 3.511704401985981, - "heading": -0.45632702327380176, - "angularVelocity": 1.375990358340898, - "velocityX": 1.5194033238021896, - "velocityY": -2.4760364297333126, - "timestamp": 0.6710401006018629 - }, - { - "x": 1.3068601679390515, - "y": 3.3616165956403266, - "heading": -0.38215505844470493, - "angularVelocity": 1.3263940219829704, - "velocityX": 1.6464270249639619, - "velocityY": -2.683973244717373, - "timestamp": 0.7269601089853515 - }, - { - "x": 1.4058792704184795, - "y": 3.2000739494751453, - "heading": -0.3184323584737973, - "angularVelocity": 1.139533090444293, - "velocityX": 1.7707276043375095, - "velocityY": -2.8888165584195344, - "timestamp": 0.78288011736884 - }, - { - "x": 1.512169170978474, - "y": 3.0300749520186443, - "heading": -0.28662541962876376, - "angularVelocity": 0.5687935278354703, - "velocityX": 1.900749009747616, - "velocityY": -3.0400388406718526, - "timestamp": 0.8388001257523285 - }, - { - "x": 1.6244650109400058, - "y": 2.850864532661705, - "heading": -0.2866253125429214, - "angularVelocity": 0.0000019149825876044375, - "velocityX": 2.008151343458836, - "velocityY": -3.2047638141959607, - "timestamp": 0.8947201341358171 - }, - { - "x": 1.7352882055291936, - "y": 2.6707396888206167, - "heading": -0.2866252783553297, - "angularVelocity": 6.113659974508405e-7, - "velocityX": 1.9818164873864779, - "velocityY": -3.221116180917336, - "timestamp": 0.9506401425193056 - }, - { - "x": 1.8506362591615508, - "y": 2.493478394875359, - "heading": -0.2866252441530604, - "angularVelocity": 6.116284725625113e-7, - "velocityX": 2.0627331248114724, - "velocityY": -3.169908214777688, - "timestamp": 1.0065601509027942 - }, - { - "x": 1.9771720781105002, - "y": 2.324022128559085, - "heading": -0.2866252095630558, - "angularVelocity": 6.185622200419991e-7, - "velocityX": 2.2628004288052184, - "velocityY": -3.0303333496335663, - "timestamp": 1.0624801592862827 - }, - { - "x": 2.114370117744969, - "y": 2.1630766803808767, - "heading": -0.28662517392184755, - "angularVelocity": 6.373605669269748e-7, - "velocityX": 2.4534695827223696, - "velocityY": -2.8781370538157938, - "timestamp": 1.1184001676697712 - }, - { - "x": 2.2616596068294545, - "y": 2.011311910787294, - "heading": -0.28662513647276167, - "angularVelocity": 6.696902771759525e-7, - "velocityX": 2.6339318133574414, - "velocityY": -2.713961853381871, - "timestamp": 1.1743201760532598 - }, - { - "x": 2.418427669547749, - "y": 1.8693594092228303, - "heading": -0.2866250962912349, - "angularVelocity": 7.185536621043778e-7, - "velocityX": 2.8034341776776834, - "velocityY": -2.538492136678183, - "timestamp": 1.2302401844367483 - }, - { - "x": 2.5840219554137547, - "y": 1.7378099138847216, - "heading": -0.28662505217416917, - "angularVelocity": 7.889316725786441e-7, - "velocityX": 2.961270762521926, - "velocityY": -2.3524584337678855, - "timestamp": 1.2861601928202369 + "x": 0.6705320996373239, + "y": 4.398099342072708, + "heading": -1.0386114123729497, + "angularVelocity": 0.15927551883794178, + "velocityX": 0.12605002295897502, + "velocityY": -0.20514712040522456, + "timestamp": 0.0559200085846521 + }, + { + "x": 0.6846335257451812, + "y": 4.375148415264062, + "heading": -1.0210111037335954, + "angularVelocity": 0.3147408071783644, + "velocityX": 0.25217138667835703, + "velocityY": -0.41042423614623674, + "timestamp": 0.1118400171693042 + }, + { + "x": 0.7057920858570321, + "y": 4.340710150154942, + "heading": -0.9949634713965575, + "angularVelocity": 0.4658016512570238, + "velocityX": 0.37837190385657676, + "velocityY": -0.6158487092681056, + "timestamp": 0.1677600257539563 + }, + { + "x": 0.7340126704011031, + "y": 4.294775101282737, + "heading": -0.9607567060719395, + "angularVelocity": 0.6117088711250027, + "velocityX": 0.5046598750311442, + "velocityY": -0.8214420926396665, + "timestamp": 0.2236800343386084 + }, + { + "x": 0.7693006597994352, + "y": 4.237332365059552, + "heading": -0.9187315683544864, + "angularVelocity": 0.7515223759995503, + "velocityX": 0.6310440626079833, + "velocityY": -1.0272304614586687, + "timestamp": 0.27960004292326046 + }, + { + "x": 0.8116619603260323, + "y": 4.168369326160847, + "heading": -0.8692948285945427, + "angularVelocity": 0.8840617340948025, + "velocityX": 0.7575338702330544, + "velocityY": -1.2332444261754465, + "timestamp": 0.33552005150791253 + }, + { + "x": 0.8611030801762585, + "y": 4.087871424951387, + "heading": -0.8129373455124387, + "angularVelocity": 1.0078232194258272, + "velocityX": 0.8841400618775261, + "velocityY": -1.4395187562892047, + "timestamp": 0.3914400600925646 + }, + { + "x": 0.9176312887785456, + "y": 3.99582196643312, + "heading": -0.7502611047615361, + "angularVelocity": 1.1208195838529407, + "velocityX": 1.0108762504339452, + "velocityY": -1.6460916378244417, + "timestamp": 0.44736006867721667 + }, + { + "x": 0.9812548753941158, + "y": 3.892201995095202, + "heading": -0.6820246179562691, + "angularVelocity": 1.2202517226364538, + "velocityX": 1.1377606732526935, + "velocityY": -1.85300349482346, + "timestamp": 0.5032800772618687 + }, + { + "x": 1.0519833599861292, + "y": 3.7769903377726197, + "heading": -0.6092266874108005, + "angularVelocity": 1.301822592449473, + "velocityX": 1.2648153385910914, + "velocityY": -2.060293984901199, + "timestamp": 0.5592000858465208 + }, + { + "x": 1.129826906247576, + "y": 3.6501643769794856, + "heading": -0.5332724007265343, + "angularVelocity": 1.3582667207442602, + "velocityX": 1.3920517580680767, + "velocityY": -2.267988936395541, + "timestamp": 0.6151200944311729 + }, + { + "x": 1.214791952716682, + "y": 3.5117043986204477, + "heading": -0.45632700801637377, + "angularVelocity": 1.3759903594020386, + "velocityX": 1.51940331590767, + "velocityY": -2.4760364288828027, + "timestamp": 0.671040103015825 + }, + { + "x": 1.3068601657234094, + "y": 3.3616165918639114, + "heading": -0.3821550436425142, + "angularVelocity": 1.3263940090706097, + "velocityX": 1.6464270184678926, + "velocityY": -2.68397324240987, + "timestamp": 0.726960111600477 + }, + { + "x": 1.40587926837359, + "y": 3.2000739453958693, + "heading": -0.31843234449405566, + "angularVelocity": 1.1395330716374026, + "velocityX": 1.7707276010210988, + "velocityY": -2.8888165534434402, + "timestamp": 0.7828801201851291 + }, + { + "x": 1.5121691695533515, + "y": 3.030074948039426, + "heading": -0.28662540722201535, + "angularVelocity": 0.5687934976599797, + "velocityX": 1.9007490139930723, + "velocityY": -3.0400388279464865, + "timestamp": 0.8388001287697812 + }, + { + "x": 1.624464989491448, + "y": 2.8508645164858524, + "heading": -0.2866253001360378, + "angularVelocity": 0.000001914984998408506, + "velocityX": 2.0081509781620026, + "velocityY": -3.2047640207759205, + "timestamp": 0.8947201373544332 + }, + { + "x": 1.7352882065393072, + "y": 2.6707396868109323, + "heading": -0.2866252659484468, + "angularVelocity": 6.113659826501039e-7, + "velocityX": 1.9818168818785864, + "velocityY": -3.2211159160007226, + "timestamp": 0.9506401459390853 + }, + { + "x": 1.850636259974605, + "y": 2.493478393091365, + "heading": -0.28662523174617754, + "angularVelocity": 6.116284689254095e-7, + "velocityX": 2.0627331138671594, + "velocityY": -3.1699081993384874, + "timestamp": 1.0065601545237375 + }, + { + "x": 1.977172078744531, + "y": 2.3240221270116344, + "heading": -0.28662519715617285, + "angularVelocity": 6.185622201662938e-7, + "velocityX": 2.262800417463728, + "velocityY": -3.030333334502384, + "timestamp": 1.0624801631083896 + }, + { + "x": 2.1143701182221513, + "y": 2.163076679089521, + "heading": -0.2866251615149645, + "angularVelocity": 6.373605670854621e-7, + "velocityX": 2.4534695710915173, + "velocityY": -2.8781370388824827, + "timestamp": 1.1184001716930416 + }, + { + "x": 2.2616596071687876, + "y": 2.0113119097755345, + "heading": -0.2866251240658783, + "angularVelocity": 6.696902793737695e-7, + "velocityX": 2.633931801417164, + "velocityY": -2.713961838618882, + "timestamp": 1.1743201802776937 + }, + { + "x": 2.4184276697644367, + "y": 1.8693594085175804, + "heading": -0.28662508388435115, + "angularVelocity": 7.185536668369167e-7, + "velocityX": 2.8034341653995387, + "velocityY": -2.5384921220651377, + "timestamp": 1.2302401888623458 + }, + { + "x": 2.5840219555188284, + "y": 1.7378099135158778, + "heading": -0.28662503976728476, + "angularVelocity": 7.889316805282802e-7, + "velocityX": 2.9612707498732536, + "velocityY": -2.352458419289443, + "timestamp": 1.2861601974469978 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.28662500163792387, - "angularVelocity": 9.037238500487202e-7, - "velocityX": 3.1067845266977017, - "velocityY": -2.1566350283233993, - "timestamp": 1.3420802012037254 - }, - { - "x": 2.964172022537395, - "y": 1.4952512181839386, - "heading": -0.2866249560195876, - "angularVelocity": 7.195928187708238e-7, - "velocityX": 3.256089340971223, - "velocityY": -1.923816018708688, - "timestamp": 1.4054748531095118 - }, - { - "x": 3.1789518158724, - "y": 1.3887033110695066, - "heading": -0.28662491569189824, - "angularVelocity": 6.361370890269852e-7, - "velocityX": 3.387979693526802, - "velocityY": -1.6807081340674828, - "timestamp": 1.4688695050152982 - }, - { - "x": 3.4009440542760783, - "y": 1.2981369637223683, - "heading": -0.2866248790058733, - "angularVelocity": 5.786927429417107e-7, - "velocityX": 3.5017502538477667, - "velocityY": -1.4286117933375972, - "timestamp": 1.5322641569210846 - }, - { - "x": 3.6289614625982565, - "y": 1.2240364953903844, - "heading": -0.286624844765475, - "angularVelocity": 5.401149349395126e-7, - "velocityX": 3.5967924969608642, - "velocityY": -1.1688757033023536, - "timestamp": 1.595658808826871 - }, - { - "x": 3.861784529858251, - "y": 1.1667980667810438, - "heading": -0.2866248120410044, - "angularVelocity": 5.162023864828723e-7, - "velocityX": 3.672597928386812, - "velocityY": -0.9028904945231823, - "timestamp": 1.6590534607326575 - }, - { - "x": 4.0981679647792335, - "y": 1.1267270015307351, - "heading": -0.2866247800525485, - "angularVelocity": 5.045923424053189e-7, - "velocityX": 3.728759884544863, - "velocityY": -0.6320890492444079, - "timestamp": 1.7224481126384439 - }, - { - "x": 4.3358126512762265, - "y": 1.0949807955483848, - "heading": -0.2866247482072254, - "angularVelocity": 5.023345364444787e-7, - "velocityX": 3.7486551207847545, - "velocityY": -0.5007710434238178, - "timestamp": 1.7858427645442303 - }, - { - "x": 4.573457419312962, - "y": 1.0632351999594603, - "heading": -0.28662471636190656, - "angularVelocity": 5.023344691113163e-7, - "velocityX": 3.7486564070090496, - "velocityY": -0.5007614149550478, - "timestamp": 1.8492374164500167 - }, - { - "x": 4.811102187354235, - "y": 1.0314896044045174, - "heading": -0.28662468451658774, - "angularVelocity": 5.023344695763415e-7, - "velocityX": 3.748656407080655, - "velocityY": -0.5007614144190159, - "timestamp": 1.9126320683558031 - }, - { - "x": 5.04874695539551, - "y": 0.9997440088495765, - "heading": -0.28662465267126885, - "angularVelocity": 5.023344699297441e-7, - "velocityX": 3.7486564070806594, - "velocityY": -0.5007614144189861, - "timestamp": 1.9760267202615895 - }, - { - "x": 5.286391723436784, - "y": 0.9679984132946355, - "heading": -0.28662462082595, - "angularVelocity": 5.023344691743294e-7, - "velocityX": 3.748656407080659, - "velocityY": -0.5007614144189861, - "timestamp": 2.039421372167376 - }, - { - "x": 5.524036491478059, - "y": 0.936252817739695, - "heading": -0.28662458898063115, - "angularVelocity": 5.023344697631327e-7, - "velocityX": 3.74865640708066, - "velocityY": -0.5007614144189809, - "timestamp": 2.1028160240731624 - }, - { - "x": 5.761681259520114, - "y": 0.9045072221906025, - "heading": -0.2866245571353122, - "angularVelocity": 5.023344707942377e-7, - "velocityX": 3.748656407092983, - "velocityY": -0.5007614143267316, - "timestamp": 2.166210675978949 - }, - { - "x": 5.99932604159473, - "y": 0.8727617316882137, - "heading": -0.2866245252899934, - "angularVelocity": 5.023344694954779e-7, - "velocityX": 3.748656628445413, - "velocityY": -0.500759757298884, - "timestamp": 2.229605327884735 - }, - { - "x": 6.237212534637086, - "y": 0.8428813906316718, - "heading": -0.2866244915581877, - "angularVelocity": 5.320922919551869e-7, - "velocityX": 3.752469425905036, - "velocityY": -0.47133851450037834, - "timestamp": 2.2929999797905216 - }, - { - "x": 6.4638245505635705, - "y": 0.8220107849523658, - "heading": -0.24424436180847547, - "angularVelocity": 0.6685126974542774, - "velocityX": 3.574623554416899, - "velocityY": -0.3292171350719418, - "timestamp": 2.356394631696308 - }, - { - "x": 6.673647733933235, - "y": 0.8041360764949451, - "heading": -0.18514255243378605, - "angularVelocity": 0.9322838377994912, - "velocityX": 3.309793130207447, - "velocityY": -0.2819592492436898, - "timestamp": 2.4197892836020944 - }, - { - "x": 6.866141134149738, - "y": 0.7888964331390943, - "heading": -0.1276691267098072, - "angularVelocity": 0.9065973863125333, - "velocityX": 3.0364296424022625, - "velocityY": -0.240393201913927, - "timestamp": 2.483183935507881 - }, - { - "x": 7.04134978279503, - "y": 0.7761674362974819, - "heading": -0.07636429138531178, - "angularVelocity": 0.8092927996630026, - "velocityX": 2.7637764918352534, - "velocityY": -0.2007897584252007, - "timestamp": 2.5465785874136673 - }, - { - "x": 7.19932046714385, - "y": 0.765888430525111, - "heading": -0.03338684229285246, - "angularVelocity": 0.6779349330023924, - "velocityX": 2.4918613731579056, - "velocityY": -0.16214310613530794, - "timestamp": 2.6099732393194537 + "heading": -0.2866249892310386, + "angularVelocity": 9.037238643571085e-7, + "velocityX": 3.106784513642535, + "velocityY": -2.156635013969323, + "timestamp": 1.34208020603165 + }, + { + "x": 2.964172022531705, + "y": 1.4952512185912425, + "heading": -0.28662494361270324, + "angularVelocity": 7.19592801055205e-7, + "velocityX": 3.2560893274813307, + "velocityY": -1.9238160043665056, + "timestamp": 1.4054748581983312 + }, + { + "x": 3.178951815863355, + "y": 1.3887033119472907, + "heading": -0.28662490328501483, + "angularVelocity": 6.36137071526776e-7, + "velocityX": 3.3879796795309685, + "velocityY": -1.680708119729232, + "timestamp": 1.4688695103650125 + }, + { + "x": 3.4009440542565765, + "y": 1.2981369651359775, + "heading": -0.28662486659899084, + "angularVelocity": 5.786927242976968e-7, + "velocityX": 3.501750239271679, + "velocityY": -1.4286117790060657, + "timestamp": 1.5322641625316937 + }, + { + "x": 3.628961462551473, + "y": 1.224036497406262, + "heading": -0.28662483235859354, + "angularVelocity": 5.40114917287456e-7, + "velocityX": 3.596792481728244, + "velocityY": -1.168875688991645, + "timestamp": 1.595658814698375 + }, + { + "x": 3.861784529757441, + "y": 1.166798069465538, + "heading": -0.2866247996341239, + "angularVelocity": 5.162023688737337e-7, + "velocityX": 3.672597912420351, + "velocityY": -0.9028904802605274, + "timestamp": 1.6590534668650563 + }, + { + "x": 4.098167964587255, + "y": 1.1267270049463884, + "heading": -0.28662476764566913, + "angularVelocity": 5.045923229108656e-7, + "velocityX": 3.7287598677613794, + "velocityY": -0.6320890351096494, + "timestamp": 1.7224481190317376 + }, + { + "x": 4.335812648113977, + "y": 1.0949807783310244, + "heading": -0.2866247358003473, + "angularVelocity": 5.023345150165169e-7, + "velocityX": 3.748655058503865, + "velocityY": -0.500771366832249, + "timestamp": 1.7858427711984188 + }, + { + "x": 4.573457416127459, + "y": 1.0632351841697858, + "heading": -0.2866247039550299, + "angularVelocity": 5.023344440869941e-7, + "velocityX": 3.7486563912149893, + "velocityY": -0.5007613903736075, + "timestamp": 1.8492374233651 + }, + { + "x": 4.8111021841458665, + "y": 1.0314895900454293, + "heading": -0.2866246721097125, + "angularVelocity": 5.023344453927301e-7, + "velocityX": 3.7486563912927067, + "velocityY": -0.5007613897918232, + "timestamp": 1.9126320755317814 + }, + { + "x": 5.048746952164275, + "y": 0.9997439959210748, + "heading": -0.28662464026439505, + "angularVelocity": 5.023344449153729e-7, + "velocityX": 3.748656391292711, + "velocityY": -0.5007613897917893, + "timestamp": 1.9760267276984627 + }, + { + "x": 5.286391720182684, + "y": 0.9679984017967204, + "heading": -0.28662460841907766, + "angularVelocity": 5.023344443752201e-7, + "velocityX": 3.7486563912927116, + "velocityY": -0.5007613897917892, + "timestamp": 2.039421379865144 + }, + { + "x": 5.524036488201093, + "y": 0.9362528076723663, + "heading": -0.28662457657376017, + "angularVelocity": 5.023344462660274e-7, + "velocityX": 3.748656391292712, + "velocityY": -0.5007613897917833, + "timestamp": 2.1028160320318254 + }, + { + "x": 5.761681256220357, + "y": 0.9045072135544255, + "heading": -0.28662454472844273, + "angularVelocity": 5.023344454687972e-7, + "velocityX": 3.748656391306226, + "velocityY": -0.5007613896906189, + "timestamp": 2.1662106841985067 + }, + { + "x": 5.9993260389305805, + "y": 0.8727617294119203, + "heading": -0.28662451288312535, + "angularVelocity": 5.023344447486752e-7, + "velocityX": 3.748656623044358, + "velocityY": -0.5007596549159697, + "timestamp": 2.229605336365188 + }, + { + "x": 6.2372125316404645, + "y": 0.8428813874102156, + "heading": -0.28662447915131883, + "angularVelocity": 5.320923039829879e-7, + "velocityX": 3.752469405217604, + "velocityY": -0.4713385274698144, + "timestamp": 2.2929999885318693 + }, + { + "x": 6.463824548233647, + "y": 0.8220107822514932, + "heading": -0.244244352051896, + "angularVelocity": 0.6685126528968754, + "velocityX": 3.5746235502224897, + "velocityY": -0.32921712550528753, + "timestamp": 2.3563946406985505 + }, + { + "x": 6.673647732212342, + "y": 0.8041360744021581, + "heading": -0.1851425455819086, + "angularVelocity": 0.9322837881434082, + "velocityX": 3.309793126193282, + "velocityY": -0.28195923849124843, + "timestamp": 2.419789292865232 + }, + { + "x": 6.866141132976164, + "y": 0.7888964316418127, + "heading": -0.12766912233996416, + "angularVelocity": 0.9065973434294042, + "velocityX": 3.0364296385396288, + "velocityY": -0.24039319153098848, + "timestamp": 2.483183945031913 + }, + { + "x": 7.041349782094471, + "y": 0.7761674353551624, + "heading": -0.07636428899969135, + "angularVelocity": 0.8092927650329051, + "velocityX": 2.7637764879226316, + "velocityY": -0.2007897488447803, + "timestamp": 2.5465785971985944 + }, + { + "x": 7.199320466835692, + "y": 0.7658884300843142, + "heading": -0.0333868413674214, + "angularVelocity": 0.677934907179092, + "velocityX": 2.4918613690926774, + "velocityY": -0.16214309755690418, + "timestamp": 2.6099732493652756 }, { "x": 7.340087890625, "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": 0.5266507708326894, - "velocityX": 2.220493673351971, - "velocityY": -0.12406863156475356, - "timestamp": 2.67336789122524 - }, - { - "x": 7.464023522460178, - "y": 0.7525372143128384, - "heading": 0.02301011936038792, - "angularVelocity": 0.3617896361662683, - "velocityX": 1.9486481772395665, - "velocityY": -0.08625561847872572, - "timestamp": 2.736968716290925 - }, - { - "x": 7.570582319535539, - "y": 0.7491648354718851, - "heading": 0.036983700221316605, - "angularVelocity": 0.21970754068830917, - "velocityX": 1.675431049287655, - "velocityY": -0.053024136675436706, - "timestamp": 2.8005695413566096 - }, - { - "x": 7.659708944521439, - "y": 0.7476928551810322, - "heading": 0.04295335113137197, - "angularVelocity": 0.09386121805637201, - "velocityX": 1.4013438488235506, - "velocityY": -0.02314404395434427, - "timestamp": 2.8641703664222944 - }, - { - "x": 7.731365798341117, - "y": 0.7479588271083956, - "heading": 0.041693436231508264, - "angularVelocity": -0.019809725716020715, - "velocityX": 1.1266654755763426, - "velocityY": 0.0041818942928592215, - "timestamp": 2.927771191487979 - }, - { - "x": 7.785525986892142, - "y": 0.7498349370538775, - "heading": 0.03380693253041853, - "angularVelocity": -0.12400002190136777, - "velocityX": 0.8515642445690119, - "velocityY": 0.029498201376229192, - "timestamp": 2.991372016553664 - }, - { - "x": 7.822169492500932, - "y": 0.7532180353995639, - "heading": 0.0197770843540876, - "angularVelocity": -0.220592235428419, - "velocityX": 0.5761482743493489, - "velocityY": 0.053192680160869574, - "timestamp": 3.0549728416193487 + "heading": 6.333223838299427e-25, + "angularVelocity": 0.5266507540673704, + "velocityX": 2.2204936690746884, + "velocityY": -0.12406862410094172, + "timestamp": 2.673367901531957 + }, + { + "x": 7.464023523183681, + "y": 0.7525372146747857, + "heading": 0.023010119033740877, + "angularVelocity": 0.36178962784032076, + "velocityX": 1.9486481714332364, + "velocityY": -0.08625561202725132, + "timestamp": 2.7369687271584358 + }, + { + "x": 7.57058232073761, + "y": 0.7491648361128735, + "heading": 0.03698369983604058, + "angularVelocity": 0.21970753782922778, + "velocityX": 1.675431042039261, + "velocityY": -0.05302413182051879, + "timestamp": 2.8005695527849146 + }, + { + "x": 7.659708945960142, + "y": 0.7476928559896933, + "heading": 0.042953350833786465, + "angularVelocity": 0.09386121860752329, + "velocityX": 1.4013438401879301, + "velocityY": -0.02314404111394868, + "timestamp": 2.8641703784113934 + }, + { + "x": 7.731365799776583, + "y": 0.747958827951943, + "heading": 0.041693436078442773, + "angularVelocity": -0.01980972326905067, + "velocityX": 1.1266654655911963, + "velocityY": 0.0041818948045075285, + "timestamp": 2.9277712040378723 + }, + { + "x": 7.785525988086016, + "y": 0.7498349377828146, + "heading": 0.03380693250989334, + "angularVelocity": -0.1240000187240661, + "velocityX": 0.85156423326185, + "velocityY": 0.029498199314107475, + "timestamp": 2.991372029664351 + }, + { + "x": 7.822169493216012, + "y": 0.7532180358509858, + "heading": 0.01977708439944101, + "angularVelocity": -0.2205922324475507, + "velocityX": 0.5761482617411275, + "velocityY": 0.05319267532845736, + "timestamp": 3.05497285529083 }, { "x": 7.841280937194824, "y": 0.7580231428146362, - "heading": -3.3329541710174495e-31, - "angularVelocity": -0.3109564118651419, - "velocityX": 0.3004905152433859, - "velocityY": 0.07555102327854826, - "timestamp": 3.1185736666850334 - }, - { - "x": 7.837626456422163, - "y": 0.7661832239405314, - "heading": -0.033360531548023514, - "angularVelocity": -0.4178611188210231, - "velocityX": -0.04577461309858743, - "velocityY": 0.10221002096529454, - "timestamp": 3.1984100765480727 - }, - { - "x": 7.806322741144082, - "y": 0.7764707746744536, - "heading": -0.07501113915617397, - "angularVelocity": -0.521699406068016, - "velocityX": -0.3920982335225694, - "velocityY": 0.12885788265743237, - "timestamp": 3.278246486411112 - }, - { - "x": 7.74736425619444, - "y": 0.7888847549403223, - "heading": -0.12465356813987527, - "angularVelocity": -0.6218018704606526, - "velocityX": -0.7384911852973625, - "velocityY": 0.15549271675874526, - "timestamp": 3.3580828962741514 - }, - { - "x": 7.660744366778727, - "y": 0.8034239689194147, - "heading": -0.18191677738707465, - "angularVelocity": -0.7172568173523234, - "velocityX": -1.0849672419427567, - "velocityY": 0.182112572497119, - "timestamp": 3.4379193061371907 - }, - { - "x": 7.5464550145644145, - "y": 0.8200870575241466, - "heading": -0.24632578527266566, - "angularVelocity": -0.8067623280666746, - "velocityX": -1.4315442341455173, - "velocityY": 0.20871540482992249, - "timestamp": 3.51775571600023 - }, - { - "x": 7.404486266909035, - "y": 0.8388724767291539, - "heading": -0.3172478637527479, - "angularVelocity": -0.8883425319569104, - "velocityX": -1.7782456387872363, - "velocityY": 0.23529889729803757, - "timestamp": 3.5975921258632693 - }, - { - "x": 7.234825726560123, - "y": 0.8597784078771568, - "heading": -0.39379130752240776, - "angularVelocity": -0.958753579989021, - "velocityX": -2.125102326619727, - "velocityY": 0.26185960996827495, - "timestamp": 3.6774285357263086 - }, - { - "x": 7.037458048630302, - "y": 0.8828023770067689, - "heading": -0.47459284644074845, - "angularVelocity": -1.0120888333650886, - "velocityX": -2.4721512185781784, - "velocityY": 0.28838933475478157, - "timestamp": 3.757264945589348 - }, - { - "x": 6.812366694532574, - "y": 0.9079395818644891, - "heading": -0.5572950195818098, - "angularVelocity": -1.0358954427301785, - "velocityX": -2.8194072664825045, - "velocityY": 0.31485890837079844, - "timestamp": 3.837101355452387 - }, - { - "x": 6.559557151686244, - "y": 0.9351738572850867, - "heading": -0.6368741954392069, - "angularVelocity": -0.9967779863087077, - "velocityX": -3.1665945810943685, - "velocityY": 0.3411260033776371, - "timestamp": 3.9169377653154265 - }, - { - "x": 6.2794552371906684, - "y": 0.9644007497338526, - "heading": -0.6963848347023109, - "angularVelocity": -0.7454072567290455, - "velocityX": -3.5084482753682624, - "velocityY": 0.36608475379723326, - "timestamp": 3.996774175178466 - }, - { - "x": 5.978414020607044, - "y": 0.9876514129078965, - "heading": -0.6963848502753706, - "angularVelocity": -1.9506212382871808e-7, - "velocityX": -3.7707258768281893, - "velocityY": 0.2912288167006853, - "timestamp": 4.0766105850415055 - }, - { - "x": 5.677372687056552, - "y": 1.0109005615970947, - "heading": -0.6963848658464015, - "angularVelocity": -1.950367123738679e-7, - "velocityX": -3.770727341909936, - "velocityY": 0.29120984684910617, - "timestamp": 4.156446994904545 - }, - { - "x": 5.376331353502739, - "y": 1.0341497102432655, - "heading": -0.6963848814174324, - "angularVelocity": -1.950367127895845e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.29120984631016156, - "timestamp": 4.236283404767585 - }, - { - "x": 5.075290019948924, - "y": 1.0573988588894352, - "heading": -0.6963848969884634, - "angularVelocity": -1.950367129001825e-7, - "velocityX": -3.770727341951559, - "velocityY": 0.2912098463101467, - "timestamp": 4.316119814630625 - }, - { - "x": 4.774248686395111, - "y": 1.0806480075356062, - "heading": -0.6963849125594943, - "angularVelocity": -1.9503671289084429e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.2912098463101629, - "timestamp": 4.3959562244936645 - }, - { - "x": 4.473207352844907, - "y": 1.1038971562285251, - "heading": -0.6963849281305252, - "angularVelocity": -1.950367124712723e-7, - "velocityX": -3.770727341906337, - "velocityY": 0.2912098468957114, - "timestamp": 4.475792634356704 - }, - { - "x": 4.172166146375442, - "y": 1.1271479503662634, - "heading": -0.6963849437015552, - "angularVelocity": -1.9503670107200537e-7, - "velocityX": -3.770725750142127, - "velocityY": 0.29123045710128126, - "timestamp": 4.555629044219744 - }, - { - "x": 3.8735860999949274, - "y": 1.1720518831992321, - "heading": -0.6963849593703558, - "angularVelocity": -1.9626133737167217e-7, - "velocityX": -3.7398982105123864, - "velocityY": 0.5624492998871315, - "timestamp": 4.635465454082784 - }, - { - "x": 3.580403442801602, - "y": 1.244234542261993, - "heading": -0.6963849756495777, - "angularVelocity": -2.0390723857242194e-7, - "velocityX": -3.672292600535076, - "velocityY": 0.9041320769131688, - "timestamp": 4.7153018639458235 - }, - { - "x": 3.295105029858622, - "y": 1.3430840304472003, - "heading": -0.6963849932202171, - "angularVelocity": -2.20083034395915e-7, - "velocityX": -3.5735376056164516, - "velocityY": 1.238150467371779, - "timestamp": 4.795138273808863 - }, - { - "x": 3.0201108216375614, - "y": 1.4677619739384065, - "heading": -0.6963850130039021, - "angularVelocity": -2.4780278968053093e-7, - "velocityX": -3.444471121544888, - "velocityY": 1.5616677115753714, - "timestamp": 4.874974683671903 + "heading": -7.691291273987186e-25, + "angularVelocity": -0.3109564098364043, + "velocityX": 0.30049050135059296, + "velocityY": 0.07555101551464699, + "timestamp": 3.118573680917309 + }, + { + "x": 7.837626455350944, + "y": 0.7661832230204207, + "heading": -0.033360531574280844, + "angularVelocity": -0.41786111810293475, + "velocityX": -0.045774626401580844, + "velocityY": 0.10221000918425123, + "timestamp": 3.1984100909803836 + }, + { + "x": 7.806322738977659, + "y": 0.7764707725073046, + "heading": -0.07501113928857397, + "angularVelocity": -0.5216994060903668, + "velocityX": -0.39209824625822215, + "velocityY": 0.12885786671465047, + "timestamp": 3.2782465010434585 + }, + { + "x": 7.747364252906707, + "y": 0.7888847511850451, + "heading": -0.12465356840287752, + "angularVelocity": -0.6218018705385611, + "velocityX": -0.7384911974921111, + "velocityY": 0.15549269647686922, + "timestamp": 3.3580829111065333 + }, + { + "x": 7.660744362341046, + "y": 0.8034239632171427, + "heading": -0.18191677773733375, + "angularVelocity": -0.7172568166481353, + "velocityX": -1.084967253628106, + "velocityY": 0.18211254765351886, + "timestamp": 3.437919321169608 + }, + { + "x": 7.5464550089450855, + "y": 0.820087049493035, + "heading": -0.24632578558189497, + "angularVelocity": -0.8067623255313559, + "velocityX": -1.4315442453595524, + "velocityY": 0.20871537513682828, + "timestamp": 3.517755731232683 + }, + { + "x": 7.4044862600725985, + "y": 0.8388724659564971, + "heading": -0.31724786378277436, + "angularVelocity": -0.8883425262339242, + "velocityX": -1.7782456495767374, + "velocityY": 0.23529886236894457, + "timestamp": 3.597592141295758 + }, + { + "x": 7.234825718466435, + "y": 0.859778393906575, + "heading": -0.393791306886596, + "angularVelocity": -0.958753569246769, + "velocityX": -2.1251023370430016, + "velocityY": 0.261859569256197, + "timestamp": 3.6774285513588327 + }, + { + "x": 7.037458039233374, + "y": 0.8828023593152449, + "heading": -0.47459284454035716, + "angularVelocity": -1.012088814989599, + "velocityX": -2.4721512287079195, + "velocityY": 0.2883892874251208, + "timestamp": 3.7572649614219076 + }, + { + "x": 6.812366683779643, + "y": 0.9079395598147495, + "heading": -0.5572950154955562, + "angularVelocity": -1.0358954127554088, + "velocityX": -2.8194072764030573, + "velocityY": 0.3148588529925756, + "timestamp": 3.8371013714849824 + }, + { + "x": 6.559557139520272, + "y": 0.9351738299996345, + "heading": -0.6368741877538795, + "angularVelocity": -0.9967779387306077, + "velocityX": -3.166594590859465, + "velocityY": 0.3411259369424114, + "timestamp": 3.9169377815480573 + }, + { + "x": 6.279455223531701, + "y": 0.9644007155406045, + "heading": -0.6963848224463702, + "angularVelocity": -0.7454071976116461, + "velocityX": -3.5084482852782983, + "velocityY": 0.3660846663556045, + "timestamp": 3.996774191611132 + }, + { + "x": 5.978414008634455, + "y": 0.9876513907684487, + "heading": -0.696384838019443, + "angularVelocity": -1.9506228555195058e-7, + "velocityX": -3.770725846257472, + "velocityY": 0.2912289669522333, + "timestamp": 4.076610601674207 + }, + { + "x": 5.677372675665616, + "y": 1.0109005372077842, + "heading": -0.6963848535904748, + "angularVelocity": -1.950367229938273e-7, + "velocityX": -3.770727325176581, + "velocityY": 0.2912098179385523, + "timestamp": 4.156447011737281 + }, + { + "x": 5.3763313426932635, + "y": 1.0341496836016242, + "heading": -0.6963848691615065, + "angularVelocity": -1.950367226738497e-7, + "velocityX": -3.770727325220591, + "velocityY": 0.29120981736869106, + "timestamp": 4.236283421800357 + }, + { + "x": 5.075290009720911, + "y": 1.0573988299954629, + "heading": -0.6963848847325383, + "angularVelocity": -1.950367234539369e-7, + "velocityX": -3.7707273252205917, + "velocityY": 0.2912098173686746, + "timestamp": 4.316119831863432 + }, + { + "x": 4.774248676748558, + "y": 1.080647976389303, + "heading": -0.6963849003035701, + "angularVelocity": -1.9503672325222526e-7, + "velocityX": -3.7707273252205904, + "velocityY": 0.2912098173686933, + "timestamp": 4.395956241926507 + }, + { + "x": 4.473207343780055, + "y": 1.1038971228329835, + "heading": -0.696384915874602, + "angularVelocity": -1.950367229679151e-7, + "velocityX": -3.7707273251723774, + "velocityY": 0.29120981799297246, + "timestamp": 4.4757926519895825 + }, + { + "x": 4.172166140154539, + "y": 1.1271479440122283, + "heading": -0.696384931445633, + "angularVelocity": -1.9503671234983974e-7, + "velocityX": -3.770725705072129, + "velocityY": 0.2912307950830405, + "timestamp": 4.555629062052658 + }, + { + "x": 3.8735860947392786, + "y": 1.1720518781991272, + "heading": -0.6963849471144347, + "angularVelocity": -1.9626135115744237e-7, + "velocityX": -3.739898189051426, + "velocityY": 0.5624493154366842, + "timestamp": 4.635465472115733 + }, + { + "x": 3.580403438648322, + "y": 1.2442345385888913, + "heading": -0.6963849633936577, + "angularVelocity": -2.0390725406473894e-7, + "velocityX": -3.672292577526056, + "velocityY": 0.9041320912693386, + "timestamp": 4.715301882178808 + }, + { + "x": 3.2951050269479554, + "y": 1.343084028059958, + "heading": -0.6963849809642987, + "angularVelocity": -2.2008305293195764e-7, + "velocityX": -3.5735375810982304, + "velocityY": 1.2381504803756895, + "timestamp": 4.795138292241884 + }, + { + "x": 3.020110820111339, + "y": 1.4677619727807747, + "heading": -0.6963850007479858, + "angularVelocity": -2.4780281394367673e-7, + "velocityX": -3.444471095573519, + "velocityY": 1.5616677230641343, + "timestamp": 4.874974702304959 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.696385035506177, - "angularVelocity": -2.8185479518318006e-7, - "velocityX": -3.286187967310359, - "velocityY": 1.8719390230438346, - "timestamp": 4.954811093534943 - }, - { - "x": 2.5741841985169076, - "y": 1.7387873379942203, - "heading": -0.6963850589279509, - "angularVelocity": -4.023107769663099e-7, - "velocityX": -3.153128257888759, - "velocityY": 2.0882929565548642, - "timestamp": 5.0130292054104135 - }, - { - "x": 2.3991894720533877, - "y": 1.872411165939459, - "heading": -0.6963850794155533, - "angularVelocity": -3.519111438487621e-7, - "velocityX": -3.0058468202788267, - "velocityY": 2.2952277846293376, - "timestamp": 5.071247317285884 - }, - { - "x": 2.2335584908852413, - "y": 2.017479635272907, - "heading": -0.6963850978369923, - "angularVelocity": -3.1642110134470593e-7, - "velocityX": -2.845007779064253, - "velocityY": 2.4918099309670456, - "timestamp": 5.129465429161355 - }, - { - "x": 2.0780383093475274, - "y": 2.173338406648706, - "heading": -0.696385114808838, - "angularVelocity": -2.9152174806319146e-7, - "velocityX": -2.671336746034877, - "velocityY": 2.6771526309404154, - "timestamp": 5.187683541036826 - }, - { - "x": 1.9333303562340614, - "y": 2.339284459115452, - "heading": -0.6963851307944144, - "angularVelocity": -2.7458081036886754e-7, - "velocityX": -2.485617421310377, - "velocityY": 2.8504196910698067, - "timestamp": 5.245901652912297 - }, - { - "x": 1.8000872249814166, - "y": 2.5145692341351995, - "heading": -0.6963851461633941, - "angularVelocity": -2.6398966215373687e-7, - "velocityX": -2.2886886393302137, - "velocityY": 3.0108289220145825, - "timestamp": 5.304119764787767 - }, - { - "x": 1.6789094155739503, - "y": 2.698401829383423, - "heading": -0.6963851612308563, - "angularVelocity": -2.588105610313673e-7, - "velocityX": -2.0814451981312536, - "velocityY": 3.157652993649885, - "timestamp": 5.362337876663238 - }, - { - "x": 1.5665736982883989, - "y": 2.8877670089616614, - "heading": -0.6963851762457391, - "angularVelocity": -2.5790741716981964e-7, - "velocityX": -1.929566481404267, - "velocityY": 3.2526850060560912, - "timestamp": 5.420555988538709 - }, - { - "x": 1.4540456075762054, - "y": 3.0767463777174253, - "heading": -0.6971250426595832, - "angularVelocity": -0.012708526436353156, - "velocityX": -1.9328708384238364, - "velocityY": 3.24605801644674, - "timestamp": 5.47877410041418 - }, - { - "x": 1.3486317039814464, - "y": 3.254480208772174, - "heading": -0.7253513181524345, - "angularVelocity": -0.48483666995638336, - "velocityX": -1.8106719747325544, - "velocityY": 3.0528958313681422, - "timestamp": 5.53699221228965 - }, - { - "x": 1.2507109072486164, - "y": 3.4195717896549707, - "heading": -0.7601787682651481, - "angularVelocity": -0.5982236281934066, - "velocityX": -1.6819644879978934, - "velocityY": 2.835742616248544, - "timestamp": 5.595210324165121 - }, - { - "x": 1.160328320301069, - "y": 3.5719513486090504, - "heading": -0.7969704460064527, - "angularVelocity": -0.631962744171482, - "velocityX": -1.5524822780387872, - "velocityY": 2.6173909466528555, - "timestamp": 5.653428436040592 - }, - { - "x": 1.0774888591394964, - "y": 3.71161238889488, - "heading": -0.8336344675120989, - "angularVelocity": -0.6297700204374729, - "velocityX": -1.422915626991949, - "velocityY": 2.3989276839579965, - "timestamp": 5.711646547916063 - }, - { - "x": 1.0021908033919844, - "y": 3.838558158350536, - "heading": -0.8689739346571358, - "angularVelocity": -0.6070184347549548, - "velocityX": -1.2933785264038764, - "velocityY": 2.1805202086799977, - "timestamp": 5.7698646597915335 - }, - { - "x": 0.9344313462846832, - "y": 3.9527935365952946, - "heading": -0.9022118432658016, - "angularVelocity": -0.5709204152783613, - "velocityX": -1.1638896371672067, - "velocityY": 1.962196549573947, - "timestamp": 5.828082771667004 - }, - { - "x": 0.8742077702690397, - "y": 4.054323289061519, - "heading": -0.9328020557936417, - "angularVelocity": -0.5254415085338586, - "velocityX": -1.034447426678191, - "velocityY": 1.7439547452758104, - "timestamp": 5.886300883542475 - }, - { - "x": 0.8215176865940929, - "y": 4.143151679881797, - "heading": -0.9603390876580659, - "angularVelocity": -0.4729976802292441, - "velocityX": -0.9050462472512312, - "velocityY": 1.5257861850670007, - "timestamp": 5.944518995417946 - }, - { - "x": 0.7763590444591775, - "y": 4.21928243186123, - "heading": -0.984509600370908, - "angularVelocity": -0.41517170403161197, - "velocityX": -0.7756802939866982, - "velocityY": 1.3076815706816212, - "timestamp": 6.002737107293417 - }, - { - "x": 0.7387300854642734, - "y": 4.282718776761074, - "heading": -1.0050639989905081, - "angularVelocity": -0.3530584891445192, - "velocityX": -0.6463445443815322, - "velocityY": 1.0896324675649907, - "timestamp": 6.060955219168887 - }, - { - "x": 0.7086292911290721, - "y": 4.333463522832413, - "heading": -1.0217986916878445, - "angularVelocity": -0.28744822115035373, - "velocityX": -0.5170348773863939, - "velocityY": 0.8716316011739073, - "timestamp": 6.119173331044358 - }, - { - "x": 0.6860553360496396, - "y": 4.371519118466469, - "heading": -1.0345444282293665, - "angularVelocity": -0.21893077825652074, - "velocityX": -0.3877479765698806, - "velocityY": 0.6536727902728675, - "timestamp": 6.177391442919829 - }, - { - "x": 0.6710070489868055, - "y": 4.396887706764773, - "heading": -1.0431583071392123, - "angularVelocity": -0.14795874741302203, - "velocityX": -0.25848119387696106, - "velocityY": 0.4357507909663603, - "timestamp": 6.2356095547953 + "heading": -0.6963850232502631, + "angularVelocity": -2.818548242668458e-7, + "velocityX": -3.286187939959726, + "velocityY": 1.8719390328536167, + "timestamp": 4.954811112368034 + }, + { + "x": 2.574184199627506, + "y": 1.7387873388514508, + "heading": -0.696385046672039, + "angularVelocity": -4.0231081168076635e-7, + "velocityX": -3.153128229528173, + "velocityY": 2.0882929651305573, + "timestamp": 5.013029224414923 + }, + { + "x": 2.3991894743560565, + "y": 1.8724111676120654, + "heading": -0.6963850671596432, + "angularVelocity": -3.5191117316400294e-7, + "velocityX": -3.005846790952465, + "velocityY": 2.295227791876793, + "timestamp": 5.071247336461811 + }, + { + "x": 2.2335584944607234, + "y": 2.017479637711806, + "heading": -0.6963850855810837, + "angularVelocity": -3.164211261355859e-7, + "velocityX": -2.8450077488245626, + "velocityY": 2.4918099367925968, + "timestamp": 5.129465448508699 + }, + { + "x": 2.0780383142752217, + "y": 2.1733384097975033, + "heading": -0.6963851025529307, + "angularVelocity": -2.915217692304984e-7, + "velocityX": -2.6713367149427287, + "velocityY": 2.6771526352515695, + "timestamp": 5.1876835605555875 + }, + { + "x": 1.933330362591389, + "y": 2.339284462910405, + "heading": -0.6963851185385084, + "angularVelocity": -2.745808308512789e-7, + "velocityX": -2.4856173894352156, + "velocityY": 2.8504196937758937, + "timestamp": 5.245901672602476 + }, + { + "x": 1.8000872328431394, + "y": 2.5145692385052043, + "heading": -0.6963851339074891, + "angularVelocity": -2.63989679511656e-7, + "velocityX": -2.2886886067507177, + "velocityY": 3.010828923027044, + "timestamp": 5.304119784649364 + }, + { + "x": 1.6789094250107635, + "y": 2.6984018342496054, + "heading": -0.6963851489749523, + "angularVelocity": -2.5881057720147785e-7, + "velocityX": -2.0814451649476586, + "velocityY": 3.1576529928752177, + "timestamp": 5.3623378966962525 + }, + { + "x": 1.5665736783825392, + "y": 2.887766995894858, + "heading": -0.6963851639898362, + "angularVelocity": -2.5790743394160864e-7, + "velocityX": -1.9295669797356307, + "velocityY": 3.2526846884478298, + "timestamp": 5.420556008743141 + }, + { + "x": 1.454045590270684, + "y": 3.0767463661393277, + "heading": -0.6971250291224751, + "angularVelocity": -0.012708504391950055, + "velocityX": -1.9328707880672382, + "velocityY": 3.246058032460206, + "timestamp": 5.478774120790029 + }, + { + "x": 1.3486316890487728, + "y": 3.2544801987729373, + "heading": -0.7253513053199856, + "angularVelocity": -0.48483668063260893, + "velocityX": -1.8106719286433102, + "velocityY": 3.0528958494989498, + "timestamp": 5.536992232836917 + }, + { + "x": 1.250710894485142, + "y": 3.4195717811137616, + "heading": -0.7601787566171799, + "angularVelocity": -0.5982236467775679, + "velocityX": -1.6819644457856464, + "velocityY": 2.8357426329432034, + "timestamp": 5.595210344883806 + }, + { + "x": 1.1603283095238273, + "y": 3.571951341403833, + "heading": -0.796970435726129, + "angularVelocity": -0.631962765802461, + "velocityX": -1.5524822393505606, + "velocityY": 2.6173909618942433, + "timestamp": 5.653428456930694 + }, + { + "x": 1.0774888501735351, + "y": 3.7116123829065786, + "heading": -0.8336344586551708, + "angularVelocity": -0.6297700430325383, + "velocityX": -1.4229155916903404, + "velocityY": 2.398927697797285, + "timestamp": 5.711646568977582 + }, + { + "x": 1.0021907960664813, + "y": 3.8385581534626447, + "heading": -0.8689739272088923, + "angularVelocity": -0.6070184571643223, + "velocityX": -1.2933784944178508, + "velocityY": 2.1805202211611707, + "timestamp": 5.769864681024471 + }, + { + "x": 0.9344313404313289, + "y": 3.952793532693306, + "heading": -0.9022118371667394, + "angularVelocity": -0.5709204367719397, + "velocityX": -1.1638896084534611, + "velocityY": 1.9621965607310954, + "timestamp": 5.828082793071359 + }, + { + "x": 0.8742077657212176, + "y": 4.054323286032487, + "heading": -0.932802050953144, + "angularVelocity": -0.5254415286048384, + "velocityX": -1.034447401207516, + "velocityY": 1.743954755135484, + "timestamp": 5.886300905118247 + }, + { + "x": 0.8215176831864053, + "y": 4.143151677614014, + "heading": -0.9603390839625449, + "angularVelocity": -0.4729976985035671, + "velocityX": -0.9050462250025629, + "velocityY": 1.5257861936502937, + "timestamp": 5.944519017165136 + }, + { + "x": 0.7763590420271476, + "y": 4.219282430243998, + "heading": -0.9845095976890542, + "angularVelocity": -0.4151717202207242, + "velocityX": -0.7756802749441201, + "velocityY": 1.3076815780056374, + "timestamp": 6.002737129212024 + }, + { + "x": 0.7387300838441436, + "y": 4.282718775684524, + "heading": -1.005063997176884, + "angularVelocity": -0.3530585030183643, + "velocityX": -0.6463445285325996, + "velocityY": 1.0896324736438516, + "timestamp": 6.060955241258912 + }, + { + "x": 0.7086292901576616, + "y": 4.333463522187373, + "heading": -1.0217986905854597, + "angularVelocity": -0.2874482325207969, + "velocityX": -0.5170348647211226, + "velocityY": 0.871631606019453, + "timestamp": 6.119173353305801 + }, + { + "x": 0.6860553355642394, + "y": 4.371519118144364, + "heading": -1.0345444276715927, + "angularVelocity": -0.21893078696656726, + "velocityX": -0.3877479670800986, + "velocityY": 0.6536727938951569, + "timestamp": 6.177391465352689 + }, + { + "x": 0.6710070488250989, + "y": 4.396887706657534, + "heading": -1.043158306951245, + "angularVelocity": -0.1479587533294601, + "velocityX": -0.25848118755587396, + "velocityY": 0.4357507933740352, + "timestamp": 6.235609577399577 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -0.07488723824993983, - "velocityX": -0.12923242394972312, - "velocityY": 0.2178611369128849, - "timestamp": 6.29382766667077 + "angularVelocity": -0.07488724125811806, + "velocityX": -0.12923242079161093, + "velocityY": 0.2178611381134427, + "timestamp": 6.2938276894464655 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -4.5881817726943837e-32, - "velocityX": 7.1951222626888875e-34, - "velocityY": 0, - "timestamp": 6.352045778546241 - }, - { - "x": 0.6704852067504524, - "y": 4.397985362145517, - "heading": -1.0417684453451177, - "angularVelocity": 0.10293302663784343, - "velocityX": 0.12534996227388928, - "velocityY": -0.20741457823419143, - "timestamp": 6.40790399622796 - }, - { - "x": 0.684490344241533, - "y": 4.374811471822251, - "heading": -1.0303744644017594, - "angularVelocity": 0.20398038849505432, - "velocityX": 0.25072653715666804, - "velocityY": -0.4148698487895252, - "timestamp": 6.463762213909678 - }, - { - "x": 0.7055004573054668, - "y": 4.340046908929331, - "heading": -1.0134578875635165, - "angularVelocity": 0.3028484892703545, - "velocityX": 0.3761328938859112, - "velocityY": -0.6223715029901059, - "timestamp": 6.519620431591397 - }, - { - "x": 0.7335174330880717, - "y": 4.293688705050122, - "heading": -0.9911609198549822, - "angularVelocity": 0.3991707690278106, - "velocityX": 0.5015730351843025, - "velocityY": -0.8299262991769599, - "timestamp": 6.575478649273116 - }, - { - "x": 0.768543442750825, - "y": 4.235733440885856, - "heading": -0.963651742055662, - "angularVelocity": 0.49248219762521406, - "velocityX": 0.6270520456333297, - "velocityY": -1.0375423092533531, - "timestamp": 6.631336866954834 - }, - { - "x": 0.8105810218384888, - "y": 4.16617715590748, - "heading": -0.9311321668224927, - "angularVelocity": 0.5821806814257259, - "velocityX": 0.7525764485933851, - "velocityY": -1.24522922257758, - "timestamp": 6.687195084636553 - }, - { - "x": 0.8596331801158593, - "y": 4.085015237354172, - "heading": -0.8938487217946981, - "angularVelocity": 0.667465711853477, - "velocityX": 0.8781547337738402, - "velocityY": -1.4529987157085942, - "timestamp": 6.7430533023182715 - }, - { - "x": 0.9157035549190764, - "y": 3.992242285598939, - "heading": -0.8521094876337724, - "angularVelocity": 0.7472353378469229, - "velocityX": 1.0037981362510981, - "velocityY": -1.6608648754934532, - "timestamp": 6.79891151999999 - }, - { - "x": 0.9787966256099634, - "y": 3.88785195918136, - "heading": -0.8063112110880574, - "angularVelocity": 0.8199022175514963, - "velocityX": 1.1295217303637053, - "velocityY": -1.868844563075711, - "timestamp": 6.854769737681709 - }, - { - "x": 1.0489179988671087, - "y": 3.7718368324625953, - "heading": -0.756986124262755, - "angularVelocity": 0.8830408285913816, - "velocityX": 1.2553456978648867, - "velocityY": -2.0769571879257267, - "timestamp": 6.910627955363427 - }, - { - "x": 1.1260747058581024, - "y": 3.644188427808813, - "heading": -0.7048901245652565, - "angularVelocity": 0.9326470098695786, - "velocityX": 1.381295540624565, - "velocityY": -2.2852215833510225, - "timestamp": 6.966486173045146 - }, - { - "x": 1.210275065586603, - "y": 3.504898185530833, - "heading": -0.6511892097412871, - "angularVelocity": 0.9613789528688276, - "velocityX": 1.5073943140877994, - "velocityY": -2.4936392183449136, - "timestamp": 7.022344390726865 - }, - { - "x": 1.3015253174439423, - "y": 3.3539635614800978, - "heading": -0.5979268038301901, - "angularVelocity": 0.95352856073188, - "velocityX": 1.6336047880597582, - "velocityY": -2.7021023998790135, - "timestamp": 7.078202608408583 - }, - { - "x": 1.3998011054306767, - "y": 3.1914330516367446, - "heading": -0.5495841694257507, - "angularVelocity": 0.865452504766571, - "velocityX": 1.7593792295112722, - "velocityY": -2.909697383641142, - "timestamp": 7.134060826090302 - }, - { - "x": 1.504860441802437, - "y": 3.018388823031747, - "heading": -0.5232691895419826, - "angularVelocity": 0.4711031066854229, - "velocityX": 1.8808214929160718, - "velocityY": -3.0979189058807544, - "timestamp": 7.18991904377202 - }, - { - "x": 1.6140923684634172, - "y": 2.8375675393930413, - "heading": -0.5232691688026271, - "angularVelocity": 3.712856650457212e-7, - "velocityX": 1.9555211604385054, - "velocityY": -3.237147391079152, - "timestamp": 7.245777261453739 - }, - { - "x": 1.7233256383679056, - "y": 2.6567470671976134, - "heading": -0.5232691480644303, - "angularVelocity": 3.7126492252401944e-7, - "velocityX": 1.9555452078135154, - "velocityY": -3.2371328642411785, - "timestamp": 7.301635479135458 - }, - { - "x": 1.84078732146222, - "y": 2.4811603268646896, - "heading": -0.5232691272677354, - "angularVelocity": 3.72312181556421e-7, - "velocityX": 2.102854118325333, - "velocityY": -3.143436142796787, - "timestamp": 7.357493696817176 - }, - { - "x": 1.9693130123787856, - "y": 2.313502648911294, - "heading": -0.5232691060970931, - "angularVelocity": 3.790067651022537e-7, - "velocityX": 2.3009271733106282, - "velocityY": -3.0014863508304788, - "timestamp": 7.413351914498895 - }, - { - "x": 2.1083695489461487, - "y": 2.154470483882732, - "heading": -0.5232690841533676, - "angularVelocity": 3.928468612451328e-7, - "velocityX": 2.4894553091491622, - "velocityY": -2.847068374697022, - "timestamp": 7.4692101321806135 - }, - { - "x": 2.2573796500994945, - "y": 2.0047242347352134, - "heading": -0.5232690609733034, - "angularVelocity": 4.1498037571099624e-7, - "velocityX": 2.6676486887284834, - "velocityY": -2.6808275552359513, - "timestamp": 7.525068349862332 - }, - { - "x": 2.4157246359676448, - "y": 1.8648857059384452, - "heading": -0.5232690359846437, - "angularVelocity": 4.47358702857604e-7, - "velocityX": 2.8347661712087695, - "velocityY": -2.5034549006481424, - "timestamp": 7.580926567544051 - }, - { - "x": 2.582747044375024, - "y": 1.7355355500002163, - "heading": -0.523269008439205, - "angularVelocity": 4.931313565110266e-7, - "velocityX": 2.9901134575950428, - "velocityY": -2.3156871326484083, - "timestamp": 7.636784785225769 + "angularVelocity": 5.982509217155958e-25, + "velocityX": 4.848331418564855e-25, + "velocityY": 2.001430584095579e-25, + "timestamp": 6.352045801493354 + }, + { + "x": 0.6704852066209848, + "y": 4.3979853620663825, + "heading": -1.0417684451600073, + "angularVelocity": 0.10293302967346842, + "velocityX": 0.1253499596171797, + "velocityY": -0.20741457909008942, + "timestamp": 6.407904019326101 + }, + { + "x": 0.684490343853258, + "y": 4.3748114715841036, + "heading": -1.0303744638705372, + "angularVelocity": 0.20398039413979072, + "velocityX": 0.2507265318454658, + "velocityY": -0.414869850514514, + "timestamp": 6.463762237158848 + }, + { + "x": 0.7055004565291726, + "y": 4.340046908451465, + "heading": -1.013457886552716, + "angularVelocity": 0.3028484970371536, + "velocityX": 0.37613288592242283, + "velocityY": -0.6223715055989062, + "timestamp": 6.519620454991595 + }, + { + "x": 0.7335174317946694, + "y": 4.293688704250901, + "heading": -0.9911609182628701, + "angularVelocity": 0.39917077835544595, + "velocityX": 0.501573024570643, + "velocityY": -0.8299263026860625, + "timestamp": 6.575478672824342 + }, + { + "x": 0.7685434408113375, + "y": 4.235733439682599, + "heading": -0.9636517398174463, + "angularVelocity": 0.4924822078604928, + "velocityX": 0.6270520323713931, + "velocityY": -1.0375423136812831, + "timestamp": 6.6313368906570895 + }, + { + "x": 0.8105810191240276, + "y": 4.166177154216333, + "heading": -0.9311321639168866, + "angularVelocity": 0.5821806917995676, + "velocityX": 0.7525764326846329, + "velocityY": -1.2452292279451722, + "timestamp": 6.687195108489837 + }, + { + "x": 0.8596331764975813, + "y": 4.085015235089962, + "heading": -0.8938487182523878, + "angularVelocity": 0.6674657214473685, + "velocityX": 0.8781547152189431, + "velocityY": -1.4529987220392506, + "timestamp": 6.743053326322584 + }, + { + "x": 0.9157035502681051, + "y": 3.9922422826750155, + "heading": -0.85210948354869, + "angularVelocity": 0.7472353455435148, + "velocityX": 1.0037981150492816, + "velocityY": -1.660864882813316, + "timestamp": 6.798911544155331 + }, + { + "x": 0.9787966197972374, + "y": 3.8878519555094497, + "heading": -0.8063112066330053, + "angularVelocity": 0.8199022219580233, + "velocityX": 1.1295217065114396, + "velocityY": -1.8688445714135682, + "timestamp": 6.854769761988078 + }, + { + "x": 1.0489179917630689, + "y": 3.7718368279526966, + "heading": -0.7569861197121166, + "angularVelocity": 0.883040827915056, + "velocityX": 1.255345671352987, + "velocityY": -2.076957197312126, + "timestamp": 6.910627979820825 + }, + { + "x": 1.1260746973319842, + "y": 3.6441884223692846, + "heading": -0.7048901203297451, + "angularVelocity": 0.9326470017063425, + "velocityX": 1.3812955114311163, + "velocityY": -2.2852215938149305, + "timestamp": 6.966486197653572 + }, + { + "x": 1.2102750555046553, + "y": 3.5048981790691265, + "heading": -0.6511892064235582, + "angularVelocity": 0.9613789338388887, + "velocityX": 1.5073942821589288, + "velocityY": -2.493639229902148, + "timestamp": 7.0223444154863195 + }, + { + "x": 1.301525305664333, + "y": 3.3539635539059023, + "heading": -0.5979268023129009, + "angularVelocity": 0.9535285259214237, + "velocityX": 1.6336047532505, + "velocityY": -2.7021024124893804, + "timestamp": 7.078202633319067 + }, + { + "x": 1.3998010917877863, + "y": 3.1914330428750026, + "heading": -0.5495841709370638, + "angularVelocity": 0.8654524482071109, + "velocityX": 1.759379191396948, + "velocityY": -2.909697397033947, + "timestamp": 7.134060851151814 + }, + { + "x": 1.504860426118042, + "y": 3.0183888128426037, + "heading": -0.5232691930367682, + "angularVelocity": 0.4711030699026016, + "velocityX": 1.880821451282748, + "velocityY": -3.0979189230586286, + "timestamp": 7.189919068984561 + }, + { + "x": 1.6140923512047893, + "y": 2.837567528819654, + "heading": -0.5232691722973926, + "angularVelocity": 3.712860260075055e-7, + "velocityX": 1.9555211269685275, + "velocityY": -3.2371473892055076, + "timestamp": 7.245777286817308 + }, + { + "x": 1.723325651827911, + "y": 2.656747075748015, + "heading": -0.5232691515591942, + "angularVelocity": 3.712649483056031e-7, + "velocityX": 1.9555457524655004, + "velocityY": -3.2371325131256046, + "timestamp": 7.301635504650055 + }, + { + "x": 1.840787332697237, + "y": 2.481160334510265, + "heading": -0.5232691307624974, + "angularVelocity": 3.7231221667363717e-7, + "velocityX": 2.1028540728068745, + "velocityY": -3.143436150496223, + "timestamp": 7.357493722482802 + }, + { + "x": 1.969313021479893, + "y": 2.3135026555322407, + "heading": -0.5232691095918528, + "angularVelocity": 3.790068020002191e-7, + "velocityX": 2.300927128887148, + "velocityY": -3.0014863610584763, + "timestamp": 7.4133519403155494 + }, + { + "x": 2.108369556011401, + "y": 2.15447048936791, + "heading": -0.5232690876481252, + "angularVelocity": 3.9284689919031285e-7, + "velocityX": 2.4894552659713574, + "velocityY": -2.8470683873322, + "timestamp": 7.469210158148297 + }, + { + "x": 2.2573796552323504, + "y": 2.004724238981821, + "heading": -0.5232690644680587, + "angularVelocity": 4.1498041678709737e-7, + "velocityX": 2.6676486469210485, + "velocityY": -2.680827570161024, + "timestamp": 7.525068375981044 + }, + { + "x": 2.4157246392762546, + "y": 1.8648857088521948, + "heading": -0.5232690394793963, + "angularVelocity": 4.473587475801905e-7, + "velocityX": 2.8347661308856478, + "velocityY": -2.5034549177407666, + "timestamp": 7.580926593813791 + }, + { + "x": 2.582747045971562, + "y": 1.7355355514954984, + "heading": -0.5232690119339547, + "angularVelocity": 4.931314075834542e-7, + "velocityX": 2.990113418860087, + "velocityY": -2.3156871517813467, + "timestamp": 7.636784811646538 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.5232689753097743, - "angularVelocity": 5.930985989560848e-7, - "velocityX": 3.133045325838173, - "velocityY": -2.1183039826598433, - "timestamp": 7.692643002907488 - }, - { - "x": 3.027691141146945, - "y": 1.4695236072310565, - "heading": -0.5232689479285146, - "angularVelocity": 3.365468040910692e-7, - "velocityX": 3.3178420048715034, - "velocityY": -1.8152442667680868, - "timestamp": 7.7740024433384125 - }, - { - "x": 3.3102859882061892, - "y": 1.3477940719025636, - "heading": -0.523268924510295, - "angularVelocity": 2.8783653775450794e-7, - "velocityX": 3.473411881429686, - "velocityY": -1.4961943529078627, - "timestamp": 7.855361883769337 - }, - { - "x": 3.6030485594101127, - "y": 1.2530945172715204, - "heading": -0.5232689034957836, - "angularVelocity": 2.582922326531371e-7, - "velocityX": 3.5983847682984376, - "velocityY": -1.1639651665432065, - "timestamp": 7.936721324200262 - }, - { - "x": 3.9033999272897986, - "y": 1.1862590073495238, - "heading": -0.5232688838271395, - "angularVelocity": 2.4174999152857024e-7, - "velocityX": 3.6916597052396023, - "velocityY": -0.8214843854382324, - "timestamp": 8.018080764631186 - }, - { - "x": 4.2086942600952995, - "y": 1.1478754132776305, - "heading": -0.5232688646920325, - "angularVelocity": 2.3519221562308788e-7, - "velocityX": 3.752414362592638, - "velocityY": -0.471777999806691, - "timestamp": 8.09944020506211 - }, - { - "x": 4.515097551396115, - "y": 1.1196805012022473, - "heading": -0.5232688456111896, - "angularVelocity": 2.345252478429259e-7, - "velocityX": 3.7660447229963117, - "velocityY": -0.3465475171172218, - "timestamp": 8.180799645493035 - }, - { - "x": 4.821500870663513, - "y": 1.0914858930504614, - "heading": -0.5232688265303463, - "angularVelocity": 2.3452525108540152e-7, - "velocityX": 3.7660450667373904, - "velocityY": -0.3465437815507533, - "timestamp": 8.26215908592396 - }, - { - "x": 5.127904189931636, - "y": 1.0632912849065406, - "heading": -0.5232688074495031, - "angularVelocity": 2.3452525107290264e-7, - "velocityX": 3.7660450667462855, - "velocityY": -0.34654378145408427, - "timestamp": 8.343518526354885 - }, - { - "x": 5.4343075091997575, - "y": 1.0350966767626197, - "heading": -0.5232687883686599, - "angularVelocity": 2.3452525075472094e-7, - "velocityX": 3.7660450667462855, - "velocityY": -0.3465437814540822, - "timestamp": 8.42487796678581 - }, - { - "x": 5.740710828467879, - "y": 1.0069020686186991, - "heading": -0.5232687692878167, - "angularVelocity": 2.3452525040262446e-7, - "velocityX": 3.7660450667462855, - "velocityY": -0.3465437814540818, - "timestamp": 8.506237407216734 - }, - { - "x": 6.047114147736001, - "y": 0.9787074604747784, - "heading": -0.5232687502069736, - "angularVelocity": 2.345252500146494e-7, - "velocityX": 3.766045066746286, - "velocityY": -0.34654378145408327, - "timestamp": 8.587596847647658 - }, - { - "x": 6.353517467003837, - "y": 0.9505128523277552, - "heading": -0.5232687311261306, - "angularVelocity": 2.3452524994005725e-7, - "velocityX": 3.7660450667427767, - "velocityY": -0.3465437814922157, - "timestamp": 8.668956288078583 - }, - { - "x": 6.659920775239498, - "y": 0.9223181242925758, - "heading": -0.5232687120445318, - "angularVelocity": 2.3453453739742123e-7, - "velocityX": 3.7660449311448136, - "velocityY": -0.3465452550539253, - "timestamp": 8.750315728509507 - }, - { - "x": 6.946350295372631, - "y": 0.8955257235290481, - "heading": -0.4609901004295649, - "angularVelocity": 0.7654749256522044, - "velocityX": 3.5205443721840264, - "velocityY": -0.3293090589318255, - "timestamp": 8.831675168940432 - }, - { - "x": 7.204305307998309, - "y": 0.8715129005451068, - "heading": -0.38283013166037505, - "angularVelocity": 0.9606748566019065, - "velocityX": 3.170560309404857, - "velocityY": -0.2951448886171808, - "timestamp": 8.913034609371357 - }, - { - "x": 7.433510095635826, - "y": 0.8502195253793108, - "heading": -0.30543025852913497, - "angularVelocity": 0.9513324172497697, - "velocityX": 2.8171873653938824, - "velocityY": -0.2617197838752883, - "timestamp": 8.994394049802281 - }, - { - "x": 7.633994056446923, - "y": 0.8316157363547472, - "heading": -0.2335287634090791, - "angularVelocity": 0.8837511017679792, - "velocityX": 2.4641757582060952, - "velocityY": -0.22866171308489489, - "timestamp": 9.075753490233206 - }, - { - "x": 7.805788503499911, - "y": 0.8156861724274476, - "heading": -0.16939985967365537, - "angularVelocity": 0.7882171189447914, - "velocityX": 2.1115490242198023, - "velocityY": -0.19579244698498122, - "timestamp": 9.15711293066413 - }, - { - "x": 7.948917170139319, - "y": 0.802421883536533, - "heading": -0.1143881084380515, - "angularVelocity": 0.6761569517222741, - "velocityX": 1.759214000014262, - "velocityY": -0.16303318730634864, - "timestamp": 9.238472371095055 - }, - { - "x": 8.063397872753223, - "y": 0.7918170647898031, - "heading": -0.0693839808587397, - "angularVelocity": 0.5531518818338115, - "velocityX": 1.407097959469132, - "velocityY": -0.13034527635097967, - "timestamp": 9.31983181152598 - }, - { - "x": 8.149244274991434, - "y": 0.7838676118817843, - "heading": -0.0350206058237791, - "angularVelocity": 0.4223649382659615, - "velocityX": 1.0551498607109575, - "velocityY": -0.09770781197503392, - "timestamp": 9.401191251956904 - }, - { - "x": 8.20646710981356, - "y": 0.7785704017832406, - "heading": -0.011770656016753542, - "angularVelocity": 0.2857683101540642, - "velocityX": 0.703333682225969, - "velocityY": -0.06510873317818848, - "timestamp": 9.482550692387829 + "heading": -0.5232689788045201, + "angularVelocity": 5.930986675824575e-7, + "velocityX": 3.1330452887851035, + "velocityY": -2.1183040037016325, + "timestamp": 7.692643029479285 + }, + { + "x": 3.027691139544385, + "y": 1.469523604710322, + "heading": -0.5232689514232575, + "angularVelocity": 3.3654683913245854e-7, + "velocityX": 3.317841970698504, + "velocityY": -1.8152442898308883, + "timestamp": 7.77400247026518 + }, + { + "x": 3.310285985294353, + "y": 1.3477940668377997, + "heading": -0.5232689280050355, + "angularVelocity": 2.8783656375516125e-7, + "velocityX": 3.473411850182739, + "velocityY": -1.4961943776489928, + "timestamp": 7.855361911051075 + }, + { + "x": 3.603048555472094, + "y": 1.2530945096711932, + "heading": -0.5232689069905222, + "angularVelocity": 2.58292255489841e-7, + "velocityX": 3.5983847399857765, + "velocityY": -1.1639651926297916, + "timestamp": 7.93672135183697 + }, + { + "x": 3.9033999225952147, + "y": 1.1862589972516577, + "heading": -0.5232688873218765, + "angularVelocity": 2.41750010150048e-7, + "velocityX": 3.6916596798338883, + "velocityY": -0.8214844125516969, + "timestamp": 8.018080792622865 + }, + { + "x": 4.208694254897112, + "y": 1.1478754007454768, + "heading": -0.5232688681867681, + "angularVelocity": 2.3519223109451003e-7, + "velocityX": 3.7524143400310193, + "velocityY": -0.47177802766849325, + "timestamp": 8.09944023340876 + }, + { + "x": 4.515097545025261, + "y": 1.1196804780654097, + "heading": -0.523268849105924, + "angularVelocity": 2.3452526230425957e-7, + "velocityX": 3.7660446921516857, + "velocityY": -0.34654764594885085, + "timestamp": 8.180799674194656 + }, + { + "x": 4.821500864274129, + "y": 1.0914858718513367, + "heading": -0.5232688300250796, + "angularVelocity": 2.3452526515688585e-7, + "velocityX": 3.766045050078413, + "velocityY": -0.34654375622209216, + "timestamp": 8.262159114980552 + }, + { + "x": 5.1279041835237855, + "y": 1.0632912656458422, + "heading": -0.523268810944235, + "angularVelocity": 2.3452526522511142e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.34654375611665356, + "timestamp": 8.343518555766448 + }, + { + "x": 5.434307502773443, + "y": 1.035096659440348, + "heading": -0.5232687918633908, + "angularVelocity": 2.3452526444881321e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.3465437561166505, + "timestamp": 8.424877996552343 + }, + { + "x": 5.7407108220231, + "y": 1.0069020532348534, + "heading": -0.5232687727825464, + "angularVelocity": 2.345252646023774e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.34654375611665045, + "timestamp": 8.506237437338239 + }, + { + "x": 6.047114141272757, + "y": 0.9787074470293591, + "heading": -0.5232687537017021, + "angularVelocity": 2.3452526402445505e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.3465437561166515, + "timestamp": 8.587596878124135 + }, + { + "x": 6.353517460522115, + "y": 0.9505128408206217, + "heading": -0.5232687346208578, + "angularVelocity": 2.3452526381848406e-7, + "velocityX": 3.766045050084448, + "velocityY": -0.3465437561565115, + "timestamp": 8.66895631891003 + }, + { + "x": 6.659920768762443, + "y": 0.9223181149752775, + "heading": -0.5232687155392478, + "angularVelocity": 2.3453467400509184e-7, + "velocityX": 3.7660449147709487, + "velocityY": -0.3465452266263895, + "timestamp": 8.750315759695926 + }, + { + "x": 6.9463502900273895, + "y": 0.8955257159158118, + "heading": -0.4609901078524047, + "angularVelocity": 0.7654748740313396, + "velocityX": 3.5205443707352013, + "velocityY": -0.3293090365501957, + "timestamp": 8.831675200481822 + }, + { + "x": 7.204305303751847, + "y": 0.8715128944520383, + "heading": -0.38283014022876516, + "angularVelocity": 0.9606748383303768, + "velocityX": 3.1705603090769854, + "velocityY": -0.29514486864487394, + "timestamp": 8.913034641267718 + }, + { + "x": 7.433510092361594, + "y": 0.8502195206373326, + "heading": -0.3054302667455531, + "angularVelocity": 0.9513324174252511, + "velocityX": 2.8171873650523347, + "velocityY": -0.2617197661269713, + "timestamp": 8.994394082053613 + }, + { + "x": 7.6339940540118905, + "y": 0.8316157327961914, + "heading": -0.23352877050863402, + "angularVelocity": 0.8837511116397012, + "velocityX": 2.464175757769631, + "velocityY": -0.2286616975416389, + "timestamp": 9.07575352283951 + }, + { + "x": 7.805788501774165, + "y": 0.8156861698842703, + "heading": -0.16939986530394652, + "angularVelocity": 0.7882171335647298, + "velocityX": 2.111549023725079, + "velocityY": -0.1957924336505852, + "timestamp": 9.157112963625405 + }, + { + "x": 7.948917168997056, + "y": 0.8024218818402508, + "heading": -0.11438811251079256, + "angularVelocity": 0.6761569679162687, + "velocityX": 1.7592139995104843, + "velocityY": -0.16303317618573446, + "timestamp": 9.2384724044113 + }, + { + "x": 8.063397872072363, + "y": 0.7918170637715687, + "heading": -0.06938398346942785, + "angularVelocity": 0.5531518973907075, + "velocityX": 1.4070979590011485, + "velocityY": -0.13034526744830524, + "timestamp": 9.319831845197196 + }, + { + "x": 8.149244274653059, + "y": 0.7838676113724481, + "heading": -0.03502060720303478, + "angularVelocity": 0.422364951558892, + "velocityX": 1.0551498603168807, + "velocityY": -0.09770780529379805, + "timestamp": 9.401191285983092 + }, + { + "x": 8.206467109701398, + "y": 0.7785704016133925, + "heading": -0.011770656498572084, + "angularVelocity": 0.2857683199377824, + "velocityX": 0.7033336819377413, + "velocityY": -0.06510872872142374, + "timestamp": 9.482550726768988 + }, + { + "x": 8.235074996948242, + "y": 0.7759228944778442, + "heading": 4.4114982490549674e-27, + "angularVelocity": 0.14467474683789522, + "velocityX": 0.35162344984804855, + "velocityY": -0.032540871839514214, + "timestamp": 9.563910167554884 }, { "x": 8.235074996948242, "y": 0.7759228944778442, - "heading": -3.5413188929792264e-38, - "angularVelocity": 0.1446747415470122, - "velocityX": 0.35162345000356726, - "velocityY": -0.03254087406911382, - "timestamp": 9.563910132818753 + "heading": 2.1650776701546007e-27, + "angularVelocity": -8.454240815185174e-28, + "velocityX": -3.4252443089053894e-26, + "velocityY": -2.509402795665863e-26, + "timestamp": 9.64526960834078 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "timestamp": 1.34208020603165, + "isStopPoint": false, + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 21 + }, + { + "timestamp": 2.673367901531957, + "isStopPoint": false, + "x": 7.340087890625, + "y": 0.7580231428146362, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 3.118573680917309, + "isStopPoint": false, + "x": 7.841280937194824, + "y": 0.7580231428146362, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 4.954811112368034, + "isStopPoint": false, + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "timestamp": 6.352045801493354, + "isStopPoint": true, + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "timestamp": 7.692643029479285, + "isStopPoint": false, + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 }, { + "timestamp": 9.64526960834078, + "isStopPoint": true, "x": 8.235074996948242, "y": 0.7759228944778442, - "heading": -7.544724392466369e-38, - "angularVelocity": -4.920640404901405e-37, - "velocityX": -6.041520920103385e-38, - "velocityY": 3.1783876166730987e-37, - "timestamp": 9.645269573249678 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -3767,25 +3129,30 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 5 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "CenterLanePSubCSubBSubASubSprint": { "waypoints": [ @@ -3893,1318 +3260,1448 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -7.2072188294937895e-31, - "velocityX": -1.1048793733131498e-32, - "velocityY": -5.425421859025623e-32, + "heading": -4.063364103945615e-24, + "angularVelocity": -2.020629205800752e-24, + "velocityX": -1.3550816656042328e-24, + "velocityY": 1.2971572394891071e-24, "timestamp": 0 }, { - "x": 1.3063416372973582, - "y": 5.5750227907482355, - "heading": -0.009007895171544168, - "angularVelocity": -0.12420787311314503, - "velocityX": 0.22568350897133957, - "velocityY": -0.21967610709029223, - "timestamp": 0.07252273906455099 - }, - { - "x": 1.339076097606074, - "y": 5.5431599730454195, - "heading": -0.02701891548605667, - "angularVelocity": -0.24834997335784384, - "velocityX": 0.4513682292059063, - "velocityY": -0.43934934220379995, - "timestamp": 0.14504547812910198 - }, - { - "x": 1.388178099815531, - "y": 5.4953660820311345, - "heading": -0.05401968965283247, - "angularVelocity": -0.3723076998340179, - "velocityX": 0.677056642410746, - "velocityY": -0.6590193866197215, - "timestamp": 0.217568217193653 - }, - { - "x": 1.4536480974194865, - "y": 5.431641407654605, - "heading": -0.08998953758011774, - "angularVelocity": -0.49598027310141063, - "velocityX": 0.9027513087406556, - "velocityY": -0.8786854329899796, - "timestamp": 0.29009095625820397 - }, - { - "x": 1.5354867341790108, - "y": 5.351986341420191, - "heading": -0.13490221177586337, - "angularVelocity": -0.6192909255093908, - "velocityX": 1.1284548517495026, - "velocityY": -1.0983460809932237, - "timestamp": 0.36261369532275495 - }, - { - "x": 1.6336948546750298, - "y": 5.2564014251813616, - "heading": -0.18872783549465025, - "angularVelocity": -0.7421896140863778, - "velocityX": 1.3541700404978287, - "velocityY": -1.317999257498918, - "timestamp": 0.4351364343873059 - }, - { - "x": 1.7482735602297292, - "y": 5.144887415721353, - "heading": -0.25143410509925246, - "angularVelocity": -0.8646428749583058, - "velocityX": 1.579900415134063, - "velocityY": -1.5376419988880228, - "timestamp": 0.507659173451857 - }, - { - "x": 1.87922458951427, - "y": 5.017445479597787, - "heading": -0.32298143653104017, - "angularVelocity": -0.9865503200055588, - "velocityX": 1.8056547639221738, - "velocityY": -1.7572686548722531, - "timestamp": 0.5801819125164079 - }, - { - "x": 2.026417113452321, - "y": 4.874266246183284, - "heading": -0.4026826239248217, - "angularVelocity": -1.0989820354525424, - "velocityX": 2.0296051395270776, - "velocityY": -1.9742667646213359, - "timestamp": 0.6527046515809589 - }, - { - "x": 2.1572394289562786, - "y": 4.747014797378468, - "heading": -0.4735071808339005, - "angularVelocity": -0.9765841420579419, - "velocityX": 1.8038799580839464, - "velocityY": -1.7546420673865035, - "timestamp": 0.7252273906455099 - }, - { - "x": 2.27168995904481, - "y": 4.635690037684074, - "heading": -0.535478944048102, - "angularVelocity": -0.8545149288832774, - "velocityX": 1.578133031994185, - "velocityY": -1.5350324757486038, - "timestamp": 0.7977501297100609 - }, - { - "x": 2.369767924446142, - "y": 4.540291261559593, - "heading": -0.5886064902849578, - "angularVelocity": -0.732563978168669, - "velocityX": 1.35237536069838, - "velocityY": -1.3154326126526381, - "timestamp": 0.8702728687746119 - }, - { - "x": 2.4514728066095985, - "y": 4.460817939321001, - "heading": -0.6328935848970949, - "angularVelocity": -0.6106649470681784, - "velocityX": 1.1266105392220715, - "velocityY": -1.0958400532524892, - "timestamp": 0.9427956078391628 - }, - { - "x": 2.5168042340609067, - "y": 4.397269668318764, - "heading": -0.6683418026498548, - "angularVelocity": -0.48878763005849607, - "velocityX": 0.9008405955692155, - "velocityY": -0.8762530458976289, - "timestamp": 1.015318346903714 - }, - { - "x": 2.5657619428441123, - "y": 4.349646151783931, - "heading": -0.6949516025618677, - "angularVelocity": -0.36691664235512766, - "velocityX": 0.6750670122861077, - "velocityY": -0.6566701306241847, - "timestamp": 1.087841085968265 - }, - { - "x": 2.5983457601971756, - "y": 4.317947187379743, - "heading": -0.7127228546140986, - "angularVelocity": -0.24504386184805346, - "velocityX": 0.4492910468254882, - "velocityY": -0.4370900053275183, - "timestamp": 1.160363825032816 + "x": 1.3063227037698195, + "y": 5.575041446046491, + "heading": -0.008997103572259477, + "angularVelocity": -0.12413133595120028, + "velocityX": 0.22555374987191792, + "velocityY": -0.2195466872880714, + "timestamp": 0.0724805183422541 + }, + { + "x": 1.3390193004314572, + "y": 5.543215943390215, + "heading": -0.026986588243377234, + "angularVelocity": -0.2481975168302625, + "velocityX": 0.45110875873215955, + "velocityY": -0.4390904395301923, + "timestamp": 0.1449610366845082 + }, + { + "x": 1.3880645131173623, + "y": 5.495478033090953, + "heading": -0.053955156680249416, + "angularVelocity": -0.3720802369200259, + "velocityX": 0.6766675212546471, + "velocityY": -0.6586309175362454, + "timestamp": 0.21744155502676227 + }, + { + "x": 1.4534588011433731, + "y": 5.431828013300948, + "heading": -0.0898822289659018, + "angularVelocity": -0.4956790197885205, + "velocityX": 0.9022326208708659, + "velocityY": -0.878167281992224, + "timestamp": 0.2899220733690164 + }, + { + "x": 1.535202817150975, + "y": 5.352266287723616, + "heading": -0.1347416873383059, + "angularVelocity": -0.6189174608351592, + "velocityX": 1.1278067248581949, + "velocityY": -1.0976980766285507, + "timestamp": 0.3624025917112705 + }, + { + "x": 1.6332974209934847, + "y": 5.25679341851918, + "heading": -0.1885038314662501, + "angularVelocity": -0.7417461320306592, + "velocityX": 1.3533926920790726, + "velocityY": -1.3172211152465878, + "timestamp": 0.4348831100535246 + }, + { + "x": 1.7477437450196447, + "y": 5.145410203108038, + "heading": -0.2511366541198319, + "angularVelocity": -0.8641332055301907, + "velocityX": 1.5789942821013316, + "velocityY": -1.5367331520062975, + "timestamp": 0.5073636283957786 + }, + { + "x": 1.8785436154915462, + "y": 5.018117926270065, + "heading": -0.3226014402327865, + "angularVelocity": -0.9859861345844242, + "velocityX": 1.8046210687161843, + "velocityY": -1.7562274629010988, + "timestamp": 0.5798441467380328 + }, + { + "x": 2.0257354638148333, + "y": 4.874937893317761, + "heading": -0.40230013407560183, + "angularVelocity": -1.0995878018763188, + "velocityX": 2.0307780861644047, + "velocityY": -1.975427828429777, + "timestamp": 0.6523246650802869 + }, + { + "x": 2.156576334610242, + "y": 4.74766733108212, + "heading": -0.4731338136564732, + "angularVelocity": -0.9772788771514256, + "velocityX": 1.8051867424233425, + "velocityY": -1.7559278706406076, + "timestamp": 0.724805183422541 + }, + { + "x": 2.271064512399631, + "y": 4.636304989038449, + "heading": -0.5351259426845023, + "angularVelocity": -0.8552936767822404, + "velocityX": 1.579571730554888, + "velocityY": -1.5364451661040166, + "timestamp": 0.7972857017647951 + }, + { + "x": 2.3691991759335136, + "y": 4.540850112955004, + "heading": -0.5882849412728991, + "angularVelocity": -0.7334246471221314, + "velocityX": 1.3539453880626124, + "velocityY": -1.3169728675601617, + "timestamp": 0.8697662201070492 + }, + { + "x": 2.4509797863223954, + "y": 4.461302149238075, + "heading": -0.6326144930208853, + "angularVelocity": -0.611606439383635, + "velocityX": 1.1283116106139404, + "velocityY": -1.0975082068439748, + "timestamp": 0.9422467384493033 + }, + { + "x": 2.5164059599659825, + "y": 4.397660680894242, + "heading": -0.6681161230461814, + "angularVelocity": -0.4898092734058101, + "velocityX": 0.9026725407045777, + "velocityY": -0.8780492993071322, + "timestamp": 1.0147272567915573 + }, + { + "x": 2.5654774247056453, + "y": 4.349925401446345, + "heading": -0.6947902571446234, + "angularVelocity": -0.3680179820525904, + "velocityX": 0.677029715874088, + "velocityY": -0.6585946201776504, + "timestamp": 1.0872077751338114 + }, + { + "x": 2.598194001702078, + "y": 4.3180961013994725, + "heading": -0.7126367415892834, + "angularVelocity": -0.24622456975802207, + "velocityX": 0.45138442363152775, + "velocityY": -0.4391428314098807, + "timestamp": 1.1596882934760655 }, { "x": 2.614555597305298, "y": 4.302172660827637, "heading": -0.7216551150961179, - "angularVelocity": -0.12316496311606469, - "velocityX": 0.22351385671724336, - "velocityY": -0.21751145579268175, - "timestamp": 1.2328865640973672 - }, - { - "x": 2.6122418058900085, - "y": 4.3044120467003895, - "heading": -0.7206083682746678, - "angularVelocity": 0.01292691365951148, - "velocityX": -0.028574418607166113, - "velocityY": 0.027655539271243604, - "timestamp": 1.3138607895750445 - }, - { - "x": 2.589515477392911, - "y": 4.326503828401164, - "heading": -0.7085432954922883, - "angularVelocity": 0.148998927882974, - "velocityX": -0.2806612642842681, - "velocityY": 0.2728248596434546, - "timestamp": 1.3948350150527218 - }, - { - "x": 2.54637684601469, - "y": 4.3684483333856905, - "heading": -0.6854613533337227, - "angularVelocity": 0.2850529538548107, - "velocityX": -0.5327452176745692, - "velocityY": 0.5179982239657722, - "timestamp": 1.475809240530399 - }, - { - "x": 2.4828262685453337, - "y": 4.430246041385043, - "heading": -0.6513636060932391, - "angularVelocity": 0.42109383620888585, - "velocityX": -0.784824764848103, - "velocityY": 0.7631775127791823, - "timestamp": 1.5567834660080764 - }, - { - "x": 2.398864238908538, - "y": 4.511897605460961, - "heading": -0.6062501465002338, - "angularVelocity": 0.5571335733911974, - "velocityX": -1.0368982122572787, - "velocityY": 1.0083648666494986, - "timestamp": 1.6377576914857537 - }, - { - "x": 2.2944914266815206, - "y": 4.613403885366389, - "heading": -0.5501188636580988, - "angularVelocity": 0.6931993793206396, - "velocityX": -1.2889633906498084, - "velocityY": 1.2535628381323076, - "timestamp": 1.718731916963431 - }, - { - "x": 2.1697087770291006, - "y": 4.73476601037198, - "heading": -0.48296289489208905, - "angularVelocity": 0.8293499365978624, - "velocityX": -1.5410168966266635, - "velocityY": 1.4987747556657698, - "timestamp": 1.7997061424411083 - }, - { - "x": 2.0245178868987646, - "y": 4.87598561985106, - "heading": -0.4047647421109401, - "angularVelocity": 0.9657165884553096, - "velocityX": -1.7930506809269309, - "velocityY": 1.7440069188183338, - "timestamp": 1.8806803679187856 - }, - { - "x": 1.8612798092271896, - "y": 5.034863868368553, - "heading": -0.31505899294827, - "angularVelocity": 1.1078309009257712, - "velocityX": -2.0159263853239904, - "velocityY": 1.9620842012417825, - "timestamp": 1.9616545933964629 - }, - { - "x": 1.7184500483895917, - "y": 5.1738844559195085, - "heading": -0.23644393225782434, - "angularVelocity": 0.9708652380027345, - "velocityX": -1.7638916580562496, - "velocityY": 1.7168498584691285, - "timestamp": 2.04262881887414 - }, - { - "x": 1.5960267405860193, - "y": 5.293046114041946, - "heading": -0.168968478820667, - "angularVelocity": 0.8332954472751476, - "velocityX": -1.5118799479886125, - "velocityY": 1.4715998506873489, - "timestamp": 2.1236030443518175 - }, - { - "x": 1.4940086408037947, - "y": 5.392348179003114, - "heading": -0.1126782173092116, - "angularVelocity": 0.6951627036802206, - "velocityX": -1.2598836133403888, - "velocityY": 1.2263416460657963, - "timestamp": 2.2045772698294948 - }, - { - "x": 1.4123948378953157, - "y": 5.471790254952654, - "heading": -0.06761272810355828, - "angularVelocity": 0.5565411578785837, - "velocityX": -1.0078985310085185, - "velocityY": 0.9810785528472173, - "timestamp": 2.285551495307172 - }, - { - "x": 1.3511847156576737, - "y": 5.531372091014257, - "heading": -0.03380194743073204, - "angularVelocity": 0.417549911387026, - "velocityX": -0.7559210585414546, - "velocityY": 0.7358123614043836, - "timestamp": 2.3665257207848494 - }, - { - "x": 1.3103779403872662, - "y": 5.571093500047757, - "heading": -0.011263323026729973, - "angularVelocity": 0.27834319218308196, - "velocityX": -0.50394770718331, - "velocityY": 0.4905438588537352, - "timestamp": 2.4474999462625266 + "angularVelocity": -0.1244247932147739, + "velocityX": 0.2257378393178725, + "velocityY": -0.21969269723824078, + "timestamp": 1.2321688118183196 + }, + { + "x": 2.6124101268407562, + "y": 4.3042467200349375, + "heading": -0.7207049042678578, + "angularVelocity": 0.011728656954985593, + "velocityX": -0.026482004137690758, + "velocityY": 0.025600559605606452, + "timestamp": 1.3131849786230068 + }, + { + "x": 2.5898308635318203, + "y": 4.326193667394344, + "heading": -0.7087258164674446, + "angularVelocity": 0.14786046134831782, + "velocityX": -0.27870071122187107, + "velocityY": 0.2708958992384371, + "timestamp": 1.394201145427694 + }, + { + "x": 2.546818010460629, + "y": 4.368013804819267, + "heading": -0.6857195036998125, + "angularVelocity": 0.28397187468885515, + "velocityX": -0.5309169116194552, + "velocityY": 0.51619496545352, + "timestamp": 1.475217312232381 + }, + { + "x": 2.4833718812656413, + "y": 4.429707576791875, + "heading": -0.6516873068635105, + "angularVelocity": 0.4200667370297391, + "velocityX": -0.7831292407099776, + "velocityY": 0.7614995180078072, + "timestamp": 1.5562334790370682 + }, + { + "x": 2.3994929054836645, + "y": 4.511275584027978, + "heading": -0.6066297376902362, + "angularVelocity": 0.5561552829560362, + "velocityX": -1.035336267935158, + "velocityY": 1.0068114853266914, + "timestamp": 1.6372496458417554 + }, + { + "x": 2.295181645720894, + "y": 4.612718599682289, + "heading": -0.5505453877273064, + "angularVelocity": 0.6922612137172227, + "velocityX": -1.2875363507908548, + "velocityY": 1.2521329958607974, + "timestamp": 1.7182658126464425 + }, + { + "x": 2.1704388340864518, + "y": 4.734037581039087, + "heading": -0.4834287968254485, + "angularVelocity": 0.8284345402771498, + "velocityX": -1.539727397065957, + "velocityY": 1.4974663223609717, + "timestamp": 1.7992819794511297 + }, + { + "x": 2.025265436304345, + "y": 4.875233658266282, + "heading": -0.405266607510017, + "angularVelocity": 0.9647727410241949, + "velocityX": -1.7919065232015794, + "velocityY": 1.7428135987671272, + "timestamp": 1.8802981462558168 + }, + { + "x": 1.8618624811004039, + "y": 5.034280009168128, + "heading": -0.3154414224064508, + "angularVelocity": 1.10873161056997, + "velocityX": -2.0169178776116525, + "velocityY": 1.9631433721774778, + "timestamp": 1.961314313060504 + }, + { + "x": 1.7188875745497745, + "y": 5.1734469779831365, + "heading": -0.2367274347449033, + "angularVelocity": 0.9715837068829752, + "velocityX": -1.7647700723154625, + "velocityY": 1.7177678765092634, + "timestamp": 2.0423304798651913 + }, + { + "x": 1.5963395221215884, + "y": 5.292733837393614, + "heading": -0.1691692747677602, + "angularVelocity": 0.8338849224996224, + "velocityX": -1.5126370113711174, + "velocityY": 1.4723834034020926, + "timestamp": 2.1233466466698787 + }, + { + "x": 1.4942172977681851, + "y": 5.392140100481127, + "heading": -0.11281115760570573, + "angularVelocity": 0.695640381233068, + "velocityX": -1.2605166151540885, + "velocityY": 1.2269929201558054, + "timestamp": 2.204362813474566 + }, + { + "x": 1.41252009894491, + "y": 5.47166545898136, + "heading": -0.06769200462657954, + "angularVelocity": 0.5569154251385268, + "velocityX": -1.008406124918618, + "velocityY": 0.9815986319366667, + "timestamp": 2.2853789802792535 + }, + { + "x": 1.3512473746231104, + "y": 5.531309714267714, + "heading": -0.033841358591587725, + "angularVelocity": 0.4178258163780882, + "velocityX": -0.7563024361485172, + "velocityY": 0.7362018920265989, + "timestamp": 2.366395147083941 + }, + { + "x": 1.3103988349917597, + "y": 5.571072713807996, + "heading": -0.011276386726511467, + "angularVelocity": 0.2785243088515359, + "velocityX": -0.5042023245783488, + "velocityY": 0.4908032693788472, + "timestamp": 2.447411313888628 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 4.4390879396333275e-31, - "angularVelocity": 0.13909763212046558, - "velocityX": -0.25197510938629825, - "velocityY": 0.24527315422336363, - "timestamp": 2.528474171740204 + "heading": 7.044896847536941e-24, + "angularVelocity": 0.1391868706118427, + "velocityX": -0.2521025707860859, + "velocityY": 0.24540274759468042, + "timestamp": 2.5284274806933156 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 2.494908675164405e-31, - "angularVelocity": -2.400971097599731e-30, - "velocityX": 9.117669624782265e-34, - "velocityY": -1.69916768015881e-32, - "timestamp": 2.6094483972178812 - }, - { - "x": 1.3242823472826057, - "y": 5.590457081039532, - "heading": -3.1525835471969304e-19, - "angularVelocity": -3.554932323947037e-18, - "velocityX": 0.3868644475376546, - "velocityY": -0.00560680796813751, - "timestamp": 2.6981303540012136 - }, - { - "x": 1.3928981385918453, - "y": 5.589462635652003, - "heading": -9.247304393859629e-19, - "angularVelocity": -6.8725601776619685e-18, - "velocityX": 0.7737288823800034, - "velocityY": -0.011213615752282564, - "timestamp": 2.786812310784546 - }, - { - "x": 1.4958218231349187, - "y": 5.587970967605794, - "heading": -1.5461935946313141e-18, - "angularVelocity": -7.007774498748477e-18, - "velocityX": 1.1605932962726162, - "velocityY": -0.016820423232804093, - "timestamp": 2.8754942675678783 - }, - { - "x": 1.6330533972641115, - "y": 5.585982076953771, - "heading": -2.4826620193785056e-18, - "angularVelocity": -1.0559852970262818e-17, - "velocityX": 1.5474576690326838, - "velocityY": -0.022427230117193615, - "timestamp": 2.9641762243512106 - }, - { - "x": 1.8045928505515578, - "y": 5.5834959638470645, - "heading": -3.548549667861495e-18, - "angularVelocity": -1.201921661569969e-17, - "velocityX": 1.934321924205522, - "velocityY": -0.028034035297397054, - "timestamp": 3.052858181134543 - }, - { - "x": 2.0104398981448357, - "y": 5.5805126324140275, - "heading": -4.504226548304647e-18, - "angularVelocity": -1.0776452337176765e-17, - "velocityX": 2.321182967310963, - "velocityY": -0.033640793925260974, - "timestamp": 3.1415401379178753 - }, - { - "x": 2.1819790701409207, - "y": 5.578026523384064, - "heading": -4.574305172786567e-18, - "angularVelocity": -7.902241563425968e-19, - "velocityX": 1.934318752293539, - "velocityY": -0.02803398932702808, - "timestamp": 3.2302220947012077 - }, - { - "x": 2.31921035343729, - "y": 5.576037636947067, - "heading": -3.929985868072649e-18, - "angularVelocity": 7.2655061760547e-18, - "velocityX": 1.547454389528782, - "velocityY": -0.022427182587500143, - "timestamp": 3.31890405148454 - }, - { - "x": 2.422133743893467, - "y": 5.574545973163046, - "heading": -2.934544233389305e-18, - "angularVelocity": 1.1224849685211679e-17, - "velocityX": 1.1605899800749648, - "velocityY": -0.016820375171309878, - "timestamp": 3.4075860082678724 - }, - { - "x": 2.490749239479307, - "y": 5.573551532061422, - "heading": -1.705878005512682e-18, - "angularVelocity": 1.385474872728055e-17, - "velocityX": 0.7737255477287375, - "velocityY": -0.011213567423341012, - "timestamp": 3.4962679650512047 + "heading": 3.2679300421363754e-24, + "angularVelocity": -5.4036172137189415e-24, + "velocityX": -5.375446023780689e-25, + "velocityY": 8.028108369127909e-25, + "timestamp": 2.609443647498003 + }, + { + "x": 1.3242823472826726, + "y": 5.590457081039531, + "heading": -2.9445272594890385e-20, + "angularVelocity": -3.320691324750262e-19, + "velocityX": 0.3868644465708696, + "velocityY": -0.005606807954126726, + "timestamp": 2.6981256045031268 + }, + { + "x": 1.3928981385920456, + "y": 5.5894626356520005, + "heading": 1.967498349802273e-20, + "angularVelocity": 5.538923333201553e-19, + "velocityX": 0.7737288804464331, + "velocityY": -0.01121361572426099, + "timestamp": 2.7868075615082506 + }, + { + "x": 1.495821823135319, + "y": 5.587970967605789, + "heading": -2.85375470863899e-20, + "angularVelocity": -5.436565928013428e-19, + "velocityX": 1.1605932933722591, + "velocityY": -0.016820423190771712, + "timestamp": 2.8754895185133744 + }, + { + "x": 1.6330533972647785, + "y": 5.585982076953762, + "heading": -1.8597210244417365e-22, + "angularVelocity": 3.1969947368616956e-19, + "velocityX": 1.5474576651655374, + "velocityY": -0.022427230061150386, + "timestamp": 2.9641714755184982 + }, + { + "x": 1.8045928505525564, + "y": 5.583495963847051, + "heading": 1.7052075155994419e-19, + "angularVelocity": 1.9249318511157463e-18, + "velocityX": 1.9343219193715702, + "velocityY": -0.028034035227342758, + "timestamp": 3.052853432523622 + }, + { + "x": 2.010439898145835, + "y": 5.580512632414013, + "heading": 4.48101102281287e-19, + "angularVelocity": 3.1300656874471152e-18, + "velocityX": 2.321182961505746, + "velocityY": -0.03364079384113171, + "timestamp": 3.141535389528746 + }, + { + "x": 2.1819790701418538, + "y": 5.57802652338405, + "heading": 3.8109031414693246e-19, + "angularVelocity": -7.556304619487216e-19, + "velocityX": 1.934318747455112, + "velocityY": -0.028033989256909718, + "timestamp": 3.2302173465338697 + }, + { + "x": 2.3192103534380903, + "y": 5.5760376369470555, + "heading": 2.513466977645824e-19, + "angularVelocity": -1.4630215747133308e-18, + "velocityX": 1.54745438565714, + "velocityY": -0.022427182531392593, + "timestamp": 3.3188993035389935 + }, + { + "x": 2.4221337438940673, + "y": 5.574545973163037, + "heading": 2.3569701876978443e-19, + "angularVelocity": -1.7646970733669464e-19, + "velocityX": 1.1605899771701071, + "velocityY": -0.01682037512921312, + "timestamp": 3.4075812605441174 + }, + { + "x": 2.4907492394796407, + "y": 5.573551532061417, + "heading": 1.521167829061186e-19, + "angularVelocity": -9.424717122873263e-19, + "velocityX": 0.773725545790664, + "velocityY": -0.011213567395255041, + "timestamp": 3.496263217549241 }, { "x": 2.525056838989258, "y": 5.573054313659668, - "heading": 0, - "angularVelocity": 1.9235908491289617e-17, - "velocityX": 0.3868611017883933, - "velocityY": -0.005606759478353257, - "timestamp": 3.584949921834537 - }, - { - "x": 2.5176787496862993, - "y": 5.573161243955685, - "heading": 2.6679509724403924e-18, - "angularVelocity": 2.5453436150449368e-17, - "velocityX": -0.07039024589473145, - "velocityY": 0.0010201624731186972, - "timestamp": 3.689766849367258 - }, - { - "x": 2.4623729801708616, - "y": 5.573962787832815, - "heading": 6.066089004278022e-18, - "angularVelocity": 3.2419744709429903e-17, - "velocityX": -0.5276415824931802, - "velocityY": 0.007647084263940974, - "timestamp": 3.794583776899979 - }, - { - "x": 2.3591395323793796, - "y": 5.575458945262994, - "heading": 1.0152873197492895e-17, - "angularVelocity": 3.898973466799139e-17, - "velocityX": -0.9848929006171766, - "velocityY": 0.014274005787013901, - "timestamp": 3.8994007044326997 - }, - { - "x": 2.207978410184721, - "y": 5.577649716190093, - "heading": 1.5606892400662525e-17, - "angularVelocity": 5.2033763358184983e-17, - "velocityX": -1.4421441817922973, - "velocityY": 0.020900926774588543, - "timestamp": 4.004217631965421 - }, - { - "x": 2.0088896252054647, - "y": 5.580535100445723, - "heading": 1.7204191476600093e-18, - "angularVelocity": -1.324831168006478e-16, - "velocityX": -1.8993953521210223, - "velocityY": 0.02752784615567168, - "timestamp": 4.109034559498142 - }, - { - "x": 1.769251248155021, - "y": 5.584008168003288, - "heading": 6.282149498472273e-20, - "angularVelocity": -1.5814217146918653e-17, - "velocityX": -2.2862564539075523, - "velocityY": 0.03313460563399779, - "timestamp": 4.213851487030864 - }, - { - "x": 1.577540533889042, - "y": 5.586786622232322, - "heading": -2.476430565711122e-19, - "angularVelocity": -2.961969587010803e-18, - "velocityX": -1.829005283580098, - "velocityY": 0.02650768625293313, - "timestamp": 4.318668414563585 - }, - { - "x": 1.4337574940262061, - "y": 5.588870462964437, - "heading": -3.350775678748249e-19, - "angularVelocity": -8.341640359227092e-19, - "velocityX": -1.3717540024052963, - "velocityY": 0.01988076526536314, - "timestamp": 4.423485342096306 - }, - { - "x": 1.3379021324393994, - "y": 5.590259690143503, - "heading": -2.0793104080618583e-19, - "angularVelocity": 1.2130342880824037e-18, - "velocityX": -0.914502684281442, - "velocityY": 0.013253843742292293, - "timestamp": 4.528302269629028 + "heading": -7.785018050329552e-30, + "angularVelocity": -1.7153070145831324e-18, + "velocityX": 0.386861100817104, + "velocityY": -0.005606759464278068, + "timestamp": 3.584945174554365 + }, + { + "x": 2.5176787496859108, + "y": 5.57316124395569, + "heading": -2.760990246718961e-19, + "angularVelocity": -2.634107204526249e-18, + "velocityX": -0.0703902457225223, + "velocityY": 0.0010201624706221363, + "timestamp": 3.6897621023490434 + }, + { + "x": 2.4623729801701617, + "y": 5.573962787832825, + "heading": -5.185614830886611e-19, + "angularVelocity": -2.3131994374789403e-18, + "velocityX": -0.5276415811774727, + "velocityY": 0.007647084244872661, + "timestamp": 3.794579030143722 + }, + { + "x": 2.359139532378446, + "y": 5.575458945263007, + "heading": -6.8142886513825e-19, + "angularVelocity": -1.5538270914101072e-18, + "velocityX": -0.9848928981579708, + "velocityY": 0.014274005751373834, + "timestamp": 3.8993959579384003 + }, + { + "x": 2.2079784101836313, + "y": 5.577649716190108, + "heading": -5.427283873734921e-19, + "angularVelocity": 1.3232641013891109e-18, + "velocityX": -1.442144178189593, + "velocityY": 0.02090092672237671, + "timestamp": 4.004212885733079 + }, + { + "x": 2.0088896252042967, + "y": 5.58053510044574, + "heading": -5.007294376921598e-19, + "angularVelocity": 4.006886155587225e-19, + "velocityX": -1.8993953473748186, + "velocityY": 0.027527846086888057, + "timestamp": 4.109029813527758 + }, + { + "x": 1.7692512481542437, + "y": 5.584008168003299, + "heading": -1.0777156334696855e-19, + "angularVelocity": 3.7489924825765976e-18, + "velocityX": -2.286256448190035, + "velocityY": 0.033134605551138764, + "timestamp": 4.2138467413224365 + }, + { + "x": 1.5775405338885757, + "y": 5.586786622232329, + "heading": 2.4679892039672843e-19, + "angularVelocity": 3.382759743178975e-18, + "velocityX": -1.8290052790060969, + "velocityY": 0.02650768618664608, + "timestamp": 4.318663669117115 + }, + { + "x": 1.433757494025973, + "y": 5.58887046296444, + "heading": 3.0327550884617235e-19, + "angularVelocity": 5.388117133514026e-19, + "velocityX": -1.3717539989747978, + "velocityY": 0.019880765215647883, + "timestamp": 4.423480596911794 + }, + { + "x": 1.3379021324393217, + "y": 5.590259690143504, + "heading": 1.0372328795673318e-19, + "angularVelocity": -1.9038167315540544e-18, + "velocityX": -0.9145026819944435, + "velocityY": 0.0132538437091488, + "timestamp": 4.528297524706473 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.4049323971929142e-31, - "angularVelocity": 1.9837543963605952e-18, - "velocityX": -0.4572513476830733, - "velocityY": 0.006626921951471195, - "timestamp": 4.633119197161749 + "heading": 1.6477091007835168e-22, + "angularVelocity": -9.879942030515793e-19, + "velocityX": -0.45725134653957433, + "velocityY": 0.00662692193489945, + "timestamp": 4.633114452501152 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 4.571690374498227e-31, - "angularVelocity": 5.701965460392164e-30, - "velocityX": 1.9882633246635272e-32, - "velocityY": -1.2353765948799823e-31, - "timestamp": 4.73793612469447 - }, - { - "x": 1.3072266031673745, - "y": 5.605560947932051, - "heading": 0.009988018032587274, - "angularVelocity": 0.1383542475734168, - "velocityX": 0.2389771940088317, - "velocityY": 0.20233155967170455, - "timestamp": 4.810127750233767 - }, - { - "x": 1.3417310637547577, - "y": 5.63477381293564, - "heading": 0.029960721397497427, - "angularVelocity": 0.2766623305087681, - "velocityX": 0.4779565542351512, - "velocityY": 0.40465725470744984, - "timestamp": 4.882319375773064 - }, - { - "x": 1.393488225984334, - "y": 5.678592353413093, - "heading": 0.05990698047800166, - "angularVelocity": 0.4148162457460571, - "velocityX": 0.7169413604826331, - "velocityY": 0.606975395693467, - "timestamp": 4.954511001312361 - }, - { - "x": 1.4624987017850304, - "y": 5.737015858733095, - "heading": 0.09981017735525757, - "angularVelocity": 0.5527399692025371, - "velocityX": 0.9559346431831991, - "velocityY": 0.8092836929987566, - "timestamp": 5.026702626851658 - }, - { - "x": 1.54876329298479, - "y": 5.810043407637518, - "heading": 0.1496511403376383, - "angularVelocity": 0.6903981259608908, - "velocityX": 1.1949390328222467, - "velocityY": 1.0115792290159562, - "timestamp": 5.098894252390955 - }, - { - "x": 1.6522829608961203, - "y": 5.897673825591776, - "heading": 0.20941128647918397, - "angularVelocity": 0.8277988713391718, - "velocityX": 1.4339567385834835, - "velocityY": 1.2138584953540306, - "timestamp": 5.171085877930252 - }, - { - "x": 1.7730588392541233, - "y": 5.999905624129209, - "heading": 0.2790750774652724, - "angularVelocity": 0.9649843796382769, - "velocityX": 1.6729901488685341, - "velocityY": 1.4161171434188378, - "timestamp": 5.243277503469549 - }, - { - "x": 1.911092570994838, - "y": 6.1167366962116585, - "heading": 0.35862892849655154, - "angularVelocity": 1.1019817109951129, - "velocityX": 1.9120463171400826, - "velocityY": 1.6183466047443176, - "timestamp": 5.315469129008846 - }, - { - "x": 2.0662405750494774, - "y": 6.247971693585017, - "heading": 0.4478813931179114, - "angularVelocity": 1.2363271218036194, - "velocityX": 2.1491135972568896, - "velocityY": 1.8178700977147564, - "timestamp": 5.3876607545481425 - }, - { - "x": 2.204132997305319, - "y": 6.36460684022272, - "heading": 0.5272152358459444, - "angularVelocity": 1.0989341510922093, - "velocityX": 1.9100888950161639, - "velocityY": 1.6156326411317472, - "timestamp": 5.4598523800874394 - }, - { - "x": 2.3247684211138937, - "y": 6.466643771235888, - "heading": 0.5966353508762373, - "angularVelocity": 0.9616089748867341, - "velocityX": 1.6710445693293852, - "velocityY": 1.4134178341442696, - "timestamp": 5.532044005626736 - }, - { - "x": 2.428146136331826, - "y": 6.5540833745423095, - "heading": 0.6561424196084651, - "angularVelocity": 0.8242932374446845, - "velocityX": 1.4319904067208047, - "velocityY": 1.2112153266137329, - "timestamp": 5.604235631166033 - }, - { - "x": 2.51426563036811, - "y": 6.626926253781894, - "heading": 0.705736875442574, - "angularVelocity": 0.686983503468318, - "velocityX": 1.192929143690227, - "velocityY": 1.0090211807166112, - "timestamp": 5.67642725670533 - }, - { - "x": 2.5831264968268224, - "y": 6.6851728424603465, - "heading": 0.7454193798278042, - "angularVelocity": 0.5496829319015093, - "velocityX": 0.95386225125429, - "velocityY": 0.8068330397501333, - "timestamp": 5.748618882244627 - }, - { - "x": 2.6347284176402783, - "y": 6.72882344947858, - "heading": 0.7751907373365541, - "angularVelocity": 0.4123935052894287, - "velocityX": 0.7147909529377248, - "velocityY": 0.6046491776859777, - "timestamp": 5.820810507783924 - }, - { - "x": 2.669071164543137, - "y": 6.75787827866861, - "heading": 0.7950517689437672, - "angularVelocity": 0.27511545084049754, - "velocityX": 0.4757164926845912, - "velocityY": 0.4024681391074233, - "timestamp": 5.893002133323221 + "heading": 1.5500454228160796e-22, + "angularVelocity": -9.317544566447768e-23, + "velocityX": -6.472519788584152e-21, + "velocityY": -2.347379322977946e-21, + "timestamp": 4.737931380295831 + }, + { + "x": 1.3072065943694386, + "y": 5.605543739625148, + "heading": 0.00997794612171538, + "angularVelocity": 0.1382953520830163, + "velocityX": 0.23883926575200254, + "velocityY": 0.20221107105767083, + "timestamp": 4.810080920908923 + }, + { + "x": 1.3416710394123585, + "y": 5.634722180998455, + "heading": 0.029930634172052512, + "angularVelocity": 0.2765462937225786, + "velocityX": 0.47768072741777556, + "velocityY": 0.4044161768094943, + "timestamp": 4.882230461522014 + }, + { + "x": 1.3933681818707737, + "y": 5.678489073214057, + "heading": 0.05984711285989078, + "angularVelocity": 0.414645449348986, + "velocityX": 0.7165276732064818, + "velocityY": 0.6066135950928104, + "timestamp": 4.954380002135106 + }, + { + "x": 1.4622986372338835, + "y": 5.736843692636249, + "heading": 0.09971100751764889, + "angularVelocity": 0.5525176504107191, + "velocityX": 0.9553831497383417, + "velocityY": 0.8088009837113753, + "timestamp": 5.0265295427481975 + }, + { + "x": 1.548463213011665, + "y": 5.809785098412347, + "heading": 0.14950349033581017, + "angularVelocity": 0.6901288961100662, + "velocityX": 1.194249818440935, + "velocityY": 1.0109753320156587, + "timestamp": 5.098679083361289 + }, + { + "x": 1.6518628806196247, + "y": 5.897312083044383, + "heading": 0.20920651705409263, + "angularVelocity": 0.8274900465194276, + "velocityX": 1.4331299510616444, + "velocityY": 1.2131329442748184, + "timestamp": 5.170828623974381 + }, + { + "x": 1.772498794450995, + "y": 5.999423091967104, + "heading": 0.2788055813251054, + "angularVelocity": 0.9646501374727254, + "velocityX": 1.6720260836904182, + "velocityY": 1.4152690101008414, + "timestamp": 5.2429781645874725 + }, + { + "x": 1.9103726539115997, + "y": 6.1161158275998355, + "heading": 0.35829010832272556, + "angularVelocity": 1.1016636602561762, + "velocityX": 1.9109457702574424, + "velocityY": 1.6173732312241136, + "timestamp": 5.315127705200564 + }, + { + "x": 2.0655201843500692, + "y": 6.247352096059746, + "heading": 0.44752582385757206, + "angularVelocity": 1.2368161290642319, + "velocityX": 2.1503606138043514, + "velocityY": 1.8189480812314087, + "timestamp": 5.387277245813656 + }, + { + "x": 2.203432352747445, + "y": 6.364005171819918, + "heading": 0.526860073391937, + "angularVelocity": 1.0995807992708087, + "velocityX": 1.911476736032763, + "velocityY": 1.6168235413408145, + "timestamp": 5.459426786426747 + }, + { + "x": 2.3241076430309073, + "y": 6.466076944016547, + "heading": 0.596294464302791, + "angularVelocity": 0.9623677478863277, + "velocityX": 1.6725718453371032, + "velocityY": 1.4147251850707963, + "timestamp": 5.531576327039839 + }, + { + "x": 2.427545315940862, + "y": 6.553568380245639, + "heading": 0.6558285941122223, + "angularVelocity": 0.8251491181168898, + "velocityX": 1.4336567084279637, + "velocityY": 1.2126402397802263, + "timestamp": 5.603725867652931 + }, + { + "x": 2.51374484513646, + "y": 6.626480123164196, + "heading": 0.7054623538170143, + "angularVelocity": 0.687928977551743, + "velocityX": 1.1947342763809239, + "velocityY": 1.0105642017813024, + "timestamp": 5.675875408266022 + }, + { + "x": 2.582705816133726, + "y": 6.684812629515298, + "heading": 0.7451960796056276, + "angularVelocity": 0.5507134965929852, + "velocityX": 0.9558060995436645, + "velocityY": 0.8084944942881265, + "timestamp": 5.748024948879114 + }, + { + "x": 2.6344279053527613, + "y": 6.728566223750801, + "heading": 0.7750303601927481, + "angularVelocity": 0.41350617527988826, + "velocityX": 0.7168734378559617, + "velocityY": 0.6064292837307924, + "timestamp": 5.820174489492206 + }, + { + "x": 2.6689108803199226, + "y": 6.757741121003901, + "heading": 0.7949658633330718, + "angularVelocity": 0.2763081091150633, + "velocityX": 0.477937554059832, + "velocityY": 0.4043670549415301, + "timestamp": 5.892324030105297 }, { "x": 2.68615460395813, "y": 6.772337436676025, "heading": 0.8050032485420815, - "angularVelocity": 0.13784811636956903, - "velocityX": 0.23664018211616192, - "velocityY": 0.20028857778593961, - "timestamp": 5.965193758862518 - }, - { - "x": 2.6837128162746895, - "y": 6.770286154899203, - "heading": 0.8037616945531193, - "angularVelocity": -0.015403237566514301, - "velocityX": -0.03029383829428867, - "velocityY": -0.025449058845075586, - "timestamp": 6.045797202361987 - }, - { - "x": 2.6597554401054357, - "y": 6.750039490992799, - "heading": 0.790165886718463, - "angularVelocity": -0.1686752729700538, - "velocityX": -0.29722521928375617, - "velocityY": -0.2511885724392666, - "timestamp": 6.126400645861456 - }, - { - "x": 2.614282818196885, - "y": 6.71159714288196, - "heading": 0.7642144593793307, - "angularVelocity": -0.32196425130707784, - "velocityX": -0.5641523480167241, - "velocityY": -0.4769318337003116, - "timestamp": 6.207004089360925 - }, - { - "x": 2.547295402718566, - "y": 6.65495864191724, - "heading": 0.7259067765041173, - "angularVelocity": -0.4752611205179619, - "velocityX": -0.8310738669455855, - "velocityY": -0.702680909222201, - "timestamp": 6.287607532860394 - }, - { - "x": 2.45879373609576, - "y": 6.580123315301835, - "heading": 0.6752436075608932, - "angularVelocity": -0.6285484433864407, - "velocityX": -1.0979886563197567, - "velocityY": -0.9284383317423857, - "timestamp": 6.3682109763598636 - }, - { - "x": 2.3487784541804713, - "y": 6.487090205965796, - "heading": 0.612227929672179, - "angularVelocity": -0.7817988308289465, - "velocityX": -1.3648955570482664, - "velocityY": -1.1542076280722422, - "timestamp": 6.448814419859333 - }, - { - "x": 2.2172503640031413, - "y": 6.3758579034961445, - "heading": 0.536865388228733, - "angularVelocity": -0.934979179194793, - "velocityX": -1.6317924454199069, - "velocityY": -1.3799944225748537, - "timestamp": 6.529417863358802 - }, - { - "x": 2.0642108270205712, - "y": 6.2464240090225065, - "heading": 0.4491644340858314, - "angularVelocity": -1.0880546827189126, - "velocityX": -1.8986724429895898, - "velocityY": -1.605810978465707, - "timestamp": 6.610021306858271 - }, - { - "x": 1.892150582189426, - "y": 6.100772712319997, - "heading": 0.34952884105977006, - "angularVelocity": -1.236120799562961, - "velocityX": -2.134651292315194, - "velocityY": -1.8070108469188693, - "timestamp": 6.69062475035774 - }, - { - "x": 1.7416020056028705, - "y": 5.973323101720471, - "heading": 0.2622670318762804, - "angularVelocity": -1.0826064668575972, - "velocityX": -1.8677685474764083, - "velocityY": -1.5811931235965593, - "timestamp": 6.771228193857209 - }, - { - "x": 1.6125631547118695, - "y": 5.864077706390992, - "heading": 0.18740079602910192, - "angularVelocity": -0.9288218045882161, - "velocityX": -1.600909902712647, - "velocityY": -1.3553440223708513, - "timestamp": 6.851831637356678 - }, - { - "x": 1.5050325714338102, - "y": 5.7730380802152625, - "heading": 0.12496122653203612, - "angularVelocity": -0.7746513893974771, - "velocityX": -1.3340693475310352, - "velocityY": -1.1294756430146329, - "timestamp": 6.9324350808561475 - }, - { - "x": 1.4190090841759289, - "y": 5.700205295684954, - "heading": 0.0749803520329015, - "angularVelocity": -0.6200836134188114, - "velocityX": -1.0672433276187945, - "velocityY": -0.9035939578786114, - "timestamp": 7.013038524355617 - }, - { - "x": 1.3544918382019193, - "y": 5.645580092417221, - "heading": 0.037484747068995454, - "angularVelocity": -0.4651861426263309, - "velocityX": -0.8004279119223182, - "velocityY": -0.6777030967431115, - "timestamp": 7.093641967855086 - }, - { - "x": 1.3114803343182175, - "y": 5.609162978300701, - "heading": 0.012490529490997385, - "angularVelocity": -0.31008870704351826, - "velocityX": -0.5336186894297898, - "velocityY": -0.45180593452897394, - "timestamp": 7.174245411354555 + "angularVelocity": 0.13911918390216763, + "velocityX": 0.23899977036136932, + "velocityY": 0.2023064256278295, + "timestamp": 5.964473570718389 + }, + { + "x": 2.6838905941834166, + "y": 6.770438484115006, + "heading": 0.8038568906266841, + "angularVelocity": -0.014214822664441809, + "velocityX": -0.028073690621275776, + "velocityY": -0.023546986103133997, + "timestamp": 6.0451188198684145 + }, + { + "x": 2.6600885687415188, + "y": 6.7503254094883065, + "heading": 0.7903430319109747, + "angularVelocity": -0.16757166551211813, + "velocityX": -0.2951447939309862, + "velocityY": -0.24940185365765316, + "timestamp": 6.12576406901844 + }, + { + "x": 2.614748840901014, + "y": 6.711997955351817, + "heading": 0.7644600728285338, + "angularVelocity": -0.3209483429617853, + "velocityX": -0.562212012714593, + "velocityY": -0.47525991351563107, + "timestamp": 6.206409318168466 + }, + { + "x": 2.547871822089795, + "y": 6.655455715047437, + "heading": 0.7262070502709054, + "angularVelocity": -0.47433696294329564, + "velocityX": -0.8292741297978391, + "velocityY": -0.7011230159286398, + "timestamp": 6.287054567318491 + }, + { + "x": 2.45945799388215, + "y": 6.580698108278845, + "heading": 0.6755842481215987, + "angularVelocity": -0.627722062773128, + "velocityX": -1.0963302753664865, + "velocityY": -0.9269933140081175, + "timestamp": 6.367699816468517 + }, + { + "x": 2.349507890636972, + "y": 6.487724331378611, + "heading": 0.6125938511497702, + "angularVelocity": -0.781080071494933, + "velocityX": -1.3633797948920177, + "velocityY": -1.152873577552878, + "timestamp": 6.448345065618542 + }, + { + "x": 2.218022116383117, + "y": 6.3765332785035795, + "heading": 0.5372399641012893, + "angularVelocity": -0.9343871814234115, + "velocityX": -1.6304218244679243, + "velocityY": -1.3787675535378683, + "timestamp": 6.528990314768568 + }, + { + "x": 2.065001429825468, + "y": 6.2471234504060735, + "heading": 0.4495265554466888, + "angularVelocity": -1.0876450823708927, + "velocityX": -1.897454446113545, + "velocityY": -1.6046801201737573, + "timestamp": 6.6096355639185935 + }, + { + "x": 1.892766685520709, + "y": 6.101314959564872, + "heading": 0.34981930117759164, + "angularVelocity": -1.2363686059622758, + "velocityX": -2.1357085026093356, + "velocityY": -1.8080233166612498, + "timestamp": 6.690280813068619 + }, + { + "x": 1.7420645817615519, + "y": 5.973729053643758, + "heading": 0.26248852330737293, + "angularVelocity": -1.08290046581363, + "velocityX": -1.868704050734679, + "velocityY": -1.582063509826458, + "timestamp": 6.770926062218645 + }, + { + "x": 1.612893816606675, + "y": 5.8643673067305935, + "heading": 0.18756078557963143, + "angularVelocity": -0.929102935603218, + "velocityX": -1.6017157429147117, + "velocityY": -1.3560841843233018, + "timestamp": 6.85157131136867 + }, + { + "x": 1.5052531418262654, + "y": 5.773230960685921, + "heading": 0.12506875746451004, + "angularVelocity": -0.7749003044043764, + "velocityX": -1.3347429131276467, + "velocityY": -1.1300894597663134, + "timestamp": 6.932216560518696 + }, + { + "x": 1.4191414897869865, + "y": 5.700320933784421, + "heading": 0.0750452611365071, + "angularVelocity": -0.6202906786851591, + "velocityX": -1.0677833219794879, + "velocityY": -0.9040833486156766, + "timestamp": 7.012861809668721 + }, + { + "x": 1.3545580679213909, + "y": 5.645637873878687, + "heading": 0.03751734547660986, + "angularVelocity": -0.46534564720711064, + "velocityX": -0.8008335586570275, + "velocityY": -0.6780692041015978, + "timestamp": 7.093507058818747 + }, + { + "x": 1.311502418725051, + "y": 5.609182228096023, + "heading": 0.01250142936060969, + "angularVelocity": -0.31019702189105725, + "velocityX": -0.533889468383214, + "velocityY": -0.4520495152150475, + "timestamp": 7.174152307968773 }, { "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 1.0757846254441862e-30, - "angularVelocity": -0.1549627280016075, - "velocityX": -0.2668109738177861, - "velocityY": -0.22590442503082891, - "timestamp": 7.254848854854024 + "y": 5.590954303741457, + "heading": 2.7832801817325185e-21, + "angularVelocity": -0.1550175551860858, + "velocityX": -0.2669465081562177, + "velocityY": -0.22602601575026499, + "timestamp": 7.254797557118798 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 2.1112287936414426e-30, - "angularVelocity": 1.2846097126015132e-29, - "velocityX": 1.1473496118122496e-30, - "velocityY": -2.480854756847108e-31, - "timestamp": 7.335452298353493 - }, - { - "x": 1.3107332172473851, - "y": 5.587721554520521, - "heading": -5.6112723310966165e-9, - "angularVelocity": -8.086181485047517e-8, - "velocityX": 0.2991463288589595, - "velocityY": -0.04658586416314624, - "timestamp": 7.40484564929817 - }, - { - "x": 1.3522507006814268, - "y": 5.581255743814154, - "heading": -1.6136427820687844e-8, - "angularVelocity": -1.516738315771432e-7, - "velocityX": 0.5982919525984769, - "velocityY": -0.09317622824587016, - "timestamp": 7.4742390002428465 - }, - { - "x": 1.414526842614822, - "y": 5.571556496717738, - "heading": -3.074141680025046e-8, - "angularVelocity": -2.1046669010163338e-7, - "velocityX": 0.8974367296809845, - "velocityY": -0.13977199493000497, - "timestamp": 7.543632351187523 - }, - { - "x": 1.4975615711863124, - "y": 5.558623354737258, - "heading": -4.841098342837698e-8, - "angularVelocity": -2.546291003036043e-7, - "velocityX": 1.196580471199459, - "velocityY": -0.1863743687891775, - "timestamp": 7.6130257021322 - }, - { - "x": 1.6013547964876313, - "y": 5.542455744326475, - "heading": -6.788238796631325e-8, - "angularVelocity": -2.8059467012358587e-7, - "velocityX": 1.4957229170856399, - "velocityY": -0.2329850077952488, - "timestamp": 7.682419053076877 - }, - { - "x": 1.7259064027890496, - "y": 5.5230529273691795, - "heading": -8.754228960685774e-8, - "angularVelocity": -2.8331102694259857e-7, - "velocityX": 1.7948636952366863, - "velocityY": -0.27960628350061884, - "timestamp": 7.751812404021553 - }, - { - "x": 1.8712162355508042, - "y": 5.500413918486819, - "heading": -1.0525703637468966e-7, - "angularVelocity": -2.552801733617599e-7, - "velocityX": 2.0940022463766295, - "velocityY": -0.3262417591047122, - "timestamp": 7.82120575496623 - }, - { - "x": 2.0372840779688866, - "y": 5.474537335801765, - "heading": -1.1807218385471473e-7, - "angularVelocity": -1.8467399582926014e-7, - "velocityX": 2.393137673240176, - "velocityY": -0.3728971483980547, - "timestamp": 7.890599105910907 - }, - { - "x": 2.2241096038561166, - "y": 5.445421101366719, - "heading": -1.2162497814922265e-7, - "angularVelocity": -5.119790624873975e-8, - "velocityX": 2.692268399549916, - "velocityY": -0.41958248216406707, - "timestamp": 7.959992456855583 - }, - { - "x": 2.431692266948392, - "y": 5.413061738269229, - "heading": -1.0881824696224333e-7, - "angularVelocity": 1.845527118847122e-7, - "velocityX": 2.99139125386478, - "velocityY": -0.46631792033351455, - "timestamp": 8.02938580780026 - }, - { - "x": 2.660030964874599, - "y": 5.377452241496065, - "heading": -6.605232487729304e-8, - "angularVelocity": 6.162827019703426e-7, - "velocityX": 3.290498222347668, - "velocityY": -0.5131543049644982, - "timestamp": 8.098779158744936 - }, - { - "x": 2.9091223110729234, - "y": 5.338571186157936, - "heading": 4.364254321616404e-8, - "angularVelocity": 0.0000015807691363697316, - "velocityX": 3.5895563884342283, - "velocityY": -0.5602994351597471, - "timestamp": 8.168172509689612 - }, - { - "x": 3.168557402823767, - "y": 5.298953853406429, - "heading": 4.3641894183803e-8, - "angularVelocity": -9.352947371519385e-12, - "velocityX": 3.738615994458649, - "velocityY": -0.5709096363289035, - "timestamp": 8.237565860634287 - }, - { - "x": 3.4266802039415243, - "y": 5.251533194096367, - "heading": 4.3642354317485314e-8, - "angularVelocity": 6.630803609714539e-12, - "velocityX": 3.719705095716507, - "velocityY": -0.6833602739240744, - "timestamp": 8.306959211578963 - }, - { - "x": 3.6803449026923616, - "y": 5.18422554387417, - "heading": 4.364233337760302e-8, - "angularVelocity": -3.0175631893268995e-13, - "velocityX": 3.6554611543845263, - "velocityY": -0.9699437958523844, - "timestamp": 8.376352562523639 - }, - { - "x": 3.9278131468583686, - "y": 5.096843813495861, - "heading": 4.364231130349348e-8, - "angularVelocity": -3.1810121673972006e-13, - "velocityX": 3.5661665101502296, - "velocityY": -1.2592233865168847, - "timestamp": 8.445745913468315 + "heading": 1.3491121023361147e-21, + "angularVelocity": -9.888514076614425e-22, + "velocityX": -3.220372602008045e-20, + "velocityY": -2.3512590671688983e-20, + "timestamp": 7.335442806268824 + }, + { + "x": 1.310729419094693, + "y": 5.587708246254127, + "heading": 1.3104272140971465e-18, + "angularVelocity": 1.8865367753568018e-17, + "velocityX": 0.2991037005434092, + "velocityY": -0.04677953756663729, + "timestamp": 7.404833348719224 + }, + { + "x": 1.3522393548624092, + "y": 5.581216131325057, + "heading": -6.810029035561697e-19, + "angularVelocity": -2.869886943346906e-17, + "velocityX": 0.5982073968853444, + "velocityY": -0.0935590744763348, + "timestamp": 7.474223891169625 + }, + { + "x": 1.4145042580183609, + "y": 5.571477959008947, + "heading": -1.7830392147175414e-18, + "angularVelocity": -1.5881650038949302e-17, + "velocityX": 0.8973110881855106, + "velocityY": -0.1403386105977046, + "timestamp": 7.543614433620025 + }, + { + "x": 1.4975241281349527, + "y": 5.558493729372655, + "heading": -6.515620811670549e-18, + "angularVelocity": -6.820211270699198e-17, + "velocityX": 1.1964147733235146, + "velocityY": -0.18711814575556276, + "timestamp": 7.6130049760704255 + }, + { + "x": 1.6012989646776896, + "y": 5.542263442499756, + "heading": -1.6376922566585543e-17, + "angularVelocity": -1.4211305181843934e-16, + "velocityX": 1.4955184507588155, + "velocityY": -0.23389767970903125, + "timestamp": 7.682395518520826 + }, + { + "x": 1.725828766959364, + "y": 5.522787098497699, + "heading": -2.5308091780963488e-17, + "angularVelocity": -1.2870873895834211e-16, + "velocityX": 1.7946221182906406, + "velocityY": -0.28067721211399865, + "timestamp": 7.751786060971226 + }, + { + "x": 1.8711135340636993, + "y": 5.500064697509755, + "heading": -3.3349995940615036e-17, + "angularVelocity": -1.1589337502943322e-16, + "velocityX": 2.0937257726178307, + "velocityY": -0.3274567424542979, + "timestamp": 7.821176603421627 + }, + { + "x": 2.0371532647079076, + "y": 5.474096239736498, + "heading": -4.30035583427312e-17, + "angularVelocity": -1.3911928140733344e-16, + "velocityX": 2.3928294084585304, + "velocityY": -0.37423626990406167, + "timestamp": 7.890567145872027 + }, + { + "x": 2.223947956967808, + "y": 5.444881725478791, + "heading": -5.0761776746981987e-17, + "angularVelocity": -1.118051268996076e-16, + "velocityX": 2.691933016569494, + "velocityY": -0.4210157930180218, + "timestamp": 7.959957688322428 + }, + { + "x": 2.4314976076364307, + "y": 5.412421155238076, + "heading": -5.64791774774602e-17, + "angularVelocity": -8.239452421940524e-17, + "velocityX": 2.991036578464229, + "velocityY": -0.4677953089056427, + "timestamp": 8.029348230772829 + }, + { + "x": 2.659802210299839, + "y": 5.37671453001723, + "heading": -6.06625739671826e-17, + "angularVelocity": -6.028770408822991e-17, + "velocityX": 3.290140047926519, + "velocityY": -0.5145748103405868, + "timestamp": 8.09873877322323 + }, + { + "x": 2.9088617457162362, + "y": 5.337761852824889, + "heading": -6.241000651317059e-17, + "angularVelocity": -2.5182575093519166e-17, + "velocityX": 3.589243240091715, + "velocityY": -0.5613542684175394, + "timestamp": 8.168129315673632 + }, + { + "x": 3.168141716720517, + "y": 5.297210399977503, + "heading": -4.3453663669154356e-17, + "angularVelocity": 2.7318337880868624e-16, + "velocityX": 3.7365318363033526, + "velocityY": -0.5843945214345647, + "timestamp": 8.237519858124033 + }, + { + "x": 3.426418453660738, + "y": 5.250695339800734, + "heading": -4.238140416902354e-17, + "angularVelocity": 1.5452530881159203e-17, + "velocityX": 3.7220740438055158, + "velocityY": -0.6703371746952049, + "timestamp": 8.306910400574434 + }, + { + "x": 3.680147658052144, + "y": 5.183672848816506, + "heading": -4.587291099772401e-17, + "angularVelocity": -5.0316753627269466e-17, + "velocityX": 3.65653870731402, + "velocityY": -0.9658735703375574, + "timestamp": 8.376300943024836 + }, + { + "x": 3.9277032968256567, + "y": 5.096571019436482, + "heading": -4.576332276708217e-17, + "angularVelocity": 1.5792963566594492e-18, + "velocityX": 3.567570306147443, + "velocityY": -1.2552406466959498, + "timestamp": 8.445691485475237 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 4.364228801520247e-8, - "angularVelocity": -3.3559830331814666e-13, - "velocityX": 3.45401850865928, - "velocityY": -1.5404360240963577, - "timestamp": 8.51513926441299 - }, - { - "x": 4.284862555065841, - "y": 4.931880235034004, - "heading": 4.364226510492993e-8, - "angularVelocity": -6.617055985872344e-13, - "velocityX": 3.3897491580338537, - "velocityY": -1.67713541924223, - "timestamp": 8.549762324762018 - }, - { - "x": 4.399813643173519, - "y": 4.869172357324464, - "heading": 4.364224321511573e-8, - "angularVelocity": -6.322322259809295e-13, - "velocityX": 3.3200730076683276, - "velocityY": -1.81115929895842, - "timestamp": 8.584385385111045 - }, - { - "x": 4.512168964860153, - "y": 4.801924216205689, - "heading": 4.364222211690104e-8, - "angularVelocity": -6.093688654191888e-13, - "velocityX": 3.245100824537311, - "velocityY": -1.9422933859936342, - "timestamp": 8.619008445460071 - }, - { - "x": 4.621749311428581, - "y": 4.730243126546711, - "heading": 4.364220167371938e-8, - "angularVelocity": -5.904498836065766e-13, - "velocityX": 3.164952649008305, - "velocityY": -2.070327952999433, - "timestamp": 8.653631505809098 - }, - { - "x": 4.728650036507715, - "y": 4.654623908914118, - "heading": 4.364241447304788e-8, - "angularVelocity": 6.146173210402922e-12, - "velocityX": 3.0875585231776617, - "velocityY": -2.184070872715801, - "timestamp": 8.688254566158125 - }, - { - "x": 4.8372727310201835, - "y": 4.581499759061361, - "heading": 4.3642159997006347e-8, - "angularVelocity": -7.349900240613306e-12, - "velocityX": 3.137293278452735, - "velocityY": -2.112007116517435, - "timestamp": 8.722877626507152 - }, - { - "x": 4.948612503037568, - "y": 4.5125832438429105, - "heading": 4.3642137671211746e-8, - "angularVelocity": -6.448244222387315e-13, - "velocityX": 3.2157692270698366, - "velocityY": -1.9904801748811223, - "timestamp": 8.75750068685618 - }, - { - "x": 5.062589095284458, - "y": 4.448121106677303, - "heading": 4.364211455765666e-8, - "angularVelocity": -6.675769025667281e-13, - "velocityX": 3.2919271461828314, - "velocityY": -1.8618266703110185, - "timestamp": 8.792123747205206 - }, - { - "x": 5.17904891222934, - "y": 4.388261758970167, - "heading": 4.3642090495999965e-8, - "angularVelocity": -6.94960432877984e-13, - "velocityX": 3.3636488447548265, - "velocityY": -1.728886675634883, - "timestamp": 8.826746807554233 - }, - { - "x": 5.297806179962019, - "y": 4.333100707972697, - "heading": 4.364206516158606e-8, - "angularVelocity": -7.317208234311228e-13, - "velocityX": 3.4300049312658345, - "velocityY": -1.5931881942671235, - "timestamp": 8.86136986790326 - }, - { - "x": 5.418671454426997, - "y": 4.282725959570945, - "heading": 4.364203830903825e-8, - "angularVelocity": -7.755683028589255e-13, - "velocityX": 3.4908894028015367, - "velocityY": -1.454947884269515, - "timestamp": 8.895992928252287 - }, - { - "x": 5.541451926168614, - "y": 4.237217881100571, - "heading": 4.364200955008555e-8, - "angularVelocity": -8.306300130132497e-13, - "velocityX": 3.546205058244276, - "velocityY": -1.3143863659542425, - "timestamp": 8.930615988601314 + "heading": -5.405308439751368e-17, + "angularVelocity": -1.194652950873824e-16, + "velocityX": 3.455741374913111, + "velocityY": -1.5365670854176536, + "timestamp": 8.515082027925638 + }, + { + "x": 4.284931451916926, + "y": 4.932009555387415, + "heading": -5.371492234038301e-17, + "angularVelocity": 9.766625728969003e-18, + "velocityX": 3.3916228655411733, + "velocityY": -1.6733429936143844, + "timestamp": 8.549706274539513 + }, + { + "x": 4.39995646079623, + "y": 4.869427987028382, + "heading": -6.433424938599782e-17, + "angularVelocity": -3.067020392759585e-16, + "velocityX": 3.32209420069255, + "velocityY": -1.8074492437895704, + "timestamp": 8.584330521153388 + }, + { + "x": 4.51239059708215, + "y": 4.8023029501801755, + "heading": -5.362012153015504e-17, + "angularVelocity": 3.0944002843229445e-16, + "velocityX": 3.2472659272494777, + "velocityY": -1.9386714055261611, + "timestamp": 8.618954767767264 + }, + { + "x": 4.622054515133808, + "y": 4.730741572190097, + "heading": -5.247217419715393e-17, + "angularVelocity": 3.315443500231424e-17, + "velocityX": 3.1672578836043357, + "velocityY": -2.06679956933421, + "timestamp": 8.653579014381139 + }, + { + "x": 4.728773352551376, + "y": 4.65485813780801, + "heading": -6.448401171604869e-17, + "angularVelocity": -3.469198233269833e-16, + "velocityX": 3.08219955246049, + "velocityY": -2.19162701872845, + "timestamp": 8.688203260995014 + }, + { + "x": 4.837250041803269, + "y": 4.581509560989999, + "heading": -7.180181418195748e-17, + "angularVelocity": -2.1134907415213478e-16, + "velocityX": 3.1329689411472432, + "velocityY": -2.1184165430654605, + "timestamp": 8.722827507608889 + }, + { + "x": 4.948569206649129, + "y": 4.512551245909545, + "heading": -5.993451372549524e-17, + "angularVelocity": 3.427453769485116e-16, + "velocityX": 3.2150638853539646, + "velocityY": -1.9916192213354724, + "timestamp": 8.757451754222764 + }, + { + "x": 5.062553327275946, + "y": 4.4480933081859515, + "heading": -6.273480456472046e-17, + "angularVelocity": -8.08765854062184e-17, + "velocityX": 3.2920317919968585, + "velocityY": -1.8616415959145531, + "timestamp": 8.792076000836639 + }, + { + "x": 5.17902058017923, + "y": 4.38823861530867, + "heading": -5.454489606023899e-17, + "angularVelocity": 2.365368002574791e-16, + "velocityX": 3.3637483640325954, + "velocityY": -1.728693003627544, + "timestamp": 8.826700247450514 + }, + { + "x": 5.297785167201245, + "y": 4.333082674122234, + "heading": -4.169311661554588e-17, + "angularVelocity": 3.711786017787193e-16, + "velocityX": 3.43009880753392, + "velocityY": -1.5929860309027757, + "timestamp": 8.86132449406439 + }, + { + "x": 5.4186576198068925, + "y": 4.282713488635075, + "heading": -3.1063280252894355e-17, + "angularVelocity": 3.0700556411606523e-16, + "velocityX": 3.490977116516063, + "velocityY": -1.4547373708623472, + "timestamp": 8.895948740678264 + }, + { + "x": 5.541445103981359, + "y": 4.23721142295766, + "heading": -1.7411383468509857e-17, + "angularVelocity": 3.9428718659707233e-16, + "velocityX": 3.5462860908939025, + "velocityY": -1.3141676751799676, + "timestamp": 8.93057298729214 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 4.364197825891865e-8, - "angularVelocity": -9.037666511803314e-13, - "velocityX": 3.5958636064268057, - "velocityY": -1.171727921713515, - "timestamp": 8.965239048950341 - }, - { - "x": 5.80048142988227, - "y": 4.159069716430897, - "heading": 4.364194761543742e-8, - "angularVelocity": -8.296994367988738e-13, - "velocityX": 3.642511096804761, - "velocityY": -1.0174944855729342, - "timestamp": 9.00217227920995 - }, - { - "x": 5.936489765010674, - "y": 4.127254911109845, - "heading": 4.364191906311607e-8, - "angularVelocity": -7.730794282349154e-13, - "velocityX": 3.6825464269543904, - "velocityY": -0.8614141004570673, - "timestamp": 9.039105509469557 - }, - { - "x": 6.073729843507157, - "y": 4.101262406827453, - "heading": 4.3641892220156324e-8, - "angularVelocity": -7.267969466454947e-13, - "velocityX": 3.7158969722335238, - "velocityY": -0.7037701305758657, - "timestamp": 9.076038739729166 - }, - { - "x": 6.211952539114824, - "y": 4.081139381676687, - "heading": 4.3641866716107116e-8, - "angularVelocity": -6.905447558160659e-13, - "velocityX": 3.742502202923576, - "velocityY": -0.5448487719411382, - "timestamp": 9.112971969988774 - }, - { - "x": 6.350906942398405, - "y": 4.066922357709394, - "heading": 4.364184226449404e-8, - "angularVelocity": -6.620491141311732e-13, - "velocityX": 3.7623138378867345, - "velocityY": -0.38493854632702584, - "timestamp": 9.149905200248382 - }, - { - "x": 6.490340706014696, - "y": 4.058637143370339, - "heading": 4.364062068850958e-8, - "angularVelocity": -3.3075253340927824e-11, - "velocityX": 3.775292944489131, - "velocityY": -0.22432953415709472, - "timestamp": 9.18683843050799 - }, - { - "x": 6.625895301013595, - "y": 4.054865050461972, - "heading": 3.8021532173824235e-8, - "angularVelocity": -1.521418068678583e-7, - "velocityX": 3.6702610101003303, - "velocityY": -0.10213276450108619, - "timestamp": 9.223771660767598 - }, - { - "x": 6.755934035567496, - "y": 4.053327166371606, - "heading": 3.0189413996037156e-8, - "angularVelocity": -2.1206154817940447e-7, - "velocityX": 3.52091419136215, - "velocityY": -0.04163957713841639, - "timestamp": 9.260704891027206 - }, - { - "x": 6.880153762983055, - "y": 4.0530365385095966, - "heading": 2.1734826824788128e-8, - "angularVelocity": -2.2891544824669977e-7, - "velocityX": 3.3633594067565498, - "velocityY": -0.007869007394285612, - "timestamp": 9.297638121286814 - }, - { - "x": 6.998470106919647, - "y": 4.0534986388519805, - "heading": 1.3540571224920633e-8, - "angularVelocity": -2.2186673987685247e-7, - "velocityX": 3.2035200578159904, - "velocityY": 0.012511777040293628, - "timestamp": 9.334571351546423 - }, - { - "x": 7.110853220415867, - "y": 4.054422625435363, - "heading": 6.1666659449803414e-9, - "angularVelocity": -1.9965502711211782e-7, - "velocityX": 3.042872575896168, - "velocityY": 0.025017757095422304, - "timestamp": 9.37150458180603 + "heading": -2.060214519362369e-17, + "angularVelocity": -9.215396827963617e-17, + "velocityX": 3.5959374431427245, + "velocityY": -1.1715012562024099, + "timestamp": 8.965197233906014 + }, + { + "x": 5.800482065965206, + "y": 4.159078601267801, + "heading": -2.3136950753163445e-17, + "angularVelocity": -6.863298410721474e-17, + "velocityX": 3.6425746580693774, + "velocityY": -1.0172668618134628, + "timestamp": 9.002129994322162 + }, + { + "x": 5.936490640801473, + "y": 4.1272726257790175, + "heading": -1.8598714484333613e-17, + "angularVelocity": 1.228783394072173e-16, + "velocityX": 3.682599765188514, + "velocityY": -0.8611859804250445, + "timestamp": 9.03906275473831 + }, + { + "x": 6.073730568489753, + "y": 4.101288878730342, + "heading": -4.2719996436236496e-17, + "angularVelocity": -6.531134331502948e-16, + "velocityX": 3.7159401610359515, + "velocityY": -0.7035419707570875, + "timestamp": 9.075995515154458 + }, + { + "x": 6.2119527294046275, + "y": 4.081174520992199, + "heading": -2.525874984240029e-17, + "angularVelocity": 4.727847688027027e-16, + "velocityX": 3.742535336038484, + "velocityY": -0.5446210224066904, + "timestamp": 9.112928275570606 + }, + { + "x": 6.350906221496031, + "y": 4.066966057794483, + "heading": 5.651500811291553e-19, + "angularVelocity": 6.992139128857502e-16, + "velocityX": 3.7623370288523534, + "velocityY": -0.3847116499719891, + "timestamp": 9.149861035986754 + }, + { + "x": 6.49033871531031, + "y": 4.058689280403353, + "heading": 9.896194803608596e-18, + "angularVelocity": 2.526495343979898e-16, + "velocityX": 3.7753065907664864, + "velocityY": -0.2241039472238093, + "timestamp": 9.186793796402903 + }, + { + "x": 6.62590091042267, + "y": 4.054933032848218, + "heading": 1.2997488502950548e-17, + "angularVelocity": 8.397134860988979e-17, + "velocityX": 3.67051348409614, + "velocityY": -0.10170503132750916, + "timestamp": 9.22372655681905 + }, + { + "x": 6.7559406325562135, + "y": 4.053394192007746, + "heading": 1.2355813206346684e-17, + "angularVelocity": -1.7374149364595927e-17, + "velocityX": 3.520985722927149, + "velocityY": -0.04166601204816814, + "timestamp": 9.260659317235199 + }, + { + "x": 6.880159033617416, + "y": 4.05309103302126, + "heading": 1.0199232759050705e-17, + "angularVelocity": -5.839207311638201e-17, + "velocityX": 3.3633662813596223, + "velocityY": -0.008208403137756505, + "timestamp": 9.297592077651347 + }, + { + "x": 6.998473495645727, + "y": 4.053535026725706, + "heading": 6.484491281135395e-18, + "angularVelocity": -1.0058120312082129e-16, + "velocityX": 3.2035098567011917, + "velocityY": 0.012021676675224401, + "timestamp": 9.334524838067495 + }, + { + "x": 7.1108547909012785, + "y": 4.054439771564095, + "heading": 3.84540621087932e-18, + "angularVelocity": -7.14564803668238e-17, + "velocityX": 3.0428620549689076, + "velocityY": 0.024497081404008324, + "timestamp": 9.371457598483643 }, { "x": 7.217291355133057, "y": 4.0556182861328125, - "heading": -3.6086824530888626e-33, - "angularVelocity": -1.6696795107584883e-7, - "velocityX": 2.8819069972765123, - "velocityY": 0.03237357493636809, - "timestamp": 9.408437812065639 - }, - { - "x": 7.405345242374078, - "y": 4.058539790558069, - "heading": -6.074177156590073e-9, - "angularVelocity": -8.274407475492158e-8, - "velocityX": 2.561720622548375, - "velocityY": 0.03979751891783301, - "timestamp": 9.481847022104299 - }, - { - "x": 7.569888594263639, - "y": 4.061600743201228, - "heading": -8.336905285122374e-9, - "angularVelocity": -3.0823491082343837e-8, - "velocityX": 2.241453787650163, - "velocityY": 0.04169712004183413, - "timestamp": 9.555256232142959 - }, - { - "x": 7.71092123884848, - "y": 4.064554734769034, - "heading": -8.259320686300401e-9, - "angularVelocity": 1.056878269798436e-9, - "velocityX": 1.9211846103583805, - "velocityY": 0.04024006750993344, - "timestamp": 9.62866544218162 - }, - { - "x": 7.828444510796415, - "y": 4.067236236264996, - "heading": -6.843181194470064e-9, - "angularVelocity": 1.929103299991646e-8, - "velocityX": 1.600933614270509, - "velocityY": 0.03652813447454146, - "timestamp": 9.70207465222028 - }, - { - "x": 7.922460087151161, - "y": 4.069526407614099, - "heading": -4.8146064610859125e-9, - "angularVelocity": 2.7633790562623428e-8, - "velocityX": 1.2807054633231172, - "velocityY": 0.031197329979384493, - "timestamp": 9.77548386225894 - }, - { - "x": 7.992969626740666, - "y": 4.071335791937442, - "heading": -2.7240893137969417e-9, - "angularVelocity": 2.8477586783070736e-8, - "velocityX": 0.9604999093761202, - "velocityY": 0.024647919823550288, - "timestamp": 9.8488930722976 - }, - { - "x": 8.039974659720452, - "y": 4.072594619838684, - "heading": -1.0033424996203534e-9, - "angularVelocity": 2.3440475863128373e-8, - "velocityX": 0.6403151996190951, - "velocityY": 0.01714809218869153, - "timestamp": 9.92230228233626 + "heading": 9.032556666486843e-28, + "angularVelocity": -1.0411911175769075e-16, + "velocityX": 2.881901136889931, + "velocityY": 0.03190973421532392, + "timestamp": 9.40839035889979 + }, + { + "x": 7.405345313483542, + "y": 4.0585235652542355, + "heading": -6.2741861661918545e-18, + "angularVelocity": -8.546860903842953e-17, + "velocityX": 2.5617203276716056, + "velocityY": 0.039576473944975675, + "timestamp": 9.481799605146984 + }, + { + "x": 7.569888777644686, + "y": 4.061578730558428, + "heading": -8.568202082154757e-18, + "angularVelocity": -3.124968629224788e-17, + "velocityX": 2.241454211463674, + "velocityY": 0.04161826282625519, + "timestamp": 9.555208851394177 + }, + { + "x": 7.710921484094839, + "y": 4.064533122417797, + "heading": -8.450454397008985e-18, + "angularVelocity": 1.603989837150655e-18, + "velocityX": 1.9211845054947108, + "velocityY": 0.040245500538451925, + "timestamp": 9.62861809764137 + }, + { + "x": 7.828444755273268, + "y": 4.067218459024374, + "heading": -7.03981174142149e-18, + "angularVelocity": 1.921614411657278e-17, + "velocityX": 1.6009328141401953, + "velocityY": 0.03658035933967648, + "timestamp": 9.702027343888563 + }, + { + "x": 7.922460282697814, + "y": 4.069513975215649, + "heading": -4.798172497228405e-18, + "angularVelocity": 3.053619753065246e-17, + "velocityX": 1.2807041650852087, + "velocityY": 0.03127012343303199, + "timestamp": 9.775436590135756 + }, + { + "x": 7.992969748455868, + "y": 4.071328793464346, + "heading": -2.7225824385142056e-18, + "angularVelocity": 2.8274231988604325e-17, + "velocityX": 0.9604984298657211, + "velocityY": 0.024721930022069162, + "timestamp": 9.84884583638295 + }, + { + "x": 8.039974708011181, + "y": 4.0725920534308, + "heading": -1.0566305754795318e-18, + "angularVelocity": 2.2694033081505876e-17, + "velocityX": 0.6403138835812464, + "velocityY": 0.01720845848492492, + "timestamp": 9.922255082630143 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 1.0901032811452175e-35, - "angularVelocity": 1.3667801358727972e-8, - "velocityX": 0.32014923968219366, - "velocityY": 0.00888629686322534, - "timestamp": 9.99571149237492 + "heading": -4.292440471277117e-28, + "angularVelocity": 1.4393698734895493e-17, + "velocityX": 0.32014842394214194, + "velocityY": 0.0089212527612154, + "timestamp": 9.995664328877336 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 2.780011652171749e-35, - "angularVelocity": 2.3020443776927775e-34, - "velocityX": -1.517814230631444e-35, - "velocityY": 4.908282159537256e-34, - "timestamp": 10.06912070241358 + "heading": -2.12372396116075e-28, + "angularVelocity": 6.129002987458633e-29, + "velocityX": -2.550409184277031e-23, + "velocityY": -6.242614501745333e-23, + "timestamp": 10.069073575124529 } ], - "constraints": [ + "trajectoryWaypoints": [ { - "scope": [ + "timestamp": 0, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 1.2321688118183196, + "isStopPoint": false, + "x": 2.614555597305298, + "y": 4.302172660827637, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 2.609443647498003, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 3.584945174554365, + "isStopPoint": false, + "x": 2.525056838989258, + "y": 5.573054313659668, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 4.737931380295831, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 5.964473570718389, + "isStopPoint": false, + "x": 2.68615460395813, + "y": 6.772337436676025, + "heading": 0.8050032485420815, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 7.335442806268824, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 8.515082027925638, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 8.965197233906014, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "timestamp": 9.40839035889979, + "isStopPoint": false, + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 10.069073575124529, + "isStopPoint": true, + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 2 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 4 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 6 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "CenterLanePSubCSubBSubASubFSub": { "waypoints": [ @@ -5332,15 +4829,6 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 - }, - { - "x": 2.462606906890869, - "y": 6.522137641906738, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, "controlIntervalCount": 40 } ], @@ -5348,1965 +4836,2020 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -6.079995614896932e-33, - "velocityX": 0, - "velocityY": 0, + "heading": 4.7251350507187945e-25, + "angularVelocity": 2.5513158875858104e-25, + "velocityX": 5.636670431941991e-25, + "velocityY": -5.06363269097065e-25, "timestamp": 0 }, { - "x": 1.306947763000364, - "y": 5.574973108257302, - "heading": -0.008994433988091095, - "angularVelocity": -0.12277149964907928, - "velocityX": 0.23168094435597542, - "velocityY": -0.21813883323536412, - "timestamp": 0.07326157955062942 - }, - { - "x": 1.3408944736698363, - "y": 5.543010933982932, - "heading": -0.026978879063552612, - "angularVelocity": -0.24548262794460876, - "velocityX": 0.463363073492191, - "velocityY": -0.4362747086592839, - "timestamp": 0.14652315910125885 - }, - { - "x": 1.3918148440551168, - "y": 5.495068027805583, - "heading": -0.05394057952659419, - "angularVelocity": -0.3680196445178875, - "velocityX": 0.695048764954495, - "velocityY": -0.6544072141417159, - "timestamp": 0.21978473865188827 - }, - { - "x": 1.4597093144768722, - "y": 5.431144700814373, - "heading": -0.08985971213865897, - "angularVelocity": -0.4902860794482582, - "velocityX": 0.9267404666703231, - "velocityY": -0.8725354733449758, - "timestamp": 0.2930463182025177 - }, - { - "x": 1.5445785095442424, - "y": 5.351241370164591, - "heading": -0.13471107898519388, - "angularVelocity": -0.6122085699167754, - "velocityX": 1.1584406941256162, - "velocityY": -1.0906580384956333, - "timestamp": 0.3663078977531471 - }, - { - "x": 1.6464232512218273, - "y": 5.2553586089581215, - "heading": -0.1884659824129613, - "angularVelocity": -0.733739345472597, - "velocityX": 1.390152141167011, - "velocityY": -1.3087727809664234, - "timestamp": 0.4395694773037765 - }, - { - "x": 1.7652446226105383, - "y": 5.143497220947543, - "heading": -0.25109340653071877, - "angularVelocity": -0.8548467628175698, - "velocityX": 1.6218783722318773, - "velocityY": -1.5268765524400676, - "timestamp": 0.5128310568544059 - }, - { - "x": 1.9010443771824763, - "y": 5.015658495720495, - "heading": -0.32255561727496634, - "angularVelocity": -0.9754391207858365, - "velocityX": 1.8536285376988066, - "velocityY": -1.7449627214043517, - "timestamp": 0.5860926364050353 - }, - { - "x": 2.053859595250169, - "y": 4.871864096281773, - "heading": -0.4022671776466062, - "angularVelocity": -1.0880404280193408, - "velocityX": 2.0858848390251463, - "velocityY": -1.9627531964329257, - "timestamp": 0.6593542159556647 - }, - { - "x": 2.1896989198822134, - "y": 4.744047518400832, - "heading": -0.47311179344304544, - "angularVelocity": -0.9670091230763074, - "velocityX": 1.8541686579139196, - "velocityY": -1.7446604163456465, - "timestamp": 0.7326157955062941 - }, - { - "x": 2.308560686135844, - "y": 4.632207510590712, - "heading": -0.535112719027594, - "angularVelocity": -0.846295233666115, - "velocityX": 1.6224297508012064, - "velocityY": -1.5265847187041606, - "timestamp": 0.8058773750569235 - }, - { - "x": 2.410444090432368, - "y": 4.536343322874329, - "heading": -0.5882784511543253, - "angularVelocity": -0.72569732256441, - "velocityX": 1.3906798750648661, - "velocityY": -1.3085192580393974, - "timestamp": 0.8791389546075529 - }, - { - "x": 2.49534860091136, - "y": 4.4564544045860375, - "heading": -0.6326127919617512, - "angularVelocity": -0.6051513095863228, - "velocityX": 1.158922739582985, - "velocityY": -1.090461313806123, - "timestamp": 0.9524005341581823 - }, - { - "x": 2.5632738362111045, - "y": 4.3925403400214185, - "heading": -0.6681173722205684, - "angularVelocity": -0.48462755617056985, - "velocityX": 0.9271603986207089, - "velocityY": -0.8724090438215177, - "timestamp": 1.0256621137088118 - }, - { - "x": 2.6142195241645787, - "y": 4.344600823178412, - "heading": -0.6947927082053813, - "angularVelocity": -0.36411084975827024, - "velocityX": 0.6953943426549579, - "velocityY": -0.6543609506791417, - "timestamp": 1.0989236932594413 - }, - { - "x": 2.6481854848919673, - "y": 4.312635645188571, - "heading": -0.7126387384821254, - "angularVelocity": -0.24359330478823768, - "velocityX": 0.4636258313802147, - "velocityY": -0.43631570853248003, - "timestamp": 1.1721852728100708 + "x": 1.3069477629998796, + "y": 5.574973108256567, + "heading": -0.008994433993404482, + "angularVelocity": -0.12277149941396481, + "velocityX": 0.23168094376881843, + "velocityY": -0.2181388326987634, + "timestamp": 0.07326157973420823 + }, + { + "x": 1.3408944736683446, + "y": 5.5430109339807085, + "heading": -0.02697887908003383, + "angularVelocity": -0.2454826274819163, + "velocityX": 0.46336307231734836, + "velocityY": -0.4362747075863933, + "timestamp": 0.14652315946841646 + }, + { + "x": 1.3918148440520437, + "y": 5.495068027801086, + "heading": -0.05394057956084241, + "angularVelocity": -0.3680196438382188, + "velocityX": 0.6950487631912567, + "velocityY": -0.6544072125329379, + "timestamp": 0.2197847392026247 + }, + { + "x": 1.4597093144715716, + "y": 5.431144700806779, + "heading": -0.0898597121983473, + "angularVelocity": -0.4902860785669502, + "velocityX": 0.9267404643176941, + "velocityY": -0.8725354712008585, + "timestamp": 0.2930463189368329 + }, + { + "x": 1.5445785095359612, + "y": 5.351241370153018, + "heading": -0.13471107907964566, + "angularVelocity": -0.6122085688572148, + "velocityX": 1.1584406911821108, + "velocityY": -1.090658035816974, + "timestamp": 0.36630789867104113 + }, + { + "x": 1.646423251209635, + "y": 5.255358608941595, + "heading": -0.18846598255428795, + "angularVelocity": -0.733739344273822, + "velocityX": 1.390152137630185, + "velocityY": -1.308772777754508, + "timestamp": 0.43956947840524935 + }, + { + "x": 1.7652446225931595, + "y": 5.143497220924912, + "heading": -0.2510934067365697, + "angularVelocity": -0.854846761556235, + "velocityX": 1.6218783680969795, + "velocityY": -1.5268765486973532, + "timestamp": 0.5128310581394576 + }, + { + "x": 1.9010443771576746, + "y": 5.0156584956901185, + "heading": -0.3225556175785802, + "angularVelocity": -0.975439119676018, + "velocityX": 1.8536285329526645, + "velocityY": -1.7449627171375512, + "timestamp": 0.5860926378736658 + }, + { + "x": 2.053859595231987, + "y": 4.871864096254743, + "heading": -0.40226717784175786, + "angularVelocity": -1.0880404238124515, + "velocityX": 2.0858848338886964, + "velocityY": -1.9627531914689686, + "timestamp": 0.659354217607874 + }, + { + "x": 2.189698919868216, + "y": 4.744047518376405, + "heading": -0.4731117935721764, + "angularVelocity": -0.9670091197520122, + "velocityX": 1.8541686533248656, + "velocityY": -1.7446604119383569, + "timestamp": 0.7326157973420823 + }, + { + "x": 2.3085606861249848, + "y": 4.632207510568855, + "heading": -0.5351127191107611, + "angularVelocity": -0.8462952309180741, + "velocityX": 1.6224297467785629, + "velocityY": -1.5265847148437626, + "timestamp": 0.8058773770762905 + }, + { + "x": 2.410444090424028, + "y": 4.5363433228552275, + "heading": -0.5882784512046502, + "angularVelocity": -0.7256973202976708, + "velocityX": 1.3906798716144855, + "velocityY": -1.3085192547228937, + "timestamp": 0.8791389568104987 + }, + { + "x": 2.495348600905127, + "y": 4.456454404569983, + "heading": -0.6326127919889597, + "angularVelocity": -0.6051513077544012, + "velocityX": 1.158922736707715, + "velocityY": -1.0904613110320627, + "timestamp": 0.9524005365447069 + }, + { + "x": 2.563273836206689, + "y": 4.392540340008763, + "heading": -0.6681173722323603, + "angularVelocity": -0.4846275547457583, + "velocityX": 0.927160396322236, + "velocityY": -0.8724090415890471, + "timestamp": 1.0256621162789152 + }, + { + "x": 2.6142195241617716, + "y": 4.344600823169547, + "heading": -0.6947927082081052, + "angularVelocity": -0.364110848722108, + "velocityX": 0.6953943409343986, + "velocityY": -0.6543609489877007, + "timestamp": 1.0989236960131235 + }, + { + "x": 2.6481854848906172, + "y": 4.312635645183918, + "heading": -0.712638738481162, + "angularVelocity": -0.24359330412750974, + "velocityX": 0.46362583023836135, + "velocityY": -0.4363157073816743, + "timestamp": 1.1721852757473317 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": -0.12307101033443453, - "velocityX": 0.23185602115327572, - "velocityY": -0.2182720825031122, - "timestamp": 1.2454468523607003 - }, - { - "x": 2.662943710448623, - "y": 4.298728558024191, - "heading": -0.72070100113944, - "angularVelocity": 0.011651349233264986, - "velocityX": -0.027206592771411266, - "velocityY": 0.025447590705983056, - "timestamp": 1.3273355622067569 - }, - { - "x": 2.639501593010729, - "y": 4.320770481747221, - "heading": -0.7087160822503835, - "angularVelocity": 0.14635618159801306, - "velocityX": -0.2862680030246291, - "velocityY": 0.26916926355864296, - "timestamp": 1.4092242720528134 - }, - { - "x": 2.594845480264057, - "y": 4.36277075661854, - "heading": -0.6857017147672647, - "angularVelocity": 0.2810444507720759, - "velocityX": -0.5453268567867454, - "velocityY": 0.5128945730159264, - "timestamp": 1.49111298189887 - }, - { - "x": 2.5289756911084322, - "y": 4.424729822216665, - "heading": -0.6516589308540864, - "angularVelocity": 0.4157201154734962, - "velocityX": -0.8043818162412647, - "velocityY": 0.7566252504722968, - "timestamp": 1.5730016917449265 - }, - { - "x": 2.4418926576481637, - "y": 4.5066482714289275, - "heading": -0.6065878922273316, - "angularVelocity": 0.550393805342452, - "velocityX": -1.0634314989694753, - "velocityY": 1.000363168088272, - "timestamp": 1.654890401590983 - }, - { - "x": 2.3335969392795284, - "y": 4.608526864652899, - "heading": -0.5504868101397262, - "angularVelocity": 0.6850893388486693, - "velocityX": -1.322474350520624, - "velocityY": 1.2441103714479649, - "timestamp": 1.7367791114370397 - }, - { - "x": 2.2040892544412327, - "y": 4.730366540742225, - "heading": -0.48334990238896663, - "angularVelocity": 0.8198554828494793, - "velocityX": -1.5815084287169574, - "velocityY": 1.4878690398025962, - "timestamp": 1.8186678212830962 - }, - { - "x": 2.0533705390200705, - "y": 4.8721684079929855, - "heading": -0.4051637468918412, - "angularVelocity": 0.9547855332451622, - "velocityX": -1.840531078148622, - "velocityY": 1.7316412423316419, - "timestamp": 1.9005565311291528 - }, - { - "x": 1.8837222142414454, - "y": 5.031895792612386, - "heading": -0.31535335280985394, - "angularVelocity": 1.0967371967493782, - "velocityX": -2.071693705976669, - "velocityY": 1.9505422044097782, - "timestamp": 1.9824452409752094 - }, - { - "x": 1.7352824931464605, - "y": 5.171658722106698, - "heading": -0.2366561880408508, - "angularVelocity": 0.9610258229363577, - "velocityX": -1.8127006930996745, - "velocityY": 1.706742355045694, - "timestamp": 2.0643339508212657 - }, - { - "x": 1.6080502362813274, - "y": 5.291456457157948, - "heading": -0.16911546139257225, - "angularVelocity": 0.8247867963147685, - "velocityX": -1.5537215948855285, - "velocityY": 1.4629334773555265, - "timestamp": 2.146222660667322 - }, - { - "x": 1.5020244624691528, - "y": 5.391288489879848, - "heading": -0.11277393629567076, - "angularVelocity": 0.6880255556940454, - "velocityX": -1.294754478504957, - "velocityY": 1.219118397512621, - "timestamp": 2.2281113705133784 - }, - { - "x": 1.4172044018013137, - "y": 5.471154488105965, - "heading": -0.06766925261840594, - "angularVelocity": 0.550804668458663, - "velocityX": -1.0357967640141517, - "velocityY": 0.9752992613543129, - "timestamp": 2.3100000803594347 - }, - { - "x": 1.3535895239653573, - "y": 5.531054232218378, - "heading": -0.03382997301748837, - "angularVelocity": 0.4132349827532073, - "velocityX": -0.7768455255376048, - "velocityY": 0.7314774432888099, - "timestamp": 2.391888790205491 - }, - { - "x": 1.3111795494900627, - "y": 5.570987556283822, - "heading": -0.011272655480386088, - "angularVelocity": 0.27546309594458174, - "velocityX": -0.5178976998785506, - "velocityY": 0.48765359889678683, - "timestamp": 2.4737775000515474 + "angularVelocity": -0.12307101003919331, + "velocityX": 0.2318560205907196, + "velocityY": -0.2182720818926514, + "timestamp": 1.2454468554815399 + }, + { + "x": 2.6629437104502958, + "y": 4.2987285580297225, + "heading": -0.7207010011457456, + "angularVelocity": 0.011651349127194545, + "velocityX": -0.027206592683106, + "velocityY": 0.025447590710046028, + "timestamp": 1.3273355655318961 + }, + { + "x": 2.6395015930145194, + "y": 4.320770481758526, + "heading": -0.7087160822692053, + "angularVelocity": 0.14635618108003276, + "velocityX": -0.28626800228457533, + "velocityY": 0.2691692629576105, + "timestamp": 1.4092242755822524 + }, + { + "x": 2.594845480270507, + "y": 4.362770756636002, + "heading": -0.6857017148059629, + "angularVelocity": 0.2810444498281884, + "velocityX": -0.545326855393761, + "velocityY": 0.5128945718115293, + "timestamp": 1.4911129856326086 + }, + { + "x": 2.52897569111822, + "y": 4.424729822240869, + "heading": -0.6516589309216121, + "angularVelocity": 0.415720114084306, + "velocityX": -0.8043818141936927, + "velocityY": 0.7566252486669522, + "timestamp": 1.5730016956829649 + }, + { + "x": 2.4418926576621733, + "y": 4.506648271460754, + "heading": -0.6065878923349923, + "angularVelocity": 0.550393803479186, + "velocityX": -1.0634314962648228, + "velocityY": 1.0003631656856, + "timestamp": 1.6548904057333211 + }, + { + "x": 2.333596939298986, + "y": 4.608526864693725, + "heading": -0.5504868103027082, + "angularVelocity": 0.6850893364639103, + "velocityX": -1.322474347154724, + "velocityY": 1.2441103684539934, + "timestamp": 1.7367791157836774 + }, + { + "x": 2.2040892544680513, + "y": 4.7303665407944155, + "heading": -0.48334990263012423, + "angularVelocity": 0.8198554798494097, + "velocityX": -1.5815084246814508, + "velocityY": 1.4878690362293745, + "timestamp": 1.8186678258340336 + }, + { + "x": 2.0533705390582235, + "y": 4.872168408061871, + "heading": -0.40516374725682197, + "angularVelocity": 0.9547855293510289, + "velocityX": -1.8405310734183675, + "velocityY": 1.7316412382153215, + "timestamp": 1.9005565358843899 + }, + { + "x": 1.8837222142672279, + "y": 5.0318957926604115, + "heading": -0.31535335305240253, + "angularVelocity": 1.0967371955082925, + "velocityX": -2.0716937009591816, + "velocityY": 1.9505421992887515, + "timestamp": 1.9824452459347461 + }, + { + "x": 1.7352824931641453, + "y": 5.171658722140381, + "heading": -0.23665618820579565, + "angularVelocity": 0.9610258214864199, + "velocityX": -1.8127006886761512, + "velocityY": 1.7067423506124824, + "timestamp": 2.064333955985102 + }, + { + "x": 1.608050236293127, + "y": 5.291456457180838, + "heading": -0.1691154615021004, + "angularVelocity": 0.8247867949337834, + "velocityX": -1.5537215910810984, + "velocityY": 1.4629334735739428, + "timestamp": 2.146222666035458 + }, + { + "x": 1.5020244624765908, + "y": 5.391288489894511, + "heading": -0.11277393636454519, + "angularVelocity": 0.6880255544739787, + "velocityX": -1.2947544753280062, + "velocityY": 1.2191183943706372, + "timestamp": 2.228111376085814 + }, + { + "x": 1.4172044018055678, + "y": 5.471154488114474, + "heading": -0.06766925265776652, + "angularVelocity": 0.5508046674449045, + "velocityX": -1.03579676146888, + "velocityY": 0.9752992588459369, + "timestamp": 2.3100000861361702 + }, + { + "x": 1.353589523967397, + "y": 5.531054232222511, + "heading": -0.033829973036366284, + "angularVelocity": 0.4132349819723778, + "velocityX": -0.7768455236265391, + "velocityY": 0.7314774414104627, + "timestamp": 2.3918887961865263 + }, + { + "x": 1.3111795494907175, + "y": 5.570987556285165, + "heading": -0.011272655486454928, + "angularVelocity": 0.27546309541376424, + "velocityX": -0.517897698603385, + "velocityY": 0.4876535976460953, + "timestamp": 2.4737775062368823 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0.13765823764445279, - "velocityX": -0.2589502077253713, - "velocityY": 0.24382784263139995, - "timestamp": 2.5556662098976037 + "heading": -7.670908759441255e-25, + "angularVelocity": 0.13765823737512747, + "velocityX": -0.2589502070873256, + "velocityY": 0.24382784200668048, + "timestamp": 2.5556662162872383 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.169901203643428e-30, - "velocityY": 4.744348570353821e-29, - "timestamp": 2.63755491974366 - }, - { - "x": 1.3279764392732873, - "y": 5.589659578477318, - "heading": -3.8286967101155775e-19, - "angularVelocity": -4.101142681562708e-18, - "velocityX": 0.40706315074964167, - "velocityY": -0.013868615044214471, - "timestamp": 2.7309114128077048 - }, - { - "x": 1.4039804148734387, - "y": 5.5870701279768555, - "heading": -4.0712541473530154e-19, - "angularVelocity": -2.5982712070316094e-19, - "velocityX": 0.814126292755424, - "velocityY": -0.027737229790526226, - "timestamp": 2.8242679058717495 - }, - { - "x": 1.517986376641016, - "y": 5.583185952281787, - "heading": 4.833998363566258e-19, - "angularVelocity": 9.538831821902836e-18, - "velocityX": 1.2211894216447965, - "velocityY": -0.04160584408996275, - "timestamp": 2.917624398935794 - }, - { - "x": 1.6699943225350276, - "y": 5.578007051461648, - "heading": 1.3619244496265011e-18, - "angularVelocity": 9.410228943340493e-18, - "velocityX": 1.6282525286718303, - "velocityY": -0.05547445764455082, - "timestamp": 3.010980891999839 - }, - { - "x": 1.8600042484729113, - "y": 5.571533425655531, - "heading": 3.711758987250246e-18, - "angularVelocity": 2.5170201581763002e-17, - "velocityX": 2.0353155919679775, - "velocityY": -0.06934306970923045, - "timestamp": 3.1043373850638836 - }, - { - "x": 2.0880161422023416, - "y": 5.563765075280871, - "heading": -9.287085834288012e-18, - "angularVelocity": -1.392382283532996e-16, - "velocityX": 2.4423785240217755, - "velocityY": -0.08321167730249175, - "timestamp": 3.1976938781279283 - }, - { - "x": 2.2780692584306133, - "y": 5.5572899779843725, - "heading": -5.547928961434018e-18, - "angularVelocity": 4.0052703333031833e-17, - "velocityX": 2.03577823010496, - "velocityY": -0.06935883176043627, - "timestamp": 3.291050371191973 - }, - { - "x": 2.430120406839514, - "y": 5.552109605257364, - "heading": -3.2776580295073633e-18, - "angularVelocity": 2.4318713794052425e-17, - "velocityX": 1.6287152977531212, - "velocityY": -0.05549022415702071, - "timestamp": 3.3844068642560177 - }, - { - "x": 2.5441695751975866, - "y": 5.548223957516569, - "heading": -1.9211838299437728e-18, - "angularVelocity": 1.4530579976060665e-17, - "velocityX": 1.2216522343824612, - "velocityY": -0.04162161208980244, - "timestamp": 3.4777633573200624 - }, - { - "x": 2.6202167594261327, - "y": 5.545633034900951, - "heading": -1.8132356413213059e-19, - "angularVelocity": 1.8636270440618356e-17, - "velocityX": 0.8145891273223099, - "velocityY": -0.02775299853408606, - "timestamp": 3.571119850384107 - }, - { - "x": 2.6582619574855144, - "y": 5.5443368374799995, - "heading": 8.843883513334677e-20, - "angularVelocity": 2.8892814546014257e-18, - "velocityX": 0.40752599841430864, - "velocityY": -0.013884384234014852, - "timestamp": 3.6644763434481518 + "heading": -3.41171038639136e-25, + "angularVelocity": 8.93245678861437e-25, + "velocityX": 7.394213704779868e-19, + "velocityY": -2.5191717110574877e-20, + "timestamp": 2.6375549263375944 + }, + { + "x": 1.3279764533229341, + "y": 5.589659577998647, + "heading": -1.0733184537140863e-18, + "angularVelocity": -1.1496982275828825e-17, + "velocityX": 0.4070632249719813, + "velocityY": -0.013868617572964747, + "timestamp": 2.7309114368906817 + }, + { + "x": 1.4039804570223795, + "y": 5.587070126540843, + "heading": -3.1026019980884896e-18, + "angularVelocity": -2.173692641491287e-17, + "velocityX": 0.8141264412001065, + "velocityY": -0.02773723484802693, + "timestamp": 2.824267947443769 + }, + { + "x": 1.5179864609388978, + "y": 5.5831859494097635, + "heading": -5.833562477390286e-18, + "angularVelocity": -2.9253027311160176e-17, + "velocityX": 1.2211896443118286, + "velocityY": -0.041605851676214084, + "timestamp": 2.9176244579968564 + }, + { + "x": 1.669994463031499, + "y": 5.578007046674942, + "heading": -9.265282118286952e-18, + "angularVelocity": -3.6759297272081945e-17, + "velocityX": 1.628252825561226, + "velocityY": -0.05547446775955324, + "timestamp": 3.010980968549944 + }, + { + "x": 1.8600044592176233, + "y": 5.571533418475472, + "heading": -1.3542122810335315e-17, + "angularVelocity": -4.5811917888604e-17, + "velocityX": 2.035315963079774, + "velocityY": -0.0693430823529852, + "timestamp": 3.104337479103031 + }, + { + "x": 2.08801643724496, + "y": 5.563765065228789, + "heading": -1.806715455043103e-17, + "angularVelocity": -4.8470447549846784e-17, + "velocityX": 2.442378969356152, + "velocityY": -0.08321169247500493, + "timestamp": 3.1976939896561185 + }, + { + "x": 2.278069539423553, + "y": 5.557289968410962, + "heading": -2.8931134060980015e-18, + "angularVelocity": 1.6253865304001022e-16, + "velocityX": 2.0357776983375815, + "velocityY": -0.06935881364315644, + "timestamp": 3.291050500209206 + }, + { + "x": 2.430120659733151, + "y": 5.552109596641294, + "heading": -3.268581465491534e-18, + "angularVelocity": -4.0218733262966905e-18, + "velocityX": 1.6287146917636088, + "velocityY": -0.05549020351099765, + "timestamp": 3.384407010762293 + }, + { + "x": 2.5441697859422803, + "y": 5.548223950336512, + "heading": -3.490008764973647e-18, + "angularVelocity": -2.371846303430449e-18, + "velocityX": 1.2216515541706623, + "velocityY": -0.04162158891503094, + "timestamp": 3.4777635213153806 + }, + { + "x": 2.62021691397224, + "y": 5.545633029635576, + "heading": -3.003302345401883e-18, + "angularVelocity": 5.213417288227865e-18, + "velocityX": 0.8145883728881999, + "velocityY": -0.027752972830565246, + "timestamp": 3.571120031868468 + }, + { + "x": 2.6582620417833907, + "y": 5.544336834607976, + "heading": -2.0773852345449385e-18, + "angularVelocity": 9.91807795339057e-18, + "velocityX": 0.4075251697578793, + "velocityY": -0.013884356001744477, + "timestamp": 3.6644765424215553 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": 1.7598923552723236e-34, - "angularVelocity": -9.476131898054967e-19, - "velocityX": 0.0004628563966037854, - "velocityY": -0.000015769487296898133, - "timestamp": 3.7578328365121965 - }, - { - "x": 2.620331993851312, - "y": 5.545629108871589, - "heading": 2.945836613273263e-19, - "angularVelocity": 3.1551525697239727e-18, - "velocityX": -0.4066774370940851, - "velocityY": 0.013855473805082683, - "timestamp": 3.8512070216080794 - }, - { - "x": 2.5443424272440374, - "y": 5.548218068458602, - "heading": 2.309166242357916e-19, - "angularVelocity": -6.815922301609038e-19, - "velocityX": -0.8138177218425701, - "velocityY": 0.027726716799615945, - "timestamp": 3.9445812067039623 - }, - { - "x": 2.4303364695545335, - "y": 5.552102244014731, - "heading": -7.469284722560017e-19, - "angularVelocity": -1.047218864069531e-17, - "velocityX": -1.220957993477128, - "velocityY": 0.041597959347358555, - "timestamp": 4.037955391799845 - }, - { - "x": 2.278314122823792, - "y": 5.55728163547044, - "heading": -1.7117540142209262e-18, - "angularVelocity": -1.0332835908519937e-17, - "velocityX": -1.628098243253484, - "velocityY": 0.055469201150393666, - "timestamp": 4.131329576895728 - }, - { - "x": 2.0882753911343768, - "y": 5.563756242686637, - "heading": -3.8779782019319956e-18, - "angularVelocity": -2.3199468985970604e-17, - "velocityX": -2.035238449307225, - "velocityY": 0.06934044146380212, - "timestamp": 4.22470376199161 - }, - { - "x": 1.8602202867386202, - "y": 5.571526065245884, - "heading": -6.955816575429636e-18, - "angularVelocity": -3.296264638305206e-17, - "velocityX": -2.4423785241434, - "velocityY": 0.08321167730663664, - "timestamp": 4.318077947087493 - }, - { - "x": 1.6701383566026529, - "y": 5.578002144230341, - "heading": -1.819668166700424e-18, - "angularVelocity": 5.5006116981871155e-17, - "velocityX": -2.035701087485777, - "velocityY": 0.0693562035164242, - "timestamp": 4.411452132183375 - }, - { - "x": 1.518072799200871, - "y": 5.5831830078708, - "heading": 4.234959937878501e-18, - "angularVelocity": 6.484281229150569e-17, - "velocityX": -1.6285610123513967, - "velocityY": 0.05548496766342984, - "timestamp": 4.5048263172792575 - }, - { - "x": 1.4040236267647241, - "y": 5.587068655750534, - "heading": 2.524941481520431e-18, - "angularVelocity": -1.831387557299539e-17, - "velocityX": -1.221420806223103, - "velocityY": 0.041613727347481366, - "timestamp": 4.59820050237514 - }, - { - "x": 1.327990843372909, - "y": 5.589659087730582, - "heading": 8.045224971714285e-19, - "angularVelocity": -1.842525382897137e-17, - "velocityX": -0.814280556413611, - "velocityY": 0.027742485543317336, - "timestamp": 4.6915746874710225 + "heading": 6.735492092706879e-29, + "angularVelocity": 2.2252173720073512e-17, + "velocityX": 0.00046195351785172767, + "velocityY": -0.00001573872627686245, + "timestamp": 3.7578330529746427 + }, + { + "x": 2.6203319236037554, + "y": 5.545629111264917, + "heading": 2.4136302284603065e-18, + "angularVelocity": 2.584901393114583e-17, + "velocityX": -0.40667826371536864, + "velocityY": 0.013855501968015705, + "timestamp": 3.851207221048392 + }, + { + "x": 2.544342300798434, + "y": 5.548218072766594, + "heading": 5.5338149846267035e-18, + "angularVelocity": 3.341593018331809e-17, + "velocityX": -0.8138184722063877, + "velocityY": 0.02772674236446206, + "timestamp": 3.9445813891221415 + }, + { + "x": 2.4303363009603927, + "y": 5.552102249758721, + "heading": 9.360515374591917e-18, + "angularVelocity": 4.0982429991846804e-17, + "velocityX": -1.220958667583486, + "velocityY": 0.04159798231411795, + "timestamp": 4.0379555571958905 + }, + { + "x": 2.278313926130621, + "y": 5.557281642171763, + "heading": 1.3522883806487307e-17, + "angularVelocity": 4.457730047824434e-17, + "velocityX": -1.6280988411023956, + "velocityY": 0.055469221519066796, + "timestamp": 4.13132972526964 + }, + { + "x": 2.088275180391678, + "y": 5.563756249866626, + "heading": 1.8245422432915976e-17, + "angularVelocity": 5.057649913145605e-17, + "velocityX": -2.03523897089873, + "velocityY": 0.06934045923439035, + "timestamp": 4.224703893343389 + }, + { + "x": 1.860220075995873, + "y": 5.571526072425875, + "heading": 6.8469520838322655e-18, + "angularVelocity": -1.2207305633045037e-16, + "velocityX": -2.4423789694777343, + "velocityY": 0.08321169247914799, + "timestamp": 4.318078061417139 + }, + { + "x": 1.6701382161075125, + "y": 5.578002149017001, + "heading": -6.786620946288625e-18, + "angularVelocity": -1.460101118846944e-16, + "velocityX": -2.035700706197767, + "velocityY": 0.06935619052596638, + "timestamp": 4.411452229490888 + }, + { + "x": 1.5180727149037914, + "y": 5.583183010742795, + "heading": -4.3005972299368415e-18, + "angularVelocity": 2.6624319168550005e-17, + "velocityX": -1.6285607073212787, + "velocityY": 0.05548495727107347, + "timestamp": 4.504826397564638 + }, + { + "x": 1.4040235846161857, + "y": 5.5870686571865305, + "heading": -1.9657241654870744e-18, + "angularVelocity": 2.5005556186566034e-17, + "velocityX": -1.221420577450572, + "velocityY": 0.04161371955321608, + "timestamp": 4.598200565638387 + }, + { + "x": 1.3279908293233964, + "y": 5.589659088209248, + "heading": -7.083875411773879e-19, + "angularVelocity": 1.3465572054358342e-17, + "velocityX": -0.8142804038986078, + "velocityY": 0.027742480347141066, + "timestamp": 4.6915747337121365 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 6.74699155537553e-28, - "angularVelocity": -8.616220285509517e-18, - "velocityX": -0.40714028476041414, - "velocityY": 0.013871242994939689, - "timestamp": 4.784948872566905 + "heading": -9.307960730995794e-22, + "angularVelocity": 7.576578596147462e-18, + "velocityX": -0.40714020850291777, + "velocityY": 0.013871240396851729, + "timestamp": 4.784948901785886 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -7.225786160817796e-27, - "velocityX": -2.979354413794432e-29, - "velocityY": 7.541641997510845e-28, - "timestamp": 4.878323057662787 - }, - { - "x": 1.3078091735186845, - "y": 5.606755773559667, - "heading": 0.009999668662888749, - "angularVelocity": 0.1349541802858926, - "velocityX": 0.24069501005494717, - "velocityY": 0.21325450657673967, - "timestamp": 4.952419826226246 - }, - { - "x": 1.3434786990122702, - "y": 5.638358364996936, - "heading": 0.029996210094567113, - "angularVelocity": 0.2698706275619814, - "velocityX": 0.48139110769232624, - "velocityY": 0.42650431390462895, - "timestamp": 5.0265165947897055 - }, - { - "x": 1.3969833303762573, - "y": 5.6857616519517675, - "heading": 0.05997803368561842, - "angularVelocity": 0.4046306495359042, - "velocityX": 0.7220912922884387, - "velocityY": 0.6397483706242104, - "timestamp": 5.1006133633531645 - }, - { - "x": 1.468323578276302, - "y": 5.748965089703653, - "heading": 0.09992693775422122, - "angularVelocity": 0.5391450240657001, - "velocityX": 0.9627983687594002, - "velocityY": 0.8529850758692316, - "timestamp": 5.1747101319166235 - }, - { - "x": 1.5575001323238937, - "y": 5.827967973754339, - "heading": 0.14982100646683655, - "angularVelocity": 0.6733636254605087, - "velocityX": 1.203514752134039, - "velocityY": 1.0662122733008166, - "timestamp": 5.248806900480083 - }, - { - "x": 1.6645138155479717, - "y": 5.922769410023865, - "heading": 0.20963796260623796, - "angularVelocity": 0.8072815765348765, - "velocityX": 1.444242242996271, - "velocityY": 1.279427404349801, - "timestamp": 5.322903669043542 - }, - { - "x": 1.789365525357909, - "y": 6.033368309242042, - "heading": 0.27935847839394334, - "angularVelocity": 0.9409386825166515, - "velocityX": 1.6849818451743819, - "velocityY": 1.4926278347551774, - "timestamp": 5.397000437607001 - }, - { - "x": 1.9320561732696155, - "y": 6.159763414266322, - "heading": 0.35896870362448313, - "angularVelocity": 1.074408867993172, - "velocityX": 1.925733749048249, - "velocityY": 1.7058112989342795, - "timestamp": 5.47109720617046 - }, - { - "x": 2.092586653477802, - "y": 6.301953347709479, - "heading": 0.44846110031344777, - "angularVelocity": 1.2077773218503376, - "velocityX": 2.1664977208531133, - "velocityY": 1.9189761740812865, - "timestamp": 5.545193974733919 - }, - { - "x": 2.2353547101625333, - "y": 6.428313752108457, - "heading": 0.5276057235865483, - "angularVelocity": 1.068125166619866, - "velocityX": 1.9267784472209708, - "velocityY": 1.7053429837972895, - "timestamp": 5.619290743297378 - }, - { - "x": 2.360281889838346, - "y": 6.538881119368732, - "heading": 0.5968684037316069, - "angularVelocity": 0.9347597942868423, - "velocityX": 1.686000376116786, - "velocityY": 1.4922022835269577, - "timestamp": 5.693387511860837 - }, - { - "x": 2.4673676751793194, - "y": 6.633656126085495, - "heading": 0.6562491552260078, - "angularVelocity": 0.8013946173366472, - "velocityX": 1.4452153232519684, - "velocityY": 1.279070714493898, - "timestamp": 5.767484280424296 - }, - { - "x": 2.5566116515465542, - "y": 6.712639287159663, - "heading": 0.7057476703680625, - "angularVelocity": 0.668025287685273, - "velocityX": 1.2044246744765545, - "velocityY": 1.0659460944941062, - "timestamp": 5.841581048987755 - }, - { - "x": 2.6280134773537864, - "y": 6.7758309931109295, - "heading": 0.7453637127245453, - "angularVelocity": 0.5346527671626246, - "velocityX": 0.9636294156990158, - "velocityY": 0.8528267448954308, - "timestamp": 5.915677817551214 - }, - { - "x": 2.6815728846604703, - "y": 6.823231527897903, - "heading": 0.7750971751130776, - "angularVelocity": 0.4012788001208449, - "velocityX": 0.7228305408320154, - "velocityY": 0.6397112275638166, - "timestamp": 5.989774586114673 - }, - { - "x": 2.7172896860156874, - "y": 6.854841077120622, - "heading": 0.7949480902508361, - "angularVelocity": 0.26790527462553493, - "velocityX": 0.4820291361527109, - "velocityY": 0.42659821506532714, - "timestamp": 6.063871354678132 - }, - { - "x": 2.7351637810076403, - "y": 6.870659730936303, - "heading": 0.8049166539682622, - "angularVelocity": 0.13453439203137166, - "velocityX": 0.2412263763293308, - "velocityY": 0.2134864193164304, - "timestamp": 6.137968123241591 + "heading": -2.878097861680603e-22, + "angularVelocity": 6.886148795301622e-21, + "velocityX": 9.386048457152016e-19, + "velocityY": 1.5189228236971111e-19, + "timestamp": 4.878323069859635 + }, + { + "x": 1.3078091854990117, + "y": 5.606755784251363, + "heading": 0.009999675661372767, + "angularVelocity": 0.13495422890571762, + "velocityX": 0.24069508999867756, + "velocityY": 0.21325457844806686, + "timestamp": 4.952419863583371 + }, + { + "x": 1.3434787349490442, + "y": 5.638358397074998, + "heading": 0.0299962311234221, + "angularVelocity": 0.26987072526518463, + "velocityX": 0.48139126752263756, + "velocityY": 0.426504457688991, + "timestamp": 5.0265166573071065 + }, + { + "x": 1.3969834022405148, + "y": 5.685761716114703, + "heading": 0.0599780758079259, + "angularVelocity": 0.40463079679653674, + "velocityX": 0.7220915319353576, + "velocityY": 0.6397485863753444, + "timestamp": 5.100613451030842 + }, + { + "x": 1.4683236980324899, + "y": 5.748965196655092, + "heading": 0.09992700807120888, + "angularVelocity": 0.5391452214819116, + "velocityX": 0.9627986881316648, + "velocityY": 0.8529853636587607, + "timestamp": 5.174710244754578 + }, + { + "x": 1.5575003119272133, + "y": 5.82796813420515, + "heading": 0.14982111213912733, + "angularVelocity": 0.673363873934206, + "velocityX": 1.2035151511037259, + "velocityY": 1.066212633229638, + "timestamp": 5.248807038478313 + }, + { + "x": 1.6645140669394536, + "y": 5.922769634696042, + "heading": 0.20963811089831047, + "angularVelocity": 0.8072818775695793, + "velocityX": 1.444242721368388, + "velocityY": 1.2794278365721479, + "timestamp": 5.322903832202049 + }, + { + "x": 1.7893658604543141, + "y": 6.033368608876843, + "heading": 0.27935867676309706, + "angularVelocity": 0.9409390388028722, + "velocityX": 1.684982402617327, + "velocityY": 1.4926283395359905, + "timestamp": 5.397000625925784 + }, + { + "x": 1.9320566039380949, + "y": 6.1597637996451535, + "heading": 0.35896895992945943, + "angularVelocity": 1.074409285011486, + "velocityX": 1.925734384888401, + "velocityY": 1.705811876821096, + "timestamp": 5.47109741964952 + }, + { + "x": 2.0925871914348457, + "y": 6.3019538297380615, + "heading": 0.4484614236084208, + "angularVelocity": 1.207777815766594, + "velocityX": 2.1664984330533477, + "velocityY": 1.9189768267578884, + "timestamp": 5.5451942133732555 + }, + { + "x": 2.235355236977834, + "y": 6.4283142227315375, + "heading": 0.5276060335900633, + "angularVelocity": 1.0681246246190785, + "velocityX": 1.926777642704601, + "velocityY": 1.7053422509022635, + "timestamp": 5.619291007096991 + }, + { + "x": 2.3602823931901473, + "y": 6.538881568185102, + "heading": 0.5968686960571826, + "angularVelocity": 0.9347592383735241, + "velocityX": 1.685999487077598, + "velocityY": 1.4922014826418244, + "timestamp": 5.693387800820727 + }, + { + "x": 2.467368142896788, + "y": 6.633656542567922, + "heading": 0.6562494243147645, + "angularVelocity": 0.8013940316902027, + "velocityX": 1.4452143517289322, + "velocityY": 1.2790698439150068, + "timestamp": 5.767484594544462 + }, + { + "x": 2.556612071509118, + "y": 6.712639660738819, + "heading": 0.7057479102710464, + "angularVelocity": 0.6680246670433977, + "velocityX": 1.2044236211497805, + "velocityY": 1.0659451536510254, + "timestamp": 5.841581388268198 + }, + { + "x": 2.62801383746604, + "y": 6.7758313131963215, + "heading": 0.7453639172984617, + "angularVelocity": 0.5346521089039379, + "velocityX": 0.9636282809096683, + "velocityY": 0.8528257335008133, + "timestamp": 5.915678181991933 + }, + { + "x": 2.681573172842232, + "y": 6.823231783886196, + "heading": 0.7750973380985307, + "angularVelocity": 0.4012781026791483, + "velocityX": 0.7228293247867619, + "velocityY": 0.6397101454430356, + "timestamp": 5.989774975715669 + }, + { + "x": 2.7172898901971134, + "y": 6.854841258399732, + "heading": 0.7949482053115821, + "angularVelocity": 0.2679045369636881, + "velocityX": 0.48202783899191026, + "velocityY": 0.42659706209947756, + "timestamp": 6.063871769439404 + }, + { + "x": 2.735163889126497, + "y": 6.870659826887679, + "heading": 0.8049167147138492, + "angularVelocity": 0.1345336134169838, + "velocityX": 0.24122499815612897, + "velocityY": 0.2134851954178351, + "timestamp": 6.13796856316314 }, { "x": 2.73519515991211, "y": 6.870687484741211, "heading": 0.8050032485420815, - "angularVelocity": 0.0011686685922978992, - "velocityX": 0.00042348526922062564, - "velocityY": 0.00037456143614395893, - "timestamp": 6.21206489180505 - }, - { - "x": 2.71737666825008, - "y": 6.854917835875481, - "heading": 0.7952044521096777, - "angularVelocity": -0.13221640762197384, - "velocityX": -0.2404271762292292, - "velocityY": -0.21278187956754013, - "timestamp": 6.286176695336588 - }, - { - "x": 2.6817084896089662, - "y": 6.823350601971179, - "heading": 0.775519887263024, - "angularVelocity": -0.2656063393700981, - "velocityX": -0.4812752750383339, - "velocityY": -0.4259407056452727, - "timestamp": 6.360288498868125 - }, - { - "x": 2.628190888379515, - "y": 6.7759855045171085, - "heading": 0.7459495915696174, - "angularVelocity": -0.398995764215558, - "velocityX": -0.7221198063894422, - "velocityY": -0.6391032897100266, - "timestamp": 6.434400302399663 - }, - { - "x": 2.5568241932220688, - "y": 6.712822154407591, - "heading": 0.7064940900377142, - "angularVelocity": -0.532378105058251, - "velocityX": -0.9629599030846991, - "velocityY": -0.8522711240272193, - "timestamp": 6.5085121059312 - }, - { - "x": 2.467608789693866, - "y": 6.633860035088962, - "heading": 0.657154460511053, - "angularVelocity": -0.6657459024188197, - "velocityX": -1.2037947974070313, - "velocityY": -1.0654459282687603, - "timestamp": 6.582623909462738 - }, - { - "x": 2.3605451226537615, - "y": 6.539098475645005, - "heading": 0.597932194997477, - "angularVelocity": -0.7990935679285951, - "velocityX": -1.4446236891921902, - "velocityY": -1.2786297852682258, - "timestamp": 6.6567357129942755 - }, - { - "x": 2.2356337183339816, - "y": 6.428536615850716, - "heading": 0.528828452368811, - "angularVelocity": -0.9324255966349123, - "velocityX": -1.685445480458886, - "velocityY": -1.4918252493931579, - "timestamp": 6.730847516525813 - }, - { - "x": 2.092875237629583, - "y": 6.30217337339218, - "heading": 0.4498420161681241, - "angularVelocity": -1.0657740391526649, - "velocityX": -1.9262583541843843, - "velocityY": -1.7050353173174484, - "timestamp": 6.804959320057351 - }, - { - "x": 1.9322705754719112, - "y": 6.16000743597858, - "heading": 0.3609649957464903, - "angularVelocity": -1.1992289512105054, - "velocityX": -2.167059152487097, - "velocityY": -1.9182630921487032, - "timestamp": 6.879071123588888 - }, - { - "x": 1.789533871896463, - "y": 6.033554554877999, - "heading": 0.28090803782331497, - "angularVelocity": -1.0802187250553423, - "velocityX": -1.9259645128083558, - "velocityY": -1.7062448231010843, - "timestamp": 6.953182927120426 - }, - { - "x": 1.6646414904257845, - "y": 5.922906567233019, - "heading": 0.2107799682947157, - "angularVelocity": -0.9462469699444712, - "velocityX": -1.6851888029309197, - "velocityY": -1.4929873836216447, - "timestamp": 7.027294730651963 - }, - { - "x": 1.5575924667954766, - "y": 5.828064283268442, - "heading": 0.15061002754742392, - "angularVelocity": -0.8118806705635195, - "velocityX": -1.4444261039959587, - "velocityY": -1.2797190116759578, - "timestamp": 7.101406534183501 - }, - { - "x": 1.4683859542310973, - "y": 5.7490283016453585, - "heading": 0.10042779042808991, - "angularVelocity": -0.6771153140084615, - "velocityX": -1.203674830696421, - "velocityY": -1.066442561920003, - "timestamp": 7.175518337715038 - }, - { - "x": 1.3970212709065528, - "y": 5.685799049936121, - "heading": 0.06025974538395147, - "angularVelocity": -0.5419925454610606, - "velocityX": -0.9629327573631904, - "velocityY": -0.8531603428758896, - "timestamp": 7.249630141246576 - }, - { - "x": 1.343497933013845, - "y": 5.638376829349959, - "heading": 0.03012622810608221, - "angularVelocity": -0.40659538484416025, - "velocityX": -0.7221972121128929, - "velocityY": -0.639874059577299, - "timestamp": 7.323741944778114 - }, - { - "x": 1.3078156736079332, - "y": 5.606761857880495, - "heading": 0.010039060501896319, - "angularVelocity": -0.2710387097248632, - "velocityX": -0.4814652687860333, - "velocityY": -0.4265848348630388, - "timestamp": 7.397853748309651 + "angularVelocity": 0.001167848484172869, + "velocityX": 0.00042202616390890065, + "velocityY": 0.0003732665361331451, + "timestamp": 6.2120653568868756 + }, + { + "x": 2.717376572124918, + "y": 6.854917750576811, + "heading": 0.7952043981716957, + "angularVelocity": -0.13221717973766972, + "velocityX": -0.24042855386900916, + "velocityY": -0.2127831018556256, + "timestamp": 6.286177135627184 + }, + { + "x": 2.6817083093652894, + "y": 6.823350442039261, + "heading": 0.7755197862739446, + "angularVelocity": -0.26560706317314603, + "velocityX": -0.4812765712264476, + "velocityY": -0.42594185531780887, + "timestamp": 6.360288914367492 + }, + { + "x": 2.6281906360223433, + "y": 6.775985280617999, + "heading": 0.7459494504462719, + "angularVelocity": -0.39899643930135414, + "velocityX": -0.722121021146653, + "velocityY": -0.6391043667597266, + "timestamp": 6.4344006931078 + }, + { + "x": 2.5568238807540884, + "y": 6.712821877208373, + "heading": 0.7064939157358233, + "angularVelocity": -0.5323787308992164, + "velocityX": -0.9629610364410259, + "velocityY": -0.8522721284420127, + "timestamp": 6.508512471848108 + }, + { + "x": 2.4676084291144194, + "y": 6.633859715258306, + "heading": 0.6571542600389383, + "angularVelocity": -0.6657464782996794, + "velocityX": -1.203795849405873, + "velocityY": -1.0654468600295754, + "timestamp": 6.582624250588416 + }, + { + "x": 2.360544725957208, + "y": 6.5390981238541, + "heading": 0.5979319754390301, + "angularVelocity": -0.7990940928219623, + "velocityX": -1.4446246598987866, + "velocityY": -1.2786306443440876, + "timestamp": 6.656736029328724 + }, + { + "x": 2.235633297506525, + "y": 6.4285362427751105, + "heading": 0.528828220928822, + "angularVelocity": -0.9324260689026471, + "velocityX": -1.6854463699809454, + "velocityY": -1.4918260357291568, + "timestamp": 6.730847808069032 + }, + { + "x": 2.092874804641344, + "y": 6.302172989716305, + "heading": 0.44984178028878635, + "angularVelocity": -1.0657744555937454, + "velocityX": -1.9262591627360037, + "velocityY": -1.7050360307986958, + "timestamp": 6.80495958680934 + }, + { + "x": 1.9322701422450308, + "y": 6.160007052413367, + "heading": 0.360964763590885, + "angularVelocity": -1.1992293021239147, + "velocityX": -2.1670598807118275, + "velocityY": -1.9182637324236342, + "timestamp": 6.879071365549648 + }, + { + "x": 1.7895335350373793, + "y": 6.033554256495722, + "heading": 0.28090785571409216, + "angularVelocity": -1.0802184111289128, + "velocityX": -1.9259638566739734, + "velocityY": -1.7062442443966088, + "timestamp": 6.953183144289956 + }, + { + "x": 1.6646412378227142, + "y": 5.922906343422477, + "heading": 0.21077983102217354, + "angularVelocity": -0.9462466814843488, + "velocityX": -1.6851882296914678, + "velocityY": -1.4929868767684242, + "timestamp": 7.027294923030264 + }, + { + "x": 1.5575922863855105, + "y": 5.828064123391515, + "heading": 0.15060992914364188, + "angularVelocity": -0.8118804176778789, + "velocityX": -1.4444256129961441, + "velocityY": -1.279718577033419, + "timestamp": 7.101406701770572 + }, + { + "x": 1.468385833968275, + "y": 5.749028195054374, + "heading": 0.1004277246519939, + "angularVelocity": -0.67711510025268, + "velocityX": -1.2036744217112965, + "velocityY": -1.0664421996142908, + "timestamp": 7.17551848051088 + }, + { + "x": 1.397021198753748, + "y": 5.685798985978408, + "heading": 0.06025970584312968, + "angularVelocity": -0.5419923727591991, + "velocityX": -0.9629324302766086, + "velocityY": -0.853160052972471, + "timestamp": 7.249630259251188 + }, + { + "x": 1.3434978969393505, + "y": 5.638376797369784, + "heading": 0.030126208310740164, + "angularVelocity": -0.4065952544193969, + "velocityX": -0.7221969668538984, + "velocityY": -0.6398738421161722, + "timestamp": 7.323742037991496 + }, + { + "x": 1.307815661583592, + "y": 5.6067618472200556, + "heading": 0.010039053898985358, + "angularVelocity": -0.2710386223779815, + "velocityX": -0.4814651053079036, + "velocityY": -0.4265846898710802, + "timestamp": 7.397853816731804 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -5.584258539451151e-28, - "angularVelocity": -0.1354583213971952, - "velocityX": -0.24073388709378457, - "velocityY": -0.21329334041956155, - "timestamp": 7.471965551841189 + "heading": 1.285773257756444e-22, + "angularVelocity": -0.13545827761283144, + "velocityX": -0.24073380536507133, + "velocityY": -0.21329326791617506, + "timestamp": 7.4719655954721125 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 7.53493083126559e-27, - "velocityX": -3.871179527516382e-30, - "velocityY": -3.125557960458497e-29, - "timestamp": 7.546077355372726 - }, - { - "x": 1.310726436274015, - "y": 5.58769762128994, - "heading": -1.5579970000423894e-18, - "angularVelocity": -2.2453280466561467e-17, - "velocityX": 0.299069985980355, - "velocityY": -0.04693411089639644, - "timestamp": 7.615465746658217 - }, - { - "x": 1.3522304064004054, - "y": 5.581184256432646, - "heading": -2.2539666819865276e-18, - "angularVelocity": -1.003005957705595e-17, - "velocityX": 0.598139967759544, - "velocityY": -0.09386822113363098, - "timestamp": 7.6848541379437085 - }, - { - "x": 1.4144863610944205, - "y": 5.571414209224462, - "heading": -1.1138395195686094e-18, - "angularVelocity": 1.6431093577313864e-17, - "velocityX": 0.8972099444973367, - "velocityY": -0.1408023305798717, - "timestamp": 7.7542425292292 - }, - { - "x": 1.49749429992851, - "y": 5.558387479732467, - "heading": 1.8047013110476196e-18, - "angularVelocity": 4.206093786115289e-17, - "velocityX": 1.1962799150734271, - "velocityY": -0.18773643905934295, - "timestamp": 7.823630920514691 - }, - { - "x": 1.6012542223682356, - "y": 5.5421040680405165, - "heading": 7.934655780307681e-18, - "angularVelocity": 8.834265128972616e-17, - "velocityX": 1.4953498779473966, - "velocityY": -0.23467054633035336, - "timestamp": 7.893019311800182 - }, - { - "x": 1.725766127726464, - "y": 5.522563974256421, - "heading": 1.6517348693937324e-17, - "angularVelocity": 1.236906166709818e-16, - "velocityX": 1.7944198309186505, - "velocityY": -0.28160465204763024, - "timestamp": 7.962407703085673 - }, - { - "x": 1.8710300150870187, - "y": 5.499767198523928, - "heading": 2.797873673898563e-17, - "angularVelocity": 1.6517731335259598e-16, - "velocityX": 2.0934897706863036, - "velocityY": -0.3285387556932655, - "timestamp": 8.031796094371165 - }, - { - "x": 2.0370458831672553, - "y": 5.4737137410442855, - "heading": 4.626535556608686e-17, - "angularVelocity": 2.635400319271784e-16, - "velocityX": 2.3925596919689536, - "velocityY": -0.3754728564386086, - "timestamp": 8.101184485656656 - }, - { - "x": 2.223813730043212, - "y": 5.444403602119362, - "heading": 6.626560991799023e-17, - "angularVelocity": 2.882363175884601e-16, - "velocityX": 2.691629585524184, - "velocityY": -0.42240695283352697, - "timestamp": 8.170572876942147 - }, - { - "x": 2.4313335525083035, - "y": 5.411836782252268, - "heading": 9.137945639838438e-17, - "angularVelocity": 3.6193152862799336e-16, - "velocityX": 2.99069943286728, - "velocityY": -0.46934104197777365, - "timestamp": 8.239961268227638 - }, - { - "x": 2.65960534414942, - "y": 5.376013282449221, - "heading": 1.1821641145167503e-16, - "angularVelocity": 3.8676433675745036e-16, - "velocityX": 3.2897691877869866, - "velocityY": -0.5162751166208155, - "timestamp": 8.30934965951313 - }, - { - "x": 2.9086290857277297, - "y": 5.336933105728788, - "heading": 1.4538386371763974e-16, - "angularVelocity": 3.915273420502118e-16, - "velocityX": 3.5888386654437228, - "velocityY": -0.5632091477613738, - "timestamp": 8.37873805079862 - }, - { - "x": 3.1678798362557536, - "y": 5.296247694105415, - "heading": 1.2970532145326214e-16, - "angularVelocity": -2.259533930819528e-16, - "velocityX": 3.73622656074219, - "velocityY": -0.5863432033749367, - "timestamp": 8.448126442084112 - }, - { - "x": 3.426192624956805, - "y": 5.24997937308727, - "heading": 1.1963113847649017e-16, - "angularVelocity": -1.451854235288838e-16, - "velocityX": 3.7227089995248686, - "velocityY": -0.6668020422577368, - "timestamp": 8.517514833369603 - }, - { - "x": 3.6799776944966163, - "y": 5.18320068927956, - "heading": 1.442058714433213e-16, - "angularVelocity": 3.5416202208408226e-16, - "velocityX": 3.657457174581828, - "velocityY": -0.9623898547086362, - "timestamp": 8.586903224655094 - }, - { - "x": 3.9276087380924305, - "y": 5.096338039673236, - "heading": 1.2678863352315073e-16, - "angularVelocity": -2.510108333324681e-16, - "velocityX": 3.568767613835609, - "velocityY": -1.251832590395956, - "timestamp": 8.656291615940585 + "heading": 2.570159716602358e-22, + "angularVelocity": 4.6040759154316275e-21, + "velocityX": 1.4250190224081253e-19, + "velocityY": 1.259493468984856e-19, + "timestamp": 7.5460773742124205 + }, + { + "x": 1.310734152886531, + "y": 5.587725049797117, + "heading": -4.624423370992563e-19, + "angularVelocity": -6.667715053540946e-18, + "velocityX": 0.2991570561378951, + "velocityY": -0.04653506643871116, + "timestamp": 7.615471364410994 + }, + { + "x": 1.3522535562378788, + "y": 5.581266541953786, + "heading": -1.1759601942239705e-18, + "angularVelocity": -1.0282127531279589e-17, + "velocityX": 0.5983141080738859, + "velocityY": -0.09307013222398468, + "timestamp": 7.684865354609568 + }, + { + "x": 1.4145326607692024, + "y": 5.571578780265876, + "heading": 1.4139400808476166e-18, + "angularVelocity": 3.732167969674453e-17, + "velocityX": 0.8974711549675912, + "velocityY": -0.13960519722513298, + "timestamp": 7.754259344808141 + }, + { + "x": 1.4975714660528414, + "y": 5.55866176479989, + "heading": 7.102195539243054e-18, + "angularVelocity": 8.197043349311479e-17, + "velocityX": 1.196628195698503, + "velocityY": -0.18614026126790603, + "timestamp": 7.823653335006715 + }, + { + "x": 1.6013699715542196, + "y": 5.542515495638963, + "heading": 1.3318887961611829e-17, + "angularVelocity": 8.958545840321423e-17, + "velocityX": 1.4957852287259228, + "velocityY": -0.23267532411270994, + "timestamp": 7.893047325205289 + }, + { + "x": 1.7259281765860246, + "y": 5.523139972889977, + "heading": 1.841155729782659e-17, + "angularVelocity": 7.338775766662924e-17, + "velocityX": 1.7949422518488518, + "velocityY": -0.27921038541726767, + "timestamp": 7.962441315403862 + }, + { + "x": 1.8712460802318407, + "y": 5.500535196695444, + "heading": 2.3529721894456916e-17, + "angularVelocity": 7.37551563468695e-17, + "velocityX": 2.094099261765793, + "velocityY": -0.32574544466816385, + "timestamp": 8.031835305602437 + }, + { + "x": 2.037323681208685, + "y": 5.4747011672548815, + "heading": 2.8673364146250235e-17, + "angularVelocity": 7.412230132714935e-17, + "velocityX": 2.3932562531943504, + "velocityY": -0.37228050104393373, + "timestamp": 8.10122929580101 + }, + { + "x": 2.2241609775920828, + "y": 5.445637884867564, + "heading": 3.384245946066316e-17, + "angularVelocity": 7.448909191732528e-17, + "velocityX": 2.6924132168903303, + "velocityY": -0.4188155531070138, + "timestamp": 8.170623285999584 + }, + { + "x": 2.431757966174578, + "y": 5.413345350032281, + "heading": 3.903697176098764e-17, + "angularVelocity": 7.485536262518973e-17, + "velocityX": 2.9915701343653476, + "velocityY": -0.4653505979822776, + "timestamp": 8.240017276198158 + }, + { + "x": 2.660114640541256, + "y": 5.377823563746616, + "heading": 4.4256799680232527e-17, + "angularVelocity": 7.522017258708922e-17, + "velocityX": 3.29072695939845, + "velocityY": -0.5118856284819102, + "timestamp": 8.309411266396731 + }, + { + "x": 2.909230981447395, + "y": 5.339072529003315, + "heading": 4.9501702917687895e-17, + "angularVelocity": 7.558151970286575e-17, + "velocityX": 3.589883507106047, + "velocityY": -0.5584206158546873, + "timestamp": 8.378805256595305 + }, + { + "x": 3.168557198602556, + "y": 5.29873296059366, + "heading": 6.319291433274611e-17, + "angularVelocity": 1.9729678861063994e-16, + "velocityX": 3.737012620445782, + "velocityY": -0.5813121322786239, + "timestamp": 8.448199246793878 + }, + { + "x": 3.4267765970934825, + "y": 5.251827631481489, + "heading": 8.420269956089665e-17, + "angularVelocity": 3.0276087551661663e-16, + "velocityX": 3.7210628435116764, + "velocityY": -0.6759278285907664, + "timestamp": 8.517593236992452 + }, + { + "x": 3.6804171256627907, + "y": 5.18441954809062, + "heading": 4.866640802350699e-17, + "angularVelocity": -5.120946559749006e-16, + "velocityX": 3.6550791767919044, + "velocityY": -0.9713821499236162, + "timestamp": 8.586987227191026 + }, + { + "x": 3.9278531799158563, + "y": 5.096939458942259, + "heading": 2.4335451025021366e-17, + "angularVelocity": -3.5062052101080536e-16, + "velocityX": 3.5656697870379945, + "velocityY": -1.260629182700587, + "timestamp": 8.6563812173896 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 9.872947837544061e-17, - "angularVelocity": -4.0437823428221097e-16, - "velocityX": 3.457211254829872, - "velocityY": -1.5332571030157462, - "timestamp": 8.725680007226076 - }, - { - "x": 4.284983330328794, - "y": 4.932123965200671, - "heading": 8.520616401533837e-17, - "angularVelocity": -3.905855769726077e-16, - "velocityX": 3.3932258158637794, - "velocityY": -1.6700901615188843, - "timestamp": 8.760303186261309 - }, - { - "x": 4.400064822274762, - "y": 4.86965478591225, - "heading": 8.38275920998652e-17, - "angularVelocity": -3.9816445337684444e-17, - "velocityX": 3.3238280005674867, - "velocityY": -1.8042589106232223, - "timestamp": 8.794926365296542 - }, - { - "x": 4.512559967686789, - "y": 4.802639932227702, - "heading": 8.775922770148411e-17, - "angularVelocity": 1.1355501462237473e-16, - "velocityX": 3.2491281432462653, - "velocityY": -1.935548830347224, - "timestamp": 8.829549544331774 - }, - { - "x": 4.622289335106214, - "y": 4.731186349760053, - "heading": 1.0501758472421099e-16, - "angularVelocity": 4.984625185678289e-16, - "velocityX": 3.169245877386488, - "velocityY": -2.06374990566114, - "timestamp": 8.864172723367007 - }, - { - "x": 4.729077970033436, - "y": 4.65540814642046, - "heading": 1.123300173637084e-16, - "angularVelocity": 2.11200497564256e-16, - "velocityX": 3.0843105082451987, - "velocityY": -2.188655272309947, - "timestamp": 8.89879590240224 - }, - { - "x": 4.83750746703025, - "y": 4.5819970246859665, - "heading": 1.0947895650457139e-16, - "angularVelocity": -8.234543847737888e-17, - "velocityX": 3.131702518895681, - "velocityY": -2.120288309163928, - "timestamp": 8.933419081437473 - }, - { - "x": 4.94878188699609, - "y": 4.512974188419722, - "heading": 1.0086901564782932e-16, - "angularVelocity": -2.486756299293195e-16, - "velocityX": 3.213870680465465, - "velocityY": -1.9935441571094976, - "timestamp": 8.968042260472705 - }, - { - "x": 5.062723794471585, - "y": 4.448449851931572, - "heading": 9.759628790443595e-17, - "angularVelocity": -9.452418393074315e-17, - "velocityX": 3.2909140827174443, - "velocityY": -1.8636167528836842, - "timestamp": 9.002665439507938 - }, - { - "x": 5.179151445124142, - "y": 4.388526983063662, - "heading": 9.598506413460085e-17, - "angularVelocity": -4.6535985854895585e-17, - "velocityX": 3.362708275114807, - "velocityY": -1.7307153917591276, - "timestamp": 9.03728861854317 - }, - { - "x": 5.297879115709132, - "y": 4.333301191879142, - "heading": 9.429439507749846e-17, - "angularVelocity": -4.883055525842942e-17, - "velocityX": 3.429138337186562, - "velocityY": -1.5950525839444993, - "timestamp": 9.071911797578403 - }, - { - "x": 5.418717408418424, - "y": 4.28286058857882, - "heading": 9.044194331783773e-17, - "angularVelocity": -1.112679963830143e-16, - "velocityX": 3.4900981387736523, - "velocityY": -1.4568449433541117, - "timestamp": 9.106534976613636 - }, - { - "x": 5.54147355571485, - "y": 4.237285646363086, - "heading": 8.606851716017984e-17, - "angularVelocity": -1.2631497971943828e-16, - "velocityX": 3.545490354063358, - "velocityY": -1.3163130447772213, - "timestamp": 9.141158155648869 + "heading": 1.2553482249572005e-18, + "angularVelocity": -3.325951243636478e-16, + "velocityX": 3.4534097952511087, + "velocityY": -1.541800129495057, + "timestamp": 8.725775207588173 + }, + { + "x": 4.284849298076782, + "y": 4.931828595221009, + "heading": -1.3377953270770433e-17, + "angularVelocity": -4.226105068324817e-16, + "velocityX": 3.38908081155215, + "velocityY": -1.6784855317357408, + "timestamp": 8.760401184129543 + }, + { + "x": 4.399784866536065, + "y": 4.869069261630718, + "heading": -2.6846630837218187e-17, + "angularVelocity": -3.8897610729556746e-16, + "velocityX": 3.3193451835780268, + "velocityY": -1.8124928120166828, + "timestamp": 8.795027160670914 + }, + { + "x": 4.512122399892567, + "y": 4.801769947661279, + "heading": -3.804014031225808e-17, + "angularVelocity": -3.2326913470628567e-16, + "velocityX": 3.2443137949419496, + "velocityY": -1.9436076810434628, + "timestamp": 8.829653137212285 + }, + { + "x": 4.621682688033729, + "y": 4.730038068451707, + "heading": -4.7868809258190414e-17, + "angularVelocity": -2.838524693725381e-16, + "velocityX": 3.1641068089520323, + "velocityY": -2.0716203952795396, + "timestamp": 8.864279113753655 + }, + { + "x": 4.728291014117835, + "y": 4.653988188924879, + "heading": -5.969081362943227e-17, + "angularVelocity": -3.414200999257287e-16, + "velocityX": 3.0788539915035598, + "velocityY": -2.1963244685955665, + "timestamp": 8.898905090295026 + }, + { + "x": 4.836842403001111, + "y": 4.580738511169008, + "heading": -6.391442093146582e-17, + "angularVelocity": -1.2197799812053293e-16, + "velocityX": 3.134969746010814, + "velocityY": -2.1154544960878003, + "timestamp": 8.933531066836396 + }, + { + "x": 4.948232400240648, + "y": 4.5118822283902675, + "heading": -5.985061600801175e-17, + "angularVelocity": 1.173628971657168e-16, + "velocityX": 3.216948902695972, + "velocityY": -1.9885730210806443, + "timestamp": 8.968157043377767 + }, + { + "x": 5.062283352257089, + "y": 4.447529300627276, + "heading": -5.566760825345324e-17, + "angularVelocity": 1.2080548110971404e-16, + "velocityX": 3.293797414787035, + "velocityY": -1.8585158944500793, + "timestamp": 9.002783019919137 + }, + { + "x": 5.178813309821618, + "y": 4.387782436951599, + "heading": -4.4683720245590447e-17, + "angularVelocity": 3.1721525586676e-16, + "velocityX": 3.36539122370464, + "velocityY": -1.7254925245008748, + "timestamp": 9.037408996460508 + }, + { + "x": 5.297636355743312, + "y": 4.332736981184801, + "heading": -3.4831170249496923e-17, + "angularVelocity": 2.845421553553544e-16, + "velocityX": 3.431615734497117, + "velocityY": -1.5897156200354856, + "timestamp": 9.072034973001879 + }, + { + "x": 5.418562909052227, + "y": 4.282480769562024, + "heading": -3.435849457009512e-17, + "angularVelocity": 1.3650898164143384e-17, + "velocityX": 3.4923651370362165, + "velocityY": -1.4514020005394612, + "timestamp": 9.10666094954325 + }, + { + "x": 5.541400030004997, + "y": 4.2370939937981875, + "heading": -3.7514977433318736e-17, + "angularVelocity": -9.115938894395056e-17, + "velocityX": 3.5475424297711617, + "velocityY": -1.3107724401537064, + "timestamp": 9.14128692608462 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 8.12244185045661e-17, - "angularVelocity": -1.3990912419348855e-16, - "velocityX": 3.5952265671295462, - "velocityY": -1.1736811275270398, - "timestamp": 9.175781334684102 - }, - { - "x": 5.800884886979515, - "y": 4.158897293130728, - "heading": 7.604329686230843e-17, - "angularVelocity": -1.3984796334066921e-16, - "velocityX": 3.642093094613744, - "velocityY": -1.0189897302407367, - "timestamp": 9.212829579924803 - }, - { - "x": 5.9373078989205395, - "y": 4.126945510909642, - "heading": 7.099587364491845e-17, - "angularVelocity": -1.3623919795923125e-16, - "velocityX": 3.682306977150727, - "velocityY": -0.8624371279529385, - "timestamp": 9.249877825165504 - }, - { - "x": 6.074971576496844, - "y": 4.100852086793861, - "heading": 6.753360793468667e-17, - "angularVelocity": -9.345289332160286e-17, - "velocityX": 3.7157948151635507, - "velocityY": -0.7043093119863827, - "timestamp": 9.286926070406205 - }, - { - "x": 6.213624465766887, - "y": 4.080664678033529, - "heading": 6.709732769415932e-17, - "angularVelocity": -1.177600282260163e-17, - "velocityX": 3.742495450708113, - "velocityY": -0.5448951395450716, - "timestamp": 9.323974315646906 - }, - { - "x": 6.353013304056433, - "y": 4.066420153436822, - "heading": 4.721163336657454e-17, - "angularVelocity": -5.367513143575007e-16, - "velocityX": 3.7623600627760427, - "velocityY": -0.38448581043883095, - "timestamp": 9.361022560887607 - }, - { - "x": 6.4925719820191485, - "y": 4.0581614935185435, - "heading": 3.328638124108273e-17, - "angularVelocity": -3.758680611067538e-16, - "velocityX": 3.766944346648758, - "velocityY": -0.22291635851097177, - "timestamp": 9.398070806128308 - }, - { - "x": 6.627754792512696, - "y": 4.053990782696842, - "heading": 2.356760119630702e-17, - "angularVelocity": -2.6232767457636995e-16, - "velocityX": 3.6488316684169346, - "velocityY": -0.1125751245330268, - "timestamp": 9.435119051369009 - }, - { - "x": 6.757419046645761, - "y": 4.0521449831028535, - "heading": 1.6377013350687517e-17, - "angularVelocity": -1.940871361679193e-16, - "velocityX": 3.4998757239551144, - "velocityY": -0.049821511977060404, - "timestamp": 9.47216729660971 - }, - { - "x": 6.881278674219902, - "y": 4.051770935128315, - "heading": 1.0749401519445311e-17, - "angularVelocity": -1.51899550746631e-16, - "velocityX": 3.3431982208450166, - "velocityY": -0.010096239973268065, - "timestamp": 9.509215541850411 - }, - { - "x": 6.999234961376727, - "y": 4.052402049117112, - "heading": 6.6780180524147395e-18, - "angularVelocity": -1.0989409673710028e-16, - "velocityX": 3.183856249883563, - "velocityY": 0.017034922563732322, - "timestamp": 9.546263787091112 - }, - { - "x": 7.111245945109509, - "y": 4.053749520879657, - "heading": 3.4043054479450288e-18, - "angularVelocity": -8.836349922350792e-17, - "velocityX": 3.0233816205072856, - "velocityY": 0.03637073102354394, - "timestamp": 9.583312032331813 + "heading": -3.82351344872691e-17, + "angularVelocity": -2.0798173101011824e-17, + "velocityX": 3.597059527461787, + "velocityY": -1.1680513673144106, + "timestamp": 9.17591290262599 + }, + { + "x": 5.800859276314264, + "y": 4.159128243363963, + "heading": -3.121153141892538e-17, + "angularVelocity": 1.896973231484086e-16, + "velocityX": 3.643657020852143, + "velocityY": -1.013383183783887, + "timestamp": 9.212938217226515 + }, + { + "x": 5.937245993628074, + "y": 4.127402498051012, + "heading": -2.332987946719607e-17, + "angularVelocity": 2.1287197791179265e-16, + "velocityX": 3.6836072504803297, + "velocityY": -0.8568663265997021, + "timestamp": 9.24996353182704 + }, + { + "x": 6.074863067137857, + "y": 4.101529712913805, + "heading": -3.4147305540686986e-17, + "angularVelocity": -2.921629752561486e-16, + "velocityX": 3.7168373853015195, + "velocityY": -0.6987863686333223, + "timestamp": 9.286988846427565 + }, + { + "x": 6.213459439027469, + "y": 4.081557083562315, + "heading": -1.42164023960539e-17, + "angularVelocity": 5.383047614782771e-16, + "velocityX": 3.743286812953797, + "velocityY": -0.5394317257524714, + "timestamp": 9.32401416102809 + }, + { + "x": 6.352782264616147, + "y": 4.067521040771664, + "heading": -1.2952725040414258e-17, + "angularVelocity": 3.413009097467113e-17, + "velocityX": 3.762907272817643, + "velocityY": -0.3790931405199291, + "timestamp": 9.361039475628615 + }, + { + "x": 6.492463118765251, + "y": 4.059455134364433, + "heading": -1.1051006489340503e-17, + "angularVelocity": 5.13626574588434e-17, + "velocityX": 3.772577104506811, + "velocityY": -0.2178484232816345, + "timestamp": 9.39806479022914 + }, + { + "x": 6.627732984606863, + "y": 4.055428289013381, + "heading": -8.35420674945777e-18, + "angularVelocity": 7.283664619836627e-17, + "velocityX": 3.653442713480481, + "velocityY": -0.10875924740947829, + "timestamp": 9.435090104829666 + }, + { + "x": 6.7574379860569564, + "y": 4.05359307719316, + "heading": -5.856926112032921e-18, + "angularVelocity": 6.744792487052549e-17, + "velocityX": 3.503143804435185, + "velocityY": -0.04956640720064634, + "timestamp": 9.47211541943019 + }, + { + "x": 6.881310147885468, + "y": 4.053080371999493, + "heading": -3.474844730669908e-18, + "angularVelocity": 6.433656019193302e-17, + "velocityX": 3.345607273428941, + "velocityY": -0.013847423018504562, + "timestamp": 9.509140734030716 + }, + { + "x": 6.999262823921184, + "y": 4.05342251600114, + "heading": -1.8665913885819445e-18, + "angularVelocity": 4.3436588169438206e-17, + "velocityX": 3.1857305551170585, + "velocityY": 0.009240812815188005, + "timestamp": 9.54616604863124 + }, + { + "x": 7.111261640539779, + "y": 4.0543326919625615, + "heading": -7.554572779591287e-19, + "angularVelocity": 3.001011936481945e-17, + "velocityX": 3.0249254551103046, + "velocityY": 0.024582531471829573, + "timestamp": 9.583191363231766 }, { "x": 7.217291355133057, "y": 4.0556182861328125, - "heading": 0, - "angularVelocity": -9.188843930234661e-17, - "velocityX": 2.862359859004765, - "velocityY": 0.0504413971839862, - "timestamp": 9.620360277572514 - }, - { - "x": 7.386141990779784, - "y": 4.060115199818634, - "heading": -6.285690223777642e-18, - "angularVelocity": -9.593548047553597e-17, - "velocityX": 2.5770864103628295, - "velocityY": 0.06863424057552284, - "timestamp": 9.685880251977624 - }, - { - "x": 7.536265872308939, - "y": 4.064907354049287, - "heading": -1.0202276969724947e-17, - "angularVelocity": -5.97769883515376e-17, - "velocityX": 2.2912689281735665, - "velocityY": 0.07314035565737474, - "timestamp": 9.751400226382733 - }, - { - "x": 7.667662893519374, - "y": 4.069410994150776, - "heading": -1.2426686282698196e-17, - "angularVelocity": -3.395009435762481e-17, - "velocityX": 2.005449825087065, - "velocityY": 0.06873690263742, - "timestamp": 9.816920200787843 - }, - { - "x": 7.780343830854776, - "y": 4.073216939396029, - "heading": -1.3104250722395789e-17, - "angularVelocity": -1.03413416429204e-17, - "velocityX": 1.7197951977010522, - "velocityY": 0.05808832008572811, - "timestamp": 9.882440175192952 - }, - { - "x": 7.874322400829606, - "y": 4.07602285690707, - "heading": -1.2397928827700396e-17, - "angularVelocity": 1.0780252923201902e-17, - "velocityX": 1.4343499189081053, - "velocityY": 0.04282537556703236, - "timestamp": 9.947960149598062 - }, - { - "x": 7.949612474283929, - "y": 4.07759641137373, - "heading": -1.096542742535041e-17, - "angularVelocity": 2.1863583068378234e-17, - "velocityX": 1.1491163441060581, - "velocityY": 0.024016408445632246, - "timestamp": 10.013480124003172 - }, - { - "x": 8.006227098004604, - "y": 4.077753556862197, - "heading": -9.354890651543402e-18, - "angularVelocity": 2.458085169441397e-17, - "velocityX": 0.8640818961654191, - "velocityY": 0.0023984363530761407, - "timestamp": 10.079000098408281 - }, - { - "x": 8.044178200157203, - "y": 4.076344939580228, - "heading": -5.6627659945325885e-18, - "angularVelocity": 5.635113094762732e-17, - "velocityX": 0.579229502104909, - "velocityY": -0.021499051163544333, - "timestamp": 10.14452007281339 + "heading": -4.589044952591168e-28, + "angularVelocity": 2.0403804416247763e-17, + "velocityX": 2.8637086743828277, + "velocityY": 0.034722032320904934, + "timestamp": 9.62021667783229 + }, + { + "x": 7.386460306360812, + "y": 4.05864347722903, + "heading": 4.974033033156732e-19, + "angularVelocity": 7.57885321670836e-18, + "velocityX": 2.5775997849542525, + "velocityY": 0.04609434451454496, + "timestamp": 9.68584709579524 + }, + { + "x": 7.53683861526572, + "y": 4.0619164310557965, + "heading": 2.2229573505485046e-18, + "angularVelocity": 2.6291986266563396e-17, + "velocityX": 2.291289825242349, + "velocityY": 0.04986946492729605, + "timestamp": 9.751477513758187 + }, + { + "x": 7.6684247981223, + "y": 4.0651140489216235, + "heading": 6.3469540781667075e-18, + "angularVelocity": 6.283666714516732e-17, + "velocityX": 2.0049572582467055, + "velocityY": 0.048721583148094445, + "timestamp": 9.817107931721136 + }, + { + "x": 7.781221122380145, + "y": 4.068010125801229, + "heading": 5.162171035103576e-18, + "angularVelocity": -1.8052346447254954e-17, + "velocityX": 1.718659240011596, + "velocityY": 0.04412705220375746, + "timestamp": 9.882738349684084 + }, + { + "x": 7.87523101347, + "y": 4.070437514376607, + "heading": 7.951055539821409e-18, + "angularVelocity": 4.2493779436390584e-17, + "velocityX": 1.432413414507389, + "velocityY": 0.03698572477092609, + "timestamp": 9.948368767647032 + }, + { + "x": 7.950458117127661, + "y": 4.072267695467205, + "heading": 6.860884770186536e-18, + "angularVelocity": -1.661075463106874e-17, + "velocityX": 1.1462231384863357, + "velocityY": 0.027886171494901118, + "timestamp": 10.01399918560998 + }, + { + "x": 8.006905948924626, + "y": 4.073398787704802, + "heading": 2.4339380746534332e-18, + "angularVelocity": -6.745266649855045e-17, + "velocityX": 0.8600864286561777, + "velocityY": 0.01723426838811251, + "timestamp": 10.07962960357293 + }, + { + "x": 8.044577771228491, + "y": 4.0737480502725605, + "heading": 2.9083264970800702e-18, + "angularVelocity": 7.228179214282684e-18, + "velocityX": 0.5739994269902885, + "velocityY": 0.0053216569176217, + "timestamp": 10.145260021535877 }, { "x": 8.0634765625, "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 8.642808619399734e-17, - "velocityX": 0.2945416648589691, - "velocityY": -0.04728304210698771, - "timestamp": 10.2100400472185 - }, - { - "x": 8.06151161206475, - "y": 4.067466722329622, - "heading": 7.499406153547556e-18, - "angularVelocity": 1.0142249804726799e-16, - "velocityX": -0.026574128354115305, - "velocityY": -0.07817228633622807, - "timestamp": 10.283982281901235 - }, - { - "x": 8.0357781795929, - "y": 4.059672922438978, - "heading": 2.0171548175080756e-17, - "angularVelocity": 1.7137894295700475e-16, - "velocityX": -0.34802075677407873, - "velocityY": -0.10540389973451367, - "timestamp": 10.357924516583969 - }, - { - "x": 7.986251299714503, - "y": 4.050185891028673, - "heading": 3.259640252688688e-17, - "angularVelocity": 1.6803460705902343e-16, - "velocityX": -0.6698050186189494, - "velocityY": -0.12830328229881344, - "timestamp": 10.431866751266703 - }, - { - "x": 7.9129066919285, - "y": 4.039390810897977, - "heading": 4.5982022945133473e-17, - "angularVelocity": 1.81028075140569e-16, - "velocityX": -0.9919176516736913, - "velocityY": -0.1459934255032282, - "timestamp": 10.505808985949438 - }, - { - "x": 7.8157231380567715, - "y": 4.027759201442648, - "heading": 5.916672321533067e-17, - "angularVelocity": 1.783108168799044e-16, - "velocityX": -1.314317240866671, - "velocityY": -0.1573067071239705, - "timestamp": 10.579751220632172 - }, - { - "x": 7.694687242510101, - "y": 4.015880811506709, - "heading": 7.241021515042493e-17, - "angularVelocity": 1.7910591950468916e-16, - "velocityX": -1.6368979929535623, - "velocityY": -0.16064418375919837, - "timestamp": 10.653693455314906 - }, - { - "x": 7.549803190746082, - "y": 4.0045128268534045, - "heading": 8.516404983291186e-17, - "angularVelocity": 1.7248376026751216e-16, - "velocityX": -1.9594221406165937, - "velocityY": -0.1537414266972227, - "timestamp": 10.72763568999764 - }, - { - "x": 7.381113647590609, - "y": 3.9946593063008904, - "heading": 9.839417244429125e-17, - "angularVelocity": 1.7892511182976314e-16, - "velocityX": -2.281369286163345, - "velocityY": -0.13325970732143547, - "timestamp": 10.801577924680375 - }, - { - "x": 7.188747477257485, - "y": 3.987706179482555, - "heading": 1.110446332225851e-16, - "angularVelocity": 1.7108572429643452e-16, - "velocityX": -2.6015736629885633, - "velocityY": -0.09403457777777122, - "timestamp": 10.87552015936311 - }, - { - "x": 6.973038504473885, - "y": 3.9856630874408094, - "heading": 1.1423152127106693e-16, - "angularVelocity": 4.309969887337601e-17, - "velocityX": -2.91726337064529, - "velocityY": -0.027630920954873416, - "timestamp": 10.949462394045844 - }, - { - "x": 6.734853840508573, - "y": 3.9916098664093003, - "heading": 1.1357543736188786e-16, - "angularVelocity": -8.872925078647214e-18, - "velocityX": -3.221226204310621, - "velocityY": 0.0804246584378634, - "timestamp": 11.023404628728578 - }, - { - "x": 6.47658832080339, - "y": 4.010431184705039, - "heading": 1.1486840283254658e-16, - "angularVelocity": 1.7486156277295078e-17, - "velocityX": -3.4928011144554563, - "velocityY": 0.25454083686402557, - "timestamp": 11.097346863411312 - }, - { - "x": 6.204860010324235, - "y": 4.048943809403862, - "heading": 1.1487825258491134e-16, - "angularVelocity": 1.3320874674868913e-19, - "velocityX": -3.6748728469603784, - "velocityY": 0.5208474542873691, - "timestamp": 11.171289098094046 - }, - { - "x": 5.932253242931649, - "y": 4.111293853023339, - "heading": 1.0773394940295777e-16, - "angularVelocity": -9.662006041077537e-17, - "velocityX": -3.686753160250877, - "velocityY": 0.8432263899921877, - "timestamp": 11.24523133277678 + "heading": 9.97684240054841e-28, + "angularVelocity": -4.4313697609032606e-17, + "velocityX": 0.2879578076461169, + "velocityY": -0.007635093856342742, + "timestamp": 10.210890439498826 + }, + { + "x": 8.061114768714358, + "y": 4.071548552312887, + "heading": -6.576009001115218e-18, + "angularVelocity": -8.952107660855974e-17, + "velocityX": -0.032151769003766255, + "velocityY": -0.023120849595889583, + "timestamp": 10.284348101668675 + }, + { + "x": 8.03523325714352, + "y": 4.068826117392824, + "heading": -1.7302069231102504e-17, + "angularVelocity": -1.4601690162241075e-16, + "velocityX": -0.3523323613402589, + "velocityY": -0.037061279104803016, + "timestamp": 10.357805763838524 + }, + { + "x": 7.985826542335116, + "y": 4.0652145468753025, + "heading": -2.1436247637285606e-17, + "angularVelocity": -5.627974379451221e-17, + "velocityX": -0.6725876286964619, + "velocityY": -0.04916533430060717, + "timestamp": 10.431263426008373 + }, + { + "x": 7.912889031757725, + "y": 4.060876766115487, + "heading": -3.018602234671886e-17, + "angularVelocity": -1.1911316602591952e-16, + "velocityX": -0.9929190287698493, + "velocityY": -0.05905144040367088, + "timestamp": 10.504721088178222 + }, + { + "x": 7.816415389029205, + "y": 4.056013446336911, + "heading": -4.386893441840666e-17, + "angularVelocity": -1.8626936480131915e-16, + "velocityX": -1.3133230745276494, + "velocityY": -0.06620575219684201, + "timestamp": 10.578178750348071 + }, + { + "x": 7.696401323527335, + "y": 4.050877776360081, + "heading": -5.3011403841257886e-17, + "angularVelocity": -1.244590305935936e-16, + "velocityX": -1.6337855297433015, + "velocityY": -0.06991333272974812, + "timestamp": 10.65163641251792 + }, + { + "x": 7.552845329490783, + "y": 4.04579904746213, + "heading": -5.8407144486754e-17, + "angularVelocity": -7.345374854837963e-17, + "velocityX": -1.9542684833152193, + "velocityY": -0.06913817766494233, + "timestamp": 10.72509407468777 + }, + { + "x": 7.385752700395874, + "y": 4.04122271365356, + "heading": -6.122941827410045e-17, + "angularVelocity": -3.8420413942670663e-17, + "velocityX": -2.2746793752918006, + "velocityY": -0.062298930749920316, + "timestamp": 10.798551736857618 + }, + { + "x": 7.195145586336795, + "y": 4.037784147568392, + "heading": -6.335567714222456e-17, + "angularVelocity": -2.894536534102561e-17, + "velocityX": -2.5947887317507625, + "velocityY": -0.046810175869971896, + "timestamp": 10.872009399027467 + }, + { + "x": 6.981091630995715, + "y": 4.036460065314617, + "heading": -6.194065422525818e-17, + "angularVelocity": 1.926310849935096e-17, + "velocityX": -2.9139772355692886, + "velocityY": -0.01802510745187768, + "timestamp": 10.945467061197316 + }, + { + "x": 6.743803602394158, + "y": 4.0389311192423705, + "heading": -6.317374994659029e-17, + "angularVelocity": -1.6786481973259e-17, + "velocityX": -3.230269267934209, + "velocityY": 0.03363915832279882, + "timestamp": 11.018924723367165 + }, + { + "x": 6.484122272478982, + "y": 4.04866611767419, + "heading": -6.348618330459737e-17, + "angularVelocity": -4.253243967450325e-18, + "velocityX": -3.535115633202991, + "velocityY": 0.13252529612649763, + "timestamp": 11.092382385537014 + }, + { + "x": 6.20753972881787, + "y": 4.074789307976153, + "heading": -6.404970424521136e-17, + "angularVelocity": -7.671370479922628e-18, + "velocityX": -3.76519665193805, + "velocityY": 0.35562240248758487, + "timestamp": 11.165840047706864 + }, + { + "x": 5.9341618042346855, + "y": 4.124235317195696, + "heading": -6.716116670118769e-17, + "angularVelocity": -4.235722135085759e-17, + "velocityX": -3.7215712630641393, + "velocityY": 0.6731225546657573, + "timestamp": 11.239297709876713 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 9.686455716091523e-17, - "angularVelocity": -1.4699842774133078e-16, - "velocityX": -3.6014804699029255, - "velocityY": 1.1543500395591086, - "timestamp": 11.319173567459515 - }, - { - "x": 5.543077137245883, - "y": 4.241512673300566, - "heading": 9.161999350902719e-17, - "angularVelocity": -1.5163138697871951e-16, - "velocityX": -3.552563374503243, - "velocityY": 1.2971011802444727, - "timestamp": 11.353761153554894 - }, - { - "x": 5.422090048254055, - "y": 4.291242278322373, - "heading": 8.675864551293739e-17, - "angularVelocity": -1.4055181482408883e-16, - "velocityX": -3.4979916973158245, - "velocityY": 1.4377876757479933, - "timestamp": 11.388348739650272 - }, - { - "x": 5.303183074500814, - "y": 4.34575871765036, - "heading": 8.202370382364629e-17, - "angularVelocity": -1.368971421201274e-16, - "velocityX": -3.437851182367712, - "velocityY": 1.5761851427750155, - "timestamp": 11.42293632574565 - }, - { - "x": 5.186545516053187, - "y": 4.404975197881964, - "heading": 7.882281033080331e-17, - "angularVelocity": -9.254457608045447e-17, - "velocityX": -3.372237603572297, - "velocityY": 1.712073229635336, - "timestamp": 11.45752391184103 - }, - { - "x": 5.072363058490256, - "y": 4.468797441531519, - "heading": 7.597641724828609e-17, - "angularVelocity": -8.229522218370685e-17, - "velocityX": -3.3012554633926476, - "velocityY": 1.845235555715285, - "timestamp": 11.492111497936408 - }, - { - "x": 4.960817475466504, - "y": 4.537123834649348, - "heading": 7.51892087087232e-17, - "angularVelocity": -2.2759857753359948e-17, - "velocityX": -3.225017863812662, - "velocityY": 1.975460008380286, - "timestamp": 11.526699084031787 - }, - { - "x": 4.852086330068725, - "y": 4.609845576159071, - "heading": 7.00186010980759e-17, - "angularVelocity": -1.4949316197981958e-16, - "velocityX": -3.143646541216975, - "velocityY": 2.1025387926519685, - "timestamp": 11.561286670127165 - }, - { - "x": 4.744615410553515, - "y": 4.6844171206048495, - "heading": 6.888474965661026e-17, - "angularVelocity": -3.278203452357036e-17, - "velocityX": -3.10721075529382, - "velocityY": 2.1560204935996916, - "timestamp": 11.595874256222544 - }, - { - "x": 4.634255259438232, - "y": 4.75464211813236, - "heading": 6.027771904333573e-17, - "angularVelocity": -2.4884739251649313e-16, - "velocityX": -3.190744529293135, - "velocityY": 2.0303526627692405, - "timestamp": 11.630461842317922 - }, - { - "x": 4.521181551584758, - "y": 4.820408744704174, - "heading": 5.1990889261648227e-17, - "angularVelocity": -2.395897117201502e-16, - "velocityX": -3.269199172838061, - "velocityY": 1.9014517633713166, - "timestamp": 11.665049428413301 - }, - { - "x": 4.405574295139685, - "y": 4.881612290783718, - "heading": 4.5533169228459885e-17, - "angularVelocity": -1.867062944312063e-16, - "velocityX": -3.3424494015360238, - "velocityY": 1.7695234906179804, - "timestamp": 11.69963701450868 - }, - { - "x": 4.287617535211261, - "y": 4.938155315408326, - "heading": 4.203003409755996e-17, - "angularVelocity": -1.0128301874666137e-16, - "velocityX": -3.410378498318693, - "velocityY": 1.6347779942979137, - "timestamp": 11.734224600604058 + "heading": -6.707406307437602e-17, + "angularVelocity": 1.1857663806024648e-18, + "velocityX": -3.651219865855907, + "velocityY": 0.9857890275804273, + "timestamp": 11.312755372046562 + }, + { + "x": 5.540922192391165, + "y": 4.235828574357528, + "heading": -6.659781323672848e-17, + "angularVelocity": 1.3746706587182516e-17, + "velocityX": -3.608913259332814, + "velocityY": 1.130896109600127, + "timestamp": 11.347400021957807 + }, + { + "x": 5.417558030914266, + "y": 4.279972683638325, + "heading": -6.705658889375764e-17, + "angularVelocity": -1.3242323379839645e-17, + "velocityX": -3.560843067917901, + "velocityY": 1.2741970085969618, + "timestamp": 11.382044671869052 + }, + { + "x": 5.296056287935261, + "y": 4.329010889435292, + "heading": -6.943855501790622e-17, + "angularVelocity": -6.875422700655523e-17, + "velocityX": -3.507085315922588, + "velocityY": 1.4154625872276303, + "timestamp": 11.416689321780296 + }, + { + "x": 5.176611031188361, + "y": 4.382864860494658, + "heading": -7.367311041415583e-17, + "angularVelocity": -1.2222826345940313e-16, + "velocityX": -3.4477258985991424, + "velocityY": 1.55446717450842, + "timestamp": 11.451333971691541 + }, + { + "x": 5.059413042015962, + "y": 4.441448571672919, + "heading": -7.769450380917627e-17, + "angularVelocity": -1.160754519341825e-16, + "velocityX": -3.3828596759570035, + "velocityY": 1.6909886902694797, + "timestamp": 11.485978621602786 + }, + { + "x": 4.94464950941122, + "y": 4.504668439881924, + "heading": -7.988620676495281e-17, + "angularVelocity": -6.326237851534012e-17, + "velocityX": -3.312590339309282, + "velocityY": 1.8248089783261385, + "timestamp": 11.520623271514031 + }, + { + "x": 4.8325037281342125, + "y": 4.572423470089542, + "heading": -7.896813531381333e-17, + "angularVelocity": 2.6499660209021626e-17, + "velocityX": -3.237030293690655, + "velocityY": 1.9557140967276965, + "timestamp": 11.555267921425276 + }, + { + "x": 4.723154797252288, + "y": 4.644605406017416, + "heading": -6.685297337107051e-17, + "angularVelocity": 3.496979179591587e-16, + "velocityX": -3.1563006456137206, + "velocityY": 2.083494453336746, + "timestamp": 11.589912571336521 + }, + { + "x": 4.616777290879426, + "y": 4.721098849146534, + "heading": -4.689132664597989e-17, + "angularVelocity": 5.761826653182846e-16, + "velocityX": -3.070532005530056, + "velocityY": 2.207943891050558, + "timestamp": 11.624557221247766 + }, + { + "x": 4.50857855839689, + "y": 4.794993740354878, + "heading": -5.0771067360923566e-17, + "angularVelocity": -1.1198672017249538e-16, + "velocityX": -3.1231007604269903, + "velocityY": 2.1329380264384965, + "timestamp": 11.659201871159011 + }, + { + "x": 4.397513666551277, + "y": 4.8645064031640235, + "heading": -6.128696485502522e-17, + "angularVelocity": -3.035359722499063e-16, + "velocityX": -3.2058309762155544, + "velocityY": 2.006447257721705, + "timestamp": 11.693846521070256 + }, + { + "x": 4.283759944670474, + "y": 4.929525716016982, + "heading": -7.10068611169889e-17, + "angularVelocity": -2.8055980610512555e-16, + "velocityX": -3.2834426721651013, + "velocityY": 1.8767490224184165, + "timestamp": 11.728491170981501 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 4.0833427337575637e-17, - "angularVelocity": -3.459642302543387e-17, - "velocityX": -3.4728780863994273, - "velocityY": 1.4974297517257, - "timestamp": 11.768812186699437 - }, - { - "x": 3.9262086603019655, - "y": 5.072405406382895, - "heading": 4.69185026085022e-17, - "angularVelocity": 9.025220132506432e-17, - "velocityX": -3.5787544523378267, - "velocityY": 1.2229891223429616, - "timestamp": 11.83623519889844 - }, - { - "x": 3.6792395049411635, - "y": 5.135860556463708, - "heading": 5.846977032472703e-17, - "angularVelocity": 1.713252988776416e-16, - "velocityX": -3.6629801503359496, - "velocityY": 0.9411497352494828, - "timestamp": 11.903658211097442 - }, - { - "x": 3.4280856473907875, - "y": 5.179929383329658, - "heading": 6.25257434115025e-17, - "angularVelocity": 6.01571029606942e-17, - "velocityX": -3.7250465287591408, - "velocityY": 0.6536169985387049, - "timestamp": 11.971081223296444 - }, - { - "x": 3.174266449354605, - "y": 5.204345315411431, - "heading": 8.981508430066906e-17, - "angularVelocity": 4.0474817127156825e-16, - "velocityX": -3.7645781426529163, - "velocityY": 0.3621305439411197, - "timestamp": 12.038504235495447 - }, - { - "x": 2.919317391668449, - "y": 5.208960723831647, - "heading": 3.637405660157601e-17, - "angularVelocity": -7.926229629337798e-16, - "velocityX": -3.781335917381794, - "velocityY": 0.06845449750291784, - "timestamp": 12.10592724769445 - }, - { - "x": 2.6778199582724502, - "y": 5.213921183762201, - "heading": 2.780922480909488e-17, - "angularVelocity": -1.270312837495845e-16, - "velocityX": -3.581825040435896, - "velocityY": 0.07357220878699586, - "timestamp": 12.173350259893452 - }, - { - "x": 2.4555441151084385, - "y": 5.223767512233129, - "heading": 2.258282570608997e-17, - "angularVelocity": -7.751654714759439e-17, - "velocityX": -3.2967355790624753, - "velocityY": 0.14603809811799462, - "timestamp": 12.240773272092454 - }, - { - "x": 2.2524898779476, - "y": 5.238499719208596, - "heading": 1.8179159660483545e-17, - "angularVelocity": -6.531399151152503e-17, - "velocityX": -3.0116458837750555, - "velocityY": 0.2185041352347682, - "timestamp": 12.308196284291457 - }, - { - "x": 2.068657252210669, - "y": 5.258117807366149, - "heading": 1.4559970052520395e-17, - "angularVelocity": -5.367884777868486e-17, - "velocityX": -2.726556108088765, - "velocityY": 0.29097021206423906, - "timestamp": 12.37561929649046 - }, - { - "x": 1.9040462406637386, - "y": 5.282621777825342, - "heading": 1.0447672435760193e-17, - "angularVelocity": -6.099249326267761e-17, - "velocityX": -2.441466291376519, - "velocityY": 0.3634363054985942, - "timestamp": 12.443042308689462 - }, - { - "x": 1.7586568449915738, - "y": 5.312011631159126, - "heading": 6.39006439635328e-18, - "angularVelocity": -6.018135214654068e-17, - "velocityX": -2.156376449676296, - "velocityY": 0.4359024074308457, - "timestamp": 12.510465320888464 - }, - { - "x": 1.6324890663305314, - "y": 5.34628736769762, - "heading": 2.9527164791572072e-18, - "angularVelocity": -5.0981820681006015e-17, - "velocityX": -1.8712865911219376, - "velocityY": 0.508368514259319, - "timestamp": 12.577888333087467 - }, - { - "x": 1.5255429054998768, - "y": 5.385448987646794, - "heading": 6.750997693764207e-19, - "angularVelocity": -3.378099903552108e-17, - "velocityX": -1.5861967204164489, - "velocityY": 0.5808346241426653, - "timestamp": 12.64531134528647 - }, - { - "x": 1.437818363118664, - "y": 5.4294964911430075, - "heading": 3.7918321325869045e-19, - "angularVelocity": -4.388954848629936e-18, - "velocityX": -1.301106840529317, - "velocityY": 0.6533007360484864, - "timestamp": 12.712734357485472 - }, - { - "x": 1.3693154396712461, - "y": 5.478429878281045, - "heading": 1.633332658934955e-18, - "angularVelocity": 1.8601207579968088e-17, - "velocityX": -1.0160169534583994, - "velocityY": 0.7257668493601166, - "timestamp": 12.780157369684474 - }, - { - "x": 1.3200341355468752, - "y": 5.532249149129769, - "heading": 3.8843995459700536e-19, - "angularVelocity": -1.8463914088315652e-17, - "velocityX": -0.7309270606141895, - "velocityY": 0.798232963693069, - "timestamp": 12.847580381883477 + "heading": -7.867131264216922e-17, + "angularVelocity": -2.2123045100748588e-16, + "velocityX": -3.355810480670139, + "velocityY": 1.74405225643013, + "timestamp": 11.763135820892746 + }, + { + "x": 3.910732016936242, + "y": 5.096825920979702, + "heading": -8.738685068125367e-17, + "angularVelocity": -1.1851522353036446e-16, + "velocityX": -3.4915577224647327, + "velocityY": 1.453345144287359, + "timestamp": 11.83667521759551 + }, + { + "x": 3.6458301860416573, + "y": 5.181556502689957, + "heading": -6.150568630518277e-17, + "angularVelocity": 3.51936044311529e-16, + "velocityX": -3.6021757421438494, + "velocityY": 1.1521794508693837, + "timestamp": 11.910214614298273 + }, + { + "x": 3.3747001133794594, + "y": 5.243530170834085, + "heading": -3.504475258762766e-17, + "angularVelocity": 3.598198367759973e-16, + "velocityX": -3.68686832934017, + "velocityY": 0.8427274484534926, + "timestamp": 11.983754011001036 + }, + { + "x": 3.1003610694822004, + "y": 5.289250417175462, + "heading": -4.352123055745274e-17, + "angularVelocity": -1.1526444802732673e-16, + "velocityX": -3.7305044125682025, + "velocityY": 0.621710924909727, + "timestamp": 12.0572934077038 + }, + { + "x": 2.826021831554975, + "y": 5.334969499245887, + "heading": -7.949559303016089e-17, + "angularVelocity": -4.891849006802281e-16, + "velocityX": -3.7305070510174576, + "velocityY": 0.6216950929746646, + "timestamp": 12.130832804406563 + }, + { + "x": 2.5700139771360067, + "y": 5.3776336262742275, + "heading": -7.076803890272921e-17, + "angularVelocity": 1.186786201586086e-16, + "velocityX": -3.481234085366803, + "velocityY": 0.5801533455704441, + "timestamp": 12.204372201109326 + }, + { + "x": 2.337279538147283, + "y": 5.41641920061303, + "heading": -5.893119866883478e-17, + "angularVelocity": 1.6095916971261216e-16, + "velocityX": -3.1647586113523465, + "velocityY": 0.5274121909861365, + "timestamp": 12.27791159781209 + }, + { + "x": 2.12781853242598, + "y": 5.451326219289701, + "heading": -5.026414725549793e-17, + "angularVelocity": 1.178558949690138e-16, + "velocityX": -2.8482828947852163, + "velocityY": 0.4746709959800321, + "timestamp": 12.351450994514853 + }, + { + "x": 1.9416309659178494, + "y": 5.482354681313371, + "heading": -4.204375061179442e-17, + "angularVelocity": 1.1178221487834326e-16, + "velocityX": -2.531807097366853, + "velocityY": 0.42192978749993854, + "timestamp": 12.424990391217616 + }, + { + "x": 1.7787168415957708, + "y": 5.509504586188606, + "heading": -3.165703866093114e-17, + "angularVelocity": 1.4124010281718074e-16, + "velocityX": -2.2153312595228143, + "velocityY": 0.36918857228284047, + "timestamp": 12.49852978792038 + }, + { + "x": 1.6390761612434732, + "y": 5.532775933618142, + "heading": -2.0086106192649845e-17, + "angularVelocity": 1.5734331511581504e-16, + "velocityX": -1.8988553974233522, + "velocityY": 0.3164473530235367, + "timestamp": 12.572069184623142 + }, + { + "x": 1.5227089260501099, + "y": 5.552168723403807, + "heading": -1.374168138388034e-17, + "angularVelocity": 8.627246202414872e-17, + "velocityX": -1.5823795191536005, + "velocityY": 0.2637061310694281, + "timestamp": 12.645608581325906 + }, + { + "x": 1.4296151368650767, + "y": 5.567682955404048, + "heading": -6.859284014118712e-18, + "angularVelocity": 9.358789545320571e-17, + "velocityX": -1.2659036293336394, + "velocityY": 0.21096490719045813, + "timestamp": 12.71914797802867 + }, + { + "x": 1.35979479432542, + "y": 5.579318629512699, + "heading": -3.246170104472464e-18, + "angularVelocity": 4.913167733478094e-17, + "velocityX": -0.949427730851019, + "velocityY": 0.15822368186784203, + "timestamp": 12.792687374731432 + }, + { + "x": 1.3132478989266207, + "y": 5.587075745647187, + "heading": -2.3462842211621804e-18, + "angularVelocity": 1.223678632415003e-17, + "velocityX": -0.6329518256307737, + "velocityY": 0.10548245542238979, + "timestamp": 12.866226771434196 }, { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -5.761237033299429e-18, - "velocityX": -0.4458371630310619, - "velocityY": 0.8706990788013996, - "timestamp": 12.915003394082479 - }, - { - "x": 1.2868335136673428, - "y": 5.68644048793425, - "heading": -1.3391788419363526e-18, - "angularVelocity": -1.3686362473728894e-17, - "velocityX": -0.03210027395244838, - "velocityY": 0.9758655723245745, - "timestamp": 13.012851076079642 - }, - { - "x": 1.3242854311298142, - "y": 5.791775499822582, - "heading": 2.339658407779995e-18, - "angularVelocity": 3.759759224271494e-17, - "velocityX": 0.3827573295354856, - "velocityY": 1.076520258204863, - "timestamp": 13.110698758076804 - }, - { - "x": 1.4024881161679177, - "y": 5.906284066156856, - "heading": 5.091444284753208e-18, - "angularVelocity": 2.812315857202659e-17, - "velocityX": 0.7992287956333111, - "velocityY": 1.1702736743175466, - "timestamp": 13.208546440073967 - }, - { - "x": 1.5216854983005004, - "y": 6.028805982329465, - "heading": 6.6702155316476646e-18, - "angularVelocity": 1.6134988737668432e-17, - "velocityX": 1.218193213162059, - "velocityY": 1.2521698385881244, - "timestamp": 13.30639412207113 - }, - { - "x": 1.682281091295048, - "y": 6.156892671597042, - "heading": 8.737231811568005e-18, - "angularVelocity": 2.1124836460875052e-17, - "velocityX": 1.6412815277443633, - "velocityY": 1.3090416313724325, - "timestamp": 13.404241804068292 - }, - { - "x": 1.884561264278339, - "y": 6.2823011062588545, - "heading": 4.474450456421642e-18, - "angularVelocity": -4.3565481249463154e-17, - "velocityX": 2.06729652511499, - "velocityY": 1.2816699599020622, - "timestamp": 13.502089486065454 - }, - { - "x": 2.074597433027926, - "y": 6.367773862583125, - "heading": 1.3148438373880832e-18, - "angularVelocity": -3.229107274750916e-17, - "velocityX": 1.9421632160391744, - "velocityY": 0.8735286782445029, - "timestamp": 13.599937168062617 - }, - { - "x": 2.2289686427209534, - "y": 6.431502913374048, - "heading": 4.376002899474807e-18, - "angularVelocity": 3.128494207400489e-17, - "velocityX": 1.5776685409625097, - "velocityY": 0.6513087432441247, - "timestamp": 13.69778485005978 - }, - { - "x": 2.345509714236833, - "y": 6.4775215317101145, - "heading": 6.666442968036081e-18, - "angularVelocity": 2.3408220019295113e-17, - "velocityX": 1.1910458085175606, - "velocityY": 0.4703087226675544, - "timestamp": 13.795632532056942 - }, - { - "x": 2.4235057296596247, - "y": 6.507444880215651, - "heading": 4.695809387720839e-18, - "angularVelocity": -2.013980853353183e-17, - "velocityX": 0.7971166391560869, - "velocityY": 0.30581560947355857, - "timestamp": 13.893480214054104 - }, - { - "x": 2.462606906890869, - "y": 6.522137641906738, - "heading": 1.18409559006069e-37, - "angularVelocity": -4.799101310235197e-17, - "velocityX": 0.3996127085808573, - "velocityY": 0.15015952745322653, - "timestamp": 13.991327896051267 - }, - { - "x": 2.462606906890869, - "y": 6.522137641906738, - "heading": 7.445601745662792e-38, - "angularVelocity": -4.492041573884033e-37, - "velocityX": -6.88570869510922e-34, - "velocityY": -3.3118407683240263e-34, - "timestamp": 14.08917557804843 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "heading": 1.1322625168775542e-27, + "angularVelocity": 3.1905132868116714e-17, + "velocityX": -0.31647591502042816, + "velocityY": 0.05274122807866861, + "timestamp": 12.93976616813696 }, { - "scope": [ - "last" - ], - "type": "StopPoint" - }, + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 5.483370217924037e-28, + "angularVelocity": -4.839375204095192e-28, + "velocityX": -1.0823834640575863e-26, + "velocityY": -1.7334223684022012e-26, + "timestamp": 13.013305564839722 + } + ], + "trajectoryWaypoints": [ { - "scope": [ - 2 - ], - "type": "StopPoint" + "timestamp": 0, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "scope": [ - 4 - ], - "type": "StopPoint" + "timestamp": 1.2454468554815399, + "isStopPoint": false, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "scope": [ - 6 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "SourceLanePSubHSubGSub": { - "waypoints": [ - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, + "timestamp": 2.6375549263375944, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 24 + "controlIntervalCount": 12 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, + "timestamp": 3.7578330529746427, + "isStopPoint": false, + "x": 2.6583051681518555, + "y": 5.54433536529541, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 21 + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 7.340087890625, - "y": 0.7580231428146362, + "timestamp": 4.878323069859635, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 18 }, { - "x": 7.841280937194824, - "y": 0.7580231428146362, - "heading": 0, + "timestamp": 6.2120653568868756, + "isStopPoint": false, + "x": 2.7351951599121094, + "y": 6.870687484741211, + "heading": 0.8050032485420815, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 23 + "controlIntervalCount": 18 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, + "timestamp": 7.5460773742124205, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 24 + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, + "timestamp": 8.725775207588173, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 24 + "headingConstrained": false, + "controlIntervalCount": 13 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, + "timestamp": 9.17591290262599, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 23 + "controlIntervalCount": 12 }, { - "x": 7.481724262237549, - "y": 2.2045881748199463, - "heading": 0.23374329695765314, + "timestamp": 9.62021667783229, + "isStopPoint": false, + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 9 }, { - "x": 7.975332260131836, - "y": 2.3279902935028076, - "heading": 0.23374329695765314, + "timestamp": 10.210890439498826, + "isStopPoint": false, + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 24 + "controlIntervalCount": 15 }, { - "x": 2.757753372192383, - "y": 1.617210865020752, + "timestamp": 11.312755372046562, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 24 + "controlIntervalCount": 13 }, { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "isInitialGuess": false, + "timestamp": 11.763135820892746, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 + }, + { + "timestamp": 13.013305564839722, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 2 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 4 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 6 + ], + "type": "StopPoint", + "direction": 0 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "SourceLanePSubHSubGSub": { + "waypoints": [ + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 21 + }, + { + "x": 7.340087890625, + "y": 0.7580231428146362, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 7.841280937194824, + "y": 0.7580231428146362, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 23 + }, + { + "x": 7.481724262237549, + "y": 2.2045881748199463, + "heading": 0.23374329695765314, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 7.975332260131836, + "y": 2.3279902935028076, + "heading": 0.23374329695765314, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 40 @@ -7317,1928 +6860,2056 @@ "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": -1.7128958192175968e-32, - "velocityY": -9.692960229070848e-33, + "angularVelocity": -7.591443414914956e-27, + "velocityX": -1.3210318643006072e-26, + "velocityY": -4.725695657383892e-27, "timestamp": 0 }, { - "x": 0.6705320996855912, - "y": 4.398099342118358, - "heading": -1.0386114127976835, - "angularVelocity": 0.15927551181553445, - "velocityX": 0.1260500242755718, - "velocityY": -0.20514712032687513, - "timestamp": 0.05592000838348859 - }, - { - "x": 0.6846335258864488, - "y": 4.375148415401169, - "heading": -1.0210111049652701, - "angularVelocity": 0.31474079388031545, - "velocityX": 0.25217138924859694, - "velocityY": -0.41042423598715305, - "timestamp": 0.11184001676697718 - }, - { - "x": 0.7057920861320249, - "y": 4.340710150429397, - "heading": -0.99496347376924, - "angularVelocity": 0.4658016325283906, - "velocityX": 0.3783719076090773, - "velocityY": -0.6158487090273924, - "timestamp": 0.16776002515046576 - }, - { - "x": 0.7340126708459819, - "y": 4.29477510174039, - "heading": -0.9607567098648512, - "angularVelocity": 0.6117088479280167, - "velocityX": 0.5046598798845959, - "velocityY": -0.8214420923185946, - "timestamp": 0.22368003353395435 - }, - { - "x": 0.7693006604451206, - "y": 4.237332365746024, - "heading": -0.918731573784226, - "angularVelocity": 0.7515223494321578, - "velocityX": 0.6310440684690269, - "velocityY": -1.0272304610620717, - "timestamp": 0.2796000419174429 - }, - { - "x": 0.8116619611973517, - "y": 4.168369327121192, - "heading": -0.8692948358062459, - "angularVelocity": 0.8840617054087768, - "velocityX": 0.7575338769931039, - "velocityY": -1.2332444257142754, - "timestamp": 0.3355200503009315 - }, - { - "x": 0.8611030812908155, - "y": 4.087871426229515, - "heading": -0.8129373545696883, - "angularVelocity": 1.0078231900479873, - "velocityX": 0.8841400694078274, - "velocityY": -1.4395187557848361, - "timestamp": 0.3914400586844201 - }, - { - "x": 0.9176312901451578, - "y": 3.995821968070834, - "heading": -0.7502611156342615, - "angularVelocity": 1.120819555419329, - "velocityX": 1.0108762585778373, - "velocityY": -1.6460916373156267, - "timestamp": 0.4473600670679087 - }, - { - "x": 0.9812548770105831, - "y": 3.8922019971306105, - "heading": -0.6820246305048665, - "angularVelocity": 1.2202516970570252, - "velocityX": 1.1377606818136925, - "velocityY": -1.8530034943775027, - "timestamp": 0.5032800754513973 - }, - { - "x": 1.0519833618359966, - "y": 3.77699034023748, - "heading": -0.6092267013635411, - "angularVelocity": 1.3018225720227254, - "velocityX": 1.2648153473148886, - "velocityY": -2.060293984633039, - "timestamp": 0.5592000838348858 - }, - { - "x": 1.129826908295346, - "y": 3.6501643798944907, - "heading": -0.5332724156478236, - "angularVelocity": 1.3582667083101592, - "velocityX": 1.3920517666147918, - "velocityY": -2.26798893650447, - "timestamp": 0.6151200922183744 - }, - { - "x": 1.214791954900265, - "y": 3.511704401985981, - "heading": -0.45632702327380104, - "angularVelocity": 1.3759903583409006, - "velocityX": 1.5194033238021893, - "velocityY": -2.4760364297333126, - "timestamp": 0.6710401006018629 - }, - { - "x": 1.306860167939052, - "y": 3.361616595640326, - "heading": -0.38215505844470404, - "angularVelocity": 1.3263940219829728, - "velocityX": 1.6464270249639614, - "velocityY": -2.6839732447173734, - "timestamp": 0.7269601089853515 - }, - { - "x": 1.4058792704184802, - "y": 3.2000739494751453, - "heading": -0.3184323584737964, - "angularVelocity": 1.1395330904442935, - "velocityX": 1.770727604337509, - "velocityY": -2.888816558419535, - "timestamp": 0.78288011736884 - }, - { - "x": 1.5121691709784744, - "y": 3.0300749520186443, - "heading": -0.28662541962876287, - "angularVelocity": 0.5687935278354703, - "velocityX": 1.9007490097476158, - "velocityY": -3.040038840671852, - "timestamp": 0.8388001257523285 - }, - { - "x": 1.6244650109400063, - "y": 2.8508645326617055, - "heading": -0.2866253125429205, - "angularVelocity": 0.0000019149825875479786, - "velocityX": 2.00815134345883, - "velocityY": -3.2047638141959642, - "timestamp": 0.8947201341358171 - }, - { - "x": 1.735288205529194, - "y": 2.670739688820617, - "heading": -0.28662527835532886, - "angularVelocity": 6.113659969296124e-7, - "velocityX": 1.9818164873864772, - "velocityY": -3.2211161809173365, - "timestamp": 0.9506401425193056 - }, - { - "x": 1.8506362591615513, - "y": 2.493478394875359, - "heading": -0.2866252441530595, - "angularVelocity": 6.116284727574992e-7, - "velocityX": 2.062733124811472, - "velocityY": -3.1699082147776885, - "timestamp": 1.0065601509027942 - }, - { - "x": 1.9771720781105007, - "y": 2.324022128559085, - "heading": -0.2866252095630549, - "angularVelocity": 6.185622213406466e-7, - "velocityX": 2.2628004288052175, - "velocityY": -3.0303333496335663, - "timestamp": 1.0624801592862827 - }, - { - "x": 2.1143701177449694, - "y": 2.1630766803808767, - "heading": -0.28662517392184667, - "angularVelocity": 6.373605666632096e-7, - "velocityX": 2.4534695827223683, - "velocityY": -2.878137053815794, - "timestamp": 1.1184001676697712 - }, - { - "x": 2.261659606829455, - "y": 2.011311910787294, - "heading": -0.2866251364727607, - "angularVelocity": 6.696902775127292e-7, - "velocityX": 2.633931813357441, - "velocityY": -2.7139618533818717, - "timestamp": 1.1743201760532598 - }, - { - "x": 2.418427669547749, - "y": 1.8693594092228303, - "heading": -0.2866250962912339, - "angularVelocity": 7.185536621676126e-7, - "velocityX": 2.803434177677683, - "velocityY": -2.538492136678184, - "timestamp": 1.2302401844367483 - }, - { - "x": 2.5840219554137547, - "y": 1.7378099138847216, - "heading": -0.28662505217416817, - "angularVelocity": 7.889316720181228e-7, - "velocityX": 2.961270762521925, - "velocityY": -2.3524584337678864, - "timestamp": 1.2861601928202369 + "x": 0.6705320996373237, + "y": 4.398099342072708, + "heading": -1.0386114123729513, + "angularVelocity": 0.159275518837918, + "velocityX": 0.1260500229589734, + "velocityY": -0.20514712040522684, + "timestamp": 0.055920008584652126 + }, + { + "x": 0.6846335257451809, + "y": 4.375148415264061, + "heading": -1.0210111037335994, + "angularVelocity": 0.3147408071783192, + "velocityX": 0.25217138667835376, + "velocityY": -0.41042423614624113, + "timestamp": 0.11184001716930425 + }, + { + "x": 0.7057920858570316, + "y": 4.3407101501549405, + "heading": -0.9949634713965649, + "angularVelocity": 0.4658016512569599, + "velocityX": 0.37837190385657166, + "velocityY": -0.6158487092681121, + "timestamp": 0.16776002575395638 + }, + { + "x": 0.7340126704011022, + "y": 4.2947751012827355, + "heading": -0.9607567060719514, + "angularVelocity": 0.6117088711249232, + "velocityX": 0.5046598750311373, + "velocityY": -0.8214420926396748, + "timestamp": 0.2236800343386085 + }, + { + "x": 0.769300659799434, + "y": 4.23733236505955, + "heading": -0.9187315683545035, + "angularVelocity": 0.7515223759994586, + "velocityX": 0.6310440626079745, + "velocityY": -1.0272304614586787, + "timestamp": 0.2796000429232606 + }, + { + "x": 0.8116619603260306, + "y": 4.168369326160844, + "heading": -0.8692948285945653, + "angularVelocity": 0.8840617340947026, + "velocityX": 0.7575338702330436, + "velocityY": -1.233244426175458, + "timestamp": 0.33552005150791275 + }, + { + "x": 0.8611030801762559, + "y": 4.087871424951383, + "heading": -0.8129373455124671, + "angularVelocity": 1.0078232194257235, + "velocityX": 0.8841400618775134, + "velocityY": -1.439518756289218, + "timestamp": 0.3914400600925649 + }, + { + "x": 0.9176312887785422, + "y": 3.995821966433115, + "heading": -0.7502611047615702, + "angularVelocity": 1.1208195838528383, + "velocityX": 1.0108762504339304, + "velocityY": -1.646091637824456, + "timestamp": 0.447360068677217 + }, + { + "x": 0.9812548753941115, + "y": 3.892201995095196, + "heading": -0.6820246179563085, + "angularVelocity": 1.2202517226363587, + "velocityX": 1.1377606732526766, + "velocityY": -1.8530034948234757, + "timestamp": 0.5032800772618691 + }, + { + "x": 1.051983359986124, + "y": 3.776990337772613, + "heading": -0.6092266874108444, + "angularVelocity": 1.3018225924493927, + "velocityX": 1.2648153385910723, + "velocityY": -2.060293984901216, + "timestamp": 0.5592000858465211 + }, + { + "x": 1.1298269062475699, + "y": 3.6501643769794776, + "heading": -0.5332724007265813, + "angularVelocity": 1.358266720744203, + "velocityX": 1.3920517580680556, + "velocityY": -2.267988936395559, + "timestamp": 0.6151200944311732 + }, + { + "x": 1.2147919527166744, + "y": 3.511704398620439, + "heading": -0.4563270080164221, + "angularVelocity": 1.375990359402014, + "velocityX": 1.5194033159076472, + "velocityY": -2.476036428882822, + "timestamp": 0.6710401030158253 + }, + { + "x": 1.306860165723401, + "y": 3.3616165918639007, + "heading": -0.3821550436425617, + "angularVelocity": 1.3263940090706232, + "velocityX": 1.6464270184678684, + "velocityY": -2.6839732424098903, + "timestamp": 0.7269601116004774 + }, + { + "x": 1.4058792683735797, + "y": 3.2000739453958573, + "heading": -0.3184323444941022, + "angularVelocity": 1.1395330716374203, + "velocityX": 1.7707276010210737, + "velocityY": -2.888816553443462, + "timestamp": 0.7828801201851294 + }, + { + "x": 1.51216916955334, + "y": 3.030074948039413, + "heading": -0.28662540722206215, + "angularVelocity": 0.5687934976599732, + "velocityX": 1.9007490139930476, + "velocityY": -3.040038827946505, + "timestamp": 0.8388001287697815 + }, + { + "x": 1.6244649894914296, + "y": 2.850864516485835, + "heading": -0.28662530013608456, + "angularVelocity": 0.000001914984999387773, + "velocityX": 2.0081509781618743, + "velocityY": -3.2047640207760013, + "timestamp": 0.8947201373544336 + }, + { + "x": 1.7352882065392865, + "y": 2.6707396868109132, + "heading": -0.2866252659484935, + "angularVelocity": 6.113659837473025e-7, + "velocityX": 1.9818168818785473, + "velocityY": -3.221115916000747, + "timestamp": 0.9506401459390856 + }, + { + "x": 1.8506362599745876, + "y": 2.4934783930913484, + "heading": -0.28662523174622423, + "angularVelocity": 6.116284691333173e-7, + "velocityX": 2.06273311386722, + "velocityY": -3.169908199338448, + "timestamp": 1.0065601545237377 + }, + { + "x": 1.977172078744517, + "y": 2.3240221270116197, + "heading": -0.2866251971562196, + "angularVelocity": 6.18562220740454e-7, + "velocityX": 2.262800417463786, + "velocityY": -3.0303333345023407, + "timestamp": 1.0624801631083898 + }, + { + "x": 2.11437011822214, + "y": 2.163076679089509, + "heading": -0.2866251615150111, + "angularVelocity": 6.373605678250823e-7, + "velocityX": 2.4534695710915724, + "velocityY": -2.8781370388824357, + "timestamp": 1.1184001716930418 + }, + { + "x": 2.2616596071687796, + "y": 2.011311909775525, + "heading": -0.28662512406592494, + "angularVelocity": 6.696902799491419e-7, + "velocityX": 2.633931801417216, + "velocityY": -2.7139618386188316, + "timestamp": 1.174320180277694 + }, + { + "x": 2.4184276697644314, + "y": 1.869359408517574, + "heading": -0.2866250838843977, + "angularVelocity": 7.185536662845936e-7, + "velocityX": 2.803434165399588, + "velocityY": -2.5384921220650836, + "timestamp": 1.230240188862346 + }, + { + "x": 2.5840219555188257, + "y": 1.7378099135158744, + "heading": -0.2866250397673314, + "angularVelocity": 7.889316803983354e-7, + "velocityX": 2.9612707498732993, + "velocityY": -2.352458419289386, + "timestamp": 1.286160197446998 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.2866250016379229, - "angularVelocity": 9.037238488587988e-7, - "velocityX": 3.1067845266977008, - "velocityY": -2.1566350283233997, - "timestamp": 1.3420802012037254 - }, - { - "x": 2.964172022537395, - "y": 1.4952512181839386, - "heading": -0.28662495601958665, - "angularVelocity": 7.195928192095116e-7, - "velocityX": 3.256089340971223, - "velocityY": -1.9238160187086888, - "timestamp": 1.4054748531095118 - }, - { - "x": 3.1789518158723995, - "y": 1.3887033110695066, - "heading": -0.2866249156918973, - "angularVelocity": 6.361370893313497e-7, - "velocityX": 3.3879796935268014, - "velocityY": -1.6807081340674839, - "timestamp": 1.4688695050152982 - }, - { - "x": 3.4009440542760783, - "y": 1.2981369637223683, - "heading": -0.28662487900587236, - "angularVelocity": 5.786927430417384e-7, - "velocityX": 3.501750253847766, - "velocityY": -1.4286117933375984, - "timestamp": 1.5322641569210846 - }, - { - "x": 3.6289614625982565, - "y": 1.2240364953903842, - "heading": -0.2866248447654741, - "angularVelocity": 5.401149339588049e-7, - "velocityX": 3.596792496960864, - "velocityY": -1.168875703302355, - "timestamp": 1.595658808826871 - }, - { - "x": 3.8617845298582507, - "y": 1.1667980667810436, - "heading": -0.2866248120410035, - "angularVelocity": 5.162023862741612e-7, - "velocityX": 3.672597928386812, - "velocityY": -0.9028904945231835, - "timestamp": 1.6590534607326575 - }, - { - "x": 4.0981679647792335, - "y": 1.1267270015307347, - "heading": -0.28662478005254766, - "angularVelocity": 5.045923416827531e-7, - "velocityX": 3.728759884544863, - "velocityY": -0.6320890492444092, - "timestamp": 1.7224481126384439 - }, - { - "x": 4.3358126512762265, - "y": 1.0949807955483843, - "heading": -0.28662474820722456, - "angularVelocity": 5.023345368252209e-7, - "velocityX": 3.7486551207847545, - "velocityY": -0.5007710434238176, - "timestamp": 1.7858427645442303 - }, - { - "x": 4.573457419312962, - "y": 1.06323519995946, - "heading": -0.28662471636190573, - "angularVelocity": 5.023344693474633e-7, - "velocityX": 3.7486564070090496, - "velocityY": -0.5007614149550472, - "timestamp": 1.8492374164500167 - }, - { - "x": 4.811102187354235, - "y": 1.0314896044045172, - "heading": -0.28662468451658685, - "angularVelocity": 5.02334469511756e-7, - "velocityX": 3.748656407080655, - "velocityY": -0.5007614144190154, - "timestamp": 1.9126320683558031 - }, - { - "x": 5.04874695539551, - "y": 0.9997440088495764, - "heading": -0.286624652671268, - "angularVelocity": 5.023344701827313e-7, - "velocityX": 3.748656407080659, - "velocityY": -0.5007614144189856, - "timestamp": 1.9760267202615895 - }, - { - "x": 5.286391723436784, - "y": 0.9679984132946354, - "heading": -0.2866246208259492, - "angularVelocity": 5.023344694262712e-7, - "velocityX": 3.7486564070806585, - "velocityY": -0.500761414418986, - "timestamp": 2.039421372167376 - }, - { - "x": 5.524036491478058, - "y": 0.9362528177396949, - "heading": -0.28662458898063026, - "angularVelocity": 5.023344704301764e-7, - "velocityX": 3.74865640708066, - "velocityY": -0.5007614144189806, - "timestamp": 2.1028160240731624 - }, - { - "x": 5.761681259520114, - "y": 0.9045072221906024, - "heading": -0.2866245571353113, - "angularVelocity": 5.02334470827004e-7, - "velocityX": 3.748656407092983, - "velocityY": -0.5007614143267314, - "timestamp": 2.166210675978949 - }, - { - "x": 5.99932604159473, - "y": 0.8727617316882137, - "heading": -0.28662452528999255, - "angularVelocity": 5.023344683092169e-7, - "velocityX": 3.748656628445413, - "velocityY": -0.5007597572988834, - "timestamp": 2.229605327884735 - }, - { - "x": 6.237212534637086, - "y": 0.8428813906316717, - "heading": -0.286624491558187, - "angularVelocity": 5.320922916664829e-7, - "velocityX": 3.7524694259050357, - "velocityY": -0.47133851450037906, - "timestamp": 2.2929999797905216 - }, - { - "x": 6.4638245505635705, - "y": 0.8220107849523657, - "heading": -0.24424436180847475, - "angularVelocity": 0.6685126974542769, - "velocityX": 3.5746235544169, - "velocityY": -0.32921713507194117, - "timestamp": 2.356394631696308 - }, - { - "x": 6.673647733933235, - "y": 0.8041360764949451, - "heading": -0.18514255243378552, - "angularVelocity": 0.9322838377994884, - "velocityX": 3.309793130207448, - "velocityY": -0.2819592492436893, - "timestamp": 2.4197892836020944 - }, - { - "x": 6.866141134149738, - "y": 0.7888964331390943, - "heading": -0.12766912670980685, - "angularVelocity": 0.9065973863125305, - "velocityX": 3.0364296424022634, - "velocityY": -0.24039320191392657, - "timestamp": 2.483183935507881 - }, - { - "x": 7.04134978279503, - "y": 0.7761674362974819, - "heading": -0.07636429138531158, - "angularVelocity": 0.8092927996630003, - "velocityX": 2.7637764918352543, - "velocityY": -0.20078975842520036, - "timestamp": 2.5465785874136673 - }, - { - "x": 7.19932046714385, - "y": 0.765888430525111, - "heading": -0.03338684229285238, - "angularVelocity": 0.6779349330023905, - "velocityX": 2.4918613731579065, - "velocityY": -0.16214310613530766, - "timestamp": 2.6099732393194537 + "heading": -0.2866249892310852, + "angularVelocity": 9.037238633691851e-7, + "velocityX": 3.1067845136425762, + "velocityY": -2.1566350139692623, + "timestamp": 1.3420802060316501 + }, + { + "x": 2.9641720225317068, + "y": 1.495251218591247, + "heading": -0.2866249436127499, + "angularVelocity": 7.195928005652006e-7, + "velocityX": 3.256089327481367, + "velocityY": -1.9238160043664425, + "timestamp": 1.4054748581983314 + }, + { + "x": 3.1789518158633587, + "y": 1.3887033119472996, + "heading": -0.28662490328506157, + "angularVelocity": 6.361370710101161e-7, + "velocityX": 3.3879796795310004, + "velocityY": -1.6807081197291671, + "timestamp": 1.4688695103650127 + }, + { + "x": 3.4009440542565814, + "y": 1.2981369651359904, + "heading": -0.2866248665990377, + "angularVelocity": 5.78692723773973e-7, + "velocityX": 3.501750239271706, + "velocityY": -1.4286117790059998, + "timestamp": 1.532264162531694 + }, + { + "x": 3.628961462551479, + "y": 1.2240364974062796, + "heading": -0.28662483235864045, + "angularVelocity": 5.401149161418495e-7, + "velocityX": 3.596792481728267, + "velocityY": -1.168875688991578, + "timestamp": 1.5956588146983752 + }, + { + "x": 3.8617845297574473, + "y": 1.16679806946556, + "heading": -0.28662479963417076, + "angularVelocity": 5.162023699881254e-7, + "velocityX": 3.672597912420367, + "velocityY": -0.9028904802604593, + "timestamp": 1.6590534668650565 + }, + { + "x": 4.098167964587261, + "y": 1.1267270049464149, + "heading": -0.286624767645716, + "angularVelocity": 5.045923223989343e-7, + "velocityX": 3.7287598677613913, + "velocityY": -0.6320890351095811, + "timestamp": 1.7224481190317378 + }, + { + "x": 4.335812648113985, + "y": 1.0949807783310654, + "heading": -0.28662473580039416, + "angularVelocity": 5.023345147080502e-7, + "velocityX": 3.7486550585038954, + "velocityY": -0.500771366832023, + "timestamp": 1.785842771198419 + }, + { + "x": 4.573457416127468, + "y": 1.063235184169841, + "heading": -0.28662470395507683, + "angularVelocity": 5.023344436089823e-7, + "velocityX": 3.74865639121502, + "velocityY": -0.5007613903733815, + "timestamp": 1.8492374233651003 + }, + { + "x": 4.811102184145877, + "y": 1.031489590045499, + "heading": -0.2866246721097595, + "angularVelocity": 5.023344446091564e-7, + "velocityX": 3.748656391292737, + "velocityY": -0.5007613897915972, + "timestamp": 1.9126320755317816 + }, + { + "x": 5.048746952164287, + "y": 0.9997439959211589, + "heading": -0.2866246402644421, + "angularVelocity": 5.023344444502557e-7, + "velocityX": 3.7486563912927418, + "velocityY": -0.5007613897915636, + "timestamp": 1.9760267276984629 + }, + { + "x": 5.2863917201826975, + "y": 0.9679984017968188, + "heading": -0.2866246084191247, + "angularVelocity": 5.023344456533015e-7, + "velocityX": 3.748656391292741, + "velocityY": -0.5007613897915631, + "timestamp": 2.039421379865144 + }, + { + "x": 5.524036488201108, + "y": 0.9362528076724792, + "heading": -0.2866245765738072, + "angularVelocity": 5.023344456596004e-7, + "velocityX": 3.748656391292742, + "velocityY": -0.5007613897915576, + "timestamp": 2.1028160320318254 + }, + { + "x": 5.761681256220374, + "y": 0.904507213554553, + "heading": -0.28662454472848975, + "angularVelocity": 5.023344455217454e-7, + "velocityX": 3.7486563913062563, + "velocityY": -0.500761389690393, + "timestamp": 2.1662106841985067 + }, + { + "x": 5.999326038930599, + "y": 0.8727617294120621, + "heading": -0.28662451288317237, + "angularVelocity": 5.023344437091617e-7, + "velocityX": 3.748656623044388, + "velocityY": -0.5007596549157438, + "timestamp": 2.229605336365188 + }, + { + "x": 6.23721253164048, + "y": 0.8428813874103429, + "heading": -0.28662447915136585, + "angularVelocity": 5.320923033369603e-7, + "velocityX": 3.7524694052175755, + "velocityY": -0.4713385274700439, + "timestamp": 2.2929999885318693 + }, + { + "x": 6.46382454823366, + "y": 0.8220107822516065, + "heading": -0.24424435205194292, + "angularVelocity": 0.6685126528968796, + "velocityX": 3.57462355022247, + "velocityY": -0.32921712550550913, + "timestamp": 2.3563946406985505 + }, + { + "x": 6.673647732212354, + "y": 0.804136074402253, + "heading": -0.1851425455819509, + "angularVelocity": 0.932283788143483, + "velocityX": 3.3097931261932545, + "velocityY": -0.28195923849154, + "timestamp": 2.419789292865232 + }, + { + "x": 6.866141132976172, + "y": 0.7888964316418866, + "heading": -0.12766912233999997, + "angularVelocity": 0.9065973434295085, + "velocityX": 3.036429638539596, + "velocityY": -0.24039319153132044, + "timestamp": 2.483183945031913 + }, + { + "x": 7.0413497820944775, + "y": 0.7761674353552135, + "heading": -0.07636428899971823, + "angularVelocity": 0.8092927650330481, + "velocityX": 2.763776487922594, + "velocityY": -0.2007897488451438, + "timestamp": 2.5465785971985944 + }, + { + "x": 7.199320466835695, + "y": 0.7658884300843406, + "heading": -0.03338684136743644, + "angularVelocity": 0.6779349071792803, + "velocityX": 2.4918613690926352, + "velocityY": -0.16214309755729506, + "timestamp": 2.6099732493652756 }, { "x": 7.340087890625, "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": 0.5266507708326882, - "velocityX": 2.220493673351972, - "velocityY": -0.12406863156475333, - "timestamp": 2.67336789122524 - }, - { - "x": 7.464023522460179, - "y": 0.7525372143128384, - "heading": 0.023010119360387892, - "angularVelocity": 0.36178963616626764, - "velocityX": 1.9486481772395672, - "velocityY": -0.08625561847872552, - "timestamp": 2.736968716290925 - }, - { - "x": 7.57058231953554, - "y": 0.7491648354718851, - "heading": 0.03698370022131657, - "angularVelocity": 0.21970754068830903, - "velocityX": 1.6754310492876554, - "velocityY": -0.05302413667543652, - "timestamp": 2.8005695413566096 - }, - { - "x": 7.659708944521439, - "y": 0.7476928551810322, - "heading": 0.04295335113137195, - "angularVelocity": 0.09386121805637215, - "velocityX": 1.4013438488235506, - "velocityY": -0.02314404395434409, - "timestamp": 2.8641703664222944 - }, - { - "x": 7.731365798341117, - "y": 0.7479588271083956, - "heading": 0.041693436231508264, - "angularVelocity": -0.019809725716020507, - "velocityX": 1.1266654755763426, - "velocityY": 0.004181894292859409, - "timestamp": 2.927771191487979 - }, - { - "x": 7.785525986892143, - "y": 0.7498349370538775, - "heading": 0.03380693253041854, - "angularVelocity": -0.1240000219013676, - "velocityX": 0.8515642445690118, - "velocityY": 0.029498201376229376, - "timestamp": 2.991372016553664 - }, - { - "x": 7.822169492500932, - "y": 0.7532180353995639, - "heading": 0.01977708435408761, - "angularVelocity": -0.22059223542841888, - "velocityX": 0.5761482743493486, - "velocityY": 0.05319268016086978, - "timestamp": 3.0549728416193487 + "heading": 2.6436628934070276e-25, + "angularVelocity": 0.5266507540676091, + "velocityX": 2.2204936690746413, + "velocityY": -0.1240686241013575, + "timestamp": 2.673367901531957 + }, + { + "x": 7.46402352318357, + "y": 0.752537214674761, + "heading": 0.023010119033746678, + "angularVelocity": 0.36178962784078056, + "velocityX": 1.9486481714334611, + "velocityY": -0.08625561202772893, + "timestamp": 2.736968727158371 + }, + { + "x": 7.570582320737423, + "y": 0.749164836112825, + "heading": 0.036983699836062915, + "angularVelocity": 0.2197075378297117, + "velocityX": 1.6754310420397782, + "velocityY": -0.05302413182094548, + "timestamp": 2.800569552784785 + }, + { + "x": 7.659708945959918, + "y": 0.7476928559896275, + "heading": 0.042953350833826356, + "angularVelocity": 0.09386121860789494, + "velocityX": 1.4013438401887508, + "velocityY": -0.023144041114242904, + "timestamp": 2.864170378411199 + }, + { + "x": 7.731365799776358, + "y": 0.7479588279518706, + "heading": 0.041693436078494114, + "angularVelocity": -0.019809723268890925, + "velocityX": 1.1266654655923272, + "velocityY": 0.00418189480440736, + "timestamp": 2.927771204037613 + }, + { + "x": 7.785525988085828, + "y": 0.7498349377827495, + "heading": 0.03380693250994457, + "angularVelocity": -0.12400001872419436, + "velocityX": 0.8515642332632949, + "velocityY": 0.029498199314249407, + "timestamp": 2.991372029664027 + }, + { + "x": 7.822169493215899, + "y": 0.7532180358509439, + "heading": 0.01977708439947626, + "angularVelocity": -0.22059223244802648, + "velocityX": 0.5761482617428887, + "velocityY": 0.05319267532887989, + "timestamp": 3.054972855290441 }, { "x": 7.841280937194824, "y": 0.7580231428146362, - "heading": 2.046055081284657e-30, - "angularVelocity": -0.310956411865142, - "velocityX": 0.30049051524338544, - "velocityY": 0.07555102327854848, - "timestamp": 3.1185736666850334 - }, - { - "x": 7.837626456422163, - "y": 0.7661832239405314, - "heading": -0.033360531548023535, - "angularVelocity": -0.41786111882102345, - "velocityX": -0.045774613098587845, - "velocityY": 0.10221002096529477, - "timestamp": 3.1984100765480727 - }, - { - "x": 7.806322741144082, - "y": 0.7764707746744537, - "heading": -0.07501113915617402, - "angularVelocity": -0.5216994060680163, - "velocityX": -0.3920982335225698, - "velocityY": 0.12885788265743264, - "timestamp": 3.278246486411112 - }, - { - "x": 7.74736425619444, - "y": 0.7888847549403224, - "heading": -0.12465356813987538, - "angularVelocity": -0.6218018704606533, - "velocityX": -0.738491185297363, - "velocityY": 0.15549271675874554, - "timestamp": 3.3580828962741514 - }, - { - "x": 7.660744366778727, - "y": 0.8034239689194149, - "heading": -0.18191677738707485, - "angularVelocity": -0.7172568173523243, - "velocityX": -1.084967241942757, - "velocityY": 0.18211257249711932, - "timestamp": 3.4379193061371907 - }, - { - "x": 7.5464550145644145, - "y": 0.8200870575241468, - "heading": -0.24632578527266594, - "angularVelocity": -0.8067623280666757, - "velocityX": -1.4315442341455176, - "velocityY": 0.20871540482992282, - "timestamp": 3.51775571600023 - }, - { - "x": 7.404486266909035, - "y": 0.8388724767291542, - "heading": -0.3172478637527482, - "angularVelocity": -0.8883425319569115, - "velocityX": -1.7782456387872365, - "velocityY": 0.23529889729803788, - "timestamp": 3.5975921258632693 - }, - { - "x": 7.234825726560123, - "y": 0.8597784078771571, - "heading": -0.3937913075224082, - "angularVelocity": -0.958753579989022, - "velocityX": -2.1251023266197273, - "velocityY": 0.2618596099682753, - "timestamp": 3.6774285357263086 - }, - { - "x": 7.037458048630302, - "y": 0.8828023770067691, - "heading": -0.474592846440749, - "angularVelocity": -1.0120888333650895, - "velocityX": -2.472151218578179, - "velocityY": 0.28838933475478196, - "timestamp": 3.757264945589348 - }, - { - "x": 6.812366694532574, - "y": 0.9079395818644894, - "heading": -0.5572950195818104, - "angularVelocity": -1.0358954427301792, - "velocityX": -2.8194072664825045, - "velocityY": 0.3148589083707988, - "timestamp": 3.837101355452387 - }, - { - "x": 6.559557151686243, - "y": 0.9351738572850871, - "heading": -0.6368741954392075, - "angularVelocity": -0.996777986308708, - "velocityX": -3.1665945810943685, - "velocityY": 0.3411260033776375, - "timestamp": 3.9169377653154265 - }, - { - "x": 6.2794552371906684, - "y": 0.9644007497338529, - "heading": -0.6963848347023115, - "angularVelocity": -0.7454072567290441, - "velocityX": -3.5084482753682624, - "velocityY": 0.3660847537972336, - "timestamp": 3.996774175178466 - }, - { - "x": 5.978414020607044, - "y": 0.9876514129078968, - "heading": -0.6963848502753711, - "angularVelocity": -1.950621226343756e-7, - "velocityX": -3.770725876828189, - "velocityY": 0.2912288167006853, - "timestamp": 4.0766105850415055 - }, - { - "x": 5.677372687056552, - "y": 1.010900561597095, - "heading": -0.6963848658464019, - "angularVelocity": -1.9503671222417687e-7, - "velocityX": -3.770727341909935, - "velocityY": 0.29120984684910667, - "timestamp": 4.156446994904545 - }, - { - "x": 5.376331353502739, - "y": 1.034149710243266, - "heading": -0.6963848814174328, - "angularVelocity": -1.9503671215554748e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.2912098463101618, - "timestamp": 4.236283404767585 - }, - { - "x": 5.075290019948924, - "y": 1.0573988588894354, - "heading": -0.6963848969884637, - "angularVelocity": -1.9503671305455908e-7, - "velocityX": -3.770727341951559, - "velocityY": 0.2912098463101465, - "timestamp": 4.316119814630625 - }, - { - "x": 4.774248686395111, - "y": 1.0806480075356062, - "heading": -0.6963849125594946, - "angularVelocity": -1.95036712978891e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.29120984631016333, - "timestamp": 4.3959562244936645 - }, - { - "x": 4.473207352844907, - "y": 1.1038971562285251, - "heading": -0.6963849281305255, - "angularVelocity": -1.9503671242042344e-7, - "velocityX": -3.770727341906336, - "velocityY": 0.29120984689571133, - "timestamp": 4.475792634356704 - }, - { - "x": 4.172166146375442, - "y": 1.1271479503662634, - "heading": -0.6963849437015556, - "angularVelocity": -1.9503670063864683e-7, - "velocityX": -3.770725750142127, - "velocityY": 0.2912304571012814, - "timestamp": 4.555629044219744 - }, - { - "x": 3.8735860999949274, - "y": 1.1720518831992321, - "heading": -0.696384959370356, - "angularVelocity": -1.9626133629188135e-7, - "velocityX": -3.7398982105123864, - "velocityY": 0.5624492998871312, - "timestamp": 4.635465454082784 - }, - { - "x": 3.580403442801602, - "y": 1.244234542261993, - "heading": -0.6963849756495779, - "angularVelocity": -2.0390723854783083e-7, - "velocityX": -3.672292600535076, - "velocityY": 0.9041320769131684, - "timestamp": 4.7153018639458235 - }, - { - "x": 3.295105029858622, - "y": 1.3430840304472003, - "heading": -0.6963849932202171, - "angularVelocity": -2.2008303364523734e-7, - "velocityX": -3.573537605616451, - "velocityY": 1.2381504673717785, - "timestamp": 4.795138273808863 - }, - { - "x": 3.020110821637561, - "y": 1.4677619739384067, - "heading": -0.6963850130039023, - "angularVelocity": -2.4780279138256264e-7, - "velocityX": -3.4444711215448884, - "velocityY": 1.5616677115753714, - "timestamp": 4.874974683671903 + "heading": -3.944931722509772e-25, + "angularVelocity": -0.31095640983727535, + "velocityX": 0.30049050135267197, + "velocityY": 0.07555101551538161, + "timestamp": 3.118573680916855 + }, + { + "x": 7.8376264553511055, + "y": 0.7661832230205181, + "heading": -0.033360531574413245, + "angularVelocity": -0.41786111810448695, + "velocityX": -0.04577462639954243, + "velocityY": 0.10221000918544447, + "timestamp": 3.19841009097995 + }, + { + "x": 7.806322738977973, + "y": 0.7764707725075406, + "heading": -0.07501113928889683, + "angularVelocity": -0.52169940609262, + "velocityX": -0.3920982462562205, + "velocityY": 0.12885786671635327, + "timestamp": 3.2782465010430455 + }, + { + "x": 7.747364252907164, + "y": 0.788884751185466, + "heading": -0.12465356840345063, + "angularVelocity": -0.6218018705415377, + "velocityX": -0.7384911974901414, + "velocityY": 0.15549269647914413, + "timestamp": 3.3580829111061408 + }, + { + "x": 7.660744362341635, + "y": 0.8034239632178009, + "heading": -0.18191677773821885, + "angularVelocity": -0.7172568166518608, + "velocityX": -1.0849672536261623, + "velocityY": 0.1821125476564444, + "timestamp": 3.437919321169236 + }, + { + "x": 7.5464550089458, + "y": 0.820087049493991, + "heading": -0.2463257855831559, + "angularVelocity": -0.8067623255358589, + "velocityX": -1.4315442453576273, + "velocityY": 0.2087153751405061, + "timestamp": 3.5177557312323313 + }, + { + "x": 7.404486260073429, + "y": 0.8388724659578224, + "heading": -0.3172478637844774, + "angularVelocity": -0.8883425262392362, + "velocityX": -1.7782456495748211, + "velocityY": 0.23529886237351122, + "timestamp": 3.5975921412954266 + }, + { + "x": 7.234825718467376, + "y": 0.8597783939083566, + "heading": -0.39379130688880964, + "angularVelocity": -0.9587535692529209, + "velocityX": -2.125102337041081, + "velocityY": 0.26185956926184595, + "timestamp": 3.677428551358522 + }, + { + "x": 7.037458039234421, + "y": 0.8828023593175933, + "heading": -0.4745928445431507, + "angularVelocity": -1.0120888149966063, + "velocityX": -2.4721512287059744, + "velocityY": 0.2883892874321469, + "timestamp": 3.757264961421617 + }, + { + "x": 6.812366683780793, + "y": 0.9079395598178155, + "heading": -0.5572950154989933, + "angularVelocity": -1.035895412763206, + "velocityX": -2.819407276401054, + "velocityY": 0.31485885300148303, + "timestamp": 3.8371013714847124 + }, + { + "x": 6.5595571395215275, + "y": 0.9351738300036528, + "heading": -0.6368741877579803, + "angularVelocity": -0.9967779387386686, + "velocityX": -3.16659459085733, + "velocityY": 0.34112593695425386, + "timestamp": 3.9169377815478077 + }, + { + "x": 6.279455223533108, + "y": 0.9644007155460906, + "heading": -0.6963848224506938, + "angularVelocity": -0.7454071976142457, + "velocityX": -3.5084482852755343, + "velocityY": 0.36608466637389636, + "timestamp": 3.996774191610903 + }, + { + "x": 5.978414008635392, + "y": 0.98765139076885, + "heading": -0.6963848380237664, + "angularVelocity": -1.9506228543282429e-7, + "velocityX": -3.7707258462623967, + "velocityY": 0.29122896688846905, + "timestamp": 4.076610601673998 + }, + { + "x": 5.677372675666473, + "y": 1.0109005372081505, + "heading": -0.6963848535947982, + "angularVelocity": -1.9503672258290444e-7, + "velocityX": -3.7707273251766207, + "velocityY": 0.2912098179380391, + "timestamp": 4.156447011737093 + }, + { + "x": 5.376331342694041, + "y": 1.0341496836019548, + "heading": -0.69638486916583, + "angularVelocity": -1.9503672329128174e-7, + "velocityX": -3.7707273252206313, + "velocityY": 0.291209817368169, + "timestamp": 4.236283421800188 + }, + { + "x": 5.075290009721609, + "y": 1.0573988299957575, + "heading": -0.6963848847368618, + "angularVelocity": -1.9503672272562427e-7, + "velocityX": -3.770727325220632, + "velocityY": 0.29120981736815194, + "timestamp": 4.316119831863283 + }, + { + "x": 4.774248676749177, + "y": 1.080647976389562, + "heading": -0.6963849003078936, + "angularVelocity": -1.9503672313945863e-7, + "velocityX": -3.770727325220631, + "velocityY": 0.29120981736817053, + "timestamp": 4.395956241926378 + }, + { + "x": 4.473207343780594, + "y": 1.1038971228332064, + "heading": -0.6963849158789254, + "angularVelocity": -1.950367233647788e-7, + "velocityX": -3.770727325172418, + "velocityY": 0.2912098179924497, + "timestamp": 4.475792651989472 + }, + { + "x": 4.1721661401549985, + "y": 1.1271479440124157, + "heading": -0.6963849314499564, + "angularVelocity": -1.9503671206893558e-7, + "velocityX": -3.77072570507217, + "velocityY": 0.2912307950825182, + "timestamp": 4.555629062052567 + }, + { + "x": 3.8735860947396503, + "y": 1.1720518781992464, + "heading": -0.6963849471187581, + "angularVelocity": -1.9626135080534044e-7, + "velocityX": -3.739898189051575, + "velocityY": 0.5624493154356899, + "timestamp": 4.635465472115662 + }, + { + "x": 3.5804034386486023, + "y": 1.2442345385889582, + "heading": -0.6963849633979813, + "angularVelocity": -2.0390725479039878e-7, + "velocityX": -3.6722925775262745, + "velocityY": 0.9041320912684484, + "timestamp": 4.715301882178757 + }, + { + "x": 3.295105026948141, + "y": 1.3430840280599874, + "heading": -0.6963849809686221, + "angularVelocity": -2.2008305287856005e-7, + "velocityX": -3.573537581098501, + "velocityY": 1.238150480374907, + "timestamp": 4.795138292241852 + }, + { + "x": 3.020110820111431, + "y": 1.467761972780782, + "heading": -0.6963850007523091, + "angularVelocity": -2.47802813909356e-7, + "velocityX": -3.444471095573825, + "velocityY": 1.561667723063461, + "timestamp": 4.874974702304947 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.6963850355061774, - "angularVelocity": -2.8185479639389566e-7, - "velocityX": -3.2861879673103584, - "velocityY": 1.8719390230438342, - "timestamp": 4.954811093534943 - }, - { - "x": 2.5741841985169076, - "y": 1.73878733799422, - "heading": -0.6963850589279512, - "angularVelocity": -4.0231077616908087e-7, - "velocityX": -3.153128257888759, - "velocityY": 2.0882929565548642, - "timestamp": 5.0130292054104135 - }, - { - "x": 2.3991894720533877, - "y": 1.872411165939459, - "heading": -0.6963850794155536, - "angularVelocity": -3.519111451084776e-7, - "velocityX": -3.0058468202788267, - "velocityY": 2.2952277846293376, - "timestamp": 5.071247317285884 - }, - { - "x": 2.2335584908852413, - "y": 2.017479635272907, - "heading": -0.6963850978369928, - "angularVelocity": -3.164211012595542e-7, - "velocityX": -2.845007779064253, - "velocityY": 2.491809930967045, - "timestamp": 5.129465429161355 - }, - { - "x": 2.0780383093475274, - "y": 2.173338406648706, - "heading": -0.6963851148088384, - "angularVelocity": -2.9152174806156023e-7, - "velocityX": -2.6713367460348776, - "velocityY": 2.6771526309404154, - "timestamp": 5.187683541036826 - }, - { - "x": 1.9333303562340614, - "y": 2.339284459115452, - "heading": -0.6963851307944149, - "angularVelocity": -2.745808111537906e-7, - "velocityX": -2.4856174213103777, - "velocityY": 2.850419691069807, - "timestamp": 5.245901652912297 - }, - { - "x": 1.8000872249814166, - "y": 2.514569234135199, - "heading": -0.6963851461633945, - "angularVelocity": -2.639896616028141e-7, - "velocityX": -2.288688639330214, - "velocityY": 3.0108289220145816, - "timestamp": 5.304119764787767 - }, - { - "x": 1.6789094155739503, - "y": 2.6984018293834224, - "heading": -0.6963851612308567, - "angularVelocity": -2.588105615203023e-7, - "velocityX": -2.0814451981312545, - "velocityY": 3.1576529936498847, - "timestamp": 5.362337876663238 - }, - { - "x": 1.5665736982883989, - "y": 2.8877670089616614, - "heading": -0.6963851762457395, - "angularVelocity": -2.5790741626665006e-7, - "velocityX": -1.9295664814042677, - "velocityY": 3.2526850060560912, - "timestamp": 5.420555988538709 - }, - { - "x": 1.4540456075762054, - "y": 3.0767463777174253, - "heading": -0.6971250426595835, - "angularVelocity": -0.012708526436355243, - "velocityX": -1.9328708384238367, - "velocityY": 3.246058016446739, - "timestamp": 5.47877410041418 - }, - { - "x": 1.3486317039814464, - "y": 3.2544802087721734, - "heading": -0.7253513181524348, - "angularVelocity": -0.48483666995638414, - "velocityX": -1.8106719747325546, - "velocityY": 3.052895831368142, - "timestamp": 5.53699221228965 - }, - { - "x": 1.2507109072486164, - "y": 3.4195717896549707, - "heading": -0.7601787682651484, - "angularVelocity": -0.598223628193407, - "velocityX": -1.6819644879978937, - "velocityY": 2.8357426162485435, - "timestamp": 5.595210324165121 - }, - { - "x": 1.160328320301069, - "y": 3.57195134860905, - "heading": -0.7969704460064531, - "angularVelocity": -0.6319627441714823, - "velocityX": -1.5524822780387875, - "velocityY": 2.617390946652855, - "timestamp": 5.653428436040592 - }, - { - "x": 1.0774888591394964, - "y": 3.71161238889488, - "heading": -0.8336344675120992, - "angularVelocity": -0.6297700204374731, - "velocityX": -1.422915626991949, - "velocityY": 2.398927683957996, - "timestamp": 5.711646547916063 - }, - { - "x": 1.0021908033919844, - "y": 3.838558158350536, - "heading": -0.8689739346571361, - "angularVelocity": -0.6070184347549549, - "velocityX": -1.2933785264038766, - "velocityY": 2.1805202086799973, - "timestamp": 5.7698646597915335 - }, - { - "x": 0.9344313462846833, - "y": 3.952793536595294, - "heading": -0.9022118432658017, - "angularVelocity": -0.5709204152783613, - "velocityX": -1.1638896371672067, - "velocityY": 1.9621965495739466, - "timestamp": 5.828082771667004 - }, - { - "x": 0.8742077702690397, - "y": 4.05432328906152, - "heading": -0.932802055793642, - "angularVelocity": -0.5254415085338586, - "velocityX": -1.0344474266781911, - "velocityY": 1.74395474527581, - "timestamp": 5.886300883542475 - }, - { - "x": 0.8215176865940929, - "y": 4.143151679881797, - "heading": -0.9603390876580661, - "angularVelocity": -0.47299768022924404, - "velocityX": -0.9050462472512313, - "velocityY": 1.5257861850670003, - "timestamp": 5.944518995417946 - }, - { - "x": 0.7763590444591775, - "y": 4.21928243186123, - "heading": -0.9845096003709082, - "angularVelocity": -0.4151717040316119, - "velocityX": -0.7756802939866984, - "velocityY": 1.307681570681621, - "timestamp": 6.002737107293417 - }, - { - "x": 0.7387300854642734, - "y": 4.282718776761074, - "heading": -1.0050639989905084, - "angularVelocity": -0.35305848914451915, - "velocityX": -0.6463445443815323, - "velocityY": 1.0896324675649904, - "timestamp": 6.060955219168887 - }, - { - "x": 0.7086292911290722, - "y": 4.333463522832413, - "heading": -1.0217986916878445, - "angularVelocity": -0.28744822115035373, - "velocityX": -0.5170348773863939, - "velocityY": 0.871631601173907, - "timestamp": 6.119173331044358 - }, - { - "x": 0.6860553360496396, - "y": 4.371519118466469, - "heading": -1.0345444282293665, - "angularVelocity": -0.21893077825652071, - "velocityX": -0.3877479765698806, - "velocityY": 0.6536727902728674, - "timestamp": 6.177391442919829 - }, - { - "x": 0.6710070489868055, - "y": 4.396887706764773, - "heading": -1.0431583071392123, - "angularVelocity": -0.14795874741302198, - "velocityX": -0.2584811938769611, - "velocityY": 0.43575079096636016, - "timestamp": 6.2356095547953 + "heading": -0.6963850232545865, + "angularVelocity": -2.8185482385848004e-7, + "velocityX": -3.286187939960048, + "velocityY": 1.871939032853051, + "timestamp": 4.954811112368041 + }, + { + "x": 2.5741841996274863, + "y": 1.7387873388514183, + "heading": -0.6963850466763624, + "angularVelocity": -4.023108112528609e-7, + "velocityX": -3.153128229528534, + "velocityY": 2.088292965130013, + "timestamp": 5.013029224414929 + }, + { + "x": 2.399189474356015, + "y": 1.8724111676120014, + "heading": -0.6963850671639665, + "angularVelocity": -3.519111736959287e-7, + "velocityX": -3.0058467909528623, + "velocityY": 2.295227791876272, + "timestamp": 5.071247336461816 + }, + { + "x": 2.233558494460658, + "y": 2.017479637711712, + "heading": -0.6963850855854071, + "angularVelocity": -3.164211270787494e-7, + "velocityX": -2.845007748824996, + "velocityY": 2.4918099367921025, + "timestamp": 5.129465448508704 + }, + { + "x": 2.0780383142751306, + "y": 2.1733384097973816, + "heading": -0.6963851025572542, + "angularVelocity": -2.9152176872861595e-7, + "velocityX": -2.671336714943196, + "velocityY": 2.6771526352511037, + "timestamp": 5.187683560555591 + }, + { + "x": 1.93333036259127, + "y": 2.339284462910256, + "heading": -0.6963851185428317, + "angularVelocity": -2.745808302376555e-7, + "velocityX": -2.4856173894357143, + "velocityY": 2.850419693775459, + "timestamp": 5.2459016726024785 + }, + { + "x": 1.800087232842991, + "y": 2.514569238505031, + "heading": -0.6963851339118123, + "angularVelocity": -2.639896791762275e-7, + "velocityX": -2.2886886067512457, + "velocityY": 3.0108289230266427, + "timestamp": 5.304119784649366 + }, + { + "x": 1.6789094250105836, + "y": 2.698401834249409, + "heading": -0.6963851489792755, + "angularVelocity": -2.5881057704594286e-7, + "velocityX": -2.0814451649482146, + "velocityY": 3.157652992874852, + "timestamp": 5.362337896696253 + }, + { + "x": 1.5665736783823851, + "y": 2.8877669958946757, + "heading": -0.6963851639941595, + "angularVelocity": -2.5790743404475406e-7, + "velocityX": -1.9295669797352, + "velocityY": 3.2526846884480856, + "timestamp": 5.420556008743141 + }, + { + "x": 1.4540455902705487, + "y": 3.076746366139197, + "heading": -0.6971250291266823, + "angularVelocity": -0.012708504389955933, + "velocityX": -1.9328707880669334, + "velocityY": 3.2460580324611277, + "timestamp": 5.478774120790028 + }, + { + "x": 1.348631689048656, + "y": 3.2544801987728227, + "heading": -0.7253513053237864, + "angularVelocity": -0.48483668062563473, + "velocityX": -1.8106719286430082, + "velocityY": 3.0528958494992513, + "timestamp": 5.536992232836916 + }, + { + "x": 1.250710894485043, + "y": 3.4195717811136617, + "heading": -0.7601787566205287, + "angularVelocity": -0.5982236467698062, + "velocityX": -1.681964445785358, + "velocityY": 2.8357426329434743, + "timestamp": 5.595210344883803 + }, + { + "x": 1.1603283095237442, + "y": 3.5719513414037465, + "heading": -0.7969704357290219, + "angularVelocity": -0.6319627657946384, + "velocityX": -1.5524822393502977, + "velocityY": 2.617390961894501, + "timestamp": 5.6534284569306905 + }, + { + "x": 1.0774888501734665, + "y": 3.7116123829065053, + "heading": -0.8336344586576223, + "angularVelocity": -0.6297700430249606, + "velocityX": -1.4229155916901037, + "velocityY": 2.398927697797529, + "timestamp": 5.711646568977578 + }, + { + "x": 1.0021907960664256, + "y": 3.838558153462584, + "heading": -0.8689739272109267, + "angularVelocity": -0.6070184571571581, + "velocityX": -1.2933784944176396, + "velocityY": 2.1805202211614, + "timestamp": 5.769864681024465 + }, + { + "x": 0.9344313404312847, + "y": 3.9527935326932564, + "heading": -0.9022118371683866, + "angularVelocity": -0.5709204367652957, + "velocityX": -1.1638896084532742, + "velocityY": 1.962196560731307, + "timestamp": 5.828082793071353 + }, + { + "x": 0.8742077657211834, + "y": 4.054323286032448, + "heading": -0.9328020509544388, + "angularVelocity": -0.5254415285987893, + "velocityX": -1.0344474012073521, + "velocityY": 1.7439547551356767, + "timestamp": 5.88630090511824 + }, + { + "x": 0.8215176831863799, + "y": 4.143151677613985, + "heading": -0.9603390839635252, + "angularVelocity": -0.47299769849816853, + "velocityX": -0.9050462250024212, + "velocityY": 1.5257861936504657, + "timestamp": 5.944519017165128 + }, + { + "x": 0.7763590420271295, + "y": 4.219282430243976, + "heading": -0.9845095976897603, + "angularVelocity": -0.4151717202160187, + "velocityX": -0.7756802749439998, + "velocityY": 1.3076815780057875, + "timestamp": 6.002737129212015 + }, + { + "x": 0.7387300838441316, + "y": 4.282718775684509, + "heading": -1.0050639971773583, + "angularVelocity": -0.3530585030143863, + "velocityX": -0.6463445285325005, + "velocityY": 1.0896324736439786, + "timestamp": 6.0609552412589025 + }, + { + "x": 0.7086292901576546, + "y": 4.3334635221873645, + "heading": -1.0217986905857463, + "angularVelocity": -0.2874482325175743, + "velocityX": -0.517034864721044, + "velocityY": 0.8716316060195561, + "timestamp": 6.11917335330579 + }, + { + "x": 0.6860553355642359, + "y": 4.371519118144359, + "heading": -1.0345444276717368, + "angularVelocity": -0.21893078696412344, + "velocityX": -0.3877479670800402, + "velocityY": 0.6536727938952351, + "timestamp": 6.177391465352677 + }, + { + "x": 0.6710070488250978, + "y": 4.396887706657532, + "heading": -1.0431583069512933, + "angularVelocity": -0.14795875332781483, + "velocityX": -0.2584811875558354, + "velocityY": 0.435750793374088, + "timestamp": 6.235609577399565 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -0.07488723824993972, - "velocityX": -0.12923242394972312, - "velocityY": 0.2178611369128848, - "timestamp": 6.29382766667077 + "angularVelocity": -0.074887241257288, + "velocityX": -0.12923242079159178, + "velocityY": 0.21786113811346933, + "timestamp": 6.293827689446452 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -7.364462273467936e-32, - "velocityX": -8.426727085890477e-35, - "velocityY": -7.327805300908389e-34, - "timestamp": 6.352045778546241 - }, - { - "x": 0.6687542994396359, - "y": 4.396916362187497, - "heading": -1.0350125956919525, - "angularVelocity": 0.2204003026897336, - "velocityX": 0.09289604490221867, - "velocityY": -0.22303166776742134, - "timestamp": 6.408785741736511 - }, - { - "x": 0.6793565646010321, - "y": 4.371607473302922, - "heading": -1.0104451675948027, - "angularVelocity": 0.4329827993501856, - "velocityX": 0.18685710327027932, - "velocityY": -0.4460504988292779, - "timestamp": 6.465525704926781 - }, - { - "x": 0.6953596015342451, - "y": 4.333646306767502, - "heading": -0.974331857670356, - "angularVelocity": 0.6364704503481122, - "velocityX": 0.2820417221553139, - "velocityY": -0.6690375601429702, - "timestamp": 6.522265668117051 - }, - { - "x": 0.7168434967725004, - "y": 4.283036502515157, - "heading": -0.9272707047547418, - "angularVelocity": 0.8294181079709282, - "velocityX": 0.3786378071168583, - "velocityY": -0.8919604703061096, - "timestamp": 6.5790056313073215 - }, - { - "x": 0.7439015517455688, - "y": 4.2197848874550585, - "heading": -0.8699519201955506, - "angularVelocity": 1.0102012996903078, - "velocityX": 0.4768782609592592, - "velocityY": -1.114763061230607, - "timestamp": 6.635745594497592 - }, - { - "x": 0.7766442528059767, - "y": 4.143903696796417, - "heading": -0.8031709855452435, - "angularVelocity": 1.1769647157923862, - "velocityX": 0.5770659552705253, - "velocityY": -1.3373500156174554, - "timestamp": 6.692485557687862 - }, - { - "x": 0.8152053931020986, - "y": 4.055414010177326, - "heading": -0.7278520306671347, - "angularVelocity": 1.3274410246890034, - "velocityX": 0.6796116551364678, - "velocityY": -1.559565456931146, - "timestamp": 6.749225520878132 - }, - { - "x": 0.8597512927978146, - "y": 3.9543509740987406, - "heading": -0.6450962807712558, - "angularVelocity": 1.4585090515192565, - "velocityX": 0.7850886252135366, - "velocityY": -1.7811614670894944, - "timestamp": 6.805965484068402 - }, - { - "x": 0.9104943772436287, - "y": 3.840772171273386, - "heading": -0.5562797497368643, - "angularVelocity": 1.5653258486713548, - "velocityX": 0.8943094354089278, - "velocityY": -2.0017426244088754, - "timestamp": 6.862705447258672 - }, - { - "x": 0.9677131149809225, - "y": 3.7147727983516208, - "heading": -0.46323645073147074, - "angularVelocity": 1.6398195165087535, - "velocityX": 1.0084380482486026, - "velocityY": -2.2206460109824513, - "timestamp": 6.919445410448942 - }, - { - "x": 1.0317820620609501, - "y": 3.576517638239885, - "heading": -0.3685846112186535, - "angularVelocity": 1.6681688564973882, - "velocityX": 1.1291679352201949, - "velocityY": -2.436645220373414, - "timestamp": 6.976185373639212 - }, - { - "x": 1.1032183238971194, - "y": 3.4263179326907856, - "heading": -0.2763023679431359, - "angularVelocity": 1.6264064706221368, - "velocityX": 1.2590114236876868, - "velocityY": -2.64715902344566, - "timestamp": 7.032925336829482 - }, - { - "x": 1.1827463587467486, - "y": 3.2648378432524847, - "heading": -0.1927401719055838, - "angularVelocity": 1.4727220699339676, - "velocityX": 1.4016229545821517, - "velocityY": -2.8459674691151595, - "timestamp": 7.089665300019752 - }, - { - "x": 1.2713975131080564, - "y": 3.0937718159398226, - "heading": -0.12815970051626668, - "angularVelocity": 1.1381831738726118, - "velocityX": 1.562411206789599, - "velocityY": -3.014912553591437, - "timestamp": 7.146405263210022 - }, - { - "x": 1.3706976001434645, - "y": 2.9168916321975784, - "heading": -0.09351951840137682, - "angularVelocity": 0.610507659279375, - "velocityX": 1.7500907905494714, - "velocityY": -3.117382772158287, - "timestamp": 7.203145226400292 - }, - { - "x": 1.4825331259415389, - "y": 2.737551336103052, - "heading": -0.084031813825935, - "angularVelocity": 0.1672137950394149, - "velocityX": 1.9710186526390192, - "velocityY": -3.1607404377956887, - "timestamp": 7.2598851895905625 - }, - { - "x": 1.6073609242840539, - "y": 2.563006200971128, - "heading": -0.08403157292142222, - "angularVelocity": 0.000004245764347455466, - "velocityX": 2.199997873173106, - "velocityY": -3.0762292627263235, - "timestamp": 7.316625152780833 - }, - { - "x": 1.7433400400906298, - "y": 2.3970011200993575, - "heading": -0.08403150231700539, - "angularVelocity": 0.0000012443507689652163, - "velocityX": 2.3965316182984986, - "velocityY": -2.925717105509803, - "timestamp": 7.373365115971103 - }, - { - "x": 1.889887864003564, - "y": 2.2402473630588187, - "heading": -0.08403142284388228, - "angularVelocity": 0.0000014006551755862827, - "velocityX": 2.5827973032253166, - "velocityY": -2.7626693467333516, - "timestamp": 7.430105079161373 - }, - { - "x": 2.046376539489404, - "y": 2.0934165181715283, - "heading": -0.08403133019563887, - "angularVelocity": 0.0000016328569530158415, - "velocityX": 2.757997479855154, - "velocityY": -2.58778533914292, - "timestamp": 7.486845042351643 - }, - { - "x": 2.2121356179273546, - "y": 1.9571376609651308, - "heading": -0.08403121735664319, - "angularVelocity": 0.00000198870406896459, - "velocityX": 2.921381494064391, - "velocityY": -2.401814339381988, - "timestamp": 7.543585005541913 - }, - { - "x": 2.3864549305613383, - "y": 1.8319946594051495, - "heading": -0.08403107170612155, - "angularVelocity": 0.0000025669830126630976, - "velocityX": 3.0722493077661266, - "velocityY": -2.2055530973879893, - "timestamp": 7.600324968732183 - }, - { - "x": 2.5685876291674634, - "y": 1.7185236734546225, - "heading": -0.08403086720089675, - "angularVelocity": 0.000003604253744603396, - "velocityX": 3.2099544723948052, - "velocityY": -1.9998424315154402, - "timestamp": 7.657064931922453 + "angularVelocity": 1.6934356345077546e-25, + "velocityX": 7.869408079103222e-27, + "velocityY": 8.248206791285532e-26, + "timestamp": 6.35204580149334 + }, + { + "x": 0.6687542994417671, + "y": 4.396916362162236, + "heading": -1.0350125954694533, + "angularVelocity": 0.22040030578546457, + "velocityX": 0.09289604459177633, + "velocityY": -0.22303166737709929, + "timestamp": 6.408785764896168 + }, + { + "x": 0.679356564606483, + "y": 4.37160747322566, + "heading": -1.010445166940442, + "angularVelocity": 0.4329828053394013, + "velocityX": 0.18685710262878952, + "velocityY": -0.44605049807478747, + "timestamp": 6.465525728298996 + }, + { + "x": 0.6953596015431592, + "y": 4.333646306609793, + "heading": -0.9743318563908197, + "angularVelocity": 0.6364704589820466, + "velocityX": 0.28204172115977494, + "velocityY": -0.6690375590544457, + "timestamp": 6.522265691701824 + }, + { + "x": 0.7168434967838145, + "y": 4.283036502246609, + "heading": -0.9272707026758202, + "angularVelocity": 0.8294181189523645, + "velocityX": 0.37863780574071754, + "velocityY": -0.8919604689181454, + "timestamp": 6.579005655104652 + }, + { + "x": 0.7439015517568099, + "y": 4.219784887043078, + "heading": -0.8699519171651382, + "angularVelocity": 1.010201312675245, + "velocityX": 0.47687825917150645, + "velocityY": -1.114763059582432, + "timestamp": 6.63574561850748 + }, + { + "x": 0.7766442528130582, + "y": 4.143903696205975, + "heading": -0.8031709814362636, + "angularVelocity": 1.176964730392239, + "velocityX": 0.5770659530354297, + "velocityY": -1.3373500137527523, + "timestamp": 6.692485581910308 + }, + { + "x": 0.8152053930991828, + "y": 4.055414009370765, + "heading": -0.7278520253810473, + "angularVelocity": 1.3274410404618378, + "velocityX": 0.679611652414335, + "velocityY": -1.5595654548977111, + "timestamp": 6.749225545313136 + }, + { + "x": 0.8597512927773133, + "y": 3.954350973035596, + "heading": -0.6450962742441403, + "angularVelocity": 1.4585090679276476, + "velocityX": 0.7850886219625312, + "velocityY": -1.7811614649390577, + "timestamp": 6.805965508715964 + }, + { + "x": 0.9104943771963883, + "y": 3.8407721699102697, + "heading": -0.556279741952017, + "angularVelocity": 1.56532586497398, + "velocityX": 0.8943094315874387, + "velocityY": -2.001742622196794, + "timestamp": 6.862705472118792 + }, + { + "x": 0.9677131148965982, + "y": 3.714772796642351, + "heading": -0.4632364417450351, + "angularVelocity": 1.6398195315428243, + "velocityX": 1.0084380438172502, + "velocityY": -2.2206460087642403, + "timestamp": 6.91944543552162 + }, + { + "x": 1.0317820619285505, + "y": 3.5765176361367876, + "heading": -0.36858460120873987, + "angularVelocity": 1.6681688682861946, + "velocityX": 1.1291679301428568, + "velocityY": -2.4366452181862606, + "timestamp": 6.976185398924448 + }, + { + "x": 1.1032183237055362, + "y": 3.4263179301504665, + "heading": -0.2763023573010561, + "angularVelocity": 1.6264064756708094, + "velocityX": 1.2590114179281549, + "velocityY": -2.6471590212346836, + "timestamp": 7.032925362327276 + }, + { + "x": 1.1827463584827187, + "y": 3.264837840257316, + "heading": -0.19274016139718564, + "angularVelocity": 1.472722062060866, + "velocityX": 1.401622948054619, + "velocityY": -2.845967466470068, + "timestamp": 7.089665325730104 + }, + { + "x": 1.2713975127609685, + "y": 3.0937718125950266, + "heading": -0.12815969147628775, + "angularVelocity": 1.1381831437289833, + "velocityX": 1.5624111994727097, + "velocityY": -3.0149125484589985, + "timestamp": 7.146405289132932 + }, + { + "x": 1.3706975997688013, + "y": 2.916891628537348, + "heading": -0.09351951086762286, + "angularVelocity": 0.6105076304462074, + "velocityX": 1.7500907835073471, + "velocityY": -3.117382766039356, + "timestamp": 7.20314525253576 + }, + { + "x": 1.4825331256479723, + "y": 2.7375513323263876, + "heading": -0.08403180748655616, + "angularVelocity": 0.16721377336302404, + "velocityX": 1.971018646684527, + "velocityY": -3.160740428007094, + "timestamp": 7.2598852159385885 + }, + { + "x": 1.6073609239957887, + "y": 2.563006197529012, + "heading": -0.08403156658210396, + "angularVelocity": 0.000004245763263575802, + "velocityX": 2.199997865024975, + "velocityY": -3.0762292453060858, + "timestamp": 7.3166251793414165 + }, + { + "x": 1.743340039826477, + "y": 2.397001117024792, + "heading": -0.08403149597768683, + "angularVelocity": 0.000001244350769606544, + "velocityX": 2.3965316097456575, + "velocityY": -2.925717088071768, + "timestamp": 7.373365142744245 + }, + { + "x": 1.889887863777881, + "y": 2.2402473603885453, + "heading": -0.08403141650456336, + "angularVelocity": 0.0000014006551769644768, + "velocityX": 2.5827972942277393, + "velocityY": -2.7626693292585887, + "timestamp": 7.430105106147073 + }, + { + "x": 2.0463765393116815, + "y": 2.0934165159455875, + "heading": -0.08403132385631941, + "angularVelocity": 0.000001632856956195875, + "velocityX": 2.757997470368504, + "velocityY": -2.587785321617605, + "timestamp": 7.486845069549901 + }, + { + "x": 2.212135617801855, + "y": 1.957137659226374, + "heading": -0.08403121101732289, + "angularVelocity": 0.000001988704076477053, + "velocityX": 2.9213814840407957, + "velocityY": -2.401814321798131, + "timestamp": 7.543585032952729 + }, + { + "x": 2.3864549304867793, + "y": 1.8319946581987157, + "heading": -0.08403106536679993, + "angularVelocity": 0.0000025669830265249184, + "velocityX": 3.072249297154761, + "velocityY": -2.2055530797438214, + "timestamp": 7.600324996355557 + }, + { + "x": 2.5685876291367493, + "y": 1.7185236728273872, + "heading": -0.08403086086157263, + "angularVelocity": 0.000003604253774998199, + "velocityX": 3.209954461142516, + "velocityY": -1.9998424138157533, + "timestamp": 7.657064959758385 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.08403051576382121, - "angularVelocity": 0.000006193819237400721, - "velocityX": 3.3339066927233554, - "velocityY": -1.7855635206200389, - "timestamp": 7.713804895112723 - }, - { - "x": 2.9656624873675215, - "y": 1.523823440642391, - "heading": -0.08403020563552344, - "angularVelocity": 0.0000051460695851667095, - "velocityX": 3.449910187925088, - "velocityY": -1.5496108793286638, - "timestamp": 7.7740699756075236 - }, - { - "x": 3.179557704009846, - "y": 1.4451070785194118, - "heading": -0.08403001029754076, - "angularVelocity": 0.0000032413128973800796, - "velocityX": 3.5492397070767696, - "velocityY": -1.306168704607808, - "timestamp": 7.834335056102324 - }, - { - "x": 3.3984052007772028, - "y": 1.3814422389869163, - "heading": -0.0840298717822531, - "angularVelocity": 0.0000022984336290020716, - "velocityX": 3.631414659542976, - "velocityY": -1.0564134156924954, - "timestamp": 7.894600136597124 - }, - { - "x": 3.6211472313814954, - "y": 1.3331366287340103, - "heading": -0.08402976608446186, - "angularVelocity": 0.000001753881192375512, - "velocityX": 3.6960380501525916, - "velocityY": -0.8015522398094749, - "timestamp": 7.954865217091925 - }, - { - "x": 3.846707230492903, - "y": 1.3004237167488812, - "heading": -0.08402968105378755, - "angularVelocity": 0.0000014109443416028125, - "velocityX": 3.7427976078264336, - "velocityY": -0.5428170296387713, - "timestamp": 8.015130297586726 - }, - { - "x": 4.073995015295796, - "y": 1.2834616061839175, - "heading": -0.08402960975207145, - "angularVelocity": 0.0000011831348355128416, - "velocityX": 3.7714673727599504, - "velocityY": -0.2814583574052856, - "timestamp": 8.075395378081527 - }, - { - "x": 4.301912054355543, - "y": 1.282332269070522, - "heading": -0.08402954787131725, - "angularVelocity": 0.0000010268094506712622, - "velocityX": 3.78190881333695, - "velocityY": -0.018739493984294066, - "timestamp": 8.135660458576329 - }, - { - "x": 4.529356777936335, - "y": 1.2970411469267544, - "heading": -0.08402949255215464, - "angularVelocity": 9.179306183548953e-7, - "velocityX": 3.7740715139410477, - "velocityY": 0.2440696624889034, - "timestamp": 8.19592553907113 - }, - { - "x": 4.755229905146536, - "y": 1.32751711580047, - "heading": -0.08402944177914254, - "angularVelocity": 8.424947198098048e-7, - "velocityX": 3.7479934541809494, - "velocityY": 0.5056986338273413, - "timestamp": 8.256190619565931 - }, - { - "x": 4.978439767588294, - "y": 1.373612800948183, - "heading": -0.08402939404329389, - "angularVelocity": 7.920979819424461e-7, - "velocityX": 3.7038009508842378, - "velocityY": 0.7648821634228096, - "timestamp": 8.316455700060732 - }, - { - "x": 5.197907642868858, - "y": 1.435105133457297, - "heading": -0.08402934813807361, - "angularVelocity": 7.617217119968608e-7, - "velocityX": 3.6417088217363065, - "velocityY": 1.0203642308985088, - "timestamp": 8.376720780555534 - }, - { - "x": 5.41257465992254, - "y": 1.51169186312203, - "heading": -0.08402930302434115, - "angularVelocity": 7.485882719571497e-7, - "velocityX": 3.562046466895548, - "velocityY": 1.2708309527826855, - "timestamp": 8.436985861050335 - }, - { - "x": 5.626738902818376, - "y": 1.589673479175761, - "heading": -0.0840292579538456, - "angularVelocity": 7.478708262765028e-7, - "velocityX": 3.5537037557647206, - "velocityY": 1.2939768007189265, - "timestamp": 8.497250941545136 - }, - { - "x": 5.8409031120271635, - "y": 1.6676551877456105, - "heading": -0.0840292128833514, - "angularVelocity": 7.478708040156826e-7, - "velocityX": 3.5537031967834927, - "velocityY": 1.2939783358719235, - "timestamp": 8.557516022039938 - }, - { - "x": 6.055067321252931, - "y": 1.745636896268829, - "heading": -0.08402916781285703, - "angularVelocity": 7.478708068018038e-7, - "velocityX": 3.5537031970652393, - "velocityY": 1.2939783350981522, - "timestamp": 8.617781102534739 - }, - { - "x": 6.269231822230628, - "y": 1.8236178035375321, - "heading": -0.08402912274221092, - "angularVelocity": 7.478733249018271e-7, - "velocityX": 3.5537080382091872, - "velocityY": 1.2939650395958724, - "timestamp": 8.67804618302954 - }, - { - "x": 6.484552473362613, - "y": 1.8927933712391167, - "heading": -0.0785738032137305, - "angularVelocity": 0.09052206491205224, - "velocityX": 3.572892450555367, - "velocityY": 1.1478548959633867, - "timestamp": 8.738311263524341 - }, - { - "x": 6.6874523544155435, - "y": 1.9584473392764923, - "heading": -0.032553192185028265, - "angularVelocity": 0.7636364317587296, - "velocityX": 3.3667901774467204, - "velocityY": 1.0894197352485022, - "timestamp": 8.798576344019143 - }, - { - "x": 6.875915672715089, - "y": 2.018652626020107, - "heading": 0.028144572860875648, - "angularVelocity": 1.0071796892586995, - "velocityX": 3.1272391366971615, - "velocityY": 0.9990078209355214, - "timestamp": 8.858841424513944 - }, - { - "x": 7.049532373282227, - "y": 2.073323850727038, - "heading": 0.08948102802542592, - "angularVelocity": 1.0177777024597514, - "velocityX": 2.8808839072589665, - "velocityY": 0.9071791534676087, - "timestamp": 8.919106505008745 - }, - { - "x": 7.208331847820926, - "y": 2.1225088918275805, - "heading": 0.1460274817443076, - "angularVelocity": 0.9382954980664153, - "velocityX": 2.635016384859868, - "velocityY": 0.8161449498899447, - "timestamp": 8.979371585503547 - }, - { - "x": 7.352376622997373, - "y": 2.1662517609495984, - "heading": 0.19476972653717045, - "angularVelocity": 0.8087974726436816, - "velocityX": 2.3901863897597297, - "velocityY": 0.72584104696901, - "timestamp": 9.039636665998348 + "heading": -0.0840305094244903, + "angularVelocity": 0.000006193819334023837, + "velocityX": 3.333906680775301, + "velocityY": -1.7855635028764607, + "timestamp": 7.713804923161213 + }, + { + "x": 2.9656624873135113, + "y": 1.5238234413872127, + "heading": -0.08403019929618429, + "angularVelocity": 0.000005146069704041112, + "velocityX": 3.449910175144429, + "velocityY": -1.5496108616313848, + "timestamp": 7.774070003863618 + }, + { + "x": 3.179557703869071, + "y": 1.4451070800547587, + "heading": -0.08403000395819739, + "angularVelocity": 0.000003241312956351986, + "velocityX": 3.549239693410422, + "velocityY": -1.306168686990774, + "timestamp": 7.834335084566023 + }, + { + "x": 3.398405200510204, + "y": 1.3814422413572929, + "heading": -0.084029865442907, + "angularVelocity": 0.000002298433666202308, + "velocityX": 3.631414644938793, + "velocityY": -1.0564133981973587, + "timestamp": 7.894600165268428 + }, + { + "x": 3.6211472309421358, + "y": 1.3331366319820044, + "heading": -0.08402975974511386, + "angularVelocity": 0.0000017538812179176465, + "velocityX": 3.696038034560229, + "velocityY": -0.8015522224856215, + "timestamp": 7.954865245970833 + }, + { + "x": 3.8467072298284624, + "y": 1.3004237209144467, + "heading": -0.08402967471443804, + "angularVelocity": 0.0000014109443619153728, + "velocityX": 3.742797591198178, + "velocityY": -0.5428170125432534, + "timestamp": 8.015130326673239 + }, + { + "x": 4.07399501434713, + "y": 1.283461611303661, + "heading": -0.08402960341272074, + "angularVelocity": 0.000001183134851133743, + "velocityX": 3.771467355051538, + "velocityY": -0.28145834060267944, + "timestamp": 8.075395407375645 + }, + { + "x": 4.301912053057315, + "y": 1.282332275177001, + "heading": -0.08402954153196555, + "angularVelocity": 0.000001026809463787284, + "velocityX": 3.7819087945083893, + "velocityY": -0.01873947754649237, + "timestamp": 8.13566048807805 + }, + { + "x": 4.529356776217307, + "y": 1.2970411540477857, + "heading": -0.08402948621280212, + "angularVelocity": 9.179306286652721e-7, + "velocityX": 3.774071493957432, + "velocityY": 0.2440696784829437, + "timestamp": 8.195925568780456 + }, + { + "x": 4.755229902929938, + "y": 1.3275171239584524, + "heading": -0.08402943543978927, + "angularVelocity": 8.424947292982353e-7, + "velocityX": 3.747993433013281, + "velocityY": 0.505698649291784, + "timestamp": 8.256190649482862 + }, + { + "x": 4.978439764792266, + "y": 1.3736128101593832, + "heading": -0.08402938770393992, + "angularVelocity": 7.920979912088378e-7, + "velocityX": 3.703800928510481, + "velocityY": 0.7648821782643186, + "timestamp": 8.316455730185268 + }, + { + "x": 5.197907639407193, + "y": 1.435105143730422, + "heading": -0.08402934179871885, + "angularVelocity": 7.617217222606304e-7, + "velocityX": 3.6417087981459852, + "velocityY": 1.020364245004398, + "timestamp": 8.376720810887674 + }, + { + "x": 5.4125746557646295, + "y": 1.5116918742918497, + "heading": -0.08402929668498581, + "angularVelocity": 7.485882790977605e-7, + "velocityX": 3.562046443071779, + "velocityY": 1.270830963284032, + "timestamp": 8.43698589159008 + }, + { + "x": 5.626738899704859, + "y": 1.5896734864413642, + "heading": -0.08402925161448958, + "angularVelocity": 7.478708348501504e-7, + "velocityX": 3.553703760852683, + "velocityY": 1.293976731477308, + "timestamp": 8.497250972292486 + }, + { + "x": 5.840903108372873, + "y": 1.667655195460401, + "heading": -0.08402920654399476, + "angularVelocity": 7.478708115803468e-7, + "velocityX": 3.553703175568263, + "velocityY": 1.2939783388678752, + "timestamp": 8.557516052994892 + }, + { + "x": 6.055067317059206, + "y": 1.7456369044291342, + "heading": -0.08402916147349976, + "angularVelocity": 7.478708148850431e-7, + "velocityX": 3.5537031758721978, + "velocityY": 1.293978338033166, + "timestamp": 8.617781133697298 + }, + { + "x": 6.269231826783721, + "y": 1.8236177866398526, + "heading": -0.08402911640284853, + "angularVelocity": 7.47873406932242e-7, + "velocityX": 3.5537081711062477, + "velocityY": 1.2939646193422618, + "timestamp": 8.678046214399703 + }, + { + "x": 6.484552476173004, + "y": 1.8927933599363171, + "heading": -0.07857379817757901, + "angularVelocity": 0.09052204297557322, + "velocityX": 3.5728924093299965, + "velocityY": 1.1478549848470312, + "timestamp": 8.73831129510211 + }, + { + "x": 6.687452356091927, + "y": 1.958447331788137, + "heading": -0.03255318883880569, + "angularVelocity": 0.763636401086517, + "velocityX": 3.3667901470316046, + "velocityY": 1.089419794790041, + "timestamp": 8.798576375804515 + }, + { + "x": 6.875915673704612, + "y": 2.0186526211606592, + "heading": 0.028144575028097817, + "angularVelocity": 1.0071796662255343, + "velocityX": 3.127239114526933, + "velocityY": 0.9990078611164873, + "timestamp": 8.858841456506921 + }, + { + "x": 7.0495323738250235, + "y": 2.0733238477692897, + "heading": 0.08948102939748631, + "angularVelocity": 1.0177776857592493, + "velocityX": 2.880883889922049, + "velocityY": 0.9071791818980943, + "timestamp": 8.919106537209327 + }, + { + "x": 7.208331848074836, + "y": 2.122508890246591, + "heading": 0.14602748254405165, + "angularVelocity": 0.938295485337472, + "velocityX": 2.635016370989014, + "velocityY": 0.8161449699234864, + "timestamp": 8.979371617911733 + }, + { + "x": 7.352376623078616, + "y": 2.166251760331207, + "heading": 0.19476972690009334, + "angularVelocity": 0.8087974626091599, + "velocityX": 2.39018637866075, + "velocityY": 0.7258410604413249, + "timestamp": 9.039636698614139 }, { "x": 7.481724262237549, "y": 2.2045881748199463, "heading": 0.2337432969576531, - "angularVelocity": 0.6467023706015796, - "velocityX": 2.146311565141514, - "velocityY": 0.6361298044504436, - "timestamp": 9.099901746493149 - }, - { - "x": 7.603777233505286, - "y": 2.239565945400217, - "heading": 0.2627306680277325, - "angularVelocity": 0.44772760939813167, - "velocityX": 1.8851825132237892, - "velocityY": 0.540252980034664, - "timestamp": 9.164645064666104 - }, - { - "x": 7.70877923669531, - "y": 2.2685458571337813, - "heading": 0.28047750188422, - "angularVelocity": 0.2741106628035178, - "velocityX": 1.621819921393637, - "velocityY": 0.4476123954003698, - "timestamp": 9.229388382839058 - }, - { - "x": 7.796631047750002, - "y": 2.291686366419086, - "heading": 0.28817114093339696, - "angularVelocity": 0.11883294317143692, - "velocityX": 1.3569247535321998, - "velocityY": 0.3574192663942211, - "timestamp": 9.294131701012013 - }, - { - "x": 7.867260828377704, - "y": 2.309110963970804, - "heading": 0.2867085865097153, - "angularVelocity": -0.022590044269504023, - "velocityX": 1.0909199994819943, - "velocityY": 0.2691335267242617, - "timestamp": 9.358875019184968 - }, - { - "x": 7.920614376230513, - "y": 2.320918360587503, - "heading": 0.2767910958513383, - "angularVelocity": -0.15318168636157245, - "velocityX": 0.824078057140675, - "velocityY": 0.1823724354867997, - "timestamp": 9.423618337357922 - }, - { - "x": 7.956649448780364, - "y": 2.327189146390105, - "heading": 0.25898192892634386, - "angularVelocity": -0.27507343502876075, - "velocityX": 0.5565836532132435, - "velocityY": 0.09685610777394495, - "timestamp": 9.488361655530877 + "angularVelocity": 0.646702362351673, + "velocityX": 2.1463115563996884, + "velocityY": 0.6361298125202616, + "timestamp": 9.099901779316545 + }, + { + "x": 7.603777232623247, + "y": 2.2395659454817083, + "heading": 0.2627306675325177, + "angularVelocity": 0.44772760396065475, + "velocityX": 1.8851825089114527, + "velocityY": 0.54025298396176, + "timestamp": 9.164645097169718 + }, + { + "x": 7.708779235262183, + "y": 2.2685458571632657, + "heading": 0.28047750108878416, + "angularVelocity": 0.27411065952030855, + "velocityX": 1.6218199208922495, + "velocityY": 0.4476123968079393, + "timestamp": 9.22938841502289 + }, + { + "x": 7.796631046068626, + "y": 2.2916863663414535, + "heading": 0.2881711400063891, + "angularVelocity": 0.1188329417261678, + "velocityX": 1.3569247563999605, + "velocityY": 0.3574192665050968, + "timestamp": 9.294131732876064 + }, + { + "x": 7.867260826730005, + "y": 2.309110963792019, + "heading": 0.28670858560451956, + "angularVelocity": -0.02259004404417999, + "velocityX": 1.0909200053904464, + "velocityY": 0.2691335264912042, + "timestamp": 9.358875050729237 + }, + { + "x": 7.920614374882339, + "y": 2.320918360362405, + "heading": 0.27679109511178074, + "angularVelocity": -0.1531816845597854, + "velocityX": 0.8240780658373096, + "velocityY": 0.18237243567224945, + "timestamp": 9.42361836858241 + }, + { + "x": 7.95664944798485, + "y": 2.3271891462135073, + "heading": 0.25898192848994617, + "angularVelocity": -0.27507343170491383, + "velocityX": 0.5565836644984964, + "velocityY": 0.09685610900144763, + "timestamp": 9.488361686435583 }, { "x": 7.975332260131836, "y": 2.3279902935028076, "heading": 0.2337432969576531, - "angularVelocity": -0.38982604971324913, - "velocityX": 0.2885674055438917, - "velocityY": 0.012374205328225838, - "timestamp": 9.553104973703832 - }, - { - "x": 7.972208451820267, - "y": 2.320885188130861, - "heading": 0.19238106068409147, - "angularVelocity": -0.5227816803364386, - "velocityX": -0.039482143745094074, - "velocityY": -0.08980217850764118, - "timestamp": 9.632224494967359 - }, - { - "x": 7.9431197679460075, - "y": 2.3056970040477323, - "heading": 0.14088004342725263, - "angularVelocity": -0.6509268058549238, - "velocityX": -0.3676549530345614, - "velocityY": -0.19196506551829257, - "timestamp": 9.711344016230886 - }, - { - "x": 7.888054782175393, - "y": 2.282427289475609, - "heading": 0.07969868788669623, - "angularVelocity": -0.7732776255909873, - "velocityX": -0.695972180964121, - "velocityY": -0.2941083843849025, - "timestamp": 9.790463537494412 - }, - { - "x": 7.806999942498959, - "y": 2.251078456184475, - "heading": 0.009399898258955654, - "angularVelocity": -0.8885138396324784, - "velocityX": -1.0244606941750847, - "velocityY": -0.39622122063553056, - "timestamp": 9.86958305875794 - }, - { - "x": 7.699938927127902, - "y": 2.2116544317191673, - "heading": -0.06930512925958275, - "angularVelocity": -0.9947611697041394, - "velocityX": -1.353155500201559, - "velocityY": -0.49828441623143704, - "timestamp": 9.948702580021466 - }, - { - "x": 7.56685172715319, - "y": 2.1641617607796966, - "heading": -0.15547855671843705, - "angularVelocity": -1.089155066697531, - "velocityX": -1.682103200946216, - "velocityY": -0.6002648926714905, - "timestamp": 10.027822101284993 - }, - { - "x": 7.407713494064462, - "y": 2.1086115226344657, - "heading": -0.24780538645569025, - "angularVelocity": -1.1669285691167814, - "velocityX": -2.011364964642267, - "velocityY": -0.702105337066011, - "timestamp": 10.10694162254852 - }, - { - "x": 7.222493943520295, - "y": 2.045023086580113, - "heading": -0.3442810817864959, - "angularVelocity": -1.2193665202986854, - "velocityX": -2.3410094953335943, - "velocityY": -0.8037009708710834, - "timestamp": 10.186061143812047 - }, - { - "x": 7.0111623875747595, - "y": 1.9734335261753837, - "heading": -0.4414827777687788, - "angularVelocity": -1.2285425193427022, - "velocityX": -2.6710418942202843, - "velocityY": -0.9048280280448349, - "timestamp": 10.265180665075574 - }, - { - "x": 6.773734962048751, - "y": 1.893931948844204, - "heading": -0.5324651154460041, - "angularVelocity": -1.1499353917244444, - "velocityX": -3.0008703507595227, - "velocityY": -1.0048288470601288, - "timestamp": 10.344300186339101 - }, - { - "x": 6.510837595414292, - "y": 1.8068437015263887, - "heading": -0.5976366951472135, - "angularVelocity": -0.8237104909184032, - "velocityX": -3.3227876311184024, - "velocityY": -1.1007175716817783, - "timestamp": 10.423419707602628 - }, - { - "x": 6.227373100797986, - "y": 1.7110085398670838, - "heading": -0.597636758184297, - "angularVelocity": -7.967323658608173e-7, - "velocityX": -3.5827377376583796, - "velocityY": -1.2112707474568936, - "timestamp": 10.502539228866155 - }, - { - "x": 5.941831354312318, - "y": 1.6215523108704284, - "heading": -0.5976367779578059, - "angularVelocity": -2.499194721356179e-7, - "velocityX": -3.6089923438060088, - "velocityY": -1.1306467426502582, - "timestamp": 10.581658750129682 - }, - { - "x": 5.656289545112644, - "y": 1.532096282055343, - "heading": -0.5976367977313131, - "angularVelocity": -2.499194483006569e-7, - "velocityX": -3.6089931364549517, - "velocityY": -1.130644212534215, - "timestamp": 10.660778271393209 - }, - { - "x": 5.370747722448539, - "y": 1.442640296218458, - "heading": -0.5976368175048198, - "angularVelocity": -2.499194443736624e-7, - "velocityX": -3.6089933066333257, - "velocityY": -1.130643669328191, - "timestamp": 10.739897792656736 - }, - { - "x": 5.084751687474933, - "y": 1.3546472631644115, - "heading": -0.5976368372766676, - "angularVelocity": -2.4989847693362694e-7, - "velocityX": -3.6147341440682292, - "velocityY": -1.112153254327249, - "timestamp": 10.819017313920263 - }, - { - "x": 4.791925527807106, - "y": 1.2930896001142946, - "heading": -0.5976368572220272, - "angularVelocity": -2.52091508667939e-7, - "velocityX": -3.7010608126974813, - "velocityY": -0.7780338160172059, - "timestamp": 10.89813683518379 - }, - { - "x": 4.494706489260304, - "y": 1.2584868813594625, - "heading": -0.5976368780658494, - "angularVelocity": -2.63447274854176e-7, - "velocityX": -3.756582873610133, - "velocityY": -0.4373474232683845, - "timestamp": 10.977256356447317 - }, - { - "x": 4.195570491935266, - "y": 1.2511276025347224, - "heading": -0.597636900694303, - "angularVelocity": -2.860034177860552e-7, - "velocityX": -3.780811518420208, - "velocityY": -0.09301470366874595, - "timestamp": 11.056375877710844 - }, - { - "x": 3.897009479956395, - "y": 1.2710731343763437, - "heading": -0.597636926337917, - "angularVelocity": -3.24112350243062e-7, - "velocityX": -3.7735442177972423, - "velocityY": 0.2520936871595517, - "timestamp": 11.13549539897437 - }, - { - "x": 3.6015106196507847, - "y": 1.3181573465478027, - "heading": -0.5976369569136154, - "angularVelocity": -3.8644948741127607e-7, - "velocityX": -3.7348413588269285, - "velocityY": 0.5951023390881358, - "timestamp": 11.214614920237898 - }, - { - "x": 3.311535572783971, - "y": 1.3919880157013045, - "heading": -0.5976369957684174, - "angularVelocity": -4.910899536256082e-7, - "velocityX": -3.6650252963611574, - "velocityY": 0.933153638627185, - "timestamp": 11.293734441501424 - }, - { - "x": 3.029499988052695, - "y": 1.4919500996437052, - "heading": -0.5976370495734, - "angularVelocity": -6.80046867328716e-7, - "velocityX": -3.5646775944445412, - "velocityY": 1.263431354816361, - "timestamp": 11.372853962764951 + "angularVelocity": -0.38982604489825085, + "velocityX": 0.28856741925638435, + "velocityY": 0.012374208116996958, + "timestamp": 9.553105004288756 + }, + { + "x": 7.972208452847698, + "y": 2.3208851884260335, + "heading": 0.1923810608861147, + "angularVelocity": -0.5227816750662158, + "velocityX": -0.039482130554085705, + "velocityY": -0.08980217431023955, + "timestamp": 9.632224525963458 + }, + { + "x": 7.943119769823989, + "y": 2.3056970047129943, + "heading": 0.14088004377458682, + "angularVelocity": -0.650926800635539, + "velocityX": -0.367654940373712, + "velocityY": -0.19196505984306744, + "timestamp": 9.71134404763816 + }, + { + "x": 7.888054784726001, + "y": 2.282427290592543, + "heading": 0.07969868827872208, + "angularVelocity": -0.7732776210074993, + "velocityX": -0.6959721688458461, + "velocityY": -0.29410837714772914, + "timestamp": 9.790463569312863 + }, + { + "x": 7.80699994554277, + "y": 2.251078457842943, + "heading": 0.009399898543879681, + "angularVelocity": -0.8885138363686558, + "velocityX": -1.0244606826174518, + "velocityY": -0.3962212117319218, + "timestamp": 9.869583090987565 + }, + { + "x": 7.6999389304832775, + "y": 2.211654434019612, + "heading": -0.0693051292941164, + "angularVelocity": -0.9947611685721511, + "velocityX": -1.3531554892314888, + "velocityY": -0.4982844055278961, + "timestamp": 9.948702612662267 + }, + { + "x": 7.566851730635094, + "y": 2.1641617638366206, + "heading": -0.15547855735771587, + "angularVelocity": -1.0891550686807683, + "velocityX": -1.6821031906053239, + "velocityY": -0.6002648799907612, + "timestamp": 10.027822134336969 + }, + { + "x": 7.407713497482348, + "y": 2.1086115265820684, + "heading": -0.24780538807324573, + "angularVelocity": -1.1669285754169436, + "velocityX": -2.011364954998582, + "velocityY": -0.70210532215988, + "timestamp": 10.10694165601167 + }, + { + "x": 7.22249394667395, + "y": 2.0450230915823275, + "heading": -0.3442810848564748, + "angularVelocity": -1.2193665323191243, + "velocityX": -2.3410094865073168, + "velocityY": -0.803700953365003, + "timestamp": 10.186061177686373 + }, + { + "x": 7.0111623902454, + "y": 1.973433532446374, + "heading": -0.441482782839164, + "angularVelocity": -1.2285425382414696, + "velocityX": -2.671041886444093, + "velocityY": -0.9048280073063681, + "timestamp": 10.265180699361075 + }, + { + "x": 6.773734963981195, + "y": 1.8939319567020383, + "heading": -0.5324651228432857, + "angularVelocity": -1.1499354151582672, + "velocityX": -3.0008703444945404, + "velocityY": -1.0048288217818704, + "timestamp": 10.344300221035777 + }, + { + "x": 6.510837596418812, + "y": 1.8068437116330571, + "heading": -0.5976367021926547, + "angularVelocity": -0.8237104821907345, + "velocityX": -3.3227876255784223, + "velocityY": -1.1007175375382399, + "timestamp": 10.423419742710479 + }, + { + "x": 6.227373099976041, + "y": 1.711008555192143, + "heading": -0.5976367652296872, + "angularVelocity": -7.967317193878993e-7, + "velocityX": -3.582737742124223, + "velocityY": -1.2112706752062792, + "timestamp": 10.502539264385181 + }, + { + "x": 5.941831356009296, + "y": 1.6215523179581028, + "heading": -0.5976367850031955, + "angularVelocity": -2.499194616123805e-7, + "velocityX": -3.6089922932135883, + "velocityY": -1.1306468408876076, + "timestamp": 10.581658786059883 + }, + { + "x": 5.656289546537612, + "y": 1.5320962898142203, + "heading": -0.5976368047767018, + "angularVelocity": -2.4991943644041576e-7, + "velocityX": -3.608993121137454, + "velocityY": -1.1306441981749955, + "timestamp": 10.660778307734585 + }, + { + "x": 5.370747722964019, + "y": 1.442640306683351, + "heading": -0.5976368245502077, + "angularVelocity": -2.4991943318222364e-7, + "velocityX": -3.6089932993729574, + "velocityY": -1.130643629250762, + "timestamp": 10.739897829409287 + }, + { + "x": 5.084751688404252, + "y": 1.3546472720839122, + "heading": -0.5976368443220548, + "angularVelocity": -2.4989846513470833e-7, + "velocityX": -3.614734120052362, + "velocityY": -1.1121532680799078, + "timestamp": 10.819017351083989 + }, + { + "x": 4.791925529016698, + "y": 1.2930896074141898, + "heading": -0.5976368642674134, + "angularVelocity": -2.5209149670001704e-7, + "velocityX": -3.7010607899211223, + "velocityY": -0.7780338324442274, + "timestamp": 10.898136872758691 + }, + { + "x": 4.494706490584963, + "y": 1.2584868871615749, + "heading": -0.5976368851112348, + "angularVelocity": -2.6344726295956754e-7, + "velocityX": -3.7565828526333167, + "velocityY": -0.4373474399261832, + "timestamp": 10.977256394433393 + }, + { + "x": 4.195570493234456, + "y": 1.2511276069767714, + "heading": -0.5976369077396874, + "angularVelocity": -2.860034028815913e-7, + "velocityX": -3.7808114990937063, + "velocityY": -0.09301472037534324, + "timestamp": 11.056375916108095 + }, + { + "x": 3.8970094811156857, + "y": 1.2710731376080628, + "heading": -0.5976369333832999, + "angularVelocity": -3.2411233223341237e-7, + "velocityX": -3.7735441999548045, + "velocityY": 0.2520936705519675, + "timestamp": 11.135495437782797 + }, + { + "x": 3.6015106205827614, + "y": 1.3181573487272922, + "heading": -0.5976369639589968, + "angularVelocity": -3.8644946446935137e-7, + "velocityX": -3.734841342290479, + "velocityY": 0.5951023226962214, + "timestamp": 11.2146149594575 + }, + { + "x": 3.311535573428697, + "y": 1.3919880169913565, + "heading": -0.5976370028137967, + "angularVelocity": -4.910899244313184e-7, + "velocityX": -3.665025280945076, + "velocityY": 0.9331536225360112, + "timestamp": 11.293734481132201 + }, + { + "x": 3.0294999883777347, + "y": 1.4919501002081306, + "heading": -0.597637056618776, + "angularVelocity": -6.800468215926745e-7, + "velocityX": -3.564677579959917, + "velocityY": 1.263431339079197, + "timestamp": 11.372854002806903 }, { "x": 2.757753372192383, "y": 1.617210865020752, - "heading": -0.5976371219829064, - "angularVelocity": -9.151914125801383e-7, - "velocityX": -3.434634228323893, - "velocityY": 1.583184066038948, - "timestamp": 11.451973484028478 - }, - { - "x": 2.562615182382705, - "y": 1.7237517111402614, - "heading": -0.5976372044989885, - "angularVelocity": -0.000001403654433041047, - "velocityX": -3.319432744203681, - "velocityY": 1.8123319353797018, - "timestamp": 11.510760091514479 - }, - { - "x": 2.3751466995867516, - "y": 1.8432734201994623, - "heading": -0.5976372636960788, - "angularVelocity": -0.000001006982590926024, - "velocityX": -3.188965834447897, - "velocityY": 2.033145203823246, - "timestamp": 11.56954669900048 - }, - { - "x": 2.1962100914661193, - "y": 1.9752263102479175, - "heading": -0.5976373093137471, - "angularVelocity": -7.759874278787006e-7, - "velocityX": -3.04383286896162, - "velocityY": 2.2446080100791628, - "timestamp": 11.62833330648648 - }, - { - "x": 2.026628285101079, - "y": 2.1190035289778706, - "heading": -0.5976373463774368, - "angularVelocity": -6.304784584021761e-7, - "velocityX": -2.884701356604495, - "velocityY": 2.4457478476571914, - "timestamp": 11.68711991397248 - }, - { - "x": 1.8671811829950287, - "y": 2.2739438442752706, - "heading": -0.5976373777665476, - "angularVelocity": -5.339500279584693e-7, - "velocityX": -2.7123031745627153, - "velocityY": 2.6356396792296386, - "timestamp": 11.745906521458481 - }, - { - "x": 1.7186020763048488, - "y": 2.439334685025019, - "heading": -0.5976374052703787, - "angularVelocity": -4.678587919148875e-7, - "velocityX": -2.5274312134028762, - "velocityY": 2.813410193625064, - "timestamp": 11.804693128944482 - }, - { - "x": 1.5815742720495027, - "y": 2.6144154181152923, - "heading": -0.597637430079118, - "angularVelocity": -4.220134537614776e-7, - "velocityX": -2.330935737157126, - "velocityY": 2.978241823734581, - "timestamp": 11.863479736430483 - }, - { - "x": 1.456727954325337, - "y": 2.7983808405830617, - "heading": -0.5976374777093088, - "angularVelocity": -8.102217978593512e-7, - "velocityX": -2.123720402710731, - "velocityY": 3.129376406209191, - "timestamp": 11.922266343916483 - }, - { - "x": 1.3455561973341574, - "y": 2.9885105140912174, - "heading": -0.6039795556256176, - "angularVelocity": -0.10788303982023785, - "velocityX": -1.8911068650739042, - "velocityY": 3.234234490456602, - "timestamp": 11.981052951402484 - }, - { - "x": 1.246887053690835, - "y": 3.1733277865183314, - "heading": -0.6383468335999772, - "angularVelocity": -0.584610669743856, - "velocityX": -1.6784289460285728, - "velocityY": 3.1438669508378774, - "timestamp": 12.039839558888485 - }, - { - "x": 1.158565022655154, - "y": 3.3474027501759167, - "heading": -0.6816254514815488, - "angularVelocity": -0.736198595775039, - "velocityX": -1.5024175541464038, - "velocityY": 2.9611330046395477, - "timestamp": 12.098626166374485 - }, - { - "x": 1.0790333486785466, - "y": 3.5092497722088574, - "heading": -0.7278331515121171, - "angularVelocity": -0.7860242665231638, - "velocityX": -1.3528876282841755, - "velocityY": 2.7531274375968073, - "timestamp": 12.157412773860486 - }, - { - "x": 1.0074427197106477, - "y": 3.6582801720807683, - "heading": -0.7741804064083878, - "angularVelocity": -0.7883981892867038, - "velocityX": -1.217805075500369, - "velocityY": 2.5351080160120225, - "timestamp": 12.216199381346486 - }, - { - "x": 0.9432732996874974, - "y": 3.7941912605500616, - "heading": -0.8190469180938782, - "angularVelocity": -0.7632097446033984, - "velocityX": -1.0915652861654275, - "velocityY": 2.3119396454653414, - "timestamp": 12.274985988832487 - }, - { - "x": 0.8861770840921692, - "y": 3.9168021213870996, - "heading": -0.8613760150853045, - "angularVelocity": -0.7200466024767106, - "velocityX": -0.971245289310589, - "velocityY": 2.085693767347223, - "timestamp": 12.333772596318488 - }, - { - "x": 0.8359056858624994, - "y": 4.025993630774074, - "heading": -0.9004245404660403, - "angularVelocity": -0.6642418579781981, - "velocityX": -0.8551505245755496, - "velocityY": 1.857421512424882, - "timestamp": 12.392559203804488 - }, - { - "x": 0.7922732558571624, - "y": 4.121681912181887, - "heading": -0.9356409495537121, - "angularVelocity": -0.5990549649604897, - "velocityX": -0.7422171795800264, - "velocityY": 1.627722460946589, - "timestamp": 12.451345811290489 - }, - { - "x": 0.7551356458246514, - "y": 4.203804939460416, - "heading": -0.9665991512691937, - "angularVelocity": -0.5266199741642557, - "velocityX": -0.6317358939509348, - "velocityY": 1.3969683026544202, - "timestamp": 12.51013241877649 - }, - { - "x": 0.7243778479811152, - "y": 4.272315098350931, - "heading": -0.9929596716344966, - "angularVelocity": -0.44841030113160957, - "velocityX": -0.5232109686013307, - "velocityY": 1.1654041935798078, - "timestamp": 12.56891902626249 - }, - { - "x": 0.6999059919554825, - "y": 4.327174746467568, - "heading": -1.0144454156380707, - "angularVelocity": -0.3654870543208486, - "velocityX": -0.41628284182686576, - "velocityY": 0.9331997620325617, - "timestamp": 12.62770563374849 - }, - { - "x": 0.6816420121384978, - "y": 4.368353408748977, - "heading": -1.0308257653496888, - "angularVelocity": -0.27864084035669345, - "velocityX": -0.31068266392705546, - "velocityY": 0.7004769290559099, - "timestamp": 12.686492241234491 - }, - { - "x": 0.6695199611090151, - "y": 4.395825923599214, - "heading": -1.0419057024338247, - "angularVelocity": -0.1884772324508513, - "velocityX": -0.2062042963164626, - "velocityY": 0.46732608029439393, - "timestamp": 12.745278848720492 + "heading": -0.5976371290282776, + "angularVelocity": -9.151913479184684e-7, + "velocityX": -3.4346342145827435, + "velocityY": 1.5831840506775074, + "timestamp": 11.451973524481605 + }, + { + "x": 2.5626151824790466, + "y": 1.7237517105978553, + "heading": -0.5976372115443579, + "angularVelocity": -0.0000014036543940556534, + "velocityX": -3.319432731110931, + "velocityY": 1.8123319198994405, + "timestamp": 11.510760132170452 + }, + { + "x": 2.3751466997704354, + "y": 1.8432734191532605, + "heading": -0.5976372707414468, + "angularVelocity": -0.0000010069825624449078, + "velocityX": -3.188965821958426, + "velocityY": 2.0331451882378415, + "timestamp": 11.569546739859298 + }, + { + "x": 2.196210091733693, + "y": 1.9752263087350537, + "heading": -0.5976373163591139, + "angularVelocity": -7.759874050438566e-7, + "velocityX": -3.0438328570316586, + "velocityY": 2.244607994395761, + "timestamp": 11.628333347548145 + }, + { + "x": 2.0266282854543833, + "y": 2.1190035270334358, + "heading": -0.5976373534228027, + "angularVelocity": -6.304784410722591e-7, + "velocityX": -2.8847013451923114, + "velocityY": 2.445747831876676, + "timestamp": 11.68711995523699 + }, + { + "x": 1.867181183440892, + "y": 2.2739438419317777, + "heading": -0.5976373848119125, + "angularVelocity": -5.339500111409062e-7, + "velocityX": -2.712303163629251, + "velocityY": 2.635639663346953, + "timestamp": 11.745906562925837 + }, + { + "x": 1.718602076854733, + "y": 2.439334682311898, + "heading": -0.5976374123157431, + "angularVelocity": -4.678587798571888e-7, + "velocityX": -2.52743120291235, + "velocityY": 2.813410177629591, + "timestamp": 11.804693170614684 + }, + { + "x": 1.5815742727191044, + "y": 2.6144154150584233, + "heading": -0.5976374371244817, + "angularVelocity": -4.220134408077074e-7, + "velocityX": -2.3309357270776045, + "velocityY": 2.9782418076105714, + "timestamp": 11.86347977830353 + }, + { + "x": 1.4567279551341459, + "y": 2.7983808372043484, + "heading": -0.5976374847546617, + "angularVelocity": -8.102216113846116e-7, + "velocityX": -2.1237203930146884, + "velocityY": 3.12937638993629, + "timestamp": 11.922266385992376 + }, + { + "x": 1.345556198102205, + "y": 2.9885105109664996, + "heading": -0.6039795609021088, + "angularVelocity": -0.10788300935844368, + "velocityX": -1.891106859241893, + "velocityY": 3.2342344836173083, + "timestamp": 11.981052993681223 + }, + { + "x": 1.246887054415439, + "y": 3.17332778383869, + "heading": -0.6383468381311229, + "angularVelocity": -0.584610655047786, + "velocityX": -1.6784289409760504, + "velocityY": 3.1438669475608143, + "timestamp": 12.039839601370069 + }, + { + "x": 1.1585650233427731, + "y": 3.3474027479162674, + "heading": -0.6816254554461085, + "angularVelocity": -0.736198583596727, + "velocityX": -1.502417549591352, + "velocityY": 2.9611330015663175, + "timestamp": 12.098626209058915 + }, + { + "x": 1.079033349316087, + "y": 3.5092497703250873, + "heading": -0.7278331549531709, + "angularVelocity": -0.7860242549057459, + "velocityX": -1.352887624467824, + "velocityY": 2.753127434490931, + "timestamp": 12.157412816747762 + }, + { + "x": 1.0074427202851626, + "y": 3.6582801705311523, + "heading": -0.7741804093489261, + "angularVelocity": -0.7883981780521784, + "velocityX": -1.2178050723703648, + "velocityY": 2.535108012948657, + "timestamp": 12.216199424436608 + }, + { + "x": 0.9432733001900041, + "y": 3.7941912592964138, + "heading": -0.8190469205570853, + "angularVelocity": -0.7632097338501684, + "velocityX": -1.0915652836238168, + "velocityY": 2.3119396425224727, + "timestamp": 12.274986032125454 + }, + { + "x": 0.886177084517944, + "y": 3.9168021203940837, + "heading": -0.861376017099095, + "angularVelocity": -0.7200465923472629, + "velocityX": -0.9712452872645088, + "velocityY": 2.0856937645839286, + "timestamp": 12.3337726398143 + }, + { + "x": 0.835905686210625, + "y": 4.025993630008512, + "heading": -0.9004245420638279, + "angularVelocity": -0.6642418486097024, + "velocityX": -0.855150522945662, + "velocityY": 1.8574215098848674, + "timestamp": 12.392559247503147 + }, + { + "x": 0.79227325612998, + "y": 4.121681911612244, + "heading": -0.9356409507741646, + "angularVelocity": -0.599054956474685, + "velocityX": -0.7422171782999981, + "velocityY": 1.6277224586627488, + "timestamp": 12.451345855191994 + }, + { + "x": 0.7551356460272671, + "y": 4.203804939056435, + "heading": -0.9665991521557302, + "angularVelocity": -0.5266199666669827, + "velocityX": -0.6317358929652714, + "velocityY": 1.3969683006521094, + "timestamp": 12.51013246288084 + }, + { + "x": 0.7243778481209882, + "y": 4.272315098083365, + "heading": -0.9929596722347582, + "angularVelocity": -0.44841029471460875, + "velocityX": -0.5232109678632522, + "velocityY": 1.1654041918790294, + "timestamp": 12.568919070569686 + }, + { + "x": 0.6999059920420889, + "y": 4.327174746307989, + "heading": -1.0144454160034293, + "angularVelocity": -0.3654870490638522, + "velocityX": -0.4162828412965571, + "velocityY": 0.9331997606494288, + "timestamp": 12.627705678258533 + }, + { + "x": 0.6816420121830558, + "y": 4.368353408669628, + "heading": -1.0308257655348128, + "angularVelocity": -0.2786408363293121, + "velocityX": -0.3106826635702974, + "velocityY": 0.7004769280036421, + "timestamp": 12.686492285947379 + }, + { + "x": 0.6695199611242603, + "y": 4.3958259235729, + "heading": -1.0419057024962997, + "angularVelocity": -0.18847722971415903, + "velocityX": -0.2062042961035709, + "velocityY": 0.4673260795840121, + "timestamp": 12.745278893636225 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -0.09547069579077468, - "velocityX": -0.1026863106378516, - "velocityY": 0.23381596243572259, - "timestamp": 12.804065456206493 + "angularVelocity": -0.0954706943986077, + "velocityX": -0.10268631054285546, + "velocityY": 0.233815962076544, + "timestamp": 12.804065501325072 }, { "x": 0.6634833812713623, "y": 4.409571170806885, "heading": -1.0475181007536924, - "angularVelocity": -7.821518056781921e-37, - "velocityX": -6.302822662985753e-37, - "velocityY": 5.041148502478119e-38, - "timestamp": 12.862852063692493 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 5 - ], - "type": "StopPoint" + "angularVelocity": 7.129108746129837e-28, + "velocityX": 3.4723117091122335e-27, + "velocityY": 7.64903026201397e-27, + "timestamp": 12.862852109013918 } ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "AmpLanePSubASubDSubESub": { - "waypoints": [ + "trajectoryWaypoints": [ { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119463, + "timestamp": 0, + "isStopPoint": true, + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 24 }, { - "x": 2.6514174938201904, - "y": 6.946751594543457, + "timestamp": 1.3420802060316501, + "isStopPoint": false, + "x": 2.757753372192383, + "y": 1.617210865020752, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 + "headingConstrained": false, + "controlIntervalCount": 21 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119463, + "timestamp": 2.673367901531957, + "isStopPoint": false, + "x": 7.340087890625, + "y": 0.7580231428146362, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 32 + "controlIntervalCount": 7 }, { - "x": 7.323064804077148, - "y": 7.475617408752441, + "timestamp": 3.118573680916855, + "isStopPoint": false, + "x": 7.841280937194824, + "y": 0.7580231428146362, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 23 }, { - "x": 7.799044132232666, - "y": 7.457988739013672, + "timestamp": 4.954811112368041, + "isStopPoint": false, + "x": 2.757753372192383, + "y": 1.617210865020752, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 33 + "headingConstrained": false, + "controlIntervalCount": 24 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119463, + "timestamp": 6.35204580149334, + "isStopPoint": true, + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 28 + "controlIntervalCount": 24 }, { - "x": 5.7460856437683105, - "y": 6.598193168640137, + "timestamp": 7.713804923161213, + "isStopPoint": false, + "x": 2.757753372192383, + "y": 1.617210865020752, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 19 + "controlIntervalCount": 23 }, { - "x": 7.981563568115234, - "y": 5.947082996368408, - "heading": -0.6889234822214203, + "timestamp": 9.099901779316545, + "isStopPoint": false, + "x": 7.481724262237549, + "y": 2.2045881748199463, + "heading": 0.23374329695765314, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 7 }, { - "x": 5.7460856437683105, - "y": 6.598193168640137, + "timestamp": 9.553105004288756, + "isStopPoint": false, + "x": 7.975332260131836, + "y": 2.3279902935028076, + "heading": 0.23374329695765314, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "timestamp": 11.451973524481605, + "isStopPoint": false, + "x": 2.757753372192383, + "y": 1.617210865020752, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 28 + "controlIntervalCount": 24 + }, + { + "timestamp": 12.862852109013918, + "isStopPoint": true, + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 5 + ], + "type": "StopPoint", + "direction": 0 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "AmpLanePSubASubDSubESub": { + "waypoints": [ + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 2.6514174938201904, + "y": 6.946751594543457, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 32 + }, + { + "x": 7.323064804077148, + "y": 7.475617408752441, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 7.799044132232666, + "y": 7.457988739013672, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 33 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 28 + }, + { + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 19 + }, + { + "x": 7.981563568115234, + "y": 5.947082996368408, + "heading": -0.6889234822214203, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 28 }, { "x": 0.6769854426383972, @@ -9255,1882 +8926,3968 @@ "x": 0.6769854426383972, "y": 6.664690017700195, "heading": 0.9928942822119464, - "angularVelocity": 0, - "velocityX": 2.1425070675381243e-37, - "velocityY": 2.00620509434581e-36, + "angularVelocity": -3.9907195357987423e-28, + "velocityX": 1.1657419174076564e-26, + "velocityY": -4.7095973646167984e-27, + "timestamp": 0 + }, + { + "x": 0.6967295535340264, + "y": 6.667501793456135, + "heading": 0.9828931632504835, + "angularVelocity": -0.14737604788210726, + "velocityX": 0.2909483472755575, + "velocityY": 0.0414342035164189, + "timestamp": 0.06786122375505199 + }, + { + "x": 0.7362175895663237, + "y": 6.673125696213235, + "heading": 0.9628974920314577, + "angularVelocity": -0.2946553291051897, + "velocityX": 0.5818939572152577, + "velocityY": 0.08287358296691186, + "timestamp": 0.13572244751010398 + }, + { + "x": 0.7954495269531124, + "y": 6.681562379143333, + "heading": 0.9329256679355464, + "angularVelocity": -0.4416634778661797, + "velocityX": 0.8728392167018533, + "velocityY": 0.12432258752878259, + "timestamp": 0.20358367126515597 + }, + { + "x": 0.874425436057604, + "y": 6.692812849772419, + "heading": 0.8930052032172396, + "angularVelocity": -0.5882662072585287, + "velocityX": 1.163785513057627, + "velocityY": 0.16578643894920225, + "timestamp": 0.27144489502020797 + }, + { + "x": 0.9731453894647497, + "y": 6.7068785148517795, + "heading": 0.8431688201765567, + "angularVelocity": -0.7343867422811217, + "velocityX": 1.4547328790220406, + "velocityY": 0.2072710201945652, + "timestamp": 0.33930611877525996 + }, + { + "x": 1.0916093532830902, + "y": 6.723761203633637, + "heading": 0.7834495220073406, + "angularVelocity": -0.8800209436949645, + "velocityX": 1.745679745563404, + "velocityY": 0.24878255721994524, + "timestamp": 0.40716734253031195 + }, + { + "x": 1.229817076727268, + "y": 6.743463169981193, + "heading": 0.7138752049646239, + "angularVelocity": -1.0252440671251137, + "velocityX": 2.0366229165428056, + "velocityY": 0.29032730707408716, + "timestamp": 0.47502856628536394 + }, + { + "x": 1.387767999306249, + "y": 6.765987144870166, + "heading": 0.6344639927151499, + "angularVelocity": -1.1702001210015336, + "velocityX": 2.327557828151045, + "velocityY": 0.33191230046593584, + "timestamp": 0.5428897900404159 + }, + { + "x": 1.5654611542200556, + "y": 6.791337011842962, + "heading": 0.5452227484307262, + "angularVelocity": -1.3150550394809215, + "velocityX": 2.618478493037463, + "velocityY": 0.37355452156738955, + "timestamp": 0.6107510137954679 + }, + { + "x": 1.7628875214933553, + "y": 6.819592756978724, + "heading": 0.4462737596207199, + "angularVelocity": -1.4581079346161945, + "velocityX": 2.909266239965237, + "velocityY": 0.4163754140030483, + "timestamp": 0.6786122375505199 + }, + { + "x": 1.9405727782098796, + "y": 6.845027020300295, + "heading": 0.3571888127510162, + "angularVelocity": -1.3127518476716487, + "velocityX": 2.618362105550687, + "velocityY": 0.37479818244033536, + "timestamp": 0.7464734613055719 + }, + { + "x": 2.098517679561726, + "y": 6.867636881126044, + "heading": 0.2779750281361537, + "angularVelocity": -1.167290836086129, + "velocityX": 2.32746909961358, + "velocityY": 0.33317791184197254, + "timestamp": 0.8143346850606239 + }, + { + "x": 2.23672263265918, + "y": 6.887421047973718, + "heading": 0.20863780174038785, + "angularVelocity": -1.0217503098093481, + "velocityX": 2.036582092835676, + "velocityY": 0.2915386100180838, + "timestamp": 0.8821959088156759 + }, + { + "x": 2.355187828517663, + "y": 6.904378696630012, + "heading": 0.14917899743785948, + "angularVelocity": -0.87618231756544, + "velocityX": 1.7456979008526012, + "velocityY": 0.24988716262327937, + "timestamp": 0.9500571325707279 + }, + { + "x": 2.4539133200816936, + "y": 6.918509209536525, + "heading": 0.09959774951013609, + "angularVelocity": -0.7306270825101687, + "velocityX": 1.4548144890576191, + "velocityY": 0.20822661491514247, + "timestamp": 1.0179183563257799 + }, + { + "x": 2.5328990844951553, + "y": 6.92981209262676, + "heading": 0.05989172741762143, + "angularVelocity": -0.5851061901835907, + "velocityX": 1.1639307404559098, + "velocityY": 0.16655878666486323, + "timestamp": 1.0857795800808319 + }, + { + "x": 2.5921450685166625, + "y": 6.938286954893026, + "heading": 0.030058218738196592, + "angularVelocity": -0.43962526798971635, + "velocityX": 0.8730462072914873, + "velocityY": 0.12488519654252121, + "timestamp": 1.1536408038358839 + }, + { + "x": 2.631651217872353, + "y": 6.943933511848997, + "heading": 0.010094853212766689, + "angularVelocity": -0.2941792738293157, + "velocityX": 0.5821608743498282, + "velocityY": 0.08320741424223291, + "timestamp": 1.2215020275909358 + }, + { + "x": 2.6514174938201904, + "y": 6.946751594543457, + "heading": 1.1828263155272929e-27, + "angularVelocity": -0.14875731167484535, + "velocityX": 0.2912749705072336, + "velocityY": 0.04152714228425266, + "timestamp": 1.2893632513459878 + }, + { + "x": 2.6491463328077915, + "y": 6.946410455927006, + "heading": 0.0008990727501414491, + "angularVelocity": 0.011985225180580105, + "velocityX": -0.03027605513644419, + "velocityY": -0.004547599885899398, + "timestamp": 1.364378341725619 + }, + { + "x": 2.6227540855884963, + "y": 6.942613200487609, + "heading": 0.013859521250247003, + "angularVelocity": 0.17277121755790997, + "velocityX": -0.3518258404506491, + "velocityY": -0.05061988754768401, + "timestamp": 1.4393934321052502 + }, + { + "x": 2.572240863223279, + "y": 6.93536015645873, + "heading": 0.03888546284671504, + "angularVelocity": 0.33361209684369453, + "velocityX": -0.6733741452497535, + "velocityY": -0.09668779964370192, + "timestamp": 1.5144085224848813 + }, + { + "x": 2.497606760935717, + "y": 6.9246517669282275, + "heading": 0.07598096923656177, + "angularVelocity": 0.49450725450194494, + "velocityX": -0.9949211806565718, + "velocityY": -0.1427498050900185, + "timestamp": 1.5894236128645125 + }, + { + "x": 2.3988518004865456, + "y": 6.910488565027239, + "heading": 0.12514851964427576, + "angularVelocity": 0.6554354618369476, + "velocityX": -1.3164679259786103, + "velocityY": -0.1888047035511428, + "timestamp": 1.6644387032441437 + }, + { + "x": 2.275975845725643, + "y": 6.892871178204052, + "heading": 0.18638682893267958, + "angularVelocity": 0.8163465374565724, + "velocityX": -1.6380164862704298, + "velocityY": -0.23485123771803745, + "timestamp": 1.7394537936237748 + }, + { + "x": 2.128978500524308, + "y": 6.871800394582041, + "heading": 0.2596886161467297, + "angularVelocity": 0.9771605532045546, + "velocityX": -1.959570327215777, + "velocityY": -0.28088726568717615, + "timestamp": 1.814468884003406 + }, + { + "x": 1.957859021947552, + "y": 6.847277323064731, + "heading": 0.34503981520215615, + "angularVelocity": 1.1377870588902428, + "velocityX": -2.2811340719682667, + "velocityY": -0.32690851125028303, + "timestamp": 1.8894839743830372 + }, + { + "x": 1.76261631729721, + "y": 6.81930365549537, + "heading": 0.44242285893441274, + "angularVelocity": 1.298179382833869, + "velocityX": -2.602712383098801, + "velocityY": -0.3729072034412448, + "timestamp": 1.9644990647626683 + }, + { + "x": 1.5454887932969013, + "y": 6.788364935052348, + "heading": 0.5521114771232074, + "angularVelocity": 1.4622207029770928, + "velocityX": -2.8944512750899136, + "velocityY": -0.41243328890826375, + "timestamp": 2.0395141551422995 + }, + { + "x": 1.3524867398675213, + "y": 6.760871330824376, + "heading": 0.6497679916624863, + "angularVelocity": 1.3018249267589428, + "velocityX": -2.5728430433483203, + "velocityY": -0.3665076465126455, + "timestamp": 2.114529245521931 + }, + { + "x": 1.1836105739801852, + "y": 6.7368199629542165, + "heading": 0.7353592835672482, + "angularVelocity": 1.1409876529056668, + "velocityX": -2.2512292531102633, + "velocityY": -0.3206203944891842, + "timestamp": 2.1895443359015623 + }, + { + "x": 1.0388602648787244, + "y": 6.71620826316138, + "heading": 0.8088372426617098, + "angularVelocity": 0.9795090390827935, + "velocityX": -1.9296158728719546, + "velocityY": -0.2747673793169661, + "timestamp": 2.2645594262811937 + }, + { + "x": 0.9182354949187841, + "y": 6.699034148059042, + "heading": 0.8701477847623594, + "angularVelocity": 0.8173094478774001, + "velocityX": -1.6080067270397285, + "velocityY": -0.2289421370476856, + "timestamp": 2.339574516660825 + }, + { + "x": 0.821735849417145, + "y": 6.685296075575293, + "heading": 0.9192400732020367, + "angularVelocity": 0.6544321707970268, + "velocityX": -1.2864031092048316, + "velocityY": -0.18313745160106204, + "timestamp": 2.4145896070404564 + }, + { + "x": 0.7493609915406636, + "y": 6.674993018433249, + "heading": 0.9560742229389826, + "angularVelocity": 0.49102320013930684, + "velocityX": -0.9648039815750613, + "velocityY": -0.13734646042419815, + "timestamp": 2.489604697420088 + }, + { + "x": 0.7011107973306167, + "y": 6.6681243981888185, + "heading": 0.9806268225252659, + "angularVelocity": 0.32730213963656085, + "velocityX": -0.643206506395788, + "velocityY": -0.09156318028372816, + "timestamp": 2.564619787799719 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.16353322544301652, + "velocityX": -0.3216066870029412, + "velocityY": -0.04578252817189861, + "timestamp": 2.6396348781793506 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": -5.340812593461075e-28, + "velocityX": 3.892811349542242e-27, + "velocityY": 2.3219495328630078e-26, + "timestamp": 2.714649968558982 + }, + { + "x": 0.6985020844125115, + "y": 6.66756932185273, + "heading": 0.9798246445664861, + "angularVelocity": -0.1842987222375363, + "velocityX": 0.30341235873432515, + "velocityY": 0.04060189659731702, + "timestamp": 2.7855654772873355 + }, + { + "x": 0.7415443472286969, + "y": 6.673333307824001, + "heading": 0.9539749006718472, + "angularVelocity": -0.36451467892105427, + "velocityX": 0.6069513367105871, + "velocityY": 0.08127962521359514, + "timestamp": 2.856480986015689 + }, + { + "x": 0.806123192395711, + "y": 6.681988659680362, + "heading": 0.9157093267024382, + "angularVelocity": -0.5395938724205993, + "velocityX": 0.9106448832566048, + "velocityY": 0.12205160777335122, + "timestamp": 2.9273964947440425 + }, + { + "x": 0.8922518577310767, + "y": 6.693543414407282, + "heading": 0.8654790262942293, + "angularVelocity": -0.7083119237093733, + "velocityX": 1.2145251000777222, + "velocityY": 0.16293692217848874, + "timestamp": 2.998312003472396 + }, + { + "x": 0.9999464064120345, + "y": 6.708007002745886, + "heading": 0.8038456054132372, + "angularVelocity": -0.8691106076258047, + "velocityX": 1.518631828384521, + "velocityY": 0.2039552221786628, + "timestamp": 3.0692275122007495 + }, + { + "x": 1.1292266934715014, + "y": 6.725390389590618, + "heading": 0.7315277617443964, + "angularVelocity": -1.0197747286261354, + "velocityX": 1.823018538225309, + "velocityY": 0.2451281413113817, + "timestamp": 3.140143020929103 + }, + { + "x": 1.2801178802680748, + "y": 6.745706577222529, + "heading": 0.6494907896351716, + "angularVelocity": -1.1568269561947726, + "velocityX": 2.1277600556258043, + "velocityY": 0.2864844093515925, + "timestamp": 3.2110585296574565 + }, + { + "x": 1.4526523788182084, + "y": 6.768971930813486, + "heading": 0.5591180385402752, + "angularVelocity": -1.274372174936732, + "velocityX": 2.43295862419938, + "velocityY": 0.3280714473906901, + "timestamp": 3.28197403838581 + }, + { + "x": 1.646870967643948, + "y": 6.795208954217293, + "heading": 0.462559660855429, + "angularVelocity": -1.361597475873995, + "velocityX": 2.7387322224494954, + "velocityY": 0.36997581874945734, + "timestamp": 3.3528895471141635 + }, + { + "x": 1.862815108078828, + "y": 6.824451101510304, + "heading": 0.36357825171401237, + "angularVelocity": -1.3957653398577683, + "velocityX": 3.0450904788974817, + "velocityY": 0.41235193566790274, + "timestamp": 3.423805055842517 + }, + { + "x": 2.1004346793080155, + "y": 6.856747845634531, + "heading": 0.2705944336755815, + "angularVelocity": -1.3111915814439359, + "velocityX": 3.3507419673093723, + "velocityY": 0.4554256847813233, + "timestamp": 3.4947205645708705 + }, + { + "x": 2.3559638608729165, + "y": 6.892112274947116, + "heading": 0.23346195065053052, + "angularVelocity": -0.5236158308796662, + "velocityX": 3.6032905375285806, + "velocityY": 0.4986839965860088, + "timestamp": 3.565636073299224 + }, + { + "x": 2.6217812158343787, + "y": 6.927777331410734, + "heading": 0.2334619248619747, + "angularVelocity": -3.636518485299231e-7, + "velocityX": 3.748367031810964, + "velocityY": 0.5029232265714462, + "timestamp": 3.6365515820275776 + }, + { + "x": 2.8875985808561246, + "y": 6.963442312894318, + "heading": 0.23346189907376708, + "angularVelocity": -3.6364693779214443e-7, + "velocityX": 3.74836717367392, + "velocityY": 0.5029221692563763, + "timestamp": 3.707467090755931 + }, + { + "x": 3.153415945878299, + "y": 6.999107294374709, + "heading": 0.23346187328555953, + "angularVelocity": -3.636469371719515e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113627, + "timestamp": 3.7783825994842846 + }, + { + "x": 3.419233310900473, + "y": 7.034772275855099, + "heading": 0.23346184749735194, + "angularVelocity": -3.6364693781306164e-7, + "velocityX": 3.7483671736799598, + "velocityY": 0.502922169211361, + "timestamp": 3.849298108212638 + }, + { + "x": 3.6850506759226476, + "y": 7.070437257335489, + "heading": 0.2334618217091443, + "angularVelocity": -3.6364693810863675e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113609, + "timestamp": 3.9202136169409916 + }, + { + "x": 3.950868040944822, + "y": 7.10610223881588, + "heading": 0.23346179592093674, + "angularVelocity": -3.636469373806279e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.502922169211361, + "timestamp": 3.991129125669345 + }, + { + "x": 4.216685405966996, + "y": 7.14176722029627, + "heading": 0.23346177013272917, + "angularVelocity": -3.6364693732214093e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113611, + "timestamp": 4.062044634397698 + }, + { + "x": 4.4825027709891705, + "y": 7.17743220177666, + "heading": 0.23346174434452158, + "angularVelocity": -3.636469378203165e-7, + "velocityX": 3.7483671736799606, + "velocityY": 0.5029221692113612, + "timestamp": 4.132960143126051 + }, + { + "x": 4.748320136011345, + "y": 7.213097183257051, + "heading": 0.233461718556314, + "angularVelocity": -3.636469375156407e-7, + "velocityX": 3.7483671736799598, + "velocityY": 0.502922169211361, + "timestamp": 4.203875651854404 + }, + { + "x": 5.014137501033519, + "y": 7.248762164737441, + "heading": 0.23346169276810638, + "angularVelocity": -3.636469383573516e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113611, + "timestamp": 4.274791160582757 + }, + { + "x": 5.279954866055694, + "y": 7.284427146217831, + "heading": 0.23346166697989873, + "angularVelocity": -3.6364693850408854e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113611, + "timestamp": 4.34570666931111 + }, + { + "x": 5.545772231077868, + "y": 7.32009212769822, + "heading": 0.23346164119169116, + "angularVelocity": -3.6364693775026823e-7, + "velocityX": 3.748367173679964, + "velocityY": 0.502922169211334, + "timestamp": 4.416622178039463 + }, + { + "x": 5.811589596106133, + "y": 7.3557571091332195, + "heading": 0.23346161540348362, + "angularVelocity": -3.63646937002983e-7, + "velocityX": 3.748367173765839, + "velocityY": 0.5029221685712877, + "timestamp": 4.4875376867678165 + }, + { + "x": 6.07740710417715, + "y": 7.39142102442851, + "heading": 0.2334615896149771, + "angularVelocity": -3.636511531820921e-7, + "velocityX": 3.748369190852852, + "velocityY": 0.5029071346283975, + "timestamp": 4.55845319549617 + }, + { + "x": 6.33844426596311, + "y": 7.413466447299319, + "heading": 0.21125647994267174, + "angularVelocity": -0.3131206427265934, + "velocityX": 3.6809601519729838, + "velocityY": 0.3108688531764646, + "timestamp": 4.629368704224523 + }, + { + "x": 6.578740605741143, + "y": 7.432100566048415, + "heading": 0.16288337141874695, + "angularVelocity": -0.682123126398504, + "velocityX": 3.3884878510637786, + "velocityY": 0.2627650718896376, + "timestamp": 4.700284212952876 + }, + { + "x": 6.797330437873669, + "y": 7.447554222867888, + "heading": 0.11319689086108646, + "angularVelocity": -0.7006433634705764, + "velocityX": 3.0823981390283666, + "velocityY": 0.2179164627961563, + "timestamp": 4.771199721681229 + }, + { + "x": 6.9942244466742505, + "y": 7.4599283003931935, + "heading": 0.06789887850949264, + "angularVelocity": -0.6387603101757464, + "velocityX": 2.7764590895737267, + "velocityY": 0.1744904287820278, + "timestamp": 4.842115230409582 + }, + { + "x": 7.169459125508247, + "y": 7.4692727668351795, + "heading": 0.029649916545462486, + "angularVelocity": -0.5393596217513631, + "velocityX": 2.4710346435678088, + "velocityY": 0.13176901089126727, + "timestamp": 4.913030739137935 + }, + { + "x": 7.323064804077148, + "y": 7.475617408752441, + "heading": 4.538527756690787e-28, + "angularVelocity": -0.41810200726386243, + "velocityX": 2.1660378854123263, + "velocityY": 0.08946762183665603, + "timestamp": 4.983946247866288 + }, + { + "x": 7.44267075746469, + "y": 7.47890384849819, + "heading": -0.01877472577087381, + "angularVelocity": -0.2974384142895345, + "velocityX": 1.8948561778925237, + "velocityY": 0.05206539038508263, + "timestamp": 5.047067636593831 + }, + { + "x": 7.545114017207691, + "y": 7.480040189019033, + "heading": -0.030609170040043498, + "angularVelocity": -0.1874870706703203, + "velocityX": 1.6229563672178904, + "velocityY": 0.018002463883490395, + "timestamp": 5.110189025321374 + }, + { + "x": 7.630363412345014, + "y": 7.479184975308775, + "heading": -0.03600750296274941, + "angularVelocity": -0.08552303793579748, + "velocityX": 1.3505627308881507, + "velocityY": -0.013548715063116237, + "timestamp": 5.173310414048918 + }, + { + "x": 7.698396363563398, + "y": 7.4764617545204155, + "heading": -0.035360550234153146, + "angularVelocity": 0.010249342443791389, + "velocityX": 1.0778113820030217, + "velocityY": -0.04314259941449841, + "timestamp": 5.236431802776461 + }, + { + "x": 7.749195849524956, + "y": 7.471969501540552, + "heading": -0.0289805004906965, + "angularVelocity": 0.10107587732258928, + "velocityX": 0.804790372734491, + "velocityY": -0.07116847506720048, + "timestamp": 5.299553191504004 + }, + { + "x": 7.782748628163455, + "y": 7.46578928720822, + "heading": -0.01712270276724403, + "angularVelocity": 0.1878570475474702, + "velocityX": 0.5315595761577129, + "velocityY": -0.09790998672428226, + "timestamp": 5.362674580231547 + }, + { + "x": 7.799044132232666, + "y": 7.457988739013672, + "heading": -3.115976344858509e-29, + "angularVelocity": 0.2712662555818014, + "velocityX": 0.2581613680831416, + "velocityY": -0.12358011051084936, + "timestamp": 5.425795968959091 + }, + { + "x": 7.789461840253682, + "y": 7.444564299731131, + "heading": 0.032498030114414544, + "angularVelocity": 0.38031209691715145, + "velocityX": -0.11213792168232267, + "velocityY": -0.15710111152907683, + "timestamp": 5.511246919317617 + }, + { + "x": 7.7482316276690275, + "y": 7.4282846905716475, + "heading": 0.07405408050157687, + "angularVelocity": 0.48631466604883555, + "velocityX": -0.48250151006705927, + "velocityY": -0.19051407961151762, + "timestamp": 5.5966978696761425 + }, + { + "x": 7.675346865619377, + "y": 7.409161329569071, + "heading": 0.12434733262468406, + "angularVelocity": 0.5885628177579297, + "velocityX": -0.8529426734734683, + "velocityY": -0.2237934267827392, + "timestamp": 5.682148820034668 + }, + { + "x": 7.570799431412373, + "y": 7.387208662514977, + "heading": 0.18297244388953984, + "angularVelocity": 0.6860673991205785, + "velocityX": -1.2234788936618521, + "velocityY": -0.2569037203446768, + "timestamp": 5.767599770393194 + }, + { + "x": 7.43457917720258, + "y": 7.362445546099649, + "heading": 0.24939990020901256, + "angularVelocity": 0.7773752783411216, + "velocityX": -1.5941338702291188, + "velocityY": -0.28979334122592726, + "timestamp": 5.85305072075172 + }, + { + "x": 7.266673120798299, + "y": 7.334897702364834, + "heading": 0.32290449051554804, + "angularVelocity": 0.8601962880240946, + "velocityX": -1.9649407724524794, + "velocityY": -0.32238194682719395, + "timestamp": 5.938501671110246 + }, + { + "x": 7.067064218419677, + "y": 7.304602562446356, + "heading": 0.4024227559343364, + "angularVelocity": 0.9305720426180611, + "velocityX": -2.3359471315546996, + "velocityY": -0.3545325100700251, + "timestamp": 6.023952621468772 + }, + { + "x": 6.835729835895948, + "y": 7.271620394698958, + "heading": 0.4862275157211307, + "angularVelocity": 0.9807352573046286, + "velocityX": -2.707218369756224, + "velocityY": -0.3859777756597744, + "timestamp": 6.109403571827298 + }, + { + "x": 6.572643491372071, + "y": 7.236066664600634, + "heading": 0.5710108191953822, + "angularVelocity": 0.9921867822244987, + "velocityX": -3.0787995150439604, + "velocityY": -0.41607179263836863, + "timestamp": 6.194854522185824 + }, + { + "x": 6.277837620342988, + "y": 7.198257710886909, + "heading": 0.6480492253062652, + "angularVelocity": 0.9015511915040533, + "velocityX": -3.4500010800601615, + "velocityY": -0.44246381760636405, + "timestamp": 6.28030547254435 + }, + { + "x": 5.957362242994923, + "y": 7.156598776275489, + "heading": 0.6480492977916429, + "angularVelocity": 8.482688295948482e-7, + "velocityX": -3.7504015578931313, + "velocityY": -0.48751868102848633, + "timestamp": 6.365756422902876 + }, + { + "x": 5.635584364863326, + "y": 7.126616389470958, + "heading": 0.6480493126613769, + "angularVelocity": 1.740148467326507e-7, + "velocityX": -3.7656442296020605, + "velocityY": -0.3508724792262005, + "timestamp": 6.4512073732614015 + }, + { + "x": 5.3138064561089, + "y": 7.096634331319495, + "heading": 0.6480493275311116, + "angularVelocity": 1.7401485467118797e-7, + "velocityX": -3.7656445879693994, + "velocityY": -0.350868633124234, + "timestamp": 6.536658323619927 + }, + { + "x": 4.99202854735373, + "y": 7.066652273176011, + "heading": 0.6480493424008463, + "angularVelocity": 1.740148544690323e-7, + "velocityX": -3.7656445879780995, + "velocityY": -0.35086863303086346, + "timestamp": 6.622109273978453 + }, + { + "x": 4.67025063859856, + "y": 7.036670215032527, + "heading": 0.6480493572705811, + "angularVelocity": 1.740148551537583e-7, + "velocityX": -3.765644587978099, + "velocityY": -0.35086863303086074, + "timestamp": 6.707560224336979 + }, + { + "x": 4.34847272984339, + "y": 7.006688156889043, + "heading": 0.6480493721403158, + "angularVelocity": 1.7401485461327074e-7, + "velocityX": -3.7656445879781, + "velocityY": -0.350868633030861, + "timestamp": 6.793011174695505 + }, + { + "x": 4.0266948210882205, + "y": 6.9767060987455585, + "heading": 0.6480493870100505, + "angularVelocity": 1.7401485474732458e-7, + "velocityX": -3.7656445879781, + "velocityY": -0.3508686330308609, + "timestamp": 6.878462125054031 + }, + { + "x": 3.7049169123330508, + "y": 6.946724040602074, + "heading": 0.6480494018797853, + "angularVelocity": 1.74014855000899e-7, + "velocityX": -3.7656445879781004, + "velocityY": -0.3508686330308606, + "timestamp": 6.963913075412557 + }, + { + "x": 3.383139003577881, + "y": 6.91674198245859, + "heading": 0.64804941674952, + "angularVelocity": 1.740148546492808e-7, + "velocityX": -3.7656445879781, + "velocityY": -0.35086863303086063, + "timestamp": 7.049364025771083 + }, + { + "x": 3.0613610948227112, + "y": 6.886759924315106, + "heading": 0.6480494316192547, + "angularVelocity": 1.7401485430725364e-7, + "velocityX": -3.7656445879780995, + "velocityY": -0.3508686330308605, + "timestamp": 7.134814976129609 + }, + { + "x": 2.7395831860675424, + "y": 6.856777866171614, + "heading": 0.6480494464889893, + "angularVelocity": 1.7401485477485056e-7, + "velocityX": -3.765644587978091, + "velocityY": -0.3508686330309518, + "timestamp": 7.220265926488135 + }, + { + "x": 2.4178052773422305, + "y": 6.82679580770789, + "heading": 0.6480494613587636, + "angularVelocity": 1.7401531782389627e-7, + "velocityX": -3.7656445876286826, + "velocityY": -0.35086863677850416, + "timestamp": 7.3057168768466605 + }, + { + "x": 2.10180699075498, + "y": 6.797337251058121, + "heading": 0.6651219064311078, + "angularVelocity": 0.19979233701571972, + "velocityX": -3.6980078660496734, + "velocityY": -0.3447422939846707, + "timestamp": 7.391167827205186 + }, + { + "x": 1.8167368724291442, + "y": 6.770773756267678, + "heading": 0.7108427772926394, + "angularVelocity": 0.535053977395227, + "velocityX": -3.336067265838099, + "velocityY": -0.3108624851916846, + "timestamp": 7.476618777563712 + }, + { + "x": 1.5633497036796815, + "y": 6.747170828533709, + "heading": 0.7627235488811058, + "angularVelocity": 0.6071409547909103, + "velocityX": -2.9652937467205107, + "velocityY": -0.2762160939689793, + "timestamp": 7.562069727922238 + }, + { + "x": 1.3416809575894382, + "y": 6.72653103338719, + "heading": 0.8140209718864446, + "angularVelocity": 0.6003142479997046, + "velocityX": -2.5941050995944295, + "velocityY": -0.24153967931217257, + "timestamp": 7.647520678280764 + }, + { + "x": 1.1517177479923977, + "y": 6.708849739663476, + "heading": 0.8615071389710252, + "angularVelocity": 0.5557125682668599, + "velocityX": -2.223067254372391, + "velocityY": -0.20691746141533648, + "timestamp": 7.73297162863929 + }, + { + "x": 0.9934436218878284, + "y": 6.694122461290386, + "heading": 0.9032823337599477, + "angularVelocity": 0.4888792297060059, + "velocityX": -1.8522219523656431, + "velocityY": -0.17234774231649247, + "timestamp": 7.818422578997816 + }, + { + "x": 0.8668441825845944, + "y": 6.682345527807194, + "heading": 0.9380916675290075, + "angularVelocity": 0.4073604052735566, + "velocityX": -1.481545129364404, + "velocityY": -0.1378209772247021, + "timestamp": 7.903873529356342 + }, + { + "x": 0.7719075390509518, + "y": 6.673515992228389, + "heading": 0.9650448126274666, + "angularVelocity": 0.3154224146761601, + "velocityX": -1.1110074625889743, + "velocityY": -0.10332869958448399, + "timestamp": 7.989324479714868 + }, + { + "x": 0.708623954226878, + "y": 6.667631472026271, + "heading": 0.9834806534673777, + "angularVelocity": 0.2157476395822394, + "velocityX": -0.7405837449268271, + "velocityY": -0.06886430376057255, + "timestamp": 8.074775430073394 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.1101641199433363, + "velocityX": -0.37025347823207466, + "velocityY": -0.03442272220184603, + "timestamp": 8.16022638043192 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 2.9535092730949793e-26, + "velocityX": 4.101709673254646e-26, + "velocityY": -1.0818589619170112e-27, + "timestamp": 8.245677330790446 + }, + { + "x": 0.6937129747745413, + "y": 6.664558415884879, + "heading": 0.9790660166396069, + "angularVelocity": -0.2211476741106938, + "velocityX": 0.2675140136822165, + "velocityY": -0.0021046338178637887, + "timestamp": 8.308206885432824 + }, + { + "x": 0.7271833461439815, + "y": 6.664303045508021, + "heading": 0.951758290929694, + "angularVelocity": -0.4367170990756675, + "velocityX": 0.5352728251602833, + "velocityY": -0.004083994813630824, + "timestamp": 8.370736440075202 + }, + { + "x": 0.7774150549719171, + "y": 6.663933729358567, + "heading": 0.9114059799343049, + "angularVelocity": -0.6453318151100473, + "velocityX": 0.80332746835033, + "velocityY": -0.0059062654702356065, + "timestamp": 8.43326599471758 + }, + { + "x": 0.8444295958079634, + "y": 6.663462333290518, + "heading": 0.8585277818184469, + "angularVelocity": -0.8456512831137402, + "velocityX": 1.0717258617835783, + "velocityY": -0.007538772197327858, + "timestamp": 8.495795549359958 + }, + { + "x": 0.9282514561454864, + "y": 6.662902494939932, + "heading": 0.7937301924930702, + "angularVelocity": -1.0362714031144127, + "velocityX": 1.3405158699261557, + "velocityY": -0.00895317988091305, + "timestamp": 8.558325104002336 + }, + { + "x": 1.0289086011587232, + "y": 6.662269038795252, + "heading": 0.717728481418855, + "angularVelocity": -1.2154526209068217, + "velocityX": 1.6097531093723465, + "velocityY": -0.010130507858288727, + "timestamp": 8.620854658644713 + }, + { + "x": 1.1464337388167514, + "y": 6.661577430622322, + "heading": 0.6314012179720037, + "angularVelocity": -1.3805833727839212, + "velocityX": 1.879513428972632, + "velocityY": -0.011060500540669212, + "timestamp": 8.683384213287091 + }, + { + "x": 1.2808663521190058, + "y": 6.66084404129845, + "heading": 0.5359002071968971, + "angularVelocity": -1.5272939543756525, + "velocityX": 2.149905177977148, + "velocityY": -0.011728682989453756, + "timestamp": 8.74591376792947 + }, + { + "x": 1.4322545657568082, + "y": 6.6600882254157945, + "heading": 0.43284519285348916, + "angularVelocity": -1.6481008849783823, + "velocityX": 2.4210665581040525, + "velocityY": -0.012087338331098648, + "timestamp": 8.808443322571847 + }, + { + "x": 1.6006535235186912, + "y": 6.659336953698895, + "heading": 0.32466239343874187, + "angularVelocity": -1.730106667694512, + "velocityX": 2.693109821827408, + "velocityY": -0.012014666043865637, + "timestamp": 8.870972877214225 + }, + { + "x": 1.7861082134397477, + "y": 6.658632455209532, + "heading": 0.2152749507913156, + "angularVelocity": -1.749371849408482, + "velocityX": 2.9658725538941773, + "velocityY": -0.011266648121698327, + "timestamp": 8.933502431856603 + }, + { + "x": 1.988547032824456, + "y": 6.658050746326577, + "heading": 0.1122523271254691, + "angularVelocity": -1.6475828790890588, + "velocityX": 3.237490184321712, + "velocityY": -0.009302943004826474, + "timestamp": 8.99603198649898 + }, + { + "x": 2.206257710668096, + "y": 6.657808132322276, + "heading": 0.04234892140521699, + "angularVelocity": -1.1179258531433098, + "velocityX": 3.481724427573166, + "velocityY": -0.003879989321667614, + "timestamp": 9.058561541141358 + }, + { + "x": 2.4373706508795343, + "y": 6.657710121609846, + "heading": 0.021561234757217183, + "angularVelocity": -0.3324457813091681, + "velocityX": 3.6960592720231067, + "velocityY": -0.0015674302014480283, + "timestamp": 9.121091095783736 + }, + { + "x": 2.6738489737582136, + "y": 6.656073631484609, + "heading": 0.02156119930135256, + "angularVelocity": -5.670257020753551e-7, + "velocityX": 3.7818648194626623, + "velocityY": -0.026171466190618775, + "timestamp": 9.183620650426114 + }, + { + "x": 2.910327295829668, + "y": 6.654437024703101, + "heading": 0.021561163845773425, + "angularVelocity": -5.670211364811453e-7, + "velocityX": 3.781864806553173, + "velocityY": -0.026173331808750182, + "timestamp": 9.246150205068492 + }, + { + "x": 3.146805617901072, + "y": 6.6528004179142695, + "heading": 0.021561128390194195, + "angularVelocity": -5.670211379082889e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.0261733319258545, + "timestamp": 9.30867975971087 + }, + { + "x": 3.3832839399724763, + "y": 6.651163811125438, + "heading": 0.02156109293461502, + "angularVelocity": -5.670211370809572e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861857, + "timestamp": 9.371209314353248 + }, + { + "x": 3.6197622620438805, + "y": 6.649527204336607, + "heading": 0.02156105747903583, + "angularVelocity": -5.670211373321077e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861847, + "timestamp": 9.433738868995626 + }, + { + "x": 3.8562405841152843, + "y": 6.647890597547776, + "heading": 0.021561022023456698, + "angularVelocity": -5.67021136433779e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861833, + "timestamp": 9.496268423638003 + }, + { + "x": 4.092718906186688, + "y": 6.646253990758946, + "heading": 0.021560986567877485, + "angularVelocity": -5.670211376937565e-7, + "velocityX": 3.781864806552362, + "velocityY": -0.026173331925861826, + "timestamp": 9.558797978280381 + }, + { + "x": 4.329197228258092, + "y": 6.644617383970115, + "heading": 0.02156095111229825, + "angularVelocity": -5.670211380713392e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.02617333192586182, + "timestamp": 9.62132753292276 + }, + { + "x": 4.565675550329496, + "y": 6.642980777181283, + "heading": 0.02156091565671906, + "angularVelocity": -5.670211373267648e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861805, + "timestamp": 9.683857087565137 + }, + { + "x": 4.8021538724009005, + "y": 6.641344170392449, + "heading": 0.02156088020113987, + "angularVelocity": -5.670211373593148e-7, + "velocityX": 3.781864806552362, + "velocityY": -0.026173331925919696, + "timestamp": 9.746386642207515 + }, + { + "x": 5.038632194471905, + "y": 6.63970756354592, + "heading": 0.021560844745560682, + "angularVelocity": -5.670211372802694e-7, + "velocityX": 3.7818648065459763, + "velocityY": -0.026173332848588252, + "timestamp": 9.808916196849893 + }, + { + "x": 5.275110510179994, + "y": 6.6380700375610795, + "heading": 0.021560809289976154, + "angularVelocity": -5.670212226566905e-7, + "velocityX": 3.7818647047874645, + "velocityY": -0.026188032110664707, + "timestamp": 9.87144575149227 + }, + { + "x": 5.511317622497512, + "y": 6.6266299618393605, + "heading": 0.021560773670877074, + "angularVelocity": -5.696362189823434e-7, + "velocityX": 3.7775275014901912, + "velocityY": -0.18295469697725122, + "timestamp": 9.933975306134649 + }, + { + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0.02156073730438198, + "angularVelocity": -5.815889030387333e-7, + "velocityX": 3.7545129277426152, + "velocityY": -0.454773640430677, + "timestamp": 9.996504860777026 + }, + { + "x": 5.947788995289173, + "y": 6.560838779777381, + "heading": 0.021560704158269762, + "angularVelocity": -6.111013256538172e-7, + "velocityX": 3.7187222649013885, + "velocityY": -0.6886876024038036, + "timestamp": 10.050744822019793 + }, + { + "x": 6.146761514775588, + "y": 6.510943599042024, + "heading": 0.02156067179220178, + "angularVelocity": -5.967199688048595e-7, + "velocityX": 3.668375030650514, + "velocityY": -0.9198970573012855, + "timestamp": 10.104984783262559 + }, + { + "x": 6.342740585471881, + "y": 6.450348248758284, + "heading": 0.02156063967405059, + "angularVelocity": -5.921492283023906e-7, + "velocityX": 3.6131860385949297, + "velocityY": -1.1171717105867658, + "timestamp": 10.159224744505325 + }, + { + "x": 6.534357375004527, + "y": 6.392349058669825, + "heading": 0.005697476736664897, + "angularVelocity": -0.29246265251528625, + "velocityX": 3.532760443448913, + "velocityY": -1.0693073660002697, + "timestamp": 10.213464705748091 + }, + { + "x": 6.715860806517651, + "y": 6.336732651063718, + "heading": -0.04432151851696129, + "angularVelocity": -0.9221797749772022, + "velocityX": 3.346304594517604, + "velocityY": -1.025376979109191, + "timestamp": 10.267704666990857 + }, + { + "x": 6.885664150088088, + "y": 6.2845630937364065, + "heading": -0.11100118904659197, + "angularVelocity": -1.2293458365721763, + "velocityX": 3.1305948544180144, + "velocityY": -0.9618288090917363, + "timestamp": 10.321944628233624 + }, + { + "x": 7.043230371259609, + "y": 6.2361124460413855, + "heading": -0.1820163474700135, + "angularVelocity": -1.3092774551510717, + "velocityX": 2.9049840295108242, + "velocityY": -0.8932647919523048, + "timestamp": 10.37618458947639 + }, + { + "x": 7.188521112298814, + "y": 6.191411290458308, + "heading": -0.25302240055528713, + "angularVelocity": -1.3091095837525815, + "velocityX": 2.678666018747991, + "velocityY": -0.8241369381332195, + "timestamp": 10.430424550719156 + }, + { + "x": 7.321552039325899, + "y": 6.150465396300966, + "heading": -0.32169330843547617, + "angularVelocity": -1.2660574658752652, + "velocityX": 2.4526368378411387, + "velocityY": -0.7549027178334027, + "timestamp": 10.484664511961922 + }, + { + "x": 7.442345977289038, + "y": 6.1132739640793945, + "heading": -0.38654982839023333, + "angularVelocity": -1.195733154462882, + "velocityX": 2.2270284711762884, + "velocityY": -0.685683237403341, + "timestamp": 10.538904473204688 + }, + { + "x": 7.550924557109546, + "y": 6.079834332011057, + "heading": -0.44656245543798795, + "angularVelocity": -1.106428280417667, + "velocityX": 2.0018189049681507, + "velocityY": -0.6165128311701689, + "timestamp": 10.593144434447455 + }, + { + "x": 7.647306630963791, + "y": 6.050143543568203, + "heading": -0.5009752943623942, + "angularVelocity": -1.003187275169067, + "velocityX": 1.776956908631632, + "velocityY": -0.5473969332309115, + "timestamp": 10.64738439569022 + }, + { + "x": 7.7315082311756465, + "y": 6.024198847629992, + "heading": -0.5492132065198027, + "angularVelocity": -0.8893426737807988, + "velocityX": 1.552390493698683, + "velocityY": -0.478331756582337, + "timestamp": 10.701624356932987 + }, + { + "x": 7.803542913553516, + "y": 6.001997812730986, + "heading": -0.5908266122726411, + "angularVelocity": -0.7672093563375907, + "velocityX": 1.3280740016656365, + "velocityY": -0.4093114078684577, + "timestamp": 10.755864318175753 + }, + { + "x": 7.863422174316105, + "y": 5.983538319203457, + "heading": -0.6254559253112775, + "angularVelocity": -0.6384464930504523, + "velocityX": 1.103969460718877, + "velocityY": -0.34033013860220046, + "timestamp": 10.81010427941852 + }, + { + "x": 7.911155845783931, + "y": 5.9688185305906725, + "heading": -0.6528074437520258, + "angularVelocity": -0.5042687681565456, + "velocityX": 0.8800461942474525, + "velocityY": -0.2713827273382881, + "timestamp": 10.864344240661286 + }, + { + "x": 7.9467524419763915, + "y": 5.957836876155619, + "heading": -0.6726364655817074, + "angularVelocity": -0.3655795722443686, + "velocityX": 0.6562798972723887, + "velocityY": -0.2024642751107768, + "timestamp": 10.918584201904052 + }, + { + "x": 7.970219449419739, + "y": 5.950592046890535, + "heading": -0.6847351444633458, + "angularVelocity": -0.22305839835481103, + "velocityX": 0.4326516263224211, + "velocityY": -0.13356995652444023, + "timestamp": 10.972824163146818 + }, + { + "x": 7.981563568115234, + "y": 5.947082996368408, + "heading": -0.6889234822214203, + "angularVelocity": -0.07721867166033586, + "velocityX": 0.2091468805577016, + "velocityY": -0.06469493048531134, + "timestamp": 11.027064124389584 + }, + { + "x": 7.980191051455629, + "y": 5.94749330873125, + "heading": -0.6844922597423083, + "angularVelocity": 0.07820370271926669, + "velocityX": -0.024222635024756137, + "velocityY": 0.007241330399680058, + "timestamp": 11.083726690547342 + }, + { + "x": 7.965589050009383, + "y": 5.951981018115051, + "heading": -0.6714197010480987, + "angularVelocity": 0.23070890679065725, + "velocityX": -0.25770102620470225, + "velocityY": 0.0792006025866641, + "timestamp": 11.1403892567051 + }, + { + "x": 7.937750696996029, + "y": 5.960547682646947, + "heading": -0.6498948448329763, + "angularVelocity": 0.37987789249067105, + "velocityX": -0.4913006046328201, + "velocityY": 0.1511873731247131, + "timestamp": 11.197051822862859 + }, + { + "x": 7.896668195757291, + "y": 5.973195099977348, + "heading": -0.6201381095216878, + "angularVelocity": 0.525156859794188, + "velocityX": -0.725037781105049, + "velocityY": 0.22320586920098048, + "timestamp": 11.253714389020617 + }, + { + "x": 7.842332542589409, + "y": 5.989925311820313, + "heading": -0.58241146210736, + "angularVelocity": 0.6658125456106359, + "velocityX": -0.9589338579654723, + "velocityY": 0.2952603981327993, + "timestamp": 11.310376955178375 + }, + { + "x": 7.774733203499769, + "y": 6.010740643840145, + "heading": -0.5370321424515595, + "angularVelocity": 0.8008694757921291, + "velocityX": -1.1930158422658073, + "velocityY": 0.3673559711693639, + "timestamp": 11.367039521336133 + }, + { + "x": 7.693857751929137, + "y": 6.035643786713744, + "heading": -0.4843912545821768, + "angularVelocity": 0.9290240707210785, + "velocityX": -1.4273171346574955, + "velocityY": 0.4394990301756616, + "timestamp": 11.423702087493892 + }, + { + "x": 7.599691484700552, + "y": 6.064637898936317, + "heading": -0.42497921973533753, + "angularVelocity": 1.048523546946787, + "velocityX": -1.6618779136548574, + "velocityY": 0.511697831366272, + "timestamp": 11.48036465365165 + }, + { + "x": 7.492217047600242, + "y": 6.097726664155008, + "heading": -0.3594218947187098, + "angularVelocity": 1.1569776920110768, + "velocityX": -1.8967449656459858, + "velocityY": 0.5839616427989975, + "timestamp": 11.537027219809408 + }, + { + "x": 7.371414143059779, + "y": 6.134914157517514, + "heading": -0.2885358443103568, + "angularVelocity": 1.2510208275953218, + "velocityX": -2.1319702359425117, + "velocityY": 0.6562973738070701, + "timestamp": 11.593689785967166 + }, + { + "x": 7.237259577881299, + "y": 6.176204249541141, + "heading": -0.21342321481120613, + "angularVelocity": 1.3256129150597364, + "velocityX": -2.367604827585309, + "velocityY": 0.7287014130046445, + "timestamp": 11.650352352124925 + }, + { + "x": 7.089728781224228, + "y": 6.221599016786816, + "heading": -0.13565858875957554, + "angularVelocity": 1.3724162409997644, + "velocityX": -2.6036730536757164, + "velocityY": 0.8011421000469415, + "timestamp": 11.707014918282683 + }, + { + "x": 6.928804275182176, + "y": 6.271094675189793, + "heading": -0.05771765635307292, + "angularVelocity": 1.3755277547702696, + "velocityX": -2.8400497357287153, + "velocityY": 0.8735160046435817, + "timestamp": 11.763677484440441 + }, + { + "x": 6.754523165192255, + "y": 6.324666169886209, + "heading": 0.015819308808898584, + "angularVelocity": 1.2978050615856767, + "velocityX": -3.07577156856422, + "velocityY": 0.9454477325870393, + "timestamp": 11.8203400505982 + }, + { + "x": 6.567351146368393, + "y": 6.382062775695167, + "heading": 0.07327051868988925, + "angularVelocity": 1.0139182493259664, + "velocityX": -3.303274657606285, + "velocityY": 1.012954578321738, + "timestamp": 11.877002616755957 + }, + { + "x": 6.3689373323756, + "y": 6.441765390033087, + "heading": 0.0952315218805866, + "angularVelocity": 0.38757516081347765, + "velocityX": -3.501673634765793, + "velocityY": 1.0536517914084174, + "timestamp": 11.933665182913716 + }, + { + "x": 6.164194282224136, + "y": 6.505032727532106, + "heading": 0.09523155538228788, + "angularVelocity": 5.912492769442424e-7, + "velocityX": -3.6133741204276855, + "velocityY": 1.116563223114054, + "timestamp": 11.990327749071474 + }, + { + "x": 5.956663059713831, + "y": 6.558448682183149, + "heading": 0.09523158909306802, + "angularVelocity": 5.949391710813205e-7, + "velocityX": -3.662580722738597, + "velocityY": 0.9427027096218124, + "timestamp": 12.046990315229232 + }, + { + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0.09523162364938853, + "angularVelocity": 6.098615517108678e-7, + "velocityX": -3.7163409676723376, + "velocityY": 0.701424046809534, + "timestamp": 12.10365288138699 + }, + { + "x": 5.502705503273195, + "y": 6.625523412363324, + "heading": 0.09523165483409754, + "angularVelocity": 4.815615988787072e-7, + "velocityX": -3.758333277705387, + "velocityY": 0.4220400409979927, + "timestamp": 12.168410347665688 + }, + { + "x": 5.257964270714501, + "y": 6.634609902404007, + "heading": 0.09523168540664503, + "angularVelocity": 4.72108457235069e-7, + "velocityX": -3.779351580949706, + "velocityY": 0.1403157128102671, + "timestamp": 12.233167813944386 + }, + { + "x": 5.013061042473027, + "y": 6.636411025383571, + "heading": 0.09523171589055375, + "angularVelocity": 4.7073967636728834e-7, + "velocityX": -3.781853156321443, + "velocityY": 0.027813363972772134, + "timestamp": 12.297925280223083 + }, + { + "x": 4.76815781079403, + "y": 6.6382116808939955, + "heading": 0.09523174637446119, + "angularVelocity": 4.7073965669390476e-7, + "velocityX": -3.7818532094044772, + "velocityY": 0.02780614520456082, + "timestamp": 12.36268274650178 + }, + { + "x": 4.523254579114848, + "y": 6.640012336379186, + "heading": 0.09523177685836867, + "angularVelocity": 4.707396572416609e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814890212, + "timestamp": 12.427440212780478 + }, + { + "x": 4.278351347435667, + "y": 6.641812991864376, + "heading": 0.09523180734227604, + "angularVelocity": 4.7073965569633346e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869288, + "timestamp": 12.492197679059176 + }, + { + "x": 4.033448115756485, + "y": 6.643613647349565, + "heading": 0.0952318378261835, + "angularVelocity": 4.7073965673585443e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869194, + "timestamp": 12.556955145337874 + }, + { + "x": 3.7885448840773033, + "y": 6.645414302834754, + "heading": 0.09523186831009096, + "angularVelocity": 4.7073965722571307e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869288, + "timestamp": 12.621712611616571 + }, + { + "x": 3.5436416523981213, + "y": 6.647214958319943, + "heading": 0.09523189879399836, + "angularVelocity": 4.7073965583098703e-7, + "velocityX": -3.7818532094073416, + "velocityY": 0.027806144814869357, + "timestamp": 12.686470077895269 + }, + { + "x": 3.2987384207189394, + "y": 6.649015613805132, + "heading": 0.0952319292779057, + "angularVelocity": 4.7073965534371566e-7, + "velocityX": -3.7818532094073416, + "velocityY": 0.02780614481486934, + "timestamp": 12.751227544173966 + }, + { + "x": 3.0538351890397575, + "y": 6.650816269290322, + "heading": 0.09523195976181308, + "angularVelocity": 4.7073965558339454e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869368, + "timestamp": 12.815985010452664 + }, + { + "x": 2.808931957360575, + "y": 6.65261692477551, + "heading": 0.09523199024572042, + "angularVelocity": 4.7073965499589656e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.02780614481484276, + "timestamp": 12.880742476731362 + }, + { + "x": 2.5640287256811582, + "y": 6.654417580228766, + "heading": 0.09523202072962778, + "angularVelocity": 4.7073965543506366e-7, + "velocityX": -3.7818532094109676, + "velocityY": 0.02780614432174875, + "timestamp": 12.94549994301006 + }, + { + "x": 2.319125489653766, + "y": 6.6562176441406535, + "heading": 0.09523205121544294, + "angularVelocity": 4.7076911613064953e-7, + "velocityX": -3.7818532765534276, + "velocityY": 0.027797009601036878, + "timestamp": 13.010257409288757 + }, + { + "x": 2.0867659976251596, + "y": 6.656130735198093, + "heading": 0.13926625419972302, + "angularVelocity": 0.6799865021705632, + "velocityX": -3.5881498363230637, + "velocityY": -0.0013420682981450127, + "timestamp": 13.075014875567454 + }, + { + "x": 1.8694746618246978, + "y": 6.656990384791893, + "heading": 0.22988670590711813, + "angularVelocity": 1.3993822938870089, + "velocityX": -3.355463829688775, + "velocityY": 0.013274910882104354, + "timestamp": 13.139772341846152 + }, + { + "x": 1.6701637846976716, + "y": 6.657989605635195, + "heading": 0.33420976058736407, + "angularVelocity": 1.610981106506988, + "velocityX": -3.0778053648555033, + "velocityY": 0.01543020288968781, + "timestamp": 13.20452980812485 + }, + { + "x": 1.4891465844372453, + "y": 6.659013013376454, + "heading": 0.43962013302928266, + "angularVelocity": 1.6277717226962183, + "velocityX": -2.7953101111365104, + "velocityY": 0.015803702647255023, + "timestamp": 13.269287274403547 + }, + { + "x": 1.326396711582843, + "y": 6.660011984363377, + "heading": 0.5409065154536696, + "angularVelocity": 1.5640881004898903, + "velocityX": -2.513221751974858, + "velocityY": 0.015426344548804824, + "timestamp": 13.334044740682245 + }, + { + "x": 1.181861048474837, + "y": 6.660956547926137, + "heading": 0.6351810351446563, + "angularVelocity": 1.4558092697026699, + "velocityX": -2.2319536481857507, + "velocityY": 0.014586172329450045, + "timestamp": 13.398802206960942 + }, + { + "x": 1.0554921048807082, + "y": 6.661825696511207, + "heading": 0.7206099924431242, + "angularVelocity": 1.3192140182076537, + "velocityX": -1.9514188997184116, + "velocityY": 0.01342159653574964, + "timestamp": 13.46355967323964 + }, + { + "x": 0.9472506650106102, + "y": 6.66260279551869, + "heading": 0.7959103123026781, + "angularVelocity": 1.1628052205668817, + "velocityX": -1.6714897306861416, + "velocityY": 0.01200014534444738, + "timestamp": 13.528317139518338 + }, + { + "x": 0.8571042426441801, + "y": 6.663273874888148, + "heading": 0.8601164348709504, + "angularVelocity": 0.9914860209623859, + "velocityX": -1.392062221496826, + "velocityY": 0.010362965199572966, + "timestamp": 13.593074605797035 + }, + { + "x": 0.7850255726020201, + "y": 6.66382712376522, + "heading": 0.9124677407650426, + "angularVelocity": 0.8084211582458605, + "velocityX": -1.1130557476099119, + "velocityY": 0.00854339906830906, + "timestamp": 13.657832072075733 + }, + { + "x": 0.7309916974392009, + "y": 6.664252697305851, + "heading": 0.9523540062287302, + "angularVelocity": 0.6159330769988476, + "velocityX": -0.8344037879782468, + "velocityY": 0.006571806543502557, + "timestamp": 13.72258953835443 + }, + { + "x": 0.694983496367867, + "y": 6.664542513171668, + "heading": 0.9792885969521788, + "angularVelocity": 0.4159302744725947, + "velocityX": -0.5560470960424025, + "velocityY": 0.004475404651714408, + "timestamp": 13.787347004633128 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.21010218653726323, + "velocityX": -0.2779301718200537, + "velocityY": 0.0022777995651181563, + "timestamp": 13.852104470911826 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 2.2135835324585282e-26, + "velocityX": 2.644293811404756e-26, + "velocityY": 8.707515786736107e-27, + "timestamp": 13.916861937190523 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 1.2893632513459878, + "isStopPoint": false, + "x": 2.6514174938201904, + "y": 6.946751594543457, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 2.714649968558982, + "isStopPoint": true, + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 32 + }, + { + "timestamp": 4.983946247866288, + "isStopPoint": false, + "x": 7.323064804077148, + "y": 7.475617408752441, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 5.425795968959091, + "isStopPoint": false, + "x": 7.799044132232666, + "y": 7.457988739013672, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 33 + }, + { + "timestamp": 8.245677330790446, + "isStopPoint": true, + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 28 + }, + { + "timestamp": 9.996504860777026, + "isStopPoint": false, + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 19 + }, + { + "timestamp": 11.027064124389584, + "isStopPoint": false, + "x": 7.981563568115234, + "y": 5.947082996368408, + "heading": -0.6889234822214203, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 12.10365288138699, + "isStopPoint": false, + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 28 + }, + { + "timestamp": 13.916861937190523, + "isStopPoint": true, + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119463, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 2 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 5 + ], + "type": "StopPoint", + "direction": 0 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "CenterLanePCBAFSprint": { + "waypoints": [ + { + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -2.548660671865789e-25, + "angularVelocity": -1.3190073040663248e-25, + "velocityX": -3.501543589784201e-25, + "velocityY": -4.821953566499718e-25, "timestamp": 0 }, { - "x": 0.6967295535333877, - "y": 6.66750179345797, - "heading": 0.982893163249758, - "angularVelocity": -0.14737604826251527, - "velocityX": 0.29094834799603586, - "velocityY": 0.04143420364739335, - "timestamp": 0.06786122358481142 - }, - { - "x": 0.7362175895643961, - "y": 6.673125696218821, - "heading": 0.9628974920292898, - "angularVelocity": -0.2946553298656345, - "velocityX": 0.5818939586560375, - "velocityY": 0.08287358323008266, - "timestamp": 0.13572244716962284 - }, - { - "x": 0.7954495269492323, - "y": 6.681562379154691, - "heading": 0.9329256679312776, - "angularVelocity": -0.44166347900512215, - "velocityX": 0.8728392188627362, - "velocityY": 0.1243225879257299, - "timestamp": 0.20358367075443426 - }, - { - "x": 0.8744254360510917, - "y": 6.692812849791712, - "heading": 0.8930052032103389, - "angularVelocity": -0.5882662087730711, - "velocityX": 1.1637855159383756, - "velocityY": 0.1657864394820339, - "timestamp": 0.2714448943392457 - }, - { - "x": 0.9731453894549041, - "y": 6.7068785148813665, - "heading": 0.843168820166726, - "angularVelocity": -0.7343867441666224, - "velocityX": 1.4547328826223493, - "velocityY": 0.20727102086621554, - "timestamp": 0.33930611792405707 - }, - { - "x": 1.0916093532691815, - "y": 6.7237612036761645, - "heading": 0.7834495219946941, - "angularVelocity": -0.8800209459441316, - "velocityX": 1.7456797498828438, - "velocityY": 0.2487825580347626, - "timestamp": 0.40716734150886846 - }, - { - "x": 1.2298170767085217, - "y": 6.743463170039788, - "heading": 0.7138752049500412, - "angularVelocity": -1.025244069725628, - "velocityX": 2.0366229215807086, - "velocityY": 0.29032730803917456, - "timestamp": 0.47502856509367986 - }, - { - "x": 1.3877679992818084, - "y": 6.765987144948878, - "heading": 0.6344639927011461, - "angularVelocity": -1.1702001239286361, - "velocityX": 2.3275578339061793, - "velocityY": 0.3319123015950357, - "timestamp": 0.5428897886784912 - }, - { - "x": 1.5654611541888466, - "y": 6.791337011948423, - "heading": 0.5452227484246948, - "angularVelocity": -1.3150550426624645, - "velocityX": 2.618478499506586, - "velocityY": 0.3735545228986894, - "timestamp": 0.6107510122633026 - }, - { - "x": 1.7628875214635322, - "y": 6.819592757066269, - "heading": 0.4462737595759067, - "angularVelocity": -1.4581079388455742, - "velocityX": 2.909266247284013, - "velocityY": 0.41637541478357637, - "timestamp": 0.678612235848114 - }, - { - "x": 1.9405727781814897, - "y": 6.845027020375549, - "heading": 0.357188812683824, - "angularVelocity": -1.3127518512946696, - "velocityX": 2.6183621121403804, - "velocityY": 0.3747981831994576, - "timestamp": 0.7464734594329254 - }, - { - "x": 2.098517679535123, - "y": 6.867636881191052, - "heading": 0.2779750280562074, - "angularVelocity": -1.1672908392024057, - "velocityX": 2.3274691054787353, - "velocityY": 0.3331779125268215, - "timestamp": 0.8143346830177368 - }, - { - "x": 2.236722632634811, - "y": 6.887421048029417, - "heading": 0.20863780165516146, - "angularVelocity": -1.0217503124503762, - "velocityX": 2.036582097977683, - "velocityY": 0.29153861061228065, - "timestamp": 0.8821959066025482 - }, - { - "x": 2.3551878284960197, - "y": 6.9043786966768, - "heading": 0.1491789973537457, - "angularVelocity": -0.8761823197470863, - "velocityX": 1.745697905272119, - "velocityY": 0.24988716311885253, - "timestamp": 0.9500571301873596 - }, - { - "x": 2.4539133200632923, - "y": 6.918509209574482, - "heading": 0.09959774943286472, - "angularVelocity": -0.7306270842422328, - "velocityX": 1.454814492755025, - "velocityY": 0.20822661530737346, - "timestamp": 1.017918353772171 - }, - { - "x": 2.5328990844805293, - "y": 6.929812092655753, - "heading": 0.059891727352470836, - "angularVelocity": -0.5851061914728107, - "velocityX": 1.163930743431445, - "velocityY": 0.1665587869506308, - "timestamp": 1.0857795773569825 - }, - { - "x": 2.592145068506356, - "y": 6.938286954912776, - "heading": 0.030058218690118557, - "angularVelocity": -0.43962526884100517, - "velocityX": 0.8730462095453193, - "velocityY": 0.12488519671960763, - "timestamp": 1.153640800941794 - }, - { - "x": 2.63165121786692, - "y": 6.943933511859112, - "heading": 0.010094853186468074, - "angularVelocity": -0.2941792742463702, - "velocityX": 0.582160875882081, - "velocityY": 0.08320741430898287, - "timestamp": 1.2215020245266055 + "x": 1.3060567679582222, + "y": 5.572381918180447, + "heading": -0.010695335923127927, + "angularVelocity": -0.1421295269156777, + "velocityX": 0.21371671803125017, + "velocityY": -0.2468070561274771, + "timestamp": 0.07525062634925417 + }, + { + "x": 1.3382215476730444, + "y": 5.535237409026609, + "heading": -0.03207750843935064, + "angularVelocity": -0.2841461068640612, + "velocityX": 0.4274353752955963, + "velocityY": -0.49361063097923197, + "timestamp": 0.15050125269850834 + }, + { + "x": 1.3864692368555713, + "y": 5.479521033340732, + "heading": -0.06412446500671098, + "angularVelocity": -0.4258696322157321, + "velocityX": 0.6411599680061012, + "velocityY": -0.7404107897691923, + "timestamp": 0.2257518790477625 + }, + { + "x": 1.4508006060735015, + "y": 5.405233126364813, + "heading": -0.10680311296194826, + "angularVelocity": -0.5671533916163913, + "velocityX": 0.8548948007336773, + "velocityY": -0.9872064935530687, + "timestamp": 0.3010025053970167 + }, + { + "x": 1.5312167667505892, + "y": 5.312374207143071, + "heading": -0.1600730253151242, + "angularVelocity": -0.7078999197420489, + "velocityX": 1.0686444030892075, + "velocityY": -1.2339952997967572, + "timestamp": 0.37625313174627084 + }, + { + "x": 1.6277191879063078, + "y": 5.200945106889095, + "heading": -0.22389124150457193, + "angularVelocity": -0.8480755481456564, + "velocityX": 1.2824135271357124, + "velocityY": -1.4807730601046407, + "timestamp": 0.451503758095525 + }, + { + "x": 1.7403097782498615, + "y": 5.070947160382222, + "heading": -0.29821764025550423, + "angularVelocity": -0.987718007900262, + "velocityX": 1.496208015877461, + "velocityY": -1.7275330826287743, + "timestamp": 0.5267543844447792 + }, + { + "x": 1.8689915501241479, + "y": 4.922382870770339, + "heading": -0.3830195665070615, + "angularVelocity": -1.1269265169697522, + "velocityX": 1.7100425354208568, + "velocityY": -1.9742598410060204, + "timestamp": 0.6020050107940333 + }, + { + "x": 2.021815501878169, + "y": 4.778590249333106, + "heading": -0.46245356659686293, + "angularVelocity": -1.0555925437900195, + "velocityX": 2.03086617571437, + "velocityY": -1.9108494960541829, + "timestamp": 0.6772556371432875 + }, + { + "x": 2.1585523215512867, + "y": 4.653366950194428, + "heading": -0.5314334875144606, + "angularVelocity": -0.9166690599683149, + "velocityX": 1.8170854690098266, + "velocityY": -1.6640831473945463, + "timestamp": 0.7525062634925417 + }, + { + "x": 2.2791997725503816, + "y": 4.546710847450191, + "heading": -0.5899636474798271, + "angularVelocity": -0.7778029606519882, + "velocityX": 1.6032750403849743, + "velocityY": -1.417345049717245, + "timestamp": 0.8277568898417959 + }, + { + "x": 2.3837568392129738, + "y": 4.458620854469452, + "heading": -0.6380444617817417, + "angularVelocity": -0.6389423800774935, + "velocityX": 1.3894511146966526, + "velocityY": -1.1706213921980426, + "timestamp": 0.90300751619105 + }, + { + "x": 2.472222908494702, + "y": 4.389096261560078, + "heading": -0.6756747800114784, + "angularVelocity": -0.5000665118066445, + "velocityX": 1.1756190423072155, + "velocityY": -0.9239071657250583, + "timestamp": 0.9782581425403042 + }, + { + "x": 2.54459758971785, + "y": 4.338136588288653, + "heading": -0.7028528784400924, + "angularVelocity": -0.361167737029517, + "velocityX": 0.9617817782305474, + "velocityY": -0.6771993236961286, + "timestamp": 1.0535087688895584 + }, + { + "x": 2.600880650448604, + "y": 4.305741524929518, + "heading": -0.7195768777221777, + "angularVelocity": -0.2222439877704894, + "velocityX": 0.7479414253581413, + "velocityY": -0.43049559758855604, + "timestamp": 1.1287593952388124 + }, + { + "x": 2.6410719892542827, + "y": 4.291910904068807, + "heading": -0.7258449001015178, + "angularVelocity": -0.08329528514817768, + "velocityX": 0.5340997245543403, + "velocityY": -0.18379409623143778, + "timestamp": 1.2040100215880667 }, { - "x": 2.6514174938201904, - "y": 6.946751594543457, - "heading": 0, - "angularVelocity": -0.14875731166049128, - "velocityX": 0.2912749713180046, - "velocityY": 0.041527142239385896, - "timestamp": 1.289363248111417 - }, - { - "x": 2.649146332814054, - "y": 6.946410455915093, - "heading": 0.0008990727847708372, - "angularVelocity": 0.011985225672082133, - "velocityX": -0.030276055128411465, - "velocityY": -0.004547600056030398, - "timestamp": 1.3643783383040946 - }, - { - "x": 2.62275408560099, - "y": 6.9426132004627625, - "heading": 0.013859521326443051, - "angularVelocity": 0.17277121854260333, - "velocityX": -0.35182584124441, - "velocityY": -0.050619887846258024, - "timestamp": 1.4393934284967722 - }, - { - "x": 2.572240863242063, - "y": 6.93536015641962, - "heading": 0.03888546297295733, - "angularVelocity": 0.3336120983422759, - "velocityX": -0.6733741468440916, - "velocityY": -0.09668780007480728, - "timestamp": 1.5144085186894498 - }, - { - "x": 2.4976067609609713, - "y": 6.924651766873113, - "heading": 0.07598096942338527, - "angularVelocity": 0.4945072565419495, - "velocityX": -0.994921183049867, - "velocityY": -0.14274980565911422, - "timestamp": 1.5894236088821274 - }, - { - "x": 2.398851800518621, - "y": 6.910488564953806, - "heading": 0.12514851990507653, - "angularVelocity": 0.6554354644565975, - "velocityX": -1.3164679291686043, - "velocityY": -0.18880470426588675, - "timestamp": 1.664438699074805 - }, - { - "x": 2.275975845765142, - "y": 6.89287117810912, - "heading": 0.18638682928509862, - "angularVelocity": 0.8163465407124133, - "velocityX": -1.6380164902537586, - "velocityY": -0.23485123858993803, - "timestamp": 1.7394537892674826 - }, - { - "x": 2.1289785005722504, - "y": 6.871800394460987, - "heading": 0.2596886166153821, - "angularVelocity": 0.9771605571893133, - "velocityX": -1.95957033198688, - "velocityY": -0.28088726673543657, - "timestamp": 1.8144688794601602 - }, - { - "x": 1.9578590220057892, - "y": 6.847277322910046, - "heading": 0.34503981582540255, - "angularVelocity": 1.1377870637866898, - "velocityX": -2.2811340775161044, - "velocityY": -0.3269085125133225, - "timestamp": 1.8894839696528378 - }, - { - "x": 1.7626163173700675, - "y": 6.8193036552908985, - "heading": 0.4424228597913738, - "angularVelocity": 1.298179389184776, - "velocityX": -2.602712389390415, - "velocityY": -0.3729072050343056, - "timestamp": 1.9644990598455154 - }, - { - "x": 1.5454887933501882, - "y": 6.788364934906598, - "heading": 0.5521114777288451, - "angularVelocity": 1.4622207032709544, - "velocityX": -2.8944512825643915, - "velocityY": -0.4124332891533426, - "timestamp": 2.039514150038193 - }, - { - "x": 1.3524867399067801, - "y": 6.760871330718944, - "heading": 0.6497679920992822, - "angularVelocity": 1.3018249277525966, - "velocityX": -2.5728430499473998, - "velocityY": -0.3665076468885929, - "timestamp": 2.114529240230871 - }, - { - "x": 1.1836105740084832, - "y": 6.736819962879357, - "heading": 0.7353592838772256, - "angularVelocity": 1.140987654058677, - "velocityX": -2.251229258866918, - "velocityY": -0.3206203948806917, - "timestamp": 2.1895443304235487 - }, - { - "x": 1.0388602648983114, - "y": 6.7162082631102376, - "heading": 0.8088372428736526, - "angularVelocity": 0.9795090402170741, - "velocityX": -1.9296158777970915, - "velocityY": -0.2747673796855824, - "timestamp": 2.2645594206162265 - }, - { - "x": 0.9182354949315064, - "y": 6.6990341480262074, - "heading": 0.8701477848986364, - "angularVelocity": 0.8173094489056358, - "velocityX": -1.6080067311387376, - "velocityY": -0.22894213737419178, - "timestamp": 2.3395745108089043 - }, - { - "x": 0.8217358494246105, - "y": 6.685296075556227, - "heading": 0.919240073281317, - "angularVelocity": 0.6544321716682079, - "velocityX": -1.286403112480899, - "velocityY": -0.18313745187394112, - "timestamp": 2.414589601001582 - }, - { - "x": 0.7493609915443247, - "y": 6.674993018423988, - "heading": 0.956074222977567, - "angularVelocity": 0.49102320082053863, - "velocityX": -0.9648039840302745, - "velocityY": -0.13734646063578734, - "timestamp": 2.48960469119426 - }, - { - "x": 0.7011107973318162, - "y": 6.66812439818581, - "heading": 0.9806268225378217, - "angularVelocity": 0.32730214010529163, - "velocityX": -0.6432065080316098, - "velocityY": -0.09156318042856706, - "timestamp": 2.564619781386938 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05567774261378543, + "velocityX": 0.3202582509259972, + "velocityY": 0.06290689942978202, + "timestamp": 1.279260647937321 + }, + { + "x": 2.673111673605269, + "y": 4.320290744259783, + "heading": -0.7067971794350305, + "angularVelocity": 0.19582512759435053, + "velocityX": 0.10464854696595383, + "velocityY": 0.31165110401806617, + "timestamp": 1.3551341369201315 + }, + { + "x": 2.664692786669079, + "y": 4.362810218152184, + "heading": -0.6813099274420213, + "angularVelocity": 0.3359177538123147, + "velocityX": -0.11095953341617228, + "velocityY": 0.560399613388471, + "timestamp": 1.431007625902942 + }, + { + "x": 2.639915179239814, + "y": 4.424203570363792, + "heading": -0.6451974716196371, + "angularVelocity": 0.47595617792883843, + "velocityX": -0.32656475616770847, + "velocityY": 0.8091541991105307, + "timestamp": 1.5068811148857526 + }, + { + "x": 2.5987791710000745, + "y": 4.504471407000408, + "heading": -0.5984632131457898, + "angularVelocity": 0.6159497750846186, + "velocityX": -0.5421657655556009, + "velocityY": 1.057916773206541, + "timestamp": 1.5827546038685631 + }, + { + "x": 2.5412852001352904, + "y": 4.603614476417717, + "heading": -0.5411082975385818, + "angularVelocity": 0.7559282745018111, + "velocityX": -0.7577609997321935, + "velocityY": 1.3066892105063217, + "timestamp": 1.6586280928513737 + }, + { + "x": 2.4674338294975446, + "y": 4.721633621731354, + "heading": -0.47312860561398856, + "angularVelocity": 0.8959610640812155, + "velocityX": -0.9733488156116926, + "velocityY": 1.5554727599303477, + "timestamp": 1.7345015818341842 + }, + { + "x": 2.3772256595738117, + "y": 4.858529596063375, + "heading": -0.39450963445811277, + "angularVelocity": 1.0361849996602548, + "velocityX": -1.1889287171724772, + "velocityY": 1.804266235378164, + "timestamp": 1.8103750708169948 + }, + { + "x": 2.2706605653387557, + "y": 5.014302224131365, + "heading": -0.3052190347895922, + "angularVelocity": 1.176835293402019, + "velocityX": -1.404510266546452, + "velocityY": 2.0530574006327873, + "timestamp": 1.8862485597998053 + }, + { + "x": 2.156230166958563, + "y": 5.146478435153377, + "heading": -0.22909325420264964, + "angularVelocity": 1.0033251614959908, + "velocityX": -1.508173670596822, + "velocityY": 1.7420605377980973, + "timestamp": 1.9621220487826159 + }, + { + "x": 2.05814700584302, + "y": 5.25977079434431, + "heading": -0.16373664121531684, + "angularVelocity": 0.8613893187663968, + "velocityX": -1.2927197948912579, + "velocityY": 1.4931745028437962, + "timestamp": 2.0379955377654264 + }, + { + "x": 1.9764115799277049, + "y": 5.35418052420219, + "heading": -0.10920088427959268, + "angularVelocity": 0.7187722308127881, + "velocityX": -1.077259356477368, + "velocityY": 1.24430458021074, + "timestamp": 2.1138690267482367 + }, + { + "x": 1.9110235357976653, + "y": 5.42970804617497, + "heading": -0.06553152377220887, + "angularVelocity": 0.5755549282474322, + "velocityX": -0.8618035760139328, + "velocityY": 0.9954402121917941, + "timestamp": 2.189742515731047 + }, + { + "x": 1.8619825316383507, + "y": 5.486353607136737, + "heading": -0.03276339241171435, + "angularVelocity": 0.4318785362291475, + "velocityX": -0.6463523006095715, + "velocityY": 0.7465790979323437, + "timestamp": 2.2656160047138574 + }, + { + "x": 1.8292884005204861, + "y": 5.524117342639773, + "heading": -0.010917643304822403, + "angularVelocity": 0.287923349772952, + "velocityX": -0.4309032253053722, + "velocityY": 0.4977197702295186, + "timestamp": 2.3414894936966677 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.16353322568319786, - "velocityX": -0.32160668782044516, - "velocityY": -0.04578252824590232, - "timestamp": 2.6396348715796156 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.3743055340676842e-24, + "angularVelocity": 0.14389272789730093, + "velocityX": -0.21545347602984788, + "velocityY": 0.24886063882774975, + "timestamp": 2.417362982679478 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -9.098710086763465e-32, - "velocityX": -1.2738111607141702e-32, - "velocityY": 8.271252813231263e-33, - "timestamp": 2.7146499617722935 - }, - { - "x": 0.6985020843742907, - "y": 6.667569321822967, - "heading": 0.9798246448067045, - "angularVelocity": -0.18429871952070437, - "velocityX": 0.3034123592993086, - "velocityY": 0.04060189632535011, - "timestamp": 2.7855654702426254 - }, - { - "x": 0.7415443471125377, - "y": 6.673333307735536, - "heading": 0.9539749013573504, - "angularVelocity": -0.36451467396822734, - "velocityX": 0.6069513378199061, - "velocityY": 0.081279624681546, - "timestamp": 2.8564809787129573 - }, - { - "x": 0.8061231921601518, - "y": 6.681988659505296, - "heading": 0.9157093279961269, - "angularVelocity": -0.5395938658076773, - "velocityX": 0.9106448848862336, - "velocityY": 0.12205160699625474, - "timestamp": 2.927396487183289 - }, - { - "x": 0.8922518573326212, - "y": 6.693543414119111, - "heading": 0.8654790283080297, - "angularVelocity": -0.7083119161320186, - "velocityX": 1.2145251021996393, - "velocityY": 0.16293692117639821, - "timestamp": 2.998311995653621 - }, - { - "x": 0.9999464058047857, - "y": 6.708007002320034, - "heading": 0.8038456081958845, - "angularVelocity": -0.8691105999462724, - "velocityX": 1.5186318309657076, - "velocityY": 0.20395522097926097, - "timestamp": 3.069227504123953 - }, - { - "x": 1.1292266926067027, - "y": 6.725390389005233, - "heading": 0.7315277652632032, - "angularVelocity": -1.0197747219557198, - "velocityX": 1.8230185412264566, - "velocityY": 0.2451281399536478, - "timestamp": 3.140143012594285 - }, - { - "x": 1.280117879093583, - "y": 6.7457065764596775, - "heading": 0.6494907937479546, - "angularVelocity": -1.1568269520279806, - "velocityX": 2.127760059000449, - "velocityY": 0.28648440789144525, - "timestamp": 3.2110585210646168 - }, - { - "x": 1.452652377278095, - "y": 6.768971929861101, - "heading": 0.5591180429519423, - "angularVelocity": -1.2743721753587984, - "velocityX": 2.4329586278958, - "velocityY": 0.3280714459116738, - "timestamp": 3.2819740295349487 - }, - { - "x": 1.6468709656789369, - "y": 6.795208953072923, - "heading": 0.46255966504991763, - "angularVelocity": -1.361597483890574, - "velocityX": 2.738732226422582, - "velocityY": 0.3699758173883681, - "timestamp": 3.3528895380052806 - }, - { - "x": 1.8628151056303066, - "y": 6.824451100190135, - "heading": 0.3635782548385496, - "angularVelocity": -1.3957653600238602, - "velocityX": 3.045090483158721, - "velocityY": 0.41235193468922193, - "timestamp": 3.4238050464756125 - }, - { - "x": 2.100434676341306, - "y": 6.8567478442017284, - "heading": 0.27059443428530877, - "angularVelocity": -1.3111916216766784, - "velocityX": 3.3507419721937035, - "velocityY": 0.4554256848500797, - "timestamp": 3.4947205549459444 - }, - { - "x": 2.35596385739112, - "y": 6.892112273565999, - "heading": 0.23346194806127452, - "angularVelocity": -0.5236158778945916, - "velocityX": 3.603290543375525, - "velocityY": 0.4986839991292765, - "timestamp": 3.5656360634162763 - }, - { - "x": 2.621781213038304, - "y": 6.927777327664925, - "heading": 0.23346192227272974, - "angularVelocity": -3.6365169422849345e-7, - "velocityX": 3.7483670551187007, - "velocityY": 0.5029231950560806, - "timestamp": 3.636551571886608 - }, - { - "x": 2.887598578486195, - "y": 6.963442308718448, - "heading": 0.23346189648452345, - "angularVelocity": -3.636469208413295e-7, - "velocityX": 3.748367193321304, - "velocityY": 0.502922165021807, - "timestamp": 3.70746708035694 - }, - { - "x": 3.153415943934485, - "y": 6.999107289769, - "heading": 0.23346187069631716, - "angularVelocity": -3.636469205820025e-7, - "velocityX": 3.7483671933269203, - "velocityY": 0.5029221649799431, - "timestamp": 3.778382588827272 - }, - { - "x": 3.4192333093827747, - "y": 7.034772270819553, - "heading": 0.2334618449081109, - "angularVelocity": -3.6364692054215583e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799415, - "timestamp": 3.849298097297604 - }, - { - "x": 3.6850506748310643, - "y": 7.070437251870106, - "heading": 0.23346181911990457, - "angularVelocity": -3.6364692122502775e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799416, - "timestamp": 3.9202136057679358 - }, - { - "x": 3.950868040279354, - "y": 7.106102232920659, - "heading": 0.2334617933316983, - "angularVelocity": -3.636469204641407e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799416, - "timestamp": 3.9911291142382677 - }, - { - "x": 4.216685405727644, - "y": 7.141767213971211, - "heading": 0.23346176754349196, - "angularVelocity": -3.6364692150109834e-7, - "velocityX": 3.7483671933269203, - "velocityY": 0.5029221649799415, - "timestamp": 4.0620446227086 - }, - { - "x": 4.482502771175934, - "y": 7.1774321950217645, - "heading": 0.2334617417552857, - "angularVelocity": -3.636469207728054e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799417, - "timestamp": 4.1329601311789315 - }, - { - "x": 4.7483201366242245, - "y": 7.213097176072317, - "heading": 0.2334617159670794, - "angularVelocity": -3.636469206602435e-7, - "velocityX": 3.7483671933269203, - "velocityY": 0.5029221649799417, - "timestamp": 4.203875639649263 - }, - { - "x": 5.014137502072515, - "y": 7.2487621571228695, - "heading": 0.2334616901788731, - "angularVelocity": -3.6364692079095153e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799416, - "timestamp": 4.274791148119595 - }, - { - "x": 5.279954867520805, - "y": 7.284427138173423, - "heading": 0.23346166439066685, - "angularVelocity": -3.636469205268936e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799417, - "timestamp": 4.345706656589927 - }, - { - "x": 5.545772232969095, - "y": 7.320092119223974, - "heading": 0.23346163860246044, - "angularVelocity": -3.6364692217914213e-7, - "velocityX": 3.748367193326924, - "velocityY": 0.5029221649799175, - "timestamp": 4.416622165060259 - }, - { - "x": 5.81158959842306, - "y": 7.355757100232221, - "heading": 0.23346161281425412, - "angularVelocity": -3.636469211715136e-7, - "velocityX": 3.748367193406962, - "velocityY": 0.5029221643833769, - "timestamp": 4.487537673530591 - }, - { - "x": 6.077407103529163, - "y": 7.391421040372378, - "heading": 0.23346158702575517, - "angularVelocity": -3.63651047258994e-7, - "velocityX": 3.748369162681932, - "velocityY": 0.5029074868027953, - "timestamp": 4.558453182000923 - }, - { - "x": 6.338444265915037, - "y": 7.413466456985474, - "heading": 0.21125647663717811, - "angularVelocity": -0.3131206539662162, - "velocityX": 3.680960173825454, - "velocityY": 0.310868766065739, - "timestamp": 4.629368690471255 - }, - { - "x": 6.578740605850335, - "y": 7.432100572083313, - "heading": 0.16288336837633732, - "angularVelocity": -0.6821231251705402, - "velocityX": 3.388487865610223, - "velocityY": 0.26276502135825913, - "timestamp": 4.700284198941587 - }, - { - "x": 6.797330438016248, - "y": 7.447554226429222, - "heading": 0.11319688856078455, - "angularVelocity": -0.7006433555551449, - "velocityX": 3.082398150714271, - "velocityY": 0.21791642870860864, - "timestamp": 4.771199707411919 - }, - { - "x": 6.99422444679472, - "y": 7.45992830223967, - "heading": 0.06789887708325426, - "angularVelocity": -0.638760300174411, - "velocityX": 2.7764590993639087, - "velocityY": 0.1744904052351846, - "timestamp": 4.8421152158822505 - }, - { - "x": 7.169459125576906, - "y": 7.4692727675317165, - "heading": 0.029649915927990267, - "angularVelocity": -0.539359612309142, - "velocityX": 2.4710346518279107, - "velocityY": 0.13176899515507345, - "timestamp": 4.913030724352582 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 6.7146804288849265e-25, + "angularVelocity": -3.3897350204457557e-25, + "velocityX": 3.460398907914197e-22, + "velocityY": 1.7844799561406784e-24, + "timestamp": 2.4932364716622883 + }, + { + "x": 1.8507557540076562, + "y": 5.5430527114888575, + "heading": -2.374547601271383e-20, + "angularVelocity": -2.550633226443435e-19, + "velocityX": 0.40617567269200466, + "velocityY": 0.0005740544422026415, + "timestamp": 2.5863354999137984 + }, + { + "x": 1.9263848743459357, + "y": 5.543159599309593, + "heading": -2.866667304736003e-20, + "angularVelocity": -5.285981096799938e-20, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765753625, + "timestamp": 2.6794345281653085 + }, + { + "x": 2.0398285536670646, + "y": 5.543319931039021, + "heading": -2.74578198723704e-20, + "angularVelocity": 1.2984594980856443e-20, + "velocityX": 1.2185269970236072, + "velocityY": 0.0017221632968542275, + "timestamp": 2.7725335564168185 + }, + { + "x": 2.191086789804772, + "y": 5.543533706674078, + "heading": -4.1106983374446124e-20, + "angularVelocity": -1.4660908667312053e-19, + "velocityX": 1.6247026309348636, + "velocityY": 0.0022962176842474297, + "timestamp": 2.8656325846683286 + }, + { + "x": 2.380159571927711, + "y": 5.543800926199457, + "heading": -5.015487830921238e-20, + "angularVelocity": -9.718570757102548e-20, + "velocityX": 2.0308781485038963, + "velocityY": 0.002870271907212345, + "timestamp": 2.9587316129198387 + }, + { + "x": 2.5314178080654184, + "y": 5.544014701834514, + "heading": -3.4511971954263414e-20, + "angularVelocity": 1.6802437843559674e-19, + "velocityX": 1.6247026309348636, + "velocityY": 0.00229621768424743, + "timestamp": 3.0518306411713487 + }, + { + "x": 2.644861487386547, + "y": 5.544175033563942, + "heading": -6.180786622652372e-21, + "angularVelocity": 3.0431236355167837e-19, + "velocityX": 1.218526997023607, + "velocityY": 0.0017221632968542275, + "timestamp": 3.144929669422859 + }, + { + "x": 2.7204906077248263, + "y": 5.544281921384678, + "heading": 5.803364763600579e-22, + "angularVelocity": 7.262291804719445e-20, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765753625, + "timestamp": 3.238028697674369 }, { - "x": 7.323064804077148, - "y": 7.475617408752441, - "heading": -2.508046324740631e-30, - "angularVelocity": -0.41810200007794424, - "velocityX": 2.166037892325118, - "velocityY": 0.08946761234010188, - "timestamp": 4.983946232822914 - }, - { - "x": 7.442670757639025, - "y": 7.478903848182195, - "heading": -0.0187747254681701, - "angularVelocity": -0.297438409871576, - "velocityX": 1.8948561830587192, - "velocityY": 0.05206538544539075, - "timestamp": 5.04706762147034 - }, - { - "x": 7.5451140174844875, - "y": 7.480040188586648, - "heading": -0.030609169580020613, - "angularVelocity": -0.1874870684158837, - "velocityX": 1.6229563708994625, - "velocityY": 0.01800246206270259, - "timestamp": 5.110189010117766 - }, - { - "x": 7.630363412663352, - "y": 7.479184974890449, - "heading": -0.03600750245908947, - "angularVelocity": -0.08552303735288487, - "velocityX": 1.3505627332580479, - "velocityY": -0.013548714857401807, - "timestamp": 5.173310398765192 - }, - { - "x": 7.698396363869973, - "y": 7.476461754192338, - "heading": -0.03536054977456298, - "angularVelocity": 0.01024934175875415, - "velocityX": 1.0778113831814056, - "velocityY": -0.04314259803938343, - "timestamp": 5.236431787412618 - }, - { - "x": 7.74919584977197, - "y": 7.471969501335234, - "heading": -0.02898050014133947, - "angularVelocity": 0.10107587570456572, - "velocityX": 0.8047903728082677, - "velocityY": -0.0711684732126695, - "timestamp": 5.299553176060044 - }, - { - "x": 7.782748628307226, - "y": 7.465789287122271, - "heading": -0.017122702576199005, - "angularVelocity": 0.1878570452777808, - "velocityX": 0.5315595751917994, - "velocityY": -0.09790998495750272, - "timestamp": 5.36267456470747 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.254107177965744e-28, + "angularVelocity": -6.233540915514837e-21, + "velocityX": 0.40617567269200466, + "velocityY": 0.0005740544422026413, + "timestamp": 3.331127725925879 }, { - "x": 7.799044132232666, - "y": 7.457988739013672, - "heading": 0, - "angularVelocity": 0.2712662528992439, - "velocityX": 0.25816136612733137, - "velocityY": -0.12358010930615114, - "timestamp": 5.425795953354896 - }, - { - "x": 7.789461840084544, - "y": 7.44456429977742, - "heading": 0.032498029722470304, - "angularVelocity": 0.38031209369790614, - "velocityX": -0.11213792406398307, - "velocityY": -0.15710111155221265, - "timestamp": 5.511246903406188 - }, - { - "x": 7.748231627408751, - "y": 7.428284690558898, - "heading": 0.07405407961857692, - "angularVelocity": 0.48631466205092383, - "velocityX": -0.48250151286758974, - "velocityY": -0.1905140809873915, - "timestamp": 5.59669785345748 - }, - { - "x": 7.675346865346751, - "y": 7.40916132937864, - "heading": 0.1243473311272727, - "angularVelocity": 0.5885628126840949, - "velocityX": -0.8529426766838836, - "velocityY": -0.22379342966671825, - "timestamp": 5.682148803508771 - }, - { - "x": 7.570799431207237, - "y": 7.387208662011206, - "heading": 0.1829724416238929, - "angularVelocity": 0.6860673925971562, - "velocityX": -1.2234788972702, - "velocityY": -0.2569037249352633, - "timestamp": 5.767599753560063 - }, - { - "x": 7.434579177146227, - "y": 7.362445545124385, - "heading": 0.24939989698187892, - "angularVelocity": 0.7773752698843693, - "velocityX": -1.5941338742188234, - "velocityY": -0.28979334778557075, - "timestamp": 5.853050703611355 - }, - { - "x": 7.266673120974145, - "y": 7.334897700728802, - "heading": 0.32290448608070227, - "angularVelocity": 0.8601962769834678, - "velocityX": -1.9649407767992484, - "velocityY": -0.3223819557190156, - "timestamp": 5.938501653662646 - }, - { - "x": 7.067064218914486, - "y": 7.304602559914413, - "heading": 0.4024227499708516, - "angularVelocity": 0.9305720280744717, - "velocityX": -2.3359471362200686, - "velocityY": -0.3545325218292511, - "timestamp": 6.023952603713938 - }, - { - "x": 6.835729836802405, - "y": 7.27162039096164, - "heading": 0.48622750779609625, - "angularVelocity": 0.9807352378746566, - "velocityX": -2.7072183746718483, - "velocityY": -0.385977791153593, - "timestamp": 6.10940355376523 - }, - { - "x": 6.572643492795426, - "y": 7.236066659207437, - "heading": 0.571010808696293, - "angularVelocity": 0.9921867556667185, - "velocityX": -3.078799520063915, - "velocityY": -0.4160718135124634, - "timestamp": 6.194854503816521 - }, - { - "x": 6.277837622424222, - "y": 7.198257703016904, - "heading": 0.6480492113566031, - "angularVelocity": 0.901551154360156, - "velocityX": -3.450001084764899, - "velocityY": -0.4424638481823218, - "timestamp": 6.280305453867813 - }, - { - "x": 5.957362245190181, - "y": 7.156598764006078, - "heading": 0.6480492838419923, - "angularVelocity": 8.482689682334955e-7, - "velocityX": -3.7504015700432527, - "velocityY": -0.487518734265005, - "timestamp": 6.365756403919105 - }, - { - "x": 5.635584365409571, - "y": 7.126616390006558, - "heading": 0.6480492987117265, - "angularVelocity": 1.7401484932642463e-7, - "velocityX": -3.7656442624390154, - "velocityY": -0.3508723306355692, - "timestamp": 6.451207353970396 - }, - { - "x": 5.313806456205795, - "y": 7.096634331785028, - "heading": 0.6480493135814611, - "angularVelocity": 1.7401485546331995e-7, - "velocityX": -3.7656446067671383, - "velocityY": -0.3508686352057408, - "timestamp": 6.536658304021688 - }, - { - "x": 4.992028547001338, - "y": 7.066652273570816, - "heading": 0.6480493284511959, - "angularVelocity": 1.7401485563507226e-7, - "velocityX": -3.7656446067751177, - "velocityY": -0.3508686351200953, - "timestamp": 6.62210925407298 - }, - { - "x": 4.67025063779688, - "y": 7.036670215356605, - "heading": 0.6480493433209307, - "angularVelocity": 1.7401485639450837e-7, - "velocityX": -3.765644606775118, - "velocityY": -0.35086863512009386, - "timestamp": 6.707560204124271 - }, - { - "x": 4.348472728592423, - "y": 7.006688157142394, - "heading": 0.6480493581906654, - "angularVelocity": 1.740148559122135e-7, - "velocityX": -3.7656446067751186, - "velocityY": -0.35086863512009364, - "timestamp": 6.793011154175563 - }, - { - "x": 4.0266948193879655, - "y": 6.976706098928182, - "heading": 0.6480493730604002, - "angularVelocity": 1.740148565017462e-7, - "velocityX": -3.765644606775118, - "velocityY": -0.35086863512009375, - "timestamp": 6.878462104226855 - }, - { - "x": 3.7049169101835084, - "y": 6.946724040713971, - "heading": 0.648049387930135, - "angularVelocity": 1.7401485552645738e-7, - "velocityX": -3.765644606775118, - "velocityY": -0.3508686351200936, - "timestamp": 6.963913054278146 - }, - { - "x": 3.3831390009790514, - "y": 6.91674198249976, - "heading": 0.6480494027998698, - "angularVelocity": 1.7401485630496073e-7, - "velocityX": -3.7656446067751177, - "velocityY": -0.35086863512009364, - "timestamp": 7.049364004329438 - }, - { - "x": 3.0613610917745944, - "y": 6.886759924285549, - "heading": 0.6480494176696047, - "angularVelocity": 1.7401485676252163e-7, - "velocityX": -3.7656446067751177, - "velocityY": -0.3508686351200934, - "timestamp": 7.1348149543807295 - }, - { - "x": 2.7395831825701378, - "y": 6.85677786607133, - "heading": 0.6480494325393393, - "angularVelocity": 1.740148561275008e-7, - "velocityX": -3.7656446067751106, - "velocityY": -0.35086863512017596, - "timestamp": 7.220265904432021 - }, - { - "x": 2.4178052733940274, - "y": 6.826795807553092, - "heading": 0.648049447409112, - "angularVelocity": 1.740152990535353e-7, - "velocityX": -3.765644606443389, - "velocityY": -0.3508686386780044, - "timestamp": 7.305716854483313 - }, - { - "x": 2.101806987486401, - "y": 6.79733725091672, - "heading": 0.6651218958442715, - "angularVelocity": 0.19979237708727937, - "velocityX": -3.698007871392473, - "velocityY": -0.34474229506742526, - "timestamp": 7.3911678045346045 - }, - { - "x": 1.8167368698048942, - "y": 6.770773756148669, - "heading": 0.7108427694079615, - "angularVelocity": 0.5350540109419666, - "velocityX": -3.3360672702926424, - "velocityY": -0.31086248604736477, - "timestamp": 7.476618754585896 - }, - { - "x": 1.563349701636476, - "y": 6.747170828438536, - "heading": 0.7627235430971158, - "angularVelocity": 0.6071409815583658, - "velocityX": -2.9652937505825308, - "velocityY": -0.2762160946831818, - "timestamp": 7.562069704637188 - }, - { - "x": 1.3416809560568124, - "y": 6.726531033314501, - "heading": 0.8140209677583794, - "angularVelocity": 0.6003142695377961, - "velocityX": -2.5941051029464517, - "velocityY": -0.24153967991752093, - "timestamp": 7.6475206546884795 - }, - { - "x": 1.1517177468979243, - "y": 6.708849739610871, - "heading": 0.8615071361475246, - "angularVelocity": 0.5557125855327181, - "velocityX": -2.2230672572379273, - "velocityY": -0.20691746192426994, - "timestamp": 7.732971604739771 - }, - { - "x": 0.993443621158485, - "y": 6.694122461254965, - "heading": 0.9032823319492421, - "angularVelocity": 0.48887924331700494, - "velocityX": -1.852221954752358, - "velocityY": -0.17234774273507145, - "timestamp": 7.818422554791063 - }, - { - "x": 0.8668441821472115, - "y": 6.682345527785774, - "heading": 0.9380916664803222, - "angularVelocity": 0.40736041565660025, - "velocityX": -1.4815451312746155, - "velocityY": -0.13782097755638892, - "timestamp": 7.9038735048423545 - }, - { - "x": 0.7719075388323822, - "y": 6.673515992217611, - "heading": 0.9650448121199915, - "angularVelocity": 0.3154224221444248, - "velocityX": -1.111007464022926, - "velocityY": -0.10332869983146084, - "timestamp": 7.989324454893646 - }, - { - "x": 0.7086239541540638, - "y": 6.667631472022659, - "heading": 0.9834806533033196, - "angularVelocity": 0.21574764437724195, - "velocityX": -0.7405837458838845, - "velocityY": -0.06886430392430233, - "timestamp": 8.074775404944939 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.3203819062171384e-28, + "angularVelocity": -7.150744492931399e-29, + "velocityX": -3.49033600951782e-22, + "velocityY": -5.085821437921445e-25, + "timestamp": 3.424226754177389 + }, + { + "x": 2.7396220771473008, + "y": 5.562601682668762, + "heading": 0.0037078226871735676, + "angularVelocity": 0.0478975658583634, + "velocityX": -0.24134772812197372, + "velocityY": 0.23596385620231192, + "timestamp": 3.5016382597670024 + }, + { + "x": 2.702381046393162, + "y": 5.599259413613318, + "heading": 0.011292514660087486, + "angularVelocity": 0.09797887168250088, + "velocityX": -0.48107875528951993, + "velocityY": 0.47354370213249264, + "timestamp": 3.579049765356616 + }, + { + "x": 2.6467700945928994, + "y": 5.6544931725755525, + "heading": 0.02300756873559927, + "angularVelocity": 0.15133479172485775, + "velocityX": -0.7183809612885809, + "velocityY": 0.7135083931198739, + "timestamp": 3.656461270946229 + }, + { + "x": 2.573103126896307, + "y": 5.728602621187937, + "heading": 0.039271681184884964, + "angularVelocity": 0.21009942030462117, + "velocityX": -0.9516281479800662, + "velocityY": 0.9573441059946076, + "timestamp": 3.7338727765358426 + }, + { + "x": 2.482008526610063, + "y": 5.822157343718097, + "heading": 0.060902976833889094, + "angularVelocity": 0.27943256605393446, + "velocityX": -1.1767578939643621, + "velocityY": 1.208537694979451, + "timestamp": 3.811284282125456 + }, + { + "x": 2.3753508008518254, + "y": 5.936638558421237, + "heading": 0.09019128304072983, + "angularVelocity": 0.378345647507604, + "velocityX": -1.3778019810604039, + "velocityY": 1.4788656263843583, + "timestamp": 3.8886957877150694 + }, + { + "x": 2.2779173251453018, + "y": 6.074876969132681, + "heading": 0.1437805349404755, + "angularVelocity": 0.6922646897457585, + "velocityX": -1.258643336858141, + "velocityY": 1.7857605230454636, + "timestamp": 3.966107293304683 + }, + { + "x": 2.2032165306277185, + "y": 6.200218266147509, + "heading": 0.20002968943075358, + "angularVelocity": 0.7266252485576944, + "velocityX": -0.964983098424666, + "velocityY": 1.6191559130668154, + "timestamp": 4.043518798894296 + }, + { + "x": 2.1496243808514985, + "y": 6.310133689219402, + "heading": 0.2561009835178197, + "angularVelocity": 0.7243276520717804, + "velocityX": -0.6923021244455775, + "velocityY": 1.4198848379799673, + "timestamp": 4.12093030448391 + }, + { + "x": 2.116554400688044, + "y": 6.403853368096058, + "heading": 0.3110546267083605, + "angularVelocity": 0.7098898642001644, + "velocityX": -0.4271972223195187, + "velocityY": 1.2106685971656326, + "timestamp": 4.198341810073523 + }, + { + "x": 2.1037071305017707, + "y": 6.481007112457934, + "heading": 0.3644192794127437, + "angularVelocity": 0.6893633226472664, + "velocityX": -0.1659607326898067, + "velocityY": 0.9966702465508831, + "timestamp": 4.275753315663136 + }, + { + "x": 2.110901195101711, + "y": 6.541377623864855, + "heading": 0.41590951273781046, + "angularVelocity": 0.6651496173971262, + "velocityX": 0.09293275650881538, + "velocityY": 0.7798648398206792, + "timestamp": 4.35316482125275 + }, + { + "x": 2.138015040799913, + "y": 6.58482206941201, + "heading": 0.4653328742353283, + "angularVelocity": 0.6384498159683032, + "velocityX": 0.3502560180387482, + "velocityY": 0.5612143210012033, + "timestamp": 4.430576326842363 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.11016412225954963, - "velocityX": -0.37025347871120917, - "velocityY": -0.03442272228334841, - "timestamp": 8.160226354996231 + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.6099551352868545, + "velocityX": 0.6064539919591982, + "velocityY": 0.3412588823244487, + "timestamp": 4.507987832431977 + }, + { + "x": 2.2507078582365074, + "y": 6.648678152595124, + "heading": 0.5520861160632833, + "angularVelocity": 0.5325619619367677, + "velocityX": 0.8856294969362879, + "velocityY": 0.5043148240682422, + "timestamp": 4.582224633572456 + }, + { + "x": 2.3371668542660595, + "y": 6.698226860856562, + "heading": 0.585357485403268, + "angularVelocity": 0.4481789197385401, + "velocityX": 1.1646379518150936, + "velocityY": 0.6674413161698084, + "timestamp": 4.656461434712935 + }, + { + "x": 2.4409072196533095, + "y": 6.7586907970323, + "heading": 0.5617689305008087, + "angularVelocity": -0.31774745867379856, + "velocityX": 1.3974250478673051, + "velocityY": 0.8144738895917877, + "timestamp": 4.730698235853414 + }, + { + "x": 2.5238868313790523, + "y": 6.807069731854914, + "heading": 0.5422961869631279, + "angularVelocity": -0.2623058003379255, + "velocityX": 1.117769225652917, + "velocityY": 0.6516839906809204, + "timestamp": 4.804935036993894 + }, + { + "x": 2.586117270308082, + "y": 6.8433564035586825, + "heading": 0.5274957902762942, + "angularVelocity": -0.19936738193805836, + "velocityX": 0.8382694024122912, + "velocityY": 0.4887962728229996, + "timestamp": 4.879171838134373 + }, + { + "x": 2.6276024994187765, + "y": 6.867548495303344, + "heading": 0.5175502856493861, + "angularVelocity": -0.13397000509340504, + "velocityX": 0.5588229621073203, + "velocityY": 0.3258773461814771, + "timestamp": 4.953408639274852 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -1.2387955760475958e-29, - "velocityX": 1.44513347457817e-31, - "velocityY": -4.5041627226851734e-30, - "timestamp": 8.245677305047524 - }, - { - "x": 0.6937129747352162, - "y": 6.6645584157609425, - "heading": 0.9790660169157249, - "angularVelocity": -0.22114767057901943, - "velocityX": 0.2675140141228054, - "velocityY": -0.002104635808322496, - "timestamp": 8.308206859439915 - }, - { - "x": 0.7271833460248389, - "y": 6.664303045137076, - "heading": 0.9517582917370493, - "angularVelocity": -0.43671709232583583, - "velocityX": 0.5352728260237692, - "velocityY": -0.004083998780231037, - "timestamp": 8.370736413832306 - }, - { - "x": 0.7774150547311363, - "y": 6.663933728618546, - "heading": 0.9114059815034875, - "angularVelocity": -0.645331805506542, - "velocityX": 0.8033274696166545, - "velocityY": -0.005906271396278706, - "timestamp": 8.433265968224697 - }, - { - "x": 0.8444295954022788, - "y": 6.663462332060666, - "heading": 0.8585277843526951, - "angularVelocity": -0.8456512710608117, - "velocityX": 1.0717258634310107, - "velocityY": -0.007538780061052878, - "timestamp": 8.495795522617088 - }, - { - "x": 0.928251455530078, - "y": 6.662902493101364, - "heading": 0.793730196165171, - "angularVelocity": -1.0362713890602748, - "velocityX": 1.3405158719313994, - "velocityY": -0.008953189651546989, - "timestamp": 8.55832507700948 - }, - { - "x": 1.0289086002870482, - "y": 6.66226903623183, - "heading": 0.7177284863656903, - "angularVelocity": -1.2154526053799588, - "velocityX": 1.6097531117096386, - "velocityY": -0.010130519490973162, - "timestamp": 8.62085463140187 - }, - { - "x": 1.1464337376402622, - "y": 6.661577427221888, - "heading": 0.6314012242837341, - "angularVelocity": -1.3805833564753438, - "velocityX": 1.8795134316120001, - "velocityY": -0.011060513970755739, - "timestamp": 8.683384185794262 - }, - { - "x": 1.2808663505867972, - "y": 6.660844036954463, - "heading": 0.5359002148977876, - "angularVelocity": -1.5272939382655537, - "velocityX": 2.149905180883418, - "velocityY": -0.011728698126054097, - "timestamp": 8.745913740186653 - }, - { - "x": 1.4322545638154922, - "y": 6.660088220029616, - "heading": 0.43284520187288844, - "angularVelocity": -1.6481008704811424, - "velocityX": 2.4210665612405937, - "velocityY": -0.012087355046610722, - "timestamp": 8.808443294579044 - }, - { - "x": 1.6006535211132875, - "y": 6.65933694718367, - "heading": 0.3246624035723361, - "angularVelocity": -1.7301066567926002, - "velocityX": 2.69310982517228, - "velocityY": -0.012014684148101123, - "timestamp": 8.870972848971435 - }, - { - "x": 1.7861082105175972, - "y": 6.658632447498398, - "heading": 0.21527496164562404, - "angularVelocity": -1.7493718448763058, - "velocityX": 2.965872557487375, - "velocityY": -0.011266667292259528, - "timestamp": 8.933502403363827 - }, - { - "x": 1.9885470293511631, - "y": 6.658050737392514, - "heading": 0.11225233797545536, - "angularVelocity": -1.6475828857450405, - "velocityX": 3.2374901884507423, - "velocityY": -0.009302962599615121, - "timestamp": 8.996031957756218 - }, - { - "x": 2.206257706753187, - "y": 6.657808122247979, - "heading": 0.04234893056424459, - "angularVelocity": -1.1179258846552083, - "velocityX": 3.4817244344302054, - "velocityY": -0.0038800075722999405, - "timestamp": 9.058561512148609 - }, - { - "x": 2.437370646545927, - "y": 6.657710110655747, - "heading": 0.021561241595864127, - "angularVelocity": -0.33244581974679344, - "velocityX": 3.6960592801035483, - "velocityY": -0.0015674442779168942, - "timestamp": 9.121091066541 - }, - { - "x": 2.6738489696177092, - "y": 6.656073614188434, - "heading": 0.021561206140011186, - "angularVelocity": -5.670255175242252e-7, - "velocityX": 3.7818648376703368, - "velocityY": -0.02617156772049459, - "timestamp": 9.183620620933391 - }, - { - "x": 2.910327291911898, - "y": 6.654437005347333, - "heading": 0.021561170684433324, - "angularVelocity": -5.670211183604034e-7, - "velocityX": 3.7818648252347256, - "velocityY": -0.026173364851293936, - "timestamp": 9.246150175325782 - }, - { - "x": 3.14680561420604, - "y": 6.652800396499498, - "heading": 0.021561135228855402, - "angularVelocity": -5.670211192982607e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958984518, - "timestamp": 9.308679729718174 - }, - { - "x": 3.3832839365001823, - "y": 6.6511637876516625, - "heading": 0.02156109977327751, - "angularVelocity": -5.670211188277871e-7, - "velocityX": 3.7818648252339804, - "velocityY": -0.02617336495899097, - "timestamp": 9.371209284110565 - }, - { - "x": 3.6197622587943243, - "y": 6.649527178803828, - "heading": 0.02156106431769964, - "angularVelocity": -5.670211185570758e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958990947, - "timestamp": 9.433738838502956 - }, - { - "x": 3.8562405810884663, - "y": 6.647890569955993, - "heading": 0.02156102886212173, - "angularVelocity": -5.670211191121069e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958990954, - "timestamp": 9.496268392895347 - }, - { - "x": 4.092718903382608, - "y": 6.646253961108158, - "heading": 0.02156099340654377, - "angularVelocity": -5.670211198937042e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.02617336495899093, - "timestamp": 9.558797947287738 - }, - { - "x": 4.329197225676751, - "y": 6.6446173522603225, - "heading": 0.021560957950965815, - "angularVelocity": -5.67021119881002e-7, - "velocityX": 3.7818648252339804, - "velocityY": -0.026173364958990898, - "timestamp": 9.62132750168013 - }, - { - "x": 4.565675547970892, - "y": 6.642980743412488, - "heading": 0.021560922495387855, - "angularVelocity": -5.670211198956703e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958990943, - "timestamp": 9.68385705607252 - }, - { - "x": 4.802153870265035, - "y": 6.641344134564649, - "heading": 0.02156088703980993, - "angularVelocity": -5.670211193772164e-7, - "velocityX": 3.7818648252339795, - "velocityY": -0.026173364959042097, - "timestamp": 9.746386610464912 - }, - { - "x": 5.038632192558808, - "y": 6.639707525663405, - "heading": 0.021560851584232044, - "angularVelocity": -5.67021118724796e-7, - "velocityX": 3.781864825228069, - "velocityY": -0.02617336581314012, - "timestamp": 9.808916164857303 - }, - { - "x": 5.275110508682838, - "y": 6.638070025521481, - "heading": 0.021560816128648946, - "angularVelocity": -5.670212021371837e-7, - "velocityX": 3.781864726558842, - "velocityY": -0.02618761892412933, - "timestamp": 9.871445719249694 - }, - { - "x": 5.511317621528859, - "y": 6.626629955813148, - "heading": 0.02156078050955198, - "angularVelocity": -5.696361874718345e-7, - "velocityX": 3.777527525044398, - "velocityY": -0.18295460153997642, - "timestamp": 9.933975273642085 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06735023563211984, + "velocityX": 0.2794034362569526, + "velocityY": 0.16294311269989367, + "timestamp": 5.0276454404153315 }, { - "x": 5.7460856437683105, - "y": 6.598193168640137, - "heading": 0.021560744143059335, - "angularVelocity": -5.815888661825855e-7, - "velocityX": 3.7545129582438856, - "velocityY": -0.45477354587499197, - "timestamp": 9.996504828034476 - }, - { - "x": 5.947788996667301, - "y": 6.560838784941361, - "heading": 0.021560710996949464, - "angularVelocity": -6.111012841975544e-7, - "velocityX": 3.718722301384556, - "velocityY": -0.6886875092486842, - "timestamp": 10.050744789115704 - }, - { - "x": 6.146761517861249, - "y": 6.510943609310236, - "heading": 0.021560678630883793, - "angularVelocity": -5.967199279900907e-7, - "velocityX": 3.6683750730568514, - "velocityY": -0.9198969659363234, - "timestamp": 10.104984750196932 - }, - { - "x": 6.342740574859562, - "y": 6.4503482133201375, - "heading": 0.021560646512747805, - "angularVelocity": -5.921489497600962e-7, - "velocityX": 3.6131857968117713, - "velocityY": -1.1171725565834012, - "timestamp": 10.15922471127816 - }, - { - "x": 6.53435736566076, - "y": 6.392349027665542, - "heading": 0.005697481882948994, - "angularVelocity": -0.2924626845886352, - "velocityX": 3.532760477358042, - "velocityY": -1.0693072874395997, - "timestamp": 10.213464672359388 - }, - { - "x": 6.715860798361231, - "y": 6.336732624061585, - "heading": -0.044321515304948156, - "angularVelocity": -0.9221798133850251, - "velocityX": 3.3463046263742644, - "velocityY": -1.0253769083769828, - "timestamp": 10.267704633440616 - }, - { - "x": 6.885664142970145, - "y": 6.284563070375092, - "heading": -0.11100118706044729, - "angularVelocity": -1.2293458628342813, - "velocityX": 3.1305948828876353, - "velocityY": -0.9618287448319874, - "timestamp": 10.321944594521844 - }, - { - "x": 7.043230365082517, - "y": 6.236112426000239, - "heading": -0.18201634611185183, - "angularVelocity": -1.309277470628262, - "velocityX": 2.9049840555085917, - "velocityY": -0.8932647334000697, - "timestamp": 10.376184555603071 - }, - { - "x": 7.188521106986847, - "y": 6.19141127344733, - "heading": -0.25302239943673055, - "angularVelocity": -1.3091095920689053, - "velocityX": 2.678666042675613, - "velocityY": -0.8241368847217142, - "timestamp": 10.4304245166843 - }, - { - "x": 7.3215520348116, - "y": 6.150465382048361, - "heading": -0.3216933073087357, - "angularVelocity": -1.2660574694949878, - "velocityX": 2.4526368598519452, - "velocityY": -0.7549026692266663, - "timestamp": 10.484664477765527 - }, - { - "x": 7.4423459735084, - "y": 6.113273952325478, - "heading": -0.38654982711023245, - "angularVelocity": -1.1957331551984454, - "velocityX": 2.2270284913351, - "velocityY": -0.685683193378148, - "timestamp": 10.538904438846755 - }, - { - "x": 7.550924553999962, - "y": 6.079834322504852, - "heading": -0.4465624539391904, - "angularVelocity": -1.1064282796789968, - "velocityX": 2.001818923301949, - "velocityY": -0.6165127915661526, - "timestamp": 10.593144399927983 - }, - { - "x": 7.647306628463083, - "y": 6.050143536065283, - "heading": -0.5009752926426013, - "angularVelocity": -1.0031872740823848, - "velocityX": 1.7769569251493995, - "velocityY": -0.5473968979274295, - "timestamp": 10.64738436100921 - }, - { - "x": 7.7315082292215935, - "y": 6.024198841891014, - "heading": -0.5492132046279071, - "angularVelocity": -0.8893426732564748, - "velocityX": 1.552390508400531, - "velocityY": -0.47833172548586056, - "timestamp": 10.701624322090439 - }, - { - "x": 7.803542912083633, - "y": 6.001997808520636, - "heading": -0.5908266102991006, - "angularVelocity": -0.7672093571172569, - "velocityX": 1.3280740145473924, - "velocityY": -0.4093113809047783, - "timestamp": 10.755864283171666 - }, - { - "x": 7.863422173267542, - "y": 5.983538316289689, - "heading": -0.6254559233807054, - "angularVelocity": -0.6384464957440807, - "velocityX": 1.103969471774435, - "velocityY": -0.34033011571123517, - "timestamp": 10.810104244252894 - }, - { - "x": 7.9111558450934325, - "y": 5.968818528744131, - "heading": -0.6528074420175133, - "angularVelocity": -0.5042687732730391, - "velocityX": 0.8800462034699242, - "velocityY": -0.2713827084704847, - "timestamp": 10.864344205334122 - }, - { - "x": 7.946752441580289, - "y": 5.95783687514921, - "heading": -0.6726364642205706, - "angularVelocity": -0.36557958021692255, - "velocityX": 0.6562799046545997, - "velocityY": -0.2024642602245795, - "timestamp": 10.91858416641535 - }, - { - "x": 7.970219449253945, - "y": 5.950592046499088, - "heading": -0.6847351436740425, - "angularVelocity": -0.22305840956179235, - "velocityX": 0.43265163185705313, - "velocityY": -0.1335699455844713, - "timestamp": 10.972824127496578 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 9.95994551309633e-28, + "velocityX": -2.708487136756449e-26, + "velocityY": -1.710598190536398e-26, + "timestamp": 5.101882241555811 + }, + { + "x": 2.657033865407608, + "y": 6.8677328189813025, + "heading": 0.5065323846197595, + "angularVelocity": -0.10327905668354015, + "velocityX": 0.1491230501434533, + "velocityY": -0.20442976398577012, + "timestamp": 5.160151896348268 + }, + { + "x": 2.674422029082572, + "y": 6.843911168058592, + "heading": 0.49465789497247153, + "angularVelocity": -0.20378513807198495, + "velocityX": 0.2984085582263455, + "velocityY": -0.40881743692420125, + "timestamp": 5.218421551140726 + }, + { + "x": 2.700519894286892, + "y": 6.808182830343848, + "heading": 0.4771155795523401, + "angularVelocity": -0.30105404747312803, + "velocityX": 0.4478808961074784, + "velocityY": -0.6131551292349195, + "timestamp": 5.276691205933184 + }, + { + "x": 2.7353401457143893, + "y": 6.760551362049968, + "heading": 0.4541275879247931, + "angularVelocity": -0.3945105168277461, + "velocityX": 0.5975709235196038, + "velocityY": -0.8174317912733681, + "timestamp": 5.334960860725642 + }, + { + "x": 2.7788977690674797, + "y": 6.7010212248295815, + "heading": 0.42595830742432134, + "angularVelocity": -0.4834296788062957, + "velocityX": 0.7475181294317162, + "velocityY": -1.0216318842529362, + "timestamp": 5.393230515518099 + }, + { + "x": 2.8312107351766715, + "y": 6.629598169475443, + "heading": 0.3929264070745021, + "angularVelocity": -0.5668799732462906, + "velocityX": 0.8977737433921239, + "velocityY": -1.2257332844776365, + "timestamp": 5.451500170310557 + }, + { + "x": 2.8923009585334296, + "y": 6.546289820646855, + "heading": 0.35542214398319716, + "angularVelocity": -0.6436328347041991, + "velocityX": 1.048405444898319, + "velocityY": -1.4297038334157206, + "timestamp": 5.509769825103015 + }, + { + "x": 2.9621956903718707, + "y": 6.451106612735035, + "heading": 0.3139335291126181, + "angularVelocity": -0.7120106514849124, + "velocityX": 1.199504820946487, + "velocityY": -1.6334953115963773, + "timestamp": 5.568039479895472 + }, + { + "x": 3.040929634489161, + "y": 6.344063387407532, + "heading": 0.2690883704076084, + "angularVelocity": -0.7696142849093055, + "velocityX": 1.3511997693777469, + "velocityY": -1.8370320831445586, + "timestamp": 5.62630913468793 + }, + { + "x": 3.1285483297575434, + "y": 6.225182361182179, + "heading": 0.2217268345391298, + "angularVelocity": -0.8127993213134442, + "velocityX": 1.5036762373221262, + "velocityY": -2.0401875838938306, + "timestamp": 5.684578789480388 + }, + { + "x": 3.225113873395399, + "y": 6.094499287941398, + "heading": 0.1730380747769639, + "angularVelocity": -0.8355765953236391, + "velocityX": 1.6572183923484294, + "velocityY": -2.242729491126068, + "timestamp": 5.742848444272846 + }, + { + "x": 3.330715130826902, + "y": 5.952078350373372, + "heading": 0.12484860508535624, + "angularVelocity": -0.8270079831989134, + "velocityX": 1.8122856194639898, + "velocityY": -2.4441699212959707, + "timestamp": 5.801118099065303 + }, + { + "x": 3.4454857152688785, + "y": 5.798056975551327, + "heading": 0.08033839295880485, + "angularVelocity": -0.7638660686267251, + "velocityX": 1.9696458619973118, + "velocityY": -2.643251884203387, + "timestamp": 5.859387753857761 + }, + { + "x": 3.569612354139756, + "y": 5.632836026447166, + "heading": 0.04630815492886413, + "angularVelocity": -0.5840130364792431, + "velocityX": 2.1302106441677915, + "velocityY": -2.8354544006247413, + "timestamp": 5.917657408650219 + }, + { + "x": 3.7020237011319064, + "y": 5.458025154952382, + "heading": 0.04313214682309951, + "angularVelocity": -0.05450535303627194, + "velocityX": 2.272389418879637, + "velocityY": -3.0000327291695483, + "timestamp": 5.975927063442676 + }, + { + "x": 3.846567317399324, + "y": 5.291677583037812, + "heading": 0.04313213729822128, + "angularVelocity": -1.634620671894734e-7, + "velocityX": 2.480598465569185, + "velocityY": -2.8547890408319527, + "timestamp": 6.034196718235134 + }, + { + "x": 4.001959808565688, + "y": 5.135416438368326, + "heading": 0.043132128200091754, + "angularVelocity": -1.5613838046572486e-7, + "velocityX": 2.6667824225119015, + "velocityY": -2.6816898989027482, + "timestamp": 6.092466373027592 }, { - "x": 7.981563568115234, - "y": 5.947082996368408, - "heading": -0.6889234822214203, - "angularVelocity": -0.07721868644237204, - "velocityX": 0.2091468842372586, - "velocityY": -0.06469492346102504, - "timestamp": 11.027064088577806 - }, - { - "x": 7.980191051552582, - "y": 5.9474933088994915, - "heading": -0.684492260818683, - "angularVelocity": 0.07820368392725321, - "velocityX": -0.024222633376945003, - "velocityY": 0.007241333387762343, - "timestamp": 11.083726654587604 - }, - { - "x": 7.965589050123253, - "y": 5.951981018210289, - "heading": -0.6714197034271625, - "angularVelocity": 0.23070888440280377, - "velocityX": -0.257701026579075, - "velocityY": 0.07920060150509887, - "timestamp": 11.140389220597402 - }, - { - "x": 7.9377506970473615, - "y": 5.960547682425779, - "heading": -0.6498948487141165, - "angularVelocity": 0.37987786697348064, - "velocityX": -0.4913006070194107, - "velocityY": 0.15118736793544205, - "timestamp": 11.1970517866072 - }, - { - "x": 7.896668195667252, - "y": 5.973195099193875, - "heading": -0.6201381150730912, - "angularVelocity": 0.5251568316881386, - "velocityX": -0.7250377854932653, - "velocityY": 0.22320585986009217, - "timestamp": 11.253714352617 - }, - { - "x": 7.842332542279827, - "y": 5.989925310225715, - "heading": -0.5824114694602323, - "angularVelocity": 0.6658125155563156, - "velocityX": -0.9589338643440537, - "velocityY": 0.2952603845887764, - "timestamp": 11.310376918626798 - }, - { - "x": 7.774733202893158, - "y": 6.010740641182104, - "heading": -0.5370321516926094, - "angularVelocity": 0.8008694445602113, - "velocityX": -1.1930158506231556, - "velocityY": 0.367355953360645, - "timestamp": 11.367039484636596 - }, - { - "x": 7.693857750948662, - "y": 6.035643782735694, - "heading": -0.48439126574422153, - "angularVelocity": 0.9290240392446375, - "velocityX": -1.4273171449826625, - "velocityY": 0.43949900802735137, - "timestamp": 11.423702050646394 - }, - { - "x": 7.5996914832698845, - "y": 6.064637893376439, - "heading": -0.4249792327859352, - "angularVelocity": 1.0485235163549331, - "velocityX": -1.6618779259395846, - "velocityY": 0.5116978047858136, - "timestamp": 11.480364616656193 - }, - { - "x": 7.492217045643208, - "y": 6.097726656744903, - "heading": -0.35942190954654185, - "angularVelocity": 1.156977663667012, - "velocityX": -1.8967449798883769, - "velocityY": 0.583961611670435, - "timestamp": 11.537027182665991 - }, - { - "x": 7.371414140499637, - "y": 6.1349141479804095, - "heading": -0.28853586070822274, - "angularVelocity": 1.251020803153572, - "velocityX": -2.131970252153457, - "velocityY": 0.6562973379828368, - "timestamp": 11.59368974867579 - }, - { - "x": 7.237259574639344, - "y": 6.176204237589326, - "heading": -0.21342323245219458, - "angularVelocity": 1.3256128965822067, - "velocityX": -2.367604845800615, - "velocityY": 0.7287013722918435, - "timestamp": 11.650352314685588 - }, - { - "x": 7.0897287772168305, - "y": 6.221599002117526, - "heading": -0.13565860715800612, - "angularVelocity": 1.372416231215901, - "velocityX": -2.6036730739833454, - "velocityY": 0.8011420541800114, - "timestamp": 11.707014880695386 - }, - { - "x": 6.928804270313556, - "y": 6.271094657477637, - "heading": -0.05771767479301098, - "angularVelocity": 1.3755277576295868, - "velocityX": -2.8400497583439677, - "velocityY": 0.8735159532230397, - "timestamp": 11.763677446705184 - }, - { - "x": 6.754523159333334, - "y": 6.324666148768191, - "heading": 0.015819291394400217, - "angularVelocity": 1.297805083071882, - "velocityX": -3.0757715940730104, - "velocityY": 0.9454476749480591, - "timestamp": 11.820340012714983 - }, - { - "x": 6.56735113931042, - "y": 6.382062750762148, - "heading": 0.07327050352161106, - "angularVelocity": 1.0139182916156146, - "velocityX": -3.3032746873932677, - "velocityY": 1.012954513638379, - "timestamp": 11.877002578724781 - }, - { - "x": 6.368937324018353, - "y": 6.441765360809047, - "heading": 0.09523150855737006, - "angularVelocity": 0.38757519438780325, - "velocityX": -3.5016736668395723, - "velocityY": 1.053651718430401, - "timestamp": 11.93366514473458 - }, - { - "x": 6.164194285631387, - "y": 6.50503273811371, - "heading": 0.09523154205905172, - "angularVelocity": 5.912489325251288e-7, - "velocityX": -3.6133739222393086, - "velocityY": 1.1165639285330458, - "timestamp": 11.990327710744378 - }, - { - "x": 5.956663061240617, - "y": 6.558448687512427, - "heading": 0.09523157576982937, - "angularVelocity": 5.949391283503271e-7, - "velocityX": -3.6625807654896136, - "velocityY": 0.9427026193886323, - "timestamp": 12.046990276754176 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.043132118825415074, + "angularVelocity": -1.6088436959619284e-7, + "velocityX": 2.84091706777896, + "velocityY": -2.4964733877097585, + "timestamp": 6.15073602782005 + }, + { + "x": 4.269351778492494, + "y": 4.907420759942678, + "heading": 0.043132109937133276, + "angularVelocity": -2.564268066942143e-7, + "velocityX": 2.938449360926388, + "velocityY": -2.3809038425071845, + "timestamp": 6.1853980895345355 + }, + { + "x": 4.374422315135501, + "y": 4.829031543571565, + "heading": 0.043132101433653904, + "angularVelocity": -2.4532526192426324e-7, + "velocityX": 3.0312835257314057, + "velocityY": -2.2615278057263164, + "timestamp": 6.2200601512490215 + }, + { + "x": 4.482542676912381, + "y": 4.754905472881388, + "heading": 0.04313209323726653, + "angularVelocity": -2.3646566200370356e-7, + "velocityX": 3.119270938568917, + "velocityY": -2.1385361119243003, + "timestamp": 6.254722212963507 + }, + { + "x": 4.593539986069964, + "y": 4.685161050857923, + "heading": 0.04313208527987584, + "angularVelocity": -2.2957061125967145e-7, + "velocityX": 3.202270830623841, + "velocityY": -2.0121256086251895, + "timestamp": 6.289384274677993 + }, + { + "x": 4.707236757335738, + "y": 4.619909757466469, + "heading": 0.043132077500472085, + "angularVelocity": -2.244356903685004e-7, + "velocityX": 3.2801502750269638, + "velocityY": -1.8824989098725307, + "timestamp": 6.324046336392479 + }, + { + "x": 4.823451163522863, + "y": 4.559255830076377, + "heading": 0.0431320698430563, + "angularVelocity": -2.2091633918596145e-7, + "velocityX": 3.352784007610117, + "velocityY": -1.7498649644588975, + "timestamp": 6.358708398106965 + }, + { + "x": 4.941997186751783, + "y": 4.50329577732449, + "heading": 0.043132062254893876, + "angularVelocity": -2.1891838085258928e-7, + "velocityX": 3.4200511269465594, + "velocityY": -1.6144467462101488, + "timestamp": 6.393370459821451 + }, + { + "x": 5.061738167339899, + "y": 4.449940508624331, + "heading": 0.04313205468599839, + "angularVelocity": -2.1836252987087912e-7, + "velocityX": 3.454525630195652, + "velocityY": -1.5392987624235874, + "timestamp": 6.428032521535937 + }, + { + "x": 5.18147938835374, + "y": 4.396585779495745, + "heading": 0.04313204711710543, + "angularVelocity": -2.1836245696155624e-7, + "velocityX": 3.4545325664744473, + "velocityY": -1.53928319579118, + "timestamp": 6.462694583250423 + }, + { + "x": 5.30122060959038, + "y": 4.343231050867178, + "heading": 0.043132039548212486, + "angularVelocity": -2.1836245655235923e-7, + "velocityX": 3.454532572902229, + "velocityY": -1.539283181365645, + "timestamp": 6.497356644964909 + }, + { + "x": 5.420963132245964, + "y": 4.289879243040779, + "heading": 0.043132031979322576, + "angularVelocity": -2.1836236883930228e-7, + "velocityX": 3.4545701188207345, + "velocityY": -1.5391989162636184, + "timestamp": 6.532018706679395 + }, + { + "x": 5.54252534628876, + "y": 4.240814844116505, + "heading": 0.04313202440333319, + "angularVelocity": -2.185671886990838e-7, + "velocityX": 3.507068190118379, + "velocityY": -1.4155072288665276, + "timestamp": 6.566680768393881 }, { - "x": 5.7460856437683105, - "y": 6.598193168640137, - "heading": 0.09523161032614703, - "angularVelocity": 6.098615032122225e-7, - "velocityX": -3.716341004321884, - "velocityY": 0.7014239545882616, - "timestamp": 12.103652842763974 - }, - { - "x": 5.5027055023001905, - "y": 6.62552340619542, - "heading": 0.09523164151085345, - "angularVelocity": 4.815615607402516e-7, - "velocityX": -3.7583333072466103, - "velocityY": 0.4220399473818025, - "timestamp": 12.16841030879256 - }, - { - "x": 5.257964269234122, - "y": 6.634609890080181, - "heading": 0.09523167208339899, - "angularVelocity": 4.721084288711453e-7, - "velocityX": -3.7793516033817274, - "velocityY": 0.140315618291052, - "timestamp": 12.233167774821144 - }, - { - "x": 5.013061040594942, - "y": 6.636410996869712, - "heading": 0.09523170256730593, - "angularVelocity": 4.70739650837411e-7, - "velocityX": -3.781853177069622, - "velocityY": 0.02781311406988273, - "timestamp": 12.297925240849729 - }, - { - "x": 4.7681578086493825, - "y": 6.6382116540225224, - "heading": 0.09523173305121174, - "angularVelocity": 4.7073963340734453e-7, - "velocityX": -3.781853228127505, - "velocityY": 0.027806170674068813, - "timestamp": 12.362682706878314 - }, - { - "x": 4.5232545767036525, - "y": 6.640012311152163, - "heading": 0.09523176353511749, - "angularVelocity": 4.707396321471799e-7, - "velocityX": -3.781853228130136, - "velocityY": 0.02780617031625758, - "timestamp": 12.427440172906898 - }, - { - "x": 4.2783513447579224, - "y": 6.641812968281802, - "heading": 0.09523179401902328, - "angularVelocity": 4.707396330675372e-7, - "velocityX": -3.781853228130136, - "velocityY": 0.027806170316239148, - "timestamp": 12.492197638935483 - }, - { - "x": 4.033448112812192, - "y": 6.64361362541144, - "heading": 0.09523182450292901, - "angularVelocity": 4.707396322379774e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.027806170316239207, - "timestamp": 12.556955104964068 - }, - { - "x": 3.788544880866463, - "y": 6.645414282541079, - "heading": 0.09523185498683476, - "angularVelocity": 4.7073963225441474e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.02780617031623923, - "timestamp": 12.621712570992653 - }, - { - "x": 3.5436416489207327, - "y": 6.647214939670717, - "heading": 0.09523188547074045, - "angularVelocity": 4.707396314544476e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.02780617031623925, - "timestamp": 12.686470037021238 - }, - { - "x": 3.2987384169750027, - "y": 6.649015596800356, - "heading": 0.09523191595464614, - "angularVelocity": 4.7073963141347726e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.027806170316239207, - "timestamp": 12.751227503049822 - }, - { - "x": 3.0538351850292726, - "y": 6.650816253929995, - "heading": 0.09523194643855185, - "angularVelocity": 4.7073963175583744e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.027806170316239227, - "timestamp": 12.815984969078407 - }, - { - "x": 2.8089319530835426, - "y": 6.652616911059632, - "heading": 0.09523197692245748, - "angularVelocity": 4.7073963058043316e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.02780617031621524, - "timestamp": 12.880742435106992 - }, - { - "x": 2.5640287211375905, - "y": 6.654417568159032, - "heading": 0.09523200740636317, - "angularVelocity": 4.707396315238404e-7, - "velocityX": -3.78185322813357, - "velocityY": 0.027806169849281307, - "timestamp": 12.945499901135577 - }, - { - "x": 2.319125484878767, - "y": 6.656217638494738, - "heading": 0.09523203789215427, - "angularVelocity": 4.707687462497284e-7, - "velocityX": -3.7818532947339327, - "velocityY": 0.02779710890650077, - "timestamp": 13.010257367164161 - }, - { - "x": 2.086765993499063, - "y": 6.65613072972364, - "heading": 0.1392662443558495, - "angularVelocity": 0.6799865585268291, - "velocityX": -3.588149840161109, - "velocityY": -0.0013420656555584189, - "timestamp": 13.075014833192746 - }, - { - "x": 1.8694746583337432, - "y": 6.65699037980207, - "heading": 0.2298866992043259, - "angularVelocity": 1.399382347797175, - "velocityX": -3.3554638328406146, - "velocityY": 0.013274918417153406, - "timestamp": 13.139772299221331 - }, - { - "x": 1.6701637818051993, - "y": 6.657989601285878, - "heading": 0.3342097566920158, - "angularVelocity": 1.6109811560823084, - "velocityX": -3.077805367501045, - "velocityY": 0.015430212840099346, - "timestamp": 13.204529765249916 - }, - { - "x": 1.4891465820964496, - "y": 6.659013009703433, - "heading": 0.4396201310941332, - "angularVelocity": 1.6277717592530314, - "velocityX": -2.7953101134137524, - "velocityY": 0.01580371315183694, - "timestamp": 13.2692872312785 - }, - { - "x": 1.326396709734208, - "y": 6.660011981353038, - "heading": 0.5409065147827995, - "angularVelocity": 1.5640881260541826, - "velocityX": -2.5132217540816426, - "velocityY": 0.015426354841678473, - "timestamp": 13.334044697307085 - }, - { - "x": 1.1818610470561817, - "y": 6.660956545539143, - "heading": 0.6351810352227076, - "angularVelocity": 1.4558092868904808, - "velocityX": -2.231953650166405, - "velocityY": 0.014586182011631477, - "timestamp": 13.39880216333567 - }, - { - "x": 1.0554921038308631, - "y": 6.661825694692396, - "heading": 0.7206099929025545, - "angularVelocity": 1.3192140291922123, - "velocityX": -1.9514189015601402, - "velocityY": 0.013421605361615121, - "timestamp": 13.463559629364255 - }, - { - "x": 0.9472506642705061, - "y": 6.66260279420219, - "heading": 0.7959103128896443, - "angularVelocity": 1.1628052270274385, - "velocityX": -1.6714897323588642, - "velocityY": 0.012000153147596032, - "timestamp": 13.52831709539284 - }, - { - "x": 0.8571042421570496, - "y": 6.663273874000479, - "heading": 0.8601164354198797, - "angularVelocity": 0.9914860242044418, - "velocityX": -1.3920622229669246, - "velocityY": 0.010362971861702049, - "timestamp": 13.593074561421425 - }, - { - "x": 0.7850255723133412, - "y": 6.6638271232273425, - "heading": 0.912467741181329, - "angularVelocity": 0.8084211593199297, - "velocityX": -1.1130557488443436, - "velocityY": 0.00854340450286573, - "timestamp": 13.65783202745001 - }, - { - "x": 0.7309916972965728, - "y": 6.664252697034555, - "heading": 0.952354006476902, - "angularVelocity": 0.6159330767817102, - "velocityX": -0.8344037889456237, - "velocityY": 0.0065718106855080505, - "timestamp": 13.722589493478594 - }, - { - "x": 0.6949834963208631, - "y": 6.664542513080523, - "heading": 0.9792885970471746, - "angularVelocity": 0.41593027371366526, - "velocityX": -0.556047096713378, - "velocityY": 0.0044754074509330615, - "timestamp": 13.787346959507179 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.043132016771197046, + "angularVelocity": -2.201870219655876e-7, + "velocityX": 3.560849425193226, + "velocityY": -1.27418183966834, + "timestamp": 6.601342830108367 + }, + { + "x": 5.795862778757906, + "y": 4.156160077246944, + "heading": 0.04313200930767993, + "angularVelocity": -2.0743572849750813e-7, + "velocityX": 3.6106560546560362, + "velocityY": -1.1253226214863992, + "timestamp": 6.637322731813372 + }, + { + "x": 5.927341987568784, + "y": 4.121096456952301, + "heading": 0.043132001920743686, + "angularVelocity": -2.053072936698873e-7, + "velocityX": 3.65424035587593, + "velocityY": -0.9745335210230772, + "timestamp": 6.673302633518377 + }, + { + "x": 6.059882853366221, + "y": 4.090288037380679, + "heading": 0.04313199455852331, + "angularVelocity": -2.0462035814218294e-7, + "velocityX": 3.683747301038337, + "velocityY": -0.8562674746645255, + "timestamp": 6.709282535223382 + }, + { + "x": 6.1932695032956575, + "y": 4.063376303154627, + "heading": 0.04313198718639407, + "angularVelocity": -2.0489575822100242e-7, + "velocityX": 3.70725442840443, + "velocityY": -0.7479657517326737, + "timestamp": 6.745262436928387 + }, + { + "x": 6.327657938761633, + "y": 4.042022422624805, + "heading": 0.0431319797584895, + "angularVelocity": -2.0644593838934648e-7, + "velocityX": 3.7350973487312924, + "velocityY": -0.593494687809322, + "timestamp": 6.781242338633392 + }, + { + "x": 6.462816723725205, + "y": 4.02626353633582, + "heading": 0.04313197070248067, + "angularVelocity": -2.5169631946321463e-7, + "velocityX": 3.7565078990966625, + "velocityY": -0.437991365796106, + "timestamp": 6.817222240338397 + }, + { + "x": 6.597942968053082, + "y": 4.016131413139355, + "heading": 0.041078551168148146, + "angularVelocity": -0.057071293611868974, + "velocityX": 3.7556034876292976, + "velocityY": -0.28160508273582235, + "timestamp": 6.853202142043402 + }, + { + "x": 6.728400948341844, + "y": 4.008819261956334, + "heading": 0.03307669986160184, + "angularVelocity": -0.2223978089810408, + "velocityX": 3.625857050927263, + "velocityY": -0.20322877041112014, + "timestamp": 6.8891820437484075 + }, + { + "x": 6.853599758247463, + "y": 4.003544549173221, + "heading": 0.023873515901943563, + "angularVelocity": -0.25578680106227425, + "velocityX": 3.479687380252149, + "velocityY": -0.146601645172889, + "timestamp": 6.925161945453413 + }, + { + "x": 6.973435569834727, + "y": 4.000040202670832, + "heading": 0.014893371807540221, + "angularVelocity": -0.24958778842784254, + "velocityX": 3.3306319892084315, + "velocityY": -0.09739733396497403, + "timestamp": 6.961141847158418 + }, + { + "x": 7.087871539921083, + "y": 3.9981763906039123, + "heading": 0.00680556839180941, + "angularVelocity": -0.22478670125454425, + "velocityX": 3.1805526047459085, + "velocityY": -0.05180147745263738, + "timestamp": 6.997121748863423 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.21010218588179536, - "velocityX": -0.2779301721676604, - "velocityY": 0.0022778009813890303, - "timestamp": 13.852104425535764 + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 2.7190146206752155e-27, + "angularVelocity": -0.18914916576502855, + "velocityX": 3.0299787445803052, + "velocityY": -0.008330941864390906, + "timestamp": 7.033101650568428 + }, + { + "x": 7.373371859473657, + "y": 4.00211797408274, + "heading": -0.007235572451152828, + "angularVelocity": -0.11324239038273202, + "velocityX": 2.7620815980432543, + "velocityY": 0.06638014407023476, + "timestamp": 7.096996208310439 + }, + { + "x": 7.5322864788781265, + "y": 4.0091810283968865, + "heading": -0.011195506650135086, + "angularVelocity": -0.061976079636882384, + "velocityX": 2.4871385767489596, + "velocityY": 0.1105423460737418, + "timestamp": 7.16089076605245 + }, + { + "x": 7.673469429892467, + "y": 4.017826933148271, + "heading": -0.012870709573510192, + "angularVelocity": -0.02621824115504676, + "velocityX": 2.209624043168106, + "velocityY": 0.13531519830364583, + "timestamp": 7.224785323794461 + }, + { + "x": 7.796860934793418, + "y": 4.0272116407494, + "heading": -0.012933719915114673, + "angularVelocity": -0.0009861613231427384, + "velocityX": 1.9311739412795161, + "velocityY": 0.14687804302553767, + "timestamp": 7.288679881536472 + }, + { + "x": 7.9024438065227125, + "y": 4.036727062241868, + "heading": -0.011868644520280696, + "angularVelocity": 0.016669266248535024, + "velocityX": 1.652454848433405, + "velocityY": 0.14892381806427465, + "timestamp": 7.3525744392784835 + }, + { + "x": 7.990219348757821, + "y": 4.045915777689649, + "heading": -0.01003983054789743, + "angularVelocity": 0.02862237469061955, + "velocityX": 1.3737561591633758, + "velocityY": 0.14381061192852837, + "timestamp": 7.416468997020495 + }, + { + "x": 8.060197028820047, + "y": 4.054421881048936, + "heading": -0.007731072178165808, + "angularVelocity": 0.03613388137145833, + "velocityX": 1.095205640905745, + "velocityY": 0.13312719674235798, + "timestamp": 7.480363554762506 + }, + { + "x": 8.112389742702678, + "y": 4.061960888187355, + "heading": -0.005169493803012325, + "angularVelocity": 0.04009071297584442, + "velocityX": 0.8168569550691795, + "velocityY": 0.11799138150167889, + "timestamp": 7.544258112504517 + }, + { + "x": 8.146811548767705, + "y": 4.068300369491041, + "heading": -0.0025408878888210154, + "angularVelocity": 0.04113974659320609, + "velocityX": 0.5387282936367062, + "velocityY": 0.09921786029546131, + "timestamp": 7.608152670246528 }, { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -1.2401951851203077e-33, - "velocityX": -4.2513783914380976e-39, - "velocityY": -1.4668246371137104e-32, - "timestamp": 13.916861891564348 - } - ], - "constraints": [ + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -6.2580090272333915e-27, + "angularVelocity": 0.039766890618140496, + "velocityX": 0.26082055062629916, + "velocityY": 0.07741796101811332, + "timestamp": 7.672047227988539 + }, + { + "x": 8.160341709324497, + "y": 4.076804968421738, + "heading": 0.002530252207813134, + "angularVelocity": 0.03600456006800818, + "velocityX": -0.044607809890755405, + "velocityY": 0.050629212451328946, + "timestamp": 7.742323109794449 + }, + { + "x": 8.135735380743824, + "y": 4.078564680386853, + "heading": 0.0047972294947691944, + "angularVelocity": 0.03225825459176822, + "velocityX": -0.3501390227821203, + "velocityY": 0.025040055277777168, + "timestamp": 7.812598991600359 + }, + { + "x": 8.089649471280776, + "y": 4.078625632726097, + "heading": 0.006802259736664781, + "angularVelocity": 0.02853084430065403, + "velocityX": -0.6557855736386108, + "velocityY": 0.0008673294119977214, + "timestamp": 7.882874873406268 + }, + { + "x": 8.022074877026332, + "y": 4.077107138786556, + "heading": 0.008546934575106793, + "angularVelocity": 0.024826082485318403, + "velocityX": -0.9615616697784329, + "velocityY": -0.02160761132439043, + "timestamp": 7.953150755212178 + }, + { + "x": 7.933001389840782, + "y": 4.074154809915227, + "heading": 0.010033199259775565, + "angularVelocity": 0.02114900085883774, + "velocityX": -1.2674830239990063, + "velocityY": -0.04201055604656103, + "timestamp": 8.023426637018089 + }, + { + "x": 7.822417663549702, + "y": 4.069950298090805, + "heading": 0.0112634877661819, + "angularVelocity": 0.017506553810369533, + "velocityX": -1.573565830116407, + "velocityY": -0.05982865979589635, + "timestamp": 8.093702518824 + }, + { + "x": 7.690311402375729, + "y": 4.064726507930881, + "heading": 0.012240935764081735, + "angularVelocity": 0.013908726191431891, + "velocityX": -1.8798236006319884, + "velocityY": -0.07433261633559884, + "timestamp": 8.16397840062991 + }, + { + "x": 7.536670182325497, + "y": 4.058792719262719, + "heading": 0.012969736698331166, + "angularVelocity": 0.010370569753393549, + "velocityX": -2.186258160012313, + "velocityY": -0.08443563446915123, + "timestamp": 8.23425428243582 + }, + { + "x": 7.361484114735934, + "y": 4.052579188685834, + "heading": 0.013455784581129835, + "angularVelocity": 0.006916282945279149, + "velocityX": -2.4928334314380707, + "velocityY": -0.08841625913776113, + "timestamp": 8.304530164241731 + }, + { + "x": 7.1647543643759946, + "y": 4.04672437363507, + "heading": 0.013707961220853816, + "angularVelocity": 0.0035883810098669257, + "velocityX": -2.7993921286291723, + "velocityY": -0.08331186888461464, + "timestamp": 8.374806046047642 + }, + { + "x": 6.946523514994072, + "y": 4.042270292544886, + "heading": 0.013741127121785043, + "angularVelocity": 0.000471938595133185, + "velocityX": -3.1053448747130243, + "velocityY": -0.06337993883142518, + "timestamp": 8.445081927853552 + }, + { + "x": 6.707013005959754, + "y": 4.041197936309267, + "heading": 0.013584877310938924, + "angularVelocity": -0.0022233774494306753, + "velocityX": -3.408146619857499, + "velocityY": -0.015259235573605845, + "timestamp": 8.515357809659463 + }, + { + "x": 6.44761461575628, + "y": 4.04841614446901, + "heading": 0.013321799367880745, + "angularVelocity": -0.00374350255447176, + "velocityX": -3.6911438681038233, + "velocityY": 0.10271245232721735, + "timestamp": 8.585633691465373 + }, + { + "x": 6.183335125687841, + "y": 4.076620833220385, + "heading": 0.013321798780147703, + "angularVelocity": -8.363225426855376e-9, + "velocityX": -3.7606001273428835, + "velocityY": 0.4013423670623127, + "timestamp": 8.655909573271284 + }, + { + "x": 5.922208761336389, + "y": 4.126140293981494, + "heading": 0.013321798506864218, + "angularVelocity": -3.888723659827553e-9, + "velocityX": -3.715732305894624, + "velocityY": 0.70464374816203, + "timestamp": 8.726185455077195 + }, { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.013321798215407589, + "angularVelocity": -4.14732085648961e-9, + "velocityX": -3.6464435013896566, + "velocityY": 1.0033140639584306, + "timestamp": 8.796461336883105 + }, + { + "x": 5.541094042237222, + "y": 4.236435082517705, + "heading": 0.013321797912516515, + "angularVelocity": -8.741536718768107e-9, + "velocityX": -3.6034342058048336, + "velocityY": 1.1482373727249326, + "timestamp": 8.831110973716337 + }, + { + "x": 5.417926089652793, + "y": 4.28117906555146, + "heading": 0.013321797636094018, + "angularVelocity": -7.977644916262235e-9, + "velocityX": -3.554667922704023, + "velocityY": 1.2913261760609271, + "timestamp": 8.865760610549568 + }, + { + "x": 5.2966446591636975, + "y": 4.330809533782675, + "heading": 0.013321797380978714, + "angularVelocity": -7.362712230518396e-9, + "velocityX": -3.50022226994255, + "velocityY": 1.4323517579733251, + "timestamp": 8.9004102473828 + }, + { + "x": 5.177443524299453, + "y": 4.385247189609289, + "heading": 0.013321797143113006, + "angularVelocity": -6.86488311115801e-9, + "velocityX": -3.440184248913247, + "velocityY": 1.5710887848153525, + "timestamp": 8.935059884216031 + }, + { + "x": 5.060513134161878, + "y": 4.444405054072411, + "heading": 0.013321796919246185, + "angularVelocity": -6.460870636805561e-9, + "velocityX": -3.374649803701036, + "velocityY": 1.7073155700837472, + "timestamp": 8.969709521049262 + }, + { + "x": 4.946040308560799, + "y": 4.508188605110022, + "heading": 0.013321796706722, + "angularVelocity": -6.133518370810969e-9, + "velocityX": -3.3037236768754394, + "velocityY": 1.8408144173228282, + "timestamp": 9.004359157882494 + }, + { + "x": 4.83420793799034, + "y": 4.576495926647655, + "heading": 0.013321796503327139, + "angularVelocity": -5.870043129182345e-9, + "velocityX": -3.2275192697894797, + "velocityY": 1.9713719328833563, + "timestamp": 9.039008794715725 + }, + { + "x": 4.725194685566166, + "y": 4.649217863919521, + "heading": 0.01332179630717999, + "angularVelocity": -5.660871666352306e-9, + "velocityX": -3.146158585986232, + "velocityY": 2.098779205735374, + "timestamp": 9.073658431548957 + }, + { + "x": 4.61917463890979, + "y": 4.726238115287908, + "heading": 0.013321796115042023, + "angularVelocity": -5.545165384203739e-9, + "velocityX": -3.0597736757430263, + "velocityY": 2.2228299747869245, + "timestamp": 9.108308068382188 + }, + { + "x": 4.5101613609880875, + "y": 4.798960014309373, + "heading": 0.013321795918491775, + "angularVelocity": -5.6725052693856055e-9, + "velocityX": -3.146159321853369, + "velocityY": 2.098778101816108, + "timestamp": 9.14295770521542 + }, + { + "x": 4.398328964470022, + "y": 4.867267293334104, + "heading": 0.01332179571466789, + "angularVelocity": -5.882424857258076e-9, + "velocityX": -3.2275200186459836, + "velocityY": 1.971370705946904, + "timestamp": 9.17760734204865 + }, + { + "x": 4.283856115908692, + "y": 4.93105079879567, + "heading": 0.013321794090551628, + "angularVelocity": -4.687253345471844e-8, + "velocityX": -3.3037243395158984, + "velocityY": 1.8408131019830805, + "timestamp": 9.212256978881882 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.012938317898929116, + "angularVelocity": -0.01106724995324455, + "velocityX": -3.3581030320620826, + "velocityY": 1.6997868507459994, + "timestamp": 9.246906615715114 + }, + { + "x": 3.941212702918835, + "y": 5.086810099235691, + "heading": 0.011498426607566257, + "angularVelocity": -0.020926455047297343, + "velocityX": -3.2887006266835073, + "velocityY": 1.4077344931260583, + "timestamp": 9.315713840259761 + }, + { + "x": 3.730319373177593, + "y": 5.1698990492176655, + "heading": 0.009891204024216577, + "angularVelocity": -0.02335834055196905, + "velocityX": -3.0649881772864958, + "velocityY": 1.2075614229732525, + "timestamp": 9.384521064804408 + }, + { + "x": 3.537102784143612, + "y": 5.242302243244861, + "heading": 0.008284119125689052, + "angularVelocity": -0.023356339529212095, + "velocityX": -2.8080857833265287, + "velocityY": 1.0522615104205277, + "timestamp": 9.453328289349056 + }, + { + "x": 3.3623897849801216, + "y": 5.305525598269376, + "heading": 0.006750375983753274, + "angularVelocity": -0.02229043755340787, + "velocityX": -2.5391664947933337, + "velocityY": 0.9188476274535745, + "timestamp": 9.522135513893703 + }, + { + "x": 3.2065870996303723, + "y": 5.360439206716265, + "heading": 0.005330250476746595, + "angularVelocity": -0.020639191834938647, + "velocityX": -2.2643361417470387, + "velocityY": 0.7980791088478895, + "timestamp": 9.59094273843835 + }, + { + "x": 3.0699322051681737, + "y": 5.407605971353294, + "heading": 0.004049089893736221, + "angularVelocity": -0.01861956490018075, + "velocityX": -1.98605444946448, + "velocityY": 0.6854914574620572, + "timestamp": 9.659749962982998 + }, + { + "x": 2.9525794175973394, + "y": 5.4474188354837905, + "heading": 0.002924278144664975, + "angularVelocity": -0.01634729138567971, + "velocityX": -1.7055300275145737, + "velocityY": 0.5786145916213083, + "timestamp": 9.728557187527645 + }, + { + "x": 2.8546365555421396, + "y": 5.480167300776495, + "heading": 0.001968466770060391, + "angularVelocity": -0.013891148508459003, + "velocityX": -1.4234386389418605, + "velocityY": 0.47594515705912777, + "timestamp": 9.797364412072293 + }, + { + "x": 2.776182983656432, + "y": 5.506073375448153, + "heading": 0.0011912715035962884, + "angularVelocity": -0.011295256735138296, + "velocityX": -1.140193815473562, + "velocityY": 0.3765022473017808, + "timestamp": 9.86617163661694 + }, + { + "x": 2.717279457569953, + "y": 5.525312645366718, + "heading": 0.0006002466315783451, + "angularVelocity": -0.008589575817499225, + "velocityX": -0.8560660087118843, + "velocityY": 0.2796111897534987, + "timestamp": 9.934978861161587 + }, + { + "x": 2.6779739270458034, + "y": 5.538027422315637, + "heading": 0.0002014837935087551, + "angularVelocity": -0.0057953629245841975, + "velocityX": -0.5712413308960773, + "velocityY": 0.18478840024522195, + "timestamp": 10.003786085706235 }, { - "scope": [ - 2 - ], - "type": "StopPoint" + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 2.777517455024623e-28, + "angularVelocity": -0.0029282360223382743, + "velocityX": -0.28585310661942626, + "velocityY": 0.0916755910664572, + "timestamp": 10.072593310250882 }, { - "scope": [ - 5 - ], - "type": "StopPoint" + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 1.7679062604492033e-28, + "angularVelocity": -4.39985022540959e-29, + "velocityX": -3.674462511689619e-27, + "velocityY": -3.4538031779033175e-27, + "timestamp": 10.14140053479553 + }, + { + "x": 2.6750735377327333, + "y": 5.538716932149931, + "heading": -1.742535482636501e-18, + "angularVelocity": -2.7369611075472657e-17, + "velocityX": 0.2633769919204766, + "velocityY": -0.08824745983951204, + "timestamp": 10.20506733218207 + }, + { + "x": 2.7085920132371135, + "y": 5.52742584903886, + "heading": -8.352558789144994e-18, + "angularVelocity": -1.0382214243405793e-16, + "velocityX": 0.526467120701534, + "velocityY": -0.17734649102137234, + "timestamp": 10.26873412956861 + }, + { + "x": 2.758838975509705, + "y": 5.510398675567148, + "heading": -1.408942364356953e-17, + "angularVelocity": -9.01076399315566e-17, + "velocityX": 0.7892176822956318, + "velocityY": -0.26744196615285204, + "timestamp": 10.33240092695515 + }, + { + "x": 2.8257884382416396, + "y": 5.4875601790142925, + "heading": -1.8320735772591408e-17, + "angularVelocity": -6.646026347783005e-17, + "velocityX": 1.0515600828083749, + "velocityY": -0.35871910462523465, + "timestamp": 10.396067724341691 + }, + { + "x": 2.9094085873516815, + "y": 5.458819716039827, + "heading": -2.1055365951456844e-17, + "angularVelocity": -4.295221828639974e-17, + "velocityX": 1.3134027867360427, + "velocityY": -0.4514199575639651, + "timestamp": 10.459734521728231 + }, + { + "x": 3.0096595933993924, + "y": 5.424065969220243, + "heading": -2.2717586087398788e-17, + "angularVelocity": -2.6108116070725826e-17, + "velocityX": 1.5746198986428648, + "velocityY": -0.5458692481197496, + "timestamp": 10.523401319114772 + }, + { + "x": 3.126490185266761, + "y": 5.383158986628498, + "heading": -2.29080313224255e-17, + "angularVelocity": -2.991280273573379e-18, + "velocityX": 1.83503170668463, + "velocityY": -0.642516731969203, + "timestamp": 10.587068116501312 + }, + { + "x": 3.2598319755407235, + "y": 5.335917551231157, + "heading": -1.862517634661648e-17, + "angularVelocity": 6.726983532294288e-17, + "velocityX": 2.0943693690826515, + "velocityY": -0.7420105508138521, + "timestamp": 10.650734913887852 + }, + { + "x": 3.4095893646013797, + "y": 5.282097889138944, + "heading": -1.2913797720128003e-17, + "angularVelocity": 8.970733350596672e-17, + "velocityX": 2.3522054698531947, + "velocityY": -0.8453332710526835, + "timestamp": 10.714401711274393 + }, + { + "x": 3.575619784494897, + "y": 5.221354832224988, + "heading": -5.8099440020816116e-18, + "angularVelocity": 1.1157862511662809e-16, + "velocityX": 2.607802287988438, + "velocityY": -0.9540774690639187, + "timestamp": 10.778068508660933 + }, + { + "x": 3.7576895466655613, + "y": 5.153162990546568, + "heading": 2.5414927254501756e-18, + "angularVelocity": 1.3117412953490323e-16, + "velocityX": 2.859728612784813, + "velocityY": -1.0710738481850424, + "timestamp": 10.841735306047473 + }, + { + "x": 3.955353492285648, + "y": 5.076630508571263, + "heading": 1.2126107416115352e-17, + "angularVelocity": 1.5054337714518245e-16, + "velocityX": 3.1046629284650207, + "velocityY": -1.202078400624624, + "timestamp": 10.905402103434014 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 1.991506320539766e-17, + "angularVelocity": 1.2233936853955328e-16, + "velocityX": 3.332122579144681, + "velocityY": -1.3615057810598787, + "timestamp": 10.969068900820554 + }, + { + "x": 4.285701420717343, + "y": 4.93838888948332, + "heading": 2.4571932720973296e-17, + "angularVelocity": 1.3447790808928575e-16, + "velocityX": 3.4133671603880513, + "velocityY": -1.488883004107591, + "timestamp": 11.00369815391993 + }, + { + "x": 4.403715683241863, + "y": 4.881601537504843, + "heading": 2.682063176544399e-17, + "angularVelocity": 6.493640038785633e-17, + "velocityX": 3.407935544721574, + "velocityY": -1.6398664971350723, + "timestamp": 11.038327407019304 + }, + { + "x": 4.5193677097341265, + "y": 4.820146027732668, + "heading": 1.2264799155735384e-17, + "angularVelocity": -4.203334264259107e-16, + "velocityX": 3.339720500479131, + "velocityY": -1.7746703804386716, + "timestamp": 11.07295666011868 + }, + { + "x": 4.632472949186873, + "y": 4.754120458704841, + "heading": 9.89855142264587e-19, + "angularVelocity": -3.255901587362041e-16, + "velocityX": 3.2661761178668676, + "velocityY": -1.90664144093305, + "timestamp": 11.107585913218054 + }, + { + "x": 4.742850945772244, + "y": 4.683630254723833, + "heading": -1.0397818320257784e-17, + "angularVelocity": -3.288454830368281e-16, + "velocityX": 3.1874206546881325, + "velocityY": -2.035568130179495, + "timestamp": 11.14221516631743 + }, + { + "x": 4.850325734395693, + "y": 4.6087881488265605, + "heading": -1.7363492612518904e-17, + "angularVelocity": -2.0115000090418594e-16, + "velocityX": 3.1035837912827686, + "velocityY": -2.161239391519611, + "timestamp": 11.176844419416804 + }, + { + "x": 4.959315990041341, + "y": 4.536170691050833, + "heading": -2.5306246706392577e-17, + "angularVelocity": -2.29365446344471e-16, + "velocityX": 3.147346416420796, + "velocityY": -2.0969975173111073, + "timestamp": 11.21147367251618 + }, + { + "x": 5.071119485305154, + "y": 4.467964024057391, + "heading": -3.478725650331535e-17, + "angularVelocity": -2.7378614749218265e-16, + "velocityX": 3.228585235233696, + "velocityY": -1.9696257033817701, + "timestamp": 11.246102925615554 + }, + { + "x": 5.185557902331023, + "y": 4.404277136498651, + "heading": -4.439822175932155e-17, + "angularVelocity": -2.7753891278544145e-16, + "velocityX": 3.3046747123729983, + "velocityY": -1.839106589332955, + "timestamp": 11.28073217871493 + }, + { + "x": 5.302448643272519, + "y": 4.345211701858586, + "heading": -5.347328989806064e-17, + "angularVelocity": -2.62063640602752e-16, + "velocityX": 3.375491253191483, + "velocityY": -1.7056514176198097, + "timestamp": 11.315361431814305 + }, + { + "x": 5.421605180282958, + "y": 4.290861996168768, + "heading": -6.205824789945856e-17, + "angularVelocity": -2.4791057365365614e-16, + "velocityX": 3.440921369816929, + "velocityY": -1.5694738068375724, + "timestamp": 11.34999068491368 + }, + { + "x": 5.542837363816495, + "y": 4.241314761996146, + "heading": -6.772617298051042e-17, + "angularVelocity": -1.6367448251072389e-16, + "velocityX": 3.500860477286019, + "velocityY": -1.4307913032498076, + "timestamp": 11.384619938013055 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -7.009195303985871e-17, + "angularVelocity": -6.831738623783852e-17, + "velocityX": 3.555212832659327, + "velocityY": -1.2898253194640328, + "timestamp": 11.41924919111243 + }, + { + "x": 5.93427243169133, + "y": 4.124338107174554, + "heading": -5.732543809529257e-17, + "angularVelocity": 1.7374415360495528e-16, + "velocityX": 3.651674213100421, + "velocityY": -0.9841063029448727, + "timestamp": 11.492728008556748 + }, + { + "x": 6.207753086191834, + "y": 4.075010553952499, + "heading": -4.033997496139901e-17, + "angularVelocity": 2.3116135676628954e-16, + "velocityX": 3.7218978749589717, + "velocityY": -0.6713166452282138, + "timestamp": 11.566206826001066 + }, + { + "x": 6.484428721197197, + "y": 4.049020784065838, + "heading": -4.165967730952468e-17, + "angularVelocity": -1.796031011340145e-17, + "velocityX": 3.7653795288013057, + "velocityY": -0.35370424825299146, + "timestamp": 11.639685643445384 + }, + { + "x": 6.743277525423136, + "y": 4.038427876582129, + "heading": -4.683972332322173e-17, + "angularVelocity": -7.04971336473422e-17, + "velocityX": 3.5227676931803376, + "velocityY": -0.1441627376724693, + "timestamp": 11.713164460889702 + }, + { + "x": 6.9786729860111185, + "y": 4.030026934106214, + "heading": -5.042587996000917e-17, + "angularVelocity": -4.880531235316479e-17, + "velocityX": 3.2035825939409506, + "velocityY": -0.11433148719739977, + "timestamp": 11.78664327833402 + }, + { + "x": 7.190552838965192, + "y": 4.022997315975448, + "heading": -4.990000307378321e-17, + "angularVelocity": 7.15685015890218e-18, + "velocityX": 2.8835501212936054, + "velocityY": -0.09566863451624368, + "timestamp": 11.860122095778339 + }, + { + "x": 7.37890188057172, + "y": 4.017047213862616, + "heading": -4.8467877227374554e-17, + "angularVelocity": 1.949032246701928e-17, + "velocityX": 2.563310735767617, + "velocityY": -0.08097710768603945, + "timestamp": 11.933600913222657 + }, + { + "x": 7.5437137264303615, + "y": 4.012027115751869, + "heading": -4.529378389429077e-17, + "angularVelocity": 4.3197392712062346e-17, + "velocityX": 2.2429844626110818, + "velocityY": -0.06832034435708055, + "timestamp": 12.007079730666975 + }, + { + "x": 7.684984960485698, + "y": 4.007846136006195, + "heading": -4.052616415520898e-17, + "angularVelocity": 6.488427420284795e-17, + "velocityX": 1.9226116991116777, + "velocityY": -0.05690047677812232, + "timestamp": 12.080558548111293 + }, + { + "x": 7.802713484144924, + "y": 4.004443187628857, + "heading": -3.38523635240434e-17, + "angularVelocity": 9.082618451636533e-17, + "velocityX": 1.602210375098112, + "velocityY": -0.046311964395964134, + "timestamp": 12.154037365555611 + }, + { + "x": 7.896897887923302, + "y": 4.001774392077607, + "heading": -2.5504868250570688e-17, + "angularVelocity": 1.1360410474589884e-16, + "velocityX": 1.2817898688931846, + "velocityY": -0.03632061108321556, + "timestamp": 12.22751618299993 + }, + { + "x": 7.967537164414649, + "y": 3.9998067058039015, + "heading": -1.5265763382971852e-17, + "angularVelocity": 1.393477089561816e-16, + "velocityX": 0.9613556525304345, + "velocityY": -0.026778959462655655, + "timestamp": 12.300995000444248 + }, + { + "x": 8.014630559872813, + "y": 3.998514348187002, + "heading": -8.61347718056724e-18, + "angularVelocity": 9.053338681616732e-17, + "velocityX": 0.640911178161685, + "velocityY": -0.017588165703384288, + "timestamp": 12.374473817888566 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 1.5743578769011647e-28, + "angularVelocity": 1.1722394943724176e-16, + "velocityX": 0.3204587550610229, + "velocityY": -0.008678746809770337, + "timestamp": 12.447952635332884 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 7.635851844630149e-29, + "angularVelocity": -6.421919896967874e-29, + "velocityX": 1.2273507247615808e-27, + "velocityY": -1.4201614247601614e-26, + "timestamp": 12.521431452777202 } ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "CenterLanePCBAFSprint": { - "waypoints": [ + "trajectoryWaypoints": [ { + "timestamp": 0, + "isStopPoint": true, "x": 1.2899744510650635, "y": 5.590954303741455, "heading": 0, @@ -11140,6 +12897,8 @@ "controlIntervalCount": 17 }, { + "timestamp": 1.279260647937321, + "isStopPoint": false, "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, @@ -11149,6 +12908,8 @@ "controlIntervalCount": 16 }, { + "timestamp": 2.4932364716622883, + "isStopPoint": true, "x": 1.8129411935806274, "y": 5.542999267578125, "heading": 0, @@ -11158,6 +12919,8 @@ "controlIntervalCount": 10 }, { + "timestamp": 3.424226754177389, + "isStopPoint": true, "x": 2.7583051681518556, "y": 5.54433536529541, "heading": 0, @@ -11167,6 +12930,8 @@ "controlIntervalCount": 14 }, { + "timestamp": 4.507987832431977, + "isStopPoint": false, "x": 2.1849615573883057, "y": 6.611239433288574, "heading": 0.5125504196, @@ -11176,6 +12941,8 @@ "controlIntervalCount": 8 }, { + "timestamp": 5.101882241555811, + "isStopPoint": true, "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, @@ -11185,6 +12952,8 @@ "controlIntervalCount": 18 }, { + "timestamp": 6.15073602782005, + "isStopPoint": false, "x": 4.16749906539917, "y": 4.98994779586792, "heading": 0, @@ -11194,6 +12963,8 @@ "controlIntervalCount": 13 }, { + "timestamp": 6.601342830108367, + "isStopPoint": false, "x": 5.665951728820801, "y": 4.196649074554443, "heading": 0, @@ -11203,6 +12974,8 @@ "controlIntervalCount": 12 }, { + "timestamp": 7.033101650568428, + "isStopPoint": false, "x": 7.196889877319336, "y": 3.9978766441345215, "heading": 0, @@ -11212,6 +12985,8 @@ "controlIntervalCount": 10 }, { + "timestamp": 7.672047227988539, + "isStopPoint": false, "x": 8.1634765625, "y": 4.073246955871582, "heading": 0, @@ -11221,6 +12996,8 @@ "controlIntervalCount": 16 }, { + "timestamp": 8.796461336883105, + "isStopPoint": false, "x": 5.665951728820801, "y": 4.196649074554443, "heading": 0, @@ -11230,6 +13007,8 @@ "controlIntervalCount": 13 }, { + "timestamp": 9.246906615715114, + "isStopPoint": false, "x": 4.16749906539917, "y": 4.98994779586792, "heading": 0, @@ -11239,6 +13018,8 @@ "controlIntervalCount": 13 }, { + "timestamp": 10.14140053479553, + "isStopPoint": true, "x": 2.6583051681518555, "y": 5.54433536529541, "heading": 0, @@ -11248,26 +13029,183 @@ "controlIntervalCount": 13 }, { + "timestamp": 10.969068900820554, + "isStopPoint": false, "x": 4.16749906539917, "y": 4.98994779586792, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 11.41924919111243, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 + }, + { + "timestamp": 12.521431452777202, + "isStopPoint": true, + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 3 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 5 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 2 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 12 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 4 + ], + "type": "WptVelocityDirection", + "direction": 0.5125504196 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "CenterLanePCBADSprint": { + "waypoints": [ + { + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "x": 7.374384880065918, + "y": 7.465639114379883, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 7.923961639404297, + "y": 7.465639114379883, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 15 + "headingConstrained": true, + "controlIntervalCount": 20 }, { - "x": 8.038177490234375, - "y": 3.9978766441345215, + "x": 6.829305171966553, + "y": 6.746249198913574, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -11279,1875 +13217,1250 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -2.75917641303777e-30, - "velocityX": -8.847327306636262e-30, - "velocityY": 1.4420464788285476e-29, + "heading": -3.4787101362106846e-24, + "angularVelocity": -4.3859089659641935e-24, + "velocityX": -2.4789748003093715e-23, + "velocityY": -3.607487096592192e-23, "timestamp": 0 }, { - "x": 1.3060574436245773, - "y": 5.572382537817046, - "heading": -0.010694867587036427, - "angularVelocity": -0.14212340511596666, - "velocityX": 0.21372585011902087, - "velocityY": -0.24679899874441508, - "timestamp": 0.07525057240542533 - }, - { - "x": 1.3382236275279018, - "y": 5.53523931290218, - "heading": -0.032076067517629925, - "angularVelocity": -0.2841333859323569, - "velocityX": 0.42745434186951237, - "velocityY": -0.4935939186635594, - "timestamp": 0.15050114481085067 - }, - { - "x": 1.386473519841097, - "y": 5.479524946048311, - "heading": -0.0641215016247012, - "angularVelocity": -0.42584970563284796, - "velocityX": 0.6411897048771923, - "velocityY": -0.7403846253066342, - "timestamp": 0.22575171721627602 - }, - { - "x": 1.450807989654895, - "y": 5.405239856496701, - "heading": -0.10679801494850981, - "angularVelocity": -0.5671254312134459, - "velocityX": 0.8549366171751026, - "velocityY": -0.9871697606799944, - "timestamp": 0.30100228962170134 - }, - { - "x": 1.5312282961066404, - "y": 5.312384689270129, - "heading": -0.16006508743981135, - "angularVelocity": -0.7078626884752101, - "velocityX": 1.0687002620672836, - "velocityY": -1.23394632438782, - "timestamp": 0.37625286202712666 - }, - { - "x": 1.627736154340151, - "y": 5.200960485487107, - "heading": -0.22387960186124028, - "angularVelocity": -0.8480269635530943, - "velocityX": 1.2824866993939017, - "velocityY": -1.4807090527398261, - "timestamp": 0.451503434432552 - }, - { - "x": 1.7403339652666383, - "y": 5.0709689996393825, - "heading": -0.2982011188077779, - "angularVelocity": -0.9876538419890185, - "velocityX": 1.4963050422725384, - "velocityY": -1.7274484657660032, - "timestamp": 0.5267540068379774 - }, - { - "x": 1.8690262176186487, - "y": 4.922413993905078, - "heading": -0.382996017565801, - "angularVelocity": -1.1268339369356364, - "velocityX": 1.710183035621927, - "velocityY": -1.9741378834553056, - "timestamp": 0.6020045792434027 - }, - { - "x": 2.0218420905066052, - "y": 4.778614575286612, - "heading": -0.4624353832107095, - "angularVelocity": -1.055664602990153, - "velocityX": 2.030760272057439, - "velocityY": -1.9109411931313167, - "timestamp": 0.677255151648828 - }, - { - "x": 2.15857350503456, - "y": 4.653386666317764, - "heading": -0.5314189322497749, - "angularVelocity": -0.9167179309443209, - "velocityX": 1.8170149429960494, - "velocityY": -1.6641456000181232, - "timestamp": 0.7525057240542533 - }, - { - "x": 2.2792167348207513, - "y": 4.5467268950736175, - "heading": -0.589951951408673, - "angularVelocity": -0.7778415138506264, - "velocityX": 1.6032200942942694, - "velocityY": -1.4173948161921597, - "timestamp": 0.8277562964596786 - }, - { - "x": 2.38377026778444, - "y": 4.458633759569044, - "heading": -0.6380351777892487, - "angularVelocity": -0.6389748920543931, - "velocityX": 1.3894051516512624, - "velocityY": -1.1706639921478266, - "timestamp": 0.903006868865104 - }, - { - "x": 2.472233242714231, - "y": 4.389106342435906, - "heading": -0.6756676216200074, - "angularVelocity": -0.5000951172479533, - "velocityX": 1.1755787644266529, - "velocityY": -0.9239453589485842, - "timestamp": 0.9782574412705293 - }, - { - "x": 2.5446051200448103, - "y": 4.3381440386367105, - "heading": -0.7028476555095471, - "angularVelocity": -0.3611937161417901, - "velocityX": 0.9617452069540623, - "velocityY": -0.6772347660530061, - "timestamp": 1.0535080136759547 - }, - { - "x": 2.600885568086537, - "y": 4.305746455375921, - "heading": -0.7195734643408855, - "angularVelocity": -0.22226819406292112, - "velocityX": 0.7479072416796508, - "velocityY": -0.4305293929915994, - "timestamp": 1.12875858608138 - }, - { - "x": 2.641074414508583, - "y": 4.291913365901662, - "heading": -0.7258432162336076, - "angularVelocity": -0.08331832824324738, - "velocityX": 0.5340669863123017, - "velocityY": -0.18382703322097846, - "timestamp": 1.2040091584868053 + "x": 1.3060567681339126, + "y": 5.57238191847303, + "heading": -0.010695335590330916, + "angularVelocity": -0.1421295228209153, + "velocityX": 0.21371672085882062, + "velocityY": -0.24680705280850238, + "timestamp": 0.07525062617572531 + }, + { + "x": 1.3382215482089261, + "y": 5.535237409911883, + "heading": -0.032077507438502996, + "angularVelocity": -0.28414609864163015, + "velocityX": 0.4274353810678254, + "velocityY": -0.49361062424128055, + "timestamp": 0.15050125235145062 + }, + { + "x": 1.3864692379478656, + "y": 5.479521035128849, + "heading": -0.06412446300016969, + "angularVelocity": -0.4258696198332041, + "velocityX": 0.6411599768787516, + "velocityY": -0.7404107794787639, + "timestamp": 0.22575187852717593 + }, + { + "x": 1.450800607934819, + "y": 5.405233129379991, + "heading": -0.10680310960887816, + "angularVelocity": -0.5671533750303323, + "velocityX": 0.8548948129245705, + "velocityY": -0.9872064795232551, + "timestamp": 0.30100250470290124 + }, + { + "x": 1.531216769618125, + "y": 5.3123742117306145, + "heading": -0.1600730202690718, + "angularVelocity": -0.7078998988765586, + "velocityX": 1.0686444189250723, + "velocityY": -1.2339952817473157, + "timestamp": 0.37625313087862655 + }, + { + "x": 1.627719192058266, + "y": 5.200945113429446, + "heading": -0.2238912344076857, + "angularVelocity": -0.8480755228479505, + "velocityX": 1.282413547161571, + "velocityY": -1.4807730375686, + "timestamp": 0.45150375705435186 + }, + { + "x": 1.7403097840465742, + "y": 5.070947169326127, + "heading": -0.2982176307237913, + "angularVelocity": -0.9877179778217201, + "velocityX": 1.4962080411847536, + "velocityY": -1.7275330546718466, + "timestamp": 0.5267543832300772 + }, + { + "x": 1.8689915581726206, + "y": 4.922382882779654, + "heading": -0.3830195540733457, + "angularVelocity": -1.126926481003957, + "velocityX": 1.7100425692877124, + "velocityY": -1.974259804822693, + "timestamp": 0.6020050094058025 + }, + { + "x": 2.021815508579126, + "y": 4.778590260197623, + "heading": -0.46245355485112494, + "angularVelocity": -1.055592555366715, + "velocityX": 2.0308661624905433, + "velocityY": -1.9108495156737524, + "timestamp": 0.6772556355815278 + }, + { + "x": 2.1585523272850695, + "y": 4.6533669600969905, + "heading": -0.5314334765942994, + "angularVelocity": -0.9166690730531941, + "velocityX": 1.8170854603473379, + "velocityY": -1.664083164015284, + "timestamp": 0.7525062617572531 + }, + { + "x": 2.2791997774513058, + "y": 4.5467108563615435, + "heading": -0.5899636375558711, + "angularVelocity": -0.7778029756841082, + "velocityX": 1.6032750330143433, + "velocityY": -1.4173450661577678, + "timestamp": 0.8277568879329784 + }, + { + "x": 2.3837568433333827, + "y": 4.45862086228965, + "heading": -0.6380444530356357, + "angularVelocity": -0.6389423972032644, + "velocityX": 1.389451107528536, + "velocityY": -1.1706214093977847, + "timestamp": 0.9030075141087037 + }, + { + "x": 2.4722229118459578, + "y": 4.389096268153827, + "heading": -0.6756747726303448, + "angularVelocity": -0.500066531098822, + "velocityX": 1.1756190347969888, + "velocityY": -0.923907184153793, + "timestamp": 0.978258140284429 + }, + { + "x": 2.544597592286723, + "y": 4.338136593499442, + "heading": -0.702852872614281, + "angularVelocity": -0.361167758530936, + "velocityX": 0.9617817700514026, + "velocityY": -0.677199343635805, + "timestamp": 1.0535087664601543 + }, + { + "x": 2.600880652205466, + "y": 4.305741528586687, + "heading": -0.7195768736441226, + "angularVelocity": -0.22224401150879278, + "velocityX": 0.7479414162921453, + "velocityY": -0.4304956192272341, + "timestamp": 1.1287593926358797 + }, + { + "x": 2.6410719901577853, + "y": 4.291910905991578, + "heading": -0.7258448979650717, + "angularVelocity": -0.08329531114215386, + "velocityX": 0.534099714445754, + "velocityY": -0.18379411970354914, + "timestamp": 1.204010018811605 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.05565540572186624, - "velocityX": 0.32022625146750794, - "velocityY": 0.06287422939745346, - "timestamp": 1.2792597308922307 - }, - { - "x": 2.6731092407857906, - "y": 4.320288302812448, - "heading": -0.7067988166038234, - "angularVelocity": 0.1958033723255861, - "velocityX": 0.10461638788158735, - "velocityY": 0.31161864341117435, - "timestamp": 1.3551332887134768 - }, - { - "x": 2.664687841052256, - "y": 4.3628053226693035, - "heading": -0.6813131694887641, - "angularVelocity": 0.33589629702821755, - "velocityX": -0.11099255097832775, - "velocityY": 0.5603667611869495, - "timestamp": 1.431006846534723 - }, - { - "x": 2.6399075701479586, - "y": 4.42419614745244, - "heading": -0.6452023149092349, - "angularVelocity": 0.47593464200079133, - "velocityX": -0.32659956402163576, - "velocityY": 0.8091201539132412, - "timestamp": 1.5068804043559691 - }, - { - "x": 2.598768648766779, - "y": 4.504461298133711, - "heading": -0.5984696942887661, - "angularVelocity": 0.6159276296230222, - "velocityX": -0.5422036683592814, - "velocityY": 1.057880412955572, - "timestamp": 1.5827539621772153 - }, - { - "x": 2.5412713665853555, - "y": 4.603601395355974, - "heading": -0.5411165139934166, - "angularVelocity": 0.7559047175548449, - "velocityX": -0.7578039548110096, - "velocityY": 1.3066488519598491, - "timestamp": 1.6586275199984615 - }, - { - "x": 2.4674160388824338, - "y": 4.721617069365857, - "heading": -0.47313875765168273, - "angularVelocity": 0.8959347405484323, - "velocityX": -0.9734000859460189, - "velocityY": 1.5554255975016502, - "timestamp": 1.7345010778197076 - }, - { - "x": 2.377202770872684, - "y": 4.858508647531332, - "heading": -0.39452213175531803, - "angularVelocity": 1.0361531494380563, - "velocityX": -1.1889948303725923, - "velocityY": 1.8042066576932412, - "timestamp": 1.8103746356409538 - }, - { - "x": 2.270629951391599, - "y": 5.014274677090094, - "heading": -0.305234926659776, - "angularVelocity": 1.1767894858012566, - "velocityX": -1.4046108096674554, - "velocityY": 2.0529685707303664, - "timestamp": 1.8862481934622 - }, - { - "x": 2.1562097285316435, - "y": 5.146459905330176, - "heading": -0.22910409225153178, - "angularVelocity": 1.0033908596890408, - "velocityX": -1.5080381906707807, - "velocityY": 1.7421778026344412, - "timestamp": 1.9621217512834461 - }, - { - "x": 2.0581334055736438, - "y": 5.259758402247803, - "heading": -0.16374394944584372, - "angularVelocity": 0.8614350596401947, - "velocityX": -1.2926284963034982, - "velocityY": 1.4932540422920335, - "timestamp": 2.0379953091046925 - }, - { - "x": 1.9764029763301112, - "y": 5.354172653968883, - "heading": -0.10920555144834718, - "angularVelocity": 0.7188063874223503, - "velocityX": -1.0771925238352613, - "velocityY": 1.2443630486526756, - "timestamp": 2.1138688669259387 - }, - { - "x": 1.9110185860607423, - "y": 5.429703503317112, - "heading": -0.06553422750381226, - "angularVelocity": 0.5755802838199697, - "velocityX": -0.8617546368678625, - "velocityY": 0.9954831632816045, - "timestamp": 2.189742424747185 - }, - { - "x": 1.8619801423257207, - "y": 5.486351407872208, - "heading": -0.032764704050183134, - "angularVelocity": 0.43189649194112756, - "velocityX": -0.6463179682397073, - "velocityY": 0.7466093087307788, - "timestamp": 2.265615982568431 - }, - { - "x": 1.8292876278534882, - "y": 5.524116629586748, - "heading": -0.010918068889347814, - "angularVelocity": 0.28793476658359446, - "velocityX": -0.43088152724695716, - "velocityY": 0.4977389066689233, - "timestamp": 2.341489540389677 + "angularVelocity": 0.05567771435110341, + "velocityX": 0.32025823965793626, + "velocityY": 0.06290687402329305, + "timestamp": 1.2792606449873303 + }, + { + "x": 2.6731116725964643, + "y": 4.320290742325012, + "heading": -0.7067971816671581, + "angularVelocity": 0.19582509769912612, + "velocityX": 0.10464853341562398, + "velocityY": 0.31165107776032225, + "timestamp": 1.3551341341546281 + }, + { + "x": 2.664692784388126, + "y": 4.362810214221228, + "heading": -0.6813099320330065, + "angularVelocity": 0.33591772190617664, + "velocityX": -0.11095954991308038, + "velocityY": 0.5603995857164566, + "timestamp": 1.431007623321926 + }, + { + "x": 2.63991517535616, + "y": 4.424203564318272, + "heading": -0.645197478734488, + "angularVelocity": 0.47595614350741217, + "velocityX": -0.3265647764969981, + "velocityY": 0.8091541692734667, + "timestamp": 1.5068811124892239 + }, + { + "x": 2.598779165089059, + "y": 4.504471398642189, + "heading": -0.5984632230032911, + "angularVelocity": 0.6159497374392491, + "velocityX": -0.5421657909576058, + "velocityY": 1.0579167401531877, + "timestamp": 1.5827546016565217 + }, + { + "x": 2.5412851916310535, + "y": 4.603614465429005, + "heading": -0.5411083104388861, + "angularVelocity": 0.7559282325601216, + "velocityX": -0.7577610320679146, + "velocityY": 1.3066891726596395, + "timestamp": 1.6586280908238196 + }, + { + "x": 2.4674338175988146, + "y": 4.721633607594926, + "heading": -0.473128621994749, + "angularVelocity": 0.8959610160308396, + "velocityX": -0.9733488579838422, + "velocityY": 1.5554727146618088, + "timestamp": 1.7345015799911174 + }, + { + "x": 2.3772256430083227, + "y": 4.858529577863132, + "heading": -0.3945096550364948, + "angularVelocity": 1.036184941816799, + "velocityX": -1.1889287757886868, + "velocityY": 1.8042661774306572, + "timestamp": 1.8103750691584153 + }, + { + "x": 2.2706605414206735, + "y": 5.014302199754286, + "heading": -0.3052190611328218, + "angularVelocity": 1.1768352145607928, + "velocityX": -1.4045103600373166, + "velocityY": 2.053057314231072, + "timestamp": 1.8862485583257131 + }, + { + "x": 2.1562301513945004, + "y": 5.146478418893045, + "heading": -0.22909327294912277, + "angularVelocity": 1.0033252591803818, + "velocityX": -1.508173556825084, + "velocityY": 1.7420606405396215, + "timestamp": 1.962122047493011 + }, + { + "x": 2.058146995665746, + "y": 5.259770783530285, + "heading": -0.16373665420264177, + "angularVelocity": 0.8613893925765344, + "velocityX": -1.2927197207510102, + "velocityY": 1.4931745709945456, + "timestamp": 2.037995536660309 + }, + { + "x": 1.9764115735788519, + "y": 5.354180517364029, + "heading": -0.1092008927472175, + "angularVelocity": 0.718772288633981, + "velocityX": -1.0772593034000484, + "velocityY": 1.2443046295864166, + "timestamp": 2.113869025827607 + }, + { + "x": 1.9110235321882938, + "y": 5.429708042242192, + "heading": -0.06553152876247478, + "angularVelocity": 0.5755549726789759, + "velocityX": -0.8618035378125303, + "velocityY": 0.9954402480638209, + "timestamp": 2.189742514994905 + }, + { + "x": 1.8619825299142096, + "y": 5.48635360523882, + "heading": -0.03276339486876071, + "angularVelocity": 0.43187856856644224, + "velocityX": -0.6463522741909358, + "velocityY": 0.7465791229361657, + "timestamp": 2.2656160041622027 + }, + { + "x": 1.8292883999681575, + "y": 5.524117342026143, + "heading": -0.010917644112626662, + "angularVelocity": 0.28792337080959957, + "velocityX": -0.430903208813331, + "velocityY": 0.4977197859459911, + "timestamp": 2.3414894933295005 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 2.394848489426687e-27, - "angularVelocity": 0.14389820647675833, - "velocityX": -0.21544309693644778, - "velocityY": 0.24886981095702526, - "timestamp": 2.4173630982109233 + "heading": 1.2049864598182203e-23, + "angularVelocity": 0.14389273819414988, + "velocityX": -0.21545346822636996, + "velocityY": 0.24886064631017277, + "timestamp": 2.4173629824967984 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 2.3925172511788446e-27, - "angularVelocity": -3.168744293663733e-29, - "velocityX": 8.48223659243716e-32, - "velocityY": -3.031681211954363e-28, - "timestamp": 2.4932366560321695 - }, - { - "x": 1.8507557561824508, - "y": 5.5430527114919315, - "heading": -1.9710234547097696e-21, - "angularVelocity": -2.1171281249858964e-20, - "velocityX": 0.40617567078743816, - "velocityY": 0.0005740544395108319, - "timestamp": 2.586335690074543 - }, - { - "x": 1.9263848800936068, - "y": 5.5431595993177165, - "heading": -2.0389636321585498e-20, - "angularVelocity": -1.9783892557349584e-19, - "velocityX": 0.8123513276919052, - "velocityY": 0.001148108859400643, - "timestamp": 2.6794347241169163 - }, - { - "x": 2.039828562987611, - "y": 5.543319931052194, - "heading": -2.352458064017618e-20, - "angularVelocity": -3.3673220682050347e-20, - "velocityX": 1.2185269596070256, - "velocityY": 0.001722163243972619, - "timestamp": 2.7725337581592897 - }, - { - "x": 2.1910867994360017, - "y": 5.54353370668769, - "heading": -3.364930009404432e-20, - "angularVelocity": -1.087521428982912e-19, - "velocityX": 1.6247025332136769, - "velocityY": 0.0022962175461363225, - "timestamp": 2.865632792201663 - }, - { - "x": 2.3801595622964817, - "y": 5.543800926185845, - "heading": -1.7618072508363382e-20, - "angularVelocity": 1.721954234066794e-19, - "velocityX": 2.030877815278129, - "velocityY": 0.0028702714362588678, - "timestamp": 2.9587318262440365 - }, - { - "x": 2.5314177987448723, - "y": 5.544014701821341, - "heading": -7.829457975228232e-21, - "angularVelocity": 1.0514195591633778e-19, - "velocityX": 1.6247025332136766, - "velocityY": 0.0022962175461363225, - "timestamp": 3.05183086028641 - }, - { - "x": 2.6448614816388765, - "y": 5.544175033555819, - "heading": 1.3151205701559364e-20, - "angularVelocity": 2.253585538517852e-19, - "velocityX": 1.2185269596070254, - "velocityY": 0.001722163243972619, - "timestamp": 3.1449298943287833 - }, - { - "x": 2.720490605550032, - "y": 5.544281921381604, - "heading": 9.829941746315052e-21, - "angularVelocity": -3.567452648039799e-20, - "velocityX": 0.8123513276919051, - "velocityY": 0.0011481088594006432, - "timestamp": 3.2380289283711567 + "heading": 5.735401641845927e-24, + "angularVelocity": -6.2110589016170234e-24, + "velocityX": -6.266702545665033e-23, + "velocityY": -1.0096243291514256e-22, + "timestamp": 2.4932364716640962 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -1.6124701211579614e-34, - "angularVelocity": -1.0558586184512984e-19, - "velocityX": 0.40617567078743816, - "velocityY": 0.000574054439510832, - "timestamp": 3.33112796241353 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "angularVelocity": 1.724633516740329e-33, - "velocityX": 6.381177903729197e-38, - "velocityY": -6.411016173939856e-36, - "timestamp": 3.4242269964559036 - }, - { - "x": 2.7396220511907092, - "y": 5.562601673573336, - "heading": 0.003707886080781361, - "angularVelocity": 0.04789837119714626, - "velocityX": -0.24134799501070836, - "velocityY": 0.2359636718162648, - "timestamp": 3.5016385239903154 - }, - { - "x": 2.702380969511178, - "y": 5.599259386918254, - "heading": 0.011292704027819503, - "angularVelocity": 0.09798047123752293, - "velocityX": -0.4810792767650384, - "velocityY": 0.473543340539592, - "timestamp": 3.579050051524727 - }, - { - "x": 2.6467699433072402, - "y": 5.6544931206396525, - "heading": 0.023007945503191744, - "angularVelocity": 0.1513371696504047, - "velocityX": -0.7183817187849313, - "velocityY": 0.7135078647924299, - "timestamp": 3.656461579059139 - }, - { - "x": 2.573102880230356, - "y": 5.72860253774289, - "heading": 0.03927230490424404, - "angularVelocity": 0.21010255086133403, - "velocityX": -0.9516291103303324, - "velocityY": 0.9573434275701875, - "timestamp": 3.733873106593551 - }, - { - "x": 2.4820081686264115, - "y": 5.82215722498829, - "heading": 0.060903903359814444, - "angularVelocity": 0.2794363984867048, - "velocityX": -1.1767589983733415, - "velocityY": 1.208536896572829, - "timestamp": 3.8112846341279627 - }, - { - "x": 2.3753503305689, - "y": 5.936638406081269, - "heading": 0.09019255741494606, - "angularVelocity": 0.378350033748035, - "velocityX": -1.3778030411568696, - "velocityY": 1.4788647729769806, - "timestamp": 3.8886961616623745 - }, - { - "x": 2.2779168004384966, - "y": 6.0748767248289015, - "heading": 0.1437819345039841, - "angularVelocity": 0.6922661106928292, - "velocityX": -1.25864368310122, - "velocityY": 1.7857588288279342, - "timestamp": 3.9661076891967864 - }, - { - "x": 2.2032159820535764, - "y": 6.200217981312059, - "heading": 0.20003111930602646, - "angularVelocity": 0.7266254341388357, - "velocityX": -0.9649831331866353, - "velocityY": 1.6191549304777408, - "timestamp": 4.043519216731198 - }, - { - "x": 2.149623841782867, - "y": 6.310133396102646, - "heading": 0.25610236287269844, - "angularVelocity": 0.7243267941166335, - "velocityX": -0.6923018053982481, - "velocityY": 1.4198843284900597, - "timestamp": 4.12093074426561 - }, - { - "x": 2.1165539042639407, - "y": 6.4038530923195935, - "heading": 0.31105587872076873, - "angularVelocity": 0.7098880179523891, - "velocityX": -0.42719655033581183, - "velocityY": 1.2106684779639114, - "timestamp": 4.198342271800021 - }, - { - "x": 2.1037067095365876, - "y": 6.481006876335726, - "heading": 0.3644203290348082, - "angularVelocity": 0.6893605127520235, - "velocityX": -0.1659597108666065, - "velocityY": 0.9966704762651164, - "timestamp": 4.275753799334432 - }, - { - "x": 2.1109008821512623, - "y": 6.541377447734871, - "heading": 0.41591028587378526, - "angularVelocity": 0.6651458571992168, - "velocityX": 0.09293412549541542, - "velocityY": 0.7798653937207066, - "timestamp": 4.353165326868844 - }, - { - "x": 2.1380148682221702, - "y": 6.5848219723004595, - "heading": 0.465333297367931, - "angularVelocity": 0.6384451136450678, - "velocityX": 0.3502577320781457, - "velocityY": 0.5612151826648336, - "timestamp": 4.430576854403255 + "x": 1.8552093934543321, + "y": 5.543066072465799, + "heading": -4.219694925044242e-18, + "angularVelocity": -4.2870553829964666e-17, + "velocityX": 0.42942883486562483, + "velocityY": 0.0006787122508870844, + "timestamp": 2.591665353103628 }, { - "x": 2.1849615573883057, - "y": 6.611239433288574, - "heading": 0.5125504196, - "angularVelocity": 0.6099494963599527, - "velocityX": 0.6064560493947926, - "velocityY": 0.3412600400679556, - "timestamp": 4.5079883819376665 - }, - { - "x": 2.250707976446258, - "y": 6.648678224064634, - "heading": 0.55208552781375, - "angularVelocity": 0.5325541984016284, - "velocityX": 0.8856313560565378, - "velocityY": 0.5043159387113171, - "timestamp": 4.582225160715128 - }, - { - "x": 2.3371670619589073, - "y": 6.698226995892447, - "heading": 0.585356010779416, - "angularVelocity": 0.44816711492022837, - "velocityX": 1.1646395080237388, - "velocityY": 0.6674423734944787, - "timestamp": 4.656461939492589 - }, - { - "x": 2.4409073566894506, - "y": 6.758690881227722, - "heading": 0.5617681472290975, - "angularVelocity": -0.31773824159353026, - "velocityX": 1.3974245170513677, - "velocityY": 0.8144734501011609, - "timestamp": 4.73069871827005 - }, - { - "x": 2.523886913207481, - "y": 6.807069780587539, - "heading": 0.5422957792802172, - "angularVelocity": -0.262300820018773, - "velocityX": 1.1177688186980366, - "velocityY": 0.6516837092951012, - "timestamp": 4.804935497047511 - }, - { - "x": 2.5861173110961473, - "y": 6.843356427328165, - "heading": 0.5274956073850987, - "angularVelocity": -0.1993644139582724, - "velocityX": 0.8382691021011796, - "velocityY": 0.4887960838037209, - "timestamp": 4.879172275824972 - }, - { - "x": 2.627602512984248, - "y": 6.8675485030782895, - "heading": 0.5175502299027057, - "angularVelocity": -0.1339683327613954, - "velocityX": 0.558822763747067, - "velocityY": 0.3258772288954634, - "timestamp": 4.953409054602433 + "x": 1.939745792565551, + "y": 5.543199682240142, + "heading": -1.2239290131185836e-17, + "angularVelocity": -8.147603720153091e-17, + "velocityX": 0.858857663267793, + "velocityY": 0.0013574244915586756, + "timestamp": 2.6900942345431598 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.06734950498988629, - "velocityX": 0.27940333769169035, - "velocityY": 0.16294305705300582, - "timestamp": 5.027645833379895 + "x": 2.066550389641902, + "y": 5.543400096899142, + "heading": -5.060219765528842e-18, + "angularVelocity": 7.293662450422726e-17, + "velocityX": 1.288286478743048, + "velocityY": 0.002036136711799281, + "timestamp": 2.7885231159826915 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -1.618919283202532e-33, - "velocityX": 2.4430876435253525e-35, - "velocityY": 8.017390657472539e-35, - "timestamp": 5.101882612157356 - }, - { - "x": 2.6570338601157935, - "y": 6.867732812321398, - "heading": 0.5065324038818937, - "angularVelocity": -0.10327871439636148, - "velocityX": 0.1491229424075764, - "velocityY": -0.20442985508511316, - "timestamp": 5.160152273561258 - }, - { - "x": 2.674422013328184, - "y": 6.84391114835803, - "heading": 0.49465795152131636, - "angularVelocity": -0.2037844750507175, - "velocityX": 0.2984083448136593, - "velocityY": -0.40881761433700353, - "timestamp": 5.218421934965161 - }, - { - "x": 2.7005198630420675, - "y": 6.808182791545169, - "heading": 0.47711568998903314, - "angularVelocity": -0.30105308851354295, - "velocityX": 0.4478805794491053, - "velocityY": -0.6131553874185988, - "timestamp": 5.276691596369063 - }, - { - "x": 2.7353400941220496, - "y": 6.760551298473928, - "heading": 0.4541277672109497, - "angularVelocity": -0.39450929049922157, - "velocityX": 0.5975705065217742, - "velocityY": -0.8174321237441035, - "timestamp": 5.3349612577729655 - }, - { - "x": 2.7788976924779565, - "y": 6.701021131245012, - "heading": 0.42595856862842546, - "angularVelocity": -0.4834282181127908, - "velocityX": 0.7475176156247499, - "velocityY": -1.021632283329671, - "timestamp": 5.393230919176868 - }, - { - "x": 2.831210629197317, - "y": 6.629598041190472, - "heading": 0.39292676107093794, - "angularVelocity": -0.5668783164625577, - "velocityX": 0.8977731371519051, - "velocityY": -1.2257337409164428, - "timestamp": 5.45150058058077 - }, - { - "x": 2.8923008190981245, - "y": 6.546289653631685, - "heading": 0.3554225990889349, - "angularVelocity": -0.6436310264794379, - "velocityX": 1.0484047517859123, - "velocityY": -1.4297043358691544, - "timestamp": 5.509770241984673 - }, - { - "x": 2.962195513842206, - "y": 6.451106403793388, - "heading": 0.31393409063739947, - "angularVelocity": -0.7120087443781998, - "velocityX": 1.1995040482490311, - "velocityY": -1.6334958457802513, - "timestamp": 5.568039903388575 - }, - { - "x": 3.0409294178085977, - "y": 6.344063134427028, - "heading": 0.26908904007938106, - "angularVelocity": -0.7696123416123889, - "velocityX": 1.351198927013472, - "velocityY": -1.8370326304863622, - "timestamp": 5.626309564792478 - }, - { - "x": 3.128548070700225, - "y": 6.22518206352159, - "heading": 0.22172760975049532, - "angularVelocity": -0.8127974178637293, - "velocityX": 1.5036753394582005, - "velocityY": -2.0401881191895193, - "timestamp": 5.68457922619638 - }, - { - "x": 3.2251135709999605, - "y": 6.094498947078054, - "heading": 0.17303894760412505, - "angularVelocity": -0.8355748252745036, - "velocityX": 1.6572174605646188, - "velocityY": -2.2427299780874135, - "timestamp": 5.742848887600283 - }, - { - "x": 3.3307147862388833, - "y": 5.952077971116604, - "heading": 0.12484956119030122, - "angularVelocity": -0.8270064601850674, - "velocityX": 1.8122846897450882, - "velocityY": -2.444170302865565, - "timestamp": 5.801118549004185 - }, - { - "x": 3.4454853336507822, - "y": 5.798056568785059, - "heading": 0.08033941158282583, - "angularVelocity": -0.7638649090295626, - "velocityX": 1.9696450030206056, - "velocityY": -2.643252056399113, - "timestamp": 5.859388210408087 - }, - { - "x": 3.5696119500976344, - "y": 5.632835617985738, - "heading": 0.04630921837828673, - "angularVelocity": -0.5840122009402995, - "velocityX": 2.1302100176360224, - "velocityY": -2.8354541079975273, - "timestamp": 5.91765787181199 - }, - { - "x": 3.7020235307305986, - "y": 5.458025004439994, - "heading": 0.04313298905641362, - "angularVelocity": -0.05450914327194612, - "velocityX": 2.272393170695454, - "velocityY": -3.00002796196164, - "timestamp": 5.975927533215892 - }, - { - "x": 3.84656727081631, - "y": 5.291677538591243, - "heading": 0.04313296518805293, - "angularVelocity": -4.0961900430587924e-7, - "velocityX": 2.480600309032019, - "velocityY": -2.8547868966613104, - "timestamp": 6.034197194619795 - }, - { - "x": 4.001959806149131, - "y": 5.135416436419877, - "heading": 0.04313294238925072, - "angularVelocity": -3.912636809189277e-7, - "velocityX": 2.666782877897643, - "velocityY": -2.6816888652951767, - "timestamp": 6.092466856023697 + "x": 2.2356231808662415, + "y": 5.543667316436768, + "heading": 1.924608375875081e-20, + "angularVelocity": 5.160544115984206e-17, + "velocityX": 1.7177152554375656, + "velocityY": 0.0027148488707469335, + "timestamp": 2.8869519974222233 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.04313291889737663, - "angularVelocity": -4.031578960087653e-7, - "velocityX": 2.8409167869122447, - "velocityY": -2.496473071014189, - "timestamp": 6.1507365174276 - }, - { - "x": 4.2693517733895705, - "y": 4.907420739393082, - "heading": 0.043132896624531875, - "angularVelocity": -6.4257109962904e-7, - "velocityX": 2.9384484685182977, - "velocityY": -2.3809038315668807, - "timestamp": 6.185398587932365 - }, - { - "x": 4.374422288805421, - "y": 4.829031478695207, - "heading": 0.0431328753158102, - "angularVelocity": -6.147561689330783e-7, - "velocityX": 3.0312821445967915, - "velocityY": -2.261528511030489, - "timestamp": 6.220060658437131 - }, - { - "x": 4.482542607371009, - "y": 4.7549053274480135, - "heading": 0.043132854776618594, - "angularVelocity": -5.925552428867745e-7, - "velocityX": 3.1192689008789145, - "velocityY": -2.138537893661084, - "timestamp": 6.254722728941896 - }, - { - "x": 4.593539840912748, - "y": 4.685160765838758, - "heading": 0.04313283483631883, - "angularVelocity": -5.752772258439431e-7, - "velocityX": 3.2022678370145012, - "velocityY": -2.012129125398518, - "timestamp": 6.289384799446662 - }, - { - "x": 4.707236482283354, - "y": 4.619909225059653, - "heading": 0.04313281534202855, - "angularVelocity": -5.624098616258592e-7, - "velocityX": 3.280145695710103, - "velocityY": -1.882505569600442, - "timestamp": 6.324046869951427 - }, - { - "x": 4.823450643664921, - "y": 4.559254805625301, - "heading": 0.043132796153422026, - "angularVelocity": -5.53590891933908e-7, - "velocityX": 3.352776094711051, - "velocityY": -1.7498787161607805, - "timestamp": 6.358708940456193 - }, - { - "x": 4.9419959983331205, - "y": 4.5032933115020395, - "heading": 0.04313277713835443, - "angularVelocity": -5.485842974798401e-7, - "velocityX": 3.4200309716611494, - "velocityY": -1.6144879203210547, - "timestamp": 6.3933710109609585 - }, - { - "x": 5.061736542982073, - "y": 4.449937038090927, - "heading": 0.04313275817159016, - "angularVelocity": -5.471907475389428e-7, - "velocityX": 3.454512177294491, - "velocityY": -1.5393273579481606, - "timestamp": 6.428033081465724 - }, - { - "x": 5.1814776709209625, - "y": 4.396582073700789, - "heading": 0.04313273920484137, - "angularVelocity": -5.471903010583792e-7, - "velocityX": 3.45452900519684, - "velocityY": -1.5392895927207566, - "timestamp": 6.46269515197049 - }, - { - "x": 5.3012188001688525, - "y": 4.3432271122483534, - "heading": 0.043132720238092614, - "angularVelocity": -5.471902999394187e-7, - "velocityX": 3.454529042961442, - "velocityY": -1.5392895079680768, - "timestamp": 6.497357222475255 - }, - { - "x": 5.420963133587213, - "y": 4.289879342291843, - "heading": 0.04313270127136276, - "angularVelocity": -5.471897545932471e-7, - "velocityX": 3.4546214832116693, - "velocityY": -1.5390820334629507, - "timestamp": 6.532019292980021 - }, - { - "x": 5.5425254594597755, - "y": 4.240815191837089, - "heading": 0.04313268228677855, - "angularVelocity": -5.47704852461645e-7, - "velocityX": 3.5070705270145455, - "velocityY": -1.415499701554448, - "timestamp": 6.566681363484786 + "x": 2.4046959720905807, + "y": 5.543934535974393, + "heading": 5.109055925882335e-18, + "angularVelocity": 5.171053219292816e-17, + "velocityX": 1.7177152554375659, + "velocityY": 0.0027148488707469335, + "timestamp": 2.985380878861755 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.043132663161499156, - "angularVelocity": -5.517639056314665e-7, - "velocityX": 3.560845257182681, - "velocityY": -1.274191548268104, - "timestamp": 6.601343433989552 - }, - { - "x": 5.795862627864808, - "y": 4.156159506730396, - "heading": 0.04313264445875193, - "angularVelocity": -5.198107758201059e-7, - "velocityX": 3.6106505851434836, - "velocityY": -1.1253380804221886, - "timestamp": 6.637323348406788 - }, - { - "x": 5.927341530201614, - "y": 4.121094637119033, - "heading": 0.043132625947930554, - "angularVelocity": -5.1447652596799e-7, - "velocityX": 3.654230546858154, - "velocityY": -0.9745678993212189, - "timestamp": 6.6733032628240245 - }, - { - "x": 6.059882410873037, - "y": 4.090286167435021, - "heading": 0.04313260749907939, - "angularVelocity": -5.127541702089576e-7, - "velocityX": 3.683746412913316, - "velocityY": -0.8562685649205634, - "timestamp": 6.709283177241261 - }, - { - "x": 6.193269302687495, - "y": 4.06337550162166, - "heading": 0.043132589025398085, - "angularVelocity": -5.134442814470016e-7, - "velocityX": 3.7072598413563593, - "velocityY": -0.7479357927674594, - "timestamp": 6.745263091658497 - }, - { - "x": 6.327657815035056, - "y": 4.042021941101089, - "heading": 0.043132570411953335, - "angularVelocity": -5.173287667087509e-7, - "velocityX": 3.7350981658583504, - "velocityY": -0.5934855840107985, - "timestamp": 6.781243006075734 - }, - { - "x": 6.462816636912207, - "y": 4.026263156605077, - "heading": 0.043132547717955015, - "angularVelocity": -6.30740753253067e-7, - "velocityX": 3.7565075978169236, - "velocityY": -0.4379883818862704, - "timestamp": 6.81722292049297 - }, - { - "x": 6.597942834210822, - "y": 4.016131027814117, - "heading": 0.04107885634805962, - "angularVelocity": -0.0570788286508975, - "velocityX": 3.755600853621886, - "velocityY": -0.2816051387300244, - "timestamp": 6.853202834910206 - }, - { - "x": 6.728400824069692, - "y": 4.008818936530008, - "heading": 0.03307687475358465, - "angularVelocity": -0.2224013515341097, - "velocityX": 3.625856035843536, - "velocityY": -0.20322703381989088, - "timestamp": 6.889182749327443 - }, - { - "x": 6.853599659736003, - "y": 4.0035443033890195, - "heading": 0.023873611715651655, - "angularVelocity": -0.25578890853403846, - "velocityX": 3.47968686680181, - "velocityY": -0.14659937986014848, - "timestamp": 6.925162663744679 - }, - { - "x": 6.973435503233387, - "y": 4.000040041798795, - "heading": 0.014893418694113961, - "angularVelocity": -0.24958906009058449, - "velocityX": 3.3306316993343295, - "velocityY": -0.09739493956513012, - "timestamp": 6.961142578161915 - }, - { - "x": 7.08787150697828, - "y": 3.998176312998215, - "heading": 0.006805585628585594, - "angularVelocity": -0.22478744589936686, - "velocityX": 3.1805524164913392, - "velocityY": -0.05179914490532842, - "timestamp": 6.997122492579152 + "x": 2.531500569166932, + "y": 5.544134950633393, + "heading": 1.2272248404707649e-17, + "angularVelocity": 7.277531121284907e-17, + "velocityX": 1.288286478743048, + "velocityY": 0.002036136711799281, + "timestamp": 3.083809760301287 }, { - "x": 7.196889877319336, - "y": 3.9978766441345215, - "heading": 3.395025904591232e-32, - "angularVelocity": -0.18914957800247972, - "velocityX": 3.0299785896330556, - "velocityY": -0.008328782003720711, - "timestamp": 7.033102406996388 - }, - { - "x": 7.373371840772393, - "y": 4.002118083585038, - "heading": -0.0072355781339062996, - "angularVelocity": -0.11324248534202788, - "velocityX": 2.7620814521804613, - "velocityY": 0.06638186139590158, - "timestamp": 7.096996961341894 - }, - { - "x": 7.532286444106961, - "y": 4.009181217967762, - "heading": -0.011195509678912387, - "angularVelocity": -0.06197604139459259, - "velocityX": 2.4871384574536473, - "velocityY": 0.11054360508613674, - "timestamp": 7.1608915156874 - }, - { - "x": 7.6734693827744955, - "y": 4.017827175121804, - "heading": -0.012870706885804134, - "angularVelocity": -0.02621815308129755, - "velocityX": 2.209623967390062, - "velocityY": 0.13531602563950743, - "timestamp": 7.224786070032906 - }, - { - "x": 7.7968608797168075, - "y": 4.027211909710403, - "heading": -0.012933711131137412, - "angularVelocity": -0.0009860659641287687, - "velocityX": 1.9311739193778674, - "velocityY": 0.14687847320841513, - "timestamp": 7.2886806243784115 - }, - { - "x": 7.9024437482359025, - "y": 4.036727334915007, - "heading": -0.011868630755415614, - "angularVelocity": 0.01666934508944925, - "velocityX": 1.6524548860324264, - "velocityY": 0.14892388407859264, - "timestamp": 7.352575178723917 - }, - { - "x": 7.99021929218393, - "y": 4.0459160326905685, - "heading": -0.010039813807728299, - "angularVelocity": 0.028622422777974377, - "velocityX": 1.3737562589980292, - "velocityY": 0.143810342989083, - "timestamp": 7.416469733069423 - }, - { - "x": 8.060196978947376, - "y": 4.0544220986291215, - "heading": -0.00773105502412215, - "angularVelocity": 0.03613388976972427, - "velocityX": 1.0952058040039734, - "velocityY": 0.13312661815523763, - "timestamp": 7.480364287414929 - }, - { - "x": 8.112389704520517, - "y": 4.061961050008052, - "heading": -0.005169479155646227, - "angularVelocity": 0.04009067587551167, - "velocityX": 0.816857181457298, - "velocityY": 0.11799051509403427, - "timestamp": 7.544258841760435 - }, - { - "x": 8.146811527228987, - "y": 4.068300458431988, - "heading": -0.0025408789062790695, - "angularVelocity": 0.04113966012116123, - "velocityX": 0.5387285827574291, - "velocityY": 0.09921672494429698, - "timestamp": 7.608153396105941 + "x": 2.616036968278151, + "y": 5.544268560407736, + "heading": 4.225188349913728e-18, + "angularVelocity": -8.175506961814134e-17, + "velocityX": 0.858857663267793, + "velocityY": 0.0013574244915586754, + "timestamp": 3.1822386417408186 }, { - "x": 8.1634765625, - "y": 4.073246955871582, - "heading": 2.9433151473989685e-31, - "angularVelocity": 0.03976675214822599, - "velocityX": 0.2608209015888442, - "velocityY": 0.07741657313776515, - "timestamp": 7.6720479504514465 - }, - { - "x": 8.160341731543628, - "y": 4.076804852758406, - "heading": 0.0025302388406713986, - "angularVelocity": 0.03600436134361471, - "velocityX": -0.0446074831711938, - "velocityY": 0.05062755463137832, - "timestamp": 7.7423238488772945 - }, - { - "x": 8.135735418270036, - "y": 4.078564432167465, - "heading": 0.004797198596772283, - "angularVelocity": 0.032257997505259615, - "velocityX": -0.3501387221617275, - "velocityY": 0.025038163132351486, - "timestamp": 7.8125997473031425 - }, - { - "x": 8.089649517071235, - "y": 4.078625238037987, - "heading": 0.006802207301300417, - "angularVelocity": 0.028530531084475955, - "velocityX": -0.6557853009510525, - "velocityY": 0.0008652450112215468, - "timestamp": 7.882875645728991 - }, - { - "x": 8.02207492391753, - "y": 4.077106587290107, - "heading": 0.008546856784408466, - "angularVelocity": 0.024825715817051033, - "velocityX": -0.9615614267102518, - "velocityY": -0.02160983753886332, - "timestamp": 7.953151544154839 - }, - { - "x": 7.933001430575272, - "y": 4.074154095631599, - "heading": 0.010033092526327095, - "angularVelocity": 0.021148584012580587, - "velocityX": -1.2674828118525576, - "velocityY": -0.042012862512522016, - "timestamp": 8.023427442580687 - }, - { - "x": 7.822417690835061, - "y": 4.069949420479082, - "heading": 0.011263348791088614, - "angularVelocity": 0.017506090883485915, - "velocityX": -1.5735656493512467, - "velocityY": -0.059830969744960986, - "timestamp": 8.093703341006535 - }, - { - "x": 7.690311409008121, - "y": 4.0647254734139295, - "heading": 0.012240761619989172, - "angularVelocity": 0.013908222460249003, - "velocityX": -1.879823449946105, - "velocityY": -0.07433483145952353, - "timestamp": 8.163979239432383 - }, - { - "x": 7.536670161450006, - "y": 4.058791543485583, - "heading": 0.012969524954057786, - "angularVelocity": 0.01037003226415625, - "velocityX": -2.186258034398947, - "velocityY": -0.08443762458060709, - "timestamp": 8.23425513785823 - }, - { - "x": 7.361484060419744, - "y": 4.0525779000493936, - "heading": 0.01345553350071711, - "angularVelocity": 0.006915721570918652, - "velocityX": -2.492833317742797, - "velocityY": -0.08841784417378167, - "timestamp": 8.304531036284079 - }, - { - "x": 7.164754272980972, - "y": 4.046723019235429, - "heading": 0.013707670109403618, - "angularVelocity": 0.0035878105343975153, - "velocityX": -2.799391994203425, - "velocityY": -0.08331278496768466, - "timestamp": 8.374806934709927 - }, - { - "x": 6.9465233889455575, - "y": 4.0422689491096495, - "heading": 0.013740797003253829, - "angularVelocity": 0.0004713834272096328, - "velocityX": -3.1053446334191217, - "velocityY": -0.06337976782296549, - "timestamp": 8.445082833135775 - }, - { - "x": 6.707012866607414, - "y": 4.041196732301159, - "heading": 0.013584512522768889, - "angularVelocity": -0.002223870259728453, - "velocityX": -3.408146003154458, - "velocityY": -0.015257247968487717, - "timestamp": 8.515358731561623 - }, - { - "x": 6.4476145619873, - "y": 4.048415292748879, - "heading": 0.013321412464940579, - "angularVelocity": -0.0037438163541362824, - "velocityX": -3.691141777345175, - "velocityY": 0.10271744096358496, - "timestamp": 8.58563462998747 - }, - { - "x": 6.1833350728544865, - "y": 4.076620284873478, - "heading": 0.013321410992019302, - "angularVelocity": -2.0959124115556836e-8, - "velocityX": -3.76059922466405, - "velocityY": 0.4013465890352113, - "timestamp": 8.655910528413319 - }, - { - "x": 5.922208724930001, - "y": 4.126140027657945, - "heading": 0.013321410307177713, - "angularVelocity": -9.745042100450887e-9, - "velocityX": -3.7157311933907815, - "velocityY": 0.7046475946048248, - "timestamp": 8.726186426839167 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -1.1775900898457124e-26, + "angularVelocity": -4.2926306787465456e-17, + "velocityX": 0.42942883486562483, + "velocityY": 0.0006787122508870843, + "timestamp": 3.2806675231803504 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.013321409576784942, - "angularVelocity": -1.0393218545701755e-8, - "velocityX": -3.646442120972535, - "velocityY": 1.003317616364548, - "timestamp": 8.796462325265015 - }, - { - "x": 5.541094088534591, - "y": 4.236435199254689, - "heading": 0.013321408817775413, - "angularVelocity": -2.1905263169095435e-8, - "velocityX": -3.6034323243164974, - "velocityY": 1.148240568022246, - "timestamp": 8.831111967342002 - }, - { - "x": 5.41792618310833, - "y": 4.281179290629024, - "heading": 0.01332140812506875, - "angularVelocity": -1.9991740821363056e-8, - "velocityX": -3.5546660237528647, - "velocityY": 1.2913291073806714, - "timestamp": 8.865761609418989 - }, - { - "x": 5.2966447994258585, - "y": 4.330809856867523, - "heading": 0.01332140748575771, - "angularVelocity": -1.8450725633992864e-8, - "velocityX": -3.500220389376678, - "velocityY": 1.432354369728484, - "timestamp": 8.900411251495976 - }, - { - "x": 5.177443709175996, - "y": 4.385247597760623, - "heading": 0.013321406889674104, - "angularVelocity": -1.7203167808757175e-8, - "velocityX": -3.4401824407021615, - "velocityY": 1.5710910020988935, - "timestamp": 8.935060893572963 - }, - { - "x": 5.0605133585968645, - "y": 4.444405530588187, - "heading": 0.01332140632867156, - "angularVelocity": -1.6190716838073352e-8, - "velocityX": -3.3746481513237594, - "velocityY": 1.7073172847247369, - "timestamp": 8.96971053564995 - }, - { - "x": 4.946040562745207, - "y": 4.508189127302759, - "heading": 0.013321405796093607, - "angularVelocity": -1.5370373829355437e-8, - "velocityX": -3.3037223183234317, - "velocityY": 1.840815456992452, - "timestamp": 9.004360177726937 - }, - { - "x": 4.834208203167093, - "y": 4.576496460724576, - "heading": 0.013321405286393582, - "angularVelocity": -1.4710109443101504e-8, - "velocityX": -3.2275184641053225, - "velocityY": 1.9713719775242866, - "timestamp": 9.039009819803924 - }, - { - "x": 4.725194921038476, - "y": 4.649218348571895, - "heading": 0.013321404794856252, - "angularVelocity": -1.4185928002634242e-8, - "velocityX": -3.146158967137548, - "velocityY": 2.098777461704854, - "timestamp": 9.073659461880911 - }, - { - "x": 4.619174674827694, - "y": 4.72623832110653, - "heading": 0.013321404313366907, - "angularVelocity": -1.3895939972772152e-8, - "velocityX": -3.05977897189302, - "velocityY": 2.2228215911583726, - "timestamp": 9.108309103957899 - }, - { - "x": 4.510161328966249, - "y": 4.798960113344169, - "heading": 0.013321403820821752, - "angularVelocity": -1.421501423878401e-8, - "velocityX": -3.146160806487812, - "velocityY": 2.0987747023782037, - "timestamp": 9.142958746034886 - }, - { - "x": 4.398328904529435, - "y": 4.867267340500532, - "heading": 0.013321403310039242, - "angularVelocity": -1.4741350266093223e-8, - "velocityX": -3.2275203359486127, - "velocityY": 1.9713689106685317, - "timestamp": 9.177608388111873 - }, - { - "x": 4.283856051297854, - "y": 4.931050823285602, - "heading": 0.013321399241018467, - "angularVelocity": -1.1743327000811157e-7, - "velocityX": -3.3037239743267564, - "velocityY": 1.8408121689497896, - "timestamp": 9.21225803018886 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -1.2588122005060392e-26, + "angularVelocity": -5.516653068340682e-27, + "velocityX": 1.9209951977484535e-26, + "velocityY": 6.96713079819041e-27, + "timestamp": 3.379096404619882 + }, + { + "x": 2.6438894427168456, + "y": 5.563919137233213, + "heading": 0.003139326829545763, + "angularVelocity": 0.042039393016227077, + "velocityX": -0.19304404417939905, + "velocityY": 0.2622504536593843, + "timestamp": 3.453772240884549 + }, + { + "x": 2.6152671402107748, + "y": 5.603236779531591, + "heading": 0.009643531295019244, + "angularVelocity": 0.08709918483431768, + "velocityX": -0.3832873381508234, + "velocityY": 0.5265109072100428, + "timestamp": 3.528448077149216 + }, + { + "x": 2.5727492585947744, + "y": 5.6625051004279126, + "heading": 0.019846282811438364, + "angularVelocity": 0.13662721472925177, + "velocityX": -0.5693659923044985, + "velocityY": 0.7936746859621688, + "timestamp": 3.603123913413883 + }, + { + "x": 2.516846273489969, + "y": 5.742064038671315, + "heading": 0.03428875746248491, + "angularVelocity": 0.19340224861840632, + "velocityX": -0.7486087588851783, + "velocityY": 1.0653906567772256, + "timestamp": 3.6777997496785497 + }, + { + "x": 2.4485464886328767, + "y": 5.842518783448963, + "heading": 0.05399413536855069, + "angularVelocity": 0.26387890503463146, + "velocityX": -0.9146169400101983, + "velocityY": 1.3452108446648592, + "timestamp": 3.7524755859432166 + }, + { + "x": 2.3705191792336286, + "y": 5.965194001491249, + "heading": 0.08158339935984246, + "angularVelocity": 0.3694536997685508, + "velocityX": -1.0448802892906721, + "velocityY": 1.6427699263721678, + "timestamp": 3.8271514222078835 + }, + { + "x": 2.300330937558203, + "y": 6.110432887604612, + "heading": 0.1284815107612587, + "angularVelocity": 0.6280225806270092, + "velocityX": -0.9399056667629848, + "velocityY": 1.9449248026979609, + "timestamp": 3.9018272584725504 + }, + { + "x": 2.251762063243615, + "y": 6.244623434670601, + "heading": 0.1817545952842417, + "angularVelocity": 0.7133912010596283, + "velocityX": -0.6503961220126102, + "velocityY": 1.796974145564157, + "timestamp": 3.9765030947372173 + }, + { + "x": 2.2219257093836906, + "y": 6.363297360569798, + "heading": 0.2370564478928837, + "angularVelocity": 0.7405588658242863, + "velocityX": -0.3995449579456786, + "velocityY": 1.5891877725827055, + "timestamp": 4.051178931001885 + }, + { + "x": 2.209746331851008, + "y": 6.465234411576823, + "heading": 0.2929790595103366, + "angularVelocity": 0.7488715816887712, + "velocityX": -0.16309663395688523, + "velocityY": 1.3650607225306177, + "timestamp": 4.125854767266552 + }, + { + "x": 2.214679002088127, + "y": 6.5498771941878156, + "heading": 0.3488347523758202, + "angularVelocity": 0.7479754584537774, + "velocityX": 0.06605443586378532, + "velocityY": 1.133469497562776, + "timestamp": 4.200530603531219 + }, + { + "x": 2.236396070259457, + "y": 6.616907899287828, + "heading": 0.40421575749752775, + "angularVelocity": 0.7416188139550987, + "velocityX": 0.2908178770755253, + "velocityY": 0.897622423168334, + "timestamp": 4.275206439795887 + }, + { + "x": 2.274679156572898, + "y": 6.666121488597866, + "heading": 0.45885178755752026, + "angularVelocity": 0.7316426945170152, + "velocityX": 0.5126569480622607, + "velocityY": 0.6590296375873849, + "timestamp": 4.349882276060554 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.012938020716139158, - "angularVelocity": -0.011064429584221965, - "velocityX": -3.358100659168625, - "velocityY": 1.6997858867187727, - "timestamp": 9.246907672265847 - }, - { - "x": 3.941212684187486, - "y": 5.0868100262217055, - "heading": 0.011498233293039487, - "angularVelocity": -0.020924944231025543, - "velocityX": -3.2887007006223063, - "velocityY": 1.4077333471091193, - "timestamp": 9.315714900959192 - }, - { - "x": 3.7303193403637978, - "y": 5.1698989262671065, - "heading": 0.009891075368403834, - "angularVelocity": -0.023357399435433027, - "velocityX": -3.0649881971497734, - "velocityY": 1.2075606244178938, - "timestamp": 9.384522129652536 - }, - { - "x": 3.5371027460588107, - "y": 5.242302097421373, - "heading": 0.008284033873001792, - "angularVelocity": -0.023355707327847468, - "velocityX": -2.808085690619801, - "velocityY": 1.0522611145545084, - "timestamp": 9.453329358345881 - }, - { - "x": 3.362389747323273, - "y": 5.305525449861525, - "heading": 0.006750320632579214, - "angularVelocity": -0.022290001640058982, - "velocityX": -2.5391663354759597, - "velocityY": 0.9188475344926457, - "timestamp": 9.522136587039226 - }, - { - "x": 3.2065870657576463, - "y": 5.360439069495437, - "heading": 0.00533021576404821, - "angularVelocity": -0.0206388906441795, - "velocityX": -2.2643359502240057, - "velocityY": 0.7980792233131078, - "timestamp": 9.59094381573257 - }, - { - "x": 3.0699321768781336, - "y": 5.407605853881433, - "heading": 0.00404906921437406, - "angularVelocity": -0.018619359826041904, - "velocityX": -1.9860542485811465, - "velocityY": 0.6854917031494734, - "timestamp": 9.659751044425915 - }, - { - "x": 2.952579395626419, - "y": 5.447418742231964, - "heading": 0.002924266712171353, - "angularVelocity": -0.01634715601198879, - "velocityX": -1.705529832842474, - "velocityY": 0.578614908732415, - "timestamp": 9.72855827311926 - }, - { - "x": 2.8546365398725873, - "y": 5.480167232965782, - "heading": 0.001968461119345437, - "angularVelocity": -0.013891063642247188, - "velocityX": -1.4234384615362952, - "velocityY": 0.47594549810702536, - "timestamp": 9.797365501812605 - }, - { - "x": 2.776182973714466, - "y": 5.506073331672824, - "heading": 0.0011912691725219405, - "angularVelocity": -0.01129520780857525, - "velocityX": -1.140193663485079, - "velocityY": 0.376502573915577, - "timestamp": 9.86617273050595 - }, - { - "x": 2.7172794523584742, - "y": 5.525312622061272, - "heading": 0.0006002459449936435, - "angularVelocity": -0.008589551399640344, - "velocityX": -0.8560658883459488, - "velocityY": 0.27961147039059914, - "timestamp": 9.934979959199294 - }, - { - "x": 2.6779739252366173, - "y": 5.5380274141109735, - "heading": 0.00020148371013726726, - "angularVelocity": -0.005795353808442843, - "velocityX": -0.5712412470066293, - "velocityY": 0.18478860856854942, - "timestamp": 10.00378718789264 + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.7190897983674445, + "velocityX": 0.7324089313056569, + "velocityY": 0.41852001496969765, + "timestamp": 4.424558112325221 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -3.5000485154255244e-36, - "angularVelocity": -0.0029282346341141605, - "velocityX": -0.285853063090511, - "velocityY": 0.09167570478022308, - "timestamp": 10.072594416585984 + "x": 2.411093271023251, + "y": 6.744072527671164, + "heading": 0.5643142977465416, + "angularVelocity": 0.6537087450996224, + "velocityX": 1.032025536567027, + "velocityY": 0.5897297596346315, + "timestamp": 4.503743037621965 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 7.813037085898689e-36, - "angularVelocity": 1.6441710918960667e-34, - "velocityX": -8.361910991338164e-36, - "velocityY": -1.503965302427871e-35, - "timestamp": 10.141401645279329 - }, - { - "x": 2.6750735555579293, - "y": 5.538716972759925, - "heading": 1.350818917618456e-19, - "angularVelocity": 2.121700407628309e-18, - "velocityX": 0.2633772294028884, - "velocityY": -0.088246807749562, - "timestamp": 10.205068452938024 - }, - { - "x": 2.7085920650608806, - "y": 5.52742596598787, - "heading": 3.9875003320592027e-19, - "angularVelocity": 4.14137524936932e-18, - "velocityX": 0.5264675697678677, - "velocityY": -0.17734526336837014, - "timestamp": 10.268735260596719 - }, - { - "x": 2.7588390755328, - "y": 5.51039889888726, - "heading": 7.83389944204561e-19, - "angularVelocity": 6.041451191657363e-18, - "velocityX": 0.7892183120172117, - "velocityY": -0.2674402522565467, - "timestamp": 10.332402068255414 - }, - { - "x": 2.8257885982701296, - "y": 5.487560532002736, - "heading": 1.2799519248933233e-18, - "angularVelocity": 7.799385566035206e-18, - "velocityX": 1.0515608556381997, - "velocityY": -0.35871701007777557, - "timestamp": 10.396068875914109 - }, - { - "x": 2.9094088162269203, - "y": 5.4588202139112365, - "heading": 9.407556731948414e-19, - "angularVelocity": -5.3276780189270054e-18, - "velocityX": 1.313403656188678, - "velocityY": -0.45141760908715395, - "timestamp": 10.459735683572804 - }, - { - "x": 3.0096598962036025, - "y": 5.424066617313392, - "heading": 6.890803629951045e-19, - "angularVelocity": -3.9530065893820216e-18, - "velocityX": 1.5746208057754196, - "velocityY": -0.5458668005493699, - "timestamp": 10.523402491231499 - }, - { - "x": 3.126490562172417, - "y": 5.38315977794949, - "heading": 5.07923477935949e-19, - "angularVelocity": -2.8453897991917994e-18, - "velocityX": 1.8350325745107396, - "velocityY": -0.6425143786570174, - "timestamp": 10.587069298890194 - }, - { - "x": 3.259832420061777, - "y": 5.335918462965804, - "heading": 3.751534220684201e-19, - "angularVelocity": -2.0853889294918685e-18, - "velocityX": 2.0943700931917317, - "velocityY": -0.7420085397863481, - "timestamp": 10.650736106548889 - }, - { - "x": 3.409589860765725, - "y": 5.282098877479992, - "heading": 4.172337353071023e-18, - "angularVelocity": 5.964150034596624e-17, - "velocityX": 2.3522059014921575, - "velocityY": -0.8453319314253599, - "timestamp": 10.714402914207584 - }, - { - "x": 3.575620301881546, - "y": 5.221355824269489, - "heading": 4.9261514224901164e-18, - "angularVelocity": 1.1839985341500765e-17, - "velocityX": 2.6078022005732984, - "velocityY": -0.9540772569615101, - "timestamp": 10.778069721866279 - }, - { - "x": 3.7576900306489667, - "y": 5.153163870724839, - "heading": 5.589206376297959e-18, - "angularVelocity": 1.041445265109494e-17, - "velocityX": 2.8597276267323055, - "velocityY": -1.071075432432765, - "timestamp": 10.841736529524974 - }, - { - "x": 3.9553538414525153, - "y": 5.076631094268888, - "heading": 6.855394728324636e-18, - "angularVelocity": 1.9887731120656582e-17, - "velocityX": 3.104660310018777, - "velocityY": -1.2020828320186703, - "timestamp": 10.90540333718367 + "x": 2.50599376720386, + "y": 6.798301463699205, + "heading": 0.5436087675323019, + "angularVelocity": -0.26148323227743453, + "velocityX": 1.1984667008899996, + "velocityY": 0.6848391385711283, + "timestamp": 4.582927962918709 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 8.923179883172485e-18, - "angularVelocity": 3.2478228937327026e-17, - "velocityX": 3.332116557247906, - "velocityY": -1.3615147608100575, - "timestamp": 10.969070144842364 - }, - { - "x": 4.285701077560922, - "y": 4.938388492372474, - "heading": 1.0772342870449973e-17, - "angularVelocity": 5.3398862490245696e-17, - "velocityX": 3.4133567656946298, - "velocityY": -1.4888942599373933, - "timestamp": 11.003699402864767 - }, - { - "x": 4.403715149774036, - "y": 4.881600772971698, - "heading": 9.945107603157448e-18, - "angularVelocity": -2.3888333580736016e-17, - "velocityX": 3.4079295645540313, - "velocityY": -1.6398768741750054, - "timestamp": 11.038328660887169 - }, - { - "x": 4.519367006147829, - "y": 4.8201449517293415, - "heading": 1.0140262228054944e-17, - "angularVelocity": 5.6355416212281845e-18, - "velocityX": 3.3397151131270646, - "velocityY": -1.7746791225673952, - "timestamp": 11.072957918909571 - }, - { - "x": 4.6324721141561005, - "y": 4.75411916510052, - "heading": 1.0958701743566334e-17, - "angularVelocity": 2.3634335883891303e-17, - "velocityX": 3.2661718577713317, - "velocityY": -1.906647453610143, - "timestamp": 11.107587176931974 - }, - { - "x": 4.742850082889288, - "y": 4.683628924272634, - "heading": 1.1951109976942982e-17, - "angularVelocity": 2.8658085389373404e-17, - "velocityX": 3.1874193972560034, - "velocityY": -2.0355689048343186, - "timestamp": 11.142216434954376 - }, - { - "x": 4.850325220314879, - "y": 4.608787325463072, - "heading": 1.1732443750037932e-17, - "angularVelocity": -6.3144935638988695e-18, - "velocityX": 3.1035934225348796, - "velocityY": -2.1612244409379597, - "timestamp": 11.176845692976778 - }, - { - "x": 4.959315849281407, - "y": 4.53617043444409, - "heading": 1.1207275250284524e-17, - "angularVelocity": -1.516545631482102e-17, - "velocityX": 3.147356749486827, - "velocityY": -2.096980852780761, - "timestamp": 11.21147495099918 - }, - { - "x": 5.0711194796065495, - "y": 4.467963995966005, - "heading": 1.0229768329797992e-17, - "angularVelocity": -2.822777547974504e-17, - "velocityX": 3.228588676454311, - "velocityY": -1.969618824462288, - "timestamp": 11.246104209021583 - }, - { - "x": 5.1855579490393096, - "y": 4.404277210647011, - "heading": 8.869711544920542e-18, - "angularVelocity": -3.9274788503918017e-17, - "velocityX": 3.3046757559381783, - "velocityY": -1.8391033754691917, - "timestamp": 11.280733467043985 - }, - { - "x": 5.302448702578612, - "y": 4.345211810292891, - "heading": 8.222829686597657e-18, - "angularVelocity": -1.8680211337603908e-17, - "velocityX": 3.3754911371096723, - "velocityY": -1.705650185051869, - "timestamp": 11.315362725066388 - }, - { - "x": 5.421605230513571, - "y": 4.290862095789443, - "heading": 7.194520598849692e-18, - "angularVelocity": -2.969480567798286e-17, - "velocityX": 3.4409206185669707, - "velocityY": -1.5694738382292037, - "timestamp": 11.34999198308879 - }, - { - "x": 5.542837392691082, - "y": 4.241314822773421, - "heading": 5.567604077424851e-18, - "angularVelocity": -4.698098123766793e-17, - "velocityX": 3.500859362885684, - "velocityY": -1.4307922215361755, - "timestamp": 11.384621241111192 + "x": 2.5771691415097524, + "y": 6.838973166960486, + "heading": 0.5280796125632321, + "angularVelocity": -0.19611251650329578, + "velocityX": 0.898850053077196, + "velocityY": 0.5136293695910533, + "timestamp": 4.662112888215453 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 4.441119305733273e-18, - "angularVelocity": -3.252985585087723e-17, - "velocityX": 3.5552114934161922, - "velocityY": -1.2898268911821025, - "timestamp": 11.419250499133595 - }, - { - "x": 5.934272395491136, - "y": 4.124337934678793, - "heading": 1.9097100466976255e-18, - "angularVelocity": -3.445086702659097e-17, - "velocityX": 3.6516733021145136, - "velocityY": -0.9841085377661556, - "timestamp": 11.492729324995416 - }, - { - "x": 6.207753017493673, - "y": 4.075010140740357, - "heading": -4.66846161540337e-18, - "angularVelocity": -8.952472477542546e-17, - "velocityX": 3.721897006313398, - "velocityY": -0.6713198443208224, - "timestamp": 11.566208150857237 - }, - { - "x": 6.4844286250858945, - "y": 4.049019985828209, - "heading": -6.502926410881733e-18, - "angularVelocity": -2.4965896963679208e-17, - "velocityX": 3.7653787243758563, - "velocityY": -0.3537094476852885, - "timestamp": 11.639686976719059 - }, - { - "x": 6.743277363747609, - "y": 4.038426394199003, - "heading": -5.737032029312171e-18, - "angularVelocity": 1.0423334512854795e-17, - "velocityX": 3.5227663973358068, - "velocityY": -0.1441720319419282, - "timestamp": 11.71316580258088 - }, - { - "x": 6.978672822837657, - "y": 4.030025299692622, - "heading": -4.2593802240854646e-18, - "angularVelocity": 2.010989952405427e-17, - "velocityX": 3.2035822065626656, - "velocityY": -0.11433354313770064, - "timestamp": 11.786644628442701 - }, - { - "x": 7.19055269256627, - "y": 4.022995759457438, - "heading": -3.134299993682012e-18, - "angularVelocity": 1.5311625045821916e-17, - "velocityX": 2.883550019253943, - "velocityY": -0.09566756344750842, - "timestamp": 11.860123454304523 - }, - { - "x": 7.3789017577018114, - "y": 4.017045849522277, - "heading": -2.330563766445784e-18, - "angularVelocity": 1.0938337919929083e-17, - "velocityX": 2.5633107623376485, - "velocityY": -0.080974482993913, - "timestamp": 11.933602280166344 - }, - { - "x": 7.54371362899692, - "y": 4.012025997027571, - "heading": -1.6314906834703783e-18, - "angularVelocity": 9.513939216857223e-18, - "velocityX": 2.242984551835937, - "velocityY": -0.06831699385270738, - "timestamp": 12.007081106028165 - }, - { - "x": 7.684984887810538, - "y": 4.007845278835319, - "heading": -1.3889434062297666e-18, - "angularVelocity": 3.300913894524217e-18, - "velocityX": 1.9226118158077503, - "velocityY": -0.05689691068436309, - "timestamp": 12.080559931889987 - }, - { - "x": 7.8027134339680275, - "y": 4.0044425825275765, - "heading": -1.0305851069387887e-18, - "angularVelocity": 4.877028110994567e-18, - "velocityX": 1.6022104977409415, - "velocityY": -0.04630852858402237, - "timestamp": 12.154038757751808 - }, - { - "x": 7.896897856926731, - "y": 4.001774011196325, - "heading": -6.085636422043956e-19, - "angularVelocity": 5.743443227141563e-18, - "velocityX": 1.2817899830873647, - "velocityY": -0.036317555431137405, - "timestamp": 12.22751758361363 - }, - { - "x": 7.967537148527336, - "y": 3.999806507404438, - "heading": -1.6215491216433702e-19, - "angularVelocity": 6.07533836862799e-18, - "velocityX": 0.9613557480279205, - "velocityY": -0.026776472933673492, - "timestamp": 12.30099640947545 - }, - { - "x": 8.014630554462236, - "y": 3.9985142796609896, - "heading": 9.722147651876556e-20, - "angularVelocity": 3.5299473779135435e-18, - "velocityX": 0.6409112473226125, - "velocityY": -0.017586396193627746, - "timestamp": 12.374475235337272 + "x": 2.6246193915553895, + "y": 6.866087636091841, + "heading": 0.5177268190541022, + "angularVelocity": -0.13074197481822433, + "velocityX": 0.5992333751382385, + "velocityY": 0.34241958339600737, + "timestamp": 4.741297813512197 }, { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 9.32148398835055e-44, - "angularVelocity": -1.3231223468593855e-18, - "velocityX": 0.32045879198477595, - "velocityY": -0.00867781322019328, - "timestamp": 12.447954061199093 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06537102150066688, + "velocityX": 0.29961668979104056, + "velocityY": 0.1712097929676749, + "timestamp": 4.82048273880894 }, { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": -1.3950188036384555e-43, - "angularVelocity": -3.1671262842054474e-42, - "velocityX": -2.982658307493247e-43, - "velocityY": -2.0280554768402976e-41, - "timestamp": 12.521432887060914 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 1.4429207692433446e-21, + "velocityX": 9.55884619679387e-19, + "velocityY": -1.6623744143332693e-18, + "timestamp": 4.899667664105684 + }, + { + "x": 2.67095907816642, + "y": 6.882744787327794, + "heading": 0.5061644140699368, + "angularVelocity": -0.08818113694961922, + "velocityX": 0.3122730989760289, + "velocityY": 0.04280518804462273, + "timestamp": 4.972086842979639 + }, + { + "x": 2.716191596050434, + "y": 6.888945496159438, + "heading": 0.4936002658801072, + "angularVelocity": -0.17349200011916, + "velocityX": 0.6245930786199807, + "velocityY": 0.08562246808178013, + "timestamp": 5.044506021853594 + }, + { + "x": 2.784046041436307, + "y": 6.898248047061875, + "heading": 0.4751107385388315, + "angularVelocity": -0.2553125791919922, + "velocityX": 0.9369678922205519, + "velocityY": 0.12845424440158493, + "timestamp": 5.1169252007275485 + }, + { + "x": 2.8745270921290036, + "y": 6.910653685112006, + "heading": 0.4510077220563394, + "angularVelocity": -0.33282642605549656, + "velocityX": 1.249407299276042, + "velocityY": 0.17130321336180326, + "timestamp": 5.189344379601503 + }, + { + "x": 2.9876402967876543, + "y": 6.926163879564403, + "heading": 0.4216838617313518, + "angularVelocity": -0.4049184315666644, + "velocityX": 1.5619233249733904, + "velocityY": 0.21417247051907112, + "timestamp": 5.261763558475458 + }, + { + "x": 3.1233922684020508, + "y": 6.944780361828343, + "heading": 0.3876471304183033, + "angularVelocity": -0.4699960955410605, + "velocityX": 1.8745306661191443, + "velocityY": 0.2570656358357964, + "timestamp": 5.334182737349413 + }, + { + "x": 3.281790863806145, + "y": 6.966505170067396, + "heading": 0.34957966674470664, + "angularVelocity": -0.5256544504578377, + "velocityX": 2.1872464983314113, + "velocityY": 0.2999869451276964, + "timestamp": 5.406601916223368 + }, + { + "x": 3.4628451453091853, + "y": 6.9913406807024, + "heading": 0.3084466318231206, + "angularVelocity": -0.5679853812368929, + "velocityX": 2.5000874674119666, + "velocityY": 0.34294106921911044, + "timestamp": 5.479021095097322 + }, + { + "x": 3.6665642046771225, + "y": 7.019289526069962, + "heading": 0.26572282825309695, + "angularVelocity": -0.5899515050341022, + "velocityX": 2.813053980113594, + "velocityY": 0.3859315419221543, + "timestamp": 5.551440273971277 + }, + { + "x": 3.89294996939766, + "y": 7.050353833764572, + "heading": 0.2239583223824598, + "angularVelocity": -0.576705045818428, + "velocityX": 3.126047108523004, + "velocityY": 0.42895139350692046, + "timestamp": 5.623859452845232 + }, + { + "x": 4.141945383394743, + "y": 7.084529171934603, + "heading": 0.18873600956210052, + "angularVelocity": -0.48636719399516315, + "velocityX": 3.4382523782885963, + "velocityY": 0.4719100478826658, + "timestamp": 5.696278631719187 + }, + { + "x": 4.412342843336488, + "y": 7.12169996051531, + "heading": 0.1851319702648993, + "angularVelocity": -0.049766365115434155, + "velocityX": 3.73378246130586, + "velocityY": 0.5132727153038038, + "timestamp": 5.7686978105931415 + }, + { + "x": 4.683688666421851, + "y": 7.1589163386875585, + "heading": 0.18513196457429595, + "angularVelocity": -7.857867829794732e-8, + "velocityX": 3.7468779307431594, + "velocityY": 0.5139022390328953, + "timestamp": 5.841116989467096 + }, + { + "x": 4.955034489702315, + "y": 7.196132715437329, + "heading": 0.18513195888369474, + "angularVelocity": -7.857864847824122e-8, + "velocityX": 3.7468779334372075, + "velocityY": 0.513902219390594, + "timestamp": 5.913536168341051 + }, + { + "x": 5.226380313001078, + "y": 7.233349092053675, + "heading": 0.18513195319309358, + "angularVelocity": -7.857864765970384e-8, + "velocityX": 3.7468779336898987, + "velocityY": 0.5139022175482143, + "timestamp": 5.985955347215006 + }, + { + "x": 5.497726136146309, + "y": 7.270565469789434, + "heading": 0.1851319475024924, + "angularVelocity": -7.857864817392824e-8, + "velocityX": 3.7468779315698426, + "velocityY": 0.5139022330056134, + "timestamp": 6.0583745260889605 + }, + { + "x": 5.769071959711075, + "y": 7.307781844466341, + "heading": 0.18513194181189122, + "angularVelocity": -7.85786487043621e-8, + "velocityX": 3.7468779373630063, + "velocityY": 0.5139021907674625, + "timestamp": 6.130793704962915 + }, + { + "x": 6.040417785833277, + "y": 7.3449982004969, + "heading": 0.18513193612128995, + "angularVelocity": -7.857864920895207e-8, + "velocityX": 3.746877972677337, + "velocityY": 0.5139019332894442, + "timestamp": 6.20321288383687 + }, + { + "x": 6.311959616349093, + "y": 7.380756398600711, + "heading": 0.18513193002169934, + "angularVelocity": -8.42261776360047e-8, + "velocityX": 3.7495844987200435, + "velocityY": 0.4937669642188043, + "timestamp": 6.275632062710825 + }, + { + "x": 6.569581422501917, + "y": 7.404082737536551, + "heading": 0.1318031871794107, + "angularVelocity": -0.7363897750774955, + "velocityX": 3.557369886797705, + "velocityY": 0.3221016766351085, + "timestamp": 6.34805124158478 + }, + { + "x": 6.804636460196215, + "y": 7.424191087588351, + "heading": 0.0864226666877527, + "angularVelocity": -0.6266367721545483, + "velocityX": 3.245756736670675, + "velocityY": 0.2776660874158541, + "timestamp": 6.420470420458734 + }, + { + "x": 7.017122434869165, + "y": 7.4411416449585746, + "heading": 0.04927300576152058, + "angularVelocity": -0.5129809741545235, + "velocityX": 2.934111902080261, + "velocityY": 0.23406171726589448, + "timestamp": 6.492889599332689 + }, + { + "x": 7.207038680431131, + "y": 7.454954351507805, + "heading": 0.020449488415145097, + "angularVelocity": -0.398009447145798, + "velocityX": 2.6224578697932155, + "velocityY": 0.19073271423404176, + "timestamp": 6.565308778206644 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 7.374384880065918, + "y": 7.465639114379883, + "heading": 6.042705221121846e-21, + "angularVelocity": -0.28237669541568916, + "velocityX": 2.31079946275077, + "velocityY": 0.14754051396626502, + "timestamp": 6.637727957080599 + }, + { + "x": 7.495692822755469, + "y": 7.4722697459987435, + "heading": -0.0110806824345206, + "angularVelocity": -0.18789382846574634, + "velocityX": 2.0570045130283727, + "velocityY": 0.11243484030663832, + "timestamp": 6.696701061865361 + }, + { + "x": 7.601999179168005, + "y": 7.476911827606212, + "heading": -0.01772644697635795, + "angularVelocity": -0.11269144750124323, + "velocityX": 1.8026243793764762, + "velocityY": 0.07871523170454242, + "timestamp": 6.755674166650123 + }, + { + "x": 7.693282254298601, + "y": 7.479626196867593, + "heading": -0.020765685795326113, + "angularVelocity": -0.05153601510486283, + "velocityX": 1.5478763660783506, + "velocityY": 0.046027240235815677, + "timestamp": 6.814647271434885 + }, + { + "x": 7.769527501259725, + "y": 7.480459824413743, + "heading": -0.02082974288304602, + "angularVelocity": -0.0010862085005308956, + "velocityX": 1.292881682919731, + "velocityY": 0.014135724228753763, + "timestamp": 6.873620376219647 + }, + { + "x": 7.8307246815334155, + "y": 7.479450038620926, + "heading": -0.018416174457310776, + "angularVelocity": 0.040926595853214634, + "velocityX": 1.0377133864164767, + "velocityY": -0.0171228188935137, + "timestamp": 6.932593481004409 + }, + { + "x": 7.876866310893982, + "y": 7.476627203358953, + "heading": -0.013927394073716237, + "angularVelocity": 0.07611572088628399, + "velocityX": 0.7824181807787326, + "velocityY": -0.047866485447466633, + "timestamp": 6.991566585789172 + }, + { + "x": 7.907946749436215, + "y": 7.472016493094702, + "heading": -0.0076956641130537864, + "angularVelocity": 0.10567071181696776, + "velocityX": 0.5270273399318175, + "velocityY": -0.07818327152825819, + "timestamp": 7.050539690573934 }, { - "scope": [ - 3 - ], - "type": "StopPoint" + "x": 7.923961639404297, + "y": 7.465639114379883, + "heading": -3.0160599173944472e-21, + "angularVelocity": 0.13049447101591694, + "velocityX": 0.27156260513215996, + "velocityY": -0.10814046060649564, + "timestamp": 7.109512795358696 + }, + { + "x": 7.911903030236992, + "y": 7.451294465353008, + "heading": 0.015126055505390899, + "angularVelocity": 0.1631760399586222, + "velocityX": -0.1300852089712522, + "velocityY": -0.1547464255943911, + "timestamp": 7.202210568323566 + }, + { + "x": 7.862610962703899, + "y": 7.432637923876487, + "heading": 0.03315483702979202, + "angularVelocity": 0.19448991003520194, + "velocityX": -0.531750288669542, + "velocityY": -0.20126202474780283, + "timestamp": 7.294908341288436 + }, + { + "x": 7.776083443224882, + "y": 7.4096801426109025, + "heading": 0.05392541642359817, + "angularVelocity": 0.22406772816082293, + "velocityX": -0.9334368746033223, + "velocityY": -0.2476627057079841, + "timestamp": 7.387606114253306 + }, + { + "x": 7.652317922263172, + "y": 7.3824351254025355, + "heading": 0.07722667884981763, + "angularVelocity": 0.25136809311535513, + "velocityX": -1.3351509642913992, + "velocityY": -0.2939123167359359, + "timestamp": 7.480303887218176 + }, + { + "x": 7.49131102887938, + "y": 7.3509221102471285, + "heading": 0.10276916611506416, + "angularVelocity": 0.2755458566941678, + "velocityX": -1.736901418816265, + "velocityY": -0.3399543931583946, + "timestamp": 7.5730016601830465 + }, + { + "x": 7.293058112506846, + "y": 7.315169193574846, + "heading": 0.13013085609030867, + "angularVelocity": 0.29517095287298756, + "velocityX": -2.138702042471583, + "velocityY": -0.3856933724376746, + "timestamp": 7.665699433147917 + }, + { + "x": 7.057552405050112, + "y": 7.2752213128681245, + "heading": 0.15863793525795974, + "angularVelocity": 0.30752712018717576, + "velocityX": -2.5405756786194313, + "velocityY": -0.4309475775848565, + "timestamp": 7.758397206112787 + }, + { + "x": 6.784783507993426, + "y": 7.231161947944703, + "heading": 0.18704197948975648, + "angularVelocity": 0.30641560550285446, + "velocityX": -2.9425614913106823, + "velocityY": -0.47530122368871824, + "timestamp": 7.851094979077657 + }, + { + "x": 6.474737606760309, + "y": 7.183200115705696, + "heading": 0.2122345768516852, + "angularVelocity": 0.2717713334005995, + "velocityX": -3.3446963321396703, + "velocityY": -0.5174000486201892, + "timestamp": 7.943792752042527 + }, + { + "x": 6.12780389314354, + "y": 7.132776434801359, + "heading": 0.21223458876196108, + "angularVelocity": 1.284850268489834e-7, + "velocityX": -3.74263267088681, + "velocityY": -0.5439578459284719, + "timestamp": 8.036490525007396 + }, + { + "x": 5.779638248368889, + "y": 7.091712595403529, + "heading": 0.21223459040404852, + "angularVelocity": 1.7714421734571762e-8, + "velocityX": -3.755922430915314, + "velocityY": -0.442986256135747, + "timestamp": 8.129188297972266 + }, + { + "x": 5.431472594680733, + "y": 7.050648831580209, + "heading": 0.21223459204613604, + "angularVelocity": 1.77144224789278e-8, + "velocityX": -3.7559225270719514, + "velocityY": -0.4429854408571672, + "timestamp": 8.221886070937137 + }, + { + "x": 5.083306940982422, + "y": 7.009585067843037, + "heading": 0.21223459368823233, + "angularVelocity": 1.771451744649189e-8, + "velocityX": -3.7559225271815024, + "velocityY": -0.4429854399278211, + "timestamp": 8.314583843902007 + }, + { + "x": 4.748349644408739, + "y": 6.970085293939387, + "heading": 0.249149110447713, + "angularVelocity": 0.3982244187621458, + "velocityX": -3.6134341296486574, + "velocityY": -0.42611351535511016, + "timestamp": 8.407281616866877 + }, + { + "x": 4.450609827039412, + "y": 6.934974591277922, + "heading": 0.28197417031428224, + "angularVelocity": 0.35410839782536235, + "velocityX": -3.2119414290800963, + "velocityY": -0.37876533101577264, + "timestamp": 8.499979389831747 + }, + { + "x": 4.190087491180257, + "y": 6.904252880091878, + "heading": 0.31070546232875873, + "angularVelocity": 0.3099458713572863, + "velocityX": -2.8104487036369967, + "velocityY": -0.3314180071800382, + "timestamp": 8.592677162796617 + }, + { + "x": 3.9667826341532617, + "y": 6.877920092869689, + "heading": 0.33533894531800695, + "angularVelocity": 0.2657397497411686, + "velocityX": -2.408956007083604, + "velocityY": -0.2840714116418766, + "timestamp": 8.685374935761487 + }, + { + "x": 3.780695252616961, + "y": 6.85597617430768, + "heading": 0.3558710467629364, + "angularVelocity": 0.22149508869766035, + "velocityX": -2.0074633465770906, + "velocityY": -0.23672541270570108, + "timestamp": 8.778072708726357 + }, + { + "x": 3.6318253440600605, + "y": 6.8384210809873816, + "heading": 0.37229886580171084, + "angularVelocity": 0.17721913389440508, + "velocityX": -1.6059707131617713, + "velocityY": -0.18937988215695278, + "timestamp": 8.870770481691228 + }, + { + "x": 3.5201729076832478, + "y": 6.82525478102506, + "heading": 0.38462033829713027, + "angularVelocity": 0.13292091170398426, + "velocityX": -1.2044780883692483, + "velocityY": -0.1420346955617933, + "timestamp": 8.963468254656098 + }, + { + "x": 3.4457379449845735, + "y": 6.816477253761962, + "heading": 0.39283435937679306, + "angularVelocity": 0.08861077043108337, + "velocityX": -0.802985447416123, + "velocityY": -0.09468973182801316, + "timestamp": 9.056166027620968 }, { - "scope": [ - 5 - ], - "type": "StopPoint" + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.044299923200563064, + "velocityX": -0.4014927615347733, + "velocityY": -0.04734487236445959, + "timestamp": 9.148863800585838 }, { - "scope": [ - 2 - ], - "type": "StopPoint" + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": -1.6537958542474343e-22, + "velocityX": -4.688105571426993e-23, + "velocityY": -2.4487756057177513e-23, + "timestamp": 9.241561573550708 + }, + { + "x": 3.442803441142074, + "y": 6.8114279191425675, + "heading": 0.392498149134446, + "angularVelocity": -0.05010039849219718, + "velocityX": 0.38660846281780004, + "velocityY": -0.007449238528134429, + "timestamp": 9.330237803665844 + }, + { + "x": 3.511369395408112, + "y": 6.810106800490753, + "heading": 0.38361321030304085, + "angularVelocity": -0.10019527014025122, + "velocityX": 0.773216838120132, + "velocityY": -0.01489822751935824, + "timestamp": 9.418914033780979 + }, + { + "x": 3.6142183227587346, + "y": 6.808125168158371, + "heading": 0.3702881620154029, + "angularVelocity": -0.1502662919965921, + "velocityX": 1.1598252115260796, + "velocityY": -0.02234682653748938, + "timestamp": 9.507590263896114 + }, + { + "x": 3.751350230137559, + "y": 6.805483069997971, + "heading": 0.3525266232817515, + "angularVelocity": -0.2002965023500672, + "velocityX": 1.5464336632350622, + "velocityY": -0.02979488592342862, + "timestamp": 9.59626649401125 + }, + { + "x": 3.922765130885179, + "y": 6.8021805681387715, + "heading": 0.3303335458268625, + "angularVelocity": -0.2502708722063848, + "velocityX": 1.9330422653856434, + "velocityY": -0.03724224467945297, + "timestamp": 9.684942724126385 + }, + { + "x": 4.128463043748926, + "y": 6.7982177401202675, + "heading": 0.30371498826898996, + "angularVelocity": -0.3001769191508424, + "velocityX": 2.3196510789494917, + "velocityY": -0.04468872902421391, + "timestamp": 9.77361895424152 + }, + { + "x": 4.36844399152108, + "y": 6.793594680078811, + "heading": 0.2726778324022649, + "angularVelocity": -0.3500053602462237, + "velocityX": 2.7062601495413983, + "velocityY": -0.05213415179529137, + "timestamp": 9.862295184356656 + }, + { + "x": 4.642707998965045, + "y": 6.788311499898704, + "heading": 0.23722944053291425, + "angularVelocity": -0.39975077676763104, + "velocityX": 3.0928694993896952, + "velocityY": -0.05957831284943316, + "timestamp": 9.950971414471791 + }, + { + "x": 4.951255087719671, + "y": 6.782368330248145, + "heading": 0.19737726949719422, + "angularVelocity": -0.4494121027018933, + "velocityX": 3.4794790932588646, + "velocityY": -0.06702100036100427, + "timestamp": 10.039647644586926 + }, + { + "x": 5.286562691971156, + "y": 6.775920768512488, + "heading": 0.19737726761878174, + "angularVelocity": -2.1182818612377388e-8, + "velocityX": 3.781256869130857, + "velocityY": -0.0727090193988374, + "timestamp": 10.128323874702062 + }, + { + "x": 5.59511113537339, + "y": 6.769985792808053, + "heading": 0.15789277163023122, + "angularVelocity": -0.44526583885314686, + "velocityX": 3.4794943695917215, + "velocityY": -0.06692859740122076, + "timestamp": 10.217000104817197 + }, + { + "x": 5.869376453129913, + "y": 6.764710551221358, + "heading": 0.12280116553958556, + "angularVelocity": -0.3957273109725502, + "velocityX": 3.092884275757129, + "velocityY": -0.05948878949687072, + "timestamp": 10.305676334932333 + }, + { + "x": 6.1093586237611985, + "y": 6.760094935517236, + "heading": 0.0920991922571767, + "angularVelocity": -0.34622551322429906, + "velocityX": 2.7062739396983573, + "velocityY": -0.0520502021582253, + "timestamp": 10.394352565047468 + }, + { + "x": 6.315057635592866, + "y": 6.756138851805421, + "heading": 0.06578475722782369, + "angularVelocity": -0.2967473357311971, + "velocityX": 2.3196634719878233, + "velocityY": -0.04461267361815281, + "timestamp": 10.483028795162603 + }, + { + "x": 6.486473481982027, + "y": 6.752842221237321, + "heading": 0.04385663735915925, + "angularVelocity": -0.24728295102524525, + "velocityX": 1.9330529293656087, + "velocityY": -0.037176034252009674, + "timestamp": 10.571705025277739 + }, + { + "x": 6.623606159513812, + "y": 6.750204980424608, + "heading": 0.026314217868262327, + "angularVelocity": -0.197825499213489, + "velocityX": 1.5464423482339522, + "velocityY": -0.02974010971473329, + "timestamp": 10.660381255392874 + }, + { + "x": 6.726455666874299, + "y": 6.748227081682736, + "heading": 0.013157277625411766, + "angularVelocity": -0.14837054107699252, + "velocityX": 1.1598317522852502, + "velocityY": -0.02230472291507881, + "timestamp": 10.74905748550801 + }, + { + "x": 6.795022004077355, + "y": 6.7469084931623255, + "heading": 0.004385827059285361, + "angularVelocity": -0.09891546533651373, + "velocityX": 0.7732211564928945, + "velocityY": -0.014869695280220821, + "timestamp": 10.837733715623145 }, { - "scope": [ - 12 - ], - "type": "StopPoint" + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -2.734177125091434e-26, + "angularVelocity": -0.049458880396594275, + "velocityX": 0.38661057021351825, + "velocityY": -0.007434847510947748, + "timestamp": 10.92640994573828 }, { - "scope": [ - 4 - ], - "type": "WptVelocityDirection", - "direction": 0.5125504196 + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -1.3407189209020893e-26, + "angularVelocity": 6.223794221869343e-27, + "velocityX": 3.782126331053824e-27, + "velocityY": 9.545461702862487e-27, + "timestamp": 11.015086175853416 } ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "CenterLanePCBADSprint": { - "waypoints": [ + "trajectoryWaypoints": [ { + "timestamp": 0, + "isStopPoint": true, "x": 1.2899744510650635, "y": 5.590954303741455, "heading": 0, @@ -13157,6 +14470,8 @@ "controlIntervalCount": 17 }, { + "timestamp": 1.2792606449873303, + "isStopPoint": false, "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, @@ -13166,6 +14481,8 @@ "controlIntervalCount": 16 }, { + "timestamp": 2.4932364716640962, + "isStopPoint": true, "x": 1.8129411935806274, "y": 5.542999267578125, "heading": 0, @@ -13175,6 +14492,8 @@ "controlIntervalCount": 9 }, { + "timestamp": 3.379096404619882, + "isStopPoint": true, "x": 2.6583051681518555, "y": 5.54433536529541, "heading": 0, @@ -13184,6 +14503,8 @@ "controlIntervalCount": 14 }, { + "timestamp": 4.424558112325221, + "isStopPoint": false, "x": 2.3293724060058594, "y": 6.6973748207092285, "heading": 0.5125504196, @@ -13193,6 +14514,8 @@ "controlIntervalCount": 6 }, { + "timestamp": 4.899667664105684, + "isStopPoint": true, "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, @@ -13202,6 +14525,8 @@ "controlIntervalCount": 24 }, { + "timestamp": 6.637727957080599, + "isStopPoint": false, "x": 7.374384880065918, "y": 7.465639114379883, "heading": 0, @@ -13211,6 +14536,8 @@ "controlIntervalCount": 8 }, { + "timestamp": 7.109512795358696, + "isStopPoint": false, "x": 7.923961639404297, "y": 7.465639114379883, "heading": 0, @@ -13220,6 +14547,8 @@ "controlIntervalCount": 23 }, { + "timestamp": 9.241561573550708, + "isStopPoint": true, "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, @@ -13229,6 +14558,8 @@ "controlIntervalCount": 20 }, { + "timestamp": 11.015086175853416, + "isStopPoint": true, "x": 6.829305171966553, "y": 6.746249198913574, "heading": 0, @@ -13238,1362 +14569,64 @@ "controlIntervalCount": 40 } ], - "trajectory": [ - { - "x": 1.289974451065063, - "y": 5.590954303741456, - "heading": 6.19779206460361e-17, - "angularVelocity": 3.0320144312848013e-18, - "velocityX": 2.9896394667646305e-17, - "velocityY": -6.392586992550233e-17, - "timestamp": 0 - }, - { - "x": 1.3085927366636296, - "y": 5.570883327334207, - "heading": -0.01041389258493421, - "angularVelocity": -0.12226255374555442, - "velocityX": 0.21858484952476373, - "velocityY": -0.23563992154741434, - "timestamp": 0.08517646872162599 - }, - { - "x": 1.3442657042493709, - "y": 5.532508491008622, - "heading": -0.03089006103804589, - "angularVelocity": -0.24039701058788923, - "velocityX": 0.4188124739278457, - "velocityY": -0.45053330927585317, - "timestamp": 0.17035293744325197 - }, - { - "x": 1.3955881762749254, - "y": 5.477434538773948, - "heading": -0.060869932572704226, - "angularVelocity": -0.35197363760921707, - "velocityX": 0.6025428477586634, - "velocityY": -0.646586469963527, - "timestamp": 0.25552940616487796 - }, - { - "x": 1.4613425796478687, - "y": 5.4070770106059785, - "heading": -0.09976067247727309, - "angularVelocity": -0.45659018844361554, - "velocityX": 0.7719785095557636, - "velocityY": -0.826020721731408, - "timestamp": 0.34070587488650395 - }, - { - "x": 1.5403946295377764, - "y": 5.322786826751108, - "heading": -0.14691970563793563, - "angularVelocity": -0.5536626942681541, - "velocityX": 0.9280972911458275, - "velocityY": -0.9895947216401798, - "timestamp": 0.42588234360812993 - }, - { - "x": 1.6316218404845053, - "y": 5.225944550707253, - "heading": -0.20162745404412188, - "angularVelocity": -0.6422871155293178, - "velocityX": 1.0710377210503823, - "velocityY": -1.1369604480828668, - "timestamp": 0.5110588123297559 - }, - { - "x": 1.733844381505324, - "y": 5.118072938931053, - "heading": -0.2630340547325837, - "angularVelocity": -0.7209338636607621, - "velocityX": 1.2001265438099284, - "velocityY": -1.2664485085516666, - "timestamp": 0.5962352810513819 - }, - { - "x": 1.8457280635804503, - "y": 5.001046382736992, - "heading": -0.33003861392476214, - "angularVelocity": -0.7866557536115174, - "velocityX": 1.3135515448613582, - "velocityY": -1.373930593160977, - "timestamp": 0.6814117497730079 - }, - { - "x": 1.9656142072747882, - "y": 4.877758551128575, - "heading": -0.40085023097520717, - "angularVelocity": -0.8313518758551945, - "velocityX": 1.4075030990795228, - "velocityY": -1.447440043697368, - "timestamp": 0.7665882184946339 - }, - { - "x": 2.0856915448639635, - "y": 4.7606487505271025, - "heading": -0.4673031995363478, - "angularVelocity": -0.7801798966134944, - "velocityX": 1.4097477788332962, - "velocityY": -1.3749079101202415, - "timestamp": 0.8517646872162599 - }, - { - "x": 2.1993786689736172, - "y": 4.652829385838518, - "heading": -0.5273883646655675, - "angularVelocity": -0.7054197718103367, - "velocityX": 1.3347245526367961, - "velocityY": -1.2658351104101313, - "timestamp": 0.9369411559378859 - }, - { - "x": 2.3039823973737983, - "y": 4.5566291473122265, - "heading": -0.5809972368261334, - "angularVelocity": -0.6293859438546427, - "velocityX": 1.2280824736001708, - "velocityY": -1.1294227146313731, - "timestamp": 1.0221176246595118 - }, - { - "x": 2.3979860520770147, - "y": 4.47341696133058, - "heading": -0.6272322006251169, - "angularVelocity": -0.5428138134029581, - "velocityX": 1.1036340918339758, - "velocityY": -0.9769386689838181, - "timestamp": 1.107294093381138 - }, - { - "x": 2.480103614094797, - "y": 4.40444635319333, - "heading": -0.6652761118925774, - "angularVelocity": -0.4466481393094119, - "velocityX": 0.964087420507652, - "velocityY": -0.8097378204614404, - "timestamp": 1.192470562102764 - }, - { - "x": 2.549135601344952, - "y": 4.350942377101982, - "heading": -0.6944118607427208, - "angularVelocity": -0.3420633572561569, - "velocityX": 0.810458431609397, - "velocityY": -0.6281544292028495, - "timestamp": 1.2776470308243901 - }, - { - "x": 2.6038790433453562, - "y": 4.3141752600275325, - "heading": -0.7139618385994653, - "angularVelocity": -0.2295232257237371, - "velocityX": 0.6427061701667566, - "velocityY": -0.431658151908278, - "timestamp": 1.3628234995460162 - }, - { - "x": 2.6430389310079985, - "y": 4.2955467477546305, - "heading": -0.7232599969808831, - "angularVelocity": -0.1091634640525668, - "velocityX": 0.459750072412904, - "velocityY": -0.2187049140741477, - "timestamp": 1.4479999682676423 - }, - { - "x": 2.665171623229982, - "y": 4.296644687652586, - "heading": -0.721655115096118, - "angularVelocity": 0.018841845745096026, - "velocityX": 0.2598451491845503, - "velocityY": 0.012890178642452559, - "timestamp": 1.5331764369892684 - }, - { - "x": 2.66906909991143, - "y": 4.318945956778202, - "heading": -0.7091344221786624, - "angularVelocity": 0.14601587711521094, - "velocityX": 0.045452234946537245, - "velocityY": 0.2600766102664744, - "timestamp": 1.6189252826957972 - }, - { - "x": 2.6560642905413947, - "y": 4.360643314893585, - "heading": -0.6858248901313277, - "angularVelocity": 0.2718349367303499, - "velocityX": -0.15166162602986216, - "velocityY": 0.4862731127377579, - "timestamp": 1.704674128402326 - }, - { - "x": 2.627632992233998, - "y": 4.419910426276983, - "heading": -0.652529270737317, - "angularVelocity": 0.38829233349639936, - "velocityX": -0.331564793358298, - "velocityY": 0.6911709527407293, - "timestamp": 1.7904229741088546 - }, - { - "x": 2.5850645171946978, - "y": 4.495119603214681, - "heading": -0.6100207421436514, - "angularVelocity": 0.4957329541105318, - "velocityX": -0.4964320474352394, - "velocityY": 0.877086756305711, - "timestamp": 1.8761718198153834 - }, - { - "x": 2.5295631514165375, - "y": 4.584706937933874, - "heading": -0.5590704924389158, - "angularVelocity": 0.5941800065637087, - "velocityX": -0.6472549609368695, - "velocityY": 1.0447643228435226, - "timestamp": 1.9619206655219121 - }, - { - "x": 2.46234032691493, - "y": 4.687033037499603, - "heading": -0.5004982071284244, - "angularVelocity": 0.6830679157006081, - "velocityX": -0.7839501972035283, - "velocityY": 1.1933233470679585, - "timestamp": 2.047669511228441 - }, - { - "x": 2.384701887418879, - "y": 4.800204071292841, - "heading": -0.43526625936464175, - "angularVelocity": 0.7607326632364927, - "velocityX": -0.9054167301769336, - "velocityY": 1.319796585723857, - "timestamp": 2.1334183569349694 - }, - { - "x": 2.298178863290851, - "y": 4.921641059920068, - "heading": -0.36471802922759106, - "angularVelocity": 0.8227309598836902, - "velocityX": -1.0090284413174404, - "velocityY": 1.416193858082253, - "timestamp": 2.219167202641498 - }, - { - "x": 2.2047510720717436, - "y": 5.044849013310237, - "heading": -0.2923595970689714, - "angularVelocity": 0.8438414717122018, - "velocityX": -1.0895515904535924, - "velocityY": 1.4368467863910614, - "timestamp": 2.3049160483480264 - }, - { - "x": 2.116347360692902, - "y": 5.158956328540576, - "heading": -0.22539524544258233, - "angularVelocity": 0.7809358956920709, - "velocityX": -1.0309609493916598, - "velocityY": 1.3307154666649113, - "timestamp": 2.390664894054555 - }, - { - "x": 2.0362102002706437, - "y": 5.261312637562149, - "heading": -0.16556699187625293, - "angularVelocity": 0.6977149729932085, - "velocityX": -0.9345567250727145, - "velocityY": 1.1936756486715054, - "timestamp": 2.4764137397610835 - }, - { - "x": 1.9661275657868833, - "y": 5.350222584355773, - "heading": -0.1134489428170574, - "angularVelocity": 0.6077988412528272, - "velocityX": -0.8173011998740509, - "velocityY": 1.0368646488597075, - "timestamp": 2.562162585467612 - }, - { - "x": 1.9075019379268083, - "y": 5.42424212633875, - "heading": -0.06991588076878923, - "angularVelocity": 0.5076810269523403, - "velocityX": -0.6836899946235904, - "velocityY": 0.8632132756201296, - "timestamp": 2.6479114311741405 - }, - { - "x": 1.861590779787906, - "y": 5.48200360252104, - "heading": -0.03588037308734902, - "angularVelocity": 0.3969208844854328, - "velocityX": -0.5354142992901785, - "velocityY": 0.673612288379681, - "timestamp": 2.733660276880669 - }, - { - "x": 1.8296344375134035, - "y": 5.522099910983847, - "heading": -0.012259641477970167, - "angularVelocity": 0.2754641349951199, - "velocityX": -0.3726737311878495, - "velocityY": 0.4676017284248481, - "timestamp": 2.8194091225871976 - }, - { - "x": 1.8129411935806854, - "y": 5.542999267578127, - "heading": 2.3632850028431835e-17, - "angularVelocity": 0.14297150447865262, - "velocityX": -0.1946760191950624, - "velocityY": 0.24372755600475027, - "timestamp": 2.905157968293726 - }, - { - "x": 1.812941193580624, - "y": 5.542999267578126, - "heading": -4.920156603511903e-17, - "angularVelocity": -5.733397110540358e-17, - "velocityX": -7.214388151371406e-13, - "velocityY": -9.03683027618772e-16, - "timestamp": 2.9909068140002546 - }, - { - "x": 1.8513325137310372, - "y": 5.5430599450572595, - "heading": -0.000016850565291482023, - "angularVelocity": -0.00016991086246254008, - "velocityX": 0.3871147468977978, - "velocityY": 0.0006118348336243464, - "timestamp": 3.090079786505359 - }, - { - "x": 1.9275402570059652, - "y": 5.543180391388335, - "heading": -0.000015746696122309923, - "angularVelocity": 0.00001113074602288469, - "velocityX": 0.7684325814778618, - "velocityY": 0.0012145076227106227, - "timestamp": 3.189252759010463 - }, - { - "x": 2.0338963277323163, - "y": 5.54334848714078, - "heading": 0.000009170336978677823, - "angularVelocity": 0.00025124822289482134, - "velocityX": 1.0724299982122594, - "velocityY": 0.0016949754373461064, - "timestamp": 3.2884257315155674 - }, - { - "x": 2.163383514243291, - "y": 5.5435531416216355, - "heading": 0.000033654463962368084, - "angularVelocity": 0.0002468830606297171, - "velocityX": 1.305670115961409, - "velocityY": 0.00206361144256778, - "timestamp": 3.3875987040206716 - }, - { - "x": 2.3079360710490797, - "y": 5.54378160701675, - "heading": 0.00003317773344428256, - "angularVelocity": -0.000004807060894986283, - "velocityX": 1.457580156714077, - "velocityY": 0.0023037062351123753, - "timestamp": 3.486771676525776 - }, - { - "x": 2.437398077457075, - "y": 5.543986221690917, - "heading": 0.000022146124274066103, - "angularVelocity": -0.00011123604437218979, - "velocityX": 1.3054162151016944, - "velocityY": 0.002063210056106694, - "timestamp": 3.58594464903088 - }, - { - "x": 2.5437229473484053, - "y": 5.544154268122903, - "heading": 0.000015406432122086, - "angularVelocity": -0.00006795896080977589, - "velocityX": 1.0721153879486738, - "velocityY": 0.0016944781198031775, - "timestamp": 3.6851176215359844 - }, - { - "x": 2.619913870706316, - "y": 5.544274687859312, - "heading": 0.000009213432317016853, - "angularVelocity": -0.00006244644733859944, - "velocityX": 0.7682629796539725, - "velocityY": 0.0012142394582550931, - "timestamp": 3.7842905940410887 - }, - { - "x": 2.6583051681519314, - "y": 5.5443353652954785, - "heading": 6.370510606485668e-14, - "angularVelocity": -0.00009290265301587805, - "velocityX": 0.38711451795641516, - "velocityY": 0.0006118344003786568, - "timestamp": 3.883463566546193 - }, - { - "x": 2.658305168152, - "y": 5.544335365295488, - "heading": 6.625668653050138e-14, - "angularVelocity": 2.6906565131836242e-14, - "velocityX": 7.155954565396242e-13, - "velocityY": 8.671180221391495e-14, - "timestamp": 3.982636539051297 - }, - { - "x": 2.6398879819889713, - "y": 5.568034006444528, - "heading": 0.008388499707182374, - "angularVelocity": 0.0943787022324955, - "velocityX": -0.20721108536177554, - "velocityY": 0.26663254150593735, - "timestamp": 4.071517817439562 - }, - { - "x": 2.6055876283476542, - "y": 5.613232266775393, - "heading": 0.025296828917652628, - "angularVelocity": 0.19023499118162804, - "velocityX": -0.3859120195327329, - "velocityY": 0.5085239675934979, - "timestamp": 4.160399095827827 - }, - { - "x": 2.5578825329127857, - "y": 5.677978135359711, - "heading": 0.05006181260458975, - "angularVelocity": 0.27862992225075944, - "velocityX": -0.5367282773164671, - "velocityY": 0.7284533903912082, - "timestamp": 4.2492803742160925 - }, - { - "x": 2.4993543830957403, - "y": 5.760558609750369, - "heading": 0.0821470752651059, - "angularVelocity": 0.36099011222914734, - "velocityX": -0.6584980648176972, - "velocityY": 0.9291098855468343, - "timestamp": 4.338161652604358 - }, - { - "x": 2.433098392034208, - "y": 5.859358208334035, - "heading": 0.12114426250239245, - "angularVelocity": 0.43875592188117873, - "velocityX": -0.7454437229429913, - "velocityY": 1.1115906563809108, - "timestamp": 4.427042930992623 - }, - { - "x": 2.3637465533640976, - "y": 5.972668871410117, - "heading": 0.16678728284828395, - "angularVelocity": 0.5135279461940396, - "velocityX": -0.7802749907275444, - "velocityY": 1.2748541102325668, - "timestamp": 4.515924209380888 - }, - { - "x": 2.300988842467506, - "y": 6.095835737634532, - "heading": 0.21776795840799054, - "angularVelocity": 0.5735817090404455, - "velocityX": -0.7060847012414084, - "velocityY": 1.385745889998316, - "timestamp": 4.604805487769153 - }, - { - "x": 2.2515752134889366, - "y": 6.217231027383832, - "heading": 0.2695393710632505, - "angularVelocity": 0.582478263071738, - "velocityX": -0.5559509254902292, - "velocityY": 1.3658139481175915, - "timestamp": 4.693686766157418 - }, - { - "x": 2.217045589242532, - "y": 6.33145065782542, - "heading": 0.3198059590268783, - "angularVelocity": 0.5655475357136119, - "velocityX": -0.3884915346931451, - "velocityY": 1.2850808686903799, - "timestamp": 4.782568044545683 - }, - { - "x": 2.19907919431313, - "y": 6.435243176299548, - "heading": 0.36734143796630997, - "angularVelocity": 0.5348199283510537, - "velocityX": -0.20213924974232386, - "velocityY": 1.1677658147606698, - "timestamp": 4.871449322933948 - }, - { - "x": 2.1993959276283994, - "y": 6.526053318679287, - "heading": 0.41157070153690833, - "angularVelocity": 0.4976218206191788, - "velocityX": 0.0035635549001480525, - "velocityY": 1.021701577951703, - "timestamp": 4.960330601322213 - }, - { - "x": 2.219770279567901, - "y": 6.601608618777817, - "heading": 0.45152771224881855, - "angularVelocity": 0.4495548605556116, - "velocityX": 0.2292310856552352, - "velocityY": 0.8500699075051399, - "timestamp": 5.049211879710478 - }, - { - "x": 2.2626490848637184, - "y": 6.659247151615926, - "heading": 0.48436572022362784, - "angularVelocity": 0.36945922212429394, - "velocityX": 0.4824278641490618, - "velocityY": 0.648489016846146, - "timestamp": 5.1380931580987435 - }, - { - "x": 2.329372405994089, - "y": 6.697374820688125, - "heading": 0.5125504195998649, - "angularVelocity": 0.31710501792120865, - "velocityX": 0.7507016363871187, - "velocityY": 0.4289730049288785, - "timestamp": 5.2269744364870085 - }, - { - "x": 2.399251093648798, - "y": 6.73730555904706, - "heading": 0.5293913369356736, - "angularVelocity": 0.21745079806450165, - "velocityX": 0.902277239279105, - "velocityY": 0.5155877648973247, - "timestamp": 5.304421461219836 - }, - { - "x": 2.4725481079431484, - "y": 6.779189629834486, - "heading": 0.507422235287016, - "angularVelocity": -0.28366617987293774, - "velocityX": 0.9464148501358945, - "velocityY": 0.5408092945202858, - "timestamp": 5.381868485952664 - }, - { - "x": 2.537702213604298, - "y": 6.816420603018375, - "heading": 0.5016725044650684, - "angularVelocity": -0.07424082257084566, - "velocityX": 0.8412731915467285, - "velocityY": 0.48072825693890747, - "timestamp": 5.459315510685491 - }, - { - "x": 2.5904059035391898, - "y": 6.846537042294488, - "heading": 0.5036538467311155, - "angularVelocity": 0.025583194098508056, - "velocityX": 0.680512777970975, - "velocityY": 0.38886502613926416, - "timestamp": 5.536762535418319 - }, - { - "x": 2.6280877308244723, - "y": 6.868069547224279, - "heading": 0.508503710225543, - "angularVelocity": 0.06262168896947905, - "velocityX": 0.4865497083575037, - "velocityY": 0.27802882066934376, - "timestamp": 5.614209560151147 - }, - { - "x": 2.648344516781087, - "y": 6.8796448707953655, - "heading": 0.5125504196001363, - "angularVelocity": 0.052251321320124576, - "velocityX": 0.26155667100350394, - "velocityY": 0.14946117855453953, - "timestamp": 5.691656584883974 - }, - { - "x": 2.6483445167657806, - "y": 6.879644870779064, - "heading": 0.5125504196000689, - "angularVelocity": 1.8669291195039983e-14, - "velocityX": -4.749030905139852e-11, - "velocityY": 6.098431731550748e-11, - "timestamp": 5.769103609616802 - }, - { - "x": 2.6747339306487334, - "y": 6.883481123686636, - "heading": 0.5098224624623802, - "angularVelocity": -0.03296702980330207, - "velocityX": 0.3189128531205414, - "velocityY": 0.046360649216292196, - "timestamp": 5.851851650734406 - }, - { - "x": 2.7264961186821273, - "y": 6.89099146776212, - "heading": 0.5039121473834008, - "angularVelocity": -0.07142543798202369, - "velocityX": 0.6255397388782451, - "velocityY": 0.09076159355636307, - "timestamp": 5.93459969185201 - }, - { - "x": 2.8018050306182753, - "y": 6.9018959167453104, - "heading": 0.4949465844489951, - "angularVelocity": -0.10834773625231511, - "velocityX": 0.9100990297657242, - "velocityY": 0.13177893803804336, - "timestamp": 6.017347732969614 - }, - { - "x": 2.899101313212433, - "y": 6.915952840560763, - "heading": 0.4831136364117761, - "angularVelocity": -0.14299973603485935, - "velocityX": 1.1758137265856894, - "velocityY": 0.16987621248329202, - "timestamp": 6.100095774087218 - }, - { - "x": 3.017081432389269, - "y": 6.932956858771751, - "heading": 0.4685947114251132, - "angularVelocity": -0.1754594403754914, - "velocityX": 1.42577537284728, - "velocityY": 0.20549148936151598, - "timestamp": 6.182843815204822 - }, - { - "x": 3.1546294358748312, - "y": 6.9527283369160715, - "heading": 0.4515640044783447, - "angularVelocity": -0.20581401948312003, - "velocityX": 1.6622508717768, - "velocityY": 0.23893590563959213, - "timestamp": 6.265591856322426 - }, - { - "x": 3.3107722570719647, - "y": 6.975106161489216, - "heading": 0.43219034104896137, - "angularVelocity": -0.2341283632545328, - "velocityX": 1.8869669793779915, - "velocityY": 0.2704332848357982, - "timestamp": 6.34833989744003 - }, - { - "x": 3.484600968601402, - "y": 6.999935624537231, - "heading": 0.4106444392061959, - "angularVelocity": -0.2603795999490034, - "velocityX": 2.1006988102882644, - "velocityY": 0.3000610372485155, - "timestamp": 6.431087938557634 - }, - { - "x": 3.675234170530874, - "y": 7.027062068947035, - "heading": 0.38710417033209454, - "angularVelocity": -0.28448128265230255, - "velocityX": 2.3037790303523367, - "velocityY": 0.32781977728352046, - "timestamp": 6.513835979675238 - }, - { - "x": 3.8817881153060045, - "y": 7.056325014551516, - "heading": 0.36176128472936936, - "angularVelocity": -0.3062656862983269, - "velocityX": 2.4961792688429623, - "velocityY": 0.3536391340419632, - "timestamp": 6.596584020792842 - }, - { - "x": 4.1033458782194, - "y": 7.087551279488626, - "heading": 0.3348309137177563, - "angularVelocity": -0.32545025414364576, - "velocityX": 2.677498583906162, - "velocityY": 0.3773656090873216, - "timestamp": 6.679332061910446 - }, - { - "x": 4.338922324182521, - "y": 7.120545790816314, - "heading": 0.30656596895157695, - "angularVelocity": -0.34157841544560996, - "velocityX": 2.846912661392294, - "velocityY": 0.39873465138360303, - "timestamp": 6.76208010302805 - }, - { - "x": 4.587417768628414, - "y": 7.155077045432509, - "heading": 0.27728164973070885, - "angularVelocity": -0.3538974315929522, - "velocityX": 3.00303718480442, - "velocityY": 0.4173060068834074, - "timestamp": 6.844828144145654 - }, - { - "x": 4.847543704393879, - "y": 7.190849002148663, - "heading": 0.2474070078170501, - "angularVelocity": -0.36103140944690143, - "velocityX": 3.1435902560613274, - "velocityY": 0.4322997406709641, - "timestamp": 6.927576185263258 - }, - { - "x": 5.117670126803491, - "y": 7.227429623630717, - "heading": 0.21762399464747098, - "angularVelocity": -0.35992408723308145, - "velocityX": 3.264444919314729, - "velocityY": 0.4420723558891399, - "timestamp": 7.010324226380862 - }, - { - "x": 5.395335267694769, - "y": 7.263944271068559, - "heading": 0.1895241870207491, - "angularVelocity": -0.33958275322536735, - "velocityX": 3.3555494141142153, - "velocityY": 0.44127506759904395, - "timestamp": 7.093072267498466 - }, - { - "x": 5.667910261992729, - "y": 7.2967916361655485, - "heading": 0.1669091076503975, - "angularVelocity": -0.27330048016738406, - "velocityX": 3.294035612402776, - "velocityY": 0.3969564071048189, - "timestamp": 7.17582030861607 - }, - { - "x": 5.931901746285159, - "y": 7.327414779180602, - "heading": 0.14778373205342693, - "angularVelocity": -0.23112783503586568, - "velocityX": 3.1903049392702143, - "velocityY": 0.3700769541063461, - "timestamp": 7.258568349733674 - }, - { - "x": 6.1801220482371395, - "y": 7.354810947360831, - "heading": 0.11632647881372146, - "angularVelocity": -0.38015707459464126, - "velocityX": 2.999712121271843, - "velocityY": 0.33107935620263895, - "timestamp": 7.341316390851278 - }, - { - "x": 6.415697256681246, - "y": 7.379780463537999, - "heading": 0.08899977607154176, - "angularVelocity": -0.3302398748429845, - "velocityX": 2.84689770612573, - "velocityY": 0.3017535622589267, - "timestamp": 7.424064431968882 - }, - { - "x": 6.637987945996846, - "y": 7.402299511961184, - "heading": 0.06474721752358573, - "angularVelocity": -0.2930892166194916, - "velocityX": 2.686355910222349, - "velocityY": 0.2721399578655483, - "timestamp": 7.5068124730864865 - }, - { - "x": 6.845920803268571, - "y": 7.422253782487123, - "heading": 0.04362794173118966, - "angularVelocity": -0.2552238760840361, - "velocityX": 2.5128432584428544, - "velocityY": 0.24114492931132497, - "timestamp": 7.5895605142040905 - }, - { - "x": 7.038548371158306, - "y": 7.439534256583214, - "heading": 0.025735796286898687, - "angularVelocity": -0.2162243988212622, - "velocityX": 2.3278807001118484, - "velocityY": 0.2088324250667052, - "timestamp": 7.6723085553216945 - }, - { - "x": 7.2149794023417675, - "y": 7.454031322663086, - "heading": 0.011159229205506762, - "angularVelocity": -0.17615603807074215, - "velocityX": 2.1321475264013814, - "velocityY": 0.1751952781490545, - "timestamp": 7.755056596439299 - }, - { - "x": 7.374384880065902, - "y": 7.465639114379918, - "heading": -1.1165566205788637e-16, - "angularVelocity": -0.134857925997875, - "velocityX": 1.9263957861864134, - "velocityY": 0.14027874932231896, - "timestamp": 7.837804637556903 - }, - { - "x": 7.500924464216497, - "y": 7.473646968954779, - "heading": -0.0070937662662868915, - "angularVelocity": -0.09816609550664941, - "velocityX": 1.7511003938959169, - "velocityY": 0.11081557912801578, - "timestamp": 7.9100675327008405 - }, - { - "x": 7.613899532464792, - "y": 7.47952275118133, - "heading": -0.01171150501248939, - "angularVelocity": -0.06390193386252535, - "velocityX": 1.5633897316635108, - "velocityY": 0.08131119317636289, - "timestamp": 7.982330427844778 - }, - { - "x": 7.712046212988902, - "y": 7.483209835153203, - "heading": -0.014057088311073107, - "angularVelocity": -0.032459027470621904, - "velocityX": 1.3581891554251948, - "velocityY": 0.05102319751389575, - "timestamp": 8.054593322988717 - }, - { - "x": 7.79365194754084, - "y": 7.484595849351514, - "heading": -0.014345050898318633, - "angularVelocity": -0.003984930117620908, - "velocityX": 1.129289580626482, - "velocityY": 0.019180164253482204, - "timestamp": 8.126856218132655 - }, - { - "x": 7.856538953365895, - "y": 7.483519387813377, - "heading": -0.012796446625875068, - "angularVelocity": 0.02143014432730538, - "velocityX": 0.8702530627897808, - "velocityY": -0.0148964629220964, - "timestamp": 8.199119113276593 - }, - { - "x": 7.899249386445817, - "y": 7.479924646329156, - "heading": -0.009762276595150929, - "angularVelocity": 0.04198793896480952, - "velocityX": 0.5910423737502203, - "velocityY": -0.04974532887286408, - "timestamp": 8.271382008420531 - }, - { - "x": 7.921731862807926, - "y": 7.4739282664160145, - "heading": -0.005439516570392937, - "angularVelocity": 0.0598199119499377, - "velocityX": 0.31112061476836195, - "velocityY": -0.08298006744976984, - "timestamp": 8.343644903564469 - }, - { - "x": 7.923961639404313, - "y": 7.465639114379864, - "heading": -8.923477228591696e-17, - "angularVelocity": 0.07527399171536123, - "velocityX": 0.030856452567537763, - "velocityY": -0.11470827482980998, - "timestamp": 8.415907798708407 - }, - { - "x": 7.888861809774048, - "y": 7.450123409205229, - "heading": 0.009559512590155543, - "angularVelocity": 0.09629823646465466, - "velocityX": -0.3535799196587898, - "velocityY": -0.15629824551531185, - "timestamp": 8.515177660142104 - }, - { - "x": 7.817152523236329, - "y": 7.430666435185926, - "heading": 0.02163779335227315, - "angularVelocity": 0.12167117579976462, - "velocityX": -0.72236714650414, - "velocityY": -0.19600081775371833, - "timestamp": 8.614447521575801 - }, - { - "x": 7.711448594725456, - "y": 7.4075722780062225, - "heading": 0.036035719670363044, - "angularVelocity": 0.14503824333134957, - "velocityX": -1.064813902067085, - "velocityY": -0.23264016737978235, - "timestamp": 8.713717383009499 - }, - { - "x": 7.574048351882708, - "y": 7.3811128444264495, - "heading": 0.05252610728763101, - "angularVelocity": 0.16611675869298972, - "velocityX": -1.3841083371967766, - "velocityY": -0.2665404504210516, - "timestamp": 8.812987244443196 - }, - { - "x": 7.407019371926434, - "y": 7.351538272778396, - "heading": 0.07089974064841498, - "angularVelocity": 0.18508773050978647, - "velocityX": -1.682574928019174, - "velocityY": -0.2979209522500118, - "timestamp": 8.912257105876893 - }, - { - "x": 7.212241390572204, - "y": 7.319083353098022, - "heading": 0.09094586321763254, - "angularVelocity": 0.20193563564714437, - "velocityX": -1.96210590546985, - "velocityY": -0.3269362847056093, - "timestamp": 9.01152696731059 - }, - { - "x": 6.990210935369373, - "y": 7.283848196853213, - "heading": 0.11243058039021528, - "angularVelocity": 0.216427391579795, - "velocityX": -2.236635087388812, - "velocityY": -0.35494313919580667, - "timestamp": 9.110796828744288 - }, - { - "x": 6.744250267253016, - "y": 7.2462331722899656, - "heading": 0.1352094476478159, - "angularVelocity": 0.2294640783075409, - "velocityX": -2.4776973047417434, - "velocityY": -0.3789168637892277, - "timestamp": 9.210066690177985 - }, - { - "x": 6.474537469267562, - "y": 7.206343597573894, - "heading": 0.1591116816121769, - "angularVelocity": 0.24078036998495714, - "velocityX": -2.7169655934857664, - "velocityY": -0.401829660482739, - "timestamp": 9.309336551611683 - }, - { - "x": 6.185093329086759, - "y": 7.164701733979632, - "heading": 0.18384126922195682, - "angularVelocity": 0.24911475902781344, - "velocityX": -2.915730273020737, - "velocityY": -0.4194814316536095, - "timestamp": 9.40860641304538 - }, - { - "x": 5.8780260788239564, - "y": 7.121745615538567, - "heading": 0.2121770427035368, - "angularVelocity": 0.28544185589002363, - "velocityX": -3.0932575690950848, - "velocityY": -0.43272064472210836, - "timestamp": 9.507876274479077 - }, - { - "x": 5.557966335758099, - "y": 7.0784922097584015, - "heading": 0.24028820463072503, - "angularVelocity": 0.283179219968629, - "velocityX": -3.224138106404308, - "velocityY": -0.4357153838585266, - "timestamp": 9.607146135912775 - }, - { - "x": 5.245908943265439, - "y": 7.038535334063528, - "heading": 0.26569993334648234, - "angularVelocity": 0.25598634216619676, - "velocityX": -3.1435260207458486, - "velocityY": -0.40250762031696863, - "timestamp": 9.706415997346472 - }, - { - "x": 4.948601311707987, - "y": 7.0011016986080925, - "heading": 0.2891526852975207, - "angularVelocity": 0.23625249005411966, - "velocityX": -2.9949435534976283, - "velocityY": -0.37708963138260865, - "timestamp": 9.80568585878017 - }, - { - "x": 4.669544920179785, - "y": 6.966326786089845, - "heading": 0.31063199175620987, - "angularVelocity": 0.21637288647809105, - "velocityX": -2.811088758440401, - "velocityY": -0.35030685059909655, - "timestamp": 9.904955720213866 - }, - { - "x": 4.411446504016625, - "y": 6.934401644375056, - "heading": 0.3300269329519647, - "angularVelocity": 0.19537592694947853, - "velocityX": -2.599967527259491, - "velocityY": -0.3215995394142002, - "timestamp": 10.004225581647564 - }, - { - "x": 4.176717541970206, - "y": 6.9055345106659285, - "heading": 0.3471896751796373, - "angularVelocity": 0.17288975707028345, - "velocityX": -2.364554142177353, - "velocityY": -0.2907945401778102, - "timestamp": 10.103495443081261 - }, - { - "x": 3.9677270788540704, - "y": 6.879953869869358, - "heading": 0.3619526945831296, - "angularVelocity": 0.14871602710307644, - "velocityX": -2.1052760636290624, - "velocityY": -0.2576878866065069, - "timestamp": 10.202765304514958 - }, - { - "x": 3.7869910923072463, - "y": 6.857920125982094, - "heading": 0.37415675707125545, - "angularVelocity": 0.1229382444164791, - "velocityX": -1.8206531563211636, - "velocityY": -0.22195804012459744, - "timestamp": 10.302035165948656 - }, - { - "x": 3.6375222511124736, - "y": 6.83976261955988, - "heading": 0.3836657545916857, - "angularVelocity": 0.09578937033957077, - "velocityX": -1.5056819767458316, - "velocityY": -0.18291056479757375, - "timestamp": 10.401305027382353 - }, - { - "x": 3.5230866973435258, - "y": 6.825906628204701, - "heading": 0.39040681994356247, - "angularVelocity": 0.06790646480733925, - "velocityX": -1.1527723733691484, - "velocityY": -0.1395790339088257, - "timestamp": 10.50057488881605 - }, - { - "x": 3.4467104911360567, - "y": 6.816686862116541, - "heading": 0.39477159352312596, - "angularVelocity": 0.04396876873328538, - "velocityX": -0.7693795992500979, - "velocityY": -0.09287578279051523, - "timestamp": 10.599844750249748 - }, - { - "x": 3.4085204601287793, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": 0.021852252491790684, - "velocityX": -0.3847092204594867, - "velocityY": -0.046321940190690435, - "timestamp": 10.699114611683445 - }, - { - "x": 3.408520460128781, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": -9.258859562419926e-18, - "velocityX": 7.880837628913899e-16, - "velocityY": -2.4194099006901583e-16, - "timestamp": 10.798384473117142 - }, - { - "x": 3.4451800140480633, - "y": 6.811382414928076, - "heading": 0.39378662700224637, - "angularVelocity": -0.032542531675579224, - "velocityX": 0.37821978715244237, - "velocityY": -0.00728463273656313, - "timestamp": 10.895311062271263 - }, - { - "x": 3.518496947445585, - "y": 6.809970351527808, - "heading": 0.38746820532810716, - "angularVelocity": -0.06518770266528738, - "velocityX": 0.756417140408642, - "velocityY": -0.014568380179178244, - "timestamp": 10.992237651425384 - }, - { - "x": 3.6283397734394334, - "y": 6.8078551393922675, - "heading": 0.37750274880248313, - "angularVelocity": -0.1028144765290181, - "velocityX": 1.1332579321365746, - "velocityY": -0.021822826471036318, - "timestamp": 11.089164240579505 - }, - { - "x": 3.7707535238798844, - "y": 6.805113681812527, - "heading": 0.36197971834397535, - "angularVelocity": -0.16015244726939834, - "velocityX": 1.469294975540766, - "velocityY": -0.028283854860319504, - "timestamp": 11.186090829733626 - }, - { - "x": 3.9413941020263046, - "y": 6.801829282896055, - "heading": 0.341973429419134, - "angularVelocity": -0.20640661246244818, - "velocityX": 1.7605135973070385, - "velocityY": -0.03388542757086655, - "timestamp": 11.283017418887747 - }, - { - "x": 4.137139185749371, - "y": 6.798061838763412, - "heading": 0.3182906699990001, - "angularVelocity": -0.24433707640817395, - "velocityX": 2.0195189517276724, - "velocityY": -0.038869046827304486, - "timestamp": 11.379944008041868 - }, - { - "x": 4.3554217491364975, - "y": 6.793860650651487, - "heading": 0.2915337558008742, - "angularVelocity": -0.2760533970258723, - "velocityX": 2.2520400778783602, - "velocityY": -0.043344020960483176, - "timestamp": 11.476870597195989 - }, - { - "x": 4.593770534553052, - "y": 6.7892732393783435, - "heading": 0.26218341148270125, - "angularVelocity": -0.30281003978695553, - "velocityX": 2.459065025362269, - "velocityY": -0.04732871870534226, - "timestamp": 11.57379718635011 - }, - { - "x": 4.849438377602218, - "y": 6.784352472846038, - "heading": 0.23067476418411942, - "angularVelocity": -0.32507743822988416, - "velocityX": 2.6377472402607576, - "velocityY": -0.05076797373402456, - "timestamp": 11.67072377550423 - }, - { - "x": 5.118739979240287, - "y": 6.77916928922838, - "heading": 0.197506319622179, - "angularVelocity": -0.3422017100921637, - "velocityX": 2.7784079063161986, - "velocityY": -0.053475353490631775, - "timestamp": 11.767650364658351 - }, - { - "x": 5.388165163887155, - "y": 6.773984054928517, - "heading": 0.16458046580315022, - "angularVelocity": -0.33969888042458984, - "velocityX": 2.779682922902244, - "velocityY": -0.053496510556234986, - "timestamp": 11.864576953812472 - }, - { - "x": 5.643920085833601, - "y": 6.769061952936388, - "heading": 0.1333147392066983, - "angularVelocity": -0.32257120434452946, - "velocityX": 2.6386456407723093, - "velocityY": -0.05078175178849002, - "timestamp": 11.961503542966593 - }, - { - "x": 5.882330711464664, - "y": 6.764473684698037, - "heading": 0.10420435480854875, - "angularVelocity": -0.3003343525465651, - "velocityX": 2.4597030362017005, - "velocityY": -0.04733756008947892, - "timestamp": 12.058430132120714 - }, - { - "x": 6.100655045491889, - "y": 6.760272005917349, - "heading": 0.07768097943341207, - "angularVelocity": -0.27364395679871373, - "velocityX": 2.252471029183505, - "velocityY": -0.04334908323253468, - "timestamp": 12.155356721274835 - }, - { - "x": 6.29642574105686, - "y": 6.756504387895065, - "heading": 0.054221932212767415, - "angularVelocity": -0.2420290183052161, - "velocityX": 2.019783191314849, - "velocityY": -0.038870840861772844, - "timestamp": 12.252283310428956 - }, - { - "x": 6.467079365349485, - "y": 6.7532201591852425, - "heading": 0.03442675668029697, - "angularVelocity": -0.20422853734174792, - "velocityX": 1.7606481955253344, - "velocityY": -0.03388367153413224, - "timestamp": 12.349209899583077 - }, - { - "x": 6.60949406591357, - "y": 6.750479406444339, - "heading": 0.019100936295010484, - "angularVelocity": -0.15811781389436272, - "velocityX": 1.4693047780484314, - "velocityY": -0.028276582977095337, - "timestamp": 12.446136488737197 - }, - { - "x": 6.719322188320328, - "y": 6.748365796127614, - "heading": 0.009318208717894495, - "angularVelocity": -0.10092924616960162, - "velocityX": 1.1331062339573879, - "velocityY": -0.021806300367839485, - "timestamp": 12.543063077891318 - }, - { - "x": 6.792644320795612, - "y": 6.746954731447967, - "heading": 0.0031061365662838365, - "angularVelocity": -0.06409048544701255, - "velocityX": 0.756470779743405, - "velocityY": -0.014558076292195965, - "timestamp": 12.63998966704544 - }, - { - "x": 6.829305171966555, - "y": 6.746249198913574, - "heading": -6.996174209709525e-17, - "angularVelocity": -0.03204627949246174, - "velocityX": 0.3782331710099824, - "velocityY": -0.007279040153471663, - "timestamp": 12.73691625619956 - }, - { - "x": 6.8293051719665545, - "y": 6.746249198913574, - "heading": -3.495236266448189e-17, - "angularVelocity": 5.858001175456844e-19, - "velocityX": -7.463612085764823e-16, - "velocityY": 1.4437890171198592e-17, - "timestamp": 12.833842845353681 - } - ], "constraints": [ { "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 2 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 3 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 5 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 4, 5 ], - "type": "StraightLine" + "type": "StraightLine", + "direction": 0 }, { "scope": [ 8 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "SourceLanePGHSprint": { "waypoints": [ @@ -14604,7 +14637,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 19 }, { "x": 2.3639590740203857, @@ -14613,7 +14646,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 22 }, { "x": 5.5030035972595215, @@ -14622,7 +14655,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 16 + "controlIntervalCount": 17 }, { "x": 7.626483917236328, @@ -14631,7 +14664,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 9 }, { "x": 8.306674003601074, @@ -14640,7 +14673,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 19 }, { "x": 5.5030035972595215, @@ -14649,7 +14682,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 21 + "controlIntervalCount": 22 }, { "x": 2.3639590740203857, @@ -14658,7 +14691,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 21 }, { "x": 5.5030035972595215, @@ -14667,7 +14700,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 14 }, { "x": 7.536985397338867, @@ -14676,7 +14709,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { "x": 8.385421752929688, @@ -14685,7 +14718,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 19 }, { "x": 5.5030035972595215, @@ -14694,7 +14727,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 21 + "controlIntervalCount": 22 }, { "x": 2.37211537361145, @@ -14703,7 +14736,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 22 }, { "x": 5.5030035972595215, @@ -14712,7 +14745,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { "x": 8.063037872314453, @@ -14728,1964 +14761,2255 @@ { "x": 0.433241993188858, "y": 4.121134281158447, - "heading": 8.991694014476798e-34, - "angularVelocity": 0, - "velocityX": -4.6784915292932193e-35, - "velocityY": -2.119592179507727e-34, + "heading": -1.13547667502199e-28, + "angularVelocity": -4.993890068315335e-29, + "velocityX": 1.9809218936022493e-27, + "velocityY": 1.6056222243913227e-27, "timestamp": 0 }, { - "x": 0.4570749914043126, - "y": 4.107894224868064, - "heading": -0.009932712149027413, - "angularVelocity": -0.1373640763265399, - "velocityX": 0.3295975698116412, - "velocityY": -0.18310287014791135, - "timestamp": 0.07230938695656872 - }, - { - "x": 0.5047410210186508, - "y": 4.0814144720383245, - "heading": -0.029797898632204542, - "angularVelocity": -0.27472486380099975, - "velocityX": 0.6591955985322896, - "velocityY": -0.36620076513225003, - "timestamp": 0.14461877391313743 - }, - { - "x": 0.576240240509544, - "y": 4.041695512105355, - "heading": -0.059590394556189516, - "angularVelocity": -0.41201422357348, - "velocityX": 0.9887958189140486, - "velocityY": -0.5492918914777362, - "timestamp": 0.21692816086970615 - }, - { - "x": 0.6715729183897807, - "y": 3.9887379727274364, - "heading": -0.09930156463679939, - "angularVelocity": -0.5491841620018386, - "velocityX": 1.3183997526835154, - "velocityY": -0.7323743376461849, - "timestamp": 0.28923754782627487 - }, - { - "x": 0.7907394144382421, - "y": 3.9225426248725555, - "heading": -0.148921029894937, - "angularVelocity": -0.686210564721571, - "velocityX": 1.648008662001748, - "velocityY": -0.9154461217412811, - "timestamp": 0.3615469347828436 - }, - { - "x": 0.9337401624459083, - "y": 3.8431103831358566, - "heading": -0.20843840181811515, - "angularVelocity": -0.8230932998910107, - "velocityX": 1.9776235704163385, - "velocityY": -1.0985052574765801, - "timestamp": 0.43385632173941235 - }, - { - "x": 1.100575665552893, - "y": 3.7504423098204516, - "heading": -0.2778446966612386, - "angularVelocity": -0.9598517946889951, - "velocityX": 2.307245436988583, - "velocityY": -1.2815497021301627, - "timestamp": 0.5061657086959811 - }, - { - "x": 1.2912465471408558, - "y": 3.6445396941775194, - "heading": -0.3571330045970933, - "angularVelocity": -1.0965147302863703, - "velocityX": 2.636875924594487, - "velocityY": -1.464576317132112, - "timestamp": 0.5784750956525498 - }, - { - "x": 1.505785276048115, - "y": 3.525464237325283, - "heading": -0.4461366707355329, - "angularVelocity": -1.2308729182270346, - "velocityX": 2.966955438802405, - "velocityY": -1.6467496387952614, - "timestamp": 0.6507844826091186 - }, - { - "x": 1.6964880033537257, - "y": 3.4196219693446848, - "heading": -0.5252603846379951, - "angularVelocity": -1.094238483172127, - "velocityX": 2.637316333772171, - "velocityY": -1.463741741361608, - "timestamp": 0.7230938695656873 - }, - { - "x": 1.8633540969095699, - "y": 3.3270118414974243, - "heading": -0.5945029830335408, - "angularVelocity": -0.9575879607046729, - "velocityX": 2.3076684864728434, - "velocityY": -1.280748347415596, - "timestamp": 0.7954032565222561 - }, - { - "x": 2.0063830653266717, - "y": 3.247633047534605, - "heading": -0.6538614517041024, - "angularVelocity": -0.8208957532196227, - "velocityX": 1.9780138435278007, - "velocityY": -1.0977661034589712, - "timestamp": 0.8677126434788248 - }, - { - "x": 2.125574496423577, - "y": 3.1814849244321213, - "heading": -0.7033318821965574, - "angularVelocity": -0.6841494939262661, - "velocityX": 1.648353500334547, - "velocityY": -0.9147930287697538, - "timestamp": 0.9400220304353936 - }, - { - "x": 2.220928049802523, - "y": 3.128566939134693, - "heading": -0.7429103365332542, - "angularVelocity": -0.5473487745162161, - "velocityX": 1.318688449622979, - "velocityY": -0.7318273259488806, - "timestamp": 1.0123314173919622 - }, - { - "x": 2.292443455008997, - "y": 3.0888786919590703, - "heading": -0.7725937492005902, - "angularVelocity": -0.4105056606988637, - "velocityX": 0.989019658670723, - "velocityY": -0.5488671505327598, - "timestamp": 1.084640804348531 - }, - { - "x": 2.34012050914709, - "y": 3.0624199236271457, - "heading": -0.7923807693651485, - "angularVelocity": -0.273643865580592, - "velocityX": 0.6593480617769434, - "velocityY": -0.3659105607936737, - "timestamp": 1.1569501913050997 + "x": 0.4546918803218126, + "y": 4.109218526998405, + "heading": -0.008937300145056587, + "angularVelocity": -0.11893237232009286, + "velocityX": 0.28544257452644645, + "velocityY": -0.15856789939193544, + "timestamp": 0.07514606806129172 + }, + { + "x": 0.4975916635129914, + "y": 4.085387310900469, + "heading": -0.02681241329368555, + "angularVelocity": -0.2378715694618833, + "velocityX": 0.5708852678251674, + "velocityY": -0.31713191006213676, + "timestamp": 0.15029213612258344 + }, + { + "x": 0.5619414537509306, + "y": 4.049641027145361, + "heading": -0.05362171709181763, + "angularVelocity": -0.3567625624306182, + "velocityX": 0.8563294380945293, + "velocityY": -0.47569067387467845, + "timestamp": 0.22543820418387517 + }, + { + "x": 0.6477414523823173, + "y": 4.001980177868811, + "heading": -0.08935853103818607, + "angularVelocity": -0.47556465518888186, + "velocityX": 1.1417762877680475, + "velocityY": -0.6342427555580988, + "timestamp": 0.3005842722451669 + }, + { + "x": 0.7549919358856847, + "y": 3.942405375866793, + "heading": -0.1340144219891079, + "angularVelocity": -0.5942545245946725, + "velocityX": 1.4272268166564648, + "velocityY": -0.7927866825104751, + "timestamp": 0.3757303403064586 + }, + { + "x": 0.8836932393387834, + "y": 3.870917341642709, + "heading": -0.1875805715546105, + "angularVelocity": -0.7128270440152931, + "velocityX": 1.7126818045639527, + "velocityY": -0.9513210214242486, + "timestamp": 0.4508764083677503 + }, + { + "x": 1.0338457421573333, + "y": 3.7875168933266514, + "heading": -0.2500489994644535, + "angularVelocity": -0.8312933666588067, + "velocityX": 1.998141841514319, + "velocityY": -1.1098444731404116, + "timestamp": 0.526022476429042 + }, + { + "x": 1.2054498603433927, + "y": 3.692204931210594, + "heading": -0.32141344956061546, + "angularVelocity": -0.9496764360040055, + "velocityX": 2.2836074143771987, + "velocityY": -1.2683559442966215, + "timestamp": 0.6011685444903337 + }, + { + "x": 1.3985060453585743, + "y": 3.5849824250394744, + "heading": -0.4016698579067676, + "angularVelocity": -1.0680054248572561, + "velocityX": 2.5690789950276347, + "velocityY": -1.4268545106533708, + "timestamp": 0.6763146125516255 + }, + { + "x": 1.5915937570064054, + "y": 3.477819815501255, + "heading": -0.48176859261605665, + "angularVelocity": -1.0659071961550632, + "velocityX": 2.5694985330482245, + "velocityY": -1.426057441233171, + "timestamp": 0.7514606806129173 + }, + { + "x": 1.7632284859071354, + "y": 3.3825660365490977, + "heading": -0.5529765476421505, + "angularVelocity": -0.9475938909806194, + "velocityX": 2.2840147638960824, + "velocityY": -1.2675816767214638, + "timestamp": 0.826606748674209 + }, + { + "x": 1.9134098098681451, + "y": 3.2992204007592485, + "heading": -0.6152918417750989, + "angularVelocity": -0.8292555517624935, + "velocityX": 1.9985253764510553, + "velocityY": -1.1091150600437196, + "timestamp": 0.9017528167355008 + }, + { + "x": 2.042137357772617, + "y": 3.227782305964734, + "heading": -0.6687115003563293, + "angularVelocity": -0.7108776275248302, + "velocityX": 1.7130310504001012, + "velocityY": -0.95065645665249, + "timestamp": 0.9768988847967925 + }, + { + "x": 2.1494108084908032, + "y": 3.168251239438324, + "heading": -0.7132320417999736, + "angularVelocity": -0.5924533723751417, + "velocityX": 1.4275324509419445, + "velocityY": -0.7922046763358841, + "timestamp": 1.0520449528580842 + }, + { + "x": 2.2352298936741635, + "y": 3.120626784366874, + "heading": -0.7488501306754739, + "angularVelocity": -0.47398473126297086, + "velocityX": 1.1420302804580957, + "velocityY": -0.6337584427252518, + "timestamp": 1.1271910209193758 + }, + { + "x": 2.299594398786613, + "y": 3.0849086278606843, + "heading": -0.7755632624576652, + "angularVelocity": -0.3554827613921603, + "velocityX": 0.8565252550532846, + "velocityY": -0.475316372868058, + "timestamp": 1.2023370889806675 + }, + { + "x": 2.3425041622841327, + "y": 3.061096568391248, + "heading": -0.7933703919070813, + "angularVelocity": -0.23696688208479472, + "velocityX": 0.5710180799149875, + "velocityY": -0.31687698483458104, + "timestamp": 1.2774831570419591 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288571271456, + "velocityX": 0.28550943901354336, + "velocityY": -0.15843872418317836, + "timestamp": 1.3526292251032508 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.4441811570154594e-25, + "velocityX": -6.270976814566572e-25, + "velocityY": -1.3160668498175877e-24, + "timestamp": 1.4277752931645424 + }, + { + "x": 2.3773546784477104, + "y": 3.0407954087369538, + "heading": -0.78607704068416, + "angularVelocity": 0.26500042843772503, + "velocityX": 0.21918860936679116, + "velocityY": -0.13736692846187587, + "timestamp": 1.4888898030198798 + }, + { + "x": 2.4041715280717404, + "y": 3.023997545091338, + "heading": -0.7541197912958612, + "angularVelocity": 0.5229077262329992, + "velocityX": 0.4387967716260398, + "velocityY": -0.27485884588418563, + "timestamp": 1.5500043128752172 + }, + { + "x": 2.444438816560299, + "y": 2.9987884456278273, + "heading": -0.7069088629247025, + "angularVelocity": 0.772499501066286, + "velocityX": 0.6588826218826678, + "velocityY": -0.41248959573074695, + "timestamp": 1.6111188227305546 + }, + { + "x": 2.498190797810618, + "y": 2.965157151416224, + "heading": -0.6450804742614368, + "angularVelocity": 1.0116810035721142, + "velocityX": 0.8795289592856741, + "velocityY": -0.5502996635530664, + "timestamp": 1.672233332585892 + }, + { + "x": 2.565467715954461, + "y": 2.92308800675928, + "heading": -0.5694581744936127, + "angularVelocity": 1.2373869960967963, + "velocityX": 1.1008338004034173, + "velocityY": -0.6883659012650978, + "timestamp": 1.7333478424412294 + }, + { + "x": 2.6463157744818333, + "y": 2.8725583790255045, + "heading": -0.4811095208345568, + "angularVelocity": 1.4456248420904245, + "velocityX": 1.3228946565839355, + "velocityY": -0.8268024705325063, + "timestamp": 1.7944623522965668 + }, + { + "x": 2.7407862852871125, + "y": 2.8135375953698065, + "heading": -0.3813947607246218, + "angularVelocity": 1.6316053314665737, + "velocityX": 1.5457951152500125, + "velocityY": -0.9657409311700984, + "timestamp": 1.8555768621519042 + }, + { + "x": 2.8489350075595548, + "y": 2.74598863201195, + "heading": -0.27202744081575525, + "angularVelocity": 1.7895475259107383, + "velocityX": 1.7696079462706686, + "velocityY": -1.1052852017916852, + "timestamp": 1.9166913720072416 + }, + { + "x": 2.97082263377999, + "y": 2.6698730886283255, + "heading": -0.15523118772045078, + "angularVelocity": 1.9111051266183738, + "velocityX": 1.994413871745882, + "velocityY": -1.2454578063997521, + "timestamp": 1.977805881862579 + }, + { + "x": 3.1065122610280413, + "y": 2.5851587283166806, + "heading": -0.034196755068831956, + "angularVelocity": 1.9804532988666075, + "velocityX": 2.220252237467639, + "velocityY": -1.3861578946173312, + "timestamp": 2.0389203917179164 + }, + { + "x": 3.2560356540344637, + "y": 2.4918371862270576, + "heading": 0.08572827567170235, + "angularVelocity": 1.9623004589974744, + "velocityX": 2.4466103607859297, + "velocityY": -1.5269948545856247, + "timestamp": 2.100034901573254 + }, + { + "x": 3.419180856935893, + "y": 2.390029020511174, + "heading": 0.1941501841545546, + "angularVelocity": 1.7740780174707271, + "velocityX": 2.669500308316405, + "velocityY": -1.6658591545096526, + "timestamp": 2.161149411428591 + }, + { + "x": 3.5929644614197396, + "y": 2.2794553379158704, + "heading": 0.2670576621771365, + "angularVelocity": 1.1929651108248978, + "velocityX": 2.843573562075607, + "velocityY": -1.8092869084124177, + "timestamp": 2.2222639212839286 + }, + { + "x": 3.7768655447565394, + "y": 2.1600020217160374, + "heading": 0.3015223680601122, + "angularVelocity": 0.5639365506580378, + "velocityX": 3.009123099769713, + "velocityY": -1.9545819230586587, + "timestamp": 2.283378431139266 + }, + { + "x": 3.972896711212453, + "y": 2.037552262177189, + "heading": 0.301522415266864, + "angularVelocity": 7.724311622068364e-7, + "velocityX": 3.2076043302962565, + "velocityY": -2.003611905400141, + "timestamp": 2.3444929409946034 + }, + { + "x": 4.174386731425729, + "y": 1.9243094542990702, + "heading": 0.30152246246678105, + "angularVelocity": 7.723193244877141e-7, + "velocityX": 3.29692606044323, + "velocityY": -1.8529610749750456, + "timestamp": 2.4056074508499408 + }, + { + "x": 4.383354455500062, + "y": 1.8255438133936408, + "heading": 0.30152251049528017, + "angularVelocity": 7.858771882784632e-7, + "velocityX": 3.419281682352931, + "velocityY": -1.6160751536617886, + "timestamp": 2.466721960705278 + }, + { + "x": 4.5987615422573755, + "y": 1.741746749013425, + "heading": 0.30152256055333204, + "angularVelocity": 8.190862036393596e-7, + "velocityX": 3.52464721172105, + "velocityY": -1.3711484323210577, + "timestamp": 2.5278364705606156 + }, + { + "x": 4.819537392304322, + "y": 1.6733348767579859, + "heading": 0.30152261408094844, + "angularVelocity": 8.758577384965909e-7, + "velocityX": 3.61249481619895, + "velocityY": -1.1194047439368322, + "timestamp": 2.588950980415953 + }, + { + "x": 5.0445846783405734, + "y": 1.620648273878739, + "heading": 0.30152267296096713, + "angularVelocity": 9.634376323324065e-7, + "velocityX": 3.682387154359157, + "velocityY": -0.8620964645541634, + "timestamp": 2.6500654902712903 + }, + { + "x": 5.272784828780489, + "y": 1.5839488354188542, + "heading": 0.3015227398607896, + "angularVelocity": 0.0000010946634868711088, + "velocityX": 3.733976611774902, + "velocityY": -0.6005028682510115, + "timestamp": 2.7111800001266277 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.30152281374195866, + "angularVelocity": 0.0000012088973518937203, + "velocityX": 3.7670067063284516, + "velocityY": -0.33592433375155795, + "timestamp": 2.772294509981965 + }, + { + "x": 5.642161233112151, + "y": 1.5569512201034212, + "heading": 0.3015228937182471, + "angularVelocity": 0.0000021712104790563405, + "velocityX": 3.7778762073788132, + "velocityY": -0.17558801391627427, + "timestamp": 2.8091293950374445 + }, + { + "x": 5.781468008752501, + "y": 1.5564011001804379, + "heading": 0.3015229674749775, + "angularVelocity": 0.0000020023608117977538, + "velocityX": 3.7819250808175866, + "velocityY": -0.01493475335010743, + "timestamp": 2.845964280092924 + }, + { + "x": 5.920672391044722, + "y": 1.5617696155329066, + "heading": 0.3015230362484881, + "angularVelocity": 0.00000186707547817187, + "velocityX": 3.7791452880212755, + "velocityY": 0.14574540803868186, + "timestamp": 2.8827991651484033 + }, + { + "x": 6.059523032201408, + "y": 1.573047069761596, + "heading": 0.3015231010202909, + "angularVelocity": 0.0000017584364021609749, + "velocityX": 3.769541860862453, + "velocityY": 0.3061623298594123, + "timestamp": 2.9196340502038827 + }, + { + "x": 6.197769223699856, + "y": 1.5902130966223555, + "heading": 0.3015231625861422, + "angularVelocity": 0.0000016714006640348176, + "velocityX": 3.753132154212656, + "velocityY": 0.46602634526766634, + "timestamp": 2.956468935259362 + }, + { + "x": 6.335161349065868, + "y": 1.6132366967697473, + "heading": 0.30152322160724626, + "angularVelocity": 0.0000016023154129591626, + "velocityX": 3.729945815198736, + "velocityY": 0.6250487849416221, + "timestamp": 2.9933038203148414 + }, + { + "x": 6.471451334685286, + "y": 1.642076293800604, + "heading": 0.30152327864702066, + "angularVelocity": 0.0000015485259219178396, + "velocityX": 3.700024729658922, + "velocityY": 0.7829425010399578, + "timestamp": 3.030138705370321 + }, + { + "x": 6.606393097769013, + "y": 1.6766798096014064, + "heading": 0.30152333422462596, + "angularVelocity": 0.0000015088306981556567, + "velocityX": 3.6634229448655047, + "velocityY": 0.9394223912653392, + "timestamp": 3.0669735904258 + }, + { + "x": 6.739742462814537, + "y": 1.7169846115821055, + "heading": 0.301525563266411, + "angularVelocity": 0.000060514422176811634, + "velocityX": 3.62019224017336, + "velocityY": 1.094201920814829, + "timestamp": 3.1038084754812796 + }, + { + "x": 6.86942078984101, + "y": 1.7614703146147381, + "heading": 0.31012052274281793, + "angularVelocity": 0.23333748601255083, + "velocityX": 3.520530248191535, + "velocityY": 1.2077057649461913, + "timestamp": 3.140643360536759 + }, + { + "x": 6.994764102260858, + "y": 1.8086931054779125, + "heading": 0.330618059674586, + "angularVelocity": 0.5564707722284291, + "velocityX": 3.4028425019125397, + "velocityY": 1.2820127113753435, + "timestamp": 3.1774782455922383 + }, + { + "x": 7.114597005035733, + "y": 1.8565129182538003, + "heading": 0.35962770978990066, + "angularVelocity": 0.7875591323719712, + "velocityX": 3.2532449224257225, + "velocityY": 1.2982207682707054, + "timestamp": 3.2143131306477177 + }, + { + "x": 7.228563598521064, + "y": 1.9039637598434371, + "heading": 0.3914629491729737, + "angularVelocity": 0.8642687315332709, + "velocityX": 3.093985316193586, + "velocityY": 1.2882038729907241, + "timestamp": 3.251148015703197 + }, + { + "x": 7.3366766140353254, + "y": 1.9505339209655184, + "heading": 0.42341393018553436, + "angularVelocity": 0.8674109058420351, + "velocityX": 2.935071342055907, + "velocityY": 1.2642950032812363, + "timestamp": 3.2879829007586765 + }, + { + "x": 7.4389964644321305, + "y": 1.9959395099807475, + "heading": 0.45395278398085526, + "angularVelocity": 0.8290742254068196, + "velocityX": 2.777797466795224, + "velocityY": 1.232678992939458, + "timestamp": 3.324817785814156 + }, + { + "x": 7.535582073900071, + "y": 2.0400031621127974, + "heading": 0.48208291398817593, + "angularVelocity": 0.7636817643098928, + "velocityX": 2.6221232758692414, + "velocityY": 1.1962478521565303, + "timestamp": 3.3616526708696353 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6791276901362222, + "velocityX": 2.467819383699554, + "velocityY": 1.1565397222936757, + "timestamp": 3.3984875559251146 + }, + { + "x": 7.766333854980875, + "y": 2.1513911603861824, + "heading": 0.5393931722207971, + "angularVelocity": 0.5089507410927826, + "velocityX": 2.2039777536941085, + "velocityY": 1.084054804314438, + "timestamp": 3.4619409812195143 + }, + { + "x": 7.889716821195031, + "y": 2.214535023307618, + "heading": 0.562169089136497, + "angularVelocity": 0.35893912440547393, + "velocityX": 1.9444650252008764, + "velocityY": 0.9951214237603884, + "timestamp": 3.525394406513914 + }, + { + "x": 7.996856912790919, + "y": 2.2713427786511677, + "heading": 0.5763089078520277, + "angularVelocity": 0.22283775304370615, + "velocityX": 1.6884839722804246, + "velocityY": 0.8952669628153768, + "timestamp": 3.5888478318083137 + }, + { + "x": 8.087934078342826, + "y": 2.32132355324964, + "heading": 0.5824533262443143, + "angularVelocity": 0.09683351787202944, + "velocityX": 1.4353388352062089, + "velocityY": 0.7876765417561041, + "timestamp": 3.6523012571027134 + }, + { + "x": 8.163093938645574, + "y": 2.3641124623938485, + "heading": 0.5810872040851076, + "angularVelocity": -0.021529525835184894, + "velocityX": 1.184488622230158, + "velocityY": 0.6743356870915145, + "timestamp": 3.715754682397113 + }, + { + "x": 8.222455939868006, + "y": 2.399428067413051, + "heading": 0.5725898823037127, + "angularVelocity": -0.13391431182746497, + "velocityX": 0.9355208319647886, + "velocityY": 0.5565594742183169, + "timestamp": 3.7792081076915127 + }, + { + "x": 8.2661194627332, + "y": 2.427046891477821, + "heading": 0.5572661390575091, + "angularVelocity": -0.24149591885871216, + "velocityX": 0.6881192412011335, + "velocityY": 0.4352613580217183, + "timestamp": 3.8426615329859124 + }, + { + "x": 8.294168308225482, + "y": 2.446787281552408, + "heading": 0.535366098523775, + "angularVelocity": -0.3451356712758413, + "velocityX": 0.44203831963592227, + "velocityY": 0.31110046436420713, + "timestamp": 3.906114958280312 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.4454857086167899, + "velocityX": 0.19708463834018525, + "velocityY": 0.18456741693022066, + "timestamp": 3.9695683835747118 + }, + { + "x": 8.303024530421867, + "y": 2.461881127507041, + "heading": 0.4711910316636515, + "angularVelocity": -0.5465136629689635, + "velocityX": -0.05554517774464489, + "velocityY": 0.05148047936095934, + "timestamp": 4.03527117412091 + }, + { + "x": 8.282750419128863, + "y": 2.4565585196128312, + "heading": 0.42887568027171213, + "angularVelocity": -0.644041920292346, + "velocityX": -0.3085730624903528, + "velocityY": -0.0810103779453237, + "timestamp": 4.100973964667109 + }, + { + "x": 8.245821395574854, + "y": 2.44257692607346, + "heading": 0.38041954518186105, + "angularVelocity": -0.7375049779016557, + "velocityX": -0.5620617213821911, + "velocityY": -0.2128006044056815, + "timestamp": 4.166676755213307 + }, + { + "x": 8.19220201373393, + "y": 2.41999124764574, + "heading": 0.3261364097301619, + "angularVelocity": -0.8261922362875965, + "velocityX": -0.8160898706916083, + "velocityY": -0.3437552383994849, + "timestamp": 4.232379545759505 + }, + { + "x": 8.12185021393602, + "y": 2.3888680897109964, + "heading": 0.26640039291910717, + "angularVelocity": -0.9091853833674209, + "velocityX": -1.070758170437869, + "velocityY": -0.4736961349131665, + "timestamp": 4.298082336305703 + }, + { + "x": 8.034715295170983, + "y": 2.349289919572908, + "heading": 0.2016664425434797, + "angularVelocity": -0.9852542005823995, + "velocityX": -1.3261981422808646, + "velocityY": -0.6023818746368115, + "timestamp": 4.3637851268519015 + }, + { + "x": 7.93073498478249, + "y": 2.3013613956679024, + "heading": 0.13250258081588168, + "angularVelocity": -1.0526776892218304, + "velocityX": -1.582585907296906, + "velocityY": -0.7294747073384187, + "timestamp": 4.4294879173981 + }, + { + "x": 7.809831081003324, + "y": 2.2452194823487295, + "heading": 0.0596429891895938, + "angularVelocity": -1.1089268967207304, + "velocityX": -1.8401639074089793, + "velocityY": -0.8544829352369329, + "timestamp": 4.495190707944298 + }, + { + "x": 7.671902779277316, + "y": 2.181050669281034, + "heading": -0.015920266023457082, + "angularVelocity": -1.1500768016834724, + "velocityX": -2.099276158278614, + "velocityY": -0.9766527804108355, + "timestamp": 4.560893498490496 + }, + { + "x": 7.516816184413173, + "y": 2.1091228610813926, + "heading": -0.09276871504476969, + "angularVelocity": -1.1696375204532188, + "velocityX": -2.360426301149195, + "velocityY": -1.09474510293541, + "timestamp": 4.626596289036694 + }, + { + "x": 7.34438786737911, + "y": 2.029851654552736, + "heading": -0.16871930466642973, + "angularVelocity": -1.1559720521802803, + "velocityX": -2.6243682437326825, + "velocityY": -1.2065120198040524, + "timestamp": 4.6922990795828925 + }, + { + "x": 7.154363406661781, + "y": 1.9439629810404226, + "heading": -0.24004853733783288, + "angularVelocity": -1.0856347512553308, + "velocityX": -2.892182495410387, + "velocityY": -1.307230222617737, + "timestamp": 4.758001870129091 + }, + { + "x": 6.946444181928406, + "y": 1.853016234879977, + "heading": -0.2992772561370167, + "angularVelocity": -0.9014642803875683, + "velocityX": -3.1645417645872604, + "velocityY": -1.3842143599136347, + "timestamp": 4.823704660675289 + }, + { + "x": 6.722123267905806, + "y": 1.762675189068849, + "heading": -0.3185532311699482, + "angularVelocity": -0.29338137501751665, + "velocityX": -3.4141763562518888, + "velocityY": -1.3749955680741657, + "timestamp": 4.889407451221487 + }, + { + "x": 6.485759743484643, + "y": 1.6860130566353695, + "heading": -0.3185533048985424, + "angularVelocity": -0.0000011221531618495692, + "velocityX": -3.597465533141494, + "velocityY": -1.166801771982201, + "timestamp": 4.955110241767685 + }, + { + "x": 6.244268768249415, + "y": 1.6274732084557726, + "heading": -0.31855333054380797, + "angularVelocity": -3.903223189148686e-7, + "velocityX": -3.675505609848113, + "velocityY": -0.890979632568803, + "timestamp": 5.0208130323138835 + }, + { + "x": 5.999037658090566, + "y": 1.5873919626414805, + "heading": -0.31855335901995885, + "angularVelocity": -4.334085455089385e-7, + "velocityX": -3.732430664210775, + "velocityY": -0.6100387134410885, + "timestamp": 5.086515822860082 + }, + { + "x": 5.751475205097474, + "y": 1.5659995874731991, + "heading": -0.31855339177165976, + "angularVelocity": -4.984826465003964e-7, + "velocityX": -3.7679138273285506, + "velocityY": -0.3255930987168572, + "timestamp": 5.15221861340628 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.31855343077931203, + "angularVelocity": -5.936985618685712e-7, + "velocityX": -3.781751212884059, + "velocityY": -0.03927691713851364, + "timestamp": 5.217921403952478 + }, + { + "x": 5.26265149115341, + "y": 1.5786031057335383, + "heading": -0.31855346389778566, + "angularVelocity": -5.200844258760406e-7, + "velocityX": -3.774430808061432, + "velocityY": 0.2384477350064305, + "timestamp": 5.28160043751504 + }, + { + "x": 5.0240625557725105, + "y": 1.6113905209820258, + "heading": -0.3185534914433618, + "angularVelocity": -4.3256900426079536e-7, + "velocityX": -3.7467424053553953, + "velocityY": 0.5148855661616695, + "timestamp": 5.345279471077602 + }, + { + "x": 4.788524292077237, + "y": 1.6616042903739598, + "heading": -0.31855351524845343, + "angularVelocity": -3.738293489638866e-7, + "velocityX": -3.698835401826056, + "velocityY": 0.7885447781270322, + "timestamp": 5.4089585046401645 + }, + { + "x": 4.557307734562994, + "y": 1.7289734302363957, + "heading": -0.31855353649931417, + "angularVelocity": -3.337183294186801e-7, + "velocityX": -3.63096838282074, + "velocityY": 1.0579485286353907, + "timestamp": 5.472637538202727 + }, + { + "x": 4.331660587990519, + "y": 1.8131343686528576, + "heading": -0.31855355601779706, + "angularVelocity": -3.06513492201463e-7, + "velocityX": -3.543507712798239, + "velocityY": 1.3216428345096847, + "timestamp": 5.536316571765289 + }, + { + "x": 4.1128004797347275, + "y": 1.9136328855171, + "heading": -0.31855357441317517, + "angularVelocity": -2.888765267316508e-7, + "velocityX": -3.4369257196840386, + "velocityY": 1.5782041787036154, + "timestamp": 5.599995605327851 + }, + { + "x": 3.9019083136875623, + "y": 2.0299264516040245, + "heading": -0.3185535921713155, + "angularVelocity": -2.7886950177034126e-7, + "velocityX": -3.3117991000911333, + "velocityY": 1.826245776369568, + "timestamp": 5.663674638890413 + }, + { + "x": 3.700120312567306, + "y": 2.1613848174245227, + "heading": -0.31855360971358787, + "angularVelocity": -2.7547956295773415e-7, + "velocityX": -3.1688295162647098, + "velocityY": 2.064390089892713, + "timestamp": 5.727353672452975 + }, + { + "x": 3.5092854089981618, + "y": 2.2879634490751353, + "heading": -0.3579155948331453, + "angularVelocity": -0.6181310066662056, + "velocityX": -2.9968247458036084, + "velocityY": 1.987759935556414, + "timestamp": 5.791032706015537 + }, + { + "x": 3.3329097587868497, + "y": 2.4050971281852065, + "heading": -0.4129449306411103, + "angularVelocity": -0.8641672577191554, + "velocityX": -2.7697601603521713, + "velocityY": 1.8394387062264796, + "timestamp": 5.854711739578099 + }, + { + "x": 3.171287728623564, + "y": 2.5124799864208414, + "heading": -0.469920470896881, + "angularVelocity": -0.8947299773291117, + "velocityX": -2.5380729122482584, + "velocityY": 1.6863141952387881, + "timestamp": 5.918390773140661 + }, + { + "x": 3.0244088088448944, + "y": 2.6100923711609356, + "heading": -0.525279921939494, + "angularVelocity": -0.8693513067880482, + "velocityX": -2.3065507053332985, + "velocityY": 1.5328810642861672, + "timestamp": 5.982069806703223 + }, + { + "x": 2.892255080791877, + "y": 2.6979336459530643, + "heading": -0.5773440276114954, + "angularVelocity": -0.817602007430762, + "velocityX": -2.075309888665035, + "velocityY": 1.379437938640018, + "timestamp": 6.0457488402657855 + }, + { + "x": 2.7748123827126636, + "y": 2.776006231552379, + "heading": -0.6251288383809169, + "angularVelocity": -0.7504010047902983, + "velocityX": -1.8442914646911481, + "velocityY": 1.226032827941903, + "timestamp": 6.109427873828348 + }, + { + "x": 2.6720699827794823, + "y": 2.8443129171584167, + "heading": -0.6679860389317067, + "angularVelocity": -0.6730190166702934, + "velocityX": -1.6134415707211014, + "velocityY": 1.0726715181525133, + "timestamp": 6.17310690739091 + }, + { + "x": 2.58401962501764, + "y": 2.9028562720875715, + "heading": -0.7054558020691595, + "angularVelocity": -0.5884160145213319, + "velocityX": -1.3827213265624843, + "velocityY": 0.9193505562806369, + "timestamp": 6.236785940953472 + }, + { + "x": 2.5106548115569836, + "y": 2.9516385308148143, + "heading": -0.7371951289783589, + "angularVelocity": -0.49842664270363046, + "velocityX": -1.152103123370717, + "velocityY": 0.7660646840583188, + "timestamp": 6.300464974516034 + }, + { + "x": 2.451970312872832, + "y": 2.9906615991432, + "heading": -0.762938657381878, + "angularVelocity": -0.4042700864520384, + "velocityX": -0.921567043358115, + "velocityY": 0.6128087401019892, + "timestamp": 6.364144008078596 + }, + { + "x": 2.4079618328973456, + "y": 3.019927088676532, + "heading": -0.7824753669485657, + "angularVelocity": -0.306799718426846, + "velocityX": -0.6910984277462318, + "velocityY": 0.45957810437841673, + "timestamp": 6.427823041641158 + }, + { + "x": 2.378625774942972, + "y": 3.0394363533387363, + "heading": -0.7956338335546038, + "angularVelocity": -0.20663734780319193, + "velocityX": -0.4606862936377973, + "velocityY": 0.30636873034571754, + "timestamp": 6.49150207520372 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.10425061521167653, + "velocityX": -0.23032229137361054, + "velocityY": 0.15317707188371738, + "timestamp": 6.555181108766282 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 1.7497535778176193e-25, + "velocityX": -1.4086622042761887e-23, + "velocityY": 7.921157765160188e-25, + "timestamp": 6.6188601423288445 + }, + { + "x": 2.3794831363952245, + "y": 3.0417249906399815, + "heading": -0.7962438376212186, + "angularVelocity": 0.09576161859408154, + "velocityX": 0.24659384653798558, + "velocityY": -0.11858712382831015, + "timestamp": 6.681814115401516 + }, + { + "x": 2.41053316098515, + "y": 3.026792718508752, + "heading": -0.7842933803747459, + "angularVelocity": 0.1898284836237042, + "velocityX": 0.4932178713181911, + "velocityY": -0.23719348283216615, + "timestamp": 6.744768088474188 + }, + { + "x": 2.4571113467410304, + "y": 3.0043922677761175, + "heading": -0.766548054431847, + "angularVelocity": 0.2818777763623355, + "velocityX": 0.7398768255994334, + "velocityY": -0.3558226691550744, + "timestamp": 6.807722061546859 + }, + { + "x": 2.5192202681818534, + "y": 2.974521939997295, + "heading": -0.7431604845322959, + "angularVelocity": 0.3715026829609895, + "velocityX": 0.9865766751389508, + "velocityY": -0.4744788346295768, + "timestamp": 6.870676034619531 + }, + { + "x": 2.5968629760285724, + "y": 2.937179727170992, + "heading": -0.7143164433853058, + "angularVelocity": 0.4581766604896231, + "velocityX": 1.2333249842244594, + "velocityY": -0.593166896443484, + "timestamp": 6.933630007692202 + }, + { + "x": 2.6900431315277755, + "y": 2.8923632450565475, + "heading": -0.6802459924925017, + "angularVelocity": 0.5411961982681314, + "velocityX": 1.4801314508242465, + "velocityY": -0.7118928310165661, + "timestamp": 6.996583980764874 + }, + { + "x": 2.7987651872562824, + "y": 2.8400696406927834, + "heading": -0.6412406418470571, + "angularVelocity": 0.6195852102998923, + "velocityX": 1.72700864491909, + "velocityY": -0.8306640837965626, + "timestamp": 7.059537953837546 + }, + { + "x": 2.9230346291738636, + "y": 2.780295465724823, + "heading": -0.597681173837793, + "angularVelocity": 0.6919256384181025, + "velocityX": 1.9739729814054823, + "velocityY": -0.9494901123867101, + "timestamp": 7.122491926910217 + }, + { + "x": 3.0628582843459875, + "y": 2.713036511288724, + "heading": -0.5500859220162175, + "angularVelocity": 0.7560325345412855, + "velocityX": 2.2210457632390876, + "velocityY": -1.0683829971852359, + "timestamp": 7.185445899982889 + }, + { + "x": 3.2182446187737406, + "y": 2.638287632787952, + "heading": -0.49920268501328596, + "angularVelocity": 0.8082609328595405, + "velocityX": 2.468253024290973, + "velocityY": -1.1873576019496896, + "timestamp": 7.24839987305556 + }, + { + "x": 3.3892034703211182, + "y": 2.556042786579892, + "heading": -0.4462086227940175, + "angularVelocity": 0.8417905913276443, + "velocityX": 2.7156165560834484, + "velocityY": -1.3064282076862779, + "timestamp": 7.311353846128232 + }, + { + "x": 3.5757413262062396, + "y": 2.4662968513988717, + "heading": -0.3932458767459955, + "angularVelocity": 0.8412931458175688, + "velocityX": 2.9630831348768174, + "velocityY": -1.4255801627868938, + "timestamp": 7.3743078192009035 + }, + { + "x": 3.777810773280146, + "y": 2.3690669294508315, + "heading": -0.3455559373140241, + "angularVelocity": 0.7575366113417542, + "velocityX": 3.2097965737705954, + "velocityY": -1.5444604558286192, + "timestamp": 7.437261792273575 + }, + { + "x": 3.9925130219343306, + "y": 2.2661623700337894, + "heading": -0.34555574915683335, + "angularVelocity": 0.000002988805655755853, + "velocityX": 3.410463838499, + "velocityY": -1.6345999210924134, + "timestamp": 7.500215765346247 + }, + { + "x": 4.207067930960627, + "y": 2.1629508436660116, + "heading": -0.34555573251477295, + "angularVelocity": 2.6435282147725544e-7, + "velocityX": 3.408123404358042, + "velocityY": -1.6394759747511882, + "timestamp": 7.563169738418918 + }, + { + "x": 4.421622830783708, + "y": 2.0597392981666993, + "heading": -0.34555571587271255, + "angularVelocity": 2.643528217260083e-7, + "velocityX": 3.408123258168449, + "velocityY": -1.6394762786483632, + "timestamp": 7.62612371149159 + }, + { + "x": 4.6361777306062155, + "y": 1.956527752666195, + "heading": -0.34555569923065216, + "angularVelocity": 2.643528215615007e-7, + "velocityX": 3.408123258159341, + "velocityY": -1.6394762786672963, + "timestamp": 7.689077684564261 + }, + { + "x": 4.850732630432354, + "y": 1.85331620717324, + "heading": -0.3455556825885917, + "angularVelocity": 2.643528216339363e-7, + "velocityX": 3.4081232582170267, + "velocityY": -1.6394762785473795, + "timestamp": 7.752031657636933 + }, + { + "x": 5.065287588526139, + "y": 1.750104782806442, + "heading": -0.34555566594653137, + "angularVelocity": 2.6435282065070603e-7, + "velocityX": 3.408124183776485, + "velocityY": -1.6394743545042916, + "timestamp": 7.814985630709605 + }, + { + "x": 5.280754782593808, + "y": 1.6488115627171362, + "heading": -0.34555564929394994, + "angularVelocity": 2.645199441921782e-7, + "velocityX": 3.4226147064450507, + "velocityY": -1.6090044066381253, + "timestamp": 7.877939603782276 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 2.6733322523330606e-7, + "velocityX": 3.530338179119512, + "velocityY": -1.3564287389044698, + "timestamp": 7.940893576854948 + }, + { + "x": 5.652794667941403, + "y": 1.5139935171410897, + "heading": -0.3455556356136128, + "angularVelocity": -7.55109994192204e-8, + "velocityX": 3.591492784420599, + "velocityY": -1.1850586838463792, + "timestamp": 7.98260076650368 + }, + { + "x": 5.804789449400159, + "y": 1.47182921760144, + "heading": -0.34555563872486544, + "angularVelocity": -7.459751255727934e-8, + "velocityX": 3.644330455705477, + "velocityY": -1.0109599782379923, + "timestamp": 8.02430795615241 + }, + { + "x": 5.9583966935302906, + "y": 1.435981535401865, + "heading": -0.3455556418269747, + "angularVelocity": -7.43782867600124e-8, + "velocityX": 3.682991959512248, + "velocityY": -0.8595084564913873, + "timestamp": 8.066015145801142 + }, + { + "x": 6.112005098303726, + "y": 1.4001388269201716, + "heading": -0.34555564492908214, + "angularVelocity": -7.437824177042706e-8, + "velocityX": 3.683019787887031, + "velocityY": -0.8593892032421666, + "timestamp": 8.107722335449873 + }, + { + "x": 6.265613503315511, + "y": 1.3642961194599532, + "heading": -0.34555564803118954, + "angularVelocity": -7.437824172842392e-8, + "velocityX": 3.683019793601852, + "velocityY": -0.8593891787505857, + "timestamp": 8.149429525098604 + }, + { + "x": 6.419221908327331, + "y": 1.3284534119998854, + "heading": -0.345555651133297, + "angularVelocity": -7.437824199449142e-8, + "velocityX": 3.683019793602695, + "velocityY": -0.8593891787469733, + "timestamp": 8.191136714747335 + }, + { + "x": 6.572830313271966, + "y": 1.2926107042518886, + "heading": -0.34555565423540446, + "angularVelocity": -7.437824209379189e-8, + "velocityX": 3.6830197919918253, + "velocityY": -0.8593891856505578, + "timestamp": 8.232843904396066 + }, + { + "x": 6.726438391075464, + "y": 1.2567665945285837, + "heading": -0.3455556573375121, + "angularVelocity": -7.437824552269796e-8, + "velocityX": 3.6830119482329913, + "velocityY": -0.8594228003659058, + "timestamp": 8.274551094044797 + }, + { + "x": 6.878955915134789, + "y": 1.2165341904124123, + "heading": -0.3455556607967783, + "angularVelocity": -8.29417253790897e-8, + "velocityX": 3.6568640885148347, + "velocityY": -0.9646395370922597, + "timestamp": 8.316258283693529 + }, + { + "x": 7.025398158916378, + "y": 1.1717644348314602, + "heading": -0.3467458392299039, + "angularVelocity": -0.028536529148800397, + "velocityX": 3.5111990286317973, + "velocityY": -1.0734301677483917, + "timestamp": 8.35796547334226 + }, + { + "x": 7.164406767305733, + "y": 1.1285160001941774, + "heading": -0.3473601505437228, + "angularVelocity": -0.014729146676936172, + "velocityX": 3.3329651208849276, + "velocityY": -1.0369539401127719, + "timestamp": 8.39967266299099 + }, + { + "x": 7.296003097467406, + "y": 1.0868892243180075, + "heading": -0.34737182264197447, + "angularVelocity": -0.0002798581815252442, + "velocityX": 3.1552432870689695, + "velocityY": -0.9980719445918297, + "timestamp": 8.441379852639722 + }, + { + "x": 7.4201946902403915, + "y": 1.0469180189360368, + "heading": -0.3467718237478745, + "angularVelocity": 0.014385982348686153, + "velocityX": 2.9777022575473837, + "velocityY": -0.958376858249577, + "timestamp": 8.483087042288453 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.02916023097831232, + "velocityX": 2.8002535793496506, + "velocityY": -0.918273122155384, + "timestamp": 8.524794231937184 + }, + { + "x": 7.694109553786587, + "y": 0.9554552205250534, + "heading": -0.3423846615894022, + "angularVelocity": 0.051196248581704275, + "velocityX": 2.536815281233286, + "velocityY": -0.8583516129975453, + "timestamp": 8.586731794995119 + }, + { + "x": 7.835030505907911, + "y": 0.9064832909135739, + "heading": -0.3388644821779997, + "angularVelocity": 0.056834322140020786, + "velocityX": 2.2752098268623953, + "velocityY": -0.7906660706956153, + "timestamp": 8.648669358053054 + }, + { + "x": 7.959827165183218, + "y": 0.8619979100061141, + "heading": -0.33562038589869275, + "angularVelocity": 0.052376879540328165, + "velocityX": 2.014878421331732, + "velocityY": -0.7182294347914383, + "timestamp": 8.710606921110989 + }, + { + "x": 8.068556862392501, + "y": 0.8221974820041511, + "heading": -0.33307610796483417, + "angularVelocity": 0.04107810847318481, + "velocityX": 1.7554726379463939, + "velocityY": -0.642589505252806, + "timestamp": 8.772544484168924 + }, + { + "x": 8.161262931310752, + "y": 0.7872247537071302, + "heading": -0.33153778511771786, + "angularVelocity": 0.024836670530246144, + "velocityX": 1.4967664909827672, + "velocityY": -0.5646448870503377, + "timestamp": 8.834482047226858 + }, + { + "x": 8.237979201929296, + "y": 0.7571873231462991, + "heading": -0.3312369649013586, + "angularVelocity": 0.004856830031849059, + "velocityX": 1.2386065390849326, + "velocityY": -0.4849630672865623, + "timestamp": 8.896419610284793 + }, + { + "x": 8.298732785621018, + "y": 0.7321691840482208, + "heading": -0.33235491812017326, + "angularVelocity": -0.018049680413951164, + "velocityX": 0.9808843082007279, + "velocityY": -0.40392514433731974, + "timestamp": 8.958357173342728 + }, + { + "x": 8.34354588005975, + "y": 0.7122377172112425, + "heading": -0.33503742038505996, + "angularVelocity": -0.043309780566883505, + "velocityX": 0.7235204652274613, + "velocityY": -0.3217993387685419, + "timestamp": 9.020294736400663 + }, + { + "x": 8.37243698558101, + "y": 0.697448170361649, + "heading": -0.33940425947903785, + "angularVelocity": -0.07050388937474399, + "velocityX": 0.4664553155608451, + "velocityY": -0.2387815425634291, + "timestamp": 9.082232299458598 + }, + { + "x": 8.385421752929688, + "y": 0.6878466606140137, + "heading": -0.34555563246426124, + "angularVelocity": -0.0993157089417533, + "velocityX": 0.20964285173008823, + "velocityY": -0.15501917210811605, + "timestamp": 9.144169862516533 + }, + { + "x": 8.381221613409881, + "y": 0.6835499744193162, + "heading": -0.3542287504471472, + "angularVelocity": -0.13146726396270184, + "velocityX": -0.06366578340339453, + "velocityY": -0.06512923947740829, + "timestamp": 9.210141555556 + }, + { + "x": 8.358990441245284, + "y": 0.6851839703239002, + "heading": -0.3649454511800031, + "angularVelocity": -0.16244392464575347, + "velocityX": -0.3369804705678509, + "velocityY": 0.024768136594682598, + "timestamp": 9.276113248595466 + }, + { + "x": 8.31872779484583, + "y": 0.6927492166918153, + "heading": -0.3776152319611175, + "angularVelocity": -0.19204874389891546, + "velocityX": -0.6103018513617493, + "velocityY": 0.11467412793831581, + "timestamp": 9.342084941634933 + }, + { + "x": 8.26043318618489, + "y": 0.7062463795602301, + "heading": -0.3921309983002572, + "angularVelocity": -0.2200302231209342, + "velocityX": -0.8836306296711994, + "velocityY": 0.20459021508422195, + "timestamp": 9.4080566346744 + }, + { + "x": 8.184106078073826, + "y": 0.72567624991864, + "heading": -0.40836398748744607, + "angularVelocity": -0.24605991508324035, + "velocityX": -1.1569675506948445, + "velocityY": 0.294518292061811, + "timestamp": 9.474028327713867 + }, + { + "x": 8.089745885570975, + "y": 0.7510397809748904, + "heading": -0.426156376289344, + "angularVelocity": -0.26969731989830587, + "velocityX": -1.430313338273756, + "velocityY": 0.3844608177794827, + "timestamp": 9.540000020753334 + }, + { + "x": 7.9773519881013435, + "y": 0.7823381401243138, + "heading": -0.4453100949140704, + "angularVelocity": -0.2903323795747961, + "velocityX": -1.7036685325385499, + "velocityY": 0.47442103889465864, + "timestamp": 9.605971713792801 + }, + { + "x": 7.846923768615211, + "y": 0.8195727828481538, + "heading": -0.46556907619150867, + "angularVelocity": -0.3070859689066713, + "velocityX": -1.9770330800530667, + "velocityY": 0.5644033222182843, + "timestamp": 9.671943406832268 + }, + { + "x": 7.698460723132396, + "y": 0.8627455590379989, + "heading": -0.48658932670817834, + "angularVelocity": -0.3186253004617381, + "velocityX": -2.2504052669074097, + "velocityY": 0.6544136462288057, + "timestamp": 9.737915099871735 + }, + { + "x": 7.531962767971369, + "y": 0.9118588631154407, + "heading": -0.50788425226007, + "angularVelocity": -0.3227888291293674, + "velocityX": -2.52377872220773, + "velocityY": 0.7444602649208999, + "timestamp": 9.803886792911202 + }, + { + "x": 7.347431178555123, + "y": 0.9669158145351346, + "heading": -0.528713009661178, + "angularVelocity": -0.31572264469016176, + "velocityX": -2.79713284462553, + "velocityY": 0.8345541683575805, + "timestamp": 9.869858485950669 + }, + { + "x": 7.144872018682828, + "y": 1.0279202336148925, + "heading": -0.5478112276567515, + "angularVelocity": -0.28949110013211704, + "velocityX": -3.070395051876507, + "velocityY": 0.9247059802340185, + "timestamp": 9.935830178990136 + }, + { + "x": 6.9243138547651855, + "y": 1.094874091293601, + "heading": -0.5625298674499013, + "angularVelocity": -0.22310538224848228, + "velocityX": -3.343224249007752, + "velocityY": 1.0148876676342686, + "timestamp": 10.001801872029603 + }, + { + "x": 6.6860270471357435, + "y": 1.1677363105602094, + "heading": -0.5635939187210077, + "angularVelocity": -0.016128906536775855, + "velocityX": -3.6119553197898044, + "velocityY": 1.1044467090304817, + "timestamp": 10.06777356506907 + }, + { + "x": 6.44899650553119, + "y": 1.2456322116060934, + "heading": -0.5635939252708948, + "angularVelocity": -9.928329560968507e-8, + "velocityX": -3.59291281887752, + "velocityY": 1.1807473396094794, + "timestamp": 10.133745258108537 + }, + { + "x": 6.2119660599131885, + "y": 1.3235284047302205, + "heading": -0.5635939318207752, + "angularVelocity": -9.928319425999079e-8, + "velocityX": -3.592911363911807, + "velocityY": 1.1807517669362584, + "timestamp": 10.199716951148003 + }, + { + "x": 5.974935614307485, + "y": 1.4014245978917717, + "heading": -0.5635939383706557, + "angularVelocity": -9.928319467077491e-8, + "velocityX": -3.5929113637253804, + "velocityY": 1.1807517675035346, + "timestamp": 10.26568864418747 + }, + { + "x": 5.737905296446339, + "y": 1.4793211797663546, + "heading": -0.5635939449205359, + "angularVelocity": -9.928319168961564e-8, + "velocityX": -3.5929094273712967, + "velocityY": 1.1807576596220166, + "timestamp": 10.331660337226937 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.563593951480198, + "angularVelocity": -9.943146555012959e-8, + "velocityX": -3.5606437907587183, + "velocityY": 1.2747558956305969, + "timestamp": 10.397632030266404 + }, + { + "x": 5.285962555889255, + "y": 1.6593525613363396, + "heading": -0.5635939570041091, + "angularVelocity": -8.80379800734208e-8, + "velocityX": -3.4591170542763976, + "velocityY": 1.5289526345693891, + "timestamp": 10.460376666531877 + }, + { + "x": 5.071426961149029, + "y": 1.7607652993502259, + "heading": -0.5635939625107921, + "angularVelocity": -8.776340606411008e-8, + "velocityX": -3.419186204738301, + "velocityY": 1.6162774071206454, + "timestamp": 10.52312130279735 + }, + { + "x": 4.856891538791384, + "y": 1.862178402033475, + "heading": -0.5635939680174745, + "angularVelocity": -8.776339871909346e-8, + "velocityX": -3.4191834573706834, + "velocityY": 1.6162832190813905, + "timestamp": 10.585865939062822 + }, + { + "x": 4.642356116443861, + "y": 1.9635915047381367, + "heading": -0.5635939735241571, + "angularVelocity": -8.776339868323175e-8, + "velocityX": -3.4191834572093653, + "velocityY": 1.6162832194226513, + "timestamp": 10.648610575328295 + }, + { + "x": 4.427820694096338, + "y": 2.0650046074427992, + "heading": -0.5635939790308396, + "angularVelocity": -8.776339874733319e-8, + "velocityX": -3.4191834572093556, + "velocityY": 1.6162832194226713, + "timestamp": 10.711355211593768 + }, + { + "x": 4.213285271748816, + "y": 2.166417710147462, + "heading": -0.5635939845375221, + "angularVelocity": -8.776339865248036e-8, + "velocityX": -3.419183457209353, + "velocityY": 1.616283219422678, + "timestamp": 10.77409984785924 + }, + { + "x": 3.9987498494044535, + "y": 2.2678308128588087, + "heading": -0.5635939900442046, + "angularVelocity": -8.776339914853593e-8, + "velocityX": -3.4191834571589985, + "velocityY": 1.6162832195291985, + "timestamp": 10.836844484124713 + }, + { + "x": 3.784214480868839, + "y": 2.3692440293975783, + "heading": -0.5635939955531623, + "angularVelocity": -8.779965948091874e-8, + "velocityX": -3.419182599575763, + "velocityY": 1.6162850336670995, + "timestamp": 10.899589120390186 + }, + { + "x": 3.5824862821613, + "y": 2.464592718536819, + "heading": -0.5976475454093522, + "angularVelocity": -0.5427324450827832, + "velocityX": -3.2150668282469037, + "velocityY": 1.519630916909295, + "timestamp": 10.962333756655658 + }, + { + "x": 3.3962755579097226, + "y": 2.5526066544868016, + "heading": -0.6290952782619788, + "angularVelocity": -0.5012019309438841, + "velocityX": -2.967755259010819, + "velocityY": 1.4027324276388398, + "timestamp": 11.025078392921131 + }, + { + "x": 3.2255823355920983, + "y": 2.6332858939778268, + "heading": -0.6579338280606744, + "angularVelocity": -0.45961776998243625, + "velocityX": -2.7204432518410133, + "velocityY": 1.2858348425142074, + "timestamp": 11.087823029186604 + }, + { + "x": 3.070406633071231, + "y": 2.70663049336622, + "heading": -0.6841601724081167, + "angularVelocity": -0.41798543920915443, + "velocityX": -2.4731309599806814, + "velocityY": 1.1689381555751166, + "timestamp": 11.150567665452076 + }, + { + "x": 2.930748464054276, + "y": 2.772640503904357, + "heading": -0.7077714566039787, + "angularVelocity": -0.3763076113145772, + "velocityX": -2.225818449660962, + "velocityY": 1.0520422854767753, + "timestamp": 11.213312301717549 + }, + { + "x": 2.8066078395848333, + "y": 2.8313159708307802, + "heading": -0.7287650312356323, + "angularVelocity": -0.33458755809548013, + "velocityX": -1.9785057633325538, + "velocityY": 0.9351471363730166, + "timestamp": 11.276056937983022 + }, + { + "x": 2.697984768845601, + "y": 2.8826569331257605, + "heading": -0.7471385161808816, + "angularVelocity": -0.2928295713997149, + "velocityX": -1.731192930653829, + "velocityY": 0.8182526085218894, + "timestamp": 11.338801574248494 + }, + { + "x": 2.6048792597285715, + "y": 2.926663423472239, + "heading": -0.7628898638650828, + "angularVelocity": -0.2510389512428944, + "velocityX": -1.483879972195546, + "velocityY": 0.7013586015589744, + "timestamp": 11.401546210513967 + }, + { + "x": 2.5272913192824404, + "y": 2.9633354682979096, + "heading": -0.7760174151917997, + "angularVelocity": -0.20922188904202302, + "velocityX": -1.236566901397856, + "velocityY": 0.5844650157905298, + "timestamp": 11.46429084677944 + }, + { + "x": 2.465220954071045, + "y": 2.992673087849862, + "heading": -0.7865199462233702, + "angularVelocity": -0.16738532019110303, + "velocityX": -0.9892537259882359, + "velocityY": 0.4675717527124547, + "timestamp": 11.527035483044912 + }, + { + "x": 2.4186681704546857, + "y": 3.014676296277141, + "heading": -0.7943967050876205, + "angularVelocity": -0.12553676828922553, + "velocityX": -0.7419404492105731, + "velocityY": 0.35067871513643956, + "timestamp": 11.589780119310385 + }, + { + "x": 2.3876329747987564, + "y": 3.0293451017064936, + "heading": -0.7996474390669244, + "angularVelocity": -0.08368418867053315, + "velocityX": -0.4946270709837142, + "velocityY": 0.23378580708140337, + "timestamp": 11.652524755575858 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": -0.04183581368700928, + "velocityX": -0.2473135890317597, + "velocityY": 0.11689293351474017, + "timestamp": 11.71526939184133 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": 8.260301472108595e-27, + "velocityX": 2.4653400544228422e-26, + "velocityY": -1.932180439483193e-25, + "timestamp": 11.778014028106803 + }, + { + "x": 2.3862784269914368, + "y": 3.0299921206081337, + "heading": -0.7979445021330078, + "angularVelocity": 0.07214760810661107, + "velocityX": 0.23610252086471115, + "velocityY": -0.11148080699315335, + "timestamp": 11.838000907230818 + }, + { + "x": 2.4146057122945543, + "y": 3.016617241069024, + "heading": -0.7893555779678016, + "angularVelocity": 0.14318004688074568, + "velocityX": 0.47222468841152654, + "velocityY": -0.22296341690754115, + "timestamp": 11.897987786354832 + }, + { + "x": 2.4570985950819932, + "y": 2.9965547338415663, + "heading": -0.7765849361695806, + "angularVelocity": 0.21289058515312126, + "velocityX": 0.7083696202896328, + "velocityY": -0.3344482580262399, + "timestamp": 11.957974665478847 + }, + { + "x": 2.5137586742181073, + "y": 2.9698044451299115, + "heading": -0.7597273140396543, + "angularVelocity": 0.28102182304025686, + "velocityX": 0.9445412057356202, + "velocityY": -0.445935662969764, + "timestamp": 12.017961544602862 + }, + { + "x": 2.5845878441848784, + "y": 2.9363662103913186, + "heading": -0.7388971765018356, + "angularVelocity": 0.34724489491702143, + "velocityX": 1.1807443727876086, + "velocityY": -0.5574258108921492, + "timestamp": 12.077948423726877 + }, + { + "x": 2.669588380905788, + "y": 2.8962398697740044, + "heading": -0.7142349758160352, + "angularVelocity": 0.41112658377867206, + "velocityX": 1.416985480194453, + "velocityY": -0.6689186235936444, + "timestamp": 12.137935302850892 + }, + { + "x": 2.7687630627135538, + "y": 2.8494252954239245, + "heading": -0.6859166587732123, + "angularVelocity": 0.47207518471295223, + "velocityX": 1.65327290327499, + "velocityY": -0.7804135676619703, + "timestamp": 12.197922181974906 + }, + { + "x": 2.8821153447700505, + "y": 2.7959224429728664, + "heading": -0.6541688089081689, + "angularVelocity": 0.5292465673936607, + "velocityX": 1.8896179249824876, + "velocityY": -0.8919092513622485, + "timestamp": 12.257909061098921 + }, + { + "x": 3.00964961510041, + "y": 2.7357314565429642, + "heading": -0.6192942625032292, + "angularVelocity": 0.5813695747171842, + "velocityX": 2.126036096438681, + "velocityY": -1.00340253250157, + "timestamp": 12.317895940222936 + }, + { + "x": 3.151371568939876, + "y": 2.668852905662324, + "heading": -0.5817190532214994, + "angularVelocity": 0.6263904678896166, + "velocityX": 2.3625492092441416, + "velocityY": -1.114886319429574, + "timestamp": 12.37788281934695 + }, + { + "x": 3.307288700965784, + "y": 2.595288399732038, + "heading": -0.5420886206685631, + "angularVelocity": 0.6606516813619456, + "velocityX": 2.5991872606602855, + "velocityY": -1.2263432771390068, + "timestamp": 12.437869698470966 + }, + { + "x": 3.477410464476997, + "y": 2.5150425548921422, + "heading": -0.5015011072339337, + "angularVelocity": 0.6766065184141348, + "velocityX": 2.8359829015193223, + "velocityY": -1.3377232823531002, + "timestamp": 12.49785657759498 + }, + { + "x": 3.661742704023134, + "y": 2.428132077104721, + "heading": -0.46226256504462326, + "angularVelocity": 0.6541187466710837, + "velocityX": 3.072875972844897, + "velocityY": -1.44882479396444, + "timestamp": 12.557843456718995 + }, + { + "x": 3.8601464032626667, + "y": 2.3346891273821493, + "heading": -0.4327535625874362, + "angularVelocity": 0.4919242822448071, + "velocityX": 3.307451598363016, + "velocityY": -1.55772314024523, + "timestamp": 12.61783033584301 + }, + { + "x": 4.065242172431025, + "y": 2.2377114042955792, + "heading": -0.4327535491582794, + "angularVelocity": 2.2386823508713805e-7, + "velocityX": 3.4190104930171277, + "velocityY": -1.6166489156083852, + "timestamp": 12.677817214967025 + }, + { + "x": 4.270337923318267, + "y": 2.140733642546332, + "heading": -0.4327535357295662, + "angularVelocity": 2.238608404824225e-7, + "velocityX": 3.4190101882652333, + "velocityY": -1.6166495601272812, + "timestamp": 12.73780409409104 + }, + { + "x": 4.475433674204211, + "y": 2.043755880794341, + "heading": -0.43275352230085307, + "angularVelocity": 2.2386084074280025e-7, + "velocityX": 3.4190101882436053, + "velocityY": -1.6166495601730213, + "timestamp": 12.797790973215054 + }, + { + "x": 4.680529425090156, + "y": 1.9467781190423497, + "heading": -0.43275350887213987, + "angularVelocity": 2.238608397454811e-7, + "velocityX": 3.4190101882436035, + "velocityY": -1.6166495601730246, + "timestamp": 12.85777785233907 + }, + { + "x": 4.885625175976101, + "y": 1.8498003572903603, + "heading": -0.4327534954434267, + "angularVelocity": 2.2386084091908683e-7, + "velocityX": 3.4190101882436177, + "velocityY": -1.6166495601729944, + "timestamp": 12.917764731463084 + }, + { + "x": 5.090720926874051, + "y": 1.7528225955637593, + "heading": -0.43275348201471353, + "angularVelocity": 2.238608408563689e-7, + "velocityX": 3.41901018844374, + "velocityY": -1.616649559749761, + "timestamp": 12.977751610587099 + }, + { + "x": 5.295816846922337, + "y": 1.6558451915696206, + "heading": -0.4327534685859999, + "angularVelocity": 2.2386084788003102e-7, + "velocityX": 3.41901300823263, + "velocityY": -1.6166435962379466, + "timestamp": 13.037738489711113 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.4327534551249964, + "angularVelocity": 2.2439912995828436e-7, + "velocityX": 3.453867801804683, + "velocityY": -1.5407737242905175, + "timestamp": 13.097725368835128 + }, + { + "x": 5.767637731173938, + "y": 1.4715486411010592, + "heading": -0.43275344331162413, + "angularVelocity": 1.594904372832459e-7, + "velocityX": 3.572782848206104, + "velocityY": -1.2403267182093187, + "timestamp": 13.17179483937328 + }, + { + "x": 6.033774187788168, + "y": 1.3841256962215103, + "heading": -0.43275343151544077, + "angularVelocity": 1.5925837367602458e-7, + "velocityX": 3.5930654651722995, + "velocityY": -1.1802831077956568, + "timestamp": 13.245864309911433 + }, + { + "x": 6.29991069823436, + "y": 1.2967029152195577, + "heading": -0.43275341971925746, + "angularVelocity": 1.5925837245370656e-7, + "velocityX": 3.593066191948934, + "velocityY": -1.1802808953106085, + "timestamp": 13.319933780449585 + }, + { + "x": 6.566047156059249, + "y": 1.2092799740277729, + "heading": -0.4327534079212868, + "angularVelocity": 1.5928250285484768e-7, + "velocityX": 3.5930654815171974, + "velocityY": -1.180283058007746, + "timestamp": 13.394003250987737 + }, + { + "x": 6.815604164652681, + "y": 1.1270295176704332, + "heading": -0.3680065962902011, + "angularVelocity": 0.8741362826096571, + "velocityX": 3.369229005962575, + "velocityY": -1.1104501727870977, + "timestamp": 13.468072721525889 + }, + { + "x": 7.042448674730911, + "y": 1.0523256933844904, + "heading": -0.3045889928856297, + "angularVelocity": 0.8561908562841154, + "velocityX": 3.0625912191634557, + "velocityY": -1.0085643078475075, + "timestamp": 13.542142192064041 + }, + { + "x": 7.24659057217413, + "y": 0.9851213013692786, + "heading": -0.24567984884817082, + "angularVelocity": 0.7953228720207434, + "velocityX": 2.7560869000416006, + "velocityY": -0.9073156798197457, + "timestamp": 13.616211662602193 + }, + { + "x": 7.4280385469425845, + "y": 0.925399905884711, + "heading": -0.19230648340236473, + "angularVelocity": 0.7205852162574101, + "velocityX": 2.4496999026743933, + "velocityY": -0.8062889480734987, + "timestamp": 13.690281133140346 + }, + { + "x": 7.586797948314142, + "y": 0.8731533384819596, + "heading": -0.1449799977839161, + "angularVelocity": 0.6389472649743265, + "velocityX": 2.1433851250466707, + "velocityY": -0.7053724972401416, + "timestamp": 13.764350603678498 + }, + { + "x": 7.722872326109614, + "y": 0.8283767850935353, + "heading": -0.10400779947540247, + "angularVelocity": 0.5531590547472527, + "velocityX": 1.8371182729783764, + "velocityY": -0.6045210403571151, + "timestamp": 13.83842007421665 + }, + { + "x": 7.836264193936779, + "y": 0.7910670987596001, + "heading": -0.06959571066012236, + "angularVelocity": 0.46459207235125305, + "velocityX": 1.530885356724122, + "velocityY": -0.5037120700723456, + "timestamp": 13.912489544754802 + }, + { + "x": 7.92697541388319, + "y": 0.7612220649081012, + "heading": -0.04189104486357632, + "angularVelocity": 0.37403623375808837, + "velocityX": 1.2246775802141858, + "velocityY": -0.40293299836842056, + "timestamp": 13.986559015292954 + }, + { + "x": 7.995007406321081, + "y": 0.7388400303082948, + "heading": -0.02100380767783558, + "angularVelocity": 0.2819952273721474, + "velocityX": 0.918488979921211, + "velocityY": -0.3021762466666727, + "timestamp": 14.060628485831106 + }, + { + "x": 8.04036127275019, + "y": 0.7239196955099017, + "heading": -0.007018297514014948, + "angularVelocity": 0.1888161217058643, + "velocityX": 0.6123152507988858, + "velocityY": -0.2014370386339925, + "timestamp": 14.134697956369259 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -1.7184723296669235e-26, + "angularVelocity": 0.09475290511763465, + "velocityX": 0.30615312084056384, + "velocityY": -0.10071228953000028, + "timestamp": 14.20876742690741 }, { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.13679610671265519, - "velocityX": 0.3296745537009426, - "velocityY": -0.18295553238277804, - "timestamp": 1.2292595782616684 + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -8.404488237973452e-27, + "angularVelocity": 4.825075090660763e-27, + "velocityX": -1.1513118466109779e-26, + "velocityY": -4.361967999563915e-27, + "timestamp": 14.282836897445563 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.43324199318885803, + "y": 4.121134281158447, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { + "timestamp": 1.4277752931645424, + "isStopPoint": true, "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 2.834394108236562e-32, - "velocityX": -3.2904683025944325e-34, - "velocityY": 0, - "timestamp": 1.3015689652182372 - }, - { - "x": 2.379804145625828, - "y": 3.040219511725313, - "heading": -0.7770894823147659, - "angularVelocity": 0.41593022428853316, - "velocityX": 0.2617028389642093, - "velocityY": -0.14816838427057621, - "timestamp": 1.3621150092281344 - }, - { - "x": 2.411575732183553, - "y": 3.0222750861201635, - "heading": -0.7276303412852089, - "angularVelocity": 0.8168847665996504, - "velocityX": 0.524750825215457, - "velocityY": -0.2963765163949562, - "timestamp": 1.4226610532380317 - }, - { - "x": 2.459369710281825, - "y": 2.995353279300503, - "heading": -0.6550403019178791, - "angularVelocity": 1.1989229115524662, - "velocityX": 0.789382343303212, - "velocityY": -0.4446501379224719, - "timestamp": 1.483207097247929 - }, - { - "x": 2.5233031088128075, - "y": 2.959437775047564, - "heading": -0.5609434444834801, - "angularVelocity": 1.5541371690447268, - "velocityX": 1.0559467522028614, - "velocityY": -0.5931932439230606, - "timestamp": 1.5437531412578263 - }, - { - "x": 2.6035132843463873, - "y": 2.9144863047853526, - "heading": -0.44765744633251, - "angularVelocity": 1.8710718429836914, - "velocityX": 1.3247797910705466, - "velocityY": -0.7424344727603186, - "timestamp": 1.6042991852677235 - }, - { - "x": 2.700148811858614, - "y": 2.8604246981568315, - "heading": -0.31830060076747974, - "angularVelocity": 2.136503675514867, - "velocityX": 1.5960667470930034, - "velocityY": -0.8929007255979282, - "timestamp": 1.6648452292776208 - }, - { - "x": 2.8133605147902916, - "y": 2.7971622097455233, - "heading": -0.1768724119476308, - "angularVelocity": 2.3358782746686186, - "velocityX": 1.869844756713935, - "velocityY": -1.0448657620135586, - "timestamp": 1.725391273287518 - }, - { - "x": 2.943296329434224, - "y": 2.7246257074465063, - "heading": -0.02888673141070084, - "angularVelocity": 2.4441841404657145, - "velocityX": 2.1460661347699634, - "velocityY": -1.1980386742882738, - "timestamp": 1.7859373172974153 - }, - { - "x": 3.0900160520490236, - "y": 2.6428091950168, - "heading": 0.11617462075416056, - "angularVelocity": 2.3958848928453333, - "velocityX": 2.4232751291036574, - "velocityY": -1.3513106226450158, - "timestamp": 1.8464833613073126 - }, - { - "x": 3.252842131476166, - "y": 2.55207108446101, - "heading": 0.23929724530787766, - "angularVelocity": 2.0335370636864525, - "velocityX": 2.6892934474880965, - "velocityY": -1.4986629108411686, - "timestamp": 1.9070294053172099 - }, - { - "x": 3.4280030327225406, - "y": 2.451410973839388, - "heading": 0.3174619447233944, - "angularVelocity": 1.2909959798981987, - "velocityX": 2.8930197523349617, - "velocityY": -1.662538193332126, - "timestamp": 1.9675754493271072 - }, - { - "x": 3.6156107870917813, - "y": 2.340964520507575, - "heading": 0.3503075182755731, - "angularVelocity": 0.5424891764490772, - "velocityX": 3.098596406043856, - "velocityY": -1.8241729106819762, - "timestamp": 2.028121493337004 - }, - { - "x": 3.8155697764375462, - "y": 2.229387834544172, - "heading": 0.3503075829776931, - "angularVelocity": 0.0000010686432293648565, - "velocityX": 3.302593796434966, - "velocityY": -1.8428402348659505, - "timestamp": 2.0886675373469012 - }, - { - "x": 4.015529033815858, - "y": 2.117811628928195, - "heading": 0.35030764767858236, - "angularVelocity": 0.000001068622902276308, - "velocityX": 3.3025982233558384, - "velocityY": -1.8428323012769912, - "timestamp": 2.1492135813567983 - }, - { - "x": 4.215488291220884, - "y": 2.0062354233600974, - "heading": 0.35030771237947195, - "angularVelocity": 0.0000010686229082180607, - "velocityX": 3.302598223797092, - "velocityY": -1.8428323004862048, - "timestamp": 2.2097596253666953 - }, - { - "x": 4.415447878546615, - "y": 1.8946598090554148, - "heading": 0.350307777080342, - "angularVelocity": 0.0000010686225853054014, - "velocityX": 3.3026036728847936, - "velocityY": -1.842822534969312, - "timestamp": 2.2703056693765924 - }, - { - "x": 4.620289311400001, - "y": 1.7923224180578192, - "heading": 0.35030784179343544, - "angularVelocity": 0.00000106882447086681, - "velocityX": 3.383233970164951, - "velocityY": -1.6902407526553997, - "timestamp": 2.3308517133864894 - }, - { - "x": 4.832980772388526, - "y": 1.7074973259478416, - "heading": 0.35030790812662205, - "angularVelocity": 0.0000010955825050486418, - "velocityX": 3.512887827217186, - "velocityY": -1.401001394841118, - "timestamp": 2.3913977573963865 - }, - { - "x": 5.052028438396142, - "y": 1.6407809635052637, - "heading": 0.3503079787205065, - "angularVelocity": 0.0000011659537069164618, - "velocityX": 3.617869170309609, - "velocityY": -1.101911174108616, - "timestamp": 2.4519438014062835 - }, - { - "x": 5.275893582425239, - "y": 1.5926421132936275, - "heading": 0.3503080570287918, - "angularVelocity": 0.0000012933674954586782, - "velocityX": 3.697436350961301, - "velocityY": -0.7950783738037029, - "timestamp": 2.5124898454161806 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 }, { + "timestamp": 2.772294509981965, + "isStopPoint": false, "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": 0.35030814446785413, - "angularVelocity": 0.0000014441746575542642, - "velocityX": 3.751029791428781, - "velocityY": -0.4826595916936126, - "timestamp": 2.5730358894260776 - }, - { - "x": 5.645916996346935, - "y": 1.5526078041590061, - "heading": 0.35030823517092025, - "angularVelocity": 0.0000023934601751232044, - "velocityX": 3.7711793420471578, - "velocityY": -0.28528395446410554, - "timestamp": 2.6109320980454838 - }, - { - "x": 5.78920070535826, - "y": 1.5493061559809091, - "heading": 0.350308318678533, - "angularVelocity": 0.000002203587530623072, - "velocityX": 3.7809510299653186, - "velocityY": -0.08712344317225716, - "timestamp": 2.64882830666489 - }, - { - "x": 5.932460399917018, - "y": 1.5535231156733713, - "heading": 0.3503083968024594, - "angularVelocity": 0.0000020615235464554193, - "velocityX": 3.7803173398564307, - "velocityY": 0.11127655895113732, - "timestamp": 2.686724515284296 - }, - { - "x": 6.075301824148574, - "y": 1.5652470604158988, - "heading": 0.3503084709306375, - "angularVelocity": 0.0000019560842841988308, - "velocityX": 3.769280079337772, - "velocityY": 0.3093698596678062, - "timestamp": 2.724620723903702 - }, - { - "x": 6.217331878166829, - "y": 1.5844456932271112, - "heading": 0.3503085421651287, - "angularVelocity": 0.0000018797260658699983, - "velocityX": 3.747869752477631, - "velocityY": 0.5066109120314745, - "timestamp": 2.7625169325231083 - }, - { - "x": 6.358159707247505, - "y": 1.6110661098029424, - "heading": 0.3503086114142786, - "angularVelocity": 0.000001827337150563986, - "velocityX": 3.716145604300894, - "velocityY": 0.7024559328132687, - "timestamp": 2.8004131411425144 - }, - { - "x": 6.497397807591943, - "y": 1.6450348544134032, - "heading": 0.35030867945721406, - "angularVelocity": 0.0000017955077283623068, - "velocityX": 3.6741960585771425, - "velocityY": 0.8963626137804634, - "timestamp": 2.8383093497619205 - }, - { - "x": 6.634663343210931, - "y": 1.6862573848132372, - "heading": 0.3503087469924593, - "angularVelocity": 0.000001782110868887289, - "velocityX": 3.622144288827235, - "velocityY": 1.0877745268355006, - "timestamp": 2.8762055583813266 - }, - { - "x": 6.770832471288569, - "y": 1.7309686010151983, - "heading": 0.350308814465375, - "angularVelocity": 0.0000017804661250723641, - "velocityX": 3.593212435713354, - "velocityY": 1.179833493397679, - "timestamp": 2.9141017670007328 - }, - { - "x": 6.907000934216078, - "y": 1.775681842897667, - "heading": 0.35030888193834014, - "angularVelocity": 0.0000017804674286764754, - "velocityX": 3.5931948838222003, - "velocityY": 1.1798869467794602, - "timestamp": 2.951997975620139 - }, - { - "x": 7.041712141501968, - "y": 1.8246105114710787, - "heading": 0.3503089535974655, - "angularVelocity": 0.0000018909312562539825, - "velocityX": 3.5547410201057947, - "velocityY": 1.2911230530949709, - "timestamp": 2.989894184239545 - }, - { - "x": 7.171770756302622, - "y": 1.8790616059317657, - "heading": 0.3592109050640362, - "angularVelocity": 0.2349034848307258, - "velocityX": 3.43196904225537, - "velocityY": 1.43684807648023, - "timestamp": 3.027790392858951 - }, - { - "x": 7.296132052444026, - "y": 1.9323306109051834, - "heading": 0.3900794027691138, - "angularVelocity": 0.8145537200064432, - "velocityX": 3.281628972184848, - "velocityY": 1.4056552598282668, - "timestamp": 3.0656866014783573 - }, - { - "x": 7.41350002407911, - "y": 1.9843017273758428, - "heading": 0.4310938047294686, - "angularVelocity": 1.0822824618754054, - "velocityX": 3.097090076050037, - "velocityY": 1.371406754501705, - "timestamp": 3.1035828100977634 - }, - { - "x": 7.523578248846203, - "y": 2.0344435812093704, - "heading": 0.471270328487765, - "angularVelocity": 1.0601726458124487, - "velocityX": 2.904729226942307, - "velocityY": 1.3231364207724636, - "timestamp": 3.1414790187171695 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 }, { + "timestamp": 3.3984875559251146, + "isStopPoint": false, "x": 7.626483917236328, "y": 2.082604169845581, "heading": 0.507098504392337, - "angularVelocity": 0.9454290339278092, - "velocityX": 2.715460784576445, - "velocityY": 1.2708550641540455, - "timestamp": 3.1793752273365756 - }, - { - "x": 7.782116606596641, - "y": 2.1591418247265515, - "heading": 0.5502904833862925, - "angularVelocity": 0.6648323287723413, - "velocityX": 2.395575421885603, - "velocityY": 1.1781054843634051, - "timestamp": 3.244341952215483 - }, - { - "x": 7.917274495842872, - "y": 2.2282082387650113, - "heading": 0.5787513876779874, - "angularVelocity": 0.4380843323215672, - "velocityX": 2.0804171596791274, - "velocityY": 1.0631044456557224, - "timestamp": 3.30930867709439 - }, - { - "x": 8.032169166662662, - "y": 2.2890615062609574, - "heading": 0.5943003276772478, - "angularVelocity": 0.2393369840982844, - "velocityX": 1.768515667581298, - "velocityY": 0.9366836270316962, - "timestamp": 3.3742754019732972 - }, - { - "x": 8.12694730000964, - "y": 2.3412528312827354, - "heading": 0.5980433212943994, - "angularVelocity": 0.05761401123618554, - "velocityX": 1.458871961356148, - "velocityY": 0.8033547191898265, - "timestamp": 3.4392421268522044 - }, - { - "x": 8.201715109739398, - "y": 2.3844820330172567, - "heading": 0.590722775552934, - "angularVelocity": -0.11268146509017382, - "velocityX": 1.1508631513920338, - "velocityY": 0.6654052796273325, - "timestamp": 3.5042088517311116 - }, - { - "x": 8.256552657549896, - "y": 2.418534329374767, - "heading": 0.572871769156374, - "angularVelocity": -0.27477153003838295, - "velocityX": 0.8440866907283819, - "velocityY": 0.5241498077820845, - "timestamp": 3.569175576610019 - }, - { - "x": 8.291522317600203, - "y": 2.443248428942356, - "heading": 0.5448922111974887, - "angularVelocity": -0.4306752112106213, - "velocityX": 0.538270324007952, - "velocityY": 0.3804116586399298, - "timestamp": 3.634142301488926 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { + "timestamp": 3.9695683835747118, + "isStopPoint": false, "x": 8.306674003601074, "y": 2.45849871635437, "heading": 0.507098504392337, - "angularVelocity": -0.5817394500892022, - "velocityX": 0.23322225384012837, - "velocityY": 0.2347399755865949, - "timestamp": 3.6991090263678332 - }, - { - "x": 8.301908465997242, - "y": 2.464162051150854, - "heading": 0.4594182049840147, - "angularVelocity": -0.7297490572088752, - "velocityX": -0.07293676039464714, - "velocityY": 0.08667758549500083, - "timestamp": 3.7644469653994896 - }, - { - "x": 8.277129357589926, - "y": 2.4601442119160213, - "heading": 0.4025234718451046, - "angularVelocity": -0.8707763664131557, - "velocityX": -0.3792453324141557, - "velocityY": -0.061493204321711674, - "timestamp": 3.829784904431146 - }, - { - "x": 8.232325554908503, - "y": 2.44643699593842, - "heading": 0.33696231020008877, - "angularVelocity": -1.0034164318107932, - "velocityX": -0.6857241496355521, - "velocityY": -0.20978953699413314, - "timestamp": 3.8951228434628025 - }, - { - "x": 8.167484390379387, - "y": 2.42303097033958, - "heading": 0.26340235513425153, - "angularVelocity": -1.1258383131766259, - "velocityX": -0.992396844621899, - "velocityY": -0.35823024028199546, - "timestamp": 3.960460782494459 - }, - { - "x": 8.082591447673355, - "y": 2.3899154191246006, - "heading": 0.18267464694130434, - "angularVelocity": -1.235541086685248, - "velocityX": -1.2992901821543714, - "velocityY": -0.5068349523381112, - "timestamp": 4.025798721526115 - }, - { - "x": 7.9776304347285665, - "y": 2.347078349358033, - "heading": 0.09584952732096214, - "angularVelocity": -1.328862233904795, - "velocityX": -1.6064328704022224, - "velocityY": -0.6556232167931427, - "timestamp": 4.091136660557772 - }, - { - "x": 7.852583598052917, - "y": 2.2945065473278667, - "heading": 0.004378357289613106, - "angularVelocity": -1.3999702376138785, - "velocityX": -1.9138472766192256, - "velocityY": -0.8046137176854637, - "timestamp": 4.156474599589428 - }, - { - "x": 7.707434523289432, - "y": 2.232185897198679, - "heading": -0.08962534429630348, - "angularVelocity": -1.4387307432573238, - "velocityX": -2.221512905283988, - "velocityY": -0.9538202620531743, - "timestamp": 4.2218125386210845 - }, - { - "x": 7.542180705960218, - "y": 2.1601037497785995, - "heading": -0.1827828763211059, - "angularVelocity": -1.42578008130418, - "velocityX": -2.5292168650919318, - "velocityY": -1.1032204028528767, - "timestamp": 4.287150477652741 - }, - { - "x": 7.35689136818444, - "y": 2.078264678726032, - "heading": -0.2689709068831343, - "angularVelocity": -1.3191115581449608, - "velocityX": -2.835861377353881, - "velocityY": -1.252550543611691, - "timestamp": 4.352488416684397 - }, - { - "x": 7.152059282385439, - "y": 1.9867952525471149, - "heading": -0.3348300100346558, - "angularVelocity": -1.007976439532523, - "velocityX": -3.134963986234081, - "velocityY": -1.3999435478765458, - "timestamp": 4.417826355716054 - }, - { - "x": 6.932416862636984, - "y": 1.8843357350106482, - "heading": -0.3485403575636041, - "angularVelocity": -0.20983746552376023, - "velocityX": -3.3616367917885555, - "velocityY": -1.5681473743275887, - "timestamp": 4.48316429474771 - }, - { - "x": 6.70958980281502, - "y": 1.7775224962078198, - "heading": -0.34854038203919246, - "angularVelocity": -3.7459994478515855e-7, - "velocityX": -3.410377846690442, - "velocityY": -1.6347812677571678, - "timestamp": 4.548502233779367 - }, - { - "x": 6.478027967608038, - "y": 1.6912665812341474, - "heading": -0.3485404067374628, - "angularVelocity": -3.780081020278718e-7, - "velocityX": -3.5440639640437577, - "velocityY": -1.3201505320191125, - "timestamp": 4.613840172811023 - }, - { - "x": 6.239619726654668, - "y": 1.6262859956866853, - "heading": -0.34854043255776096, - "angularVelocity": -3.951807866461482e-7, - "velocityX": -3.6488485019072954, - "velocityY": -0.9945306893745133, - "timestamp": 4.679178111842679 - }, - { - "x": 5.996315346781838, - "y": 1.5831125822372847, - "heading": -0.3485404605967219, - "angularVelocity": -4.291375179720768e-7, - "velocityX": -3.7237841211206324, - "velocityY": -0.6607709714945885, - "timestamp": 4.744516050874336 - }, - { - "x": 5.7501052372367845, - "y": 1.5620996014320112, - "heading": -0.34854049237288, - "angularVelocity": -4.863354823397662e-7, - "velocityX": -3.7682564401941674, - "velocityY": -0.3216045855853207, - "timestamp": 4.809853989905992 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { + "timestamp": 5.217921403952478, + "isStopPoint": false, "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.348540530032263, - "angularVelocity": -5.763784949174497e-7, - "velocityX": -3.7819013522532754, - "velocityY": 0.02019321393800792, - "timestamp": 4.875191928937649 - }, - { - "x": 5.265773365103949, - "y": 1.585443267719361, - "heading": -0.3485405619317938, - "angularVelocity": -5.063689324990554e-7, - "velocityX": -3.7657613238076486, - "velocityY": 0.34961055977354083, - "timestamp": 4.9381885481486885 - }, - { - "x": 5.031364041570911, - "y": 1.6280522215500413, - "heading": -0.3485405890146467, - "angularVelocity": -4.299096234955573e-7, - "velocityX": -3.720982593490641, - "velocityY": 0.6763688966853497, - "timestamp": 5.001185167359728 - }, - { - "x": 4.801558310439742, - "y": 1.6909217775230576, - "heading": -0.3485406130254673, - "angularVelocity": -3.811445901784285e-7, - "velocityX": -3.6479057766785146, - "velocityY": 0.9979830149678419, - "timestamp": 5.064181786570768 - }, - { - "x": 4.57810382416931, - "y": 1.7735737496127562, - "heading": -0.34854063513060685, - "angularVelocity": -3.5089406173482316e-7, - "velocityX": -3.5470869559183034, - "velocityY": 1.3120064715347763, - "timestamp": 5.127178405781808 - }, - { - "x": 4.3626998372206165, - "y": 1.8753793427019887, - "heading": -0.34854065619045194, - "angularVelocity": -3.3430119484907394e-7, - "velocityX": -3.4192943946259384, - "velocityY": 1.6160485175907584, - "timestamp": 5.190175024992848 - }, - { - "x": 4.156979523974483, - "y": 1.9955559179970916, - "heading": -0.34854067691382595, - "angularVelocity": -3.2896009785914497e-7, - "velocityX": -3.265577039253029, - "velocityY": 1.907667058965506, - "timestamp": 5.253171644203888 - }, - { - "x": 3.9516956402233028, - "y": 2.116476485340398, - "heading": -0.3485406976311522, - "angularVelocity": -3.288640962267947e-7, - "velocityX": -3.258649215182055, - "velocityY": 1.9194770903216498, - "timestamp": 5.316168263414927 - }, - { - "x": 3.746411773108691, - "y": 2.2373970809271935, - "heading": -0.34854071834847844, - "angularVelocity": -3.288640958736373e-7, - "velocityX": -3.2586489510953776, - "velocityY": 1.9194775386550607, - "timestamp": 5.379164882625967 - }, - { - "x": 3.5411278789507334, - "y": 2.3583176306028646, - "heading": -0.34854073906621164, - "angularVelocity": -3.2887055643589624e-7, - "velocityX": -3.2586493803778747, - "velocityY": 1.919476809867947, - "timestamp": 5.442161501837007 - }, - { - "x": 3.345073520020233, - "y": 2.47307412374708, - "heading": -0.3873766723080469, - "angularVelocity": -0.6164764669629881, - "velocityX": -3.1121409590840567, - "velocityY": 1.821629391884994, - "timestamp": 5.505158121048047 - }, - { - "x": 3.1664636949700515, - "y": 2.5778338770119946, - "heading": -0.44780580157610866, - "angularVelocity": -0.9592440042793773, - "velocityX": -2.835228735876023, - "velocityY": 1.6629424654356146, - "timestamp": 5.568154740259087 - }, - { - "x": 3.0057996639738764, - "y": 2.672135804524931, - "heading": -0.5103083460228613, - "angularVelocity": -0.9921571225491842, - "velocityX": -2.5503595749788768, - "velocityY": 1.496936322837019, - "timestamp": 5.631151359470127 - }, - { - "x": 2.8630598597209174, - "y": 2.7559513152021253, - "heading": -0.5701467016897592, - "angularVelocity": -0.9498661422835671, - "velocityX": -2.265832770720234, - "velocityY": 1.3304763291568347, - "timestamp": 5.694147978681166 - }, - { - "x": 2.738214483951598, - "y": 2.829279302871158, - "heading": -0.6251012943842897, - "angularVelocity": -0.8723419348970285, - "velocityX": -1.9817789800923042, - "velocityY": 1.1639987762419943, - "timestamp": 5.757144597892206 - }, - { - "x": 2.6312405711995455, - "y": 2.892122971419519, - "heading": -0.6738749327170909, - "angularVelocity": -0.7742262829916016, - "velocityX": -1.698089740239665, - "velocityY": 0.9975720814133604, - "timestamp": 5.820141217103246 - }, - { - "x": 2.5421208919645824, - "y": 2.9444859859038597, - "heading": -0.7156160888836572, - "angularVelocity": -0.6625935913597552, - "velocityX": -1.414673999193671, - "velocityY": 0.8312035652091746, - "timestamp": 5.883137836314286 - }, - { - "x": 2.4708422721118457, - "y": 2.98637166116498, - "heading": -0.749723844710458, - "angularVelocity": -0.5414220041322991, - "velocityX": -1.1314673826217794, - "velocityY": 0.6648876683493641, - "timestamp": 5.946134455525326 - }, - { - "x": 2.4173943837507443, - "y": 3.0177828179574866, - "heading": -0.775752501403198, - "angularVelocity": -0.4131754532023893, - "velocityX": -0.84842470961893, - "velocityY": 0.4986165477750299, - "timestamp": 6.0091310747363655 - }, - { - "x": 2.3817689275168203, - "y": 3.038721799552771, - "heading": -0.7933591587514436, - "angularVelocity": -0.2794857496917854, - "velocityX": -0.565513779629629, - "velocityY": 0.3323826239808009, - "timestamp": 6.072127693947405 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.14148780267529462, - "velocityX": -0.28271125846882317, - "velocityY": 0.16617910323716242, - "timestamp": 6.135124313158445 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 }, { + "timestamp": 6.6188601423288445, + "isStopPoint": true, "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": 1.2572572851920672e-28, - "velocityX": 2.1335040537014396e-30, - "velocityY": 1.8634721866519574e-29, - "timestamp": 6.198120932369485 - }, - { - "x": 2.384240228496088, - "y": 3.0394763007756747, - "heading": -0.7921549333865273, - "angularVelocity": 0.15389773911589558, - "velocityX": 0.30849818872977186, - "velocityY": -0.14776374894381028, - "timestamp": 6.263862499797187 - }, - { - "x": 2.4248103079231944, - "y": 3.020044385403153, - "heading": -0.7722377725963315, - "angularVelocity": 0.3029614529969845, - "velocityX": 0.6171145747585456, - "velocityY": -0.2955803479114183, - "timestamp": 6.329604067224889 - }, - { - "x": 2.485678594891513, - "y": 2.9908905714242224, - "heading": -0.7429157303412026, - "angularVelocity": 0.4460198228065572, - "velocityX": 0.9258721589693997, - "velocityY": -0.4434608896569423, - "timestamp": 6.395345634652592 - }, - { - "x": 2.5668563689761195, - "y": 2.952009793968192, - "heading": -0.7046902372675247, - "angularVelocity": 0.581450892781281, - "velocityX": 1.2348013176576613, - "velocityY": -0.5914184735371368, - "timestamp": 6.461087202080294 - }, - { - "x": 2.668357540158173, - "y": 2.903395952391573, - "heading": -0.6582160423239786, - "angularVelocity": 0.706922526522589, - "velocityX": 1.5439420621310505, - "velocityY": -0.7394688547710901, - "timestamp": 6.526828769507996 - }, - { - "x": 2.790199449693154, - "y": 2.8450417291439147, - "heading": -0.604379415056612, - "angularVelocity": 0.8189130465526614, - "velocityX": 1.8533465857651596, - "velocityY": -0.8876305438234711, - "timestamp": 6.592570336935698 - }, - { - "x": 2.9324037360004773, - "y": 2.7769385796688133, - "heading": -0.5444410242237507, - "angularVelocity": 0.9117274378767651, - "velocityX": 2.1630802530485744, - "velocityY": -1.0359222047754857, - "timestamp": 6.6583119043634005 - }, - { - "x": 3.094996415116772, - "y": 2.6990776165764148, - "heading": -0.4803328988113282, - "angularVelocity": 0.975153588829843, - "velocityX": 2.473209652250889, - "velocityY": -1.1843490524929314, - "timestamp": 6.724053471791103 - }, - { - "x": 3.2780012401054357, - "y": 2.6114549065561117, - "heading": -0.4154109268291881, - "angularVelocity": 0.9875330711202905, - "velocityX": 2.783700361113535, - "velocityY": -1.3328357300982285, - "timestamp": 6.789795039218805 - }, - { - "x": 3.48136962382921, - "y": 2.514109658078373, - "heading": -0.3571766392011983, - "angularVelocity": 0.8858061939583661, - "velocityX": 3.0934520073228393, - "velocityY": -1.4807260046665736, - "timestamp": 6.855536606646507 - }, - { - "x": 3.702896130222067, - "y": 2.408427187890042, - "heading": -0.3455558213194451, - "angularVelocity": 0.17676514778162136, - "velocityX": 3.3696565971974572, - "velocityY": -1.6075441204615863, - "timestamp": 6.921278174074209 - }, - { - "x": 3.927079400169571, - "y": 2.300911734169414, - "heading": -0.34555579773751244, - "angularVelocity": 3.5870657763866696e-7, - "velocityX": 3.4100688304100117, - "velocityY": -1.6354257728775006, - "timestamp": 6.9870197415019115 - }, - { - "x": 4.15126266204063, - "y": 2.1933962636083315, - "heading": -0.3455557741556376, - "angularVelocity": 3.587056984423895e-7, - "velocityX": 3.4100687075585983, - "velocityY": -1.6354260290389449, - "timestamp": 7.052761308929614 - }, - { - "x": 4.375445923911402, - "y": 2.0858807930466483, - "heading": -0.3455557505737627, - "angularVelocity": 3.5870569914452407e-7, - "velocityX": 3.4100687075542147, - "velocityY": -1.6354260290480847, - "timestamp": 7.118502876357316 - }, - { - "x": 4.599629185782173, - "y": 1.978365322484965, - "heading": -0.34555572699188775, - "angularVelocity": 3.5870569932883336e-7, - "velocityX": 3.4100687075542147, - "velocityY": -1.6354260290480853, - "timestamp": 7.184244443785018 - }, - { - "x": 4.823812447652946, - "y": 1.8708498519232832, - "heading": -0.3455557034100129, - "angularVelocity": 3.5870569853758494e-7, - "velocityX": 3.4100687075542258, - "velocityY": -1.6354260290480622, - "timestamp": 7.24998601121272 - }, - { - "x": 5.047995709543858, - "y": 1.7633343814035949, - "heading": -0.34555567982813806, - "angularVelocity": 3.5870569820699746e-7, - "velocityX": 3.410068707860571, - "velocityY": -1.6354260284092945, - "timestamp": 7.315727578640423 - }, - { - "x": 5.27217953587615, - "y": 1.65582008782303, - "heading": -0.34555565624625534, - "angularVelocity": 3.587058178727361e-7, - "velocityX": 3.410077293621499, - "velocityY": -1.6354081258984543, - "timestamp": 7.381469146068125 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 }, { + "timestamp": 7.940893576854948, + "isStopPoint": false, "x": 5.5030035972595215, "y": 1.563418984413147, "heading": -0.34555563246426124, - "angularVelocity": 3.617497277100986e-7, - "velocityX": 3.511082415812739, - "velocityY": -1.405520236667608, - "timestamp": 7.447210713495827 - }, - { - "x": 5.6599765905642645, - "y": 1.5113593672660246, - "heading": -0.34555563704903824, - "angularVelocity": -1.0484559207434358e-7, - "velocityX": 3.589689630249378, - "velocityY": -1.1905096787253004, - "timestamp": 7.490939561801255 - }, - { - "x": 5.819811381479902, - "y": 1.4688909784273936, - "heading": -0.3455556415836398, - "angularVelocity": -1.0369817036745698e-7, - "velocityX": 3.6551337871799947, - "velocityY": -0.9711755622285476, - "timestamp": 7.534668410106684 - }, - { - "x": 5.98037051193366, - "y": 1.4292489543347602, - "heading": -0.34555564611341977, - "angularVelocity": -1.0358790889899858e-7, - "velocityX": 3.6716981277969567, - "velocityY": -0.9065416910994355, - "timestamp": 7.578397258412112 - }, - { - "x": 6.140929731813359, - "y": 1.389607292439096, - "heading": -0.3455556506431997, - "angularVelocity": -1.0358790900922708e-7, - "velocityX": 3.6717001728071876, - "velocityY": -0.9065334083071072, - "timestamp": 7.62212610671754 - }, - { - "x": 6.30148895170346, - "y": 1.3499656305855623, - "heading": -0.34555565517297965, - "angularVelocity": -1.0358790856487398e-7, - "velocityX": 3.6717001730450596, - "velocityY": -0.9065334073436624, - "timestamp": 7.665854955022969 - }, - { - "x": 6.462048171593562, - "y": 1.3103239687320334, - "heading": -0.34555565970275964, - "angularVelocity": -1.0358791005302326e-7, - "velocityX": 3.671700173045087, - "velocityY": -0.9065334073435504, - "timestamp": 7.709583803328397 - }, - { - "x": 6.622607391483664, - "y": 1.2706823068785016, - "heading": -0.34555566423253964, - "angularVelocity": -1.0358790893463982e-7, - "velocityX": 3.6717001730450707, - "velocityY": -0.9065334073436169, - "timestamp": 7.753312651633825 - }, - { - "x": 6.783166611367605, - "y": 1.2310406450000155, - "heading": -0.3455556687623196, - "angularVelocity": -1.0358790885289903e-7, - "velocityX": 3.6717001729041776, - "velocityY": -0.9065334079142718, - "timestamp": 7.797041499939254 - }, - { - "x": 6.943725778284007, - "y": 1.1913987685896004, - "heading": -0.34555567329209946, - "angularVelocity": -1.0358790829702175e-7, - "velocityX": 3.6716989616319724, - "velocityY": -0.9065383138730976, - "timestamp": 7.840770348244682 - }, - { - "x": 7.103836002524736, - "y": 1.1499808922348331, - "heading": -0.34555567791225333, - "angularVelocity": -1.0565459632350647e-7, - "velocityX": 3.661432451237353, - "velocityY": -0.9471522338178334, - "timestamp": 7.88449919655011 - }, - { - "x": 7.257955684496521, - "y": 1.1005509138662795, - "heading": -0.346747423550466, - "angularVelocity": -0.02725307627333064, - "velocityX": 3.524439539210384, - "velocityY": -1.1303745761449258, - "timestamp": 7.928228044855539 - }, - { - "x": 7.402334567868656, - "y": 1.0534126604700325, - "heading": -0.34675046354447403, - "angularVelocity": -0.00006951918758018307, - "velocityX": 3.3016850195483918, - "velocityY": -1.07796695369166, - "timestamp": 7.971956893160967 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { + "timestamp": 8.524794231937184, + "isStopPoint": false, "x": 7.536985397338867, "y": 1.0086194276809692, "heading": -0.34555563246426124, - "angularVelocity": 0.027323634774631837, - "velocityX": 3.079221948168643, - "velocityY": -1.024340556060407, - "timestamp": 8.015685741466395 - }, - { - "x": 7.709533523799992, - "y": 0.9494390432841444, - "heading": -0.3413965784412158, - "angularVelocity": 0.06656216442856697, - "velocityX": 2.7614877570011993, - "velocityY": -0.9471323179117597, - "timestamp": 8.078169497196857 - }, - { - "x": 7.862380855125707, - "y": 0.8957055369328039, - "heading": -0.33682515317576994, - "angularVelocity": 0.07316181961221624, - "velocityX": 2.4461930871290236, - "velocityY": -0.8599596122731851, - "timestamp": 8.140653252927319 - }, - { - "x": 7.99561550345175, - "y": 0.8477270543434197, - "heading": -0.33285887044966567, - "angularVelocity": 0.06347702182330028, - "velocityX": 2.1323085779411386, - "velocityY": -0.7678552934037809, - "timestamp": 8.20313700865778 - }, - { - "x": 8.109294083490143, - "y": 0.8056872786590918, - "heading": -0.33010810532148027, - "angularVelocity": 0.04402368417243478, - "velocityX": 1.8193301396409471, - "velocityY": -0.6728112801937814, - "timestamp": 8.265620764388242 - }, - { - "x": 8.20345588526635, - "y": 0.7697081195865146, - "heading": -0.32897987990878186, - "angularVelocity": 0.018056299585531694, - "velocityX": 1.5069805051795295, - "velocityY": -0.5758162045793389, - "timestamp": 8.328104520118703 - }, - { - "x": 8.278129725528439, - "y": 0.7398763726381793, - "heading": -0.32976503867911666, - "angularVelocity": -0.012565806282865027, - "velocityX": 1.1950920585537632, - "velocityY": -0.4774320397291953, - "timestamp": 8.390588275849165 - }, - { - "x": 8.333337625588607, - "y": 0.7162569714576368, - "heading": -0.33268181216540116, - "angularVelocity": -0.04668050843272986, - "velocityX": 0.883556044523325, - "velocityY": -0.37800866648333437, - "timestamp": 8.453072031579627 - }, - { - "x": 8.369096953381794, - "y": 0.698900317224484, - "heading": -0.33790002427933163, - "angularVelocity": -0.08351309957167831, - "velocityX": 0.572297989695798, - "velocityY": -0.2777786647144699, - "timestamp": 8.515555787310088 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 }, { + "timestamp": 9.144169862516533, + "isStopPoint": false, "x": 8.385421752929688, "y": 0.6878466606140137, "heading": -0.34555563246426124, - "angularVelocity": -0.12252157533477633, - "velocityX": 0.26126469763300175, - "velocityY": -0.1769044847136382, - "timestamp": 8.57803954304055 - }, - { - "x": 8.379560991493735, - "y": 0.6833892008568203, - "heading": -0.35721868503670856, - "angularVelocity": -0.1678763417451177, - "velocityX": -0.08435897751441682, - "velocityY": -0.06416005011256522, - "timestamp": 8.64751361217505 - }, - { - "x": 8.349687812290387, - "y": 0.6867654591427064, - "heading": -0.37193021085772177, - "angularVelocity": -0.2117556378126066, - "velocityX": -0.42999034856465757, - "velocityY": 0.048597387887987537, - "timestamp": 8.71698768130955 - }, - { - "x": 8.295801597591089, - "y": 0.6979765367761911, - "heading": -0.38956490915625874, - "angularVelocity": -0.253831372168465, - "velocityX": -0.7756306111129838, - "velocityY": 0.1613706779112114, - "timestamp": 8.786461750444051 - }, - { - "x": 8.217901630671626, - "y": 0.7170238074049998, - "heading": -0.40996601054380316, - "angularVelocity": -0.2936505899495876, - "velocityX": -1.121281190089058, - "velocityY": 0.2741637400269978, - "timestamp": 8.855935819578551 - }, - { - "x": 8.115987079646073, - "y": 0.7439090305870278, - "heading": -0.4329315185089462, - "angularVelocity": -0.3305622982969707, - "velocityX": -1.4669437431144206, - "velocityY": 0.3869821289721611, - "timestamp": 8.925409888713052 - }, - { - "x": 7.990056990056933, - "y": 0.778634533730576, - "heading": -0.45819105339643357, - "angularVelocity": -0.36358219983610734, - "velocityX": -1.812620034466978, - "velocityY": 0.4998340183057432, - "timestamp": 8.994883957847552 - }, - { - "x": 7.840110319883288, - "y": 0.8212035218059903, - "heading": -0.4853637226253089, - "angularVelocity": -0.3911195870256229, - "velocityX": -2.1583113245224226, - "velocityY": 0.6127320395326444, - "timestamp": 9.064358026982053 - }, - { - "x": 7.6661461501938755, - "y": 0.8716206459377261, - "heading": -0.5138727598830971, - "angularVelocity": -0.4103550808661484, - "velocityX": -2.504015841545443, - "velocityY": 0.7256970083921437, - "timestamp": 9.133832096116553 - }, - { - "x": 7.468164698581641, - "y": 0.9298931437973682, - "heading": -0.5427426487462225, - "angularVelocity": -0.41554912822558204, - "velocityX": -2.8497172265661783, - "velocityY": 0.8387661552805812, - "timestamp": 9.203306165251053 - }, - { - "x": 7.24617329762414, - "y": 0.9960332266921984, - "heading": -0.5699703376985038, - "angularVelocity": -0.391911533201964, - "velocityX": -3.1953130674947268, - "velocityY": 0.9520110700121011, - "timestamp": 9.272780234385554 - }, - { - "x": 7.00025853984304, - "y": 1.0700564595014088, - "heading": -0.589089272180988, - "angularVelocity": -0.2751952594783323, - "velocityX": -3.53966250782022, - "velocityY": 1.065480023430083, - "timestamp": 9.342254303520054 - }, - { - "x": 6.750442302486931, - "y": 1.1514706650489683, - "heading": -0.5890892788822688, - "angularVelocity": -9.645729514768673e-8, - "velocityX": -3.5958198572257243, - "velocityY": 1.171864647656428, - "timestamp": 9.411728372654554 - }, - { - "x": 6.500626129709104, - "y": 1.232885068752249, - "heading": -0.5890892855834843, - "angularVelocity": -9.645635490096592e-8, - "velocityX": -3.5958189276949932, - "velocityY": 1.1718674998820673, - "timestamp": 9.481202441789055 - }, - { - "x": 6.250809956933241, - "y": 1.3142994724615544, - "heading": -0.5890892922846998, - "angularVelocity": -9.645635424551899e-8, - "velocityX": -3.595818927666732, - "velocityY": 1.1718674999687875, - "timestamp": 9.550676510923555 - }, - { - "x": 6.000993784158921, - "y": 1.395713876175591, - "heading": -0.589089298985915, - "angularVelocity": -9.645635348778811e-8, - "velocityX": -3.595818927644538, - "velocityY": 1.1718675000368866, - "timestamp": 9.620150580058056 - }, - { - "x": 5.751177662095486, - "y": 1.477128435493372, - "heading": -0.5890893056871302, - "angularVelocity": -9.64563521650664e-8, - "velocityX": -3.5958181977191597, - "velocityY": 1.1718697397753421, - "timestamp": 9.689624649192556 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { + "timestamp": 10.397632030266404, + "isStopPoint": false, "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.5890893123960679, - "angularVelocity": -9.65675052838285e-8, - "velocityX": -3.5721826564600034, - "velocityY": 1.242054049730676, - "timestamp": 9.759098718327056 - }, - { - "x": 5.287967104202961, - "y": 1.6596756394253018, - "heading": -0.5890893181956361, - "angularVelocity": -9.309834476919773e-8, - "velocityX": -3.4519020760085812, - "velocityY": 1.5451728334255468, - "timestamp": 9.821393791687077 - }, - { - "x": 5.074932733784246, - "y": 1.7602857483579184, - "heading": -0.5890893239791856, - "angularVelocity": -9.284120212486628e-8, - "velocityX": -3.419762734486735, - "velocityY": 1.6150572349624415, - "timestamp": 9.883688865047098 - }, - { - "x": 4.861898448797989, - "y": 1.860896038187167, - "heading": -0.5890893297627348, - "angularVelocity": -9.284119839047264e-8, - "velocityX": -3.4197613630707817, - "velocityY": 1.615060138829829, - "timestamp": 9.94598393840712 - }, - { - "x": 4.648864163815087, - "y": 1.9615063280235205, - "heading": -0.5890893355462841, - "angularVelocity": -9.28411976539737e-8, - "velocityX": -3.419761363016917, - "velocityY": 1.6150601389438841, - "timestamp": 10.00827901176714 - }, - { - "x": 4.435829878832186, - "y": 2.0621166178598744, - "heading": -0.5890893413298333, - "angularVelocity": -9.28411982771023e-8, - "velocityX": -3.419761363016914, - "velocityY": 1.6150601389438883, - "timestamp": 10.070574085127161 - }, - { - "x": 4.222795593849285, - "y": 2.162726907696228, - "heading": -0.5890893471133825, - "angularVelocity": -9.284119843074624e-8, - "velocityX": -3.4197613630169146, - "velocityY": 1.6150601389438883, - "timestamp": 10.132869158487182 - }, - { - "x": 4.009761308866383, - "y": 2.2633371975325822, - "heading": -0.5890893528969318, - "angularVelocity": -9.284119832193768e-8, - "velocityX": -3.419761363016914, - "velocityY": 1.6150601389438886, - "timestamp": 10.195164231847203 - }, - { - "x": 3.7967270238835944, - "y": 2.363947487369175, - "heading": -0.589089358680481, - "angularVelocity": -9.284119741513914e-8, - "velocityX": -3.4197613630151045, - "velocityY": 1.615060138947721, - "timestamp": 10.257459305207224 - }, - { - "x": 3.583692741771847, - "y": 2.464557783284562, - "heading": -0.5890893644643718, - "angularVelocity": -9.284668184194445e-8, - "velocityX": -3.419761316927328, - "velocityY": 1.615060236528384, - "timestamp": 10.319754378567245 - }, - { - "x": 3.381763406617466, - "y": 2.559912098840239, - "heading": -0.6245815423960026, - "angularVelocity": -0.5697429349912143, - "velocityX": -3.241497670086629, - "velocityY": 1.530687908570203, - "timestamp": 10.382049451927266 - }, - { - "x": 3.1981911985495857, - "y": 2.646597580557341, - "heading": -0.6568615726262876, - "angularVelocity": -0.5181795042399261, - "velocityX": -2.9468174314037126, - "velocityY": 1.3915302935133123, - "timestamp": 10.444344525287287 - }, - { - "x": 3.032976151851517, - "y": 2.7246143065734305, - "heading": -0.6859252496642307, - "angularVelocity": -0.46654856428174046, - "velocityX": -2.652136642383354, - "velocityY": 1.2523739327700791, - "timestamp": 10.506639598647308 - }, - { - "x": 2.8861182875087947, - "y": 2.793962348685928, - "heading": -0.7117686923384959, - "angularVelocity": -0.414855321301394, - "velocityX": -2.3574555164898965, - "velocityY": 1.1132187245645664, - "timestamp": 10.568934672007329 - }, - { - "x": 2.7576176211174075, - "y": 2.854641769350887, - "heading": -0.7343883425051251, - "angularVelocity": -0.3631049607390947, - "velocityX": -2.0627741402397475, - "velocityY": 0.9740645189431933, - "timestamp": 10.63122974536735 - }, - { - "x": 2.6474741650844584, - "y": 2.906652621199817, - "heading": -0.7537810666899133, - "angularVelocity": -0.31130429966287193, - "velocityX": -1.7680925648228973, - "velocityY": 0.8349111581961731, - "timestamp": 10.69352481872737 - }, - { - "x": 2.5556879298075064, - "y": 2.9499949470036255, - "heading": -0.7699442582183806, - "angularVelocity": -0.2594617945957906, - "velocityX": -1.473410822497863, - "velocityY": 0.6957584840348696, - "timestamp": 10.755819892087391 - }, - { - "x": 2.482258924484474, - "y": 2.9846687797700127, - "heading": -0.7828759248842103, - "angularVelocity": -0.20758730937026382, - "velocityX": -1.1787289325219326, - "velocityY": 0.556606339734085, - "timestamp": 10.818114965447412 - }, - { - "x": 2.4271871577101587, - "y": 3.010674142880251, - "heading": -0.7925747589157716, - "angularVelocity": -0.15569183096565328, - "velocityX": -0.8840469045768797, - "velocityY": 0.4174545707643066, - "timestamp": 10.880410038807433 - }, - { - "x": 2.3904726379075623, - "y": 3.0280110502230517, - "heading": -0.7990401886427835, - "angularVelocity": -0.10378717574737188, - "velocityX": -0.5893647414204509, - "velocityY": 0.27830302474492646, - "timestamp": 10.942705112167454 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -0.05188569757551244, - "velocityX": -0.29468244125856385, - "velocityY": 0.13915155101802326, - "timestamp": 11.005000185527475 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 }, { + "timestamp": 11.778014028106803, + "isStopPoint": true, "x": 2.37211537361145, "y": 3.03667950630188, "heading": -0.8022724119795859, - "angularVelocity": -7.343237406242101e-30, - "velocityX": 6.07048254686404e-33, - "velocityY": -1.403576741314563e-31, - "timestamp": 11.067295258887496 - }, - { - "x": 2.3887780433959134, - "y": 3.0288255570872233, - "heading": -0.7958553483876907, - "angularVelocity": 0.1079808929405059, - "velocityX": 0.2803852472906352, - "velocityY": -0.13215958314273293, - "timestamp": 11.126723028163784 - }, - { - "x": 2.422106198393558, - "y": 3.0131153644204134, - "heading": -0.7831901230540868, - "angularVelocity": 0.2131196490779077, - "velocityX": 0.5608178702232047, - "velocityY": -0.26435777176444086, - "timestamp": 11.186150797440073 - }, - { - "x": 2.47210314120033, - "y": 2.9895461805025576, - "heading": -0.7644823111559433, - "angularVelocity": 0.3147991608305137, - "velocityX": 0.8413060664338303, - "velocityY": -0.3966021980108423, - "timestamp": 11.245578566716361 - }, - { - "x": 2.5387727913774722, - "y": 2.9581147004806385, - "heading": -0.7399860035384287, - "angularVelocity": 0.41220304776557914, - "velocityX": 1.1218602176902186, - "velocityY": -0.5289022355153943, - "timestamp": 11.30500633599265 - }, - { - "x": 2.6221198536798296, - "y": 2.9188169151709924, - "heading": -0.7100217609714737, - "angularVelocity": 0.5042128104749645, - "velocityX": 1.4024935365630693, - "velocityY": -0.6612697361556471, - "timestamp": 11.364434105268938 - }, - { - "x": 2.7221500309650937, - "y": 2.871647898493915, - "heading": -0.6750052177423963, - "angularVelocity": 0.5892286326010091, - "velocityX": 1.6832228182788898, - "velocityY": -0.7937201286790226, - "timestamp": 11.423861874545226 - }, - { - "x": 2.8388702680559588, - "y": 2.816601499667555, - "heading": -0.6354955831852485, - "angularVelocity": 0.6648345552665271, - "velocityX": 1.9640689615693319, - "velocityY": -0.9262740213325141, - "timestamp": 11.483289643821514 - }, - { - "x": 2.9722889163933544, - "y": 2.753669910841413, - "heading": -0.5922850618953597, - "angularVelocity": 0.7271099322127873, - "velocityX": 2.2450556358107243, - "velocityY": -1.0589592978591336, - "timestamp": 11.542717413097803 - }, - { - "x": 3.122415239721002, - "y": 2.6828431673634756, - "heading": -0.5465844996458821, - "angularVelocity": 0.7690102254582669, - "velocityX": 2.5261981924591717, - "velocityY": -1.191812251081979, - "timestamp": 11.602145182374091 - }, - { - "x": 3.289254937095535, - "y": 2.6041094136623544, - "heading": -0.5004858972625192, - "angularVelocity": 0.7757081065791619, - "velocityX": 2.8074366479896216, - "velocityY": -1.3248646997853857, - "timestamp": 11.66157295165038 - }, - { - "x": 3.4727741175032825, - "y": 2.5174653392644726, - "heading": -0.45856185133391086, - "angularVelocity": 0.7054622180728715, - "velocityX": 3.0881048143423393, - "velocityY": -1.4579728543249981, - "timestamp": 11.721000720926668 - }, - { - "x": 3.672087548521264, - "y": 2.4232619622496343, - "heading": -0.44156909021440377, - "angularVelocity": 0.2859397437680458, - "velocityX": 3.353877041746403, - "velocityY": -1.5851743749110756, - "timestamp": 11.780428490202956 - }, - { - "x": 3.87540622002593, - "y": 2.327473100540026, - "heading": -0.4415690736830568, - "angularVelocity": 2.781754584465502e-7, - "velocityX": 3.4212738250264785, - "velocityY": -1.6118535640173974, - "timestamp": 11.839856259479244 - }, - { - "x": 4.078724897772698, - "y": 2.231684252079618, - "heading": -0.4415690571518241, - "angularVelocity": 2.781735362772021e-7, - "velocityX": 3.4212739300632746, - "velocityY": -1.6118533410711124, - "timestamp": 11.899284028755533 - }, - { - "x": 4.282043575519764, - "y": 2.135895403619844, - "heading": -0.4415690406205913, - "angularVelocity": 2.7817353604412086e-7, - "velocityX": 3.4212739300683004, - "velocityY": -1.611853341060444, - "timestamp": 11.958711798031821 - }, - { - "x": 4.485362253266831, - "y": 2.0401065551600697, - "heading": -0.44156902408935866, - "angularVelocity": 2.781735361125895e-7, - "velocityX": 3.421273930068301, - "velocityY": -1.611853341060444, - "timestamp": 12.01813956730811 - }, - { - "x": 4.688680931013898, - "y": 1.9443177067002955, - "heading": -0.4415690075581259, - "angularVelocity": 2.781735368678374e-7, - "velocityX": 3.421273930068301, - "velocityY": -1.611853341060444, - "timestamp": 12.077567336584398 - }, - { - "x": 4.8919996087609645, - "y": 1.8485288582405217, - "heading": -0.4415689910268932, - "angularVelocity": 2.7817353639391735e-7, - "velocityX": 3.4212739300683026, - "velocityY": -1.6118533410604396, - "timestamp": 12.136995105860686 - }, - { - "x": 5.0953182865105076, - "y": 1.7527400097860029, - "heading": -0.4415689744956604, - "angularVelocity": 2.781735367341045e-7, - "velocityX": 3.4212739301099617, - "velocityY": -1.611853340972016, - "timestamp": 12.196422875136975 - }, - { - "x": 5.298637015997747, - "y": 1.656951271148522, - "heading": -0.4415689579644276, - "angularVelocity": 2.7817353744823303e-7, - "velocityX": 3.4212748007079674, - "velocityY": -1.6118514930642196, - "timestamp": 12.255850644413263 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 }, { + "timestamp": 13.097725368835128, + "isStopPoint": false, "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.44156894141194364, - "angularVelocity": 2.7853113329051353e-7, - "velocityX": 3.4389071599112104, - "velocityY": -1.5738818379758603, - "timestamp": 12.315278413689551 - }, - { - "x": 5.76887189670182, - "y": 1.4734081866468696, - "heading": -0.441568927295876, - "angularVelocity": 1.9019557057481577e-7, - "velocityX": 3.5822280134028937, - "velocityY": -1.2127779127617127, - "timestamp": 12.389497111786811 - }, - { - "x": 6.03546847764148, - "y": 1.3855778991398175, - "heading": -0.4415689131909676, - "angularVelocity": 1.9004521532739413e-7, - "velocityX": 3.5920406551769672, - "velocityY": -1.1833983855651649, - "timestamp": 12.463715809884071 - }, - { - "x": 6.302065075021444, - "y": 1.2977476615350296, - "heading": -0.441568899086059, - "angularVelocity": 1.900452165326357e-7, - "velocityX": 3.592040876688587, - "velocityY": -1.1833977131973445, - "timestamp": 12.537934507981332 - }, - { - "x": 6.568661672398624, - "y": 1.2099174239217916, - "heading": -0.44156888498115054, - "angularVelocity": 1.9004521568233082e-7, - "velocityX": 3.5920408766510765, - "velocityY": -1.1833977133112008, - "timestamp": 12.612153206078592 - }, - { - "x": 6.835258130689826, - "y": 1.1220867641390964, - "heading": -0.44156887087145064, - "angularVelocity": 1.9010977386064987e-7, - "velocityX": 3.5920390026492015, - "velocityY": -1.1834034014931274, - "timestamp": 12.686371904175852 - }, - { - "x": 7.080885784636739, - "y": 1.0408339813622511, - "heading": -0.35950242791659653, - "angularVelocity": 1.105738109919598, - "velocityX": 3.3095117570646315, - "velocityY": -1.0947751019611969, - "timestamp": 12.760590602273112 - }, - { - "x": 7.299180381748869, - "y": 0.9686950138524031, - "heading": -0.28235870642192235, - "angularVelocity": 1.03941086912835, - "velocityX": 2.9412345232203254, - "velocityY": -0.9719783472261578, - "timestamp": 12.834809300370372 - }, - { - "x": 7.490166378274822, - "y": 0.9056069788063349, - "heading": -0.2132225753720476, - "angularVelocity": 0.9315190487378978, - "velocityX": 2.5732868053771534, - "velocityY": -0.8500288561164988, - "timestamp": 12.909027998467632 - }, - { - "x": 7.653856153896351, - "y": 0.8515488890315335, - "heading": -0.15310137838681676, - "angularVelocity": 0.8100545890297152, - "velocityX": 2.2055058875732163, - "velocityY": -0.7283621400089914, - "timestamp": 12.983246696564892 - }, - { - "x": 7.790256705792751, - "y": 0.8065104939114175, - "heading": -0.10249736767748478, - "angularVelocity": 0.6818229369001992, - "velocityX": 1.8378192476193804, - "velocityY": -0.6068335375688586, - "timestamp": 13.057465394662152 - }, - { - "x": 7.899372493751606, - "y": 0.7704858089539702, - "heading": -0.06171206576746063, - "angularVelocity": 0.5495286626665922, - "velocityX": 1.470192697477151, - "velocityY": -0.48538556834071367, - "timestamp": 13.131684092759413 - }, - { - "x": 7.981206582030799, - "y": 0.7434709210905415, - "heading": -0.030946027709333555, - "angularVelocity": 0.4145321710976964, - "velocityX": 1.1026074342068202, - "velocityY": -0.3639903226007307, - "timestamp": 13.205902790856673 - }, - { - "x": 8.035761170601306, - "y": 0.7254630347979746, - "heading": -0.010340878696429398, - "angularVelocity": 0.2776274650631065, - "velocityX": 0.7350518126714517, - "velocityY": -0.24263274288330566, - "timestamp": 13.280121488953933 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": -7.30119772009808e-38, - "angularVelocity": 0.13932983145219902, - "velocityX": 0.3675179222006851, - "velocityY": -0.12130427346551735, - "timestamp": 13.354340187051193 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 16 }, { + "timestamp": 14.282836897445563, + "isStopPoint": true, "x": 8.063037872314453, "y": 0.7164599895477295, - "heading": 1.121139660303252e-31, - "angularVelocity": 1.5105915295246559e-30, - "velocityX": -7.247567295016868e-32, - "velocityY": -3.7967728194239397e-31, - "timestamp": 13.428558885148453 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -16693,37 +17017,44 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 6 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 1 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 11 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "AmpLanePADESprint": { "waypoints": [ @@ -16840,1577 +17171,1711 @@ { "x": 0.43297290802001953, "y": 6.9807281494140625, - "heading": 0, - "angularVelocity": 2.735053895521923e-38, - "velocityX": -3.8673469053295564e-39, - "velocityY": 1.858643532249755e-38, + "heading": 6.831435290856854e-28, + "angularVelocity": 9.118091433542868e-28, + "velocityX": -1.02680113599063e-27, + "velocityY": -2.9257138265977544e-27, "timestamp": 0 }, { - "x": 0.46193105617219093, - "y": 6.976128411049919, - "heading": 0.007924686694784022, - "angularVelocity": 0.0965568963773332, - "velocityX": 0.35283526252830383, - "velocityY": -0.05604467125266167, - "timestamp": 0.08207271559159549 - }, - { - "x": 0.5198473628641517, - "y": 6.966928755942856, - "heading": 0.023773564700110772, - "angularVelocity": 0.19310775683593598, - "velocityX": 0.7056706516226394, - "velocityY": -0.11209151592888836, - "timestamp": 0.16414543118319097 - }, - { - "x": 0.6067218129336875, - "y": 6.9531289220308485, - "heading": 0.04754806846022335, - "angularVelocity": 0.2896760950181983, - "velocityX": 1.0585058559757474, - "velocityY": -0.16814155365198188, - "timestamp": 0.24621814677478646 - }, - { - "x": 0.7225543617111448, - "y": 6.934728565336742, - "heading": 0.07925217561750111, - "angularVelocity": 0.38629289805690764, - "velocityX": 1.4113405160596262, - "velocityY": -0.22419578250180797, - "timestamp": 0.32829086236638194 - }, - { - "x": 0.8673449286095066, - "y": 6.9117272629835975, - "heading": 0.11889328943914418, - "angularVelocity": 0.48299990485147826, - "velocityX": 1.764174194245729, - "velocityY": -0.2802551638184133, - "timestamp": 0.4103635779579774 - }, - { - "x": 1.0410933872613453, - "y": 6.884124518124938, - "heading": 0.16648343863275697, - "angularVelocity": 0.5798534732349732, - "velocityX": 2.117006332731008, - "velocityY": -0.33632059887008814, - "timestamp": 0.49243629354957286 - }, - { - "x": 1.243799549589456, - "y": 6.851919767180746, - "heading": 0.22204089753091144, - "angularVelocity": 0.6769297018830912, - "velocityX": 2.4698361796239694, - "velocityY": -0.39239290075946437, - "timestamp": 0.5745090091411683 - }, - { - "x": 1.4754631289031934, - "y": 6.815112383244328, - "heading": 0.28559273385120976, - "angularVelocity": 0.7743357321882771, - "velocityX": 2.8226625333871667, - "velocityY": -0.44847284107882956, - "timestamp": 0.6565817247327638 - }, - { - "x": 1.6781593090979035, - "y": 6.782884452250807, - "heading": 0.3422497740441209, - "angularVelocity": 0.6903273491624664, - "velocityX": 2.4697145541443093, - "velocityY": -0.3926753338331338, - "timestamp": 0.7386544403243592 - }, - { - "x": 1.8518984587746443, - "y": 6.755259929387126, - "heading": 0.39086385083963315, - "angularVelocity": 0.5923293319234401, - "velocityX": 2.1168929092256374, - "velocityY": -0.33658594899118865, - "timestamp": 0.8207271559159547 - }, - { - "x": 1.9966808291576696, - "y": 6.732239139219163, - "heading": 0.43140805286474204, - "angularVelocity": 0.4940034179795163, - "velocityX": 1.7640743253029585, - "velocityY": -0.28049260953564537, - "timestamp": 0.9027998715075501 - }, - { - "x": 2.1125066103123933, - "y": 6.7138223061958735, - "heading": 0.463861007608982, - "angularVelocity": 0.39541709458878044, - "velocityX": 1.4112580571975715, - "velocityY": -0.22439653532283038, - "timestamp": 0.9848725870991456 - }, - { - "x": 2.199375929341993, - "y": 6.700009583150749, - "heading": 0.4882072246931022, - "angularVelocity": 0.29664203150374724, - "velocityX": 1.0584433377576143, - "velocityY": -0.16829859893826818, - "timestamp": 1.0669453026907412 - }, - { - "x": 2.257288845375976, - "y": 6.690801066111525, - "heading": 0.5044377142261048, - "angularVelocity": 0.19775743273523133, - "velocityX": 0.7056293387703308, - "velocityY": -0.11219949252132488, - "timestamp": 1.1490180182823366 + "x": 0.46193105617201946, + "y": 6.976128411049744, + "heading": 0.007924686703269965, + "angularVelocity": 0.09655689623933598, + "velocityX": 0.35283526164412465, + "velocityY": -0.056044671114677404, + "timestamp": 0.08207271579677758 + }, + { + "x": 0.5198473628636088, + "y": 6.966928755942304, + "heading": 0.023773564726982017, + "angularVelocity": 0.19310775657717816, + "velocityX": 0.7056706498539357, + "velocityY": -0.11209151565325522, + "timestamp": 0.16414543159355516 + }, + { + "x": 0.6067218129325357, + "y": 6.953128922029678, + "heading": 0.04754806851726075, + "angularVelocity": 0.2896760946615609, + "velocityX": 1.0585058533220593, + "velocityY": -0.16814155323914987, + "timestamp": 0.24621814739033274 + }, + { + "x": 0.7225543617090937, + "y": 6.9347285653346615, + "heading": 0.0792521757191159, + "angularVelocity": 0.386292897634319, + "velocityX": 1.4113405125203116, + "velocityY": -0.22419578195241194, + "timestamp": 0.3282908631871103 + }, + { + "x": 0.8673449286061865, + "y": 6.911727262980233, + "heading": 0.11889328960368833, + "angularVelocity": 0.4829999044107281, + "velocityX": 1.7641741898198249, + "velocityY": -0.2802551631334134, + "timestamp": 0.41036357898388787 + }, + { + "x": 1.0410933872562544, + "y": 6.884124518119783, + "heading": 0.166483438885144, + "angularVelocity": 0.5798534728556428, + "velocityX": 2.1170063274169095, + "velocityY": -0.3363205980510971, + "timestamp": 0.4924362947806654 + }, + { + "x": 1.2437995495818281, + "y": 6.851919767173026, + "heading": 0.2220408979091711, + "angularVelocity": 0.6769297017244366, + "velocityX": 2.4698361734184595, + "velocityY": -0.3923928998097307, + "timestamp": 0.574509010577443 + }, + { + "x": 1.4754631288914648, + "y": 6.815112383232462, + "heading": 0.28559273443277505, + "angularVelocity": 0.7743357327295749, + "velocityX": 2.822662526280537, + "velocityY": -0.44847284000816645, + "timestamp": 0.6565817263742205 + }, + { + "x": 1.6781593090904183, + "y": 6.782884452243251, + "heading": 0.3422497744154681, + "angularVelocity": 0.6903273448752822, + "velocityX": 2.4697145480217175, + "velocityY": -0.3926753327989152, + "timestamp": 0.7386544421709981 + }, + { + "x": 1.8518984587698175, + "y": 6.755259929382264, + "heading": 0.3908638510792468, + "angularVelocity": 0.5923293288375308, + "velocityX": 2.1168929039657844, + "velocityY": -0.3365859481168989, + "timestamp": 0.8207271579677756 + }, + { + "x": 1.9966808291546925, + "y": 6.732239139216171, + "heading": 0.43140805301261054, + "angularVelocity": 0.49400341562665323, + "velocityX": 1.7640743209153051, + "velocityY": -0.2804926088116284, + "timestamp": 0.9027998737645532 + }, + { + "x": 2.1125066103107177, + "y": 6.713822306194192, + "heading": 0.46386100769226224, + "angularVelocity": 0.39541709281327203, + "velocityX": 1.4112580536852743, + "velocityY": -0.2243965347458558, + "timestamp": 0.9848725895613307 + }, + { + "x": 2.1993759293412, + "y": 6.700009583149955, + "heading": 0.4882072247325505, + "angularVelocity": 0.2966420302280796, + "velocityX": 1.0584433351222509, + "velocityY": -0.16829859850671558, + "timestamp": 1.0669453053581084 + }, + { + "x": 2.257288845375724, + "y": 6.690801066111273, + "heading": 0.5044377142386447, + "angularVelocity": 0.197757431912976, + "velocityX": 0.7056293370128478, + "velocityY": -0.11219949223421748, + "timestamp": 1.149018021154886 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 0.0988477756026137, - "velocityX": 0.3528151893675758, - "velocityY": -0.0560997894575031, - "timestamp": 1.231090733873932 + "angularVelocity": 0.09884777520270435, + "velocityX": 0.35281518848860643, + "velocityY": -0.05609978931418552, + "timestamp": 1.2310907369516635 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 0, - "velocityX": 3.087311543739499e-36, - "velocityY": 0, - "timestamp": 1.3131634494655275 + "angularVelocity": -9.87865478987197e-28, + "velocityX": 1.3097638047391152e-27, + "velocityY": 4.333298605574227e-28, + "timestamp": 1.313163452748441 }, { "x": 2.3177158256290964, "y": 6.704097470773247, "heading": 0.5125504196, - "angularVelocity": -9.1973743139294e-17, - "velocityX": 0.34546318060283326, - "velocityY": 0.1965022887733569, - "timestamp": 1.4042599289943862 + "angularVelocity": -1.9471938619576857e-16, + "velocityX": 0.3454631797391752, + "velocityY": 0.19650228828210153, + "timestamp": 1.404259932505041 }, { "x": 2.380656782769881, "y": 6.7398988031009575, "heading": 0.5125504196, - "angularVelocity": -1.3336684994607708e-16, - "velocityX": 0.6909263394843395, - "velocityY": 0.3930045651914449, - "timestamp": 1.495356408523245 + "angularVelocity": -2.58789471726069e-16, + "velocityX": 0.6909263377570228, + "velocityY": 0.3930045642089339, + "timestamp": 1.4953564122616412 }, { "x": 2.475068211555481, "y": 6.793600797653198, "heading": 0.5125504196, - "angularVelocity": 6.653839443641591e-19, - "velocityX": 1.0363894332018742, - "velocityY": 0.5895068045437317, - "timestamp": 1.5864528880521036 + "angularVelocity": -4.208588013010918e-16, + "velocityX": 1.0363894306108958, + "velocityY": 0.5895068030699635, + "timestamp": 1.5864528920182412 }, { "x": 2.569479640341081, "y": 6.847302792205439, "heading": 0.5125504196, - "angularVelocity": 5.174999729369457e-17, - "velocityX": 1.0363894332018742, - "velocityY": 0.5895068045437317, - "timestamp": 1.6775493675809623 + "angularVelocity": -2.430413480392466e-16, + "velocityX": 1.0363894306108958, + "velocityY": 0.5895068030699636, + "timestamp": 1.6775493717748413 }, { "x": 2.6324205974818655, "y": 6.8831041245331495, "heading": 0.5125504196, - "angularVelocity": -1.3901806543706914e-17, - "velocityX": 0.6909263394843393, - "velocityY": 0.39300456519144494, - "timestamp": 1.768645847109821 + "angularVelocity": -8.097281474944775e-17, + "velocityX": 0.6909263377570227, + "velocityY": 0.393004564208934, + "timestamp": 1.7686458515314414 }, { "x": 2.663891077041626, "y": 6.901004791259766, "heading": 0.5125504196, - "angularVelocity": -1.8725484396545262e-17, - "velocityX": 0.3454631806028332, - "velocityY": 0.19650228877335693, - "timestamp": 1.8597423266386797 + "angularVelocity": -1.1468969708015058e-16, + "velocityX": 0.3454631797391751, + "velocityY": 0.19650228828210156, + "timestamp": 1.8597423312880415 }, { "x": 2.663891077041626, "y": 6.901004791259766, "heading": 0.5125504196, - "angularVelocity": 1.431796971368011e-33, - "velocityX": 3.743360269099248e-35, - "velocityY": -8.057233109044706e-35, - "timestamp": 1.9508388061675384 - }, - { - "x": 2.6835271230542426, - "y": 6.903699548014665, - "heading": 0.5052880772246282, - "angularVelocity": -0.10752621184517612, - "velocityX": 0.2907312178663514, - "velocityY": 0.039898557617057115, - "timestamp": 2.018379011023157 - }, - { - "x": 2.7228033814945674, - "y": 6.909089957964811, - "heading": 0.49095749727137794, - "angularVelocity": -0.21217850884350908, - "velocityX": 0.5815241236576919, - "velocityY": 0.0798103879262595, - "timestamp": 2.0859192158787754 - }, - { - "x": 2.7817246923505206, - "y": 6.91717710303389, - "heading": 0.46979167660622734, - "angularVelocity": -0.31338105518627096, - "velocityX": 0.8723886902906202, - "velocityY": 0.11973823719319023, - "timestamp": 2.153459420734394 - }, - { - "x": 2.860296723174767, - "y": 6.927962260865171, - "heading": 0.4420737054643263, - "angularVelocity": -0.41039216865205086, - "velocityX": 1.1633371706853932, - "velocityY": 0.15968500324119947, - "timestamp": 2.220999625590012 - }, - { - "x": 2.9585261643123166, - "y": 6.941446920815512, - "heading": 0.40815310467896726, - "angularVelocity": -0.502228278073356, - "velocityX": 1.454384708301312, - "velocityY": 0.19965382070083557, - "timestamp": 2.2885398304456306 - }, - { - "x": 3.0764209718394073, - "y": 6.9576328058659405, - "heading": 0.3684706356327604, - "angularVelocity": -0.5875384762459123, - "velocityX": 1.7455500435498434, - "velocityY": 0.23964814861057657, - "timestamp": 2.356080035301249 - }, - { - "x": 3.2139906503876703, - "y": 6.9765218958360915, - "heading": 0.323597871318007, - "angularVelocity": -0.6643859670055544, - "velocityX": 2.0368561043358877, - "velocityY": 0.27967178972185397, - "timestamp": 2.4236202401568674 - }, - { - "x": 3.3712465090237984, - "y": 6.998116434011635, - "heading": 0.27430452270447037, - "angularVelocity": -0.7298371202591344, - "velocityX": 2.3283296071176522, - "velocityY": 0.3197286449116718, - "timestamp": 2.491160445012486 - }, - { - "x": 3.5482015820439243, - "y": 7.022418860702876, - "heading": 0.22168358710657873, - "angularVelocity": -0.7791053596947181, - "velocityX": 2.619996095635254, - "velocityY": 0.3598216313260006, - "timestamp": 2.558700649868104 - }, - { - "x": 3.7448688058303015, - "y": 7.04943148246024, - "heading": 0.16741520227188003, - "angularVelocity": -0.8034974864335817, - "velocityX": 2.911854120176202, - "velocityY": 0.3999487685166161, - "timestamp": 2.6262408547237226 - }, - { - "x": 3.961249724160237, - "y": 7.079155012062387, - "heading": 0.1144423105156959, - "angularVelocity": -0.7843164211512963, - "velocityX": 3.2037350018777047, - "velocityY": 0.4400864591051642, - "timestamp": 2.693781059579341 - }, - { - "x": 4.197248092224393, - "y": 7.111577952654102, - "heading": 0.06945280731873603, - "angularVelocity": -0.6661144024234814, - "velocityX": 3.494190883321295, - "velocityY": 0.4800539273019009, - "timestamp": 2.7613212644349594 - }, - { - "x": 4.450279816565845, - "y": 7.14615999986413, - "heading": 0.06926458523360347, - "angularVelocity": -0.0027868154314146247, - "velocityX": 3.746386687490236, - "velocityY": 0.5120216511625231, - "timestamp": 2.828861469290578 - }, - { - "x": 4.703335353284152, - "y": 7.180937109425349, - "heading": 0.06926456657564138, - "angularVelocity": -2.762497114544702e-7, - "velocityX": 3.746739253445653, - "velocityY": 0.5149097435455242, - "timestamp": 2.896401674146196 - }, - { - "x": 4.956390888818978, - "y": 7.215714227598158, - "heading": 0.06926454791768001, - "angularVelocity": -2.7624970048363726e-7, - "velocityX": 3.746739235923052, - "velocityY": 0.5149098710487069, - "timestamp": 2.9639418790018146 - }, - { - "x": 5.209446424353711, - "y": 7.250491345771652, - "heading": 0.0692645292597187, - "angularVelocity": -2.7624969990656975e-7, - "velocityX": 3.7467392359216607, - "velocityY": 0.5149098710588286, - "timestamp": 3.031482083857433 - }, - { - "x": 5.462501959888314, - "y": 7.285268463946087, - "heading": 0.0692645106017574, - "angularVelocity": -2.7624969939846597e-7, - "velocityX": 3.7467392359197462, - "velocityY": 0.5149098710727624, - "timestamp": 3.0990222887130514 - }, - { - "x": 5.715557495424461, - "y": 7.320045582109288, - "heading": 0.06926449194379608, - "angularVelocity": -2.762496999067462e-7, - "velocityX": 3.746739235942603, - "velocityY": 0.5149098709064452, - "timestamp": 3.16656249356867 - }, - { - "x": 5.968613025759916, - "y": 7.354822738115272, - "heading": 0.06926447328583468, - "angularVelocity": -2.762497009102298e-7, - "velocityX": 3.7467391589411925, - "velocityY": 0.5149104312065347, - "timestamp": 3.234102698424288 - }, - { - "x": 6.221690653746151, - "y": 7.389438720345797, - "heading": 0.06926445462738323, - "angularVelocity": -2.7625695670270355e-7, - "velocityX": 3.74706633666926, - "velocityY": 0.5125240929387752, - "timestamp": 3.3016429032799066 - }, - { - "x": 6.476102895472122, - "y": 7.412263085598601, - "heading": 0.06926437909315673, - "angularVelocity": -0.0000011183594520291744, - "velocityX": 3.766826622303432, - "velocityY": 0.33793745964490024, - "timestamp": 3.369183108135525 - }, - { - "x": 6.712136074986816, - "y": 7.430641724558031, - "heading": 0.04341074490415746, - "angularVelocity": -0.38278880326565307, - "velocityX": 3.4947063015172035, - "velocityY": 0.27211405412107026, - "timestamp": 3.4367233129911434 - }, - { - "x": 6.9285315703702866, - "y": 7.445814297629393, - "heading": 0.019185726579151432, - "angularVelocity": -0.3586755233685199, - "velocityX": 3.203950829673405, - "velocityY": 0.2246450555457597, - "timestamp": 3.504263517846762 + "angularVelocity": 2.11830418797891e-27, + "velocityX": 1.5918238245881604e-27, + "velocityY": -9.542216118093543e-27, + "timestamp": 1.9508388110446415 + }, + { + "x": 2.6835272178710947, + "y": 6.903699093874769, + "heading": 0.5052895060867068, + "angularVelocity": -0.10750505591698582, + "velocityX": 0.2907326211779112, + "velocityY": 0.039891833548624495, + "timestamp": 2.018379016027187 + }, + { + "x": 2.722803664548093, + "y": 6.909088595285118, + "heading": 0.4909617568575599, + "angularVelocity": -0.21213659675521643, + "velocityX": 0.5815269095962702, + "velocityY": 0.0797969359397381, + "timestamp": 2.0859192210097324 + }, + { + "x": 2.7817252554319474, + "y": 6.9171743770902525, + "heading": 0.4698001364111611, + "angularVelocity": -0.3133188661756009, + "velocityX": 0.872392834743124, + "velocityY": 0.119718052487766, + "timestamp": 2.1534594259922777 + }, + { + "x": 2.8602976561490117, + "y": 6.927957716546488, + "heading": 0.44208769535784764, + "angularVelocity": -0.410310289411695, + "velocityX": 1.1633426451307045, + "velocityY": 0.1596580800875858, + "timestamp": 2.220999630974823 + }, + { + "x": 2.958527554736122, + "y": 6.9414401025597545, + "heading": 0.4081739049952151, + "angularVelocity": -0.5021274420383679, + "velocityX": 1.4543914785644525, + "velocityY": 0.19962015242254944, + "timestamp": 2.2885398359573683 + }, + { + "x": 3.0764229044730724, + "y": 6.9576232575928, + "heading": 0.3684994634316936, + "angularVelocity": -0.5874196202658062, + "velocityX": 1.7455580682264529, + "velocityY": 0.2396077275339608, + "timestamp": 2.3560800409399136 + }, + { + "x": 3.2139932065748873, + "y": 6.976509160880165, + "heading": 0.32363586089987717, + "angularVelocity": -0.6642503164361231, + "velocityX": 2.036865332839415, + "velocityY": 0.2796246071839072, + "timestamp": 2.423620245922459 + }, + { + "x": 3.371249765918335, + "y": 6.998100055077647, + "heading": 0.2743526968387638, + "angularVelocity": -0.7296863264458512, + "velocityX": 2.3283399774117917, + "velocityY": 0.31967469158647477, + "timestamp": 2.4911604509050043 + }, + { + "x": 3.548205611708083, + "y": 7.022398379906059, + "heading": 0.22174281137258775, + "angularVelocity": -0.7789417500253684, + "velocityX": 2.620007532335429, + "velocityY": 0.3597608984854559, + "timestamp": 2.5587006558875496 + }, + { + "x": 3.7448736745121907, + "y": 7.049406441609737, + "heading": 0.16748610321735644, + "angularVelocity": -0.8033246000549304, + "velocityX": 2.911866537197107, + "velocityY": 0.3998812516286613, + "timestamp": 2.626240860870095 + }, + { + "x": 3.9612554938842215, + "y": 7.079124953628088, + "heading": 0.11452509717802098, + "angularVelocity": -0.7841404398020747, + "velocityX": 3.203748336682593, + "velocityY": 0.44001216795286324, + "timestamp": 2.69378106585264 + }, + { + "x": 4.19725484401393, + "y": 7.11154242557216, + "heading": 0.06954671604514642, + "angularVelocity": -0.6659497279361002, + "velocityX": 3.494205417213319, + "velocityY": 0.4799729576250136, + "timestamp": 2.7613212708351855 + }, + { + "x": 4.4502883587972, + "y": 7.14611913479446, + "heading": 0.06936242836661095, + "angularVelocity": -0.002728562618119077, + "velocityX": 3.7464131897240014, + "velocityY": 0.5119426159757035, + "timestamp": 2.828861475817731 + }, + { + "x": 4.703344709577149, + "y": 7.180890314474715, + "heading": 0.0693624097128088, + "angularVelocity": -2.761881187265661e-7, + "velocityX": 3.7467512993978347, + "velocityY": 0.5148219447844706, + "timestamp": 2.896401680800276 + }, + { + "x": 4.956401059058472, + "y": 7.215661503606044, + "heading": 0.06936239105900772, + "angularVelocity": -2.761881027324315e-7, + "velocityX": 3.7467512801704235, + "velocityY": 0.5148220847170133, + "timestamp": 2.9639418857828215 + }, + { + "x": 5.209457408539732, + "y": 7.250432692737848, + "heading": 0.06936237240520664, + "angularVelocity": -2.761881030342619e-7, + "velocityX": 3.7467512801694554, + "velocityY": 0.5148220847240553, + "timestamp": 3.031482090765367 + }, + { + "x": 5.462513758020992, + "y": 7.285203881869653, + "heading": 0.06936235375140556, + "angularVelocity": -2.761881026512252e-7, + "velocityX": 3.746751280169456, + "velocityY": 0.5148220847240557, + "timestamp": 3.099022295747912 + }, + { + "x": 5.715570107502252, + "y": 7.319975071001454, + "heading": 0.0693623350976045, + "angularVelocity": -2.761881026475552e-7, + "velocityX": 3.7467512801694602, + "velocityY": 0.51482208472402, + "timestamp": 3.1665625007304574 + }, + { + "x": 5.968626456990083, + "y": 7.354746260085438, + "heading": 0.06936231644380343, + "angularVelocity": -2.761881026693173e-7, + "velocityX": 3.746751280266743, + "velocityY": 0.5148220840160207, + "timestamp": 3.2341027057130027 + }, + { + "x": 6.221682937041094, + "y": 7.389516498948747, + "heading": 0.06936229778999946, + "angularVelocity": -2.761881456453648e-7, + "velocityX": 3.746753213384661, + "velocityY": 0.5148080150525901, + "timestamp": 3.301642910695548 + }, + { + "x": 6.476098697142329, + "y": 7.412301605590586, + "heading": 0.0693622227466872, + "angularVelocity": -0.0000011110909759881076, + "velocityX": 3.7668787082743447, + "velocityY": 0.33735619617571233, + "timestamp": 3.3691831156780934 + }, + { + "x": 6.712134128533923, + "y": 7.430661265319214, + "heading": 0.043459048416696004, + "angularVelocity": -0.38352229367212326, + "velocityX": 3.4947396362298986, + "velocityY": 0.27183304719570117, + "timestamp": 3.4367233206606387 + }, + { + "x": 6.928530828538707, + "y": 7.445821895754349, + "heading": 0.019203692642594847, + "angularVelocity": -0.3591246988422603, + "velocityX": 3.2039686592705516, + "velocityY": 0.22446823250023049, + "timestamp": 3.504263525643184 }, { "x": 7.125290870666504, "y": 7.458, - "heading": 0, - "angularVelocity": -0.2840637901552857, - "velocityX": 2.913217404608591, - "velocityY": 0.18042146002750595, - "timestamp": 3.5718037227023802 - }, - { - "x": 7.284551146493795, - "y": 7.4665356658331685, - "heading": -0.011983560395956015, - "angularVelocity": -0.19978893791899283, - "velocityX": 2.6551742811725556, - "velocityY": 0.1423059220209307, - "timestamp": 3.6317848233617918 - }, - { - "x": 7.428286998104642, - "y": 7.47296871812108, - "heading": -0.020071347748117502, - "angularVelocity": -0.13483892864997732, - "velocityX": 2.396352351501819, - "velocityY": 0.10725132111930623, - "timestamp": 3.6917659240212033 - }, - { - "x": 7.556472866857842, - "y": 7.4774155432867, - "heading": -0.02498803078182015, - "angularVelocity": -0.08197053704667534, - "velocityX": 2.137104310257201, - "velocityY": 0.0741371051336659, - "timestamp": 3.751747024680615 - }, - { - "x": 7.669092898448606, - "y": 7.47995645101358, - "heading": -0.027230015787281982, - "angularVelocity": -0.037378190476904, - "velocityX": 1.877591947341067, - "velocityY": 0.04236180561785803, - "timestamp": 3.8117281253400264 - }, - { - "x": 7.766136403587462, - "y": 7.480650177988498, - "heading": -0.027158837231048453, - "angularVelocity": 0.001186683062681687, - "velocityX": 1.6179013734658474, - "velocityY": 0.011565759335732523, - "timestamp": 3.871709225999438 - }, - { - "x": 7.847595735447885, - "y": 7.479541543526011, - "heading": -0.025049657080079103, - "angularVelocity": 0.03516407881452247, - "velocityX": 1.3580833123248393, - "velocityY": -0.018483063003166142, - "timestamp": 3.9316903266588494 - }, - { - "x": 7.913465176456017, - "y": 7.4766658686921605, - "heading": -0.021118970688138776, - "angularVelocity": 0.06553208175121362, - "velocityX": 1.0981699282605077, - "velocityY": -0.04794301542047442, - "timestamp": 3.991671427318261 - }, - { - "x": 7.963740302847386, - "y": 7.472051704673601, - "heading": -0.015541591171907823, - "angularVelocity": 0.09298561471722216, - "velocityX": 0.8381827915570343, - "velocityY": -0.07692696479112789, - "timestamp": 4.051652527977672 - }, - { - "x": 7.998417597522527, - "y": 7.465722607991469, - "heading": -0.008461644038298986, - "angularVelocity": 0.11803629903043378, - "velocityX": 0.5781370180592035, - "velocityY": -0.10551818176978627, - "timestamp": 4.111633628637083 + "heading": -3.857889621465489e-27, + "angularVelocity": -0.2843297950836498, + "velocityX": 2.9132283826891787, + "velocityY": 0.18030896188127155, + "timestamp": 3.5718037306257293 + }, + { + "x": 7.284551274277721, + "y": 7.4665314387188895, + "heading": -0.0119926176075512, + "angularVelocity": -0.19994035049895104, + "velocityX": 2.6551818761052863, + "velocityY": 0.14223574064773895, + "timestamp": 3.6317847078403016 + }, + { + "x": 7.428287142227841, + "y": 7.472962064944413, + "heading": -0.020085170649452998, + "angularVelocity": -0.1349186595101974, + "velocityX": 2.396357555761855, + "velocityY": 0.10721109465287079, + "timestamp": 3.691765685054874 + }, + { + "x": 7.556472961771024, + "y": 7.477407773553819, + "heading": -0.025003758777922633, + "angularVelocity": -0.0820024673968577, + "velocityX": 2.137107888132909, + "velocityY": 0.07411864254065184, + "timestamp": 3.751746662269446 + }, + { + "x": 7.669092912784827, + "y": 7.479948525190578, + "heading": -0.027245693064255495, + "angularVelocity": -0.037377421816798616, + "velocityX": 1.8775944681748589, + "velocityY": 0.04235929047423255, + "timestamp": 3.8117276394840185 + }, + { + "x": 7.766136331910851, + "y": 7.480642796103092, + "heading": -0.027173129314670138, + "angularVelocity": 0.0012097793826494725, + "velocityX": 1.617903269212617, + "velocityY": 0.01157485164055792, + "timestamp": 3.871708616698591 + }, + { + "x": 7.84759559249558, + "y": 7.479535203925268, + "heading": -0.025061681495632242, + "angularVelocity": 0.03520195763874475, + "velocityX": 1.3580849190455957, + "velocityY": -0.018465724122206602, + "timestamp": 3.931689593913163 + }, + { + "x": 7.91346499303532, + "y": 7.476660908974376, + "heading": -0.021128188015767303, + "angularVelocity": 0.06557901625699587, + "velocityX": 1.0981715136801353, + "velocityY": -0.047920108747306074, + "timestamp": 3.9916705711277354 + }, + { + "x": 7.963740122835744, + "y": 7.472048331328805, + "heading": -0.015547730952116253, + "angularVelocity": 0.0930371148120487, + "velocityX": 0.8381845734285565, + "velocityY": -0.0769006751768895, + "timestamp": 4.051651548342308 + }, + { + "x": 7.998417475618676, + "y": 7.46572091854732, + "heading": -0.008464652154293805, + "angularVelocity": 0.1180887529138424, + "velocityX": 0.5781391766739492, + "velocityY": -0.10549032502171554, + "timestamp": 4.111632525556881 }, { "x": 8.017494201660156, "y": 7.457698345184326, - "heading": 0, - "angularVelocity": 0.14107183671647575, - "velocityX": 0.3180435825269393, - "velocityY": -0.1337798526356988, - "timestamp": 4.171614729296494 - }, - { - "x": 8.008975668857596, - "y": 7.440959395735479, - "heading": 0.016440344425850486, - "angularVelocity": 0.17454816573497248, - "velocityX": -0.09044179592138195, - "velocityY": -0.17771847395317164, - "timestamp": 4.265802739560738 - }, - { - "x": 7.961981832760565, - "y": 7.4200887947149115, - "heading": 0.0360050890089686, - "angularVelocity": 0.20772011775415367, - "velocityX": -0.4989364990850843, - "velocityY": -0.22158447728129313, - "timestamp": 4.3599907498249815 - }, - { - "x": 7.876511570645417, - "y": 7.39509531765153, - "heading": 0.058657775076795544, - "angularVelocity": 0.24050498576490828, - "velocityX": -0.9074431222759898, - "velocityY": -0.2653573102697685, - "timestamp": 4.454178760089225 - }, - { - "x": 7.752563396875145, - "y": 7.365990633137338, - "heading": 0.08435039467917142, - "angularVelocity": 0.2727801503641011, - "velocityX": -1.315965518567977, - "velocityY": -0.3090062570866462, - "timestamp": 4.548366770353469 - }, - { - "x": 7.590135252234061, - "y": 7.332791018782474, - "heading": 0.11301654484596554, - "angularVelocity": 0.3043503104733978, - "velocityX": -1.7245097776818146, - "velocityY": -0.35248238349789773, - "timestamp": 4.642554780617712 - }, - { - "x": 7.3892240901384, - "y": 7.295520773303632, - "heading": 0.14455765680612448, - "angularVelocity": 0.33487395977121315, - "velocityX": -2.13308638256617, - "velocityY": -0.39570052891318314, - "timestamp": 4.736742790881956 - }, - { - "x": 7.149824938411661, - "y": 7.254220108341429, - "heading": 0.1788109128374065, - "angularVelocity": 0.36366896312157765, - "velocityX": -2.54171577735964, - "velocityY": -0.4384917448232911, - "timestamp": 4.8309308011461995 - }, - { - "x": 6.871928226350113, - "y": 7.208968463075644, - "heading": 0.21545413209734332, - "angularVelocity": 0.3890433523028524, - "velocityX": -2.9504467849136042, - "velocityY": -0.4804395499897761, - "timestamp": 4.925118811410443 - }, - { - "x": 6.555508023323144, - "y": 7.15999689291417, - "heading": 0.25354713749374036, - "angularVelocity": 0.40443582245264, - "velocityX": -3.3594530995957568, - "velocityY": -0.5199342254293745, - "timestamp": 5.019306821674687 - }, - { - "x": 6.201946981022424, - "y": 7.116596127430346, - "heading": 0.25354714311943394, - "angularVelocity": 5.972834101491316e-8, - "velocityX": -3.753779714730222, - "velocityY": -0.46078864350212084, - "timestamp": 5.11349483193893 - }, - { - "x": 5.847829680383322, - "y": 7.077995922009697, - "heading": 0.25354714640605686, - "angularVelocity": 3.4894280958051e-8, - "velocityX": -3.7596855443238373, - "velocityY": -0.4098207968546854, - "timestamp": 5.207682842203174 - }, - { - "x": 5.493712379149646, - "y": 7.039395722043647, - "heading": 0.25354714969267966, - "angularVelocity": 3.4894280048226024e-8, - "velocityX": -3.759685550636459, - "velocityY": -0.40982073894285953, - "timestamp": 5.301870852467418 - }, - { - "x": 5.13959507794107, - "y": 7.0007955218474756, - "heading": 0.25354715297933, - "angularVelocity": 3.489457219723442e-8, - "velocityX": -3.7596855503699804, - "velocityY": -0.4098207413860709, - "timestamp": 5.396058862731661 - }, - { - "x": 4.793375065328026, - "y": 6.963049240971564, - "heading": 0.2809926873119857, - "angularVelocity": 0.2913909557666365, - "velocityX": -3.675839543076952, - "velocityY": -0.4007546265178986, - "timestamp": 5.490246872995905 - }, - { - "x": 4.4856271372174605, - "y": 6.929500155423233, - "heading": 0.3062072463057812, - "angularVelocity": 0.2677045509620205, - "velocityX": -3.2673790140292933, - "velocityY": -0.3561927410315718, - "timestamp": 5.5844348832601485 - }, - { - "x": 4.216349125910067, - "y": 6.900146018877801, - "heading": 0.32859640421091696, - "angularVelocity": 0.23770709076795637, - "velocityX": -2.858941499580855, - "velocityY": -0.31165470491497754, - "timestamp": 5.678622893524392 - }, - { - "x": 3.9855401854111148, - "y": 6.874986045056756, - "heading": 0.34796083888612495, - "angularVelocity": 0.2055934149249067, - "velocityX": -2.450512967111424, - "velocityY": -0.26712501676655553, - "timestamp": 5.772810903788636 - }, - { - "x": 3.7931998687162816, - "y": 6.854019825581219, - "heading": 0.36419988308525797, - "angularVelocity": 0.17241094862896578, - "velocityX": -2.042089180514842, - "velocityY": -0.22259966440226955, - "timestamp": 5.866998914052879 - }, - { - "x": 3.639327899901621, - "y": 6.837247108195556, - "heading": 0.37725261237367164, - "angularVelocity": 0.13858164379727655, - "velocityX": -1.6336683234200817, - "velocityY": -0.17807699025180046, - "timestamp": 5.961186924317123 - }, - { - "x": 3.5239240923326007, - "y": 6.824667722043819, - "heading": 0.38707830352299477, - "angularVelocity": 0.10431997790119188, - "velocityX": -1.2252494478358333, - "velocityY": -0.13355613008965372, - "timestamp": 6.0553749345813666 - }, - { - "x": 3.4469883122501845, - "y": 6.8162815454148475, - "heading": 0.39364810050149374, - "angularVelocity": 0.06975194571015403, - "velocityX": -0.8168319923796448, - "velocityY": -0.08903656214250527, - "timestamp": 6.14956294484561 + "heading": -2.2921744122616346e-27, + "angularVelocity": 0.1411222782185235, + "velocityX": 0.318046269457016, + "velocityY": -0.13375196163099834, + "timestamp": 4.171613502771454 + }, + { + "x": 8.008975913339244, + "y": 7.440961813507422, + "heading": 0.016444448853137204, + "angularVelocity": 0.1745916436893877, + "velocityX": -0.09043914895843151, + "velocityY": -0.17769270355161393, + "timestamp": 4.265801566447589 + }, + { + "x": 7.96198229634572, + "y": 7.420093426479306, + "heading": 0.036012649187603114, + "angularVelocity": 0.20775668986837795, + "velocityX": -0.49893388991528925, + "velocityY": -0.2215608455427296, + "timestamp": 4.359989630123724 + }, + { + "x": 7.876512228216728, + "y": 7.395101957451527, + "heading": 0.05866814941015883, + "angularVelocity": 0.24053472741998802, + "velocityX": -0.9074405481238055, + "velocityY": -0.2653358403641312, + "timestamp": 4.45417769379986 + }, + { + "x": 7.752564223658973, + "y": 7.365999072123293, + "heading": 0.0843629510276007, + "angularVelocity": 0.2728031622541168, + "velocityX": -1.3159629757752496, + "velocityY": -0.3089869797971924, + "timestamp": 4.548365757475995 + }, + { + "x": 7.590136223929822, + "y": 7.332801044076014, + "heading": 0.1130306642056742, + "angularVelocity": 0.3043667324624816, + "velocityX": -1.7245072612136703, + "velocityY": -0.3524653416958384, + "timestamp": 4.642553821152131 + }, + { + "x": 7.389225183137582, + "y": 7.295532166028874, + "heading": 0.14457273960345385, + "angularVelocity": 0.334883998743585, + "velocityX": -2.1330838850564935, + "velocityY": -0.3956857864207608, + "timestamp": 4.736741884828266 + }, + { + "x": 7.149826130214695, + "y": 7.254232639741168, + "heading": 0.17882639129814942, + "angularVelocity": 0.36367295767408864, + "velocityX": -2.5417132870047907, + "velocityY": -0.43847940679313435, + "timestamp": 4.8309299485044015 + }, + { + "x": 6.8719294965182485, + "y": 7.208981885028011, + "heading": 0.21546950044257604, + "angularVelocity": 0.38904196258268475, + "velocityX": -2.950444279776156, + "velocityY": -0.48042982249588434, + "timestamp": 4.925118012180537 + }, + { + "x": 6.555509356459519, + "y": 7.160010902115707, + "heading": 0.2535620654404122, + "angularVelocity": 0.40443091736992315, + "velocityX": -3.359450525989546, + "velocityY": -0.51992769572894, + "timestamp": 5.019306075856672 + }, + { + "x": 6.20194815910043, + "y": 7.116609756478925, + "heading": 0.2535620710648909, + "angularVelocity": 5.971540845532163e-8, + "velocityX": -3.753779232311261, + "velocityY": -0.4607924183048939, + "timestamp": 5.113494139532808 + }, + { + "x": 5.8478308523572835, + "y": 7.078007759401044, + "heading": 0.2535620743508316, + "angularVelocity": 3.488701802778144e-8, + "velocityX": -3.7596834770993417, + "velocityY": -0.40983958658088665, + "timestamp": 5.207682203208943 + }, + { + "x": 5.493713536071184, + "y": 7.039405849865993, + "heading": 0.2535620776367723, + "angularVelocity": 3.488701827976151e-8, + "velocityX": -3.759683578417396, + "velocityY": -0.40983865713370193, + "timestamp": 5.301870266885079 + }, + { + "x": 5.139596219795213, + "y": 7.00080394023819, + "heading": 0.2535620809227431, + "angularVelocity": 3.488733818198086e-8, + "velocityX": -3.759683578309871, + "velocityY": -0.4098386581184616, + "timestamp": 5.396058330561214 + }, + { + "x": 4.793375987292505, + "y": 6.963055984000021, + "heading": 0.2810067536056752, + "angularVelocity": 0.2913816423416479, + "velocityX": -3.6758397931736098, + "velocityY": -0.40077218667499337, + "timestamp": 5.490246394237349 + }, + { + "x": 4.485627858757115, + "y": 6.929505403826658, + "heading": 0.30621912928575457, + "angularVelocity": 0.2676812187876781, + "velocityX": -3.2673792890952553, + "velocityY": -0.35620840756134725, + "timestamp": 5.584434457913485 + }, + { + "x": 4.216349669557381, + "y": 6.9001499572465725, + "heading": 0.32860581789982174, + "angularVelocity": 0.23768073936675813, + "velocityX": -2.8589417670337087, + "velocityY": -0.3116684368947614, + "timestamp": 5.67862252158962 + }, + { + "x": 3.985540575144852, + "y": 6.8749888593305, + "heading": 0.3479678404777486, + "angularVelocity": 0.20556768896431482, + "velocityX": -2.450513211590839, + "velocityY": -0.26713679986657873, + "timestamp": 5.772810585265756 + }, + { + "x": 3.793200129317667, + "y": 6.854021702392079, + "heading": 0.36420470133991156, + "angularVelocity": 0.17238767024655313, + "velocityX": -2.0420893934983666, + "velocityY": -0.22260949126755833, + "timestamp": 5.866998648941891 + }, + { + "x": 3.6393280566589286, + "y": 6.837248234599062, + "heading": 0.37725557912927543, + "angularVelocity": 0.13856190774065766, + "velocityX": -1.633668499522676, + "velocityY": -0.1780848563857597, + "timestamp": 5.9611867126180265 + }, + { + "x": 3.5239241708826787, + "y": 6.824668285381395, + "heading": 0.3870798194050559, + "angularVelocity": 0.10430451473724978, + "velocityX": -1.225249583355537, + "velocityY": -0.13356203245586246, + "timestamp": 6.055374776294162 + }, + { + "x": 3.4469883384836706, + "y": 6.8162817332333585, + "heading": 0.3936486152349144, + "angularVelocity": 0.06974127690367621, + "velocityX": -0.8168320846211579, + "velocityY": -0.08904049855907127, + "timestamp": 6.149562839970297 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 0.03495947190378589, - "velocityX": -0.4084155935928461, - "velocityY": -0.04451793673752315, - "timestamp": 6.243750955109854 + "angularVelocity": 0.0349539871252265, + "velocityX": -0.4084156405121326, + "velocityY": -0.04451990557217318, + "timestamp": 6.243750903646433 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": -1.9952146739162206e-34, - "velocityY": -3.951795765453016e-33, - "timestamp": 6.337938965374097 - }, - { - "x": 3.425127749537185, - "y": 6.810478633677757, - "heading": 0.3886892234850989, - "angularVelocity": -0.13293807639169805, - "velocityX": 0.2675517930120232, - "velocityY": -0.025935588272518423, - "timestamp": 6.400010272097332 - }, - { - "x": 3.458347138585669, - "y": 6.807258412874914, - "heading": 0.3723514064813247, - "angularVelocity": -0.26321045691257755, - "velocityX": 0.535181081278085, - "velocityY": -0.05187937829625554, - "timestamp": 6.462081578820567 - }, - { - "x": 3.508184213080087, - "y": 6.802427294600358, - "heading": 0.34812438720722594, - "angularVelocity": -0.3903094771652888, - "velocityX": 0.8029003596884762, - "velocityY": -0.07783174754300737, - "timestamp": 6.524152885543802 - }, - { - "x": 3.5746455076736363, - "y": 6.795984642746885, - "heading": 0.31624502791908643, - "angularVelocity": -0.5135925272251962, - "velocityX": 1.070724914651617, - "velocityY": -0.10379436479708294, - "timestamp": 6.586224192267037 - }, - { - "x": 3.6577387353786484, - "y": 6.787929625128416, - "heading": 0.2770021560106679, - "angularVelocity": -0.6322224225666108, - "velocityX": 1.3386737301264124, - "velocityY": -0.12977038898779852, - "timestamp": 6.648295498990271 - }, - { - "x": 3.7574730901724314, - "y": 6.7782611086258875, - "heading": 0.23075444743671972, - "angularVelocity": -0.7450738677080124, - "velocityX": 1.6067706652041787, - "velocityY": -0.1557646682971112, - "timestamp": 6.710366805713506 - }, - { - "x": 3.873859634378807, - "y": 6.766977536857145, - "heading": 0.17795757602156642, - "angularVelocity": -0.8505841781386996, - "velocityX": 1.875045819887845, - "velocityY": -0.18178402170643457, - "timestamp": 6.772438112436741 - }, - { - "x": 4.006911762109537, - "y": 6.754076769706622, - "heading": 0.11920696294593584, - "angularVelocity": -0.9465019535937051, - "velocityX": 2.1435367604550115, - "velocityY": -0.2078378534549514, - "timestamp": 6.834509419159976 - }, - { - "x": 4.156645657189765, - "y": 6.73955582918857, - "heading": 0.055309270826440785, - "angularVelocity": -1.029423988195197, - "velocityX": 2.4122884305927053, - "velocityY": -0.23393966205310818, - "timestamp": 6.896580725883211 - }, - { - "x": 4.323080375842652, - "y": 6.723410435666096, - "heading": -0.012586184407094544, - "angularVelocity": -1.0938299645646508, - "velocityX": 2.681347106079046, - "velocityY": -0.2601104177560936, - "timestamp": 6.958652032606445 - }, - { - "x": 4.506235840968647, - "y": 6.705634179985004, - "heading": -0.08270962096165607, - "angularVelocity": -1.1297238652836274, - "velocityX": 2.9507267495214404, - "velocityY": -0.2863844281597597, - "timestamp": 7.02072333932968 - }, - { - "x": 4.706118949857515, - "y": 6.686217722826257, - "heading": -0.15194437984101536, - "angularVelocity": -1.1154068205470853, - "velocityX": 3.2202175117742615, - "velocityY": -0.3128088996947802, - "timestamp": 7.082794646052915 - }, - { - "x": 4.922607059053276, - "y": 6.66515425622058, - "heading": -0.21301282950448638, - "angularVelocity": -0.9838434678965439, - "velocityX": 3.4877324262085807, - "velocityY": -0.3393430510421988, - "timestamp": 7.14486595277615 - }, - { - "x": 5.152321807662402, - "y": 6.642265811014449, - "heading": -0.22692060137612477, - "angularVelocity": -0.22406120647088962, - "velocityX": 3.7008202458728654, - "velocityY": -0.3687443750482909, - "timestamp": 7.206937259499385 - }, - { - "x": 5.385997622338085, - "y": 6.619824669792801, - "heading": -0.22692062699124488, - "angularVelocity": -4.12672480414244e-7, - "velocityX": 3.7646350143327725, - "velocityY": -0.36153808267225507, - "timestamp": 7.269008566222619 - }, - { - "x": 5.6195399174914655, - "y": 6.5960341766076915, - "heading": -0.2269206525768778, - "angularVelocity": -4.121974272362167e-7, - "velocityX": 3.7624839476103156, - "velocityY": -0.3832768221124699, - "timestamp": 7.331079872945854 + "angularVelocity": 4.764169197941492e-24, + "velocityX": 1.177617036966837e-23, + "velocityY": -7.781360494191974e-22, + "timestamp": 6.337938967322568 + }, + { + "x": 3.4251270675193637, + "y": 6.810469355892218, + "heading": 0.38870394635032085, + "angularVelocity": -0.13270177736033364, + "velocityX": 0.26754260724678264, + "velocityY": -0.02608523375098152, + "timestamp": 6.400009855998859 + }, + { + "x": 3.458345061227577, + "y": 6.807230578787204, + "heading": 0.3723948211461713, + "angularVelocity": -0.26274998718326975, + "velocityX": 0.5351622059328033, + "velocityY": -0.05217868108681347, + "timestamp": 6.46208074467515 + }, + { + "x": 3.508179991445113, + "y": 6.802371624572939, + "heading": 0.3482095805490521, + "angularVelocity": -0.38963902584428683, + "velocityX": 0.8028712216032942, + "velocityY": -0.07828072576186927, + "timestamp": 6.524151633351441 + }, + { + "x": 3.5746383517078186, + "y": 6.795891856003428, + "heading": 0.3163840274624053, + "angularVelocity": -0.5127291354345188, + "velocityX": 1.070684852109927, + "velocityY": -0.10439303685990675, + "timestamp": 6.586222522027732 + }, + { + "x": 3.6577278069428134, + "y": 6.787790439971061, + "heading": 0.2772056832992266, + "angularVelocity": -0.63118709911662, + "velocityX": 1.3386219692828794, + "velocityY": -0.1305187698313394, + "timestamp": 6.6482934107040235 + }, + { + "x": 3.7574574943964785, + "y": 6.778066242632264, + "heading": 0.23103157299019372, + "angularVelocity": -0.7438931726890192, + "velocityX": 1.6067062930864489, + "velocityY": -0.15666276971655022, + "timestamp": 6.7103642993803145 + }, + { + "x": 3.8738384093946294, + "y": 6.766717706731516, + "heading": 0.1783152332262761, + "angularVelocity": -0.8492924926343718, + "velocityX": 1.8749677583173519, + "velocityY": -0.18283185794122117, + "timestamp": 6.772435188056606 + }, + { + "x": 4.00688386795334, + "y": 6.753742690540388, + "heading": 0.11964925208533374, + "angularVelocity": -0.9451448560192912, + "velocityX": 2.1434437527157497, + "velocityY": -0.2090354507214015, + "timestamp": 6.834506076732897 + }, + { + "x": 4.156609966611215, + "y": 6.739138213215621, + "heading": 0.05583642242794389, + "angularVelocity": -1.028063735162284, + "velocityX": 2.412179072201139, + "velocityY": -0.2352870667106592, + "timestamp": 6.896576965409188 + }, + { + "x": 4.323035675773869, + "y": 6.722899990969598, + "heading": -0.011979486180355738, + "angularVelocity": -1.09255578669044, + "velocityX": 2.681220016529636, + "velocityY": -0.26160769713976106, + "timestamp": 6.958647854085479 + }, + { + "x": 4.506180877109483, + "y": 6.705021607935757, + "heading": -0.08203737634019735, + "angularVelocity": -1.1286754814354962, + "velocityX": 2.950581266698877, + "velocityY": -0.28803169110530014, + "timestamp": 7.02071874276177 + }, + { + "x": 4.706052676090064, + "y": 6.685493696321985, + "heading": -0.1512367442368059, + "angularVelocity": -1.1148441624139431, + "velocityX": 3.220056990370196, + "velocityY": -0.3146066059342914, + "timestamp": 7.082789631438061 + }, + { + "x": 4.922530525854263, + "y": 6.664309314644424, + "heading": -0.21234440720432737, + "angularVelocity": -0.9844818443990274, + "velocityX": 3.4875906303382327, + "velocityY": -0.3412933523159342, + "timestamp": 7.144860520114352 + }, + { + "x": 5.152231029581242, + "y": 6.641295647238388, + "heading": -0.22626463143704686, + "angularVelocity": -0.22426333067850218, + "velocityX": 3.700615677099489, + "velocityY": -0.37076426480787344, + "timestamp": 7.206931408790643 + }, + { + "x": 5.385889088159938, + "y": 6.618686799272017, + "heading": -0.22626465694222986, + "angularVelocity": -4.1090410586524524e-7, + "velocityX": 3.764374307531794, + "velocityY": -0.36424237591119274, + "timestamp": 7.269002297466934 + }, + { + "x": 5.619544792113157, + "y": 6.5960536299134915, + "heading": -0.22626468244744516, + "angularVelocity": -4.109046256964509e-7, + "velocityX": 3.764336373074485, + "velocityY": -0.3646342084219269, + "timestamp": 7.331073186143225 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -0.22692067835854435, - "angularVelocity": -4.1535562716932914e-7, - "velocityX": 3.729650183615213, - "velocityY": -0.6268142199732243, - "timestamp": 7.393151179669089 - }, - { - "x": 5.959383441341322, - "y": 6.535142573313356, - "heading": -0.22692070161190955, - "angularVelocity": -7.95525127627382e-7, - "velocityX": 3.706414340926548, - "velocityY": -0.7521132022680976, - "timestamp": 7.422381387996315 - }, - { - "x": 6.066940887967902, - "y": 6.509606194124557, - "heading": -0.22692072465630755, - "angularVelocity": -7.883761123678933e-7, - "velocityX": 3.6796674667011553, - "velocityY": -0.8736297361581465, - "timestamp": 7.4516115963235405 - }, - { - "x": 6.17368571603366, - "y": 6.480861582677863, - "heading": -0.2269207475995078, - "angularVelocity": -7.849140177187675e-7, - "velocityX": 3.651866824580282, - "velocityY": -0.9833871563580519, - "timestamp": 7.480841804650766 - }, - { - "x": 6.2799139038286755, - "y": 6.450262838777624, - "heading": -0.2269207705194487, - "angularVelocity": -7.841182881547448e-7, - "velocityX": 3.63419194984251, - "velocityY": -1.0468192206387272, - "timestamp": 7.510072012977992 - }, - { - "x": 6.38600768248991, - "y": 6.419201268635774, - "heading": -0.22692079344039778, - "angularVelocity": -7.841527793264923e-7, - "velocityX": 3.629593654398156, - "velocityY": -1.0626530537902528, - "timestamp": 7.539302221305218 - }, - { - "x": 6.4915378820851375, - "y": 6.386275784732387, - "heading": -0.22692082347226308, - "angularVelocity": -0.0000010274256324387564, - "velocityX": 3.6103129479556895, - "velocityY": -1.1264197481865303, - "timestamp": 7.568532429632444 - }, - { - "x": 6.594254502115419, - "y": 6.3529873750963075, - "heading": -0.23642872835410836, - "angularVelocity": -0.32527667183951403, - "velocityX": 3.514057063171563, - "velocityY": -1.1388358667671954, - "timestamp": 7.597762637959669 - }, - { - "x": 6.69366972769076, - "y": 6.320452523543764, - "heading": -0.25267195567048195, - "angularVelocity": -0.5557000187824542, - "velocityX": 3.4011124540202067, - "velocityY": -1.1130557534298289, - "timestamp": 7.626992846286895 - }, - { - "x": 6.789488054498416, - "y": 6.288864279277486, - "heading": -0.26990654049679197, - "angularVelocity": -0.5896155317621415, - "velocityX": 3.278058292810801, - "velocityY": -1.0806711985311153, - "timestamp": 7.656223054614121 - }, - { - "x": 6.881710669103943, - "y": 6.258257632428043, - "heading": -0.28694313831911705, - "angularVelocity": -0.5828421621787924, - "velocityX": 3.1550447254125045, - "velocityY": -1.047089589879878, - "timestamp": 7.685453262941347 - }, - { - "x": 6.970345859458022, - "y": 6.228646491338886, - "heading": -0.3032367291527766, - "angularVelocity": -0.5574230142767849, - "velocityX": 3.0323146986098455, - "velocityY": -1.0130321603485573, - "timestamp": 7.714683471268573 - }, - { - "x": 7.055400617247705, - "y": 6.200038168598195, - "heading": -0.3184704659597927, - "angularVelocity": -0.5211641544429054, - "velocityX": 2.909823865690951, - "velocityY": -0.978724558526454, - "timestamp": 7.743913679595798 - }, - { - "x": 7.136880378911089, - "y": 6.172437128670844, - "heading": -0.33243641899697507, - "angularVelocity": -0.477791772157844, - "velocityX": 2.7875190197494866, - "velocityY": -0.9442642220804569, - "timestamp": 7.773143887923024 + "heading": -0.22626470814139457, + "angularVelocity": -4.139452486047814e-7, + "velocityX": 3.7295967696415837, + "velocityY": -0.6271318462207411, + "timestamp": 7.393144074819516 + }, + { + "x": 5.959381121610303, + "y": 6.535130978016939, + "heading": -0.22626473128429372, + "angularVelocity": -7.917457113083445e-7, + "velocityX": 3.70633385065427, + "velocityY": -0.7525096617429247, + "timestamp": 7.422374292054843 + }, + { + "x": 6.066915021909061, + "y": 6.5094954829249705, + "heading": -0.22626475422294146, + "angularVelocity": -7.84758031451317e-7, + "velocityX": 3.678860797818322, + "velocityY": -0.8770203411621033, + "timestamp": 7.45160450929017 + }, + { + "x": 6.173524224764707, + "y": 6.480251745020554, + "heading": -0.22626477706662856, + "angularVelocity": -7.815093169473892e-7, + "velocityX": 3.6472258142098704, + "velocityY": -1.0004625579406388, + "timestamp": 7.480834726525496 + }, + { + "x": 6.279732582783464, + "y": 6.449584123171384, + "heading": -0.22626479989606985, + "angularVelocity": -7.810219505026809e-7, + "velocityX": 3.6335124424048417, + "velocityY": -1.0491752969972816, + "timestamp": 7.510064943760823 + }, + { + "x": 6.385940150925491, + "y": 6.4189137659468605, + "heading": -0.22626482272551648, + "angularVelocity": -7.810221338422918e-7, + "velocityX": 3.633485419795893, + "velocityY": -1.0492688773950392, + "timestamp": 7.53929516099615 + }, + { + "x": 6.491550017633834, + "y": 6.3862446041508125, + "heading": -0.22626484978268144, + "angularVelocity": -9.256573336962615e-7, + "velocityX": 3.613037353027402, + "velocityY": -1.1176503251082635, + "timestamp": 7.5685253782314765 + }, + { + "x": 6.594250357432935, + "y": 6.352942011609429, + "heading": -0.23580774999861734, + "angularVelocity": -0.32647380411537286, + "velocityX": 3.513499026445183, + "velocityY": -1.1393207335159796, + "timestamp": 7.597755595466803 + }, + { + "x": 6.693663742086507, + "y": 6.320404663734079, + "heading": -0.25220771678314496, + "angularVelocity": -0.5610620903873084, + "velocityX": 3.4010484374171166, + "velocityY": -1.1131408163475485, + "timestamp": 7.62698581270213 + }, + { + "x": 6.789481492756262, + "y": 6.2888206325426586, + "heading": -0.2695690677329979, + "angularVelocity": -0.5939521697728065, + "velocityX": 3.278037583448113, + "velocityY": -1.0805267349587082, + "timestamp": 7.656216029937457 + }, + { + "x": 6.881704495384483, + "y": 6.2582213842117005, + "heading": -0.2867065303563255, + "angularVelocity": -0.5862926876443351, + "velocityX": 3.155057038603278, + "velocityY": -1.0468361587808552, + "timestamp": 7.685446247172783 + }, + { + "x": 6.970340753224835, + "y": 6.228619131395559, + "heading": -0.3030805193415338, + "angularVelocity": -0.5601733594172232, + "velocityX": 3.0323502944489906, + "velocityY": -1.0127277733797784, + "timestamp": 7.71467646440811 + }, + { + "x": 7.055397018445757, + "y": 6.200020205130934, + "heading": -0.3183784421956162, + "angularVelocity": -0.5233598755329688, + "velocityX": 2.9098745498930065, + "velocityY": -0.9784027957911439, + "timestamp": 7.743906681643437 + }, + { + "x": 7.1368785342038406, + "y": 6.172428433477075, + "heading": -0.33239562176130555, + "angularVelocity": -0.4795441461430052, + "velocityX": 2.787578179870906, + "velocityY": -0.9439468558075184, + "timestamp": 7.7731368988787635 }, { "x": 7.214789390563965, "y": 6.145846366882324, "heading": -0.344987478573796, - "angularVelocity": -0.42938659336036833, - "velocityX": 2.665359438451487, - "velocityY": -0.9097014120066562, - "timestamp": 7.80237409625025 - }, - { - "x": 7.370577859130934, - "y": 6.0916577592619054, - "heading": -0.3653758531922844, - "angularVelocity": -0.3132570133567013, - "velocityX": 2.393610638018096, - "velocityY": -0.8325804140238011, - "timestamp": 7.867459230019526 - }, - { - "x": 7.5087533836626745, - "y": 6.042843250581257, - "heading": -0.37947223160146565, - "angularVelocity": -0.21658368958955537, - "velocityX": 2.1229967049244562, - "velocityY": -0.7500101152698444, - "timestamp": 7.932544363788803 - }, - { - "x": 7.629367562165718, - "y": 5.999619792415774, - "heading": -0.38804895009153, - "angularVelocity": -0.1317769203712161, - "velocityX": 1.85317554897581, - "velocityY": -0.6641064658284028, - "timestamp": 7.997629497558079 - }, - { - "x": 7.7324579600950285, - "y": 5.962133690972344, - "heading": -0.39162657356882036, - "angularVelocity": -0.05496836635495089, - "velocityX": 1.5839315671495955, - "velocityY": -0.5759548958801737, - "timestamp": 8.062714631327355 - }, - { - "x": 7.81805300550911, - "y": 5.930490256491027, - "heading": -0.39057997729782273, - "angularVelocity": 0.01608041975771331, - "velocityX": 1.3151243679933469, - "velocityY": -0.4861852876186942, - "timestamp": 8.127799765096631 - }, - { - "x": 7.886174910867723, - "y": 5.904768911877698, - "heading": -0.38519213768662347, - "angularVelocity": 0.08278141718657105, - "velocityX": 1.0466584519915365, - "velocityY": -0.39519538677620675, - "timestamp": 8.192884898865907 - }, - { - "x": 7.936841490598973, - "y": 5.885031694896893, - "heading": -0.3756842946663964, - "angularVelocity": 0.1460831755210139, - "velocityX": 0.7784662456231665, - "velocityY": -0.3032523072130836, - "timestamp": 8.257970032635184 - }, - { - "x": 7.970067341476633, - "y": 5.8713284070268905, - "heading": -0.36223415085714655, - "angularVelocity": 0.206654623418748, - "velocityX": 0.510498311264806, - "velocityY": -0.21054405324847428, - "timestamp": 8.32305516640446 + "angularVelocity": -0.4307821837626389, + "velocityX": 2.6654217357633208, + "velocityY": -0.9094036619962027, + "timestamp": 7.80236711611409 + }, + { + "x": 7.370581924170205, + "y": 6.091672602019875, + "heading": -0.3654249911056099, + "angularVelocity": -0.3140115764707324, + "velocityX": 2.3936699245523703, + "velocityY": -0.8323512600338246, + "timestamp": 7.867452336097115 + }, + { + "x": 7.508760881953895, + "y": 6.042869206500167, + "heading": -0.379545281879092, + "angularVelocity": -0.21695080353365873, + "velocityX": 2.123046642843499, + "velocityY": -0.7498383739417093, + "timestamp": 7.932537556080139 + }, + { + "x": 7.629377621865933, + "y": 5.999653284534207, + "heading": -0.38812959068868996, + "angularVelocity": -0.13189336706301324, + "velocityX": 1.8532124489015709, + "velocityY": -0.6639897964119993, + "timestamp": 7.997622776063164 + }, + { + "x": 7.7324695426404055, + "y": 5.962171118621702, + "heading": -0.3917039945024171, + "angularVelocity": -0.05491882511358906, + "velocityX": 1.5839528667393499, + "velocityY": -0.5758936656015066, + "timestamp": 8.062707996046187 + }, + { + "x": 7.818064946675874, + "y": 5.930527938551499, + "heading": -0.39064709699012523, + "angularVelocity": 0.016238671584232672, + "velocityX": 1.315128135969949, + "velocityY": -0.48618073471145673, + "timestamp": 8.127793216029211 + }, + { + "x": 7.886185947266274, + "y": 5.90480307138058, + "heading": -0.38524455647534983, + "angularVelocity": 0.08300717914427408, + "velocityX": 1.0466431642724277, + "velocityY": -0.39524898552433385, + "timestamp": 8.192878436012235 + }, + { + "x": 7.936850278188107, + "y": 5.885058458931867, + "heading": -0.37571963101716943, + "angularVelocity": 0.14634544464418678, + "velocityX": 0.778430662676533, + "velocityY": -0.3033655329714407, + "timestamp": 8.257963655995258 + }, + { + "x": 7.970072469166475, + "y": 5.871343812268201, + "heading": -0.3622515950174618, + "angularVelocity": 0.20692925372642912, + "velocityX": 0.5104414026261703, + "velocityY": -0.21071829621597563, + "timestamp": 8.323048875978282 }, { "x": 7.985864639282227, "y": 5.863699913024902, "heading": -0.344987478573796, - "angularVelocity": 0.2649863537883773, - "velocityX": 0.24271745160106833, - "velocityY": -0.1172079330593501, - "timestamp": 8.388140300173736 - }, - { - "x": 7.985045656249217, - "y": 5.861990715165632, - "heading": -0.3251381624172612, - "angularVelocity": 0.31897227576671205, - "velocityX": -0.01316080008970204, - "velocityY": -0.02746627272232015, - "timestamp": 8.450369270101682 - }, - { - "x": 7.968302412952749, - "y": 5.865866788787815, - "heading": -0.3020436028872691, - "angularVelocity": 0.37112231741475454, - "velocityX": -0.26905866055398503, - "velocityY": 0.062287285594966935, - "timestamp": 8.512598240029627 - }, - { - "x": 7.935633552055529, - "y": 5.875328955639361, - "heading": -0.2758355383990043, - "angularVelocity": 0.4211553641111372, - "velocityX": -0.5249783330022537, - "velocityY": 0.1520540491430483, - "timestamp": 8.574827209957572 - }, - { - "x": 7.887037558008595, - "y": 5.8903781286161365, - "heading": -0.24666763452179902, - "angularVelocity": 0.468719053376244, - "velocityX": -0.7809223598462517, - "velocityY": 0.24183548264097016, - "timestamp": 8.637056179885517 - }, - { - "x": 7.822512734192608, - "y": 5.911015324029987, - "heading": -0.21472164060862464, - "angularVelocity": 0.5133620876283883, - "velocityX": -1.0368936508301385, - "velocityY": 0.33163324795100674, - "timestamp": 8.699285149813463 - }, - { - "x": 7.742057181096897, - "y": 5.937241677003945, - "heading": -0.1802161756247138, - "angularVelocity": 0.5544919837796514, - "velocityX": -1.29289546635384, - "velocityY": 0.4214492543316279, - "timestamp": 8.761514119741408 - }, - { - "x": 7.645668783226514, - "y": 5.9690584605532635, - "heading": -0.14341967249459378, - "angularVelocity": 0.5913082471512265, - "velocityX": -1.5489312772168387, - "velocityY": 0.5112857176674774, - "timestamp": 8.823743089669353 - }, - { - "x": 7.533345224831275, - "y": 6.006467108033043, - "heading": -0.10467019490543185, - "angularVelocity": 0.6226919332592115, - "velocityX": -1.8050043014579413, - "velocityY": 0.6011452145695896, - "timestamp": 8.885972059597298 - }, - { - "x": 7.405084087505747, - "y": 6.04946923502926, - "heading": -0.06440725119965417, - "angularVelocity": 0.6470128583583754, - "velocityX": -2.0611161887146876, - "velocityY": 0.6910306734308482, - "timestamp": 8.948201029525244 - }, - { - "x": 7.260883177292953, - "y": 6.098066643466027, - "heading": -0.023226072117979224, - "angularVelocity": 0.6617686124221249, - "velocityX": -2.3172633321708815, - "velocityY": 0.7809450886466088, - "timestamp": 9.010429999453189 - }, - { - "x": 7.100741532540205, - "y": 6.152261239581916, - "heading": 0.018021968863737303, - "angularVelocity": 0.6628430621538008, - "velocityX": -2.5734259290837285, - "velocityY": 0.8708901365174432, - "timestamp": 9.072658969381134 - }, - { - "x": 6.924662708731195, - "y": 6.212054571383467, - "heading": 0.05802124542072086, - "angularVelocity": 0.6427758100977419, - "velocityX": -2.829531390490462, - "velocityY": 0.9608600603671181, - "timestamp": 9.13488793930908 - }, - { - "x": 6.732667433541327, - "y": 6.277445378277029, - "heading": 0.09448040335619008, - "angularVelocity": 0.5858872158366861, - "velocityX": -3.0853037646642187, - "velocityY": 1.050809726872821, - "timestamp": 9.197116909237025 - }, - { - "x": 6.524862060286313, - "y": 6.348410321942265, - "heading": 0.12245319566137218, - "angularVelocity": 0.44951398581032764, - "velocityX": -3.33936707446111, - "velocityY": 1.1403843539014786, - "timestamp": 9.25934587916497 - }, - { - "x": 6.30228026524581, - "y": 6.424202518084616, - "heading": 0.1231057628389814, - "angularVelocity": 0.010486549566300301, - "velocityX": -3.5768195311320152, - "velocityY": 1.2179567849204198, - "timestamp": 9.321574849092915 - }, - { - "x": 6.078984969036473, - "y": 6.498549743391548, - "heading": 0.1231057748040032, - "angularVelocity": 1.9227414178241924e-7, - "velocityX": -3.5882852707972814, - "velocityY": 1.194736557475109, - "timestamp": 9.38380381902086 + "angularVelocity": 0.2652540230818704, + "velocityX": 0.2426383458467277, + "velocityY": -0.11744447119165673, + "timestamp": 8.388134095961306 + }, + { + "x": 7.985039462235232, + "y": 5.861972195893162, + "heading": -0.32512296531969853, + "angularVelocity": 0.31921787404219815, + "velocityX": -0.013260393510813807, + "velocityY": -0.027763992134404936, + "timestamp": 8.450362795759837 + }, + { + "x": 7.968288815736249, + "y": 5.865825907000098, + "heading": -0.30201458190342884, + "angularVelocity": 0.3713460748992679, + "velocityX": -0.2691787961698452, + "velocityY": 0.06192819582304089, + "timestamp": 8.512591495558368 + }, + { + "x": 7.9356113385118245, + "y": 5.8752618544788175, + "heading": -0.2757940495370948, + "angularVelocity": 0.421357548064224, + "velocityX": -0.5251190741606199, + "velocityY": 0.15163337028201795, + "timestamp": 8.5748201953569 + }, + { + "x": 7.88700551038904, + "y": 5.890280935303068, + "heading": -0.2466150135455457, + "angularVelocity": 0.46889997840252956, + "velocityX": -0.781083780958754, + "velocityY": 0.24135295888994696, + "timestamp": 8.63704889515543 + }, + { + "x": 7.8224696292437494, + "y": 5.910884146922841, + "heading": -0.2146591990965777, + "angularVelocity": 0.5135221296994205, + "velocityX": -1.0370758404759837, + "velocityY": 0.3310885762755298, + "timestamp": 8.699277594953962 + }, + { + "x": 7.742001788904935, + "y": 5.9370726017699464, + "heading": -0.18014519573995202, + "angularVelocity": 0.5546316003446364, + "velocityX": -1.2930985316956969, + "velocityY": 0.4208420701684547, + "timestamp": 8.761506294752493 + }, + { + "x": 7.6455998656570765, + "y": 5.9688475450465655, + "heading": -0.14334139980997887, + "angularVelocity": 0.5914280074809063, + "velocityX": -1.5491553505048972, + "velocityY": 0.5106155741561712, + "timestamp": 8.823734994551025 + }, + { + "x": 7.533261533346939, + "y": 6.006210375220679, + "heading": -0.10458582814826359, + "angularVelocity": 0.6227925665679687, + "velocityX": -1.8052495500281769, + "velocityY": 0.6004115511825956, + "timestamp": 8.885963694349556 + }, + { + "x": 7.404984359973908, + "y": 6.049162662823707, + "heading": -0.06431792685231795, + "angularVelocity": 0.6470953342479389, + "velocityX": -2.0613828312070357, + "velocityY": 0.6902327662652126, + "timestamp": 8.948192394148087 + }, + { + "x": 7.260766133037973, + "y": 6.097706149350396, + "heading": -0.023132839886022792, + "angularVelocity": 0.6618342838534901, + "velocityX": -2.317551666720475, + "velocityY": 0.7800819667428388, + "timestamp": 9.010421093946618 + }, + { + "x": 7.100605864047409, + "y": 6.151842655747804, + "heading": 0.018118188112097987, + "angularVelocity": 0.6628939401220464, + "velocityX": -2.573736387054354, + "velocityY": 0.8699604293947623, + "timestamp": 9.07264979374515 + }, + { + "x": 6.924507066086204, + "y": 6.21157360061122, + "heading": 0.058119739083768836, + "angularVelocity": 0.6428151496203137, + "velocityX": -2.829864652986999, + "velocityY": 0.9598616885263167, + "timestamp": 9.134878493543681 + }, + { + "x": 6.73249038975253, + "y": 6.276897504068343, + "heading": 0.09458083823039053, + "angularVelocity": 0.585920953911396, + "velocityX": -3.0856610688530672, + "velocityY": 1.0497391664716116, + "timestamp": 9.197107193342212 + }, + { + "x": 6.524662008432933, + "y": 6.347790578963825, + "heading": 0.12255603947008778, + "angularVelocity": 0.449554648100767, + "velocityX": -3.3397513043410987, + "velocityY": 1.1392343906429359, + "timestamp": 9.259335893140744 + }, + { + "x": 6.302055285584133, + "y": 6.423504678750072, + "heading": 0.12321029314887402, + "angularVelocity": 0.01051369674932009, + "velocityX": -3.5772356415849678, + "velocityY": 1.2167070826061643, + "timestamp": 9.321564592939275 + }, + { + "x": 6.078948914811354, + "y": 6.498413724394235, + "heading": 0.1232103051115219, + "angularVelocity": 1.9223682817827964e-7, + "velocityX": -3.585264861632913, + "velocityY": 1.2037700592601788, + "timestamp": 9.383793292737806 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": 0.12310578689683861, - "angularVelocity": 1.9432806685756468e-7, - "velocityX": -3.662936913327793, - "velocityY": 0.941318096340742, - "timestamp": 9.446032788948806 - }, - { - "x": 5.606427257807288, - "y": 6.600564902613298, - "heading": 0.1231057972666647, - "angularVelocity": 1.578555424299444e-7, - "velocityX": -3.7237014740499577, - "velocityY": 0.6612371129043467, - "timestamp": 9.511724660841988 - }, - { - "x": 5.35931577124569, - "y": 6.62625930787543, - "heading": 0.12310580746861746, - "angularVelocity": 1.5530007712924032e-7, - "velocityX": -3.7616752185629396, - "velocityY": 0.39113522756534835, - "timestamp": 9.577416532735171 - }, - { - "x": 5.111994168115865, - "y": 6.649845615003531, - "heading": 0.1231058176602883, - "angularVelocity": 1.551435596748186e-7, - "velocityX": -3.764873735550995, - "velocityY": 0.35904452785959023, - "timestamp": 9.643108404628354 - }, - { - "x": 4.868859374012009, - "y": 6.67294055859981, - "heading": 0.13716866748145323, - "angularVelocity": 0.21407290454489739, - "velocityX": -3.701139685883833, - "velocityY": 0.35156470550620644, - "timestamp": 9.708800276521536 - }, - { - "x": 4.644141674488456, - "y": 6.6943246178055915, - "heading": 0.16550217221046287, - "angularVelocity": 0.43130913935716303, - "velocityX": -3.4207839272559015, - "velocityY": 0.3255206251475534, - "timestamp": 9.77449214841472 - }, - { - "x": 4.438149971299598, - "y": 6.713938607045374, - "heading": 0.1971398735951766, - "angularVelocity": 0.48160754858923427, - "velocityX": -3.1357258859027066, - "velocityY": 0.2985755874284469, - "timestamp": 9.840184020307902 - }, - { - "x": 4.2509016025753406, - "y": 6.731775563046276, - "heading": 0.22901708759854217, - "angularVelocity": 0.48525354940713283, - "velocityX": -2.8504039134206605, - "velocityY": 0.27152455070706877, - "timestamp": 9.905875892201085 - }, - { - "x": 4.082393919350406, - "y": 6.747832566507683, - "heading": 0.25967107343501483, - "angularVelocity": 0.46663285659323517, - "velocityX": -2.565122265033534, - "velocityY": 0.24442907468850525, - "timestamp": 9.971567764094267 - }, - { - "x": 3.9326220842038664, - "y": 6.762107971713798, - "heading": 0.28824128245235414, - "angularVelocity": 0.43491239013245725, - "velocityX": -2.279914254690053, - "velocityY": 0.21730854662395976, - "timestamp": 10.03725963598745 - }, - { - "x": 3.801581586235111, - "y": 6.77460070477556, - "heading": 0.314159509734349, - "angularVelocity": 0.3945423769342238, - "velocityX": -1.9947749119073828, - "velocityY": 0.19017167119358822, - "timestamp": 10.102951507880633 - }, - { - "x": 3.6892685916749244, - "y": 6.785310000165767, - "heading": 0.3370219943875935, - "angularVelocity": 0.34802607985383205, - "velocityX": -1.7096939289964497, - "velocityY": 0.16302314246152652, - "timestamp": 10.168643379773815 - }, - { - "x": 3.595679898494151, - "y": 6.794235280663889, - "heading": 0.35652698961990326, - "angularVelocity": 0.2969164170572818, - "velocityX": -1.4246616892414212, - "velocityY": 0.135865826941808, - "timestamp": 10.234335251666998 - }, - { - "x": 3.520812828526041, - "y": 6.801376095406927, - "heading": 0.37244068726433127, - "angularVelocity": 0.2422475899347831, - "velocityX": -1.1396702181031793, - "velocityY": 0.10870164812244498, - "timestamp": 10.30002712356018 - }, - { - "x": 3.464665127058438, - "y": 6.806732084957018, - "heading": 0.38457706026543664, - "angularVelocity": 0.18474695044220302, - "velocityX": -0.8547130695088268, - "velocityY": 0.08153199773025213, - "timestamp": 10.365718995453364 - }, - { - "x": 3.4272348818352074, - "y": 6.810302960282004, - "heading": 0.39278518447772715, - "angularVelocity": 0.12494885555456782, - "velocityX": -0.5697850303930024, - "velocityY": 0.05435794752192765, - "timestamp": 10.431410867346546 + "heading": 0.12321031719433864, + "angularVelocity": 1.9416791274703824e-7, + "velocityX": -3.6623734312330534, + "velocityY": 0.9435079745715298, + "timestamp": 9.446021992536338 + }, + { + "x": 5.606452972493988, + "y": 6.600711194819738, + "heading": 0.12321032755676568, + "angularVelocity": 1.577427164908284e-7, + "velocityX": -3.7233054627128834, + "velocityY": 0.6634632443513382, + "timestamp": 9.511713945013648 + }, + { + "x": 5.35926430365433, + "y": 6.6256551734400135, + "heading": 0.12321033775139768, + "angularVelocity": 1.551884456824965e-7, + "velocityX": -3.7628455163520536, + "velocityY": 0.3797113296166925, + "timestamp": 9.577405897490959 + }, + { + "x": 5.111950804404792, + "y": 6.649329487896052, + "heading": 0.12321034793997507, + "angularVelocity": 1.5509627898966467e-7, + "velocityX": -3.7647457553489407, + "velocityY": 0.3603837846685449, + "timestamp": 9.64309784996827 + }, + { + "x": 4.868821995322067, + "y": 6.672498234290298, + "heading": 0.1372675563656116, + "angularVelocity": 0.2139867654335871, + "velocityX": -3.7010440383348504, + "velocityY": 0.35268774211343507, + "timestamp": 9.70878980244558 + }, + { + "x": 4.644110082814918, + "y": 6.693950400788965, + "heading": 0.16558913990545626, + "angularVelocity": 0.4311271391975579, + "velocityX": -3.420691637758275, + "velocityY": 0.3265569935080811, + "timestamp": 9.77448175492289 + }, + { + "x": 4.438123681636039, + "y": 6.713626799516342, + "heading": 0.19721421356331742, + "angularVelocity": 0.48141473141302027, + "velocityX": -3.13564132912667, + "velocityY": 0.29952525363244636, + "timestamp": 9.8401737074002 + }, + { + "x": 4.250880120713019, + "y": 6.73152047543666, + "heading": 0.22907908319149847, + "angularVelocity": 0.4850650410974934, + "velocityX": -2.8503272297728453, + "velocityY": 0.27238764027449763, + "timestamp": 9.905865659877511 + }, + { + "x": 4.082376753854165, + "y": 6.74762851546552, + "heading": 0.25972143161316247, + "angularVelocity": 0.466455132875641, + "velocityX": -2.565053412243361, + "velocityY": 0.24520568230062648, + "timestamp": 9.971557612354822 + }, + { + "x": 3.9326087471810762, + "y": 6.761949278304569, + "heading": 0.28828094811629446, + "angularVelocity": 0.4347490891368528, + "velocityX": -2.279853178740861, + "velocityY": 0.21799873955634386, + "timestamp": 10.037249564832132 + }, + { + "x": 3.8015715928550198, + "y": 6.774481693316805, + "heading": 0.31418958063772184, + "angularVelocity": 0.39439583608625584, + "velocityX": -1.9947215661053177, + "velocityY": 0.190775498970961, + "timestamp": 10.102941517309443 + }, + { + "x": 3.6892614596122306, + "y": 6.785224997460466, + "heading": 0.33704367479862785, + "angularVelocity": 0.3478979281183608, + "velocityX": -1.709648275130517, + "velocityY": 0.16354064293296997, + "timestamp": 10.168633469786753 + }, + { + "x": 3.595675147466301, + "y": 6.794178615472137, + "heading": 0.3565415622292007, + "angularVelocity": 0.29680785385861885, + "velocityX": -1.4246236961559957, + "velocityY": 0.1362970299103891, + "timestamp": 10.234325422264064 + }, + { + "x": 3.5208099799304895, + "y": 6.801342098064149, + "heading": 0.37244949483993084, + "angularVelocity": 0.24215953417162805, + "velocityX": -1.139639860174189, + "velocityY": 0.10904657757715387, + "timestamp": 10.300017374741374 + }, + { + "x": 3.4646637036902503, + "y": 6.806715087085814, + "heading": 0.3845814929922257, + "angularVelocity": 0.18468012739437525, + "velocityX": -0.8546903254189608, + "velocityY": 0.08179067327191092, + "timestamp": 10.365709327218685 + }, + { + "x": 3.427234407665484, + "y": 6.81029729456599, + "heading": 0.39278667084002433, + "angularVelocity": 0.12490385105592111, + "velocityX": -0.569769882204283, + "velocityY": 0.054530385307290076, + "timestamp": 10.431401279695995 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 0.0632601721112495, - "velocityX": -0.28488184560874424, - "velocityY": 0.027180367966521624, - "timestamp": 10.497102739239729 + "angularVelocity": 0.06323746826387204, + "velocityX": -0.2848742780656992, + "velocityY": 0.02726658135331313, + "timestamp": 10.497093232173306 }, { "x": 3.408520460128784, "y": 6.812088489532471, "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": 1.9034366224257262e-34, - "velocityY": -2.547324229323533e-33, - "timestamp": 10.562794611132912 - }, - { - "x": 3.4428034429775143, - "y": 6.811427919070607, - "heading": 0.39249813327278055, - "angularVelocity": -0.050100574262489476, - "velocityX": 0.38660845958378076, - "velocityY": -0.007449238878501296, - "timestamp": 10.651470846737375 - }, - { - "x": 3.5113694006298597, - "y": 6.810106800279428, - "heading": 0.3836131655700934, - "angularVelocity": -0.10019558951868686, - "velocityX": 0.7732168284429818, - "velocityY": -0.014898228168725063, - "timestamp": 10.740147082341839 - }, - { - "x": 3.614218332551768, - "y": 6.808125167746075, - "heading": 0.37028807906085554, - "angularVelocity": -0.1502667137188131, - "velocityX": 1.1598251912796755, - "velocityY": -0.022346827420505266, - "timestamp": 10.828823317946302 - }, - { - "x": 3.7513502451988723, - "y": 6.8054830693306245, - "heading": 0.3525264976102127, - "angularVelocity": -0.20029697166969943, - "velocityX": 1.5464336269164098, - "velocityY": -0.02979488695523812, - "timestamp": 10.917499553550766 - }, - { - "x": 3.9227651512282486, - "y": 6.802180567172425, - "heading": 0.3303333796886753, - "angularVelocity": -0.2502713130553812, - "velocityX": 1.9330422052867193, - "velocityY": -0.03724224574585722, - "timestamp": 11.006175789155229 - }, - { - "x": 4.128463068361194, - "y": 6.79821773882544, - "heading": 0.3037147939289921, - "angularVelocity": -0.3001772186001935, - "velocityX": 2.319650983499712, - "velocityY": -0.04468872996211646, - "timestamp": 11.094852024759692 - }, - { - "x": 4.368444017678072, - "y": 6.793594678448866, - "heading": 0.2726776385751228, - "angularVelocity": -0.3500053327963678, - "velocityX": 2.706259999435507, - "velocityY": -0.05213415234714548, - "timestamp": 11.183528260364156 - }, - { - "x": 4.642708020512335, - "y": 6.788311497970363, - "heading": 0.23722930806330966, - "angularVelocity": -0.3997500600942169, - "velocityX": 3.092869255948188, - "velocityY": -0.05957831252634942, - "timestamp": 11.27220449596862 - }, - { - "x": 4.951255088173746, - "y": 6.782368328182758, - "heading": 0.19737735072702745, - "angularVelocity": -0.44940966499796203, - "velocityX": 3.479478640000833, - "velocityY": -0.0670209977576627, - "timestamp": 11.360880731573083 - }, - { - "x": 5.286562692788269, - "y": 6.775920770205836, - "heading": 0.19737734601955828, - "angularVelocity": -5.3086028753514235e-8, - "velocityX": 3.781256639153567, - "velocityY": -0.07270897251075666, - "timestamp": 11.449556967177546 - }, - { - "x": 5.595111114675473, - "y": 6.769985794431735, - "heading": 0.15789301075905496, - "angularVelocity": -0.4452639987631135, - "velocityX": 3.4794939115759345, - "velocityY": -0.06692859404378032, - "timestamp": 11.53823320278201 - }, - { - "x": 5.8693764275565705, - "y": 6.76471055259859, - "heading": 0.12280143262174693, - "angularVelocity": -0.3957269712466434, - "velocityX": 3.092884029318143, - "velocityY": -0.059488788593539996, - "timestamp": 11.626909438386473 - }, - { - "x": 6.109358599544783, - "y": 6.760094936608861, - "heading": 0.09209943623091729, - "angularVelocity": -0.3462257523850543, - "velocityX": 2.7062737874738274, - "velocityY": -0.05205020215694004, - "timestamp": 11.715585673990937 - }, - { - "x": 6.315057615509001, - "y": 6.756138852616793, - "heading": 0.06578495581515914, - "angularVelocity": -0.29674782918315123, - "velocityX": 2.3196633749963063, - "velocityY": -0.04461267401690494, - "timestamp": 11.8042619095954 - }, - { - "x": 6.4864734670806214, - "y": 6.752842221795132, - "heading": 0.043856783005957245, - "angularVelocity": -0.24728353272698234, - "velocityX": 1.9330528681462538, - "velocityY": -0.037176034810112814, - "timestamp": 11.892938145199864 - }, - { - "x": 6.623606149810205, - "y": 6.750204980767773, - "heading": 0.026314311963007715, - "angularVelocity": -0.19782606831887856, - "velocityX": 1.5464423111199512, - "velocityY": -0.029740110294281013, - "timestamp": 11.981614380804327 - }, - { - "x": 6.726455661694478, - "y": 6.74822708185799, - "heading": 0.013157327563715553, - "angularVelocity": -0.14837102984364775, - "velocityX": 1.1598317315027789, - "velocityY": -0.022304723427886322, - "timestamp": 12.07029061640879 - }, - { - "x": 6.795022002255089, - "y": 6.7469084932218335, - "heading": 0.004385844550104781, - "angularVelocity": -0.09891582512292933, - "velocityX": 0.7732211464912397, - "velocityY": -0.014869695664985829, - "timestamp": 12.158966852013254 + "angularVelocity": 2.0447435740886937e-25, + "velocityX": -6.220702573449239e-25, + "velocityY": -1.1679895694126021e-23, + "timestamp": 10.562785184650616 + }, + { + "x": 3.4428034429755563, + "y": 6.811427919071949, + "heading": 0.392498131917024, + "angularVelocity": -0.05010058941962764, + "velocityX": 0.38660845854541764, + "velocityY": -0.007449238843783358, + "timestamp": 10.651461420488184 + }, + { + "x": 3.511369400623633, + "y": 6.8101068002836795, + "heading": 0.38361316145922575, + "angularVelocity": -0.1001956203246293, + "velocityX": 0.7732168263622722, + "velocityY": -0.014898228096758812, + "timestamp": 10.740137656325752 + }, + { + "x": 3.614218332538516, + "y": 6.808125167755098, + "heading": 0.3702880707403863, + "angularVelocity": -0.15026676079539045, + "velocityX": 1.159825188151597, + "velocityY": -0.022346827307951896, + "timestamp": 10.82881389216332 + }, + { + "x": 3.7513502451752516, + "y": 6.805483069346682, + "heading": 0.35252648355271166, + "angularVelocity": -0.20029703583955935, + "velocityX": 1.5464336227343458, + "velocityY": -0.029794886797588863, + "timestamp": 10.917490128000887 + }, + { + "x": 3.9227651511900996, + "y": 6.8021805671983415, + "heading": 0.3303333582647084, + "angularVelocity": -0.2502713954689655, + "velocityX": 1.9330422000414584, + "velocityY": -0.03724224553678828, + "timestamp": 11.006166363838455 + }, + { + "x": 4.128463068303133, + "y": 6.7982177388648735, + "heading": 0.3037147633556069, + "angularVelocity": -0.3001773209889048, + "velocityX": 2.319650977177454, + "velocityY": -0.044688729692209925, + "timestamp": 11.094842599676022 + }, + { + "x": 4.3684440175926715, + "y": 6.793594678506847, + "heading": 0.27267759681211456, + "angularVelocity": -0.3500054580614399, + "velocityX": 2.7062599920132113, + "velocityY": -0.05213415200092422, + "timestamp": 11.18351883551359 + }, + { + "x": 4.642708020388071, + "y": 6.788311498054621, + "heading": 0.2372292525531904, + "angularVelocity": -0.3997502140692617, + "velocityX": 3.092869247379649, + "velocityY": -0.059578312073416795, + "timestamp": 11.272195071351158 + }, + { + "x": 4.9512550879867705, + "y": 6.7823683283089595, + "heading": 0.19737727735435437, + "angularVelocity": -0.44940986525222537, + "velocityX": 3.47947863014707, + "velocityY": -0.0670209971084869, + "timestamp": 11.360871307188726 + }, + { + "x": 5.286562691805109, + "y": 6.775920770291965, + "heading": 0.19737727264687205, + "angularVelocity": -5.308617659691649e-8, + "velocityX": 3.781256620235165, + "velocityY": -0.07270897277153024, + "timestamp": 11.449547543026293 + }, + { + "x": 5.59511111399723, + "y": 6.769985794491263, + "heading": 0.15789296561858565, + "angularVelocity": -0.44526367921853854, + "velocityX": 3.479493905867875, + "velocityY": -0.06692859416780687, + "timestamp": 11.53822377886386 + }, + { + "x": 5.869376427077159, + "y": 6.764710552640504, + "heading": 0.12280140353680047, + "angularVelocity": -0.3957267891486041, + "velocityX": 3.0928840234300474, + "velocityY": -0.05948878863580233, + "timestamp": 11.626900014701429 + }, + { + "x": 6.109358599210802, + "y": 6.760094936637857, + "heading": 0.0920994176200499, + "angularVelocity": -0.3462256333589605, + "velocityX": 2.706273781999814, + "velocityY": -0.05205020216578816, + "timestamp": 11.715576250538996 + }, + { + "x": 6.3150576152845925, + "y": 6.7561388526360995, + "heading": 0.06578494429363466, + "angularVelocity": -0.2967477484567179, + "velocityX": 2.3196633701342284, + "velocityY": -0.04461267400889573, + "timestamp": 11.804252486376564 + }, + { + "x": 6.486473466938695, + "y": 6.752842221807213, + "heading": 0.04385677628651476, + "angularVelocity": -0.24728347792396857, + "velocityX": 1.9330528639949482, + "velocityY": -0.037176034793867205, + "timestamp": 11.892928722214132 + }, + { + "x": 6.623606149728922, + "y": 6.750204980774611, + "heading": 0.02631430841636879, + "angularVelocity": -0.19782603201921237, + "velocityX": 1.546442307738673, + "velocityY": -0.029740110275233655, + "timestamp": 11.9816049580517 + }, + { + "x": 6.7264556616555025, + "y": 6.748227081861227, + "heading": 0.013157325998979796, + "angularVelocity": -0.14837100710374293, + "velocityX": 1.1598317289310056, + "velocityY": -0.022304723409858212, + "timestamp": 12.070281193889267 + }, + { + "x": 6.7950220022425825, + "y": 6.746908493222859, + "heading": 0.004385844089357537, + "angularVelocity": -0.09891581241325288, + "velocityX": 0.7732211447571573, + "velocityY": -0.01486969565085219, + "timestamp": 12.158957429726835 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -2.582999463513829e-28, + "angularVelocity": -0.04945906925268308, + "velocityX": 0.3866105659555495, + "velocityY": -0.007434847713796995, + "timestamp": 12.247633665564402 }, { "x": 6.829305171966553, "y": 6.746249198913574, - "heading": -4.086871260037018e-38, - "angularVelocity": -0.04945907457853377, - "velocityX": 0.3866105668308079, - "velocityY": -0.007434847721776456, - "timestamp": 12.247643087617718 + "heading": -1.1865093331139584e-28, + "angularVelocity": 5.378776164649684e-29, + "velocityX": -2.235889133711042e-27, + "velocityY": -8.75203817649461e-27, + "timestamp": 12.33630990140197 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 1.313163452748441, + "isStopPoint": true, + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 1.9508388110446415, + "isStopPoint": true, + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "timestamp": 3.5718037306257293, + "isStopPoint": false, + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 4.171613502771454, + "isStopPoint": false, + "x": 8.017494201660156, + "y": 7.457698345184326, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 6.337938967322568, + "isStopPoint": true, + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 7.393144074819516, + "isStopPoint": false, + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 14 + }, + { + "timestamp": 7.80236711611409, + "isStopPoint": false, + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 8.388134095961306, + "isStopPoint": false, + "x": 7.985864639282227, + "y": 5.863699913024902, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 9.446021992536338, + "isStopPoint": false, + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 + }, + { + "timestamp": 10.562785184650616, + "isStopPoint": true, + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 }, { + "timestamp": 12.33630990140197, + "isStopPoint": true, "x": 6.829305171966553, "y": 6.746249198913574, - "heading": -5.480350490542979e-38, - "angularVelocity": -1.571392014077605e-37, - "velocityX": 4.425248303324229e-37, - "velocityY": 1.8999126670467387e-37, - "timestamp": 12.336319323222181 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -18418,43 +18883,51 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 1 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 5 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 10 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 2 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "CenterLanePBDA": { "waypoints": [ @@ -18526,884 +18999,963 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 2.2921355473422064e-38, - "velocityX": 1.1360383232068741e-38, - "velocityY": -2.144062321628808e-38, + "heading": 1.5131221782640854e-28, + "angularVelocity": 8.299873633167507e-29, + "velocityX": 6.262502855031199e-23, + "velocityY": -1.723228528384229e-24, "timestamp": 0 }, { - "x": 1.32825806778149, + "x": 1.3282580677814901, "y": 5.589895062873393, - "heading": -2.8062774074031584e-19, - "angularVelocity": -2.995198561755265e-18, - "velocityX": 0.40860904707084805, - "velocityY": -0.011305499292899219, - "timestamp": 0.09369253321938388 + "heading": 3.410110874929926e-18, + "angularVelocity": 3.6396826395088883e-17, + "velocityX": 0.4086090460493266, + "velocityY": -0.011305499264635099, + "timestamp": 0.09369253345361556 }, { - "x": 1.4048253002214361, + "x": 1.4048253002214366, "y": 5.587776581164742, - "heading": -1.768868342127426e-19, - "angularVelocity": 1.1072483896415275e-18, - "velocityX": 0.8172180835441991, - "velocityY": -0.022610998292584186, - "timestamp": 0.18738506643876776 + "heading": -6.538031746043414e-18, + "angularVelocity": -1.0617860628189422e-16, + "velocityX": 0.8172180815011558, + "velocityY": -0.022610998236055942, + "timestamp": 0.1873850669072311 }, { - "x": 1.519676146824621, + "x": 1.5196761468246218, "y": 5.584598858658671, - "heading": -2.386873521248768e-19, - "angularVelocity": -6.596098514057519e-19, - "velocityX": 1.225827103364341, - "velocityY": -0.03391649683150392, - "timestamp": 0.28107759965815166 + "heading": -1.263908620936914e-17, + "angularVelocity": -6.511782997625411e-17, + "velocityX": 1.2258271002997754, + "velocityY": -0.03391649674671154, + "timestamp": 0.28107760036084667 }, { - "x": 1.6728106047825377, + "x": 1.6728106047825395, "y": 5.580361895432887, - "heading": -7.795008789281329e-19, - "angularVelocity": -5.772215866345043e-18, - "velocityX": 1.6344360932087083, - "velocityY": -0.04522199454104627, - "timestamp": 0.3747701328775355 + "heading": -1.4189092760310783e-17, + "angularVelocity": -1.654354401527969e-17, + "velocityX": 1.6344360891226193, + "velocityY": -0.04522199442798972, + "timestamp": 0.3747701338144622 }, { - "x": 1.8642286675420066, - "y": 5.575065691668706, - "heading": -1.0638718286937245e-18, - "angularVelocity": -3.0351506143991226e-18, - "velocityX": 2.0430450131096105, - "velocityY": -0.056527490315375004, - "timestamp": 0.4684626660969194 + "x": 1.8642286675420086, + "y": 5.575065691668705, + "heading": -1.1778293764866163e-17, + "angularVelocity": 2.5730961757873988e-17, + "velocityX": 2.0430450080019944, + "velocityY": -0.05652749017405418, + "timestamp": 0.4684626672680778 }, { - "x": 2.0939303023371445, - "y": 5.5687102482727004, - "heading": -1.0113200205360793e-18, - "angularVelocity": 5.608964381023318e-19, - "velocityX": 2.451653583293393, - "velocityY": -0.0678329764136413, - "timestamp": 0.5621551993163033 + "x": 2.0939303023371414, + "y": 5.568710248272701, + "heading": -1.2220695981018318e-17, + "angularVelocity": -4.721851357797737e-18, + "velocityX": 2.451653577164197, + "velocityY": -0.06783297624405477, + "timestamp": 0.5621552007216933 }, { - "x": 2.285348365096613, + "x": 2.2853483650966107, "y": 5.563414044508519, - "heading": -7.732946400687193e-19, - "angularVelocity": 2.5404946615264457e-18, - "velocityX": 2.04304501310961, - "velocityY": -0.056527490315375004, - "timestamp": 0.6558477325356872 + "heading": -5.403675872721793e-18, + "angularVelocity": 7.275948100517659e-17, + "velocityX": 2.0430450080019944, + "velocityY": -0.056527490174054194, + "timestamp": 0.6558477341753088 + }, + { + "x": 2.438482823054528, + "y": 5.559177081282735, + "heading": -1.4049765598642364e-18, + "angularVelocity": 4.267895386440672e-17, + "velocityX": 1.6344360891226193, + "velocityY": -0.04522199442798973, + "timestamp": 0.7495402676289245 + }, + { + "x": 2.5533336696577136, + "y": 5.555999358776664, + "heading": -6.020213691896355e-19, + "angularVelocity": 8.570108640736073e-18, + "velocityX": 1.2258271002997754, + "velocityY": -0.033916496746711544, + "timestamp": 0.8432328010825401 + }, + { + "x": 2.6299009020976603, + "y": 5.553880877068013, + "heading": -2.9970702235685054e-18, + "angularVelocity": -2.5562857212265518e-17, + "velocityX": 0.8172180815011557, + "velocityY": -0.022610998236055942, + "timestamp": 0.9369253345361557 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": -2.3882654133259072e-26, + "angularVelocity": 3.198835690592465e-17, + "velocityX": 0.40860904604932646, + "velocityY": -0.0113054992646351, + "timestamp": 1.0306178679897713 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": -3.2765992030044065e-26, + "angularVelocity": -9.642870627859863e-26, + "velocityX": -6.242213516414814e-23, + "velocityY": 1.2639029431778876e-24, + "timestamp": 1.124310401443387 + }, + { + "x": 2.6874100640586183, + "y": 5.560258515785914, + "heading": 0.002817682314802049, + "angularVelocity": 0.040980028708864365, + "velocityX": 0.2796139905217839, + "velocityY": 0.10816107172058755, + "timestamp": 1.193067852415903 + }, + { + "x": 2.7258611530795362, + "y": 5.575132263557277, + "heading": 0.00845311638740209, + "angularVelocity": 0.08196106738821132, + "velocityX": 0.5592279596910537, + "velocityY": 0.21632197763277797, + "timestamp": 1.2618253033884193 + }, + { + "x": 2.783537789070723, + "y": 5.597442863907905, + "heading": 0.01690577022745014, + "angularVelocity": 0.12293436886464493, + "velocityX": 0.8388419753117596, + "velocityY": 0.32448265657123054, + "timestamp": 1.3305827543609354 + }, + { + "x": 2.860439979827146, + "y": 5.6271902967884255, + "heading": 0.0281745416659326, + "angularVelocity": 0.1638916405290653, + "velocityX": 1.118456104301514, + "velocityY": 0.4326430439140473, + "timestamp": 1.3993402053334516 + }, + { + "x": 2.956567737639549, + "y": 5.664374537399098, + "heading": 0.042257802321547584, + "angularVelocity": 0.20482522921398508, + "velocityX": 1.3980704120463603, + "velocityY": 0.5408030705724766, + "timestamp": 1.4680976563059678 + }, + { + "x": 3.0719210791335985, + "y": 5.708995555819805, + "heading": 0.05915345476625426, + "angularVelocity": 0.24572831316071073, + "velocityX": 1.6776849615928617, + "velocityY": 0.6489626620763342, + "timestamp": 1.536855107278484 + }, + { + "x": 3.206500025035437, + "y": 5.761053316583894, + "heading": 0.07885900334385978, + "angularVelocity": 0.28659510058745824, + "velocityX": 1.9572998125778749, + "velocityY": 0.7571217377575138, + "timestamp": 1.6056125582510001 + }, + { + "x": 3.3603045998308527, + "y": 5.820547778199662, + "heading": 0.10137163887853046, + "angularVelocity": 0.32742103170272313, + "velocityX": 2.2369150196812018, + "velocityY": 0.865280209988437, + "timestamp": 1.6743700092235163 + }, + { + "x": 3.533334831246345, + "y": 5.88747889261074, + "heading": 0.12668833684449327, + "angularVelocity": 0.36820297448319306, + "velocityX": 2.5165306300353754, + "velocityY": 0.9734379832933463, + "timestamp": 1.7431274601960325 + }, + { + "x": 3.725590749352267, + "y": 5.961846604538833, + "heading": 0.15480596643975877, + "angularVelocity": 0.4089393832605092, + "velocityX": 2.796146677729085, + "velocityY": 1.081594952637503, + "timestamp": 1.8118849111685487 + }, + { + "x": 3.9370723845110165, + "y": 6.043650850427416, + "heading": 0.18572139907813143, + "angularVelocity": 0.4496302902609083, + "velocityX": 3.075763167009828, + "velocityY": 1.189750997623249, + "timestamp": 1.8806423621410648 + }, + { + "x": 4.167779758734844, + "y": 6.1328915548731855, + "heading": 0.2194315399767765, + "angularVelocity": 0.4902761871163567, + "velocityX": 3.3553799764340586, + "velocityY": 1.2979059459525522, + "timestamp": 1.949399813113581 + }, + { + "x": 4.410308645337483, + "y": 6.226696146473074, + "heading": 0.2194315452235168, + "angularVelocity": 7.630795245724027e-8, + "velocityX": 3.5273106139374217, + "velocityY": 1.3642825653525794, + "timestamp": 2.0181572640860974 + }, + { + "x": 4.652837676496205, + "y": 6.320500364327338, + "heading": 0.2194315504698994, + "angularVelocity": 7.630275018427045e-8, + "velocityX": 3.5273127163435154, + "velocityY": 1.364277129641693, + "timestamp": 2.086914715058614 + }, + { + "x": 4.897946357727051, + "y": 6.407341003417969, + "heading": 0.21943155572783843, + "angularVelocity": 7.647082562756639e-8, + "velocityX": 3.564830833080488, + "velocityY": 1.2629996874861198, + "timestamp": 2.15567216603113 + }, + { + "x": 5.130868247940416, + "y": 6.471009903129215, + "heading": 0.21943156034758332, + "angularVelocity": 7.235632921744003e-8, + "velocityX": 3.648117688337217, + "velocityY": 0.9972082873825086, + "timestamp": 2.2195193086159217 + }, + { + "x": 5.365529564309454, + "y": 6.527934435249553, + "heading": 0.21943156495282606, + "angularVelocity": 7.212919079793877e-8, + "velocityX": 3.6753612905604753, + "velocityY": 0.891575250133395, + "timestamp": 2.2833664512007132 + }, + { + "x": 5.60019098861159, + "y": 6.584858522432942, + "heading": 0.21943156955806847, + "angularVelocity": 7.212918610327084e-8, + "velocityX": 3.675362981052711, + "velocityY": 0.8915682813493487, + "timestamp": 2.3472135937855048 + }, + { + "x": 5.834852412919734, + "y": 6.6417826095915675, + "heading": 0.21943157416331086, + "angularVelocity": 7.212918546345333e-8, + "velocityX": 3.675362981146796, + "velocityY": 0.8915682809614962, + "timestamp": 2.4110607363702963 + }, + { + "x": 6.069513837223475, + "y": 6.698706696768341, + "heading": 0.21943157876855326, + "angularVelocity": 7.21291856487456e-8, + "velocityX": 3.6753629810778445, + "velocityY": 0.8915682812457394, + "timestamp": 2.474907878955088 + }, + { + "x": 6.304175182424159, + "y": 6.755631110035184, + "heading": 0.2194315833737959, + "angularVelocity": 7.212918914147318e-8, + "velocityX": 3.675361742133493, + "velocityY": 0.8915733886014583, + "timestamp": 2.5387550215398793 + }, + { + "x": 6.5374892679107495, + "y": 6.8178474433780725, + "heading": 0.2194315924580089, + "angularVelocity": 1.4228065067709048e-7, + "velocityX": 3.6542604107418026, + "velocityY": 0.9744576002013384, + "timestamp": 2.602602164124671 + }, + { + "x": 6.754532777659766, + "y": 6.881509233956424, + "heading": 0.2489007631101974, + "angularVelocity": 0.46155817565449186, + "velocityX": 3.399424014328823, + "velocityY": 0.9970969412422411, + "timestamp": 2.6664493067094623 + }, + { + "x": 6.95421230704469, + "y": 6.941327038715391, + "heading": 0.2778274120513566, + "angularVelocity": 0.4530609792402775, + "velocityX": 3.1274622684913846, + "velocityY": 0.9368908667999729, + "timestamp": 2.730296449294254 + }, + { + "x": 7.136568973398106, + "y": 6.997132034172886, + "heading": 0.3053765730205754, + "angularVelocity": 0.4314862005395546, + "velocityX": 2.8561445190947854, + "velocityY": 0.8740406100928324, + "timestamp": 2.7941435918790454 + }, + { + "x": 7.301617944582347, + "y": 7.048866046398329, + "heading": 0.3312582379964112, + "angularVelocity": 0.40536919786917625, + "velocityX": 2.585064334947336, + "velocityY": 0.8102792095470503, + "timestamp": 2.857990734463837 + }, + { + "x": 7.449367096097909, + "y": 7.096499636385878, + "heading": 0.3553246002750777, + "angularVelocity": 0.376937186291547, + "velocityX": 2.314107500102227, + "velocityY": 0.7460567232791978, + "timestamp": 2.9218378770486284 + }, + { + "x": 7.579821248483784, + "y": 7.140015034676201, + "heading": 0.37748571646004736, + "angularVelocity": 0.3470964445360872, + "velocityX": 2.0432261665058515, + "velocityY": 0.6815559244884293, + "timestamp": 2.98568501963342 + }, + { + "x": 7.692983655491274, + "y": 7.179400354716541, + "heading": 0.3976808702861806, + "angularVelocity": 0.31630473986072827, + "velocityX": 1.7723957944900266, + "velocityY": 0.6168689536581131, + "timestamp": 3.0495321622182114 + }, + { + "x": 7.78885666094427, + "y": 7.214647088460025, + "heading": 0.4158661907347705, + "angularVelocity": 0.28482590938880487, + "velocityX": 1.5016021324004238, + "velocityY": 0.5520487263259451, + "timestamp": 3.113379304803003 + }, + { + "x": 7.867442033486985, + "y": 7.245748846091361, + "heading": 0.43200843887480883, + "angularVelocity": 0.2528264772162807, + "velocityX": 1.2308361715381555, + "velocityY": 0.4871284190992695, + "timestamp": 3.1772264473877945 + }, + { + "x": 7.92874115504245, + "y": 7.272700652676186, + "heading": 0.4460815560128383, + "angularVelocity": 0.2204189031535715, + "velocityX": 0.9600918549182749, + "velocityY": 0.4221301924206222, + "timestamp": 3.241073589972586 + }, + { + "x": 7.972755134992524, + "y": 7.295498524714787, + "heading": 0.4580645976614163, + "angularVelocity": 0.18768328798213776, + "velocityX": 0.689364913889801, + "velocityY": 0.35706957454398996, + "timestamp": 3.3049207325573775 + }, + { + "x": 7.9994848833762395, + "y": 7.314139199968811, + "heading": 0.467940423936039, + "angularVelocity": 0.15467922094567124, + "velocityX": 0.4186522262639504, + "velocityY": 0.29195786215910857, + "timestamp": 3.368767875142169 + }, + { + "x": 8.008931159973145, + "y": 7.328619956970215, + "heading": 0.4756948301053548, + "angularVelocity": 0.12145267361053229, + "velocityX": 0.1479514386154507, + "velocityY": 0.2268035250312554, + "timestamp": 3.4326150177269605 + }, + { + "x": 7.983802853314603, + "y": 7.34099497259759, + "heading": 0.48253914652919067, + "angularVelocity": 0.07100820527560414, + "velocityX": -0.2607003894828745, + "velocityY": 0.12838793468070023, + "timestamp": 3.5290027013372 + }, + { + "x": 7.919285557898086, + "y": 7.343883947492977, + "heading": 0.48451888567323514, + "angularVelocity": 0.020539337287634327, + "velocityX": -0.6693520686460666, + "velocityY": 0.029972448628073807, + "timestamp": 3.6253903849474396 + }, + { + "x": 7.8153792867404235, + "y": 7.337286874127341, + "heading": 0.48163302720996115, + "angularVelocity": -0.029940116363242703, + "velocityX": -1.078003612762665, + "velocityY": -0.06844311553655845, + "timestamp": 3.721778068557679 + }, + { + "x": 7.672084051684559, + "y": 7.321203727123482, + "heading": 0.4738818599108745, + "angularVelocity": -0.08041657407630928, + "velocityX": -1.4866550340114346, + "velocityY": -0.16685894298377135, + "timestamp": 3.8181657521679186 + }, + { + "x": 7.489399863960927, + "y": 7.295634462778821, + "heading": 0.46126688957646533, + "angularVelocity": -0.1308774094563781, + "velocityX": -1.895306338747057, + "velocityY": -0.26527522383518304, + "timestamp": 3.914553435778158 + }, + { + "x": 7.26732673524187, + "y": 7.260579018302084, + "heading": 0.4437906761804973, + "angularVelocity": -0.18131168569872746, + "velocityX": -2.3039575223848003, + "velocityY": -0.3636921561315803, + "timestamp": 4.010941119388398 + }, + { + "x": 7.005864679481663, + "y": 7.216037310494284, + "heading": 0.42145659939316793, + "angularVelocity": -0.23171089864179226, + "velocityX": -2.712608561250158, + "velocityY": -0.46210995159831264, + "timestamp": 4.107328802998637 + }, + { + "x": 6.705013716725352, + "y": 7.162009232661868, + "heading": 0.39426855573546815, + "angularVelocity": -0.28206968607772986, + "velocityX": -3.1212593921527856, + "velocityY": -0.5605288539860318, + "timestamp": 4.203716486608877 + }, + { + "x": 6.364773887222121, + "y": 7.098494640966319, + "heading": 0.36223062299373554, + "angularVelocity": -0.3323861674203487, + "velocityX": -3.5299098054794116, + "velocityY": -0.6589492486652313, + "timestamp": 4.300104170219116 + }, + { + "x": 6.0103743066555975, + "y": 7.0131369660627, + "heading": 0.3622306184721976, + "angularVelocity": -4.6909913247045394e-8, + "velocityX": -3.6768139589244604, + "velocityY": -0.8855662020967079, + "timestamp": 4.396491853829356 + }, + { + "x": 5.655974854365273, + "y": 6.927778758566046, + "heading": 0.3622306139507427, + "angularVelocity": -4.690905296707159e-8, + "velocityX": -3.6768126280884714, + "velocityY": -0.8855717276266857, + "timestamp": 4.492879537439595 + }, + { + "x": 5.301574924606988, + "y": 6.842422680000666, + "heading": 0.3622305723626749, + "angularVelocity": -4.3146661671098113e-7, + "velocityX": -3.676817581708511, + "velocityY": -0.8855496404554449, + "timestamp": 4.589267221049835 + }, + { + "x": 4.986369111177269, + "y": 6.766504431310858, + "heading": 0.34532944076922883, + "angularVelocity": -0.17534534455448447, + "velocityX": -3.2701876590821435, + "velocityY": -0.7876343309254682, + "timestamp": 4.685654904660074 + }, + { + "x": 4.710566830571078, + "y": 6.700076677592526, + "heading": 0.3264669889522253, + "angularVelocity": -0.19569358978764517, + "velocityX": -2.8613850885912564, + "velocityY": -0.6891726331648808, + "timestamp": 4.782042588270314 + }, + { + "x": 4.474170499658151, + "y": 6.6431400154165186, + "heading": 0.30840377599143814, + "angularVelocity": -0.18740167087974618, + "velocityX": -2.452557443633948, + "velocityY": -0.5907047461192427, + "timestamp": 4.8784302718805534 + }, + { + "x": 4.277177709226617, + "y": 6.595693819551337, + "heading": 0.29230604083143724, + "angularVelocity": -0.16701029174116175, + "velocityX": -2.043754793694494, + "velocityY": -0.4922433457062721, + "timestamp": 4.974817955490793 + }, + { + "x": 4.119586252031972, + "y": 6.557737537391428, + "heading": 0.2788176445470824, + "angularVelocity": -0.13993900236152076, + "velocityX": -1.6349750434079608, + "velocityY": -0.393787678448541, + "timestamp": 5.0712056391010325 + }, + { + "x": 4.001394403519539, + "y": 6.529270747914268, + "heading": 0.2683469283541488, + "angularVelocity": -0.10863126699126655, + "velocityX": -1.226213184978727, + "velocityY": -0.2953363792024593, + "timestamp": 5.167593322711272 + }, + { + "x": 3.922600824939347, + "y": 6.510293128644836, + "heading": 0.26117603270245576, + "angularVelocity": -0.074396389487788, + "velocityX": -0.8174652157718444, + "velocityY": -0.1968884255603806, + "timestamp": 5.263981006321512 + }, + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": -0.038017455811817785, + "velocityX": -0.40872820385029696, + "velocityY": -0.09844312056835454, + "timestamp": 5.360368689931751 }, { - "x": 2.43848282305453, - "y": 5.559177081282735, - "heading": -4.312009437616493e-19, - "angularVelocity": 3.651237559230762e-18, - "velocityX": 1.634436093208708, - "velocityY": -0.04522199454104628, - "timestamp": 0.749540265755071 + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": -1.8368418613420646e-25, + "velocityX": -1.6872129777974468e-25, + "velocityY": 1.375029629161337e-25, + "timestamp": 5.456756373541991 + }, + { + "x": 3.856532695000803, + "y": 6.488812456981308, + "heading": 0.2579986728633844, + "angularVelocity": 0.005949010101817782, + "velocityX": -0.32577575413793, + "velocityY": -0.14647295262485988, + "timestamp": 5.538627921084927 + }, + { + "x": 3.8031475649179134, + "y": 6.464921556304128, + "heading": 0.2590017936055681, + "angularVelocity": 0.012252373043976347, + "velocityX": -0.6520596188180307, + "velocityY": -0.29180956503419114, + "timestamp": 5.620499468627864 + }, + { + "x": 3.7229939182763383, + "y": 6.429256431280001, + "heading": 0.2605597691269097, + "angularVelocity": 0.019029511082889358, + "velocityX": -0.979017119464356, + "velocityY": -0.43562294954083813, + "timestamp": 5.702371016170801 + }, + { + "x": 3.615995163614758, + "y": 6.38199294529701, + "heading": 0.26272711898941353, + "angularVelocity": 0.026472564981957637, + "velocityX": -1.3069101277884805, + "velocityY": -0.577288293692067, + "timestamp": 5.784242563713738 + }, + { + "x": 3.4820378233974707, + "y": 6.323397660276972, + "heading": 0.26558610226294477, + "angularVelocity": 0.03492035217768224, + "velocityX": -1.6361891797273596, + "velocityY": -0.7156977824232311, + "timestamp": 5.866114111256675 + }, + { + "x": 3.320936743362508, + "y": 6.253922161955944, + "heading": 0.26927508652417376, + "angularVelocity": 0.045058196307995436, + "velocityX": -1.967729753129128, + "velocityY": -0.8485914875933167, + "timestamp": 5.947985658799611 + }, + { + "x": 3.1323379926537895, + "y": 6.1744965280304065, + "heading": 0.2740753686856588, + "angularVelocity": 0.0586318728001031, + "velocityX": -2.3035933284369614, + "velocityY": -0.9701249861422718, + "timestamp": 6.029857206342548 + }, + { + "x": 2.9153402053664754, + "y": 6.088110219486258, + "heading": 0.2808618012574976, + "angularVelocity": 0.0828912213767512, + "velocityX": -2.650466417206927, + "velocityY": -1.0551444443974112, + "timestamp": 6.111728753885485 + }, + { + "x": 2.698015397521732, + "y": 6.030900368675538, + "heading": 0.2958528133927384, + "angularVelocity": 0.18310405244726724, + "velocityX": -2.6544607298496428, + "velocityY": -0.6987757350099817, + "timestamp": 6.193600301428422 + }, + { + "x": 2.504490270491426, + "y": 5.9906815262758615, + "heading": 0.3120114847728296, + "angularVelocity": 0.19736614080266282, + "velocityX": -2.363765347525813, + "velocityY": -0.49124321704783075, + "timestamp": 6.275471848971359 + }, + { + "x": 2.336082741941251, + "y": 5.965439222431395, + "heading": 0.3286454370988765, + "angularVelocity": 0.20317134371160356, + "velocityX": -2.0569725821031444, + "velocityY": -0.30831594860508243, + "timestamp": 6.357343396514295 + }, + { + "x": 2.1932220965517097, + "y": 5.954429216642808, + "heading": 0.3455083861254867, + "angularVelocity": 0.20596836792130419, + "velocityX": -1.744936423909899, + "velocityY": -0.1344790237757827, + "timestamp": 6.439214944057232 + }, + { + "x": 2.0761197199383044, + "y": 5.957265361507106, + "heading": 0.36247439681677673, + "angularVelocity": 0.20722718942612423, + "velocityX": -1.4303183477995456, + "velocityY": 0.034641398012055055, + "timestamp": 6.521086491600169 + }, + { + "x": 1.9849012059686333, + "y": 5.973711453341115, + "heading": 0.37946704953730365, + "angularVelocity": 0.20755260197830386, + "velocityX": -1.1141662361009146, + "velocityY": 0.2008767676632038, + "timestamp": 6.602958039143106 + }, + { + "x": 1.9196497213524515, + "y": 6.003608138127398, + "heading": 0.39643505242175614, + "angularVelocity": 0.20725152258241855, + "velocityX": -0.7969983049601116, + "velocityY": 0.36516574663016305, + "timestamp": 6.6848295866860425 }, { - "x": 2.553333669657715, - "y": 5.555999358776664, - "heading": -7.015602955490743e-20, - "angularVelocity": 3.853507870950435e-18, - "velocityX": 1.2258271033643409, - "velocityY": -0.033916496831503926, - "timestamp": 0.8432327989744549 + "x": 1.8804243803024292, + "y": 6.046840667724609, + "heading": 0.41334160220496513, + "angularVelocity": 0.2065009187024656, + "velocityX": -0.47910833772197736, + "velocityY": 0.5280531624803828, + "timestamp": 6.766701134228979 + }, + { + "x": 1.8671378435771844, + "y": 6.102598644221004, + "heading": 0.42998500556879043, + "angularVelocity": 0.2054233650605208, + "velocityX": -0.1639908031089557, + "velocityY": 0.6882000580332149, + "timestamp": 6.8477211455549645 + }, + { + "x": 1.8794728316222993, + "y": 6.171151657888497, + "heading": 0.44648318199526155, + "angularVelocity": 0.2036308827468621, + "velocityX": 0.15224619008611265, + "velocityY": 0.8461244641360118, + "timestamp": 6.92874115688095 + }, + { + "x": 1.9175617070092363, + "y": 6.252231071792971, + "heading": 0.462750092926245, + "angularVelocity": 0.2007764583682102, + "velocityX": 0.4701168854900097, + "velocityY": 1.000733184030919, + "timestamp": 7.009761168206935 + }, + { + "x": 1.9816154560724233, + "y": 6.345393254847428, + "heading": 0.47864458300503304, + "angularVelocity": 0.1961798056882073, + "velocityX": 0.7905917070964839, + "velocityY": 1.1498663296851208, + "timestamp": 7.09078117953292 + }, + { + "x": 2.0720214338642493, + "y": 6.44976750066492, + "heading": 0.4938927703977008, + "angularVelocity": 0.1882027309440445, + "velocityX": 1.1158475086861754, + "velocityY": 1.288252668807229, + "timestamp": 7.171801190858905 + }, + { + "x": 2.189694481994004, + "y": 6.562891621237751, + "heading": 0.5077401370803508, + "angularVelocity": 0.17091291961111932, + "velocityX": 1.452394861515095, + "velocityY": 1.3962491330404174, + "timestamp": 7.25282120218489 + }, + { + "x": 2.330980243223138, + "y": 6.6599214066012715, + "heading": 0.5136983140037735, + "angularVelocity": 0.07353957159361435, + "velocityX": 1.7438378360707576, + "velocityY": 1.197602712904113, + "timestamp": 7.333841213510875 + }, + { + "x": 2.4512963275001454, + "y": 6.737453803424918, + "heading": 0.5174619765609972, + "angularVelocity": 0.04645349334845922, + "velocityX": 1.48501687802677, + "velocityY": 0.9569536655789208, + "timestamp": 7.41486122483686 + }, + { + "x": 2.548343024270106, + "y": 6.798294060832851, + "heading": 0.5200641230004012, + "angularVelocity": 0.032117329988194755, + "velocityX": 1.1978114441319052, + "velocityY": 0.7509287694757523, + "timestamp": 7.4958812361628455 + }, + { + "x": 2.6214712468816064, + "y": 6.843385043467383, + "heading": 0.5218324598045924, + "angularVelocity": 0.021825926400778378, + "velocityX": 0.9025945740400864, + "velocityY": 0.5565413025321349, + "timestamp": 7.576901247488831 + }, + { + "x": 2.6703780246763915, + "y": 6.873196303781186, + "heading": 0.5229271765337693, + "angularVelocity": 0.01351168324048117, + "velocityX": 0.6036382492963173, + "velocityY": 0.367949347647667, + "timestamp": 7.657921258814816 }, { - "x": 2.6299009020976607, - "y": 5.553880877068013, - "heading": -2.7026882274997263e-20, - "angularVelocity": 4.603264080766157e-19, - "velocityX": 0.817218083544199, - "velocityY": -0.022610998292584186, - "timestamp": 0.9369253321938388 + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": 0.006368644706239802, + "velocityX": 0.30252191119511246, + "velocityY": 0.1828226198606205, + "timestamp": 7.738941270140801 }, { - "x": 2.668184518814087, - "y": 5.552821636199951, + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": 4.786924324290607e-29, + "velocityX": -3.940142843909209e-27, + "velocityY": 5.40414297722828e-27, + "timestamp": 7.819961281466786 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, - "angularVelocity": 2.884635663770645e-19, - "velocityX": 0.40860904707084805, - "velocityY": -0.011305499292899219, - "timestamp": 1.0306178654132228 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { + "timestamp": 1.124310401443387, + "isStopPoint": true, "x": 2.668184518814087, "y": 5.552821636199951, "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.1410946614987667e-37, - "timestamp": 1.1243103986326066 - }, - { - "x": 2.687410064097537, - "y": 5.560258515634833, - "heading": 0.0028176822125938034, - "angularVelocity": 0.04098002734374148, - "velocityX": 0.2796139919160097, - "velocityY": 0.1081610698436357, - "timestamp": 1.1930678494014684 - }, - { - "x": 2.7258611531960137, - "y": 5.575132263104083, - "heading": 0.008453116109374614, - "angularVelocity": 0.08196106507388369, - "velocityX": 0.5592279624754555, - "velocityY": 0.2163219738796251, - "timestamp": 1.2618253001703301 - }, - { - "x": 2.783537789303066, - "y": 5.597442863001631, - "heading": 0.016905769734324238, - "angularVelocity": 0.12293436610040698, - "velocityX": 0.8388419794814749, - "velocityY": 0.3244826509427688, - "timestamp": 1.3305827509391919 - }, - { - "x": 2.8604399802132514, - "y": 5.627190295278175, - "heading": 0.028174540960364546, - "angularVelocity": 0.1638916379247738, - "velocityX": 1.118456109850604, - "velocityY": 0.4326430364113507, - "timestamp": 1.3993402017080536 - }, - { - "x": 2.9565677382168056, - "y": 5.664374535134068, - "heading": 0.042257801458542396, - "angularVelocity": 0.20482522753092394, - "velocityX": 1.3980704189674162, - "velocityY": 0.5408030611968617, - "timestamp": 1.4680976524769154 - }, - { - "x": 3.0719210799387406, - "y": 5.708995552649304, - "heading": 0.05915345386800317, - "angularVelocity": 0.2457283133759373, - "velocityX": 1.6776849698763825, - "velocityY": 0.648962650829455, - "timestamp": 1.5368551032457771 - }, - { - "x": 3.2065000261043313, - "y": 5.761053312357379, - "heading": 0.07885900262196403, - "angularVelocity": 0.2865951040012339, - "velocityX": 1.9572998222112317, - "velocityY": 0.757121724641519, - "timestamp": 1.6056125540146389 - }, - { - "x": 3.360304601198153, - "y": 5.820547772766793, - "heading": 0.1013716386695389, - "angularVelocity": 0.32742104013214957, - "velocityX": 2.2369150306467707, - "velocityY": 0.8652801950062525, - "timestamp": 1.6743700047835006 - }, - { - "x": 3.5333348329448953, - "y": 5.8874788858214675, - "heading": 0.12668833767209217, - "angularVelocity": 0.36820299064984424, - "velocityX": 2.5165306423068015, - "velocityY": 0.973437966449241, - "timestamp": 1.7431274555523624 - }, - { - "x": 3.7255907514118967, - "y": 5.961846596243587, - "heading": 0.15480596913918515, - "angularVelocity": 0.4089394116954055, - "velocityX": 2.796146691262561, - "velocityY": 1.0815949339384094, - "timestamp": 1.8118849063212241 - }, - { - "x": 3.937072386955544, - "y": 6.04365084047756, - "heading": 0.18572140510730675, - "angularVelocity": 0.44963034002016317, - "velocityX": 3.075763181717921, - "velocityY": 1.1897509770827353, - "timestamp": 1.8806423570900859 - }, - { - "x": 4.167779761570079, - "y": 6.132891543122838, - "heading": 0.21943155266112502, - "angularVelocity": 0.4902762853605699, - "velocityX": 3.355379992054849, - "velocityY": 1.2979059236107295, - "timestamp": 1.9493998078589476 - }, - { - "x": 4.410308649457132, - "y": 6.2266961328710915, - "heading": 0.21943155790785696, - "angularVelocity": 7.630783127279499e-8, - "velocityX": 3.5273106430654275, - "velocityY": 1.3642825424635276, - "timestamp": 2.0181572586278094 - }, - { - "x": 4.652837676050608, - "y": 6.320500363997887, - "heading": 0.21943156315423873, - "angularVelocity": 7.630273806741925e-8, - "velocityX": 3.5273126603947684, - "velocityY": 1.3642773267166644, - "timestamp": 2.0869147093966713 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { + "timestamp": 2.15567216603113, + "isStopPoint": false, "x": 4.897946357727051, "y": 6.407341003417969, - "heading": 0.21943156841217684, - "angularVelocity": 7.647081243446701e-8, - "velocityX": 3.5648308501199604, - "velocityY": 1.2629996960185401, - "timestamp": 2.1556721601655333 - }, - { - "x": 5.1308682473529785, - "y": 6.471009903235158, - "heading": 0.21943157303192085, - "angularVelocity": 7.23563158909772e-8, - "velocityX": 3.6481177055177683, - "velocityY": 0.9972082962532426, - "timestamp": 2.2195193022886155 - }, - { - "x": 5.365529567427774, - "y": 6.527934417793533, - "heading": 0.21943157763716262, - "angularVelocity": 7.21291764013049e-8, - "velocityX": 3.6753613751798433, - "velocityY": 0.8915749815181846, - "timestamp": 2.2833664444116977 - }, - { - "x": 5.600190991292763, - "y": 6.584858504493345, - "heading": 0.21943158224240422, - "angularVelocity": 7.21291734995501e-8, - "velocityX": 3.6753630007842206, - "velocityY": 0.8915682802227483, - "timestamp": 2.34721358653478 - }, - { - "x": 5.8348524151632635, - "y": 6.641782591170481, - "heading": 0.2194315868476458, - "angularVelocity": 7.212917336838375e-8, - "velocityX": 3.6753630008705414, - "velocityY": 0.8915682798675781, - "timestamp": 2.411060728657862 - }, - { - "x": 6.069513839029734, - "y": 6.698706677864191, - "heading": 0.21943159145288735, - "angularVelocity": 7.212917280605725e-8, - "velocityX": 3.6753630008074327, - "velocityY": 0.891568280127171, - "timestamp": 2.474907870780944 - }, - { - "x": 6.3041751870414915, - "y": 6.75563107725722, - "heading": 0.21943159605812906, - "angularVelocity": 7.212917561549481e-8, - "velocityX": 3.6753618127399528, - "velocityY": 0.8915731777514998, - "timestamp": 2.5387550129040264 - }, - { - "x": 6.537489271202827, - "y": 6.817847413489264, - "heading": 0.21943160514124108, - "angularVelocity": 1.4226340838423697e-7, - "velocityX": 3.6542604164106804, - "velocityY": 0.9744576524998471, - "timestamp": 2.6026021550271086 - }, - { - "x": 6.7545327794045615, - "y": 6.881509207420976, - "heading": 0.24890077922126197, - "angularVelocity": 0.46155823268064305, - "velocityX": 3.399424014677561, - "velocityY": 0.9970970009742466, - "timestamp": 2.6664492971501907 - }, - { - "x": 6.954212307545749, - "y": 6.941327014961267, - "heading": 0.2778274290995393, - "angularVelocity": 0.45306099719431336, - "velocityX": 3.1274622716276532, - "velocityY": 0.9368909171372027, - "timestamp": 2.730296439273273 - }, - { - "x": 7.1365689728963595, - "y": 6.997132012930522, - "heading": 0.305376589845945, - "angularVelocity": 0.43148620017020184, - "velocityX": 2.856144524042602, - "velocityY": 0.8740406557535503, - "timestamp": 2.794143581396355 - }, - { - "x": 7.301617943292834, - "y": 7.048866027505986, - "heading": 0.33125825392103403, - "angularVelocity": 0.40536918669284566, - "velocityX": 2.585064341302851, - "velocityY": 0.8102792522134878, - "timestamp": 2.8579907235194373 - }, - { - "x": 7.449367094221676, - "y": 7.096499619737108, - "heading": 0.35532461486989275, - "angularVelocity": 0.37693716818945566, - "velocityX": 2.3141075076472033, - "velocityY": 0.7460567638139751, - "timestamp": 2.9218378656425195 - }, - { - "x": 7.579821246213137, - "y": 7.140015020198173, - "heading": 0.3774857294479878, - "angularVelocity": 0.347096421878647, - "velocityX": 2.04322617510391, - "velocityY": 0.681555963416076, - "timestamp": 2.9856850077656016 - }, - { - "x": 7.69298365301254, - "y": 7.179400342359022, - "heading": 0.3976808814927073, - "angularVelocity": 0.3163047142468991, - "velocityX": 1.7723958040479335, - "velocityY": 0.6168689913312067, - "timestamp": 3.049532149888684 - }, - { - "x": 7.788856658439436, - "y": 7.214647078189013, - "heading": 0.41586620005922853, - "angularVelocity": 0.2848258819708253, - "velocityX": 1.5016021428504343, - "velocityY": 0.5520487629977263, - "timestamp": 3.113379292011766 - }, - { - "x": 7.867442031134741, - "y": 7.2457488378850705, - "heading": 0.43200844627235874, - "angularVelocity": 0.2528264488645923, - "velocityX": 1.2308361828288252, - "velocityY": 0.48712845496042256, - "timestamp": 3.177226434134848 - }, - { - "x": 7.928741153018892, - "y": 7.27270064652237, - "heading": 0.4460815614823235, - "angularVelocity": 0.22041887454940703, - "velocityX": 0.9600918670092122, - "velocityY": 0.4221302276198955, - "timestamp": 3.2410735762579304 - }, - { - "x": 7.9727551334716695, - "y": 7.29549852060884, - "heading": 0.45806460123681797, - "angularVelocity": 0.1876832596734667, - "velocityX": 0.6893649267484526, - "velocityY": 0.35706960920065395, - "timestamp": 3.3049207183810125 - }, - { - "x": 7.999484882530383, - "y": 7.314139197912392, - "heading": 0.4679404256802237, - "angularVelocity": 0.1546791933829663, - "velocityX": 0.4186522398635212, - "velocityY": 0.2919578963709263, - "timestamp": 3.3687678605040947 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 20 }, { + "timestamp": 3.4326150177269605, + "isStopPoint": false, "x": 8.008931159973145, "y": 7.328619956970215, "heading": 0.4756948301053548, - "angularVelocity": 0.12145264717068183, - "velocityX": 0.1479514529335022, - "velocityY": 0.22680355887981873, - "timestamp": 3.432615002627177 - }, - { - "x": 7.9838028545378235, - "y": 7.340994975747599, - "heading": 0.4825391441548679, - "angularVelocity": 0.07100818067139564, - "velocityX": -0.26070037689812714, - "velocityY": 0.12838796741348574, - "timestamp": 3.5290026861982695 - }, - { - "x": 7.919285560188609, - "y": 7.343883953710174, - "heading": 0.4845188810802152, - "angularVelocity": 0.02053931427751041, - "velocityX": -0.6693520578449088, - "velocityY": 0.029972480461632738, - "timestamp": 3.625390369769362 - }, - { - "x": 7.815379289936097, - "y": 7.337286883355744, - "heading": 0.4816330205236378, - "angularVelocity": -0.029940138092932247, - "velocityX": -1.0780036038097405, - "velocityY": -0.06844308432376209, - "timestamp": 3.7217780533404548 - }, - { - "x": 7.6720840556149295, - "y": 7.321203739342901, - "heading": 0.47388185121605153, - "angularVelocity": -0.08041659494667612, - "velocityX": -1.4866550269929244, - "velocityY": -0.16685891202042144, - "timestamp": 3.8181657369115474 - }, - { - "x": 7.489399868443907, - "y": 7.295634478019173, - "heading": 0.46126687890088164, - "angularVelocity": -0.13087743005945188, - "velocityX": -1.895306333783606, - "velocityY": -0.2652751926014197, - "timestamp": 3.91455342048264 - }, - { - "x": 7.267326740077932, - "y": 7.2605790366684495, - "heading": 0.44379066346613116, - "angularVelocity": -0.1813117069242423, - "velocityX": -2.3039575196573705, - "velocityY": -0.36369212384761535, - "timestamp": 4.010941104053733 - }, - { - "x": 7.00586468444221, - "y": 7.216037332217007, - "heading": 0.4214565844390286, - "angularVelocity": -0.23171092197300747, - "velocityX": -2.7126085610603474, - "velocityY": -0.4621099169645314, - "timestamp": 4.107328787624826 - }, - { - "x": 6.7050137215236445, - "y": 7.162009258221821, - "heading": 0.3942685380548319, - "angularVelocity": -0.2820697144790415, - "velocityX": -3.1212593951037992, - "velocityY": -0.5605288144032944, - "timestamp": 4.203716471195919 - }, - { - "x": 6.364773891396992, - "y": 7.098494671595923, - "heading": 0.3622306012435557, - "angularVelocity": -0.3323862097758952, - "velocityX": -3.529909813380894, - "velocityY": -0.658949196336373, - "timestamp": 4.300104154767012 - }, - { - "x": 6.010374312951181, - "y": 7.013136980735367, - "heading": 0.36223059672201996, - "angularVelocity": -4.690989126354215e-8, - "velocityX": -3.6768139384158545, - "velocityY": -0.8855663680059631, - "timestamp": 4.396491838338105 - }, - { - "x": 5.655974859532265, - "y": 6.927778770772817, - "heading": 0.36223059220056536, - "angularVelocity": -4.690904952415849e-8, - "velocityX": -3.676812641290635, - "velocityY": -0.885571753569461, - "timestamp": 4.492879521909198 - }, - { - "x": 5.301574928644714, - "y": 6.842422689744232, - "heading": 0.3622305506125328, - "angularVelocity": -4.314662515297724e-7, - "velocityX": -3.6768175949176816, - "velocityY": -0.8855496663702899, - "timestamp": 4.589267205480291 - }, - { - "x": 4.986369114322999, - "y": 6.766504438890574, - "heading": 0.34532942408176365, - "angularVelocity": -0.1753452921016411, - "velocityX": -3.270187669664538, - "velocityY": -0.7876343536948325, - "timestamp": 4.685654889051384 - }, - { - "x": 4.710566832932512, - "y": 6.70007668327783, - "heading": 0.3264669766789321, - "angularVelocity": -0.1956935440711402, - "velocityX": -2.861385097890258, - "velocityY": -0.6891726530988762, - "timestamp": 4.782042572622477 - }, - { - "x": 4.474170501345758, - "y": 6.643140019477641, - "heading": 0.30840376738356945, - "angularVelocity": -0.18740163292794487, - "velocityX": -2.4525574516208333, - "velocityY": -0.5907047632096585, - "timestamp": 4.87843025619357 - }, - { - "x": 4.277177710352041, - "y": 6.5956938222588235, - "heading": 0.2923060351860776, - "angularVelocity": -0.16701026107365813, - "velocityX": -2.0437548003570405, - "velocityY": -0.4922433599498598, - "timestamp": 4.974817939764663 - }, - { - "x": 4.1195862527073634, - "y": 6.557737539015946, - "heading": 0.27881764120910085, - "angularVelocity": -0.1399389784798502, - "velocityX": -1.6349750487409798, - "velocityY": -0.3937876898440308, - "timestamp": 5.071205623335756 - }, - { - "x": 4.0013944038572795, - "y": 6.529270748726535, - "heading": 0.26834692670698207, - "angularVelocity": -0.10863124949357753, - "velocityX": -1.226213188979785, - "velocityY": -0.2953363877493204, - "timestamp": 5.167593306906849 - }, - { - "x": 3.9226008250519366, - "y": 6.510293128915594, - "heading": 0.26117603215990287, - "angularVelocity": -0.07439637805789222, - "velocityX": -0.817465218439728, - "velocityY": -0.19688843125838204, - "timestamp": 5.263980990477942 - }, - { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "angularVelocity": -0.038017450198398375, - "velocityX": -0.40872820518438957, - "velocityY": -0.09844312341738799, - "timestamp": 5.3603686740490355 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 }, { + "timestamp": 5.456756373541991, + "isStopPoint": true, "x": 3.883204460144043, "y": 6.500804424285889, "heading": 0.2575116182, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.0638409913121086e-32, - "timestamp": 5.456756357620129 - }, - { - "x": 3.856532695000848, - "y": 6.488812456981272, - "heading": 0.25799867286334044, - "angularVelocity": 0.005949010116156059, - "velocityX": -0.32577575495196986, - "velocityY": -0.14647295299154703, - "timestamp": 5.53862790495835 - }, - { - "x": 3.8031475649180457, - "y": 6.464921556304023, - "heading": 0.25900179360543873, - "angularVelocity": 0.01225237307357005, - "velocityX": -0.6520596204474033, - "velocityY": -0.29180956576467965, - "timestamp": 5.620499452296571 - }, - { - "x": 3.7229939182765985, - "y": 6.429256431279799, - "heading": 0.2605597691266567, - "angularVelocity": 0.019029511128961094, - "velocityX": -0.9790171219107733, - "velocityY": -0.43562295063126366, - "timestamp": 5.702370999634793 - }, - { - "x": 3.6159951636151835, - "y": 6.38199294529669, - "heading": 0.26272711898900253, - "angularVelocity": 0.02647256504622092, - "velocityX": -1.3069101310543174, - "velocityY": -0.5772882951369854, - "timestamp": 5.784242546973014 - }, - { - "x": 3.4820378233980955, - "y": 6.3233976602765205, - "heading": 0.2655861022623468, - "angularVelocity": 0.03492035226271501, - "velocityX": -1.6361891838161302, - "velocityY": -0.7156977842143984, - "timestamp": 5.866114094311236 - }, - { - "x": 3.3209367433633594, - "y": 6.253922161955361, - "heading": 0.26927508652336807, - "angularVelocity": 0.04505819641812337, - "velocityX": -1.9677297580465591, - "velocityY": -0.8485914897167804, - "timestamp": 5.947985641649457 - }, - { - "x": 3.132337992654885, - "y": 6.1744965280297155, - "heading": 0.27407536868463933, - "angularVelocity": 0.0586318729440976, - "velocityX": -2.303593334193982, - "velocityY": -0.9701249885693223, - "timestamp": 6.029857188987679 - }, - { - "x": 2.9153402053678152, - "y": 6.088110219485557, - "heading": 0.2808618012562985, - "angularVelocity": 0.08289122158182176, - "velocityX": -2.650466423831284, - "velocityY": -1.0551444470358684, - "timestamp": 6.1117287363259 - }, - { - "x": 2.6980153975230565, - "y": 6.030900368674856, - "heading": 0.29585281339155545, - "angularVelocity": 0.1831040529053057, - "velocityX": -2.6544607364871555, - "velocityY": -0.6987757367569974, - "timestamp": 6.1936002836641215 - }, - { - "x": 2.5044902704926533, - "y": 5.990681526275272, - "heading": 0.31201148477172486, - "angularVelocity": 0.1973661412971212, - "velocityX": -2.3637653534374596, - "velocityY": -0.4912432182750236, - "timestamp": 6.275471831002343 - }, - { - "x": 2.3360827419423367, - "y": 5.965439222430923, - "heading": 0.328645437097879, - "angularVelocity": 0.20317134422093217, - "velocityX": -2.0569725872482194, - "velocityY": -0.30831594937456525, - "timestamp": 6.357343378340564 - }, - { - "x": 2.1932220965526215, - "y": 5.95442921664246, - "heading": 0.34550838612461254, - "angularVelocity": 0.2059683684378234, - "velocityX": -1.744936428275136, - "velocityY": -0.13447902411053952, - "timestamp": 6.439214925678786 - }, - { - "x": 2.076119719939017, - "y": 5.957265361506873, - "heading": 0.3624743968160351, - "angularVelocity": 0.20722718994590164, - "velocityX": -1.4303183513784072, - "velocityY": 0.03464139810008477, - "timestamp": 6.521086473017007 - }, - { - "x": 1.984901205969126, - "y": 5.973711453340982, - "heading": 0.37946704953669896, - "angularVelocity": 0.20755260249895005, - "velocityX": -1.1141662388895142, - "velocityY": 0.20087676816670857, - "timestamp": 6.602958020355229 - }, - { - "x": 1.9196497213527057, - "y": 6.003608138127345, - "heading": 0.39643505242128974, - "angularVelocity": 0.207251523102328, - "velocityX": -0.7969983069558729, - "velocityY": 0.3651657475442049, - "timestamp": 6.68482956769345 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { + "timestamp": 6.766701134228979, + "isStopPoint": false, "x": 1.8804243803024292, "y": 6.046840667724609, - "heading": 0.41334160220463595, - "angularVelocity": 0.20650091922048514, - "velocityX": -0.4791083389230676, - "velocityY": 0.5280531638014023, - "timestamp": 6.766701115031672 - }, - { - "x": 1.8671378435769306, - "y": 6.1025986442210955, - "heading": 0.4299850055686104, - "angularVelocity": 0.20542336557580967, - "velocityX": -0.16399080352197462, - "velocityY": 0.6882000597544763, - "timestamp": 6.84772112615515 - }, - { - "x": 1.8794728316218305, - "y": 6.171151657888672, - "heading": 0.4464831819952248, - "angularVelocity": 0.20363088325759873, - "velocityX": 0.1522461904639937, - "velocityY": 0.8461244662518951, - "timestamp": 6.928741137278629 - }, - { - "x": 1.9175617070085949, - "y": 6.252231071793212, - "heading": 0.46275009292634184, - "angularVelocity": 0.2007764588716919, - "velocityX": 0.4701168866629144, - "velocityY": 1.0007331865330475, - "timestamp": 7.009761148402108 - }, - { - "x": 1.9816154560716552, - "y": 6.34539325484771, - "heading": 0.47864458300524776, - "angularVelocity": 0.19617980618000597, - "velocityX": 0.7905917090709768, - "velocityY": 1.1498663325596743, - "timestamp": 7.090781159525586 - }, - { - "x": 2.0720214338634078, - "y": 6.449767500665198, - "heading": 0.4938927703980073, - "angularVelocity": 0.18820273141558316, - "velocityX": 1.115847511474288, - "velocityY": 1.2882526720271141, - "timestamp": 7.171801170649065 - }, - { - "x": 2.1896944819931505, - "y": 6.562891621237937, - "heading": 0.5077401370806992, - "angularVelocity": 0.17091292003882794, - "velocityX": 1.4523948651451515, - "velocityY": 1.3962491365291643, - "timestamp": 7.252821181772544 - }, - { - "x": 2.3309802432224416, - "y": 6.659921406601519, - "heading": 0.5136983140040886, - "angularVelocity": 0.07353957177701248, - "velocityX": 1.7438378404313606, - "velocityY": 1.1976027158982174, - "timestamp": 7.333841192896022 - }, - { - "x": 2.4512963274996546, - "y": 6.737453803425126, - "heading": 0.5174619765612319, - "angularVelocity": 0.04645349346357551, - "velocityX": 1.4850168817410538, - "velocityY": 0.9569536679703065, - "timestamp": 7.414861204019501 - }, - { - "x": 2.5483430242698013, - "y": 6.798294060832995, - "heading": 0.5200641230005525, - "angularVelocity": 0.03211733006744314, - "velocityX": 1.197811447128081, - "velocityY": 0.7509287713518723, - "timestamp": 7.49588121514298 - }, - { - "x": 2.62147124688145, - "y": 6.843385043467462, - "heading": 0.5218324598046723, - "angularVelocity": 0.02182592645444906, - "velocityX": 0.9025945762979182, - "velocityY": 0.556541303922395, - "timestamp": 7.576901226266458 - }, - { - "x": 2.6703780246763387, - "y": 6.8731963037812145, - "heading": 0.5229271765337971, - "angularVelocity": 0.013511683273610298, - "velocityX": 0.6036382508063606, - "velocityY": 0.367949348566719, - "timestamp": 7.657921237389937 - }, - { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, - "angularVelocity": 0.006368644721814939, - "velocityX": 0.30252191195191025, - "velocityY": 0.18282262031723026, - "timestamp": 7.738941248513416 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { + "timestamp": 7.819961281466786, + "isStopPoint": true, "x": 2.6948883533477783, "y": 6.8880085945129395, "heading": 0.5234431642, - "angularVelocity": -2.018007055520513e-33, - "velocityX": -1.0144673376047062e-35, - "velocityY": 0, - "timestamp": 7.819961259636894 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -19411,31 +19963,37 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 4 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 1 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "CenterLanePCBA": { "waypoints": [ @@ -19498,569 +20056,637 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.7297728380351187e-19, - "angularVelocity": 4.2448351957919954e-19, - "velocityX": -3.109246931729259e-17, - "velocityY": 1.4154954258989747e-18, + "heading": -1.1581045676284431e-20, + "angularVelocity": -1.6335002675710077e-20, + "velocityX": -6.634658420439012e-20, + "velocityY": -8.04935442938013e-20, "timestamp": 0 }, { - "x": 1.3060810448259774, - "y": 5.572403683390414, - "heading": -0.01067910748684314, - "angularVelocity": -0.14191654637088458, - "velocityX": 0.21404337049401068, - "velocityY": -0.246522471704611, - "timestamp": 0.07524920638169826 - }, - { - "x": 1.338296279992689, - "y": 5.535304313687478, - "heading": -0.032027558921844806, - "angularVelocity": -0.28370334335098873, - "velocityX": 0.4281139525027307, - "velocityY": -0.4930200793709309, - "timestamp": 0.1504984127633965 - }, - { - "x": 1.3866231363779933, - "y": 5.479658598357654, - "heading": -0.06402169471627948, - "angularVelocity": -0.42517572387591435, - "velocityX": 0.6422241337702286, - "velocityY": -0.7394857435109029, - "timestamp": 0.22574761914509478 - }, - { - "x": 1.4510659275520246, - "y": 5.405469878379882, - "heading": -0.1066262273585151, - "angularVelocity": -0.5661791624235272, - "velocityX": 0.8563916388334698, - "velocityY": -0.9859070087913582, - "timestamp": 0.300996825526793 - }, - { - "x": 1.5316310742105061, - "y": 5.312743180460654, - "heading": -0.15979745091713102, - "angularVelocity": -0.7066017851257455, - "velocityX": 1.0706444696567097, - "velocityY": -1.2322614732810149, - "timestamp": 0.37624603190849126 - }, - { - "x": 1.6283288838862067, - "y": 5.201486845299536, - "heading": -0.22348688396132305, - "angularVelocity": -0.8463801295274643, - "velocityX": 1.2850342791041873, - "velocityY": -1.4785050967404112, - "timestamp": 0.4514952382901895 - }, - { - "x": 1.7411789077586277, - "y": 5.071717216156576, - "heading": -0.2976431936929222, - "angularVelocity": -0.9854763033040427, - "velocityX": 1.4996839076304158, - "velocityY": -1.7245315317294676, - "timestamp": 0.5267444446718877 - }, - { - "x": 1.8702368010806378, - "y": 4.923481749892843, - "heading": -0.3821997162379228, - "angularVelocity": -1.1236865690804896, - "velocityX": 1.7150731486446302, - "velocityY": -1.9699273041075376, - "timestamp": 0.601993651053586 - }, - { - "x": 2.022770759979851, - "y": 4.779444476824575, - "heading": -0.46182371324885607, - "angularVelocity": -1.0581373656886817, - "velocityX": 2.0270507322765163, - "velocityY": -1.914136772919067, - "timestamp": 0.6772428574352843 - }, - { - "x": 2.159313623811303, - "y": 4.6540559405768205, - "heading": -0.5309316944889129, - "angularVelocity": -0.9183881739497737, - "velocityX": 1.8145422443221764, - "velocityY": -1.666310414116386, - "timestamp": 0.7524920638169826 - }, - { - "x": 2.2798095691663756, - "y": 4.547269113259306, - "heading": -0.5895622742606058, - "angularVelocity": -0.7791521344995035, - "velocityX": 1.601291909229005, - "velocityY": -1.4191090172553904, - "timestamp": 0.8277412701986809 - }, - { - "x": 2.3842397515062683, - "y": 4.459067906121371, - "heading": -0.6377272819831894, - "angularVelocity": -0.6400732982918503, - "velocityX": 1.3877911457302048, - "velocityY": -1.172121426644562, - "timestamp": 0.9029904765803792 - }, - { - "x": 2.47259465372947, - "y": 4.389444105411809, - "heading": -0.675431270833758, - "angularVelocity": -0.5010549700591305, - "velocityX": 1.1741639077877584, - "velocityY": -0.9252429900248669, - "timestamp": 0.9782396829620775 - }, - { - "x": 2.5448685468329297, - "y": 4.338392728081247, - "heading": -0.7026759403224371, - "angularVelocity": -0.36205922691677456, - "velocityX": 0.9604605361114344, - "velocityY": -0.6784307740288329, - "timestamp": 1.0534888893437757 - }, - { - "x": 2.601057641331632, - "y": 4.305910462744002, - "heading": -0.7194616948028688, - "angularVelocity": -0.2230688573014308, - "velocityX": 0.7467068052991092, - "velocityY": -0.43166256362226185, - "timestamp": 1.128738095725474 - }, - { - "x": 2.641159295921209, - "y": 4.291994997972796, - "heading": -0.7257882885399572, - "angularVelocity": -0.08407522206898749, - "velocityX": 0.5329179737262918, - "velocityY": -0.18492507018124238, - "timestamp": 1.2039873021071723 + "x": 1.3060567728280696, + "y": 5.572381922263347, + "heading": -0.010695267009096829, + "angularVelocity": -0.14212861592009643, + "velocityX": 0.21371678996078675, + "velocityY": -0.2468070102016555, + "timestamp": 0.07525062380899862 + }, + { + "x": 1.3382215627560385, + "y": 5.535237421485166, + "heading": -0.032077296039988955, + "angularVelocity": -0.2841442096900626, + "velocityX": 0.42743552544639934, + "velocityY": -0.4936105363386875, + "timestamp": 0.15050124761799724 + }, + { + "x": 1.3864692681213642, + "y": 5.4795210587499135, + "heading": -0.06412402718123793, + "angularVelocity": -0.4258666509209292, + "velocityX": 0.6411602047019342, + "velocityY": -0.7404106426635293, + "timestamp": 0.22575187142699588 + }, + { + "x": 1.450800660367866, + "y": 5.405233169697022, + "heading": -0.10680235748792238, + "angularVelocity": -0.567149189553707, + "velocityX": 0.8548951356175836, + "velocityY": -0.9872062887006725, + "timestamp": 0.3010024952359945 + }, + { + "x": 1.5312168522436431, + "y": 5.312374273972817, + "heading": -0.16007184436590008, + "angularVelocity": -0.7078942895302298, + "velocityX": 1.0686448537608113, + "velocityY": -1.2339950291960333, + "timestamp": 0.3762531190449931 + }, + { + "x": 1.627719315017737, + "y": 5.20094520381907, + "heading": -0.22388950072960903, + "angularVelocity": -0.8480681372913416, + "velocityX": 1.2824141234900144, + "velocityY": -1.4807727100917756, + "timestamp": 0.4515037428539917 + }, + { + "x": 1.7403099620745723, + "y": 5.070947296194424, + "heading": -0.29821515111728925, + "angularVelocity": -0.9877080962987606, + "velocityX": 1.4962088200439807, + "velocityY": -1.727532624242517, + "timestamp": 0.5267543666629904 + }, + { + "x": 1.8689918213422534, + "y": 4.922383062014296, + "heading": -0.3830159737868923, + "angularVelocity": -1.1269118895923678, + "velocityX": 1.7100437545116134, + "velocityY": -1.9742591710231445, + "timestamp": 0.602004990471989 + }, + { + "x": 2.0218156992973517, + "y": 4.778590406876992, + "heading": -0.46245085296190774, + "angularVelocity": -1.0556042615226333, + "velocityX": 2.030865263562416, + "velocityY": -1.910850008396984, + "timestamp": 0.6772556142809876 + }, + { + "x": 2.158552472181925, + "y": 4.653367084915143, + "heading": -0.5314313533845169, + "angularVelocity": -0.9166767919119934, + "velocityX": 1.8170849085801528, + "velocityY": -1.6640835068649051, + "timestamp": 0.7525062380899862 + }, + { + "x": 2.27919988810187, + "y": 4.546710962687177, + "heading": -0.5899619594153146, + "angularVelocity": -0.7778089146391693, + "velocityX": 1.603274628342918, + "velocityY": -1.4173453564807712, + "timestamp": 0.8277568618989848 + }, + { + "x": 2.3837569267821266, + "y": 4.458620951443789, + "heading": -0.6380431409710475, + "angularVelocity": -0.6389472820553975, + "velocityX": 1.3894507897455282, + "velocityY": -1.1706216744060274, + "timestamp": 0.9030074857079834 + }, + { + "x": 2.4722229729489946, + "y": 4.389096340497206, + "heading": -0.6756737748841694, + "angularVelocity": -0.5000707237807894, + "velocityX": 1.17561877482123, + "velocityY": -0.9239074366087717, + "timestamp": 0.978258109516982 + }, + { + "x": 2.544597634609631, + "y": 4.338136648836026, + "heading": -0.7028521537713689, + "angularVelocity": -0.3611714762150514, + "velocityX": 0.9617815507328988, + "velocityY": -0.6771995909366235, + "timestamp": 1.0535087333259807 + }, + { + "x": 2.6008806784620506, + "y": 4.30574156635696, + "heading": -0.7195764091910871, + "angularVelocity": -0.22224739906698765, + "velocityX": 0.7479412263116554, + "velocityY": -0.4304958662042791, + "timestamp": 1.1287593571349794 + }, + { + "x": 2.6410720024572596, + "y": 4.291910925379844, + "heading": -0.7258446711788211, + "angularVelocity": -0.08329847209830624, + "velocityX": 0.5340995457688548, + "velocityY": -0.1837943697612413, + "timestamp": 1.2040099809439782 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.054926472219667455, - "velocityX": 0.31910406053771284, - "velocityY": 0.061790547746105144, - "timestamp": 1.2792365084888706 - }, - { - "x": 2.6730242117812852, - "y": 4.320207160810404, - "heading": -0.7068522062263088, - "angularVelocity": 0.19509567726207716, - "velocityX": 0.10349358326673892, - "velocityY": 0.31054279257798323, - "timestamp": 1.3551116333379198 - }, - { - "x": 2.6645156104831624, - "y": 4.362642495661916, - "heading": -0.6814188000083412, - "angularVelocity": 0.33520084834820957, - "velocityX": -0.1121395360458166, - "velocityY": 0.5592786164892918, - "timestamp": 1.430986758186969 - }, - { - "x": 2.6396435810539995, - "y": 4.423949055779942, - "heading": -0.6453599487845141, - "angularVelocity": 0.47523943183649325, - "velocityX": -0.32780215490271986, - "velocityY": 0.807992873026772, - "timestamp": 1.5068618830360183 - }, - { - "x": 2.5984050053739445, - "y": 4.5041245094115245, - "heading": -0.5986803757122448, - "angularVelocity": 0.615215766235525, - "velocityX": -0.5435058691760214, - "velocityY": 1.0566763981096883, - "timestamp": 1.5827370078850675 - }, - { - "x": 2.540795163397632, - "y": 4.603165195262531, - "heading": -0.541383229045499, - "angularVelocity": 0.755150608064052, - "velocityX": -0.7592717915230448, - "velocityY": 1.3053116689836102, - "timestamp": 1.6586121327341168 - }, - { - "x": 2.4668060178101583, - "y": 4.721064600465768, - "heading": -0.4734677826566921, - "angularVelocity": 0.8950950199273465, - "velocityX": -0.9751436420635095, - "velocityY": 1.5538611031988796, - "timestamp": 1.734487257583166 - }, - { - "x": 2.3764209643089336, - "y": 4.857808746061696, - "heading": -0.3949264705539342, - "angularVelocity": 1.0351391481588907, - "velocityX": -1.1912343298399446, - "velocityY": 1.8022263010181736, - "timestamp": 1.8103623824322153 - }, - { - "x": 2.269588305263905, - "y": 5.013353141552007, - "heading": -0.3057482687442009, - "angularVelocity": 1.175328567657553, - "velocityX": -1.4080063691172056, - "velocityY": 2.0500051340898513, - "timestamp": 1.8862375072812645 - }, - { - "x": 2.155512806030656, - "y": 5.145840094887286, - "heading": -0.22945454409592414, - "angularVelocity": 1.0055169569743476, - "velocityX": -1.5034637433586158, - "velocityY": 1.7461184228535829, - "timestamp": 1.9621126321303137 - }, - { - "x": 2.0576690248190195, - "y": 5.259344023095087, - "heading": -0.1639803843714035, - "angularVelocity": 0.8629199603276121, - "velocityX": -1.289537004472555, - "velocityY": 1.4959306944600357, - "timestamp": 2.037987756979363 - }, - { - "x": 1.9761088976814785, - "y": 5.353909559223238, - "heading": -0.10935658983885492, - "angularVelocity": 0.7199170300053718, - "velocityX": -1.0749257717858158, - "velocityY": 1.2463312095522094, - "timestamp": 2.113862881828412 - }, - { - "x": 1.9108492511031485, - "y": 5.429551680381221, - "heading": -0.06562174275393094, - "angularVelocity": 0.5764056029164016, - "velocityX": -0.8600927735973349, - "velocityY": 0.9969291162074897, - "timestamp": 2.1897380066774614 - }, - { - "x": 1.8618983395529738, - "y": 5.486277925937523, - "heading": -0.03280716495777854, - "angularVelocity": 0.4324813680566584, - "velocityX": -0.6451509852228532, - "velocityY": 0.7476263883465671, - "timestamp": 2.2656131315265107 - }, - { - "x": 1.8292611562031649, - "y": 5.524092810268815, - "heading": -0.010931847108471086, - "angularVelocity": 0.2883068448697581, - "velocityX": -0.430143389085703, - "velocityY": 0.4983831579385639, - "timestamp": 2.34148825637556 + "angularVelocity": 0.05567470235645032, + "velocityX": 0.3202580862836482, + "velocityY": 0.06290661835254617, + "timestamp": 1.2792606047529769 + }, + { + "x": 2.6731116609802053, + "y": 4.320290724350463, + "heading": -0.7067974022392469, + "angularVelocity": 0.19582218410154278, + "velocityX": 0.10464837684480303, + "velocityY": 0.31165083052347464, + "timestamp": 1.3551340964364653 + }, + { + "x": 2.664692760034393, + "y": 4.362810179364145, + "heading": -0.6813103687909546, + "angularVelocity": 0.3359148614724796, + "velocityX": -0.11095971411111749, + "velocityY": 0.5603993446229535, + "timestamp": 1.4310075881199538 + }, + { + "x": 2.63991513669479, + "y": 4.424203513438027, + "heading": -0.6451981318661814, + "angularVelocity": 0.4759532759533194, + "velocityX": -0.32656495423941434, + "velocityY": 0.8091539312568886, + "timestamp": 1.5068810798034422 + }, + { + "x": 2.598779109919189, + "y": 4.50447133226996, + "heading": -0.5984640990912053, + "angularVelocity": 0.6159467784866202, + "velocityX": -0.5421659905570602, + "velocityY": 1.0579165008877693, + "timestamp": 1.5827545714869307 + }, + { + "x": 2.54128511679975, + "y": 4.60361438359764, + "heading": -0.5411094256407595, + "angularVelocity": 0.7559250560090859, + "velocityX": -0.7577612660727276, + "velocityY": 1.3066889255771041, + "timestamp": 1.658628063170419 + }, + { + "x": 2.467433718348128, + "y": 4.721633509490798, + "heading": -0.4731300084226628, + "angularVelocity": 0.8959574116040114, + "velocityX": -0.9733491475481174, + "velocityY": 1.5554724486054035, + "timestamp": 1.7345015548539076 + }, + { + "x": 2.377225511297283, + "y": 4.8585294609234, + "heading": -0.3945113768792691, + "angularVelocity": 1.036180486741758, + "velocityX": -1.1889291641823332, + "velocityY": 1.8042658693456888, + "timestamp": 1.810375046537396 + }, + { + "x": 2.2706603587085, + "y": 5.014302055693198, + "heading": -0.3052212812212148, + "angularVelocity": 1.1768286087390614, + "velocityX": -1.4045109856460358, + "velocityY": 2.053056888690639, + "timestamp": 1.8862485382208845 + }, + { + "x": 2.156230031792854, + "y": 5.146478320337647, + "heading": -0.22909477561234234, + "angularVelocity": 1.003334681451587, + "velocityX": -1.508172675023323, + "velocityY": 1.7420611825251466, + "timestamp": 1.962122029904373 + }, + { + "x": 2.058146916882525, + "y": 5.2597707167035095, + "heading": -0.1637376643935269, + "angularVelocity": 0.8613958547137548, + "velocityX": -1.2927191399005253, + "velocityY": 1.493174939654406, + "timestamp": 2.0379955215878613 + }, + { + "x": 1.9764115241122762, + "y": 5.354180474443043, + "heading": -0.10920153692146195, + "angularVelocity": 0.7187770888357993, + "velocityX": -1.0772588812864141, + "velocityY": 1.2443049033959122, + "timestamp": 2.1138690132713496 + }, + { + "x": 1.911023503907161, + "y": 5.429708017230294, + "heading": -0.06553190171122841, + "angularVelocity": 0.5755585282987177, + "velocityX": -0.861803230011952, + "velocityY": 0.9954404510908861, + "timestamp": 2.189742504954838 + }, + { + "x": 1.8619825163381478, + "y": 5.486353593028934, + "heading": -0.0327635757896738, + "angularVelocity": 0.43188108513906287, + "velocityX": -0.646352058945582, + "velocityY": 0.7465792669057553, + "timestamp": 2.265615996638326 + }, + { + "x": 1.8292883956003307, + "y": 5.524117338037825, + "heading": -0.01091770284008431, + "angularVelocity": 0.2879249717506227, + "velocityX": -0.4309030731602948, + "velocityY": 0.49771987779902493, + "timestamp": 2.3414894883218142 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -1.6776667047563746e-20, - "angularVelocity": 0.1440768253130289, - "velocityX": -0.2150897630156809, - "velocityY": 0.24917859900600148, - "timestamp": 2.417363381224609 + "heading": 2.996746418062504e-20, + "angularVelocity": 0.14389350744036247, + "velocityX": -0.21545340351406272, + "velocityY": 0.24886069062260008, + "timestamp": 2.4173629800053025 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.1170600847702852e-20, - "angularVelocity": 5.938902420193292e-19, - "velocityX": -2.686779016810018e-17, - "velocityY": -1.2969153357483792e-17, - "timestamp": 2.4932385060736584 + "heading": 1.4094054867508415e-20, + "angularVelocity": -1.911842718131469e-20, + "velocityX": -7.069031221189556e-20, + "velocityY": -4.343771313501213e-20, + "timestamp": 2.4932364716887907 }, { - "x": 1.8552093951788071, - "y": 5.543066072468525, - "heading": -1.0867013680305264e-17, - "angularVelocity": -1.1051820926914384e-16, - "velocityX": 0.42942883009919236, - "velocityY": 0.0006787122433536914, - "timestamp": 2.5916673926214395 + "x": 1.8552093934543321, + "y": 5.543066072465799, + "heading": -2.0527826070429256e-18, + "angularVelocity": -2.0998680789192775e-17, + "velocityX": 0.4294288348656248, + "velocityY": 0.000678712250886674, + "timestamp": 2.5916653531283225 }, { - "x": 1.9397457967809342, - "y": 5.543199682246804, - "heading": -3.2635799657096195e-17, - "angularVelocity": -2.2116257473548814e-16, - "velocityX": 0.8588576440015888, - "velocityY": 0.001357424461108283, - "timestamp": 2.6900962791692207 + "x": 1.939745792565551, + "y": 5.543199682240142, + "heading": -6.048126842026002e-18, + "angularVelocity": -4.05911778781429e-17, + "velocityX": 0.8588576632677929, + "velocityY": 0.0013574244915579316, + "timestamp": 2.6900942345678542 }, { - "x": 2.0665503951985436, - "y": 5.5434000969079245, - "heading": -4.603800029490119e-17, - "angularVelocity": -1.3616125416355767e-16, - "velocityX": 1.2882864255103954, - "velocityY": 0.002036136627664645, - "timestamp": 2.788525165717002 + "x": 2.066550389641902, + "y": 5.543400096899142, + "heading": -1.1691231309928304e-17, + "angularVelocity": -5.733179515246905e-17, + "velocityX": 1.2882864787430477, + "velocityY": 0.0020361367117983195, + "timestamp": 2.788523116007386 }, { - "x": 2.235623180866242, + "x": 2.2356231808662415, "y": 5.543667316436768, - "heading": -4.798133527402523e-17, - "angularVelocity": -1.9743543259972466e-17, - "velocityX": 1.7177151098384498, - "velocityY": 0.002714848640626296, - "timestamp": 2.886954052264783 + "heading": 9.487524674650316e-21, + "angularVelocity": 1.188748532301011e-16, + "velocityX": 1.7177152554375654, + "velocityY": 0.0027148488707459404, + "timestamp": 2.8869519974469178 }, { - "x": 2.40469596653394, - "y": 5.54393453596561, - "heading": -3.9330353852399465e-17, - "angularVelocity": 8.789067645262313e-17, - "velocityX": 1.7177151098384498, - "velocityY": 0.0027148486406302456, - "timestamp": 2.985382938812564 + "x": 2.4046959720905807, + "y": 5.543934535974393, + "heading": 1.1697892357164231e-17, + "angularVelocity": 1.187497476520098e-16, + "velocityX": 1.7177152554375656, + "velocityY": 0.002714848870745941, + "timestamp": 2.9853808788864495 }, { - "x": 2.531500564951549, - "y": 5.544134950626731, - "heading": -2.0009563266152006e-17, - "angularVelocity": 1.96291873894211e-16, - "velocityX": 1.2882864255103954, - "velocityY": 0.002036136627666603, - "timestamp": 3.0838118253603453 + "x": 2.531500569166932, + "y": 5.544134950633393, + "heading": 6.051439837245386e-18, + "angularVelocity": -5.736581008759357e-17, + "velocityX": 1.2882864787430477, + "velocityY": 0.00203613671179832, + "timestamp": 3.0838097603259813 }, { - "x": 2.616036966553676, - "y": 5.54426856040501, - "heading": -7.02269200923652e-18, - "angularVelocity": 1.3194166582538958e-16, - "velocityX": 0.8588576440015888, - "velocityY": 0.0013574244611093242, - "timestamp": 3.1822407119081264 + "x": 2.616036968278151, + "y": 5.544268560407736, + "heading": 2.0590195546334623e-18, + "angularVelocity": -4.0561471634117896e-17, + "velocityX": 0.8588576632677929, + "velocityY": 0.0013574244915579318, + "timestamp": 3.182238641765513 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -3.8911537114769476e-19, - "angularVelocity": 6.739461219451495e-17, - "velocityX": 0.4294288300991924, - "velocityY": 0.0006787122433541438, - "timestamp": 3.2806695984559076 + "heading": -7.01229032287185e-26, + "angularVelocity": -2.0918856279971937e-17, + "velocityX": 0.4294288348656248, + "velocityY": 0.0006787122508866741, + "timestamp": 3.280667523205045 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -4.27312910681112e-19, - "angularVelocity": -3.880724585667238e-19, - "velocityX": -2.3908482729518656e-18, - "velocityY": 6.838225330043828e-18, - "timestamp": 3.3790984850036887 - }, - { - "x": 2.6438893629989613, - "y": 5.563919104052164, - "heading": 0.003139586303789949, - "angularVelocity": 0.042042846518139325, - "velocityX": -0.19304501448070868, - "velocityY": 0.26224987725536203, - "timestamp": 3.453774358875246 - }, - { - "x": 2.6152669032432256, - "y": 5.6032366820496495, - "heading": 0.009644303445155527, - "angularVelocity": 0.0871060063194363, - "velocityX": -0.38328925088933385, - "velocityY": 0.5265097809918172, - "timestamp": 3.5284502327468035 - }, - { - "x": 2.5727487902328847, - "y": 5.662504910556293, - "heading": 0.019847811829768587, - "angularVelocity": 0.13663728130135686, - "velocityX": -0.5693688042201159, - "velocityY": 0.7936730490571009, - "timestamp": 3.603126106618361 - }, - { - "x": 2.5168455054942007, - "y": 5.742063733198033, - "heading": 0.03429127316793741, - "angularVelocity": 0.19341536415109115, - "velocityX": -0.7486123943435556, - "velocityY": 1.0653885721991296, - "timestamp": 3.6778019804899182 - }, - { - "x": 2.4485453653591587, - "y": 5.842518348175502, - "heading": 0.05399784068706335, - "angularVelocity": 0.2638947024981649, - "velocityX": -0.9146212370077963, - "velocityY": 1.3452084290335973, - "timestamp": 3.7524778543614756 - }, - { - "x": 2.370517685528427, - "y": 5.96519344251776, - "heading": 0.08158842838664396, - "angularVelocity": 0.369471239761256, - "velocityX": -1.0448847236115244, - "velocityY": 1.642767442578038, - "timestamp": 3.827153728233033 - }, - { - "x": 2.3003293154277267, - "y": 6.110432122884094, - "heading": 0.1284873855211158, - "angularVelocity": 0.6280335897392839, - "velocityX": -0.9399069131942741, - "velocityY": 1.94492106802986, - "timestamp": 3.9018296021045904 - }, - { - "x": 2.2517603849650345, - "y": 6.244622565856429, - "heading": 0.18176076028404833, - "angularVelocity": 0.7133947284575884, - "velocityX": -0.6503965463628967, - "velocityY": 1.796971846665519, - "timestamp": 3.976505475976148 - }, - { - "x": 2.22192407028561, - "y": 6.363296472335711, - "heading": 0.23706248397325824, - "angularVelocity": 0.7405567664909981, - "velocityX": -0.39954423206005096, - "velocityY": 1.5891867122091021, - "timestamp": 4.051181349847705 - }, - { - "x": 2.2097448289139274, - "y": 6.465233576489708, - "heading": 0.2929845885143906, - "angularVelocity": 0.7488644141924427, - "velocityX": -0.16309472846117115, - "velocityY": 1.3650607467858884, - "timestamp": 4.125857223719263 - }, - { - "x": 2.2146777318065234, - "y": 6.549876478070479, - "heading": 0.348839415183568, - "angularVelocity": 0.747963482359082, - "velocityX": 0.06605751813605938, - "velocityY": 1.1334705198944957, - "timestamp": 4.20053309759082 - }, - { - "x": 2.236395128424509, - "y": 6.61690736376859, - "heading": 0.4042192057980219, - "angularVelocity": 0.7416021767580177, - "velocityX": 0.2908221289159501, - "velocityY": 0.8976243895505626, - "timestamp": 4.275208971462377 - }, - { - "x": 2.274678638341779, - "y": 6.666121192465354, - "heading": 0.45885367997336884, - "angularVelocity": 0.7316214908889929, - "velocityX": 0.5126623624534666, - "velocityY": 0.6590325113759182, - "timestamp": 4.349884845333935 + "heading": -7.502242975241614e-26, + "angularVelocity": -4.851342689713789e-26, + "velocityX": 1.33942470461485e-25, + "velocityY": -1.0236263470109501e-25, + "timestamp": 3.3790964046445766 + }, + { + "x": 2.643889442719825, + "y": 5.563919137234555, + "heading": 0.0031393268173354576, + "angularVelocity": 0.04203939285338829, + "velocityX": -0.19304404414258594, + "velocityY": 0.26225045368154326, + "timestamp": 3.45377224090805 + }, + { + "x": 2.615267140219649, + "y": 5.603236779535534, + "heading": 0.009643531258689539, + "angularVelocity": 0.08709918451272194, + "velocityX": -0.3832873380780111, + "velocityY": 0.5265109072532814, + "timestamp": 3.528448077171523 + }, + { + "x": 2.5727492586123577, + "y": 5.662505100435592, + "heading": 0.019846282739514817, + "angularVelocity": 0.13662721425479105, + "velocityX": -0.5693659921969727, + "velocityY": 0.7936746860248878, + "timestamp": 3.603123913434996 + }, + { + "x": 2.516846273518896, + "y": 5.742064038683667, + "heading": 0.03428875734419064, + "angularVelocity": 0.19340224800053757, + "velocityX": -0.7486087587452387, + "velocityY": 1.0653906568568225, + "timestamp": 3.6777997496984693 + }, + { + "x": 2.448546488675387, + "y": 5.842518783466547, + "heading": 0.05399413519442509, + "angularVelocity": 0.2638789042912005, + "velocityX": -0.9146169398429232, + "velocityY": 1.3452108447564306, + "timestamp": 3.7524755859619425 + }, + { + "x": 2.3705191792906315, + "y": 5.9651940015137335, + "heading": 0.0815833991238304, + "angularVelocity": 0.36945369894572166, + "velocityX": -1.0448802891132984, + "velocityY": 1.6427699264640412, + "timestamp": 3.8271514222254157 + }, + { + "x": 2.300330937619818, + "y": 6.110432887633571, + "heading": 0.12848151048537504, + "angularVelocity": 0.6280225801031192, + "velocityX": -0.9399056667162509, + "velocityY": 1.944924802815761, + "timestamp": 3.901827258488889 + }, + { + "x": 2.2517620633069444, + "y": 6.244623434702984, + "heading": 0.18175459499441046, + "angularVelocity": 0.7133912008842571, + "velocityX": -0.6503961220000475, + "velocityY": 1.7969741456387136, + "timestamp": 3.976503094752362 + }, + { + "x": 2.2219257094452107, + "y": 6.36329736060272, + "heading": 0.23705644760887684, + "angularVelocity": 0.7405588659141199, + "velocityX": -0.39954495797628836, + "velocityY": 1.589187772615341, + "timestamp": 4.051178931015835 + }, + { + "x": 2.2097463319071795, + "y": 6.465234411607719, + "heading": 0.29297905925002543, + "angularVelocity": 0.7488715820180554, + "velocityX": -0.16309663403111632, + "velocityY": 1.3650607225253184, + "timestamp": 4.125854767279309 + }, + { + "x": 2.2146790021354428, + "y": 6.549877194214307, + "heading": 0.3488347521561843, + "angularVelocity": 0.7479754590104253, + "velocityX": 0.06605443574625332, + "velocityY": 1.133469497521908, + "timestamp": 4.2005306035427825 + }, + { + "x": 2.2363960702944423, + "y": 6.616907899307652, + "heading": 0.40421575733503773, + "angularVelocity": 0.7416188147322054, + "velocityX": 0.29081787691505323, + "velocityY": 0.8976224230933981, + "timestamp": 4.275206439806256 + }, + { + "x": 2.2746791565921045, + "y": 6.66612148860884, + "heading": 0.45885178746831873, + "angularVelocity": 0.7316426955101331, + "velocityX": 0.5126569478591565, + "velocityY": 0.6590296374794072, + "timestamp": 4.34988227606973 }, { "x": 2.3293724060058594, "y": 6.6973748207092285, "heading": 0.5125504196, - "angularVelocity": 0.719064094502462, - "velocityX": 0.7324155022029422, - "velocityY": 0.41852376977375944, - "timestamp": 4.424560719205492 + "angularVelocity": 0.7190897995734556, + "velocityX": 0.7324089310601718, + "velocityY": 0.4185200148294202, + "timestamp": 4.424558112333203 }, { - "x": 2.4110936768539344, - "y": 6.744072759574757, - "heading": 0.5643107226604884, - "angularVelocity": 0.653664165942785, - "velocityX": 1.0320315606781885, - "velocityY": 0.5897332019890127, - "timestamp": 4.503745575523619 + "x": 2.4110932710082595, + "y": 6.744072527662596, + "heading": 0.564314297915607, + "angularVelocity": 0.6537087472117014, + "velocityX": 1.0320255363414028, + "velocityY": 0.5897297595057032, + "timestamp": 4.503743037632732 }, { - "x": 2.50599401367539, - "y": 6.798301604540289, - "heading": 0.5436072588792712, - "angularVelocity": -0.2614573637418783, - "velocityX": 1.198465732389427, - "velocityY": 0.6848385851414024, - "timestamp": 4.5829304318417465 + "x": 2.50599376719455, + "y": 6.798301463693884, + "heading": 0.5436087676029941, + "angularVelocity": -0.2614832335105605, + "velocityX": 1.1984667009195895, + "velocityY": 0.6848391385880369, + "timestamp": 4.58292796293226 }, { - "x": 2.577169265515479, - "y": 6.838973237821007, - "heading": 0.528079022912807, - "angularVelocity": -0.19610108155123726, - "velocityX": 0.8988492894921869, - "velocityY": 0.51362893325611, - "timestamp": 4.662115288159874 + "x": 2.5771691415050153, + "y": 6.8389731669577785, + "heading": 0.5280796125906262, + "angularVelocity": -0.19611251704319405, + "velocityX": 0.8988500531033392, + "velocityY": 0.5136293696059923, + "timestamp": 4.662112888231788 }, { - "x": 2.6246194330616937, - "y": 6.8660876598097635, - "heading": 0.5177266590936774, - "angularVelocity": -0.13073666229232678, - "velocityX": 0.5992328552770498, - "velocityY": 0.3424192863320269, - "timestamp": 4.741300144478001 + "x": 2.624619391553792, + "y": 6.866087636090928, + "heading": 0.5177268190614664, + "angularVelocity": -0.13074197506657764, + "velocityX": 0.5992333751568124, + "velocityY": 0.34241958340662104, + "timestamp": 4.741297813531316 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.06536905835734146, - "velocityX": 0.2996164266200211, - "velocityY": 0.1712096425840104, - "timestamp": 4.820485000796128 + "angularVelocity": -0.06537102159136951, + "velocityX": 0.29961668980067113, + "velocityY": 0.1712097929731781, + "timestamp": 4.820482738830845 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -6.678803262228469e-27, + "velocityX": 1.419138537377677e-18, + "velocityY": -2.483488681188097e-18, + "timestamp": 4.899667664130373 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 1.2792606047529769, + "isStopPoint": false, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 2.4932364716887907, + "isStopPoint": true, + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 3.3790964046445766, + "isStopPoint": true, + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 4.424558112333203, + "isStopPoint": false, + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { + "timestamp": 4.899667664130373, + "isStopPoint": true, "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -8.381816905081638e-20, - "velocityX": -7.007327358505968e-19, - "velocityY": 8.954594613208896e-19, - "timestamp": 4.899669857114255 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -20068,44 +20694,52 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 3 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 5 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 4, 5 ], - "type": "StraightLine" + "type": "StraightLine", + "direction": 0 }, { "scope": [ 2 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "CenterLanePCBAFE": { "waypoints": [ @@ -20116,7 +20750,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 17 }, { "x": 2.6651716232299805, @@ -20125,7 +20759,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { "x": 1.8129411935806274, @@ -20134,7 +20768,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { "x": 2.7583051681518556, @@ -20143,7 +20777,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 14 }, { "x": 2.3293724060058594, @@ -20161,7 +20795,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 18 }, { "x": 4.16749906539917, @@ -20170,7 +20804,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 12 + "controlIntervalCount": 13 }, { "x": 5.665951728820801, @@ -20179,7 +20813,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 11 + "controlIntervalCount": 12 }, { "x": 7.217291355133057, @@ -20197,7 +20831,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 15 }, { "x": 5.665951728820801, @@ -20206,7 +20840,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 12 + "controlIntervalCount": 13 }, { "x": 4.16749906539917, @@ -20215,7 +20849,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 13 }, { "x": 5.665951728820801, @@ -20224,7 +20858,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 18 + "controlIntervalCount": 20 }, { "x": 7.984478950500488, @@ -20233,7 +20867,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 6 }, { "x": 8.217175483703613, @@ -20242,7 +20876,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 20 }, { "x": 5.665951728820801, @@ -20251,7 +20885,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 12 + "controlIntervalCount": 13 }, { "x": 4.16749906539917, @@ -20267,1811 +20901,2135 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -8.364664059409844e-30, - "velocityX": -6.773677606947568e-30, - "velocityY": -2.308132540143278e-30, + "heading": -1.0042344276654815e-24, + "angularVelocity": -6.621110938678157e-25, + "velocityX": -2.249108899033261e-24, + "velocityY": -2.0825834551113612e-24, "timestamp": 0 }, { - "x": 1.3079899492454155, - "y": 5.5701428985236205, - "heading": -0.011966271801548406, - "angularVelocity": -0.16457159783011938, - "velocityX": 0.24776633609486393, - "velocityY": -0.2862183198381729, - "timestamp": 0.07271164623703327 - }, - { - "x": 1.3440211933652915, - "y": 5.528520469164234, - "heading": -0.03588810704483216, - "angularVelocity": -0.328995924069994, - "velocityX": 0.49553607960947427, - "velocityY": -0.5724313987522216, - "timestamp": 0.14542329247406655 - }, - { - "x": 1.398068831056723, - "y": 5.466087418740183, - "heading": -0.07173813692543105, - "angularVelocity": -0.49304384837450815, - "velocityX": 0.7433147299769343, - "velocityY": -0.8586389341691013, - "timestamp": 0.21813493871109982 - }, - { - "x": 1.4701339496615213, - "y": 5.382844295736326, - "heading": -0.11947590052159912, - "angularVelocity": -0.6565353154432494, - "velocityX": 0.9911083345055308, - "velocityY": -1.1448389262999967, - "timestamp": 0.2908465849481331 - }, - { - "x": 1.5602181142824867, - "y": 5.2787919544216955, - "heading": -0.17905308310612672, - "angularVelocity": -0.819362312243441, - "velocityX": 1.2389234638197153, - "velocityY": -1.431027169709696, - "timestamp": 0.3635582311851664 - }, - { - "x": 1.668323429942649, - "y": 5.153931763984036, - "heading": -0.25042016061718175, - "angularVelocity": -0.9815082068514337, - "velocityX": 1.4867675434162453, - "velocityY": -1.717196582745939, - "timestamp": 0.4362698774221997 - }, - { - "x": 1.7944527928954905, - "y": 5.008265979195592, - "heading": -0.33353344500312104, - "angularVelocity": -1.1430532616554276, - "velocityX": 1.7346514550472283, - "velocityY": -2.003334986008902, - "timestamp": 0.508981523659233 - }, - { - "x": 1.938611553409707, - "y": 4.8417992681651185, - "heading": -0.42836015839962444, - "angularVelocity": -1.3041475239812899, - "velocityX": 1.98260894865471, - "velocityY": -2.2894091890060286, - "timestamp": 0.5816931698962663 - }, - { - "x": 2.09250704020051, - "y": 4.700832043022275, - "heading": -0.5059805202146961, - "angularVelocity": -1.0675093444479318, - "velocityX": 2.116517707491784, - "velocityY": -1.9387159062918606, - "timestamp": 0.6544048161332996 - }, - { - "x": 2.228385575629257, - "y": 4.5806756631537535, - "heading": -0.5718968876072178, - "angularVelocity": -0.906544835648882, - "velocityX": 1.8687313857908063, - "velocityY": -1.652505287385456, - "timestamp": 0.7271164623703329 - }, - { - "x": 2.346242849494598, - "y": 4.481326236786965, - "heading": -0.6261128487399352, - "angularVelocity": -0.7456296746751315, - "velocityX": 1.6208857861127495, - "velocityY": -1.3663481918284743, - "timestamp": 0.7998281086073662 - }, - { - "x": 2.446077177803767, - "y": 4.402782122456306, - "heading": -0.6686283268670905, - "angularVelocity": -0.5847134582152551, - "velocityX": 1.3730170265673516, - "velocityY": -1.080213671266156, - "timestamp": 0.8725397548443995 - }, - { - "x": 2.5278876403134056, - "y": 4.3450423723526415, - "heading": -0.6994420265312672, - "angularVelocity": -0.423779425385222, - "velocityX": 1.12513561096113, - "velocityY": -0.7940921858762265, - "timestamp": 0.9452514010814328 - }, - { - "x": 2.591673696422735, - "y": 4.308106404528037, - "heading": -0.718552253950641, - "angularVelocity": -0.2628220980484028, - "velocityX": 0.8772467604995492, - "velocityY": -0.5079787039876672, - "timestamp": 1.017963047318466 - }, - { - "x": 2.637435055651834, - "y": 4.2919738867991235, - "heading": -0.7259572155509334, - "angularVelocity": -0.10184010376417504, - "velocityX": 0.6293539150825374, - "velocityY": -0.22186979060047113, - "timestamp": 1.0906746935554992 + "x": 1.3060567679582313, + "y": 5.572381918180444, + "heading": -0.010695335923114668, + "angularVelocity": -0.14212952691547795, + "velocityX": 0.2137167180313357, + "velocityY": -0.24680705612748235, + "timestamp": 0.07525062634926666 + }, + { + "x": 1.3382215476730719, + "y": 5.535237409026599, + "heading": -0.03207750843931141, + "angularVelocity": -0.2841461068636689, + "velocityX": 0.4274353752957696, + "velocityY": -0.49361063097924024, + "timestamp": 0.15050125269853332 + }, + { + "x": 1.3864692368556266, + "y": 5.479521033340712, + "heading": -0.06412446500663382, + "angularVelocity": -0.4258696322151573, + "velocityX": 0.6411599680063655, + "velocityY": -0.7404107897692007, + "timestamp": 0.22575187904779997 + }, + { + "x": 1.4508006060735943, + "y": 5.40523312636478, + "heading": -0.10680311296182225, + "angularVelocity": -0.5671533916156479, + "velocityX": 0.8548948007340368, + "velocityY": -0.987206493553073, + "timestamp": 0.30100250539706663 + }, + { + "x": 1.5312167667507302, + "y": 5.312374207143024, + "heading": -0.16007302531493992, + "angularVelocity": -0.7078999197411572, + "velocityX": 1.0686444030896685, + "velocityY": -1.2339952997967512, + "timestamp": 0.3762531317463333 + }, + { + "x": 1.6277191879065078, + "y": 5.200945106889031, + "heading": -0.22389124150432238, + "angularVelocity": -0.8480755481446488, + "velocityX": 1.2824135271362858, + "velocityY": -1.480773060104614, + "timestamp": 0.45150375809559995 + }, + { + "x": 1.7403097782501336, + "y": 5.070947160382142, + "heading": -0.2982176402551873, + "angularVelocity": -0.9877180078992028, + "velocityX": 1.496208015878168, + "velocityY": -1.7275330826287059, + "timestamp": 0.5267543844448666 + }, + { + "x": 1.8689915501245091, + "y": 4.922382870770249, + "heading": -0.3830195665066897, + "angularVelocity": -1.126926516968836, + "velocityX": 1.7100425354217623, + "velocityY": -1.9742598410058476, + "timestamp": 0.6020050107941333 + }, + { + "x": 2.021815501878503, + "y": 4.778590249332989, + "heading": -0.46245356659644804, + "angularVelocity": -1.0555925437892708, + "velocityX": 2.0308661757136712, + "velocityY": -1.9108494960541986, + "timestamp": 0.6772556371434 + }, + { + "x": 2.158552321551595, + "y": 4.6533669501943, + "heading": -0.5314334875140496, + "angularVelocity": -0.916669059968213, + "velocityX": 1.8170854690091889, + "velocityY": -1.6640831473944147, + "timestamp": 0.7525062634926667 + }, + { + "x": 2.279199772550662, + "y": 4.546710847450061, + "heading": -0.589963647479443, + "angularVelocity": -0.7778029606522173, + "velocityX": 1.6032750403843345, + "velocityY": -1.417345049717031, + "timestamp": 0.8277568898419334 + }, + { + "x": 2.383756839213222, + "y": 4.45862085446933, + "heading": -0.6380444617813997, + "angularVelocity": -0.6389423800779483, + "velocityX": 1.38945111469599, + "velocityY": -1.1706213921977682, + "timestamp": 0.9030075161912001 + }, + { + "x": 2.4722229084949126, + "y": 4.389096261559967, + "heading": -0.67567478001119, + "angularVelocity": -0.5000665118072721, + "velocityX": 1.1756190423065196, + "velocityY": -0.9239071657247342, + "timestamp": 0.9782581425404668 + }, + { + "x": 2.544597589718017, + "y": 4.338136588288561, + "heading": -0.7028528784398662, + "angularVelocity": -0.36116773703028543, + "velocityX": 0.9617817782298118, + "velocityY": -0.6771993236957614, + "timestamp": 1.0535087688897335 + }, + { + "x": 2.6008806504487216, + "y": 4.305741524929451, + "heading": -0.7195768777220212, + "angularVelocity": -0.22224398777137738, + "velocityX": 0.7479414253573617, + "velocityY": -0.4304955975881504, + "timestamp": 1.1287593952390003 + }, + { + "x": 2.6410719892543444, + "y": 4.2919109040687715, + "heading": -0.7258449001014369, + "angularVelocity": -0.08329528514916955, + "velocityX": 0.5340997245535138, + "velocityY": -0.1837940962309967, + "timestamp": 1.204010021588267 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.05916659407406203, - "velocityX": 0.38145976628941564, - "velocityY": 0.06423731409121712, - "timestamp": 1.1633863397925324 - }, - { - "x": 2.6747076115982096, - "y": 4.323065260999168, - "heading": -0.7050775877835139, - "angularVelocity": 0.2234613634683759, - "velocityX": 0.12854299211755085, - "velocityY": 0.35614342430781004, - "timestamp": 1.237571545918339 - }, - { - "x": 2.6654811031724366, - "y": 4.37114137531458, - "heading": -0.6763165724415564, - "angularVelocity": 0.38769205939503093, - "velocityX": -0.1243712717061632, - "velocityY": 0.6480552770879449, - "timestamp": 1.3117567520441455 - }, - { - "x": 2.6374924128586628, - "y": 4.440873637695316, - "heading": -0.6353766207501428, - "angularVelocity": 0.5518613996935385, - "velocityX": -0.3772812906679928, - "velocityY": 0.9399753133925751, - "timestamp": 1.3859419581699521 - }, - { - "x": 2.590742000006679, - "y": 4.532262849422754, - "heading": -0.5822609370446827, - "angularVelocity": 0.7159875462663353, - "velocityX": -0.6301851176982644, - "velocityY": 1.231906150790299, - "timestamp": 1.4601271642957587 - }, - { - "x": 2.525230488767617, - "y": 4.645309988591984, - "heading": -0.5169687243847326, - "angularVelocity": 0.8801244354174206, - "velocityX": -0.8830805313123746, - "velocityY": 1.5238501726898892, - "timestamp": 1.5343123704215653 - }, - { - "x": 2.4409586345992826, - "y": 4.780016081558229, - "heading": -0.4394900359726193, - "angularVelocity": 1.0443954051766633, - "velocityX": -1.1359657615273402, - "velocityY": 1.8158080296257462, - "timestamp": 1.608497576547372 - }, - { - "x": 2.337926784653071, - "y": 4.936381539754754, - "heading": -0.3497973159677186, - "angularVelocity": 1.2090378215045976, - "velocityX": -1.3888463122287895, - "velocityY": 2.107771432581833, - "timestamp": 1.6826827826731785 - }, - { - "x": 2.2066781223966365, - "y": 5.088037063238321, - "heading": -0.26257787440108193, - "angularVelocity": 1.1756985809634768, - "velocityX": -1.7692026362756765, - "velocityY": 2.044282565348452, - "timestamp": 1.756867988798985 - }, - { - "x": 2.094180452686652, - "y": 5.218026595083209, - "heading": -0.18768356705291486, - "angularVelocity": 1.009558526050363, - "velocityX": -1.5164434471606913, - "velocityY": 1.7522298398549971, - "timestamp": 1.8310531949247917 - }, - { - "x": 2.0004335399095625, - "y": 5.326350877049268, - "heading": -0.12517882612598913, - "angularVelocity": 0.8425499394477148, - "velocityX": -1.2636874340540831, - "velocityY": 1.460187113126797, - "timestamp": 1.9052384010505983 - }, - { - "x": 1.9254366143080366, - "y": 5.4130101547737155, - "heading": -0.07512185913342963, - "angularVelocity": 0.6747567285776663, - "velocityX": -1.0109417971010437, - "velocityY": 1.1681476975361738, - "timestamp": 1.9794236071764049 - }, - { - "x": 1.869189070291752, - "y": 5.478004602849777, - "heading": -0.037557912625022415, - "angularVelocity": 0.5063536043287745, - "velocityX": -0.7582043233686032, - "velocityY": 0.876110635424555, - "timestamp": 2.0536088133022115 - }, - { - "x": 1.8316905948823166, - "y": 5.5213343174096465, - "heading": -0.012514751718529437, - "angularVelocity": 0.3375762124977515, - "velocityX": -0.5054710685139713, - "velocityY": 0.5840748691542834, - "timestamp": 2.127794019428018 + "angularVelocity": 0.055677742612701135, + "velocityX": 0.3202582509251215, + "velocityY": 0.06290689943025622, + "timestamp": 1.2792606479375337 + }, + { + "x": 2.6731116736052027, + "y": 4.320290744259811, + "heading": -0.7067971794351253, + "angularVelocity": 0.19582512759313456, + "velocityX": 0.10464854696510043, + "velocityY": 0.31165110401848706, + "timestamp": 1.355134136920331 + }, + { + "x": 2.664692786668951, + "y": 4.362810218152233, + "heading": -0.6813099274422239, + "angularVelocity": 0.3359177538109565, + "velocityX": -0.1109595334170119, + "velocityY": 0.5603996133888316, + "timestamp": 1.4310076259031281 + }, + { + "x": 2.639915179239627, + "y": 4.424203570363852, + "heading": -0.6451974716199608, + "angularVelocity": 0.47595617792732364, + "velocityX": -0.3265647561685457, + "velocityY": 0.8091541991108215, + "timestamp": 1.5068811148859254 + }, + { + "x": 2.59877917099983, + "y": 4.50447140700047, + "heading": -0.59846321314625, + "angularVelocity": 0.6159497750829267, + "velocityX": -0.5421657655564515, + "velocityY": 1.0579167732067494, + "timestamp": 1.5827546038687226 + }, + { + "x": 2.5412852001349884, + "y": 4.6036144764177696, + "heading": -0.541108297539196, + "angularVelocity": 0.755928274499912, + "velocityX": -0.7577609997330812, + "velocityY": 1.3066892105064276, + "timestamp": 1.6586280928515198 + }, + { + "x": 2.4674338294971827, + "y": 4.7216336217313835, + "heading": -0.4731286056147784, + "angularVelocity": 0.8959610640790578, + "velocityX": -0.973348815612657, + "velocityY": 1.5554727599303189, + "timestamp": 1.734501581834317 + }, + { + "x": 2.3772256595733805, + "y": 4.858529596063364, + "heading": -0.3945096344591076, + "angularVelocity": 1.0361849996577348, + "velocityX": -1.1889287171735974, + "velocityY": 1.8042662353779348, + "timestamp": 1.8103750708171142 + }, + { + "x": 2.270660565338228, + "y": 5.014302224131279, + "heading": -0.305219034790845, + "angularVelocity": 1.1768352933988244, + "velocityX": -1.4045102665479658, + "velocityY": 2.0530574006321625, + "timestamp": 1.8862485597999115 + }, + { + "x": 2.156230166958197, + "y": 5.146478435153338, + "heading": -0.2290932542035935, + "angularVelocity": 1.0033251615002383, + "velocityX": -1.5081736705949598, + "velocityY": 1.7420605377990246, + "timestamp": 1.9621220487827087 + }, + { + "x": 2.05814700584277, + "y": 5.259770794344292, + "heading": -0.16373664121599296, + "angularVelocity": 0.8613893187700765, + "velocityX": -1.292719794889953, + "velocityY": 1.4931745028443373, + "timestamp": 2.037995537765506 + }, + { + "x": 1.976411579927544, + "y": 5.3541805242021825, + "heading": -0.10920088428004432, + "angularVelocity": 0.7187722308158727, + "velocityX": -1.077259356476389, + "velocityY": 1.2443045802110966, + "timestamp": 2.113869026748303 + }, + { + "x": 1.9110235357975711, + "y": 5.4297080461749685, + "heading": -0.06553152377248019, + "angularVelocity": 0.5755549282499098, + "velocityX": -0.8618035760132025, + "velocityY": 0.9954402121920332, + "timestamp": 2.1897425157311 + }, + { + "x": 1.8619825316383047, + "y": 5.486353607136737, + "heading": -0.03276339241185007, + "angularVelocity": 0.43187853623101025, + "velocityX": -0.6463523006090516, + "velocityY": 0.7465790979324988, + "timestamp": 2.265616004713897 + }, + { + "x": 1.829288400520471, + "y": 5.524117342639773, + "heading": -0.010917643304867627, + "angularVelocity": 0.2879233497741951, + "velocityX": -0.43090322530503955, + "velocityY": 0.49771977022960984, + "timestamp": 2.341489493696694 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 2.368719711095728e-29, - "angularVelocity": 0.1686960564308085, - "velocityX": -0.2527377395078362, - "velocityY": 0.2920386866937519, - "timestamp": 2.2019792255538246 + "heading": 5.072711091682045e-24, + "angularVelocity": 0.14389272789792218, + "velocityX": -0.21545347602968715, + "velocityY": 0.24886063882779058, + "timestamp": 2.417362982679491 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 1.1956060311838367e-29, - "angularVelocity": -1.025164181333131e-28, - "velocityX": 1.21885672142624e-30, - "velocityY": -5.1343833235782786e-30, - "timestamp": 2.2761644316796312 + "heading": 2.471925946464062e-24, + "angularVelocity": -1.3881126396060902e-24, + "velocityX": -6.902910153275516e-22, + "velocityY": 3.1112652084354562e-24, + "timestamp": 2.493236471662288 + }, + { + "x": 1.8507557540076562, + "y": 5.5430527114888575, + "heading": -8.800046052500135e-19, + "angularVelocity": -9.452376611096758e-18, + "velocityX": 0.4061756726920047, + "velocityY": 0.0005740544422031257, + "timestamp": 2.586335499913798 + }, + { + "x": 1.9263848743459357, + "y": 5.543159599309593, + "heading": -2.769921496163024e-18, + "angularVelocity": -2.0300071079064193e-17, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765763312, + "timestamp": 2.679434528165308 + }, + { + "x": 2.0398285536670646, + "y": 5.54331993103902, + "heading": -5.903589755199969e-18, + "angularVelocity": -3.365951630081127e-17, + "velocityX": 1.2185269970236072, + "velocityY": 0.0017221632968556807, + "timestamp": 2.772533556416818 + }, + { + "x": 2.191086789804772, + "y": 5.5435337066740775, + "heading": -1.0884334063348694e-17, + "angularVelocity": -5.34994231591497e-17, + "velocityX": 1.6247026309348636, + "velocityY": 0.0022962176842493674, + "timestamp": 2.865632584668328 + }, + { + "x": 2.380159571927711, + "y": 5.543800926199458, + "heading": -4.173692512602447e-18, + "angularVelocity": 7.208068308331619e-17, + "velocityX": 2.0308781485038967, + "velocityY": 0.0028702719072147666, + "timestamp": 2.958731612919838 + }, + { + "x": 2.5314178080654184, + "y": 5.544014701834515, + "heading": -1.8153111851912927e-18, + "angularVelocity": 2.5331965023903977e-17, + "velocityX": 1.6247026309348636, + "velocityY": 0.002296217684249368, + "timestamp": 3.0518306411713483 + }, + { + "x": 2.644861487386547, + "y": 5.544175033563942, + "heading": -7.161538717554587e-19, + "angularVelocity": 1.1806324234469696e-17, + "velocityX": 1.2185269970236072, + "velocityY": 0.001722163296855681, + "timestamp": 3.1449296694228583 + }, + { + "x": 2.7204906077248263, + "y": 5.544281921384678, + "heading": -1.8348593704837507e-19, + "angularVelocity": 5.72151981311354e-18, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765763316, + "timestamp": 3.2380286976743684 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -9.055003253623357e-30, + "angularVelocity": 1.970868445148275e-18, + "velocityX": 0.4061756726920047, + "velocityY": 0.0005740544422031258, + "timestamp": 3.3311277259258785 }, { - "x": 1.8602093956334294, - "y": 5.5430660724686875, - "heading": -1.6999094202020734e-20, - "angularVelocity": -1.7890242544799916e-19, - "velocityX": 0.49746156397034486, - "velocityY": 0.0007030702226192673, - "timestamp": 2.3711832340882935 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.6159098195352036e-29, + "angularVelocity": -1.6705479392630998e-29, + "velocityX": 6.780641024803866e-22, + "velocityY": 9.718698194414986e-25, + "timestamp": 3.4242267541773885 + }, + { + "x": 2.746560227605712, + "y": 5.55825095759549, + "heading": 0.00742109779657271, + "angularVelocity": 0.11458935665570884, + "velocityX": -0.18135392067783562, + "velocityY": 0.21487100869168943, + "timestamp": 3.4889892941999308 + }, + { + "x": 2.7232728362575633, + "y": 5.58624983061418, + "heading": 0.022298554503038515, + "angularVelocity": 0.22972318104396974, + "velocityX": -0.35958119215279744, + "velocityY": 0.43233129844728024, + "timestamp": 3.553751834222473 + }, + { + "x": 2.6887224275841572, + "y": 5.628555352569631, + "heading": 0.04468536543200197, + "angularVelocity": 0.3456753073794072, + "velocityX": -0.5334937243255126, + "velocityY": 0.6532406224451052, + "timestamp": 3.618514374245015 + }, + { + "x": 2.6433185485147903, + "y": 5.685479106691384, + "heading": 0.07465726375014338, + "angularVelocity": 0.46279683143540923, + "velocityX": -0.7010824321214573, + "velocityY": 0.8789611108819849, + "timestamp": 3.6832769142675574 + }, + { + "x": 2.587716352440023, + "y": 5.7574833976129485, + "heading": 0.11231839668543797, + "angularVelocity": 0.5815264954429781, + "velocityX": -0.8585549000303846, + "velocityY": 1.1118200567257257, + "timestamp": 3.7480394542900997 + }, + { + "x": 2.523118270580454, + "y": 5.845312686573325, + "heading": 0.15779630703388142, + "angularVelocity": 0.7022255509529696, + "velocityX": -0.9974605973929359, + "velocityY": 1.356174247177545, + "timestamp": 3.812801994312642 + }, + { + "x": 2.4523262633845233, + "y": 5.950260122565857, + "heading": 0.21110300167691656, + "angularVelocity": 0.8231100050195744, + "velocityX": -1.093101153402716, + "velocityY": 1.6204959835732518, + "timestamp": 3.877564534335184 + }, + { + "x": 2.385035672029833, + "y": 6.073114038949963, + "heading": 0.27020607963640353, + "angularVelocity": 0.9126121047586251, + "velocityX": -1.0390357038384854, + "velocityY": 1.896990395085535, + "timestamp": 3.9423270743577263 + }, + { + "x": 2.335844965932351, + "y": 6.198572107213467, + "heading": 0.32662517660992413, + "angularVelocity": 0.8711686872362097, + "velocityX": -0.7595549229594762, + "velocityY": 1.9372011693771587, + "timestamp": 4.007089614380269 + }, + { + "x": 2.303814652096041, + "y": 6.317831411051833, + "heading": 0.37736366027868895, + "angularVelocity": 0.7834541951428096, + "velocityX": -0.4945808769260876, + "velocityY": 1.8414858928765832, + "timestamp": 4.071852154402811 + }, + { + "x": 2.2877361979926687, + "y": 6.4282644247646346, + "heading": 0.42161092558434743, + "angularVelocity": 0.6832231300726793, + "velocityX": -0.24826781188285701, + "velocityY": 1.7051989263293559, + "timestamp": 4.1366146944253535 + }, + { + "x": 2.2869063645502266, + "y": 6.528717533266064, + "heading": 0.45901061919668296, + "angularVelocity": 0.5774896043193742, + "velocityX": -0.012813478936328582, + "velocityY": 1.5510989603938259, + "timestamp": 4.201377234447896 + }, + { + "x": 2.3008846173823154, + "y": 6.618554541912377, + "heading": 0.48936477750092344, + "angularVelocity": 0.46869931743991045, + "velocityX": 0.21583855153339288, + "velocityY": 1.3871754970549879, + "timestamp": 4.266139774470438 }, { - "x": 1.9547457978922331, - "y": 5.543199682247202, - "heading": -3.210902716840507e-20, - "angularVelocity": -1.5902045262092918e-19, - "velocityX": 0.9949231085045297, - "velocityY": 0.001406140417769105, - "timestamp": 2.4662020364969557 + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.3580100794534351, + "velocityX": 0.43988065652810937, + "velocityY": 1.21706589595492, + "timestamp": 4.33090231449298 }, { - "x": 2.0965503966634373, - "y": 5.543400096908449, - "heading": -4.532979164623425e-20, - "angularVelocity": -1.3913840358532023e-19, - "velocityX": 1.492384614166397, - "velocityY": 0.0021092105579800877, - "timestamp": 2.561220838905618 + "x": 2.3907092837867907, + "y": 6.780867422722411, + "heading": 0.5304481214064672, + "angularVelocity": 0.21222434211791516, + "velocityX": 0.7273100577595625, + "velocityY": 0.990024458199304, + "timestamp": 4.415236192632761 }, { - "x": 2.2856231808662413, - "y": 5.543667316436768, - "heading": -7.554638691710094e-20, - "angularVelocity": -3.1800648403053284e-19, - "velocityX": 1.9898460032113345, - "velocityY": 0.0028122805333745363, - "timestamp": 2.65623964131428 + "x": 2.471644870352802, + "y": 6.840510057948011, + "heading": 0.53507672796877, + "angularVelocity": 0.05488430823293737, + "velocityX": 0.9597043128013546, + "velocityY": 0.7072203548702543, + "timestamp": 4.499570070772541 + }, + { + "x": 2.556252265889606, + "y": 6.8694746689081185, + "heading": 0.5277386544868528, + "angularVelocity": -0.08701216692246314, + "velocityX": 1.0032432683407548, + "velocityY": 0.3434516661512893, + "timestamp": 4.5839039489123214 + }, + { + "x": 2.617383928971812, + "y": 6.87815517989328, + "heading": 0.5183265349753615, + "angularVelocity": -0.1116054392268187, + "velocityX": 0.7248766976052303, + "velocityY": 0.1029302953526471, + "timestamp": 4.668237827052102 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06849104420156939, + "velocityX": 0.36711922260971214, + "velocityY": 0.01766420444115541, + "timestamp": 4.752571705191882 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -1.2403630086849167e-25, + "velocityX": 1.2566075146445203e-23, + "velocityY": 7.418296886220231e-24, + "timestamp": 4.836905583331663 + }, + { + "x": 2.65710997558339, + "y": 6.867790932222418, + "heading": 0.5069947283791145, + "angularVelocity": -0.09538266194620745, + "velocityX": 0.1504894284206614, + "velocityY": -0.20351386841397834, + "timestamp": 4.895151926089623 + }, + { + "x": 2.6746461929799414, + "y": 6.844083289351916, + "heading": 0.4960269163567629, + "angularVelocity": -0.18830044090369472, + "velocityX": 0.3010698451819098, + "velocityY": -0.40702371596133824, + "timestamp": 4.953398268847583 + }, + { + "x": 2.7009592416637416, + "y": 6.808522263895978, + "heading": 0.4798144698796375, + "angularVelocity": -0.27834273723408604, + "velocityX": 0.4517545211918758, + "velocityY": -0.6105280395665419, + "timestamp": 5.0116446116055435 + }, + { + "x": 2.736056173569292, + "y": 6.76110833670321, + "heading": 0.4585545185998355, + "angularVelocity": -0.3650006210372158, + "velocityX": 0.6025602680565616, + "velocityY": -0.8140241077417322, + "timestamp": 5.069890954363504 + }, + { + "x": 2.7799452908871816, + "y": 6.701842252248569, + "heading": 0.4324815844111489, + "angularVelocity": -0.4476321251109512, + "velocityX": 0.7535085507474468, + "velocityY": -1.017507394428501, + "timestamp": 5.128137297121464 + }, + { + "x": 2.8326365093294883, + "y": 6.63072517677858, + "heading": 0.40187833162186215, + "angularVelocity": -0.5254107183425597, + "velocityX": 0.9046270709435361, + "velocityY": -1.2209706584585491, + "timestamp": 5.186383639879424 + }, + { + "x": 2.8941418577048204, + "y": 6.54775894755859, + "heading": 0.367091043865372, + "angularVelocity": -0.5972441548999393, + "velocityX": 1.0559521072578695, + "velocityY": -1.4244023794721719, + "timestamp": 5.244629982637385 + }, + { + "x": 2.9644761872265666, + "y": 6.452946486925878, + "heading": 0.3285529983813312, + "angularVelocity": -0.661638888542476, + "velocityX": 1.2075321160337456, + "velocityY": -1.6277839284553843, + "timestamp": 5.302876325395345 + }, + { + "x": 3.043658216776231, + "y": 6.346292539288771, + "heading": 0.2868218829421827, + "angularVelocity": -0.7164589820267359, + "velocityX": 1.359433499176103, + "velocityY": -1.8310840232545251, + "timestamp": 5.361122668153305 + }, + { + "x": 3.131712135507504, + "y": 6.227805106020448, + "heading": 0.24264403908378393, + "angularVelocity": -0.7584655407804345, + "velocityX": 1.5117501728336273, + "velocityY": -2.03424674679905, + "timestamp": 5.419369010911265 + }, + { + "x": 3.2286701423571795, + "y": 6.097498584088062, + "heading": 0.19707481942882135, + "angularVelocity": -0.7823533203504854, + "velocityX": 1.6646196526463415, + "velocityY": -2.2371622965903253, + "timestamp": 5.477615353669226 + }, + { + "x": 3.33457642569322, + "y": 5.9554017916384385, + "heading": 0.1517318182422537, + "angularVelocity": -0.7784694976470596, + "velocityX": 1.8182477786824947, + "velocityY": -2.439583083183451, + "timestamp": 5.535861696427186 + }, + { + "x": 3.449491590462662, + "y": 5.801583658823103, + "heading": 0.10942509922281701, + "angularVelocity": -0.726341209013589, + "velocityX": 1.9729163983216391, + "velocityY": -2.6408204452341257, + "timestamp": 5.594108039185146 + }, + { + "x": 3.5734690777406897, + "y": 5.636270409713289, + "heading": 0.07620523271942112, + "angularVelocity": -0.5703339459687529, + "velocityX": 2.128502518917107, + "velocityY": -2.838173888389265, + "timestamp": 5.652354381943106 + }, + { + "x": 3.704953237459551, + "y": 5.460551173556115, + "heading": 0.07376572964977675, + "angularVelocity": -0.04188251062872067, + "velocityX": 2.2573805237049305, + "velocityY": -3.016828659738646, + "timestamp": 5.710600724701067 + }, + { + "x": 3.8484687741574635, + "y": 5.2934320188975885, + "heading": 0.07376572135485625, + "angularVelocity": -1.4241101012224094e-7, + "velocityX": 2.4639407369194757, + "velocityY": -2.86917850538672, + "timestamp": 5.768847067459027 + }, + { + "x": 4.002883086238839, + "y": 5.136327781933491, + "heading": 0.07376571311141436, + "angularVelocity": -1.4152720162027837e-7, + "velocityX": 2.651055925056733, + "velocityY": -2.697237792541529, + "timestamp": 5.827093410216987 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.07376570463306892, + "angularVelocity": -1.4556013372477054e-7, + "velocityX": 2.826202837221629, + "velocityY": -2.51311892102558, + "timestamp": 5.885339752974947 + }, + { + "x": 4.268981030537756, + "y": 4.906736402889882, + "heading": 0.07376569669894775, + "angularVelocity": -2.2864637143597095e-7, + "velocityX": 2.9245183697828536, + "velocityY": -2.397995023127166, + "timestamp": 5.920040155676169 + }, + { + "x": 4.373711969606293, + "y": 4.827653183122543, + "heading": 0.07376568907042552, + "angularVelocity": -2.198395881628289e-7, + "velocityX": 3.0181476558152474, + "velocityY": -2.2790288760699386, + "timestamp": 5.954740558377391 + }, + { + "x": 4.481524054047376, + "y": 4.75282484787289, + "heading": 0.0737656816823934, + "angularVelocity": -2.129091176782688e-7, + "velocityX": 3.106940440125975, + "velocityY": -2.1564111487103386, + "timestamp": 5.989440961078612 + }, + { + "x": 4.592244511621915, + "y": 4.682371277147909, + "heading": 0.07376567447691734, + "angularVelocity": -2.0764819690223906e-7, + "velocityX": 3.1907542551556984, + "velocityY": -2.030338706198957, + "timestamp": 6.024141363779834 + }, + { + "x": 4.705695889724197, + "y": 4.616405299617974, + "heading": 0.07376566740123429, + "angularVelocity": -2.0390780833167242e-7, + "velocityX": 3.2694542215871127, + "velocityY": -1.90101475472538, + "timestamp": 6.058841766481056 + }, + { + "x": 4.821696252971789, + "y": 4.555032332206395, + "heading": 0.07376566040606643, + "angularVelocity": -2.0158751247220773e-7, + "velocityX": 3.342911154270497, + "velocityY": -1.76865288682714, + "timestamp": 6.093542169182277 + }, + { + "x": 4.939640387985606, + "y": 4.497483166935729, + "heading": 0.07376565344322925, + "angularVelocity": -2.0065580355812287e-7, + "velocityX": 3.398926981607191, + "velocityY": -1.6584581385460047, + "timestamp": 6.128242571883499 + }, + { + "x": 5.057585335343478, + "y": 4.439935666553369, + "heading": 0.07376564648040178, + "angularVelocity": -2.0065552340961329e-7, + "velocityX": 3.3989503918269923, + "velocityY": -1.658410159612758, + "timestamp": 6.162942974584721 + }, + { + "x": 5.1755309999931605, + "y": 4.382389636300601, + "heading": 0.07376563951757731, + "angularVelocity": -2.0065543705761684e-7, + "velocityX": 3.398971062820868, + "velocityY": -1.658367793257397, + "timestamp": 6.197643377285942 + }, + { + "x": 5.295248531429034, + "y": 4.328626841825402, + "heading": 0.07376563255290919, + "angularVelocity": -2.007085673925138e-7, + "velocityX": 3.45003291364278, + "velocityY": -1.5493420908716486, + "timestamp": 6.232343779987164 + }, + { + "x": 5.41702166916594, + "y": 4.27969803322079, + "heading": 0.07376562554616754, + "angularVelocity": -2.019210467909896e-7, + "velocityX": 3.5092716008341425, + "velocityY": -1.410035757391629, + "timestamp": 6.267044182688386 + }, + { + "x": 5.540655427942115, + "y": 4.235681910653619, + "heading": 0.07376561844909023, + "angularVelocity": -2.0452435036769212e-7, + "velocityX": 3.5628911814277564, + "velocityY": -1.2684614338963205, + "timestamp": 6.301744585389607 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.07376561120324407, + "angularVelocity": -2.0881158710741676e-7, + "velocityX": 3.610802501559291, + "velocityY": -1.1248525394721576, + "timestamp": 6.336444988090829 + }, + { + "x": 5.801240954312848, + "y": 4.160760783914465, + "heading": 0.0737656041692584, + "angularVelocity": -1.900588111168926e-7, + "velocityX": 3.655524839727154, + "velocityY": -0.9697042570289608, + "timestamp": 6.37345450968449 + }, + { + "x": 5.937938725958539, + "y": 4.130679839530998, + "heading": 0.07376559729974105, + "angularVelocity": -1.8561486466113367e-7, + "velocityX": 3.6935838605680487, + "velocityY": -0.8127893333433076, + "timestamp": 6.410464031278151 + }, + { + "x": 6.075795864969434, + "y": 4.106460978217925, + "heading": 0.07376559053845443, + "angularVelocity": -1.8269046288328192e-7, + "velocityX": 3.724910052187935, + "velocityY": -0.654395416914037, + "timestamp": 6.447473552871812 + }, + { + "x": 6.214561046214486, + "y": 4.088147908867804, + "heading": 0.07376558383255526, + "angularVelocity": -1.8119388963756882e-7, + "velocityX": 3.7494454202515035, + "velocityY": -0.4948204829877394, + "timestamp": 6.484483074465473 + }, + { + "x": 6.353981153300122, + "y": 4.0757714079596425, + "heading": 0.07376557713086826, + "angularVelocity": -1.8108007594786474e-7, + "velocityX": 3.7671415647133246, + "velocityY": -0.33441396633135523, + "timestamp": 6.521492596059134 + }, + { + "x": 6.493802446577043, + "y": 4.0693566803624766, + "heading": 0.07376555560001419, + "angularVelocity": -5.817652634682061e-7, + "velocityX": 3.7779816451577393, + "velocityY": -0.17332641225670456, + "timestamp": 6.558502117652795 + }, + { + "x": 6.62912467253087, + "y": 4.064792329127301, + "heading": 0.05903973203041549, + "angularVelocity": -0.39789283772100603, + "velocityX": 3.656416514635636, + "velocityY": -0.12332910663606324, + "timestamp": 6.595511639246456 + }, + { + "x": 6.758568056447626, + "y": 4.061299539806061, + "heading": 0.04418585550364903, + "angularVelocity": -0.4013528380575057, + "velocityX": 3.4975697696920878, + "velocityY": -0.09437542477821895, + "timestamp": 6.632521160840117 + }, + { + "x": 6.882108384071856, + "y": 4.058691153499139, + "heading": 0.0304778500113165, + "angularVelocity": -0.3703913182893011, + "velocityX": 3.3380687537822458, + "velocityY": -0.07047879017620408, + "timestamp": 6.669530682433778 + }, + { + "x": 6.999742512686264, + "y": 4.056896966002031, + "heading": 0.018392940314004957, + "angularVelocity": -0.3265351503322698, + "velocityX": 3.1784828214195313, + "velocityY": -0.048479078351971995, + "timestamp": 6.706540204027439 + }, + { + "x": 7.111470092404711, + "y": 4.055880215977976, + "heading": 0.008182013942056482, + "angularVelocity": -0.2758999828221868, + "velocityX": 3.0188874351076422, + "velocityY": -0.027472660555273344, + "timestamp": 6.7435497256211 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": -2.0237932968568665e-24, + "angularVelocity": -0.22107861949390495, + "velocityX": 2.859298314909037, + "velocityY": -0.007077363713034477, + "timestamp": 6.780559247214761 + }, + { + "x": 7.386520754967261, + "y": 4.057483355651357, + "heading": -0.007799169879435119, + "angularVelocity": -0.11872019703645657, + "velocityX": 2.5760366812439752, + "velocityY": 0.028390383098604113, + "timestamp": 6.846252954554551 + }, + { + "x": 7.536988258489095, + "y": 4.060513288338981, + "heading": -0.011310959722615328, + "angularVelocity": -0.053457020244208106, + "velocityX": 2.2904401291217464, + "velocityY": 0.04612211443560253, + "timestamp": 6.911946661894341 + }, + { + "x": 7.668645085347693, + "y": 4.063976331623079, + "heading": -0.012040004750973126, + "angularVelocity": -0.01109763869143412, + "velocityX": 2.004101034785968, + "velocityY": 0.05271499241448283, + "timestamp": 6.9776403692341304 + }, + { + "x": 7.78147865771714, + "y": 4.06737291870627, + "heading": -0.011006488514551415, + "angularVelocity": 0.015732347560718764, + "velocityX": 1.7175704787953916, + "velocityY": 0.051703385616870434, + "timestamp": 7.04333407657392 + }, + { + "x": 7.87549031086396, + "y": 4.070341151997571, + "heading": -0.008947695308103269, + "angularVelocity": 0.03133927570565307, + "velocityX": 1.4310602484429864, + "velocityY": 0.04518291646943911, + "timestamp": 7.10902778391371 + }, + { + "x": 7.9506868858752515, + "y": 4.072607124350486, + "heading": -0.006421398747140829, + "angularVelocity": 0.03845568568532146, + "velocityX": 1.1446541542000461, + "velocityY": 0.03449298943040145, + "timestamp": 7.1747214912535 + }, + { + "x": 8.00707724488628, + "y": 4.073956458693622, + "heading": -0.003864315760597132, + "angularVelocity": 0.038924321523180636, + "velocityX": 0.8583829607812892, + "velocityY": 0.020539780715319512, + "timestamp": 7.24041519859329 + }, + { + "x": 8.044670745645819, + "y": 4.074216878653992, + "heading": -0.0016276569414438076, + "angularVelocity": 0.034046774184696094, + "velocityX": 0.5722542124939418, + "velocityY": 0.003964153811924349, + "timestamp": 7.30610890593308 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 1.6866276328916314e-24, + "angularVelocity": 0.02477645131252876, + "velocityX": 0.286265117553968, + "velocityY": -0.014764317948939539, + "timestamp": 7.37180261327287 + }, + { + "x": 8.060961534281953, + "y": 4.070465444687161, + "heading": 0.0007560599723107442, + "angularVelocity": 0.010267428471842888, + "velocityX": -0.03415452911035645, + "velocityY": -0.03777341504058265, + "timestamp": 7.445439354389123 + }, + { + "x": 8.034839704830464, + "y": 4.066167075236277, + "heading": 0.00046881224437648735, + "angularVelocity": -0.0039008750737728444, + "velocityX": -0.3547390752973492, + "velocityY": -0.0583726192349958, + "timestamp": 7.519076095505376 + }, + { + "x": 7.985098416570273, + "y": 4.060562217226684, + "heading": -0.0008319841828060777, + "angularVelocity": -0.017665046109644415, + "velocityX": -0.6754955136004895, + "velocityY": -0.0761149654999585, + "timestamp": 7.592712836621629 + }, + { + "x": 7.9117249252468085, + "y": 4.053904178499818, + "heading": -0.0031104504440480647, + "angularVelocity": -0.03094197579500262, + "velocityX": -0.9964250211402843, + "velocityY": -0.09041734636729296, + "timestamp": 7.666349577737883 + }, + { + "x": 7.814707316285252, + "y": 4.046503723453766, + "heading": -0.00632248118792148, + "angularVelocity": -0.04361994698818144, + "velocityX": -1.3175163307185394, + "velocityY": -0.10049949161068944, + "timestamp": 7.739986318854136 + }, + { + "x": 7.69403642851654, + "y": 4.038750888931838, + "heading": -0.010412565479377186, + "angularVelocity": -0.055544069841419484, + "velocityX": -1.6387320505969085, + "velocityY": -0.10528486736925086, + "timestamp": 7.813623059970389 + }, + { + "x": 7.549709960346403, + "y": 4.031149265281445, + "heading": -0.015308761000708897, + "angularVelocity": -0.06649120326498265, + "velocityX": -1.9599790265335464, + "velocityY": -0.10323139692441612, + "timestamp": 7.887259801086643 + }, + { + "x": 7.381741634294396, + "y": 4.0243729190236985, + "heading": -0.02091427607708914, + "angularVelocity": -0.07612388858342581, + "velocityX": -2.2810396482216517, + "velocityY": -0.0920239836123183, + "timestamp": 7.960896542202896 + }, + { + "x": 7.1901832106568095, + "y": 4.019367673272749, + "heading": -0.027092292311801627, + "angularVelocity": -0.08389855581684406, + "velocityX": -2.6013973559091212, + "velocityY": -0.06797212471757884, + "timestamp": 8.03453328331915 + }, + { + "x": 6.975183472586086, + "y": 4.017547771662454, + "heading": -0.03363583639032012, + "angularVelocity": -0.08886248874305737, + "velocityX": -2.9197345620074877, + "velocityY": -0.02471458653257856, + "timestamp": 8.108170024435402 + }, + { + "x": 6.737173435740335, + "y": 4.021221144917956, + "heading": -0.04019984018910067, + "angularVelocity": -0.0891403353716819, + "velocityX": -3.232218499050576, + "velocityY": 0.04988506009116617, + "timestamp": 8.181806765551656 + }, + { + "x": 6.477592542757965, + "y": 4.034606496898433, + "heading": -0.04612188209053887, + "angularVelocity": -0.08042237898726157, + "velocityX": -3.5251545498538244, + "velocityY": 0.18177545308999152, + "timestamp": 8.255443506667909 + }, + { + "x": 6.202379983792451, + "y": 4.065741183912862, + "heading": -0.04994434785858321, + "angularVelocity": -0.05190976284528472, + "velocityX": -3.737435345366867, + "velocityY": 0.4228145697713012, + "timestamp": 8.329080247784162 + }, + { + "x": 5.92930929060311, + "y": 4.120418111584284, + "heading": -0.049944382856938176, + "angularVelocity": -4.752838655296773e-7, + "velocityX": -3.7083484283780552, + "velocityY": 0.7425223718836313, + "timestamp": 8.402716988900416 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.05351191132153618, + "angularVelocity": -0.04844766906462906, + "velocityX": -3.5764423817525626, + "velocityY": 1.0352299927261919, + "timestamp": 8.476353730016669 + }, + { + "x": 5.427595279774474, + "y": 4.2855510061769815, + "heading": -0.058564536366975395, + "angularVelocity": -0.07109019818968403, + "velocityX": -3.3536640954167023, + "velocityY": 1.2508460219498765, + "timestamp": 8.547427171561418 + }, + { + "x": 5.210001648120265, + "y": 4.381839057848659, + "heading": -0.06451807274962795, + "angularVelocity": -0.08376597858855642, + "velocityX": -3.0615322253278787, + "velocityY": 1.3547683857556179, + "timestamp": 8.618500613106168 + }, + { + "x": 5.014433901254649, + "y": 4.478870971040357, + "heading": -0.07079419051912546, + "angularVelocity": -0.08830468362146032, + "velocityX": -2.751629056016378, + "velocityY": 1.365234482568344, + "timestamp": 8.689574054650917 + }, + { + "x": 4.840611358699117, + "y": 4.572318839972298, + "heading": -0.07701240147326043, + "angularVelocity": -0.08748993743633168, + "velocityX": -2.445675047916283, + "velocityY": 1.3148071473801226, + "timestamp": 8.760647496195666 + }, + { + "x": 4.687900440256207, + "y": 4.6594427038525446, + "heading": -0.08292539389195669, + "angularVelocity": -0.08319552691103697, + "velocityX": -2.148635483575938, + "velocityY": 1.2258286919367853, + "timestamp": 8.831720937740416 + }, + { + "x": 4.555672965258425, + "y": 4.738435503151125, + "heading": -0.088365740210127, + "angularVelocity": -0.07654541837185252, + "velocityX": -1.8604343918610988, + "velocityY": 1.1114249933829867, + "timestamp": 8.902794379285165 + }, + { + "x": 4.443385396304237, + "y": 4.808043311257638, + "heading": -0.09321471505627854, + "angularVelocity": -0.06822484940592717, + "velocityX": -1.5798808459765967, + "velocityY": 0.9793786060392976, + "timestamp": 8.973867820829915 + }, + { + "x": 4.3505830000627, + "y": 4.867355530244293, + "heading": -0.09738458875338916, + "angularVelocity": -0.058669927985478765, + "velocityX": -1.3057253768006336, + "velocityY": 0.8345201484201303, + "timestamp": 9.044941262374664 + }, + { + "x": 4.2768869568191645, + "y": 4.915685373463825, + "heading": -0.10080825239426806, + "angularVelocity": -0.04817078737805652, + "velocityX": -1.0368998833007734, + "velocityY": 0.6799986347798151, + "timestamp": 9.116014703919413 + }, + { + "x": 4.22197990513087, + "y": 4.95249852612905, + "heading": -0.10343286860563979, + "angularVelocity": -0.03692822739868577, + "velocityX": -0.7725396504645569, + "velocityY": 0.5179593370618798, + "timestamp": 9.187088145464163 + }, + { + "x": 4.185593591353172, + "y": 4.97736854606154, + "heading": -0.10521581647883109, + "angularVelocity": -0.025085993226720307, + "velocityX": -0.5119537338682039, + "velocityY": 0.3499200178287514, + "timestamp": 9.258161587008912 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.012749959780663622, + "velocityX": -0.25458913429159136, + "velocityY": 0.1769894567221649, + "timestamp": 9.329235028553661 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": 1.153459712624117e-25, + "velocityX": -3.247224190829055e-25, + "velocityY": -4.023670796703132e-25, + "timestamp": 9.40030847009841 + }, + { + "x": 4.180652954196475, + "y": 4.972113880358222, + "heading": -0.10282545923039073, + "angularVelocity": 0.04623963021087373, + "velocityX": 0.18450581877512717, + "velocityY": -0.2501512088012644, + "timestamp": 9.471601011898064 + }, + { + "x": 4.207780209062833, + "y": 4.937082603957029, + "heading": -0.09606696501055557, + "angularVelocity": 0.09479945656626756, + "velocityX": 0.380506209788268, + "velocityY": -0.4913736488683167, + "timestamp": 9.542893553697718 + }, + { + "x": 4.249890859346282, + "y": 4.885729175148701, + "heading": -0.08564531821991797, + "angularVelocity": 0.1461814451773148, + "velocityX": 0.5906739922639744, + "velocityY": -0.7203197909907696, + "timestamp": 9.614186095497372 + }, + { + "x": 4.308238374054552, + "y": 4.819303841665251, + "heading": -0.07131614738285201, + "angularVelocity": 0.20099116226398103, + "velocityX": 0.8184238243635266, + "velocityY": -0.931729067398359, + "timestamp": 9.685478637297026 + }, + { + "x": 4.384365934500173, + "y": 4.739668033977246, + "heading": -0.05278830976865122, + "angularVelocity": 0.2598846547829301, + "velocityX": 1.0678194173459812, + "velocityY": -1.1170285934228166, + "timestamp": 9.75677117909668 + }, + { + "x": 4.480080041577144, + "y": 4.649696456670524, + "heading": -0.02973745107706772, + "angularVelocity": 0.3233277718777487, + "velocityX": 1.3425542793234355, + "velocityY": -1.262005464183884, + "timestamp": 9.828063720896333 + }, + { + "x": 4.597147014770728, + "y": 4.553865445568556, + "heading": -0.0018737410680010795, + "angularVelocity": 0.3908362544762254, + "velocityX": 1.642064797220515, + "velocityY": -1.344194058493121, + "timestamp": 9.899356262695987 + }, + { + "x": 4.7363464626810785, + "y": 4.458651640127929, + "heading": 0.030870956626381736, + "angularVelocity": 0.4593004663293065, + "velocityX": 1.9525106609542362, + "velocityY": -1.3355366920174645, + "timestamp": 9.97064880449564 + }, + { + "x": 4.8961444414371185, + "y": 4.371572487351198, + "heading": 0.06809613303796244, + "angularVelocity": 0.5221468539611136, + "velocityX": 2.2414403347422116, + "velocityY": -1.2214342563551692, + "timestamp": 10.041941346295294 + }, + { + "x": 5.072824953795758, + "y": 4.298842121244781, + "heading": 0.10897366885891092, + "angularVelocity": 0.5733774499978155, + "velocityX": 2.478246782884325, + "velocityY": -1.0201679484342572, + "timestamp": 10.113233888094948 + }, + { + "x": 5.262214444102689, + "y": 4.244271073764123, + "heading": 0.1525665304554643, + "angularVelocity": 0.6114645444828949, + "velocityX": 2.6565119650124474, + "velocityY": -0.7654524036190654, + "timestamp": 10.184526429894602 + }, + { + "x": 5.460811083302549, + "y": 4.20986764407754, + "heading": 0.19806311217601916, + "angularVelocity": 0.638167479684045, + "velocityX": 2.785657997129001, + "velocityY": -0.48256702339585567, + "timestamp": 10.255818971694255 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.24483305037299086, + "angularVelocity": 0.6560284851170571, + "velocityX": 2.8774488935285283, + "velocityY": -0.18541307673169433, + "timestamp": 10.327111513493909 + }, + { + "x": 5.802993350275697, + "y": 4.197353588266771, + "heading": 0.2759731999769641, + "angularVelocity": 0.6634007595054764, + "velocityX": 2.919495150576939, + "velocityY": 0.015008756790958989, + "timestamp": 10.374051691294106 + }, + { + "x": 5.941816707126591, + "y": 4.207504283843094, + "heading": 0.3073984177151568, + "angularVelocity": 0.669473768760635, + "velocityX": 2.9574527272095095, + "velocityY": 0.21624748886826958, + "timestamp": 10.420991869094303 + }, + { + "x": 6.082189718099688, + "y": 4.227142107895406, + "heading": 0.33903536512016264, + "angularVelocity": 0.6739843964730912, + "velocityX": 2.990466111368347, + "velocityY": 0.4183585357494954, + "timestamp": 10.4679320468945 + }, + { + "x": 6.223826665767993, + "y": 4.256309424549035, + "heading": 0.37079417516105295, + "angularVelocity": 0.6765805228960279, + "velocityX": 3.01739265392617, + "velocityY": 0.6213720957295971, + "timestamp": 10.514872224694697 + }, + { + "x": 6.366368317456564, + "y": 4.295047066484075, + "heading": 0.4025622544615229, + "angularVelocity": 0.6767779925268211, + "velocityX": 3.0366662072586053, + "velocityY": 0.8252555433413251, + "timestamp": 10.561812402494894 + }, + { + "x": 6.509351710185931, + "y": 4.3433874902945675, + "heading": 0.4341948054163167, + "angularVelocity": 0.6738907357666759, + "velocityX": 3.0460769308966165, + "velocityY": 1.0298304368648719, + "timestamp": 10.608752580295091 + }, + { + "x": 6.652162883267978, + "y": 4.401339041781646, + "heading": 0.4654999103087445, + "angularVelocity": 0.6669149193613809, + "velocityX": 3.0424080132360682, + "velocityY": 1.2345831269270133, + "timestamp": 10.655692758095288 + }, + { + "x": 6.793961557546229, + "y": 4.468849069286898, + "heading": 0.4962146580453544, + "angularVelocity": 0.654338120902485, + "velocityX": 3.0208380309469742, + "velocityY": 1.4382141412546399, + "timestamp": 10.702632935895485 + }, + { + "x": 6.933563512190389, + "y": 4.545714915555555, + "heading": 0.5259679790741631, + "angularVelocity": 0.6338561638060835, + "velocityX": 2.9740397498786737, + "velocityY": 1.6375278039175576, + "timestamp": 10.749573113695682 + }, + { + "x": 7.069284940391168, + "y": 4.631368590173797, + "heading": 0.5542337619189218, + "angularVelocity": 0.6021660796657563, + "velocityX": 2.8913701345248803, + "velocityY": 1.8247411627375887, + "timestamp": 10.796513291495879 + }, + { + "x": 7.1988866969659675, + "y": 4.724417833549329, + "heading": 0.5803339735600787, + "angularVelocity": 0.5560313757705206, + "velocityX": 2.7609984164621943, + "velocityY": 1.9822942250367808, + "timestamp": 10.843453469296076 + }, + { + "x": 7.32010179733884, + "y": 4.822118790801402, + "heading": 0.6036855499367045, + "angularVelocity": 0.49747524340496785, + "velocityX": 2.5823315132896965, + "velocityY": 2.0813929948868157, + "timestamp": 10.890393647096273 + }, + { + "x": 7.431787144885325, + "y": 4.920866698749227, + "heading": 0.6241547457610245, + "angularVelocity": 0.43606984003016963, + "velocityX": 2.379312409549823, + "velocityY": 2.103696930338622, + "timestamp": 10.93733382489647 + }, + { + "x": 7.534091847804722, + "y": 5.017622526169069, + "heading": 0.6419335715808812, + "angularVelocity": 0.37875497394860314, + "velocityX": 2.1794698638522156, + "velocityY": 2.0612582217239557, + "timestamp": 10.984274002696667 + }, + { + "x": 7.627668873285194, + "y": 5.110394301950092, + "heading": 0.6572905320305523, + "angularVelocity": 0.32716025309998836, + "velocityX": 1.9935379426721815, + "velocityY": 1.9763831354859511, + "timestamp": 11.031214180496864 + }, + { + "x": 7.7131909565949455, + "y": 5.197947878934244, + "heading": 0.6704631268278166, + "angularVelocity": 0.2806251576918574, + "velocityX": 1.8219377794813416, + "velocityY": 1.8652161343918634, + "timestamp": 11.078154358297061 + }, + { + "x": 7.791223380396529, + "y": 5.279496011851437, + "heading": 0.6816390937397763, + "angularVelocity": 0.2380895734892706, + "velocityX": 1.6623802349818717, + "velocityY": 1.7372778872782495, + "timestamp": 11.125094536097258 + }, + { + "x": 7.86221923742719, + "y": 5.354510990808827, + "heading": 0.6909640471575174, + "angularVelocity": 0.19865611624721707, + "velocityX": 1.5124752473852405, + "velocityY": 1.5980974609149294, + "timestamp": 11.172034713897455 + }, + { + "x": 7.926540429785996, + "y": 5.422621631484432, + "heading": 0.6985519881256509, + "angularVelocity": 0.1616513043566187, + "velocityX": 1.370280117655087, + "velocityY": 1.4510094308871155, + "timestamp": 11.218974891697652 }, { - "x": 2.474695965069045, - "y": 5.543934535965086, - "heading": -6.169486717931722e-20, - "angularVelocity": 1.457766188026957e-19, - "velocityX": 1.9898460032113345, - "velocityY": 0.002812280533374536, - "timestamp": 2.7512584437229424 + "x": 7.984478950500488, + "y": 5.483555793762207, + "heading": 0.7044936776046891, + "angularVelocity": 0.12658003777337848, + "velocityX": 1.2343055231087734, + "velocityY": 1.2981238063720744, + "timestamp": 11.265915069497849 + }, + { + "x": 8.047923980956542, + "y": 5.548807333254465, + "heading": 0.7094925329826927, + "angularVelocity": 0.08420982403970191, + "velocityX": 1.0687836408324876, + "velocityY": 1.099215769102154, + "timestamp": 11.325276972161923 + }, + { + "x": 8.10152245585906, + "y": 5.602265273787256, + "heading": 0.7122491464591354, + "angularVelocity": 0.04643741781732065, + "velocityX": 0.9029103262715175, + "velocityY": 0.9005429094027965, + "timestamp": 11.384638874825997 + }, + { + "x": 8.14525837183985, + "y": 5.64394060886005, + "heading": 0.7129744623769576, + "angularVelocity": 0.012218542285053226, + "velocityX": 0.7367674218309503, + "velocityY": 0.7020552442301676, + "timestamp": 11.444000777490071 + }, + { + "x": 8.179119065532058, + "y": 5.673842216437666, + "heading": 0.7118363633541982, + "angularVelocity": -0.01917221267653394, + "velocityX": 0.5704111925762211, + "velocityY": 0.5037171356657365, + "timestamp": 11.503362680154146 + }, + { + "x": 8.203094267741402, + "y": 5.691977414243475, + "heading": 0.7089715992476469, + "angularVelocity": -0.048259303997766526, + "velocityX": 0.40388197031045997, + "velocityY": 0.3055023001610055, + "timestamp": 11.56272458281822 + }, + { + "x": 8.217175483703613, + "y": 5.698352336883545, + "heading": 0.7044936776046891, + "angularVelocity": -0.07543426746777364, + "velocityX": 0.23720964676445846, + "velocityY": 0.10739080713341471, + "timestamp": 11.622086485482294 + }, + { + "x": 8.220424741398494, + "y": 5.690497924270521, + "heading": 0.6974771095930071, + "angularVelocity": -0.10438216452389025, + "velocityX": 0.04833767031445264, + "velocityY": -0.11684638248302778, + "timestamp": 11.689306475062756 + }, + { + "x": 8.210761211514034, + "y": 5.66775585785793, + "heading": 0.6884846443385353, + "angularVelocity": -0.1337766535013804, + "velocityX": -0.14375976468862095, + "velocityY": -0.338322968428447, + "timestamp": 11.756526464643217 + }, + { + "x": 8.187932028417118, + "y": 5.630349579704253, + "heading": 0.6774812110255055, + "angularVelocity": -0.1636928744218101, + "velocityX": -0.33961896214800763, + "velocityY": -0.556475512524468, + "timestamp": 11.823746454223679 + }, + { + "x": 8.15163887353547, + "y": 5.578552873386559, + "heading": 0.6644254264440203, + "angularVelocity": -0.19422473378782149, + "velocityX": -0.5399161039471123, + "velocityY": -0.7705551078030757, + "timestamp": 11.89096644380414 + }, + { + "x": 8.101525228747773, + "y": 5.512708245233119, + "heading": 0.6492678901971639, + "angularVelocity": -0.225491499499757, + "velocityX": -0.745516997257366, + "velocityY": -0.9795393983901896, + "timestamp": 11.958186433384602 + }, + { + "x": 8.037158859322696, + "y": 5.433255022389235, + "heading": 0.6319488484169726, + "angularVelocity": -0.2576471952507599, + "velocityX": -0.9575480422833206, + "velocityY": -1.1819880267725829, + "timestamp": 12.025406422965064 + }, + { + "x": 7.958007564987826, + "y": 5.340774148745389, + "heading": 0.6123949630340744, + "angularVelocity": -0.29089390678188665, + "velocityX": -1.177496379110975, + "velocityY": -1.3757942275957413, + "timestamp": 12.092626412545526 + }, + { + "x": 7.863405980160758, + "y": 5.236063150659359, + "heading": 0.5905148719771925, + "angularVelocity": -0.3254997686468183, + "velocityX": -1.4073430450897342, + "velocityY": -1.5577360059048004, + "timestamp": 12.159846402125988 + }, + { + "x": 7.752513083642703, + "y": 5.120268560189199, + "heading": 0.5661934059220105, + "angularVelocity": -0.36181895009176485, + "velocityX": -1.649701185765849, + "velocityY": -1.7226213689241165, + "timestamp": 12.22706639170645 + }, + { + "x": 7.624272333970784, + "y": 4.995132700896389, + "heading": 0.5392856965961144, + "angularVelocity": -0.40029326832441703, + "velocityX": -1.9077769941992746, + "velocityY": -1.8615870081774413, + "timestamp": 12.294286381286911 + }, + { + "x": 7.477443680694748, + "y": 4.863466852853807, + "heading": 0.5096191616794202, + "angularVelocity": -0.4413350121273609, + "velocityX": -2.1843004468229417, + "velocityY": -1.958730563101009, + "timestamp": 12.361506370867373 + }, + { + "x": 7.310999420897597, + "y": 4.729976120271212, + "heading": 0.47703933517045805, + "angularVelocity": -0.48467467359488975, + "velocityX": -2.4761125497932377, + "velocityY": -1.9858785075056882, + "timestamp": 12.428726360447834 + }, + { + "x": 7.12561513664023, + "y": 4.601908860623506, + "heading": 0.44160618415094194, + "angularVelocity": -0.5271222331432077, + "velocityX": -2.757874337892657, + "velocityY": -1.9051960651438506, + "timestamp": 12.495946350028296 + }, + { + "x": 6.9255695292750366, + "y": 4.487009075034074, + "heading": 0.4039304316786653, + "angularVelocity": -0.5604843545412803, + "velocityX": -2.9759839091575766, + "velocityY": -1.709309779822242, + "timestamp": 12.563166339608758 + }, + { + "x": 6.716882603619491, + "y": 4.389825418108365, + "heading": 0.36497586306620833, + "angularVelocity": -0.5795086975702185, + "velocityX": -3.1045367153136607, + "velocityY": -1.445755310767803, + "timestamp": 12.63038632918922 + }, + { + "x": 6.504347696212581, + "y": 4.311975808393504, + "heading": 0.32552791036504214, + "angularVelocity": -0.5868485393611587, + "velocityX": -3.1617813203096174, + "velocityY": -1.1581318325209748, + "timestamp": 12.697606318769681 + }, + { + "x": 6.29116656289608, + "y": 4.2538292064786, + "heading": 0.2861053737979932, + "angularVelocity": -0.5864704355519208, + "velocityX": -3.1713949175985174, + "velocityY": -0.8650195020530711, + "timestamp": 12.764826308350143 + }, + { + "x": 6.079464519610655, + "y": 4.215340449642915, + "heading": 0.24704786030835676, + "angularVelocity": -0.5810401598305069, + "velocityX": -3.1493911945942843, + "velocityY": -0.572579036026387, + "timestamp": 12.832046297930605 + }, + { + "x": 5.870707606275243, + "y": 4.1963435376359834, + "heading": 0.20858746560478297, + "angularVelocity": -0.5721571059980171, + "velocityX": -3.1055778889333454, + "velocityY": -0.28260807723263415, + "timestamp": 12.899266287511066 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.17089065340850093, + "angularVelocity": -0.560797650097213, + "velocityX": -3.0460563700229573, + "velocityY": 0.004545328262717531, + "timestamp": 12.966486277091528 + }, + { + "x": 5.441376331690843, + "y": 4.221257817184443, + "heading": 0.12955296713969652, + "angularVelocity": -0.5459093121032401, + "velocityX": -2.965763486744691, + "velocityY": 0.3249853335649047, + "timestamp": 13.042208902356046 + }, + { + "x": 5.2258290849692735, + "y": 4.2691932604024965, + "heading": 0.08990916431610245, + "angularVelocity": -0.5235397304980968, + "velocityX": -2.8465368965828737, + "velocityY": 0.6330399012263854, + "timestamp": 13.117931527620565 + }, + { + "x": 5.023235975053, + "y": 4.338521550335442, + "heading": 0.05269444989052449, + "angularVelocity": -0.4914609642174678, + "velocityX": -2.6754633665772425, + "velocityY": 0.9155558155936205, + "timestamp": 13.193654152885083 + }, + { + "x": 4.838427025225484, + "y": 4.425425000911894, + "heading": 0.018769229111462726, + "angularVelocity": -0.44801960656478235, + "velocityX": -2.440604101903904, + "velocityY": 1.1476550142427961, + "timestamp": 13.269376778149601 + }, + { + "x": 4.676043362291933, + "y": 4.523369450086871, + "heading": -0.011096289897494078, + "angularVelocity": -0.3944068091224933, + "velocityX": -2.144453686943697, + "velocityY": 1.2934634639624794, + "timestamp": 13.34509940341412 + }, + { + "x": 4.53850801944686, + "y": 4.62398511499902, + "heading": -0.03651765926491392, + "angularVelocity": -0.33571695749608116, + "velocityX": -1.8163044712809187, + "velocityY": 1.3287397863013088, + "timestamp": 13.420822028678637 + }, + { + "x": 4.425480168441604, + "y": 4.719703555950957, + "heading": -0.05754144603227514, + "angularVelocity": -0.2776420745308276, + "velocityX": -1.4926562650254422, + "velocityY": 1.2640665932747182, + "timestamp": 13.496544653943156 + }, + { + "x": 4.335238129667097, + "y": 4.805147915212772, + "heading": -0.07443003472477187, + "angularVelocity": -0.22303226589808134, + "velocityX": -1.1917447190884998, + "velocityY": 1.128386119252163, + "timestamp": 13.572267279207674 + }, + { + "x": 4.265861114202726, + "y": 4.876824239203163, + "heading": -0.08748354859792261, + "angularVelocity": -0.17238591276453477, + "velocityX": -0.9161992894728712, + "velocityY": 0.9465641707482745, + "timestamp": 13.647989904472192 + }, + { + "x": 4.215660401957236, + "y": 4.932459766827933, + "heading": -0.09697232555255134, + "angularVelocity": -0.1253096669784238, + "velocityX": -0.6629552537319688, + "velocityY": 0.7347279288115158, + "timestamp": 13.72371252973671 + }, + { + "x": 4.183247912596747, + "y": 4.970524954667524, + "heading": -0.10312305256683521, + "angularVelocity": -0.08122707041386726, + "velocityX": -0.4280423353953174, + "velocityY": 0.5026923948637503, + "timestamp": 13.799435155001229 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.03960437745903177, + "velocityX": -0.2079807341935533, + "velocityY": 0.25649983915041824, + "timestamp": 13.875157780265747 }, { - "x": 2.6165005638402494, - "y": 5.544134950626333, - "heading": -3.084743805748469e-20, - "angularVelocity": 3.2464552635862006e-19, - "velocityX": 1.492384614166397, - "velocityY": 0.0021092105579800877, - "timestamp": 2.8462772461316046 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -1.1491916765427864e-28, + "velocityX": -1.6722970053411976e-26, + "velocityY": -1.611730320150929e-26, + "timestamp": 13.950880405530265 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 2.711036966099053, - "y": 5.544268560404848, - "heading": -1.8891361886639493e-21, - "angularVelocity": 3.047639112965043e-19, - "velocityX": 0.9949231085045297, - "velocityY": 0.001406140417769105, - "timestamp": 2.941296048540267 + "timestamp": 1.2792606479375337, + "isStopPoint": false, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -8.137968740525874e-34, - "angularVelocity": 1.9881709101649526e-20, - "velocityX": 0.49746156397034486, - "velocityY": 0.0007030702226192673, - "timestamp": 3.036314850948929 + "timestamp": 2.493236471662288, + "isStopPoint": true, + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 }, { + "timestamp": 3.4242267541773885, + "isStopPoint": true, "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -8.137968740525874e-34, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -9.422453604168856e-36, - "timestamp": 3.1313336533575913 - }, - { - "x": 2.744670053824415, - "y": 5.560420833392183, - "heading": 0.008590361313936763, - "angularVelocity": 0.13502733079351698, - "velocityX": -0.21432312628245512, - "velocityY": 0.2528389368381776, - "timestamp": 3.1949530796133994 - }, - { - "x": 2.717653230778774, - "y": 5.592802243922747, - "heading": 0.025816975580253556, - "angularVelocity": 0.27077600789812356, - "velocityX": -0.4246631042695725, - "velocityY": 0.5089862080862002, - "timestamp": 3.2585725058692074 - }, - { - "x": 2.6776153836811907, - "y": 5.641767759246714, - "heading": 0.051750317714318324, - "angularVelocity": 0.4076324427980389, - "velocityX": -0.629333671394567, - "velocityY": 0.7696629505440931, - "timestamp": 3.3221919321250155 - }, - { - "x": 2.6251092372323575, - "y": 5.707734732807458, - "heading": 0.0864931852991208, - "angularVelocity": 0.5461046983527398, - "velocityX": -0.8253162522672026, - "velocityY": 1.0368998502988076, - "timestamp": 3.3858113583808236 - }, - { - "x": 2.5610825168108655, - "y": 5.791356317104455, - "heading": 0.1301885817245676, - "angularVelocity": 0.686824748304888, - "velocityX": -1.006402040849065, - "velocityY": 1.314403307580339, - "timestamp": 3.4494307846366317 - }, - { - "x": 2.48749773492398, - "y": 5.89376509224382, - "heading": 0.18299253621322714, - "angularVelocity": 0.8299973388055953, - "velocityX": -1.1566401367878931, - "velocityY": 1.6097091905165586, - "timestamp": 3.5130502108924397 - }, - { - "x": 2.410156329475762, - "y": 6.016926517894053, - "heading": 0.24447965431113647, - "angularVelocity": 0.9664833796311684, - "velocityX": -1.215688508997792, - "velocityY": 1.9359090909592849, - "timestamp": 3.576669637148248 - }, - { - "x": 2.3496693828620137, - "y": 6.152928215304285, - "heading": 0.3070071930810449, - "angularVelocity": 0.9828372000478389, - "velocityX": -0.9507622148388409, - "velocityY": 2.137738508728141, - "timestamp": 3.640289063404056 - }, - { - "x": 2.309701308910896, - "y": 6.283843034176789, - "heading": 0.36362933631377437, - "angularVelocity": 0.8900134214517557, - "velocityX": -0.6282369443322133, - "velocityY": 2.0577805644789557, - "timestamp": 3.703908489659864 - }, - { - "x": 2.288494901023514, - "y": 6.40507103844379, - "heading": 0.41290495871744193, - "angularVelocity": 0.7745373591634508, - "velocityX": -0.3333322718459105, - "velocityY": 1.905518666256975, - "timestamp": 3.767527915915672 - }, - { - "x": 2.285060268530139, - "y": 6.514915678699493, - "heading": 0.45431002431284384, - "angularVelocity": 0.6508242534114624, - "velocityX": -0.05398716548566081, - "velocityY": 1.7265896082436825, - "timestamp": 3.83114734217148 - }, - { - "x": 2.2988127872129502, - "y": 6.612520892931977, - "heading": 0.48757772897770557, - "angularVelocity": 0.522917395248037, - "velocityX": 0.2161685430408261, - "velocityY": 1.5342045657567287, - "timestamp": 3.894766768427288 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { + "timestamp": 4.33090231449298, + "isStopPoint": false, "x": 2.3293724060058594, "y": 6.6973748207092285, "heading": 0.5125504196, - "angularVelocity": 0.3925324714794108, - "velocityX": 0.48035043054352317, - "velocityY": 1.3337738607711032, - "timestamp": 3.9583861946830963 - }, - { - "x": 2.3906193814869496, - "y": 6.780920533993851, - "heading": 0.5304788719493444, - "angularVelocity": 0.23275934723222522, - "velocityX": 0.7951498408867412, - "velocityY": 1.0846472026287595, - "timestamp": 4.035411897678633 - }, - { - "x": 2.471491792404757, - "y": 6.8405967325057775, - "heading": 0.5351323155604377, - "angularVelocity": 0.06041416605263601, - "velocityX": 1.0499405753231936, - "velocityY": 0.7747569472411664, - "timestamp": 4.112437600674169 - }, - { - "x": 2.556148280048404, - "y": 6.869576729136414, - "heading": 0.5277899567279947, - "angularVelocity": -0.09532349004160981, - "velocityX": 1.0990680299088356, - "velocityY": 0.37623800242778493, - "timestamp": 4.189463303669705 - }, - { - "x": 2.6173499244947225, - "y": 6.878199709364964, - "heading": 0.518346888239368, - "angularVelocity": -0.12259632981439711, - "velocityX": 0.794561322599876, - "velocityY": 0.11194938693450328, - "timestamp": 4.266489006665242 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.0752536934288536, - "velocityX": 0.4023928513995403, - "velocityY": 0.0187620669061233, - "timestamp": 4.343514709660778 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { + "timestamp": 4.836905583331663, + "isStopPoint": true, "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -4.4995568742972564e-34, - "velocityX": 2.8230892463303505e-33, - "velocityY": 2.0807279166194578e-33, - "timestamp": 4.420540412656314 - }, - { - "x": 2.658706958910465, - "y": 6.86593478207681, - "heading": 0.5043163629464208, - "angularVelocity": -0.14325761626715308, - "velocityX": 0.1802876546124509, - "velocityY": -0.23853061812890558, - "timestamp": 4.4780176824038715 - }, - { - "x": 2.679434758469343, - "y": 6.838509950906704, - "heading": 0.48805940847309726, - "angularVelocity": -0.2828414527120879, - "velocityX": 0.36062602920974224, - "velocityY": -0.4771422040496659, - "timestamp": 4.535494952151429 - }, - { - "x": 2.7105313297582017, - "y": 6.797364836772264, - "heading": 0.464037646160856, - "angularVelocity": -0.4179349926979131, - "velocityX": 0.5410238068968174, - "velocityY": -0.7158501841710775, - "timestamp": 4.592972221898986 - }, - { - "x": 2.7520007950829926, - "y": 6.742492873007072, - "heading": 0.4325704761896421, - "angularVelocity": -0.5474715502218408, - "velocityX": 0.721493305213118, - "velocityY": -0.954672412350015, - "timestamp": 4.650449491646543 - }, - { - "x": 2.803848241216662, - "y": 6.673886273182379, - "heading": 0.39405966194845793, - "angularVelocity": -0.6700181551824853, - "velocityX": 0.9020513041309295, - "velocityY": -1.1936301102334148, - "timestamp": 4.7079267613941 - }, - { - "x": 2.866080023237396, - "y": 6.591535755483362, - "heading": 0.3490218715149645, - "angularVelocity": -0.7835756748937736, - "velocityX": 1.0827198698556684, - "velocityY": -1.4327492948204654, - "timestamp": 4.7654040311416574 - }, - { - "x": 2.938704098816793, - "y": 6.495430149531881, - "heading": 0.29814315673038183, - "angularVelocity": -0.8851971398092591, - "velocityX": 1.2635268845991694, - "velocityY": -1.6720628236793682, - "timestamp": 4.822881300889215 - }, - { - "x": 3.0217302964606527, - "y": 6.385555893825644, - "heading": 0.2423799369377359, - "angularVelocity": -0.9701786469253116, - "velocityX": 1.4445048974753851, - "velocityY": -1.9116122980233827, - "timestamp": 4.880358570636772 - }, - { - "x": 3.1151700095083186, - "y": 6.261896797084101, - "heading": 0.1831716018579807, - "angularVelocity": -1.0301173897055516, - "velocityX": 1.6256811337430896, - "velocityY": -2.151443471213232, - "timestamp": 4.937835840384329 - }, - { - "x": 3.2190322842248182, - "y": 6.124436959606287, - "heading": 0.12297302537717429, - "angularVelocity": -1.0473457898261622, - "velocityX": 1.8070147585761873, - "velocityY": -2.39155127029422, - "timestamp": 4.995313110131886 - }, - { - "x": 3.3332908123673026, - "y": 5.973192452697109, - "heading": 0.06701518728601086, - "angularVelocity": -0.9735646515036827, - "velocityX": 1.9878906678120518, - "velocityY": -2.6313794578874745, - "timestamp": 5.052790379879443 - }, - { - "x": 3.457219648102922, - "y": 5.80867115561654, - "heading": 0.033419490428721374, - "angularVelocity": -0.5845040483802287, - "velocityX": 2.1561364393249893, - "velocityY": -2.8623714696810656, - "timestamp": 5.1102676496270005 - }, - { - "x": 3.588371868131788, - "y": 5.635317112607293, - "heading": 0.03341946780631197, - "angularVelocity": -3.935887960456646e-7, - "velocityX": 2.2818101939234974, - "velocityY": -3.0160451909185437, - "timestamp": 5.167744919374558 - }, - { - "x": 3.719524526285539, - "y": 5.461963401064903, - "heading": 0.033419445184933325, - "angularVelocity": -3.9357086276096175e-7, - "velocityX": 2.281817816499982, - "velocityY": -3.0160394239978356, - "timestamp": 5.225222189122115 - }, - { - "x": 3.8561567314585825, - "y": 5.292894866069507, - "heading": 0.0334194225694706, - "angularVelocity": -3.9346793646231113e-7, - "velocityX": 2.3771519728257604, - "velocityY": -2.9414851425259596, - "timestamp": 5.282699458869672 - }, - { - "x": 4.00579710900262, - "y": 5.135223415824853, - "heading": 0.03341939963642931, - "angularVelocity": -3.9899322628139097e-7, - "velocityX": 2.6034705232392743, - "velocityY": -2.743196587749476, - "timestamp": 5.340176728617229 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 }, { + "timestamp": 5.885339752974947, + "isStopPoint": false, "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.03341937596859772, - "angularVelocity": -4.117772415638071e-7, - "velocityX": 2.813320067337111, - "velocityY": -2.527531676347718, - "timestamp": 5.397653998364786 - }, - { - "x": 4.277933740289957, - "y": 4.900652577427371, - "heading": 0.03341935253431191, - "angularVelocity": -6.240523322284166e-7, - "velocityX": 2.940862673655656, - "velocityY": -2.37792138300258, - "timestamp": 5.435205794264698 - }, - { - "x": 4.3928594316694705, - "y": 4.81721673895127, - "heading": 0.033419330076064284, - "angularVelocity": -5.980605477839765e-7, - "velocityX": 3.060457925523134, - "velocityY": -2.2218867693700792, - "timestamp": 5.47275759016461 - }, - { - "x": 4.5119655190903245, - "y": 4.739865631807423, - "heading": 0.03341930829497473, - "angularVelocity": -5.800279064172658e-7, - "velocityX": 3.1717813906506853, - "velocityY": -2.0598510747665353, - "timestamp": 5.510309386064522 - }, - { - "x": 4.634929904279705, - "y": 4.6688078109694935, - "heading": 0.033419286924895195, - "angularVelocity": -5.690827567994812e-7, - "velocityX": 3.2745274158690463, - "velocityY": -1.8922615852334157, - "timestamp": 5.547861181964434 - }, - { - "x": 4.761195179582567, - "y": 4.6037960104372555, - "heading": 0.03341926571852252, - "angularVelocity": -5.647232617468814e-7, - "velocityX": 3.362429739429795, - "velocityY": -1.7312567608088656, - "timestamp": 5.585412977864346 - }, - { - "x": 4.8874652266060234, - "y": 4.538793478329322, - "heading": 0.03341924451230877, - "angularVelocity": -5.647190300189245e-7, - "velocityX": 3.3625568097996403, - "velocityY": -1.7310099437370914, - "timestamp": 5.622964773764258 - }, - { - "x": 5.0137352745443575, - "y": 4.473790947998577, - "heading": 0.03341922330609504, - "angularVelocity": -5.647190293196981e-7, - "velocityX": 3.3625568341627456, - "velocityY": -1.731009896410775, - "timestamp": 5.6605165696641695 - }, - { - "x": 5.140005505333594, - "y": 4.408788772864344, - "heading": 0.03341920209988365, - "angularVelocity": -5.64718967252867e-7, - "velocityX": 3.362561703461214, - "velocityY": -1.7310004375685482, - "timestamp": 5.698068365564081 - }, - { - "x": 5.267176463182282, - "y": 4.34556692987905, - "heading": 0.033419180889344834, - "angularVelocity": -5.648342058496427e-7, - "velocityX": 3.3865479613183083, - "velocityY": -1.6835903974819586, - "timestamp": 5.735620161463993 - }, - { - "x": 5.39745989861742, - "y": 4.289036580908955, - "heading": 0.03341915955151481, - "angularVelocity": -5.682239561025287e-7, - "velocityX": 3.4694328809835504, - "velocityY": -1.5053966825119904, - "timestamp": 5.773171957363905 - }, - { - "x": 5.530504784906373, - "y": 4.239352458442723, - "heading": 0.03341913783750815, - "angularVelocity": -5.782414966353427e-7, - "velocityX": 3.542969999186219, - "velocityY": -1.323082459189313, - "timestamp": 5.810723753263817 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { + "timestamp": 6.336444988090829, + "isStopPoint": false, "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.033419115463141504, - "angularVelocity": -5.958268068735077e-7, - "velocityX": 3.6069365171092738, - "velocityY": -1.1371861948253605, - "timestamp": 5.848275549163729 - }, - { - "x": 5.808220957669321, - "y": 4.160073254115033, - "heading": 0.03341909344161441, - "angularVelocity": -5.669632847998976e-7, - "velocityX": 3.6628444951256323, - "velocityY": -0.941676170142482, - "timestamp": 5.887116736398908 - }, - { - "x": 5.952250404538064, - "y": 4.131196918833519, - "heading": 0.033419072126499465, - "angularVelocity": -5.487760919877281e-7, - "velocityX": 3.708162832321777, - "velocityY": -0.7434462573626127, - "timestamp": 5.925957923634087 - }, - { - "x": 6.09762364792411, - "y": 4.110103210641541, - "heading": 0.03341905123842176, - "angularVelocity": -5.377816485869475e-7, - "velocityX": 3.7427600373239653, - "velocityY": -0.5430757835556856, - "timestamp": 5.964799110869266 - }, - { - "x": 6.24392007770515, - "y": 4.096848811559285, - "heading": 0.0334190305197359, - "angularVelocity": -5.334205087871439e-7, - "velocityX": 3.7665282704987906, - "velocityY": -0.3412459820551187, - "timestamp": 6.003640298104445 - }, - { - "x": 6.390314919287249, - "y": 4.0847296192319185, - "heading": 0.03341900981240726, - "angularVelocity": -5.331281075500129e-7, - "velocityX": 3.7690619675370143, - "velocityY": -0.31201910111516795, - "timestamp": 6.042481485339624 - }, - { - "x": 6.536709800200056, - "y": 4.07261090201266, - "heading": 0.033418989105078956, - "angularVelocity": -5.331280988936517e-7, - "velocityX": 3.769062980140168, - "velocityY": -0.31200686904551156, - "timestamp": 6.081322672574803 - }, - { - "x": 6.68323027882942, - "y": 4.062119297351164, - "heading": 0.03341896797261103, - "angularVelocity": -5.440736864773728e-7, - "velocityX": 3.772296602114613, - "velocityY": -0.27011544724343456, - "timestamp": 6.120163859809982 - }, - { - "x": 6.828478445747191, - "y": 4.059283961769526, - "heading": 0.02725441210059553, - "angularVelocity": -0.15871182913873783, - "velocityX": 3.739539835337918, - "velocityY": -0.07299816981572388, - "timestamp": 6.159005047045161 - }, - { - "x": 6.965940131033012, - "y": 4.0573223410737915, - "heading": 0.017080975304275284, - "angularVelocity": -0.2619239400361572, - "velocityX": 3.5390701229986914, - "velocityY": -0.05050362348240824, - "timestamp": 6.19784623428034 - }, - { - "x": 7.095541107200079, - "y": 4.056112825219225, - "heading": 0.007563330070755733, - "angularVelocity": -0.2450400183673897, - "velocityX": 3.336689359734222, - "velocityY": -0.03114003305929124, - "timestamp": 6.236687421515519 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { + "timestamp": 6.780559247214761, + "isStopPoint": false, "x": 7.217291355133057, "y": 4.0556182861328125, "heading": 0, - "angularVelocity": -0.194724482157576, - "velocityX": 3.1345655629883886, - "velocityY": -0.012732337027157752, - "timestamp": 6.275528608750698 - }, - { - "x": 7.3857970557158446, - "y": 4.056503208949951, - "heading": -0.005539516561442828, - "angularVelocity": -0.09285314353970942, - "velocityX": 2.8244854636552765, - "velocityY": 0.014833039029661928, - "timestamp": 6.335187510434235 - }, - { - "x": 7.5357095386456, - "y": 4.058265721996549, - "heading": -0.007640794437617486, - "angularVelocity": -0.03522153135371121, - "velocityX": 2.5128267316246156, - "velocityY": 0.029543169533142722, - "timestamp": 6.394846412117771 - }, - { - "x": 7.667000937583321, - "y": 4.060490426598279, - "heading": -0.007708640614146542, - "angularVelocity": -0.0011372347564986972, - "velocityX": 2.200700905192029, - "velocityY": 0.037290404934565556, - "timestamp": 6.454505313801308 - }, - { - "x": 7.779661923967275, - "y": 4.062917456014214, - "heading": -0.0066168114481254744, - "angularVelocity": 0.018301194544491067, - "velocityX": 1.888418713800162, - "velocityY": 0.0406817649578728, - "timestamp": 6.514164215484844 - }, - { - "x": 7.873689685619936, - "y": 4.06536905162046, - "heading": -0.004961765552273349, - "angularVelocity": 0.027741809673791765, - "velocityX": 1.5760893848068933, - "velocityY": 0.04109354240631638, - "timestamp": 6.573823117168381 - }, - { - "x": 7.949083994059634, - "y": 4.067716008149933, - "heading": -0.003176764942269326, - "angularVelocity": 0.029920105124841948, - "velocityX": 1.263756226013528, - "velocityY": 0.03933958660390468, - "timestamp": 6.633482018851917 - }, - { - "x": 8.005845696502893, - "y": 4.069860191657625, - "heading": -0.0015908607397525072, - "angularVelocity": 0.02658285951909277, - "velocityX": 0.951437268227904, - "velocityY": 0.035940713744034375, - "timestamp": 6.693140920535454 - }, - { - "x": 8.043976076014772, - "y": 4.071724543898811, - "heading": -0.00046248549538235704, - "angularVelocity": 0.018913778372181077, - "velocityX": 0.6391398171247351, - "velocityY": 0.03125019382816773, - "timestamp": 6.75279982221899 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { + "timestamp": 7.37180261327287, + "isStopPoint": false, "x": 8.0634765625, "y": 4.073246955871582, - "heading": 3.4463826664070866e-33, - "angularVelocity": 0.00775216241552071, - "velocityX": 0.32686633402451676, - "velocityY": 0.02551860543538527, - "timestamp": 6.812458723902527 - }, - { - "x": 8.059339450711764, - "y": 4.074523581107775, - "heading": -0.0006944740465081056, - "angularVelocity": -0.009481698382290156, - "velocityX": -0.0564842507032717, - "velocityY": 0.017429845647464255, - "timestamp": 6.88570235795497 - }, - { - "x": 8.027124349390371, - "y": 4.075207755344852, - "heading": -0.002651211191279339, - "angularVelocity": -0.026715456845985846, - "velocityX": -0.43983482985464195, - "velocityY": 0.009341074428224473, - "timestamp": 6.958945992007413 - }, - { - "x": 7.96683125907587, - "y": 4.075299477238389, - "heading": -0.0058702138612145086, - "angularVelocity": -0.043949248444312826, - "velocityX": -0.8231854016327222, - "velocityY": 0.0012522848534625944, - "timestamp": 7.032189626059856 - }, - { - "x": 7.878460180490193, - "y": 4.074798744928973, - "heading": -0.0103514975430217, - "angularVelocity": -0.06118325148365151, - "velocityX": -1.2065359635542152, - "velocityY": -0.006836530107951673, - "timestamp": 7.105433260112299 - }, - { - "x": 7.7620111146201785, - "y": 4.073705556029158, - "heading": -0.01609509527793809, - "angularVelocity": -0.07841770563710548, - "velocityX": -1.5898865120023296, - "velocityY": -0.01492537766534737, - "timestamp": 7.1786768941647425 - }, - { - "x": 7.617484062863899, - "y": 4.072019907599753, - "heading": -0.023101063438175296, - "angularVelocity": -0.0956529294439546, - "velocityX": -1.9732370413624691, - "velocityY": -0.02301426535169652, - "timestamp": 7.251920528217186 - }, - { - "x": 7.444879027316714, - "y": 4.06974179610174, - "heading": -0.031369488604002356, - "angularVelocity": -0.1128893353367305, - "velocityX": -2.356587542114559, - "velocityY": -0.031103201356480925, - "timestamp": 7.325164162269629 - }, - { - "x": 7.244196011426359, - "y": 4.066871217282435, - "heading": -0.040900495023888005, - "angularVelocity": -0.13012743760176224, - "velocityX": -2.7399379957944494, - "velocityY": -0.0391921954234215, - "timestamp": 7.398407796322072 - }, - { - "x": 7.015435021934675, - "y": 4.063408165830539, - "heading": -0.05169425070794916, - "angularVelocity": -0.14736783371961967, - "velocityX": -3.123288357427606, - "velocityY": -0.047281262005875575, - "timestamp": 7.471651430374515 - }, - { - "x": 6.758596078532163, - "y": 4.059352633648335, - "heading": -0.06375095974291681, - "angularVelocity": -0.1646110162466115, - "velocityX": -3.5066384502250503, - "velocityY": -0.05537043914696755, - "timestamp": 7.544895064426958 - }, - { - "x": 6.481653456514826, - "y": 4.053513800761945, - "heading": -0.06375096147352038, - "angularVelocity": -2.3628040945559003e-8, - "velocityX": -3.7811152545904956, - "velocityY": -0.07971795722490971, - "timestamp": 7.6181386984794015 - }, - { - "x": 6.205372409181029, - "y": 4.073516064315128, - "heading": -0.06375096323470678, - "angularVelocity": -2.4045589953178915e-8, - "velocityX": -3.7720827333058655, - "velocityY": 0.2730921780705308, - "timestamp": 7.691382332531845 - }, - { - "x": 5.9325368870621995, - "y": 4.121391787593611, - "heading": -0.0637509651063581, - "angularVelocity": -2.555377459766275e-8, - "velocityX": -3.725040758128897, - "velocityY": 0.6536502987304462, - "timestamp": 7.764625966584288 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { + "timestamp": 8.476353730016669, + "isStopPoint": false, "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -0.063750967187294, - "angularVelocity": -2.84111505129003e-8, - "velocityX": -3.639704142076304, - "velocityY": 1.0274925313911556, - "timestamp": 7.837869600636731 - }, - { - "x": 5.417504232216337, - "y": 4.293821782165803, - "heading": -0.06375097411514968, - "angularVelocity": -9.821339780633191e-8, - "velocityX": -3.5221393138546646, - "velocityY": 1.3775780331429743, - "timestamp": 7.90840840517157 - }, - { - "x": 5.186014168311204, - "y": 4.410724788288498, - "heading": -0.06922328693791815, - "angularVelocity": -0.07757875766190163, - "velocityX": -3.281740673543775, - "velocityY": 1.6572864665569722, - "timestamp": 7.9789472097064085 - }, - { - "x": 4.979961148964822, - "y": 4.522014233902487, - "heading": -0.07551597396386121, - "angularVelocity": -0.08920886974821321, - "velocityX": -2.9211300177991313, - "velocityY": 1.5777052977843462, - "timestamp": 8.049486014241246 - }, - { - "x": 4.7980291404215, - "y": 4.623466282585381, - "heading": -0.08170492719079357, - "angularVelocity": -0.08773827778546625, - "velocityX": -2.579176238427287, - "velocityY": 1.4382445145180842, - "timestamp": 8.120024818776084 - }, - { - "x": 4.639563192627529, - "y": 4.713607705102715, - "heading": -0.0874491132160183, - "angularVelocity": -0.08143299369906003, - "velocityX": -2.246507420120879, - "velocityY": 1.27789835838249, - "timestamp": 8.190563623310922 - }, - { - "x": 4.504192007690659, - "y": 4.791698947450768, - "heading": -0.09257307344830198, - "angularVelocity": -0.07264030438385177, - "velocityX": -1.919102341322099, - "velocityY": 1.1070678453231335, - "timestamp": 8.26110242784576 - }, - { - "x": 4.391678654940531, - "y": 4.857296763925047, - "heading": -0.09697023729098278, - "angularVelocity": -0.0623368069770602, - "velocityX": -1.5950561324662005, - "velocityY": 0.9299536178257779, - "timestamp": 8.331641232380598 - }, - { - "x": 4.301859289367306, - "y": 4.910106206970951, - "heading": -0.10056909340764136, - "angularVelocity": -0.051019522380495494, - "velocityX": -1.2733326878096392, - "velocityY": 0.7486580385668955, - "timestamp": 8.402180036915436 - }, - { - "x": 4.234614001747884, - "y": 4.949916980366573, - "heading": -0.10331834169263278, - "angularVelocity": -0.03897497700905296, - "velocityX": -0.9533091475375062, - "velocityY": 0.5643811751297677, - "timestamp": 8.472718841450273 - }, - { - "x": 4.18985129168719, - "y": 4.976571613170809, - "heading": -0.10517936365869324, - "angularVelocity": -0.026382953019019773, - "velocityX": -0.6345827712261082, - "velocityY": 0.37787191007854753, - "timestamp": 8.543257645985111 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.013363372792080578, - "velocityX": -0.31687843925651377, - "velocityY": 0.18962871266842563, - "timestamp": 8.61379645051995 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { + "timestamp": 9.40030847009841, + "isStopPoint": true, "x": 4.16749906539917, "y": 4.98994779586792, "heading": -0.106122, - "angularVelocity": 0, - "velocityX": 8.237561081237056e-34, - "velocityY": 0, - "timestamp": 8.684335255054787 - }, - { - "x": 4.18294196758914, - "y": 4.969364336221789, - "heading": -0.10045524830547124, - "angularVelocity": 0.08077125535022475, - "velocityX": 0.22011597884885625, - "velocityY": -0.29338710511594707, - "timestamp": 8.754493279341492 - }, - { - "x": 4.2148538086473515, - "y": 4.929011600999615, - "heading": -0.08896968745487453, - "angularVelocity": 0.16370986736542367, - "velocityX": 0.4548566095276789, - "velocityY": -0.5751692074062589, - "timestamp": 8.824651303628197 - }, - { - "x": 4.264519080629496, - "y": 4.870038214957966, - "heading": -0.07148588735689271, - "angularVelocity": 0.24920599283886813, - "velocityX": 0.7079057953397331, - "velocityY": -0.8405793441481644, - "timestamp": 8.894809327914903 - }, - { - "x": 4.333555645892384, - "y": 4.794138441539978, - "heading": -0.04780159185128592, - "angularVelocity": 0.3375849839901328, - "velocityX": 0.9840152422303832, - "velocityY": -1.081840234095224, - "timestamp": 8.964967352201608 - }, - { - "x": 4.423967613250648, - "y": 4.703932489290492, - "heading": -0.017716503350327845, - "angularVelocity": 0.4288189242332906, - "velocityX": 1.2886903284047762, - "velocityY": -1.2857538844146224, - "timestamp": 9.035125376488313 - }, - { - "x": 4.53803034325044, - "y": 4.603622363293204, - "heading": 0.0188848190363467, - "angularVelocity": 0.5216983054867799, - "velocityX": 1.6257973504736565, - "velocityY": -1.429774099500926, - "timestamp": 9.105283400775019 - }, - { - "x": 4.677578748555051, - "y": 4.49984021599331, - "heading": 0.06181064388576721, - "angularVelocity": 0.6118448357952799, - "velocityX": 1.9890583682108496, - "velocityY": -1.4792626838489913, - "timestamp": 9.175441425061724 - }, - { - "x": 4.842258553230926, - "y": 4.40159118744757, - "heading": 0.11019595374155315, - "angularVelocity": 0.6896618077221814, - "velocityX": 2.3472697007957355, - "velocityY": -1.4003961705682948, - "timestamp": 9.245599449348429 - }, - { - "x": 5.028363428376419, - "y": 4.3176283558190685, - "heading": 0.16247927063581383, - "angularVelocity": 0.7452221955481739, - "velocityX": 2.6526527369836272, - "velocityY": -1.1967673332045712, - "timestamp": 9.315757473635134 - }, - { - "x": 5.230618576677741, - "y": 4.253738965485636, - "heading": 0.2169576127744663, - "angularVelocity": 0.7765090692409344, - "velocityX": 2.8828512541173206, - "velocityY": -0.9106497935623799, - "timestamp": 9.38591549792184 - }, - { - "x": 5.444331250681613, - "y": 4.2129309832647035, - "heading": 0.2722364606827335, - "angularVelocity": 0.7879191079037022, - "velocityX": 3.0461615214607765, - "velocityY": -0.5816580873795356, - "timestamp": 9.456073522208545 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 }, { + "timestamp": 10.327111513493909, + "isStopPoint": false, "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.3273042274639755, - "angularVelocity": 0.7849104552346565, - "velocityX": 3.1588757008538453, - "velocityY": -0.23207478938864265, - "timestamp": 9.52623154649525 - }, - { - "x": 5.81887557997068, - "y": 4.1972183908996925, - "heading": 0.36426543181084436, - "angularVelocity": 0.7759151841130933, - "velocityX": 3.210283328613804, - "velocityY": 0.011951482768166844, - "timestamp": 9.573867170985725 - }, - { - "x": 5.973953798857342, - "y": 4.209469324927334, - "heading": 0.4004931510026103, - "angularVelocity": 0.7605173560600648, - "velocityX": 3.255509307276357, - "velocityY": 0.257180086514691, - "timestamp": 9.6215027954762 - }, - { - "x": 6.130819634438986, - "y": 4.233461866975936, - "heading": 0.4356102472173979, - "angularVelocity": 0.7372023898166727, - "velocityX": 3.2930361942250057, - "velocityY": 0.5036680489703445, - "timestamp": 9.669138419966675 - }, - { - "x": 6.289005438456121, - "y": 4.269254559307002, - "heading": 0.4691390733673604, - "angularVelocity": 0.7038603253047925, - "velocityX": 3.32074588523062, - "velocityY": 0.7513849710991581, - "timestamp": 9.71677404445715 - }, - { - "x": 6.447898093638549, - "y": 4.316894151635714, - "heading": 0.5004573197150548, - "angularVelocity": 0.6574543040567628, - "velocityX": 3.335584594135005, - "velocityY": 1.0000832956066072, - "timestamp": 9.764409668947625 - }, - { - "x": 6.606667388474461, - "y": 4.376390356038531, - "heading": 0.5287266543286071, - "angularVelocity": 0.5934494386486953, - "velocityX": 3.33299492835787, - "velocityY": 1.248985502745193, - "timestamp": 9.8120452934381 - }, - { - "x": 6.764149093497097, - "y": 4.447653029340476, - "heading": 0.5527726659639904, - "angularVelocity": 0.5047905195447082, - "velocityX": 3.3059649518004255, - "velocityY": 1.4959953619626436, - "timestamp": 9.859680917928575 - }, - { - "x": 6.918662738022944, - "y": 4.530331648811429, - "heading": 0.5708777234010636, - "angularVelocity": 0.3800738970199359, - "velocityX": 3.243657371527556, - "velocityY": 1.735646805417357, - "timestamp": 9.90731654241905 - }, - { - "x": 7.06846751913844, - "y": 4.623866769358016, - "heading": 0.5845734852773292, - "angularVelocity": 0.2875109127414552, - "velocityX": 3.144805651607445, - "velocityY": 1.9635539902555645, - "timestamp": 9.954952166909525 - }, - { - "x": 7.211529043645413, - "y": 4.727181705822193, - "heading": 0.5988053175713824, - "angularVelocity": 0.2987644739893997, - "velocityX": 3.0032465415789793, - "velocityY": 2.168858655035301, - "timestamp": 10.0025877914 - }, - { - "x": 7.344594975677938, - "y": 4.83690117801315, - "heading": 0.6149173466892092, - "angularVelocity": 0.3382348670803817, - "velocityX": 2.7934121459693166, - "velocityY": 2.303307101870708, - "timestamp": 10.050223415890475 - }, - { - "x": 7.465817541114002, - "y": 4.947544737637474, - "heading": 0.6314993734499258, - "angularVelocity": 0.34810138290582165, - "velocityX": 2.5447879970651583, - "velocityY": 2.322706184873219, - "timestamp": 10.09785904038095 - }, - { - "x": 7.575645879536293, - "y": 5.054829291458012, - "heading": 0.6474951019450449, - "angularVelocity": 0.33579340391176205, - "velocityX": 2.305592497149964, - "velocityY": 2.252191610125547, - "timestamp": 10.145494664871425 - }, - { - "x": 7.675112095346534, - "y": 5.156311134408354, - "heading": 0.6623068665871409, - "angularVelocity": 0.31093881523601674, - "velocityX": 2.0880636472002183, - "velocityY": 2.1303770872287977, - "timestamp": 10.1931302893619 - }, - { - "x": 7.76511711692712, - "y": 5.250618542879285, - "heading": 0.6755917653702976, - "angularVelocity": 0.27888579031462285, - "velocityX": 1.8894477094255981, - "velocityY": 1.979766392897571, - "timestamp": 10.240765913852375 - }, - { - "x": 7.846351174360072, - "y": 5.336925095447112, - "heading": 0.6871379054751507, - "angularVelocity": 0.2423845646688635, - "velocityX": 1.7053215592711606, - "velocityY": 1.8118068880378715, - "timestamp": 10.28840153834285 - }, - { - "x": 7.919337423332426, - "y": 5.414694341101313, - "heading": 0.6968047845571967, - "angularVelocity": 0.2029338165594727, - "velocityX": 1.5321778553978798, - "velocityY": 1.6325858322640792, - "timestamp": 10.336037162833325 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 20 }, { + "timestamp": 11.265915069497849, + "isStopPoint": false, "x": 7.984478950500488, "y": 5.483555793762207, "heading": 0.7044936776046891, - "angularVelocity": 0.16141056467161, - "velocityX": 1.3674960256076552, - "velocityY": 1.445587276276031, - "timestamp": 10.3836727873238 - }, - { - "x": 8.05911422318913, - "y": 5.560200385763573, - "heading": 0.7111561296070518, - "angularVelocity": 0.10289890039880417, - "velocityX": 1.1527118676281318, - "velocityY": 1.1837449989371336, - "timestamp": 10.44842034027666 - }, - { - "x": 8.119756898561732, - "y": 5.61995838055026, - "heading": 0.7143078455831314, - "angularVelocity": 0.04867698982190534, - "velocityX": 0.9366018112955389, - "velocityY": 0.9229382742880431, - "timestamp": 10.513167893229522 - }, - { - "x": 8.166344572702991, - "y": 5.662879129934428, - "heading": 0.7141510891264902, - "angularVelocity": -0.002421040633850211, - "velocityX": 0.7195279515069205, - "velocityY": 0.6628937685941038, - "timestamp": 10.577915446182383 - }, - { - "x": 8.198829842984775, - "y": 5.689000472186179, - "heading": 0.7108395294312745, - "angularVelocity": -0.0511457119873956, - "velocityX": 0.5017219771291601, - "velocityY": 0.4034336598136588, - "timestamp": 10.642662999135243 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { + "timestamp": 11.622086485482294, + "isStopPoint": false, "x": 8.217175483703613, "y": 5.698352336883545, "heading": 0.7044936776046891, - "angularVelocity": -0.09800913759946452, - "velocityX": 0.28334106668393205, - "velocityY": 0.1444358013680284, - "timestamp": 10.707410552088104 - }, - { - "x": 8.221318658765867, - "y": 5.6908798741594095, - "heading": 0.6951703689604403, - "angularVelocity": -0.14352542875947455, - "velocityX": 0.06378111032529457, - "velocityY": -0.11503302714668015, - "timestamp": 10.77236983299625 - }, - { - "x": 8.210964366460544, - "y": 5.666753242665305, - "heading": 0.6829793287804198, - "angularVelocity": -0.18767203099521718, - "velocityX": -0.15939665834608732, - "velocityY": -0.3714116159663335, - "timestamp": 10.837329113904394 - }, - { - "x": 8.185835060378174, - "y": 5.6262170290983935, - "heading": 0.6680275277365855, - "angularVelocity": -0.2301718989927961, - "velocityX": -0.3868470483517696, - "velocityY": -0.6240249738021217, - "timestamp": 10.902288394812539 - }, - { - "x": 8.14559839699319, - "y": 5.569575312921383, - "heading": 0.6504460796726778, - "angularVelocity": -0.2706533665107588, - "velocityX": -0.6194136206938888, - "velocityY": -0.8719572536078877, - "timestamp": 10.967247675720683 - }, - { - "x": 8.08985004858283, - "y": 5.497215477528086, - "heading": 0.6303993620580922, - "angularVelocity": -0.30860436467780866, - "velocityX": -0.858204518753663, - "velocityY": -1.11392605308573, - "timestamp": 11.032206956628828 - }, - { - "x": 8.018089118716903, - "y": 5.40964606854142, - "heading": 0.6080990792438148, - "angularVelocity": -0.3432963312172538, - "velocityX": -1.1047063462324833, - "velocityY": -1.348066169489957, - "timestamp": 11.097166237536973 - }, - { - "x": 7.9296824410081745, - "y": 5.307560214485479, - "heading": 0.5838269457597958, - "angularVelocity": -0.3736515112958315, - "velocityX": -1.3609553011176063, - "velocityY": -1.5715360858180332, - "timestamp": 11.162125518445118 - }, - { - "x": 7.823812920396748, - "y": 5.191948689740781, - "heading": 0.5579732566194568, - "angularVelocity": -0.3979983888198644, - "velocityX": -1.6297828290483853, - "velocityY": -1.779753764641825, - "timestamp": 11.227084799353262 - }, - { - "x": 7.699409898040676, - "y": 5.064316508380435, - "heading": 0.5311065320658849, - "angularVelocity": -0.41359331842916214, - "velocityX": -1.9150923565792266, - "velocityY": -1.9648028669039912, - "timestamp": 11.292044080261407 - }, - { - "x": 7.555088273925506, - "y": 4.927129260587564, - "heading": 0.5041069990932381, - "angularVelocity": -0.4156378056405048, - "velocityX": -2.221724472585296, - "velocityY": -2.111896035100176, - "timestamp": 11.357003361169552 - }, - { - "x": 7.389294037285908, - "y": 4.784758435393721, - "heading": 0.47842631175838873, - "angularVelocity": -0.3953351542047192, - "velocityX": -2.5522794329271568, - "velocityY": -2.1916933685759186, - "timestamp": 11.421962642077697 - }, - { - "x": 7.201584646398764, - "y": 4.645022669025462, - "heading": 0.45644938425338794, - "angularVelocity": -0.33831851581111005, - "velocityX": -2.8896469952087562, - "velocityY": -2.1511285903218442, - "timestamp": 11.486921922985841 - }, - { - "x": 6.996114167384252, - "y": 4.5183305598699945, - "heading": 0.4409940125357126, - "angularVelocity": -0.23792399641137907, - "velocityX": -3.16306578739771, - "velocityY": -1.9503311518274693, - "timestamp": 11.551881203893986 - }, - { - "x": 6.77954298581377, - "y": 4.410738153252425, - "heading": 0.4262239025498049, - "angularVelocity": -0.2273748997744145, - "velocityX": -3.3339528785227577, - "velocityY": -1.656305382593562, - "timestamp": 11.61684048480213 - }, - { - "x": 6.556274698008425, - "y": 4.324091773722086, - "heading": 0.4025359766205174, - "angularVelocity": -0.3646580688413556, - "velocityX": -3.43704986699365, - "velocityY": -1.3338568149000936, - "timestamp": 11.681799765710275 - }, - { - "x": 6.330620508913663, - "y": 4.259333422241355, - "heading": 0.3712479640794565, - "angularVelocity": -0.4816557711792279, - "velocityX": -3.4737790495840706, - "velocityY": -0.996906840337434, - "timestamp": 11.74675904661842 - }, - { - "x": 6.105727463248204, - "y": 4.216621857922548, - "heading": 0.33499279771425994, - "angularVelocity": -0.5581214240419701, - "velocityX": -3.462061810435755, - "velocityY": -0.6575128868683483, - "timestamp": 11.811718327526565 - }, - { - "x": 5.88368699546657, - "y": 4.195803829122771, - "heading": 0.29554108787827377, - "angularVelocity": -0.6073298423942238, - "velocityX": -3.4181484874441033, - "velocityY": -0.3204781288945418, - "timestamp": 11.87667760843471 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 }, { + "timestamp": 12.966486277091528, + "isStopPoint": false, "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.2541387867771501, - "angularVelocity": -0.6373577496904229, - "velocityX": -3.351873105763798, - "velocityY": 0.013011927161991938, - "timestamp": 11.941636889342854 - }, - { - "x": 5.421845873128357, - "y": 4.226158090162152, - "heading": 0.20498652451135702, - "angularVelocity": -0.6557520326399592, - "velocityX": -3.256674335436179, - "velocityY": 0.3936868024775833, - "timestamp": 12.01659245153595 - }, - { - "x": 5.1888462982312005, - "y": 4.282903811630084, - "heading": 0.15538584116955914, - "angularVelocity": -0.6617345249712095, - "velocityX": -3.1085027992574967, - "velocityY": 0.7570581796417871, - "timestamp": 12.091548013729044 - }, - { - "x": 4.97232882862877, - "y": 4.364011234145266, - "heading": 0.10667736071804554, - "angularVelocity": -0.6498314338038612, - "velocityX": -2.8886111086013, - "velocityY": 1.08207343313948, - "timestamp": 12.166503575922139 - }, - { - "x": 4.778685496187651, - "y": 4.463599711895904, - "heading": 0.06061512765816293, - "angularVelocity": -0.6145272173560689, - "velocityX": -2.5834417990524554, - "velocityY": 1.3286335908479392, - "timestamp": 12.241459138115234 - }, - { - "x": 4.613047571450946, - "y": 4.572114931147961, - "heading": 0.018966273025947666, - "angularVelocity": -0.5556472850530078, - "velocityX": -2.2098149875789206, - "velocityY": 1.4477273744209653, - "timestamp": 12.316414700308329 - }, - { - "x": 4.476747715125104, - "y": 4.67904248790438, - "heading": -0.017090034062622824, - "angularVelocity": -0.4810357768471503, - "velocityX": -1.8184088323521106, - "velocityY": 1.4265459910894789, - "timestamp": 12.391370262501423 - }, - { - "x": 4.368193539344783, - "y": 4.776321878924991, - "heading": -0.04707218407807139, - "angularVelocity": -0.3999990012510431, - "velocityX": -1.4482471027389752, - "velocityY": 1.2978275150549403, - "timestamp": 12.466325824694518 - }, - { - "x": 4.2849948544017495, - "y": 4.858769971637843, - "heading": -0.07089061350138144, - "angularVelocity": -0.3177673374252157, - "velocityX": -1.1099734630593825, - "velocityY": 1.0999596334219341, - "timestamp": 12.541281386887613 - }, - { - "x": 4.224952920077694, - "y": 4.923143218233113, - "heading": -0.08860420951463882, - "angularVelocity": -0.23632130151495423, - "velocityX": -0.8010337400896302, - "velocityY": 0.8588188082618416, - "timestamp": 12.616236949080708 - }, - { - "x": 4.186266834046781, - "y": 4.967343359425017, - "heading": -0.10031361199761639, - "angularVelocity": -0.1562179262002281, - "velocityX": -0.5161202837923176, - "velocityY": 0.5896846064343946, - "timestamp": 12.691192511273803 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.07749108715135035, - "velocityX": -0.250385269598314, - "velocityY": 0.3015711680564945, - "timestamp": 12.766148073466898 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { + "timestamp": 13.950880405530265, + "isStopPoint": true, "x": 4.16749906539917, "y": 4.98994779586792, "heading": -0.106122, - "angularVelocity": -7.372861805121809e-39, - "velocityX": 4.014609310130765e-40, - "velocityY": -1.5891469244205483e-39, - "timestamp": 12.841103635659993 + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -22079,43 +23037,51 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 3 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 5 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 2 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 11 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "CenterLanePCBFSprint": { "waypoints": [ @@ -22250,1568 +23216,1724 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -1.254857983919809e-31, - "velocityX": 0, - "velocityY": 0, + "heading": -5.9009444568140314e-21, + "angularVelocity": -6.3286336172777184e-21, + "velocityX": -2.3802874375881527e-19, + "velocityY": -1.4090020225698018e-19, "timestamp": 0 }, { - "x": 1.3060574276601378, - "y": 5.572382522924401, - "heading": -0.010694878831235951, - "angularVelocity": -0.1421235514303064, - "velocityX": 0.21372563329485256, - "velocityY": -0.24679919125306551, - "timestamp": 0.07525057405130896 - }, - { - "x": 1.3382235783773917, - "y": 5.535239267164631, - "heading": -0.032076102097718266, - "angularVelocity": -0.2841336898257091, - "velocityX": 0.4274538915144429, - "velocityY": -0.4935943177620345, - "timestamp": 0.15050114810261792 - }, - { - "x": 1.3864734186078633, - "y": 5.479524852099925, - "heading": -0.06412157270911513, - "angularVelocity": -0.42585018142040665, - "velocityX": 0.6411889987319064, - "velocityY": -0.7403852497811617, - "timestamp": 0.22575172215392686 - }, - { - "x": 1.4508078150986874, - "y": 5.405239694992128, - "heading": -0.1067981371766019, - "angularVelocity": -0.567126098451675, - "velocityX": 0.8549356240955547, - "velocityY": -0.9871706368349642, - "timestamp": 0.30100229620523583 - }, - { - "x": 1.5312280234732587, - "y": 5.3123844378954015, - "heading": -0.16006527765002243, - "angularVelocity": -0.7078635763993126, - "velocityX": 1.068698935356638, - "velocityY": -1.233947491673002, - "timestamp": 0.3762528702565448 - }, - { - "x": 1.6277357530197398, - "y": 5.200960116982037, - "heading": -0.2238798805982425, - "angularVelocity": -0.8480281214272752, - "velocityX": 1.2824849612365503, - "velocityY": -1.4807105768852864, - "timestamp": 0.4515034443078538 - }, - { - "x": 1.7403333929369436, - "y": 5.0709684768538175, - "heading": -0.2982015141504559, - "angularVelocity": -0.9876553699462992, - "velocityX": 1.4963027370230912, - "velocityY": -1.727450478198834, - "timestamp": 0.5267540183591627 - }, - { - "x": 1.8690253968085462, - "y": 4.922413250022354, - "heading": -0.3829965804922361, - "angularVelocity": -1.1268361392905244, - "velocityX": 1.7101796961898603, - "velocityY": -1.9741407784113794, - "timestamp": 0.6020045924104717 - }, - { - "x": 2.02184146077192, - "y": 4.778613992669738, - "heading": -0.4624358186091319, - "angularVelocity": -1.0556628851958798, - "velocityX": 2.0307627668195027, - "velocityY": -1.9109390082922875, - "timestamp": 0.6772551664617806 - }, - { - "x": 2.158573003278503, - "y": 4.653386193108499, - "heading": -0.5314192813425986, - "angularVelocity": -0.9167167639888999, - "velocityX": 1.817016603947309, - "velocityY": -1.664144109715068, - "timestamp": 0.7525057405130896 - }, - { - "x": 2.279216333070278, - "y": 4.546726509082428, - "heading": -0.5899522323979595, - "angularVelocity": -0.7778405918177544, - "velocityX": 1.6032213881911423, - "velocityY": -1.417393626160111, - "timestamp": 0.8277563145643986 - }, - { - "x": 2.3837699497753926, - "y": 4.458633448495562, - "heading": -0.6380354012157764, - "angularVelocity": -0.6389741131339646, - "velocityX": 1.3894062340921998, - "velocityY": -1.1706629709704055, - "timestamp": 0.9030068886157075 - }, - { - "x": 2.4722329980367945, - "y": 4.3891060989240405, - "heading": -0.6756677941957627, - "angularVelocity": -0.5000944305598923, - "velocityX": 1.1755797132095953, - "velocityY": -0.9239444409216138, - "timestamp": 0.9782574626670165 - }, - { - "x": 2.5446049418017402, - "y": 4.338143858296765, - "heading": -0.7028477816482255, - "angularVelocity": -0.36119309114484527, - "velocityX": 0.9617460687572824, - "velocityY": -0.6772339117562157, - "timestamp": 1.0535080367183254 - }, - { - "x": 2.6008854517223408, - "y": 4.305746335795738, - "heading": -0.7195735469228228, - "angularVelocity": -0.22226761038098528, - "velocityX": 0.7479080476225236, - "velocityY": -0.430528576145586, - "timestamp": 1.1287586107696344 - }, - { - "x": 2.641074357140525, - "y": 4.291913306081633, - "heading": -0.7258432570432893, - "angularVelocity": -0.08331777131422959, - "velocityX": 0.5340677586239448, - "velocityY": -0.1838262350545031, - "timestamp": 1.2040091848209433 + "x": 1.3060573928270223, + "y": 5.572382489770416, + "heading": -0.010694903760090106, + "angularVelocity": -0.1421238741144053, + "velocityX": 0.2137251574806739, + "velocityY": -0.24679961691155075, + "timestamp": 0.0752505786007567 + }, + { + "x": 1.3382234711320444, + "y": 5.535239165388228, + "heading": -0.03207617873440696, + "angularVelocity": -0.28413435978686086, + "velocityX": 0.42745290339416875, + "velocityY": -0.4935951998359545, + "timestamp": 0.1505011572015134 + }, + { + "x": 1.386473197712333, + "y": 5.479524643144728, + "heading": -0.06412173018225528, + "angularVelocity": -0.4258512299003913, + "velocityX": 0.6411874496855803, + "velocityY": -0.7403866293046694, + "timestamp": 0.22575173580227012 + }, + { + "x": 1.4508074341962967, + "y": 5.405239335978357, + "heading": -0.1067984078244425, + "angularVelocity": -0.5671275681295302, + "velocityX": 0.8549334460969267, + "velocityY": -0.987172571263834, + "timestamp": 0.3010023144030268 + }, + { + "x": 1.5312274285318639, + "y": 5.312383879458918, + "heading": -0.16006569861045727, + "angularVelocity": -0.707865531089473, + "velocityX": 1.0686960264083267, + "velocityY": -1.2339500671757528, + "timestamp": 0.3762528930037835 + }, + { + "x": 1.6277348772170532, + "y": 5.2009592989550075, + "heading": -0.22388049710097416, + "angularVelocity": -0.8480306686954192, + "velocityX": 1.2824811513696022, + "velocityY": -1.4807139370330336, + "timestamp": 0.4515034716045402 + }, + { + "x": 1.7403321438675061, + "y": 5.0709673174660805, + "heading": -0.29820238787936876, + "angularVelocity": -0.9876587284824841, + "velocityX": 1.4962976862668933, + "velocityY": -1.7274549100613854, + "timestamp": 0.5267540502052969 + }, + { + "x": 1.869023605292211, + "y": 4.922411602704825, + "heading": -0.3829978231491767, + "angularVelocity": -1.1268409738042233, + "velocityX": 1.7101723842877663, + "velocityY": -1.9741471430995352, + "timestamp": 0.6020046288060535 + }, + { + "x": 2.0218400866158173, + "y": 4.778612697599142, + "heading": -0.46243678265580673, + "angularVelocity": -1.0556591189563451, + "velocityX": 2.0307681902941916, + "velocityY": -1.9109342117962438, + "timestamp": 0.6772552074068102 + }, + { + "x": 2.158571908625681, + "y": 4.653385137637619, + "heading": -0.5314200564732017, + "angularVelocity": -0.9167141980846026, + "velocityX": 1.8170202083793483, + "velocityY": -1.6641408250954677, + "timestamp": 0.7525057860075669 + }, + { + "x": 2.279215456782309, + "y": 4.5467256453679665, + "heading": -0.5899528580218641, + "angularVelocity": -0.7778385580157354, + "velocityX": 1.603224193088739, + "velocityY": -1.4173909922412158, + "timestamp": 0.8277563646083236 + }, + { + "x": 2.383769256289076, + "y": 4.45863275029472, + "heading": -0.6380359000069764, + "angularVelocity": -0.6389723890392618, + "velocityX": 1.3894085793213427, + "velocityY": -1.1706607007051708, + "timestamp": 0.9030069432090803 + }, + { + "x": 2.472232464576315, + "y": 4.389105550793566, + "heading": -0.6756681804644412, + "angularVelocity": -0.500092905027661, + "velocityX": 1.1755817687008607, + "velocityY": -0.9239423907952048, + "timestamp": 0.9782575218098369 + }, + { + "x": 2.5446045532635506, + "y": 4.33814345127288, + "heading": -0.7028480646839942, + "angularVelocity": -0.36119169745865176, + "velocityX": 0.9617479364681578, + "velocityY": -0.6772319956645083, + "timestamp": 1.0535081004105937 + }, + { + "x": 2.6008851981181857, + "y": 4.305746065234092, + "heading": -0.7195737326656834, + "angularVelocity": -0.2222663040297333, + "velocityX": 0.7479097955259911, + "velocityY": -0.43052673668740377, + "timestamp": 1.1287586790113504 + }, + { + "x": 2.641074232135465, + "y": 4.291913170423193, + "heading": -0.7258433490393724, + "angularVelocity": -0.0833165204875385, + "velocityX": 0.5340694352744676, + "velocityY": -0.1838244312283474, + "timestamp": 1.204009257612107 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.05565594681973357, - "velocityX": 0.3202270068209183, - "velocityY": 0.06287502296377193, - "timestamp": 1.2792597588722523 - }, - { - "x": 2.6731092983432756, - "y": 4.320288362117908, - "heading": -0.7067987769295019, - "angularVelocity": 0.19580389973285955, - "velocityX": 0.10461714888899723, - "velocityY": 0.3116194322188291, - "timestamp": 1.3551333149479476 - }, - { - "x": 2.664687958259504, - "y": 4.362805441582899, - "heading": -0.6813130909046112, - "angularVelocity": 0.3358968175826016, - "velocityX": -0.11099176735530734, - "velocityY": 0.5603675597065643, - "timestamp": 1.431006871023643 - }, - { - "x": 2.6399077508131867, - "y": 4.424196327740775, - "heading": -0.6452021974872086, - "angularVelocity": 0.4759351648285144, - "velocityX": -0.32659873516708793, - "velocityY": 0.8091209814393522, - "timestamp": 1.5068804270993383 - }, - { - "x": 2.5987688991007616, - "y": 4.504461543612991, - "heading": -0.5984695371274339, - "angularVelocity": 0.6159281675522111, - "velocityX": -0.5422027626077326, - "velocityY": 1.057881296501775, - "timestamp": 1.5827539831750337 - }, - { - "x": 2.541271696403367, - "y": 4.60360171291691, - "heading": -0.5411163147183906, - "angularVelocity": 0.7559052899984986, - "velocityX": -0.7578029246555089, - "velocityY": 1.3066498320479574, - "timestamp": 1.658627539250729 - }, - { - "x": 2.46741646400873, - "y": 4.721617471023204, - "heading": -0.47313851139747326, - "angularVelocity": 0.8959353803401775, - "velocityX": -0.9733988521888661, - "velocityY": 1.55542674166613, - "timestamp": 1.7345010953264244 - }, - { - "x": 2.3772033191525277, - "y": 4.85850915554647, - "heading": -0.39452182857786383, - "angularVelocity": 1.036153923517168, - "velocityX": -1.1889932345783751, - "velocityY": 1.8042081009834092, - "timestamp": 1.8103746514021197 - }, - { - "x": 2.2706306867395725, - "y": 5.014275344454255, - "heading": -0.30523454109050485, - "angularVelocity": 1.1767905987884617, - "velocityX": -1.404608376447531, - "velocityY": 2.0529707181614056, - "timestamp": 1.886248207477815 - }, - { - "x": 2.1562102192291532, - "y": 5.146460354029385, - "heading": -0.2291038294161038, - "angularVelocity": 1.0033892651564627, - "velocityX": -1.5080414498275487, - "velocityY": 1.7421749607384658, - "timestamp": 1.9621217635535104 - }, - { - "x": 2.0581337319934665, - "y": 5.259758702235753, - "heading": -0.16374377227039283, - "angularVelocity": 0.8614339504706835, - "velocityX": -1.292630691201352, - "velocityY": 1.4932521166502593, - "timestamp": 2.0379953196292058 - }, - { - "x": 1.976403182773577, - "y": 5.354172844449837, - "heading": -0.1092054383318089, - "angularVelocity": 0.7188055596708355, - "velocityX": -1.077194129890758, - "velocityY": 1.244361633992147, - "timestamp": 2.113868875704901 - }, - { - "x": 1.9110187048049692, - "y": 5.429703613246766, - "heading": -0.0655341619901584, - "angularVelocity": 0.575579669661773, - "velocityX": -0.8617558125585749, - "velocityY": 0.9954821245277466, - "timestamp": 2.1897424317805965 - }, - { - "x": 1.861980199634934, - "y": 5.486351461082553, - "heading": -0.03276467227497517, - "angularVelocity": 0.4318960572088904, - "velocityX": -0.6463187928148273, - "velocityY": 0.7466085783539637, - "timestamp": 2.265615987856292 - }, - { - "x": 1.8292876463834091, - "y": 5.5241166468364415, - "heading": -0.01091805858135444, - "angularVelocity": 0.2879344902726512, - "velocityX": -0.4308820482661345, - "velocityY": 0.4977384441630725, - "timestamp": 2.341489543931987 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.72551505430432e-32, - "angularVelocity": 0.1438980739292335, - "velocityX": -0.21544334611501834, - "velocityY": 0.2488695893337634, - "timestamp": 2.4173631000076825 + "angularVelocity": 0.05565716597974986, + "velocityX": 0.3202286486370387, + "velocityY": 0.06287682191127338, + "timestamp": 1.2792598362128638 + }, + { + "x": 2.673109423896828, + "y": 4.320288496439454, + "heading": -0.7067986875650104, + "angularVelocity": 0.19580508899656304, + "velocityX": 0.10461880978884638, + "velocityY": 0.3116212207833284, + "timestamp": 1.3551333878515968 + }, + { + "x": 2.664688214670906, + "y": 4.3628057108082405, + "heading": -0.6813129138970213, + "angularVelocity": 0.3358979923509836, + "velocityX": -0.1109900491548022, + "velocityY": 0.5603693704926264, + "timestamp": 1.4310069394903298 + }, + { + "x": 2.6399081472529042, + "y": 4.424196735750699, + "heading": -0.6452019329979721, + "angularVelocity": 0.47593634565822673, + "velocityX": -0.3265969087089792, + "velocityY": 0.8091228579205605, + "timestamp": 1.5068804911290627 + }, + { + "x": 2.5987694501527234, + "y": 4.5044620989064414, + "heading": -0.5984691831252384, + "angularVelocity": 0.6159293833409661, + "velocityX": -0.542200756545013, + "velocityY": 1.05788329954391, + "timestamp": 1.5827540427677957 + }, + { + "x": 2.5412724247726852, + "y": 4.603602430919982, + "heading": -0.5411158658534186, + "angularVelocity": 0.7559065844828777, + "velocityX": -0.7578006319478658, + "velocityY": 1.3066520529527457, + "timestamp": 1.6586275944065287 + }, + { + "x": 2.467417405939932, + "y": 4.7216183787073325, + "heading": -0.473137956706953, + "angularVelocity": 0.895936827501342, + "velocityX": -0.9733960943919171, + "velocityY": 1.5554293325984934, + "timestamp": 1.7345011460452617 + }, + { + "x": 2.37720453797495, + "y": 4.858510302970022, + "heading": -0.3945211456551798, + "angularVelocity": 1.0361556741946414, + "velocityX": -1.1889896547134935, + "velocityY": 1.8042113662279782, + "timestamp": 1.8103746976839947 + }, + { + "x": 2.2706323270523923, + "y": 5.014276850913776, + "heading": -0.3052336725466659, + "angularVelocity": 1.1767931140702435, + "velocityX": -1.4046029033952228, + "velocityY": 2.052975570268153, + "timestamp": 1.8862482493227277 + }, + { + "x": 2.156211312441972, + "y": 5.146461367106831, + "heading": -0.22910323737059946, + "angularVelocity": 1.0033856796180658, + "velocityX": -1.5080487487284289, + "velocityY": 1.742168559900677, + "timestamp": 1.9621218009614607 + }, + { + "x": 2.058134458607592, + "y": 5.259759379647075, + "heading": -0.16374337318870086, + "angularVelocity": 0.8614314576060107, + "velocityX": -1.2926355985202171, + "velocityY": 1.4932477799347985, + "timestamp": 2.0379953526001935 + }, + { + "x": 1.976403642015832, + "y": 5.354173274627719, + "heading": -0.10920518354656349, + "angularVelocity": 0.7188036998955512, + "velocityX": -1.0771977168121305, + "velocityY": 1.2443584482531382, + "timestamp": 2.1138689042389265 + }, + { + "x": 1.9110189688101253, + "y": 5.429703861532151, + "heading": -0.06553401442898629, + "angularVelocity": 0.5755782901203135, + "velocityX": -0.8617584361561702, + "velocityY": 0.9954797854217321, + "timestamp": 2.1897424558776595 + }, + { + "x": 1.8619803269894504, + "y": 5.486351581272176, + "heading": -0.032764600706343905, + "angularVelocity": 0.4318950808929029, + "velocityX": -0.6463206316489477, + "velocityY": 0.7466069337268786, + "timestamp": 2.2656160075163925 + }, + { + "x": 1.829287687543546, + "y": 5.524116685802078, + "heading": -0.01091803536445235, + "angularVelocity": 0.2879338698406078, + "velocityX": -0.4308832094957455, + "velocityY": 0.4977374027489312, + "timestamp": 2.3414895591551255 + }, + { + "x": 1.812941193580627, + "y": 5.542999267578124, + "heading": 1.0528400336269664e-20, + "angularVelocity": 0.14389777634817838, + "velocityX": -0.2154439011994184, + "velocityY": 0.248869090323821, + "timestamp": 2.4173631107938585 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": -2.2798037235511704e-30, - "angularVelocity": -3.1967802998211905e-29, - "velocityX": 0, - "velocityY": 3.158110370432563e-31, - "timestamp": 2.493236656083378 + "heading": 4.797022054850397e-21, + "angularVelocity": -9.847000764445998e-21, + "velocityX": -1.972173923637389e-19, + "velocityY": -3.0551788776993856e-19, + "timestamp": 2.4932366624325915 }, { - "x": 1.8507557561824508, + "x": 1.850755756182451, "y": 5.5430527114919315, - "heading": 1.2021210509026438e-21, - "angularVelocity": 1.2912282770144636e-20, - "velocityX": 0.40617567078743805, - "velocityY": 0.0005740544395093852, - "timestamp": 2.5863356901257513 + "heading": -8.297424237870081e-19, + "angularVelocity": -8.963996805344217e-18, + "velocityX": 0.4061756697719998, + "velocityY": 0.0005740544380748758, + "timestamp": 2.586335696707713 }, { - "x": 1.9263848800936068, + "x": 1.9263848800936074, "y": 5.543159599317717, - "heading": 3.6194046492049965e-21, - "angularVelocity": 2.5964647465645482e-20, - "velocityX": 0.8123513276919048, - "velocityY": 0.00114810885939775, - "timestamp": 2.6794347241681247 + "heading": -2.6749716281445808e-18, + "angularVelocity": -1.9820068153291573e-17, + "velocityX": 0.8123513256610282, + "velocityY": 0.001148108856528731, + "timestamp": 2.6794347309828344 }, { - "x": 2.039828562987611, - "y": 5.543319931052194, - "heading": -1.5854010106883968e-20, - "angularVelocity": -2.0916881637274291e-19, - "velocityX": 1.2185269596070247, - "velocityY": 0.0017221632439682793, - "timestamp": 2.772533758210498 + "x": 2.039828562987612, + "y": 5.543319931052195, + "heading": -5.8149048269782575e-18, + "angularVelocity": -3.372680740742886e-17, + "velocityX": 1.218526956560709, + "velocityY": 0.00172216323966475, + "timestamp": 2.772533765257956 }, { - "x": 2.1910867994360017, + "x": 2.191086799436003, "y": 5.543533706687691, - "heading": 4.514872599726518e-21, - "angularVelocity": 2.1878726150197974e-19, - "velocityX": 1.624702533213676, - "velocityY": 0.0022962175461305355, - "timestamp": 2.8656327922528715 + "heading": -1.0854467146744384e-17, + "angularVelocity": -5.413119866372016e-17, + "velocityX": 1.6247025291519184, + "velocityY": 0.0022962175403924926, + "timestamp": 2.8656327995330773 }, { - "x": 2.380159562296481, + "x": 2.3801595622964795, "y": 5.5438009261858445, - "heading": -2.4606104769465887e-21, - "angularVelocity": -7.492540764169747e-20, - "velocityX": 2.0308778152781284, - "velocityY": 0.002870271436251635, - "timestamp": 2.958731826295245 + "heading": -4.1769885430235806e-18, + "angularVelocity": 7.172446691559558e-17, + "velocityX": 2.0308778102008924, + "velocityY": 0.0028702714290790255, + "timestamp": 2.9587318338081987 }, { - "x": 2.5314177987448714, - "y": 5.544014701821341, - "heading": 1.8729877149138444e-20, - "angularVelocity": 2.2761232534851326e-19, - "velocityX": 1.6247025332136762, - "velocityY": 0.002296217546130536, - "timestamp": 3.0518308603376183 + "x": 2.53141779874487, + "y": 5.54401470182134, + "heading": -1.8296127242246427e-18, + "angularVelocity": 2.5213750465779628e-17, + "velocityX": 1.6247025291519184, + "velocityY": 0.0022962175403924926, + "timestamp": 3.05183086808332 }, { - "x": 2.6448614816388756, + "x": 2.644861481638875, "y": 5.544175033555818, - "heading": 3.8900824798152035e-20, - "angularVelocity": 2.1666119156330769e-19, - "velocityX": 1.218526959607025, - "velocityY": 0.0017221632439682793, - "timestamp": 3.1449298943799917 + "heading": -7.228112419808573e-19, + "angularVelocity": 1.1888431398676619e-17, + "velocityX": 1.218526956560709, + "velocityY": 0.0017221632396647501, + "timestamp": 3.1449299023584416 }, { "x": 2.7204906055500317, "y": 5.544281921381604, - "heading": 1.1738818455912713e-20, - "angularVelocity": -2.9175390079640015e-19, - "velocityX": 0.8123513276919049, - "velocityY": 0.00114810885939775, - "timestamp": 3.238028928422365 + "heading": -1.9384912404248153e-19, + "angularVelocity": 5.681714338671363e-18, + "velocityX": 0.8123513256610282, + "velocityY": 0.0011481088565287313, + "timestamp": 3.238028936633563 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -1.0197992544535218e-37, - "angularVelocity": -1.260895838142623e-19, - "velocityX": 0.40617567078743805, - "velocityY": 0.0005740544395093853, - "timestamp": 3.3311279624647385 + "heading": 9.777796865534126e-30, + "angularVelocity": 2.0821818999587538e-18, + "velocityX": 0.4061756697719998, + "velocityY": 0.0005740544380748761, + "timestamp": 3.3311279709086845 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": 0, - "angularVelocity": 9.592877809620058e-37, - "velocityX": 0, - "velocityY": 7.039095899612751e-38, - "timestamp": 3.424226996507112 - }, - { - "x": 2.7765323823959327, - "y": 5.537624192256726, - "heading": 9.00740418544712e-19, - "angularVelocity": 1.3499597384555195e-17, - "velocityX": 0.2731753218474804, - "velocityY": -0.10058184592921769, - "timestamp": 3.490950498911381 - }, - { - "x": 2.812967100056819, - "y": 5.524148553837151, - "heading": 1.885084819596924e-20, - "angularVelocity": -1.3217075521281405e-17, - "velocityX": 0.5460552331340873, - "velocityY": -0.2019623960674071, - "timestamp": 3.55767400131565 - }, - { - "x": 2.8675858546495747, - "y": 5.503845620088409, - "heading": 3.43159270927708e-20, - "angularVelocity": 2.317785843530086e-19, - "velocityX": 0.8185834469813632, - "velocityY": -0.30428459264216856, - "timestamp": 3.6243975037199188 - }, - { - "x": 2.9403602428294473, - "y": 5.476640214649627, - "heading": 3.5333541604123114e-18, - "angularVelocity": 5.244086577062937e-17, - "velocityX": 1.0906859735710903, - "velocityY": -0.40773347409056915, - "timestamp": 3.6911210061241877 - }, - { - "x": 3.031255192543775, - "y": 5.4424407847147105, - "heading": 5.715508292123115e-18, - "angularVelocity": 3.2704430269696984e-17, - "velocityX": 1.3622628674916857, - "velocityY": -0.5125544778465974, - "timestamp": 3.7578445085284566 - }, - { - "x": 3.140226319352468, - "y": 5.40113340648718, - "heading": 6.027086577670902e-18, - "angularVelocity": 4.669693201563416e-18, - "velocityX": 1.6331745619175013, - "velocityY": -0.619082882928634, - "timestamp": 3.8245680109327256 - }, - { - "x": 3.267215684627595, - "y": 5.352572466235242, - "heading": 7.223223650198719e-18, - "angularVelocity": 1.7926772867621648e-17, - "velocityX": 1.9032179172147636, - "velocityY": -0.7277936334593704, - "timestamp": 3.8912915133369945 - }, - { - "x": 3.41214454573205, - "y": 5.296565350322459, - "heading": 8.483175715581024e-18, - "angularVelocity": 1.888318238685755e-17, - "velocityX": 2.172081139061778, - "velocityY": -0.8393911274837239, - "timestamp": 3.9580150157412635 - }, - { - "x": 3.5748998961264853, - "y": 5.232845435249424, - "heading": 1.0703776873779667e-17, - "angularVelocity": 3.3280644423357047e-17, - "velocityX": 2.4392507067198372, - "velocityY": -0.9549845673113, - "timestamp": 4.024738518145532 - }, - { - "x": 3.755306479840233, - "y": 5.161019665655948, - "heading": 1.3781541182824059e-17, - "angularVelocity": 4.612713958550321e-17, - "velocityX": 2.7037936740893525, - "velocityY": -1.0764688154152113, - "timestamp": 4.091462020549801 - }, - { - "x": 3.953058178777003, - "y": 5.080452170332065, - "heading": 1.6552203248378283e-17, - "angularVelocity": 4.15245298240439e-17, - "velocityX": 2.9637487813307404, - "velocityY": -1.2074830070480274, - "timestamp": 4.15818552295407 + "heading": -1.1644118306851733e-29, + "angularVelocity": -2.560461213964373e-29, + "velocityX": -2.249094171598222e-22, + "velocityY": 3.2281317136840683e-24, + "timestamp": 3.424227005183806 + }, + { + "x": 2.7765323826419794, + "y": 5.537624192846842, + "heading": -2.642342456596521e-18, + "angularVelocity": -3.960137512120418e-17, + "velocityX": 0.2731753246622511, + "velocityY": -0.1005818367636714, + "timestamp": 3.4909505078012537 + }, + { + "x": 2.812967100767047, + "y": 5.524148555527004, + "heading": -5.7083699786826614e-18, + "angularVelocity": -4.595123759665632e-17, + "velocityX": 0.5460552383462592, + "velocityY": -0.20196237894013302, + "timestamp": 3.5576740104187015 + }, + { + "x": 2.867585856008562, + "y": 5.5038456232928015, + "heading": -9.193458656984461e-18, + "angularVelocity": -5.223180051341883e-17, + "velocityX": 0.8185834540891116, + "velocityY": -0.3042845689712529, + "timestamp": 3.624397513036149 + }, + { + "x": 2.9403602449806896, + "y": 5.476640219670028, + "heading": -1.3713804964383516e-17, + "angularVelocity": -6.774743726038035e-17, + "velocityX": 1.0906859819600914, + "velocityY": -0.40773344557094127, + "timestamp": 3.691121015653597 + }, + { + "x": 3.0312551955793734, + "y": 5.442440791714766, + "heading": -1.9689161108713677e-17, + "angularVelocity": -8.955399386867554e-17, + "velocityX": 1.3622628763933429, + "velocityY": -0.51255444653948, + "timestamp": 3.7578445182710447 + }, + { + "x": 3.1402263232984726, + "y": 5.401133415459366, + "heading": -2.6276370451665866e-17, + "angularVelocity": -9.872397407984698e-17, + "velocityX": 1.6331745703440297, + "velocityY": -0.6190828513939344, + "timestamp": 3.8245680208884925 + }, + { + "x": 3.2672156894222453, + "y": 5.352572476953852, + "heading": -3.586672627631812e-17, + "angularVelocity": -1.4373279951454157e-16, + "velocityX": 1.9032179238529063, + "velocityY": -0.7277936049600628, + "timestamp": 3.89129152350594 + }, + { + "x": 3.4121445511915924, + "y": 5.2965653622743645, + "heading": -4.6442314764739977e-17, + "angularVelocity": -1.5849870096036839e-16, + "velocityX": 2.172081142086939, + "velocityY": -0.8393911063182322, + "timestamp": 3.958015026123388 + }, + { + "x": 3.5748999018873118, + "y": 5.232845447526237, + "heading": -5.766195301574919e-17, + "angularVelocity": -1.6815121824921553e-16, + "velocityX": 2.439250703441936, + "velocityY": -0.9549845593907098, + "timestamp": 4.024738528740836 + }, + { + "x": 3.7553064852503644, + "y": 5.161019676773233, + "heading": -6.954653481566044e-17, + "angularVelocity": -1.7811687536878263e-16, + "velocityX": 2.70379366019489, + "velocityY": -1.0764688293540257, + "timestamp": 4.091462031358284 + }, + { + "x": 3.953058182659589, + "y": 5.080452177894761, + "heading": -7.943549000582077e-17, + "angularVelocity": -1.4820797473458394e-16, + "velocityX": 2.9637487489680097, + "velocityY": -1.2074830564635766, + "timestamp": 4.158185533975732 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 2.0019563257216408e-17, - "angularVelocity": 5.196609716079013e-17, - "velocityX": 3.2138733563909483, - "velocityY": -1.356409229176722, - "timestamp": 4.224909025358339 - }, - { - "x": 4.282917561329041, - "y": 4.939413083786088, - "heading": 2.2898103599715735e-17, - "angularVelocity": 8.297955477827177e-17, - "velocityX": 3.327163862876717, - "velocityY": -1.456761903756842, - "timestamp": 4.25959877936104 - }, - { - "x": 4.401833285665519, - "y": 4.884962621939577, - "heading": 2.513067280390373e-17, - "angularVelocity": 6.43581734252565e-17, - "velocityX": 3.4279783110372324, - "velocityY": -1.5696410485433538, - "timestamp": 4.2942885333637415 - }, - { - "x": 4.518840762702163, - "y": 4.825621230712266, - "heading": 2.8188632508696854e-17, - "angularVelocity": 8.815166877675102e-17, - "velocityX": 3.372969350763695, - "velocityY": -1.7106316528704353, - "timestamp": 4.328978287366443 - }, - { - "x": 4.633380439466489, - "y": 4.761646132811981, - "heading": 3.13421659224765e-17, - "angularVelocity": 9.090676784661175e-17, - "velocityX": 3.3018301817708395, - "velocityY": -1.8442073096080187, - "timestamp": 4.363668041369144 - }, - { - "x": 4.745269125938669, - "y": 4.693140128704652, - "heading": 3.2482743151706595e-17, - "angularVelocity": 3.287936919763874e-17, - "velocityX": 3.2254102022016937, - "velocityY": -1.9748195418737586, - "timestamp": 4.398357795371846 - }, - { - "x": 4.854330043574812, - "y": 4.6202164218202615, - "heading": 3.23593373316049e-17, - "angularVelocity": -3.55741410250663e-18, - "velocityX": 3.1438942353886365, - "velocityY": -2.1021684638845084, - "timestamp": 4.433047549374547 - }, - { - "x": 4.962829604926255, - "y": 4.546460093483164, - "heading": 3.1455405916033935e-17, - "angularVelocity": -2.605759082352001e-17, - "velocityX": 3.1277120426680747, - "velocityY": -2.1261704055714574, - "timestamp": 4.467737303377248 - }, - { - "x": 5.07386693975175, - "y": 4.476582621829349, - "heading": 2.90082292000263e-17, - "angularVelocity": -7.054465464981545e-17, - "velocityX": 3.200868326043711, - "velocityY": -2.014354776005967, - "timestamp": 4.50242705737995 - }, - { - "x": 5.187610618884458, - "y": 4.411202761148161, - "heading": 2.619388391387083e-17, - "angularVelocity": -8.112900673600335e-17, - "velocityX": 3.2788839933500147, - "velocityY": -1.884702343985982, - "timestamp": 4.537116811382651 - }, - { - "x": 5.303878902064225, - "y": 4.350425816718677, - "heading": 2.3840017411584327e-17, - "angularVelocity": -6.785480525757533e-17, - "velocityX": 3.35166064223783, - "velocityY": -1.7520142813566866, - "timestamp": 4.571806565385352 - }, - { - "x": 5.422485692712026, - "y": 4.294349264150295, - "heading": 2.0445926478566092e-17, - "angularVelocity": -9.784130878396876e-17, - "velocityX": 3.419072693296267, - "velocityY": -1.6165162936588033, - "timestamp": 4.606496319388054 - }, - { - "x": 5.5432410890609845, - "y": 4.243062963447586, - "heading": 1.6758899100249362e-17, - "angularVelocity": -1.0628577469963028e-16, - "velocityX": 3.4810104545438403, - "velocityY": -1.478427915594755, - "timestamp": 4.641186073390755 + "heading": -8.307291105750155e-17, + "angularVelocity": -5.451483973353195e-17, + "velocityX": 3.2138732879335503, + "velocityY": -1.3564093381868512, + "timestamp": 4.224909036593179 + }, + { + "x": 4.2829175575872735, + "y": 4.93941307796568, + "heading": -8.597616346613658e-17, + "angularVelocity": -8.369192817411254e-17, + "velocityX": 3.3271637455782836, + "velocityY": -1.45676206741062, + "timestamp": 4.259598790694249 + }, + { + "x": 4.40183327548322, + "y": 4.884962607883731, + "heading": -8.781389044285282e-17, + "angularVelocity": -5.2976073895804537e-17, + "velocityX": 3.4279781156557663, + "velocityY": -1.5696412814950174, + "timestamp": 4.294288544795319 + }, + { + "x": 4.518840747701827, + "y": 4.825621207784163, + "heading": -9.315781416713264e-17, + "angularVelocity": -1.5404905173670211e-16, + "velocityX": 3.3729692023097777, + "velocityY": -1.7106319037798774, + "timestamp": 4.328978298896389 + }, + { + "x": 4.633380419289881, + "y": 4.7616461011987, + "heading": -9.908185769391999e-17, + "angularVelocity": -1.7077213950707558e-16, + "velocityX": 3.301830023191876, + "velocityY": -1.8442075547457024, + "timestamp": 4.3636680529974585 + }, + { + "x": 4.745269100236443, + "y": 4.693140088610349, + "heading": -1.025376712876981e-16, + "angularVelocity": -9.9620584908128e-17, + "velocityX": 3.2254100337687888, + "velocityY": -1.9748197807559171, + "timestamp": 4.398357807098528 + }, + { + "x": 4.854330012145195, + "y": 4.620216373671222, + "heading": -1.025310951791751e-16, + "angularVelocity": 1.8956918365515503e-19, + "velocityX": 3.1438940613704167, + "velocityY": -2.1021686901170322, + "timestamp": 4.433047561199598 + }, + { + "x": 4.962829609437795, + "y": 4.5464600987106305, + "heading": -9.93623379907482e-17, + "angularVelocity": 9.134562265751682e-17, + "velocityX": 3.127713069873129, + "velocityY": -2.1261688608602203, + "timestamp": 4.467737315300668 + }, + { + "x": 5.073866943405842, + "y": 4.47658262622743, + "heading": -9.505129174841953e-17, + "angularVelocity": 1.2427433845795495e-16, + "velocityX": 3.2008682922495537, + "velocityY": -2.014354794202651, + "timestamp": 4.502427069401738 + }, + { + "x": 5.187610621724133, + "y": 4.411202764699172, + "heading": -8.900790995033302e-17, + "angularVelocity": 1.7421229853948998e-16, + "velocityX": 3.278883960575102, + "velocityY": -1.8847023630600785, + "timestamp": 4.537116823502807 + }, + { + "x": 5.303878904131373, + "y": 4.350425819404775, + "heading": -8.318308330059428e-17, + "angularVelocity": 1.6791201899662455e-16, + "velocityX": 3.3516606104641187, + "velocityY": -1.7520143013214118, + "timestamp": 4.571806577603877 + }, + { + "x": 5.422485694048398, + "y": 4.294349265955072, + "heading": -7.684855050558625e-17, + "angularVelocity": 1.8260529539962743e-16, + "velocityX": 3.4190726625349193, + "velocityY": -1.616516314480737, + "timestamp": 4.606496331704947 + }, + { + "x": 5.543241089708319, + "y": 4.243062964356387, + "heading": -6.896458750545335e-17, + "angularVelocity": 2.2727065107078374e-16, + "velocityX": 3.4810104248100697, + "velocityY": -1.4784279372307074, + "timestamp": 4.641186085806017 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 1.3081934400390577e-17, - "angularVelocity": -1.0599569831404499e-16, - "velocityX": 3.537374169625408, - "velocityY": -1.337971116472263, - "timestamp": 4.675875827393456 - }, - { - "x": 5.795129656789146, - "y": 4.153827522905326, - "heading": 9.653855416146634e-18, - "angularVelocity": -9.526631807314967e-17, - "velocityX": 3.589854735093561, - "velocityY": -1.1900109590649786, - "timestamp": 4.711859993188672 - }, - { - "x": 5.925973457628177, - "y": 4.116403970594212, - "heading": 6.773802428340066e-18, - "angularVelocity": -8.003667513641558e-17, - "velocityX": 3.636149343676849, - "velocityY": -1.0400005525788714, - "timestamp": 4.747844158983888 - }, - { - "x": 6.058257666523593, - "y": 4.0844428903853265, - "heading": 3.7266948743050956e-18, - "angularVelocity": -8.467912168301893e-17, - "velocityX": 3.6761782848667472, - "velocityY": -0.8881984479166671, - "timestamp": 4.783828324779104 - }, - { - "x": 6.1917543385226965, - "y": 4.0579993432791355, - "heading": 2.3240409715281496e-19, - "angularVelocity": -9.710634385796704e-17, - "velocityX": 3.7098726356150347, - "velocityY": -0.7348661980016377, - "timestamp": 4.81981249057432 - }, - { - "x": 6.326233441808095, - "y": 4.037118888120514, - "heading": -2.9507025724087755e-18, - "angularVelocity": -8.845853722652732e-17, - "velocityX": 3.7371744019498516, - "velocityY": -0.5802678677464012, - "timestamp": 4.855796656369535 - }, - { - "x": 6.461463252337996, - "y": 4.021837506709874, - "heading": -5.178975495681446e-18, - "angularVelocity": -6.192370655342397e-17, - "velocityX": 3.7580365569536736, - "velocityY": -0.4246696032250841, - "timestamp": 4.891780822164751 - }, - { - "x": 6.596863602394611, - "y": 4.012202736620321, - "heading": -4.3543144284076125e-18, - "angularVelocity": 2.2917331805219353e-17, - "velocityX": 3.7627758505553097, - "velocityY": -0.26775026950420705, - "timestamp": 4.927764987959967 - }, - { - "x": 6.727539043403903, - "y": 4.005664590276287, - "heading": -3.5766746235771005e-18, - "angularVelocity": 2.161061087932747e-17, - "velocityX": 3.6314706238561003, - "velocityY": -0.1816950928150608, - "timestamp": 4.963749153755183 - }, - { - "x": 6.85298501008117, - "y": 4.001263406795894, - "heading": -2.645512635943814e-18, - "angularVelocity": 2.5876992478309148e-17, - "velocityX": 3.4861435274385992, - "velocityY": -0.122308892901394, - "timestamp": 4.999733319550399 - }, - { - "x": 6.973061817653349, - "y": 3.9986189838302026, - "heading": -1.6196069084437975e-18, - "angularVelocity": 2.850992109492732e-17, - "velocityX": 3.336934591051298, - "velocityY": -0.07348851660866845, - "timestamp": 5.035717485345614 - }, - { - "x": 7.0877080332373295, - "y": 3.99753108954472, - "heading": -5.829248205352589e-19, - "angularVelocity": 2.880939616045139e-17, - "velocityX": 3.186018434786799, - "velocityY": -0.030232583177662708, - "timestamp": 5.07170165114083 + "heading": -6.104113455464306e-17, + "angularVelocity": 2.2840902612270297e-16, + "velocityX": 3.5373741409339847, + "velocityY": -1.33797113887621, + "timestamp": 4.6758758399070866 + }, + { + "x": 5.795129656406628, + "y": 4.153827521880981, + "heading": -5.26502063293767e-17, + "angularVelocity": 2.331839018689876e-16, + "velocityX": 3.589854707589498, + "velocityY": -1.1900109819379716, + "timestamp": 4.711860005871443 + }, + { + "x": 5.925973456913311, + "y": 4.116403968556151, + "heading": -4.449297326372924e-17, + "angularVelocity": 2.2668951320100914e-16, + "velocityX": 3.636149317349419, + "velocityY": -1.0400005758616107, + "timestamp": 4.747844171835799 + }, + { + "x": 6.058257665524998, + "y": 4.08444288734655, + "heading": -3.6710199664402193e-17, + "angularVelocity": 2.162832843430151e-16, + "velocityX": 3.6761782597023362, + "velocityY": -0.8881984715516176, + "timestamp": 4.783828337800155 + }, + { + "x": 6.191754337287333, + "y": 4.057999339254895, + "heading": -3.0635067113443973e-17, + "angularVelocity": 1.6882793828813163e-16, + "velocityX": 3.7098726115972305, + "velocityY": -0.7348662219335074, + "timestamp": 4.819812503764511 + }, + { + "x": 6.326233440381154, + "y": 4.037118883128164, + "heading": -2.7100684489157953e-17, + "angularVelocity": 9.822049586045954e-17, + "velocityX": 3.7371743790595944, + "velocityY": -0.5802678919226052, + "timestamp": 4.855796669728867 + }, + { + "x": 6.461463250762805, + "y": 4.021837500768739, + "heading": -2.6741166877102065e-17, + "angularVelocity": 9.990994712261855e-18, + "velocityX": 3.758036535169413, + "velocityY": -0.42466962759569293, + "timestamp": 4.891780835693223 + }, + { + "x": 6.596863600046523, + "y": 4.012202729774147, + "heading": -2.0941092472280878e-17, + "angularVelocity": 1.6118407219507027e-16, + "velocityX": 3.7627758113898246, + "velocityY": -0.26775029339668854, + "timestamp": 4.927765001657579 + }, + { + "x": 6.727539041313674, + "y": 4.005664584143169, + "heading": -1.5390888357792948e-17, + "angularVelocity": 1.5424017663331785e-16, + "velocityX": 3.631470613952549, + "velocityY": -0.1816950721451956, + "timestamp": 4.963749167621935 + }, + { + "x": 6.852985008513406, + "y": 4.001263402089072, + "heading": -1.0075825367590821e-17, + "angularVelocity": 1.4770560460742425e-16, + "velocityX": 3.4861435255715034, + "velocityY": -0.12230885268972179, + "timestamp": 4.999733333586291 + }, + { + "x": 6.973061816658764, + "y": 3.998618980784619, + "heading": -5.1577923312514375e-18, + "angularVelocity": 1.3667214188229055e-16, + "velocityX": 3.336934591294929, + "velocityY": -0.07348847009744364, + "timestamp": 5.035717499550647 + }, + { + "x": 7.087708032779949, + "y": 3.9975310881250836, + "heading": -2.6946740234793898e-18, + "angularVelocity": 6.845005967538265e-17, + "velocityX": 3.186018434740021, + "velocityY": -0.030232537850478983, + "timestamp": 5.071701665515003 }, { "x": 7.196889877319336, "y": 3.9978766441345215, - "heading": 0, - "angularVelocity": 1.619948128975407e-17, - "velocityX": 3.034163545803906, - "velocityY": 0.009602962363163213, - "timestamp": 5.107685816936046 - }, - { - "x": 7.37338840046626, - "y": 4.002743213470309, - "heading": 8.093787297850302e-19, - "angularVelocity": 1.2673214059219633e-17, - "velocityX": 2.7636055688801218, - "velocityY": 0.07620051362427777, - "timestamp": 5.171551125768248 - }, - { - "x": 7.532268457404618, - "y": 4.010110422409507, - "heading": 7.012678022232893e-19, - "angularVelocity": -1.6927958157108362e-18, - "velocityX": 2.487736454164719, - "velocityY": 0.11535541084684194, - "timestamp": 5.235416434600451 - }, - { - "x": 7.673406960294269, - "y": 4.018856350887051, - "heading": 5.093320205791772e-19, - "angularVelocity": -3.0053214359033797e-18, - "velocityX": 2.2099400358412575, - "velocityY": 0.13694333649152024, - "timestamp": 5.299281743432653 - }, - { - "x": 7.796760795915294, - "y": 4.028209945243461, - "heading": 2.737170340594828e-19, - "angularVelocity": -3.689248370192142e-18, - "velocityX": 1.931468552749354, - "velocityY": 0.14645814022421355, - "timestamp": 5.363147052264855 - }, - { - "x": 7.902319650014668, - "y": 4.037611597874035, - "heading": 3.811839387951723e-20, - "angularVelocity": -3.688992419979718e-18, - "velocityX": 1.65283556956901, - "velocityY": 0.14721063441931925, - "timestamp": 5.4270123610970575 - }, - { - "x": 7.990087417936277, - "y": 4.046637814965993, - "heading": -1.8999762440941744e-19, - "angularVelocity": -3.5718298785688795e-18, - "velocityX": 1.3742635794998819, - "velocityY": 0.14133208242480425, - "timestamp": 5.49087766992926 - }, - { - "x": 8.06007417147421, - "y": 4.0549574599073335, - "heading": -2.1124739162594107e-19, - "angularVelocity": -3.327278549958625e-19, - "velocityX": 1.0958492931086379, - "velocityY": 0.1302686089438499, - "timestamp": 5.554742978761462 - }, - { - "x": 8.112292461632407, - "y": 4.062304767499659, - "heading": -1.9392441616154913e-19, - "angularVelocity": 2.712423345539538e-19, - "velocityX": 0.8176315297463507, - "velocityY": 0.11504379649410336, - "timestamp": 5.6186082875936645 - }, - { - "x": 8.146755551617066, - "y": 4.068461861174775, - "heading": -1.2666965805994144e-19, - "angularVelocity": 1.0530718371509655e-18, - "velocityX": 0.5396214410425377, - "velocityY": 0.0964074829935333, - "timestamp": 5.682473596425867 + "heading": -6.591539659972594e-30, + "angularVelocity": 7.488499319392298e-17, + "velocityX": 3.034163544252643, + "velocityY": 0.009603001769728973, + "timestamp": 5.107685831479359 + }, + { + "x": 7.373388400468218, + "y": 4.002743214942896, + "heading": -4.953200001247003e-18, + "angularVelocity": -7.75569723773575e-17, + "velocityX": 2.7636055642205424, + "velocityY": 0.07620053655266033, + "timestamp": 5.17155114041995 + }, + { + "x": 7.532268457334727, + "y": 4.010110424631325, + "heading": -1.3097129093463054e-17, + "angularVelocity": -1.2751725823370097e-16, + "velocityX": 2.48773644881765, + "velocityY": 0.1153554223824672, + "timestamp": 5.2354164493605415 + }, + { + "x": 7.673406960142625, + "y": 4.018856353361859, + "heading": -1.8281069995800668e-17, + "angularVelocity": -8.116990254124082e-17, + "velocityX": 2.209940030810585, + "velocityY": 0.13694334022043622, + "timestamp": 5.299281758301133 + }, + { + "x": 7.796760795699071, + "y": 4.0282099476371425, + "heading": -2.4884139524870184e-17, + "angularVelocity": -1.03390551752411e-16, + "velocityX": 1.9314685484602037, + "velocityY": 0.1464581387053742, + "timestamp": 5.363147067241724 + }, + { + "x": 7.902319649761861, + "y": 4.0376115999719895, + "heading": -2.509945215126813e-17, + "angularVelocity": -3.3713549672899342e-18, + "velocityX": 1.6528355661910739, + "velocityY": 0.14721062953900424, + "timestamp": 5.427012376182315 + }, + { + "x": 7.9900874176780725, + "y": 4.046637816644616, + "heading": -1.900477135412208e-17, + "angularVelocity": 9.543022492350273e-17, + "velocityX": 1.3742635770830707, + "velocityY": 0.14133207561907785, + "timestamp": 5.490877685122906 + }, + { + "x": 8.060074171241288, + "y": 4.054957461114281, + "heading": -1.4433757000062975e-17, + "angularVelocity": 7.157272750648916e-17, + "velocityX": 1.0958492916446645, + "velocityY": 0.13026860133729024, + "timestamp": 5.554742994063497 + }, + { + "x": 8.112292461453118, + "y": 4.062304768239738, + "heading": -9.3901869246783e-18, + "angularVelocity": 7.897198273854564e-17, + "velocityX": 0.8176315291985099, + "velocityY": 0.11504378898864956, + "timestamp": 5.6186083030040885 + }, + { + "x": 8.146755551516593, + "y": 4.0684618614995625, + "heading": -4.677400275001561e-18, + "angularVelocity": 7.379259143526985e-17, + "velocityX": 0.5396214413607953, + "velocityY": 0.09640747632727756, + "timestamp": 5.68247361194468 }, { "x": 8.1634765625, "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 1.9833875444262685e-18, - "velocityX": 0.26181680146361314, - "velocityY": 0.07492478756156581, - "timestamp": 5.746338905258069 - }, - { - "x": 8.160404735397709, - "y": 4.0766649695322, - "heading": 2.2110153580287816e-19, - "angularVelocity": 3.145700398787454e-18, - "velocityX": -0.04370411858778341, - "velocityY": 0.04862945386701764, - "timestamp": 5.816625807306458 - }, - { - "x": 8.135851784135758, - "y": 4.07831805033267, - "heading": 5.233120115827785e-19, - "angularVelocity": 4.299669881203419e-18, - "velocityX": -0.34932470412548816, - "velocityY": 0.02351904483329859, - "timestamp": 5.8869127093548475 - }, - { - "x": 8.089809855784104, - "y": 4.078304537638241, - "heading": 9.059382457941767e-19, - "angularVelocity": 5.443777191210811e-18, - "velocityX": -0.6550570164545902, - "velocityY": -0.00019225053366650092, - "timestamp": 5.957199611403237 - }, - { - "x": 8.022270134044854, - "y": 4.076742304755474, - "heading": 1.3681480133267195e-18, - "angularVelocity": 6.5760441001468545e-18, - "velocityX": -0.9609147617966158, - "velocityY": -0.022226515001204008, - "timestamp": 6.027486513451626 - }, - { - "x": 7.9332227395210335, - "y": 4.0737752063023285, - "heading": 1.9089237482918726e-18, - "angularVelocity": 7.693833690281903e-18, - "velocityX": -1.2669130652893827, - "velocityY": -0.04221410201153685, - "timestamp": 6.097773415500015 - }, - { - "x": 7.822656701940191, - "y": 4.069582704513375, - "heading": 2.0768842809054915e-18, - "angularVelocity": 2.3896419918926418e-18, - "velocityX": -1.5730674472567026, - "velocityY": -0.05964840769432409, - "timestamp": 6.168060317548404 - }, - { - "x": 7.690560152144021, - "y": 4.064394900776391, - "heading": 2.063501960546536e-18, - "angularVelocity": -1.9039564936380514e-19, - "velocityX": -1.8793906965088152, - "velocityY": -0.07380896846772396, - "timestamp": 6.238347219596793 - }, - { - "x": 7.536921137087825, - "y": 4.058517363178713, - "heading": 1.8242393677495476e-18, - "angularVelocity": -3.404085054560583e-18, - "velocityX": -2.185884006531148, - "velocityY": -0.08362208927108938, - "timestamp": 6.3086341216451824 - }, - { - "x": 7.361730243526304, - "y": 4.0523752131799995, - "heading": 1.7449161631907716e-18, - "angularVelocity": -1.128563106982712e-18, - "velocityX": -2.492511242577032, - "velocityY": -0.08738683623422486, - "timestamp": 6.378921023693572 - }, - { - "x": 7.164988961998875, - "y": 4.046599373609111, - "heading": 1.6203310877610785e-18, - "angularVelocity": -1.7725219321907188e-18, - "velocityX": -2.799117272119643, - "velocityY": -0.0821751905769253, - "timestamp": 6.449207925741961 - }, - { - "x": 6.946739461316107, - "y": 4.042219879396985, - "heading": 9.714507263424963e-19, - "angularVelocity": -9.231881652246047e-18, - "velocityX": -3.105123349048912, - "velocityY": -0.062308824041090796, - "timestamp": 6.51949482779035 - }, - { - "x": 6.707199372936614, - "y": 4.041196558011354, - "heading": 4.304213175776067e-18, - "angularVelocity": 4.741655062759286e-17, - "velocityX": -3.408033095761992, - "velocityY": -0.014559204571673643, - "timestamp": 6.589781729838739 - }, - { - "x": 6.4477381349567215, - "y": 4.048401250319157, - "heading": 3.981085161265745e-18, - "angularVelocity": -4.597272110228882e-18, - "velocityX": -3.691459296374506, - "velocityY": 0.102504052644734, - "timestamp": 6.660068631887128 - }, - { - "x": 6.183416579926013, - "y": 4.076604236120931, - "heading": 3.783709688486887e-18, - "angularVelocity": -2.808140165901377e-18, - "velocityX": -3.7606089801587, - "velocityY": 0.4012552122777795, - "timestamp": 6.730355533935517 - }, - { - "x": 5.922248771081443, - "y": 4.126128703964576, - "heading": 4.4649221139499255e-18, - "angularVelocity": 9.691882920007657e-18, - "velocityX": -3.7157393658460984, - "velocityY": 0.7046045052540438, - "timestamp": 6.800642435983907 + "heading": -3.026226030610745e-28, + "angularVelocity": 7.323851324219789e-17, + "velocityX": 0.2618168025924992, + "velocityY": 0.07492478234891005, + "timestamp": 5.746338920885271 + }, + { + "x": 8.160404735497007, + "y": 4.076664969326324, + "heading": 5.687417788003061e-18, + "angularVelocity": 8.091717809987449e-17, + "velocityX": -0.04370411704181345, + "velocityY": 0.04862945078972498, + "timestamp": 5.816625823147878 + }, + { + "x": 8.13585178429954, + "y": 4.078318050056324, + "heading": 9.715319347453537e-18, + "angularVelocity": 5.730657391283969e-17, + "velocityX": -0.34932470214341615, + "velocityY": 0.02351904375900896, + "timestamp": 5.886912725410485 + }, + { + "x": 8.089809855979007, + "y": 4.078304537416186, + "heading": 1.421739507206778e-17, + "angularVelocity": 6.405284028537325e-17, + "velocityX": -0.6550570140153344, + "velocityY": -0.00019224976064422702, + "timestamp": 5.957199627673092 + }, + { + "x": 8.022270134239134, + "y": 4.076742304699779, + "heading": 2.3560062445993e-17, + "angularVelocity": 1.3292188264586088e-16, + "velocityX": -0.9609147588768534, + "velocityY": -0.022226512566613944, + "timestamp": 6.0274865299356994 + }, + { + "x": 7.933222739684713, + "y": 4.073775206509657, + "heading": 3.0510865621673614e-17, + "angularVelocity": 9.889186963848529e-17, + "velocityX": -1.2669130618635012, + "velocityY": -0.04221409814073001, + "timestamp": 6.097773432198307 + }, + { + "x": 7.822656702045201, + "y": 4.069582705061331, + "heading": 3.6286017590367637e-17, + "angularVelocity": 8.21654075361425e-17, + "velocityX": -1.573067443297094, + "velocityY": -0.05964840266629161, + "timestamp": 6.168060334460914 + }, + { + "x": 7.690560152164254, + "y": 4.0643949017183925, + "heading": 4.582493898384423e-17, + "angularVelocity": 1.3571406743753222e-16, + "velocityX": -1.8793906919870382, + "velocityY": -0.07380896263653505, + "timestamp": 6.238347236723521 + }, + { + "x": 7.53692113699896, + "y": 4.058517364536531, + "heading": 5.495539333254171e-17, + "angularVelocity": 1.2990264266804977e-16, + "velocityX": -2.1858840014212815, + "velocityY": -0.0836220831002289, + "timestamp": 6.308634138986128 + }, + { + "x": 7.361730243304987, + "y": 4.052375214932341, + "heading": 6.396812526564607e-17, + "angularVelocity": 1.2822775856015118e-16, + "velocityX": -2.4925112368648836, + "velocityY": -0.08738683035483485, + "timestamp": 6.378921041248735 + }, + { + "x": 7.164988961620082, + "y": 4.046599375673031, + "heading": 7.356788600543696e-17, + "angularVelocity": 1.3657965326299643e-16, + "velocityX": -2.799117265829076, + "velocityY": -0.08217518589352116, + "timestamp": 6.449207943511342 + }, + { + "x": 6.946739460744683, + "y": 4.042219881595439, + "heading": 8.53416334460989e-17, + "angularVelocity": 1.6750983557133948e-16, + "velocityX": -3.105123342325879, + "velocityY": -0.06230882193712492, + "timestamp": 6.519494845773949 + }, + { + "x": 6.707199372096715, + "y": 4.041196560014842, + "heading": 9.976814238544392e-17, + "angularVelocity": 2.052517393014489e-16, + "velocityX": -3.408033089194812, + "velocityY": -0.014559207301144003, + "timestamp": 6.589781748036557 + }, + { + "x": 6.447738133610024, + "y": 4.048401251612496, + "heading": 1.0469202046971274e-16, + "angularVelocity": 7.00539919377632e-17, + "velocityX": -3.691459292334275, + "velocityY": 0.10250404222876178, + "timestamp": 6.660068650299164 + }, + { + "x": 6.18341657905221, + "y": 4.076604236955095, + "heading": 9.517171884835342e-17, + "angularVelocity": -1.354491564534057e-16, + "velocityX": -3.7606089619691896, + "velocityY": 0.4012552045219841, + "timestamp": 6.730355552561771 + }, + { + "x": 5.922248770654031, + "y": 4.126128704367341, + "heading": 8.827514914451601e-17, + "angularVelocity": -9.812026823816515e-17, + "velocityX": -3.7157393481704415, + "velocityY": 0.7046044969688833, + "timestamp": 6.800642454824378 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 6.738429572739451e-18, - "angularVelocity": 3.2346104217601064e-17, - "velocityX": -3.646441012355221, - "velocityY": 1.003321650758162, - "timestamp": 6.870929338032296 - }, - { - "x": 5.5410942529337, - "y": 4.236435293525444, - "heading": 7.176696434761227e-18, - "angularVelocity": 1.2648537308401216e-17, - "velocityX": -3.603431102925481, - "velocityY": 1.1482444113922432, - "timestamp": 6.90557894623092 - }, - { - "x": 5.417926513758212, - "y": 4.281179467560016, - "heading": 6.902122193005105e-18, - "angularVelocity": -7.924310144639137e-18, - "velocityX": -3.554664701241286, - "velocityY": 1.2913327555706813, - "timestamp": 6.9402285544295435 - }, - { - "x": 5.296645297491296, - "y": 4.3308101048073215, - "heading": 7.073122455788382e-18, - "angularVelocity": 4.9351283224632194e-18, - "velocityX": -3.5002189800153154, - "velocityY": 1.432357819540261, - "timestamp": 6.974878162628167 - }, - { - "x": 5.177444375118266, - "y": 4.38524790502043, - "heading": 7.030277898528261e-18, - "angularVelocity": -1.2365091407245627e-18, - "velocityX": -3.44018095932656, - "velocityY": 1.5710942502164025, - "timestamp": 7.009527770826791 - }, - { - "x": 5.06051419213652, - "y": 4.444405885414455, - "heading": 6.93623096110632e-18, - "angularVelocity": -2.7142280190538443e-18, - "velocityX": -3.3746466139374856, - "velocityY": 1.7073203268248038, - "timestamp": 7.044177379025415 - }, - { - "x": 4.9460415627669985, - "y": 4.508189517767328, - "heading": 6.830736149305672e-18, - "angularVelocity": -3.0446177398576756e-18, - "velocityX": -3.303720743776551, - "velocityY": 1.8408182853682935, - "timestamp": 7.078826987224039 - }, - { - "x": 4.834209367418348, - "y": 4.576496874327604, - "heading": 6.686796334021e-18, - "angularVelocity": -4.154154195902009e-18, - "velocityX": -3.2275168800637117, - "velocityY": 1.971374572800783, - "timestamp": 7.113476595422663 - }, - { - "x": 4.725196244628599, - "y": 4.649218770197504, - "heading": 6.8822518039480765e-18, - "angularVelocity": 5.6409142870147405e-18, - "velocityX": -3.1461574446916942, - "velocityY": 2.0987797453014108, - "timestamp": 7.1481262036212865 - }, - { - "x": 4.61917611476972, - "y": 4.726238684910051, - "heading": 7.643017507811422e-18, - "angularVelocity": 2.195596843411265e-17, - "velocityX": -3.0597786056088934, - "velocityY": 2.2228220957374782, - "timestamp": 7.18277581181991 - }, - { - "x": 4.510162876116158, - "y": 4.798960406973094, - "heading": 8.156998265512583e-18, - "angularVelocity": 1.483366723094954e-17, - "velocityX": -3.14616078856249, - "velocityY": 2.098774729173764, - "timestamp": 7.217425420018534 - }, - { - "x": 4.3983305631985266, - "y": 4.867267570914994, - "heading": 9.169956130720048e-18, - "angularVelocity": 2.9234323788045264e-17, - "velocityX": -3.227520273146238, - "velocityY": 1.9713690137660609, - "timestamp": 7.252075028217158 - }, - { - "x": 4.28385782617867, - "y": 4.93105099911187, - "heading": 9.858535854282379e-18, - "angularVelocity": 1.9872655402484013e-17, - "velocityX": -3.3037238506034297, - "velocityY": 1.840812393353667, - "timestamp": 7.286724636415782 + "heading": 8.108309692704171e-17, + "angularVelocity": -1.0232421668622016e-16, + "velocityX": -3.6464409951607806, + "velocityY": 1.003321641969972, + "timestamp": 6.870929357086985 + }, + { + "x": 5.541094252914905, + "y": 4.236435293416023, + "heading": 7.779103781629071e-17, + "angularVelocity": -9.500999479304098e-17, + "velocityX": -3.603431085871871, + "velocityY": 1.1482444026273093, + "timestamp": 6.905578965454807 + }, + { + "x": 5.417926513725, + "y": 4.281179467366055, + "heading": 7.404581584331534e-17, + "angularVelocity": -1.0808843589526725e-16, + "velocityX": -3.5546646842995244, + "velocityY": 1.291332746825043, + "timestamp": 6.940228573822629 + }, + { + "x": 5.2966452974499845, + "y": 4.330810104553132, + "heading": 7.048745543918544e-17, + "angularVelocity": -1.0269554467404476e-16, + "velocityX": -3.5002189631570437, + "velocityY": 1.4323578108076789, + "timestamp": 6.974878182190451 + }, + { + "x": 5.177444375077077, + "y": 4.385247904729644, + "heading": 6.777735008198806e-17, + "angularVelocity": -7.821460284920669e-17, + "velocityX": -3.440180942524166, + "velocityY": 1.5710942414883695, + "timestamp": 7.0095277905582725 + }, + { + "x": 5.060514192105533, + "y": 4.444405885109906, + "heading": 6.539767218111103e-17, + "angularVelocity": -6.867834914008308e-17, + "velocityX": -3.3746465971642725, + "velocityY": 1.7073203180905416, + "timestamp": 7.044177398926094 + }, + { + "x": 4.946041562758086, + "y": 4.5081895174709326, + "heading": 6.339841419086817e-17, + "angularVelocity": -5.769929544659045e-17, + "velocityX": -3.3037207270069695, + "velocityY": 1.8408182766146604, + "timestamp": 7.078827007293916 + }, + { + "x": 4.834209367445105, + "y": 4.5764968740602265, + "heading": 6.206624882969642e-17, + "angularVelocity": -3.844676532404254e-17, + "velocityX": -3.2275168632738827, + "velocityY": 1.9713745640117823, + "timestamp": 7.113476615661738 + }, + { + "x": 4.725196244706173, + "y": 4.649218769978707, + "heading": 6.152068396615488e-17, + "angularVelocity": -1.574519567617046e-17, + "velocityX": -3.1461574278620072, + "velocityY": 2.098779736454826, + "timestamp": 7.14812622402956 + }, + { + "x": 4.619176114911752, + "y": 4.726238684753922, + "heading": 5.2556937935057745e-17, + "angularVelocity": -2.58696893087623e-16, + "velocityX": -3.0597785888073585, + "velocityY": 2.2228220866918083, + "timestamp": 7.182775832397382 + }, + { + "x": 4.51016287630692, + "y": 4.798960406862416, + "heading": 6.64273126982982e-17, + "angularVelocity": 4.003039403991449e-16, + "velocityX": -3.1461607717930216, + "velocityY": 2.0987747202369076, + "timestamp": 7.217425440765203 + }, + { + "x": 4.398330563422939, + "y": 4.8672675708300295, + "heading": 7.26949465762174e-17, + "angularVelocity": 1.8088613906315835e-16, + "velocityX": -3.2275202564146483, + "velocityY": 1.9713690048817056, + "timestamp": 7.252075049133025 + }, + { + "x": 4.283857826423259, + "y": 4.9310509990316564, + "heading": 7.854869048227426e-17, + "angularVelocity": 1.6894112751752813e-16, + "velocityX": -3.303723833888627, + "velocityY": 1.840812384501881, + "timestamp": 7.286724657500847 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 9.428613355674913e-18, - "angularVelocity": -1.2407716016383212e-17, - "velocityX": -3.3581551662140523, - "velocityY": 1.6997824742615957, - "timestamp": 7.321374244614406 - }, - { - "x": 3.9412126137988053, - "y": 5.086809869447978, - "heading": 6.455008109104801e-18, - "angularVelocity": -4.321651467897992e-17, - "velocityX": -3.28870544216361, - "velocityY": 1.4077326603917448, - "timestamp": 7.390181395507035 - }, - { - "x": 3.7303194478817963, - "y": 5.1698988995186355, - "heading": 5.431152098522906e-18, - "angularVelocity": -1.488008146391029e-17, - "velocityX": -3.0649890771686916, - "velocityY": 1.2075638795204993, - "timestamp": 7.458988546399665 - }, - { - "x": 3.537102982396402, - "y": 5.24230220978644, - "heading": 5.612600329775147e-18, - "angularVelocity": 2.6370548540132515e-18, - "velocityX": -2.8080869935582533, - "velocityY": 1.052264326142308, - "timestamp": 7.527795697292294 - }, - { - "x": 3.3623900460315483, - "y": 5.305525654536907, - "heading": 4.225050788166454e-18, - "angularVelocity": -2.0165775266205907e-17, - "velocityX": -2.5391683000722396, - "velocityY": 0.918849915020073, - "timestamp": 7.596602848184924 - }, - { - "x": 3.2065873746476083, - "y": 5.360439315991953, - "heading": 1.940037667439135e-18, - "angularVelocity": -3.3208948359058894e-17, - "velocityX": -2.2643383625498803, - "velocityY": 0.7980807335088809, - "timestamp": 7.6654099990775535 - }, - { - "x": 3.069932459985333, - "y": 5.407606100907469, - "heading": -1.0877040544793261e-18, - "angularVelocity": -4.4003300276785115e-17, - "velocityX": -1.986056868936749, - "velocityY": 0.6854924859353159, - "timestamp": 7.734217149970183 - }, - { - "x": 2.9525796309081422, - "y": 5.44741896036095, - "heading": -3.706402695175446e-18, - "angularVelocity": -3.805852453885881e-17, - "velocityX": -1.705532456362192, - "velocityY": 0.5786151430046301, - "timestamp": 7.803024300862813 - }, - { - "x": 2.854636716681842, - "y": 5.480167404381309, - "heading": -5.982131114439951e-18, - "angularVelocity": -3.307401032802041e-17, - "velocityX": -1.4234409208300893, - "velocityY": 0.4759453573577233, - "timestamp": 7.871831451755442 - }, - { - "x": 2.776183090706554, - "y": 5.506073449109121, - "heading": -4.835349945984594e-18, - "angularVelocity": 1.666659865403876e-17, - "velocityX": -1.1401958220550505, - "velocityY": 0.3765022151293121, - "timestamp": 7.940638602648072 - }, - { - "x": 2.7172795158509544, - "y": 5.525312687608081, - "heading": -3.43564479511919e-18, - "angularVelocity": 2.034243727152086e-17, - "velocityX": -0.8560676338352585, - "velocityY": 0.2796110324199007, - "timestamp": 8.009445753540701 - }, - { - "x": 2.6779739479348046, - "y": 5.538027438094956, - "heading": -1.81505607662002e-18, - "angularVelocity": 2.355262058485764e-17, - "velocityX": -0.5712424857916886, - "velocityY": 0.18478821346223093, - "timestamp": 8.07825290443333 + "heading": 7.967166003614947e-17, + "angularVelocity": 3.240930002777194e-17, + "velocityX": -3.358155156874672, + "velocityY": 1.6997824682762939, + "timestamp": 7.321374265868669 + }, + { + "x": 3.941212613699701, + "y": 5.086809869285441, + "heading": 7.443579239948039e-17, + "angularVelocity": -7.609481799868211e-17, + "velocityX": -3.288705435486243, + "velocityY": 1.4077326545547513, + "timestamp": 7.3901814169311395 + }, + { + "x": 3.7303194477377524, + "y": 5.1698988992560615, + "heading": 6.400702034661796e-17, + "angularVelocity": -1.515652354772208e-16, + "velocityX": -3.0649890702563414, + "velocityY": 1.2075638750859479, + "timestamp": 7.45898856799361 + }, + { + "x": 3.5371029822412163, + "y": 5.24230220948227, + "heading": 4.92329505445724e-17, + "angularVelocity": -2.1471706900158835e-16, + "velocityX": -2.8080869867888283, + "velocityY": 1.0522643229404085, + "timestamp": 7.527795719056081 + }, + { + "x": 3.362390045884083, + "y": 5.305525654232102, + "heading": 3.1750019865336384e-17, + "angularVelocity": -2.5408595487118374e-16, + "velocityX": -2.5391682936924758, + "velocityY": 0.9188499127428014, + "timestamp": 7.596602870118551 + }, + { + "x": 3.2065873745181794, + "y": 5.36043931571324, + "heading": 2.169929247164939e-17, + "angularVelocity": -1.4607097138783564e-16, + "velocityX": -2.2643383566985644, + "velocityY": 0.7980807319181392, + "timestamp": 7.665410021181022 + }, + { + "x": 3.069932459879016, + "y": 5.407606100670878, + "heading": 1.5380181020622108e-17, + "angularVelocity": -9.183800453716929e-17, + "velocityX": -1.9860568636985672, + "velocityY": 0.6854924848554687, + "timestamp": 7.7342171722434925 + }, + { + "x": 2.952579630826552, + "y": 5.447418960174395, + "heading": 8.435159362125091e-18, + "angularVelocity": -1.0093459112453562e-16, + "velocityX": -1.7055324517929655, + "velocityY": 0.5786151423035916, + "timestamp": 7.803024323305963 + }, + { + "x": 2.8546367166241713, + "y": 5.480167404246393, + "heading": 5.0453569918778545e-18, + "angularVelocity": -4.926526267025923e-17, + "velocityX": -1.423440916968907, + "velocityY": 0.4759453569334053, + "timestamp": 7.871831474368434 + }, + { + "x": 2.7761830906702163, + "y": 5.506073449022424, + "heading": 3.1520224912430142e-18, + "angularVelocity": -2.7516536752894226e-17, + "velocityX": -1.1401958189305985, + "velocityY": 0.3765022149007732, + "timestamp": 7.940638625430904 + }, + { + "x": 2.7172795158320118, + "y": 5.5253126875621055, + "heading": 4.0055040762053705e-18, + "angularVelocity": 1.2403966322481763e-17, + "velocityX": -0.8560676314693773, + "velocityY": 0.2796110323215353, + "timestamp": 8.009445776493374 + }, + { + "x": 2.677973947928258, + "y": 5.538027438078825, + "heading": 2.2657574428082367e-18, + "angularVelocity": -2.5284386970987473e-17, + "velocityX": -0.571242484201505, + "velocityY": 0.18478821343985707, + "timestamp": 8.078252927555845 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -8.520572182632915e-28, + "angularVelocity": -3.292909834387365e-17, + "velocityX": -0.2858537153870097, + "velocityY": 0.09167545987855215, + "timestamp": 8.147060078618315 }, { "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": -7.816373438340897e-46, - "angularVelocity": 2.6378887267870092e-17, - "velocityX": -0.2858537161877381, - "velocityY": 0.09167545987040067, - "timestamp": 8.14706005532596 + "heading": -3.9086161010585418e-28, + "angularVelocity": 3.4350134480026537e-28, + "velocityX": 1.6920853023952865e-26, + "velocityY": 2.9127538584633355e-26, + "timestamp": 8.215867229680786 + }, + { + "x": 2.675073555498551, + "y": 5.538716972612474, + "heading": 1.6968135653440987e-18, + "angularVelocity": 2.665146289407491e-17, + "velocityX": 0.2633772278822265, + "velocityY": -0.08824680986851689, + "timestamp": 8.279534037481625 + }, + { + "x": 2.7085920648883794, + "y": 5.5274259655637525, + "heading": 3.725095749658628e-18, + "angularVelocity": 3.185776473374715e-17, + "velocityX": 0.5264675668156642, + "velocityY": -0.17734526731797434, + "timestamp": 8.343200845282464 + }, + { + "x": 2.7588390752001515, + "y": 5.510398898078488, + "heading": 7.631145981641487e-18, + "angularVelocity": 6.13514383217542e-17, + "velocityX": 0.7892183077397765, + "velocityY": -0.267440257701116, + "timestamp": 8.406867653083303 + }, + { + "x": 2.8257885977384563, + "y": 5.48756053072635, + "heading": 1.178722114456003e-17, + "angularVelocity": 6.527852277238055e-17, + "velocityX": 1.0515608501644154, + "velocityY": -0.3587170166216, + "timestamp": 8.470534460884142 + }, + { + "x": 2.909408815467416, + "y": 5.4588202121142055, + "heading": 1.707663826690775e-17, + "angularVelocity": 8.307966592022717e-17, + "velocityX": 1.3134036496778398, + "velocityY": -0.45141761625695104, + "timestamp": 8.534201268684981 + }, + { + "x": 3.0096598952002043, + "y": 5.424066614979104, + "heading": 2.1547525099461292e-17, + "angularVelocity": 7.022319772168498e-17, + "velocityX": 1.5746207984290834, + "velocityY": -0.5458668077692407, + "timestamp": 8.59786807648582 + }, + { + "x": 3.1264905609256615, + "y": 5.383159775106569, + "heading": 2.7044139747893618e-17, + "angularVelocity": 8.633407011044114e-17, + "velocityX": 1.835032566591432, + "velocityY": -0.6425143852114948, + "timestamp": 8.66153488428666 + }, + { + "x": 3.259832418594633, + "y": 5.335918459700519, + "heading": 3.2480422722951275e-17, + "angularVelocity": 8.538645430467616e-17, + "velocityX": 2.0943700850541704, + "velocityY": -0.7420085447636868, + "timestamp": 8.725201692087499 + }, + { + "x": 3.409589859132952, + "y": 5.282098873954419, + "heading": 3.770995869084488e-17, + "angularVelocity": 8.213912631218793e-17, + "velocityX": 2.3522058936390553, + "velocityY": -0.84533193362635, + "timestamp": 8.788868499888338 + }, + { + "x": 3.575620300185956, + "y": 5.221355820749434, + "heading": 4.453751126086424e-17, + "angularVelocity": 1.0723880787820308e-16, + "velocityX": 2.6078021937643934, + "velocityY": -0.9540772547447327, + "timestamp": 8.852535307689177 + }, + { + "x": 3.7576900290728465, + "y": 5.153163867625262, + "heading": 5.330339940674851e-17, + "angularVelocity": 1.3768380178976122e-16, + "velocityX": 2.8597276222240717, + "velocityY": -1.0710754234370974, + "timestamp": 8.916202115490016 + }, + { + "x": 3.955353840328095, + "y": 5.076631092230983, + "heading": 6.003878998986586e-17, + "angularVelocity": 1.0579124061222185e-16, + "velocityX": 3.10466031018196, + "velocityY": -1.2020828126594152, + "timestamp": 8.979868923290855 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 6.251629909739218e-17, + "angularVelocity": 3.89136693523207e-17, + "velocityX": 3.3321165674695394, + "velocityY": -1.361514725761388, + "timestamp": 9.043535731091694 + }, + { + "x": 4.285701078795114, + "y": 4.9383884935900895, + "heading": 6.191867349467079e-17, + "angularVelocity": -1.7257822842751934e-17, + "velocityX": 3.4133567862477086, + "velocityY": -1.4888942181949925, + "timestamp": 9.078164989267158 + }, + { + "x": 4.403715151512854, + "y": 4.881600775413148, + "heading": 5.87936361182216e-17, + "angularVelocity": -9.024268901925555e-17, + "velocityX": 3.4079295640631644, + "velocityY": -1.6398768315856658, + "timestamp": 9.112794247442622 + }, + { + "x": 4.5193670084290005, + "y": 4.8201449553533005, + "heading": 6.393419644700586e-17, + "angularVelocity": 1.484455804018553e-16, + "velocityX": 3.3397151140271815, + "velocityY": -1.7746790805755832, + "timestamp": 9.147423505618086 + }, + { + "x": 4.63247211701513, + "y": 4.7541191698650405, + "heading": 6.969863736825022e-17, + "angularVelocity": 1.664615768552329e-16, + "velocityX": 3.2661718600217573, + "velocityY": -1.9066474122463712, + "timestamp": 9.18205276379355 + }, + { + "x": 4.742850086360494, + "y": 4.6836289301368605, + "heading": 7.573971746769133e-17, + "angularVelocity": 1.7445017357345818e-16, + "velocityX": 3.187419400845542, + "velocityY": -2.035568864080495, + "timestamp": 9.216682021969014 + }, + { + "x": 4.850325224446504, + "y": 4.608787332408603, + "heading": 7.95254691851129e-17, + "angularVelocity": 1.0932234523778094e-16, + "velocityX": 3.1035934278880775, + "velocityY": -2.161224400160131, + "timestamp": 9.251311280144478 + }, + { + "x": 4.959315852672914, + "y": 4.536170440415771, + "heading": 8.132510178394684e-17, + "angularVelocity": 5.1968557625797656e-17, + "velocityX": 3.1473567142028136, + "velocityY": -2.0969808716342646, + "timestamp": 9.285940538319942 + }, + { + "x": 5.071119482307337, + "y": 4.467964000951313, + "heading": 8.196310550599571e-17, + "angularVelocity": 1.842383452283251e-17, + "velocityX": 3.2285886422377668, + "velocityY": -1.969618844240326, + "timestamp": 9.320569796495406 + }, + { + "x": 5.185557951100297, + "y": 4.404277214638869, + "heading": 8.211438156325687e-17, + "angularVelocity": 4.3684463753045605e-18, + "velocityX": 3.3046757228557744, + "velocityY": -1.8391033960284853, + "timestamp": 9.35519905467087 + }, + { + "x": 5.302448704050349, + "y": 4.345211813287043, + "heading": 8.111338797415481e-17, + "angularVelocity": -2.8906007282049283e-17, + "velocityX": 3.37549110517396, + "velocityY": -1.7056502063239702, + "timestamp": 9.389828312846333 + }, + { + "x": 5.421605231445948, + "y": 4.290862097784136, + "heading": 7.868835126918448e-17, + "angularVelocity": -7.002854905419881e-17, + "velocityX": 3.4409205877827787, + "velocityY": -1.5694738601537552, + "timestamp": 9.424457571021797 + }, + { + "x": 5.542837393133187, + "y": 4.2413148237692955, + "heading": 7.613874923845377e-17, + "angularVelocity": -7.362566122075624e-17, + "velocityX": 3.5008593332540854, + "velocityY": -1.4307922440552452, + "timestamp": 9.459086829197261 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 7.348031788343025e-17, + "angularVelocity": -7.676835991047679e-17, + "velocityX": 3.5552114649352746, + "velocityY": -1.289826914239228, + "timestamp": 9.493716087372725 + }, + { + "x": 5.934272394454851, + "y": 4.1243379326499765, + "heading": 6.301578755269389e-17, + "angularVelocity": -1.4241558943688294e-16, + "velocityX": 3.6516732759640362, + "velocityY": -0.9841085621303537, + "timestamp": 9.567194913476962 + }, + { + "x": 6.207753015610448, + "y": 4.075010136678792, + "heading": 7.880077877117389e-17, + "angularVelocity": 2.148236717340586e-16, + "velocityX": 3.721896982508106, + "velocityY": -0.6713198697704936, + "timestamp": 9.6406737395812 + }, + { + "x": 6.484428622538185, + "y": 4.049019979746833, + "heading": 4.7380289374867153e-17, + "angularVelocity": -4.2761283844936273e-16, + "velocityX": 3.7653787029102097, + "velocityY": -0.3537094740066765, + "timestamp": 9.714152565685437 + }, + { + "x": 6.743277361498475, + "y": 4.038426387270388, + "heading": 3.80588957590445e-17, + "angularVelocity": -1.2685822719245393e-16, + "velocityX": 3.5227663897772135, + "velocityY": -0.1441720429966888, + "timestamp": 9.787631391789674 + }, + { + "x": 6.978672820957151, + "y": 4.030025293070568, + "heading": 3.301536102704029e-17, + "angularVelocity": -6.863929378502764e-17, + "velocityX": 3.20358220101046, + "velocityY": -0.11433353858840364, + "timestamp": 9.861110217893911 + }, + { + "x": 7.190552691043286, + "y": 4.022995753598098, + "heading": 2.761165208046759e-17, + "angularVelocity": -7.354103532969467e-17, + "velocityX": 2.8835500146064224, + "velocityY": -0.09566755275184706, + "timestamp": 9.934589043998148 + }, + { + "x": 7.378901756507518, + "y": 4.0170458446186466, + "heading": 2.2049007606542194e-17, + "angularVelocity": -7.570404657768667e-17, + "velocityX": 2.563310758354254, + "velocityY": -0.08097446972017466, + "timestamp": 10.008067870102385 + }, + { + "x": 7.543713628095823, + "y": 4.012025993134882, + "heading": 1.7040082343642412e-17, + "angularVelocity": -6.816828096488101e-17, + "velocityX": 2.2429845484262843, + "velocityY": -0.06831697986905093, + "timestamp": 10.081546696206622 + }, + { + "x": 7.684984887163916, + "y": 4.007845275924109, + "heading": 1.249754397163312e-17, + "angularVelocity": -6.182105257790516e-17, + "velocityX": 1.922611812928072, + "velocityY": -0.05689689713935573, + "timestamp": 10.15502552231086 + }, + { + "x": 7.802713433535344, + "y": 4.004442580511056, + "heading": 8.52630628004598e-18, + "angularVelocity": -5.4046014370140334e-17, + "velocityX": 1.6022104953666365, + "velocityY": -0.046308516255087515, + "timestamp": 10.228504348415097 + }, + { + "x": 7.8968978566663255, + "y": 4.001774009946332, + "heading": 5.220002438781615e-18, + "angularVelocity": -4.499668838635962e-17, + "velocityX": 1.2817899812031746, + "velocityY": -0.03631754487937096, + "timestamp": 10.301983174519334 + }, + { + "x": 7.967537148396796, + "y": 3.9998065067615447, + "heading": 2.6237181268422074e-18, + "angularVelocity": -3.5333775041531244e-17, + "velocityX": 0.9613557466236943, + "velocityY": -0.026776464583087325, + "timestamp": 10.375462000623571 + }, + { + "x": 8.014630554418627, + "y": 3.998514279441312, + "heading": 9.593365131903801e-19, + "angularVelocity": -2.265117315883944e-17, + "velocityX": 0.6409112463912332, + "velocityY": -0.01758639037590862, + "timestamp": 10.448940826727808 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 1.1585643597990433e-28, + "angularVelocity": -1.3055958619386531e-17, + "velocityX": 0.3204587915210466, + "velocityY": -0.008677810201891931, + "timestamp": 10.522419652832046 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 5.650895976930055e-29, + "angularVelocity": -3.8630399964154185e-29, + "velocityX": 1.5845610208926219e-27, + "velocityY": -6.207718033855077e-27, + "timestamp": 10.595898478936283 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 1.2792598362128638, + "isStopPoint": false, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 2.4932366624325915, + "isStopPoint": true, + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 3.424227005183806, + "isStopPoint": true, + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 4.224909036593179, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 4.6758758399070866, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "timestamp": 5.107685831479359, + "isStopPoint": false, + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 5.746338920885271, + "isStopPoint": false, + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 6.870929357086985, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 7.321374265868669, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { + "timestamp": 8.215867229680786, + "isStopPoint": true, "x": 2.6583051681518555, "y": 5.54433536529541, - "heading": 1.7878647482773802e-40, - "angularVelocity": 2.5983819147247646e-39, - "velocityX": 0, - "velocityY": -2.2530838665454385e-40, - "timestamp": 8.21586720621859 - }, - { - "x": 2.6750735555579293, - "y": 5.538716972759925, - "heading": 1.5037626920568013e-19, - "angularVelocity": 2.3619256993662716e-18, - "velocityX": 0.26337722940288844, - "velocityY": -0.08824680774956185, - "timestamp": 8.279534013877285 - }, - { - "x": 2.7085920650608806, - "y": 5.52742596598787, - "heading": 4.62580789697953e-19, - "angularVelocity": 4.9037250644942105e-18, - "velocityX": 0.5264675697678678, - "velocityY": -0.17734526336836984, - "timestamp": 8.34320082153598 - }, - { - "x": 2.7588390755328, - "y": 5.51039889888726, - "heading": 7.215027779377387e-19, - "angularVelocity": 4.066828505487763e-18, - "velocityX": 0.789218312017212, - "velocityY": -0.2674402522565463, - "timestamp": 8.406867629194675 - }, - { - "x": 2.8257885982701296, - "y": 5.487560532002736, - "heading": -1.3111959461224426e-18, - "angularVelocity": -3.192713438621083e-17, - "velocityX": 1.0515608556382001, - "velocityY": -0.35871701007777496, - "timestamp": 8.47053443685337 - }, - { - "x": 2.9094088162269207, - "y": 5.458820213911237, - "heading": -3.133450423089841e-18, - "angularVelocity": -2.8621734683732664e-17, - "velocityX": 1.3134036561886782, - "velocityY": -0.4514176090871532, - "timestamp": 8.534201244512065 - }, - { - "x": 3.009659896203603, - "y": 5.424066617313393, - "heading": -4.721555014357318e-18, - "angularVelocity": -2.494399593240158e-17, - "velocityX": 1.5746208057754199, - "velocityY": -0.5458668005493692, - "timestamp": 8.59786805217076 - }, - { - "x": 3.1264905621724175, - "y": 5.383159777949491, - "heading": -8.056338252897749e-18, - "angularVelocity": -5.237867832823588e-17, - "velocityX": 1.8350325745107399, - "velocityY": -0.6425143786570167, - "timestamp": 8.661534859829455 - }, - { - "x": 3.259832420061778, - "y": 5.335918462965804, - "heading": -1.1096621419747018e-17, - "angularVelocity": -4.775303299558587e-17, - "velocityX": 2.094370093191732, - "velocityY": -0.7420085397863474, - "timestamp": 8.72520166748815 - }, - { - "x": 3.409589860765726, - "y": 5.282098877479992, - "heading": -1.3592361718623002e-17, - "angularVelocity": -3.9200022596627705e-17, - "velocityX": 2.352205901492158, - "velocityY": -0.8453319314253591, - "timestamp": 8.788868475146845 - }, - { - "x": 3.5756203018815467, - "y": 5.221355824269489, - "heading": -1.4313494756802784e-17, - "angularVelocity": -1.132667185145573e-17, - "velocityX": 2.607802200573299, - "velocityY": -0.9540772569615096, - "timestamp": 8.85253528280554 - }, - { - "x": 3.7576900306489676, - "y": 5.153163870724839, - "heading": -1.1221400900164693e-17, - "angularVelocity": 4.856681166133233e-17, - "velocityX": 2.8597276267323055, - "velocityY": -1.0710754324327645, - "timestamp": 8.916202090464235 - }, - { - "x": 3.955353841452516, - "y": 5.076631094268888, - "heading": -7.381711218130638e-18, - "angularVelocity": 6.030912846483351e-17, - "velocityX": 3.104660310018777, - "velocityY": -1.2020828320186705, - "timestamp": 8.97986889812293 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 }, { + "timestamp": 9.043535731091694, + "isStopPoint": false, "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -4.354722543751261e-18, - "angularVelocity": 4.754421944015892e-17, - "velocityX": 3.332116557247906, - "velocityY": -1.3615147608100584, - "timestamp": 9.043535705781625 - }, - { - "x": 4.285701077560922, - "y": 4.938388492372474, - "heading": -4.775397572984342e-18, - "angularVelocity": -1.2147965427412122e-17, - "velocityX": 3.4133567656946284, - "velocityY": -1.4888942599373947, - "timestamp": 9.078164963804028 - }, - { - "x": 4.403715149774036, - "y": 4.881600772971697, - "heading": -7.574216488503896e-18, - "angularVelocity": -8.082237608755476e-17, - "velocityX": 3.4079295645540304, - "velocityY": -1.6398768741750067, - "timestamp": 9.11279422182643 - }, - { - "x": 4.519367006147829, - "y": 4.820144951729341, - "heading": -1.1070492199966305e-17, - "angularVelocity": -1.009630558414108e-16, - "velocityX": 3.3397151131270637, - "velocityY": -1.7746791225673963, - "timestamp": 9.147423479848833 - }, - { - "x": 4.6324721141561005, - "y": 4.754119165100519, - "heading": -1.4960169278634737e-17, - "angularVelocity": -1.123234311330653e-16, - "velocityX": 3.266171857771331, - "velocityY": -1.9066474536101443, - "timestamp": 9.182052737871235 - }, - { - "x": 4.742850082889288, - "y": 4.683628924272633, - "heading": -1.7795441089169654e-17, - "angularVelocity": -8.18750378278602e-17, - "velocityX": 3.187419397256003, - "velocityY": -2.0355689048343195, - "timestamp": 9.216681995893637 - }, - { - "x": 4.850325220314879, - "y": 4.608787325463071, - "heading": -1.8976395039793544e-17, - "angularVelocity": -3.410277950107739e-17, - "velocityX": 3.1035934225348787, - "velocityY": -2.1612244409379606, - "timestamp": 9.25131125391604 - }, - { - "x": 4.959315849281407, - "y": 4.536170434444089, - "heading": -1.7824338333790707e-17, - "angularVelocity": 3.3268304658955216e-17, - "velocityX": 3.1473567494868275, - "velocityY": -2.09698085278076, - "timestamp": 9.285940511938442 - }, - { - "x": 5.0711194796065495, - "y": 4.467963995966004, - "heading": -1.5727172733760973e-17, - "angularVelocity": 6.056051211588329e-17, - "velocityX": 3.228588676454312, - "velocityY": -1.9696188244622874, - "timestamp": 9.320569769960844 - }, - { - "x": 5.1855579490393096, - "y": 4.4042772106470105, - "heading": -1.2316155333836473e-17, - "angularVelocity": 9.850102470338397e-17, - "velocityX": 3.3046757559381787, - "velocityY": -1.839103375469191, - "timestamp": 9.355199027983247 - }, - { - "x": 5.302448702578612, - "y": 4.345211810292891, - "heading": -8.765491689722714e-18, - "angularVelocity": 1.0253363331714457e-16, - "velocityX": 3.3754911371096727, - "velocityY": -1.7056501850518684, - "timestamp": 9.389828286005649 - }, - { - "x": 5.421605230513571, - "y": 4.290862095789443, - "heading": -5.5119830878393785e-18, - "angularVelocity": 9.395259349127813e-17, - "velocityX": 3.4409206185669707, - "velocityY": -1.5694738382292028, - "timestamp": 9.424457544028051 - }, - { - "x": 5.542837392691082, - "y": 4.241314822773421, - "heading": -2.184892589584252e-18, - "angularVelocity": 9.60774411078281e-17, - "velocityX": 3.500859362885684, - "velocityY": -1.4307922215361746, - "timestamp": 9.459086802050454 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { + "timestamp": 9.493716087372725, + "isStopPoint": false, "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 1.1869648608997422e-18, - "angularVelocity": 9.737019049910581e-17, - "velocityX": 3.5552114934161927, - "velocityY": -1.2898268911821018, - "timestamp": 9.493716060072856 - }, - { - "x": 5.934272395491136, - "y": 4.124337934678792, - "heading": 7.497166766936204e-18, - "angularVelocity": 8.587782714305931e-17, - "velocityX": 3.6516733021145136, - "velocityY": -0.9841085377661548, - "timestamp": 9.567194885934677 - }, - { - "x": 6.2077530174936735, - "y": 4.075010140740357, - "heading": 1.3006105833327096e-17, - "angularVelocity": 7.497315045222091e-17, - "velocityX": 3.7218970063133985, - "velocityY": -0.6713198443208217, - "timestamp": 9.640673711796499 - }, - { - "x": 6.4844286250858945, - "y": 4.049019985828209, - "heading": 1.5986683133968542e-17, - "angularVelocity": 4.0563757867422685e-17, - "velocityX": 3.7653787243758563, - "velocityY": -0.3537094476852878, - "timestamp": 9.71415253765832 - }, - { - "x": 6.743277363747608, - "y": 4.038426394199003, - "heading": 1.2719246482061777e-17, - "angularVelocity": -4.446773085420891e-17, - "velocityX": 3.522766397335807, - "velocityY": -0.1441720319419269, - "timestamp": 9.787631363520141 - }, - { - "x": 6.978672822837657, - "y": 4.030025299692623, - "heading": 8.975092196512461e-18, - "angularVelocity": -5.095555408833389e-17, - "velocityX": 3.2035822065626665, - "velocityY": -0.11433354313769856, - "timestamp": 9.861110189381963 - }, - { - "x": 7.1905526925662695, - "y": 4.022995759457438, - "heading": 5.368720420708233e-18, - "angularVelocity": -4.9080421924352456e-17, - "velocityX": 2.8835500192539434, - "velocityY": -0.09566756344750614, - "timestamp": 9.934589015243784 - }, - { - "x": 7.3789017577018114, - "y": 4.017045849522277, - "heading": 1.5887833889908863e-18, - "angularVelocity": -5.144253446326957e-17, - "velocityX": 2.563310762337649, - "velocityY": -0.08097448299391075, - "timestamp": 10.008067841105605 - }, - { - "x": 7.54371362899692, - "y": 4.012025997027571, - "heading": -2.7250521222406746e-18, - "angularVelocity": -5.870855257463982e-17, - "velocityX": 2.2429845518359373, - "velocityY": -0.06831699385270525, - "timestamp": 10.081546666967427 - }, - { - "x": 7.684984887810538, - "y": 4.007845278835319, - "heading": -2.10421445779977e-18, - "angularVelocity": 8.449205021435769e-18, - "velocityX": 1.9226118158077505, - "velocityY": -0.056896910684361174, - "timestamp": 10.155025492829248 - }, - { - "x": 7.8027134339680275, - "y": 4.0044425825275765, - "heading": -1.435402908569355e-18, - "angularVelocity": 9.102099024937298e-18, - "velocityX": 1.6022104977409413, - "velocityY": -0.0463085285840207, - "timestamp": 10.22850431869107 - }, - { - "x": 7.896897856926731, - "y": 4.001774011196325, - "heading": -8.461914935171916e-19, - "angularVelocity": 8.01879191918765e-18, - "velocityX": 1.2817899830873647, - "velocityY": -0.03631755543113603, - "timestamp": 10.30198314455289 - }, - { - "x": 7.967537148527336, - "y": 3.999806507404438, - "heading": -3.275924545852618e-19, - "angularVelocity": 7.057802473697343e-18, - "velocityX": 0.9613557480279206, - "velocityY": -0.026776472933672434, - "timestamp": 10.375461970414712 - }, - { - "x": 8.014630554462236, - "y": 3.9985142796609896, - "heading": -5.3367647403100726e-20, - "angularVelocity": 3.732024892420789e-18, - "velocityX": 0.6409112473226126, - "velocityY": -0.01758639619362703, - "timestamp": 10.448940796276533 - }, - { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 7.342940745159904e-44, - "angularVelocity": 7.262996758203436e-19, - "velocityX": 0.320458791984776, - "velocityY": -0.008677813220192914, - "timestamp": 10.522419622138354 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 }, { + "timestamp": 10.595898478936283, + "isStopPoint": true, "x": 8.038177490234375, "y": 3.9978766441345215, - "heading": 1.3038141825006824e-44, - "angularVelocity": -8.034632634038792e-43, - "velocityX": -5.7118786505402475e-43, - "velocityY": 0, - "timestamp": 10.595898448000176 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -23819,37 +24941,44 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 3 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 2 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 10 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, "SourceLanePGCSprint": { "waypoints": [ @@ -23966,1752 +25095,1700 @@ { "x": 0.433241993188858, "y": 4.121134281158447, - "heading": 9.759657159056985e-29, - "angularVelocity": 1.5485227916478622e-29, - "velocityX": -1.3617494152189479e-27, - "velocityY": -5.3557690609839536e-27, + "heading": 7.738396447124189e-29, + "angularVelocity": 1.1248344329069217e-28, + "velocityX": -3.4263332588426173e-28, + "velocityY": 2.738469239483736e-27, "timestamp": 0 }, { - "x": 0.4546918803213756, - "y": 4.109218526997661, - "heading": -0.008937300145995024, - "angularVelocity": -0.11893237262991248, - "velocityX": 0.28544257523423766, - "velocityY": -0.15856789979825608, - "timestamp": 0.07514606787342623 - }, - { - "x": 0.4975916635116252, - "y": 4.0853873108981436, - "heading": -0.02681241329663613, - "angularVelocity": -0.23787157008333967, - "velocityX": 0.5708852692400183, - "velocityY": -0.3171319108760033, - "timestamp": 0.15029213574685246 - }, - { - "x": 0.5619414537480726, - "y": 4.049641027140498, - "heading": -0.05362171709802378, - "angularVelocity": -0.3567625633658495, - "velocityX": 0.8563294402155045, - "velocityY": -0.4756906750976654, - "timestamp": 0.2254382036202787 - }, - { - "x": 0.6477414523773108, - "y": 4.001980177860296, - "heading": -0.08935853104911338, - "angularVelocity": -0.4755646564406219, - "velocityX": 1.1417762905939028, - "velocityY": -0.6342427571923064, - "timestamp": 0.3005842714937049 - }, - { - "x": 0.7549919358777417, - "y": 3.9424053758532884, - "heading": -0.13401442200652888, - "angularVelocity": -0.5942545261667251, - "velocityX": 1.4272268201854599, - "velocityY": -0.7927866845588466, - "timestamp": 0.3757303393671312 - }, - { - "x": 0.8836932393269187, - "y": 3.8709173416225426, - "heading": -0.18758057158075359, - "angularVelocity": -0.7128270459134325, - "velocityX": 1.7126818087934759, - "velocityY": -0.9513210238912064, - "timestamp": 0.45087640724055744 - }, - { - "x": 1.0338457421402332, - "y": 3.7875168932975942, - "heading": -0.2500489995023012, - "angularVelocity": -0.8312933688928013, - "velocityX": 1.9981418464400145, - "velocityY": -1.1098444760333412, - "timestamp": 0.5260224751139837 - }, - { - "x": 1.2054498603190875, - "y": 3.692204931169302, - "heading": -0.3214134496146474, - "angularVelocity": -0.9496764385935714, - "velocityX": 2.283607419990346, - "velocityY": -1.268355947630322, - "timestamp": 0.60116854298741 - }, - { - "x": 1.3985060453231266, - "y": 3.584982424979261, - "heading": -0.40166985798592575, - "angularVelocity": -1.0680054278616389, - "velocityX": 2.569079001302066, - "velocityY": -1.4268545144723166, - "timestamp": 0.6763146108608362 - }, - { - "x": 1.5915937569821017, - "y": 3.4778198154599655, - "heading": -0.48176859267037747, - "angularVelocity": -1.0659071984893158, - "velocityX": 2.5694985396202785, - "velocityY": -1.4260574445464886, - "timestamp": 0.7514606787342625 - }, - { - "x": 1.7632284858900344, - "y": 3.382566036520045, - "heading": -0.552976547680448, - "angularVelocity": -0.9475938931363788, - "velocityX": 2.2840147697019764, - "velocityY": -1.2675816797275836, - "timestamp": 0.8266067466076887 - }, - { - "x": 1.9134098098562773, - "y": 3.299220400739088, - "heading": -0.6152918418017485, - "angularVelocity": -0.8292555536806328, - "velocityX": 1.9985253815170176, - "velocityY": -1.1091150626981823, - "timestamp": 0.901752814481115 - }, - { - "x": 2.04213735776467, - "y": 3.2277823059512354, - "heading": -0.6687115003742338, - "angularVelocity": -0.7108776291856513, - "velocityX": 1.7130310547348613, - "velocityY": -0.9506564589404838, - "timestamp": 0.9768988823545413 - }, - { - "x": 2.1494108084857926, - "y": 3.1682512394298143, - "heading": -0.7132320418113052, - "angularVelocity": -0.5924533737688112, - "velocityX": 1.4275324545498602, - "velocityY": -0.7922046782500088, - "timestamp": 1.0520449502279674 - }, - { - "x": 2.235229893671302, - "y": 3.120626784362015, - "heading": -0.7488501306819726, - "angularVelocity": -0.4739847323836216, - "velocityX": 1.142030283341778, - "velocityY": -0.6337584442610695, - "timestamp": 1.1271910181013935 - }, - { - "x": 2.2995943987852447, - "y": 3.0849086278583613, - "heading": -0.7755632624607874, - "angularVelocity": -0.35548276223593744, - "velocityX": 0.8565252572144713, - "velocityY": -0.4753163740226105, - "timestamp": 1.2023370859748197 - }, - { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080856, - "angularVelocity": -0.2369668826490278, - "velocityX": 0.5710180813549185, - "velocityY": -0.31687698560575434, - "timestamp": 1.2774831538482458 + "x": 0.4546918803218127, + "y": 4.109218526998405, + "heading": -0.00893730014505659, + "angularVelocity": -0.11893237232009289, + "velocityX": 0.28544257452644645, + "velocityY": -0.1585678993919355, + "timestamp": 0.07514606806129173 + }, + { + "x": 0.49759166351299144, + "y": 4.085387310900469, + "heading": -0.026812413293685552, + "angularVelocity": -0.23787156946188326, + "velocityX": 0.5708852678251672, + "velocityY": -0.3171319100621368, + "timestamp": 0.15029213612258346 + }, + { + "x": 0.5619414537509306, + "y": 4.049641027145361, + "heading": -0.05362171709181762, + "angularVelocity": -0.35676256243061816, + "velocityX": 0.8563294380945293, + "velocityY": -0.47569067387467856, + "timestamp": 0.2254382041838752 + }, + { + "x": 0.6477414523823173, + "y": 4.001980177868811, + "heading": -0.08935853103818607, + "angularVelocity": -0.4755646551888817, + "velocityX": 1.1417762877680475, + "velocityY": -0.634242755558099, + "timestamp": 0.30058427224516693 + }, + { + "x": 0.7549919358856848, + "y": 3.942405375866793, + "heading": -0.13401442198910787, + "angularVelocity": -0.5942545245946725, + "velocityX": 1.4272268166564648, + "velocityY": -0.7927866825104752, + "timestamp": 0.3757303403064587 + }, + { + "x": 0.8836932393387835, + "y": 3.870917341642709, + "heading": -0.18758057155461047, + "angularVelocity": -0.7128270440152931, + "velocityX": 1.7126818045639527, + "velocityY": -0.9513210214242487, + "timestamp": 0.45087640836775045 + }, + { + "x": 1.0338457421573335, + "y": 3.7875168933266514, + "heading": -0.2500489994644535, + "angularVelocity": -0.8312933666588067, + "velocityX": 1.998141841514319, + "velocityY": -1.1098444731404118, + "timestamp": 0.5260224764290422 + }, + { + "x": 1.2054498603433927, + "y": 3.692204931210594, + "heading": -0.3214134495606155, + "angularVelocity": -0.9496764360040053, + "velocityX": 2.2836074143771987, + "velocityY": -1.2683559442966215, + "timestamp": 0.601168544490334 + }, + { + "x": 1.3985060453585743, + "y": 3.5849824250394744, + "heading": -0.4016698579067676, + "angularVelocity": -1.068005424857256, + "velocityX": 2.569078995027635, + "velocityY": -1.4268545106533708, + "timestamp": 0.6763146125516257 + }, + { + "x": 1.5915937570064054, + "y": 3.477819815501255, + "heading": -0.4817685926160567, + "angularVelocity": -1.0659071961550632, + "velocityX": 2.5694985330482245, + "velocityY": -1.426057441233171, + "timestamp": 0.7514606806129175 + }, + { + "x": 1.7632284859071354, + "y": 3.3825660365490977, + "heading": -0.5529765476421505, + "angularVelocity": -0.9475938909806194, + "velocityX": 2.2840147638960824, + "velocityY": -1.2675816767214638, + "timestamp": 0.8266067486742092 + }, + { + "x": 1.9134098098681454, + "y": 3.2992204007592485, + "heading": -0.6152918417750989, + "angularVelocity": -0.8292555517624935, + "velocityX": 1.9985253764510553, + "velocityY": -1.1091150600437194, + "timestamp": 0.901752816735501 + }, + { + "x": 2.042137357772617, + "y": 3.227782305964734, + "heading": -0.6687115003563293, + "angularVelocity": -0.7108776275248302, + "velocityX": 1.7130310504001012, + "velocityY": -0.9506564566524899, + "timestamp": 0.9768988847967928 + }, + { + "x": 2.1494108084908032, + "y": 3.168251239438324, + "heading": -0.7132320417999736, + "angularVelocity": -0.5924533723751416, + "velocityX": 1.4275324509419445, + "velocityY": -0.792204676335884, + "timestamp": 1.0520449528580844 + }, + { + "x": 2.2352298936741635, + "y": 3.120626784366874, + "heading": -0.7488501306754739, + "angularVelocity": -0.4739847312629708, + "velocityX": 1.1420302804580957, + "velocityY": -0.6337584427252518, + "timestamp": 1.127191020919376 + }, + { + "x": 2.299594398786613, + "y": 3.0849086278606843, + "heading": -0.7755632624576652, + "angularVelocity": -0.3554827613921602, + "velocityX": 0.8565252550532846, + "velocityY": -0.475316372868058, + "timestamp": 1.2023370889806677 + }, + { + "x": 2.3425041622841327, + "y": 3.061096568391248, + "heading": -0.7933703919070813, + "angularVelocity": -0.2369668820847947, + "velocityX": 0.5710180799149875, + "velocityY": -0.31687698483458104, + "timestamp": 1.2774831570419594 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550668, - "velocityX": 0.28550943973314763, - "velocityY": -0.15843872456938438, - "timestamp": 1.352629221721672 + "angularVelocity": -0.11846288571271453, + "velocityX": 0.2855094390135433, + "velocityY": -0.1584387241831784, + "timestamp": 1.352629225103251 }, { "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, - "angularVelocity": -1.820153695476826e-18, - "velocityX": -5.7963239407122954e-18, - "velocityY": -1.1530643297922074e-18, - "timestamp": 1.4277752895950981 - }, - { - "x": 2.37735740952237, - "y": 3.0408026970784268, - "heading": -0.7860869247798871, - "angularVelocity": 0.2648554274741457, - "velocityX": 0.21924714610297322, - "velocityY": -0.1372563412236913, - "timestamp": 1.4888859391070042 - }, - { - "x": 2.404179793787175, - "y": 3.024019444447085, - "heading": -0.7541504516362038, - "angularVelocity": 0.5226007806947081, - "velocityX": 0.4389150578342217, - "velocityY": -0.2746371175137346, - "timestamp": 1.5499965886189102 - }, - { - "x": 2.4444554974169383, - "y": 2.998832316814256, - "heading": -0.7069723408476191, - "angularVelocity": 0.7720112806098135, - "velocityX": 0.6590619466729163, - "velocityY": -0.41215611082519754, - "timestamp": 1.6111072381308162 - }, - { - "x": 2.4982188560395806, - "y": 2.96523040511413, - "heading": -0.645190074686331, - "angularVelocity": 1.0109901736398696, - "velocityX": 0.8797706954852063, - "velocityY": -0.5498536174710421, - "timestamp": 1.6722178876427223 - }, - { - "x": 2.5655102024042766, - "y": 2.923198127539445, - "heading": -0.5696285601591381, - "angularVelocity": 1.2364704864161304, - "velocityX": 1.101139439723772, - "velocityY": -0.6878061010707456, - "timestamp": 1.7333285371546283 - }, - { - "x": 2.646375841205288, - "y": 2.8727129664624207, - "heading": -0.48135684674506, - "angularVelocity": 1.4444571301255718, - "velocityX": 1.3232659028645477, - "velocityY": -0.8261270577264651, - "timestamp": 1.7944391866665343 - }, - { - "x": 2.740867207337065, - "y": 2.813744425609022, - "heading": -0.38173698900245784, - "angularVelocity": 1.6301554399809315, - "velocityX": 1.5462340342720005, - "velocityY": -0.9649470480903679, - "timestamp": 1.8555498361784404 - }, - { - "x": 2.849040211493812, - "y": 2.746255741762665, - "heading": -0.2724850390795677, - "angularVelocity": 1.7877726843928354, - "velocityX": 1.7701170748589676, - "velocityY": -1.1043686229060403, - "timestamp": 1.9166604856903464 - }, - { - "x": 2.9709557136972373, - "y": 2.6702088910620962, - "heading": -0.155828372393994, - "angularVelocity": 1.9089416921161393, - "velocityX": 1.994996014232719, - "velocityY": -1.2444124110602532, - "timestamp": 1.9777711352022525 - }, - { - "x": 3.1066769296559538, - "y": 2.585572206493352, - "heading": -0.03496320783780511, - "angularVelocity": 1.977808541089754, - "velocityX": 2.2209094002882996, - "velocityY": -1.3849743906298195, - "timestamp": 2.0388817847141585 - }, - { - "x": 3.2562355055408387, - "y": 2.4923383100856262, - "heading": 0.08475597327951304, - "angularVelocity": 1.959055943170691, - "velocityX": 2.4473406366879895, - "velocityY": -1.5256571015427038, - "timestamp": 2.0999924342260647 - }, - { - "x": 3.4194188611023764, - "y": 2.3906298265628587, - "heading": 0.19293288100296513, - "angularVelocity": 1.7701809518876104, - "velocityX": 2.6702932609110017, - "velocityY": -1.6643332109070865, - "timestamp": 2.161103083737971 - }, - { - "x": 3.5932872633720114, - "y": 2.2801851899216916, - "heading": 0.26578166446591744, - "angularVelocity": 1.192080006427692, - "velocityX": 2.845140800471438, - "velocityY": -1.8072895235657866, - "timestamp": 2.2222137332498773 - }, - { - "x": 3.7772765002319786, - "y": 2.1608722925529302, - "heading": 0.30020319888903974, - "angularVelocity": 0.5632657269731189, - "velocityX": 3.0107557083666587, - "velocityY": -1.9524076134310657, - "timestamp": 2.2833243827617835 - }, - { - "x": 3.9732507395108834, - "y": 2.038358982543431, - "heading": 0.30020324669419357, - "angularVelocity": 7.822720631834703e-7, - "velocityX": 3.2068754111462736, - "velocityY": -2.0047783976771947, - "timestamp": 2.3444350322736898 - }, - { - "x": 4.174667645417682, - "y": 1.9250159473015482, - "heading": 0.3002032939445946, - "angularVelocity": 7.731942209741845e-7, - "velocityX": 3.2959379014218597, - "velocityY": -1.8547182225546543, - "timestamp": 2.405545681785596 - }, - { - "x": 4.383569049898422, - "y": 1.8261442295125712, - "heading": 0.30020334201253857, - "angularVelocity": 7.865722968351813e-7, - "velocityX": 3.4184124395542623, - "velocityY": -1.6179130573585891, - "timestamp": 2.4666563312975023 - }, - { - "x": 4.59891708073721, - "y": 1.7422357169202292, - "heading": 0.3002033920988207, - "angularVelocity": 8.195998987503142e-7, - "velocityX": 3.52390348587004, - "velocityY": -1.3730587591937538, - "timestamp": 2.5277669808094085 - }, - { - "x": 4.819641568305972, - "y": 1.6737075280608862, - "heading": 0.30020344564176327, - "angularVelocity": 8.761638607213102e-7, - "velocityX": 3.6118825332687328, - "velocityY": -1.1213788334223447, - "timestamp": 2.588877630321315 - }, - { - "x": 5.044645579459654, - "y": 1.6209002757550093, - "heading": 0.30020350452029126, - "angularVelocity": 9.634740995599556e-7, - "velocityX": 3.681911629982681, - "velocityY": -0.8641252012153561, - "timestamp": 2.649988279833221 - }, - { - "x": 5.272810899090047, - "y": 1.584076421741111, - "heading": 0.3002035713939988, - "angularVelocity": 0.0000010943052979537505, - "velocityX": 3.733642523075129, - "velocityY": -0.6025767081190017, - "timestamp": 2.7110989293451273 + "angularVelocity": 1.9944978930198404e-24, + "velocityX": 7.05107927160795e-24, + "velocityY": 9.089761159537798e-24, + "timestamp": 1.4277752931645427 + }, + { + "x": 2.3773573652919286, + "y": 3.040801892787579, + "heading": -0.7860925568420652, + "angularVelocity": 0.2647627943666689, + "velocityX": 0.2192460319725613, + "velocityY": -0.13726925804658435, + "timestamp": 1.4888860514802038 + }, + { + "x": 2.4041795588293358, + "y": 3.024017002068538, + "heading": -0.754166261219504, + "angularVelocity": 0.5224333080216316, + "velocityX": 0.43891115536253184, + "velocityY": -0.27466343376629077, + "timestamp": 1.549996809795865 + }, + { + "x": 2.4444548115194884, + "y": 2.998827368062219, + "heading": -0.7070016065582421, + "angularVelocity": 0.7717897136480927, + "velocityX": 0.6590533942013143, + "velocityY": -0.4121963906290397, + "timestamp": 1.6111075681115261 + }, + { + "x": 2.4982173316537715, + "y": 2.9652220428664715, + "heading": -0.6452345488674253, + "angularVelocity": 1.010739506320076, + "velocityX": 0.8797554083125275, + "velocityY": -0.5499084960157551, + "timestamp": 1.6722183264271873 + }, + { + "x": 2.565507310796468, + "y": 2.9231853998386073, + "heading": -0.5696881555564717, + "angularVelocity": 1.2362208454479753, + "velocityX": 1.1011151063633902, + "velocityY": -0.687876311577224, + "timestamp": 1.7333290847428484 + }, + { + "x": 2.646370899659449, + "y": 2.872694862845298, + "heading": -0.4814293119182948, + "angularVelocity": 1.444243960814317, + "velocityX": 1.3232300022409869, + "velocityY": -0.8262135569077015, + "timestamp": 1.7944398430585096 + }, + { + "x": 2.740859365850477, + "y": 2.8137198450437673, + "heading": -0.38181759469062915, + "angularVelocity": 1.6300193283992952, + "velocityX": 1.5461838274524098, + "velocityY": -0.9650513171003614, + "timestamp": 1.8555506013741707 + }, + { + "x": 2.8490284341196963, + "y": 2.7462234286806457, + "heading": -0.27256619156885004, + "angularVelocity": 1.787760553672932, + "velocityX": 1.7700495174758504, + "velocityY": -1.1044931894720724, + "timestamp": 1.9166613596898319 + }, + { + "x": 2.97093875412634, + "y": 2.6701673256371343, + "heading": -0.15589887958558987, + "angularVelocity": 1.909112490154803, + "velocityX": 1.994907662198022, + "velocityY": -1.2445615983138625, + "timestamp": 1.977772118005493 + }, + { + "x": 3.1066533293644194, + "y": 2.5855194210396055, + "heading": -0.03500682401650803, + "angularVelocity": 1.9782450570262398, + "velocityX": 2.220796779137645, + "velocityY": -1.3851555263033848, + "timestamp": 2.038882876321154 + }, + { + "x": 3.2562037135860464, + "y": 2.492271514146211, + "heading": 0.08476246781347457, + "angularVelocity": 1.959872453412, + "velocityX": 2.4472022331835617, + "velocityY": -1.5258836490251428, + "timestamp": 2.099993634636815 + }, + { + "x": 3.419377549511949, + "y": 2.390544477111736, + "heading": 0.19301716737769203, + "angularVelocity": 1.7714507649379716, + "velocityX": 2.6701327298713187, + "velocityY": -1.6646338523409365, + "timestamp": 2.1611043929524762 + }, + { + "x": 3.5932192698224203, + "y": 2.280069793657614, + "heading": 0.2658871078344446, + "angularVelocity": 1.1924240913580317, + "velocityX": 2.8446991184843475, + "velocityY": -1.8077779837631391, + "timestamp": 2.2222151512681374 + }, + { + "x": 3.777176249725073, + "y": 2.160717630889141, + "heading": 0.3003277545805106, + "angularVelocity": 0.5635774730231045, + "velocityX": 3.010222503743814, + "velocityY": -1.9530466657273664, + "timestamp": 2.2833259095837986 + }, + { + "x": 3.9732480656956923, + "y": 2.03835976913752, + "heading": 0.3003278017850349, + "angularVelocity": 7.724421305582541e-7, + "velocityX": 3.208466420230474, + "velocityY": -2.0022311148487826, + "timestamp": 2.3444366678994597 + }, + { + "x": 4.174665289116757, + "y": 1.9250164613959089, + "heading": 0.3003278489820802, + "angularVelocity": 7.723197446110156e-7, + "velocityX": 3.295937228935466, + "velocityY": -1.8547193794609556, + "timestamp": 2.405547426215121 + }, + { + "x": 4.383567042240086, + "y": 1.82614452102736, + "heading": 0.30032789699541584, + "angularVelocity": 7.856773019778577e-7, + "velocityX": 3.4184120583853495, + "velocityY": -1.6179138190011713, + "timestamp": 2.466658184530782 + }, + { + "x": 4.598915447104952, + "y": 1.7422358380933087, + "heading": 0.30032794702414056, + "angularVelocity": 8.186565851278932e-7, + "velocityX": 3.5239033322497164, + "velocityY": -1.3730591019772607, + "timestamp": 2.527768942846443 + }, + { + "x": 4.819640328386772, + "y": 1.6737075334125113, + "heading": 0.30032800050450253, + "angularVelocity": 8.751382481764384e-7, + "velocityX": 3.611882545159867, + "velocityY": -1.1213787321509214, + "timestamp": 2.5888797011621043 + }, + { + "x": 5.044644747071726, + "y": 1.620900221581336, + "heading": 0.30032805931258105, + "angularVelocity": 9.623195678675265e-7, + "velocityX": 3.681911743309069, + "velocityY": -0.8641246367522506, + "timestamp": 2.6499904594777655 + }, + { + "x": 5.27281048204291, + "y": 1.5840763655637535, + "heading": 0.3003281261036848, + "angularVelocity": 0.0000010929516437587844, + "velocityX": 3.7336426720908507, + "velocityY": -0.6025756680578687, + "timestamp": 2.7111012177934266 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": 0.3002036452391599, - "angularVelocity": 0.0000012083844913604929, - "velocityX": 3.7668180588495743, - "velocityY": -0.33803334595452383, - "timestamp": 2.7722095788570336 - }, - { - "x": 5.642203177619129, - "y": 1.5568731039887571, - "heading": 0.3002037251666547, - "angularVelocity": 0.0000021691766198010436, - "velocityX": 3.7777797970827858, - "velocityY": -0.177650642031358, - "timestamp": 2.8090565069037923 - }, - { - "x": 5.781555187145968, - "y": 1.5562486567630798, - "heading": 0.30020379895395954, - "angularVelocity": 0.0000020025361337225055, - "velocityX": 3.78191661866635, - "velocityY": -0.016947063399286564, - "timestamp": 2.845903434950551 - }, - { - "x": 5.9208078464820115, - "y": 1.5615467685843438, - "heading": 0.30020386782175307, - "angularVelocity": 0.0000018690240183201108, - "velocityX": 3.7792203235866277, - "velocityY": 0.14378706997068585, - "timestamp": 2.8827503629973097 - }, - { - "x": 6.059709556234372, - "y": 1.5727578639536914, - "heading": 0.3002039327408759, - "angularVelocity": 0.0000017618598429234572, - "velocityX": 3.7696957959723445, - "velocityY": 0.30426133096145186, - "timestamp": 2.9195972910440684 - }, - { - "x": 6.198009351638548, - "y": 1.5898616832743162, - "heading": 0.30020399449987484, - "angularVelocity": 0.0000016760962775678482, - "velocityX": 3.753360259196494, - "velocityY": 0.46418576058553174, - "timestamp": 2.956444219090827 - }, - { - "x": 6.335457356092555, - "y": 1.6128273194464042, - "heading": 0.30020405375523124, - "angularVelocity": 0.0000016081491612309129, - "velocityX": 3.730243245233006, - "velocityY": 0.6232713930166671, - "timestamp": 2.993291147137586 - }, - { - "x": 6.471805232718414, - "y": 1.6416132738069629, - "heading": 0.3002041110675489, - "angularVelocity": 0.0000015554164405322294, - "velocityX": 3.700386541120455, - "velocityY": 0.7812307805966762, - "timestamp": 3.0301380751843445 - }, - { - "x": 6.60680663253268, - "y": 1.676167531284266, - "heading": 0.3002041691027596, - "angularVelocity": 0.0000015750352547385845, - "velocityX": 3.6638440969339445, - "velocityY": 0.937778515306732, - "timestamp": 3.0669850032311032 - }, - { - "x": 6.740176057879373, - "y": 1.7164157557118485, - "heading": 0.3003755003099619, - "angularVelocity": 0.004649809801927131, - "velocityX": 3.6195534449289073, - "velocityY": 1.0923088181600296, - "timestamp": 3.103831931277862 - }, - { - "x": 6.869837525657242, - "y": 1.7607930294165437, - "heading": 0.3093430143729343, - "angularVelocity": 0.2433720947264084, - "velocityX": 3.5189220554106844, - "velocityY": 1.2043683437702197, - "timestamp": 3.1406788593246207 - }, - { - "x": 6.995165288352465, - "y": 1.807928452248917, - "heading": 0.3301850856624388, - "angularVelocity": 0.5656393190514163, - "velocityX": 3.4013083135772337, - "velocityY": 1.2792225927914125, - "timestamp": 3.1775257873713794 - }, - { - "x": 7.114963240628345, - "y": 1.8557254140007795, - "heading": 0.35927322814233364, - "angularVelocity": 0.7894319559824341, - "velocityX": 3.2512331048020355, - "velocityY": 1.2971762989633377, - "timestamp": 3.214372715418138 - }, - { - "x": 7.228886146559884, - "y": 1.9032191557267804, - "heading": 0.3911508310575271, - "angularVelocity": 0.8651359721153189, - "velocityX": 3.091788433135363, - "velocityY": 1.2889471183522156, - "timestamp": 3.251219643464897 - }, - { - "x": 7.336942556890349, - "y": 1.949894399055616, - "heading": 0.423151819771819, - "angularVelocity": 0.8684845768875223, - "velocityX": 2.9325758226938556, - "velocityY": 1.2667336411221237, - "timestamp": 3.2880665715116555 - }, - { - "x": 7.43919043499382, - "y": 1.9954619855368936, - "heading": 0.45375826572504047, - "angularVelocity": 0.8306376562622595, - "velocityX": 2.7749362979111796, - "velocityY": 1.2366726046592738, - "timestamp": 3.324913499558414 - }, - { - "x": 7.535687560612936, - "y": 2.0397400223416313, - "heading": 0.48197582868140687, - "angularVelocity": 0.7658050332054913, - "velocityX": 2.618864875157601, - "velocityY": 1.201675123325048, - "timestamp": 3.361760427605173 + "heading": 0.3003281998542592, + "angularVelocity": 0.0000012068345473522654, + "velocityX": 3.7668181767205913, + "velocityY": -0.3380318248368492, + "timestamp": 2.7722119761090878 + }, + { + "x": 5.642202615777798, + "y": 1.556873162710076, + "heading": 0.30032827970972786, + "angularVelocity": 0.0000021672306415301637, + "velocityX": 3.777779819586101, + "velocityY": -0.17764976646888633, + "timestamp": 2.809058755213733 + }, + { + "x": 5.781554061367138, + "y": 1.556248726330051, + "heading": 0.3003283534295918, + "angularVelocity": 0.0000020007139214038515, + "velocityX": 3.781916601002728, + "velocityY": -0.016946837558083135, + "timestamp": 2.8459055343183786 + }, + { + "x": 5.920806157723901, + "y": 1.5615468010933287, + "heading": 0.30032842223361694, + "angularVelocity": 0.000001867300937043874, + "velocityX": 3.7792203210295474, + "velocityY": 0.14378664545497588, + "timestamp": 2.882752313423024 + }, + { + "x": 6.059707308506225, + "y": 1.5727578116463863, + "heading": 0.3003284870919326, + "angularVelocity": 0.000001760216693496236, + "velocityX": 3.7696958637237334, + "velocityY": 0.3042602589826913, + "timestamp": 2.9195990925276694 + }, + { + "x": 6.198006551988053, + "y": 1.5898614987106987, + "heading": 0.3003285487925156, + "angularVelocity": 0.0000016745176784450775, + "velocityX": 3.753360452186466, + "velocityY": 0.4641840475591571, + "timestamp": 2.9564458716323148 + }, + { + "x": 6.335454014584064, + "y": 1.6128269556768218, + "heading": 0.30032860799137623, + "angularVelocity": 0.0000016066223984499512, + "velocityX": 3.730243617919986, + "velocityY": 0.6232690488604353, + "timestamp": 2.99329265073696 + }, + { + "x": 6.47180136240203, + "y": 1.641612684542814, + "heading": 0.30032866524871465, + "angularVelocity": 0.0000015539306228254143, + "velocityX": 3.700387147292773, + "velocityY": 0.7812278186986222, + "timestamp": 3.0301394298416056 + }, + { + "x": 6.606802249418837, + "y": 1.6761666710700263, + "heading": 0.30032872317249165, + "angularVelocity": 0.0000015720173768370867, + "velocityX": 3.6638449899081134, + "velocityY": 0.937774952569887, + "timestamp": 3.066986208946251 + }, + { + "x": 6.740172242433044, + "y": 1.7164148916635715, + "heading": 0.30049565594915995, + "angularVelocity": 0.004530457769298999, + "velocityX": 3.619583482057768, + "velocityY": 1.0923131294390627, + "timestamp": 3.1038329880508964 + }, + { + "x": 6.869834708516544, + "y": 1.760792645305141, + "heading": 0.3094576493704555, + "angularVelocity": 0.2432232515043802, + "velocityX": 3.5189633730334187, + "velocityY": 1.2043862372756071, + "timestamp": 3.1406797671555418 + }, + { + "x": 6.995163652975955, + "y": 1.807928879541571, + "heading": 0.3302942112120631, + "angularVelocity": 0.5654920822911383, + "velocityX": 3.4013541347392664, + "velocityY": 1.2792497846979316, + "timestamp": 3.177526546260187 + }, + { + "x": 7.114961828122759, + "y": 1.8557263226321454, + "heading": 0.3593656658524349, + "angularVelocity": 0.7889822488366823, + "velocityX": 3.2512522955283503, + "velocityY": 1.2971946056622388, + "timestamp": 3.2143733253648326 + }, + { + "x": 7.2288849177114916, + "y": 1.9032201895276672, + "heading": 0.39122292476577963, + "angularVelocity": 0.8645873448767251, + "velocityX": 3.091805915116442, + "velocityY": 1.2889557255639152, + "timestamp": 3.251220104469478 + }, + { + "x": 7.3369415857594324, + "y": 1.9498953701157864, + "heading": 0.4232031564001013, + "angularVelocity": 0.8679247524864317, + "velocityX": 2.932594671058162, + "velocityY": 1.2667370587687046, + "timestamp": 3.2880668835741234 + }, + { + "x": 7.4391897785347085, + "y": 1.9954627576139183, + "heading": 0.4537900488962921, + "angularVelocity": 0.8301103444977762, + "velocityX": 2.774956054771842, + "velocityY": 1.236672203253364, + "timestamp": 3.3249136626787688 + }, + { + "x": 7.535687239808996, + "y": 2.0397404710658504, + "heading": 0.48199026779702103, + "angularVelocity": 0.7653374212340174, + "velocityX": 2.618884570622394, + "velocityY": 1.2016712051325453, + "timestamp": 3.361760441783414 }, { "x": 7.626483917236328, "y": 2.082604169845581, "heading": 0.507098504392337, - "angularVelocity": 0.6818119458710672, - "velocityX": 2.464149969521805, - "velocityY": 1.163303151067446, - "timestamp": 3.3986073556519316 - }, - { - "x": 7.766065583230135, - "y": 2.1519617180113952, - "heading": 0.5396264377138885, - "angularVelocity": 0.5126079654708053, - "velocityX": 2.1996686083558368, - "velocityY": 1.0930061650047849, - "timestamp": 3.4620631259727794 - }, - { - "x": 7.889178097615864, - "y": 2.215679412144434, - "heading": 0.5626262771579723, - "angularVelocity": 0.3624546566496878, - "velocityX": 1.9401311143689866, - "velocityY": 1.004127659484183, - "timestamp": 3.5255188962936272 - }, - { - "x": 7.996082408378713, - "y": 2.2729729622929304, - "heading": 0.5769453477724344, - "angularVelocity": 0.22565435014123394, - "velocityX": 1.6847059018008188, - "velocityY": 0.90288952224213, - "timestamp": 3.588974666614475 - }, - { - "x": 8.086990111713977, - "y": 2.3232868849538337, - "heading": 0.5832040110121259, - "angularVelocity": 0.09863032483958853, - "velocityX": 1.432615235393288, - "velocityY": 0.7928975159627525, - "timestamp": 3.652430436935323 - }, - { - "x": 8.16207340516984, - "y": 2.3662086171292933, - "heading": 0.581874089552229, - "angularVelocity": -0.020958243090781536, - "velocityX": 1.183238231546559, - "velocityY": 0.676403926048603, - "timestamp": 3.7158862072561707 - }, - { - "x": 8.221474033046453, - "y": 2.401420292157025, - "heading": 0.5733258497363348, - "angularVelocity": -0.1347117807044565, - "velocityX": 0.9360949772773745, - "velocityY": 0.5549010728211649, - "timestamp": 3.7793419775770185 - }, - { - "x": 8.265310204045273, - "y": 2.4286697945211704, - "heading": 0.5578573342178689, - "angularVelocity": -0.24376846172150818, - "velocityX": 0.6908145748948221, - "velocityY": 0.4294251291311274, - "timestamp": 3.8427977478978663 - }, - { - "x": 8.293681756385627, - "y": 2.447752418352113, - "heading": 0.5357134031725193, - "angularVelocity": -0.34896638924064666, - "velocityX": 0.44710752382171043, - "velocityY": 0.30072322397878726, - "timestamp": 3.906253518218714 + "angularVelocity": 0.681422832753116, + "velocityX": 2.464168636543981, + "velocityY": 1.1632956752609667, + "timestamp": 3.3986072208880596 + }, + { + "x": 7.766066221949162, + "y": 2.1519606772611084, + "heading": 0.5396117040985706, + "angularVelocity": 0.5123769157726334, + "velocityX": 2.199683557182638, + "velocityY": 1.0929921902308588, + "timestamp": 3.4620628503385116 + }, + { + "x": 7.88917922885163, + "y": 2.215677284676183, + "heading": 0.5626038110324001, + "angularVelocity": 0.36233360433028805, + "velocityX": 1.9401431830189138, + "velocityY": 1.0041127629949167, + "timestamp": 3.5255184797889636 + }, + { + "x": 7.996083890855478, + "y": 2.2729699125828824, + "heading": 0.576920084072235, + "angularVelocity": 0.2256107639908224, + "velocityX": 1.6847151770407296, + "velocityY": 0.9028769929929541, + "timestamp": 3.5889741092394156 + }, + { + "x": 8.086991794921751, + "y": 2.3232832101942007, + "heading": 0.5831794167828934, + "angularVelocity": 0.09864109401902421, + "velocityX": 1.4326215791028423, + "velocityY": 0.792889425998124, + "timestamp": 3.6524297386898676 + }, + { + "x": 8.162075125847174, + "y": 2.3662047030478557, + "heading": 0.5818525424229896, + "angularVelocity": -0.02091027023756688, + "velocityX": 1.1832414487992695, + "velocityY": 0.6764016561709427, + "timestamp": 3.7158853681403197 + }, + { + "x": 8.2214756151654, + "y": 2.4014165863300603, + "heading": 0.5733088893147004, + "angularVelocity": -0.1346397976393925, + "velocityX": 0.936094871844373, + "velocityY": 0.5549055865831228, + "timestamp": 3.7793409975907717 + }, + { + "x": 8.265311459891254, + "y": 2.4286667893871807, + "heading": 0.5578458366246501, + "angularVelocity": -0.24368291393475716, + "velocityX": 0.6908109667414808, + "velocityY": 0.4294371246982685, + "timestamp": 3.8427966270412237 + }, + { + "x": 8.29368248788513, + "y": 2.4477506399099855, + "heading": 0.5357077069480222, + "angularVelocity": -0.34887573992019155, + "velocityX": 0.44710025319389435, + "velocityY": 0.30074322306905504, + "timestamp": 3.9062522564916757 }, { "x": 8.306674003601074, "y": 2.45849871635437, "heading": 0.507098504392337, - "angularVelocity": -0.45094242234392296, - "velocityX": 0.20474492941705758, - "velocityY": 0.16935099752034305, - "timestamp": 3.969709288539562 - }, - { - "x": 8.303680612059921, - "y": 2.4605220907329786, - "heading": 0.4706360870687623, - "angularVelocity": -0.5539055745477068, - "velocityX": -0.04547302080206286, - "velocityY": 0.030737357256443772, - "timestamp": 4.035537146208892 - }, - { - "x": 8.28417079095848, - "y": 2.4534909416021047, - "heading": 0.42765554839737563, - "angularVelocity": -0.6529232485020114, - "velocityX": -0.2963763639315537, - "velocityY": -0.1068111492583101, - "timestamp": 4.101365003878222 - }, - { - "x": 8.248092493080108, - "y": 2.437487576077448, - "heading": 0.3784580606750378, - "angularVelocity": -0.7473657728536394, - "velocityX": -0.5480703634561933, - "velocityY": -0.24310931710774963, - "timestamp": 4.1671928615475515 - }, - { - "x": 8.195385029545998, - "y": 2.412609981570135, - "heading": 0.3233968459430461, - "angularVelocity": -0.8364424528074197, - "velocityX": -0.8006862960492162, - "velocityY": -0.37791894477683524, - "timestamp": 4.233020719216881 - }, - { - "x": 8.125976752501963, - "y": 2.378976757579925, - "heading": 0.2628923564512202, - "angularVelocity": -0.9191319850595123, - "velocityX": -1.0543906410063166, - "velocityY": -0.5109269112046505, - "timestamp": 4.298848576886211 - }, - { - "x": 8.039781850826214, - "y": 2.336734306251694, - "heading": 0.19745501482460134, - "angularVelocity": -0.9940676173198235, - "velocityX": -1.3093985544650038, - "velocityY": -0.641710862602064, - "timestamp": 4.364676434555541 - }, - { - "x": 7.936695821305385, - "y": 2.286067717588345, - "heading": 0.12772072746128918, - "angularVelocity": -1.0593431084086873, - "velocityX": -1.565993990548153, - "velocityY": -0.7696830864200519, - "timestamp": 4.430504292224871 - }, - { - "x": 7.816588930914964, - "y": 2.2272180395232652, - "heading": 0.05450854772010129, - "angularVelocity": -1.1121762477665882, - "velocityX": -1.8245602187716414, - "velocityY": -0.8939935180740173, - "timestamp": 4.496332149894201 - }, - { - "x": 7.679296617994528, - "y": 2.1605113660425403, - "heading": -0.02108172308249024, - "angularVelocity": -1.1483021547245422, - "velocityX": -2.085626325712903, - "velocityY": -1.01335021133165, - "timestamp": 4.562160007563531 - }, - { - "x": 7.52460540494712, - "y": 2.086411774056454, - "heading": -0.09749530920005847, - "angularVelocity": -1.16080925041513, - "velocityX": -2.34993540006212, - "velocityY": -1.1256570486967377, - "timestamp": 4.6279878652328605 - }, - { - "x": 7.352233710537335, - "y": 2.0056280451301003, - "heading": -0.17238692358324137, - "angularVelocity": -1.1376887693867086, - "velocityX": -2.6185220135167313, - "velocityY": -1.2271966882493952, - "timestamp": 4.69381572290219 - }, - { - "x": 7.161817946345765, - "y": 1.9193611743859953, - "heading": -0.24189742475427997, - "angularVelocity": -1.055943541717672, - "velocityX": -2.8926319484385736, - "velocityY": -1.3104918464375106, - "timestamp": 4.75964358057152 - }, - { - "x": 6.953014973362115, - "y": 1.8300106922230066, - "heading": -0.298719434373675, - "angularVelocity": -0.8631909290566133, - "velocityX": -3.1719545550536123, - "velocityY": -1.3573354097564532, - "timestamp": 4.82547143824085 - }, - { - "x": 6.727431268978415, - "y": 1.7436413956339059, - "heading": -0.3201876383615551, - "angularVelocity": -0.32612642653085444, - "velocityX": -3.4268729436231227, - "velocityY": -1.3120478114745338, - "timestamp": 4.89129929591018 - }, - { - "x": 6.489449879790782, - "y": 1.6705324422161696, - "heading": -0.3201878815696182, - "angularVelocity": -0.0000036946069898928346, - "velocityX": -3.61520787115807, - "velocityY": -1.1106081225517261, - "timestamp": 4.95712715357951 - }, - { - "x": 6.246606826324383, - "y": 1.6156931769447658, - "heading": -0.3201879117478121, - "angularVelocity": -4.584410758613646e-7, - "velocityX": -3.689062078949349, - "velocityY": -0.8330707881589553, - "timestamp": 5.02295501124884 - }, - { - "x": 6.0003025647982255, - "y": 1.5794398654466013, - "heading": -0.32018794628515007, - "angularVelocity": -5.246614307457641e-7, - "velocityX": -3.741641764546064, - "velocityY": -0.5507290193199733, - "timestamp": 5.0887828689181696 - }, - { - "x": 5.7519574472803, - "y": 1.5619815744715873, - "heading": -0.32018798749762734, - "angularVelocity": -6.260643854910128e-7, - "velocityX": -3.772644687381865, - "velocityY": -0.26521128885450773, - "timestamp": 5.154610726587499 + "angularVelocity": -0.45085365638085945, + "velocityX": 0.20473385621503526, + "velocityY": 0.16937940002276766, + "timestamp": 3.9697078859421278 + }, + { + "x": 8.303679627930654, + "y": 2.4605245534000177, + "heading": 0.47064149696897567, + "angularVelocity": -0.5538247976382693, + "velocityX": -0.04548808629479942, + "velocityY": 0.03077484607623031, + "timestamp": 4.035535576538194 + }, + { + "x": 8.284168636347566, + "y": 2.453496432112781, + "heading": 0.4276658140977222, + "angularVelocity": -0.6528511403348756, + "velocityX": -0.29639489713850375, + "velocityY": -0.10676542384515514, + "timestamp": 4.1013632671342615 + }, + { + "x": 8.248089021230637, + "y": 2.4374965938671473, + "heading": 0.378472576604384, + "angularVelocity": -0.7473031037226994, + "velocityX": -0.5480917648823836, + "velocityY": -0.24305635061409855, + "timestamp": 4.1671909577303285 + }, + { + "x": 8.195380139344671, + "y": 2.412622948042365, + "heading": 0.3234149472418141, + "angularVelocity": -0.8363901097551141, + "velocityX": -0.8007098746544274, + "velocityY": -0.37785991882067893, + "timestamp": 4.233018648326396 + }, + { + "x": 8.125970395948434, + "y": 2.3789940000965495, + "heading": 0.2629133084829053, + "angularVelocity": -0.9190910118685506, + "velocityX": -1.0544155927047596, + "velocityY": -0.5108632498164021, + "timestamp": 4.298846338922463 + }, + { + "x": 8.039774042344641, + "y": 2.3367560367748186, + "heading": 0.19747799875428093, + "angularVelocity": -0.9940392733834453, + "velocityX": -1.3094239342636282, + "velocityY": -0.6416443131950763, + "timestamp": 4.36467402951853 + }, + { + "x": 7.936686649427289, + "y": 2.286094003355545, + "heading": 0.12774482040942284, + "angularVelocity": -1.0593289497691225, + "velocityX": -1.5660186766982322, + "velocityY": -0.7696158403937725, + "timestamp": 4.430501720114597 + }, + { + "x": 7.816578572926189, + "y": 2.2272487613470173, + "heading": 0.05453269620309629, + "angularVelocity": -1.1121782268737388, + "velocityX": -1.8245828679925904, + "velocityY": -0.8939283981510958, + "timestamp": 4.496329410710664 + }, + { + "x": 7.679285357941873, + "y": 2.160546156413331, + "heading": -0.02105873619647849, + "angularVelocity": -1.1483227151841096, + "velocityX": -2.085645322525112, + "velocityY": -1.0132909772422172, + "timestamp": 4.562157101306731 + }, + { + "x": 7.52459365410468, + "y": 2.086449920391606, + "heading": -0.09747489817686252, + "angularVelocity": -1.16085132697865, + "velocityX": -2.349948819964157, + "velocityY": -1.1256089246149548, + "timestamp": 4.627984791902798 + }, + { + "x": 7.352222023674252, + "y": 2.0056683293260935, + "heading": -0.17237070551570732, + "angularVelocity": -1.1377553528107507, + "velocityX": -2.6185276875067363, + "velocityY": -1.2271673263035494, + "timestamp": 4.693812482498865 + }, + { + "x": 7.1618070022795735, + "y": 1.9194015924783232, + "heading": -0.24188707800247594, + "angularVelocity": -1.0560354139314483, + "velocityX": -2.8926280060941036, + "velocityY": -1.3104931384745382, + "timestamp": 4.759640173094932 + }, + { + "x": 6.953005373134873, + "y": 1.8300479988886789, + "heading": -0.29871595232882114, + "angularVelocity": -0.8632974028370489, + "velocityX": -3.1719421911054577, + "velocityY": -1.3573861209553544, + "timestamp": 4.825467863690999 + }, + { + "x": 6.727423586928556, + "y": 1.7436722229358965, + "heading": -0.32018076563144104, + "angularVelocity": -0.3260757457577048, + "velocityX": -3.426852501791936, + "velocityY": -1.3121495706541393, + "timestamp": 4.891295554287066 + }, + { + "x": 6.489444642291127, + "y": 1.670557467554106, + "heading": -0.32018100817910516, + "angularVelocity": -0.0000036845841301644418, + "velocityX": -3.615179911106431, + "velocityY": -1.1106990799728806, + "timestamp": 4.9571232448831335 + }, + { + "x": 6.246603596847642, + "y": 1.615712184721808, + "heading": -0.3201810383505521, + "angularVelocity": -4.583397455824824e-7, + "velocityX": -3.6890409377052413, + "velocityY": -0.8331643163488851, + "timestamp": 5.022950935479201 + }, + { + "x": 6.0003008879136885, + "y": 1.5794526731151552, + "heading": -0.3201810728782718, + "angularVelocity": -5.245166498428474e-7, + "velocityX": -3.7416276752791178, + "velocityY": -0.5508246040279513, + "timestamp": 5.088778626075268 + }, + { + "x": 5.751956851390291, + "y": 1.561988034181555, + "heading": -0.3201811140764657, + "angularVelocity": -6.258489930223753e-7, + "velocityX": -3.7726378409245966, + "velocityY": -0.265308394924062, + "timestamp": 5.154606316671335 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": -0.32018803965163717, - "angularVelocity": -7.922787054710825e-7, - "velocityX": -3.7818920261895626, - "velocityY": 0.021835891254127007, - "timestamp": 5.220438584256829 - }, - { - "x": 5.250083628410863, - "y": 1.584512642850435, - "heading": -0.3201880864656727, - "angularVelocity": -6.975962896264183e-7, - "velocityX": -3.768870382202017, - "velocityY": 0.31432577229262343, - "timestamp": 5.287546216836352 - }, - { - "x": 4.999553273526972, - "y": 1.6251081857259102, - "heading": -0.32018812296137045, - "angularVelocity": -5.438382542161688e-7, - "velocityX": -3.7332617059171653, - "velocityY": 0.6049318283932773, - "timestamp": 5.354653849415875 - }, - { - "x": 4.752913977063861, - "y": 1.6849623155206188, - "heading": -0.3201881529794151, - "angularVelocity": -4.473119302496649e-7, - "velocityX": -3.675279353221748, - "velocityY": 0.8919124024198194, - "timestamp": 5.421761481995398 - }, - { - "x": 4.511643860653374, - "y": 1.7637163133941143, - "heading": -0.3201881787520496, - "angularVelocity": -3.8404922716455346e-7, - "velocityX": -3.595270867655492, - "velocityY": 1.1735475510952014, - "timestamp": 5.488869114574921 - }, - { - "x": 4.277188862383835, - "y": 1.86089818593956, - "heading": -0.3201882016908444, - "angularVelocity": -3.41820951724987e-7, - "velocityX": -3.4937158301883047, - "velocityY": 1.4481493208732221, - "timestamp": 5.555976747154444 - }, - { - "x": 4.050954064298834, - "y": 1.9759254853688604, - "heading": -0.32018822275897413, - "angularVelocity": -3.1394535808439923e-7, - "velocityX": -3.3712230544999895, - "velocityY": 1.7140717830120589, - "timestamp": 5.623084379733967 - }, - { - "x": 3.834295246075004, - "y": 2.1081087693328064, - "heading": -0.32018824266587215, - "angularVelocity": -2.966413400864343e-7, - "velocityX": -3.2285272165887804, - "velocityY": 1.969720565053586, - "timestamp": 5.69019201231349 - }, - { - "x": 3.6285105963453295, - "y": 2.256655537498403, - "heading": -0.32018826197928, - "angularVelocity": -2.8779748460931035e-7, - "velocityX": -3.066486505626901, - "velocityY": 2.213559955189428, - "timestamp": 5.757299644893013 - }, - { - "x": 3.4323361610843617, - "y": 2.417680552436319, - "heading": -0.3201882811600919, - "angularVelocity": -2.858216149634079e-7, - "velocityX": -2.9232805229494647, - "velocityY": 2.399503733753405, - "timestamp": 5.824407277472536 - }, - { - "x": 3.2362856931678534, - "y": 2.578856476556199, - "heading": -0.32018830034052725, - "angularVelocity": -2.858160039511642e-7, - "velocityX": -2.921433231669861, - "velocityY": 2.401752497063368, - "timestamp": 5.891514910052059 - }, - { - "x": 3.0402351711109183, - "y": 2.7400323348209366, - "heading": -0.3201883195209977, - "angularVelocity": -2.858165259418001e-7, - "velocityX": -2.9214340384398776, - "velocityY": 2.401751515727269, - "timestamp": 5.958622542631582 - }, - { - "x": 2.8448193652926044, - "y": 2.899781496680513, - "heading": -0.32456406822795664, - "angularVelocity": -0.06520493331028634, - "velocityX": -2.911975855902028, - "velocityY": 2.380491692510139, - "timestamp": 6.025730175211105 - }, - { - "x": 2.66318575993582, - "y": 3.04911523833719, - "heading": -0.3692876119349906, - "angularVelocity": -0.6664449629337832, - "velocityX": -2.706601296678858, - "velocityY": 2.2252869892216163, - "timestamp": 6.0928378077906284 - }, - { - "x": 2.496643428698106, - "y": 3.186035937240345, - "heading": -0.4222622822262224, - "angularVelocity": -0.7893985863449552, - "velocityX": -2.4817196619231154, - "velocityY": 2.040314844081317, - "timestamp": 6.1599454403701515 - }, - { - "x": 2.3452748100493945, - "y": 3.3104804771184444, - "heading": -0.4767233164983405, - "angularVelocity": -0.8115475420412368, - "velocityX": -2.255609575696765, - "velocityY": 1.8544021759467144, - "timestamp": 6.2270530729496745 - }, - { - "x": 2.209079907655855, - "y": 3.4224492823331634, - "heading": -0.5296433021172221, - "angularVelocity": -0.7885837062747133, - "velocityX": -2.0294994348392206, - "velocityY": 1.6684958314098723, - "timestamp": 6.2941607055291975 - }, - { - "x": 2.088048432415951, - "y": 3.5219507247047837, - "heading": -0.5792907495482946, - "angularVelocity": -0.7398181923977142, - "velocityX": -1.803542616355617, - "velocityY": 1.4827142390056927, - "timestamp": 6.3612683381087205 - }, - { - "x": 1.9821698412647601, - "y": 3.6089936285690154, - "heading": -0.624538681260696, - "angularVelocity": -0.6742590965160707, - "velocityX": -1.5777429046647484, - "velocityY": 1.297064141863228, - "timestamp": 6.4283759706882435 - }, - { - "x": 1.8914349570352331, - "y": 3.6835858822586136, - "heading": -0.6645923745497577, - "angularVelocity": -0.5968574922025148, - "velocityX": -1.3520799459287431, - "velocityY": 1.1115315921956592, - "timestamp": 6.4954836032677665 - }, - { - "x": 1.815836039628641, - "y": 3.7457342610555044, - "heading": -0.698860244587487, - "angularVelocity": -0.5106404252470358, - "velocityX": -1.1265323257679698, - "velocityY": 0.926100003948804, - "timestamp": 6.5625912358472895 - }, - { - "x": 1.7553665690239355, - "y": 3.7954445247907986, - "heading": -0.7268845476309118, - "angularVelocity": -0.41760231982876106, - "velocityX": -0.9010818632746272, - "velocityY": 0.7407542454487245, - "timestamp": 6.6296988684268126 - }, - { - "x": 1.7100210087875185, - "y": 3.8327215653417337, - "heading": -0.7483006908429537, - "angularVelocity": -0.31913125808251036, - "velocityX": -0.6757139015846285, - "velocityY": 0.5554813829374979, - "timestamp": 6.696806501006336 - }, - { - "x": 1.6797946046172731, - "y": 3.8575695457400823, - "heading": -0.7628116421295846, - "angularVelocity": -0.21623399200434476, - "velocityX": -0.4504167858168276, - "velocityY": 0.37027055557210875, - "timestamp": 6.763914133585859 + "heading": -0.3201811662076607, + "angularVelocity": -7.919341312199672e-7, + "velocityX": -3.7818925725102797, + "velocityY": 0.02173781608673755, + "timestamp": 5.220434007267402 + }, + { + "x": 5.250082401017072, + "y": 1.5845061941743261, + "heading": -0.3201812130071943, + "angularVelocity": -6.973782997664008e-7, + "velocityX": -3.7688784475631176, + "velocityY": 0.3142288252976883, + "timestamp": 5.28754182190281 + }, + { + "x": 4.999550332100038, + "y": 1.6250954552906307, + "heading": -0.3201812494932628, + "angularVelocity": -5.436932891401309e-7, + "velocityX": -3.7332771194853676, + "velocityY": 0.6048365803121931, + "timestamp": 5.354649636538218 + }, + { + "x": 4.752908853379521, + "y": 1.6849435065641751, + "heading": -0.32018127950441017, + "angularVelocity": -4.4720793763185246e-7, + "velocityX": -3.6753019012838326, + "velocityY": 0.8918194043226525, + "timestamp": 5.421757451173626 + }, + { + "x": 4.5116361075787825, + "y": 1.7636916636244688, + "heading": -0.32018130527180205, + "angularVelocity": -3.839700641654236e-7, + "velocityX": -3.595300295674319, + "velocityY": 1.1734573311338945, + "timestamp": 5.488865265809034 + }, + { + "x": 4.277178056371613, + "y": 1.8608679654762732, + "heading": -0.32018132820641354, + "angularVelocity": -3.4175768691264823e-7, + "velocityX": -3.493751845160869, + "velocityY": 1.4480623811065336, + "timestamp": 5.555973080444442 + }, + { + "x": 4.05093980766007, + "y": 1.975889994456223, + "heading": -0.3201813492710647, + "angularVelocity": -3.138926699011739e-7, + "velocityX": -3.3712653279007916, + "velocityY": 1.7139885958864325, + "timestamp": 5.62308089507985 + }, + { + "x": 3.834277168836918, + "y": 2.1080683355859073, + "heading": -0.3201813691749636, + "angularVelocity": -2.965958441785772e-7, + "velocityX": -3.2285753902174523, + "velocityY": 1.9696415663034246, + "timestamp": 5.690188709715258 + }, + { + "x": 3.6284883544416036, + "y": 2.2566105084469403, + "heading": -0.32018138848592925, + "angularVelocity": -2.877603113639349e-7, + "velocityX": -3.0665402459214466, + "velocityY": 2.2134854736076965, + "timestamp": 5.757296524350666 + }, + { + "x": 3.4324267312993486, + "y": 2.4177739449564437, + "heading": -0.3201814076661106, + "angularVelocity": -2.8581144314704067e-7, + "velocityX": -2.921591534569326, + "velocityY": 2.4015598985765383, + "timestamp": 5.824404338986074 + }, + { + "x": 3.2363665756420468, + "y": 2.578939166702229, + "heading": -0.3201814268462876, + "angularVelocity": -2.858113772828912e-7, + "velocityX": -2.921569666997558, + "velocityY": 2.401586501086132, + "timestamp": 5.891512153621482 + }, + { + "x": 3.04030639781219, + "y": 2.740104361474713, + "heading": -0.3201814460264912, + "angularVelocity": -2.8581177503839375e-7, + "velocityX": -2.9215699973995357, + "velocityY": 2.4015860991463303, + "timestamp": 5.95861996825689 + }, + { + "x": 2.8448806552051593, + "y": 2.899843193504081, + "heading": -0.32455581469161493, + "angularVelocity": -0.06518419186929905, + "velocityX": -2.912116028047787, + "velocityY": 2.380331305634352, + "timestamp": 6.025727782892298 + }, + { + "x": 2.663237615437101, + "y": 3.049167443178603, + "heading": -0.36928029407596225, + "angularVelocity": -0.6664570978407269, + "velocityX": -2.706734539858174, + "velocityY": 2.225139508502705, + "timestamp": 6.092835597527706 + }, + { + "x": 2.4966866332566564, + "y": 3.186079438683932, + "heading": -0.4222560068283227, + "angularVelocity": -0.7894119789203976, + "velocityX": -2.481841840407185, + "velocityY": 2.0401796161767862, + "timestamp": 6.1599434121631145 + }, + { + "x": 2.3453101524500037, + "y": 3.3105160667275593, + "heading": -0.47671806974270053, + "angularVelocity": -0.8115606686086642, + "velocityX": -2.2557206135987404, + "velocityY": 1.8542792477997239, + "timestamp": 6.2270512267985225 + }, + { + "x": 2.209108176535601, + "y": 3.4224777520191383, + "heading": -0.5296390322123383, + "angularVelocity": -0.7885961233748087, + "velocityX": -2.029599334360373, + "velocityY": 1.6683852081886403, + "timestamp": 6.294159041433931 + }, + { + "x": 2.0880704157388417, + "y": 3.5219728662741354, + "heading": -0.579287382494677, + "angularVelocity": -0.7398296390975455, + "velocityX": -1.8036313871096692, + "velocityY": 1.4826159188098624, + "timestamp": 6.361266856069339 + }, + { + "x": 1.9821863263109802, + "y": 3.609010233643184, + "heading": -0.6245361276970436, + "angularVelocity": -0.6742693894623133, + "velocityX": -1.577820556415411, + "velocityY": 1.2969781215781884, + "timestamp": 6.428374670704747 + }, + { + "x": 1.8914467304783316, + "y": 3.6835977422656017, + "heading": -0.664590534011598, + "angularVelocity": -0.5968664980698198, + "velocityX": -1.3521464873447422, + "velocityY": 1.1114578686200178, + "timestamp": 6.495482485340155 + }, + { + "x": 1.815843887624038, + "y": 3.745742167237612, + "heading": -0.6988590085071317, + "angularVelocity": -0.510648047201536, + "velocityX": -1.1265877642572446, + "velocityY": 0.9260385740414423, + "timestamp": 6.562590299975563 + }, + { + "x": 1.7553712772903527, + "y": 3.7954492682162915, + "heading": -0.7268838015342108, + "angularVelocity": -0.41760848836064807, + "velocityX": -0.9011262050810134, + "velocityY": 0.7407051063834291, + "timestamp": 6.629698114610971 + }, + { + "x": 1.710023362676749, + "y": 3.8327239369169632, + "heading": -0.7483003159780411, + "angularVelocity": -0.3191359241868454, + "velocityX": -0.675747151952063, + "velocityY": 0.5554445321037842, + "timestamp": 6.696805929246379 + }, + { + "x": 1.679795389171883, + "y": 3.8575703362206024, + "heading": -0.7628115166816798, + "angularVelocity": -0.21623712204721673, + "velocityX": -0.4504389491610239, + "velocityY": 0.37024599055457114, + "timestamp": 6.763913743881787 }, { "x": 1.664683222770691, "y": 3.869992017745972, "heading": -0.7701709445795863, - "angularVelocity": -0.10966416437475777, - "velocityX": -0.22518126874279432, - "velocityY": 0.1851126545280661, - "timestamp": 6.831021766165382 + "angularVelocity": -0.10966573621700788, + "velocityX": -0.22519234880908606, + "velocityY": 0.1851003730766039, + "timestamp": 6.831021558517195 }, { "x": 1.664683222770691, "y": 3.869992017745972, "heading": -0.7701709445795863, - "angularVelocity": -1.211900873027185e-19, - "velocityX": -5.462077711506373e-19, - "velocityY": -3.512888080115249e-19, - "timestamp": 6.898129398744905 - }, - { - "x": 1.659892349205253, - "y": 3.8800826826438297, - "heading": -0.7264777443526473, - "angularVelocity": 0.7466474211356967, - "velocityX": -0.08186842286768115, - "velocityY": 0.1724334423754288, - "timestamp": 6.95664858596781 - }, - { - "x": 1.6562452238185605, - "y": 3.90221669421699, - "heading": -0.6460681306884017, - "angularVelocity": 1.3740726329290587, - "velocityX": -0.06232358239700476, - "velocityY": 0.3782351160970439, - "timestamp": 7.015167773190715 - }, - { - "x": 1.6553319250883793, - "y": 3.9350953760295733, - "heading": -0.5183992679116819, - "angularVelocity": 2.1816581677803772, - "velocityX": -0.01560682527428515, - "velocityY": 0.5618444714097892, - "timestamp": 7.07368696041362 - }, - { - "x": 1.6623538133314129, - "y": 3.9788519148323376, - "heading": -0.3621106955703112, - "angularVelocity": 2.670723565350507, - "velocityX": 0.11999292157436166, - "velocityY": 0.7477297768352748, - "timestamp": 7.132206147636525 - }, - { - "x": 1.6832034751641156, - "y": 4.019420947962781, - "heading": -0.22540240918746318, - "angularVelocity": 2.3361275655130602, - "velocityX": 0.35628761816674753, - "velocityY": 0.6932603656286723, - "timestamp": 7.19072533485943 - }, - { - "x": 1.7146647081648767, - "y": 4.052413452071288, - "heading": -0.11878752772286896, - "angularVelocity": 1.8218790541038834, - "velocityX": 0.5376225216683612, - "velocityY": 0.5637895137339223, - "timestamp": 7.249244522082336 - }, - { - "x": 1.7541026264736177, - "y": 4.074977116543675, - "heading": -0.0408151972626467, - "angularVelocity": 1.332423332593777, - "velocityX": 0.6739314091721454, - "velocityY": 0.38557720199428364, - "timestamp": 7.307763709305241 - }, - { - "x": 1.799090355046403, - "y": 4.0866639858943925, - "heading": 1.7934233852716553e-24, - "angularVelocity": 0.6974669198185859, - "velocityX": 0.7687688552717376, - "velocityY": 0.199710042215758, - "timestamp": 7.366282896528146 + "angularVelocity": 8.092112436868569e-26, + "velocityX": -3.5782911869768004e-26, + "velocityY": 1.7219402564933353e-26, + "timestamp": 6.898129373152603 + }, + { + "x": 1.659892350481847, + "y": 3.8800826841683307, + "heading": -0.726477756364864, + "angularVelocity": 0.7466472297695131, + "velocityX": -0.08186840257721437, + "velocityY": 0.1724334716376635, + "timestamp": 6.956648559285806 + }, + { + "x": 1.6562452284597964, + "y": 3.902216695654719, + "heading": -0.6460681483712878, + "angularVelocity": 1.3740725616133136, + "velocityX": -0.06232352606116078, + "velocityY": 0.3782351216574767, + "timestamp": 7.015167745419008 + }, + { + "x": 1.655331933364656, + "y": 3.9350953770599486, + "heading": -0.5183992946484082, + "angularVelocity": 2.1816580536898917, + "velocityX": -0.015606763447832819, + "velocityY": 0.5618444749110212, + "timestamp": 7.073686931552211 + }, + { + "x": 1.6623538228794565, + "y": 3.9788519154462327, + "heading": -0.3621107234214173, + "angularVelocity": 2.67072359603982, + "velocityX": 0.11999294554126883, + "velocityY": 0.7477297836419755, + "timestamp": 7.1322061176854135 + }, + { + "x": 1.6832034861301444, + "y": 4.019420949275189, + "heading": -0.22540243054151132, + "angularVelocity": 2.336127720039156, + "velocityX": 0.35628764903239357, + "velocityY": 0.6932603904745416, + "timestamp": 7.190725303818616 + }, + { + "x": 1.7146647196939016, + "y": 4.052413453417189, + "heading": -0.11878753895047192, + "angularVelocity": 1.8218792610744803, + "velocityX": 0.537622541300291, + "velocityY": 0.5637895248047576, + "timestamp": 7.249244489951819 + }, + { + "x": 1.7541026360793814, + "y": 4.074977117129141, + "heading": -0.04081520050411857, + "angularVelocity": 1.3324234938755106, + "velocityX": 0.6739313888561295, + "velocityY": 0.3855771961795887, + "timestamp": 7.307763676085021 + }, + { + "x": 1.7990903606675779, + "y": 4.0866639859913105, + "heading": 4.0044035166870527e-26, + "angularVelocity": 0.6974669881979226, + "velocityX": 0.7687688014969033, + "velocityY": 0.1997100375861074, + "timestamp": 7.366282862218224 }, { "x": 1.848745584487915, "y": 4.087520122528076, - "heading": 7.573339964499894e-25, - "angularVelocity": -5.2358045608260825e-25, - "velocityX": 0.848529034628775, - "velocityY": 0.014630015800164336, - "timestamp": 7.424802083751051 - }, - { - "x": 1.9382598731728313, - "y": 4.089063493944204, - "heading": 6.819777064270339e-25, - "angularVelocity": -2.006050179968467e-26, - "velocityX": 1.1795649340806893, - "velocityY": 0.02033761122914328, - "timestamp": 7.500689630166358 - }, - { - "x": 2.0528956622357724, - "y": 4.091040000742217, - "heading": 6.067470023829544e-25, - "angularVelocity": -1.8954686232133377e-26, - "velocityX": 1.510600809724148, - "velocityY": 0.026045206247625866, - "timestamp": 7.576577176581664 - }, - { - "x": 2.1926529480236727, - "y": 4.09344964285913, - "heading": 5.320081952060173e-25, - "angularVelocity": -2.1711320260805291e-26, - "velocityX": 1.8416366372297248, - "velocityY": 0.03175280043613335, - "timestamp": 7.652464722996971 - }, - { - "x": 2.3575317192504395, - "y": 4.096292420100353, - "heading": 4.5695624647021355e-25, - "angularVelocity": -2.0004263459154622e-26, - "velocityX": 2.1726723160140353, - "velocityY": 0.03746039206044517, - "timestamp": 7.7283522694122775 - }, - { - "x": 2.4972894766259857, - "y": 4.098702070348201, - "heading": 3.822235688125773e-25, - "angularVelocity": -1.9197661326309743e-26, - "velocityX": 1.8416428515253338, - "velocityY": 0.0317529075806614, - "timestamp": 7.804239815827584 - }, - { - "x": 2.611925746748375, - "y": 4.100678585440459, - "heading": 3.063572284762821e-25, - "angularVelocity": -2.4686338901390002e-26, - "velocityX": 1.5106071488334032, - "velocityY": 0.026045315544143462, - "timestamp": 7.880127362242891 - }, - { - "x": 2.701440519692856, - "y": 4.102221965206007, - "heading": 2.3114445870748474e-25, - "angularVelocity": -1.5210807516831905e-26, - "velocityX": 1.1795713153591425, - "velocityY": 0.020337721252726194, - "timestamp": 7.956014908658197 - }, - { - "x": 2.7658337920580536, - "y": 4.1033322095862, - "heading": 1.561656112047754e-25, - "angularVelocity": -1.9040979941775662e-26, - "velocityX": 0.8485354370636105, - "velocityY": 0.01463012618851756, - "timestamp": 8.031902455073505 - }, - { - "x": 2.805105562125262, - "y": 4.104009318551404, - "heading": 8.077058611592956e-26, - "angularVelocity": -3.263981497829794e-26, - "velocityX": 0.5174995361200375, - "velocityY": 0.008922530733819996, - "timestamp": 8.107790001488812 + "heading": 1.8053103807695408e-26, + "angularVelocity": -4.864634969888421e-27, + "velocityX": 0.848528954372517, + "velocityY": 0.014630014416416372, + "timestamp": 7.4248020483514265 + }, + { + "x": 1.938259872491009, + "y": 4.089063493932448, + "heading": 1.6974407662610403e-26, + "angularVelocity": -2.7257088903490675e-29, + "velocityX": 1.1795648681505524, + "velocityY": 0.020337610092400867, + "timestamp": 7.500689598430333 + }, + { + "x": 2.052895663172084, + "y": 4.09104000075836, + "heading": 1.589827161160837e-26, + "angularVelocity": 6.478274543003658e-30, + "velocityX": 1.510600758120114, + "velocityY": 0.02604520535788869, + "timestamp": 7.57657714850924 + }, + { + "x": 2.1926529528780687, + "y": 4.093449642942828, + "heading": 1.4819575466507505e-26, + "angularVelocity": -2.7257089112437415e-29, + "velocityX": 1.8416365999517403, + "velocityY": 0.03175279979340049, + "timestamp": 7.652464698588147 + }, + { + "x": 2.3575317303228487, + "y": 4.096292420291259, + "heading": 1.3740875703299928e-26, + "angularVelocity": -2.7304766323100225e-29, + "velocityX": 2.1726722930617965, + "velocityY": 0.03746039166471138, + "timestamp": 7.728352248667053 + }, + { + "x": 2.497289491602916, + "y": 4.0987020706064285, + "heading": 1.2662183176306477e-26, + "angularVelocity": -2.720941189252962e-29, + "velocityX": 1.8416428140683072, + "velocityY": 0.031752906934841556, + "timestamp": 7.80423979874596 + }, + { + "x": 2.611925763329844, + "y": 4.10067858572635, + "heading": 1.1583487031205877e-26, + "angularVelocity": -2.725708910890731e-29, + "velocityX": 1.510607097049927, + "velocityY": 0.02604531465131241, + "timestamp": 7.880127348824867 + }, + { + "x": 2.7014405355788997, + "y": 4.1022219654799095, + "heading": 1.0506887852514804e-26, + "angularVelocity": 3.7545940988338324e-31, + "velocityX": 1.1795712492494417, + "velocityY": 0.02033772011288781, + "timestamp": 7.956014898903773 + }, + { + "x": 2.7658338049487097, + "y": 4.103332209808455, + "heading": 9.428654835122248e-27, + "angularVelocity": -2.1154273725241504e-29, + "velocityX": 0.8485353566277292, + "velocityY": 0.014630124801672608, + "timestamp": 8.031902448982681 + }, + { + "x": 2.8051055697205705, + "y": 4.10400931868236, + "heading": 8.349263998410887e-27, + "angularVelocity": -3.641131282577579e-29, + "velocityX": 0.5174994413579915, + "velocityY": 0.008922529099968745, + "timestamp": 8.107789999061588 }, { "x": 2.819255828857422, "y": 4.10425329208374, - "heading": 3.4975515409752656e-27, - "angularVelocity": -3.9242842268499737e-26, - "velocityX": 0.18646362151070262, - "velocityY": 0.0032149350435025517, - "timestamp": 8.18367754790412 - }, - { - "x": 2.810470941680652, - "y": 4.100290874791341, - "heading": -0.013665728423960572, - "angularVelocity": -0.18841669161843644, - "velocityX": -0.12112192828199068, - "velocityY": -0.05463196207942443, - "timestamp": 8.256206835551495 - }, - { - "x": 2.779376565530904, - "y": 4.092132450200435, - "heading": -0.040981035402949714, - "angularVelocity": -0.3766107163742046, - "velocityX": -0.4287147600418107, - "velocityY": -0.11248455424752818, - "timestamp": 8.32873612319887 - }, - { - "x": 2.7259718800290944, - "y": 4.079777209778572, - "heading": -0.08192049181497908, - "angularVelocity": -0.5644541362527821, - "velocityX": -0.7363189028058957, - "velocityY": -0.17034829408406268, - "timestamp": 8.401265410846246 - }, - { - "x": 2.650255783412156, - "y": 4.06322370149856, - "heading": -0.13644943849416394, - "angularVelocity": -0.7518196917125987, - "velocityX": -1.0439382361654468, - "velocityY": -0.22823205379447506, - "timestamp": 8.473794698493622 - }, - { - "x": 2.5522270104067304, - "y": 4.04246950295157, - "heading": -0.20452759410435262, - "angularVelocity": -0.9386298668914527, - "velocityX": -1.3515750145241086, - "velocityY": -0.2861492125483705, - "timestamp": 8.546323986140997 - }, - { - "x": 2.4318844389859766, - "y": 4.01751088765649, - "heading": -0.2861194089559698, - "angularVelocity": -1.1249498995261227, - "velocityX": -1.6592272628656817, - "velocityY": -0.34411775028624336, - "timestamp": 8.618853273788373 - }, - { - "x": 2.2892276526666033, - "y": 3.9883426531177015, - "heading": -0.3812142224775482, - "angularVelocity": -1.311122949172083, - "velocityX": -1.9668852534846832, - "velocityY": -0.40215801760798864, - "timestamp": 8.691382561435748 - }, - { - "x": 2.1330829846519728, - "y": 3.9587634030266514, - "heading": -0.4780632841326763, - "angularVelocity": -1.3353097044877917, - "velocityX": -2.1528498773319837, - "velocityY": -0.40782490839919366, - "timestamp": 8.763911849083124 - }, - { - "x": 1.9992497997330843, - "y": 3.9334042162180256, - "heading": -0.5612966415288853, - "angularVelocity": -1.1475827227322806, - "velocityX": -1.8452295515373094, - "velocityY": -0.349640643541367, - "timestamp": 8.8364411367305 - }, - { - "x": 1.8877256900718635, - "y": 3.9122685100016006, - "heading": -0.6308164435957174, - "angularVelocity": -0.9585066160421192, - "velocityX": -1.537642423891296, - "velocityY": -0.29140926240973475, - "timestamp": 8.908970424377875 - }, - { - "x": 1.7985083705984093, - "y": 3.895358481595443, - "heading": -0.6865270710591681, - "angularVelocity": -0.768112155386183, - "velocityX": -1.2300868017236275, - "velocityY": -0.23314758705988012, - "timestamp": 8.98149971202525 - }, - { - "x": 1.731596065924516, - "y": 3.88267537699245, - "heading": -0.7283506709725182, - "angularVelocity": -0.5766442946012179, - "velocityX": -0.9225556577807338, - "velocityY": -0.17486873254092689, - "timestamp": 9.054028999672626 - }, - { - "x": 1.6869877424298634, - "y": 3.8742198088507074, - "heading": -0.7562373778990242, - "angularVelocity": -0.3844889124250871, - "velocityX": -0.6150387649128701, - "velocityY": -0.11658143097795469, - "timestamp": 9.126558287320002 + "heading": 5.817602179940264e-27, + "angularVelocity": -1.9173555419457397e-26, + "velocityX": 0.18646351242249948, + "velocityY": 0.0032149331626451292, + "timestamp": 8.183677549140494 + }, + { + "x": 2.810470934258171, + "y": 4.100290875532643, + "heading": -0.013665725456507695, + "angularVelocity": -0.18841665432335325, + "velocityX": -0.12112203294600554, + "velocityY": -0.054631952907971924, + "timestamp": 8.256206835394856 + }, + { + "x": 2.7793765513907402, + "y": 4.0921324527065925, + "heading": -0.04098102601248858, + "angularVelocity": -0.37661063505003184, + "velocityX": -0.4287148608960741, + "velocityY": -0.11248453207493378, + "timestamp": 8.328736121649218 + }, + { + "x": 2.7259718598142455, + "y": 4.079777215271425, + "heading": -0.08192047187969453, + "angularVelocity": -0.5644540017067066, + "velocityX": -0.7363190007027361, + "velocityY": -0.17034825617665642, + "timestamp": 8.40126540790358 + }, + { + "x": 2.650255757673548, + "y": 4.0632237114983365, + "heading": -0.13644940291380203, + "angularVelocity": -0.7518194904451844, + "velocityX": -1.0439383323745826, + "velocityY": -0.22823199603860284, + "timestamp": 8.473794694157942 + }, + { + "x": 2.5522269795422527, + "y": 4.04246951947779, + "heading": -0.20452753616691674, + "angularVelocity": -0.9386295766700757, + "velocityX": -1.3515751111558711, + "velocityY": -0.28614912806063464, + "timestamp": 8.546323980412303 + }, + { + "x": 2.4318844030868068, + "y": 4.017510913730928, + "heading": -0.28611931872413315, + "angularVelocity": -1.1249494758720198, + "velocityX": -1.6592273641491695, + "velocityY": -0.344117625249086, + "timestamp": 8.618853266666665 + }, + { + "x": 2.2892276108991876, + "y": 3.988342694775403, + "heading": -0.38121408025371734, + "angularVelocity": -1.3111222575124248, + "velocityX": -1.9668853721697708, + "velocityY": -0.40215781047715543, + "timestamp": 8.691382552921027 + }, + { + "x": 2.133082954891742, + "y": 3.958763429178337, + "heading": -0.47806319392600216, + "angularVelocity": -1.335310447322373, + "velocityX": -2.1528497531306416, + "velocityY": -0.4078251300216943, + "timestamp": 8.763911839175389 + }, + { + "x": 1.999249779098319, + "y": 3.933404232877465, + "heading": -0.561296583651025, + "angularVelocity": -1.1475831905076381, + "velocityX": -1.8452294611595452, + "velocityY": -0.3496407811313142, + "timestamp": 8.83644112542975 + }, + { + "x": 1.8877256766061599, + "y": 3.91226852016899, + "heading": -0.6308164080928251, + "angularVelocity": -0.9585069429470503, + "velocityX": -1.5376423545799296, + "velocityY": -0.29140935751595565, + "timestamp": 8.908970411684113 + }, + { + "x": 1.7985083626535399, + "y": 3.8953584872647626, + "heading": -0.6865270511993388, + "angularVelocity": -0.7681123858179831, + "velocityX": -1.2300867492302674, + "velocityY": -0.2331476535550589, + "timestamp": 8.981499697938474 + }, + { + "x": 1.7315960620068493, + "y": 3.8826753796533127, + "heading": -0.7283506616380105, + "angularVelocity": -0.57664445079461, + "velocityX": -0.9225556199743571, + "velocityY": -0.17486877737869985, + "timestamp": 9.054028984192836 + }, + { + "x": 1.68698774113937, + "y": 3.874219809689345, + "heading": -0.7562373749577054, + "angularVelocity": -0.38448900795597707, + "velocityX": -0.6150387405032061, + "velocityY": -0.11658145834103056, + "timestamp": 9.126558270447198 }, { "x": 1.664683222770691, "y": 3.869992017745972, "heading": -0.7701709445795863, - "angularVelocity": -0.19210952061606446, - "velocityX": -0.3075243171780863, - "velocityY": -0.05829081246861152, - "timestamp": 9.199087574967377 + "angularVelocity": -0.1921095648592989, + "velocityX": -0.3075243052917468, + "velocityY": -0.05829082515090241, + "timestamp": 9.19908755670156 }, { "x": 1.664683222770691, "y": 3.869992017745972, "heading": -0.7701709445795863, - "angularVelocity": -5.999453085593573e-20, - "velocityX": -1.6973677435282043e-18, - "velocityY": -2.503873223513922e-18, - "timestamp": 9.271616862614753 - }, - { - "x": 1.6770247995246623, - "y": 3.8595873316999896, - "heading": -0.7649878221642326, - "angularVelocity": 0.08507451391469989, - "velocityX": 0.20257164680788678, - "velocityY": -0.1707799926111949, - "timestamp": 9.332541364875802 - }, - { - "x": 1.70170978110981, - "y": 3.8387765844705157, - "heading": -0.7547381408402699, - "angularVelocity": 0.1682357827076709, - "velocityX": 0.405173299231529, - "velocityY": -0.3415825563958536, - "timestamp": 9.39346586713685 - }, - { - "x": 1.7387402457793653, - "y": 3.80755819428449, - "heading": -0.7395583157876386, - "angularVelocity": 0.24915796583103744, - "velocityX": 0.607809063599532, - "velocityY": -0.5124110830197925, - "timestamp": 9.454390369397899 - }, - { - "x": 1.7881185850906471, - "y": 3.7659303409499723, - "heading": -0.7196099497583256, - "angularVelocity": 0.3274276405876679, - "velocityX": 0.8104840824091846, - "velocityY": -0.6832694858326719, - "timestamp": 9.515314871658948 - }, - { - "x": 1.8498475856412335, - "y": 3.713890927230462, - "heading": -0.6950873259687543, - "angularVelocity": 0.4025083977624826, - "velocityX": 1.0132048397554583, - "velocityY": -0.8541623121766772, - "timestamp": 9.576239373919996 - }, - { - "x": 1.9239305360498986, - "y": 3.6514375329357844, - "heading": -0.6662283152580175, - "angularVelocity": 0.47368480069122665, - "velocityX": 1.2159795756924812, - "velocityY": -1.025094862935087, - "timestamp": 9.637163876181045 - }, - { - "x": 2.010371367313072, - "y": 3.5785673636651287, - "heading": -0.6333309419257536, - "angularVelocity": 0.539969505065564, - "velocityX": 1.4188188340512586, - "velocityY": -1.1960732803104883, - "timestamp": 9.698088378442094 - }, - { - "x": 2.1091748328759063, - "y": 3.4952772039974476, - "heading": -0.5967798759520212, - "angularVelocity": 0.5999403297070707, - "velocityX": 1.6217361143055882, - "velocityY": -1.3671044748269063, - "timestamp": 9.759012880703143 - }, - { - "x": 2.220346716936752, - "y": 3.40156341112366, - "heading": -0.5570916262340567, - "angularVelocity": 0.6514333026129457, - "velocityX": 1.82474833498841, - "velocityY": -1.5381954615278515, - "timestamp": 9.819937382964191 - }, - { - "x": 2.3438939700884585, - "y": 3.297422079154605, - "heading": -0.5149984011781104, - "angularVelocity": 0.6909079843703304, - "velocityX": 2.027874641016087, - "velocityY": -1.7093505585459128, - "timestamp": 9.88086188522524 - }, - { - "x": 2.4798242349395756, - "y": 3.182849894813871, - "heading": -0.4716228537300689, - "angularVelocity": 0.7119557130263738, - "velocityX": 2.2311263909664074, - "velocityY": -1.8805600388792227, - "timestamp": 9.941786387486289 - }, - { - "x": 2.628141721858615, - "y": 3.057848265310819, - "heading": -0.42891150330345956, - "angularVelocity": 0.7010537442489162, - "velocityX": 2.4344472488840574, - "velocityY": -2.0517464216194523, - "timestamp": 10.002710889747338 - }, - { - "x": 2.788815804109156, - "y": 2.922450520332472, - "heading": -0.3910899170501554, - "angularVelocity": 0.6207943413513182, - "velocityX": 2.6372654069800667, - "velocityY": -2.2223857389625743, - "timestamp": 10.063635392008386 - }, - { - "x": 2.9613127738384817, - "y": 2.777334038124512, - "heading": -0.375202514467059, - "angularVelocity": 0.2607719717597751, - "velocityX": 2.831323413857593, - "velocityY": -2.3819067341111335, - "timestamp": 10.124559894269435 - }, - { - "x": 3.137446263833113, - "y": 2.628782733735228, - "heading": -0.3752024981016849, - "angularVelocity": 2.6861728103084975e-7, - "velocityX": 2.891012375282727, - "velocityY": -2.438285072117188, - "timestamp": 10.185484396530484 - }, - { - "x": 3.313579787334416, - "y": 2.480231469074176, - "heading": -0.37520248173599324, - "angularVelocity": 2.686224924493493e-7, - "velocityX": 2.8910129252531243, - "velocityY": -2.4382844200276295, - "timestamp": 10.246408898791532 - }, - { - "x": 3.489713311593346, - "y": 2.3316802053114225, - "heading": -0.37520246537030155, - "angularVelocity": 2.6862249292188543e-7, - "velocityX": 2.8910129376886133, - "velocityY": -2.4382844052831705, - "timestamp": 10.307333401052581 - }, - { - "x": 3.6658468653792484, - "y": 2.183128976558075, - "heading": -0.37520244900461003, - "angularVelocity": 2.6862249035440315e-7, - "velocityX": 2.89101342233718, - "velocityY": -2.4382838306472756, - "timestamp": 10.36825790331363 - }, - { - "x": 3.8419803666744587, - "y": 2.034577685567575, - "heading": -0.37520243263891834, - "angularVelocity": 2.686224937536833e-7, - "velocityX": 2.8910125607676815, - "velocityY": -2.4382848521928207, - "timestamp": 10.429182405574679 - }, - { - "x": 4.017984969789346, - "y": 1.8858736976718657, - "heading": -0.37520241627306933, - "angularVelocity": 2.6862507525069377e-7, - "velocityX": 2.8888968573061926, - "velocityY": -2.440791182151078, - "timestamp": 10.490106907835727 - }, - { - "x": 4.198449630537114, - "y": 1.742615181247194, - "heading": -0.37520239987870785, - "angularVelocity": 2.6909307205451447e-7, - "velocityX": 2.9621031612948867, - "velocityY": -2.3514105344815093, - "timestamp": 10.551031410096776 - }, - { - "x": 4.38853011399539, - "y": 1.6123849775845955, - "heading": -0.3752023832167796, - "angularVelocity": 2.734848475989746e-7, - "velocityX": 3.1199349424936167, - "velocityY": -2.137566969436863, - "timestamp": 10.611955912357825 - }, - { - "x": 4.587288186636938, - "y": 1.495827162172114, - "heading": -0.3752023659192684, - "angularVelocity": 2.8391715306138947e-7, - "velocityX": 3.2623667861891117, - "velocityY": -1.9131517055823732, - "timestamp": 10.672880414618874 + "angularVelocity": 4.278722998474624e-26, + "velocityX": 4.546149419559626e-26, + "velocityY": -6.368263045423934e-27, + "timestamp": 9.271616842955922 + }, + { + "x": 1.6770242080176545, + "y": 3.8595866692257377, + "heading": -0.7649877966744867, + "angularVelocity": 0.08507499669287022, + "velocityX": 0.20256209128031585, + "velocityY": -0.17079099557937222, + "timestamp": 9.332541299101496 + }, + { + "x": 1.7017080065350787, + "y": 3.838774596868545, + "heading": -0.7547380668498572, + "angularVelocity": 0.16823670612895278, + "velocityX": 0.40515418731755265, + "velocityY": -0.3416045652908843, + "timestamp": 9.393465755247071 + }, + { + "x": 1.738736696509759, + "y": 3.8075542186994835, + "heading": -0.739558172921761, + "angularVelocity": 0.2491592849319001, + "velocityX": 0.6077803942345016, + "velocityY": -0.5124441011744508, + "timestamp": 9.454390211392646 + }, + { + "x": 1.7881126694173817, + "y": 3.76592371429674, + "heading": -0.7196097205107205, + "angularVelocity": 0.32742930627685823, + "velocityX": 0.8104458542829184, + "velocityY": -0.6833135170426513, + "timestamp": 9.51531466753822 + }, + { + "x": 1.849838711755514, + "y": 3.713880986159023, + "heading": -0.6950869960421026, + "angularVelocity": 0.4025103549553654, + "velocityX": 1.0131570512610228, + "velocityY": -0.8542173608142635, + "timestamp": 9.576239123683795 + }, + { + "x": 1.923918112020319, + "y": 3.6514236137874088, + "heading": -0.6662278740694468, + "angularVelocity": 0.4736869854644038, + "velocityX": 1.2159222248582278, + "velocityY": -1.025160934098061, + "timestamp": 9.63716357982937 + }, + { + "x": 2.010354801057128, + "y": 3.578548802416206, + "heading": -0.6333303833823517, + "angularVelocity": 0.5399718400192002, + "velocityX": 1.4187519184459274, + "velocityY": -1.1961503800226454, + "timestamp": 9.698088035974944 + }, + { + "x": 2.109153532122565, + "y": 3.4952533361865883, + "heading": -0.5967791996957547, + "angularVelocity": 0.5999427159310254, + "velocityX": 1.6216596308937807, + "velocityY": -1.367192610313814, + "timestamp": 9.759012492120519 + }, + { + "x": 2.2203200891736463, + "y": 3.4015335717559703, + "heading": -0.5570908397473109, + "angularVelocity": 0.6514356049992648, + "velocityX": 1.824662279880775, + "velocityY": -1.5382946415915537, + "timestamp": 9.819936948266093 + }, + { + "x": 2.3438614224783176, + "y": 3.2973856025788515, + "heading": -0.5149975235515339, + "angularVelocity": 0.690910003286656, + "velocityX": 2.0277790089660783, + "velocityY": -1.7094607940079831, + "timestamp": 9.880861404411668 + }, + { + "x": 2.4797851741645616, + "y": 3.182806114592359, + "heading": -0.47162192283324345, + "angularVelocity": 0.7119571262917361, + "velocityX": 2.231021174181074, + "velocityY": -1.8806813426895879, + "timestamp": 9.941785860557243 + }, + { + "x": 2.628095553772068, + "y": 3.057796514272803, + "heading": -0.4289105902248852, + "angularVelocity": 0.7010539824319861, + "velocityX": 2.4343324338116106, + "velocityY": -2.0518788057927626, + "timestamp": 10.002710316702817 + }, + { + "x": 2.788761933089725, + "y": 2.9223901324293977, + "heading": -0.3910891505574595, + "angularVelocity": 0.6207924052215403, + "velocityX": 2.6371409690347543, + "velocityY": -2.222529184665362, + "timestamp": 10.063634772848392 + }, + { + "x": 2.9612506555206113, + "y": 2.7772643724590593, + "heading": -0.37520163462976347, + "angularVelocity": 0.2607740295577497, + "velocityX": 2.8311901877094443, + "velocityY": -2.382060820101041, + "timestamp": 10.124559228993967 + }, + { + "x": 3.1373758975337274, + "y": 2.6287035614867915, + "heading": -0.37520161826362025, + "angularVelocity": 2.686301076874653e-7, + "velocityX": 2.8908791831030336, + "velocityY": -2.4384429565902384, + "timestamp": 10.185483685139541 + }, + { + "x": 3.313501125191111, + "y": 2.480142733495068, + "heading": -0.3752016018976461, + "angularVelocity": 2.686273330621961e-7, + "velocityX": 2.8908789474713403, + "velocityY": -2.4384432359436565, + "timestamp": 10.246408141285116 + }, + { + "x": 3.4896263528475178, + "y": 2.3315819055021865, + "heading": -0.37520158553167193, + "angularVelocity": 2.686273331449891e-7, + "velocityX": 2.890878947455303, + "velocityY": -2.4384432359626693, + "timestamp": 10.30733259743069 + }, + { + "x": 3.665751580503926, + "y": 2.183021077509307, + "heading": -0.3752015691656978, + "angularVelocity": 2.6862733313245716e-7, + "velocityX": 2.8908789474553287, + "velocityY": -2.438443235962638, + "timestamp": 10.368257053576265 + }, + { + "x": 3.841876808185214, + "y": 2.0344602495459236, + "heading": -0.37520155279972356, + "angularVelocity": 2.6862733360962026e-7, + "velocityX": 2.890878947863701, + "velocityY": -2.4384432354784953, + "timestamp": 10.42918150972184 + }, + { + "x": 4.018002401412412, + "y": 1.8858998549539654, + "heading": -0.3752015364337498, + "angularVelocity": 2.686273261605019e-7, + "velocityX": 2.8908849478501355, + "velocityY": -2.4384361222196733, + "timestamp": 10.490105965867414 + }, + { + "x": 4.198462016558864, + "y": 1.7426352651038108, + "heading": -0.375201520037625, + "angularVelocity": 2.691222177389301e-7, + "velocityX": 2.962022586057329, + "velocityY": -2.351512002139716, + "timestamp": 10.551030422012989 + }, + { + "x": 4.3885379093915455, + "y": 1.6123986719437928, + "heading": -0.3752015033732071, + "angularVelocity": 2.7352591969451763e-7, + "velocityX": 3.119861954590255, + "velocityY": -2.1376734631627445, + "timestamp": 10.611954878158564 + }, + { + "x": 4.587291846391845, + "y": 1.49583415162556, + "heading": -0.37520148607296444, + "angularVelocity": 2.839622014925957e-7, + "velocityX": 3.2623013741048372, + "velocityY": -1.9132632064816424, + "timestamp": 10.672879334304138 }, { "x": 4.7937421798706055, "y": 1.3935176134109497, - "heading": -0.3752023473605566, - "angularVelocity": 3.046181942517293e-7, - "velocityX": 3.3886857597794955, - "velocityY": -1.6792841133570497, - "timestamp": 10.733804916879922 - }, - { - "x": 5.0647852667187605, - "y": 1.2879733857720441, - "heading": -0.3752023309051681, - "angularVelocity": 2.139583629256744e-7, - "velocityX": 3.524191190248399, - "velocityY": -1.3723206946612971, - "timestamp": 10.81071421953017 - }, - { - "x": 5.34411641028699, - "y": 1.2068681953777092, - "heading": -0.37520231550400196, - "angularVelocity": 2.0025101753440068e-7, - "velocityX": 3.6319552244350577, - "velocityY": -1.0545563098285724, - "timestamp": 10.88762352218042 - }, - { - "x": 5.629536746845857, - "y": 1.1508398853670567, - "heading": -0.375202300582326, - "angularVelocity": 1.940165285663064e-7, - "velocityX": 3.711128910592765, - "velocityY": -0.7284984791169633, - "timestamp": 10.964532824830668 - }, - { - "x": 5.917306487549604, - "y": 1.108501764833761, - "heading": -0.37520228573814113, - "angularVelocity": 1.9300896478374464e-7, - "velocityX": 3.741676634521091, - "velocityY": -0.5504941414672958, - "timestamp": 11.041442127480916 - }, - { - "x": 6.2050577552774975, - "y": 1.0660382742134262, - "heading": -0.37520227089383906, - "angularVelocity": 1.9301048840600142e-7, - "velocityX": 3.7414364428249387, - "velocityY": -0.5521242444940722, - "timestamp": 11.118351430131165 - }, - { - "x": 6.492809022919495, - "y": 1.0235747830116217, - "heading": -0.3752022560493111, - "angularVelocity": 1.9301342582848665e-7, - "velocityX": 3.7414364417080774, - "velocityY": -0.5521242520545259, - "timestamp": 11.195260732781414 - }, - { - "x": 6.772658036083171, - "y": 0.9819575118333774, - "heading": -0.34826612342726787, - "angularVelocity": 0.3502324386496837, - "velocityX": 3.638688734915609, - "velocityY": -0.541121421520385, - "timestamp": 11.272170035431662 - }, - { - "x": 7.027520568845826, - "y": 0.9441505058911935, - "heading": -0.3001058813779335, - "angularVelocity": 0.6261952766409414, - "velocityX": 3.313806314454086, - "velocityY": -0.49157910212908845, - "timestamp": 11.34907933808191 - }, - { - "x": 7.256873768560855, - "y": 0.9101570740557017, - "heading": -0.24860478708038097, - "angularVelocity": 0.6696341342705694, - "velocityX": 2.9821255922450605, - "velocityY": -0.44199375971564203, - "timestamp": 11.42598864073216 - }, - { - "x": 7.460704020800595, - "y": 0.8799617502558256, - "heading": -0.1984971865955797, - "angularVelocity": 0.6515154702763284, - "velocityX": 2.65026785077839, - "velocityY": -0.3926095122353607, - "timestamp": 11.502897943382408 - }, - { - "x": 7.6390235862814775, - "y": 0.8535543484261158, - "heading": -0.15201891065632164, - "angularVelocity": 0.6043258011403606, - "velocityX": 2.318569527171569, - "velocityY": -0.3433577073218239, - "timestamp": 11.579807246032656 - }, - { - "x": 7.7918452312093995, - "y": 0.8309283282540864, - "heading": -0.11047967273973482, - "angularVelocity": 0.5401068074364143, - "velocityX": 1.9870371939645617, - "velocityY": -0.29419094169820903, - "timestamp": 11.656716548682905 - }, - { - "x": 7.9191796079863, - "y": 0.812079256076005, - "heading": -0.07474204501348639, - "angularVelocity": 0.46467236725273836, - "velocityX": 1.6556433667844261, - "velocityY": -0.2450818240258775, - "timestamp": 11.733625851333153 - }, - { - "x": 8.021035341477521, - "y": 0.7970039548993032, - "heading": -0.04541767666824434, - "angularVelocity": 0.3812850635065168, - "velocityX": 1.3243616829347478, - "velocityY": -0.1960140146538302, - "timestamp": 11.810535153983402 - }, - { - "x": 8.097419437322872, - "y": 0.7857000289910334, - "heading": -0.022962750583734995, - "angularVelocity": 0.29196632020738633, - "velocityX": 0.9931710887135993, - "velocityY": -0.14697735538800996, - "timestamp": 11.88744445663365 - }, - { - "x": 8.148337646767295, - "y": 0.7781655823556067, - "heading": -0.0077300757248004864, - "angularVelocity": 0.19806023893112515, - "velocityX": 0.662055274067148, - "velocityY": -0.09796534847923602, - "timestamp": 11.964353759283899 + "heading": -0.3752014675107609, + "angularVelocity": 3.0467573669290407e-7, + "velocityX": 3.3886282543985438, + "velocityY": -1.679400107735582, + "timestamp": 10.733803790449713 + }, + { + "x": 5.064781872639773, + "y": 1.2879640439263618, + "heading": -0.3752014510522667, + "angularVelocity": 2.1399857496737658e-7, + "velocityX": 3.52414428888234, + "velocityY": -1.3724410814878705, + "timestamp": 10.810713153560702 + }, + { + "x": 5.344110469349436, + "y": 1.2068492677876004, + "heading": -0.3752014356488163, + "angularVelocity": 2.0028056132299027e-7, + "velocityX": 3.631919254181875, + "velocityY": -1.0546801177082226, + "timestamp": 10.88762251667169 + }, + { + "x": 5.629529124295685, + "y": 1.1508112126364383, + "heading": -0.37520142072574625, + "angularVelocity": 1.940345031855091e-7, + "velocityX": 3.711104128301771, + "velocityY": -0.7286246158389513, + "timestamp": 10.96453187978268 + }, + { + "x": 5.917283115305387, + "y": 1.1083646233041369, + "heading": -0.3752014058811779, + "angularVelocity": 1.9301379957946635e-7, + "velocityX": 3.7414689105466086, + "velocityY": -0.5519040545303511, + "timestamp": 11.041441242893669 + }, + { + "x": 6.205037197250251, + "y": 1.0659186504442004, + "heading": -0.37520139103661015, + "angularVelocity": 1.9301379056768152e-7, + "velocityX": 3.7414700929145033, + "velocityY": -0.5518960389605939, + "timestamp": 11.118350606004658 + }, + { + "x": 6.492791277445097, + "y": 1.0234726657206588, + "heading": -0.37520137619194305, + "angularVelocity": 1.9301508362380675e-7, + "velocityX": 3.741470070160208, + "velocityY": -0.5518961932149626, + "timestamp": 11.195259969115646 + }, + { + "x": 6.772643212230185, + "y": 0.9818724107444464, + "heading": -0.34826678356511775, + "angularVelocity": 0.35021213981392485, + "velocityX": 3.638723862284922, + "velocityY": -0.5408997460579521, + "timestamp": 11.272169332226635 + }, + { + "x": 7.027508446069354, + "y": 0.9440808910715871, + "heading": -0.30010699502671095, + "angularVelocity": 0.6261888876768776, + "velocityX": 3.313838829628223, + "velocityY": -0.49137735828500057, + "timestamp": 11.349078695337624 + }, + { + "x": 7.256864076564165, + "y": 0.9101013911553985, + "heading": -0.24860599276707973, + "angularVelocity": 0.66963241114492, + "velocityX": 2.9821548536792237, + "velocityY": -0.44181226500540305, + "timestamp": 11.425988058448613 + }, + { + "x": 7.460696487188184, + "y": 0.8799184472229736, + "heading": -0.19849831332672846, + "angularVelocity": 0.6515159847057984, + "velocityX": 2.6502938313228213, + "velocityY": -0.39244823661935213, + "timestamp": 11.502897421559602 + }, + { + "x": 7.639017939225139, + "y": 0.8535218749551334, + "heading": -0.15201987225869848, + "angularVelocity": 0.6043274731186726, + "velocityX": 2.3185922340771317, + "velocityY": -0.3432166279903691, + "timestamp": 11.579806784670591 + }, + { + "x": 7.7918411996459325, + "y": 0.8309051352911735, + "heading": -0.11048043087448678, + "angularVelocity": 0.5401090283931487, + "velocityX": 1.987056637047598, + "velocityY": -0.29407004230839406, + "timestamp": 11.65671614778158 + }, + { + "x": 7.919176921526017, + "y": 0.8120637954987597, + "heading": -0.0747425922933183, + "angularVelocity": 0.46467474356269306, + "velocityX": 1.655659554693303, + "velocityY": -0.24498109242204588, + "timestamp": 11.733625510892569 + }, + { + "x": 8.021033730291848, + "y": 0.7969946793008421, + "heading": -0.045418027645302235, + "angularVelocity": 0.3812873161580816, + "velocityX": 1.3243746228770636, + "velocityY": -0.19593344150011263, + "timestamp": 11.810534874003558 + }, + { + "x": 8.097418632050843, + "y": 0.7856953915277307, + "heading": -0.022962936429777186, + "angularVelocity": 0.29196823776995817, + "velocityX": 0.9931807866977714, + "velocityY": -0.14691693333626094, + "timestamp": 11.887444237114547 + }, + { + "x": 8.148337378443449, + "y": 0.7781640366357435, + "heading": -0.007730140883461889, + "angularVelocity": 0.198061652445786, + "velocityX": 0.6620617351768197, + "velocityY": -0.09792507163423873, + "timestamp": 11.964353600225536 }, { "x": 8.173794746398926, "y": 0.7743990421295166, - "heading": 1.1088837243155707e-19, - "angularVelocity": 0.10050898211824409, - "velocityX": 0.3310015661876223, - "velocityY": -0.04897379245809494, - "timestamp": 12.041263061934147 + "heading": -6.501598853172508e-27, + "angularVelocity": 0.10050975031878098, + "velocityX": 0.33100479480942824, + "velocityY": -0.04895365601706529, + "timestamp": 12.041262963336525 }, { "x": 8.173794746398926, "y": 0.7743990421295166, - "heading": 5.633915553103528e-20, - "angularVelocity": 2.8365160942051004e-20, - "velocityX": -1.2324505157130932e-18, - "velocityY": 3.669165812220002e-18, - "timestamp": 12.118172364584396 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 6 - ], - "type": "StopPoint", - "direction": 0 - }, - { - "scope": [ - 1 - ], - "type": "StopPoint", - "direction": 0 - }, - { - "scope": [ - 9 - ], - "type": "StopPoint" - }, - { - "scope": [ - 7, - 8 - ], - "type": "ZeroAngularVelocity" - }, - { - "scope": [ - 7, - 8 - ], - "type": "StraightLine" + "heading": -3.1768871176909477e-27, + "angularVelocity": 1.9243307208194474e-27, + "velocityX": 5.67400379522632e-27, + "velocityY": -1.0775303089806919e-26, + "timestamp": 12.118172326447514 } ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "SourceLanePHGSprint (1)": { - "waypoints": [ + "trajectoryWaypoints": [ { + "timestamp": 0, + "isStopPoint": true, "x": 0.43324199318885803, "y": 4.121134281158447, "heading": 0, @@ -25721,6 +26798,8 @@ "controlIntervalCount": 19 }, { + "timestamp": 1.4277752931645427, + "isStopPoint": true, "x": 2.3639590740203857, "y": 3.0491905212402344, "heading": -0.8022724119795859, @@ -25730,6 +26809,8 @@ "controlIntervalCount": 22 }, { + "timestamp": 2.7722119761090878, + "isStopPoint": false, "x": 5.5030035972595215, "y": 1.563418984413147, "heading": 0, @@ -25739,6 +26820,8 @@ "controlIntervalCount": 17 }, { + "timestamp": 3.3986072208880596, + "isStopPoint": false, "x": 7.626483917236328, "y": 2.082604169845581, "heading": 0.507098504392337, @@ -25748,6 +26831,8 @@ "controlIntervalCount": 9 }, { + "timestamp": 3.9697078859421278, + "isStopPoint": false, "x": 8.306674003601074, "y": 2.45849871635437, "heading": 0.507098504392337, @@ -25757,80 +26842,76 @@ "controlIntervalCount": 19 }, { + "timestamp": 5.220434007267402, + "isStopPoint": false, "x": 5.5030035972595215, "y": 1.563418984413147, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 22 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 25 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, + "timestamp": 6.898129373152603, + "isStopPoint": true, + "x": 1.664683222770691, + "y": 3.8699920177459717, + "heading": -0.7701709445795863, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 9 }, { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, + "timestamp": 7.4248020483514265, + "isStopPoint": false, + "x": 1.848745584487915, + "y": 4.087520122528076, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 10 }, { - "x": 8.385421752929688, - "y": 0.6878466606140137, - "heading": -0.34555563246426124, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, + "timestamp": 8.183677549140494, + "isStopPoint": false, + "x": 2.819255828857422, + "y": 4.10425329208374, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 22 + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, + "timestamp": 9.271616842955922, + "isStopPoint": true, + "x": 1.664683222770691, + "y": 3.8699920177459717, + "heading": -0.7701709445795863, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 24 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, + "timestamp": 10.733803790449713, + "isStopPoint": false, + "x": 4.7937421798706055, + "y": 1.3935176134109497, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 16 + "controlIntervalCount": 18 }, { - "x": 8.063037872314453, - "y": 0.7164599895477295, + "timestamp": 12.118172326447514, + "isStopPoint": true, + "x": 8.173794746398926, + "y": 0.7743990421295166, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -25838,2141 +26919,65 @@ "controlIntervalCount": 40 } ], - "trajectory": [ - { - "x": 0.433241993188858, - "y": 4.121134281158447, - "heading": -3.528930169589175e-36, - "angularVelocity": 0, - "velocityX": -3.4629926489463697e-37, - "velocityY": -7.866244288467422e-37, - "timestamp": 0 - }, - { - "x": 0.45469188032137553, - "y": 4.109218526997661, - "heading": -0.008937300145995173, - "angularVelocity": -0.1189323726299145, - "velocityX": 0.2854425752342375, - "velocityY": -0.15856789979825622, - "timestamp": 0.07514606787342622 - }, - { - "x": 0.49759166351162515, - "y": 4.0853873108981436, - "heading": -0.026812413296636604, - "angularVelocity": -0.237871570083344, - "velocityX": 0.5708852692400179, - "velocityY": -0.3171319108760035, - "timestamp": 0.15029213574685243 - }, - { - "x": 0.5619414537480725, - "y": 4.049641027140498, - "heading": -0.053621717098024775, - "angularVelocity": -0.3567625633658565, - "velocityX": 0.856329440215504, - "velocityY": -0.4756906750976659, - "timestamp": 0.22543820362027867 - }, - { - "x": 0.6477414523773106, - "y": 4.001980177860296, - "heading": -0.08935853104911512, - "angularVelocity": -0.47556465644063195, - "velocityX": 1.141776290593902, - "velocityY": -0.6342427571923072, - "timestamp": 0.30058427149370487 - }, - { - "x": 0.7549919358777414, - "y": 3.942405375853288, - "heading": -0.1340144220065316, - "angularVelocity": -0.5942545261667388, - "velocityX": 1.4272268201854585, - "velocityY": -0.7927866845588477, - "timestamp": 0.37573033936713107 - }, - { - "x": 0.8836932393269182, - "y": 3.8709173416225418, - "heading": -0.18758057158075767, - "angularVelocity": -0.7128270459134506, - "velocityX": 1.712681808793474, - "velocityY": -0.951321023891208, - "timestamp": 0.4508764072405573 - }, - { - "x": 1.0338457421402325, - "y": 3.787516893297593, - "heading": -0.2500489995023071, - "angularVelocity": -0.8312933688928251, - "velocityX": 1.9981418464400118, - "velocityY": -1.1098444760333435, - "timestamp": 0.5260224751139835 - }, - { - "x": 1.2054498603190866, - "y": 3.6922049311693006, - "heading": -0.32141344961465573, - "angularVelocity": -0.9496764385936043, - "velocityX": 2.283607419990343, - "velocityY": -1.268355947630325, - "timestamp": 0.6011685429874097 - }, - { - "x": 1.3985060453231255, - "y": 3.584982424979259, - "heading": -0.4016698579859379, - "angularVelocity": -1.0680054278616897, - "velocityX": 2.5690790013020615, - "velocityY": -1.4268545144723217, - "timestamp": 0.676314610860836 - }, - { - "x": 1.5915937569821008, - "y": 3.4778198154599647, - "heading": -0.48176859267038585, - "angularVelocity": -1.065907198489266, - "velocityX": 2.5694985396202825, - "velocityY": -1.4260574445464829, - "timestamp": 0.7514606787342623 - }, - { - "x": 1.7632284858900336, - "y": 3.3825660365200445, - "heading": -0.5529765476804539, - "angularVelocity": -0.9475938931363466, - "velocityX": 2.2840147697019786, - "velocityY": -1.2675816797275798, - "timestamp": 0.8266067466076885 - }, - { - "x": 1.9134098098562764, - "y": 3.2992204007390877, - "heading": -0.6152918418017526, - "angularVelocity": -0.8292555536806094, - "velocityX": 1.9985253815170194, - "velocityY": -1.1091150626981794, - "timestamp": 0.9017528144811148 - }, - { - "x": 2.0421373577646693, - "y": 3.2277823059512354, - "heading": -0.6687115003742365, - "angularVelocity": -0.7108776291856339, - "velocityX": 1.7130310547348626, - "velocityY": -0.9506564589404817, - "timestamp": 0.976898882354541 - }, - { - "x": 2.149410808485792, - "y": 3.1682512394298143, - "heading": -0.7132320418113071, - "angularVelocity": -0.5924533737687983, - "velocityX": 1.4275324545498613, - "velocityY": -0.7922046782500071, - "timestamp": 1.0520449502279672 - }, - { - "x": 2.2352298936713018, - "y": 3.120626784362015, - "heading": -0.7488501306819737, - "angularVelocity": -0.4739847323836121, - "velocityX": 1.1420302833417786, - "velocityY": -0.6337584442610683, - "timestamp": 1.1271910181013933 - }, - { - "x": 2.2995943987852443, - "y": 3.0849086278583613, - "heading": -0.775563262460788, - "angularVelocity": -0.3554827622359309, - "velocityX": 0.8565252572144717, - "velocityY": -0.4753163740226096, - "timestamp": 1.2023370859748195 - }, - { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080858, - "angularVelocity": -0.23696688264902366, - "velocityX": 0.5710180813549187, - "velocityY": -0.3168769856057537, - "timestamp": 1.2774831538482456 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550472, - "velocityX": 0.28550943973314774, - "velocityY": -0.15843872456938407, - "timestamp": 1.3526292217216718 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 9.576319315825878e-33, - "velocityX": -3.340074147697442e-33, - "velocityY": 1.4972964833121403e-33, - "timestamp": 1.427775289595098 - }, - { - "x": 2.3773546785315864, - "y": 3.0407954088560687, - "heading": -0.7860770412156382, - "angularVelocity": 0.26500042058499174, - "velocityX": 0.21918861143707746, - "velocityY": -0.13736692695017713, - "timestamp": 1.4888897992558612 - }, - { - "x": 2.4041715283150715, - "y": 3.0239975454455843, - "heading": -0.7541197928104101, - "angularVelocity": 0.5229077118120982, - "velocityX": 0.438796775632191, - "velocityY": -0.27485884291187945, - "timestamp": 1.5500043089166244 - }, - { - "x": 2.4444388170293236, - "y": 2.998788446329585, - "heading": -0.7069088657797715, - "angularVelocity": 0.7724994815911764, - "velocityX": 0.6588826276733535, - "velocityY": -0.41248959135778174, - "timestamp": 1.6111188185773877 - }, - { - "x": 2.498190798561039, - "y": 2.965157152573787, - "heading": -0.6450804787000076, - "angularVelocity": 1.0116809808826634, - "velocityX": 0.8795289666903056, - "velocityY": -0.5502996578468836, - "timestamp": 1.672233328238151 - }, - { - "x": 2.5654677170302604, - "y": 2.923088008476279, - "heading": -0.5694581806203519, - "angularVelocity": 1.2373869724133075, - "velocityX": 1.1008338092322851, - "velocityY": -0.6883658943027918, - "timestamp": 1.7333478378989142 - }, - { - "x": 2.646315775914165, - "y": 2.8725583813996622, - "heading": -0.48110952859275347, - "angularVelocity": 1.4456248199978636, - "velocityX": 1.3228946666295571, - "velocityY": -0.8268024624119353, - "timestamp": 1.7944623475596775 - }, - { - "x": 2.7407862870930626, - "y": 2.813537598490058, - "heading": -0.3813947698765895, - "angularVelocity": 1.631605313855327, - "velocityX": 1.5457951262848801, - "velocityY": -0.965740922036666, - "timestamp": 1.8555768572204407 - }, - { - "x": 2.84893500974016, - "y": 2.7459886359527177, - "heading": -0.2720274509201168, - "angularVelocity": 1.7895475160244767, - "velocityX": 1.7696079580350594, - "velocityY": -1.105285191884772, - "timestamp": 1.916691366881204 - }, - { - "x": 2.9708226363170516, - "y": 2.669873093439706, - "heading": -0.15523119808259803, - "angularVelocity": 1.9111051284848068, - "velocityX": 1.9944138839282306, - "velocityY": -1.2454577961194009, - "timestamp": 1.9778058765419673 - }, - { - "x": 3.1065122638812066, - "y": 2.5851587340080315, - "heading": -0.03419676462946552, - "angularVelocity": 1.9804533182868547, - "velocityX": 2.220252249708728, - "velocityY": -1.3861578846318265, - "timestamp": 2.0389203862027303 - }, - { - "x": 3.2560356571448454, - "y": 2.4918371927335334, - "heading": 0.08572826850512158, - "angularVelocity": 1.9623005044182085, - "velocityX": 2.446610372784124, - "velocityY": -1.5269948461095475, - "timestamp": 2.1000348958634936 - }, - { - "x": 3.419180860242083, - "y": 2.390029027608911, - "heading": 0.19415018142326163, - "angularVelocity": 1.7740780956923754, - "velocityX": 2.6695003200194134, - "velocityY": -1.6658591501387063, - "timestamp": 2.161149405524257 - }, - { - "x": 3.5929644641848295, - "y": 2.279455345020229, - "heading": 0.2670576620012504, - "angularVelocity": 1.1929651564364419, - "velocityX": 2.8435735622750062, - "velocityY": -1.809286914064415, - "timestamp": 2.22226391518502 - }, - { - "x": 3.776865546474961, - "y": 2.1600020280366277, - "heading": 0.3015223704483068, - "angularVelocity": 0.5639365944088309, - "velocityX": 3.0091230922237155, - "velocityY": -1.9545819421061736, - "timestamp": 2.2833784248457833 - }, - { - "x": 3.9728967066647076, - "y": 2.0375522576737963, - "heading": 0.30152241765502197, - "angularVelocity": 7.724305629887772e-7, - "velocityX": 3.2076042379769354, - "velocityY": -2.003612088889046, - "timestamp": 2.3444929345065466 - }, - { - "x": 4.174386727718979, - "y": 1.9243094504352285, - "heading": 0.30152246485493484, - "angularVelocity": 7.72319260587173e-7, - "velocityX": 3.2969260847008353, - "velocityY": -1.8529610704096402, - "timestamp": 2.40560744416731 - }, - { - "x": 4.383354452565465, - "y": 1.8255438101811148, - "heading": 0.3015225128834295, - "angularVelocity": 7.858771167686507e-7, - "velocityX": 3.4192817058736558, - "velocityY": -1.6160751481496929, - "timestamp": 2.466721953828073 - }, - { - "x": 4.598761540028333, - "y": 1.7417467464566943, - "heading": 0.30152256294147645, - "angularVelocity": 8.190861254028443e-7, - "velocityX": 3.524647234487482, - "velocityY": -1.3711484259558735, - "timestamp": 2.5278364634888364 - }, - { - "x": 4.819537390717514, - "y": 1.6733348748555603, - "heading": 0.3015226164690873, - "angularVelocity": 8.758576513463982e-7, - "velocityX": 3.612494838208991, - "velocityY": -1.1194047367945394, - "timestamp": 2.5889509731495997 - }, - { - "x": 5.04458467733647, - "y": 1.620648272623701, - "heading": 0.30152267534909954, - "angularVelocity": 9.634375302003616e-7, - "velocityX": 3.682387175617667, - "velocityY": -0.8620964567058493, - "timestamp": 2.650065482810363 - }, - { - "x": 5.27278482830379, - "y": 1.5839488347993986, - "heading": 0.3015227422489141, - "angularVelocity": 0.0000010946633609592033, - "velocityX": 3.7339766322927472, - "velocityY": -0.6005028597630111, - "timestamp": 2.711179992471126 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.3015228161300745, - "angularVelocity": 0.0000012088972118666571, - "velocityX": 3.7670067261218194, - "velocityY": -0.33592432468507805, - "timestamp": 2.7722945021318894 - }, - { - "x": 5.642161233224445, - "y": 1.5569512204704454, - "heading": 0.30152289610635336, - "angularVelocity": 0.0000021712102287950276, - "velocityX": 3.777876226736954, - "velocityY": -0.1755880047102645, - "timestamp": 2.809129387028348 - }, - { - "x": 5.781468008961295, - "y": 1.556401100893367, - "heading": 0.3015229698630755, - "angularVelocity": 0.0000020023605971612715, - "velocityX": 3.7819250997644014, - "velocityY": -0.01493474402390838, - "timestamp": 2.8459642719248066 - }, - { - "x": 5.920672391336217, - "y": 1.5617696165699877, - "heading": 0.3015230386365789, - "angularVelocity": 0.0000018670752917486593, - "velocityX": 3.779145306581512, - "velocityY": 0.14574541746801997, - "timestamp": 2.882799156821265 - }, - { - "x": 6.0595230325638285, - "y": 1.5730470711005764, - "heading": 0.30152310340837535, - "angularVelocity": 0.000001758436237719359, - "velocityX": 3.769541879061509, - "velocityY": 0.3061623393771694, - "timestamp": 2.919634041717724 - }, - { - "x": 6.197769224123459, - "y": 1.5902130982406153, - "heading": 0.3015231649742211, - "angularVelocity": 0.0000016714005183062998, - "velocityX": 3.753132172076425, - "velocityY": 0.4660263548614807, - "timestamp": 2.9564689266141824 - }, - { - "x": 6.335161349542959, - "y": 1.6132366986444322, - "heading": 0.3015232239953201, - "angularVelocity": 0.0000016023152826544925, - "velocityX": 3.7299458327534505, - "velocityY": 0.6250487946015111, - "timestamp": 2.993303811510641 - }, - { - "x": 6.471451335210211, - "y": 1.6420762959087596, - "heading": 0.3015232810350899, - "angularVelocity": 0.000001548525804727782, - "velocityX": 3.7000247469309935, - "velocityY": 0.7829425107583309, - "timestamp": 3.0301386964070995 - }, - { - "x": 6.606393098338155, - "y": 1.6766798119201123, - "heading": 0.3015233366126912, - "angularVelocity": 0.0000015088305949550033, - "velocityX": 3.6634229618813627, - "velocityY": 0.9394224010369983, - "timestamp": 3.066973581303558 - }, - { - "x": 6.739742463422568, - "y": 1.7169846140875678, - "heading": 0.30152556567084704, - "angularVelocity": 0.00006051486687687375, - "velocityX": 3.6201922568579414, - "velocityY": 1.0942019306087243, - "timestamp": 3.1038084662000167 - }, - { - "x": 6.8694207904091344, - "y": 1.7614703171513668, - "heading": 0.3101205256410533, - "angularVelocity": 0.23333750042565127, - "velocityX": 3.520530262306672, - "velocityY": 1.2077057710061154, - "timestamp": 3.1406433510964753 - }, - { - "x": 6.994764102779472, - "y": 1.8086931078781319, - "heading": 0.3306180633638748, - "angularVelocity": 0.5564707961064402, - "velocityX": 3.4028425152588877, - "velocityY": 1.2820127132066854, - "timestamp": 3.177478235992934 - }, - { - "x": 7.114597005425528, - "y": 1.8565129203183168, - "heading": 0.359627713397502, - "angularVelocity": 0.7875591335542955, - "velocityX": 3.2532449329732183, - "velocityY": 1.298220764761567, - "timestamp": 3.2143131208893925 - }, - { - "x": 7.228563598795877, - "y": 1.903963761474251, - "heading": 0.39146295222083616, - "angularVelocity": 0.8642687200685345, - "velocityX": 3.0939853264291135, - "velocityY": 1.288203866777826, - "timestamp": 3.251148005785851 - }, - { - "x": 7.336676614220082, - "y": 1.9505339221399112, - "heading": 0.42341393247502157, - "angularVelocity": 0.8674108889982476, - "velocityX": 2.9350713522821095, - "velocityY": 1.2642949963483492, - "timestamp": 3.2879828906823096 - }, - { - "x": 7.438996464546078, - "y": 1.9959395107165516, - "heading": 0.45395278545537515, - "angularVelocity": 0.8290742068611577, - "velocityX": 2.777797476865021, - "velocityY": 1.2326789863542045, - "timestamp": 3.3248177755787682 - }, - { - "x": 7.535582073954796, - "y": 2.0400031624519186, - "heading": 0.4820829146791016, - "angularVelocity": 0.7636817463336469, - "velocityX": 2.622123285581467, - "velocityY": 1.1962478465516526, - "timestamp": 3.361652660475227 - }, - { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.6791276743107278, - "velocityX": 2.4678193928677445, - "velocityY": 1.1565397180800696, - "timestamp": 3.3984875453716854 - }, - { - "x": 7.766333855095899, - "y": 2.151391160116789, - "heading": 0.5393931714770351, - "angularVelocity": 0.508950730683683, - "velocityX": 2.2039777611896034, - "velocityY": 1.0840548028640593, - "timestamp": 3.461940970502476 - }, - { - "x": 7.8897168213730975, - "y": 2.2145350229010927, - "heading": 0.5621690879226864, - "angularVelocity": 0.3589391179231936, - "velocityX": 1.9444650312080196, - "velocityY": 0.9951214241650663, - "timestamp": 3.5253943956332665 - }, - { - "x": 7.996856912991746, - "y": 2.2713427782005033, - "heading": 0.5763089063837279, - "angularVelocity": 0.22283774960762856, - "velocityX": 1.6884839769927564, - "velocityY": 0.8952669644281364, - "timestamp": 3.588847820764057 - }, - { - "x": 8.08793407853681, - "y": 2.3213235528188796, - "heading": 0.5824533246990756, - "angularVelocity": 0.09683351690917823, - "velocityX": 1.4353388387992243, - "velocityY": 0.7876765441007405, - "timestamp": 3.6523012458948476 - }, - { - "x": 8.16309393881241, - "y": 2.364112462024999, - "heading": 0.5810872026138336, - "angularVelocity": -0.021529524725041688, - "velocityX": 1.1844886248564515, - "velocityY": 0.6743356898059116, - "timestamp": 3.715754671025638 - }, - { - "x": 8.222455939995415, - "y": 2.3994280671308665, - "heading": 0.5725898810376202, - "angularVelocity": -0.13391430893917458, - "velocityX": 0.9355208337555886, - "velocityY": 0.5565594770191679, - "timestamp": 3.7792080961564287 - }, - { - "x": 8.266119462815809, - "y": 2.427046891293226, - "heading": 0.5572661381126698, - "angularVelocity": -0.24149591441856877, - "velocityX": 0.6881192422693246, - "velocityY": 0.4352613606819736, - "timestamp": 3.8426615212872193 - }, - { - "x": 8.294168308263885, - "y": 2.446787281465013, - "heading": 0.5353660980042331, - "angularVelocity": -0.3451356654632322, - "velocityX": 0.4420383200790658, - "velocityY": 0.31110046669817776, - "timestamp": 3.90611494641801 - }, - { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.4454857015776663, - "velocityX": 0.19708463824313482, - "velocityY": 0.18456741878341767, - "timestamp": 3.9695683715488004 - }, - { - "x": 8.303024530407173, - "y": 2.4618811275821986, - "heading": 0.4711910323232683, - "angularVelocity": -0.5465136547157353, - "velocityX": -0.055545178149817624, - "velocityY": 0.05148048067311967, - "timestamp": 4.035271161880262 - }, - { - "x": 8.282750419132642, - "y": 2.456558519757899, - "heading": 0.42887568169046636, - "angularVelocity": -0.6440419108431609, - "velocityX": -0.30857306321773753, - "velocityY": -0.08101037714605691, - "timestamp": 4.1009739522117235 - }, - { - "x": 8.245821395629305, - "y": 2.4425769262851884, - "heading": 0.3804195474569322, - "angularVelocity": -0.7375049672788591, - "velocityX": -0.5620617224479451, - "velocityY": -0.2128006040866038, - "timestamp": 4.166676742543185 - }, - { - "x": 8.19220201387014, - "y": 2.4199912479232255, - "heading": 0.32613641295591106, - "angularVelocity": -0.8261922245184766, - "velocityX": -0.8160898721144464, - "velocityY": -0.3437552385221621, - "timestamp": 4.2323795328746465 - }, - { - "x": 8.121850214183777, - "y": 2.388868090056124, - "heading": 0.2664003971864744, - "angularVelocity": -0.9091853704854365, - "velocityX": -1.0707581722397086, - "velocityY": -0.4736961354318394, - "timestamp": 4.298082323206108 - }, - { - "x": 8.034715295558527, - "y": 2.3492899199909276, - "heading": 0.20166644793914107, - "angularVelocity": -0.9852541866298157, - "velocityX": -1.32619814448768, - "velocityY": -0.6023818754961511, - "timestamp": 4.36378511353757 - }, - { - "x": 7.930734985336223, - "y": 2.3013613961682027, - "heading": 0.13250258742090648, - "angularVelocity": -1.0526776742557293, - "velocityX": -1.582585909939886, - "velocityY": -0.7294747084702521, - "timestamp": 4.429487903869031 - }, - { - "x": 7.809831081747402, - "y": 2.2452194829459002, - "heading": 0.05964299707705179, - "angularVelocity": -1.1089268808263428, - "velocityX": -1.8401639105261383, - "velocityY": -0.8544829365552755, - "timestamp": 4.495190694200493 - }, - { - "x": 7.671902780233126, - "y": 2.1810506699963903, - "heading": -0.015920256793081004, - "angularVelocity": -1.1500767850029923, - "velocityX": -2.099276161917122, - "velocityY": -0.976652781804051, - "timestamp": 4.560893484531954 - }, - { - "x": 7.516816185598605, - "y": 2.109122861945235, - "heading": -0.09276870443286901, - "angularVelocity": -1.1696375032490753, - "velocityX": -2.360426305368932, - "velocityY": -1.0947451042534073, - "timestamp": 4.626596274863416 - }, - { - "x": 7.344387868807497, - "y": 2.0298516556077795, - "heading": -0.16871929267657423, - "angularVelocity": -1.155972034985807, - "velocityX": -2.624368248612144, - "velocityY": -1.2065120208372122, - "timestamp": 4.692299065194877 - }, - { - "x": 7.154363408340053, - "y": 1.943962982346676, - "heading": -0.2400485240661189, - "angularVelocity": -1.0856347352935647, - "velocityX": -2.892182501059652, - "velocityY": -1.3072302230667407, - "timestamp": 4.758001855526339 - }, - { - "x": 6.946444183851081, - "y": 1.8530162365161487, - "heading": -0.2992772419214472, - "angularVelocity": -0.9014642689682999, - "velocityX": -3.164541771210153, - "velocityY": -1.3842143594162997, - "timestamp": 4.8237046458578 - }, - { - "x": 6.72212326982476, - "y": 1.762675190878268, - "heading": -0.31855321839260486, - "angularVelocity": -0.29338139786625583, - "velocityX": -3.4141763674670904, - "velocityY": -1.3749955699312393, - "timestamp": 4.889407436189262 - }, - { - "x": 6.4857597450479005, - "y": 1.6860130581467423, - "heading": -0.3185532921212083, - "angularVelocity": -0.000001122153306519187, - "velocityX": -3.5974655503128186, - "velocityY": -1.1668017803319486, - "timestamp": 4.955110226520723 - }, - { - "x": 6.24426876944993, - "y": 1.6274732096370486, - "heading": -0.3185533177664737, - "angularVelocity": -3.903223175651228e-7, - "velocityX": -3.6755056273817623, - "velocityY": -0.8909796405048931, - "timestamp": 5.020813016852185 - }, - { - "x": 5.999037658914315, - "y": 1.5873919634602989, - "heading": -0.3185533462426245, - "angularVelocity": -4.3340854500246066e-7, - "velocityX": -3.732430682143934, - "velocityY": -0.6100387209515066, - "timestamp": 5.0865158071836465 - }, - { - "x": 5.751475205523387, - "y": 1.5659995878977724, - "heading": -0.3185533789943253, - "angularVelocity": -4.984826460154038e-7, - "velocityX": -3.7679138456983283, - "velocityY": -0.32559310578142847, - "timestamp": 5.152218597515108 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.3185534180019773, - "angularVelocity": -5.936985598886166e-7, - "velocityX": -3.781751231726404, - "velocityY": -0.03927692372891183, - "timestamp": 5.21792138784657 - }, - { - "x": 5.26265149067387, - "y": 1.5786031053003824, - "heading": -0.31855345112044986, - "angularVelocity": -5.200844119271939e-7, - "velocityX": -3.774430827391544, - "velocityY": 0.23844772894968422, - "timestamp": 5.28160042121006 - }, - { - "x": 5.0240625547753925, - "y": 1.61139052009726, - "heading": -0.3185534786660252, - "angularVelocity": -4.325689925822067e-7, - "velocityX": -3.746742425196293, - "velocityY": 0.5148855606793217, - "timestamp": 5.34527945457355 - }, - { - "x": 4.788524290519247, - "y": 1.6616042890226763, - "heading": -0.31855350247111613, - "angularVelocity": -3.7382933976434193e-7, - "velocityX": -3.698835422197071, - "velocityY": 0.7885447732660757, - "timestamp": 5.40895848793704 - }, - { - "x": 4.55730773239592, - "y": 1.7289734284079032, - "heading": -0.3185535237219762, - "angularVelocity": -3.3371831981217076e-7, - "velocityX": -3.6309684037367047, - "velocityY": 1.0579485244487483, - "timestamp": 5.4726375213005305 - }, - { - "x": 4.33166058516164, - "y": 1.8131343663413289, - "heading": -0.31855354324045854, - "angularVelocity": -3.06513484100539e-7, - "velocityX": -3.5435077342686676, - "velocityY": 1.3216428310558945, - "timestamp": 5.536316554664021 - }, - { - "x": 4.112800476187319, - "y": 1.9136328827222213, - "heading": -0.31855356163583604, - "angularVelocity": -2.8887651909639526e-7, - "velocityX": -3.4369257417120753, - "velocityY": 1.5782041760469425, - "timestamp": 5.599995588027511 - }, - { - "x": 3.9019083093617053, - "y": 2.029926448332014, - "heading": -0.3185535793939757, - "angularVelocity": -2.788694925452677e-7, - "velocityX": -3.3117991226689654, - "velocityY": 1.8262457745859704, - "timestamp": 5.663674621391001 - }, - { - "x": 3.700120307432907, - "y": 2.161384813738153, - "heading": -0.3185535969362317, - "angularVelocity": -2.7547930719470604e-7, - "velocityX": -3.1688295388681516, - "velocityY": 2.0643900898393666, - "timestamp": 5.727353654754491 - }, - { - "x": 3.5092854049648876, - "y": 2.2879634464070207, - "heading": -0.3579155843505842, - "angularVelocity": -0.618131044635493, - "velocityX": -2.996824737880426, - "velocityY": 1.9877599577609333, - "timestamp": 5.791032688117982 - }, - { - "x": 3.3329097555386666, - "y": 2.4050971261724468, - "heading": -0.41294492327597715, - "angularVelocity": -0.8641673093760206, - "velocityX": -2.7697601566820413, - "velocityY": 1.8394387222684179, - "timestamp": 5.854711721481472 - }, - { - "x": 3.171287726021972, - "y": 2.512479984884094, - "heading": -0.46992046584984914, - "angularVelocity": -0.8947300165290927, - "velocityX": -2.538072910028812, - "velocityY": 1.686314207985683, - "timestamp": 5.918390754844962 - }, - { - "x": 3.024408806786741, - "y": 2.6100923699912792, - "heading": -0.5252799185719805, - "angularVelocity": -0.8693513358805383, - "velocityX": -2.306550704009953, - "velocityY": 1.532881074842923, - "timestamp": 5.982069788208452 - }, - { - "x": 2.892255079193046, - "y": 2.6979336450739244, - "heading": -0.5773440254447586, - "angularVelocity": -0.8176020288434342, - "velocityX": -2.0753098879397363, - "velocityY": 1.3794379475145806, - "timestamp": 6.045748821571943 - }, - { - "x": 2.7748123815012584, - "y": 2.776006230905406, - "heading": -0.625128837055519, - "angularVelocity": -0.7504010203483642, - "velocityX": -1.8442914643726742, - "velocityY": 1.226032835420591, - "timestamp": 6.109427854935433 - }, - { - "x": 2.6720699818921556, - "y": 2.8443129166968544, - "heading": -0.6679860381773632, - "angularVelocity": -0.6730190277419648, - "velocityX": -1.6134415706757543, - "velocityY": 1.0726715244175118, - "timestamp": 6.173106888298923 - }, - { - "x": 2.5840196243973645, - "y": 2.9028562717726505, - "heading": -0.7054558016839699, - "angularVelocity": -0.5884160221579269, - "velocityX": -1.382721326691407, - "velocityY": 0.9193505614575074, - "timestamp": 6.236785921662413 - }, - { - "x": 2.510654811151598, - "y": 2.95163853061359, - "heading": -0.7371951288142529, - "angularVelocity": -0.4984266477336411, - "velocityX": -1.1521031235978156, - "velocityY": 0.7660646882386302, - "timestamp": 6.3004649550259035 - }, - { - "x": 2.4519703126340358, - "y": 2.9906615990271557, - "heading": -0.7629386573341135, - "angularVelocity": -0.4042700895428554, - "velocityX": -0.9215670436230067, - "velocityY": 0.6128087433553877, - "timestamp": 6.364143988389394 - }, - { - "x": 2.4079618327799786, - "y": 3.0199270886206313, - "heading": -0.7824753669479414, - "angularVelocity": -0.30679972012623624, - "velocityX": -0.6910984279998401, - "velocityY": 0.45957810675962124, - "timestamp": 6.427823021752884 - }, - { - "x": 2.378625774904473, - "y": 3.039436353320747, - "heading": -0.795633833561549, - "angularVelocity": -0.2066373485680448, - "velocityX": -0.4606862938394533, - "velocityY": 0.3063687318988397, - "timestamp": 6.491502055116374 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.10425061542851646, - "velocityX": -0.2303222914890624, - "velocityY": 0.15317707264507013, - "timestamp": 6.5551810884798645 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -7.869331036874185e-31, - "timestamp": 6.618860121843355 - }, - { - "x": 2.3794831364602356, - "y": 3.04172499089676, - "heading": -0.7962438371233982, - "angularVelocity": 0.09576162685715454, - "velocityX": 0.24659384848580665, - "velocityY": -0.11858712018957836, - "timestamp": 6.6818140946823945 - }, - { - "x": 2.4105331611816494, - "y": 3.0267927192788293, - "heading": -0.7842933789401895, - "angularVelocity": 0.1898284992078828, - "velocityX": 0.49321787523723476, - "velocityY": -0.23719347555886855, - "timestamp": 6.744768067521434 - }, - { - "x": 2.457111347137164, - "y": 3.0043922693157348, - "heading": -0.7665480516906409, - "angularVelocity": 0.2818777981640571, - "velocityX": 0.7398768315163422, - "velocityY": -0.3558226582517257, - "timestamp": 6.807722040360474 - }, - { - "x": 2.5192202688476826, - "y": 2.9745219425624034, - "heading": -0.7431604801963628, - "angularVelocity": 0.37150270967132, - "velocityX": 0.9865766830842898, - "velocityY": -0.4744788201008906, - "timestamp": 6.870676013199514 - }, - { - "x": 2.5968629770363694, - "y": 2.9371797310172574, - "heading": -0.7143164372650311, - "angularVelocity": 0.45817669053357035, - "velocityX": 1.2333249942335256, - "velocityY": -0.5931668782941151, - "timestamp": 6.933629986038554 - }, - { - "x": 2.690043132952377, - "y": 2.892363250439385, - "heading": -0.6802459845187668, - "angularVelocity": 0.5411962297181008, - "velocityX": 1.4801314629379994, - "velocityY": -0.711892809250637, - "timestamp": 6.9965839588775935 - }, - { - "x": 2.798765189175504, - "y": 2.840069647867451, - "heading": -0.6412406321012497, - "angularVelocity": 0.6195852407479587, - "velocityX": 1.7270086591851181, - "velocityY": -0.8306640584167341, - "timestamp": 7.059537931716633 - }, - { - "x": 2.9230346316689584, - "y": 2.7802954749466373, - "heading": -0.5976811625941267, - "angularVelocity": 0.6919256647788576, - "velocityX": 1.9739729978787044, - "velocityY": -0.9494900833922507, - "timestamp": 7.122491904555673 - }, - { - "x": 3.0628582875020656, - "y": 2.7130365228135442, - "heading": -0.550085909804202, - "angularVelocity": 0.7560325527288918, - "velocityX": 2.2210457819811675, - "velocityY": -1.0683829645677803, - "timestamp": 7.185445877394713 - }, - { - "x": 3.2182446226798445, - "y": 2.6382876468732976, - "heading": -0.4992026727158649, - "angularVelocity": 0.8082609372157451, - "velocityX": 2.4682530453648863, - "velocityY": -1.1873575656831603, - "timestamp": 7.248399850233753 - }, - { - "x": 3.3892034750687743, - "y": 2.5560428034877645, - "heading": -0.4462086118156311, - "angularVelocity": 0.8417905734992818, - "velocityX": 2.7156165795292284, - "velocityY": -1.306428167699852, - "timestamp": 7.311353823072793 - }, - { - "x": 3.57574133188106, - "y": 2.4662968714042055, - "heading": -0.3932458693377865, - "angularVelocity": 0.8412930922288103, - "velocityX": 2.963083160600891, - "velocityY": -1.4255801188754254, - "timestamp": 7.374307795911832 - }, - { - "x": 3.777810779911522, - "y": 2.369066952878771, - "heading": -0.3455559373144148, - "angularVelocity": 0.7575364964702819, - "velocityX": 3.209796600877132, - "velocityY": -1.5444604071935277, - "timestamp": 7.437261768750872 - }, - { - "x": 3.9925130304181535, - "y": 2.2661623966169606, - "heading": -0.3455557491568548, - "angularVelocity": 0.0000029888115329808544, - "velocityX": 3.4104638805811245, - "velocityY": -1.63459987703898, - "timestamp": 7.500215741589912 - }, - { - "x": 4.207067940824861, - "y": 2.162950872410893, - "heading": -0.34555573251479127, - "angularVelocity": 2.643528717349897e-7, - "velocityX": 3.408123438933375, - "velocityY": -1.6394759464975768, - "timestamp": 7.563169714428952 - }, - { - "x": 4.421622842445454, - "y": 2.0597393299403546, - "heading": -0.34555571587272765, - "angularVelocity": 2.6435287281597733e-7, - "velocityX": 3.408123299369276, - "velocityY": -1.6394762366217663, - "timestamp": 7.626123687267992 - }, - { - "x": 4.636177744065525, - "y": 1.9565277874687297, - "heading": -0.3455556992306641, - "angularVelocity": 2.6435287314542613e-7, - "velocityX": 3.4081232993609745, - "velocityY": -1.6394762366390223, - "timestamp": 7.689077660107031 - }, - { - "x": 4.850732645688907, - "y": 1.8533162450039902, - "heading": -0.34555568258860053, - "angularVelocity": 2.643528725209065e-7, - "velocityX": 3.408123299413585, - "velocityY": -1.6394762365296556, - "timestamp": 7.752031632946071 - }, - { - "x": 5.065287602977393, - "y": 1.7501048182552732, - "heading": -0.345555665946537, - "angularVelocity": 2.6435287138683083e-7, - "velocityX": 3.4081241836326663, - "velocityY": -1.63947439842449, - "timestamp": 7.814985605785111 - }, - { - "x": 5.280754789180012, - "y": 1.6488115807144432, - "heading": -0.34555564929395255, - "angularVelocity": 2.6451999265849417e-7, - "velocityX": 3.4226145942134942, - "velocityY": -1.6090046898202166, - "timestamp": 7.877939578624151 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, - "angularVelocity": 2.673332686258823e-7, - "velocityX": 3.530338087601756, - "velocityY": -1.3564290298187671, - "timestamp": 7.940893551463191 - }, - { - "x": 5.652794664229654, - "y": 1.5139935049725783, - "heading": -0.34555563561361163, - "angularVelocity": -7.551097078688917e-8, - "velocityX": 3.591492706822241, - "velocityY": -1.185058979367509, - "timestamp": 7.982600740979571 - }, - { - "x": 5.804789442560329, - "y": 1.4718291930788845, - "heading": -0.345555638724863, - "angularVelocity": -7.459748339352162e-8, - "velocityX": 3.644330392269195, - "velocityY": -1.010960277655107, - "timestamp": 8.02430793049595 - }, - { - "x": 5.958396699024851, - "y": 1.435981562464388, - "heading": -0.3455556418269712, - "angularVelocity": -7.437825995471752e-8, - "velocityX": 3.6829922669374113, - "velocityY": -0.8595072223799174, - "timestamp": 8.066015120012331 - }, - { - "x": 6.112005102436535, - "y": 1.4001388468787674, - "heading": -0.3455556449290776, - "angularVelocity": -7.437821592181375e-8, - "velocityX": 3.6830197669242586, - "velocityY": -0.8593893762979032, - "timestamp": 8.107722309528711 - }, - { - "x": 6.2656135060730795, - "y": 1.3642961322568121, - "heading": -0.3455556480311839, - "angularVelocity": -7.437821455888137e-8, - "velocityX": 3.683019772315654, - "velocityY": -0.8593893531924107, - "timestamp": 8.149429499045091 - }, - { - "x": 6.419221909709655, - "y": 1.3284534176349936, - "heading": -0.3455556511332902, - "angularVelocity": -7.437821520024811e-8, - "velocityX": 3.6830197723164173, - "velocityY": -0.8593893531891351, - "timestamp": 8.191136688561471 - }, - { - "x": 6.572830313283965, - "y": 1.292610702746327, - "heading": -0.3455556542353965, - "angularVelocity": -7.437821564310945e-8, - "velocityX": 3.6830197708234897, - "velocityY": -0.859389359587264, - "timestamp": 8.232843878077851 - }, - { - "x": 6.72643839927336, - "y": 1.2567666268353574, - "heading": -0.34555565733750304, - "angularVelocity": -7.43782201062097e-8, - "velocityX": 3.6830121561911366, - "velocityY": -0.8594219923855618, - "timestamp": 8.274551067594231 - }, - { - "x": 6.87895592150965, - "y": 1.216534214678565, - "heading": -0.3455556607967647, - "angularVelocity": -8.294161584911855e-8, - "velocityX": 3.656864056409029, - "velocityY": -0.9646397329408027, - "timestamp": 8.316258257110611 - }, - { - "x": 7.025398163354816, - "y": 1.1717644512232448, - "heading": -0.34674583940477344, - "angularVelocity": -0.02853653343247435, - "velocityX": 3.5111989933450576, - "velocityY": -1.073430359955979, - "timestamp": 8.357965446626991 - }, - { - "x": 7.164406770259058, - "y": 1.1285160108597303, - "heading": -0.3473601506358553, - "angularVelocity": -0.014729144739916763, - "velocityX": 3.3329650958535093, - "velocityY": -1.0369540806994204, - "timestamp": 8.399672636143372 - }, - { - "x": 7.296003099222487, - "y": 1.086889230520204, - "heading": -0.3473718226301098, - "angularVelocity": -0.0002798556889061875, - "velocityX": 3.155243268351752, - "velocityY": -0.9980720547755352, - "timestamp": 8.441379825659752 - }, - { - "x": 7.420194691021883, - "y": 1.0469180216341771, - "heading": -0.34677182369168125, - "angularVelocity": 0.014385983457189529, - "velocityX": 2.977702243653219, - "velocityY": -0.9583769453064839, - "timestamp": 8.483087015176132 - }, - { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, - "angularVelocity": 0.02916022972351966, - "velocityX": 2.8002535694982806, - "velocityY": -0.9182731897618387, - "timestamp": 8.524794204692512 - }, - { - "x": 7.694109552956691, - "y": 0.9554552181085981, - "heading": -0.3423846619503678, - "angularVelocity": 0.05119624293404516, - "velocityX": 2.536815276765165, - "velocityY": -0.8583516550337287, - "timestamp": 8.586731767532397 - }, - { - "x": 7.835030504559327, - "y": 0.9064832871477253, - "heading": -0.33886448301682054, - "angularVelocity": 0.05683431462499228, - "velocityX": 2.2752098264978664, - "velocityY": -0.7906660952654867, - "timestamp": 8.648669330372282 - }, - { - "x": 7.959827163561028, - "y": 0.861997905644216, - "heading": -0.3356203872211221, - "angularVelocity": 0.05237687191671972, - "velocityX": 2.014878424007598, - "velocityY": -0.7182294469433418, - "timestamp": 8.710606893212168 - }, - { - "x": 8.068556860689503, - "y": 0.8221974775860657, - "heading": -0.3330761096963967, - "angularVelocity": 0.04107810201222393, - "velocityX": 1.7554726428218221, - "velocityY": -0.6425895084221868, - "timestamp": 8.772544456052053 - }, - { - "x": 8.161262929678218, - "y": 0.7872247496182815, - "heading": -0.33153778712366017, - "angularVelocity": 0.02483666618774324, - "velocityX": 1.4967664973897774, - "velocityY": -0.5646448837225355, - "timestamp": 8.834482018891938 - }, - { - "x": 8.237979200485006, - "y": 0.7571873196552872, - "heading": -0.33123696700010097, - "angularVelocity": 0.0048568285506627, - "velocityX": 1.2386065464846836, - "velocityY": -0.48496305934161354, - "timestamp": 8.896419581731823 - }, - { - "x": 8.298732784455282, - "y": 0.7321691813322279, - "heading": -0.3323549200927614, - "angularVelocity": -0.018049678440697953, - "velocityX": 0.9808843161512628, - "velocityY": -0.4039251332464228, - "timestamp": 8.958357144571709 - }, - { - "x": 8.34354587924, - "y": 0.712237715373984, - "heading": -0.3350374219820423, - "angularVelocity": -0.04330977465508917, - "velocityX": 0.7235204733606144, - "velocityY": -0.3217993257140046, - "timestamp": 9.020294707411594 - }, - { - "x": 8.372436985155346, - "y": 0.6974481694465224, - "heading": -0.33940426042561933, - "angularVelocity": -0.07050387912204041, - "velocityX": 0.46645532356563796, - "velocityY": -0.23878152851596665, - "timestamp": 9.08223227025148 - }, - { - "x": 8.385421752929688, - "y": 0.6878466606140137, - "heading": -0.34555563246426124, - "angularVelocity": -0.09931569400855825, - "velocityX": 0.20964285934061647, - "velocityY": -0.15501915787887396, - "timestamp": 9.144169833091365 - }, - { - "x": 8.381221613866826, - "y": 0.6835499753387894, - "heading": -0.35422874908994917, - "angularVelocity": -0.13146724375807764, - "velocityX": -0.06366577665514053, - "velocityY": -0.06512922572222897, - "timestamp": 9.210141525946263 - }, - { - "x": 8.358990442151228, - "y": 0.6851839721097847, - "heading": -0.36494544812778174, - "angularVelocity": -0.16244389940702994, - "velocityX": -0.33698046470466403, - "velocityY": 0.024768149797052455, - "timestamp": 9.27611321880116 - }, - { - "x": 8.318727796191062, - "y": 0.692749219284977, - "heading": -0.37761522689458643, - "angularVelocity": -0.1920487139032685, - "velocityX": -0.6103018464104298, - "velocityY": 0.1146741404958606, - "timestamp": 9.342084911656059 - }, - { - "x": 8.260433187957593, - "y": 0.7062463828943455, - "heading": -0.3921309909220783, - "angularVelocity": -0.22003018869651583, - "velocityX": -0.8836306256637128, - "velocityY": 0.20459022688799033, - "timestamp": 9.408056604510957 - }, - { - "x": 8.184106080259626, - "y": 0.7256762539187386, - "heading": -0.40836397752669645, - "angularVelocity": -0.24605987662499226, - "velocityX": -1.1569675476699668, - "velocityY": 0.2945183029807682, - "timestamp": 9.474028297365855 - }, - { - "x": 8.089745888152331, - "y": 0.7510397855554056, - "heading": -0.4261563635075344, - "angularVelocity": -0.26969727789116266, - "velocityX": -1.4303133362794993, - "velocityY": 0.3844608276530502, - "timestamp": 9.539999990220753 - }, - { - "x": 7.977351991056692, - "y": 0.7823381451863918, - "heading": -0.445310079113529, - "angularVelocity": -0.2903323346290704, - "velocityX": -1.7036685316358853, - "velocityY": 0.47442104752147907, - "timestamp": 9.605971683075651 - }, - { - "x": 7.846923771917719, - "y": 0.8195727882757919, - "heading": -0.4655690572275494, - "angularVelocity": -0.30708592181466465, - "velocityX": -1.9770330803219518, - "velocityY": 0.5644033293384785, - "timestamp": 9.67194337593055 - }, - { - "x": 7.6984607267480145, - "y": 0.8627455646922209, - "heading": -0.48658930450770715, - "angularVelocity": -0.31862525229405353, - "velocityX": -2.2504052684572193, - "velocityY": 0.6544136514942158, - "timestamp": 9.737915068785448 - }, - { - "x": 7.531962771855591, - "y": 0.9118588688249081, - "heading": -0.5078842268520795, - "angularVelocity": -0.32278878141280287, - "velocityX": -2.5237787251969923, - "velocityY": 0.7444602678410827, - "timestamp": 9.803886761640346 - }, - { - "x": 7.347431182646945, - "y": 0.9669158200795512, - "heading": -0.5287129812318903, - "angularVelocity": -0.31572259977658373, - "velocityX": -2.7971328493042473, - "velocityY": 0.8345541681905657, - "timestamp": 9.869858454495244 - }, - { - "x": 7.144872022891467, - "y": 1.0279202386913098, - "heading": -0.5478111966657127, - "angularVelocity": -0.28949106211095804, - "velocityX": -3.0703950586958326, - "velocityY": 0.9247059757271185, - "timestamp": 9.935830147350142 - }, - { - "x": 6.92431385893063, - "y": 1.0948740954299652, - "heading": -0.56252983495188, - "angularVelocity": -0.2231053600297971, - "velocityX": -3.343224259015806, - "velocityY": 1.0148876562242728, - "timestamp": 10.00180184020504 - }, - { - "x": 6.686027050772059, - "y": 1.1677363127465568, - "heading": -0.5635938882487441, - "angularVelocity": -0.016128937288368943, - "velocityX": -3.611955337915506, - "velocityY": 1.1044466825619976, - "timestamp": 10.067773533059938 - }, - { - "x": 6.448996512762522, - "y": 1.2456322264917639, - "heading": -0.5635938947986299, - "angularVelocity": -9.928327608034937e-8, - "velocityX": -3.592912774436053, - "velocityY": 1.180747535409401, - "timestamp": 10.133745225914836 - }, - { - "x": 6.2119660667132655, - "y": 1.3235284200636117, - "heading": -0.5635939013485092, - "angularVelocity": -9.928317804375559e-8, - "velocityX": -3.5929113805006425, - "velocityY": 1.180751777026201, - "timestamp": 10.199716918769735 - }, - { - "x": 5.97493562067527, - "y": 1.4014246136697293, - "heading": -0.5635939078983885, - "angularVelocity": -9.928317794724392e-8, - "velocityX": -3.5929113803299297, - "velocityY": 1.180751777545662, - "timestamp": 10.265688611624633 - }, - { - "x": 5.737905297280568, - "y": 1.4793211804663164, - "heading": -0.5635939144482677, - "angularVelocity": -9.928317524609207e-8, - "velocityX": -3.5929095213008893, - "velocityY": 1.180757434372905, - "timestamp": 10.33166030447953 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.5635939210079286, - "angularVelocity": -9.943144757674115e-8, - "velocityX": -3.5606438133655525, - "velocityY": 1.2747558885869388, - "timestamp": 10.397631997334429 - }, - { - "x": 5.2859625551559315, - "y": 1.6593525606269757, - "heading": -0.5635939265318385, - "angularVelocity": -8.803796196510374e-8, - "velocityX": -3.459117077662582, - "velocityY": 1.5289526284347386, - "timestamp": 10.4603766333877 - }, - { - "x": 5.071426966449666, - "y": 1.7607653123039275, - "heading": -0.5635939320385204, - "angularVelocity": -8.776338927441567e-8, - "velocityX": -3.4191861201350386, - "velocityY": 1.6162776303436093, - "timestamp": 10.52312126944097 - }, - { - "x": 4.856891543272159, - "y": 1.8621784141511948, - "heading": -0.5635939375452019, - "angularVelocity": -8.776338338833754e-8, - "velocityX": -3.419183482000995, - "velocityY": 1.6162832112241017, - "timestamp": 10.585865905494241 - }, - { - "x": 4.642356120103932, - "y": 1.9635915160180906, - "heading": -0.5635939430518834, - "angularVelocity": -8.776338232016452e-8, - "velocityX": -3.419183481853115, - "velocityY": 1.6162832115369357, - "timestamp": 10.648610541547512 - }, - { - "x": 4.427820696935705, - "y": 2.0650046178849877, - "heading": -0.5635939485585649, - "angularVelocity": -8.776338320403535e-8, - "velocityX": -3.419183481853107, - "velocityY": 1.616283211536953, - "timestamp": 10.711355177600783 - }, - { - "x": 4.213285273767479, - "y": 2.1664177197518852, - "heading": -0.5635939540652464, - "angularVelocity": -8.776338205784925e-8, - "velocityX": -3.419183481853105, - "velocityY": 1.616283211536959, - "timestamp": 10.774099813654054 - }, - { - "x": 3.99874985060226, - "y": 2.2678308216251453, - "heading": -0.5635939595719279, - "angularVelocity": -8.776338204240658e-8, - "velocityX": -3.4191834818051703, - "velocityY": 1.616283211638361, - "timestamp": 10.836844449707325 - }, - { - "x": 3.7842144810928517, - "y": 2.369244037002313, - "heading": -0.5635939650808715, - "angularVelocity": -8.779943360299573e-8, - "velocityX": -3.4191826266593917, - "velocityY": 1.6162850206202093, - "timestamp": 10.899589085760596 - }, - { - "x": 3.582486282432141, - "y": 2.464592725052892, - "heading": -0.5976475269249973, - "angularVelocity": -0.5427326379774409, - "velocityX": -3.21506683837393, - "velocityY": 1.5196309046980254, - "timestamp": 10.962333721813867 - }, - { - "x": 3.3962755581787456, - "y": 2.5526066599990593, - "heading": -0.6290952664889575, - "angularVelocity": -0.5012020396016188, - "velocityX": -2.9677552690767404, - "velocityY": 1.4027324163844592, - "timestamp": 11.025078357867137 - }, - { - "x": 3.2255823358404876, - "y": 2.63328589857046, - "heading": -0.6579338206032609, - "angularVelocity": -0.4596178403173682, - "velocityX": -2.7204432613704155, - "velocityY": 1.2858348322062758, - "timestamp": 11.087822993920408 - }, - { - "x": 3.0704066332901094, - "y": 2.7066304971231805, - "heading": -0.684160167831089, - "angularVelocity": -0.4179854865292692, - "velocityX": -2.4731309688151453, - "velocityY": 1.168938146209836, - "timestamp": 11.15056762997368 - }, - { - "x": 2.930748464239733, - "y": 2.7726405069094633, - "heading": -0.7077714539525848, - "angularVelocity": -0.3763076432772636, - "velocityX": -2.22581845772133, - "velocityY": 1.05204227705202, - "timestamp": 11.21331226602695 - }, - { - "x": 2.8066078397359373, - "y": 2.83131597316776, - "heading": -0.7287650298436226, - "angularVelocity": -0.3345875792986393, - "velocityX": -1.978505770571369, - "velocityY": 0.9351471288873412, - "timestamp": 11.27605690208022 - }, - { - "x": 2.697984768963403, - "y": 2.882656934878273, - "heading": -0.7471385155745514, - "angularVelocity": -0.2928295849119221, - "velocityX": -1.7311929370394774, - "velocityY": 0.8182526019742015, - "timestamp": 11.338801538133492 - }, - { - "x": 2.604879259815539, - "y": 2.926663424723888, - "heading": -0.7628898637084475, - "angularVelocity": -0.25103895925897446, - "velocityX": -1.4838799777054614, - "velocityY": 0.7013585959483968, - "timestamp": 11.401546174186763 - }, - { - "x": 2.5272913193421016, - "y": 2.963335469132253, - "heading": -0.7760174152523261, - "angularVelocity": -0.20922189321065382, - "velocityX": -1.236566906015123, - "velocityY": 0.584465011116332, - "timestamp": 11.464290810240033 - }, - { - "x": 2.465220954107753, - "y": 2.9926730883504185, - "heading": -0.7865199463491176, - "angularVelocity": -0.1673853217966667, - "velocityX": -0.9892537296997038, - "velocityY": 0.46757174897401693, - "timestamp": 11.527035446293304 - }, - { - "x": 2.418668170473455, - "y": 3.014676296527397, - "heading": -0.7943967051912195, - "angularVelocity": -0.12553676836079783, - "velocityX": -0.7419404520057251, - "velocityY": 0.3506787123332304, - "timestamp": 11.589780082346575 - }, - { - "x": 2.38763297480514, - "y": 3.0293451017899056, - "heading": -0.7996474391139023, - "angularVelocity": -0.08368418805115034, - "velocityX": -0.49462707285394797, - "velocityY": 0.23378580521296682, - "timestamp": 11.652524718399846 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -0.0418358130797821, - "velocityX": -0.24731358996990807, - "velocityY": 0.11689293258068652, - "timestamp": 11.715269354453117 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": 1.2905382194087526e-36, - "velocityX": 1.2188676912146853e-38, - "velocityY": -1.9456328527917923e-39, - "timestamp": 11.778013990506388 - }, - { - "x": 2.3862784269967885, - "y": 3.0299921206795677, - "heading": -0.7979445022798209, - "angularVelocity": 0.07214760590413372, - "velocityX": 0.23610252175550234, - "velocityY": -0.11148080618080955, - "timestamp": 11.838000869426745 - }, - { - "x": 2.414605712310544, - "y": 3.016617241283919, - "heading": -0.7893555783917804, - "angularVelocity": 0.14318004274641105, - "velocityX": 0.4722246901920793, - "velocityY": -0.22296341527296173, - "timestamp": 11.897987748347102 - }, - { - "x": 2.457098595113841, - "y": 2.996554734272641, - "heading": -0.7765849369819305, - "angularVelocity": 0.21289057940162145, - "velocityX": 0.7083696229589316, - "velocityY": -0.3344482555579276, - "timestamp": 11.95797462726746 - }, - { - "x": 2.5137586742709717, - "y": 2.9698044458506936, - "heading": -0.7597273153290622, - "angularVelocity": 0.28102181604163, - "velocityX": 0.9445412092927212, - "velocityY": -0.44593565965421605, - "timestamp": 12.017961506187817 - }, - { - "x": 2.5845878442638655, - "y": 2.9363662114763, - "heading": -0.7388971783301495, - "angularVelocity": 0.34724488711219914, - "velocityX": 1.180744377231747, - "velocityY": -0.5574258067133014, - "timestamp": 12.077948385108174 - }, - { - "x": 2.6695883810159726, - "y": 2.8962398712988433, - "heading": -0.7142349782126007, - "angularVelocity": 0.4111265757015274, - "velocityX": 1.4169854855252446, - "velocityY": -0.6689186185320777, - "timestamp": 12.137935264028531 - }, - { - "x": 2.7687630628600255, - "y": 2.849425297465716, - "heading": -0.6859166617272401, - "angularVelocity": 0.4720751770225938, - "velocityX": 1.653272909492815, - "velocityY": -0.7804135616937368, - "timestamp": 12.197922142948888 - }, - { - "x": 2.882115344958, - "y": 2.795922445610523, - "heading": -0.6541688123580988, - "angularVelocity": 0.529246560923628, - "velocityX": 1.8896179320892517, - "velocityY": -0.8919092444570627, - "timestamp": 12.257909021869246 - }, - { - "x": 3.009649615335316, - "y": 2.73573145985777, - "heading": -0.6192942663212875, - "angularVelocity": 0.5813695705541366, - "velocityX": 2.1260361044394105, - "velocityY": -1.0034025246198652, - "timestamp": 12.317895900789603 - }, - { - "x": 3.1513715692279023, - "y": 2.6688529097388005, - "heading": -0.5817190571900686, - "angularVelocity": 0.626390467507171, - "velocityX": 2.3625492181506127, - "velocityY": -1.114886310517361, - "timestamp": 12.37788277970996 - }, - { - "x": 3.3072887013147403, - "y": 2.5952884046594, - "heading": -0.5420886244414693, - "angularVelocity": 0.6606516868666416, - "velocityX": 2.5991872705003334, - "velocityY": -1.2263432671179573, - "timestamp": 12.437869658630317 - }, - { - "x": 3.4774104648990076, - "y": 2.5150425607670805, - "heading": -0.50150111026719, - "angularVelocity": 0.6766065330414328, - "velocityX": 2.8359829123654157, - "velocityY": -1.33772327109832, - "timestamp": 12.497856537550675 - }, - { - "x": 3.6617427045443476, - "y": 2.428132084038069, - "heading": -0.4622625664613271, - "angularVelocity": 0.6541187758402723, - "velocityX": 3.0728759849311564, - "velocityY": -1.4488247812392312, - "timestamp": 12.557843416471032 - }, - { - "x": 3.8601464039752953, - "y": 2.3346891355318427, - "heading": -0.4327535611710116, - "angularVelocity": 0.49192433114404066, - "velocityX": 3.3074516127828706, - "velocityY": -1.5577231252568986, - "timestamp": 12.61783029539139 - }, - { - "x": 4.065242173401204, - "y": 2.237711412138144, - "heading": -0.4327535477418676, - "angularVelocity": 2.2386802351026997e-7, - "velocityX": 3.4190105089182294, - "velocityY": -1.6166489262168848, - "timestamp": 12.677817174311746 - }, - { - "x": 4.2703379249938225, - "y": 2.140733651028875, - "heading": -0.43275353431315516, - "angularVelocity": 2.2386082807599726e-7, - "velocityX": 3.419010211631728, - "velocityY": -1.6166495549472233, - "timestamp": 12.737804053232104 - }, - { - "x": 4.475433676585232, - "y": 2.0437558899170503, - "heading": -0.43275352088444274, - "angularVelocity": 2.2386082918741598e-7, - "velocityX": 3.4190102116115857, - "velocityY": -1.616649554989821, - "timestamp": 12.797790932152461 - }, - { - "x": 4.6805294281766425, - "y": 1.9467781288052255, - "heading": -0.4327535074557304, - "angularVelocity": 2.2386082819075723e-7, - "velocityX": 3.4190102116115852, - "velocityY": -1.616649554989824, - "timestamp": 12.857777811072818 - }, - { - "x": 4.885625179768054, - "y": 1.8498003676934025, - "heading": -0.43275349402701796, - "angularVelocity": 2.2386082903608912e-7, - "velocityX": 3.4190102116115972, - "velocityY": -1.6166495549897977, - "timestamp": 12.917764689993176 - }, - { - "x": 5.090720931370459, - "y": 1.7528226066048298, - "heading": -0.43275348059830554, - "angularVelocity": 2.2386082830289002e-7, - "velocityX": 3.4190102117948658, - "velocityY": -1.6166495546022077, - "timestamp": 12.977751568913533 - }, - { - "x": 5.29581684523517, - "y": 1.6558451886813434, - "heading": -0.43275346716959256, - "angularVelocity": 2.2386083719980442e-7, - "velocityX": 3.4190129167581613, - "velocityY": -1.6166438339330798, - "timestamp": 13.03773844783389 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.43275345370859003, - "angularVelocity": 2.243991161112281e-7, - "velocityX": 3.453867841656271, - "velocityY": -1.5407736813730113, - "timestamp": 13.097725326754247 - }, - { - "x": 5.767637732978587, - "y": 1.4715486447521433, - "heading": -0.4327534418952186, - "angularVelocity": 1.5949042666645356e-7, - "velocityX": 3.572782883962456, - "velocityY": -1.2403266728714897, - "timestamp": 13.171794797056224 - }, - { - "x": 6.033774192023134, - "y": 1.384125705645109, - "heading": -0.432753430099036, - "angularVelocity": 1.5925836332865082e-7, - "velocityX": 3.593065509440347, - "velocityY": -1.1802830336252828, - "timestamp": 13.2458642673582 - }, - { - "x": 6.2999107025526015, - "y": 1.2967029232707166, - "heading": -0.43275341830285335, - "angularVelocity": 1.5925836332521055e-7, - "velocityX": 3.5930662045299844, - "velocityY": -1.1802809176031086, - "timestamp": 13.319933737660177 - }, - { - "x": 6.566047160708243, - "y": 1.2092799814598572, - "heading": -0.43275340650489624, - "angularVelocity": 1.5928232033441407e-7, - "velocityX": 3.593065497439389, - "velocityY": -1.1802830701291938, - "timestamp": 13.394003207962154 - }, - { - "x": 6.815604168443372, - "y": 1.1270295237235333, - "heading": -0.36800659105834704, - "angularVelocity": 0.8741363369088632, - "velocityX": 3.3692290051177802, - "velocityY": -1.110450194945293, - "timestamp": 13.46807267826413 - }, - { - "x": 7.0424486777806585, - "y": 1.0523256982782683, - "heading": -0.304588986464792, - "angularVelocity": 0.8561908750664192, - "velocityX": 3.0625912189253843, - "velocityY": -1.0085643267152118, - "timestamp": 13.542142148566107 - }, - { - "x": 7.246590574581559, - "y": 0.9851213052520007, - "heading": -0.24567984244132185, - "angularVelocity": 0.7953228743678282, - "velocityX": 2.7560869001577597, - "velocityY": -0.9073156963628868, - "timestamp": 13.616211618868084 - }, - { - "x": 7.428038548794457, - "y": 0.9253999088853845, - "heading": -0.19230647763371986, - "angularVelocity": 0.7205852099387576, - "velocityX": 2.4496999029849618, - "velocityY": -0.8062889625528061, - "timestamp": 13.69028108917006 - }, - { - "x": 7.586797949690041, - "y": 0.8731533407207944, - "heading": -0.1449799929692073, - "angularVelocity": 0.6389472541327181, - "velocityX": 2.1433851254549694, - "velocityY": -0.705372509774734, - "timestamp": 13.764350559472037 - }, - { - "x": 7.722872327084393, - "y": 0.8283767866857153, - "heading": -0.10400779573923778, - "angularVelocity": 0.5531590419497899, - "velocityX": 1.8371182734206921, - "velocityY": -0.6045210510150558, - "timestamp": 13.838420029774014 - }, - { - "x": 7.836264194581954, - "y": 0.7910670998170554, - "heading": -0.06959570799723012, - "angularVelocity": 0.46459205934255987, - "velocityX": 1.530885357155523, - "velocityY": -0.5037120788977025, - "timestamp": 13.91248950007599 - }, - { - "x": 7.926975414267802, - "y": 0.7612220655404804, - "heading": -0.04189104317473826, - "angularVelocity": 0.3740362218001811, - "velocityX": 1.224677580601353, - "velocityY": -0.4029330053920843, - "timestamp": 13.986558970377967 - }, - { - "x": 7.995007406512269, - "y": 0.7388400306235591, - "heading": -0.021003806792755122, - "angularVelocity": 0.28199521741990763, - "velocityX": 0.9184889802384877, - "velocityY": -0.3021762519115013, - "timestamp": 14.060628440679944 - }, - { - "x": 8.040361272813582, - "y": 0.7239196956147146, - "heading": -0.0070182972067734635, - "angularVelocity": 0.18881611450660718, - "velocityX": 0.612315251025934, - "velocityY": -0.20143704211755914, - "timestamp": 14.13469791098192 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": -5.172058523180806e-42, - "angularVelocity": 0.09475290127174331, - "velocityX": 0.30615312096089364, - "velocityY": -0.10071229126619016, - "timestamp": 14.208767381283897 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": -2.2197990868657932e-42, - "angularVelocity": 3.9858008032330975e-41, - "velocityX": -4.443522904196121e-42, - "velocityY": 7.940802598890531e-42, - "timestamp": 14.282836851585873 - } - ], "constraints": [ { "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ "last" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 6 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 1 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ - 11 + 9 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 7, + 8 + ], + "type": "ZeroAngularVelocity", + "direction": 0 + }, + { + "scope": [ + 7, + 8 + ], + "type": "StraightLine", + "direction": 0 } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/deploy/choreo/AmpLanePABCSprint.1.traj b/src/main/deploy/choreo/AmpLanePABCSprint.1.traj deleted file mode 100644 index 432331c6..00000000 --- a/src/main/deploy/choreo/AmpLanePABCSprint.1.traj +++ /dev/null @@ -1,139 +0,0 @@ -{ - "samples": [ - { - "x": 1.468971848487854, - "y": 6.933434963226318, - "heading": 0, - "angularVelocity": 2.1788179528279398e-36, - "velocityX": -1.368271424490806e-35, - "velocityY": -2.8235582012814922e-36, - "timestamp": 0 - }, - { - "x": 1.4919852747959301, - "y": 6.931977994889289, - "heading": 0.010182405612263554, - "angularVelocity": 0.13209480038984944, - "velocityX": 0.29854968169707696, - "velocityY": -0.018901028792491784, - "timestamp": 0.07708407584713683 - }, - { - "x": 1.538012185703421, - "y": 6.9290637945517, - "heading": 0.030545200045879524, - "angularVelocity": 0.26416343725773955, - "velocityX": 0.5971001195988331, - "velocityY": -0.037805478051881794, - "timestamp": 0.15416815169427367 - }, - { - "x": 1.6070526656722113, - "y": 6.924691955570066, - "heading": 0.06108581336299875, - "angularVelocity": 0.39619873471251593, - "velocityX": 0.895651653211777, - "velocityY": -0.056715202635427786, - "timestamp": 0.2312522275414105 - }, - { - "x": 1.699106838032385, - "y": 6.918861907235003, - "heading": 0.10180106951516922, - "angularVelocity": 0.5281928297734501, - "velocityX": 1.1942047867671626, - "velocityY": -0.07563233094502542, - "timestamp": 0.30833630338854734 - }, - { - "x": 1.8141748726519586, - "y": 6.911572878632461, - "heading": 0.15268749584080604, - "angularVelocity": 0.6601418745234515, - "velocityX": 1.492760123994504, - "velocityY": -0.09455946020546946, - "timestamp": 0.3854203792356842 - }, - { - "x": 1.952256977521906, - "y": 6.902823843817773, - "heading": 0.2137424539083774, - "angularVelocity": 0.7920566913022042, - "velocityX": 1.7913181594571344, - "velocityY": -0.11349989889010634, - "timestamp": 0.462504455082821 - }, - { - "x": 2.1133533536109033, - "y": 6.892613440825621, - "heading": 0.2849668134211211, - "angularVelocity": 0.9239827906088756, - "velocityX": 2.089878802055858, - "velocityY": -0.132458006143837, - "timestamp": 0.5395885309299578 - }, - { - "x": 2.251439762891142, - "y": 6.8838222842371355, - "heading": 0.3461905263541886, - "angularVelocity": 0.7942459225233288, - "velocityX": 1.7913739999176204, - "velocityY": -0.11404633825952669, - "timestamp": 0.6166726067770947 - }, - { - "x": 2.3665117189435922, - "y": 6.876494714440304, - "heading": 0.39723045192442524, - "angularVelocity": 0.6621331968933817, - "velocityX": 1.492810996147195, - "velocityY": -0.09505944926110971, - "timestamp": 0.6937566826242315 - }, - { - "x": 2.4585692711434595, - "y": 6.8706316961774005, - "heading": 0.43807452893142784, - "angularVelocity": 0.5298640031437787, - "velocityX": 1.1942486329138104, - "velocityY": -0.07606004480784023, - "timestamp": 0.7708407584713683 - }, - { - "x": 2.527612473955509, - "y": 6.866233949603197, - "heading": 0.46871228827353184, - "angularVelocity": 0.3974589953294743, - "velocityX": 0.8956869762435377, - "velocityY": -0.057051297896133134, - "timestamp": 0.8479248343185052 - }, - { - "x": 2.5736413469679076, - "y": 6.863301955753749, - "heading": 0.48913694367966715, - "angularVelocity": 0.26496595025201874, - "velocityX": 0.5971255736875779, - "velocityY": -0.038036310576809694, - "timestamp": 0.925008910165642 - }, - { - "x": 2.59665584564209, - "y": 6.861835956573486, - "heading": 0.499346976196016, - "angularVelocity": 0.1324532000175497, - "velocityX": 0.298563593339587, - "velocityY": -0.019018184549158077, - "timestamp": 1.0020929860127787 - }, - { - "x": 2.59665584564209, - "y": 6.861835956573486, - "heading": 0.499346976196016, - "angularVelocity": 4.187102555037149e-35, - "velocityX": 0, - "velocityY": 1.1382387123849342e-35, - "timestamp": 1.0791770618599155 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePABCSprint.2.traj b/src/main/deploy/choreo/AmpLanePABCSprint.2.traj deleted file mode 100644 index bf26ed8b..00000000 --- a/src/main/deploy/choreo/AmpLanePABCSprint.2.traj +++ /dev/null @@ -1,148 +0,0 @@ -{ - "samples": [ - { - "x": 2.59665584564209, - "y": 6.861835956573486, - "heading": 0.499346976196016, - "angularVelocity": 4.187102555037149e-35, - "velocityX": 0, - "velocityY": 1.1382387123849342e-35, - "timestamp": 0 - }, - { - "x": 2.5947418201277674, - "y": 6.838819593279985, - "heading": 0.49061685118958936, - "angularVelocity": -0.11327050952346711, - "velocityX": -0.024833853477310732, - "velocityY": -0.2986297671239509, - "timestamp": 0.07707323859629955 - }, - { - "x": 2.590913679072716, - "y": 6.792786870102318, - "heading": 0.47314863568765103, - "angularVelocity": -0.2266443686560877, - "velocityX": -0.0496688750177347, - "velocityY": -0.5972594900128827, - "timestamp": 0.1541464771925991 - }, - { - "x": 2.5851712843326613, - "y": 6.7237379431029165, - "heading": 0.4469236634346606, - "angularVelocity": -0.34026041633404946, - "velocityX": -0.07450568893481502, - "velocityY": -0.8958871880429407, - "timestamp": 0.23121971578889866 - }, - { - "x": 2.5775144084565245, - "y": 6.631673124734894, - "heading": 0.41191275130054616, - "angularVelocity": -0.45425510555612447, - "velocityX": -0.09934545395507093, - "velocityY": -1.1945108320963094, - "timestamp": 0.3082929543851982 - }, - { - "x": 2.567942678897023, - "y": 6.516592884025286, - "heading": 0.36807680590612885, - "angularVelocity": -0.568757018555102, - "velocityX": -0.12419005265416512, - "velocityY": -1.4931283906776436, - "timestamp": 0.38536619298149777 - }, - { - "x": 2.55645550871041, - "y": 6.378497838506786, - "heading": 0.31536820111597746, - "angularVelocity": -0.6838768650456329, - "velocityX": -0.14904226675592608, - "velocityY": -1.791737936974802, - "timestamp": 0.4624394315777973 - }, - { - "x": 2.543052018141666, - "y": 6.217388734848081, - "heading": 0.2537333559903418, - "angularVelocity": -0.7996919066612944, - "velocityX": -0.17390589539062137, - "velocityY": -2.0903377954905458, - "timestamp": 0.5395126701740969 - }, - { - "x": 2.529607207091661, - "y": 6.056304138753961, - "heading": 0.19035809858653865, - "angularVelocity": -0.8222731853238346, - "velocityX": -0.174442015086806, - "velocityY": -2.0900198178755933, - "timestamp": 0.6165859087703964 - }, - { - "x": 2.518080937879921, - "y": 5.918232305257069, - "heading": 0.13599588472897933, - "angularVelocity": -0.7053319005096194, - "velocityX": -0.14954956378715376, - "velocityY": -1.7914367686051997, - "timestamp": 0.693659147366696 - }, - { - "x": 2.5084742268750184, - "y": 5.803172814891468, - "heading": 0.09067284716302221, - "angularVelocity": -0.5880515518927888, - "velocityX": -0.12464392543852573, - "velocityY": -1.4928591617677907, - "timestamp": 0.7707323859629955 - }, - { - "x": 2.5007879415658545, - "y": 5.711125391009445, - "heading": 0.054405158598584594, - "angularVelocity": -0.4705613676674917, - "velocityX": -0.09972703170582883, - "velocityY": -1.194285144343763, - "timestamp": 0.8478056245592951 - }, - { - "x": 2.495022759802516, - "y": 5.642089871721298, - "heading": 0.027201747413913846, - "angularVelocity": -0.35295534066187323, - "velocityX": -0.07480134309050894, - "velocityY": -0.8957132273855447, - "timestamp": 0.9248788631555946 - }, - { - "x": 2.491179145461549, - "y": 5.596066182088173, - "heading": 0.009066533897654676, - "angularVelocity": -0.2352984491964748, - "velocityX": -0.049869635829104156, - "velocityY": -0.5971422827343685, - "timestamp": 1.0019521017518942 - }, - { - "x": 2.489257335662842, - "y": 5.573054313659668, - "heading": -1.290637644918987e-42, - "angularVelocity": -0.11763530458534513, - "velocityX": -0.024934852014888275, - "velocityY": -0.2985714477243978, - "timestamp": 1.0790253403481938 - }, - { - "x": 2.489257335662842, - "y": 5.573054313659668, - "heading": 3.5400616344493244e-42, - "angularVelocity": 6.267668380261972e-41, - "velocityX": 0, - "velocityY": -1.030203639730239e-41, - "timestamp": 1.1560985789444933 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePABCSprint.3.traj b/src/main/deploy/choreo/AmpLanePABCSprint.3.traj deleted file mode 100644 index 83208279..00000000 --- a/src/main/deploy/choreo/AmpLanePABCSprint.3.traj +++ /dev/null @@ -1,148 +0,0 @@ -{ - "samples": [ - { - "x": 2.489257335662842, - "y": 5.573054313659668, - "heading": 3.5400616344493244e-42, - "angularVelocity": 6.267668380261972e-41, - "velocityX": 0, - "velocityY": -1.030203639730239e-41, - "timestamp": 0 - }, - { - "x": 2.490545501701153, - "y": 5.550368032283891, - "heading": -0.012391572665595225, - "angularVelocity": -0.16176043708854124, - "velocityX": 0.016815807567216828, - "velocityY": -0.29614826868975896, - "timestamp": 0.0766044707137663 - }, - { - "x": 2.4931203515767737, - "y": 5.504995392935276, - "heading": -0.03717086071864737, - "angularVelocity": -0.32347052100444956, - "velocityX": 0.033612266381182004, - "velocityY": -0.592297537282776, - "timestamp": 0.1532089414275326 - }, - { - "x": 2.4969797328869174, - "y": 5.436936063650543, - "heading": -0.07432246767470774, - "angularVelocity": -0.48497961815933416, - "velocityX": 0.05038062758196422, - "velocityY": -0.8884511393471786, - "timestamp": 0.2298134121412989 - }, - { - "x": 2.5021209417107193, - "y": 5.346189326696005, - "heading": -0.12381236756993079, - "angularVelocity": -0.6460445380550001, - "velocityX": 0.06711369161484333, - "velocityY": -1.1846141107561945, - "timestamp": 0.3064178828550652 - }, - { - "x": 2.5085409420107596, - "y": 5.23275393263894, - "heading": -0.18557968154848511, - "angularVelocity": -0.8063147412028813, - "velocityX": 0.08380712300759265, - "velocityY": -1.4807933923454437, - "timestamp": 0.3830223535688315 - }, - { - "x": 2.516236715901849, - "y": 5.096627982067514, - "heading": -0.2595292463430887, - "angularVelocity": -0.9653426765510463, - "velocityX": 0.10046115872068286, - "velocityY": -1.7769974689866566, - "timestamp": 0.4596268242825978 - }, - { - "x": 2.525205747324836, - "y": 4.937808900810427, - "heading": -0.3455278832614149, - "angularVelocity": -1.1226320881409273, - "velocityX": 0.11708234962551842, - "velocityY": -2.073235149036095, - "timestamp": 0.5362312949963641 - }, - { - "x": 2.534115785608997, - "y": 4.778903807663307, - "heading": -0.42682973592121154, - "angularVelocity": -1.0613199451972188, - "velocityX": 0.11631224915649217, - "velocityY": -2.074357954131309, - "timestamp": 0.6128357657101304 - }, - { - "x": 2.541754175790119, - "y": 4.6426966190718195, - "heading": -0.4963415581149959, - "angularVelocity": -0.9074120811240399, - "velocityX": 0.09971206784605312, - "velocityY": -1.7780579556567497, - "timestamp": 0.6894402364238967 - }, - { - "x": 2.5481206072451936, - "y": 4.529189096235521, - "heading": -0.5541628291197376, - "angularVelocity": -0.7548028263362259, - "velocityX": 0.08310783164161595, - "velocityY": -1.481734966362742, - "timestamp": 0.766044707137663 - }, - { - "x": 2.553214526426421, - "y": 4.438382432251887, - "heading": -0.6003661240439829, - "angularVelocity": -0.6031409719790967, - "velocityX": 0.06649636938633684, - "velocityY": -1.1853964022926788, - "timestamp": 0.8426491778514293 - }, - { - "x": 2.5570353684728135, - "y": 4.370277332669397, - "heading": -0.6349992929465087, - "angularVelocity": -0.4521037555619042, - "velocityX": 0.04987753339709207, - "velocityY": -0.8890486279445305, - "timestamp": 0.9192536485651956 - }, - { - "x": 2.55958270709048, - "y": 4.3248740935696794, - "heading": -0.6580881830664621, - "angularVelocity": -0.3014039507723422, - "velocityX": 0.03325313253824122, - "velocityY": -0.5926969885265119, - "timestamp": 0.995858119278962 - }, - { - "x": 2.560856342315674, - "y": 4.302172660827637, - "heading": -0.6696388746005083, - "angularVelocity": -0.1507835172858963, - "velocityX": 0.016626121338957477, - "velocityY": -0.2963460556612492, - "timestamp": 1.0724625899927283 - }, - { - "x": 2.560856342315674, - "y": 4.302172660827637, - "heading": -0.6696388746005083, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -7.298514595392752e-42, - "timestamp": 1.1490670607064946 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePABCSprint.4.traj b/src/main/deploy/choreo/AmpLanePABCSprint.4.traj deleted file mode 100644 index 56f336f2..00000000 --- a/src/main/deploy/choreo/AmpLanePABCSprint.4.traj +++ /dev/null @@ -1,454 +0,0 @@ -{ - "samples": [ - { - "x": 2.560856342315674, - "y": 4.302172660827637, - "heading": -0.6696388746005083, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -7.298514595392752e-42, - "timestamp": 0 - }, - { - "x": 2.555942923489597, - "y": 4.287142223743098, - "heading": -0.6619594332481032, - "angularVelocity": 0.12026451741757689, - "velocityX": -0.07694699612537333, - "velocityY": -0.23538538542014806, - "timestamp": 0.06385458917812725 - }, - { - "x": 2.546450590215251, - "y": 4.256979387192444, - "heading": -0.646488848875746, - "angularVelocity": 0.2422783479070068, - "velocityX": -0.1486554591693719, - "velocityY": -0.47236756103015765, - "timestamp": 0.1277091783562545 - }, - { - "x": 2.5328417294288803, - "y": 4.211556395130359, - "heading": -0.6230747892348256, - "angularVelocity": 0.3666777899957196, - "velocityX": -0.21312267389909825, - "velocityY": -0.7113504706039822, - "timestamp": 0.19156376753438176 - }, - { - "x": 2.5157945553970475, - "y": 4.150712589378183, - "heading": -0.5915084780030003, - "angularVelocity": 0.49434679070236714, - "velocityX": -0.26696865881132753, - "velocityY": -0.9528493806834795, - "timestamp": 0.255418356712509 - }, - { - "x": 2.4963909891627085, - "y": 4.074255034081668, - "heading": -0.5515032223856839, - "angularVelocity": 0.6265055672931927, - "velocityX": -0.30387113101943486, - "velocityY": -1.19736977843881, - "timestamp": 0.31927294589063626 - }, - { - "x": 2.4765840952198315, - "y": 3.9820247553374495, - "heading": -0.502689860958291, - "angularVelocity": 0.7644456264721134, - "velocityX": -0.3101874774830074, - "velocityY": -1.4443797999691932, - "timestamp": 0.3831275350687635 - }, - { - "x": 2.460561114471436, - "y": 3.8744889188548193, - "heading": -0.4448360830574491, - "angularVelocity": 0.9060238057354711, - "velocityX": -0.2509291963917905, - "velocityY": -1.6840737348203896, - "timestamp": 0.44698212424689077 - }, - { - "x": 2.4570633682910814, - "y": 3.757300143536982, - "heading": -0.37961365420873555, - "angularVelocity": 1.021421164683569, - "velocityX": -0.0547767392347868, - "velocityY": -1.8352443704700634, - "timestamp": 0.510836713425018 - }, - { - "x": 2.4691364189829197, - "y": 3.643410857725524, - "heading": -0.313317924824764, - "angularVelocity": 1.038229675224039, - "velocityX": 0.18907099469640304, - "velocityY": -1.78357244604229, - "timestamp": 0.5746913026031453 - }, - { - "x": 2.4943273688840915, - "y": 3.5385198822209882, - "heading": -0.2494838956973126, - "angularVelocity": 0.999678017650094, - "velocityX": 0.39450492478934795, - "velocityY": -1.6426536738328135, - "timestamp": 0.638545891781273 - }, - { - "x": 2.530951098426381, - "y": 3.444665017333843, - "heading": -0.18957977093606787, - "angularVelocity": 0.9381334299111623, - "velocityX": 0.5735489024935178, - "velocityY": -1.469821763715844, - "timestamp": 0.7024004809594007 - }, - { - "x": 2.577997037977533, - "y": 3.3627901789759114, - "heading": -0.13433219775233787, - "angularVelocity": 0.8652091242747275, - "velocityX": 0.7367667720782568, - "velocityY": -1.282207581502646, - "timestamp": 0.7662550701375284 - }, - { - "x": 2.6348140958033937, - "y": 3.293422653958234, - "heading": -0.08415783264044516, - "angularVelocity": 0.7857597356382248, - "velocityX": 0.8897881664756364, - "velocityY": -1.0863357811945384, - "timestamp": 0.8301096593156561 - }, - { - "x": 2.7009526723986657, - "y": 3.2368947239219907, - "heading": -0.03932053859858279, - "angularVelocity": 0.7021780990053088, - "velocityX": 1.035768571163667, - "velocityY": -0.8852602571532359, - "timestamp": 0.8939642484937838 - }, - { - "x": 2.776085138320923, - "y": 3.1934335231781006, - "heading": 0, - "angularVelocity": 0.6157825005951433, - "velocityX": 1.1766181082563942, - "velocityY": -0.6806276777171288, - "timestamp": 0.9578188376719114 - }, - { - "x": 2.850874204910404, - "y": 3.1650170711048373, - "heading": 0.030840918072749232, - "angularVelocity": 0.536085986843282, - "velocityX": 1.3000057415001567, - "velocityY": -0.4939431996267531, - "timestamp": 1.0153486348120206 - }, - { - "x": 2.9329362221959228, - "y": 3.147223404249923, - "heading": 0.05711727907812475, - "angularVelocity": 0.4567435018305658, - "velocityX": 1.426426328006394, - "velocityY": -0.3092947957313142, - "timestamp": 1.0728784319521298 - }, - { - "x": 3.0224739967279888, - "y": 3.13991100956747, - "heading": 0.07884780123756048, - "angularVelocity": 0.37772638249554086, - "velocityX": 1.5563721581357917, - "velocityY": -0.12710621357910618, - "timestamp": 1.130408229092239 - }, - { - "x": 3.119725791831946, - "y": 3.1429060034855505, - "heading": 0.09605037284161863, - "angularVelocity": 0.2990202027335969, - "velocityX": 1.6904595520667056, - "velocityY": 0.0520598727436279, - "timestamp": 1.1879380262323482 - }, - { - "x": 3.2249748898290944, - "y": 3.1559904101117278, - "heading": 0.10874346264141094, - "angularVelocity": 0.22063505228219965, - "velocityX": 1.8294710433416355, - "velocityY": 0.22743703744185526, - "timestamp": 1.2454678233724574 - }, - { - "x": 3.3385625417753837, - "y": 3.1788843955905044, - "heading": 0.11694851256012442, - "angularVelocity": 0.14262261170034615, - "velocityX": 1.9744142617026068, - "velocityY": 0.3979500470516161, - "timestamp": 1.3029976205125666 - }, - { - "x": 3.4609055664038406, - "y": 3.2112183215831944, - "heading": 0.12069410858340832, - "angularVelocity": 0.06510706120102956, - "velocityX": 2.1266027469295685, - "velocityY": 0.5620378934058015, - "timestamp": 1.3605274176526758 - }, - { - "x": 3.592519911370183, - "y": 3.252486932254075, - "heading": 0.12002359804350324, - "angularVelocity": -0.011655013110373149, - "velocityX": 2.2877595873631624, - "velocityY": 0.7173432329402094, - "timestamp": 1.418057214792785 - }, - { - "x": 3.7340501302900706, - "y": 3.301970950084958, - "heading": 0.11500982718595847, - "angularVelocity": -0.08715085237193064, - "velocityX": 2.4601202499498154, - "velocityY": 0.8601458772811008, - "timestamp": 1.4755870119328942 - }, - { - "x": 3.8862975149517003, - "y": 3.358597851211711, - "heading": 0.10578551382477244, - "angularVelocity": -0.16033975122006716, - "velocityX": 2.646409204100688, - "velocityY": 0.9843055936533752, - "timestamp": 1.5331168090730034 - }, - { - "x": 4.050210856024716, - "y": 3.420693253911601, - "heading": 0.09260907243382549, - "angularVelocity": -0.22903681302502776, - "velocityX": 2.849190319128336, - "velocityY": 1.0793607102187723, - "timestamp": 1.5906466062131126 - }, - { - "x": 4.226710679977497, - "y": 3.485583407764258, - "heading": 0.07600213210042343, - "angularVelocity": -0.2886667632941102, - "velocityX": 3.0679722983018776, - "velocityY": 1.127939903814055, - "timestamp": 1.6481764033532218 - }, - { - "x": 4.41606032843016, - "y": 3.549260198670433, - "heading": 0.05695039663507196, - "angularVelocity": -0.3311629175217238, - "velocityX": 3.291331759636103, - "velocityY": 1.1068488691363714, - "timestamp": 1.705706200493331 - }, - { - "x": 4.616894975800281, - "y": 3.607020793660326, - "heading": 0.03693511887030335, - "angularVelocity": -0.3479114956032789, - "velocityX": 3.490967417823566, - "velocityY": 1.0040117966907118, - "timestamp": 1.7632359976334402 - }, - { - "x": 4.826412483110604, - "y": 3.6552133079506532, - "heading": 0.017545300992940664, - "angularVelocity": -0.337039566298841, - "velocityX": 3.6418954650589184, - "velocityY": 0.8376965796169683, - "timestamp": 1.8207657947735494 - }, - { - "x": 5.041757106781006, - "y": 3.6918814182281494, - "heading": 0, - "angularVelocity": -0.3049776266412084, - "velocityX": 3.7431841302333626, - "velocityY": 0.6373759703722572, - "timestamp": 1.8782955919136586 - }, - { - "x": 5.318468189453411, - "y": 3.7184056106005574, - "heading": -0.017910640554298394, - "angularVelocity": -0.247405457661866, - "velocityX": 3.822299478409622, - "velocityY": 0.3663872284808979, - "timestamp": 1.950689470058324 - }, - { - "x": 5.597577139842402, - "y": 3.724637925984956, - "heading": -0.03131839137135645, - "angularVelocity": -0.1852055886585485, - "velocityX": 3.8554219989602294, - "velocityY": 0.08608898354560332, - "timestamp": 2.0230833482029893 - }, - { - "x": 5.874223781041164, - "y": 3.7105889006362736, - "heading": -0.03999959733056603, - "angularVelocity": -0.11991629930174207, - "velocityX": 3.8214093275392043, - "velocityY": -0.194063720700357, - "timestamp": 2.0954772263476547 - }, - { - "x": 6.142135014447851, - "y": 3.6780687785591137, - "heading": -0.04416827770374048, - "angularVelocity": -0.05758332720957581, - "velocityX": 3.7007443208293824, - "velocityY": -0.4492109403529163, - "timestamp": 2.16787110449232 - }, - { - "x": 6.395319274879328, - "y": 3.631372287860468, - "heading": -0.04465854916399648, - "angularVelocity": -0.0067722778889713385, - "velocityX": 3.49731589079308, - "velocityY": -0.6450336947736285, - "timestamp": 2.2402649826369854 - }, - { - "x": 6.630037177214525, - "y": 3.5758872476628394, - "heading": -0.042558245366794624, - "angularVelocity": 0.029012174109595102, - "velocityX": 3.2422341273961908, - "velocityY": -0.7664327650295508, - "timestamp": 2.3126588607816507 - }, - { - "x": 6.844724227675687, - "y": 3.5162488607697617, - "heading": -0.03880262566507215, - "angularVelocity": 0.05187758686193831, - "velocityX": 2.965541506591899, - "velocityY": -0.8238042832005938, - "timestamp": 2.385052738926316 - }, - { - "x": 7.038965694034982, - "y": 3.4558518067523507, - "heading": -0.03408005700737659, - "angularVelocity": 0.06523436482099192, - "velocityX": 2.6831200556922665, - "velocityY": -0.8342839970075755, - "timestamp": 2.4574466170709814 - }, - { - "x": 7.212812310142381, - "y": 3.3970955320884197, - "heading": -0.02888390227680435, - "angularVelocity": 0.07177616206979107, - "velocityX": 2.4013994078338827, - "velocityY": -0.8116193823256383, - "timestamp": 2.529840495215647 - }, - { - "x": 7.366475721480626, - "y": 3.3416998550485086, - "heading": -0.023574199348340368, - "angularVelocity": 0.07334463996877644, - "velocityX": 2.122602287325693, - "velocityY": -0.7651983628949022, - "timestamp": 2.602234373360312 - }, - { - "x": 7.500208431967021, - "y": 3.2909325516518573, - "heading": -0.018421222254793455, - "angularVelocity": 0.07117973543632572, - "velocityX": 1.847293084909134, - "velocityY": -0.7012651442046296, - "timestamp": 2.6746282515049775 - }, - { - "x": 7.614258897319107, - "y": 3.245756058228632, - "heading": -0.013633605820409669, - "angularVelocity": 0.0661328907510193, - "velocityX": 1.5754158814945374, - "velocityY": -0.6240374819117769, - "timestamp": 2.747022129649643 - }, - { - "x": 7.70885616888735, - "y": 3.206920861206511, - "heading": -0.009376453647745345, - "angularVelocity": 0.05880541672539207, - "velocityX": 1.3067026382977804, - "velocityY": -0.5364431084147118, - "timestamp": 2.8194160077943082 - }, - { - "x": 7.784206086933648, - "y": 3.1750260064914886, - "heading": -0.005783242083132249, - "angularVelocity": 0.04963419085565119, - "velocityX": 1.0408327330623905, - "velocityY": -0.4405739205086726, - "timestamp": 2.8918098859389736 - }, - { - "x": 7.840491875309718, - "y": 3.150559356787349, - "heading": -0.0029638574374597423, - "angularVelocity": 0.03894506991376383, - "velocityX": 0.7774937580162987, - "velocityY": -0.3379657276440889, - "timestamp": 2.964203764083639 - }, - { - "x": 7.877876256619461, - "y": 3.1339251307436706, - "heading": -0.0010101702732461582, - "angularVelocity": 0.026986911245582144, - "velocityX": 0.5164025228077691, - "velocityY": -0.22977393213329078, - "timestamp": 3.0365976422283043 - }, - { - "x": 7.896503925323486, - "y": 3.1254632472991943, - "heading": 0, - "angularVelocity": 0.013953807961876554, - "velocityX": 0.25730999887588185, - "velocityY": -0.11688672663131537, - "timestamp": 3.1089915203729697 - }, - { - "x": 7.896503925323486, - "y": 3.1254632472991943, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.3140323971906594e-42, - "velocityY": 0, - "timestamp": 3.181385398517635 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.1.traj b/src/main/deploy/choreo/AmpLanePADESprint.1.traj index 814d4db7..13d3aea1 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.1.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.1.traj @@ -1,157 +1,158 @@ { - "samples": [ - { - "x": 0.43297290802001953, - "y": 6.9807281494140625, - "heading": 0, - "angularVelocity": 2.735053895521923e-38, - "velocityX": -3.8673469053295564e-39, - "velocityY": 1.858643532249755e-38, - "timestamp": 0 - }, - { - "x": 0.46193105617219093, - "y": 6.976128411049919, - "heading": 0.007924686694784022, - "angularVelocity": 0.0965568963773332, - "velocityX": 0.35283526252830383, - "velocityY": -0.05604467125266167, - "timestamp": 0.08207271559159549 - }, - { - "x": 0.5198473628641517, - "y": 6.966928755942856, - "heading": 0.023773564700110772, - "angularVelocity": 0.19310775683593598, - "velocityX": 0.7056706516226394, - "velocityY": -0.11209151592888836, - "timestamp": 0.16414543118319097 - }, - { - "x": 0.6067218129336875, - "y": 6.9531289220308485, - "heading": 0.04754806846022335, - "angularVelocity": 0.2896760950181983, - "velocityX": 1.0585058559757474, - "velocityY": -0.16814155365198188, - "timestamp": 0.24621814677478646 - }, - { - "x": 0.7225543617111448, - "y": 6.934728565336742, - "heading": 0.07925217561750111, - "angularVelocity": 0.38629289805690764, - "velocityX": 1.4113405160596262, - "velocityY": -0.22419578250180797, - "timestamp": 0.32829086236638194 - }, - { - "x": 0.8673449286095066, - "y": 6.9117272629835975, - "heading": 0.11889328943914418, - "angularVelocity": 0.48299990485147826, - "velocityX": 1.764174194245729, - "velocityY": -0.2802551638184133, - "timestamp": 0.4103635779579774 - }, - { - "x": 1.0410933872613453, - "y": 6.884124518124938, - "heading": 0.16648343863275697, - "angularVelocity": 0.5798534732349732, - "velocityX": 2.117006332731008, - "velocityY": -0.33632059887008814, - "timestamp": 0.49243629354957286 - }, - { - "x": 1.243799549589456, - "y": 6.851919767180746, - "heading": 0.22204089753091144, - "angularVelocity": 0.6769297018830912, - "velocityX": 2.4698361796239694, - "velocityY": -0.39239290075946437, - "timestamp": 0.5745090091411683 - }, - { - "x": 1.4754631289031934, - "y": 6.815112383244328, - "heading": 0.28559273385120976, - "angularVelocity": 0.7743357321882771, - "velocityX": 2.8226625333871667, - "velocityY": -0.44847284107882956, - "timestamp": 0.6565817247327638 - }, - { - "x": 1.6781593090979035, - "y": 6.782884452250807, - "heading": 0.3422497740441209, - "angularVelocity": 0.6903273491624664, - "velocityX": 2.4697145541443093, - "velocityY": -0.3926753338331338, - "timestamp": 0.7386544403243592 - }, - { - "x": 1.8518984587746443, - "y": 6.755259929387126, - "heading": 0.39086385083963315, - "angularVelocity": 0.5923293319234401, - "velocityX": 2.1168929092256374, - "velocityY": -0.33658594899118865, - "timestamp": 0.8207271559159547 - }, - { - "x": 1.9966808291576696, - "y": 6.732239139219163, - "heading": 0.43140805286474204, - "angularVelocity": 0.4940034179795163, - "velocityX": 1.7640743253029585, - "velocityY": -0.28049260953564537, - "timestamp": 0.9027998715075501 - }, - { - "x": 2.1125066103123933, - "y": 6.7138223061958735, - "heading": 0.463861007608982, - "angularVelocity": 0.39541709458878044, - "velocityX": 1.4112580571975715, - "velocityY": -0.22439653532283038, - "timestamp": 0.9848725870991456 - }, - { - "x": 2.199375929341993, - "y": 6.700009583150749, - "heading": 0.4882072246931022, - "angularVelocity": 0.29664203150374724, - "velocityX": 1.0584433377576143, - "velocityY": -0.16829859893826818, - "timestamp": 1.0669453026907412 - }, - { - "x": 2.257288845375976, - "y": 6.690801066111525, - "heading": 0.5044377142261048, - "angularVelocity": 0.19775743273523133, - "velocityX": 0.7056293387703308, - "velocityY": -0.11219949252132488, - "timestamp": 1.1490180182823366 - }, - { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": 0.0988477756026137, - "velocityX": 0.3528151893675758, - "velocityY": -0.0560997894575031, - "timestamp": 1.231090733873932 - }, - { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": 0, - "velocityX": 3.087311543739499e-36, - "velocityY": 0, - "timestamp": 1.3131634494655275 - } - ] + "samples": [ + { + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": 6.831435290856854e-28, + "angularVelocity": 9.118091433542868e-28, + "velocityX": -1.02680113599063e-27, + "velocityY": -2.9257138265977544e-27, + "timestamp": 0 + }, + { + "x": 0.46193105617201946, + "y": 6.976128411049744, + "heading": 0.007924686703269965, + "angularVelocity": 0.09655689623933598, + "velocityX": 0.35283526164412465, + "velocityY": -0.056044671114677404, + "timestamp": 0.08207271579677758 + }, + { + "x": 0.5198473628636088, + "y": 6.966928755942304, + "heading": 0.023773564726982017, + "angularVelocity": 0.19310775657717816, + "velocityX": 0.7056706498539357, + "velocityY": -0.11209151565325522, + "timestamp": 0.16414543159355516 + }, + { + "x": 0.6067218129325357, + "y": 6.953128922029678, + "heading": 0.04754806851726075, + "angularVelocity": 0.2896760946615609, + "velocityX": 1.0585058533220593, + "velocityY": -0.16814155323914987, + "timestamp": 0.24621814739033274 + }, + { + "x": 0.7225543617090937, + "y": 6.9347285653346615, + "heading": 0.0792521757191159, + "angularVelocity": 0.386292897634319, + "velocityX": 1.4113405125203116, + "velocityY": -0.22419578195241194, + "timestamp": 0.3282908631871103 + }, + { + "x": 0.8673449286061865, + "y": 6.911727262980233, + "heading": 0.11889328960368833, + "angularVelocity": 0.4829999044107281, + "velocityX": 1.7641741898198249, + "velocityY": -0.2802551631334134, + "timestamp": 0.41036357898388787 + }, + { + "x": 1.0410933872562544, + "y": 6.884124518119783, + "heading": 0.166483438885144, + "angularVelocity": 0.5798534728556428, + "velocityX": 2.1170063274169095, + "velocityY": -0.3363205980510971, + "timestamp": 0.4924362947806654 + }, + { + "x": 1.2437995495818281, + "y": 6.851919767173026, + "heading": 0.2220408979091711, + "angularVelocity": 0.6769297017244366, + "velocityX": 2.4698361734184595, + "velocityY": -0.3923928998097307, + "timestamp": 0.574509010577443 + }, + { + "x": 1.4754631288914648, + "y": 6.815112383232462, + "heading": 0.28559273443277505, + "angularVelocity": 0.7743357327295749, + "velocityX": 2.822662526280537, + "velocityY": -0.44847284000816645, + "timestamp": 0.6565817263742205 + }, + { + "x": 1.6781593090904183, + "y": 6.782884452243251, + "heading": 0.3422497744154681, + "angularVelocity": 0.6903273448752822, + "velocityX": 2.4697145480217175, + "velocityY": -0.3926753327989152, + "timestamp": 0.7386544421709981 + }, + { + "x": 1.8518984587698175, + "y": 6.755259929382264, + "heading": 0.3908638510792468, + "angularVelocity": 0.5923293288375308, + "velocityX": 2.1168929039657844, + "velocityY": -0.3365859481168989, + "timestamp": 0.8207271579677756 + }, + { + "x": 1.9966808291546925, + "y": 6.732239139216171, + "heading": 0.43140805301261054, + "angularVelocity": 0.49400341562665323, + "velocityX": 1.7640743209153051, + "velocityY": -0.2804926088116284, + "timestamp": 0.9027998737645532 + }, + { + "x": 2.1125066103107177, + "y": 6.713822306194192, + "heading": 0.46386100769226224, + "angularVelocity": 0.39541709281327203, + "velocityX": 1.4112580536852743, + "velocityY": -0.2243965347458558, + "timestamp": 0.9848725895613307 + }, + { + "x": 2.1993759293412, + "y": 6.700009583149955, + "heading": 0.4882072247325505, + "angularVelocity": 0.2966420302280796, + "velocityX": 1.0584433351222509, + "velocityY": -0.16829859850671558, + "timestamp": 1.0669453053581084 + }, + { + "x": 2.257288845375724, + "y": 6.690801066111273, + "heading": 0.5044377142386447, + "angularVelocity": 0.197757431912976, + "velocityX": 0.7056293370128478, + "velocityY": -0.11219949223421748, + "timestamp": 1.149018021154886 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.09884777520270435, + "velocityX": 0.35281518848860643, + "velocityY": -0.05609978931418552, + "timestamp": 1.2310907369516635 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -9.87865478987197e-28, + "velocityX": 1.3097638047391152e-27, + "velocityY": 4.333298605574227e-28, + "timestamp": 1.313163452748441 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.2.traj b/src/main/deploy/choreo/AmpLanePADESprint.2.traj index 4fd97589..3410773a 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.2.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.2.traj @@ -1,76 +1,77 @@ { - "samples": [ - { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": 0, - "velocityX": 3.087311543739499e-36, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 2.3177158256290964, - "y": 6.704097470773247, - "heading": 0.5125504196, - "angularVelocity": -9.1973743139294e-17, - "velocityX": 0.34546318060283326, - "velocityY": 0.1965022887733569, - "timestamp": 0.09109647952885869 - }, - { - "x": 2.380656782769881, - "y": 6.7398988031009575, - "heading": 0.5125504196, - "angularVelocity": -1.3336684994607708e-16, - "velocityX": 0.6909263394843395, - "velocityY": 0.3930045651914449, - "timestamp": 0.18219295905771737 - }, - { - "x": 2.475068211555481, - "y": 6.793600797653198, - "heading": 0.5125504196, - "angularVelocity": 6.653839443641591e-19, - "velocityX": 1.0363894332018742, - "velocityY": 0.5895068045437317, - "timestamp": 0.27328943858657606 - }, - { - "x": 2.569479640341081, - "y": 6.847302792205439, - "heading": 0.5125504196, - "angularVelocity": 5.174999729369457e-17, - "velocityX": 1.0363894332018742, - "velocityY": 0.5895068045437317, - "timestamp": 0.36438591811543475 - }, - { - "x": 2.6324205974818655, - "y": 6.8831041245331495, - "heading": 0.5125504196, - "angularVelocity": -1.3901806543706914e-17, - "velocityX": 0.6909263394843393, - "velocityY": 0.39300456519144494, - "timestamp": 0.45548239764429344 - }, - { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "angularVelocity": -1.8725484396545262e-17, - "velocityX": 0.3454631806028332, - "velocityY": 0.19650228877335693, - "timestamp": 0.5465788771731521 - }, - { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "angularVelocity": 1.431796971368011e-33, - "velocityX": 3.743360269099248e-35, - "velocityY": -8.057233109044706e-35, - "timestamp": 0.6376753567020108 - } - ] + "samples": [ + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -9.87865478987197e-28, + "velocityX": 1.3097638047391152e-27, + "velocityY": 4.333298605574227e-28, + "timestamp": 0 + }, + { + "x": 2.3177158256290964, + "y": 6.704097470773247, + "heading": 0.5125504196, + "angularVelocity": -1.9471938619576857e-16, + "velocityX": 0.3454631797391752, + "velocityY": 0.19650228828210153, + "timestamp": 0.09109647975660007 + }, + { + "x": 2.380656782769881, + "y": 6.7398988031009575, + "heading": 0.5125504196, + "angularVelocity": -2.58789471726069e-16, + "velocityX": 0.6909263377570228, + "velocityY": 0.3930045642089339, + "timestamp": 0.18219295951320014 + }, + { + "x": 2.475068211555481, + "y": 6.793600797653198, + "heading": 0.5125504196, + "angularVelocity": -4.208588013010918e-16, + "velocityX": 1.0363894306108958, + "velocityY": 0.5895068030699635, + "timestamp": 0.2732894392698002 + }, + { + "x": 2.569479640341081, + "y": 6.847302792205439, + "heading": 0.5125504196, + "angularVelocity": -2.430413480392466e-16, + "velocityX": 1.0363894306108958, + "velocityY": 0.5895068030699636, + "timestamp": 0.3643859190264003 + }, + { + "x": 2.6324205974818655, + "y": 6.8831041245331495, + "heading": 0.5125504196, + "angularVelocity": -8.097281474944775e-17, + "velocityX": 0.6909263377570227, + "velocityY": 0.393004564208934, + "timestamp": 0.45548239878300034 + }, + { + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "angularVelocity": -1.1468969708015058e-16, + "velocityX": 0.3454631797391751, + "velocityY": 0.19650228828210156, + "timestamp": 0.5465788785396004 + }, + { + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "angularVelocity": 2.11830418797891e-27, + "velocityX": 1.5918238245881604e-27, + "velocityY": -9.542216118093543e-27, + "timestamp": 0.6376753582962005 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.3.traj b/src/main/deploy/choreo/AmpLanePADESprint.3.traj index 963ea88e..2fce25c4 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.3.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.3.traj @@ -1,526 +1,527 @@ { - "samples": [ - { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "angularVelocity": 1.431796971368011e-33, - "velocityX": 3.743360269099248e-35, - "velocityY": -8.057233109044706e-35, - "timestamp": 0 - }, - { - "x": 2.6835271230542426, - "y": 6.903699548014665, - "heading": 0.5052880772246282, - "angularVelocity": -0.10752621184517612, - "velocityX": 0.2907312178663514, - "velocityY": 0.039898557617057115, - "timestamp": 0.06754020485561862 - }, - { - "x": 2.7228033814945674, - "y": 6.909089957964811, - "heading": 0.49095749727137794, - "angularVelocity": -0.21217850884350908, - "velocityX": 0.5815241236576919, - "velocityY": 0.0798103879262595, - "timestamp": 0.13508040971123703 - }, - { - "x": 2.7817246923505206, - "y": 6.91717710303389, - "heading": 0.46979167660622734, - "angularVelocity": -0.31338105518627096, - "velocityX": 0.8723886902906202, - "velocityY": 0.11973823719319023, - "timestamp": 0.20262061456685543 - }, - { - "x": 2.860296723174767, - "y": 6.927962260865171, - "heading": 0.4420737054643263, - "angularVelocity": -0.41039216865205086, - "velocityX": 1.1633371706853932, - "velocityY": 0.15968500324119947, - "timestamp": 0.27016081942247383 - }, - { - "x": 2.9585261643123166, - "y": 6.941446920815512, - "heading": 0.40815310467896726, - "angularVelocity": -0.502228278073356, - "velocityX": 1.454384708301312, - "velocityY": 0.19965382070083557, - "timestamp": 0.33770102427809223 - }, - { - "x": 3.0764209718394073, - "y": 6.9576328058659405, - "heading": 0.3684706356327604, - "angularVelocity": -0.5875384762459123, - "velocityX": 1.7455500435498434, - "velocityY": 0.23964814861057657, - "timestamp": 0.40524122913371063 - }, - { - "x": 3.2139906503876703, - "y": 6.9765218958360915, - "heading": 0.323597871318007, - "angularVelocity": -0.6643859670055544, - "velocityX": 2.0368561043358877, - "velocityY": 0.27967178972185397, - "timestamp": 0.47278143398932904 - }, - { - "x": 3.3712465090237984, - "y": 6.998116434011635, - "heading": 0.27430452270447037, - "angularVelocity": -0.7298371202591344, - "velocityX": 2.3283296071176522, - "velocityY": 0.3197286449116718, - "timestamp": 0.5403216388449474 - }, - { - "x": 3.5482015820439243, - "y": 7.022418860702876, - "heading": 0.22168358710657873, - "angularVelocity": -0.7791053596947181, - "velocityX": 2.619996095635254, - "velocityY": 0.3598216313260006, - "timestamp": 0.6078618437005658 - }, - { - "x": 3.7448688058303015, - "y": 7.04943148246024, - "heading": 0.16741520227188003, - "angularVelocity": -0.8034974864335817, - "velocityX": 2.911854120176202, - "velocityY": 0.3999487685166161, - "timestamp": 0.6754020485561842 - }, - { - "x": 3.961249724160237, - "y": 7.079155012062387, - "heading": 0.1144423105156959, - "angularVelocity": -0.7843164211512963, - "velocityX": 3.2037350018777047, - "velocityY": 0.4400864591051642, - "timestamp": 0.7429422534118026 - }, - { - "x": 4.197248092224393, - "y": 7.111577952654102, - "heading": 0.06945280731873603, - "angularVelocity": -0.6661144024234814, - "velocityX": 3.494190883321295, - "velocityY": 0.4800539273019009, - "timestamp": 0.810482458267421 - }, - { - "x": 4.450279816565845, - "y": 7.14615999986413, - "heading": 0.06926458523360347, - "angularVelocity": -0.0027868154314146247, - "velocityX": 3.746386687490236, - "velocityY": 0.5120216511625231, - "timestamp": 0.8780226631230394 - }, - { - "x": 4.703335353284152, - "y": 7.180937109425349, - "heading": 0.06926456657564138, - "angularVelocity": -2.762497114544702e-7, - "velocityX": 3.746739253445653, - "velocityY": 0.5149097435455242, - "timestamp": 0.9455628679786579 - }, - { - "x": 4.956390888818978, - "y": 7.215714227598158, - "heading": 0.06926454791768001, - "angularVelocity": -2.7624970048363726e-7, - "velocityX": 3.746739235923052, - "velocityY": 0.5149098710487069, - "timestamp": 1.0131030728342763 - }, - { - "x": 5.209446424353711, - "y": 7.250491345771652, - "heading": 0.0692645292597187, - "angularVelocity": -2.7624969990656975e-7, - "velocityX": 3.7467392359216607, - "velocityY": 0.5149098710588286, - "timestamp": 1.0806432776898947 - }, - { - "x": 5.462501959888314, - "y": 7.285268463946087, - "heading": 0.0692645106017574, - "angularVelocity": -2.7624969939846597e-7, - "velocityX": 3.7467392359197462, - "velocityY": 0.5149098710727624, - "timestamp": 1.148183482545513 - }, - { - "x": 5.715557495424461, - "y": 7.320045582109288, - "heading": 0.06926449194379608, - "angularVelocity": -2.762496999067462e-7, - "velocityX": 3.746739235942603, - "velocityY": 0.5149098709064452, - "timestamp": 1.2157236874011315 - }, - { - "x": 5.968613025759916, - "y": 7.354822738115272, - "heading": 0.06926447328583468, - "angularVelocity": -2.762497009102298e-7, - "velocityX": 3.7467391589411925, - "velocityY": 0.5149104312065347, - "timestamp": 1.2832638922567499 - }, - { - "x": 6.221690653746151, - "y": 7.389438720345797, - "heading": 0.06926445462738323, - "angularVelocity": -2.7625695670270355e-7, - "velocityX": 3.74706633666926, - "velocityY": 0.5125240929387752, - "timestamp": 1.3508040971123683 - }, - { - "x": 6.476102895472122, - "y": 7.412263085598601, - "heading": 0.06926437909315673, - "angularVelocity": -0.0000011183594520291744, - "velocityX": 3.766826622303432, - "velocityY": 0.33793745964490024, - "timestamp": 1.4183443019679867 - }, - { - "x": 6.712136074986816, - "y": 7.430641724558031, - "heading": 0.04341074490415746, - "angularVelocity": -0.38278880326565307, - "velocityX": 3.4947063015172035, - "velocityY": 0.27211405412107026, - "timestamp": 1.485884506823605 - }, - { - "x": 6.9285315703702866, - "y": 7.445814297629393, - "heading": 0.019185726579151432, - "angularVelocity": -0.3586755233685199, - "velocityX": 3.203950829673405, - "velocityY": 0.2246450555457597, - "timestamp": 1.5534247116792235 - }, - { - "x": 7.125290870666504, - "y": 7.458, - "heading": 0, - "angularVelocity": -0.2840637901552857, - "velocityX": 2.913217404608591, - "velocityY": 0.18042146002750595, - "timestamp": 1.6209649165348419 - }, - { - "x": 7.284551146493795, - "y": 7.4665356658331685, - "heading": -0.011983560395956015, - "angularVelocity": -0.19978893791899283, - "velocityX": 2.6551742811725556, - "velocityY": 0.1423059220209307, - "timestamp": 1.6809460171942534 - }, - { - "x": 7.428286998104642, - "y": 7.47296871812108, - "heading": -0.020071347748117502, - "angularVelocity": -0.13483892864997732, - "velocityX": 2.396352351501819, - "velocityY": 0.10725132111930623, - "timestamp": 1.740927117853665 - }, - { - "x": 7.556472866857842, - "y": 7.4774155432867, - "heading": -0.02498803078182015, - "angularVelocity": -0.08197053704667534, - "velocityX": 2.137104310257201, - "velocityY": 0.0741371051336659, - "timestamp": 1.8009082185130765 - }, - { - "x": 7.669092898448606, - "y": 7.47995645101358, - "heading": -0.027230015787281982, - "angularVelocity": -0.037378190476904, - "velocityX": 1.877591947341067, - "velocityY": 0.04236180561785803, - "timestamp": 1.860889319172488 - }, - { - "x": 7.766136403587462, - "y": 7.480650177988498, - "heading": -0.027158837231048453, - "angularVelocity": 0.001186683062681687, - "velocityX": 1.6179013734658474, - "velocityY": 0.011565759335732523, - "timestamp": 1.9208704198318995 - }, - { - "x": 7.847595735447885, - "y": 7.479541543526011, - "heading": -0.025049657080079103, - "angularVelocity": 0.03516407881452247, - "velocityX": 1.3580833123248393, - "velocityY": -0.018483063003166142, - "timestamp": 1.980851520491311 - }, - { - "x": 7.913465176456017, - "y": 7.4766658686921605, - "heading": -0.021118970688138776, - "angularVelocity": 0.06553208175121362, - "velocityX": 1.0981699282605077, - "velocityY": -0.04794301542047442, - "timestamp": 2.040832621150723 - }, - { - "x": 7.963740302847386, - "y": 7.472051704673601, - "heading": -0.015541591171907823, - "angularVelocity": 0.09298561471722216, - "velocityX": 0.8381827915570343, - "velocityY": -0.07692696479112789, - "timestamp": 2.100813721810134 - }, - { - "x": 7.998417597522527, - "y": 7.465722607991469, - "heading": -0.008461644038298986, - "angularVelocity": 0.11803629903043378, - "velocityX": 0.5781370180592035, - "velocityY": -0.10551818176978627, - "timestamp": 2.160794822469545 - }, - { - "x": 8.017494201660156, - "y": 7.457698345184326, - "heading": 0, - "angularVelocity": 0.14107183671647575, - "velocityX": 0.3180435825269393, - "velocityY": -0.1337798526356988, - "timestamp": 2.220775923128956 - }, - { - "x": 8.008975668857596, - "y": 7.440959395735479, - "heading": 0.016440344425850486, - "angularVelocity": 0.17454816573497248, - "velocityX": -0.09044179592138195, - "velocityY": -0.17771847395317164, - "timestamp": 2.3149639333931997 - }, - { - "x": 7.961981832760565, - "y": 7.4200887947149115, - "heading": 0.0360050890089686, - "angularVelocity": 0.20772011775415367, - "velocityX": -0.4989364990850843, - "velocityY": -0.22158447728129313, - "timestamp": 2.4091519436574433 - }, - { - "x": 7.876511570645417, - "y": 7.39509531765153, - "heading": 0.058657775076795544, - "angularVelocity": 0.24050498576490828, - "velocityX": -0.9074431222759898, - "velocityY": -0.2653573102697685, - "timestamp": 2.503339953921687 - }, - { - "x": 7.752563396875145, - "y": 7.365990633137338, - "heading": 0.08435039467917142, - "angularVelocity": 0.2727801503641011, - "velocityX": -1.315965518567977, - "velocityY": -0.3090062570866462, - "timestamp": 2.5975279641859306 - }, - { - "x": 7.590135252234061, - "y": 7.332791018782474, - "heading": 0.11301654484596554, - "angularVelocity": 0.3043503104733978, - "velocityX": -1.7245097776818146, - "velocityY": -0.35248238349789773, - "timestamp": 2.691715974450174 - }, - { - "x": 7.3892240901384, - "y": 7.295520773303632, - "heading": 0.14455765680612448, - "angularVelocity": 0.33487395977121315, - "velocityX": -2.13308638256617, - "velocityY": -0.39570052891318314, - "timestamp": 2.785903984714418 - }, - { - "x": 7.149824938411661, - "y": 7.254220108341429, - "heading": 0.1788109128374065, - "angularVelocity": 0.36366896312157765, - "velocityX": -2.54171577735964, - "velocityY": -0.4384917448232911, - "timestamp": 2.8800919949786614 - }, - { - "x": 6.871928226350113, - "y": 7.208968463075644, - "heading": 0.21545413209734332, - "angularVelocity": 0.3890433523028524, - "velocityX": -2.9504467849136042, - "velocityY": -0.4804395499897761, - "timestamp": 2.974280005242905 - }, - { - "x": 6.555508023323144, - "y": 7.15999689291417, - "heading": 0.25354713749374036, - "angularVelocity": 0.40443582245264, - "velocityX": -3.3594530995957568, - "velocityY": -0.5199342254293745, - "timestamp": 3.0684680155071486 - }, - { - "x": 6.201946981022424, - "y": 7.116596127430346, - "heading": 0.25354714311943394, - "angularVelocity": 5.972834101491316e-8, - "velocityX": -3.753779714730222, - "velocityY": -0.46078864350212084, - "timestamp": 3.1626560257713923 - }, - { - "x": 5.847829680383322, - "y": 7.077995922009697, - "heading": 0.25354714640605686, - "angularVelocity": 3.4894280958051e-8, - "velocityX": -3.7596855443238373, - "velocityY": -0.4098207968546854, - "timestamp": 3.256844036035636 - }, - { - "x": 5.493712379149646, - "y": 7.039395722043647, - "heading": 0.25354714969267966, - "angularVelocity": 3.4894280048226024e-8, - "velocityX": -3.759685550636459, - "velocityY": -0.40982073894285953, - "timestamp": 3.3510320462998795 - }, - { - "x": 5.13959507794107, - "y": 7.0007955218474756, - "heading": 0.25354715297933, - "angularVelocity": 3.489457219723442e-8, - "velocityX": -3.7596855503699804, - "velocityY": -0.4098207413860709, - "timestamp": 3.445220056564123 - }, - { - "x": 4.793375065328026, - "y": 6.963049240971564, - "heading": 0.2809926873119857, - "angularVelocity": 0.2913909557666365, - "velocityX": -3.675839543076952, - "velocityY": -0.4007546265178986, - "timestamp": 3.5394080668283667 - }, - { - "x": 4.4856271372174605, - "y": 6.929500155423233, - "heading": 0.3062072463057812, - "angularVelocity": 0.2677045509620205, - "velocityX": -3.2673790140292933, - "velocityY": -0.3561927410315718, - "timestamp": 3.6335960770926103 - }, - { - "x": 4.216349125910067, - "y": 6.900146018877801, - "heading": 0.32859640421091696, - "angularVelocity": 0.23770709076795637, - "velocityX": -2.858941499580855, - "velocityY": -0.31165470491497754, - "timestamp": 3.727784087356854 - }, - { - "x": 3.9855401854111148, - "y": 6.874986045056756, - "heading": 0.34796083888612495, - "angularVelocity": 0.2055934149249067, - "velocityX": -2.450512967111424, - "velocityY": -0.26712501676655553, - "timestamp": 3.8219720976210976 - }, - { - "x": 3.7931998687162816, - "y": 6.854019825581219, - "heading": 0.36419988308525797, - "angularVelocity": 0.17241094862896578, - "velocityX": -2.042089180514842, - "velocityY": -0.22259966440226955, - "timestamp": 3.916160107885341 - }, - { - "x": 3.639327899901621, - "y": 6.837247108195556, - "heading": 0.37725261237367164, - "angularVelocity": 0.13858164379727655, - "velocityX": -1.6336683234200817, - "velocityY": -0.17807699025180046, - "timestamp": 4.010348118149585 - }, - { - "x": 3.5239240923326007, - "y": 6.824667722043819, - "heading": 0.38707830352299477, - "angularVelocity": 0.10431997790119188, - "velocityX": -1.2252494478358333, - "velocityY": -0.13355613008965372, - "timestamp": 4.104536128413828 - }, - { - "x": 3.4469883122501845, - "y": 6.8162815454148475, - "heading": 0.39364810050149374, - "angularVelocity": 0.06975194571015403, - "velocityX": -0.8168319923796448, - "velocityY": -0.08903656214250527, - "timestamp": 4.198724138678072 - }, - { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0.03495947190378589, - "velocityX": -0.4084155935928461, - "velocityY": -0.04451793673752315, - "timestamp": 4.292912148942316 - }, - { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": -1.9952146739162206e-34, - "velocityY": -3.951795765453016e-33, - "timestamp": 4.387100159206559 - } - ] + "samples": [ + { + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "angularVelocity": 2.11830418797891e-27, + "velocityX": 1.5918238245881604e-27, + "velocityY": -9.542216118093543e-27, + "timestamp": 0 + }, + { + "x": 2.6835272178710947, + "y": 6.903699093874769, + "heading": 0.5052895060867068, + "angularVelocity": -0.10750505591698582, + "velocityX": 0.2907326211779112, + "velocityY": 0.039891833548624495, + "timestamp": 0.06754020498254554 + }, + { + "x": 2.722803664548093, + "y": 6.909088595285118, + "heading": 0.4909617568575599, + "angularVelocity": -0.21213659675521643, + "velocityX": 0.5815269095962702, + "velocityY": 0.0797969359397381, + "timestamp": 0.13508040996509085 + }, + { + "x": 2.7817252554319474, + "y": 6.9171743770902525, + "heading": 0.4698001364111611, + "angularVelocity": -0.3133188661756009, + "velocityX": 0.872392834743124, + "velocityY": 0.119718052487766, + "timestamp": 0.20262061494763617 + }, + { + "x": 2.8602976561490117, + "y": 6.927957716546488, + "heading": 0.44208769535784764, + "angularVelocity": -0.410310289411695, + "velocityX": 1.1633426451307045, + "velocityY": 0.1596580800875858, + "timestamp": 0.2701608199301815 + }, + { + "x": 2.958527554736122, + "y": 6.9414401025597545, + "heading": 0.4081739049952151, + "angularVelocity": -0.5021274420383679, + "velocityX": 1.4543914785644525, + "velocityY": 0.19962015242254944, + "timestamp": 0.3377010249127268 + }, + { + "x": 3.0764229044730724, + "y": 6.9576232575928, + "heading": 0.3684994634316936, + "angularVelocity": -0.5874196202658062, + "velocityX": 1.7455580682264529, + "velocityY": 0.2396077275339608, + "timestamp": 0.4052412298952721 + }, + { + "x": 3.2139932065748873, + "y": 6.976509160880165, + "heading": 0.32363586089987717, + "angularVelocity": -0.6642503164361231, + "velocityX": 2.036865332839415, + "velocityY": 0.2796246071839072, + "timestamp": 0.47278143487781743 + }, + { + "x": 3.371249765918335, + "y": 6.998100055077647, + "heading": 0.2743526968387638, + "angularVelocity": -0.7296863264458512, + "velocityX": 2.3283399774117917, + "velocityY": 0.31967469158647477, + "timestamp": 0.5403216398603627 + }, + { + "x": 3.548205611708083, + "y": 7.022398379906059, + "heading": 0.22174281137258775, + "angularVelocity": -0.7789417500253684, + "velocityX": 2.620007532335429, + "velocityY": 0.3597608984854559, + "timestamp": 0.6078618448429081 + }, + { + "x": 3.7448736745121907, + "y": 7.049406441609737, + "heading": 0.16748610321735644, + "angularVelocity": -0.8033246000549304, + "velocityX": 2.911866537197107, + "velocityY": 0.3998812516286613, + "timestamp": 0.6754020498254534 + }, + { + "x": 3.9612554938842215, + "y": 7.079124953628088, + "heading": 0.11452509717802098, + "angularVelocity": -0.7841404398020747, + "velocityX": 3.203748336682593, + "velocityY": 0.44001216795286324, + "timestamp": 0.7429422548079987 + }, + { + "x": 4.19725484401393, + "y": 7.11154242557216, + "heading": 0.06954671604514642, + "angularVelocity": -0.6659497279361002, + "velocityX": 3.494205417213319, + "velocityY": 0.4799729576250136, + "timestamp": 0.810482459790544 + }, + { + "x": 4.4502883587972, + "y": 7.14611913479446, + "heading": 0.06936242836661095, + "angularVelocity": -0.002728562618119077, + "velocityX": 3.7464131897240014, + "velocityY": 0.5119426159757035, + "timestamp": 0.8780226647730893 + }, + { + "x": 4.703344709577149, + "y": 7.180890314474715, + "heading": 0.0693624097128088, + "angularVelocity": -2.761881187265661e-7, + "velocityX": 3.7467512993978347, + "velocityY": 0.5148219447844706, + "timestamp": 0.9455628697556346 + }, + { + "x": 4.956401059058472, + "y": 7.215661503606044, + "heading": 0.06936239105900772, + "angularVelocity": -2.761881027324315e-7, + "velocityX": 3.7467512801704235, + "velocityY": 0.5148220847170133, + "timestamp": 1.01310307473818 + }, + { + "x": 5.209457408539732, + "y": 7.250432692737848, + "heading": 0.06936237240520664, + "angularVelocity": -2.761881030342619e-7, + "velocityX": 3.7467512801694554, + "velocityY": 0.5148220847240553, + "timestamp": 1.0806432797207253 + }, + { + "x": 5.462513758020992, + "y": 7.285203881869653, + "heading": 0.06936235375140556, + "angularVelocity": -2.761881026512252e-7, + "velocityX": 3.746751280169456, + "velocityY": 0.5148220847240557, + "timestamp": 1.1481834847032706 + }, + { + "x": 5.715570107502252, + "y": 7.319975071001454, + "heading": 0.0693623350976045, + "angularVelocity": -2.761881026475552e-7, + "velocityX": 3.7467512801694602, + "velocityY": 0.51482208472402, + "timestamp": 1.215723689685816 + }, + { + "x": 5.968626456990083, + "y": 7.354746260085438, + "heading": 0.06936231644380343, + "angularVelocity": -2.761881026693173e-7, + "velocityX": 3.746751280266743, + "velocityY": 0.5148220840160207, + "timestamp": 1.2832638946683612 + }, + { + "x": 6.221682937041094, + "y": 7.389516498948747, + "heading": 0.06936229778999946, + "angularVelocity": -2.761881456453648e-7, + "velocityX": 3.746753213384661, + "velocityY": 0.5148080150525901, + "timestamp": 1.3508040996509065 + }, + { + "x": 6.476098697142329, + "y": 7.412301605590586, + "heading": 0.0693622227466872, + "angularVelocity": -0.0000011110909759881076, + "velocityX": 3.7668787082743447, + "velocityY": 0.33735619617571233, + "timestamp": 1.4183443046334518 + }, + { + "x": 6.712134128533923, + "y": 7.430661265319214, + "heading": 0.043459048416696004, + "angularVelocity": -0.38352229367212326, + "velocityX": 3.4947396362298986, + "velocityY": 0.27183304719570117, + "timestamp": 1.4858845096159972 + }, + { + "x": 6.928530828538707, + "y": 7.445821895754349, + "heading": 0.019203692642594847, + "angularVelocity": -0.3591246988422603, + "velocityX": 3.2039686592705516, + "velocityY": 0.22446823250023049, + "timestamp": 1.5534247145985425 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": -3.857889621465489e-27, + "angularVelocity": -0.2843297950836498, + "velocityX": 2.9132283826891787, + "velocityY": 0.18030896188127155, + "timestamp": 1.6209649195810878 + }, + { + "x": 7.284551274277721, + "y": 7.4665314387188895, + "heading": -0.0119926176075512, + "angularVelocity": -0.19994035049895104, + "velocityX": 2.6551818761052863, + "velocityY": 0.14223574064773895, + "timestamp": 1.68094589679566 + }, + { + "x": 7.428287142227841, + "y": 7.472962064944413, + "heading": -0.020085170649452998, + "angularVelocity": -0.1349186595101974, + "velocityX": 2.396357555761855, + "velocityY": 0.10721109465287079, + "timestamp": 1.7409268740102324 + }, + { + "x": 7.556472961771024, + "y": 7.477407773553819, + "heading": -0.025003758777922633, + "angularVelocity": -0.0820024673968577, + "velocityX": 2.137107888132909, + "velocityY": 0.07411864254065184, + "timestamp": 1.8009078512248047 + }, + { + "x": 7.669092912784827, + "y": 7.479948525190578, + "heading": -0.027245693064255495, + "angularVelocity": -0.037377421816798616, + "velocityX": 1.8775944681748589, + "velocityY": 0.04235929047423255, + "timestamp": 1.860888828439377 + }, + { + "x": 7.766136331910851, + "y": 7.480642796103092, + "heading": -0.027173129314670138, + "angularVelocity": 0.0012097793826494725, + "velocityX": 1.617903269212617, + "velocityY": 0.01157485164055792, + "timestamp": 1.9208698056539493 + }, + { + "x": 7.84759559249558, + "y": 7.479535203925268, + "heading": -0.025061681495632242, + "angularVelocity": 0.03520195763874475, + "velocityX": 1.3580849190455957, + "velocityY": -0.018465724122206602, + "timestamp": 1.9808507828685216 + }, + { + "x": 7.91346499303532, + "y": 7.476660908974376, + "heading": -0.021128188015767303, + "angularVelocity": 0.06557901625699587, + "velocityX": 1.0981715136801353, + "velocityY": -0.047920108747306074, + "timestamp": 2.040831760083094 + }, + { + "x": 7.963740122835744, + "y": 7.472048331328805, + "heading": -0.015547730952116253, + "angularVelocity": 0.0930371148120487, + "velocityX": 0.8381845734285565, + "velocityY": -0.0769006751768895, + "timestamp": 2.100812737297667 + }, + { + "x": 7.998417475618676, + "y": 7.46572091854732, + "heading": -0.008464652154293805, + "angularVelocity": 0.1180887529138424, + "velocityX": 0.5781391766739492, + "velocityY": -0.10549032502171554, + "timestamp": 2.1607937145122396 + }, + { + "x": 8.017494201660156, + "y": 7.457698345184326, + "heading": -2.2921744122616346e-27, + "angularVelocity": 0.1411222782185235, + "velocityX": 0.318046269457016, + "velocityY": -0.13375196163099834, + "timestamp": 2.2207746917268123 + }, + { + "x": 8.008975913339244, + "y": 7.440961813507422, + "heading": 0.016444448853137204, + "angularVelocity": 0.1745916436893877, + "velocityX": -0.09043914895843151, + "velocityY": -0.17769270355161393, + "timestamp": 2.3149627554029477 + }, + { + "x": 7.96198229634572, + "y": 7.420093426479306, + "heading": 0.036012649187603114, + "angularVelocity": 0.20775668986837795, + "velocityX": -0.49893388991528925, + "velocityY": -0.2215608455427296, + "timestamp": 2.409150819079083 + }, + { + "x": 7.876512228216728, + "y": 7.395101957451527, + "heading": 0.05866814941015883, + "angularVelocity": 0.24053472741998802, + "velocityX": -0.9074405481238055, + "velocityY": -0.2653358403641312, + "timestamp": 2.5033388827552185 + }, + { + "x": 7.752564223658973, + "y": 7.365999072123293, + "heading": 0.0843629510276007, + "angularVelocity": 0.2728031622541168, + "velocityX": -1.3159629757752496, + "velocityY": -0.3089869797971924, + "timestamp": 2.597526946431354 + }, + { + "x": 7.590136223929822, + "y": 7.332801044076014, + "heading": 0.1130306642056742, + "angularVelocity": 0.3043667324624816, + "velocityX": -1.7245072612136703, + "velocityY": -0.3524653416958384, + "timestamp": 2.6917150101074894 + }, + { + "x": 7.389225183137582, + "y": 7.295532166028874, + "heading": 0.14457273960345385, + "angularVelocity": 0.334883998743585, + "velocityX": -2.1330838850564935, + "velocityY": -0.3956857864207608, + "timestamp": 2.785903073783625 + }, + { + "x": 7.149826130214695, + "y": 7.254232639741168, + "heading": 0.17882639129814942, + "angularVelocity": 0.36367295767408864, + "velocityX": -2.5417132870047907, + "velocityY": -0.43847940679313435, + "timestamp": 2.88009113745976 + }, + { + "x": 6.8719294965182485, + "y": 7.208981885028011, + "heading": 0.21546950044257604, + "angularVelocity": 0.38904196258268475, + "velocityX": -2.950444279776156, + "velocityY": -0.48042982249588434, + "timestamp": 2.9742792011358956 + }, + { + "x": 6.555509356459519, + "y": 7.160010902115707, + "heading": 0.2535620654404122, + "angularVelocity": 0.40443091736992315, + "velocityX": -3.359450525989546, + "velocityY": -0.51992769572894, + "timestamp": 3.068467264812031 + }, + { + "x": 6.20194815910043, + "y": 7.116609756478925, + "heading": 0.2535620710648909, + "angularVelocity": 5.971540845532163e-8, + "velocityX": -3.753779232311261, + "velocityY": -0.4607924183048939, + "timestamp": 3.1626553284881664 + }, + { + "x": 5.8478308523572835, + "y": 7.078007759401044, + "heading": 0.2535620743508316, + "angularVelocity": 3.488701802778144e-8, + "velocityX": -3.7596834770993417, + "velocityY": -0.40983958658088665, + "timestamp": 3.256843392164302 + }, + { + "x": 5.493713536071184, + "y": 7.039405849865993, + "heading": 0.2535620776367723, + "angularVelocity": 3.488701827976151e-8, + "velocityX": -3.759683578417396, + "velocityY": -0.40983865713370193, + "timestamp": 3.3510314558404373 + }, + { + "x": 5.139596219795213, + "y": 7.00080394023819, + "heading": 0.2535620809227431, + "angularVelocity": 3.488733818198086e-8, + "velocityX": -3.759683578309871, + "velocityY": -0.4098386581184616, + "timestamp": 3.4452195195165727 + }, + { + "x": 4.793375987292505, + "y": 6.963055984000021, + "heading": 0.2810067536056752, + "angularVelocity": 0.2913816423416479, + "velocityX": -3.6758397931736098, + "velocityY": -0.40077218667499337, + "timestamp": 3.539407583192708 + }, + { + "x": 4.485627858757115, + "y": 6.929505403826658, + "heading": 0.30621912928575457, + "angularVelocity": 0.2676812187876781, + "velocityX": -3.2673792890952553, + "velocityY": -0.35620840756134725, + "timestamp": 3.6335956468688435 + }, + { + "x": 4.216349669557381, + "y": 6.9001499572465725, + "heading": 0.32860581789982174, + "angularVelocity": 0.23768073936675813, + "velocityX": -2.8589417670337087, + "velocityY": -0.3116684368947614, + "timestamp": 3.727783710544979 + }, + { + "x": 3.985540575144852, + "y": 6.8749888593305, + "heading": 0.3479678404777486, + "angularVelocity": 0.20556768896431482, + "velocityX": -2.450513211590839, + "velocityY": -0.26713679986657873, + "timestamp": 3.8219717742211143 + }, + { + "x": 3.793200129317667, + "y": 6.854021702392079, + "heading": 0.36420470133991156, + "angularVelocity": 0.17238767024655313, + "velocityX": -2.0420893934983666, + "velocityY": -0.22260949126755833, + "timestamp": 3.9161598378972498 + }, + { + "x": 3.6393280566589286, + "y": 6.837248234599062, + "heading": 0.37725557912927543, + "angularVelocity": 0.13856190774065766, + "velocityX": -1.633668499522676, + "velocityY": -0.1780848563857597, + "timestamp": 4.010347901573385 + }, + { + "x": 3.5239241708826787, + "y": 6.824668285381395, + "heading": 0.3870798194050559, + "angularVelocity": 0.10430451473724978, + "velocityX": -1.225249583355537, + "velocityY": -0.13356203245586246, + "timestamp": 4.104535965249521 + }, + { + "x": 3.4469883384836706, + "y": 6.8162817332333585, + "heading": 0.3936486152349144, + "angularVelocity": 0.06974127690367621, + "velocityX": -0.8168320846211579, + "velocityY": -0.08904049855907127, + "timestamp": 4.198724028925656 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.0349539871252265, + "velocityX": -0.4084156405121326, + "velocityY": -0.04451990557217318, + "timestamp": 4.292912092601791 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 4.764169197941492e-24, + "velocityX": 1.177617036966837e-23, + "velocityY": -7.781360494191974e-22, + "timestamp": 4.387100156277927 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.4.traj b/src/main/deploy/choreo/AmpLanePADESprint.4.traj index ab69c40b..70a91a01 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.4.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.4.traj @@ -1,679 +1,680 @@ { - "samples": [ - { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": -1.9952146739162206e-34, - "velocityY": -3.951795765453016e-33, - "timestamp": 0 - }, - { - "x": 3.425127749537185, - "y": 6.810478633677757, - "heading": 0.3886892234850989, - "angularVelocity": -0.13293807639169805, - "velocityX": 0.2675517930120232, - "velocityY": -0.025935588272518423, - "timestamp": 0.0620713067232348 - }, - { - "x": 3.458347138585669, - "y": 6.807258412874914, - "heading": 0.3723514064813247, - "angularVelocity": -0.26321045691257755, - "velocityX": 0.535181081278085, - "velocityY": -0.05187937829625554, - "timestamp": 0.1241426134464696 - }, - { - "x": 3.508184213080087, - "y": 6.802427294600358, - "heading": 0.34812438720722594, - "angularVelocity": -0.3903094771652888, - "velocityX": 0.8029003596884762, - "velocityY": -0.07783174754300737, - "timestamp": 0.1862139201697044 - }, - { - "x": 3.5746455076736363, - "y": 6.795984642746885, - "heading": 0.31624502791908643, - "angularVelocity": -0.5135925272251962, - "velocityX": 1.070724914651617, - "velocityY": -0.10379436479708294, - "timestamp": 0.2482852268929392 - }, - { - "x": 3.6577387353786484, - "y": 6.787929625128416, - "heading": 0.2770021560106679, - "angularVelocity": -0.6322224225666108, - "velocityX": 1.3386737301264124, - "velocityY": -0.12977038898779852, - "timestamp": 0.310356533616174 - }, - { - "x": 3.7574730901724314, - "y": 6.7782611086258875, - "heading": 0.23075444743671972, - "angularVelocity": -0.7450738677080124, - "velocityX": 1.6067706652041787, - "velocityY": -0.1557646682971112, - "timestamp": 0.3724278403394088 - }, - { - "x": 3.873859634378807, - "y": 6.766977536857145, - "heading": 0.17795757602156642, - "angularVelocity": -0.8505841781386996, - "velocityX": 1.875045819887845, - "velocityY": -0.18178402170643457, - "timestamp": 0.4344991470626436 - }, - { - "x": 4.006911762109537, - "y": 6.754076769706622, - "heading": 0.11920696294593584, - "angularVelocity": -0.9465019535937051, - "velocityX": 2.1435367604550115, - "velocityY": -0.2078378534549514, - "timestamp": 0.4965704537858784 - }, - { - "x": 4.156645657189765, - "y": 6.73955582918857, - "heading": 0.055309270826440785, - "angularVelocity": -1.029423988195197, - "velocityX": 2.4122884305927053, - "velocityY": -0.23393966205310818, - "timestamp": 0.5586417605091132 - }, - { - "x": 4.323080375842652, - "y": 6.723410435666096, - "heading": -0.012586184407094544, - "angularVelocity": -1.0938299645646508, - "velocityX": 2.681347106079046, - "velocityY": -0.2601104177560936, - "timestamp": 0.620713067232348 - }, - { - "x": 4.506235840968647, - "y": 6.705634179985004, - "heading": -0.08270962096165607, - "angularVelocity": -1.1297238652836274, - "velocityX": 2.9507267495214404, - "velocityY": -0.2863844281597597, - "timestamp": 0.6827843739555828 - }, - { - "x": 4.706118949857515, - "y": 6.686217722826257, - "heading": -0.15194437984101536, - "angularVelocity": -1.1154068205470853, - "velocityX": 3.2202175117742615, - "velocityY": -0.3128088996947802, - "timestamp": 0.7448556806788176 - }, - { - "x": 4.922607059053276, - "y": 6.66515425622058, - "heading": -0.21301282950448638, - "angularVelocity": -0.9838434678965439, - "velocityX": 3.4877324262085807, - "velocityY": -0.3393430510421988, - "timestamp": 0.8069269874020524 - }, - { - "x": 5.152321807662402, - "y": 6.642265811014449, - "heading": -0.22692060137612477, - "angularVelocity": -0.22406120647088962, - "velocityX": 3.7008202458728654, - "velocityY": -0.3687443750482909, - "timestamp": 0.8689982941252872 - }, - { - "x": 5.385997622338085, - "y": 6.619824669792801, - "heading": -0.22692062699124488, - "angularVelocity": -4.12672480414244e-7, - "velocityX": 3.7646350143327725, - "velocityY": -0.36153808267225507, - "timestamp": 0.931069600848522 - }, - { - "x": 5.6195399174914655, - "y": 6.5960341766076915, - "heading": -0.2269206525768778, - "angularVelocity": -4.121974272362167e-7, - "velocityX": 3.7624839476103156, - "velocityY": -0.3832768221124699, - "timestamp": 0.9931409075717568 - }, - { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": -0.22692067835854435, - "angularVelocity": -4.1535562716932914e-7, - "velocityX": 3.729650183615213, - "velocityY": -0.6268142199732243, - "timestamp": 1.0552122142949916 - }, - { - "x": 5.959383441341322, - "y": 6.535142573313356, - "heading": -0.22692070161190955, - "angularVelocity": -7.95525127627382e-7, - "velocityX": 3.706414340926548, - "velocityY": -0.7521132022680976, - "timestamp": 1.0844424226222174 - }, - { - "x": 6.066940887967902, - "y": 6.509606194124557, - "heading": -0.22692072465630755, - "angularVelocity": -7.883761123678933e-7, - "velocityX": 3.6796674667011553, - "velocityY": -0.8736297361581465, - "timestamp": 1.1136726309494431 - }, - { - "x": 6.17368571603366, - "y": 6.480861582677863, - "heading": -0.2269207475995078, - "angularVelocity": -7.849140177187675e-7, - "velocityX": 3.651866824580282, - "velocityY": -0.9833871563580519, - "timestamp": 1.142902839276669 - }, - { - "x": 6.2799139038286755, - "y": 6.450262838777624, - "heading": -0.2269207705194487, - "angularVelocity": -7.841182881547448e-7, - "velocityX": 3.63419194984251, - "velocityY": -1.0468192206387272, - "timestamp": 1.1721330476038947 - }, - { - "x": 6.38600768248991, - "y": 6.419201268635774, - "heading": -0.22692079344039778, - "angularVelocity": -7.841527793264923e-7, - "velocityX": 3.629593654398156, - "velocityY": -1.0626530537902528, - "timestamp": 1.2013632559311205 - }, - { - "x": 6.4915378820851375, - "y": 6.386275784732387, - "heading": -0.22692082347226308, - "angularVelocity": -0.0000010274256324387564, - "velocityX": 3.6103129479556895, - "velocityY": -1.1264197481865303, - "timestamp": 1.2305934642583463 - }, - { - "x": 6.594254502115419, - "y": 6.3529873750963075, - "heading": -0.23642872835410836, - "angularVelocity": -0.32527667183951403, - "velocityX": 3.514057063171563, - "velocityY": -1.1388358667671954, - "timestamp": 1.259823672585572 - }, - { - "x": 6.69366972769076, - "y": 6.320452523543764, - "heading": -0.25267195567048195, - "angularVelocity": -0.5557000187824542, - "velocityX": 3.4011124540202067, - "velocityY": -1.1130557534298289, - "timestamp": 1.2890538809127978 - }, - { - "x": 6.789488054498416, - "y": 6.288864279277486, - "heading": -0.26990654049679197, - "angularVelocity": -0.5896155317621415, - "velocityX": 3.278058292810801, - "velocityY": -1.0806711985311153, - "timestamp": 1.3182840892400236 - }, - { - "x": 6.881710669103943, - "y": 6.258257632428043, - "heading": -0.28694313831911705, - "angularVelocity": -0.5828421621787924, - "velocityX": 3.1550447254125045, - "velocityY": -1.047089589879878, - "timestamp": 1.3475142975672494 - }, - { - "x": 6.970345859458022, - "y": 6.228646491338886, - "heading": -0.3032367291527766, - "angularVelocity": -0.5574230142767849, - "velocityX": 3.0323146986098455, - "velocityY": -1.0130321603485573, - "timestamp": 1.3767445058944752 - }, - { - "x": 7.055400617247705, - "y": 6.200038168598195, - "heading": -0.3184704659597927, - "angularVelocity": -0.5211641544429054, - "velocityX": 2.909823865690951, - "velocityY": -0.978724558526454, - "timestamp": 1.405974714221701 - }, - { - "x": 7.136880378911089, - "y": 6.172437128670844, - "heading": -0.33243641899697507, - "angularVelocity": -0.477791772157844, - "velocityX": 2.7875190197494866, - "velocityY": -0.9442642220804569, - "timestamp": 1.4352049225489267 - }, - { - "x": 7.214789390563965, - "y": 6.145846366882324, - "heading": -0.344987478573796, - "angularVelocity": -0.42938659336036833, - "velocityX": 2.665359438451487, - "velocityY": -0.9097014120066562, - "timestamp": 1.4644351308761525 - }, - { - "x": 7.370577859130934, - "y": 6.0916577592619054, - "heading": -0.3653758531922844, - "angularVelocity": -0.3132570133567013, - "velocityX": 2.393610638018096, - "velocityY": -0.8325804140238011, - "timestamp": 1.529520264645429 - }, - { - "x": 7.5087533836626745, - "y": 6.042843250581257, - "heading": -0.37947223160146565, - "angularVelocity": -0.21658368958955537, - "velocityX": 2.1229967049244562, - "velocityY": -0.7500101152698444, - "timestamp": 1.5946053984147053 - }, - { - "x": 7.629367562165718, - "y": 5.999619792415774, - "heading": -0.38804895009153, - "angularVelocity": -0.1317769203712161, - "velocityX": 1.85317554897581, - "velocityY": -0.6641064658284028, - "timestamp": 1.6596905321839817 - }, - { - "x": 7.7324579600950285, - "y": 5.962133690972344, - "heading": -0.39162657356882036, - "angularVelocity": -0.05496836635495089, - "velocityX": 1.5839315671495955, - "velocityY": -0.5759548958801737, - "timestamp": 1.7247756659532572 - }, - { - "x": 7.81805300550911, - "y": 5.930490256491027, - "heading": -0.39057997729782273, - "angularVelocity": 0.01608041975771331, - "velocityX": 1.3151243679933469, - "velocityY": -0.4861852876186942, - "timestamp": 1.7898607997225335 - }, - { - "x": 7.886174910867723, - "y": 5.904768911877698, - "heading": -0.38519213768662347, - "angularVelocity": 0.08278141718657105, - "velocityX": 1.0466584519915365, - "velocityY": -0.39519538677620675, - "timestamp": 1.85494593349181 - }, - { - "x": 7.936841490598973, - "y": 5.885031694896893, - "heading": -0.3756842946663964, - "angularVelocity": 0.1460831755210139, - "velocityX": 0.7784662456231665, - "velocityY": -0.3032523072130836, - "timestamp": 1.9200310672610863 - }, - { - "x": 7.970067341476633, - "y": 5.8713284070268905, - "heading": -0.36223415085714655, - "angularVelocity": 0.206654623418748, - "velocityX": 0.510498311264806, - "velocityY": -0.21054405324847428, - "timestamp": 1.9851162010303627 - }, - { - "x": 7.985864639282227, - "y": 5.863699913024902, - "heading": -0.344987478573796, - "angularVelocity": 0.2649863537883773, - "velocityX": 0.24271745160106833, - "velocityY": -0.1172079330593501, - "timestamp": 2.050201334799639 - }, - { - "x": 7.985045656249217, - "y": 5.861990715165632, - "heading": -0.3251381624172612, - "angularVelocity": 0.31897227576671205, - "velocityX": -0.01316080008970204, - "velocityY": -0.02746627272232015, - "timestamp": 2.1124303047275843 - }, - { - "x": 7.968302412952749, - "y": 5.865866788787815, - "heading": -0.3020436028872691, - "angularVelocity": 0.37112231741475454, - "velocityX": -0.26905866055398503, - "velocityY": 0.062287285594966935, - "timestamp": 2.1746592746555296 - }, - { - "x": 7.935633552055529, - "y": 5.875328955639361, - "heading": -0.2758355383990043, - "angularVelocity": 0.4211553641111372, - "velocityX": -0.5249783330022537, - "velocityY": 0.1520540491430483, - "timestamp": 2.236888244583475 - }, - { - "x": 7.887037558008595, - "y": 5.8903781286161365, - "heading": -0.24666763452179902, - "angularVelocity": 0.468719053376244, - "velocityX": -0.7809223598462517, - "velocityY": 0.24183548264097016, - "timestamp": 2.29911721451142 - }, - { - "x": 7.822512734192608, - "y": 5.911015324029987, - "heading": -0.21472164060862464, - "angularVelocity": 0.5133620876283883, - "velocityX": -1.0368936508301385, - "velocityY": 0.33163324795100674, - "timestamp": 2.3613461844393653 - }, - { - "x": 7.742057181096897, - "y": 5.937241677003945, - "heading": -0.1802161756247138, - "angularVelocity": 0.5544919837796514, - "velocityX": -1.29289546635384, - "velocityY": 0.4214492543316279, - "timestamp": 2.4235751543673105 - }, - { - "x": 7.645668783226514, - "y": 5.9690584605532635, - "heading": -0.14341967249459378, - "angularVelocity": 0.5913082471512265, - "velocityX": -1.5489312772168387, - "velocityY": 0.5112857176674774, - "timestamp": 2.4858041242952558 - }, - { - "x": 7.533345224831275, - "y": 6.006467108033043, - "heading": -0.10467019490543185, - "angularVelocity": 0.6226919332592115, - "velocityX": -1.8050043014579413, - "velocityY": 0.6011452145695896, - "timestamp": 2.548033094223201 - }, - { - "x": 7.405084087505747, - "y": 6.04946923502926, - "heading": -0.06440725119965417, - "angularVelocity": 0.6470128583583754, - "velocityX": -2.0611161887146876, - "velocityY": 0.6910306734308482, - "timestamp": 2.6102620641511463 - }, - { - "x": 7.260883177292953, - "y": 6.098066643466027, - "heading": -0.023226072117979224, - "angularVelocity": 0.6617686124221249, - "velocityX": -2.3172633321708815, - "velocityY": 0.7809450886466088, - "timestamp": 2.6724910340790915 - }, - { - "x": 7.100741532540205, - "y": 6.152261239581916, - "heading": 0.018021968863737303, - "angularVelocity": 0.6628430621538008, - "velocityX": -2.5734259290837285, - "velocityY": 0.8708901365174432, - "timestamp": 2.7347200040070367 - }, - { - "x": 6.924662708731195, - "y": 6.212054571383467, - "heading": 0.05802124542072086, - "angularVelocity": 0.6427758100977419, - "velocityX": -2.829531390490462, - "velocityY": 0.9608600603671181, - "timestamp": 2.796948973934982 - }, - { - "x": 6.732667433541327, - "y": 6.277445378277029, - "heading": 0.09448040335619008, - "angularVelocity": 0.5858872158366861, - "velocityX": -3.0853037646642187, - "velocityY": 1.050809726872821, - "timestamp": 2.8591779438629272 - }, - { - "x": 6.524862060286313, - "y": 6.348410321942265, - "heading": 0.12245319566137218, - "angularVelocity": 0.44951398581032764, - "velocityX": -3.33936707446111, - "velocityY": 1.1403843539014786, - "timestamp": 2.9214069137908725 - }, - { - "x": 6.30228026524581, - "y": 6.424202518084616, - "heading": 0.1231057628389814, - "angularVelocity": 0.010486549566300301, - "velocityX": -3.5768195311320152, - "velocityY": 1.2179567849204198, - "timestamp": 2.9836358837188177 - }, - { - "x": 6.078984969036473, - "y": 6.498549743391548, - "heading": 0.1231057748040032, - "angularVelocity": 1.9227414178241924e-7, - "velocityX": -3.5882852707972814, - "velocityY": 1.194736557475109, - "timestamp": 3.045864853646763 - }, - { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": 0.12310578689683861, - "angularVelocity": 1.9432806685756468e-7, - "velocityX": -3.662936913327793, - "velocityY": 0.941318096340742, - "timestamp": 3.108093823574708 - }, - { - "x": 5.606427257807288, - "y": 6.600564902613298, - "heading": 0.1231057972666647, - "angularVelocity": 1.578555424299444e-7, - "velocityX": -3.7237014740499577, - "velocityY": 0.6612371129043467, - "timestamp": 3.173785695467891 - }, - { - "x": 5.35931577124569, - "y": 6.62625930787543, - "heading": 0.12310580746861746, - "angularVelocity": 1.5530007712924032e-7, - "velocityX": -3.7616752185629396, - "velocityY": 0.39113522756534835, - "timestamp": 3.2394775673610736 - }, - { - "x": 5.111994168115865, - "y": 6.649845615003531, - "heading": 0.1231058176602883, - "angularVelocity": 1.551435596748186e-7, - "velocityX": -3.764873735550995, - "velocityY": 0.35904452785959023, - "timestamp": 3.3051694392542563 - }, - { - "x": 4.868859374012009, - "y": 6.67294055859981, - "heading": 0.13716866748145323, - "angularVelocity": 0.21407290454489739, - "velocityX": -3.701139685883833, - "velocityY": 0.35156470550620644, - "timestamp": 3.370861311147439 - }, - { - "x": 4.644141674488456, - "y": 6.6943246178055915, - "heading": 0.16550217221046287, - "angularVelocity": 0.43130913935716303, - "velocityX": -3.4207839272559015, - "velocityY": 0.3255206251475534, - "timestamp": 3.4365531830406217 - }, - { - "x": 4.438149971299598, - "y": 6.713938607045374, - "heading": 0.1971398735951766, - "angularVelocity": 0.48160754858923427, - "velocityX": -3.1357258859027066, - "velocityY": 0.2985755874284469, - "timestamp": 3.5022450549338044 - }, - { - "x": 4.2509016025753406, - "y": 6.731775563046276, - "heading": 0.22901708759854217, - "angularVelocity": 0.48525354940713283, - "velocityX": -2.8504039134206605, - "velocityY": 0.27152455070706877, - "timestamp": 3.567936926826987 - }, - { - "x": 4.082393919350406, - "y": 6.747832566507683, - "heading": 0.25967107343501483, - "angularVelocity": 0.46663285659323517, - "velocityX": -2.565122265033534, - "velocityY": 0.24442907468850525, - "timestamp": 3.63362879872017 - }, - { - "x": 3.9326220842038664, - "y": 6.762107971713798, - "heading": 0.28824128245235414, - "angularVelocity": 0.43491239013245725, - "velocityX": -2.279914254690053, - "velocityY": 0.21730854662395976, - "timestamp": 3.6993206706133526 - }, - { - "x": 3.801581586235111, - "y": 6.77460070477556, - "heading": 0.314159509734349, - "angularVelocity": 0.3945423769342238, - "velocityX": -1.9947749119073828, - "velocityY": 0.19017167119358822, - "timestamp": 3.7650125425065353 - }, - { - "x": 3.6892685916749244, - "y": 6.785310000165767, - "heading": 0.3370219943875935, - "angularVelocity": 0.34802607985383205, - "velocityX": -1.7096939289964497, - "velocityY": 0.16302314246152652, - "timestamp": 3.830704414399718 - }, - { - "x": 3.595679898494151, - "y": 6.794235280663889, - "heading": 0.35652698961990326, - "angularVelocity": 0.2969164170572818, - "velocityX": -1.4246616892414212, - "velocityY": 0.135865826941808, - "timestamp": 3.8963962862929007 - }, - { - "x": 3.520812828526041, - "y": 6.801376095406927, - "heading": 0.37244068726433127, - "angularVelocity": 0.2422475899347831, - "velocityX": -1.1396702181031793, - "velocityY": 0.10870164812244498, - "timestamp": 3.9620881581860834 - }, - { - "x": 3.464665127058438, - "y": 6.806732084957018, - "heading": 0.38457706026543664, - "angularVelocity": 0.18474695044220302, - "velocityX": -0.8547130695088268, - "velocityY": 0.08153199773025213, - "timestamp": 4.027780030079266 - }, - { - "x": 3.4272348818352074, - "y": 6.810302960282004, - "heading": 0.39278518447772715, - "angularVelocity": 0.12494885555456782, - "velocityX": -0.5697850303930024, - "velocityY": 0.05435794752192765, - "timestamp": 4.093471901972449 - }, - { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0.0632601721112495, - "velocityX": -0.28488184560874424, - "velocityY": 0.027180367966521624, - "timestamp": 4.1591637738656315 - }, - { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": 1.9034366224257262e-34, - "velocityY": -2.547324229323533e-33, - "timestamp": 4.224855645758814 - } - ] + "samples": [ + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 4.764169197941492e-24, + "velocityX": 1.177617036966837e-23, + "velocityY": -7.781360494191974e-22, + "timestamp": 0 + }, + { + "x": 3.4251270675193637, + "y": 6.810469355892218, + "heading": 0.38870394635032085, + "angularVelocity": -0.13270177736033364, + "velocityX": 0.26754260724678264, + "velocityY": -0.02608523375098152, + "timestamp": 0.06207088867629107 + }, + { + "x": 3.458345061227577, + "y": 6.807230578787204, + "heading": 0.3723948211461713, + "angularVelocity": -0.26274998718326975, + "velocityX": 0.5351622059328033, + "velocityY": -0.05217868108681347, + "timestamp": 0.12414177735258214 + }, + { + "x": 3.508179991445113, + "y": 6.802371624572939, + "heading": 0.3482095805490521, + "angularVelocity": -0.38963902584428683, + "velocityX": 0.8028712216032942, + "velocityY": -0.07828072576186927, + "timestamp": 0.1862126660288732 + }, + { + "x": 3.5746383517078186, + "y": 6.795891856003428, + "heading": 0.3163840274624053, + "angularVelocity": -0.5127291354345188, + "velocityX": 1.070684852109927, + "velocityY": -0.10439303685990675, + "timestamp": 0.24828355470516428 + }, + { + "x": 3.6577278069428134, + "y": 6.787790439971061, + "heading": 0.2772056832992266, + "angularVelocity": -0.63118709911662, + "velocityX": 1.3386219692828794, + "velocityY": -0.1305187698313394, + "timestamp": 0.31035444338145535 + }, + { + "x": 3.7574574943964785, + "y": 6.778066242632264, + "heading": 0.23103157299019372, + "angularVelocity": -0.7438931726890192, + "velocityX": 1.6067062930864489, + "velocityY": -0.15666276971655022, + "timestamp": 0.3724253320577464 + }, + { + "x": 3.8738384093946294, + "y": 6.766717706731516, + "heading": 0.1783152332262761, + "angularVelocity": -0.8492924926343718, + "velocityX": 1.8749677583173519, + "velocityY": -0.18283185794122117, + "timestamp": 0.4344962207340375 + }, + { + "x": 4.00688386795334, + "y": 6.753742690540388, + "heading": 0.11964925208533374, + "angularVelocity": -0.9451448560192912, + "velocityX": 2.1434437527157497, + "velocityY": -0.2090354507214015, + "timestamp": 0.49656710941032856 + }, + { + "x": 4.156609966611215, + "y": 6.739138213215621, + "heading": 0.05583642242794389, + "angularVelocity": -1.028063735162284, + "velocityX": 2.412179072201139, + "velocityY": -0.2352870667106592, + "timestamp": 0.5586379980866196 + }, + { + "x": 4.323035675773869, + "y": 6.722899990969598, + "heading": -0.011979486180355738, + "angularVelocity": -1.09255578669044, + "velocityX": 2.681220016529636, + "velocityY": -0.26160769713976106, + "timestamp": 0.6207088867629107 + }, + { + "x": 4.506180877109483, + "y": 6.705021607935757, + "heading": -0.08203737634019735, + "angularVelocity": -1.1286754814354962, + "velocityX": 2.950581266698877, + "velocityY": -0.28803169110530014, + "timestamp": 0.6827797754392018 + }, + { + "x": 4.706052676090064, + "y": 6.685493696321985, + "heading": -0.1512367442368059, + "angularVelocity": -1.1148441624139431, + "velocityX": 3.220056990370196, + "velocityY": -0.3146066059342914, + "timestamp": 0.7448506641154928 + }, + { + "x": 4.922530525854263, + "y": 6.664309314644424, + "heading": -0.21234440720432737, + "angularVelocity": -0.9844818443990274, + "velocityX": 3.4875906303382327, + "velocityY": -0.3412933523159342, + "timestamp": 0.8069215527917839 + }, + { + "x": 5.152231029581242, + "y": 6.641295647238388, + "heading": -0.22626463143704686, + "angularVelocity": -0.22426333067850218, + "velocityX": 3.700615677099489, + "velocityY": -0.37076426480787344, + "timestamp": 0.868992441468075 + }, + { + "x": 5.385889088159938, + "y": 6.618686799272017, + "heading": -0.22626465694222986, + "angularVelocity": -4.1090410586524524e-7, + "velocityX": 3.764374307531794, + "velocityY": -0.36424237591119274, + "timestamp": 0.931063330144366 + }, + { + "x": 5.619544792113157, + "y": 6.5960536299134915, + "heading": -0.22626468244744516, + "angularVelocity": -4.109046256964509e-7, + "velocityX": 3.764336373074485, + "velocityY": -0.3646342084219269, + "timestamp": 0.9931342188206571 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.22626470814139457, + "angularVelocity": -4.139452486047814e-7, + "velocityX": 3.7295967696415837, + "velocityY": -0.6271318462207411, + "timestamp": 1.0552051074969482 + }, + { + "x": 5.959381121610303, + "y": 6.535130978016939, + "heading": -0.22626473128429372, + "angularVelocity": -7.917457113083445e-7, + "velocityX": 3.70633385065427, + "velocityY": -0.7525096617429247, + "timestamp": 1.0844353247322749 + }, + { + "x": 6.066915021909061, + "y": 6.5094954829249705, + "heading": -0.22626475422294146, + "angularVelocity": -7.84758031451317e-7, + "velocityX": 3.678860797818322, + "velocityY": -0.8770203411621033, + "timestamp": 1.1136655419676016 + }, + { + "x": 6.173524224764707, + "y": 6.480251745020554, + "heading": -0.22626477706662856, + "angularVelocity": -7.815093169473892e-7, + "velocityX": 3.6472258142098704, + "velocityY": -1.0004625579406388, + "timestamp": 1.1428957592029283 + }, + { + "x": 6.279732582783464, + "y": 6.449584123171384, + "heading": -0.22626479989606985, + "angularVelocity": -7.810219505026809e-7, + "velocityX": 3.6335124424048417, + "velocityY": -1.0491752969972816, + "timestamp": 1.172125976438255 + }, + { + "x": 6.385940150925491, + "y": 6.4189137659468605, + "heading": -0.22626482272551648, + "angularVelocity": -7.810221338422918e-7, + "velocityX": 3.633485419795893, + "velocityY": -1.0492688773950392, + "timestamp": 1.2013561936735817 + }, + { + "x": 6.491550017633834, + "y": 6.3862446041508125, + "heading": -0.22626484978268144, + "angularVelocity": -9.256573336962615e-7, + "velocityX": 3.613037353027402, + "velocityY": -1.1176503251082635, + "timestamp": 1.2305864109089084 + }, + { + "x": 6.594250357432935, + "y": 6.352942011609429, + "heading": -0.23580774999861734, + "angularVelocity": -0.32647380411537286, + "velocityX": 3.513499026445183, + "velocityY": -1.1393207335159796, + "timestamp": 1.2598166281442351 + }, + { + "x": 6.693663742086507, + "y": 6.320404663734079, + "heading": -0.25220771678314496, + "angularVelocity": -0.5610620903873084, + "velocityX": 3.4010484374171166, + "velocityY": -1.1131408163475485, + "timestamp": 1.2890468453795618 + }, + { + "x": 6.789481492756262, + "y": 6.2888206325426586, + "heading": -0.2695690677329979, + "angularVelocity": -0.5939521697728065, + "velocityX": 3.278037583448113, + "velocityY": -1.0805267349587082, + "timestamp": 1.3182770626148885 + }, + { + "x": 6.881704495384483, + "y": 6.2582213842117005, + "heading": -0.2867065303563255, + "angularVelocity": -0.5862926876443351, + "velocityX": 3.155057038603278, + "velocityY": -1.0468361587808552, + "timestamp": 1.3475072798502152 + }, + { + "x": 6.970340753224835, + "y": 6.228619131395559, + "heading": -0.3030805193415338, + "angularVelocity": -0.5601733594172232, + "velocityX": 3.0323502944489906, + "velocityY": -1.0127277733797784, + "timestamp": 1.376737497085542 + }, + { + "x": 7.055397018445757, + "y": 6.200020205130934, + "heading": -0.3183784421956162, + "angularVelocity": -0.5233598755329688, + "velocityX": 2.9098745498930065, + "velocityY": -0.9784027957911439, + "timestamp": 1.4059677143208686 + }, + { + "x": 7.1368785342038406, + "y": 6.172428433477075, + "heading": -0.33239562176130555, + "angularVelocity": -0.4795441461430052, + "velocityX": 2.787578179870906, + "velocityY": -0.9439468558075184, + "timestamp": 1.4351979315561953 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.4307821837626389, + "velocityX": 2.6654217357633208, + "velocityY": -0.9094036619962027, + "timestamp": 1.464428148791522 + }, + { + "x": 7.370581924170205, + "y": 6.091672602019875, + "heading": -0.3654249911056099, + "angularVelocity": -0.3140115764707324, + "velocityX": 2.3936699245523703, + "velocityY": -0.8323512600338246, + "timestamp": 1.5295133687745466 + }, + { + "x": 7.508760881953895, + "y": 6.042869206500167, + "heading": -0.379545281879092, + "angularVelocity": -0.21695080353365873, + "velocityX": 2.123046642843499, + "velocityY": -0.7498383739417093, + "timestamp": 1.594598588757571 + }, + { + "x": 7.629377621865933, + "y": 5.999653284534207, + "heading": -0.38812959068868996, + "angularVelocity": -0.13189336706301324, + "velocityX": 1.8532124489015709, + "velocityY": -0.6639897964119993, + "timestamp": 1.6596838087405956 + }, + { + "x": 7.7324695426404055, + "y": 5.962171118621702, + "heading": -0.3917039945024171, + "angularVelocity": -0.05491882511358906, + "velocityX": 1.5839528667393499, + "velocityY": -0.5758936656015066, + "timestamp": 1.7247690287236193 + }, + { + "x": 7.818064946675874, + "y": 5.930527938551499, + "heading": -0.39064709699012523, + "angularVelocity": 0.016238671584232672, + "velocityX": 1.315128135969949, + "velocityY": -0.48618073471145673, + "timestamp": 1.789854248706643 + }, + { + "x": 7.886185947266274, + "y": 5.90480307138058, + "heading": -0.38524455647534983, + "angularVelocity": 0.08300717914427408, + "velocityX": 1.0466431642724277, + "velocityY": -0.39524898552433385, + "timestamp": 1.8549394686896665 + }, + { + "x": 7.936850278188107, + "y": 5.885058458931867, + "heading": -0.37571963101716943, + "angularVelocity": 0.14634544464418678, + "velocityX": 0.778430662676533, + "velocityY": -0.3033655329714407, + "timestamp": 1.9200246886726902 + }, + { + "x": 7.970072469166475, + "y": 5.871343812268201, + "heading": -0.3622515950174618, + "angularVelocity": 0.20692925372642912, + "velocityX": 0.5104414026261703, + "velocityY": -0.21071829621597563, + "timestamp": 1.9851099086557138 + }, + { + "x": 7.985864639282227, + "y": 5.863699913024902, + "heading": -0.344987478573796, + "angularVelocity": 0.2652540230818704, + "velocityX": 0.2426383458467277, + "velocityY": -0.11744447119165673, + "timestamp": 2.0501951286387374 + }, + { + "x": 7.985039462235232, + "y": 5.861972195893162, + "heading": -0.32512296531969853, + "angularVelocity": 0.31921787404219815, + "velocityX": -0.013260393510813807, + "velocityY": -0.027763992134404936, + "timestamp": 2.1124238284372687 + }, + { + "x": 7.968288815736249, + "y": 5.865825907000098, + "heading": -0.30201458190342884, + "angularVelocity": 0.3713460748992679, + "velocityX": -0.2691787961698452, + "velocityY": 0.06192819582304089, + "timestamp": 2.1746525282358 + }, + { + "x": 7.9356113385118245, + "y": 5.8752618544788175, + "heading": -0.2757940495370948, + "angularVelocity": 0.421357548064224, + "velocityX": -0.5251190741606199, + "velocityY": 0.15163337028201795, + "timestamp": 2.2368812280343313 + }, + { + "x": 7.88700551038904, + "y": 5.890280935303068, + "heading": -0.2466150135455457, + "angularVelocity": 0.46889997840252956, + "velocityX": -0.781083780958754, + "velocityY": 0.24135295888994696, + "timestamp": 2.2991099278328626 + }, + { + "x": 7.8224696292437494, + "y": 5.910884146922841, + "heading": -0.2146591990965777, + "angularVelocity": 0.5135221296994205, + "velocityX": -1.0370758404759837, + "velocityY": 0.3310885762755298, + "timestamp": 2.361338627631394 + }, + { + "x": 7.742001788904935, + "y": 5.9370726017699464, + "heading": -0.18014519573995202, + "angularVelocity": 0.5546316003446364, + "velocityX": -1.2930985316956969, + "velocityY": 0.4208420701684547, + "timestamp": 2.423567327429925 + }, + { + "x": 7.6455998656570765, + "y": 5.9688475450465655, + "heading": -0.14334139980997887, + "angularVelocity": 0.5914280074809063, + "velocityX": -1.5491553505048972, + "velocityY": 0.5106155741561712, + "timestamp": 2.4857960272284565 + }, + { + "x": 7.533261533346939, + "y": 6.006210375220679, + "heading": -0.10458582814826359, + "angularVelocity": 0.6227925665679687, + "velocityX": -1.8052495500281769, + "velocityY": 0.6004115511825956, + "timestamp": 2.548024727026988 + }, + { + "x": 7.404984359973908, + "y": 6.049162662823707, + "heading": -0.06431792685231795, + "angularVelocity": 0.6470953342479389, + "velocityX": -2.0613828312070357, + "velocityY": 0.6902327662652126, + "timestamp": 2.610253426825519 + }, + { + "x": 7.260766133037973, + "y": 6.097706149350396, + "heading": -0.023132839886022792, + "angularVelocity": 0.6618342838534901, + "velocityX": -2.317551666720475, + "velocityY": 0.7800819667428388, + "timestamp": 2.6724821266240504 + }, + { + "x": 7.100605864047409, + "y": 6.151842655747804, + "heading": 0.018118188112097987, + "angularVelocity": 0.6628939401220464, + "velocityX": -2.573736387054354, + "velocityY": 0.8699604293947623, + "timestamp": 2.7347108264225817 + }, + { + "x": 6.924507066086204, + "y": 6.21157360061122, + "heading": 0.058119739083768836, + "angularVelocity": 0.6428151496203137, + "velocityX": -2.829864652986999, + "velocityY": 0.9598616885263167, + "timestamp": 2.796939526221113 + }, + { + "x": 6.73249038975253, + "y": 6.276897504068343, + "heading": 0.09458083823039053, + "angularVelocity": 0.585920953911396, + "velocityX": -3.0856610688530672, + "velocityY": 1.0497391664716116, + "timestamp": 2.8591682260196443 + }, + { + "x": 6.524662008432933, + "y": 6.347790578963825, + "heading": 0.12255603947008778, + "angularVelocity": 0.449554648100767, + "velocityX": -3.3397513043410987, + "velocityY": 1.1392343906429359, + "timestamp": 2.9213969258181756 + }, + { + "x": 6.302055285584133, + "y": 6.423504678750072, + "heading": 0.12321029314887402, + "angularVelocity": 0.01051369674932009, + "velocityX": -3.5772356415849678, + "velocityY": 1.2167070826061643, + "timestamp": 2.983625625616707 + }, + { + "x": 6.078948914811354, + "y": 6.498413724394235, + "heading": 0.1232103051115219, + "angularVelocity": 1.9223682817827964e-7, + "velocityX": -3.585264861632913, + "velocityY": 1.2037700592601788, + "timestamp": 3.045854325415238 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0.12321031719433864, + "angularVelocity": 1.9416791274703824e-7, + "velocityX": -3.6623734312330534, + "velocityY": 0.9435079745715298, + "timestamp": 3.1080830252137694 + }, + { + "x": 5.606452972493988, + "y": 6.600711194819738, + "heading": 0.12321032755676568, + "angularVelocity": 1.577427164908284e-7, + "velocityX": -3.7233054627128834, + "velocityY": 0.6634632443513382, + "timestamp": 3.17377497769108 + }, + { + "x": 5.35926430365433, + "y": 6.6256551734400135, + "heading": 0.12321033775139768, + "angularVelocity": 1.551884456824965e-7, + "velocityX": -3.7628455163520536, + "velocityY": 0.3797113296166925, + "timestamp": 3.2394669301683905 + }, + { + "x": 5.111950804404792, + "y": 6.649329487896052, + "heading": 0.12321034793997507, + "angularVelocity": 1.5509627898966467e-7, + "velocityX": -3.7647457553489407, + "velocityY": 0.3603837846685449, + "timestamp": 3.305158882645701 + }, + { + "x": 4.868821995322067, + "y": 6.672498234290298, + "heading": 0.1372675563656116, + "angularVelocity": 0.2139867654335871, + "velocityX": -3.7010440383348504, + "velocityY": 0.35268774211343507, + "timestamp": 3.3708508351230115 + }, + { + "x": 4.644110082814918, + "y": 6.693950400788965, + "heading": 0.16558913990545626, + "angularVelocity": 0.4311271391975579, + "velocityX": -3.420691637758275, + "velocityY": 0.3265569935080811, + "timestamp": 3.436542787600322 + }, + { + "x": 4.438123681636039, + "y": 6.713626799516342, + "heading": 0.19721421356331742, + "angularVelocity": 0.48141473141302027, + "velocityX": -3.13564132912667, + "velocityY": 0.29952525363244636, + "timestamp": 3.5022347400776326 + }, + { + "x": 4.250880120713019, + "y": 6.73152047543666, + "heading": 0.22907908319149847, + "angularVelocity": 0.4850650410974934, + "velocityX": -2.8503272297728453, + "velocityY": 0.27238764027449763, + "timestamp": 3.567926692554943 + }, + { + "x": 4.082376753854165, + "y": 6.74762851546552, + "heading": 0.25972143161316247, + "angularVelocity": 0.466455132875641, + "velocityX": -2.565053412243361, + "velocityY": 0.24520568230062648, + "timestamp": 3.6336186450322536 + }, + { + "x": 3.9326087471810762, + "y": 6.761949278304569, + "heading": 0.28828094811629446, + "angularVelocity": 0.4347490891368528, + "velocityX": -2.279853178740861, + "velocityY": 0.21799873955634386, + "timestamp": 3.699310597509564 + }, + { + "x": 3.8015715928550198, + "y": 6.774481693316805, + "heading": 0.31418958063772184, + "angularVelocity": 0.39439583608625584, + "velocityX": -1.9947215661053177, + "velocityY": 0.190775498970961, + "timestamp": 3.7650025499868747 + }, + { + "x": 3.6892614596122306, + "y": 6.785224997460466, + "heading": 0.33704367479862785, + "angularVelocity": 0.3478979281183608, + "velocityX": -1.709648275130517, + "velocityY": 0.16354064293296997, + "timestamp": 3.830694502464185 + }, + { + "x": 3.595675147466301, + "y": 6.794178615472137, + "heading": 0.3565415622292007, + "angularVelocity": 0.29680785385861885, + "velocityX": -1.4246236961559957, + "velocityY": 0.1362970299103891, + "timestamp": 3.8963864549414957 + }, + { + "x": 3.5208099799304895, + "y": 6.801342098064149, + "heading": 0.37244949483993084, + "angularVelocity": 0.24215953417162805, + "velocityX": -1.139639860174189, + "velocityY": 0.10904657757715387, + "timestamp": 3.9620784074188062 + }, + { + "x": 3.4646637036902503, + "y": 6.806715087085814, + "heading": 0.3845814929922257, + "angularVelocity": 0.18468012739437525, + "velocityX": -0.8546903254189608, + "velocityY": 0.08179067327191092, + "timestamp": 4.027770359896117 + }, + { + "x": 3.427234407665484, + "y": 6.81029729456599, + "heading": 0.39278667084002433, + "angularVelocity": 0.12490385105592111, + "velocityX": -0.569769882204283, + "velocityY": 0.054530385307290076, + "timestamp": 4.093462312373427 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.06323746826387204, + "velocityX": -0.2848742780656992, + "velocityY": 0.02726658135331313, + "timestamp": 4.159154264850738 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 2.0447435740886937e-25, + "velocityX": -6.220702573449239e-25, + "velocityY": -1.1679895694126021e-23, + "timestamp": 4.224846217328048 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.5.traj b/src/main/deploy/choreo/AmpLanePADESprint.5.traj index 4848618c..c4de1041 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.5.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.5.traj @@ -1,193 +1,194 @@ { - "samples": [ - { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": 1.9034366224257262e-34, - "velocityY": -2.547324229323533e-33, - "timestamp": 0 - }, - { - "x": 3.4428034429775143, - "y": 6.811427919070607, - "heading": 0.39249813327278055, - "angularVelocity": -0.050100574262489476, - "velocityX": 0.38660845958378076, - "velocityY": -0.007449238878501296, - "timestamp": 0.08867623560446347 - }, - { - "x": 3.5113694006298597, - "y": 6.810106800279428, - "heading": 0.3836131655700934, - "angularVelocity": -0.10019558951868686, - "velocityX": 0.7732168284429818, - "velocityY": -0.014898228168725063, - "timestamp": 0.17735247120892694 - }, - { - "x": 3.614218332551768, - "y": 6.808125167746075, - "heading": 0.37028807906085554, - "angularVelocity": -0.1502667137188131, - "velocityX": 1.1598251912796755, - "velocityY": -0.022346827420505266, - "timestamp": 0.2660287068133904 - }, - { - "x": 3.7513502451988723, - "y": 6.8054830693306245, - "heading": 0.3525264976102127, - "angularVelocity": -0.20029697166969943, - "velocityX": 1.5464336269164098, - "velocityY": -0.02979488695523812, - "timestamp": 0.3547049424178539 - }, - { - "x": 3.9227651512282486, - "y": 6.802180567172425, - "heading": 0.3303333796886753, - "angularVelocity": -0.2502713130553812, - "velocityX": 1.9330422052867193, - "velocityY": -0.03724224574585722, - "timestamp": 0.44338117802231736 - }, - { - "x": 4.128463068361194, - "y": 6.79821773882544, - "heading": 0.3037147939289921, - "angularVelocity": -0.3001772186001935, - "velocityX": 2.319650983499712, - "velocityY": -0.04468872996211646, - "timestamp": 0.5320574136267808 - }, - { - "x": 4.368444017678072, - "y": 6.793594678448866, - "heading": 0.2726776385751228, - "angularVelocity": -0.3500053327963678, - "velocityX": 2.706259999435507, - "velocityY": -0.05213415234714548, - "timestamp": 0.6207336492312443 - }, - { - "x": 4.642708020512335, - "y": 6.788311497970363, - "heading": 0.23722930806330966, - "angularVelocity": -0.3997500600942169, - "velocityX": 3.092869255948188, - "velocityY": -0.05957831252634942, - "timestamp": 0.7094098848357078 - }, - { - "x": 4.951255088173746, - "y": 6.782368328182758, - "heading": 0.19737735072702745, - "angularVelocity": -0.44940966499796203, - "velocityX": 3.479478640000833, - "velocityY": -0.0670209977576627, - "timestamp": 0.7980861204401712 - }, - { - "x": 5.286562692788269, - "y": 6.775920770205836, - "heading": 0.19737734601955828, - "angularVelocity": -5.3086028753514235e-8, - "velocityX": 3.781256639153567, - "velocityY": -0.07270897251075666, - "timestamp": 0.8867623560446347 - }, - { - "x": 5.595111114675473, - "y": 6.769985794431735, - "heading": 0.15789301075905496, - "angularVelocity": -0.4452639987631135, - "velocityX": 3.4794939115759345, - "velocityY": -0.06692859404378032, - "timestamp": 0.9754385916490982 - }, - { - "x": 5.8693764275565705, - "y": 6.76471055259859, - "heading": 0.12280143262174693, - "angularVelocity": -0.3957269712466434, - "velocityX": 3.092884029318143, - "velocityY": -0.059488788593539996, - "timestamp": 1.0641148272535617 - }, - { - "x": 6.109358599544783, - "y": 6.760094936608861, - "heading": 0.09209943623091729, - "angularVelocity": -0.3462257523850543, - "velocityX": 2.7062737874738274, - "velocityY": -0.05205020215694004, - "timestamp": 1.1527910628580251 - }, - { - "x": 6.315057615509001, - "y": 6.756138852616793, - "heading": 0.06578495581515914, - "angularVelocity": -0.29674782918315123, - "velocityX": 2.3196633749963063, - "velocityY": -0.04461267401690494, - "timestamp": 1.2414672984624886 - }, - { - "x": 6.4864734670806214, - "y": 6.752842221795132, - "heading": 0.043856783005957245, - "angularVelocity": -0.24728353272698234, - "velocityX": 1.9330528681462538, - "velocityY": -0.037176034810112814, - "timestamp": 1.330143534066952 - }, - { - "x": 6.623606149810205, - "y": 6.750204980767773, - "heading": 0.026314311963007715, - "angularVelocity": -0.19782606831887856, - "velocityX": 1.5464423111199512, - "velocityY": -0.029740110294281013, - "timestamp": 1.4188197696714155 - }, - { - "x": 6.726455661694478, - "y": 6.74822708185799, - "heading": 0.013157327563715553, - "angularVelocity": -0.14837102984364775, - "velocityX": 1.1598317315027789, - "velocityY": -0.022304723427886322, - "timestamp": 1.507496005275879 - }, - { - "x": 6.795022002255089, - "y": 6.7469084932218335, - "heading": 0.004385844550104781, - "angularVelocity": -0.09891582512292933, - "velocityX": 0.7732211464912397, - "velocityY": -0.014869695664985829, - "timestamp": 1.5961722408803425 - }, - { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": -4.086871260037018e-38, - "angularVelocity": -0.04945907457853377, - "velocityX": 0.3866105668308079, - "velocityY": -0.007434847721776456, - "timestamp": 1.684848476484806 - }, - { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": -5.480350490542979e-38, - "angularVelocity": -1.571392014077605e-37, - "velocityX": 4.425248303324229e-37, - "velocityY": 1.8999126670467387e-37, - "timestamp": 1.7735247120892694 - } - ] + "samples": [ + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 2.0447435740886937e-25, + "velocityX": -6.220702573449239e-25, + "velocityY": -1.1679895694126021e-23, + "timestamp": 0 + }, + { + "x": 3.4428034429755563, + "y": 6.811427919071949, + "heading": 0.392498131917024, + "angularVelocity": -0.05010058941962764, + "velocityX": 0.38660845854541764, + "velocityY": -0.007449238843783358, + "timestamp": 0.08867623583756767 + }, + { + "x": 3.511369400623633, + "y": 6.8101068002836795, + "heading": 0.38361316145922575, + "angularVelocity": -0.1001956203246293, + "velocityX": 0.7732168263622722, + "velocityY": -0.014898228096758812, + "timestamp": 0.17735247167513535 + }, + { + "x": 3.614218332538516, + "y": 6.808125167755098, + "heading": 0.3702880707403863, + "angularVelocity": -0.15026676079539045, + "velocityX": 1.159825188151597, + "velocityY": -0.022346827307951896, + "timestamp": 0.266028707512703 + }, + { + "x": 3.7513502451752516, + "y": 6.805483069346682, + "heading": 0.35252648355271166, + "angularVelocity": -0.20029703583955935, + "velocityX": 1.5464336227343458, + "velocityY": -0.029794886797588863, + "timestamp": 0.3547049433502707 + }, + { + "x": 3.9227651511900996, + "y": 6.8021805671983415, + "heading": 0.3303333582647084, + "angularVelocity": -0.2502713954689655, + "velocityX": 1.9330422000414584, + "velocityY": -0.03724224553678828, + "timestamp": 0.44338117918783837 + }, + { + "x": 4.128463068303133, + "y": 6.7982177388648735, + "heading": 0.3037147633556069, + "angularVelocity": -0.3001773209889048, + "velocityX": 2.319650977177454, + "velocityY": -0.044688729692209925, + "timestamp": 0.532057415025406 + }, + { + "x": 4.3684440175926715, + "y": 6.793594678506847, + "heading": 0.27267759681211456, + "angularVelocity": -0.3500054580614399, + "velocityX": 2.7062599920132113, + "velocityY": -0.05213415200092422, + "timestamp": 0.6207336508629737 + }, + { + "x": 4.642708020388071, + "y": 6.788311498054621, + "heading": 0.2372292525531904, + "angularVelocity": -0.3997502140692617, + "velocityX": 3.092869247379649, + "velocityY": -0.059578312073416795, + "timestamp": 0.7094098867005414 + }, + { + "x": 4.9512550879867705, + "y": 6.7823683283089595, + "heading": 0.19737727735435437, + "angularVelocity": -0.44940986525222537, + "velocityX": 3.47947863014707, + "velocityY": -0.0670209971084869, + "timestamp": 0.7980861225381091 + }, + { + "x": 5.286562691805109, + "y": 6.775920770291965, + "heading": 0.19737727264687205, + "angularVelocity": -5.308617659691649e-8, + "velocityX": 3.781256620235165, + "velocityY": -0.07270897277153024, + "timestamp": 0.8867623583756767 + }, + { + "x": 5.59511111399723, + "y": 6.769985794491263, + "heading": 0.15789296561858565, + "angularVelocity": -0.44526367921853854, + "velocityX": 3.479493905867875, + "velocityY": -0.06692859416780687, + "timestamp": 0.9754385942132444 + }, + { + "x": 5.869376427077159, + "y": 6.764710552640504, + "heading": 0.12280140353680047, + "angularVelocity": -0.3957267891486041, + "velocityX": 3.0928840234300474, + "velocityY": -0.05948878863580233, + "timestamp": 1.064114830050812 + }, + { + "x": 6.109358599210802, + "y": 6.760094936637857, + "heading": 0.0920994176200499, + "angularVelocity": -0.3462256333589605, + "velocityX": 2.706273781999814, + "velocityY": -0.05205020216578816, + "timestamp": 1.1527910658883798 + }, + { + "x": 6.3150576152845925, + "y": 6.7561388526360995, + "heading": 0.06578494429363466, + "angularVelocity": -0.2967477484567179, + "velocityX": 2.3196633701342284, + "velocityY": -0.04461267400889573, + "timestamp": 1.2414673017259474 + }, + { + "x": 6.486473466938695, + "y": 6.752842221807213, + "heading": 0.04385677628651476, + "angularVelocity": -0.24728347792396857, + "velocityX": 1.9330528639949482, + "velocityY": -0.037176034793867205, + "timestamp": 1.330143537563515 + }, + { + "x": 6.623606149728922, + "y": 6.750204980774611, + "heading": 0.02631430841636879, + "angularVelocity": -0.19782603201921237, + "velocityX": 1.546442307738673, + "velocityY": -0.029740110275233655, + "timestamp": 1.4188197734010828 + }, + { + "x": 6.7264556616555025, + "y": 6.748227081861227, + "heading": 0.013157325998979796, + "angularVelocity": -0.14837100710374293, + "velocityX": 1.1598317289310056, + "velocityY": -0.022304723409858212, + "timestamp": 1.5074960092386505 + }, + { + "x": 6.7950220022425825, + "y": 6.746908493222859, + "heading": 0.004385844089357537, + "angularVelocity": -0.09891581241325288, + "velocityX": 0.7732211447571573, + "velocityY": -0.01486969565085219, + "timestamp": 1.5961722450762181 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -2.582999463513829e-28, + "angularVelocity": -0.04945906925268308, + "velocityX": 0.3866105659555495, + "velocityY": -0.007434847713796995, + "timestamp": 1.6848484809137858 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -1.1865093331139584e-28, + "angularVelocity": 5.378776164649684e-29, + "velocityX": -2.235889133711042e-27, + "velocityY": -8.75203817649461e-27, + "timestamp": 1.7735247167513535 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADESprint.traj b/src/main/deploy/choreo/AmpLanePADESprint.traj index c05a0903..96b62754 100644 --- a/src/main/deploy/choreo/AmpLanePADESprint.traj +++ b/src/main/deploy/choreo/AmpLanePADESprint.traj @@ -1,1579 +1,1580 @@ { - "samples": [ - { - "x": 0.43297290802001953, - "y": 6.9807281494140625, - "heading": 0, - "angularVelocity": 2.735053895521923e-38, - "velocityX": -3.8673469053295564e-39, - "velocityY": 1.858643532249755e-38, - "timestamp": 0 - }, - { - "x": 0.46193105617219093, - "y": 6.976128411049919, - "heading": 0.007924686694784022, - "angularVelocity": 0.0965568963773332, - "velocityX": 0.35283526252830383, - "velocityY": -0.05604467125266167, - "timestamp": 0.08207271559159549 - }, - { - "x": 0.5198473628641517, - "y": 6.966928755942856, - "heading": 0.023773564700110772, - "angularVelocity": 0.19310775683593598, - "velocityX": 0.7056706516226394, - "velocityY": -0.11209151592888836, - "timestamp": 0.16414543118319097 - }, - { - "x": 0.6067218129336875, - "y": 6.9531289220308485, - "heading": 0.04754806846022335, - "angularVelocity": 0.2896760950181983, - "velocityX": 1.0585058559757474, - "velocityY": -0.16814155365198188, - "timestamp": 0.24621814677478646 - }, - { - "x": 0.7225543617111448, - "y": 6.934728565336742, - "heading": 0.07925217561750111, - "angularVelocity": 0.38629289805690764, - "velocityX": 1.4113405160596262, - "velocityY": -0.22419578250180797, - "timestamp": 0.32829086236638194 - }, - { - "x": 0.8673449286095066, - "y": 6.9117272629835975, - "heading": 0.11889328943914418, - "angularVelocity": 0.48299990485147826, - "velocityX": 1.764174194245729, - "velocityY": -0.2802551638184133, - "timestamp": 0.4103635779579774 - }, - { - "x": 1.0410933872613453, - "y": 6.884124518124938, - "heading": 0.16648343863275697, - "angularVelocity": 0.5798534732349732, - "velocityX": 2.117006332731008, - "velocityY": -0.33632059887008814, - "timestamp": 0.49243629354957286 - }, - { - "x": 1.243799549589456, - "y": 6.851919767180746, - "heading": 0.22204089753091144, - "angularVelocity": 0.6769297018830912, - "velocityX": 2.4698361796239694, - "velocityY": -0.39239290075946437, - "timestamp": 0.5745090091411683 - }, - { - "x": 1.4754631289031934, - "y": 6.815112383244328, - "heading": 0.28559273385120976, - "angularVelocity": 0.7743357321882771, - "velocityX": 2.8226625333871667, - "velocityY": -0.44847284107882956, - "timestamp": 0.6565817247327638 - }, - { - "x": 1.6781593090979035, - "y": 6.782884452250807, - "heading": 0.3422497740441209, - "angularVelocity": 0.6903273491624664, - "velocityX": 2.4697145541443093, - "velocityY": -0.3926753338331338, - "timestamp": 0.7386544403243592 - }, - { - "x": 1.8518984587746443, - "y": 6.755259929387126, - "heading": 0.39086385083963315, - "angularVelocity": 0.5923293319234401, - "velocityX": 2.1168929092256374, - "velocityY": -0.33658594899118865, - "timestamp": 0.8207271559159547 - }, - { - "x": 1.9966808291576696, - "y": 6.732239139219163, - "heading": 0.43140805286474204, - "angularVelocity": 0.4940034179795163, - "velocityX": 1.7640743253029585, - "velocityY": -0.28049260953564537, - "timestamp": 0.9027998715075501 - }, - { - "x": 2.1125066103123933, - "y": 6.7138223061958735, - "heading": 0.463861007608982, - "angularVelocity": 0.39541709458878044, - "velocityX": 1.4112580571975715, - "velocityY": -0.22439653532283038, - "timestamp": 0.9848725870991456 - }, - { - "x": 2.199375929341993, - "y": 6.700009583150749, - "heading": 0.4882072246931022, - "angularVelocity": 0.29664203150374724, - "velocityX": 1.0584433377576143, - "velocityY": -0.16829859893826818, - "timestamp": 1.0669453026907412 - }, - { - "x": 2.257288845375976, - "y": 6.690801066111525, - "heading": 0.5044377142261048, - "angularVelocity": 0.19775743273523133, - "velocityX": 0.7056293387703308, - "velocityY": -0.11219949252132488, - "timestamp": 1.1490180182823366 - }, - { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": 0.0988477756026137, - "velocityX": 0.3528151893675758, - "velocityY": -0.0560997894575031, - "timestamp": 1.231090733873932 - }, - { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": 0, - "velocityX": 3.087311543739499e-36, - "velocityY": 0, - "timestamp": 1.3131634494655275 - }, - { - "x": 2.3177158256290964, - "y": 6.704097470773247, - "heading": 0.5125504196, - "angularVelocity": -9.1973743139294e-17, - "velocityX": 0.34546318060283326, - "velocityY": 0.1965022887733569, - "timestamp": 1.4042599289943862 - }, - { - "x": 2.380656782769881, - "y": 6.7398988031009575, - "heading": 0.5125504196, - "angularVelocity": -1.3336684994607708e-16, - "velocityX": 0.6909263394843395, - "velocityY": 0.3930045651914449, - "timestamp": 1.495356408523245 - }, - { - "x": 2.475068211555481, - "y": 6.793600797653198, - "heading": 0.5125504196, - "angularVelocity": 6.653839443641591e-19, - "velocityX": 1.0363894332018742, - "velocityY": 0.5895068045437317, - "timestamp": 1.5864528880521036 - }, - { - "x": 2.569479640341081, - "y": 6.847302792205439, - "heading": 0.5125504196, - "angularVelocity": 5.174999729369457e-17, - "velocityX": 1.0363894332018742, - "velocityY": 0.5895068045437317, - "timestamp": 1.6775493675809623 - }, - { - "x": 2.6324205974818655, - "y": 6.8831041245331495, - "heading": 0.5125504196, - "angularVelocity": -1.3901806543706914e-17, - "velocityX": 0.6909263394843393, - "velocityY": 0.39300456519144494, - "timestamp": 1.768645847109821 - }, - { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "angularVelocity": -1.8725484396545262e-17, - "velocityX": 0.3454631806028332, - "velocityY": 0.19650228877335693, - "timestamp": 1.8597423266386797 - }, - { - "x": 2.663891077041626, - "y": 6.901004791259766, - "heading": 0.5125504196, - "angularVelocity": 1.431796971368011e-33, - "velocityX": 3.743360269099248e-35, - "velocityY": -8.057233109044706e-35, - "timestamp": 1.9508388061675384 - }, - { - "x": 2.6835271230542426, - "y": 6.903699548014665, - "heading": 0.5052880772246282, - "angularVelocity": -0.10752621184517612, - "velocityX": 0.2907312178663514, - "velocityY": 0.039898557617057115, - "timestamp": 2.018379011023157 - }, - { - "x": 2.7228033814945674, - "y": 6.909089957964811, - "heading": 0.49095749727137794, - "angularVelocity": -0.21217850884350908, - "velocityX": 0.5815241236576919, - "velocityY": 0.0798103879262595, - "timestamp": 2.0859192158787754 - }, - { - "x": 2.7817246923505206, - "y": 6.91717710303389, - "heading": 0.46979167660622734, - "angularVelocity": -0.31338105518627096, - "velocityX": 0.8723886902906202, - "velocityY": 0.11973823719319023, - "timestamp": 2.153459420734394 - }, - { - "x": 2.860296723174767, - "y": 6.927962260865171, - "heading": 0.4420737054643263, - "angularVelocity": -0.41039216865205086, - "velocityX": 1.1633371706853932, - "velocityY": 0.15968500324119947, - "timestamp": 2.220999625590012 - }, - { - "x": 2.9585261643123166, - "y": 6.941446920815512, - "heading": 0.40815310467896726, - "angularVelocity": -0.502228278073356, - "velocityX": 1.454384708301312, - "velocityY": 0.19965382070083557, - "timestamp": 2.2885398304456306 - }, - { - "x": 3.0764209718394073, - "y": 6.9576328058659405, - "heading": 0.3684706356327604, - "angularVelocity": -0.5875384762459123, - "velocityX": 1.7455500435498434, - "velocityY": 0.23964814861057657, - "timestamp": 2.356080035301249 - }, - { - "x": 3.2139906503876703, - "y": 6.9765218958360915, - "heading": 0.323597871318007, - "angularVelocity": -0.6643859670055544, - "velocityX": 2.0368561043358877, - "velocityY": 0.27967178972185397, - "timestamp": 2.4236202401568674 - }, - { - "x": 3.3712465090237984, - "y": 6.998116434011635, - "heading": 0.27430452270447037, - "angularVelocity": -0.7298371202591344, - "velocityX": 2.3283296071176522, - "velocityY": 0.3197286449116718, - "timestamp": 2.491160445012486 - }, - { - "x": 3.5482015820439243, - "y": 7.022418860702876, - "heading": 0.22168358710657873, - "angularVelocity": -0.7791053596947181, - "velocityX": 2.619996095635254, - "velocityY": 0.3598216313260006, - "timestamp": 2.558700649868104 - }, - { - "x": 3.7448688058303015, - "y": 7.04943148246024, - "heading": 0.16741520227188003, - "angularVelocity": -0.8034974864335817, - "velocityX": 2.911854120176202, - "velocityY": 0.3999487685166161, - "timestamp": 2.6262408547237226 - }, - { - "x": 3.961249724160237, - "y": 7.079155012062387, - "heading": 0.1144423105156959, - "angularVelocity": -0.7843164211512963, - "velocityX": 3.2037350018777047, - "velocityY": 0.4400864591051642, - "timestamp": 2.693781059579341 - }, - { - "x": 4.197248092224393, - "y": 7.111577952654102, - "heading": 0.06945280731873603, - "angularVelocity": -0.6661144024234814, - "velocityX": 3.494190883321295, - "velocityY": 0.4800539273019009, - "timestamp": 2.7613212644349594 - }, - { - "x": 4.450279816565845, - "y": 7.14615999986413, - "heading": 0.06926458523360347, - "angularVelocity": -0.0027868154314146247, - "velocityX": 3.746386687490236, - "velocityY": 0.5120216511625231, - "timestamp": 2.828861469290578 - }, - { - "x": 4.703335353284152, - "y": 7.180937109425349, - "heading": 0.06926456657564138, - "angularVelocity": -2.762497114544702e-7, - "velocityX": 3.746739253445653, - "velocityY": 0.5149097435455242, - "timestamp": 2.896401674146196 - }, - { - "x": 4.956390888818978, - "y": 7.215714227598158, - "heading": 0.06926454791768001, - "angularVelocity": -2.7624970048363726e-7, - "velocityX": 3.746739235923052, - "velocityY": 0.5149098710487069, - "timestamp": 2.9639418790018146 - }, - { - "x": 5.209446424353711, - "y": 7.250491345771652, - "heading": 0.0692645292597187, - "angularVelocity": -2.7624969990656975e-7, - "velocityX": 3.7467392359216607, - "velocityY": 0.5149098710588286, - "timestamp": 3.031482083857433 - }, - { - "x": 5.462501959888314, - "y": 7.285268463946087, - "heading": 0.0692645106017574, - "angularVelocity": -2.7624969939846597e-7, - "velocityX": 3.7467392359197462, - "velocityY": 0.5149098710727624, - "timestamp": 3.0990222887130514 - }, - { - "x": 5.715557495424461, - "y": 7.320045582109288, - "heading": 0.06926449194379608, - "angularVelocity": -2.762496999067462e-7, - "velocityX": 3.746739235942603, - "velocityY": 0.5149098709064452, - "timestamp": 3.16656249356867 - }, - { - "x": 5.968613025759916, - "y": 7.354822738115272, - "heading": 0.06926447328583468, - "angularVelocity": -2.762497009102298e-7, - "velocityX": 3.7467391589411925, - "velocityY": 0.5149104312065347, - "timestamp": 3.234102698424288 - }, - { - "x": 6.221690653746151, - "y": 7.389438720345797, - "heading": 0.06926445462738323, - "angularVelocity": -2.7625695670270355e-7, - "velocityX": 3.74706633666926, - "velocityY": 0.5125240929387752, - "timestamp": 3.3016429032799066 - }, - { - "x": 6.476102895472122, - "y": 7.412263085598601, - "heading": 0.06926437909315673, - "angularVelocity": -0.0000011183594520291744, - "velocityX": 3.766826622303432, - "velocityY": 0.33793745964490024, - "timestamp": 3.369183108135525 - }, - { - "x": 6.712136074986816, - "y": 7.430641724558031, - "heading": 0.04341074490415746, - "angularVelocity": -0.38278880326565307, - "velocityX": 3.4947063015172035, - "velocityY": 0.27211405412107026, - "timestamp": 3.4367233129911434 - }, - { - "x": 6.9285315703702866, - "y": 7.445814297629393, - "heading": 0.019185726579151432, - "angularVelocity": -0.3586755233685199, - "velocityX": 3.203950829673405, - "velocityY": 0.2246450555457597, - "timestamp": 3.504263517846762 - }, - { - "x": 7.125290870666504, - "y": 7.458, - "heading": 0, - "angularVelocity": -0.2840637901552857, - "velocityX": 2.913217404608591, - "velocityY": 0.18042146002750595, - "timestamp": 3.5718037227023802 - }, - { - "x": 7.284551146493795, - "y": 7.4665356658331685, - "heading": -0.011983560395956015, - "angularVelocity": -0.19978893791899283, - "velocityX": 2.6551742811725556, - "velocityY": 0.1423059220209307, - "timestamp": 3.6317848233617918 - }, - { - "x": 7.428286998104642, - "y": 7.47296871812108, - "heading": -0.020071347748117502, - "angularVelocity": -0.13483892864997732, - "velocityX": 2.396352351501819, - "velocityY": 0.10725132111930623, - "timestamp": 3.6917659240212033 - }, - { - "x": 7.556472866857842, - "y": 7.4774155432867, - "heading": -0.02498803078182015, - "angularVelocity": -0.08197053704667534, - "velocityX": 2.137104310257201, - "velocityY": 0.0741371051336659, - "timestamp": 3.751747024680615 - }, - { - "x": 7.669092898448606, - "y": 7.47995645101358, - "heading": -0.027230015787281982, - "angularVelocity": -0.037378190476904, - "velocityX": 1.877591947341067, - "velocityY": 0.04236180561785803, - "timestamp": 3.8117281253400264 - }, - { - "x": 7.766136403587462, - "y": 7.480650177988498, - "heading": -0.027158837231048453, - "angularVelocity": 0.001186683062681687, - "velocityX": 1.6179013734658474, - "velocityY": 0.011565759335732523, - "timestamp": 3.871709225999438 - }, - { - "x": 7.847595735447885, - "y": 7.479541543526011, - "heading": -0.025049657080079103, - "angularVelocity": 0.03516407881452247, - "velocityX": 1.3580833123248393, - "velocityY": -0.018483063003166142, - "timestamp": 3.9316903266588494 - }, - { - "x": 7.913465176456017, - "y": 7.4766658686921605, - "heading": -0.021118970688138776, - "angularVelocity": 0.06553208175121362, - "velocityX": 1.0981699282605077, - "velocityY": -0.04794301542047442, - "timestamp": 3.991671427318261 - }, - { - "x": 7.963740302847386, - "y": 7.472051704673601, - "heading": -0.015541591171907823, - "angularVelocity": 0.09298561471722216, - "velocityX": 0.8381827915570343, - "velocityY": -0.07692696479112789, - "timestamp": 4.051652527977672 - }, - { - "x": 7.998417597522527, - "y": 7.465722607991469, - "heading": -0.008461644038298986, - "angularVelocity": 0.11803629903043378, - "velocityX": 0.5781370180592035, - "velocityY": -0.10551818176978627, - "timestamp": 4.111633628637083 - }, - { - "x": 8.017494201660156, - "y": 7.457698345184326, - "heading": 0, - "angularVelocity": 0.14107183671647575, - "velocityX": 0.3180435825269393, - "velocityY": -0.1337798526356988, - "timestamp": 4.171614729296494 - }, - { - "x": 8.008975668857596, - "y": 7.440959395735479, - "heading": 0.016440344425850486, - "angularVelocity": 0.17454816573497248, - "velocityX": -0.09044179592138195, - "velocityY": -0.17771847395317164, - "timestamp": 4.265802739560738 - }, - { - "x": 7.961981832760565, - "y": 7.4200887947149115, - "heading": 0.0360050890089686, - "angularVelocity": 0.20772011775415367, - "velocityX": -0.4989364990850843, - "velocityY": -0.22158447728129313, - "timestamp": 4.3599907498249815 - }, - { - "x": 7.876511570645417, - "y": 7.39509531765153, - "heading": 0.058657775076795544, - "angularVelocity": 0.24050498576490828, - "velocityX": -0.9074431222759898, - "velocityY": -0.2653573102697685, - "timestamp": 4.454178760089225 - }, - { - "x": 7.752563396875145, - "y": 7.365990633137338, - "heading": 0.08435039467917142, - "angularVelocity": 0.2727801503641011, - "velocityX": -1.315965518567977, - "velocityY": -0.3090062570866462, - "timestamp": 4.548366770353469 - }, - { - "x": 7.590135252234061, - "y": 7.332791018782474, - "heading": 0.11301654484596554, - "angularVelocity": 0.3043503104733978, - "velocityX": -1.7245097776818146, - "velocityY": -0.35248238349789773, - "timestamp": 4.642554780617712 - }, - { - "x": 7.3892240901384, - "y": 7.295520773303632, - "heading": 0.14455765680612448, - "angularVelocity": 0.33487395977121315, - "velocityX": -2.13308638256617, - "velocityY": -0.39570052891318314, - "timestamp": 4.736742790881956 - }, - { - "x": 7.149824938411661, - "y": 7.254220108341429, - "heading": 0.1788109128374065, - "angularVelocity": 0.36366896312157765, - "velocityX": -2.54171577735964, - "velocityY": -0.4384917448232911, - "timestamp": 4.8309308011461995 - }, - { - "x": 6.871928226350113, - "y": 7.208968463075644, - "heading": 0.21545413209734332, - "angularVelocity": 0.3890433523028524, - "velocityX": -2.9504467849136042, - "velocityY": -0.4804395499897761, - "timestamp": 4.925118811410443 - }, - { - "x": 6.555508023323144, - "y": 7.15999689291417, - "heading": 0.25354713749374036, - "angularVelocity": 0.40443582245264, - "velocityX": -3.3594530995957568, - "velocityY": -0.5199342254293745, - "timestamp": 5.019306821674687 - }, - { - "x": 6.201946981022424, - "y": 7.116596127430346, - "heading": 0.25354714311943394, - "angularVelocity": 5.972834101491316e-8, - "velocityX": -3.753779714730222, - "velocityY": -0.46078864350212084, - "timestamp": 5.11349483193893 - }, - { - "x": 5.847829680383322, - "y": 7.077995922009697, - "heading": 0.25354714640605686, - "angularVelocity": 3.4894280958051e-8, - "velocityX": -3.7596855443238373, - "velocityY": -0.4098207968546854, - "timestamp": 5.207682842203174 - }, - { - "x": 5.493712379149646, - "y": 7.039395722043647, - "heading": 0.25354714969267966, - "angularVelocity": 3.4894280048226024e-8, - "velocityX": -3.759685550636459, - "velocityY": -0.40982073894285953, - "timestamp": 5.301870852467418 - }, - { - "x": 5.13959507794107, - "y": 7.0007955218474756, - "heading": 0.25354715297933, - "angularVelocity": 3.489457219723442e-8, - "velocityX": -3.7596855503699804, - "velocityY": -0.4098207413860709, - "timestamp": 5.396058862731661 - }, - { - "x": 4.793375065328026, - "y": 6.963049240971564, - "heading": 0.2809926873119857, - "angularVelocity": 0.2913909557666365, - "velocityX": -3.675839543076952, - "velocityY": -0.4007546265178986, - "timestamp": 5.490246872995905 - }, - { - "x": 4.4856271372174605, - "y": 6.929500155423233, - "heading": 0.3062072463057812, - "angularVelocity": 0.2677045509620205, - "velocityX": -3.2673790140292933, - "velocityY": -0.3561927410315718, - "timestamp": 5.5844348832601485 - }, - { - "x": 4.216349125910067, - "y": 6.900146018877801, - "heading": 0.32859640421091696, - "angularVelocity": 0.23770709076795637, - "velocityX": -2.858941499580855, - "velocityY": -0.31165470491497754, - "timestamp": 5.678622893524392 - }, - { - "x": 3.9855401854111148, - "y": 6.874986045056756, - "heading": 0.34796083888612495, - "angularVelocity": 0.2055934149249067, - "velocityX": -2.450512967111424, - "velocityY": -0.26712501676655553, - "timestamp": 5.772810903788636 - }, - { - "x": 3.7931998687162816, - "y": 6.854019825581219, - "heading": 0.36419988308525797, - "angularVelocity": 0.17241094862896578, - "velocityX": -2.042089180514842, - "velocityY": -0.22259966440226955, - "timestamp": 5.866998914052879 - }, - { - "x": 3.639327899901621, - "y": 6.837247108195556, - "heading": 0.37725261237367164, - "angularVelocity": 0.13858164379727655, - "velocityX": -1.6336683234200817, - "velocityY": -0.17807699025180046, - "timestamp": 5.961186924317123 - }, - { - "x": 3.5239240923326007, - "y": 6.824667722043819, - "heading": 0.38707830352299477, - "angularVelocity": 0.10431997790119188, - "velocityX": -1.2252494478358333, - "velocityY": -0.13355613008965372, - "timestamp": 6.0553749345813666 - }, - { - "x": 3.4469883122501845, - "y": 6.8162815454148475, - "heading": 0.39364810050149374, - "angularVelocity": 0.06975194571015403, - "velocityX": -0.8168319923796448, - "velocityY": -0.08903656214250527, - "timestamp": 6.14956294484561 - }, - { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0.03495947190378589, - "velocityX": -0.4084155935928461, - "velocityY": -0.04451793673752315, - "timestamp": 6.243750955109854 - }, - { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": -1.9952146739162206e-34, - "velocityY": -3.951795765453016e-33, - "timestamp": 6.337938965374097 - }, - { - "x": 3.425127749537185, - "y": 6.810478633677757, - "heading": 0.3886892234850989, - "angularVelocity": -0.13293807639169805, - "velocityX": 0.2675517930120232, - "velocityY": -0.025935588272518423, - "timestamp": 6.400010272097332 - }, - { - "x": 3.458347138585669, - "y": 6.807258412874914, - "heading": 0.3723514064813247, - "angularVelocity": -0.26321045691257755, - "velocityX": 0.535181081278085, - "velocityY": -0.05187937829625554, - "timestamp": 6.462081578820567 - }, - { - "x": 3.508184213080087, - "y": 6.802427294600358, - "heading": 0.34812438720722594, - "angularVelocity": -0.3903094771652888, - "velocityX": 0.8029003596884762, - "velocityY": -0.07783174754300737, - "timestamp": 6.524152885543802 - }, - { - "x": 3.5746455076736363, - "y": 6.795984642746885, - "heading": 0.31624502791908643, - "angularVelocity": -0.5135925272251962, - "velocityX": 1.070724914651617, - "velocityY": -0.10379436479708294, - "timestamp": 6.586224192267037 - }, - { - "x": 3.6577387353786484, - "y": 6.787929625128416, - "heading": 0.2770021560106679, - "angularVelocity": -0.6322224225666108, - "velocityX": 1.3386737301264124, - "velocityY": -0.12977038898779852, - "timestamp": 6.648295498990271 - }, - { - "x": 3.7574730901724314, - "y": 6.7782611086258875, - "heading": 0.23075444743671972, - "angularVelocity": -0.7450738677080124, - "velocityX": 1.6067706652041787, - "velocityY": -0.1557646682971112, - "timestamp": 6.710366805713506 - }, - { - "x": 3.873859634378807, - "y": 6.766977536857145, - "heading": 0.17795757602156642, - "angularVelocity": -0.8505841781386996, - "velocityX": 1.875045819887845, - "velocityY": -0.18178402170643457, - "timestamp": 6.772438112436741 - }, - { - "x": 4.006911762109537, - "y": 6.754076769706622, - "heading": 0.11920696294593584, - "angularVelocity": -0.9465019535937051, - "velocityX": 2.1435367604550115, - "velocityY": -0.2078378534549514, - "timestamp": 6.834509419159976 - }, - { - "x": 4.156645657189765, - "y": 6.73955582918857, - "heading": 0.055309270826440785, - "angularVelocity": -1.029423988195197, - "velocityX": 2.4122884305927053, - "velocityY": -0.23393966205310818, - "timestamp": 6.896580725883211 - }, - { - "x": 4.323080375842652, - "y": 6.723410435666096, - "heading": -0.012586184407094544, - "angularVelocity": -1.0938299645646508, - "velocityX": 2.681347106079046, - "velocityY": -0.2601104177560936, - "timestamp": 6.958652032606445 - }, - { - "x": 4.506235840968647, - "y": 6.705634179985004, - "heading": -0.08270962096165607, - "angularVelocity": -1.1297238652836274, - "velocityX": 2.9507267495214404, - "velocityY": -0.2863844281597597, - "timestamp": 7.02072333932968 - }, - { - "x": 4.706118949857515, - "y": 6.686217722826257, - "heading": -0.15194437984101536, - "angularVelocity": -1.1154068205470853, - "velocityX": 3.2202175117742615, - "velocityY": -0.3128088996947802, - "timestamp": 7.082794646052915 - }, - { - "x": 4.922607059053276, - "y": 6.66515425622058, - "heading": -0.21301282950448638, - "angularVelocity": -0.9838434678965439, - "velocityX": 3.4877324262085807, - "velocityY": -0.3393430510421988, - "timestamp": 7.14486595277615 - }, - { - "x": 5.152321807662402, - "y": 6.642265811014449, - "heading": -0.22692060137612477, - "angularVelocity": -0.22406120647088962, - "velocityX": 3.7008202458728654, - "velocityY": -0.3687443750482909, - "timestamp": 7.206937259499385 - }, - { - "x": 5.385997622338085, - "y": 6.619824669792801, - "heading": -0.22692062699124488, - "angularVelocity": -4.12672480414244e-7, - "velocityX": 3.7646350143327725, - "velocityY": -0.36153808267225507, - "timestamp": 7.269008566222619 - }, - { - "x": 5.6195399174914655, - "y": 6.5960341766076915, - "heading": -0.2269206525768778, - "angularVelocity": -4.121974272362167e-7, - "velocityX": 3.7624839476103156, - "velocityY": -0.3832768221124699, - "timestamp": 7.331079872945854 - }, - { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": -0.22692067835854435, - "angularVelocity": -4.1535562716932914e-7, - "velocityX": 3.729650183615213, - "velocityY": -0.6268142199732243, - "timestamp": 7.393151179669089 - }, - { - "x": 5.959383441341322, - "y": 6.535142573313356, - "heading": -0.22692070161190955, - "angularVelocity": -7.95525127627382e-7, - "velocityX": 3.706414340926548, - "velocityY": -0.7521132022680976, - "timestamp": 7.422381387996315 - }, - { - "x": 6.066940887967902, - "y": 6.509606194124557, - "heading": -0.22692072465630755, - "angularVelocity": -7.883761123678933e-7, - "velocityX": 3.6796674667011553, - "velocityY": -0.8736297361581465, - "timestamp": 7.4516115963235405 - }, - { - "x": 6.17368571603366, - "y": 6.480861582677863, - "heading": -0.2269207475995078, - "angularVelocity": -7.849140177187675e-7, - "velocityX": 3.651866824580282, - "velocityY": -0.9833871563580519, - "timestamp": 7.480841804650766 - }, - { - "x": 6.2799139038286755, - "y": 6.450262838777624, - "heading": -0.2269207705194487, - "angularVelocity": -7.841182881547448e-7, - "velocityX": 3.63419194984251, - "velocityY": -1.0468192206387272, - "timestamp": 7.510072012977992 - }, - { - "x": 6.38600768248991, - "y": 6.419201268635774, - "heading": -0.22692079344039778, - "angularVelocity": -7.841527793264923e-7, - "velocityX": 3.629593654398156, - "velocityY": -1.0626530537902528, - "timestamp": 7.539302221305218 - }, - { - "x": 6.4915378820851375, - "y": 6.386275784732387, - "heading": -0.22692082347226308, - "angularVelocity": -0.0000010274256324387564, - "velocityX": 3.6103129479556895, - "velocityY": -1.1264197481865303, - "timestamp": 7.568532429632444 - }, - { - "x": 6.594254502115419, - "y": 6.3529873750963075, - "heading": -0.23642872835410836, - "angularVelocity": -0.32527667183951403, - "velocityX": 3.514057063171563, - "velocityY": -1.1388358667671954, - "timestamp": 7.597762637959669 - }, - { - "x": 6.69366972769076, - "y": 6.320452523543764, - "heading": -0.25267195567048195, - "angularVelocity": -0.5557000187824542, - "velocityX": 3.4011124540202067, - "velocityY": -1.1130557534298289, - "timestamp": 7.626992846286895 - }, - { - "x": 6.789488054498416, - "y": 6.288864279277486, - "heading": -0.26990654049679197, - "angularVelocity": -0.5896155317621415, - "velocityX": 3.278058292810801, - "velocityY": -1.0806711985311153, - "timestamp": 7.656223054614121 - }, - { - "x": 6.881710669103943, - "y": 6.258257632428043, - "heading": -0.28694313831911705, - "angularVelocity": -0.5828421621787924, - "velocityX": 3.1550447254125045, - "velocityY": -1.047089589879878, - "timestamp": 7.685453262941347 - }, - { - "x": 6.970345859458022, - "y": 6.228646491338886, - "heading": -0.3032367291527766, - "angularVelocity": -0.5574230142767849, - "velocityX": 3.0323146986098455, - "velocityY": -1.0130321603485573, - "timestamp": 7.714683471268573 - }, - { - "x": 7.055400617247705, - "y": 6.200038168598195, - "heading": -0.3184704659597927, - "angularVelocity": -0.5211641544429054, - "velocityX": 2.909823865690951, - "velocityY": -0.978724558526454, - "timestamp": 7.743913679595798 - }, - { - "x": 7.136880378911089, - "y": 6.172437128670844, - "heading": -0.33243641899697507, - "angularVelocity": -0.477791772157844, - "velocityX": 2.7875190197494866, - "velocityY": -0.9442642220804569, - "timestamp": 7.773143887923024 - }, - { - "x": 7.214789390563965, - "y": 6.145846366882324, - "heading": -0.344987478573796, - "angularVelocity": -0.42938659336036833, - "velocityX": 2.665359438451487, - "velocityY": -0.9097014120066562, - "timestamp": 7.80237409625025 - }, - { - "x": 7.370577859130934, - "y": 6.0916577592619054, - "heading": -0.3653758531922844, - "angularVelocity": -0.3132570133567013, - "velocityX": 2.393610638018096, - "velocityY": -0.8325804140238011, - "timestamp": 7.867459230019526 - }, - { - "x": 7.5087533836626745, - "y": 6.042843250581257, - "heading": -0.37947223160146565, - "angularVelocity": -0.21658368958955537, - "velocityX": 2.1229967049244562, - "velocityY": -0.7500101152698444, - "timestamp": 7.932544363788803 - }, - { - "x": 7.629367562165718, - "y": 5.999619792415774, - "heading": -0.38804895009153, - "angularVelocity": -0.1317769203712161, - "velocityX": 1.85317554897581, - "velocityY": -0.6641064658284028, - "timestamp": 7.997629497558079 - }, - { - "x": 7.7324579600950285, - "y": 5.962133690972344, - "heading": -0.39162657356882036, - "angularVelocity": -0.05496836635495089, - "velocityX": 1.5839315671495955, - "velocityY": -0.5759548958801737, - "timestamp": 8.062714631327355 - }, - { - "x": 7.81805300550911, - "y": 5.930490256491027, - "heading": -0.39057997729782273, - "angularVelocity": 0.01608041975771331, - "velocityX": 1.3151243679933469, - "velocityY": -0.4861852876186942, - "timestamp": 8.127799765096631 - }, - { - "x": 7.886174910867723, - "y": 5.904768911877698, - "heading": -0.38519213768662347, - "angularVelocity": 0.08278141718657105, - "velocityX": 1.0466584519915365, - "velocityY": -0.39519538677620675, - "timestamp": 8.192884898865907 - }, - { - "x": 7.936841490598973, - "y": 5.885031694896893, - "heading": -0.3756842946663964, - "angularVelocity": 0.1460831755210139, - "velocityX": 0.7784662456231665, - "velocityY": -0.3032523072130836, - "timestamp": 8.257970032635184 - }, - { - "x": 7.970067341476633, - "y": 5.8713284070268905, - "heading": -0.36223415085714655, - "angularVelocity": 0.206654623418748, - "velocityX": 0.510498311264806, - "velocityY": -0.21054405324847428, - "timestamp": 8.32305516640446 - }, - { - "x": 7.985864639282227, - "y": 5.863699913024902, - "heading": -0.344987478573796, - "angularVelocity": 0.2649863537883773, - "velocityX": 0.24271745160106833, - "velocityY": -0.1172079330593501, - "timestamp": 8.388140300173736 - }, - { - "x": 7.985045656249217, - "y": 5.861990715165632, - "heading": -0.3251381624172612, - "angularVelocity": 0.31897227576671205, - "velocityX": -0.01316080008970204, - "velocityY": -0.02746627272232015, - "timestamp": 8.450369270101682 - }, - { - "x": 7.968302412952749, - "y": 5.865866788787815, - "heading": -0.3020436028872691, - "angularVelocity": 0.37112231741475454, - "velocityX": -0.26905866055398503, - "velocityY": 0.062287285594966935, - "timestamp": 8.512598240029627 - }, - { - "x": 7.935633552055529, - "y": 5.875328955639361, - "heading": -0.2758355383990043, - "angularVelocity": 0.4211553641111372, - "velocityX": -0.5249783330022537, - "velocityY": 0.1520540491430483, - "timestamp": 8.574827209957572 - }, - { - "x": 7.887037558008595, - "y": 5.8903781286161365, - "heading": -0.24666763452179902, - "angularVelocity": 0.468719053376244, - "velocityX": -0.7809223598462517, - "velocityY": 0.24183548264097016, - "timestamp": 8.637056179885517 - }, - { - "x": 7.822512734192608, - "y": 5.911015324029987, - "heading": -0.21472164060862464, - "angularVelocity": 0.5133620876283883, - "velocityX": -1.0368936508301385, - "velocityY": 0.33163324795100674, - "timestamp": 8.699285149813463 - }, - { - "x": 7.742057181096897, - "y": 5.937241677003945, - "heading": -0.1802161756247138, - "angularVelocity": 0.5544919837796514, - "velocityX": -1.29289546635384, - "velocityY": 0.4214492543316279, - "timestamp": 8.761514119741408 - }, - { - "x": 7.645668783226514, - "y": 5.9690584605532635, - "heading": -0.14341967249459378, - "angularVelocity": 0.5913082471512265, - "velocityX": -1.5489312772168387, - "velocityY": 0.5112857176674774, - "timestamp": 8.823743089669353 - }, - { - "x": 7.533345224831275, - "y": 6.006467108033043, - "heading": -0.10467019490543185, - "angularVelocity": 0.6226919332592115, - "velocityX": -1.8050043014579413, - "velocityY": 0.6011452145695896, - "timestamp": 8.885972059597298 - }, - { - "x": 7.405084087505747, - "y": 6.04946923502926, - "heading": -0.06440725119965417, - "angularVelocity": 0.6470128583583754, - "velocityX": -2.0611161887146876, - "velocityY": 0.6910306734308482, - "timestamp": 8.948201029525244 - }, - { - "x": 7.260883177292953, - "y": 6.098066643466027, - "heading": -0.023226072117979224, - "angularVelocity": 0.6617686124221249, - "velocityX": -2.3172633321708815, - "velocityY": 0.7809450886466088, - "timestamp": 9.010429999453189 - }, - { - "x": 7.100741532540205, - "y": 6.152261239581916, - "heading": 0.018021968863737303, - "angularVelocity": 0.6628430621538008, - "velocityX": -2.5734259290837285, - "velocityY": 0.8708901365174432, - "timestamp": 9.072658969381134 - }, - { - "x": 6.924662708731195, - "y": 6.212054571383467, - "heading": 0.05802124542072086, - "angularVelocity": 0.6427758100977419, - "velocityX": -2.829531390490462, - "velocityY": 0.9608600603671181, - "timestamp": 9.13488793930908 - }, - { - "x": 6.732667433541327, - "y": 6.277445378277029, - "heading": 0.09448040335619008, - "angularVelocity": 0.5858872158366861, - "velocityX": -3.0853037646642187, - "velocityY": 1.050809726872821, - "timestamp": 9.197116909237025 - }, - { - "x": 6.524862060286313, - "y": 6.348410321942265, - "heading": 0.12245319566137218, - "angularVelocity": 0.44951398581032764, - "velocityX": -3.33936707446111, - "velocityY": 1.1403843539014786, - "timestamp": 9.25934587916497 - }, - { - "x": 6.30228026524581, - "y": 6.424202518084616, - "heading": 0.1231057628389814, - "angularVelocity": 0.010486549566300301, - "velocityX": -3.5768195311320152, - "velocityY": 1.2179567849204198, - "timestamp": 9.321574849092915 - }, - { - "x": 6.078984969036473, - "y": 6.498549743391548, - "heading": 0.1231057748040032, - "angularVelocity": 1.9227414178241924e-7, - "velocityX": -3.5882852707972814, - "velocityY": 1.194736557475109, - "timestamp": 9.38380381902086 - }, - { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": 0.12310578689683861, - "angularVelocity": 1.9432806685756468e-7, - "velocityX": -3.662936913327793, - "velocityY": 0.941318096340742, - "timestamp": 9.446032788948806 - }, - { - "x": 5.606427257807288, - "y": 6.600564902613298, - "heading": 0.1231057972666647, - "angularVelocity": 1.578555424299444e-7, - "velocityX": -3.7237014740499577, - "velocityY": 0.6612371129043467, - "timestamp": 9.511724660841988 - }, - { - "x": 5.35931577124569, - "y": 6.62625930787543, - "heading": 0.12310580746861746, - "angularVelocity": 1.5530007712924032e-7, - "velocityX": -3.7616752185629396, - "velocityY": 0.39113522756534835, - "timestamp": 9.577416532735171 - }, - { - "x": 5.111994168115865, - "y": 6.649845615003531, - "heading": 0.1231058176602883, - "angularVelocity": 1.551435596748186e-7, - "velocityX": -3.764873735550995, - "velocityY": 0.35904452785959023, - "timestamp": 9.643108404628354 - }, - { - "x": 4.868859374012009, - "y": 6.67294055859981, - "heading": 0.13716866748145323, - "angularVelocity": 0.21407290454489739, - "velocityX": -3.701139685883833, - "velocityY": 0.35156470550620644, - "timestamp": 9.708800276521536 - }, - { - "x": 4.644141674488456, - "y": 6.6943246178055915, - "heading": 0.16550217221046287, - "angularVelocity": 0.43130913935716303, - "velocityX": -3.4207839272559015, - "velocityY": 0.3255206251475534, - "timestamp": 9.77449214841472 - }, - { - "x": 4.438149971299598, - "y": 6.713938607045374, - "heading": 0.1971398735951766, - "angularVelocity": 0.48160754858923427, - "velocityX": -3.1357258859027066, - "velocityY": 0.2985755874284469, - "timestamp": 9.840184020307902 - }, - { - "x": 4.2509016025753406, - "y": 6.731775563046276, - "heading": 0.22901708759854217, - "angularVelocity": 0.48525354940713283, - "velocityX": -2.8504039134206605, - "velocityY": 0.27152455070706877, - "timestamp": 9.905875892201085 - }, - { - "x": 4.082393919350406, - "y": 6.747832566507683, - "heading": 0.25967107343501483, - "angularVelocity": 0.46663285659323517, - "velocityX": -2.565122265033534, - "velocityY": 0.24442907468850525, - "timestamp": 9.971567764094267 - }, - { - "x": 3.9326220842038664, - "y": 6.762107971713798, - "heading": 0.28824128245235414, - "angularVelocity": 0.43491239013245725, - "velocityX": -2.279914254690053, - "velocityY": 0.21730854662395976, - "timestamp": 10.03725963598745 - }, - { - "x": 3.801581586235111, - "y": 6.77460070477556, - "heading": 0.314159509734349, - "angularVelocity": 0.3945423769342238, - "velocityX": -1.9947749119073828, - "velocityY": 0.19017167119358822, - "timestamp": 10.102951507880633 - }, - { - "x": 3.6892685916749244, - "y": 6.785310000165767, - "heading": 0.3370219943875935, - "angularVelocity": 0.34802607985383205, - "velocityX": -1.7096939289964497, - "velocityY": 0.16302314246152652, - "timestamp": 10.168643379773815 - }, - { - "x": 3.595679898494151, - "y": 6.794235280663889, - "heading": 0.35652698961990326, - "angularVelocity": 0.2969164170572818, - "velocityX": -1.4246616892414212, - "velocityY": 0.135865826941808, - "timestamp": 10.234335251666998 - }, - { - "x": 3.520812828526041, - "y": 6.801376095406927, - "heading": 0.37244068726433127, - "angularVelocity": 0.2422475899347831, - "velocityX": -1.1396702181031793, - "velocityY": 0.10870164812244498, - "timestamp": 10.30002712356018 - }, - { - "x": 3.464665127058438, - "y": 6.806732084957018, - "heading": 0.38457706026543664, - "angularVelocity": 0.18474695044220302, - "velocityX": -0.8547130695088268, - "velocityY": 0.08153199773025213, - "timestamp": 10.365718995453364 - }, - { - "x": 3.4272348818352074, - "y": 6.810302960282004, - "heading": 0.39278518447772715, - "angularVelocity": 0.12494885555456782, - "velocityX": -0.5697850303930024, - "velocityY": 0.05435794752192765, - "timestamp": 10.431410867346546 - }, - { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0.0632601721112495, - "velocityX": -0.28488184560874424, - "velocityY": 0.027180367966521624, - "timestamp": 10.497102739239729 - }, - { - "x": 3.408520460128784, - "y": 6.812088489532471, - "heading": 0.3969408636, - "angularVelocity": 0, - "velocityX": 1.9034366224257262e-34, - "velocityY": -2.547324229323533e-33, - "timestamp": 10.562794611132912 - }, - { - "x": 3.4428034429775143, - "y": 6.811427919070607, - "heading": 0.39249813327278055, - "angularVelocity": -0.050100574262489476, - "velocityX": 0.38660845958378076, - "velocityY": -0.007449238878501296, - "timestamp": 10.651470846737375 - }, - { - "x": 3.5113694006298597, - "y": 6.810106800279428, - "heading": 0.3836131655700934, - "angularVelocity": -0.10019558951868686, - "velocityX": 0.7732168284429818, - "velocityY": -0.014898228168725063, - "timestamp": 10.740147082341839 - }, - { - "x": 3.614218332551768, - "y": 6.808125167746075, - "heading": 0.37028807906085554, - "angularVelocity": -0.1502667137188131, - "velocityX": 1.1598251912796755, - "velocityY": -0.022346827420505266, - "timestamp": 10.828823317946302 - }, - { - "x": 3.7513502451988723, - "y": 6.8054830693306245, - "heading": 0.3525264976102127, - "angularVelocity": -0.20029697166969943, - "velocityX": 1.5464336269164098, - "velocityY": -0.02979488695523812, - "timestamp": 10.917499553550766 - }, - { - "x": 3.9227651512282486, - "y": 6.802180567172425, - "heading": 0.3303333796886753, - "angularVelocity": -0.2502713130553812, - "velocityX": 1.9330422052867193, - "velocityY": -0.03724224574585722, - "timestamp": 11.006175789155229 - }, - { - "x": 4.128463068361194, - "y": 6.79821773882544, - "heading": 0.3037147939289921, - "angularVelocity": -0.3001772186001935, - "velocityX": 2.319650983499712, - "velocityY": -0.04468872996211646, - "timestamp": 11.094852024759692 - }, - { - "x": 4.368444017678072, - "y": 6.793594678448866, - "heading": 0.2726776385751228, - "angularVelocity": -0.3500053327963678, - "velocityX": 2.706259999435507, - "velocityY": -0.05213415234714548, - "timestamp": 11.183528260364156 - }, - { - "x": 4.642708020512335, - "y": 6.788311497970363, - "heading": 0.23722930806330966, - "angularVelocity": -0.3997500600942169, - "velocityX": 3.092869255948188, - "velocityY": -0.05957831252634942, - "timestamp": 11.27220449596862 - }, - { - "x": 4.951255088173746, - "y": 6.782368328182758, - "heading": 0.19737735072702745, - "angularVelocity": -0.44940966499796203, - "velocityX": 3.479478640000833, - "velocityY": -0.0670209977576627, - "timestamp": 11.360880731573083 - }, - { - "x": 5.286562692788269, - "y": 6.775920770205836, - "heading": 0.19737734601955828, - "angularVelocity": -5.3086028753514235e-8, - "velocityX": 3.781256639153567, - "velocityY": -0.07270897251075666, - "timestamp": 11.449556967177546 - }, - { - "x": 5.595111114675473, - "y": 6.769985794431735, - "heading": 0.15789301075905496, - "angularVelocity": -0.4452639987631135, - "velocityX": 3.4794939115759345, - "velocityY": -0.06692859404378032, - "timestamp": 11.53823320278201 - }, - { - "x": 5.8693764275565705, - "y": 6.76471055259859, - "heading": 0.12280143262174693, - "angularVelocity": -0.3957269712466434, - "velocityX": 3.092884029318143, - "velocityY": -0.059488788593539996, - "timestamp": 11.626909438386473 - }, - { - "x": 6.109358599544783, - "y": 6.760094936608861, - "heading": 0.09209943623091729, - "angularVelocity": -0.3462257523850543, - "velocityX": 2.7062737874738274, - "velocityY": -0.05205020215694004, - "timestamp": 11.715585673990937 - }, - { - "x": 6.315057615509001, - "y": 6.756138852616793, - "heading": 0.06578495581515914, - "angularVelocity": -0.29674782918315123, - "velocityX": 2.3196633749963063, - "velocityY": -0.04461267401690494, - "timestamp": 11.8042619095954 - }, - { - "x": 6.4864734670806214, - "y": 6.752842221795132, - "heading": 0.043856783005957245, - "angularVelocity": -0.24728353272698234, - "velocityX": 1.9330528681462538, - "velocityY": -0.037176034810112814, - "timestamp": 11.892938145199864 - }, - { - "x": 6.623606149810205, - "y": 6.750204980767773, - "heading": 0.026314311963007715, - "angularVelocity": -0.19782606831887856, - "velocityX": 1.5464423111199512, - "velocityY": -0.029740110294281013, - "timestamp": 11.981614380804327 - }, - { - "x": 6.726455661694478, - "y": 6.74822708185799, - "heading": 0.013157327563715553, - "angularVelocity": -0.14837102984364775, - "velocityX": 1.1598317315027789, - "velocityY": -0.022304723427886322, - "timestamp": 12.07029061640879 - }, - { - "x": 6.795022002255089, - "y": 6.7469084932218335, - "heading": 0.004385844550104781, - "angularVelocity": -0.09891582512292933, - "velocityX": 0.7732211464912397, - "velocityY": -0.014869695664985829, - "timestamp": 12.158966852013254 - }, - { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": -4.086871260037018e-38, - "angularVelocity": -0.04945907457853377, - "velocityX": 0.3866105668308079, - "velocityY": -0.007434847721776456, - "timestamp": 12.247643087617718 - }, - { - "x": 6.829305171966553, - "y": 6.746249198913574, - "heading": -5.480350490542979e-38, - "angularVelocity": -1.571392014077605e-37, - "velocityX": 4.425248303324229e-37, - "velocityY": 1.8999126670467387e-37, - "timestamp": 12.336319323222181 - } - ] + "samples": [ + { + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": 6.831435290856854e-28, + "angularVelocity": 9.118091433542868e-28, + "velocityX": -1.02680113599063e-27, + "velocityY": -2.9257138265977544e-27, + "timestamp": 0 + }, + { + "x": 0.46193105617201946, + "y": 6.976128411049744, + "heading": 0.007924686703269965, + "angularVelocity": 0.09655689623933598, + "velocityX": 0.35283526164412465, + "velocityY": -0.056044671114677404, + "timestamp": 0.08207271579677758 + }, + { + "x": 0.5198473628636088, + "y": 6.966928755942304, + "heading": 0.023773564726982017, + "angularVelocity": 0.19310775657717816, + "velocityX": 0.7056706498539357, + "velocityY": -0.11209151565325522, + "timestamp": 0.16414543159355516 + }, + { + "x": 0.6067218129325357, + "y": 6.953128922029678, + "heading": 0.04754806851726075, + "angularVelocity": 0.2896760946615609, + "velocityX": 1.0585058533220593, + "velocityY": -0.16814155323914987, + "timestamp": 0.24621814739033274 + }, + { + "x": 0.7225543617090937, + "y": 6.9347285653346615, + "heading": 0.0792521757191159, + "angularVelocity": 0.386292897634319, + "velocityX": 1.4113405125203116, + "velocityY": -0.22419578195241194, + "timestamp": 0.3282908631871103 + }, + { + "x": 0.8673449286061865, + "y": 6.911727262980233, + "heading": 0.11889328960368833, + "angularVelocity": 0.4829999044107281, + "velocityX": 1.7641741898198249, + "velocityY": -0.2802551631334134, + "timestamp": 0.41036357898388787 + }, + { + "x": 1.0410933872562544, + "y": 6.884124518119783, + "heading": 0.166483438885144, + "angularVelocity": 0.5798534728556428, + "velocityX": 2.1170063274169095, + "velocityY": -0.3363205980510971, + "timestamp": 0.4924362947806654 + }, + { + "x": 1.2437995495818281, + "y": 6.851919767173026, + "heading": 0.2220408979091711, + "angularVelocity": 0.6769297017244366, + "velocityX": 2.4698361734184595, + "velocityY": -0.3923928998097307, + "timestamp": 0.574509010577443 + }, + { + "x": 1.4754631288914648, + "y": 6.815112383232462, + "heading": 0.28559273443277505, + "angularVelocity": 0.7743357327295749, + "velocityX": 2.822662526280537, + "velocityY": -0.44847284000816645, + "timestamp": 0.6565817263742205 + }, + { + "x": 1.6781593090904183, + "y": 6.782884452243251, + "heading": 0.3422497744154681, + "angularVelocity": 0.6903273448752822, + "velocityX": 2.4697145480217175, + "velocityY": -0.3926753327989152, + "timestamp": 0.7386544421709981 + }, + { + "x": 1.8518984587698175, + "y": 6.755259929382264, + "heading": 0.3908638510792468, + "angularVelocity": 0.5923293288375308, + "velocityX": 2.1168929039657844, + "velocityY": -0.3365859481168989, + "timestamp": 0.8207271579677756 + }, + { + "x": 1.9966808291546925, + "y": 6.732239139216171, + "heading": 0.43140805301261054, + "angularVelocity": 0.49400341562665323, + "velocityX": 1.7640743209153051, + "velocityY": -0.2804926088116284, + "timestamp": 0.9027998737645532 + }, + { + "x": 2.1125066103107177, + "y": 6.713822306194192, + "heading": 0.46386100769226224, + "angularVelocity": 0.39541709281327203, + "velocityX": 1.4112580536852743, + "velocityY": -0.2243965347458558, + "timestamp": 0.9848725895613307 + }, + { + "x": 2.1993759293412, + "y": 6.700009583149955, + "heading": 0.4882072247325505, + "angularVelocity": 0.2966420302280796, + "velocityX": 1.0584433351222509, + "velocityY": -0.16829859850671558, + "timestamp": 1.0669453053581084 + }, + { + "x": 2.257288845375724, + "y": 6.690801066111273, + "heading": 0.5044377142386447, + "angularVelocity": 0.197757431912976, + "velocityX": 0.7056293370128478, + "velocityY": -0.11219949223421748, + "timestamp": 1.149018021154886 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.09884777520270435, + "velocityX": 0.35281518848860643, + "velocityY": -0.05609978931418552, + "timestamp": 1.2310907369516635 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -9.87865478987197e-28, + "velocityX": 1.3097638047391152e-27, + "velocityY": 4.333298605574227e-28, + "timestamp": 1.313163452748441 + }, + { + "x": 2.3177158256290964, + "y": 6.704097470773247, + "heading": 0.5125504196, + "angularVelocity": -1.9471938619576857e-16, + "velocityX": 0.3454631797391752, + "velocityY": 0.19650228828210153, + "timestamp": 1.404259932505041 + }, + { + "x": 2.380656782769881, + "y": 6.7398988031009575, + "heading": 0.5125504196, + "angularVelocity": -2.58789471726069e-16, + "velocityX": 0.6909263377570228, + "velocityY": 0.3930045642089339, + "timestamp": 1.4953564122616412 + }, + { + "x": 2.475068211555481, + "y": 6.793600797653198, + "heading": 0.5125504196, + "angularVelocity": -4.208588013010918e-16, + "velocityX": 1.0363894306108958, + "velocityY": 0.5895068030699635, + "timestamp": 1.5864528920182412 + }, + { + "x": 2.569479640341081, + "y": 6.847302792205439, + "heading": 0.5125504196, + "angularVelocity": -2.430413480392466e-16, + "velocityX": 1.0363894306108958, + "velocityY": 0.5895068030699636, + "timestamp": 1.6775493717748413 + }, + { + "x": 2.6324205974818655, + "y": 6.8831041245331495, + "heading": 0.5125504196, + "angularVelocity": -8.097281474944775e-17, + "velocityX": 0.6909263377570227, + "velocityY": 0.393004564208934, + "timestamp": 1.7686458515314414 + }, + { + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "angularVelocity": -1.1468969708015058e-16, + "velocityX": 0.3454631797391751, + "velocityY": 0.19650228828210156, + "timestamp": 1.8597423312880415 + }, + { + "x": 2.663891077041626, + "y": 6.901004791259766, + "heading": 0.5125504196, + "angularVelocity": 2.11830418797891e-27, + "velocityX": 1.5918238245881604e-27, + "velocityY": -9.542216118093543e-27, + "timestamp": 1.9508388110446415 + }, + { + "x": 2.6835272178710947, + "y": 6.903699093874769, + "heading": 0.5052895060867068, + "angularVelocity": -0.10750505591698582, + "velocityX": 0.2907326211779112, + "velocityY": 0.039891833548624495, + "timestamp": 2.018379016027187 + }, + { + "x": 2.722803664548093, + "y": 6.909088595285118, + "heading": 0.4909617568575599, + "angularVelocity": -0.21213659675521643, + "velocityX": 0.5815269095962702, + "velocityY": 0.0797969359397381, + "timestamp": 2.0859192210097324 + }, + { + "x": 2.7817252554319474, + "y": 6.9171743770902525, + "heading": 0.4698001364111611, + "angularVelocity": -0.3133188661756009, + "velocityX": 0.872392834743124, + "velocityY": 0.119718052487766, + "timestamp": 2.1534594259922777 + }, + { + "x": 2.8602976561490117, + "y": 6.927957716546488, + "heading": 0.44208769535784764, + "angularVelocity": -0.410310289411695, + "velocityX": 1.1633426451307045, + "velocityY": 0.1596580800875858, + "timestamp": 2.220999630974823 + }, + { + "x": 2.958527554736122, + "y": 6.9414401025597545, + "heading": 0.4081739049952151, + "angularVelocity": -0.5021274420383679, + "velocityX": 1.4543914785644525, + "velocityY": 0.19962015242254944, + "timestamp": 2.2885398359573683 + }, + { + "x": 3.0764229044730724, + "y": 6.9576232575928, + "heading": 0.3684994634316936, + "angularVelocity": -0.5874196202658062, + "velocityX": 1.7455580682264529, + "velocityY": 0.2396077275339608, + "timestamp": 2.3560800409399136 + }, + { + "x": 3.2139932065748873, + "y": 6.976509160880165, + "heading": 0.32363586089987717, + "angularVelocity": -0.6642503164361231, + "velocityX": 2.036865332839415, + "velocityY": 0.2796246071839072, + "timestamp": 2.423620245922459 + }, + { + "x": 3.371249765918335, + "y": 6.998100055077647, + "heading": 0.2743526968387638, + "angularVelocity": -0.7296863264458512, + "velocityX": 2.3283399774117917, + "velocityY": 0.31967469158647477, + "timestamp": 2.4911604509050043 + }, + { + "x": 3.548205611708083, + "y": 7.022398379906059, + "heading": 0.22174281137258775, + "angularVelocity": -0.7789417500253684, + "velocityX": 2.620007532335429, + "velocityY": 0.3597608984854559, + "timestamp": 2.5587006558875496 + }, + { + "x": 3.7448736745121907, + "y": 7.049406441609737, + "heading": 0.16748610321735644, + "angularVelocity": -0.8033246000549304, + "velocityX": 2.911866537197107, + "velocityY": 0.3998812516286613, + "timestamp": 2.626240860870095 + }, + { + "x": 3.9612554938842215, + "y": 7.079124953628088, + "heading": 0.11452509717802098, + "angularVelocity": -0.7841404398020747, + "velocityX": 3.203748336682593, + "velocityY": 0.44001216795286324, + "timestamp": 2.69378106585264 + }, + { + "x": 4.19725484401393, + "y": 7.11154242557216, + "heading": 0.06954671604514642, + "angularVelocity": -0.6659497279361002, + "velocityX": 3.494205417213319, + "velocityY": 0.4799729576250136, + "timestamp": 2.7613212708351855 + }, + { + "x": 4.4502883587972, + "y": 7.14611913479446, + "heading": 0.06936242836661095, + "angularVelocity": -0.002728562618119077, + "velocityX": 3.7464131897240014, + "velocityY": 0.5119426159757035, + "timestamp": 2.828861475817731 + }, + { + "x": 4.703344709577149, + "y": 7.180890314474715, + "heading": 0.0693624097128088, + "angularVelocity": -2.761881187265661e-7, + "velocityX": 3.7467512993978347, + "velocityY": 0.5148219447844706, + "timestamp": 2.896401680800276 + }, + { + "x": 4.956401059058472, + "y": 7.215661503606044, + "heading": 0.06936239105900772, + "angularVelocity": -2.761881027324315e-7, + "velocityX": 3.7467512801704235, + "velocityY": 0.5148220847170133, + "timestamp": 2.9639418857828215 + }, + { + "x": 5.209457408539732, + "y": 7.250432692737848, + "heading": 0.06936237240520664, + "angularVelocity": -2.761881030342619e-7, + "velocityX": 3.7467512801694554, + "velocityY": 0.5148220847240553, + "timestamp": 3.031482090765367 + }, + { + "x": 5.462513758020992, + "y": 7.285203881869653, + "heading": 0.06936235375140556, + "angularVelocity": -2.761881026512252e-7, + "velocityX": 3.746751280169456, + "velocityY": 0.5148220847240557, + "timestamp": 3.099022295747912 + }, + { + "x": 5.715570107502252, + "y": 7.319975071001454, + "heading": 0.0693623350976045, + "angularVelocity": -2.761881026475552e-7, + "velocityX": 3.7467512801694602, + "velocityY": 0.51482208472402, + "timestamp": 3.1665625007304574 + }, + { + "x": 5.968626456990083, + "y": 7.354746260085438, + "heading": 0.06936231644380343, + "angularVelocity": -2.761881026693173e-7, + "velocityX": 3.746751280266743, + "velocityY": 0.5148220840160207, + "timestamp": 3.2341027057130027 + }, + { + "x": 6.221682937041094, + "y": 7.389516498948747, + "heading": 0.06936229778999946, + "angularVelocity": -2.761881456453648e-7, + "velocityX": 3.746753213384661, + "velocityY": 0.5148080150525901, + "timestamp": 3.301642910695548 + }, + { + "x": 6.476098697142329, + "y": 7.412301605590586, + "heading": 0.0693622227466872, + "angularVelocity": -0.0000011110909759881076, + "velocityX": 3.7668787082743447, + "velocityY": 0.33735619617571233, + "timestamp": 3.3691831156780934 + }, + { + "x": 6.712134128533923, + "y": 7.430661265319214, + "heading": 0.043459048416696004, + "angularVelocity": -0.38352229367212326, + "velocityX": 3.4947396362298986, + "velocityY": 0.27183304719570117, + "timestamp": 3.4367233206606387 + }, + { + "x": 6.928530828538707, + "y": 7.445821895754349, + "heading": 0.019203692642594847, + "angularVelocity": -0.3591246988422603, + "velocityX": 3.2039686592705516, + "velocityY": 0.22446823250023049, + "timestamp": 3.504263525643184 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": -3.857889621465489e-27, + "angularVelocity": -0.2843297950836498, + "velocityX": 2.9132283826891787, + "velocityY": 0.18030896188127155, + "timestamp": 3.5718037306257293 + }, + { + "x": 7.284551274277721, + "y": 7.4665314387188895, + "heading": -0.0119926176075512, + "angularVelocity": -0.19994035049895104, + "velocityX": 2.6551818761052863, + "velocityY": 0.14223574064773895, + "timestamp": 3.6317847078403016 + }, + { + "x": 7.428287142227841, + "y": 7.472962064944413, + "heading": -0.020085170649452998, + "angularVelocity": -0.1349186595101974, + "velocityX": 2.396357555761855, + "velocityY": 0.10721109465287079, + "timestamp": 3.691765685054874 + }, + { + "x": 7.556472961771024, + "y": 7.477407773553819, + "heading": -0.025003758777922633, + "angularVelocity": -0.0820024673968577, + "velocityX": 2.137107888132909, + "velocityY": 0.07411864254065184, + "timestamp": 3.751746662269446 + }, + { + "x": 7.669092912784827, + "y": 7.479948525190578, + "heading": -0.027245693064255495, + "angularVelocity": -0.037377421816798616, + "velocityX": 1.8775944681748589, + "velocityY": 0.04235929047423255, + "timestamp": 3.8117276394840185 + }, + { + "x": 7.766136331910851, + "y": 7.480642796103092, + "heading": -0.027173129314670138, + "angularVelocity": 0.0012097793826494725, + "velocityX": 1.617903269212617, + "velocityY": 0.01157485164055792, + "timestamp": 3.871708616698591 + }, + { + "x": 7.84759559249558, + "y": 7.479535203925268, + "heading": -0.025061681495632242, + "angularVelocity": 0.03520195763874475, + "velocityX": 1.3580849190455957, + "velocityY": -0.018465724122206602, + "timestamp": 3.931689593913163 + }, + { + "x": 7.91346499303532, + "y": 7.476660908974376, + "heading": -0.021128188015767303, + "angularVelocity": 0.06557901625699587, + "velocityX": 1.0981715136801353, + "velocityY": -0.047920108747306074, + "timestamp": 3.9916705711277354 + }, + { + "x": 7.963740122835744, + "y": 7.472048331328805, + "heading": -0.015547730952116253, + "angularVelocity": 0.0930371148120487, + "velocityX": 0.8381845734285565, + "velocityY": -0.0769006751768895, + "timestamp": 4.051651548342308 + }, + { + "x": 7.998417475618676, + "y": 7.46572091854732, + "heading": -0.008464652154293805, + "angularVelocity": 0.1180887529138424, + "velocityX": 0.5781391766739492, + "velocityY": -0.10549032502171554, + "timestamp": 4.111632525556881 + }, + { + "x": 8.017494201660156, + "y": 7.457698345184326, + "heading": -2.2921744122616346e-27, + "angularVelocity": 0.1411222782185235, + "velocityX": 0.318046269457016, + "velocityY": -0.13375196163099834, + "timestamp": 4.171613502771454 + }, + { + "x": 8.008975913339244, + "y": 7.440961813507422, + "heading": 0.016444448853137204, + "angularVelocity": 0.1745916436893877, + "velocityX": -0.09043914895843151, + "velocityY": -0.17769270355161393, + "timestamp": 4.265801566447589 + }, + { + "x": 7.96198229634572, + "y": 7.420093426479306, + "heading": 0.036012649187603114, + "angularVelocity": 0.20775668986837795, + "velocityX": -0.49893388991528925, + "velocityY": -0.2215608455427296, + "timestamp": 4.359989630123724 + }, + { + "x": 7.876512228216728, + "y": 7.395101957451527, + "heading": 0.05866814941015883, + "angularVelocity": 0.24053472741998802, + "velocityX": -0.9074405481238055, + "velocityY": -0.2653358403641312, + "timestamp": 4.45417769379986 + }, + { + "x": 7.752564223658973, + "y": 7.365999072123293, + "heading": 0.0843629510276007, + "angularVelocity": 0.2728031622541168, + "velocityX": -1.3159629757752496, + "velocityY": -0.3089869797971924, + "timestamp": 4.548365757475995 + }, + { + "x": 7.590136223929822, + "y": 7.332801044076014, + "heading": 0.1130306642056742, + "angularVelocity": 0.3043667324624816, + "velocityX": -1.7245072612136703, + "velocityY": -0.3524653416958384, + "timestamp": 4.642553821152131 + }, + { + "x": 7.389225183137582, + "y": 7.295532166028874, + "heading": 0.14457273960345385, + "angularVelocity": 0.334883998743585, + "velocityX": -2.1330838850564935, + "velocityY": -0.3956857864207608, + "timestamp": 4.736741884828266 + }, + { + "x": 7.149826130214695, + "y": 7.254232639741168, + "heading": 0.17882639129814942, + "angularVelocity": 0.36367295767408864, + "velocityX": -2.5417132870047907, + "velocityY": -0.43847940679313435, + "timestamp": 4.8309299485044015 + }, + { + "x": 6.8719294965182485, + "y": 7.208981885028011, + "heading": 0.21546950044257604, + "angularVelocity": 0.38904196258268475, + "velocityX": -2.950444279776156, + "velocityY": -0.48042982249588434, + "timestamp": 4.925118012180537 + }, + { + "x": 6.555509356459519, + "y": 7.160010902115707, + "heading": 0.2535620654404122, + "angularVelocity": 0.40443091736992315, + "velocityX": -3.359450525989546, + "velocityY": -0.51992769572894, + "timestamp": 5.019306075856672 + }, + { + "x": 6.20194815910043, + "y": 7.116609756478925, + "heading": 0.2535620710648909, + "angularVelocity": 5.971540845532163e-8, + "velocityX": -3.753779232311261, + "velocityY": -0.4607924183048939, + "timestamp": 5.113494139532808 + }, + { + "x": 5.8478308523572835, + "y": 7.078007759401044, + "heading": 0.2535620743508316, + "angularVelocity": 3.488701802778144e-8, + "velocityX": -3.7596834770993417, + "velocityY": -0.40983958658088665, + "timestamp": 5.207682203208943 + }, + { + "x": 5.493713536071184, + "y": 7.039405849865993, + "heading": 0.2535620776367723, + "angularVelocity": 3.488701827976151e-8, + "velocityX": -3.759683578417396, + "velocityY": -0.40983865713370193, + "timestamp": 5.301870266885079 + }, + { + "x": 5.139596219795213, + "y": 7.00080394023819, + "heading": 0.2535620809227431, + "angularVelocity": 3.488733818198086e-8, + "velocityX": -3.759683578309871, + "velocityY": -0.4098386581184616, + "timestamp": 5.396058330561214 + }, + { + "x": 4.793375987292505, + "y": 6.963055984000021, + "heading": 0.2810067536056752, + "angularVelocity": 0.2913816423416479, + "velocityX": -3.6758397931736098, + "velocityY": -0.40077218667499337, + "timestamp": 5.490246394237349 + }, + { + "x": 4.485627858757115, + "y": 6.929505403826658, + "heading": 0.30621912928575457, + "angularVelocity": 0.2676812187876781, + "velocityX": -3.2673792890952553, + "velocityY": -0.35620840756134725, + "timestamp": 5.584434457913485 + }, + { + "x": 4.216349669557381, + "y": 6.9001499572465725, + "heading": 0.32860581789982174, + "angularVelocity": 0.23768073936675813, + "velocityX": -2.8589417670337087, + "velocityY": -0.3116684368947614, + "timestamp": 5.67862252158962 + }, + { + "x": 3.985540575144852, + "y": 6.8749888593305, + "heading": 0.3479678404777486, + "angularVelocity": 0.20556768896431482, + "velocityX": -2.450513211590839, + "velocityY": -0.26713679986657873, + "timestamp": 5.772810585265756 + }, + { + "x": 3.793200129317667, + "y": 6.854021702392079, + "heading": 0.36420470133991156, + "angularVelocity": 0.17238767024655313, + "velocityX": -2.0420893934983666, + "velocityY": -0.22260949126755833, + "timestamp": 5.866998648941891 + }, + { + "x": 3.6393280566589286, + "y": 6.837248234599062, + "heading": 0.37725557912927543, + "angularVelocity": 0.13856190774065766, + "velocityX": -1.633668499522676, + "velocityY": -0.1780848563857597, + "timestamp": 5.9611867126180265 + }, + { + "x": 3.5239241708826787, + "y": 6.824668285381395, + "heading": 0.3870798194050559, + "angularVelocity": 0.10430451473724978, + "velocityX": -1.225249583355537, + "velocityY": -0.13356203245586246, + "timestamp": 6.055374776294162 + }, + { + "x": 3.4469883384836706, + "y": 6.8162817332333585, + "heading": 0.3936486152349144, + "angularVelocity": 0.06974127690367621, + "velocityX": -0.8168320846211579, + "velocityY": -0.08904049855907127, + "timestamp": 6.149562839970297 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.0349539871252265, + "velocityX": -0.4084156405121326, + "velocityY": -0.04451990557217318, + "timestamp": 6.243750903646433 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 4.764169197941492e-24, + "velocityX": 1.177617036966837e-23, + "velocityY": -7.781360494191974e-22, + "timestamp": 6.337938967322568 + }, + { + "x": 3.4251270675193637, + "y": 6.810469355892218, + "heading": 0.38870394635032085, + "angularVelocity": -0.13270177736033364, + "velocityX": 0.26754260724678264, + "velocityY": -0.02608523375098152, + "timestamp": 6.400009855998859 + }, + { + "x": 3.458345061227577, + "y": 6.807230578787204, + "heading": 0.3723948211461713, + "angularVelocity": -0.26274998718326975, + "velocityX": 0.5351622059328033, + "velocityY": -0.05217868108681347, + "timestamp": 6.46208074467515 + }, + { + "x": 3.508179991445113, + "y": 6.802371624572939, + "heading": 0.3482095805490521, + "angularVelocity": -0.38963902584428683, + "velocityX": 0.8028712216032942, + "velocityY": -0.07828072576186927, + "timestamp": 6.524151633351441 + }, + { + "x": 3.5746383517078186, + "y": 6.795891856003428, + "heading": 0.3163840274624053, + "angularVelocity": -0.5127291354345188, + "velocityX": 1.070684852109927, + "velocityY": -0.10439303685990675, + "timestamp": 6.586222522027732 + }, + { + "x": 3.6577278069428134, + "y": 6.787790439971061, + "heading": 0.2772056832992266, + "angularVelocity": -0.63118709911662, + "velocityX": 1.3386219692828794, + "velocityY": -0.1305187698313394, + "timestamp": 6.6482934107040235 + }, + { + "x": 3.7574574943964785, + "y": 6.778066242632264, + "heading": 0.23103157299019372, + "angularVelocity": -0.7438931726890192, + "velocityX": 1.6067062930864489, + "velocityY": -0.15666276971655022, + "timestamp": 6.7103642993803145 + }, + { + "x": 3.8738384093946294, + "y": 6.766717706731516, + "heading": 0.1783152332262761, + "angularVelocity": -0.8492924926343718, + "velocityX": 1.8749677583173519, + "velocityY": -0.18283185794122117, + "timestamp": 6.772435188056606 + }, + { + "x": 4.00688386795334, + "y": 6.753742690540388, + "heading": 0.11964925208533374, + "angularVelocity": -0.9451448560192912, + "velocityX": 2.1434437527157497, + "velocityY": -0.2090354507214015, + "timestamp": 6.834506076732897 + }, + { + "x": 4.156609966611215, + "y": 6.739138213215621, + "heading": 0.05583642242794389, + "angularVelocity": -1.028063735162284, + "velocityX": 2.412179072201139, + "velocityY": -0.2352870667106592, + "timestamp": 6.896576965409188 + }, + { + "x": 4.323035675773869, + "y": 6.722899990969598, + "heading": -0.011979486180355738, + "angularVelocity": -1.09255578669044, + "velocityX": 2.681220016529636, + "velocityY": -0.26160769713976106, + "timestamp": 6.958647854085479 + }, + { + "x": 4.506180877109483, + "y": 6.705021607935757, + "heading": -0.08203737634019735, + "angularVelocity": -1.1286754814354962, + "velocityX": 2.950581266698877, + "velocityY": -0.28803169110530014, + "timestamp": 7.02071874276177 + }, + { + "x": 4.706052676090064, + "y": 6.685493696321985, + "heading": -0.1512367442368059, + "angularVelocity": -1.1148441624139431, + "velocityX": 3.220056990370196, + "velocityY": -0.3146066059342914, + "timestamp": 7.082789631438061 + }, + { + "x": 4.922530525854263, + "y": 6.664309314644424, + "heading": -0.21234440720432737, + "angularVelocity": -0.9844818443990274, + "velocityX": 3.4875906303382327, + "velocityY": -0.3412933523159342, + "timestamp": 7.144860520114352 + }, + { + "x": 5.152231029581242, + "y": 6.641295647238388, + "heading": -0.22626463143704686, + "angularVelocity": -0.22426333067850218, + "velocityX": 3.700615677099489, + "velocityY": -0.37076426480787344, + "timestamp": 7.206931408790643 + }, + { + "x": 5.385889088159938, + "y": 6.618686799272017, + "heading": -0.22626465694222986, + "angularVelocity": -4.1090410586524524e-7, + "velocityX": 3.764374307531794, + "velocityY": -0.36424237591119274, + "timestamp": 7.269002297466934 + }, + { + "x": 5.619544792113157, + "y": 6.5960536299134915, + "heading": -0.22626468244744516, + "angularVelocity": -4.109046256964509e-7, + "velocityX": 3.764336373074485, + "velocityY": -0.3646342084219269, + "timestamp": 7.331073186143225 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.22626470814139457, + "angularVelocity": -4.139452486047814e-7, + "velocityX": 3.7295967696415837, + "velocityY": -0.6271318462207411, + "timestamp": 7.393144074819516 + }, + { + "x": 5.959381121610303, + "y": 6.535130978016939, + "heading": -0.22626473128429372, + "angularVelocity": -7.917457113083445e-7, + "velocityX": 3.70633385065427, + "velocityY": -0.7525096617429247, + "timestamp": 7.422374292054843 + }, + { + "x": 6.066915021909061, + "y": 6.5094954829249705, + "heading": -0.22626475422294146, + "angularVelocity": -7.84758031451317e-7, + "velocityX": 3.678860797818322, + "velocityY": -0.8770203411621033, + "timestamp": 7.45160450929017 + }, + { + "x": 6.173524224764707, + "y": 6.480251745020554, + "heading": -0.22626477706662856, + "angularVelocity": -7.815093169473892e-7, + "velocityX": 3.6472258142098704, + "velocityY": -1.0004625579406388, + "timestamp": 7.480834726525496 + }, + { + "x": 6.279732582783464, + "y": 6.449584123171384, + "heading": -0.22626479989606985, + "angularVelocity": -7.810219505026809e-7, + "velocityX": 3.6335124424048417, + "velocityY": -1.0491752969972816, + "timestamp": 7.510064943760823 + }, + { + "x": 6.385940150925491, + "y": 6.4189137659468605, + "heading": -0.22626482272551648, + "angularVelocity": -7.810221338422918e-7, + "velocityX": 3.633485419795893, + "velocityY": -1.0492688773950392, + "timestamp": 7.53929516099615 + }, + { + "x": 6.491550017633834, + "y": 6.3862446041508125, + "heading": -0.22626484978268144, + "angularVelocity": -9.256573336962615e-7, + "velocityX": 3.613037353027402, + "velocityY": -1.1176503251082635, + "timestamp": 7.5685253782314765 + }, + { + "x": 6.594250357432935, + "y": 6.352942011609429, + "heading": -0.23580774999861734, + "angularVelocity": -0.32647380411537286, + "velocityX": 3.513499026445183, + "velocityY": -1.1393207335159796, + "timestamp": 7.597755595466803 + }, + { + "x": 6.693663742086507, + "y": 6.320404663734079, + "heading": -0.25220771678314496, + "angularVelocity": -0.5610620903873084, + "velocityX": 3.4010484374171166, + "velocityY": -1.1131408163475485, + "timestamp": 7.62698581270213 + }, + { + "x": 6.789481492756262, + "y": 6.2888206325426586, + "heading": -0.2695690677329979, + "angularVelocity": -0.5939521697728065, + "velocityX": 3.278037583448113, + "velocityY": -1.0805267349587082, + "timestamp": 7.656216029937457 + }, + { + "x": 6.881704495384483, + "y": 6.2582213842117005, + "heading": -0.2867065303563255, + "angularVelocity": -0.5862926876443351, + "velocityX": 3.155057038603278, + "velocityY": -1.0468361587808552, + "timestamp": 7.685446247172783 + }, + { + "x": 6.970340753224835, + "y": 6.228619131395559, + "heading": -0.3030805193415338, + "angularVelocity": -0.5601733594172232, + "velocityX": 3.0323502944489906, + "velocityY": -1.0127277733797784, + "timestamp": 7.71467646440811 + }, + { + "x": 7.055397018445757, + "y": 6.200020205130934, + "heading": -0.3183784421956162, + "angularVelocity": -0.5233598755329688, + "velocityX": 2.9098745498930065, + "velocityY": -0.9784027957911439, + "timestamp": 7.743906681643437 + }, + { + "x": 7.1368785342038406, + "y": 6.172428433477075, + "heading": -0.33239562176130555, + "angularVelocity": -0.4795441461430052, + "velocityX": 2.787578179870906, + "velocityY": -0.9439468558075184, + "timestamp": 7.7731368988787635 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.4307821837626389, + "velocityX": 2.6654217357633208, + "velocityY": -0.9094036619962027, + "timestamp": 7.80236711611409 + }, + { + "x": 7.370581924170205, + "y": 6.091672602019875, + "heading": -0.3654249911056099, + "angularVelocity": -0.3140115764707324, + "velocityX": 2.3936699245523703, + "velocityY": -0.8323512600338246, + "timestamp": 7.867452336097115 + }, + { + "x": 7.508760881953895, + "y": 6.042869206500167, + "heading": -0.379545281879092, + "angularVelocity": -0.21695080353365873, + "velocityX": 2.123046642843499, + "velocityY": -0.7498383739417093, + "timestamp": 7.932537556080139 + }, + { + "x": 7.629377621865933, + "y": 5.999653284534207, + "heading": -0.38812959068868996, + "angularVelocity": -0.13189336706301324, + "velocityX": 1.8532124489015709, + "velocityY": -0.6639897964119993, + "timestamp": 7.997622776063164 + }, + { + "x": 7.7324695426404055, + "y": 5.962171118621702, + "heading": -0.3917039945024171, + "angularVelocity": -0.05491882511358906, + "velocityX": 1.5839528667393499, + "velocityY": -0.5758936656015066, + "timestamp": 8.062707996046187 + }, + { + "x": 7.818064946675874, + "y": 5.930527938551499, + "heading": -0.39064709699012523, + "angularVelocity": 0.016238671584232672, + "velocityX": 1.315128135969949, + "velocityY": -0.48618073471145673, + "timestamp": 8.127793216029211 + }, + { + "x": 7.886185947266274, + "y": 5.90480307138058, + "heading": -0.38524455647534983, + "angularVelocity": 0.08300717914427408, + "velocityX": 1.0466431642724277, + "velocityY": -0.39524898552433385, + "timestamp": 8.192878436012235 + }, + { + "x": 7.936850278188107, + "y": 5.885058458931867, + "heading": -0.37571963101716943, + "angularVelocity": 0.14634544464418678, + "velocityX": 0.778430662676533, + "velocityY": -0.3033655329714407, + "timestamp": 8.257963655995258 + }, + { + "x": 7.970072469166475, + "y": 5.871343812268201, + "heading": -0.3622515950174618, + "angularVelocity": 0.20692925372642912, + "velocityX": 0.5104414026261703, + "velocityY": -0.21071829621597563, + "timestamp": 8.323048875978282 + }, + { + "x": 7.985864639282227, + "y": 5.863699913024902, + "heading": -0.344987478573796, + "angularVelocity": 0.2652540230818704, + "velocityX": 0.2426383458467277, + "velocityY": -0.11744447119165673, + "timestamp": 8.388134095961306 + }, + { + "x": 7.985039462235232, + "y": 5.861972195893162, + "heading": -0.32512296531969853, + "angularVelocity": 0.31921787404219815, + "velocityX": -0.013260393510813807, + "velocityY": -0.027763992134404936, + "timestamp": 8.450362795759837 + }, + { + "x": 7.968288815736249, + "y": 5.865825907000098, + "heading": -0.30201458190342884, + "angularVelocity": 0.3713460748992679, + "velocityX": -0.2691787961698452, + "velocityY": 0.06192819582304089, + "timestamp": 8.512591495558368 + }, + { + "x": 7.9356113385118245, + "y": 5.8752618544788175, + "heading": -0.2757940495370948, + "angularVelocity": 0.421357548064224, + "velocityX": -0.5251190741606199, + "velocityY": 0.15163337028201795, + "timestamp": 8.5748201953569 + }, + { + "x": 7.88700551038904, + "y": 5.890280935303068, + "heading": -0.2466150135455457, + "angularVelocity": 0.46889997840252956, + "velocityX": -0.781083780958754, + "velocityY": 0.24135295888994696, + "timestamp": 8.63704889515543 + }, + { + "x": 7.8224696292437494, + "y": 5.910884146922841, + "heading": -0.2146591990965777, + "angularVelocity": 0.5135221296994205, + "velocityX": -1.0370758404759837, + "velocityY": 0.3310885762755298, + "timestamp": 8.699277594953962 + }, + { + "x": 7.742001788904935, + "y": 5.9370726017699464, + "heading": -0.18014519573995202, + "angularVelocity": 0.5546316003446364, + "velocityX": -1.2930985316956969, + "velocityY": 0.4208420701684547, + "timestamp": 8.761506294752493 + }, + { + "x": 7.6455998656570765, + "y": 5.9688475450465655, + "heading": -0.14334139980997887, + "angularVelocity": 0.5914280074809063, + "velocityX": -1.5491553505048972, + "velocityY": 0.5106155741561712, + "timestamp": 8.823734994551025 + }, + { + "x": 7.533261533346939, + "y": 6.006210375220679, + "heading": -0.10458582814826359, + "angularVelocity": 0.6227925665679687, + "velocityX": -1.8052495500281769, + "velocityY": 0.6004115511825956, + "timestamp": 8.885963694349556 + }, + { + "x": 7.404984359973908, + "y": 6.049162662823707, + "heading": -0.06431792685231795, + "angularVelocity": 0.6470953342479389, + "velocityX": -2.0613828312070357, + "velocityY": 0.6902327662652126, + "timestamp": 8.948192394148087 + }, + { + "x": 7.260766133037973, + "y": 6.097706149350396, + "heading": -0.023132839886022792, + "angularVelocity": 0.6618342838534901, + "velocityX": -2.317551666720475, + "velocityY": 0.7800819667428388, + "timestamp": 9.010421093946618 + }, + { + "x": 7.100605864047409, + "y": 6.151842655747804, + "heading": 0.018118188112097987, + "angularVelocity": 0.6628939401220464, + "velocityX": -2.573736387054354, + "velocityY": 0.8699604293947623, + "timestamp": 9.07264979374515 + }, + { + "x": 6.924507066086204, + "y": 6.21157360061122, + "heading": 0.058119739083768836, + "angularVelocity": 0.6428151496203137, + "velocityX": -2.829864652986999, + "velocityY": 0.9598616885263167, + "timestamp": 9.134878493543681 + }, + { + "x": 6.73249038975253, + "y": 6.276897504068343, + "heading": 0.09458083823039053, + "angularVelocity": 0.585920953911396, + "velocityX": -3.0856610688530672, + "velocityY": 1.0497391664716116, + "timestamp": 9.197107193342212 + }, + { + "x": 6.524662008432933, + "y": 6.347790578963825, + "heading": 0.12255603947008778, + "angularVelocity": 0.449554648100767, + "velocityX": -3.3397513043410987, + "velocityY": 1.1392343906429359, + "timestamp": 9.259335893140744 + }, + { + "x": 6.302055285584133, + "y": 6.423504678750072, + "heading": 0.12321029314887402, + "angularVelocity": 0.01051369674932009, + "velocityX": -3.5772356415849678, + "velocityY": 1.2167070826061643, + "timestamp": 9.321564592939275 + }, + { + "x": 6.078948914811354, + "y": 6.498413724394235, + "heading": 0.1232103051115219, + "angularVelocity": 1.9223682817827964e-7, + "velocityX": -3.585264861632913, + "velocityY": 1.2037700592601788, + "timestamp": 9.383793292737806 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0.12321031719433864, + "angularVelocity": 1.9416791274703824e-7, + "velocityX": -3.6623734312330534, + "velocityY": 0.9435079745715298, + "timestamp": 9.446021992536338 + }, + { + "x": 5.606452972493988, + "y": 6.600711194819738, + "heading": 0.12321032755676568, + "angularVelocity": 1.577427164908284e-7, + "velocityX": -3.7233054627128834, + "velocityY": 0.6634632443513382, + "timestamp": 9.511713945013648 + }, + { + "x": 5.35926430365433, + "y": 6.6256551734400135, + "heading": 0.12321033775139768, + "angularVelocity": 1.551884456824965e-7, + "velocityX": -3.7628455163520536, + "velocityY": 0.3797113296166925, + "timestamp": 9.577405897490959 + }, + { + "x": 5.111950804404792, + "y": 6.649329487896052, + "heading": 0.12321034793997507, + "angularVelocity": 1.5509627898966467e-7, + "velocityX": -3.7647457553489407, + "velocityY": 0.3603837846685449, + "timestamp": 9.64309784996827 + }, + { + "x": 4.868821995322067, + "y": 6.672498234290298, + "heading": 0.1372675563656116, + "angularVelocity": 0.2139867654335871, + "velocityX": -3.7010440383348504, + "velocityY": 0.35268774211343507, + "timestamp": 9.70878980244558 + }, + { + "x": 4.644110082814918, + "y": 6.693950400788965, + "heading": 0.16558913990545626, + "angularVelocity": 0.4311271391975579, + "velocityX": -3.420691637758275, + "velocityY": 0.3265569935080811, + "timestamp": 9.77448175492289 + }, + { + "x": 4.438123681636039, + "y": 6.713626799516342, + "heading": 0.19721421356331742, + "angularVelocity": 0.48141473141302027, + "velocityX": -3.13564132912667, + "velocityY": 0.29952525363244636, + "timestamp": 9.8401737074002 + }, + { + "x": 4.250880120713019, + "y": 6.73152047543666, + "heading": 0.22907908319149847, + "angularVelocity": 0.4850650410974934, + "velocityX": -2.8503272297728453, + "velocityY": 0.27238764027449763, + "timestamp": 9.905865659877511 + }, + { + "x": 4.082376753854165, + "y": 6.74762851546552, + "heading": 0.25972143161316247, + "angularVelocity": 0.466455132875641, + "velocityX": -2.565053412243361, + "velocityY": 0.24520568230062648, + "timestamp": 9.971557612354822 + }, + { + "x": 3.9326087471810762, + "y": 6.761949278304569, + "heading": 0.28828094811629446, + "angularVelocity": 0.4347490891368528, + "velocityX": -2.279853178740861, + "velocityY": 0.21799873955634386, + "timestamp": 10.037249564832132 + }, + { + "x": 3.8015715928550198, + "y": 6.774481693316805, + "heading": 0.31418958063772184, + "angularVelocity": 0.39439583608625584, + "velocityX": -1.9947215661053177, + "velocityY": 0.190775498970961, + "timestamp": 10.102941517309443 + }, + { + "x": 3.6892614596122306, + "y": 6.785224997460466, + "heading": 0.33704367479862785, + "angularVelocity": 0.3478979281183608, + "velocityX": -1.709648275130517, + "velocityY": 0.16354064293296997, + "timestamp": 10.168633469786753 + }, + { + "x": 3.595675147466301, + "y": 6.794178615472137, + "heading": 0.3565415622292007, + "angularVelocity": 0.29680785385861885, + "velocityX": -1.4246236961559957, + "velocityY": 0.1362970299103891, + "timestamp": 10.234325422264064 + }, + { + "x": 3.5208099799304895, + "y": 6.801342098064149, + "heading": 0.37244949483993084, + "angularVelocity": 0.24215953417162805, + "velocityX": -1.139639860174189, + "velocityY": 0.10904657757715387, + "timestamp": 10.300017374741374 + }, + { + "x": 3.4646637036902503, + "y": 6.806715087085814, + "heading": 0.3845814929922257, + "angularVelocity": 0.18468012739437525, + "velocityX": -0.8546903254189608, + "velocityY": 0.08179067327191092, + "timestamp": 10.365709327218685 + }, + { + "x": 3.427234407665484, + "y": 6.81029729456599, + "heading": 0.39278667084002433, + "angularVelocity": 0.12490385105592111, + "velocityX": -0.569769882204283, + "velocityY": 0.054530385307290076, + "timestamp": 10.431401279695995 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.06323746826387204, + "velocityX": -0.2848742780656992, + "velocityY": 0.02726658135331313, + "timestamp": 10.497093232173306 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 2.0447435740886937e-25, + "velocityX": -6.220702573449239e-25, + "velocityY": -1.1679895694126021e-23, + "timestamp": 10.562785184650616 + }, + { + "x": 3.4428034429755563, + "y": 6.811427919071949, + "heading": 0.392498131917024, + "angularVelocity": -0.05010058941962764, + "velocityX": 0.38660845854541764, + "velocityY": -0.007449238843783358, + "timestamp": 10.651461420488184 + }, + { + "x": 3.511369400623633, + "y": 6.8101068002836795, + "heading": 0.38361316145922575, + "angularVelocity": -0.1001956203246293, + "velocityX": 0.7732168263622722, + "velocityY": -0.014898228096758812, + "timestamp": 10.740137656325752 + }, + { + "x": 3.614218332538516, + "y": 6.808125167755098, + "heading": 0.3702880707403863, + "angularVelocity": -0.15026676079539045, + "velocityX": 1.159825188151597, + "velocityY": -0.022346827307951896, + "timestamp": 10.82881389216332 + }, + { + "x": 3.7513502451752516, + "y": 6.805483069346682, + "heading": 0.35252648355271166, + "angularVelocity": -0.20029703583955935, + "velocityX": 1.5464336227343458, + "velocityY": -0.029794886797588863, + "timestamp": 10.917490128000887 + }, + { + "x": 3.9227651511900996, + "y": 6.8021805671983415, + "heading": 0.3303333582647084, + "angularVelocity": -0.2502713954689655, + "velocityX": 1.9330422000414584, + "velocityY": -0.03724224553678828, + "timestamp": 11.006166363838455 + }, + { + "x": 4.128463068303133, + "y": 6.7982177388648735, + "heading": 0.3037147633556069, + "angularVelocity": -0.3001773209889048, + "velocityX": 2.319650977177454, + "velocityY": -0.044688729692209925, + "timestamp": 11.094842599676022 + }, + { + "x": 4.3684440175926715, + "y": 6.793594678506847, + "heading": 0.27267759681211456, + "angularVelocity": -0.3500054580614399, + "velocityX": 2.7062599920132113, + "velocityY": -0.05213415200092422, + "timestamp": 11.18351883551359 + }, + { + "x": 4.642708020388071, + "y": 6.788311498054621, + "heading": 0.2372292525531904, + "angularVelocity": -0.3997502140692617, + "velocityX": 3.092869247379649, + "velocityY": -0.059578312073416795, + "timestamp": 11.272195071351158 + }, + { + "x": 4.9512550879867705, + "y": 6.7823683283089595, + "heading": 0.19737727735435437, + "angularVelocity": -0.44940986525222537, + "velocityX": 3.47947863014707, + "velocityY": -0.0670209971084869, + "timestamp": 11.360871307188726 + }, + { + "x": 5.286562691805109, + "y": 6.775920770291965, + "heading": 0.19737727264687205, + "angularVelocity": -5.308617659691649e-8, + "velocityX": 3.781256620235165, + "velocityY": -0.07270897277153024, + "timestamp": 11.449547543026293 + }, + { + "x": 5.59511111399723, + "y": 6.769985794491263, + "heading": 0.15789296561858565, + "angularVelocity": -0.44526367921853854, + "velocityX": 3.479493905867875, + "velocityY": -0.06692859416780687, + "timestamp": 11.53822377886386 + }, + { + "x": 5.869376427077159, + "y": 6.764710552640504, + "heading": 0.12280140353680047, + "angularVelocity": -0.3957267891486041, + "velocityX": 3.0928840234300474, + "velocityY": -0.05948878863580233, + "timestamp": 11.626900014701429 + }, + { + "x": 6.109358599210802, + "y": 6.760094936637857, + "heading": 0.0920994176200499, + "angularVelocity": -0.3462256333589605, + "velocityX": 2.706273781999814, + "velocityY": -0.05205020216578816, + "timestamp": 11.715576250538996 + }, + { + "x": 6.3150576152845925, + "y": 6.7561388526360995, + "heading": 0.06578494429363466, + "angularVelocity": -0.2967477484567179, + "velocityX": 2.3196633701342284, + "velocityY": -0.04461267400889573, + "timestamp": 11.804252486376564 + }, + { + "x": 6.486473466938695, + "y": 6.752842221807213, + "heading": 0.04385677628651476, + "angularVelocity": -0.24728347792396857, + "velocityX": 1.9330528639949482, + "velocityY": -0.037176034793867205, + "timestamp": 11.892928722214132 + }, + { + "x": 6.623606149728922, + "y": 6.750204980774611, + "heading": 0.02631430841636879, + "angularVelocity": -0.19782603201921237, + "velocityX": 1.546442307738673, + "velocityY": -0.029740110275233655, + "timestamp": 11.9816049580517 + }, + { + "x": 6.7264556616555025, + "y": 6.748227081861227, + "heading": 0.013157325998979796, + "angularVelocity": -0.14837100710374293, + "velocityX": 1.1598317289310056, + "velocityY": -0.022304723409858212, + "timestamp": 12.070281193889267 + }, + { + "x": 6.7950220022425825, + "y": 6.746908493222859, + "heading": 0.004385844089357537, + "angularVelocity": -0.09891581241325288, + "velocityX": 0.7732211447571573, + "velocityY": -0.01486969565085219, + "timestamp": 12.158957429726835 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -2.582999463513829e-28, + "angularVelocity": -0.04945906925268308, + "velocityX": 0.3866105659555495, + "velocityY": -0.007434847713796995, + "timestamp": 12.247633665564402 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -1.1865093331139584e-28, + "angularVelocity": 5.378776164649684e-29, + "velocityX": -2.235889133711042e-27, + "velocityY": -8.75203817649461e-27, + "timestamp": 12.33630990140197 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEf.1.traj b/src/main/deploy/choreo/AmpLanePADEf.1.traj deleted file mode 100644 index b5205101..00000000 --- a/src/main/deploy/choreo/AmpLanePADEf.1.traj +++ /dev/null @@ -1,130 +0,0 @@ -{ - "samples": [ - { - "x": 1.451512098312378, - "y": 7.362619876861572, - "heading": 0, - "angularVelocity": -1.626357329534646e-41, - "velocityX": 6.883179938389857e-43, - "velocityY": -3.544509713626213e-42, - "timestamp": 0 - }, - { - "x": 1.471098087600976, - "y": 7.348406138802381, - "heading": 0.01253561485916891, - "angularVelocity": 0.15858130999249215, - "velocityX": 0.2477717984940342, - "velocityY": -0.17981034250331, - "timestamp": 0.07904850111127469 - }, - { - "x": 1.5102697358053931, - "y": 7.319977714643011, - "heading": 0.03761033729519426, - "angularVelocity": 0.31720680447473965, - "velocityX": 0.4955394176200278, - "velocityY": -0.3596326781623792, - "timestamp": 0.15809700222254938 - }, - { - "x": 1.569026235907136, - "y": 7.277333432043545, - "heading": 0.07524357427525706, - "angularVelocity": 0.47607780604324657, - "velocityX": 0.7432968275898469, - "velocityY": -0.5394698444621486, - "timestamp": 0.23714550333382406 - }, - { - "x": 1.6473662329442567, - "y": 7.220472042003613, - "heading": 0.12547694655707634, - "angularVelocity": 0.6354753293943798, - "velocityX": 0.9910370966660474, - "velocityY": -0.7193228111927129, - "timestamp": 0.31619400444509876 - }, - { - "x": 1.745287770045571, - "y": 7.149392472942429, - "heading": 0.18838242237158065, - "angularVelocity": 0.7957832840619425, - "velocityX": 1.2387526104191713, - "velocityY": -0.8991893339144678, - "timestamp": 0.39524250555637347 - }, - { - "x": 1.8627883050488596, - "y": 7.064094226316957, - "heading": 0.2640709709173305, - "angularVelocity": 0.957495050275588, - "velocityX": 1.486435964647655, - "velocityY": -1.0790621634355757, - "timestamp": 0.4742910066676482 - }, - { - "x": 1.9802240062556191, - "y": 6.978809171813768, - "heading": 0.34336916171885673, - "angularVelocity": 1.003158689750487, - "velocityX": 1.4856157872171172, - "velocityY": -1.0788952770038656, - "timestamp": 0.5533395077789228 - }, - { - "x": 2.078085335558388, - "y": 6.907741106237698, - "heading": 0.40961857461491347, - "angularVelocity": 0.8380856305269982, - "velocityX": 1.2379909539968612, - "velocityY": -0.899043809521805, - "timestamp": 0.6323880088901975 - }, - { - "x": 2.1563737104997935, - "y": 6.850888499073482, - "heading": 0.46271135866543855, - "angularVelocity": 0.6716482071657204, - "velocityX": 0.9903840533446798, - "velocityY": -0.7192117037638214, - "timestamp": 0.7114365100014722 - }, - { - "x": 2.2150899115476355, - "y": 6.808249956097028, - "heading": 0.5025701306982392, - "angularVelocity": 0.5042318509833907, - "velocityX": 0.7427870259701495, - "velocityY": -0.5393972355836724, - "timestamp": 0.7904850111127469 - }, - { - "x": 2.25423422583693, - "y": 6.779824483661568, - "heading": 0.529148704819503, - "angularVelocity": 0.33623122194119537, - "velocityX": 0.49519363098601865, - "velocityY": -0.3595953374934483, - "timestamp": 0.8695335122240216 - }, - { - "x": 2.273806571960449, - "y": 6.76561164855957, - "heading": 0.5424312320356962, - "angularVelocity": 0.1680300958204839, - "velocityX": 0.24759920616290554, - "velocityY": -0.1797989196783214, - "timestamp": 0.9485820133352963 - }, - { - "x": 2.273806571960449, - "y": 6.76561164855957, - "heading": 0.5424312320356962, - "angularVelocity": 4.2126535792079476e-33, - "velocityX": -2.203749386654579e-34, - "velocityY": -1.145242422674013e-34, - "timestamp": 1.027630514446571 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEf.2.traj b/src/main/deploy/choreo/AmpLanePADEf.2.traj deleted file mode 100644 index 56f90c42..00000000 --- a/src/main/deploy/choreo/AmpLanePADEf.2.traj +++ /dev/null @@ -1,103 +0,0 @@ -{ - "samples": [ - { - "x": 2.273806571960449, - "y": 6.76561164855957, - "heading": 0.5424312320356962, - "angularVelocity": 4.2126535792079476e-33, - "velocityX": -2.203749386654579e-34, - "velocityY": -1.145242422674013e-34, - "timestamp": 0 - }, - { - "x": 2.3052968816975965, - "y": 6.784998971806612, - "heading": 0.542413982759797, - "angularVelocity": -0.00017723560938373895, - "velocityX": 0.3235616537503915, - "velocityY": 0.19920395905812027, - "timestamp": 0.097323985621115 - }, - { - "x": 2.3682775001422844, - "y": 6.823773617666811, - "heading": 0.5423794842069812, - "angularVelocity": -0.0003544712292205405, - "velocityX": 0.6471232969216173, - "velocityY": 0.39840791160310296, - "timestamp": 0.19464797124223 - }, - { - "x": 2.4627484254412217, - "y": 6.881935584999175, - "heading": 0.5423277363751632, - "angularVelocity": -0.00053170687048794, - "velocityX": 0.9706849210503492, - "velocityY": 0.5976118524244335, - "timestamp": 0.291971956863345 - }, - { - "x": 2.588709653270064, - "y": 6.959484871141385, - "heading": 0.5422587392604604, - "angularVelocity": -0.0007089425516464455, - "velocityX": 1.2942465007466153, - "velocityY": 0.7968157658905536, - "timestamp": 0.38929594248446 - }, - { - "x": 2.746161162007105, - "y": 7.056421462781845, - "heading": 0.5421724928506289, - "angularVelocity": -0.0008861783586156058, - "velocityX": 1.6178078582807358, - "velocityY": 0.996019542580562, - "timestamp": 0.486619928105575 - }, - { - "x": 2.8721223898359267, - "y": 7.133970748924088, - "heading": 0.5421034957131426, - "angularVelocity": -0.0007089427857476058, - "velocityX": 1.2942465007464055, - "velocityY": 0.7968157658908929, - "timestamp": 0.58394391372669 - }, - { - "x": 2.9665933151348423, - "y": 7.192132716256486, - "heading": 0.5420517478572688, - "angularVelocity": -0.0005317071176599402, - "velocityX": 0.9706849210501236, - "velocityY": 0.5976118524247968, - "timestamp": 0.681267899347805 - }, - { - "x": 3.029573933579513, - "y": 7.230907362116713, - "heading": 0.5420172492858153, - "angularVelocity": -0.00035447142072369024, - "velocityX": 0.6471232969214384, - "velocityY": 0.3984079116033909, - "timestamp": 0.77859188496892 - }, - { - "x": 3.0610642433166504, - "y": 7.2502946853637695, - "heading": 0.542, - "angularVelocity": -0.0001772357112703998, - "velocityX": 0.3235616537502931, - "velocityY": 0.19920395905827845, - "timestamp": 0.875915870590035 - }, - { - "x": 3.0610642433166504, - "y": 7.2502946853637695, - "heading": 0.542, - "angularVelocity": 8.233407245564493e-29, - "velocityX": 8.650056098040804e-30, - "velocityY": 3.151108217990947e-30, - "timestamp": 0.97323985621115 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEf.3.traj b/src/main/deploy/choreo/AmpLanePADEf.3.traj deleted file mode 100644 index c3305be0..00000000 --- a/src/main/deploy/choreo/AmpLanePADEf.3.traj +++ /dev/null @@ -1,463 +0,0 @@ -{ - "samples": [ - { - "x": 3.0610642433166504, - "y": 7.2502946853637695, - "heading": 0.542, - "angularVelocity": 8.233407245564493e-29, - "velocityX": 8.650056098040804e-30, - "velocityY": 3.151108217990947e-30, - "timestamp": 0 - }, - { - "x": 3.0887984705253144, - "y": 7.254141628194322, - "heading": 0.5386292393754978, - "angularVelocity": -0.03979388904876881, - "velocityX": 0.3274195006260423, - "velocityY": 0.04541551098718751, - "timestamp": 0.08470548380790399 - }, - { - "x": 3.144266916294855, - "y": 7.26183552381785, - "heading": 0.5318876375624417, - "angularVelocity": -0.07958872920539878, - "velocityX": 0.6548388991596666, - "velocityY": 0.09083113958686537, - "timestamp": 0.16941096761580798 - }, - { - "x": 3.2274695749900846, - "y": 7.273376389298639, - "heading": 0.5217760579061892, - "angularVelocity": -0.1193733770424954, - "velocityX": 0.9822582311664592, - "velocityY": 0.1362469696408709, - "timestamp": 0.254116451423712 - }, - { - "x": 3.3384064437500833, - "y": 7.288764249060912, - "heading": 0.5082962561247577, - "angularVelocity": -0.15913729755547365, - "velocityX": 1.3096775293983471, - "velocityY": 0.1816630880377333, - "timestamp": 0.33882193523161597 - }, - { - "x": 3.4770775221460872, - "y": 7.307999135190426, - "heading": 0.4914508070037636, - "angularVelocity": -0.19887082115079507, - "velocityX": 1.6370968225684077, - "velocityY": 0.22707958522658897, - "timestamp": 0.42352741903951996 - }, - { - "x": 3.6434828117292577, - "y": 7.331081087741682, - "heading": 0.4712430079600464, - "angularVelocity": -0.2385654167252698, - "velocityX": 1.964516134050886, - "velocityY": 0.27249655528328975, - "timestamp": 0.508232902847424 - }, - { - "x": 3.837622315458121, - "y": 7.358010154988738, - "heading": 0.4476767580715169, - "angularVelocity": -0.27821398130227654, - "velocityX": 2.2919354804605674, - "velocityY": 0.31791409524475356, - "timestamp": 0.5929383866553279 - }, - { - "x": 4.059496036986131, - "y": 7.3887863935071385, - "heading": 0.4207564114260032, - "angularVelocity": -0.31781114321168447, - "velocityX": 2.6193548700015095, - "velocityY": 0.3633323031151034, - "timestamp": 0.6776438704632319 - }, - { - "x": 4.3091039797632575, - "y": 7.423409867844704, - "heading": 0.3904866045233033, - "angularVelocity": -0.35735356840760346, - "velocityX": 2.9467743002714557, - "velocityY": 0.40875127301190084, - "timestamp": 0.7623493542711359 - }, - { - "x": 4.586446145814129, - "y": 7.4618806491187275, - "heading": 0.3568720602117417, - "angularVelocity": -0.39684023749291464, - "velocityX": 3.2741937544433752, - "velocityY": 0.4541710824918035, - "timestamp": 0.8470548380790399 - }, - { - "x": 4.8915225336365555, - "y": 7.504198810004116, - "heading": 0.31991738183873225, - "angularVelocity": -0.436272561248892, - "velocityX": 3.6016131908810025, - "velocityY": 0.4995917499434115, - "timestamp": 0.9317603218869439 - }, - { - "x": 5.224333131274253, - "y": 7.55036439862341, - "heading": 0.2796269341485423, - "angularVelocity": -0.47565335653149277, - "velocityX": 3.9290324861674275, - "velocityY": 0.5450129855109993, - "timestamp": 1.016465805694848 - }, - { - "x": 5.579793390330806, - "y": 7.592855289621793, - "heading": 0.2796269285432754, - "angularVelocity": -6.617360068351878e-8, - "velocityX": 4.196425580457948, - "velocityY": 0.5016309344825798, - "timestamp": 1.101171289502752 - }, - { - "x": 5.931486953635787, - "y": 7.608007053524113, - "heading": 0.2595234955759573, - "angularVelocity": -0.23733331142057107, - "velocityX": 4.151957435276098, - "velocityY": 0.17887583213235417, - "timestamp": 1.1858767733106559 - }, - { - "x": 6.255598185321619, - "y": 7.618552102175372, - "heading": 0.23287181404120497, - "angularVelocity": -0.3146393874022023, - "velocityX": 3.826331154907898, - "velocityY": 0.1244907434230404, - "timestamp": 1.2705822571185599 - }, - { - "x": 6.5520193977896835, - "y": 7.624877033279807, - "heading": 0.20472915346535583, - "angularVelocity": -0.33224130612140174, - "velocityX": 3.4994335566299437, - "velocityY": 0.07466967686265122, - "timestamp": 1.3552877409264639 - }, - { - "x": 6.8207292064754315, - "y": 7.627107624996143, - "heading": 0.17669875637349494, - "angularVelocity": -0.3309159670883467, - "velocityX": 3.1722835004993724, - "velocityY": 0.02633349832923241, - "timestamp": 1.4399932247343679 - }, - { - "x": 7.0617193847944275, - "y": 7.625306131272256, - "heading": 0.1495692247668899, - "angularVelocity": -0.3202806995146003, - "velocityX": 2.8450363245122436, - "velocityY": -0.021267734306042424, - "timestamp": 1.5246987085422719 - }, - { - "x": 7.274985760405132, - "y": 7.619509682191873, - "heading": 0.12380998991728469, - "angularVelocity": -0.30410350890784327, - "velocityX": 2.5177398914835423, - "velocityY": -0.06843062361266532, - "timestamp": 1.6094041923501758 - }, - { - "x": 7.460525865716125, - "y": 7.609742928512009, - "heading": 0.0997327292133379, - "angularVelocity": -0.2842467762615057, - "velocityX": 2.19041432703016, - "velocityY": -0.1153024956686039, - "timestamp": 1.6941096761580798 - }, - { - "x": 7.618338091434345, - "y": 7.59602342195819, - "heading": 0.07755965741979146, - "angularVelocity": -0.26176666252165137, - "velocityX": 1.8630697638887466, - "velocityY": -0.1619671588788346, - "timestamp": 1.7788151599659838 - }, - { - "x": 7.7484213145990894, - "y": 7.578364292552694, - "heading": 0.05745736086921445, - "angularVelocity": -0.23731989532468997, - "velocityX": 1.5357119435100053, - "velocityY": -0.20847681415229657, - "timestamp": 1.8635206437738878 - }, - { - "x": 7.850774711771762, - "y": 7.55677573093629, - "heading": 0.039555451052523435, - "angularVelocity": -0.21134298527170414, - "velocityX": 1.208344401935423, - "velocityY": -0.25486616268396156, - "timestamp": 1.9482261275817918 - }, - { - "x": 7.925397655711937, - "y": 7.531265875463379, - "heading": 0.023957677737365072, - "angularVelocity": -0.18414124581002833, - "velocityX": 0.880969455404328, - "velocityY": -0.3011594329685677, - "timestamp": 2.0329316113896962 - }, - { - "x": 7.972289653892672, - "y": 7.501841375568497, - "heading": 0.010748952471097088, - "angularVelocity": -0.15593707364223489, - "velocityX": 0.5535886942916515, - "velocityY": -0.3473742026149794, - "timestamp": 2.1176370951976007 - }, - { - "x": 7.991450309753418, - "y": 7.468507766723633, - "heading": 0, - "angularVelocity": -0.12689795262461315, - "velocityX": 0.22620325153938764, - "velocityY": -0.39352362263184965, - "timestamp": 2.202342579005505 - }, - { - "x": 7.979625066028202, - "y": 7.427776895642819, - "heading": -0.00869243368919643, - "angularVelocity": -0.09464796670145847, - "velocityX": -0.1287597138354107, - "velocityY": -0.4434999756945785, - "timestamp": 2.294182203161107 - }, - { - "x": 7.935198288089413, - "y": 7.3824696790264746, - "heading": -0.014418639723911789, - "angularVelocity": -0.06235005954253872, - "velocityX": -0.48374302864455354, - "velocityY": -0.4933297259533529, - "timestamp": 2.3860218273167093 - }, - { - "x": 7.858167736484691, - "y": 7.332602290488192, - "heading": -0.01717387059125372, - "angularVelocity": -0.030000458869875093, - "velocityX": -0.8387507278344164, - "velocityY": -0.5429833690716247, - "timestamp": 2.4778614514723114 - }, - { - "x": 7.7485306804365655, - "y": 7.278194518720073, - "heading": -0.01695283231692861, - "angularVelocity": 0.002406785486756842, - "velocityX": -1.193788161221191, - "velocityY": -0.5924215420997102, - "timestamp": 2.5697010756279135 - }, - { - "x": 7.6062837180875285, - "y": 7.219271127781688, - "heading": -0.013749356302551094, - "angularVelocity": 0.03488119691221441, - "velocityX": -1.5488626358909692, - "velocityY": -0.6415900705185047, - "timestamp": 2.6615406997835156 - }, - { - "x": 7.431422496644012, - "y": 7.155863998893124, - "heading": -0.007555863200914232, - "angularVelocity": 0.06743813641027274, - "velocityX": -1.9039845061550473, - "velocityY": -0.6904114587962236, - "timestamp": 2.7533803239391177 - }, - { - "x": 7.223941251479605, - "y": 7.0880157090079505, - "heading": 0.0016375412086007608, - "angularVelocity": 0.10010280958712602, - "velocityX": -2.2591691448207247, - "velocityY": -0.738769245943591, - "timestamp": 2.84521994809472 - }, - { - "x": 6.9838319898121135, - "y": 7.015785993604, - "heading": 0.013844691092692804, - "angularVelocity": 0.13291811672788056, - "velocityX": -2.6144408132654218, - "velocityY": -0.786476600574666, - "timestamp": 2.937059572250322 - }, - { - "x": 6.711082895811614, - "y": 6.939264732321087, - "heading": 0.029086591938052034, - "angularVelocity": 0.16596214308193039, - "velocityX": -2.969841138921331, - "velocityY": -0.8332052965859897, - "timestamp": 3.028899196405924 - }, - { - "x": 6.405674745772718, - "y": 6.858602462265845, - "heading": 0.04739892839527075, - "angularVelocity": 0.199394723404341, - "velocityX": -3.325450782783501, - "velocityY": -0.8782948623414927, - "timestamp": 3.120738820561526 - }, - { - "x": 6.067570892053079, - "y": 6.774103037070206, - "heading": 0.06885462737025146, - "angularVelocity": 0.2336213717145368, - "velocityX": -3.681459466196236, - "velocityY": -0.9200759037565454, - "timestamp": 3.212578444717128 - }, - { - "x": 5.696676642086654, - "y": 6.686700262340111, - "heading": 0.0936784749866653, - "angularVelocity": 0.27029561412875813, - "velocityX": -4.038499213998925, - "velocityY": -0.9516891595936975, - "timestamp": 3.3044180688727303 - }, - { - "x": 5.339135311514303, - "y": 6.629362903719828, - "heading": 0.12448875151110148, - "angularVelocity": 0.3354791225308384, - "velocityX": -3.893105332908111, - "velocityY": -0.6243204841851022, - "timestamp": 3.3962576930283324 - }, - { - "x": 5.013882344908921, - "y": 6.578461952217419, - "heading": 0.15282110086738224, - "angularVelocity": 0.3084980978280288, - "velocityX": -3.541531986829529, - "velocityY": -0.5542373672623276, - "timestamp": 3.4880973171839345 - }, - { - "x": 4.721071580025458, - "y": 6.533164538371719, - "heading": 0.17845671775380376, - "angularVelocity": 0.27913460146500385, - "velocityX": -3.188283571233179, - "velocityY": -0.4932229880309821, - "timestamp": 3.5799369413395365 - }, - { - "x": 4.4607505833449235, - "y": 6.4931855756353425, - "heading": 0.20132072068763487, - "angularVelocity": 0.24895575460958952, - "velocityX": -2.834517225805949, - "velocityY": -0.43531278687094876, - "timestamp": 3.6717765654951386 - }, - { - "x": 4.232942393025197, - "y": 6.458381106409632, - "heading": 0.22137489645108566, - "angularVelocity": 0.21836082136266488, - "velocityX": -2.480500028329932, - "velocityY": -0.37897007468939997, - "timestamp": 3.7636161896507407 - }, - { - "x": 4.0376605870531215, - "y": 6.428664298965915, - "heading": 0.2385958332745625, - "angularVelocity": 0.1875109679783531, - "velocityX": -2.1263349863154883, - "velocityY": -0.3235728338060169, - "timestamp": 3.855455813806343 - }, - { - "x": 3.874914113830387, - "y": 6.403977073661965, - "heading": 0.2529676231231021, - "angularVelocity": 0.15648789921866602, - "velocityX": -1.7720725092149265, - "velocityY": -0.2688079957961907, - "timestamp": 3.947295437961945 - }, - { - "x": 3.744709313892485, - "y": 6.384277851345689, - "heading": 0.2644787572649854, - "angularVelocity": 0.12533951710070187, - "velocityX": -1.4177409929000644, - "velocityY": -0.21449589430914948, - "timestamp": 4.039135062117547 - }, - { - "x": 3.6470509145783416, - "y": 6.369535397539679, - "heading": 0.27312059313662507, - "angularVelocity": 0.09409703002889537, - "velocityX": -1.0633580027364409, - "velocityY": -0.16052389087555669, - "timestamp": 4.130974686273149 - }, - { - "x": 3.5819425761583696, - "y": 6.359725390676191, - "heading": 0.2788865130907595, - "angularVelocity": 0.06278248639842567, - "velocityX": -0.7089351575489377, - "velocityY": -0.10681671395852226, - "timestamp": 4.222814310428751 - }, - { - "x": 3.549387216567993, - "y": 6.354828357696533, - "heading": 0.2817714218737248, - "angularVelocity": 0.031412462862481436, - "velocityX": -0.3544805402864683, - "velocityY": -0.05332157034267822, - "timestamp": 4.314653934584353 - }, - { - "x": 3.549387216567993, - "y": 6.354828357696533, - "heading": 0.2817714218737248, - "angularVelocity": -1.3478129621099947e-29, - "velocityX": -2.7087222342243667e-31, - "velocityY": -4.691094059490074e-29, - "timestamp": 4.406493558739955 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEf.4.traj b/src/main/deploy/choreo/AmpLanePADEf.4.traj deleted file mode 100644 index 0a361935..00000000 --- a/src/main/deploy/choreo/AmpLanePADEf.4.traj +++ /dev/null @@ -1,661 +0,0 @@ -{ - "samples": [ - { - "x": 3.549387216567993, - "y": 6.354828357696533, - "heading": 0.2817714218737248, - "angularVelocity": -1.3478129621099947e-29, - "velocityX": -2.7087222342243667e-31, - "velocityY": -4.691094059490074e-29, - "timestamp": 0 - }, - { - "x": 3.5643145112589276, - "y": 6.359340723434126, - "heading": 0.2784812557968366, - "angularVelocity": -0.0520228583688743, - "velocityX": 0.23602472318704046, - "velocityY": 0.07134781594289749, - "timestamp": 0.06324462323002056 - }, - { - "x": 3.59421509509005, - "y": 6.368209865282409, - "heading": 0.2718895474558649, - "angularVelocity": -0.10422559269580298, - "velocityX": 0.47277669316448045, - "velocityY": 0.140235507705144, - "timestamp": 0.12648924646004112 - }, - { - "x": 3.6391396771921247, - "y": 6.38125694268572, - "heading": 0.26198366744840057, - "angularVelocity": -0.15662801834452575, - "velocityX": 0.7103304566885299, - "velocityY": 0.20629544041805936, - "timestamp": 0.18973386969006167 - }, - { - "x": 3.6991442631106195, - "y": 6.398274326559935, - "heading": 0.24874957308777174, - "angularVelocity": -0.20925248162988414, - "velocityX": 0.9487697586600851, - "velocityY": 0.2690724207862332, - "timestamp": 0.25297849292008223 - }, - { - "x": 3.774290733668231, - "y": 6.419018192662677, - "heading": 0.23217163029829782, - "angularVelocity": -0.26212414499142417, - "velocityX": 1.1881874967347645, - "velocityY": 0.32799414469267935, - "timestamp": 0.3162231161501028 - }, - { - "x": 3.8646473183010075, - "y": 6.4431984071841955, - "heading": 0.21223243762750219, - "angularVelocity": -0.31527095352085177, - "velocityX": 1.42868405277947, - "velocityY": 0.38232838281879794, - "timestamp": 0.37946773938012335 - }, - { - "x": 3.9702887319682127, - "y": 6.47046438647985, - "heading": 0.18891269789783047, - "angularVelocity": -0.36872288170423295, - "velocityX": 1.6703619734279613, - "velocityY": 0.4311193252980343, - "timestamp": 0.4427123626101439 - }, - { - "x": 4.091295462853014, - "y": 6.5003847972803275, - "heading": 0.1621912387892295, - "angularVelocity": -0.4225095785837003, - "velocityX": 1.9133125427073783, - "velocityY": 0.4730901896854845, - "timestamp": 0.5059569858401645 - }, - { - "x": 4.227751057788416, - "y": 6.532417534961208, - "heading": 0.13204541033302933, - "angularVelocity": -0.47665440817885446, - "velocityX": 2.1575841228291166, - "velocityY": 0.506489501318997, - "timestamp": 0.569201609070185 - }, - { - "x": 4.379734734514181, - "y": 6.565863853542482, - "heading": 0.09845238767865087, - "angularVelocity": -0.5311601356561273, - "velocityX": 2.403108263812402, - "velocityY": 0.5288405064827253, - "timestamp": 0.6324462323002056 - }, - { - "x": 4.547302866246675, - "y": 6.599795890388135, - "heading": 0.061392656400376396, - "angularVelocity": -0.5859744178329315, - "velocityX": 2.6495237567160146, - "velocityY": 0.5365204994935601, - "timestamp": 0.6956908555302261 - }, - { - "x": 4.730442949191217, - "y": 6.632938881927366, - "heading": 0.02085892319849224, - "angularVelocity": -0.640904018899171, - "velocityX": 2.8957415443596144, - "velocityY": 0.5240444143163087, - "timestamp": 0.7589354787602467 - }, - { - "x": 4.928956637722946, - "y": 6.663479274492677, - "heading": -0.02312096955214101, - "angularVelocity": -0.6953933868920108, - "velocityX": 3.1388231661960395, - "velocityY": 0.48289310625244986, - "timestamp": 0.8221801019902673 - }, - { - "x": 5.142158150786473, - "y": 6.688780976787343, - "heading": -0.07042466867749009, - "angularVelocity": -0.7479481528936539, - "velocityX": 3.3710614780344836, - "velocityY": 0.40006092221695244, - "timestamp": 0.8854247252202878 - }, - { - "x": 5.368156918828487, - "y": 6.705163329380271, - "heading": -0.12069128737602382, - "angularVelocity": -0.7947967136386285, - "velocityX": 3.573406821004469, - "velocityY": 0.2590315469719096, - "timestamp": 0.9486693484503084 - }, - { - "x": 5.602787591868942, - "y": 6.70854231082706, - "heading": -0.17306941127730663, - "angularVelocity": -0.828183033216651, - "velocityX": 3.7098912296006943, - "velocityY": 0.05342717331872859, - "timestamp": 1.011913971680329 - }, - { - "x": 5.840023517608643, - "y": 6.6965250968933105, - "heading": -0.22619108927305293, - "angularVelocity": -0.8399398286008083, - "velocityX": 3.7510844973630975, - "velocityY": -0.19001162976404115, - "timestamp": 1.0751585949103495 - }, - { - "x": 6.044562434699064, - "y": 6.674397447942719, - "heading": -0.2720344485514847, - "angularVelocity": -0.8324214431482473, - "velocityX": 3.7140075078331716, - "velocityY": -0.4017927517278426, - "timestamp": 1.1302308899068008 - }, - { - "x": 6.24348632132104, - "y": 6.641849320208312, - "heading": -0.31657574679893374, - "angularVelocity": -0.808778683552582, - "velocityX": 3.612050063190479, - "velocityY": -0.5910072884470288, - "timestamp": 1.185303184903252 - }, - { - "x": 6.4336299472398455, - "y": 6.6013719538289495, - "heading": -0.3590556305763903, - "angularVelocity": -0.7713476218885391, - "velocityX": 3.4526185249962773, - "velocityY": -0.7349860103337076, - "timestamp": 1.2403754798997033 - }, - { - "x": 6.613077092610287, - "y": 6.55585155346098, - "heading": -0.39905440385271473, - "angularVelocity": -0.7262957405153002, - "velocityX": 3.258392361930904, - "velocityY": -0.8265571712764752, - "timestamp": 1.2954477748961546 - }, - { - "x": 6.7809899014103525, - "y": 6.507733437798222, - "heading": -0.4364070087513559, - "angularVelocity": -0.6782467464093925, - "velocityX": 3.0489524507901287, - "velocityY": -0.8737263567072827, - "timestamp": 1.3505200698926059 - }, - { - "x": 6.937105965748617, - "y": 6.458836458241438, - "heading": -0.4710688606060818, - "angularVelocity": -0.6293881861462208, - "velocityX": 2.834747750176842, - "velocityY": -0.8878689286497744, - "timestamp": 1.4055923648890571 - }, - { - "x": 7.081411799592395, - "y": 6.41047495052379, - "heading": -0.5030439244323226, - "angularVelocity": -0.5806016224364979, - "velocityX": 2.620298170851195, - "velocityY": -0.8781458575634948, - "timestamp": 1.4606646598855084 - }, - { - "x": 7.213990143110089, - "y": 6.363610015779139, - "heading": -0.5323539451412649, - "angularVelocity": -0.532209901745171, - "velocityX": 2.4073509833980524, - "velocityY": -0.8509711597759041, - "timestamp": 1.5157369548819597 - }, - { - "x": 7.334955224032392, - "y": 6.318961770507673, - "heading": -0.5590259257379866, - "angularVelocity": -0.4843085002802334, - "velocityX": 2.1964779374111463, - "velocityY": -0.8107206223082474, - "timestamp": 1.570809249878411 - }, - { - "x": 7.444426209898462, - "y": 6.27708400760693, - "heading": -0.583087151971681, - "angularVelocity": -0.4369025520952958, - "velocityX": 1.987768729687496, - "velocityY": -0.7604143408848628, - "timestamp": 1.6258815448748623 - }, - { - "x": 7.542516765240442, - "y": 6.238413025121382, - "heading": -0.6045633303006558, - "angularVelocity": -0.3899633805048192, - "velocityX": 1.78112343689872, - "velocityY": -0.7021857812179472, - "timestamp": 1.6809538398713135 - }, - { - "x": 7.629331416896003, - "y": 6.203299971030318, - "heading": -0.6234780189407864, - "angularVelocity": -0.34345197782931597, - "velocityX": 1.576376137242069, - "velocityY": -0.6375810939661717, - "timestamp": 1.7360261348677648 - }, - { - "x": 7.704964792064279, - "y": 6.172032753280793, - "heading": -0.639852590711617, - "angularVelocity": -0.29732866175063044, - "velocityX": 1.3733470735721198, - "velocityY": -0.567748588496981, - "timestamp": 1.791098429864216 - }, - { - "x": 7.769502031127833, - "y": 6.144851250422818, - "heading": -0.653706402402048, - "angularVelocity": -0.25155682528436213, - "velocityX": 1.1718639847442827, - "velocityY": -0.49356038021888754, - "timestamp": 1.8461707248606674 - }, - { - "x": 7.823019631820591, - "y": 6.121958127000515, - "heading": -0.665057029957846, - "angularVelocity": -0.20610413196924784, - "velocityX": 0.9717699379734768, - "velocityY": -0.41569219920429124, - "timestamp": 1.9012430198571186 - }, - { - "x": 7.865586399595873, - "y": 6.1035266968155515, - "heading": -0.6739205091019282, - "angularVelocity": -0.16094261451521974, - "velocityX": 0.7729252572100992, - "velocityY": -0.3346769947784376, - "timestamp": 1.95631531485357 - }, - { - "x": 7.89726436642432, - "y": 6.089706757076858, - "heading": -0.6803115566586851, - "angularVelocity": -0.11604832442825835, - "velocityX": 0.575206949891734, - "velocityY": -0.25094178006534995, - "timestamp": 2.011387609850021 - }, - { - "x": 7.918109625191074, - "y": 6.080628997223027, - "heading": -0.6842437638150144, - "angularVelocity": -0.0714008224386257, - "velocityX": 0.3785071743986183, - "velocityY": -0.1648335130107772, - "timestamp": 2.0664599048464725 - }, - { - "x": 7.928173065185547, - "y": 6.076408386230469, - "heading": -0.6857297596086507, - "angularVelocity": -0.026982637889560627, - "velocityX": 0.18273144409763778, - "velocityY": -0.07663764498701091, - "timestamp": 2.1215321998429237 - }, - { - "x": 7.926985551108671, - "y": 6.077403500536459, - "heading": -0.6846299180981561, - "angularVelocity": 0.019139958746209124, - "velocityX": -0.020665677940935028, - "velocityY": 0.01731744672543921, - "timestamp": 2.1789953075266046 - }, - { - "x": 7.914061487019017, - "y": 6.08369055810287, - "heading": -0.6808709179866076, - "angularVelocity": 0.06541588617588866, - "velocityX": -0.2249106358952533, - "velocityY": 0.10941032985930138, - "timestamp": 2.2364584152102855 - }, - { - "x": 7.889347344730943, - "y": 6.09514891705372, - "heading": -0.6744435459188646, - "angularVelocity": 0.11185214874078991, - "velocityX": -0.43008711648732323, - "velocityY": 0.19940374638150343, - "timestamp": 2.2939215228939664 - }, - { - "x": 7.852784107363759, - "y": 6.111641566888316, - "heading": -0.6653381137940755, - "angularVelocity": 0.1584570081888393, - "velocityX": -0.6362906365672962, - "velocityY": 0.28701284179380265, - "timestamp": 2.3513846305776473 - }, - { - "x": 7.8043064822748365, - "y": 6.1330116259943965, - "heading": -0.6535443768551544, - "angularVelocity": 0.20524015171338303, - "velocityX": -0.8436304098932481, - "velocityY": 0.37189180967581936, - "timestamp": 2.4088477382613283 - }, - { - "x": 7.743842000412036, - "y": 6.159077789214998, - "heading": -0.6390514483000621, - "angularVelocity": 0.2522127524823789, - "velocityX": -1.0522313237154144, - "velocityY": 0.45361562002682787, - "timestamp": 2.466310845945009 - }, - { - "x": 7.671310010243926, - "y": 6.189628312245146, - "heading": -0.6218477227565106, - "angularVelocity": 0.29938731539292485, - "velocityX": -1.2622357733831673, - "velocityY": 0.5316545564907617, - "timestamp": 2.52377395362869 - }, - { - "x": 7.586620608820786, - "y": 6.224412915379664, - "heading": -0.6019208328060185, - "angularVelocity": 0.3467771019309393, - "velocityX": -1.4738047564244807, - "velocityY": 0.6053380079267261, - "timestamp": 2.581237061312371 - }, - { - "x": 7.4896736303193014, - "y": 6.263131662730417, - "heading": -0.579257685947244, - "angularVelocity": 0.39439473032905314, - "velocityX": -1.6871168721878604, - "velocityY": 0.6738018341070235, - "timestamp": 2.638700168996052 - }, - { - "x": 7.380357986253064, - "y": 6.305419339829435, - "heading": -0.5538446760345179, - "angularVelocity": 0.4422491392672011, - "velocityX": -1.9023622019886608, - "velocityY": 0.7359100265130355, - "timestamp": 2.6961632766797328 - }, - { - "x": 7.2585520430714885, - "y": 6.350822964880551, - "heading": -0.5256682599840283, - "angularVelocity": 0.49033923131330814, - "velocityX": -2.11972425598849, - "velocityY": 0.7901352168603886, - "timestamp": 2.7536263843634137 - }, - { - "x": 7.124126623207013, - "y": 6.39876857667167, - "heading": -0.49471629853932575, - "angularVelocity": 0.5386405764039992, - "velocityX": -2.3393343187154714, - "velocityY": 0.8343720645087159, - "timestamp": 2.8110894920470946 - }, - { - "x": 6.976954343667873, - "y": 6.448510970245314, - "heading": -0.4609810242297525, - "angularVelocity": 0.5870770946687629, - "velocityX": -2.5611611601183455, - "velocityY": 0.8656405053388868, - "timestamp": 2.8685525997307755 - }, - { - "x": 6.816934169679487, - "y": 6.499056298326003, - "heading": -0.42446557252575, - "angularVelocity": 0.6354590480036472, - "velocityX": -2.7847462561415193, - "velocityY": 0.8796135489039021, - "timestamp": 2.9260157074144564 - }, - { - "x": 6.644052672707634, - "y": 6.549043660727809, - "heading": -0.3851985362553806, - "angularVelocity": 0.6833434155097291, - "velocityX": -3.0085650418268486, - "velocityY": 0.8699035679896195, - "timestamp": 2.9834788150981373 - }, - { - "x": 6.458532330331388, - "y": 6.59657803483745, - "heading": -0.3432667508289566, - "angularVelocity": 0.7297166324043566, - "velocityX": -3.22851216814601, - "velocityY": 0.827215513148135, - "timestamp": 3.040941922781818 - }, - { - "x": 6.261163379536797, - "y": 6.6390684651491405, - "heading": -0.2988870811303611, - "angularVelocity": 0.7723158646917302, - "velocityX": -3.4347072191266927, - "velocityY": 0.739438433187235, - "timestamp": 3.098405030465499 - }, - { - "x": 6.053868375355581, - "y": 6.673346912019586, - "heading": -0.2525370578781306, - "angularVelocity": 0.8066048830386201, - "velocityX": -3.607445064097865, - "velocityY": 0.5965296387925805, - "timestamp": 3.15586813814918 - }, - { - "x": 5.840023517608643, - "y": 6.6965250968933105, - "heading": -0.20504871101552202, - "angularVelocity": 0.8264145253685168, - "velocityX": -3.721428693416656, - "velocityY": 0.4033576638652211, - "timestamp": 3.213331245832861 - }, - { - "x": 5.587338481977311, - "y": 6.706373818687575, - "heading": -0.1494877569952003, - "angularVelocity": 0.8296437426811367, - "velocityX": -3.773127412535394, - "velocityY": 0.14706245697346967, - "timestamp": 3.2803008993865257 - }, - { - "x": 5.338190707712937, - "y": 6.699077253084743, - "heading": -0.09537489680540706, - "angularVelocity": 0.8080206081166429, - "velocityX": -3.720308543402029, - "velocityY": -0.10895331266697955, - "timestamp": 3.3472705529401905 - }, - { - "x": 5.100125269441275, - "y": 6.678243970961195, - "heading": -0.04426061900833425, - "angularVelocity": 0.7632453668901434, - "velocityX": -3.554825590980445, - "velocityY": -0.31108540985439403, - "timestamp": 3.4142402064938553 - }, - { - "x": 4.877424383320222, - "y": 6.649062308491903, - "heading": 0.003174164888945651, - "angularVelocity": 0.7083026621792041, - "velocityX": -3.3254000028922905, - "velocityY": -0.43574456370613274, - "timestamp": 3.48120986004752 - }, - { - "x": 4.671656077162392, - "y": 6.615525997239799, - "heading": 0.04676178800760498, - "angularVelocity": 0.6508563327676637, - "velocityX": -3.0725604096628847, - "velocityY": -0.5007687732060658, - "timestamp": 3.548179513601185 - }, - { - "x": 4.483285608527601, - "y": 6.580261612415482, - "heading": 0.08650266367845444, - "angularVelocity": 0.5934161752681579, - "velocityX": -2.812773527099757, - "velocityY": -0.5265726034562577, - "timestamp": 3.6151491671548497 - }, - { - "x": 4.312399145034821, - "y": 6.545009459316239, - "heading": 0.12244130026065857, - "angularVelocity": 0.5366406226576237, - "velocityX": -2.5516999778988487, - "velocityY": -0.5263899576693309, - "timestamp": 3.6821188207085145 - }, - { - "x": 4.15895498200223, - "y": 6.510974013828318, - "heading": 0.154629202188202, - "angularVelocity": 0.4806341412793812, - "velocityX": -2.291249168694441, - "velocityY": -0.5082219136858214, - "timestamp": 3.7490884742621793 - }, - { - "x": 4.022870974483442, - "y": 6.47902695106831, - "heading": 0.18311449792201717, - "angularVelocity": 0.4253463206434088, - "velocityX": -2.0320249590322352, - "velocityY": -0.4770378979847717, - "timestamp": 3.816058127815844 - }, - { - "x": 3.904056208354398, - "y": 6.449823791113161, - "heading": 0.20793958437304633, - "angularVelocity": 0.3706915764635989, - "velocityX": -1.7741582914690397, - "velocityY": -0.43606556709670047, - "timestamp": 3.883027781369509 - }, - { - "x": 3.8024225885779073, - "y": 6.4238734041283765, - "heading": 0.22914116425220346, - "angularVelocity": 0.3165848821685723, - "velocityX": -1.5176070710153553, - "velocityY": -0.38749471749901515, - "timestamp": 3.9499974349231737 - }, - { - "x": 3.717888783576791, - "y": 6.4015812804640495, - "heading": 0.24675092998759593, - "angularVelocity": 0.26295142353217016, - "velocityX": -1.2622703047638866, - "velocityY": -0.33286903069408746, - "timestamp": 4.0169670884768385 - }, - { - "x": 3.650381165527701, - "y": 6.383277608926662, - "heading": 0.26079634034279287, - "angularVelocity": 0.20972798289813258, - "velocityX": -1.0080329592117891, - "velocityY": -0.27331291960052795, - "timestamp": 4.083936742030503 - }, - { - "x": 3.599833578591879, - "y": 6.369236181318641, - "heading": 0.2713013211201865, - "angularVelocity": 0.156861805608352, - "velocityX": -0.75478346166621, - "velocityY": -0.20966851197413772, - "timestamp": 4.150906395584168 - }, - { - "x": 3.566186687042349, - "y": 6.359687536429892, - "heading": 0.278286849324081, - "angularVelocity": 0.10430885980762557, - "velocityX": -0.5024199732878734, - "velocityY": -0.1425816676966564, - "timestamp": 4.217876049137833 - }, - { - "x": 3.549387216567993, - "y": 6.354828357696533, - "heading": 0.2817714218737248, - "angularVelocity": 0.05203211252767611, - "velocityX": -0.2508519841885362, - "velocityY": -0.07255791952790196, - "timestamp": 4.284845702691498 - }, - { - "x": 3.549387216567993, - "y": 6.354828357696533, - "heading": 0.2817714218737248, - "angularVelocity": 1.3070851292063258e-34, - "velocityX": 2.4712329832144155e-36, - "velocityY": -1.59860712332012e-35, - "timestamp": 4.3518153562451625 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEf.5.traj b/src/main/deploy/choreo/AmpLanePADEf.5.traj deleted file mode 100644 index fa69b717..00000000 --- a/src/main/deploy/choreo/AmpLanePADEf.5.traj +++ /dev/null @@ -1,373 +0,0 @@ -{ - "samples": [ - { - "x": 3.549387216567993, - "y": 6.354828357696533, - "heading": 0.2817714218737248, - "angularVelocity": 1.3070851292063258e-34, - "velocityX": 2.4712329832144155e-36, - "velocityY": -1.59860712332012e-35, - "timestamp": 0 - }, - { - "x": 3.5643398773963306, - "y": 6.364136155877829, - "heading": 0.2792491518630383, - "angularVelocity": -0.03754044435969045, - "velocityX": 0.22254934225013753, - "velocityY": 0.13853349492946027, - "timestamp": 0.06718807019223227 - }, - { - "x": 3.5945207682775075, - "y": 6.382292207869205, - "heading": 0.2741228266018106, - "angularVelocity": -0.07629814707522789, - "velocityX": 0.4492001451273435, - "velocityY": 0.2702273177281226, - "timestamp": 0.13437614038446455 - }, - { - "x": 3.6402410701247, - "y": 6.408735917417985, - "heading": 0.26629714167044793, - "angularVelocity": -0.11647432213743464, - "velocityX": 0.6804824385695469, - "velocityY": 0.39357745315681286, - "timestamp": 0.20156421057669682 - }, - { - "x": 3.70185092947519, - "y": 6.442771832829285, - "heading": 0.2556605160900753, - "angularVelocity": -0.1583112232564508, - "velocityX": 0.9169761711300415, - "velocityY": 0.5065767674814764, - "timestamp": 0.2687522807689291 - }, - { - "x": 3.7797397123516454, - "y": 6.483521063112053, - "heading": 0.24208195835845453, - "angularVelocity": -0.20209774879336384, - "velocityX": 1.1592650697304827, - "velocityY": 0.6064950245806952, - "timestamp": 0.33594035096116137 - }, - { - "x": 3.8743279453072446, - "y": 6.5298507780720865, - "heading": 0.22540803468111403, - "angularVelocity": -0.24816792072810653, - "velocityX": 1.4078129150751257, - "velocityY": 0.6895526962968116, - "timestamp": 0.40312842115339365 - }, - { - "x": 3.986039187026963, - "y": 6.580272560176402, - "heading": 0.20546145681784672, - "angularVelocity": -0.29687677896087594, - "velocityX": 1.6626648361844574, - "velocityY": 0.7504573648276082, - "timestamp": 0.4703164913456259 - }, - { - "x": 4.1152265805278505, - "y": 6.632802410732947, - "heading": 0.18204538697232525, - "angularVelocity": -0.34851529115995444, - "velocityX": 1.922772794802219, - "velocityY": 0.7818330011005054, - "timestamp": 0.5375045615378582 - }, - { - "x": 4.26200619887233, - "y": 6.684795357547629, - "heading": 0.15496301112004685, - "angularVelocity": -0.4030831035151423, - "velocityX": 2.184608337827327, - "velocityY": 0.7738419434569909, - "timestamp": 0.6046926317300905 - }, - { - "x": 4.4259357152840915, - "y": 6.732839229139314, - "heading": 0.12406897274637517, - "angularVelocity": -0.45981434330946414, - "velocityX": 2.4398604684245346, - "velocityY": 0.7150655087164378, - "timestamp": 0.6718807019223227 - }, - { - "x": 4.605565017161512, - "y": 6.772930427351676, - "heading": 0.08936310747680958, - "angularVelocity": -0.516548029587219, - "velocityX": 2.67352971090672, - "velocityY": 0.5967011419982232, - "timestamp": 0.739068772114555 - }, - { - "x": 4.798189232740571, - "y": 6.801147761389703, - "heading": 0.05109160546500808, - "angularVelocity": -0.569617521418644, - "velocityX": 2.8669407385558037, - "velocityY": 0.41997536106176914, - "timestamp": 0.8062568423067873 - }, - { - "x": 5.000257222010958, - "y": 6.81450295269708, - "heading": 0.00976022819990098, - "angularVelocity": -0.6151594642747336, - "velocityX": 3.007498037854751, - "velocityY": 0.19877325348335143, - "timestamp": 0.8734449124990196 - }, - { - "x": 5.208197035287627, - "y": 6.811250961542502, - "heading": -0.03399201647165303, - "angularVelocity": -0.6511906733795721, - "velocityX": 3.094891886040635, - "velocityY": -0.04840131805056809, - "timestamp": 0.9406329826912518 - }, - { - "x": 5.418969435905623, - "y": 6.790609200342764, - "heading": -0.07954736509617215, - "angularVelocity": -0.6780273416721162, - "velocityX": 3.137050967752942, - "velocityY": -0.30722360592705716, - "timestamp": 1.0078210528834841 - }, - { - "x": 5.630187479337023, - "y": 6.752351513869626, - "heading": -0.12638406136404196, - "angularVelocity": -0.6970984005622526, - "velocityX": 3.14368373473268, - "velocityY": -0.5694118965417182, - "timestamp": 1.0750091230757164 - }, - { - "x": 5.840023517608643, - "y": 6.6965250968933105, - "heading": -0.17408450791871416, - "angularVelocity": -0.7099541096833972, - "velocityX": 3.1231145301726144, - "velocityY": -0.8308977593282543, - "timestamp": 1.1421971932679487 - }, - { - "x": 6.036235910117842, - "y": 6.62805322637145, - "heading": -0.21973420116527131, - "angularVelocity": -0.7174774364630384, - "velocityX": 3.0838753640561896, - "velocityY": -1.0761742004818062, - "timestamp": 1.2058224599684557 - }, - { - "x": 6.22928822165698, - "y": 6.544096058330141, - "heading": -0.26570275361240514, - "angularVelocity": -0.722488954954231, - "velocityX": 3.0342082878467362, - "velocityY": -1.3195570312735396, - "timestamp": 1.2694477266689628 - }, - { - "x": 6.418343643134528, - "y": 6.444848030005144, - "heading": -0.3117858049536479, - "angularVelocity": -0.7242885371021207, - "velocityX": 2.9713890610071143, - "velocityY": -1.5598838868868077, - "timestamp": 1.3330729933694698 - }, - { - "x": 6.602325627695358, - "y": 6.330631863429966, - "heading": -0.3577155072604087, - "angularVelocity": -0.7218783462701701, - "velocityX": 2.8916497187643664, - "velocityY": -1.7951385117615022, - "timestamp": 1.3966982600699769 - }, - { - "x": 6.779825036433125, - "y": 6.202002246175028, - "heading": -0.4031322541711418, - "angularVelocity": -0.713816212740071, - "velocityX": 2.78976290305045, - "velocityY": -2.021675097417111, - "timestamp": 1.460323526770484 - }, - { - "x": 6.948987271183792, - "y": 6.0599479941376755, - "heading": -0.4475462113978453, - "angularVelocity": -0.6980553407464041, - "velocityX": 2.6587273189271783, - "velocityY": -2.2326704374540527, - "timestamp": 1.523948793470991 - }, - { - "x": 7.107446982391506, - "y": 5.906271088162383, - "heading": -0.49030348777981764, - "angularVelocity": -0.6720172440806661, - "velocityX": 2.490515473256959, - "velocityY": -2.415343996885224, - "timestamp": 1.587574060171498 - }, - { - "x": 7.252559698477184, - "y": 5.744150133640305, - "heading": -0.5306268209455859, - "angularVelocity": -0.6337628941592588, - "velocityX": 2.280740397816213, - "velocityY": -2.548059331290548, - "timestamp": 1.651199326872005 - }, - { - "x": 7.3823106542125965, - "y": 5.578392081546099, - "heading": -0.5678417641583621, - "angularVelocity": -0.5849082470326117, - "velocityX": 2.039299203980828, - "velocityY": -2.605223690054648, - "timestamp": 1.7148245935725122 - }, - { - "x": 7.496377774122124, - "y": 5.4143908579628635, - "heading": -0.601634051783977, - "angularVelocity": -0.531114278619525, - "velocityX": 1.7927959413743022, - "velocityY": -2.57761156986912, - "timestamp": 1.7784498602730192 - }, - { - "x": 7.595902773974046, - "y": 5.256520892458376, - "heading": -0.6320227570911706, - "angularVelocity": -0.4776200852758283, - "velocityX": 1.564237055703042, - "velocityY": -2.4812464244370447, - "timestamp": 1.8420751269735263 - }, - { - "x": 7.682486623568508, - "y": 5.107687495162123, - "heading": -0.6591789189260638, - "angularVelocity": -0.4268141140016103, - "velocityX": 1.3608406547358454, - "velocityY": -2.3392184428370397, - "timestamp": 1.9057003936740333 - }, - { - "x": 7.757608087548005, - "y": 4.96970877855346, - "heading": -0.6833001262658936, - "angularVelocity": -0.3791136539100364, - "velocityX": 1.1806860367769285, - "velocityY": -2.16861513929911, - "timestamp": 1.9693256603745404 - }, - { - "x": 7.822480324423007, - "y": 4.8437396511320765, - "heading": -0.7045645809800652, - "angularVelocity": -0.33421399731440377, - "velocityX": 1.019598663222343, - "velocityY": -1.9798601083172942, - "timestamp": 2.0329509270750474 - }, - { - "x": 7.878069173517787, - "y": 4.73054348581536, - "heading": -0.723122113591442, - "angularVelocity": -0.2916692310105266, - "velocityX": 0.8736914118795616, - "velocityY": -1.779107124997141, - "timestamp": 2.0965761937755545 - }, - { - "x": 7.925144072930378, - "y": 4.630647098306409, - "heading": -0.739096492137341, - "angularVelocity": -0.25106973022356954, - "velocityX": 0.7398774394798081, - "velocityY": -1.5700741653339847, - "timestamp": 2.1602014604760615 - }, - { - "x": 7.964324855555486, - "y": 4.544428693054829, - "heading": -0.7525900028699446, - "angularVelocity": -0.21207786516824206, - "velocityX": 0.6158053970844276, - "velocityY": -1.3550969563305832, - "timestamp": 2.2238267271765686 - }, - { - "x": 7.996117929245261, - "y": 4.47216923914691, - "heading": -0.7636877566594242, - "angularVelocity": -0.1744236899110885, - "velocityX": 0.4996925802987862, - "velocityY": -1.1357037487646877, - "timestamp": 2.2874519938770757 - }, - { - "x": 8.020942965262389, - "y": 4.414083646905636, - "heading": -0.7724611893434623, - "angularVelocity": -0.13789227360469508, - "velocityX": 0.39017574785158765, - "velocityY": -0.9129327899667726, - "timestamp": 2.3510772605775827 - }, - { - "x": 8.039152422853524, - "y": 4.370340424914289, - "heading": -0.7789707828145268, - "angularVelocity": -0.10231145280232755, - "velocityX": 0.2861985267087195, - "velocityY": -0.6875133773073633, - "timestamp": 2.4147025272780898 - }, - { - "x": 8.051045922155595, - "y": 4.341074521860988, - "heading": -0.7832681551705855, - "angularVelocity": -0.06754191501132724, - "velocityX": 0.18693044318469762, - "velocityY": -0.4599729725465729, - "timestamp": 2.478327793978597 - }, - { - "x": 8.056880950927734, - "y": 4.3263959884643555, - "heading": -0.7853976676290793, - "angularVelocity": -0.033469603648464484, - "velocityX": 0.09170930158305961, - "velocityY": -0.2307028977297052, - "timestamp": 2.541953060679104 - }, - { - "x": 8.056880950927734, - "y": 4.3263959884643555, - "heading": -0.7853976676290793, - "angularVelocity": 2.2926913486866945e-40, - "velocityX": 1.8371861901596144e-41, - "velocityY": 1.3726367222942074e-41, - "timestamp": 2.605578327379611 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEf.traj b/src/main/deploy/choreo/AmpLanePADEf.traj deleted file mode 100644 index 65fd5831..00000000 --- a/src/main/deploy/choreo/AmpLanePADEf.traj +++ /dev/null @@ -1,1678 +0,0 @@ -{ - "samples": [ - { - "x": 1.451512098312378, - "y": 7.362619876861572, - "heading": 0, - "angularVelocity": -1.626357329534646e-41, - "velocityX": 6.883179938389857e-43, - "velocityY": -3.544509713626213e-42, - "timestamp": 0 - }, - { - "x": 1.471098087600976, - "y": 7.348406138802381, - "heading": 0.01253561485916891, - "angularVelocity": 0.15858130999249215, - "velocityX": 0.2477717984940342, - "velocityY": -0.17981034250331, - "timestamp": 0.07904850111127469 - }, - { - "x": 1.5102697358053931, - "y": 7.319977714643011, - "heading": 0.03761033729519426, - "angularVelocity": 0.31720680447473965, - "velocityX": 0.4955394176200278, - "velocityY": -0.3596326781623792, - "timestamp": 0.15809700222254938 - }, - { - "x": 1.569026235907136, - "y": 7.277333432043545, - "heading": 0.07524357427525706, - "angularVelocity": 0.47607780604324657, - "velocityX": 0.7432968275898469, - "velocityY": -0.5394698444621486, - "timestamp": 0.23714550333382406 - }, - { - "x": 1.6473662329442567, - "y": 7.220472042003613, - "heading": 0.12547694655707634, - "angularVelocity": 0.6354753293943798, - "velocityX": 0.9910370966660474, - "velocityY": -0.7193228111927129, - "timestamp": 0.31619400444509876 - }, - { - "x": 1.745287770045571, - "y": 7.149392472942429, - "heading": 0.18838242237158065, - "angularVelocity": 0.7957832840619425, - "velocityX": 1.2387526104191713, - "velocityY": -0.8991893339144678, - "timestamp": 0.39524250555637347 - }, - { - "x": 1.8627883050488596, - "y": 7.064094226316957, - "heading": 0.2640709709173305, - "angularVelocity": 0.957495050275588, - "velocityX": 1.486435964647655, - "velocityY": -1.0790621634355757, - "timestamp": 0.4742910066676482 - }, - { - "x": 1.9802240062556191, - "y": 6.978809171813768, - "heading": 0.34336916171885673, - "angularVelocity": 1.003158689750487, - "velocityX": 1.4856157872171172, - "velocityY": -1.0788952770038656, - "timestamp": 0.5533395077789228 - }, - { - "x": 2.078085335558388, - "y": 6.907741106237698, - "heading": 0.40961857461491347, - "angularVelocity": 0.8380856305269982, - "velocityX": 1.2379909539968612, - "velocityY": -0.899043809521805, - "timestamp": 0.6323880088901975 - }, - { - "x": 2.1563737104997935, - "y": 6.850888499073482, - "heading": 0.46271135866543855, - "angularVelocity": 0.6716482071657204, - "velocityX": 0.9903840533446798, - "velocityY": -0.7192117037638214, - "timestamp": 0.7114365100014722 - }, - { - "x": 2.2150899115476355, - "y": 6.808249956097028, - "heading": 0.5025701306982392, - "angularVelocity": 0.5042318509833907, - "velocityX": 0.7427870259701495, - "velocityY": -0.5393972355836724, - "timestamp": 0.7904850111127469 - }, - { - "x": 2.25423422583693, - "y": 6.779824483661568, - "heading": 0.529148704819503, - "angularVelocity": 0.33623122194119537, - "velocityX": 0.49519363098601865, - "velocityY": -0.3595953374934483, - "timestamp": 0.8695335122240216 - }, - { - "x": 2.273806571960449, - "y": 6.76561164855957, - "heading": 0.5424312320356962, - "angularVelocity": 0.1680300958204839, - "velocityX": 0.24759920616290554, - "velocityY": -0.1797989196783214, - "timestamp": 0.9485820133352963 - }, - { - "x": 2.273806571960449, - "y": 6.76561164855957, - "heading": 0.5424312320356962, - "angularVelocity": 4.2126535792079476e-33, - "velocityX": -2.203749386654579e-34, - "velocityY": -1.145242422674013e-34, - "timestamp": 1.027630514446571 - }, - { - "x": 2.3052968816975965, - "y": 6.784998971806612, - "heading": 0.542413982759797, - "angularVelocity": -0.00017723560938373895, - "velocityX": 0.3235616537503915, - "velocityY": 0.19920395905812027, - "timestamp": 1.124954500067686 - }, - { - "x": 2.3682775001422844, - "y": 6.823773617666811, - "heading": 0.5423794842069812, - "angularVelocity": -0.0003544712292205405, - "velocityX": 0.6471232969216173, - "velocityY": 0.39840791160310296, - "timestamp": 1.222278485688801 - }, - { - "x": 2.4627484254412217, - "y": 6.881935584999175, - "heading": 0.5423277363751632, - "angularVelocity": -0.00053170687048794, - "velocityX": 0.9706849210503492, - "velocityY": 0.5976118524244335, - "timestamp": 1.319602471309916 - }, - { - "x": 2.588709653270064, - "y": 6.959484871141385, - "heading": 0.5422587392604604, - "angularVelocity": -0.0007089425516464455, - "velocityX": 1.2942465007466153, - "velocityY": 0.7968157658905536, - "timestamp": 1.416926456931031 - }, - { - "x": 2.746161162007105, - "y": 7.056421462781845, - "heading": 0.5421724928506289, - "angularVelocity": -0.0008861783586156058, - "velocityX": 1.6178078582807358, - "velocityY": 0.996019542580562, - "timestamp": 1.514250442552146 - }, - { - "x": 2.8721223898359267, - "y": 7.133970748924088, - "heading": 0.5421034957131426, - "angularVelocity": -0.0007089427857476058, - "velocityX": 1.2942465007464055, - "velocityY": 0.7968157658908929, - "timestamp": 1.611574428173261 - }, - { - "x": 2.9665933151348423, - "y": 7.192132716256486, - "heading": 0.5420517478572688, - "angularVelocity": -0.0005317071176599402, - "velocityX": 0.9706849210501236, - "velocityY": 0.5976118524247968, - "timestamp": 1.708898413794376 - }, - { - "x": 3.029573933579513, - "y": 7.230907362116713, - "heading": 0.5420172492858153, - "angularVelocity": -0.00035447142072369024, - "velocityX": 0.6471232969214384, - "velocityY": 0.3984079116033909, - "timestamp": 1.806222399415491 - }, - { - "x": 3.0610642433166504, - "y": 7.2502946853637695, - "heading": 0.542, - "angularVelocity": -0.0001772357112703998, - "velocityX": 0.3235616537502931, - "velocityY": 0.19920395905827845, - "timestamp": 1.903546385036606 - }, - { - "x": 3.0610642433166504, - "y": 7.2502946853637695, - "heading": 0.542, - "angularVelocity": 8.233407245564493e-29, - "velocityX": 8.650056098040804e-30, - "velocityY": 3.151108217990947e-30, - "timestamp": 2.000870370657721 - }, - { - "x": 3.0887984705253144, - "y": 7.254141628194322, - "heading": 0.5386292393754978, - "angularVelocity": -0.03979388904876881, - "velocityX": 0.3274195006260423, - "velocityY": 0.04541551098718751, - "timestamp": 2.085575854465625 - }, - { - "x": 3.144266916294855, - "y": 7.26183552381785, - "heading": 0.5318876375624417, - "angularVelocity": -0.07958872920539878, - "velocityX": 0.6548388991596666, - "velocityY": 0.09083113958686537, - "timestamp": 2.170281338273529 - }, - { - "x": 3.2274695749900846, - "y": 7.273376389298639, - "heading": 0.5217760579061892, - "angularVelocity": -0.1193733770424954, - "velocityX": 0.9822582311664592, - "velocityY": 0.1362469696408709, - "timestamp": 2.254986822081433 - }, - { - "x": 3.3384064437500833, - "y": 7.288764249060912, - "heading": 0.5082962561247577, - "angularVelocity": -0.15913729755547365, - "velocityX": 1.3096775293983471, - "velocityY": 0.1816630880377333, - "timestamp": 2.339692305889337 - }, - { - "x": 3.4770775221460872, - "y": 7.307999135190426, - "heading": 0.4914508070037636, - "angularVelocity": -0.19887082115079507, - "velocityX": 1.6370968225684077, - "velocityY": 0.22707958522658897, - "timestamp": 2.424397789697241 - }, - { - "x": 3.6434828117292577, - "y": 7.331081087741682, - "heading": 0.4712430079600464, - "angularVelocity": -0.2385654167252698, - "velocityX": 1.964516134050886, - "velocityY": 0.27249655528328975, - "timestamp": 2.509103273505145 - }, - { - "x": 3.837622315458121, - "y": 7.358010154988738, - "heading": 0.4476767580715169, - "angularVelocity": -0.27821398130227654, - "velocityX": 2.2919354804605674, - "velocityY": 0.31791409524475356, - "timestamp": 2.593808757313049 - }, - { - "x": 4.059496036986131, - "y": 7.3887863935071385, - "heading": 0.4207564114260032, - "angularVelocity": -0.31781114321168447, - "velocityX": 2.6193548700015095, - "velocityY": 0.3633323031151034, - "timestamp": 2.678514241120953 - }, - { - "x": 4.3091039797632575, - "y": 7.423409867844704, - "heading": 0.3904866045233033, - "angularVelocity": -0.35735356840760346, - "velocityX": 2.9467743002714557, - "velocityY": 0.40875127301190084, - "timestamp": 2.763219724928857 - }, - { - "x": 4.586446145814129, - "y": 7.4618806491187275, - "heading": 0.3568720602117417, - "angularVelocity": -0.39684023749291464, - "velocityX": 3.2741937544433752, - "velocityY": 0.4541710824918035, - "timestamp": 2.847925208736761 - }, - { - "x": 4.8915225336365555, - "y": 7.504198810004116, - "heading": 0.31991738183873225, - "angularVelocity": -0.436272561248892, - "velocityX": 3.6016131908810025, - "velocityY": 0.4995917499434115, - "timestamp": 2.932630692544665 - }, - { - "x": 5.224333131274253, - "y": 7.55036439862341, - "heading": 0.2796269341485423, - "angularVelocity": -0.47565335653149277, - "velocityX": 3.9290324861674275, - "velocityY": 0.5450129855109993, - "timestamp": 3.017336176352569 - }, - { - "x": 5.579793390330806, - "y": 7.592855289621793, - "heading": 0.2796269285432754, - "angularVelocity": -6.617360068351878e-8, - "velocityX": 4.196425580457948, - "velocityY": 0.5016309344825798, - "timestamp": 3.102041660160473 - }, - { - "x": 5.931486953635787, - "y": 7.608007053524113, - "heading": 0.2595234955759573, - "angularVelocity": -0.23733331142057107, - "velocityX": 4.151957435276098, - "velocityY": 0.17887583213235417, - "timestamp": 3.1867471439683768 - }, - { - "x": 6.255598185321619, - "y": 7.618552102175372, - "heading": 0.23287181404120497, - "angularVelocity": -0.3146393874022023, - "velocityX": 3.826331154907898, - "velocityY": 0.1244907434230404, - "timestamp": 3.2714526277762808 - }, - { - "x": 6.5520193977896835, - "y": 7.624877033279807, - "heading": 0.20472915346535583, - "angularVelocity": -0.33224130612140174, - "velocityX": 3.4994335566299437, - "velocityY": 0.07466967686265122, - "timestamp": 3.3561581115841848 - }, - { - "x": 6.8207292064754315, - "y": 7.627107624996143, - "heading": 0.17669875637349494, - "angularVelocity": -0.3309159670883467, - "velocityX": 3.1722835004993724, - "velocityY": 0.02633349832923241, - "timestamp": 3.4408635953920887 - }, - { - "x": 7.0617193847944275, - "y": 7.625306131272256, - "heading": 0.1495692247668899, - "angularVelocity": -0.3202806995146003, - "velocityX": 2.8450363245122436, - "velocityY": -0.021267734306042424, - "timestamp": 3.5255690791999927 - }, - { - "x": 7.274985760405132, - "y": 7.619509682191873, - "heading": 0.12380998991728469, - "angularVelocity": -0.30410350890784327, - "velocityX": 2.5177398914835423, - "velocityY": -0.06843062361266532, - "timestamp": 3.6102745630078967 - }, - { - "x": 7.460525865716125, - "y": 7.609742928512009, - "heading": 0.0997327292133379, - "angularVelocity": -0.2842467762615057, - "velocityX": 2.19041432703016, - "velocityY": -0.1153024956686039, - "timestamp": 3.6949800468158007 - }, - { - "x": 7.618338091434345, - "y": 7.59602342195819, - "heading": 0.07755965741979146, - "angularVelocity": -0.26176666252165137, - "velocityX": 1.8630697638887466, - "velocityY": -0.1619671588788346, - "timestamp": 3.7796855306237047 - }, - { - "x": 7.7484213145990894, - "y": 7.578364292552694, - "heading": 0.05745736086921445, - "angularVelocity": -0.23731989532468997, - "velocityX": 1.5357119435100053, - "velocityY": -0.20847681415229657, - "timestamp": 3.8643910144316087 - }, - { - "x": 7.850774711771762, - "y": 7.55677573093629, - "heading": 0.039555451052523435, - "angularVelocity": -0.21134298527170414, - "velocityX": 1.208344401935423, - "velocityY": -0.25486616268396156, - "timestamp": 3.9490964982395127 - }, - { - "x": 7.925397655711937, - "y": 7.531265875463379, - "heading": 0.023957677737365072, - "angularVelocity": -0.18414124581002833, - "velocityX": 0.880969455404328, - "velocityY": -0.3011594329685677, - "timestamp": 4.033801982047417 - }, - { - "x": 7.972289653892672, - "y": 7.501841375568497, - "heading": 0.010748952471097088, - "angularVelocity": -0.15593707364223489, - "velocityX": 0.5535886942916515, - "velocityY": -0.3473742026149794, - "timestamp": 4.118507465855322 - }, - { - "x": 7.991450309753418, - "y": 7.468507766723633, - "heading": 0, - "angularVelocity": -0.12689795262461315, - "velocityX": 0.22620325153938764, - "velocityY": -0.39352362263184965, - "timestamp": 4.203212949663226 - }, - { - "x": 7.979625066028202, - "y": 7.427776895642819, - "heading": -0.00869243368919643, - "angularVelocity": -0.09464796670145847, - "velocityX": -0.1287597138354107, - "velocityY": -0.4434999756945785, - "timestamp": 4.295052573818828 - }, - { - "x": 7.935198288089413, - "y": 7.3824696790264746, - "heading": -0.014418639723911789, - "angularVelocity": -0.06235005954253872, - "velocityX": -0.48374302864455354, - "velocityY": -0.4933297259533529, - "timestamp": 4.38689219797443 - }, - { - "x": 7.858167736484691, - "y": 7.332602290488192, - "heading": -0.01717387059125372, - "angularVelocity": -0.030000458869875093, - "velocityX": -0.8387507278344164, - "velocityY": -0.5429833690716247, - "timestamp": 4.478731822130032 - }, - { - "x": 7.7485306804365655, - "y": 7.278194518720073, - "heading": -0.01695283231692861, - "angularVelocity": 0.002406785486756842, - "velocityX": -1.193788161221191, - "velocityY": -0.5924215420997102, - "timestamp": 4.570571446285634 - }, - { - "x": 7.6062837180875285, - "y": 7.219271127781688, - "heading": -0.013749356302551094, - "angularVelocity": 0.03488119691221441, - "velocityX": -1.5488626358909692, - "velocityY": -0.6415900705185047, - "timestamp": 4.6624110704412365 - }, - { - "x": 7.431422496644012, - "y": 7.155863998893124, - "heading": -0.007555863200914232, - "angularVelocity": 0.06743813641027274, - "velocityX": -1.9039845061550473, - "velocityY": -0.6904114587962236, - "timestamp": 4.754250694596839 - }, - { - "x": 7.223941251479605, - "y": 7.0880157090079505, - "heading": 0.0016375412086007608, - "angularVelocity": 0.10010280958712602, - "velocityX": -2.2591691448207247, - "velocityY": -0.738769245943591, - "timestamp": 4.846090318752441 - }, - { - "x": 6.9838319898121135, - "y": 7.015785993604, - "heading": 0.013844691092692804, - "angularVelocity": 0.13291811672788056, - "velocityX": -2.6144408132654218, - "velocityY": -0.786476600574666, - "timestamp": 4.937929942908043 - }, - { - "x": 6.711082895811614, - "y": 6.939264732321087, - "heading": 0.029086591938052034, - "angularVelocity": 0.16596214308193039, - "velocityX": -2.969841138921331, - "velocityY": -0.8332052965859897, - "timestamp": 5.029769567063645 - }, - { - "x": 6.405674745772718, - "y": 6.858602462265845, - "heading": 0.04739892839527075, - "angularVelocity": 0.199394723404341, - "velocityX": -3.325450782783501, - "velocityY": -0.8782948623414927, - "timestamp": 5.121609191219247 - }, - { - "x": 6.067570892053079, - "y": 6.774103037070206, - "heading": 0.06885462737025146, - "angularVelocity": 0.2336213717145368, - "velocityX": -3.681459466196236, - "velocityY": -0.9200759037565454, - "timestamp": 5.213448815374849 - }, - { - "x": 5.696676642086654, - "y": 6.686700262340111, - "heading": 0.0936784749866653, - "angularVelocity": 0.27029561412875813, - "velocityX": -4.038499213998925, - "velocityY": -0.9516891595936975, - "timestamp": 5.305288439530451 - }, - { - "x": 5.339135311514303, - "y": 6.629362903719828, - "heading": 0.12448875151110148, - "angularVelocity": 0.3354791225308384, - "velocityX": -3.893105332908111, - "velocityY": -0.6243204841851022, - "timestamp": 5.397128063686053 - }, - { - "x": 5.013882344908921, - "y": 6.578461952217419, - "heading": 0.15282110086738224, - "angularVelocity": 0.3084980978280288, - "velocityX": -3.541531986829529, - "velocityY": -0.5542373672623276, - "timestamp": 5.488967687841655 - }, - { - "x": 4.721071580025458, - "y": 6.533164538371719, - "heading": 0.17845671775380376, - "angularVelocity": 0.27913460146500385, - "velocityX": -3.188283571233179, - "velocityY": -0.4932229880309821, - "timestamp": 5.580807311997257 - }, - { - "x": 4.4607505833449235, - "y": 6.4931855756353425, - "heading": 0.20132072068763487, - "angularVelocity": 0.24895575460958952, - "velocityX": -2.834517225805949, - "velocityY": -0.43531278687094876, - "timestamp": 5.6726469361528595 - }, - { - "x": 4.232942393025197, - "y": 6.458381106409632, - "heading": 0.22137489645108566, - "angularVelocity": 0.21836082136266488, - "velocityX": -2.480500028329932, - "velocityY": -0.37897007468939997, - "timestamp": 5.764486560308462 - }, - { - "x": 4.0376605870531215, - "y": 6.428664298965915, - "heading": 0.2385958332745625, - "angularVelocity": 0.1875109679783531, - "velocityX": -2.1263349863154883, - "velocityY": -0.3235728338060169, - "timestamp": 5.856326184464064 - }, - { - "x": 3.874914113830387, - "y": 6.403977073661965, - "heading": 0.2529676231231021, - "angularVelocity": 0.15648789921866602, - "velocityX": -1.7720725092149265, - "velocityY": -0.2688079957961907, - "timestamp": 5.948165808619666 - }, - { - "x": 3.744709313892485, - "y": 6.384277851345689, - "heading": 0.2644787572649854, - "angularVelocity": 0.12533951710070187, - "velocityX": -1.4177409929000644, - "velocityY": -0.21449589430914948, - "timestamp": 6.040005432775268 - }, - { - "x": 3.6470509145783416, - "y": 6.369535397539679, - "heading": 0.27312059313662507, - "angularVelocity": 0.09409703002889537, - "velocityX": -1.0633580027364409, - "velocityY": -0.16052389087555669, - "timestamp": 6.13184505693087 - }, - { - "x": 3.5819425761583696, - "y": 6.359725390676191, - "heading": 0.2788865130907595, - "angularVelocity": 0.06278248639842567, - "velocityX": -0.7089351575489377, - "velocityY": -0.10681671395852226, - "timestamp": 6.223684681086472 - }, - { - "x": 3.549387216567993, - "y": 6.354828357696533, - "heading": 0.2817714218737248, - "angularVelocity": 0.031412462862481436, - "velocityX": -0.3544805402864683, - "velocityY": -0.05332157034267822, - "timestamp": 6.315524305242074 - }, - { - "x": 3.549387216567993, - "y": 6.354828357696533, - "heading": 0.2817714218737248, - "angularVelocity": -1.3478129621099947e-29, - "velocityX": -2.7087222342243667e-31, - "velocityY": -4.691094059490074e-29, - "timestamp": 6.407363929397676 - }, - { - "x": 3.5643145112589276, - "y": 6.359340723434126, - "heading": 0.2784812557968366, - "angularVelocity": -0.0520228583688743, - "velocityX": 0.23602472318704046, - "velocityY": 0.07134781594289749, - "timestamp": 6.470608552627697 - }, - { - "x": 3.59421509509005, - "y": 6.368209865282409, - "heading": 0.2718895474558649, - "angularVelocity": -0.10422559269580298, - "velocityX": 0.47277669316448045, - "velocityY": 0.140235507705144, - "timestamp": 6.533853175857717 - }, - { - "x": 3.6391396771921247, - "y": 6.38125694268572, - "heading": 0.26198366744840057, - "angularVelocity": -0.15662801834452575, - "velocityX": 0.7103304566885299, - "velocityY": 0.20629544041805936, - "timestamp": 6.597097799087738 - }, - { - "x": 3.6991442631106195, - "y": 6.398274326559935, - "heading": 0.24874957308777174, - "angularVelocity": -0.20925248162988414, - "velocityX": 0.9487697586600851, - "velocityY": 0.2690724207862332, - "timestamp": 6.6603424223177585 - }, - { - "x": 3.774290733668231, - "y": 6.419018192662677, - "heading": 0.23217163029829782, - "angularVelocity": -0.26212414499142417, - "velocityX": 1.1881874967347645, - "velocityY": 0.32799414469267935, - "timestamp": 6.723587045547779 - }, - { - "x": 3.8646473183010075, - "y": 6.4431984071841955, - "heading": 0.21223243762750219, - "angularVelocity": -0.31527095352085177, - "velocityX": 1.42868405277947, - "velocityY": 0.38232838281879794, - "timestamp": 6.7868316687778 - }, - { - "x": 3.9702887319682127, - "y": 6.47046438647985, - "heading": 0.18891269789783047, - "angularVelocity": -0.36872288170423295, - "velocityX": 1.6703619734279613, - "velocityY": 0.4311193252980343, - "timestamp": 6.85007629200782 - }, - { - "x": 4.091295462853014, - "y": 6.5003847972803275, - "heading": 0.1621912387892295, - "angularVelocity": -0.4225095785837003, - "velocityX": 1.9133125427073783, - "velocityY": 0.4730901896854845, - "timestamp": 6.913320915237841 - }, - { - "x": 4.227751057788416, - "y": 6.532417534961208, - "heading": 0.13204541033302933, - "angularVelocity": -0.47665440817885446, - "velocityX": 2.1575841228291166, - "velocityY": 0.506489501318997, - "timestamp": 6.976565538467861 - }, - { - "x": 4.379734734514181, - "y": 6.565863853542482, - "heading": 0.09845238767865087, - "angularVelocity": -0.5311601356561273, - "velocityX": 2.403108263812402, - "velocityY": 0.5288405064827253, - "timestamp": 7.039810161697882 - }, - { - "x": 4.547302866246675, - "y": 6.599795890388135, - "heading": 0.061392656400376396, - "angularVelocity": -0.5859744178329315, - "velocityX": 2.6495237567160146, - "velocityY": 0.5365204994935601, - "timestamp": 7.103054784927902 - }, - { - "x": 4.730442949191217, - "y": 6.632938881927366, - "heading": 0.02085892319849224, - "angularVelocity": -0.640904018899171, - "velocityX": 2.8957415443596144, - "velocityY": 0.5240444143163087, - "timestamp": 7.166299408157923 - }, - { - "x": 4.928956637722946, - "y": 6.663479274492677, - "heading": -0.02312096955214101, - "angularVelocity": -0.6953933868920108, - "velocityX": 3.1388231661960395, - "velocityY": 0.48289310625244986, - "timestamp": 7.2295440313879435 - }, - { - "x": 5.142158150786473, - "y": 6.688780976787343, - "heading": -0.07042466867749009, - "angularVelocity": -0.7479481528936539, - "velocityX": 3.3710614780344836, - "velocityY": 0.40006092221695244, - "timestamp": 7.292788654617964 - }, - { - "x": 5.368156918828487, - "y": 6.705163329380271, - "heading": -0.12069128737602382, - "angularVelocity": -0.7947967136386285, - "velocityX": 3.573406821004469, - "velocityY": 0.2590315469719096, - "timestamp": 7.356033277847985 - }, - { - "x": 5.602787591868942, - "y": 6.70854231082706, - "heading": -0.17306941127730663, - "angularVelocity": -0.828183033216651, - "velocityX": 3.7098912296006943, - "velocityY": 0.05342717331872859, - "timestamp": 7.419277901078005 - }, - { - "x": 5.840023517608643, - "y": 6.6965250968933105, - "heading": -0.22619108927305293, - "angularVelocity": -0.8399398286008083, - "velocityX": 3.7510844973630975, - "velocityY": -0.19001162976404115, - "timestamp": 7.482522524308026 - }, - { - "x": 6.044562434699064, - "y": 6.674397447942719, - "heading": -0.2720344485514847, - "angularVelocity": -0.8324214431482473, - "velocityX": 3.7140075078331716, - "velocityY": -0.4017927517278426, - "timestamp": 7.537594819304477 - }, - { - "x": 6.24348632132104, - "y": 6.641849320208312, - "heading": -0.31657574679893374, - "angularVelocity": -0.808778683552582, - "velocityX": 3.612050063190479, - "velocityY": -0.5910072884470288, - "timestamp": 7.592667114300928 - }, - { - "x": 6.4336299472398455, - "y": 6.6013719538289495, - "heading": -0.3590556305763903, - "angularVelocity": -0.7713476218885391, - "velocityX": 3.4526185249962773, - "velocityY": -0.7349860103337076, - "timestamp": 7.64773940929738 - }, - { - "x": 6.613077092610287, - "y": 6.55585155346098, - "heading": -0.39905440385271473, - "angularVelocity": -0.7262957405153002, - "velocityX": 3.258392361930904, - "velocityY": -0.8265571712764752, - "timestamp": 7.702811704293831 - }, - { - "x": 6.7809899014103525, - "y": 6.507733437798222, - "heading": -0.4364070087513559, - "angularVelocity": -0.6782467464093925, - "velocityX": 3.0489524507901287, - "velocityY": -0.8737263567072827, - "timestamp": 7.757883999290282 - }, - { - "x": 6.937105965748617, - "y": 6.458836458241438, - "heading": -0.4710688606060818, - "angularVelocity": -0.6293881861462208, - "velocityX": 2.834747750176842, - "velocityY": -0.8878689286497744, - "timestamp": 7.812956294286733 - }, - { - "x": 7.081411799592395, - "y": 6.41047495052379, - "heading": -0.5030439244323226, - "angularVelocity": -0.5806016224364979, - "velocityX": 2.620298170851195, - "velocityY": -0.8781458575634948, - "timestamp": 7.868028589283185 - }, - { - "x": 7.213990143110089, - "y": 6.363610015779139, - "heading": -0.5323539451412649, - "angularVelocity": -0.532209901745171, - "velocityX": 2.4073509833980524, - "velocityY": -0.8509711597759041, - "timestamp": 7.923100884279636 - }, - { - "x": 7.334955224032392, - "y": 6.318961770507673, - "heading": -0.5590259257379866, - "angularVelocity": -0.4843085002802334, - "velocityX": 2.1964779374111463, - "velocityY": -0.8107206223082474, - "timestamp": 7.978173179276087 - }, - { - "x": 7.444426209898462, - "y": 6.27708400760693, - "heading": -0.583087151971681, - "angularVelocity": -0.4369025520952958, - "velocityX": 1.987768729687496, - "velocityY": -0.7604143408848628, - "timestamp": 8.033245474272539 - }, - { - "x": 7.542516765240442, - "y": 6.238413025121382, - "heading": -0.6045633303006558, - "angularVelocity": -0.3899633805048192, - "velocityX": 1.78112343689872, - "velocityY": -0.7021857812179472, - "timestamp": 8.08831776926899 - }, - { - "x": 7.629331416896003, - "y": 6.203299971030318, - "heading": -0.6234780189407864, - "angularVelocity": -0.34345197782931597, - "velocityX": 1.576376137242069, - "velocityY": -0.6375810939661717, - "timestamp": 8.143390064265441 - }, - { - "x": 7.704964792064279, - "y": 6.172032753280793, - "heading": -0.639852590711617, - "angularVelocity": -0.29732866175063044, - "velocityX": 1.3733470735721198, - "velocityY": -0.567748588496981, - "timestamp": 8.198462359261892 - }, - { - "x": 7.769502031127833, - "y": 6.144851250422818, - "heading": -0.653706402402048, - "angularVelocity": -0.25155682528436213, - "velocityX": 1.1718639847442827, - "velocityY": -0.49356038021888754, - "timestamp": 8.253534654258344 - }, - { - "x": 7.823019631820591, - "y": 6.121958127000515, - "heading": -0.665057029957846, - "angularVelocity": -0.20610413196924784, - "velocityX": 0.9717699379734768, - "velocityY": -0.41569219920429124, - "timestamp": 8.308606949254795 - }, - { - "x": 7.865586399595873, - "y": 6.1035266968155515, - "heading": -0.6739205091019282, - "angularVelocity": -0.16094261451521974, - "velocityX": 0.7729252572100992, - "velocityY": -0.3346769947784376, - "timestamp": 8.363679244251246 - }, - { - "x": 7.89726436642432, - "y": 6.089706757076858, - "heading": -0.6803115566586851, - "angularVelocity": -0.11604832442825835, - "velocityX": 0.575206949891734, - "velocityY": -0.25094178006534995, - "timestamp": 8.418751539247697 - }, - { - "x": 7.918109625191074, - "y": 6.080628997223027, - "heading": -0.6842437638150144, - "angularVelocity": -0.0714008224386257, - "velocityX": 0.3785071743986183, - "velocityY": -0.1648335130107772, - "timestamp": 8.473823834244149 - }, - { - "x": 7.928173065185547, - "y": 6.076408386230469, - "heading": -0.6857297596086507, - "angularVelocity": -0.026982637889560627, - "velocityX": 0.18273144409763778, - "velocityY": -0.07663764498701091, - "timestamp": 8.5288961292406 - }, - { - "x": 7.926985551108671, - "y": 6.077403500536459, - "heading": -0.6846299180981561, - "angularVelocity": 0.019139958746209124, - "velocityX": -0.020665677940935028, - "velocityY": 0.01731744672543921, - "timestamp": 8.586359236924281 - }, - { - "x": 7.914061487019017, - "y": 6.08369055810287, - "heading": -0.6808709179866076, - "angularVelocity": 0.06541588617588866, - "velocityX": -0.2249106358952533, - "velocityY": 0.10941032985930138, - "timestamp": 8.643822344607962 - }, - { - "x": 7.889347344730943, - "y": 6.09514891705372, - "heading": -0.6744435459188646, - "angularVelocity": 0.11185214874078991, - "velocityX": -0.43008711648732323, - "velocityY": 0.19940374638150343, - "timestamp": 8.701285452291643 - }, - { - "x": 7.852784107363759, - "y": 6.111641566888316, - "heading": -0.6653381137940755, - "angularVelocity": 0.1584570081888393, - "velocityX": -0.6362906365672962, - "velocityY": 0.28701284179380265, - "timestamp": 8.758748559975324 - }, - { - "x": 7.8043064822748365, - "y": 6.1330116259943965, - "heading": -0.6535443768551544, - "angularVelocity": 0.20524015171338303, - "velocityX": -0.8436304098932481, - "velocityY": 0.37189180967581936, - "timestamp": 8.816211667659005 - }, - { - "x": 7.743842000412036, - "y": 6.159077789214998, - "heading": -0.6390514483000621, - "angularVelocity": 0.2522127524823789, - "velocityX": -1.0522313237154144, - "velocityY": 0.45361562002682787, - "timestamp": 8.873674775342685 - }, - { - "x": 7.671310010243926, - "y": 6.189628312245146, - "heading": -0.6218477227565106, - "angularVelocity": 0.29938731539292485, - "velocityX": -1.2622357733831673, - "velocityY": 0.5316545564907617, - "timestamp": 8.931137883026366 - }, - { - "x": 7.586620608820786, - "y": 6.224412915379664, - "heading": -0.6019208328060185, - "angularVelocity": 0.3467771019309393, - "velocityX": -1.4738047564244807, - "velocityY": 0.6053380079267261, - "timestamp": 8.988600990710047 - }, - { - "x": 7.4896736303193014, - "y": 6.263131662730417, - "heading": -0.579257685947244, - "angularVelocity": 0.39439473032905314, - "velocityX": -1.6871168721878604, - "velocityY": 0.6738018341070235, - "timestamp": 9.046064098393728 - }, - { - "x": 7.380357986253064, - "y": 6.305419339829435, - "heading": -0.5538446760345179, - "angularVelocity": 0.4422491392672011, - "velocityX": -1.9023622019886608, - "velocityY": 0.7359100265130355, - "timestamp": 9.103527206077409 - }, - { - "x": 7.2585520430714885, - "y": 6.350822964880551, - "heading": -0.5256682599840283, - "angularVelocity": 0.49033923131330814, - "velocityX": -2.11972425598849, - "velocityY": 0.7901352168603886, - "timestamp": 9.16099031376109 - }, - { - "x": 7.124126623207013, - "y": 6.39876857667167, - "heading": -0.49471629853932575, - "angularVelocity": 0.5386405764039992, - "velocityX": -2.3393343187154714, - "velocityY": 0.8343720645087159, - "timestamp": 9.21845342144477 - }, - { - "x": 6.976954343667873, - "y": 6.448510970245314, - "heading": -0.4609810242297525, - "angularVelocity": 0.5870770946687629, - "velocityX": -2.5611611601183455, - "velocityY": 0.8656405053388868, - "timestamp": 9.275916529128452 - }, - { - "x": 6.816934169679487, - "y": 6.499056298326003, - "heading": -0.42446557252575, - "angularVelocity": 0.6354590480036472, - "velocityX": -2.7847462561415193, - "velocityY": 0.8796135489039021, - "timestamp": 9.333379636812133 - }, - { - "x": 6.644052672707634, - "y": 6.549043660727809, - "heading": -0.3851985362553806, - "angularVelocity": 0.6833434155097291, - "velocityX": -3.0085650418268486, - "velocityY": 0.8699035679896195, - "timestamp": 9.390842744495814 - }, - { - "x": 6.458532330331388, - "y": 6.59657803483745, - "heading": -0.3432667508289566, - "angularVelocity": 0.7297166324043566, - "velocityX": -3.22851216814601, - "velocityY": 0.827215513148135, - "timestamp": 9.448305852179494 - }, - { - "x": 6.261163379536797, - "y": 6.6390684651491405, - "heading": -0.2988870811303611, - "angularVelocity": 0.7723158646917302, - "velocityX": -3.4347072191266927, - "velocityY": 0.739438433187235, - "timestamp": 9.505768959863175 - }, - { - "x": 6.053868375355581, - "y": 6.673346912019586, - "heading": -0.2525370578781306, - "angularVelocity": 0.8066048830386201, - "velocityX": -3.607445064097865, - "velocityY": 0.5965296387925805, - "timestamp": 9.563232067546856 - }, - { - "x": 5.840023517608643, - "y": 6.6965250968933105, - "heading": -0.20504871101552202, - "angularVelocity": 0.8264145253685168, - "velocityX": -3.721428693416656, - "velocityY": 0.4033576638652211, - "timestamp": 9.620695175230537 - }, - { - "x": 5.587338481977311, - "y": 6.706373818687575, - "heading": -0.1494877569952003, - "angularVelocity": 0.8296437426811367, - "velocityX": -3.773127412535394, - "velocityY": 0.14706245697346967, - "timestamp": 9.687664828784202 - }, - { - "x": 5.338190707712937, - "y": 6.699077253084743, - "heading": -0.09537489680540706, - "angularVelocity": 0.8080206081166429, - "velocityX": -3.720308543402029, - "velocityY": -0.10895331266697955, - "timestamp": 9.754634482337867 - }, - { - "x": 5.100125269441275, - "y": 6.678243970961195, - "heading": -0.04426061900833425, - "angularVelocity": 0.7632453668901434, - "velocityX": -3.554825590980445, - "velocityY": -0.31108540985439403, - "timestamp": 9.821604135891532 - }, - { - "x": 4.877424383320222, - "y": 6.649062308491903, - "heading": 0.003174164888945651, - "angularVelocity": 0.7083026621792041, - "velocityX": -3.3254000028922905, - "velocityY": -0.43574456370613274, - "timestamp": 9.888573789445196 - }, - { - "x": 4.671656077162392, - "y": 6.615525997239799, - "heading": 0.04676178800760498, - "angularVelocity": 0.6508563327676637, - "velocityX": -3.0725604096628847, - "velocityY": -0.5007687732060658, - "timestamp": 9.955543442998861 - }, - { - "x": 4.483285608527601, - "y": 6.580261612415482, - "heading": 0.08650266367845444, - "angularVelocity": 0.5934161752681579, - "velocityX": -2.812773527099757, - "velocityY": -0.5265726034562577, - "timestamp": 10.022513096552526 - }, - { - "x": 4.312399145034821, - "y": 6.545009459316239, - "heading": 0.12244130026065857, - "angularVelocity": 0.5366406226576237, - "velocityX": -2.5516999778988487, - "velocityY": -0.5263899576693309, - "timestamp": 10.08948275010619 - }, - { - "x": 4.15895498200223, - "y": 6.510974013828318, - "heading": 0.154629202188202, - "angularVelocity": 0.4806341412793812, - "velocityX": -2.291249168694441, - "velocityY": -0.5082219136858214, - "timestamp": 10.156452403659856 - }, - { - "x": 4.022870974483442, - "y": 6.47902695106831, - "heading": 0.18311449792201717, - "angularVelocity": 0.4253463206434088, - "velocityX": -2.0320249590322352, - "velocityY": -0.4770378979847717, - "timestamp": 10.22342205721352 - }, - { - "x": 3.904056208354398, - "y": 6.449823791113161, - "heading": 0.20793958437304633, - "angularVelocity": 0.3706915764635989, - "velocityX": -1.7741582914690397, - "velocityY": -0.43606556709670047, - "timestamp": 10.290391710767185 - }, - { - "x": 3.8024225885779073, - "y": 6.4238734041283765, - "heading": 0.22914116425220346, - "angularVelocity": 0.3165848821685723, - "velocityX": -1.5176070710153553, - "velocityY": -0.38749471749901515, - "timestamp": 10.35736136432085 - }, - { - "x": 3.717888783576791, - "y": 6.4015812804640495, - "heading": 0.24675092998759593, - "angularVelocity": 0.26295142353217016, - "velocityX": -1.2622703047638866, - "velocityY": -0.33286903069408746, - "timestamp": 10.424331017874515 - }, - { - "x": 3.650381165527701, - "y": 6.383277608926662, - "heading": 0.26079634034279287, - "angularVelocity": 0.20972798289813258, - "velocityX": -1.0080329592117891, - "velocityY": -0.27331291960052795, - "timestamp": 10.49130067142818 - }, - { - "x": 3.599833578591879, - "y": 6.369236181318641, - "heading": 0.2713013211201865, - "angularVelocity": 0.156861805608352, - "velocityX": -0.75478346166621, - "velocityY": -0.20966851197413772, - "timestamp": 10.558270324981844 - }, - { - "x": 3.566186687042349, - "y": 6.359687536429892, - "heading": 0.278286849324081, - "angularVelocity": 0.10430885980762557, - "velocityX": -0.5024199732878734, - "velocityY": -0.1425816676966564, - "timestamp": 10.62523997853551 - }, - { - "x": 3.549387216567993, - "y": 6.354828357696533, - "heading": 0.2817714218737248, - "angularVelocity": 0.05203211252767611, - "velocityX": -0.2508519841885362, - "velocityY": -0.07255791952790196, - "timestamp": 10.692209632089174 - }, - { - "x": 3.549387216567993, - "y": 6.354828357696533, - "heading": 0.2817714218737248, - "angularVelocity": 1.3070851292063258e-34, - "velocityX": 2.4712329832144155e-36, - "velocityY": -1.59860712332012e-35, - "timestamp": 10.759179285642839 - }, - { - "x": 3.5643398773963306, - "y": 6.364136155877829, - "heading": 0.2792491518630383, - "angularVelocity": -0.03754044435969045, - "velocityX": 0.22254934225013753, - "velocityY": 0.13853349492946027, - "timestamp": 10.826367355835071 - }, - { - "x": 3.5945207682775075, - "y": 6.382292207869205, - "heading": 0.2741228266018106, - "angularVelocity": -0.07629814707522789, - "velocityX": 0.4492001451273435, - "velocityY": 0.2702273177281226, - "timestamp": 10.893555426027303 - }, - { - "x": 3.6402410701247, - "y": 6.408735917417985, - "heading": 0.26629714167044793, - "angularVelocity": -0.11647432213743464, - "velocityX": 0.6804824385695469, - "velocityY": 0.39357745315681286, - "timestamp": 10.960743496219536 - }, - { - "x": 3.70185092947519, - "y": 6.442771832829285, - "heading": 0.2556605160900753, - "angularVelocity": -0.1583112232564508, - "velocityX": 0.9169761711300415, - "velocityY": 0.5065767674814764, - "timestamp": 11.027931566411768 - }, - { - "x": 3.7797397123516454, - "y": 6.483521063112053, - "heading": 0.24208195835845453, - "angularVelocity": -0.20209774879336384, - "velocityX": 1.1592650697304827, - "velocityY": 0.6064950245806952, - "timestamp": 11.095119636604 - }, - { - "x": 3.8743279453072446, - "y": 6.5298507780720865, - "heading": 0.22540803468111403, - "angularVelocity": -0.24816792072810653, - "velocityX": 1.4078129150751257, - "velocityY": 0.6895526962968116, - "timestamp": 11.162307706796232 - }, - { - "x": 3.986039187026963, - "y": 6.580272560176402, - "heading": 0.20546145681784672, - "angularVelocity": -0.29687677896087594, - "velocityX": 1.6626648361844574, - "velocityY": 0.7504573648276082, - "timestamp": 11.229495776988465 - }, - { - "x": 4.1152265805278505, - "y": 6.632802410732947, - "heading": 0.18204538697232525, - "angularVelocity": -0.34851529115995444, - "velocityX": 1.922772794802219, - "velocityY": 0.7818330011005054, - "timestamp": 11.296683847180697 - }, - { - "x": 4.26200619887233, - "y": 6.684795357547629, - "heading": 0.15496301112004685, - "angularVelocity": -0.4030831035151423, - "velocityX": 2.184608337827327, - "velocityY": 0.7738419434569909, - "timestamp": 11.36387191737293 - }, - { - "x": 4.4259357152840915, - "y": 6.732839229139314, - "heading": 0.12406897274637517, - "angularVelocity": -0.45981434330946414, - "velocityX": 2.4398604684245346, - "velocityY": 0.7150655087164378, - "timestamp": 11.431059987565162 - }, - { - "x": 4.605565017161512, - "y": 6.772930427351676, - "heading": 0.08936310747680958, - "angularVelocity": -0.516548029587219, - "velocityX": 2.67352971090672, - "velocityY": 0.5967011419982232, - "timestamp": 11.498248057757394 - }, - { - "x": 4.798189232740571, - "y": 6.801147761389703, - "heading": 0.05109160546500808, - "angularVelocity": -0.569617521418644, - "velocityX": 2.8669407385558037, - "velocityY": 0.41997536106176914, - "timestamp": 11.565436127949626 - }, - { - "x": 5.000257222010958, - "y": 6.81450295269708, - "heading": 0.00976022819990098, - "angularVelocity": -0.6151594642747336, - "velocityX": 3.007498037854751, - "velocityY": 0.19877325348335143, - "timestamp": 11.632624198141858 - }, - { - "x": 5.208197035287627, - "y": 6.811250961542502, - "heading": -0.03399201647165303, - "angularVelocity": -0.6511906733795721, - "velocityX": 3.094891886040635, - "velocityY": -0.04840131805056809, - "timestamp": 11.69981226833409 - }, - { - "x": 5.418969435905623, - "y": 6.790609200342764, - "heading": -0.07954736509617215, - "angularVelocity": -0.6780273416721162, - "velocityX": 3.137050967752942, - "velocityY": -0.30722360592705716, - "timestamp": 11.767000338526323 - }, - { - "x": 5.630187479337023, - "y": 6.752351513869626, - "heading": -0.12638406136404196, - "angularVelocity": -0.6970984005622526, - "velocityX": 3.14368373473268, - "velocityY": -0.5694118965417182, - "timestamp": 11.834188408718555 - }, - { - "x": 5.840023517608643, - "y": 6.6965250968933105, - "heading": -0.17408450791871416, - "angularVelocity": -0.7099541096833972, - "velocityX": 3.1231145301726144, - "velocityY": -0.8308977593282543, - "timestamp": 11.901376478910787 - }, - { - "x": 6.036235910117842, - "y": 6.62805322637145, - "heading": -0.21973420116527131, - "angularVelocity": -0.7174774364630384, - "velocityX": 3.0838753640561896, - "velocityY": -1.0761742004818062, - "timestamp": 11.965001745611294 - }, - { - "x": 6.22928822165698, - "y": 6.544096058330141, - "heading": -0.26570275361240514, - "angularVelocity": -0.722488954954231, - "velocityX": 3.0342082878467362, - "velocityY": -1.3195570312735396, - "timestamp": 12.028627012311802 - }, - { - "x": 6.418343643134528, - "y": 6.444848030005144, - "heading": -0.3117858049536479, - "angularVelocity": -0.7242885371021207, - "velocityX": 2.9713890610071143, - "velocityY": -1.5598838868868077, - "timestamp": 12.092252279012309 - }, - { - "x": 6.602325627695358, - "y": 6.330631863429966, - "heading": -0.3577155072604087, - "angularVelocity": -0.7218783462701701, - "velocityX": 2.8916497187643664, - "velocityY": -1.7951385117615022, - "timestamp": 12.155877545712816 - }, - { - "x": 6.779825036433125, - "y": 6.202002246175028, - "heading": -0.4031322541711418, - "angularVelocity": -0.713816212740071, - "velocityX": 2.78976290305045, - "velocityY": -2.021675097417111, - "timestamp": 12.219502812413323 - }, - { - "x": 6.948987271183792, - "y": 6.0599479941376755, - "heading": -0.4475462113978453, - "angularVelocity": -0.6980553407464041, - "velocityX": 2.6587273189271783, - "velocityY": -2.2326704374540527, - "timestamp": 12.28312807911383 - }, - { - "x": 7.107446982391506, - "y": 5.906271088162383, - "heading": -0.49030348777981764, - "angularVelocity": -0.6720172440806661, - "velocityX": 2.490515473256959, - "velocityY": -2.415343996885224, - "timestamp": 12.346753345814337 - }, - { - "x": 7.252559698477184, - "y": 5.744150133640305, - "heading": -0.5306268209455859, - "angularVelocity": -0.6337628941592588, - "velocityX": 2.280740397816213, - "velocityY": -2.548059331290548, - "timestamp": 12.410378612514844 - }, - { - "x": 7.3823106542125965, - "y": 5.578392081546099, - "heading": -0.5678417641583621, - "angularVelocity": -0.5849082470326117, - "velocityX": 2.039299203980828, - "velocityY": -2.605223690054648, - "timestamp": 12.474003879215351 - }, - { - "x": 7.496377774122124, - "y": 5.4143908579628635, - "heading": -0.601634051783977, - "angularVelocity": -0.531114278619525, - "velocityX": 1.7927959413743022, - "velocityY": -2.57761156986912, - "timestamp": 12.537629145915858 - }, - { - "x": 7.595902773974046, - "y": 5.256520892458376, - "heading": -0.6320227570911706, - "angularVelocity": -0.4776200852758283, - "velocityX": 1.564237055703042, - "velocityY": -2.4812464244370447, - "timestamp": 12.601254412616365 - }, - { - "x": 7.682486623568508, - "y": 5.107687495162123, - "heading": -0.6591789189260638, - "angularVelocity": -0.4268141140016103, - "velocityX": 1.3608406547358454, - "velocityY": -2.3392184428370397, - "timestamp": 12.664879679316872 - }, - { - "x": 7.757608087548005, - "y": 4.96970877855346, - "heading": -0.6833001262658936, - "angularVelocity": -0.3791136539100364, - "velocityX": 1.1806860367769285, - "velocityY": -2.16861513929911, - "timestamp": 12.72850494601738 - }, - { - "x": 7.822480324423007, - "y": 4.8437396511320765, - "heading": -0.7045645809800652, - "angularVelocity": -0.33421399731440377, - "velocityX": 1.019598663222343, - "velocityY": -1.9798601083172942, - "timestamp": 12.792130212717886 - }, - { - "x": 7.878069173517787, - "y": 4.73054348581536, - "heading": -0.723122113591442, - "angularVelocity": -0.2916692310105266, - "velocityX": 0.8736914118795616, - "velocityY": -1.779107124997141, - "timestamp": 12.855755479418393 - }, - { - "x": 7.925144072930378, - "y": 4.630647098306409, - "heading": -0.739096492137341, - "angularVelocity": -0.25106973022356954, - "velocityX": 0.7398774394798081, - "velocityY": -1.5700741653339847, - "timestamp": 12.9193807461189 - }, - { - "x": 7.964324855555486, - "y": 4.544428693054829, - "heading": -0.7525900028699446, - "angularVelocity": -0.21207786516824206, - "velocityX": 0.6158053970844276, - "velocityY": -1.3550969563305832, - "timestamp": 12.983006012819407 - }, - { - "x": 7.996117929245261, - "y": 4.47216923914691, - "heading": -0.7636877566594242, - "angularVelocity": -0.1744236899110885, - "velocityX": 0.4996925802987862, - "velocityY": -1.1357037487646877, - "timestamp": 13.046631279519914 - }, - { - "x": 8.020942965262389, - "y": 4.414083646905636, - "heading": -0.7724611893434623, - "angularVelocity": -0.13789227360469508, - "velocityX": 0.39017574785158765, - "velocityY": -0.9129327899667726, - "timestamp": 13.110256546220421 - }, - { - "x": 8.039152422853524, - "y": 4.370340424914289, - "heading": -0.7789707828145268, - "angularVelocity": -0.10231145280232755, - "velocityX": 0.2861985267087195, - "velocityY": -0.6875133773073633, - "timestamp": 13.173881812920929 - }, - { - "x": 8.051045922155595, - "y": 4.341074521860988, - "heading": -0.7832681551705855, - "angularVelocity": -0.06754191501132724, - "velocityX": 0.18693044318469762, - "velocityY": -0.4599729725465729, - "timestamp": 13.237507079621436 - }, - { - "x": 8.056880950927734, - "y": 4.3263959884643555, - "heading": -0.7853976676290793, - "angularVelocity": -0.033469603648464484, - "velocityX": 0.09170930158305961, - "velocityY": -0.2307028977297052, - "timestamp": 13.301132346321943 - }, - { - "x": 8.056880950927734, - "y": 4.3263959884643555, - "heading": -0.7853976676290793, - "angularVelocity": 2.2926913486866945e-40, - "velocityX": 1.8371861901596144e-41, - "velocityY": 1.3726367222942074e-41, - "timestamp": 13.36475761302245 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePAmpAAmpSprint.1.traj b/src/main/deploy/choreo/AmpLanePAmpAAmpSprint.1.traj deleted file mode 100644 index 3eeea5a5..00000000 --- a/src/main/deploy/choreo/AmpLanePAmpAAmpSprint.1.traj +++ /dev/null @@ -1,391 +0,0 @@ -{ - "samples": [ - { - "x": 1.228834867477417, - "y": 7.059738636016846, - "heading": 1.0088745501500993, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.2472218556025594, - "y": 7.08948625549367, - "heading": 1.0202423017774995, - "angularVelocity": 0.17517082812845983, - "velocityX": 0.2833334191526489, - "velocityY": 0.4583945277310271, - "timestamp": 0.0648952325501577 - }, - { - "x": 1.2842125125618584, - "y": 7.148836974041856, - "heading": 1.0432930699841776, - "angularVelocity": 0.35519971654099614, - "velocityX": 0.5700057693869717, - "velocityY": 0.9145620751464901, - "timestamp": 0.1297904651003154 - }, - { - "x": 1.3401709968856181, - "y": 7.237542105970113, - "heading": 1.0785564128874017, - "angularVelocity": 0.5433888055793495, - "velocityX": 0.8622896031770767, - "velocityY": 1.366897512227833, - "timestamp": 0.19468569765047308 - }, - { - "x": 1.4158364633522358, - "y": 7.355073707856503, - "heading": 1.1271095473140742, - "angularVelocity": 0.748177216086649, - "velocityX": 1.1659634073756595, - "velocityY": 1.8110976302542545, - "timestamp": 0.2595809302006308 - }, - { - "x": 1.5134954683399044, - "y": 7.499582339482829, - "heading": 1.1923271139091147, - "angularVelocity": 1.0049669911365822, - "velocityX": 1.504871793350733, - "velocityY": 2.226798887185344, - "timestamp": 0.3244761627507885 - }, - { - "x": 1.6221627697852088, - "y": 7.611419443621613, - "heading": 1.2812853901227188, - "angularVelocity": 1.3707983270550417, - "velocityX": 1.674503614133984, - "velocityY": 1.723348538004631, - "timestamp": 0.3893713953009462 - }, - { - "x": 1.7171722095056663, - "y": 7.6909348739854755, - "heading": 1.365395685532232, - "angularVelocity": 1.2960935973918277, - "velocityX": 1.4640434433611809, - "velocityY": 1.2252892429718023, - "timestamp": 0.45426662785110394 - }, - { - "x": 1.7966706946595994, - "y": 7.739013205131253, - "heading": 1.442194898226158, - "angularVelocity": 1.183433815952006, - "velocityX": 1.225028126565204, - "velocityY": 0.740860757508164, - "timestamp": 0.5191618604012617 - }, - { - "x": 1.8600025780551501, - "y": 7.756003639472796, - "heading": 1.5108089040033452, - "angularVelocity": 1.0573042592020157, - "velocityX": 0.9759096455444755, - "velocityY": 0.26181329003499115, - "timestamp": 0.5840570929514194 - }, - { - "x": 1.906832933425903, - "y": 7.742092132568359, - "heading": 1.5707963267948966, - "angularVelocity": 0.9243733389072425, - "velocityX": 0.721630134148261, - "velocityY": -0.21436870410603143, - "timestamp": 0.6489523255015771 - }, - { - "x": 1.9348222809552327, - "y": 7.70709813704719, - "heading": 1.6159821878126526, - "angularVelocity": 0.8059959086126847, - "velocityX": 0.49925572037929106, - "velocityY": -0.6242000612755755, - "timestamp": 0.7050144723870765 - }, - { - "x": 1.9504564809107534, - "y": 7.649074139407456, - "heading": 1.6542622365278856, - "angularVelocity": 0.6828145342599153, - "velocityX": 0.27887265872018596, - "velocityY": -1.03499421380064, - "timestamp": 0.7610766192725759 - }, - { - "x": 1.9538923564371444, - "y": 7.567945861709625, - "heading": 1.6852575777384742, - "angularVelocity": 0.5528746744910241, - "velocityX": 0.06128690600109267, - "velocityY": -1.4471132877505835, - "timestamp": 0.8171387661580752 - }, - { - "x": 1.9453662221730657, - "y": 7.463604685889236, - "heading": 1.7083990376780211, - "angularVelocity": 0.4127822644182869, - "velocityX": -0.15208362037031065, - "velocityY": -1.8611698198696156, - "timestamp": 0.8732009130435746 - }, - { - "x": 1.9252745544910315, - "y": 7.335877061754608, - "heading": 1.7227399248535007, - "angularVelocity": 0.2558033891347235, - "velocityX": -0.3583820598784613, - "velocityY": -2.2783220270800135, - "timestamp": 0.929263059929074 - }, - { - "x": 1.8944186328107546, - "y": 7.18444418557171, - "heading": 1.7263995638042064, - "angularVelocity": 0.0652782519759728, - "velocityX": -0.5503877998696084, - "velocityY": -2.7011608473036492, - "timestamp": 0.9853252068145734 - }, - { - "x": 1.8552427052945584, - "y": 7.008579942403864, - "heading": 1.7137957175671894, - "angularVelocity": -0.2248191861570831, - "velocityX": -0.6987946358210054, - "velocityY": -3.136951632034859, - "timestamp": 1.0413873537000726 - }, - { - "x": 1.8355326622111128, - "y": 6.825535979197357, - "heading": 1.640395376122022, - "angularVelocity": -1.309267402746448, - "velocityX": -0.3515748892688867, - "velocityY": -3.2650187938816213, - "timestamp": 1.097449500585572 - }, - { - "x": 1.833115207732126, - "y": 6.66209634502094, - "heading": 1.5604071280005414, - "angularVelocity": -1.4267781839473175, - "velocityX": -0.04312097579716382, - "velocityY": -2.9153295629263725, - "timestamp": 1.1535116474710714 - }, - { - "x": 1.8460319685909659, - "y": 6.51990665983822, - "heading": 1.4794141363456246, - "angularVelocity": -1.444700143580583, - "velocityX": 0.23040075302897206, - "velocityY": -2.5362868366979616, - "timestamp": 1.2095737943565708 - }, - { - "x": 1.873572431596194, - "y": 6.399462364741938, - "heading": 1.3993197480219786, - "angularVelocity": -1.4286714436254064, - "velocityX": 0.4912488111002284, - "velocityY": -2.1484067554936095, - "timestamp": 1.2656359412420701 - }, - { - "x": 1.9153719761938701, - "y": 6.300999585265943, - "heading": 1.321078679680736, - "angularVelocity": -1.3956131309248871, - "velocityX": 0.7455930056165527, - "velocityY": -1.756314821069794, - "timestamp": 1.3216980881275695 - }, - { - "x": 1.9712090492248535, - "y": 6.224656105041504, - "heading": 1.245265054015088, - "angularVelocity": -1.3523139921931384, - "velocityX": 0.9959852794261412, - "velocityY": -1.3617651921244192, - "timestamp": 1.3777602350130689 - }, - { - "x": 2.064814407086994, - "y": 6.163458138030285, - "heading": 1.1532966262976785, - "angularVelocity": -1.2884705110143861, - "velocityX": 1.311403774879204, - "velocityY": -0.8573787525244388, - "timestamp": 1.4491382191665476 - }, - { - "x": 2.1766550209872007, - "y": 6.140572903808983, - "heading": 1.0687068975747052, - "angularVelocity": -1.1850955126595724, - "velocityX": 1.5668782920476474, - "velocityY": -0.320620349435673, - "timestamp": 1.5205162033200263 - }, - { - "x": 2.3001626100829933, - "y": 6.158413772092023, - "heading": 0.9953748205049017, - "angularVelocity": -1.0273766895983407, - "velocityX": 1.7303317060653256, - "velocityY": 0.24994917543031295, - "timestamp": 1.591894187473505 - }, - { - "x": 2.425284064916022, - "y": 6.218463097089712, - "heading": 0.938188682532721, - "angularVelocity": -0.801173340076652, - "velocityX": 1.7529418393770948, - "velocityY": 0.8412863673562128, - "timestamp": 1.6632721716269838 - }, - { - "x": 2.53825605792979, - "y": 6.318777115014249, - "heading": 0.9023285345109315, - "angularVelocity": -0.5023978814627547, - "velocityX": 1.5827288253315277, - "velocityY": 1.4053915799700756, - "timestamp": 1.7346501557804626 - }, - { - "x": 2.6251680020884685, - "y": 6.451883384879072, - "heading": 0.8920123220612021, - "angularVelocity": -0.1445293331280909, - "velocityX": 1.2176295700898225, - "velocityY": 1.864808476218863, - "timestamp": 1.8060281399339413 - }, - { - "x": 2.676754131455261, - "y": 6.607487226475557, - "heading": 0.9074045656132124, - "angularVelocity": 0.21564413361567006, - "velocityX": 0.7227176555710894, - "velocityY": 2.179997704360796, - "timestamp": 1.87740612408742 - }, - { - "x": 2.688542366027832, - "y": 6.776451110839844, - "heading": 0.9461608763456704, - "angularVelocity": 0.5429728955236821, - "velocityX": 0.16515224844713225, - "velocityY": 2.36717086323111, - "timestamp": 1.9487841082408988 - }, - { - "x": 2.668148061826547, - "y": 6.925994274438749, - "heading": 0.9947841955704393, - "angularVelocity": 0.7952759643567511, - "velocityX": -0.3335662846480402, - "velocityY": 2.445906316973464, - "timestamp": 2.00992429278564 - }, - { - "x": 2.6170148922734087, - "y": 7.078531073223728, - "heading": 1.0584459400733053, - "angularVelocity": 1.0412422693340033, - "velocityX": -0.8363267126830451, - "velocityY": 2.4948697803382527, - "timestamp": 2.0710644773303812 - }, - { - "x": 2.534967598528086, - "y": 7.2307949110157566, - "heading": 1.1362937192744667, - "angularVelocity": 1.2732669974882682, - "velocityX": -1.341953648917783, - "velocityY": 2.4904052698860224, - "timestamp": 2.1322046618751225 - }, - { - "x": 2.4228120639199595, - "y": 7.375707468805499, - "heading": 1.2259465098868205, - "angularVelocity": 1.4663480537378424, - "velocityX": -1.8343996741791555, - "velocityY": 2.370168799273059, - "timestamp": 2.1933448464198637 - }, - { - "x": 2.2907884513434476, - "y": 7.496616253659566, - "heading": 1.3183186813859389, - "angularVelocity": 1.5108258535190064, - "velocityX": -2.159359078805188, - "velocityY": 1.9775665669701716, - "timestamp": 2.254485030964605 - }, - { - "x": 2.167838907491469, - "y": 7.587898169721127, - "heading": 1.3986249202549763, - "angularVelocity": 1.313477207617367, - "velocityX": -2.0109449254607106, - "velocityY": 1.4929937935460815, - "timestamp": 2.315625215509346 - }, - { - "x": 2.0653171309222214, - "y": 7.656036198681006, - "heading": 1.4635925633092643, - "angularVelocity": 1.062601356833424, - "velocityX": -1.6768313235008876, - "velocityY": 1.1144557293577975, - "timestamp": 2.3767654000540874 - }, - { - "x": 1.9867414717925997, - "y": 7.704735664128929, - "heading": 1.5125342952845975, - "angularVelocity": 0.8004838771711337, - "velocityX": -1.2851720961378765, - "velocityY": 0.7965214009500676, - "timestamp": 2.4379055845988287 - }, - { - "x": 1.933638629948872, - "y": 7.736059479054318, - "heading": 1.545239790247936, - "angularVelocity": 0.534926337021526, - "velocityX": -0.8685423872881548, - "velocityY": 0.5123277785082085, - "timestamp": 2.49904576914357 - }, - { - "x": 1.906832933425903, - "y": 7.751288414001465, - "heading": 1.5616220933383522, - "angularVelocity": 0.26794657576520653, - "velocityX": -0.43843008853453735, - "velocityY": 0.24908225352186592, - "timestamp": 2.560185953688311 - }, - { - "x": 1.906832933425903, - "y": 7.751288414001465, - "heading": 1.5616220933383522, - "angularVelocity": 1.5826865975077623e-32, - "velocityX": -2.879811776726256e-33, - "velocityY": -5.7654843876684295e-33, - "timestamp": 2.6213261382330524 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePAmpAAmpSprint.traj b/src/main/deploy/choreo/AmpLanePAmpAAmpSprint.traj deleted file mode 100644 index 3eeea5a5..00000000 --- a/src/main/deploy/choreo/AmpLanePAmpAAmpSprint.traj +++ /dev/null @@ -1,391 +0,0 @@ -{ - "samples": [ - { - "x": 1.228834867477417, - "y": 7.059738636016846, - "heading": 1.0088745501500993, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.2472218556025594, - "y": 7.08948625549367, - "heading": 1.0202423017774995, - "angularVelocity": 0.17517082812845983, - "velocityX": 0.2833334191526489, - "velocityY": 0.4583945277310271, - "timestamp": 0.0648952325501577 - }, - { - "x": 1.2842125125618584, - "y": 7.148836974041856, - "heading": 1.0432930699841776, - "angularVelocity": 0.35519971654099614, - "velocityX": 0.5700057693869717, - "velocityY": 0.9145620751464901, - "timestamp": 0.1297904651003154 - }, - { - "x": 1.3401709968856181, - "y": 7.237542105970113, - "heading": 1.0785564128874017, - "angularVelocity": 0.5433888055793495, - "velocityX": 0.8622896031770767, - "velocityY": 1.366897512227833, - "timestamp": 0.19468569765047308 - }, - { - "x": 1.4158364633522358, - "y": 7.355073707856503, - "heading": 1.1271095473140742, - "angularVelocity": 0.748177216086649, - "velocityX": 1.1659634073756595, - "velocityY": 1.8110976302542545, - "timestamp": 0.2595809302006308 - }, - { - "x": 1.5134954683399044, - "y": 7.499582339482829, - "heading": 1.1923271139091147, - "angularVelocity": 1.0049669911365822, - "velocityX": 1.504871793350733, - "velocityY": 2.226798887185344, - "timestamp": 0.3244761627507885 - }, - { - "x": 1.6221627697852088, - "y": 7.611419443621613, - "heading": 1.2812853901227188, - "angularVelocity": 1.3707983270550417, - "velocityX": 1.674503614133984, - "velocityY": 1.723348538004631, - "timestamp": 0.3893713953009462 - }, - { - "x": 1.7171722095056663, - "y": 7.6909348739854755, - "heading": 1.365395685532232, - "angularVelocity": 1.2960935973918277, - "velocityX": 1.4640434433611809, - "velocityY": 1.2252892429718023, - "timestamp": 0.45426662785110394 - }, - { - "x": 1.7966706946595994, - "y": 7.739013205131253, - "heading": 1.442194898226158, - "angularVelocity": 1.183433815952006, - "velocityX": 1.225028126565204, - "velocityY": 0.740860757508164, - "timestamp": 0.5191618604012617 - }, - { - "x": 1.8600025780551501, - "y": 7.756003639472796, - "heading": 1.5108089040033452, - "angularVelocity": 1.0573042592020157, - "velocityX": 0.9759096455444755, - "velocityY": 0.26181329003499115, - "timestamp": 0.5840570929514194 - }, - { - "x": 1.906832933425903, - "y": 7.742092132568359, - "heading": 1.5707963267948966, - "angularVelocity": 0.9243733389072425, - "velocityX": 0.721630134148261, - "velocityY": -0.21436870410603143, - "timestamp": 0.6489523255015771 - }, - { - "x": 1.9348222809552327, - "y": 7.70709813704719, - "heading": 1.6159821878126526, - "angularVelocity": 0.8059959086126847, - "velocityX": 0.49925572037929106, - "velocityY": -0.6242000612755755, - "timestamp": 0.7050144723870765 - }, - { - "x": 1.9504564809107534, - "y": 7.649074139407456, - "heading": 1.6542622365278856, - "angularVelocity": 0.6828145342599153, - "velocityX": 0.27887265872018596, - "velocityY": -1.03499421380064, - "timestamp": 0.7610766192725759 - }, - { - "x": 1.9538923564371444, - "y": 7.567945861709625, - "heading": 1.6852575777384742, - "angularVelocity": 0.5528746744910241, - "velocityX": 0.06128690600109267, - "velocityY": -1.4471132877505835, - "timestamp": 0.8171387661580752 - }, - { - "x": 1.9453662221730657, - "y": 7.463604685889236, - "heading": 1.7083990376780211, - "angularVelocity": 0.4127822644182869, - "velocityX": -0.15208362037031065, - "velocityY": -1.8611698198696156, - "timestamp": 0.8732009130435746 - }, - { - "x": 1.9252745544910315, - "y": 7.335877061754608, - "heading": 1.7227399248535007, - "angularVelocity": 0.2558033891347235, - "velocityX": -0.3583820598784613, - "velocityY": -2.2783220270800135, - "timestamp": 0.929263059929074 - }, - { - "x": 1.8944186328107546, - "y": 7.18444418557171, - "heading": 1.7263995638042064, - "angularVelocity": 0.0652782519759728, - "velocityX": -0.5503877998696084, - "velocityY": -2.7011608473036492, - "timestamp": 0.9853252068145734 - }, - { - "x": 1.8552427052945584, - "y": 7.008579942403864, - "heading": 1.7137957175671894, - "angularVelocity": -0.2248191861570831, - "velocityX": -0.6987946358210054, - "velocityY": -3.136951632034859, - "timestamp": 1.0413873537000726 - }, - { - "x": 1.8355326622111128, - "y": 6.825535979197357, - "heading": 1.640395376122022, - "angularVelocity": -1.309267402746448, - "velocityX": -0.3515748892688867, - "velocityY": -3.2650187938816213, - "timestamp": 1.097449500585572 - }, - { - "x": 1.833115207732126, - "y": 6.66209634502094, - "heading": 1.5604071280005414, - "angularVelocity": -1.4267781839473175, - "velocityX": -0.04312097579716382, - "velocityY": -2.9153295629263725, - "timestamp": 1.1535116474710714 - }, - { - "x": 1.8460319685909659, - "y": 6.51990665983822, - "heading": 1.4794141363456246, - "angularVelocity": -1.444700143580583, - "velocityX": 0.23040075302897206, - "velocityY": -2.5362868366979616, - "timestamp": 1.2095737943565708 - }, - { - "x": 1.873572431596194, - "y": 6.399462364741938, - "heading": 1.3993197480219786, - "angularVelocity": -1.4286714436254064, - "velocityX": 0.4912488111002284, - "velocityY": -2.1484067554936095, - "timestamp": 1.2656359412420701 - }, - { - "x": 1.9153719761938701, - "y": 6.300999585265943, - "heading": 1.321078679680736, - "angularVelocity": -1.3956131309248871, - "velocityX": 0.7455930056165527, - "velocityY": -1.756314821069794, - "timestamp": 1.3216980881275695 - }, - { - "x": 1.9712090492248535, - "y": 6.224656105041504, - "heading": 1.245265054015088, - "angularVelocity": -1.3523139921931384, - "velocityX": 0.9959852794261412, - "velocityY": -1.3617651921244192, - "timestamp": 1.3777602350130689 - }, - { - "x": 2.064814407086994, - "y": 6.163458138030285, - "heading": 1.1532966262976785, - "angularVelocity": -1.2884705110143861, - "velocityX": 1.311403774879204, - "velocityY": -0.8573787525244388, - "timestamp": 1.4491382191665476 - }, - { - "x": 2.1766550209872007, - "y": 6.140572903808983, - "heading": 1.0687068975747052, - "angularVelocity": -1.1850955126595724, - "velocityX": 1.5668782920476474, - "velocityY": -0.320620349435673, - "timestamp": 1.5205162033200263 - }, - { - "x": 2.3001626100829933, - "y": 6.158413772092023, - "heading": 0.9953748205049017, - "angularVelocity": -1.0273766895983407, - "velocityX": 1.7303317060653256, - "velocityY": 0.24994917543031295, - "timestamp": 1.591894187473505 - }, - { - "x": 2.425284064916022, - "y": 6.218463097089712, - "heading": 0.938188682532721, - "angularVelocity": -0.801173340076652, - "velocityX": 1.7529418393770948, - "velocityY": 0.8412863673562128, - "timestamp": 1.6632721716269838 - }, - { - "x": 2.53825605792979, - "y": 6.318777115014249, - "heading": 0.9023285345109315, - "angularVelocity": -0.5023978814627547, - "velocityX": 1.5827288253315277, - "velocityY": 1.4053915799700756, - "timestamp": 1.7346501557804626 - }, - { - "x": 2.6251680020884685, - "y": 6.451883384879072, - "heading": 0.8920123220612021, - "angularVelocity": -0.1445293331280909, - "velocityX": 1.2176295700898225, - "velocityY": 1.864808476218863, - "timestamp": 1.8060281399339413 - }, - { - "x": 2.676754131455261, - "y": 6.607487226475557, - "heading": 0.9074045656132124, - "angularVelocity": 0.21564413361567006, - "velocityX": 0.7227176555710894, - "velocityY": 2.179997704360796, - "timestamp": 1.87740612408742 - }, - { - "x": 2.688542366027832, - "y": 6.776451110839844, - "heading": 0.9461608763456704, - "angularVelocity": 0.5429728955236821, - "velocityX": 0.16515224844713225, - "velocityY": 2.36717086323111, - "timestamp": 1.9487841082408988 - }, - { - "x": 2.668148061826547, - "y": 6.925994274438749, - "heading": 0.9947841955704393, - "angularVelocity": 0.7952759643567511, - "velocityX": -0.3335662846480402, - "velocityY": 2.445906316973464, - "timestamp": 2.00992429278564 - }, - { - "x": 2.6170148922734087, - "y": 7.078531073223728, - "heading": 1.0584459400733053, - "angularVelocity": 1.0412422693340033, - "velocityX": -0.8363267126830451, - "velocityY": 2.4948697803382527, - "timestamp": 2.0710644773303812 - }, - { - "x": 2.534967598528086, - "y": 7.2307949110157566, - "heading": 1.1362937192744667, - "angularVelocity": 1.2732669974882682, - "velocityX": -1.341953648917783, - "velocityY": 2.4904052698860224, - "timestamp": 2.1322046618751225 - }, - { - "x": 2.4228120639199595, - "y": 7.375707468805499, - "heading": 1.2259465098868205, - "angularVelocity": 1.4663480537378424, - "velocityX": -1.8343996741791555, - "velocityY": 2.370168799273059, - "timestamp": 2.1933448464198637 - }, - { - "x": 2.2907884513434476, - "y": 7.496616253659566, - "heading": 1.3183186813859389, - "angularVelocity": 1.5108258535190064, - "velocityX": -2.159359078805188, - "velocityY": 1.9775665669701716, - "timestamp": 2.254485030964605 - }, - { - "x": 2.167838907491469, - "y": 7.587898169721127, - "heading": 1.3986249202549763, - "angularVelocity": 1.313477207617367, - "velocityX": -2.0109449254607106, - "velocityY": 1.4929937935460815, - "timestamp": 2.315625215509346 - }, - { - "x": 2.0653171309222214, - "y": 7.656036198681006, - "heading": 1.4635925633092643, - "angularVelocity": 1.062601356833424, - "velocityX": -1.6768313235008876, - "velocityY": 1.1144557293577975, - "timestamp": 2.3767654000540874 - }, - { - "x": 1.9867414717925997, - "y": 7.704735664128929, - "heading": 1.5125342952845975, - "angularVelocity": 0.8004838771711337, - "velocityX": -1.2851720961378765, - "velocityY": 0.7965214009500676, - "timestamp": 2.4379055845988287 - }, - { - "x": 1.933638629948872, - "y": 7.736059479054318, - "heading": 1.545239790247936, - "angularVelocity": 0.534926337021526, - "velocityX": -0.8685423872881548, - "velocityY": 0.5123277785082085, - "timestamp": 2.49904576914357 - }, - { - "x": 1.906832933425903, - "y": 7.751288414001465, - "heading": 1.5616220933383522, - "angularVelocity": 0.26794657576520653, - "velocityX": -0.43843008853453735, - "velocityY": 0.24908225352186592, - "timestamp": 2.560185953688311 - }, - { - "x": 1.906832933425903, - "y": 7.751288414001465, - "heading": 1.5616220933383522, - "angularVelocity": 1.5826865975077623e-32, - "velocityX": -2.879811776726256e-33, - "velocityY": -5.7654843876684295e-33, - "timestamp": 2.6213261382330524 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePSprint.1.traj b/src/main/deploy/choreo/AmpLanePSprint.1.traj deleted file mode 100644 index 213426f1..00000000 --- a/src/main/deploy/choreo/AmpLanePSprint.1.traj +++ /dev/null @@ -1,148 +0,0 @@ -{ - "samples": [ - { - "x": 1.4685733318328855, - "y": 7.0094075202941895, - "heading": 6.049320357152304e-20, - "angularVelocity": 5.735208962067612e-20, - "velocityX": 1.2024934428735394e-18, - "velocityY": 5.740194914400006e-19, - "timestamp": 0 - }, - { - "x": 1.4539549354530197, - "y": 7.003550802171312, - "heading": 0.018695171024518124, - "angularVelocity": 0.28848806436406915, - "velocityX": -0.22557872673127724, - "velocityY": -0.09037591967352629, - "timestamp": 0.06480396707478588 - }, - { - "x": 1.4247253207548556, - "y": 6.991839516204374, - "heading": 0.05623662341413578, - "angularVelocity": 0.5793079356745707, - "velocityX": -0.45104668768861206, - "velocityY": -0.1807186580633629, - "timestamp": 0.12960793414957175 - }, - { - "x": 1.3808979484465476, - "y": 6.974274102040644, - "heading": 0.11287119216832771, - "angularVelocity": 0.8739367558907741, - "velocityX": -0.6763069343845826, - "velocityY": -0.2710546121884731, - "timestamp": 0.19441190122435764 - }, - { - "x": 1.3224921483061116, - "y": 6.950848437335187, - "heading": 0.18890117826912542, - "angularVelocity": 1.1732304291349192, - "velocityX": -0.9012689002979366, - "velocityY": -0.36148504116147834, - "timestamp": 0.2592158682991435 - }, - { - "x": 1.2495302432323667, - "y": 6.921544701913893, - "heading": 0.28459715171144206, - "angularVelocity": 1.4766993096561234, - "velocityX": -1.1258863981204084, - "velocityY": -0.45219045598668955, - "timestamp": 0.32401983537392937 - }, - { - "x": 1.1620307559069942, - "y": 6.886330686053142, - "heading": 0.40005759835574417, - "angularVelocity": 1.7816879406634767, - "velocityX": -1.3502180695881552, - "velocityY": -0.5433929040194694, - "timestamp": 0.38882380244871523 - }, - { - "x": 1.0599976129357116, - "y": 6.845163950858531, - "heading": 0.5350484398222678, - "angularVelocity": 2.083064472129233, - "velocityX": -1.574489148998107, - "velocityY": -0.6352502331701971, - "timestamp": 0.4536277695235011 - }, - { - "x": 0.9575825133734215, - "y": 6.803331405068127, - "heading": 0.6590788452444744, - "angularVelocity": 1.9139322949021502, - "velocityX": -1.5803831800003518, - "velocityY": -0.6455244590524392, - "timestamp": 0.518431736598287 - }, - { - "x": 0.8697427157925169, - "y": 6.767497218853931, - "heading": 0.7646238641532083, - "angularVelocity": 1.6286814476486076, - "velocityX": -1.3554694495714084, - "velocityY": -0.5529628482904638, - "timestamp": 0.5832357036730729 - }, - { - "x": 0.7965061565345849, - "y": 6.737660077605358, - "heading": 0.8521458046235697, - "angularVelocity": 1.3505645475277608, - "velocityX": -1.130124629151394, - "velocityY": -0.4604215234869663, - "timestamp": 0.6480396707478587 - }, - { - "x": 0.7378960834776489, - "y": 6.713809764371052, - "heading": 0.9219596832810217, - "angularVelocity": 1.0773087174876965, - "velocityX": -0.9044210671438955, - "velocityY": -0.3680378580339382, - "timestamp": 0.7128436378226446 - }, - { - "x": 0.6939290380184983, - "y": 6.695933388139099, - "heading": 0.9742459780095203, - "angularVelocity": 0.80683786948041, - "velocityX": -0.6784622522940782, - "velocityY": -0.27585311577181243, - "timestamp": 0.7776476048974305 - }, - { - "x": 0.6646150045031116, - "y": 6.6840200000677585, - "heading": 1.0090892197053958, - "angularVelocity": 0.5376714307577112, - "velocityX": -0.4523493674632192, - "velocityY": -0.18383732677339054, - "timestamp": 0.8424515719722163 - }, - { - "x": 0.6499583125114444, - "y": 6.67806339263916, - "heading": 1.0265157581506656, - "angularVelocity": 0.26891159958091726, - "velocityX": -0.22616967221702353, - "velocityY": -0.09191732693964001, - "timestamp": 0.9072555390470022 - }, - { - "x": 0.6499583125114444, - "y": 6.67806339263916, - "heading": 1.0265157581506656, - "angularVelocity": 9.703439676406597e-18, - "velocityX": 1.8974989673442822e-18, - "velocityY": 3.469574557132838e-18, - "timestamp": 0.972059506121788 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePSprint.2.traj b/src/main/deploy/choreo/AmpLanePSprint.2.traj deleted file mode 100644 index 36670375..00000000 --- a/src/main/deploy/choreo/AmpLanePSprint.2.traj +++ /dev/null @@ -1,301 +0,0 @@ -{ - "samples": [ - { - "x": 0.6499583125114444, - "y": 6.67806339263916, - "heading": 1.0265157581506656, - "angularVelocity": 9.703439676406597e-18, - "velocityX": 1.8974989673442822e-18, - "velocityY": 3.469574557132838e-18, - "timestamp": 0 - }, - { - "x": 0.677098524254357, - "y": 6.676790336244545, - "heading": 1.0174178053772396, - "angularVelocity": -0.10888280660674853, - "velocityX": 0.32480960278249144, - "velocityY": -0.015235730132528697, - "timestamp": 0.08355729482876995 - }, - { - "x": 0.7313843379611403, - "y": 6.674244414674947, - "heading": 0.999511909828794, - "angularVelocity": -0.21429482111807416, - "velocityX": 0.649683714845345, - "velocityY": -0.03046917177986014, - "timestamp": 0.1671145896575399 - }, - { - "x": 0.8128220453410844, - "y": 6.670425841513566, - "heading": 0.9731501881376037, - "angularVelocity": -0.31549276152616484, - "velocityX": 0.9746331250530577, - "velocityY": -0.04570005729847779, - "timestamp": 0.25067188448630984 - }, - { - "x": 0.9214189734172925, - "y": 6.665334889481153, - "heading": 0.938761218692723, - "angularVelocity": -0.41156154606662587, - "velocityX": 1.299670223871541, - "velocityY": -0.06092767893988849, - "timestamp": 0.3342291793150798 - }, - { - "x": 1.057183650080325, - "y": 6.658971921820971, - "heading": 0.8968713117735069, - "angularVelocity": -0.5013315355056764, - "velocityX": 1.6248093830855639, - "velocityY": -0.07615095334551644, - "timestamp": 0.41778647414384973 - }, - { - "x": 1.2201259982302939, - "y": 6.651337410962851, - "heading": 0.8481374363541635, - "angularVelocity": -0.5832390280130826, - "velocityX": 1.9500672979406355, - "velocityY": -0.09136857378837147, - "timestamp": 0.5013437689726197 - }, - { - "x": 1.410257523594535, - "y": 6.642431937989669, - "heading": 0.7934008809085091, - "angularVelocity": -0.65507811804839, - "velocityX": 2.275462911453366, - "velocityY": -0.1065792399267018, - "timestamp": 0.5849010638013896 - }, - { - "x": 1.6275913452117106, - "y": 6.632256164958325, - "heading": 0.7337801464779199, - "angularVelocity": -0.7135311710696056, - "velocityX": 2.601015531469118, - "velocityY": -0.12178198267664266, - "timestamp": 0.6684583586301596 - }, - { - "x": 1.8721414897299389, - "y": 6.620810753247128, - "heading": 0.6708446221029851, - "angularVelocity": -0.7532020334537907, - "velocityX": 2.9267360201089847, - "velocityY": -0.13697681015944838, - "timestamp": 0.7520156534589295 - }, - { - "x": 2.1439190987685945, - "y": 6.6080960989852695, - "heading": 0.6069767178684699, - "angularVelocity": -0.76436060269055, - "velocityX": 3.2525898498221513, - "velocityY": -0.15216689683305046, - "timestamp": 0.8355729482876995 - }, - { - "x": 2.442913985874264, - "y": 6.5941112960890615, - "heading": 0.5462664171235281, - "angularVelocity": -0.7265709220164723, - "velocityX": 3.578321769731578, - "velocityY": -0.16736782736764083, - "timestamp": 0.9191302431164694 - }, - { - "x": 2.7689778327808057, - "y": 6.578852444576511, - "heading": 0.4974709825138351, - "angularVelocity": -0.5839757583068491, - "velocityX": 3.9022786409579977, - "velocityY": -0.1826154322470609, - "timestamp": 1.0026875379452393 - }, - { - "x": 3.120091446148044, - "y": 6.56305392023088, - "heading": 0.49252318839031584, - "angularVelocity": -0.059214388566174905, - "velocityX": 4.202070137463798, - "velocityY": -0.18907414819982873, - "timestamp": 1.0862448327740093 - }, - { - "x": 3.4728474074143234, - "y": 6.5466252324603085, - "heading": 0.4925231817165128, - "angularVelocity": -7.987099095905687e-8, - "velocityX": 4.221725487752611, - "velocityY": -0.19661584071429156, - "timestamp": 1.169802127602779 - }, - { - "x": 3.825603367792492, - "y": 6.5301965258536905, - "heading": 0.49252317502370946, - "angularVelocity": -8.009838379406554e-8, - "velocityX": 4.221725477123844, - "velocityY": -0.1966160661410119, - "timestamp": 1.2533594224315487 - }, - { - "x": 4.1783593283685585, - "y": 6.513767823497187, - "heading": 0.49252316833088156, - "angularVelocity": -8.009867777354286e-8, - "velocityX": 4.221725479492243, - "velocityY": -0.19661601527633685, - "timestamp": 1.3369167172603185 - }, - { - "x": 4.5311152888161, - "y": 6.497339118379854, - "heading": 0.49252316163811777, - "angularVelocity": -8.009791003841919e-8, - "velocityX": 4.2217254779540765, - "velocityY": -0.19661604831749765, - "timestamp": 1.4204740120890882 - }, - { - "x": 4.883871249955156, - "y": 6.480910427791649, - "heading": 0.49252315497752247, - "angularVelocity": -7.971292311350099e-8, - "velocityX": 4.221725486230021, - "velocityY": -0.19661587443527756, - "timestamp": 1.504031306917858 - }, - { - "x": 5.235292142896319, - "y": 6.465020831099543, - "heading": 0.4884937210472112, - "angularVelocity": -0.04822360439721594, - "velocityX": 4.205747609006577, - "velocityY": -0.19016408710534566, - "timestamp": 1.5875886017466276 - }, - { - "x": 5.56143349165223, - "y": 6.449807932309173, - "heading": 0.4427260149092047, - "angularVelocity": -0.5477404005457096, - "velocityX": 3.903206170379902, - "velocityY": -0.1820654776048722, - "timestamp": 1.6711458965753974 - }, - { - "x": 5.860479824683085, - "y": 6.4358664343174405, - "heading": 0.38609345994808875, - "angularVelocity": -0.6777691292804887, - "velocityX": 3.578937466126377, - "velocityY": -0.16684956137344756, - "timestamp": 1.754703191404167 - }, - { - "x": 6.132305844443899, - "y": 6.42319455429606, - "heading": 0.3267902425497731, - "angularVelocity": -0.7097311793045699, - "velocityX": 3.253169221404942, - "velocityY": -0.1516549817385581, - "timestamp": 1.8382604862329368 - }, - { - "x": 6.376904781169397, - "y": 6.41179175824138, - "heading": 0.2685758916504145, - "angularVelocity": -0.6966998036336328, - "velocityX": 2.927319957243016, - "velocityY": -0.13646679297179057, - "timestamp": 1.9218177810617065 - }, - { - "x": 6.594287281116567, - "y": 6.401657882345162, - "heading": 0.21360285288348452, - "angularVelocity": -0.6579083116511133, - "velocityX": 2.6015981057385837, - "velocityY": -0.12128056463513756, - "timestamp": 2.005375075890476 - }, - { - "x": 6.784465944209141, - "y": 6.392792582186275, - "heading": 0.16326745687666674, - "angularVelocity": -0.6024057637333325, - "velocityX": 2.2760270480548472, - "velocityY": -0.10609845827410212, - "timestamp": 2.088932370719246 - }, - { - "x": 6.947452313541236, - "y": 6.385195340051002, - "heading": 0.11855161426546505, - "angularVelocity": -0.5351518703762895, - "velocityX": 1.950594136228255, - "velocityY": -0.09092254782591588, - "timestamp": 2.1724896655480155 - }, - { - "x": 7.083256400212696, - "y": 6.3788655571350015, - "heading": 0.08018557174620769, - "angularVelocity": -0.45915850432779387, - "velocityX": 1.6252810356026588, - "velocityY": -0.07575380376988836, - "timestamp": 2.2560469603767856 - }, - { - "x": 7.191886785288102, - "y": 6.373802632634511, - "heading": 0.04873492465610617, - "angularVelocity": -0.37639618604873454, - "velocityX": 1.300070631750541, - "velocityY": -0.06059225003472367, - "timestamp": 2.339604255205555 - }, - { - "x": 7.273350833513733, - "y": 6.370006018118368, - "heading": 0.024651280459860023, - "angularVelocity": -0.2882291037018025, - "velocityX": 0.9749483679739933, - "velocityY": -0.04543725983378117, - "timestamp": 2.423161550034325 - }, - { - "x": 7.32765490169191, - "y": 6.367475252448548, - "heading": 0.008303807824017625, - "angularVelocity": -0.1956438713022077, - "velocityX": 0.6499021813650045, - "velocityY": -0.03028778845710075, - "timestamp": 2.5067188448630944 - }, - { - "x": 7.354804515838622, - "y": 6.366209983825684, - "heading": -4.8930235830534376e-17, - "angularVelocity": -0.09937861010260368, - "velocityX": 0.32492212920904096, - "velocityY": -0.015142527357507724, - "timestamp": 2.5902761396918645 - }, - { - "x": 7.354804515838623, - "y": 6.366209983825684, - "heading": -2.4216548868392683e-17, - "angularVelocity": 5.9907409116151e-18, - "velocityX": -8.630903523575738e-17, - "velocityY": 6.541583629386208e-18, - "timestamp": 2.673833434520634 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.1.traj b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.1.traj index e90ff055..e7e52d5a 100644 --- a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.1.traj +++ b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.1.traj @@ -1,355 +1,356 @@ { - "samples": [ - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0, - "velocityX": 2.1425070675381243e-37, - "velocityY": 2.00620509434581e-36, - "timestamp": 0 - }, - { - "x": 0.6967295535333877, - "y": 6.66750179345797, - "heading": 0.982893163249758, - "angularVelocity": -0.14737604826251527, - "velocityX": 0.29094834799603586, - "velocityY": 0.04143420364739335, - "timestamp": 0.06786122358481142 - }, - { - "x": 0.7362175895643961, - "y": 6.673125696218821, - "heading": 0.9628974920292898, - "angularVelocity": -0.2946553298656345, - "velocityX": 0.5818939586560375, - "velocityY": 0.08287358323008266, - "timestamp": 0.13572244716962284 - }, - { - "x": 0.7954495269492323, - "y": 6.681562379154691, - "heading": 0.9329256679312776, - "angularVelocity": -0.44166347900512215, - "velocityX": 0.8728392188627362, - "velocityY": 0.1243225879257299, - "timestamp": 0.20358367075443426 - }, - { - "x": 0.8744254360510917, - "y": 6.692812849791712, - "heading": 0.8930052032103389, - "angularVelocity": -0.5882662087730711, - "velocityX": 1.1637855159383756, - "velocityY": 0.1657864394820339, - "timestamp": 0.2714448943392457 - }, - { - "x": 0.9731453894549041, - "y": 6.7068785148813665, - "heading": 0.843168820166726, - "angularVelocity": -0.7343867441666224, - "velocityX": 1.4547328826223493, - "velocityY": 0.20727102086621554, - "timestamp": 0.33930611792405707 - }, - { - "x": 1.0916093532691815, - "y": 6.7237612036761645, - "heading": 0.7834495219946941, - "angularVelocity": -0.8800209459441316, - "velocityX": 1.7456797498828438, - "velocityY": 0.2487825580347626, - "timestamp": 0.40716734150886846 - }, - { - "x": 1.2298170767085217, - "y": 6.743463170039788, - "heading": 0.7138752049500412, - "angularVelocity": -1.025244069725628, - "velocityX": 2.0366229215807086, - "velocityY": 0.29032730803917456, - "timestamp": 0.47502856509367986 - }, - { - "x": 1.3877679992818084, - "y": 6.765987144948878, - "heading": 0.6344639927011461, - "angularVelocity": -1.1702001239286361, - "velocityX": 2.3275578339061793, - "velocityY": 0.3319123015950357, - "timestamp": 0.5428897886784912 - }, - { - "x": 1.5654611541888466, - "y": 6.791337011948423, - "heading": 0.5452227484246948, - "angularVelocity": -1.3150550426624645, - "velocityX": 2.618478499506586, - "velocityY": 0.3735545228986894, - "timestamp": 0.6107510122633026 - }, - { - "x": 1.7628875214635322, - "y": 6.819592757066269, - "heading": 0.4462737595759067, - "angularVelocity": -1.4581079388455742, - "velocityX": 2.909266247284013, - "velocityY": 0.41637541478357637, - "timestamp": 0.678612235848114 - }, - { - "x": 1.9405727781814897, - "y": 6.845027020375549, - "heading": 0.357188812683824, - "angularVelocity": -1.3127518512946696, - "velocityX": 2.6183621121403804, - "velocityY": 0.3747981831994576, - "timestamp": 0.7464734594329254 - }, - { - "x": 2.098517679535123, - "y": 6.867636881191052, - "heading": 0.2779750280562074, - "angularVelocity": -1.1672908392024057, - "velocityX": 2.3274691054787353, - "velocityY": 0.3331779125268215, - "timestamp": 0.8143346830177368 - }, - { - "x": 2.236722632634811, - "y": 6.887421048029417, - "heading": 0.20863780165516146, - "angularVelocity": -1.0217503124503762, - "velocityX": 2.036582097977683, - "velocityY": 0.29153861061228065, - "timestamp": 0.8821959066025482 - }, - { - "x": 2.3551878284960197, - "y": 6.9043786966768, - "heading": 0.1491789973537457, - "angularVelocity": -0.8761823197470863, - "velocityX": 1.745697905272119, - "velocityY": 0.24988716311885253, - "timestamp": 0.9500571301873596 - }, - { - "x": 2.4539133200632923, - "y": 6.918509209574482, - "heading": 0.09959774943286472, - "angularVelocity": -0.7306270842422328, - "velocityX": 1.454814492755025, - "velocityY": 0.20822661530737346, - "timestamp": 1.017918353772171 - }, - { - "x": 2.5328990844805293, - "y": 6.929812092655753, - "heading": 0.059891727352470836, - "angularVelocity": -0.5851061914728107, - "velocityX": 1.163930743431445, - "velocityY": 0.1665587869506308, - "timestamp": 1.0857795773569825 - }, - { - "x": 2.592145068506356, - "y": 6.938286954912776, - "heading": 0.030058218690118557, - "angularVelocity": -0.43962526884100517, - "velocityX": 0.8730462095453193, - "velocityY": 0.12488519671960763, - "timestamp": 1.153640800941794 - }, - { - "x": 2.63165121786692, - "y": 6.943933511859112, - "heading": 0.010094853186468074, - "angularVelocity": -0.2941792742463702, - "velocityX": 0.582160875882081, - "velocityY": 0.08320741430898287, - "timestamp": 1.2215020245266055 - }, - { - "x": 2.6514174938201904, - "y": 6.946751594543457, - "heading": 0, - "angularVelocity": -0.14875731166049128, - "velocityX": 0.2912749713180046, - "velocityY": 0.041527142239385896, - "timestamp": 1.289363248111417 - }, - { - "x": 2.649146332814054, - "y": 6.946410455915093, - "heading": 0.0008990727847708372, - "angularVelocity": 0.011985225672082133, - "velocityX": -0.030276055128411465, - "velocityY": -0.004547600056030398, - "timestamp": 1.3643783383040946 - }, - { - "x": 2.62275408560099, - "y": 6.9426132004627625, - "heading": 0.013859521326443051, - "angularVelocity": 0.17277121854260333, - "velocityX": -0.35182584124441, - "velocityY": -0.050619887846258024, - "timestamp": 1.4393934284967722 - }, - { - "x": 2.572240863242063, - "y": 6.93536015641962, - "heading": 0.03888546297295733, - "angularVelocity": 0.3336120983422759, - "velocityX": -0.6733741468440916, - "velocityY": -0.09668780007480728, - "timestamp": 1.5144085186894498 - }, - { - "x": 2.4976067609609713, - "y": 6.924651766873113, - "heading": 0.07598096942338527, - "angularVelocity": 0.4945072565419495, - "velocityX": -0.994921183049867, - "velocityY": -0.14274980565911422, - "timestamp": 1.5894236088821274 - }, - { - "x": 2.398851800518621, - "y": 6.910488564953806, - "heading": 0.12514851990507653, - "angularVelocity": 0.6554354644565975, - "velocityX": -1.3164679291686043, - "velocityY": -0.18880470426588675, - "timestamp": 1.664438699074805 - }, - { - "x": 2.275975845765142, - "y": 6.89287117810912, - "heading": 0.18638682928509862, - "angularVelocity": 0.8163465407124133, - "velocityX": -1.6380164902537586, - "velocityY": -0.23485123858993803, - "timestamp": 1.7394537892674826 - }, - { - "x": 2.1289785005722504, - "y": 6.871800394460987, - "heading": 0.2596886166153821, - "angularVelocity": 0.9771605571893133, - "velocityX": -1.95957033198688, - "velocityY": -0.28088726673543657, - "timestamp": 1.8144688794601602 - }, - { - "x": 1.9578590220057892, - "y": 6.847277322910046, - "heading": 0.34503981582540255, - "angularVelocity": 1.1377870637866898, - "velocityX": -2.2811340775161044, - "velocityY": -0.3269085125133225, - "timestamp": 1.8894839696528378 - }, - { - "x": 1.7626163173700675, - "y": 6.8193036552908985, - "heading": 0.4424228597913738, - "angularVelocity": 1.298179389184776, - "velocityX": -2.602712389390415, - "velocityY": -0.3729072050343056, - "timestamp": 1.9644990598455154 - }, - { - "x": 1.5454887933501882, - "y": 6.788364934906598, - "heading": 0.5521114777288451, - "angularVelocity": 1.4622207032709544, - "velocityX": -2.8944512825643915, - "velocityY": -0.4124332891533426, - "timestamp": 2.039514150038193 - }, - { - "x": 1.3524867399067801, - "y": 6.760871330718944, - "heading": 0.6497679920992822, - "angularVelocity": 1.3018249277525966, - "velocityX": -2.5728430499473998, - "velocityY": -0.3665076468885929, - "timestamp": 2.114529240230871 - }, - { - "x": 1.1836105740084832, - "y": 6.736819962879357, - "heading": 0.7353592838772256, - "angularVelocity": 1.140987654058677, - "velocityX": -2.251229258866918, - "velocityY": -0.3206203948806917, - "timestamp": 2.1895443304235487 - }, - { - "x": 1.0388602648983114, - "y": 6.7162082631102376, - "heading": 0.8088372428736526, - "angularVelocity": 0.9795090402170741, - "velocityX": -1.9296158777970915, - "velocityY": -0.2747673796855824, - "timestamp": 2.2645594206162265 - }, - { - "x": 0.9182354949315064, - "y": 6.6990341480262074, - "heading": 0.8701477848986364, - "angularVelocity": 0.8173094489056358, - "velocityX": -1.6080067311387376, - "velocityY": -0.22894213737419178, - "timestamp": 2.3395745108089043 - }, - { - "x": 0.8217358494246105, - "y": 6.685296075556227, - "heading": 0.919240073281317, - "angularVelocity": 0.6544321716682079, - "velocityX": -1.286403112480899, - "velocityY": -0.18313745187394112, - "timestamp": 2.414589601001582 - }, - { - "x": 0.7493609915443247, - "y": 6.674993018423988, - "heading": 0.956074222977567, - "angularVelocity": 0.49102320082053863, - "velocityX": -0.9648039840302745, - "velocityY": -0.13734646063578734, - "timestamp": 2.48960469119426 - }, - { - "x": 0.7011107973318162, - "y": 6.66812439818581, - "heading": 0.9806268225378217, - "angularVelocity": 0.32730214010529163, - "velocityX": -0.6432065080316098, - "velocityY": -0.09156318042856706, - "timestamp": 2.564619781386938 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.16353322568319786, - "velocityX": -0.32160668782044516, - "velocityY": -0.04578252824590232, - "timestamp": 2.6396348715796156 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -9.098710086763465e-32, - "velocityX": -1.2738111607141702e-32, - "velocityY": 8.271252813231263e-33, - "timestamp": 2.7146499617722935 - } - ] + "samples": [ + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": -3.9907195357987423e-28, + "velocityX": 1.1657419174076564e-26, + "velocityY": -4.7095973646167984e-27, + "timestamp": 0 + }, + { + "x": 0.6967295535340264, + "y": 6.667501793456135, + "heading": 0.9828931632504835, + "angularVelocity": -0.14737604788210726, + "velocityX": 0.2909483472755575, + "velocityY": 0.0414342035164189, + "timestamp": 0.06786122375505199 + }, + { + "x": 0.7362175895663237, + "y": 6.673125696213235, + "heading": 0.9628974920314577, + "angularVelocity": -0.2946553291051897, + "velocityX": 0.5818939572152577, + "velocityY": 0.08287358296691186, + "timestamp": 0.13572244751010398 + }, + { + "x": 0.7954495269531124, + "y": 6.681562379143333, + "heading": 0.9329256679355464, + "angularVelocity": -0.4416634778661797, + "velocityX": 0.8728392167018533, + "velocityY": 0.12432258752878259, + "timestamp": 0.20358367126515597 + }, + { + "x": 0.874425436057604, + "y": 6.692812849772419, + "heading": 0.8930052032172396, + "angularVelocity": -0.5882662072585287, + "velocityX": 1.163785513057627, + "velocityY": 0.16578643894920225, + "timestamp": 0.27144489502020797 + }, + { + "x": 0.9731453894647497, + "y": 6.7068785148517795, + "heading": 0.8431688201765567, + "angularVelocity": -0.7343867422811217, + "velocityX": 1.4547328790220406, + "velocityY": 0.2072710201945652, + "timestamp": 0.33930611877525996 + }, + { + "x": 1.0916093532830902, + "y": 6.723761203633637, + "heading": 0.7834495220073406, + "angularVelocity": -0.8800209436949645, + "velocityX": 1.745679745563404, + "velocityY": 0.24878255721994524, + "timestamp": 0.40716734253031195 + }, + { + "x": 1.229817076727268, + "y": 6.743463169981193, + "heading": 0.7138752049646239, + "angularVelocity": -1.0252440671251137, + "velocityX": 2.0366229165428056, + "velocityY": 0.29032730707408716, + "timestamp": 0.47502856628536394 + }, + { + "x": 1.387767999306249, + "y": 6.765987144870166, + "heading": 0.6344639927151499, + "angularVelocity": -1.1702001210015336, + "velocityX": 2.327557828151045, + "velocityY": 0.33191230046593584, + "timestamp": 0.5428897900404159 + }, + { + "x": 1.5654611542200556, + "y": 6.791337011842962, + "heading": 0.5452227484307262, + "angularVelocity": -1.3150550394809215, + "velocityX": 2.618478493037463, + "velocityY": 0.37355452156738955, + "timestamp": 0.6107510137954679 + }, + { + "x": 1.7628875214933553, + "y": 6.819592756978724, + "heading": 0.4462737596207199, + "angularVelocity": -1.4581079346161945, + "velocityX": 2.909266239965237, + "velocityY": 0.4163754140030483, + "timestamp": 0.6786122375505199 + }, + { + "x": 1.9405727782098796, + "y": 6.845027020300295, + "heading": 0.3571888127510162, + "angularVelocity": -1.3127518476716487, + "velocityX": 2.618362105550687, + "velocityY": 0.37479818244033536, + "timestamp": 0.7464734613055719 + }, + { + "x": 2.098517679561726, + "y": 6.867636881126044, + "heading": 0.2779750281361537, + "angularVelocity": -1.167290836086129, + "velocityX": 2.32746909961358, + "velocityY": 0.33317791184197254, + "timestamp": 0.8143346850606239 + }, + { + "x": 2.23672263265918, + "y": 6.887421047973718, + "heading": 0.20863780174038785, + "angularVelocity": -1.0217503098093481, + "velocityX": 2.036582092835676, + "velocityY": 0.2915386100180838, + "timestamp": 0.8821959088156759 + }, + { + "x": 2.355187828517663, + "y": 6.904378696630012, + "heading": 0.14917899743785948, + "angularVelocity": -0.87618231756544, + "velocityX": 1.7456979008526012, + "velocityY": 0.24988716262327937, + "timestamp": 0.9500571325707279 + }, + { + "x": 2.4539133200816936, + "y": 6.918509209536525, + "heading": 0.09959774951013609, + "angularVelocity": -0.7306270825101687, + "velocityX": 1.4548144890576191, + "velocityY": 0.20822661491514247, + "timestamp": 1.0179183563257799 + }, + { + "x": 2.5328990844951553, + "y": 6.92981209262676, + "heading": 0.05989172741762143, + "angularVelocity": -0.5851061901835907, + "velocityX": 1.1639307404559098, + "velocityY": 0.16655878666486323, + "timestamp": 1.0857795800808319 + }, + { + "x": 2.5921450685166625, + "y": 6.938286954893026, + "heading": 0.030058218738196592, + "angularVelocity": -0.43962526798971635, + "velocityX": 0.8730462072914873, + "velocityY": 0.12488519654252121, + "timestamp": 1.1536408038358839 + }, + { + "x": 2.631651217872353, + "y": 6.943933511848997, + "heading": 0.010094853212766689, + "angularVelocity": -0.2941792738293157, + "velocityX": 0.5821608743498282, + "velocityY": 0.08320741424223291, + "timestamp": 1.2215020275909358 + }, + { + "x": 2.6514174938201904, + "y": 6.946751594543457, + "heading": 1.1828263155272929e-27, + "angularVelocity": -0.14875731167484535, + "velocityX": 0.2912749705072336, + "velocityY": 0.04152714228425266, + "timestamp": 1.2893632513459878 + }, + { + "x": 2.6491463328077915, + "y": 6.946410455927006, + "heading": 0.0008990727501414491, + "angularVelocity": 0.011985225180580105, + "velocityX": -0.03027605513644419, + "velocityY": -0.004547599885899398, + "timestamp": 1.364378341725619 + }, + { + "x": 2.6227540855884963, + "y": 6.942613200487609, + "heading": 0.013859521250247003, + "angularVelocity": 0.17277121755790997, + "velocityX": -0.3518258404506491, + "velocityY": -0.05061988754768401, + "timestamp": 1.4393934321052502 + }, + { + "x": 2.572240863223279, + "y": 6.93536015645873, + "heading": 0.03888546284671504, + "angularVelocity": 0.33361209684369453, + "velocityX": -0.6733741452497535, + "velocityY": -0.09668779964370192, + "timestamp": 1.5144085224848813 + }, + { + "x": 2.497606760935717, + "y": 6.9246517669282275, + "heading": 0.07598096923656177, + "angularVelocity": 0.49450725450194494, + "velocityX": -0.9949211806565718, + "velocityY": -0.1427498050900185, + "timestamp": 1.5894236128645125 + }, + { + "x": 2.3988518004865456, + "y": 6.910488565027239, + "heading": 0.12514851964427576, + "angularVelocity": 0.6554354618369476, + "velocityX": -1.3164679259786103, + "velocityY": -0.1888047035511428, + "timestamp": 1.6644387032441437 + }, + { + "x": 2.275975845725643, + "y": 6.892871178204052, + "heading": 0.18638682893267958, + "angularVelocity": 0.8163465374565724, + "velocityX": -1.6380164862704298, + "velocityY": -0.23485123771803745, + "timestamp": 1.7394537936237748 + }, + { + "x": 2.128978500524308, + "y": 6.871800394582041, + "heading": 0.2596886161467297, + "angularVelocity": 0.9771605532045546, + "velocityX": -1.959570327215777, + "velocityY": -0.28088726568717615, + "timestamp": 1.814468884003406 + }, + { + "x": 1.957859021947552, + "y": 6.847277323064731, + "heading": 0.34503981520215615, + "angularVelocity": 1.1377870588902428, + "velocityX": -2.2811340719682667, + "velocityY": -0.32690851125028303, + "timestamp": 1.8894839743830372 + }, + { + "x": 1.76261631729721, + "y": 6.81930365549537, + "heading": 0.44242285893441274, + "angularVelocity": 1.298179382833869, + "velocityX": -2.602712383098801, + "velocityY": -0.3729072034412448, + "timestamp": 1.9644990647626683 + }, + { + "x": 1.5454887932969013, + "y": 6.788364935052348, + "heading": 0.5521114771232074, + "angularVelocity": 1.4622207029770928, + "velocityX": -2.8944512750899136, + "velocityY": -0.41243328890826375, + "timestamp": 2.0395141551422995 + }, + { + "x": 1.3524867398675213, + "y": 6.760871330824376, + "heading": 0.6497679916624863, + "angularVelocity": 1.3018249267589428, + "velocityX": -2.5728430433483203, + "velocityY": -0.3665076465126455, + "timestamp": 2.114529245521931 + }, + { + "x": 1.1836105739801852, + "y": 6.7368199629542165, + "heading": 0.7353592835672482, + "angularVelocity": 1.1409876529056668, + "velocityX": -2.2512292531102633, + "velocityY": -0.3206203944891842, + "timestamp": 2.1895443359015623 + }, + { + "x": 1.0388602648787244, + "y": 6.71620826316138, + "heading": 0.8088372426617098, + "angularVelocity": 0.9795090390827935, + "velocityX": -1.9296158728719546, + "velocityY": -0.2747673793169661, + "timestamp": 2.2645594262811937 + }, + { + "x": 0.9182354949187841, + "y": 6.699034148059042, + "heading": 0.8701477847623594, + "angularVelocity": 0.8173094478774001, + "velocityX": -1.6080067270397285, + "velocityY": -0.2289421370476856, + "timestamp": 2.339574516660825 + }, + { + "x": 0.821735849417145, + "y": 6.685296075575293, + "heading": 0.9192400732020367, + "angularVelocity": 0.6544321707970268, + "velocityX": -1.2864031092048316, + "velocityY": -0.18313745160106204, + "timestamp": 2.4145896070404564 + }, + { + "x": 0.7493609915406636, + "y": 6.674993018433249, + "heading": 0.9560742229389826, + "angularVelocity": 0.49102320013930684, + "velocityX": -0.9648039815750613, + "velocityY": -0.13734646042419815, + "timestamp": 2.489604697420088 + }, + { + "x": 0.7011107973306167, + "y": 6.6681243981888185, + "heading": 0.9806268225252659, + "angularVelocity": 0.32730213963656085, + "velocityX": -0.643206506395788, + "velocityY": -0.09156318028372816, + "timestamp": 2.564619787799719 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.16353322544301652, + "velocityX": -0.3216066870029412, + "velocityY": -0.04578252817189861, + "timestamp": 2.6396348781793506 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": -5.340812593461075e-28, + "velocityX": 3.892811349542242e-27, + "velocityY": 2.3219495328630078e-26, + "timestamp": 2.714649968558982 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.2.traj b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.2.traj index 4123f0eb..b295df47 100644 --- a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.2.traj +++ b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.2.traj @@ -1,661 +1,662 @@ { - "samples": [ - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -9.098710086763465e-32, - "velocityX": -1.2738111607141702e-32, - "velocityY": 8.271252813231263e-33, - "timestamp": 0 - }, - { - "x": 0.6985020843742907, - "y": 6.667569321822967, - "heading": 0.9798246448067045, - "angularVelocity": -0.18429871952070437, - "velocityX": 0.3034123592993086, - "velocityY": 0.04060189632535011, - "timestamp": 0.0709155084703319 - }, - { - "x": 0.7415443471125377, - "y": 6.673333307735536, - "heading": 0.9539749013573504, - "angularVelocity": -0.36451467396822734, - "velocityX": 0.6069513378199061, - "velocityY": 0.081279624681546, - "timestamp": 0.1418310169406638 - }, - { - "x": 0.8061231921601518, - "y": 6.681988659505296, - "heading": 0.9157093279961269, - "angularVelocity": -0.5395938658076773, - "velocityX": 0.9106448848862336, - "velocityY": 0.12205160699625474, - "timestamp": 0.2127465254109957 - }, - { - "x": 0.8922518573326212, - "y": 6.693543414119111, - "heading": 0.8654790283080297, - "angularVelocity": -0.7083119161320186, - "velocityX": 1.2145251021996393, - "velocityY": 0.16293692117639821, - "timestamp": 0.2836620338813276 - }, - { - "x": 0.9999464058047857, - "y": 6.708007002320034, - "heading": 0.8038456081958845, - "angularVelocity": -0.8691105999462724, - "velocityX": 1.5186318309657076, - "velocityY": 0.20395522097926097, - "timestamp": 0.3545775423516595 - }, - { - "x": 1.1292266926067027, - "y": 6.725390389005233, - "heading": 0.7315277652632032, - "angularVelocity": -1.0197747219557198, - "velocityX": 1.8230185412264566, - "velocityY": 0.2451281399536478, - "timestamp": 0.4254930508219914 - }, - { - "x": 1.280117879093583, - "y": 6.7457065764596775, - "heading": 0.6494907937479546, - "angularVelocity": -1.1568269520279806, - "velocityX": 2.127760059000449, - "velocityY": 0.28648440789144525, - "timestamp": 0.4964085592923233 - }, - { - "x": 1.452652377278095, - "y": 6.768971929861101, - "heading": 0.5591180429519423, - "angularVelocity": -1.2743721753587984, - "velocityX": 2.4329586278958, - "velocityY": 0.3280714459116738, - "timestamp": 0.5673240677626552 - }, - { - "x": 1.6468709656789369, - "y": 6.795208953072923, - "heading": 0.46255966504991763, - "angularVelocity": -1.361597483890574, - "velocityX": 2.738732226422582, - "velocityY": 0.3699758173883681, - "timestamp": 0.6382395762329871 - }, - { - "x": 1.8628151056303066, - "y": 6.824451100190135, - "heading": 0.3635782548385496, - "angularVelocity": -1.3957653600238602, - "velocityX": 3.045090483158721, - "velocityY": 0.41235193468922193, - "timestamp": 0.709155084703319 - }, - { - "x": 2.100434676341306, - "y": 6.8567478442017284, - "heading": 0.27059443428530877, - "angularVelocity": -1.3111916216766784, - "velocityX": 3.3507419721937035, - "velocityY": 0.4554256848500797, - "timestamp": 0.7800705931736509 - }, - { - "x": 2.35596385739112, - "y": 6.892112273565999, - "heading": 0.23346194806127452, - "angularVelocity": -0.5236158778945916, - "velocityX": 3.603290543375525, - "velocityY": 0.4986839991292765, - "timestamp": 0.8509861016439828 - }, - { - "x": 2.621781213038304, - "y": 6.927777327664925, - "heading": 0.23346192227272974, - "angularVelocity": -3.6365169422849345e-7, - "velocityX": 3.7483670551187007, - "velocityY": 0.5029231950560806, - "timestamp": 0.9219016101143147 - }, - { - "x": 2.887598578486195, - "y": 6.963442308718448, - "heading": 0.23346189648452345, - "angularVelocity": -3.636469208413295e-7, - "velocityX": 3.748367193321304, - "velocityY": 0.502922165021807, - "timestamp": 0.9928171185846466 - }, - { - "x": 3.153415943934485, - "y": 6.999107289769, - "heading": 0.23346187069631716, - "angularVelocity": -3.636469205820025e-7, - "velocityX": 3.7483671933269203, - "velocityY": 0.5029221649799431, - "timestamp": 1.0637326270549785 - }, - { - "x": 3.4192333093827747, - "y": 7.034772270819553, - "heading": 0.2334618449081109, - "angularVelocity": -3.6364692054215583e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799415, - "timestamp": 1.1346481355253104 - }, - { - "x": 3.6850506748310643, - "y": 7.070437251870106, - "heading": 0.23346181911990457, - "angularVelocity": -3.6364692122502775e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799416, - "timestamp": 1.2055636439956423 - }, - { - "x": 3.950868040279354, - "y": 7.106102232920659, - "heading": 0.2334617933316983, - "angularVelocity": -3.636469204641407e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799416, - "timestamp": 1.2764791524659742 - }, - { - "x": 4.216685405727644, - "y": 7.141767213971211, - "heading": 0.23346176754349196, - "angularVelocity": -3.6364692150109834e-7, - "velocityX": 3.7483671933269203, - "velocityY": 0.5029221649799415, - "timestamp": 1.347394660936306 - }, - { - "x": 4.482502771175934, - "y": 7.1774321950217645, - "heading": 0.2334617417552857, - "angularVelocity": -3.636469207728054e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799417, - "timestamp": 1.418310169406638 - }, - { - "x": 4.7483201366242245, - "y": 7.213097176072317, - "heading": 0.2334617159670794, - "angularVelocity": -3.636469206602435e-7, - "velocityX": 3.7483671933269203, - "velocityY": 0.5029221649799417, - "timestamp": 1.48922567787697 - }, - { - "x": 5.014137502072515, - "y": 7.2487621571228695, - "heading": 0.2334616901788731, - "angularVelocity": -3.6364692079095153e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799416, - "timestamp": 1.5601411863473018 - }, - { - "x": 5.279954867520805, - "y": 7.284427138173423, - "heading": 0.23346166439066685, - "angularVelocity": -3.636469205268936e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799417, - "timestamp": 1.6310566948176337 - }, - { - "x": 5.545772232969095, - "y": 7.320092119223974, - "heading": 0.23346163860246044, - "angularVelocity": -3.6364692217914213e-7, - "velocityX": 3.748367193326924, - "velocityY": 0.5029221649799175, - "timestamp": 1.7019722032879656 - }, - { - "x": 5.81158959842306, - "y": 7.355757100232221, - "heading": 0.23346161281425412, - "angularVelocity": -3.636469211715136e-7, - "velocityX": 3.748367193406962, - "velocityY": 0.5029221643833769, - "timestamp": 1.7728877117582975 - }, - { - "x": 6.077407103529163, - "y": 7.391421040372378, - "heading": 0.23346158702575517, - "angularVelocity": -3.63651047258994e-7, - "velocityX": 3.748369162681932, - "velocityY": 0.5029074868027953, - "timestamp": 1.8438032202286294 - }, - { - "x": 6.338444265915037, - "y": 7.413466456985474, - "heading": 0.21125647663717811, - "angularVelocity": -0.3131206539662162, - "velocityX": 3.680960173825454, - "velocityY": 0.310868766065739, - "timestamp": 1.9147187286989613 - }, - { - "x": 6.578740605850335, - "y": 7.432100572083313, - "heading": 0.16288336837633732, - "angularVelocity": -0.6821231251705402, - "velocityX": 3.388487865610223, - "velocityY": 0.26276502135825913, - "timestamp": 1.9856342371692932 - }, - { - "x": 6.797330438016248, - "y": 7.447554226429222, - "heading": 0.11319688856078455, - "angularVelocity": -0.7006433555551449, - "velocityX": 3.082398150714271, - "velocityY": 0.21791642870860864, - "timestamp": 2.056549745639625 - }, - { - "x": 6.99422444679472, - "y": 7.45992830223967, - "heading": 0.06789887708325426, - "angularVelocity": -0.638760300174411, - "velocityX": 2.7764590993639087, - "velocityY": 0.1744904052351846, - "timestamp": 2.127465254109957 - }, - { - "x": 7.169459125576906, - "y": 7.4692727675317165, - "heading": 0.029649915927990267, - "angularVelocity": -0.539359612309142, - "velocityX": 2.4710346518279107, - "velocityY": 0.13176899515507345, - "timestamp": 2.198380762580289 - }, - { - "x": 7.323064804077148, - "y": 7.475617408752441, - "heading": -2.508046324740631e-30, - "angularVelocity": -0.41810200007794424, - "velocityX": 2.166037892325118, - "velocityY": 0.08946761234010188, - "timestamp": 2.269296271050621 - }, - { - "x": 7.442670757639025, - "y": 7.478903848182195, - "heading": -0.0187747254681701, - "angularVelocity": -0.297438409871576, - "velocityX": 1.8948561830587192, - "velocityY": 0.05206538544539075, - "timestamp": 2.332417659698047 - }, - { - "x": 7.5451140174844875, - "y": 7.480040188586648, - "heading": -0.030609169580020613, - "angularVelocity": -0.1874870684158837, - "velocityX": 1.6229563708994625, - "velocityY": 0.01800246206270259, - "timestamp": 2.395539048345473 - }, - { - "x": 7.630363412663352, - "y": 7.479184974890449, - "heading": -0.03600750245908947, - "angularVelocity": -0.08552303735288487, - "velocityX": 1.3505627332580479, - "velocityY": -0.013548714857401807, - "timestamp": 2.458660436992899 - }, - { - "x": 7.698396363869973, - "y": 7.476461754192338, - "heading": -0.03536054977456298, - "angularVelocity": 0.01024934175875415, - "velocityX": 1.0778113831814056, - "velocityY": -0.04314259803938343, - "timestamp": 2.521781825640325 - }, - { - "x": 7.74919584977197, - "y": 7.471969501335234, - "heading": -0.02898050014133947, - "angularVelocity": 0.10107587570456572, - "velocityX": 0.8047903728082677, - "velocityY": -0.0711684732126695, - "timestamp": 2.584903214287751 - }, - { - "x": 7.782748628307226, - "y": 7.465789287122271, - "heading": -0.017122702576199005, - "angularVelocity": 0.1878570452777808, - "velocityX": 0.5315595751917994, - "velocityY": -0.09790998495750272, - "timestamp": 2.648024602935177 - }, - { - "x": 7.799044132232666, - "y": 7.457988739013672, - "heading": 0, - "angularVelocity": 0.2712662528992439, - "velocityX": 0.25816136612733137, - "velocityY": -0.12358010930615114, - "timestamp": 2.711145991582603 - }, - { - "x": 7.789461840084544, - "y": 7.44456429977742, - "heading": 0.032498029722470304, - "angularVelocity": 0.38031209369790614, - "velocityX": -0.11213792406398307, - "velocityY": -0.15710111155221265, - "timestamp": 2.7965969416338945 - }, - { - "x": 7.748231627408751, - "y": 7.428284690558898, - "heading": 0.07405407961857692, - "angularVelocity": 0.48631466205092383, - "velocityX": -0.48250151286758974, - "velocityY": -0.1905140809873915, - "timestamp": 2.882047891685186 - }, - { - "x": 7.675346865346751, - "y": 7.40916132937864, - "heading": 0.1243473311272727, - "angularVelocity": 0.5885628126840949, - "velocityX": -0.8529426766838836, - "velocityY": -0.22379342966671825, - "timestamp": 2.967498841736478 - }, - { - "x": 7.570799431207237, - "y": 7.387208662011206, - "heading": 0.1829724416238929, - "angularVelocity": 0.6860673925971562, - "velocityX": -1.2234788972702, - "velocityY": -0.2569037249352633, - "timestamp": 3.0529497917877695 - }, - { - "x": 7.434579177146227, - "y": 7.362445545124385, - "heading": 0.24939989698187892, - "angularVelocity": 0.7773752698843693, - "velocityX": -1.5941338742188234, - "velocityY": -0.28979334778557075, - "timestamp": 3.138400741839061 - }, - { - "x": 7.266673120974145, - "y": 7.334897700728802, - "heading": 0.32290448608070227, - "angularVelocity": 0.8601962769834678, - "velocityX": -1.9649407767992484, - "velocityY": -0.3223819557190156, - "timestamp": 3.223851691890353 - }, - { - "x": 7.067064218914486, - "y": 7.304602559914413, - "heading": 0.4024227499708516, - "angularVelocity": 0.9305720280744717, - "velocityX": -2.3359471362200686, - "velocityY": -0.3545325218292511, - "timestamp": 3.3093026419416445 - }, - { - "x": 6.835729836802405, - "y": 7.27162039096164, - "heading": 0.48622750779609625, - "angularVelocity": 0.9807352378746566, - "velocityX": -2.7072183746718483, - "velocityY": -0.385977791153593, - "timestamp": 3.394753591992936 - }, - { - "x": 6.572643492795426, - "y": 7.236066659207437, - "heading": 0.571010808696293, - "angularVelocity": 0.9921867556667185, - "velocityX": -3.078799520063915, - "velocityY": -0.4160718135124634, - "timestamp": 3.480204542044228 - }, - { - "x": 6.277837622424222, - "y": 7.198257703016904, - "heading": 0.6480492113566031, - "angularVelocity": 0.901551154360156, - "velocityX": -3.450001084764899, - "velocityY": -0.4424638481823218, - "timestamp": 3.5656554920955195 - }, - { - "x": 5.957362245190181, - "y": 7.156598764006078, - "heading": 0.6480492838419923, - "angularVelocity": 8.482689682334955e-7, - "velocityX": -3.7504015700432527, - "velocityY": -0.487518734265005, - "timestamp": 3.651106442146811 - }, - { - "x": 5.635584365409571, - "y": 7.126616390006558, - "heading": 0.6480492987117265, - "angularVelocity": 1.7401484932642463e-7, - "velocityX": -3.7656442624390154, - "velocityY": -0.3508723306355692, - "timestamp": 3.736557392198103 - }, - { - "x": 5.313806456205795, - "y": 7.096634331785028, - "heading": 0.6480493135814611, - "angularVelocity": 1.7401485546331995e-7, - "velocityX": -3.7656446067671383, - "velocityY": -0.3508686352057408, - "timestamp": 3.8220083422493945 - }, - { - "x": 4.992028547001338, - "y": 7.066652273570816, - "heading": 0.6480493284511959, - "angularVelocity": 1.7401485563507226e-7, - "velocityX": -3.7656446067751177, - "velocityY": -0.3508686351200953, - "timestamp": 3.907459292300686 - }, - { - "x": 4.67025063779688, - "y": 7.036670215356605, - "heading": 0.6480493433209307, - "angularVelocity": 1.7401485639450837e-7, - "velocityX": -3.765644606775118, - "velocityY": -0.35086863512009386, - "timestamp": 3.992910242351978 - }, - { - "x": 4.348472728592423, - "y": 7.006688157142394, - "heading": 0.6480493581906654, - "angularVelocity": 1.740148559122135e-7, - "velocityX": -3.7656446067751186, - "velocityY": -0.35086863512009364, - "timestamp": 4.078361192403269 - }, - { - "x": 4.0266948193879655, - "y": 6.976706098928182, - "heading": 0.6480493730604002, - "angularVelocity": 1.740148565017462e-7, - "velocityX": -3.765644606775118, - "velocityY": -0.35086863512009375, - "timestamp": 4.163812142454561 - }, - { - "x": 3.7049169101835084, - "y": 6.946724040713971, - "heading": 0.648049387930135, - "angularVelocity": 1.7401485552645738e-7, - "velocityX": -3.765644606775118, - "velocityY": -0.3508686351200936, - "timestamp": 4.249263092505853 - }, - { - "x": 3.3831390009790514, - "y": 6.91674198249976, - "heading": 0.6480494027998698, - "angularVelocity": 1.7401485630496073e-7, - "velocityX": -3.7656446067751177, - "velocityY": -0.35086863512009364, - "timestamp": 4.334714042557144 - }, - { - "x": 3.0613610917745944, - "y": 6.886759924285549, - "heading": 0.6480494176696047, - "angularVelocity": 1.7401485676252163e-7, - "velocityX": -3.7656446067751177, - "velocityY": -0.3508686351200934, - "timestamp": 4.420164992608436 - }, - { - "x": 2.7395831825701378, - "y": 6.85677786607133, - "heading": 0.6480494325393393, - "angularVelocity": 1.740148561275008e-7, - "velocityX": -3.7656446067751106, - "velocityY": -0.35086863512017596, - "timestamp": 4.505615942659728 - }, - { - "x": 2.4178052733940274, - "y": 6.826795807553092, - "heading": 0.648049447409112, - "angularVelocity": 1.740152990535353e-7, - "velocityX": -3.765644606443389, - "velocityY": -0.3508686386780044, - "timestamp": 4.591066892711019 - }, - { - "x": 2.101806987486401, - "y": 6.79733725091672, - "heading": 0.6651218958442715, - "angularVelocity": 0.19979237708727937, - "velocityX": -3.698007871392473, - "velocityY": -0.34474229506742526, - "timestamp": 4.676517842762311 - }, - { - "x": 1.8167368698048942, - "y": 6.770773756148669, - "heading": 0.7108427694079615, - "angularVelocity": 0.5350540109419666, - "velocityX": -3.3360672702926424, - "velocityY": -0.31086248604736477, - "timestamp": 4.761968792813603 - }, - { - "x": 1.563349701636476, - "y": 6.747170828438536, - "heading": 0.7627235430971158, - "angularVelocity": 0.6071409815583658, - "velocityX": -2.9652937505825308, - "velocityY": -0.2762160946831818, - "timestamp": 4.847419742864894 - }, - { - "x": 1.3416809560568124, - "y": 6.726531033314501, - "heading": 0.8140209677583794, - "angularVelocity": 0.6003142695377961, - "velocityX": -2.5941051029464517, - "velocityY": -0.24153967991752093, - "timestamp": 4.932870692916186 - }, - { - "x": 1.1517177468979243, - "y": 6.708849739610871, - "heading": 0.8615071361475246, - "angularVelocity": 0.5557125855327181, - "velocityX": -2.2230672572379273, - "velocityY": -0.20691746192426994, - "timestamp": 5.018321642967478 - }, - { - "x": 0.993443621158485, - "y": 6.694122461254965, - "heading": 0.9032823319492421, - "angularVelocity": 0.48887924331700494, - "velocityX": -1.852221954752358, - "velocityY": -0.17234774273507145, - "timestamp": 5.103772593018769 - }, - { - "x": 0.8668441821472115, - "y": 6.682345527785774, - "heading": 0.9380916664803222, - "angularVelocity": 0.40736041565660025, - "velocityX": -1.4815451312746155, - "velocityY": -0.13782097755638892, - "timestamp": 5.189223543070061 - }, - { - "x": 0.7719075388323822, - "y": 6.673515992217611, - "heading": 0.9650448121199915, - "angularVelocity": 0.3154224221444248, - "velocityX": -1.111007464022926, - "velocityY": -0.10332869983146084, - "timestamp": 5.274674493121353 - }, - { - "x": 0.7086239541540638, - "y": 6.667631472022659, - "heading": 0.9834806533033196, - "angularVelocity": 0.21574764437724195, - "velocityX": -0.7405837458838845, - "velocityY": -0.06886430392430233, - "timestamp": 5.360125443172645 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.11016412225954963, - "velocityX": -0.37025347871120917, - "velocityY": -0.03442272228334841, - "timestamp": 5.445576393223938 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -1.2387955760475958e-29, - "velocityX": 1.44513347457817e-31, - "velocityY": -4.5041627226851734e-30, - "timestamp": 5.53102734327523 - } - ] + "samples": [ + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": -5.340812593461075e-28, + "velocityX": 3.892811349542242e-27, + "velocityY": 2.3219495328630078e-26, + "timestamp": 0 + }, + { + "x": 0.6985020844125115, + "y": 6.66756932185273, + "heading": 0.9798246445664861, + "angularVelocity": -0.1842987222375363, + "velocityX": 0.30341235873432515, + "velocityY": 0.04060189659731702, + "timestamp": 0.0709155087283535 + }, + { + "x": 0.7415443472286969, + "y": 6.673333307824001, + "heading": 0.9539749006718472, + "angularVelocity": -0.36451467892105427, + "velocityX": 0.6069513367105871, + "velocityY": 0.08127962521359514, + "timestamp": 0.141831017456707 + }, + { + "x": 0.806123192395711, + "y": 6.681988659680362, + "heading": 0.9157093267024382, + "angularVelocity": -0.5395938724205993, + "velocityX": 0.9106448832566048, + "velocityY": 0.12205160777335122, + "timestamp": 0.2127465261850605 + }, + { + "x": 0.8922518577310767, + "y": 6.693543414407282, + "heading": 0.8654790262942293, + "angularVelocity": -0.7083119237093733, + "velocityX": 1.2145251000777222, + "velocityY": 0.16293692217848874, + "timestamp": 0.283662034913414 + }, + { + "x": 0.9999464064120345, + "y": 6.708007002745886, + "heading": 0.8038456054132372, + "angularVelocity": -0.8691106076258047, + "velocityX": 1.518631828384521, + "velocityY": 0.2039552221786628, + "timestamp": 0.3545775436417675 + }, + { + "x": 1.1292266934715014, + "y": 6.725390389590618, + "heading": 0.7315277617443964, + "angularVelocity": -1.0197747286261354, + "velocityX": 1.823018538225309, + "velocityY": 0.2451281413113817, + "timestamp": 0.425493052370121 + }, + { + "x": 1.2801178802680748, + "y": 6.745706577222529, + "heading": 0.6494907896351716, + "angularVelocity": -1.1568269561947726, + "velocityX": 2.1277600556258043, + "velocityY": 0.2864844093515925, + "timestamp": 0.49640856109847453 + }, + { + "x": 1.4526523788182084, + "y": 6.768971930813486, + "heading": 0.5591180385402752, + "angularVelocity": -1.274372174936732, + "velocityX": 2.43295862419938, + "velocityY": 0.3280714473906901, + "timestamp": 0.567324069826828 + }, + { + "x": 1.646870967643948, + "y": 6.795208954217293, + "heading": 0.462559660855429, + "angularVelocity": -1.361597475873995, + "velocityX": 2.7387322224494954, + "velocityY": 0.36997581874945734, + "timestamp": 0.6382395785551815 + }, + { + "x": 1.862815108078828, + "y": 6.824451101510304, + "heading": 0.36357825171401237, + "angularVelocity": -1.3957653398577683, + "velocityX": 3.0450904788974817, + "velocityY": 0.41235193566790274, + "timestamp": 0.709155087283535 + }, + { + "x": 2.1004346793080155, + "y": 6.856747845634531, + "heading": 0.2705944336755815, + "angularVelocity": -1.3111915814439359, + "velocityX": 3.3507419673093723, + "velocityY": 0.4554256847813233, + "timestamp": 0.7800705960118886 + }, + { + "x": 2.3559638608729165, + "y": 6.892112274947116, + "heading": 0.23346195065053052, + "angularVelocity": -0.5236158308796662, + "velocityX": 3.6032905375285806, + "velocityY": 0.4986839965860088, + "timestamp": 0.850986104740242 + }, + { + "x": 2.6217812158343787, + "y": 6.927777331410734, + "heading": 0.2334619248619747, + "angularVelocity": -3.636518485299231e-7, + "velocityX": 3.748367031810964, + "velocityY": 0.5029232265714462, + "timestamp": 0.9219016134685956 + }, + { + "x": 2.8875985808561246, + "y": 6.963442312894318, + "heading": 0.23346189907376708, + "angularVelocity": -3.6364693779214443e-7, + "velocityX": 3.74836717367392, + "velocityY": 0.5029221692563763, + "timestamp": 0.9928171221969491 + }, + { + "x": 3.153415945878299, + "y": 6.999107294374709, + "heading": 0.23346187328555953, + "angularVelocity": -3.636469371719515e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113627, + "timestamp": 1.0637326309253026 + }, + { + "x": 3.419233310900473, + "y": 7.034772275855099, + "heading": 0.23346184749735194, + "angularVelocity": -3.6364693781306164e-7, + "velocityX": 3.7483671736799598, + "velocityY": 0.502922169211361, + "timestamp": 1.134648139653656 + }, + { + "x": 3.6850506759226476, + "y": 7.070437257335489, + "heading": 0.2334618217091443, + "angularVelocity": -3.6364693810863675e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113609, + "timestamp": 1.2055636483820096 + }, + { + "x": 3.950868040944822, + "y": 7.10610223881588, + "heading": 0.23346179592093674, + "angularVelocity": -3.636469373806279e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.502922169211361, + "timestamp": 1.276479157110363 + }, + { + "x": 4.216685405966996, + "y": 7.14176722029627, + "heading": 0.23346177013272917, + "angularVelocity": -3.6364693732214093e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113611, + "timestamp": 1.3473946658387161 + }, + { + "x": 4.4825027709891705, + "y": 7.17743220177666, + "heading": 0.23346174434452158, + "angularVelocity": -3.636469378203165e-7, + "velocityX": 3.7483671736799606, + "velocityY": 0.5029221692113612, + "timestamp": 1.4183101745670692 + }, + { + "x": 4.748320136011345, + "y": 7.213097183257051, + "heading": 0.233461718556314, + "angularVelocity": -3.636469375156407e-7, + "velocityX": 3.7483671736799598, + "velocityY": 0.502922169211361, + "timestamp": 1.4892256832954223 + }, + { + "x": 5.014137501033519, + "y": 7.248762164737441, + "heading": 0.23346169276810638, + "angularVelocity": -3.636469383573516e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113611, + "timestamp": 1.5601411920237753 + }, + { + "x": 5.279954866055694, + "y": 7.284427146217831, + "heading": 0.23346166697989873, + "angularVelocity": -3.6364693850408854e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113611, + "timestamp": 1.6310567007521284 + }, + { + "x": 5.545772231077868, + "y": 7.32009212769822, + "heading": 0.23346164119169116, + "angularVelocity": -3.6364693775026823e-7, + "velocityX": 3.748367173679964, + "velocityY": 0.502922169211334, + "timestamp": 1.7019722094804814 + }, + { + "x": 5.811589596106133, + "y": 7.3557571091332195, + "heading": 0.23346161540348362, + "angularVelocity": -3.63646937002983e-7, + "velocityX": 3.748367173765839, + "velocityY": 0.5029221685712877, + "timestamp": 1.7728877182088345 + }, + { + "x": 6.07740710417715, + "y": 7.39142102442851, + "heading": 0.2334615896149771, + "angularVelocity": -3.636511531820921e-7, + "velocityX": 3.748369190852852, + "velocityY": 0.5029071346283975, + "timestamp": 1.8438032269371876 + }, + { + "x": 6.33844426596311, + "y": 7.413466447299319, + "heading": 0.21125647994267174, + "angularVelocity": -0.3131206427265934, + "velocityX": 3.6809601519729838, + "velocityY": 0.3108688531764646, + "timestamp": 1.9147187356655406 + }, + { + "x": 6.578740605741143, + "y": 7.432100566048415, + "heading": 0.16288337141874695, + "angularVelocity": -0.682123126398504, + "velocityX": 3.3884878510637786, + "velocityY": 0.2627650718896376, + "timestamp": 1.9856342443938937 + }, + { + "x": 6.797330437873669, + "y": 7.447554222867888, + "heading": 0.11319689086108646, + "angularVelocity": -0.7006433634705764, + "velocityX": 3.0823981390283666, + "velocityY": 0.2179164627961563, + "timestamp": 2.0565497531222467 + }, + { + "x": 6.9942244466742505, + "y": 7.4599283003931935, + "heading": 0.06789887850949264, + "angularVelocity": -0.6387603101757464, + "velocityX": 2.7764590895737267, + "velocityY": 0.1744904287820278, + "timestamp": 2.1274652618506 + }, + { + "x": 7.169459125508247, + "y": 7.4692727668351795, + "heading": 0.029649916545462486, + "angularVelocity": -0.5393596217513631, + "velocityX": 2.4710346435678088, + "velocityY": 0.13176901089126727, + "timestamp": 2.198380770578953 + }, + { + "x": 7.323064804077148, + "y": 7.475617408752441, + "heading": 4.538527756690787e-28, + "angularVelocity": -0.41810200726386243, + "velocityX": 2.1660378854123263, + "velocityY": 0.08946762183665603, + "timestamp": 2.269296279307306 + }, + { + "x": 7.44267075746469, + "y": 7.47890384849819, + "heading": -0.01877472577087381, + "angularVelocity": -0.2974384142895345, + "velocityX": 1.8948561778925237, + "velocityY": 0.05206539038508263, + "timestamp": 2.332417668034849 + }, + { + "x": 7.545114017207691, + "y": 7.480040189019033, + "heading": -0.030609170040043498, + "angularVelocity": -0.1874870706703203, + "velocityX": 1.6229563672178904, + "velocityY": 0.018002463883490395, + "timestamp": 2.3955390567623924 + }, + { + "x": 7.630363412345014, + "y": 7.479184975308775, + "heading": -0.03600750296274941, + "angularVelocity": -0.08552303793579748, + "velocityX": 1.3505627308881507, + "velocityY": -0.013548715063116237, + "timestamp": 2.4586604454899357 + }, + { + "x": 7.698396363563398, + "y": 7.4764617545204155, + "heading": -0.035360550234153146, + "angularVelocity": 0.010249342443791389, + "velocityX": 1.0778113820030217, + "velocityY": -0.04314259941449841, + "timestamp": 2.521781834217479 + }, + { + "x": 7.749195849524956, + "y": 7.471969501540552, + "heading": -0.0289805004906965, + "angularVelocity": 0.10107587732258928, + "velocityX": 0.804790372734491, + "velocityY": -0.07116847506720048, + "timestamp": 2.584903222945022 + }, + { + "x": 7.782748628163455, + "y": 7.46578928720822, + "heading": -0.01712270276724403, + "angularVelocity": 0.1878570475474702, + "velocityX": 0.5315595761577129, + "velocityY": -0.09790998672428226, + "timestamp": 2.6480246116725654 + }, + { + "x": 7.799044132232666, + "y": 7.457988739013672, + "heading": -3.115976344858509e-29, + "angularVelocity": 0.2712662555818014, + "velocityX": 0.2581613680831416, + "velocityY": -0.12358011051084936, + "timestamp": 2.7111460004001087 + }, + { + "x": 7.789461840253682, + "y": 7.444564299731131, + "heading": 0.032498030114414544, + "angularVelocity": 0.38031209691715145, + "velocityX": -0.11213792168232267, + "velocityY": -0.15710111152907683, + "timestamp": 2.7965969507586346 + }, + { + "x": 7.7482316276690275, + "y": 7.4282846905716475, + "heading": 0.07405408050157687, + "angularVelocity": 0.48631466604883555, + "velocityX": -0.48250151006705927, + "velocityY": -0.19051407961151762, + "timestamp": 2.8820479011171605 + }, + { + "x": 7.675346865619377, + "y": 7.409161329569071, + "heading": 0.12434733262468406, + "angularVelocity": 0.5885628177579297, + "velocityX": -0.8529426734734683, + "velocityY": -0.2237934267827392, + "timestamp": 2.9674988514756864 + }, + { + "x": 7.570799431412373, + "y": 7.387208662514977, + "heading": 0.18297244388953984, + "angularVelocity": 0.6860673991205785, + "velocityX": -1.2234788936618521, + "velocityY": -0.2569037203446768, + "timestamp": 3.0529498018342123 + }, + { + "x": 7.43457917720258, + "y": 7.362445546099649, + "heading": 0.24939990020901256, + "angularVelocity": 0.7773752783411216, + "velocityX": -1.5941338702291188, + "velocityY": -0.28979334122592726, + "timestamp": 3.138400752192738 + }, + { + "x": 7.266673120798299, + "y": 7.334897702364834, + "heading": 0.32290449051554804, + "angularVelocity": 0.8601962880240946, + "velocityX": -1.9649407724524794, + "velocityY": -0.32238194682719395, + "timestamp": 3.223851702551264 + }, + { + "x": 7.067064218419677, + "y": 7.304602562446356, + "heading": 0.4024227559343364, + "angularVelocity": 0.9305720426180611, + "velocityX": -2.3359471315546996, + "velocityY": -0.3545325100700251, + "timestamp": 3.30930265290979 + }, + { + "x": 6.835729835895948, + "y": 7.271620394698958, + "heading": 0.4862275157211307, + "angularVelocity": 0.9807352573046286, + "velocityX": -2.707218369756224, + "velocityY": -0.3859777756597744, + "timestamp": 3.394753603268316 + }, + { + "x": 6.572643491372071, + "y": 7.236066664600634, + "heading": 0.5710108191953822, + "angularVelocity": 0.9921867822244987, + "velocityX": -3.0787995150439604, + "velocityY": -0.41607179263836863, + "timestamp": 3.480204553626842 + }, + { + "x": 6.277837620342988, + "y": 7.198257710886909, + "heading": 0.6480492253062652, + "angularVelocity": 0.9015511915040533, + "velocityX": -3.4500010800601615, + "velocityY": -0.44246381760636405, + "timestamp": 3.5656555039853677 + }, + { + "x": 5.957362242994923, + "y": 7.156598776275489, + "heading": 0.6480492977916429, + "angularVelocity": 8.482688295948482e-7, + "velocityX": -3.7504015578931313, + "velocityY": -0.48751868102848633, + "timestamp": 3.6511064543438936 + }, + { + "x": 5.635584364863326, + "y": 7.126616389470958, + "heading": 0.6480493126613769, + "angularVelocity": 1.740148467326507e-7, + "velocityX": -3.7656442296020605, + "velocityY": -0.3508724792262005, + "timestamp": 3.7365574047024195 + }, + { + "x": 5.3138064561089, + "y": 7.096634331319495, + "heading": 0.6480493275311116, + "angularVelocity": 1.7401485467118797e-7, + "velocityX": -3.7656445879693994, + "velocityY": -0.350868633124234, + "timestamp": 3.8220083550609454 + }, + { + "x": 4.99202854735373, + "y": 7.066652273176011, + "heading": 0.6480493424008463, + "angularVelocity": 1.740148544690323e-7, + "velocityX": -3.7656445879780995, + "velocityY": -0.35086863303086346, + "timestamp": 3.9074593054194713 + }, + { + "x": 4.67025063859856, + "y": 7.036670215032527, + "heading": 0.6480493572705811, + "angularVelocity": 1.740148551537583e-7, + "velocityX": -3.765644587978099, + "velocityY": -0.35086863303086074, + "timestamp": 3.992910255777997 + }, + { + "x": 4.34847272984339, + "y": 7.006688156889043, + "heading": 0.6480493721403158, + "angularVelocity": 1.7401485461327074e-7, + "velocityX": -3.7656445879781, + "velocityY": -0.350868633030861, + "timestamp": 4.078361206136523 + }, + { + "x": 4.0266948210882205, + "y": 6.9767060987455585, + "heading": 0.6480493870100505, + "angularVelocity": 1.7401485474732458e-7, + "velocityX": -3.7656445879781, + "velocityY": -0.3508686330308609, + "timestamp": 4.163812156495049 + }, + { + "x": 3.7049169123330508, + "y": 6.946724040602074, + "heading": 0.6480494018797853, + "angularVelocity": 1.74014855000899e-7, + "velocityX": -3.7656445879781004, + "velocityY": -0.3508686330308606, + "timestamp": 4.249263106853575 + }, + { + "x": 3.383139003577881, + "y": 6.91674198245859, + "heading": 0.64804941674952, + "angularVelocity": 1.740148546492808e-7, + "velocityX": -3.7656445879781, + "velocityY": -0.35086863303086063, + "timestamp": 4.334714057212101 + }, + { + "x": 3.0613610948227112, + "y": 6.886759924315106, + "heading": 0.6480494316192547, + "angularVelocity": 1.7401485430725364e-7, + "velocityX": -3.7656445879780995, + "velocityY": -0.3508686330308605, + "timestamp": 4.420165007570627 + }, + { + "x": 2.7395831860675424, + "y": 6.856777866171614, + "heading": 0.6480494464889893, + "angularVelocity": 1.7401485477485056e-7, + "velocityX": -3.765644587978091, + "velocityY": -0.3508686330309518, + "timestamp": 4.505615957929153 + }, + { + "x": 2.4178052773422305, + "y": 6.82679580770789, + "heading": 0.6480494613587636, + "angularVelocity": 1.7401531782389627e-7, + "velocityX": -3.7656445876286826, + "velocityY": -0.35086863677850416, + "timestamp": 4.5910669082876785 + }, + { + "x": 2.10180699075498, + "y": 6.797337251058121, + "heading": 0.6651219064311078, + "angularVelocity": 0.19979233701571972, + "velocityX": -3.6980078660496734, + "velocityY": -0.3447422939846707, + "timestamp": 4.676517858646204 + }, + { + "x": 1.8167368724291442, + "y": 6.770773756267678, + "heading": 0.7108427772926394, + "angularVelocity": 0.535053977395227, + "velocityX": -3.336067265838099, + "velocityY": -0.3108624851916846, + "timestamp": 4.76196880900473 + }, + { + "x": 1.5633497036796815, + "y": 6.747170828533709, + "heading": 0.7627235488811058, + "angularVelocity": 0.6071409547909103, + "velocityX": -2.9652937467205107, + "velocityY": -0.2762160939689793, + "timestamp": 4.847419759363256 + }, + { + "x": 1.3416809575894382, + "y": 6.72653103338719, + "heading": 0.8140209718864446, + "angularVelocity": 0.6003142479997046, + "velocityX": -2.5941050995944295, + "velocityY": -0.24153967931217257, + "timestamp": 4.932870709721782 + }, + { + "x": 1.1517177479923977, + "y": 6.708849739663476, + "heading": 0.8615071389710252, + "angularVelocity": 0.5557125682668599, + "velocityX": -2.223067254372391, + "velocityY": -0.20691746141533648, + "timestamp": 5.018321660080308 + }, + { + "x": 0.9934436218878284, + "y": 6.694122461290386, + "heading": 0.9032823337599477, + "angularVelocity": 0.4888792297060059, + "velocityX": -1.8522219523656431, + "velocityY": -0.17234774231649247, + "timestamp": 5.103772610438834 + }, + { + "x": 0.8668441825845944, + "y": 6.682345527807194, + "heading": 0.9380916675290075, + "angularVelocity": 0.4073604052735566, + "velocityX": -1.481545129364404, + "velocityY": -0.1378209772247021, + "timestamp": 5.18922356079736 + }, + { + "x": 0.7719075390509518, + "y": 6.673515992228389, + "heading": 0.9650448126274666, + "angularVelocity": 0.3154224146761601, + "velocityX": -1.1110074625889743, + "velocityY": -0.10332869958448399, + "timestamp": 5.274674511155886 + }, + { + "x": 0.708623954226878, + "y": 6.667631472026271, + "heading": 0.9834806534673777, + "angularVelocity": 0.2157476395822394, + "velocityX": -0.7405837449268271, + "velocityY": -0.06886430376057255, + "timestamp": 5.3601254615144125 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.1101641199433363, + "velocityX": -0.37025347823207466, + "velocityY": -0.03442272220184603, + "timestamp": 5.445576411872938 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 2.9535092730949793e-26, + "velocityX": 4.101709673254646e-26, + "velocityY": -1.0818589619170112e-27, + "timestamp": 5.531027362231464 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.3.traj b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.3.traj index 4311bf73..8e12b903 100644 --- a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.3.traj +++ b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.3.traj @@ -1,859 +1,860 @@ { - "samples": [ - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -1.2387955760475958e-29, - "velocityX": 1.44513347457817e-31, - "velocityY": -4.5041627226851734e-30, - "timestamp": 0 - }, - { - "x": 0.6937129747352162, - "y": 6.6645584157609425, - "heading": 0.9790660169157249, - "angularVelocity": -0.22114767057901943, - "velocityX": 0.2675140141228054, - "velocityY": -0.002104635808322496, - "timestamp": 0.06252955439239116 - }, - { - "x": 0.7271833460248389, - "y": 6.664303045137076, - "heading": 0.9517582917370493, - "angularVelocity": -0.43671709232583583, - "velocityX": 0.5352728260237692, - "velocityY": -0.004083998780231037, - "timestamp": 0.12505910878478232 - }, - { - "x": 0.7774150547311363, - "y": 6.663933728618546, - "heading": 0.9114059815034875, - "angularVelocity": -0.645331805506542, - "velocityX": 0.8033274696166545, - "velocityY": -0.005906271396278706, - "timestamp": 0.1875886631771735 - }, - { - "x": 0.8444295954022788, - "y": 6.663462332060666, - "heading": 0.8585277843526951, - "angularVelocity": -0.8456512710608117, - "velocityX": 1.0717258634310107, - "velocityY": -0.007538780061052878, - "timestamp": 0.25011821756956465 - }, - { - "x": 0.928251455530078, - "y": 6.662902493101364, - "heading": 0.793730196165171, - "angularVelocity": -1.0362713890602748, - "velocityX": 1.3405158719313994, - "velocityY": -0.008953189651546989, - "timestamp": 0.3126477719619558 - }, - { - "x": 1.0289086002870482, - "y": 6.66226903623183, - "heading": 0.7177284863656903, - "angularVelocity": -1.2154526053799588, - "velocityX": 1.6097531117096386, - "velocityY": -0.010130519490973162, - "timestamp": 0.375177326354347 - }, - { - "x": 1.1464337376402622, - "y": 6.661577427221888, - "heading": 0.6314012242837341, - "angularVelocity": -1.3805833564753438, - "velocityX": 1.8795134316120001, - "velocityY": -0.011060513970755739, - "timestamp": 0.43770688074673814 - }, - { - "x": 1.2808663505867972, - "y": 6.660844036954463, - "heading": 0.5359002148977876, - "angularVelocity": -1.5272939382655537, - "velocityX": 2.149905180883418, - "velocityY": -0.011728698126054097, - "timestamp": 0.5002364351391293 - }, - { - "x": 1.4322545638154922, - "y": 6.660088220029616, - "heading": 0.43284520187288844, - "angularVelocity": -1.6481008704811424, - "velocityX": 2.4210665612405937, - "velocityY": -0.012087355046610722, - "timestamp": 0.5627659895315205 - }, - { - "x": 1.6006535211132875, - "y": 6.65933694718367, - "heading": 0.3246624035723361, - "angularVelocity": -1.7301066567926002, - "velocityX": 2.69310982517228, - "velocityY": -0.012014684148101123, - "timestamp": 0.6252955439239116 - }, - { - "x": 1.7861082105175972, - "y": 6.658632447498398, - "heading": 0.21527496164562404, - "angularVelocity": -1.7493718448763058, - "velocityX": 2.965872557487375, - "velocityY": -0.011266667292259528, - "timestamp": 0.6878250983163028 - }, - { - "x": 1.9885470293511631, - "y": 6.658050737392514, - "heading": 0.11225233797545536, - "angularVelocity": -1.6475828857450405, - "velocityX": 3.2374901884507423, - "velocityY": -0.009302962599615121, - "timestamp": 0.750354652708694 - }, - { - "x": 2.206257706753187, - "y": 6.657808122247979, - "heading": 0.04234893056424459, - "angularVelocity": -1.1179258846552083, - "velocityX": 3.4817244344302054, - "velocityY": -0.0038800075722999405, - "timestamp": 0.8128842071010851 - }, - { - "x": 2.437370646545927, - "y": 6.657710110655747, - "heading": 0.021561241595864127, - "angularVelocity": -0.33244581974679344, - "velocityX": 3.6960592801035483, - "velocityY": -0.0015674442779168942, - "timestamp": 0.8754137614934763 - }, - { - "x": 2.6738489696177092, - "y": 6.656073614188434, - "heading": 0.021561206140011186, - "angularVelocity": -5.670255175242252e-7, - "velocityX": 3.7818648376703368, - "velocityY": -0.02617156772049459, - "timestamp": 0.9379433158858674 - }, - { - "x": 2.910327291911898, - "y": 6.654437005347333, - "heading": 0.021561170684433324, - "angularVelocity": -5.670211183604034e-7, - "velocityX": 3.7818648252347256, - "velocityY": -0.026173364851293936, - "timestamp": 1.0004728702782586 - }, - { - "x": 3.14680561420604, - "y": 6.652800396499498, - "heading": 0.021561135228855402, - "angularVelocity": -5.670211192982607e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958984518, - "timestamp": 1.0630024246706498 - }, - { - "x": 3.3832839365001823, - "y": 6.6511637876516625, - "heading": 0.02156109977327751, - "angularVelocity": -5.670211188277871e-7, - "velocityX": 3.7818648252339804, - "velocityY": -0.02617336495899097, - "timestamp": 1.125531979063041 - }, - { - "x": 3.6197622587943243, - "y": 6.649527178803828, - "heading": 0.02156106431769964, - "angularVelocity": -5.670211185570758e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958990947, - "timestamp": 1.188061533455432 - }, - { - "x": 3.8562405810884663, - "y": 6.647890569955993, - "heading": 0.02156102886212173, - "angularVelocity": -5.670211191121069e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958990954, - "timestamp": 1.2505910878478232 - }, - { - "x": 4.092718903382608, - "y": 6.646253961108158, - "heading": 0.02156099340654377, - "angularVelocity": -5.670211198937042e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.02617336495899093, - "timestamp": 1.3131206422402144 - }, - { - "x": 4.329197225676751, - "y": 6.6446173522603225, - "heading": 0.021560957950965815, - "angularVelocity": -5.67021119881002e-7, - "velocityX": 3.7818648252339804, - "velocityY": -0.026173364958990898, - "timestamp": 1.3756501966326056 - }, - { - "x": 4.565675547970892, - "y": 6.642980743412488, - "heading": 0.021560922495387855, - "angularVelocity": -5.670211198956703e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958990943, - "timestamp": 1.4381797510249967 - }, - { - "x": 4.802153870265035, - "y": 6.641344134564649, - "heading": 0.02156088703980993, - "angularVelocity": -5.670211193772164e-7, - "velocityX": 3.7818648252339795, - "velocityY": -0.026173364959042097, - "timestamp": 1.500709305417388 - }, - { - "x": 5.038632192558808, - "y": 6.639707525663405, - "heading": 0.021560851584232044, - "angularVelocity": -5.67021118724796e-7, - "velocityX": 3.781864825228069, - "velocityY": -0.02617336581314012, - "timestamp": 1.563238859809779 - }, - { - "x": 5.275110508682838, - "y": 6.638070025521481, - "heading": 0.021560816128648946, - "angularVelocity": -5.670212021371837e-7, - "velocityX": 3.781864726558842, - "velocityY": -0.02618761892412933, - "timestamp": 1.6257684142021702 - }, - { - "x": 5.511317621528859, - "y": 6.626629955813148, - "heading": 0.02156078050955198, - "angularVelocity": -5.696361874718345e-7, - "velocityX": 3.777527525044398, - "velocityY": -0.18295460153997642, - "timestamp": 1.6882979685945614 - }, - { - "x": 5.7460856437683105, - "y": 6.598193168640137, - "heading": 0.021560744143059335, - "angularVelocity": -5.815888661825855e-7, - "velocityX": 3.7545129582438856, - "velocityY": -0.45477354587499197, - "timestamp": 1.7508275229869525 - }, - { - "x": 5.947788996667301, - "y": 6.560838784941361, - "heading": 0.021560710996949464, - "angularVelocity": -6.111012841975544e-7, - "velocityX": 3.718722301384556, - "velocityY": -0.6886875092486842, - "timestamp": 1.8050674840681804 - }, - { - "x": 6.146761517861249, - "y": 6.510943609310236, - "heading": 0.021560678630883793, - "angularVelocity": -5.967199279900907e-7, - "velocityX": 3.6683750730568514, - "velocityY": -0.9198969659363234, - "timestamp": 1.8593074451494083 - }, - { - "x": 6.342740574859562, - "y": 6.4503482133201375, - "heading": 0.021560646512747805, - "angularVelocity": -5.921489497600962e-7, - "velocityX": 3.6131857968117713, - "velocityY": -1.1171725565834012, - "timestamp": 1.9135474062306361 - }, - { - "x": 6.53435736566076, - "y": 6.392349027665542, - "heading": 0.005697481882948994, - "angularVelocity": -0.2924626845886352, - "velocityX": 3.532760477358042, - "velocityY": -1.0693072874395997, - "timestamp": 1.967787367311864 - }, - { - "x": 6.715860798361231, - "y": 6.336732624061585, - "heading": -0.044321515304948156, - "angularVelocity": -0.9221798133850251, - "velocityX": 3.3463046263742644, - "velocityY": -1.0253769083769828, - "timestamp": 2.022027328393092 - }, - { - "x": 6.885664142970145, - "y": 6.284563070375092, - "heading": -0.11100118706044729, - "angularVelocity": -1.2293458628342813, - "velocityX": 3.1305948828876353, - "velocityY": -0.9618287448319874, - "timestamp": 2.0762672894743197 - }, - { - "x": 7.043230365082517, - "y": 6.236112426000239, - "heading": -0.18201634611185183, - "angularVelocity": -1.309277470628262, - "velocityX": 2.9049840555085917, - "velocityY": -0.8932647334000697, - "timestamp": 2.1305072505555476 - }, - { - "x": 7.188521106986847, - "y": 6.19141127344733, - "heading": -0.25302239943673055, - "angularVelocity": -1.3091095920689053, - "velocityX": 2.678666042675613, - "velocityY": -0.8241368847217142, - "timestamp": 2.1847472116367754 - }, - { - "x": 7.3215520348116, - "y": 6.150465382048361, - "heading": -0.3216933073087357, - "angularVelocity": -1.2660574694949878, - "velocityX": 2.4526368598519452, - "velocityY": -0.7549026692266663, - "timestamp": 2.2389871727180033 - }, - { - "x": 7.4423459735084, - "y": 6.113273952325478, - "heading": -0.38654982711023245, - "angularVelocity": -1.1957331551984454, - "velocityX": 2.2270284913351, - "velocityY": -0.685683193378148, - "timestamp": 2.293227133799231 - }, - { - "x": 7.550924553999962, - "y": 6.079834322504852, - "heading": -0.4465624539391904, - "angularVelocity": -1.1064282796789968, - "velocityX": 2.001818923301949, - "velocityY": -0.6165127915661526, - "timestamp": 2.347467094880459 - }, - { - "x": 7.647306628463083, - "y": 6.050143536065283, - "heading": -0.5009752926426013, - "angularVelocity": -1.0031872740823848, - "velocityX": 1.7769569251493995, - "velocityY": -0.5473968979274295, - "timestamp": 2.401707055961687 - }, - { - "x": 7.7315082292215935, - "y": 6.024198841891014, - "heading": -0.5492132046279071, - "angularVelocity": -0.8893426732564748, - "velocityX": 1.552390508400531, - "velocityY": -0.47833172548586056, - "timestamp": 2.4559470170429147 - }, - { - "x": 7.803542912083633, - "y": 6.001997808520636, - "heading": -0.5908266102991006, - "angularVelocity": -0.7672093571172569, - "velocityX": 1.3280740145473924, - "velocityY": -0.4093113809047783, - "timestamp": 2.5101869781241426 - }, - { - "x": 7.863422173267542, - "y": 5.983538316289689, - "heading": -0.6254559233807054, - "angularVelocity": -0.6384464957440807, - "velocityX": 1.103969471774435, - "velocityY": -0.34033011571123517, - "timestamp": 2.5644269392053705 - }, - { - "x": 7.9111558450934325, - "y": 5.968818528744131, - "heading": -0.6528074420175133, - "angularVelocity": -0.5042687732730391, - "velocityX": 0.8800462034699242, - "velocityY": -0.2713827084704847, - "timestamp": 2.6186669002865983 - }, - { - "x": 7.946752441580289, - "y": 5.95783687514921, - "heading": -0.6726364642205706, - "angularVelocity": -0.36557958021692255, - "velocityX": 0.6562799046545997, - "velocityY": -0.2024642602245795, - "timestamp": 2.672906861367826 - }, - { - "x": 7.970219449253945, - "y": 5.950592046499088, - "heading": -0.6847351436740425, - "angularVelocity": -0.22305840956179235, - "velocityX": 0.43265163185705313, - "velocityY": -0.1335699455844713, - "timestamp": 2.727146822449054 - }, - { - "x": 7.981563568115234, - "y": 5.947082996368408, - "heading": -0.6889234822214203, - "angularVelocity": -0.07721868644237204, - "velocityX": 0.2091468842372586, - "velocityY": -0.06469492346102504, - "timestamp": 2.781386783530282 - }, - { - "x": 7.980191051552582, - "y": 5.9474933088994915, - "heading": -0.684492260818683, - "angularVelocity": 0.07820368392725321, - "velocityX": -0.024222633376945003, - "velocityY": 0.007241333387762343, - "timestamp": 2.8380493495400803 - }, - { - "x": 7.965589050123253, - "y": 5.951981018210289, - "heading": -0.6714197034271625, - "angularVelocity": 0.23070888440280377, - "velocityX": -0.257701026579075, - "velocityY": 0.07920060150509887, - "timestamp": 2.8947119155498786 - }, - { - "x": 7.9377506970473615, - "y": 5.960547682425779, - "heading": -0.6498948487141165, - "angularVelocity": 0.37987786697348064, - "velocityX": -0.4913006070194107, - "velocityY": 0.15118736793544205, - "timestamp": 2.951374481559677 - }, - { - "x": 7.896668195667252, - "y": 5.973195099193875, - "heading": -0.6201381150730912, - "angularVelocity": 0.5251568316881386, - "velocityX": -0.7250377854932653, - "velocityY": 0.22320585986009217, - "timestamp": 3.0080370475694753 - }, - { - "x": 7.842332542279827, - "y": 5.989925310225715, - "heading": -0.5824114694602323, - "angularVelocity": 0.6658125155563156, - "velocityX": -0.9589338643440537, - "velocityY": 0.2952603845887764, - "timestamp": 3.0646996135792737 - }, - { - "x": 7.774733202893158, - "y": 6.010740641182104, - "heading": -0.5370321516926094, - "angularVelocity": 0.8008694445602113, - "velocityX": -1.1930158506231556, - "velocityY": 0.367355953360645, - "timestamp": 3.121362179589072 - }, - { - "x": 7.693857750948662, - "y": 6.035643782735694, - "heading": -0.48439126574422153, - "angularVelocity": 0.9290240392446375, - "velocityX": -1.4273171449826625, - "velocityY": 0.43949900802735137, - "timestamp": 3.1780247455988704 - }, - { - "x": 7.5996914832698845, - "y": 6.064637893376439, - "heading": -0.4249792327859352, - "angularVelocity": 1.0485235163549331, - "velocityX": -1.6618779259395846, - "velocityY": 0.5116978047858136, - "timestamp": 3.2346873116086687 - }, - { - "x": 7.492217045643208, - "y": 6.097726656744903, - "heading": -0.35942190954654185, - "angularVelocity": 1.156977663667012, - "velocityX": -1.8967449798883769, - "velocityY": 0.583961611670435, - "timestamp": 3.291349877618467 - }, - { - "x": 7.371414140499637, - "y": 6.1349141479804095, - "heading": -0.28853586070822274, - "angularVelocity": 1.251020803153572, - "velocityX": -2.131970252153457, - "velocityY": 0.6562973379828368, - "timestamp": 3.3480124436282654 - }, - { - "x": 7.237259574639344, - "y": 6.176204237589326, - "heading": -0.21342323245219458, - "angularVelocity": 1.3256128965822067, - "velocityX": -2.367604845800615, - "velocityY": 0.7287013722918435, - "timestamp": 3.404675009638064 - }, - { - "x": 7.0897287772168305, - "y": 6.221599002117526, - "heading": -0.13565860715800612, - "angularVelocity": 1.372416231215901, - "velocityX": -2.6036730739833454, - "velocityY": 0.8011420541800114, - "timestamp": 3.461337575647862 - }, - { - "x": 6.928804270313556, - "y": 6.271094657477637, - "heading": -0.05771767479301098, - "angularVelocity": 1.3755277576295868, - "velocityX": -2.8400497583439677, - "velocityY": 0.8735159532230397, - "timestamp": 3.5180001416576605 - }, - { - "x": 6.754523159333334, - "y": 6.324666148768191, - "heading": 0.015819291394400217, - "angularVelocity": 1.297805083071882, - "velocityX": -3.0757715940730104, - "velocityY": 0.9454476749480591, - "timestamp": 3.574662707667459 - }, - { - "x": 6.56735113931042, - "y": 6.382062750762148, - "heading": 0.07327050352161106, - "angularVelocity": 1.0139182916156146, - "velocityX": -3.3032746873932677, - "velocityY": 1.012954513638379, - "timestamp": 3.631325273677257 - }, - { - "x": 6.368937324018353, - "y": 6.441765360809047, - "heading": 0.09523150855737006, - "angularVelocity": 0.38757519438780325, - "velocityX": -3.5016736668395723, - "velocityY": 1.053651718430401, - "timestamp": 3.6879878396870556 - }, - { - "x": 6.164194285631387, - "y": 6.50503273811371, - "heading": 0.09523154205905172, - "angularVelocity": 5.912489325251288e-7, - "velocityX": -3.6133739222393086, - "velocityY": 1.1165639285330458, - "timestamp": 3.744650405696854 - }, - { - "x": 5.956663061240617, - "y": 6.558448687512427, - "heading": 0.09523157576982937, - "angularVelocity": 5.949391283503271e-7, - "velocityX": -3.6625807654896136, - "velocityY": 0.9427026193886323, - "timestamp": 3.8013129717066523 - }, - { - "x": 5.7460856437683105, - "y": 6.598193168640137, - "heading": 0.09523161032614703, - "angularVelocity": 6.098615032122225e-7, - "velocityX": -3.716341004321884, - "velocityY": 0.7014239545882616, - "timestamp": 3.8579755377164506 - }, - { - "x": 5.5027055023001905, - "y": 6.62552340619542, - "heading": 0.09523164151085345, - "angularVelocity": 4.815615607402516e-7, - "velocityX": -3.7583333072466103, - "velocityY": 0.4220399473818025, - "timestamp": 3.9227330037450354 - }, - { - "x": 5.257964269234122, - "y": 6.634609890080181, - "heading": 0.09523167208339899, - "angularVelocity": 4.721084288711453e-7, - "velocityX": -3.7793516033817274, - "velocityY": 0.140315618291052, - "timestamp": 3.98749046977362 - }, - { - "x": 5.013061040594942, - "y": 6.636410996869712, - "heading": 0.09523170256730593, - "angularVelocity": 4.70739650837411e-7, - "velocityX": -3.781853177069622, - "velocityY": 0.02781311406988273, - "timestamp": 4.052247935802205 - }, - { - "x": 4.7681578086493825, - "y": 6.6382116540225224, - "heading": 0.09523173305121174, - "angularVelocity": 4.7073963340734453e-7, - "velocityX": -3.781853228127505, - "velocityY": 0.027806170674068813, - "timestamp": 4.11700540183079 - }, - { - "x": 4.5232545767036525, - "y": 6.640012311152163, - "heading": 0.09523176353511749, - "angularVelocity": 4.707396321471799e-7, - "velocityX": -3.781853228130136, - "velocityY": 0.02780617031625758, - "timestamp": 4.181762867859375 - }, - { - "x": 4.2783513447579224, - "y": 6.641812968281802, - "heading": 0.09523179401902328, - "angularVelocity": 4.707396330675372e-7, - "velocityX": -3.781853228130136, - "velocityY": 0.027806170316239148, - "timestamp": 4.246520333887959 - }, - { - "x": 4.033448112812192, - "y": 6.64361362541144, - "heading": 0.09523182450292901, - "angularVelocity": 4.707396322379774e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.027806170316239207, - "timestamp": 4.311277799916544 - }, - { - "x": 3.788544880866463, - "y": 6.645414282541079, - "heading": 0.09523185498683476, - "angularVelocity": 4.7073963225441474e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.02780617031623923, - "timestamp": 4.376035265945129 - }, - { - "x": 3.5436416489207327, - "y": 6.647214939670717, - "heading": 0.09523188547074045, - "angularVelocity": 4.707396314544476e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.02780617031623925, - "timestamp": 4.440792731973714 - }, - { - "x": 3.2987384169750027, - "y": 6.649015596800356, - "heading": 0.09523191595464614, - "angularVelocity": 4.7073963141347726e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.027806170316239207, - "timestamp": 4.5055501980022985 - }, - { - "x": 3.0538351850292726, - "y": 6.650816253929995, - "heading": 0.09523194643855185, - "angularVelocity": 4.7073963175583744e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.027806170316239227, - "timestamp": 4.570307664030883 - }, - { - "x": 2.8089319530835426, - "y": 6.652616911059632, - "heading": 0.09523197692245748, - "angularVelocity": 4.7073963058043316e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.02780617031621524, - "timestamp": 4.635065130059468 - }, - { - "x": 2.5640287211375905, - "y": 6.654417568159032, - "heading": 0.09523200740636317, - "angularVelocity": 4.707396315238404e-7, - "velocityX": -3.78185322813357, - "velocityY": 0.027806169849281307, - "timestamp": 4.699822596088053 - }, - { - "x": 2.319125484878767, - "y": 6.656217638494738, - "heading": 0.09523203789215427, - "angularVelocity": 4.707687462497284e-7, - "velocityX": -3.7818532947339327, - "velocityY": 0.02779710890650077, - "timestamp": 4.764580062116638 - }, - { - "x": 2.086765993499063, - "y": 6.65613072972364, - "heading": 0.1392662443558495, - "angularVelocity": 0.6799865585268291, - "velocityX": -3.588149840161109, - "velocityY": -0.0013420656555584189, - "timestamp": 4.829337528145222 - }, - { - "x": 1.8694746583337432, - "y": 6.65699037980207, - "heading": 0.2298866992043259, - "angularVelocity": 1.399382347797175, - "velocityX": -3.3554638328406146, - "velocityY": 0.013274918417153406, - "timestamp": 4.894094994173807 - }, - { - "x": 1.6701637818051993, - "y": 6.657989601285878, - "heading": 0.3342097566920158, - "angularVelocity": 1.6109811560823084, - "velocityX": -3.077805367501045, - "velocityY": 0.015430212840099346, - "timestamp": 4.958852460202392 - }, - { - "x": 1.4891465820964496, - "y": 6.659013009703433, - "heading": 0.4396201310941332, - "angularVelocity": 1.6277717592530314, - "velocityX": -2.7953101134137524, - "velocityY": 0.01580371315183694, - "timestamp": 5.023609926230977 - }, - { - "x": 1.326396709734208, - "y": 6.660011981353038, - "heading": 0.5409065147827995, - "angularVelocity": 1.5640881260541826, - "velocityX": -2.5132217540816426, - "velocityY": 0.015426354841678473, - "timestamp": 5.088367392259562 - }, - { - "x": 1.1818610470561817, - "y": 6.660956545539143, - "heading": 0.6351810352227076, - "angularVelocity": 1.4558092868904808, - "velocityX": -2.231953650166405, - "velocityY": 0.014586182011631477, - "timestamp": 5.153124858288146 - }, - { - "x": 1.0554921038308631, - "y": 6.661825694692396, - "heading": 0.7206099929025545, - "angularVelocity": 1.3192140291922123, - "velocityX": -1.9514189015601402, - "velocityY": 0.013421605361615121, - "timestamp": 5.217882324316731 - }, - { - "x": 0.9472506642705061, - "y": 6.66260279420219, - "heading": 0.7959103128896443, - "angularVelocity": 1.1628052270274385, - "velocityX": -1.6714897323588642, - "velocityY": 0.012000153147596032, - "timestamp": 5.282639790345316 - }, - { - "x": 0.8571042421570496, - "y": 6.663273874000479, - "heading": 0.8601164354198797, - "angularVelocity": 0.9914860242044418, - "velocityX": -1.3920622229669246, - "velocityY": 0.010362971861702049, - "timestamp": 5.347397256373901 - }, - { - "x": 0.7850255723133412, - "y": 6.6638271232273425, - "heading": 0.912467741181329, - "angularVelocity": 0.8084211593199297, - "velocityX": -1.1130557488443436, - "velocityY": 0.00854340450286573, - "timestamp": 5.4121547224024855 - }, - { - "x": 0.7309916972965728, - "y": 6.664252697034555, - "heading": 0.952354006476902, - "angularVelocity": 0.6159330767817102, - "velocityX": -0.8344037889456237, - "velocityY": 0.0065718106855080505, - "timestamp": 5.47691218843107 - }, - { - "x": 0.6949834963208631, - "y": 6.664542513080523, - "heading": 0.9792885970471746, - "angularVelocity": 0.41593027371366526, - "velocityX": -0.556047096713378, - "velocityY": 0.0044754074509330615, - "timestamp": 5.541669654459655 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.21010218588179536, - "velocityX": -0.2779301721676604, - "velocityY": 0.0022778009813890303, - "timestamp": 5.60642712048824 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -1.2401951851203077e-33, - "velocityX": -4.2513783914380976e-39, - "velocityY": -1.4668246371137104e-32, - "timestamp": 5.671184586516825 - } - ] + "samples": [ + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 2.9535092730949793e-26, + "velocityX": 4.101709673254646e-26, + "velocityY": -1.0818589619170112e-27, + "timestamp": 0 + }, + { + "x": 0.6937129747745413, + "y": 6.664558415884879, + "heading": 0.9790660166396069, + "angularVelocity": -0.2211476741106938, + "velocityX": 0.2675140136822165, + "velocityY": -0.0021046338178637887, + "timestamp": 0.06252955464237786 + }, + { + "x": 0.7271833461439815, + "y": 6.664303045508021, + "heading": 0.951758290929694, + "angularVelocity": -0.4367170990756675, + "velocityX": 0.5352728251602833, + "velocityY": -0.004083994813630824, + "timestamp": 0.12505910928475572 + }, + { + "x": 0.7774150549719171, + "y": 6.663933729358567, + "heading": 0.9114059799343049, + "angularVelocity": -0.6453318151100473, + "velocityX": 0.80332746835033, + "velocityY": -0.0059062654702356065, + "timestamp": 0.18758866392713358 + }, + { + "x": 0.8444295958079634, + "y": 6.663462333290518, + "heading": 0.8585277818184469, + "angularVelocity": -0.8456512831137402, + "velocityX": 1.0717258617835783, + "velocityY": -0.007538772197327858, + "timestamp": 0.25011821856951144 + }, + { + "x": 0.9282514561454864, + "y": 6.662902494939932, + "heading": 0.7937301924930702, + "angularVelocity": -1.0362714031144127, + "velocityX": 1.3405158699261557, + "velocityY": -0.00895317988091305, + "timestamp": 0.3126477732118893 + }, + { + "x": 1.0289086011587232, + "y": 6.662269038795252, + "heading": 0.717728481418855, + "angularVelocity": -1.2154526209068217, + "velocityX": 1.6097531093723465, + "velocityY": -0.010130507858288727, + "timestamp": 0.37517732785426716 + }, + { + "x": 1.1464337388167514, + "y": 6.661577430622322, + "heading": 0.6314012179720037, + "angularVelocity": -1.3805833727839212, + "velocityX": 1.879513428972632, + "velocityY": -0.011060500540669212, + "timestamp": 0.437706882496645 + }, + { + "x": 1.2808663521190058, + "y": 6.66084404129845, + "heading": 0.5359002071968971, + "angularVelocity": -1.5272939543756525, + "velocityX": 2.149905177977148, + "velocityY": -0.011728682989453756, + "timestamp": 0.5002364371390229 + }, + { + "x": 1.4322545657568082, + "y": 6.6600882254157945, + "heading": 0.43284519285348916, + "angularVelocity": -1.6481008849783823, + "velocityX": 2.4210665581040525, + "velocityY": -0.012087338331098648, + "timestamp": 0.5627659917814007 + }, + { + "x": 1.6006535235186912, + "y": 6.659336953698895, + "heading": 0.32466239343874187, + "angularVelocity": -1.730106667694512, + "velocityX": 2.693109821827408, + "velocityY": -0.012014666043865637, + "timestamp": 0.6252955464237786 + }, + { + "x": 1.7861082134397477, + "y": 6.658632455209532, + "heading": 0.2152749507913156, + "angularVelocity": -1.749371849408482, + "velocityX": 2.9658725538941773, + "velocityY": -0.011266648121698327, + "timestamp": 0.6878251010661565 + }, + { + "x": 1.988547032824456, + "y": 6.658050746326577, + "heading": 0.1122523271254691, + "angularVelocity": -1.6475828790890588, + "velocityX": 3.237490184321712, + "velocityY": -0.009302943004826474, + "timestamp": 0.7503546557085343 + }, + { + "x": 2.206257710668096, + "y": 6.657808132322276, + "heading": 0.04234892140521699, + "angularVelocity": -1.1179258531433098, + "velocityX": 3.481724427573166, + "velocityY": -0.003879989321667614, + "timestamp": 0.8128842103509122 + }, + { + "x": 2.4373706508795343, + "y": 6.657710121609846, + "heading": 0.021561234757217183, + "angularVelocity": -0.3324457813091681, + "velocityX": 3.6960592720231067, + "velocityY": -0.0015674302014480283, + "timestamp": 0.87541376499329 + }, + { + "x": 2.6738489737582136, + "y": 6.656073631484609, + "heading": 0.02156119930135256, + "angularVelocity": -5.670257020753551e-7, + "velocityX": 3.7818648194626623, + "velocityY": -0.026171466190618775, + "timestamp": 0.9379433196356679 + }, + { + "x": 2.910327295829668, + "y": 6.654437024703101, + "heading": 0.021561163845773425, + "angularVelocity": -5.670211364811453e-7, + "velocityX": 3.781864806553173, + "velocityY": -0.026173331808750182, + "timestamp": 1.0004728742780458 + }, + { + "x": 3.146805617901072, + "y": 6.6528004179142695, + "heading": 0.021561128390194195, + "angularVelocity": -5.670211379082889e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.0261733319258545, + "timestamp": 1.0630024289204236 + }, + { + "x": 3.3832839399724763, + "y": 6.651163811125438, + "heading": 0.02156109293461502, + "angularVelocity": -5.670211370809572e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861857, + "timestamp": 1.1255319835628015 + }, + { + "x": 3.6197622620438805, + "y": 6.649527204336607, + "heading": 0.02156105747903583, + "angularVelocity": -5.670211373321077e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861847, + "timestamp": 1.1880615382051793 + }, + { + "x": 3.8562405841152843, + "y": 6.647890597547776, + "heading": 0.021561022023456698, + "angularVelocity": -5.67021136433779e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861833, + "timestamp": 1.2505910928475572 + }, + { + "x": 4.092718906186688, + "y": 6.646253990758946, + "heading": 0.021560986567877485, + "angularVelocity": -5.670211376937565e-7, + "velocityX": 3.781864806552362, + "velocityY": -0.026173331925861826, + "timestamp": 1.313120647489935 + }, + { + "x": 4.329197228258092, + "y": 6.644617383970115, + "heading": 0.02156095111229825, + "angularVelocity": -5.670211380713392e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.02617333192586182, + "timestamp": 1.375650202132313 + }, + { + "x": 4.565675550329496, + "y": 6.642980777181283, + "heading": 0.02156091565671906, + "angularVelocity": -5.670211373267648e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861805, + "timestamp": 1.4381797567746908 + }, + { + "x": 4.8021538724009005, + "y": 6.641344170392449, + "heading": 0.02156088020113987, + "angularVelocity": -5.670211373593148e-7, + "velocityX": 3.781864806552362, + "velocityY": -0.026173331925919696, + "timestamp": 1.5007093114170686 + }, + { + "x": 5.038632194471905, + "y": 6.63970756354592, + "heading": 0.021560844745560682, + "angularVelocity": -5.670211372802694e-7, + "velocityX": 3.7818648065459763, + "velocityY": -0.026173332848588252, + "timestamp": 1.5632388660594465 + }, + { + "x": 5.275110510179994, + "y": 6.6380700375610795, + "heading": 0.021560809289976154, + "angularVelocity": -5.670212226566905e-7, + "velocityX": 3.7818647047874645, + "velocityY": -0.026188032110664707, + "timestamp": 1.6257684207018244 + }, + { + "x": 5.511317622497512, + "y": 6.6266299618393605, + "heading": 0.021560773670877074, + "angularVelocity": -5.696362189823434e-7, + "velocityX": 3.7775275014901912, + "velocityY": -0.18295469697725122, + "timestamp": 1.6882979753442022 + }, + { + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0.02156073730438198, + "angularVelocity": -5.815889030387333e-7, + "velocityX": 3.7545129277426152, + "velocityY": -0.454773640430677, + "timestamp": 1.75082752998658 + }, + { + "x": 5.947788995289173, + "y": 6.560838779777381, + "heading": 0.021560704158269762, + "angularVelocity": -6.111013256538172e-7, + "velocityX": 3.7187222649013885, + "velocityY": -0.6886876024038036, + "timestamp": 1.8050674912293463 + }, + { + "x": 6.146761514775588, + "y": 6.510943599042024, + "heading": 0.02156067179220178, + "angularVelocity": -5.967199688048595e-7, + "velocityX": 3.668375030650514, + "velocityY": -0.9198970573012855, + "timestamp": 1.8593074524721125 + }, + { + "x": 6.342740585471881, + "y": 6.450348248758284, + "heading": 0.02156063967405059, + "angularVelocity": -5.921492283023906e-7, + "velocityX": 3.6131860385949297, + "velocityY": -1.1171717105867658, + "timestamp": 1.9135474137148787 + }, + { + "x": 6.534357375004527, + "y": 6.392349058669825, + "heading": 0.005697476736664897, + "angularVelocity": -0.29246265251528625, + "velocityX": 3.532760443448913, + "velocityY": -1.0693073660002697, + "timestamp": 1.9677873749576449 + }, + { + "x": 6.715860806517651, + "y": 6.336732651063718, + "heading": -0.04432151851696129, + "angularVelocity": -0.9221797749772022, + "velocityX": 3.346304594517604, + "velocityY": -1.025376979109191, + "timestamp": 2.022027336200411 + }, + { + "x": 6.885664150088088, + "y": 6.2845630937364065, + "heading": -0.11100118904659197, + "angularVelocity": -1.2293458365721763, + "velocityX": 3.1305948544180144, + "velocityY": -0.9618288090917363, + "timestamp": 2.0762672974431773 + }, + { + "x": 7.043230371259609, + "y": 6.2361124460413855, + "heading": -0.1820163474700135, + "angularVelocity": -1.3092774551510717, + "velocityX": 2.9049840295108242, + "velocityY": -0.8932647919523048, + "timestamp": 2.1305072586859435 + }, + { + "x": 7.188521112298814, + "y": 6.191411290458308, + "heading": -0.25302240055528713, + "angularVelocity": -1.3091095837525815, + "velocityX": 2.678666018747991, + "velocityY": -0.8241369381332195, + "timestamp": 2.1847472199287097 + }, + { + "x": 7.321552039325899, + "y": 6.150465396300966, + "heading": -0.32169330843547617, + "angularVelocity": -1.2660574658752652, + "velocityX": 2.4526368378411387, + "velocityY": -0.7549027178334027, + "timestamp": 2.238987181171476 + }, + { + "x": 7.442345977289038, + "y": 6.1132739640793945, + "heading": -0.38654982839023333, + "angularVelocity": -1.195733154462882, + "velocityX": 2.2270284711762884, + "velocityY": -0.685683237403341, + "timestamp": 2.293227142414242 + }, + { + "x": 7.550924557109546, + "y": 6.079834332011057, + "heading": -0.44656245543798795, + "angularVelocity": -1.106428280417667, + "velocityX": 2.0018189049681507, + "velocityY": -0.6165128311701689, + "timestamp": 2.3474671036570083 + }, + { + "x": 7.647306630963791, + "y": 6.050143543568203, + "heading": -0.5009752943623942, + "angularVelocity": -1.003187275169067, + "velocityX": 1.776956908631632, + "velocityY": -0.5473969332309115, + "timestamp": 2.4017070648997745 + }, + { + "x": 7.7315082311756465, + "y": 6.024198847629992, + "heading": -0.5492132065198027, + "angularVelocity": -0.8893426737807988, + "velocityX": 1.552390493698683, + "velocityY": -0.478331756582337, + "timestamp": 2.4559470261425407 + }, + { + "x": 7.803542913553516, + "y": 6.001997812730986, + "heading": -0.5908266122726411, + "angularVelocity": -0.7672093563375907, + "velocityX": 1.3280740016656365, + "velocityY": -0.4093114078684577, + "timestamp": 2.510186987385307 + }, + { + "x": 7.863422174316105, + "y": 5.983538319203457, + "heading": -0.6254559253112775, + "angularVelocity": -0.6384464930504523, + "velocityX": 1.103969460718877, + "velocityY": -0.34033013860220046, + "timestamp": 2.564426948628073 + }, + { + "x": 7.911155845783931, + "y": 5.9688185305906725, + "heading": -0.6528074437520258, + "angularVelocity": -0.5042687681565456, + "velocityX": 0.8800461942474525, + "velocityY": -0.2713827273382881, + "timestamp": 2.6186669098708393 + }, + { + "x": 7.9467524419763915, + "y": 5.957836876155619, + "heading": -0.6726364655817074, + "angularVelocity": -0.3655795722443686, + "velocityX": 0.6562798972723887, + "velocityY": -0.2024642751107768, + "timestamp": 2.6729068711136055 + }, + { + "x": 7.970219449419739, + "y": 5.950592046890535, + "heading": -0.6847351444633458, + "angularVelocity": -0.22305839835481103, + "velocityX": 0.4326516263224211, + "velocityY": -0.13356995652444023, + "timestamp": 2.7271468323563717 + }, + { + "x": 7.981563568115234, + "y": 5.947082996368408, + "heading": -0.6889234822214203, + "angularVelocity": -0.07721867166033586, + "velocityX": 0.2091468805577016, + "velocityY": -0.06469493048531134, + "timestamp": 2.781386793599138 + }, + { + "x": 7.980191051455629, + "y": 5.94749330873125, + "heading": -0.6844922597423083, + "angularVelocity": 0.07820370271926669, + "velocityX": -0.024222635024756137, + "velocityY": 0.007241330399680058, + "timestamp": 2.838049359756896 + }, + { + "x": 7.965589050009383, + "y": 5.951981018115051, + "heading": -0.6714197010480987, + "angularVelocity": 0.23070890679065725, + "velocityX": -0.25770102620470225, + "velocityY": 0.0792006025866641, + "timestamp": 2.8947119259146543 + }, + { + "x": 7.937750696996029, + "y": 5.960547682646947, + "heading": -0.6498948448329763, + "angularVelocity": 0.37987789249067105, + "velocityX": -0.4913006046328201, + "velocityY": 0.1511873731247131, + "timestamp": 2.9513744920724125 + }, + { + "x": 7.896668195757291, + "y": 5.973195099977348, + "heading": -0.6201381095216878, + "angularVelocity": 0.525156859794188, + "velocityX": -0.725037781105049, + "velocityY": 0.22320586920098048, + "timestamp": 3.0080370582301708 + }, + { + "x": 7.842332542589409, + "y": 5.989925311820313, + "heading": -0.58241146210736, + "angularVelocity": 0.6658125456106359, + "velocityX": -0.9589338579654723, + "velocityY": 0.2952603981327993, + "timestamp": 3.064699624387929 + }, + { + "x": 7.774733203499769, + "y": 6.010740643840145, + "heading": -0.5370321424515595, + "angularVelocity": 0.8008694757921291, + "velocityX": -1.1930158422658073, + "velocityY": 0.3673559711693639, + "timestamp": 3.121362190545687 + }, + { + "x": 7.693857751929137, + "y": 6.035643786713744, + "heading": -0.4843912545821768, + "angularVelocity": 0.9290240707210785, + "velocityX": -1.4273171346574955, + "velocityY": 0.4394990301756616, + "timestamp": 3.1780247567034454 + }, + { + "x": 7.599691484700552, + "y": 6.064637898936317, + "heading": -0.42497921973533753, + "angularVelocity": 1.048523546946787, + "velocityX": -1.6618779136548574, + "velocityY": 0.511697831366272, + "timestamp": 3.2346873228612036 + }, + { + "x": 7.492217047600242, + "y": 6.097726664155008, + "heading": -0.3594218947187098, + "angularVelocity": 1.1569776920110768, + "velocityX": -1.8967449656459858, + "velocityY": 0.5839616427989975, + "timestamp": 3.291349889018962 + }, + { + "x": 7.371414143059779, + "y": 6.134914157517514, + "heading": -0.2885358443103568, + "angularVelocity": 1.2510208275953218, + "velocityX": -2.1319702359425117, + "velocityY": 0.6562973738070701, + "timestamp": 3.34801245517672 + }, + { + "x": 7.237259577881299, + "y": 6.176204249541141, + "heading": -0.21342321481120613, + "angularVelocity": 1.3256129150597364, + "velocityX": -2.367604827585309, + "velocityY": 0.7287014130046445, + "timestamp": 3.4046750213344783 + }, + { + "x": 7.089728781224228, + "y": 6.221599016786816, + "heading": -0.13565858875957554, + "angularVelocity": 1.3724162409997644, + "velocityX": -2.6036730536757164, + "velocityY": 0.8011421000469415, + "timestamp": 3.4613375874922365 + }, + { + "x": 6.928804275182176, + "y": 6.271094675189793, + "heading": -0.05771765635307292, + "angularVelocity": 1.3755277547702696, + "velocityX": -2.8400497357287153, + "velocityY": 0.8735160046435817, + "timestamp": 3.5180001536499947 + }, + { + "x": 6.754523165192255, + "y": 6.324666169886209, + "heading": 0.015819308808898584, + "angularVelocity": 1.2978050615856767, + "velocityX": -3.07577156856422, + "velocityY": 0.9454477325870393, + "timestamp": 3.574662719807753 + }, + { + "x": 6.567351146368393, + "y": 6.382062775695167, + "heading": 0.07327051868988925, + "angularVelocity": 1.0139182493259664, + "velocityX": -3.303274657606285, + "velocityY": 1.012954578321738, + "timestamp": 3.631325285965511 + }, + { + "x": 6.3689373323756, + "y": 6.441765390033087, + "heading": 0.0952315218805866, + "angularVelocity": 0.38757516081347765, + "velocityX": -3.501673634765793, + "velocityY": 1.0536517914084174, + "timestamp": 3.6879878521232694 + }, + { + "x": 6.164194282224136, + "y": 6.505032727532106, + "heading": 0.09523155538228788, + "angularVelocity": 5.912492769442424e-7, + "velocityX": -3.6133741204276855, + "velocityY": 1.116563223114054, + "timestamp": 3.7446504182810276 + }, + { + "x": 5.956663059713831, + "y": 6.558448682183149, + "heading": 0.09523158909306802, + "angularVelocity": 5.949391710813205e-7, + "velocityX": -3.662580722738597, + "velocityY": 0.9427027096218124, + "timestamp": 3.801312984438786 + }, + { + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0.09523162364938853, + "angularVelocity": 6.098615517108678e-7, + "velocityX": -3.7163409676723376, + "velocityY": 0.701424046809534, + "timestamp": 3.857975550596544 + }, + { + "x": 5.502705503273195, + "y": 6.625523412363324, + "heading": 0.09523165483409754, + "angularVelocity": 4.815615988787072e-7, + "velocityX": -3.758333277705387, + "velocityY": 0.4220400409979927, + "timestamp": 3.9227330168752417 + }, + { + "x": 5.257964270714501, + "y": 6.634609902404007, + "heading": 0.09523168540664503, + "angularVelocity": 4.72108457235069e-7, + "velocityX": -3.779351580949706, + "velocityY": 0.1403157128102671, + "timestamp": 3.9874904831539393 + }, + { + "x": 5.013061042473027, + "y": 6.636411025383571, + "heading": 0.09523171589055375, + "angularVelocity": 4.7073967636728834e-7, + "velocityX": -3.781853156321443, + "velocityY": 0.027813363972772134, + "timestamp": 4.052247949432637 + }, + { + "x": 4.76815781079403, + "y": 6.6382116808939955, + "heading": 0.09523174637446119, + "angularVelocity": 4.7073965669390476e-7, + "velocityX": -3.7818532094044772, + "velocityY": 0.02780614520456082, + "timestamp": 4.1170054157113345 + }, + { + "x": 4.523254579114848, + "y": 6.640012336379186, + "heading": 0.09523177685836867, + "angularVelocity": 4.707396572416609e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814890212, + "timestamp": 4.181762881990032 + }, + { + "x": 4.278351347435667, + "y": 6.641812991864376, + "heading": 0.09523180734227604, + "angularVelocity": 4.7073965569633346e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869288, + "timestamp": 4.24652034826873 + }, + { + "x": 4.033448115756485, + "y": 6.643613647349565, + "heading": 0.0952318378261835, + "angularVelocity": 4.7073965673585443e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869194, + "timestamp": 4.311277814547427 + }, + { + "x": 3.7885448840773033, + "y": 6.645414302834754, + "heading": 0.09523186831009096, + "angularVelocity": 4.7073965722571307e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869288, + "timestamp": 4.376035280826125 + }, + { + "x": 3.5436416523981213, + "y": 6.647214958319943, + "heading": 0.09523189879399836, + "angularVelocity": 4.7073965583098703e-7, + "velocityX": -3.7818532094073416, + "velocityY": 0.027806144814869357, + "timestamp": 4.4407927471048225 + }, + { + "x": 3.2987384207189394, + "y": 6.649015613805132, + "heading": 0.0952319292779057, + "angularVelocity": 4.7073965534371566e-7, + "velocityX": -3.7818532094073416, + "velocityY": 0.02780614481486934, + "timestamp": 4.50555021338352 + }, + { + "x": 3.0538351890397575, + "y": 6.650816269290322, + "heading": 0.09523195976181308, + "angularVelocity": 4.7073965558339454e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869368, + "timestamp": 4.570307679662218 + }, + { + "x": 2.808931957360575, + "y": 6.65261692477551, + "heading": 0.09523199024572042, + "angularVelocity": 4.7073965499589656e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.02780614481484276, + "timestamp": 4.635065145940915 + }, + { + "x": 2.5640287256811582, + "y": 6.654417580228766, + "heading": 0.09523202072962778, + "angularVelocity": 4.7073965543506366e-7, + "velocityX": -3.7818532094109676, + "velocityY": 0.02780614432174875, + "timestamp": 4.699822612219613 + }, + { + "x": 2.319125489653766, + "y": 6.6562176441406535, + "heading": 0.09523205121544294, + "angularVelocity": 4.7076911613064953e-7, + "velocityX": -3.7818532765534276, + "velocityY": 0.027797009601036878, + "timestamp": 4.7645800784983106 + }, + { + "x": 2.0867659976251596, + "y": 6.656130735198093, + "heading": 0.13926625419972302, + "angularVelocity": 0.6799865021705632, + "velocityX": -3.5881498363230637, + "velocityY": -0.0013420682981450127, + "timestamp": 4.829337544777008 + }, + { + "x": 1.8694746618246978, + "y": 6.656990384791893, + "heading": 0.22988670590711813, + "angularVelocity": 1.3993822938870089, + "velocityX": -3.355463829688775, + "velocityY": 0.013274910882104354, + "timestamp": 4.894095011055706 + }, + { + "x": 1.6701637846976716, + "y": 6.657989605635195, + "heading": 0.33420976058736407, + "angularVelocity": 1.610981106506988, + "velocityX": -3.0778053648555033, + "velocityY": 0.01543020288968781, + "timestamp": 4.958852477334403 + }, + { + "x": 1.4891465844372453, + "y": 6.659013013376454, + "heading": 0.43962013302928266, + "angularVelocity": 1.6277717226962183, + "velocityX": -2.7953101111365104, + "velocityY": 0.015803702647255023, + "timestamp": 5.023609943613101 + }, + { + "x": 1.326396711582843, + "y": 6.660011984363377, + "heading": 0.5409065154536696, + "angularVelocity": 1.5640881004898903, + "velocityX": -2.513221751974858, + "velocityY": 0.015426344548804824, + "timestamp": 5.088367409891799 + }, + { + "x": 1.181861048474837, + "y": 6.660956547926137, + "heading": 0.6351810351446563, + "angularVelocity": 1.4558092697026699, + "velocityX": -2.2319536481857507, + "velocityY": 0.014586172329450045, + "timestamp": 5.153124876170496 + }, + { + "x": 1.0554921048807082, + "y": 6.661825696511207, + "heading": 0.7206099924431242, + "angularVelocity": 1.3192140182076537, + "velocityX": -1.9514188997184116, + "velocityY": 0.01342159653574964, + "timestamp": 5.217882342449194 + }, + { + "x": 0.9472506650106102, + "y": 6.66260279551869, + "heading": 0.7959103123026781, + "angularVelocity": 1.1628052205668817, + "velocityX": -1.6714897306861416, + "velocityY": 0.01200014534444738, + "timestamp": 5.282639808727891 + }, + { + "x": 0.8571042426441801, + "y": 6.663273874888148, + "heading": 0.8601164348709504, + "angularVelocity": 0.9914860209623859, + "velocityX": -1.392062221496826, + "velocityY": 0.010362965199572966, + "timestamp": 5.347397275006589 + }, + { + "x": 0.7850255726020201, + "y": 6.66382712376522, + "heading": 0.9124677407650426, + "angularVelocity": 0.8084211582458605, + "velocityX": -1.1130557476099119, + "velocityY": 0.00854339906830906, + "timestamp": 5.412154741285287 + }, + { + "x": 0.7309916974392009, + "y": 6.664252697305851, + "heading": 0.9523540062287302, + "angularVelocity": 0.6159330769988476, + "velocityX": -0.8344037879782468, + "velocityY": 0.006571806543502557, + "timestamp": 5.476912207563984 + }, + { + "x": 0.694983496367867, + "y": 6.664542513171668, + "heading": 0.9792885969521788, + "angularVelocity": 0.4159302744725947, + "velocityX": -0.5560470960424025, + "velocityY": 0.004475404651714408, + "timestamp": 5.541669673842682 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.21010218653726323, + "velocityX": -0.2779301718200537, + "velocityY": 0.0022777995651181563, + "timestamp": 5.606427140121379 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 2.2135835324585282e-26, + "velocityX": 2.644293811404756e-26, + "velocityY": 8.707515786736107e-27, + "timestamp": 5.671184606400077 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.traj b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.traj index a5e6f83f..eba31dff 100644 --- a/src/main/deploy/choreo/AmpLanePSubASubDSubESub.traj +++ b/src/main/deploy/choreo/AmpLanePSubASubDSubESub.traj @@ -1,1849 +1,1850 @@ { - "samples": [ - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0, - "velocityX": 2.1425070675381243e-37, - "velocityY": 2.00620509434581e-36, - "timestamp": 0 - }, - { - "x": 0.6967295535333877, - "y": 6.66750179345797, - "heading": 0.982893163249758, - "angularVelocity": -0.14737604826251527, - "velocityX": 0.29094834799603586, - "velocityY": 0.04143420364739335, - "timestamp": 0.06786122358481142 - }, - { - "x": 0.7362175895643961, - "y": 6.673125696218821, - "heading": 0.9628974920292898, - "angularVelocity": -0.2946553298656345, - "velocityX": 0.5818939586560375, - "velocityY": 0.08287358323008266, - "timestamp": 0.13572244716962284 - }, - { - "x": 0.7954495269492323, - "y": 6.681562379154691, - "heading": 0.9329256679312776, - "angularVelocity": -0.44166347900512215, - "velocityX": 0.8728392188627362, - "velocityY": 0.1243225879257299, - "timestamp": 0.20358367075443426 - }, - { - "x": 0.8744254360510917, - "y": 6.692812849791712, - "heading": 0.8930052032103389, - "angularVelocity": -0.5882662087730711, - "velocityX": 1.1637855159383756, - "velocityY": 0.1657864394820339, - "timestamp": 0.2714448943392457 - }, - { - "x": 0.9731453894549041, - "y": 6.7068785148813665, - "heading": 0.843168820166726, - "angularVelocity": -0.7343867441666224, - "velocityX": 1.4547328826223493, - "velocityY": 0.20727102086621554, - "timestamp": 0.33930611792405707 - }, - { - "x": 1.0916093532691815, - "y": 6.7237612036761645, - "heading": 0.7834495219946941, - "angularVelocity": -0.8800209459441316, - "velocityX": 1.7456797498828438, - "velocityY": 0.2487825580347626, - "timestamp": 0.40716734150886846 - }, - { - "x": 1.2298170767085217, - "y": 6.743463170039788, - "heading": 0.7138752049500412, - "angularVelocity": -1.025244069725628, - "velocityX": 2.0366229215807086, - "velocityY": 0.29032730803917456, - "timestamp": 0.47502856509367986 - }, - { - "x": 1.3877679992818084, - "y": 6.765987144948878, - "heading": 0.6344639927011461, - "angularVelocity": -1.1702001239286361, - "velocityX": 2.3275578339061793, - "velocityY": 0.3319123015950357, - "timestamp": 0.5428897886784912 - }, - { - "x": 1.5654611541888466, - "y": 6.791337011948423, - "heading": 0.5452227484246948, - "angularVelocity": -1.3150550426624645, - "velocityX": 2.618478499506586, - "velocityY": 0.3735545228986894, - "timestamp": 0.6107510122633026 - }, - { - "x": 1.7628875214635322, - "y": 6.819592757066269, - "heading": 0.4462737595759067, - "angularVelocity": -1.4581079388455742, - "velocityX": 2.909266247284013, - "velocityY": 0.41637541478357637, - "timestamp": 0.678612235848114 - }, - { - "x": 1.9405727781814897, - "y": 6.845027020375549, - "heading": 0.357188812683824, - "angularVelocity": -1.3127518512946696, - "velocityX": 2.6183621121403804, - "velocityY": 0.3747981831994576, - "timestamp": 0.7464734594329254 - }, - { - "x": 2.098517679535123, - "y": 6.867636881191052, - "heading": 0.2779750280562074, - "angularVelocity": -1.1672908392024057, - "velocityX": 2.3274691054787353, - "velocityY": 0.3331779125268215, - "timestamp": 0.8143346830177368 - }, - { - "x": 2.236722632634811, - "y": 6.887421048029417, - "heading": 0.20863780165516146, - "angularVelocity": -1.0217503124503762, - "velocityX": 2.036582097977683, - "velocityY": 0.29153861061228065, - "timestamp": 0.8821959066025482 - }, - { - "x": 2.3551878284960197, - "y": 6.9043786966768, - "heading": 0.1491789973537457, - "angularVelocity": -0.8761823197470863, - "velocityX": 1.745697905272119, - "velocityY": 0.24988716311885253, - "timestamp": 0.9500571301873596 - }, - { - "x": 2.4539133200632923, - "y": 6.918509209574482, - "heading": 0.09959774943286472, - "angularVelocity": -0.7306270842422328, - "velocityX": 1.454814492755025, - "velocityY": 0.20822661530737346, - "timestamp": 1.017918353772171 - }, - { - "x": 2.5328990844805293, - "y": 6.929812092655753, - "heading": 0.059891727352470836, - "angularVelocity": -0.5851061914728107, - "velocityX": 1.163930743431445, - "velocityY": 0.1665587869506308, - "timestamp": 1.0857795773569825 - }, - { - "x": 2.592145068506356, - "y": 6.938286954912776, - "heading": 0.030058218690118557, - "angularVelocity": -0.43962526884100517, - "velocityX": 0.8730462095453193, - "velocityY": 0.12488519671960763, - "timestamp": 1.153640800941794 - }, - { - "x": 2.63165121786692, - "y": 6.943933511859112, - "heading": 0.010094853186468074, - "angularVelocity": -0.2941792742463702, - "velocityX": 0.582160875882081, - "velocityY": 0.08320741430898287, - "timestamp": 1.2215020245266055 - }, - { - "x": 2.6514174938201904, - "y": 6.946751594543457, - "heading": 0, - "angularVelocity": -0.14875731166049128, - "velocityX": 0.2912749713180046, - "velocityY": 0.041527142239385896, - "timestamp": 1.289363248111417 - }, - { - "x": 2.649146332814054, - "y": 6.946410455915093, - "heading": 0.0008990727847708372, - "angularVelocity": 0.011985225672082133, - "velocityX": -0.030276055128411465, - "velocityY": -0.004547600056030398, - "timestamp": 1.3643783383040946 - }, - { - "x": 2.62275408560099, - "y": 6.9426132004627625, - "heading": 0.013859521326443051, - "angularVelocity": 0.17277121854260333, - "velocityX": -0.35182584124441, - "velocityY": -0.050619887846258024, - "timestamp": 1.4393934284967722 - }, - { - "x": 2.572240863242063, - "y": 6.93536015641962, - "heading": 0.03888546297295733, - "angularVelocity": 0.3336120983422759, - "velocityX": -0.6733741468440916, - "velocityY": -0.09668780007480728, - "timestamp": 1.5144085186894498 - }, - { - "x": 2.4976067609609713, - "y": 6.924651766873113, - "heading": 0.07598096942338527, - "angularVelocity": 0.4945072565419495, - "velocityX": -0.994921183049867, - "velocityY": -0.14274980565911422, - "timestamp": 1.5894236088821274 - }, - { - "x": 2.398851800518621, - "y": 6.910488564953806, - "heading": 0.12514851990507653, - "angularVelocity": 0.6554354644565975, - "velocityX": -1.3164679291686043, - "velocityY": -0.18880470426588675, - "timestamp": 1.664438699074805 - }, - { - "x": 2.275975845765142, - "y": 6.89287117810912, - "heading": 0.18638682928509862, - "angularVelocity": 0.8163465407124133, - "velocityX": -1.6380164902537586, - "velocityY": -0.23485123858993803, - "timestamp": 1.7394537892674826 - }, - { - "x": 2.1289785005722504, - "y": 6.871800394460987, - "heading": 0.2596886166153821, - "angularVelocity": 0.9771605571893133, - "velocityX": -1.95957033198688, - "velocityY": -0.28088726673543657, - "timestamp": 1.8144688794601602 - }, - { - "x": 1.9578590220057892, - "y": 6.847277322910046, - "heading": 0.34503981582540255, - "angularVelocity": 1.1377870637866898, - "velocityX": -2.2811340775161044, - "velocityY": -0.3269085125133225, - "timestamp": 1.8894839696528378 - }, - { - "x": 1.7626163173700675, - "y": 6.8193036552908985, - "heading": 0.4424228597913738, - "angularVelocity": 1.298179389184776, - "velocityX": -2.602712389390415, - "velocityY": -0.3729072050343056, - "timestamp": 1.9644990598455154 - }, - { - "x": 1.5454887933501882, - "y": 6.788364934906598, - "heading": 0.5521114777288451, - "angularVelocity": 1.4622207032709544, - "velocityX": -2.8944512825643915, - "velocityY": -0.4124332891533426, - "timestamp": 2.039514150038193 - }, - { - "x": 1.3524867399067801, - "y": 6.760871330718944, - "heading": 0.6497679920992822, - "angularVelocity": 1.3018249277525966, - "velocityX": -2.5728430499473998, - "velocityY": -0.3665076468885929, - "timestamp": 2.114529240230871 - }, - { - "x": 1.1836105740084832, - "y": 6.736819962879357, - "heading": 0.7353592838772256, - "angularVelocity": 1.140987654058677, - "velocityX": -2.251229258866918, - "velocityY": -0.3206203948806917, - "timestamp": 2.1895443304235487 - }, - { - "x": 1.0388602648983114, - "y": 6.7162082631102376, - "heading": 0.8088372428736526, - "angularVelocity": 0.9795090402170741, - "velocityX": -1.9296158777970915, - "velocityY": -0.2747673796855824, - "timestamp": 2.2645594206162265 - }, - { - "x": 0.9182354949315064, - "y": 6.6990341480262074, - "heading": 0.8701477848986364, - "angularVelocity": 0.8173094489056358, - "velocityX": -1.6080067311387376, - "velocityY": -0.22894213737419178, - "timestamp": 2.3395745108089043 - }, - { - "x": 0.8217358494246105, - "y": 6.685296075556227, - "heading": 0.919240073281317, - "angularVelocity": 0.6544321716682079, - "velocityX": -1.286403112480899, - "velocityY": -0.18313745187394112, - "timestamp": 2.414589601001582 - }, - { - "x": 0.7493609915443247, - "y": 6.674993018423988, - "heading": 0.956074222977567, - "angularVelocity": 0.49102320082053863, - "velocityX": -0.9648039840302745, - "velocityY": -0.13734646063578734, - "timestamp": 2.48960469119426 - }, - { - "x": 0.7011107973318162, - "y": 6.66812439818581, - "heading": 0.9806268225378217, - "angularVelocity": 0.32730214010529163, - "velocityX": -0.6432065080316098, - "velocityY": -0.09156318042856706, - "timestamp": 2.564619781386938 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.16353322568319786, - "velocityX": -0.32160668782044516, - "velocityY": -0.04578252824590232, - "timestamp": 2.6396348715796156 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -9.098710086763465e-32, - "velocityX": -1.2738111607141702e-32, - "velocityY": 8.271252813231263e-33, - "timestamp": 2.7146499617722935 - }, - { - "x": 0.6985020843742907, - "y": 6.667569321822967, - "heading": 0.9798246448067045, - "angularVelocity": -0.18429871952070437, - "velocityX": 0.3034123592993086, - "velocityY": 0.04060189632535011, - "timestamp": 2.7855654702426254 - }, - { - "x": 0.7415443471125377, - "y": 6.673333307735536, - "heading": 0.9539749013573504, - "angularVelocity": -0.36451467396822734, - "velocityX": 0.6069513378199061, - "velocityY": 0.081279624681546, - "timestamp": 2.8564809787129573 - }, - { - "x": 0.8061231921601518, - "y": 6.681988659505296, - "heading": 0.9157093279961269, - "angularVelocity": -0.5395938658076773, - "velocityX": 0.9106448848862336, - "velocityY": 0.12205160699625474, - "timestamp": 2.927396487183289 - }, - { - "x": 0.8922518573326212, - "y": 6.693543414119111, - "heading": 0.8654790283080297, - "angularVelocity": -0.7083119161320186, - "velocityX": 1.2145251021996393, - "velocityY": 0.16293692117639821, - "timestamp": 2.998311995653621 - }, - { - "x": 0.9999464058047857, - "y": 6.708007002320034, - "heading": 0.8038456081958845, - "angularVelocity": -0.8691105999462724, - "velocityX": 1.5186318309657076, - "velocityY": 0.20395522097926097, - "timestamp": 3.069227504123953 - }, - { - "x": 1.1292266926067027, - "y": 6.725390389005233, - "heading": 0.7315277652632032, - "angularVelocity": -1.0197747219557198, - "velocityX": 1.8230185412264566, - "velocityY": 0.2451281399536478, - "timestamp": 3.140143012594285 - }, - { - "x": 1.280117879093583, - "y": 6.7457065764596775, - "heading": 0.6494907937479546, - "angularVelocity": -1.1568269520279806, - "velocityX": 2.127760059000449, - "velocityY": 0.28648440789144525, - "timestamp": 3.2110585210646168 - }, - { - "x": 1.452652377278095, - "y": 6.768971929861101, - "heading": 0.5591180429519423, - "angularVelocity": -1.2743721753587984, - "velocityX": 2.4329586278958, - "velocityY": 0.3280714459116738, - "timestamp": 3.2819740295349487 - }, - { - "x": 1.6468709656789369, - "y": 6.795208953072923, - "heading": 0.46255966504991763, - "angularVelocity": -1.361597483890574, - "velocityX": 2.738732226422582, - "velocityY": 0.3699758173883681, - "timestamp": 3.3528895380052806 - }, - { - "x": 1.8628151056303066, - "y": 6.824451100190135, - "heading": 0.3635782548385496, - "angularVelocity": -1.3957653600238602, - "velocityX": 3.045090483158721, - "velocityY": 0.41235193468922193, - "timestamp": 3.4238050464756125 - }, - { - "x": 2.100434676341306, - "y": 6.8567478442017284, - "heading": 0.27059443428530877, - "angularVelocity": -1.3111916216766784, - "velocityX": 3.3507419721937035, - "velocityY": 0.4554256848500797, - "timestamp": 3.4947205549459444 - }, - { - "x": 2.35596385739112, - "y": 6.892112273565999, - "heading": 0.23346194806127452, - "angularVelocity": -0.5236158778945916, - "velocityX": 3.603290543375525, - "velocityY": 0.4986839991292765, - "timestamp": 3.5656360634162763 - }, - { - "x": 2.621781213038304, - "y": 6.927777327664925, - "heading": 0.23346192227272974, - "angularVelocity": -3.6365169422849345e-7, - "velocityX": 3.7483670551187007, - "velocityY": 0.5029231950560806, - "timestamp": 3.636551571886608 - }, - { - "x": 2.887598578486195, - "y": 6.963442308718448, - "heading": 0.23346189648452345, - "angularVelocity": -3.636469208413295e-7, - "velocityX": 3.748367193321304, - "velocityY": 0.502922165021807, - "timestamp": 3.70746708035694 - }, - { - "x": 3.153415943934485, - "y": 6.999107289769, - "heading": 0.23346187069631716, - "angularVelocity": -3.636469205820025e-7, - "velocityX": 3.7483671933269203, - "velocityY": 0.5029221649799431, - "timestamp": 3.778382588827272 - }, - { - "x": 3.4192333093827747, - "y": 7.034772270819553, - "heading": 0.2334618449081109, - "angularVelocity": -3.6364692054215583e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799415, - "timestamp": 3.849298097297604 - }, - { - "x": 3.6850506748310643, - "y": 7.070437251870106, - "heading": 0.23346181911990457, - "angularVelocity": -3.6364692122502775e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799416, - "timestamp": 3.9202136057679358 - }, - { - "x": 3.950868040279354, - "y": 7.106102232920659, - "heading": 0.2334617933316983, - "angularVelocity": -3.636469204641407e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799416, - "timestamp": 3.9911291142382677 - }, - { - "x": 4.216685405727644, - "y": 7.141767213971211, - "heading": 0.23346176754349196, - "angularVelocity": -3.6364692150109834e-7, - "velocityX": 3.7483671933269203, - "velocityY": 0.5029221649799415, - "timestamp": 4.0620446227086 - }, - { - "x": 4.482502771175934, - "y": 7.1774321950217645, - "heading": 0.2334617417552857, - "angularVelocity": -3.636469207728054e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799417, - "timestamp": 4.1329601311789315 - }, - { - "x": 4.7483201366242245, - "y": 7.213097176072317, - "heading": 0.2334617159670794, - "angularVelocity": -3.636469206602435e-7, - "velocityX": 3.7483671933269203, - "velocityY": 0.5029221649799417, - "timestamp": 4.203875639649263 - }, - { - "x": 5.014137502072515, - "y": 7.2487621571228695, - "heading": 0.2334616901788731, - "angularVelocity": -3.6364692079095153e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799416, - "timestamp": 4.274791148119595 - }, - { - "x": 5.279954867520805, - "y": 7.284427138173423, - "heading": 0.23346166439066685, - "angularVelocity": -3.636469205268936e-7, - "velocityX": 3.7483671933269207, - "velocityY": 0.5029221649799417, - "timestamp": 4.345706656589927 - }, - { - "x": 5.545772232969095, - "y": 7.320092119223974, - "heading": 0.23346163860246044, - "angularVelocity": -3.6364692217914213e-7, - "velocityX": 3.748367193326924, - "velocityY": 0.5029221649799175, - "timestamp": 4.416622165060259 - }, - { - "x": 5.81158959842306, - "y": 7.355757100232221, - "heading": 0.23346161281425412, - "angularVelocity": -3.636469211715136e-7, - "velocityX": 3.748367193406962, - "velocityY": 0.5029221643833769, - "timestamp": 4.487537673530591 - }, - { - "x": 6.077407103529163, - "y": 7.391421040372378, - "heading": 0.23346158702575517, - "angularVelocity": -3.63651047258994e-7, - "velocityX": 3.748369162681932, - "velocityY": 0.5029074868027953, - "timestamp": 4.558453182000923 - }, - { - "x": 6.338444265915037, - "y": 7.413466456985474, - "heading": 0.21125647663717811, - "angularVelocity": -0.3131206539662162, - "velocityX": 3.680960173825454, - "velocityY": 0.310868766065739, - "timestamp": 4.629368690471255 - }, - { - "x": 6.578740605850335, - "y": 7.432100572083313, - "heading": 0.16288336837633732, - "angularVelocity": -0.6821231251705402, - "velocityX": 3.388487865610223, - "velocityY": 0.26276502135825913, - "timestamp": 4.700284198941587 - }, - { - "x": 6.797330438016248, - "y": 7.447554226429222, - "heading": 0.11319688856078455, - "angularVelocity": -0.7006433555551449, - "velocityX": 3.082398150714271, - "velocityY": 0.21791642870860864, - "timestamp": 4.771199707411919 - }, - { - "x": 6.99422444679472, - "y": 7.45992830223967, - "heading": 0.06789887708325426, - "angularVelocity": -0.638760300174411, - "velocityX": 2.7764590993639087, - "velocityY": 0.1744904052351846, - "timestamp": 4.8421152158822505 - }, - { - "x": 7.169459125576906, - "y": 7.4692727675317165, - "heading": 0.029649915927990267, - "angularVelocity": -0.539359612309142, - "velocityX": 2.4710346518279107, - "velocityY": 0.13176899515507345, - "timestamp": 4.913030724352582 - }, - { - "x": 7.323064804077148, - "y": 7.475617408752441, - "heading": -2.508046324740631e-30, - "angularVelocity": -0.41810200007794424, - "velocityX": 2.166037892325118, - "velocityY": 0.08946761234010188, - "timestamp": 4.983946232822914 - }, - { - "x": 7.442670757639025, - "y": 7.478903848182195, - "heading": -0.0187747254681701, - "angularVelocity": -0.297438409871576, - "velocityX": 1.8948561830587192, - "velocityY": 0.05206538544539075, - "timestamp": 5.04706762147034 - }, - { - "x": 7.5451140174844875, - "y": 7.480040188586648, - "heading": -0.030609169580020613, - "angularVelocity": -0.1874870684158837, - "velocityX": 1.6229563708994625, - "velocityY": 0.01800246206270259, - "timestamp": 5.110189010117766 - }, - { - "x": 7.630363412663352, - "y": 7.479184974890449, - "heading": -0.03600750245908947, - "angularVelocity": -0.08552303735288487, - "velocityX": 1.3505627332580479, - "velocityY": -0.013548714857401807, - "timestamp": 5.173310398765192 - }, - { - "x": 7.698396363869973, - "y": 7.476461754192338, - "heading": -0.03536054977456298, - "angularVelocity": 0.01024934175875415, - "velocityX": 1.0778113831814056, - "velocityY": -0.04314259803938343, - "timestamp": 5.236431787412618 - }, - { - "x": 7.74919584977197, - "y": 7.471969501335234, - "heading": -0.02898050014133947, - "angularVelocity": 0.10107587570456572, - "velocityX": 0.8047903728082677, - "velocityY": -0.0711684732126695, - "timestamp": 5.299553176060044 - }, - { - "x": 7.782748628307226, - "y": 7.465789287122271, - "heading": -0.017122702576199005, - "angularVelocity": 0.1878570452777808, - "velocityX": 0.5315595751917994, - "velocityY": -0.09790998495750272, - "timestamp": 5.36267456470747 - }, - { - "x": 7.799044132232666, - "y": 7.457988739013672, - "heading": 0, - "angularVelocity": 0.2712662528992439, - "velocityX": 0.25816136612733137, - "velocityY": -0.12358010930615114, - "timestamp": 5.425795953354896 - }, - { - "x": 7.789461840084544, - "y": 7.44456429977742, - "heading": 0.032498029722470304, - "angularVelocity": 0.38031209369790614, - "velocityX": -0.11213792406398307, - "velocityY": -0.15710111155221265, - "timestamp": 5.511246903406188 - }, - { - "x": 7.748231627408751, - "y": 7.428284690558898, - "heading": 0.07405407961857692, - "angularVelocity": 0.48631466205092383, - "velocityX": -0.48250151286758974, - "velocityY": -0.1905140809873915, - "timestamp": 5.59669785345748 - }, - { - "x": 7.675346865346751, - "y": 7.40916132937864, - "heading": 0.1243473311272727, - "angularVelocity": 0.5885628126840949, - "velocityX": -0.8529426766838836, - "velocityY": -0.22379342966671825, - "timestamp": 5.682148803508771 - }, - { - "x": 7.570799431207237, - "y": 7.387208662011206, - "heading": 0.1829724416238929, - "angularVelocity": 0.6860673925971562, - "velocityX": -1.2234788972702, - "velocityY": -0.2569037249352633, - "timestamp": 5.767599753560063 - }, - { - "x": 7.434579177146227, - "y": 7.362445545124385, - "heading": 0.24939989698187892, - "angularVelocity": 0.7773752698843693, - "velocityX": -1.5941338742188234, - "velocityY": -0.28979334778557075, - "timestamp": 5.853050703611355 - }, - { - "x": 7.266673120974145, - "y": 7.334897700728802, - "heading": 0.32290448608070227, - "angularVelocity": 0.8601962769834678, - "velocityX": -1.9649407767992484, - "velocityY": -0.3223819557190156, - "timestamp": 5.938501653662646 - }, - { - "x": 7.067064218914486, - "y": 7.304602559914413, - "heading": 0.4024227499708516, - "angularVelocity": 0.9305720280744717, - "velocityX": -2.3359471362200686, - "velocityY": -0.3545325218292511, - "timestamp": 6.023952603713938 - }, - { - "x": 6.835729836802405, - "y": 7.27162039096164, - "heading": 0.48622750779609625, - "angularVelocity": 0.9807352378746566, - "velocityX": -2.7072183746718483, - "velocityY": -0.385977791153593, - "timestamp": 6.10940355376523 - }, - { - "x": 6.572643492795426, - "y": 7.236066659207437, - "heading": 0.571010808696293, - "angularVelocity": 0.9921867556667185, - "velocityX": -3.078799520063915, - "velocityY": -0.4160718135124634, - "timestamp": 6.194854503816521 - }, - { - "x": 6.277837622424222, - "y": 7.198257703016904, - "heading": 0.6480492113566031, - "angularVelocity": 0.901551154360156, - "velocityX": -3.450001084764899, - "velocityY": -0.4424638481823218, - "timestamp": 6.280305453867813 - }, - { - "x": 5.957362245190181, - "y": 7.156598764006078, - "heading": 0.6480492838419923, - "angularVelocity": 8.482689682334955e-7, - "velocityX": -3.7504015700432527, - "velocityY": -0.487518734265005, - "timestamp": 6.365756403919105 - }, - { - "x": 5.635584365409571, - "y": 7.126616390006558, - "heading": 0.6480492987117265, - "angularVelocity": 1.7401484932642463e-7, - "velocityX": -3.7656442624390154, - "velocityY": -0.3508723306355692, - "timestamp": 6.451207353970396 - }, - { - "x": 5.313806456205795, - "y": 7.096634331785028, - "heading": 0.6480493135814611, - "angularVelocity": 1.7401485546331995e-7, - "velocityX": -3.7656446067671383, - "velocityY": -0.3508686352057408, - "timestamp": 6.536658304021688 - }, - { - "x": 4.992028547001338, - "y": 7.066652273570816, - "heading": 0.6480493284511959, - "angularVelocity": 1.7401485563507226e-7, - "velocityX": -3.7656446067751177, - "velocityY": -0.3508686351200953, - "timestamp": 6.62210925407298 - }, - { - "x": 4.67025063779688, - "y": 7.036670215356605, - "heading": 0.6480493433209307, - "angularVelocity": 1.7401485639450837e-7, - "velocityX": -3.765644606775118, - "velocityY": -0.35086863512009386, - "timestamp": 6.707560204124271 - }, - { - "x": 4.348472728592423, - "y": 7.006688157142394, - "heading": 0.6480493581906654, - "angularVelocity": 1.740148559122135e-7, - "velocityX": -3.7656446067751186, - "velocityY": -0.35086863512009364, - "timestamp": 6.793011154175563 - }, - { - "x": 4.0266948193879655, - "y": 6.976706098928182, - "heading": 0.6480493730604002, - "angularVelocity": 1.740148565017462e-7, - "velocityX": -3.765644606775118, - "velocityY": -0.35086863512009375, - "timestamp": 6.878462104226855 - }, - { - "x": 3.7049169101835084, - "y": 6.946724040713971, - "heading": 0.648049387930135, - "angularVelocity": 1.7401485552645738e-7, - "velocityX": -3.765644606775118, - "velocityY": -0.3508686351200936, - "timestamp": 6.963913054278146 - }, - { - "x": 3.3831390009790514, - "y": 6.91674198249976, - "heading": 0.6480494027998698, - "angularVelocity": 1.7401485630496073e-7, - "velocityX": -3.7656446067751177, - "velocityY": -0.35086863512009364, - "timestamp": 7.049364004329438 - }, - { - "x": 3.0613610917745944, - "y": 6.886759924285549, - "heading": 0.6480494176696047, - "angularVelocity": 1.7401485676252163e-7, - "velocityX": -3.7656446067751177, - "velocityY": -0.3508686351200934, - "timestamp": 7.1348149543807295 - }, - { - "x": 2.7395831825701378, - "y": 6.85677786607133, - "heading": 0.6480494325393393, - "angularVelocity": 1.740148561275008e-7, - "velocityX": -3.7656446067751106, - "velocityY": -0.35086863512017596, - "timestamp": 7.220265904432021 - }, - { - "x": 2.4178052733940274, - "y": 6.826795807553092, - "heading": 0.648049447409112, - "angularVelocity": 1.740152990535353e-7, - "velocityX": -3.765644606443389, - "velocityY": -0.3508686386780044, - "timestamp": 7.305716854483313 - }, - { - "x": 2.101806987486401, - "y": 6.79733725091672, - "heading": 0.6651218958442715, - "angularVelocity": 0.19979237708727937, - "velocityX": -3.698007871392473, - "velocityY": -0.34474229506742526, - "timestamp": 7.3911678045346045 - }, - { - "x": 1.8167368698048942, - "y": 6.770773756148669, - "heading": 0.7108427694079615, - "angularVelocity": 0.5350540109419666, - "velocityX": -3.3360672702926424, - "velocityY": -0.31086248604736477, - "timestamp": 7.476618754585896 - }, - { - "x": 1.563349701636476, - "y": 6.747170828438536, - "heading": 0.7627235430971158, - "angularVelocity": 0.6071409815583658, - "velocityX": -2.9652937505825308, - "velocityY": -0.2762160946831818, - "timestamp": 7.562069704637188 - }, - { - "x": 1.3416809560568124, - "y": 6.726531033314501, - "heading": 0.8140209677583794, - "angularVelocity": 0.6003142695377961, - "velocityX": -2.5941051029464517, - "velocityY": -0.24153967991752093, - "timestamp": 7.6475206546884795 - }, - { - "x": 1.1517177468979243, - "y": 6.708849739610871, - "heading": 0.8615071361475246, - "angularVelocity": 0.5557125855327181, - "velocityX": -2.2230672572379273, - "velocityY": -0.20691746192426994, - "timestamp": 7.732971604739771 - }, - { - "x": 0.993443621158485, - "y": 6.694122461254965, - "heading": 0.9032823319492421, - "angularVelocity": 0.48887924331700494, - "velocityX": -1.852221954752358, - "velocityY": -0.17234774273507145, - "timestamp": 7.818422554791063 - }, - { - "x": 0.8668441821472115, - "y": 6.682345527785774, - "heading": 0.9380916664803222, - "angularVelocity": 0.40736041565660025, - "velocityX": -1.4815451312746155, - "velocityY": -0.13782097755638892, - "timestamp": 7.9038735048423545 - }, - { - "x": 0.7719075388323822, - "y": 6.673515992217611, - "heading": 0.9650448121199915, - "angularVelocity": 0.3154224221444248, - "velocityX": -1.111007464022926, - "velocityY": -0.10332869983146084, - "timestamp": 7.989324454893646 - }, - { - "x": 0.7086239541540638, - "y": 6.667631472022659, - "heading": 0.9834806533033196, - "angularVelocity": 0.21574764437724195, - "velocityX": -0.7405837458838845, - "velocityY": -0.06886430392430233, - "timestamp": 8.074775404944939 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.11016412225954963, - "velocityX": -0.37025347871120917, - "velocityY": -0.03442272228334841, - "timestamp": 8.160226354996231 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -1.2387955760475958e-29, - "velocityX": 1.44513347457817e-31, - "velocityY": -4.5041627226851734e-30, - "timestamp": 8.245677305047524 - }, - { - "x": 0.6937129747352162, - "y": 6.6645584157609425, - "heading": 0.9790660169157249, - "angularVelocity": -0.22114767057901943, - "velocityX": 0.2675140141228054, - "velocityY": -0.002104635808322496, - "timestamp": 8.308206859439915 - }, - { - "x": 0.7271833460248389, - "y": 6.664303045137076, - "heading": 0.9517582917370493, - "angularVelocity": -0.43671709232583583, - "velocityX": 0.5352728260237692, - "velocityY": -0.004083998780231037, - "timestamp": 8.370736413832306 - }, - { - "x": 0.7774150547311363, - "y": 6.663933728618546, - "heading": 0.9114059815034875, - "angularVelocity": -0.645331805506542, - "velocityX": 0.8033274696166545, - "velocityY": -0.005906271396278706, - "timestamp": 8.433265968224697 - }, - { - "x": 0.8444295954022788, - "y": 6.663462332060666, - "heading": 0.8585277843526951, - "angularVelocity": -0.8456512710608117, - "velocityX": 1.0717258634310107, - "velocityY": -0.007538780061052878, - "timestamp": 8.495795522617088 - }, - { - "x": 0.928251455530078, - "y": 6.662902493101364, - "heading": 0.793730196165171, - "angularVelocity": -1.0362713890602748, - "velocityX": 1.3405158719313994, - "velocityY": -0.008953189651546989, - "timestamp": 8.55832507700948 - }, - { - "x": 1.0289086002870482, - "y": 6.66226903623183, - "heading": 0.7177284863656903, - "angularVelocity": -1.2154526053799588, - "velocityX": 1.6097531117096386, - "velocityY": -0.010130519490973162, - "timestamp": 8.62085463140187 - }, - { - "x": 1.1464337376402622, - "y": 6.661577427221888, - "heading": 0.6314012242837341, - "angularVelocity": -1.3805833564753438, - "velocityX": 1.8795134316120001, - "velocityY": -0.011060513970755739, - "timestamp": 8.683384185794262 - }, - { - "x": 1.2808663505867972, - "y": 6.660844036954463, - "heading": 0.5359002148977876, - "angularVelocity": -1.5272939382655537, - "velocityX": 2.149905180883418, - "velocityY": -0.011728698126054097, - "timestamp": 8.745913740186653 - }, - { - "x": 1.4322545638154922, - "y": 6.660088220029616, - "heading": 0.43284520187288844, - "angularVelocity": -1.6481008704811424, - "velocityX": 2.4210665612405937, - "velocityY": -0.012087355046610722, - "timestamp": 8.808443294579044 - }, - { - "x": 1.6006535211132875, - "y": 6.65933694718367, - "heading": 0.3246624035723361, - "angularVelocity": -1.7301066567926002, - "velocityX": 2.69310982517228, - "velocityY": -0.012014684148101123, - "timestamp": 8.870972848971435 - }, - { - "x": 1.7861082105175972, - "y": 6.658632447498398, - "heading": 0.21527496164562404, - "angularVelocity": -1.7493718448763058, - "velocityX": 2.965872557487375, - "velocityY": -0.011266667292259528, - "timestamp": 8.933502403363827 - }, - { - "x": 1.9885470293511631, - "y": 6.658050737392514, - "heading": 0.11225233797545536, - "angularVelocity": -1.6475828857450405, - "velocityX": 3.2374901884507423, - "velocityY": -0.009302962599615121, - "timestamp": 8.996031957756218 - }, - { - "x": 2.206257706753187, - "y": 6.657808122247979, - "heading": 0.04234893056424459, - "angularVelocity": -1.1179258846552083, - "velocityX": 3.4817244344302054, - "velocityY": -0.0038800075722999405, - "timestamp": 9.058561512148609 - }, - { - "x": 2.437370646545927, - "y": 6.657710110655747, - "heading": 0.021561241595864127, - "angularVelocity": -0.33244581974679344, - "velocityX": 3.6960592801035483, - "velocityY": -0.0015674442779168942, - "timestamp": 9.121091066541 - }, - { - "x": 2.6738489696177092, - "y": 6.656073614188434, - "heading": 0.021561206140011186, - "angularVelocity": -5.670255175242252e-7, - "velocityX": 3.7818648376703368, - "velocityY": -0.02617156772049459, - "timestamp": 9.183620620933391 - }, - { - "x": 2.910327291911898, - "y": 6.654437005347333, - "heading": 0.021561170684433324, - "angularVelocity": -5.670211183604034e-7, - "velocityX": 3.7818648252347256, - "velocityY": -0.026173364851293936, - "timestamp": 9.246150175325782 - }, - { - "x": 3.14680561420604, - "y": 6.652800396499498, - "heading": 0.021561135228855402, - "angularVelocity": -5.670211192982607e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958984518, - "timestamp": 9.308679729718174 - }, - { - "x": 3.3832839365001823, - "y": 6.6511637876516625, - "heading": 0.02156109977327751, - "angularVelocity": -5.670211188277871e-7, - "velocityX": 3.7818648252339804, - "velocityY": -0.02617336495899097, - "timestamp": 9.371209284110565 - }, - { - "x": 3.6197622587943243, - "y": 6.649527178803828, - "heading": 0.02156106431769964, - "angularVelocity": -5.670211185570758e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958990947, - "timestamp": 9.433738838502956 - }, - { - "x": 3.8562405810884663, - "y": 6.647890569955993, - "heading": 0.02156102886212173, - "angularVelocity": -5.670211191121069e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958990954, - "timestamp": 9.496268392895347 - }, - { - "x": 4.092718903382608, - "y": 6.646253961108158, - "heading": 0.02156099340654377, - "angularVelocity": -5.670211198937042e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.02617336495899093, - "timestamp": 9.558797947287738 - }, - { - "x": 4.329197225676751, - "y": 6.6446173522603225, - "heading": 0.021560957950965815, - "angularVelocity": -5.67021119881002e-7, - "velocityX": 3.7818648252339804, - "velocityY": -0.026173364958990898, - "timestamp": 9.62132750168013 - }, - { - "x": 4.565675547970892, - "y": 6.642980743412488, - "heading": 0.021560922495387855, - "angularVelocity": -5.670211198956703e-7, - "velocityX": 3.78186482523398, - "velocityY": -0.026173364958990943, - "timestamp": 9.68385705607252 - }, - { - "x": 4.802153870265035, - "y": 6.641344134564649, - "heading": 0.02156088703980993, - "angularVelocity": -5.670211193772164e-7, - "velocityX": 3.7818648252339795, - "velocityY": -0.026173364959042097, - "timestamp": 9.746386610464912 - }, - { - "x": 5.038632192558808, - "y": 6.639707525663405, - "heading": 0.021560851584232044, - "angularVelocity": -5.67021118724796e-7, - "velocityX": 3.781864825228069, - "velocityY": -0.02617336581314012, - "timestamp": 9.808916164857303 - }, - { - "x": 5.275110508682838, - "y": 6.638070025521481, - "heading": 0.021560816128648946, - "angularVelocity": -5.670212021371837e-7, - "velocityX": 3.781864726558842, - "velocityY": -0.02618761892412933, - "timestamp": 9.871445719249694 - }, - { - "x": 5.511317621528859, - "y": 6.626629955813148, - "heading": 0.02156078050955198, - "angularVelocity": -5.696361874718345e-7, - "velocityX": 3.777527525044398, - "velocityY": -0.18295460153997642, - "timestamp": 9.933975273642085 - }, - { - "x": 5.7460856437683105, - "y": 6.598193168640137, - "heading": 0.021560744143059335, - "angularVelocity": -5.815888661825855e-7, - "velocityX": 3.7545129582438856, - "velocityY": -0.45477354587499197, - "timestamp": 9.996504828034476 - }, - { - "x": 5.947788996667301, - "y": 6.560838784941361, - "heading": 0.021560710996949464, - "angularVelocity": -6.111012841975544e-7, - "velocityX": 3.718722301384556, - "velocityY": -0.6886875092486842, - "timestamp": 10.050744789115704 - }, - { - "x": 6.146761517861249, - "y": 6.510943609310236, - "heading": 0.021560678630883793, - "angularVelocity": -5.967199279900907e-7, - "velocityX": 3.6683750730568514, - "velocityY": -0.9198969659363234, - "timestamp": 10.104984750196932 - }, - { - "x": 6.342740574859562, - "y": 6.4503482133201375, - "heading": 0.021560646512747805, - "angularVelocity": -5.921489497600962e-7, - "velocityX": 3.6131857968117713, - "velocityY": -1.1171725565834012, - "timestamp": 10.15922471127816 - }, - { - "x": 6.53435736566076, - "y": 6.392349027665542, - "heading": 0.005697481882948994, - "angularVelocity": -0.2924626845886352, - "velocityX": 3.532760477358042, - "velocityY": -1.0693072874395997, - "timestamp": 10.213464672359388 - }, - { - "x": 6.715860798361231, - "y": 6.336732624061585, - "heading": -0.044321515304948156, - "angularVelocity": -0.9221798133850251, - "velocityX": 3.3463046263742644, - "velocityY": -1.0253769083769828, - "timestamp": 10.267704633440616 - }, - { - "x": 6.885664142970145, - "y": 6.284563070375092, - "heading": -0.11100118706044729, - "angularVelocity": -1.2293458628342813, - "velocityX": 3.1305948828876353, - "velocityY": -0.9618287448319874, - "timestamp": 10.321944594521844 - }, - { - "x": 7.043230365082517, - "y": 6.236112426000239, - "heading": -0.18201634611185183, - "angularVelocity": -1.309277470628262, - "velocityX": 2.9049840555085917, - "velocityY": -0.8932647334000697, - "timestamp": 10.376184555603071 - }, - { - "x": 7.188521106986847, - "y": 6.19141127344733, - "heading": -0.25302239943673055, - "angularVelocity": -1.3091095920689053, - "velocityX": 2.678666042675613, - "velocityY": -0.8241368847217142, - "timestamp": 10.4304245166843 - }, - { - "x": 7.3215520348116, - "y": 6.150465382048361, - "heading": -0.3216933073087357, - "angularVelocity": -1.2660574694949878, - "velocityX": 2.4526368598519452, - "velocityY": -0.7549026692266663, - "timestamp": 10.484664477765527 - }, - { - "x": 7.4423459735084, - "y": 6.113273952325478, - "heading": -0.38654982711023245, - "angularVelocity": -1.1957331551984454, - "velocityX": 2.2270284913351, - "velocityY": -0.685683193378148, - "timestamp": 10.538904438846755 - }, - { - "x": 7.550924553999962, - "y": 6.079834322504852, - "heading": -0.4465624539391904, - "angularVelocity": -1.1064282796789968, - "velocityX": 2.001818923301949, - "velocityY": -0.6165127915661526, - "timestamp": 10.593144399927983 - }, - { - "x": 7.647306628463083, - "y": 6.050143536065283, - "heading": -0.5009752926426013, - "angularVelocity": -1.0031872740823848, - "velocityX": 1.7769569251493995, - "velocityY": -0.5473968979274295, - "timestamp": 10.64738436100921 - }, - { - "x": 7.7315082292215935, - "y": 6.024198841891014, - "heading": -0.5492132046279071, - "angularVelocity": -0.8893426732564748, - "velocityX": 1.552390508400531, - "velocityY": -0.47833172548586056, - "timestamp": 10.701624322090439 - }, - { - "x": 7.803542912083633, - "y": 6.001997808520636, - "heading": -0.5908266102991006, - "angularVelocity": -0.7672093571172569, - "velocityX": 1.3280740145473924, - "velocityY": -0.4093113809047783, - "timestamp": 10.755864283171666 - }, - { - "x": 7.863422173267542, - "y": 5.983538316289689, - "heading": -0.6254559233807054, - "angularVelocity": -0.6384464957440807, - "velocityX": 1.103969471774435, - "velocityY": -0.34033011571123517, - "timestamp": 10.810104244252894 - }, - { - "x": 7.9111558450934325, - "y": 5.968818528744131, - "heading": -0.6528074420175133, - "angularVelocity": -0.5042687732730391, - "velocityX": 0.8800462034699242, - "velocityY": -0.2713827084704847, - "timestamp": 10.864344205334122 - }, - { - "x": 7.946752441580289, - "y": 5.95783687514921, - "heading": -0.6726364642205706, - "angularVelocity": -0.36557958021692255, - "velocityX": 0.6562799046545997, - "velocityY": -0.2024642602245795, - "timestamp": 10.91858416641535 - }, - { - "x": 7.970219449253945, - "y": 5.950592046499088, - "heading": -0.6847351436740425, - "angularVelocity": -0.22305840956179235, - "velocityX": 0.43265163185705313, - "velocityY": -0.1335699455844713, - "timestamp": 10.972824127496578 - }, - { - "x": 7.981563568115234, - "y": 5.947082996368408, - "heading": -0.6889234822214203, - "angularVelocity": -0.07721868644237204, - "velocityX": 0.2091468842372586, - "velocityY": -0.06469492346102504, - "timestamp": 11.027064088577806 - }, - { - "x": 7.980191051552582, - "y": 5.9474933088994915, - "heading": -0.684492260818683, - "angularVelocity": 0.07820368392725321, - "velocityX": -0.024222633376945003, - "velocityY": 0.007241333387762343, - "timestamp": 11.083726654587604 - }, - { - "x": 7.965589050123253, - "y": 5.951981018210289, - "heading": -0.6714197034271625, - "angularVelocity": 0.23070888440280377, - "velocityX": -0.257701026579075, - "velocityY": 0.07920060150509887, - "timestamp": 11.140389220597402 - }, - { - "x": 7.9377506970473615, - "y": 5.960547682425779, - "heading": -0.6498948487141165, - "angularVelocity": 0.37987786697348064, - "velocityX": -0.4913006070194107, - "velocityY": 0.15118736793544205, - "timestamp": 11.1970517866072 - }, - { - "x": 7.896668195667252, - "y": 5.973195099193875, - "heading": -0.6201381150730912, - "angularVelocity": 0.5251568316881386, - "velocityX": -0.7250377854932653, - "velocityY": 0.22320585986009217, - "timestamp": 11.253714352617 - }, - { - "x": 7.842332542279827, - "y": 5.989925310225715, - "heading": -0.5824114694602323, - "angularVelocity": 0.6658125155563156, - "velocityX": -0.9589338643440537, - "velocityY": 0.2952603845887764, - "timestamp": 11.310376918626798 - }, - { - "x": 7.774733202893158, - "y": 6.010740641182104, - "heading": -0.5370321516926094, - "angularVelocity": 0.8008694445602113, - "velocityX": -1.1930158506231556, - "velocityY": 0.367355953360645, - "timestamp": 11.367039484636596 - }, - { - "x": 7.693857750948662, - "y": 6.035643782735694, - "heading": -0.48439126574422153, - "angularVelocity": 0.9290240392446375, - "velocityX": -1.4273171449826625, - "velocityY": 0.43949900802735137, - "timestamp": 11.423702050646394 - }, - { - "x": 7.5996914832698845, - "y": 6.064637893376439, - "heading": -0.4249792327859352, - "angularVelocity": 1.0485235163549331, - "velocityX": -1.6618779259395846, - "velocityY": 0.5116978047858136, - "timestamp": 11.480364616656193 - }, - { - "x": 7.492217045643208, - "y": 6.097726656744903, - "heading": -0.35942190954654185, - "angularVelocity": 1.156977663667012, - "velocityX": -1.8967449798883769, - "velocityY": 0.583961611670435, - "timestamp": 11.537027182665991 - }, - { - "x": 7.371414140499637, - "y": 6.1349141479804095, - "heading": -0.28853586070822274, - "angularVelocity": 1.251020803153572, - "velocityX": -2.131970252153457, - "velocityY": 0.6562973379828368, - "timestamp": 11.59368974867579 - }, - { - "x": 7.237259574639344, - "y": 6.176204237589326, - "heading": -0.21342323245219458, - "angularVelocity": 1.3256128965822067, - "velocityX": -2.367604845800615, - "velocityY": 0.7287013722918435, - "timestamp": 11.650352314685588 - }, - { - "x": 7.0897287772168305, - "y": 6.221599002117526, - "heading": -0.13565860715800612, - "angularVelocity": 1.372416231215901, - "velocityX": -2.6036730739833454, - "velocityY": 0.8011420541800114, - "timestamp": 11.707014880695386 - }, - { - "x": 6.928804270313556, - "y": 6.271094657477637, - "heading": -0.05771767479301098, - "angularVelocity": 1.3755277576295868, - "velocityX": -2.8400497583439677, - "velocityY": 0.8735159532230397, - "timestamp": 11.763677446705184 - }, - { - "x": 6.754523159333334, - "y": 6.324666148768191, - "heading": 0.015819291394400217, - "angularVelocity": 1.297805083071882, - "velocityX": -3.0757715940730104, - "velocityY": 0.9454476749480591, - "timestamp": 11.820340012714983 - }, - { - "x": 6.56735113931042, - "y": 6.382062750762148, - "heading": 0.07327050352161106, - "angularVelocity": 1.0139182916156146, - "velocityX": -3.3032746873932677, - "velocityY": 1.012954513638379, - "timestamp": 11.877002578724781 - }, - { - "x": 6.368937324018353, - "y": 6.441765360809047, - "heading": 0.09523150855737006, - "angularVelocity": 0.38757519438780325, - "velocityX": -3.5016736668395723, - "velocityY": 1.053651718430401, - "timestamp": 11.93366514473458 - }, - { - "x": 6.164194285631387, - "y": 6.50503273811371, - "heading": 0.09523154205905172, - "angularVelocity": 5.912489325251288e-7, - "velocityX": -3.6133739222393086, - "velocityY": 1.1165639285330458, - "timestamp": 11.990327710744378 - }, - { - "x": 5.956663061240617, - "y": 6.558448687512427, - "heading": 0.09523157576982937, - "angularVelocity": 5.949391283503271e-7, - "velocityX": -3.6625807654896136, - "velocityY": 0.9427026193886323, - "timestamp": 12.046990276754176 - }, - { - "x": 5.7460856437683105, - "y": 6.598193168640137, - "heading": 0.09523161032614703, - "angularVelocity": 6.098615032122225e-7, - "velocityX": -3.716341004321884, - "velocityY": 0.7014239545882616, - "timestamp": 12.103652842763974 - }, - { - "x": 5.5027055023001905, - "y": 6.62552340619542, - "heading": 0.09523164151085345, - "angularVelocity": 4.815615607402516e-7, - "velocityX": -3.7583333072466103, - "velocityY": 0.4220399473818025, - "timestamp": 12.16841030879256 - }, - { - "x": 5.257964269234122, - "y": 6.634609890080181, - "heading": 0.09523167208339899, - "angularVelocity": 4.721084288711453e-7, - "velocityX": -3.7793516033817274, - "velocityY": 0.140315618291052, - "timestamp": 12.233167774821144 - }, - { - "x": 5.013061040594942, - "y": 6.636410996869712, - "heading": 0.09523170256730593, - "angularVelocity": 4.70739650837411e-7, - "velocityX": -3.781853177069622, - "velocityY": 0.02781311406988273, - "timestamp": 12.297925240849729 - }, - { - "x": 4.7681578086493825, - "y": 6.6382116540225224, - "heading": 0.09523173305121174, - "angularVelocity": 4.7073963340734453e-7, - "velocityX": -3.781853228127505, - "velocityY": 0.027806170674068813, - "timestamp": 12.362682706878314 - }, - { - "x": 4.5232545767036525, - "y": 6.640012311152163, - "heading": 0.09523176353511749, - "angularVelocity": 4.707396321471799e-7, - "velocityX": -3.781853228130136, - "velocityY": 0.02780617031625758, - "timestamp": 12.427440172906898 - }, - { - "x": 4.2783513447579224, - "y": 6.641812968281802, - "heading": 0.09523179401902328, - "angularVelocity": 4.707396330675372e-7, - "velocityX": -3.781853228130136, - "velocityY": 0.027806170316239148, - "timestamp": 12.492197638935483 - }, - { - "x": 4.033448112812192, - "y": 6.64361362541144, - "heading": 0.09523182450292901, - "angularVelocity": 4.707396322379774e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.027806170316239207, - "timestamp": 12.556955104964068 - }, - { - "x": 3.788544880866463, - "y": 6.645414282541079, - "heading": 0.09523185498683476, - "angularVelocity": 4.7073963225441474e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.02780617031623923, - "timestamp": 12.621712570992653 - }, - { - "x": 3.5436416489207327, - "y": 6.647214939670717, - "heading": 0.09523188547074045, - "angularVelocity": 4.707396314544476e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.02780617031623925, - "timestamp": 12.686470037021238 - }, - { - "x": 3.2987384169750027, - "y": 6.649015596800356, - "heading": 0.09523191595464614, - "angularVelocity": 4.7073963141347726e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.027806170316239207, - "timestamp": 12.751227503049822 - }, - { - "x": 3.0538351850292726, - "y": 6.650816253929995, - "heading": 0.09523194643855185, - "angularVelocity": 4.7073963175583744e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.027806170316239227, - "timestamp": 12.815984969078407 - }, - { - "x": 2.8089319530835426, - "y": 6.652616911059632, - "heading": 0.09523197692245748, - "angularVelocity": 4.7073963058043316e-7, - "velocityX": -3.7818532281301365, - "velocityY": 0.02780617031621524, - "timestamp": 12.880742435106992 - }, - { - "x": 2.5640287211375905, - "y": 6.654417568159032, - "heading": 0.09523200740636317, - "angularVelocity": 4.707396315238404e-7, - "velocityX": -3.78185322813357, - "velocityY": 0.027806169849281307, - "timestamp": 12.945499901135577 - }, - { - "x": 2.319125484878767, - "y": 6.656217638494738, - "heading": 0.09523203789215427, - "angularVelocity": 4.707687462497284e-7, - "velocityX": -3.7818532947339327, - "velocityY": 0.02779710890650077, - "timestamp": 13.010257367164161 - }, - { - "x": 2.086765993499063, - "y": 6.65613072972364, - "heading": 0.1392662443558495, - "angularVelocity": 0.6799865585268291, - "velocityX": -3.588149840161109, - "velocityY": -0.0013420656555584189, - "timestamp": 13.075014833192746 - }, - { - "x": 1.8694746583337432, - "y": 6.65699037980207, - "heading": 0.2298866992043259, - "angularVelocity": 1.399382347797175, - "velocityX": -3.3554638328406146, - "velocityY": 0.013274918417153406, - "timestamp": 13.139772299221331 - }, - { - "x": 1.6701637818051993, - "y": 6.657989601285878, - "heading": 0.3342097566920158, - "angularVelocity": 1.6109811560823084, - "velocityX": -3.077805367501045, - "velocityY": 0.015430212840099346, - "timestamp": 13.204529765249916 - }, - { - "x": 1.4891465820964496, - "y": 6.659013009703433, - "heading": 0.4396201310941332, - "angularVelocity": 1.6277717592530314, - "velocityX": -2.7953101134137524, - "velocityY": 0.01580371315183694, - "timestamp": 13.2692872312785 - }, - { - "x": 1.326396709734208, - "y": 6.660011981353038, - "heading": 0.5409065147827995, - "angularVelocity": 1.5640881260541826, - "velocityX": -2.5132217540816426, - "velocityY": 0.015426354841678473, - "timestamp": 13.334044697307085 - }, - { - "x": 1.1818610470561817, - "y": 6.660956545539143, - "heading": 0.6351810352227076, - "angularVelocity": 1.4558092868904808, - "velocityX": -2.231953650166405, - "velocityY": 0.014586182011631477, - "timestamp": 13.39880216333567 - }, - { - "x": 1.0554921038308631, - "y": 6.661825694692396, - "heading": 0.7206099929025545, - "angularVelocity": 1.3192140291922123, - "velocityX": -1.9514189015601402, - "velocityY": 0.013421605361615121, - "timestamp": 13.463559629364255 - }, - { - "x": 0.9472506642705061, - "y": 6.66260279420219, - "heading": 0.7959103128896443, - "angularVelocity": 1.1628052270274385, - "velocityX": -1.6714897323588642, - "velocityY": 0.012000153147596032, - "timestamp": 13.52831709539284 - }, - { - "x": 0.8571042421570496, - "y": 6.663273874000479, - "heading": 0.8601164354198797, - "angularVelocity": 0.9914860242044418, - "velocityX": -1.3920622229669246, - "velocityY": 0.010362971861702049, - "timestamp": 13.593074561421425 - }, - { - "x": 0.7850255723133412, - "y": 6.6638271232273425, - "heading": 0.912467741181329, - "angularVelocity": 0.8084211593199297, - "velocityX": -1.1130557488443436, - "velocityY": 0.00854340450286573, - "timestamp": 13.65783202745001 - }, - { - "x": 0.7309916972965728, - "y": 6.664252697034555, - "heading": 0.952354006476902, - "angularVelocity": 0.6159330767817102, - "velocityX": -0.8344037889456237, - "velocityY": 0.0065718106855080505, - "timestamp": 13.722589493478594 - }, - { - "x": 0.6949834963208631, - "y": 6.664542513080523, - "heading": 0.9792885970471746, - "angularVelocity": 0.41593027371366526, - "velocityX": -0.556047096713378, - "velocityY": 0.0044754074509330615, - "timestamp": 13.787346959507179 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.21010218588179536, - "velocityX": -0.2779301721676604, - "velocityY": 0.0022778009813890303, - "timestamp": 13.852104425535764 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -1.2401951851203077e-33, - "velocityX": -4.2513783914380976e-39, - "velocityY": -1.4668246371137104e-32, - "timestamp": 13.916861891564348 - } - ] + "samples": [ + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": -3.9907195357987423e-28, + "velocityX": 1.1657419174076564e-26, + "velocityY": -4.7095973646167984e-27, + "timestamp": 0 + }, + { + "x": 0.6967295535340264, + "y": 6.667501793456135, + "heading": 0.9828931632504835, + "angularVelocity": -0.14737604788210726, + "velocityX": 0.2909483472755575, + "velocityY": 0.0414342035164189, + "timestamp": 0.06786122375505199 + }, + { + "x": 0.7362175895663237, + "y": 6.673125696213235, + "heading": 0.9628974920314577, + "angularVelocity": -0.2946553291051897, + "velocityX": 0.5818939572152577, + "velocityY": 0.08287358296691186, + "timestamp": 0.13572244751010398 + }, + { + "x": 0.7954495269531124, + "y": 6.681562379143333, + "heading": 0.9329256679355464, + "angularVelocity": -0.4416634778661797, + "velocityX": 0.8728392167018533, + "velocityY": 0.12432258752878259, + "timestamp": 0.20358367126515597 + }, + { + "x": 0.874425436057604, + "y": 6.692812849772419, + "heading": 0.8930052032172396, + "angularVelocity": -0.5882662072585287, + "velocityX": 1.163785513057627, + "velocityY": 0.16578643894920225, + "timestamp": 0.27144489502020797 + }, + { + "x": 0.9731453894647497, + "y": 6.7068785148517795, + "heading": 0.8431688201765567, + "angularVelocity": -0.7343867422811217, + "velocityX": 1.4547328790220406, + "velocityY": 0.2072710201945652, + "timestamp": 0.33930611877525996 + }, + { + "x": 1.0916093532830902, + "y": 6.723761203633637, + "heading": 0.7834495220073406, + "angularVelocity": -0.8800209436949645, + "velocityX": 1.745679745563404, + "velocityY": 0.24878255721994524, + "timestamp": 0.40716734253031195 + }, + { + "x": 1.229817076727268, + "y": 6.743463169981193, + "heading": 0.7138752049646239, + "angularVelocity": -1.0252440671251137, + "velocityX": 2.0366229165428056, + "velocityY": 0.29032730707408716, + "timestamp": 0.47502856628536394 + }, + { + "x": 1.387767999306249, + "y": 6.765987144870166, + "heading": 0.6344639927151499, + "angularVelocity": -1.1702001210015336, + "velocityX": 2.327557828151045, + "velocityY": 0.33191230046593584, + "timestamp": 0.5428897900404159 + }, + { + "x": 1.5654611542200556, + "y": 6.791337011842962, + "heading": 0.5452227484307262, + "angularVelocity": -1.3150550394809215, + "velocityX": 2.618478493037463, + "velocityY": 0.37355452156738955, + "timestamp": 0.6107510137954679 + }, + { + "x": 1.7628875214933553, + "y": 6.819592756978724, + "heading": 0.4462737596207199, + "angularVelocity": -1.4581079346161945, + "velocityX": 2.909266239965237, + "velocityY": 0.4163754140030483, + "timestamp": 0.6786122375505199 + }, + { + "x": 1.9405727782098796, + "y": 6.845027020300295, + "heading": 0.3571888127510162, + "angularVelocity": -1.3127518476716487, + "velocityX": 2.618362105550687, + "velocityY": 0.37479818244033536, + "timestamp": 0.7464734613055719 + }, + { + "x": 2.098517679561726, + "y": 6.867636881126044, + "heading": 0.2779750281361537, + "angularVelocity": -1.167290836086129, + "velocityX": 2.32746909961358, + "velocityY": 0.33317791184197254, + "timestamp": 0.8143346850606239 + }, + { + "x": 2.23672263265918, + "y": 6.887421047973718, + "heading": 0.20863780174038785, + "angularVelocity": -1.0217503098093481, + "velocityX": 2.036582092835676, + "velocityY": 0.2915386100180838, + "timestamp": 0.8821959088156759 + }, + { + "x": 2.355187828517663, + "y": 6.904378696630012, + "heading": 0.14917899743785948, + "angularVelocity": -0.87618231756544, + "velocityX": 1.7456979008526012, + "velocityY": 0.24988716262327937, + "timestamp": 0.9500571325707279 + }, + { + "x": 2.4539133200816936, + "y": 6.918509209536525, + "heading": 0.09959774951013609, + "angularVelocity": -0.7306270825101687, + "velocityX": 1.4548144890576191, + "velocityY": 0.20822661491514247, + "timestamp": 1.0179183563257799 + }, + { + "x": 2.5328990844951553, + "y": 6.92981209262676, + "heading": 0.05989172741762143, + "angularVelocity": -0.5851061901835907, + "velocityX": 1.1639307404559098, + "velocityY": 0.16655878666486323, + "timestamp": 1.0857795800808319 + }, + { + "x": 2.5921450685166625, + "y": 6.938286954893026, + "heading": 0.030058218738196592, + "angularVelocity": -0.43962526798971635, + "velocityX": 0.8730462072914873, + "velocityY": 0.12488519654252121, + "timestamp": 1.1536408038358839 + }, + { + "x": 2.631651217872353, + "y": 6.943933511848997, + "heading": 0.010094853212766689, + "angularVelocity": -0.2941792738293157, + "velocityX": 0.5821608743498282, + "velocityY": 0.08320741424223291, + "timestamp": 1.2215020275909358 + }, + { + "x": 2.6514174938201904, + "y": 6.946751594543457, + "heading": 1.1828263155272929e-27, + "angularVelocity": -0.14875731167484535, + "velocityX": 0.2912749705072336, + "velocityY": 0.04152714228425266, + "timestamp": 1.2893632513459878 + }, + { + "x": 2.6491463328077915, + "y": 6.946410455927006, + "heading": 0.0008990727501414491, + "angularVelocity": 0.011985225180580105, + "velocityX": -0.03027605513644419, + "velocityY": -0.004547599885899398, + "timestamp": 1.364378341725619 + }, + { + "x": 2.6227540855884963, + "y": 6.942613200487609, + "heading": 0.013859521250247003, + "angularVelocity": 0.17277121755790997, + "velocityX": -0.3518258404506491, + "velocityY": -0.05061988754768401, + "timestamp": 1.4393934321052502 + }, + { + "x": 2.572240863223279, + "y": 6.93536015645873, + "heading": 0.03888546284671504, + "angularVelocity": 0.33361209684369453, + "velocityX": -0.6733741452497535, + "velocityY": -0.09668779964370192, + "timestamp": 1.5144085224848813 + }, + { + "x": 2.497606760935717, + "y": 6.9246517669282275, + "heading": 0.07598096923656177, + "angularVelocity": 0.49450725450194494, + "velocityX": -0.9949211806565718, + "velocityY": -0.1427498050900185, + "timestamp": 1.5894236128645125 + }, + { + "x": 2.3988518004865456, + "y": 6.910488565027239, + "heading": 0.12514851964427576, + "angularVelocity": 0.6554354618369476, + "velocityX": -1.3164679259786103, + "velocityY": -0.1888047035511428, + "timestamp": 1.6644387032441437 + }, + { + "x": 2.275975845725643, + "y": 6.892871178204052, + "heading": 0.18638682893267958, + "angularVelocity": 0.8163465374565724, + "velocityX": -1.6380164862704298, + "velocityY": -0.23485123771803745, + "timestamp": 1.7394537936237748 + }, + { + "x": 2.128978500524308, + "y": 6.871800394582041, + "heading": 0.2596886161467297, + "angularVelocity": 0.9771605532045546, + "velocityX": -1.959570327215777, + "velocityY": -0.28088726568717615, + "timestamp": 1.814468884003406 + }, + { + "x": 1.957859021947552, + "y": 6.847277323064731, + "heading": 0.34503981520215615, + "angularVelocity": 1.1377870588902428, + "velocityX": -2.2811340719682667, + "velocityY": -0.32690851125028303, + "timestamp": 1.8894839743830372 + }, + { + "x": 1.76261631729721, + "y": 6.81930365549537, + "heading": 0.44242285893441274, + "angularVelocity": 1.298179382833869, + "velocityX": -2.602712383098801, + "velocityY": -0.3729072034412448, + "timestamp": 1.9644990647626683 + }, + { + "x": 1.5454887932969013, + "y": 6.788364935052348, + "heading": 0.5521114771232074, + "angularVelocity": 1.4622207029770928, + "velocityX": -2.8944512750899136, + "velocityY": -0.41243328890826375, + "timestamp": 2.0395141551422995 + }, + { + "x": 1.3524867398675213, + "y": 6.760871330824376, + "heading": 0.6497679916624863, + "angularVelocity": 1.3018249267589428, + "velocityX": -2.5728430433483203, + "velocityY": -0.3665076465126455, + "timestamp": 2.114529245521931 + }, + { + "x": 1.1836105739801852, + "y": 6.7368199629542165, + "heading": 0.7353592835672482, + "angularVelocity": 1.1409876529056668, + "velocityX": -2.2512292531102633, + "velocityY": -0.3206203944891842, + "timestamp": 2.1895443359015623 + }, + { + "x": 1.0388602648787244, + "y": 6.71620826316138, + "heading": 0.8088372426617098, + "angularVelocity": 0.9795090390827935, + "velocityX": -1.9296158728719546, + "velocityY": -0.2747673793169661, + "timestamp": 2.2645594262811937 + }, + { + "x": 0.9182354949187841, + "y": 6.699034148059042, + "heading": 0.8701477847623594, + "angularVelocity": 0.8173094478774001, + "velocityX": -1.6080067270397285, + "velocityY": -0.2289421370476856, + "timestamp": 2.339574516660825 + }, + { + "x": 0.821735849417145, + "y": 6.685296075575293, + "heading": 0.9192400732020367, + "angularVelocity": 0.6544321707970268, + "velocityX": -1.2864031092048316, + "velocityY": -0.18313745160106204, + "timestamp": 2.4145896070404564 + }, + { + "x": 0.7493609915406636, + "y": 6.674993018433249, + "heading": 0.9560742229389826, + "angularVelocity": 0.49102320013930684, + "velocityX": -0.9648039815750613, + "velocityY": -0.13734646042419815, + "timestamp": 2.489604697420088 + }, + { + "x": 0.7011107973306167, + "y": 6.6681243981888185, + "heading": 0.9806268225252659, + "angularVelocity": 0.32730213963656085, + "velocityX": -0.643206506395788, + "velocityY": -0.09156318028372816, + "timestamp": 2.564619787799719 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.16353322544301652, + "velocityX": -0.3216066870029412, + "velocityY": -0.04578252817189861, + "timestamp": 2.6396348781793506 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": -5.340812593461075e-28, + "velocityX": 3.892811349542242e-27, + "velocityY": 2.3219495328630078e-26, + "timestamp": 2.714649968558982 + }, + { + "x": 0.6985020844125115, + "y": 6.66756932185273, + "heading": 0.9798246445664861, + "angularVelocity": -0.1842987222375363, + "velocityX": 0.30341235873432515, + "velocityY": 0.04060189659731702, + "timestamp": 2.7855654772873355 + }, + { + "x": 0.7415443472286969, + "y": 6.673333307824001, + "heading": 0.9539749006718472, + "angularVelocity": -0.36451467892105427, + "velocityX": 0.6069513367105871, + "velocityY": 0.08127962521359514, + "timestamp": 2.856480986015689 + }, + { + "x": 0.806123192395711, + "y": 6.681988659680362, + "heading": 0.9157093267024382, + "angularVelocity": -0.5395938724205993, + "velocityX": 0.9106448832566048, + "velocityY": 0.12205160777335122, + "timestamp": 2.9273964947440425 + }, + { + "x": 0.8922518577310767, + "y": 6.693543414407282, + "heading": 0.8654790262942293, + "angularVelocity": -0.7083119237093733, + "velocityX": 1.2145251000777222, + "velocityY": 0.16293692217848874, + "timestamp": 2.998312003472396 + }, + { + "x": 0.9999464064120345, + "y": 6.708007002745886, + "heading": 0.8038456054132372, + "angularVelocity": -0.8691106076258047, + "velocityX": 1.518631828384521, + "velocityY": 0.2039552221786628, + "timestamp": 3.0692275122007495 + }, + { + "x": 1.1292266934715014, + "y": 6.725390389590618, + "heading": 0.7315277617443964, + "angularVelocity": -1.0197747286261354, + "velocityX": 1.823018538225309, + "velocityY": 0.2451281413113817, + "timestamp": 3.140143020929103 + }, + { + "x": 1.2801178802680748, + "y": 6.745706577222529, + "heading": 0.6494907896351716, + "angularVelocity": -1.1568269561947726, + "velocityX": 2.1277600556258043, + "velocityY": 0.2864844093515925, + "timestamp": 3.2110585296574565 + }, + { + "x": 1.4526523788182084, + "y": 6.768971930813486, + "heading": 0.5591180385402752, + "angularVelocity": -1.274372174936732, + "velocityX": 2.43295862419938, + "velocityY": 0.3280714473906901, + "timestamp": 3.28197403838581 + }, + { + "x": 1.646870967643948, + "y": 6.795208954217293, + "heading": 0.462559660855429, + "angularVelocity": -1.361597475873995, + "velocityX": 2.7387322224494954, + "velocityY": 0.36997581874945734, + "timestamp": 3.3528895471141635 + }, + { + "x": 1.862815108078828, + "y": 6.824451101510304, + "heading": 0.36357825171401237, + "angularVelocity": -1.3957653398577683, + "velocityX": 3.0450904788974817, + "velocityY": 0.41235193566790274, + "timestamp": 3.423805055842517 + }, + { + "x": 2.1004346793080155, + "y": 6.856747845634531, + "heading": 0.2705944336755815, + "angularVelocity": -1.3111915814439359, + "velocityX": 3.3507419673093723, + "velocityY": 0.4554256847813233, + "timestamp": 3.4947205645708705 + }, + { + "x": 2.3559638608729165, + "y": 6.892112274947116, + "heading": 0.23346195065053052, + "angularVelocity": -0.5236158308796662, + "velocityX": 3.6032905375285806, + "velocityY": 0.4986839965860088, + "timestamp": 3.565636073299224 + }, + { + "x": 2.6217812158343787, + "y": 6.927777331410734, + "heading": 0.2334619248619747, + "angularVelocity": -3.636518485299231e-7, + "velocityX": 3.748367031810964, + "velocityY": 0.5029232265714462, + "timestamp": 3.6365515820275776 + }, + { + "x": 2.8875985808561246, + "y": 6.963442312894318, + "heading": 0.23346189907376708, + "angularVelocity": -3.6364693779214443e-7, + "velocityX": 3.74836717367392, + "velocityY": 0.5029221692563763, + "timestamp": 3.707467090755931 + }, + { + "x": 3.153415945878299, + "y": 6.999107294374709, + "heading": 0.23346187328555953, + "angularVelocity": -3.636469371719515e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113627, + "timestamp": 3.7783825994842846 + }, + { + "x": 3.419233310900473, + "y": 7.034772275855099, + "heading": 0.23346184749735194, + "angularVelocity": -3.6364693781306164e-7, + "velocityX": 3.7483671736799598, + "velocityY": 0.502922169211361, + "timestamp": 3.849298108212638 + }, + { + "x": 3.6850506759226476, + "y": 7.070437257335489, + "heading": 0.2334618217091443, + "angularVelocity": -3.6364693810863675e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113609, + "timestamp": 3.9202136169409916 + }, + { + "x": 3.950868040944822, + "y": 7.10610223881588, + "heading": 0.23346179592093674, + "angularVelocity": -3.636469373806279e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.502922169211361, + "timestamp": 3.991129125669345 + }, + { + "x": 4.216685405966996, + "y": 7.14176722029627, + "heading": 0.23346177013272917, + "angularVelocity": -3.6364693732214093e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113611, + "timestamp": 4.062044634397698 + }, + { + "x": 4.4825027709891705, + "y": 7.17743220177666, + "heading": 0.23346174434452158, + "angularVelocity": -3.636469378203165e-7, + "velocityX": 3.7483671736799606, + "velocityY": 0.5029221692113612, + "timestamp": 4.132960143126051 + }, + { + "x": 4.748320136011345, + "y": 7.213097183257051, + "heading": 0.233461718556314, + "angularVelocity": -3.636469375156407e-7, + "velocityX": 3.7483671736799598, + "velocityY": 0.502922169211361, + "timestamp": 4.203875651854404 + }, + { + "x": 5.014137501033519, + "y": 7.248762164737441, + "heading": 0.23346169276810638, + "angularVelocity": -3.636469383573516e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113611, + "timestamp": 4.274791160582757 + }, + { + "x": 5.279954866055694, + "y": 7.284427146217831, + "heading": 0.23346166697989873, + "angularVelocity": -3.6364693850408854e-7, + "velocityX": 3.74836717367996, + "velocityY": 0.5029221692113611, + "timestamp": 4.34570666931111 + }, + { + "x": 5.545772231077868, + "y": 7.32009212769822, + "heading": 0.23346164119169116, + "angularVelocity": -3.6364693775026823e-7, + "velocityX": 3.748367173679964, + "velocityY": 0.502922169211334, + "timestamp": 4.416622178039463 + }, + { + "x": 5.811589596106133, + "y": 7.3557571091332195, + "heading": 0.23346161540348362, + "angularVelocity": -3.63646937002983e-7, + "velocityX": 3.748367173765839, + "velocityY": 0.5029221685712877, + "timestamp": 4.4875376867678165 + }, + { + "x": 6.07740710417715, + "y": 7.39142102442851, + "heading": 0.2334615896149771, + "angularVelocity": -3.636511531820921e-7, + "velocityX": 3.748369190852852, + "velocityY": 0.5029071346283975, + "timestamp": 4.55845319549617 + }, + { + "x": 6.33844426596311, + "y": 7.413466447299319, + "heading": 0.21125647994267174, + "angularVelocity": -0.3131206427265934, + "velocityX": 3.6809601519729838, + "velocityY": 0.3108688531764646, + "timestamp": 4.629368704224523 + }, + { + "x": 6.578740605741143, + "y": 7.432100566048415, + "heading": 0.16288337141874695, + "angularVelocity": -0.682123126398504, + "velocityX": 3.3884878510637786, + "velocityY": 0.2627650718896376, + "timestamp": 4.700284212952876 + }, + { + "x": 6.797330437873669, + "y": 7.447554222867888, + "heading": 0.11319689086108646, + "angularVelocity": -0.7006433634705764, + "velocityX": 3.0823981390283666, + "velocityY": 0.2179164627961563, + "timestamp": 4.771199721681229 + }, + { + "x": 6.9942244466742505, + "y": 7.4599283003931935, + "heading": 0.06789887850949264, + "angularVelocity": -0.6387603101757464, + "velocityX": 2.7764590895737267, + "velocityY": 0.1744904287820278, + "timestamp": 4.842115230409582 + }, + { + "x": 7.169459125508247, + "y": 7.4692727668351795, + "heading": 0.029649916545462486, + "angularVelocity": -0.5393596217513631, + "velocityX": 2.4710346435678088, + "velocityY": 0.13176901089126727, + "timestamp": 4.913030739137935 + }, + { + "x": 7.323064804077148, + "y": 7.475617408752441, + "heading": 4.538527756690787e-28, + "angularVelocity": -0.41810200726386243, + "velocityX": 2.1660378854123263, + "velocityY": 0.08946762183665603, + "timestamp": 4.983946247866288 + }, + { + "x": 7.44267075746469, + "y": 7.47890384849819, + "heading": -0.01877472577087381, + "angularVelocity": -0.2974384142895345, + "velocityX": 1.8948561778925237, + "velocityY": 0.05206539038508263, + "timestamp": 5.047067636593831 + }, + { + "x": 7.545114017207691, + "y": 7.480040189019033, + "heading": -0.030609170040043498, + "angularVelocity": -0.1874870706703203, + "velocityX": 1.6229563672178904, + "velocityY": 0.018002463883490395, + "timestamp": 5.110189025321374 + }, + { + "x": 7.630363412345014, + "y": 7.479184975308775, + "heading": -0.03600750296274941, + "angularVelocity": -0.08552303793579748, + "velocityX": 1.3505627308881507, + "velocityY": -0.013548715063116237, + "timestamp": 5.173310414048918 + }, + { + "x": 7.698396363563398, + "y": 7.4764617545204155, + "heading": -0.035360550234153146, + "angularVelocity": 0.010249342443791389, + "velocityX": 1.0778113820030217, + "velocityY": -0.04314259941449841, + "timestamp": 5.236431802776461 + }, + { + "x": 7.749195849524956, + "y": 7.471969501540552, + "heading": -0.0289805004906965, + "angularVelocity": 0.10107587732258928, + "velocityX": 0.804790372734491, + "velocityY": -0.07116847506720048, + "timestamp": 5.299553191504004 + }, + { + "x": 7.782748628163455, + "y": 7.46578928720822, + "heading": -0.01712270276724403, + "angularVelocity": 0.1878570475474702, + "velocityX": 0.5315595761577129, + "velocityY": -0.09790998672428226, + "timestamp": 5.362674580231547 + }, + { + "x": 7.799044132232666, + "y": 7.457988739013672, + "heading": -3.115976344858509e-29, + "angularVelocity": 0.2712662555818014, + "velocityX": 0.2581613680831416, + "velocityY": -0.12358011051084936, + "timestamp": 5.425795968959091 + }, + { + "x": 7.789461840253682, + "y": 7.444564299731131, + "heading": 0.032498030114414544, + "angularVelocity": 0.38031209691715145, + "velocityX": -0.11213792168232267, + "velocityY": -0.15710111152907683, + "timestamp": 5.511246919317617 + }, + { + "x": 7.7482316276690275, + "y": 7.4282846905716475, + "heading": 0.07405408050157687, + "angularVelocity": 0.48631466604883555, + "velocityX": -0.48250151006705927, + "velocityY": -0.19051407961151762, + "timestamp": 5.5966978696761425 + }, + { + "x": 7.675346865619377, + "y": 7.409161329569071, + "heading": 0.12434733262468406, + "angularVelocity": 0.5885628177579297, + "velocityX": -0.8529426734734683, + "velocityY": -0.2237934267827392, + "timestamp": 5.682148820034668 + }, + { + "x": 7.570799431412373, + "y": 7.387208662514977, + "heading": 0.18297244388953984, + "angularVelocity": 0.6860673991205785, + "velocityX": -1.2234788936618521, + "velocityY": -0.2569037203446768, + "timestamp": 5.767599770393194 + }, + { + "x": 7.43457917720258, + "y": 7.362445546099649, + "heading": 0.24939990020901256, + "angularVelocity": 0.7773752783411216, + "velocityX": -1.5941338702291188, + "velocityY": -0.28979334122592726, + "timestamp": 5.85305072075172 + }, + { + "x": 7.266673120798299, + "y": 7.334897702364834, + "heading": 0.32290449051554804, + "angularVelocity": 0.8601962880240946, + "velocityX": -1.9649407724524794, + "velocityY": -0.32238194682719395, + "timestamp": 5.938501671110246 + }, + { + "x": 7.067064218419677, + "y": 7.304602562446356, + "heading": 0.4024227559343364, + "angularVelocity": 0.9305720426180611, + "velocityX": -2.3359471315546996, + "velocityY": -0.3545325100700251, + "timestamp": 6.023952621468772 + }, + { + "x": 6.835729835895948, + "y": 7.271620394698958, + "heading": 0.4862275157211307, + "angularVelocity": 0.9807352573046286, + "velocityX": -2.707218369756224, + "velocityY": -0.3859777756597744, + "timestamp": 6.109403571827298 + }, + { + "x": 6.572643491372071, + "y": 7.236066664600634, + "heading": 0.5710108191953822, + "angularVelocity": 0.9921867822244987, + "velocityX": -3.0787995150439604, + "velocityY": -0.41607179263836863, + "timestamp": 6.194854522185824 + }, + { + "x": 6.277837620342988, + "y": 7.198257710886909, + "heading": 0.6480492253062652, + "angularVelocity": 0.9015511915040533, + "velocityX": -3.4500010800601615, + "velocityY": -0.44246381760636405, + "timestamp": 6.28030547254435 + }, + { + "x": 5.957362242994923, + "y": 7.156598776275489, + "heading": 0.6480492977916429, + "angularVelocity": 8.482688295948482e-7, + "velocityX": -3.7504015578931313, + "velocityY": -0.48751868102848633, + "timestamp": 6.365756422902876 + }, + { + "x": 5.635584364863326, + "y": 7.126616389470958, + "heading": 0.6480493126613769, + "angularVelocity": 1.740148467326507e-7, + "velocityX": -3.7656442296020605, + "velocityY": -0.3508724792262005, + "timestamp": 6.4512073732614015 + }, + { + "x": 5.3138064561089, + "y": 7.096634331319495, + "heading": 0.6480493275311116, + "angularVelocity": 1.7401485467118797e-7, + "velocityX": -3.7656445879693994, + "velocityY": -0.350868633124234, + "timestamp": 6.536658323619927 + }, + { + "x": 4.99202854735373, + "y": 7.066652273176011, + "heading": 0.6480493424008463, + "angularVelocity": 1.740148544690323e-7, + "velocityX": -3.7656445879780995, + "velocityY": -0.35086863303086346, + "timestamp": 6.622109273978453 + }, + { + "x": 4.67025063859856, + "y": 7.036670215032527, + "heading": 0.6480493572705811, + "angularVelocity": 1.740148551537583e-7, + "velocityX": -3.765644587978099, + "velocityY": -0.35086863303086074, + "timestamp": 6.707560224336979 + }, + { + "x": 4.34847272984339, + "y": 7.006688156889043, + "heading": 0.6480493721403158, + "angularVelocity": 1.7401485461327074e-7, + "velocityX": -3.7656445879781, + "velocityY": -0.350868633030861, + "timestamp": 6.793011174695505 + }, + { + "x": 4.0266948210882205, + "y": 6.9767060987455585, + "heading": 0.6480493870100505, + "angularVelocity": 1.7401485474732458e-7, + "velocityX": -3.7656445879781, + "velocityY": -0.3508686330308609, + "timestamp": 6.878462125054031 + }, + { + "x": 3.7049169123330508, + "y": 6.946724040602074, + "heading": 0.6480494018797853, + "angularVelocity": 1.74014855000899e-7, + "velocityX": -3.7656445879781004, + "velocityY": -0.3508686330308606, + "timestamp": 6.963913075412557 + }, + { + "x": 3.383139003577881, + "y": 6.91674198245859, + "heading": 0.64804941674952, + "angularVelocity": 1.740148546492808e-7, + "velocityX": -3.7656445879781, + "velocityY": -0.35086863303086063, + "timestamp": 7.049364025771083 + }, + { + "x": 3.0613610948227112, + "y": 6.886759924315106, + "heading": 0.6480494316192547, + "angularVelocity": 1.7401485430725364e-7, + "velocityX": -3.7656445879780995, + "velocityY": -0.3508686330308605, + "timestamp": 7.134814976129609 + }, + { + "x": 2.7395831860675424, + "y": 6.856777866171614, + "heading": 0.6480494464889893, + "angularVelocity": 1.7401485477485056e-7, + "velocityX": -3.765644587978091, + "velocityY": -0.3508686330309518, + "timestamp": 7.220265926488135 + }, + { + "x": 2.4178052773422305, + "y": 6.82679580770789, + "heading": 0.6480494613587636, + "angularVelocity": 1.7401531782389627e-7, + "velocityX": -3.7656445876286826, + "velocityY": -0.35086863677850416, + "timestamp": 7.3057168768466605 + }, + { + "x": 2.10180699075498, + "y": 6.797337251058121, + "heading": 0.6651219064311078, + "angularVelocity": 0.19979233701571972, + "velocityX": -3.6980078660496734, + "velocityY": -0.3447422939846707, + "timestamp": 7.391167827205186 + }, + { + "x": 1.8167368724291442, + "y": 6.770773756267678, + "heading": 0.7108427772926394, + "angularVelocity": 0.535053977395227, + "velocityX": -3.336067265838099, + "velocityY": -0.3108624851916846, + "timestamp": 7.476618777563712 + }, + { + "x": 1.5633497036796815, + "y": 6.747170828533709, + "heading": 0.7627235488811058, + "angularVelocity": 0.6071409547909103, + "velocityX": -2.9652937467205107, + "velocityY": -0.2762160939689793, + "timestamp": 7.562069727922238 + }, + { + "x": 1.3416809575894382, + "y": 6.72653103338719, + "heading": 0.8140209718864446, + "angularVelocity": 0.6003142479997046, + "velocityX": -2.5941050995944295, + "velocityY": -0.24153967931217257, + "timestamp": 7.647520678280764 + }, + { + "x": 1.1517177479923977, + "y": 6.708849739663476, + "heading": 0.8615071389710252, + "angularVelocity": 0.5557125682668599, + "velocityX": -2.223067254372391, + "velocityY": -0.20691746141533648, + "timestamp": 7.73297162863929 + }, + { + "x": 0.9934436218878284, + "y": 6.694122461290386, + "heading": 0.9032823337599477, + "angularVelocity": 0.4888792297060059, + "velocityX": -1.8522219523656431, + "velocityY": -0.17234774231649247, + "timestamp": 7.818422578997816 + }, + { + "x": 0.8668441825845944, + "y": 6.682345527807194, + "heading": 0.9380916675290075, + "angularVelocity": 0.4073604052735566, + "velocityX": -1.481545129364404, + "velocityY": -0.1378209772247021, + "timestamp": 7.903873529356342 + }, + { + "x": 0.7719075390509518, + "y": 6.673515992228389, + "heading": 0.9650448126274666, + "angularVelocity": 0.3154224146761601, + "velocityX": -1.1110074625889743, + "velocityY": -0.10332869958448399, + "timestamp": 7.989324479714868 + }, + { + "x": 0.708623954226878, + "y": 6.667631472026271, + "heading": 0.9834806534673777, + "angularVelocity": 0.2157476395822394, + "velocityX": -0.7405837449268271, + "velocityY": -0.06886430376057255, + "timestamp": 8.074775430073394 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.1101641199433363, + "velocityX": -0.37025347823207466, + "velocityY": -0.03442272220184603, + "timestamp": 8.16022638043192 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 2.9535092730949793e-26, + "velocityX": 4.101709673254646e-26, + "velocityY": -1.0818589619170112e-27, + "timestamp": 8.245677330790446 + }, + { + "x": 0.6937129747745413, + "y": 6.664558415884879, + "heading": 0.9790660166396069, + "angularVelocity": -0.2211476741106938, + "velocityX": 0.2675140136822165, + "velocityY": -0.0021046338178637887, + "timestamp": 8.308206885432824 + }, + { + "x": 0.7271833461439815, + "y": 6.664303045508021, + "heading": 0.951758290929694, + "angularVelocity": -0.4367170990756675, + "velocityX": 0.5352728251602833, + "velocityY": -0.004083994813630824, + "timestamp": 8.370736440075202 + }, + { + "x": 0.7774150549719171, + "y": 6.663933729358567, + "heading": 0.9114059799343049, + "angularVelocity": -0.6453318151100473, + "velocityX": 0.80332746835033, + "velocityY": -0.0059062654702356065, + "timestamp": 8.43326599471758 + }, + { + "x": 0.8444295958079634, + "y": 6.663462333290518, + "heading": 0.8585277818184469, + "angularVelocity": -0.8456512831137402, + "velocityX": 1.0717258617835783, + "velocityY": -0.007538772197327858, + "timestamp": 8.495795549359958 + }, + { + "x": 0.9282514561454864, + "y": 6.662902494939932, + "heading": 0.7937301924930702, + "angularVelocity": -1.0362714031144127, + "velocityX": 1.3405158699261557, + "velocityY": -0.00895317988091305, + "timestamp": 8.558325104002336 + }, + { + "x": 1.0289086011587232, + "y": 6.662269038795252, + "heading": 0.717728481418855, + "angularVelocity": -1.2154526209068217, + "velocityX": 1.6097531093723465, + "velocityY": -0.010130507858288727, + "timestamp": 8.620854658644713 + }, + { + "x": 1.1464337388167514, + "y": 6.661577430622322, + "heading": 0.6314012179720037, + "angularVelocity": -1.3805833727839212, + "velocityX": 1.879513428972632, + "velocityY": -0.011060500540669212, + "timestamp": 8.683384213287091 + }, + { + "x": 1.2808663521190058, + "y": 6.66084404129845, + "heading": 0.5359002071968971, + "angularVelocity": -1.5272939543756525, + "velocityX": 2.149905177977148, + "velocityY": -0.011728682989453756, + "timestamp": 8.74591376792947 + }, + { + "x": 1.4322545657568082, + "y": 6.6600882254157945, + "heading": 0.43284519285348916, + "angularVelocity": -1.6481008849783823, + "velocityX": 2.4210665581040525, + "velocityY": -0.012087338331098648, + "timestamp": 8.808443322571847 + }, + { + "x": 1.6006535235186912, + "y": 6.659336953698895, + "heading": 0.32466239343874187, + "angularVelocity": -1.730106667694512, + "velocityX": 2.693109821827408, + "velocityY": -0.012014666043865637, + "timestamp": 8.870972877214225 + }, + { + "x": 1.7861082134397477, + "y": 6.658632455209532, + "heading": 0.2152749507913156, + "angularVelocity": -1.749371849408482, + "velocityX": 2.9658725538941773, + "velocityY": -0.011266648121698327, + "timestamp": 8.933502431856603 + }, + { + "x": 1.988547032824456, + "y": 6.658050746326577, + "heading": 0.1122523271254691, + "angularVelocity": -1.6475828790890588, + "velocityX": 3.237490184321712, + "velocityY": -0.009302943004826474, + "timestamp": 8.99603198649898 + }, + { + "x": 2.206257710668096, + "y": 6.657808132322276, + "heading": 0.04234892140521699, + "angularVelocity": -1.1179258531433098, + "velocityX": 3.481724427573166, + "velocityY": -0.003879989321667614, + "timestamp": 9.058561541141358 + }, + { + "x": 2.4373706508795343, + "y": 6.657710121609846, + "heading": 0.021561234757217183, + "angularVelocity": -0.3324457813091681, + "velocityX": 3.6960592720231067, + "velocityY": -0.0015674302014480283, + "timestamp": 9.121091095783736 + }, + { + "x": 2.6738489737582136, + "y": 6.656073631484609, + "heading": 0.02156119930135256, + "angularVelocity": -5.670257020753551e-7, + "velocityX": 3.7818648194626623, + "velocityY": -0.026171466190618775, + "timestamp": 9.183620650426114 + }, + { + "x": 2.910327295829668, + "y": 6.654437024703101, + "heading": 0.021561163845773425, + "angularVelocity": -5.670211364811453e-7, + "velocityX": 3.781864806553173, + "velocityY": -0.026173331808750182, + "timestamp": 9.246150205068492 + }, + { + "x": 3.146805617901072, + "y": 6.6528004179142695, + "heading": 0.021561128390194195, + "angularVelocity": -5.670211379082889e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.0261733319258545, + "timestamp": 9.30867975971087 + }, + { + "x": 3.3832839399724763, + "y": 6.651163811125438, + "heading": 0.02156109293461502, + "angularVelocity": -5.670211370809572e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861857, + "timestamp": 9.371209314353248 + }, + { + "x": 3.6197622620438805, + "y": 6.649527204336607, + "heading": 0.02156105747903583, + "angularVelocity": -5.670211373321077e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861847, + "timestamp": 9.433738868995626 + }, + { + "x": 3.8562405841152843, + "y": 6.647890597547776, + "heading": 0.021561022023456698, + "angularVelocity": -5.67021136433779e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861833, + "timestamp": 9.496268423638003 + }, + { + "x": 4.092718906186688, + "y": 6.646253990758946, + "heading": 0.021560986567877485, + "angularVelocity": -5.670211376937565e-7, + "velocityX": 3.781864806552362, + "velocityY": -0.026173331925861826, + "timestamp": 9.558797978280381 + }, + { + "x": 4.329197228258092, + "y": 6.644617383970115, + "heading": 0.02156095111229825, + "angularVelocity": -5.670211380713392e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.02617333192586182, + "timestamp": 9.62132753292276 + }, + { + "x": 4.565675550329496, + "y": 6.642980777181283, + "heading": 0.02156091565671906, + "angularVelocity": -5.670211373267648e-7, + "velocityX": 3.7818648065523623, + "velocityY": -0.026173331925861805, + "timestamp": 9.683857087565137 + }, + { + "x": 4.8021538724009005, + "y": 6.641344170392449, + "heading": 0.02156088020113987, + "angularVelocity": -5.670211373593148e-7, + "velocityX": 3.781864806552362, + "velocityY": -0.026173331925919696, + "timestamp": 9.746386642207515 + }, + { + "x": 5.038632194471905, + "y": 6.63970756354592, + "heading": 0.021560844745560682, + "angularVelocity": -5.670211372802694e-7, + "velocityX": 3.7818648065459763, + "velocityY": -0.026173332848588252, + "timestamp": 9.808916196849893 + }, + { + "x": 5.275110510179994, + "y": 6.6380700375610795, + "heading": 0.021560809289976154, + "angularVelocity": -5.670212226566905e-7, + "velocityX": 3.7818647047874645, + "velocityY": -0.026188032110664707, + "timestamp": 9.87144575149227 + }, + { + "x": 5.511317622497512, + "y": 6.6266299618393605, + "heading": 0.021560773670877074, + "angularVelocity": -5.696362189823434e-7, + "velocityX": 3.7775275014901912, + "velocityY": -0.18295469697725122, + "timestamp": 9.933975306134649 + }, + { + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0.02156073730438198, + "angularVelocity": -5.815889030387333e-7, + "velocityX": 3.7545129277426152, + "velocityY": -0.454773640430677, + "timestamp": 9.996504860777026 + }, + { + "x": 5.947788995289173, + "y": 6.560838779777381, + "heading": 0.021560704158269762, + "angularVelocity": -6.111013256538172e-7, + "velocityX": 3.7187222649013885, + "velocityY": -0.6886876024038036, + "timestamp": 10.050744822019793 + }, + { + "x": 6.146761514775588, + "y": 6.510943599042024, + "heading": 0.02156067179220178, + "angularVelocity": -5.967199688048595e-7, + "velocityX": 3.668375030650514, + "velocityY": -0.9198970573012855, + "timestamp": 10.104984783262559 + }, + { + "x": 6.342740585471881, + "y": 6.450348248758284, + "heading": 0.02156063967405059, + "angularVelocity": -5.921492283023906e-7, + "velocityX": 3.6131860385949297, + "velocityY": -1.1171717105867658, + "timestamp": 10.159224744505325 + }, + { + "x": 6.534357375004527, + "y": 6.392349058669825, + "heading": 0.005697476736664897, + "angularVelocity": -0.29246265251528625, + "velocityX": 3.532760443448913, + "velocityY": -1.0693073660002697, + "timestamp": 10.213464705748091 + }, + { + "x": 6.715860806517651, + "y": 6.336732651063718, + "heading": -0.04432151851696129, + "angularVelocity": -0.9221797749772022, + "velocityX": 3.346304594517604, + "velocityY": -1.025376979109191, + "timestamp": 10.267704666990857 + }, + { + "x": 6.885664150088088, + "y": 6.2845630937364065, + "heading": -0.11100118904659197, + "angularVelocity": -1.2293458365721763, + "velocityX": 3.1305948544180144, + "velocityY": -0.9618288090917363, + "timestamp": 10.321944628233624 + }, + { + "x": 7.043230371259609, + "y": 6.2361124460413855, + "heading": -0.1820163474700135, + "angularVelocity": -1.3092774551510717, + "velocityX": 2.9049840295108242, + "velocityY": -0.8932647919523048, + "timestamp": 10.37618458947639 + }, + { + "x": 7.188521112298814, + "y": 6.191411290458308, + "heading": -0.25302240055528713, + "angularVelocity": -1.3091095837525815, + "velocityX": 2.678666018747991, + "velocityY": -0.8241369381332195, + "timestamp": 10.430424550719156 + }, + { + "x": 7.321552039325899, + "y": 6.150465396300966, + "heading": -0.32169330843547617, + "angularVelocity": -1.2660574658752652, + "velocityX": 2.4526368378411387, + "velocityY": -0.7549027178334027, + "timestamp": 10.484664511961922 + }, + { + "x": 7.442345977289038, + "y": 6.1132739640793945, + "heading": -0.38654982839023333, + "angularVelocity": -1.195733154462882, + "velocityX": 2.2270284711762884, + "velocityY": -0.685683237403341, + "timestamp": 10.538904473204688 + }, + { + "x": 7.550924557109546, + "y": 6.079834332011057, + "heading": -0.44656245543798795, + "angularVelocity": -1.106428280417667, + "velocityX": 2.0018189049681507, + "velocityY": -0.6165128311701689, + "timestamp": 10.593144434447455 + }, + { + "x": 7.647306630963791, + "y": 6.050143543568203, + "heading": -0.5009752943623942, + "angularVelocity": -1.003187275169067, + "velocityX": 1.776956908631632, + "velocityY": -0.5473969332309115, + "timestamp": 10.64738439569022 + }, + { + "x": 7.7315082311756465, + "y": 6.024198847629992, + "heading": -0.5492132065198027, + "angularVelocity": -0.8893426737807988, + "velocityX": 1.552390493698683, + "velocityY": -0.478331756582337, + "timestamp": 10.701624356932987 + }, + { + "x": 7.803542913553516, + "y": 6.001997812730986, + "heading": -0.5908266122726411, + "angularVelocity": -0.7672093563375907, + "velocityX": 1.3280740016656365, + "velocityY": -0.4093114078684577, + "timestamp": 10.755864318175753 + }, + { + "x": 7.863422174316105, + "y": 5.983538319203457, + "heading": -0.6254559253112775, + "angularVelocity": -0.6384464930504523, + "velocityX": 1.103969460718877, + "velocityY": -0.34033013860220046, + "timestamp": 10.81010427941852 + }, + { + "x": 7.911155845783931, + "y": 5.9688185305906725, + "heading": -0.6528074437520258, + "angularVelocity": -0.5042687681565456, + "velocityX": 0.8800461942474525, + "velocityY": -0.2713827273382881, + "timestamp": 10.864344240661286 + }, + { + "x": 7.9467524419763915, + "y": 5.957836876155619, + "heading": -0.6726364655817074, + "angularVelocity": -0.3655795722443686, + "velocityX": 0.6562798972723887, + "velocityY": -0.2024642751107768, + "timestamp": 10.918584201904052 + }, + { + "x": 7.970219449419739, + "y": 5.950592046890535, + "heading": -0.6847351444633458, + "angularVelocity": -0.22305839835481103, + "velocityX": 0.4326516263224211, + "velocityY": -0.13356995652444023, + "timestamp": 10.972824163146818 + }, + { + "x": 7.981563568115234, + "y": 5.947082996368408, + "heading": -0.6889234822214203, + "angularVelocity": -0.07721867166033586, + "velocityX": 0.2091468805577016, + "velocityY": -0.06469493048531134, + "timestamp": 11.027064124389584 + }, + { + "x": 7.980191051455629, + "y": 5.94749330873125, + "heading": -0.6844922597423083, + "angularVelocity": 0.07820370271926669, + "velocityX": -0.024222635024756137, + "velocityY": 0.007241330399680058, + "timestamp": 11.083726690547342 + }, + { + "x": 7.965589050009383, + "y": 5.951981018115051, + "heading": -0.6714197010480987, + "angularVelocity": 0.23070890679065725, + "velocityX": -0.25770102620470225, + "velocityY": 0.0792006025866641, + "timestamp": 11.1403892567051 + }, + { + "x": 7.937750696996029, + "y": 5.960547682646947, + "heading": -0.6498948448329763, + "angularVelocity": 0.37987789249067105, + "velocityX": -0.4913006046328201, + "velocityY": 0.1511873731247131, + "timestamp": 11.197051822862859 + }, + { + "x": 7.896668195757291, + "y": 5.973195099977348, + "heading": -0.6201381095216878, + "angularVelocity": 0.525156859794188, + "velocityX": -0.725037781105049, + "velocityY": 0.22320586920098048, + "timestamp": 11.253714389020617 + }, + { + "x": 7.842332542589409, + "y": 5.989925311820313, + "heading": -0.58241146210736, + "angularVelocity": 0.6658125456106359, + "velocityX": -0.9589338579654723, + "velocityY": 0.2952603981327993, + "timestamp": 11.310376955178375 + }, + { + "x": 7.774733203499769, + "y": 6.010740643840145, + "heading": -0.5370321424515595, + "angularVelocity": 0.8008694757921291, + "velocityX": -1.1930158422658073, + "velocityY": 0.3673559711693639, + "timestamp": 11.367039521336133 + }, + { + "x": 7.693857751929137, + "y": 6.035643786713744, + "heading": -0.4843912545821768, + "angularVelocity": 0.9290240707210785, + "velocityX": -1.4273171346574955, + "velocityY": 0.4394990301756616, + "timestamp": 11.423702087493892 + }, + { + "x": 7.599691484700552, + "y": 6.064637898936317, + "heading": -0.42497921973533753, + "angularVelocity": 1.048523546946787, + "velocityX": -1.6618779136548574, + "velocityY": 0.511697831366272, + "timestamp": 11.48036465365165 + }, + { + "x": 7.492217047600242, + "y": 6.097726664155008, + "heading": -0.3594218947187098, + "angularVelocity": 1.1569776920110768, + "velocityX": -1.8967449656459858, + "velocityY": 0.5839616427989975, + "timestamp": 11.537027219809408 + }, + { + "x": 7.371414143059779, + "y": 6.134914157517514, + "heading": -0.2885358443103568, + "angularVelocity": 1.2510208275953218, + "velocityX": -2.1319702359425117, + "velocityY": 0.6562973738070701, + "timestamp": 11.593689785967166 + }, + { + "x": 7.237259577881299, + "y": 6.176204249541141, + "heading": -0.21342321481120613, + "angularVelocity": 1.3256129150597364, + "velocityX": -2.367604827585309, + "velocityY": 0.7287014130046445, + "timestamp": 11.650352352124925 + }, + { + "x": 7.089728781224228, + "y": 6.221599016786816, + "heading": -0.13565858875957554, + "angularVelocity": 1.3724162409997644, + "velocityX": -2.6036730536757164, + "velocityY": 0.8011421000469415, + "timestamp": 11.707014918282683 + }, + { + "x": 6.928804275182176, + "y": 6.271094675189793, + "heading": -0.05771765635307292, + "angularVelocity": 1.3755277547702696, + "velocityX": -2.8400497357287153, + "velocityY": 0.8735160046435817, + "timestamp": 11.763677484440441 + }, + { + "x": 6.754523165192255, + "y": 6.324666169886209, + "heading": 0.015819308808898584, + "angularVelocity": 1.2978050615856767, + "velocityX": -3.07577156856422, + "velocityY": 0.9454477325870393, + "timestamp": 11.8203400505982 + }, + { + "x": 6.567351146368393, + "y": 6.382062775695167, + "heading": 0.07327051868988925, + "angularVelocity": 1.0139182493259664, + "velocityX": -3.303274657606285, + "velocityY": 1.012954578321738, + "timestamp": 11.877002616755957 + }, + { + "x": 6.3689373323756, + "y": 6.441765390033087, + "heading": 0.0952315218805866, + "angularVelocity": 0.38757516081347765, + "velocityX": -3.501673634765793, + "velocityY": 1.0536517914084174, + "timestamp": 11.933665182913716 + }, + { + "x": 6.164194282224136, + "y": 6.505032727532106, + "heading": 0.09523155538228788, + "angularVelocity": 5.912492769442424e-7, + "velocityX": -3.6133741204276855, + "velocityY": 1.116563223114054, + "timestamp": 11.990327749071474 + }, + { + "x": 5.956663059713831, + "y": 6.558448682183149, + "heading": 0.09523158909306802, + "angularVelocity": 5.949391710813205e-7, + "velocityX": -3.662580722738597, + "velocityY": 0.9427027096218124, + "timestamp": 12.046990315229232 + }, + { + "x": 5.7460856437683105, + "y": 6.598193168640137, + "heading": 0.09523162364938853, + "angularVelocity": 6.098615517108678e-7, + "velocityX": -3.7163409676723376, + "velocityY": 0.701424046809534, + "timestamp": 12.10365288138699 + }, + { + "x": 5.502705503273195, + "y": 6.625523412363324, + "heading": 0.09523165483409754, + "angularVelocity": 4.815615988787072e-7, + "velocityX": -3.758333277705387, + "velocityY": 0.4220400409979927, + "timestamp": 12.168410347665688 + }, + { + "x": 5.257964270714501, + "y": 6.634609902404007, + "heading": 0.09523168540664503, + "angularVelocity": 4.72108457235069e-7, + "velocityX": -3.779351580949706, + "velocityY": 0.1403157128102671, + "timestamp": 12.233167813944386 + }, + { + "x": 5.013061042473027, + "y": 6.636411025383571, + "heading": 0.09523171589055375, + "angularVelocity": 4.7073967636728834e-7, + "velocityX": -3.781853156321443, + "velocityY": 0.027813363972772134, + "timestamp": 12.297925280223083 + }, + { + "x": 4.76815781079403, + "y": 6.6382116808939955, + "heading": 0.09523174637446119, + "angularVelocity": 4.7073965669390476e-7, + "velocityX": -3.7818532094044772, + "velocityY": 0.02780614520456082, + "timestamp": 12.36268274650178 + }, + { + "x": 4.523254579114848, + "y": 6.640012336379186, + "heading": 0.09523177685836867, + "angularVelocity": 4.707396572416609e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814890212, + "timestamp": 12.427440212780478 + }, + { + "x": 4.278351347435667, + "y": 6.641812991864376, + "heading": 0.09523180734227604, + "angularVelocity": 4.7073965569633346e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869288, + "timestamp": 12.492197679059176 + }, + { + "x": 4.033448115756485, + "y": 6.643613647349565, + "heading": 0.0952318378261835, + "angularVelocity": 4.7073965673585443e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869194, + "timestamp": 12.556955145337874 + }, + { + "x": 3.7885448840773033, + "y": 6.645414302834754, + "heading": 0.09523186831009096, + "angularVelocity": 4.7073965722571307e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869288, + "timestamp": 12.621712611616571 + }, + { + "x": 3.5436416523981213, + "y": 6.647214958319943, + "heading": 0.09523189879399836, + "angularVelocity": 4.7073965583098703e-7, + "velocityX": -3.7818532094073416, + "velocityY": 0.027806144814869357, + "timestamp": 12.686470077895269 + }, + { + "x": 3.2987384207189394, + "y": 6.649015613805132, + "heading": 0.0952319292779057, + "angularVelocity": 4.7073965534371566e-7, + "velocityX": -3.7818532094073416, + "velocityY": 0.02780614481486934, + "timestamp": 12.751227544173966 + }, + { + "x": 3.0538351890397575, + "y": 6.650816269290322, + "heading": 0.09523195976181308, + "angularVelocity": 4.7073965558339454e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.027806144814869368, + "timestamp": 12.815985010452664 + }, + { + "x": 2.808931957360575, + "y": 6.65261692477551, + "heading": 0.09523199024572042, + "angularVelocity": 4.7073965499589656e-7, + "velocityX": -3.781853209407342, + "velocityY": 0.02780614481484276, + "timestamp": 12.880742476731362 + }, + { + "x": 2.5640287256811582, + "y": 6.654417580228766, + "heading": 0.09523202072962778, + "angularVelocity": 4.7073965543506366e-7, + "velocityX": -3.7818532094109676, + "velocityY": 0.02780614432174875, + "timestamp": 12.94549994301006 + }, + { + "x": 2.319125489653766, + "y": 6.6562176441406535, + "heading": 0.09523205121544294, + "angularVelocity": 4.7076911613064953e-7, + "velocityX": -3.7818532765534276, + "velocityY": 0.027797009601036878, + "timestamp": 13.010257409288757 + }, + { + "x": 2.0867659976251596, + "y": 6.656130735198093, + "heading": 0.13926625419972302, + "angularVelocity": 0.6799865021705632, + "velocityX": -3.5881498363230637, + "velocityY": -0.0013420682981450127, + "timestamp": 13.075014875567454 + }, + { + "x": 1.8694746618246978, + "y": 6.656990384791893, + "heading": 0.22988670590711813, + "angularVelocity": 1.3993822938870089, + "velocityX": -3.355463829688775, + "velocityY": 0.013274910882104354, + "timestamp": 13.139772341846152 + }, + { + "x": 1.6701637846976716, + "y": 6.657989605635195, + "heading": 0.33420976058736407, + "angularVelocity": 1.610981106506988, + "velocityX": -3.0778053648555033, + "velocityY": 0.01543020288968781, + "timestamp": 13.20452980812485 + }, + { + "x": 1.4891465844372453, + "y": 6.659013013376454, + "heading": 0.43962013302928266, + "angularVelocity": 1.6277717226962183, + "velocityX": -2.7953101111365104, + "velocityY": 0.015803702647255023, + "timestamp": 13.269287274403547 + }, + { + "x": 1.326396711582843, + "y": 6.660011984363377, + "heading": 0.5409065154536696, + "angularVelocity": 1.5640881004898903, + "velocityX": -2.513221751974858, + "velocityY": 0.015426344548804824, + "timestamp": 13.334044740682245 + }, + { + "x": 1.181861048474837, + "y": 6.660956547926137, + "heading": 0.6351810351446563, + "angularVelocity": 1.4558092697026699, + "velocityX": -2.2319536481857507, + "velocityY": 0.014586172329450045, + "timestamp": 13.398802206960942 + }, + { + "x": 1.0554921048807082, + "y": 6.661825696511207, + "heading": 0.7206099924431242, + "angularVelocity": 1.3192140182076537, + "velocityX": -1.9514188997184116, + "velocityY": 0.01342159653574964, + "timestamp": 13.46355967323964 + }, + { + "x": 0.9472506650106102, + "y": 6.66260279551869, + "heading": 0.7959103123026781, + "angularVelocity": 1.1628052205668817, + "velocityX": -1.6714897306861416, + "velocityY": 0.01200014534444738, + "timestamp": 13.528317139518338 + }, + { + "x": 0.8571042426441801, + "y": 6.663273874888148, + "heading": 0.8601164348709504, + "angularVelocity": 0.9914860209623859, + "velocityX": -1.392062221496826, + "velocityY": 0.010362965199572966, + "timestamp": 13.593074605797035 + }, + { + "x": 0.7850255726020201, + "y": 6.66382712376522, + "heading": 0.9124677407650426, + "angularVelocity": 0.8084211582458605, + "velocityX": -1.1130557476099119, + "velocityY": 0.00854339906830906, + "timestamp": 13.657832072075733 + }, + { + "x": 0.7309916974392009, + "y": 6.664252697305851, + "heading": 0.9523540062287302, + "angularVelocity": 0.6159330769988476, + "velocityX": -0.8344037879782468, + "velocityY": 0.006571806543502557, + "timestamp": 13.72258953835443 + }, + { + "x": 0.694983496367867, + "y": 6.664542513171668, + "heading": 0.9792885969521788, + "angularVelocity": 0.4159302744725947, + "velocityX": -0.5560470960424025, + "velocityY": 0.004475404651714408, + "timestamp": 13.787347004633128 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 0.21010218653726323, + "velocityX": -0.2779301718200537, + "velocityY": 0.0022777995651181563, + "timestamp": 13.852104470911826 + }, + { + "x": 0.6769854426383972, + "y": 6.664690017700195, + "heading": 0.9928942822119464, + "angularVelocity": 2.2135835324585282e-26, + "velocityX": 2.644293811404756e-26, + "velocityY": 8.707515786736107e-27, + "timestamp": 13.916861937190523 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLaneSprint.1.traj b/src/main/deploy/choreo/AmpLaneSprint.1.traj deleted file mode 100644 index d2e23d21..00000000 --- a/src/main/deploy/choreo/AmpLaneSprint.1.traj +++ /dev/null @@ -1,238 +0,0 @@ -{ - "samples": [ - { - "x": 1.493931531906128, - "y": 7.39399003982544, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.82118644197349e-37, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.5360008147282442, - "y": 7.39399003982544, - "heading": 3.2333963174943666e-19, - "angularVelocity": 3.292766805519721e-18, - "velocityX": 0.4284174372625368, - "velocityY": 1.8392808793575992e-26, - "timestamp": 0.09819694336189172 - }, - { - "x": 1.6201393797142145, - "y": 7.39399003982544, - "heading": 7.027380416999147e-19, - "angularVelocity": 3.863647922469736e-18, - "velocityX": 0.8568348678215871, - "velocityY": 3.963342552313322e-26, - "timestamp": 0.19639388672378344 - }, - { - "x": 1.746347225986537, - "y": 7.39399003982544, - "heading": 8.361393183209098e-19, - "angularVelocity": 1.3585074996311424e-18, - "velocityX": 1.2852522894444904, - "velocityY": 6.468173032965746e-26, - "timestamp": 0.29459083008567516 - }, - { - "x": 1.9146243523169562, - "y": 7.39399003982544, - "heading": 3.80235401897157e-19, - "angularVelocity": -4.642750516315599e-18, - "velocityX": 1.7136696985593167, - "velocityY": 9.506575448004094e-26, - "timestamp": 0.3927877734475669 - }, - { - "x": 2.12497075686256, - "y": 7.39399003982544, - "heading": -1.0623970199278788e-18, - "angularVelocity": -1.4691215100259113e-17, - "velocityX": 2.1420870889066284, - "velocityY": 1.3343481989110454e-25, - "timestamp": 0.4909847168094586 - }, - { - "x": 2.3773864365383615, - "y": 7.39399003982544, - "heading": -3.9564889545725385e-18, - "angularVelocity": -2.947232200024047e-17, - "velocityX": 2.5705044478376236, - "velocityY": 1.8500903675379378e-25, - "timestamp": 0.5891816601713503 - }, - { - "x": 2.6718713848686027, - "y": 7.39399003982544, - "heading": -8.853646546888228e-18, - "angularVelocity": -4.987077429157769e-17, - "velocityX": 2.9989217408219764, - "velocityY": 2.625793984498987e-25, - "timestamp": 0.6873786035332421 - }, - { - "x": 3.0084255847706767, - "y": 7.39399003982544, - "heading": -1.6413613329692498e-17, - "angularVelocity": -7.698780165331962e-17, - "velocityX": 3.4273388598436147, - "velocityY": 4.161152605292971e-25, - "timestamp": 0.7855755468951338 - }, - { - "x": 3.3798020724552167, - "y": 7.39399003982544, - "heading": -2.58741769005412e-17, - "angularVelocity": -9.634275005875749e-17, - "velocityX": 3.781955679779987, - "velocityY": -5.487818039619291e-25, - "timestamp": 0.8837724902570254 - }, - { - "x": 3.7511785589388484, - "y": 7.39399003982544, - "heading": 5.146033898598045e-18, - "angularVelocity": 3.158979265252523e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.295977316525143e-24, - "timestamp": 0.9819694336189171 - }, - { - "x": 4.122555045422488, - "y": 7.39399003982544, - "heading": 4.0038826563950476e-17, - "angularVelocity": 3.553348146159671e-16, - "velocityX": 3.7819556675504797, - "velocityY": 7.623936784532972e-25, - "timestamp": 1.0801663769808088 - }, - { - "x": 4.493931531906129, - "y": 7.39399003982544, - "heading": 6.570668459860429e-17, - "angularVelocity": 2.613916192895976e-16, - "velocityX": 3.7819556675504797, - "velocityY": 3.4278643163770276e-26, - "timestamp": 1.1783633203427004 - }, - { - "x": 4.865308018389769, - "y": 7.39399003982544, - "heading": 6.663782988380241e-17, - "angularVelocity": 9.482426370100795e-18, - "velocityX": 3.7819556675504797, - "velocityY": 2.693381196266241e-25, - "timestamp": 1.276560263704592 - }, - { - "x": 5.236684504873409, - "y": 7.39399003982544, - "heading": 5.2326006182614025e-17, - "angularVelocity": -1.4574612214194992e-16, - "velocityX": 3.7819556675504797, - "velocityY": 5.124149697811417e-24, - "timestamp": 1.3747572070664837 - }, - { - "x": 5.60806099135704, - "y": 7.39399003982544, - "heading": 2.884064553605338e-17, - "angularVelocity": -2.3916590315859936e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.1014556301472372e-23, - "timestamp": 1.4729541504283754 - }, - { - "x": 5.9794374790415805, - "y": 7.39399003982544, - "heading": 1.6502190572837753e-17, - "angularVelocity": -1.2565009195595738e-16, - "velocityX": 3.7819556797799856, - "velocityY": 6.524035237986936e-24, - "timestamp": 1.571151093790267 - }, - { - "x": 6.315991678943653, - "y": 7.39399003982544, - "heading": 9.967918627860597e-18, - "angularVelocity": -6.65425189408073e-17, - "velocityX": 3.427338859843601, - "velocityY": -3.262675265981889e-25, - "timestamp": 1.6693480371521587 - }, - { - "x": 6.610476627273894, - "y": 7.39399003982544, - "heading": 5.2731830670839714e-18, - "angularVelocity": -4.78093860085579e-17, - "velocityX": 2.9989217408219764, - "velocityY": -2.379324733264266e-25, - "timestamp": 1.7675449805140504 - }, - { - "x": 6.862892306949696, - "y": 7.39399003982544, - "heading": 2.2289001785398693e-18, - "angularVelocity": -3.10018090849907e-17, - "velocityX": 2.5705044478376236, - "velocityY": -1.7314309405461854e-25, - "timestamp": 1.865741923875942 - }, - { - "x": 7.0732387114953, - "y": 7.39399003982544, - "heading": 5.002812433628901e-19, - "angularVelocity": -1.760359206249271e-17, - "velocityX": 2.1420870889066284, - "velocityY": -1.2661949502166272e-25, - "timestamp": 1.9639388672378337 - }, - { - "x": 7.24151583782572, - "y": 7.39399003982544, - "heading": -2.663585350994398e-19, - "angularVelocity": -7.807165345874594e-18, - "velocityX": 1.7136696985593165, - "velocityY": -9.096401200333518e-26, - "timestamp": 2.0621358105997256 - }, - { - "x": 7.367723684098041, - "y": 7.39399003982544, - "heading": -4.029353027704155e-19, - "angularVelocity": -1.390845344762092e-18, - "velocityX": 1.2852522894444902, - "velocityY": -6.226126404979919e-26, - "timestamp": 2.1603327539616175 - }, - { - "x": 7.451862249084011, - "y": 7.39399003982544, - "heading": -2.1995708883691042e-19, - "angularVelocity": 1.8633799906775463e-18, - "velocityX": 0.8568348678215871, - "velocityY": -3.833262935804246e-26, - "timestamp": 2.2585296973235094 - }, - { - "x": 7.493931531906128, - "y": 7.39399003982544, - "heading": 0, - "angularVelocity": 2.2399586452459634e-18, - "velocityX": 0.42841743726253684, - "velocityY": -1.7862050890834192e-26, - "timestamp": 2.3567266406854013 - }, - { - "x": 7.493931531906128, - "y": 7.39399003982544, - "heading": 0, - "angularVelocity": 0, - "velocityX": -2.1810930337522913e-39, - "velocityY": 0, - "timestamp": 2.454923584047293 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLaneSprint.traj b/src/main/deploy/choreo/AmpLaneSprint.traj deleted file mode 100644 index d2e23d21..00000000 --- a/src/main/deploy/choreo/AmpLaneSprint.traj +++ /dev/null @@ -1,238 +0,0 @@ -{ - "samples": [ - { - "x": 1.493931531906128, - "y": 7.39399003982544, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.82118644197349e-37, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.5360008147282442, - "y": 7.39399003982544, - "heading": 3.2333963174943666e-19, - "angularVelocity": 3.292766805519721e-18, - "velocityX": 0.4284174372625368, - "velocityY": 1.8392808793575992e-26, - "timestamp": 0.09819694336189172 - }, - { - "x": 1.6201393797142145, - "y": 7.39399003982544, - "heading": 7.027380416999147e-19, - "angularVelocity": 3.863647922469736e-18, - "velocityX": 0.8568348678215871, - "velocityY": 3.963342552313322e-26, - "timestamp": 0.19639388672378344 - }, - { - "x": 1.746347225986537, - "y": 7.39399003982544, - "heading": 8.361393183209098e-19, - "angularVelocity": 1.3585074996311424e-18, - "velocityX": 1.2852522894444904, - "velocityY": 6.468173032965746e-26, - "timestamp": 0.29459083008567516 - }, - { - "x": 1.9146243523169562, - "y": 7.39399003982544, - "heading": 3.80235401897157e-19, - "angularVelocity": -4.642750516315599e-18, - "velocityX": 1.7136696985593167, - "velocityY": 9.506575448004094e-26, - "timestamp": 0.3927877734475669 - }, - { - "x": 2.12497075686256, - "y": 7.39399003982544, - "heading": -1.0623970199278788e-18, - "angularVelocity": -1.4691215100259113e-17, - "velocityX": 2.1420870889066284, - "velocityY": 1.3343481989110454e-25, - "timestamp": 0.4909847168094586 - }, - { - "x": 2.3773864365383615, - "y": 7.39399003982544, - "heading": -3.9564889545725385e-18, - "angularVelocity": -2.947232200024047e-17, - "velocityX": 2.5705044478376236, - "velocityY": 1.8500903675379378e-25, - "timestamp": 0.5891816601713503 - }, - { - "x": 2.6718713848686027, - "y": 7.39399003982544, - "heading": -8.853646546888228e-18, - "angularVelocity": -4.987077429157769e-17, - "velocityX": 2.9989217408219764, - "velocityY": 2.625793984498987e-25, - "timestamp": 0.6873786035332421 - }, - { - "x": 3.0084255847706767, - "y": 7.39399003982544, - "heading": -1.6413613329692498e-17, - "angularVelocity": -7.698780165331962e-17, - "velocityX": 3.4273388598436147, - "velocityY": 4.161152605292971e-25, - "timestamp": 0.7855755468951338 - }, - { - "x": 3.3798020724552167, - "y": 7.39399003982544, - "heading": -2.58741769005412e-17, - "angularVelocity": -9.634275005875749e-17, - "velocityX": 3.781955679779987, - "velocityY": -5.487818039619291e-25, - "timestamp": 0.8837724902570254 - }, - { - "x": 3.7511785589388484, - "y": 7.39399003982544, - "heading": 5.146033898598045e-18, - "angularVelocity": 3.158979265252523e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.295977316525143e-24, - "timestamp": 0.9819694336189171 - }, - { - "x": 4.122555045422488, - "y": 7.39399003982544, - "heading": 4.0038826563950476e-17, - "angularVelocity": 3.553348146159671e-16, - "velocityX": 3.7819556675504797, - "velocityY": 7.623936784532972e-25, - "timestamp": 1.0801663769808088 - }, - { - "x": 4.493931531906129, - "y": 7.39399003982544, - "heading": 6.570668459860429e-17, - "angularVelocity": 2.613916192895976e-16, - "velocityX": 3.7819556675504797, - "velocityY": 3.4278643163770276e-26, - "timestamp": 1.1783633203427004 - }, - { - "x": 4.865308018389769, - "y": 7.39399003982544, - "heading": 6.663782988380241e-17, - "angularVelocity": 9.482426370100795e-18, - "velocityX": 3.7819556675504797, - "velocityY": 2.693381196266241e-25, - "timestamp": 1.276560263704592 - }, - { - "x": 5.236684504873409, - "y": 7.39399003982544, - "heading": 5.2326006182614025e-17, - "angularVelocity": -1.4574612214194992e-16, - "velocityX": 3.7819556675504797, - "velocityY": 5.124149697811417e-24, - "timestamp": 1.3747572070664837 - }, - { - "x": 5.60806099135704, - "y": 7.39399003982544, - "heading": 2.884064553605338e-17, - "angularVelocity": -2.3916590315859936e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.1014556301472372e-23, - "timestamp": 1.4729541504283754 - }, - { - "x": 5.9794374790415805, - "y": 7.39399003982544, - "heading": 1.6502190572837753e-17, - "angularVelocity": -1.2565009195595738e-16, - "velocityX": 3.7819556797799856, - "velocityY": 6.524035237986936e-24, - "timestamp": 1.571151093790267 - }, - { - "x": 6.315991678943653, - "y": 7.39399003982544, - "heading": 9.967918627860597e-18, - "angularVelocity": -6.65425189408073e-17, - "velocityX": 3.427338859843601, - "velocityY": -3.262675265981889e-25, - "timestamp": 1.6693480371521587 - }, - { - "x": 6.610476627273894, - "y": 7.39399003982544, - "heading": 5.2731830670839714e-18, - "angularVelocity": -4.78093860085579e-17, - "velocityX": 2.9989217408219764, - "velocityY": -2.379324733264266e-25, - "timestamp": 1.7675449805140504 - }, - { - "x": 6.862892306949696, - "y": 7.39399003982544, - "heading": 2.2289001785398693e-18, - "angularVelocity": -3.10018090849907e-17, - "velocityX": 2.5705044478376236, - "velocityY": -1.7314309405461854e-25, - "timestamp": 1.865741923875942 - }, - { - "x": 7.0732387114953, - "y": 7.39399003982544, - "heading": 5.002812433628901e-19, - "angularVelocity": -1.760359206249271e-17, - "velocityX": 2.1420870889066284, - "velocityY": -1.2661949502166272e-25, - "timestamp": 1.9639388672378337 - }, - { - "x": 7.24151583782572, - "y": 7.39399003982544, - "heading": -2.663585350994398e-19, - "angularVelocity": -7.807165345874594e-18, - "velocityX": 1.7136696985593165, - "velocityY": -9.096401200333518e-26, - "timestamp": 2.0621358105997256 - }, - { - "x": 7.367723684098041, - "y": 7.39399003982544, - "heading": -4.029353027704155e-19, - "angularVelocity": -1.390845344762092e-18, - "velocityX": 1.2852522894444902, - "velocityY": -6.226126404979919e-26, - "timestamp": 2.1603327539616175 - }, - { - "x": 7.451862249084011, - "y": 7.39399003982544, - "heading": -2.1995708883691042e-19, - "angularVelocity": 1.8633799906775463e-18, - "velocityX": 0.8568348678215871, - "velocityY": -3.833262935804246e-26, - "timestamp": 2.2585296973235094 - }, - { - "x": 7.493931531906128, - "y": 7.39399003982544, - "heading": 0, - "angularVelocity": 2.2399586452459634e-18, - "velocityX": 0.42841743726253684, - "velocityY": -1.7862050890834192e-26, - "timestamp": 2.3567266406854013 - }, - { - "x": 7.493931531906128, - "y": 7.39399003982544, - "heading": 0, - "angularVelocity": 0, - "velocityX": -2.1810930337522913e-39, - "velocityY": 0, - "timestamp": 2.454923584047293 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLaneTaxi.1.traj b/src/main/deploy/choreo/AmpLaneTaxi.1.traj deleted file mode 100644 index 7e838880..00000000 --- a/src/main/deploy/choreo/AmpLaneTaxi.1.traj +++ /dev/null @@ -1,139 +0,0 @@ -{ - "samples": [ - { - "x": 1.468971848487854, - "y": 7.309329509735107, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -5.968004408679696e-41, - "timestamp": 0 - }, - { - "x": 1.5097881789229493, - "y": 7.309329509735107, - "heading": 1.867923413309381e-18, - "angularVelocity": 1.9312421328909037e-17, - "velocityX": 0.42198942031848213, - "velocityY": -8.175734433695576e-17, - "timestamp": 0.09672358706123589 - }, - { - "x": 1.5914208389649154, - "y": 7.309329509735107, - "heading": -2.7784523382293792e-18, - "angularVelocity": -4.803678645072435e-17, - "velocityX": 0.843978832074171, - "velocityY": -1.635143003079991e-16, - "timestamp": 0.19344717412247178 - }, - { - "x": 1.713869827417429, - "y": 7.309329509735108, - "heading": -6.149022858043543e-18, - "angularVelocity": -3.4846149955338706e-17, - "velocityX": 1.2659682314613812, - "velocityY": -2.4527069236460427e-16, - "timestamp": 0.29017076118370766 - }, - { - "x": 1.8771351424005531, - "y": 7.309329509735107, - "heading": 9.334684137308459e-20, - "angularVelocity": 6.453994057032915e-17, - "velocityX": 1.6879576114124106, - "velocityY": -3.2702618744361184e-16, - "timestamp": 0.38689434824494356 - }, - { - "x": 2.081216780530401, - "y": 7.309329509735108, - "heading": 7.900764051063292e-18, - "angularVelocity": 8.072096604598862e-17, - "velocityX": 2.1099469563783173, - "velocityY": -4.087800036020465e-16, - "timestamp": 0.48361793530617947 - }, - { - "x": 2.326114733911239, - "y": 7.309329509735108, - "heading": -4.0292085158169106e-19, - "angularVelocity": -8.584682780302335e-17, - "velocityX": 2.5319362197122866, - "velocityY": -4.905289237309452e-16, - "timestamp": 0.5803415223674153 - }, - { - "x": 2.611828963064465, - "y": 7.309329509735108, - "heading": 4.895920849005717e-18, - "angularVelocity": 5.476420787705161e-17, - "velocityX": 2.9539250748872585, - "velocityY": -5.724892251787591e-16, - "timestamp": 0.6770651094286512 - }, - { - "x": 2.856726916445305, - "y": 7.309329509735108, - "heading": 1.0580912487609066e-17, - "angularVelocity": 5.877889823861001e-17, - "velocityX": 2.531936219712306, - "velocityY": -4.90569760600763e-16, - "timestamp": 0.773788696489887 - }, - { - "x": 3.0608085545751536, - "y": 7.309329509735108, - "heading": 1.2560262621019845e-17, - "angularVelocity": 2.0466206900583654e-17, - "velocityX": 2.109946956378327, - "velocityY": -4.088065981186001e-16, - "timestamp": 0.8705122835511229 - }, - { - "x": 3.224073869558278, - "y": 7.309329509735107, - "heading": 1.163556067743937e-17, - "angularVelocity": -9.558506562343336e-18, - "velocityX": 1.687957611412416, - "velocityY": -3.270442505924889e-16, - "timestamp": 0.9672358706123587 - }, - { - "x": 3.3465228580107924, - "y": 7.309329509735107, - "heading": 1.1661599473339483e-17, - "angularVelocity": 2.705282706656373e-19, - "velocityX": 1.2659682314613843, - "velocityY": -2.4528264910978726e-16, - "timestamp": 1.0639594576735947 - }, - { - "x": 3.4281555180527588, - "y": 7.309329509735107, - "heading": 1.4643178138706357e-18, - "angularVelocity": -1.0542614870618343e-16, - "velocityX": 0.8439788320741728, - "velocityY": -1.6352149946538548e-16, - "timestamp": 1.1606830447348306 - }, - { - "x": 3.468971848487854, - "y": 7.309329509735107, - "heading": -3.620479666148997e-40, - "angularVelocity": -1.5138749754669695e-17, - "velocityX": 0.42198942031848286, - "velocityY": -8.176064575101043e-17, - "timestamp": 1.2574066317960666 - }, - { - "x": 3.468971848487854, - "y": 7.309329509735107, - "heading": -2.966209034098039e-40, - "angularVelocity": 6.774689529655474e-40, - "velocityX": 3.003387584304273e-39, - "velocityY": 2.5761438166078746e-41, - "timestamp": 1.3541302188573026 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLaneTaxi.traj b/src/main/deploy/choreo/AmpLaneTaxi.traj deleted file mode 100644 index 7e838880..00000000 --- a/src/main/deploy/choreo/AmpLaneTaxi.traj +++ /dev/null @@ -1,139 +0,0 @@ -{ - "samples": [ - { - "x": 1.468971848487854, - "y": 7.309329509735107, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -5.968004408679696e-41, - "timestamp": 0 - }, - { - "x": 1.5097881789229493, - "y": 7.309329509735107, - "heading": 1.867923413309381e-18, - "angularVelocity": 1.9312421328909037e-17, - "velocityX": 0.42198942031848213, - "velocityY": -8.175734433695576e-17, - "timestamp": 0.09672358706123589 - }, - { - "x": 1.5914208389649154, - "y": 7.309329509735107, - "heading": -2.7784523382293792e-18, - "angularVelocity": -4.803678645072435e-17, - "velocityX": 0.843978832074171, - "velocityY": -1.635143003079991e-16, - "timestamp": 0.19344717412247178 - }, - { - "x": 1.713869827417429, - "y": 7.309329509735108, - "heading": -6.149022858043543e-18, - "angularVelocity": -3.4846149955338706e-17, - "velocityX": 1.2659682314613812, - "velocityY": -2.4527069236460427e-16, - "timestamp": 0.29017076118370766 - }, - { - "x": 1.8771351424005531, - "y": 7.309329509735107, - "heading": 9.334684137308459e-20, - "angularVelocity": 6.453994057032915e-17, - "velocityX": 1.6879576114124106, - "velocityY": -3.2702618744361184e-16, - "timestamp": 0.38689434824494356 - }, - { - "x": 2.081216780530401, - "y": 7.309329509735108, - "heading": 7.900764051063292e-18, - "angularVelocity": 8.072096604598862e-17, - "velocityX": 2.1099469563783173, - "velocityY": -4.087800036020465e-16, - "timestamp": 0.48361793530617947 - }, - { - "x": 2.326114733911239, - "y": 7.309329509735108, - "heading": -4.0292085158169106e-19, - "angularVelocity": -8.584682780302335e-17, - "velocityX": 2.5319362197122866, - "velocityY": -4.905289237309452e-16, - "timestamp": 0.5803415223674153 - }, - { - "x": 2.611828963064465, - "y": 7.309329509735108, - "heading": 4.895920849005717e-18, - "angularVelocity": 5.476420787705161e-17, - "velocityX": 2.9539250748872585, - "velocityY": -5.724892251787591e-16, - "timestamp": 0.6770651094286512 - }, - { - "x": 2.856726916445305, - "y": 7.309329509735108, - "heading": 1.0580912487609066e-17, - "angularVelocity": 5.877889823861001e-17, - "velocityX": 2.531936219712306, - "velocityY": -4.90569760600763e-16, - "timestamp": 0.773788696489887 - }, - { - "x": 3.0608085545751536, - "y": 7.309329509735108, - "heading": 1.2560262621019845e-17, - "angularVelocity": 2.0466206900583654e-17, - "velocityX": 2.109946956378327, - "velocityY": -4.088065981186001e-16, - "timestamp": 0.8705122835511229 - }, - { - "x": 3.224073869558278, - "y": 7.309329509735107, - "heading": 1.163556067743937e-17, - "angularVelocity": -9.558506562343336e-18, - "velocityX": 1.687957611412416, - "velocityY": -3.270442505924889e-16, - "timestamp": 0.9672358706123587 - }, - { - "x": 3.3465228580107924, - "y": 7.309329509735107, - "heading": 1.1661599473339483e-17, - "angularVelocity": 2.705282706656373e-19, - "velocityX": 1.2659682314613843, - "velocityY": -2.4528264910978726e-16, - "timestamp": 1.0639594576735947 - }, - { - "x": 3.4281555180527588, - "y": 7.309329509735107, - "heading": 1.4643178138706357e-18, - "angularVelocity": -1.0542614870618343e-16, - "velocityX": 0.8439788320741728, - "velocityY": -1.6352149946538548e-16, - "timestamp": 1.1606830447348306 - }, - { - "x": 3.468971848487854, - "y": 7.309329509735107, - "heading": -3.620479666148997e-40, - "angularVelocity": -1.5138749754669695e-17, - "velocityX": 0.42198942031848286, - "velocityY": -8.176064575101043e-17, - "timestamp": 1.2574066317960666 - }, - { - "x": 3.468971848487854, - "y": 7.309329509735107, - "heading": -2.966209034098039e-40, - "angularVelocity": 6.774689529655474e-40, - "velocityX": 3.003387584304273e-39, - "velocityY": 2.5761438166078746e-41, - "timestamp": 1.3541302188573026 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBDA.1.traj b/src/main/deploy/choreo/CenterLanePBDA.1.traj index 40f0d8e3..9081b882 100644 --- a/src/main/deploy/choreo/CenterLanePBDA.1.traj +++ b/src/main/deploy/choreo/CenterLanePBDA.1.traj @@ -1,121 +1,122 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 2.2921355473422064e-38, - "velocityX": 1.1360383232068741e-38, - "velocityY": -2.144062321628808e-38, - "timestamp": 0 - }, - { - "x": 1.32825806778149, - "y": 5.589895062873393, - "heading": -2.8062774074031584e-19, - "angularVelocity": -2.995198561755265e-18, - "velocityX": 0.40860904707084805, - "velocityY": -0.011305499292899219, - "timestamp": 0.09369253321938388 - }, - { - "x": 1.4048253002214361, - "y": 5.587776581164742, - "heading": -1.768868342127426e-19, - "angularVelocity": 1.1072483896415275e-18, - "velocityX": 0.8172180835441991, - "velocityY": -0.022610998292584186, - "timestamp": 0.18738506643876776 - }, - { - "x": 1.519676146824621, - "y": 5.584598858658671, - "heading": -2.386873521248768e-19, - "angularVelocity": -6.596098514057519e-19, - "velocityX": 1.225827103364341, - "velocityY": -0.03391649683150392, - "timestamp": 0.28107759965815166 - }, - { - "x": 1.6728106047825377, - "y": 5.580361895432887, - "heading": -7.795008789281329e-19, - "angularVelocity": -5.772215866345043e-18, - "velocityX": 1.6344360932087083, - "velocityY": -0.04522199454104627, - "timestamp": 0.3747701328775355 - }, - { - "x": 1.8642286675420066, - "y": 5.575065691668706, - "heading": -1.0638718286937245e-18, - "angularVelocity": -3.0351506143991226e-18, - "velocityX": 2.0430450131096105, - "velocityY": -0.056527490315375004, - "timestamp": 0.4684626660969194 - }, - { - "x": 2.0939303023371445, - "y": 5.5687102482727004, - "heading": -1.0113200205360793e-18, - "angularVelocity": 5.608964381023318e-19, - "velocityX": 2.451653583293393, - "velocityY": -0.0678329764136413, - "timestamp": 0.5621551993163033 - }, - { - "x": 2.285348365096613, - "y": 5.563414044508519, - "heading": -7.732946400687193e-19, - "angularVelocity": 2.5404946615264457e-18, - "velocityX": 2.04304501310961, - "velocityY": -0.056527490315375004, - "timestamp": 0.6558477325356872 - }, - { - "x": 2.43848282305453, - "y": 5.559177081282735, - "heading": -4.312009437616493e-19, - "angularVelocity": 3.651237559230762e-18, - "velocityX": 1.634436093208708, - "velocityY": -0.04522199454104628, - "timestamp": 0.749540265755071 - }, - { - "x": 2.553333669657715, - "y": 5.555999358776664, - "heading": -7.015602955490743e-20, - "angularVelocity": 3.853507870950435e-18, - "velocityX": 1.2258271033643409, - "velocityY": -0.033916496831503926, - "timestamp": 0.8432327989744549 - }, - { - "x": 2.6299009020976607, - "y": 5.553880877068013, - "heading": -2.7026882274997263e-20, - "angularVelocity": 4.603264080766157e-19, - "velocityX": 0.817218083544199, - "velocityY": -0.022610998292584186, - "timestamp": 0.9369253321938388 - }, - { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, - "angularVelocity": 2.884635663770645e-19, - "velocityX": 0.40860904707084805, - "velocityY": -0.011305499292899219, - "timestamp": 1.0306178654132228 - }, - { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.1410946614987667e-37, - "timestamp": 1.1243103986326066 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.5131221782640854e-28, + "angularVelocity": 8.299873633167507e-29, + "velocityX": 6.262502855031199e-23, + "velocityY": -1.723228528384229e-24, + "timestamp": 0 + }, + { + "x": 1.3282580677814901, + "y": 5.589895062873393, + "heading": 3.410110874929926e-18, + "angularVelocity": 3.6396826395088883e-17, + "velocityX": 0.4086090460493266, + "velocityY": -0.011305499264635099, + "timestamp": 0.09369253345361556 + }, + { + "x": 1.4048253002214366, + "y": 5.587776581164742, + "heading": -6.538031746043414e-18, + "angularVelocity": -1.0617860628189422e-16, + "velocityX": 0.8172180815011558, + "velocityY": -0.022610998236055942, + "timestamp": 0.1873850669072311 + }, + { + "x": 1.5196761468246218, + "y": 5.584598858658671, + "heading": -1.263908620936914e-17, + "angularVelocity": -6.511782997625411e-17, + "velocityX": 1.2258271002997754, + "velocityY": -0.03391649674671154, + "timestamp": 0.28107760036084667 + }, + { + "x": 1.6728106047825395, + "y": 5.580361895432887, + "heading": -1.4189092760310783e-17, + "angularVelocity": -1.654354401527969e-17, + "velocityX": 1.6344360891226193, + "velocityY": -0.04522199442798972, + "timestamp": 0.3747701338144622 + }, + { + "x": 1.8642286675420086, + "y": 5.575065691668705, + "heading": -1.1778293764866163e-17, + "angularVelocity": 2.5730961757873988e-17, + "velocityX": 2.0430450080019944, + "velocityY": -0.05652749017405418, + "timestamp": 0.4684626672680778 + }, + { + "x": 2.0939303023371414, + "y": 5.568710248272701, + "heading": -1.2220695981018318e-17, + "angularVelocity": -4.721851357797737e-18, + "velocityX": 2.451653577164197, + "velocityY": -0.06783297624405477, + "timestamp": 0.5621552007216933 + }, + { + "x": 2.2853483650966107, + "y": 5.563414044508519, + "heading": -5.403675872721793e-18, + "angularVelocity": 7.275948100517659e-17, + "velocityX": 2.0430450080019944, + "velocityY": -0.056527490174054194, + "timestamp": 0.6558477341753088 + }, + { + "x": 2.438482823054528, + "y": 5.559177081282735, + "heading": -1.4049765598642364e-18, + "angularVelocity": 4.267895386440672e-17, + "velocityX": 1.6344360891226193, + "velocityY": -0.04522199442798973, + "timestamp": 0.7495402676289245 + }, + { + "x": 2.5533336696577136, + "y": 5.555999358776664, + "heading": -6.020213691896355e-19, + "angularVelocity": 8.570108640736073e-18, + "velocityX": 1.2258271002997754, + "velocityY": -0.033916496746711544, + "timestamp": 0.8432328010825401 + }, + { + "x": 2.6299009020976603, + "y": 5.553880877068013, + "heading": -2.9970702235685054e-18, + "angularVelocity": -2.5562857212265518e-17, + "velocityX": 0.8172180815011557, + "velocityY": -0.022610998236055942, + "timestamp": 0.9369253345361557 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": -2.3882654133259072e-26, + "angularVelocity": 3.198835690592465e-17, + "velocityX": 0.40860904604932646, + "velocityY": -0.0113054992646351, + "timestamp": 1.0306178679897713 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": -3.2765992030044065e-26, + "angularVelocity": -9.642870627859863e-26, + "velocityX": -6.242213516414814e-23, + "velocityY": 1.2639029431778876e-24, + "timestamp": 1.124310401443387 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBDA.2.traj b/src/main/deploy/choreo/CenterLanePBDA.2.traj index 3b4bdef8..91b5b6cf 100644 --- a/src/main/deploy/choreo/CenterLanePBDA.2.traj +++ b/src/main/deploy/choreo/CenterLanePBDA.2.traj @@ -1,517 +1,518 @@ { - "samples": [ - { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.1410946614987667e-37, - "timestamp": 0 - }, - { - "x": 2.687410064097537, - "y": 5.560258515634833, - "heading": 0.0028176822125938034, - "angularVelocity": 0.04098002734374148, - "velocityX": 0.2796139919160097, - "velocityY": 0.1081610698436357, - "timestamp": 0.06875745076886175 - }, - { - "x": 2.7258611531960137, - "y": 5.575132263104083, - "heading": 0.008453116109374614, - "angularVelocity": 0.08196106507388369, - "velocityX": 0.5592279624754555, - "velocityY": 0.2163219738796251, - "timestamp": 0.1375149015377235 - }, - { - "x": 2.783537789303066, - "y": 5.597442863001631, - "heading": 0.016905769734324238, - "angularVelocity": 0.12293436610040698, - "velocityX": 0.8388419794814749, - "velocityY": 0.3244826509427688, - "timestamp": 0.20627235230658525 - }, - { - "x": 2.8604399802132514, - "y": 5.627190295278175, - "heading": 0.028174540960364546, - "angularVelocity": 0.1638916379247738, - "velocityX": 1.118456109850604, - "velocityY": 0.4326430364113507, - "timestamp": 0.275029803075447 - }, - { - "x": 2.9565677382168056, - "y": 5.664374535134068, - "heading": 0.042257801458542396, - "angularVelocity": 0.20482522753092394, - "velocityX": 1.3980704189674162, - "velocityY": 0.5408030611968617, - "timestamp": 0.34378725384430875 - }, - { - "x": 3.0719210799387406, - "y": 5.708995552649304, - "heading": 0.05915345386800317, - "angularVelocity": 0.2457283133759373, - "velocityX": 1.6776849698763825, - "velocityY": 0.648962650829455, - "timestamp": 0.4125447046131705 - }, - { - "x": 3.2065000261043313, - "y": 5.761053312357379, - "heading": 0.07885900262196403, - "angularVelocity": 0.2865951040012339, - "velocityX": 1.9572998222112317, - "velocityY": 0.757121724641519, - "timestamp": 0.48130215538203225 - }, - { - "x": 3.360304601198153, - "y": 5.820547772766793, - "heading": 0.1013716386695389, - "angularVelocity": 0.32742104013214957, - "velocityX": 2.2369150306467707, - "velocityY": 0.8652801950062525, - "timestamp": 0.550059606150894 - }, - { - "x": 3.5333348329448953, - "y": 5.8874788858214675, - "heading": 0.12668833767209217, - "angularVelocity": 0.36820299064984424, - "velocityX": 2.5165306423068015, - "velocityY": 0.973437966449241, - "timestamp": 0.6188170569197557 - }, - { - "x": 3.7255907514118967, - "y": 5.961846596243587, - "heading": 0.15480596913918515, - "angularVelocity": 0.4089394116954055, - "velocityX": 2.796146691262561, - "velocityY": 1.0815949339384094, - "timestamp": 0.6875745076886175 - }, - { - "x": 3.937072386955544, - "y": 6.04365084047756, - "heading": 0.18572140510730675, - "angularVelocity": 0.44963034002016317, - "velocityX": 3.075763181717921, - "velocityY": 1.1897509770827353, - "timestamp": 0.7563319584574792 - }, - { - "x": 4.167779761570079, - "y": 6.132891543122838, - "heading": 0.21943155266112502, - "angularVelocity": 0.4902762853605699, - "velocityX": 3.355379992054849, - "velocityY": 1.2979059236107295, - "timestamp": 0.825089409226341 - }, - { - "x": 4.410308649457132, - "y": 6.2266961328710915, - "heading": 0.21943155790785696, - "angularVelocity": 7.630783127279499e-8, - "velocityX": 3.5273106430654275, - "velocityY": 1.3642825424635276, - "timestamp": 0.8938468599952027 - }, - { - "x": 4.652837676050608, - "y": 6.320500363997887, - "heading": 0.21943156315423873, - "angularVelocity": 7.630273806741925e-8, - "velocityX": 3.5273126603947684, - "velocityY": 1.3642773267166644, - "timestamp": 0.9626043107640647 - }, - { - "x": 4.897946357727051, - "y": 6.407341003417969, - "heading": 0.21943156841217684, - "angularVelocity": 7.647081243446701e-8, - "velocityX": 3.5648308501199604, - "velocityY": 1.2629996960185401, - "timestamp": 1.0313617615329267 - }, - { - "x": 5.1308682473529785, - "y": 6.471009903235158, - "heading": 0.21943157303192085, - "angularVelocity": 7.23563158909772e-8, - "velocityX": 3.6481177055177683, - "velocityY": 0.9972082962532426, - "timestamp": 1.0952089036560089 - }, - { - "x": 5.365529567427774, - "y": 6.527934417793533, - "heading": 0.21943157763716262, - "angularVelocity": 7.21291764013049e-8, - "velocityX": 3.6753613751798433, - "velocityY": 0.8915749815181846, - "timestamp": 1.159056045779091 - }, - { - "x": 5.600190991292763, - "y": 6.584858504493345, - "heading": 0.21943158224240422, - "angularVelocity": 7.21291734995501e-8, - "velocityX": 3.6753630007842206, - "velocityY": 0.8915682802227483, - "timestamp": 1.2229031879021732 - }, - { - "x": 5.8348524151632635, - "y": 6.641782591170481, - "heading": 0.2194315868476458, - "angularVelocity": 7.212917336838375e-8, - "velocityX": 3.6753630008705414, - "velocityY": 0.8915682798675781, - "timestamp": 1.2867503300252554 - }, - { - "x": 6.069513839029734, - "y": 6.698706677864191, - "heading": 0.21943159145288735, - "angularVelocity": 7.212917280605725e-8, - "velocityX": 3.6753630008074327, - "velocityY": 0.891568280127171, - "timestamp": 1.3505974721483376 - }, - { - "x": 6.3041751870414915, - "y": 6.75563107725722, - "heading": 0.21943159605812906, - "angularVelocity": 7.212917561549481e-8, - "velocityX": 3.6753618127399528, - "velocityY": 0.8915731777514998, - "timestamp": 1.4144446142714198 - }, - { - "x": 6.537489271202827, - "y": 6.817847413489264, - "heading": 0.21943160514124108, - "angularVelocity": 1.4226340838423697e-7, - "velocityX": 3.6542604164106804, - "velocityY": 0.9744576524998471, - "timestamp": 1.478291756394502 - }, - { - "x": 6.7545327794045615, - "y": 6.881509207420976, - "heading": 0.24890077922126197, - "angularVelocity": 0.46155823268064305, - "velocityX": 3.399424014677561, - "velocityY": 0.9970970009742466, - "timestamp": 1.5421388985175841 - }, - { - "x": 6.954212307545749, - "y": 6.941327014961267, - "heading": 0.2778274290995393, - "angularVelocity": 0.45306099719431336, - "velocityX": 3.1274622716276532, - "velocityY": 0.9368909171372027, - "timestamp": 1.6059860406406663 - }, - { - "x": 7.1365689728963595, - "y": 6.997132012930522, - "heading": 0.305376589845945, - "angularVelocity": 0.43148620017020184, - "velocityX": 2.856144524042602, - "velocityY": 0.8740406557535503, - "timestamp": 1.6698331827637485 - }, - { - "x": 7.301617943292834, - "y": 7.048866027505986, - "heading": 0.33125825392103403, - "angularVelocity": 0.40536918669284566, - "velocityX": 2.585064341302851, - "velocityY": 0.8102792522134878, - "timestamp": 1.7336803248868307 - }, - { - "x": 7.449367094221676, - "y": 7.096499619737108, - "heading": 0.35532461486989275, - "angularVelocity": 0.37693716818945566, - "velocityX": 2.3141075076472033, - "velocityY": 0.7460567638139751, - "timestamp": 1.7975274670099128 - }, - { - "x": 7.579821246213137, - "y": 7.140015020198173, - "heading": 0.3774857294479878, - "angularVelocity": 0.347096421878647, - "velocityX": 2.04322617510391, - "velocityY": 0.681555963416076, - "timestamp": 1.861374609132995 - }, - { - "x": 7.69298365301254, - "y": 7.179400342359022, - "heading": 0.3976808814927073, - "angularVelocity": 0.3163047142468991, - "velocityX": 1.7723958040479335, - "velocityY": 0.6168689913312067, - "timestamp": 1.9252217512560772 - }, - { - "x": 7.788856658439436, - "y": 7.214647078189013, - "heading": 0.41586620005922853, - "angularVelocity": 0.2848258819708253, - "velocityX": 1.5016021428504343, - "velocityY": 0.5520487629977263, - "timestamp": 1.9890688933791594 - }, - { - "x": 7.867442031134741, - "y": 7.2457488378850705, - "heading": 0.43200844627235874, - "angularVelocity": 0.2528264488645923, - "velocityX": 1.2308361828288252, - "velocityY": 0.48712845496042256, - "timestamp": 2.0529160355022418 - }, - { - "x": 7.928741153018892, - "y": 7.27270064652237, - "heading": 0.4460815614823235, - "angularVelocity": 0.22041887454940703, - "velocityX": 0.9600918670092122, - "velocityY": 0.4221302276198955, - "timestamp": 2.116763177625324 - }, - { - "x": 7.9727551334716695, - "y": 7.29549852060884, - "heading": 0.45806460123681797, - "angularVelocity": 0.1876832596734667, - "velocityX": 0.6893649267484526, - "velocityY": 0.35706960920065395, - "timestamp": 2.180610319748406 - }, - { - "x": 7.999484882530383, - "y": 7.314139197912392, - "heading": 0.4679404256802237, - "angularVelocity": 0.1546791933829663, - "velocityX": 0.4186522398635212, - "velocityY": 0.2919578963709263, - "timestamp": 2.2444574618714883 - }, - { - "x": 8.008931159973145, - "y": 7.328619956970215, - "heading": 0.4756948301053548, - "angularVelocity": 0.12145264717068183, - "velocityX": 0.1479514529335022, - "velocityY": 0.22680355887981873, - "timestamp": 2.3083046039945705 - }, - { - "x": 7.9838028545378235, - "y": 7.340994975747599, - "heading": 0.4825391441548679, - "angularVelocity": 0.07100818067139564, - "velocityX": -0.26070037689812714, - "velocityY": 0.12838796741348574, - "timestamp": 2.4046922875656627 - }, - { - "x": 7.919285560188609, - "y": 7.343883953710174, - "heading": 0.4845188810802152, - "angularVelocity": 0.02053931427751041, - "velocityX": -0.6693520578449088, - "velocityY": 0.029972480461632738, - "timestamp": 2.5010799711367557 - }, - { - "x": 7.815379289936097, - "y": 7.337286883355744, - "heading": 0.4816330205236378, - "angularVelocity": -0.029940138092932247, - "velocityX": -1.0780036038097405, - "velocityY": -0.06844308432376209, - "timestamp": 2.597467654707848 - }, - { - "x": 7.6720840556149295, - "y": 7.321203739342901, - "heading": 0.47388185121605153, - "angularVelocity": -0.08041659494667612, - "velocityX": -1.4866550269929244, - "velocityY": -0.16685891202042144, - "timestamp": 2.693855338278941 - }, - { - "x": 7.489399868443907, - "y": 7.295634478019173, - "heading": 0.46126687890088164, - "angularVelocity": -0.13087743005945188, - "velocityX": -1.895306333783606, - "velocityY": -0.2652751926014197, - "timestamp": 2.790243021850033 - }, - { - "x": 7.267326740077932, - "y": 7.2605790366684495, - "heading": 0.44379066346613116, - "angularVelocity": -0.1813117069242423, - "velocityX": -2.3039575196573705, - "velocityY": -0.36369212384761535, - "timestamp": 2.886630705421126 - }, - { - "x": 7.00586468444221, - "y": 7.216037332217007, - "heading": 0.4214565844390286, - "angularVelocity": -0.23171092197300747, - "velocityX": -2.7126085610603474, - "velocityY": -0.4621099169645314, - "timestamp": 2.9830183889922193 - }, - { - "x": 6.7050137215236445, - "y": 7.162009258221821, - "heading": 0.3942685380548319, - "angularVelocity": -0.2820697144790415, - "velocityX": -3.1212593951037992, - "velocityY": -0.5605288144032944, - "timestamp": 3.0794060725633123 - }, - { - "x": 6.364773891396992, - "y": 7.098494671595923, - "heading": 0.3622306012435557, - "angularVelocity": -0.3323862097758952, - "velocityX": -3.529909813380894, - "velocityY": -0.658949196336373, - "timestamp": 3.1757937561344054 - }, - { - "x": 6.010374312951181, - "y": 7.013136980735367, - "heading": 0.36223059672201996, - "angularVelocity": -4.690989126354215e-8, - "velocityX": -3.6768139384158545, - "velocityY": -0.8855663680059631, - "timestamp": 3.2721814397054985 - }, - { - "x": 5.655974859532265, - "y": 6.927778770772817, - "heading": 0.36223059220056536, - "angularVelocity": -4.690904952415849e-8, - "velocityX": -3.676812641290635, - "velocityY": -0.885571753569461, - "timestamp": 3.3685691232765915 - }, - { - "x": 5.301574928644714, - "y": 6.842422689744232, - "heading": 0.3622305506125328, - "angularVelocity": -4.314662515297724e-7, - "velocityX": -3.6768175949176816, - "velocityY": -0.8855496663702899, - "timestamp": 3.4649568068476846 - }, - { - "x": 4.986369114322999, - "y": 6.766504438890574, - "heading": 0.34532942408176365, - "angularVelocity": -0.1753452921016411, - "velocityX": -3.270187669664538, - "velocityY": -0.7876343536948325, - "timestamp": 3.5613444904187777 - }, - { - "x": 4.710566832932512, - "y": 6.70007668327783, - "heading": 0.3264669766789321, - "angularVelocity": -0.1956935440711402, - "velocityX": -2.861385097890258, - "velocityY": -0.6891726530988762, - "timestamp": 3.6577321739898707 - }, - { - "x": 4.474170501345758, - "y": 6.643140019477641, - "heading": 0.30840376738356945, - "angularVelocity": -0.18740163292794487, - "velocityX": -2.4525574516208333, - "velocityY": -0.5907047632096585, - "timestamp": 3.754119857560964 - }, - { - "x": 4.277177710352041, - "y": 6.5956938222588235, - "heading": 0.2923060351860776, - "angularVelocity": -0.16701026107365813, - "velocityX": -2.0437548003570405, - "velocityY": -0.4922433599498598, - "timestamp": 3.850507541132057 - }, - { - "x": 4.1195862527073634, - "y": 6.557737539015946, - "heading": 0.27881764120910085, - "angularVelocity": -0.1399389784798502, - "velocityX": -1.6349750487409798, - "velocityY": -0.3937876898440308, - "timestamp": 3.94689522470315 - }, - { - "x": 4.0013944038572795, - "y": 6.529270748726535, - "heading": 0.26834692670698207, - "angularVelocity": -0.10863124949357753, - "velocityX": -1.226213188979785, - "velocityY": -0.2953363877493204, - "timestamp": 4.043282908274243 - }, - { - "x": 3.9226008250519366, - "y": 6.510293128915594, - "heading": 0.26117603215990287, - "angularVelocity": -0.07439637805789222, - "velocityX": -0.817465218439728, - "velocityY": -0.19688843125838204, - "timestamp": 4.139670591845336 - }, - { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "angularVelocity": -0.038017450198398375, - "velocityX": -0.40872820518438957, - "velocityY": -0.09844312341738799, - "timestamp": 4.236058275416429 - }, - { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.0638409913121086e-32, - "timestamp": 4.332445958987522 - } - ] + "samples": [ + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": -3.2765992030044065e-26, + "angularVelocity": -9.642870627859863e-26, + "velocityX": -6.242213516414814e-23, + "velocityY": 1.2639029431778876e-24, + "timestamp": 0 + }, + { + "x": 2.6874100640586183, + "y": 5.560258515785914, + "heading": 0.002817682314802049, + "angularVelocity": 0.040980028708864365, + "velocityX": 0.2796139905217839, + "velocityY": 0.10816107172058755, + "timestamp": 0.06875745097251618 + }, + { + "x": 2.7258611530795362, + "y": 5.575132263557277, + "heading": 0.00845311638740209, + "angularVelocity": 0.08196106738821132, + "velocityX": 0.5592279596910537, + "velocityY": 0.21632197763277797, + "timestamp": 0.13751490194503235 + }, + { + "x": 2.783537789070723, + "y": 5.597442863907905, + "heading": 0.01690577022745014, + "angularVelocity": 0.12293436886464493, + "velocityX": 0.8388419753117596, + "velocityY": 0.32448265657123054, + "timestamp": 0.20627235291754853 + }, + { + "x": 2.860439979827146, + "y": 5.6271902967884255, + "heading": 0.0281745416659326, + "angularVelocity": 0.1638916405290653, + "velocityX": 1.118456104301514, + "velocityY": 0.4326430439140473, + "timestamp": 0.2750298038900647 + }, + { + "x": 2.956567737639549, + "y": 5.664374537399098, + "heading": 0.042257802321547584, + "angularVelocity": 0.20482522921398508, + "velocityX": 1.3980704120463603, + "velocityY": 0.5408030705724766, + "timestamp": 0.3437872548625809 + }, + { + "x": 3.0719210791335985, + "y": 5.708995555819805, + "heading": 0.05915345476625426, + "angularVelocity": 0.24572831316071073, + "velocityX": 1.6776849615928617, + "velocityY": 0.6489626620763342, + "timestamp": 0.41254470583509706 + }, + { + "x": 3.206500025035437, + "y": 5.761053316583894, + "heading": 0.07885900334385978, + "angularVelocity": 0.28659510058745824, + "velocityX": 1.9572998125778749, + "velocityY": 0.7571217377575138, + "timestamp": 0.48130215680761324 + }, + { + "x": 3.3603045998308527, + "y": 5.820547778199662, + "heading": 0.10137163887853046, + "angularVelocity": 0.32742103170272313, + "velocityX": 2.2369150196812018, + "velocityY": 0.865280209988437, + "timestamp": 0.5500596077801294 + }, + { + "x": 3.533334831246345, + "y": 5.88747889261074, + "heading": 0.12668833684449327, + "angularVelocity": 0.36820297448319306, + "velocityX": 2.5165306300353754, + "velocityY": 0.9734379832933463, + "timestamp": 0.6188170587526456 + }, + { + "x": 3.725590749352267, + "y": 5.961846604538833, + "heading": 0.15480596643975877, + "angularVelocity": 0.4089393832605092, + "velocityX": 2.796146677729085, + "velocityY": 1.081594952637503, + "timestamp": 0.6875745097251618 + }, + { + "x": 3.9370723845110165, + "y": 6.043650850427416, + "heading": 0.18572139907813143, + "angularVelocity": 0.4496302902609083, + "velocityX": 3.075763167009828, + "velocityY": 1.189750997623249, + "timestamp": 0.7563319606976779 + }, + { + "x": 4.167779758734844, + "y": 6.1328915548731855, + "heading": 0.2194315399767765, + "angularVelocity": 0.4902761871163567, + "velocityX": 3.3553799764340586, + "velocityY": 1.2979059459525522, + "timestamp": 0.8250894116701941 + }, + { + "x": 4.410308645337483, + "y": 6.226696146473074, + "heading": 0.2194315452235168, + "angularVelocity": 7.630795245724027e-8, + "velocityX": 3.5273106139374217, + "velocityY": 1.3642825653525794, + "timestamp": 0.8938468626427105 + }, + { + "x": 4.652837676496205, + "y": 6.320500364327338, + "heading": 0.2194315504698994, + "angularVelocity": 7.630275018427045e-8, + "velocityX": 3.5273127163435154, + "velocityY": 1.364277129641693, + "timestamp": 0.9626043136152269 + }, + { + "x": 4.897946357727051, + "y": 6.407341003417969, + "heading": 0.21943155572783843, + "angularVelocity": 7.647082562756639e-8, + "velocityX": 3.564830833080488, + "velocityY": 1.2629996874861198, + "timestamp": 1.0313617645877433 + }, + { + "x": 5.130868247940416, + "y": 6.471009903129215, + "heading": 0.21943156034758332, + "angularVelocity": 7.235632921744003e-8, + "velocityX": 3.648117688337217, + "velocityY": 0.9972082873825086, + "timestamp": 1.0952089071725348 + }, + { + "x": 5.365529564309454, + "y": 6.527934435249553, + "heading": 0.21943156495282606, + "angularVelocity": 7.212919079793877e-8, + "velocityX": 3.6753612905604753, + "velocityY": 0.891575250133395, + "timestamp": 1.1590560497573263 + }, + { + "x": 5.60019098861159, + "y": 6.584858522432942, + "heading": 0.21943156955806847, + "angularVelocity": 7.212918610327084e-8, + "velocityX": 3.675362981052711, + "velocityY": 0.8915682813493487, + "timestamp": 1.2229031923421179 + }, + { + "x": 5.834852412919734, + "y": 6.6417826095915675, + "heading": 0.21943157416331086, + "angularVelocity": 7.212918546345333e-8, + "velocityX": 3.675362981146796, + "velocityY": 0.8915682809614962, + "timestamp": 1.2867503349269094 + }, + { + "x": 6.069513837223475, + "y": 6.698706696768341, + "heading": 0.21943157876855326, + "angularVelocity": 7.21291856487456e-8, + "velocityX": 3.6753629810778445, + "velocityY": 0.8915682812457394, + "timestamp": 1.350597477511701 + }, + { + "x": 6.304175182424159, + "y": 6.755631110035184, + "heading": 0.2194315833737959, + "angularVelocity": 7.212918914147318e-8, + "velocityX": 3.675361742133493, + "velocityY": 0.8915733886014583, + "timestamp": 1.4144446200964924 + }, + { + "x": 6.5374892679107495, + "y": 6.8178474433780725, + "heading": 0.2194315924580089, + "angularVelocity": 1.4228065067709048e-7, + "velocityX": 3.6542604107418026, + "velocityY": 0.9744576002013384, + "timestamp": 1.478291762681284 + }, + { + "x": 6.754532777659766, + "y": 6.881509233956424, + "heading": 0.2489007631101974, + "angularVelocity": 0.46155817565449186, + "velocityX": 3.399424014328823, + "velocityY": 0.9970969412422411, + "timestamp": 1.5421389052660754 + }, + { + "x": 6.95421230704469, + "y": 6.941327038715391, + "heading": 0.2778274120513566, + "angularVelocity": 0.4530609792402775, + "velocityX": 3.1274622684913846, + "velocityY": 0.9368908667999729, + "timestamp": 1.605986047850867 + }, + { + "x": 7.136568973398106, + "y": 6.997132034172886, + "heading": 0.3053765730205754, + "angularVelocity": 0.4314862005395546, + "velocityX": 2.8561445190947854, + "velocityY": 0.8740406100928324, + "timestamp": 1.6698331904356585 + }, + { + "x": 7.301617944582347, + "y": 7.048866046398329, + "heading": 0.3312582379964112, + "angularVelocity": 0.40536919786917625, + "velocityX": 2.585064334947336, + "velocityY": 0.8102792095470503, + "timestamp": 1.73368033302045 + }, + { + "x": 7.449367096097909, + "y": 7.096499636385878, + "heading": 0.3553246002750777, + "angularVelocity": 0.376937186291547, + "velocityX": 2.314107500102227, + "velocityY": 0.7460567232791978, + "timestamp": 1.7975274756052415 + }, + { + "x": 7.579821248483784, + "y": 7.140015034676201, + "heading": 0.37748571646004736, + "angularVelocity": 0.3470964445360872, + "velocityX": 2.0432261665058515, + "velocityY": 0.6815559244884293, + "timestamp": 1.861374618190033 + }, + { + "x": 7.692983655491274, + "y": 7.179400354716541, + "heading": 0.3976808702861806, + "angularVelocity": 0.31630473986072827, + "velocityX": 1.7723957944900266, + "velocityY": 0.6168689536581131, + "timestamp": 1.9252217607748245 + }, + { + "x": 7.78885666094427, + "y": 7.214647088460025, + "heading": 0.4158661907347705, + "angularVelocity": 0.28482590938880487, + "velocityX": 1.5016021324004238, + "velocityY": 0.5520487263259451, + "timestamp": 1.989068903359616 + }, + { + "x": 7.867442033486985, + "y": 7.245748846091361, + "heading": 0.43200843887480883, + "angularVelocity": 0.2528264772162807, + "velocityX": 1.2308361715381555, + "velocityY": 0.4871284190992695, + "timestamp": 2.0529160459444076 + }, + { + "x": 7.92874115504245, + "y": 7.272700652676186, + "heading": 0.4460815560128383, + "angularVelocity": 0.2204189031535715, + "velocityX": 0.9600918549182749, + "velocityY": 0.4221301924206222, + "timestamp": 2.116763188529199 + }, + { + "x": 7.972755134992524, + "y": 7.295498524714787, + "heading": 0.4580645976614163, + "angularVelocity": 0.18768328798213776, + "velocityX": 0.689364913889801, + "velocityY": 0.35706957454398996, + "timestamp": 2.1806103311139906 + }, + { + "x": 7.9994848833762395, + "y": 7.314139199968811, + "heading": 0.467940423936039, + "angularVelocity": 0.15467922094567124, + "velocityX": 0.4186522262639504, + "velocityY": 0.29195786215910857, + "timestamp": 2.244457473698782 + }, + { + "x": 8.008931159973145, + "y": 7.328619956970215, + "heading": 0.4756948301053548, + "angularVelocity": 0.12145267361053229, + "velocityX": 0.1479514386154507, + "velocityY": 0.2268035250312554, + "timestamp": 2.3083046162835736 + }, + { + "x": 7.983802853314603, + "y": 7.34099497259759, + "heading": 0.48253914652919067, + "angularVelocity": 0.07100820527560414, + "velocityX": -0.2607003894828745, + "velocityY": 0.12838793468070023, + "timestamp": 2.404692299893813 + }, + { + "x": 7.919285557898086, + "y": 7.343883947492977, + "heading": 0.48451888567323514, + "angularVelocity": 0.020539337287634327, + "velocityX": -0.6693520686460666, + "velocityY": 0.029972448628073807, + "timestamp": 2.5010799835040527 + }, + { + "x": 7.8153792867404235, + "y": 7.337286874127341, + "heading": 0.48163302720996115, + "angularVelocity": -0.029940116363242703, + "velocityX": -1.078003612762665, + "velocityY": -0.06844311553655845, + "timestamp": 2.5974676671142922 + }, + { + "x": 7.672084051684559, + "y": 7.321203727123482, + "heading": 0.4738818599108745, + "angularVelocity": -0.08041657407630928, + "velocityX": -1.4866550340114346, + "velocityY": -0.16685894298377135, + "timestamp": 2.6938553507245317 + }, + { + "x": 7.489399863960927, + "y": 7.295634462778821, + "heading": 0.46126688957646533, + "angularVelocity": -0.1308774094563781, + "velocityX": -1.895306338747057, + "velocityY": -0.26527522383518304, + "timestamp": 2.7902430343347713 + }, + { + "x": 7.26732673524187, + "y": 7.260579018302084, + "heading": 0.4437906761804973, + "angularVelocity": -0.18131168569872746, + "velocityX": -2.3039575223848003, + "velocityY": -0.3636921561315803, + "timestamp": 2.886630717945011 + }, + { + "x": 7.005864679481663, + "y": 7.216037310494284, + "heading": 0.42145659939316793, + "angularVelocity": -0.23171089864179226, + "velocityX": -2.712608561250158, + "velocityY": -0.46210995159831264, + "timestamp": 2.9830184015552503 + }, + { + "x": 6.705013716725352, + "y": 7.162009232661868, + "heading": 0.39426855573546815, + "angularVelocity": -0.28206968607772986, + "velocityX": -3.1212593921527856, + "velocityY": -0.5605288539860318, + "timestamp": 3.07940608516549 + }, + { + "x": 6.364773887222121, + "y": 7.098494640966319, + "heading": 0.36223062299373554, + "angularVelocity": -0.3323861674203487, + "velocityX": -3.5299098054794116, + "velocityY": -0.6589492486652313, + "timestamp": 3.1757937687757294 + }, + { + "x": 6.0103743066555975, + "y": 7.0131369660627, + "heading": 0.3622306184721976, + "angularVelocity": -4.6909913247045394e-8, + "velocityX": -3.6768139589244604, + "velocityY": -0.8855662020967079, + "timestamp": 3.272181452385969 + }, + { + "x": 5.655974854365273, + "y": 6.927778758566046, + "heading": 0.3622306139507427, + "angularVelocity": -4.690905296707159e-8, + "velocityX": -3.6768126280884714, + "velocityY": -0.8855717276266857, + "timestamp": 3.3685691359962084 + }, + { + "x": 5.301574924606988, + "y": 6.842422680000666, + "heading": 0.3622305723626749, + "angularVelocity": -4.3146661671098113e-7, + "velocityX": -3.676817581708511, + "velocityY": -0.8855496404554449, + "timestamp": 3.464956819606448 + }, + { + "x": 4.986369111177269, + "y": 6.766504431310858, + "heading": 0.34532944076922883, + "angularVelocity": -0.17534534455448447, + "velocityX": -3.2701876590821435, + "velocityY": -0.7876343309254682, + "timestamp": 3.5613445032166875 + }, + { + "x": 4.710566830571078, + "y": 6.700076677592526, + "heading": 0.3264669889522253, + "angularVelocity": -0.19569358978764517, + "velocityX": -2.8613850885912564, + "velocityY": -0.6891726331648808, + "timestamp": 3.657732186826927 + }, + { + "x": 4.474170499658151, + "y": 6.6431400154165186, + "heading": 0.30840377599143814, + "angularVelocity": -0.18740167087974618, + "velocityX": -2.452557443633948, + "velocityY": -0.5907047461192427, + "timestamp": 3.7541198704371666 + }, + { + "x": 4.277177709226617, + "y": 6.595693819551337, + "heading": 0.29230604083143724, + "angularVelocity": -0.16701029174116175, + "velocityX": -2.043754793694494, + "velocityY": -0.4922433457062721, + "timestamp": 3.850507554047406 + }, + { + "x": 4.119586252031972, + "y": 6.557737537391428, + "heading": 0.2788176445470824, + "angularVelocity": -0.13993900236152076, + "velocityX": -1.6349750434079608, + "velocityY": -0.393787678448541, + "timestamp": 3.9468952376576456 + }, + { + "x": 4.001394403519539, + "y": 6.529270747914268, + "heading": 0.2683469283541488, + "angularVelocity": -0.10863126699126655, + "velocityX": -1.226213184978727, + "velocityY": -0.2953363792024593, + "timestamp": 4.043282921267885 + }, + { + "x": 3.922600824939347, + "y": 6.510293128644836, + "heading": 0.26117603270245576, + "angularVelocity": -0.074396389487788, + "velocityX": -0.8174652157718444, + "velocityY": -0.1968884255603806, + "timestamp": 4.139670604878125 + }, + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": -0.038017455811817785, + "velocityX": -0.40872820385029696, + "velocityY": -0.09844312056835454, + "timestamp": 4.236058288488364 + }, + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": -1.8368418613420646e-25, + "velocityX": -1.6872129777974468e-25, + "velocityY": 1.375029629161337e-25, + "timestamp": 4.332445972098604 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBDA.3.traj b/src/main/deploy/choreo/CenterLanePBDA.3.traj index 78de849d..86aebdec 100644 --- a/src/main/deploy/choreo/CenterLanePBDA.3.traj +++ b/src/main/deploy/choreo/CenterLanePBDA.3.traj @@ -1,274 +1,275 @@ { - "samples": [ - { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.0638409913121086e-32, - "timestamp": 0 - }, - { - "x": 3.856532695000848, - "y": 6.488812456981272, - "heading": 0.25799867286334044, - "angularVelocity": 0.005949010116156059, - "velocityX": -0.32577575495196986, - "velocityY": -0.14647295299154703, - "timestamp": 0.08187154733822144 - }, - { - "x": 3.8031475649180457, - "y": 6.464921556304023, - "heading": 0.25900179360543873, - "angularVelocity": 0.01225237307357005, - "velocityX": -0.6520596204474033, - "velocityY": -0.29180956576467965, - "timestamp": 0.16374309467644288 - }, - { - "x": 3.7229939182765985, - "y": 6.429256431279799, - "heading": 0.2605597691266567, - "angularVelocity": 0.019029511128961094, - "velocityX": -0.9790171219107733, - "velocityY": -0.43562295063126366, - "timestamp": 0.24561464201466432 - }, - { - "x": 3.6159951636151835, - "y": 6.38199294529669, - "heading": 0.26272711898900253, - "angularVelocity": 0.02647256504622092, - "velocityX": -1.3069101310543174, - "velocityY": -0.5772882951369854, - "timestamp": 0.32748618935288576 - }, - { - "x": 3.4820378233980955, - "y": 6.3233976602765205, - "heading": 0.2655861022623468, - "angularVelocity": 0.03492035226271501, - "velocityX": -1.6361891838161302, - "velocityY": -0.7156977842143984, - "timestamp": 0.4093577366911072 - }, - { - "x": 3.3209367433633594, - "y": 6.253922161955361, - "heading": 0.26927508652336807, - "angularVelocity": 0.04505819641812337, - "velocityX": -1.9677297580465591, - "velocityY": -0.8485914897167804, - "timestamp": 0.49122928402932864 - }, - { - "x": 3.132337992654885, - "y": 6.1744965280297155, - "heading": 0.27407536868463933, - "angularVelocity": 0.0586318729440976, - "velocityX": -2.303593334193982, - "velocityY": -0.9701249885693223, - "timestamp": 0.5731008313675501 - }, - { - "x": 2.9153402053678152, - "y": 6.088110219485557, - "heading": 0.2808618012562985, - "angularVelocity": 0.08289122158182176, - "velocityX": -2.650466423831284, - "velocityY": -1.0551444470358684, - "timestamp": 0.6549723787057715 - }, - { - "x": 2.6980153975230565, - "y": 6.030900368674856, - "heading": 0.29585281339155545, - "angularVelocity": 0.1831040529053057, - "velocityX": -2.6544607364871555, - "velocityY": -0.6987757367569974, - "timestamp": 0.736843926043993 - }, - { - "x": 2.5044902704926533, - "y": 5.990681526275272, - "heading": 0.31201148477172486, - "angularVelocity": 0.1973661412971212, - "velocityX": -2.3637653534374596, - "velocityY": -0.4912432182750236, - "timestamp": 0.8187154733822144 - }, - { - "x": 2.3360827419423367, - "y": 5.965439222430923, - "heading": 0.328645437097879, - "angularVelocity": 0.20317134422093217, - "velocityX": -2.0569725872482194, - "velocityY": -0.30831594937456525, - "timestamp": 0.9005870207204358 - }, - { - "x": 2.1932220965526215, - "y": 5.95442921664246, - "heading": 0.34550838612461254, - "angularVelocity": 0.2059683684378234, - "velocityX": -1.744936428275136, - "velocityY": -0.13447902411053952, - "timestamp": 0.9824585680586573 - }, - { - "x": 2.076119719939017, - "y": 5.957265361506873, - "heading": 0.3624743968160351, - "angularVelocity": 0.20722718994590164, - "velocityX": -1.4303183513784072, - "velocityY": 0.03464139810008477, - "timestamp": 1.0643301153968787 - }, - { - "x": 1.984901205969126, - "y": 5.973711453340982, - "heading": 0.37946704953669896, - "angularVelocity": 0.20755260249895005, - "velocityX": -1.1141662388895142, - "velocityY": 0.20087676816670857, - "timestamp": 1.1462016627351002 - }, - { - "x": 1.9196497213527057, - "y": 6.003608138127345, - "heading": 0.39643505242128974, - "angularVelocity": 0.207251523102328, - "velocityX": -0.7969983069558729, - "velocityY": 0.3651657475442049, - "timestamp": 1.2280732100733216 - }, - { - "x": 1.8804243803024292, - "y": 6.046840667724609, - "heading": 0.41334160220463595, - "angularVelocity": 0.20650091922048514, - "velocityX": -0.4791083389230676, - "velocityY": 0.5280531638014023, - "timestamp": 1.309944757411543 - }, - { - "x": 1.8671378435769306, - "y": 6.1025986442210955, - "heading": 0.4299850055686104, - "angularVelocity": 0.20542336557580967, - "velocityX": -0.16399080352197462, - "velocityY": 0.6882000597544763, - "timestamp": 1.3909647685350217 - }, - { - "x": 1.8794728316218305, - "y": 6.171151657888672, - "heading": 0.4464831819952248, - "angularVelocity": 0.20363088325759873, - "velocityX": 0.1522461904639937, - "velocityY": 0.8461244662518951, - "timestamp": 1.4719847796585004 - }, - { - "x": 1.9175617070085949, - "y": 6.252231071793212, - "heading": 0.46275009292634184, - "angularVelocity": 0.2007764588716919, - "velocityX": 0.4701168866629144, - "velocityY": 1.0007331865330475, - "timestamp": 1.553004790781979 - }, - { - "x": 1.9816154560716552, - "y": 6.34539325484771, - "heading": 0.47864458300524776, - "angularVelocity": 0.19617980618000597, - "velocityX": 0.7905917090709768, - "velocityY": 1.1498663325596743, - "timestamp": 1.6340248019054577 - }, - { - "x": 2.0720214338634078, - "y": 6.449767500665198, - "heading": 0.4938927703980073, - "angularVelocity": 0.18820273141558316, - "velocityX": 1.115847511474288, - "velocityY": 1.2882526720271141, - "timestamp": 1.7150448130289364 - }, - { - "x": 2.1896944819931505, - "y": 6.562891621237937, - "heading": 0.5077401370806992, - "angularVelocity": 0.17091292003882794, - "velocityX": 1.4523948651451515, - "velocityY": 1.3962491365291643, - "timestamp": 1.796064824152415 - }, - { - "x": 2.3309802432224416, - "y": 6.659921406601519, - "heading": 0.5136983140040886, - "angularVelocity": 0.07353957177701248, - "velocityX": 1.7438378404313606, - "velocityY": 1.1976027158982174, - "timestamp": 1.8770848352758938 - }, - { - "x": 2.4512963274996546, - "y": 6.737453803425126, - "heading": 0.5174619765612319, - "angularVelocity": 0.04645349346357551, - "velocityX": 1.4850168817410538, - "velocityY": 0.9569536679703065, - "timestamp": 1.9581048463993724 - }, - { - "x": 2.5483430242698013, - "y": 6.798294060832995, - "heading": 0.5200641230005525, - "angularVelocity": 0.03211733006744314, - "velocityX": 1.197811447128081, - "velocityY": 0.7509287713518723, - "timestamp": 2.039124857522851 - }, - { - "x": 2.62147124688145, - "y": 6.843385043467462, - "heading": 0.5218324598046723, - "angularVelocity": 0.02182592645444906, - "velocityX": 0.9025945762979182, - "velocityY": 0.556541303922395, - "timestamp": 2.12014486864633 - }, - { - "x": 2.6703780246763387, - "y": 6.8731963037812145, - "heading": 0.5229271765337971, - "angularVelocity": 0.013511683273610298, - "velocityX": 0.6036382508063606, - "velocityY": 0.367949348566719, - "timestamp": 2.2011648797698085 - }, - { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, - "angularVelocity": 0.006368644721814939, - "velocityX": 0.30252191195191025, - "velocityY": 0.18282262031723026, - "timestamp": 2.282184890893287 - }, - { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, - "angularVelocity": -2.018007055520513e-33, - "velocityX": -1.0144673376047062e-35, - "velocityY": 0, - "timestamp": 2.363204902016766 - } - ] + "samples": [ + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": -1.8368418613420646e-25, + "velocityX": -1.6872129777974468e-25, + "velocityY": 1.375029629161337e-25, + "timestamp": 0 + }, + { + "x": 3.856532695000803, + "y": 6.488812456981308, + "heading": 0.2579986728633844, + "angularVelocity": 0.005949010101817782, + "velocityX": -0.32577575413793, + "velocityY": -0.14647295262485988, + "timestamp": 0.0818715475429368 + }, + { + "x": 3.8031475649179134, + "y": 6.464921556304128, + "heading": 0.2590017936055681, + "angularVelocity": 0.012252373043976347, + "velocityX": -0.6520596188180307, + "velocityY": -0.29180956503419114, + "timestamp": 0.1637430950858736 + }, + { + "x": 3.7229939182763383, + "y": 6.429256431280001, + "heading": 0.2605597691269097, + "angularVelocity": 0.019029511082889358, + "velocityX": -0.979017119464356, + "velocityY": -0.43562294954083813, + "timestamp": 0.2456146426288104 + }, + { + "x": 3.615995163614758, + "y": 6.38199294529701, + "heading": 0.26272711898941353, + "angularVelocity": 0.026472564981957637, + "velocityX": -1.3069101277884805, + "velocityY": -0.577288293692067, + "timestamp": 0.3274861901717472 + }, + { + "x": 3.4820378233974707, + "y": 6.323397660276972, + "heading": 0.26558610226294477, + "angularVelocity": 0.03492035217768224, + "velocityX": -1.6361891797273596, + "velocityY": -0.7156977824232311, + "timestamp": 0.409357737714684 + }, + { + "x": 3.320936743362508, + "y": 6.253922161955944, + "heading": 0.26927508652417376, + "angularVelocity": 0.045058196307995436, + "velocityX": -1.967729753129128, + "velocityY": -0.8485914875933167, + "timestamp": 0.4912292852576208 + }, + { + "x": 3.1323379926537895, + "y": 6.1744965280304065, + "heading": 0.2740753686856588, + "angularVelocity": 0.0586318728001031, + "velocityX": -2.3035933284369614, + "velocityY": -0.9701249861422718, + "timestamp": 0.5731008328005576 + }, + { + "x": 2.9153402053664754, + "y": 6.088110219486258, + "heading": 0.2808618012574976, + "angularVelocity": 0.0828912213767512, + "velocityX": -2.650466417206927, + "velocityY": -1.0551444443974112, + "timestamp": 0.6549723803434944 + }, + { + "x": 2.698015397521732, + "y": 6.030900368675538, + "heading": 0.2958528133927384, + "angularVelocity": 0.18310405244726724, + "velocityX": -2.6544607298496428, + "velocityY": -0.6987757350099817, + "timestamp": 0.7368439278864312 + }, + { + "x": 2.504490270491426, + "y": 5.9906815262758615, + "heading": 0.3120114847728296, + "angularVelocity": 0.19736614080266282, + "velocityX": -2.363765347525813, + "velocityY": -0.49124321704783075, + "timestamp": 0.818715475429368 + }, + { + "x": 2.336082741941251, + "y": 5.965439222431395, + "heading": 0.3286454370988765, + "angularVelocity": 0.20317134371160356, + "velocityX": -2.0569725821031444, + "velocityY": -0.30831594860508243, + "timestamp": 0.9005870229723048 + }, + { + "x": 2.1932220965517097, + "y": 5.954429216642808, + "heading": 0.3455083861254867, + "angularVelocity": 0.20596836792130419, + "velocityX": -1.744936423909899, + "velocityY": -0.1344790237757827, + "timestamp": 0.9824585705152415 + }, + { + "x": 2.0761197199383044, + "y": 5.957265361507106, + "heading": 0.36247439681677673, + "angularVelocity": 0.20722718942612423, + "velocityX": -1.4303183477995456, + "velocityY": 0.034641398012055055, + "timestamp": 1.0643301180581783 + }, + { + "x": 1.9849012059686333, + "y": 5.973711453341115, + "heading": 0.37946704953730365, + "angularVelocity": 0.20755260197830386, + "velocityX": -1.1141662361009146, + "velocityY": 0.2008767676632038, + "timestamp": 1.1462016656011151 + }, + { + "x": 1.9196497213524515, + "y": 6.003608138127398, + "heading": 0.39643505242175614, + "angularVelocity": 0.20725152258241855, + "velocityX": -0.7969983049601116, + "velocityY": 0.36516574663016305, + "timestamp": 1.228073213144052 + }, + { + "x": 1.8804243803024292, + "y": 6.046840667724609, + "heading": 0.41334160220496513, + "angularVelocity": 0.2065009187024656, + "velocityX": -0.47910833772197736, + "velocityY": 0.5280531624803828, + "timestamp": 1.3099447606869887 + }, + { + "x": 1.8671378435771844, + "y": 6.102598644221004, + "heading": 0.42998500556879043, + "angularVelocity": 0.2054233650605208, + "velocityX": -0.1639908031089557, + "velocityY": 0.6882000580332149, + "timestamp": 1.3909647720129739 + }, + { + "x": 1.8794728316222993, + "y": 6.171151657888497, + "heading": 0.44648318199526155, + "angularVelocity": 0.2036308827468621, + "velocityX": 0.15224619008611265, + "velocityY": 0.8461244641360118, + "timestamp": 1.471984783338959 + }, + { + "x": 1.9175617070092363, + "y": 6.252231071792971, + "heading": 0.462750092926245, + "angularVelocity": 0.2007764583682102, + "velocityX": 0.4701168854900097, + "velocityY": 1.000733184030919, + "timestamp": 1.5530047946649441 + }, + { + "x": 1.9816154560724233, + "y": 6.345393254847428, + "heading": 0.47864458300503304, + "angularVelocity": 0.1961798056882073, + "velocityX": 0.7905917070964839, + "velocityY": 1.1498663296851208, + "timestamp": 1.6340248059909293 + }, + { + "x": 2.0720214338642493, + "y": 6.44976750066492, + "heading": 0.4938927703977008, + "angularVelocity": 0.1882027309440445, + "velocityX": 1.1158475086861754, + "velocityY": 1.288252668807229, + "timestamp": 1.7150448173169144 + }, + { + "x": 2.189694481994004, + "y": 6.562891621237751, + "heading": 0.5077401370803508, + "angularVelocity": 0.17091291961111932, + "velocityX": 1.452394861515095, + "velocityY": 1.3962491330404174, + "timestamp": 1.7960648286428995 + }, + { + "x": 2.330980243223138, + "y": 6.6599214066012715, + "heading": 0.5136983140037735, + "angularVelocity": 0.07353957159361435, + "velocityX": 1.7438378360707576, + "velocityY": 1.197602712904113, + "timestamp": 1.8770848399688846 + }, + { + "x": 2.4512963275001454, + "y": 6.737453803424918, + "heading": 0.5174619765609972, + "angularVelocity": 0.04645349334845922, + "velocityX": 1.48501687802677, + "velocityY": 0.9569536655789208, + "timestamp": 1.9581048512948698 + }, + { + "x": 2.548343024270106, + "y": 6.798294060832851, + "heading": 0.5200641230004012, + "angularVelocity": 0.032117329988194755, + "velocityX": 1.1978114441319052, + "velocityY": 0.7509287694757523, + "timestamp": 2.039124862620855 + }, + { + "x": 2.6214712468816064, + "y": 6.843385043467383, + "heading": 0.5218324598045924, + "angularVelocity": 0.021825926400778378, + "velocityX": 0.9025945740400864, + "velocityY": 0.5565413025321349, + "timestamp": 2.12014487394684 + }, + { + "x": 2.6703780246763915, + "y": 6.873196303781186, + "heading": 0.5229271765337693, + "angularVelocity": 0.01351168324048117, + "velocityX": 0.6036382492963173, + "velocityY": 0.367949347647667, + "timestamp": 2.201164885272825 + }, + { + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": 0.006368644706239802, + "velocityX": 0.30252191119511246, + "velocityY": 0.1828226198606205, + "timestamp": 2.2821848965988103 + }, + { + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": 4.786924324290607e-29, + "velocityX": -3.940142843909209e-27, + "velocityY": 5.40414297722828e-27, + "timestamp": 2.3632049079247954 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBDA.traj b/src/main/deploy/choreo/CenterLanePBDA.traj index cb20734f..9fab5e0f 100644 --- a/src/main/deploy/choreo/CenterLanePBDA.traj +++ b/src/main/deploy/choreo/CenterLanePBDA.traj @@ -1,886 +1,887 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 2.2921355473422064e-38, - "velocityX": 1.1360383232068741e-38, - "velocityY": -2.144062321628808e-38, - "timestamp": 0 - }, - { - "x": 1.32825806778149, - "y": 5.589895062873393, - "heading": -2.8062774074031584e-19, - "angularVelocity": -2.995198561755265e-18, - "velocityX": 0.40860904707084805, - "velocityY": -0.011305499292899219, - "timestamp": 0.09369253321938388 - }, - { - "x": 1.4048253002214361, - "y": 5.587776581164742, - "heading": -1.768868342127426e-19, - "angularVelocity": 1.1072483896415275e-18, - "velocityX": 0.8172180835441991, - "velocityY": -0.022610998292584186, - "timestamp": 0.18738506643876776 - }, - { - "x": 1.519676146824621, - "y": 5.584598858658671, - "heading": -2.386873521248768e-19, - "angularVelocity": -6.596098514057519e-19, - "velocityX": 1.225827103364341, - "velocityY": -0.03391649683150392, - "timestamp": 0.28107759965815166 - }, - { - "x": 1.6728106047825377, - "y": 5.580361895432887, - "heading": -7.795008789281329e-19, - "angularVelocity": -5.772215866345043e-18, - "velocityX": 1.6344360932087083, - "velocityY": -0.04522199454104627, - "timestamp": 0.3747701328775355 - }, - { - "x": 1.8642286675420066, - "y": 5.575065691668706, - "heading": -1.0638718286937245e-18, - "angularVelocity": -3.0351506143991226e-18, - "velocityX": 2.0430450131096105, - "velocityY": -0.056527490315375004, - "timestamp": 0.4684626660969194 - }, - { - "x": 2.0939303023371445, - "y": 5.5687102482727004, - "heading": -1.0113200205360793e-18, - "angularVelocity": 5.608964381023318e-19, - "velocityX": 2.451653583293393, - "velocityY": -0.0678329764136413, - "timestamp": 0.5621551993163033 - }, - { - "x": 2.285348365096613, - "y": 5.563414044508519, - "heading": -7.732946400687193e-19, - "angularVelocity": 2.5404946615264457e-18, - "velocityX": 2.04304501310961, - "velocityY": -0.056527490315375004, - "timestamp": 0.6558477325356872 - }, - { - "x": 2.43848282305453, - "y": 5.559177081282735, - "heading": -4.312009437616493e-19, - "angularVelocity": 3.651237559230762e-18, - "velocityX": 1.634436093208708, - "velocityY": -0.04522199454104628, - "timestamp": 0.749540265755071 - }, - { - "x": 2.553333669657715, - "y": 5.555999358776664, - "heading": -7.015602955490743e-20, - "angularVelocity": 3.853507870950435e-18, - "velocityX": 1.2258271033643409, - "velocityY": -0.033916496831503926, - "timestamp": 0.8432327989744549 - }, - { - "x": 2.6299009020976607, - "y": 5.553880877068013, - "heading": -2.7026882274997263e-20, - "angularVelocity": 4.603264080766157e-19, - "velocityX": 0.817218083544199, - "velocityY": -0.022610998292584186, - "timestamp": 0.9369253321938388 - }, - { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, - "angularVelocity": 2.884635663770645e-19, - "velocityX": 0.40860904707084805, - "velocityY": -0.011305499292899219, - "timestamp": 1.0306178654132228 - }, - { - "x": 2.668184518814087, - "y": 5.552821636199951, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.1410946614987667e-37, - "timestamp": 1.1243103986326066 - }, - { - "x": 2.687410064097537, - "y": 5.560258515634833, - "heading": 0.0028176822125938034, - "angularVelocity": 0.04098002734374148, - "velocityX": 0.2796139919160097, - "velocityY": 0.1081610698436357, - "timestamp": 1.1930678494014684 - }, - { - "x": 2.7258611531960137, - "y": 5.575132263104083, - "heading": 0.008453116109374614, - "angularVelocity": 0.08196106507388369, - "velocityX": 0.5592279624754555, - "velocityY": 0.2163219738796251, - "timestamp": 1.2618253001703301 - }, - { - "x": 2.783537789303066, - "y": 5.597442863001631, - "heading": 0.016905769734324238, - "angularVelocity": 0.12293436610040698, - "velocityX": 0.8388419794814749, - "velocityY": 0.3244826509427688, - "timestamp": 1.3305827509391919 - }, - { - "x": 2.8604399802132514, - "y": 5.627190295278175, - "heading": 0.028174540960364546, - "angularVelocity": 0.1638916379247738, - "velocityX": 1.118456109850604, - "velocityY": 0.4326430364113507, - "timestamp": 1.3993402017080536 - }, - { - "x": 2.9565677382168056, - "y": 5.664374535134068, - "heading": 0.042257801458542396, - "angularVelocity": 0.20482522753092394, - "velocityX": 1.3980704189674162, - "velocityY": 0.5408030611968617, - "timestamp": 1.4680976524769154 - }, - { - "x": 3.0719210799387406, - "y": 5.708995552649304, - "heading": 0.05915345386800317, - "angularVelocity": 0.2457283133759373, - "velocityX": 1.6776849698763825, - "velocityY": 0.648962650829455, - "timestamp": 1.5368551032457771 - }, - { - "x": 3.2065000261043313, - "y": 5.761053312357379, - "heading": 0.07885900262196403, - "angularVelocity": 0.2865951040012339, - "velocityX": 1.9572998222112317, - "velocityY": 0.757121724641519, - "timestamp": 1.6056125540146389 - }, - { - "x": 3.360304601198153, - "y": 5.820547772766793, - "heading": 0.1013716386695389, - "angularVelocity": 0.32742104013214957, - "velocityX": 2.2369150306467707, - "velocityY": 0.8652801950062525, - "timestamp": 1.6743700047835006 - }, - { - "x": 3.5333348329448953, - "y": 5.8874788858214675, - "heading": 0.12668833767209217, - "angularVelocity": 0.36820299064984424, - "velocityX": 2.5165306423068015, - "velocityY": 0.973437966449241, - "timestamp": 1.7431274555523624 - }, - { - "x": 3.7255907514118967, - "y": 5.961846596243587, - "heading": 0.15480596913918515, - "angularVelocity": 0.4089394116954055, - "velocityX": 2.796146691262561, - "velocityY": 1.0815949339384094, - "timestamp": 1.8118849063212241 - }, - { - "x": 3.937072386955544, - "y": 6.04365084047756, - "heading": 0.18572140510730675, - "angularVelocity": 0.44963034002016317, - "velocityX": 3.075763181717921, - "velocityY": 1.1897509770827353, - "timestamp": 1.8806423570900859 - }, - { - "x": 4.167779761570079, - "y": 6.132891543122838, - "heading": 0.21943155266112502, - "angularVelocity": 0.4902762853605699, - "velocityX": 3.355379992054849, - "velocityY": 1.2979059236107295, - "timestamp": 1.9493998078589476 - }, - { - "x": 4.410308649457132, - "y": 6.2266961328710915, - "heading": 0.21943155790785696, - "angularVelocity": 7.630783127279499e-8, - "velocityX": 3.5273106430654275, - "velocityY": 1.3642825424635276, - "timestamp": 2.0181572586278094 - }, - { - "x": 4.652837676050608, - "y": 6.320500363997887, - "heading": 0.21943156315423873, - "angularVelocity": 7.630273806741925e-8, - "velocityX": 3.5273126603947684, - "velocityY": 1.3642773267166644, - "timestamp": 2.0869147093966713 - }, - { - "x": 4.897946357727051, - "y": 6.407341003417969, - "heading": 0.21943156841217684, - "angularVelocity": 7.647081243446701e-8, - "velocityX": 3.5648308501199604, - "velocityY": 1.2629996960185401, - "timestamp": 2.1556721601655333 - }, - { - "x": 5.1308682473529785, - "y": 6.471009903235158, - "heading": 0.21943157303192085, - "angularVelocity": 7.23563158909772e-8, - "velocityX": 3.6481177055177683, - "velocityY": 0.9972082962532426, - "timestamp": 2.2195193022886155 - }, - { - "x": 5.365529567427774, - "y": 6.527934417793533, - "heading": 0.21943157763716262, - "angularVelocity": 7.21291764013049e-8, - "velocityX": 3.6753613751798433, - "velocityY": 0.8915749815181846, - "timestamp": 2.2833664444116977 - }, - { - "x": 5.600190991292763, - "y": 6.584858504493345, - "heading": 0.21943158224240422, - "angularVelocity": 7.21291734995501e-8, - "velocityX": 3.6753630007842206, - "velocityY": 0.8915682802227483, - "timestamp": 2.34721358653478 - }, - { - "x": 5.8348524151632635, - "y": 6.641782591170481, - "heading": 0.2194315868476458, - "angularVelocity": 7.212917336838375e-8, - "velocityX": 3.6753630008705414, - "velocityY": 0.8915682798675781, - "timestamp": 2.411060728657862 - }, - { - "x": 6.069513839029734, - "y": 6.698706677864191, - "heading": 0.21943159145288735, - "angularVelocity": 7.212917280605725e-8, - "velocityX": 3.6753630008074327, - "velocityY": 0.891568280127171, - "timestamp": 2.474907870780944 - }, - { - "x": 6.3041751870414915, - "y": 6.75563107725722, - "heading": 0.21943159605812906, - "angularVelocity": 7.212917561549481e-8, - "velocityX": 3.6753618127399528, - "velocityY": 0.8915731777514998, - "timestamp": 2.5387550129040264 - }, - { - "x": 6.537489271202827, - "y": 6.817847413489264, - "heading": 0.21943160514124108, - "angularVelocity": 1.4226340838423697e-7, - "velocityX": 3.6542604164106804, - "velocityY": 0.9744576524998471, - "timestamp": 2.6026021550271086 - }, - { - "x": 6.7545327794045615, - "y": 6.881509207420976, - "heading": 0.24890077922126197, - "angularVelocity": 0.46155823268064305, - "velocityX": 3.399424014677561, - "velocityY": 0.9970970009742466, - "timestamp": 2.6664492971501907 - }, - { - "x": 6.954212307545749, - "y": 6.941327014961267, - "heading": 0.2778274290995393, - "angularVelocity": 0.45306099719431336, - "velocityX": 3.1274622716276532, - "velocityY": 0.9368909171372027, - "timestamp": 2.730296439273273 - }, - { - "x": 7.1365689728963595, - "y": 6.997132012930522, - "heading": 0.305376589845945, - "angularVelocity": 0.43148620017020184, - "velocityX": 2.856144524042602, - "velocityY": 0.8740406557535503, - "timestamp": 2.794143581396355 - }, - { - "x": 7.301617943292834, - "y": 7.048866027505986, - "heading": 0.33125825392103403, - "angularVelocity": 0.40536918669284566, - "velocityX": 2.585064341302851, - "velocityY": 0.8102792522134878, - "timestamp": 2.8579907235194373 - }, - { - "x": 7.449367094221676, - "y": 7.096499619737108, - "heading": 0.35532461486989275, - "angularVelocity": 0.37693716818945566, - "velocityX": 2.3141075076472033, - "velocityY": 0.7460567638139751, - "timestamp": 2.9218378656425195 - }, - { - "x": 7.579821246213137, - "y": 7.140015020198173, - "heading": 0.3774857294479878, - "angularVelocity": 0.347096421878647, - "velocityX": 2.04322617510391, - "velocityY": 0.681555963416076, - "timestamp": 2.9856850077656016 - }, - { - "x": 7.69298365301254, - "y": 7.179400342359022, - "heading": 0.3976808814927073, - "angularVelocity": 0.3163047142468991, - "velocityX": 1.7723958040479335, - "velocityY": 0.6168689913312067, - "timestamp": 3.049532149888684 - }, - { - "x": 7.788856658439436, - "y": 7.214647078189013, - "heading": 0.41586620005922853, - "angularVelocity": 0.2848258819708253, - "velocityX": 1.5016021428504343, - "velocityY": 0.5520487629977263, - "timestamp": 3.113379292011766 - }, - { - "x": 7.867442031134741, - "y": 7.2457488378850705, - "heading": 0.43200844627235874, - "angularVelocity": 0.2528264488645923, - "velocityX": 1.2308361828288252, - "velocityY": 0.48712845496042256, - "timestamp": 3.177226434134848 - }, - { - "x": 7.928741153018892, - "y": 7.27270064652237, - "heading": 0.4460815614823235, - "angularVelocity": 0.22041887454940703, - "velocityX": 0.9600918670092122, - "velocityY": 0.4221302276198955, - "timestamp": 3.2410735762579304 - }, - { - "x": 7.9727551334716695, - "y": 7.29549852060884, - "heading": 0.45806460123681797, - "angularVelocity": 0.1876832596734667, - "velocityX": 0.6893649267484526, - "velocityY": 0.35706960920065395, - "timestamp": 3.3049207183810125 - }, - { - "x": 7.999484882530383, - "y": 7.314139197912392, - "heading": 0.4679404256802237, - "angularVelocity": 0.1546791933829663, - "velocityX": 0.4186522398635212, - "velocityY": 0.2919578963709263, - "timestamp": 3.3687678605040947 - }, - { - "x": 8.008931159973145, - "y": 7.328619956970215, - "heading": 0.4756948301053548, - "angularVelocity": 0.12145264717068183, - "velocityX": 0.1479514529335022, - "velocityY": 0.22680355887981873, - "timestamp": 3.432615002627177 - }, - { - "x": 7.9838028545378235, - "y": 7.340994975747599, - "heading": 0.4825391441548679, - "angularVelocity": 0.07100818067139564, - "velocityX": -0.26070037689812714, - "velocityY": 0.12838796741348574, - "timestamp": 3.5290026861982695 - }, - { - "x": 7.919285560188609, - "y": 7.343883953710174, - "heading": 0.4845188810802152, - "angularVelocity": 0.02053931427751041, - "velocityX": -0.6693520578449088, - "velocityY": 0.029972480461632738, - "timestamp": 3.625390369769362 - }, - { - "x": 7.815379289936097, - "y": 7.337286883355744, - "heading": 0.4816330205236378, - "angularVelocity": -0.029940138092932247, - "velocityX": -1.0780036038097405, - "velocityY": -0.06844308432376209, - "timestamp": 3.7217780533404548 - }, - { - "x": 7.6720840556149295, - "y": 7.321203739342901, - "heading": 0.47388185121605153, - "angularVelocity": -0.08041659494667612, - "velocityX": -1.4866550269929244, - "velocityY": -0.16685891202042144, - "timestamp": 3.8181657369115474 - }, - { - "x": 7.489399868443907, - "y": 7.295634478019173, - "heading": 0.46126687890088164, - "angularVelocity": -0.13087743005945188, - "velocityX": -1.895306333783606, - "velocityY": -0.2652751926014197, - "timestamp": 3.91455342048264 - }, - { - "x": 7.267326740077932, - "y": 7.2605790366684495, - "heading": 0.44379066346613116, - "angularVelocity": -0.1813117069242423, - "velocityX": -2.3039575196573705, - "velocityY": -0.36369212384761535, - "timestamp": 4.010941104053733 - }, - { - "x": 7.00586468444221, - "y": 7.216037332217007, - "heading": 0.4214565844390286, - "angularVelocity": -0.23171092197300747, - "velocityX": -2.7126085610603474, - "velocityY": -0.4621099169645314, - "timestamp": 4.107328787624826 - }, - { - "x": 6.7050137215236445, - "y": 7.162009258221821, - "heading": 0.3942685380548319, - "angularVelocity": -0.2820697144790415, - "velocityX": -3.1212593951037992, - "velocityY": -0.5605288144032944, - "timestamp": 4.203716471195919 - }, - { - "x": 6.364773891396992, - "y": 7.098494671595923, - "heading": 0.3622306012435557, - "angularVelocity": -0.3323862097758952, - "velocityX": -3.529909813380894, - "velocityY": -0.658949196336373, - "timestamp": 4.300104154767012 - }, - { - "x": 6.010374312951181, - "y": 7.013136980735367, - "heading": 0.36223059672201996, - "angularVelocity": -4.690989126354215e-8, - "velocityX": -3.6768139384158545, - "velocityY": -0.8855663680059631, - "timestamp": 4.396491838338105 - }, - { - "x": 5.655974859532265, - "y": 6.927778770772817, - "heading": 0.36223059220056536, - "angularVelocity": -4.690904952415849e-8, - "velocityX": -3.676812641290635, - "velocityY": -0.885571753569461, - "timestamp": 4.492879521909198 - }, - { - "x": 5.301574928644714, - "y": 6.842422689744232, - "heading": 0.3622305506125328, - "angularVelocity": -4.314662515297724e-7, - "velocityX": -3.6768175949176816, - "velocityY": -0.8855496663702899, - "timestamp": 4.589267205480291 - }, - { - "x": 4.986369114322999, - "y": 6.766504438890574, - "heading": 0.34532942408176365, - "angularVelocity": -0.1753452921016411, - "velocityX": -3.270187669664538, - "velocityY": -0.7876343536948325, - "timestamp": 4.685654889051384 - }, - { - "x": 4.710566832932512, - "y": 6.70007668327783, - "heading": 0.3264669766789321, - "angularVelocity": -0.1956935440711402, - "velocityX": -2.861385097890258, - "velocityY": -0.6891726530988762, - "timestamp": 4.782042572622477 - }, - { - "x": 4.474170501345758, - "y": 6.643140019477641, - "heading": 0.30840376738356945, - "angularVelocity": -0.18740163292794487, - "velocityX": -2.4525574516208333, - "velocityY": -0.5907047632096585, - "timestamp": 4.87843025619357 - }, - { - "x": 4.277177710352041, - "y": 6.5956938222588235, - "heading": 0.2923060351860776, - "angularVelocity": -0.16701026107365813, - "velocityX": -2.0437548003570405, - "velocityY": -0.4922433599498598, - "timestamp": 4.974817939764663 - }, - { - "x": 4.1195862527073634, - "y": 6.557737539015946, - "heading": 0.27881764120910085, - "angularVelocity": -0.1399389784798502, - "velocityX": -1.6349750487409798, - "velocityY": -0.3937876898440308, - "timestamp": 5.071205623335756 - }, - { - "x": 4.0013944038572795, - "y": 6.529270748726535, - "heading": 0.26834692670698207, - "angularVelocity": -0.10863124949357753, - "velocityX": -1.226213188979785, - "velocityY": -0.2953363877493204, - "timestamp": 5.167593306906849 - }, - { - "x": 3.9226008250519366, - "y": 6.510293128915594, - "heading": 0.26117603215990287, - "angularVelocity": -0.07439637805789222, - "velocityX": -0.817465218439728, - "velocityY": -0.19688843125838204, - "timestamp": 5.263980990477942 - }, - { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "angularVelocity": -0.038017450198398375, - "velocityX": -0.40872820518438957, - "velocityY": -0.09844312341738799, - "timestamp": 5.3603686740490355 - }, - { - "x": 3.883204460144043, - "y": 6.500804424285889, - "heading": 0.2575116182, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.0638409913121086e-32, - "timestamp": 5.456756357620129 - }, - { - "x": 3.856532695000848, - "y": 6.488812456981272, - "heading": 0.25799867286334044, - "angularVelocity": 0.005949010116156059, - "velocityX": -0.32577575495196986, - "velocityY": -0.14647295299154703, - "timestamp": 5.53862790495835 - }, - { - "x": 3.8031475649180457, - "y": 6.464921556304023, - "heading": 0.25900179360543873, - "angularVelocity": 0.01225237307357005, - "velocityX": -0.6520596204474033, - "velocityY": -0.29180956576467965, - "timestamp": 5.620499452296571 - }, - { - "x": 3.7229939182765985, - "y": 6.429256431279799, - "heading": 0.2605597691266567, - "angularVelocity": 0.019029511128961094, - "velocityX": -0.9790171219107733, - "velocityY": -0.43562295063126366, - "timestamp": 5.702370999634793 - }, - { - "x": 3.6159951636151835, - "y": 6.38199294529669, - "heading": 0.26272711898900253, - "angularVelocity": 0.02647256504622092, - "velocityX": -1.3069101310543174, - "velocityY": -0.5772882951369854, - "timestamp": 5.784242546973014 - }, - { - "x": 3.4820378233980955, - "y": 6.3233976602765205, - "heading": 0.2655861022623468, - "angularVelocity": 0.03492035226271501, - "velocityX": -1.6361891838161302, - "velocityY": -0.7156977842143984, - "timestamp": 5.866114094311236 - }, - { - "x": 3.3209367433633594, - "y": 6.253922161955361, - "heading": 0.26927508652336807, - "angularVelocity": 0.04505819641812337, - "velocityX": -1.9677297580465591, - "velocityY": -0.8485914897167804, - "timestamp": 5.947985641649457 - }, - { - "x": 3.132337992654885, - "y": 6.1744965280297155, - "heading": 0.27407536868463933, - "angularVelocity": 0.0586318729440976, - "velocityX": -2.303593334193982, - "velocityY": -0.9701249885693223, - "timestamp": 6.029857188987679 - }, - { - "x": 2.9153402053678152, - "y": 6.088110219485557, - "heading": 0.2808618012562985, - "angularVelocity": 0.08289122158182176, - "velocityX": -2.650466423831284, - "velocityY": -1.0551444470358684, - "timestamp": 6.1117287363259 - }, - { - "x": 2.6980153975230565, - "y": 6.030900368674856, - "heading": 0.29585281339155545, - "angularVelocity": 0.1831040529053057, - "velocityX": -2.6544607364871555, - "velocityY": -0.6987757367569974, - "timestamp": 6.1936002836641215 - }, - { - "x": 2.5044902704926533, - "y": 5.990681526275272, - "heading": 0.31201148477172486, - "angularVelocity": 0.1973661412971212, - "velocityX": -2.3637653534374596, - "velocityY": -0.4912432182750236, - "timestamp": 6.275471831002343 - }, - { - "x": 2.3360827419423367, - "y": 5.965439222430923, - "heading": 0.328645437097879, - "angularVelocity": 0.20317134422093217, - "velocityX": -2.0569725872482194, - "velocityY": -0.30831594937456525, - "timestamp": 6.357343378340564 - }, - { - "x": 2.1932220965526215, - "y": 5.95442921664246, - "heading": 0.34550838612461254, - "angularVelocity": 0.2059683684378234, - "velocityX": -1.744936428275136, - "velocityY": -0.13447902411053952, - "timestamp": 6.439214925678786 - }, - { - "x": 2.076119719939017, - "y": 5.957265361506873, - "heading": 0.3624743968160351, - "angularVelocity": 0.20722718994590164, - "velocityX": -1.4303183513784072, - "velocityY": 0.03464139810008477, - "timestamp": 6.521086473017007 - }, - { - "x": 1.984901205969126, - "y": 5.973711453340982, - "heading": 0.37946704953669896, - "angularVelocity": 0.20755260249895005, - "velocityX": -1.1141662388895142, - "velocityY": 0.20087676816670857, - "timestamp": 6.602958020355229 - }, - { - "x": 1.9196497213527057, - "y": 6.003608138127345, - "heading": 0.39643505242128974, - "angularVelocity": 0.207251523102328, - "velocityX": -0.7969983069558729, - "velocityY": 0.3651657475442049, - "timestamp": 6.68482956769345 - }, - { - "x": 1.8804243803024292, - "y": 6.046840667724609, - "heading": 0.41334160220463595, - "angularVelocity": 0.20650091922048514, - "velocityX": -0.4791083389230676, - "velocityY": 0.5280531638014023, - "timestamp": 6.766701115031672 - }, - { - "x": 1.8671378435769306, - "y": 6.1025986442210955, - "heading": 0.4299850055686104, - "angularVelocity": 0.20542336557580967, - "velocityX": -0.16399080352197462, - "velocityY": 0.6882000597544763, - "timestamp": 6.84772112615515 - }, - { - "x": 1.8794728316218305, - "y": 6.171151657888672, - "heading": 0.4464831819952248, - "angularVelocity": 0.20363088325759873, - "velocityX": 0.1522461904639937, - "velocityY": 0.8461244662518951, - "timestamp": 6.928741137278629 - }, - { - "x": 1.9175617070085949, - "y": 6.252231071793212, - "heading": 0.46275009292634184, - "angularVelocity": 0.2007764588716919, - "velocityX": 0.4701168866629144, - "velocityY": 1.0007331865330475, - "timestamp": 7.009761148402108 - }, - { - "x": 1.9816154560716552, - "y": 6.34539325484771, - "heading": 0.47864458300524776, - "angularVelocity": 0.19617980618000597, - "velocityX": 0.7905917090709768, - "velocityY": 1.1498663325596743, - "timestamp": 7.090781159525586 - }, - { - "x": 2.0720214338634078, - "y": 6.449767500665198, - "heading": 0.4938927703980073, - "angularVelocity": 0.18820273141558316, - "velocityX": 1.115847511474288, - "velocityY": 1.2882526720271141, - "timestamp": 7.171801170649065 - }, - { - "x": 2.1896944819931505, - "y": 6.562891621237937, - "heading": 0.5077401370806992, - "angularVelocity": 0.17091292003882794, - "velocityX": 1.4523948651451515, - "velocityY": 1.3962491365291643, - "timestamp": 7.252821181772544 - }, - { - "x": 2.3309802432224416, - "y": 6.659921406601519, - "heading": 0.5136983140040886, - "angularVelocity": 0.07353957177701248, - "velocityX": 1.7438378404313606, - "velocityY": 1.1976027158982174, - "timestamp": 7.333841192896022 - }, - { - "x": 2.4512963274996546, - "y": 6.737453803425126, - "heading": 0.5174619765612319, - "angularVelocity": 0.04645349346357551, - "velocityX": 1.4850168817410538, - "velocityY": 0.9569536679703065, - "timestamp": 7.414861204019501 - }, - { - "x": 2.5483430242698013, - "y": 6.798294060832995, - "heading": 0.5200641230005525, - "angularVelocity": 0.03211733006744314, - "velocityX": 1.197811447128081, - "velocityY": 0.7509287713518723, - "timestamp": 7.49588121514298 - }, - { - "x": 2.62147124688145, - "y": 6.843385043467462, - "heading": 0.5218324598046723, - "angularVelocity": 0.02182592645444906, - "velocityX": 0.9025945762979182, - "velocityY": 0.556541303922395, - "timestamp": 7.576901226266458 - }, - { - "x": 2.6703780246763387, - "y": 6.8731963037812145, - "heading": 0.5229271765337971, - "angularVelocity": 0.013511683273610298, - "velocityX": 0.6036382508063606, - "velocityY": 0.367949348566719, - "timestamp": 7.657921237389937 - }, - { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, - "angularVelocity": 0.006368644721814939, - "velocityX": 0.30252191195191025, - "velocityY": 0.18282262031723026, - "timestamp": 7.738941248513416 - }, - { - "x": 2.6948883533477783, - "y": 6.8880085945129395, - "heading": 0.5234431642, - "angularVelocity": -2.018007055520513e-33, - "velocityX": -1.0144673376047062e-35, - "velocityY": 0, - "timestamp": 7.819961259636894 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.5131221782640854e-28, + "angularVelocity": 8.299873633167507e-29, + "velocityX": 6.262502855031199e-23, + "velocityY": -1.723228528384229e-24, + "timestamp": 0 + }, + { + "x": 1.3282580677814901, + "y": 5.589895062873393, + "heading": 3.410110874929926e-18, + "angularVelocity": 3.6396826395088883e-17, + "velocityX": 0.4086090460493266, + "velocityY": -0.011305499264635099, + "timestamp": 0.09369253345361556 + }, + { + "x": 1.4048253002214366, + "y": 5.587776581164742, + "heading": -6.538031746043414e-18, + "angularVelocity": -1.0617860628189422e-16, + "velocityX": 0.8172180815011558, + "velocityY": -0.022610998236055942, + "timestamp": 0.1873850669072311 + }, + { + "x": 1.5196761468246218, + "y": 5.584598858658671, + "heading": -1.263908620936914e-17, + "angularVelocity": -6.511782997625411e-17, + "velocityX": 1.2258271002997754, + "velocityY": -0.03391649674671154, + "timestamp": 0.28107760036084667 + }, + { + "x": 1.6728106047825395, + "y": 5.580361895432887, + "heading": -1.4189092760310783e-17, + "angularVelocity": -1.654354401527969e-17, + "velocityX": 1.6344360891226193, + "velocityY": -0.04522199442798972, + "timestamp": 0.3747701338144622 + }, + { + "x": 1.8642286675420086, + "y": 5.575065691668705, + "heading": -1.1778293764866163e-17, + "angularVelocity": 2.5730961757873988e-17, + "velocityX": 2.0430450080019944, + "velocityY": -0.05652749017405418, + "timestamp": 0.4684626672680778 + }, + { + "x": 2.0939303023371414, + "y": 5.568710248272701, + "heading": -1.2220695981018318e-17, + "angularVelocity": -4.721851357797737e-18, + "velocityX": 2.451653577164197, + "velocityY": -0.06783297624405477, + "timestamp": 0.5621552007216933 + }, + { + "x": 2.2853483650966107, + "y": 5.563414044508519, + "heading": -5.403675872721793e-18, + "angularVelocity": 7.275948100517659e-17, + "velocityX": 2.0430450080019944, + "velocityY": -0.056527490174054194, + "timestamp": 0.6558477341753088 + }, + { + "x": 2.438482823054528, + "y": 5.559177081282735, + "heading": -1.4049765598642364e-18, + "angularVelocity": 4.267895386440672e-17, + "velocityX": 1.6344360891226193, + "velocityY": -0.04522199442798973, + "timestamp": 0.7495402676289245 + }, + { + "x": 2.5533336696577136, + "y": 5.555999358776664, + "heading": -6.020213691896355e-19, + "angularVelocity": 8.570108640736073e-18, + "velocityX": 1.2258271002997754, + "velocityY": -0.033916496746711544, + "timestamp": 0.8432328010825401 + }, + { + "x": 2.6299009020976603, + "y": 5.553880877068013, + "heading": -2.9970702235685054e-18, + "angularVelocity": -2.5562857212265518e-17, + "velocityX": 0.8172180815011557, + "velocityY": -0.022610998236055942, + "timestamp": 0.9369253345361557 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": -2.3882654133259072e-26, + "angularVelocity": 3.198835690592465e-17, + "velocityX": 0.40860904604932646, + "velocityY": -0.0113054992646351, + "timestamp": 1.0306178679897713 + }, + { + "x": 2.668184518814087, + "y": 5.552821636199951, + "heading": -3.2765992030044065e-26, + "angularVelocity": -9.642870627859863e-26, + "velocityX": -6.242213516414814e-23, + "velocityY": 1.2639029431778876e-24, + "timestamp": 1.124310401443387 + }, + { + "x": 2.6874100640586183, + "y": 5.560258515785914, + "heading": 0.002817682314802049, + "angularVelocity": 0.040980028708864365, + "velocityX": 0.2796139905217839, + "velocityY": 0.10816107172058755, + "timestamp": 1.193067852415903 + }, + { + "x": 2.7258611530795362, + "y": 5.575132263557277, + "heading": 0.00845311638740209, + "angularVelocity": 0.08196106738821132, + "velocityX": 0.5592279596910537, + "velocityY": 0.21632197763277797, + "timestamp": 1.2618253033884193 + }, + { + "x": 2.783537789070723, + "y": 5.597442863907905, + "heading": 0.01690577022745014, + "angularVelocity": 0.12293436886464493, + "velocityX": 0.8388419753117596, + "velocityY": 0.32448265657123054, + "timestamp": 1.3305827543609354 + }, + { + "x": 2.860439979827146, + "y": 5.6271902967884255, + "heading": 0.0281745416659326, + "angularVelocity": 0.1638916405290653, + "velocityX": 1.118456104301514, + "velocityY": 0.4326430439140473, + "timestamp": 1.3993402053334516 + }, + { + "x": 2.956567737639549, + "y": 5.664374537399098, + "heading": 0.042257802321547584, + "angularVelocity": 0.20482522921398508, + "velocityX": 1.3980704120463603, + "velocityY": 0.5408030705724766, + "timestamp": 1.4680976563059678 + }, + { + "x": 3.0719210791335985, + "y": 5.708995555819805, + "heading": 0.05915345476625426, + "angularVelocity": 0.24572831316071073, + "velocityX": 1.6776849615928617, + "velocityY": 0.6489626620763342, + "timestamp": 1.536855107278484 + }, + { + "x": 3.206500025035437, + "y": 5.761053316583894, + "heading": 0.07885900334385978, + "angularVelocity": 0.28659510058745824, + "velocityX": 1.9572998125778749, + "velocityY": 0.7571217377575138, + "timestamp": 1.6056125582510001 + }, + { + "x": 3.3603045998308527, + "y": 5.820547778199662, + "heading": 0.10137163887853046, + "angularVelocity": 0.32742103170272313, + "velocityX": 2.2369150196812018, + "velocityY": 0.865280209988437, + "timestamp": 1.6743700092235163 + }, + { + "x": 3.533334831246345, + "y": 5.88747889261074, + "heading": 0.12668833684449327, + "angularVelocity": 0.36820297448319306, + "velocityX": 2.5165306300353754, + "velocityY": 0.9734379832933463, + "timestamp": 1.7431274601960325 + }, + { + "x": 3.725590749352267, + "y": 5.961846604538833, + "heading": 0.15480596643975877, + "angularVelocity": 0.4089393832605092, + "velocityX": 2.796146677729085, + "velocityY": 1.081594952637503, + "timestamp": 1.8118849111685487 + }, + { + "x": 3.9370723845110165, + "y": 6.043650850427416, + "heading": 0.18572139907813143, + "angularVelocity": 0.4496302902609083, + "velocityX": 3.075763167009828, + "velocityY": 1.189750997623249, + "timestamp": 1.8806423621410648 + }, + { + "x": 4.167779758734844, + "y": 6.1328915548731855, + "heading": 0.2194315399767765, + "angularVelocity": 0.4902761871163567, + "velocityX": 3.3553799764340586, + "velocityY": 1.2979059459525522, + "timestamp": 1.949399813113581 + }, + { + "x": 4.410308645337483, + "y": 6.226696146473074, + "heading": 0.2194315452235168, + "angularVelocity": 7.630795245724027e-8, + "velocityX": 3.5273106139374217, + "velocityY": 1.3642825653525794, + "timestamp": 2.0181572640860974 + }, + { + "x": 4.652837676496205, + "y": 6.320500364327338, + "heading": 0.2194315504698994, + "angularVelocity": 7.630275018427045e-8, + "velocityX": 3.5273127163435154, + "velocityY": 1.364277129641693, + "timestamp": 2.086914715058614 + }, + { + "x": 4.897946357727051, + "y": 6.407341003417969, + "heading": 0.21943155572783843, + "angularVelocity": 7.647082562756639e-8, + "velocityX": 3.564830833080488, + "velocityY": 1.2629996874861198, + "timestamp": 2.15567216603113 + }, + { + "x": 5.130868247940416, + "y": 6.471009903129215, + "heading": 0.21943156034758332, + "angularVelocity": 7.235632921744003e-8, + "velocityX": 3.648117688337217, + "velocityY": 0.9972082873825086, + "timestamp": 2.2195193086159217 + }, + { + "x": 5.365529564309454, + "y": 6.527934435249553, + "heading": 0.21943156495282606, + "angularVelocity": 7.212919079793877e-8, + "velocityX": 3.6753612905604753, + "velocityY": 0.891575250133395, + "timestamp": 2.2833664512007132 + }, + { + "x": 5.60019098861159, + "y": 6.584858522432942, + "heading": 0.21943156955806847, + "angularVelocity": 7.212918610327084e-8, + "velocityX": 3.675362981052711, + "velocityY": 0.8915682813493487, + "timestamp": 2.3472135937855048 + }, + { + "x": 5.834852412919734, + "y": 6.6417826095915675, + "heading": 0.21943157416331086, + "angularVelocity": 7.212918546345333e-8, + "velocityX": 3.675362981146796, + "velocityY": 0.8915682809614962, + "timestamp": 2.4110607363702963 + }, + { + "x": 6.069513837223475, + "y": 6.698706696768341, + "heading": 0.21943157876855326, + "angularVelocity": 7.21291856487456e-8, + "velocityX": 3.6753629810778445, + "velocityY": 0.8915682812457394, + "timestamp": 2.474907878955088 + }, + { + "x": 6.304175182424159, + "y": 6.755631110035184, + "heading": 0.2194315833737959, + "angularVelocity": 7.212918914147318e-8, + "velocityX": 3.675361742133493, + "velocityY": 0.8915733886014583, + "timestamp": 2.5387550215398793 + }, + { + "x": 6.5374892679107495, + "y": 6.8178474433780725, + "heading": 0.2194315924580089, + "angularVelocity": 1.4228065067709048e-7, + "velocityX": 3.6542604107418026, + "velocityY": 0.9744576002013384, + "timestamp": 2.602602164124671 + }, + { + "x": 6.754532777659766, + "y": 6.881509233956424, + "heading": 0.2489007631101974, + "angularVelocity": 0.46155817565449186, + "velocityX": 3.399424014328823, + "velocityY": 0.9970969412422411, + "timestamp": 2.6664493067094623 + }, + { + "x": 6.95421230704469, + "y": 6.941327038715391, + "heading": 0.2778274120513566, + "angularVelocity": 0.4530609792402775, + "velocityX": 3.1274622684913846, + "velocityY": 0.9368908667999729, + "timestamp": 2.730296449294254 + }, + { + "x": 7.136568973398106, + "y": 6.997132034172886, + "heading": 0.3053765730205754, + "angularVelocity": 0.4314862005395546, + "velocityX": 2.8561445190947854, + "velocityY": 0.8740406100928324, + "timestamp": 2.7941435918790454 + }, + { + "x": 7.301617944582347, + "y": 7.048866046398329, + "heading": 0.3312582379964112, + "angularVelocity": 0.40536919786917625, + "velocityX": 2.585064334947336, + "velocityY": 0.8102792095470503, + "timestamp": 2.857990734463837 + }, + { + "x": 7.449367096097909, + "y": 7.096499636385878, + "heading": 0.3553246002750777, + "angularVelocity": 0.376937186291547, + "velocityX": 2.314107500102227, + "velocityY": 0.7460567232791978, + "timestamp": 2.9218378770486284 + }, + { + "x": 7.579821248483784, + "y": 7.140015034676201, + "heading": 0.37748571646004736, + "angularVelocity": 0.3470964445360872, + "velocityX": 2.0432261665058515, + "velocityY": 0.6815559244884293, + "timestamp": 2.98568501963342 + }, + { + "x": 7.692983655491274, + "y": 7.179400354716541, + "heading": 0.3976808702861806, + "angularVelocity": 0.31630473986072827, + "velocityX": 1.7723957944900266, + "velocityY": 0.6168689536581131, + "timestamp": 3.0495321622182114 + }, + { + "x": 7.78885666094427, + "y": 7.214647088460025, + "heading": 0.4158661907347705, + "angularVelocity": 0.28482590938880487, + "velocityX": 1.5016021324004238, + "velocityY": 0.5520487263259451, + "timestamp": 3.113379304803003 + }, + { + "x": 7.867442033486985, + "y": 7.245748846091361, + "heading": 0.43200843887480883, + "angularVelocity": 0.2528264772162807, + "velocityX": 1.2308361715381555, + "velocityY": 0.4871284190992695, + "timestamp": 3.1772264473877945 + }, + { + "x": 7.92874115504245, + "y": 7.272700652676186, + "heading": 0.4460815560128383, + "angularVelocity": 0.2204189031535715, + "velocityX": 0.9600918549182749, + "velocityY": 0.4221301924206222, + "timestamp": 3.241073589972586 + }, + { + "x": 7.972755134992524, + "y": 7.295498524714787, + "heading": 0.4580645976614163, + "angularVelocity": 0.18768328798213776, + "velocityX": 0.689364913889801, + "velocityY": 0.35706957454398996, + "timestamp": 3.3049207325573775 + }, + { + "x": 7.9994848833762395, + "y": 7.314139199968811, + "heading": 0.467940423936039, + "angularVelocity": 0.15467922094567124, + "velocityX": 0.4186522262639504, + "velocityY": 0.29195786215910857, + "timestamp": 3.368767875142169 + }, + { + "x": 8.008931159973145, + "y": 7.328619956970215, + "heading": 0.4756948301053548, + "angularVelocity": 0.12145267361053229, + "velocityX": 0.1479514386154507, + "velocityY": 0.2268035250312554, + "timestamp": 3.4326150177269605 + }, + { + "x": 7.983802853314603, + "y": 7.34099497259759, + "heading": 0.48253914652919067, + "angularVelocity": 0.07100820527560414, + "velocityX": -0.2607003894828745, + "velocityY": 0.12838793468070023, + "timestamp": 3.5290027013372 + }, + { + "x": 7.919285557898086, + "y": 7.343883947492977, + "heading": 0.48451888567323514, + "angularVelocity": 0.020539337287634327, + "velocityX": -0.6693520686460666, + "velocityY": 0.029972448628073807, + "timestamp": 3.6253903849474396 + }, + { + "x": 7.8153792867404235, + "y": 7.337286874127341, + "heading": 0.48163302720996115, + "angularVelocity": -0.029940116363242703, + "velocityX": -1.078003612762665, + "velocityY": -0.06844311553655845, + "timestamp": 3.721778068557679 + }, + { + "x": 7.672084051684559, + "y": 7.321203727123482, + "heading": 0.4738818599108745, + "angularVelocity": -0.08041657407630928, + "velocityX": -1.4866550340114346, + "velocityY": -0.16685894298377135, + "timestamp": 3.8181657521679186 + }, + { + "x": 7.489399863960927, + "y": 7.295634462778821, + "heading": 0.46126688957646533, + "angularVelocity": -0.1308774094563781, + "velocityX": -1.895306338747057, + "velocityY": -0.26527522383518304, + "timestamp": 3.914553435778158 + }, + { + "x": 7.26732673524187, + "y": 7.260579018302084, + "heading": 0.4437906761804973, + "angularVelocity": -0.18131168569872746, + "velocityX": -2.3039575223848003, + "velocityY": -0.3636921561315803, + "timestamp": 4.010941119388398 + }, + { + "x": 7.005864679481663, + "y": 7.216037310494284, + "heading": 0.42145659939316793, + "angularVelocity": -0.23171089864179226, + "velocityX": -2.712608561250158, + "velocityY": -0.46210995159831264, + "timestamp": 4.107328802998637 + }, + { + "x": 6.705013716725352, + "y": 7.162009232661868, + "heading": 0.39426855573546815, + "angularVelocity": -0.28206968607772986, + "velocityX": -3.1212593921527856, + "velocityY": -0.5605288539860318, + "timestamp": 4.203716486608877 + }, + { + "x": 6.364773887222121, + "y": 7.098494640966319, + "heading": 0.36223062299373554, + "angularVelocity": -0.3323861674203487, + "velocityX": -3.5299098054794116, + "velocityY": -0.6589492486652313, + "timestamp": 4.300104170219116 + }, + { + "x": 6.0103743066555975, + "y": 7.0131369660627, + "heading": 0.3622306184721976, + "angularVelocity": -4.6909913247045394e-8, + "velocityX": -3.6768139589244604, + "velocityY": -0.8855662020967079, + "timestamp": 4.396491853829356 + }, + { + "x": 5.655974854365273, + "y": 6.927778758566046, + "heading": 0.3622306139507427, + "angularVelocity": -4.690905296707159e-8, + "velocityX": -3.6768126280884714, + "velocityY": -0.8855717276266857, + "timestamp": 4.492879537439595 + }, + { + "x": 5.301574924606988, + "y": 6.842422680000666, + "heading": 0.3622305723626749, + "angularVelocity": -4.3146661671098113e-7, + "velocityX": -3.676817581708511, + "velocityY": -0.8855496404554449, + "timestamp": 4.589267221049835 + }, + { + "x": 4.986369111177269, + "y": 6.766504431310858, + "heading": 0.34532944076922883, + "angularVelocity": -0.17534534455448447, + "velocityX": -3.2701876590821435, + "velocityY": -0.7876343309254682, + "timestamp": 4.685654904660074 + }, + { + "x": 4.710566830571078, + "y": 6.700076677592526, + "heading": 0.3264669889522253, + "angularVelocity": -0.19569358978764517, + "velocityX": -2.8613850885912564, + "velocityY": -0.6891726331648808, + "timestamp": 4.782042588270314 + }, + { + "x": 4.474170499658151, + "y": 6.6431400154165186, + "heading": 0.30840377599143814, + "angularVelocity": -0.18740167087974618, + "velocityX": -2.452557443633948, + "velocityY": -0.5907047461192427, + "timestamp": 4.8784302718805534 + }, + { + "x": 4.277177709226617, + "y": 6.595693819551337, + "heading": 0.29230604083143724, + "angularVelocity": -0.16701029174116175, + "velocityX": -2.043754793694494, + "velocityY": -0.4922433457062721, + "timestamp": 4.974817955490793 + }, + { + "x": 4.119586252031972, + "y": 6.557737537391428, + "heading": 0.2788176445470824, + "angularVelocity": -0.13993900236152076, + "velocityX": -1.6349750434079608, + "velocityY": -0.393787678448541, + "timestamp": 5.0712056391010325 + }, + { + "x": 4.001394403519539, + "y": 6.529270747914268, + "heading": 0.2683469283541488, + "angularVelocity": -0.10863126699126655, + "velocityX": -1.226213184978727, + "velocityY": -0.2953363792024593, + "timestamp": 5.167593322711272 + }, + { + "x": 3.922600824939347, + "y": 6.510293128644836, + "heading": 0.26117603270245576, + "angularVelocity": -0.074396389487788, + "velocityX": -0.8174652157718444, + "velocityY": -0.1968884255603806, + "timestamp": 5.263981006321512 + }, + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": -0.038017455811817785, + "velocityX": -0.40872820385029696, + "velocityY": -0.09844312056835454, + "timestamp": 5.360368689931751 + }, + { + "x": 3.883204460144043, + "y": 6.500804424285889, + "heading": 0.2575116182, + "angularVelocity": -1.8368418613420646e-25, + "velocityX": -1.6872129777974468e-25, + "velocityY": 1.375029629161337e-25, + "timestamp": 5.456756373541991 + }, + { + "x": 3.856532695000803, + "y": 6.488812456981308, + "heading": 0.2579986728633844, + "angularVelocity": 0.005949010101817782, + "velocityX": -0.32577575413793, + "velocityY": -0.14647295262485988, + "timestamp": 5.538627921084927 + }, + { + "x": 3.8031475649179134, + "y": 6.464921556304128, + "heading": 0.2590017936055681, + "angularVelocity": 0.012252373043976347, + "velocityX": -0.6520596188180307, + "velocityY": -0.29180956503419114, + "timestamp": 5.620499468627864 + }, + { + "x": 3.7229939182763383, + "y": 6.429256431280001, + "heading": 0.2605597691269097, + "angularVelocity": 0.019029511082889358, + "velocityX": -0.979017119464356, + "velocityY": -0.43562294954083813, + "timestamp": 5.702371016170801 + }, + { + "x": 3.615995163614758, + "y": 6.38199294529701, + "heading": 0.26272711898941353, + "angularVelocity": 0.026472564981957637, + "velocityX": -1.3069101277884805, + "velocityY": -0.577288293692067, + "timestamp": 5.784242563713738 + }, + { + "x": 3.4820378233974707, + "y": 6.323397660276972, + "heading": 0.26558610226294477, + "angularVelocity": 0.03492035217768224, + "velocityX": -1.6361891797273596, + "velocityY": -0.7156977824232311, + "timestamp": 5.866114111256675 + }, + { + "x": 3.320936743362508, + "y": 6.253922161955944, + "heading": 0.26927508652417376, + "angularVelocity": 0.045058196307995436, + "velocityX": -1.967729753129128, + "velocityY": -0.8485914875933167, + "timestamp": 5.947985658799611 + }, + { + "x": 3.1323379926537895, + "y": 6.1744965280304065, + "heading": 0.2740753686856588, + "angularVelocity": 0.0586318728001031, + "velocityX": -2.3035933284369614, + "velocityY": -0.9701249861422718, + "timestamp": 6.029857206342548 + }, + { + "x": 2.9153402053664754, + "y": 6.088110219486258, + "heading": 0.2808618012574976, + "angularVelocity": 0.0828912213767512, + "velocityX": -2.650466417206927, + "velocityY": -1.0551444443974112, + "timestamp": 6.111728753885485 + }, + { + "x": 2.698015397521732, + "y": 6.030900368675538, + "heading": 0.2958528133927384, + "angularVelocity": 0.18310405244726724, + "velocityX": -2.6544607298496428, + "velocityY": -0.6987757350099817, + "timestamp": 6.193600301428422 + }, + { + "x": 2.504490270491426, + "y": 5.9906815262758615, + "heading": 0.3120114847728296, + "angularVelocity": 0.19736614080266282, + "velocityX": -2.363765347525813, + "velocityY": -0.49124321704783075, + "timestamp": 6.275471848971359 + }, + { + "x": 2.336082741941251, + "y": 5.965439222431395, + "heading": 0.3286454370988765, + "angularVelocity": 0.20317134371160356, + "velocityX": -2.0569725821031444, + "velocityY": -0.30831594860508243, + "timestamp": 6.357343396514295 + }, + { + "x": 2.1932220965517097, + "y": 5.954429216642808, + "heading": 0.3455083861254867, + "angularVelocity": 0.20596836792130419, + "velocityX": -1.744936423909899, + "velocityY": -0.1344790237757827, + "timestamp": 6.439214944057232 + }, + { + "x": 2.0761197199383044, + "y": 5.957265361507106, + "heading": 0.36247439681677673, + "angularVelocity": 0.20722718942612423, + "velocityX": -1.4303183477995456, + "velocityY": 0.034641398012055055, + "timestamp": 6.521086491600169 + }, + { + "x": 1.9849012059686333, + "y": 5.973711453341115, + "heading": 0.37946704953730365, + "angularVelocity": 0.20755260197830386, + "velocityX": -1.1141662361009146, + "velocityY": 0.2008767676632038, + "timestamp": 6.602958039143106 + }, + { + "x": 1.9196497213524515, + "y": 6.003608138127398, + "heading": 0.39643505242175614, + "angularVelocity": 0.20725152258241855, + "velocityX": -0.7969983049601116, + "velocityY": 0.36516574663016305, + "timestamp": 6.6848295866860425 + }, + { + "x": 1.8804243803024292, + "y": 6.046840667724609, + "heading": 0.41334160220496513, + "angularVelocity": 0.2065009187024656, + "velocityX": -0.47910833772197736, + "velocityY": 0.5280531624803828, + "timestamp": 6.766701134228979 + }, + { + "x": 1.8671378435771844, + "y": 6.102598644221004, + "heading": 0.42998500556879043, + "angularVelocity": 0.2054233650605208, + "velocityX": -0.1639908031089557, + "velocityY": 0.6882000580332149, + "timestamp": 6.8477211455549645 + }, + { + "x": 1.8794728316222993, + "y": 6.171151657888497, + "heading": 0.44648318199526155, + "angularVelocity": 0.2036308827468621, + "velocityX": 0.15224619008611265, + "velocityY": 0.8461244641360118, + "timestamp": 6.92874115688095 + }, + { + "x": 1.9175617070092363, + "y": 6.252231071792971, + "heading": 0.462750092926245, + "angularVelocity": 0.2007764583682102, + "velocityX": 0.4701168854900097, + "velocityY": 1.000733184030919, + "timestamp": 7.009761168206935 + }, + { + "x": 1.9816154560724233, + "y": 6.345393254847428, + "heading": 0.47864458300503304, + "angularVelocity": 0.1961798056882073, + "velocityX": 0.7905917070964839, + "velocityY": 1.1498663296851208, + "timestamp": 7.09078117953292 + }, + { + "x": 2.0720214338642493, + "y": 6.44976750066492, + "heading": 0.4938927703977008, + "angularVelocity": 0.1882027309440445, + "velocityX": 1.1158475086861754, + "velocityY": 1.288252668807229, + "timestamp": 7.171801190858905 + }, + { + "x": 2.189694481994004, + "y": 6.562891621237751, + "heading": 0.5077401370803508, + "angularVelocity": 0.17091291961111932, + "velocityX": 1.452394861515095, + "velocityY": 1.3962491330404174, + "timestamp": 7.25282120218489 + }, + { + "x": 2.330980243223138, + "y": 6.6599214066012715, + "heading": 0.5136983140037735, + "angularVelocity": 0.07353957159361435, + "velocityX": 1.7438378360707576, + "velocityY": 1.197602712904113, + "timestamp": 7.333841213510875 + }, + { + "x": 2.4512963275001454, + "y": 6.737453803424918, + "heading": 0.5174619765609972, + "angularVelocity": 0.04645349334845922, + "velocityX": 1.48501687802677, + "velocityY": 0.9569536655789208, + "timestamp": 7.41486122483686 + }, + { + "x": 2.548343024270106, + "y": 6.798294060832851, + "heading": 0.5200641230004012, + "angularVelocity": 0.032117329988194755, + "velocityX": 1.1978114441319052, + "velocityY": 0.7509287694757523, + "timestamp": 7.4958812361628455 + }, + { + "x": 2.6214712468816064, + "y": 6.843385043467383, + "heading": 0.5218324598045924, + "angularVelocity": 0.021825926400778378, + "velocityX": 0.9025945740400864, + "velocityY": 0.5565413025321349, + "timestamp": 7.576901247488831 + }, + { + "x": 2.6703780246763915, + "y": 6.873196303781186, + "heading": 0.5229271765337693, + "angularVelocity": 0.01351168324048117, + "velocityX": 0.6036382492963173, + "velocityY": 0.367949347647667, + "timestamp": 7.657921258814816 + }, + { + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": 0.006368644706239802, + "velocityX": 0.30252191119511246, + "velocityY": 0.1828226198606205, + "timestamp": 7.738941270140801 + }, + { + "x": 2.6948883533477783, + "y": 6.8880085945129395, + "heading": 0.5234431642, + "angularVelocity": 4.786924324290607e-29, + "velocityX": -3.940142843909209e-27, + "velocityY": 5.40414297722828e-27, + "timestamp": 7.819961281466786 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBc.1.traj b/src/main/deploy/choreo/CenterLanePBc.1.traj deleted file mode 100644 index 1d81c97b..00000000 --- a/src/main/deploy/choreo/CenterLanePBc.1.traj +++ /dev/null @@ -1,256 +0,0 @@ -{ - "samples": [ - { - "x": 1.5270464420318604, - "y": 4.085783958435059, - "heading": 1.4625077505834915e-24, - "angularVelocity": -1.6968405368267337e-23, - "velocityX": 1.1721587912394621e-19, - "velocityY": -9.898260288411433e-19, - "timestamp": 0 - }, - { - "x": 1.5260826099479785, - "y": 4.093922987832265, - "heading": -1.5564984556705778e-16, - "angularVelocity": -1.5951860339453124e-15, - "velocityX": -0.009878020828395265, - "velocityY": 0.08341442794136009, - "timestamp": 0.09757340064631857 - }, - { - "x": 1.524154945788772, - "y": 4.11020104655442, - "heading": -4.825478082550605e-16, - "angularVelocity": -3.3502446331081196e-15, - "velocityX": -0.019756041569095335, - "velocityY": 0.16682885514218276, - "timestamp": 0.19514680129263715 - }, - { - "x": 1.5212634495643529, - "y": 4.1346181345161295, - "heading": -9.964831586930951e-16, - "angularVelocity": -5.2671216122656385e-15, - "velocityX": -0.02963406220615563, - "velocityY": 0.2502432814678248, - "timestamp": 0.2927202019389557 - }, - { - "x": 1.5174081212868562, - "y": 4.16717425161492, - "heading": -1.717782516967208e-15, - "angularVelocity": -7.392318655053113e-15, - "velocityX": -0.0395120827188482, - "velocityY": 0.33365770674325024, - "timestamp": 0.3902936025852743 - }, - { - "x": 1.5125889609711136, - "y": 4.207869397725548, - "heading": -2.6688719095730424e-15, - "angularVelocity": -9.74735165634569e-15, - "velocityX": -0.049390103079535765, - "velocityY": 0.4170721307350775, - "timestamp": 0.4878670032315929 - }, - { - "x": 1.5068059686356647, - "y": 4.256703572691455, - "heading": -3.876697094067346e-15, - "angularVelocity": -1.2378541573688779e-14, - "velocityX": -0.05926812325021708, - "velocityY": 0.500486553122407, - "timestamp": 0.5854404038779114 - }, - { - "x": 1.5000591443043463, - "y": 4.313676776311357, - "heading": -5.376745558038763e-15, - "angularVelocity": -1.5373429438679563e-14, - "velocityX": -0.06914614317660467, - "velocityY": 0.5839009734468111, - "timestamp": 0.68301380452423 - }, - { - "x": 1.49234848800894, - "y": 4.378789008316868, - "heading": -7.221997958230257e-15, - "angularVelocity": -1.8911296572887638e-14, - "velocityX": -0.07902416277726727, - "velocityY": 0.667315391020648, - "timestamp": 0.7805872051705486 - }, - { - "x": 1.4836739997939412, - "y": 4.452040268332257, - "heading": -9.479155947701628e-15, - "angularVelocity": -2.3132763955751144e-14, - "velocityX": -0.08890218192191494, - "velocityY": 0.7507298047436908, - "timestamp": 0.8781606058168672 - }, - { - "x": 1.4740356797260918, - "y": 4.533430555793919, - "heading": -1.2250780387615259e-14, - "angularVelocity": -2.8405339345394318e-14, - "velocityX": -0.09878020038254028, - "velocityY": 0.8341442126905433, - "timestamp": 0.9757340064631858 - }, - { - "x": 1.4634335279166297, - "y": 4.622959869762518, - "heading": -1.570747306998558e-14, - "angularVelocity": -3.542634883593584e-14, - "velocityX": -0.1086582177031285, - "velocityY": 0.9175586110104128, - "timestamp": 1.0733074071095043 - }, - { - "x": 1.451867544588029, - "y": 4.720628208359378, - "heading": -2.0193306127373128e-14, - "angularVelocity": -4.5973625402297885e-14, - "velocityX": -0.11853623274364325, - "velocityY": 1.000972990076323, - "timestamp": 1.1708808077558228 - }, - { - "x": 1.439337730407712, - "y": 4.826435565948485, - "heading": -2.6736262460658644e-14, - "angularVelocity": -6.705631534728917e-14, - "velocityX": -0.12841424094394735, - "velocityY": 1.0843873113804372, - "timestamp": 1.2684542084021413 - }, - { - "x": 1.4268079162273966, - "y": 4.9322429235375935, - "heading": -2.0195304822343385e-14, - "angularVelocity": 6.703581864794123e-14, - "velocityX": -0.12841424094393503, - "velocityY": 1.0843873113804385, - "timestamp": 1.3660276090484598 - }, - { - "x": 1.4152419328987969, - "y": 5.029911262134455, - "heading": -1.571219926412934e-14, - "angularVelocity": 4.594566272297136e-14, - "velocityX": -0.1185362327436348, - "velocityY": 1.0009729900763238, - "timestamp": 1.4636010096947782 - }, - { - "x": 1.4046397810893352, - "y": 5.119440576103053, - "heading": -1.2258131187906243e-14, - "angularVelocity": 3.539944307707758e-14, - "velocityX": -0.10865821770312199, - "velocityY": 0.9175586110104136, - "timestamp": 1.5611744103410967 - }, - { - "x": 1.3950014610214865, - "y": 5.200830863564716, - "heading": -9.488622066072567e-15, - "angularVelocity": 2.8383655202850955e-14, - "velocityX": -0.09878020038253506, - "velocityY": 0.8341442126905438, - "timestamp": 1.6587478109874152 - }, - { - "x": 1.386326972806488, - "y": 5.274082123580104, - "heading": -7.232328083000986e-15, - "angularVelocity": 2.3123906034472235e-14, - "velocityX": -0.0889021819219107, - "velocityY": 0.7507298047436913, - "timestamp": 1.7563212116337337 - }, - { - "x": 1.378616316511082, - "y": 5.339194355585617, - "heading": -5.387907876762194e-15, - "angularVelocity": 1.8902766491774747e-14, - "velocityX": -0.0790241627772638, - "velocityY": 0.6673153910206483, - "timestamp": 1.8538946122800521 - }, - { - "x": 1.3718694921797638, - "y": 5.396167559205518, - "heading": -3.885859488553591e-15, - "angularVelocity": 1.53939266022293e-14, - "velocityX": -0.06914614317660184, - "velocityY": 0.5839009734468115, - "timestamp": 1.9514680129263706 - }, - { - "x": 1.3660864998443152, - "y": 5.4450017341714245, - "heading": -2.6761448049829828e-15, - "angularVelocity": 1.2397908698261985e-14, - "velocityX": -0.05926812325021481, - "velocityY": 0.5004865531224073, - "timestamp": 2.0490414135726893 - }, - { - "x": 1.361267339528573, - "y": 5.4856968802820525, - "heading": -1.7234986300483674e-15, - "angularVelocity": 9.76331049910467e-15, - "velocityX": -0.04939010307953397, - "velocityY": 0.4170721307350777, - "timestamp": 2.146614814219008 - }, - { - "x": 1.3574120112510764, - "y": 5.518252997380844, - "heading": -1.0006590963793495e-15, - "angularVelocity": 7.408108892000521e-15, - "velocityX": -0.03951208271884683, - "velocityY": 0.3336577067432504, - "timestamp": 2.2441882148653263 - }, - { - "x": 1.3545205150266573, - "y": 5.542670085342553, - "heading": -4.849115866184459e-16, - "angularVelocity": 5.285700891601152e-15, - "velocityX": -0.029634062206154655, - "velocityY": 0.25024328146782493, - "timestamp": 2.3417616155116447 - }, - { - "x": 1.3525928508674507, - "y": 5.558948144064708, - "heading": -1.5693409310506777e-16, - "angularVelocity": 3.361316960958839e-15, - "velocityX": -0.019756041569094714, - "velocityY": 0.1668288551421828, - "timestamp": 2.439335016157963 - }, - { - "x": 1.3516290187835691, - "y": 5.567087173461914, - "heading": -6.1887132115889425e-24, - "angularVelocity": 1.608357872874533e-15, - "velocityX": -0.009878020828394967, - "velocityY": 0.08341442794136011, - "timestamp": 2.5369084168042817 - }, - { - "x": 1.3516290187835691, - "y": 5.567087173461914, - "heading": -1.4648141818546668e-24, - "angularVelocity": 3.340172165504719e-23, - "velocityX": -1.5435615328511767e-18, - "velocityY": -2.1040451608667207e-18, - "timestamp": 2.6344818174506 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBc.2.traj b/src/main/deploy/choreo/CenterLanePBc.2.traj deleted file mode 100644 index 2a2f7d38..00000000 --- a/src/main/deploy/choreo/CenterLanePBc.2.traj +++ /dev/null @@ -1,220 +0,0 @@ -{ - "samples": [ - { - "x": 1.3516290187835691, - "y": 5.567087173461914, - "heading": -1.4648141818546668e-24, - "angularVelocity": 3.340172165504719e-23, - "velocityX": -1.5435615328511767e-18, - "velocityY": -2.1040451608667207e-18, - "timestamp": 0 - }, - { - "x": 1.3600455150653603, - "y": 5.566939516487978, - "heading": 6.686728970627398e-20, - "angularVelocity": 6.762246861371845e-19, - "velocityX": 0.08511363372225911, - "velocityY": -0.0014932129921227434, - "timestamp": 0.09888540664655165 - }, - { - "x": 1.376878507539919, - "y": 5.566644202541668, - "heading": 1.580133567010756e-19, - "angularVelocity": 9.217342317784428e-19, - "velocityX": 0.17022726654424447, - "velocityY": -0.0029864259684513, - "timestamp": 0.1977708132931033 - }, - { - "x": 1.4021279960984379, - "y": 5.566201231624893, - "heading": 2.8447751001050527e-19, - "angularVelocity": 1.2788959837210388e-18, - "velocityX": 0.2553408982658953, - "velocityY": -0.004479638925475853, - "timestamp": 0.29665621993965496 - }, - { - "x": 1.435793980604908, - "y": 5.565610603740038, - "heading": 3.7920321307665064e-19, - "angularVelocity": 9.579340703122642e-19, - "velocityX": 0.3404545286121278, - "velocityY": -0.005972851858370403, - "timestamp": 0.3955416265862066 - }, - { - "x": 1.4778764608844615, - "y": 5.564872318890172, - "heading": 4.3118128283917494e-19, - "angularVelocity": 5.256394158249627e-19, - "velocityX": 0.42556815718996566, - "velocityY": -0.007466064760240663, - "timestamp": 0.4944270332327583 - }, - { - "x": 1.52837543670394, - "y": 5.563986377079386, - "heading": 3.7231429179967247e-19, - "angularVelocity": -5.953051342426901e-19, - "velocityX": 0.5106817834099439, - "velocityY": -0.008959277620745203, - "timestamp": 0.5933124398793099 - }, - { - "x": 1.5872909077369224, - "y": 5.562952778313405, - "heading": 3.665507864267625e-19, - "angularVelocity": -5.828470212315425e-20, - "velocityX": 0.5957954063289188, - "velocityY": -0.01045249042333774, - "timestamp": 0.6921978465258616 - }, - { - "x": 1.6546228734937773, - "y": 5.56177152260082, - "heading": 3.3289358715677737e-19, - "angularVelocity": -3.403656887203302e-19, - "velocityX": 0.6809090242963888, - "velocityY": -0.011945703139062274, - "timestamp": 0.7910832531724132 - }, - { - "x": 1.7303713331584527, - "y": 5.560442609955947, - "heading": 2.1844998249066e-19, - "angularVelocity": -1.1573356581936454e-18, - "velocityX": 0.766022634011352, - "velocityY": -0.013438915710006823, - "timestamp": 0.8899686598189649 - }, - { - "x": 1.8145362850988431, - "y": 5.55896604040742, - "heading": 5.382114736148874e-20, - "angularVelocity": -1.6648446043389792e-18, - "velocityX": 0.851136227221305, - "velocityY": -0.01493212799139147, - "timestamp": 0.9888540664655165 - }, - { - "x": 1.90711772441864, - "y": 5.557341814041138, - "heading": -1.1309409770731682e-19, - "angularVelocity": -1.6879664104157646e-18, - "velocityX": 0.9362497709162765, - "velocityY": -0.016425339404097267, - "timestamp": 1.0877394731120682 - }, - { - "x": 1.9996991637384367, - "y": 5.555717587674857, - "heading": -1.8044549355651484e-19, - "angularVelocity": -6.811054697269865e-19, - "velocityX": 0.9362497709162764, - "velocityY": -0.016425339404097263, - "timestamp": 1.1866248797586199 - }, - { - "x": 2.083864115678827, - "y": 5.554241018126329, - "heading": -2.080650691294579e-19, - "angularVelocity": -2.793088809488762e-19, - "velocityX": 0.8511362272213049, - "velocityY": -0.014932127991391469, - "timestamp": 1.2855102864051715 - }, - { - "x": 2.1596125753435023, - "y": 5.552912105481456, - "heading": -1.7750016719575358e-19, - "angularVelocity": 3.090941806706561e-19, - "velocityX": 0.7660226340113518, - "velocityY": -0.013438915710006822, - "timestamp": 1.3843956930517227 - }, - { - "x": 2.226944541100357, - "y": 5.551730849768871, - "heading": -2.0769787641047505e-19, - "angularVelocity": -3.0538082503662157e-19, - "velocityX": 0.6809090242963888, - "velocityY": -0.011945703139062272, - "timestamp": 1.483281099698274 - }, - { - "x": 2.2858600121333397, - "y": 5.550697251002891, - "heading": -2.0325843947083703e-19, - "angularVelocity": 4.4894778552600243e-20, - "velocityX": 0.5957954063289187, - "velocityY": -0.010452490423337736, - "timestamp": 1.5821665063448251 - }, - { - "x": 2.3363589879528184, - "y": 5.549811309192104, - "heading": -2.3083936836877355e-19, - "angularVelocity": -2.7891805869836823e-19, - "velocityX": 0.5106817834099439, - "velocityY": -0.008959277620745202, - "timestamp": 1.6810519129913764 - }, - { - "x": 2.3784414682323716, - "y": 5.5490730243422375, - "heading": -2.907085889576077e-19, - "angularVelocity": -6.054403614475218e-19, - "velocityX": 0.42556815718996566, - "velocityY": -0.00746606476024066, - "timestamp": 1.7799373196379276 - }, - { - "x": 2.4121074527388418, - "y": 5.548482396457383, - "heading": -2.2908841084806075e-19, - "angularVelocity": 6.231473438855849e-19, - "velocityX": 0.3404545286121278, - "velocityY": -0.0059728518583704, - "timestamp": 1.8788227262844788 - }, - { - "x": 2.437356941297361, - "y": 5.548039425540607, - "heading": -1.3373199383404307e-19, - "angularVelocity": 9.643123213362854e-19, - "velocityX": 0.2553408982658953, - "velocityY": -0.004479638925475851, - "timestamp": 1.97770813293103 - }, - { - "x": 2.4541899337719197, - "y": 5.547744111594297, - "heading": -3.289437685355794e-20, - "angularVelocity": 1.019742123971898e-18, - "velocityX": 0.17022726654424447, - "velocityY": -0.002986425968451297, - "timestamp": 2.076593539577581 - }, - { - "x": 2.462606430053711, - "y": 5.547596454620361, - "heading": -2.886570593000217e-29, - "angularVelocity": 3.3265147049069624e-19, - "velocityX": 0.08511363372225912, - "velocityY": -0.001493212992122741, - "timestamp": 2.1754789462241324 - }, - { - "x": 2.462606430053711, - "y": 5.547596454620361, - "heading": -7.821656462951895e-30, - "angularVelocity": 2.943074068153252e-29, - "velocityX": -2.2275173681467236e-19, - "velocityY": 4.4340986172999205e-21, - "timestamp": 2.2743643528706836 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBc.3.traj b/src/main/deploy/choreo/CenterLanePBc.3.traj deleted file mode 100644 index da59f987..00000000 --- a/src/main/deploy/choreo/CenterLanePBc.3.traj +++ /dev/null @@ -1,220 +0,0 @@ -{ - "samples": [ - { - "x": 2.462606430053711, - "y": 5.547596454620361, - "heading": -7.821656462951895e-30, - "angularVelocity": 2.943074068153252e-29, - "velocityX": -2.2275173681467236e-19, - "velocityY": 4.4340986172999205e-21, - "timestamp": 0 - }, - { - "x": 2.454042275901417, - "y": 5.547744111594184, - "heading": 1.4460933754378984e-20, - "angularVelocity": 1.4497351179100133e-19, - "velocityX": -0.08585722436689831, - "velocityY": 0.0014802883863889518, - "timestamp": 0.09974878893937245 - }, - { - "x": 2.436913967686631, - "y": 5.548039425540282, - "heading": 4.286650670020682e-20, - "angularVelocity": 2.8477109182723563e-19, - "velocityX": -0.17171444783351814, - "velocityY": 0.0029605767572559506, - "timestamp": 0.1994975778787449 - }, - { - "x": 2.41122150551911, - "y": 5.548482396456763, - "heading": 5.855738018623403e-21, - "angularVelocity": -3.710397725109959e-19, - "velocityX": -0.2575716701997976, - "velocityY": 0.004440865109151674, - "timestamp": 0.29924636681811734 - }, - { - "x": 2.3769648895360507, - "y": 5.549073024341261, - "heading": -6.291625399026022e-20, - "angularVelocity": -6.894518818577401e-19, - "velocityX": -0.3434288911906516, - "velocityY": 0.005921153437333303, - "timestamp": 0.3989951557574898 - }, - { - "x": 2.33414411991385, - "y": 5.549811309190735, - "heading": -1.2017788841555738e-19, - "angularVelocity": -5.740584286417081e-19, - "velocityX": -0.42928611041310155, - "velocityY": 0.007401441735025384, - "timestamp": 0.49874394469686223 - }, - { - "x": 2.282759196887703, - "y": 5.5506972510011305, - "heading": -1.0823396901286469e-19, - "angularVelocity": 1.1973998537101906e-19, - "velocityX": -0.5151433272776794, - "velocityY": 0.008881729992064734, - "timestamp": 0.5984927336362347 - }, - { - "x": 2.2228101207868822, - "y": 5.551730849766769, - "heading": -8.802649819919278e-20, - "angularVelocity": 2.0258361490738492e-19, - "velocityX": -0.6010005408412367, - "velocityY": 0.010362018192190264, - "timestamp": 0.6982415225756071 - }, - { - "x": 2.1542968921052967, - "y": 5.552912105479136, - "heading": -6.239601383466543e-20, - "angularVelocity": 2.569503383512215e-19, - "velocityX": -0.6868577494532631, - "velocityY": 0.01184230630694507, - "timestamp": 0.7979903115149796 - }, - { - "x": 2.0772195116661285, - "y": 5.554241018124038, - "heading": -3.607669582656793e-20, - "angularVelocity": 2.6385601752371007e-19, - "velocityX": -0.7727149498127389, - "velocityY": 0.013322594279415347, - "timestamp": 0.897739100454352 - }, - { - "x": 1.991577981115741, - "y": 5.55571758767309, - "heading": -6.282144315835921e-20, - "angularVelocity": -2.6812099635964853e-19, - "velocityX": -0.8585721336671177, - "velocityY": 0.014802881967316633, - "timestamp": 0.9974878893937245 - }, - { - "x": 1.8973723053932194, - "y": 5.557341814041137, - "heading": -1.267501677946567e-19, - "angularVelocity": -6.408972052260157e-19, - "velocityX": -0.9444292680062534, - "velocityY": 0.016283168801511785, - "timestamp": 1.097236678333097 - }, - { - "x": 1.8031666296706979, - "y": 5.558966040409183, - "heading": -3.795289314001396e-21, - "angularVelocity": 1.2326452916170473e-18, - "velocityX": -0.9444292680062534, - "velocityY": 0.016283168801511785, - "timestamp": 1.1969854672724694 - }, - { - "x": 1.7175250991203104, - "y": 5.560442609958236, - "heading": 8.700208420675153e-20, - "angularVelocity": 9.1026038606046e-19, - "velocityX": -0.8585721336671177, - "velocityY": 0.014802881967316633, - "timestamp": 1.2967342562118418 - }, - { - "x": 1.640447718681142, - "y": 5.561771522603139, - "heading": 1.2976083579479636e-19, - "angularVelocity": 4.2866435269702167e-19, - "velocityX": -0.7727149498127389, - "velocityY": 0.013322594279415347, - "timestamp": 1.3964830451512142 - }, - { - "x": 1.5719344899995562, - "y": 5.5629527783155055, - "heading": 7.490403325878171e-20, - "angularVelocity": -5.49949547949794e-19, - "velocityX": -0.686857749453263, - "velocityY": 0.01184230630694507, - "timestamp": 1.4962318340905867 - }, - { - "x": 1.5119854138987352, - "y": 5.563986377081145, - "heading": 7.478581489863291e-20, - "angularVelocity": -1.1851672756803286e-21, - "velocityX": -0.6010005408412366, - "velocityY": 0.010362018192190266, - "timestamp": 1.5959806230299591 - }, - { - "x": 1.460600490872588, - "y": 5.56487231889154, - "heading": 7.910534561996949e-20, - "angularVelocity": 4.3304083992516454e-20, - "velocityX": -0.5151433272776794, - "velocityY": 0.008881729992064737, - "timestamp": 1.6957294119693316 - }, - { - "x": 1.4177797212503878, - "y": 5.565610603741014, - "heading": 5.432779100593734e-20, - "angularVelocity": -2.483995467741431e-19, - "velocityX": -0.4292861104131015, - "velocityY": 0.007401441735025386, - "timestamp": 1.795478200908704 - }, - { - "x": 1.383523105267329, - "y": 5.566201231625511, - "heading": -1.3462949300383398e-21, - "angularVelocity": -5.581429670757211e-19, - "velocityX": -0.34342889119065156, - "velocityY": 0.005921153437333306, - "timestamp": 1.8952269898480765 - }, - { - "x": 1.3578306430998077, - "y": 5.566644202541992, - "heading": -3.946401324540515e-20, - "angularVelocity": -3.821371505085094e-19, - "velocityX": -0.25757167019979754, - "velocityY": 0.004440865109151676, - "timestamp": 1.994975778787449 - }, - { - "x": 1.3407023348850213, - "y": 5.566939516488091, - "heading": -2.4791971136884522e-20, - "angularVelocity": 1.470899198317256e-19, - "velocityX": -0.1717144478335181, - "velocityY": 0.0029605767572559527, - "timestamp": 2.0947245677268214 - }, - { - "x": 1.332138180732727, - "y": 5.567087173461914, - "heading": 7.598893748852645e-29, - "angularVelocity": 2.4854406877542406e-19, - "velocityX": -0.0858572243668983, - "velocityY": 0.001480288386388954, - "timestamp": 2.194473356666194 - }, - { - "x": 1.332138180732727, - "y": 5.567087173461914, - "heading": 1.0804753349489245e-28, - "angularVelocity": 2.1800931515228082e-28, - "velocityX": 1.941111784767018e-19, - "velocityY": 2.1082261122464193e-18, - "timestamp": 2.2942221456055663 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePBc.4.traj b/src/main/deploy/choreo/CenterLanePBc.4.traj deleted file mode 100644 index 63d5e9d2..00000000 --- a/src/main/deploy/choreo/CenterLanePBc.4.traj +++ /dev/null @@ -1,283 +0,0 @@ -{ - "samples": [ - { - "x": 1.332138180732727, - "y": 5.567087173461914, - "heading": 1.0804753349489245e-28, - "angularVelocity": 2.1800931515228082e-28, - "velocityX": 1.941111784767018e-19, - "velocityY": 2.1082261122464193e-18, - "timestamp": 0 - }, - { - "x": 1.3372491110466118, - "y": 5.560676853815777, - "heading": 3.558639537501783e-19, - "angularVelocity": 3.646584120755799e-18, - "velocityX": 0.05237236819901584, - "velocityY": -0.06568737982374474, - "timestamp": 0.09758829874683439 - }, - { - "x": 1.3474709716341546, - "y": 5.547856214573958, - "heading": 1.0721703768123472e-18, - "angularVelocity": 7.340084933358087e-18, - "velocityX": 0.10474473598582199, - "velocityY": -0.1313747591304807, - "timestamp": 0.19517659749366878 - }, - { - "x": 1.3628037624486922, - "y": 5.528625255794984, - "heading": -1.2934737138196825e-18, - "angularVelocity": -2.4241062607509123e-17, - "velocityX": 0.15711710329446488, - "velocityY": -0.19706213783748644, - "timestamp": 0.29276489624050317 - }, - { - "x": 1.383247483435446, - "y": 5.50298397754756, - "heading": -3.2871298566762286e-18, - "angularVelocity": -2.0429253778468544e-17, - "velocityX": 0.20948947004178572, - "velocityY": -0.262749515840461, - "timestamp": 0.39035319498733756 - }, - { - "x": 1.4088021345292037, - "y": 5.470932379913476, - "heading": -6.392169366146804e-18, - "angularVelocity": -3.181774355862696e-17, - "velocityX": 0.261861836120866, - "velocityY": -0.32843689300530343, - "timestamp": 0.48794149373417195 - }, - { - "x": 1.439467715651024, - "y": 5.432470462991745, - "heading": -9.037790663968063e-18, - "angularVelocity": -2.7110025471414772e-17, - "velocityX": 0.31423420139102354, - "velocityY": -0.39412426915556464, - "timestamp": 0.5855297924810063 - }, - { - "x": 1.4752442267033907, - "y": 5.387598226904674, - "heading": -1.128391773258776e-17, - "angularVelocity": -2.301635620338134e-17, - "velocityX": 0.36660656566192357, - "velocityY": -0.45981164405251984, - "timestamp": 0.6831180912278407 - }, - { - "x": 1.516131667562784, - "y": 5.336315671807188, - "heading": -1.3116491479431834e-17, - "angularVelocity": -1.877862095234715e-17, - "velocityX": 0.41897892866709746, - "velocityY": -0.525499017361954, - "timestamp": 0.7807063899746751 - }, - { - "x": 1.562130038067678, - "y": 5.278622797901877, - "heading": -1.4552519190706605e-17, - "angularVelocity": -1.4715162201094296e-17, - "velocityX": 0.47135129001709103, - "velocityY": -0.5911863885953993, - "timestamp": 0.8782946887215095 - }, - { - "x": 1.613239337997809, - "y": 5.214519605465006, - "heading": -1.5531545587943304e-17, - "angularVelocity": -1.0032210364686455e-17, - "velocityX": 0.5237236491100207, - "velocityY": -0.6568737569979507, - "timestamp": 0.9758829874683448 - }, - { - "x": 1.6694595670350196, - "y": 5.1440060948956186, - "heading": -1.6968450650800506e-17, - "angularVelocity": -1.4724152370496227e-17, - "velocityX": 0.5760960049427472, - "velocityY": -0.7225611213114336, - "timestamp": 1.07347128621518 - }, - { - "x": 1.7307907246793477, - "y": 5.067082266820785, - "heading": -1.785524132033205e-17, - "angularVelocity": -9.08705871531393e-18, - "velocityX": 0.6284683556522977, - "velocityY": -0.788248479199238, - "timestamp": 1.1710595849620153 - }, - { - "x": 1.7972328100308623, - "y": 4.983748122369234, - "heading": -1.8156964270181303e-17, - "angularVelocity": -3.091793819261216e-18, - "velocityX": 0.680840697140134, - "velocityY": -0.8539358255208239, - "timestamp": 1.2686478837088506 - }, - { - "x": 1.8687858209897246, - "y": 4.894003664174662, - "heading": -1.7567148894927193e-17, - "angularVelocity": 6.043915190863691e-18, - "velocityX": 0.7332130171106522, - "velocityY": -0.919623144854586, - "timestamp": 1.366236182455686 - }, - { - "x": 1.9454497470568448, - "y": 4.797848905405417, - "heading": -1.7590364829650386e-17, - "angularVelocity": -2.378972880130156e-19, - "velocityX": 0.7855852294956268, - "velocityY": -0.985310329250542, - "timestamp": 1.4638244812025212 - }, - { - "x": 2.0170027580157073, - "y": 4.708104447210844, - "heading": -1.8525240985906295e-17, - "angularVelocity": -9.57979819187636e-18, - "velocityX": 0.7332130171106522, - "velocityY": -0.9196231448545861, - "timestamp": 1.5614127799493565 - }, - { - "x": 2.083444843367222, - "y": 4.624770302759293, - "heading": -1.62920217217427e-17, - "angularVelocity": 2.2884087968719853e-17, - "velocityX": 0.6808406971401341, - "velocityY": -0.8539358255208239, - "timestamp": 1.6590010786961917 - }, - { - "x": 2.14477600101155, - "y": 4.54784647468446, - "heading": -1.4137613991552986e-17, - "angularVelocity": 2.207649592959214e-17, - "velocityX": 0.6284683556522977, - "velocityY": -0.7882484791992381, - "timestamp": 1.756589377443027 - }, - { - "x": 2.200996230048761, - "y": 4.477332964115072, - "heading": -1.2021193262691405e-17, - "angularVelocity": 2.1687238185481475e-17, - "velocityX": 0.5760960049427472, - "velocityY": -0.7225611213114337, - "timestamp": 1.8541776761898623 - }, - { - "x": 2.252105529978892, - "y": 4.413229771678201, - "heading": -9.999661721633516e-18, - "angularVelocity": 2.07148963451225e-17, - "velocityX": 0.5237236491100207, - "velocityY": -0.6568737569979508, - "timestamp": 1.9517659749366976 - }, - { - "x": 2.2981039004837855, - "y": 4.3555368977728905, - "heading": -8.10923369851807e-18, - "angularVelocity": 1.9371461559461523e-17, - "velocityX": 0.471351290017091, - "velocityY": -0.5911863885953994, - "timestamp": 2.049354273683533 - }, - { - "x": 2.338991341343179, - "y": 4.304254342675404, - "heading": -6.3749798263727294e-18, - "angularVelocity": 1.7771124740362523e-17, - "velocityX": 0.41897892866709735, - "velocityY": -0.525499017361954, - "timestamp": 2.146942572430368 - }, - { - "x": 2.374767852395546, - "y": 4.259382106588334, - "heading": -4.81528393523032e-18, - "angularVelocity": 1.5982406426947388e-17, - "velocityX": 0.3666065656619235, - "velocityY": -0.4598116440525199, - "timestamp": 2.2445308711772034 - }, - { - "x": 2.405433433517366, - "y": 4.220920189666603, - "heading": -3.4442054151676063e-18, - "angularVelocity": 1.4049619717228675e-17, - "velocityX": 0.3142342013910235, - "velocityY": -0.39412426915556475, - "timestamp": 2.3421191699240387 - }, - { - "x": 2.430988084611124, - "y": 4.188868592032519, - "heading": -2.2728437711124333e-18, - "angularVelocity": 1.2003094938541682e-17, - "velocityX": 0.261861836120866, - "velocityY": -0.3284368930053035, - "timestamp": 2.439707468670874 - }, - { - "x": 2.4514318055978777, - "y": 4.163227313785095, - "heading": -1.3744434339694666e-18, - "angularVelocity": 9.206024983663883e-18, - "velocityX": 0.2094894700417857, - "velocityY": -0.2627495158404611, - "timestamp": 2.5372957674177092 - }, - { - "x": 2.4667645964124154, - "y": 4.14399635500612, - "heading": -6.921668969846993e-19, - "angularVelocity": 6.991376321961435e-18, - "velocityX": 0.15711710329446488, - "velocityY": -0.19706213783748644, - "timestamp": 2.6348840661645445 - }, - { - "x": 2.4769864569999585, - "y": 4.131175715764301, - "heading": -2.3224840986113696e-19, - "angularVelocity": 4.712844493605461e-18, - "velocityX": 0.10474473598582197, - "velocityY": -0.1313747591304807, - "timestamp": 2.73247236491138 - }, - { - "x": 2.4820973873138428, - "y": 4.124765396118164, - "heading": -2.507555799405664e-28, - "angularVelocity": 2.3798796279739872e-18, - "velocityX": 0.05237236819901583, - "velocityY": -0.06568737982374474, - "timestamp": 2.830060663658215 - }, - { - "x": 2.4820973873138428, - "y": 4.124765396118164, - "heading": -1.183597031867973e-28, - "angularVelocity": 1.4383049425452364e-28, - "velocityX": -1.8276514384427982e-18, - "velocityY": 2.2923091413933995e-18, - "timestamp": 2.9276489624050503 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBA.1.traj b/src/main/deploy/choreo/CenterLanePCBA.1.traj index 4199a7d3..c62302d9 100644 --- a/src/main/deploy/choreo/CenterLanePCBA.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBA.1.traj @@ -1,310 +1,311 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.7297728380351187e-19, - "angularVelocity": 4.2448351957919954e-19, - "velocityX": -3.109246931729259e-17, - "velocityY": 1.4154954258989747e-18, - "timestamp": 0 - }, - { - "x": 1.3060810448259774, - "y": 5.572403683390414, - "heading": -0.01067910748684314, - "angularVelocity": -0.14191654637088458, - "velocityX": 0.21404337049401068, - "velocityY": -0.246522471704611, - "timestamp": 0.07524920638169826 - }, - { - "x": 1.338296279992689, - "y": 5.535304313687478, - "heading": -0.032027558921844806, - "angularVelocity": -0.28370334335098873, - "velocityX": 0.4281139525027307, - "velocityY": -0.4930200793709309, - "timestamp": 0.1504984127633965 - }, - { - "x": 1.3866231363779933, - "y": 5.479658598357654, - "heading": -0.06402169471627948, - "angularVelocity": -0.42517572387591435, - "velocityX": 0.6422241337702286, - "velocityY": -0.7394857435109029, - "timestamp": 0.22574761914509478 - }, - { - "x": 1.4510659275520246, - "y": 5.405469878379882, - "heading": -0.1066262273585151, - "angularVelocity": -0.5661791624235272, - "velocityX": 0.8563916388334698, - "velocityY": -0.9859070087913582, - "timestamp": 0.300996825526793 - }, - { - "x": 1.5316310742105061, - "y": 5.312743180460654, - "heading": -0.15979745091713102, - "angularVelocity": -0.7066017851257455, - "velocityX": 1.0706444696567097, - "velocityY": -1.2322614732810149, - "timestamp": 0.37624603190849126 - }, - { - "x": 1.6283288838862067, - "y": 5.201486845299536, - "heading": -0.22348688396132305, - "angularVelocity": -0.8463801295274643, - "velocityX": 1.2850342791041873, - "velocityY": -1.4785050967404112, - "timestamp": 0.4514952382901895 - }, - { - "x": 1.7411789077586277, - "y": 5.071717216156576, - "heading": -0.2976431936929222, - "angularVelocity": -0.9854763033040427, - "velocityX": 1.4996839076304158, - "velocityY": -1.7245315317294676, - "timestamp": 0.5267444446718877 - }, - { - "x": 1.8702368010806378, - "y": 4.923481749892843, - "heading": -0.3821997162379228, - "angularVelocity": -1.1236865690804896, - "velocityX": 1.7150731486446302, - "velocityY": -1.9699273041075376, - "timestamp": 0.601993651053586 - }, - { - "x": 2.022770759979851, - "y": 4.779444476824575, - "heading": -0.46182371324885607, - "angularVelocity": -1.0581373656886817, - "velocityX": 2.0270507322765163, - "velocityY": -1.914136772919067, - "timestamp": 0.6772428574352843 - }, - { - "x": 2.159313623811303, - "y": 4.6540559405768205, - "heading": -0.5309316944889129, - "angularVelocity": -0.9183881739497737, - "velocityX": 1.8145422443221764, - "velocityY": -1.666310414116386, - "timestamp": 0.7524920638169826 - }, - { - "x": 2.2798095691663756, - "y": 4.547269113259306, - "heading": -0.5895622742606058, - "angularVelocity": -0.7791521344995035, - "velocityX": 1.601291909229005, - "velocityY": -1.4191090172553904, - "timestamp": 0.8277412701986809 - }, - { - "x": 2.3842397515062683, - "y": 4.459067906121371, - "heading": -0.6377272819831894, - "angularVelocity": -0.6400732982918503, - "velocityX": 1.3877911457302048, - "velocityY": -1.172121426644562, - "timestamp": 0.9029904765803792 - }, - { - "x": 2.47259465372947, - "y": 4.389444105411809, - "heading": -0.675431270833758, - "angularVelocity": -0.5010549700591305, - "velocityX": 1.1741639077877584, - "velocityY": -0.9252429900248669, - "timestamp": 0.9782396829620775 - }, - { - "x": 2.5448685468329297, - "y": 4.338392728081247, - "heading": -0.7026759403224371, - "angularVelocity": -0.36205922691677456, - "velocityX": 0.9604605361114344, - "velocityY": -0.6784307740288329, - "timestamp": 1.0534888893437757 - }, - { - "x": 2.601057641331632, - "y": 4.305910462744002, - "heading": -0.7194616948028688, - "angularVelocity": -0.2230688573014308, - "velocityX": 0.7467068052991092, - "velocityY": -0.43166256362226185, - "timestamp": 1.128738095725474 - }, - { - "x": 2.641159295921209, - "y": 4.291994997972796, - "heading": -0.7257882885399572, - "angularVelocity": -0.08407522206898749, - "velocityX": 0.5329179737262918, - "velocityY": -0.18492507018124238, - "timestamp": 1.2039873021071723 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.054926472219667455, - "velocityX": 0.31910406053771284, - "velocityY": 0.061790547746105144, - "timestamp": 1.2792365084888706 - }, - { - "x": 2.6730242117812852, - "y": 4.320207160810404, - "heading": -0.7068522062263088, - "angularVelocity": 0.19509567726207716, - "velocityX": 0.10349358326673892, - "velocityY": 0.31054279257798323, - "timestamp": 1.3551116333379198 - }, - { - "x": 2.6645156104831624, - "y": 4.362642495661916, - "heading": -0.6814188000083412, - "angularVelocity": 0.33520084834820957, - "velocityX": -0.1121395360458166, - "velocityY": 0.5592786164892918, - "timestamp": 1.430986758186969 - }, - { - "x": 2.6396435810539995, - "y": 4.423949055779942, - "heading": -0.6453599487845141, - "angularVelocity": 0.47523943183649325, - "velocityX": -0.32780215490271986, - "velocityY": 0.807992873026772, - "timestamp": 1.5068618830360183 - }, - { - "x": 2.5984050053739445, - "y": 4.5041245094115245, - "heading": -0.5986803757122448, - "angularVelocity": 0.615215766235525, - "velocityX": -0.5435058691760214, - "velocityY": 1.0566763981096883, - "timestamp": 1.5827370078850675 - }, - { - "x": 2.540795163397632, - "y": 4.603165195262531, - "heading": -0.541383229045499, - "angularVelocity": 0.755150608064052, - "velocityX": -0.7592717915230448, - "velocityY": 1.3053116689836102, - "timestamp": 1.6586121327341168 - }, - { - "x": 2.4668060178101583, - "y": 4.721064600465768, - "heading": -0.4734677826566921, - "angularVelocity": 0.8950950199273465, - "velocityX": -0.9751436420635095, - "velocityY": 1.5538611031988796, - "timestamp": 1.734487257583166 - }, - { - "x": 2.3764209643089336, - "y": 4.857808746061696, - "heading": -0.3949264705539342, - "angularVelocity": 1.0351391481588907, - "velocityX": -1.1912343298399446, - "velocityY": 1.8022263010181736, - "timestamp": 1.8103623824322153 - }, - { - "x": 2.269588305263905, - "y": 5.013353141552007, - "heading": -0.3057482687442009, - "angularVelocity": 1.175328567657553, - "velocityX": -1.4080063691172056, - "velocityY": 2.0500051340898513, - "timestamp": 1.8862375072812645 - }, - { - "x": 2.155512806030656, - "y": 5.145840094887286, - "heading": -0.22945454409592414, - "angularVelocity": 1.0055169569743476, - "velocityX": -1.5034637433586158, - "velocityY": 1.7461184228535829, - "timestamp": 1.9621126321303137 - }, - { - "x": 2.0576690248190195, - "y": 5.259344023095087, - "heading": -0.1639803843714035, - "angularVelocity": 0.8629199603276121, - "velocityX": -1.289537004472555, - "velocityY": 1.4959306944600357, - "timestamp": 2.037987756979363 - }, - { - "x": 1.9761088976814785, - "y": 5.353909559223238, - "heading": -0.10935658983885492, - "angularVelocity": 0.7199170300053718, - "velocityX": -1.0749257717858158, - "velocityY": 1.2463312095522094, - "timestamp": 2.113862881828412 - }, - { - "x": 1.9108492511031485, - "y": 5.429551680381221, - "heading": -0.06562174275393094, - "angularVelocity": 0.5764056029164016, - "velocityX": -0.8600927735973349, - "velocityY": 0.9969291162074897, - "timestamp": 2.1897380066774614 - }, - { - "x": 1.8618983395529738, - "y": 5.486277925937523, - "heading": -0.03280716495777854, - "angularVelocity": 0.4324813680566584, - "velocityX": -0.6451509852228532, - "velocityY": 0.7476263883465671, - "timestamp": 2.2656131315265107 - }, - { - "x": 1.8292611562031649, - "y": 5.524092810268815, - "heading": -0.010931847108471086, - "angularVelocity": 0.2883068448697581, - "velocityX": -0.430143389085703, - "velocityY": 0.4983831579385639, - "timestamp": 2.34148825637556 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": -1.6776667047563746e-20, - "angularVelocity": 0.1440768253130289, - "velocityX": -0.2150897630156809, - "velocityY": 0.24917859900600148, - "timestamp": 2.417363381224609 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 1.1170600847702852e-20, - "angularVelocity": 5.938902420193292e-19, - "velocityX": -2.686779016810018e-17, - "velocityY": -1.2969153357483792e-17, - "timestamp": 2.4932385060736584 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -1.1581045676284431e-20, + "angularVelocity": -1.6335002675710077e-20, + "velocityX": -6.634658420439012e-20, + "velocityY": -8.04935442938013e-20, + "timestamp": 0 + }, + { + "x": 1.3060567728280696, + "y": 5.572381922263347, + "heading": -0.010695267009096829, + "angularVelocity": -0.14212861592009643, + "velocityX": 0.21371678996078675, + "velocityY": -0.2468070102016555, + "timestamp": 0.07525062380899862 + }, + { + "x": 1.3382215627560385, + "y": 5.535237421485166, + "heading": -0.032077296039988955, + "angularVelocity": -0.2841442096900626, + "velocityX": 0.42743552544639934, + "velocityY": -0.4936105363386875, + "timestamp": 0.15050124761799724 + }, + { + "x": 1.3864692681213642, + "y": 5.4795210587499135, + "heading": -0.06412402718123793, + "angularVelocity": -0.4258666509209292, + "velocityX": 0.6411602047019342, + "velocityY": -0.7404106426635293, + "timestamp": 0.22575187142699588 + }, + { + "x": 1.450800660367866, + "y": 5.405233169697022, + "heading": -0.10680235748792238, + "angularVelocity": -0.567149189553707, + "velocityX": 0.8548951356175836, + "velocityY": -0.9872062887006725, + "timestamp": 0.3010024952359945 + }, + { + "x": 1.5312168522436431, + "y": 5.312374273972817, + "heading": -0.16007184436590008, + "angularVelocity": -0.7078942895302298, + "velocityX": 1.0686448537608113, + "velocityY": -1.2339950291960333, + "timestamp": 0.3762531190449931 + }, + { + "x": 1.627719315017737, + "y": 5.20094520381907, + "heading": -0.22388950072960903, + "angularVelocity": -0.8480681372913416, + "velocityX": 1.2824141234900144, + "velocityY": -1.4807727100917756, + "timestamp": 0.4515037428539917 + }, + { + "x": 1.7403099620745723, + "y": 5.070947296194424, + "heading": -0.29821515111728925, + "angularVelocity": -0.9877080962987606, + "velocityX": 1.4962088200439807, + "velocityY": -1.727532624242517, + "timestamp": 0.5267543666629904 + }, + { + "x": 1.8689918213422534, + "y": 4.922383062014296, + "heading": -0.3830159737868923, + "angularVelocity": -1.1269118895923678, + "velocityX": 1.7100437545116134, + "velocityY": -1.9742591710231445, + "timestamp": 0.602004990471989 + }, + { + "x": 2.0218156992973517, + "y": 4.778590406876992, + "heading": -0.46245085296190774, + "angularVelocity": -1.0556042615226333, + "velocityX": 2.030865263562416, + "velocityY": -1.910850008396984, + "timestamp": 0.6772556142809876 + }, + { + "x": 2.158552472181925, + "y": 4.653367084915143, + "heading": -0.5314313533845169, + "angularVelocity": -0.9166767919119934, + "velocityX": 1.8170849085801528, + "velocityY": -1.6640835068649051, + "timestamp": 0.7525062380899862 + }, + { + "x": 2.27919988810187, + "y": 4.546710962687177, + "heading": -0.5899619594153146, + "angularVelocity": -0.7778089146391693, + "velocityX": 1.603274628342918, + "velocityY": -1.4173453564807712, + "timestamp": 0.8277568618989848 + }, + { + "x": 2.3837569267821266, + "y": 4.458620951443789, + "heading": -0.6380431409710475, + "angularVelocity": -0.6389472820553975, + "velocityX": 1.3894507897455282, + "velocityY": -1.1706216744060274, + "timestamp": 0.9030074857079834 + }, + { + "x": 2.4722229729489946, + "y": 4.389096340497206, + "heading": -0.6756737748841694, + "angularVelocity": -0.5000707237807894, + "velocityX": 1.17561877482123, + "velocityY": -0.9239074366087717, + "timestamp": 0.978258109516982 + }, + { + "x": 2.544597634609631, + "y": 4.338136648836026, + "heading": -0.7028521537713689, + "angularVelocity": -0.3611714762150514, + "velocityX": 0.9617815507328988, + "velocityY": -0.6771995909366235, + "timestamp": 1.0535087333259807 + }, + { + "x": 2.6008806784620506, + "y": 4.30574156635696, + "heading": -0.7195764091910871, + "angularVelocity": -0.22224739906698765, + "velocityX": 0.7479412263116554, + "velocityY": -0.4304958662042791, + "timestamp": 1.1287593571349794 + }, + { + "x": 2.6410720024572596, + "y": 4.291910925379844, + "heading": -0.7258446711788211, + "angularVelocity": -0.08329847209830624, + "velocityX": 0.5340995457688548, + "velocityY": -0.1837943697612413, + "timestamp": 1.2040099809439782 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05567470235645032, + "velocityX": 0.3202580862836482, + "velocityY": 0.06290661835254617, + "timestamp": 1.2792606047529769 + }, + { + "x": 2.6731116609802053, + "y": 4.320290724350463, + "heading": -0.7067974022392469, + "angularVelocity": 0.19582218410154278, + "velocityX": 0.10464837684480303, + "velocityY": 0.31165083052347464, + "timestamp": 1.3551340964364653 + }, + { + "x": 2.664692760034393, + "y": 4.362810179364145, + "heading": -0.6813103687909546, + "angularVelocity": 0.3359148614724796, + "velocityX": -0.11095971411111749, + "velocityY": 0.5603993446229535, + "timestamp": 1.4310075881199538 + }, + { + "x": 2.63991513669479, + "y": 4.424203513438027, + "heading": -0.6451981318661814, + "angularVelocity": 0.4759532759533194, + "velocityX": -0.32656495423941434, + "velocityY": 0.8091539312568886, + "timestamp": 1.5068810798034422 + }, + { + "x": 2.598779109919189, + "y": 4.50447133226996, + "heading": -0.5984640990912053, + "angularVelocity": 0.6159467784866202, + "velocityX": -0.5421659905570602, + "velocityY": 1.0579165008877693, + "timestamp": 1.5827545714869307 + }, + { + "x": 2.54128511679975, + "y": 4.60361438359764, + "heading": -0.5411094256407595, + "angularVelocity": 0.7559250560090859, + "velocityX": -0.7577612660727276, + "velocityY": 1.3066889255771041, + "timestamp": 1.658628063170419 + }, + { + "x": 2.467433718348128, + "y": 4.721633509490798, + "heading": -0.4731300084226628, + "angularVelocity": 0.8959574116040114, + "velocityX": -0.9733491475481174, + "velocityY": 1.5554724486054035, + "timestamp": 1.7345015548539076 + }, + { + "x": 2.377225511297283, + "y": 4.8585294609234, + "heading": -0.3945113768792691, + "angularVelocity": 1.036180486741758, + "velocityX": -1.1889291641823332, + "velocityY": 1.8042658693456888, + "timestamp": 1.810375046537396 + }, + { + "x": 2.2706603587085, + "y": 5.014302055693198, + "heading": -0.3052212812212148, + "angularVelocity": 1.1768286087390614, + "velocityX": -1.4045109856460358, + "velocityY": 2.053056888690639, + "timestamp": 1.8862485382208845 + }, + { + "x": 2.156230031792854, + "y": 5.146478320337647, + "heading": -0.22909477561234234, + "angularVelocity": 1.003334681451587, + "velocityX": -1.508172675023323, + "velocityY": 1.7420611825251466, + "timestamp": 1.962122029904373 + }, + { + "x": 2.058146916882525, + "y": 5.2597707167035095, + "heading": -0.1637376643935269, + "angularVelocity": 0.8613958547137548, + "velocityX": -1.2927191399005253, + "velocityY": 1.493174939654406, + "timestamp": 2.0379955215878613 + }, + { + "x": 1.9764115241122762, + "y": 5.354180474443043, + "heading": -0.10920153692146195, + "angularVelocity": 0.7187770888357993, + "velocityX": -1.0772588812864141, + "velocityY": 1.2443049033959122, + "timestamp": 2.1138690132713496 + }, + { + "x": 1.911023503907161, + "y": 5.429708017230294, + "heading": -0.06553190171122841, + "angularVelocity": 0.5755585282987177, + "velocityX": -0.861803230011952, + "velocityY": 0.9954404510908861, + "timestamp": 2.189742504954838 + }, + { + "x": 1.8619825163381478, + "y": 5.486353593028934, + "heading": -0.0327635757896738, + "angularVelocity": 0.43188108513906287, + "velocityX": -0.646352058945582, + "velocityY": 0.7465792669057553, + "timestamp": 2.265615996638326 + }, + { + "x": 1.8292883956003307, + "y": 5.524117338037825, + "heading": -0.01091770284008431, + "angularVelocity": 0.2879249717506227, + "velocityX": -0.4309030731602948, + "velocityY": 0.49771987779902493, + "timestamp": 2.3414894883218142 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 2.996746418062504e-20, + "angularVelocity": 0.14389350744036247, + "velocityX": -0.21545340351406272, + "velocityY": 0.24886069062260008, + "timestamp": 2.4173629800053025 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.4094054867508415e-20, + "angularVelocity": -1.911842718131469e-20, + "velocityX": -7.069031221189556e-20, + "velocityY": -4.343771313501213e-20, + "timestamp": 2.4932364716887907 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBA.2.traj b/src/main/deploy/choreo/CenterLanePCBA.2.traj index 637f6b56..56220181 100644 --- a/src/main/deploy/choreo/CenterLanePCBA.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBA.2.traj @@ -1,94 +1,95 @@ { - "samples": [ - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 1.1170600847702852e-20, - "angularVelocity": 5.938902420193292e-19, - "velocityX": -2.686779016810018e-17, - "velocityY": -1.2969153357483792e-17, - "timestamp": 0 - }, - { - "x": 1.8552093951788071, - "y": 5.543066072468525, - "heading": -1.0867013680305264e-17, - "angularVelocity": -1.1051820926914384e-16, - "velocityX": 0.42942883009919236, - "velocityY": 0.0006787122433536914, - "timestamp": 0.09842888654778115 - }, - { - "x": 1.9397457967809342, - "y": 5.543199682246804, - "heading": -3.2635799657096195e-17, - "angularVelocity": -2.2116257473548814e-16, - "velocityX": 0.8588576440015888, - "velocityY": 0.001357424461108283, - "timestamp": 0.1968577730955623 - }, - { - "x": 2.0665503951985436, - "y": 5.5434000969079245, - "heading": -4.603800029490119e-17, - "angularVelocity": -1.3616125416355767e-16, - "velocityX": 1.2882864255103954, - "velocityY": 0.002036136627664645, - "timestamp": 0.29528665964334344 - }, - { - "x": 2.235623180866242, - "y": 5.543667316436768, - "heading": -4.798133527402523e-17, - "angularVelocity": -1.9743543259972466e-17, - "velocityX": 1.7177151098384498, - "velocityY": 0.002714848640626296, - "timestamp": 0.3937155461911246 - }, - { - "x": 2.40469596653394, - "y": 5.54393453596561, - "heading": -3.9330353852399465e-17, - "angularVelocity": 8.789067645262313e-17, - "velocityX": 1.7177151098384498, - "velocityY": 0.0027148486406302456, - "timestamp": 0.49214443273890573 - }, - { - "x": 2.531500564951549, - "y": 5.544134950626731, - "heading": -2.0009563266152006e-17, - "angularVelocity": 1.96291873894211e-16, - "velocityX": 1.2882864255103954, - "velocityY": 0.002036136627666603, - "timestamp": 0.5905733192866869 - }, - { - "x": 2.616036966553676, - "y": 5.54426856040501, - "heading": -7.02269200923652e-18, - "angularVelocity": 1.3194166582538958e-16, - "velocityX": 0.8588576440015888, - "velocityY": 0.0013574244611093242, - "timestamp": 0.689002205834468 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -3.8911537114769476e-19, - "angularVelocity": 6.739461219451495e-17, - "velocityX": 0.4294288300991924, - "velocityY": 0.0006787122433541438, - "timestamp": 0.7874310923822492 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -4.27312910681112e-19, - "angularVelocity": -3.880724585667238e-19, - "velocityX": -2.3908482729518656e-18, - "velocityY": 6.838225330043828e-18, - "timestamp": 0.8858599789300303 - } - ] + "samples": [ + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.4094054867508415e-20, + "angularVelocity": -1.911842718131469e-20, + "velocityX": -7.069031221189556e-20, + "velocityY": -4.343771313501213e-20, + "timestamp": 0 + }, + { + "x": 1.8552093934543321, + "y": 5.543066072465799, + "heading": -2.0527826070429256e-18, + "angularVelocity": -2.0998680789192775e-17, + "velocityX": 0.4294288348656248, + "velocityY": 0.000678712250886674, + "timestamp": 0.09842888143953177 + }, + { + "x": 1.939745792565551, + "y": 5.543199682240142, + "heading": -6.048126842026002e-18, + "angularVelocity": -4.05911778781429e-17, + "velocityX": 0.8588576632677929, + "velocityY": 0.0013574244915579316, + "timestamp": 0.19685776287906354 + }, + { + "x": 2.066550389641902, + "y": 5.543400096899142, + "heading": -1.1691231309928304e-17, + "angularVelocity": -5.733179515246905e-17, + "velocityX": 1.2882864787430477, + "velocityY": 0.0020361367117983195, + "timestamp": 0.2952866443185953 + }, + { + "x": 2.2356231808662415, + "y": 5.543667316436768, + "heading": 9.487524674650316e-21, + "angularVelocity": 1.188748532301011e-16, + "velocityX": 1.7177152554375654, + "velocityY": 0.0027148488707459404, + "timestamp": 0.39371552575812707 + }, + { + "x": 2.4046959720905807, + "y": 5.543934535974393, + "heading": 1.1697892357164231e-17, + "angularVelocity": 1.187497476520098e-16, + "velocityX": 1.7177152554375656, + "velocityY": 0.002714848870745941, + "timestamp": 0.49214440719765884 + }, + { + "x": 2.531500569166932, + "y": 5.544134950633393, + "heading": 6.051439837245386e-18, + "angularVelocity": -5.736581008759357e-17, + "velocityX": 1.2882864787430477, + "velocityY": 0.00203613671179832, + "timestamp": 0.5905732886371906 + }, + { + "x": 2.616036968278151, + "y": 5.544268560407736, + "heading": 2.0590195546334623e-18, + "angularVelocity": -4.0561471634117896e-17, + "velocityX": 0.8588576632677929, + "velocityY": 0.0013574244915579318, + "timestamp": 0.6890021700767224 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -7.01229032287185e-26, + "angularVelocity": -2.0918856279971937e-17, + "velocityX": 0.4294288348656248, + "velocityY": 0.0006787122508866741, + "timestamp": 0.7874310515162541 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -7.502242975241614e-26, + "angularVelocity": -4.851342689713789e-26, + "velocityX": 1.33942470461485e-25, + "velocityY": -1.0236263470109501e-25, + "timestamp": 0.8858599329557859 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBA.3.traj b/src/main/deploy/choreo/CenterLanePCBA.3.traj index d48ace6f..48756bc2 100644 --- a/src/main/deploy/choreo/CenterLanePCBA.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBA.3.traj @@ -1,193 +1,194 @@ { - "samples": [ - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -4.27312910681112e-19, - "angularVelocity": -3.880724585667238e-19, - "velocityX": -2.3908482729518656e-18, - "velocityY": 6.838225330043828e-18, - "timestamp": 0 - }, - { - "x": 2.6438893629989613, - "y": 5.563919104052164, - "heading": 0.003139586303789949, - "angularVelocity": 0.042042846518139325, - "velocityX": -0.19304501448070868, - "velocityY": 0.26224987725536203, - "timestamp": 0.07467587387155739 - }, - { - "x": 2.6152669032432256, - "y": 5.6032366820496495, - "heading": 0.009644303445155527, - "angularVelocity": 0.0871060063194363, - "velocityX": -0.38328925088933385, - "velocityY": 0.5265097809918172, - "timestamp": 0.14935174774311477 - }, - { - "x": 2.5727487902328847, - "y": 5.662504910556293, - "heading": 0.019847811829768587, - "angularVelocity": 0.13663728130135686, - "velocityX": -0.5693688042201159, - "velocityY": 0.7936730490571009, - "timestamp": 0.22402762161467216 - }, - { - "x": 2.5168455054942007, - "y": 5.742063733198033, - "heading": 0.03429127316793741, - "angularVelocity": 0.19341536415109115, - "velocityX": -0.7486123943435556, - "velocityY": 1.0653885721991296, - "timestamp": 0.29870349548622954 - }, - { - "x": 2.4485453653591587, - "y": 5.842518348175502, - "heading": 0.05399784068706335, - "angularVelocity": 0.2638947024981649, - "velocityX": -0.9146212370077963, - "velocityY": 1.3452084290335973, - "timestamp": 0.3733793693577869 - }, - { - "x": 2.370517685528427, - "y": 5.96519344251776, - "heading": 0.08158842838664396, - "angularVelocity": 0.369471239761256, - "velocityX": -1.0448847236115244, - "velocityY": 1.642767442578038, - "timestamp": 0.4480552432293443 - }, - { - "x": 2.3003293154277267, - "y": 6.110432122884094, - "heading": 0.1284873855211158, - "angularVelocity": 0.6280335897392839, - "velocityX": -0.9399069131942741, - "velocityY": 1.94492106802986, - "timestamp": 0.5227311171009017 - }, - { - "x": 2.2517603849650345, - "y": 6.244622565856429, - "heading": 0.18176076028404833, - "angularVelocity": 0.7133947284575884, - "velocityX": -0.6503965463628967, - "velocityY": 1.796971846665519, - "timestamp": 0.5974069909724591 - }, - { - "x": 2.22192407028561, - "y": 6.363296472335711, - "heading": 0.23706248397325824, - "angularVelocity": 0.7405567664909981, - "velocityX": -0.39954423206005096, - "velocityY": 1.5891867122091021, - "timestamp": 0.6720828648440165 - }, - { - "x": 2.2097448289139274, - "y": 6.465233576489708, - "heading": 0.2929845885143906, - "angularVelocity": 0.7488644141924427, - "velocityX": -0.16309472846117115, - "velocityY": 1.3650607467858884, - "timestamp": 0.7467587387155739 - }, - { - "x": 2.2146777318065234, - "y": 6.549876478070479, - "heading": 0.348839415183568, - "angularVelocity": 0.747963482359082, - "velocityX": 0.06605751813605938, - "velocityY": 1.1334705198944957, - "timestamp": 0.8214346125871312 - }, - { - "x": 2.236395128424509, - "y": 6.61690736376859, - "heading": 0.4042192057980219, - "angularVelocity": 0.7416021767580177, - "velocityX": 0.2908221289159501, - "velocityY": 0.8976243895505626, - "timestamp": 0.8961104864586886 - }, - { - "x": 2.274678638341779, - "y": 6.666121192465354, - "heading": 0.45885367997336884, - "angularVelocity": 0.7316214908889929, - "velocityX": 0.5126623624534666, - "velocityY": 0.6590325113759182, - "timestamp": 0.970786360330246 - }, - { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "angularVelocity": 0.719064094502462, - "velocityX": 0.7324155022029422, - "velocityY": 0.41852376977375944, - "timestamp": 1.0454622342018034 - }, - { - "x": 2.4110936768539344, - "y": 6.744072759574757, - "heading": 0.5643107226604884, - "angularVelocity": 0.653664165942785, - "velocityX": 1.0320315606781885, - "velocityY": 0.5897332019890127, - "timestamp": 1.1246470905199306 - }, - { - "x": 2.50599401367539, - "y": 6.798301604540289, - "heading": 0.5436072588792712, - "angularVelocity": -0.2614573637418783, - "velocityX": 1.198465732389427, - "velocityY": 0.6848385851414024, - "timestamp": 1.2038319468380578 - }, - { - "x": 2.577169265515479, - "y": 6.838973237821007, - "heading": 0.528079022912807, - "angularVelocity": -0.19610108155123726, - "velocityX": 0.8988492894921869, - "velocityY": 0.51362893325611, - "timestamp": 1.283016803156185 - }, - { - "x": 2.6246194330616937, - "y": 6.8660876598097635, - "heading": 0.5177266590936774, - "angularVelocity": -0.13073666229232678, - "velocityX": 0.5992328552770498, - "velocityY": 0.3424192863320269, - "timestamp": 1.3622016594743123 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.06536905835734146, - "velocityX": 0.2996164266200211, - "velocityY": 0.1712096425840104, - "timestamp": 1.4413865157924395 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -8.381816905081638e-20, - "velocityX": -7.007327358505968e-19, - "velocityY": 8.954594613208896e-19, - "timestamp": 1.5205713721105667 - } - ] + "samples": [ + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -7.502242975241614e-26, + "angularVelocity": -4.851342689713789e-26, + "velocityX": 1.33942470461485e-25, + "velocityY": -1.0236263470109501e-25, + "timestamp": 0 + }, + { + "x": 2.643889442719825, + "y": 5.563919137234555, + "heading": 0.0031393268173354576, + "angularVelocity": 0.04203939285338829, + "velocityX": -0.19304404414258594, + "velocityY": 0.26225045368154326, + "timestamp": 0.07467583626347318 + }, + { + "x": 2.615267140219649, + "y": 5.603236779535534, + "heading": 0.009643531258689539, + "angularVelocity": 0.08709918451272194, + "velocityX": -0.3832873380780111, + "velocityY": 0.5265109072532814, + "timestamp": 0.14935167252694637 + }, + { + "x": 2.5727492586123577, + "y": 5.662505100435592, + "heading": 0.019846282739514817, + "angularVelocity": 0.13662721425479105, + "velocityX": -0.5693659921969727, + "velocityY": 0.7936746860248878, + "timestamp": 0.22402750879041955 + }, + { + "x": 2.516846273518896, + "y": 5.742064038683667, + "heading": 0.03428875734419064, + "angularVelocity": 0.19340224800053757, + "velocityX": -0.7486087587452387, + "velocityY": 1.0653906568568225, + "timestamp": 0.29870334505389273 + }, + { + "x": 2.448546488675387, + "y": 5.842518783466547, + "heading": 0.05399413519442509, + "angularVelocity": 0.2638789042912005, + "velocityX": -0.9146169398429232, + "velocityY": 1.3452108447564306, + "timestamp": 0.3733791813173659 + }, + { + "x": 2.3705191792906315, + "y": 5.9651940015137335, + "heading": 0.0815833991238304, + "angularVelocity": 0.36945369894572166, + "velocityX": -1.0448802891132984, + "velocityY": 1.6427699264640412, + "timestamp": 0.4480550175808391 + }, + { + "x": 2.300330937619818, + "y": 6.110432887633571, + "heading": 0.12848151048537504, + "angularVelocity": 0.6280225801031192, + "velocityX": -0.9399056667162509, + "velocityY": 1.944924802815761, + "timestamp": 0.5227308538443123 + }, + { + "x": 2.2517620633069444, + "y": 6.244623434702984, + "heading": 0.18175459499441046, + "angularVelocity": 0.7133912008842571, + "velocityX": -0.6503961220000475, + "velocityY": 1.7969741456387136, + "timestamp": 0.5974066901077855 + }, + { + "x": 2.2219257094452107, + "y": 6.36329736060272, + "heading": 0.23705644760887684, + "angularVelocity": 0.7405588659141199, + "velocityX": -0.39954495797628836, + "velocityY": 1.589187772615341, + "timestamp": 0.6720825263712586 + }, + { + "x": 2.2097463319071795, + "y": 6.465234411607719, + "heading": 0.29297905925002543, + "angularVelocity": 0.7488715820180554, + "velocityX": -0.16309663403111632, + "velocityY": 1.3650607225253184, + "timestamp": 0.7467583626347323 + }, + { + "x": 2.2146790021354428, + "y": 6.549877194214307, + "heading": 0.3488347521561843, + "angularVelocity": 0.7479754590104253, + "velocityX": 0.06605443574625332, + "velocityY": 1.133469497521908, + "timestamp": 0.8214341988982059 + }, + { + "x": 2.2363960702944423, + "y": 6.616907899307652, + "heading": 0.40421575733503773, + "angularVelocity": 0.7416188147322054, + "velocityX": 0.29081787691505323, + "velocityY": 0.8976224230933981, + "timestamp": 0.8961100351616795 + }, + { + "x": 2.2746791565921045, + "y": 6.66612148860884, + "heading": 0.45885178746831873, + "angularVelocity": 0.7316426955101331, + "velocityX": 0.5126569478591565, + "velocityY": 0.6590296374794072, + "timestamp": 0.9707858714251532 + }, + { + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.7190897995734556, + "velocityX": 0.7324089310601718, + "velocityY": 0.4185200148294202, + "timestamp": 1.0454617076886268 + }, + { + "x": 2.4110932710082595, + "y": 6.744072527662596, + "heading": 0.564314297915607, + "angularVelocity": 0.6537087472117014, + "velocityX": 1.0320255363414028, + "velocityY": 0.5897297595057032, + "timestamp": 1.124646632988155 + }, + { + "x": 2.50599376719455, + "y": 6.798301463693884, + "heading": 0.5436087676029941, + "angularVelocity": -0.2614832335105605, + "velocityX": 1.1984667009195895, + "velocityY": 0.6848391385880369, + "timestamp": 1.2038315582876833 + }, + { + "x": 2.5771691415050153, + "y": 6.8389731669577785, + "heading": 0.5280796125906262, + "angularVelocity": -0.19611251704319405, + "velocityX": 0.8988500531033392, + "velocityY": 0.5136293696059923, + "timestamp": 1.2830164835872115 + }, + { + "x": 2.624619391553792, + "y": 6.866087636090928, + "heading": 0.5177268190614664, + "angularVelocity": -0.13074197506657764, + "velocityX": 0.5992333751568124, + "velocityY": 0.34241958340662104, + "timestamp": 1.3622014088867398 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06537102159136951, + "velocityX": 0.29961668980067113, + "velocityY": 0.1712097929731781, + "timestamp": 1.441386334186268 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -6.678803262228469e-27, + "velocityX": 1.419138537377677e-18, + "velocityY": -2.483488681188097e-18, + "timestamp": 1.5205712594857963 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBA.traj b/src/main/deploy/choreo/CenterLanePCBA.traj index 7ac60797..650325fd 100644 --- a/src/main/deploy/choreo/CenterLanePCBA.traj +++ b/src/main/deploy/choreo/CenterLanePCBA.traj @@ -1,571 +1,572 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.7297728380351187e-19, - "angularVelocity": 4.2448351957919954e-19, - "velocityX": -3.109246931729259e-17, - "velocityY": 1.4154954258989747e-18, - "timestamp": 0 - }, - { - "x": 1.3060810448259774, - "y": 5.572403683390414, - "heading": -0.01067910748684314, - "angularVelocity": -0.14191654637088458, - "velocityX": 0.21404337049401068, - "velocityY": -0.246522471704611, - "timestamp": 0.07524920638169826 - }, - { - "x": 1.338296279992689, - "y": 5.535304313687478, - "heading": -0.032027558921844806, - "angularVelocity": -0.28370334335098873, - "velocityX": 0.4281139525027307, - "velocityY": -0.4930200793709309, - "timestamp": 0.1504984127633965 - }, - { - "x": 1.3866231363779933, - "y": 5.479658598357654, - "heading": -0.06402169471627948, - "angularVelocity": -0.42517572387591435, - "velocityX": 0.6422241337702286, - "velocityY": -0.7394857435109029, - "timestamp": 0.22574761914509478 - }, - { - "x": 1.4510659275520246, - "y": 5.405469878379882, - "heading": -0.1066262273585151, - "angularVelocity": -0.5661791624235272, - "velocityX": 0.8563916388334698, - "velocityY": -0.9859070087913582, - "timestamp": 0.300996825526793 - }, - { - "x": 1.5316310742105061, - "y": 5.312743180460654, - "heading": -0.15979745091713102, - "angularVelocity": -0.7066017851257455, - "velocityX": 1.0706444696567097, - "velocityY": -1.2322614732810149, - "timestamp": 0.37624603190849126 - }, - { - "x": 1.6283288838862067, - "y": 5.201486845299536, - "heading": -0.22348688396132305, - "angularVelocity": -0.8463801295274643, - "velocityX": 1.2850342791041873, - "velocityY": -1.4785050967404112, - "timestamp": 0.4514952382901895 - }, - { - "x": 1.7411789077586277, - "y": 5.071717216156576, - "heading": -0.2976431936929222, - "angularVelocity": -0.9854763033040427, - "velocityX": 1.4996839076304158, - "velocityY": -1.7245315317294676, - "timestamp": 0.5267444446718877 - }, - { - "x": 1.8702368010806378, - "y": 4.923481749892843, - "heading": -0.3821997162379228, - "angularVelocity": -1.1236865690804896, - "velocityX": 1.7150731486446302, - "velocityY": -1.9699273041075376, - "timestamp": 0.601993651053586 - }, - { - "x": 2.022770759979851, - "y": 4.779444476824575, - "heading": -0.46182371324885607, - "angularVelocity": -1.0581373656886817, - "velocityX": 2.0270507322765163, - "velocityY": -1.914136772919067, - "timestamp": 0.6772428574352843 - }, - { - "x": 2.159313623811303, - "y": 4.6540559405768205, - "heading": -0.5309316944889129, - "angularVelocity": -0.9183881739497737, - "velocityX": 1.8145422443221764, - "velocityY": -1.666310414116386, - "timestamp": 0.7524920638169826 - }, - { - "x": 2.2798095691663756, - "y": 4.547269113259306, - "heading": -0.5895622742606058, - "angularVelocity": -0.7791521344995035, - "velocityX": 1.601291909229005, - "velocityY": -1.4191090172553904, - "timestamp": 0.8277412701986809 - }, - { - "x": 2.3842397515062683, - "y": 4.459067906121371, - "heading": -0.6377272819831894, - "angularVelocity": -0.6400732982918503, - "velocityX": 1.3877911457302048, - "velocityY": -1.172121426644562, - "timestamp": 0.9029904765803792 - }, - { - "x": 2.47259465372947, - "y": 4.389444105411809, - "heading": -0.675431270833758, - "angularVelocity": -0.5010549700591305, - "velocityX": 1.1741639077877584, - "velocityY": -0.9252429900248669, - "timestamp": 0.9782396829620775 - }, - { - "x": 2.5448685468329297, - "y": 4.338392728081247, - "heading": -0.7026759403224371, - "angularVelocity": -0.36205922691677456, - "velocityX": 0.9604605361114344, - "velocityY": -0.6784307740288329, - "timestamp": 1.0534888893437757 - }, - { - "x": 2.601057641331632, - "y": 4.305910462744002, - "heading": -0.7194616948028688, - "angularVelocity": -0.2230688573014308, - "velocityX": 0.7467068052991092, - "velocityY": -0.43166256362226185, - "timestamp": 1.128738095725474 - }, - { - "x": 2.641159295921209, - "y": 4.291994997972796, - "heading": -0.7257882885399572, - "angularVelocity": -0.08407522206898749, - "velocityX": 0.5329179737262918, - "velocityY": -0.18492507018124238, - "timestamp": 1.2039873021071723 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.054926472219667455, - "velocityX": 0.31910406053771284, - "velocityY": 0.061790547746105144, - "timestamp": 1.2792365084888706 - }, - { - "x": 2.6730242117812852, - "y": 4.320207160810404, - "heading": -0.7068522062263088, - "angularVelocity": 0.19509567726207716, - "velocityX": 0.10349358326673892, - "velocityY": 0.31054279257798323, - "timestamp": 1.3551116333379198 - }, - { - "x": 2.6645156104831624, - "y": 4.362642495661916, - "heading": -0.6814188000083412, - "angularVelocity": 0.33520084834820957, - "velocityX": -0.1121395360458166, - "velocityY": 0.5592786164892918, - "timestamp": 1.430986758186969 - }, - { - "x": 2.6396435810539995, - "y": 4.423949055779942, - "heading": -0.6453599487845141, - "angularVelocity": 0.47523943183649325, - "velocityX": -0.32780215490271986, - "velocityY": 0.807992873026772, - "timestamp": 1.5068618830360183 - }, - { - "x": 2.5984050053739445, - "y": 4.5041245094115245, - "heading": -0.5986803757122448, - "angularVelocity": 0.615215766235525, - "velocityX": -0.5435058691760214, - "velocityY": 1.0566763981096883, - "timestamp": 1.5827370078850675 - }, - { - "x": 2.540795163397632, - "y": 4.603165195262531, - "heading": -0.541383229045499, - "angularVelocity": 0.755150608064052, - "velocityX": -0.7592717915230448, - "velocityY": 1.3053116689836102, - "timestamp": 1.6586121327341168 - }, - { - "x": 2.4668060178101583, - "y": 4.721064600465768, - "heading": -0.4734677826566921, - "angularVelocity": 0.8950950199273465, - "velocityX": -0.9751436420635095, - "velocityY": 1.5538611031988796, - "timestamp": 1.734487257583166 - }, - { - "x": 2.3764209643089336, - "y": 4.857808746061696, - "heading": -0.3949264705539342, - "angularVelocity": 1.0351391481588907, - "velocityX": -1.1912343298399446, - "velocityY": 1.8022263010181736, - "timestamp": 1.8103623824322153 - }, - { - "x": 2.269588305263905, - "y": 5.013353141552007, - "heading": -0.3057482687442009, - "angularVelocity": 1.175328567657553, - "velocityX": -1.4080063691172056, - "velocityY": 2.0500051340898513, - "timestamp": 1.8862375072812645 - }, - { - "x": 2.155512806030656, - "y": 5.145840094887286, - "heading": -0.22945454409592414, - "angularVelocity": 1.0055169569743476, - "velocityX": -1.5034637433586158, - "velocityY": 1.7461184228535829, - "timestamp": 1.9621126321303137 - }, - { - "x": 2.0576690248190195, - "y": 5.259344023095087, - "heading": -0.1639803843714035, - "angularVelocity": 0.8629199603276121, - "velocityX": -1.289537004472555, - "velocityY": 1.4959306944600357, - "timestamp": 2.037987756979363 - }, - { - "x": 1.9761088976814785, - "y": 5.353909559223238, - "heading": -0.10935658983885492, - "angularVelocity": 0.7199170300053718, - "velocityX": -1.0749257717858158, - "velocityY": 1.2463312095522094, - "timestamp": 2.113862881828412 - }, - { - "x": 1.9108492511031485, - "y": 5.429551680381221, - "heading": -0.06562174275393094, - "angularVelocity": 0.5764056029164016, - "velocityX": -0.8600927735973349, - "velocityY": 0.9969291162074897, - "timestamp": 2.1897380066774614 - }, - { - "x": 1.8618983395529738, - "y": 5.486277925937523, - "heading": -0.03280716495777854, - "angularVelocity": 0.4324813680566584, - "velocityX": -0.6451509852228532, - "velocityY": 0.7476263883465671, - "timestamp": 2.2656131315265107 - }, - { - "x": 1.8292611562031649, - "y": 5.524092810268815, - "heading": -0.010931847108471086, - "angularVelocity": 0.2883068448697581, - "velocityX": -0.430143389085703, - "velocityY": 0.4983831579385639, - "timestamp": 2.34148825637556 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": -1.6776667047563746e-20, - "angularVelocity": 0.1440768253130289, - "velocityX": -0.2150897630156809, - "velocityY": 0.24917859900600148, - "timestamp": 2.417363381224609 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 1.1170600847702852e-20, - "angularVelocity": 5.938902420193292e-19, - "velocityX": -2.686779016810018e-17, - "velocityY": -1.2969153357483792e-17, - "timestamp": 2.4932385060736584 - }, - { - "x": 1.8552093951788071, - "y": 5.543066072468525, - "heading": -1.0867013680305264e-17, - "angularVelocity": -1.1051820926914384e-16, - "velocityX": 0.42942883009919236, - "velocityY": 0.0006787122433536914, - "timestamp": 2.5916673926214395 - }, - { - "x": 1.9397457967809342, - "y": 5.543199682246804, - "heading": -3.2635799657096195e-17, - "angularVelocity": -2.2116257473548814e-16, - "velocityX": 0.8588576440015888, - "velocityY": 0.001357424461108283, - "timestamp": 2.6900962791692207 - }, - { - "x": 2.0665503951985436, - "y": 5.5434000969079245, - "heading": -4.603800029490119e-17, - "angularVelocity": -1.3616125416355767e-16, - "velocityX": 1.2882864255103954, - "velocityY": 0.002036136627664645, - "timestamp": 2.788525165717002 - }, - { - "x": 2.235623180866242, - "y": 5.543667316436768, - "heading": -4.798133527402523e-17, - "angularVelocity": -1.9743543259972466e-17, - "velocityX": 1.7177151098384498, - "velocityY": 0.002714848640626296, - "timestamp": 2.886954052264783 - }, - { - "x": 2.40469596653394, - "y": 5.54393453596561, - "heading": -3.9330353852399465e-17, - "angularVelocity": 8.789067645262313e-17, - "velocityX": 1.7177151098384498, - "velocityY": 0.0027148486406302456, - "timestamp": 2.985382938812564 - }, - { - "x": 2.531500564951549, - "y": 5.544134950626731, - "heading": -2.0009563266152006e-17, - "angularVelocity": 1.96291873894211e-16, - "velocityX": 1.2882864255103954, - "velocityY": 0.002036136627666603, - "timestamp": 3.0838118253603453 - }, - { - "x": 2.616036966553676, - "y": 5.54426856040501, - "heading": -7.02269200923652e-18, - "angularVelocity": 1.3194166582538958e-16, - "velocityX": 0.8588576440015888, - "velocityY": 0.0013574244611093242, - "timestamp": 3.1822407119081264 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -3.8911537114769476e-19, - "angularVelocity": 6.739461219451495e-17, - "velocityX": 0.4294288300991924, - "velocityY": 0.0006787122433541438, - "timestamp": 3.2806695984559076 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -4.27312910681112e-19, - "angularVelocity": -3.880724585667238e-19, - "velocityX": -2.3908482729518656e-18, - "velocityY": 6.838225330043828e-18, - "timestamp": 3.3790984850036887 - }, - { - "x": 2.6438893629989613, - "y": 5.563919104052164, - "heading": 0.003139586303789949, - "angularVelocity": 0.042042846518139325, - "velocityX": -0.19304501448070868, - "velocityY": 0.26224987725536203, - "timestamp": 3.453774358875246 - }, - { - "x": 2.6152669032432256, - "y": 5.6032366820496495, - "heading": 0.009644303445155527, - "angularVelocity": 0.0871060063194363, - "velocityX": -0.38328925088933385, - "velocityY": 0.5265097809918172, - "timestamp": 3.5284502327468035 - }, - { - "x": 2.5727487902328847, - "y": 5.662504910556293, - "heading": 0.019847811829768587, - "angularVelocity": 0.13663728130135686, - "velocityX": -0.5693688042201159, - "velocityY": 0.7936730490571009, - "timestamp": 3.603126106618361 - }, - { - "x": 2.5168455054942007, - "y": 5.742063733198033, - "heading": 0.03429127316793741, - "angularVelocity": 0.19341536415109115, - "velocityX": -0.7486123943435556, - "velocityY": 1.0653885721991296, - "timestamp": 3.6778019804899182 - }, - { - "x": 2.4485453653591587, - "y": 5.842518348175502, - "heading": 0.05399784068706335, - "angularVelocity": 0.2638947024981649, - "velocityX": -0.9146212370077963, - "velocityY": 1.3452084290335973, - "timestamp": 3.7524778543614756 - }, - { - "x": 2.370517685528427, - "y": 5.96519344251776, - "heading": 0.08158842838664396, - "angularVelocity": 0.369471239761256, - "velocityX": -1.0448847236115244, - "velocityY": 1.642767442578038, - "timestamp": 3.827153728233033 - }, - { - "x": 2.3003293154277267, - "y": 6.110432122884094, - "heading": 0.1284873855211158, - "angularVelocity": 0.6280335897392839, - "velocityX": -0.9399069131942741, - "velocityY": 1.94492106802986, - "timestamp": 3.9018296021045904 - }, - { - "x": 2.2517603849650345, - "y": 6.244622565856429, - "heading": 0.18176076028404833, - "angularVelocity": 0.7133947284575884, - "velocityX": -0.6503965463628967, - "velocityY": 1.796971846665519, - "timestamp": 3.976505475976148 - }, - { - "x": 2.22192407028561, - "y": 6.363296472335711, - "heading": 0.23706248397325824, - "angularVelocity": 0.7405567664909981, - "velocityX": -0.39954423206005096, - "velocityY": 1.5891867122091021, - "timestamp": 4.051181349847705 - }, - { - "x": 2.2097448289139274, - "y": 6.465233576489708, - "heading": 0.2929845885143906, - "angularVelocity": 0.7488644141924427, - "velocityX": -0.16309472846117115, - "velocityY": 1.3650607467858884, - "timestamp": 4.125857223719263 - }, - { - "x": 2.2146777318065234, - "y": 6.549876478070479, - "heading": 0.348839415183568, - "angularVelocity": 0.747963482359082, - "velocityX": 0.06605751813605938, - "velocityY": 1.1334705198944957, - "timestamp": 4.20053309759082 - }, - { - "x": 2.236395128424509, - "y": 6.61690736376859, - "heading": 0.4042192057980219, - "angularVelocity": 0.7416021767580177, - "velocityX": 0.2908221289159501, - "velocityY": 0.8976243895505626, - "timestamp": 4.275208971462377 - }, - { - "x": 2.274678638341779, - "y": 6.666121192465354, - "heading": 0.45885367997336884, - "angularVelocity": 0.7316214908889929, - "velocityX": 0.5126623624534666, - "velocityY": 0.6590325113759182, - "timestamp": 4.349884845333935 - }, - { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "angularVelocity": 0.719064094502462, - "velocityX": 0.7324155022029422, - "velocityY": 0.41852376977375944, - "timestamp": 4.424560719205492 - }, - { - "x": 2.4110936768539344, - "y": 6.744072759574757, - "heading": 0.5643107226604884, - "angularVelocity": 0.653664165942785, - "velocityX": 1.0320315606781885, - "velocityY": 0.5897332019890127, - "timestamp": 4.503745575523619 - }, - { - "x": 2.50599401367539, - "y": 6.798301604540289, - "heading": 0.5436072588792712, - "angularVelocity": -0.2614573637418783, - "velocityX": 1.198465732389427, - "velocityY": 0.6848385851414024, - "timestamp": 4.5829304318417465 - }, - { - "x": 2.577169265515479, - "y": 6.838973237821007, - "heading": 0.528079022912807, - "angularVelocity": -0.19610108155123726, - "velocityX": 0.8988492894921869, - "velocityY": 0.51362893325611, - "timestamp": 4.662115288159874 - }, - { - "x": 2.6246194330616937, - "y": 6.8660876598097635, - "heading": 0.5177266590936774, - "angularVelocity": -0.13073666229232678, - "velocityX": 0.5992328552770498, - "velocityY": 0.3424192863320269, - "timestamp": 4.741300144478001 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.06536905835734146, - "velocityX": 0.2996164266200211, - "velocityY": 0.1712096425840104, - "timestamp": 4.820485000796128 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -8.381816905081638e-20, - "velocityX": -7.007327358505968e-19, - "velocityY": 8.954594613208896e-19, - "timestamp": 4.899669857114255 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -1.1581045676284431e-20, + "angularVelocity": -1.6335002675710077e-20, + "velocityX": -6.634658420439012e-20, + "velocityY": -8.04935442938013e-20, + "timestamp": 0 + }, + { + "x": 1.3060567728280696, + "y": 5.572381922263347, + "heading": -0.010695267009096829, + "angularVelocity": -0.14212861592009643, + "velocityX": 0.21371678996078675, + "velocityY": -0.2468070102016555, + "timestamp": 0.07525062380899862 + }, + { + "x": 1.3382215627560385, + "y": 5.535237421485166, + "heading": -0.032077296039988955, + "angularVelocity": -0.2841442096900626, + "velocityX": 0.42743552544639934, + "velocityY": -0.4936105363386875, + "timestamp": 0.15050124761799724 + }, + { + "x": 1.3864692681213642, + "y": 5.4795210587499135, + "heading": -0.06412402718123793, + "angularVelocity": -0.4258666509209292, + "velocityX": 0.6411602047019342, + "velocityY": -0.7404106426635293, + "timestamp": 0.22575187142699588 + }, + { + "x": 1.450800660367866, + "y": 5.405233169697022, + "heading": -0.10680235748792238, + "angularVelocity": -0.567149189553707, + "velocityX": 0.8548951356175836, + "velocityY": -0.9872062887006725, + "timestamp": 0.3010024952359945 + }, + { + "x": 1.5312168522436431, + "y": 5.312374273972817, + "heading": -0.16007184436590008, + "angularVelocity": -0.7078942895302298, + "velocityX": 1.0686448537608113, + "velocityY": -1.2339950291960333, + "timestamp": 0.3762531190449931 + }, + { + "x": 1.627719315017737, + "y": 5.20094520381907, + "heading": -0.22388950072960903, + "angularVelocity": -0.8480681372913416, + "velocityX": 1.2824141234900144, + "velocityY": -1.4807727100917756, + "timestamp": 0.4515037428539917 + }, + { + "x": 1.7403099620745723, + "y": 5.070947296194424, + "heading": -0.29821515111728925, + "angularVelocity": -0.9877080962987606, + "velocityX": 1.4962088200439807, + "velocityY": -1.727532624242517, + "timestamp": 0.5267543666629904 + }, + { + "x": 1.8689918213422534, + "y": 4.922383062014296, + "heading": -0.3830159737868923, + "angularVelocity": -1.1269118895923678, + "velocityX": 1.7100437545116134, + "velocityY": -1.9742591710231445, + "timestamp": 0.602004990471989 + }, + { + "x": 2.0218156992973517, + "y": 4.778590406876992, + "heading": -0.46245085296190774, + "angularVelocity": -1.0556042615226333, + "velocityX": 2.030865263562416, + "velocityY": -1.910850008396984, + "timestamp": 0.6772556142809876 + }, + { + "x": 2.158552472181925, + "y": 4.653367084915143, + "heading": -0.5314313533845169, + "angularVelocity": -0.9166767919119934, + "velocityX": 1.8170849085801528, + "velocityY": -1.6640835068649051, + "timestamp": 0.7525062380899862 + }, + { + "x": 2.27919988810187, + "y": 4.546710962687177, + "heading": -0.5899619594153146, + "angularVelocity": -0.7778089146391693, + "velocityX": 1.603274628342918, + "velocityY": -1.4173453564807712, + "timestamp": 0.8277568618989848 + }, + { + "x": 2.3837569267821266, + "y": 4.458620951443789, + "heading": -0.6380431409710475, + "angularVelocity": -0.6389472820553975, + "velocityX": 1.3894507897455282, + "velocityY": -1.1706216744060274, + "timestamp": 0.9030074857079834 + }, + { + "x": 2.4722229729489946, + "y": 4.389096340497206, + "heading": -0.6756737748841694, + "angularVelocity": -0.5000707237807894, + "velocityX": 1.17561877482123, + "velocityY": -0.9239074366087717, + "timestamp": 0.978258109516982 + }, + { + "x": 2.544597634609631, + "y": 4.338136648836026, + "heading": -0.7028521537713689, + "angularVelocity": -0.3611714762150514, + "velocityX": 0.9617815507328988, + "velocityY": -0.6771995909366235, + "timestamp": 1.0535087333259807 + }, + { + "x": 2.6008806784620506, + "y": 4.30574156635696, + "heading": -0.7195764091910871, + "angularVelocity": -0.22224739906698765, + "velocityX": 0.7479412263116554, + "velocityY": -0.4304958662042791, + "timestamp": 1.1287593571349794 + }, + { + "x": 2.6410720024572596, + "y": 4.291910925379844, + "heading": -0.7258446711788211, + "angularVelocity": -0.08329847209830624, + "velocityX": 0.5340995457688548, + "velocityY": -0.1837943697612413, + "timestamp": 1.2040099809439782 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05567470235645032, + "velocityX": 0.3202580862836482, + "velocityY": 0.06290661835254617, + "timestamp": 1.2792606047529769 + }, + { + "x": 2.6731116609802053, + "y": 4.320290724350463, + "heading": -0.7067974022392469, + "angularVelocity": 0.19582218410154278, + "velocityX": 0.10464837684480303, + "velocityY": 0.31165083052347464, + "timestamp": 1.3551340964364653 + }, + { + "x": 2.664692760034393, + "y": 4.362810179364145, + "heading": -0.6813103687909546, + "angularVelocity": 0.3359148614724796, + "velocityX": -0.11095971411111749, + "velocityY": 0.5603993446229535, + "timestamp": 1.4310075881199538 + }, + { + "x": 2.63991513669479, + "y": 4.424203513438027, + "heading": -0.6451981318661814, + "angularVelocity": 0.4759532759533194, + "velocityX": -0.32656495423941434, + "velocityY": 0.8091539312568886, + "timestamp": 1.5068810798034422 + }, + { + "x": 2.598779109919189, + "y": 4.50447133226996, + "heading": -0.5984640990912053, + "angularVelocity": 0.6159467784866202, + "velocityX": -0.5421659905570602, + "velocityY": 1.0579165008877693, + "timestamp": 1.5827545714869307 + }, + { + "x": 2.54128511679975, + "y": 4.60361438359764, + "heading": -0.5411094256407595, + "angularVelocity": 0.7559250560090859, + "velocityX": -0.7577612660727276, + "velocityY": 1.3066889255771041, + "timestamp": 1.658628063170419 + }, + { + "x": 2.467433718348128, + "y": 4.721633509490798, + "heading": -0.4731300084226628, + "angularVelocity": 0.8959574116040114, + "velocityX": -0.9733491475481174, + "velocityY": 1.5554724486054035, + "timestamp": 1.7345015548539076 + }, + { + "x": 2.377225511297283, + "y": 4.8585294609234, + "heading": -0.3945113768792691, + "angularVelocity": 1.036180486741758, + "velocityX": -1.1889291641823332, + "velocityY": 1.8042658693456888, + "timestamp": 1.810375046537396 + }, + { + "x": 2.2706603587085, + "y": 5.014302055693198, + "heading": -0.3052212812212148, + "angularVelocity": 1.1768286087390614, + "velocityX": -1.4045109856460358, + "velocityY": 2.053056888690639, + "timestamp": 1.8862485382208845 + }, + { + "x": 2.156230031792854, + "y": 5.146478320337647, + "heading": -0.22909477561234234, + "angularVelocity": 1.003334681451587, + "velocityX": -1.508172675023323, + "velocityY": 1.7420611825251466, + "timestamp": 1.962122029904373 + }, + { + "x": 2.058146916882525, + "y": 5.2597707167035095, + "heading": -0.1637376643935269, + "angularVelocity": 0.8613958547137548, + "velocityX": -1.2927191399005253, + "velocityY": 1.493174939654406, + "timestamp": 2.0379955215878613 + }, + { + "x": 1.9764115241122762, + "y": 5.354180474443043, + "heading": -0.10920153692146195, + "angularVelocity": 0.7187770888357993, + "velocityX": -1.0772588812864141, + "velocityY": 1.2443049033959122, + "timestamp": 2.1138690132713496 + }, + { + "x": 1.911023503907161, + "y": 5.429708017230294, + "heading": -0.06553190171122841, + "angularVelocity": 0.5755585282987177, + "velocityX": -0.861803230011952, + "velocityY": 0.9954404510908861, + "timestamp": 2.189742504954838 + }, + { + "x": 1.8619825163381478, + "y": 5.486353593028934, + "heading": -0.0327635757896738, + "angularVelocity": 0.43188108513906287, + "velocityX": -0.646352058945582, + "velocityY": 0.7465792669057553, + "timestamp": 2.265615996638326 + }, + { + "x": 1.8292883956003307, + "y": 5.524117338037825, + "heading": -0.01091770284008431, + "angularVelocity": 0.2879249717506227, + "velocityX": -0.4309030731602948, + "velocityY": 0.49771987779902493, + "timestamp": 2.3414894883218142 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 2.996746418062504e-20, + "angularVelocity": 0.14389350744036247, + "velocityX": -0.21545340351406272, + "velocityY": 0.24886069062260008, + "timestamp": 2.4173629800053025 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.4094054867508415e-20, + "angularVelocity": -1.911842718131469e-20, + "velocityX": -7.069031221189556e-20, + "velocityY": -4.343771313501213e-20, + "timestamp": 2.4932364716887907 + }, + { + "x": 1.8552093934543321, + "y": 5.543066072465799, + "heading": -2.0527826070429256e-18, + "angularVelocity": -2.0998680789192775e-17, + "velocityX": 0.4294288348656248, + "velocityY": 0.000678712250886674, + "timestamp": 2.5916653531283225 + }, + { + "x": 1.939745792565551, + "y": 5.543199682240142, + "heading": -6.048126842026002e-18, + "angularVelocity": -4.05911778781429e-17, + "velocityX": 0.8588576632677929, + "velocityY": 0.0013574244915579316, + "timestamp": 2.6900942345678542 + }, + { + "x": 2.066550389641902, + "y": 5.543400096899142, + "heading": -1.1691231309928304e-17, + "angularVelocity": -5.733179515246905e-17, + "velocityX": 1.2882864787430477, + "velocityY": 0.0020361367117983195, + "timestamp": 2.788523116007386 + }, + { + "x": 2.2356231808662415, + "y": 5.543667316436768, + "heading": 9.487524674650316e-21, + "angularVelocity": 1.188748532301011e-16, + "velocityX": 1.7177152554375654, + "velocityY": 0.0027148488707459404, + "timestamp": 2.8869519974469178 + }, + { + "x": 2.4046959720905807, + "y": 5.543934535974393, + "heading": 1.1697892357164231e-17, + "angularVelocity": 1.187497476520098e-16, + "velocityX": 1.7177152554375656, + "velocityY": 0.002714848870745941, + "timestamp": 2.9853808788864495 + }, + { + "x": 2.531500569166932, + "y": 5.544134950633393, + "heading": 6.051439837245386e-18, + "angularVelocity": -5.736581008759357e-17, + "velocityX": 1.2882864787430477, + "velocityY": 0.00203613671179832, + "timestamp": 3.0838097603259813 + }, + { + "x": 2.616036968278151, + "y": 5.544268560407736, + "heading": 2.0590195546334623e-18, + "angularVelocity": -4.0561471634117896e-17, + "velocityX": 0.8588576632677929, + "velocityY": 0.0013574244915579318, + "timestamp": 3.182238641765513 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -7.01229032287185e-26, + "angularVelocity": -2.0918856279971937e-17, + "velocityX": 0.4294288348656248, + "velocityY": 0.0006787122508866741, + "timestamp": 3.280667523205045 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -7.502242975241614e-26, + "angularVelocity": -4.851342689713789e-26, + "velocityX": 1.33942470461485e-25, + "velocityY": -1.0236263470109501e-25, + "timestamp": 3.3790964046445766 + }, + { + "x": 2.643889442719825, + "y": 5.563919137234555, + "heading": 0.0031393268173354576, + "angularVelocity": 0.04203939285338829, + "velocityX": -0.19304404414258594, + "velocityY": 0.26225045368154326, + "timestamp": 3.45377224090805 + }, + { + "x": 2.615267140219649, + "y": 5.603236779535534, + "heading": 0.009643531258689539, + "angularVelocity": 0.08709918451272194, + "velocityX": -0.3832873380780111, + "velocityY": 0.5265109072532814, + "timestamp": 3.528448077171523 + }, + { + "x": 2.5727492586123577, + "y": 5.662505100435592, + "heading": 0.019846282739514817, + "angularVelocity": 0.13662721425479105, + "velocityX": -0.5693659921969727, + "velocityY": 0.7936746860248878, + "timestamp": 3.603123913434996 + }, + { + "x": 2.516846273518896, + "y": 5.742064038683667, + "heading": 0.03428875734419064, + "angularVelocity": 0.19340224800053757, + "velocityX": -0.7486087587452387, + "velocityY": 1.0653906568568225, + "timestamp": 3.6777997496984693 + }, + { + "x": 2.448546488675387, + "y": 5.842518783466547, + "heading": 0.05399413519442509, + "angularVelocity": 0.2638789042912005, + "velocityX": -0.9146169398429232, + "velocityY": 1.3452108447564306, + "timestamp": 3.7524755859619425 + }, + { + "x": 2.3705191792906315, + "y": 5.9651940015137335, + "heading": 0.0815833991238304, + "angularVelocity": 0.36945369894572166, + "velocityX": -1.0448802891132984, + "velocityY": 1.6427699264640412, + "timestamp": 3.8271514222254157 + }, + { + "x": 2.300330937619818, + "y": 6.110432887633571, + "heading": 0.12848151048537504, + "angularVelocity": 0.6280225801031192, + "velocityX": -0.9399056667162509, + "velocityY": 1.944924802815761, + "timestamp": 3.901827258488889 + }, + { + "x": 2.2517620633069444, + "y": 6.244623434702984, + "heading": 0.18175459499441046, + "angularVelocity": 0.7133912008842571, + "velocityX": -0.6503961220000475, + "velocityY": 1.7969741456387136, + "timestamp": 3.976503094752362 + }, + { + "x": 2.2219257094452107, + "y": 6.36329736060272, + "heading": 0.23705644760887684, + "angularVelocity": 0.7405588659141199, + "velocityX": -0.39954495797628836, + "velocityY": 1.589187772615341, + "timestamp": 4.051178931015835 + }, + { + "x": 2.2097463319071795, + "y": 6.465234411607719, + "heading": 0.29297905925002543, + "angularVelocity": 0.7488715820180554, + "velocityX": -0.16309663403111632, + "velocityY": 1.3650607225253184, + "timestamp": 4.125854767279309 + }, + { + "x": 2.2146790021354428, + "y": 6.549877194214307, + "heading": 0.3488347521561843, + "angularVelocity": 0.7479754590104253, + "velocityX": 0.06605443574625332, + "velocityY": 1.133469497521908, + "timestamp": 4.2005306035427825 + }, + { + "x": 2.2363960702944423, + "y": 6.616907899307652, + "heading": 0.40421575733503773, + "angularVelocity": 0.7416188147322054, + "velocityX": 0.29081787691505323, + "velocityY": 0.8976224230933981, + "timestamp": 4.275206439806256 + }, + { + "x": 2.2746791565921045, + "y": 6.66612148860884, + "heading": 0.45885178746831873, + "angularVelocity": 0.7316426955101331, + "velocityX": 0.5126569478591565, + "velocityY": 0.6590296374794072, + "timestamp": 4.34988227606973 + }, + { + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.7190897995734556, + "velocityX": 0.7324089310601718, + "velocityY": 0.4185200148294202, + "timestamp": 4.424558112333203 + }, + { + "x": 2.4110932710082595, + "y": 6.744072527662596, + "heading": 0.564314297915607, + "angularVelocity": 0.6537087472117014, + "velocityX": 1.0320255363414028, + "velocityY": 0.5897297595057032, + "timestamp": 4.503743037632732 + }, + { + "x": 2.50599376719455, + "y": 6.798301463693884, + "heading": 0.5436087676029941, + "angularVelocity": -0.2614832335105605, + "velocityX": 1.1984667009195895, + "velocityY": 0.6848391385880369, + "timestamp": 4.58292796293226 + }, + { + "x": 2.5771691415050153, + "y": 6.8389731669577785, + "heading": 0.5280796125906262, + "angularVelocity": -0.19611251704319405, + "velocityX": 0.8988500531033392, + "velocityY": 0.5136293696059923, + "timestamp": 4.662112888231788 + }, + { + "x": 2.624619391553792, + "y": 6.866087636090928, + "heading": 0.5177268190614664, + "angularVelocity": -0.13074197506657764, + "velocityX": 0.5992333751568124, + "velocityY": 0.34241958340662104, + "timestamp": 4.741297813531316 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06537102159136951, + "velocityX": 0.29961668980067113, + "velocityY": 0.1712097929731781, + "timestamp": 4.820482738830845 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -6.678803262228469e-27, + "velocityX": 1.419138537377677e-18, + "velocityY": -2.483488681188097e-18, + "timestamp": 4.899667664130373 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAD.1.traj b/src/main/deploy/choreo/CenterLanePCBAD.1.traj deleted file mode 100644 index 7d68d234..00000000 --- a/src/main/deploy/choreo/CenterLanePCBAD.1.traj +++ /dev/null @@ -1,328 +0,0 @@ -{ - "samples": [ - { - "x": 1.289974451065063, - "y": 5.590954303741456, - "heading": 6.19779206460361e-17, - "angularVelocity": 3.0320144312848013e-18, - "velocityX": 2.9896394667646305e-17, - "velocityY": -6.392586992550233e-17, - "timestamp": 0 - }, - { - "x": 1.3085927366636296, - "y": 5.570883327334207, - "heading": -0.01041389258493421, - "angularVelocity": -0.12226255374555442, - "velocityX": 0.21858484952476373, - "velocityY": -0.23563992154741434, - "timestamp": 0.08517646872162599 - }, - { - "x": 1.3442657042493709, - "y": 5.532508491008622, - "heading": -0.03089006103804589, - "angularVelocity": -0.24039701058788923, - "velocityX": 0.4188124739278457, - "velocityY": -0.45053330927585317, - "timestamp": 0.17035293744325197 - }, - { - "x": 1.3955881762749254, - "y": 5.477434538773948, - "heading": -0.060869932572704226, - "angularVelocity": -0.35197363760921707, - "velocityX": 0.6025428477586634, - "velocityY": -0.646586469963527, - "timestamp": 0.25552940616487796 - }, - { - "x": 1.4613425796478687, - "y": 5.4070770106059785, - "heading": -0.09976067247727309, - "angularVelocity": -0.45659018844361554, - "velocityX": 0.7719785095557636, - "velocityY": -0.826020721731408, - "timestamp": 0.34070587488650395 - }, - { - "x": 1.5403946295377764, - "y": 5.322786826751108, - "heading": -0.14691970563793563, - "angularVelocity": -0.5536626942681541, - "velocityX": 0.9280972911458275, - "velocityY": -0.9895947216401798, - "timestamp": 0.42588234360812993 - }, - { - "x": 1.6316218404845053, - "y": 5.225944550707253, - "heading": -0.20162745404412188, - "angularVelocity": -0.6422871155293178, - "velocityX": 1.0710377210503823, - "velocityY": -1.1369604480828668, - "timestamp": 0.5110588123297559 - }, - { - "x": 1.733844381505324, - "y": 5.118072938931053, - "heading": -0.2630340547325837, - "angularVelocity": -0.7209338636607621, - "velocityX": 1.2001265438099284, - "velocityY": -1.2664485085516666, - "timestamp": 0.5962352810513819 - }, - { - "x": 1.8457280635804503, - "y": 5.001046382736992, - "heading": -0.33003861392476214, - "angularVelocity": -0.7866557536115174, - "velocityX": 1.3135515448613582, - "velocityY": -1.373930593160977, - "timestamp": 0.6814117497730079 - }, - { - "x": 1.9656142072747882, - "y": 4.877758551128575, - "heading": -0.40085023097520717, - "angularVelocity": -0.8313518758551945, - "velocityX": 1.4075030990795228, - "velocityY": -1.447440043697368, - "timestamp": 0.7665882184946339 - }, - { - "x": 2.0856915448639635, - "y": 4.7606487505271025, - "heading": -0.4673031995363478, - "angularVelocity": -0.7801798966134944, - "velocityX": 1.4097477788332962, - "velocityY": -1.3749079101202415, - "timestamp": 0.8517646872162599 - }, - { - "x": 2.1993786689736172, - "y": 4.652829385838518, - "heading": -0.5273883646655675, - "angularVelocity": -0.7054197718103367, - "velocityX": 1.3347245526367961, - "velocityY": -1.2658351104101313, - "timestamp": 0.9369411559378859 - }, - { - "x": 2.3039823973737983, - "y": 4.5566291473122265, - "heading": -0.5809972368261334, - "angularVelocity": -0.6293859438546427, - "velocityX": 1.2280824736001708, - "velocityY": -1.1294227146313731, - "timestamp": 1.0221176246595118 - }, - { - "x": 2.3979860520770147, - "y": 4.47341696133058, - "heading": -0.6272322006251169, - "angularVelocity": -0.5428138134029581, - "velocityX": 1.1036340918339758, - "velocityY": -0.9769386689838181, - "timestamp": 1.107294093381138 - }, - { - "x": 2.480103614094797, - "y": 4.40444635319333, - "heading": -0.6652761118925774, - "angularVelocity": -0.4466481393094119, - "velocityX": 0.964087420507652, - "velocityY": -0.8097378204614404, - "timestamp": 1.192470562102764 - }, - { - "x": 2.549135601344952, - "y": 4.350942377101982, - "heading": -0.6944118607427208, - "angularVelocity": -0.3420633572561569, - "velocityX": 0.810458431609397, - "velocityY": -0.6281544292028495, - "timestamp": 1.2776470308243901 - }, - { - "x": 2.6038790433453562, - "y": 4.3141752600275325, - "heading": -0.7139618385994653, - "angularVelocity": -0.2295232257237371, - "velocityX": 0.6427061701667566, - "velocityY": -0.431658151908278, - "timestamp": 1.3628234995460162 - }, - { - "x": 2.6430389310079985, - "y": 4.2955467477546305, - "heading": -0.7232599969808831, - "angularVelocity": -0.1091634640525668, - "velocityX": 0.459750072412904, - "velocityY": -0.2187049140741477, - "timestamp": 1.4479999682676423 - }, - { - "x": 2.665171623229982, - "y": 4.296644687652586, - "heading": -0.721655115096118, - "angularVelocity": 0.018841845745096026, - "velocityX": 0.2598451491845503, - "velocityY": 0.012890178642452559, - "timestamp": 1.5331764369892684 - }, - { - "x": 2.66906909991143, - "y": 4.318945956778202, - "heading": -0.7091344221786624, - "angularVelocity": 0.14601587711521094, - "velocityX": 0.045452234946537245, - "velocityY": 0.2600766102664744, - "timestamp": 1.6189252826957972 - }, - { - "x": 2.6560642905413947, - "y": 4.360643314893585, - "heading": -0.6858248901313277, - "angularVelocity": 0.2718349367303499, - "velocityX": -0.15166162602986216, - "velocityY": 0.4862731127377579, - "timestamp": 1.704674128402326 - }, - { - "x": 2.627632992233998, - "y": 4.419910426276983, - "heading": -0.652529270737317, - "angularVelocity": 0.38829233349639936, - "velocityX": -0.331564793358298, - "velocityY": 0.6911709527407293, - "timestamp": 1.7904229741088546 - }, - { - "x": 2.5850645171946978, - "y": 4.495119603214681, - "heading": -0.6100207421436514, - "angularVelocity": 0.4957329541105318, - "velocityX": -0.4964320474352394, - "velocityY": 0.877086756305711, - "timestamp": 1.8761718198153834 - }, - { - "x": 2.5295631514165375, - "y": 4.584706937933874, - "heading": -0.5590704924389158, - "angularVelocity": 0.5941800065637087, - "velocityX": -0.6472549609368695, - "velocityY": 1.0447643228435226, - "timestamp": 1.9619206655219121 - }, - { - "x": 2.46234032691493, - "y": 4.687033037499603, - "heading": -0.5004982071284244, - "angularVelocity": 0.6830679157006081, - "velocityX": -0.7839501972035283, - "velocityY": 1.1933233470679585, - "timestamp": 2.047669511228441 - }, - { - "x": 2.384701887418879, - "y": 4.800204071292841, - "heading": -0.43526625936464175, - "angularVelocity": 0.7607326632364927, - "velocityX": -0.9054167301769336, - "velocityY": 1.319796585723857, - "timestamp": 2.1334183569349694 - }, - { - "x": 2.298178863290851, - "y": 4.921641059920068, - "heading": -0.36471802922759106, - "angularVelocity": 0.8227309598836902, - "velocityX": -1.0090284413174404, - "velocityY": 1.416193858082253, - "timestamp": 2.219167202641498 - }, - { - "x": 2.2047510720717436, - "y": 5.044849013310237, - "heading": -0.2923595970689714, - "angularVelocity": 0.8438414717122018, - "velocityX": -1.0895515904535924, - "velocityY": 1.4368467863910614, - "timestamp": 2.3049160483480264 - }, - { - "x": 2.116347360692902, - "y": 5.158956328540576, - "heading": -0.22539524544258233, - "angularVelocity": 0.7809358956920709, - "velocityX": -1.0309609493916598, - "velocityY": 1.3307154666649113, - "timestamp": 2.390664894054555 - }, - { - "x": 2.0362102002706437, - "y": 5.261312637562149, - "heading": -0.16556699187625293, - "angularVelocity": 0.6977149729932085, - "velocityX": -0.9345567250727145, - "velocityY": 1.1936756486715054, - "timestamp": 2.4764137397610835 - }, - { - "x": 1.9661275657868833, - "y": 5.350222584355773, - "heading": -0.1134489428170574, - "angularVelocity": 0.6077988412528272, - "velocityX": -0.8173011998740509, - "velocityY": 1.0368646488597075, - "timestamp": 2.562162585467612 - }, - { - "x": 1.9075019379268083, - "y": 5.42424212633875, - "heading": -0.06991588076878923, - "angularVelocity": 0.5076810269523403, - "velocityX": -0.6836899946235904, - "velocityY": 0.8632132756201296, - "timestamp": 2.6479114311741405 - }, - { - "x": 1.861590779787906, - "y": 5.48200360252104, - "heading": -0.03588037308734902, - "angularVelocity": 0.3969208844854328, - "velocityX": -0.5354142992901785, - "velocityY": 0.673612288379681, - "timestamp": 2.733660276880669 - }, - { - "x": 1.8296344375134035, - "y": 5.522099910983847, - "heading": -0.012259641477970167, - "angularVelocity": 0.2754641349951199, - "velocityX": -0.3726737311878495, - "velocityY": 0.4676017284248481, - "timestamp": 2.8194091225871976 - }, - { - "x": 1.8129411935806854, - "y": 5.542999267578127, - "heading": 2.3632850028431835e-17, - "angularVelocity": 0.14297150447865262, - "velocityX": -0.1946760191950624, - "velocityY": 0.24372755600475027, - "timestamp": 2.905157968293726 - }, - { - "x": 1.812941193580624, - "y": 5.542999267578126, - "heading": -4.920156603511903e-17, - "angularVelocity": -5.733397110540358e-17, - "velocityX": -7.214388151371406e-13, - "velocityY": -9.03683027618772e-16, - "timestamp": 2.9909068140002546 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAD.2.traj b/src/main/deploy/choreo/CenterLanePCBAD.2.traj deleted file mode 100644 index c49e7767..00000000 --- a/src/main/deploy/choreo/CenterLanePCBAD.2.traj +++ /dev/null @@ -1,103 +0,0 @@ -{ - "samples": [ - { - "x": 1.812941193580624, - "y": 5.542999267578126, - "heading": -4.920156603511903e-17, - "angularVelocity": -5.733397110540358e-17, - "velocityX": -7.214388151371406e-13, - "velocityY": -9.03683027618772e-16, - "timestamp": 0 - }, - { - "x": 1.8513325137310372, - "y": 5.5430599450572595, - "heading": -0.000016850565291482023, - "angularVelocity": -0.00016991086246254008, - "velocityX": 0.3871147468977978, - "velocityY": 0.0006118348336243464, - "timestamp": 0.09917297250510426 - }, - { - "x": 1.9275402570059652, - "y": 5.543180391388335, - "heading": -0.000015746696122309923, - "angularVelocity": 0.00001113074602288469, - "velocityX": 0.7684325814778618, - "velocityY": 0.0012145076227106227, - "timestamp": 0.19834594501020852 - }, - { - "x": 2.0338963277323163, - "y": 5.54334848714078, - "heading": 0.000009170336978677823, - "angularVelocity": 0.00025124822289482134, - "velocityX": 1.0724299982122594, - "velocityY": 0.0016949754373461064, - "timestamp": 0.2975189175153128 - }, - { - "x": 2.163383514243291, - "y": 5.5435531416216355, - "heading": 0.000033654463962368084, - "angularVelocity": 0.0002468830606297171, - "velocityX": 1.305670115961409, - "velocityY": 0.00206361144256778, - "timestamp": 0.39669189002041705 - }, - { - "x": 2.3079360710490797, - "y": 5.54378160701675, - "heading": 0.00003317773344428256, - "angularVelocity": -0.000004807060894986283, - "velocityX": 1.457580156714077, - "velocityY": 0.0023037062351123753, - "timestamp": 0.4958648625255213 - }, - { - "x": 2.437398077457075, - "y": 5.543986221690917, - "heading": 0.000022146124274066103, - "angularVelocity": -0.00011123604437218979, - "velocityX": 1.3054162151016944, - "velocityY": 0.002063210056106694, - "timestamp": 0.5950378350306256 - }, - { - "x": 2.5437229473484053, - "y": 5.544154268122903, - "heading": 0.000015406432122086, - "angularVelocity": -0.00006795896080977589, - "velocityX": 1.0721153879486738, - "velocityY": 0.0016944781198031775, - "timestamp": 0.6942108075357298 - }, - { - "x": 2.619913870706316, - "y": 5.544274687859312, - "heading": 0.000009213432317016853, - "angularVelocity": -0.00006244644733859944, - "velocityX": 0.7682629796539725, - "velocityY": 0.0012142394582550931, - "timestamp": 0.7933837800408341 - }, - { - "x": 2.6583051681519314, - "y": 5.5443353652954785, - "heading": 6.370510606485668e-14, - "angularVelocity": -0.00009290265301587805, - "velocityX": 0.38711451795641516, - "velocityY": 0.0006118344003786568, - "timestamp": 0.8925567525459384 - }, - { - "x": 2.658305168152, - "y": 5.544335365295488, - "heading": 6.625668653050138e-14, - "angularVelocity": 2.6906565131836242e-14, - "velocityX": 7.155954565396242e-13, - "velocityY": 8.671180221391495e-14, - "timestamp": 0.9917297250510426 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAD.3.traj b/src/main/deploy/choreo/CenterLanePCBAD.3.traj deleted file mode 100644 index c0121fc6..00000000 --- a/src/main/deploy/choreo/CenterLanePCBAD.3.traj +++ /dev/null @@ -1,202 +0,0 @@ -{ - "samples": [ - { - "x": 2.658305168152, - "y": 5.544335365295488, - "heading": 6.625668653050138e-14, - "angularVelocity": 2.6906565131836242e-14, - "velocityX": 7.155954565396242e-13, - "velocityY": 8.671180221391495e-14, - "timestamp": 0 - }, - { - "x": 2.6398879819889713, - "y": 5.568034006444528, - "heading": 0.008388499707182374, - "angularVelocity": 0.0943787022324955, - "velocityX": -0.20721108536177554, - "velocityY": 0.26663254150593735, - "timestamp": 0.0888812783882651 - }, - { - "x": 2.6055876283476542, - "y": 5.613232266775393, - "heading": 0.025296828917652628, - "angularVelocity": 0.19023499118162804, - "velocityX": -0.3859120195327329, - "velocityY": 0.5085239675934979, - "timestamp": 0.1777625567765302 - }, - { - "x": 2.5578825329127857, - "y": 5.677978135359711, - "heading": 0.05006181260458975, - "angularVelocity": 0.27862992225075944, - "velocityX": -0.5367282773164671, - "velocityY": 0.7284533903912082, - "timestamp": 0.2666438351647953 - }, - { - "x": 2.4993543830957403, - "y": 5.760558609750369, - "heading": 0.0821470752651059, - "angularVelocity": 0.36099011222914734, - "velocityX": -0.6584980648176972, - "velocityY": 0.9291098855468343, - "timestamp": 0.3555251135530604 - }, - { - "x": 2.433098392034208, - "y": 5.859358208334035, - "heading": 0.12114426250239245, - "angularVelocity": 0.43875592188117873, - "velocityX": -0.7454437229429913, - "velocityY": 1.1115906563809108, - "timestamp": 0.4444063919413255 - }, - { - "x": 2.3637465533640976, - "y": 5.972668871410117, - "heading": 0.16678728284828395, - "angularVelocity": 0.5135279461940396, - "velocityX": -0.7802749907275444, - "velocityY": 1.2748541102325668, - "timestamp": 0.5332876703295906 - }, - { - "x": 2.300988842467506, - "y": 6.095835737634532, - "heading": 0.21776795840799054, - "angularVelocity": 0.5735817090404455, - "velocityX": -0.7060847012414084, - "velocityY": 1.385745889998316, - "timestamp": 0.6221689487178557 - }, - { - "x": 2.2515752134889366, - "y": 6.217231027383832, - "heading": 0.2695393710632505, - "angularVelocity": 0.582478263071738, - "velocityX": -0.5559509254902292, - "velocityY": 1.3658139481175915, - "timestamp": 0.7110502271061208 - }, - { - "x": 2.217045589242532, - "y": 6.33145065782542, - "heading": 0.3198059590268783, - "angularVelocity": 0.5655475357136119, - "velocityX": -0.3884915346931451, - "velocityY": 1.2850808686903799, - "timestamp": 0.7999315054943859 - }, - { - "x": 2.19907919431313, - "y": 6.435243176299548, - "heading": 0.36734143796630997, - "angularVelocity": 0.5348199283510537, - "velocityX": -0.20213924974232386, - "velocityY": 1.1677658147606698, - "timestamp": 0.888812783882651 - }, - { - "x": 2.1993959276283994, - "y": 6.526053318679287, - "heading": 0.41157070153690833, - "angularVelocity": 0.4976218206191788, - "velocityX": 0.0035635549001480525, - "velocityY": 1.021701577951703, - "timestamp": 0.977694062270916 - }, - { - "x": 2.219770279567901, - "y": 6.601608618777817, - "heading": 0.45152771224881855, - "angularVelocity": 0.4495548605556116, - "velocityX": 0.2292310856552352, - "velocityY": 0.8500699075051399, - "timestamp": 1.0665753406591811 - }, - { - "x": 2.2626490848637184, - "y": 6.659247151615926, - "heading": 0.48436572022362784, - "angularVelocity": 0.36945922212429394, - "velocityX": 0.4824278641490618, - "velocityY": 0.648489016846146, - "timestamp": 1.1554566190474462 - }, - { - "x": 2.329372405994089, - "y": 6.697374820688125, - "heading": 0.5125504195998649, - "angularVelocity": 0.31710501792120865, - "velocityX": 0.7507016363871187, - "velocityY": 0.4289730049288785, - "timestamp": 1.2443378974357113 - }, - { - "x": 2.399251093648798, - "y": 6.73730555904706, - "heading": 0.5293913369356736, - "angularVelocity": 0.21745079806450165, - "velocityX": 0.902277239279105, - "velocityY": 0.5155877648973247, - "timestamp": 1.321784922168539 - }, - { - "x": 2.4725481079431484, - "y": 6.779189629834486, - "heading": 0.507422235287016, - "angularVelocity": -0.28366617987293774, - "velocityX": 0.9464148501358945, - "velocityY": 0.5408092945202858, - "timestamp": 1.3992319469013665 - }, - { - "x": 2.537702213604298, - "y": 6.816420603018375, - "heading": 0.5016725044650684, - "angularVelocity": -0.07424082257084566, - "velocityX": 0.8412731915467285, - "velocityY": 0.48072825693890747, - "timestamp": 1.4766789716341941 - }, - { - "x": 2.5904059035391898, - "y": 6.846537042294488, - "heading": 0.5036538467311155, - "angularVelocity": 0.025583194098508056, - "velocityX": 0.680512777970975, - "velocityY": 0.38886502613926416, - "timestamp": 1.5541259963670218 - }, - { - "x": 2.6280877308244723, - "y": 6.868069547224279, - "heading": 0.508503710225543, - "angularVelocity": 0.06262168896947905, - "velocityX": 0.4865497083575037, - "velocityY": 0.27802882066934376, - "timestamp": 1.6315730210998494 - }, - { - "x": 2.648344516781087, - "y": 6.8796448707953655, - "heading": 0.5125504196001363, - "angularVelocity": 0.052251321320124576, - "velocityX": 0.26155667100350394, - "velocityY": 0.14946117855453953, - "timestamp": 1.709020045832677 - }, - { - "x": 2.6483445167657806, - "y": 6.879644870779064, - "heading": 0.5125504196000689, - "angularVelocity": 1.8669291195039983e-14, - "velocityX": -4.749030905139852e-11, - "velocityY": 6.098431731550748e-11, - "timestamp": 1.7864670705655046 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAD.4.traj b/src/main/deploy/choreo/CenterLanePCBAD.4.traj deleted file mode 100644 index 29278195..00000000 --- a/src/main/deploy/choreo/CenterLanePCBAD.4.traj +++ /dev/null @@ -1,526 +0,0 @@ -{ - "samples": [ - { - "x": 2.6483445167657806, - "y": 6.879644870779064, - "heading": 0.5125504196000689, - "angularVelocity": 1.8669291195039983e-14, - "velocityX": -4.749030905139852e-11, - "velocityY": 6.098431731550748e-11, - "timestamp": 0 - }, - { - "x": 2.6747339306487334, - "y": 6.883481123686636, - "heading": 0.5098224624623802, - "angularVelocity": -0.03296702980330207, - "velocityX": 0.3189128531205414, - "velocityY": 0.046360649216292196, - "timestamp": 0.08274804111760403 - }, - { - "x": 2.7264961186821273, - "y": 6.89099146776212, - "heading": 0.5039121473834008, - "angularVelocity": -0.07142543798202369, - "velocityX": 0.6255397388782451, - "velocityY": 0.09076159355636307, - "timestamp": 0.16549608223520806 - }, - { - "x": 2.8018050306182753, - "y": 6.9018959167453104, - "heading": 0.4949465844489951, - "angularVelocity": -0.10834773625231511, - "velocityX": 0.9100990297657242, - "velocityY": 0.13177893803804336, - "timestamp": 0.2482441233528121 - }, - { - "x": 2.899101313212433, - "y": 6.915952840560763, - "heading": 0.4831136364117761, - "angularVelocity": -0.14299973603485935, - "velocityX": 1.1758137265856894, - "velocityY": 0.16987621248329202, - "timestamp": 0.33099216447041613 - }, - { - "x": 3.017081432389269, - "y": 6.932956858771751, - "heading": 0.4685947114251132, - "angularVelocity": -0.1754594403754914, - "velocityX": 1.42577537284728, - "velocityY": 0.20549148936151598, - "timestamp": 0.41374020558802016 - }, - { - "x": 3.1546294358748312, - "y": 6.9527283369160715, - "heading": 0.4515640044783447, - "angularVelocity": -0.20581401948312003, - "velocityX": 1.6622508717768, - "velocityY": 0.23893590563959213, - "timestamp": 0.4964882467056242 - }, - { - "x": 3.3107722570719647, - "y": 6.975106161489216, - "heading": 0.43219034104896137, - "angularVelocity": -0.2341283632545328, - "velocityX": 1.8869669793779915, - "velocityY": 0.2704332848357982, - "timestamp": 0.5792362878232282 - }, - { - "x": 3.484600968601402, - "y": 6.999935624537231, - "heading": 0.4106444392061959, - "angularVelocity": -0.2603795999490034, - "velocityX": 2.1006988102882644, - "velocityY": 0.3000610372485155, - "timestamp": 0.6619843289408323 - }, - { - "x": 3.675234170530874, - "y": 7.027062068947035, - "heading": 0.38710417033209454, - "angularVelocity": -0.28448128265230255, - "velocityX": 2.3037790303523367, - "velocityY": 0.32781977728352046, - "timestamp": 0.7447323700584363 - }, - { - "x": 3.8817881153060045, - "y": 7.056325014551516, - "heading": 0.36176128472936936, - "angularVelocity": -0.3062656862983269, - "velocityX": 2.4961792688429623, - "velocityY": 0.3536391340419632, - "timestamp": 0.8274804111760403 - }, - { - "x": 4.1033458782194, - "y": 7.087551279488626, - "heading": 0.3348309137177563, - "angularVelocity": -0.32545025414364576, - "velocityX": 2.677498583906162, - "velocityY": 0.3773656090873216, - "timestamp": 0.9102284522936444 - }, - { - "x": 4.338922324182521, - "y": 7.120545790816314, - "heading": 0.30656596895157695, - "angularVelocity": -0.34157841544560996, - "velocityX": 2.846912661392294, - "velocityY": 0.39873465138360303, - "timestamp": 0.9929764934112484 - }, - { - "x": 4.587417768628414, - "y": 7.155077045432509, - "heading": 0.27728164973070885, - "angularVelocity": -0.3538974315929522, - "velocityX": 3.00303718480442, - "velocityY": 0.4173060068834074, - "timestamp": 1.0757245345288524 - }, - { - "x": 4.847543704393879, - "y": 7.190849002148663, - "heading": 0.2474070078170501, - "angularVelocity": -0.36103140944690143, - "velocityX": 3.1435902560613274, - "velocityY": 0.4322997406709641, - "timestamp": 1.1584725756464564 - }, - { - "x": 5.117670126803491, - "y": 7.227429623630717, - "heading": 0.21762399464747098, - "angularVelocity": -0.35992408723308145, - "velocityX": 3.264444919314729, - "velocityY": 0.4420723558891399, - "timestamp": 1.2412206167640605 - }, - { - "x": 5.395335267694769, - "y": 7.263944271068559, - "heading": 0.1895241870207491, - "angularVelocity": -0.33958275322536735, - "velocityX": 3.3555494141142153, - "velocityY": 0.44127506759904395, - "timestamp": 1.3239686578816645 - }, - { - "x": 5.667910261992729, - "y": 7.2967916361655485, - "heading": 0.1669091076503975, - "angularVelocity": -0.27330048016738406, - "velocityX": 3.294035612402776, - "velocityY": 0.3969564071048189, - "timestamp": 1.4067166989992685 - }, - { - "x": 5.931901746285159, - "y": 7.327414779180602, - "heading": 0.14778373205342693, - "angularVelocity": -0.23112783503586568, - "velocityX": 3.1903049392702143, - "velocityY": 0.3700769541063461, - "timestamp": 1.4894647401168726 - }, - { - "x": 6.1801220482371395, - "y": 7.354810947360831, - "heading": 0.11632647881372146, - "angularVelocity": -0.38015707459464126, - "velocityX": 2.999712121271843, - "velocityY": 0.33107935620263895, - "timestamp": 1.5722127812344766 - }, - { - "x": 6.415697256681246, - "y": 7.379780463537999, - "heading": 0.08899977607154176, - "angularVelocity": -0.3302398748429845, - "velocityX": 2.84689770612573, - "velocityY": 0.3017535622589267, - "timestamp": 1.6549608223520806 - }, - { - "x": 6.637987945996846, - "y": 7.402299511961184, - "heading": 0.06474721752358573, - "angularVelocity": -0.2930892166194916, - "velocityX": 2.686355910222349, - "velocityY": 0.2721399578655483, - "timestamp": 1.7377088634696847 - }, - { - "x": 6.845920803268571, - "y": 7.422253782487123, - "heading": 0.04362794173118966, - "angularVelocity": -0.2552238760840361, - "velocityX": 2.5128432584428544, - "velocityY": 0.24114492931132497, - "timestamp": 1.8204569045872887 - }, - { - "x": 7.038548371158306, - "y": 7.439534256583214, - "heading": 0.025735796286898687, - "angularVelocity": -0.2162243988212622, - "velocityX": 2.3278807001118484, - "velocityY": 0.2088324250667052, - "timestamp": 1.9032049457048927 - }, - { - "x": 7.2149794023417675, - "y": 7.454031322663086, - "heading": 0.011159229205506762, - "angularVelocity": -0.17615603807074215, - "velocityX": 2.1321475264013814, - "velocityY": 0.1751952781490545, - "timestamp": 1.9859529868224968 - }, - { - "x": 7.374384880065902, - "y": 7.465639114379918, - "heading": -1.1165566205788637e-16, - "angularVelocity": -0.134857925997875, - "velocityX": 1.9263957861864134, - "velocityY": 0.14027874932231896, - "timestamp": 2.068701027940101 - }, - { - "x": 7.500924464216497, - "y": 7.473646968954779, - "heading": -0.0070937662662868915, - "angularVelocity": -0.09816609550664941, - "velocityX": 1.7511003938959169, - "velocityY": 0.11081557912801578, - "timestamp": 2.1409639230840387 - }, - { - "x": 7.613899532464792, - "y": 7.47952275118133, - "heading": -0.01171150501248939, - "angularVelocity": -0.06390193386252535, - "velocityX": 1.5633897316635108, - "velocityY": 0.08131119317636289, - "timestamp": 2.2132268182279766 - }, - { - "x": 7.712046212988902, - "y": 7.483209835153203, - "heading": -0.014057088311073107, - "angularVelocity": -0.032459027470621904, - "velocityX": 1.3581891554251948, - "velocityY": 0.05102319751389575, - "timestamp": 2.2854897133719154 - }, - { - "x": 7.79365194754084, - "y": 7.484595849351514, - "heading": -0.014345050898318633, - "angularVelocity": -0.003984930117620908, - "velocityX": 1.129289580626482, - "velocityY": 0.019180164253482204, - "timestamp": 2.3577526085158533 - }, - { - "x": 7.856538953365895, - "y": 7.483519387813377, - "heading": -0.012796446625875068, - "angularVelocity": 0.02143014432730538, - "velocityX": 0.8702530627897808, - "velocityY": -0.0148964629220964, - "timestamp": 2.4300155036597912 - }, - { - "x": 7.899249386445817, - "y": 7.479924646329156, - "heading": -0.009762276595150929, - "angularVelocity": 0.04198793896480952, - "velocityX": 0.5910423737502203, - "velocityY": -0.04974532887286408, - "timestamp": 2.502278398803729 - }, - { - "x": 7.921731862807926, - "y": 7.4739282664160145, - "heading": -0.005439516570392937, - "angularVelocity": 0.0598199119499377, - "velocityX": 0.31112061476836195, - "velocityY": -0.08298006744976984, - "timestamp": 2.574541293947667 - }, - { - "x": 7.923961639404313, - "y": 7.465639114379864, - "heading": -8.923477228591696e-17, - "angularVelocity": 0.07527399171536123, - "velocityX": 0.030856452567537763, - "velocityY": -0.11470827482980998, - "timestamp": 2.646804189091605 - }, - { - "x": 7.888861809774048, - "y": 7.450123409205229, - "heading": 0.009559512590155543, - "angularVelocity": 0.09629823646465466, - "velocityX": -0.3535799196587898, - "velocityY": -0.15629824551531185, - "timestamp": 2.7460740505253023 - }, - { - "x": 7.817152523236329, - "y": 7.430666435185926, - "heading": 0.02163779335227315, - "angularVelocity": 0.12167117579976462, - "velocityX": -0.72236714650414, - "velocityY": -0.19600081775371833, - "timestamp": 2.8453439119589996 - }, - { - "x": 7.711448594725456, - "y": 7.4075722780062225, - "heading": 0.036035719670363044, - "angularVelocity": 0.14503824333134957, - "velocityX": -1.064813902067085, - "velocityY": -0.23264016737978235, - "timestamp": 2.944613773392697 - }, - { - "x": 7.574048351882708, - "y": 7.3811128444264495, - "heading": 0.05252610728763101, - "angularVelocity": 0.16611675869298972, - "velocityX": -1.3841083371967766, - "velocityY": -0.2665404504210516, - "timestamp": 3.0438836348263942 - }, - { - "x": 7.407019371926434, - "y": 7.351538272778396, - "heading": 0.07089974064841498, - "angularVelocity": 0.18508773050978647, - "velocityX": -1.682574928019174, - "velocityY": -0.2979209522500118, - "timestamp": 3.1431534962600916 - }, - { - "x": 7.212241390572204, - "y": 7.319083353098022, - "heading": 0.09094586321763254, - "angularVelocity": 0.20193563564714437, - "velocityX": -1.96210590546985, - "velocityY": -0.3269362847056093, - "timestamp": 3.242423357693789 - }, - { - "x": 6.990210935369373, - "y": 7.283848196853213, - "heading": 0.11243058039021528, - "angularVelocity": 0.216427391579795, - "velocityX": -2.236635087388812, - "velocityY": -0.35494313919580667, - "timestamp": 3.341693219127486 - }, - { - "x": 6.744250267253016, - "y": 7.2462331722899656, - "heading": 0.1352094476478159, - "angularVelocity": 0.2294640783075409, - "velocityX": -2.4776973047417434, - "velocityY": -0.3789168637892277, - "timestamp": 3.4409630805611835 - }, - { - "x": 6.474537469267562, - "y": 7.206343597573894, - "heading": 0.1591116816121769, - "angularVelocity": 0.24078036998495714, - "velocityX": -2.7169655934857664, - "velocityY": -0.401829660482739, - "timestamp": 3.540232941994881 - }, - { - "x": 6.185093329086759, - "y": 7.164701733979632, - "heading": 0.18384126922195682, - "angularVelocity": 0.24911475902781344, - "velocityX": -2.915730273020737, - "velocityY": -0.4194814316536095, - "timestamp": 3.639502803428578 - }, - { - "x": 5.8780260788239564, - "y": 7.121745615538567, - "heading": 0.2121770427035368, - "angularVelocity": 0.28544185589002363, - "velocityX": -3.0932575690950848, - "velocityY": -0.43272064472210836, - "timestamp": 3.7387726648622754 - }, - { - "x": 5.557966335758099, - "y": 7.0784922097584015, - "heading": 0.24028820463072503, - "angularVelocity": 0.283179219968629, - "velocityX": -3.224138106404308, - "velocityY": -0.4357153838585266, - "timestamp": 3.8380425262959728 - }, - { - "x": 5.245908943265439, - "y": 7.038535334063528, - "heading": 0.26569993334648234, - "angularVelocity": 0.25598634216619676, - "velocityX": -3.1435260207458486, - "velocityY": -0.40250762031696863, - "timestamp": 3.93731238772967 - }, - { - "x": 4.948601311707987, - "y": 7.0011016986080925, - "heading": 0.2891526852975207, - "angularVelocity": 0.23625249005411966, - "velocityX": -2.9949435534976283, - "velocityY": -0.37708963138260865, - "timestamp": 4.036582249163367 - }, - { - "x": 4.669544920179785, - "y": 6.966326786089845, - "heading": 0.31063199175620987, - "angularVelocity": 0.21637288647809105, - "velocityX": -2.811088758440401, - "velocityY": -0.35030685059909655, - "timestamp": 4.135852110597065 - }, - { - "x": 4.411446504016625, - "y": 6.934401644375056, - "heading": 0.3300269329519647, - "angularVelocity": 0.19537592694947853, - "velocityX": -2.599967527259491, - "velocityY": -0.3215995394142002, - "timestamp": 4.235121972030762 - }, - { - "x": 4.176717541970206, - "y": 6.9055345106659285, - "heading": 0.3471896751796373, - "angularVelocity": 0.17288975707028345, - "velocityX": -2.364554142177353, - "velocityY": -0.2907945401778102, - "timestamp": 4.334391833464459 - }, - { - "x": 3.9677270788540704, - "y": 6.879953869869358, - "heading": 0.3619526945831296, - "angularVelocity": 0.14871602710307644, - "velocityX": -2.1052760636290624, - "velocityY": -0.2576878866065069, - "timestamp": 4.433661694898157 - }, - { - "x": 3.7869910923072463, - "y": 6.857920125982094, - "heading": 0.37415675707125545, - "angularVelocity": 0.1229382444164791, - "velocityX": -1.8206531563211636, - "velocityY": -0.22195804012459744, - "timestamp": 4.532931556331854 - }, - { - "x": 3.6375222511124736, - "y": 6.83976261955988, - "heading": 0.3836657545916857, - "angularVelocity": 0.09578937033957077, - "velocityX": -1.5056819767458316, - "velocityY": -0.18291056479757375, - "timestamp": 4.632201417765551 - }, - { - "x": 3.5230866973435258, - "y": 6.825906628204701, - "heading": 0.39040681994356247, - "angularVelocity": 0.06790646480733925, - "velocityX": -1.1527723733691484, - "velocityY": -0.1395790339088257, - "timestamp": 4.731471279199249 - }, - { - "x": 3.4467104911360567, - "y": 6.816686862116541, - "heading": 0.39477159352312596, - "angularVelocity": 0.04396876873328538, - "velocityX": -0.7693795992500979, - "velocityY": -0.09287578279051523, - "timestamp": 4.830741140632946 - }, - { - "x": 3.4085204601287793, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": 0.021852252491790684, - "velocityX": -0.3847092204594867, - "velocityY": -0.046321940190690435, - "timestamp": 4.930011002066643 - }, - { - "x": 3.408520460128781, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": -9.258859562419926e-18, - "velocityX": 7.880837628913899e-16, - "velocityY": -2.4194099006901583e-16, - "timestamp": 5.0292808635003405 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAD.5.traj b/src/main/deploy/choreo/CenterLanePCBAD.5.traj deleted file mode 100644 index cb5a691c..00000000 --- a/src/main/deploy/choreo/CenterLanePCBAD.5.traj +++ /dev/null @@ -1,202 +0,0 @@ -{ - "samples": [ - { - "x": 3.408520460128781, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": -9.258859562419926e-18, - "velocityX": 7.880837628913899e-16, - "velocityY": -2.4194099006901583e-16, - "timestamp": 0 - }, - { - "x": 3.4451800140480633, - "y": 6.811382414928076, - "heading": 0.39378662700224637, - "angularVelocity": -0.032542531675579224, - "velocityX": 0.37821978715244237, - "velocityY": -0.00728463273656313, - "timestamp": 0.09692658915412089 - }, - { - "x": 3.518496947445585, - "y": 6.809970351527808, - "heading": 0.38746820532810716, - "angularVelocity": -0.06518770266528738, - "velocityX": 0.756417140408642, - "velocityY": -0.014568380179178244, - "timestamp": 0.19385317830824178 - }, - { - "x": 3.6283397734394334, - "y": 6.8078551393922675, - "heading": 0.37750274880248313, - "angularVelocity": -0.1028144765290181, - "velocityX": 1.1332579321365746, - "velocityY": -0.021822826471036318, - "timestamp": 0.29077976746236267 - }, - { - "x": 3.7707535238798844, - "y": 6.805113681812527, - "heading": 0.36197971834397535, - "angularVelocity": -0.16015244726939834, - "velocityX": 1.469294975540766, - "velocityY": -0.028283854860319504, - "timestamp": 0.38770635661648356 - }, - { - "x": 3.9413941020263046, - "y": 6.801829282896055, - "heading": 0.341973429419134, - "angularVelocity": -0.20640661246244818, - "velocityX": 1.7605135973070385, - "velocityY": -0.03388542757086655, - "timestamp": 0.48463294577060445 - }, - { - "x": 4.137139185749371, - "y": 6.798061838763412, - "heading": 0.3182906699990001, - "angularVelocity": -0.24433707640817395, - "velocityX": 2.0195189517276724, - "velocityY": -0.038869046827304486, - "timestamp": 0.5815595349247253 - }, - { - "x": 4.3554217491364975, - "y": 6.793860650651487, - "heading": 0.2915337558008742, - "angularVelocity": -0.2760533970258723, - "velocityX": 2.2520400778783602, - "velocityY": -0.043344020960483176, - "timestamp": 0.6784861240788462 - }, - { - "x": 4.593770534553052, - "y": 6.7892732393783435, - "heading": 0.26218341148270125, - "angularVelocity": -0.30281003978695553, - "velocityX": 2.459065025362269, - "velocityY": -0.04732871870534226, - "timestamp": 0.7754127132329671 - }, - { - "x": 4.849438377602218, - "y": 6.784352472846038, - "heading": 0.23067476418411942, - "angularVelocity": -0.32507743822988416, - "velocityX": 2.6377472402607576, - "velocityY": -0.05076797373402456, - "timestamp": 0.872339302387088 - }, - { - "x": 5.118739979240287, - "y": 6.77916928922838, - "heading": 0.197506319622179, - "angularVelocity": -0.3422017100921637, - "velocityX": 2.7784079063161986, - "velocityY": -0.053475353490631775, - "timestamp": 0.9692658915412089 - }, - { - "x": 5.388165163887155, - "y": 6.773984054928517, - "heading": 0.16458046580315022, - "angularVelocity": -0.33969888042458984, - "velocityX": 2.779682922902244, - "velocityY": -0.053496510556234986, - "timestamp": 1.0661924806953298 - }, - { - "x": 5.643920085833601, - "y": 6.769061952936388, - "heading": 0.1333147392066983, - "angularVelocity": -0.32257120434452946, - "velocityX": 2.6386456407723093, - "velocityY": -0.05078175178849002, - "timestamp": 1.1631190698494507 - }, - { - "x": 5.882330711464664, - "y": 6.764473684698037, - "heading": 0.10420435480854875, - "angularVelocity": -0.3003343525465651, - "velocityX": 2.4597030362017005, - "velocityY": -0.04733756008947892, - "timestamp": 1.2600456590035716 - }, - { - "x": 6.100655045491889, - "y": 6.760272005917349, - "heading": 0.07768097943341207, - "angularVelocity": -0.27364395679871373, - "velocityX": 2.252471029183505, - "velocityY": -0.04334908323253468, - "timestamp": 1.3569722481576925 - }, - { - "x": 6.29642574105686, - "y": 6.756504387895065, - "heading": 0.054221932212767415, - "angularVelocity": -0.2420290183052161, - "velocityX": 2.019783191314849, - "velocityY": -0.038870840861772844, - "timestamp": 1.4538988373118134 - }, - { - "x": 6.467079365349485, - "y": 6.7532201591852425, - "heading": 0.03442675668029697, - "angularVelocity": -0.20422853734174792, - "velocityX": 1.7606481955253344, - "velocityY": -0.03388367153413224, - "timestamp": 1.5508254264659342 - }, - { - "x": 6.60949406591357, - "y": 6.750479406444339, - "heading": 0.019100936295010484, - "angularVelocity": -0.15811781389436272, - "velocityX": 1.4693047780484314, - "velocityY": -0.028276582977095337, - "timestamp": 1.6477520156200551 - }, - { - "x": 6.719322188320328, - "y": 6.748365796127614, - "heading": 0.009318208717894495, - "angularVelocity": -0.10092924616960162, - "velocityX": 1.1331062339573879, - "velocityY": -0.021806300367839485, - "timestamp": 1.744678604774176 - }, - { - "x": 6.792644320795612, - "y": 6.746954731447967, - "heading": 0.0031061365662838365, - "angularVelocity": -0.06409048544701255, - "velocityX": 0.756470779743405, - "velocityY": -0.014558076292195965, - "timestamp": 1.841605193928297 - }, - { - "x": 6.829305171966555, - "y": 6.746249198913574, - "heading": -6.996174209709525e-17, - "angularVelocity": -0.03204627949246174, - "velocityX": 0.3782331710099824, - "velocityY": -0.007279040153471663, - "timestamp": 1.9385317830824178 - }, - { - "x": 6.8293051719665545, - "y": 6.746249198913574, - "heading": -3.495236266448189e-17, - "angularVelocity": 5.858001175456844e-19, - "velocityX": -7.463612085764823e-16, - "velocityY": 1.4437890171198592e-17, - "timestamp": 2.0354583722365387 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAD.traj b/src/main/deploy/choreo/CenterLanePCBAD.traj deleted file mode 100644 index 6788eb8f..00000000 --- a/src/main/deploy/choreo/CenterLanePCBAD.traj +++ /dev/null @@ -1,1309 +0,0 @@ -{ - "samples": [ - { - "x": 1.289974451065063, - "y": 5.590954303741456, - "heading": 6.19779206460361e-17, - "angularVelocity": 3.0320144312848013e-18, - "velocityX": 2.9896394667646305e-17, - "velocityY": -6.392586992550233e-17, - "timestamp": 0 - }, - { - "x": 1.3085927366636296, - "y": 5.570883327334207, - "heading": -0.01041389258493421, - "angularVelocity": -0.12226255374555442, - "velocityX": 0.21858484952476373, - "velocityY": -0.23563992154741434, - "timestamp": 0.08517646872162599 - }, - { - "x": 1.3442657042493709, - "y": 5.532508491008622, - "heading": -0.03089006103804589, - "angularVelocity": -0.24039701058788923, - "velocityX": 0.4188124739278457, - "velocityY": -0.45053330927585317, - "timestamp": 0.17035293744325197 - }, - { - "x": 1.3955881762749254, - "y": 5.477434538773948, - "heading": -0.060869932572704226, - "angularVelocity": -0.35197363760921707, - "velocityX": 0.6025428477586634, - "velocityY": -0.646586469963527, - "timestamp": 0.25552940616487796 - }, - { - "x": 1.4613425796478687, - "y": 5.4070770106059785, - "heading": -0.09976067247727309, - "angularVelocity": -0.45659018844361554, - "velocityX": 0.7719785095557636, - "velocityY": -0.826020721731408, - "timestamp": 0.34070587488650395 - }, - { - "x": 1.5403946295377764, - "y": 5.322786826751108, - "heading": -0.14691970563793563, - "angularVelocity": -0.5536626942681541, - "velocityX": 0.9280972911458275, - "velocityY": -0.9895947216401798, - "timestamp": 0.42588234360812993 - }, - { - "x": 1.6316218404845053, - "y": 5.225944550707253, - "heading": -0.20162745404412188, - "angularVelocity": -0.6422871155293178, - "velocityX": 1.0710377210503823, - "velocityY": -1.1369604480828668, - "timestamp": 0.5110588123297559 - }, - { - "x": 1.733844381505324, - "y": 5.118072938931053, - "heading": -0.2630340547325837, - "angularVelocity": -0.7209338636607621, - "velocityX": 1.2001265438099284, - "velocityY": -1.2664485085516666, - "timestamp": 0.5962352810513819 - }, - { - "x": 1.8457280635804503, - "y": 5.001046382736992, - "heading": -0.33003861392476214, - "angularVelocity": -0.7866557536115174, - "velocityX": 1.3135515448613582, - "velocityY": -1.373930593160977, - "timestamp": 0.6814117497730079 - }, - { - "x": 1.9656142072747882, - "y": 4.877758551128575, - "heading": -0.40085023097520717, - "angularVelocity": -0.8313518758551945, - "velocityX": 1.4075030990795228, - "velocityY": -1.447440043697368, - "timestamp": 0.7665882184946339 - }, - { - "x": 2.0856915448639635, - "y": 4.7606487505271025, - "heading": -0.4673031995363478, - "angularVelocity": -0.7801798966134944, - "velocityX": 1.4097477788332962, - "velocityY": -1.3749079101202415, - "timestamp": 0.8517646872162599 - }, - { - "x": 2.1993786689736172, - "y": 4.652829385838518, - "heading": -0.5273883646655675, - "angularVelocity": -0.7054197718103367, - "velocityX": 1.3347245526367961, - "velocityY": -1.2658351104101313, - "timestamp": 0.9369411559378859 - }, - { - "x": 2.3039823973737983, - "y": 4.5566291473122265, - "heading": -0.5809972368261334, - "angularVelocity": -0.6293859438546427, - "velocityX": 1.2280824736001708, - "velocityY": -1.1294227146313731, - "timestamp": 1.0221176246595118 - }, - { - "x": 2.3979860520770147, - "y": 4.47341696133058, - "heading": -0.6272322006251169, - "angularVelocity": -0.5428138134029581, - "velocityX": 1.1036340918339758, - "velocityY": -0.9769386689838181, - "timestamp": 1.107294093381138 - }, - { - "x": 2.480103614094797, - "y": 4.40444635319333, - "heading": -0.6652761118925774, - "angularVelocity": -0.4466481393094119, - "velocityX": 0.964087420507652, - "velocityY": -0.8097378204614404, - "timestamp": 1.192470562102764 - }, - { - "x": 2.549135601344952, - "y": 4.350942377101982, - "heading": -0.6944118607427208, - "angularVelocity": -0.3420633572561569, - "velocityX": 0.810458431609397, - "velocityY": -0.6281544292028495, - "timestamp": 1.2776470308243901 - }, - { - "x": 2.6038790433453562, - "y": 4.3141752600275325, - "heading": -0.7139618385994653, - "angularVelocity": -0.2295232257237371, - "velocityX": 0.6427061701667566, - "velocityY": -0.431658151908278, - "timestamp": 1.3628234995460162 - }, - { - "x": 2.6430389310079985, - "y": 4.2955467477546305, - "heading": -0.7232599969808831, - "angularVelocity": -0.1091634640525668, - "velocityX": 0.459750072412904, - "velocityY": -0.2187049140741477, - "timestamp": 1.4479999682676423 - }, - { - "x": 2.665171623229982, - "y": 4.296644687652586, - "heading": -0.721655115096118, - "angularVelocity": 0.018841845745096026, - "velocityX": 0.2598451491845503, - "velocityY": 0.012890178642452559, - "timestamp": 1.5331764369892684 - }, - { - "x": 2.66906909991143, - "y": 4.318945956778202, - "heading": -0.7091344221786624, - "angularVelocity": 0.14601587711521094, - "velocityX": 0.045452234946537245, - "velocityY": 0.2600766102664744, - "timestamp": 1.6189252826957972 - }, - { - "x": 2.6560642905413947, - "y": 4.360643314893585, - "heading": -0.6858248901313277, - "angularVelocity": 0.2718349367303499, - "velocityX": -0.15166162602986216, - "velocityY": 0.4862731127377579, - "timestamp": 1.704674128402326 - }, - { - "x": 2.627632992233998, - "y": 4.419910426276983, - "heading": -0.652529270737317, - "angularVelocity": 0.38829233349639936, - "velocityX": -0.331564793358298, - "velocityY": 0.6911709527407293, - "timestamp": 1.7904229741088546 - }, - { - "x": 2.5850645171946978, - "y": 4.495119603214681, - "heading": -0.6100207421436514, - "angularVelocity": 0.4957329541105318, - "velocityX": -0.4964320474352394, - "velocityY": 0.877086756305711, - "timestamp": 1.8761718198153834 - }, - { - "x": 2.5295631514165375, - "y": 4.584706937933874, - "heading": -0.5590704924389158, - "angularVelocity": 0.5941800065637087, - "velocityX": -0.6472549609368695, - "velocityY": 1.0447643228435226, - "timestamp": 1.9619206655219121 - }, - { - "x": 2.46234032691493, - "y": 4.687033037499603, - "heading": -0.5004982071284244, - "angularVelocity": 0.6830679157006081, - "velocityX": -0.7839501972035283, - "velocityY": 1.1933233470679585, - "timestamp": 2.047669511228441 - }, - { - "x": 2.384701887418879, - "y": 4.800204071292841, - "heading": -0.43526625936464175, - "angularVelocity": 0.7607326632364927, - "velocityX": -0.9054167301769336, - "velocityY": 1.319796585723857, - "timestamp": 2.1334183569349694 - }, - { - "x": 2.298178863290851, - "y": 4.921641059920068, - "heading": -0.36471802922759106, - "angularVelocity": 0.8227309598836902, - "velocityX": -1.0090284413174404, - "velocityY": 1.416193858082253, - "timestamp": 2.219167202641498 - }, - { - "x": 2.2047510720717436, - "y": 5.044849013310237, - "heading": -0.2923595970689714, - "angularVelocity": 0.8438414717122018, - "velocityX": -1.0895515904535924, - "velocityY": 1.4368467863910614, - "timestamp": 2.3049160483480264 - }, - { - "x": 2.116347360692902, - "y": 5.158956328540576, - "heading": -0.22539524544258233, - "angularVelocity": 0.7809358956920709, - "velocityX": -1.0309609493916598, - "velocityY": 1.3307154666649113, - "timestamp": 2.390664894054555 - }, - { - "x": 2.0362102002706437, - "y": 5.261312637562149, - "heading": -0.16556699187625293, - "angularVelocity": 0.6977149729932085, - "velocityX": -0.9345567250727145, - "velocityY": 1.1936756486715054, - "timestamp": 2.4764137397610835 - }, - { - "x": 1.9661275657868833, - "y": 5.350222584355773, - "heading": -0.1134489428170574, - "angularVelocity": 0.6077988412528272, - "velocityX": -0.8173011998740509, - "velocityY": 1.0368646488597075, - "timestamp": 2.562162585467612 - }, - { - "x": 1.9075019379268083, - "y": 5.42424212633875, - "heading": -0.06991588076878923, - "angularVelocity": 0.5076810269523403, - "velocityX": -0.6836899946235904, - "velocityY": 0.8632132756201296, - "timestamp": 2.6479114311741405 - }, - { - "x": 1.861590779787906, - "y": 5.48200360252104, - "heading": -0.03588037308734902, - "angularVelocity": 0.3969208844854328, - "velocityX": -0.5354142992901785, - "velocityY": 0.673612288379681, - "timestamp": 2.733660276880669 - }, - { - "x": 1.8296344375134035, - "y": 5.522099910983847, - "heading": -0.012259641477970167, - "angularVelocity": 0.2754641349951199, - "velocityX": -0.3726737311878495, - "velocityY": 0.4676017284248481, - "timestamp": 2.8194091225871976 - }, - { - "x": 1.8129411935806854, - "y": 5.542999267578127, - "heading": 2.3632850028431835e-17, - "angularVelocity": 0.14297150447865262, - "velocityX": -0.1946760191950624, - "velocityY": 0.24372755600475027, - "timestamp": 2.905157968293726 - }, - { - "x": 1.812941193580624, - "y": 5.542999267578126, - "heading": -4.920156603511903e-17, - "angularVelocity": -5.733397110540358e-17, - "velocityX": -7.214388151371406e-13, - "velocityY": -9.03683027618772e-16, - "timestamp": 2.9909068140002546 - }, - { - "x": 1.8513325137310372, - "y": 5.5430599450572595, - "heading": -0.000016850565291482023, - "angularVelocity": -0.00016991086246254008, - "velocityX": 0.3871147468977978, - "velocityY": 0.0006118348336243464, - "timestamp": 3.090079786505359 - }, - { - "x": 1.9275402570059652, - "y": 5.543180391388335, - "heading": -0.000015746696122309923, - "angularVelocity": 0.00001113074602288469, - "velocityX": 0.7684325814778618, - "velocityY": 0.0012145076227106227, - "timestamp": 3.189252759010463 - }, - { - "x": 2.0338963277323163, - "y": 5.54334848714078, - "heading": 0.000009170336978677823, - "angularVelocity": 0.00025124822289482134, - "velocityX": 1.0724299982122594, - "velocityY": 0.0016949754373461064, - "timestamp": 3.2884257315155674 - }, - { - "x": 2.163383514243291, - "y": 5.5435531416216355, - "heading": 0.000033654463962368084, - "angularVelocity": 0.0002468830606297171, - "velocityX": 1.305670115961409, - "velocityY": 0.00206361144256778, - "timestamp": 3.3875987040206716 - }, - { - "x": 2.3079360710490797, - "y": 5.54378160701675, - "heading": 0.00003317773344428256, - "angularVelocity": -0.000004807060894986283, - "velocityX": 1.457580156714077, - "velocityY": 0.0023037062351123753, - "timestamp": 3.486771676525776 - }, - { - "x": 2.437398077457075, - "y": 5.543986221690917, - "heading": 0.000022146124274066103, - "angularVelocity": -0.00011123604437218979, - "velocityX": 1.3054162151016944, - "velocityY": 0.002063210056106694, - "timestamp": 3.58594464903088 - }, - { - "x": 2.5437229473484053, - "y": 5.544154268122903, - "heading": 0.000015406432122086, - "angularVelocity": -0.00006795896080977589, - "velocityX": 1.0721153879486738, - "velocityY": 0.0016944781198031775, - "timestamp": 3.6851176215359844 - }, - { - "x": 2.619913870706316, - "y": 5.544274687859312, - "heading": 0.000009213432317016853, - "angularVelocity": -0.00006244644733859944, - "velocityX": 0.7682629796539725, - "velocityY": 0.0012142394582550931, - "timestamp": 3.7842905940410887 - }, - { - "x": 2.6583051681519314, - "y": 5.5443353652954785, - "heading": 6.370510606485668e-14, - "angularVelocity": -0.00009290265301587805, - "velocityX": 0.38711451795641516, - "velocityY": 0.0006118344003786568, - "timestamp": 3.883463566546193 - }, - { - "x": 2.658305168152, - "y": 5.544335365295488, - "heading": 6.625668653050138e-14, - "angularVelocity": 2.6906565131836242e-14, - "velocityX": 7.155954565396242e-13, - "velocityY": 8.671180221391495e-14, - "timestamp": 3.982636539051297 - }, - { - "x": 2.6398879819889713, - "y": 5.568034006444528, - "heading": 0.008388499707182374, - "angularVelocity": 0.0943787022324955, - "velocityX": -0.20721108536177554, - "velocityY": 0.26663254150593735, - "timestamp": 4.071517817439562 - }, - { - "x": 2.6055876283476542, - "y": 5.613232266775393, - "heading": 0.025296828917652628, - "angularVelocity": 0.19023499118162804, - "velocityX": -0.3859120195327329, - "velocityY": 0.5085239675934979, - "timestamp": 4.160399095827827 - }, - { - "x": 2.5578825329127857, - "y": 5.677978135359711, - "heading": 0.05006181260458975, - "angularVelocity": 0.27862992225075944, - "velocityX": -0.5367282773164671, - "velocityY": 0.7284533903912082, - "timestamp": 4.2492803742160925 - }, - { - "x": 2.4993543830957403, - "y": 5.760558609750369, - "heading": 0.0821470752651059, - "angularVelocity": 0.36099011222914734, - "velocityX": -0.6584980648176972, - "velocityY": 0.9291098855468343, - "timestamp": 4.338161652604358 - }, - { - "x": 2.433098392034208, - "y": 5.859358208334035, - "heading": 0.12114426250239245, - "angularVelocity": 0.43875592188117873, - "velocityX": -0.7454437229429913, - "velocityY": 1.1115906563809108, - "timestamp": 4.427042930992623 - }, - { - "x": 2.3637465533640976, - "y": 5.972668871410117, - "heading": 0.16678728284828395, - "angularVelocity": 0.5135279461940396, - "velocityX": -0.7802749907275444, - "velocityY": 1.2748541102325668, - "timestamp": 4.515924209380888 - }, - { - "x": 2.300988842467506, - "y": 6.095835737634532, - "heading": 0.21776795840799054, - "angularVelocity": 0.5735817090404455, - "velocityX": -0.7060847012414084, - "velocityY": 1.385745889998316, - "timestamp": 4.604805487769153 - }, - { - "x": 2.2515752134889366, - "y": 6.217231027383832, - "heading": 0.2695393710632505, - "angularVelocity": 0.582478263071738, - "velocityX": -0.5559509254902292, - "velocityY": 1.3658139481175915, - "timestamp": 4.693686766157418 - }, - { - "x": 2.217045589242532, - "y": 6.33145065782542, - "heading": 0.3198059590268783, - "angularVelocity": 0.5655475357136119, - "velocityX": -0.3884915346931451, - "velocityY": 1.2850808686903799, - "timestamp": 4.782568044545683 - }, - { - "x": 2.19907919431313, - "y": 6.435243176299548, - "heading": 0.36734143796630997, - "angularVelocity": 0.5348199283510537, - "velocityX": -0.20213924974232386, - "velocityY": 1.1677658147606698, - "timestamp": 4.871449322933948 - }, - { - "x": 2.1993959276283994, - "y": 6.526053318679287, - "heading": 0.41157070153690833, - "angularVelocity": 0.4976218206191788, - "velocityX": 0.0035635549001480525, - "velocityY": 1.021701577951703, - "timestamp": 4.960330601322213 - }, - { - "x": 2.219770279567901, - "y": 6.601608618777817, - "heading": 0.45152771224881855, - "angularVelocity": 0.4495548605556116, - "velocityX": 0.2292310856552352, - "velocityY": 0.8500699075051399, - "timestamp": 5.049211879710478 - }, - { - "x": 2.2626490848637184, - "y": 6.659247151615926, - "heading": 0.48436572022362784, - "angularVelocity": 0.36945922212429394, - "velocityX": 0.4824278641490618, - "velocityY": 0.648489016846146, - "timestamp": 5.1380931580987435 - }, - { - "x": 2.329372405994089, - "y": 6.697374820688125, - "heading": 0.5125504195998649, - "angularVelocity": 0.31710501792120865, - "velocityX": 0.7507016363871187, - "velocityY": 0.4289730049288785, - "timestamp": 5.2269744364870085 - }, - { - "x": 2.399251093648798, - "y": 6.73730555904706, - "heading": 0.5293913369356736, - "angularVelocity": 0.21745079806450165, - "velocityX": 0.902277239279105, - "velocityY": 0.5155877648973247, - "timestamp": 5.304421461219836 - }, - { - "x": 2.4725481079431484, - "y": 6.779189629834486, - "heading": 0.507422235287016, - "angularVelocity": -0.28366617987293774, - "velocityX": 0.9464148501358945, - "velocityY": 0.5408092945202858, - "timestamp": 5.381868485952664 - }, - { - "x": 2.537702213604298, - "y": 6.816420603018375, - "heading": 0.5016725044650684, - "angularVelocity": -0.07424082257084566, - "velocityX": 0.8412731915467285, - "velocityY": 0.48072825693890747, - "timestamp": 5.459315510685491 - }, - { - "x": 2.5904059035391898, - "y": 6.846537042294488, - "heading": 0.5036538467311155, - "angularVelocity": 0.025583194098508056, - "velocityX": 0.680512777970975, - "velocityY": 0.38886502613926416, - "timestamp": 5.536762535418319 - }, - { - "x": 2.6280877308244723, - "y": 6.868069547224279, - "heading": 0.508503710225543, - "angularVelocity": 0.06262168896947905, - "velocityX": 0.4865497083575037, - "velocityY": 0.27802882066934376, - "timestamp": 5.614209560151147 - }, - { - "x": 2.648344516781087, - "y": 6.8796448707953655, - "heading": 0.5125504196001363, - "angularVelocity": 0.052251321320124576, - "velocityX": 0.26155667100350394, - "velocityY": 0.14946117855453953, - "timestamp": 5.691656584883974 - }, - { - "x": 2.6483445167657806, - "y": 6.879644870779064, - "heading": 0.5125504196000689, - "angularVelocity": 1.8669291195039983e-14, - "velocityX": -4.749030905139852e-11, - "velocityY": 6.098431731550748e-11, - "timestamp": 5.769103609616802 - }, - { - "x": 2.6747339306487334, - "y": 6.883481123686636, - "heading": 0.5098224624623802, - "angularVelocity": -0.03296702980330207, - "velocityX": 0.3189128531205414, - "velocityY": 0.046360649216292196, - "timestamp": 5.851851650734406 - }, - { - "x": 2.7264961186821273, - "y": 6.89099146776212, - "heading": 0.5039121473834008, - "angularVelocity": -0.07142543798202369, - "velocityX": 0.6255397388782451, - "velocityY": 0.09076159355636307, - "timestamp": 5.93459969185201 - }, - { - "x": 2.8018050306182753, - "y": 6.9018959167453104, - "heading": 0.4949465844489951, - "angularVelocity": -0.10834773625231511, - "velocityX": 0.9100990297657242, - "velocityY": 0.13177893803804336, - "timestamp": 6.017347732969614 - }, - { - "x": 2.899101313212433, - "y": 6.915952840560763, - "heading": 0.4831136364117761, - "angularVelocity": -0.14299973603485935, - "velocityX": 1.1758137265856894, - "velocityY": 0.16987621248329202, - "timestamp": 6.100095774087218 - }, - { - "x": 3.017081432389269, - "y": 6.932956858771751, - "heading": 0.4685947114251132, - "angularVelocity": -0.1754594403754914, - "velocityX": 1.42577537284728, - "velocityY": 0.20549148936151598, - "timestamp": 6.182843815204822 - }, - { - "x": 3.1546294358748312, - "y": 6.9527283369160715, - "heading": 0.4515640044783447, - "angularVelocity": -0.20581401948312003, - "velocityX": 1.6622508717768, - "velocityY": 0.23893590563959213, - "timestamp": 6.265591856322426 - }, - { - "x": 3.3107722570719647, - "y": 6.975106161489216, - "heading": 0.43219034104896137, - "angularVelocity": -0.2341283632545328, - "velocityX": 1.8869669793779915, - "velocityY": 0.2704332848357982, - "timestamp": 6.34833989744003 - }, - { - "x": 3.484600968601402, - "y": 6.999935624537231, - "heading": 0.4106444392061959, - "angularVelocity": -0.2603795999490034, - "velocityX": 2.1006988102882644, - "velocityY": 0.3000610372485155, - "timestamp": 6.431087938557634 - }, - { - "x": 3.675234170530874, - "y": 7.027062068947035, - "heading": 0.38710417033209454, - "angularVelocity": -0.28448128265230255, - "velocityX": 2.3037790303523367, - "velocityY": 0.32781977728352046, - "timestamp": 6.513835979675238 - }, - { - "x": 3.8817881153060045, - "y": 7.056325014551516, - "heading": 0.36176128472936936, - "angularVelocity": -0.3062656862983269, - "velocityX": 2.4961792688429623, - "velocityY": 0.3536391340419632, - "timestamp": 6.596584020792842 - }, - { - "x": 4.1033458782194, - "y": 7.087551279488626, - "heading": 0.3348309137177563, - "angularVelocity": -0.32545025414364576, - "velocityX": 2.677498583906162, - "velocityY": 0.3773656090873216, - "timestamp": 6.679332061910446 - }, - { - "x": 4.338922324182521, - "y": 7.120545790816314, - "heading": 0.30656596895157695, - "angularVelocity": -0.34157841544560996, - "velocityX": 2.846912661392294, - "velocityY": 0.39873465138360303, - "timestamp": 6.76208010302805 - }, - { - "x": 4.587417768628414, - "y": 7.155077045432509, - "heading": 0.27728164973070885, - "angularVelocity": -0.3538974315929522, - "velocityX": 3.00303718480442, - "velocityY": 0.4173060068834074, - "timestamp": 6.844828144145654 - }, - { - "x": 4.847543704393879, - "y": 7.190849002148663, - "heading": 0.2474070078170501, - "angularVelocity": -0.36103140944690143, - "velocityX": 3.1435902560613274, - "velocityY": 0.4322997406709641, - "timestamp": 6.927576185263258 - }, - { - "x": 5.117670126803491, - "y": 7.227429623630717, - "heading": 0.21762399464747098, - "angularVelocity": -0.35992408723308145, - "velocityX": 3.264444919314729, - "velocityY": 0.4420723558891399, - "timestamp": 7.010324226380862 - }, - { - "x": 5.395335267694769, - "y": 7.263944271068559, - "heading": 0.1895241870207491, - "angularVelocity": -0.33958275322536735, - "velocityX": 3.3555494141142153, - "velocityY": 0.44127506759904395, - "timestamp": 7.093072267498466 - }, - { - "x": 5.667910261992729, - "y": 7.2967916361655485, - "heading": 0.1669091076503975, - "angularVelocity": -0.27330048016738406, - "velocityX": 3.294035612402776, - "velocityY": 0.3969564071048189, - "timestamp": 7.17582030861607 - }, - { - "x": 5.931901746285159, - "y": 7.327414779180602, - "heading": 0.14778373205342693, - "angularVelocity": -0.23112783503586568, - "velocityX": 3.1903049392702143, - "velocityY": 0.3700769541063461, - "timestamp": 7.258568349733674 - }, - { - "x": 6.1801220482371395, - "y": 7.354810947360831, - "heading": 0.11632647881372146, - "angularVelocity": -0.38015707459464126, - "velocityX": 2.999712121271843, - "velocityY": 0.33107935620263895, - "timestamp": 7.341316390851278 - }, - { - "x": 6.415697256681246, - "y": 7.379780463537999, - "heading": 0.08899977607154176, - "angularVelocity": -0.3302398748429845, - "velocityX": 2.84689770612573, - "velocityY": 0.3017535622589267, - "timestamp": 7.424064431968882 - }, - { - "x": 6.637987945996846, - "y": 7.402299511961184, - "heading": 0.06474721752358573, - "angularVelocity": -0.2930892166194916, - "velocityX": 2.686355910222349, - "velocityY": 0.2721399578655483, - "timestamp": 7.5068124730864865 - }, - { - "x": 6.845920803268571, - "y": 7.422253782487123, - "heading": 0.04362794173118966, - "angularVelocity": -0.2552238760840361, - "velocityX": 2.5128432584428544, - "velocityY": 0.24114492931132497, - "timestamp": 7.5895605142040905 - }, - { - "x": 7.038548371158306, - "y": 7.439534256583214, - "heading": 0.025735796286898687, - "angularVelocity": -0.2162243988212622, - "velocityX": 2.3278807001118484, - "velocityY": 0.2088324250667052, - "timestamp": 7.6723085553216945 - }, - { - "x": 7.2149794023417675, - "y": 7.454031322663086, - "heading": 0.011159229205506762, - "angularVelocity": -0.17615603807074215, - "velocityX": 2.1321475264013814, - "velocityY": 0.1751952781490545, - "timestamp": 7.755056596439299 - }, - { - "x": 7.374384880065902, - "y": 7.465639114379918, - "heading": -1.1165566205788637e-16, - "angularVelocity": -0.134857925997875, - "velocityX": 1.9263957861864134, - "velocityY": 0.14027874932231896, - "timestamp": 7.837804637556903 - }, - { - "x": 7.500924464216497, - "y": 7.473646968954779, - "heading": -0.0070937662662868915, - "angularVelocity": -0.09816609550664941, - "velocityX": 1.7511003938959169, - "velocityY": 0.11081557912801578, - "timestamp": 7.9100675327008405 - }, - { - "x": 7.613899532464792, - "y": 7.47952275118133, - "heading": -0.01171150501248939, - "angularVelocity": -0.06390193386252535, - "velocityX": 1.5633897316635108, - "velocityY": 0.08131119317636289, - "timestamp": 7.982330427844778 - }, - { - "x": 7.712046212988902, - "y": 7.483209835153203, - "heading": -0.014057088311073107, - "angularVelocity": -0.032459027470621904, - "velocityX": 1.3581891554251948, - "velocityY": 0.05102319751389575, - "timestamp": 8.054593322988717 - }, - { - "x": 7.79365194754084, - "y": 7.484595849351514, - "heading": -0.014345050898318633, - "angularVelocity": -0.003984930117620908, - "velocityX": 1.129289580626482, - "velocityY": 0.019180164253482204, - "timestamp": 8.126856218132655 - }, - { - "x": 7.856538953365895, - "y": 7.483519387813377, - "heading": -0.012796446625875068, - "angularVelocity": 0.02143014432730538, - "velocityX": 0.8702530627897808, - "velocityY": -0.0148964629220964, - "timestamp": 8.199119113276593 - }, - { - "x": 7.899249386445817, - "y": 7.479924646329156, - "heading": -0.009762276595150929, - "angularVelocity": 0.04198793896480952, - "velocityX": 0.5910423737502203, - "velocityY": -0.04974532887286408, - "timestamp": 8.271382008420531 - }, - { - "x": 7.921731862807926, - "y": 7.4739282664160145, - "heading": -0.005439516570392937, - "angularVelocity": 0.0598199119499377, - "velocityX": 0.31112061476836195, - "velocityY": -0.08298006744976984, - "timestamp": 8.343644903564469 - }, - { - "x": 7.923961639404313, - "y": 7.465639114379864, - "heading": -8.923477228591696e-17, - "angularVelocity": 0.07527399171536123, - "velocityX": 0.030856452567537763, - "velocityY": -0.11470827482980998, - "timestamp": 8.415907798708407 - }, - { - "x": 7.888861809774048, - "y": 7.450123409205229, - "heading": 0.009559512590155543, - "angularVelocity": 0.09629823646465466, - "velocityX": -0.3535799196587898, - "velocityY": -0.15629824551531185, - "timestamp": 8.515177660142104 - }, - { - "x": 7.817152523236329, - "y": 7.430666435185926, - "heading": 0.02163779335227315, - "angularVelocity": 0.12167117579976462, - "velocityX": -0.72236714650414, - "velocityY": -0.19600081775371833, - "timestamp": 8.614447521575801 - }, - { - "x": 7.711448594725456, - "y": 7.4075722780062225, - "heading": 0.036035719670363044, - "angularVelocity": 0.14503824333134957, - "velocityX": -1.064813902067085, - "velocityY": -0.23264016737978235, - "timestamp": 8.713717383009499 - }, - { - "x": 7.574048351882708, - "y": 7.3811128444264495, - "heading": 0.05252610728763101, - "angularVelocity": 0.16611675869298972, - "velocityX": -1.3841083371967766, - "velocityY": -0.2665404504210516, - "timestamp": 8.812987244443196 - }, - { - "x": 7.407019371926434, - "y": 7.351538272778396, - "heading": 0.07089974064841498, - "angularVelocity": 0.18508773050978647, - "velocityX": -1.682574928019174, - "velocityY": -0.2979209522500118, - "timestamp": 8.912257105876893 - }, - { - "x": 7.212241390572204, - "y": 7.319083353098022, - "heading": 0.09094586321763254, - "angularVelocity": 0.20193563564714437, - "velocityX": -1.96210590546985, - "velocityY": -0.3269362847056093, - "timestamp": 9.01152696731059 - }, - { - "x": 6.990210935369373, - "y": 7.283848196853213, - "heading": 0.11243058039021528, - "angularVelocity": 0.216427391579795, - "velocityX": -2.236635087388812, - "velocityY": -0.35494313919580667, - "timestamp": 9.110796828744288 - }, - { - "x": 6.744250267253016, - "y": 7.2462331722899656, - "heading": 0.1352094476478159, - "angularVelocity": 0.2294640783075409, - "velocityX": -2.4776973047417434, - "velocityY": -0.3789168637892277, - "timestamp": 9.210066690177985 - }, - { - "x": 6.474537469267562, - "y": 7.206343597573894, - "heading": 0.1591116816121769, - "angularVelocity": 0.24078036998495714, - "velocityX": -2.7169655934857664, - "velocityY": -0.401829660482739, - "timestamp": 9.309336551611683 - }, - { - "x": 6.185093329086759, - "y": 7.164701733979632, - "heading": 0.18384126922195682, - "angularVelocity": 0.24911475902781344, - "velocityX": -2.915730273020737, - "velocityY": -0.4194814316536095, - "timestamp": 9.40860641304538 - }, - { - "x": 5.8780260788239564, - "y": 7.121745615538567, - "heading": 0.2121770427035368, - "angularVelocity": 0.28544185589002363, - "velocityX": -3.0932575690950848, - "velocityY": -0.43272064472210836, - "timestamp": 9.507876274479077 - }, - { - "x": 5.557966335758099, - "y": 7.0784922097584015, - "heading": 0.24028820463072503, - "angularVelocity": 0.283179219968629, - "velocityX": -3.224138106404308, - "velocityY": -0.4357153838585266, - "timestamp": 9.607146135912775 - }, - { - "x": 5.245908943265439, - "y": 7.038535334063528, - "heading": 0.26569993334648234, - "angularVelocity": 0.25598634216619676, - "velocityX": -3.1435260207458486, - "velocityY": -0.40250762031696863, - "timestamp": 9.706415997346472 - }, - { - "x": 4.948601311707987, - "y": 7.0011016986080925, - "heading": 0.2891526852975207, - "angularVelocity": 0.23625249005411966, - "velocityX": -2.9949435534976283, - "velocityY": -0.37708963138260865, - "timestamp": 9.80568585878017 - }, - { - "x": 4.669544920179785, - "y": 6.966326786089845, - "heading": 0.31063199175620987, - "angularVelocity": 0.21637288647809105, - "velocityX": -2.811088758440401, - "velocityY": -0.35030685059909655, - "timestamp": 9.904955720213866 - }, - { - "x": 4.411446504016625, - "y": 6.934401644375056, - "heading": 0.3300269329519647, - "angularVelocity": 0.19537592694947853, - "velocityX": -2.599967527259491, - "velocityY": -0.3215995394142002, - "timestamp": 10.004225581647564 - }, - { - "x": 4.176717541970206, - "y": 6.9055345106659285, - "heading": 0.3471896751796373, - "angularVelocity": 0.17288975707028345, - "velocityX": -2.364554142177353, - "velocityY": -0.2907945401778102, - "timestamp": 10.103495443081261 - }, - { - "x": 3.9677270788540704, - "y": 6.879953869869358, - "heading": 0.3619526945831296, - "angularVelocity": 0.14871602710307644, - "velocityX": -2.1052760636290624, - "velocityY": -0.2576878866065069, - "timestamp": 10.202765304514958 - }, - { - "x": 3.7869910923072463, - "y": 6.857920125982094, - "heading": 0.37415675707125545, - "angularVelocity": 0.1229382444164791, - "velocityX": -1.8206531563211636, - "velocityY": -0.22195804012459744, - "timestamp": 10.302035165948656 - }, - { - "x": 3.6375222511124736, - "y": 6.83976261955988, - "heading": 0.3836657545916857, - "angularVelocity": 0.09578937033957077, - "velocityX": -1.5056819767458316, - "velocityY": -0.18291056479757375, - "timestamp": 10.401305027382353 - }, - { - "x": 3.5230866973435258, - "y": 6.825906628204701, - "heading": 0.39040681994356247, - "angularVelocity": 0.06790646480733925, - "velocityX": -1.1527723733691484, - "velocityY": -0.1395790339088257, - "timestamp": 10.50057488881605 - }, - { - "x": 3.4467104911360567, - "y": 6.816686862116541, - "heading": 0.39477159352312596, - "angularVelocity": 0.04396876873328538, - "velocityX": -0.7693795992500979, - "velocityY": -0.09287578279051523, - "timestamp": 10.599844750249748 - }, - { - "x": 3.4085204601287793, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": 0.021852252491790684, - "velocityX": -0.3847092204594867, - "velocityY": -0.046321940190690435, - "timestamp": 10.699114611683445 - }, - { - "x": 3.408520460128781, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": -9.258859562419926e-18, - "velocityX": 7.880837628913899e-16, - "velocityY": -2.4194099006901583e-16, - "timestamp": 10.798384473117142 - }, - { - "x": 3.4451800140480633, - "y": 6.811382414928076, - "heading": 0.39378662700224637, - "angularVelocity": -0.032542531675579224, - "velocityX": 0.37821978715244237, - "velocityY": -0.00728463273656313, - "timestamp": 10.895311062271263 - }, - { - "x": 3.518496947445585, - "y": 6.809970351527808, - "heading": 0.38746820532810716, - "angularVelocity": -0.06518770266528738, - "velocityX": 0.756417140408642, - "velocityY": -0.014568380179178244, - "timestamp": 10.992237651425384 - }, - { - "x": 3.6283397734394334, - "y": 6.8078551393922675, - "heading": 0.37750274880248313, - "angularVelocity": -0.1028144765290181, - "velocityX": 1.1332579321365746, - "velocityY": -0.021822826471036318, - "timestamp": 11.089164240579505 - }, - { - "x": 3.7707535238798844, - "y": 6.805113681812527, - "heading": 0.36197971834397535, - "angularVelocity": -0.16015244726939834, - "velocityX": 1.469294975540766, - "velocityY": -0.028283854860319504, - "timestamp": 11.186090829733626 - }, - { - "x": 3.9413941020263046, - "y": 6.801829282896055, - "heading": 0.341973429419134, - "angularVelocity": -0.20640661246244818, - "velocityX": 1.7605135973070385, - "velocityY": -0.03388542757086655, - "timestamp": 11.283017418887747 - }, - { - "x": 4.137139185749371, - "y": 6.798061838763412, - "heading": 0.3182906699990001, - "angularVelocity": -0.24433707640817395, - "velocityX": 2.0195189517276724, - "velocityY": -0.038869046827304486, - "timestamp": 11.379944008041868 - }, - { - "x": 4.3554217491364975, - "y": 6.793860650651487, - "heading": 0.2915337558008742, - "angularVelocity": -0.2760533970258723, - "velocityX": 2.2520400778783602, - "velocityY": -0.043344020960483176, - "timestamp": 11.476870597195989 - }, - { - "x": 4.593770534553052, - "y": 6.7892732393783435, - "heading": 0.26218341148270125, - "angularVelocity": -0.30281003978695553, - "velocityX": 2.459065025362269, - "velocityY": -0.04732871870534226, - "timestamp": 11.57379718635011 - }, - { - "x": 4.849438377602218, - "y": 6.784352472846038, - "heading": 0.23067476418411942, - "angularVelocity": -0.32507743822988416, - "velocityX": 2.6377472402607576, - "velocityY": -0.05076797373402456, - "timestamp": 11.67072377550423 - }, - { - "x": 5.118739979240287, - "y": 6.77916928922838, - "heading": 0.197506319622179, - "angularVelocity": -0.3422017100921637, - "velocityX": 2.7784079063161986, - "velocityY": -0.053475353490631775, - "timestamp": 11.767650364658351 - }, - { - "x": 5.388165163887155, - "y": 6.773984054928517, - "heading": 0.16458046580315022, - "angularVelocity": -0.33969888042458984, - "velocityX": 2.779682922902244, - "velocityY": -0.053496510556234986, - "timestamp": 11.864576953812472 - }, - { - "x": 5.643920085833601, - "y": 6.769061952936388, - "heading": 0.1333147392066983, - "angularVelocity": -0.32257120434452946, - "velocityX": 2.6386456407723093, - "velocityY": -0.05078175178849002, - "timestamp": 11.961503542966593 - }, - { - "x": 5.882330711464664, - "y": 6.764473684698037, - "heading": 0.10420435480854875, - "angularVelocity": -0.3003343525465651, - "velocityX": 2.4597030362017005, - "velocityY": -0.04733756008947892, - "timestamp": 12.058430132120714 - }, - { - "x": 6.100655045491889, - "y": 6.760272005917349, - "heading": 0.07768097943341207, - "angularVelocity": -0.27364395679871373, - "velocityX": 2.252471029183505, - "velocityY": -0.04334908323253468, - "timestamp": 12.155356721274835 - }, - { - "x": 6.29642574105686, - "y": 6.756504387895065, - "heading": 0.054221932212767415, - "angularVelocity": -0.2420290183052161, - "velocityX": 2.019783191314849, - "velocityY": -0.038870840861772844, - "timestamp": 12.252283310428956 - }, - { - "x": 6.467079365349485, - "y": 6.7532201591852425, - "heading": 0.03442675668029697, - "angularVelocity": -0.20422853734174792, - "velocityX": 1.7606481955253344, - "velocityY": -0.03388367153413224, - "timestamp": 12.349209899583077 - }, - { - "x": 6.60949406591357, - "y": 6.750479406444339, - "heading": 0.019100936295010484, - "angularVelocity": -0.15811781389436272, - "velocityX": 1.4693047780484314, - "velocityY": -0.028276582977095337, - "timestamp": 12.446136488737197 - }, - { - "x": 6.719322188320328, - "y": 6.748365796127614, - "heading": 0.009318208717894495, - "angularVelocity": -0.10092924616960162, - "velocityX": 1.1331062339573879, - "velocityY": -0.021806300367839485, - "timestamp": 12.543063077891318 - }, - { - "x": 6.792644320795612, - "y": 6.746954731447967, - "heading": 0.0031061365662838365, - "angularVelocity": -0.06409048544701255, - "velocityX": 0.756470779743405, - "velocityY": -0.014558076292195965, - "timestamp": 12.63998966704544 - }, - { - "x": 6.829305171966555, - "y": 6.746249198913574, - "heading": -6.996174209709525e-17, - "angularVelocity": -0.03204627949246174, - "velocityX": 0.3782331710099824, - "velocityY": -0.007279040153471663, - "timestamp": 12.73691625619956 - }, - { - "x": 6.8293051719665545, - "y": 6.746249198913574, - "heading": -3.495236266448189e-17, - "angularVelocity": 5.858001175456844e-19, - "velocityX": -7.463612085764823e-16, - "velocityY": 1.4437890171198592e-17, - "timestamp": 12.833842845353681 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADEF.1.traj b/src/main/deploy/choreo/CenterLanePCBADEF.1.traj deleted file mode 100644 index 44f1f2c8..00000000 --- a/src/main/deploy/choreo/CenterLanePCBADEF.1.traj +++ /dev/null @@ -1,157 +0,0 @@ -{ - "samples": [ - { - "x": 1.3078742027282717, - "y": 5.537254810333252, - "heading": 5.4444853034280076e-17, - "angularVelocity": -1.7134882602513402e-15, - "velocityX": 6.870651213375127e-14, - "velocityY": -6.867248083045425e-14, - "timestamp": 0 - }, - { - "x": 1.327170103173586, - "y": 5.517955423153612, - "heading": -0.008175377183837335, - "angularVelocity": -0.09765213543106893, - "velocityX": 0.23048305187379453, - "velocityY": -0.23052469974457607, - "timestamp": 0.08371938972710291 - }, - { - "x": 1.3657619180934388, - "y": 5.479356717010402, - "heading": -0.024521335360667328, - "angularVelocity": -0.19524698197290347, - "velocityX": 0.4609662713219617, - "velocityY": -0.4610485846711018, - "timestamp": 0.16743877945420582 - }, - { - "x": 1.423649754196992, - "y": 5.421458744466686, - "heading": -0.04902545949966477, - "angularVelocity": -0.2926935351401013, - "velocityX": 0.6914507653752389, - "velocityY": -0.6915718417495008, - "timestamp": 0.25115816918130873 - }, - { - "x": 1.500833817418447, - "y": 5.344261559488766, - "heading": -0.08166832650675512, - "angularVelocity": -0.3899080859702319, - "velocityX": 0.9219377192434074, - "velocityY": -0.9220944542185107, - "timestamp": 0.33487755890841164 - }, - { - "x": 1.5973144196195872, - "y": 5.247765239032123, - "heading": -0.1224243942133242, - "angularVelocity": -0.48681754417250844, - "velocityX": 1.1524283981958594, - "velocityY": -1.1526161474801648, - "timestamp": 0.41859694863551455 - }, - { - "x": 1.71309198631675, - "y": 5.131969908089758, - "heading": -0.17126309032246767, - "angularVelocity": -0.5833618265534611, - "velocityX": 1.3829241597980892, - "velocityY": -1.3831363477423682, - "timestamp": 0.5023163383626175 - }, - { - "x": 1.84816708196567, - "y": 4.996875770190428, - "heading": -0.22814932169203733, - "angularVelocity": -0.6794869331346065, - "velocityX": 1.6134266636345465, - "velocityY": -1.613654116921907, - "timestamp": 0.5860357280897204 - }, - { - "x": 2.0025627636590344, - "y": 4.842495557885179, - "heading": -0.2918324673553054, - "angularVelocity": -0.7606737921866519, - "velocityX": 1.8442045767013158, - "velocityY": -1.8440198000543986, - "timestamp": 0.6697551178168233 - }, - { - "x": 2.137660080489021, - "y": 4.707413614165848, - "heading": -0.34751647944173375, - "angularVelocity": -0.6651268274642137, - "velocityX": 1.6136920881812329, - "velocityY": -1.613508461536256, - "timestamp": 0.7534745075439262 - }, - { - "x": 2.2534584850572914, - "y": 4.591629667252782, - "heading": -0.3952239272052681, - "angularVelocity": -0.569849444902123, - "velocityX": 1.3831730611717787, - "velocityY": -1.3830003693347765, - "timestamp": 0.8371938972710291 - }, - { - "x": 2.3499575912437507, - "y": 4.495143461778056, - "heading": -0.4349689949602177, - "angularVelocity": -0.4747414892118174, - "velocityX": 1.15264942208745, - "velocityY": -1.1524953274174494, - "timestamp": 0.920913286998132 - }, - { - "x": 2.4271571129829432, - "y": 4.417954772578404, - "heading": -0.46676067803032645, - "angularVelocity": -0.3797409796432922, - "velocityX": 0.9221223660473082, - "velocityY": -0.9219929749997143, - "timestamp": 1.004632676725235 - }, - { - "x": 2.485056851699902, - "y": 4.3600634144135135, - "heading": -0.49060405496538323, - "angularVelocity": -0.28480113164677934, - "velocityX": 0.6915929380958452, - "velocityY": -0.691492835215309, - "timestamp": 1.0883520664523378 - }, - { - "x": 2.523656690441612, - "y": 4.321469248073772, - "heading": -0.5065010886126352, - "angularVelocity": -0.1898847292015727, - "velocityX": 0.4610621131799047, - "velocityY": -0.460994358243006, - "timestamp": 1.1720714561794408 - }, - { - "x": 2.5429565906524716, - "y": 4.302172183990473, - "heading": -0.514451122232844, - "angularVelocity": -0.09496048222667672, - "velocityX": 0.23053082773023703, - "velocityY": -0.23049695113880436, - "timestamp": 1.2557908459065437 - }, - { - "x": 2.542956590652466, - "y": 4.3021721839904785, - "heading": -0.5144511222328438, - "angularVelocity": 1.7361053619799633e-15, - "velocityX": -6.870229148895338e-14, - "velocityY": 6.86710519982077e-14, - "timestamp": 1.3395102356336466 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADEF.2.traj b/src/main/deploy/choreo/CenterLanePCBADEF.2.traj deleted file mode 100644 index be37f8ad..00000000 --- a/src/main/deploy/choreo/CenterLanePCBADEF.2.traj +++ /dev/null @@ -1,211 +0,0 @@ -{ - "samples": [ - { - "x": 2.542956590652466, - "y": 4.3021721839904785, - "heading": -0.5144511222328438, - "angularVelocity": 1.7361053619799633e-15, - "velocityX": -6.870229148895338e-14, - "velocityY": 6.86710519982077e-14, - "timestamp": 0 - }, - { - "x": 2.5305584868573745, - "y": 4.308773452126341, - "heading": -0.5122554014922367, - "angularVelocity": 0.03659296401704725, - "velocityX": -0.20662161524599887, - "velocityY": 0.11001397531805143, - "timestamp": 0.06000390511094533 - }, - { - "x": 2.5059487154966082, - "y": 4.322311826419086, - "heading": -0.50771784412737, - "angularVelocity": 0.07562103427230116, - "velocityX": -0.4101361622258381, - "velocityY": 0.22562488670882788, - "timestamp": 0.12000781022189067 - }, - { - "x": 2.4694009892818585, - "y": 4.343244277958484, - "heading": -0.5006377479710926, - "angularVelocity": 0.11799392294862207, - "velocityX": -0.6090891275688425, - "velocityY": 0.3488514872606191, - "timestamp": 0.180011715332836 - }, - { - "x": 2.4213500625117965, - "y": 4.372224481954429, - "heading": -0.49072373538892167, - "angularVelocity": 0.16522278948078847, - "velocityX": -0.800796659504371, - "velocityY": 0.48297196561394073, - "timestamp": 0.24001562044378133 - }, - { - "x": 2.36256927737863, - "y": 4.410248403268193, - "heading": -0.4775208921950913, - "angularVelocity": 0.2200330656716209, - "velocityX": -0.9796159937337826, - "velocityY": 0.6336907780161798, - "timestamp": 0.30001952555472666 - }, - { - "x": 2.294685044293501, - "y": 4.45893547211586, - "heading": -0.46025532607703995, - "angularVelocity": 0.28774070764440873, - "velocityX": -1.1313302519163222, - "velocityY": 0.8113983374522957, - "timestamp": 0.360023430665672 - }, - { - "x": 2.2219430126998723, - "y": 4.5207468069818875, - "heading": -0.4375774026440522, - "angularVelocity": 0.3779407922043896, - "velocityX": -1.2122882912225668, - "velocityY": 1.0301218687640459, - "timestamp": 0.4200273357766173 - }, - { - "x": 2.1543482605183306, - "y": 4.595554934892757, - "heading": -0.4086588228612356, - "angularVelocity": 0.48194496223782435, - "velocityX": -1.1265058841847213, - "velocityY": 1.2467209887848296, - "timestamp": 0.48003124088756266 - }, - { - "x": 2.099014749930773, - "y": 4.677184514185738, - "heading": -0.3762413759174227, - "angularVelocity": 0.5402556197613143, - "velocityX": -0.9221651571718094, - "velocityY": 1.3604044460448101, - "timestamp": 0.540035145998508 - }, - { - "x": 2.057477466278187, - "y": 4.7614747474435095, - "heading": -0.3421230482764429, - "angularVelocity": 0.5686017864653305, - "velocityX": -0.692243006114092, - "velocityY": 1.4047457928266707, - "timestamp": 0.6000390511094533 - }, - { - "x": 2.029984208014376, - "y": 4.846326580474425, - "heading": -0.30718701619967975, - "angularVelocity": 0.5822293067787422, - "velocityX": -0.4581911496089643, - "velocityY": 1.4141051798883366, - "timestamp": 0.6600429562203987 - }, - { - "x": 2.0165337281988327, - "y": 4.930557757850057, - "heading": -0.2719247119892122, - "angularVelocity": 0.5876668217721605, - "velocityX": -0.2241600740930749, - "velocityY": 1.4037615921812139, - "timestamp": 0.720046861331344 - }, - { - "x": 2.017073631286621, - "y": 5.013424873352051, - "heading": -0.23664369858950388, - "angularVelocity": 0.58797862129931, - "velocityX": 0.008997799173072052, - "velocityY": 1.3810287071945508, - "timestamp": 0.7800507664422893 - }, - { - "x": 2.0456987990727344, - "y": 5.12687213061024, - "heading": -0.1871316885421809, - "angularVelocity": 0.5834829718812528, - "velocityX": 0.3373383135622466, - "velocityY": 1.336939113429588, - "timestamp": 0.8649067304264437 - }, - { - "x": 2.101800262534532, - "y": 5.2343841203738455, - "heading": -0.13890470594146578, - "angularVelocity": 0.5683393404112499, - "velocityX": 0.6611375421092794, - "velocityY": 1.2669939119857474, - "timestamp": 0.9497626944105981 - }, - { - "x": 2.1841232780581077, - "y": 5.331790166507821, - "heading": -0.09372578148319055, - "angularVelocity": 0.5324189642900253, - "velocityX": 0.9701500243276792, - "velocityY": 1.1478986456646028, - "timestamp": 1.0346186583947525 - }, - { - "x": 2.2868206827051014, - "y": 5.409909264516836, - "heading": -0.05582004781301006, - "angularVelocity": 0.4467067709849929, - "velocityX": 1.2102555887076, - "velocityY": 0.9206082205795442, - "timestamp": 1.119474622378907 - }, - { - "x": 2.385467579367521, - "y": 5.4603756892360344, - "heading": -0.030726479319374042, - "angularVelocity": 0.2957195618957538, - "velocityX": 1.1625216664893496, - "velocityY": 0.5947304390840709, - "timestamp": 1.2043305863630613 - }, - { - "x": 2.463261391908942, - "y": 5.492112879652653, - "heading": -0.014444831338496888, - "angularVelocity": 0.1918739380995954, - "velocityX": 0.916774836898297, - "velocityY": 0.3740124904189999, - "timestamp": 1.2891865503472157 - }, - { - "x": 2.5161956459202934, - "y": 5.510821266131426, - "heading": -0.0045926513861969465, - "angularVelocity": 0.11610474372950008, - "velocityX": 0.6238130064875114, - "velocityY": 0.22047226382658144, - "timestamp": 1.3740425143313701 - }, - { - "x": 2.542956590652466, - "y": 5.519354820251465, - "heading": -1.9593529845154978e-18, - "angularVelocity": 0.05412290628216246, - "velocityX": 0.3153690498073838, - "velocityY": 0.10056516618716548, - "timestamp": 1.4588984783155245 - }, - { - "x": 2.542956590652466, - "y": 5.519354820251465, - "heading": -3.879161841617201e-19, - "angularVelocity": 1.851886937303712e-17, - "velocityX": 1.8638350784782603e-17, - "velocityY": 7.390265321449622e-16, - "timestamp": 1.543754442299679 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADEF.3.traj b/src/main/deploy/choreo/CenterLanePCBADEF.3.traj deleted file mode 100644 index cae0edb6..00000000 --- a/src/main/deploy/choreo/CenterLanePCBADEF.3.traj +++ /dev/null @@ -1,157 +0,0 @@ -{ - "samples": [ - { - "x": 2.542956590652466, - "y": 5.519354820251465, - "heading": -3.879161841617201e-19, - "angularVelocity": 1.851886937303712e-17, - "velocityX": 1.8638350784782603e-17, - "velocityY": 7.390265321449622e-16, - "timestamp": 0 - }, - { - "x": 2.54352414805122, - "y": 5.5417247332646085, - "heading": 0.010369864258801478, - "angularVelocity": 0.1365642797123967, - "velocityX": 0.007474356984997478, - "velocityY": 0.294597015122544, - "timestamp": 0.07593394319979074 - }, - { - "x": 2.5446584508658887, - "y": 5.5864645661514665, - "heading": 0.031108645745408125, - "angularVelocity": 0.27311608765055906, - "velocityX": 0.01493802069101894, - "velocityY": 0.5891941205943013, - "timestamp": 0.1518678863995815 - }, - { - "x": 2.5463582976769072, - "y": 5.6535744095701785, - "heading": 0.062210562054685774, - "angularVelocity": 0.4095917451230601, - "velocityX": 0.022385862492957306, - "velocityY": 0.8837924199740047, - "timestamp": 0.22780182959937223 - }, - { - "x": 2.5486221305372125, - "y": 5.743054499123618, - "heading": 0.10366116200111275, - "angularVelocity": 0.545877090003949, - "velocityX": 0.029813187158595614, - "velocityY": 1.1783938220883299, - "timestamp": 0.303735772799163 - }, - { - "x": 2.551448101584002, - "y": 5.8549052924787315, - "heading": 0.15543248064293438, - "angularVelocity": 0.6817941550276853, - "velocityX": 0.03721617668864778, - "velocityY": 1.473001251374778, - "timestamp": 0.3796697159989537 - }, - { - "x": 2.5548341868258384, - "y": 5.989127553824463, - "heading": 0.2174776706164971, - "angularVelocity": 0.8170942711392554, - "velocityX": 0.04459251158506785, - "velocityY": 1.7676187445261111, - "timestamp": 0.45560365919874446 - }, - { - "x": 2.5587783438313068, - "y": 6.145722446236129, - "heading": 0.28972525218567036, - "angularVelocity": 0.9514530462231059, - "velocityX": 0.05194194900547891, - "velocityY": 2.0622515546130344, - "timestamp": 0.5315376023985352 - }, - { - "x": 2.563218286954705, - "y": 6.324743248037854, - "heading": 0.36865194803097623, - "angularVelocity": 1.039412580453524, - "velocityX": 0.058471125511242075, - "velocityY": 2.357586005124216, - "timestamp": 0.607471545598326 - }, - { - "x": 2.5671026763479077, - "y": 6.481388915794399, - "heading": 0.43754245115183227, - "angularVelocity": 0.9072425349964675, - "velocityX": 0.05115484893208649, - "velocityY": 2.0629202324498515, - "timestamp": 0.6834054887981167 - }, - { - "x": 2.5704320555482925, - "y": 6.615658176157516, - "heading": 0.4964771667239727, - "angularVelocity": 0.7761313727258498, - "velocityX": 0.04384573038206363, - "velocityY": 1.7682376906180024, - "timestamp": 0.7593394319979074 - }, - { - "x": 2.573206618251443, - "y": 6.727550075342484, - "heading": 0.5455200541615356, - "angularVelocity": 0.6458625137973618, - "velocityX": 0.03653916267524383, - "velocityY": 1.4735425880698358, - "timestamp": 0.8352733751976982 - }, - { - "x": 2.575426370371336, - "y": 6.8170639437544045, - "heading": 0.5847189679540402, - "angularVelocity": 0.5162238669651036, - "velocityX": 0.02923267285162808, - "velocityY": 1.1788386673980507, - "timestamp": 0.9112073183974889 - }, - { - "x": 2.577091240828593, - "y": 6.884199374047621, - "heading": 0.6141061452751087, - "angularVelocity": 0.38700976246877483, - "velocityX": 0.021925246959405044, - "velocityY": 0.884129382252347, - "timestamp": 0.9871412615972797 - }, - { - "x": 2.5782011573891666, - "y": 6.928956199415487, - "heading": 0.6336989699349357, - "angularVelocity": 0.25802459129872973, - "velocityX": 0.014616869792382615, - "velocityY": 0.5894179003730387, - "timestamp": 1.0630752047970704 - }, - { - "x": 2.578756093978882, - "y": 6.951334476470947, - "heading": 0.643500664804274, - "angularVelocity": 0.1290818632129897, - "velocityX": 0.007308149245661792, - "velocityY": 0.2947071640488953, - "timestamp": 1.1390091479968611 - }, - { - "x": 2.578756093978882, - "y": 6.951334476470947, - "heading": 0.643500664804274, - "angularVelocity": -1.7225163541747852e-17, - "velocityX": -8.122490732634945e-17, - "velocityY": -7.511091870024743e-16, - "timestamp": 1.2149430911966523 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADEF.4.traj b/src/main/deploy/choreo/CenterLanePCBADEF.4.traj deleted file mode 100644 index de047ec1..00000000 --- a/src/main/deploy/choreo/CenterLanePCBADEF.4.traj +++ /dev/null @@ -1,427 +0,0 @@ -{ - "samples": [ - { - "x": 2.578756093978882, - "y": 6.951334476470947, - "heading": 0.643500664804274, - "angularVelocity": -1.7225163541747852e-17, - "velocityX": -8.122490732634945e-17, - "velocityY": -7.511091870024743e-16, - "timestamp": 0 - }, - { - "x": 2.605882805940747, - "y": 6.95657005522081, - "heading": 0.6386264747117201, - "angularVelocity": -0.05791461301218251, - "velocityX": 0.3223167327766511, - "velocityY": 0.06220859495330271, - "timestamp": 0.08416166212711573 - }, - { - "x": 2.6601362026337743, - "y": 6.967041240264821, - "heading": 0.6288779935837058, - "angularVelocity": -0.11583042541734088, - "velocityX": 0.6446331420009866, - "velocityY": 0.12441751718491452, - "timestamp": 0.16832332425423147 - }, - { - "x": 2.7415162653471867, - "y": 6.98274808058871, - "heading": 0.6142571418365809, - "angularVelocity": -0.17372341963781532, - "velocityX": 0.966949328905814, - "velocityY": 0.1866270214597963, - "timestamp": 0.2524849863813472 - }, - { - "x": 2.8500229829470065, - "y": 7.003690647779458, - "heading": 0.594767720039105, - "angularVelocity": -0.23157125590081054, - "velocityX": 1.2892653835179018, - "velocityY": 0.24883737632363517, - "timestamp": 0.33664664850846293 - }, - { - "x": 2.9856563505646374, - "y": 7.029869037377032, - "heading": 0.5704152085572207, - "angularVelocity": -0.28935397503322835, - "velocityX": 1.6115813802818453, - "velocityY": 0.3110488663832933, - "timestamp": 0.42080831063557866 - }, - { - "x": 3.1484163679588817, - "y": 7.06128337029242, - "heading": 0.5412065071898937, - "angularVelocity": -0.3470547114814702, - "velocityX": 1.933897374179897, - "velocityY": 0.37326179309459206, - "timestamp": 0.5049699727626944 - }, - { - "x": 3.3383030376303036, - "y": 7.097933794128878, - "heading": 0.5071496172056923, - "angularVelocity": -0.40466037769979885, - "velocityX": 2.2562133977893946, - "velocityY": 0.4354764736121926, - "timestamp": 0.5891316348898101 - }, - { - "x": 3.555316362844474, - "y": 7.139820484190559, - "heading": 0.4682532762402473, - "angularVelocity": -0.46216222425238096, - "velocityX": 2.578529460200051, - "velocityY": 0.49769323707530094, - "timestamp": 0.6732932970169259 - }, - { - "x": 3.799456345915655, - "y": 7.186943643856573, - "heading": 0.424526577651669, - "angularVelocity": -0.5195560244822011, - "velocityX": 2.9008455501085413, - "velocityY": 0.5599124170675296, - "timestamp": 0.7574549591440416 - }, - { - "x": 4.07072298772812, - "y": 7.239303503682716, - "heading": 0.37597867364929377, - "angularVelocity": -0.5768410791251948, - "velocityX": 3.2231616505238603, - "velocityY": 0.6221343364994486, - "timestamp": 0.8416166212711573 - }, - { - "x": 4.369116292263885, - "y": 7.296900317207954, - "heading": 0.32261896114654104, - "angularVelocity": -0.634014480633234, - "velocityX": 3.5454777982530907, - "velocityY": 0.6843592684546252, - "timestamp": 0.9257782833982731 - }, - { - "x": 4.69463630229783, - "y": 7.359734340352348, - "heading": 0.2644605433432946, - "angularVelocity": -0.6910321913011374, - "velocityX": 3.8677944542289113, - "velocityY": 0.7465872412250016, - "timestamp": 1.0099399455253888 - }, - { - "x": 5.043878143874846, - "y": 7.42716808821782, - "heading": 0.26446053600249464, - "angularVelocity": -8.722261133391044e-8, - "velocityX": 4.149654756693505, - "velocityY": 0.8012406856179081, - "timestamp": 1.0941016076525045 - }, - { - "x": 5.3943988333147095, - "y": 7.487603347084882, - "heading": 0.2644605286726163, - "angularVelocity": -8.709284181708098e-8, - "velocityX": 4.164849892228206, - "velocityY": 0.7180853768760186, - "timestamp": 1.1782632697796203 - }, - { - "x": 5.748554706480114, - "y": 7.520626966261676, - "heading": 0.26445951238259063, - "angularVelocity": -0.000012075450982753597, - "velocityX": 4.2080427621604946, - "velocityY": 0.3923831628576489, - "timestamp": 1.262424931906736 - }, - { - "x": 6.076359506533188, - "y": 7.546997553632611, - "heading": 0.24369373540901995, - "angularVelocity": -0.24673677359421306, - "velocityX": 3.894942088453133, - "velocityY": 0.3133325400715727, - "timestamp": 1.3465865940338517 - }, - { - "x": 6.37718666957215, - "y": 7.567469602956491, - "heading": 0.21707426667956767, - "angularVelocity": -0.31628972214506673, - "velocityX": 3.574396648495391, - "velocityY": 0.24324673261514576, - "timestamp": 1.4307482561609675 - }, - { - "x": 6.650951360645961, - "y": 7.582275601726408, - "heading": 0.18847076354087783, - "angularVelocity": -0.33986380990774573, - "velocityX": 3.2528432085897303, - "velocityY": 0.17592331705087103, - "timestamp": 1.5149099182880832 - }, - { - "x": 6.897625912379422, - "y": 7.591527252808584, - "heading": 0.15968573269508482, - "angularVelocity": -0.3420207029932105, - "velocityX": 2.9309610278477014, - "velocityY": 0.1099271431712301, - "timestamp": 1.599071580415199 - }, - { - "x": 7.1171977964953355, - "y": 7.595290076757919, - "heading": 0.13176491488072156, - "angularVelocity": -0.3317522148290329, - "velocityX": 2.6089299874362895, - "velocityY": 0.04470947761999478, - "timestamp": 1.6832332425423147 - }, - { - "x": 7.309660237553315, - "y": 7.5936071064013415, - "heading": 0.10539226300609161, - "angularVelocity": -0.3133570702869136, - "velocityX": 2.286818441956269, - "velocityY": -0.019996876416657455, - "timestamp": 1.7673949046694304 - }, - { - "x": 7.475009136714085, - "y": 7.586508749225222, - "heading": 0.0810505950391384, - "angularVelocity": -0.28922513353156276, - "velocityX": 1.9646581944999189, - "velocityY": -0.08434193190479512, - "timestamp": 1.8515565667965461 - }, - { - "x": 7.613241814863639, - "y": 7.574017622800665, - "heading": 0.059099291100008985, - "angularVelocity": -0.26082308006197424, - "velocityX": 1.642466114093265, - "velocityY": -0.14841824779661225, - "timestamp": 1.9357182289236619 - }, - { - "x": 7.724356418372183, - "y": 7.556151203473209, - "heading": 0.03981643413778623, - "angularVelocity": -0.2291168742972126, - "velocityX": 1.3202520090527614, - "velocityY": -0.21228691159250246, - "timestamp": 2.0198798910507776 - }, - { - "x": 7.80835160711804, - "y": 7.5329233986954955, - "heading": 0.023423620240304886, - "angularVelocity": -0.1947776871697487, - "velocityX": 0.998021980827717, - "velocityY": -0.27599032850171645, - "timestamp": 2.1040415531778933 - }, - { - "x": 7.865226377203999, - "y": 7.5043455392513225, - "heading": 0.010101499773974827, - "angularVelocity": -0.15829203142647919, - "velocityX": 0.6757800244018113, - "velocityY": -0.3395591142319701, - "timestamp": 2.188203215305009 - }, - { - "x": 7.894979953765869, - "y": 7.4704270362854, - "heading": -6.199321830779289e-18, - "angularVelocity": -0.12002495576570035, - "velocityX": 0.35352886112124365, - "velocityY": -0.4030160777325445, - "timestamp": 2.272364877432125 - }, - { - "x": 7.894584049642754, - "y": 7.426198672978825, - "heading": -0.007089479197593018, - "angularVelocity": -0.07587613336913762, - "velocityX": -0.004237218730680758, - "velocityY": -0.4733601861879681, - "timestamp": 2.365799780909647 - }, - { - "x": 7.86075117401306, - "y": 7.375444727785397, - "heading": -0.010043840448864623, - "angularVelocity": -0.03161946062246776, - "velocityX": -0.3621010390173313, - "velocityY": -0.5432011304601843, - "timestamp": 2.4592346843871695 - }, - { - "x": 7.793469713318101, - "y": 7.318225470876191, - "heading": -0.00885143445531551, - "angularVelocity": 0.012761890355416958, - "velocityX": -0.7200891550248647, - "velocityY": -0.6123970248759323, - "timestamp": 2.552669587864692 - }, - { - "x": 7.692724412768832, - "y": 7.254620943238055, - "heading": -0.0034978695073770924, - "angularVelocity": 0.057297270598950276, - "velocityX": -1.078240537525724, - "velocityY": -0.6807362695401956, - "timestamp": 2.6461044913422143 - }, - { - "x": 7.558494360533822, - "y": 7.184742577510254, - "heading": 0.006036114483891398, - "angularVelocity": 0.10203878461288343, - "velocityX": -1.4366157317998562, - "velocityY": -0.747882890943545, - "timestamp": 2.7395393948197366 - }, - { - "x": 7.390749153984831, - "y": 7.108756149139622, - "heading": 0.019778831050343372, - "angularVelocity": 0.14708332812435573, - "velocityX": -1.795316314415032, - "velocityY": -0.8132552776587466, - "timestamp": 2.832974298297259 - }, - { - "x": 7.189440632055284, - "y": 7.026934307092558, - "heading": 0.03777709910485055, - "angularVelocity": 0.19262895753766113, - "velocityX": -2.1545323475180176, - "velocityY": -0.8757096010351999, - "timestamp": 2.9264092017747814 - }, - { - "x": 6.954481412279556, - "y": 6.93980890428099, - "heading": 0.06012313662850344, - "angularVelocity": 0.2391615626705131, - "velocityX": -2.514683603566329, - "velocityY": -0.93247169493281, - "timestamp": 3.0198441052523037 - }, - { - "x": 6.685672117824161, - "y": 6.848872549717405, - "heading": 0.08707457656091541, - "angularVelocity": 0.2884515200349684, - "velocityX": -2.8769687177989223, - "velocityY": -0.9732589340712626, - "timestamp": 3.113279008729826 - }, - { - "x": 6.387993943545244, - "y": 6.775960862611549, - "heading": 0.12150620101602463, - "angularVelocity": 0.3685092312787836, - "velocityX": -3.1859419039323758, - "velocityY": -0.780347433262947, - "timestamp": 3.2067139122073485 - }, - { - "x": 6.122643180061621, - "y": 6.713816410019983, - "heading": 0.15262946767199945, - "angularVelocity": 0.3331010735561154, - "velocityX": -2.839953310889416, - "velocityY": -0.6651096140589043, - "timestamp": 3.300148815684871 - }, - { - "x": 5.890214769411301, - "y": 6.660443600401627, - "heading": 0.1800531937910235, - "angularVelocity": 0.29350622838307167, - "velocityX": -2.4875972682546292, - "velocityY": -0.5712298898151609, - "timestamp": 3.393583719162393 - }, - { - "x": 5.69086823658003, - "y": 6.615216731280533, - "heading": 0.2036583525973359, - "angularVelocity": 0.2526374826511273, - "velocityX": -2.1335338873574874, - "velocityY": -0.4840468330121896, - "timestamp": 3.4870186226399156 - }, - { - "x": 5.5246769076205275, - "y": 6.577830247082936, - "heading": 0.22338706488015242, - "angularVelocity": 0.21114927664652222, - "velocityX": -1.7786857242217098, - "velocityY": -0.4001340270725567, - "timestamp": 3.580453526117438 - }, - { - "x": 5.391682817900511, - "y": 6.548103124628764, - "heading": 0.2392048833965071, - "angularVelocity": 0.16929239425136144, - "velocityX": -1.4233876717388545, - "velocityY": -0.31815864679865835, - "timestamp": 3.6738884295949603 - }, - { - "x": 5.291913195980534, - "y": 6.525915639931743, - "heading": 0.2510888771367695, - "angularVelocity": 0.12719008954850897, - "velocityX": -1.067798201814147, - "velocityY": -0.23746462907578247, - "timestamp": 3.7673233330724827 - }, - { - "x": 5.225387107672126, - "y": 6.511182746987271, - "heading": 0.25902271171762453, - "angularVelocity": 0.08491296384507606, - "velocityX": -0.7120046773999501, - "velocityY": -0.15768082800039224, - "timestamp": 3.860758236550005 - }, - { - "x": 5.1921186447143555, - "y": 6.503840923309326, - "heading": 0.26299425401034693, - "angularVelocity": 0.04250598165039902, - "velocityX": -0.3560603341959137, - "velocityY": -0.07857688513276403, - "timestamp": 3.9541931400275274 - }, - { - "x": 5.1921186447143555, - "y": 6.503840923309326, - "heading": 0.26299425401034693, - "angularVelocity": -1.0095416875346628e-18, - "velocityX": 2.3925664461171115e-17, - "velocityY": 7.068676560436054e-18, - "timestamp": 4.04762804350505 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADEF.5.traj b/src/main/deploy/choreo/CenterLanePCBADEF.5.traj deleted file mode 100644 index 0753b0bc..00000000 --- a/src/main/deploy/choreo/CenterLanePCBADEF.5.traj +++ /dev/null @@ -1,436 +0,0 @@ -{ - "samples": [ - { - "x": 5.1921186447143555, - "y": 6.503840923309326, - "heading": 0.26299425401034693, - "angularVelocity": -1.0095416875346628e-18, - "velocityX": 2.3925664461171115e-17, - "velocityY": 7.068676560436054e-18, - "timestamp": 0 - }, - { - "x": 5.218809865013869, - "y": 6.495313073233126, - "heading": 0.260871879579823, - "angularVelocity": -0.025050084717691923, - "velocityX": 0.3150326917368419, - "velocityY": -0.10065300627272344, - "timestamp": 0.08472523963262191 - }, - { - "x": 5.272196468754811, - "y": 6.478270476254072, - "heading": 0.2566236198235182, - "angularVelocity": -0.05014160803469802, - "velocityX": 0.6301145204478967, - "velocityY": -0.2011513576468166, - "timestamp": 0.16945047926524381 - }, - { - "x": 5.352283792023045, - "y": 6.452729954483983, - "heading": 0.25024542978948194, - "angularVelocity": -0.07528087334651229, - "velocityX": 0.9452593302244072, - "velocityY": -0.3014511600183777, - "timestamp": 0.2541757188978657 - }, - { - "x": 5.4590789190058935, - "y": 6.418713893785019, - "heading": 0.2417322157383651, - "angularVelocity": -0.10048025934221186, - "velocityX": 1.2604877536602386, - "velocityY": -0.40148674523037453, - "timestamp": 0.33890095853048763 - }, - { - "x": 5.592591707275698, - "y": 6.376253550611183, - "heading": 0.23107691549234363, - "angularVelocity": -0.12576299922223963, - "velocityX": 1.575832524625849, - "velocityY": -0.5011534149439888, - "timestamp": 0.42362619816310954 - }, - { - "x": 5.752836810532502, - "y": 6.32539562702347, - "heading": 0.21826867910988845, - "angularVelocity": -0.15117379942497813, - "velocityX": 1.8913502511370301, - "velocityY": -0.6002688668481637, - "timestamp": 0.5083514377957314 - }, - { - "x": 5.939838300939574, - "y": 6.266217475974801, - "heading": 0.20328868573896017, - "angularVelocity": -0.17680673947790815, - "velocityX": 2.207152097980862, - "velocityY": -0.6984713328079369, - "timestamp": 0.5930766774283533 - }, - { - "x": 6.153643038134906, - "y": 6.198872009915045, - "heading": 0.18609787015363735, - "angularVelocity": -0.20290076085785252, - "velocityX": 2.5235070224931055, - "velocityY": -0.7948689947856541, - "timestamp": 0.6778019170609753 - }, - { - "x": 6.394381892977596, - "y": 6.123803995127001, - "heading": 0.16657845141587072, - "angularVelocity": -0.23038493396306808, - "velocityX": 2.841406597214261, - "velocityY": -0.8860171433394348, - "timestamp": 0.7625271566935972 - }, - { - "x": 6.661972791946675, - "y": 6.056622100950942, - "heading": 0.1411655347825555, - "angularVelocity": -0.2999450546673997, - "velocityX": 3.1583374697951134, - "velocityY": -0.7929383790163071, - "timestamp": 0.8472523963262191 - }, - { - "x": 6.9032496475776535, - "y": 5.999073073447199, - "heading": 0.11754236952388923, - "angularVelocity": -0.27882087275408163, - "velocityX": 2.847756544297579, - "velocityY": -0.6792430184120029, - "timestamp": 0.931977635958841 - }, - { - "x": 7.118031418712295, - "y": 5.950645420596591, - "heading": 0.09585021181159172, - "angularVelocity": -0.2560294642583155, - "velocityX": 2.5350388156582375, - "velocityY": -0.5715847256448627, - "timestamp": 1.0167028755914629 - }, - { - "x": 7.306261982278755, - "y": 5.911173870778648, - "heading": 0.07613235590314016, - "angularVelocity": -0.23272705977522684, - "velocityX": 2.221658674353119, - "velocityY": -0.46587711039940277, - "timestamp": 1.1014281152240848 - }, - { - "x": 7.467914023047444, - "y": 5.8805767421157285, - "heading": 0.05840904716752805, - "angularVelocity": -0.20918570207015538, - "velocityX": 1.9079561352630119, - "velocityY": -0.36113357478352465, - "timestamp": 1.1861533548567067 - }, - { - "x": 7.602971384357183, - "y": 5.858805325942835, - "heading": 0.0426917303890982, - "angularVelocity": -0.18550926319691677, - "velocityX": 1.594062901389981, - "velocityY": -0.2569649406398561, - "timestamp": 1.2708785944893286 - }, - { - "x": 7.711423390458376, - "y": 5.845827273377796, - "heading": 0.028987684559485204, - "angularVelocity": -0.16174691141666067, - "velocityX": 1.2800436631569734, - "velocityY": -0.15317811576944246, - "timestamp": 1.3556038341219505 - }, - { - "x": 7.793262462229127, - "y": 5.841619537994678, - "heading": 0.017301971212697442, - "angularVelocity": -0.13792481906759255, - "velocityX": 0.9659349696219826, - "velocityY": -0.0496633045992219, - "timestamp": 1.4403290737545724 - }, - { - "x": 7.848482940858505, - "y": 5.846164867627585, - "heading": 0.0076383901458884716, - "angularVelocity": -0.11405787825105443, - "velocityX": 0.6517594859432665, - "velocityY": 0.053647881701079494, - "timestamp": 1.5250543133871943 - }, - { - "x": 7.87708044052124, - "y": 5.859449863433838, - "heading": 2.497416150959083e-27, - "angularVelocity": -0.09015483672881196, - "velocityX": 0.3375322369902693, - "velocityY": 0.15680092335953588, - "timestamp": 1.6097795530198162 - }, - { - "x": 7.881596231266855, - "y": 5.877948058282651, - "heading": -0.005158165074008103, - "angularVelocity": -0.06905249312983469, - "velocityX": 0.060453010898891046, - "velocityY": 0.2476358267688033, - "timestamp": 1.6844787396850904 - }, - { - "x": 7.865360693168207, - "y": 5.903065257170116, - "heading": -0.008731993343707898, - "angularVelocity": -0.0478429341635813, - "velocityX": -0.21734558063394657, - "velocityY": 0.3362446099984395, - "timestamp": 1.7591779263503646 - }, - { - "x": 7.82831181618014, - "y": 5.934603651644639, - "heading": -0.010711784706445375, - "angularVelocity": -0.026503519664931297, - "velocityX": -0.4959743022917116, - "velocityY": 0.42220532622189155, - "timestamp": 1.8338771130156388 - }, - { - "x": 7.770377377578464, - "y": 5.972324090639225, - "heading": -0.0110857390814715, - "angularVelocity": -0.005006137171235987, - "velocityX": -0.7755698714803699, - "velocityY": 0.5049645207465326, - "timestamp": 1.908576299680913 - }, - { - "x": 7.691472435431706, - "y": 6.0159317447407945, - "heading": -0.009839381087694242, - "angularVelocity": 0.016685027634399323, - "velocityX": -1.0563025605664207, - "velocityY": 0.583776826071393, - "timestamp": 1.9832754863461872 - }, - { - "x": 7.591496122933511, - "y": 6.065054366948533, - "heading": -0.006954729975421036, - "angularVelocity": 0.038616901214725004, - "velocityX": -1.338385556273679, - "velocityY": 0.6576058508890599, - "timestamp": 2.0579746730114614 - }, - { - "x": 7.470327733756537, - "y": 6.119207749983016, - "heading": -0.0024090670382981736, - "angularVelocity": 0.06085291072166403, - "velocityX": -1.6220844507976708, - "velocityY": 0.7249527799699275, - "timestamp": 2.1326738596767356 - }, - { - "x": 7.32782268400487, - "y": 6.177737571853534, - "heading": 0.0038269526830036, - "angularVelocity": 0.08348176198015718, - "velocityX": -1.9077188937843252, - "velocityY": 0.7835402831464292, - "timestamp": 2.20737304634201 - }, - { - "x": 7.163811403874334, - "y": 6.2397140922834655, - "heading": 0.01179227386310508, - "angularVelocity": 0.10663196663430786, - "velocityX": -2.195623372252593, - "velocityY": 0.8296813284948795, - "timestamp": 2.282072233007284 - }, - { - "x": 6.978114754903294, - "y": 6.303722941599131, - "heading": 0.021540119487110404, - "angularVelocity": 0.13049466880657248, - "velocityX": -2.4859259820745265, - "velocityY": 0.8568881693784349, - "timestamp": 2.356771419672558 - }, - { - "x": 6.770640654116627, - "y": 6.367398692968795, - "heading": 0.03314423110651253, - "angularVelocity": 0.15534455109129794, - "velocityX": -2.7774613091351856, - "velocityY": 0.8524289783099493, - "timestamp": 2.4314706063378324 - }, - { - "x": 6.541925002807842, - "y": 6.426263625769268, - "heading": 0.046694627071435006, - "angularVelocity": 0.18139951142495733, - "velocityX": -3.06182251131669, - "velocityY": 0.7880264220846899, - "timestamp": 2.5061697930031066 - }, - { - "x": 6.296286014325702, - "y": 6.471418454274109, - "heading": 0.06214080908224435, - "angularVelocity": 0.20677844967741577, - "velocityX": -3.288375676469458, - "velocityY": 0.6044888909859091, - "timestamp": 2.580868979668381 - }, - { - "x": 6.048501736882419, - "y": 6.494895270234614, - "heading": 0.07847895732781966, - "angularVelocity": 0.2187192254018271, - "velocityX": -3.3170947168889495, - "velocityY": 0.3142847600965746, - "timestamp": 2.655568166333655 - }, - { - "x": 5.810207804194887, - "y": 6.498763072278465, - "heading": 0.09455495650408928, - "angularVelocity": 0.2152098288339583, - "velocityX": -3.190047219059051, - "velocityY": 0.051778368902233175, - "timestamp": 2.730267352998929 - }, - { - "x": 5.585913181304932, - "y": 6.485940456390381, - "heading": 0.10987224560749582, - "angularVelocity": 0.2050529568955417, - "velocityX": -3.002638086208546, - "velocityY": -0.17165670016652868, - "timestamp": 2.8049665396642034 - }, - { - "x": 5.393948798137783, - "y": 6.461779934393033, - "heading": 0.12306987524609604, - "angularVelocity": 0.19297917140210155, - "velocityX": -2.8069531133047367, - "velocityY": -0.3532814333608597, - "timestamp": 2.8733554209238346 - }, - { - "x": 5.21822858427444, - "y": 6.42928387175211, - "heading": 0.13522346577307512, - "angularVelocity": 0.17771296010588833, - "velocityX": -2.569426646946333, - "velocityY": -0.4751658755398492, - "timestamp": 2.9417443021834657 - }, - { - "x": 5.060196677201855, - "y": 6.392264431165203, - "heading": 0.1462160897383734, - "angularVelocity": 0.1607370052387037, - "velocityX": -2.3107836268389246, - "velocityY": -0.5413078837533164, - "timestamp": 3.010133183443097 - }, - { - "x": 4.920363237530942, - "y": 6.353780654626919, - "heading": 0.15599475116563047, - "angularVelocity": 0.14298612942845923, - "velocityX": -2.0446809056585025, - "velocityY": -0.5627197846998563, - "timestamp": 3.078522064702728 - }, - { - "x": 4.798767231888284, - "y": 6.3161460308896284, - "heading": 0.164539456070254, - "angularVelocity": 0.12494289637791432, - "velocityX": -1.7780084043345064, - "velocityY": -0.550303251699868, - "timestamp": 3.146910945962359 - }, - { - "x": 4.695244909056443, - "y": 6.281093323544826, - "heading": 0.17184599738491455, - "angularVelocity": 0.10683814649521928, - "velocityX": -1.5137303158803064, - "velocityY": -0.5125497990196586, - "timestamp": 3.2152998272219904 - }, - { - "x": 4.609559458166785, - "y": 6.2499355875386025, - "heading": 0.17791742648618403, - "angularVelocity": 0.08877801463398721, - "velocityX": -1.2529149375080866, - "velocityY": -0.45559651557885517, - "timestamp": 3.2836887084816215 - }, - { - "x": 4.541458842404482, - "y": 6.223686967257069, - "heading": 0.18275993511057495, - "angularVelocity": 0.070808419953631, - "velocityX": -0.9957849069612339, - "velocityY": -0.3838141492896176, - "timestamp": 3.3520775897412527 - }, - { - "x": 4.490700080737798, - "y": 6.203146864655871, - "heading": 0.18638086356776906, - "angularVelocity": 0.052946157189611806, - "velocityX": -0.7422078082252005, - "velocityY": -0.30034271979417115, - "timestamp": 3.420466471000884 - }, - { - "x": 4.4570583798792995, - "y": 6.188957712133428, - "heading": 0.1887877351300412, - "angularVelocity": 0.03519390166852904, - "velocityX": -0.49191769537480406, - "velocityY": -0.2074774767637344, - "timestamp": 3.488855352260515 - }, - { - "x": 4.440329551696777, - "y": 6.181644916534424, - "heading": 0.18998779389424572, - "angularVelocity": 0.017547571214809383, - "velocityX": -0.2446132744738579, - "velocityY": -0.10692959826674406, - "timestamp": 3.557244233520146 - }, - { - "x": 4.440329551696777, - "y": 6.181644916534424, - "heading": 0.18998779389424572, - "angularVelocity": -4.688613187627218e-28, - "velocityX": 1.0698378725405786e-25, - "velocityY": 1.9964070921469186e-25, - "timestamp": 3.6256331147797773 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADEF.6.traj b/src/main/deploy/choreo/CenterLanePCBADEF.6.traj deleted file mode 100644 index 244e50f8..00000000 --- a/src/main/deploy/choreo/CenterLanePCBADEF.6.traj +++ /dev/null @@ -1,589 +0,0 @@ -{ - "samples": [ - { - "x": 4.440329551696777, - "y": 6.181644916534424, - "heading": 0.18998779389424572, - "angularVelocity": -4.688613187627218e-28, - "velocityX": 1.0698378725405786e-25, - "velocityY": 1.9964070921469186e-25, - "timestamp": 0 - }, - { - "x": 4.439579335848565, - "y": 6.164768781343018, - "heading": 0.18962382517380036, - "angularVelocity": -0.00553315604255771, - "velocityX": -0.011404994771739275, - "velocityY": -0.2565558086834744, - "timestamp": 0.06577958720952637 - }, - { - "x": 4.438629166641586, - "y": 6.131001049566098, - "heading": 0.18887821684872066, - "angularVelocity": -0.01133494989417828, - "velocityX": -0.014444742621334671, - "velocityY": -0.5133466658791909, - "timestamp": 0.13155917441905274 - }, - { - "x": 4.4381503145293495, - "y": 6.080347141602359, - "heading": 0.18773000934555084, - "angularVelocity": -0.01745537714477353, - "velocityX": -0.00727964605054284, - "velocityY": -0.7700551206317605, - "timestamp": 0.1973387616285791 - }, - { - "x": 4.438975373866779, - "y": 6.012850914220267, - "heading": 0.18615416077885757, - "angularVelocity": -0.023956498262503952, - "velocityX": 0.012542786788878593, - "velocityY": -1.0260968523122063, - "timestamp": 0.2631183488381055 - }, - { - "x": 4.442155378938134, - "y": 5.928626966420744, - "heading": 0.1841206539292539, - "angularVelocity": -0.030913949689686845, - "velocityX": 0.04834334185201353, - "velocityY": -1.2803964173755829, - "timestamp": 0.32889793604763184 - }, - { - "x": 4.449039762750971, - "y": 5.827921573409061, - "heading": 0.1815937471088899, - "angularVelocity": -0.038414756424589545, - "velocityX": 0.10465836142917671, - "velocityY": -1.5309520366995943, - "timestamp": 0.3946775232571582 - }, - { - "x": 4.461381608820963, - "y": 5.71122949511904, - "heading": 0.1785321677552386, - "angularVelocity": -0.04654300039766565, - "velocityX": 0.18762425538913388, - "velocityY": -1.7739861747432784, - "timestamp": 0.4604571104666846 - }, - { - "x": 4.481448613268838, - "y": 5.57951486602773, - "heading": 0.1748924161672784, - "angularVelocity": -0.055332539201965195, - "velocityX": 0.30506431096863224, - "velocityY": -2.0023632661567494, - "timestamp": 0.526236697676211 - }, - { - "x": 4.5120438147722846, - "y": 5.434590034225561, - "heading": 0.17063962874888192, - "angularVelocity": -0.06465208431378763, - "velocityX": 0.4651169580312549, - "velocityY": -2.2031885262603184, - "timestamp": 0.5920162848857373 - }, - { - "x": 4.556188469541101, - "y": 5.279577397974022, - "heading": 0.1657694617290444, - "angularVelocity": -0.07403766466829045, - "velocityX": 0.6710996015861679, - "velocityY": -2.356546199625419, - "timestamp": 0.6577958720952637 - }, - { - "x": 4.616267960626566, - "y": 5.118958057204039, - "heading": 0.16032836999773611, - "angularVelocity": -0.08271702456837958, - "velocityX": 0.9133455169625041, - "velocityY": -2.441780916903065, - "timestamp": 0.7235754593047901 - }, - { - "x": 4.6932263289877705, - "y": 4.957658238915064, - "heading": 0.15440456088239551, - "angularVelocity": -0.09005543158049287, - "velocityX": 1.1699430115921938, - "velocityY": -2.4521257297524386, - "timestamp": 0.7893550465143164 - }, - { - "x": 4.786707207470846, - "y": 4.799875743117793, - "heading": 0.14809694695472803, - "angularVelocity": -0.09589014153548765, - "velocityX": 1.4211229113571657, - "velocityY": -2.39865439250461, - "timestamp": 0.8551346337238428 - }, - { - "x": 4.895747846043762, - "y": 4.648670678033537, - "heading": 0.14149381469729397, - "angularVelocity": -0.10038269526382383, - "velocityX": 1.6576668112189705, - "velocityY": -2.298662419431504, - "timestamp": 0.9209142209333692 - }, - { - "x": 5.019277488115815, - "y": 4.506151226786177, - "heading": 0.13466693251764544, - "angularVelocity": -0.10378420524140636, - "velocityX": 1.8779327647431951, - "velocityY": -2.1666212467006614, - "timestamp": 0.9866938081428955 - }, - { - "x": 5.1563191413879395, - "y": 4.373770713806152, - "heading": 0.1276729143915235, - "angularVelocity": -0.10632505345228195, - "velocityX": 2.083346203368645, - "velocityY": -2.0124862224865496, - "timestamp": 1.052473395352422 - }, - { - "x": 5.333320265467547, - "y": 4.234610501686042, - "heading": 0.11935234394669658, - "angularVelocity": -0.10849739642365235, - "velocityX": 2.30803419717898, - "velocityY": -1.814601631091569, - "timestamp": 1.1291625207977685 - }, - { - "x": 5.526705635502563, - "y": 4.1115365335688585, - "heading": 0.11088465842948947, - "angularVelocity": -0.11041572671527947, - "velocityX": 2.5216791678350265, - "velocityY": -1.604842504103067, - "timestamp": 1.205851646243115 - }, - { - "x": 5.7352637837384215, - "y": 4.005696194354983, - "heading": 0.10229707398048027, - "angularVelocity": -0.11197916783037644, - "velocityX": 2.7195270127899818, - "velocityY": -1.3801218699423634, - "timestamp": 1.2825407716884616 - }, - { - "x": 5.95717408037646, - "y": 3.9185355692390273, - "heading": 0.09362936542456811, - "angularVelocity": -0.11302395881524778, - "velocityX": 2.8936344670690923, - "velocityY": -1.1365447788040335, - "timestamp": 1.3592298971338082 - }, - { - "x": 6.189557120584961, - "y": 3.851808580822914, - "heading": 0.08494227942173509, - "angularVelocity": -0.11327663410406259, - "velocityX": 3.030195465902305, - "velocityY": -0.8700971360492012, - "timestamp": 1.4359190225791547 - }, - { - "x": 6.427716553529618, - "y": 3.8073042106200683, - "heading": 0.0763299644889769, - "angularVelocity": -0.11230164489091633, - "velocityX": 3.1055176540562526, - "velocityY": -0.5803217854474332, - "timestamp": 1.5126081480245013 - }, - { - "x": 6.664399710640609, - "y": 3.7857133712502065, - "heading": 0.06792429760501602, - "angularVelocity": -0.109607024922345, - "velocityX": 3.0862675214580135, - "velocityY": -0.2815371702895352, - "timestamp": 1.5892972734698478 - }, - { - "x": 6.890876802597326, - "y": 3.7846905044197223, - "heading": 0.059852862986743374, - "angularVelocity": -0.1052487503462913, - "velocityX": 2.9531839180787136, - "velocityY": -0.013337834074180547, - "timestamp": 1.6659863989151944 - }, - { - "x": 7.100368603662355, - "y": 3.799117631492698, - "heading": 0.05217739052380199, - "angularVelocity": -0.10008553909525866, - "velocityX": 2.7317015267611535, - "velocityY": 0.18812480895035505, - "timestamp": 1.742675524360541 - }, - { - "x": 7.289321103149217, - "y": 3.823808235084369, - "heading": 0.044905901451856856, - "angularVelocity": -0.09481773367108355, - "velocityX": 2.4638760500864425, - "velocityY": 0.3219570369108888, - "timestamp": 1.8193646498058875 - }, - { - "x": 7.456197806965785, - "y": 3.8548124388815017, - "heading": 0.038028185708090405, - "angularVelocity": -0.08968306397845134, - "velocityX": 2.176015215292753, - "velocityY": 0.40428422696290434, - "timestamp": 1.896053775251234 - }, - { - "x": 7.600383975861882, - "y": 3.8893298407285344, - "heading": 0.0315311794356911, - "angularVelocity": -0.08471874251623218, - "velocityX": 1.880138390662096, - "velocityY": 0.4500951294800189, - "timestamp": 1.9727429006965806 - }, - { - "x": 7.721659266950539, - "y": 3.9253622327898867, - "heading": 0.02540302978123265, - "angularVelocity": -0.07990897821394244, - "velocityX": 1.581388370051051, - "velocityY": 0.4698500843777596, - "timestamp": 2.049432026141927 - }, - { - "x": 7.819973686957294, - "y": 3.9614436370035824, - "heading": 0.019633766687128053, - "angularVelocity": -0.07522922005697107, - "velocityX": 1.281986454217959, - "velocityY": 0.4704891861025283, - "timestamp": 2.1261211515872738 - }, - { - "x": 7.895351647860016, - "y": 3.9964652092207733, - "heading": 0.014215138878361277, - "angularVelocity": -0.07065705570770218, - "velocityX": 0.9829028622375049, - "velocityY": 0.45666933889016026, - "timestamp": 2.2028102770326203 - }, - { - "x": 7.947849278823351, - "y": 4.029564484128199, - "heading": 0.009140302774238713, - "angularVelocity": -0.0661741293130177, - "velocityX": 0.6845511753911964, - "velocityY": 0.4316032385975494, - "timestamp": 2.279499402477967 - }, - { - "x": 7.9775347399255425, - "y": 4.060054180581902, - "heading": 0.004403534947127983, - "angularVelocity": -0.06176583445962591, - "velocityX": 0.3870882726827797, - "velocityY": 0.39757522695226577, - "timestamp": 2.3561885279233135 - }, - { - "x": 7.984478950500488, - "y": 4.087375164031982, - "heading": 2.1272994684354894e-27, - "angularVelocity": -0.05742059153179738, - "velocityX": 0.09055013386343501, - "velocityY": 0.35625629176787293, - "timestamp": 2.43287765336866 - }, - { - "x": 7.971946711920367, - "y": 4.1092761415431145, - "heading": -0.003743193123138188, - "angularVelocity": -0.05350584756338805, - "velocityX": -0.1791379779341521, - "velocityY": 0.31305634672073157, - "timestamp": 2.5028362334790604 - }, - { - "x": 7.9405304961902266, - "y": 4.1282629145767356, - "heading": -0.007208614807300815, - "angularVelocity": -0.04953533474655825, - "velocityX": -0.44906880157605467, - "velocityY": 0.2714002057168399, - "timestamp": 2.5727948135894607 - }, - { - "x": 7.890211232557957, - "y": 4.14446189431145, - "heading": -0.010391724637129625, - "angularVelocity": -0.045499920450151686, - "velocityX": -0.7192722258350864, - "velocityY": 0.2315510078842599, - "timestamp": 2.642753393699861 - }, - { - "x": 7.820967420002681, - "y": 4.158023023328294, - "heading": -0.013287169417379553, - "angularVelocity": -0.04138798665840062, - "velocityX": -0.9897829893917608, - "velocityY": 0.19384511514446137, - "timestamp": 2.7127119738102614 - }, - { - "x": 7.732774758572793, - "y": 4.169126981913827, - "heading": -0.015888540423835095, - "angularVelocity": -0.03718444545830319, - "velocityX": -1.2606411006443015, - "velocityY": 0.1587218975572353, - "timestamp": 2.7826705539206618 - }, - { - "x": 7.6256058033641905, - "y": 4.177995649727882, - "heading": -0.0181880242087685, - "angularVelocity": -0.032869217489900605, - "velocityX": -1.5318915140856217, - "velocityY": 0.1267702660640051, - "timestamp": 2.852629134031062 - }, - { - "x": 7.499429791688496, - "y": 4.184907877485426, - "heading": -0.020175882010708173, - "angularVelocity": -0.0284147819867506, - "velocityX": -1.8035816546959227, - "velocityY": 0.09880457474459138, - "timestamp": 2.9225877141414625 - }, - { - "x": 7.3542130604785445, - "y": 4.190224383970643, - "heading": -0.021839639250263897, - "angularVelocity": -0.023782032696063358, - "velocityX": -2.075752980989419, - "velocityY": 0.07599505988868352, - "timestamp": 2.992546294251863 - }, - { - "x": 7.189921237384926, - "y": 4.194429374442866, - "heading": -0.023162754266758116, - "angularVelocity": -0.01891283405704093, - "velocityX": -2.348415631568725, - "velocityY": 0.060106858452354715, - "timestamp": 3.062504874362263 - }, - { - "x": 7.006526810014663, - "y": 4.198205365374793, - "heading": -0.024122279434321083, - "angularVelocity": -0.01371561809929168, - "velocityX": -2.6214715490344274, - "velocityY": 0.05397466509423875, - "timestamp": 3.1324634544726635 - }, - { - "x": 6.804034419528801, - "y": 4.202581059133061, - "heading": -0.024684383598055692, - "angularVelocity": -0.008034813783349493, - "velocityX": -2.8944611249443675, - "velocityY": 0.06254692064022167, - "timestamp": 3.202422034583064 - }, - { - "x": 6.582574314888401, - "y": 4.209262107218236, - "heading": -0.024794803569428318, - "angularVelocity": -0.0015783620993789793, - "velocityX": -3.1655888997592005, - "velocityY": 0.09550005266877039, - "timestamp": 3.272380614693464 - }, - { - "x": 6.342829267680752, - "y": 4.221487838378473, - "heading": -0.02435617244589905, - "angularVelocity": 0.006269868868651565, - "velocityX": -3.4269570198438615, - "velocityY": 0.17475670805416932, - "timestamp": 3.3423391948038645 - }, - { - "x": 6.088597301309428, - "y": 4.246171426870893, - "heading": -0.02318863403361665, - "angularVelocity": 0.01668899526605478, - "velocityX": -3.6340355388877676, - "velocityY": 0.3528314676122184, - "timestamp": 3.412297774914265 - }, - { - "x": 5.834918091687161, - "y": 4.289953926308307, - "heading": -0.021332710171534618, - "angularVelocity": 0.02652889551436317, - "velocityX": -3.626134338660665, - "velocityY": 0.6258345919588656, - "timestamp": 3.4822563550246652 - }, - { - "x": 5.593079348347317, - "y": 4.348733091622046, - "heading": -0.01919697004887221, - "angularVelocity": 0.030528637363594544, - "velocityX": -3.4568846731623317, - "velocityY": 0.8401995183575885, - "timestamp": 3.5522149351350656 - }, - { - "x": 5.366458906870789, - "y": 4.419066666987696, - "heading": -0.01697675272885257, - "angularVelocity": 0.031736168980501495, - "velocityX": -3.239351643771207, - "velocityY": 1.005360246801131, - "timestamp": 3.622173515245466 - }, - { - "x": 5.1563191413879395, - "y": 4.4990692138671875, - "heading": -0.014762910019925463, - "angularVelocity": 0.03164504919101363, - "velocityX": -3.003774021016862, - "velocityY": 1.1435701918655279, - "timestamp": 3.6921320953558663 - }, - { - "x": 4.95401886311352, - "y": 4.5927689377457765, - "heading": -0.01249503476608761, - "angularVelocity": 0.03078994837779946, - "velocityX": -2.7465422158220725, - "velocityY": 1.2721200852443764, - "timestamp": 3.765788446643324 - }, - { - "x": 4.772461025085408, - "y": 4.690756196606082, - "heading": -0.010343440097087077, - "angularVelocity": 0.02921125783985097, - "velocityX": -2.4649311954043354, - "velocityY": 1.3303300685896478, - "timestamp": 3.8394447979307813 - }, - { - "x": 4.612079283555691, - "y": 4.78829172244435, - "heading": -0.008348351574694893, - "angularVelocity": 0.027086442479427285, - "velocityX": -2.177432614110885, - "velocityY": 1.3241970873308493, - "timestamp": 3.913101149218239 - }, - { - "x": 4.472451682258585, - "y": 4.881596852952091, - "heading": -0.006537499869867372, - "angularVelocity": 0.024585139952973775, - "velocityX": -1.8956627481068924, - "velocityY": 1.2667628639871322, - "timestamp": 3.9867575005056963 - }, - { - "x": 4.352795824787486, - "y": 4.967847931994665, - "heading": -0.004929288926669055, - "angularVelocity": 0.021833975143867725, - "velocityX": -1.6245151352138218, - "velocityY": 1.1709930988295092, - "timestamp": 4.060413851793154 - }, - { - "x": 4.252252080841926, - "y": 5.0449690027725635, - "heading": -0.0035358640735745368, - "angularVelocity": 0.018917918533005222, - "velocityX": -1.3650383461593192, - "velocityY": 1.0470389780362597, - "timestamp": 4.134070203080611 - }, - { - "x": 4.170005928828097, - "y": 5.111421057915132, - "heading": -0.0023653291529936566, - "angularVelocity": 0.015891839605422012, - "velocityX": -1.1166199598029207, - "velocityY": 0.9021904286736582, - "timestamp": 4.207726554368069 - }, - { - "x": 4.105328367937727, - "y": 5.166042041203556, - "heading": -0.00142318887082745, - "angularVelocity": 0.012791025698372453, - "velocityX": -0.8780988979206226, - "velocityY": 0.7415651513235484, - "timestamp": 4.281382905655526 - }, - { - "x": 4.057581454405702, - "y": 5.207935779542024, - "heading": -0.0007132554579850938, - "angularVelocity": 0.009638454803058576, - "velocityX": -0.6482389189451429, - "velocityY": 0.56877292461815, - "timestamp": 4.355039256942984 - }, - { - "x": 4.026210974061915, - "y": 5.236396648720956, - "heading": -0.00023821729765836225, - "angularVelocity": 0.006449384907389903, - "velocityX": -0.42590326286131003, - "velocityY": 0.3864007472737587, - "timestamp": 4.428695608230441 - }, - { - "x": 4.010735511779785, - "y": 5.250858306884766, - "heading": -1.8831642522301752e-27, - "angularVelocity": 0.0032341718466161605, - "velocityX": -0.2101035689608668, - "velocityY": 0.1963395947672006, - "timestamp": 4.502351959517899 - }, - { - "x": 4.010735511779785, - "y": 5.250858306884766, - "heading": -9.309999607017892e-28, - "angularVelocity": 2.912789507524277e-28, - "velocityX": -3.6885695211923144e-25, - "velocityY": -3.7751755858346917e-25, - "timestamp": 4.576008310805356 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.1.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.1.traj index 96a07c42..27f41867 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.1.traj @@ -1,310 +1,311 @@ { - "samples": [ - { - "x": 1.289974451065063, - "y": 5.590954303741456, - "heading": 6.19779206460361e-17, - "angularVelocity": 3.0320144312848013e-18, - "velocityX": 2.9896394667646305e-17, - "velocityY": -6.392586992550233e-17, - "timestamp": 0 - }, - { - "x": 1.3085927366636296, - "y": 5.570883327334207, - "heading": -0.01041389258493421, - "angularVelocity": -0.12226255374555442, - "velocityX": 0.21858484952476373, - "velocityY": -0.23563992154741434, - "timestamp": 0.08517646872162599 - }, - { - "x": 1.3442657042493709, - "y": 5.532508491008622, - "heading": -0.03089006103804589, - "angularVelocity": -0.24039701058788923, - "velocityX": 0.4188124739278457, - "velocityY": -0.45053330927585317, - "timestamp": 0.17035293744325197 - }, - { - "x": 1.3955881762749254, - "y": 5.477434538773948, - "heading": -0.060869932572704226, - "angularVelocity": -0.35197363760921707, - "velocityX": 0.6025428477586634, - "velocityY": -0.646586469963527, - "timestamp": 0.25552940616487796 - }, - { - "x": 1.4613425796478687, - "y": 5.4070770106059785, - "heading": -0.09976067247727309, - "angularVelocity": -0.45659018844361554, - "velocityX": 0.7719785095557636, - "velocityY": -0.826020721731408, - "timestamp": 0.34070587488650395 - }, - { - "x": 1.5403946295377764, - "y": 5.322786826751108, - "heading": -0.14691970563793563, - "angularVelocity": -0.5536626942681541, - "velocityX": 0.9280972911458275, - "velocityY": -0.9895947216401798, - "timestamp": 0.42588234360812993 - }, - { - "x": 1.6316218404845053, - "y": 5.225944550707253, - "heading": -0.20162745404412188, - "angularVelocity": -0.6422871155293178, - "velocityX": 1.0710377210503823, - "velocityY": -1.1369604480828668, - "timestamp": 0.5110588123297559 - }, - { - "x": 1.733844381505324, - "y": 5.118072938931053, - "heading": -0.2630340547325837, - "angularVelocity": -0.7209338636607621, - "velocityX": 1.2001265438099284, - "velocityY": -1.2664485085516666, - "timestamp": 0.5962352810513819 - }, - { - "x": 1.8457280635804503, - "y": 5.001046382736992, - "heading": -0.33003861392476214, - "angularVelocity": -0.7866557536115174, - "velocityX": 1.3135515448613582, - "velocityY": -1.373930593160977, - "timestamp": 0.6814117497730079 - }, - { - "x": 1.9656142072747882, - "y": 4.877758551128575, - "heading": -0.40085023097520717, - "angularVelocity": -0.8313518758551945, - "velocityX": 1.4075030990795228, - "velocityY": -1.447440043697368, - "timestamp": 0.7665882184946339 - }, - { - "x": 2.0856915448639635, - "y": 4.7606487505271025, - "heading": -0.4673031995363478, - "angularVelocity": -0.7801798966134944, - "velocityX": 1.4097477788332962, - "velocityY": -1.3749079101202415, - "timestamp": 0.8517646872162599 - }, - { - "x": 2.1993786689736172, - "y": 4.652829385838518, - "heading": -0.5273883646655675, - "angularVelocity": -0.7054197718103367, - "velocityX": 1.3347245526367961, - "velocityY": -1.2658351104101313, - "timestamp": 0.9369411559378859 - }, - { - "x": 2.3039823973737983, - "y": 4.5566291473122265, - "heading": -0.5809972368261334, - "angularVelocity": -0.6293859438546427, - "velocityX": 1.2280824736001708, - "velocityY": -1.1294227146313731, - "timestamp": 1.0221176246595118 - }, - { - "x": 2.3979860520770147, - "y": 4.47341696133058, - "heading": -0.6272322006251169, - "angularVelocity": -0.5428138134029581, - "velocityX": 1.1036340918339758, - "velocityY": -0.9769386689838181, - "timestamp": 1.107294093381138 - }, - { - "x": 2.480103614094797, - "y": 4.40444635319333, - "heading": -0.6652761118925774, - "angularVelocity": -0.4466481393094119, - "velocityX": 0.964087420507652, - "velocityY": -0.8097378204614404, - "timestamp": 1.192470562102764 - }, - { - "x": 2.549135601344952, - "y": 4.350942377101982, - "heading": -0.6944118607427208, - "angularVelocity": -0.3420633572561569, - "velocityX": 0.810458431609397, - "velocityY": -0.6281544292028495, - "timestamp": 1.2776470308243901 - }, - { - "x": 2.6038790433453562, - "y": 4.3141752600275325, - "heading": -0.7139618385994653, - "angularVelocity": -0.2295232257237371, - "velocityX": 0.6427061701667566, - "velocityY": -0.431658151908278, - "timestamp": 1.3628234995460162 - }, - { - "x": 2.6430389310079985, - "y": 4.2955467477546305, - "heading": -0.7232599969808831, - "angularVelocity": -0.1091634640525668, - "velocityX": 0.459750072412904, - "velocityY": -0.2187049140741477, - "timestamp": 1.4479999682676423 - }, - { - "x": 2.665171623229982, - "y": 4.296644687652586, - "heading": -0.721655115096118, - "angularVelocity": 0.018841845745096026, - "velocityX": 0.2598451491845503, - "velocityY": 0.012890178642452559, - "timestamp": 1.5331764369892684 - }, - { - "x": 2.66906909991143, - "y": 4.318945956778202, - "heading": -0.7091344221786624, - "angularVelocity": 0.14601587711521094, - "velocityX": 0.045452234946537245, - "velocityY": 0.2600766102664744, - "timestamp": 1.6189252826957972 - }, - { - "x": 2.6560642905413947, - "y": 4.360643314893585, - "heading": -0.6858248901313277, - "angularVelocity": 0.2718349367303499, - "velocityX": -0.15166162602986216, - "velocityY": 0.4862731127377579, - "timestamp": 1.704674128402326 - }, - { - "x": 2.627632992233998, - "y": 4.419910426276983, - "heading": -0.652529270737317, - "angularVelocity": 0.38829233349639936, - "velocityX": -0.331564793358298, - "velocityY": 0.6911709527407293, - "timestamp": 1.7904229741088546 - }, - { - "x": 2.5850645171946978, - "y": 4.495119603214681, - "heading": -0.6100207421436514, - "angularVelocity": 0.4957329541105318, - "velocityX": -0.4964320474352394, - "velocityY": 0.877086756305711, - "timestamp": 1.8761718198153834 - }, - { - "x": 2.5295631514165375, - "y": 4.584706937933874, - "heading": -0.5590704924389158, - "angularVelocity": 0.5941800065637087, - "velocityX": -0.6472549609368695, - "velocityY": 1.0447643228435226, - "timestamp": 1.9619206655219121 - }, - { - "x": 2.46234032691493, - "y": 4.687033037499603, - "heading": -0.5004982071284244, - "angularVelocity": 0.6830679157006081, - "velocityX": -0.7839501972035283, - "velocityY": 1.1933233470679585, - "timestamp": 2.047669511228441 - }, - { - "x": 2.384701887418879, - "y": 4.800204071292841, - "heading": -0.43526625936464175, - "angularVelocity": 0.7607326632364927, - "velocityX": -0.9054167301769336, - "velocityY": 1.319796585723857, - "timestamp": 2.1334183569349694 - }, - { - "x": 2.298178863290851, - "y": 4.921641059920068, - "heading": -0.36471802922759106, - "angularVelocity": 0.8227309598836902, - "velocityX": -1.0090284413174404, - "velocityY": 1.416193858082253, - "timestamp": 2.219167202641498 - }, - { - "x": 2.2047510720717436, - "y": 5.044849013310237, - "heading": -0.2923595970689714, - "angularVelocity": 0.8438414717122018, - "velocityX": -1.0895515904535924, - "velocityY": 1.4368467863910614, - "timestamp": 2.3049160483480264 - }, - { - "x": 2.116347360692902, - "y": 5.158956328540576, - "heading": -0.22539524544258233, - "angularVelocity": 0.7809358956920709, - "velocityX": -1.0309609493916598, - "velocityY": 1.3307154666649113, - "timestamp": 2.390664894054555 - }, - { - "x": 2.0362102002706437, - "y": 5.261312637562149, - "heading": -0.16556699187625293, - "angularVelocity": 0.6977149729932085, - "velocityX": -0.9345567250727145, - "velocityY": 1.1936756486715054, - "timestamp": 2.4764137397610835 - }, - { - "x": 1.9661275657868833, - "y": 5.350222584355773, - "heading": -0.1134489428170574, - "angularVelocity": 0.6077988412528272, - "velocityX": -0.8173011998740509, - "velocityY": 1.0368646488597075, - "timestamp": 2.562162585467612 - }, - { - "x": 1.9075019379268083, - "y": 5.42424212633875, - "heading": -0.06991588076878923, - "angularVelocity": 0.5076810269523403, - "velocityX": -0.6836899946235904, - "velocityY": 0.8632132756201296, - "timestamp": 2.6479114311741405 - }, - { - "x": 1.861590779787906, - "y": 5.48200360252104, - "heading": -0.03588037308734902, - "angularVelocity": 0.3969208844854328, - "velocityX": -0.5354142992901785, - "velocityY": 0.673612288379681, - "timestamp": 2.733660276880669 - }, - { - "x": 1.8296344375134035, - "y": 5.522099910983847, - "heading": -0.012259641477970167, - "angularVelocity": 0.2754641349951199, - "velocityX": -0.3726737311878495, - "velocityY": 0.4676017284248481, - "timestamp": 2.8194091225871976 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -3.4787101362106846e-24, + "angularVelocity": -4.3859089659641935e-24, + "velocityX": -2.4789748003093715e-23, + "velocityY": -3.607487096592192e-23, + "timestamp": 0 + }, + { + "x": 1.3060567681339126, + "y": 5.57238191847303, + "heading": -0.010695335590330916, + "angularVelocity": -0.1421295228209153, + "velocityX": 0.21371672085882062, + "velocityY": -0.24680705280850238, + "timestamp": 0.07525062617572531 + }, + { + "x": 1.3382215482089261, + "y": 5.535237409911883, + "heading": -0.032077507438502996, + "angularVelocity": -0.28414609864163015, + "velocityX": 0.4274353810678254, + "velocityY": -0.49361062424128055, + "timestamp": 0.15050125235145062 + }, + { + "x": 1.3864692379478656, + "y": 5.479521035128849, + "heading": -0.06412446300016969, + "angularVelocity": -0.4258696198332041, + "velocityX": 0.6411599768787516, + "velocityY": -0.7404107794787639, + "timestamp": 0.22575187852717593 + }, + { + "x": 1.450800607934819, + "y": 5.405233129379991, + "heading": -0.10680310960887816, + "angularVelocity": -0.5671533750303323, + "velocityX": 0.8548948129245705, + "velocityY": -0.9872064795232551, + "timestamp": 0.30100250470290124 + }, + { + "x": 1.531216769618125, + "y": 5.3123742117306145, + "heading": -0.1600730202690718, + "angularVelocity": -0.7078998988765586, + "velocityX": 1.0686444189250723, + "velocityY": -1.2339952817473157, + "timestamp": 0.37625313087862655 + }, + { + "x": 1.627719192058266, + "y": 5.200945113429446, + "heading": -0.2238912344076857, + "angularVelocity": -0.8480755228479505, + "velocityX": 1.282413547161571, + "velocityY": -1.4807730375686, + "timestamp": 0.45150375705435186 + }, + { + "x": 1.7403097840465742, + "y": 5.070947169326127, + "heading": -0.2982176307237913, + "angularVelocity": -0.9877179778217201, + "velocityX": 1.4962080411847536, + "velocityY": -1.7275330546718466, + "timestamp": 0.5267543832300772 + }, + { + "x": 1.8689915581726206, + "y": 4.922382882779654, + "heading": -0.3830195540733457, + "angularVelocity": -1.126926481003957, + "velocityX": 1.7100425692877124, + "velocityY": -1.974259804822693, + "timestamp": 0.6020050094058025 + }, + { + "x": 2.021815508579126, + "y": 4.778590260197623, + "heading": -0.46245355485112494, + "angularVelocity": -1.055592555366715, + "velocityX": 2.0308661624905433, + "velocityY": -1.9108495156737524, + "timestamp": 0.6772556355815278 + }, + { + "x": 2.1585523272850695, + "y": 4.6533669600969905, + "heading": -0.5314334765942994, + "angularVelocity": -0.9166690730531941, + "velocityX": 1.8170854603473379, + "velocityY": -1.664083164015284, + "timestamp": 0.7525062617572531 + }, + { + "x": 2.2791997774513058, + "y": 4.5467108563615435, + "heading": -0.5899636375558711, + "angularVelocity": -0.7778029756841082, + "velocityX": 1.6032750330143433, + "velocityY": -1.4173450661577678, + "timestamp": 0.8277568879329784 + }, + { + "x": 2.3837568433333827, + "y": 4.45862086228965, + "heading": -0.6380444530356357, + "angularVelocity": -0.6389423972032644, + "velocityX": 1.389451107528536, + "velocityY": -1.1706214093977847, + "timestamp": 0.9030075141087037 + }, + { + "x": 2.4722229118459578, + "y": 4.389096268153827, + "heading": -0.6756747726303448, + "angularVelocity": -0.500066531098822, + "velocityX": 1.1756190347969888, + "velocityY": -0.923907184153793, + "timestamp": 0.978258140284429 + }, + { + "x": 2.544597592286723, + "y": 4.338136593499442, + "heading": -0.702852872614281, + "angularVelocity": -0.361167758530936, + "velocityX": 0.9617817700514026, + "velocityY": -0.677199343635805, + "timestamp": 1.0535087664601543 + }, + { + "x": 2.600880652205466, + "y": 4.305741528586687, + "heading": -0.7195768736441226, + "angularVelocity": -0.22224401150879278, + "velocityX": 0.7479414162921453, + "velocityY": -0.4304956192272341, + "timestamp": 1.1287593926358797 + }, + { + "x": 2.6410719901577853, + "y": 4.291910905991578, + "heading": -0.7258448979650717, + "angularVelocity": -0.08329531114215386, + "velocityX": 0.534099714445754, + "velocityY": -0.18379411970354914, + "timestamp": 1.204010018811605 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05567771435110341, + "velocityX": 0.32025823965793626, + "velocityY": 0.06290687402329305, + "timestamp": 1.2792606449873303 + }, + { + "x": 2.6731116725964643, + "y": 4.320290742325012, + "heading": -0.7067971816671581, + "angularVelocity": 0.19582509769912612, + "velocityX": 0.10464853341562398, + "velocityY": 0.31165107776032225, + "timestamp": 1.3551341341546281 + }, + { + "x": 2.664692784388126, + "y": 4.362810214221228, + "heading": -0.6813099320330065, + "angularVelocity": 0.33591772190617664, + "velocityX": -0.11095954991308038, + "velocityY": 0.5603995857164566, + "timestamp": 1.431007623321926 + }, + { + "x": 2.63991517535616, + "y": 4.424203564318272, + "heading": -0.645197478734488, + "angularVelocity": 0.47595614350741217, + "velocityX": -0.3265647764969981, + "velocityY": 0.8091541692734667, + "timestamp": 1.5068811124892239 + }, + { + "x": 2.598779165089059, + "y": 4.504471398642189, + "heading": -0.5984632230032911, + "angularVelocity": 0.6159497374392491, + "velocityX": -0.5421657909576058, + "velocityY": 1.0579167401531877, + "timestamp": 1.5827546016565217 + }, + { + "x": 2.5412851916310535, + "y": 4.603614465429005, + "heading": -0.5411083104388861, + "angularVelocity": 0.7559282325601216, + "velocityX": -0.7577610320679146, + "velocityY": 1.3066891726596395, + "timestamp": 1.6586280908238196 + }, + { + "x": 2.4674338175988146, + "y": 4.721633607594926, + "heading": -0.473128621994749, + "angularVelocity": 0.8959610160308396, + "velocityX": -0.9733488579838422, + "velocityY": 1.5554727146618088, + "timestamp": 1.7345015799911174 + }, + { + "x": 2.3772256430083227, + "y": 4.858529577863132, + "heading": -0.3945096550364948, + "angularVelocity": 1.036184941816799, + "velocityX": -1.1889287757886868, + "velocityY": 1.8042661774306572, + "timestamp": 1.8103750691584153 + }, + { + "x": 2.2706605414206735, + "y": 5.014302199754286, + "heading": -0.3052190611328218, + "angularVelocity": 1.1768352145607928, + "velocityX": -1.4045103600373166, + "velocityY": 2.053057314231072, + "timestamp": 1.8862485583257131 + }, + { + "x": 2.1562301513945004, + "y": 5.146478418893045, + "heading": -0.22909327294912277, + "angularVelocity": 1.0033252591803818, + "velocityX": -1.508173556825084, + "velocityY": 1.7420606405396215, + "timestamp": 1.962122047493011 + }, + { + "x": 2.058146995665746, + "y": 5.259770783530285, + "heading": -0.16373665420264177, + "angularVelocity": 0.8613893925765344, + "velocityX": -1.2927197207510102, + "velocityY": 1.4931745709945456, + "timestamp": 2.037995536660309 + }, + { + "x": 1.9764115735788519, + "y": 5.354180517364029, + "heading": -0.1092008927472175, + "angularVelocity": 0.718772288633981, + "velocityX": -1.0772593034000484, + "velocityY": 1.2443046295864166, + "timestamp": 2.113869025827607 + }, + { + "x": 1.9110235321882938, + "y": 5.429708042242192, + "heading": -0.06553152876247478, + "angularVelocity": 0.5755549726789759, + "velocityX": -0.8618035378125303, + "velocityY": 0.9954402480638209, + "timestamp": 2.189742514994905 + }, + { + "x": 1.8619825299142096, + "y": 5.48635360523882, + "heading": -0.03276339486876071, + "angularVelocity": 0.43187856856644224, + "velocityX": -0.6463522741909358, + "velocityY": 0.7465791229361657, + "timestamp": 2.2656160041622027 + }, + { + "x": 1.8292883999681575, + "y": 5.524117342026143, + "heading": -0.010917644112626662, + "angularVelocity": 0.28792337080959957, + "velocityX": -0.430903208813331, + "velocityY": 0.4977197859459911, + "timestamp": 2.3414894933295005 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.2049864598182203e-23, + "angularVelocity": 0.14389273819414988, + "velocityX": -0.21545346822636996, + "velocityY": 0.24886064631017277, + "timestamp": 2.4173629824967984 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 5.735401641845927e-24, + "angularVelocity": -6.2110589016170234e-24, + "velocityX": -6.266702545665033e-23, + "velocityY": -1.0096243291514256e-22, + "timestamp": 2.4932364716640962 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.2.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.2.traj index 4ff3aa49..ee3d96ef 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.2.traj @@ -1,94 +1,95 @@ { - "samples": [ - { - "x": 1.8296344375134035, - "y": 5.522099910983847, - "heading": -0.012259641477970167, - "angularVelocity": 0.2754641349951199, - "velocityX": -0.3726737311878495, - "velocityY": 0.4676017284248481, - "timestamp": 0 - }, - { - "x": 1.8129411935806854, - "y": 5.542999267578127, - "heading": 2.3632850028431835e-17, - "angularVelocity": 0.14297150447865262, - "velocityX": -0.1946760191950624, - "velocityY": 0.24372755600475027, - "timestamp": 0.08574884570652852 - }, - { - "x": 1.812941193580624, - "y": 5.542999267578126, - "heading": -4.920156603511903e-17, - "angularVelocity": -5.733397110540358e-17, - "velocityX": -7.214388151371406e-13, - "velocityY": -9.03683027618772e-16, - "timestamp": 0.17149769141305704 - }, - { - "x": 1.8513325137310372, - "y": 5.5430599450572595, - "heading": -0.000016850565291482023, - "angularVelocity": -0.00016991086246254008, - "velocityX": 0.3871147468977978, - "velocityY": 0.0006118348336243464, - "timestamp": 0.2706706639181613 - }, - { - "x": 1.9275402570059652, - "y": 5.543180391388335, - "heading": -0.000015746696122309923, - "angularVelocity": 0.00001113074602288469, - "velocityX": 0.7684325814778618, - "velocityY": 0.0012145076227106227, - "timestamp": 0.36984363642326556 - }, - { - "x": 2.0338963277323163, - "y": 5.54334848714078, - "heading": 0.000009170336978677823, - "angularVelocity": 0.00025124822289482134, - "velocityX": 1.0724299982122594, - "velocityY": 0.0016949754373461064, - "timestamp": 0.4690166089283698 - }, - { - "x": 2.163383514243291, - "y": 5.5435531416216355, - "heading": 0.000033654463962368084, - "angularVelocity": 0.0002468830606297171, - "velocityX": 1.305670115961409, - "velocityY": 0.00206361144256778, - "timestamp": 0.5681895814334741 - }, - { - "x": 2.3079360710490797, - "y": 5.54378160701675, - "heading": 0.00003317773344428256, - "angularVelocity": -0.000004807060894986283, - "velocityX": 1.457580156714077, - "velocityY": 0.0023037062351123753, - "timestamp": 0.6673625539385784 - }, - { - "x": 2.437398077457075, - "y": 5.543986221690917, - "heading": 0.000022146124274066103, - "angularVelocity": -0.00011123604437218979, - "velocityX": 1.3054162151016944, - "velocityY": 0.002063210056106694, - "timestamp": 0.7665355264436826 - }, - { - "x": 2.5437229473484053, - "y": 5.544154268122903, - "heading": 0.000015406432122086, - "angularVelocity": -0.00006795896080977589, - "velocityX": 1.0721153879486738, - "velocityY": 0.0016944781198031775, - "timestamp": 0.8657084989487869 - } - ] + "samples": [ + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 5.735401641845927e-24, + "angularVelocity": -6.2110589016170234e-24, + "velocityX": -6.266702545665033e-23, + "velocityY": -1.0096243291514256e-22, + "timestamp": 0 + }, + { + "x": 1.8552093934543321, + "y": 5.543066072465799, + "heading": -4.219694925044242e-18, + "angularVelocity": -4.2870553829964666e-17, + "velocityX": 0.42942883486562483, + "velocityY": 0.0006787122508870844, + "timestamp": 0.09842888143953177 + }, + { + "x": 1.939745792565551, + "y": 5.543199682240142, + "heading": -1.2239290131185836e-17, + "angularVelocity": -8.147603720153091e-17, + "velocityX": 0.858857663267793, + "velocityY": 0.0013574244915586756, + "timestamp": 0.19685776287906354 + }, + { + "x": 2.066550389641902, + "y": 5.543400096899142, + "heading": -5.060219765528842e-18, + "angularVelocity": 7.293662450422726e-17, + "velocityX": 1.288286478743048, + "velocityY": 0.002036136711799281, + "timestamp": 0.2952866443185953 + }, + { + "x": 2.2356231808662415, + "y": 5.543667316436768, + "heading": 1.924608375875081e-20, + "angularVelocity": 5.160544115984206e-17, + "velocityX": 1.7177152554375656, + "velocityY": 0.0027148488707469335, + "timestamp": 0.39371552575812707 + }, + { + "x": 2.4046959720905807, + "y": 5.543934535974393, + "heading": 5.109055925882335e-18, + "angularVelocity": 5.171053219292816e-17, + "velocityX": 1.7177152554375659, + "velocityY": 0.0027148488707469335, + "timestamp": 0.49214440719765884 + }, + { + "x": 2.531500569166932, + "y": 5.544134950633393, + "heading": 1.2272248404707649e-17, + "angularVelocity": 7.277531121284907e-17, + "velocityX": 1.288286478743048, + "velocityY": 0.002036136711799281, + "timestamp": 0.5905732886371906 + }, + { + "x": 2.616036968278151, + "y": 5.544268560407736, + "heading": 4.225188349913728e-18, + "angularVelocity": -8.175506961814134e-17, + "velocityX": 0.858857663267793, + "velocityY": 0.0013574244915586754, + "timestamp": 0.6890021700767224 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -1.1775900898457124e-26, + "angularVelocity": -4.2926306787465456e-17, + "velocityX": 0.42942883486562483, + "velocityY": 0.0006787122508870843, + "timestamp": 0.7874310515162541 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -1.2588122005060392e-26, + "angularVelocity": -5.516653068340682e-27, + "velocityX": 1.9209951977484535e-26, + "velocityY": 6.96713079819041e-27, + "timestamp": 0.8858599329557859 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.3.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.3.traj index 16980d6c..0f0c9f5d 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.3.traj @@ -1,193 +1,194 @@ { - "samples": [ - { - "x": 2.5437229473484053, - "y": 5.544154268122903, - "heading": 0.000015406432122086, - "angularVelocity": -0.00006795896080977589, - "velocityX": 1.0721153879486738, - "velocityY": 0.0016944781198031775, - "timestamp": 0 - }, - { - "x": 2.619913870706316, - "y": 5.544274687859312, - "heading": 0.000009213432317016853, - "angularVelocity": -0.00006244644733859944, - "velocityX": 0.7682629796539725, - "velocityY": 0.0012142394582550931, - "timestamp": 0.09917297250510426 - }, - { - "x": 2.6583051681519314, - "y": 5.5443353652954785, - "heading": 6.370510606485668e-14, - "angularVelocity": -0.00009290265301587805, - "velocityX": 0.38711451795641516, - "velocityY": 0.0006118344003786568, - "timestamp": 0.19834594501020852 - }, - { - "x": 2.658305168152, - "y": 5.544335365295488, - "heading": 6.625668653050138e-14, - "angularVelocity": 2.6906565131836242e-14, - "velocityX": 7.155954565396242e-13, - "velocityY": 8.671180221391495e-14, - "timestamp": 0.2975189175153128 - }, - { - "x": 2.6398879819889713, - "y": 5.568034006444528, - "heading": 0.008388499707182374, - "angularVelocity": 0.0943787022324955, - "velocityX": -0.20721108536177554, - "velocityY": 0.26663254150593735, - "timestamp": 0.3864001959035779 - }, - { - "x": 2.6055876283476542, - "y": 5.613232266775393, - "heading": 0.025296828917652628, - "angularVelocity": 0.19023499118162804, - "velocityX": -0.3859120195327329, - "velocityY": 0.5085239675934979, - "timestamp": 0.475281474291843 - }, - { - "x": 2.5578825329127857, - "y": 5.677978135359711, - "heading": 0.05006181260458975, - "angularVelocity": 0.27862992225075944, - "velocityX": -0.5367282773164671, - "velocityY": 0.7284533903912082, - "timestamp": 0.5641627526801081 - }, - { - "x": 2.4993543830957403, - "y": 5.760558609750369, - "heading": 0.0821470752651059, - "angularVelocity": 0.36099011222914734, - "velocityX": -0.6584980648176972, - "velocityY": 0.9291098855468343, - "timestamp": 0.6530440310683732 - }, - { - "x": 2.433098392034208, - "y": 5.859358208334035, - "heading": 0.12114426250239245, - "angularVelocity": 0.43875592188117873, - "velocityX": -0.7454437229429913, - "velocityY": 1.1115906563809108, - "timestamp": 0.7419253094566383 - }, - { - "x": 2.3637465533640976, - "y": 5.972668871410117, - "heading": 0.16678728284828395, - "angularVelocity": 0.5135279461940396, - "velocityX": -0.7802749907275444, - "velocityY": 1.2748541102325668, - "timestamp": 0.8308065878449034 - }, - { - "x": 2.300988842467506, - "y": 6.095835737634532, - "heading": 0.21776795840799054, - "angularVelocity": 0.5735817090404455, - "velocityX": -0.7060847012414084, - "velocityY": 1.385745889998316, - "timestamp": 0.9196878662331684 - }, - { - "x": 2.2515752134889366, - "y": 6.217231027383832, - "heading": 0.2695393710632505, - "angularVelocity": 0.582478263071738, - "velocityX": -0.5559509254902292, - "velocityY": 1.3658139481175915, - "timestamp": 1.0085691446214335 - }, - { - "x": 2.217045589242532, - "y": 6.33145065782542, - "heading": 0.3198059590268783, - "angularVelocity": 0.5655475357136119, - "velocityX": -0.3884915346931451, - "velocityY": 1.2850808686903799, - "timestamp": 1.0974504230096986 - }, - { - "x": 2.19907919431313, - "y": 6.435243176299548, - "heading": 0.36734143796630997, - "angularVelocity": 0.5348199283510537, - "velocityX": -0.20213924974232386, - "velocityY": 1.1677658147606698, - "timestamp": 1.1863317013979637 - }, - { - "x": 2.1993959276283994, - "y": 6.526053318679287, - "heading": 0.41157070153690833, - "angularVelocity": 0.4976218206191788, - "velocityX": 0.0035635549001480525, - "velocityY": 1.021701577951703, - "timestamp": 1.2752129797862288 - }, - { - "x": 2.219770279567901, - "y": 6.601608618777817, - "heading": 0.45152771224881855, - "angularVelocity": 0.4495548605556116, - "velocityX": 0.2292310856552352, - "velocityY": 0.8500699075051399, - "timestamp": 1.364094258174494 - }, - { - "x": 2.2626490848637184, - "y": 6.659247151615926, - "heading": 0.48436572022362784, - "angularVelocity": 0.36945922212429394, - "velocityX": 0.4824278641490618, - "velocityY": 0.648489016846146, - "timestamp": 1.452975536562759 - }, - { - "x": 2.329372405994089, - "y": 6.697374820688125, - "heading": 0.5125504195998649, - "angularVelocity": 0.31710501792120865, - "velocityX": 0.7507016363871187, - "velocityY": 0.4289730049288785, - "timestamp": 1.5418568149510241 - }, - { - "x": 2.399251093648798, - "y": 6.73730555904706, - "heading": 0.5293913369356736, - "angularVelocity": 0.21745079806450165, - "velocityX": 0.902277239279105, - "velocityY": 0.5155877648973247, - "timestamp": 1.6193038396838517 - }, - { - "x": 2.4725481079431484, - "y": 6.779189629834486, - "heading": 0.507422235287016, - "angularVelocity": -0.28366617987293774, - "velocityX": 0.9464148501358945, - "velocityY": 0.5408092945202858, - "timestamp": 1.6967508644166793 - }, - { - "x": 2.537702213604298, - "y": 6.816420603018375, - "heading": 0.5016725044650684, - "angularVelocity": -0.07424082257084566, - "velocityX": 0.8412731915467285, - "velocityY": 0.48072825693890747, - "timestamp": 1.774197889149507 - } - ] + "samples": [ + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -1.2588122005060392e-26, + "angularVelocity": -5.516653068340682e-27, + "velocityX": 1.9209951977484535e-26, + "velocityY": 6.96713079819041e-27, + "timestamp": 0 + }, + { + "x": 2.6438894427168456, + "y": 5.563919137233213, + "heading": 0.003139326829545763, + "angularVelocity": 0.042039393016227077, + "velocityX": -0.19304404417939905, + "velocityY": 0.2622504536593843, + "timestamp": 0.0746758362646669 + }, + { + "x": 2.6152671402107748, + "y": 5.603236779531591, + "heading": 0.009643531295019244, + "angularVelocity": 0.08709918483431768, + "velocityX": -0.3832873381508234, + "velocityY": 0.5265109072100428, + "timestamp": 0.1493516725293338 + }, + { + "x": 2.5727492585947744, + "y": 5.6625051004279126, + "heading": 0.019846282811438364, + "angularVelocity": 0.13662721472925177, + "velocityX": -0.5693659923044985, + "velocityY": 0.7936746859621688, + "timestamp": 0.22402750879400068 + }, + { + "x": 2.516846273489969, + "y": 5.742064038671315, + "heading": 0.03428875746248491, + "angularVelocity": 0.19340224861840632, + "velocityX": -0.7486087588851783, + "velocityY": 1.0653906567772256, + "timestamp": 0.2987033450586676 + }, + { + "x": 2.4485464886328767, + "y": 5.842518783448963, + "heading": 0.05399413536855069, + "angularVelocity": 0.26387890503463146, + "velocityX": -0.9146169400101983, + "velocityY": 1.3452108446648592, + "timestamp": 0.3733791813233345 + }, + { + "x": 2.3705191792336286, + "y": 5.965194001491249, + "heading": 0.08158339935984246, + "angularVelocity": 0.3694536997685508, + "velocityX": -1.0448802892906721, + "velocityY": 1.6427699263721678, + "timestamp": 0.44805501758800137 + }, + { + "x": 2.300330937558203, + "y": 6.110432887604612, + "heading": 0.1284815107612587, + "angularVelocity": 0.6280225806270092, + "velocityX": -0.9399056667629848, + "velocityY": 1.9449248026979609, + "timestamp": 0.5227308538526683 + }, + { + "x": 2.251762063243615, + "y": 6.244623434670601, + "heading": 0.1817545952842417, + "angularVelocity": 0.7133912010596283, + "velocityX": -0.6503961220126102, + "velocityY": 1.796974145564157, + "timestamp": 0.5974066901173352 + }, + { + "x": 2.2219257093836906, + "y": 6.363297360569798, + "heading": 0.2370564478928837, + "angularVelocity": 0.7405588658242863, + "velocityX": -0.3995449579456786, + "velocityY": 1.5891877725827055, + "timestamp": 0.6720825263820025 + }, + { + "x": 2.209746331851008, + "y": 6.465234411576823, + "heading": 0.2929790595103366, + "angularVelocity": 0.7488715816887712, + "velocityX": -0.16309663395688523, + "velocityY": 1.3650607225306177, + "timestamp": 0.7467583626466698 + }, + { + "x": 2.214679002088127, + "y": 6.5498771941878156, + "heading": 0.3488347523758202, + "angularVelocity": 0.7479754584537774, + "velocityX": 0.06605443586378532, + "velocityY": 1.133469497562776, + "timestamp": 0.8214341989113372 + }, + { + "x": 2.236396070259457, + "y": 6.616907899287828, + "heading": 0.40421575749752775, + "angularVelocity": 0.7416188139550987, + "velocityX": 0.2908178770755253, + "velocityY": 0.897622423168334, + "timestamp": 0.8961100351760045 + }, + { + "x": 2.274679156572898, + "y": 6.666121488597866, + "heading": 0.45885178755752026, + "angularVelocity": 0.7316426945170152, + "velocityX": 0.5126569480622607, + "velocityY": 0.6590296375873849, + "timestamp": 0.9707858714406719 + }, + { + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.7190897983674445, + "velocityX": 0.7324089313056569, + "velocityY": 0.41852001496969765, + "timestamp": 1.0454617077053392 + }, + { + "x": 2.411093271023251, + "y": 6.744072527671164, + "heading": 0.5643142977465416, + "angularVelocity": 0.6537087450996224, + "velocityX": 1.032025536567027, + "velocityY": 0.5897297596346315, + "timestamp": 1.124646633002083 + }, + { + "x": 2.50599376720386, + "y": 6.798301463699205, + "heading": 0.5436087675323019, + "angularVelocity": -0.26148323227743453, + "velocityX": 1.1984667008899996, + "velocityY": 0.6848391385711283, + "timestamp": 1.2038315582988268 + }, + { + "x": 2.5771691415097524, + "y": 6.838973166960486, + "heading": 0.5280796125632321, + "angularVelocity": -0.19611251650329578, + "velocityX": 0.898850053077196, + "velocityY": 0.5136293695910533, + "timestamp": 1.2830164835955706 + }, + { + "x": 2.6246193915553895, + "y": 6.866087636091841, + "heading": 0.5177268190541022, + "angularVelocity": -0.13074197481822433, + "velocityX": 0.5992333751382385, + "velocityY": 0.34241958339600737, + "timestamp": 1.3622014088923144 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06537102150066688, + "velocityX": 0.29961668979104056, + "velocityY": 0.1712097929676749, + "timestamp": 1.4413863341890583 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 1.4429207692433446e-21, + "velocityX": 9.55884619679387e-19, + "velocityY": -1.6623744143332693e-18, + "timestamp": 1.520571259485802 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.4.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.4.traj index 81537aca..47801079 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.4.traj @@ -1,508 +1,509 @@ { - "samples": [ - { - "x": 2.537702213604298, - "y": 6.816420603018375, - "heading": 0.5016725044650684, - "angularVelocity": -0.07424082257084566, - "velocityX": 0.8412731915467285, - "velocityY": 0.48072825693890747, - "timestamp": 0 - }, - { - "x": 2.5904059035391898, - "y": 6.846537042294488, - "heading": 0.5036538467311155, - "angularVelocity": 0.025583194098508056, - "velocityX": 0.680512777970975, - "velocityY": 0.38886502613926416, - "timestamp": 0.07744702473282761 - }, - { - "x": 2.6280877308244723, - "y": 6.868069547224279, - "heading": 0.508503710225543, - "angularVelocity": 0.06262168896947905, - "velocityX": 0.4865497083575037, - "velocityY": 0.27802882066934376, - "timestamp": 0.15489404946565521 - }, - { - "x": 2.648344516781087, - "y": 6.8796448707953655, - "heading": 0.5125504196001363, - "angularVelocity": 0.052251321320124576, - "velocityX": 0.26155667100350394, - "velocityY": 0.14946117855453953, - "timestamp": 0.23234107419848282 - }, - { - "x": 2.6483445167657806, - "y": 6.879644870779064, - "heading": 0.5125504196000689, - "angularVelocity": 1.8669291195039983e-14, - "velocityX": -4.749030905139852e-11, - "velocityY": 6.098431731550748e-11, - "timestamp": 0.30978809893131043 - }, - { - "x": 2.6747339306487334, - "y": 6.883481123686636, - "heading": 0.5098224624623802, - "angularVelocity": -0.03296702980330207, - "velocityX": 0.3189128531205414, - "velocityY": 0.046360649216292196, - "timestamp": 0.39253614004891446 - }, - { - "x": 2.7264961186821273, - "y": 6.89099146776212, - "heading": 0.5039121473834008, - "angularVelocity": -0.07142543798202369, - "velocityX": 0.6255397388782451, - "velocityY": 0.09076159355636307, - "timestamp": 0.4752841811665185 - }, - { - "x": 2.8018050306182753, - "y": 6.9018959167453104, - "heading": 0.4949465844489951, - "angularVelocity": -0.10834773625231511, - "velocityX": 0.9100990297657242, - "velocityY": 0.13177893803804336, - "timestamp": 0.5580322222841225 - }, - { - "x": 2.899101313212433, - "y": 6.915952840560763, - "heading": 0.4831136364117761, - "angularVelocity": -0.14299973603485935, - "velocityX": 1.1758137265856894, - "velocityY": 0.16987621248329202, - "timestamp": 0.6407802634017266 - }, - { - "x": 3.017081432389269, - "y": 6.932956858771751, - "heading": 0.4685947114251132, - "angularVelocity": -0.1754594403754914, - "velocityX": 1.42577537284728, - "velocityY": 0.20549148936151598, - "timestamp": 0.7235283045193306 - }, - { - "x": 3.1546294358748312, - "y": 6.9527283369160715, - "heading": 0.4515640044783447, - "angularVelocity": -0.20581401948312003, - "velocityX": 1.6622508717768, - "velocityY": 0.23893590563959213, - "timestamp": 0.8062763456369346 - }, - { - "x": 3.3107722570719647, - "y": 6.975106161489216, - "heading": 0.43219034104896137, - "angularVelocity": -0.2341283632545328, - "velocityX": 1.8869669793779915, - "velocityY": 0.2704332848357982, - "timestamp": 0.8890243867545387 - }, - { - "x": 3.484600968601402, - "y": 6.999935624537231, - "heading": 0.4106444392061959, - "angularVelocity": -0.2603795999490034, - "velocityX": 2.1006988102882644, - "velocityY": 0.3000610372485155, - "timestamp": 0.9717724278721427 - }, - { - "x": 3.675234170530874, - "y": 7.027062068947035, - "heading": 0.38710417033209454, - "angularVelocity": -0.28448128265230255, - "velocityX": 2.3037790303523367, - "velocityY": 0.32781977728352046, - "timestamp": 1.0545204689897467 - }, - { - "x": 3.8817881153060045, - "y": 7.056325014551516, - "heading": 0.36176128472936936, - "angularVelocity": -0.3062656862983269, - "velocityX": 2.4961792688429623, - "velocityY": 0.3536391340419632, - "timestamp": 1.1372685101073507 - }, - { - "x": 4.1033458782194, - "y": 7.087551279488626, - "heading": 0.3348309137177563, - "angularVelocity": -0.32545025414364576, - "velocityX": 2.677498583906162, - "velocityY": 0.3773656090873216, - "timestamp": 1.2200165512249548 - }, - { - "x": 4.338922324182521, - "y": 7.120545790816314, - "heading": 0.30656596895157695, - "angularVelocity": -0.34157841544560996, - "velocityX": 2.846912661392294, - "velocityY": 0.39873465138360303, - "timestamp": 1.3027645923425588 - }, - { - "x": 4.587417768628414, - "y": 7.155077045432509, - "heading": 0.27728164973070885, - "angularVelocity": -0.3538974315929522, - "velocityX": 3.00303718480442, - "velocityY": 0.4173060068834074, - "timestamp": 1.3855126334601628 - }, - { - "x": 4.847543704393879, - "y": 7.190849002148663, - "heading": 0.2474070078170501, - "angularVelocity": -0.36103140944690143, - "velocityX": 3.1435902560613274, - "velocityY": 0.4322997406709641, - "timestamp": 1.4682606745777669 - }, - { - "x": 5.117670126803491, - "y": 7.227429623630717, - "heading": 0.21762399464747098, - "angularVelocity": -0.35992408723308145, - "velocityX": 3.264444919314729, - "velocityY": 0.4420723558891399, - "timestamp": 1.551008715695371 - }, - { - "x": 5.395335267694769, - "y": 7.263944271068559, - "heading": 0.1895241870207491, - "angularVelocity": -0.33958275322536735, - "velocityX": 3.3555494141142153, - "velocityY": 0.44127506759904395, - "timestamp": 1.633756756812975 - }, - { - "x": 5.667910261992729, - "y": 7.2967916361655485, - "heading": 0.1669091076503975, - "angularVelocity": -0.27330048016738406, - "velocityX": 3.294035612402776, - "velocityY": 0.3969564071048189, - "timestamp": 1.716504797930579 - }, - { - "x": 5.931901746285159, - "y": 7.327414779180602, - "heading": 0.14778373205342693, - "angularVelocity": -0.23112783503586568, - "velocityX": 3.1903049392702143, - "velocityY": 0.3700769541063461, - "timestamp": 1.799252839048183 - }, - { - "x": 6.1801220482371395, - "y": 7.354810947360831, - "heading": 0.11632647881372146, - "angularVelocity": -0.38015707459464126, - "velocityX": 2.999712121271843, - "velocityY": 0.33107935620263895, - "timestamp": 1.882000880165787 - }, - { - "x": 6.415697256681246, - "y": 7.379780463537999, - "heading": 0.08899977607154176, - "angularVelocity": -0.3302398748429845, - "velocityX": 2.84689770612573, - "velocityY": 0.3017535622589267, - "timestamp": 1.964748921283391 - }, - { - "x": 6.637987945996846, - "y": 7.402299511961184, - "heading": 0.06474721752358573, - "angularVelocity": -0.2930892166194916, - "velocityX": 2.686355910222349, - "velocityY": 0.2721399578655483, - "timestamp": 2.047496962400995 - }, - { - "x": 6.845920803268571, - "y": 7.422253782487123, - "heading": 0.04362794173118966, - "angularVelocity": -0.2552238760840361, - "velocityX": 2.5128432584428544, - "velocityY": 0.24114492931132497, - "timestamp": 2.130245003518599 - }, - { - "x": 7.038548371158306, - "y": 7.439534256583214, - "heading": 0.025735796286898687, - "angularVelocity": -0.2162243988212622, - "velocityX": 2.3278807001118484, - "velocityY": 0.2088324250667052, - "timestamp": 2.212993044636203 - }, - { - "x": 7.2149794023417675, - "y": 7.454031322663086, - "heading": 0.011159229205506762, - "angularVelocity": -0.17615603807074215, - "velocityX": 2.1321475264013814, - "velocityY": 0.1751952781490545, - "timestamp": 2.295741085753807 - }, - { - "x": 7.374384880065902, - "y": 7.465639114379918, - "heading": -1.1165566205788637e-16, - "angularVelocity": -0.134857925997875, - "velocityX": 1.9263957861864134, - "velocityY": 0.14027874932231896, - "timestamp": 2.3784891268714112 - }, - { - "x": 7.500924464216497, - "y": 7.473646968954779, - "heading": -0.0070937662662868915, - "angularVelocity": -0.09816609550664941, - "velocityX": 1.7511003938959169, - "velocityY": 0.11081557912801578, - "timestamp": 2.450752022015349 - }, - { - "x": 7.613899532464792, - "y": 7.47952275118133, - "heading": -0.01171150501248939, - "angularVelocity": -0.06390193386252535, - "velocityX": 1.5633897316635108, - "velocityY": 0.08131119317636289, - "timestamp": 2.523014917159287 - }, - { - "x": 7.712046212988902, - "y": 7.483209835153203, - "heading": -0.014057088311073107, - "angularVelocity": -0.032459027470621904, - "velocityX": 1.3581891554251948, - "velocityY": 0.05102319751389575, - "timestamp": 2.595277812303226 - }, - { - "x": 7.79365194754084, - "y": 7.484595849351514, - "heading": -0.014345050898318633, - "angularVelocity": -0.003984930117620908, - "velocityX": 1.129289580626482, - "velocityY": 0.019180164253482204, - "timestamp": 2.6675407074471638 - }, - { - "x": 7.856538953365895, - "y": 7.483519387813377, - "heading": -0.012796446625875068, - "angularVelocity": 0.02143014432730538, - "velocityX": 0.8702530627897808, - "velocityY": -0.0148964629220964, - "timestamp": 2.7398036025911017 - }, - { - "x": 7.899249386445817, - "y": 7.479924646329156, - "heading": -0.009762276595150929, - "angularVelocity": 0.04198793896480952, - "velocityX": 0.5910423737502203, - "velocityY": -0.04974532887286408, - "timestamp": 2.8120664977350396 - }, - { - "x": 7.921731862807926, - "y": 7.4739282664160145, - "heading": -0.005439516570392937, - "angularVelocity": 0.0598199119499377, - "velocityX": 0.31112061476836195, - "velocityY": -0.08298006744976984, - "timestamp": 2.8843293928789775 - }, - { - "x": 7.923961639404313, - "y": 7.465639114379864, - "heading": -8.923477228591696e-17, - "angularVelocity": 0.07527399171536123, - "velocityX": 0.030856452567537763, - "velocityY": -0.11470827482980998, - "timestamp": 2.9565922880229154 - }, - { - "x": 7.888861809774048, - "y": 7.450123409205229, - "heading": 0.009559512590155543, - "angularVelocity": 0.09629823646465466, - "velocityX": -0.3535799196587898, - "velocityY": -0.15629824551531185, - "timestamp": 3.0558621494566127 - }, - { - "x": 7.817152523236329, - "y": 7.430666435185926, - "heading": 0.02163779335227315, - "angularVelocity": 0.12167117579976462, - "velocityX": -0.72236714650414, - "velocityY": -0.19600081775371833, - "timestamp": 3.15513201089031 - }, - { - "x": 7.711448594725456, - "y": 7.4075722780062225, - "heading": 0.036035719670363044, - "angularVelocity": 0.14503824333134957, - "velocityX": -1.064813902067085, - "velocityY": -0.23264016737978235, - "timestamp": 3.2544018723240073 - }, - { - "x": 7.574048351882708, - "y": 7.3811128444264495, - "heading": 0.05252610728763101, - "angularVelocity": 0.16611675869298972, - "velocityX": -1.3841083371967766, - "velocityY": -0.2665404504210516, - "timestamp": 3.3536717337577047 - }, - { - "x": 7.407019371926434, - "y": 7.351538272778396, - "heading": 0.07089974064841498, - "angularVelocity": 0.18508773050978647, - "velocityX": -1.682574928019174, - "velocityY": -0.2979209522500118, - "timestamp": 3.452941595191402 - }, - { - "x": 7.212241390572204, - "y": 7.319083353098022, - "heading": 0.09094586321763254, - "angularVelocity": 0.20193563564714437, - "velocityX": -1.96210590546985, - "velocityY": -0.3269362847056093, - "timestamp": 3.5522114566250993 - }, - { - "x": 6.990210935369373, - "y": 7.283848196853213, - "heading": 0.11243058039021528, - "angularVelocity": 0.216427391579795, - "velocityX": -2.236635087388812, - "velocityY": -0.35494313919580667, - "timestamp": 3.6514813180587966 - }, - { - "x": 6.744250267253016, - "y": 7.2462331722899656, - "heading": 0.1352094476478159, - "angularVelocity": 0.2294640783075409, - "velocityX": -2.4776973047417434, - "velocityY": -0.3789168637892277, - "timestamp": 3.750751179492494 - }, - { - "x": 6.474537469267562, - "y": 7.206343597573894, - "heading": 0.1591116816121769, - "angularVelocity": 0.24078036998495714, - "velocityX": -2.7169655934857664, - "velocityY": -0.401829660482739, - "timestamp": 3.8500210409261912 - }, - { - "x": 6.185093329086759, - "y": 7.164701733979632, - "heading": 0.18384126922195682, - "angularVelocity": 0.24911475902781344, - "velocityX": -2.915730273020737, - "velocityY": -0.4194814316536095, - "timestamp": 3.9492909023598886 - }, - { - "x": 5.8780260788239564, - "y": 7.121745615538567, - "heading": 0.2121770427035368, - "angularVelocity": 0.28544185589002363, - "velocityX": -3.0932575690950848, - "velocityY": -0.43272064472210836, - "timestamp": 4.048560763793586 - }, - { - "x": 5.557966335758099, - "y": 7.0784922097584015, - "heading": 0.24028820463072503, - "angularVelocity": 0.283179219968629, - "velocityX": -3.224138106404308, - "velocityY": -0.4357153838585266, - "timestamp": 4.147830625227283 - }, - { - "x": 5.245908943265439, - "y": 7.038535334063528, - "heading": 0.26569993334648234, - "angularVelocity": 0.25598634216619676, - "velocityX": -3.1435260207458486, - "velocityY": -0.40250762031696863, - "timestamp": 4.2471004866609805 - }, - { - "x": 4.948601311707987, - "y": 7.0011016986080925, - "heading": 0.2891526852975207, - "angularVelocity": 0.23625249005411966, - "velocityX": -2.9949435534976283, - "velocityY": -0.37708963138260865, - "timestamp": 4.346370348094678 - }, - { - "x": 4.669544920179785, - "y": 6.966326786089845, - "heading": 0.31063199175620987, - "angularVelocity": 0.21637288647809105, - "velocityX": -2.811088758440401, - "velocityY": -0.35030685059909655, - "timestamp": 4.445640209528375 - }, - { - "x": 4.411446504016625, - "y": 6.934401644375056, - "heading": 0.3300269329519647, - "angularVelocity": 0.19537592694947853, - "velocityX": -2.599967527259491, - "velocityY": -0.3215995394142002, - "timestamp": 4.5449100709620724 - }, - { - "x": 4.176717541970206, - "y": 6.9055345106659285, - "heading": 0.3471896751796373, - "angularVelocity": 0.17288975707028345, - "velocityX": -2.364554142177353, - "velocityY": -0.2907945401778102, - "timestamp": 4.64417993239577 - }, - { - "x": 3.9677270788540704, - "y": 6.879953869869358, - "heading": 0.3619526945831296, - "angularVelocity": 0.14871602710307644, - "velocityX": -2.1052760636290624, - "velocityY": -0.2576878866065069, - "timestamp": 4.743449793829467 - } - ] + "samples": [ + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 1.4429207692433446e-21, + "velocityX": 9.55884619679387e-19, + "velocityY": -1.6623744143332693e-18, + "timestamp": 0 + }, + { + "x": 2.67095907816642, + "y": 6.882744787327794, + "heading": 0.5061644140699368, + "angularVelocity": -0.08818113694961922, + "velocityX": 0.3122730989760289, + "velocityY": 0.04280518804462273, + "timestamp": 0.07241917887395477 + }, + { + "x": 2.716191596050434, + "y": 6.888945496159438, + "heading": 0.4936002658801072, + "angularVelocity": -0.17349200011916, + "velocityX": 0.6245930786199807, + "velocityY": 0.08562246808178013, + "timestamp": 0.14483835774790954 + }, + { + "x": 2.784046041436307, + "y": 6.898248047061875, + "heading": 0.4751107385388315, + "angularVelocity": -0.2553125791919922, + "velocityX": 0.9369678922205519, + "velocityY": 0.12845424440158493, + "timestamp": 0.2172575366218643 + }, + { + "x": 2.8745270921290036, + "y": 6.910653685112006, + "heading": 0.4510077220563394, + "angularVelocity": -0.33282642605549656, + "velocityX": 1.249407299276042, + "velocityY": 0.17130321336180326, + "timestamp": 0.2896767154958191 + }, + { + "x": 2.9876402967876543, + "y": 6.926163879564403, + "heading": 0.4216838617313518, + "angularVelocity": -0.4049184315666644, + "velocityX": 1.5619233249733904, + "velocityY": 0.21417247051907112, + "timestamp": 0.36209589436977385 + }, + { + "x": 3.1233922684020508, + "y": 6.944780361828343, + "heading": 0.3876471304183033, + "angularVelocity": -0.4699960955410605, + "velocityX": 1.8745306661191443, + "velocityY": 0.2570656358357964, + "timestamp": 0.4345150732437286 + }, + { + "x": 3.281790863806145, + "y": 6.966505170067396, + "heading": 0.34957966674470664, + "angularVelocity": -0.5256544504578377, + "velocityX": 2.1872464983314113, + "velocityY": 0.2999869451276964, + "timestamp": 0.5069342521176834 + }, + { + "x": 3.4628451453091853, + "y": 6.9913406807024, + "heading": 0.3084466318231206, + "angularVelocity": -0.5679853812368929, + "velocityX": 2.5000874674119666, + "velocityY": 0.34294106921911044, + "timestamp": 0.5793534309916382 + }, + { + "x": 3.6665642046771225, + "y": 7.019289526069962, + "heading": 0.26572282825309695, + "angularVelocity": -0.5899515050341022, + "velocityX": 2.813053980113594, + "velocityY": 0.3859315419221543, + "timestamp": 0.6517726098655929 + }, + { + "x": 3.89294996939766, + "y": 7.050353833764572, + "heading": 0.2239583223824598, + "angularVelocity": -0.576705045818428, + "velocityX": 3.126047108523004, + "velocityY": 0.42895139350692046, + "timestamp": 0.7241917887395477 + }, + { + "x": 4.141945383394743, + "y": 7.084529171934603, + "heading": 0.18873600956210052, + "angularVelocity": -0.48636719399516315, + "velocityX": 3.4382523782885963, + "velocityY": 0.4719100478826658, + "timestamp": 0.7966109676135025 + }, + { + "x": 4.412342843336488, + "y": 7.12169996051531, + "heading": 0.1851319702648993, + "angularVelocity": -0.049766365115434155, + "velocityX": 3.73378246130586, + "velocityY": 0.5132727153038038, + "timestamp": 0.8690301464874572 + }, + { + "x": 4.683688666421851, + "y": 7.1589163386875585, + "heading": 0.18513196457429595, + "angularVelocity": -7.857867829794732e-8, + "velocityX": 3.7468779307431594, + "velocityY": 0.5139022390328953, + "timestamp": 0.941449325361412 + }, + { + "x": 4.955034489702315, + "y": 7.196132715437329, + "heading": 0.18513195888369474, + "angularVelocity": -7.857864847824122e-8, + "velocityX": 3.7468779334372075, + "velocityY": 0.513902219390594, + "timestamp": 1.0138685042353668 + }, + { + "x": 5.226380313001078, + "y": 7.233349092053675, + "heading": 0.18513195319309358, + "angularVelocity": -7.857864765970384e-8, + "velocityX": 3.7468779336898987, + "velocityY": 0.5139022175482143, + "timestamp": 1.0862876831093216 + }, + { + "x": 5.497726136146309, + "y": 7.270565469789434, + "heading": 0.1851319475024924, + "angularVelocity": -7.857864817392824e-8, + "velocityX": 3.7468779315698426, + "velocityY": 0.5139022330056134, + "timestamp": 1.1587068619832763 + }, + { + "x": 5.769071959711075, + "y": 7.307781844466341, + "heading": 0.18513194181189122, + "angularVelocity": -7.85786487043621e-8, + "velocityX": 3.7468779373630063, + "velocityY": 0.5139021907674625, + "timestamp": 1.231126040857231 + }, + { + "x": 6.040417785833277, + "y": 7.3449982004969, + "heading": 0.18513193612128995, + "angularVelocity": -7.857864920895207e-8, + "velocityX": 3.746877972677337, + "velocityY": 0.5139019332894442, + "timestamp": 1.3035452197311859 + }, + { + "x": 6.311959616349093, + "y": 7.380756398600711, + "heading": 0.18513193002169934, + "angularVelocity": -8.42261776360047e-8, + "velocityX": 3.7495844987200435, + "velocityY": 0.4937669642188043, + "timestamp": 1.3759643986051406 + }, + { + "x": 6.569581422501917, + "y": 7.404082737536551, + "heading": 0.1318031871794107, + "angularVelocity": -0.7363897750774955, + "velocityX": 3.557369886797705, + "velocityY": 0.3221016766351085, + "timestamp": 1.4483835774790954 + }, + { + "x": 6.804636460196215, + "y": 7.424191087588351, + "heading": 0.0864226666877527, + "angularVelocity": -0.6266367721545483, + "velocityX": 3.245756736670675, + "velocityY": 0.2776660874158541, + "timestamp": 1.5208027563530502 + }, + { + "x": 7.017122434869165, + "y": 7.4411416449585746, + "heading": 0.04927300576152058, + "angularVelocity": -0.5129809741545235, + "velocityX": 2.934111902080261, + "velocityY": 0.23406171726589448, + "timestamp": 1.593221935227005 + }, + { + "x": 7.207038680431131, + "y": 7.454954351507805, + "heading": 0.020449488415145097, + "angularVelocity": -0.398009447145798, + "velocityX": 2.6224578697932155, + "velocityY": 0.19073271423404176, + "timestamp": 1.6656411141009597 + }, + { + "x": 7.374384880065918, + "y": 7.465639114379883, + "heading": 6.042705221121846e-21, + "angularVelocity": -0.28237669541568916, + "velocityX": 2.31079946275077, + "velocityY": 0.14754051396626502, + "timestamp": 1.7380602929749145 + }, + { + "x": 7.495692822755469, + "y": 7.4722697459987435, + "heading": -0.0110806824345206, + "angularVelocity": -0.18789382846574634, + "velocityX": 2.0570045130283727, + "velocityY": 0.11243484030663832, + "timestamp": 1.7970333977596766 + }, + { + "x": 7.601999179168005, + "y": 7.476911827606212, + "heading": -0.01772644697635795, + "angularVelocity": -0.11269144750124323, + "velocityX": 1.8026243793764762, + "velocityY": 0.07871523170454242, + "timestamp": 1.8560065025444388 + }, + { + "x": 7.693282254298601, + "y": 7.479626196867593, + "heading": -0.020765685795326113, + "angularVelocity": -0.05153601510486283, + "velocityX": 1.5478763660783506, + "velocityY": 0.046027240235815677, + "timestamp": 1.914979607329201 + }, + { + "x": 7.769527501259725, + "y": 7.480459824413743, + "heading": -0.02082974288304602, + "angularVelocity": -0.0010862085005308956, + "velocityX": 1.292881682919731, + "velocityY": 0.014135724228753763, + "timestamp": 1.973952712113963 + }, + { + "x": 7.8307246815334155, + "y": 7.479450038620926, + "heading": -0.018416174457310776, + "angularVelocity": 0.040926595853214634, + "velocityX": 1.0377133864164767, + "velocityY": -0.0171228188935137, + "timestamp": 2.032925816898725 + }, + { + "x": 7.876866310893982, + "y": 7.476627203358953, + "heading": -0.013927394073716237, + "angularVelocity": 0.07611572088628399, + "velocityX": 0.7824181807787326, + "velocityY": -0.047866485447466633, + "timestamp": 2.0918989216834873 + }, + { + "x": 7.907946749436215, + "y": 7.472016493094702, + "heading": -0.0076956641130537864, + "angularVelocity": 0.10567071181696776, + "velocityX": 0.5270273399318175, + "velocityY": -0.07818327152825819, + "timestamp": 2.1508720264682495 + }, + { + "x": 7.923961639404297, + "y": 7.465639114379883, + "heading": -3.0160599173944472e-21, + "angularVelocity": 0.13049447101591694, + "velocityX": 0.27156260513215996, + "velocityY": -0.10814046060649564, + "timestamp": 2.2098451312530116 + }, + { + "x": 7.911903030236992, + "y": 7.451294465353008, + "heading": 0.015126055505390899, + "angularVelocity": 0.1631760399586222, + "velocityX": -0.1300852089712522, + "velocityY": -0.1547464255943911, + "timestamp": 2.3025429042178818 + }, + { + "x": 7.862610962703899, + "y": 7.432637923876487, + "heading": 0.03315483702979202, + "angularVelocity": 0.19448991003520194, + "velocityX": -0.531750288669542, + "velocityY": -0.20126202474780283, + "timestamp": 2.395240677182752 + }, + { + "x": 7.776083443224882, + "y": 7.4096801426109025, + "heading": 0.05392541642359817, + "angularVelocity": 0.22406772816082293, + "velocityX": -0.9334368746033223, + "velocityY": -0.2476627057079841, + "timestamp": 2.487938450147622 + }, + { + "x": 7.652317922263172, + "y": 7.3824351254025355, + "heading": 0.07722667884981763, + "angularVelocity": 0.25136809311535513, + "velocityX": -1.3351509642913992, + "velocityY": -0.2939123167359359, + "timestamp": 2.580636223112492 + }, + { + "x": 7.49131102887938, + "y": 7.3509221102471285, + "heading": 0.10276916611506416, + "angularVelocity": 0.2755458566941678, + "velocityX": -1.736901418816265, + "velocityY": -0.3399543931583946, + "timestamp": 2.6733339960773623 + }, + { + "x": 7.293058112506846, + "y": 7.315169193574846, + "heading": 0.13013085609030867, + "angularVelocity": 0.29517095287298756, + "velocityX": -2.138702042471583, + "velocityY": -0.3856933724376746, + "timestamp": 2.7660317690422325 + }, + { + "x": 7.057552405050112, + "y": 7.2752213128681245, + "heading": 0.15863793525795974, + "angularVelocity": 0.30752712018717576, + "velocityX": -2.5405756786194313, + "velocityY": -0.4309475775848565, + "timestamp": 2.8587295420071026 + }, + { + "x": 6.784783507993426, + "y": 7.231161947944703, + "heading": 0.18704197948975648, + "angularVelocity": 0.30641560550285446, + "velocityX": -2.9425614913106823, + "velocityY": -0.47530122368871824, + "timestamp": 2.9514273149719727 + }, + { + "x": 6.474737606760309, + "y": 7.183200115705696, + "heading": 0.2122345768516852, + "angularVelocity": 0.2717713334005995, + "velocityX": -3.3446963321396703, + "velocityY": -0.5174000486201892, + "timestamp": 3.044125087936843 + }, + { + "x": 6.12780389314354, + "y": 7.132776434801359, + "heading": 0.21223458876196108, + "angularVelocity": 1.284850268489834e-7, + "velocityX": -3.74263267088681, + "velocityY": -0.5439578459284719, + "timestamp": 3.136822860901712 + }, + { + "x": 5.779638248368889, + "y": 7.091712595403529, + "heading": 0.21223459040404852, + "angularVelocity": 1.7714421734571762e-8, + "velocityX": -3.755922430915314, + "velocityY": -0.442986256135747, + "timestamp": 3.2295206338665823 + }, + { + "x": 5.431472594680733, + "y": 7.050648831580209, + "heading": 0.21223459204613604, + "angularVelocity": 1.77144224789278e-8, + "velocityX": -3.7559225270719514, + "velocityY": -0.4429854408571672, + "timestamp": 3.3222184068314524 + }, + { + "x": 5.083306940982422, + "y": 7.009585067843037, + "heading": 0.21223459368823233, + "angularVelocity": 1.771451744649189e-8, + "velocityX": -3.7559225271815024, + "velocityY": -0.4429854399278211, + "timestamp": 3.4149161797963226 + }, + { + "x": 4.748349644408739, + "y": 6.970085293939387, + "heading": 0.249149110447713, + "angularVelocity": 0.3982244187621458, + "velocityX": -3.6134341296486574, + "velocityY": -0.42611351535511016, + "timestamp": 3.5076139527611927 + }, + { + "x": 4.450609827039412, + "y": 6.934974591277922, + "heading": 0.28197417031428224, + "angularVelocity": 0.35410839782536235, + "velocityX": -3.2119414290800963, + "velocityY": -0.37876533101577264, + "timestamp": 3.600311725726063 + }, + { + "x": 4.190087491180257, + "y": 6.904252880091878, + "heading": 0.31070546232875873, + "angularVelocity": 0.3099458713572863, + "velocityX": -2.8104487036369967, + "velocityY": -0.3314180071800382, + "timestamp": 3.693009498690933 + }, + { + "x": 3.9667826341532617, + "y": 6.877920092869689, + "heading": 0.33533894531800695, + "angularVelocity": 0.2657397497411686, + "velocityX": -2.408956007083604, + "velocityY": -0.2840714116418766, + "timestamp": 3.785707271655803 + }, + { + "x": 3.780695252616961, + "y": 6.85597617430768, + "heading": 0.3558710467629364, + "angularVelocity": 0.22149508869766035, + "velocityX": -2.0074633465770906, + "velocityY": -0.23672541270570108, + "timestamp": 3.8784050446206733 + }, + { + "x": 3.6318253440600605, + "y": 6.8384210809873816, + "heading": 0.37229886580171084, + "angularVelocity": 0.17721913389440508, + "velocityX": -1.6059707131617713, + "velocityY": -0.18937988215695278, + "timestamp": 3.9711028175855434 + }, + { + "x": 3.5201729076832478, + "y": 6.82525478102506, + "heading": 0.38462033829713027, + "angularVelocity": 0.13292091170398426, + "velocityX": -1.2044780883692483, + "velocityY": -0.1420346955617933, + "timestamp": 4.0638005905504135 + }, + { + "x": 3.4457379449845735, + "y": 6.816477253761962, + "heading": 0.39283435937679306, + "angularVelocity": 0.08861077043108337, + "velocityX": -0.802985447416123, + "velocityY": -0.09468973182801316, + "timestamp": 4.156498363515284 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.044299923200563064, + "velocityX": -0.4014927615347733, + "velocityY": -0.04734487236445959, + "timestamp": 4.249196136480154 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": -1.6537958542474343e-22, + "velocityX": -4.688105571426993e-23, + "velocityY": -2.4487756057177513e-23, + "timestamp": 4.341893909445024 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.5.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.5.traj index 53f46346..0b452dc8 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.5.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.5.traj @@ -1,193 +1,194 @@ { - "samples": [ - { - "x": 3.9677270788540704, - "y": 6.879953869869358, - "heading": 0.3619526945831296, - "angularVelocity": 0.14871602710307644, - "velocityX": -2.1052760636290624, - "velocityY": -0.2576878866065069, - "timestamp": 0 - }, - { - "x": 3.7869910923072463, - "y": 6.857920125982094, - "heading": 0.37415675707125545, - "angularVelocity": 0.1229382444164791, - "velocityX": -1.8206531563211636, - "velocityY": -0.22195804012459744, - "timestamp": 0.09926986143369732 - }, - { - "x": 3.6375222511124736, - "y": 6.83976261955988, - "heading": 0.3836657545916857, - "angularVelocity": 0.09578937033957077, - "velocityX": -1.5056819767458316, - "velocityY": -0.18291056479757375, - "timestamp": 0.19853972286739463 - }, - { - "x": 3.5230866973435258, - "y": 6.825906628204701, - "heading": 0.39040681994356247, - "angularVelocity": 0.06790646480733925, - "velocityX": -1.1527723733691484, - "velocityY": -0.1395790339088257, - "timestamp": 0.29780958430109195 - }, - { - "x": 3.4467104911360567, - "y": 6.816686862116541, - "heading": 0.39477159352312596, - "angularVelocity": 0.04396876873328538, - "velocityX": -0.7693795992500979, - "velocityY": -0.09287578279051523, - "timestamp": 0.39707944573478926 - }, - { - "x": 3.4085204601287793, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": 0.021852252491790684, - "velocityX": -0.3847092204594867, - "velocityY": -0.046321940190690435, - "timestamp": 0.4963493071684866 - }, - { - "x": 3.408520460128781, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": -9.258859562419926e-18, - "velocityX": 7.880837628913899e-16, - "velocityY": -2.4194099006901583e-16, - "timestamp": 0.5956191686021839 - }, - { - "x": 3.4451800140480633, - "y": 6.811382414928076, - "heading": 0.39378662700224637, - "angularVelocity": -0.032542531675579224, - "velocityX": 0.37821978715244237, - "velocityY": -0.00728463273656313, - "timestamp": 0.6925457577563048 - }, - { - "x": 3.518496947445585, - "y": 6.809970351527808, - "heading": 0.38746820532810716, - "angularVelocity": -0.06518770266528738, - "velocityX": 0.756417140408642, - "velocityY": -0.014568380179178244, - "timestamp": 0.7894723469104257 - }, - { - "x": 3.6283397734394334, - "y": 6.8078551393922675, - "heading": 0.37750274880248313, - "angularVelocity": -0.1028144765290181, - "velocityX": 1.1332579321365746, - "velocityY": -0.021822826471036318, - "timestamp": 0.8863989360645466 - }, - { - "x": 3.7707535238798844, - "y": 6.805113681812527, - "heading": 0.36197971834397535, - "angularVelocity": -0.16015244726939834, - "velocityX": 1.469294975540766, - "velocityY": -0.028283854860319504, - "timestamp": 0.9833255252186675 - }, - { - "x": 3.9413941020263046, - "y": 6.801829282896055, - "heading": 0.341973429419134, - "angularVelocity": -0.20640661246244818, - "velocityX": 1.7605135973070385, - "velocityY": -0.03388542757086655, - "timestamp": 1.0802521143727883 - }, - { - "x": 4.137139185749371, - "y": 6.798061838763412, - "heading": 0.3182906699990001, - "angularVelocity": -0.24433707640817395, - "velocityX": 2.0195189517276724, - "velocityY": -0.038869046827304486, - "timestamp": 1.1771787035269092 - }, - { - "x": 4.3554217491364975, - "y": 6.793860650651487, - "heading": 0.2915337558008742, - "angularVelocity": -0.2760533970258723, - "velocityX": 2.2520400778783602, - "velocityY": -0.043344020960483176, - "timestamp": 1.2741052926810301 - }, - { - "x": 4.593770534553052, - "y": 6.7892732393783435, - "heading": 0.26218341148270125, - "angularVelocity": -0.30281003978695553, - "velocityX": 2.459065025362269, - "velocityY": -0.04732871870534226, - "timestamp": 1.371031881835151 - }, - { - "x": 4.849438377602218, - "y": 6.784352472846038, - "heading": 0.23067476418411942, - "angularVelocity": -0.32507743822988416, - "velocityX": 2.6377472402607576, - "velocityY": -0.05076797373402456, - "timestamp": 1.467958470989272 - }, - { - "x": 5.118739979240287, - "y": 6.77916928922838, - "heading": 0.197506319622179, - "angularVelocity": -0.3422017100921637, - "velocityX": 2.7784079063161986, - "velocityY": -0.053475353490631775, - "timestamp": 1.5648850601433928 - }, - { - "x": 5.388165163887155, - "y": 6.773984054928517, - "heading": 0.16458046580315022, - "angularVelocity": -0.33969888042458984, - "velocityX": 2.779682922902244, - "velocityY": -0.053496510556234986, - "timestamp": 1.6618116492975137 - }, - { - "x": 5.643920085833601, - "y": 6.769061952936388, - "heading": 0.1333147392066983, - "angularVelocity": -0.32257120434452946, - "velocityX": 2.6386456407723093, - "velocityY": -0.05078175178849002, - "timestamp": 1.7587382384516346 - }, - { - "x": 5.882330711464664, - "y": 6.764473684698037, - "heading": 0.10420435480854875, - "angularVelocity": -0.3003343525465651, - "velocityX": 2.4597030362017005, - "velocityY": -0.04733756008947892, - "timestamp": 1.8556648276057555 - }, - { - "x": 6.100655045491889, - "y": 6.760272005917349, - "heading": 0.07768097943341207, - "angularVelocity": -0.27364395679871373, - "velocityX": 2.252471029183505, - "velocityY": -0.04334908323253468, - "timestamp": 1.9525914167598764 - } - ] + "samples": [ + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": -1.6537958542474343e-22, + "velocityX": -4.688105571426993e-23, + "velocityY": -2.4487756057177513e-23, + "timestamp": 0 + }, + { + "x": 3.442803441142074, + "y": 6.8114279191425675, + "heading": 0.392498149134446, + "angularVelocity": -0.05010039849219718, + "velocityX": 0.38660846281780004, + "velocityY": -0.007449238528134429, + "timestamp": 0.08867623011513537 + }, + { + "x": 3.511369395408112, + "y": 6.810106800490753, + "heading": 0.38361321030304085, + "angularVelocity": -0.10019527014025122, + "velocityX": 0.773216838120132, + "velocityY": -0.01489822751935824, + "timestamp": 0.17735246023027074 + }, + { + "x": 3.6142183227587346, + "y": 6.808125168158371, + "heading": 0.3702881620154029, + "angularVelocity": -0.1502662919965921, + "velocityX": 1.1598252115260796, + "velocityY": -0.02234682653748938, + "timestamp": 0.2660286903454061 + }, + { + "x": 3.751350230137559, + "y": 6.805483069997971, + "heading": 0.3525266232817515, + "angularVelocity": -0.2002965023500672, + "velocityX": 1.5464336632350622, + "velocityY": -0.02979488592342862, + "timestamp": 0.3547049204605415 + }, + { + "x": 3.922765130885179, + "y": 6.8021805681387715, + "heading": 0.3303335458268625, + "angularVelocity": -0.2502708722063848, + "velocityX": 1.9330422653856434, + "velocityY": -0.03724224467945297, + "timestamp": 0.44338115057567684 + }, + { + "x": 4.128463043748926, + "y": 6.7982177401202675, + "heading": 0.30371498826898996, + "angularVelocity": -0.3001769191508424, + "velocityX": 2.3196510789494917, + "velocityY": -0.04468872902421391, + "timestamp": 0.5320573806908122 + }, + { + "x": 4.36844399152108, + "y": 6.793594680078811, + "heading": 0.2726778324022649, + "angularVelocity": -0.3500053602462237, + "velocityX": 2.7062601495413983, + "velocityY": -0.05213415179529137, + "timestamp": 0.6207336108059476 + }, + { + "x": 4.642707998965045, + "y": 6.788311499898704, + "heading": 0.23722944053291425, + "angularVelocity": -0.39975077676763104, + "velocityX": 3.0928694993896952, + "velocityY": -0.05957831284943316, + "timestamp": 0.709409840921083 + }, + { + "x": 4.951255087719671, + "y": 6.782368330248145, + "heading": 0.19737726949719422, + "angularVelocity": -0.4494121027018933, + "velocityX": 3.4794790932588646, + "velocityY": -0.06702100036100427, + "timestamp": 0.7980860710362183 + }, + { + "x": 5.286562691971156, + "y": 6.775920768512488, + "heading": 0.19737726761878174, + "angularVelocity": -2.1182818612377388e-8, + "velocityX": 3.781256869130857, + "velocityY": -0.0727090193988374, + "timestamp": 0.8867623011513537 + }, + { + "x": 5.59511113537339, + "y": 6.769985792808053, + "heading": 0.15789277163023122, + "angularVelocity": -0.44526583885314686, + "velocityX": 3.4794943695917215, + "velocityY": -0.06692859740122076, + "timestamp": 0.975438531266489 + }, + { + "x": 5.869376453129913, + "y": 6.764710551221358, + "heading": 0.12280116553958556, + "angularVelocity": -0.3957273109725502, + "velocityX": 3.092884275757129, + "velocityY": -0.05948878949687072, + "timestamp": 1.0641147613816244 + }, + { + "x": 6.1093586237611985, + "y": 6.760094935517236, + "heading": 0.0920991922571767, + "angularVelocity": -0.34622551322429906, + "velocityX": 2.7062739396983573, + "velocityY": -0.0520502021582253, + "timestamp": 1.1527909914967598 + }, + { + "x": 6.315057635592866, + "y": 6.756138851805421, + "heading": 0.06578475722782369, + "angularVelocity": -0.2967473357311971, + "velocityX": 2.3196634719878233, + "velocityY": -0.04461267361815281, + "timestamp": 1.2414672216118952 + }, + { + "x": 6.486473481982027, + "y": 6.752842221237321, + "heading": 0.04385663735915925, + "angularVelocity": -0.24728295102524525, + "velocityX": 1.9330529293656087, + "velocityY": -0.037176034252009674, + "timestamp": 1.3301434517270305 + }, + { + "x": 6.623606159513812, + "y": 6.750204980424608, + "heading": 0.026314217868262327, + "angularVelocity": -0.197825499213489, + "velocityX": 1.5464423482339522, + "velocityY": -0.02974010971473329, + "timestamp": 1.418819681842166 + }, + { + "x": 6.726455666874299, + "y": 6.748227081682736, + "heading": 0.013157277625411766, + "angularVelocity": -0.14837054107699252, + "velocityX": 1.1598317522852502, + "velocityY": -0.02230472291507881, + "timestamp": 1.5074959119573013 + }, + { + "x": 6.795022004077355, + "y": 6.7469084931623255, + "heading": 0.004385827059285361, + "angularVelocity": -0.09891546533651373, + "velocityX": 0.7732211564928945, + "velocityY": -0.014869695280220821, + "timestamp": 1.5961721420724366 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -2.734177125091434e-26, + "angularVelocity": -0.049458880396594275, + "velocityX": 0.38661057021351825, + "velocityY": -0.007434847510947748, + "timestamp": 1.684848372187572 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -1.3407189209020893e-26, + "angularVelocity": 6.223794221869343e-27, + "velocityX": 3.782126331053824e-27, + "velocityY": 9.545461702862487e-27, + "timestamp": 1.7735246023027074 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBADSprint.traj b/src/main/deploy/choreo/CenterLanePCBADSprint.traj index 6788eb8f..df968d2a 100644 --- a/src/main/deploy/choreo/CenterLanePCBADSprint.traj +++ b/src/main/deploy/choreo/CenterLanePCBADSprint.traj @@ -1,1309 +1,1247 @@ { - "samples": [ - { - "x": 1.289974451065063, - "y": 5.590954303741456, - "heading": 6.19779206460361e-17, - "angularVelocity": 3.0320144312848013e-18, - "velocityX": 2.9896394667646305e-17, - "velocityY": -6.392586992550233e-17, - "timestamp": 0 - }, - { - "x": 1.3085927366636296, - "y": 5.570883327334207, - "heading": -0.01041389258493421, - "angularVelocity": -0.12226255374555442, - "velocityX": 0.21858484952476373, - "velocityY": -0.23563992154741434, - "timestamp": 0.08517646872162599 - }, - { - "x": 1.3442657042493709, - "y": 5.532508491008622, - "heading": -0.03089006103804589, - "angularVelocity": -0.24039701058788923, - "velocityX": 0.4188124739278457, - "velocityY": -0.45053330927585317, - "timestamp": 0.17035293744325197 - }, - { - "x": 1.3955881762749254, - "y": 5.477434538773948, - "heading": -0.060869932572704226, - "angularVelocity": -0.35197363760921707, - "velocityX": 0.6025428477586634, - "velocityY": -0.646586469963527, - "timestamp": 0.25552940616487796 - }, - { - "x": 1.4613425796478687, - "y": 5.4070770106059785, - "heading": -0.09976067247727309, - "angularVelocity": -0.45659018844361554, - "velocityX": 0.7719785095557636, - "velocityY": -0.826020721731408, - "timestamp": 0.34070587488650395 - }, - { - "x": 1.5403946295377764, - "y": 5.322786826751108, - "heading": -0.14691970563793563, - "angularVelocity": -0.5536626942681541, - "velocityX": 0.9280972911458275, - "velocityY": -0.9895947216401798, - "timestamp": 0.42588234360812993 - }, - { - "x": 1.6316218404845053, - "y": 5.225944550707253, - "heading": -0.20162745404412188, - "angularVelocity": -0.6422871155293178, - "velocityX": 1.0710377210503823, - "velocityY": -1.1369604480828668, - "timestamp": 0.5110588123297559 - }, - { - "x": 1.733844381505324, - "y": 5.118072938931053, - "heading": -0.2630340547325837, - "angularVelocity": -0.7209338636607621, - "velocityX": 1.2001265438099284, - "velocityY": -1.2664485085516666, - "timestamp": 0.5962352810513819 - }, - { - "x": 1.8457280635804503, - "y": 5.001046382736992, - "heading": -0.33003861392476214, - "angularVelocity": -0.7866557536115174, - "velocityX": 1.3135515448613582, - "velocityY": -1.373930593160977, - "timestamp": 0.6814117497730079 - }, - { - "x": 1.9656142072747882, - "y": 4.877758551128575, - "heading": -0.40085023097520717, - "angularVelocity": -0.8313518758551945, - "velocityX": 1.4075030990795228, - "velocityY": -1.447440043697368, - "timestamp": 0.7665882184946339 - }, - { - "x": 2.0856915448639635, - "y": 4.7606487505271025, - "heading": -0.4673031995363478, - "angularVelocity": -0.7801798966134944, - "velocityX": 1.4097477788332962, - "velocityY": -1.3749079101202415, - "timestamp": 0.8517646872162599 - }, - { - "x": 2.1993786689736172, - "y": 4.652829385838518, - "heading": -0.5273883646655675, - "angularVelocity": -0.7054197718103367, - "velocityX": 1.3347245526367961, - "velocityY": -1.2658351104101313, - "timestamp": 0.9369411559378859 - }, - { - "x": 2.3039823973737983, - "y": 4.5566291473122265, - "heading": -0.5809972368261334, - "angularVelocity": -0.6293859438546427, - "velocityX": 1.2280824736001708, - "velocityY": -1.1294227146313731, - "timestamp": 1.0221176246595118 - }, - { - "x": 2.3979860520770147, - "y": 4.47341696133058, - "heading": -0.6272322006251169, - "angularVelocity": -0.5428138134029581, - "velocityX": 1.1036340918339758, - "velocityY": -0.9769386689838181, - "timestamp": 1.107294093381138 - }, - { - "x": 2.480103614094797, - "y": 4.40444635319333, - "heading": -0.6652761118925774, - "angularVelocity": -0.4466481393094119, - "velocityX": 0.964087420507652, - "velocityY": -0.8097378204614404, - "timestamp": 1.192470562102764 - }, - { - "x": 2.549135601344952, - "y": 4.350942377101982, - "heading": -0.6944118607427208, - "angularVelocity": -0.3420633572561569, - "velocityX": 0.810458431609397, - "velocityY": -0.6281544292028495, - "timestamp": 1.2776470308243901 - }, - { - "x": 2.6038790433453562, - "y": 4.3141752600275325, - "heading": -0.7139618385994653, - "angularVelocity": -0.2295232257237371, - "velocityX": 0.6427061701667566, - "velocityY": -0.431658151908278, - "timestamp": 1.3628234995460162 - }, - { - "x": 2.6430389310079985, - "y": 4.2955467477546305, - "heading": -0.7232599969808831, - "angularVelocity": -0.1091634640525668, - "velocityX": 0.459750072412904, - "velocityY": -0.2187049140741477, - "timestamp": 1.4479999682676423 - }, - { - "x": 2.665171623229982, - "y": 4.296644687652586, - "heading": -0.721655115096118, - "angularVelocity": 0.018841845745096026, - "velocityX": 0.2598451491845503, - "velocityY": 0.012890178642452559, - "timestamp": 1.5331764369892684 - }, - { - "x": 2.66906909991143, - "y": 4.318945956778202, - "heading": -0.7091344221786624, - "angularVelocity": 0.14601587711521094, - "velocityX": 0.045452234946537245, - "velocityY": 0.2600766102664744, - "timestamp": 1.6189252826957972 - }, - { - "x": 2.6560642905413947, - "y": 4.360643314893585, - "heading": -0.6858248901313277, - "angularVelocity": 0.2718349367303499, - "velocityX": -0.15166162602986216, - "velocityY": 0.4862731127377579, - "timestamp": 1.704674128402326 - }, - { - "x": 2.627632992233998, - "y": 4.419910426276983, - "heading": -0.652529270737317, - "angularVelocity": 0.38829233349639936, - "velocityX": -0.331564793358298, - "velocityY": 0.6911709527407293, - "timestamp": 1.7904229741088546 - }, - { - "x": 2.5850645171946978, - "y": 4.495119603214681, - "heading": -0.6100207421436514, - "angularVelocity": 0.4957329541105318, - "velocityX": -0.4964320474352394, - "velocityY": 0.877086756305711, - "timestamp": 1.8761718198153834 - }, - { - "x": 2.5295631514165375, - "y": 4.584706937933874, - "heading": -0.5590704924389158, - "angularVelocity": 0.5941800065637087, - "velocityX": -0.6472549609368695, - "velocityY": 1.0447643228435226, - "timestamp": 1.9619206655219121 - }, - { - "x": 2.46234032691493, - "y": 4.687033037499603, - "heading": -0.5004982071284244, - "angularVelocity": 0.6830679157006081, - "velocityX": -0.7839501972035283, - "velocityY": 1.1933233470679585, - "timestamp": 2.047669511228441 - }, - { - "x": 2.384701887418879, - "y": 4.800204071292841, - "heading": -0.43526625936464175, - "angularVelocity": 0.7607326632364927, - "velocityX": -0.9054167301769336, - "velocityY": 1.319796585723857, - "timestamp": 2.1334183569349694 - }, - { - "x": 2.298178863290851, - "y": 4.921641059920068, - "heading": -0.36471802922759106, - "angularVelocity": 0.8227309598836902, - "velocityX": -1.0090284413174404, - "velocityY": 1.416193858082253, - "timestamp": 2.219167202641498 - }, - { - "x": 2.2047510720717436, - "y": 5.044849013310237, - "heading": -0.2923595970689714, - "angularVelocity": 0.8438414717122018, - "velocityX": -1.0895515904535924, - "velocityY": 1.4368467863910614, - "timestamp": 2.3049160483480264 - }, - { - "x": 2.116347360692902, - "y": 5.158956328540576, - "heading": -0.22539524544258233, - "angularVelocity": 0.7809358956920709, - "velocityX": -1.0309609493916598, - "velocityY": 1.3307154666649113, - "timestamp": 2.390664894054555 - }, - { - "x": 2.0362102002706437, - "y": 5.261312637562149, - "heading": -0.16556699187625293, - "angularVelocity": 0.6977149729932085, - "velocityX": -0.9345567250727145, - "velocityY": 1.1936756486715054, - "timestamp": 2.4764137397610835 - }, - { - "x": 1.9661275657868833, - "y": 5.350222584355773, - "heading": -0.1134489428170574, - "angularVelocity": 0.6077988412528272, - "velocityX": -0.8173011998740509, - "velocityY": 1.0368646488597075, - "timestamp": 2.562162585467612 - }, - { - "x": 1.9075019379268083, - "y": 5.42424212633875, - "heading": -0.06991588076878923, - "angularVelocity": 0.5076810269523403, - "velocityX": -0.6836899946235904, - "velocityY": 0.8632132756201296, - "timestamp": 2.6479114311741405 - }, - { - "x": 1.861590779787906, - "y": 5.48200360252104, - "heading": -0.03588037308734902, - "angularVelocity": 0.3969208844854328, - "velocityX": -0.5354142992901785, - "velocityY": 0.673612288379681, - "timestamp": 2.733660276880669 - }, - { - "x": 1.8296344375134035, - "y": 5.522099910983847, - "heading": -0.012259641477970167, - "angularVelocity": 0.2754641349951199, - "velocityX": -0.3726737311878495, - "velocityY": 0.4676017284248481, - "timestamp": 2.8194091225871976 - }, - { - "x": 1.8129411935806854, - "y": 5.542999267578127, - "heading": 2.3632850028431835e-17, - "angularVelocity": 0.14297150447865262, - "velocityX": -0.1946760191950624, - "velocityY": 0.24372755600475027, - "timestamp": 2.905157968293726 - }, - { - "x": 1.812941193580624, - "y": 5.542999267578126, - "heading": -4.920156603511903e-17, - "angularVelocity": -5.733397110540358e-17, - "velocityX": -7.214388151371406e-13, - "velocityY": -9.03683027618772e-16, - "timestamp": 2.9909068140002546 - }, - { - "x": 1.8513325137310372, - "y": 5.5430599450572595, - "heading": -0.000016850565291482023, - "angularVelocity": -0.00016991086246254008, - "velocityX": 0.3871147468977978, - "velocityY": 0.0006118348336243464, - "timestamp": 3.090079786505359 - }, - { - "x": 1.9275402570059652, - "y": 5.543180391388335, - "heading": -0.000015746696122309923, - "angularVelocity": 0.00001113074602288469, - "velocityX": 0.7684325814778618, - "velocityY": 0.0012145076227106227, - "timestamp": 3.189252759010463 - }, - { - "x": 2.0338963277323163, - "y": 5.54334848714078, - "heading": 0.000009170336978677823, - "angularVelocity": 0.00025124822289482134, - "velocityX": 1.0724299982122594, - "velocityY": 0.0016949754373461064, - "timestamp": 3.2884257315155674 - }, - { - "x": 2.163383514243291, - "y": 5.5435531416216355, - "heading": 0.000033654463962368084, - "angularVelocity": 0.0002468830606297171, - "velocityX": 1.305670115961409, - "velocityY": 0.00206361144256778, - "timestamp": 3.3875987040206716 - }, - { - "x": 2.3079360710490797, - "y": 5.54378160701675, - "heading": 0.00003317773344428256, - "angularVelocity": -0.000004807060894986283, - "velocityX": 1.457580156714077, - "velocityY": 0.0023037062351123753, - "timestamp": 3.486771676525776 - }, - { - "x": 2.437398077457075, - "y": 5.543986221690917, - "heading": 0.000022146124274066103, - "angularVelocity": -0.00011123604437218979, - "velocityX": 1.3054162151016944, - "velocityY": 0.002063210056106694, - "timestamp": 3.58594464903088 - }, - { - "x": 2.5437229473484053, - "y": 5.544154268122903, - "heading": 0.000015406432122086, - "angularVelocity": -0.00006795896080977589, - "velocityX": 1.0721153879486738, - "velocityY": 0.0016944781198031775, - "timestamp": 3.6851176215359844 - }, - { - "x": 2.619913870706316, - "y": 5.544274687859312, - "heading": 0.000009213432317016853, - "angularVelocity": -0.00006244644733859944, - "velocityX": 0.7682629796539725, - "velocityY": 0.0012142394582550931, - "timestamp": 3.7842905940410887 - }, - { - "x": 2.6583051681519314, - "y": 5.5443353652954785, - "heading": 6.370510606485668e-14, - "angularVelocity": -0.00009290265301587805, - "velocityX": 0.38711451795641516, - "velocityY": 0.0006118344003786568, - "timestamp": 3.883463566546193 - }, - { - "x": 2.658305168152, - "y": 5.544335365295488, - "heading": 6.625668653050138e-14, - "angularVelocity": 2.6906565131836242e-14, - "velocityX": 7.155954565396242e-13, - "velocityY": 8.671180221391495e-14, - "timestamp": 3.982636539051297 - }, - { - "x": 2.6398879819889713, - "y": 5.568034006444528, - "heading": 0.008388499707182374, - "angularVelocity": 0.0943787022324955, - "velocityX": -0.20721108536177554, - "velocityY": 0.26663254150593735, - "timestamp": 4.071517817439562 - }, - { - "x": 2.6055876283476542, - "y": 5.613232266775393, - "heading": 0.025296828917652628, - "angularVelocity": 0.19023499118162804, - "velocityX": -0.3859120195327329, - "velocityY": 0.5085239675934979, - "timestamp": 4.160399095827827 - }, - { - "x": 2.5578825329127857, - "y": 5.677978135359711, - "heading": 0.05006181260458975, - "angularVelocity": 0.27862992225075944, - "velocityX": -0.5367282773164671, - "velocityY": 0.7284533903912082, - "timestamp": 4.2492803742160925 - }, - { - "x": 2.4993543830957403, - "y": 5.760558609750369, - "heading": 0.0821470752651059, - "angularVelocity": 0.36099011222914734, - "velocityX": -0.6584980648176972, - "velocityY": 0.9291098855468343, - "timestamp": 4.338161652604358 - }, - { - "x": 2.433098392034208, - "y": 5.859358208334035, - "heading": 0.12114426250239245, - "angularVelocity": 0.43875592188117873, - "velocityX": -0.7454437229429913, - "velocityY": 1.1115906563809108, - "timestamp": 4.427042930992623 - }, - { - "x": 2.3637465533640976, - "y": 5.972668871410117, - "heading": 0.16678728284828395, - "angularVelocity": 0.5135279461940396, - "velocityX": -0.7802749907275444, - "velocityY": 1.2748541102325668, - "timestamp": 4.515924209380888 - }, - { - "x": 2.300988842467506, - "y": 6.095835737634532, - "heading": 0.21776795840799054, - "angularVelocity": 0.5735817090404455, - "velocityX": -0.7060847012414084, - "velocityY": 1.385745889998316, - "timestamp": 4.604805487769153 - }, - { - "x": 2.2515752134889366, - "y": 6.217231027383832, - "heading": 0.2695393710632505, - "angularVelocity": 0.582478263071738, - "velocityX": -0.5559509254902292, - "velocityY": 1.3658139481175915, - "timestamp": 4.693686766157418 - }, - { - "x": 2.217045589242532, - "y": 6.33145065782542, - "heading": 0.3198059590268783, - "angularVelocity": 0.5655475357136119, - "velocityX": -0.3884915346931451, - "velocityY": 1.2850808686903799, - "timestamp": 4.782568044545683 - }, - { - "x": 2.19907919431313, - "y": 6.435243176299548, - "heading": 0.36734143796630997, - "angularVelocity": 0.5348199283510537, - "velocityX": -0.20213924974232386, - "velocityY": 1.1677658147606698, - "timestamp": 4.871449322933948 - }, - { - "x": 2.1993959276283994, - "y": 6.526053318679287, - "heading": 0.41157070153690833, - "angularVelocity": 0.4976218206191788, - "velocityX": 0.0035635549001480525, - "velocityY": 1.021701577951703, - "timestamp": 4.960330601322213 - }, - { - "x": 2.219770279567901, - "y": 6.601608618777817, - "heading": 0.45152771224881855, - "angularVelocity": 0.4495548605556116, - "velocityX": 0.2292310856552352, - "velocityY": 0.8500699075051399, - "timestamp": 5.049211879710478 - }, - { - "x": 2.2626490848637184, - "y": 6.659247151615926, - "heading": 0.48436572022362784, - "angularVelocity": 0.36945922212429394, - "velocityX": 0.4824278641490618, - "velocityY": 0.648489016846146, - "timestamp": 5.1380931580987435 - }, - { - "x": 2.329372405994089, - "y": 6.697374820688125, - "heading": 0.5125504195998649, - "angularVelocity": 0.31710501792120865, - "velocityX": 0.7507016363871187, - "velocityY": 0.4289730049288785, - "timestamp": 5.2269744364870085 - }, - { - "x": 2.399251093648798, - "y": 6.73730555904706, - "heading": 0.5293913369356736, - "angularVelocity": 0.21745079806450165, - "velocityX": 0.902277239279105, - "velocityY": 0.5155877648973247, - "timestamp": 5.304421461219836 - }, - { - "x": 2.4725481079431484, - "y": 6.779189629834486, - "heading": 0.507422235287016, - "angularVelocity": -0.28366617987293774, - "velocityX": 0.9464148501358945, - "velocityY": 0.5408092945202858, - "timestamp": 5.381868485952664 - }, - { - "x": 2.537702213604298, - "y": 6.816420603018375, - "heading": 0.5016725044650684, - "angularVelocity": -0.07424082257084566, - "velocityX": 0.8412731915467285, - "velocityY": 0.48072825693890747, - "timestamp": 5.459315510685491 - }, - { - "x": 2.5904059035391898, - "y": 6.846537042294488, - "heading": 0.5036538467311155, - "angularVelocity": 0.025583194098508056, - "velocityX": 0.680512777970975, - "velocityY": 0.38886502613926416, - "timestamp": 5.536762535418319 - }, - { - "x": 2.6280877308244723, - "y": 6.868069547224279, - "heading": 0.508503710225543, - "angularVelocity": 0.06262168896947905, - "velocityX": 0.4865497083575037, - "velocityY": 0.27802882066934376, - "timestamp": 5.614209560151147 - }, - { - "x": 2.648344516781087, - "y": 6.8796448707953655, - "heading": 0.5125504196001363, - "angularVelocity": 0.052251321320124576, - "velocityX": 0.26155667100350394, - "velocityY": 0.14946117855453953, - "timestamp": 5.691656584883974 - }, - { - "x": 2.6483445167657806, - "y": 6.879644870779064, - "heading": 0.5125504196000689, - "angularVelocity": 1.8669291195039983e-14, - "velocityX": -4.749030905139852e-11, - "velocityY": 6.098431731550748e-11, - "timestamp": 5.769103609616802 - }, - { - "x": 2.6747339306487334, - "y": 6.883481123686636, - "heading": 0.5098224624623802, - "angularVelocity": -0.03296702980330207, - "velocityX": 0.3189128531205414, - "velocityY": 0.046360649216292196, - "timestamp": 5.851851650734406 - }, - { - "x": 2.7264961186821273, - "y": 6.89099146776212, - "heading": 0.5039121473834008, - "angularVelocity": -0.07142543798202369, - "velocityX": 0.6255397388782451, - "velocityY": 0.09076159355636307, - "timestamp": 5.93459969185201 - }, - { - "x": 2.8018050306182753, - "y": 6.9018959167453104, - "heading": 0.4949465844489951, - "angularVelocity": -0.10834773625231511, - "velocityX": 0.9100990297657242, - "velocityY": 0.13177893803804336, - "timestamp": 6.017347732969614 - }, - { - "x": 2.899101313212433, - "y": 6.915952840560763, - "heading": 0.4831136364117761, - "angularVelocity": -0.14299973603485935, - "velocityX": 1.1758137265856894, - "velocityY": 0.16987621248329202, - "timestamp": 6.100095774087218 - }, - { - "x": 3.017081432389269, - "y": 6.932956858771751, - "heading": 0.4685947114251132, - "angularVelocity": -0.1754594403754914, - "velocityX": 1.42577537284728, - "velocityY": 0.20549148936151598, - "timestamp": 6.182843815204822 - }, - { - "x": 3.1546294358748312, - "y": 6.9527283369160715, - "heading": 0.4515640044783447, - "angularVelocity": -0.20581401948312003, - "velocityX": 1.6622508717768, - "velocityY": 0.23893590563959213, - "timestamp": 6.265591856322426 - }, - { - "x": 3.3107722570719647, - "y": 6.975106161489216, - "heading": 0.43219034104896137, - "angularVelocity": -0.2341283632545328, - "velocityX": 1.8869669793779915, - "velocityY": 0.2704332848357982, - "timestamp": 6.34833989744003 - }, - { - "x": 3.484600968601402, - "y": 6.999935624537231, - "heading": 0.4106444392061959, - "angularVelocity": -0.2603795999490034, - "velocityX": 2.1006988102882644, - "velocityY": 0.3000610372485155, - "timestamp": 6.431087938557634 - }, - { - "x": 3.675234170530874, - "y": 7.027062068947035, - "heading": 0.38710417033209454, - "angularVelocity": -0.28448128265230255, - "velocityX": 2.3037790303523367, - "velocityY": 0.32781977728352046, - "timestamp": 6.513835979675238 - }, - { - "x": 3.8817881153060045, - "y": 7.056325014551516, - "heading": 0.36176128472936936, - "angularVelocity": -0.3062656862983269, - "velocityX": 2.4961792688429623, - "velocityY": 0.3536391340419632, - "timestamp": 6.596584020792842 - }, - { - "x": 4.1033458782194, - "y": 7.087551279488626, - "heading": 0.3348309137177563, - "angularVelocity": -0.32545025414364576, - "velocityX": 2.677498583906162, - "velocityY": 0.3773656090873216, - "timestamp": 6.679332061910446 - }, - { - "x": 4.338922324182521, - "y": 7.120545790816314, - "heading": 0.30656596895157695, - "angularVelocity": -0.34157841544560996, - "velocityX": 2.846912661392294, - "velocityY": 0.39873465138360303, - "timestamp": 6.76208010302805 - }, - { - "x": 4.587417768628414, - "y": 7.155077045432509, - "heading": 0.27728164973070885, - "angularVelocity": -0.3538974315929522, - "velocityX": 3.00303718480442, - "velocityY": 0.4173060068834074, - "timestamp": 6.844828144145654 - }, - { - "x": 4.847543704393879, - "y": 7.190849002148663, - "heading": 0.2474070078170501, - "angularVelocity": -0.36103140944690143, - "velocityX": 3.1435902560613274, - "velocityY": 0.4322997406709641, - "timestamp": 6.927576185263258 - }, - { - "x": 5.117670126803491, - "y": 7.227429623630717, - "heading": 0.21762399464747098, - "angularVelocity": -0.35992408723308145, - "velocityX": 3.264444919314729, - "velocityY": 0.4420723558891399, - "timestamp": 7.010324226380862 - }, - { - "x": 5.395335267694769, - "y": 7.263944271068559, - "heading": 0.1895241870207491, - "angularVelocity": -0.33958275322536735, - "velocityX": 3.3555494141142153, - "velocityY": 0.44127506759904395, - "timestamp": 7.093072267498466 - }, - { - "x": 5.667910261992729, - "y": 7.2967916361655485, - "heading": 0.1669091076503975, - "angularVelocity": -0.27330048016738406, - "velocityX": 3.294035612402776, - "velocityY": 0.3969564071048189, - "timestamp": 7.17582030861607 - }, - { - "x": 5.931901746285159, - "y": 7.327414779180602, - "heading": 0.14778373205342693, - "angularVelocity": -0.23112783503586568, - "velocityX": 3.1903049392702143, - "velocityY": 0.3700769541063461, - "timestamp": 7.258568349733674 - }, - { - "x": 6.1801220482371395, - "y": 7.354810947360831, - "heading": 0.11632647881372146, - "angularVelocity": -0.38015707459464126, - "velocityX": 2.999712121271843, - "velocityY": 0.33107935620263895, - "timestamp": 7.341316390851278 - }, - { - "x": 6.415697256681246, - "y": 7.379780463537999, - "heading": 0.08899977607154176, - "angularVelocity": -0.3302398748429845, - "velocityX": 2.84689770612573, - "velocityY": 0.3017535622589267, - "timestamp": 7.424064431968882 - }, - { - "x": 6.637987945996846, - "y": 7.402299511961184, - "heading": 0.06474721752358573, - "angularVelocity": -0.2930892166194916, - "velocityX": 2.686355910222349, - "velocityY": 0.2721399578655483, - "timestamp": 7.5068124730864865 - }, - { - "x": 6.845920803268571, - "y": 7.422253782487123, - "heading": 0.04362794173118966, - "angularVelocity": -0.2552238760840361, - "velocityX": 2.5128432584428544, - "velocityY": 0.24114492931132497, - "timestamp": 7.5895605142040905 - }, - { - "x": 7.038548371158306, - "y": 7.439534256583214, - "heading": 0.025735796286898687, - "angularVelocity": -0.2162243988212622, - "velocityX": 2.3278807001118484, - "velocityY": 0.2088324250667052, - "timestamp": 7.6723085553216945 - }, - { - "x": 7.2149794023417675, - "y": 7.454031322663086, - "heading": 0.011159229205506762, - "angularVelocity": -0.17615603807074215, - "velocityX": 2.1321475264013814, - "velocityY": 0.1751952781490545, - "timestamp": 7.755056596439299 - }, - { - "x": 7.374384880065902, - "y": 7.465639114379918, - "heading": -1.1165566205788637e-16, - "angularVelocity": -0.134857925997875, - "velocityX": 1.9263957861864134, - "velocityY": 0.14027874932231896, - "timestamp": 7.837804637556903 - }, - { - "x": 7.500924464216497, - "y": 7.473646968954779, - "heading": -0.0070937662662868915, - "angularVelocity": -0.09816609550664941, - "velocityX": 1.7511003938959169, - "velocityY": 0.11081557912801578, - "timestamp": 7.9100675327008405 - }, - { - "x": 7.613899532464792, - "y": 7.47952275118133, - "heading": -0.01171150501248939, - "angularVelocity": -0.06390193386252535, - "velocityX": 1.5633897316635108, - "velocityY": 0.08131119317636289, - "timestamp": 7.982330427844778 - }, - { - "x": 7.712046212988902, - "y": 7.483209835153203, - "heading": -0.014057088311073107, - "angularVelocity": -0.032459027470621904, - "velocityX": 1.3581891554251948, - "velocityY": 0.05102319751389575, - "timestamp": 8.054593322988717 - }, - { - "x": 7.79365194754084, - "y": 7.484595849351514, - "heading": -0.014345050898318633, - "angularVelocity": -0.003984930117620908, - "velocityX": 1.129289580626482, - "velocityY": 0.019180164253482204, - "timestamp": 8.126856218132655 - }, - { - "x": 7.856538953365895, - "y": 7.483519387813377, - "heading": -0.012796446625875068, - "angularVelocity": 0.02143014432730538, - "velocityX": 0.8702530627897808, - "velocityY": -0.0148964629220964, - "timestamp": 8.199119113276593 - }, - { - "x": 7.899249386445817, - "y": 7.479924646329156, - "heading": -0.009762276595150929, - "angularVelocity": 0.04198793896480952, - "velocityX": 0.5910423737502203, - "velocityY": -0.04974532887286408, - "timestamp": 8.271382008420531 - }, - { - "x": 7.921731862807926, - "y": 7.4739282664160145, - "heading": -0.005439516570392937, - "angularVelocity": 0.0598199119499377, - "velocityX": 0.31112061476836195, - "velocityY": -0.08298006744976984, - "timestamp": 8.343644903564469 - }, - { - "x": 7.923961639404313, - "y": 7.465639114379864, - "heading": -8.923477228591696e-17, - "angularVelocity": 0.07527399171536123, - "velocityX": 0.030856452567537763, - "velocityY": -0.11470827482980998, - "timestamp": 8.415907798708407 - }, - { - "x": 7.888861809774048, - "y": 7.450123409205229, - "heading": 0.009559512590155543, - "angularVelocity": 0.09629823646465466, - "velocityX": -0.3535799196587898, - "velocityY": -0.15629824551531185, - "timestamp": 8.515177660142104 - }, - { - "x": 7.817152523236329, - "y": 7.430666435185926, - "heading": 0.02163779335227315, - "angularVelocity": 0.12167117579976462, - "velocityX": -0.72236714650414, - "velocityY": -0.19600081775371833, - "timestamp": 8.614447521575801 - }, - { - "x": 7.711448594725456, - "y": 7.4075722780062225, - "heading": 0.036035719670363044, - "angularVelocity": 0.14503824333134957, - "velocityX": -1.064813902067085, - "velocityY": -0.23264016737978235, - "timestamp": 8.713717383009499 - }, - { - "x": 7.574048351882708, - "y": 7.3811128444264495, - "heading": 0.05252610728763101, - "angularVelocity": 0.16611675869298972, - "velocityX": -1.3841083371967766, - "velocityY": -0.2665404504210516, - "timestamp": 8.812987244443196 - }, - { - "x": 7.407019371926434, - "y": 7.351538272778396, - "heading": 0.07089974064841498, - "angularVelocity": 0.18508773050978647, - "velocityX": -1.682574928019174, - "velocityY": -0.2979209522500118, - "timestamp": 8.912257105876893 - }, - { - "x": 7.212241390572204, - "y": 7.319083353098022, - "heading": 0.09094586321763254, - "angularVelocity": 0.20193563564714437, - "velocityX": -1.96210590546985, - "velocityY": -0.3269362847056093, - "timestamp": 9.01152696731059 - }, - { - "x": 6.990210935369373, - "y": 7.283848196853213, - "heading": 0.11243058039021528, - "angularVelocity": 0.216427391579795, - "velocityX": -2.236635087388812, - "velocityY": -0.35494313919580667, - "timestamp": 9.110796828744288 - }, - { - "x": 6.744250267253016, - "y": 7.2462331722899656, - "heading": 0.1352094476478159, - "angularVelocity": 0.2294640783075409, - "velocityX": -2.4776973047417434, - "velocityY": -0.3789168637892277, - "timestamp": 9.210066690177985 - }, - { - "x": 6.474537469267562, - "y": 7.206343597573894, - "heading": 0.1591116816121769, - "angularVelocity": 0.24078036998495714, - "velocityX": -2.7169655934857664, - "velocityY": -0.401829660482739, - "timestamp": 9.309336551611683 - }, - { - "x": 6.185093329086759, - "y": 7.164701733979632, - "heading": 0.18384126922195682, - "angularVelocity": 0.24911475902781344, - "velocityX": -2.915730273020737, - "velocityY": -0.4194814316536095, - "timestamp": 9.40860641304538 - }, - { - "x": 5.8780260788239564, - "y": 7.121745615538567, - "heading": 0.2121770427035368, - "angularVelocity": 0.28544185589002363, - "velocityX": -3.0932575690950848, - "velocityY": -0.43272064472210836, - "timestamp": 9.507876274479077 - }, - { - "x": 5.557966335758099, - "y": 7.0784922097584015, - "heading": 0.24028820463072503, - "angularVelocity": 0.283179219968629, - "velocityX": -3.224138106404308, - "velocityY": -0.4357153838585266, - "timestamp": 9.607146135912775 - }, - { - "x": 5.245908943265439, - "y": 7.038535334063528, - "heading": 0.26569993334648234, - "angularVelocity": 0.25598634216619676, - "velocityX": -3.1435260207458486, - "velocityY": -0.40250762031696863, - "timestamp": 9.706415997346472 - }, - { - "x": 4.948601311707987, - "y": 7.0011016986080925, - "heading": 0.2891526852975207, - "angularVelocity": 0.23625249005411966, - "velocityX": -2.9949435534976283, - "velocityY": -0.37708963138260865, - "timestamp": 9.80568585878017 - }, - { - "x": 4.669544920179785, - "y": 6.966326786089845, - "heading": 0.31063199175620987, - "angularVelocity": 0.21637288647809105, - "velocityX": -2.811088758440401, - "velocityY": -0.35030685059909655, - "timestamp": 9.904955720213866 - }, - { - "x": 4.411446504016625, - "y": 6.934401644375056, - "heading": 0.3300269329519647, - "angularVelocity": 0.19537592694947853, - "velocityX": -2.599967527259491, - "velocityY": -0.3215995394142002, - "timestamp": 10.004225581647564 - }, - { - "x": 4.176717541970206, - "y": 6.9055345106659285, - "heading": 0.3471896751796373, - "angularVelocity": 0.17288975707028345, - "velocityX": -2.364554142177353, - "velocityY": -0.2907945401778102, - "timestamp": 10.103495443081261 - }, - { - "x": 3.9677270788540704, - "y": 6.879953869869358, - "heading": 0.3619526945831296, - "angularVelocity": 0.14871602710307644, - "velocityX": -2.1052760636290624, - "velocityY": -0.2576878866065069, - "timestamp": 10.202765304514958 - }, - { - "x": 3.7869910923072463, - "y": 6.857920125982094, - "heading": 0.37415675707125545, - "angularVelocity": 0.1229382444164791, - "velocityX": -1.8206531563211636, - "velocityY": -0.22195804012459744, - "timestamp": 10.302035165948656 - }, - { - "x": 3.6375222511124736, - "y": 6.83976261955988, - "heading": 0.3836657545916857, - "angularVelocity": 0.09578937033957077, - "velocityX": -1.5056819767458316, - "velocityY": -0.18291056479757375, - "timestamp": 10.401305027382353 - }, - { - "x": 3.5230866973435258, - "y": 6.825906628204701, - "heading": 0.39040681994356247, - "angularVelocity": 0.06790646480733925, - "velocityX": -1.1527723733691484, - "velocityY": -0.1395790339088257, - "timestamp": 10.50057488881605 - }, - { - "x": 3.4467104911360567, - "y": 6.816686862116541, - "heading": 0.39477159352312596, - "angularVelocity": 0.04396876873328538, - "velocityX": -0.7693795992500979, - "velocityY": -0.09287578279051523, - "timestamp": 10.599844750249748 - }, - { - "x": 3.4085204601287793, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": 0.021852252491790684, - "velocityX": -0.3847092204594867, - "velocityY": -0.046321940190690435, - "timestamp": 10.699114611683445 - }, - { - "x": 3.408520460128781, - "y": 6.812088489532471, - "heading": 0.39694086360000014, - "angularVelocity": -9.258859562419926e-18, - "velocityX": 7.880837628913899e-16, - "velocityY": -2.4194099006901583e-16, - "timestamp": 10.798384473117142 - }, - { - "x": 3.4451800140480633, - "y": 6.811382414928076, - "heading": 0.39378662700224637, - "angularVelocity": -0.032542531675579224, - "velocityX": 0.37821978715244237, - "velocityY": -0.00728463273656313, - "timestamp": 10.895311062271263 - }, - { - "x": 3.518496947445585, - "y": 6.809970351527808, - "heading": 0.38746820532810716, - "angularVelocity": -0.06518770266528738, - "velocityX": 0.756417140408642, - "velocityY": -0.014568380179178244, - "timestamp": 10.992237651425384 - }, - { - "x": 3.6283397734394334, - "y": 6.8078551393922675, - "heading": 0.37750274880248313, - "angularVelocity": -0.1028144765290181, - "velocityX": 1.1332579321365746, - "velocityY": -0.021822826471036318, - "timestamp": 11.089164240579505 - }, - { - "x": 3.7707535238798844, - "y": 6.805113681812527, - "heading": 0.36197971834397535, - "angularVelocity": -0.16015244726939834, - "velocityX": 1.469294975540766, - "velocityY": -0.028283854860319504, - "timestamp": 11.186090829733626 - }, - { - "x": 3.9413941020263046, - "y": 6.801829282896055, - "heading": 0.341973429419134, - "angularVelocity": -0.20640661246244818, - "velocityX": 1.7605135973070385, - "velocityY": -0.03388542757086655, - "timestamp": 11.283017418887747 - }, - { - "x": 4.137139185749371, - "y": 6.798061838763412, - "heading": 0.3182906699990001, - "angularVelocity": -0.24433707640817395, - "velocityX": 2.0195189517276724, - "velocityY": -0.038869046827304486, - "timestamp": 11.379944008041868 - }, - { - "x": 4.3554217491364975, - "y": 6.793860650651487, - "heading": 0.2915337558008742, - "angularVelocity": -0.2760533970258723, - "velocityX": 2.2520400778783602, - "velocityY": -0.043344020960483176, - "timestamp": 11.476870597195989 - }, - { - "x": 4.593770534553052, - "y": 6.7892732393783435, - "heading": 0.26218341148270125, - "angularVelocity": -0.30281003978695553, - "velocityX": 2.459065025362269, - "velocityY": -0.04732871870534226, - "timestamp": 11.57379718635011 - }, - { - "x": 4.849438377602218, - "y": 6.784352472846038, - "heading": 0.23067476418411942, - "angularVelocity": -0.32507743822988416, - "velocityX": 2.6377472402607576, - "velocityY": -0.05076797373402456, - "timestamp": 11.67072377550423 - }, - { - "x": 5.118739979240287, - "y": 6.77916928922838, - "heading": 0.197506319622179, - "angularVelocity": -0.3422017100921637, - "velocityX": 2.7784079063161986, - "velocityY": -0.053475353490631775, - "timestamp": 11.767650364658351 - }, - { - "x": 5.388165163887155, - "y": 6.773984054928517, - "heading": 0.16458046580315022, - "angularVelocity": -0.33969888042458984, - "velocityX": 2.779682922902244, - "velocityY": -0.053496510556234986, - "timestamp": 11.864576953812472 - }, - { - "x": 5.643920085833601, - "y": 6.769061952936388, - "heading": 0.1333147392066983, - "angularVelocity": -0.32257120434452946, - "velocityX": 2.6386456407723093, - "velocityY": -0.05078175178849002, - "timestamp": 11.961503542966593 - }, - { - "x": 5.882330711464664, - "y": 6.764473684698037, - "heading": 0.10420435480854875, - "angularVelocity": -0.3003343525465651, - "velocityX": 2.4597030362017005, - "velocityY": -0.04733756008947892, - "timestamp": 12.058430132120714 - }, - { - "x": 6.100655045491889, - "y": 6.760272005917349, - "heading": 0.07768097943341207, - "angularVelocity": -0.27364395679871373, - "velocityX": 2.252471029183505, - "velocityY": -0.04334908323253468, - "timestamp": 12.155356721274835 - }, - { - "x": 6.29642574105686, - "y": 6.756504387895065, - "heading": 0.054221932212767415, - "angularVelocity": -0.2420290183052161, - "velocityX": 2.019783191314849, - "velocityY": -0.038870840861772844, - "timestamp": 12.252283310428956 - }, - { - "x": 6.467079365349485, - "y": 6.7532201591852425, - "heading": 0.03442675668029697, - "angularVelocity": -0.20422853734174792, - "velocityX": 1.7606481955253344, - "velocityY": -0.03388367153413224, - "timestamp": 12.349209899583077 - }, - { - "x": 6.60949406591357, - "y": 6.750479406444339, - "heading": 0.019100936295010484, - "angularVelocity": -0.15811781389436272, - "velocityX": 1.4693047780484314, - "velocityY": -0.028276582977095337, - "timestamp": 12.446136488737197 - }, - { - "x": 6.719322188320328, - "y": 6.748365796127614, - "heading": 0.009318208717894495, - "angularVelocity": -0.10092924616960162, - "velocityX": 1.1331062339573879, - "velocityY": -0.021806300367839485, - "timestamp": 12.543063077891318 - }, - { - "x": 6.792644320795612, - "y": 6.746954731447967, - "heading": 0.0031061365662838365, - "angularVelocity": -0.06409048544701255, - "velocityX": 0.756470779743405, - "velocityY": -0.014558076292195965, - "timestamp": 12.63998966704544 - }, - { - "x": 6.829305171966555, - "y": 6.746249198913574, - "heading": -6.996174209709525e-17, - "angularVelocity": -0.03204627949246174, - "velocityX": 0.3782331710099824, - "velocityY": -0.007279040153471663, - "timestamp": 12.73691625619956 - }, - { - "x": 6.8293051719665545, - "y": 6.746249198913574, - "heading": -3.495236266448189e-17, - "angularVelocity": 5.858001175456844e-19, - "velocityX": -7.463612085764823e-16, - "velocityY": 1.4437890171198592e-17, - "timestamp": 12.833842845353681 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -3.4787101362106846e-24, + "angularVelocity": -4.3859089659641935e-24, + "velocityX": -2.4789748003093715e-23, + "velocityY": -3.607487096592192e-23, + "timestamp": 0 + }, + { + "x": 1.3060567681339126, + "y": 5.57238191847303, + "heading": -0.010695335590330916, + "angularVelocity": -0.1421295228209153, + "velocityX": 0.21371672085882062, + "velocityY": -0.24680705280850238, + "timestamp": 0.07525062617572531 + }, + { + "x": 1.3382215482089261, + "y": 5.535237409911883, + "heading": -0.032077507438502996, + "angularVelocity": -0.28414609864163015, + "velocityX": 0.4274353810678254, + "velocityY": -0.49361062424128055, + "timestamp": 0.15050125235145062 + }, + { + "x": 1.3864692379478656, + "y": 5.479521035128849, + "heading": -0.06412446300016969, + "angularVelocity": -0.4258696198332041, + "velocityX": 0.6411599768787516, + "velocityY": -0.7404107794787639, + "timestamp": 0.22575187852717593 + }, + { + "x": 1.450800607934819, + "y": 5.405233129379991, + "heading": -0.10680310960887816, + "angularVelocity": -0.5671533750303323, + "velocityX": 0.8548948129245705, + "velocityY": -0.9872064795232551, + "timestamp": 0.30100250470290124 + }, + { + "x": 1.531216769618125, + "y": 5.3123742117306145, + "heading": -0.1600730202690718, + "angularVelocity": -0.7078998988765586, + "velocityX": 1.0686444189250723, + "velocityY": -1.2339952817473157, + "timestamp": 0.37625313087862655 + }, + { + "x": 1.627719192058266, + "y": 5.200945113429446, + "heading": -0.2238912344076857, + "angularVelocity": -0.8480755228479505, + "velocityX": 1.282413547161571, + "velocityY": -1.4807730375686, + "timestamp": 0.45150375705435186 + }, + { + "x": 1.7403097840465742, + "y": 5.070947169326127, + "heading": -0.2982176307237913, + "angularVelocity": -0.9877179778217201, + "velocityX": 1.4962080411847536, + "velocityY": -1.7275330546718466, + "timestamp": 0.5267543832300772 + }, + { + "x": 1.8689915581726206, + "y": 4.922382882779654, + "heading": -0.3830195540733457, + "angularVelocity": -1.126926481003957, + "velocityX": 1.7100425692877124, + "velocityY": -1.974259804822693, + "timestamp": 0.6020050094058025 + }, + { + "x": 2.021815508579126, + "y": 4.778590260197623, + "heading": -0.46245355485112494, + "angularVelocity": -1.055592555366715, + "velocityX": 2.0308661624905433, + "velocityY": -1.9108495156737524, + "timestamp": 0.6772556355815278 + }, + { + "x": 2.1585523272850695, + "y": 4.6533669600969905, + "heading": -0.5314334765942994, + "angularVelocity": -0.9166690730531941, + "velocityX": 1.8170854603473379, + "velocityY": -1.664083164015284, + "timestamp": 0.7525062617572531 + }, + { + "x": 2.2791997774513058, + "y": 4.5467108563615435, + "heading": -0.5899636375558711, + "angularVelocity": -0.7778029756841082, + "velocityX": 1.6032750330143433, + "velocityY": -1.4173450661577678, + "timestamp": 0.8277568879329784 + }, + { + "x": 2.3837568433333827, + "y": 4.45862086228965, + "heading": -0.6380444530356357, + "angularVelocity": -0.6389423972032644, + "velocityX": 1.389451107528536, + "velocityY": -1.1706214093977847, + "timestamp": 0.9030075141087037 + }, + { + "x": 2.4722229118459578, + "y": 4.389096268153827, + "heading": -0.6756747726303448, + "angularVelocity": -0.500066531098822, + "velocityX": 1.1756190347969888, + "velocityY": -0.923907184153793, + "timestamp": 0.978258140284429 + }, + { + "x": 2.544597592286723, + "y": 4.338136593499442, + "heading": -0.702852872614281, + "angularVelocity": -0.361167758530936, + "velocityX": 0.9617817700514026, + "velocityY": -0.677199343635805, + "timestamp": 1.0535087664601543 + }, + { + "x": 2.600880652205466, + "y": 4.305741528586687, + "heading": -0.7195768736441226, + "angularVelocity": -0.22224401150879278, + "velocityX": 0.7479414162921453, + "velocityY": -0.4304956192272341, + "timestamp": 1.1287593926358797 + }, + { + "x": 2.6410719901577853, + "y": 4.291910905991578, + "heading": -0.7258448979650717, + "angularVelocity": -0.08329531114215386, + "velocityX": 0.534099714445754, + "velocityY": -0.18379411970354914, + "timestamp": 1.204010018811605 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05567771435110341, + "velocityX": 0.32025823965793626, + "velocityY": 0.06290687402329305, + "timestamp": 1.2792606449873303 + }, + { + "x": 2.6731116725964643, + "y": 4.320290742325012, + "heading": -0.7067971816671581, + "angularVelocity": 0.19582509769912612, + "velocityX": 0.10464853341562398, + "velocityY": 0.31165107776032225, + "timestamp": 1.3551341341546281 + }, + { + "x": 2.664692784388126, + "y": 4.362810214221228, + "heading": -0.6813099320330065, + "angularVelocity": 0.33591772190617664, + "velocityX": -0.11095954991308038, + "velocityY": 0.5603995857164566, + "timestamp": 1.431007623321926 + }, + { + "x": 2.63991517535616, + "y": 4.424203564318272, + "heading": -0.645197478734488, + "angularVelocity": 0.47595614350741217, + "velocityX": -0.3265647764969981, + "velocityY": 0.8091541692734667, + "timestamp": 1.5068811124892239 + }, + { + "x": 2.598779165089059, + "y": 4.504471398642189, + "heading": -0.5984632230032911, + "angularVelocity": 0.6159497374392491, + "velocityX": -0.5421657909576058, + "velocityY": 1.0579167401531877, + "timestamp": 1.5827546016565217 + }, + { + "x": 2.5412851916310535, + "y": 4.603614465429005, + "heading": -0.5411083104388861, + "angularVelocity": 0.7559282325601216, + "velocityX": -0.7577610320679146, + "velocityY": 1.3066891726596395, + "timestamp": 1.6586280908238196 + }, + { + "x": 2.4674338175988146, + "y": 4.721633607594926, + "heading": -0.473128621994749, + "angularVelocity": 0.8959610160308396, + "velocityX": -0.9733488579838422, + "velocityY": 1.5554727146618088, + "timestamp": 1.7345015799911174 + }, + { + "x": 2.3772256430083227, + "y": 4.858529577863132, + "heading": -0.3945096550364948, + "angularVelocity": 1.036184941816799, + "velocityX": -1.1889287757886868, + "velocityY": 1.8042661774306572, + "timestamp": 1.8103750691584153 + }, + { + "x": 2.2706605414206735, + "y": 5.014302199754286, + "heading": -0.3052190611328218, + "angularVelocity": 1.1768352145607928, + "velocityX": -1.4045103600373166, + "velocityY": 2.053057314231072, + "timestamp": 1.8862485583257131 + }, + { + "x": 2.1562301513945004, + "y": 5.146478418893045, + "heading": -0.22909327294912277, + "angularVelocity": 1.0033252591803818, + "velocityX": -1.508173556825084, + "velocityY": 1.7420606405396215, + "timestamp": 1.962122047493011 + }, + { + "x": 2.058146995665746, + "y": 5.259770783530285, + "heading": -0.16373665420264177, + "angularVelocity": 0.8613893925765344, + "velocityX": -1.2927197207510102, + "velocityY": 1.4931745709945456, + "timestamp": 2.037995536660309 + }, + { + "x": 1.9764115735788519, + "y": 5.354180517364029, + "heading": -0.1092008927472175, + "angularVelocity": 0.718772288633981, + "velocityX": -1.0772593034000484, + "velocityY": 1.2443046295864166, + "timestamp": 2.113869025827607 + }, + { + "x": 1.9110235321882938, + "y": 5.429708042242192, + "heading": -0.06553152876247478, + "angularVelocity": 0.5755549726789759, + "velocityX": -0.8618035378125303, + "velocityY": 0.9954402480638209, + "timestamp": 2.189742514994905 + }, + { + "x": 1.8619825299142096, + "y": 5.48635360523882, + "heading": -0.03276339486876071, + "angularVelocity": 0.43187856856644224, + "velocityX": -0.6463522741909358, + "velocityY": 0.7465791229361657, + "timestamp": 2.2656160041622027 + }, + { + "x": 1.8292883999681575, + "y": 5.524117342026143, + "heading": -0.010917644112626662, + "angularVelocity": 0.28792337080959957, + "velocityX": -0.430903208813331, + "velocityY": 0.4977197859459911, + "timestamp": 2.3414894933295005 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.2049864598182203e-23, + "angularVelocity": 0.14389273819414988, + "velocityX": -0.21545346822636996, + "velocityY": 0.24886064631017277, + "timestamp": 2.4173629824967984 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 5.735401641845927e-24, + "angularVelocity": -6.2110589016170234e-24, + "velocityX": -6.266702545665033e-23, + "velocityY": -1.0096243291514256e-22, + "timestamp": 2.4932364716640962 + }, + { + "x": 1.8552093934543321, + "y": 5.543066072465799, + "heading": -4.219694925044242e-18, + "angularVelocity": -4.2870553829964666e-17, + "velocityX": 0.42942883486562483, + "velocityY": 0.0006787122508870844, + "timestamp": 2.591665353103628 + }, + { + "x": 1.939745792565551, + "y": 5.543199682240142, + "heading": -1.2239290131185836e-17, + "angularVelocity": -8.147603720153091e-17, + "velocityX": 0.858857663267793, + "velocityY": 0.0013574244915586756, + "timestamp": 2.6900942345431598 + }, + { + "x": 2.066550389641902, + "y": 5.543400096899142, + "heading": -5.060219765528842e-18, + "angularVelocity": 7.293662450422726e-17, + "velocityX": 1.288286478743048, + "velocityY": 0.002036136711799281, + "timestamp": 2.7885231159826915 + }, + { + "x": 2.2356231808662415, + "y": 5.543667316436768, + "heading": 1.924608375875081e-20, + "angularVelocity": 5.160544115984206e-17, + "velocityX": 1.7177152554375656, + "velocityY": 0.0027148488707469335, + "timestamp": 2.8869519974222233 + }, + { + "x": 2.4046959720905807, + "y": 5.543934535974393, + "heading": 5.109055925882335e-18, + "angularVelocity": 5.171053219292816e-17, + "velocityX": 1.7177152554375659, + "velocityY": 0.0027148488707469335, + "timestamp": 2.985380878861755 + }, + { + "x": 2.531500569166932, + "y": 5.544134950633393, + "heading": 1.2272248404707649e-17, + "angularVelocity": 7.277531121284907e-17, + "velocityX": 1.288286478743048, + "velocityY": 0.002036136711799281, + "timestamp": 3.083809760301287 + }, + { + "x": 2.616036968278151, + "y": 5.544268560407736, + "heading": 4.225188349913728e-18, + "angularVelocity": -8.175506961814134e-17, + "velocityX": 0.858857663267793, + "velocityY": 0.0013574244915586754, + "timestamp": 3.1822386417408186 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -1.1775900898457124e-26, + "angularVelocity": -4.2926306787465456e-17, + "velocityX": 0.42942883486562483, + "velocityY": 0.0006787122508870843, + "timestamp": 3.2806675231803504 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -1.2588122005060392e-26, + "angularVelocity": -5.516653068340682e-27, + "velocityX": 1.9209951977484535e-26, + "velocityY": 6.96713079819041e-27, + "timestamp": 3.379096404619882 + }, + { + "x": 2.6438894427168456, + "y": 5.563919137233213, + "heading": 0.003139326829545763, + "angularVelocity": 0.042039393016227077, + "velocityX": -0.19304404417939905, + "velocityY": 0.2622504536593843, + "timestamp": 3.453772240884549 + }, + { + "x": 2.6152671402107748, + "y": 5.603236779531591, + "heading": 0.009643531295019244, + "angularVelocity": 0.08709918483431768, + "velocityX": -0.3832873381508234, + "velocityY": 0.5265109072100428, + "timestamp": 3.528448077149216 + }, + { + "x": 2.5727492585947744, + "y": 5.6625051004279126, + "heading": 0.019846282811438364, + "angularVelocity": 0.13662721472925177, + "velocityX": -0.5693659923044985, + "velocityY": 0.7936746859621688, + "timestamp": 3.603123913413883 + }, + { + "x": 2.516846273489969, + "y": 5.742064038671315, + "heading": 0.03428875746248491, + "angularVelocity": 0.19340224861840632, + "velocityX": -0.7486087588851783, + "velocityY": 1.0653906567772256, + "timestamp": 3.6777997496785497 + }, + { + "x": 2.4485464886328767, + "y": 5.842518783448963, + "heading": 0.05399413536855069, + "angularVelocity": 0.26387890503463146, + "velocityX": -0.9146169400101983, + "velocityY": 1.3452108446648592, + "timestamp": 3.7524755859432166 + }, + { + "x": 2.3705191792336286, + "y": 5.965194001491249, + "heading": 0.08158339935984246, + "angularVelocity": 0.3694536997685508, + "velocityX": -1.0448802892906721, + "velocityY": 1.6427699263721678, + "timestamp": 3.8271514222078835 + }, + { + "x": 2.300330937558203, + "y": 6.110432887604612, + "heading": 0.1284815107612587, + "angularVelocity": 0.6280225806270092, + "velocityX": -0.9399056667629848, + "velocityY": 1.9449248026979609, + "timestamp": 3.9018272584725504 + }, + { + "x": 2.251762063243615, + "y": 6.244623434670601, + "heading": 0.1817545952842417, + "angularVelocity": 0.7133912010596283, + "velocityX": -0.6503961220126102, + "velocityY": 1.796974145564157, + "timestamp": 3.9765030947372173 + }, + { + "x": 2.2219257093836906, + "y": 6.363297360569798, + "heading": 0.2370564478928837, + "angularVelocity": 0.7405588658242863, + "velocityX": -0.3995449579456786, + "velocityY": 1.5891877725827055, + "timestamp": 4.051178931001885 + }, + { + "x": 2.209746331851008, + "y": 6.465234411576823, + "heading": 0.2929790595103366, + "angularVelocity": 0.7488715816887712, + "velocityX": -0.16309663395688523, + "velocityY": 1.3650607225306177, + "timestamp": 4.125854767266552 + }, + { + "x": 2.214679002088127, + "y": 6.5498771941878156, + "heading": 0.3488347523758202, + "angularVelocity": 0.7479754584537774, + "velocityX": 0.06605443586378532, + "velocityY": 1.133469497562776, + "timestamp": 4.200530603531219 + }, + { + "x": 2.236396070259457, + "y": 6.616907899287828, + "heading": 0.40421575749752775, + "angularVelocity": 0.7416188139550987, + "velocityX": 0.2908178770755253, + "velocityY": 0.897622423168334, + "timestamp": 4.275206439795887 + }, + { + "x": 2.274679156572898, + "y": 6.666121488597866, + "heading": 0.45885178755752026, + "angularVelocity": 0.7316426945170152, + "velocityX": 0.5126569480622607, + "velocityY": 0.6590296375873849, + "timestamp": 4.349882276060554 + }, + { + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.7190897983674445, + "velocityX": 0.7324089313056569, + "velocityY": 0.41852001496969765, + "timestamp": 4.424558112325221 + }, + { + "x": 2.411093271023251, + "y": 6.744072527671164, + "heading": 0.5643142977465416, + "angularVelocity": 0.6537087450996224, + "velocityX": 1.032025536567027, + "velocityY": 0.5897297596346315, + "timestamp": 4.503743037621965 + }, + { + "x": 2.50599376720386, + "y": 6.798301463699205, + "heading": 0.5436087675323019, + "angularVelocity": -0.26148323227743453, + "velocityX": 1.1984667008899996, + "velocityY": 0.6848391385711283, + "timestamp": 4.582927962918709 + }, + { + "x": 2.5771691415097524, + "y": 6.838973166960486, + "heading": 0.5280796125632321, + "angularVelocity": -0.19611251650329578, + "velocityX": 0.898850053077196, + "velocityY": 0.5136293695910533, + "timestamp": 4.662112888215453 + }, + { + "x": 2.6246193915553895, + "y": 6.866087636091841, + "heading": 0.5177268190541022, + "angularVelocity": -0.13074197481822433, + "velocityX": 0.5992333751382385, + "velocityY": 0.34241958339600737, + "timestamp": 4.741297813512197 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06537102150066688, + "velocityX": 0.29961668979104056, + "velocityY": 0.1712097929676749, + "timestamp": 4.82048273880894 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 1.4429207692433446e-21, + "velocityX": 9.55884619679387e-19, + "velocityY": -1.6623744143332693e-18, + "timestamp": 4.899667664105684 + }, + { + "x": 2.67095907816642, + "y": 6.882744787327794, + "heading": 0.5061644140699368, + "angularVelocity": -0.08818113694961922, + "velocityX": 0.3122730989760289, + "velocityY": 0.04280518804462273, + "timestamp": 4.972086842979639 + }, + { + "x": 2.716191596050434, + "y": 6.888945496159438, + "heading": 0.4936002658801072, + "angularVelocity": -0.17349200011916, + "velocityX": 0.6245930786199807, + "velocityY": 0.08562246808178013, + "timestamp": 5.044506021853594 + }, + { + "x": 2.784046041436307, + "y": 6.898248047061875, + "heading": 0.4751107385388315, + "angularVelocity": -0.2553125791919922, + "velocityX": 0.9369678922205519, + "velocityY": 0.12845424440158493, + "timestamp": 5.1169252007275485 + }, + { + "x": 2.8745270921290036, + "y": 6.910653685112006, + "heading": 0.4510077220563394, + "angularVelocity": -0.33282642605549656, + "velocityX": 1.249407299276042, + "velocityY": 0.17130321336180326, + "timestamp": 5.189344379601503 + }, + { + "x": 2.9876402967876543, + "y": 6.926163879564403, + "heading": 0.4216838617313518, + "angularVelocity": -0.4049184315666644, + "velocityX": 1.5619233249733904, + "velocityY": 0.21417247051907112, + "timestamp": 5.261763558475458 + }, + { + "x": 3.1233922684020508, + "y": 6.944780361828343, + "heading": 0.3876471304183033, + "angularVelocity": -0.4699960955410605, + "velocityX": 1.8745306661191443, + "velocityY": 0.2570656358357964, + "timestamp": 5.334182737349413 + }, + { + "x": 3.281790863806145, + "y": 6.966505170067396, + "heading": 0.34957966674470664, + "angularVelocity": -0.5256544504578377, + "velocityX": 2.1872464983314113, + "velocityY": 0.2999869451276964, + "timestamp": 5.406601916223368 + }, + { + "x": 3.4628451453091853, + "y": 6.9913406807024, + "heading": 0.3084466318231206, + "angularVelocity": -0.5679853812368929, + "velocityX": 2.5000874674119666, + "velocityY": 0.34294106921911044, + "timestamp": 5.479021095097322 + }, + { + "x": 3.6665642046771225, + "y": 7.019289526069962, + "heading": 0.26572282825309695, + "angularVelocity": -0.5899515050341022, + "velocityX": 2.813053980113594, + "velocityY": 0.3859315419221543, + "timestamp": 5.551440273971277 + }, + { + "x": 3.89294996939766, + "y": 7.050353833764572, + "heading": 0.2239583223824598, + "angularVelocity": -0.576705045818428, + "velocityX": 3.126047108523004, + "velocityY": 0.42895139350692046, + "timestamp": 5.623859452845232 + }, + { + "x": 4.141945383394743, + "y": 7.084529171934603, + "heading": 0.18873600956210052, + "angularVelocity": -0.48636719399516315, + "velocityX": 3.4382523782885963, + "velocityY": 0.4719100478826658, + "timestamp": 5.696278631719187 + }, + { + "x": 4.412342843336488, + "y": 7.12169996051531, + "heading": 0.1851319702648993, + "angularVelocity": -0.049766365115434155, + "velocityX": 3.73378246130586, + "velocityY": 0.5132727153038038, + "timestamp": 5.7686978105931415 + }, + { + "x": 4.683688666421851, + "y": 7.1589163386875585, + "heading": 0.18513196457429595, + "angularVelocity": -7.857867829794732e-8, + "velocityX": 3.7468779307431594, + "velocityY": 0.5139022390328953, + "timestamp": 5.841116989467096 + }, + { + "x": 4.955034489702315, + "y": 7.196132715437329, + "heading": 0.18513195888369474, + "angularVelocity": -7.857864847824122e-8, + "velocityX": 3.7468779334372075, + "velocityY": 0.513902219390594, + "timestamp": 5.913536168341051 + }, + { + "x": 5.226380313001078, + "y": 7.233349092053675, + "heading": 0.18513195319309358, + "angularVelocity": -7.857864765970384e-8, + "velocityX": 3.7468779336898987, + "velocityY": 0.5139022175482143, + "timestamp": 5.985955347215006 + }, + { + "x": 5.497726136146309, + "y": 7.270565469789434, + "heading": 0.1851319475024924, + "angularVelocity": -7.857864817392824e-8, + "velocityX": 3.7468779315698426, + "velocityY": 0.5139022330056134, + "timestamp": 6.0583745260889605 + }, + { + "x": 5.769071959711075, + "y": 7.307781844466341, + "heading": 0.18513194181189122, + "angularVelocity": -7.85786487043621e-8, + "velocityX": 3.7468779373630063, + "velocityY": 0.5139021907674625, + "timestamp": 6.130793704962915 + }, + { + "x": 6.040417785833277, + "y": 7.3449982004969, + "heading": 0.18513193612128995, + "angularVelocity": -7.857864920895207e-8, + "velocityX": 3.746877972677337, + "velocityY": 0.5139019332894442, + "timestamp": 6.20321288383687 + }, + { + "x": 6.311959616349093, + "y": 7.380756398600711, + "heading": 0.18513193002169934, + "angularVelocity": -8.42261776360047e-8, + "velocityX": 3.7495844987200435, + "velocityY": 0.4937669642188043, + "timestamp": 6.275632062710825 + }, + { + "x": 6.569581422501917, + "y": 7.404082737536551, + "heading": 0.1318031871794107, + "angularVelocity": -0.7363897750774955, + "velocityX": 3.557369886797705, + "velocityY": 0.3221016766351085, + "timestamp": 6.34805124158478 + }, + { + "x": 6.804636460196215, + "y": 7.424191087588351, + "heading": 0.0864226666877527, + "angularVelocity": -0.6266367721545483, + "velocityX": 3.245756736670675, + "velocityY": 0.2776660874158541, + "timestamp": 6.420470420458734 + }, + { + "x": 7.017122434869165, + "y": 7.4411416449585746, + "heading": 0.04927300576152058, + "angularVelocity": -0.5129809741545235, + "velocityX": 2.934111902080261, + "velocityY": 0.23406171726589448, + "timestamp": 6.492889599332689 + }, + { + "x": 7.207038680431131, + "y": 7.454954351507805, + "heading": 0.020449488415145097, + "angularVelocity": -0.398009447145798, + "velocityX": 2.6224578697932155, + "velocityY": 0.19073271423404176, + "timestamp": 6.565308778206644 + }, + { + "x": 7.374384880065918, + "y": 7.465639114379883, + "heading": 6.042705221121846e-21, + "angularVelocity": -0.28237669541568916, + "velocityX": 2.31079946275077, + "velocityY": 0.14754051396626502, + "timestamp": 6.637727957080599 + }, + { + "x": 7.495692822755469, + "y": 7.4722697459987435, + "heading": -0.0110806824345206, + "angularVelocity": -0.18789382846574634, + "velocityX": 2.0570045130283727, + "velocityY": 0.11243484030663832, + "timestamp": 6.696701061865361 + }, + { + "x": 7.601999179168005, + "y": 7.476911827606212, + "heading": -0.01772644697635795, + "angularVelocity": -0.11269144750124323, + "velocityX": 1.8026243793764762, + "velocityY": 0.07871523170454242, + "timestamp": 6.755674166650123 + }, + { + "x": 7.693282254298601, + "y": 7.479626196867593, + "heading": -0.020765685795326113, + "angularVelocity": -0.05153601510486283, + "velocityX": 1.5478763660783506, + "velocityY": 0.046027240235815677, + "timestamp": 6.814647271434885 + }, + { + "x": 7.769527501259725, + "y": 7.480459824413743, + "heading": -0.02082974288304602, + "angularVelocity": -0.0010862085005308956, + "velocityX": 1.292881682919731, + "velocityY": 0.014135724228753763, + "timestamp": 6.873620376219647 + }, + { + "x": 7.8307246815334155, + "y": 7.479450038620926, + "heading": -0.018416174457310776, + "angularVelocity": 0.040926595853214634, + "velocityX": 1.0377133864164767, + "velocityY": -0.0171228188935137, + "timestamp": 6.932593481004409 + }, + { + "x": 7.876866310893982, + "y": 7.476627203358953, + "heading": -0.013927394073716237, + "angularVelocity": 0.07611572088628399, + "velocityX": 0.7824181807787326, + "velocityY": -0.047866485447466633, + "timestamp": 6.991566585789172 + }, + { + "x": 7.907946749436215, + "y": 7.472016493094702, + "heading": -0.0076956641130537864, + "angularVelocity": 0.10567071181696776, + "velocityX": 0.5270273399318175, + "velocityY": -0.07818327152825819, + "timestamp": 7.050539690573934 + }, + { + "x": 7.923961639404297, + "y": 7.465639114379883, + "heading": -3.0160599173944472e-21, + "angularVelocity": 0.13049447101591694, + "velocityX": 0.27156260513215996, + "velocityY": -0.10814046060649564, + "timestamp": 7.109512795358696 + }, + { + "x": 7.911903030236992, + "y": 7.451294465353008, + "heading": 0.015126055505390899, + "angularVelocity": 0.1631760399586222, + "velocityX": -0.1300852089712522, + "velocityY": -0.1547464255943911, + "timestamp": 7.202210568323566 + }, + { + "x": 7.862610962703899, + "y": 7.432637923876487, + "heading": 0.03315483702979202, + "angularVelocity": 0.19448991003520194, + "velocityX": -0.531750288669542, + "velocityY": -0.20126202474780283, + "timestamp": 7.294908341288436 + }, + { + "x": 7.776083443224882, + "y": 7.4096801426109025, + "heading": 0.05392541642359817, + "angularVelocity": 0.22406772816082293, + "velocityX": -0.9334368746033223, + "velocityY": -0.2476627057079841, + "timestamp": 7.387606114253306 + }, + { + "x": 7.652317922263172, + "y": 7.3824351254025355, + "heading": 0.07722667884981763, + "angularVelocity": 0.25136809311535513, + "velocityX": -1.3351509642913992, + "velocityY": -0.2939123167359359, + "timestamp": 7.480303887218176 + }, + { + "x": 7.49131102887938, + "y": 7.3509221102471285, + "heading": 0.10276916611506416, + "angularVelocity": 0.2755458566941678, + "velocityX": -1.736901418816265, + "velocityY": -0.3399543931583946, + "timestamp": 7.5730016601830465 + }, + { + "x": 7.293058112506846, + "y": 7.315169193574846, + "heading": 0.13013085609030867, + "angularVelocity": 0.29517095287298756, + "velocityX": -2.138702042471583, + "velocityY": -0.3856933724376746, + "timestamp": 7.665699433147917 + }, + { + "x": 7.057552405050112, + "y": 7.2752213128681245, + "heading": 0.15863793525795974, + "angularVelocity": 0.30752712018717576, + "velocityX": -2.5405756786194313, + "velocityY": -0.4309475775848565, + "timestamp": 7.758397206112787 + }, + { + "x": 6.784783507993426, + "y": 7.231161947944703, + "heading": 0.18704197948975648, + "angularVelocity": 0.30641560550285446, + "velocityX": -2.9425614913106823, + "velocityY": -0.47530122368871824, + "timestamp": 7.851094979077657 + }, + { + "x": 6.474737606760309, + "y": 7.183200115705696, + "heading": 0.2122345768516852, + "angularVelocity": 0.2717713334005995, + "velocityX": -3.3446963321396703, + "velocityY": -0.5174000486201892, + "timestamp": 7.943792752042527 + }, + { + "x": 6.12780389314354, + "y": 7.132776434801359, + "heading": 0.21223458876196108, + "angularVelocity": 1.284850268489834e-7, + "velocityX": -3.74263267088681, + "velocityY": -0.5439578459284719, + "timestamp": 8.036490525007396 + }, + { + "x": 5.779638248368889, + "y": 7.091712595403529, + "heading": 0.21223459040404852, + "angularVelocity": 1.7714421734571762e-8, + "velocityX": -3.755922430915314, + "velocityY": -0.442986256135747, + "timestamp": 8.129188297972266 + }, + { + "x": 5.431472594680733, + "y": 7.050648831580209, + "heading": 0.21223459204613604, + "angularVelocity": 1.77144224789278e-8, + "velocityX": -3.7559225270719514, + "velocityY": -0.4429854408571672, + "timestamp": 8.221886070937137 + }, + { + "x": 5.083306940982422, + "y": 7.009585067843037, + "heading": 0.21223459368823233, + "angularVelocity": 1.771451744649189e-8, + "velocityX": -3.7559225271815024, + "velocityY": -0.4429854399278211, + "timestamp": 8.314583843902007 + }, + { + "x": 4.748349644408739, + "y": 6.970085293939387, + "heading": 0.249149110447713, + "angularVelocity": 0.3982244187621458, + "velocityX": -3.6134341296486574, + "velocityY": -0.42611351535511016, + "timestamp": 8.407281616866877 + }, + { + "x": 4.450609827039412, + "y": 6.934974591277922, + "heading": 0.28197417031428224, + "angularVelocity": 0.35410839782536235, + "velocityX": -3.2119414290800963, + "velocityY": -0.37876533101577264, + "timestamp": 8.499979389831747 + }, + { + "x": 4.190087491180257, + "y": 6.904252880091878, + "heading": 0.31070546232875873, + "angularVelocity": 0.3099458713572863, + "velocityX": -2.8104487036369967, + "velocityY": -0.3314180071800382, + "timestamp": 8.592677162796617 + }, + { + "x": 3.9667826341532617, + "y": 6.877920092869689, + "heading": 0.33533894531800695, + "angularVelocity": 0.2657397497411686, + "velocityX": -2.408956007083604, + "velocityY": -0.2840714116418766, + "timestamp": 8.685374935761487 + }, + { + "x": 3.780695252616961, + "y": 6.85597617430768, + "heading": 0.3558710467629364, + "angularVelocity": 0.22149508869766035, + "velocityX": -2.0074633465770906, + "velocityY": -0.23672541270570108, + "timestamp": 8.778072708726357 + }, + { + "x": 3.6318253440600605, + "y": 6.8384210809873816, + "heading": 0.37229886580171084, + "angularVelocity": 0.17721913389440508, + "velocityX": -1.6059707131617713, + "velocityY": -0.18937988215695278, + "timestamp": 8.870770481691228 + }, + { + "x": 3.5201729076832478, + "y": 6.82525478102506, + "heading": 0.38462033829713027, + "angularVelocity": 0.13292091170398426, + "velocityX": -1.2044780883692483, + "velocityY": -0.1420346955617933, + "timestamp": 8.963468254656098 + }, + { + "x": 3.4457379449845735, + "y": 6.816477253761962, + "heading": 0.39283435937679306, + "angularVelocity": 0.08861077043108337, + "velocityX": -0.802985447416123, + "velocityY": -0.09468973182801316, + "timestamp": 9.056166027620968 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": 0.044299923200563064, + "velocityX": -0.4014927615347733, + "velocityY": -0.04734487236445959, + "timestamp": 9.148863800585838 + }, + { + "x": 3.408520460128784, + "y": 6.812088489532471, + "heading": 0.3969408636, + "angularVelocity": -1.6537958542474343e-22, + "velocityX": -4.688105571426993e-23, + "velocityY": -2.4487756057177513e-23, + "timestamp": 9.241561573550708 + }, + { + "x": 3.442803441142074, + "y": 6.8114279191425675, + "heading": 0.392498149134446, + "angularVelocity": -0.05010039849219718, + "velocityX": 0.38660846281780004, + "velocityY": -0.007449238528134429, + "timestamp": 9.330237803665844 + }, + { + "x": 3.511369395408112, + "y": 6.810106800490753, + "heading": 0.38361321030304085, + "angularVelocity": -0.10019527014025122, + "velocityX": 0.773216838120132, + "velocityY": -0.01489822751935824, + "timestamp": 9.418914033780979 + }, + { + "x": 3.6142183227587346, + "y": 6.808125168158371, + "heading": 0.3702881620154029, + "angularVelocity": -0.1502662919965921, + "velocityX": 1.1598252115260796, + "velocityY": -0.02234682653748938, + "timestamp": 9.507590263896114 + }, + { + "x": 3.751350230137559, + "y": 6.805483069997971, + "heading": 0.3525266232817515, + "angularVelocity": -0.2002965023500672, + "velocityX": 1.5464336632350622, + "velocityY": -0.02979488592342862, + "timestamp": 9.59626649401125 + }, + { + "x": 3.922765130885179, + "y": 6.8021805681387715, + "heading": 0.3303335458268625, + "angularVelocity": -0.2502708722063848, + "velocityX": 1.9330422653856434, + "velocityY": -0.03724224467945297, + "timestamp": 9.684942724126385 + }, + { + "x": 4.128463043748926, + "y": 6.7982177401202675, + "heading": 0.30371498826898996, + "angularVelocity": -0.3001769191508424, + "velocityX": 2.3196510789494917, + "velocityY": -0.04468872902421391, + "timestamp": 9.77361895424152 + }, + { + "x": 4.36844399152108, + "y": 6.793594680078811, + "heading": 0.2726778324022649, + "angularVelocity": -0.3500053602462237, + "velocityX": 2.7062601495413983, + "velocityY": -0.05213415179529137, + "timestamp": 9.862295184356656 + }, + { + "x": 4.642707998965045, + "y": 6.788311499898704, + "heading": 0.23722944053291425, + "angularVelocity": -0.39975077676763104, + "velocityX": 3.0928694993896952, + "velocityY": -0.05957831284943316, + "timestamp": 9.950971414471791 + }, + { + "x": 4.951255087719671, + "y": 6.782368330248145, + "heading": 0.19737726949719422, + "angularVelocity": -0.4494121027018933, + "velocityX": 3.4794790932588646, + "velocityY": -0.06702100036100427, + "timestamp": 10.039647644586926 + }, + { + "x": 5.286562691971156, + "y": 6.775920768512488, + "heading": 0.19737726761878174, + "angularVelocity": -2.1182818612377388e-8, + "velocityX": 3.781256869130857, + "velocityY": -0.0727090193988374, + "timestamp": 10.128323874702062 + }, + { + "x": 5.59511113537339, + "y": 6.769985792808053, + "heading": 0.15789277163023122, + "angularVelocity": -0.44526583885314686, + "velocityX": 3.4794943695917215, + "velocityY": -0.06692859740122076, + "timestamp": 10.217000104817197 + }, + { + "x": 5.869376453129913, + "y": 6.764710551221358, + "heading": 0.12280116553958556, + "angularVelocity": -0.3957273109725502, + "velocityX": 3.092884275757129, + "velocityY": -0.05948878949687072, + "timestamp": 10.305676334932333 + }, + { + "x": 6.1093586237611985, + "y": 6.760094935517236, + "heading": 0.0920991922571767, + "angularVelocity": -0.34622551322429906, + "velocityX": 2.7062739396983573, + "velocityY": -0.0520502021582253, + "timestamp": 10.394352565047468 + }, + { + "x": 6.315057635592866, + "y": 6.756138851805421, + "heading": 0.06578475722782369, + "angularVelocity": -0.2967473357311971, + "velocityX": 2.3196634719878233, + "velocityY": -0.04461267361815281, + "timestamp": 10.483028795162603 + }, + { + "x": 6.486473481982027, + "y": 6.752842221237321, + "heading": 0.04385663735915925, + "angularVelocity": -0.24728295102524525, + "velocityX": 1.9330529293656087, + "velocityY": -0.037176034252009674, + "timestamp": 10.571705025277739 + }, + { + "x": 6.623606159513812, + "y": 6.750204980424608, + "heading": 0.026314217868262327, + "angularVelocity": -0.197825499213489, + "velocityX": 1.5464423482339522, + "velocityY": -0.02974010971473329, + "timestamp": 10.660381255392874 + }, + { + "x": 6.726455666874299, + "y": 6.748227081682736, + "heading": 0.013157277625411766, + "angularVelocity": -0.14837054107699252, + "velocityX": 1.1598317522852502, + "velocityY": -0.02230472291507881, + "timestamp": 10.74905748550801 + }, + { + "x": 6.795022004077355, + "y": 6.7469084931623255, + "heading": 0.004385827059285361, + "angularVelocity": -0.09891546533651373, + "velocityX": 0.7732211564928945, + "velocityY": -0.014869695280220821, + "timestamp": 10.837733715623145 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -2.734177125091434e-26, + "angularVelocity": -0.049458880396594275, + "velocityX": 0.38661057021351825, + "velocityY": -0.007434847510947748, + "timestamp": 10.92640994573828 + }, + { + "x": 6.829305171966553, + "y": 6.746249198913574, + "heading": -1.3407189209020893e-26, + "angularVelocity": 6.223794221869343e-27, + "velocityX": 3.782126331053824e-27, + "velocityY": 9.545461702862487e-27, + "timestamp": 11.015086175853416 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.1.traj b/src/main/deploy/choreo/CenterLanePCBAFE.1.traj index 9119888f..54e8c43e 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.1.traj @@ -1,292 +1,311 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -8.364664059409844e-30, - "velocityX": -6.773677606947568e-30, - "velocityY": -2.308132540143278e-30, - "timestamp": 0 - }, - { - "x": 1.3079899492454155, - "y": 5.5701428985236205, - "heading": -0.011966271801548406, - "angularVelocity": -0.16457159783011938, - "velocityX": 0.24776633609486393, - "velocityY": -0.2862183198381729, - "timestamp": 0.07271164623703327 - }, - { - "x": 1.3440211933652915, - "y": 5.528520469164234, - "heading": -0.03588810704483216, - "angularVelocity": -0.328995924069994, - "velocityX": 0.49553607960947427, - "velocityY": -0.5724313987522216, - "timestamp": 0.14542329247406655 - }, - { - "x": 1.398068831056723, - "y": 5.466087418740183, - "heading": -0.07173813692543105, - "angularVelocity": -0.49304384837450815, - "velocityX": 0.7433147299769343, - "velocityY": -0.8586389341691013, - "timestamp": 0.21813493871109982 - }, - { - "x": 1.4701339496615213, - "y": 5.382844295736326, - "heading": -0.11947590052159912, - "angularVelocity": -0.6565353154432494, - "velocityX": 0.9911083345055308, - "velocityY": -1.1448389262999967, - "timestamp": 0.2908465849481331 - }, - { - "x": 1.5602181142824867, - "y": 5.2787919544216955, - "heading": -0.17905308310612672, - "angularVelocity": -0.819362312243441, - "velocityX": 1.2389234638197153, - "velocityY": -1.431027169709696, - "timestamp": 0.3635582311851664 - }, - { - "x": 1.668323429942649, - "y": 5.153931763984036, - "heading": -0.25042016061718175, - "angularVelocity": -0.9815082068514337, - "velocityX": 1.4867675434162453, - "velocityY": -1.717196582745939, - "timestamp": 0.4362698774221997 - }, - { - "x": 1.7944527928954905, - "y": 5.008265979195592, - "heading": -0.33353344500312104, - "angularVelocity": -1.1430532616554276, - "velocityX": 1.7346514550472283, - "velocityY": -2.003334986008902, - "timestamp": 0.508981523659233 - }, - { - "x": 1.938611553409707, - "y": 4.8417992681651185, - "heading": -0.42836015839962444, - "angularVelocity": -1.3041475239812899, - "velocityX": 1.98260894865471, - "velocityY": -2.2894091890060286, - "timestamp": 0.5816931698962663 - }, - { - "x": 2.09250704020051, - "y": 4.700832043022275, - "heading": -0.5059805202146961, - "angularVelocity": -1.0675093444479318, - "velocityX": 2.116517707491784, - "velocityY": -1.9387159062918606, - "timestamp": 0.6544048161332996 - }, - { - "x": 2.228385575629257, - "y": 4.5806756631537535, - "heading": -0.5718968876072178, - "angularVelocity": -0.906544835648882, - "velocityX": 1.8687313857908063, - "velocityY": -1.652505287385456, - "timestamp": 0.7271164623703329 - }, - { - "x": 2.346242849494598, - "y": 4.481326236786965, - "heading": -0.6261128487399352, - "angularVelocity": -0.7456296746751315, - "velocityX": 1.6208857861127495, - "velocityY": -1.3663481918284743, - "timestamp": 0.7998281086073662 - }, - { - "x": 2.446077177803767, - "y": 4.402782122456306, - "heading": -0.6686283268670905, - "angularVelocity": -0.5847134582152551, - "velocityX": 1.3730170265673516, - "velocityY": -1.080213671266156, - "timestamp": 0.8725397548443995 - }, - { - "x": 2.5278876403134056, - "y": 4.3450423723526415, - "heading": -0.6994420265312672, - "angularVelocity": -0.423779425385222, - "velocityX": 1.12513561096113, - "velocityY": -0.7940921858762265, - "timestamp": 0.9452514010814328 - }, - { - "x": 2.591673696422735, - "y": 4.308106404528037, - "heading": -0.718552253950641, - "angularVelocity": -0.2628220980484028, - "velocityX": 0.8772467604995492, - "velocityY": -0.5079787039876672, - "timestamp": 1.017963047318466 - }, - { - "x": 2.637435055651834, - "y": 4.2919738867991235, - "heading": -0.7259572155509334, - "angularVelocity": -0.10184010376417504, - "velocityX": 0.6293539150825374, - "velocityY": -0.22186979060047113, - "timestamp": 1.0906746935554992 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.05916659407406203, - "velocityX": 0.38145976628941564, - "velocityY": 0.06423731409121712, - "timestamp": 1.1633863397925324 - }, - { - "x": 2.6747076115982096, - "y": 4.323065260999168, - "heading": -0.7050775877835139, - "angularVelocity": 0.2234613634683759, - "velocityX": 0.12854299211755085, - "velocityY": 0.35614342430781004, - "timestamp": 1.237571545918339 - }, - { - "x": 2.6654811031724366, - "y": 4.37114137531458, - "heading": -0.6763165724415564, - "angularVelocity": 0.38769205939503093, - "velocityX": -0.1243712717061632, - "velocityY": 0.6480552770879449, - "timestamp": 1.3117567520441455 - }, - { - "x": 2.6374924128586628, - "y": 4.440873637695316, - "heading": -0.6353766207501428, - "angularVelocity": 0.5518613996935385, - "velocityX": -0.3772812906679928, - "velocityY": 0.9399753133925751, - "timestamp": 1.3859419581699521 - }, - { - "x": 2.590742000006679, - "y": 4.532262849422754, - "heading": -0.5822609370446827, - "angularVelocity": 0.7159875462663353, - "velocityX": -0.6301851176982644, - "velocityY": 1.231906150790299, - "timestamp": 1.4601271642957587 - }, - { - "x": 2.525230488767617, - "y": 4.645309988591984, - "heading": -0.5169687243847326, - "angularVelocity": 0.8801244354174206, - "velocityX": -0.8830805313123746, - "velocityY": 1.5238501726898892, - "timestamp": 1.5343123704215653 - }, - { - "x": 2.4409586345992826, - "y": 4.780016081558229, - "heading": -0.4394900359726193, - "angularVelocity": 1.0443954051766633, - "velocityX": -1.1359657615273402, - "velocityY": 1.8158080296257462, - "timestamp": 1.608497576547372 - }, - { - "x": 2.337926784653071, - "y": 4.936381539754754, - "heading": -0.3497973159677186, - "angularVelocity": 1.2090378215045976, - "velocityX": -1.3888463122287895, - "velocityY": 2.107771432581833, - "timestamp": 1.6826827826731785 - }, - { - "x": 2.2066781223966365, - "y": 5.088037063238321, - "heading": -0.26257787440108193, - "angularVelocity": 1.1756985809634768, - "velocityX": -1.7692026362756765, - "velocityY": 2.044282565348452, - "timestamp": 1.756867988798985 - }, - { - "x": 2.094180452686652, - "y": 5.218026595083209, - "heading": -0.18768356705291486, - "angularVelocity": 1.009558526050363, - "velocityX": -1.5164434471606913, - "velocityY": 1.7522298398549971, - "timestamp": 1.8310531949247917 - }, - { - "x": 2.0004335399095625, - "y": 5.326350877049268, - "heading": -0.12517882612598913, - "angularVelocity": 0.8425499394477148, - "velocityX": -1.2636874340540831, - "velocityY": 1.460187113126797, - "timestamp": 1.9052384010505983 - }, - { - "x": 1.9254366143080366, - "y": 5.4130101547737155, - "heading": -0.07512185913342963, - "angularVelocity": 0.6747567285776663, - "velocityX": -1.0109417971010437, - "velocityY": 1.1681476975361738, - "timestamp": 1.9794236071764049 - }, - { - "x": 1.869189070291752, - "y": 5.478004602849777, - "heading": -0.037557912625022415, - "angularVelocity": 0.5063536043287745, - "velocityX": -0.7582043233686032, - "velocityY": 0.876110635424555, - "timestamp": 2.0536088133022115 - }, - { - "x": 1.8316905948823166, - "y": 5.5213343174096465, - "heading": -0.012514751718529437, - "angularVelocity": 0.3375762124977515, - "velocityX": -0.5054710685139713, - "velocityY": 0.5840748691542834, - "timestamp": 2.127794019428018 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.368719711095728e-29, - "angularVelocity": 0.1686960564308085, - "velocityX": -0.2527377395078362, - "velocityY": 0.2920386866937519, - "timestamp": 2.2019792255538246 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 1.1956060311838367e-29, - "angularVelocity": -1.025164181333131e-28, - "velocityX": 1.21885672142624e-30, - "velocityY": -5.1343833235782786e-30, - "timestamp": 2.2761644316796312 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -1.0042344276654815e-24, + "angularVelocity": -6.621110938678157e-25, + "velocityX": -2.249108899033261e-24, + "velocityY": -2.0825834551113612e-24, + "timestamp": 0 + }, + { + "x": 1.3060567679582313, + "y": 5.572381918180444, + "heading": -0.010695335923114668, + "angularVelocity": -0.14212952691547795, + "velocityX": 0.2137167180313357, + "velocityY": -0.24680705612748235, + "timestamp": 0.07525062634926666 + }, + { + "x": 1.3382215476730719, + "y": 5.535237409026599, + "heading": -0.03207750843931141, + "angularVelocity": -0.2841461068636689, + "velocityX": 0.4274353752957696, + "velocityY": -0.49361063097924024, + "timestamp": 0.15050125269853332 + }, + { + "x": 1.3864692368556266, + "y": 5.479521033340712, + "heading": -0.06412446500663382, + "angularVelocity": -0.4258696322151573, + "velocityX": 0.6411599680063655, + "velocityY": -0.7404107897692007, + "timestamp": 0.22575187904779997 + }, + { + "x": 1.4508006060735943, + "y": 5.40523312636478, + "heading": -0.10680311296182225, + "angularVelocity": -0.5671533916156479, + "velocityX": 0.8548948007340368, + "velocityY": -0.987206493553073, + "timestamp": 0.30100250539706663 + }, + { + "x": 1.5312167667507302, + "y": 5.312374207143024, + "heading": -0.16007302531493992, + "angularVelocity": -0.7078999197411572, + "velocityX": 1.0686444030896685, + "velocityY": -1.2339952997967512, + "timestamp": 0.3762531317463333 + }, + { + "x": 1.6277191879065078, + "y": 5.200945106889031, + "heading": -0.22389124150432238, + "angularVelocity": -0.8480755481446488, + "velocityX": 1.2824135271362858, + "velocityY": -1.480773060104614, + "timestamp": 0.45150375809559995 + }, + { + "x": 1.7403097782501336, + "y": 5.070947160382142, + "heading": -0.2982176402551873, + "angularVelocity": -0.9877180078992028, + "velocityX": 1.496208015878168, + "velocityY": -1.7275330826287059, + "timestamp": 0.5267543844448666 + }, + { + "x": 1.8689915501245091, + "y": 4.922382870770249, + "heading": -0.3830195665066897, + "angularVelocity": -1.126926516968836, + "velocityX": 1.7100425354217623, + "velocityY": -1.9742598410058476, + "timestamp": 0.6020050107941333 + }, + { + "x": 2.021815501878503, + "y": 4.778590249332989, + "heading": -0.46245356659644804, + "angularVelocity": -1.0555925437892708, + "velocityX": 2.0308661757136712, + "velocityY": -1.9108494960541986, + "timestamp": 0.6772556371434 + }, + { + "x": 2.158552321551595, + "y": 4.6533669501943, + "heading": -0.5314334875140496, + "angularVelocity": -0.916669059968213, + "velocityX": 1.8170854690091889, + "velocityY": -1.6640831473944147, + "timestamp": 0.7525062634926667 + }, + { + "x": 2.279199772550662, + "y": 4.546710847450061, + "heading": -0.589963647479443, + "angularVelocity": -0.7778029606522173, + "velocityX": 1.6032750403843345, + "velocityY": -1.417345049717031, + "timestamp": 0.8277568898419334 + }, + { + "x": 2.383756839213222, + "y": 4.45862085446933, + "heading": -0.6380444617813997, + "angularVelocity": -0.6389423800779483, + "velocityX": 1.38945111469599, + "velocityY": -1.1706213921977682, + "timestamp": 0.9030075161912001 + }, + { + "x": 2.4722229084949126, + "y": 4.389096261559967, + "heading": -0.67567478001119, + "angularVelocity": -0.5000665118072721, + "velocityX": 1.1756190423065196, + "velocityY": -0.9239071657247342, + "timestamp": 0.9782581425404668 + }, + { + "x": 2.544597589718017, + "y": 4.338136588288561, + "heading": -0.7028528784398662, + "angularVelocity": -0.36116773703028543, + "velocityX": 0.9617817782298118, + "velocityY": -0.6771993236957614, + "timestamp": 1.0535087688897335 + }, + { + "x": 2.6008806504487216, + "y": 4.305741524929451, + "heading": -0.7195768777220212, + "angularVelocity": -0.22224398777137738, + "velocityX": 0.7479414253573617, + "velocityY": -0.4304955975881504, + "timestamp": 1.1287593952390003 + }, + { + "x": 2.6410719892543444, + "y": 4.2919109040687715, + "heading": -0.7258449001014369, + "angularVelocity": -0.08329528514916955, + "velocityX": 0.5340997245535138, + "velocityY": -0.1837940962309967, + "timestamp": 1.204010021588267 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.055677742612701135, + "velocityX": 0.3202582509251215, + "velocityY": 0.06290689943025622, + "timestamp": 1.2792606479375337 + }, + { + "x": 2.6731116736052027, + "y": 4.320290744259811, + "heading": -0.7067971794351253, + "angularVelocity": 0.19582512759313456, + "velocityX": 0.10464854696510043, + "velocityY": 0.31165110401848706, + "timestamp": 1.355134136920331 + }, + { + "x": 2.664692786668951, + "y": 4.362810218152233, + "heading": -0.6813099274422239, + "angularVelocity": 0.3359177538109565, + "velocityX": -0.1109595334170119, + "velocityY": 0.5603996133888316, + "timestamp": 1.4310076259031281 + }, + { + "x": 2.639915179239627, + "y": 4.424203570363852, + "heading": -0.6451974716199608, + "angularVelocity": 0.47595617792732364, + "velocityX": -0.3265647561685457, + "velocityY": 0.8091541991108215, + "timestamp": 1.5068811148859254 + }, + { + "x": 2.59877917099983, + "y": 4.50447140700047, + "heading": -0.59846321314625, + "angularVelocity": 0.6159497750829267, + "velocityX": -0.5421657655564515, + "velocityY": 1.0579167732067494, + "timestamp": 1.5827546038687226 + }, + { + "x": 2.5412852001349884, + "y": 4.6036144764177696, + "heading": -0.541108297539196, + "angularVelocity": 0.755928274499912, + "velocityX": -0.7577609997330812, + "velocityY": 1.3066892105064276, + "timestamp": 1.6586280928515198 + }, + { + "x": 2.4674338294971827, + "y": 4.7216336217313835, + "heading": -0.4731286056147784, + "angularVelocity": 0.8959610640790578, + "velocityX": -0.973348815612657, + "velocityY": 1.5554727599303189, + "timestamp": 1.734501581834317 + }, + { + "x": 2.3772256595733805, + "y": 4.858529596063364, + "heading": -0.3945096344591076, + "angularVelocity": 1.0361849996577348, + "velocityX": -1.1889287171735974, + "velocityY": 1.8042662353779348, + "timestamp": 1.8103750708171142 + }, + { + "x": 2.270660565338228, + "y": 5.014302224131279, + "heading": -0.305219034790845, + "angularVelocity": 1.1768352933988244, + "velocityX": -1.4045102665479658, + "velocityY": 2.0530574006321625, + "timestamp": 1.8862485597999115 + }, + { + "x": 2.156230166958197, + "y": 5.146478435153338, + "heading": -0.2290932542035935, + "angularVelocity": 1.0033251615002383, + "velocityX": -1.5081736705949598, + "velocityY": 1.7420605377990246, + "timestamp": 1.9621220487827087 + }, + { + "x": 2.05814700584277, + "y": 5.259770794344292, + "heading": -0.16373664121599296, + "angularVelocity": 0.8613893187700765, + "velocityX": -1.292719794889953, + "velocityY": 1.4931745028443373, + "timestamp": 2.037995537765506 + }, + { + "x": 1.976411579927544, + "y": 5.3541805242021825, + "heading": -0.10920088428004432, + "angularVelocity": 0.7187722308158727, + "velocityX": -1.077259356476389, + "velocityY": 1.2443045802110966, + "timestamp": 2.113869026748303 + }, + { + "x": 1.9110235357975711, + "y": 5.4297080461749685, + "heading": -0.06553152377248019, + "angularVelocity": 0.5755549282499098, + "velocityX": -0.8618035760132025, + "velocityY": 0.9954402121920332, + "timestamp": 2.1897425157311 + }, + { + "x": 1.8619825316383047, + "y": 5.486353607136737, + "heading": -0.03276339241185007, + "angularVelocity": 0.43187853623101025, + "velocityX": -0.6463523006090516, + "velocityY": 0.7465790979324988, + "timestamp": 2.265616004713897 + }, + { + "x": 1.829288400520471, + "y": 5.524117342639773, + "heading": -0.010917643304867627, + "angularVelocity": 0.2879233497741951, + "velocityX": -0.43090322530503955, + "velocityY": 0.49771977022960984, + "timestamp": 2.341489493696694 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 5.072711091682045e-24, + "angularVelocity": 0.14389272789792218, + "velocityX": -0.21545347602968715, + "velocityY": 0.24886063882779058, + "timestamp": 2.417362982679491 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 2.471925946464062e-24, + "angularVelocity": -1.3881126396060902e-24, + "velocityX": -6.902910153275516e-22, + "velocityY": 3.1112652084354562e-24, + "timestamp": 2.493236471662288 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.2.traj b/src/main/deploy/choreo/CenterLanePCBAFE.2.traj index e6682c02..c5fcbaa6 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.2.traj @@ -1,94 +1,104 @@ { - "samples": [ - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 1.1956060311838367e-29, - "angularVelocity": -1.025164181333131e-28, - "velocityX": 1.21885672142624e-30, - "velocityY": -5.1343833235782786e-30, - "timestamp": 0 - }, - { - "x": 1.8602093956334294, - "y": 5.5430660724686875, - "heading": -1.6999094202020734e-20, - "angularVelocity": -1.7890242544799916e-19, - "velocityX": 0.49746156397034486, - "velocityY": 0.0007030702226192673, - "timestamp": 0.09501880240866223 - }, - { - "x": 1.9547457978922331, - "y": 5.543199682247202, - "heading": -3.210902716840507e-20, - "angularVelocity": -1.5902045262092918e-19, - "velocityX": 0.9949231085045297, - "velocityY": 0.001406140417769105, - "timestamp": 0.19003760481732446 - }, - { - "x": 2.0965503966634373, - "y": 5.543400096908449, - "heading": -4.532979164623425e-20, - "angularVelocity": -1.3913840358532023e-19, - "velocityX": 1.492384614166397, - "velocityY": 0.0021092105579800877, - "timestamp": 0.2850564072259867 - }, - { - "x": 2.2856231808662413, - "y": 5.543667316436768, - "heading": -7.554638691710094e-20, - "angularVelocity": -3.1800648403053284e-19, - "velocityX": 1.9898460032113345, - "velocityY": 0.0028122805333745363, - "timestamp": 0.3800752096346489 - }, - { - "x": 2.474695965069045, - "y": 5.543934535965086, - "heading": -6.169486717931722e-20, - "angularVelocity": 1.457766188026957e-19, - "velocityX": 1.9898460032113345, - "velocityY": 0.002812280533374536, - "timestamp": 0.47509401204331114 - }, - { - "x": 2.6165005638402494, - "y": 5.544134950626333, - "heading": -3.084743805748469e-20, - "angularVelocity": 3.2464552635862006e-19, - "velocityX": 1.492384614166397, - "velocityY": 0.0021092105579800877, - "timestamp": 0.5701128144519734 - }, - { - "x": 2.711036966099053, - "y": 5.544268560404848, - "heading": -1.8891361886639493e-21, - "angularVelocity": 3.047639112965043e-19, - "velocityX": 0.9949231085045297, - "velocityY": 0.001406140417769105, - "timestamp": 0.6651316168606356 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -8.137968740525874e-34, - "angularVelocity": 1.9881709101649526e-20, - "velocityX": 0.49746156397034486, - "velocityY": 0.0007030702226192673, - "timestamp": 0.7601504192692978 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -8.137968740525874e-34, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -9.422453604168856e-36, - "timestamp": 0.8551692216779601 - } - ] + "samples": [ + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 2.471925946464062e-24, + "angularVelocity": -1.3881126396060902e-24, + "velocityX": -6.902910153275516e-22, + "velocityY": 3.1112652084354562e-24, + "timestamp": 0 + }, + { + "x": 1.8507557540076562, + "y": 5.5430527114888575, + "heading": -8.800046052500135e-19, + "angularVelocity": -9.452376611096758e-18, + "velocityX": 0.4061756726920047, + "velocityY": 0.0005740544422031257, + "timestamp": 0.09309902825151006 + }, + { + "x": 1.9263848743459357, + "y": 5.543159599309593, + "heading": -2.769921496163024e-18, + "angularVelocity": -2.0300071079064193e-17, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765763312, + "timestamp": 0.18619805650302013 + }, + { + "x": 2.0398285536670646, + "y": 5.54331993103902, + "heading": -5.903589755199969e-18, + "angularVelocity": -3.365951630081127e-17, + "velocityX": 1.2185269970236072, + "velocityY": 0.0017221632968556807, + "timestamp": 0.2792970847545302 + }, + { + "x": 2.191086789804772, + "y": 5.5435337066740775, + "heading": -1.0884334063348694e-17, + "angularVelocity": -5.34994231591497e-17, + "velocityX": 1.6247026309348636, + "velocityY": 0.0022962176842493674, + "timestamp": 0.37239611300604025 + }, + { + "x": 2.380159571927711, + "y": 5.543800926199458, + "heading": -4.173692512602447e-18, + "angularVelocity": 7.208068308331619e-17, + "velocityX": 2.0308781485038967, + "velocityY": 0.0028702719072147666, + "timestamp": 0.4654951412575503 + }, + { + "x": 2.5314178080654184, + "y": 5.544014701834515, + "heading": -1.8153111851912927e-18, + "angularVelocity": 2.5331965023903977e-17, + "velocityX": 1.6247026309348636, + "velocityY": 0.002296217684249368, + "timestamp": 0.5585941695090604 + }, + { + "x": 2.644861487386547, + "y": 5.544175033563942, + "heading": -7.161538717554587e-19, + "angularVelocity": 1.1806324234469696e-17, + "velocityX": 1.2185269970236072, + "velocityY": 0.001722163296855681, + "timestamp": 0.6516931977605704 + }, + { + "x": 2.7204906077248263, + "y": 5.544281921384678, + "heading": -1.8348593704837507e-19, + "angularVelocity": 5.72151981311354e-18, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765763316, + "timestamp": 0.7447922260120805 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -9.055003253623357e-30, + "angularVelocity": 1.970868445148275e-18, + "velocityX": 0.4061756726920047, + "velocityY": 0.0005740544422031258, + "timestamp": 0.8378912542635906 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.6159098195352036e-29, + "angularVelocity": -1.6705479392630998e-29, + "velocityX": 6.780641024803866e-22, + "velocityY": 9.718698194414986e-25, + "timestamp": 0.9309902825151006 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.3.traj b/src/main/deploy/choreo/CenterLanePCBAFE.3.traj index 61dbe9b5..25831f55 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.3.traj @@ -1,184 +1,194 @@ { - "samples": [ - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -8.137968740525874e-34, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -9.422453604168856e-36, - "timestamp": 0 - }, - { - "x": 2.744670053824415, - "y": 5.560420833392183, - "heading": 0.008590361313936763, - "angularVelocity": 0.13502733079351698, - "velocityX": -0.21432312628245512, - "velocityY": 0.2528389368381776, - "timestamp": 0.06361942625580808 - }, - { - "x": 2.717653230778774, - "y": 5.592802243922747, - "heading": 0.025816975580253556, - "angularVelocity": 0.27077600789812356, - "velocityX": -0.4246631042695725, - "velocityY": 0.5089862080862002, - "timestamp": 0.12723885251161615 - }, - { - "x": 2.6776153836811907, - "y": 5.641767759246714, - "heading": 0.051750317714318324, - "angularVelocity": 0.4076324427980389, - "velocityX": -0.629333671394567, - "velocityY": 0.7696629505440931, - "timestamp": 0.19085827876742423 - }, - { - "x": 2.6251092372323575, - "y": 5.707734732807458, - "heading": 0.0864931852991208, - "angularVelocity": 0.5461046983527398, - "velocityX": -0.8253162522672026, - "velocityY": 1.0368998502988076, - "timestamp": 0.2544777050232323 - }, - { - "x": 2.5610825168108655, - "y": 5.791356317104455, - "heading": 0.1301885817245676, - "angularVelocity": 0.686824748304888, - "velocityX": -1.006402040849065, - "velocityY": 1.314403307580339, - "timestamp": 0.3180971312790404 - }, - { - "x": 2.48749773492398, - "y": 5.89376509224382, - "heading": 0.18299253621322714, - "angularVelocity": 0.8299973388055953, - "velocityX": -1.1566401367878931, - "velocityY": 1.6097091905165586, - "timestamp": 0.38171655753484846 - }, - { - "x": 2.410156329475762, - "y": 6.016926517894053, - "heading": 0.24447965431113647, - "angularVelocity": 0.9664833796311684, - "velocityX": -1.215688508997792, - "velocityY": 1.9359090909592849, - "timestamp": 0.44533598379065653 - }, - { - "x": 2.3496693828620137, - "y": 6.152928215304285, - "heading": 0.3070071930810449, - "angularVelocity": 0.9828372000478389, - "velocityX": -0.9507622148388409, - "velocityY": 2.137738508728141, - "timestamp": 0.5089554100464646 - }, - { - "x": 2.309701308910896, - "y": 6.283843034176789, - "heading": 0.36362933631377437, - "angularVelocity": 0.8900134214517557, - "velocityX": -0.6282369443322133, - "velocityY": 2.0577805644789557, - "timestamp": 0.5725748363022727 - }, - { - "x": 2.288494901023514, - "y": 6.40507103844379, - "heading": 0.41290495871744193, - "angularVelocity": 0.7745373591634508, - "velocityX": -0.3333322718459105, - "velocityY": 1.905518666256975, - "timestamp": 0.6361942625580808 - }, - { - "x": 2.285060268530139, - "y": 6.514915678699493, - "heading": 0.45431002431284384, - "angularVelocity": 0.6508242534114624, - "velocityX": -0.05398716548566081, - "velocityY": 1.7265896082436825, - "timestamp": 0.6998136888138888 - }, - { - "x": 2.2988127872129502, - "y": 6.612520892931977, - "heading": 0.48757772897770557, - "angularVelocity": 0.522917395248037, - "velocityX": 0.2161685430408261, - "velocityY": 1.5342045657567287, - "timestamp": 0.7634331150696969 - }, - { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "angularVelocity": 0.3925324714794108, - "velocityX": 0.48035043054352317, - "velocityY": 1.3337738607711032, - "timestamp": 0.827052541325505 - }, - { - "x": 2.3906193814869496, - "y": 6.780920533993851, - "heading": 0.5304788719493444, - "angularVelocity": 0.23275934723222522, - "velocityX": 0.7951498408867412, - "velocityY": 1.0846472026287595, - "timestamp": 0.9040782443210413 - }, - { - "x": 2.471491792404757, - "y": 6.8405967325057775, - "heading": 0.5351323155604377, - "angularVelocity": 0.06041416605263601, - "velocityX": 1.0499405753231936, - "velocityY": 0.7747569472411664, - "timestamp": 0.9811039473165777 - }, - { - "x": 2.556148280048404, - "y": 6.869576729136414, - "heading": 0.5277899567279947, - "angularVelocity": -0.09532349004160981, - "velocityX": 1.0990680299088356, - "velocityY": 0.37623800242778493, - "timestamp": 1.058129650312114 - }, - { - "x": 2.6173499244947225, - "y": 6.878199709364964, - "heading": 0.518346888239368, - "angularVelocity": -0.12259632981439711, - "velocityX": 0.794561322599876, - "velocityY": 0.11194938693450328, - "timestamp": 1.1351553533076504 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.0752536934288536, - "velocityX": 0.4023928513995403, - "velocityY": 0.0187620669061233, - "timestamp": 1.2121810563031867 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -4.4995568742972564e-34, - "velocityX": 2.8230892463303505e-33, - "velocityY": 2.0807279166194578e-33, - "timestamp": 1.289206759298723 - } - ] + "samples": [ + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.6159098195352036e-29, + "angularVelocity": -1.6705479392630998e-29, + "velocityX": 6.780641024803866e-22, + "velocityY": 9.718698194414986e-25, + "timestamp": 0 + }, + { + "x": 2.746560227605712, + "y": 5.55825095759549, + "heading": 0.00742109779657271, + "angularVelocity": 0.11458935665570884, + "velocityX": -0.18135392067783562, + "velocityY": 0.21487100869168943, + "timestamp": 0.06476254002254223 + }, + { + "x": 2.7232728362575633, + "y": 5.58624983061418, + "heading": 0.022298554503038515, + "angularVelocity": 0.22972318104396974, + "velocityX": -0.35958119215279744, + "velocityY": 0.43233129844728024, + "timestamp": 0.12952508004508445 + }, + { + "x": 2.6887224275841572, + "y": 5.628555352569631, + "heading": 0.04468536543200197, + "angularVelocity": 0.3456753073794072, + "velocityX": -0.5334937243255126, + "velocityY": 0.6532406224451052, + "timestamp": 0.19428762006762668 + }, + { + "x": 2.6433185485147903, + "y": 5.685479106691384, + "heading": 0.07465726375014338, + "angularVelocity": 0.46279683143540923, + "velocityX": -0.7010824321214573, + "velocityY": 0.8789611108819849, + "timestamp": 0.2590501600901689 + }, + { + "x": 2.587716352440023, + "y": 5.7574833976129485, + "heading": 0.11231839668543797, + "angularVelocity": 0.5815264954429781, + "velocityX": -0.8585549000303846, + "velocityY": 1.1118200567257257, + "timestamp": 0.3238127001127111 + }, + { + "x": 2.523118270580454, + "y": 5.845312686573325, + "heading": 0.15779630703388142, + "angularVelocity": 0.7022255509529696, + "velocityX": -0.9974605973929359, + "velocityY": 1.356174247177545, + "timestamp": 0.38857524013525335 + }, + { + "x": 2.4523262633845233, + "y": 5.950260122565857, + "heading": 0.21110300167691656, + "angularVelocity": 0.8231100050195744, + "velocityX": -1.093101153402716, + "velocityY": 1.6204959835732518, + "timestamp": 0.4533377801577956 + }, + { + "x": 2.385035672029833, + "y": 6.073114038949963, + "heading": 0.27020607963640353, + "angularVelocity": 0.9126121047586251, + "velocityX": -1.0390357038384854, + "velocityY": 1.896990395085535, + "timestamp": 0.5181003201803378 + }, + { + "x": 2.335844965932351, + "y": 6.198572107213467, + "heading": 0.32662517660992413, + "angularVelocity": 0.8711686872362097, + "velocityX": -0.7595549229594762, + "velocityY": 1.9372011693771587, + "timestamp": 0.5828628602028805 + }, + { + "x": 2.303814652096041, + "y": 6.317831411051833, + "heading": 0.37736366027868895, + "angularVelocity": 0.7834541951428096, + "velocityX": -0.4945808769260876, + "velocityY": 1.8414858928765832, + "timestamp": 0.6476254002254227 + }, + { + "x": 2.2877361979926687, + "y": 6.4282644247646346, + "heading": 0.42161092558434743, + "angularVelocity": 0.6832231300726793, + "velocityX": -0.24826781188285701, + "velocityY": 1.7051989263293559, + "timestamp": 0.7123879402479649 + }, + { + "x": 2.2869063645502266, + "y": 6.528717533266064, + "heading": 0.45901061919668296, + "angularVelocity": 0.5774896043193742, + "velocityX": -0.012813478936328582, + "velocityY": 1.5510989603938259, + "timestamp": 0.7771504802705071 + }, + { + "x": 2.3008846173823154, + "y": 6.618554541912377, + "heading": 0.48936477750092344, + "angularVelocity": 0.46869931743991045, + "velocityX": 0.21583855153339288, + "velocityY": 1.3871754970549879, + "timestamp": 0.8419130202930494 + }, + { + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.3580100794534351, + "velocityX": 0.43988065652810937, + "velocityY": 1.21706589595492, + "timestamp": 0.9066755603155916 + }, + { + "x": 2.3907092837867907, + "y": 6.780867422722411, + "heading": 0.5304481214064672, + "angularVelocity": 0.21222434211791516, + "velocityX": 0.7273100577595625, + "velocityY": 0.990024458199304, + "timestamp": 0.991009438455372 + }, + { + "x": 2.471644870352802, + "y": 6.840510057948011, + "heading": 0.53507672796877, + "angularVelocity": 0.05488430823293737, + "velocityX": 0.9597043128013546, + "velocityY": 0.7072203548702543, + "timestamp": 1.0753433165951525 + }, + { + "x": 2.556252265889606, + "y": 6.8694746689081185, + "heading": 0.5277386544868528, + "angularVelocity": -0.08701216692246314, + "velocityX": 1.0032432683407548, + "velocityY": 0.3434516661512893, + "timestamp": 1.159677194734933 + }, + { + "x": 2.617383928971812, + "y": 6.87815517989328, + "heading": 0.5183265349753615, + "angularVelocity": -0.1116054392268187, + "velocityX": 0.7248766976052303, + "velocityY": 0.1029302953526471, + "timestamp": 1.2440110728747134 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06849104420156939, + "velocityX": 0.36711922260971214, + "velocityY": 0.01766420444115541, + "timestamp": 1.3283449510144938 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -1.2403630086849167e-25, + "velocityX": 1.2566075146445203e-23, + "velocityY": 7.418296886220231e-24, + "timestamp": 1.4126788291542742 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.4.traj b/src/main/deploy/choreo/CenterLanePCBAFE.4.traj index d5e91e8f..a66f4247 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.4.traj @@ -1,688 +1,734 @@ { - "samples": [ - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -4.4995568742972564e-34, - "velocityX": 2.8230892463303505e-33, - "velocityY": 2.0807279166194578e-33, - "timestamp": 0 - }, - { - "x": 2.658706958910465, - "y": 6.86593478207681, - "heading": 0.5043163629464208, - "angularVelocity": -0.14325761626715308, - "velocityX": 0.1802876546124509, - "velocityY": -0.23853061812890558, - "timestamp": 0.05747726974755718 - }, - { - "x": 2.679434758469343, - "y": 6.838509950906704, - "heading": 0.48805940847309726, - "angularVelocity": -0.2828414527120879, - "velocityX": 0.36062602920974224, - "velocityY": -0.4771422040496659, - "timestamp": 0.11495453949511436 - }, - { - "x": 2.7105313297582017, - "y": 6.797364836772264, - "heading": 0.464037646160856, - "angularVelocity": -0.4179349926979131, - "velocityX": 0.5410238068968174, - "velocityY": -0.7158501841710775, - "timestamp": 0.17243180924267154 - }, - { - "x": 2.7520007950829926, - "y": 6.742492873007072, - "heading": 0.4325704761896421, - "angularVelocity": -0.5474715502218408, - "velocityX": 0.721493305213118, - "velocityY": -0.954672412350015, - "timestamp": 0.22990907899022872 - }, - { - "x": 2.803848241216662, - "y": 6.673886273182379, - "heading": 0.39405966194845793, - "angularVelocity": -0.6700181551824853, - "velocityX": 0.9020513041309295, - "velocityY": -1.1936301102334148, - "timestamp": 0.2873863487377859 - }, - { - "x": 2.866080023237396, - "y": 6.591535755483362, - "heading": 0.3490218715149645, - "angularVelocity": -0.7835756748937736, - "velocityX": 1.0827198698556684, - "velocityY": -1.4327492948204654, - "timestamp": 0.3448636184853431 - }, - { - "x": 2.938704098816793, - "y": 6.495430149531881, - "heading": 0.29814315673038183, - "angularVelocity": -0.8851971398092591, - "velocityX": 1.2635268845991694, - "velocityY": -1.6720628236793682, - "timestamp": 0.40234088823290026 - }, - { - "x": 3.0217302964606527, - "y": 6.385555893825644, - "heading": 0.2423799369377359, - "angularVelocity": -0.9701786469253116, - "velocityX": 1.4445048974753851, - "velocityY": -1.9116122980233827, - "timestamp": 0.45981815798045744 - }, - { - "x": 3.1151700095083186, - "y": 6.261896797084101, - "heading": 0.1831716018579807, - "angularVelocity": -1.0301173897055516, - "velocityX": 1.6256811337430896, - "velocityY": -2.151443471213232, - "timestamp": 0.5172954277280146 - }, - { - "x": 3.2190322842248182, - "y": 6.124436959606287, - "heading": 0.12297302537717429, - "angularVelocity": -1.0473457898261622, - "velocityX": 1.8070147585761873, - "velocityY": -2.39155127029422, - "timestamp": 0.5747726974755718 - }, - { - "x": 3.3332908123673026, - "y": 5.973192452697109, - "heading": 0.06701518728601086, - "angularVelocity": -0.9735646515036827, - "velocityX": 1.9878906678120518, - "velocityY": -2.6313794578874745, - "timestamp": 0.632249967223129 - }, - { - "x": 3.457219648102922, - "y": 5.80867115561654, - "heading": 0.033419490428721374, - "angularVelocity": -0.5845040483802287, - "velocityX": 2.1561364393249893, - "velocityY": -2.8623714696810656, - "timestamp": 0.6897272369706862 - }, - { - "x": 3.588371868131788, - "y": 5.635317112607293, - "heading": 0.03341946780631197, - "angularVelocity": -3.935887960456646e-7, - "velocityX": 2.2818101939234974, - "velocityY": -3.0160451909185437, - "timestamp": 0.7472045067182433 - }, - { - "x": 3.719524526285539, - "y": 5.461963401064903, - "heading": 0.033419445184933325, - "angularVelocity": -3.9357086276096175e-7, - "velocityX": 2.281817816499982, - "velocityY": -3.0160394239978356, - "timestamp": 0.8046817764658005 - }, - { - "x": 3.8561567314585825, - "y": 5.292894866069507, - "heading": 0.0334194225694706, - "angularVelocity": -3.9346793646231113e-7, - "velocityX": 2.3771519728257604, - "velocityY": -2.9414851425259596, - "timestamp": 0.8621590462133577 - }, - { - "x": 4.00579710900262, - "y": 5.135223415824853, - "heading": 0.03341939963642931, - "angularVelocity": -3.9899322628139097e-7, - "velocityX": 2.6034705232392743, - "velocityY": -2.743196587749476, - "timestamp": 0.9196363159609149 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.03341937596859772, - "angularVelocity": -4.117772415638071e-7, - "velocityX": 2.813320067337111, - "velocityY": -2.527531676347718, - "timestamp": 0.9771135857084721 - }, - { - "x": 4.277933740289957, - "y": 4.900652577427371, - "heading": 0.03341935253431191, - "angularVelocity": -6.240523322284166e-7, - "velocityX": 2.940862673655656, - "velocityY": -2.37792138300258, - "timestamp": 1.014665381608384 - }, - { - "x": 4.3928594316694705, - "y": 4.81721673895127, - "heading": 0.033419330076064284, - "angularVelocity": -5.980605477839765e-7, - "velocityX": 3.060457925523134, - "velocityY": -2.2218867693700792, - "timestamp": 1.0522171775082958 - }, - { - "x": 4.5119655190903245, - "y": 4.739865631807423, - "heading": 0.03341930829497473, - "angularVelocity": -5.800279064172658e-7, - "velocityX": 3.1717813906506853, - "velocityY": -2.0598510747665353, - "timestamp": 1.0897689734082077 - }, - { - "x": 4.634929904279705, - "y": 4.6688078109694935, - "heading": 0.033419286924895195, - "angularVelocity": -5.690827567994812e-7, - "velocityX": 3.2745274158690463, - "velocityY": -1.8922615852334157, - "timestamp": 1.1273207693081195 - }, - { - "x": 4.761195179582567, - "y": 4.6037960104372555, - "heading": 0.03341926571852252, - "angularVelocity": -5.647232617468814e-7, - "velocityX": 3.362429739429795, - "velocityY": -1.7312567608088656, - "timestamp": 1.1648725652080314 - }, - { - "x": 4.8874652266060234, - "y": 4.538793478329322, - "heading": 0.03341924451230877, - "angularVelocity": -5.647190300189245e-7, - "velocityX": 3.3625568097996403, - "velocityY": -1.7310099437370914, - "timestamp": 1.2024243611079433 - }, - { - "x": 5.0137352745443575, - "y": 4.473790947998577, - "heading": 0.03341922330609504, - "angularVelocity": -5.647190293196981e-7, - "velocityX": 3.3625568341627456, - "velocityY": -1.731009896410775, - "timestamp": 1.2399761570078551 - }, - { - "x": 5.140005505333594, - "y": 4.408788772864344, - "heading": 0.03341920209988365, - "angularVelocity": -5.64718967252867e-7, - "velocityX": 3.362561703461214, - "velocityY": -1.7310004375685482, - "timestamp": 1.277527952907767 - }, - { - "x": 5.267176463182282, - "y": 4.34556692987905, - "heading": 0.033419180889344834, - "angularVelocity": -5.648342058496427e-7, - "velocityX": 3.3865479613183083, - "velocityY": -1.6835903974819586, - "timestamp": 1.3150797488076789 - }, - { - "x": 5.39745989861742, - "y": 4.289036580908955, - "heading": 0.03341915955151481, - "angularVelocity": -5.682239561025287e-7, - "velocityX": 3.4694328809835504, - "velocityY": -1.5053966825119904, - "timestamp": 1.3526315447075907 - }, - { - "x": 5.530504784906373, - "y": 4.239352458442723, - "heading": 0.03341913783750815, - "angularVelocity": -5.782414966353427e-7, - "velocityX": 3.542969999186219, - "velocityY": -1.323082459189313, - "timestamp": 1.3901833406075026 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.033419115463141504, - "angularVelocity": -5.958268068735077e-7, - "velocityX": 3.6069365171092738, - "velocityY": -1.1371861948253605, - "timestamp": 1.4277351365074145 - }, - { - "x": 5.808220957669321, - "y": 4.160073254115033, - "heading": 0.03341909344161441, - "angularVelocity": -5.669632847998976e-7, - "velocityX": 3.6628444951256323, - "velocityY": -0.941676170142482, - "timestamp": 1.4665763237425935 - }, - { - "x": 5.952250404538064, - "y": 4.131196918833519, - "heading": 0.033419072126499465, - "angularVelocity": -5.487760919877281e-7, - "velocityX": 3.708162832321777, - "velocityY": -0.7434462573626127, - "timestamp": 1.5054175109777725 - }, - { - "x": 6.09762364792411, - "y": 4.110103210641541, - "heading": 0.03341905123842176, - "angularVelocity": -5.377816485869475e-7, - "velocityX": 3.7427600373239653, - "velocityY": -0.5430757835556856, - "timestamp": 1.5442586982129516 - }, - { - "x": 6.24392007770515, - "y": 4.096848811559285, - "heading": 0.0334190305197359, - "angularVelocity": -5.334205087871439e-7, - "velocityX": 3.7665282704987906, - "velocityY": -0.3412459820551187, - "timestamp": 1.5830998854481306 - }, - { - "x": 6.390314919287249, - "y": 4.0847296192319185, - "heading": 0.03341900981240726, - "angularVelocity": -5.331281075500129e-7, - "velocityX": 3.7690619675370143, - "velocityY": -0.31201910111516795, - "timestamp": 1.6219410726833097 - }, - { - "x": 6.536709800200056, - "y": 4.07261090201266, - "heading": 0.033418989105078956, - "angularVelocity": -5.331280988936517e-7, - "velocityX": 3.769062980140168, - "velocityY": -0.31200686904551156, - "timestamp": 1.6607822599184887 - }, - { - "x": 6.68323027882942, - "y": 4.062119297351164, - "heading": 0.03341896797261103, - "angularVelocity": -5.440736864773728e-7, - "velocityX": 3.772296602114613, - "velocityY": -0.27011544724343456, - "timestamp": 1.6996234471536678 - }, - { - "x": 6.828478445747191, - "y": 4.059283961769526, - "heading": 0.02725441210059553, - "angularVelocity": -0.15871182913873783, - "velocityX": 3.739539835337918, - "velocityY": -0.07299816981572388, - "timestamp": 1.7384646343888468 - }, - { - "x": 6.965940131033012, - "y": 4.0573223410737915, - "heading": 0.017080975304275284, - "angularVelocity": -0.2619239400361572, - "velocityX": 3.5390701229986914, - "velocityY": -0.05050362348240824, - "timestamp": 1.7773058216240258 - }, - { - "x": 7.095541107200079, - "y": 4.056112825219225, - "heading": 0.007563330070755733, - "angularVelocity": -0.2450400183673897, - "velocityX": 3.336689359734222, - "velocityY": -0.03114003305929124, - "timestamp": 1.8161470088592049 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, - "angularVelocity": -0.194724482157576, - "velocityX": 3.1345655629883886, - "velocityY": -0.012732337027157752, - "timestamp": 1.854988196094384 - }, - { - "x": 7.3857970557158446, - "y": 4.056503208949951, - "heading": -0.005539516561442828, - "angularVelocity": -0.09285314353970942, - "velocityX": 2.8244854636552765, - "velocityY": 0.014833039029661928, - "timestamp": 1.9146470977779204 - }, - { - "x": 7.5357095386456, - "y": 4.058265721996549, - "heading": -0.007640794437617486, - "angularVelocity": -0.03522153135371121, - "velocityX": 2.5128267316246156, - "velocityY": 0.029543169533142722, - "timestamp": 1.974305999461457 - }, - { - "x": 7.667000937583321, - "y": 4.060490426598279, - "heading": -0.007708640614146542, - "angularVelocity": -0.0011372347564986972, - "velocityX": 2.200700905192029, - "velocityY": 0.037290404934565556, - "timestamp": 2.0339649011449934 - }, - { - "x": 7.779661923967275, - "y": 4.062917456014214, - "heading": -0.0066168114481254744, - "angularVelocity": 0.018301194544491067, - "velocityX": 1.888418713800162, - "velocityY": 0.0406817649578728, - "timestamp": 2.09362380282853 - }, - { - "x": 7.873689685619936, - "y": 4.06536905162046, - "heading": -0.004961765552273349, - "angularVelocity": 0.027741809673791765, - "velocityX": 1.5760893848068933, - "velocityY": 0.04109354240631638, - "timestamp": 2.1532827045120664 - }, - { - "x": 7.949083994059634, - "y": 4.067716008149933, - "heading": -0.003176764942269326, - "angularVelocity": 0.029920105124841948, - "velocityX": 1.263756226013528, - "velocityY": 0.03933958660390468, - "timestamp": 2.212941606195603 - }, - { - "x": 8.005845696502893, - "y": 4.069860191657625, - "heading": -0.0015908607397525072, - "angularVelocity": 0.02658285951909277, - "velocityX": 0.951437268227904, - "velocityY": 0.035940713744034375, - "timestamp": 2.2726005078791394 - }, - { - "x": 8.043976076014772, - "y": 4.071724543898811, - "heading": -0.00046248549538235704, - "angularVelocity": 0.018913778372181077, - "velocityX": 0.6391398171247351, - "velocityY": 0.03125019382816773, - "timestamp": 2.332259409562676 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 3.4463826664070866e-33, - "angularVelocity": 0.00775216241552071, - "velocityX": 0.32686633402451676, - "velocityY": 0.02551860543538527, - "timestamp": 2.3919183112462123 - }, - { - "x": 8.059339450711764, - "y": 4.074523581107775, - "heading": -0.0006944740465081056, - "angularVelocity": -0.009481698382290156, - "velocityX": -0.0564842507032717, - "velocityY": 0.017429845647464255, - "timestamp": 2.4651619452986555 - }, - { - "x": 8.027124349390371, - "y": 4.075207755344852, - "heading": -0.002651211191279339, - "angularVelocity": -0.026715456845985846, - "velocityX": -0.43983482985464195, - "velocityY": 0.009341074428224473, - "timestamp": 2.5384055793510987 - }, - { - "x": 7.96683125907587, - "y": 4.075299477238389, - "heading": -0.0058702138612145086, - "angularVelocity": -0.043949248444312826, - "velocityX": -0.8231854016327222, - "velocityY": 0.0012522848534625944, - "timestamp": 2.611649213403542 - }, - { - "x": 7.878460180490193, - "y": 4.074798744928973, - "heading": -0.0103514975430217, - "angularVelocity": -0.06118325148365151, - "velocityX": -1.2065359635542152, - "velocityY": -0.006836530107951673, - "timestamp": 2.684892847455985 - }, - { - "x": 7.7620111146201785, - "y": 4.073705556029158, - "heading": -0.01609509527793809, - "angularVelocity": -0.07841770563710548, - "velocityX": -1.5898865120023296, - "velocityY": -0.01492537766534737, - "timestamp": 2.758136481508428 - }, - { - "x": 7.617484062863899, - "y": 4.072019907599753, - "heading": -0.023101063438175296, - "angularVelocity": -0.0956529294439546, - "velocityX": -1.9732370413624691, - "velocityY": -0.02301426535169652, - "timestamp": 2.8313801155608713 - }, - { - "x": 7.444879027316714, - "y": 4.06974179610174, - "heading": -0.031369488604002356, - "angularVelocity": -0.1128893353367305, - "velocityX": -2.356587542114559, - "velocityY": -0.031103201356480925, - "timestamp": 2.9046237496133145 - }, - { - "x": 7.244196011426359, - "y": 4.066871217282435, - "heading": -0.040900495023888005, - "angularVelocity": -0.13012743760176224, - "velocityX": -2.7399379957944494, - "velocityY": -0.0391921954234215, - "timestamp": 2.9778673836657577 - }, - { - "x": 7.015435021934675, - "y": 4.063408165830539, - "heading": -0.05169425070794916, - "angularVelocity": -0.14736783371961967, - "velocityX": -3.123288357427606, - "velocityY": -0.047281262005875575, - "timestamp": 3.051111017718201 - }, - { - "x": 6.758596078532163, - "y": 4.059352633648335, - "heading": -0.06375095974291681, - "angularVelocity": -0.1646110162466115, - "velocityX": -3.5066384502250503, - "velocityY": -0.05537043914696755, - "timestamp": 3.124354651770644 - }, - { - "x": 6.481653456514826, - "y": 4.053513800761945, - "heading": -0.06375096147352038, - "angularVelocity": -2.3628040945559003e-8, - "velocityX": -3.7811152545904956, - "velocityY": -0.07971795722490971, - "timestamp": 3.197598285823087 - }, - { - "x": 6.205372409181029, - "y": 4.073516064315128, - "heading": -0.06375096323470678, - "angularVelocity": -2.4045589953178915e-8, - "velocityX": -3.7720827333058655, - "velocityY": 0.2730921780705308, - "timestamp": 3.2708419198755303 - }, - { - "x": 5.9325368870621995, - "y": 4.121391787593611, - "heading": -0.0637509651063581, - "angularVelocity": -2.555377459766275e-8, - "velocityX": -3.725040758128897, - "velocityY": 0.6536502987304462, - "timestamp": 3.3440855539279735 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -0.063750967187294, - "angularVelocity": -2.84111505129003e-8, - "velocityX": -3.639704142076304, - "velocityY": 1.0274925313911556, - "timestamp": 3.4173291879804166 - }, - { - "x": 5.417504232216337, - "y": 4.293821782165803, - "heading": -0.06375097411514968, - "angularVelocity": -9.821339780633191e-8, - "velocityX": -3.5221393138546646, - "velocityY": 1.3775780331429743, - "timestamp": 3.4878679925152554 - }, - { - "x": 5.186014168311204, - "y": 4.410724788288498, - "heading": -0.06922328693791815, - "angularVelocity": -0.07757875766190163, - "velocityX": -3.281740673543775, - "velocityY": 1.6572864665569722, - "timestamp": 3.558406797050094 - }, - { - "x": 4.979961148964822, - "y": 4.522014233902487, - "heading": -0.07551597396386121, - "angularVelocity": -0.08920886974821321, - "velocityX": -2.9211300177991313, - "velocityY": 1.5777052977843462, - "timestamp": 3.628945601584932 - }, - { - "x": 4.7980291404215, - "y": 4.623466282585381, - "heading": -0.08170492719079357, - "angularVelocity": -0.08773827778546625, - "velocityX": -2.579176238427287, - "velocityY": 1.4382445145180842, - "timestamp": 3.69948440611977 - }, - { - "x": 4.639563192627529, - "y": 4.713607705102715, - "heading": -0.0874491132160183, - "angularVelocity": -0.08143299369906003, - "velocityX": -2.246507420120879, - "velocityY": 1.27789835838249, - "timestamp": 3.7700232106546077 - }, - { - "x": 4.504192007690659, - "y": 4.791698947450768, - "heading": -0.09257307344830198, - "angularVelocity": -0.07264030438385177, - "velocityX": -1.919102341322099, - "velocityY": 1.1070678453231335, - "timestamp": 3.8405620151894455 - }, - { - "x": 4.391678654940531, - "y": 4.857296763925047, - "heading": -0.09697023729098278, - "angularVelocity": -0.0623368069770602, - "velocityX": -1.5950561324662005, - "velocityY": 0.9299536178257779, - "timestamp": 3.9111008197242834 - }, - { - "x": 4.301859289367306, - "y": 4.910106206970951, - "heading": -0.10056909340764136, - "angularVelocity": -0.051019522380495494, - "velocityX": -1.2733326878096392, - "velocityY": 0.7486580385668955, - "timestamp": 3.981639624259121 - }, - { - "x": 4.234614001747884, - "y": 4.949916980366573, - "heading": -0.10331834169263278, - "angularVelocity": -0.03897497700905296, - "velocityX": -0.9533091475375062, - "velocityY": 0.5643811751297677, - "timestamp": 4.052178428793959 - }, - { - "x": 4.18985129168719, - "y": 4.976571613170809, - "heading": -0.10517936365869324, - "angularVelocity": -0.026382953019019773, - "velocityX": -0.6345827712261082, - "velocityY": 0.37787191007854753, - "timestamp": 4.122717233328797 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.013363372792080578, - "velocityX": -0.31687843925651377, - "velocityY": 0.18962871266842563, - "timestamp": 4.193256037863635 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": 0, - "velocityX": 8.237561081237056e-34, - "velocityY": 0, - "timestamp": 4.263794842398473 - } - ] + "samples": [ + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -1.2403630086849167e-25, + "velocityX": 1.2566075146445203e-23, + "velocityY": 7.418296886220231e-24, + "timestamp": 0 + }, + { + "x": 2.65710997558339, + "y": 6.867790932222418, + "heading": 0.5069947283791145, + "angularVelocity": -0.09538266194620745, + "velocityX": 0.1504894284206614, + "velocityY": -0.20351386841397834, + "timestamp": 0.05824634275796026 + }, + { + "x": 2.6746461929799414, + "y": 6.844083289351916, + "heading": 0.4960269163567629, + "angularVelocity": -0.18830044090369472, + "velocityX": 0.3010698451819098, + "velocityY": -0.40702371596133824, + "timestamp": 0.11649268551592051 + }, + { + "x": 2.7009592416637416, + "y": 6.808522263895978, + "heading": 0.4798144698796375, + "angularVelocity": -0.27834273723408604, + "velocityX": 0.4517545211918758, + "velocityY": -0.6105280395665419, + "timestamp": 0.17473902827388077 + }, + { + "x": 2.736056173569292, + "y": 6.76110833670321, + "heading": 0.4585545185998355, + "angularVelocity": -0.3650006210372158, + "velocityX": 0.6025602680565616, + "velocityY": -0.8140241077417322, + "timestamp": 0.23298537103184103 + }, + { + "x": 2.7799452908871816, + "y": 6.701842252248569, + "heading": 0.4324815844111489, + "angularVelocity": -0.4476321251109512, + "velocityX": 0.7535085507474468, + "velocityY": -1.017507394428501, + "timestamp": 0.2912317137898013 + }, + { + "x": 2.8326365093294883, + "y": 6.63072517677858, + "heading": 0.40187833162186215, + "angularVelocity": -0.5254107183425597, + "velocityX": 0.9046270709435361, + "velocityY": -1.2209706584585491, + "timestamp": 0.34947805654776154 + }, + { + "x": 2.8941418577048204, + "y": 6.54775894755859, + "heading": 0.367091043865372, + "angularVelocity": -0.5972441548999393, + "velocityX": 1.0559521072578695, + "velocityY": -1.4244023794721719, + "timestamp": 0.4077243993057218 + }, + { + "x": 2.9644761872265666, + "y": 6.452946486925878, + "heading": 0.3285529983813312, + "angularVelocity": -0.661638888542476, + "velocityX": 1.2075321160337456, + "velocityY": -1.6277839284553843, + "timestamp": 0.46597074206368205 + }, + { + "x": 3.043658216776231, + "y": 6.346292539288771, + "heading": 0.2868218829421827, + "angularVelocity": -0.7164589820267359, + "velocityX": 1.359433499176103, + "velocityY": -1.8310840232545251, + "timestamp": 0.5242170848216423 + }, + { + "x": 3.131712135507504, + "y": 6.227805106020448, + "heading": 0.24264403908378393, + "angularVelocity": -0.7584655407804345, + "velocityX": 1.5117501728336273, + "velocityY": -2.03424674679905, + "timestamp": 0.5824634275796026 + }, + { + "x": 3.2286701423571795, + "y": 6.097498584088062, + "heading": 0.19707481942882135, + "angularVelocity": -0.7823533203504854, + "velocityX": 1.6646196526463415, + "velocityY": -2.2371622965903253, + "timestamp": 0.6407097703375628 + }, + { + "x": 3.33457642569322, + "y": 5.9554017916384385, + "heading": 0.1517318182422537, + "angularVelocity": -0.7784694976470596, + "velocityX": 1.8182477786824947, + "velocityY": -2.439583083183451, + "timestamp": 0.6989561130955231 + }, + { + "x": 3.449491590462662, + "y": 5.801583658823103, + "heading": 0.10942509922281701, + "angularVelocity": -0.726341209013589, + "velocityX": 1.9729163983216391, + "velocityY": -2.6408204452341257, + "timestamp": 0.7572024558534833 + }, + { + "x": 3.5734690777406897, + "y": 5.636270409713289, + "heading": 0.07620523271942112, + "angularVelocity": -0.5703339459687529, + "velocityX": 2.128502518917107, + "velocityY": -2.838173888389265, + "timestamp": 0.8154487986114436 + }, + { + "x": 3.704953237459551, + "y": 5.460551173556115, + "heading": 0.07376572964977675, + "angularVelocity": -0.04188251062872067, + "velocityX": 2.2573805237049305, + "velocityY": -3.016828659738646, + "timestamp": 0.8736951413694038 + }, + { + "x": 3.8484687741574635, + "y": 5.2934320188975885, + "heading": 0.07376572135485625, + "angularVelocity": -1.4241101012224094e-7, + "velocityX": 2.4639407369194757, + "velocityY": -2.86917850538672, + "timestamp": 0.9319414841273641 + }, + { + "x": 4.002883086238839, + "y": 5.136327781933491, + "heading": 0.07376571311141436, + "angularVelocity": -1.4152720162027837e-7, + "velocityX": 2.651055925056733, + "velocityY": -2.697237792541529, + "timestamp": 0.9901878268853244 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.07376570463306892, + "angularVelocity": -1.4556013372477054e-7, + "velocityX": 2.826202837221629, + "velocityY": -2.51311892102558, + "timestamp": 1.0484341696432846 + }, + { + "x": 4.268981030537756, + "y": 4.906736402889882, + "heading": 0.07376569669894775, + "angularVelocity": -2.2864637143597095e-7, + "velocityX": 2.9245183697828536, + "velocityY": -2.397995023127166, + "timestamp": 1.0831345723445063 + }, + { + "x": 4.373711969606293, + "y": 4.827653183122543, + "heading": 0.07376568907042552, + "angularVelocity": -2.198395881628289e-7, + "velocityX": 3.0181476558152474, + "velocityY": -2.2790288760699386, + "timestamp": 1.117834975045728 + }, + { + "x": 4.481524054047376, + "y": 4.75282484787289, + "heading": 0.0737656816823934, + "angularVelocity": -2.129091176782688e-7, + "velocityX": 3.106940440125975, + "velocityY": -2.1564111487103386, + "timestamp": 1.1525353777469496 + }, + { + "x": 4.592244511621915, + "y": 4.682371277147909, + "heading": 0.07376567447691734, + "angularVelocity": -2.0764819690223906e-7, + "velocityX": 3.1907542551556984, + "velocityY": -2.030338706198957, + "timestamp": 1.1872357804481712 + }, + { + "x": 4.705695889724197, + "y": 4.616405299617974, + "heading": 0.07376566740123429, + "angularVelocity": -2.0390780833167242e-7, + "velocityX": 3.2694542215871127, + "velocityY": -1.90101475472538, + "timestamp": 1.221936183149393 + }, + { + "x": 4.821696252971789, + "y": 4.555032332206395, + "heading": 0.07376566040606643, + "angularVelocity": -2.0158751247220773e-7, + "velocityX": 3.342911154270497, + "velocityY": -1.76865288682714, + "timestamp": 1.2566365858506146 + }, + { + "x": 4.939640387985606, + "y": 4.497483166935729, + "heading": 0.07376565344322925, + "angularVelocity": -2.0065580355812287e-7, + "velocityX": 3.398926981607191, + "velocityY": -1.6584581385460047, + "timestamp": 1.2913369885518362 + }, + { + "x": 5.057585335343478, + "y": 4.439935666553369, + "heading": 0.07376564648040178, + "angularVelocity": -2.0065552340961329e-7, + "velocityX": 3.3989503918269923, + "velocityY": -1.658410159612758, + "timestamp": 1.3260373912530579 + }, + { + "x": 5.1755309999931605, + "y": 4.382389636300601, + "heading": 0.07376563951757731, + "angularVelocity": -2.0065543705761684e-7, + "velocityX": 3.398971062820868, + "velocityY": -1.658367793257397, + "timestamp": 1.3607377939542795 + }, + { + "x": 5.295248531429034, + "y": 4.328626841825402, + "heading": 0.07376563255290919, + "angularVelocity": -2.007085673925138e-7, + "velocityX": 3.45003291364278, + "velocityY": -1.5493420908716486, + "timestamp": 1.3954381966555012 + }, + { + "x": 5.41702166916594, + "y": 4.27969803322079, + "heading": 0.07376562554616754, + "angularVelocity": -2.019210467909896e-7, + "velocityX": 3.5092716008341425, + "velocityY": -1.410035757391629, + "timestamp": 1.4301385993567228 + }, + { + "x": 5.540655427942115, + "y": 4.235681910653619, + "heading": 0.07376561844909023, + "angularVelocity": -2.0452435036769212e-7, + "velocityX": 3.5628911814277564, + "velocityY": -1.2684614338963205, + "timestamp": 1.4648390020579445 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.07376561120324407, + "angularVelocity": -2.0881158710741676e-7, + "velocityX": 3.610802501559291, + "velocityY": -1.1248525394721576, + "timestamp": 1.4995394047591661 + }, + { + "x": 5.801240954312848, + "y": 4.160760783914465, + "heading": 0.0737656041692584, + "angularVelocity": -1.900588111168926e-7, + "velocityX": 3.655524839727154, + "velocityY": -0.9697042570289608, + "timestamp": 1.5365489263528271 + }, + { + "x": 5.937938725958539, + "y": 4.130679839530998, + "heading": 0.07376559729974105, + "angularVelocity": -1.8561486466113367e-7, + "velocityX": 3.6935838605680487, + "velocityY": -0.8127893333433076, + "timestamp": 1.5735584479464881 + }, + { + "x": 6.075795864969434, + "y": 4.106460978217925, + "heading": 0.07376559053845443, + "angularVelocity": -1.8269046288328192e-7, + "velocityX": 3.724910052187935, + "velocityY": -0.654395416914037, + "timestamp": 1.6105679695401491 + }, + { + "x": 6.214561046214486, + "y": 4.088147908867804, + "heading": 0.07376558383255526, + "angularVelocity": -1.8119388963756882e-7, + "velocityX": 3.7494454202515035, + "velocityY": -0.4948204829877394, + "timestamp": 1.6475774911338101 + }, + { + "x": 6.353981153300122, + "y": 4.0757714079596425, + "heading": 0.07376557713086826, + "angularVelocity": -1.8108007594786474e-7, + "velocityX": 3.7671415647133246, + "velocityY": -0.33441396633135523, + "timestamp": 1.6845870127274711 + }, + { + "x": 6.493802446577043, + "y": 4.0693566803624766, + "heading": 0.07376555560001419, + "angularVelocity": -5.817652634682061e-7, + "velocityX": 3.7779816451577393, + "velocityY": -0.17332641225670456, + "timestamp": 1.7215965343211321 + }, + { + "x": 6.62912467253087, + "y": 4.064792329127301, + "heading": 0.05903973203041549, + "angularVelocity": -0.39789283772100603, + "velocityX": 3.656416514635636, + "velocityY": -0.12332910663606324, + "timestamp": 1.7586060559147931 + }, + { + "x": 6.758568056447626, + "y": 4.061299539806061, + "heading": 0.04418585550364903, + "angularVelocity": -0.4013528380575057, + "velocityX": 3.4975697696920878, + "velocityY": -0.09437542477821895, + "timestamp": 1.7956155775084541 + }, + { + "x": 6.882108384071856, + "y": 4.058691153499139, + "heading": 0.0304778500113165, + "angularVelocity": -0.3703913182893011, + "velocityX": 3.3380687537822458, + "velocityY": -0.07047879017620408, + "timestamp": 1.8326250991021151 + }, + { + "x": 6.999742512686264, + "y": 4.056896966002031, + "heading": 0.018392940314004957, + "angularVelocity": -0.3265351503322698, + "velocityX": 3.1784828214195313, + "velocityY": -0.048479078351971995, + "timestamp": 1.8696346206957761 + }, + { + "x": 7.111470092404711, + "y": 4.055880215977976, + "heading": 0.008182013942056482, + "angularVelocity": -0.2758999828221868, + "velocityX": 3.0188874351076422, + "velocityY": -0.027472660555273344, + "timestamp": 1.9066441422894371 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": -2.0237932968568665e-24, + "angularVelocity": -0.22107861949390495, + "velocityX": 2.859298314909037, + "velocityY": -0.007077363713034477, + "timestamp": 1.9436536638830981 + }, + { + "x": 7.386520754967261, + "y": 4.057483355651357, + "heading": -0.007799169879435119, + "angularVelocity": -0.11872019703645657, + "velocityX": 2.5760366812439752, + "velocityY": 0.028390383098604113, + "timestamp": 2.009347371222888 + }, + { + "x": 7.536988258489095, + "y": 4.060513288338981, + "heading": -0.011310959722615328, + "angularVelocity": -0.053457020244208106, + "velocityX": 2.2904401291217464, + "velocityY": 0.04612211443560253, + "timestamp": 2.075041078562678 + }, + { + "x": 7.668645085347693, + "y": 4.063976331623079, + "heading": -0.012040004750973126, + "angularVelocity": -0.01109763869143412, + "velocityX": 2.004101034785968, + "velocityY": 0.05271499241448283, + "timestamp": 2.1407347859024677 + }, + { + "x": 7.78147865771714, + "y": 4.06737291870627, + "heading": -0.011006488514551415, + "angularVelocity": 0.015732347560718764, + "velocityX": 1.7175704787953916, + "velocityY": 0.051703385616870434, + "timestamp": 2.2064284932422575 + }, + { + "x": 7.87549031086396, + "y": 4.070341151997571, + "heading": -0.008947695308103269, + "angularVelocity": 0.03133927570565307, + "velocityX": 1.4310602484429864, + "velocityY": 0.04518291646943911, + "timestamp": 2.2721222005820474 + }, + { + "x": 7.9506868858752515, + "y": 4.072607124350486, + "heading": -0.006421398747140829, + "angularVelocity": 0.03845568568532146, + "velocityX": 1.1446541542000461, + "velocityY": 0.03449298943040145, + "timestamp": 2.3378159079218372 + }, + { + "x": 8.00707724488628, + "y": 4.073956458693622, + "heading": -0.003864315760597132, + "angularVelocity": 0.038924321523180636, + "velocityX": 0.8583829607812892, + "velocityY": 0.020539780715319512, + "timestamp": 2.403509615261627 + }, + { + "x": 8.044670745645819, + "y": 4.074216878653992, + "heading": -0.0016276569414438076, + "angularVelocity": 0.034046774184696094, + "velocityX": 0.5722542124939418, + "velocityY": 0.003964153811924349, + "timestamp": 2.469203322601417 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 1.6866276328916314e-24, + "angularVelocity": 0.02477645131252876, + "velocityX": 0.286265117553968, + "velocityY": -0.014764317948939539, + "timestamp": 2.534897029941207 + }, + { + "x": 8.060961534281953, + "y": 4.070465444687161, + "heading": 0.0007560599723107442, + "angularVelocity": 0.010267428471842888, + "velocityX": -0.03415452911035645, + "velocityY": -0.03777341504058265, + "timestamp": 2.60853377105746 + }, + { + "x": 8.034839704830464, + "y": 4.066167075236277, + "heading": 0.00046881224437648735, + "angularVelocity": -0.0039008750737728444, + "velocityX": -0.3547390752973492, + "velocityY": -0.0583726192349958, + "timestamp": 2.6821705121737134 + }, + { + "x": 7.985098416570273, + "y": 4.060562217226684, + "heading": -0.0008319841828060777, + "angularVelocity": -0.017665046109644415, + "velocityX": -0.6754955136004895, + "velocityY": -0.0761149654999585, + "timestamp": 2.7558072532899667 + }, + { + "x": 7.9117249252468085, + "y": 4.053904178499818, + "heading": -0.0031104504440480647, + "angularVelocity": -0.03094197579500262, + "velocityX": -0.9964250211402843, + "velocityY": -0.09041734636729296, + "timestamp": 2.82944399440622 + }, + { + "x": 7.814707316285252, + "y": 4.046503723453766, + "heading": -0.00632248118792148, + "angularVelocity": -0.04361994698818144, + "velocityX": -1.3175163307185394, + "velocityY": -0.10049949161068944, + "timestamp": 2.9030807355224733 + }, + { + "x": 7.69403642851654, + "y": 4.038750888931838, + "heading": -0.010412565479377186, + "angularVelocity": -0.055544069841419484, + "velocityX": -1.6387320505969085, + "velocityY": -0.10528486736925086, + "timestamp": 2.9767174766387265 + }, + { + "x": 7.549709960346403, + "y": 4.031149265281445, + "heading": -0.015308761000708897, + "angularVelocity": -0.06649120326498265, + "velocityX": -1.9599790265335464, + "velocityY": -0.10323139692441612, + "timestamp": 3.05035421775498 + }, + { + "x": 7.381741634294396, + "y": 4.0243729190236985, + "heading": -0.02091427607708914, + "angularVelocity": -0.07612388858342581, + "velocityX": -2.2810396482216517, + "velocityY": -0.0920239836123183, + "timestamp": 3.123990958871233 + }, + { + "x": 7.1901832106568095, + "y": 4.019367673272749, + "heading": -0.027092292311801627, + "angularVelocity": -0.08389855581684406, + "velocityX": -2.6013973559091212, + "velocityY": -0.06797212471757884, + "timestamp": 3.1976276999874864 + }, + { + "x": 6.975183472586086, + "y": 4.017547771662454, + "heading": -0.03363583639032012, + "angularVelocity": -0.08886248874305737, + "velocityX": -2.9197345620074877, + "velocityY": -0.02471458653257856, + "timestamp": 3.2712644411037397 + }, + { + "x": 6.737173435740335, + "y": 4.021221144917956, + "heading": -0.04019984018910067, + "angularVelocity": -0.0891403353716819, + "velocityX": -3.232218499050576, + "velocityY": 0.04988506009116617, + "timestamp": 3.344901182219993 + }, + { + "x": 6.477592542757965, + "y": 4.034606496898433, + "heading": -0.04612188209053887, + "angularVelocity": -0.08042237898726157, + "velocityX": -3.5251545498538244, + "velocityY": 0.18177545308999152, + "timestamp": 3.4185379233362463 + }, + { + "x": 6.202379983792451, + "y": 4.065741183912862, + "heading": -0.04994434785858321, + "angularVelocity": -0.05190976284528472, + "velocityX": -3.737435345366867, + "velocityY": 0.4228145697713012, + "timestamp": 3.4921746644524996 + }, + { + "x": 5.92930929060311, + "y": 4.120418111584284, + "heading": -0.049944382856938176, + "angularVelocity": -4.752838655296773e-7, + "velocityX": -3.7083484283780552, + "velocityY": 0.7425223718836313, + "timestamp": 3.565811405568753 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.05351191132153618, + "angularVelocity": -0.04844766906462906, + "velocityX": -3.5764423817525626, + "velocityY": 1.0352299927261919, + "timestamp": 3.639448146685006 + }, + { + "x": 5.427595279774474, + "y": 4.2855510061769815, + "heading": -0.058564536366975395, + "angularVelocity": -0.07109019818968403, + "velocityX": -3.3536640954167023, + "velocityY": 1.2508460219498765, + "timestamp": 3.7105215882297555 + }, + { + "x": 5.210001648120265, + "y": 4.381839057848659, + "heading": -0.06451807274962795, + "angularVelocity": -0.08376597858855642, + "velocityX": -3.0615322253278787, + "velocityY": 1.3547683857556179, + "timestamp": 3.781595029774505 + }, + { + "x": 5.014433901254649, + "y": 4.478870971040357, + "heading": -0.07079419051912546, + "angularVelocity": -0.08830468362146032, + "velocityX": -2.751629056016378, + "velocityY": 1.365234482568344, + "timestamp": 3.8526684713192543 + }, + { + "x": 4.840611358699117, + "y": 4.572318839972298, + "heading": -0.07701240147326043, + "angularVelocity": -0.08748993743633168, + "velocityX": -2.445675047916283, + "velocityY": 1.3148071473801226, + "timestamp": 3.9237419128640036 + }, + { + "x": 4.687900440256207, + "y": 4.6594427038525446, + "heading": -0.08292539389195669, + "angularVelocity": -0.08319552691103697, + "velocityX": -2.148635483575938, + "velocityY": 1.2258286919367853, + "timestamp": 3.994815354408753 + }, + { + "x": 4.555672965258425, + "y": 4.738435503151125, + "heading": -0.088365740210127, + "angularVelocity": -0.07654541837185252, + "velocityX": -1.8604343918610988, + "velocityY": 1.1114249933829867, + "timestamp": 4.065888795953502 + }, + { + "x": 4.443385396304237, + "y": 4.808043311257638, + "heading": -0.09321471505627854, + "angularVelocity": -0.06822484940592717, + "velocityX": -1.5798808459765967, + "velocityY": 0.9793786060392976, + "timestamp": 4.136962237498252 + }, + { + "x": 4.3505830000627, + "y": 4.867355530244293, + "heading": -0.09738458875338916, + "angularVelocity": -0.058669927985478765, + "velocityX": -1.3057253768006336, + "velocityY": 0.8345201484201303, + "timestamp": 4.208035679043001 + }, + { + "x": 4.2768869568191645, + "y": 4.915685373463825, + "heading": -0.10080825239426806, + "angularVelocity": -0.04817078737805652, + "velocityX": -1.0368998833007734, + "velocityY": 0.6799986347798151, + "timestamp": 4.2791091205877505 + }, + { + "x": 4.22197990513087, + "y": 4.95249852612905, + "heading": -0.10343286860563979, + "angularVelocity": -0.03692822739868577, + "velocityX": -0.7725396504645569, + "velocityY": 0.5179593370618798, + "timestamp": 4.3501825621325 + }, + { + "x": 4.185593591353172, + "y": 4.97736854606154, + "heading": -0.10521581647883109, + "angularVelocity": -0.025085993226720307, + "velocityX": -0.5119537338682039, + "velocityY": 0.3499200178287514, + "timestamp": 4.421256003677249 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.012749959780663622, + "velocityX": -0.25458913429159136, + "velocityY": 0.1769894567221649, + "timestamp": 4.492329445221999 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": 1.153459712624117e-25, + "velocityX": -3.247224190829055e-25, + "velocityY": -4.023670796703132e-25, + "timestamp": 4.563402886766748 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.5.traj b/src/main/deploy/choreo/CenterLanePCBAFE.5.traj index fa564de5..2eceaa32 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.5.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.5.traj @@ -1,607 +1,662 @@ { - "samples": [ - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": 0, - "velocityX": 8.237561081237056e-34, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 4.18294196758914, - "y": 4.969364336221789, - "heading": -0.10045524830547124, - "angularVelocity": 0.08077125535022475, - "velocityX": 0.22011597884885625, - "velocityY": -0.29338710511594707, - "timestamp": 0.07015802428670526 - }, - { - "x": 4.2148538086473515, - "y": 4.929011600999615, - "heading": -0.08896968745487453, - "angularVelocity": 0.16370986736542367, - "velocityX": 0.4548566095276789, - "velocityY": -0.5751692074062589, - "timestamp": 0.1403160485734105 - }, - { - "x": 4.264519080629496, - "y": 4.870038214957966, - "heading": -0.07148588735689271, - "angularVelocity": 0.24920599283886813, - "velocityX": 0.7079057953397331, - "velocityY": -0.8405793441481644, - "timestamp": 0.21047407286011577 - }, - { - "x": 4.333555645892384, - "y": 4.794138441539978, - "heading": -0.04780159185128592, - "angularVelocity": 0.3375849839901328, - "velocityX": 0.9840152422303832, - "velocityY": -1.081840234095224, - "timestamp": 0.280632097146821 - }, - { - "x": 4.423967613250648, - "y": 4.703932489290492, - "heading": -0.017716503350327845, - "angularVelocity": 0.4288189242332906, - "velocityX": 1.2886903284047762, - "velocityY": -1.2857538844146224, - "timestamp": 0.3507901214335263 - }, - { - "x": 4.53803034325044, - "y": 4.603622363293204, - "heading": 0.0188848190363467, - "angularVelocity": 0.5216983054867799, - "velocityX": 1.6257973504736565, - "velocityY": -1.429774099500926, - "timestamp": 0.42094814572023154 - }, - { - "x": 4.677578748555051, - "y": 4.49984021599331, - "heading": 0.06181064388576721, - "angularVelocity": 0.6118448357952799, - "velocityX": 1.9890583682108496, - "velocityY": -1.4792626838489913, - "timestamp": 0.4911061700069368 - }, - { - "x": 4.842258553230926, - "y": 4.40159118744757, - "heading": 0.11019595374155315, - "angularVelocity": 0.6896618077221814, - "velocityX": 2.3472697007957355, - "velocityY": -1.4003961705682948, - "timestamp": 0.561264194293642 - }, - { - "x": 5.028363428376419, - "y": 4.3176283558190685, - "heading": 0.16247927063581383, - "angularVelocity": 0.7452221955481739, - "velocityX": 2.6526527369836272, - "velocityY": -1.1967673332045712, - "timestamp": 0.6314222185803473 - }, - { - "x": 5.230618576677741, - "y": 4.253738965485636, - "heading": 0.2169576127744663, - "angularVelocity": 0.7765090692409344, - "velocityX": 2.8828512541173206, - "velocityY": -0.9106497935623799, - "timestamp": 0.7015802428670526 - }, - { - "x": 5.444331250681613, - "y": 4.2129309832647035, - "heading": 0.2722364606827335, - "angularVelocity": 0.7879191079037022, - "velocityX": 3.0461615214607765, - "velocityY": -0.5816580873795356, - "timestamp": 0.7717382671537578 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.3273042274639755, - "angularVelocity": 0.7849104552346565, - "velocityX": 3.1588757008538453, - "velocityY": -0.23207478938864265, - "timestamp": 0.8418962914404631 - }, - { - "x": 5.81887557997068, - "y": 4.1972183908996925, - "heading": 0.36426543181084436, - "angularVelocity": 0.7759151841130933, - "velocityX": 3.210283328613804, - "velocityY": 0.011951482768166844, - "timestamp": 0.889531915930938 - }, - { - "x": 5.973953798857342, - "y": 4.209469324927334, - "heading": 0.4004931510026103, - "angularVelocity": 0.7605173560600648, - "velocityX": 3.255509307276357, - "velocityY": 0.257180086514691, - "timestamp": 0.937167540421413 - }, - { - "x": 6.130819634438986, - "y": 4.233461866975936, - "heading": 0.4356102472173979, - "angularVelocity": 0.7372023898166727, - "velocityX": 3.2930361942250057, - "velocityY": 0.5036680489703445, - "timestamp": 0.984803164911888 - }, - { - "x": 6.289005438456121, - "y": 4.269254559307002, - "heading": 0.4691390733673604, - "angularVelocity": 0.7038603253047925, - "velocityX": 3.32074588523062, - "velocityY": 0.7513849710991581, - "timestamp": 1.032438789402363 - }, - { - "x": 6.447898093638549, - "y": 4.316894151635714, - "heading": 0.5004573197150548, - "angularVelocity": 0.6574543040567628, - "velocityX": 3.335584594135005, - "velocityY": 1.0000832956066072, - "timestamp": 1.080074413892838 - }, - { - "x": 6.606667388474461, - "y": 4.376390356038531, - "heading": 0.5287266543286071, - "angularVelocity": 0.5934494386486953, - "velocityX": 3.33299492835787, - "velocityY": 1.248985502745193, - "timestamp": 1.127710038383313 - }, - { - "x": 6.764149093497097, - "y": 4.447653029340476, - "heading": 0.5527726659639904, - "angularVelocity": 0.5047905195447082, - "velocityX": 3.3059649518004255, - "velocityY": 1.4959953619626436, - "timestamp": 1.175345662873788 - }, - { - "x": 6.918662738022944, - "y": 4.530331648811429, - "heading": 0.5708777234010636, - "angularVelocity": 0.3800738970199359, - "velocityX": 3.243657371527556, - "velocityY": 1.735646805417357, - "timestamp": 1.222981287364263 - }, - { - "x": 7.06846751913844, - "y": 4.623866769358016, - "heading": 0.5845734852773292, - "angularVelocity": 0.2875109127414552, - "velocityX": 3.144805651607445, - "velocityY": 1.9635539902555645, - "timestamp": 1.270616911854738 - }, - { - "x": 7.211529043645413, - "y": 4.727181705822193, - "heading": 0.5988053175713824, - "angularVelocity": 0.2987644739893997, - "velocityX": 3.0032465415789793, - "velocityY": 2.168858655035301, - "timestamp": 1.3182525363452129 - }, - { - "x": 7.344594975677938, - "y": 4.83690117801315, - "heading": 0.6149173466892092, - "angularVelocity": 0.3382348670803817, - "velocityX": 2.7934121459693166, - "velocityY": 2.303307101870708, - "timestamp": 1.3658881608356879 - }, - { - "x": 7.465817541114002, - "y": 4.947544737637474, - "heading": 0.6314993734499258, - "angularVelocity": 0.34810138290582165, - "velocityX": 2.5447879970651583, - "velocityY": 2.322706184873219, - "timestamp": 1.4135237853261629 - }, - { - "x": 7.575645879536293, - "y": 5.054829291458012, - "heading": 0.6474951019450449, - "angularVelocity": 0.33579340391176205, - "velocityX": 2.305592497149964, - "velocityY": 2.252191610125547, - "timestamp": 1.4611594098166378 - }, - { - "x": 7.675112095346534, - "y": 5.156311134408354, - "heading": 0.6623068665871409, - "angularVelocity": 0.31093881523601674, - "velocityX": 2.0880636472002183, - "velocityY": 2.1303770872287977, - "timestamp": 1.5087950343071128 - }, - { - "x": 7.76511711692712, - "y": 5.250618542879285, - "heading": 0.6755917653702976, - "angularVelocity": 0.27888579031462285, - "velocityX": 1.8894477094255981, - "velocityY": 1.979766392897571, - "timestamp": 1.5564306587975878 - }, - { - "x": 7.846351174360072, - "y": 5.336925095447112, - "heading": 0.6871379054751507, - "angularVelocity": 0.2423845646688635, - "velocityX": 1.7053215592711606, - "velocityY": 1.8118068880378715, - "timestamp": 1.6040662832880628 - }, - { - "x": 7.919337423332426, - "y": 5.414694341101313, - "heading": 0.6968047845571967, - "angularVelocity": 0.2029338165594727, - "velocityX": 1.5321778553978798, - "velocityY": 1.6325858322640792, - "timestamp": 1.6517019077785378 - }, - { - "x": 7.984478950500488, - "y": 5.483555793762207, - "heading": 0.7044936776046891, - "angularVelocity": 0.16141056467161, - "velocityX": 1.3674960256076552, - "velocityY": 1.445587276276031, - "timestamp": 1.6993375322690127 - }, - { - "x": 8.05911422318913, - "y": 5.560200385763573, - "heading": 0.7111561296070518, - "angularVelocity": 0.10289890039880417, - "velocityX": 1.1527118676281318, - "velocityY": 1.1837449989371336, - "timestamp": 1.7640850852218737 - }, - { - "x": 8.119756898561732, - "y": 5.61995838055026, - "heading": 0.7143078455831314, - "angularVelocity": 0.04867698982190534, - "velocityX": 0.9366018112955389, - "velocityY": 0.9229382742880431, - "timestamp": 1.8288326381747346 - }, - { - "x": 8.166344572702991, - "y": 5.662879129934428, - "heading": 0.7141510891264902, - "angularVelocity": -0.002421040633850211, - "velocityX": 0.7195279515069205, - "velocityY": 0.6628937685941038, - "timestamp": 1.8935801911275956 - }, - { - "x": 8.198829842984775, - "y": 5.689000472186179, - "heading": 0.7108395294312745, - "angularVelocity": -0.0511457119873956, - "velocityX": 0.5017219771291601, - "velocityY": 0.4034336598136588, - "timestamp": 1.9583277440804565 - }, - { - "x": 8.217175483703613, - "y": 5.698352336883545, - "heading": 0.7044936776046891, - "angularVelocity": -0.09800913759946452, - "velocityX": 0.28334106668393205, - "velocityY": 0.1444358013680284, - "timestamp": 2.0230752970333175 - }, - { - "x": 8.221318658765867, - "y": 5.6908798741594095, - "heading": 0.6951703689604403, - "angularVelocity": -0.14352542875947455, - "velocityX": 0.06378111032529457, - "velocityY": -0.11503302714668015, - "timestamp": 2.088034577941462 - }, - { - "x": 8.210964366460544, - "y": 5.666753242665305, - "heading": 0.6829793287804198, - "angularVelocity": -0.18767203099521718, - "velocityX": -0.15939665834608732, - "velocityY": -0.3714116159663335, - "timestamp": 2.152993858849607 - }, - { - "x": 8.185835060378174, - "y": 5.6262170290983935, - "heading": 0.6680275277365855, - "angularVelocity": -0.2301718989927961, - "velocityX": -0.3868470483517696, - "velocityY": -0.6240249738021217, - "timestamp": 2.2179531397577517 - }, - { - "x": 8.14559839699319, - "y": 5.569575312921383, - "heading": 0.6504460796726778, - "angularVelocity": -0.2706533665107588, - "velocityX": -0.6194136206938888, - "velocityY": -0.8719572536078877, - "timestamp": 2.2829124206658964 - }, - { - "x": 8.08985004858283, - "y": 5.497215477528086, - "heading": 0.6303993620580922, - "angularVelocity": -0.30860436467780866, - "velocityX": -0.858204518753663, - "velocityY": -1.11392605308573, - "timestamp": 2.347871701574041 - }, - { - "x": 8.018089118716903, - "y": 5.40964606854142, - "heading": 0.6080990792438148, - "angularVelocity": -0.3432963312172538, - "velocityX": -1.1047063462324833, - "velocityY": -1.348066169489957, - "timestamp": 2.412830982482186 - }, - { - "x": 7.9296824410081745, - "y": 5.307560214485479, - "heading": 0.5838269457597958, - "angularVelocity": -0.3736515112958315, - "velocityX": -1.3609553011176063, - "velocityY": -1.5715360858180332, - "timestamp": 2.4777902633903306 - }, - { - "x": 7.823812920396748, - "y": 5.191948689740781, - "heading": 0.5579732566194568, - "angularVelocity": -0.3979983888198644, - "velocityX": -1.6297828290483853, - "velocityY": -1.779753764641825, - "timestamp": 2.5427495442984753 - }, - { - "x": 7.699409898040676, - "y": 5.064316508380435, - "heading": 0.5311065320658849, - "angularVelocity": -0.41359331842916214, - "velocityX": -1.9150923565792266, - "velocityY": -1.9648028669039912, - "timestamp": 2.60770882520662 - }, - { - "x": 7.555088273925506, - "y": 4.927129260587564, - "heading": 0.5041069990932381, - "angularVelocity": -0.4156378056405048, - "velocityX": -2.221724472585296, - "velocityY": -2.111896035100176, - "timestamp": 2.672668106114765 - }, - { - "x": 7.389294037285908, - "y": 4.784758435393721, - "heading": 0.47842631175838873, - "angularVelocity": -0.3953351542047192, - "velocityX": -2.5522794329271568, - "velocityY": -2.1916933685759186, - "timestamp": 2.7376273870229095 - }, - { - "x": 7.201584646398764, - "y": 4.645022669025462, - "heading": 0.45644938425338794, - "angularVelocity": -0.33831851581111005, - "velocityX": -2.8896469952087562, - "velocityY": -2.1511285903218442, - "timestamp": 2.8025866679310543 - }, - { - "x": 6.996114167384252, - "y": 4.5183305598699945, - "heading": 0.4409940125357126, - "angularVelocity": -0.23792399641137907, - "velocityX": -3.16306578739771, - "velocityY": -1.9503311518274693, - "timestamp": 2.867545948839199 - }, - { - "x": 6.77954298581377, - "y": 4.410738153252425, - "heading": 0.4262239025498049, - "angularVelocity": -0.2273748997744145, - "velocityX": -3.3339528785227577, - "velocityY": -1.656305382593562, - "timestamp": 2.9325052297473437 - }, - { - "x": 6.556274698008425, - "y": 4.324091773722086, - "heading": 0.4025359766205174, - "angularVelocity": -0.3646580688413556, - "velocityX": -3.43704986699365, - "velocityY": -1.3338568149000936, - "timestamp": 2.9974645106554885 - }, - { - "x": 6.330620508913663, - "y": 4.259333422241355, - "heading": 0.3712479640794565, - "angularVelocity": -0.4816557711792279, - "velocityX": -3.4737790495840706, - "velocityY": -0.996906840337434, - "timestamp": 3.062423791563633 - }, - { - "x": 6.105727463248204, - "y": 4.216621857922548, - "heading": 0.33499279771425994, - "angularVelocity": -0.5581214240419701, - "velocityX": -3.462061810435755, - "velocityY": -0.6575128868683483, - "timestamp": 3.127383072471778 - }, - { - "x": 5.88368699546657, - "y": 4.195803829122771, - "heading": 0.29554108787827377, - "angularVelocity": -0.6073298423942238, - "velocityX": -3.4181484874441033, - "velocityY": -0.3204781288945418, - "timestamp": 3.1923423533799227 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.2541387867771501, - "angularVelocity": -0.6373577496904229, - "velocityX": -3.351873105763798, - "velocityY": 0.013011927161991938, - "timestamp": 3.2573016342880674 - }, - { - "x": 5.421845873128357, - "y": 4.226158090162152, - "heading": 0.20498652451135702, - "angularVelocity": -0.6557520326399592, - "velocityX": -3.256674335436179, - "velocityY": 0.3936868024775833, - "timestamp": 3.3322571964811623 - }, - { - "x": 5.1888462982312005, - "y": 4.282903811630084, - "heading": 0.15538584116955914, - "angularVelocity": -0.6617345249712095, - "velocityX": -3.1085027992574967, - "velocityY": 0.7570581796417871, - "timestamp": 3.407212758674257 - }, - { - "x": 4.97232882862877, - "y": 4.364011234145266, - "heading": 0.10667736071804554, - "angularVelocity": -0.6498314338038612, - "velocityX": -2.8886111086013, - "velocityY": 1.08207343313948, - "timestamp": 3.482168320867352 - }, - { - "x": 4.778685496187651, - "y": 4.463599711895904, - "heading": 0.06061512765816293, - "angularVelocity": -0.6145272173560689, - "velocityX": -2.5834417990524554, - "velocityY": 1.3286335908479392, - "timestamp": 3.557123883060447 - }, - { - "x": 4.613047571450946, - "y": 4.572114931147961, - "heading": 0.018966273025947666, - "angularVelocity": -0.5556472850530078, - "velocityX": -2.2098149875789206, - "velocityY": 1.4477273744209653, - "timestamp": 3.6320794452535416 - }, - { - "x": 4.476747715125104, - "y": 4.67904248790438, - "heading": -0.017090034062622824, - "angularVelocity": -0.4810357768471503, - "velocityX": -1.8184088323521106, - "velocityY": 1.4265459910894789, - "timestamp": 3.7070350074466365 - }, - { - "x": 4.368193539344783, - "y": 4.776321878924991, - "heading": -0.04707218407807139, - "angularVelocity": -0.3999990012510431, - "velocityX": -1.4482471027389752, - "velocityY": 1.2978275150549403, - "timestamp": 3.7819905696397313 - }, - { - "x": 4.2849948544017495, - "y": 4.858769971637843, - "heading": -0.07089061350138144, - "angularVelocity": -0.3177673374252157, - "velocityX": -1.1099734630593825, - "velocityY": 1.0999596334219341, - "timestamp": 3.856946131832826 - }, - { - "x": 4.224952920077694, - "y": 4.923143218233113, - "heading": -0.08860420951463882, - "angularVelocity": -0.23632130151495423, - "velocityX": -0.8010337400896302, - "velocityY": 0.8588188082618416, - "timestamp": 3.931901694025921 - }, - { - "x": 4.186266834046781, - "y": 4.967343359425017, - "heading": -0.10031361199761639, - "angularVelocity": -0.1562179262002281, - "velocityX": -0.5161202837923176, - "velocityY": 0.5896846064343946, - "timestamp": 4.006857256219016 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.07749108715135035, - "velocityX": -0.250385269598314, - "velocityY": 0.3015711680564945, - "timestamp": 4.081812818412111 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -7.372861805121809e-39, - "velocityX": 4.014609310130765e-40, - "velocityY": -1.5891469244205483e-39, - "timestamp": 4.156768380605206 - } - ] + "samples": [ + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": 1.153459712624117e-25, + "velocityX": -3.247224190829055e-25, + "velocityY": -4.023670796703132e-25, + "timestamp": 0 + }, + { + "x": 4.180652954196475, + "y": 4.972113880358222, + "heading": -0.10282545923039073, + "angularVelocity": 0.04623963021087373, + "velocityX": 0.18450581877512717, + "velocityY": -0.2501512088012644, + "timestamp": 0.07129254179965372 + }, + { + "x": 4.207780209062833, + "y": 4.937082603957029, + "heading": -0.09606696501055557, + "angularVelocity": 0.09479945656626756, + "velocityX": 0.380506209788268, + "velocityY": -0.4913736488683167, + "timestamp": 0.14258508359930744 + }, + { + "x": 4.249890859346282, + "y": 4.885729175148701, + "heading": -0.08564531821991797, + "angularVelocity": 0.1461814451773148, + "velocityX": 0.5906739922639744, + "velocityY": -0.7203197909907696, + "timestamp": 0.21387762539896116 + }, + { + "x": 4.308238374054552, + "y": 4.819303841665251, + "heading": -0.07131614738285201, + "angularVelocity": 0.20099116226398103, + "velocityX": 0.8184238243635266, + "velocityY": -0.931729067398359, + "timestamp": 0.2851701671986149 + }, + { + "x": 4.384365934500173, + "y": 4.739668033977246, + "heading": -0.05278830976865122, + "angularVelocity": 0.2598846547829301, + "velocityX": 1.0678194173459812, + "velocityY": -1.1170285934228166, + "timestamp": 0.3564627089982686 + }, + { + "x": 4.480080041577144, + "y": 4.649696456670524, + "heading": -0.02973745107706772, + "angularVelocity": 0.3233277718777487, + "velocityX": 1.3425542793234355, + "velocityY": -1.262005464183884, + "timestamp": 0.4277552507979223 + }, + { + "x": 4.597147014770728, + "y": 4.553865445568556, + "heading": -0.0018737410680010795, + "angularVelocity": 0.3908362544762254, + "velocityX": 1.642064797220515, + "velocityY": -1.344194058493121, + "timestamp": 0.49904779259757603 + }, + { + "x": 4.7363464626810785, + "y": 4.458651640127929, + "heading": 0.030870956626381736, + "angularVelocity": 0.4593004663293065, + "velocityX": 1.9525106609542362, + "velocityY": -1.3355366920174645, + "timestamp": 0.5703403343972298 + }, + { + "x": 4.8961444414371185, + "y": 4.371572487351198, + "heading": 0.06809613303796244, + "angularVelocity": 0.5221468539611136, + "velocityX": 2.2414403347422116, + "velocityY": -1.2214342563551692, + "timestamp": 0.6416328761968835 + }, + { + "x": 5.072824953795758, + "y": 4.298842121244781, + "heading": 0.10897366885891092, + "angularVelocity": 0.5733774499978155, + "velocityX": 2.478246782884325, + "velocityY": -1.0201679484342572, + "timestamp": 0.7129254179965372 + }, + { + "x": 5.262214444102689, + "y": 4.244271073764123, + "heading": 0.1525665304554643, + "angularVelocity": 0.6114645444828949, + "velocityX": 2.6565119650124474, + "velocityY": -0.7654524036190654, + "timestamp": 0.7842179597961909 + }, + { + "x": 5.460811083302549, + "y": 4.20986764407754, + "heading": 0.19806311217601916, + "angularVelocity": 0.638167479684045, + "velocityX": 2.785657997129001, + "velocityY": -0.48256702339585567, + "timestamp": 0.8555105015958446 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.24483305037299086, + "angularVelocity": 0.6560284851170571, + "velocityX": 2.8774488935285283, + "velocityY": -0.18541307673169433, + "timestamp": 0.9268030433954983 + }, + { + "x": 5.802993350275697, + "y": 4.197353588266771, + "heading": 0.2759731999769641, + "angularVelocity": 0.6634007595054764, + "velocityX": 2.919495150576939, + "velocityY": 0.015008756790958989, + "timestamp": 0.9737432211956953 + }, + { + "x": 5.941816707126591, + "y": 4.207504283843094, + "heading": 0.3073984177151568, + "angularVelocity": 0.669473768760635, + "velocityX": 2.9574527272095095, + "velocityY": 0.21624748886826958, + "timestamp": 1.0206833989958923 + }, + { + "x": 6.082189718099688, + "y": 4.227142107895406, + "heading": 0.33903536512016264, + "angularVelocity": 0.6739843964730912, + "velocityX": 2.990466111368347, + "velocityY": 0.4183585357494954, + "timestamp": 1.0676235767960893 + }, + { + "x": 6.223826665767993, + "y": 4.256309424549035, + "heading": 0.37079417516105295, + "angularVelocity": 0.6765805228960279, + "velocityX": 3.01739265392617, + "velocityY": 0.6213720957295971, + "timestamp": 1.1145637545962863 + }, + { + "x": 6.366368317456564, + "y": 4.295047066484075, + "heading": 0.4025622544615229, + "angularVelocity": 0.6767779925268211, + "velocityX": 3.0366662072586053, + "velocityY": 0.8252555433413251, + "timestamp": 1.1615039323964833 + }, + { + "x": 6.509351710185931, + "y": 4.3433874902945675, + "heading": 0.4341948054163167, + "angularVelocity": 0.6738907357666759, + "velocityX": 3.0460769308966165, + "velocityY": 1.0298304368648719, + "timestamp": 1.2084441101966803 + }, + { + "x": 6.652162883267978, + "y": 4.401339041781646, + "heading": 0.4654999103087445, + "angularVelocity": 0.6669149193613809, + "velocityX": 3.0424080132360682, + "velocityY": 1.2345831269270133, + "timestamp": 1.2553842879968773 + }, + { + "x": 6.793961557546229, + "y": 4.468849069286898, + "heading": 0.4962146580453544, + "angularVelocity": 0.654338120902485, + "velocityX": 3.0208380309469742, + "velocityY": 1.4382141412546399, + "timestamp": 1.3023244657970743 + }, + { + "x": 6.933563512190389, + "y": 4.545714915555555, + "heading": 0.5259679790741631, + "angularVelocity": 0.6338561638060835, + "velocityX": 2.9740397498786737, + "velocityY": 1.6375278039175576, + "timestamp": 1.3492646435972713 + }, + { + "x": 7.069284940391168, + "y": 4.631368590173797, + "heading": 0.5542337619189218, + "angularVelocity": 0.6021660796657563, + "velocityX": 2.8913701345248803, + "velocityY": 1.8247411627375887, + "timestamp": 1.3962048213974683 + }, + { + "x": 7.1988866969659675, + "y": 4.724417833549329, + "heading": 0.5803339735600787, + "angularVelocity": 0.5560313757705206, + "velocityX": 2.7609984164621943, + "velocityY": 1.9822942250367808, + "timestamp": 1.4431449991976653 + }, + { + "x": 7.32010179733884, + "y": 4.822118790801402, + "heading": 0.6036855499367045, + "angularVelocity": 0.49747524340496785, + "velocityX": 2.5823315132896965, + "velocityY": 2.0813929948868157, + "timestamp": 1.4900851769978622 + }, + { + "x": 7.431787144885325, + "y": 4.920866698749227, + "heading": 0.6241547457610245, + "angularVelocity": 0.43606984003016963, + "velocityX": 2.379312409549823, + "velocityY": 2.103696930338622, + "timestamp": 1.5370253547980592 + }, + { + "x": 7.534091847804722, + "y": 5.017622526169069, + "heading": 0.6419335715808812, + "angularVelocity": 0.37875497394860314, + "velocityX": 2.1794698638522156, + "velocityY": 2.0612582217239557, + "timestamp": 1.5839655325982562 + }, + { + "x": 7.627668873285194, + "y": 5.110394301950092, + "heading": 0.6572905320305523, + "angularVelocity": 0.32716025309998836, + "velocityX": 1.9935379426721815, + "velocityY": 1.9763831354859511, + "timestamp": 1.6309057103984532 + }, + { + "x": 7.7131909565949455, + "y": 5.197947878934244, + "heading": 0.6704631268278166, + "angularVelocity": 0.2806251576918574, + "velocityX": 1.8219377794813416, + "velocityY": 1.8652161343918634, + "timestamp": 1.6778458881986502 + }, + { + "x": 7.791223380396529, + "y": 5.279496011851437, + "heading": 0.6816390937397763, + "angularVelocity": 0.2380895734892706, + "velocityX": 1.6623802349818717, + "velocityY": 1.7372778872782495, + "timestamp": 1.7247860659988472 + }, + { + "x": 7.86221923742719, + "y": 5.354510990808827, + "heading": 0.6909640471575174, + "angularVelocity": 0.19865611624721707, + "velocityX": 1.5124752473852405, + "velocityY": 1.5980974609149294, + "timestamp": 1.7717262437990442 + }, + { + "x": 7.926540429785996, + "y": 5.422621631484432, + "heading": 0.6985519881256509, + "angularVelocity": 0.1616513043566187, + "velocityX": 1.370280117655087, + "velocityY": 1.4510094308871155, + "timestamp": 1.8186664215992412 + }, + { + "x": 7.984478950500488, + "y": 5.483555793762207, + "heading": 0.7044936776046891, + "angularVelocity": 0.12658003777337848, + "velocityX": 1.2343055231087734, + "velocityY": 1.2981238063720744, + "timestamp": 1.8656065993994382 + }, + { + "x": 8.047923980956542, + "y": 5.548807333254465, + "heading": 0.7094925329826927, + "angularVelocity": 0.08420982403970191, + "velocityX": 1.0687836408324876, + "velocityY": 1.099215769102154, + "timestamp": 1.9249685020635123 + }, + { + "x": 8.10152245585906, + "y": 5.602265273787256, + "heading": 0.7122491464591354, + "angularVelocity": 0.04643741781732065, + "velocityX": 0.9029103262715175, + "velocityY": 0.9005429094027965, + "timestamp": 1.9843304047275865 + }, + { + "x": 8.14525837183985, + "y": 5.64394060886005, + "heading": 0.7129744623769576, + "angularVelocity": 0.012218542285053226, + "velocityX": 0.7367674218309503, + "velocityY": 0.7020552442301676, + "timestamp": 2.0436923073916606 + }, + { + "x": 8.179119065532058, + "y": 5.673842216437666, + "heading": 0.7118363633541982, + "angularVelocity": -0.01917221267653394, + "velocityX": 0.5704111925762211, + "velocityY": 0.5037171356657365, + "timestamp": 2.103054210055735 + }, + { + "x": 8.203094267741402, + "y": 5.691977414243475, + "heading": 0.7089715992476469, + "angularVelocity": -0.048259303997766526, + "velocityX": 0.40388197031045997, + "velocityY": 0.3055023001610055, + "timestamp": 2.162416112719809 + }, + { + "x": 8.217175483703613, + "y": 5.698352336883545, + "heading": 0.7044936776046891, + "angularVelocity": -0.07543426746777364, + "velocityX": 0.23720964676445846, + "velocityY": 0.10739080713341471, + "timestamp": 2.221778015383883 + }, + { + "x": 8.220424741398494, + "y": 5.690497924270521, + "heading": 0.6974771095930071, + "angularVelocity": -0.10438216452389025, + "velocityX": 0.04833767031445264, + "velocityY": -0.11684638248302778, + "timestamp": 2.288998004964345 + }, + { + "x": 8.210761211514034, + "y": 5.66775585785793, + "heading": 0.6884846443385353, + "angularVelocity": -0.1337766535013804, + "velocityX": -0.14375976468862095, + "velocityY": -0.338322968428447, + "timestamp": 2.3562179945448065 + }, + { + "x": 8.187932028417118, + "y": 5.630349579704253, + "heading": 0.6774812110255055, + "angularVelocity": -0.1636928744218101, + "velocityX": -0.33961896214800763, + "velocityY": -0.556475512524468, + "timestamp": 2.4234379841252682 + }, + { + "x": 8.15163887353547, + "y": 5.578552873386559, + "heading": 0.6644254264440203, + "angularVelocity": -0.19422473378782149, + "velocityX": -0.5399161039471123, + "velocityY": -0.7705551078030757, + "timestamp": 2.49065797370573 + }, + { + "x": 8.101525228747773, + "y": 5.512708245233119, + "heading": 0.6492678901971639, + "angularVelocity": -0.225491499499757, + "velocityX": -0.745516997257366, + "velocityY": -0.9795393983901896, + "timestamp": 2.5578779632861917 + }, + { + "x": 8.037158859322696, + "y": 5.433255022389235, + "heading": 0.6319488484169726, + "angularVelocity": -0.2576471952507599, + "velocityX": -0.9575480422833206, + "velocityY": -1.1819880267725829, + "timestamp": 2.6250979528666534 + }, + { + "x": 7.958007564987826, + "y": 5.340774148745389, + "heading": 0.6123949630340744, + "angularVelocity": -0.29089390678188665, + "velocityX": -1.177496379110975, + "velocityY": -1.3757942275957413, + "timestamp": 2.692317942447115 + }, + { + "x": 7.863405980160758, + "y": 5.236063150659359, + "heading": 0.5905148719771925, + "angularVelocity": -0.3254997686468183, + "velocityX": -1.4073430450897342, + "velocityY": -1.5577360059048004, + "timestamp": 2.759537932027577 + }, + { + "x": 7.752513083642703, + "y": 5.120268560189199, + "heading": 0.5661934059220105, + "angularVelocity": -0.36181895009176485, + "velocityX": -1.649701185765849, + "velocityY": -1.7226213689241165, + "timestamp": 2.8267579216080385 + }, + { + "x": 7.624272333970784, + "y": 4.995132700896389, + "heading": 0.5392856965961144, + "angularVelocity": -0.40029326832441703, + "velocityX": -1.9077769941992746, + "velocityY": -1.8615870081774413, + "timestamp": 2.8939779111885002 + }, + { + "x": 7.477443680694748, + "y": 4.863466852853807, + "heading": 0.5096191616794202, + "angularVelocity": -0.4413350121273609, + "velocityX": -2.1843004468229417, + "velocityY": -1.958730563101009, + "timestamp": 2.961197900768962 + }, + { + "x": 7.310999420897597, + "y": 4.729976120271212, + "heading": 0.47703933517045805, + "angularVelocity": -0.48467467359488975, + "velocityX": -2.4761125497932377, + "velocityY": -1.9858785075056882, + "timestamp": 3.0284178903494237 + }, + { + "x": 7.12561513664023, + "y": 4.601908860623506, + "heading": 0.44160618415094194, + "angularVelocity": -0.5271222331432077, + "velocityX": -2.757874337892657, + "velocityY": -1.9051960651438506, + "timestamp": 3.0956378799298854 + }, + { + "x": 6.9255695292750366, + "y": 4.487009075034074, + "heading": 0.4039304316786653, + "angularVelocity": -0.5604843545412803, + "velocityX": -2.9759839091575766, + "velocityY": -1.709309779822242, + "timestamp": 3.162857869510347 + }, + { + "x": 6.716882603619491, + "y": 4.389825418108365, + "heading": 0.36497586306620833, + "angularVelocity": -0.5795086975702185, + "velocityX": -3.1045367153136607, + "velocityY": -1.445755310767803, + "timestamp": 3.230077859090809 + }, + { + "x": 6.504347696212581, + "y": 4.311975808393504, + "heading": 0.32552791036504214, + "angularVelocity": -0.5868485393611587, + "velocityX": -3.1617813203096174, + "velocityY": -1.1581318325209748, + "timestamp": 3.2972978486712705 + }, + { + "x": 6.29116656289608, + "y": 4.2538292064786, + "heading": 0.2861053737979932, + "angularVelocity": -0.5864704355519208, + "velocityX": -3.1713949175985174, + "velocityY": -0.8650195020530711, + "timestamp": 3.3645178382517322 + }, + { + "x": 6.079464519610655, + "y": 4.215340449642915, + "heading": 0.24704786030835676, + "angularVelocity": -0.5810401598305069, + "velocityX": -3.1493911945942843, + "velocityY": -0.572579036026387, + "timestamp": 3.431737827832194 + }, + { + "x": 5.870707606275243, + "y": 4.1963435376359834, + "heading": 0.20858746560478297, + "angularVelocity": -0.5721571059980171, + "velocityX": -3.1055778889333454, + "velocityY": -0.28260807723263415, + "timestamp": 3.4989578174126557 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.17089065340850093, + "angularVelocity": -0.560797650097213, + "velocityX": -3.0460563700229573, + "velocityY": 0.004545328262717531, + "timestamp": 3.5661778069931174 + }, + { + "x": 5.441376331690843, + "y": 4.221257817184443, + "heading": 0.12955296713969652, + "angularVelocity": -0.5459093121032401, + "velocityX": -2.965763486744691, + "velocityY": 0.3249853335649047, + "timestamp": 3.6419004322576356 + }, + { + "x": 5.2258290849692735, + "y": 4.2691932604024965, + "heading": 0.08990916431610245, + "angularVelocity": -0.5235397304980968, + "velocityX": -2.8465368965828737, + "velocityY": 0.6330399012263854, + "timestamp": 3.717623057522154 + }, + { + "x": 5.023235975053, + "y": 4.338521550335442, + "heading": 0.05269444989052449, + "angularVelocity": -0.4914609642174678, + "velocityX": -2.6754633665772425, + "velocityY": 0.9155558155936205, + "timestamp": 3.793345682786672 + }, + { + "x": 4.838427025225484, + "y": 4.425425000911894, + "heading": 0.018769229111462726, + "angularVelocity": -0.44801960656478235, + "velocityX": -2.440604101903904, + "velocityY": 1.1476550142427961, + "timestamp": 3.8690683080511903 + }, + { + "x": 4.676043362291933, + "y": 4.523369450086871, + "heading": -0.011096289897494078, + "angularVelocity": -0.3944068091224933, + "velocityX": -2.144453686943697, + "velocityY": 1.2934634639624794, + "timestamp": 3.9447909333157085 + }, + { + "x": 4.53850801944686, + "y": 4.62398511499902, + "heading": -0.03651765926491392, + "angularVelocity": -0.33571695749608116, + "velocityX": -1.8163044712809187, + "velocityY": 1.3287397863013088, + "timestamp": 4.020513558580227 + }, + { + "x": 4.425480168441604, + "y": 4.719703555950957, + "heading": -0.05754144603227514, + "angularVelocity": -0.2776420745308276, + "velocityX": -1.4926562650254422, + "velocityY": 1.2640665932747182, + "timestamp": 4.096236183844745 + }, + { + "x": 4.335238129667097, + "y": 4.805147915212772, + "heading": -0.07443003472477187, + "angularVelocity": -0.22303226589808134, + "velocityX": -1.1917447190884998, + "velocityY": 1.128386119252163, + "timestamp": 4.171958809109263 + }, + { + "x": 4.265861114202726, + "y": 4.876824239203163, + "heading": -0.08748354859792261, + "angularVelocity": -0.17238591276453477, + "velocityX": -0.9161992894728712, + "velocityY": 0.9465641707482745, + "timestamp": 4.247681434373781 + }, + { + "x": 4.215660401957236, + "y": 4.932459766827933, + "heading": -0.09697232555255134, + "angularVelocity": -0.1253096669784238, + "velocityX": -0.6629552537319688, + "velocityY": 0.7347279288115158, + "timestamp": 4.3234040596383 + }, + { + "x": 4.183247912596747, + "y": 4.970524954667524, + "heading": -0.10312305256683521, + "angularVelocity": -0.08122707041386726, + "velocityX": -0.4280423353953174, + "velocityY": 0.5026923948637503, + "timestamp": 4.399126684902818 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.03960437745903177, + "velocityX": -0.2079807341935533, + "velocityY": 0.25649983915041824, + "timestamp": 4.474849310167336 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -1.1491916765427864e-28, + "velocityX": -1.6722970053411976e-26, + "velocityY": -1.611730320150929e-26, + "timestamp": 4.550571935431854 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.traj b/src/main/deploy/choreo/CenterLanePCBAFE.traj index 15ead10a..3ba776c9 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.traj @@ -1,1813 +1,1949 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -8.364664059409844e-30, - "velocityX": -6.773677606947568e-30, - "velocityY": -2.308132540143278e-30, - "timestamp": 0 - }, - { - "x": 1.3079899492454155, - "y": 5.5701428985236205, - "heading": -0.011966271801548406, - "angularVelocity": -0.16457159783011938, - "velocityX": 0.24776633609486393, - "velocityY": -0.2862183198381729, - "timestamp": 0.07271164623703327 - }, - { - "x": 1.3440211933652915, - "y": 5.528520469164234, - "heading": -0.03588810704483216, - "angularVelocity": -0.328995924069994, - "velocityX": 0.49553607960947427, - "velocityY": -0.5724313987522216, - "timestamp": 0.14542329247406655 - }, - { - "x": 1.398068831056723, - "y": 5.466087418740183, - "heading": -0.07173813692543105, - "angularVelocity": -0.49304384837450815, - "velocityX": 0.7433147299769343, - "velocityY": -0.8586389341691013, - "timestamp": 0.21813493871109982 - }, - { - "x": 1.4701339496615213, - "y": 5.382844295736326, - "heading": -0.11947590052159912, - "angularVelocity": -0.6565353154432494, - "velocityX": 0.9911083345055308, - "velocityY": -1.1448389262999967, - "timestamp": 0.2908465849481331 - }, - { - "x": 1.5602181142824867, - "y": 5.2787919544216955, - "heading": -0.17905308310612672, - "angularVelocity": -0.819362312243441, - "velocityX": 1.2389234638197153, - "velocityY": -1.431027169709696, - "timestamp": 0.3635582311851664 - }, - { - "x": 1.668323429942649, - "y": 5.153931763984036, - "heading": -0.25042016061718175, - "angularVelocity": -0.9815082068514337, - "velocityX": 1.4867675434162453, - "velocityY": -1.717196582745939, - "timestamp": 0.4362698774221997 - }, - { - "x": 1.7944527928954905, - "y": 5.008265979195592, - "heading": -0.33353344500312104, - "angularVelocity": -1.1430532616554276, - "velocityX": 1.7346514550472283, - "velocityY": -2.003334986008902, - "timestamp": 0.508981523659233 - }, - { - "x": 1.938611553409707, - "y": 4.8417992681651185, - "heading": -0.42836015839962444, - "angularVelocity": -1.3041475239812899, - "velocityX": 1.98260894865471, - "velocityY": -2.2894091890060286, - "timestamp": 0.5816931698962663 - }, - { - "x": 2.09250704020051, - "y": 4.700832043022275, - "heading": -0.5059805202146961, - "angularVelocity": -1.0675093444479318, - "velocityX": 2.116517707491784, - "velocityY": -1.9387159062918606, - "timestamp": 0.6544048161332996 - }, - { - "x": 2.228385575629257, - "y": 4.5806756631537535, - "heading": -0.5718968876072178, - "angularVelocity": -0.906544835648882, - "velocityX": 1.8687313857908063, - "velocityY": -1.652505287385456, - "timestamp": 0.7271164623703329 - }, - { - "x": 2.346242849494598, - "y": 4.481326236786965, - "heading": -0.6261128487399352, - "angularVelocity": -0.7456296746751315, - "velocityX": 1.6208857861127495, - "velocityY": -1.3663481918284743, - "timestamp": 0.7998281086073662 - }, - { - "x": 2.446077177803767, - "y": 4.402782122456306, - "heading": -0.6686283268670905, - "angularVelocity": -0.5847134582152551, - "velocityX": 1.3730170265673516, - "velocityY": -1.080213671266156, - "timestamp": 0.8725397548443995 - }, - { - "x": 2.5278876403134056, - "y": 4.3450423723526415, - "heading": -0.6994420265312672, - "angularVelocity": -0.423779425385222, - "velocityX": 1.12513561096113, - "velocityY": -0.7940921858762265, - "timestamp": 0.9452514010814328 - }, - { - "x": 2.591673696422735, - "y": 4.308106404528037, - "heading": -0.718552253950641, - "angularVelocity": -0.2628220980484028, - "velocityX": 0.8772467604995492, - "velocityY": -0.5079787039876672, - "timestamp": 1.017963047318466 - }, - { - "x": 2.637435055651834, - "y": 4.2919738867991235, - "heading": -0.7259572155509334, - "angularVelocity": -0.10184010376417504, - "velocityX": 0.6293539150825374, - "velocityY": -0.22186979060047113, - "timestamp": 1.0906746935554992 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.05916659407406203, - "velocityX": 0.38145976628941564, - "velocityY": 0.06423731409121712, - "timestamp": 1.1633863397925324 - }, - { - "x": 2.6747076115982096, - "y": 4.323065260999168, - "heading": -0.7050775877835139, - "angularVelocity": 0.2234613634683759, - "velocityX": 0.12854299211755085, - "velocityY": 0.35614342430781004, - "timestamp": 1.237571545918339 - }, - { - "x": 2.6654811031724366, - "y": 4.37114137531458, - "heading": -0.6763165724415564, - "angularVelocity": 0.38769205939503093, - "velocityX": -0.1243712717061632, - "velocityY": 0.6480552770879449, - "timestamp": 1.3117567520441455 - }, - { - "x": 2.6374924128586628, - "y": 4.440873637695316, - "heading": -0.6353766207501428, - "angularVelocity": 0.5518613996935385, - "velocityX": -0.3772812906679928, - "velocityY": 0.9399753133925751, - "timestamp": 1.3859419581699521 - }, - { - "x": 2.590742000006679, - "y": 4.532262849422754, - "heading": -0.5822609370446827, - "angularVelocity": 0.7159875462663353, - "velocityX": -0.6301851176982644, - "velocityY": 1.231906150790299, - "timestamp": 1.4601271642957587 - }, - { - "x": 2.525230488767617, - "y": 4.645309988591984, - "heading": -0.5169687243847326, - "angularVelocity": 0.8801244354174206, - "velocityX": -0.8830805313123746, - "velocityY": 1.5238501726898892, - "timestamp": 1.5343123704215653 - }, - { - "x": 2.4409586345992826, - "y": 4.780016081558229, - "heading": -0.4394900359726193, - "angularVelocity": 1.0443954051766633, - "velocityX": -1.1359657615273402, - "velocityY": 1.8158080296257462, - "timestamp": 1.608497576547372 - }, - { - "x": 2.337926784653071, - "y": 4.936381539754754, - "heading": -0.3497973159677186, - "angularVelocity": 1.2090378215045976, - "velocityX": -1.3888463122287895, - "velocityY": 2.107771432581833, - "timestamp": 1.6826827826731785 - }, - { - "x": 2.2066781223966365, - "y": 5.088037063238321, - "heading": -0.26257787440108193, - "angularVelocity": 1.1756985809634768, - "velocityX": -1.7692026362756765, - "velocityY": 2.044282565348452, - "timestamp": 1.756867988798985 - }, - { - "x": 2.094180452686652, - "y": 5.218026595083209, - "heading": -0.18768356705291486, - "angularVelocity": 1.009558526050363, - "velocityX": -1.5164434471606913, - "velocityY": 1.7522298398549971, - "timestamp": 1.8310531949247917 - }, - { - "x": 2.0004335399095625, - "y": 5.326350877049268, - "heading": -0.12517882612598913, - "angularVelocity": 0.8425499394477148, - "velocityX": -1.2636874340540831, - "velocityY": 1.460187113126797, - "timestamp": 1.9052384010505983 - }, - { - "x": 1.9254366143080366, - "y": 5.4130101547737155, - "heading": -0.07512185913342963, - "angularVelocity": 0.6747567285776663, - "velocityX": -1.0109417971010437, - "velocityY": 1.1681476975361738, - "timestamp": 1.9794236071764049 - }, - { - "x": 1.869189070291752, - "y": 5.478004602849777, - "heading": -0.037557912625022415, - "angularVelocity": 0.5063536043287745, - "velocityX": -0.7582043233686032, - "velocityY": 0.876110635424555, - "timestamp": 2.0536088133022115 - }, - { - "x": 1.8316905948823166, - "y": 5.5213343174096465, - "heading": -0.012514751718529437, - "angularVelocity": 0.3375762124977515, - "velocityX": -0.5054710685139713, - "velocityY": 0.5840748691542834, - "timestamp": 2.127794019428018 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.368719711095728e-29, - "angularVelocity": 0.1686960564308085, - "velocityX": -0.2527377395078362, - "velocityY": 0.2920386866937519, - "timestamp": 2.2019792255538246 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 1.1956060311838367e-29, - "angularVelocity": -1.025164181333131e-28, - "velocityX": 1.21885672142624e-30, - "velocityY": -5.1343833235782786e-30, - "timestamp": 2.2761644316796312 - }, - { - "x": 1.8602093956334294, - "y": 5.5430660724686875, - "heading": -1.6999094202020734e-20, - "angularVelocity": -1.7890242544799916e-19, - "velocityX": 0.49746156397034486, - "velocityY": 0.0007030702226192673, - "timestamp": 2.3711832340882935 - }, - { - "x": 1.9547457978922331, - "y": 5.543199682247202, - "heading": -3.210902716840507e-20, - "angularVelocity": -1.5902045262092918e-19, - "velocityX": 0.9949231085045297, - "velocityY": 0.001406140417769105, - "timestamp": 2.4662020364969557 - }, - { - "x": 2.0965503966634373, - "y": 5.543400096908449, - "heading": -4.532979164623425e-20, - "angularVelocity": -1.3913840358532023e-19, - "velocityX": 1.492384614166397, - "velocityY": 0.0021092105579800877, - "timestamp": 2.561220838905618 - }, - { - "x": 2.2856231808662413, - "y": 5.543667316436768, - "heading": -7.554638691710094e-20, - "angularVelocity": -3.1800648403053284e-19, - "velocityX": 1.9898460032113345, - "velocityY": 0.0028122805333745363, - "timestamp": 2.65623964131428 - }, - { - "x": 2.474695965069045, - "y": 5.543934535965086, - "heading": -6.169486717931722e-20, - "angularVelocity": 1.457766188026957e-19, - "velocityX": 1.9898460032113345, - "velocityY": 0.002812280533374536, - "timestamp": 2.7512584437229424 - }, - { - "x": 2.6165005638402494, - "y": 5.544134950626333, - "heading": -3.084743805748469e-20, - "angularVelocity": 3.2464552635862006e-19, - "velocityX": 1.492384614166397, - "velocityY": 0.0021092105579800877, - "timestamp": 2.8462772461316046 - }, - { - "x": 2.711036966099053, - "y": 5.544268560404848, - "heading": -1.8891361886639493e-21, - "angularVelocity": 3.047639112965043e-19, - "velocityX": 0.9949231085045297, - "velocityY": 0.001406140417769105, - "timestamp": 2.941296048540267 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -8.137968740525874e-34, - "angularVelocity": 1.9881709101649526e-20, - "velocityX": 0.49746156397034486, - "velocityY": 0.0007030702226192673, - "timestamp": 3.036314850948929 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -8.137968740525874e-34, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -9.422453604168856e-36, - "timestamp": 3.1313336533575913 - }, - { - "x": 2.744670053824415, - "y": 5.560420833392183, - "heading": 0.008590361313936763, - "angularVelocity": 0.13502733079351698, - "velocityX": -0.21432312628245512, - "velocityY": 0.2528389368381776, - "timestamp": 3.1949530796133994 - }, - { - "x": 2.717653230778774, - "y": 5.592802243922747, - "heading": 0.025816975580253556, - "angularVelocity": 0.27077600789812356, - "velocityX": -0.4246631042695725, - "velocityY": 0.5089862080862002, - "timestamp": 3.2585725058692074 - }, - { - "x": 2.6776153836811907, - "y": 5.641767759246714, - "heading": 0.051750317714318324, - "angularVelocity": 0.4076324427980389, - "velocityX": -0.629333671394567, - "velocityY": 0.7696629505440931, - "timestamp": 3.3221919321250155 - }, - { - "x": 2.6251092372323575, - "y": 5.707734732807458, - "heading": 0.0864931852991208, - "angularVelocity": 0.5461046983527398, - "velocityX": -0.8253162522672026, - "velocityY": 1.0368998502988076, - "timestamp": 3.3858113583808236 - }, - { - "x": 2.5610825168108655, - "y": 5.791356317104455, - "heading": 0.1301885817245676, - "angularVelocity": 0.686824748304888, - "velocityX": -1.006402040849065, - "velocityY": 1.314403307580339, - "timestamp": 3.4494307846366317 - }, - { - "x": 2.48749773492398, - "y": 5.89376509224382, - "heading": 0.18299253621322714, - "angularVelocity": 0.8299973388055953, - "velocityX": -1.1566401367878931, - "velocityY": 1.6097091905165586, - "timestamp": 3.5130502108924397 - }, - { - "x": 2.410156329475762, - "y": 6.016926517894053, - "heading": 0.24447965431113647, - "angularVelocity": 0.9664833796311684, - "velocityX": -1.215688508997792, - "velocityY": 1.9359090909592849, - "timestamp": 3.576669637148248 - }, - { - "x": 2.3496693828620137, - "y": 6.152928215304285, - "heading": 0.3070071930810449, - "angularVelocity": 0.9828372000478389, - "velocityX": -0.9507622148388409, - "velocityY": 2.137738508728141, - "timestamp": 3.640289063404056 - }, - { - "x": 2.309701308910896, - "y": 6.283843034176789, - "heading": 0.36362933631377437, - "angularVelocity": 0.8900134214517557, - "velocityX": -0.6282369443322133, - "velocityY": 2.0577805644789557, - "timestamp": 3.703908489659864 - }, - { - "x": 2.288494901023514, - "y": 6.40507103844379, - "heading": 0.41290495871744193, - "angularVelocity": 0.7745373591634508, - "velocityX": -0.3333322718459105, - "velocityY": 1.905518666256975, - "timestamp": 3.767527915915672 - }, - { - "x": 2.285060268530139, - "y": 6.514915678699493, - "heading": 0.45431002431284384, - "angularVelocity": 0.6508242534114624, - "velocityX": -0.05398716548566081, - "velocityY": 1.7265896082436825, - "timestamp": 3.83114734217148 - }, - { - "x": 2.2988127872129502, - "y": 6.612520892931977, - "heading": 0.48757772897770557, - "angularVelocity": 0.522917395248037, - "velocityX": 0.2161685430408261, - "velocityY": 1.5342045657567287, - "timestamp": 3.894766768427288 - }, - { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "angularVelocity": 0.3925324714794108, - "velocityX": 0.48035043054352317, - "velocityY": 1.3337738607711032, - "timestamp": 3.9583861946830963 - }, - { - "x": 2.3906193814869496, - "y": 6.780920533993851, - "heading": 0.5304788719493444, - "angularVelocity": 0.23275934723222522, - "velocityX": 0.7951498408867412, - "velocityY": 1.0846472026287595, - "timestamp": 4.035411897678633 - }, - { - "x": 2.471491792404757, - "y": 6.8405967325057775, - "heading": 0.5351323155604377, - "angularVelocity": 0.06041416605263601, - "velocityX": 1.0499405753231936, - "velocityY": 0.7747569472411664, - "timestamp": 4.112437600674169 - }, - { - "x": 2.556148280048404, - "y": 6.869576729136414, - "heading": 0.5277899567279947, - "angularVelocity": -0.09532349004160981, - "velocityX": 1.0990680299088356, - "velocityY": 0.37623800242778493, - "timestamp": 4.189463303669705 - }, - { - "x": 2.6173499244947225, - "y": 6.878199709364964, - "heading": 0.518346888239368, - "angularVelocity": -0.12259632981439711, - "velocityX": 0.794561322599876, - "velocityY": 0.11194938693450328, - "timestamp": 4.266489006665242 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.0752536934288536, - "velocityX": 0.4023928513995403, - "velocityY": 0.0187620669061233, - "timestamp": 4.343514709660778 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -4.4995568742972564e-34, - "velocityX": 2.8230892463303505e-33, - "velocityY": 2.0807279166194578e-33, - "timestamp": 4.420540412656314 - }, - { - "x": 2.658706958910465, - "y": 6.86593478207681, - "heading": 0.5043163629464208, - "angularVelocity": -0.14325761626715308, - "velocityX": 0.1802876546124509, - "velocityY": -0.23853061812890558, - "timestamp": 4.4780176824038715 - }, - { - "x": 2.679434758469343, - "y": 6.838509950906704, - "heading": 0.48805940847309726, - "angularVelocity": -0.2828414527120879, - "velocityX": 0.36062602920974224, - "velocityY": -0.4771422040496659, - "timestamp": 4.535494952151429 - }, - { - "x": 2.7105313297582017, - "y": 6.797364836772264, - "heading": 0.464037646160856, - "angularVelocity": -0.4179349926979131, - "velocityX": 0.5410238068968174, - "velocityY": -0.7158501841710775, - "timestamp": 4.592972221898986 - }, - { - "x": 2.7520007950829926, - "y": 6.742492873007072, - "heading": 0.4325704761896421, - "angularVelocity": -0.5474715502218408, - "velocityX": 0.721493305213118, - "velocityY": -0.954672412350015, - "timestamp": 4.650449491646543 - }, - { - "x": 2.803848241216662, - "y": 6.673886273182379, - "heading": 0.39405966194845793, - "angularVelocity": -0.6700181551824853, - "velocityX": 0.9020513041309295, - "velocityY": -1.1936301102334148, - "timestamp": 4.7079267613941 - }, - { - "x": 2.866080023237396, - "y": 6.591535755483362, - "heading": 0.3490218715149645, - "angularVelocity": -0.7835756748937736, - "velocityX": 1.0827198698556684, - "velocityY": -1.4327492948204654, - "timestamp": 4.7654040311416574 - }, - { - "x": 2.938704098816793, - "y": 6.495430149531881, - "heading": 0.29814315673038183, - "angularVelocity": -0.8851971398092591, - "velocityX": 1.2635268845991694, - "velocityY": -1.6720628236793682, - "timestamp": 4.822881300889215 - }, - { - "x": 3.0217302964606527, - "y": 6.385555893825644, - "heading": 0.2423799369377359, - "angularVelocity": -0.9701786469253116, - "velocityX": 1.4445048974753851, - "velocityY": -1.9116122980233827, - "timestamp": 4.880358570636772 - }, - { - "x": 3.1151700095083186, - "y": 6.261896797084101, - "heading": 0.1831716018579807, - "angularVelocity": -1.0301173897055516, - "velocityX": 1.6256811337430896, - "velocityY": -2.151443471213232, - "timestamp": 4.937835840384329 - }, - { - "x": 3.2190322842248182, - "y": 6.124436959606287, - "heading": 0.12297302537717429, - "angularVelocity": -1.0473457898261622, - "velocityX": 1.8070147585761873, - "velocityY": -2.39155127029422, - "timestamp": 4.995313110131886 - }, - { - "x": 3.3332908123673026, - "y": 5.973192452697109, - "heading": 0.06701518728601086, - "angularVelocity": -0.9735646515036827, - "velocityX": 1.9878906678120518, - "velocityY": -2.6313794578874745, - "timestamp": 5.052790379879443 - }, - { - "x": 3.457219648102922, - "y": 5.80867115561654, - "heading": 0.033419490428721374, - "angularVelocity": -0.5845040483802287, - "velocityX": 2.1561364393249893, - "velocityY": -2.8623714696810656, - "timestamp": 5.1102676496270005 - }, - { - "x": 3.588371868131788, - "y": 5.635317112607293, - "heading": 0.03341946780631197, - "angularVelocity": -3.935887960456646e-7, - "velocityX": 2.2818101939234974, - "velocityY": -3.0160451909185437, - "timestamp": 5.167744919374558 - }, - { - "x": 3.719524526285539, - "y": 5.461963401064903, - "heading": 0.033419445184933325, - "angularVelocity": -3.9357086276096175e-7, - "velocityX": 2.281817816499982, - "velocityY": -3.0160394239978356, - "timestamp": 5.225222189122115 - }, - { - "x": 3.8561567314585825, - "y": 5.292894866069507, - "heading": 0.0334194225694706, - "angularVelocity": -3.9346793646231113e-7, - "velocityX": 2.3771519728257604, - "velocityY": -2.9414851425259596, - "timestamp": 5.282699458869672 - }, - { - "x": 4.00579710900262, - "y": 5.135223415824853, - "heading": 0.03341939963642931, - "angularVelocity": -3.9899322628139097e-7, - "velocityX": 2.6034705232392743, - "velocityY": -2.743196587749476, - "timestamp": 5.340176728617229 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.03341937596859772, - "angularVelocity": -4.117772415638071e-7, - "velocityX": 2.813320067337111, - "velocityY": -2.527531676347718, - "timestamp": 5.397653998364786 - }, - { - "x": 4.277933740289957, - "y": 4.900652577427371, - "heading": 0.03341935253431191, - "angularVelocity": -6.240523322284166e-7, - "velocityX": 2.940862673655656, - "velocityY": -2.37792138300258, - "timestamp": 5.435205794264698 - }, - { - "x": 4.3928594316694705, - "y": 4.81721673895127, - "heading": 0.033419330076064284, - "angularVelocity": -5.980605477839765e-7, - "velocityX": 3.060457925523134, - "velocityY": -2.2218867693700792, - "timestamp": 5.47275759016461 - }, - { - "x": 4.5119655190903245, - "y": 4.739865631807423, - "heading": 0.03341930829497473, - "angularVelocity": -5.800279064172658e-7, - "velocityX": 3.1717813906506853, - "velocityY": -2.0598510747665353, - "timestamp": 5.510309386064522 - }, - { - "x": 4.634929904279705, - "y": 4.6688078109694935, - "heading": 0.033419286924895195, - "angularVelocity": -5.690827567994812e-7, - "velocityX": 3.2745274158690463, - "velocityY": -1.8922615852334157, - "timestamp": 5.547861181964434 - }, - { - "x": 4.761195179582567, - "y": 4.6037960104372555, - "heading": 0.03341926571852252, - "angularVelocity": -5.647232617468814e-7, - "velocityX": 3.362429739429795, - "velocityY": -1.7312567608088656, - "timestamp": 5.585412977864346 - }, - { - "x": 4.8874652266060234, - "y": 4.538793478329322, - "heading": 0.03341924451230877, - "angularVelocity": -5.647190300189245e-7, - "velocityX": 3.3625568097996403, - "velocityY": -1.7310099437370914, - "timestamp": 5.622964773764258 - }, - { - "x": 5.0137352745443575, - "y": 4.473790947998577, - "heading": 0.03341922330609504, - "angularVelocity": -5.647190293196981e-7, - "velocityX": 3.3625568341627456, - "velocityY": -1.731009896410775, - "timestamp": 5.6605165696641695 - }, - { - "x": 5.140005505333594, - "y": 4.408788772864344, - "heading": 0.03341920209988365, - "angularVelocity": -5.64718967252867e-7, - "velocityX": 3.362561703461214, - "velocityY": -1.7310004375685482, - "timestamp": 5.698068365564081 - }, - { - "x": 5.267176463182282, - "y": 4.34556692987905, - "heading": 0.033419180889344834, - "angularVelocity": -5.648342058496427e-7, - "velocityX": 3.3865479613183083, - "velocityY": -1.6835903974819586, - "timestamp": 5.735620161463993 - }, - { - "x": 5.39745989861742, - "y": 4.289036580908955, - "heading": 0.03341915955151481, - "angularVelocity": -5.682239561025287e-7, - "velocityX": 3.4694328809835504, - "velocityY": -1.5053966825119904, - "timestamp": 5.773171957363905 - }, - { - "x": 5.530504784906373, - "y": 4.239352458442723, - "heading": 0.03341913783750815, - "angularVelocity": -5.782414966353427e-7, - "velocityX": 3.542969999186219, - "velocityY": -1.323082459189313, - "timestamp": 5.810723753263817 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.033419115463141504, - "angularVelocity": -5.958268068735077e-7, - "velocityX": 3.6069365171092738, - "velocityY": -1.1371861948253605, - "timestamp": 5.848275549163729 - }, - { - "x": 5.808220957669321, - "y": 4.160073254115033, - "heading": 0.03341909344161441, - "angularVelocity": -5.669632847998976e-7, - "velocityX": 3.6628444951256323, - "velocityY": -0.941676170142482, - "timestamp": 5.887116736398908 - }, - { - "x": 5.952250404538064, - "y": 4.131196918833519, - "heading": 0.033419072126499465, - "angularVelocity": -5.487760919877281e-7, - "velocityX": 3.708162832321777, - "velocityY": -0.7434462573626127, - "timestamp": 5.925957923634087 - }, - { - "x": 6.09762364792411, - "y": 4.110103210641541, - "heading": 0.03341905123842176, - "angularVelocity": -5.377816485869475e-7, - "velocityX": 3.7427600373239653, - "velocityY": -0.5430757835556856, - "timestamp": 5.964799110869266 - }, - { - "x": 6.24392007770515, - "y": 4.096848811559285, - "heading": 0.0334190305197359, - "angularVelocity": -5.334205087871439e-7, - "velocityX": 3.7665282704987906, - "velocityY": -0.3412459820551187, - "timestamp": 6.003640298104445 - }, - { - "x": 6.390314919287249, - "y": 4.0847296192319185, - "heading": 0.03341900981240726, - "angularVelocity": -5.331281075500129e-7, - "velocityX": 3.7690619675370143, - "velocityY": -0.31201910111516795, - "timestamp": 6.042481485339624 - }, - { - "x": 6.536709800200056, - "y": 4.07261090201266, - "heading": 0.033418989105078956, - "angularVelocity": -5.331280988936517e-7, - "velocityX": 3.769062980140168, - "velocityY": -0.31200686904551156, - "timestamp": 6.081322672574803 - }, - { - "x": 6.68323027882942, - "y": 4.062119297351164, - "heading": 0.03341896797261103, - "angularVelocity": -5.440736864773728e-7, - "velocityX": 3.772296602114613, - "velocityY": -0.27011544724343456, - "timestamp": 6.120163859809982 - }, - { - "x": 6.828478445747191, - "y": 4.059283961769526, - "heading": 0.02725441210059553, - "angularVelocity": -0.15871182913873783, - "velocityX": 3.739539835337918, - "velocityY": -0.07299816981572388, - "timestamp": 6.159005047045161 - }, - { - "x": 6.965940131033012, - "y": 4.0573223410737915, - "heading": 0.017080975304275284, - "angularVelocity": -0.2619239400361572, - "velocityX": 3.5390701229986914, - "velocityY": -0.05050362348240824, - "timestamp": 6.19784623428034 - }, - { - "x": 7.095541107200079, - "y": 4.056112825219225, - "heading": 0.007563330070755733, - "angularVelocity": -0.2450400183673897, - "velocityX": 3.336689359734222, - "velocityY": -0.03114003305929124, - "timestamp": 6.236687421515519 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, - "angularVelocity": -0.194724482157576, - "velocityX": 3.1345655629883886, - "velocityY": -0.012732337027157752, - "timestamp": 6.275528608750698 - }, - { - "x": 7.3857970557158446, - "y": 4.056503208949951, - "heading": -0.005539516561442828, - "angularVelocity": -0.09285314353970942, - "velocityX": 2.8244854636552765, - "velocityY": 0.014833039029661928, - "timestamp": 6.335187510434235 - }, - { - "x": 7.5357095386456, - "y": 4.058265721996549, - "heading": -0.007640794437617486, - "angularVelocity": -0.03522153135371121, - "velocityX": 2.5128267316246156, - "velocityY": 0.029543169533142722, - "timestamp": 6.394846412117771 - }, - { - "x": 7.667000937583321, - "y": 4.060490426598279, - "heading": -0.007708640614146542, - "angularVelocity": -0.0011372347564986972, - "velocityX": 2.200700905192029, - "velocityY": 0.037290404934565556, - "timestamp": 6.454505313801308 - }, - { - "x": 7.779661923967275, - "y": 4.062917456014214, - "heading": -0.0066168114481254744, - "angularVelocity": 0.018301194544491067, - "velocityX": 1.888418713800162, - "velocityY": 0.0406817649578728, - "timestamp": 6.514164215484844 - }, - { - "x": 7.873689685619936, - "y": 4.06536905162046, - "heading": -0.004961765552273349, - "angularVelocity": 0.027741809673791765, - "velocityX": 1.5760893848068933, - "velocityY": 0.04109354240631638, - "timestamp": 6.573823117168381 - }, - { - "x": 7.949083994059634, - "y": 4.067716008149933, - "heading": -0.003176764942269326, - "angularVelocity": 0.029920105124841948, - "velocityX": 1.263756226013528, - "velocityY": 0.03933958660390468, - "timestamp": 6.633482018851917 - }, - { - "x": 8.005845696502893, - "y": 4.069860191657625, - "heading": -0.0015908607397525072, - "angularVelocity": 0.02658285951909277, - "velocityX": 0.951437268227904, - "velocityY": 0.035940713744034375, - "timestamp": 6.693140920535454 - }, - { - "x": 8.043976076014772, - "y": 4.071724543898811, - "heading": -0.00046248549538235704, - "angularVelocity": 0.018913778372181077, - "velocityX": 0.6391398171247351, - "velocityY": 0.03125019382816773, - "timestamp": 6.75279982221899 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 3.4463826664070866e-33, - "angularVelocity": 0.00775216241552071, - "velocityX": 0.32686633402451676, - "velocityY": 0.02551860543538527, - "timestamp": 6.812458723902527 - }, - { - "x": 8.059339450711764, - "y": 4.074523581107775, - "heading": -0.0006944740465081056, - "angularVelocity": -0.009481698382290156, - "velocityX": -0.0564842507032717, - "velocityY": 0.017429845647464255, - "timestamp": 6.88570235795497 - }, - { - "x": 8.027124349390371, - "y": 4.075207755344852, - "heading": -0.002651211191279339, - "angularVelocity": -0.026715456845985846, - "velocityX": -0.43983482985464195, - "velocityY": 0.009341074428224473, - "timestamp": 6.958945992007413 - }, - { - "x": 7.96683125907587, - "y": 4.075299477238389, - "heading": -0.0058702138612145086, - "angularVelocity": -0.043949248444312826, - "velocityX": -0.8231854016327222, - "velocityY": 0.0012522848534625944, - "timestamp": 7.032189626059856 - }, - { - "x": 7.878460180490193, - "y": 4.074798744928973, - "heading": -0.0103514975430217, - "angularVelocity": -0.06118325148365151, - "velocityX": -1.2065359635542152, - "velocityY": -0.006836530107951673, - "timestamp": 7.105433260112299 - }, - { - "x": 7.7620111146201785, - "y": 4.073705556029158, - "heading": -0.01609509527793809, - "angularVelocity": -0.07841770563710548, - "velocityX": -1.5898865120023296, - "velocityY": -0.01492537766534737, - "timestamp": 7.1786768941647425 - }, - { - "x": 7.617484062863899, - "y": 4.072019907599753, - "heading": -0.023101063438175296, - "angularVelocity": -0.0956529294439546, - "velocityX": -1.9732370413624691, - "velocityY": -0.02301426535169652, - "timestamp": 7.251920528217186 - }, - { - "x": 7.444879027316714, - "y": 4.06974179610174, - "heading": -0.031369488604002356, - "angularVelocity": -0.1128893353367305, - "velocityX": -2.356587542114559, - "velocityY": -0.031103201356480925, - "timestamp": 7.325164162269629 - }, - { - "x": 7.244196011426359, - "y": 4.066871217282435, - "heading": -0.040900495023888005, - "angularVelocity": -0.13012743760176224, - "velocityX": -2.7399379957944494, - "velocityY": -0.0391921954234215, - "timestamp": 7.398407796322072 - }, - { - "x": 7.015435021934675, - "y": 4.063408165830539, - "heading": -0.05169425070794916, - "angularVelocity": -0.14736783371961967, - "velocityX": -3.123288357427606, - "velocityY": -0.047281262005875575, - "timestamp": 7.471651430374515 - }, - { - "x": 6.758596078532163, - "y": 4.059352633648335, - "heading": -0.06375095974291681, - "angularVelocity": -0.1646110162466115, - "velocityX": -3.5066384502250503, - "velocityY": -0.05537043914696755, - "timestamp": 7.544895064426958 - }, - { - "x": 6.481653456514826, - "y": 4.053513800761945, - "heading": -0.06375096147352038, - "angularVelocity": -2.3628040945559003e-8, - "velocityX": -3.7811152545904956, - "velocityY": -0.07971795722490971, - "timestamp": 7.6181386984794015 - }, - { - "x": 6.205372409181029, - "y": 4.073516064315128, - "heading": -0.06375096323470678, - "angularVelocity": -2.4045589953178915e-8, - "velocityX": -3.7720827333058655, - "velocityY": 0.2730921780705308, - "timestamp": 7.691382332531845 - }, - { - "x": 5.9325368870621995, - "y": 4.121391787593611, - "heading": -0.0637509651063581, - "angularVelocity": -2.555377459766275e-8, - "velocityX": -3.725040758128897, - "velocityY": 0.6536502987304462, - "timestamp": 7.764625966584288 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -0.063750967187294, - "angularVelocity": -2.84111505129003e-8, - "velocityX": -3.639704142076304, - "velocityY": 1.0274925313911556, - "timestamp": 7.837869600636731 - }, - { - "x": 5.417504232216337, - "y": 4.293821782165803, - "heading": -0.06375097411514968, - "angularVelocity": -9.821339780633191e-8, - "velocityX": -3.5221393138546646, - "velocityY": 1.3775780331429743, - "timestamp": 7.90840840517157 - }, - { - "x": 5.186014168311204, - "y": 4.410724788288498, - "heading": -0.06922328693791815, - "angularVelocity": -0.07757875766190163, - "velocityX": -3.281740673543775, - "velocityY": 1.6572864665569722, - "timestamp": 7.9789472097064085 - }, - { - "x": 4.979961148964822, - "y": 4.522014233902487, - "heading": -0.07551597396386121, - "angularVelocity": -0.08920886974821321, - "velocityX": -2.9211300177991313, - "velocityY": 1.5777052977843462, - "timestamp": 8.049486014241246 - }, - { - "x": 4.7980291404215, - "y": 4.623466282585381, - "heading": -0.08170492719079357, - "angularVelocity": -0.08773827778546625, - "velocityX": -2.579176238427287, - "velocityY": 1.4382445145180842, - "timestamp": 8.120024818776084 - }, - { - "x": 4.639563192627529, - "y": 4.713607705102715, - "heading": -0.0874491132160183, - "angularVelocity": -0.08143299369906003, - "velocityX": -2.246507420120879, - "velocityY": 1.27789835838249, - "timestamp": 8.190563623310922 - }, - { - "x": 4.504192007690659, - "y": 4.791698947450768, - "heading": -0.09257307344830198, - "angularVelocity": -0.07264030438385177, - "velocityX": -1.919102341322099, - "velocityY": 1.1070678453231335, - "timestamp": 8.26110242784576 - }, - { - "x": 4.391678654940531, - "y": 4.857296763925047, - "heading": -0.09697023729098278, - "angularVelocity": -0.0623368069770602, - "velocityX": -1.5950561324662005, - "velocityY": 0.9299536178257779, - "timestamp": 8.331641232380598 - }, - { - "x": 4.301859289367306, - "y": 4.910106206970951, - "heading": -0.10056909340764136, - "angularVelocity": -0.051019522380495494, - "velocityX": -1.2733326878096392, - "velocityY": 0.7486580385668955, - "timestamp": 8.402180036915436 - }, - { - "x": 4.234614001747884, - "y": 4.949916980366573, - "heading": -0.10331834169263278, - "angularVelocity": -0.03897497700905296, - "velocityX": -0.9533091475375062, - "velocityY": 0.5643811751297677, - "timestamp": 8.472718841450273 - }, - { - "x": 4.18985129168719, - "y": 4.976571613170809, - "heading": -0.10517936365869324, - "angularVelocity": -0.026382953019019773, - "velocityX": -0.6345827712261082, - "velocityY": 0.37787191007854753, - "timestamp": 8.543257645985111 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.013363372792080578, - "velocityX": -0.31687843925651377, - "velocityY": 0.18962871266842563, - "timestamp": 8.61379645051995 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": 0, - "velocityX": 8.237561081237056e-34, - "velocityY": 0, - "timestamp": 8.684335255054787 - }, - { - "x": 4.18294196758914, - "y": 4.969364336221789, - "heading": -0.10045524830547124, - "angularVelocity": 0.08077125535022475, - "velocityX": 0.22011597884885625, - "velocityY": -0.29338710511594707, - "timestamp": 8.754493279341492 - }, - { - "x": 4.2148538086473515, - "y": 4.929011600999615, - "heading": -0.08896968745487453, - "angularVelocity": 0.16370986736542367, - "velocityX": 0.4548566095276789, - "velocityY": -0.5751692074062589, - "timestamp": 8.824651303628197 - }, - { - "x": 4.264519080629496, - "y": 4.870038214957966, - "heading": -0.07148588735689271, - "angularVelocity": 0.24920599283886813, - "velocityX": 0.7079057953397331, - "velocityY": -0.8405793441481644, - "timestamp": 8.894809327914903 - }, - { - "x": 4.333555645892384, - "y": 4.794138441539978, - "heading": -0.04780159185128592, - "angularVelocity": 0.3375849839901328, - "velocityX": 0.9840152422303832, - "velocityY": -1.081840234095224, - "timestamp": 8.964967352201608 - }, - { - "x": 4.423967613250648, - "y": 4.703932489290492, - "heading": -0.017716503350327845, - "angularVelocity": 0.4288189242332906, - "velocityX": 1.2886903284047762, - "velocityY": -1.2857538844146224, - "timestamp": 9.035125376488313 - }, - { - "x": 4.53803034325044, - "y": 4.603622363293204, - "heading": 0.0188848190363467, - "angularVelocity": 0.5216983054867799, - "velocityX": 1.6257973504736565, - "velocityY": -1.429774099500926, - "timestamp": 9.105283400775019 - }, - { - "x": 4.677578748555051, - "y": 4.49984021599331, - "heading": 0.06181064388576721, - "angularVelocity": 0.6118448357952799, - "velocityX": 1.9890583682108496, - "velocityY": -1.4792626838489913, - "timestamp": 9.175441425061724 - }, - { - "x": 4.842258553230926, - "y": 4.40159118744757, - "heading": 0.11019595374155315, - "angularVelocity": 0.6896618077221814, - "velocityX": 2.3472697007957355, - "velocityY": -1.4003961705682948, - "timestamp": 9.245599449348429 - }, - { - "x": 5.028363428376419, - "y": 4.3176283558190685, - "heading": 0.16247927063581383, - "angularVelocity": 0.7452221955481739, - "velocityX": 2.6526527369836272, - "velocityY": -1.1967673332045712, - "timestamp": 9.315757473635134 - }, - { - "x": 5.230618576677741, - "y": 4.253738965485636, - "heading": 0.2169576127744663, - "angularVelocity": 0.7765090692409344, - "velocityX": 2.8828512541173206, - "velocityY": -0.9106497935623799, - "timestamp": 9.38591549792184 - }, - { - "x": 5.444331250681613, - "y": 4.2129309832647035, - "heading": 0.2722364606827335, - "angularVelocity": 0.7879191079037022, - "velocityX": 3.0461615214607765, - "velocityY": -0.5816580873795356, - "timestamp": 9.456073522208545 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.3273042274639755, - "angularVelocity": 0.7849104552346565, - "velocityX": 3.1588757008538453, - "velocityY": -0.23207478938864265, - "timestamp": 9.52623154649525 - }, - { - "x": 5.81887557997068, - "y": 4.1972183908996925, - "heading": 0.36426543181084436, - "angularVelocity": 0.7759151841130933, - "velocityX": 3.210283328613804, - "velocityY": 0.011951482768166844, - "timestamp": 9.573867170985725 - }, - { - "x": 5.973953798857342, - "y": 4.209469324927334, - "heading": 0.4004931510026103, - "angularVelocity": 0.7605173560600648, - "velocityX": 3.255509307276357, - "velocityY": 0.257180086514691, - "timestamp": 9.6215027954762 - }, - { - "x": 6.130819634438986, - "y": 4.233461866975936, - "heading": 0.4356102472173979, - "angularVelocity": 0.7372023898166727, - "velocityX": 3.2930361942250057, - "velocityY": 0.5036680489703445, - "timestamp": 9.669138419966675 - }, - { - "x": 6.289005438456121, - "y": 4.269254559307002, - "heading": 0.4691390733673604, - "angularVelocity": 0.7038603253047925, - "velocityX": 3.32074588523062, - "velocityY": 0.7513849710991581, - "timestamp": 9.71677404445715 - }, - { - "x": 6.447898093638549, - "y": 4.316894151635714, - "heading": 0.5004573197150548, - "angularVelocity": 0.6574543040567628, - "velocityX": 3.335584594135005, - "velocityY": 1.0000832956066072, - "timestamp": 9.764409668947625 - }, - { - "x": 6.606667388474461, - "y": 4.376390356038531, - "heading": 0.5287266543286071, - "angularVelocity": 0.5934494386486953, - "velocityX": 3.33299492835787, - "velocityY": 1.248985502745193, - "timestamp": 9.8120452934381 - }, - { - "x": 6.764149093497097, - "y": 4.447653029340476, - "heading": 0.5527726659639904, - "angularVelocity": 0.5047905195447082, - "velocityX": 3.3059649518004255, - "velocityY": 1.4959953619626436, - "timestamp": 9.859680917928575 - }, - { - "x": 6.918662738022944, - "y": 4.530331648811429, - "heading": 0.5708777234010636, - "angularVelocity": 0.3800738970199359, - "velocityX": 3.243657371527556, - "velocityY": 1.735646805417357, - "timestamp": 9.90731654241905 - }, - { - "x": 7.06846751913844, - "y": 4.623866769358016, - "heading": 0.5845734852773292, - "angularVelocity": 0.2875109127414552, - "velocityX": 3.144805651607445, - "velocityY": 1.9635539902555645, - "timestamp": 9.954952166909525 - }, - { - "x": 7.211529043645413, - "y": 4.727181705822193, - "heading": 0.5988053175713824, - "angularVelocity": 0.2987644739893997, - "velocityX": 3.0032465415789793, - "velocityY": 2.168858655035301, - "timestamp": 10.0025877914 - }, - { - "x": 7.344594975677938, - "y": 4.83690117801315, - "heading": 0.6149173466892092, - "angularVelocity": 0.3382348670803817, - "velocityX": 2.7934121459693166, - "velocityY": 2.303307101870708, - "timestamp": 10.050223415890475 - }, - { - "x": 7.465817541114002, - "y": 4.947544737637474, - "heading": 0.6314993734499258, - "angularVelocity": 0.34810138290582165, - "velocityX": 2.5447879970651583, - "velocityY": 2.322706184873219, - "timestamp": 10.09785904038095 - }, - { - "x": 7.575645879536293, - "y": 5.054829291458012, - "heading": 0.6474951019450449, - "angularVelocity": 0.33579340391176205, - "velocityX": 2.305592497149964, - "velocityY": 2.252191610125547, - "timestamp": 10.145494664871425 - }, - { - "x": 7.675112095346534, - "y": 5.156311134408354, - "heading": 0.6623068665871409, - "angularVelocity": 0.31093881523601674, - "velocityX": 2.0880636472002183, - "velocityY": 2.1303770872287977, - "timestamp": 10.1931302893619 - }, - { - "x": 7.76511711692712, - "y": 5.250618542879285, - "heading": 0.6755917653702976, - "angularVelocity": 0.27888579031462285, - "velocityX": 1.8894477094255981, - "velocityY": 1.979766392897571, - "timestamp": 10.240765913852375 - }, - { - "x": 7.846351174360072, - "y": 5.336925095447112, - "heading": 0.6871379054751507, - "angularVelocity": 0.2423845646688635, - "velocityX": 1.7053215592711606, - "velocityY": 1.8118068880378715, - "timestamp": 10.28840153834285 - }, - { - "x": 7.919337423332426, - "y": 5.414694341101313, - "heading": 0.6968047845571967, - "angularVelocity": 0.2029338165594727, - "velocityX": 1.5321778553978798, - "velocityY": 1.6325858322640792, - "timestamp": 10.336037162833325 - }, - { - "x": 7.984478950500488, - "y": 5.483555793762207, - "heading": 0.7044936776046891, - "angularVelocity": 0.16141056467161, - "velocityX": 1.3674960256076552, - "velocityY": 1.445587276276031, - "timestamp": 10.3836727873238 - }, - { - "x": 8.05911422318913, - "y": 5.560200385763573, - "heading": 0.7111561296070518, - "angularVelocity": 0.10289890039880417, - "velocityX": 1.1527118676281318, - "velocityY": 1.1837449989371336, - "timestamp": 10.44842034027666 - }, - { - "x": 8.119756898561732, - "y": 5.61995838055026, - "heading": 0.7143078455831314, - "angularVelocity": 0.04867698982190534, - "velocityX": 0.9366018112955389, - "velocityY": 0.9229382742880431, - "timestamp": 10.513167893229522 - }, - { - "x": 8.166344572702991, - "y": 5.662879129934428, - "heading": 0.7141510891264902, - "angularVelocity": -0.002421040633850211, - "velocityX": 0.7195279515069205, - "velocityY": 0.6628937685941038, - "timestamp": 10.577915446182383 - }, - { - "x": 8.198829842984775, - "y": 5.689000472186179, - "heading": 0.7108395294312745, - "angularVelocity": -0.0511457119873956, - "velocityX": 0.5017219771291601, - "velocityY": 0.4034336598136588, - "timestamp": 10.642662999135243 - }, - { - "x": 8.217175483703613, - "y": 5.698352336883545, - "heading": 0.7044936776046891, - "angularVelocity": -0.09800913759946452, - "velocityX": 0.28334106668393205, - "velocityY": 0.1444358013680284, - "timestamp": 10.707410552088104 - }, - { - "x": 8.221318658765867, - "y": 5.6908798741594095, - "heading": 0.6951703689604403, - "angularVelocity": -0.14352542875947455, - "velocityX": 0.06378111032529457, - "velocityY": -0.11503302714668015, - "timestamp": 10.77236983299625 - }, - { - "x": 8.210964366460544, - "y": 5.666753242665305, - "heading": 0.6829793287804198, - "angularVelocity": -0.18767203099521718, - "velocityX": -0.15939665834608732, - "velocityY": -0.3714116159663335, - "timestamp": 10.837329113904394 - }, - { - "x": 8.185835060378174, - "y": 5.6262170290983935, - "heading": 0.6680275277365855, - "angularVelocity": -0.2301718989927961, - "velocityX": -0.3868470483517696, - "velocityY": -0.6240249738021217, - "timestamp": 10.902288394812539 - }, - { - "x": 8.14559839699319, - "y": 5.569575312921383, - "heading": 0.6504460796726778, - "angularVelocity": -0.2706533665107588, - "velocityX": -0.6194136206938888, - "velocityY": -0.8719572536078877, - "timestamp": 10.967247675720683 - }, - { - "x": 8.08985004858283, - "y": 5.497215477528086, - "heading": 0.6303993620580922, - "angularVelocity": -0.30860436467780866, - "velocityX": -0.858204518753663, - "velocityY": -1.11392605308573, - "timestamp": 11.032206956628828 - }, - { - "x": 8.018089118716903, - "y": 5.40964606854142, - "heading": 0.6080990792438148, - "angularVelocity": -0.3432963312172538, - "velocityX": -1.1047063462324833, - "velocityY": -1.348066169489957, - "timestamp": 11.097166237536973 - }, - { - "x": 7.9296824410081745, - "y": 5.307560214485479, - "heading": 0.5838269457597958, - "angularVelocity": -0.3736515112958315, - "velocityX": -1.3609553011176063, - "velocityY": -1.5715360858180332, - "timestamp": 11.162125518445118 - }, - { - "x": 7.823812920396748, - "y": 5.191948689740781, - "heading": 0.5579732566194568, - "angularVelocity": -0.3979983888198644, - "velocityX": -1.6297828290483853, - "velocityY": -1.779753764641825, - "timestamp": 11.227084799353262 - }, - { - "x": 7.699409898040676, - "y": 5.064316508380435, - "heading": 0.5311065320658849, - "angularVelocity": -0.41359331842916214, - "velocityX": -1.9150923565792266, - "velocityY": -1.9648028669039912, - "timestamp": 11.292044080261407 - }, - { - "x": 7.555088273925506, - "y": 4.927129260587564, - "heading": 0.5041069990932381, - "angularVelocity": -0.4156378056405048, - "velocityX": -2.221724472585296, - "velocityY": -2.111896035100176, - "timestamp": 11.357003361169552 - }, - { - "x": 7.389294037285908, - "y": 4.784758435393721, - "heading": 0.47842631175838873, - "angularVelocity": -0.3953351542047192, - "velocityX": -2.5522794329271568, - "velocityY": -2.1916933685759186, - "timestamp": 11.421962642077697 - }, - { - "x": 7.201584646398764, - "y": 4.645022669025462, - "heading": 0.45644938425338794, - "angularVelocity": -0.33831851581111005, - "velocityX": -2.8896469952087562, - "velocityY": -2.1511285903218442, - "timestamp": 11.486921922985841 - }, - { - "x": 6.996114167384252, - "y": 4.5183305598699945, - "heading": 0.4409940125357126, - "angularVelocity": -0.23792399641137907, - "velocityX": -3.16306578739771, - "velocityY": -1.9503311518274693, - "timestamp": 11.551881203893986 - }, - { - "x": 6.77954298581377, - "y": 4.410738153252425, - "heading": 0.4262239025498049, - "angularVelocity": -0.2273748997744145, - "velocityX": -3.3339528785227577, - "velocityY": -1.656305382593562, - "timestamp": 11.61684048480213 - }, - { - "x": 6.556274698008425, - "y": 4.324091773722086, - "heading": 0.4025359766205174, - "angularVelocity": -0.3646580688413556, - "velocityX": -3.43704986699365, - "velocityY": -1.3338568149000936, - "timestamp": 11.681799765710275 - }, - { - "x": 6.330620508913663, - "y": 4.259333422241355, - "heading": 0.3712479640794565, - "angularVelocity": -0.4816557711792279, - "velocityX": -3.4737790495840706, - "velocityY": -0.996906840337434, - "timestamp": 11.74675904661842 - }, - { - "x": 6.105727463248204, - "y": 4.216621857922548, - "heading": 0.33499279771425994, - "angularVelocity": -0.5581214240419701, - "velocityX": -3.462061810435755, - "velocityY": -0.6575128868683483, - "timestamp": 11.811718327526565 - }, - { - "x": 5.88368699546657, - "y": 4.195803829122771, - "heading": 0.29554108787827377, - "angularVelocity": -0.6073298423942238, - "velocityX": -3.4181484874441033, - "velocityY": -0.3204781288945418, - "timestamp": 11.87667760843471 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.2541387867771501, - "angularVelocity": -0.6373577496904229, - "velocityX": -3.351873105763798, - "velocityY": 0.013011927161991938, - "timestamp": 11.941636889342854 - }, - { - "x": 5.421845873128357, - "y": 4.226158090162152, - "heading": 0.20498652451135702, - "angularVelocity": -0.6557520326399592, - "velocityX": -3.256674335436179, - "velocityY": 0.3936868024775833, - "timestamp": 12.01659245153595 - }, - { - "x": 5.1888462982312005, - "y": 4.282903811630084, - "heading": 0.15538584116955914, - "angularVelocity": -0.6617345249712095, - "velocityX": -3.1085027992574967, - "velocityY": 0.7570581796417871, - "timestamp": 12.091548013729044 - }, - { - "x": 4.97232882862877, - "y": 4.364011234145266, - "heading": 0.10667736071804554, - "angularVelocity": -0.6498314338038612, - "velocityX": -2.8886111086013, - "velocityY": 1.08207343313948, - "timestamp": 12.166503575922139 - }, - { - "x": 4.778685496187651, - "y": 4.463599711895904, - "heading": 0.06061512765816293, - "angularVelocity": -0.6145272173560689, - "velocityX": -2.5834417990524554, - "velocityY": 1.3286335908479392, - "timestamp": 12.241459138115234 - }, - { - "x": 4.613047571450946, - "y": 4.572114931147961, - "heading": 0.018966273025947666, - "angularVelocity": -0.5556472850530078, - "velocityX": -2.2098149875789206, - "velocityY": 1.4477273744209653, - "timestamp": 12.316414700308329 - }, - { - "x": 4.476747715125104, - "y": 4.67904248790438, - "heading": -0.017090034062622824, - "angularVelocity": -0.4810357768471503, - "velocityX": -1.8184088323521106, - "velocityY": 1.4265459910894789, - "timestamp": 12.391370262501423 - }, - { - "x": 4.368193539344783, - "y": 4.776321878924991, - "heading": -0.04707218407807139, - "angularVelocity": -0.3999990012510431, - "velocityX": -1.4482471027389752, - "velocityY": 1.2978275150549403, - "timestamp": 12.466325824694518 - }, - { - "x": 4.2849948544017495, - "y": 4.858769971637843, - "heading": -0.07089061350138144, - "angularVelocity": -0.3177673374252157, - "velocityX": -1.1099734630593825, - "velocityY": 1.0999596334219341, - "timestamp": 12.541281386887613 - }, - { - "x": 4.224952920077694, - "y": 4.923143218233113, - "heading": -0.08860420951463882, - "angularVelocity": -0.23632130151495423, - "velocityX": -0.8010337400896302, - "velocityY": 0.8588188082618416, - "timestamp": 12.616236949080708 - }, - { - "x": 4.186266834046781, - "y": 4.967343359425017, - "heading": -0.10031361199761639, - "angularVelocity": -0.1562179262002281, - "velocityX": -0.5161202837923176, - "velocityY": 0.5896846064343946, - "timestamp": 12.691192511273803 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.07749108715135035, - "velocityX": -0.250385269598314, - "velocityY": 0.3015711680564945, - "timestamp": 12.766148073466898 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -7.372861805121809e-39, - "velocityX": 4.014609310130765e-40, - "velocityY": -1.5891469244205483e-39, - "timestamp": 12.841103635659993 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -1.0042344276654815e-24, + "angularVelocity": -6.621110938678157e-25, + "velocityX": -2.249108899033261e-24, + "velocityY": -2.0825834551113612e-24, + "timestamp": 0 + }, + { + "x": 1.3060567679582313, + "y": 5.572381918180444, + "heading": -0.010695335923114668, + "angularVelocity": -0.14212952691547795, + "velocityX": 0.2137167180313357, + "velocityY": -0.24680705612748235, + "timestamp": 0.07525062634926666 + }, + { + "x": 1.3382215476730719, + "y": 5.535237409026599, + "heading": -0.03207750843931141, + "angularVelocity": -0.2841461068636689, + "velocityX": 0.4274353752957696, + "velocityY": -0.49361063097924024, + "timestamp": 0.15050125269853332 + }, + { + "x": 1.3864692368556266, + "y": 5.479521033340712, + "heading": -0.06412446500663382, + "angularVelocity": -0.4258696322151573, + "velocityX": 0.6411599680063655, + "velocityY": -0.7404107897692007, + "timestamp": 0.22575187904779997 + }, + { + "x": 1.4508006060735943, + "y": 5.40523312636478, + "heading": -0.10680311296182225, + "angularVelocity": -0.5671533916156479, + "velocityX": 0.8548948007340368, + "velocityY": -0.987206493553073, + "timestamp": 0.30100250539706663 + }, + { + "x": 1.5312167667507302, + "y": 5.312374207143024, + "heading": -0.16007302531493992, + "angularVelocity": -0.7078999197411572, + "velocityX": 1.0686444030896685, + "velocityY": -1.2339952997967512, + "timestamp": 0.3762531317463333 + }, + { + "x": 1.6277191879065078, + "y": 5.200945106889031, + "heading": -0.22389124150432238, + "angularVelocity": -0.8480755481446488, + "velocityX": 1.2824135271362858, + "velocityY": -1.480773060104614, + "timestamp": 0.45150375809559995 + }, + { + "x": 1.7403097782501336, + "y": 5.070947160382142, + "heading": -0.2982176402551873, + "angularVelocity": -0.9877180078992028, + "velocityX": 1.496208015878168, + "velocityY": -1.7275330826287059, + "timestamp": 0.5267543844448666 + }, + { + "x": 1.8689915501245091, + "y": 4.922382870770249, + "heading": -0.3830195665066897, + "angularVelocity": -1.126926516968836, + "velocityX": 1.7100425354217623, + "velocityY": -1.9742598410058476, + "timestamp": 0.6020050107941333 + }, + { + "x": 2.021815501878503, + "y": 4.778590249332989, + "heading": -0.46245356659644804, + "angularVelocity": -1.0555925437892708, + "velocityX": 2.0308661757136712, + "velocityY": -1.9108494960541986, + "timestamp": 0.6772556371434 + }, + { + "x": 2.158552321551595, + "y": 4.6533669501943, + "heading": -0.5314334875140496, + "angularVelocity": -0.916669059968213, + "velocityX": 1.8170854690091889, + "velocityY": -1.6640831473944147, + "timestamp": 0.7525062634926667 + }, + { + "x": 2.279199772550662, + "y": 4.546710847450061, + "heading": -0.589963647479443, + "angularVelocity": -0.7778029606522173, + "velocityX": 1.6032750403843345, + "velocityY": -1.417345049717031, + "timestamp": 0.8277568898419334 + }, + { + "x": 2.383756839213222, + "y": 4.45862085446933, + "heading": -0.6380444617813997, + "angularVelocity": -0.6389423800779483, + "velocityX": 1.38945111469599, + "velocityY": -1.1706213921977682, + "timestamp": 0.9030075161912001 + }, + { + "x": 2.4722229084949126, + "y": 4.389096261559967, + "heading": -0.67567478001119, + "angularVelocity": -0.5000665118072721, + "velocityX": 1.1756190423065196, + "velocityY": -0.9239071657247342, + "timestamp": 0.9782581425404668 + }, + { + "x": 2.544597589718017, + "y": 4.338136588288561, + "heading": -0.7028528784398662, + "angularVelocity": -0.36116773703028543, + "velocityX": 0.9617817782298118, + "velocityY": -0.6771993236957614, + "timestamp": 1.0535087688897335 + }, + { + "x": 2.6008806504487216, + "y": 4.305741524929451, + "heading": -0.7195768777220212, + "angularVelocity": -0.22224398777137738, + "velocityX": 0.7479414253573617, + "velocityY": -0.4304955975881504, + "timestamp": 1.1287593952390003 + }, + { + "x": 2.6410719892543444, + "y": 4.2919109040687715, + "heading": -0.7258449001014369, + "angularVelocity": -0.08329528514916955, + "velocityX": 0.5340997245535138, + "velocityY": -0.1837940962309967, + "timestamp": 1.204010021588267 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.055677742612701135, + "velocityX": 0.3202582509251215, + "velocityY": 0.06290689943025622, + "timestamp": 1.2792606479375337 + }, + { + "x": 2.6731116736052027, + "y": 4.320290744259811, + "heading": -0.7067971794351253, + "angularVelocity": 0.19582512759313456, + "velocityX": 0.10464854696510043, + "velocityY": 0.31165110401848706, + "timestamp": 1.355134136920331 + }, + { + "x": 2.664692786668951, + "y": 4.362810218152233, + "heading": -0.6813099274422239, + "angularVelocity": 0.3359177538109565, + "velocityX": -0.1109595334170119, + "velocityY": 0.5603996133888316, + "timestamp": 1.4310076259031281 + }, + { + "x": 2.639915179239627, + "y": 4.424203570363852, + "heading": -0.6451974716199608, + "angularVelocity": 0.47595617792732364, + "velocityX": -0.3265647561685457, + "velocityY": 0.8091541991108215, + "timestamp": 1.5068811148859254 + }, + { + "x": 2.59877917099983, + "y": 4.50447140700047, + "heading": -0.59846321314625, + "angularVelocity": 0.6159497750829267, + "velocityX": -0.5421657655564515, + "velocityY": 1.0579167732067494, + "timestamp": 1.5827546038687226 + }, + { + "x": 2.5412852001349884, + "y": 4.6036144764177696, + "heading": -0.541108297539196, + "angularVelocity": 0.755928274499912, + "velocityX": -0.7577609997330812, + "velocityY": 1.3066892105064276, + "timestamp": 1.6586280928515198 + }, + { + "x": 2.4674338294971827, + "y": 4.7216336217313835, + "heading": -0.4731286056147784, + "angularVelocity": 0.8959610640790578, + "velocityX": -0.973348815612657, + "velocityY": 1.5554727599303189, + "timestamp": 1.734501581834317 + }, + { + "x": 2.3772256595733805, + "y": 4.858529596063364, + "heading": -0.3945096344591076, + "angularVelocity": 1.0361849996577348, + "velocityX": -1.1889287171735974, + "velocityY": 1.8042662353779348, + "timestamp": 1.8103750708171142 + }, + { + "x": 2.270660565338228, + "y": 5.014302224131279, + "heading": -0.305219034790845, + "angularVelocity": 1.1768352933988244, + "velocityX": -1.4045102665479658, + "velocityY": 2.0530574006321625, + "timestamp": 1.8862485597999115 + }, + { + "x": 2.156230166958197, + "y": 5.146478435153338, + "heading": -0.2290932542035935, + "angularVelocity": 1.0033251615002383, + "velocityX": -1.5081736705949598, + "velocityY": 1.7420605377990246, + "timestamp": 1.9621220487827087 + }, + { + "x": 2.05814700584277, + "y": 5.259770794344292, + "heading": -0.16373664121599296, + "angularVelocity": 0.8613893187700765, + "velocityX": -1.292719794889953, + "velocityY": 1.4931745028443373, + "timestamp": 2.037995537765506 + }, + { + "x": 1.976411579927544, + "y": 5.3541805242021825, + "heading": -0.10920088428004432, + "angularVelocity": 0.7187722308158727, + "velocityX": -1.077259356476389, + "velocityY": 1.2443045802110966, + "timestamp": 2.113869026748303 + }, + { + "x": 1.9110235357975711, + "y": 5.4297080461749685, + "heading": -0.06553152377248019, + "angularVelocity": 0.5755549282499098, + "velocityX": -0.8618035760132025, + "velocityY": 0.9954402121920332, + "timestamp": 2.1897425157311 + }, + { + "x": 1.8619825316383047, + "y": 5.486353607136737, + "heading": -0.03276339241185007, + "angularVelocity": 0.43187853623101025, + "velocityX": -0.6463523006090516, + "velocityY": 0.7465790979324988, + "timestamp": 2.265616004713897 + }, + { + "x": 1.829288400520471, + "y": 5.524117342639773, + "heading": -0.010917643304867627, + "angularVelocity": 0.2879233497741951, + "velocityX": -0.43090322530503955, + "velocityY": 0.49771977022960984, + "timestamp": 2.341489493696694 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 5.072711091682045e-24, + "angularVelocity": 0.14389272789792218, + "velocityX": -0.21545347602968715, + "velocityY": 0.24886063882779058, + "timestamp": 2.417362982679491 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 2.471925946464062e-24, + "angularVelocity": -1.3881126396060902e-24, + "velocityX": -6.902910153275516e-22, + "velocityY": 3.1112652084354562e-24, + "timestamp": 2.493236471662288 + }, + { + "x": 1.8507557540076562, + "y": 5.5430527114888575, + "heading": -8.800046052500135e-19, + "angularVelocity": -9.452376611096758e-18, + "velocityX": 0.4061756726920047, + "velocityY": 0.0005740544422031257, + "timestamp": 2.586335499913798 + }, + { + "x": 1.9263848743459357, + "y": 5.543159599309593, + "heading": -2.769921496163024e-18, + "angularVelocity": -2.0300071079064193e-17, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765763312, + "timestamp": 2.679434528165308 + }, + { + "x": 2.0398285536670646, + "y": 5.54331993103902, + "heading": -5.903589755199969e-18, + "angularVelocity": -3.365951630081127e-17, + "velocityX": 1.2185269970236072, + "velocityY": 0.0017221632968556807, + "timestamp": 2.772533556416818 + }, + { + "x": 2.191086789804772, + "y": 5.5435337066740775, + "heading": -1.0884334063348694e-17, + "angularVelocity": -5.34994231591497e-17, + "velocityX": 1.6247026309348636, + "velocityY": 0.0022962176842493674, + "timestamp": 2.865632584668328 + }, + { + "x": 2.380159571927711, + "y": 5.543800926199458, + "heading": -4.173692512602447e-18, + "angularVelocity": 7.208068308331619e-17, + "velocityX": 2.0308781485038967, + "velocityY": 0.0028702719072147666, + "timestamp": 2.958731612919838 + }, + { + "x": 2.5314178080654184, + "y": 5.544014701834515, + "heading": -1.8153111851912927e-18, + "angularVelocity": 2.5331965023903977e-17, + "velocityX": 1.6247026309348636, + "velocityY": 0.002296217684249368, + "timestamp": 3.0518306411713483 + }, + { + "x": 2.644861487386547, + "y": 5.544175033563942, + "heading": -7.161538717554587e-19, + "angularVelocity": 1.1806324234469696e-17, + "velocityX": 1.2185269970236072, + "velocityY": 0.001722163296855681, + "timestamp": 3.1449296694228583 + }, + { + "x": 2.7204906077248263, + "y": 5.544281921384678, + "heading": -1.8348593704837507e-19, + "angularVelocity": 5.72151981311354e-18, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765763316, + "timestamp": 3.2380286976743684 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -9.055003253623357e-30, + "angularVelocity": 1.970868445148275e-18, + "velocityX": 0.4061756726920047, + "velocityY": 0.0005740544422031258, + "timestamp": 3.3311277259258785 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.6159098195352036e-29, + "angularVelocity": -1.6705479392630998e-29, + "velocityX": 6.780641024803866e-22, + "velocityY": 9.718698194414986e-25, + "timestamp": 3.4242267541773885 + }, + { + "x": 2.746560227605712, + "y": 5.55825095759549, + "heading": 0.00742109779657271, + "angularVelocity": 0.11458935665570884, + "velocityX": -0.18135392067783562, + "velocityY": 0.21487100869168943, + "timestamp": 3.4889892941999308 + }, + { + "x": 2.7232728362575633, + "y": 5.58624983061418, + "heading": 0.022298554503038515, + "angularVelocity": 0.22972318104396974, + "velocityX": -0.35958119215279744, + "velocityY": 0.43233129844728024, + "timestamp": 3.553751834222473 + }, + { + "x": 2.6887224275841572, + "y": 5.628555352569631, + "heading": 0.04468536543200197, + "angularVelocity": 0.3456753073794072, + "velocityX": -0.5334937243255126, + "velocityY": 0.6532406224451052, + "timestamp": 3.618514374245015 + }, + { + "x": 2.6433185485147903, + "y": 5.685479106691384, + "heading": 0.07465726375014338, + "angularVelocity": 0.46279683143540923, + "velocityX": -0.7010824321214573, + "velocityY": 0.8789611108819849, + "timestamp": 3.6832769142675574 + }, + { + "x": 2.587716352440023, + "y": 5.7574833976129485, + "heading": 0.11231839668543797, + "angularVelocity": 0.5815264954429781, + "velocityX": -0.8585549000303846, + "velocityY": 1.1118200567257257, + "timestamp": 3.7480394542900997 + }, + { + "x": 2.523118270580454, + "y": 5.845312686573325, + "heading": 0.15779630703388142, + "angularVelocity": 0.7022255509529696, + "velocityX": -0.9974605973929359, + "velocityY": 1.356174247177545, + "timestamp": 3.812801994312642 + }, + { + "x": 2.4523262633845233, + "y": 5.950260122565857, + "heading": 0.21110300167691656, + "angularVelocity": 0.8231100050195744, + "velocityX": -1.093101153402716, + "velocityY": 1.6204959835732518, + "timestamp": 3.877564534335184 + }, + { + "x": 2.385035672029833, + "y": 6.073114038949963, + "heading": 0.27020607963640353, + "angularVelocity": 0.9126121047586251, + "velocityX": -1.0390357038384854, + "velocityY": 1.896990395085535, + "timestamp": 3.9423270743577263 + }, + { + "x": 2.335844965932351, + "y": 6.198572107213467, + "heading": 0.32662517660992413, + "angularVelocity": 0.8711686872362097, + "velocityX": -0.7595549229594762, + "velocityY": 1.9372011693771587, + "timestamp": 4.007089614380269 + }, + { + "x": 2.303814652096041, + "y": 6.317831411051833, + "heading": 0.37736366027868895, + "angularVelocity": 0.7834541951428096, + "velocityX": -0.4945808769260876, + "velocityY": 1.8414858928765832, + "timestamp": 4.071852154402811 + }, + { + "x": 2.2877361979926687, + "y": 6.4282644247646346, + "heading": 0.42161092558434743, + "angularVelocity": 0.6832231300726793, + "velocityX": -0.24826781188285701, + "velocityY": 1.7051989263293559, + "timestamp": 4.1366146944253535 + }, + { + "x": 2.2869063645502266, + "y": 6.528717533266064, + "heading": 0.45901061919668296, + "angularVelocity": 0.5774896043193742, + "velocityX": -0.012813478936328582, + "velocityY": 1.5510989603938259, + "timestamp": 4.201377234447896 + }, + { + "x": 2.3008846173823154, + "y": 6.618554541912377, + "heading": 0.48936477750092344, + "angularVelocity": 0.46869931743991045, + "velocityX": 0.21583855153339288, + "velocityY": 1.3871754970549879, + "timestamp": 4.266139774470438 + }, + { + "x": 2.3293724060058594, + "y": 6.6973748207092285, + "heading": 0.5125504196, + "angularVelocity": 0.3580100794534351, + "velocityX": 0.43988065652810937, + "velocityY": 1.21706589595492, + "timestamp": 4.33090231449298 + }, + { + "x": 2.3907092837867907, + "y": 6.780867422722411, + "heading": 0.5304481214064672, + "angularVelocity": 0.21222434211791516, + "velocityX": 0.7273100577595625, + "velocityY": 0.990024458199304, + "timestamp": 4.415236192632761 + }, + { + "x": 2.471644870352802, + "y": 6.840510057948011, + "heading": 0.53507672796877, + "angularVelocity": 0.05488430823293737, + "velocityX": 0.9597043128013546, + "velocityY": 0.7072203548702543, + "timestamp": 4.499570070772541 + }, + { + "x": 2.556252265889606, + "y": 6.8694746689081185, + "heading": 0.5277386544868528, + "angularVelocity": -0.08701216692246314, + "velocityX": 1.0032432683407548, + "velocityY": 0.3434516661512893, + "timestamp": 4.5839039489123214 + }, + { + "x": 2.617383928971812, + "y": 6.87815517989328, + "heading": 0.5183265349753615, + "angularVelocity": -0.1116054392268187, + "velocityX": 0.7248766976052303, + "velocityY": 0.1029302953526471, + "timestamp": 4.668237827052102 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06849104420156939, + "velocityX": 0.36711922260971214, + "velocityY": 0.01766420444115541, + "timestamp": 4.752571705191882 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -1.2403630086849167e-25, + "velocityX": 1.2566075146445203e-23, + "velocityY": 7.418296886220231e-24, + "timestamp": 4.836905583331663 + }, + { + "x": 2.65710997558339, + "y": 6.867790932222418, + "heading": 0.5069947283791145, + "angularVelocity": -0.09538266194620745, + "velocityX": 0.1504894284206614, + "velocityY": -0.20351386841397834, + "timestamp": 4.895151926089623 + }, + { + "x": 2.6746461929799414, + "y": 6.844083289351916, + "heading": 0.4960269163567629, + "angularVelocity": -0.18830044090369472, + "velocityX": 0.3010698451819098, + "velocityY": -0.40702371596133824, + "timestamp": 4.953398268847583 + }, + { + "x": 2.7009592416637416, + "y": 6.808522263895978, + "heading": 0.4798144698796375, + "angularVelocity": -0.27834273723408604, + "velocityX": 0.4517545211918758, + "velocityY": -0.6105280395665419, + "timestamp": 5.0116446116055435 + }, + { + "x": 2.736056173569292, + "y": 6.76110833670321, + "heading": 0.4585545185998355, + "angularVelocity": -0.3650006210372158, + "velocityX": 0.6025602680565616, + "velocityY": -0.8140241077417322, + "timestamp": 5.069890954363504 + }, + { + "x": 2.7799452908871816, + "y": 6.701842252248569, + "heading": 0.4324815844111489, + "angularVelocity": -0.4476321251109512, + "velocityX": 0.7535085507474468, + "velocityY": -1.017507394428501, + "timestamp": 5.128137297121464 + }, + { + "x": 2.8326365093294883, + "y": 6.63072517677858, + "heading": 0.40187833162186215, + "angularVelocity": -0.5254107183425597, + "velocityX": 0.9046270709435361, + "velocityY": -1.2209706584585491, + "timestamp": 5.186383639879424 + }, + { + "x": 2.8941418577048204, + "y": 6.54775894755859, + "heading": 0.367091043865372, + "angularVelocity": -0.5972441548999393, + "velocityX": 1.0559521072578695, + "velocityY": -1.4244023794721719, + "timestamp": 5.244629982637385 + }, + { + "x": 2.9644761872265666, + "y": 6.452946486925878, + "heading": 0.3285529983813312, + "angularVelocity": -0.661638888542476, + "velocityX": 1.2075321160337456, + "velocityY": -1.6277839284553843, + "timestamp": 5.302876325395345 + }, + { + "x": 3.043658216776231, + "y": 6.346292539288771, + "heading": 0.2868218829421827, + "angularVelocity": -0.7164589820267359, + "velocityX": 1.359433499176103, + "velocityY": -1.8310840232545251, + "timestamp": 5.361122668153305 + }, + { + "x": 3.131712135507504, + "y": 6.227805106020448, + "heading": 0.24264403908378393, + "angularVelocity": -0.7584655407804345, + "velocityX": 1.5117501728336273, + "velocityY": -2.03424674679905, + "timestamp": 5.419369010911265 + }, + { + "x": 3.2286701423571795, + "y": 6.097498584088062, + "heading": 0.19707481942882135, + "angularVelocity": -0.7823533203504854, + "velocityX": 1.6646196526463415, + "velocityY": -2.2371622965903253, + "timestamp": 5.477615353669226 + }, + { + "x": 3.33457642569322, + "y": 5.9554017916384385, + "heading": 0.1517318182422537, + "angularVelocity": -0.7784694976470596, + "velocityX": 1.8182477786824947, + "velocityY": -2.439583083183451, + "timestamp": 5.535861696427186 + }, + { + "x": 3.449491590462662, + "y": 5.801583658823103, + "heading": 0.10942509922281701, + "angularVelocity": -0.726341209013589, + "velocityX": 1.9729163983216391, + "velocityY": -2.6408204452341257, + "timestamp": 5.594108039185146 + }, + { + "x": 3.5734690777406897, + "y": 5.636270409713289, + "heading": 0.07620523271942112, + "angularVelocity": -0.5703339459687529, + "velocityX": 2.128502518917107, + "velocityY": -2.838173888389265, + "timestamp": 5.652354381943106 + }, + { + "x": 3.704953237459551, + "y": 5.460551173556115, + "heading": 0.07376572964977675, + "angularVelocity": -0.04188251062872067, + "velocityX": 2.2573805237049305, + "velocityY": -3.016828659738646, + "timestamp": 5.710600724701067 + }, + { + "x": 3.8484687741574635, + "y": 5.2934320188975885, + "heading": 0.07376572135485625, + "angularVelocity": -1.4241101012224094e-7, + "velocityX": 2.4639407369194757, + "velocityY": -2.86917850538672, + "timestamp": 5.768847067459027 + }, + { + "x": 4.002883086238839, + "y": 5.136327781933491, + "heading": 0.07376571311141436, + "angularVelocity": -1.4152720162027837e-7, + "velocityX": 2.651055925056733, + "velocityY": -2.697237792541529, + "timestamp": 5.827093410216987 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.07376570463306892, + "angularVelocity": -1.4556013372477054e-7, + "velocityX": 2.826202837221629, + "velocityY": -2.51311892102558, + "timestamp": 5.885339752974947 + }, + { + "x": 4.268981030537756, + "y": 4.906736402889882, + "heading": 0.07376569669894775, + "angularVelocity": -2.2864637143597095e-7, + "velocityX": 2.9245183697828536, + "velocityY": -2.397995023127166, + "timestamp": 5.920040155676169 + }, + { + "x": 4.373711969606293, + "y": 4.827653183122543, + "heading": 0.07376568907042552, + "angularVelocity": -2.198395881628289e-7, + "velocityX": 3.0181476558152474, + "velocityY": -2.2790288760699386, + "timestamp": 5.954740558377391 + }, + { + "x": 4.481524054047376, + "y": 4.75282484787289, + "heading": 0.0737656816823934, + "angularVelocity": -2.129091176782688e-7, + "velocityX": 3.106940440125975, + "velocityY": -2.1564111487103386, + "timestamp": 5.989440961078612 + }, + { + "x": 4.592244511621915, + "y": 4.682371277147909, + "heading": 0.07376567447691734, + "angularVelocity": -2.0764819690223906e-7, + "velocityX": 3.1907542551556984, + "velocityY": -2.030338706198957, + "timestamp": 6.024141363779834 + }, + { + "x": 4.705695889724197, + "y": 4.616405299617974, + "heading": 0.07376566740123429, + "angularVelocity": -2.0390780833167242e-7, + "velocityX": 3.2694542215871127, + "velocityY": -1.90101475472538, + "timestamp": 6.058841766481056 + }, + { + "x": 4.821696252971789, + "y": 4.555032332206395, + "heading": 0.07376566040606643, + "angularVelocity": -2.0158751247220773e-7, + "velocityX": 3.342911154270497, + "velocityY": -1.76865288682714, + "timestamp": 6.093542169182277 + }, + { + "x": 4.939640387985606, + "y": 4.497483166935729, + "heading": 0.07376565344322925, + "angularVelocity": -2.0065580355812287e-7, + "velocityX": 3.398926981607191, + "velocityY": -1.6584581385460047, + "timestamp": 6.128242571883499 + }, + { + "x": 5.057585335343478, + "y": 4.439935666553369, + "heading": 0.07376564648040178, + "angularVelocity": -2.0065552340961329e-7, + "velocityX": 3.3989503918269923, + "velocityY": -1.658410159612758, + "timestamp": 6.162942974584721 + }, + { + "x": 5.1755309999931605, + "y": 4.382389636300601, + "heading": 0.07376563951757731, + "angularVelocity": -2.0065543705761684e-7, + "velocityX": 3.398971062820868, + "velocityY": -1.658367793257397, + "timestamp": 6.197643377285942 + }, + { + "x": 5.295248531429034, + "y": 4.328626841825402, + "heading": 0.07376563255290919, + "angularVelocity": -2.007085673925138e-7, + "velocityX": 3.45003291364278, + "velocityY": -1.5493420908716486, + "timestamp": 6.232343779987164 + }, + { + "x": 5.41702166916594, + "y": 4.27969803322079, + "heading": 0.07376562554616754, + "angularVelocity": -2.019210467909896e-7, + "velocityX": 3.5092716008341425, + "velocityY": -1.410035757391629, + "timestamp": 6.267044182688386 + }, + { + "x": 5.540655427942115, + "y": 4.235681910653619, + "heading": 0.07376561844909023, + "angularVelocity": -2.0452435036769212e-7, + "velocityX": 3.5628911814277564, + "velocityY": -1.2684614338963205, + "timestamp": 6.301744585389607 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.07376561120324407, + "angularVelocity": -2.0881158710741676e-7, + "velocityX": 3.610802501559291, + "velocityY": -1.1248525394721576, + "timestamp": 6.336444988090829 + }, + { + "x": 5.801240954312848, + "y": 4.160760783914465, + "heading": 0.0737656041692584, + "angularVelocity": -1.900588111168926e-7, + "velocityX": 3.655524839727154, + "velocityY": -0.9697042570289608, + "timestamp": 6.37345450968449 + }, + { + "x": 5.937938725958539, + "y": 4.130679839530998, + "heading": 0.07376559729974105, + "angularVelocity": -1.8561486466113367e-7, + "velocityX": 3.6935838605680487, + "velocityY": -0.8127893333433076, + "timestamp": 6.410464031278151 + }, + { + "x": 6.075795864969434, + "y": 4.106460978217925, + "heading": 0.07376559053845443, + "angularVelocity": -1.8269046288328192e-7, + "velocityX": 3.724910052187935, + "velocityY": -0.654395416914037, + "timestamp": 6.447473552871812 + }, + { + "x": 6.214561046214486, + "y": 4.088147908867804, + "heading": 0.07376558383255526, + "angularVelocity": -1.8119388963756882e-7, + "velocityX": 3.7494454202515035, + "velocityY": -0.4948204829877394, + "timestamp": 6.484483074465473 + }, + { + "x": 6.353981153300122, + "y": 4.0757714079596425, + "heading": 0.07376557713086826, + "angularVelocity": -1.8108007594786474e-7, + "velocityX": 3.7671415647133246, + "velocityY": -0.33441396633135523, + "timestamp": 6.521492596059134 + }, + { + "x": 6.493802446577043, + "y": 4.0693566803624766, + "heading": 0.07376555560001419, + "angularVelocity": -5.817652634682061e-7, + "velocityX": 3.7779816451577393, + "velocityY": -0.17332641225670456, + "timestamp": 6.558502117652795 + }, + { + "x": 6.62912467253087, + "y": 4.064792329127301, + "heading": 0.05903973203041549, + "angularVelocity": -0.39789283772100603, + "velocityX": 3.656416514635636, + "velocityY": -0.12332910663606324, + "timestamp": 6.595511639246456 + }, + { + "x": 6.758568056447626, + "y": 4.061299539806061, + "heading": 0.04418585550364903, + "angularVelocity": -0.4013528380575057, + "velocityX": 3.4975697696920878, + "velocityY": -0.09437542477821895, + "timestamp": 6.632521160840117 + }, + { + "x": 6.882108384071856, + "y": 4.058691153499139, + "heading": 0.0304778500113165, + "angularVelocity": -0.3703913182893011, + "velocityX": 3.3380687537822458, + "velocityY": -0.07047879017620408, + "timestamp": 6.669530682433778 + }, + { + "x": 6.999742512686264, + "y": 4.056896966002031, + "heading": 0.018392940314004957, + "angularVelocity": -0.3265351503322698, + "velocityX": 3.1784828214195313, + "velocityY": -0.048479078351971995, + "timestamp": 6.706540204027439 + }, + { + "x": 7.111470092404711, + "y": 4.055880215977976, + "heading": 0.008182013942056482, + "angularVelocity": -0.2758999828221868, + "velocityX": 3.0188874351076422, + "velocityY": -0.027472660555273344, + "timestamp": 6.7435497256211 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": -2.0237932968568665e-24, + "angularVelocity": -0.22107861949390495, + "velocityX": 2.859298314909037, + "velocityY": -0.007077363713034477, + "timestamp": 6.780559247214761 + }, + { + "x": 7.386520754967261, + "y": 4.057483355651357, + "heading": -0.007799169879435119, + "angularVelocity": -0.11872019703645657, + "velocityX": 2.5760366812439752, + "velocityY": 0.028390383098604113, + "timestamp": 6.846252954554551 + }, + { + "x": 7.536988258489095, + "y": 4.060513288338981, + "heading": -0.011310959722615328, + "angularVelocity": -0.053457020244208106, + "velocityX": 2.2904401291217464, + "velocityY": 0.04612211443560253, + "timestamp": 6.911946661894341 + }, + { + "x": 7.668645085347693, + "y": 4.063976331623079, + "heading": -0.012040004750973126, + "angularVelocity": -0.01109763869143412, + "velocityX": 2.004101034785968, + "velocityY": 0.05271499241448283, + "timestamp": 6.9776403692341304 + }, + { + "x": 7.78147865771714, + "y": 4.06737291870627, + "heading": -0.011006488514551415, + "angularVelocity": 0.015732347560718764, + "velocityX": 1.7175704787953916, + "velocityY": 0.051703385616870434, + "timestamp": 7.04333407657392 + }, + { + "x": 7.87549031086396, + "y": 4.070341151997571, + "heading": -0.008947695308103269, + "angularVelocity": 0.03133927570565307, + "velocityX": 1.4310602484429864, + "velocityY": 0.04518291646943911, + "timestamp": 7.10902778391371 + }, + { + "x": 7.9506868858752515, + "y": 4.072607124350486, + "heading": -0.006421398747140829, + "angularVelocity": 0.03845568568532146, + "velocityX": 1.1446541542000461, + "velocityY": 0.03449298943040145, + "timestamp": 7.1747214912535 + }, + { + "x": 8.00707724488628, + "y": 4.073956458693622, + "heading": -0.003864315760597132, + "angularVelocity": 0.038924321523180636, + "velocityX": 0.8583829607812892, + "velocityY": 0.020539780715319512, + "timestamp": 7.24041519859329 + }, + { + "x": 8.044670745645819, + "y": 4.074216878653992, + "heading": -0.0016276569414438076, + "angularVelocity": 0.034046774184696094, + "velocityX": 0.5722542124939418, + "velocityY": 0.003964153811924349, + "timestamp": 7.30610890593308 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 1.6866276328916314e-24, + "angularVelocity": 0.02477645131252876, + "velocityX": 0.286265117553968, + "velocityY": -0.014764317948939539, + "timestamp": 7.37180261327287 + }, + { + "x": 8.060961534281953, + "y": 4.070465444687161, + "heading": 0.0007560599723107442, + "angularVelocity": 0.010267428471842888, + "velocityX": -0.03415452911035645, + "velocityY": -0.03777341504058265, + "timestamp": 7.445439354389123 + }, + { + "x": 8.034839704830464, + "y": 4.066167075236277, + "heading": 0.00046881224437648735, + "angularVelocity": -0.0039008750737728444, + "velocityX": -0.3547390752973492, + "velocityY": -0.0583726192349958, + "timestamp": 7.519076095505376 + }, + { + "x": 7.985098416570273, + "y": 4.060562217226684, + "heading": -0.0008319841828060777, + "angularVelocity": -0.017665046109644415, + "velocityX": -0.6754955136004895, + "velocityY": -0.0761149654999585, + "timestamp": 7.592712836621629 + }, + { + "x": 7.9117249252468085, + "y": 4.053904178499818, + "heading": -0.0031104504440480647, + "angularVelocity": -0.03094197579500262, + "velocityX": -0.9964250211402843, + "velocityY": -0.09041734636729296, + "timestamp": 7.666349577737883 + }, + { + "x": 7.814707316285252, + "y": 4.046503723453766, + "heading": -0.00632248118792148, + "angularVelocity": -0.04361994698818144, + "velocityX": -1.3175163307185394, + "velocityY": -0.10049949161068944, + "timestamp": 7.739986318854136 + }, + { + "x": 7.69403642851654, + "y": 4.038750888931838, + "heading": -0.010412565479377186, + "angularVelocity": -0.055544069841419484, + "velocityX": -1.6387320505969085, + "velocityY": -0.10528486736925086, + "timestamp": 7.813623059970389 + }, + { + "x": 7.549709960346403, + "y": 4.031149265281445, + "heading": -0.015308761000708897, + "angularVelocity": -0.06649120326498265, + "velocityX": -1.9599790265335464, + "velocityY": -0.10323139692441612, + "timestamp": 7.887259801086643 + }, + { + "x": 7.381741634294396, + "y": 4.0243729190236985, + "heading": -0.02091427607708914, + "angularVelocity": -0.07612388858342581, + "velocityX": -2.2810396482216517, + "velocityY": -0.0920239836123183, + "timestamp": 7.960896542202896 + }, + { + "x": 7.1901832106568095, + "y": 4.019367673272749, + "heading": -0.027092292311801627, + "angularVelocity": -0.08389855581684406, + "velocityX": -2.6013973559091212, + "velocityY": -0.06797212471757884, + "timestamp": 8.03453328331915 + }, + { + "x": 6.975183472586086, + "y": 4.017547771662454, + "heading": -0.03363583639032012, + "angularVelocity": -0.08886248874305737, + "velocityX": -2.9197345620074877, + "velocityY": -0.02471458653257856, + "timestamp": 8.108170024435402 + }, + { + "x": 6.737173435740335, + "y": 4.021221144917956, + "heading": -0.04019984018910067, + "angularVelocity": -0.0891403353716819, + "velocityX": -3.232218499050576, + "velocityY": 0.04988506009116617, + "timestamp": 8.181806765551656 + }, + { + "x": 6.477592542757965, + "y": 4.034606496898433, + "heading": -0.04612188209053887, + "angularVelocity": -0.08042237898726157, + "velocityX": -3.5251545498538244, + "velocityY": 0.18177545308999152, + "timestamp": 8.255443506667909 + }, + { + "x": 6.202379983792451, + "y": 4.065741183912862, + "heading": -0.04994434785858321, + "angularVelocity": -0.05190976284528472, + "velocityX": -3.737435345366867, + "velocityY": 0.4228145697713012, + "timestamp": 8.329080247784162 + }, + { + "x": 5.92930929060311, + "y": 4.120418111584284, + "heading": -0.049944382856938176, + "angularVelocity": -4.752838655296773e-7, + "velocityX": -3.7083484283780552, + "velocityY": 0.7425223718836313, + "timestamp": 8.402716988900416 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.05351191132153618, + "angularVelocity": -0.04844766906462906, + "velocityX": -3.5764423817525626, + "velocityY": 1.0352299927261919, + "timestamp": 8.476353730016669 + }, + { + "x": 5.427595279774474, + "y": 4.2855510061769815, + "heading": -0.058564536366975395, + "angularVelocity": -0.07109019818968403, + "velocityX": -3.3536640954167023, + "velocityY": 1.2508460219498765, + "timestamp": 8.547427171561418 + }, + { + "x": 5.210001648120265, + "y": 4.381839057848659, + "heading": -0.06451807274962795, + "angularVelocity": -0.08376597858855642, + "velocityX": -3.0615322253278787, + "velocityY": 1.3547683857556179, + "timestamp": 8.618500613106168 + }, + { + "x": 5.014433901254649, + "y": 4.478870971040357, + "heading": -0.07079419051912546, + "angularVelocity": -0.08830468362146032, + "velocityX": -2.751629056016378, + "velocityY": 1.365234482568344, + "timestamp": 8.689574054650917 + }, + { + "x": 4.840611358699117, + "y": 4.572318839972298, + "heading": -0.07701240147326043, + "angularVelocity": -0.08748993743633168, + "velocityX": -2.445675047916283, + "velocityY": 1.3148071473801226, + "timestamp": 8.760647496195666 + }, + { + "x": 4.687900440256207, + "y": 4.6594427038525446, + "heading": -0.08292539389195669, + "angularVelocity": -0.08319552691103697, + "velocityX": -2.148635483575938, + "velocityY": 1.2258286919367853, + "timestamp": 8.831720937740416 + }, + { + "x": 4.555672965258425, + "y": 4.738435503151125, + "heading": -0.088365740210127, + "angularVelocity": -0.07654541837185252, + "velocityX": -1.8604343918610988, + "velocityY": 1.1114249933829867, + "timestamp": 8.902794379285165 + }, + { + "x": 4.443385396304237, + "y": 4.808043311257638, + "heading": -0.09321471505627854, + "angularVelocity": -0.06822484940592717, + "velocityX": -1.5798808459765967, + "velocityY": 0.9793786060392976, + "timestamp": 8.973867820829915 + }, + { + "x": 4.3505830000627, + "y": 4.867355530244293, + "heading": -0.09738458875338916, + "angularVelocity": -0.058669927985478765, + "velocityX": -1.3057253768006336, + "velocityY": 0.8345201484201303, + "timestamp": 9.044941262374664 + }, + { + "x": 4.2768869568191645, + "y": 4.915685373463825, + "heading": -0.10080825239426806, + "angularVelocity": -0.04817078737805652, + "velocityX": -1.0368998833007734, + "velocityY": 0.6799986347798151, + "timestamp": 9.116014703919413 + }, + { + "x": 4.22197990513087, + "y": 4.95249852612905, + "heading": -0.10343286860563979, + "angularVelocity": -0.03692822739868577, + "velocityX": -0.7725396504645569, + "velocityY": 0.5179593370618798, + "timestamp": 9.187088145464163 + }, + { + "x": 4.185593591353172, + "y": 4.97736854606154, + "heading": -0.10521581647883109, + "angularVelocity": -0.025085993226720307, + "velocityX": -0.5119537338682039, + "velocityY": 0.3499200178287514, + "timestamp": 9.258161587008912 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.012749959780663622, + "velocityX": -0.25458913429159136, + "velocityY": 0.1769894567221649, + "timestamp": 9.329235028553661 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": 1.153459712624117e-25, + "velocityX": -3.247224190829055e-25, + "velocityY": -4.023670796703132e-25, + "timestamp": 9.40030847009841 + }, + { + "x": 4.180652954196475, + "y": 4.972113880358222, + "heading": -0.10282545923039073, + "angularVelocity": 0.04623963021087373, + "velocityX": 0.18450581877512717, + "velocityY": -0.2501512088012644, + "timestamp": 9.471601011898064 + }, + { + "x": 4.207780209062833, + "y": 4.937082603957029, + "heading": -0.09606696501055557, + "angularVelocity": 0.09479945656626756, + "velocityX": 0.380506209788268, + "velocityY": -0.4913736488683167, + "timestamp": 9.542893553697718 + }, + { + "x": 4.249890859346282, + "y": 4.885729175148701, + "heading": -0.08564531821991797, + "angularVelocity": 0.1461814451773148, + "velocityX": 0.5906739922639744, + "velocityY": -0.7203197909907696, + "timestamp": 9.614186095497372 + }, + { + "x": 4.308238374054552, + "y": 4.819303841665251, + "heading": -0.07131614738285201, + "angularVelocity": 0.20099116226398103, + "velocityX": 0.8184238243635266, + "velocityY": -0.931729067398359, + "timestamp": 9.685478637297026 + }, + { + "x": 4.384365934500173, + "y": 4.739668033977246, + "heading": -0.05278830976865122, + "angularVelocity": 0.2598846547829301, + "velocityX": 1.0678194173459812, + "velocityY": -1.1170285934228166, + "timestamp": 9.75677117909668 + }, + { + "x": 4.480080041577144, + "y": 4.649696456670524, + "heading": -0.02973745107706772, + "angularVelocity": 0.3233277718777487, + "velocityX": 1.3425542793234355, + "velocityY": -1.262005464183884, + "timestamp": 9.828063720896333 + }, + { + "x": 4.597147014770728, + "y": 4.553865445568556, + "heading": -0.0018737410680010795, + "angularVelocity": 0.3908362544762254, + "velocityX": 1.642064797220515, + "velocityY": -1.344194058493121, + "timestamp": 9.899356262695987 + }, + { + "x": 4.7363464626810785, + "y": 4.458651640127929, + "heading": 0.030870956626381736, + "angularVelocity": 0.4593004663293065, + "velocityX": 1.9525106609542362, + "velocityY": -1.3355366920174645, + "timestamp": 9.97064880449564 + }, + { + "x": 4.8961444414371185, + "y": 4.371572487351198, + "heading": 0.06809613303796244, + "angularVelocity": 0.5221468539611136, + "velocityX": 2.2414403347422116, + "velocityY": -1.2214342563551692, + "timestamp": 10.041941346295294 + }, + { + "x": 5.072824953795758, + "y": 4.298842121244781, + "heading": 0.10897366885891092, + "angularVelocity": 0.5733774499978155, + "velocityX": 2.478246782884325, + "velocityY": -1.0201679484342572, + "timestamp": 10.113233888094948 + }, + { + "x": 5.262214444102689, + "y": 4.244271073764123, + "heading": 0.1525665304554643, + "angularVelocity": 0.6114645444828949, + "velocityX": 2.6565119650124474, + "velocityY": -0.7654524036190654, + "timestamp": 10.184526429894602 + }, + { + "x": 5.460811083302549, + "y": 4.20986764407754, + "heading": 0.19806311217601916, + "angularVelocity": 0.638167479684045, + "velocityX": 2.785657997129001, + "velocityY": -0.48256702339585567, + "timestamp": 10.255818971694255 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.24483305037299086, + "angularVelocity": 0.6560284851170571, + "velocityX": 2.8774488935285283, + "velocityY": -0.18541307673169433, + "timestamp": 10.327111513493909 + }, + { + "x": 5.802993350275697, + "y": 4.197353588266771, + "heading": 0.2759731999769641, + "angularVelocity": 0.6634007595054764, + "velocityX": 2.919495150576939, + "velocityY": 0.015008756790958989, + "timestamp": 10.374051691294106 + }, + { + "x": 5.941816707126591, + "y": 4.207504283843094, + "heading": 0.3073984177151568, + "angularVelocity": 0.669473768760635, + "velocityX": 2.9574527272095095, + "velocityY": 0.21624748886826958, + "timestamp": 10.420991869094303 + }, + { + "x": 6.082189718099688, + "y": 4.227142107895406, + "heading": 0.33903536512016264, + "angularVelocity": 0.6739843964730912, + "velocityX": 2.990466111368347, + "velocityY": 0.4183585357494954, + "timestamp": 10.4679320468945 + }, + { + "x": 6.223826665767993, + "y": 4.256309424549035, + "heading": 0.37079417516105295, + "angularVelocity": 0.6765805228960279, + "velocityX": 3.01739265392617, + "velocityY": 0.6213720957295971, + "timestamp": 10.514872224694697 + }, + { + "x": 6.366368317456564, + "y": 4.295047066484075, + "heading": 0.4025622544615229, + "angularVelocity": 0.6767779925268211, + "velocityX": 3.0366662072586053, + "velocityY": 0.8252555433413251, + "timestamp": 10.561812402494894 + }, + { + "x": 6.509351710185931, + "y": 4.3433874902945675, + "heading": 0.4341948054163167, + "angularVelocity": 0.6738907357666759, + "velocityX": 3.0460769308966165, + "velocityY": 1.0298304368648719, + "timestamp": 10.608752580295091 + }, + { + "x": 6.652162883267978, + "y": 4.401339041781646, + "heading": 0.4654999103087445, + "angularVelocity": 0.6669149193613809, + "velocityX": 3.0424080132360682, + "velocityY": 1.2345831269270133, + "timestamp": 10.655692758095288 + }, + { + "x": 6.793961557546229, + "y": 4.468849069286898, + "heading": 0.4962146580453544, + "angularVelocity": 0.654338120902485, + "velocityX": 3.0208380309469742, + "velocityY": 1.4382141412546399, + "timestamp": 10.702632935895485 + }, + { + "x": 6.933563512190389, + "y": 4.545714915555555, + "heading": 0.5259679790741631, + "angularVelocity": 0.6338561638060835, + "velocityX": 2.9740397498786737, + "velocityY": 1.6375278039175576, + "timestamp": 10.749573113695682 + }, + { + "x": 7.069284940391168, + "y": 4.631368590173797, + "heading": 0.5542337619189218, + "angularVelocity": 0.6021660796657563, + "velocityX": 2.8913701345248803, + "velocityY": 1.8247411627375887, + "timestamp": 10.796513291495879 + }, + { + "x": 7.1988866969659675, + "y": 4.724417833549329, + "heading": 0.5803339735600787, + "angularVelocity": 0.5560313757705206, + "velocityX": 2.7609984164621943, + "velocityY": 1.9822942250367808, + "timestamp": 10.843453469296076 + }, + { + "x": 7.32010179733884, + "y": 4.822118790801402, + "heading": 0.6036855499367045, + "angularVelocity": 0.49747524340496785, + "velocityX": 2.5823315132896965, + "velocityY": 2.0813929948868157, + "timestamp": 10.890393647096273 + }, + { + "x": 7.431787144885325, + "y": 4.920866698749227, + "heading": 0.6241547457610245, + "angularVelocity": 0.43606984003016963, + "velocityX": 2.379312409549823, + "velocityY": 2.103696930338622, + "timestamp": 10.93733382489647 + }, + { + "x": 7.534091847804722, + "y": 5.017622526169069, + "heading": 0.6419335715808812, + "angularVelocity": 0.37875497394860314, + "velocityX": 2.1794698638522156, + "velocityY": 2.0612582217239557, + "timestamp": 10.984274002696667 + }, + { + "x": 7.627668873285194, + "y": 5.110394301950092, + "heading": 0.6572905320305523, + "angularVelocity": 0.32716025309998836, + "velocityX": 1.9935379426721815, + "velocityY": 1.9763831354859511, + "timestamp": 11.031214180496864 + }, + { + "x": 7.7131909565949455, + "y": 5.197947878934244, + "heading": 0.6704631268278166, + "angularVelocity": 0.2806251576918574, + "velocityX": 1.8219377794813416, + "velocityY": 1.8652161343918634, + "timestamp": 11.078154358297061 + }, + { + "x": 7.791223380396529, + "y": 5.279496011851437, + "heading": 0.6816390937397763, + "angularVelocity": 0.2380895734892706, + "velocityX": 1.6623802349818717, + "velocityY": 1.7372778872782495, + "timestamp": 11.125094536097258 + }, + { + "x": 7.86221923742719, + "y": 5.354510990808827, + "heading": 0.6909640471575174, + "angularVelocity": 0.19865611624721707, + "velocityX": 1.5124752473852405, + "velocityY": 1.5980974609149294, + "timestamp": 11.172034713897455 + }, + { + "x": 7.926540429785996, + "y": 5.422621631484432, + "heading": 0.6985519881256509, + "angularVelocity": 0.1616513043566187, + "velocityX": 1.370280117655087, + "velocityY": 1.4510094308871155, + "timestamp": 11.218974891697652 + }, + { + "x": 7.984478950500488, + "y": 5.483555793762207, + "heading": 0.7044936776046891, + "angularVelocity": 0.12658003777337848, + "velocityX": 1.2343055231087734, + "velocityY": 1.2981238063720744, + "timestamp": 11.265915069497849 + }, + { + "x": 8.047923980956542, + "y": 5.548807333254465, + "heading": 0.7094925329826927, + "angularVelocity": 0.08420982403970191, + "velocityX": 1.0687836408324876, + "velocityY": 1.099215769102154, + "timestamp": 11.325276972161923 + }, + { + "x": 8.10152245585906, + "y": 5.602265273787256, + "heading": 0.7122491464591354, + "angularVelocity": 0.04643741781732065, + "velocityX": 0.9029103262715175, + "velocityY": 0.9005429094027965, + "timestamp": 11.384638874825997 + }, + { + "x": 8.14525837183985, + "y": 5.64394060886005, + "heading": 0.7129744623769576, + "angularVelocity": 0.012218542285053226, + "velocityX": 0.7367674218309503, + "velocityY": 0.7020552442301676, + "timestamp": 11.444000777490071 + }, + { + "x": 8.179119065532058, + "y": 5.673842216437666, + "heading": 0.7118363633541982, + "angularVelocity": -0.01917221267653394, + "velocityX": 0.5704111925762211, + "velocityY": 0.5037171356657365, + "timestamp": 11.503362680154146 + }, + { + "x": 8.203094267741402, + "y": 5.691977414243475, + "heading": 0.7089715992476469, + "angularVelocity": -0.048259303997766526, + "velocityX": 0.40388197031045997, + "velocityY": 0.3055023001610055, + "timestamp": 11.56272458281822 + }, + { + "x": 8.217175483703613, + "y": 5.698352336883545, + "heading": 0.7044936776046891, + "angularVelocity": -0.07543426746777364, + "velocityX": 0.23720964676445846, + "velocityY": 0.10739080713341471, + "timestamp": 11.622086485482294 + }, + { + "x": 8.220424741398494, + "y": 5.690497924270521, + "heading": 0.6974771095930071, + "angularVelocity": -0.10438216452389025, + "velocityX": 0.04833767031445264, + "velocityY": -0.11684638248302778, + "timestamp": 11.689306475062756 + }, + { + "x": 8.210761211514034, + "y": 5.66775585785793, + "heading": 0.6884846443385353, + "angularVelocity": -0.1337766535013804, + "velocityX": -0.14375976468862095, + "velocityY": -0.338322968428447, + "timestamp": 11.756526464643217 + }, + { + "x": 8.187932028417118, + "y": 5.630349579704253, + "heading": 0.6774812110255055, + "angularVelocity": -0.1636928744218101, + "velocityX": -0.33961896214800763, + "velocityY": -0.556475512524468, + "timestamp": 11.823746454223679 + }, + { + "x": 8.15163887353547, + "y": 5.578552873386559, + "heading": 0.6644254264440203, + "angularVelocity": -0.19422473378782149, + "velocityX": -0.5399161039471123, + "velocityY": -0.7705551078030757, + "timestamp": 11.89096644380414 + }, + { + "x": 8.101525228747773, + "y": 5.512708245233119, + "heading": 0.6492678901971639, + "angularVelocity": -0.225491499499757, + "velocityX": -0.745516997257366, + "velocityY": -0.9795393983901896, + "timestamp": 11.958186433384602 + }, + { + "x": 8.037158859322696, + "y": 5.433255022389235, + "heading": 0.6319488484169726, + "angularVelocity": -0.2576471952507599, + "velocityX": -0.9575480422833206, + "velocityY": -1.1819880267725829, + "timestamp": 12.025406422965064 + }, + { + "x": 7.958007564987826, + "y": 5.340774148745389, + "heading": 0.6123949630340744, + "angularVelocity": -0.29089390678188665, + "velocityX": -1.177496379110975, + "velocityY": -1.3757942275957413, + "timestamp": 12.092626412545526 + }, + { + "x": 7.863405980160758, + "y": 5.236063150659359, + "heading": 0.5905148719771925, + "angularVelocity": -0.3254997686468183, + "velocityX": -1.4073430450897342, + "velocityY": -1.5577360059048004, + "timestamp": 12.159846402125988 + }, + { + "x": 7.752513083642703, + "y": 5.120268560189199, + "heading": 0.5661934059220105, + "angularVelocity": -0.36181895009176485, + "velocityX": -1.649701185765849, + "velocityY": -1.7226213689241165, + "timestamp": 12.22706639170645 + }, + { + "x": 7.624272333970784, + "y": 4.995132700896389, + "heading": 0.5392856965961144, + "angularVelocity": -0.40029326832441703, + "velocityX": -1.9077769941992746, + "velocityY": -1.8615870081774413, + "timestamp": 12.294286381286911 + }, + { + "x": 7.477443680694748, + "y": 4.863466852853807, + "heading": 0.5096191616794202, + "angularVelocity": -0.4413350121273609, + "velocityX": -2.1843004468229417, + "velocityY": -1.958730563101009, + "timestamp": 12.361506370867373 + }, + { + "x": 7.310999420897597, + "y": 4.729976120271212, + "heading": 0.47703933517045805, + "angularVelocity": -0.48467467359488975, + "velocityX": -2.4761125497932377, + "velocityY": -1.9858785075056882, + "timestamp": 12.428726360447834 + }, + { + "x": 7.12561513664023, + "y": 4.601908860623506, + "heading": 0.44160618415094194, + "angularVelocity": -0.5271222331432077, + "velocityX": -2.757874337892657, + "velocityY": -1.9051960651438506, + "timestamp": 12.495946350028296 + }, + { + "x": 6.9255695292750366, + "y": 4.487009075034074, + "heading": 0.4039304316786653, + "angularVelocity": -0.5604843545412803, + "velocityX": -2.9759839091575766, + "velocityY": -1.709309779822242, + "timestamp": 12.563166339608758 + }, + { + "x": 6.716882603619491, + "y": 4.389825418108365, + "heading": 0.36497586306620833, + "angularVelocity": -0.5795086975702185, + "velocityX": -3.1045367153136607, + "velocityY": -1.445755310767803, + "timestamp": 12.63038632918922 + }, + { + "x": 6.504347696212581, + "y": 4.311975808393504, + "heading": 0.32552791036504214, + "angularVelocity": -0.5868485393611587, + "velocityX": -3.1617813203096174, + "velocityY": -1.1581318325209748, + "timestamp": 12.697606318769681 + }, + { + "x": 6.29116656289608, + "y": 4.2538292064786, + "heading": 0.2861053737979932, + "angularVelocity": -0.5864704355519208, + "velocityX": -3.1713949175985174, + "velocityY": -0.8650195020530711, + "timestamp": 12.764826308350143 + }, + { + "x": 6.079464519610655, + "y": 4.215340449642915, + "heading": 0.24704786030835676, + "angularVelocity": -0.5810401598305069, + "velocityX": -3.1493911945942843, + "velocityY": -0.572579036026387, + "timestamp": 12.832046297930605 + }, + { + "x": 5.870707606275243, + "y": 4.1963435376359834, + "heading": 0.20858746560478297, + "angularVelocity": -0.5721571059980171, + "velocityX": -3.1055778889333454, + "velocityY": -0.28260807723263415, + "timestamp": 12.899266287511066 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.17089065340850093, + "angularVelocity": -0.560797650097213, + "velocityX": -3.0460563700229573, + "velocityY": 0.004545328262717531, + "timestamp": 12.966486277091528 + }, + { + "x": 5.441376331690843, + "y": 4.221257817184443, + "heading": 0.12955296713969652, + "angularVelocity": -0.5459093121032401, + "velocityX": -2.965763486744691, + "velocityY": 0.3249853335649047, + "timestamp": 13.042208902356046 + }, + { + "x": 5.2258290849692735, + "y": 4.2691932604024965, + "heading": 0.08990916431610245, + "angularVelocity": -0.5235397304980968, + "velocityX": -2.8465368965828737, + "velocityY": 0.6330399012263854, + "timestamp": 13.117931527620565 + }, + { + "x": 5.023235975053, + "y": 4.338521550335442, + "heading": 0.05269444989052449, + "angularVelocity": -0.4914609642174678, + "velocityX": -2.6754633665772425, + "velocityY": 0.9155558155936205, + "timestamp": 13.193654152885083 + }, + { + "x": 4.838427025225484, + "y": 4.425425000911894, + "heading": 0.018769229111462726, + "angularVelocity": -0.44801960656478235, + "velocityX": -2.440604101903904, + "velocityY": 1.1476550142427961, + "timestamp": 13.269376778149601 + }, + { + "x": 4.676043362291933, + "y": 4.523369450086871, + "heading": -0.011096289897494078, + "angularVelocity": -0.3944068091224933, + "velocityX": -2.144453686943697, + "velocityY": 1.2934634639624794, + "timestamp": 13.34509940341412 + }, + { + "x": 4.53850801944686, + "y": 4.62398511499902, + "heading": -0.03651765926491392, + "angularVelocity": -0.33571695749608116, + "velocityX": -1.8163044712809187, + "velocityY": 1.3287397863013088, + "timestamp": 13.420822028678637 + }, + { + "x": 4.425480168441604, + "y": 4.719703555950957, + "heading": -0.05754144603227514, + "angularVelocity": -0.2776420745308276, + "velocityX": -1.4926562650254422, + "velocityY": 1.2640665932747182, + "timestamp": 13.496544653943156 + }, + { + "x": 4.335238129667097, + "y": 4.805147915212772, + "heading": -0.07443003472477187, + "angularVelocity": -0.22303226589808134, + "velocityX": -1.1917447190884998, + "velocityY": 1.128386119252163, + "timestamp": 13.572267279207674 + }, + { + "x": 4.265861114202726, + "y": 4.876824239203163, + "heading": -0.08748354859792261, + "angularVelocity": -0.17238591276453477, + "velocityX": -0.9161992894728712, + "velocityY": 0.9465641707482745, + "timestamp": 13.647989904472192 + }, + { + "x": 4.215660401957236, + "y": 4.932459766827933, + "heading": -0.09697232555255134, + "angularVelocity": -0.1253096669784238, + "velocityX": -0.6629552537319688, + "velocityY": 0.7347279288115158, + "timestamp": 13.72371252973671 + }, + { + "x": 4.183247912596747, + "y": 4.970524954667524, + "heading": -0.10312305256683521, + "angularVelocity": -0.08122707041386726, + "velocityX": -0.4280423353953174, + "velocityY": 0.5026923948637503, + "timestamp": 13.799435155001229 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -0.03960437745903177, + "velocityX": -0.2079807341935533, + "velocityY": 0.25649983915041824, + "timestamp": 13.875157780265747 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.106122, + "angularVelocity": -1.1491916765427864e-28, + "velocityX": -1.6722970053411976e-26, + "velocityY": -1.611730320150929e-26, + "timestamp": 13.950880405530265 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj index 2bcc94b3..9960fe17 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.1.traj @@ -1,310 +1,311 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -2.75917641303777e-30, - "velocityX": -8.847327306636262e-30, - "velocityY": 1.4420464788285476e-29, - "timestamp": 0 - }, - { - "x": 1.3060574436245773, - "y": 5.572382537817046, - "heading": -0.010694867587036427, - "angularVelocity": -0.14212340511596666, - "velocityX": 0.21372585011902087, - "velocityY": -0.24679899874441508, - "timestamp": 0.07525057240542533 - }, - { - "x": 1.3382236275279018, - "y": 5.53523931290218, - "heading": -0.032076067517629925, - "angularVelocity": -0.2841333859323569, - "velocityX": 0.42745434186951237, - "velocityY": -0.4935939186635594, - "timestamp": 0.15050114481085067 - }, - { - "x": 1.386473519841097, - "y": 5.479524946048311, - "heading": -0.0641215016247012, - "angularVelocity": -0.42584970563284796, - "velocityX": 0.6411897048771923, - "velocityY": -0.7403846253066342, - "timestamp": 0.22575171721627602 - }, - { - "x": 1.450807989654895, - "y": 5.405239856496701, - "heading": -0.10679801494850981, - "angularVelocity": -0.5671254312134459, - "velocityX": 0.8549366171751026, - "velocityY": -0.9871697606799944, - "timestamp": 0.30100228962170134 - }, - { - "x": 1.5312282961066404, - "y": 5.312384689270129, - "heading": -0.16006508743981135, - "angularVelocity": -0.7078626884752101, - "velocityX": 1.0687002620672836, - "velocityY": -1.23394632438782, - "timestamp": 0.37625286202712666 - }, - { - "x": 1.627736154340151, - "y": 5.200960485487107, - "heading": -0.22387960186124028, - "angularVelocity": -0.8480269635530943, - "velocityX": 1.2824866993939017, - "velocityY": -1.4807090527398261, - "timestamp": 0.451503434432552 - }, - { - "x": 1.7403339652666383, - "y": 5.0709689996393825, - "heading": -0.2982011188077779, - "angularVelocity": -0.9876538419890185, - "velocityX": 1.4963050422725384, - "velocityY": -1.7274484657660032, - "timestamp": 0.5267540068379774 - }, - { - "x": 1.8690262176186487, - "y": 4.922413993905078, - "heading": -0.382996017565801, - "angularVelocity": -1.1268339369356364, - "velocityX": 1.710183035621927, - "velocityY": -1.9741378834553056, - "timestamp": 0.6020045792434027 - }, - { - "x": 2.0218420905066052, - "y": 4.778614575286612, - "heading": -0.4624353832107095, - "angularVelocity": -1.055664602990153, - "velocityX": 2.030760272057439, - "velocityY": -1.9109411931313167, - "timestamp": 0.677255151648828 - }, - { - "x": 2.15857350503456, - "y": 4.653386666317764, - "heading": -0.5314189322497749, - "angularVelocity": -0.9167179309443209, - "velocityX": 1.8170149429960494, - "velocityY": -1.6641456000181232, - "timestamp": 0.7525057240542533 - }, - { - "x": 2.2792167348207513, - "y": 4.5467268950736175, - "heading": -0.589951951408673, - "angularVelocity": -0.7778415138506264, - "velocityX": 1.6032200942942694, - "velocityY": -1.4173948161921597, - "timestamp": 0.8277562964596786 - }, - { - "x": 2.38377026778444, - "y": 4.458633759569044, - "heading": -0.6380351777892487, - "angularVelocity": -0.6389748920543931, - "velocityX": 1.3894051516512624, - "velocityY": -1.1706639921478266, - "timestamp": 0.903006868865104 - }, - { - "x": 2.472233242714231, - "y": 4.389106342435906, - "heading": -0.6756676216200074, - "angularVelocity": -0.5000951172479533, - "velocityX": 1.1755787644266529, - "velocityY": -0.9239453589485842, - "timestamp": 0.9782574412705293 - }, - { - "x": 2.5446051200448103, - "y": 4.3381440386367105, - "heading": -0.7028476555095471, - "angularVelocity": -0.3611937161417901, - "velocityX": 0.9617452069540623, - "velocityY": -0.6772347660530061, - "timestamp": 1.0535080136759547 - }, - { - "x": 2.600885568086537, - "y": 4.305746455375921, - "heading": -0.7195734643408855, - "angularVelocity": -0.22226819406292112, - "velocityX": 0.7479072416796508, - "velocityY": -0.4305293929915994, - "timestamp": 1.12875858608138 - }, - { - "x": 2.641074414508583, - "y": 4.291913365901662, - "heading": -0.7258432162336076, - "angularVelocity": -0.08331832824324738, - "velocityX": 0.5340669863123017, - "velocityY": -0.18382703322097846, - "timestamp": 1.2040091584868053 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.05565540572186624, - "velocityX": 0.32022625146750794, - "velocityY": 0.06287422939745346, - "timestamp": 1.2792597308922307 - }, - { - "x": 2.6731092407857906, - "y": 4.320288302812448, - "heading": -0.7067988166038234, - "angularVelocity": 0.1958033723255861, - "velocityX": 0.10461638788158735, - "velocityY": 0.31161864341117435, - "timestamp": 1.3551332887134768 - }, - { - "x": 2.664687841052256, - "y": 4.3628053226693035, - "heading": -0.6813131694887641, - "angularVelocity": 0.33589629702821755, - "velocityX": -0.11099255097832775, - "velocityY": 0.5603667611869495, - "timestamp": 1.431006846534723 - }, - { - "x": 2.6399075701479586, - "y": 4.42419614745244, - "heading": -0.6452023149092349, - "angularVelocity": 0.47593464200079133, - "velocityX": -0.32659956402163576, - "velocityY": 0.8091201539132412, - "timestamp": 1.5068804043559691 - }, - { - "x": 2.598768648766779, - "y": 4.504461298133711, - "heading": -0.5984696942887661, - "angularVelocity": 0.6159276296230222, - "velocityX": -0.5422036683592814, - "velocityY": 1.057880412955572, - "timestamp": 1.5827539621772153 - }, - { - "x": 2.5412713665853555, - "y": 4.603601395355974, - "heading": -0.5411165139934166, - "angularVelocity": 0.7559047175548449, - "velocityX": -0.7578039548110096, - "velocityY": 1.3066488519598491, - "timestamp": 1.6586275199984615 - }, - { - "x": 2.4674160388824338, - "y": 4.721617069365857, - "heading": -0.47313875765168273, - "angularVelocity": 0.8959347405484323, - "velocityX": -0.9734000859460189, - "velocityY": 1.5554255975016502, - "timestamp": 1.7345010778197076 - }, - { - "x": 2.377202770872684, - "y": 4.858508647531332, - "heading": -0.39452213175531803, - "angularVelocity": 1.0361531494380563, - "velocityX": -1.1889948303725923, - "velocityY": 1.8042066576932412, - "timestamp": 1.8103746356409538 - }, - { - "x": 2.270629951391599, - "y": 5.014274677090094, - "heading": -0.305234926659776, - "angularVelocity": 1.1767894858012566, - "velocityX": -1.4046108096674554, - "velocityY": 2.0529685707303664, - "timestamp": 1.8862481934622 - }, - { - "x": 2.1562097285316435, - "y": 5.146459905330176, - "heading": -0.22910409225153178, - "angularVelocity": 1.0033908596890408, - "velocityX": -1.5080381906707807, - "velocityY": 1.7421778026344412, - "timestamp": 1.9621217512834461 - }, - { - "x": 2.0581334055736438, - "y": 5.259758402247803, - "heading": -0.16374394944584372, - "angularVelocity": 0.8614350596401947, - "velocityX": -1.2926284963034982, - "velocityY": 1.4932540422920335, - "timestamp": 2.0379953091046925 - }, - { - "x": 1.9764029763301112, - "y": 5.354172653968883, - "heading": -0.10920555144834718, - "angularVelocity": 0.7188063874223503, - "velocityX": -1.0771925238352613, - "velocityY": 1.2443630486526756, - "timestamp": 2.1138688669259387 - }, - { - "x": 1.9110185860607423, - "y": 5.429703503317112, - "heading": -0.06553422750381226, - "angularVelocity": 0.5755802838199697, - "velocityX": -0.8617546368678625, - "velocityY": 0.9954831632816045, - "timestamp": 2.189742424747185 - }, - { - "x": 1.8619801423257207, - "y": 5.486351407872208, - "heading": -0.032764704050183134, - "angularVelocity": 0.43189649194112756, - "velocityX": -0.6463179682397073, - "velocityY": 0.7466093087307788, - "timestamp": 2.265615982568431 - }, - { - "x": 1.8292876278534882, - "y": 5.524116629586748, - "heading": -0.010918068889347814, - "angularVelocity": 0.28793476658359446, - "velocityX": -0.43088152724695716, - "velocityY": 0.4977389066689233, - "timestamp": 2.341489540389677 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.394848489426687e-27, - "angularVelocity": 0.14389820647675833, - "velocityX": -0.21544309693644778, - "velocityY": 0.24886981095702526, - "timestamp": 2.4173630982109233 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.3925172511788446e-27, - "angularVelocity": -3.168744293663733e-29, - "velocityX": 8.48223659243716e-32, - "velocityY": -3.031681211954363e-28, - "timestamp": 2.4932366560321695 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -2.548660671865789e-25, + "angularVelocity": -1.3190073040663248e-25, + "velocityX": -3.501543589784201e-25, + "velocityY": -4.821953566499718e-25, + "timestamp": 0 + }, + { + "x": 1.3060567679582222, + "y": 5.572381918180447, + "heading": -0.010695335923127927, + "angularVelocity": -0.1421295269156777, + "velocityX": 0.21371671803125017, + "velocityY": -0.2468070561274771, + "timestamp": 0.07525062634925417 + }, + { + "x": 1.3382215476730444, + "y": 5.535237409026609, + "heading": -0.03207750843935064, + "angularVelocity": -0.2841461068640612, + "velocityX": 0.4274353752955963, + "velocityY": -0.49361063097923197, + "timestamp": 0.15050125269850834 + }, + { + "x": 1.3864692368555713, + "y": 5.479521033340732, + "heading": -0.06412446500671098, + "angularVelocity": -0.4258696322157321, + "velocityX": 0.6411599680061012, + "velocityY": -0.7404107897691923, + "timestamp": 0.2257518790477625 + }, + { + "x": 1.4508006060735015, + "y": 5.405233126364813, + "heading": -0.10680311296194826, + "angularVelocity": -0.5671533916163913, + "velocityX": 0.8548948007336773, + "velocityY": -0.9872064935530687, + "timestamp": 0.3010025053970167 + }, + { + "x": 1.5312167667505892, + "y": 5.312374207143071, + "heading": -0.1600730253151242, + "angularVelocity": -0.7078999197420489, + "velocityX": 1.0686444030892075, + "velocityY": -1.2339952997967572, + "timestamp": 0.37625313174627084 + }, + { + "x": 1.6277191879063078, + "y": 5.200945106889095, + "heading": -0.22389124150457193, + "angularVelocity": -0.8480755481456564, + "velocityX": 1.2824135271357124, + "velocityY": -1.4807730601046407, + "timestamp": 0.451503758095525 + }, + { + "x": 1.7403097782498615, + "y": 5.070947160382222, + "heading": -0.29821764025550423, + "angularVelocity": -0.987718007900262, + "velocityX": 1.496208015877461, + "velocityY": -1.7275330826287743, + "timestamp": 0.5267543844447792 + }, + { + "x": 1.8689915501241479, + "y": 4.922382870770339, + "heading": -0.3830195665070615, + "angularVelocity": -1.1269265169697522, + "velocityX": 1.7100425354208568, + "velocityY": -1.9742598410060204, + "timestamp": 0.6020050107940333 + }, + { + "x": 2.021815501878169, + "y": 4.778590249333106, + "heading": -0.46245356659686293, + "angularVelocity": -1.0555925437900195, + "velocityX": 2.03086617571437, + "velocityY": -1.9108494960541829, + "timestamp": 0.6772556371432875 + }, + { + "x": 2.1585523215512867, + "y": 4.653366950194428, + "heading": -0.5314334875144606, + "angularVelocity": -0.9166690599683149, + "velocityX": 1.8170854690098266, + "velocityY": -1.6640831473945463, + "timestamp": 0.7525062634925417 + }, + { + "x": 2.2791997725503816, + "y": 4.546710847450191, + "heading": -0.5899636474798271, + "angularVelocity": -0.7778029606519882, + "velocityX": 1.6032750403849743, + "velocityY": -1.417345049717245, + "timestamp": 0.8277568898417959 + }, + { + "x": 2.3837568392129738, + "y": 4.458620854469452, + "heading": -0.6380444617817417, + "angularVelocity": -0.6389423800774935, + "velocityX": 1.3894511146966526, + "velocityY": -1.1706213921980426, + "timestamp": 0.90300751619105 + }, + { + "x": 2.472222908494702, + "y": 4.389096261560078, + "heading": -0.6756747800114784, + "angularVelocity": -0.5000665118066445, + "velocityX": 1.1756190423072155, + "velocityY": -0.9239071657250583, + "timestamp": 0.9782581425403042 + }, + { + "x": 2.54459758971785, + "y": 4.338136588288653, + "heading": -0.7028528784400924, + "angularVelocity": -0.361167737029517, + "velocityX": 0.9617817782305474, + "velocityY": -0.6771993236961286, + "timestamp": 1.0535087688895584 + }, + { + "x": 2.600880650448604, + "y": 4.305741524929518, + "heading": -0.7195768777221777, + "angularVelocity": -0.2222439877704894, + "velocityX": 0.7479414253581413, + "velocityY": -0.43049559758855604, + "timestamp": 1.1287593952388124 + }, + { + "x": 2.6410719892542827, + "y": 4.291910904068807, + "heading": -0.7258449001015178, + "angularVelocity": -0.08329528514817768, + "velocityX": 0.5340997245543403, + "velocityY": -0.18379409623143778, + "timestamp": 1.2040100215880667 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05567774261378543, + "velocityX": 0.3202582509259972, + "velocityY": 0.06290689942978202, + "timestamp": 1.279260647937321 + }, + { + "x": 2.673111673605269, + "y": 4.320290744259783, + "heading": -0.7067971794350305, + "angularVelocity": 0.19582512759435053, + "velocityX": 0.10464854696595383, + "velocityY": 0.31165110401806617, + "timestamp": 1.3551341369201315 + }, + { + "x": 2.664692786669079, + "y": 4.362810218152184, + "heading": -0.6813099274420213, + "angularVelocity": 0.3359177538123147, + "velocityX": -0.11095953341617228, + "velocityY": 0.560399613388471, + "timestamp": 1.431007625902942 + }, + { + "x": 2.639915179239814, + "y": 4.424203570363792, + "heading": -0.6451974716196371, + "angularVelocity": 0.47595617792883843, + "velocityX": -0.32656475616770847, + "velocityY": 0.8091541991105307, + "timestamp": 1.5068811148857526 + }, + { + "x": 2.5987791710000745, + "y": 4.504471407000408, + "heading": -0.5984632131457898, + "angularVelocity": 0.6159497750846186, + "velocityX": -0.5421657655556009, + "velocityY": 1.057916773206541, + "timestamp": 1.5827546038685631 + }, + { + "x": 2.5412852001352904, + "y": 4.603614476417717, + "heading": -0.5411082975385818, + "angularVelocity": 0.7559282745018111, + "velocityX": -0.7577609997321935, + "velocityY": 1.3066892105063217, + "timestamp": 1.6586280928513737 + }, + { + "x": 2.4674338294975446, + "y": 4.721633621731354, + "heading": -0.47312860561398856, + "angularVelocity": 0.8959610640812155, + "velocityX": -0.9733488156116926, + "velocityY": 1.5554727599303477, + "timestamp": 1.7345015818341842 + }, + { + "x": 2.3772256595738117, + "y": 4.858529596063375, + "heading": -0.39450963445811277, + "angularVelocity": 1.0361849996602548, + "velocityX": -1.1889287171724772, + "velocityY": 1.804266235378164, + "timestamp": 1.8103750708169948 + }, + { + "x": 2.2706605653387557, + "y": 5.014302224131365, + "heading": -0.3052190347895922, + "angularVelocity": 1.176835293402019, + "velocityX": -1.404510266546452, + "velocityY": 2.0530574006327873, + "timestamp": 1.8862485597998053 + }, + { + "x": 2.156230166958563, + "y": 5.146478435153377, + "heading": -0.22909325420264964, + "angularVelocity": 1.0033251614959908, + "velocityX": -1.508173670596822, + "velocityY": 1.7420605377980973, + "timestamp": 1.9621220487826159 + }, + { + "x": 2.05814700584302, + "y": 5.25977079434431, + "heading": -0.16373664121531684, + "angularVelocity": 0.8613893187663968, + "velocityX": -1.2927197948912579, + "velocityY": 1.4931745028437962, + "timestamp": 2.0379955377654264 + }, + { + "x": 1.9764115799277049, + "y": 5.35418052420219, + "heading": -0.10920088427959268, + "angularVelocity": 0.7187722308127881, + "velocityX": -1.077259356477368, + "velocityY": 1.24430458021074, + "timestamp": 2.1138690267482367 + }, + { + "x": 1.9110235357976653, + "y": 5.42970804617497, + "heading": -0.06553152377220887, + "angularVelocity": 0.5755549282474322, + "velocityX": -0.8618035760139328, + "velocityY": 0.9954402121917941, + "timestamp": 2.189742515731047 + }, + { + "x": 1.8619825316383507, + "y": 5.486353607136737, + "heading": -0.03276339241171435, + "angularVelocity": 0.4318785362291475, + "velocityX": -0.6463523006095715, + "velocityY": 0.7465790979323437, + "timestamp": 2.2656160047138574 + }, + { + "x": 1.8292884005204861, + "y": 5.524117342639773, + "heading": -0.010917643304822403, + "angularVelocity": 0.287923349772952, + "velocityX": -0.4309032253053722, + "velocityY": 0.4977197702295186, + "timestamp": 2.3414894936966677 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.3743055340676842e-24, + "angularVelocity": 0.14389272789730093, + "velocityX": -0.21545347602984788, + "velocityY": 0.24886063882774975, + "timestamp": 2.417362982679478 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 6.7146804288849265e-25, + "angularVelocity": -3.3897350204457557e-25, + "velocityX": 3.460398907914197e-22, + "velocityY": 1.7844799561406784e-24, + "timestamp": 2.4932364716622883 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj index d4ca8cc4..61729206 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.2.traj @@ -1,103 +1,104 @@ { - "samples": [ - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.3925172511788446e-27, - "angularVelocity": -3.168744293663733e-29, - "velocityX": 8.48223659243716e-32, - "velocityY": -3.031681211954363e-28, - "timestamp": 0 - }, - { - "x": 1.8507557561824508, - "y": 5.5430527114919315, - "heading": -1.9710234547097696e-21, - "angularVelocity": -2.1171281249858964e-20, - "velocityX": 0.40617567078743816, - "velocityY": 0.0005740544395108319, - "timestamp": 0.09309903404237341 - }, - { - "x": 1.9263848800936068, - "y": 5.5431595993177165, - "heading": -2.0389636321585498e-20, - "angularVelocity": -1.9783892557349584e-19, - "velocityX": 0.8123513276919052, - "velocityY": 0.001148108859400643, - "timestamp": 0.18619806808474682 - }, - { - "x": 2.039828562987611, - "y": 5.543319931052194, - "heading": -2.352458064017618e-20, - "angularVelocity": -3.3673220682050347e-20, - "velocityX": 1.2185269596070256, - "velocityY": 0.001722163243972619, - "timestamp": 0.2792971021271202 - }, - { - "x": 2.1910867994360017, - "y": 5.54353370668769, - "heading": -3.364930009404432e-20, - "angularVelocity": -1.087521428982912e-19, - "velocityX": 1.6247025332136769, - "velocityY": 0.0022962175461363225, - "timestamp": 0.37239613616949363 - }, - { - "x": 2.3801595622964817, - "y": 5.543800926185845, - "heading": -1.7618072508363382e-20, - "angularVelocity": 1.721954234066794e-19, - "velocityX": 2.030877815278129, - "velocityY": 0.0028702714362588678, - "timestamp": 0.46549517021186704 - }, - { - "x": 2.5314177987448723, - "y": 5.544014701821341, - "heading": -7.829457975228232e-21, - "angularVelocity": 1.0514195591633778e-19, - "velocityX": 1.6247025332136766, - "velocityY": 0.0022962175461363225, - "timestamp": 0.5585942042542404 - }, - { - "x": 2.6448614816388765, - "y": 5.544175033555819, - "heading": 1.3151205701559364e-20, - "angularVelocity": 2.253585538517852e-19, - "velocityX": 1.2185269596070254, - "velocityY": 0.001722163243972619, - "timestamp": 0.6516932382966139 - }, - { - "x": 2.720490605550032, - "y": 5.544281921381604, - "heading": 9.829941746315052e-21, - "angularVelocity": -3.567452648039799e-20, - "velocityX": 0.8123513276919051, - "velocityY": 0.0011481088594006432, - "timestamp": 0.7447922723389873 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -1.6124701211579614e-34, - "angularVelocity": -1.0558586184512984e-19, - "velocityX": 0.40617567078743816, - "velocityY": 0.000574054439510832, - "timestamp": 0.8378913063813607 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "angularVelocity": 1.724633516740329e-33, - "velocityX": 6.381177903729197e-38, - "velocityY": -6.411016173939856e-36, - "timestamp": 0.9309903404237341 - } - ] + "samples": [ + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 6.7146804288849265e-25, + "angularVelocity": -3.3897350204457557e-25, + "velocityX": 3.460398907914197e-22, + "velocityY": 1.7844799561406784e-24, + "timestamp": 0 + }, + { + "x": 1.8507557540076562, + "y": 5.5430527114888575, + "heading": -2.374547601271383e-20, + "angularVelocity": -2.550633226443435e-19, + "velocityX": 0.40617567269200466, + "velocityY": 0.0005740544422026415, + "timestamp": 0.09309902825151006 + }, + { + "x": 1.9263848743459357, + "y": 5.543159599309593, + "heading": -2.866667304736003e-20, + "angularVelocity": -5.285981096799938e-20, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765753625, + "timestamp": 0.18619805650302013 + }, + { + "x": 2.0398285536670646, + "y": 5.543319931039021, + "heading": -2.74578198723704e-20, + "angularVelocity": 1.2984594980856443e-20, + "velocityX": 1.2185269970236072, + "velocityY": 0.0017221632968542275, + "timestamp": 0.2792970847545302 + }, + { + "x": 2.191086789804772, + "y": 5.543533706674078, + "heading": -4.1106983374446124e-20, + "angularVelocity": -1.4660908667312053e-19, + "velocityX": 1.6247026309348636, + "velocityY": 0.0022962176842474297, + "timestamp": 0.37239611300604025 + }, + { + "x": 2.380159571927711, + "y": 5.543800926199457, + "heading": -5.015487830921238e-20, + "angularVelocity": -9.718570757102548e-20, + "velocityX": 2.0308781485038963, + "velocityY": 0.002870271907212345, + "timestamp": 0.4654951412575503 + }, + { + "x": 2.5314178080654184, + "y": 5.544014701834514, + "heading": -3.4511971954263414e-20, + "angularVelocity": 1.6802437843559674e-19, + "velocityX": 1.6247026309348636, + "velocityY": 0.00229621768424743, + "timestamp": 0.5585941695090604 + }, + { + "x": 2.644861487386547, + "y": 5.544175033563942, + "heading": -6.180786622652372e-21, + "angularVelocity": 3.0431236355167837e-19, + "velocityX": 1.218526997023607, + "velocityY": 0.0017221632968542275, + "timestamp": 0.6516931977605704 + }, + { + "x": 2.7204906077248263, + "y": 5.544281921384678, + "heading": 5.803364763600579e-22, + "angularVelocity": 7.262291804719445e-20, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765753625, + "timestamp": 0.7447922260120805 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.254107177965744e-28, + "angularVelocity": -6.233540915514837e-21, + "velocityX": 0.40617567269200466, + "velocityY": 0.0005740544422026413, + "timestamp": 0.8378912542635906 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.3203819062171384e-28, + "angularVelocity": -7.150744492931399e-29, + "velocityX": -3.49033600951782e-22, + "velocityY": -5.085821437921445e-25, + "timestamp": 0.9309902825151006 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj index 8f1e23cf..09717c9c 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.3.traj @@ -1,211 +1,212 @@ { - "samples": [ - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "angularVelocity": 1.724633516740329e-33, - "velocityX": 6.381177903729197e-38, - "velocityY": -6.411016173939856e-36, - "timestamp": 0 - }, - { - "x": 2.7396220511907092, - "y": 5.562601673573336, - "heading": 0.003707886080781361, - "angularVelocity": 0.04789837119714626, - "velocityX": -0.24134799501070836, - "velocityY": 0.2359636718162648, - "timestamp": 0.07741152753441183 - }, - { - "x": 2.702380969511178, - "y": 5.599259386918254, - "heading": 0.011292704027819503, - "angularVelocity": 0.09798047123752293, - "velocityX": -0.4810792767650384, - "velocityY": 0.473543340539592, - "timestamp": 0.15482305506882366 - }, - { - "x": 2.6467699433072402, - "y": 5.6544931206396525, - "heading": 0.023007945503191744, - "angularVelocity": 0.1513371696504047, - "velocityX": -0.7183817187849313, - "velocityY": 0.7135078647924299, - "timestamp": 0.23223458260323548 - }, - { - "x": 2.573102880230356, - "y": 5.72860253774289, - "heading": 0.03927230490424404, - "angularVelocity": 0.21010255086133403, - "velocityX": -0.9516291103303324, - "velocityY": 0.9573434275701875, - "timestamp": 0.3096461101376473 - }, - { - "x": 2.4820081686264115, - "y": 5.82215722498829, - "heading": 0.060903903359814444, - "angularVelocity": 0.2794363984867048, - "velocityX": -1.1767589983733415, - "velocityY": 1.208536896572829, - "timestamp": 0.38705763767205914 - }, - { - "x": 2.3753503305689, - "y": 5.936638406081269, - "heading": 0.09019255741494606, - "angularVelocity": 0.378350033748035, - "velocityX": -1.3778030411568696, - "velocityY": 1.4788647729769806, - "timestamp": 0.46446916520647097 - }, - { - "x": 2.2779168004384966, - "y": 6.0748767248289015, - "heading": 0.1437819345039841, - "angularVelocity": 0.6922661106928292, - "velocityX": -1.25864368310122, - "velocityY": 1.7857588288279342, - "timestamp": 0.5418806927408828 - }, - { - "x": 2.2032159820535764, - "y": 6.200217981312059, - "heading": 0.20003111930602646, - "angularVelocity": 0.7266254341388357, - "velocityX": -0.9649831331866353, - "velocityY": 1.6191549304777408, - "timestamp": 0.6192922202752946 - }, - { - "x": 2.149623841782867, - "y": 6.310133396102646, - "heading": 0.25610236287269844, - "angularVelocity": 0.7243267941166335, - "velocityX": -0.6923018053982481, - "velocityY": 1.4198843284900597, - "timestamp": 0.696703747809706 - }, - { - "x": 2.1165539042639407, - "y": 6.4038530923195935, - "heading": 0.31105587872076873, - "angularVelocity": 0.7098880179523891, - "velocityX": -0.42719655033581183, - "velocityY": 1.2106684779639114, - "timestamp": 0.7741152753441174 - }, - { - "x": 2.1037067095365876, - "y": 6.481006876335726, - "heading": 0.3644203290348082, - "angularVelocity": 0.6893605127520235, - "velocityX": -0.1659597108666065, - "velocityY": 0.9966704762651164, - "timestamp": 0.8515268028785288 - }, - { - "x": 2.1109008821512623, - "y": 6.541377447734871, - "heading": 0.41591028587378526, - "angularVelocity": 0.6651458571992168, - "velocityX": 0.09293412549541542, - "velocityY": 0.7798653937207066, - "timestamp": 0.9289383304129402 - }, - { - "x": 2.1380148682221702, - "y": 6.5848219723004595, - "heading": 0.465333297367931, - "angularVelocity": 0.6384451136450678, - "velocityX": 0.3502577320781457, - "velocityY": 0.5612151826648336, - "timestamp": 1.0063498579473515 - }, - { - "x": 2.1849615573883057, - "y": 6.611239433288574, - "heading": 0.5125504196, - "angularVelocity": 0.6099494963599527, - "velocityX": 0.6064560493947926, - "velocityY": 0.3412600400679556, - "timestamp": 1.083761385481763 - }, - { - "x": 2.250707976446258, - "y": 6.648678224064634, - "heading": 0.55208552781375, - "angularVelocity": 0.5325541984016284, - "velocityX": 0.8856313560565378, - "velocityY": 0.5043159387113171, - "timestamp": 1.157998164259224 - }, - { - "x": 2.3371670619589073, - "y": 6.698226995892447, - "heading": 0.585356010779416, - "angularVelocity": 0.44816711492022837, - "velocityX": 1.1646395080237388, - "velocityY": 0.6674423734944787, - "timestamp": 1.2322349430366852 - }, - { - "x": 2.4409073566894506, - "y": 6.758690881227722, - "heading": 0.5617681472290975, - "angularVelocity": -0.31773824159353026, - "velocityX": 1.3974245170513677, - "velocityY": 0.8144734501011609, - "timestamp": 1.3064717218141464 - }, - { - "x": 2.523886913207481, - "y": 6.807069780587539, - "heading": 0.5422957792802172, - "angularVelocity": -0.262300820018773, - "velocityX": 1.1177688186980366, - "velocityY": 0.6516837092951012, - "timestamp": 1.3807085005916075 - }, - { - "x": 2.5861173110961473, - "y": 6.843356427328165, - "heading": 0.5274956073850987, - "angularVelocity": -0.1993644139582724, - "velocityX": 0.8382691021011796, - "velocityY": 0.4887960838037209, - "timestamp": 1.4549452793690687 - }, - { - "x": 2.627602512984248, - "y": 6.8675485030782895, - "heading": 0.5175502299027057, - "angularVelocity": -0.1339683327613954, - "velocityX": 0.558822763747067, - "velocityY": 0.3258772288954634, - "timestamp": 1.5291820581465299 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.06734950498988629, - "velocityX": 0.27940333769169035, - "velocityY": 0.16294305705300582, - "timestamp": 1.603418836923991 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -1.618919283202532e-33, - "velocityX": 2.4430876435253525e-35, - "velocityY": 8.017390657472539e-35, - "timestamp": 1.6776556157014522 - } - ] + "samples": [ + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.3203819062171384e-28, + "angularVelocity": -7.150744492931399e-29, + "velocityX": -3.49033600951782e-22, + "velocityY": -5.085821437921445e-25, + "timestamp": 0 + }, + { + "x": 2.7396220771473008, + "y": 5.562601682668762, + "heading": 0.0037078226871735676, + "angularVelocity": 0.0478975658583634, + "velocityX": -0.24134772812197372, + "velocityY": 0.23596385620231192, + "timestamp": 0.0774115055896134 + }, + { + "x": 2.702381046393162, + "y": 5.599259413613318, + "heading": 0.011292514660087486, + "angularVelocity": 0.09797887168250088, + "velocityX": -0.48107875528951993, + "velocityY": 0.47354370213249264, + "timestamp": 0.1548230111792268 + }, + { + "x": 2.6467700945928994, + "y": 5.6544931725755525, + "heading": 0.02300756873559927, + "angularVelocity": 0.15133479172485775, + "velocityX": -0.7183809612885809, + "velocityY": 0.7135083931198739, + "timestamp": 0.23223451676884022 + }, + { + "x": 2.573103126896307, + "y": 5.728602621187937, + "heading": 0.039271681184884964, + "angularVelocity": 0.21009942030462117, + "velocityX": -0.9516281479800662, + "velocityY": 0.9573441059946076, + "timestamp": 0.3096460223584536 + }, + { + "x": 2.482008526610063, + "y": 5.822157343718097, + "heading": 0.060902976833889094, + "angularVelocity": 0.27943256605393446, + "velocityX": -1.1767578939643621, + "velocityY": 1.208537694979451, + "timestamp": 0.387057527948067 + }, + { + "x": 2.3753508008518254, + "y": 5.936638558421237, + "heading": 0.09019128304072983, + "angularVelocity": 0.378345647507604, + "velocityX": -1.3778019810604039, + "velocityY": 1.4788656263843583, + "timestamp": 0.46446903353768043 + }, + { + "x": 2.2779173251453018, + "y": 6.074876969132681, + "heading": 0.1437805349404755, + "angularVelocity": 0.6922646897457585, + "velocityX": -1.258643336858141, + "velocityY": 1.7857605230454636, + "timestamp": 0.5418805391272938 + }, + { + "x": 2.2032165306277185, + "y": 6.200218266147509, + "heading": 0.20002968943075358, + "angularVelocity": 0.7266252485576944, + "velocityX": -0.964983098424666, + "velocityY": 1.6191559130668154, + "timestamp": 0.6192920447169072 + }, + { + "x": 2.1496243808514985, + "y": 6.310133689219402, + "heading": 0.2561009835178197, + "angularVelocity": 0.7243276520717804, + "velocityX": -0.6923021244455775, + "velocityY": 1.4198848379799673, + "timestamp": 0.6967035503065206 + }, + { + "x": 2.116554400688044, + "y": 6.403853368096058, + "heading": 0.3110546267083605, + "angularVelocity": 0.7098898642001644, + "velocityX": -0.4271972223195187, + "velocityY": 1.2106685971656326, + "timestamp": 0.774115055896134 + }, + { + "x": 2.1037071305017707, + "y": 6.481007112457934, + "heading": 0.3644192794127437, + "angularVelocity": 0.6893633226472664, + "velocityX": -0.1659607326898067, + "velocityY": 0.9966702465508831, + "timestamp": 0.8515265614857475 + }, + { + "x": 2.110901195101711, + "y": 6.541377623864855, + "heading": 0.41590951273781046, + "angularVelocity": 0.6651496173971262, + "velocityX": 0.09293275650881538, + "velocityY": 0.7798648398206792, + "timestamp": 0.9289380670753609 + }, + { + "x": 2.138015040799913, + "y": 6.58482206941201, + "heading": 0.4653328742353283, + "angularVelocity": 0.6384498159683032, + "velocityX": 0.3502560180387482, + "velocityY": 0.5612143210012033, + "timestamp": 1.0063495726649743 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.6099551352868545, + "velocityX": 0.6064539919591982, + "velocityY": 0.3412588823244487, + "timestamp": 1.0837610782545877 + }, + { + "x": 2.2507078582365074, + "y": 6.648678152595124, + "heading": 0.5520861160632833, + "angularVelocity": 0.5325619619367677, + "velocityX": 0.8856294969362879, + "velocityY": 0.5043148240682422, + "timestamp": 1.157997879395067 + }, + { + "x": 2.3371668542660595, + "y": 6.698226860856562, + "heading": 0.585357485403268, + "angularVelocity": 0.4481789197385401, + "velocityX": 1.1646379518150936, + "velocityY": 0.6674413161698084, + "timestamp": 1.2322346805355462 + }, + { + "x": 2.4409072196533095, + "y": 6.7586907970323, + "heading": 0.5617689305008087, + "angularVelocity": -0.31774745867379856, + "velocityX": 1.3974250478673051, + "velocityY": 0.8144738895917877, + "timestamp": 1.3064714816760254 + }, + { + "x": 2.5238868313790523, + "y": 6.807069731854914, + "heading": 0.5422961869631279, + "angularVelocity": -0.2623058003379255, + "velocityX": 1.117769225652917, + "velocityY": 0.6516839906809204, + "timestamp": 1.3807082828165047 + }, + { + "x": 2.586117270308082, + "y": 6.8433564035586825, + "heading": 0.5274957902762942, + "angularVelocity": -0.19936738193805836, + "velocityX": 0.8382694024122912, + "velocityY": 0.4887962728229996, + "timestamp": 1.454945083956984 + }, + { + "x": 2.6276024994187765, + "y": 6.867548495303344, + "heading": 0.5175502856493861, + "angularVelocity": -0.13397000509340504, + "velocityX": 0.5588229621073203, + "velocityY": 0.3258773461814771, + "timestamp": 1.5291818850974632 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06735023563211984, + "velocityX": 0.2794034362569526, + "velocityY": 0.16294311269989367, + "timestamp": 1.6034186862379425 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 9.95994551309633e-28, + "velocityX": -2.708487136756449e-26, + "velocityY": -1.710598190536398e-26, + "timestamp": 1.6776554873784217 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj index 2c5b2ae4..2cb04b94 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.4.traj @@ -1,868 +1,869 @@ { - "samples": [ - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -1.618919283202532e-33, - "velocityX": 2.4430876435253525e-35, - "velocityY": 8.017390657472539e-35, - "timestamp": 0 - }, - { - "x": 2.6570338601157935, - "y": 6.867732812321398, - "heading": 0.5065324038818937, - "angularVelocity": -0.10327871439636148, - "velocityX": 0.1491229424075764, - "velocityY": -0.20442985508511316, - "timestamp": 0.05826966140390244 - }, - { - "x": 2.674422013328184, - "y": 6.84391114835803, - "heading": 0.49465795152131636, - "angularVelocity": -0.2037844750507175, - "velocityX": 0.2984083448136593, - "velocityY": -0.40881761433700353, - "timestamp": 0.11653932280780488 - }, - { - "x": 2.7005198630420675, - "y": 6.808182791545169, - "heading": 0.47711568998903314, - "angularVelocity": -0.30105308851354295, - "velocityX": 0.4478805794491053, - "velocityY": -0.6131553874185988, - "timestamp": 0.17480898421170732 - }, - { - "x": 2.7353400941220496, - "y": 6.760551298473928, - "heading": 0.4541277672109497, - "angularVelocity": -0.39450929049922157, - "velocityX": 0.5975705065217742, - "velocityY": -0.8174321237441035, - "timestamp": 0.23307864561560976 - }, - { - "x": 2.7788976924779565, - "y": 6.701021131245012, - "heading": 0.42595856862842546, - "angularVelocity": -0.4834282181127908, - "velocityX": 0.7475176156247499, - "velocityY": -1.021632283329671, - "timestamp": 0.2913483070195122 - }, - { - "x": 2.831210629197317, - "y": 6.629598041190472, - "heading": 0.39292676107093794, - "angularVelocity": -0.5668783164625577, - "velocityX": 0.8977731371519051, - "velocityY": -1.2257337409164428, - "timestamp": 0.34961796842341464 - }, - { - "x": 2.8923008190981245, - "y": 6.546289653631685, - "heading": 0.3554225990889349, - "angularVelocity": -0.6436310264794379, - "velocityX": 1.0484047517859123, - "velocityY": -1.4297043358691544, - "timestamp": 0.4078876298273171 - }, - { - "x": 2.962195513842206, - "y": 6.451106403793388, - "heading": 0.31393409063739947, - "angularVelocity": -0.7120087443781998, - "velocityX": 1.1995040482490311, - "velocityY": -1.6334958457802513, - "timestamp": 0.4661572912312195 - }, - { - "x": 3.0409294178085977, - "y": 6.344063134427028, - "heading": 0.26908904007938106, - "angularVelocity": -0.7696123416123889, - "velocityX": 1.351198927013472, - "velocityY": -1.8370326304863622, - "timestamp": 0.524426952635122 - }, - { - "x": 3.128548070700225, - "y": 6.22518206352159, - "heading": 0.22172760975049532, - "angularVelocity": -0.8127974178637293, - "velocityX": 1.5036753394582005, - "velocityY": -2.0401881191895193, - "timestamp": 0.5826966140390244 - }, - { - "x": 3.2251135709999605, - "y": 6.094498947078054, - "heading": 0.17303894760412505, - "angularVelocity": -0.8355748252745036, - "velocityX": 1.6572174605646188, - "velocityY": -2.2427299780874135, - "timestamp": 0.6409662754429268 - }, - { - "x": 3.3307147862388833, - "y": 5.952077971116604, - "heading": 0.12484956119030122, - "angularVelocity": -0.8270064601850674, - "velocityX": 1.8122846897450882, - "velocityY": -2.444170302865565, - "timestamp": 0.6992359368468293 - }, - { - "x": 3.4454853336507822, - "y": 5.798056568785059, - "heading": 0.08033941158282583, - "angularVelocity": -0.7638649090295626, - "velocityX": 1.9696450030206056, - "velocityY": -2.643252056399113, - "timestamp": 0.7575055982507317 - }, - { - "x": 3.5696119500976344, - "y": 5.632835617985738, - "heading": 0.04630921837828673, - "angularVelocity": -0.5840122009402995, - "velocityX": 2.1302100176360224, - "velocityY": -2.8354541079975273, - "timestamp": 0.8157752596546342 - }, - { - "x": 3.7020235307305986, - "y": 5.458025004439994, - "heading": 0.04313298905641362, - "angularVelocity": -0.05450914327194612, - "velocityX": 2.272393170695454, - "velocityY": -3.00002796196164, - "timestamp": 0.8740449210585366 - }, - { - "x": 3.84656727081631, - "y": 5.291677538591243, - "heading": 0.04313296518805293, - "angularVelocity": -4.0961900430587924e-7, - "velocityX": 2.480600309032019, - "velocityY": -2.8547868966613104, - "timestamp": 0.932314582462439 - }, - { - "x": 4.001959806149131, - "y": 5.135416436419877, - "heading": 0.04313294238925072, - "angularVelocity": -3.912636809189277e-7, - "velocityX": 2.666782877897643, - "velocityY": -2.6816888652951767, - "timestamp": 0.9905842438663415 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.04313291889737663, - "angularVelocity": -4.031578960087653e-7, - "velocityX": 2.8409167869122447, - "velocityY": -2.496473071014189, - "timestamp": 1.048853905270244 - }, - { - "x": 4.2693517733895705, - "y": 4.907420739393082, - "heading": 0.043132896624531875, - "angularVelocity": -6.4257109962904e-7, - "velocityX": 2.9384484685182977, - "velocityY": -2.3809038315668807, - "timestamp": 1.0835159757750095 - }, - { - "x": 4.374422288805421, - "y": 4.829031478695207, - "heading": 0.0431328753158102, - "angularVelocity": -6.147561689330783e-7, - "velocityX": 3.0312821445967915, - "velocityY": -2.261528511030489, - "timestamp": 1.118178046279775 - }, - { - "x": 4.482542607371009, - "y": 4.7549053274480135, - "heading": 0.043132854776618594, - "angularVelocity": -5.925552428867745e-7, - "velocityX": 3.1192689008789145, - "velocityY": -2.138537893661084, - "timestamp": 1.1528401167845406 - }, - { - "x": 4.593539840912748, - "y": 4.685160765838758, - "heading": 0.04313283483631883, - "angularVelocity": -5.752772258439431e-7, - "velocityX": 3.2022678370145012, - "velocityY": -2.012129125398518, - "timestamp": 1.1875021872893061 - }, - { - "x": 4.707236482283354, - "y": 4.619909225059653, - "heading": 0.04313281534202855, - "angularVelocity": -5.624098616258592e-7, - "velocityX": 3.280145695710103, - "velocityY": -1.882505569600442, - "timestamp": 1.2221642577940717 - }, - { - "x": 4.823450643664921, - "y": 4.559254805625301, - "heading": 0.043132796153422026, - "angularVelocity": -5.53590891933908e-7, - "velocityX": 3.352776094711051, - "velocityY": -1.7498787161607805, - "timestamp": 1.2568263282988372 - }, - { - "x": 4.9419959983331205, - "y": 4.5032933115020395, - "heading": 0.04313277713835443, - "angularVelocity": -5.485842974798401e-7, - "velocityX": 3.4200309716611494, - "velocityY": -1.6144879203210547, - "timestamp": 1.2914883988036028 - }, - { - "x": 5.061736542982073, - "y": 4.449937038090927, - "heading": 0.04313275817159016, - "angularVelocity": -5.471907475389428e-7, - "velocityX": 3.454512177294491, - "velocityY": -1.5393273579481606, - "timestamp": 1.3261504693083683 - }, - { - "x": 5.1814776709209625, - "y": 4.396582073700789, - "heading": 0.04313273920484137, - "angularVelocity": -5.471903010583792e-7, - "velocityX": 3.45452900519684, - "velocityY": -1.5392895927207566, - "timestamp": 1.3608125398131339 - }, - { - "x": 5.3012188001688525, - "y": 4.3432271122483534, - "heading": 0.043132720238092614, - "angularVelocity": -5.471902999394187e-7, - "velocityX": 3.454529042961442, - "velocityY": -1.5392895079680768, - "timestamp": 1.3954746103178994 - }, - { - "x": 5.420963133587213, - "y": 4.289879342291843, - "heading": 0.04313270127136276, - "angularVelocity": -5.471897545932471e-7, - "velocityX": 3.4546214832116693, - "velocityY": -1.5390820334629507, - "timestamp": 1.430136680822665 - }, - { - "x": 5.5425254594597755, - "y": 4.240815191837089, - "heading": 0.04313268228677855, - "angularVelocity": -5.47704852461645e-7, - "velocityX": 3.5070705270145455, - "velocityY": -1.415499701554448, - "timestamp": 1.4647987513274305 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.043132663161499156, - "angularVelocity": -5.517639056314665e-7, - "velocityX": 3.560845257182681, - "velocityY": -1.274191548268104, - "timestamp": 1.499460821832196 - }, - { - "x": 5.795862627864808, - "y": 4.156159506730396, - "heading": 0.04313264445875193, - "angularVelocity": -5.198107758201059e-7, - "velocityX": 3.6106505851434836, - "velocityY": -1.1253380804221886, - "timestamp": 1.5354407362494324 - }, - { - "x": 5.927341530201614, - "y": 4.121094637119033, - "heading": 0.043132625947930554, - "angularVelocity": -5.1447652596799e-7, - "velocityX": 3.654230546858154, - "velocityY": -0.9745678993212189, - "timestamp": 1.5714206506666688 - }, - { - "x": 6.059882410873037, - "y": 4.090286167435021, - "heading": 0.04313260749907939, - "angularVelocity": -5.127541702089576e-7, - "velocityX": 3.683746412913316, - "velocityY": -0.8562685649205634, - "timestamp": 1.6074005650839052 - }, - { - "x": 6.193269302687495, - "y": 4.06337550162166, - "heading": 0.043132589025398085, - "angularVelocity": -5.134442814470016e-7, - "velocityX": 3.7072598413563593, - "velocityY": -0.7479357927674594, - "timestamp": 1.6433804795011415 - }, - { - "x": 6.327657815035056, - "y": 4.042021941101089, - "heading": 0.043132570411953335, - "angularVelocity": -5.173287667087509e-7, - "velocityX": 3.7350981658583504, - "velocityY": -0.5934855840107985, - "timestamp": 1.6793603939183779 - }, - { - "x": 6.462816636912207, - "y": 4.026263156605077, - "heading": 0.043132547717955015, - "angularVelocity": -6.30740753253067e-7, - "velocityX": 3.7565075978169236, - "velocityY": -0.4379883818862704, - "timestamp": 1.7153403083356142 - }, - { - "x": 6.597942834210822, - "y": 4.016131027814117, - "heading": 0.04107885634805962, - "angularVelocity": -0.0570788286508975, - "velocityX": 3.755600853621886, - "velocityY": -0.2816051387300244, - "timestamp": 1.7513202227528506 - }, - { - "x": 6.728400824069692, - "y": 4.008818936530008, - "heading": 0.03307687475358465, - "angularVelocity": -0.2224013515341097, - "velocityX": 3.625856035843536, - "velocityY": -0.20322703381989088, - "timestamp": 1.787300137170087 - }, - { - "x": 6.853599659736003, - "y": 4.0035443033890195, - "heading": 0.023873611715651655, - "angularVelocity": -0.25578890853403846, - "velocityX": 3.47968686680181, - "velocityY": -0.14659937986014848, - "timestamp": 1.8232800515873233 - }, - { - "x": 6.973435503233387, - "y": 4.000040041798795, - "heading": 0.014893418694113961, - "angularVelocity": -0.24958906009058449, - "velocityX": 3.3306316993343295, - "velocityY": -0.09739493956513012, - "timestamp": 1.8592599660045597 - }, - { - "x": 7.08787150697828, - "y": 3.998176312998215, - "heading": 0.006805585628585594, - "angularVelocity": -0.22478744589936686, - "velocityX": 3.1805524164913392, - "velocityY": -0.05179914490532842, - "timestamp": 1.895239880421796 - }, - { - "x": 7.196889877319336, - "y": 3.9978766441345215, - "heading": 3.395025904591232e-32, - "angularVelocity": -0.18914957800247972, - "velocityX": 3.0299785896330556, - "velocityY": -0.008328782003720711, - "timestamp": 1.9312197948390324 - }, - { - "x": 7.373371840772393, - "y": 4.002118083585038, - "heading": -0.0072355781339062996, - "angularVelocity": -0.11324248534202788, - "velocityX": 2.7620814521804613, - "velocityY": 0.06638186139590158, - "timestamp": 1.9951143491845382 - }, - { - "x": 7.532286444106961, - "y": 4.009181217967762, - "heading": -0.011195509678912387, - "angularVelocity": -0.06197604139459259, - "velocityX": 2.4871384574536473, - "velocityY": 0.11054360508613674, - "timestamp": 2.059008903530044 - }, - { - "x": 7.6734693827744955, - "y": 4.017827175121804, - "heading": -0.012870706885804134, - "angularVelocity": -0.02621815308129755, - "velocityX": 2.209623967390062, - "velocityY": 0.13531602563950743, - "timestamp": 2.12290345787555 - }, - { - "x": 7.7968608797168075, - "y": 4.027211909710403, - "heading": -0.012933711131137412, - "angularVelocity": -0.0009860659641287687, - "velocityX": 1.9311739193778674, - "velocityY": 0.14687847320841513, - "timestamp": 2.1867980122210557 - }, - { - "x": 7.9024437482359025, - "y": 4.036727334915007, - "heading": -0.011868630755415614, - "angularVelocity": 0.01666934508944925, - "velocityX": 1.6524548860324264, - "velocityY": 0.14892388407859264, - "timestamp": 2.2506925665665616 - }, - { - "x": 7.99021929218393, - "y": 4.0459160326905685, - "heading": -0.010039813807728299, - "angularVelocity": 0.028622422777974377, - "velocityX": 1.3737562589980292, - "velocityY": 0.143810342989083, - "timestamp": 2.3145871209120674 - }, - { - "x": 8.060196978947376, - "y": 4.0544220986291215, - "heading": -0.00773105502412215, - "angularVelocity": 0.03613388976972427, - "velocityX": 1.0952058040039734, - "velocityY": 0.13312661815523763, - "timestamp": 2.3784816752575733 - }, - { - "x": 8.112389704520517, - "y": 4.061961050008052, - "heading": -0.005169479155646227, - "angularVelocity": 0.04009067587551167, - "velocityX": 0.816857181457298, - "velocityY": 0.11799051509403427, - "timestamp": 2.442376229603079 - }, - { - "x": 8.146811527228987, - "y": 4.068300458431988, - "heading": -0.0025408789062790695, - "angularVelocity": 0.04113966012116123, - "velocityX": 0.5387285827574291, - "velocityY": 0.09921672494429698, - "timestamp": 2.506270783948585 - }, - { - "x": 8.1634765625, - "y": 4.073246955871582, - "heading": 2.9433151473989685e-31, - "angularVelocity": 0.03976675214822599, - "velocityX": 0.2608209015888442, - "velocityY": 0.07741657313776515, - "timestamp": 2.5701653382940908 - }, - { - "x": 8.160341731543628, - "y": 4.076804852758406, - "heading": 0.0025302388406713986, - "angularVelocity": 0.03600436134361471, - "velocityX": -0.0446074831711938, - "velocityY": 0.05062755463137832, - "timestamp": 2.640441236719939 - }, - { - "x": 8.135735418270036, - "y": 4.078564432167465, - "heading": 0.004797198596772283, - "angularVelocity": 0.032257997505259615, - "velocityX": -0.3501387221617275, - "velocityY": 0.025038163132351486, - "timestamp": 2.710717135145787 - }, - { - "x": 8.089649517071235, - "y": 4.078625238037987, - "heading": 0.006802207301300417, - "angularVelocity": 0.028530531084475955, - "velocityX": -0.6557853009510525, - "velocityY": 0.0008652450112215468, - "timestamp": 2.780993033571635 - }, - { - "x": 8.02207492391753, - "y": 4.077106587290107, - "heading": 0.008546856784408466, - "angularVelocity": 0.024825715817051033, - "velocityX": -0.9615614267102518, - "velocityY": -0.02160983753886332, - "timestamp": 2.851268931997483 - }, - { - "x": 7.933001430575272, - "y": 4.074154095631599, - "heading": 0.010033092526327095, - "angularVelocity": 0.021148584012580587, - "velocityX": -1.2674828118525576, - "velocityY": -0.042012862512522016, - "timestamp": 2.921544830423331 - }, - { - "x": 7.822417690835061, - "y": 4.069949420479082, - "heading": 0.011263348791088614, - "angularVelocity": 0.017506090883485915, - "velocityX": -1.5735656493512467, - "velocityY": -0.059830969744960986, - "timestamp": 2.991820728849179 - }, - { - "x": 7.690311409008121, - "y": 4.0647254734139295, - "heading": 0.012240761619989172, - "angularVelocity": 0.013908222460249003, - "velocityX": -1.879823449946105, - "velocityY": -0.07433483145952353, - "timestamp": 3.062096627275027 - }, - { - "x": 7.536670161450006, - "y": 4.058791543485583, - "heading": 0.012969524954057786, - "angularVelocity": 0.01037003226415625, - "velocityX": -2.186258034398947, - "velocityY": -0.08443762458060709, - "timestamp": 3.132372525700875 - }, - { - "x": 7.361484060419744, - "y": 4.0525779000493936, - "heading": 0.01345553350071711, - "angularVelocity": 0.006915721570918652, - "velocityX": -2.492833317742797, - "velocityY": -0.08841784417378167, - "timestamp": 3.202648424126723 - }, - { - "x": 7.164754272980972, - "y": 4.046723019235429, - "heading": 0.013707670109403618, - "angularVelocity": 0.0035878105343975153, - "velocityX": -2.799391994203425, - "velocityY": -0.08331278496768466, - "timestamp": 3.272924322552571 - }, - { - "x": 6.9465233889455575, - "y": 4.0422689491096495, - "heading": 0.013740797003253829, - "angularVelocity": 0.0004713834272096328, - "velocityX": -3.1053446334191217, - "velocityY": -0.06337976782296549, - "timestamp": 3.343200220978419 - }, - { - "x": 6.707012866607414, - "y": 4.041196732301159, - "heading": 0.013584512522768889, - "angularVelocity": -0.002223870259728453, - "velocityX": -3.408146003154458, - "velocityY": -0.015257247968487717, - "timestamp": 3.413476119404267 - }, - { - "x": 6.4476145619873, - "y": 4.048415292748879, - "heading": 0.013321412464940579, - "angularVelocity": -0.0037438163541362824, - "velocityX": -3.691141777345175, - "velocityY": 0.10271744096358496, - "timestamp": 3.483752017830115 - }, - { - "x": 6.1833350728544865, - "y": 4.076620284873478, - "heading": 0.013321410992019302, - "angularVelocity": -2.0959124115556836e-8, - "velocityX": -3.76059922466405, - "velocityY": 0.4013465890352113, - "timestamp": 3.554027916255963 - }, - { - "x": 5.922208724930001, - "y": 4.126140027657945, - "heading": 0.013321410307177713, - "angularVelocity": -9.745042100450887e-9, - "velocityX": -3.7157311933907815, - "velocityY": 0.7046475946048248, - "timestamp": 3.624303814681811 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.013321409576784942, - "angularVelocity": -1.0393218545701755e-8, - "velocityX": -3.646442120972535, - "velocityY": 1.003317616364548, - "timestamp": 3.694579713107659 - }, - { - "x": 5.541094088534591, - "y": 4.236435199254689, - "heading": 0.013321408817775413, - "angularVelocity": -2.1905263169095435e-8, - "velocityX": -3.6034323243164974, - "velocityY": 1.148240568022246, - "timestamp": 3.7292293551846463 - }, - { - "x": 5.41792618310833, - "y": 4.281179290629024, - "heading": 0.01332140812506875, - "angularVelocity": -1.9991740821363056e-8, - "velocityX": -3.5546660237528647, - "velocityY": 1.2913291073806714, - "timestamp": 3.7638789972616333 - }, - { - "x": 5.2966447994258585, - "y": 4.330809856867523, - "heading": 0.01332140748575771, - "angularVelocity": -1.8450725633992864e-8, - "velocityX": -3.500220389376678, - "velocityY": 1.432354369728484, - "timestamp": 3.7985286393386204 - }, - { - "x": 5.177443709175996, - "y": 4.385247597760623, - "heading": 0.013321406889674104, - "angularVelocity": -1.7203167808757175e-8, - "velocityX": -3.4401824407021615, - "velocityY": 1.5710910020988935, - "timestamp": 3.8331782814156075 - }, - { - "x": 5.0605133585968645, - "y": 4.444405530588187, - "heading": 0.01332140632867156, - "angularVelocity": -1.6190716838073352e-8, - "velocityX": -3.3746481513237594, - "velocityY": 1.7073172847247369, - "timestamp": 3.8678279234925945 - }, - { - "x": 4.946040562745207, - "y": 4.508189127302759, - "heading": 0.013321405796093607, - "angularVelocity": -1.5370373829355437e-8, - "velocityX": -3.3037223183234317, - "velocityY": 1.840815456992452, - "timestamp": 3.9024775655695816 - }, - { - "x": 4.834208203167093, - "y": 4.576496460724576, - "heading": 0.013321405286393582, - "angularVelocity": -1.4710109443101504e-8, - "velocityX": -3.2275184641053225, - "velocityY": 1.9713719775242866, - "timestamp": 3.9371272076465686 - }, - { - "x": 4.725194921038476, - "y": 4.649218348571895, - "heading": 0.013321404794856252, - "angularVelocity": -1.4185928002634242e-8, - "velocityX": -3.146158967137548, - "velocityY": 2.098777461704854, - "timestamp": 3.9717768497235557 - }, - { - "x": 4.619174674827694, - "y": 4.72623832110653, - "heading": 0.013321404313366907, - "angularVelocity": -1.3895939972772152e-8, - "velocityX": -3.05977897189302, - "velocityY": 2.2228215911583726, - "timestamp": 4.006426491800543 - }, - { - "x": 4.510161328966249, - "y": 4.798960113344169, - "heading": 0.013321403820821752, - "angularVelocity": -1.421501423878401e-8, - "velocityX": -3.146160806487812, - "velocityY": 2.0987747023782037, - "timestamp": 4.04107613387753 - }, - { - "x": 4.398328904529435, - "y": 4.867267340500532, - "heading": 0.013321403310039242, - "angularVelocity": -1.4741350266093223e-8, - "velocityX": -3.2275203359486127, - "velocityY": 1.9713689106685317, - "timestamp": 4.075725775954517 - }, - { - "x": 4.283856051297854, - "y": 4.931050823285602, - "heading": 0.013321399241018467, - "angularVelocity": -1.1743327000811157e-7, - "velocityX": -3.3037239743267564, - "velocityY": 1.8408121689497896, - "timestamp": 4.110375418031504 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.012938020716139158, - "angularVelocity": -0.011064429584221965, - "velocityX": -3.358100659168625, - "velocityY": 1.6997858867187727, - "timestamp": 4.145025060108491 - }, - { - "x": 3.941212684187486, - "y": 5.0868100262217055, - "heading": 0.011498233293039487, - "angularVelocity": -0.020924944231025543, - "velocityX": -3.2887007006223063, - "velocityY": 1.4077333471091193, - "timestamp": 4.213832288801836 - }, - { - "x": 3.7303193403637978, - "y": 5.1698989262671065, - "heading": 0.009891075368403834, - "angularVelocity": -0.023357399435433027, - "velocityX": -3.0649881971497734, - "velocityY": 1.2075606244178938, - "timestamp": 4.282639517495181 - }, - { - "x": 3.5371027460588107, - "y": 5.242302097421373, - "heading": 0.008284033873001792, - "angularVelocity": -0.023355707327847468, - "velocityX": -2.808085690619801, - "velocityY": 1.0522611145545084, - "timestamp": 4.351446746188525 - }, - { - "x": 3.362389747323273, - "y": 5.305525449861525, - "heading": 0.006750320632579214, - "angularVelocity": -0.022290001640058982, - "velocityX": -2.5391663354759597, - "velocityY": 0.9188475344926457, - "timestamp": 4.42025397488187 - }, - { - "x": 3.2065870657576463, - "y": 5.360439069495437, - "heading": 0.00533021576404821, - "angularVelocity": -0.0206388906441795, - "velocityX": -2.2643359502240057, - "velocityY": 0.7980792233131078, - "timestamp": 4.489061203575215 - }, - { - "x": 3.0699321768781336, - "y": 5.407605853881433, - "heading": 0.00404906921437406, - "angularVelocity": -0.018619359826041904, - "velocityX": -1.9860542485811465, - "velocityY": 0.6854917031494734, - "timestamp": 4.55786843226856 - }, - { - "x": 2.952579395626419, - "y": 5.447418742231964, - "heading": 0.002924266712171353, - "angularVelocity": -0.01634715601198879, - "velocityX": -1.705529832842474, - "velocityY": 0.578614908732415, - "timestamp": 4.626675660961904 - }, - { - "x": 2.8546365398725873, - "y": 5.480167232965782, - "heading": 0.001968461119345437, - "angularVelocity": -0.013891063642247188, - "velocityX": -1.4234384615362952, - "velocityY": 0.47594549810702536, - "timestamp": 4.695482889655249 - }, - { - "x": 2.776182973714466, - "y": 5.506073331672824, - "heading": 0.0011912691725219405, - "angularVelocity": -0.01129520780857525, - "velocityX": -1.140193663485079, - "velocityY": 0.376502573915577, - "timestamp": 4.764290118348594 - }, - { - "x": 2.7172794523584742, - "y": 5.525312622061272, - "heading": 0.0006002459449936435, - "angularVelocity": -0.008589551399640344, - "velocityX": -0.8560658883459488, - "velocityY": 0.27961147039059914, - "timestamp": 4.833097347041939 - }, - { - "x": 2.6779739252366173, - "y": 5.5380274141109735, - "heading": 0.00020148371013726726, - "angularVelocity": -0.005795353808442843, - "velocityX": -0.5712412470066293, - "velocityY": 0.18478860856854942, - "timestamp": 4.9019045757352835 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -3.5000485154255244e-36, - "angularVelocity": -0.0029282346341141605, - "velocityX": -0.285853063090511, - "velocityY": 0.09167570478022308, - "timestamp": 4.970711804428628 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 7.813037085898689e-36, - "angularVelocity": 1.6441710918960667e-34, - "velocityX": -8.361910991338164e-36, - "velocityY": -1.503965302427871e-35, - "timestamp": 5.039519033121973 - } - ] + "samples": [ + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 9.95994551309633e-28, + "velocityX": -2.708487136756449e-26, + "velocityY": -1.710598190536398e-26, + "timestamp": 0 + }, + { + "x": 2.657033865407608, + "y": 6.8677328189813025, + "heading": 0.5065323846197595, + "angularVelocity": -0.10327905668354015, + "velocityX": 0.1491230501434533, + "velocityY": -0.20442976398577012, + "timestamp": 0.058269654792457715 + }, + { + "x": 2.674422029082572, + "y": 6.843911168058592, + "heading": 0.49465789497247153, + "angularVelocity": -0.20378513807198495, + "velocityX": 0.2984085582263455, + "velocityY": -0.40881743692420125, + "timestamp": 0.11653930958491543 + }, + { + "x": 2.700519894286892, + "y": 6.808182830343848, + "heading": 0.4771155795523401, + "angularVelocity": -0.30105404747312803, + "velocityX": 0.4478808961074784, + "velocityY": -0.6131551292349195, + "timestamp": 0.17480896437737314 + }, + { + "x": 2.7353401457143893, + "y": 6.760551362049968, + "heading": 0.4541275879247931, + "angularVelocity": -0.3945105168277461, + "velocityX": 0.5975709235196038, + "velocityY": -0.8174317912733681, + "timestamp": 0.23307861916983086 + }, + { + "x": 2.7788977690674797, + "y": 6.7010212248295815, + "heading": 0.42595830742432134, + "angularVelocity": -0.4834296788062957, + "velocityX": 0.7475181294317162, + "velocityY": -1.0216318842529362, + "timestamp": 0.2913482739622886 + }, + { + "x": 2.8312107351766715, + "y": 6.629598169475443, + "heading": 0.3929264070745021, + "angularVelocity": -0.5668799732462906, + "velocityX": 0.8977737433921239, + "velocityY": -1.2257332844776365, + "timestamp": 0.3496179287547463 + }, + { + "x": 2.8923009585334296, + "y": 6.546289820646855, + "heading": 0.35542214398319716, + "angularVelocity": -0.6436328347041991, + "velocityX": 1.048405444898319, + "velocityY": -1.4297038334157206, + "timestamp": 0.407887583547204 + }, + { + "x": 2.9621956903718707, + "y": 6.451106612735035, + "heading": 0.3139335291126181, + "angularVelocity": -0.7120106514849124, + "velocityX": 1.199504820946487, + "velocityY": -1.6334953115963773, + "timestamp": 0.4661572383396617 + }, + { + "x": 3.040929634489161, + "y": 6.344063387407532, + "heading": 0.2690883704076084, + "angularVelocity": -0.7696142849093055, + "velocityX": 1.3511997693777469, + "velocityY": -1.8370320831445586, + "timestamp": 0.5244268931321194 + }, + { + "x": 3.1285483297575434, + "y": 6.225182361182179, + "heading": 0.2217268345391298, + "angularVelocity": -0.8127993213134442, + "velocityX": 1.5036762373221262, + "velocityY": -2.0401875838938306, + "timestamp": 0.5826965479245771 + }, + { + "x": 3.225113873395399, + "y": 6.094499287941398, + "heading": 0.1730380747769639, + "angularVelocity": -0.8355765953236391, + "velocityX": 1.6572183923484294, + "velocityY": -2.242729491126068, + "timestamp": 0.6409662027170349 + }, + { + "x": 3.330715130826902, + "y": 5.952078350373372, + "heading": 0.12484860508535624, + "angularVelocity": -0.8270079831989134, + "velocityX": 1.8122856194639898, + "velocityY": -2.4441699212959707, + "timestamp": 0.6992358575094926 + }, + { + "x": 3.4454857152688785, + "y": 5.798056975551327, + "heading": 0.08033839295880485, + "angularVelocity": -0.7638660686267251, + "velocityX": 1.9696458619973118, + "velocityY": -2.643251884203387, + "timestamp": 0.7575055123019503 + }, + { + "x": 3.569612354139756, + "y": 5.632836026447166, + "heading": 0.04630815492886413, + "angularVelocity": -0.5840130364792431, + "velocityX": 2.1302106441677915, + "velocityY": -2.8354544006247413, + "timestamp": 0.815775167094408 + }, + { + "x": 3.7020237011319064, + "y": 5.458025154952382, + "heading": 0.04313214682309951, + "angularVelocity": -0.05450535303627194, + "velocityX": 2.272389418879637, + "velocityY": -3.0000327291695483, + "timestamp": 0.8740448218868657 + }, + { + "x": 3.846567317399324, + "y": 5.291677583037812, + "heading": 0.04313213729822128, + "angularVelocity": -1.634620671894734e-7, + "velocityX": 2.480598465569185, + "velocityY": -2.8547890408319527, + "timestamp": 0.9323144766793234 + }, + { + "x": 4.001959808565688, + "y": 5.135416438368326, + "heading": 0.043132128200091754, + "angularVelocity": -1.5613838046572486e-7, + "velocityX": 2.6667824225119015, + "velocityY": -2.6816898989027482, + "timestamp": 0.9905841314717811 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.043132118825415074, + "angularVelocity": -1.6088436959619284e-7, + "velocityX": 2.84091706777896, + "velocityY": -2.4964733877097585, + "timestamp": 1.0488537862642389 + }, + { + "x": 4.269351778492494, + "y": 4.907420759942678, + "heading": 0.043132109937133276, + "angularVelocity": -2.564268066942143e-7, + "velocityX": 2.938449360926388, + "velocityY": -2.3809038425071845, + "timestamp": 1.0835158479787248 + }, + { + "x": 4.374422315135501, + "y": 4.829031543571565, + "heading": 0.043132101433653904, + "angularVelocity": -2.4532526192426324e-7, + "velocityX": 3.0312835257314057, + "velocityY": -2.2615278057263164, + "timestamp": 1.1181779096932107 + }, + { + "x": 4.482542676912381, + "y": 4.754905472881388, + "heading": 0.04313209323726653, + "angularVelocity": -2.3646566200370356e-7, + "velocityX": 3.119270938568917, + "velocityY": -2.1385361119243003, + "timestamp": 1.1528399714076967 + }, + { + "x": 4.593539986069964, + "y": 4.685161050857923, + "heading": 0.04313208527987584, + "angularVelocity": -2.2957061125967145e-7, + "velocityX": 3.202270830623841, + "velocityY": -2.0121256086251895, + "timestamp": 1.1875020331221826 + }, + { + "x": 4.707236757335738, + "y": 4.619909757466469, + "heading": 0.043132077500472085, + "angularVelocity": -2.244356903685004e-7, + "velocityX": 3.2801502750269638, + "velocityY": -1.8824989098725307, + "timestamp": 1.2221640948366685 + }, + { + "x": 4.823451163522863, + "y": 4.559255830076377, + "heading": 0.0431320698430563, + "angularVelocity": -2.2091633918596145e-7, + "velocityX": 3.352784007610117, + "velocityY": -1.7498649644588975, + "timestamp": 1.2568261565511545 + }, + { + "x": 4.941997186751783, + "y": 4.50329577732449, + "heading": 0.043132062254893876, + "angularVelocity": -2.1891838085258928e-7, + "velocityX": 3.4200511269465594, + "velocityY": -1.6144467462101488, + "timestamp": 1.2914882182656404 + }, + { + "x": 5.061738167339899, + "y": 4.449940508624331, + "heading": 0.04313205468599839, + "angularVelocity": -2.1836252987087912e-7, + "velocityX": 3.454525630195652, + "velocityY": -1.5392987624235874, + "timestamp": 1.3261502799801264 + }, + { + "x": 5.18147938835374, + "y": 4.396585779495745, + "heading": 0.04313204711710543, + "angularVelocity": -2.1836245696155624e-7, + "velocityX": 3.4545325664744473, + "velocityY": -1.53928319579118, + "timestamp": 1.3608123416946123 + }, + { + "x": 5.30122060959038, + "y": 4.343231050867178, + "heading": 0.043132039548212486, + "angularVelocity": -2.1836245655235923e-7, + "velocityX": 3.454532572902229, + "velocityY": -1.539283181365645, + "timestamp": 1.3954744034090982 + }, + { + "x": 5.420963132245964, + "y": 4.289879243040779, + "heading": 0.043132031979322576, + "angularVelocity": -2.1836236883930228e-7, + "velocityX": 3.4545701188207345, + "velocityY": -1.5391989162636184, + "timestamp": 1.4301364651235842 + }, + { + "x": 5.54252534628876, + "y": 4.240814844116505, + "heading": 0.04313202440333319, + "angularVelocity": -2.185671886990838e-7, + "velocityX": 3.507068190118379, + "velocityY": -1.4155072288665276, + "timestamp": 1.46479852683807 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.043132016771197046, + "angularVelocity": -2.201870219655876e-7, + "velocityX": 3.560849425193226, + "velocityY": -1.27418183966834, + "timestamp": 1.499460588552556 + }, + { + "x": 5.795862778757906, + "y": 4.156160077246944, + "heading": 0.04313200930767993, + "angularVelocity": -2.0743572849750813e-7, + "velocityX": 3.6106560546560362, + "velocityY": -1.1253226214863992, + "timestamp": 1.5354404902575611 + }, + { + "x": 5.927341987568784, + "y": 4.121096456952301, + "heading": 0.043132001920743686, + "angularVelocity": -2.053072936698873e-7, + "velocityX": 3.65424035587593, + "velocityY": -0.9745335210230772, + "timestamp": 1.5714203919625662 + }, + { + "x": 6.059882853366221, + "y": 4.090288037380679, + "heading": 0.04313199455852331, + "angularVelocity": -2.0462035814218294e-7, + "velocityX": 3.683747301038337, + "velocityY": -0.8562674746645255, + "timestamp": 1.6074002936675713 + }, + { + "x": 6.1932695032956575, + "y": 4.063376303154627, + "heading": 0.04313198718639407, + "angularVelocity": -2.0489575822100242e-7, + "velocityX": 3.70725442840443, + "velocityY": -0.7479657517326737, + "timestamp": 1.6433801953725764 + }, + { + "x": 6.327657938761633, + "y": 4.042022422624805, + "heading": 0.0431319797584895, + "angularVelocity": -2.0644593838934648e-7, + "velocityX": 3.7350973487312924, + "velocityY": -0.593494687809322, + "timestamp": 1.6793600970775815 + }, + { + "x": 6.462816723725205, + "y": 4.02626353633582, + "heading": 0.04313197070248067, + "angularVelocity": -2.5169631946321463e-7, + "velocityX": 3.7565078990966625, + "velocityY": -0.437991365796106, + "timestamp": 1.7153399987825866 + }, + { + "x": 6.597942968053082, + "y": 4.016131413139355, + "heading": 0.041078551168148146, + "angularVelocity": -0.057071293611868974, + "velocityX": 3.7556034876292976, + "velocityY": -0.28160508273582235, + "timestamp": 1.7513199004875917 + }, + { + "x": 6.728400948341844, + "y": 4.008819261956334, + "heading": 0.03307669986160184, + "angularVelocity": -0.2223978089810408, + "velocityX": 3.625857050927263, + "velocityY": -0.20322877041112014, + "timestamp": 1.7872998021925968 + }, + { + "x": 6.853599758247463, + "y": 4.003544549173221, + "heading": 0.023873515901943563, + "angularVelocity": -0.25578680106227425, + "velocityX": 3.479687380252149, + "velocityY": -0.146601645172889, + "timestamp": 1.8232797038976019 + }, + { + "x": 6.973435569834727, + "y": 4.000040202670832, + "heading": 0.014893371807540221, + "angularVelocity": -0.24958778842784254, + "velocityX": 3.3306319892084315, + "velocityY": -0.09739733396497403, + "timestamp": 1.859259605602607 + }, + { + "x": 7.087871539921083, + "y": 3.9981763906039123, + "heading": 0.00680556839180941, + "angularVelocity": -0.22478670125454425, + "velocityX": 3.1805526047459085, + "velocityY": -0.05180147745263738, + "timestamp": 1.895239507307612 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 2.7190146206752155e-27, + "angularVelocity": -0.18914916576502855, + "velocityX": 3.0299787445803052, + "velocityY": -0.008330941864390906, + "timestamp": 1.9312194090126171 + }, + { + "x": 7.373371859473657, + "y": 4.00211797408274, + "heading": -0.007235572451152828, + "angularVelocity": -0.11324239038273202, + "velocityX": 2.7620815980432543, + "velocityY": 0.06638014407023476, + "timestamp": 1.9951139667546283 + }, + { + "x": 7.5322864788781265, + "y": 4.0091810283968865, + "heading": -0.011195506650135086, + "angularVelocity": -0.061976079636882384, + "velocityX": 2.4871385767489596, + "velocityY": 0.1105423460737418, + "timestamp": 2.0590085244966394 + }, + { + "x": 7.673469429892467, + "y": 4.017826933148271, + "heading": -0.012870709573510192, + "angularVelocity": -0.02621824115504676, + "velocityX": 2.209624043168106, + "velocityY": 0.13531519830364583, + "timestamp": 2.1229030822386505 + }, + { + "x": 7.796860934793418, + "y": 4.0272116407494, + "heading": -0.012933719915114673, + "angularVelocity": -0.0009861613231427384, + "velocityX": 1.9311739412795161, + "velocityY": 0.14687804302553767, + "timestamp": 2.1867976399806617 + }, + { + "x": 7.9024438065227125, + "y": 4.036727062241868, + "heading": -0.011868644520280696, + "angularVelocity": 0.016669266248535024, + "velocityX": 1.652454848433405, + "velocityY": 0.14892381806427465, + "timestamp": 2.250692197722673 + }, + { + "x": 7.990219348757821, + "y": 4.045915777689649, + "heading": -0.01003983054789743, + "angularVelocity": 0.02862237469061955, + "velocityX": 1.3737561591633758, + "velocityY": 0.14381061192852837, + "timestamp": 2.314586755464684 + }, + { + "x": 8.060197028820047, + "y": 4.054421881048936, + "heading": -0.007731072178165808, + "angularVelocity": 0.03613388137145833, + "velocityX": 1.095205640905745, + "velocityY": 0.13312719674235798, + "timestamp": 2.378481313206695 + }, + { + "x": 8.112389742702678, + "y": 4.061960888187355, + "heading": -0.005169493803012325, + "angularVelocity": 0.04009071297584442, + "velocityX": 0.8168569550691795, + "velocityY": 0.11799138150167889, + "timestamp": 2.4423758709487062 + }, + { + "x": 8.146811548767705, + "y": 4.068300369491041, + "heading": -0.0025408878888210154, + "angularVelocity": 0.04113974659320609, + "velocityX": 0.5387282936367062, + "velocityY": 0.09921786029546131, + "timestamp": 2.5062704286907174 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -6.2580090272333915e-27, + "angularVelocity": 0.039766890618140496, + "velocityX": 0.26082055062629916, + "velocityY": 0.07741796101811332, + "timestamp": 2.5701649864327285 + }, + { + "x": 8.160341709324497, + "y": 4.076804968421738, + "heading": 0.002530252207813134, + "angularVelocity": 0.03600456006800818, + "velocityX": -0.044607809890755405, + "velocityY": 0.050629212451328946, + "timestamp": 2.640440868238638 + }, + { + "x": 8.135735380743824, + "y": 4.078564680386853, + "heading": 0.0047972294947691944, + "angularVelocity": 0.03225825459176822, + "velocityX": -0.3501390227821203, + "velocityY": 0.025040055277777168, + "timestamp": 2.710716750044548 + }, + { + "x": 8.089649471280776, + "y": 4.078625632726097, + "heading": 0.006802259736664781, + "angularVelocity": 0.02853084430065403, + "velocityX": -0.6557855736386108, + "velocityY": 0.0008673294119977214, + "timestamp": 2.7809926318504576 + }, + { + "x": 8.022074877026332, + "y": 4.077107138786556, + "heading": 0.008546934575106793, + "angularVelocity": 0.024826082485318403, + "velocityX": -0.9615616697784329, + "velocityY": -0.02160761132439043, + "timestamp": 2.8512685136563674 + }, + { + "x": 7.933001389840782, + "y": 4.074154809915227, + "heading": 0.010033199259775565, + "angularVelocity": 0.02114900085883774, + "velocityX": -1.2674830239990063, + "velocityY": -0.04201055604656103, + "timestamp": 2.921544395462278 + }, + { + "x": 7.822417663549702, + "y": 4.069950298090805, + "heading": 0.0112634877661819, + "angularVelocity": 0.017506553810369533, + "velocityX": -1.573565830116407, + "velocityY": -0.05982865979589635, + "timestamp": 2.9918202772681886 + }, + { + "x": 7.690311402375729, + "y": 4.064726507930881, + "heading": 0.012240935764081735, + "angularVelocity": 0.013908726191431891, + "velocityX": -1.8798236006319884, + "velocityY": -0.07433261633559884, + "timestamp": 3.062096159074099 + }, + { + "x": 7.536670182325497, + "y": 4.058792719262719, + "heading": 0.012969736698331166, + "angularVelocity": 0.010370569753393549, + "velocityX": -2.186258160012313, + "velocityY": -0.08443563446915123, + "timestamp": 3.1323720408800098 + }, + { + "x": 7.361484114735934, + "y": 4.052579188685834, + "heading": 0.013455784581129835, + "angularVelocity": 0.006916282945279149, + "velocityX": -2.4928334314380707, + "velocityY": -0.08841625913776113, + "timestamp": 3.2026479226859204 + }, + { + "x": 7.1647543643759946, + "y": 4.04672437363507, + "heading": 0.013707961220853816, + "angularVelocity": 0.0035883810098669257, + "velocityX": -2.7993921286291723, + "velocityY": -0.08331186888461464, + "timestamp": 3.272923804491831 + }, + { + "x": 6.946523514994072, + "y": 4.042270292544886, + "heading": 0.013741127121785043, + "angularVelocity": 0.000471938595133185, + "velocityX": -3.1053448747130243, + "velocityY": -0.06337993883142518, + "timestamp": 3.3431996862977416 + }, + { + "x": 6.707013005959754, + "y": 4.041197936309267, + "heading": 0.013584877310938924, + "angularVelocity": -0.0022233774494306753, + "velocityX": -3.408146619857499, + "velocityY": -0.015259235573605845, + "timestamp": 3.413475568103652 + }, + { + "x": 6.44761461575628, + "y": 4.04841614446901, + "heading": 0.013321799367880745, + "angularVelocity": -0.00374350255447176, + "velocityX": -3.6911438681038233, + "velocityY": 0.10271245232721735, + "timestamp": 3.4837514499095628 + }, + { + "x": 6.183335125687841, + "y": 4.076620833220385, + "heading": 0.013321798780147703, + "angularVelocity": -8.363225426855376e-9, + "velocityX": -3.7606001273428835, + "velocityY": 0.4013423670623127, + "timestamp": 3.5540273317154734 + }, + { + "x": 5.922208761336389, + "y": 4.126140293981494, + "heading": 0.013321798506864218, + "angularVelocity": -3.888723659827553e-9, + "velocityX": -3.715732305894624, + "velocityY": 0.70464374816203, + "timestamp": 3.624303213521384 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.013321798215407589, + "angularVelocity": -4.14732085648961e-9, + "velocityX": -3.6464435013896566, + "velocityY": 1.0033140639584306, + "timestamp": 3.6945790953272946 + }, + { + "x": 5.541094042237222, + "y": 4.236435082517705, + "heading": 0.013321797912516515, + "angularVelocity": -8.741536718768107e-9, + "velocityX": -3.6034342058048336, + "velocityY": 1.1482373727249326, + "timestamp": 3.729228732160526 + }, + { + "x": 5.417926089652793, + "y": 4.28117906555146, + "heading": 0.013321797636094018, + "angularVelocity": -7.977644916262235e-9, + "velocityX": -3.554667922704023, + "velocityY": 1.2913261760609271, + "timestamp": 3.7638783689937574 + }, + { + "x": 5.2966446591636975, + "y": 4.330809533782675, + "heading": 0.013321797380978714, + "angularVelocity": -7.362712230518396e-9, + "velocityX": -3.50022226994255, + "velocityY": 1.4323517579733251, + "timestamp": 3.798528005826989 + }, + { + "x": 5.177443524299453, + "y": 4.385247189609289, + "heading": 0.013321797143113006, + "angularVelocity": -6.86488311115801e-9, + "velocityX": -3.440184248913247, + "velocityY": 1.5710887848153525, + "timestamp": 3.83317764266022 + }, + { + "x": 5.060513134161878, + "y": 4.444405054072411, + "heading": 0.013321796919246185, + "angularVelocity": -6.460870636805561e-9, + "velocityX": -3.374649803701036, + "velocityY": 1.7073155700837472, + "timestamp": 3.8678272794934516 + }, + { + "x": 4.946040308560799, + "y": 4.508188605110022, + "heading": 0.013321796706722, + "angularVelocity": -6.133518370810969e-9, + "velocityX": -3.3037236768754394, + "velocityY": 1.8408144173228282, + "timestamp": 3.902476916326683 + }, + { + "x": 4.83420793799034, + "y": 4.576495926647655, + "heading": 0.013321796503327139, + "angularVelocity": -5.870043129182345e-9, + "velocityX": -3.2275192697894797, + "velocityY": 1.9713719328833563, + "timestamp": 3.9371265531599144 + }, + { + "x": 4.725194685566166, + "y": 4.649217863919521, + "heading": 0.01332179630717999, + "angularVelocity": -5.660871666352306e-9, + "velocityX": -3.146158585986232, + "velocityY": 2.098779205735374, + "timestamp": 3.971776189993146 + }, + { + "x": 4.61917463890979, + "y": 4.726238115287908, + "heading": 0.013321796115042023, + "angularVelocity": -5.545165384203739e-9, + "velocityX": -3.0597736757430263, + "velocityY": 2.2228299747869245, + "timestamp": 4.006425826826377 + }, + { + "x": 4.5101613609880875, + "y": 4.798960014309373, + "heading": 0.013321795918491775, + "angularVelocity": -5.6725052693856055e-9, + "velocityX": -3.146159321853369, + "velocityY": 2.098778101816108, + "timestamp": 4.041075463659609 + }, + { + "x": 4.398328964470022, + "y": 4.867267293334104, + "heading": 0.01332179571466789, + "angularVelocity": -5.882424857258076e-9, + "velocityX": -3.2275200186459836, + "velocityY": 1.971370705946904, + "timestamp": 4.07572510049284 + }, + { + "x": 4.283856115908692, + "y": 4.93105079879567, + "heading": 0.013321794090551628, + "angularVelocity": -4.687253345471844e-8, + "velocityX": -3.3037243395158984, + "velocityY": 1.8408131019830805, + "timestamp": 4.1103747373260715 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.012938317898929116, + "angularVelocity": -0.01106724995324455, + "velocityX": -3.3581030320620826, + "velocityY": 1.6997868507459994, + "timestamp": 4.145024374159303 + }, + { + "x": 3.941212702918835, + "y": 5.086810099235691, + "heading": 0.011498426607566257, + "angularVelocity": -0.020926455047297343, + "velocityX": -3.2887006266835073, + "velocityY": 1.4077344931260583, + "timestamp": 4.21383159870395 + }, + { + "x": 3.730319373177593, + "y": 5.1698990492176655, + "heading": 0.009891204024216577, + "angularVelocity": -0.02335834055196905, + "velocityX": -3.0649881772864958, + "velocityY": 1.2075614229732525, + "timestamp": 4.282638823248598 + }, + { + "x": 3.537102784143612, + "y": 5.242302243244861, + "heading": 0.008284119125689052, + "angularVelocity": -0.023356339529212095, + "velocityX": -2.8080857833265287, + "velocityY": 1.0522615104205277, + "timestamp": 4.351446047793245 + }, + { + "x": 3.3623897849801216, + "y": 5.305525598269376, + "heading": 0.006750375983753274, + "angularVelocity": -0.02229043755340787, + "velocityX": -2.5391664947933337, + "velocityY": 0.9188476274535745, + "timestamp": 4.4202532723378924 + }, + { + "x": 3.2065870996303723, + "y": 5.360439206716265, + "heading": 0.005330250476746595, + "angularVelocity": -0.020639191834938647, + "velocityX": -2.2643361417470387, + "velocityY": 0.7980791088478895, + "timestamp": 4.48906049688254 + }, + { + "x": 3.0699322051681737, + "y": 5.407605971353294, + "heading": 0.004049089893736221, + "angularVelocity": -0.01861956490018075, + "velocityX": -1.98605444946448, + "velocityY": 0.6854914574620572, + "timestamp": 4.557867721427187 + }, + { + "x": 2.9525794175973394, + "y": 5.4474188354837905, + "heading": 0.002924278144664975, + "angularVelocity": -0.01634729138567971, + "velocityX": -1.7055300275145737, + "velocityY": 0.5786145916213083, + "timestamp": 4.626674945971835 + }, + { + "x": 2.8546365555421396, + "y": 5.480167300776495, + "heading": 0.001968466770060391, + "angularVelocity": -0.013891148508459003, + "velocityX": -1.4234386389418605, + "velocityY": 0.47594515705912777, + "timestamp": 4.695482170516482 + }, + { + "x": 2.776182983656432, + "y": 5.506073375448153, + "heading": 0.0011912715035962884, + "angularVelocity": -0.011295256735138296, + "velocityX": -1.140193815473562, + "velocityY": 0.3765022473017808, + "timestamp": 4.764289395061129 + }, + { + "x": 2.717279457569953, + "y": 5.525312645366718, + "heading": 0.0006002466315783451, + "angularVelocity": -0.008589575817499225, + "velocityX": -0.8560660087118843, + "velocityY": 0.2796111897534987, + "timestamp": 4.833096619605777 + }, + { + "x": 2.6779739270458034, + "y": 5.538027422315637, + "heading": 0.0002014837935087551, + "angularVelocity": -0.0057953629245841975, + "velocityX": -0.5712413308960773, + "velocityY": 0.18478840024522195, + "timestamp": 4.901903844150424 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 2.777517455024623e-28, + "angularVelocity": -0.0029282360223382743, + "velocityX": -0.28585310661942626, + "velocityY": 0.0916755910664572, + "timestamp": 4.9707110686950715 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 1.7679062604492033e-28, + "angularVelocity": -4.39985022540959e-29, + "velocityX": -3.674462511689619e-27, + "velocityY": -3.4538031779033175e-27, + "timestamp": 5.039518293239719 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj index e8bddc0e..3fe799d8 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.5.traj @@ -1,382 +1,383 @@ { - "samples": [ - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 7.813037085898689e-36, - "angularVelocity": 1.6441710918960667e-34, - "velocityX": -8.361910991338164e-36, - "velocityY": -1.503965302427871e-35, - "timestamp": 0 - }, - { - "x": 2.6750735555579293, - "y": 5.538716972759925, - "heading": 1.350818917618456e-19, - "angularVelocity": 2.121700407628309e-18, - "velocityX": 0.2633772294028884, - "velocityY": -0.088246807749562, - "timestamp": 0.06366680765869503 - }, - { - "x": 2.7085920650608806, - "y": 5.52742596598787, - "heading": 3.9875003320592027e-19, - "angularVelocity": 4.14137524936932e-18, - "velocityX": 0.5264675697678677, - "velocityY": -0.17734526336837014, - "timestamp": 0.12733361531739007 - }, - { - "x": 2.7588390755328, - "y": 5.51039889888726, - "heading": 7.83389944204561e-19, - "angularVelocity": 6.041451191657363e-18, - "velocityX": 0.7892183120172117, - "velocityY": -0.2674402522565467, - "timestamp": 0.1910004229760851 - }, - { - "x": 2.8257885982701296, - "y": 5.487560532002736, - "heading": 1.2799519248933233e-18, - "angularVelocity": 7.799385566035206e-18, - "velocityX": 1.0515608556381997, - "velocityY": -0.35871701007777557, - "timestamp": 0.25466723063478014 - }, - { - "x": 2.9094088162269203, - "y": 5.4588202139112365, - "heading": 9.407556731948414e-19, - "angularVelocity": -5.3276780189270054e-18, - "velocityX": 1.313403656188678, - "velocityY": -0.45141760908715395, - "timestamp": 0.3183340382934752 - }, - { - "x": 3.0096598962036025, - "y": 5.424066617313392, - "heading": 6.890803629951045e-19, - "angularVelocity": -3.9530065893820216e-18, - "velocityX": 1.5746208057754196, - "velocityY": -0.5458668005493699, - "timestamp": 0.3820008459521702 - }, - { - "x": 3.126490562172417, - "y": 5.38315977794949, - "heading": 5.07923477935949e-19, - "angularVelocity": -2.8453897991917994e-18, - "velocityX": 1.8350325745107396, - "velocityY": -0.6425143786570174, - "timestamp": 0.44566765361086524 - }, - { - "x": 3.259832420061777, - "y": 5.335918462965804, - "heading": 3.751534220684201e-19, - "angularVelocity": -2.0853889294918685e-18, - "velocityX": 2.0943700931917317, - "velocityY": -0.7420085397863481, - "timestamp": 0.5093344612695603 - }, - { - "x": 3.409589860765725, - "y": 5.282098877479992, - "heading": 4.172337353071023e-18, - "angularVelocity": 5.964150034596624e-17, - "velocityX": 2.3522059014921575, - "velocityY": -0.8453319314253599, - "timestamp": 0.5730012689282553 - }, - { - "x": 3.575620301881546, - "y": 5.221355824269489, - "heading": 4.9261514224901164e-18, - "angularVelocity": 1.1839985341500765e-17, - "velocityX": 2.6078022005732984, - "velocityY": -0.9540772569615101, - "timestamp": 0.6366680765869503 - }, - { - "x": 3.7576900306489667, - "y": 5.153163870724839, - "heading": 5.589206376297959e-18, - "angularVelocity": 1.041445265109494e-17, - "velocityX": 2.8597276267323055, - "velocityY": -1.071075432432765, - "timestamp": 0.7003348842456454 - }, - { - "x": 3.9553538414525153, - "y": 5.076631094268888, - "heading": 6.855394728324636e-18, - "angularVelocity": 1.9887731120656582e-17, - "velocityX": 3.104660310018777, - "velocityY": -1.2020828320186703, - "timestamp": 0.7640016919043404 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 8.923179883172485e-18, - "angularVelocity": 3.2478228937327026e-17, - "velocityX": 3.332116557247906, - "velocityY": -1.3615147608100575, - "timestamp": 0.8276684995630355 - }, - { - "x": 4.285701077560922, - "y": 4.938388492372474, - "heading": 1.0772342870449973e-17, - "angularVelocity": 5.3398862490245696e-17, - "velocityX": 3.4133567656946298, - "velocityY": -1.4888942599373933, - "timestamp": 0.8622977575854378 - }, - { - "x": 4.403715149774036, - "y": 4.881600772971698, - "heading": 9.945107603157448e-18, - "angularVelocity": -2.3888333580736016e-17, - "velocityX": 3.4079295645540313, - "velocityY": -1.6398768741750054, - "timestamp": 0.8969270156078402 - }, - { - "x": 4.519367006147829, - "y": 4.8201449517293415, - "heading": 1.0140262228054944e-17, - "angularVelocity": 5.6355416212281845e-18, - "velocityX": 3.3397151131270646, - "velocityY": -1.7746791225673952, - "timestamp": 0.9315562736302425 - }, - { - "x": 4.6324721141561005, - "y": 4.75411916510052, - "heading": 1.0958701743566334e-17, - "angularVelocity": 2.3634335883891303e-17, - "velocityX": 3.2661718577713317, - "velocityY": -1.906647453610143, - "timestamp": 0.9661855316526449 - }, - { - "x": 4.742850082889288, - "y": 4.683628924272634, - "heading": 1.1951109976942982e-17, - "angularVelocity": 2.8658085389373404e-17, - "velocityX": 3.1874193972560034, - "velocityY": -2.0355689048343186, - "timestamp": 1.0008147896750472 - }, - { - "x": 4.850325220314879, - "y": 4.608787325463072, - "heading": 1.1732443750037932e-17, - "angularVelocity": -6.3144935638988695e-18, - "velocityX": 3.1035934225348796, - "velocityY": -2.1612244409379597, - "timestamp": 1.0354440476974496 - }, - { - "x": 4.959315849281407, - "y": 4.53617043444409, - "heading": 1.1207275250284524e-17, - "angularVelocity": -1.516545631482102e-17, - "velocityX": 3.147356749486827, - "velocityY": -2.096980852780761, - "timestamp": 1.070073305719852 - }, - { - "x": 5.0711194796065495, - "y": 4.467963995966005, - "heading": 1.0229768329797992e-17, - "angularVelocity": -2.822777547974504e-17, - "velocityX": 3.228588676454311, - "velocityY": -1.969618824462288, - "timestamp": 1.1047025637422543 - }, - { - "x": 5.1855579490393096, - "y": 4.404277210647011, - "heading": 8.869711544920542e-18, - "angularVelocity": -3.9274788503918017e-17, - "velocityX": 3.3046757559381783, - "velocityY": -1.8391033754691917, - "timestamp": 1.1393318217646566 - }, - { - "x": 5.302448702578612, - "y": 4.345211810292891, - "heading": 8.222829686597657e-18, - "angularVelocity": -1.8680211337603908e-17, - "velocityX": 3.3754911371096723, - "velocityY": -1.705650185051869, - "timestamp": 1.173961079787059 - }, - { - "x": 5.421605230513571, - "y": 4.290862095789443, - "heading": 7.194520598849692e-18, - "angularVelocity": -2.969480567798286e-17, - "velocityX": 3.4409206185669707, - "velocityY": -1.5694738382292037, - "timestamp": 1.2085903378094613 - }, - { - "x": 5.542837392691082, - "y": 4.241314822773421, - "heading": 5.567604077424851e-18, - "angularVelocity": -4.698098123766793e-17, - "velocityX": 3.500859362885684, - "velocityY": -1.4307922215361755, - "timestamp": 1.2432195958318637 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 4.441119305733273e-18, - "angularVelocity": -3.252985585087723e-17, - "velocityX": 3.5552114934161922, - "velocityY": -1.2898268911821025, - "timestamp": 1.277848853854266 - }, - { - "x": 5.934272395491136, - "y": 4.124337934678793, - "heading": 1.9097100466976255e-18, - "angularVelocity": -3.445086702659097e-17, - "velocityX": 3.6516733021145136, - "velocityY": -0.9841085377661556, - "timestamp": 1.3513276797160874 - }, - { - "x": 6.207753017493673, - "y": 4.075010140740357, - "heading": -4.66846161540337e-18, - "angularVelocity": -8.952472477542546e-17, - "velocityX": 3.721897006313398, - "velocityY": -0.6713198443208224, - "timestamp": 1.4248065055779087 - }, - { - "x": 6.4844286250858945, - "y": 4.049019985828209, - "heading": -6.502926410881733e-18, - "angularVelocity": -2.4965896963679208e-17, - "velocityX": 3.7653787243758563, - "velocityY": -0.3537094476852885, - "timestamp": 1.49828533143973 - }, - { - "x": 6.743277363747609, - "y": 4.038426394199003, - "heading": -5.737032029312171e-18, - "angularVelocity": 1.0423334512854795e-17, - "velocityX": 3.5227663973358068, - "velocityY": -0.1441720319419282, - "timestamp": 1.5717641573015513 - }, - { - "x": 6.978672822837657, - "y": 4.030025299692622, - "heading": -4.2593802240854646e-18, - "angularVelocity": 2.010989952405427e-17, - "velocityX": 3.2035822065626656, - "velocityY": -0.11433354313770064, - "timestamp": 1.6452429831633726 - }, - { - "x": 7.19055269256627, - "y": 4.022995759457438, - "heading": -3.134299993682012e-18, - "angularVelocity": 1.5311625045821916e-17, - "velocityX": 2.883550019253943, - "velocityY": -0.09566756344750842, - "timestamp": 1.718721809025194 - }, - { - "x": 7.3789017577018114, - "y": 4.017045849522277, - "heading": -2.330563766445784e-18, - "angularVelocity": 1.0938337919929083e-17, - "velocityX": 2.5633107623376485, - "velocityY": -0.080974482993913, - "timestamp": 1.7922006348870152 - }, - { - "x": 7.54371362899692, - "y": 4.012025997027571, - "heading": -1.6314906834703783e-18, - "angularVelocity": 9.513939216857223e-18, - "velocityX": 2.242984551835937, - "velocityY": -0.06831699385270738, - "timestamp": 1.8656794607488365 - }, - { - "x": 7.684984887810538, - "y": 4.007845278835319, - "heading": -1.3889434062297666e-18, - "angularVelocity": 3.300913894524217e-18, - "velocityX": 1.9226118158077503, - "velocityY": -0.05689691068436309, - "timestamp": 1.9391582866106578 - }, - { - "x": 7.8027134339680275, - "y": 4.0044425825275765, - "heading": -1.0305851069387887e-18, - "angularVelocity": 4.877028110994567e-18, - "velocityX": 1.6022104977409415, - "velocityY": -0.04630852858402237, - "timestamp": 2.012637112472479 - }, - { - "x": 7.896897856926731, - "y": 4.001774011196325, - "heading": -6.085636422043956e-19, - "angularVelocity": 5.743443227141563e-18, - "velocityX": 1.2817899830873647, - "velocityY": -0.036317555431137405, - "timestamp": 2.0861159383343004 - }, - { - "x": 7.967537148527336, - "y": 3.999806507404438, - "heading": -1.6215491216433702e-19, - "angularVelocity": 6.07533836862799e-18, - "velocityX": 0.9613557480279205, - "velocityY": -0.026776472933673492, - "timestamp": 2.1595947641961217 - }, - { - "x": 8.014630554462236, - "y": 3.9985142796609896, - "heading": 9.722147651876556e-20, - "angularVelocity": 3.5299473779135435e-18, - "velocityX": 0.6409112473226125, - "velocityY": -0.017586396193627746, - "timestamp": 2.233073590057943 - }, - { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 9.32148398835055e-44, - "angularVelocity": -1.3231223468593855e-18, - "velocityX": 0.32045879198477595, - "velocityY": -0.00867781322019328, - "timestamp": 2.3065524159197643 - }, - { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": -1.3950188036384555e-43, - "angularVelocity": -3.1671262842054474e-42, - "velocityX": -2.982658307493247e-43, - "velocityY": -2.0280554768402976e-41, - "timestamp": 2.3800312417815856 - } - ] + "samples": [ + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 1.7679062604492033e-28, + "angularVelocity": -4.39985022540959e-29, + "velocityX": -3.674462511689619e-27, + "velocityY": -3.4538031779033175e-27, + "timestamp": 0 + }, + { + "x": 2.6750735377327333, + "y": 5.538716932149931, + "heading": -1.742535482636501e-18, + "angularVelocity": -2.7369611075472657e-17, + "velocityX": 0.2633769919204766, + "velocityY": -0.08824745983951204, + "timestamp": 0.06366679738654035 + }, + { + "x": 2.7085920132371135, + "y": 5.52742584903886, + "heading": -8.352558789144994e-18, + "angularVelocity": -1.0382214243405793e-16, + "velocityX": 0.526467120701534, + "velocityY": -0.17734649102137234, + "timestamp": 0.1273335947730807 + }, + { + "x": 2.758838975509705, + "y": 5.510398675567148, + "heading": -1.408942364356953e-17, + "angularVelocity": -9.01076399315566e-17, + "velocityX": 0.7892176822956318, + "velocityY": -0.26744196615285204, + "timestamp": 0.19100039215962106 + }, + { + "x": 2.8257884382416396, + "y": 5.4875601790142925, + "heading": -1.8320735772591408e-17, + "angularVelocity": -6.646026347783005e-17, + "velocityX": 1.0515600828083749, + "velocityY": -0.35871910462523465, + "timestamp": 0.2546671895461614 + }, + { + "x": 2.9094085873516815, + "y": 5.458819716039827, + "heading": -2.1055365951456844e-17, + "angularVelocity": -4.295221828639974e-17, + "velocityX": 1.3134027867360427, + "velocityY": -0.4514199575639651, + "timestamp": 0.31833398693270176 + }, + { + "x": 3.0096595933993924, + "y": 5.424065969220243, + "heading": -2.2717586087398788e-17, + "angularVelocity": -2.6108116070725826e-17, + "velocityX": 1.5746198986428648, + "velocityY": -0.5458692481197496, + "timestamp": 0.3820007843192421 + }, + { + "x": 3.126490185266761, + "y": 5.383158986628498, + "heading": -2.29080313224255e-17, + "angularVelocity": -2.991280273573379e-18, + "velocityX": 1.83503170668463, + "velocityY": -0.642516731969203, + "timestamp": 0.44566758170578247 + }, + { + "x": 3.2598319755407235, + "y": 5.335917551231157, + "heading": -1.862517634661648e-17, + "angularVelocity": 6.726983532294288e-17, + "velocityX": 2.0943693690826515, + "velocityY": -0.7420105508138521, + "timestamp": 0.5093343790923228 + }, + { + "x": 3.4095893646013797, + "y": 5.282097889138944, + "heading": -1.2913797720128003e-17, + "angularVelocity": 8.970733350596672e-17, + "velocityX": 2.3522054698531947, + "velocityY": -0.8453332710526835, + "timestamp": 0.5730011764788632 + }, + { + "x": 3.575619784494897, + "y": 5.221354832224988, + "heading": -5.8099440020816116e-18, + "angularVelocity": 1.1157862511662809e-16, + "velocityX": 2.607802287988438, + "velocityY": -0.9540774690639187, + "timestamp": 0.6366679738654035 + }, + { + "x": 3.7576895466655613, + "y": 5.153162990546568, + "heading": 2.5414927254501756e-18, + "angularVelocity": 1.3117412953490323e-16, + "velocityX": 2.859728612784813, + "velocityY": -1.0710738481850424, + "timestamp": 0.7003347712519439 + }, + { + "x": 3.955353492285648, + "y": 5.076630508571263, + "heading": 1.2126107416115352e-17, + "angularVelocity": 1.5054337714518245e-16, + "velocityX": 3.1046629284650207, + "velocityY": -1.202078400624624, + "timestamp": 0.7640015686384842 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 1.991506320539766e-17, + "angularVelocity": 1.2233936853955328e-16, + "velocityX": 3.332122579144681, + "velocityY": -1.3615057810598787, + "timestamp": 0.8276683660250246 + }, + { + "x": 4.285701420717343, + "y": 4.93838888948332, + "heading": 2.4571932720973296e-17, + "angularVelocity": 1.3447790808928575e-16, + "velocityX": 3.4133671603880513, + "velocityY": -1.488883004107591, + "timestamp": 0.8622976191243996 + }, + { + "x": 4.403715683241863, + "y": 4.881601537504843, + "heading": 2.682063176544399e-17, + "angularVelocity": 6.493640038785633e-17, + "velocityX": 3.407935544721574, + "velocityY": -1.6398664971350723, + "timestamp": 0.8969268722237747 + }, + { + "x": 4.5193677097341265, + "y": 4.820146027732668, + "heading": 1.2264799155735384e-17, + "angularVelocity": -4.203334264259107e-16, + "velocityX": 3.339720500479131, + "velocityY": -1.7746703804386716, + "timestamp": 0.9315561253231497 + }, + { + "x": 4.632472949186873, + "y": 4.754120458704841, + "heading": 9.89855142264587e-19, + "angularVelocity": -3.255901587362041e-16, + "velocityX": 3.2661761178668676, + "velocityY": -1.90664144093305, + "timestamp": 0.9661853784225247 + }, + { + "x": 4.742850945772244, + "y": 4.683630254723833, + "heading": -1.0397818320257784e-17, + "angularVelocity": -3.288454830368281e-16, + "velocityX": 3.1874206546881325, + "velocityY": -2.035568130179495, + "timestamp": 1.0008146315218998 + }, + { + "x": 4.850325734395693, + "y": 4.6087881488265605, + "heading": -1.7363492612518904e-17, + "angularVelocity": -2.0115000090418594e-16, + "velocityX": 3.1035837912827686, + "velocityY": -2.161239391519611, + "timestamp": 1.0354438846212748 + }, + { + "x": 4.959315990041341, + "y": 4.536170691050833, + "heading": -2.5306246706392577e-17, + "angularVelocity": -2.29365446344471e-16, + "velocityX": 3.147346416420796, + "velocityY": -2.0969975173111073, + "timestamp": 1.0700731377206498 + }, + { + "x": 5.071119485305154, + "y": 4.467964024057391, + "heading": -3.478725650331535e-17, + "angularVelocity": -2.7378614749218265e-16, + "velocityX": 3.228585235233696, + "velocityY": -1.9696257033817701, + "timestamp": 1.1047023908200249 + }, + { + "x": 5.185557902331023, + "y": 4.404277136498651, + "heading": -4.439822175932155e-17, + "angularVelocity": -2.7753891278544145e-16, + "velocityX": 3.3046747123729983, + "velocityY": -1.839106589332955, + "timestamp": 1.1393316439194 + }, + { + "x": 5.302448643272519, + "y": 4.345211701858586, + "heading": -5.347328989806064e-17, + "angularVelocity": -2.62063640602752e-16, + "velocityX": 3.375491253191483, + "velocityY": -1.7056514176198097, + "timestamp": 1.173960897018775 + }, + { + "x": 5.421605180282958, + "y": 4.290861996168768, + "heading": -6.205824789945856e-17, + "angularVelocity": -2.4791057365365614e-16, + "velocityX": 3.440921369816929, + "velocityY": -1.5694738068375724, + "timestamp": 1.20859015011815 + }, + { + "x": 5.542837363816495, + "y": 4.241314761996146, + "heading": -6.772617298051042e-17, + "angularVelocity": -1.6367448251072389e-16, + "velocityX": 3.500860477286019, + "velocityY": -1.4307913032498076, + "timestamp": 1.243219403217525 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -7.009195303985871e-17, + "angularVelocity": -6.831738623783852e-17, + "velocityX": 3.555212832659327, + "velocityY": -1.2898253194640328, + "timestamp": 1.2778486563169 + }, + { + "x": 5.93427243169133, + "y": 4.124338107174554, + "heading": -5.732543809529257e-17, + "angularVelocity": 1.7374415360495528e-16, + "velocityX": 3.651674213100421, + "velocityY": -0.9841063029448727, + "timestamp": 1.3513274737612182 + }, + { + "x": 6.207753086191834, + "y": 4.075010553952499, + "heading": -4.033997496139901e-17, + "angularVelocity": 2.3116135676628954e-16, + "velocityX": 3.7218978749589717, + "velocityY": -0.6713166452282138, + "timestamp": 1.4248062912055364 + }, + { + "x": 6.484428721197197, + "y": 4.049020784065838, + "heading": -4.165967730952468e-17, + "angularVelocity": -1.796031011340145e-17, + "velocityX": 3.7653795288013057, + "velocityY": -0.35370424825299146, + "timestamp": 1.4982851086498545 + }, + { + "x": 6.743277525423136, + "y": 4.038427876582129, + "heading": -4.683972332322173e-17, + "angularVelocity": -7.04971336473422e-17, + "velocityX": 3.5227676931803376, + "velocityY": -0.1441627376724693, + "timestamp": 1.5717639260941727 + }, + { + "x": 6.9786729860111185, + "y": 4.030026934106214, + "heading": -5.042587996000917e-17, + "angularVelocity": -4.880531235316479e-17, + "velocityX": 3.2035825939409506, + "velocityY": -0.11433148719739977, + "timestamp": 1.6452427435384909 + }, + { + "x": 7.190552838965192, + "y": 4.022997315975448, + "heading": -4.990000307378321e-17, + "angularVelocity": 7.15685015890218e-18, + "velocityX": 2.8835501212936054, + "velocityY": -0.09566863451624368, + "timestamp": 1.718721560982809 + }, + { + "x": 7.37890188057172, + "y": 4.017047213862616, + "heading": -4.8467877227374554e-17, + "angularVelocity": 1.949032246701928e-17, + "velocityX": 2.563310735767617, + "velocityY": -0.08097710768603945, + "timestamp": 1.7922003784271272 + }, + { + "x": 7.5437137264303615, + "y": 4.012027115751869, + "heading": -4.529378389429077e-17, + "angularVelocity": 4.3197392712062346e-17, + "velocityX": 2.2429844626110818, + "velocityY": -0.06832034435708055, + "timestamp": 1.8656791958714454 + }, + { + "x": 7.684984960485698, + "y": 4.007846136006195, + "heading": -4.052616415520898e-17, + "angularVelocity": 6.488427420284795e-17, + "velocityX": 1.9226116991116777, + "velocityY": -0.05690047677812232, + "timestamp": 1.9391580133157635 + }, + { + "x": 7.802713484144924, + "y": 4.004443187628857, + "heading": -3.38523635240434e-17, + "angularVelocity": 9.082618451636533e-17, + "velocityX": 1.602210375098112, + "velocityY": -0.046311964395964134, + "timestamp": 2.0126368307600817 + }, + { + "x": 7.896897887923302, + "y": 4.001774392077607, + "heading": -2.5504868250570688e-17, + "angularVelocity": 1.1360410474589884e-16, + "velocityX": 1.2817898688931846, + "velocityY": -0.03632061108321556, + "timestamp": 2.0861156482044 + }, + { + "x": 7.967537164414649, + "y": 3.9998067058039015, + "heading": -1.5265763382971852e-17, + "angularVelocity": 1.393477089561816e-16, + "velocityX": 0.9613556525304345, + "velocityY": -0.026778959462655655, + "timestamp": 2.159594465648718 + }, + { + "x": 8.014630559872813, + "y": 3.998514348187002, + "heading": -8.61347718056724e-18, + "angularVelocity": 9.053338681616732e-17, + "velocityX": 0.640911178161685, + "velocityY": -0.017588165703384288, + "timestamp": 2.233073283093036 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 1.5743578769011647e-28, + "angularVelocity": 1.1722394943724176e-16, + "velocityX": 0.3204587550610229, + "velocityY": -0.008678746809770337, + "timestamp": 2.3065521005373544 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 7.635851844630149e-29, + "angularVelocity": -6.421919896967874e-29, + "velocityX": 1.2273507247615808e-27, + "velocityY": -1.4201614247601614e-26, + "timestamp": 2.3800309179816725 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFSprint.traj b/src/main/deploy/choreo/CenterLanePCBAFSprint.traj index c863df6d..1be7335b 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFSprint.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFSprint.traj @@ -1,1822 +1,1823 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -2.75917641303777e-30, - "velocityX": -8.847327306636262e-30, - "velocityY": 1.4420464788285476e-29, - "timestamp": 0 - }, - { - "x": 1.3060574436245773, - "y": 5.572382537817046, - "heading": -0.010694867587036427, - "angularVelocity": -0.14212340511596666, - "velocityX": 0.21372585011902087, - "velocityY": -0.24679899874441508, - "timestamp": 0.07525057240542533 - }, - { - "x": 1.3382236275279018, - "y": 5.53523931290218, - "heading": -0.032076067517629925, - "angularVelocity": -0.2841333859323569, - "velocityX": 0.42745434186951237, - "velocityY": -0.4935939186635594, - "timestamp": 0.15050114481085067 - }, - { - "x": 1.386473519841097, - "y": 5.479524946048311, - "heading": -0.0641215016247012, - "angularVelocity": -0.42584970563284796, - "velocityX": 0.6411897048771923, - "velocityY": -0.7403846253066342, - "timestamp": 0.22575171721627602 - }, - { - "x": 1.450807989654895, - "y": 5.405239856496701, - "heading": -0.10679801494850981, - "angularVelocity": -0.5671254312134459, - "velocityX": 0.8549366171751026, - "velocityY": -0.9871697606799944, - "timestamp": 0.30100228962170134 - }, - { - "x": 1.5312282961066404, - "y": 5.312384689270129, - "heading": -0.16006508743981135, - "angularVelocity": -0.7078626884752101, - "velocityX": 1.0687002620672836, - "velocityY": -1.23394632438782, - "timestamp": 0.37625286202712666 - }, - { - "x": 1.627736154340151, - "y": 5.200960485487107, - "heading": -0.22387960186124028, - "angularVelocity": -0.8480269635530943, - "velocityX": 1.2824866993939017, - "velocityY": -1.4807090527398261, - "timestamp": 0.451503434432552 - }, - { - "x": 1.7403339652666383, - "y": 5.0709689996393825, - "heading": -0.2982011188077779, - "angularVelocity": -0.9876538419890185, - "velocityX": 1.4963050422725384, - "velocityY": -1.7274484657660032, - "timestamp": 0.5267540068379774 - }, - { - "x": 1.8690262176186487, - "y": 4.922413993905078, - "heading": -0.382996017565801, - "angularVelocity": -1.1268339369356364, - "velocityX": 1.710183035621927, - "velocityY": -1.9741378834553056, - "timestamp": 0.6020045792434027 - }, - { - "x": 2.0218420905066052, - "y": 4.778614575286612, - "heading": -0.4624353832107095, - "angularVelocity": -1.055664602990153, - "velocityX": 2.030760272057439, - "velocityY": -1.9109411931313167, - "timestamp": 0.677255151648828 - }, - { - "x": 2.15857350503456, - "y": 4.653386666317764, - "heading": -0.5314189322497749, - "angularVelocity": -0.9167179309443209, - "velocityX": 1.8170149429960494, - "velocityY": -1.6641456000181232, - "timestamp": 0.7525057240542533 - }, - { - "x": 2.2792167348207513, - "y": 4.5467268950736175, - "heading": -0.589951951408673, - "angularVelocity": -0.7778415138506264, - "velocityX": 1.6032200942942694, - "velocityY": -1.4173948161921597, - "timestamp": 0.8277562964596786 - }, - { - "x": 2.38377026778444, - "y": 4.458633759569044, - "heading": -0.6380351777892487, - "angularVelocity": -0.6389748920543931, - "velocityX": 1.3894051516512624, - "velocityY": -1.1706639921478266, - "timestamp": 0.903006868865104 - }, - { - "x": 2.472233242714231, - "y": 4.389106342435906, - "heading": -0.6756676216200074, - "angularVelocity": -0.5000951172479533, - "velocityX": 1.1755787644266529, - "velocityY": -0.9239453589485842, - "timestamp": 0.9782574412705293 - }, - { - "x": 2.5446051200448103, - "y": 4.3381440386367105, - "heading": -0.7028476555095471, - "angularVelocity": -0.3611937161417901, - "velocityX": 0.9617452069540623, - "velocityY": -0.6772347660530061, - "timestamp": 1.0535080136759547 - }, - { - "x": 2.600885568086537, - "y": 4.305746455375921, - "heading": -0.7195734643408855, - "angularVelocity": -0.22226819406292112, - "velocityX": 0.7479072416796508, - "velocityY": -0.4305293929915994, - "timestamp": 1.12875858608138 - }, - { - "x": 2.641074414508583, - "y": 4.291913365901662, - "heading": -0.7258432162336076, - "angularVelocity": -0.08331832824324738, - "velocityX": 0.5340669863123017, - "velocityY": -0.18382703322097846, - "timestamp": 1.2040091584868053 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.05565540572186624, - "velocityX": 0.32022625146750794, - "velocityY": 0.06287422939745346, - "timestamp": 1.2792597308922307 - }, - { - "x": 2.6731092407857906, - "y": 4.320288302812448, - "heading": -0.7067988166038234, - "angularVelocity": 0.1958033723255861, - "velocityX": 0.10461638788158735, - "velocityY": 0.31161864341117435, - "timestamp": 1.3551332887134768 - }, - { - "x": 2.664687841052256, - "y": 4.3628053226693035, - "heading": -0.6813131694887641, - "angularVelocity": 0.33589629702821755, - "velocityX": -0.11099255097832775, - "velocityY": 0.5603667611869495, - "timestamp": 1.431006846534723 - }, - { - "x": 2.6399075701479586, - "y": 4.42419614745244, - "heading": -0.6452023149092349, - "angularVelocity": 0.47593464200079133, - "velocityX": -0.32659956402163576, - "velocityY": 0.8091201539132412, - "timestamp": 1.5068804043559691 - }, - { - "x": 2.598768648766779, - "y": 4.504461298133711, - "heading": -0.5984696942887661, - "angularVelocity": 0.6159276296230222, - "velocityX": -0.5422036683592814, - "velocityY": 1.057880412955572, - "timestamp": 1.5827539621772153 - }, - { - "x": 2.5412713665853555, - "y": 4.603601395355974, - "heading": -0.5411165139934166, - "angularVelocity": 0.7559047175548449, - "velocityX": -0.7578039548110096, - "velocityY": 1.3066488519598491, - "timestamp": 1.6586275199984615 - }, - { - "x": 2.4674160388824338, - "y": 4.721617069365857, - "heading": -0.47313875765168273, - "angularVelocity": 0.8959347405484323, - "velocityX": -0.9734000859460189, - "velocityY": 1.5554255975016502, - "timestamp": 1.7345010778197076 - }, - { - "x": 2.377202770872684, - "y": 4.858508647531332, - "heading": -0.39452213175531803, - "angularVelocity": 1.0361531494380563, - "velocityX": -1.1889948303725923, - "velocityY": 1.8042066576932412, - "timestamp": 1.8103746356409538 - }, - { - "x": 2.270629951391599, - "y": 5.014274677090094, - "heading": -0.305234926659776, - "angularVelocity": 1.1767894858012566, - "velocityX": -1.4046108096674554, - "velocityY": 2.0529685707303664, - "timestamp": 1.8862481934622 - }, - { - "x": 2.1562097285316435, - "y": 5.146459905330176, - "heading": -0.22910409225153178, - "angularVelocity": 1.0033908596890408, - "velocityX": -1.5080381906707807, - "velocityY": 1.7421778026344412, - "timestamp": 1.9621217512834461 - }, - { - "x": 2.0581334055736438, - "y": 5.259758402247803, - "heading": -0.16374394944584372, - "angularVelocity": 0.8614350596401947, - "velocityX": -1.2926284963034982, - "velocityY": 1.4932540422920335, - "timestamp": 2.0379953091046925 - }, - { - "x": 1.9764029763301112, - "y": 5.354172653968883, - "heading": -0.10920555144834718, - "angularVelocity": 0.7188063874223503, - "velocityX": -1.0771925238352613, - "velocityY": 1.2443630486526756, - "timestamp": 2.1138688669259387 - }, - { - "x": 1.9110185860607423, - "y": 5.429703503317112, - "heading": -0.06553422750381226, - "angularVelocity": 0.5755802838199697, - "velocityX": -0.8617546368678625, - "velocityY": 0.9954831632816045, - "timestamp": 2.189742424747185 - }, - { - "x": 1.8619801423257207, - "y": 5.486351407872208, - "heading": -0.032764704050183134, - "angularVelocity": 0.43189649194112756, - "velocityX": -0.6463179682397073, - "velocityY": 0.7466093087307788, - "timestamp": 2.265615982568431 - }, - { - "x": 1.8292876278534882, - "y": 5.524116629586748, - "heading": -0.010918068889347814, - "angularVelocity": 0.28793476658359446, - "velocityX": -0.43088152724695716, - "velocityY": 0.4977389066689233, - "timestamp": 2.341489540389677 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.394848489426687e-27, - "angularVelocity": 0.14389820647675833, - "velocityX": -0.21544309693644778, - "velocityY": 0.24886981095702526, - "timestamp": 2.4173630982109233 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.3925172511788446e-27, - "angularVelocity": -3.168744293663733e-29, - "velocityX": 8.48223659243716e-32, - "velocityY": -3.031681211954363e-28, - "timestamp": 2.4932366560321695 - }, - { - "x": 1.8507557561824508, - "y": 5.5430527114919315, - "heading": -1.9710234547097696e-21, - "angularVelocity": -2.1171281249858964e-20, - "velocityX": 0.40617567078743816, - "velocityY": 0.0005740544395108319, - "timestamp": 2.586335690074543 - }, - { - "x": 1.9263848800936068, - "y": 5.5431595993177165, - "heading": -2.0389636321585498e-20, - "angularVelocity": -1.9783892557349584e-19, - "velocityX": 0.8123513276919052, - "velocityY": 0.001148108859400643, - "timestamp": 2.6794347241169163 - }, - { - "x": 2.039828562987611, - "y": 5.543319931052194, - "heading": -2.352458064017618e-20, - "angularVelocity": -3.3673220682050347e-20, - "velocityX": 1.2185269596070256, - "velocityY": 0.001722163243972619, - "timestamp": 2.7725337581592897 - }, - { - "x": 2.1910867994360017, - "y": 5.54353370668769, - "heading": -3.364930009404432e-20, - "angularVelocity": -1.087521428982912e-19, - "velocityX": 1.6247025332136769, - "velocityY": 0.0022962175461363225, - "timestamp": 2.865632792201663 - }, - { - "x": 2.3801595622964817, - "y": 5.543800926185845, - "heading": -1.7618072508363382e-20, - "angularVelocity": 1.721954234066794e-19, - "velocityX": 2.030877815278129, - "velocityY": 0.0028702714362588678, - "timestamp": 2.9587318262440365 - }, - { - "x": 2.5314177987448723, - "y": 5.544014701821341, - "heading": -7.829457975228232e-21, - "angularVelocity": 1.0514195591633778e-19, - "velocityX": 1.6247025332136766, - "velocityY": 0.0022962175461363225, - "timestamp": 3.05183086028641 - }, - { - "x": 2.6448614816388765, - "y": 5.544175033555819, - "heading": 1.3151205701559364e-20, - "angularVelocity": 2.253585538517852e-19, - "velocityX": 1.2185269596070254, - "velocityY": 0.001722163243972619, - "timestamp": 3.1449298943287833 - }, - { - "x": 2.720490605550032, - "y": 5.544281921381604, - "heading": 9.829941746315052e-21, - "angularVelocity": -3.567452648039799e-20, - "velocityX": 0.8123513276919051, - "velocityY": 0.0011481088594006432, - "timestamp": 3.2380289283711567 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -1.6124701211579614e-34, - "angularVelocity": -1.0558586184512984e-19, - "velocityX": 0.40617567078743816, - "velocityY": 0.000574054439510832, - "timestamp": 3.33112796241353 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "angularVelocity": 1.724633516740329e-33, - "velocityX": 6.381177903729197e-38, - "velocityY": -6.411016173939856e-36, - "timestamp": 3.4242269964559036 - }, - { - "x": 2.7396220511907092, - "y": 5.562601673573336, - "heading": 0.003707886080781361, - "angularVelocity": 0.04789837119714626, - "velocityX": -0.24134799501070836, - "velocityY": 0.2359636718162648, - "timestamp": 3.5016385239903154 - }, - { - "x": 2.702380969511178, - "y": 5.599259386918254, - "heading": 0.011292704027819503, - "angularVelocity": 0.09798047123752293, - "velocityX": -0.4810792767650384, - "velocityY": 0.473543340539592, - "timestamp": 3.579050051524727 - }, - { - "x": 2.6467699433072402, - "y": 5.6544931206396525, - "heading": 0.023007945503191744, - "angularVelocity": 0.1513371696504047, - "velocityX": -0.7183817187849313, - "velocityY": 0.7135078647924299, - "timestamp": 3.656461579059139 - }, - { - "x": 2.573102880230356, - "y": 5.72860253774289, - "heading": 0.03927230490424404, - "angularVelocity": 0.21010255086133403, - "velocityX": -0.9516291103303324, - "velocityY": 0.9573434275701875, - "timestamp": 3.733873106593551 - }, - { - "x": 2.4820081686264115, - "y": 5.82215722498829, - "heading": 0.060903903359814444, - "angularVelocity": 0.2794363984867048, - "velocityX": -1.1767589983733415, - "velocityY": 1.208536896572829, - "timestamp": 3.8112846341279627 - }, - { - "x": 2.3753503305689, - "y": 5.936638406081269, - "heading": 0.09019255741494606, - "angularVelocity": 0.378350033748035, - "velocityX": -1.3778030411568696, - "velocityY": 1.4788647729769806, - "timestamp": 3.8886961616623745 - }, - { - "x": 2.2779168004384966, - "y": 6.0748767248289015, - "heading": 0.1437819345039841, - "angularVelocity": 0.6922661106928292, - "velocityX": -1.25864368310122, - "velocityY": 1.7857588288279342, - "timestamp": 3.9661076891967864 - }, - { - "x": 2.2032159820535764, - "y": 6.200217981312059, - "heading": 0.20003111930602646, - "angularVelocity": 0.7266254341388357, - "velocityX": -0.9649831331866353, - "velocityY": 1.6191549304777408, - "timestamp": 4.043519216731198 - }, - { - "x": 2.149623841782867, - "y": 6.310133396102646, - "heading": 0.25610236287269844, - "angularVelocity": 0.7243267941166335, - "velocityX": -0.6923018053982481, - "velocityY": 1.4198843284900597, - "timestamp": 4.12093074426561 - }, - { - "x": 2.1165539042639407, - "y": 6.4038530923195935, - "heading": 0.31105587872076873, - "angularVelocity": 0.7098880179523891, - "velocityX": -0.42719655033581183, - "velocityY": 1.2106684779639114, - "timestamp": 4.198342271800021 - }, - { - "x": 2.1037067095365876, - "y": 6.481006876335726, - "heading": 0.3644203290348082, - "angularVelocity": 0.6893605127520235, - "velocityX": -0.1659597108666065, - "velocityY": 0.9966704762651164, - "timestamp": 4.275753799334432 - }, - { - "x": 2.1109008821512623, - "y": 6.541377447734871, - "heading": 0.41591028587378526, - "angularVelocity": 0.6651458571992168, - "velocityX": 0.09293412549541542, - "velocityY": 0.7798653937207066, - "timestamp": 4.353165326868844 - }, - { - "x": 2.1380148682221702, - "y": 6.5848219723004595, - "heading": 0.465333297367931, - "angularVelocity": 0.6384451136450678, - "velocityX": 0.3502577320781457, - "velocityY": 0.5612151826648336, - "timestamp": 4.430576854403255 - }, - { - "x": 2.1849615573883057, - "y": 6.611239433288574, - "heading": 0.5125504196, - "angularVelocity": 0.6099494963599527, - "velocityX": 0.6064560493947926, - "velocityY": 0.3412600400679556, - "timestamp": 4.5079883819376665 - }, - { - "x": 2.250707976446258, - "y": 6.648678224064634, - "heading": 0.55208552781375, - "angularVelocity": 0.5325541984016284, - "velocityX": 0.8856313560565378, - "velocityY": 0.5043159387113171, - "timestamp": 4.582225160715128 - }, - { - "x": 2.3371670619589073, - "y": 6.698226995892447, - "heading": 0.585356010779416, - "angularVelocity": 0.44816711492022837, - "velocityX": 1.1646395080237388, - "velocityY": 0.6674423734944787, - "timestamp": 4.656461939492589 - }, - { - "x": 2.4409073566894506, - "y": 6.758690881227722, - "heading": 0.5617681472290975, - "angularVelocity": -0.31773824159353026, - "velocityX": 1.3974245170513677, - "velocityY": 0.8144734501011609, - "timestamp": 4.73069871827005 - }, - { - "x": 2.523886913207481, - "y": 6.807069780587539, - "heading": 0.5422957792802172, - "angularVelocity": -0.262300820018773, - "velocityX": 1.1177688186980366, - "velocityY": 0.6516837092951012, - "timestamp": 4.804935497047511 - }, - { - "x": 2.5861173110961473, - "y": 6.843356427328165, - "heading": 0.5274956073850987, - "angularVelocity": -0.1993644139582724, - "velocityX": 0.8382691021011796, - "velocityY": 0.4887960838037209, - "timestamp": 4.879172275824972 - }, - { - "x": 2.627602512984248, - "y": 6.8675485030782895, - "heading": 0.5175502299027057, - "angularVelocity": -0.1339683327613954, - "velocityX": 0.558822763747067, - "velocityY": 0.3258772288954634, - "timestamp": 4.953409054602433 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.06734950498988629, - "velocityX": 0.27940333769169035, - "velocityY": 0.16294305705300582, - "timestamp": 5.027645833379895 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -1.618919283202532e-33, - "velocityX": 2.4430876435253525e-35, - "velocityY": 8.017390657472539e-35, - "timestamp": 5.101882612157356 - }, - { - "x": 2.6570338601157935, - "y": 6.867732812321398, - "heading": 0.5065324038818937, - "angularVelocity": -0.10327871439636148, - "velocityX": 0.1491229424075764, - "velocityY": -0.20442985508511316, - "timestamp": 5.160152273561258 - }, - { - "x": 2.674422013328184, - "y": 6.84391114835803, - "heading": 0.49465795152131636, - "angularVelocity": -0.2037844750507175, - "velocityX": 0.2984083448136593, - "velocityY": -0.40881761433700353, - "timestamp": 5.218421934965161 - }, - { - "x": 2.7005198630420675, - "y": 6.808182791545169, - "heading": 0.47711568998903314, - "angularVelocity": -0.30105308851354295, - "velocityX": 0.4478805794491053, - "velocityY": -0.6131553874185988, - "timestamp": 5.276691596369063 - }, - { - "x": 2.7353400941220496, - "y": 6.760551298473928, - "heading": 0.4541277672109497, - "angularVelocity": -0.39450929049922157, - "velocityX": 0.5975705065217742, - "velocityY": -0.8174321237441035, - "timestamp": 5.3349612577729655 - }, - { - "x": 2.7788976924779565, - "y": 6.701021131245012, - "heading": 0.42595856862842546, - "angularVelocity": -0.4834282181127908, - "velocityX": 0.7475176156247499, - "velocityY": -1.021632283329671, - "timestamp": 5.393230919176868 - }, - { - "x": 2.831210629197317, - "y": 6.629598041190472, - "heading": 0.39292676107093794, - "angularVelocity": -0.5668783164625577, - "velocityX": 0.8977731371519051, - "velocityY": -1.2257337409164428, - "timestamp": 5.45150058058077 - }, - { - "x": 2.8923008190981245, - "y": 6.546289653631685, - "heading": 0.3554225990889349, - "angularVelocity": -0.6436310264794379, - "velocityX": 1.0484047517859123, - "velocityY": -1.4297043358691544, - "timestamp": 5.509770241984673 - }, - { - "x": 2.962195513842206, - "y": 6.451106403793388, - "heading": 0.31393409063739947, - "angularVelocity": -0.7120087443781998, - "velocityX": 1.1995040482490311, - "velocityY": -1.6334958457802513, - "timestamp": 5.568039903388575 - }, - { - "x": 3.0409294178085977, - "y": 6.344063134427028, - "heading": 0.26908904007938106, - "angularVelocity": -0.7696123416123889, - "velocityX": 1.351198927013472, - "velocityY": -1.8370326304863622, - "timestamp": 5.626309564792478 - }, - { - "x": 3.128548070700225, - "y": 6.22518206352159, - "heading": 0.22172760975049532, - "angularVelocity": -0.8127974178637293, - "velocityX": 1.5036753394582005, - "velocityY": -2.0401881191895193, - "timestamp": 5.68457922619638 - }, - { - "x": 3.2251135709999605, - "y": 6.094498947078054, - "heading": 0.17303894760412505, - "angularVelocity": -0.8355748252745036, - "velocityX": 1.6572174605646188, - "velocityY": -2.2427299780874135, - "timestamp": 5.742848887600283 - }, - { - "x": 3.3307147862388833, - "y": 5.952077971116604, - "heading": 0.12484956119030122, - "angularVelocity": -0.8270064601850674, - "velocityX": 1.8122846897450882, - "velocityY": -2.444170302865565, - "timestamp": 5.801118549004185 - }, - { - "x": 3.4454853336507822, - "y": 5.798056568785059, - "heading": 0.08033941158282583, - "angularVelocity": -0.7638649090295626, - "velocityX": 1.9696450030206056, - "velocityY": -2.643252056399113, - "timestamp": 5.859388210408087 - }, - { - "x": 3.5696119500976344, - "y": 5.632835617985738, - "heading": 0.04630921837828673, - "angularVelocity": -0.5840122009402995, - "velocityX": 2.1302100176360224, - "velocityY": -2.8354541079975273, - "timestamp": 5.91765787181199 - }, - { - "x": 3.7020235307305986, - "y": 5.458025004439994, - "heading": 0.04313298905641362, - "angularVelocity": -0.05450914327194612, - "velocityX": 2.272393170695454, - "velocityY": -3.00002796196164, - "timestamp": 5.975927533215892 - }, - { - "x": 3.84656727081631, - "y": 5.291677538591243, - "heading": 0.04313296518805293, - "angularVelocity": -4.0961900430587924e-7, - "velocityX": 2.480600309032019, - "velocityY": -2.8547868966613104, - "timestamp": 6.034197194619795 - }, - { - "x": 4.001959806149131, - "y": 5.135416436419877, - "heading": 0.04313294238925072, - "angularVelocity": -3.912636809189277e-7, - "velocityX": 2.666782877897643, - "velocityY": -2.6816888652951767, - "timestamp": 6.092466856023697 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.04313291889737663, - "angularVelocity": -4.031578960087653e-7, - "velocityX": 2.8409167869122447, - "velocityY": -2.496473071014189, - "timestamp": 6.1507365174276 - }, - { - "x": 4.2693517733895705, - "y": 4.907420739393082, - "heading": 0.043132896624531875, - "angularVelocity": -6.4257109962904e-7, - "velocityX": 2.9384484685182977, - "velocityY": -2.3809038315668807, - "timestamp": 6.185398587932365 - }, - { - "x": 4.374422288805421, - "y": 4.829031478695207, - "heading": 0.0431328753158102, - "angularVelocity": -6.147561689330783e-7, - "velocityX": 3.0312821445967915, - "velocityY": -2.261528511030489, - "timestamp": 6.220060658437131 - }, - { - "x": 4.482542607371009, - "y": 4.7549053274480135, - "heading": 0.043132854776618594, - "angularVelocity": -5.925552428867745e-7, - "velocityX": 3.1192689008789145, - "velocityY": -2.138537893661084, - "timestamp": 6.254722728941896 - }, - { - "x": 4.593539840912748, - "y": 4.685160765838758, - "heading": 0.04313283483631883, - "angularVelocity": -5.752772258439431e-7, - "velocityX": 3.2022678370145012, - "velocityY": -2.012129125398518, - "timestamp": 6.289384799446662 - }, - { - "x": 4.707236482283354, - "y": 4.619909225059653, - "heading": 0.04313281534202855, - "angularVelocity": -5.624098616258592e-7, - "velocityX": 3.280145695710103, - "velocityY": -1.882505569600442, - "timestamp": 6.324046869951427 - }, - { - "x": 4.823450643664921, - "y": 4.559254805625301, - "heading": 0.043132796153422026, - "angularVelocity": -5.53590891933908e-7, - "velocityX": 3.352776094711051, - "velocityY": -1.7498787161607805, - "timestamp": 6.358708940456193 - }, - { - "x": 4.9419959983331205, - "y": 4.5032933115020395, - "heading": 0.04313277713835443, - "angularVelocity": -5.485842974798401e-7, - "velocityX": 3.4200309716611494, - "velocityY": -1.6144879203210547, - "timestamp": 6.3933710109609585 - }, - { - "x": 5.061736542982073, - "y": 4.449937038090927, - "heading": 0.04313275817159016, - "angularVelocity": -5.471907475389428e-7, - "velocityX": 3.454512177294491, - "velocityY": -1.5393273579481606, - "timestamp": 6.428033081465724 - }, - { - "x": 5.1814776709209625, - "y": 4.396582073700789, - "heading": 0.04313273920484137, - "angularVelocity": -5.471903010583792e-7, - "velocityX": 3.45452900519684, - "velocityY": -1.5392895927207566, - "timestamp": 6.46269515197049 - }, - { - "x": 5.3012188001688525, - "y": 4.3432271122483534, - "heading": 0.043132720238092614, - "angularVelocity": -5.471902999394187e-7, - "velocityX": 3.454529042961442, - "velocityY": -1.5392895079680768, - "timestamp": 6.497357222475255 - }, - { - "x": 5.420963133587213, - "y": 4.289879342291843, - "heading": 0.04313270127136276, - "angularVelocity": -5.471897545932471e-7, - "velocityX": 3.4546214832116693, - "velocityY": -1.5390820334629507, - "timestamp": 6.532019292980021 - }, - { - "x": 5.5425254594597755, - "y": 4.240815191837089, - "heading": 0.04313268228677855, - "angularVelocity": -5.47704852461645e-7, - "velocityX": 3.5070705270145455, - "velocityY": -1.415499701554448, - "timestamp": 6.566681363484786 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.043132663161499156, - "angularVelocity": -5.517639056314665e-7, - "velocityX": 3.560845257182681, - "velocityY": -1.274191548268104, - "timestamp": 6.601343433989552 - }, - { - "x": 5.795862627864808, - "y": 4.156159506730396, - "heading": 0.04313264445875193, - "angularVelocity": -5.198107758201059e-7, - "velocityX": 3.6106505851434836, - "velocityY": -1.1253380804221886, - "timestamp": 6.637323348406788 - }, - { - "x": 5.927341530201614, - "y": 4.121094637119033, - "heading": 0.043132625947930554, - "angularVelocity": -5.1447652596799e-7, - "velocityX": 3.654230546858154, - "velocityY": -0.9745678993212189, - "timestamp": 6.6733032628240245 - }, - { - "x": 6.059882410873037, - "y": 4.090286167435021, - "heading": 0.04313260749907939, - "angularVelocity": -5.127541702089576e-7, - "velocityX": 3.683746412913316, - "velocityY": -0.8562685649205634, - "timestamp": 6.709283177241261 - }, - { - "x": 6.193269302687495, - "y": 4.06337550162166, - "heading": 0.043132589025398085, - "angularVelocity": -5.134442814470016e-7, - "velocityX": 3.7072598413563593, - "velocityY": -0.7479357927674594, - "timestamp": 6.745263091658497 - }, - { - "x": 6.327657815035056, - "y": 4.042021941101089, - "heading": 0.043132570411953335, - "angularVelocity": -5.173287667087509e-7, - "velocityX": 3.7350981658583504, - "velocityY": -0.5934855840107985, - "timestamp": 6.781243006075734 - }, - { - "x": 6.462816636912207, - "y": 4.026263156605077, - "heading": 0.043132547717955015, - "angularVelocity": -6.30740753253067e-7, - "velocityX": 3.7565075978169236, - "velocityY": -0.4379883818862704, - "timestamp": 6.81722292049297 - }, - { - "x": 6.597942834210822, - "y": 4.016131027814117, - "heading": 0.04107885634805962, - "angularVelocity": -0.0570788286508975, - "velocityX": 3.755600853621886, - "velocityY": -0.2816051387300244, - "timestamp": 6.853202834910206 - }, - { - "x": 6.728400824069692, - "y": 4.008818936530008, - "heading": 0.03307687475358465, - "angularVelocity": -0.2224013515341097, - "velocityX": 3.625856035843536, - "velocityY": -0.20322703381989088, - "timestamp": 6.889182749327443 - }, - { - "x": 6.853599659736003, - "y": 4.0035443033890195, - "heading": 0.023873611715651655, - "angularVelocity": -0.25578890853403846, - "velocityX": 3.47968686680181, - "velocityY": -0.14659937986014848, - "timestamp": 6.925162663744679 - }, - { - "x": 6.973435503233387, - "y": 4.000040041798795, - "heading": 0.014893418694113961, - "angularVelocity": -0.24958906009058449, - "velocityX": 3.3306316993343295, - "velocityY": -0.09739493956513012, - "timestamp": 6.961142578161915 - }, - { - "x": 7.08787150697828, - "y": 3.998176312998215, - "heading": 0.006805585628585594, - "angularVelocity": -0.22478744589936686, - "velocityX": 3.1805524164913392, - "velocityY": -0.05179914490532842, - "timestamp": 6.997122492579152 - }, - { - "x": 7.196889877319336, - "y": 3.9978766441345215, - "heading": 3.395025904591232e-32, - "angularVelocity": -0.18914957800247972, - "velocityX": 3.0299785896330556, - "velocityY": -0.008328782003720711, - "timestamp": 7.033102406996388 - }, - { - "x": 7.373371840772393, - "y": 4.002118083585038, - "heading": -0.0072355781339062996, - "angularVelocity": -0.11324248534202788, - "velocityX": 2.7620814521804613, - "velocityY": 0.06638186139590158, - "timestamp": 7.096996961341894 - }, - { - "x": 7.532286444106961, - "y": 4.009181217967762, - "heading": -0.011195509678912387, - "angularVelocity": -0.06197604139459259, - "velocityX": 2.4871384574536473, - "velocityY": 0.11054360508613674, - "timestamp": 7.1608915156874 - }, - { - "x": 7.6734693827744955, - "y": 4.017827175121804, - "heading": -0.012870706885804134, - "angularVelocity": -0.02621815308129755, - "velocityX": 2.209623967390062, - "velocityY": 0.13531602563950743, - "timestamp": 7.224786070032906 - }, - { - "x": 7.7968608797168075, - "y": 4.027211909710403, - "heading": -0.012933711131137412, - "angularVelocity": -0.0009860659641287687, - "velocityX": 1.9311739193778674, - "velocityY": 0.14687847320841513, - "timestamp": 7.2886806243784115 - }, - { - "x": 7.9024437482359025, - "y": 4.036727334915007, - "heading": -0.011868630755415614, - "angularVelocity": 0.01666934508944925, - "velocityX": 1.6524548860324264, - "velocityY": 0.14892388407859264, - "timestamp": 7.352575178723917 - }, - { - "x": 7.99021929218393, - "y": 4.0459160326905685, - "heading": -0.010039813807728299, - "angularVelocity": 0.028622422777974377, - "velocityX": 1.3737562589980292, - "velocityY": 0.143810342989083, - "timestamp": 7.416469733069423 - }, - { - "x": 8.060196978947376, - "y": 4.0544220986291215, - "heading": -0.00773105502412215, - "angularVelocity": 0.03613388976972427, - "velocityX": 1.0952058040039734, - "velocityY": 0.13312661815523763, - "timestamp": 7.480364287414929 - }, - { - "x": 8.112389704520517, - "y": 4.061961050008052, - "heading": -0.005169479155646227, - "angularVelocity": 0.04009067587551167, - "velocityX": 0.816857181457298, - "velocityY": 0.11799051509403427, - "timestamp": 7.544258841760435 - }, - { - "x": 8.146811527228987, - "y": 4.068300458431988, - "heading": -0.0025408789062790695, - "angularVelocity": 0.04113966012116123, - "velocityX": 0.5387285827574291, - "velocityY": 0.09921672494429698, - "timestamp": 7.608153396105941 - }, - { - "x": 8.1634765625, - "y": 4.073246955871582, - "heading": 2.9433151473989685e-31, - "angularVelocity": 0.03976675214822599, - "velocityX": 0.2608209015888442, - "velocityY": 0.07741657313776515, - "timestamp": 7.6720479504514465 - }, - { - "x": 8.160341731543628, - "y": 4.076804852758406, - "heading": 0.0025302388406713986, - "angularVelocity": 0.03600436134361471, - "velocityX": -0.0446074831711938, - "velocityY": 0.05062755463137832, - "timestamp": 7.7423238488772945 - }, - { - "x": 8.135735418270036, - "y": 4.078564432167465, - "heading": 0.004797198596772283, - "angularVelocity": 0.032257997505259615, - "velocityX": -0.3501387221617275, - "velocityY": 0.025038163132351486, - "timestamp": 7.8125997473031425 - }, - { - "x": 8.089649517071235, - "y": 4.078625238037987, - "heading": 0.006802207301300417, - "angularVelocity": 0.028530531084475955, - "velocityX": -0.6557853009510525, - "velocityY": 0.0008652450112215468, - "timestamp": 7.882875645728991 - }, - { - "x": 8.02207492391753, - "y": 4.077106587290107, - "heading": 0.008546856784408466, - "angularVelocity": 0.024825715817051033, - "velocityX": -0.9615614267102518, - "velocityY": -0.02160983753886332, - "timestamp": 7.953151544154839 - }, - { - "x": 7.933001430575272, - "y": 4.074154095631599, - "heading": 0.010033092526327095, - "angularVelocity": 0.021148584012580587, - "velocityX": -1.2674828118525576, - "velocityY": -0.042012862512522016, - "timestamp": 8.023427442580687 - }, - { - "x": 7.822417690835061, - "y": 4.069949420479082, - "heading": 0.011263348791088614, - "angularVelocity": 0.017506090883485915, - "velocityX": -1.5735656493512467, - "velocityY": -0.059830969744960986, - "timestamp": 8.093703341006535 - }, - { - "x": 7.690311409008121, - "y": 4.0647254734139295, - "heading": 0.012240761619989172, - "angularVelocity": 0.013908222460249003, - "velocityX": -1.879823449946105, - "velocityY": -0.07433483145952353, - "timestamp": 8.163979239432383 - }, - { - "x": 7.536670161450006, - "y": 4.058791543485583, - "heading": 0.012969524954057786, - "angularVelocity": 0.01037003226415625, - "velocityX": -2.186258034398947, - "velocityY": -0.08443762458060709, - "timestamp": 8.23425513785823 - }, - { - "x": 7.361484060419744, - "y": 4.0525779000493936, - "heading": 0.01345553350071711, - "angularVelocity": 0.006915721570918652, - "velocityX": -2.492833317742797, - "velocityY": -0.08841784417378167, - "timestamp": 8.304531036284079 - }, - { - "x": 7.164754272980972, - "y": 4.046723019235429, - "heading": 0.013707670109403618, - "angularVelocity": 0.0035878105343975153, - "velocityX": -2.799391994203425, - "velocityY": -0.08331278496768466, - "timestamp": 8.374806934709927 - }, - { - "x": 6.9465233889455575, - "y": 4.0422689491096495, - "heading": 0.013740797003253829, - "angularVelocity": 0.0004713834272096328, - "velocityX": -3.1053446334191217, - "velocityY": -0.06337976782296549, - "timestamp": 8.445082833135775 - }, - { - "x": 6.707012866607414, - "y": 4.041196732301159, - "heading": 0.013584512522768889, - "angularVelocity": -0.002223870259728453, - "velocityX": -3.408146003154458, - "velocityY": -0.015257247968487717, - "timestamp": 8.515358731561623 - }, - { - "x": 6.4476145619873, - "y": 4.048415292748879, - "heading": 0.013321412464940579, - "angularVelocity": -0.0037438163541362824, - "velocityX": -3.691141777345175, - "velocityY": 0.10271744096358496, - "timestamp": 8.58563462998747 - }, - { - "x": 6.1833350728544865, - "y": 4.076620284873478, - "heading": 0.013321410992019302, - "angularVelocity": -2.0959124115556836e-8, - "velocityX": -3.76059922466405, - "velocityY": 0.4013465890352113, - "timestamp": 8.655910528413319 - }, - { - "x": 5.922208724930001, - "y": 4.126140027657945, - "heading": 0.013321410307177713, - "angularVelocity": -9.745042100450887e-9, - "velocityX": -3.7157311933907815, - "velocityY": 0.7046475946048248, - "timestamp": 8.726186426839167 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.013321409576784942, - "angularVelocity": -1.0393218545701755e-8, - "velocityX": -3.646442120972535, - "velocityY": 1.003317616364548, - "timestamp": 8.796462325265015 - }, - { - "x": 5.541094088534591, - "y": 4.236435199254689, - "heading": 0.013321408817775413, - "angularVelocity": -2.1905263169095435e-8, - "velocityX": -3.6034323243164974, - "velocityY": 1.148240568022246, - "timestamp": 8.831111967342002 - }, - { - "x": 5.41792618310833, - "y": 4.281179290629024, - "heading": 0.01332140812506875, - "angularVelocity": -1.9991740821363056e-8, - "velocityX": -3.5546660237528647, - "velocityY": 1.2913291073806714, - "timestamp": 8.865761609418989 - }, - { - "x": 5.2966447994258585, - "y": 4.330809856867523, - "heading": 0.01332140748575771, - "angularVelocity": -1.8450725633992864e-8, - "velocityX": -3.500220389376678, - "velocityY": 1.432354369728484, - "timestamp": 8.900411251495976 - }, - { - "x": 5.177443709175996, - "y": 4.385247597760623, - "heading": 0.013321406889674104, - "angularVelocity": -1.7203167808757175e-8, - "velocityX": -3.4401824407021615, - "velocityY": 1.5710910020988935, - "timestamp": 8.935060893572963 - }, - { - "x": 5.0605133585968645, - "y": 4.444405530588187, - "heading": 0.01332140632867156, - "angularVelocity": -1.6190716838073352e-8, - "velocityX": -3.3746481513237594, - "velocityY": 1.7073172847247369, - "timestamp": 8.96971053564995 - }, - { - "x": 4.946040562745207, - "y": 4.508189127302759, - "heading": 0.013321405796093607, - "angularVelocity": -1.5370373829355437e-8, - "velocityX": -3.3037223183234317, - "velocityY": 1.840815456992452, - "timestamp": 9.004360177726937 - }, - { - "x": 4.834208203167093, - "y": 4.576496460724576, - "heading": 0.013321405286393582, - "angularVelocity": -1.4710109443101504e-8, - "velocityX": -3.2275184641053225, - "velocityY": 1.9713719775242866, - "timestamp": 9.039009819803924 - }, - { - "x": 4.725194921038476, - "y": 4.649218348571895, - "heading": 0.013321404794856252, - "angularVelocity": -1.4185928002634242e-8, - "velocityX": -3.146158967137548, - "velocityY": 2.098777461704854, - "timestamp": 9.073659461880911 - }, - { - "x": 4.619174674827694, - "y": 4.72623832110653, - "heading": 0.013321404313366907, - "angularVelocity": -1.3895939972772152e-8, - "velocityX": -3.05977897189302, - "velocityY": 2.2228215911583726, - "timestamp": 9.108309103957899 - }, - { - "x": 4.510161328966249, - "y": 4.798960113344169, - "heading": 0.013321403820821752, - "angularVelocity": -1.421501423878401e-8, - "velocityX": -3.146160806487812, - "velocityY": 2.0987747023782037, - "timestamp": 9.142958746034886 - }, - { - "x": 4.398328904529435, - "y": 4.867267340500532, - "heading": 0.013321403310039242, - "angularVelocity": -1.4741350266093223e-8, - "velocityX": -3.2275203359486127, - "velocityY": 1.9713689106685317, - "timestamp": 9.177608388111873 - }, - { - "x": 4.283856051297854, - "y": 4.931050823285602, - "heading": 0.013321399241018467, - "angularVelocity": -1.1743327000811157e-7, - "velocityX": -3.3037239743267564, - "velocityY": 1.8408121689497896, - "timestamp": 9.21225803018886 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.012938020716139158, - "angularVelocity": -0.011064429584221965, - "velocityX": -3.358100659168625, - "velocityY": 1.6997858867187727, - "timestamp": 9.246907672265847 - }, - { - "x": 3.941212684187486, - "y": 5.0868100262217055, - "heading": 0.011498233293039487, - "angularVelocity": -0.020924944231025543, - "velocityX": -3.2887007006223063, - "velocityY": 1.4077333471091193, - "timestamp": 9.315714900959192 - }, - { - "x": 3.7303193403637978, - "y": 5.1698989262671065, - "heading": 0.009891075368403834, - "angularVelocity": -0.023357399435433027, - "velocityX": -3.0649881971497734, - "velocityY": 1.2075606244178938, - "timestamp": 9.384522129652536 - }, - { - "x": 3.5371027460588107, - "y": 5.242302097421373, - "heading": 0.008284033873001792, - "angularVelocity": -0.023355707327847468, - "velocityX": -2.808085690619801, - "velocityY": 1.0522611145545084, - "timestamp": 9.453329358345881 - }, - { - "x": 3.362389747323273, - "y": 5.305525449861525, - "heading": 0.006750320632579214, - "angularVelocity": -0.022290001640058982, - "velocityX": -2.5391663354759597, - "velocityY": 0.9188475344926457, - "timestamp": 9.522136587039226 - }, - { - "x": 3.2065870657576463, - "y": 5.360439069495437, - "heading": 0.00533021576404821, - "angularVelocity": -0.0206388906441795, - "velocityX": -2.2643359502240057, - "velocityY": 0.7980792233131078, - "timestamp": 9.59094381573257 - }, - { - "x": 3.0699321768781336, - "y": 5.407605853881433, - "heading": 0.00404906921437406, - "angularVelocity": -0.018619359826041904, - "velocityX": -1.9860542485811465, - "velocityY": 0.6854917031494734, - "timestamp": 9.659751044425915 - }, - { - "x": 2.952579395626419, - "y": 5.447418742231964, - "heading": 0.002924266712171353, - "angularVelocity": -0.01634715601198879, - "velocityX": -1.705529832842474, - "velocityY": 0.578614908732415, - "timestamp": 9.72855827311926 - }, - { - "x": 2.8546365398725873, - "y": 5.480167232965782, - "heading": 0.001968461119345437, - "angularVelocity": -0.013891063642247188, - "velocityX": -1.4234384615362952, - "velocityY": 0.47594549810702536, - "timestamp": 9.797365501812605 - }, - { - "x": 2.776182973714466, - "y": 5.506073331672824, - "heading": 0.0011912691725219405, - "angularVelocity": -0.01129520780857525, - "velocityX": -1.140193663485079, - "velocityY": 0.376502573915577, - "timestamp": 9.86617273050595 - }, - { - "x": 2.7172794523584742, - "y": 5.525312622061272, - "heading": 0.0006002459449936435, - "angularVelocity": -0.008589551399640344, - "velocityX": -0.8560658883459488, - "velocityY": 0.27961147039059914, - "timestamp": 9.934979959199294 - }, - { - "x": 2.6779739252366173, - "y": 5.5380274141109735, - "heading": 0.00020148371013726726, - "angularVelocity": -0.005795353808442843, - "velocityX": -0.5712412470066293, - "velocityY": 0.18478860856854942, - "timestamp": 10.00378718789264 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -3.5000485154255244e-36, - "angularVelocity": -0.0029282346341141605, - "velocityX": -0.285853063090511, - "velocityY": 0.09167570478022308, - "timestamp": 10.072594416585984 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 7.813037085898689e-36, - "angularVelocity": 1.6441710918960667e-34, - "velocityX": -8.361910991338164e-36, - "velocityY": -1.503965302427871e-35, - "timestamp": 10.141401645279329 - }, - { - "x": 2.6750735555579293, - "y": 5.538716972759925, - "heading": 1.350818917618456e-19, - "angularVelocity": 2.121700407628309e-18, - "velocityX": 0.2633772294028884, - "velocityY": -0.088246807749562, - "timestamp": 10.205068452938024 - }, - { - "x": 2.7085920650608806, - "y": 5.52742596598787, - "heading": 3.9875003320592027e-19, - "angularVelocity": 4.14137524936932e-18, - "velocityX": 0.5264675697678677, - "velocityY": -0.17734526336837014, - "timestamp": 10.268735260596719 - }, - { - "x": 2.7588390755328, - "y": 5.51039889888726, - "heading": 7.83389944204561e-19, - "angularVelocity": 6.041451191657363e-18, - "velocityX": 0.7892183120172117, - "velocityY": -0.2674402522565467, - "timestamp": 10.332402068255414 - }, - { - "x": 2.8257885982701296, - "y": 5.487560532002736, - "heading": 1.2799519248933233e-18, - "angularVelocity": 7.799385566035206e-18, - "velocityX": 1.0515608556381997, - "velocityY": -0.35871701007777557, - "timestamp": 10.396068875914109 - }, - { - "x": 2.9094088162269203, - "y": 5.4588202139112365, - "heading": 9.407556731948414e-19, - "angularVelocity": -5.3276780189270054e-18, - "velocityX": 1.313403656188678, - "velocityY": -0.45141760908715395, - "timestamp": 10.459735683572804 - }, - { - "x": 3.0096598962036025, - "y": 5.424066617313392, - "heading": 6.890803629951045e-19, - "angularVelocity": -3.9530065893820216e-18, - "velocityX": 1.5746208057754196, - "velocityY": -0.5458668005493699, - "timestamp": 10.523402491231499 - }, - { - "x": 3.126490562172417, - "y": 5.38315977794949, - "heading": 5.07923477935949e-19, - "angularVelocity": -2.8453897991917994e-18, - "velocityX": 1.8350325745107396, - "velocityY": -0.6425143786570174, - "timestamp": 10.587069298890194 - }, - { - "x": 3.259832420061777, - "y": 5.335918462965804, - "heading": 3.751534220684201e-19, - "angularVelocity": -2.0853889294918685e-18, - "velocityX": 2.0943700931917317, - "velocityY": -0.7420085397863481, - "timestamp": 10.650736106548889 - }, - { - "x": 3.409589860765725, - "y": 5.282098877479992, - "heading": 4.172337353071023e-18, - "angularVelocity": 5.964150034596624e-17, - "velocityX": 2.3522059014921575, - "velocityY": -0.8453319314253599, - "timestamp": 10.714402914207584 - }, - { - "x": 3.575620301881546, - "y": 5.221355824269489, - "heading": 4.9261514224901164e-18, - "angularVelocity": 1.1839985341500765e-17, - "velocityX": 2.6078022005732984, - "velocityY": -0.9540772569615101, - "timestamp": 10.778069721866279 - }, - { - "x": 3.7576900306489667, - "y": 5.153163870724839, - "heading": 5.589206376297959e-18, - "angularVelocity": 1.041445265109494e-17, - "velocityX": 2.8597276267323055, - "velocityY": -1.071075432432765, - "timestamp": 10.841736529524974 - }, - { - "x": 3.9553538414525153, - "y": 5.076631094268888, - "heading": 6.855394728324636e-18, - "angularVelocity": 1.9887731120656582e-17, - "velocityX": 3.104660310018777, - "velocityY": -1.2020828320186703, - "timestamp": 10.90540333718367 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 8.923179883172485e-18, - "angularVelocity": 3.2478228937327026e-17, - "velocityX": 3.332116557247906, - "velocityY": -1.3615147608100575, - "timestamp": 10.969070144842364 - }, - { - "x": 4.285701077560922, - "y": 4.938388492372474, - "heading": 1.0772342870449973e-17, - "angularVelocity": 5.3398862490245696e-17, - "velocityX": 3.4133567656946298, - "velocityY": -1.4888942599373933, - "timestamp": 11.003699402864767 - }, - { - "x": 4.403715149774036, - "y": 4.881600772971698, - "heading": 9.945107603157448e-18, - "angularVelocity": -2.3888333580736016e-17, - "velocityX": 3.4079295645540313, - "velocityY": -1.6398768741750054, - "timestamp": 11.038328660887169 - }, - { - "x": 4.519367006147829, - "y": 4.8201449517293415, - "heading": 1.0140262228054944e-17, - "angularVelocity": 5.6355416212281845e-18, - "velocityX": 3.3397151131270646, - "velocityY": -1.7746791225673952, - "timestamp": 11.072957918909571 - }, - { - "x": 4.6324721141561005, - "y": 4.75411916510052, - "heading": 1.0958701743566334e-17, - "angularVelocity": 2.3634335883891303e-17, - "velocityX": 3.2661718577713317, - "velocityY": -1.906647453610143, - "timestamp": 11.107587176931974 - }, - { - "x": 4.742850082889288, - "y": 4.683628924272634, - "heading": 1.1951109976942982e-17, - "angularVelocity": 2.8658085389373404e-17, - "velocityX": 3.1874193972560034, - "velocityY": -2.0355689048343186, - "timestamp": 11.142216434954376 - }, - { - "x": 4.850325220314879, - "y": 4.608787325463072, - "heading": 1.1732443750037932e-17, - "angularVelocity": -6.3144935638988695e-18, - "velocityX": 3.1035934225348796, - "velocityY": -2.1612244409379597, - "timestamp": 11.176845692976778 - }, - { - "x": 4.959315849281407, - "y": 4.53617043444409, - "heading": 1.1207275250284524e-17, - "angularVelocity": -1.516545631482102e-17, - "velocityX": 3.147356749486827, - "velocityY": -2.096980852780761, - "timestamp": 11.21147495099918 - }, - { - "x": 5.0711194796065495, - "y": 4.467963995966005, - "heading": 1.0229768329797992e-17, - "angularVelocity": -2.822777547974504e-17, - "velocityX": 3.228588676454311, - "velocityY": -1.969618824462288, - "timestamp": 11.246104209021583 - }, - { - "x": 5.1855579490393096, - "y": 4.404277210647011, - "heading": 8.869711544920542e-18, - "angularVelocity": -3.9274788503918017e-17, - "velocityX": 3.3046757559381783, - "velocityY": -1.8391033754691917, - "timestamp": 11.280733467043985 - }, - { - "x": 5.302448702578612, - "y": 4.345211810292891, - "heading": 8.222829686597657e-18, - "angularVelocity": -1.8680211337603908e-17, - "velocityX": 3.3754911371096723, - "velocityY": -1.705650185051869, - "timestamp": 11.315362725066388 - }, - { - "x": 5.421605230513571, - "y": 4.290862095789443, - "heading": 7.194520598849692e-18, - "angularVelocity": -2.969480567798286e-17, - "velocityX": 3.4409206185669707, - "velocityY": -1.5694738382292037, - "timestamp": 11.34999198308879 - }, - { - "x": 5.542837392691082, - "y": 4.241314822773421, - "heading": 5.567604077424851e-18, - "angularVelocity": -4.698098123766793e-17, - "velocityX": 3.500859362885684, - "velocityY": -1.4307922215361755, - "timestamp": 11.384621241111192 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 4.441119305733273e-18, - "angularVelocity": -3.252985585087723e-17, - "velocityX": 3.5552114934161922, - "velocityY": -1.2898268911821025, - "timestamp": 11.419250499133595 - }, - { - "x": 5.934272395491136, - "y": 4.124337934678793, - "heading": 1.9097100466976255e-18, - "angularVelocity": -3.445086702659097e-17, - "velocityX": 3.6516733021145136, - "velocityY": -0.9841085377661556, - "timestamp": 11.492729324995416 - }, - { - "x": 6.207753017493673, - "y": 4.075010140740357, - "heading": -4.66846161540337e-18, - "angularVelocity": -8.952472477542546e-17, - "velocityX": 3.721897006313398, - "velocityY": -0.6713198443208224, - "timestamp": 11.566208150857237 - }, - { - "x": 6.4844286250858945, - "y": 4.049019985828209, - "heading": -6.502926410881733e-18, - "angularVelocity": -2.4965896963679208e-17, - "velocityX": 3.7653787243758563, - "velocityY": -0.3537094476852885, - "timestamp": 11.639686976719059 - }, - { - "x": 6.743277363747609, - "y": 4.038426394199003, - "heading": -5.737032029312171e-18, - "angularVelocity": 1.0423334512854795e-17, - "velocityX": 3.5227663973358068, - "velocityY": -0.1441720319419282, - "timestamp": 11.71316580258088 - }, - { - "x": 6.978672822837657, - "y": 4.030025299692622, - "heading": -4.2593802240854646e-18, - "angularVelocity": 2.010989952405427e-17, - "velocityX": 3.2035822065626656, - "velocityY": -0.11433354313770064, - "timestamp": 11.786644628442701 - }, - { - "x": 7.19055269256627, - "y": 4.022995759457438, - "heading": -3.134299993682012e-18, - "angularVelocity": 1.5311625045821916e-17, - "velocityX": 2.883550019253943, - "velocityY": -0.09566756344750842, - "timestamp": 11.860123454304523 - }, - { - "x": 7.3789017577018114, - "y": 4.017045849522277, - "heading": -2.330563766445784e-18, - "angularVelocity": 1.0938337919929083e-17, - "velocityX": 2.5633107623376485, - "velocityY": -0.080974482993913, - "timestamp": 11.933602280166344 - }, - { - "x": 7.54371362899692, - "y": 4.012025997027571, - "heading": -1.6314906834703783e-18, - "angularVelocity": 9.513939216857223e-18, - "velocityX": 2.242984551835937, - "velocityY": -0.06831699385270738, - "timestamp": 12.007081106028165 - }, - { - "x": 7.684984887810538, - "y": 4.007845278835319, - "heading": -1.3889434062297666e-18, - "angularVelocity": 3.300913894524217e-18, - "velocityX": 1.9226118158077503, - "velocityY": -0.05689691068436309, - "timestamp": 12.080559931889987 - }, - { - "x": 7.8027134339680275, - "y": 4.0044425825275765, - "heading": -1.0305851069387887e-18, - "angularVelocity": 4.877028110994567e-18, - "velocityX": 1.6022104977409415, - "velocityY": -0.04630852858402237, - "timestamp": 12.154038757751808 - }, - { - "x": 7.896897856926731, - "y": 4.001774011196325, - "heading": -6.085636422043956e-19, - "angularVelocity": 5.743443227141563e-18, - "velocityX": 1.2817899830873647, - "velocityY": -0.036317555431137405, - "timestamp": 12.22751758361363 - }, - { - "x": 7.967537148527336, - "y": 3.999806507404438, - "heading": -1.6215491216433702e-19, - "angularVelocity": 6.07533836862799e-18, - "velocityX": 0.9613557480279205, - "velocityY": -0.026776472933673492, - "timestamp": 12.30099640947545 - }, - { - "x": 8.014630554462236, - "y": 3.9985142796609896, - "heading": 9.722147651876556e-20, - "angularVelocity": 3.5299473779135435e-18, - "velocityX": 0.6409112473226125, - "velocityY": -0.017586396193627746, - "timestamp": 12.374475235337272 - }, - { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 9.32148398835055e-44, - "angularVelocity": -1.3231223468593855e-18, - "velocityX": 0.32045879198477595, - "velocityY": -0.00867781322019328, - "timestamp": 12.447954061199093 - }, - { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": -1.3950188036384555e-43, - "angularVelocity": -3.1671262842054474e-42, - "velocityX": -2.982658307493247e-43, - "velocityY": -2.0280554768402976e-41, - "timestamp": 12.521432887060914 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -2.548660671865789e-25, + "angularVelocity": -1.3190073040663248e-25, + "velocityX": -3.501543589784201e-25, + "velocityY": -4.821953566499718e-25, + "timestamp": 0 + }, + { + "x": 1.3060567679582222, + "y": 5.572381918180447, + "heading": -0.010695335923127927, + "angularVelocity": -0.1421295269156777, + "velocityX": 0.21371671803125017, + "velocityY": -0.2468070561274771, + "timestamp": 0.07525062634925417 + }, + { + "x": 1.3382215476730444, + "y": 5.535237409026609, + "heading": -0.03207750843935064, + "angularVelocity": -0.2841461068640612, + "velocityX": 0.4274353752955963, + "velocityY": -0.49361063097923197, + "timestamp": 0.15050125269850834 + }, + { + "x": 1.3864692368555713, + "y": 5.479521033340732, + "heading": -0.06412446500671098, + "angularVelocity": -0.4258696322157321, + "velocityX": 0.6411599680061012, + "velocityY": -0.7404107897691923, + "timestamp": 0.2257518790477625 + }, + { + "x": 1.4508006060735015, + "y": 5.405233126364813, + "heading": -0.10680311296194826, + "angularVelocity": -0.5671533916163913, + "velocityX": 0.8548948007336773, + "velocityY": -0.9872064935530687, + "timestamp": 0.3010025053970167 + }, + { + "x": 1.5312167667505892, + "y": 5.312374207143071, + "heading": -0.1600730253151242, + "angularVelocity": -0.7078999197420489, + "velocityX": 1.0686444030892075, + "velocityY": -1.2339952997967572, + "timestamp": 0.37625313174627084 + }, + { + "x": 1.6277191879063078, + "y": 5.200945106889095, + "heading": -0.22389124150457193, + "angularVelocity": -0.8480755481456564, + "velocityX": 1.2824135271357124, + "velocityY": -1.4807730601046407, + "timestamp": 0.451503758095525 + }, + { + "x": 1.7403097782498615, + "y": 5.070947160382222, + "heading": -0.29821764025550423, + "angularVelocity": -0.987718007900262, + "velocityX": 1.496208015877461, + "velocityY": -1.7275330826287743, + "timestamp": 0.5267543844447792 + }, + { + "x": 1.8689915501241479, + "y": 4.922382870770339, + "heading": -0.3830195665070615, + "angularVelocity": -1.1269265169697522, + "velocityX": 1.7100425354208568, + "velocityY": -1.9742598410060204, + "timestamp": 0.6020050107940333 + }, + { + "x": 2.021815501878169, + "y": 4.778590249333106, + "heading": -0.46245356659686293, + "angularVelocity": -1.0555925437900195, + "velocityX": 2.03086617571437, + "velocityY": -1.9108494960541829, + "timestamp": 0.6772556371432875 + }, + { + "x": 2.1585523215512867, + "y": 4.653366950194428, + "heading": -0.5314334875144606, + "angularVelocity": -0.9166690599683149, + "velocityX": 1.8170854690098266, + "velocityY": -1.6640831473945463, + "timestamp": 0.7525062634925417 + }, + { + "x": 2.2791997725503816, + "y": 4.546710847450191, + "heading": -0.5899636474798271, + "angularVelocity": -0.7778029606519882, + "velocityX": 1.6032750403849743, + "velocityY": -1.417345049717245, + "timestamp": 0.8277568898417959 + }, + { + "x": 2.3837568392129738, + "y": 4.458620854469452, + "heading": -0.6380444617817417, + "angularVelocity": -0.6389423800774935, + "velocityX": 1.3894511146966526, + "velocityY": -1.1706213921980426, + "timestamp": 0.90300751619105 + }, + { + "x": 2.472222908494702, + "y": 4.389096261560078, + "heading": -0.6756747800114784, + "angularVelocity": -0.5000665118066445, + "velocityX": 1.1756190423072155, + "velocityY": -0.9239071657250583, + "timestamp": 0.9782581425403042 + }, + { + "x": 2.54459758971785, + "y": 4.338136588288653, + "heading": -0.7028528784400924, + "angularVelocity": -0.361167737029517, + "velocityX": 0.9617817782305474, + "velocityY": -0.6771993236961286, + "timestamp": 1.0535087688895584 + }, + { + "x": 2.600880650448604, + "y": 4.305741524929518, + "heading": -0.7195768777221777, + "angularVelocity": -0.2222439877704894, + "velocityX": 0.7479414253581413, + "velocityY": -0.43049559758855604, + "timestamp": 1.1287593952388124 + }, + { + "x": 2.6410719892542827, + "y": 4.291910904068807, + "heading": -0.7258449001015178, + "angularVelocity": -0.08329528514817768, + "velocityX": 0.5340997245543403, + "velocityY": -0.18379409623143778, + "timestamp": 1.2040100215880667 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05567774261378543, + "velocityX": 0.3202582509259972, + "velocityY": 0.06290689942978202, + "timestamp": 1.279260647937321 + }, + { + "x": 2.673111673605269, + "y": 4.320290744259783, + "heading": -0.7067971794350305, + "angularVelocity": 0.19582512759435053, + "velocityX": 0.10464854696595383, + "velocityY": 0.31165110401806617, + "timestamp": 1.3551341369201315 + }, + { + "x": 2.664692786669079, + "y": 4.362810218152184, + "heading": -0.6813099274420213, + "angularVelocity": 0.3359177538123147, + "velocityX": -0.11095953341617228, + "velocityY": 0.560399613388471, + "timestamp": 1.431007625902942 + }, + { + "x": 2.639915179239814, + "y": 4.424203570363792, + "heading": -0.6451974716196371, + "angularVelocity": 0.47595617792883843, + "velocityX": -0.32656475616770847, + "velocityY": 0.8091541991105307, + "timestamp": 1.5068811148857526 + }, + { + "x": 2.5987791710000745, + "y": 4.504471407000408, + "heading": -0.5984632131457898, + "angularVelocity": 0.6159497750846186, + "velocityX": -0.5421657655556009, + "velocityY": 1.057916773206541, + "timestamp": 1.5827546038685631 + }, + { + "x": 2.5412852001352904, + "y": 4.603614476417717, + "heading": -0.5411082975385818, + "angularVelocity": 0.7559282745018111, + "velocityX": -0.7577609997321935, + "velocityY": 1.3066892105063217, + "timestamp": 1.6586280928513737 + }, + { + "x": 2.4674338294975446, + "y": 4.721633621731354, + "heading": -0.47312860561398856, + "angularVelocity": 0.8959610640812155, + "velocityX": -0.9733488156116926, + "velocityY": 1.5554727599303477, + "timestamp": 1.7345015818341842 + }, + { + "x": 2.3772256595738117, + "y": 4.858529596063375, + "heading": -0.39450963445811277, + "angularVelocity": 1.0361849996602548, + "velocityX": -1.1889287171724772, + "velocityY": 1.804266235378164, + "timestamp": 1.8103750708169948 + }, + { + "x": 2.2706605653387557, + "y": 5.014302224131365, + "heading": -0.3052190347895922, + "angularVelocity": 1.176835293402019, + "velocityX": -1.404510266546452, + "velocityY": 2.0530574006327873, + "timestamp": 1.8862485597998053 + }, + { + "x": 2.156230166958563, + "y": 5.146478435153377, + "heading": -0.22909325420264964, + "angularVelocity": 1.0033251614959908, + "velocityX": -1.508173670596822, + "velocityY": 1.7420605377980973, + "timestamp": 1.9621220487826159 + }, + { + "x": 2.05814700584302, + "y": 5.25977079434431, + "heading": -0.16373664121531684, + "angularVelocity": 0.8613893187663968, + "velocityX": -1.2927197948912579, + "velocityY": 1.4931745028437962, + "timestamp": 2.0379955377654264 + }, + { + "x": 1.9764115799277049, + "y": 5.35418052420219, + "heading": -0.10920088427959268, + "angularVelocity": 0.7187722308127881, + "velocityX": -1.077259356477368, + "velocityY": 1.24430458021074, + "timestamp": 2.1138690267482367 + }, + { + "x": 1.9110235357976653, + "y": 5.42970804617497, + "heading": -0.06553152377220887, + "angularVelocity": 0.5755549282474322, + "velocityX": -0.8618035760139328, + "velocityY": 0.9954402121917941, + "timestamp": 2.189742515731047 + }, + { + "x": 1.8619825316383507, + "y": 5.486353607136737, + "heading": -0.03276339241171435, + "angularVelocity": 0.4318785362291475, + "velocityX": -0.6463523006095715, + "velocityY": 0.7465790979323437, + "timestamp": 2.2656160047138574 + }, + { + "x": 1.8292884005204861, + "y": 5.524117342639773, + "heading": -0.010917643304822403, + "angularVelocity": 0.287923349772952, + "velocityX": -0.4309032253053722, + "velocityY": 0.4977197702295186, + "timestamp": 2.3414894936966677 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.3743055340676842e-24, + "angularVelocity": 0.14389272789730093, + "velocityX": -0.21545347602984788, + "velocityY": 0.24886063882774975, + "timestamp": 2.417362982679478 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 6.7146804288849265e-25, + "angularVelocity": -3.3897350204457557e-25, + "velocityX": 3.460398907914197e-22, + "velocityY": 1.7844799561406784e-24, + "timestamp": 2.4932364716622883 + }, + { + "x": 1.8507557540076562, + "y": 5.5430527114888575, + "heading": -2.374547601271383e-20, + "angularVelocity": -2.550633226443435e-19, + "velocityX": 0.40617567269200466, + "velocityY": 0.0005740544422026415, + "timestamp": 2.5863354999137984 + }, + { + "x": 1.9263848743459357, + "y": 5.543159599309593, + "heading": -2.866667304736003e-20, + "angularVelocity": -5.285981096799938e-20, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765753625, + "timestamp": 2.6794345281653085 + }, + { + "x": 2.0398285536670646, + "y": 5.543319931039021, + "heading": -2.74578198723704e-20, + "angularVelocity": 1.2984594980856443e-20, + "velocityX": 1.2185269970236072, + "velocityY": 0.0017221632968542275, + "timestamp": 2.7725335564168185 + }, + { + "x": 2.191086789804772, + "y": 5.543533706674078, + "heading": -4.1106983374446124e-20, + "angularVelocity": -1.4660908667312053e-19, + "velocityX": 1.6247026309348636, + "velocityY": 0.0022962176842474297, + "timestamp": 2.8656325846683286 + }, + { + "x": 2.380159571927711, + "y": 5.543800926199457, + "heading": -5.015487830921238e-20, + "angularVelocity": -9.718570757102548e-20, + "velocityX": 2.0308781485038963, + "velocityY": 0.002870271907212345, + "timestamp": 2.9587316129198387 + }, + { + "x": 2.5314178080654184, + "y": 5.544014701834514, + "heading": -3.4511971954263414e-20, + "angularVelocity": 1.6802437843559674e-19, + "velocityX": 1.6247026309348636, + "velocityY": 0.00229621768424743, + "timestamp": 3.0518306411713487 + }, + { + "x": 2.644861487386547, + "y": 5.544175033563942, + "heading": -6.180786622652372e-21, + "angularVelocity": 3.0431236355167837e-19, + "velocityX": 1.218526997023607, + "velocityY": 0.0017221632968542275, + "timestamp": 3.144929669422859 + }, + { + "x": 2.7204906077248263, + "y": 5.544281921384678, + "heading": 5.803364763600579e-22, + "angularVelocity": 7.262291804719445e-20, + "velocityX": 0.8123513398439022, + "velocityY": 0.0011481088765753625, + "timestamp": 3.238028697674369 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.254107177965744e-28, + "angularVelocity": -6.233540915514837e-21, + "velocityX": 0.40617567269200466, + "velocityY": 0.0005740544422026413, + "timestamp": 3.331127725925879 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.3203819062171384e-28, + "angularVelocity": -7.150744492931399e-29, + "velocityX": -3.49033600951782e-22, + "velocityY": -5.085821437921445e-25, + "timestamp": 3.424226754177389 + }, + { + "x": 2.7396220771473008, + "y": 5.562601682668762, + "heading": 0.0037078226871735676, + "angularVelocity": 0.0478975658583634, + "velocityX": -0.24134772812197372, + "velocityY": 0.23596385620231192, + "timestamp": 3.5016382597670024 + }, + { + "x": 2.702381046393162, + "y": 5.599259413613318, + "heading": 0.011292514660087486, + "angularVelocity": 0.09797887168250088, + "velocityX": -0.48107875528951993, + "velocityY": 0.47354370213249264, + "timestamp": 3.579049765356616 + }, + { + "x": 2.6467700945928994, + "y": 5.6544931725755525, + "heading": 0.02300756873559927, + "angularVelocity": 0.15133479172485775, + "velocityX": -0.7183809612885809, + "velocityY": 0.7135083931198739, + "timestamp": 3.656461270946229 + }, + { + "x": 2.573103126896307, + "y": 5.728602621187937, + "heading": 0.039271681184884964, + "angularVelocity": 0.21009942030462117, + "velocityX": -0.9516281479800662, + "velocityY": 0.9573441059946076, + "timestamp": 3.7338727765358426 + }, + { + "x": 2.482008526610063, + "y": 5.822157343718097, + "heading": 0.060902976833889094, + "angularVelocity": 0.27943256605393446, + "velocityX": -1.1767578939643621, + "velocityY": 1.208537694979451, + "timestamp": 3.811284282125456 + }, + { + "x": 2.3753508008518254, + "y": 5.936638558421237, + "heading": 0.09019128304072983, + "angularVelocity": 0.378345647507604, + "velocityX": -1.3778019810604039, + "velocityY": 1.4788656263843583, + "timestamp": 3.8886957877150694 + }, + { + "x": 2.2779173251453018, + "y": 6.074876969132681, + "heading": 0.1437805349404755, + "angularVelocity": 0.6922646897457585, + "velocityX": -1.258643336858141, + "velocityY": 1.7857605230454636, + "timestamp": 3.966107293304683 + }, + { + "x": 2.2032165306277185, + "y": 6.200218266147509, + "heading": 0.20002968943075358, + "angularVelocity": 0.7266252485576944, + "velocityX": -0.964983098424666, + "velocityY": 1.6191559130668154, + "timestamp": 4.043518798894296 + }, + { + "x": 2.1496243808514985, + "y": 6.310133689219402, + "heading": 0.2561009835178197, + "angularVelocity": 0.7243276520717804, + "velocityX": -0.6923021244455775, + "velocityY": 1.4198848379799673, + "timestamp": 4.12093030448391 + }, + { + "x": 2.116554400688044, + "y": 6.403853368096058, + "heading": 0.3110546267083605, + "angularVelocity": 0.7098898642001644, + "velocityX": -0.4271972223195187, + "velocityY": 1.2106685971656326, + "timestamp": 4.198341810073523 + }, + { + "x": 2.1037071305017707, + "y": 6.481007112457934, + "heading": 0.3644192794127437, + "angularVelocity": 0.6893633226472664, + "velocityX": -0.1659607326898067, + "velocityY": 0.9966702465508831, + "timestamp": 4.275753315663136 + }, + { + "x": 2.110901195101711, + "y": 6.541377623864855, + "heading": 0.41590951273781046, + "angularVelocity": 0.6651496173971262, + "velocityX": 0.09293275650881538, + "velocityY": 0.7798648398206792, + "timestamp": 4.35316482125275 + }, + { + "x": 2.138015040799913, + "y": 6.58482206941201, + "heading": 0.4653328742353283, + "angularVelocity": 0.6384498159683032, + "velocityX": 0.3502560180387482, + "velocityY": 0.5612143210012033, + "timestamp": 4.430576326842363 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.6099551352868545, + "velocityX": 0.6064539919591982, + "velocityY": 0.3412588823244487, + "timestamp": 4.507987832431977 + }, + { + "x": 2.2507078582365074, + "y": 6.648678152595124, + "heading": 0.5520861160632833, + "angularVelocity": 0.5325619619367677, + "velocityX": 0.8856294969362879, + "velocityY": 0.5043148240682422, + "timestamp": 4.582224633572456 + }, + { + "x": 2.3371668542660595, + "y": 6.698226860856562, + "heading": 0.585357485403268, + "angularVelocity": 0.4481789197385401, + "velocityX": 1.1646379518150936, + "velocityY": 0.6674413161698084, + "timestamp": 4.656461434712935 + }, + { + "x": 2.4409072196533095, + "y": 6.7586907970323, + "heading": 0.5617689305008087, + "angularVelocity": -0.31774745867379856, + "velocityX": 1.3974250478673051, + "velocityY": 0.8144738895917877, + "timestamp": 4.730698235853414 + }, + { + "x": 2.5238868313790523, + "y": 6.807069731854914, + "heading": 0.5422961869631279, + "angularVelocity": -0.2623058003379255, + "velocityX": 1.117769225652917, + "velocityY": 0.6516839906809204, + "timestamp": 4.804935036993894 + }, + { + "x": 2.586117270308082, + "y": 6.8433564035586825, + "heading": 0.5274957902762942, + "angularVelocity": -0.19936738193805836, + "velocityX": 0.8382694024122912, + "velocityY": 0.4887962728229996, + "timestamp": 4.879171838134373 + }, + { + "x": 2.6276024994187765, + "y": 6.867548495303344, + "heading": 0.5175502856493861, + "angularVelocity": -0.13397000509340504, + "velocityX": 0.5588229621073203, + "velocityY": 0.3258773461814771, + "timestamp": 4.953408639274852 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.06735023563211984, + "velocityX": 0.2794034362569526, + "velocityY": 0.16294311269989367, + "timestamp": 5.0276454404153315 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 9.95994551309633e-28, + "velocityX": -2.708487136756449e-26, + "velocityY": -1.710598190536398e-26, + "timestamp": 5.101882241555811 + }, + { + "x": 2.657033865407608, + "y": 6.8677328189813025, + "heading": 0.5065323846197595, + "angularVelocity": -0.10327905668354015, + "velocityX": 0.1491230501434533, + "velocityY": -0.20442976398577012, + "timestamp": 5.160151896348268 + }, + { + "x": 2.674422029082572, + "y": 6.843911168058592, + "heading": 0.49465789497247153, + "angularVelocity": -0.20378513807198495, + "velocityX": 0.2984085582263455, + "velocityY": -0.40881743692420125, + "timestamp": 5.218421551140726 + }, + { + "x": 2.700519894286892, + "y": 6.808182830343848, + "heading": 0.4771155795523401, + "angularVelocity": -0.30105404747312803, + "velocityX": 0.4478808961074784, + "velocityY": -0.6131551292349195, + "timestamp": 5.276691205933184 + }, + { + "x": 2.7353401457143893, + "y": 6.760551362049968, + "heading": 0.4541275879247931, + "angularVelocity": -0.3945105168277461, + "velocityX": 0.5975709235196038, + "velocityY": -0.8174317912733681, + "timestamp": 5.334960860725642 + }, + { + "x": 2.7788977690674797, + "y": 6.7010212248295815, + "heading": 0.42595830742432134, + "angularVelocity": -0.4834296788062957, + "velocityX": 0.7475181294317162, + "velocityY": -1.0216318842529362, + "timestamp": 5.393230515518099 + }, + { + "x": 2.8312107351766715, + "y": 6.629598169475443, + "heading": 0.3929264070745021, + "angularVelocity": -0.5668799732462906, + "velocityX": 0.8977737433921239, + "velocityY": -1.2257332844776365, + "timestamp": 5.451500170310557 + }, + { + "x": 2.8923009585334296, + "y": 6.546289820646855, + "heading": 0.35542214398319716, + "angularVelocity": -0.6436328347041991, + "velocityX": 1.048405444898319, + "velocityY": -1.4297038334157206, + "timestamp": 5.509769825103015 + }, + { + "x": 2.9621956903718707, + "y": 6.451106612735035, + "heading": 0.3139335291126181, + "angularVelocity": -0.7120106514849124, + "velocityX": 1.199504820946487, + "velocityY": -1.6334953115963773, + "timestamp": 5.568039479895472 + }, + { + "x": 3.040929634489161, + "y": 6.344063387407532, + "heading": 0.2690883704076084, + "angularVelocity": -0.7696142849093055, + "velocityX": 1.3511997693777469, + "velocityY": -1.8370320831445586, + "timestamp": 5.62630913468793 + }, + { + "x": 3.1285483297575434, + "y": 6.225182361182179, + "heading": 0.2217268345391298, + "angularVelocity": -0.8127993213134442, + "velocityX": 1.5036762373221262, + "velocityY": -2.0401875838938306, + "timestamp": 5.684578789480388 + }, + { + "x": 3.225113873395399, + "y": 6.094499287941398, + "heading": 0.1730380747769639, + "angularVelocity": -0.8355765953236391, + "velocityX": 1.6572183923484294, + "velocityY": -2.242729491126068, + "timestamp": 5.742848444272846 + }, + { + "x": 3.330715130826902, + "y": 5.952078350373372, + "heading": 0.12484860508535624, + "angularVelocity": -0.8270079831989134, + "velocityX": 1.8122856194639898, + "velocityY": -2.4441699212959707, + "timestamp": 5.801118099065303 + }, + { + "x": 3.4454857152688785, + "y": 5.798056975551327, + "heading": 0.08033839295880485, + "angularVelocity": -0.7638660686267251, + "velocityX": 1.9696458619973118, + "velocityY": -2.643251884203387, + "timestamp": 5.859387753857761 + }, + { + "x": 3.569612354139756, + "y": 5.632836026447166, + "heading": 0.04630815492886413, + "angularVelocity": -0.5840130364792431, + "velocityX": 2.1302106441677915, + "velocityY": -2.8354544006247413, + "timestamp": 5.917657408650219 + }, + { + "x": 3.7020237011319064, + "y": 5.458025154952382, + "heading": 0.04313214682309951, + "angularVelocity": -0.05450535303627194, + "velocityX": 2.272389418879637, + "velocityY": -3.0000327291695483, + "timestamp": 5.975927063442676 + }, + { + "x": 3.846567317399324, + "y": 5.291677583037812, + "heading": 0.04313213729822128, + "angularVelocity": -1.634620671894734e-7, + "velocityX": 2.480598465569185, + "velocityY": -2.8547890408319527, + "timestamp": 6.034196718235134 + }, + { + "x": 4.001959808565688, + "y": 5.135416438368326, + "heading": 0.043132128200091754, + "angularVelocity": -1.5613838046572486e-7, + "velocityX": 2.6667824225119015, + "velocityY": -2.6816898989027482, + "timestamp": 6.092466373027592 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.043132118825415074, + "angularVelocity": -1.6088436959619284e-7, + "velocityX": 2.84091706777896, + "velocityY": -2.4964733877097585, + "timestamp": 6.15073602782005 + }, + { + "x": 4.269351778492494, + "y": 4.907420759942678, + "heading": 0.043132109937133276, + "angularVelocity": -2.564268066942143e-7, + "velocityX": 2.938449360926388, + "velocityY": -2.3809038425071845, + "timestamp": 6.1853980895345355 + }, + { + "x": 4.374422315135501, + "y": 4.829031543571565, + "heading": 0.043132101433653904, + "angularVelocity": -2.4532526192426324e-7, + "velocityX": 3.0312835257314057, + "velocityY": -2.2615278057263164, + "timestamp": 6.2200601512490215 + }, + { + "x": 4.482542676912381, + "y": 4.754905472881388, + "heading": 0.04313209323726653, + "angularVelocity": -2.3646566200370356e-7, + "velocityX": 3.119270938568917, + "velocityY": -2.1385361119243003, + "timestamp": 6.254722212963507 + }, + { + "x": 4.593539986069964, + "y": 4.685161050857923, + "heading": 0.04313208527987584, + "angularVelocity": -2.2957061125967145e-7, + "velocityX": 3.202270830623841, + "velocityY": -2.0121256086251895, + "timestamp": 6.289384274677993 + }, + { + "x": 4.707236757335738, + "y": 4.619909757466469, + "heading": 0.043132077500472085, + "angularVelocity": -2.244356903685004e-7, + "velocityX": 3.2801502750269638, + "velocityY": -1.8824989098725307, + "timestamp": 6.324046336392479 + }, + { + "x": 4.823451163522863, + "y": 4.559255830076377, + "heading": 0.0431320698430563, + "angularVelocity": -2.2091633918596145e-7, + "velocityX": 3.352784007610117, + "velocityY": -1.7498649644588975, + "timestamp": 6.358708398106965 + }, + { + "x": 4.941997186751783, + "y": 4.50329577732449, + "heading": 0.043132062254893876, + "angularVelocity": -2.1891838085258928e-7, + "velocityX": 3.4200511269465594, + "velocityY": -1.6144467462101488, + "timestamp": 6.393370459821451 + }, + { + "x": 5.061738167339899, + "y": 4.449940508624331, + "heading": 0.04313205468599839, + "angularVelocity": -2.1836252987087912e-7, + "velocityX": 3.454525630195652, + "velocityY": -1.5392987624235874, + "timestamp": 6.428032521535937 + }, + { + "x": 5.18147938835374, + "y": 4.396585779495745, + "heading": 0.04313204711710543, + "angularVelocity": -2.1836245696155624e-7, + "velocityX": 3.4545325664744473, + "velocityY": -1.53928319579118, + "timestamp": 6.462694583250423 + }, + { + "x": 5.30122060959038, + "y": 4.343231050867178, + "heading": 0.043132039548212486, + "angularVelocity": -2.1836245655235923e-7, + "velocityX": 3.454532572902229, + "velocityY": -1.539283181365645, + "timestamp": 6.497356644964909 + }, + { + "x": 5.420963132245964, + "y": 4.289879243040779, + "heading": 0.043132031979322576, + "angularVelocity": -2.1836236883930228e-7, + "velocityX": 3.4545701188207345, + "velocityY": -1.5391989162636184, + "timestamp": 6.532018706679395 + }, + { + "x": 5.54252534628876, + "y": 4.240814844116505, + "heading": 0.04313202440333319, + "angularVelocity": -2.185671886990838e-7, + "velocityX": 3.507068190118379, + "velocityY": -1.4155072288665276, + "timestamp": 6.566680768393881 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.043132016771197046, + "angularVelocity": -2.201870219655876e-7, + "velocityX": 3.560849425193226, + "velocityY": -1.27418183966834, + "timestamp": 6.601342830108367 + }, + { + "x": 5.795862778757906, + "y": 4.156160077246944, + "heading": 0.04313200930767993, + "angularVelocity": -2.0743572849750813e-7, + "velocityX": 3.6106560546560362, + "velocityY": -1.1253226214863992, + "timestamp": 6.637322731813372 + }, + { + "x": 5.927341987568784, + "y": 4.121096456952301, + "heading": 0.043132001920743686, + "angularVelocity": -2.053072936698873e-7, + "velocityX": 3.65424035587593, + "velocityY": -0.9745335210230772, + "timestamp": 6.673302633518377 + }, + { + "x": 6.059882853366221, + "y": 4.090288037380679, + "heading": 0.04313199455852331, + "angularVelocity": -2.0462035814218294e-7, + "velocityX": 3.683747301038337, + "velocityY": -0.8562674746645255, + "timestamp": 6.709282535223382 + }, + { + "x": 6.1932695032956575, + "y": 4.063376303154627, + "heading": 0.04313198718639407, + "angularVelocity": -2.0489575822100242e-7, + "velocityX": 3.70725442840443, + "velocityY": -0.7479657517326737, + "timestamp": 6.745262436928387 + }, + { + "x": 6.327657938761633, + "y": 4.042022422624805, + "heading": 0.0431319797584895, + "angularVelocity": -2.0644593838934648e-7, + "velocityX": 3.7350973487312924, + "velocityY": -0.593494687809322, + "timestamp": 6.781242338633392 + }, + { + "x": 6.462816723725205, + "y": 4.02626353633582, + "heading": 0.04313197070248067, + "angularVelocity": -2.5169631946321463e-7, + "velocityX": 3.7565078990966625, + "velocityY": -0.437991365796106, + "timestamp": 6.817222240338397 + }, + { + "x": 6.597942968053082, + "y": 4.016131413139355, + "heading": 0.041078551168148146, + "angularVelocity": -0.057071293611868974, + "velocityX": 3.7556034876292976, + "velocityY": -0.28160508273582235, + "timestamp": 6.853202142043402 + }, + { + "x": 6.728400948341844, + "y": 4.008819261956334, + "heading": 0.03307669986160184, + "angularVelocity": -0.2223978089810408, + "velocityX": 3.625857050927263, + "velocityY": -0.20322877041112014, + "timestamp": 6.8891820437484075 + }, + { + "x": 6.853599758247463, + "y": 4.003544549173221, + "heading": 0.023873515901943563, + "angularVelocity": -0.25578680106227425, + "velocityX": 3.479687380252149, + "velocityY": -0.146601645172889, + "timestamp": 6.925161945453413 + }, + { + "x": 6.973435569834727, + "y": 4.000040202670832, + "heading": 0.014893371807540221, + "angularVelocity": -0.24958778842784254, + "velocityX": 3.3306319892084315, + "velocityY": -0.09739733396497403, + "timestamp": 6.961141847158418 + }, + { + "x": 7.087871539921083, + "y": 3.9981763906039123, + "heading": 0.00680556839180941, + "angularVelocity": -0.22478670125454425, + "velocityX": 3.1805526047459085, + "velocityY": -0.05180147745263738, + "timestamp": 6.997121748863423 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 2.7190146206752155e-27, + "angularVelocity": -0.18914916576502855, + "velocityX": 3.0299787445803052, + "velocityY": -0.008330941864390906, + "timestamp": 7.033101650568428 + }, + { + "x": 7.373371859473657, + "y": 4.00211797408274, + "heading": -0.007235572451152828, + "angularVelocity": -0.11324239038273202, + "velocityX": 2.7620815980432543, + "velocityY": 0.06638014407023476, + "timestamp": 7.096996208310439 + }, + { + "x": 7.5322864788781265, + "y": 4.0091810283968865, + "heading": -0.011195506650135086, + "angularVelocity": -0.061976079636882384, + "velocityX": 2.4871385767489596, + "velocityY": 0.1105423460737418, + "timestamp": 7.16089076605245 + }, + { + "x": 7.673469429892467, + "y": 4.017826933148271, + "heading": -0.012870709573510192, + "angularVelocity": -0.02621824115504676, + "velocityX": 2.209624043168106, + "velocityY": 0.13531519830364583, + "timestamp": 7.224785323794461 + }, + { + "x": 7.796860934793418, + "y": 4.0272116407494, + "heading": -0.012933719915114673, + "angularVelocity": -0.0009861613231427384, + "velocityX": 1.9311739412795161, + "velocityY": 0.14687804302553767, + "timestamp": 7.288679881536472 + }, + { + "x": 7.9024438065227125, + "y": 4.036727062241868, + "heading": -0.011868644520280696, + "angularVelocity": 0.016669266248535024, + "velocityX": 1.652454848433405, + "velocityY": 0.14892381806427465, + "timestamp": 7.3525744392784835 + }, + { + "x": 7.990219348757821, + "y": 4.045915777689649, + "heading": -0.01003983054789743, + "angularVelocity": 0.02862237469061955, + "velocityX": 1.3737561591633758, + "velocityY": 0.14381061192852837, + "timestamp": 7.416468997020495 + }, + { + "x": 8.060197028820047, + "y": 4.054421881048936, + "heading": -0.007731072178165808, + "angularVelocity": 0.03613388137145833, + "velocityX": 1.095205640905745, + "velocityY": 0.13312719674235798, + "timestamp": 7.480363554762506 + }, + { + "x": 8.112389742702678, + "y": 4.061960888187355, + "heading": -0.005169493803012325, + "angularVelocity": 0.04009071297584442, + "velocityX": 0.8168569550691795, + "velocityY": 0.11799138150167889, + "timestamp": 7.544258112504517 + }, + { + "x": 8.146811548767705, + "y": 4.068300369491041, + "heading": -0.0025408878888210154, + "angularVelocity": 0.04113974659320609, + "velocityX": 0.5387282936367062, + "velocityY": 0.09921786029546131, + "timestamp": 7.608152670246528 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -6.2580090272333915e-27, + "angularVelocity": 0.039766890618140496, + "velocityX": 0.26082055062629916, + "velocityY": 0.07741796101811332, + "timestamp": 7.672047227988539 + }, + { + "x": 8.160341709324497, + "y": 4.076804968421738, + "heading": 0.002530252207813134, + "angularVelocity": 0.03600456006800818, + "velocityX": -0.044607809890755405, + "velocityY": 0.050629212451328946, + "timestamp": 7.742323109794449 + }, + { + "x": 8.135735380743824, + "y": 4.078564680386853, + "heading": 0.0047972294947691944, + "angularVelocity": 0.03225825459176822, + "velocityX": -0.3501390227821203, + "velocityY": 0.025040055277777168, + "timestamp": 7.812598991600359 + }, + { + "x": 8.089649471280776, + "y": 4.078625632726097, + "heading": 0.006802259736664781, + "angularVelocity": 0.02853084430065403, + "velocityX": -0.6557855736386108, + "velocityY": 0.0008673294119977214, + "timestamp": 7.882874873406268 + }, + { + "x": 8.022074877026332, + "y": 4.077107138786556, + "heading": 0.008546934575106793, + "angularVelocity": 0.024826082485318403, + "velocityX": -0.9615616697784329, + "velocityY": -0.02160761132439043, + "timestamp": 7.953150755212178 + }, + { + "x": 7.933001389840782, + "y": 4.074154809915227, + "heading": 0.010033199259775565, + "angularVelocity": 0.02114900085883774, + "velocityX": -1.2674830239990063, + "velocityY": -0.04201055604656103, + "timestamp": 8.023426637018089 + }, + { + "x": 7.822417663549702, + "y": 4.069950298090805, + "heading": 0.0112634877661819, + "angularVelocity": 0.017506553810369533, + "velocityX": -1.573565830116407, + "velocityY": -0.05982865979589635, + "timestamp": 8.093702518824 + }, + { + "x": 7.690311402375729, + "y": 4.064726507930881, + "heading": 0.012240935764081735, + "angularVelocity": 0.013908726191431891, + "velocityX": -1.8798236006319884, + "velocityY": -0.07433261633559884, + "timestamp": 8.16397840062991 + }, + { + "x": 7.536670182325497, + "y": 4.058792719262719, + "heading": 0.012969736698331166, + "angularVelocity": 0.010370569753393549, + "velocityX": -2.186258160012313, + "velocityY": -0.08443563446915123, + "timestamp": 8.23425428243582 + }, + { + "x": 7.361484114735934, + "y": 4.052579188685834, + "heading": 0.013455784581129835, + "angularVelocity": 0.006916282945279149, + "velocityX": -2.4928334314380707, + "velocityY": -0.08841625913776113, + "timestamp": 8.304530164241731 + }, + { + "x": 7.1647543643759946, + "y": 4.04672437363507, + "heading": 0.013707961220853816, + "angularVelocity": 0.0035883810098669257, + "velocityX": -2.7993921286291723, + "velocityY": -0.08331186888461464, + "timestamp": 8.374806046047642 + }, + { + "x": 6.946523514994072, + "y": 4.042270292544886, + "heading": 0.013741127121785043, + "angularVelocity": 0.000471938595133185, + "velocityX": -3.1053448747130243, + "velocityY": -0.06337993883142518, + "timestamp": 8.445081927853552 + }, + { + "x": 6.707013005959754, + "y": 4.041197936309267, + "heading": 0.013584877310938924, + "angularVelocity": -0.0022233774494306753, + "velocityX": -3.408146619857499, + "velocityY": -0.015259235573605845, + "timestamp": 8.515357809659463 + }, + { + "x": 6.44761461575628, + "y": 4.04841614446901, + "heading": 0.013321799367880745, + "angularVelocity": -0.00374350255447176, + "velocityX": -3.6911438681038233, + "velocityY": 0.10271245232721735, + "timestamp": 8.585633691465373 + }, + { + "x": 6.183335125687841, + "y": 4.076620833220385, + "heading": 0.013321798780147703, + "angularVelocity": -8.363225426855376e-9, + "velocityX": -3.7606001273428835, + "velocityY": 0.4013423670623127, + "timestamp": 8.655909573271284 + }, + { + "x": 5.922208761336389, + "y": 4.126140293981494, + "heading": 0.013321798506864218, + "angularVelocity": -3.888723659827553e-9, + "velocityX": -3.715732305894624, + "velocityY": 0.70464374816203, + "timestamp": 8.726185455077195 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.013321798215407589, + "angularVelocity": -4.14732085648961e-9, + "velocityX": -3.6464435013896566, + "velocityY": 1.0033140639584306, + "timestamp": 8.796461336883105 + }, + { + "x": 5.541094042237222, + "y": 4.236435082517705, + "heading": 0.013321797912516515, + "angularVelocity": -8.741536718768107e-9, + "velocityX": -3.6034342058048336, + "velocityY": 1.1482373727249326, + "timestamp": 8.831110973716337 + }, + { + "x": 5.417926089652793, + "y": 4.28117906555146, + "heading": 0.013321797636094018, + "angularVelocity": -7.977644916262235e-9, + "velocityX": -3.554667922704023, + "velocityY": 1.2913261760609271, + "timestamp": 8.865760610549568 + }, + { + "x": 5.2966446591636975, + "y": 4.330809533782675, + "heading": 0.013321797380978714, + "angularVelocity": -7.362712230518396e-9, + "velocityX": -3.50022226994255, + "velocityY": 1.4323517579733251, + "timestamp": 8.9004102473828 + }, + { + "x": 5.177443524299453, + "y": 4.385247189609289, + "heading": 0.013321797143113006, + "angularVelocity": -6.86488311115801e-9, + "velocityX": -3.440184248913247, + "velocityY": 1.5710887848153525, + "timestamp": 8.935059884216031 + }, + { + "x": 5.060513134161878, + "y": 4.444405054072411, + "heading": 0.013321796919246185, + "angularVelocity": -6.460870636805561e-9, + "velocityX": -3.374649803701036, + "velocityY": 1.7073155700837472, + "timestamp": 8.969709521049262 + }, + { + "x": 4.946040308560799, + "y": 4.508188605110022, + "heading": 0.013321796706722, + "angularVelocity": -6.133518370810969e-9, + "velocityX": -3.3037236768754394, + "velocityY": 1.8408144173228282, + "timestamp": 9.004359157882494 + }, + { + "x": 4.83420793799034, + "y": 4.576495926647655, + "heading": 0.013321796503327139, + "angularVelocity": -5.870043129182345e-9, + "velocityX": -3.2275192697894797, + "velocityY": 1.9713719328833563, + "timestamp": 9.039008794715725 + }, + { + "x": 4.725194685566166, + "y": 4.649217863919521, + "heading": 0.01332179630717999, + "angularVelocity": -5.660871666352306e-9, + "velocityX": -3.146158585986232, + "velocityY": 2.098779205735374, + "timestamp": 9.073658431548957 + }, + { + "x": 4.61917463890979, + "y": 4.726238115287908, + "heading": 0.013321796115042023, + "angularVelocity": -5.545165384203739e-9, + "velocityX": -3.0597736757430263, + "velocityY": 2.2228299747869245, + "timestamp": 9.108308068382188 + }, + { + "x": 4.5101613609880875, + "y": 4.798960014309373, + "heading": 0.013321795918491775, + "angularVelocity": -5.6725052693856055e-9, + "velocityX": -3.146159321853369, + "velocityY": 2.098778101816108, + "timestamp": 9.14295770521542 + }, + { + "x": 4.398328964470022, + "y": 4.867267293334104, + "heading": 0.01332179571466789, + "angularVelocity": -5.882424857258076e-9, + "velocityX": -3.2275200186459836, + "velocityY": 1.971370705946904, + "timestamp": 9.17760734204865 + }, + { + "x": 4.283856115908692, + "y": 4.93105079879567, + "heading": 0.013321794090551628, + "angularVelocity": -4.687253345471844e-8, + "velocityX": -3.3037243395158984, + "velocityY": 1.8408131019830805, + "timestamp": 9.212256978881882 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.012938317898929116, + "angularVelocity": -0.01106724995324455, + "velocityX": -3.3581030320620826, + "velocityY": 1.6997868507459994, + "timestamp": 9.246906615715114 + }, + { + "x": 3.941212702918835, + "y": 5.086810099235691, + "heading": 0.011498426607566257, + "angularVelocity": -0.020926455047297343, + "velocityX": -3.2887006266835073, + "velocityY": 1.4077344931260583, + "timestamp": 9.315713840259761 + }, + { + "x": 3.730319373177593, + "y": 5.1698990492176655, + "heading": 0.009891204024216577, + "angularVelocity": -0.02335834055196905, + "velocityX": -3.0649881772864958, + "velocityY": 1.2075614229732525, + "timestamp": 9.384521064804408 + }, + { + "x": 3.537102784143612, + "y": 5.242302243244861, + "heading": 0.008284119125689052, + "angularVelocity": -0.023356339529212095, + "velocityX": -2.8080857833265287, + "velocityY": 1.0522615104205277, + "timestamp": 9.453328289349056 + }, + { + "x": 3.3623897849801216, + "y": 5.305525598269376, + "heading": 0.006750375983753274, + "angularVelocity": -0.02229043755340787, + "velocityX": -2.5391664947933337, + "velocityY": 0.9188476274535745, + "timestamp": 9.522135513893703 + }, + { + "x": 3.2065870996303723, + "y": 5.360439206716265, + "heading": 0.005330250476746595, + "angularVelocity": -0.020639191834938647, + "velocityX": -2.2643361417470387, + "velocityY": 0.7980791088478895, + "timestamp": 9.59094273843835 + }, + { + "x": 3.0699322051681737, + "y": 5.407605971353294, + "heading": 0.004049089893736221, + "angularVelocity": -0.01861956490018075, + "velocityX": -1.98605444946448, + "velocityY": 0.6854914574620572, + "timestamp": 9.659749962982998 + }, + { + "x": 2.9525794175973394, + "y": 5.4474188354837905, + "heading": 0.002924278144664975, + "angularVelocity": -0.01634729138567971, + "velocityX": -1.7055300275145737, + "velocityY": 0.5786145916213083, + "timestamp": 9.728557187527645 + }, + { + "x": 2.8546365555421396, + "y": 5.480167300776495, + "heading": 0.001968466770060391, + "angularVelocity": -0.013891148508459003, + "velocityX": -1.4234386389418605, + "velocityY": 0.47594515705912777, + "timestamp": 9.797364412072293 + }, + { + "x": 2.776182983656432, + "y": 5.506073375448153, + "heading": 0.0011912715035962884, + "angularVelocity": -0.011295256735138296, + "velocityX": -1.140193815473562, + "velocityY": 0.3765022473017808, + "timestamp": 9.86617163661694 + }, + { + "x": 2.717279457569953, + "y": 5.525312645366718, + "heading": 0.0006002466315783451, + "angularVelocity": -0.008589575817499225, + "velocityX": -0.8560660087118843, + "velocityY": 0.2796111897534987, + "timestamp": 9.934978861161587 + }, + { + "x": 2.6779739270458034, + "y": 5.538027422315637, + "heading": 0.0002014837935087551, + "angularVelocity": -0.0057953629245841975, + "velocityX": -0.5712413308960773, + "velocityY": 0.18478840024522195, + "timestamp": 10.003786085706235 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 2.777517455024623e-28, + "angularVelocity": -0.0029282360223382743, + "velocityX": -0.28585310661942626, + "velocityY": 0.0916755910664572, + "timestamp": 10.072593310250882 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 1.7679062604492033e-28, + "angularVelocity": -4.39985022540959e-29, + "velocityX": -3.674462511689619e-27, + "velocityY": -3.4538031779033175e-27, + "timestamp": 10.14140053479553 + }, + { + "x": 2.6750735377327333, + "y": 5.538716932149931, + "heading": -1.742535482636501e-18, + "angularVelocity": -2.7369611075472657e-17, + "velocityX": 0.2633769919204766, + "velocityY": -0.08824745983951204, + "timestamp": 10.20506733218207 + }, + { + "x": 2.7085920132371135, + "y": 5.52742584903886, + "heading": -8.352558789144994e-18, + "angularVelocity": -1.0382214243405793e-16, + "velocityX": 0.526467120701534, + "velocityY": -0.17734649102137234, + "timestamp": 10.26873412956861 + }, + { + "x": 2.758838975509705, + "y": 5.510398675567148, + "heading": -1.408942364356953e-17, + "angularVelocity": -9.01076399315566e-17, + "velocityX": 0.7892176822956318, + "velocityY": -0.26744196615285204, + "timestamp": 10.33240092695515 + }, + { + "x": 2.8257884382416396, + "y": 5.4875601790142925, + "heading": -1.8320735772591408e-17, + "angularVelocity": -6.646026347783005e-17, + "velocityX": 1.0515600828083749, + "velocityY": -0.35871910462523465, + "timestamp": 10.396067724341691 + }, + { + "x": 2.9094085873516815, + "y": 5.458819716039827, + "heading": -2.1055365951456844e-17, + "angularVelocity": -4.295221828639974e-17, + "velocityX": 1.3134027867360427, + "velocityY": -0.4514199575639651, + "timestamp": 10.459734521728231 + }, + { + "x": 3.0096595933993924, + "y": 5.424065969220243, + "heading": -2.2717586087398788e-17, + "angularVelocity": -2.6108116070725826e-17, + "velocityX": 1.5746198986428648, + "velocityY": -0.5458692481197496, + "timestamp": 10.523401319114772 + }, + { + "x": 3.126490185266761, + "y": 5.383158986628498, + "heading": -2.29080313224255e-17, + "angularVelocity": -2.991280273573379e-18, + "velocityX": 1.83503170668463, + "velocityY": -0.642516731969203, + "timestamp": 10.587068116501312 + }, + { + "x": 3.2598319755407235, + "y": 5.335917551231157, + "heading": -1.862517634661648e-17, + "angularVelocity": 6.726983532294288e-17, + "velocityX": 2.0943693690826515, + "velocityY": -0.7420105508138521, + "timestamp": 10.650734913887852 + }, + { + "x": 3.4095893646013797, + "y": 5.282097889138944, + "heading": -1.2913797720128003e-17, + "angularVelocity": 8.970733350596672e-17, + "velocityX": 2.3522054698531947, + "velocityY": -0.8453332710526835, + "timestamp": 10.714401711274393 + }, + { + "x": 3.575619784494897, + "y": 5.221354832224988, + "heading": -5.8099440020816116e-18, + "angularVelocity": 1.1157862511662809e-16, + "velocityX": 2.607802287988438, + "velocityY": -0.9540774690639187, + "timestamp": 10.778068508660933 + }, + { + "x": 3.7576895466655613, + "y": 5.153162990546568, + "heading": 2.5414927254501756e-18, + "angularVelocity": 1.3117412953490323e-16, + "velocityX": 2.859728612784813, + "velocityY": -1.0710738481850424, + "timestamp": 10.841735306047473 + }, + { + "x": 3.955353492285648, + "y": 5.076630508571263, + "heading": 1.2126107416115352e-17, + "angularVelocity": 1.5054337714518245e-16, + "velocityX": 3.1046629284650207, + "velocityY": -1.202078400624624, + "timestamp": 10.905402103434014 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 1.991506320539766e-17, + "angularVelocity": 1.2233936853955328e-16, + "velocityX": 3.332122579144681, + "velocityY": -1.3615057810598787, + "timestamp": 10.969068900820554 + }, + { + "x": 4.285701420717343, + "y": 4.93838888948332, + "heading": 2.4571932720973296e-17, + "angularVelocity": 1.3447790808928575e-16, + "velocityX": 3.4133671603880513, + "velocityY": -1.488883004107591, + "timestamp": 11.00369815391993 + }, + { + "x": 4.403715683241863, + "y": 4.881601537504843, + "heading": 2.682063176544399e-17, + "angularVelocity": 6.493640038785633e-17, + "velocityX": 3.407935544721574, + "velocityY": -1.6398664971350723, + "timestamp": 11.038327407019304 + }, + { + "x": 4.5193677097341265, + "y": 4.820146027732668, + "heading": 1.2264799155735384e-17, + "angularVelocity": -4.203334264259107e-16, + "velocityX": 3.339720500479131, + "velocityY": -1.7746703804386716, + "timestamp": 11.07295666011868 + }, + { + "x": 4.632472949186873, + "y": 4.754120458704841, + "heading": 9.89855142264587e-19, + "angularVelocity": -3.255901587362041e-16, + "velocityX": 3.2661761178668676, + "velocityY": -1.90664144093305, + "timestamp": 11.107585913218054 + }, + { + "x": 4.742850945772244, + "y": 4.683630254723833, + "heading": -1.0397818320257784e-17, + "angularVelocity": -3.288454830368281e-16, + "velocityX": 3.1874206546881325, + "velocityY": -2.035568130179495, + "timestamp": 11.14221516631743 + }, + { + "x": 4.850325734395693, + "y": 4.6087881488265605, + "heading": -1.7363492612518904e-17, + "angularVelocity": -2.0115000090418594e-16, + "velocityX": 3.1035837912827686, + "velocityY": -2.161239391519611, + "timestamp": 11.176844419416804 + }, + { + "x": 4.959315990041341, + "y": 4.536170691050833, + "heading": -2.5306246706392577e-17, + "angularVelocity": -2.29365446344471e-16, + "velocityX": 3.147346416420796, + "velocityY": -2.0969975173111073, + "timestamp": 11.21147367251618 + }, + { + "x": 5.071119485305154, + "y": 4.467964024057391, + "heading": -3.478725650331535e-17, + "angularVelocity": -2.7378614749218265e-16, + "velocityX": 3.228585235233696, + "velocityY": -1.9696257033817701, + "timestamp": 11.246102925615554 + }, + { + "x": 5.185557902331023, + "y": 4.404277136498651, + "heading": -4.439822175932155e-17, + "angularVelocity": -2.7753891278544145e-16, + "velocityX": 3.3046747123729983, + "velocityY": -1.839106589332955, + "timestamp": 11.28073217871493 + }, + { + "x": 5.302448643272519, + "y": 4.345211701858586, + "heading": -5.347328989806064e-17, + "angularVelocity": -2.62063640602752e-16, + "velocityX": 3.375491253191483, + "velocityY": -1.7056514176198097, + "timestamp": 11.315361431814305 + }, + { + "x": 5.421605180282958, + "y": 4.290861996168768, + "heading": -6.205824789945856e-17, + "angularVelocity": -2.4791057365365614e-16, + "velocityX": 3.440921369816929, + "velocityY": -1.5694738068375724, + "timestamp": 11.34999068491368 + }, + { + "x": 5.542837363816495, + "y": 4.241314761996146, + "heading": -6.772617298051042e-17, + "angularVelocity": -1.6367448251072389e-16, + "velocityX": 3.500860477286019, + "velocityY": -1.4307913032498076, + "timestamp": 11.384619938013055 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -7.009195303985871e-17, + "angularVelocity": -6.831738623783852e-17, + "velocityX": 3.555212832659327, + "velocityY": -1.2898253194640328, + "timestamp": 11.41924919111243 + }, + { + "x": 5.93427243169133, + "y": 4.124338107174554, + "heading": -5.732543809529257e-17, + "angularVelocity": 1.7374415360495528e-16, + "velocityX": 3.651674213100421, + "velocityY": -0.9841063029448727, + "timestamp": 11.492728008556748 + }, + { + "x": 6.207753086191834, + "y": 4.075010553952499, + "heading": -4.033997496139901e-17, + "angularVelocity": 2.3116135676628954e-16, + "velocityX": 3.7218978749589717, + "velocityY": -0.6713166452282138, + "timestamp": 11.566206826001066 + }, + { + "x": 6.484428721197197, + "y": 4.049020784065838, + "heading": -4.165967730952468e-17, + "angularVelocity": -1.796031011340145e-17, + "velocityX": 3.7653795288013057, + "velocityY": -0.35370424825299146, + "timestamp": 11.639685643445384 + }, + { + "x": 6.743277525423136, + "y": 4.038427876582129, + "heading": -4.683972332322173e-17, + "angularVelocity": -7.04971336473422e-17, + "velocityX": 3.5227676931803376, + "velocityY": -0.1441627376724693, + "timestamp": 11.713164460889702 + }, + { + "x": 6.9786729860111185, + "y": 4.030026934106214, + "heading": -5.042587996000917e-17, + "angularVelocity": -4.880531235316479e-17, + "velocityX": 3.2035825939409506, + "velocityY": -0.11433148719739977, + "timestamp": 11.78664327833402 + }, + { + "x": 7.190552838965192, + "y": 4.022997315975448, + "heading": -4.990000307378321e-17, + "angularVelocity": 7.15685015890218e-18, + "velocityX": 2.8835501212936054, + "velocityY": -0.09566863451624368, + "timestamp": 11.860122095778339 + }, + { + "x": 7.37890188057172, + "y": 4.017047213862616, + "heading": -4.8467877227374554e-17, + "angularVelocity": 1.949032246701928e-17, + "velocityX": 2.563310735767617, + "velocityY": -0.08097710768603945, + "timestamp": 11.933600913222657 + }, + { + "x": 7.5437137264303615, + "y": 4.012027115751869, + "heading": -4.529378389429077e-17, + "angularVelocity": 4.3197392712062346e-17, + "velocityX": 2.2429844626110818, + "velocityY": -0.06832034435708055, + "timestamp": 12.007079730666975 + }, + { + "x": 7.684984960485698, + "y": 4.007846136006195, + "heading": -4.052616415520898e-17, + "angularVelocity": 6.488427420284795e-17, + "velocityX": 1.9226116991116777, + "velocityY": -0.05690047677812232, + "timestamp": 12.080558548111293 + }, + { + "x": 7.802713484144924, + "y": 4.004443187628857, + "heading": -3.38523635240434e-17, + "angularVelocity": 9.082618451636533e-17, + "velocityX": 1.602210375098112, + "velocityY": -0.046311964395964134, + "timestamp": 12.154037365555611 + }, + { + "x": 7.896897887923302, + "y": 4.001774392077607, + "heading": -2.5504868250570688e-17, + "angularVelocity": 1.1360410474589884e-16, + "velocityX": 1.2817898688931846, + "velocityY": -0.03632061108321556, + "timestamp": 12.22751618299993 + }, + { + "x": 7.967537164414649, + "y": 3.9998067058039015, + "heading": -1.5265763382971852e-17, + "angularVelocity": 1.393477089561816e-16, + "velocityX": 0.9613556525304345, + "velocityY": -0.026778959462655655, + "timestamp": 12.300995000444248 + }, + { + "x": 8.014630559872813, + "y": 3.998514348187002, + "heading": -8.61347718056724e-18, + "angularVelocity": 9.053338681616732e-17, + "velocityX": 0.640911178161685, + "velocityY": -0.017588165703384288, + "timestamp": 12.374473817888566 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 1.5743578769011647e-28, + "angularVelocity": 1.1722394943724176e-16, + "velocityX": 0.3204587550610229, + "velocityY": -0.008678746809770337, + "timestamp": 12.447952635332884 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 7.635851844630149e-29, + "angularVelocity": -6.421919896967874e-29, + "velocityX": 1.2273507247615808e-27, + "velocityY": -1.4201614247601614e-26, + "timestamp": 12.521431452777202 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj index 602aa0e1..fbaf4332 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.1.traj @@ -1,310 +1,311 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -1.254857983919809e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.3060574276601378, - "y": 5.572382522924401, - "heading": -0.010694878831235951, - "angularVelocity": -0.1421235514303064, - "velocityX": 0.21372563329485256, - "velocityY": -0.24679919125306551, - "timestamp": 0.07525057405130896 - }, - { - "x": 1.3382235783773917, - "y": 5.535239267164631, - "heading": -0.032076102097718266, - "angularVelocity": -0.2841336898257091, - "velocityX": 0.4274538915144429, - "velocityY": -0.4935943177620345, - "timestamp": 0.15050114810261792 - }, - { - "x": 1.3864734186078633, - "y": 5.479524852099925, - "heading": -0.06412157270911513, - "angularVelocity": -0.42585018142040665, - "velocityX": 0.6411889987319064, - "velocityY": -0.7403852497811617, - "timestamp": 0.22575172215392686 - }, - { - "x": 1.4508078150986874, - "y": 5.405239694992128, - "heading": -0.1067981371766019, - "angularVelocity": -0.567126098451675, - "velocityX": 0.8549356240955547, - "velocityY": -0.9871706368349642, - "timestamp": 0.30100229620523583 - }, - { - "x": 1.5312280234732587, - "y": 5.3123844378954015, - "heading": -0.16006527765002243, - "angularVelocity": -0.7078635763993126, - "velocityX": 1.068698935356638, - "velocityY": -1.233947491673002, - "timestamp": 0.3762528702565448 - }, - { - "x": 1.6277357530197398, - "y": 5.200960116982037, - "heading": -0.2238798805982425, - "angularVelocity": -0.8480281214272752, - "velocityX": 1.2824849612365503, - "velocityY": -1.4807105768852864, - "timestamp": 0.4515034443078538 - }, - { - "x": 1.7403333929369436, - "y": 5.0709684768538175, - "heading": -0.2982015141504559, - "angularVelocity": -0.9876553699462992, - "velocityX": 1.4963027370230912, - "velocityY": -1.727450478198834, - "timestamp": 0.5267540183591627 - }, - { - "x": 1.8690253968085462, - "y": 4.922413250022354, - "heading": -0.3829965804922361, - "angularVelocity": -1.1268361392905244, - "velocityX": 1.7101796961898603, - "velocityY": -1.9741407784113794, - "timestamp": 0.6020045924104717 - }, - { - "x": 2.02184146077192, - "y": 4.778613992669738, - "heading": -0.4624358186091319, - "angularVelocity": -1.0556628851958798, - "velocityX": 2.0307627668195027, - "velocityY": -1.9109390082922875, - "timestamp": 0.6772551664617806 - }, - { - "x": 2.158573003278503, - "y": 4.653386193108499, - "heading": -0.5314192813425986, - "angularVelocity": -0.9167167639888999, - "velocityX": 1.817016603947309, - "velocityY": -1.664144109715068, - "timestamp": 0.7525057405130896 - }, - { - "x": 2.279216333070278, - "y": 4.546726509082428, - "heading": -0.5899522323979595, - "angularVelocity": -0.7778405918177544, - "velocityX": 1.6032213881911423, - "velocityY": -1.417393626160111, - "timestamp": 0.8277563145643986 - }, - { - "x": 2.3837699497753926, - "y": 4.458633448495562, - "heading": -0.6380354012157764, - "angularVelocity": -0.6389741131339646, - "velocityX": 1.3894062340921998, - "velocityY": -1.1706629709704055, - "timestamp": 0.9030068886157075 - }, - { - "x": 2.4722329980367945, - "y": 4.3891060989240405, - "heading": -0.6756677941957627, - "angularVelocity": -0.5000944305598923, - "velocityX": 1.1755797132095953, - "velocityY": -0.9239444409216138, - "timestamp": 0.9782574626670165 - }, - { - "x": 2.5446049418017402, - "y": 4.338143858296765, - "heading": -0.7028477816482255, - "angularVelocity": -0.36119309114484527, - "velocityX": 0.9617460687572824, - "velocityY": -0.6772339117562157, - "timestamp": 1.0535080367183254 - }, - { - "x": 2.6008854517223408, - "y": 4.305746335795738, - "heading": -0.7195735469228228, - "angularVelocity": -0.22226761038098528, - "velocityX": 0.7479080476225236, - "velocityY": -0.430528576145586, - "timestamp": 1.1287586107696344 - }, - { - "x": 2.641074357140525, - "y": 4.291913306081633, - "heading": -0.7258432570432893, - "angularVelocity": -0.08331777131422959, - "velocityX": 0.5340677586239448, - "velocityY": -0.1838262350545031, - "timestamp": 1.2040091848209433 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.05565594681973357, - "velocityX": 0.3202270068209183, - "velocityY": 0.06287502296377193, - "timestamp": 1.2792597588722523 - }, - { - "x": 2.6731092983432756, - "y": 4.320288362117908, - "heading": -0.7067987769295019, - "angularVelocity": 0.19580389973285955, - "velocityX": 0.10461714888899723, - "velocityY": 0.3116194322188291, - "timestamp": 1.3551333149479476 - }, - { - "x": 2.664687958259504, - "y": 4.362805441582899, - "heading": -0.6813130909046112, - "angularVelocity": 0.3358968175826016, - "velocityX": -0.11099176735530734, - "velocityY": 0.5603675597065643, - "timestamp": 1.431006871023643 - }, - { - "x": 2.6399077508131867, - "y": 4.424196327740775, - "heading": -0.6452021974872086, - "angularVelocity": 0.4759351648285144, - "velocityX": -0.32659873516708793, - "velocityY": 0.8091209814393522, - "timestamp": 1.5068804270993383 - }, - { - "x": 2.5987688991007616, - "y": 4.504461543612991, - "heading": -0.5984695371274339, - "angularVelocity": 0.6159281675522111, - "velocityX": -0.5422027626077326, - "velocityY": 1.057881296501775, - "timestamp": 1.5827539831750337 - }, - { - "x": 2.541271696403367, - "y": 4.60360171291691, - "heading": -0.5411163147183906, - "angularVelocity": 0.7559052899984986, - "velocityX": -0.7578029246555089, - "velocityY": 1.3066498320479574, - "timestamp": 1.658627539250729 - }, - { - "x": 2.46741646400873, - "y": 4.721617471023204, - "heading": -0.47313851139747326, - "angularVelocity": 0.8959353803401775, - "velocityX": -0.9733988521888661, - "velocityY": 1.55542674166613, - "timestamp": 1.7345010953264244 - }, - { - "x": 2.3772033191525277, - "y": 4.85850915554647, - "heading": -0.39452182857786383, - "angularVelocity": 1.036153923517168, - "velocityX": -1.1889932345783751, - "velocityY": 1.8042081009834092, - "timestamp": 1.8103746514021197 - }, - { - "x": 2.2706306867395725, - "y": 5.014275344454255, - "heading": -0.30523454109050485, - "angularVelocity": 1.1767905987884617, - "velocityX": -1.404608376447531, - "velocityY": 2.0529707181614056, - "timestamp": 1.886248207477815 - }, - { - "x": 2.1562102192291532, - "y": 5.146460354029385, - "heading": -0.2291038294161038, - "angularVelocity": 1.0033892651564627, - "velocityX": -1.5080414498275487, - "velocityY": 1.7421749607384658, - "timestamp": 1.9621217635535104 - }, - { - "x": 2.0581337319934665, - "y": 5.259758702235753, - "heading": -0.16374377227039283, - "angularVelocity": 0.8614339504706835, - "velocityX": -1.292630691201352, - "velocityY": 1.4932521166502593, - "timestamp": 2.0379953196292058 - }, - { - "x": 1.976403182773577, - "y": 5.354172844449837, - "heading": -0.1092054383318089, - "angularVelocity": 0.7188055596708355, - "velocityX": -1.077194129890758, - "velocityY": 1.244361633992147, - "timestamp": 2.113868875704901 - }, - { - "x": 1.9110187048049692, - "y": 5.429703613246766, - "heading": -0.0655341619901584, - "angularVelocity": 0.575579669661773, - "velocityX": -0.8617558125585749, - "velocityY": 0.9954821245277466, - "timestamp": 2.1897424317805965 - }, - { - "x": 1.861980199634934, - "y": 5.486351461082553, - "heading": -0.03276467227497517, - "angularVelocity": 0.4318960572088904, - "velocityX": -0.6463187928148273, - "velocityY": 0.7466085783539637, - "timestamp": 2.265615987856292 - }, - { - "x": 1.8292876463834091, - "y": 5.5241166468364415, - "heading": -0.01091805858135444, - "angularVelocity": 0.2879344902726512, - "velocityX": -0.4308820482661345, - "velocityY": 0.4977384441630725, - "timestamp": 2.341489543931987 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.72551505430432e-32, - "angularVelocity": 0.1438980739292335, - "velocityX": -0.21544334611501834, - "velocityY": 0.2488695893337634, - "timestamp": 2.4173631000076825 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": -2.2798037235511704e-30, - "angularVelocity": -3.1967802998211905e-29, - "velocityX": 0, - "velocityY": 3.158110370432563e-31, - "timestamp": 2.493236656083378 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -5.9009444568140314e-21, + "angularVelocity": -6.3286336172777184e-21, + "velocityX": -2.3802874375881527e-19, + "velocityY": -1.4090020225698018e-19, + "timestamp": 0 + }, + { + "x": 1.3060573928270223, + "y": 5.572382489770416, + "heading": -0.010694903760090106, + "angularVelocity": -0.1421238741144053, + "velocityX": 0.2137251574806739, + "velocityY": -0.24679961691155075, + "timestamp": 0.0752505786007567 + }, + { + "x": 1.3382234711320444, + "y": 5.535239165388228, + "heading": -0.03207617873440696, + "angularVelocity": -0.28413435978686086, + "velocityX": 0.42745290339416875, + "velocityY": -0.4935951998359545, + "timestamp": 0.1505011572015134 + }, + { + "x": 1.386473197712333, + "y": 5.479524643144728, + "heading": -0.06412173018225528, + "angularVelocity": -0.4258512299003913, + "velocityX": 0.6411874496855803, + "velocityY": -0.7403866293046694, + "timestamp": 0.22575173580227012 + }, + { + "x": 1.4508074341962967, + "y": 5.405239335978357, + "heading": -0.1067984078244425, + "angularVelocity": -0.5671275681295302, + "velocityX": 0.8549334460969267, + "velocityY": -0.987172571263834, + "timestamp": 0.3010023144030268 + }, + { + "x": 1.5312274285318639, + "y": 5.312383879458918, + "heading": -0.16006569861045727, + "angularVelocity": -0.707865531089473, + "velocityX": 1.0686960264083267, + "velocityY": -1.2339500671757528, + "timestamp": 0.3762528930037835 + }, + { + "x": 1.6277348772170532, + "y": 5.2009592989550075, + "heading": -0.22388049710097416, + "angularVelocity": -0.8480306686954192, + "velocityX": 1.2824811513696022, + "velocityY": -1.4807139370330336, + "timestamp": 0.4515034716045402 + }, + { + "x": 1.7403321438675061, + "y": 5.0709673174660805, + "heading": -0.29820238787936876, + "angularVelocity": -0.9876587284824841, + "velocityX": 1.4962976862668933, + "velocityY": -1.7274549100613854, + "timestamp": 0.5267540502052969 + }, + { + "x": 1.869023605292211, + "y": 4.922411602704825, + "heading": -0.3829978231491767, + "angularVelocity": -1.1268409738042233, + "velocityX": 1.7101723842877663, + "velocityY": -1.9741471430995352, + "timestamp": 0.6020046288060535 + }, + { + "x": 2.0218400866158173, + "y": 4.778612697599142, + "heading": -0.46243678265580673, + "angularVelocity": -1.0556591189563451, + "velocityX": 2.0307681902941916, + "velocityY": -1.9109342117962438, + "timestamp": 0.6772552074068102 + }, + { + "x": 2.158571908625681, + "y": 4.653385137637619, + "heading": -0.5314200564732017, + "angularVelocity": -0.9167141980846026, + "velocityX": 1.8170202083793483, + "velocityY": -1.6641408250954677, + "timestamp": 0.7525057860075669 + }, + { + "x": 2.279215456782309, + "y": 4.5467256453679665, + "heading": -0.5899528580218641, + "angularVelocity": -0.7778385580157354, + "velocityX": 1.603224193088739, + "velocityY": -1.4173909922412158, + "timestamp": 0.8277563646083236 + }, + { + "x": 2.383769256289076, + "y": 4.45863275029472, + "heading": -0.6380359000069764, + "angularVelocity": -0.6389723890392618, + "velocityX": 1.3894085793213427, + "velocityY": -1.1706607007051708, + "timestamp": 0.9030069432090803 + }, + { + "x": 2.472232464576315, + "y": 4.389105550793566, + "heading": -0.6756681804644412, + "angularVelocity": -0.500092905027661, + "velocityX": 1.1755817687008607, + "velocityY": -0.9239423907952048, + "timestamp": 0.9782575218098369 + }, + { + "x": 2.5446045532635506, + "y": 4.33814345127288, + "heading": -0.7028480646839942, + "angularVelocity": -0.36119169745865176, + "velocityX": 0.9617479364681578, + "velocityY": -0.6772319956645083, + "timestamp": 1.0535081004105937 + }, + { + "x": 2.6008851981181857, + "y": 4.305746065234092, + "heading": -0.7195737326656834, + "angularVelocity": -0.2222663040297333, + "velocityX": 0.7479097955259911, + "velocityY": -0.43052673668740377, + "timestamp": 1.1287586790113504 + }, + { + "x": 2.641074232135465, + "y": 4.291913170423193, + "heading": -0.7258433490393724, + "angularVelocity": -0.0833165204875385, + "velocityX": 0.5340694352744676, + "velocityY": -0.1838244312283474, + "timestamp": 1.204009257612107 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05565716597974986, + "velocityX": 0.3202286486370387, + "velocityY": 0.06287682191127338, + "timestamp": 1.2792598362128638 + }, + { + "x": 2.673109423896828, + "y": 4.320288496439454, + "heading": -0.7067986875650104, + "angularVelocity": 0.19580508899656304, + "velocityX": 0.10461880978884638, + "velocityY": 0.3116212207833284, + "timestamp": 1.3551333878515968 + }, + { + "x": 2.664688214670906, + "y": 4.3628057108082405, + "heading": -0.6813129138970213, + "angularVelocity": 0.3358979923509836, + "velocityX": -0.1109900491548022, + "velocityY": 0.5603693704926264, + "timestamp": 1.4310069394903298 + }, + { + "x": 2.6399081472529042, + "y": 4.424196735750699, + "heading": -0.6452019329979721, + "angularVelocity": 0.47593634565822673, + "velocityX": -0.3265969087089792, + "velocityY": 0.8091228579205605, + "timestamp": 1.5068804911290627 + }, + { + "x": 2.5987694501527234, + "y": 4.5044620989064414, + "heading": -0.5984691831252384, + "angularVelocity": 0.6159293833409661, + "velocityX": -0.542200756545013, + "velocityY": 1.05788329954391, + "timestamp": 1.5827540427677957 + }, + { + "x": 2.5412724247726852, + "y": 4.603602430919982, + "heading": -0.5411158658534186, + "angularVelocity": 0.7559065844828777, + "velocityX": -0.7578006319478658, + "velocityY": 1.3066520529527457, + "timestamp": 1.6586275944065287 + }, + { + "x": 2.467417405939932, + "y": 4.7216183787073325, + "heading": -0.473137956706953, + "angularVelocity": 0.895936827501342, + "velocityX": -0.9733960943919171, + "velocityY": 1.5554293325984934, + "timestamp": 1.7345011460452617 + }, + { + "x": 2.37720453797495, + "y": 4.858510302970022, + "heading": -0.3945211456551798, + "angularVelocity": 1.0361556741946414, + "velocityX": -1.1889896547134935, + "velocityY": 1.8042113662279782, + "timestamp": 1.8103746976839947 + }, + { + "x": 2.2706323270523923, + "y": 5.014276850913776, + "heading": -0.3052336725466659, + "angularVelocity": 1.1767931140702435, + "velocityX": -1.4046029033952228, + "velocityY": 2.052975570268153, + "timestamp": 1.8862482493227277 + }, + { + "x": 2.156211312441972, + "y": 5.146461367106831, + "heading": -0.22910323737059946, + "angularVelocity": 1.0033856796180658, + "velocityX": -1.5080487487284289, + "velocityY": 1.742168559900677, + "timestamp": 1.9621218009614607 + }, + { + "x": 2.058134458607592, + "y": 5.259759379647075, + "heading": -0.16374337318870086, + "angularVelocity": 0.8614314576060107, + "velocityX": -1.2926355985202171, + "velocityY": 1.4932477799347985, + "timestamp": 2.0379953526001935 + }, + { + "x": 1.976403642015832, + "y": 5.354173274627719, + "heading": -0.10920518354656349, + "angularVelocity": 0.7188036998955512, + "velocityX": -1.0771977168121305, + "velocityY": 1.2443584482531382, + "timestamp": 2.1138689042389265 + }, + { + "x": 1.9110189688101253, + "y": 5.429703861532151, + "heading": -0.06553401442898629, + "angularVelocity": 0.5755782901203135, + "velocityX": -0.8617584361561702, + "velocityY": 0.9954797854217321, + "timestamp": 2.1897424558776595 + }, + { + "x": 1.8619803269894504, + "y": 5.486351581272176, + "heading": -0.032764600706343905, + "angularVelocity": 0.4318950808929029, + "velocityX": -0.6463206316489477, + "velocityY": 0.7466069337268786, + "timestamp": 2.2656160075163925 + }, + { + "x": 1.829287687543546, + "y": 5.524116685802078, + "heading": -0.01091803536445235, + "angularVelocity": 0.2879338698406078, + "velocityX": -0.4308832094957455, + "velocityY": 0.4977374027489312, + "timestamp": 2.3414895591551255 + }, + { + "x": 1.812941193580627, + "y": 5.542999267578124, + "heading": 1.0528400336269664e-20, + "angularVelocity": 0.14389777634817838, + "velocityX": -0.2154439011994184, + "velocityY": 0.248869090323821, + "timestamp": 2.4173631107938585 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 4.797022054850397e-21, + "angularVelocity": -9.847000764445998e-21, + "velocityX": -1.972173923637389e-19, + "velocityY": -3.0551788776993856e-19, + "timestamp": 2.4932366624325915 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj index c607bce4..94642902 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.2.traj @@ -1,103 +1,104 @@ { - "samples": [ - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": -2.2798037235511704e-30, - "angularVelocity": -3.1967802998211905e-29, - "velocityX": 0, - "velocityY": 3.158110370432563e-31, - "timestamp": 0 - }, - { - "x": 1.8507557561824508, - "y": 5.5430527114919315, - "heading": 1.2021210509026438e-21, - "angularVelocity": 1.2912282770144636e-20, - "velocityX": 0.40617567078743805, - "velocityY": 0.0005740544395093852, - "timestamp": 0.09309903404237341 - }, - { - "x": 1.9263848800936068, - "y": 5.543159599317717, - "heading": 3.6194046492049965e-21, - "angularVelocity": 2.5964647465645482e-20, - "velocityX": 0.8123513276919048, - "velocityY": 0.00114810885939775, - "timestamp": 0.18619806808474682 - }, - { - "x": 2.039828562987611, - "y": 5.543319931052194, - "heading": -1.5854010106883968e-20, - "angularVelocity": -2.0916881637274291e-19, - "velocityX": 1.2185269596070247, - "velocityY": 0.0017221632439682793, - "timestamp": 0.2792971021271202 - }, - { - "x": 2.1910867994360017, - "y": 5.543533706687691, - "heading": 4.514872599726518e-21, - "angularVelocity": 2.1878726150197974e-19, - "velocityX": 1.624702533213676, - "velocityY": 0.0022962175461305355, - "timestamp": 0.37239613616949363 - }, - { - "x": 2.380159562296481, - "y": 5.5438009261858445, - "heading": -2.4606104769465887e-21, - "angularVelocity": -7.492540764169747e-20, - "velocityX": 2.0308778152781284, - "velocityY": 0.002870271436251635, - "timestamp": 0.46549517021186704 - }, - { - "x": 2.5314177987448714, - "y": 5.544014701821341, - "heading": 1.8729877149138444e-20, - "angularVelocity": 2.2761232534851326e-19, - "velocityX": 1.6247025332136762, - "velocityY": 0.002296217546130536, - "timestamp": 0.5585942042542404 - }, - { - "x": 2.6448614816388756, - "y": 5.544175033555818, - "heading": 3.8900824798152035e-20, - "angularVelocity": 2.1666119156330769e-19, - "velocityX": 1.218526959607025, - "velocityY": 0.0017221632439682793, - "timestamp": 0.6516932382966139 - }, - { - "x": 2.7204906055500317, - "y": 5.544281921381604, - "heading": 1.1738818455912713e-20, - "angularVelocity": -2.9175390079640015e-19, - "velocityX": 0.8123513276919049, - "velocityY": 0.00114810885939775, - "timestamp": 0.7447922723389873 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -1.0197992544535218e-37, - "angularVelocity": -1.260895838142623e-19, - "velocityX": 0.40617567078743805, - "velocityY": 0.0005740544395093853, - "timestamp": 0.8378913063813607 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "angularVelocity": 9.592877809620058e-37, - "velocityX": 0, - "velocityY": 7.039095899612751e-38, - "timestamp": 0.9309903404237341 - } - ] + "samples": [ + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 4.797022054850397e-21, + "angularVelocity": -9.847000764445998e-21, + "velocityX": -1.972173923637389e-19, + "velocityY": -3.0551788776993856e-19, + "timestamp": 0 + }, + { + "x": 1.850755756182451, + "y": 5.5430527114919315, + "heading": -8.297424237870081e-19, + "angularVelocity": -8.963996805344217e-18, + "velocityX": 0.4061756697719998, + "velocityY": 0.0005740544380748758, + "timestamp": 0.09309903427512145 + }, + { + "x": 1.9263848800936074, + "y": 5.543159599317717, + "heading": -2.6749716281445808e-18, + "angularVelocity": -1.9820068153291573e-17, + "velocityX": 0.8123513256610282, + "velocityY": 0.001148108856528731, + "timestamp": 0.1861980685502429 + }, + { + "x": 2.039828562987612, + "y": 5.543319931052195, + "heading": -5.8149048269782575e-18, + "angularVelocity": -3.372680740742886e-17, + "velocityX": 1.218526956560709, + "velocityY": 0.00172216323966475, + "timestamp": 0.27929710282536435 + }, + { + "x": 2.191086799436003, + "y": 5.543533706687691, + "heading": -1.0854467146744384e-17, + "angularVelocity": -5.413119866372016e-17, + "velocityX": 1.6247025291519184, + "velocityY": 0.0022962175403924926, + "timestamp": 0.3723961371004858 + }, + { + "x": 2.3801595622964795, + "y": 5.5438009261858445, + "heading": -4.1769885430235806e-18, + "angularVelocity": 7.172446691559558e-17, + "velocityX": 2.0308778102008924, + "velocityY": 0.0028702714290790255, + "timestamp": 0.46549517137560725 + }, + { + "x": 2.53141779874487, + "y": 5.54401470182134, + "heading": -1.8296127242246427e-18, + "angularVelocity": 2.5213750465779628e-17, + "velocityX": 1.6247025291519184, + "velocityY": 0.0022962175403924926, + "timestamp": 0.5585942056507287 + }, + { + "x": 2.644861481638875, + "y": 5.544175033555818, + "heading": -7.228112419808573e-19, + "angularVelocity": 1.1888431398676619e-17, + "velocityX": 1.218526956560709, + "velocityY": 0.0017221632396647501, + "timestamp": 0.6516932399258502 + }, + { + "x": 2.7204906055500317, + "y": 5.544281921381604, + "heading": -1.9384912404248153e-19, + "angularVelocity": 5.681714338671363e-18, + "velocityX": 0.8123513256610282, + "velocityY": 0.0011481088565287313, + "timestamp": 0.7447922742009716 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 9.777796865534126e-30, + "angularVelocity": 2.0821818999587538e-18, + "velocityX": 0.4061756697719998, + "velocityY": 0.0005740544380748761, + "timestamp": 0.837891308476093 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.1644118306851733e-29, + "angularVelocity": -2.560461213964373e-29, + "velocityX": -2.249094171598222e-22, + "velocityY": 3.2281317136840683e-24, + "timestamp": 0.9309903427512145 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj index 820a29f1..b2d30d99 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.3.traj @@ -1,814 +1,815 @@ { - "samples": [ - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "angularVelocity": 9.592877809620058e-37, - "velocityX": 0, - "velocityY": 7.039095899612751e-38, - "timestamp": 0 - }, - { - "x": 2.7765323823959327, - "y": 5.537624192256726, - "heading": 9.00740418544712e-19, - "angularVelocity": 1.3499597384555195e-17, - "velocityX": 0.2731753218474804, - "velocityY": -0.10058184592921769, - "timestamp": 0.06672350240426894 - }, - { - "x": 2.812967100056819, - "y": 5.524148553837151, - "heading": 1.885084819596924e-20, - "angularVelocity": -1.3217075521281405e-17, - "velocityX": 0.5460552331340873, - "velocityY": -0.2019623960674071, - "timestamp": 0.13344700480853788 - }, - { - "x": 2.8675858546495747, - "y": 5.503845620088409, - "heading": 3.43159270927708e-20, - "angularVelocity": 2.317785843530086e-19, - "velocityX": 0.8185834469813632, - "velocityY": -0.30428459264216856, - "timestamp": 0.20017050721280683 - }, - { - "x": 2.9403602428294473, - "y": 5.476640214649627, - "heading": 3.5333541604123114e-18, - "angularVelocity": 5.244086577062937e-17, - "velocityX": 1.0906859735710903, - "velocityY": -0.40773347409056915, - "timestamp": 0.26689400961707577 - }, - { - "x": 3.031255192543775, - "y": 5.4424407847147105, - "heading": 5.715508292123115e-18, - "angularVelocity": 3.2704430269696984e-17, - "velocityX": 1.3622628674916857, - "velocityY": -0.5125544778465974, - "timestamp": 0.3336175120213447 - }, - { - "x": 3.140226319352468, - "y": 5.40113340648718, - "heading": 6.027086577670902e-18, - "angularVelocity": 4.669693201563416e-18, - "velocityX": 1.6331745619175013, - "velocityY": -0.619082882928634, - "timestamp": 0.40034101442561365 - }, - { - "x": 3.267215684627595, - "y": 5.352572466235242, - "heading": 7.223223650198719e-18, - "angularVelocity": 1.7926772867621648e-17, - "velocityX": 1.9032179172147636, - "velocityY": -0.7277936334593704, - "timestamp": 0.4670645168298826 - }, - { - "x": 3.41214454573205, - "y": 5.296565350322459, - "heading": 8.483175715581024e-18, - "angularVelocity": 1.888318238685755e-17, - "velocityX": 2.172081139061778, - "velocityY": -0.8393911274837239, - "timestamp": 0.5337880192341515 - }, - { - "x": 3.5748998961264853, - "y": 5.232845435249424, - "heading": 1.0703776873779667e-17, - "angularVelocity": 3.3280644423357047e-17, - "velocityX": 2.4392507067198372, - "velocityY": -0.9549845673113, - "timestamp": 0.60051152163842 - }, - { - "x": 3.755306479840233, - "y": 5.161019665655948, - "heading": 1.3781541182824059e-17, - "angularVelocity": 4.612713958550321e-17, - "velocityX": 2.7037936740893525, - "velocityY": -1.0764688154152113, - "timestamp": 0.667235024042689 - }, - { - "x": 3.953058178777003, - "y": 5.080452170332065, - "heading": 1.6552203248378283e-17, - "angularVelocity": 4.15245298240439e-17, - "velocityX": 2.9637487813307404, - "velocityY": -1.2074830070480274, - "timestamp": 0.7339585264469579 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 2.0019563257216408e-17, - "angularVelocity": 5.196609716079013e-17, - "velocityX": 3.2138733563909483, - "velocityY": -1.356409229176722, - "timestamp": 0.8006820288512269 - }, - { - "x": 4.282917561329041, - "y": 4.939413083786088, - "heading": 2.2898103599715735e-17, - "angularVelocity": 8.297955477827177e-17, - "velocityX": 3.327163862876717, - "velocityY": -1.456761903756842, - "timestamp": 0.8353717828539282 - }, - { - "x": 4.401833285665519, - "y": 4.884962621939577, - "heading": 2.513067280390373e-17, - "angularVelocity": 6.43581734252565e-17, - "velocityX": 3.4279783110372324, - "velocityY": -1.5696410485433538, - "timestamp": 0.8700615368566296 - }, - { - "x": 4.518840762702163, - "y": 4.825621230712266, - "heading": 2.8188632508696854e-17, - "angularVelocity": 8.815166877675102e-17, - "velocityX": 3.372969350763695, - "velocityY": -1.7106316528704353, - "timestamp": 0.9047512908593309 - }, - { - "x": 4.633380439466489, - "y": 4.761646132811981, - "heading": 3.13421659224765e-17, - "angularVelocity": 9.090676784661175e-17, - "velocityX": 3.3018301817708395, - "velocityY": -1.8442073096080187, - "timestamp": 0.9394410448620323 - }, - { - "x": 4.745269125938669, - "y": 4.693140128704652, - "heading": 3.2482743151706595e-17, - "angularVelocity": 3.287936919763874e-17, - "velocityX": 3.2254102022016937, - "velocityY": -1.9748195418737586, - "timestamp": 0.9741307988647336 - }, - { - "x": 4.854330043574812, - "y": 4.6202164218202615, - "heading": 3.23593373316049e-17, - "angularVelocity": -3.55741410250663e-18, - "velocityX": 3.1438942353886365, - "velocityY": -2.1021684638845084, - "timestamp": 1.008820552867435 - }, - { - "x": 4.962829604926255, - "y": 4.546460093483164, - "heading": 3.1455405916033935e-17, - "angularVelocity": -2.605759082352001e-17, - "velocityX": 3.1277120426680747, - "velocityY": -2.1261704055714574, - "timestamp": 1.0435103068701364 - }, - { - "x": 5.07386693975175, - "y": 4.476582621829349, - "heading": 2.90082292000263e-17, - "angularVelocity": -7.054465464981545e-17, - "velocityX": 3.200868326043711, - "velocityY": -2.014354776005967, - "timestamp": 1.0782000608728377 - }, - { - "x": 5.187610618884458, - "y": 4.411202761148161, - "heading": 2.619388391387083e-17, - "angularVelocity": -8.112900673600335e-17, - "velocityX": 3.2788839933500147, - "velocityY": -1.884702343985982, - "timestamp": 1.112889814875539 - }, - { - "x": 5.303878902064225, - "y": 4.350425816718677, - "heading": 2.3840017411584327e-17, - "angularVelocity": -6.785480525757533e-17, - "velocityX": 3.35166064223783, - "velocityY": -1.7520142813566866, - "timestamp": 1.1475795688782404 - }, - { - "x": 5.422485692712026, - "y": 4.294349264150295, - "heading": 2.0445926478566092e-17, - "angularVelocity": -9.784130878396876e-17, - "velocityX": 3.419072693296267, - "velocityY": -1.6165162936588033, - "timestamp": 1.1822693228809418 - }, - { - "x": 5.5432410890609845, - "y": 4.243062963447586, - "heading": 1.6758899100249362e-17, - "angularVelocity": -1.0628577469963028e-16, - "velocityX": 3.4810104545438403, - "velocityY": -1.478427915594755, - "timestamp": 1.2169590768836431 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 1.3081934400390577e-17, - "angularVelocity": -1.0599569831404499e-16, - "velocityX": 3.537374169625408, - "velocityY": -1.337971116472263, - "timestamp": 1.2516488308863445 - }, - { - "x": 5.795129656789146, - "y": 4.153827522905326, - "heading": 9.653855416146634e-18, - "angularVelocity": -9.526631807314967e-17, - "velocityX": 3.589854735093561, - "velocityY": -1.1900109590649786, - "timestamp": 1.2876329966815603 - }, - { - "x": 5.925973457628177, - "y": 4.116403970594212, - "heading": 6.773802428340066e-18, - "angularVelocity": -8.003667513641558e-17, - "velocityX": 3.636149343676849, - "velocityY": -1.0400005525788714, - "timestamp": 1.323617162476776 - }, - { - "x": 6.058257666523593, - "y": 4.0844428903853265, - "heading": 3.7266948743050956e-18, - "angularVelocity": -8.467912168301893e-17, - "velocityX": 3.6761782848667472, - "velocityY": -0.8881984479166671, - "timestamp": 1.3596013282719919 - }, - { - "x": 6.1917543385226965, - "y": 4.0579993432791355, - "heading": 2.3240409715281496e-19, - "angularVelocity": -9.710634385796704e-17, - "velocityX": 3.7098726356150347, - "velocityY": -0.7348661980016377, - "timestamp": 1.3955854940672077 - }, - { - "x": 6.326233441808095, - "y": 4.037118888120514, - "heading": -2.9507025724087755e-18, - "angularVelocity": -8.845853722652732e-17, - "velocityX": 3.7371744019498516, - "velocityY": -0.5802678677464012, - "timestamp": 1.4315696598624235 - }, - { - "x": 6.461463252337996, - "y": 4.021837506709874, - "heading": -5.178975495681446e-18, - "angularVelocity": -6.192370655342397e-17, - "velocityX": 3.7580365569536736, - "velocityY": -0.4246696032250841, - "timestamp": 1.4675538256576393 - }, - { - "x": 6.596863602394611, - "y": 4.012202736620321, - "heading": -4.3543144284076125e-18, - "angularVelocity": 2.2917331805219353e-17, - "velocityX": 3.7627758505553097, - "velocityY": -0.26775026950420705, - "timestamp": 1.503537991452855 - }, - { - "x": 6.727539043403903, - "y": 4.005664590276287, - "heading": -3.5766746235771005e-18, - "angularVelocity": 2.161061087932747e-17, - "velocityX": 3.6314706238561003, - "velocityY": -0.1816950928150608, - "timestamp": 1.5395221572480708 - }, - { - "x": 6.85298501008117, - "y": 4.001263406795894, - "heading": -2.645512635943814e-18, - "angularVelocity": 2.5876992478309148e-17, - "velocityX": 3.4861435274385992, - "velocityY": -0.122308892901394, - "timestamp": 1.5755063230432866 - }, - { - "x": 6.973061817653349, - "y": 3.9986189838302026, - "heading": -1.6196069084437975e-18, - "angularVelocity": 2.850992109492732e-17, - "velocityX": 3.336934591051298, - "velocityY": -0.07348851660866845, - "timestamp": 1.6114904888385024 - }, - { - "x": 7.0877080332373295, - "y": 3.99753108954472, - "heading": -5.829248205352589e-19, - "angularVelocity": 2.880939616045139e-17, - "velocityX": 3.186018434786799, - "velocityY": -0.030232583177662708, - "timestamp": 1.6474746546337182 - }, - { - "x": 7.196889877319336, - "y": 3.9978766441345215, - "heading": 0, - "angularVelocity": 1.619948128975407e-17, - "velocityX": 3.034163545803906, - "velocityY": 0.009602962363163213, - "timestamp": 1.683458820428934 - }, - { - "x": 7.37338840046626, - "y": 4.002743213470309, - "heading": 8.093787297850302e-19, - "angularVelocity": 1.2673214059219633e-17, - "velocityX": 2.7636055688801218, - "velocityY": 0.07620051362427777, - "timestamp": 1.7473241292611363 - }, - { - "x": 7.532268457404618, - "y": 4.010110422409507, - "heading": 7.012678022232893e-19, - "angularVelocity": -1.6927958157108362e-18, - "velocityX": 2.487736454164719, - "velocityY": 0.11535541084684194, - "timestamp": 1.8111894380933387 - }, - { - "x": 7.673406960294269, - "y": 4.018856350887051, - "heading": 5.093320205791772e-19, - "angularVelocity": -3.0053214359033797e-18, - "velocityX": 2.2099400358412575, - "velocityY": 0.13694333649152024, - "timestamp": 1.875054746925541 - }, - { - "x": 7.796760795915294, - "y": 4.028209945243461, - "heading": 2.737170340594828e-19, - "angularVelocity": -3.689248370192142e-18, - "velocityX": 1.931468552749354, - "velocityY": 0.14645814022421355, - "timestamp": 1.9389200557577433 - }, - { - "x": 7.902319650014668, - "y": 4.037611597874035, - "heading": 3.811839387951723e-20, - "angularVelocity": -3.688992419979718e-18, - "velocityX": 1.65283556956901, - "velocityY": 0.14721063441931925, - "timestamp": 2.0027853645899456 - }, - { - "x": 7.990087417936277, - "y": 4.046637814965993, - "heading": -1.8999762440941744e-19, - "angularVelocity": -3.5718298785688795e-18, - "velocityX": 1.3742635794998819, - "velocityY": 0.14133208242480425, - "timestamp": 2.066650673422148 - }, - { - "x": 8.06007417147421, - "y": 4.0549574599073335, - "heading": -2.1124739162594107e-19, - "angularVelocity": -3.327278549958625e-19, - "velocityX": 1.0958492931086379, - "velocityY": 0.1302686089438499, - "timestamp": 2.1305159822543502 - }, - { - "x": 8.112292461632407, - "y": 4.062304767499659, - "heading": -1.9392441616154913e-19, - "angularVelocity": 2.712423345539538e-19, - "velocityX": 0.8176315297463507, - "velocityY": 0.11504379649410336, - "timestamp": 2.1943812910865526 - }, - { - "x": 8.146755551617066, - "y": 4.068461861174775, - "heading": -1.2666965805994144e-19, - "angularVelocity": 1.0530718371509655e-18, - "velocityX": 0.5396214410425377, - "velocityY": 0.0964074829935333, - "timestamp": 2.258246599918755 - }, - { - "x": 8.1634765625, - "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 1.9833875444262685e-18, - "velocityX": 0.26181680146361314, - "velocityY": 0.07492478756156581, - "timestamp": 2.322111908750957 - }, - { - "x": 8.160404735397709, - "y": 4.0766649695322, - "heading": 2.2110153580287816e-19, - "angularVelocity": 3.145700398787454e-18, - "velocityX": -0.04370411858778341, - "velocityY": 0.04862945386701764, - "timestamp": 2.3923988107993464 - }, - { - "x": 8.135851784135758, - "y": 4.07831805033267, - "heading": 5.233120115827785e-19, - "angularVelocity": 4.299669881203419e-18, - "velocityX": -0.34932470412548816, - "velocityY": 0.02351904483329859, - "timestamp": 2.4626857128477355 - }, - { - "x": 8.089809855784104, - "y": 4.078304537638241, - "heading": 9.059382457941767e-19, - "angularVelocity": 5.443777191210811e-18, - "velocityX": -0.6550570164545902, - "velocityY": -0.00019225053366650092, - "timestamp": 2.5329726148961247 - }, - { - "x": 8.022270134044854, - "y": 4.076742304755474, - "heading": 1.3681480133267195e-18, - "angularVelocity": 6.5760441001468545e-18, - "velocityX": -0.9609147617966158, - "velocityY": -0.022226515001204008, - "timestamp": 2.603259516944514 - }, - { - "x": 7.9332227395210335, - "y": 4.0737752063023285, - "heading": 1.9089237482918726e-18, - "angularVelocity": 7.693833690281903e-18, - "velocityX": -1.2669130652893827, - "velocityY": -0.04221410201153685, - "timestamp": 2.673546418992903 - }, - { - "x": 7.822656701940191, - "y": 4.069582704513375, - "heading": 2.0768842809054915e-18, - "angularVelocity": 2.3896419918926418e-18, - "velocityX": -1.5730674472567026, - "velocityY": -0.05964840769432409, - "timestamp": 2.743833321041292 - }, - { - "x": 7.690560152144021, - "y": 4.064394900776391, - "heading": 2.063501960546536e-18, - "angularVelocity": -1.9039564936380514e-19, - "velocityX": -1.8793906965088152, - "velocityY": -0.07380896846772396, - "timestamp": 2.8141202230896813 - }, - { - "x": 7.536921137087825, - "y": 4.058517363178713, - "heading": 1.8242393677495476e-18, - "angularVelocity": -3.404085054560583e-18, - "velocityX": -2.185884006531148, - "velocityY": -0.08362208927108938, - "timestamp": 2.8844071251380705 - }, - { - "x": 7.361730243526304, - "y": 4.0523752131799995, - "heading": 1.7449161631907716e-18, - "angularVelocity": -1.128563106982712e-18, - "velocityX": -2.492511242577032, - "velocityY": -0.08738683623422486, - "timestamp": 2.9546940271864597 - }, - { - "x": 7.164988961998875, - "y": 4.046599373609111, - "heading": 1.6203310877610785e-18, - "angularVelocity": -1.7725219321907188e-18, - "velocityX": -2.799117272119643, - "velocityY": -0.0821751905769253, - "timestamp": 3.024980929234849 - }, - { - "x": 6.946739461316107, - "y": 4.042219879396985, - "heading": 9.714507263424963e-19, - "angularVelocity": -9.231881652246047e-18, - "velocityX": -3.105123349048912, - "velocityY": -0.062308824041090796, - "timestamp": 3.095267831283238 - }, - { - "x": 6.707199372936614, - "y": 4.041196558011354, - "heading": 4.304213175776067e-18, - "angularVelocity": 4.741655062759286e-17, - "velocityX": -3.408033095761992, - "velocityY": -0.014559204571673643, - "timestamp": 3.165554733331627 - }, - { - "x": 6.4477381349567215, - "y": 4.048401250319157, - "heading": 3.981085161265745e-18, - "angularVelocity": -4.597272110228882e-18, - "velocityX": -3.691459296374506, - "velocityY": 0.102504052644734, - "timestamp": 3.2358416353800163 - }, - { - "x": 6.183416579926013, - "y": 4.076604236120931, - "heading": 3.783709688486887e-18, - "angularVelocity": -2.808140165901377e-18, - "velocityX": -3.7606089801587, - "velocityY": 0.4012552122777795, - "timestamp": 3.3061285374284055 - }, - { - "x": 5.922248771081443, - "y": 4.126128703964576, - "heading": 4.4649221139499255e-18, - "angularVelocity": 9.691882920007657e-18, - "velocityX": -3.7157393658460984, - "velocityY": 0.7046045052540438, - "timestamp": 3.3764154394767947 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 6.738429572739451e-18, - "angularVelocity": 3.2346104217601064e-17, - "velocityX": -3.646441012355221, - "velocityY": 1.003321650758162, - "timestamp": 3.446702341525184 - }, - { - "x": 5.5410942529337, - "y": 4.236435293525444, - "heading": 7.176696434761227e-18, - "angularVelocity": 1.2648537308401216e-17, - "velocityX": -3.603431102925481, - "velocityY": 1.1482444113922432, - "timestamp": 3.4813519497238077 - }, - { - "x": 5.417926513758212, - "y": 4.281179467560016, - "heading": 6.902122193005105e-18, - "angularVelocity": -7.924310144639137e-18, - "velocityX": -3.554664701241286, - "velocityY": 1.2913327555706813, - "timestamp": 3.5160015579224315 - }, - { - "x": 5.296645297491296, - "y": 4.3308101048073215, - "heading": 7.073122455788382e-18, - "angularVelocity": 4.9351283224632194e-18, - "velocityX": -3.5002189800153154, - "velocityY": 1.432357819540261, - "timestamp": 3.5506511661210554 - }, - { - "x": 5.177444375118266, - "y": 4.38524790502043, - "heading": 7.030277898528261e-18, - "angularVelocity": -1.2365091407245627e-18, - "velocityX": -3.44018095932656, - "velocityY": 1.5710942502164025, - "timestamp": 3.585300774319679 - }, - { - "x": 5.06051419213652, - "y": 4.444405885414455, - "heading": 6.93623096110632e-18, - "angularVelocity": -2.7142280190538443e-18, - "velocityX": -3.3746466139374856, - "velocityY": 1.7073203268248038, - "timestamp": 3.619950382518303 - }, - { - "x": 4.9460415627669985, - "y": 4.508189517767328, - "heading": 6.830736149305672e-18, - "angularVelocity": -3.0446177398576756e-18, - "velocityX": -3.303720743776551, - "velocityY": 1.8408182853682935, - "timestamp": 3.654599990716927 - }, - { - "x": 4.834209367418348, - "y": 4.576496874327604, - "heading": 6.686796334021e-18, - "angularVelocity": -4.154154195902009e-18, - "velocityX": -3.2275168800637117, - "velocityY": 1.971374572800783, - "timestamp": 3.6892495989155507 - }, - { - "x": 4.725196244628599, - "y": 4.649218770197504, - "heading": 6.8822518039480765e-18, - "angularVelocity": 5.6409142870147405e-18, - "velocityX": -3.1461574446916942, - "velocityY": 2.0987797453014108, - "timestamp": 3.7238992071141745 - }, - { - "x": 4.61917611476972, - "y": 4.726238684910051, - "heading": 7.643017507811422e-18, - "angularVelocity": 2.195596843411265e-17, - "velocityX": -3.0597786056088934, - "velocityY": 2.2228220957374782, - "timestamp": 3.7585488153127984 - }, - { - "x": 4.510162876116158, - "y": 4.798960406973094, - "heading": 8.156998265512583e-18, - "angularVelocity": 1.483366723094954e-17, - "velocityX": -3.14616078856249, - "velocityY": 2.098774729173764, - "timestamp": 3.7931984235114222 - }, - { - "x": 4.3983305631985266, - "y": 4.867267570914994, - "heading": 9.169956130720048e-18, - "angularVelocity": 2.9234323788045264e-17, - "velocityX": -3.227520273146238, - "velocityY": 1.9713690137660609, - "timestamp": 3.827848031710046 - }, - { - "x": 4.28385782617867, - "y": 4.93105099911187, - "heading": 9.858535854282379e-18, - "angularVelocity": 1.9872655402484013e-17, - "velocityX": -3.3037238506034297, - "velocityY": 1.840812393353667, - "timestamp": 3.86249763990867 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 9.428613355674913e-18, - "angularVelocity": -1.2407716016383212e-17, - "velocityX": -3.3581551662140523, - "velocityY": 1.6997824742615957, - "timestamp": 3.8971472481072937 - }, - { - "x": 3.9412126137988053, - "y": 5.086809869447978, - "heading": 6.455008109104801e-18, - "angularVelocity": -4.321651467897992e-17, - "velocityX": -3.28870544216361, - "velocityY": 1.4077326603917448, - "timestamp": 3.9659543989999233 - }, - { - "x": 3.7303194478817963, - "y": 5.1698988995186355, - "heading": 5.431152098522906e-18, - "angularVelocity": -1.488008146391029e-17, - "velocityX": -3.0649890771686916, - "velocityY": 1.2075638795204993, - "timestamp": 4.034761549892552 - }, - { - "x": 3.537102982396402, - "y": 5.24230220978644, - "heading": 5.612600329775147e-18, - "angularVelocity": 2.6370548540132515e-18, - "velocityX": -2.8080869935582533, - "velocityY": 1.052264326142308, - "timestamp": 4.103568700785182 - }, - { - "x": 3.3623900460315483, - "y": 5.305525654536907, - "heading": 4.225050788166454e-18, - "angularVelocity": -2.0165775266205907e-17, - "velocityX": -2.5391683000722396, - "velocityY": 0.918849915020073, - "timestamp": 4.1723758516778116 - }, - { - "x": 3.2065873746476083, - "y": 5.360439315991953, - "heading": 1.940037667439135e-18, - "angularVelocity": -3.3208948359058894e-17, - "velocityX": -2.2643383625498803, - "velocityY": 0.7980807335088809, - "timestamp": 4.241183002570441 - }, - { - "x": 3.069932459985333, - "y": 5.407606100907469, - "heading": -1.0877040544793261e-18, - "angularVelocity": -4.4003300276785115e-17, - "velocityX": -1.986056868936749, - "velocityY": 0.6854924859353159, - "timestamp": 4.309990153463071 - }, - { - "x": 2.9525796309081422, - "y": 5.44741896036095, - "heading": -3.706402695175446e-18, - "angularVelocity": -3.805852453885881e-17, - "velocityX": -1.705532456362192, - "velocityY": 0.5786151430046301, - "timestamp": 4.3787973043557 - }, - { - "x": 2.854636716681842, - "y": 5.480167404381309, - "heading": -5.982131114439951e-18, - "angularVelocity": -3.307401032802041e-17, - "velocityX": -1.4234409208300893, - "velocityY": 0.4759453573577233, - "timestamp": 4.44760445524833 - }, - { - "x": 2.776183090706554, - "y": 5.506073449109121, - "heading": -4.835349945984594e-18, - "angularVelocity": 1.666659865403876e-17, - "velocityX": -1.1401958220550505, - "velocityY": 0.3765022151293121, - "timestamp": 4.516411606140959 - }, - { - "x": 2.7172795158509544, - "y": 5.525312687608081, - "heading": -3.43564479511919e-18, - "angularVelocity": 2.034243727152086e-17, - "velocityX": -0.8560676338352585, - "velocityY": 0.2796110324199007, - "timestamp": 4.585218757033589 - }, - { - "x": 2.6779739479348046, - "y": 5.538027438094956, - "heading": -1.81505607662002e-18, - "angularVelocity": 2.355262058485764e-17, - "velocityX": -0.5712424857916886, - "velocityY": 0.18478821346223093, - "timestamp": 4.6540259079262185 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -7.816373438340897e-46, - "angularVelocity": 2.6378887267870092e-17, - "velocityX": -0.2858537161877381, - "velocityY": 0.09167545987040067, - "timestamp": 4.722833058818848 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 1.7878647482773802e-40, - "angularVelocity": 2.5983819147247646e-39, - "velocityX": 0, - "velocityY": -2.2530838665454385e-40, - "timestamp": 4.791640209711478 - } - ] + "samples": [ + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.1644118306851733e-29, + "angularVelocity": -2.560461213964373e-29, + "velocityX": -2.249094171598222e-22, + "velocityY": 3.2281317136840683e-24, + "timestamp": 0 + }, + { + "x": 2.7765323826419794, + "y": 5.537624192846842, + "heading": -2.642342456596521e-18, + "angularVelocity": -3.960137512120418e-17, + "velocityX": 0.2731753246622511, + "velocityY": -0.1005818367636714, + "timestamp": 0.06672350261744775 + }, + { + "x": 2.812967100767047, + "y": 5.524148555527004, + "heading": -5.7083699786826614e-18, + "angularVelocity": -4.595123759665632e-17, + "velocityX": 0.5460552383462592, + "velocityY": -0.20196237894013302, + "timestamp": 0.1334470052348955 + }, + { + "x": 2.867585856008562, + "y": 5.5038456232928015, + "heading": -9.193458656984461e-18, + "angularVelocity": -5.223180051341883e-17, + "velocityX": 0.8185834540891116, + "velocityY": -0.3042845689712529, + "timestamp": 0.20017050785234325 + }, + { + "x": 2.9403602449806896, + "y": 5.476640219670028, + "heading": -1.3713804964383516e-17, + "angularVelocity": -6.774743726038035e-17, + "velocityX": 1.0906859819600914, + "velocityY": -0.40773344557094127, + "timestamp": 0.266894010469791 + }, + { + "x": 3.0312551955793734, + "y": 5.442440791714766, + "heading": -1.9689161108713677e-17, + "angularVelocity": -8.955399386867554e-17, + "velocityX": 1.3622628763933429, + "velocityY": -0.51255444653948, + "timestamp": 0.33361751308723875 + }, + { + "x": 3.1402263232984726, + "y": 5.401133415459366, + "heading": -2.6276370451665866e-17, + "angularVelocity": -9.872397407984698e-17, + "velocityX": 1.6331745703440297, + "velocityY": -0.6190828513939344, + "timestamp": 0.4003410157046865 + }, + { + "x": 3.2672156894222453, + "y": 5.352572476953852, + "heading": -3.586672627631812e-17, + "angularVelocity": -1.4373279951454157e-16, + "velocityX": 1.9032179238529063, + "velocityY": -0.7277936049600628, + "timestamp": 0.46706451832213425 + }, + { + "x": 3.4121445511915924, + "y": 5.2965653622743645, + "heading": -4.6442314764739977e-17, + "angularVelocity": -1.5849870096036839e-16, + "velocityX": 2.172081142086939, + "velocityY": -0.8393911063182322, + "timestamp": 0.533788020939582 + }, + { + "x": 3.5748999018873118, + "y": 5.232845447526237, + "heading": -5.766195301574919e-17, + "angularVelocity": -1.6815121824921553e-16, + "velocityX": 2.439250703441936, + "velocityY": -0.9549845593907098, + "timestamp": 0.6005115235570302 + }, + { + "x": 3.7553064852503644, + "y": 5.161019676773233, + "heading": -6.954653481566044e-17, + "angularVelocity": -1.7811687536878263e-16, + "velocityX": 2.70379366019489, + "velocityY": -1.0764688293540257, + "timestamp": 0.6672350261744779 + }, + { + "x": 3.953058182659589, + "y": 5.080452177894761, + "heading": -7.943549000582077e-17, + "angularVelocity": -1.4820797473458394e-16, + "velocityX": 2.9637487489680097, + "velocityY": -1.2074830564635766, + "timestamp": 0.7339585287919257 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -8.307291105750155e-17, + "angularVelocity": -5.451483973353195e-17, + "velocityX": 3.2138732879335503, + "velocityY": -1.3564093381868512, + "timestamp": 0.8006820314093734 + }, + { + "x": 4.2829175575872735, + "y": 4.93941307796568, + "heading": -8.597616346613658e-17, + "angularVelocity": -8.369192817411254e-17, + "velocityX": 3.3271637455782836, + "velocityY": -1.45676206741062, + "timestamp": 0.8353717855104432 + }, + { + "x": 4.40183327548322, + "y": 4.884962607883731, + "heading": -8.781389044285282e-17, + "angularVelocity": -5.2976073895804537e-17, + "velocityX": 3.4279781156557663, + "velocityY": -1.5696412814950174, + "timestamp": 0.870061539611513 + }, + { + "x": 4.518840747701827, + "y": 4.825621207784163, + "heading": -9.315781416713264e-17, + "angularVelocity": -1.5404905173670211e-16, + "velocityX": 3.3729692023097777, + "velocityY": -1.7106319037798774, + "timestamp": 0.9047512937125828 + }, + { + "x": 4.633380419289881, + "y": 4.7616461011987, + "heading": -9.908185769391999e-17, + "angularVelocity": -1.7077213950707558e-16, + "velocityX": 3.301830023191876, + "velocityY": -1.8442075547457024, + "timestamp": 0.9394410478136526 + }, + { + "x": 4.745269100236443, + "y": 4.693140088610349, + "heading": -1.025376712876981e-16, + "angularVelocity": -9.9620584908128e-17, + "velocityX": 3.2254100337687888, + "velocityY": -1.9748197807559171, + "timestamp": 0.9741308019147223 + }, + { + "x": 4.854330012145195, + "y": 4.620216373671222, + "heading": -1.025310951791751e-16, + "angularVelocity": 1.8956918365515503e-19, + "velocityX": 3.1438940613704167, + "velocityY": -2.1021686901170322, + "timestamp": 1.0088205560157921 + }, + { + "x": 4.962829609437795, + "y": 4.5464600987106305, + "heading": -9.93623379907482e-17, + "angularVelocity": 9.134562265751682e-17, + "velocityX": 3.127713069873129, + "velocityY": -2.1261688608602203, + "timestamp": 1.043510310116862 + }, + { + "x": 5.073866943405842, + "y": 4.47658262622743, + "heading": -9.505129174841953e-17, + "angularVelocity": 1.2427433845795495e-16, + "velocityX": 3.2008682922495537, + "velocityY": -2.014354794202651, + "timestamp": 1.0782000642179317 + }, + { + "x": 5.187610621724133, + "y": 4.411202764699172, + "heading": -8.900790995033302e-17, + "angularVelocity": 1.7421229853948998e-16, + "velocityX": 3.278883960575102, + "velocityY": -1.8847023630600785, + "timestamp": 1.1128898183190015 + }, + { + "x": 5.303878904131373, + "y": 4.350425819404775, + "heading": -8.318308330059428e-17, + "angularVelocity": 1.6791201899662455e-16, + "velocityX": 3.3516606104641187, + "velocityY": -1.7520143013214118, + "timestamp": 1.1475795724200712 + }, + { + "x": 5.422485694048398, + "y": 4.294349265955072, + "heading": -7.684855050558625e-17, + "angularVelocity": 1.8260529539962743e-16, + "velocityX": 3.4190726625349193, + "velocityY": -1.616516314480737, + "timestamp": 1.182269326521141 + }, + { + "x": 5.543241089708319, + "y": 4.243062964356387, + "heading": -6.896458750545335e-17, + "angularVelocity": 2.2727065107078374e-16, + "velocityX": 3.4810104248100697, + "velocityY": -1.4784279372307074, + "timestamp": 1.2169590806222108 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -6.104113455464306e-17, + "angularVelocity": 2.2840902612270297e-16, + "velocityX": 3.5373741409339847, + "velocityY": -1.33797113887621, + "timestamp": 1.2516488347232806 + }, + { + "x": 5.795129656406628, + "y": 4.153827521880981, + "heading": -5.26502063293767e-17, + "angularVelocity": 2.331839018689876e-16, + "velocityX": 3.589854707589498, + "velocityY": -1.1900109819379716, + "timestamp": 1.2876330006876366 + }, + { + "x": 5.925973456913311, + "y": 4.116403968556151, + "heading": -4.449297326372924e-17, + "angularVelocity": 2.2668951320100914e-16, + "velocityX": 3.636149317349419, + "velocityY": -1.0400005758616107, + "timestamp": 1.3236171666519927 + }, + { + "x": 6.058257665524998, + "y": 4.08444288734655, + "heading": -3.6710199664402193e-17, + "angularVelocity": 2.162832843430151e-16, + "velocityX": 3.6761782597023362, + "velocityY": -0.8881984715516176, + "timestamp": 1.3596013326163487 + }, + { + "x": 6.191754337287333, + "y": 4.057999339254895, + "heading": -3.0635067113443973e-17, + "angularVelocity": 1.6882793828813163e-16, + "velocityX": 3.7098726115972305, + "velocityY": -0.7348662219335074, + "timestamp": 1.3955854985807048 + }, + { + "x": 6.326233440381154, + "y": 4.037118883128164, + "heading": -2.7100684489157953e-17, + "angularVelocity": 9.822049586045954e-17, + "velocityX": 3.7371743790595944, + "velocityY": -0.5802678919226052, + "timestamp": 1.4315696645450608 + }, + { + "x": 6.461463250762805, + "y": 4.021837500768739, + "heading": -2.6741166877102065e-17, + "angularVelocity": 9.990994712261855e-18, + "velocityX": 3.758036535169413, + "velocityY": -0.42466962759569293, + "timestamp": 1.467553830509417 + }, + { + "x": 6.596863600046523, + "y": 4.012202729774147, + "heading": -2.0941092472280878e-17, + "angularVelocity": 1.6118407219507027e-16, + "velocityX": 3.7627758113898246, + "velocityY": -0.26775029339668854, + "timestamp": 1.503537996473773 + }, + { + "x": 6.727539041313674, + "y": 4.005664584143169, + "heading": -1.5390888357792948e-17, + "angularVelocity": 1.5424017663331785e-16, + "velocityX": 3.631470613952549, + "velocityY": -0.1816950721451956, + "timestamp": 1.539522162438129 + }, + { + "x": 6.852985008513406, + "y": 4.001263402089072, + "heading": -1.0075825367590821e-17, + "angularVelocity": 1.4770560460742425e-16, + "velocityX": 3.4861435255715034, + "velocityY": -0.12230885268972179, + "timestamp": 1.575506328402485 + }, + { + "x": 6.973061816658764, + "y": 3.998618980784619, + "heading": -5.1577923312514375e-18, + "angularVelocity": 1.3667214188229055e-16, + "velocityX": 3.336934591294929, + "velocityY": -0.07348847009744364, + "timestamp": 1.611490494366841 + }, + { + "x": 7.087708032779949, + "y": 3.9975310881250836, + "heading": -2.6946740234793898e-18, + "angularVelocity": 6.845005967538265e-17, + "velocityX": 3.186018434740021, + "velocityY": -0.030232537850478983, + "timestamp": 1.6474746603311972 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -6.591539659972594e-30, + "angularVelocity": 7.488499319392298e-17, + "velocityX": 3.034163544252643, + "velocityY": 0.009603001769728973, + "timestamp": 1.6834588262955532 + }, + { + "x": 7.373388400468218, + "y": 4.002743214942896, + "heading": -4.953200001247003e-18, + "angularVelocity": -7.75569723773575e-17, + "velocityX": 2.7636055642205424, + "velocityY": 0.07620053655266033, + "timestamp": 1.7473241352361444 + }, + { + "x": 7.532268457334727, + "y": 4.010110424631325, + "heading": -1.3097129093463054e-17, + "angularVelocity": -1.2751725823370097e-16, + "velocityX": 2.48773644881765, + "velocityY": 0.1153554223824672, + "timestamp": 1.8111894441767356 + }, + { + "x": 7.673406960142625, + "y": 4.018856353361859, + "heading": -1.8281069995800668e-17, + "angularVelocity": -8.116990254124082e-17, + "velocityX": 2.209940030810585, + "velocityY": 0.13694334022043622, + "timestamp": 1.8750547531173267 + }, + { + "x": 7.796760795699071, + "y": 4.0282099476371425, + "heading": -2.4884139524870184e-17, + "angularVelocity": -1.03390551752411e-16, + "velocityX": 1.9314685484602037, + "velocityY": 0.1464581387053742, + "timestamp": 1.938920062057918 + }, + { + "x": 7.902319649761861, + "y": 4.0376115999719895, + "heading": -2.509945215126813e-17, + "angularVelocity": -3.3713549672899342e-18, + "velocityX": 1.6528355661910739, + "velocityY": 0.14721062953900424, + "timestamp": 2.002785370998509 + }, + { + "x": 7.9900874176780725, + "y": 4.046637816644616, + "heading": -1.900477135412208e-17, + "angularVelocity": 9.543022492350273e-17, + "velocityX": 1.3742635770830707, + "velocityY": 0.14133207561907785, + "timestamp": 2.0666506799391002 + }, + { + "x": 8.060074171241288, + "y": 4.054957461114281, + "heading": -1.4433757000062975e-17, + "angularVelocity": 7.157272750648916e-17, + "velocityX": 1.0958492916446645, + "velocityY": 0.13026860133729024, + "timestamp": 2.1305159888796914 + }, + { + "x": 8.112292461453118, + "y": 4.062304768239738, + "heading": -9.3901869246783e-18, + "angularVelocity": 7.897198273854564e-17, + "velocityX": 0.8176315291985099, + "velocityY": 0.11504378898864956, + "timestamp": 2.1943812978202826 + }, + { + "x": 8.146755551516593, + "y": 4.0684618614995625, + "heading": -4.677400275001561e-18, + "angularVelocity": 7.379259143526985e-17, + "velocityX": 0.5396214413607953, + "velocityY": 0.09640747632727756, + "timestamp": 2.2582466067608737 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -3.026226030610745e-28, + "angularVelocity": 7.323851324219789e-17, + "velocityX": 0.2618168025924992, + "velocityY": 0.07492478234891005, + "timestamp": 2.322111915701465 + }, + { + "x": 8.160404735497007, + "y": 4.076664969326324, + "heading": 5.687417788003061e-18, + "angularVelocity": 8.091717809987449e-17, + "velocityX": -0.04370411704181345, + "velocityY": 0.04862945078972498, + "timestamp": 2.392398817964072 + }, + { + "x": 8.13585178429954, + "y": 4.078318050056324, + "heading": 9.715319347453537e-18, + "angularVelocity": 5.730657391283969e-17, + "velocityX": -0.34932470214341615, + "velocityY": 0.02351904375900896, + "timestamp": 2.462685720226679 + }, + { + "x": 8.089809855979007, + "y": 4.078304537416186, + "heading": 1.421739507206778e-17, + "angularVelocity": 6.405284028537325e-17, + "velocityX": -0.6550570140153344, + "velocityY": -0.00019224976064422702, + "timestamp": 2.5329726224892863 + }, + { + "x": 8.022270134239134, + "y": 4.076742304699779, + "heading": 2.3560062445993e-17, + "angularVelocity": 1.3292188264586088e-16, + "velocityX": -0.9609147588768534, + "velocityY": -0.022226512566613944, + "timestamp": 2.6032595247518935 + }, + { + "x": 7.933222739684713, + "y": 4.073775206509657, + "heading": 3.0510865621673614e-17, + "angularVelocity": 9.889186963848529e-17, + "velocityX": -1.2669130618635012, + "velocityY": -0.04221409814073001, + "timestamp": 2.6735464270145006 + }, + { + "x": 7.822656702045201, + "y": 4.069582705061331, + "heading": 3.6286017590367637e-17, + "angularVelocity": 8.21654075361425e-17, + "velocityX": -1.573067443297094, + "velocityY": -0.05964840266629161, + "timestamp": 2.7438333292771078 + }, + { + "x": 7.690560152164254, + "y": 4.0643949017183925, + "heading": 4.582493898384423e-17, + "angularVelocity": 1.3571406743753222e-16, + "velocityX": -1.8793906919870382, + "velocityY": -0.07380896263653505, + "timestamp": 2.814120231539715 + }, + { + "x": 7.53692113699896, + "y": 4.058517364536531, + "heading": 5.495539333254171e-17, + "angularVelocity": 1.2990264266804977e-16, + "velocityX": -2.1858840014212815, + "velocityY": -0.0836220831002289, + "timestamp": 2.884407133802322 + }, + { + "x": 7.361730243304987, + "y": 4.052375214932341, + "heading": 6.396812526564607e-17, + "angularVelocity": 1.2822775856015118e-16, + "velocityX": -2.4925112368648836, + "velocityY": -0.08738683035483485, + "timestamp": 2.954694036064929 + }, + { + "x": 7.164988961620082, + "y": 4.046599375673031, + "heading": 7.356788600543696e-17, + "angularVelocity": 1.3657965326299643e-16, + "velocityX": -2.799117265829076, + "velocityY": -0.08217518589352116, + "timestamp": 3.0249809383275363 + }, + { + "x": 6.946739460744683, + "y": 4.042219881595439, + "heading": 8.53416334460989e-17, + "angularVelocity": 1.6750983557133948e-16, + "velocityX": -3.105123342325879, + "velocityY": -0.06230882193712492, + "timestamp": 3.0952678405901435 + }, + { + "x": 6.707199372096715, + "y": 4.041196560014842, + "heading": 9.976814238544392e-17, + "angularVelocity": 2.052517393014489e-16, + "velocityX": -3.408033089194812, + "velocityY": -0.014559207301144003, + "timestamp": 3.1655547428527506 + }, + { + "x": 6.447738133610024, + "y": 4.048401251612496, + "heading": 1.0469202046971274e-16, + "angularVelocity": 7.00539919377632e-17, + "velocityX": -3.691459292334275, + "velocityY": 0.10250404222876178, + "timestamp": 3.2358416451153578 + }, + { + "x": 6.18341657905221, + "y": 4.076604236955095, + "heading": 9.517171884835342e-17, + "angularVelocity": -1.354491564534057e-16, + "velocityX": -3.7606089619691896, + "velocityY": 0.4012552045219841, + "timestamp": 3.306128547377965 + }, + { + "x": 5.922248770654031, + "y": 4.126128704367341, + "heading": 8.827514914451601e-17, + "angularVelocity": -9.812026823816515e-17, + "velocityX": -3.7157393481704415, + "velocityY": 0.7046044969688833, + "timestamp": 3.376415449640572 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 8.108309692704171e-17, + "angularVelocity": -1.0232421668622016e-16, + "velocityX": -3.6464409951607806, + "velocityY": 1.003321641969972, + "timestamp": 3.446702351903179 + }, + { + "x": 5.541094252914905, + "y": 4.236435293416023, + "heading": 7.779103781629071e-17, + "angularVelocity": -9.500999479304098e-17, + "velocityX": -3.603431085871871, + "velocityY": 1.1482444026273093, + "timestamp": 3.481351960271001 + }, + { + "x": 5.417926513725, + "y": 4.281179467366055, + "heading": 7.404581584331534e-17, + "angularVelocity": -1.0808843589526725e-16, + "velocityX": -3.5546646842995244, + "velocityY": 1.291332746825043, + "timestamp": 3.516001568638823 + }, + { + "x": 5.2966452974499845, + "y": 4.330810104553132, + "heading": 7.048745543918544e-17, + "angularVelocity": -1.0269554467404476e-16, + "velocityX": -3.5002189631570437, + "velocityY": 1.4323578108076789, + "timestamp": 3.5506511770066447 + }, + { + "x": 5.177444375077077, + "y": 4.385247904729644, + "heading": 6.777735008198806e-17, + "angularVelocity": -7.821460284920669e-17, + "velocityX": -3.440180942524166, + "velocityY": 1.5710942414883695, + "timestamp": 3.5853007853744665 + }, + { + "x": 5.060514192105533, + "y": 4.444405885109906, + "heading": 6.539767218111103e-17, + "angularVelocity": -6.867834914008308e-17, + "velocityX": -3.3746465971642725, + "velocityY": 1.7073203180905416, + "timestamp": 3.6199503937422883 + }, + { + "x": 4.946041562758086, + "y": 4.5081895174709326, + "heading": 6.339841419086817e-17, + "angularVelocity": -5.769929544659045e-17, + "velocityX": -3.3037207270069695, + "velocityY": 1.8408182766146604, + "timestamp": 3.65460000211011 + }, + { + "x": 4.834209367445105, + "y": 4.5764968740602265, + "heading": 6.206624882969642e-17, + "angularVelocity": -3.844676532404254e-17, + "velocityX": -3.2275168632738827, + "velocityY": 1.9713745640117823, + "timestamp": 3.689249610477932 + }, + { + "x": 4.725196244706173, + "y": 4.649218769978707, + "heading": 6.152068396615488e-17, + "angularVelocity": -1.574519567617046e-17, + "velocityX": -3.1461574278620072, + "velocityY": 2.098779736454826, + "timestamp": 3.723899218845754 + }, + { + "x": 4.619176114911752, + "y": 4.726238684753922, + "heading": 5.2556937935057745e-17, + "angularVelocity": -2.58696893087623e-16, + "velocityX": -3.0597785888073585, + "velocityY": 2.2228220866918083, + "timestamp": 3.7585488272135756 + }, + { + "x": 4.51016287630692, + "y": 4.798960406862416, + "heading": 6.64273126982982e-17, + "angularVelocity": 4.003039403991449e-16, + "velocityX": -3.1461607717930216, + "velocityY": 2.0987747202369076, + "timestamp": 3.7931984355813975 + }, + { + "x": 4.398330563422939, + "y": 4.8672675708300295, + "heading": 7.26949465762174e-17, + "angularVelocity": 1.8088613906315835e-16, + "velocityX": -3.2275202564146483, + "velocityY": 1.9713690048817056, + "timestamp": 3.8278480439492193 + }, + { + "x": 4.283857826423259, + "y": 4.9310509990316564, + "heading": 7.854869048227426e-17, + "angularVelocity": 1.6894112751752813e-16, + "velocityX": -3.303723833888627, + "velocityY": 1.840812384501881, + "timestamp": 3.862497652317041 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 7.967166003614947e-17, + "angularVelocity": 3.240930002777194e-17, + "velocityX": -3.358155156874672, + "velocityY": 1.6997824682762939, + "timestamp": 3.897147260684863 + }, + { + "x": 3.941212613699701, + "y": 5.086809869285441, + "heading": 7.443579239948039e-17, + "angularVelocity": -7.609481799868211e-17, + "velocityX": -3.288705435486243, + "velocityY": 1.4077326545547513, + "timestamp": 3.9659544117473335 + }, + { + "x": 3.7303194477377524, + "y": 5.1698988992560615, + "heading": 6.400702034661796e-17, + "angularVelocity": -1.515652354772208e-16, + "velocityX": -3.0649890702563414, + "velocityY": 1.2075638750859479, + "timestamp": 4.034761562809804 + }, + { + "x": 3.5371029822412163, + "y": 5.24230220948227, + "heading": 4.92329505445724e-17, + "angularVelocity": -2.1471706900158835e-16, + "velocityX": -2.8080869867888283, + "velocityY": 1.0522643229404085, + "timestamp": 4.103568713872274 + }, + { + "x": 3.362390045884083, + "y": 5.305525654232102, + "heading": 3.1750019865336384e-17, + "angularVelocity": -2.5408595487118374e-16, + "velocityX": -2.5391682936924758, + "velocityY": 0.9188499127428014, + "timestamp": 4.172375864934745 + }, + { + "x": 3.2065873745181794, + "y": 5.36043931571324, + "heading": 2.169929247164939e-17, + "angularVelocity": -1.4607097138783564e-16, + "velocityX": -2.2643383566985644, + "velocityY": 0.7980807319181392, + "timestamp": 4.2411830159972155 + }, + { + "x": 3.069932459879016, + "y": 5.407606100670878, + "heading": 1.5380181020622108e-17, + "angularVelocity": -9.183800453716929e-17, + "velocityX": -1.9860568636985672, + "velocityY": 0.6854924848554687, + "timestamp": 4.309990167059686 + }, + { + "x": 2.952579630826552, + "y": 5.447418960174395, + "heading": 8.435159362125091e-18, + "angularVelocity": -1.0093459112453562e-16, + "velocityX": -1.7055324517929655, + "velocityY": 0.5786151423035916, + "timestamp": 4.378797318122157 + }, + { + "x": 2.8546367166241713, + "y": 5.480167404246393, + "heading": 5.0453569918778545e-18, + "angularVelocity": -4.926526267025923e-17, + "velocityX": -1.423440916968907, + "velocityY": 0.4759453569334053, + "timestamp": 4.447604469184627 + }, + { + "x": 2.7761830906702163, + "y": 5.506073449022424, + "heading": 3.1520224912430142e-18, + "angularVelocity": -2.7516536752894226e-17, + "velocityX": -1.1401958189305985, + "velocityY": 0.3765022149007732, + "timestamp": 4.516411620247098 + }, + { + "x": 2.7172795158320118, + "y": 5.5253126875621055, + "heading": 4.0055040762053705e-18, + "angularVelocity": 1.2403966322481763e-17, + "velocityX": -0.8560676314693773, + "velocityY": 0.2796110323215353, + "timestamp": 4.5852187713095685 + }, + { + "x": 2.677973947928258, + "y": 5.538027438078825, + "heading": 2.2657574428082367e-18, + "angularVelocity": -2.5284386970987473e-17, + "velocityX": -0.571242484201505, + "velocityY": 0.18478821343985707, + "timestamp": 4.654025922372039 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -8.520572182632915e-28, + "angularVelocity": -3.292909834387365e-17, + "velocityX": -0.2858537153870097, + "velocityY": 0.09167545987855215, + "timestamp": 4.72283307343451 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -3.9086161010585418e-28, + "angularVelocity": 3.4350134480026537e-28, + "velocityX": 1.6920853023952865e-26, + "velocityY": 2.9127538584633355e-26, + "timestamp": 4.79164022449698 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj index 2593024c..cdf83dcb 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.4.traj @@ -1,382 +1,383 @@ { - "samples": [ - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 1.7878647482773802e-40, - "angularVelocity": 2.5983819147247646e-39, - "velocityX": 0, - "velocityY": -2.2530838665454385e-40, - "timestamp": 0 - }, - { - "x": 2.6750735555579293, - "y": 5.538716972759925, - "heading": 1.5037626920568013e-19, - "angularVelocity": 2.3619256993662716e-18, - "velocityX": 0.26337722940288844, - "velocityY": -0.08824680774956185, - "timestamp": 0.06366680765869503 - }, - { - "x": 2.7085920650608806, - "y": 5.52742596598787, - "heading": 4.62580789697953e-19, - "angularVelocity": 4.9037250644942105e-18, - "velocityX": 0.5264675697678678, - "velocityY": -0.17734526336836984, - "timestamp": 0.12733361531739007 - }, - { - "x": 2.7588390755328, - "y": 5.51039889888726, - "heading": 7.215027779377387e-19, - "angularVelocity": 4.066828505487763e-18, - "velocityX": 0.789218312017212, - "velocityY": -0.2674402522565463, - "timestamp": 0.1910004229760851 - }, - { - "x": 2.8257885982701296, - "y": 5.487560532002736, - "heading": -1.3111959461224426e-18, - "angularVelocity": -3.192713438621083e-17, - "velocityX": 1.0515608556382001, - "velocityY": -0.35871701007777496, - "timestamp": 0.25466723063478014 - }, - { - "x": 2.9094088162269207, - "y": 5.458820213911237, - "heading": -3.133450423089841e-18, - "angularVelocity": -2.8621734683732664e-17, - "velocityX": 1.3134036561886782, - "velocityY": -0.4514176090871532, - "timestamp": 0.3183340382934752 - }, - { - "x": 3.009659896203603, - "y": 5.424066617313393, - "heading": -4.721555014357318e-18, - "angularVelocity": -2.494399593240158e-17, - "velocityX": 1.5746208057754199, - "velocityY": -0.5458668005493692, - "timestamp": 0.3820008459521702 - }, - { - "x": 3.1264905621724175, - "y": 5.383159777949491, - "heading": -8.056338252897749e-18, - "angularVelocity": -5.237867832823588e-17, - "velocityX": 1.8350325745107399, - "velocityY": -0.6425143786570167, - "timestamp": 0.44566765361086524 - }, - { - "x": 3.259832420061778, - "y": 5.335918462965804, - "heading": -1.1096621419747018e-17, - "angularVelocity": -4.775303299558587e-17, - "velocityX": 2.094370093191732, - "velocityY": -0.7420085397863474, - "timestamp": 0.5093344612695603 - }, - { - "x": 3.409589860765726, - "y": 5.282098877479992, - "heading": -1.3592361718623002e-17, - "angularVelocity": -3.9200022596627705e-17, - "velocityX": 2.352205901492158, - "velocityY": -0.8453319314253591, - "timestamp": 0.5730012689282553 - }, - { - "x": 3.5756203018815467, - "y": 5.221355824269489, - "heading": -1.4313494756802784e-17, - "angularVelocity": -1.132667185145573e-17, - "velocityX": 2.607802200573299, - "velocityY": -0.9540772569615096, - "timestamp": 0.6366680765869503 - }, - { - "x": 3.7576900306489676, - "y": 5.153163870724839, - "heading": -1.1221400900164693e-17, - "angularVelocity": 4.856681166133233e-17, - "velocityX": 2.8597276267323055, - "velocityY": -1.0710754324327645, - "timestamp": 0.7003348842456454 - }, - { - "x": 3.955353841452516, - "y": 5.076631094268888, - "heading": -7.381711218130638e-18, - "angularVelocity": 6.030912846483351e-17, - "velocityX": 3.104660310018777, - "velocityY": -1.2020828320186705, - "timestamp": 0.7640016919043404 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -4.354722543751261e-18, - "angularVelocity": 4.754421944015892e-17, - "velocityX": 3.332116557247906, - "velocityY": -1.3615147608100584, - "timestamp": 0.8276684995630355 - }, - { - "x": 4.285701077560922, - "y": 4.938388492372474, - "heading": -4.775397572984342e-18, - "angularVelocity": -1.2147965427412122e-17, - "velocityX": 3.4133567656946284, - "velocityY": -1.4888942599373947, - "timestamp": 0.8622977575854378 - }, - { - "x": 4.403715149774036, - "y": 4.881600772971697, - "heading": -7.574216488503896e-18, - "angularVelocity": -8.082237608755476e-17, - "velocityX": 3.4079295645540304, - "velocityY": -1.6398768741750067, - "timestamp": 0.8969270156078402 - }, - { - "x": 4.519367006147829, - "y": 4.820144951729341, - "heading": -1.1070492199966305e-17, - "angularVelocity": -1.009630558414108e-16, - "velocityX": 3.3397151131270637, - "velocityY": -1.7746791225673963, - "timestamp": 0.9315562736302425 - }, - { - "x": 4.6324721141561005, - "y": 4.754119165100519, - "heading": -1.4960169278634737e-17, - "angularVelocity": -1.123234311330653e-16, - "velocityX": 3.266171857771331, - "velocityY": -1.9066474536101443, - "timestamp": 0.9661855316526449 - }, - { - "x": 4.742850082889288, - "y": 4.683628924272633, - "heading": -1.7795441089169654e-17, - "angularVelocity": -8.18750378278602e-17, - "velocityX": 3.187419397256003, - "velocityY": -2.0355689048343195, - "timestamp": 1.0008147896750472 - }, - { - "x": 4.850325220314879, - "y": 4.608787325463071, - "heading": -1.8976395039793544e-17, - "angularVelocity": -3.410277950107739e-17, - "velocityX": 3.1035934225348787, - "velocityY": -2.1612244409379606, - "timestamp": 1.0354440476974496 - }, - { - "x": 4.959315849281407, - "y": 4.536170434444089, - "heading": -1.7824338333790707e-17, - "angularVelocity": 3.3268304658955216e-17, - "velocityX": 3.1473567494868275, - "velocityY": -2.09698085278076, - "timestamp": 1.070073305719852 - }, - { - "x": 5.0711194796065495, - "y": 4.467963995966004, - "heading": -1.5727172733760973e-17, - "angularVelocity": 6.056051211588329e-17, - "velocityX": 3.228588676454312, - "velocityY": -1.9696188244622874, - "timestamp": 1.1047025637422543 - }, - { - "x": 5.1855579490393096, - "y": 4.4042772106470105, - "heading": -1.2316155333836473e-17, - "angularVelocity": 9.850102470338397e-17, - "velocityX": 3.3046757559381787, - "velocityY": -1.839103375469191, - "timestamp": 1.1393318217646566 - }, - { - "x": 5.302448702578612, - "y": 4.345211810292891, - "heading": -8.765491689722714e-18, - "angularVelocity": 1.0253363331714457e-16, - "velocityX": 3.3754911371096727, - "velocityY": -1.7056501850518684, - "timestamp": 1.173961079787059 - }, - { - "x": 5.421605230513571, - "y": 4.290862095789443, - "heading": -5.5119830878393785e-18, - "angularVelocity": 9.395259349127813e-17, - "velocityX": 3.4409206185669707, - "velocityY": -1.5694738382292028, - "timestamp": 1.2085903378094613 - }, - { - "x": 5.542837392691082, - "y": 4.241314822773421, - "heading": -2.184892589584252e-18, - "angularVelocity": 9.60774411078281e-17, - "velocityX": 3.500859362885684, - "velocityY": -1.4307922215361746, - "timestamp": 1.2432195958318637 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 1.1869648608997422e-18, - "angularVelocity": 9.737019049910581e-17, - "velocityX": 3.5552114934161927, - "velocityY": -1.2898268911821018, - "timestamp": 1.277848853854266 - }, - { - "x": 5.934272395491136, - "y": 4.124337934678792, - "heading": 7.497166766936204e-18, - "angularVelocity": 8.587782714305931e-17, - "velocityX": 3.6516733021145136, - "velocityY": -0.9841085377661548, - "timestamp": 1.3513276797160874 - }, - { - "x": 6.2077530174936735, - "y": 4.075010140740357, - "heading": 1.3006105833327096e-17, - "angularVelocity": 7.497315045222091e-17, - "velocityX": 3.7218970063133985, - "velocityY": -0.6713198443208217, - "timestamp": 1.4248065055779087 - }, - { - "x": 6.4844286250858945, - "y": 4.049019985828209, - "heading": 1.5986683133968542e-17, - "angularVelocity": 4.0563757867422685e-17, - "velocityX": 3.7653787243758563, - "velocityY": -0.3537094476852878, - "timestamp": 1.49828533143973 - }, - { - "x": 6.743277363747608, - "y": 4.038426394199003, - "heading": 1.2719246482061777e-17, - "angularVelocity": -4.446773085420891e-17, - "velocityX": 3.522766397335807, - "velocityY": -0.1441720319419269, - "timestamp": 1.5717641573015513 - }, - { - "x": 6.978672822837657, - "y": 4.030025299692623, - "heading": 8.975092196512461e-18, - "angularVelocity": -5.095555408833389e-17, - "velocityX": 3.2035822065626665, - "velocityY": -0.11433354313769856, - "timestamp": 1.6452429831633726 - }, - { - "x": 7.1905526925662695, - "y": 4.022995759457438, - "heading": 5.368720420708233e-18, - "angularVelocity": -4.9080421924352456e-17, - "velocityX": 2.8835500192539434, - "velocityY": -0.09566756344750614, - "timestamp": 1.718721809025194 - }, - { - "x": 7.3789017577018114, - "y": 4.017045849522277, - "heading": 1.5887833889908863e-18, - "angularVelocity": -5.144253446326957e-17, - "velocityX": 2.563310762337649, - "velocityY": -0.08097448299391075, - "timestamp": 1.7922006348870152 - }, - { - "x": 7.54371362899692, - "y": 4.012025997027571, - "heading": -2.7250521222406746e-18, - "angularVelocity": -5.870855257463982e-17, - "velocityX": 2.2429845518359373, - "velocityY": -0.06831699385270525, - "timestamp": 1.8656794607488365 - }, - { - "x": 7.684984887810538, - "y": 4.007845278835319, - "heading": -2.10421445779977e-18, - "angularVelocity": 8.449205021435769e-18, - "velocityX": 1.9226118158077505, - "velocityY": -0.056896910684361174, - "timestamp": 1.9391582866106578 - }, - { - "x": 7.8027134339680275, - "y": 4.0044425825275765, - "heading": -1.435402908569355e-18, - "angularVelocity": 9.102099024937298e-18, - "velocityX": 1.6022104977409413, - "velocityY": -0.0463085285840207, - "timestamp": 2.012637112472479 - }, - { - "x": 7.896897856926731, - "y": 4.001774011196325, - "heading": -8.461914935171916e-19, - "angularVelocity": 8.01879191918765e-18, - "velocityX": 1.2817899830873647, - "velocityY": -0.03631755543113603, - "timestamp": 2.0861159383343004 - }, - { - "x": 7.967537148527336, - "y": 3.999806507404438, - "heading": -3.275924545852618e-19, - "angularVelocity": 7.057802473697343e-18, - "velocityX": 0.9613557480279206, - "velocityY": -0.026776472933672434, - "timestamp": 2.1595947641961217 - }, - { - "x": 8.014630554462236, - "y": 3.9985142796609896, - "heading": -5.3367647403100726e-20, - "angularVelocity": 3.732024892420789e-18, - "velocityX": 0.6409112473226126, - "velocityY": -0.01758639619362703, - "timestamp": 2.233073590057943 - }, - { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 7.342940745159904e-44, - "angularVelocity": 7.262996758203436e-19, - "velocityX": 0.320458791984776, - "velocityY": -0.008677813220192914, - "timestamp": 2.3065524159197643 - }, - { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 1.3038141825006824e-44, - "angularVelocity": -8.034632634038792e-43, - "velocityX": -5.7118786505402475e-43, - "velocityY": 0, - "timestamp": 2.3800312417815856 - } - ] + "samples": [ + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -3.9086161010585418e-28, + "angularVelocity": 3.4350134480026537e-28, + "velocityX": 1.6920853023952865e-26, + "velocityY": 2.9127538584633355e-26, + "timestamp": 0 + }, + { + "x": 2.675073555498551, + "y": 5.538716972612474, + "heading": 1.6968135653440987e-18, + "angularVelocity": 2.665146289407491e-17, + "velocityX": 0.2633772278822265, + "velocityY": -0.08824680986851689, + "timestamp": 0.06366680780083911 + }, + { + "x": 2.7085920648883794, + "y": 5.5274259655637525, + "heading": 3.725095749658628e-18, + "angularVelocity": 3.185776473374715e-17, + "velocityX": 0.5264675668156642, + "velocityY": -0.17734526731797434, + "timestamp": 0.12733361560167822 + }, + { + "x": 2.7588390752001515, + "y": 5.510398898078488, + "heading": 7.631145981641487e-18, + "angularVelocity": 6.13514383217542e-17, + "velocityX": 0.7892183077397765, + "velocityY": -0.267440257701116, + "timestamp": 0.19100042340251733 + }, + { + "x": 2.8257885977384563, + "y": 5.48756053072635, + "heading": 1.178722114456003e-17, + "angularVelocity": 6.527852277238055e-17, + "velocityX": 1.0515608501644154, + "velocityY": -0.3587170166216, + "timestamp": 0.25466723120335644 + }, + { + "x": 2.909408815467416, + "y": 5.4588202121142055, + "heading": 1.707663826690775e-17, + "angularVelocity": 8.307966592022717e-17, + "velocityX": 1.3134036496778398, + "velocityY": -0.45141761625695104, + "timestamp": 0.31833403900419555 + }, + { + "x": 3.0096598952002043, + "y": 5.424066614979104, + "heading": 2.1547525099461292e-17, + "angularVelocity": 7.022319772168498e-17, + "velocityX": 1.5746207984290834, + "velocityY": -0.5458668077692407, + "timestamp": 0.38200084680503466 + }, + { + "x": 3.1264905609256615, + "y": 5.383159775106569, + "heading": 2.7044139747893618e-17, + "angularVelocity": 8.633407011044114e-17, + "velocityX": 1.835032566591432, + "velocityY": -0.6425143852114948, + "timestamp": 0.44566765460587376 + }, + { + "x": 3.259832418594633, + "y": 5.335918459700519, + "heading": 3.2480422722951275e-17, + "angularVelocity": 8.538645430467616e-17, + "velocityX": 2.0943700850541704, + "velocityY": -0.7420085447636868, + "timestamp": 0.5093344624067129 + }, + { + "x": 3.409589859132952, + "y": 5.282098873954419, + "heading": 3.770995869084488e-17, + "angularVelocity": 8.213912631218793e-17, + "velocityX": 2.3522058936390553, + "velocityY": -0.84533193362635, + "timestamp": 0.573001270207552 + }, + { + "x": 3.575620300185956, + "y": 5.221355820749434, + "heading": 4.453751126086424e-17, + "angularVelocity": 1.0723880787820308e-16, + "velocityX": 2.6078021937643934, + "velocityY": -0.9540772547447327, + "timestamp": 0.6366680780083911 + }, + { + "x": 3.7576900290728465, + "y": 5.153163867625262, + "heading": 5.330339940674851e-17, + "angularVelocity": 1.3768380178976122e-16, + "velocityX": 2.8597276222240717, + "velocityY": -1.0710754234370974, + "timestamp": 0.7003348858092302 + }, + { + "x": 3.955353840328095, + "y": 5.076631092230983, + "heading": 6.003878998986586e-17, + "angularVelocity": 1.0579124061222185e-16, + "velocityX": 3.10466031018196, + "velocityY": -1.2020828126594152, + "timestamp": 0.7640016936100693 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 6.251629909739218e-17, + "angularVelocity": 3.89136693523207e-17, + "velocityX": 3.3321165674695394, + "velocityY": -1.361514725761388, + "timestamp": 0.8276685014109084 + }, + { + "x": 4.285701078795114, + "y": 4.9383884935900895, + "heading": 6.191867349467079e-17, + "angularVelocity": -1.7257822842751934e-17, + "velocityX": 3.4133567862477086, + "velocityY": -1.4888942181949925, + "timestamp": 0.8622977595863723 + }, + { + "x": 4.403715151512854, + "y": 4.881600775413148, + "heading": 5.87936361182216e-17, + "angularVelocity": -9.024268901925555e-17, + "velocityX": 3.4079295640631644, + "velocityY": -1.6398768315856658, + "timestamp": 0.8969270177618363 + }, + { + "x": 4.5193670084290005, + "y": 4.8201449553533005, + "heading": 6.393419644700586e-17, + "angularVelocity": 1.484455804018553e-16, + "velocityX": 3.3397151140271815, + "velocityY": -1.7746790805755832, + "timestamp": 0.9315562759373002 + }, + { + "x": 4.63247211701513, + "y": 4.7541191698650405, + "heading": 6.969863736825022e-17, + "angularVelocity": 1.664615768552329e-16, + "velocityX": 3.2661718600217573, + "velocityY": -1.9066474122463712, + "timestamp": 0.9661855341127641 + }, + { + "x": 4.742850086360494, + "y": 4.6836289301368605, + "heading": 7.573971746769133e-17, + "angularVelocity": 1.7445017357345818e-16, + "velocityX": 3.187419400845542, + "velocityY": -2.035568864080495, + "timestamp": 1.000814792288228 + }, + { + "x": 4.850325224446504, + "y": 4.608787332408603, + "heading": 7.95254691851129e-17, + "angularVelocity": 1.0932234523778094e-16, + "velocityX": 3.1035934278880775, + "velocityY": -2.161224400160131, + "timestamp": 1.035444050463692 + }, + { + "x": 4.959315852672914, + "y": 4.536170440415771, + "heading": 8.132510178394684e-17, + "angularVelocity": 5.1968557625797656e-17, + "velocityX": 3.1473567142028136, + "velocityY": -2.0969808716342646, + "timestamp": 1.0700733086391558 + }, + { + "x": 5.071119482307337, + "y": 4.467964000951313, + "heading": 8.196310550599571e-17, + "angularVelocity": 1.842383452283251e-17, + "velocityX": 3.2285886422377668, + "velocityY": -1.969618844240326, + "timestamp": 1.1047025668146198 + }, + { + "x": 5.185557951100297, + "y": 4.404277214638869, + "heading": 8.211438156325687e-17, + "angularVelocity": 4.3684463753045605e-18, + "velocityX": 3.3046757228557744, + "velocityY": -1.8391033960284853, + "timestamp": 1.1393318249900837 + }, + { + "x": 5.302448704050349, + "y": 4.345211813287043, + "heading": 8.111338797415481e-17, + "angularVelocity": -2.8906007282049283e-17, + "velocityX": 3.37549110517396, + "velocityY": -1.7056502063239702, + "timestamp": 1.1739610831655476 + }, + { + "x": 5.421605231445948, + "y": 4.290862097784136, + "heading": 7.868835126918448e-17, + "angularVelocity": -7.002854905419881e-17, + "velocityX": 3.4409205877827787, + "velocityY": -1.5694738601537552, + "timestamp": 1.2085903413410115 + }, + { + "x": 5.542837393133187, + "y": 4.2413148237692955, + "heading": 7.613874923845377e-17, + "angularVelocity": -7.362566122075624e-17, + "velocityX": 3.5008593332540854, + "velocityY": -1.4307922440552452, + "timestamp": 1.2432195995164754 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 7.348031788343025e-17, + "angularVelocity": -7.676835991047679e-17, + "velocityX": 3.5552114649352746, + "velocityY": -1.289826914239228, + "timestamp": 1.2778488576919393 + }, + { + "x": 5.934272394454851, + "y": 4.1243379326499765, + "heading": 6.301578755269389e-17, + "angularVelocity": -1.4241558943688294e-16, + "velocityX": 3.6516732759640362, + "velocityY": -0.9841085621303537, + "timestamp": 1.3513276837961765 + }, + { + "x": 6.207753015610448, + "y": 4.075010136678792, + "heading": 7.880077877117389e-17, + "angularVelocity": 2.148236717340586e-16, + "velocityX": 3.721896982508106, + "velocityY": -0.6713198697704936, + "timestamp": 1.4248065099004137 + }, + { + "x": 6.484428622538185, + "y": 4.049019979746833, + "heading": 4.7380289374867153e-17, + "angularVelocity": -4.2761283844936273e-16, + "velocityX": 3.7653787029102097, + "velocityY": -0.3537094740066765, + "timestamp": 1.4982853360046509 + }, + { + "x": 6.743277361498475, + "y": 4.038426387270388, + "heading": 3.80588957590445e-17, + "angularVelocity": -1.2685822719245393e-16, + "velocityX": 3.5227663897772135, + "velocityY": -0.1441720429966888, + "timestamp": 1.571764162108888 + }, + { + "x": 6.978672820957151, + "y": 4.030025293070568, + "heading": 3.301536102704029e-17, + "angularVelocity": -6.863929378502764e-17, + "velocityX": 3.20358220101046, + "velocityY": -0.11433353858840364, + "timestamp": 1.6452429882131252 + }, + { + "x": 7.190552691043286, + "y": 4.022995753598098, + "heading": 2.761165208046759e-17, + "angularVelocity": -7.354103532969467e-17, + "velocityX": 2.8835500146064224, + "velocityY": -0.09566755275184706, + "timestamp": 1.7187218143173624 + }, + { + "x": 7.378901756507518, + "y": 4.0170458446186466, + "heading": 2.2049007606542194e-17, + "angularVelocity": -7.570404657768667e-17, + "velocityX": 2.563310758354254, + "velocityY": -0.08097446972017466, + "timestamp": 1.7922006404215995 + }, + { + "x": 7.543713628095823, + "y": 4.012025993134882, + "heading": 1.7040082343642412e-17, + "angularVelocity": -6.816828096488101e-17, + "velocityX": 2.2429845484262843, + "velocityY": -0.06831697986905093, + "timestamp": 1.8656794665258367 + }, + { + "x": 7.684984887163916, + "y": 4.007845275924109, + "heading": 1.249754397163312e-17, + "angularVelocity": -6.182105257790516e-17, + "velocityX": 1.922611812928072, + "velocityY": -0.05689689713935573, + "timestamp": 1.9391582926300739 + }, + { + "x": 7.802713433535344, + "y": 4.004442580511056, + "heading": 8.52630628004598e-18, + "angularVelocity": -5.4046014370140334e-17, + "velocityX": 1.6022104953666365, + "velocityY": -0.046308516255087515, + "timestamp": 2.012637118734311 + }, + { + "x": 7.8968978566663255, + "y": 4.001774009946332, + "heading": 5.220002438781615e-18, + "angularVelocity": -4.499668838635962e-17, + "velocityX": 1.2817899812031746, + "velocityY": -0.03631754487937096, + "timestamp": 2.0861159448385482 + }, + { + "x": 7.967537148396796, + "y": 3.9998065067615447, + "heading": 2.6237181268422074e-18, + "angularVelocity": -3.5333775041531244e-17, + "velocityX": 0.9613557466236943, + "velocityY": -0.026776464583087325, + "timestamp": 2.1595947709427854 + }, + { + "x": 8.014630554418627, + "y": 3.998514279441312, + "heading": 9.593365131903801e-19, + "angularVelocity": -2.265117315883944e-17, + "velocityX": 0.6409112463912332, + "velocityY": -0.01758639037590862, + "timestamp": 2.2330735970470226 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 1.1585643597990433e-28, + "angularVelocity": -1.3055958619386531e-17, + "velocityX": 0.3204587915210466, + "velocityY": -0.008677810201891931, + "timestamp": 2.3065524231512597 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 5.650895976930055e-29, + "angularVelocity": -3.8630399964154185e-29, + "velocityX": 1.5845610208926219e-27, + "velocityY": -6.207718033855077e-27, + "timestamp": 2.380031249255497 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBFSprint.traj b/src/main/deploy/choreo/CenterLanePCBFSprint.traj index 5b76f9bb..8d3fbc28 100644 --- a/src/main/deploy/choreo/CenterLanePCBFSprint.traj +++ b/src/main/deploy/choreo/CenterLanePCBFSprint.traj @@ -1,1570 +1,1571 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -1.254857983919809e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.3060574276601378, - "y": 5.572382522924401, - "heading": -0.010694878831235951, - "angularVelocity": -0.1421235514303064, - "velocityX": 0.21372563329485256, - "velocityY": -0.24679919125306551, - "timestamp": 0.07525057405130896 - }, - { - "x": 1.3382235783773917, - "y": 5.535239267164631, - "heading": -0.032076102097718266, - "angularVelocity": -0.2841336898257091, - "velocityX": 0.4274538915144429, - "velocityY": -0.4935943177620345, - "timestamp": 0.15050114810261792 - }, - { - "x": 1.3864734186078633, - "y": 5.479524852099925, - "heading": -0.06412157270911513, - "angularVelocity": -0.42585018142040665, - "velocityX": 0.6411889987319064, - "velocityY": -0.7403852497811617, - "timestamp": 0.22575172215392686 - }, - { - "x": 1.4508078150986874, - "y": 5.405239694992128, - "heading": -0.1067981371766019, - "angularVelocity": -0.567126098451675, - "velocityX": 0.8549356240955547, - "velocityY": -0.9871706368349642, - "timestamp": 0.30100229620523583 - }, - { - "x": 1.5312280234732587, - "y": 5.3123844378954015, - "heading": -0.16006527765002243, - "angularVelocity": -0.7078635763993126, - "velocityX": 1.068698935356638, - "velocityY": -1.233947491673002, - "timestamp": 0.3762528702565448 - }, - { - "x": 1.6277357530197398, - "y": 5.200960116982037, - "heading": -0.2238798805982425, - "angularVelocity": -0.8480281214272752, - "velocityX": 1.2824849612365503, - "velocityY": -1.4807105768852864, - "timestamp": 0.4515034443078538 - }, - { - "x": 1.7403333929369436, - "y": 5.0709684768538175, - "heading": -0.2982015141504559, - "angularVelocity": -0.9876553699462992, - "velocityX": 1.4963027370230912, - "velocityY": -1.727450478198834, - "timestamp": 0.5267540183591627 - }, - { - "x": 1.8690253968085462, - "y": 4.922413250022354, - "heading": -0.3829965804922361, - "angularVelocity": -1.1268361392905244, - "velocityX": 1.7101796961898603, - "velocityY": -1.9741407784113794, - "timestamp": 0.6020045924104717 - }, - { - "x": 2.02184146077192, - "y": 4.778613992669738, - "heading": -0.4624358186091319, - "angularVelocity": -1.0556628851958798, - "velocityX": 2.0307627668195027, - "velocityY": -1.9109390082922875, - "timestamp": 0.6772551664617806 - }, - { - "x": 2.158573003278503, - "y": 4.653386193108499, - "heading": -0.5314192813425986, - "angularVelocity": -0.9167167639888999, - "velocityX": 1.817016603947309, - "velocityY": -1.664144109715068, - "timestamp": 0.7525057405130896 - }, - { - "x": 2.279216333070278, - "y": 4.546726509082428, - "heading": -0.5899522323979595, - "angularVelocity": -0.7778405918177544, - "velocityX": 1.6032213881911423, - "velocityY": -1.417393626160111, - "timestamp": 0.8277563145643986 - }, - { - "x": 2.3837699497753926, - "y": 4.458633448495562, - "heading": -0.6380354012157764, - "angularVelocity": -0.6389741131339646, - "velocityX": 1.3894062340921998, - "velocityY": -1.1706629709704055, - "timestamp": 0.9030068886157075 - }, - { - "x": 2.4722329980367945, - "y": 4.3891060989240405, - "heading": -0.6756677941957627, - "angularVelocity": -0.5000944305598923, - "velocityX": 1.1755797132095953, - "velocityY": -0.9239444409216138, - "timestamp": 0.9782574626670165 - }, - { - "x": 2.5446049418017402, - "y": 4.338143858296765, - "heading": -0.7028477816482255, - "angularVelocity": -0.36119309114484527, - "velocityX": 0.9617460687572824, - "velocityY": -0.6772339117562157, - "timestamp": 1.0535080367183254 - }, - { - "x": 2.6008854517223408, - "y": 4.305746335795738, - "heading": -0.7195735469228228, - "angularVelocity": -0.22226761038098528, - "velocityX": 0.7479080476225236, - "velocityY": -0.430528576145586, - "timestamp": 1.1287586107696344 - }, - { - "x": 2.641074357140525, - "y": 4.291913306081633, - "heading": -0.7258432570432893, - "angularVelocity": -0.08331777131422959, - "velocityX": 0.5340677586239448, - "velocityY": -0.1838262350545031, - "timestamp": 1.2040091848209433 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.05565594681973357, - "velocityX": 0.3202270068209183, - "velocityY": 0.06287502296377193, - "timestamp": 1.2792597588722523 - }, - { - "x": 2.6731092983432756, - "y": 4.320288362117908, - "heading": -0.7067987769295019, - "angularVelocity": 0.19580389973285955, - "velocityX": 0.10461714888899723, - "velocityY": 0.3116194322188291, - "timestamp": 1.3551333149479476 - }, - { - "x": 2.664687958259504, - "y": 4.362805441582899, - "heading": -0.6813130909046112, - "angularVelocity": 0.3358968175826016, - "velocityX": -0.11099176735530734, - "velocityY": 0.5603675597065643, - "timestamp": 1.431006871023643 - }, - { - "x": 2.6399077508131867, - "y": 4.424196327740775, - "heading": -0.6452021974872086, - "angularVelocity": 0.4759351648285144, - "velocityX": -0.32659873516708793, - "velocityY": 0.8091209814393522, - "timestamp": 1.5068804270993383 - }, - { - "x": 2.5987688991007616, - "y": 4.504461543612991, - "heading": -0.5984695371274339, - "angularVelocity": 0.6159281675522111, - "velocityX": -0.5422027626077326, - "velocityY": 1.057881296501775, - "timestamp": 1.5827539831750337 - }, - { - "x": 2.541271696403367, - "y": 4.60360171291691, - "heading": -0.5411163147183906, - "angularVelocity": 0.7559052899984986, - "velocityX": -0.7578029246555089, - "velocityY": 1.3066498320479574, - "timestamp": 1.658627539250729 - }, - { - "x": 2.46741646400873, - "y": 4.721617471023204, - "heading": -0.47313851139747326, - "angularVelocity": 0.8959353803401775, - "velocityX": -0.9733988521888661, - "velocityY": 1.55542674166613, - "timestamp": 1.7345010953264244 - }, - { - "x": 2.3772033191525277, - "y": 4.85850915554647, - "heading": -0.39452182857786383, - "angularVelocity": 1.036153923517168, - "velocityX": -1.1889932345783751, - "velocityY": 1.8042081009834092, - "timestamp": 1.8103746514021197 - }, - { - "x": 2.2706306867395725, - "y": 5.014275344454255, - "heading": -0.30523454109050485, - "angularVelocity": 1.1767905987884617, - "velocityX": -1.404608376447531, - "velocityY": 2.0529707181614056, - "timestamp": 1.886248207477815 - }, - { - "x": 2.1562102192291532, - "y": 5.146460354029385, - "heading": -0.2291038294161038, - "angularVelocity": 1.0033892651564627, - "velocityX": -1.5080414498275487, - "velocityY": 1.7421749607384658, - "timestamp": 1.9621217635535104 - }, - { - "x": 2.0581337319934665, - "y": 5.259758702235753, - "heading": -0.16374377227039283, - "angularVelocity": 0.8614339504706835, - "velocityX": -1.292630691201352, - "velocityY": 1.4932521166502593, - "timestamp": 2.0379953196292058 - }, - { - "x": 1.976403182773577, - "y": 5.354172844449837, - "heading": -0.1092054383318089, - "angularVelocity": 0.7188055596708355, - "velocityX": -1.077194129890758, - "velocityY": 1.244361633992147, - "timestamp": 2.113868875704901 - }, - { - "x": 1.9110187048049692, - "y": 5.429703613246766, - "heading": -0.0655341619901584, - "angularVelocity": 0.575579669661773, - "velocityX": -0.8617558125585749, - "velocityY": 0.9954821245277466, - "timestamp": 2.1897424317805965 - }, - { - "x": 1.861980199634934, - "y": 5.486351461082553, - "heading": -0.03276467227497517, - "angularVelocity": 0.4318960572088904, - "velocityX": -0.6463187928148273, - "velocityY": 0.7466085783539637, - "timestamp": 2.265615987856292 - }, - { - "x": 1.8292876463834091, - "y": 5.5241166468364415, - "heading": -0.01091805858135444, - "angularVelocity": 0.2879344902726512, - "velocityX": -0.4308820482661345, - "velocityY": 0.4977384441630725, - "timestamp": 2.341489543931987 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.72551505430432e-32, - "angularVelocity": 0.1438980739292335, - "velocityX": -0.21544334611501834, - "velocityY": 0.2488695893337634, - "timestamp": 2.4173631000076825 - }, - { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": -2.2798037235511704e-30, - "angularVelocity": -3.1967802998211905e-29, - "velocityX": 0, - "velocityY": 3.158110370432563e-31, - "timestamp": 2.493236656083378 - }, - { - "x": 1.8507557561824508, - "y": 5.5430527114919315, - "heading": 1.2021210509026438e-21, - "angularVelocity": 1.2912282770144636e-20, - "velocityX": 0.40617567078743805, - "velocityY": 0.0005740544395093852, - "timestamp": 2.5863356901257513 - }, - { - "x": 1.9263848800936068, - "y": 5.543159599317717, - "heading": 3.6194046492049965e-21, - "angularVelocity": 2.5964647465645482e-20, - "velocityX": 0.8123513276919048, - "velocityY": 0.00114810885939775, - "timestamp": 2.6794347241681247 - }, - { - "x": 2.039828562987611, - "y": 5.543319931052194, - "heading": -1.5854010106883968e-20, - "angularVelocity": -2.0916881637274291e-19, - "velocityX": 1.2185269596070247, - "velocityY": 0.0017221632439682793, - "timestamp": 2.772533758210498 - }, - { - "x": 2.1910867994360017, - "y": 5.543533706687691, - "heading": 4.514872599726518e-21, - "angularVelocity": 2.1878726150197974e-19, - "velocityX": 1.624702533213676, - "velocityY": 0.0022962175461305355, - "timestamp": 2.8656327922528715 - }, - { - "x": 2.380159562296481, - "y": 5.5438009261858445, - "heading": -2.4606104769465887e-21, - "angularVelocity": -7.492540764169747e-20, - "velocityX": 2.0308778152781284, - "velocityY": 0.002870271436251635, - "timestamp": 2.958731826295245 - }, - { - "x": 2.5314177987448714, - "y": 5.544014701821341, - "heading": 1.8729877149138444e-20, - "angularVelocity": 2.2761232534851326e-19, - "velocityX": 1.6247025332136762, - "velocityY": 0.002296217546130536, - "timestamp": 3.0518308603376183 - }, - { - "x": 2.6448614816388756, - "y": 5.544175033555818, - "heading": 3.8900824798152035e-20, - "angularVelocity": 2.1666119156330769e-19, - "velocityX": 1.218526959607025, - "velocityY": 0.0017221632439682793, - "timestamp": 3.1449298943799917 - }, - { - "x": 2.7204906055500317, - "y": 5.544281921381604, - "heading": 1.1738818455912713e-20, - "angularVelocity": -2.9175390079640015e-19, - "velocityX": 0.8123513276919049, - "velocityY": 0.00114810885939775, - "timestamp": 3.238028928422365 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -1.0197992544535218e-37, - "angularVelocity": -1.260895838142623e-19, - "velocityX": 0.40617567078743805, - "velocityY": 0.0005740544395093853, - "timestamp": 3.3311279624647385 - }, - { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "angularVelocity": 9.592877809620058e-37, - "velocityX": 0, - "velocityY": 7.039095899612751e-38, - "timestamp": 3.424226996507112 - }, - { - "x": 2.7765323823959327, - "y": 5.537624192256726, - "heading": 9.00740418544712e-19, - "angularVelocity": 1.3499597384555195e-17, - "velocityX": 0.2731753218474804, - "velocityY": -0.10058184592921769, - "timestamp": 3.490950498911381 - }, - { - "x": 2.812967100056819, - "y": 5.524148553837151, - "heading": 1.885084819596924e-20, - "angularVelocity": -1.3217075521281405e-17, - "velocityX": 0.5460552331340873, - "velocityY": -0.2019623960674071, - "timestamp": 3.55767400131565 - }, - { - "x": 2.8675858546495747, - "y": 5.503845620088409, - "heading": 3.43159270927708e-20, - "angularVelocity": 2.317785843530086e-19, - "velocityX": 0.8185834469813632, - "velocityY": -0.30428459264216856, - "timestamp": 3.6243975037199188 - }, - { - "x": 2.9403602428294473, - "y": 5.476640214649627, - "heading": 3.5333541604123114e-18, - "angularVelocity": 5.244086577062937e-17, - "velocityX": 1.0906859735710903, - "velocityY": -0.40773347409056915, - "timestamp": 3.6911210061241877 - }, - { - "x": 3.031255192543775, - "y": 5.4424407847147105, - "heading": 5.715508292123115e-18, - "angularVelocity": 3.2704430269696984e-17, - "velocityX": 1.3622628674916857, - "velocityY": -0.5125544778465974, - "timestamp": 3.7578445085284566 - }, - { - "x": 3.140226319352468, - "y": 5.40113340648718, - "heading": 6.027086577670902e-18, - "angularVelocity": 4.669693201563416e-18, - "velocityX": 1.6331745619175013, - "velocityY": -0.619082882928634, - "timestamp": 3.8245680109327256 - }, - { - "x": 3.267215684627595, - "y": 5.352572466235242, - "heading": 7.223223650198719e-18, - "angularVelocity": 1.7926772867621648e-17, - "velocityX": 1.9032179172147636, - "velocityY": -0.7277936334593704, - "timestamp": 3.8912915133369945 - }, - { - "x": 3.41214454573205, - "y": 5.296565350322459, - "heading": 8.483175715581024e-18, - "angularVelocity": 1.888318238685755e-17, - "velocityX": 2.172081139061778, - "velocityY": -0.8393911274837239, - "timestamp": 3.9580150157412635 - }, - { - "x": 3.5748998961264853, - "y": 5.232845435249424, - "heading": 1.0703776873779667e-17, - "angularVelocity": 3.3280644423357047e-17, - "velocityX": 2.4392507067198372, - "velocityY": -0.9549845673113, - "timestamp": 4.024738518145532 - }, - { - "x": 3.755306479840233, - "y": 5.161019665655948, - "heading": 1.3781541182824059e-17, - "angularVelocity": 4.612713958550321e-17, - "velocityX": 2.7037936740893525, - "velocityY": -1.0764688154152113, - "timestamp": 4.091462020549801 - }, - { - "x": 3.953058178777003, - "y": 5.080452170332065, - "heading": 1.6552203248378283e-17, - "angularVelocity": 4.15245298240439e-17, - "velocityX": 2.9637487813307404, - "velocityY": -1.2074830070480274, - "timestamp": 4.15818552295407 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 2.0019563257216408e-17, - "angularVelocity": 5.196609716079013e-17, - "velocityX": 3.2138733563909483, - "velocityY": -1.356409229176722, - "timestamp": 4.224909025358339 - }, - { - "x": 4.282917561329041, - "y": 4.939413083786088, - "heading": 2.2898103599715735e-17, - "angularVelocity": 8.297955477827177e-17, - "velocityX": 3.327163862876717, - "velocityY": -1.456761903756842, - "timestamp": 4.25959877936104 - }, - { - "x": 4.401833285665519, - "y": 4.884962621939577, - "heading": 2.513067280390373e-17, - "angularVelocity": 6.43581734252565e-17, - "velocityX": 3.4279783110372324, - "velocityY": -1.5696410485433538, - "timestamp": 4.2942885333637415 - }, - { - "x": 4.518840762702163, - "y": 4.825621230712266, - "heading": 2.8188632508696854e-17, - "angularVelocity": 8.815166877675102e-17, - "velocityX": 3.372969350763695, - "velocityY": -1.7106316528704353, - "timestamp": 4.328978287366443 - }, - { - "x": 4.633380439466489, - "y": 4.761646132811981, - "heading": 3.13421659224765e-17, - "angularVelocity": 9.090676784661175e-17, - "velocityX": 3.3018301817708395, - "velocityY": -1.8442073096080187, - "timestamp": 4.363668041369144 - }, - { - "x": 4.745269125938669, - "y": 4.693140128704652, - "heading": 3.2482743151706595e-17, - "angularVelocity": 3.287936919763874e-17, - "velocityX": 3.2254102022016937, - "velocityY": -1.9748195418737586, - "timestamp": 4.398357795371846 - }, - { - "x": 4.854330043574812, - "y": 4.6202164218202615, - "heading": 3.23593373316049e-17, - "angularVelocity": -3.55741410250663e-18, - "velocityX": 3.1438942353886365, - "velocityY": -2.1021684638845084, - "timestamp": 4.433047549374547 - }, - { - "x": 4.962829604926255, - "y": 4.546460093483164, - "heading": 3.1455405916033935e-17, - "angularVelocity": -2.605759082352001e-17, - "velocityX": 3.1277120426680747, - "velocityY": -2.1261704055714574, - "timestamp": 4.467737303377248 - }, - { - "x": 5.07386693975175, - "y": 4.476582621829349, - "heading": 2.90082292000263e-17, - "angularVelocity": -7.054465464981545e-17, - "velocityX": 3.200868326043711, - "velocityY": -2.014354776005967, - "timestamp": 4.50242705737995 - }, - { - "x": 5.187610618884458, - "y": 4.411202761148161, - "heading": 2.619388391387083e-17, - "angularVelocity": -8.112900673600335e-17, - "velocityX": 3.2788839933500147, - "velocityY": -1.884702343985982, - "timestamp": 4.537116811382651 - }, - { - "x": 5.303878902064225, - "y": 4.350425816718677, - "heading": 2.3840017411584327e-17, - "angularVelocity": -6.785480525757533e-17, - "velocityX": 3.35166064223783, - "velocityY": -1.7520142813566866, - "timestamp": 4.571806565385352 - }, - { - "x": 5.422485692712026, - "y": 4.294349264150295, - "heading": 2.0445926478566092e-17, - "angularVelocity": -9.784130878396876e-17, - "velocityX": 3.419072693296267, - "velocityY": -1.6165162936588033, - "timestamp": 4.606496319388054 - }, - { - "x": 5.5432410890609845, - "y": 4.243062963447586, - "heading": 1.6758899100249362e-17, - "angularVelocity": -1.0628577469963028e-16, - "velocityX": 3.4810104545438403, - "velocityY": -1.478427915594755, - "timestamp": 4.641186073390755 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 1.3081934400390577e-17, - "angularVelocity": -1.0599569831404499e-16, - "velocityX": 3.537374169625408, - "velocityY": -1.337971116472263, - "timestamp": 4.675875827393456 - }, - { - "x": 5.795129656789146, - "y": 4.153827522905326, - "heading": 9.653855416146634e-18, - "angularVelocity": -9.526631807314967e-17, - "velocityX": 3.589854735093561, - "velocityY": -1.1900109590649786, - "timestamp": 4.711859993188672 - }, - { - "x": 5.925973457628177, - "y": 4.116403970594212, - "heading": 6.773802428340066e-18, - "angularVelocity": -8.003667513641558e-17, - "velocityX": 3.636149343676849, - "velocityY": -1.0400005525788714, - "timestamp": 4.747844158983888 - }, - { - "x": 6.058257666523593, - "y": 4.0844428903853265, - "heading": 3.7266948743050956e-18, - "angularVelocity": -8.467912168301893e-17, - "velocityX": 3.6761782848667472, - "velocityY": -0.8881984479166671, - "timestamp": 4.783828324779104 - }, - { - "x": 6.1917543385226965, - "y": 4.0579993432791355, - "heading": 2.3240409715281496e-19, - "angularVelocity": -9.710634385796704e-17, - "velocityX": 3.7098726356150347, - "velocityY": -0.7348661980016377, - "timestamp": 4.81981249057432 - }, - { - "x": 6.326233441808095, - "y": 4.037118888120514, - "heading": -2.9507025724087755e-18, - "angularVelocity": -8.845853722652732e-17, - "velocityX": 3.7371744019498516, - "velocityY": -0.5802678677464012, - "timestamp": 4.855796656369535 - }, - { - "x": 6.461463252337996, - "y": 4.021837506709874, - "heading": -5.178975495681446e-18, - "angularVelocity": -6.192370655342397e-17, - "velocityX": 3.7580365569536736, - "velocityY": -0.4246696032250841, - "timestamp": 4.891780822164751 - }, - { - "x": 6.596863602394611, - "y": 4.012202736620321, - "heading": -4.3543144284076125e-18, - "angularVelocity": 2.2917331805219353e-17, - "velocityX": 3.7627758505553097, - "velocityY": -0.26775026950420705, - "timestamp": 4.927764987959967 - }, - { - "x": 6.727539043403903, - "y": 4.005664590276287, - "heading": -3.5766746235771005e-18, - "angularVelocity": 2.161061087932747e-17, - "velocityX": 3.6314706238561003, - "velocityY": -0.1816950928150608, - "timestamp": 4.963749153755183 - }, - { - "x": 6.85298501008117, - "y": 4.001263406795894, - "heading": -2.645512635943814e-18, - "angularVelocity": 2.5876992478309148e-17, - "velocityX": 3.4861435274385992, - "velocityY": -0.122308892901394, - "timestamp": 4.999733319550399 - }, - { - "x": 6.973061817653349, - "y": 3.9986189838302026, - "heading": -1.6196069084437975e-18, - "angularVelocity": 2.850992109492732e-17, - "velocityX": 3.336934591051298, - "velocityY": -0.07348851660866845, - "timestamp": 5.035717485345614 - }, - { - "x": 7.0877080332373295, - "y": 3.99753108954472, - "heading": -5.829248205352589e-19, - "angularVelocity": 2.880939616045139e-17, - "velocityX": 3.186018434786799, - "velocityY": -0.030232583177662708, - "timestamp": 5.07170165114083 - }, - { - "x": 7.196889877319336, - "y": 3.9978766441345215, - "heading": 0, - "angularVelocity": 1.619948128975407e-17, - "velocityX": 3.034163545803906, - "velocityY": 0.009602962363163213, - "timestamp": 5.107685816936046 - }, - { - "x": 7.37338840046626, - "y": 4.002743213470309, - "heading": 8.093787297850302e-19, - "angularVelocity": 1.2673214059219633e-17, - "velocityX": 2.7636055688801218, - "velocityY": 0.07620051362427777, - "timestamp": 5.171551125768248 - }, - { - "x": 7.532268457404618, - "y": 4.010110422409507, - "heading": 7.012678022232893e-19, - "angularVelocity": -1.6927958157108362e-18, - "velocityX": 2.487736454164719, - "velocityY": 0.11535541084684194, - "timestamp": 5.235416434600451 - }, - { - "x": 7.673406960294269, - "y": 4.018856350887051, - "heading": 5.093320205791772e-19, - "angularVelocity": -3.0053214359033797e-18, - "velocityX": 2.2099400358412575, - "velocityY": 0.13694333649152024, - "timestamp": 5.299281743432653 - }, - { - "x": 7.796760795915294, - "y": 4.028209945243461, - "heading": 2.737170340594828e-19, - "angularVelocity": -3.689248370192142e-18, - "velocityX": 1.931468552749354, - "velocityY": 0.14645814022421355, - "timestamp": 5.363147052264855 - }, - { - "x": 7.902319650014668, - "y": 4.037611597874035, - "heading": 3.811839387951723e-20, - "angularVelocity": -3.688992419979718e-18, - "velocityX": 1.65283556956901, - "velocityY": 0.14721063441931925, - "timestamp": 5.4270123610970575 - }, - { - "x": 7.990087417936277, - "y": 4.046637814965993, - "heading": -1.8999762440941744e-19, - "angularVelocity": -3.5718298785688795e-18, - "velocityX": 1.3742635794998819, - "velocityY": 0.14133208242480425, - "timestamp": 5.49087766992926 - }, - { - "x": 8.06007417147421, - "y": 4.0549574599073335, - "heading": -2.1124739162594107e-19, - "angularVelocity": -3.327278549958625e-19, - "velocityX": 1.0958492931086379, - "velocityY": 0.1302686089438499, - "timestamp": 5.554742978761462 - }, - { - "x": 8.112292461632407, - "y": 4.062304767499659, - "heading": -1.9392441616154913e-19, - "angularVelocity": 2.712423345539538e-19, - "velocityX": 0.8176315297463507, - "velocityY": 0.11504379649410336, - "timestamp": 5.6186082875936645 - }, - { - "x": 8.146755551617066, - "y": 4.068461861174775, - "heading": -1.2666965805994144e-19, - "angularVelocity": 1.0530718371509655e-18, - "velocityX": 0.5396214410425377, - "velocityY": 0.0964074829935333, - "timestamp": 5.682473596425867 - }, - { - "x": 8.1634765625, - "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 1.9833875444262685e-18, - "velocityX": 0.26181680146361314, - "velocityY": 0.07492478756156581, - "timestamp": 5.746338905258069 - }, - { - "x": 8.160404735397709, - "y": 4.0766649695322, - "heading": 2.2110153580287816e-19, - "angularVelocity": 3.145700398787454e-18, - "velocityX": -0.04370411858778341, - "velocityY": 0.04862945386701764, - "timestamp": 5.816625807306458 - }, - { - "x": 8.135851784135758, - "y": 4.07831805033267, - "heading": 5.233120115827785e-19, - "angularVelocity": 4.299669881203419e-18, - "velocityX": -0.34932470412548816, - "velocityY": 0.02351904483329859, - "timestamp": 5.8869127093548475 - }, - { - "x": 8.089809855784104, - "y": 4.078304537638241, - "heading": 9.059382457941767e-19, - "angularVelocity": 5.443777191210811e-18, - "velocityX": -0.6550570164545902, - "velocityY": -0.00019225053366650092, - "timestamp": 5.957199611403237 - }, - { - "x": 8.022270134044854, - "y": 4.076742304755474, - "heading": 1.3681480133267195e-18, - "angularVelocity": 6.5760441001468545e-18, - "velocityX": -0.9609147617966158, - "velocityY": -0.022226515001204008, - "timestamp": 6.027486513451626 - }, - { - "x": 7.9332227395210335, - "y": 4.0737752063023285, - "heading": 1.9089237482918726e-18, - "angularVelocity": 7.693833690281903e-18, - "velocityX": -1.2669130652893827, - "velocityY": -0.04221410201153685, - "timestamp": 6.097773415500015 - }, - { - "x": 7.822656701940191, - "y": 4.069582704513375, - "heading": 2.0768842809054915e-18, - "angularVelocity": 2.3896419918926418e-18, - "velocityX": -1.5730674472567026, - "velocityY": -0.05964840769432409, - "timestamp": 6.168060317548404 - }, - { - "x": 7.690560152144021, - "y": 4.064394900776391, - "heading": 2.063501960546536e-18, - "angularVelocity": -1.9039564936380514e-19, - "velocityX": -1.8793906965088152, - "velocityY": -0.07380896846772396, - "timestamp": 6.238347219596793 - }, - { - "x": 7.536921137087825, - "y": 4.058517363178713, - "heading": 1.8242393677495476e-18, - "angularVelocity": -3.404085054560583e-18, - "velocityX": -2.185884006531148, - "velocityY": -0.08362208927108938, - "timestamp": 6.3086341216451824 - }, - { - "x": 7.361730243526304, - "y": 4.0523752131799995, - "heading": 1.7449161631907716e-18, - "angularVelocity": -1.128563106982712e-18, - "velocityX": -2.492511242577032, - "velocityY": -0.08738683623422486, - "timestamp": 6.378921023693572 - }, - { - "x": 7.164988961998875, - "y": 4.046599373609111, - "heading": 1.6203310877610785e-18, - "angularVelocity": -1.7725219321907188e-18, - "velocityX": -2.799117272119643, - "velocityY": -0.0821751905769253, - "timestamp": 6.449207925741961 - }, - { - "x": 6.946739461316107, - "y": 4.042219879396985, - "heading": 9.714507263424963e-19, - "angularVelocity": -9.231881652246047e-18, - "velocityX": -3.105123349048912, - "velocityY": -0.062308824041090796, - "timestamp": 6.51949482779035 - }, - { - "x": 6.707199372936614, - "y": 4.041196558011354, - "heading": 4.304213175776067e-18, - "angularVelocity": 4.741655062759286e-17, - "velocityX": -3.408033095761992, - "velocityY": -0.014559204571673643, - "timestamp": 6.589781729838739 - }, - { - "x": 6.4477381349567215, - "y": 4.048401250319157, - "heading": 3.981085161265745e-18, - "angularVelocity": -4.597272110228882e-18, - "velocityX": -3.691459296374506, - "velocityY": 0.102504052644734, - "timestamp": 6.660068631887128 - }, - { - "x": 6.183416579926013, - "y": 4.076604236120931, - "heading": 3.783709688486887e-18, - "angularVelocity": -2.808140165901377e-18, - "velocityX": -3.7606089801587, - "velocityY": 0.4012552122777795, - "timestamp": 6.730355533935517 - }, - { - "x": 5.922248771081443, - "y": 4.126128703964576, - "heading": 4.4649221139499255e-18, - "angularVelocity": 9.691882920007657e-18, - "velocityX": -3.7157393658460984, - "velocityY": 0.7046045052540438, - "timestamp": 6.800642435983907 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 6.738429572739451e-18, - "angularVelocity": 3.2346104217601064e-17, - "velocityX": -3.646441012355221, - "velocityY": 1.003321650758162, - "timestamp": 6.870929338032296 - }, - { - "x": 5.5410942529337, - "y": 4.236435293525444, - "heading": 7.176696434761227e-18, - "angularVelocity": 1.2648537308401216e-17, - "velocityX": -3.603431102925481, - "velocityY": 1.1482444113922432, - "timestamp": 6.90557894623092 - }, - { - "x": 5.417926513758212, - "y": 4.281179467560016, - "heading": 6.902122193005105e-18, - "angularVelocity": -7.924310144639137e-18, - "velocityX": -3.554664701241286, - "velocityY": 1.2913327555706813, - "timestamp": 6.9402285544295435 - }, - { - "x": 5.296645297491296, - "y": 4.3308101048073215, - "heading": 7.073122455788382e-18, - "angularVelocity": 4.9351283224632194e-18, - "velocityX": -3.5002189800153154, - "velocityY": 1.432357819540261, - "timestamp": 6.974878162628167 - }, - { - "x": 5.177444375118266, - "y": 4.38524790502043, - "heading": 7.030277898528261e-18, - "angularVelocity": -1.2365091407245627e-18, - "velocityX": -3.44018095932656, - "velocityY": 1.5710942502164025, - "timestamp": 7.009527770826791 - }, - { - "x": 5.06051419213652, - "y": 4.444405885414455, - "heading": 6.93623096110632e-18, - "angularVelocity": -2.7142280190538443e-18, - "velocityX": -3.3746466139374856, - "velocityY": 1.7073203268248038, - "timestamp": 7.044177379025415 - }, - { - "x": 4.9460415627669985, - "y": 4.508189517767328, - "heading": 6.830736149305672e-18, - "angularVelocity": -3.0446177398576756e-18, - "velocityX": -3.303720743776551, - "velocityY": 1.8408182853682935, - "timestamp": 7.078826987224039 - }, - { - "x": 4.834209367418348, - "y": 4.576496874327604, - "heading": 6.686796334021e-18, - "angularVelocity": -4.154154195902009e-18, - "velocityX": -3.2275168800637117, - "velocityY": 1.971374572800783, - "timestamp": 7.113476595422663 - }, - { - "x": 4.725196244628599, - "y": 4.649218770197504, - "heading": 6.8822518039480765e-18, - "angularVelocity": 5.6409142870147405e-18, - "velocityX": -3.1461574446916942, - "velocityY": 2.0987797453014108, - "timestamp": 7.1481262036212865 - }, - { - "x": 4.61917611476972, - "y": 4.726238684910051, - "heading": 7.643017507811422e-18, - "angularVelocity": 2.195596843411265e-17, - "velocityX": -3.0597786056088934, - "velocityY": 2.2228220957374782, - "timestamp": 7.18277581181991 - }, - { - "x": 4.510162876116158, - "y": 4.798960406973094, - "heading": 8.156998265512583e-18, - "angularVelocity": 1.483366723094954e-17, - "velocityX": -3.14616078856249, - "velocityY": 2.098774729173764, - "timestamp": 7.217425420018534 - }, - { - "x": 4.3983305631985266, - "y": 4.867267570914994, - "heading": 9.169956130720048e-18, - "angularVelocity": 2.9234323788045264e-17, - "velocityX": -3.227520273146238, - "velocityY": 1.9713690137660609, - "timestamp": 7.252075028217158 - }, - { - "x": 4.28385782617867, - "y": 4.93105099911187, - "heading": 9.858535854282379e-18, - "angularVelocity": 1.9872655402484013e-17, - "velocityX": -3.3037238506034297, - "velocityY": 1.840812393353667, - "timestamp": 7.286724636415782 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 9.428613355674913e-18, - "angularVelocity": -1.2407716016383212e-17, - "velocityX": -3.3581551662140523, - "velocityY": 1.6997824742615957, - "timestamp": 7.321374244614406 - }, - { - "x": 3.9412126137988053, - "y": 5.086809869447978, - "heading": 6.455008109104801e-18, - "angularVelocity": -4.321651467897992e-17, - "velocityX": -3.28870544216361, - "velocityY": 1.4077326603917448, - "timestamp": 7.390181395507035 - }, - { - "x": 3.7303194478817963, - "y": 5.1698988995186355, - "heading": 5.431152098522906e-18, - "angularVelocity": -1.488008146391029e-17, - "velocityX": -3.0649890771686916, - "velocityY": 1.2075638795204993, - "timestamp": 7.458988546399665 - }, - { - "x": 3.537102982396402, - "y": 5.24230220978644, - "heading": 5.612600329775147e-18, - "angularVelocity": 2.6370548540132515e-18, - "velocityX": -2.8080869935582533, - "velocityY": 1.052264326142308, - "timestamp": 7.527795697292294 - }, - { - "x": 3.3623900460315483, - "y": 5.305525654536907, - "heading": 4.225050788166454e-18, - "angularVelocity": -2.0165775266205907e-17, - "velocityX": -2.5391683000722396, - "velocityY": 0.918849915020073, - "timestamp": 7.596602848184924 - }, - { - "x": 3.2065873746476083, - "y": 5.360439315991953, - "heading": 1.940037667439135e-18, - "angularVelocity": -3.3208948359058894e-17, - "velocityX": -2.2643383625498803, - "velocityY": 0.7980807335088809, - "timestamp": 7.6654099990775535 - }, - { - "x": 3.069932459985333, - "y": 5.407606100907469, - "heading": -1.0877040544793261e-18, - "angularVelocity": -4.4003300276785115e-17, - "velocityX": -1.986056868936749, - "velocityY": 0.6854924859353159, - "timestamp": 7.734217149970183 - }, - { - "x": 2.9525796309081422, - "y": 5.44741896036095, - "heading": -3.706402695175446e-18, - "angularVelocity": -3.805852453885881e-17, - "velocityX": -1.705532456362192, - "velocityY": 0.5786151430046301, - "timestamp": 7.803024300862813 - }, - { - "x": 2.854636716681842, - "y": 5.480167404381309, - "heading": -5.982131114439951e-18, - "angularVelocity": -3.307401032802041e-17, - "velocityX": -1.4234409208300893, - "velocityY": 0.4759453573577233, - "timestamp": 7.871831451755442 - }, - { - "x": 2.776183090706554, - "y": 5.506073449109121, - "heading": -4.835349945984594e-18, - "angularVelocity": 1.666659865403876e-17, - "velocityX": -1.1401958220550505, - "velocityY": 0.3765022151293121, - "timestamp": 7.940638602648072 - }, - { - "x": 2.7172795158509544, - "y": 5.525312687608081, - "heading": -3.43564479511919e-18, - "angularVelocity": 2.034243727152086e-17, - "velocityX": -0.8560676338352585, - "velocityY": 0.2796110324199007, - "timestamp": 8.009445753540701 - }, - { - "x": 2.6779739479348046, - "y": 5.538027438094956, - "heading": -1.81505607662002e-18, - "angularVelocity": 2.355262058485764e-17, - "velocityX": -0.5712424857916886, - "velocityY": 0.18478821346223093, - "timestamp": 8.07825290443333 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -7.816373438340897e-46, - "angularVelocity": 2.6378887267870092e-17, - "velocityX": -0.2858537161877381, - "velocityY": 0.09167545987040067, - "timestamp": 8.14706005532596 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 1.7878647482773802e-40, - "angularVelocity": 2.5983819147247646e-39, - "velocityX": 0, - "velocityY": -2.2530838665454385e-40, - "timestamp": 8.21586720621859 - }, - { - "x": 2.6750735555579293, - "y": 5.538716972759925, - "heading": 1.5037626920568013e-19, - "angularVelocity": 2.3619256993662716e-18, - "velocityX": 0.26337722940288844, - "velocityY": -0.08824680774956185, - "timestamp": 8.279534013877285 - }, - { - "x": 2.7085920650608806, - "y": 5.52742596598787, - "heading": 4.62580789697953e-19, - "angularVelocity": 4.9037250644942105e-18, - "velocityX": 0.5264675697678678, - "velocityY": -0.17734526336836984, - "timestamp": 8.34320082153598 - }, - { - "x": 2.7588390755328, - "y": 5.51039889888726, - "heading": 7.215027779377387e-19, - "angularVelocity": 4.066828505487763e-18, - "velocityX": 0.789218312017212, - "velocityY": -0.2674402522565463, - "timestamp": 8.406867629194675 - }, - { - "x": 2.8257885982701296, - "y": 5.487560532002736, - "heading": -1.3111959461224426e-18, - "angularVelocity": -3.192713438621083e-17, - "velocityX": 1.0515608556382001, - "velocityY": -0.35871701007777496, - "timestamp": 8.47053443685337 - }, - { - "x": 2.9094088162269207, - "y": 5.458820213911237, - "heading": -3.133450423089841e-18, - "angularVelocity": -2.8621734683732664e-17, - "velocityX": 1.3134036561886782, - "velocityY": -0.4514176090871532, - "timestamp": 8.534201244512065 - }, - { - "x": 3.009659896203603, - "y": 5.424066617313393, - "heading": -4.721555014357318e-18, - "angularVelocity": -2.494399593240158e-17, - "velocityX": 1.5746208057754199, - "velocityY": -0.5458668005493692, - "timestamp": 8.59786805217076 - }, - { - "x": 3.1264905621724175, - "y": 5.383159777949491, - "heading": -8.056338252897749e-18, - "angularVelocity": -5.237867832823588e-17, - "velocityX": 1.8350325745107399, - "velocityY": -0.6425143786570167, - "timestamp": 8.661534859829455 - }, - { - "x": 3.259832420061778, - "y": 5.335918462965804, - "heading": -1.1096621419747018e-17, - "angularVelocity": -4.775303299558587e-17, - "velocityX": 2.094370093191732, - "velocityY": -0.7420085397863474, - "timestamp": 8.72520166748815 - }, - { - "x": 3.409589860765726, - "y": 5.282098877479992, - "heading": -1.3592361718623002e-17, - "angularVelocity": -3.9200022596627705e-17, - "velocityX": 2.352205901492158, - "velocityY": -0.8453319314253591, - "timestamp": 8.788868475146845 - }, - { - "x": 3.5756203018815467, - "y": 5.221355824269489, - "heading": -1.4313494756802784e-17, - "angularVelocity": -1.132667185145573e-17, - "velocityX": 2.607802200573299, - "velocityY": -0.9540772569615096, - "timestamp": 8.85253528280554 - }, - { - "x": 3.7576900306489676, - "y": 5.153163870724839, - "heading": -1.1221400900164693e-17, - "angularVelocity": 4.856681166133233e-17, - "velocityX": 2.8597276267323055, - "velocityY": -1.0710754324327645, - "timestamp": 8.916202090464235 - }, - { - "x": 3.955353841452516, - "y": 5.076631094268888, - "heading": -7.381711218130638e-18, - "angularVelocity": 6.030912846483351e-17, - "velocityX": 3.104660310018777, - "velocityY": -1.2020828320186705, - "timestamp": 8.97986889812293 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -4.354722543751261e-18, - "angularVelocity": 4.754421944015892e-17, - "velocityX": 3.332116557247906, - "velocityY": -1.3615147608100584, - "timestamp": 9.043535705781625 - }, - { - "x": 4.285701077560922, - "y": 4.938388492372474, - "heading": -4.775397572984342e-18, - "angularVelocity": -1.2147965427412122e-17, - "velocityX": 3.4133567656946284, - "velocityY": -1.4888942599373947, - "timestamp": 9.078164963804028 - }, - { - "x": 4.403715149774036, - "y": 4.881600772971697, - "heading": -7.574216488503896e-18, - "angularVelocity": -8.082237608755476e-17, - "velocityX": 3.4079295645540304, - "velocityY": -1.6398768741750067, - "timestamp": 9.11279422182643 - }, - { - "x": 4.519367006147829, - "y": 4.820144951729341, - "heading": -1.1070492199966305e-17, - "angularVelocity": -1.009630558414108e-16, - "velocityX": 3.3397151131270637, - "velocityY": -1.7746791225673963, - "timestamp": 9.147423479848833 - }, - { - "x": 4.6324721141561005, - "y": 4.754119165100519, - "heading": -1.4960169278634737e-17, - "angularVelocity": -1.123234311330653e-16, - "velocityX": 3.266171857771331, - "velocityY": -1.9066474536101443, - "timestamp": 9.182052737871235 - }, - { - "x": 4.742850082889288, - "y": 4.683628924272633, - "heading": -1.7795441089169654e-17, - "angularVelocity": -8.18750378278602e-17, - "velocityX": 3.187419397256003, - "velocityY": -2.0355689048343195, - "timestamp": 9.216681995893637 - }, - { - "x": 4.850325220314879, - "y": 4.608787325463071, - "heading": -1.8976395039793544e-17, - "angularVelocity": -3.410277950107739e-17, - "velocityX": 3.1035934225348787, - "velocityY": -2.1612244409379606, - "timestamp": 9.25131125391604 - }, - { - "x": 4.959315849281407, - "y": 4.536170434444089, - "heading": -1.7824338333790707e-17, - "angularVelocity": 3.3268304658955216e-17, - "velocityX": 3.1473567494868275, - "velocityY": -2.09698085278076, - "timestamp": 9.285940511938442 - }, - { - "x": 5.0711194796065495, - "y": 4.467963995966004, - "heading": -1.5727172733760973e-17, - "angularVelocity": 6.056051211588329e-17, - "velocityX": 3.228588676454312, - "velocityY": -1.9696188244622874, - "timestamp": 9.320569769960844 - }, - { - "x": 5.1855579490393096, - "y": 4.4042772106470105, - "heading": -1.2316155333836473e-17, - "angularVelocity": 9.850102470338397e-17, - "velocityX": 3.3046757559381787, - "velocityY": -1.839103375469191, - "timestamp": 9.355199027983247 - }, - { - "x": 5.302448702578612, - "y": 4.345211810292891, - "heading": -8.765491689722714e-18, - "angularVelocity": 1.0253363331714457e-16, - "velocityX": 3.3754911371096727, - "velocityY": -1.7056501850518684, - "timestamp": 9.389828286005649 - }, - { - "x": 5.421605230513571, - "y": 4.290862095789443, - "heading": -5.5119830878393785e-18, - "angularVelocity": 9.395259349127813e-17, - "velocityX": 3.4409206185669707, - "velocityY": -1.5694738382292028, - "timestamp": 9.424457544028051 - }, - { - "x": 5.542837392691082, - "y": 4.241314822773421, - "heading": -2.184892589584252e-18, - "angularVelocity": 9.60774411078281e-17, - "velocityX": 3.500859362885684, - "velocityY": -1.4307922215361746, - "timestamp": 9.459086802050454 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 1.1869648608997422e-18, - "angularVelocity": 9.737019049910581e-17, - "velocityX": 3.5552114934161927, - "velocityY": -1.2898268911821018, - "timestamp": 9.493716060072856 - }, - { - "x": 5.934272395491136, - "y": 4.124337934678792, - "heading": 7.497166766936204e-18, - "angularVelocity": 8.587782714305931e-17, - "velocityX": 3.6516733021145136, - "velocityY": -0.9841085377661548, - "timestamp": 9.567194885934677 - }, - { - "x": 6.2077530174936735, - "y": 4.075010140740357, - "heading": 1.3006105833327096e-17, - "angularVelocity": 7.497315045222091e-17, - "velocityX": 3.7218970063133985, - "velocityY": -0.6713198443208217, - "timestamp": 9.640673711796499 - }, - { - "x": 6.4844286250858945, - "y": 4.049019985828209, - "heading": 1.5986683133968542e-17, - "angularVelocity": 4.0563757867422685e-17, - "velocityX": 3.7653787243758563, - "velocityY": -0.3537094476852878, - "timestamp": 9.71415253765832 - }, - { - "x": 6.743277363747608, - "y": 4.038426394199003, - "heading": 1.2719246482061777e-17, - "angularVelocity": -4.446773085420891e-17, - "velocityX": 3.522766397335807, - "velocityY": -0.1441720319419269, - "timestamp": 9.787631363520141 - }, - { - "x": 6.978672822837657, - "y": 4.030025299692623, - "heading": 8.975092196512461e-18, - "angularVelocity": -5.095555408833389e-17, - "velocityX": 3.2035822065626665, - "velocityY": -0.11433354313769856, - "timestamp": 9.861110189381963 - }, - { - "x": 7.1905526925662695, - "y": 4.022995759457438, - "heading": 5.368720420708233e-18, - "angularVelocity": -4.9080421924352456e-17, - "velocityX": 2.8835500192539434, - "velocityY": -0.09566756344750614, - "timestamp": 9.934589015243784 - }, - { - "x": 7.3789017577018114, - "y": 4.017045849522277, - "heading": 1.5887833889908863e-18, - "angularVelocity": -5.144253446326957e-17, - "velocityX": 2.563310762337649, - "velocityY": -0.08097448299391075, - "timestamp": 10.008067841105605 - }, - { - "x": 7.54371362899692, - "y": 4.012025997027571, - "heading": -2.7250521222406746e-18, - "angularVelocity": -5.870855257463982e-17, - "velocityX": 2.2429845518359373, - "velocityY": -0.06831699385270525, - "timestamp": 10.081546666967427 - }, - { - "x": 7.684984887810538, - "y": 4.007845278835319, - "heading": -2.10421445779977e-18, - "angularVelocity": 8.449205021435769e-18, - "velocityX": 1.9226118158077505, - "velocityY": -0.056896910684361174, - "timestamp": 10.155025492829248 - }, - { - "x": 7.8027134339680275, - "y": 4.0044425825275765, - "heading": -1.435402908569355e-18, - "angularVelocity": 9.102099024937298e-18, - "velocityX": 1.6022104977409413, - "velocityY": -0.0463085285840207, - "timestamp": 10.22850431869107 - }, - { - "x": 7.896897856926731, - "y": 4.001774011196325, - "heading": -8.461914935171916e-19, - "angularVelocity": 8.01879191918765e-18, - "velocityX": 1.2817899830873647, - "velocityY": -0.03631755543113603, - "timestamp": 10.30198314455289 - }, - { - "x": 7.967537148527336, - "y": 3.999806507404438, - "heading": -3.275924545852618e-19, - "angularVelocity": 7.057802473697343e-18, - "velocityX": 0.9613557480279206, - "velocityY": -0.026776472933672434, - "timestamp": 10.375461970414712 - }, - { - "x": 8.014630554462236, - "y": 3.9985142796609896, - "heading": -5.3367647403100726e-20, - "angularVelocity": 3.732024892420789e-18, - "velocityX": 0.6409112473226126, - "velocityY": -0.01758639619362703, - "timestamp": 10.448940796276533 - }, - { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 7.342940745159904e-44, - "angularVelocity": 7.262996758203436e-19, - "velocityX": 0.320458791984776, - "velocityY": -0.008677813220192914, - "timestamp": 10.522419622138354 - }, - { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 1.3038141825006824e-44, - "angularVelocity": -8.034632634038792e-43, - "velocityX": -5.7118786505402475e-43, - "velocityY": 0, - "timestamp": 10.595898448000176 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -5.9009444568140314e-21, + "angularVelocity": -6.3286336172777184e-21, + "velocityX": -2.3802874375881527e-19, + "velocityY": -1.4090020225698018e-19, + "timestamp": 0 + }, + { + "x": 1.3060573928270223, + "y": 5.572382489770416, + "heading": -0.010694903760090106, + "angularVelocity": -0.1421238741144053, + "velocityX": 0.2137251574806739, + "velocityY": -0.24679961691155075, + "timestamp": 0.0752505786007567 + }, + { + "x": 1.3382234711320444, + "y": 5.535239165388228, + "heading": -0.03207617873440696, + "angularVelocity": -0.28413435978686086, + "velocityX": 0.42745290339416875, + "velocityY": -0.4935951998359545, + "timestamp": 0.1505011572015134 + }, + { + "x": 1.386473197712333, + "y": 5.479524643144728, + "heading": -0.06412173018225528, + "angularVelocity": -0.4258512299003913, + "velocityX": 0.6411874496855803, + "velocityY": -0.7403866293046694, + "timestamp": 0.22575173580227012 + }, + { + "x": 1.4508074341962967, + "y": 5.405239335978357, + "heading": -0.1067984078244425, + "angularVelocity": -0.5671275681295302, + "velocityX": 0.8549334460969267, + "velocityY": -0.987172571263834, + "timestamp": 0.3010023144030268 + }, + { + "x": 1.5312274285318639, + "y": 5.312383879458918, + "heading": -0.16006569861045727, + "angularVelocity": -0.707865531089473, + "velocityX": 1.0686960264083267, + "velocityY": -1.2339500671757528, + "timestamp": 0.3762528930037835 + }, + { + "x": 1.6277348772170532, + "y": 5.2009592989550075, + "heading": -0.22388049710097416, + "angularVelocity": -0.8480306686954192, + "velocityX": 1.2824811513696022, + "velocityY": -1.4807139370330336, + "timestamp": 0.4515034716045402 + }, + { + "x": 1.7403321438675061, + "y": 5.0709673174660805, + "heading": -0.29820238787936876, + "angularVelocity": -0.9876587284824841, + "velocityX": 1.4962976862668933, + "velocityY": -1.7274549100613854, + "timestamp": 0.5267540502052969 + }, + { + "x": 1.869023605292211, + "y": 4.922411602704825, + "heading": -0.3829978231491767, + "angularVelocity": -1.1268409738042233, + "velocityX": 1.7101723842877663, + "velocityY": -1.9741471430995352, + "timestamp": 0.6020046288060535 + }, + { + "x": 2.0218400866158173, + "y": 4.778612697599142, + "heading": -0.46243678265580673, + "angularVelocity": -1.0556591189563451, + "velocityX": 2.0307681902941916, + "velocityY": -1.9109342117962438, + "timestamp": 0.6772552074068102 + }, + { + "x": 2.158571908625681, + "y": 4.653385137637619, + "heading": -0.5314200564732017, + "angularVelocity": -0.9167141980846026, + "velocityX": 1.8170202083793483, + "velocityY": -1.6641408250954677, + "timestamp": 0.7525057860075669 + }, + { + "x": 2.279215456782309, + "y": 4.5467256453679665, + "heading": -0.5899528580218641, + "angularVelocity": -0.7778385580157354, + "velocityX": 1.603224193088739, + "velocityY": -1.4173909922412158, + "timestamp": 0.8277563646083236 + }, + { + "x": 2.383769256289076, + "y": 4.45863275029472, + "heading": -0.6380359000069764, + "angularVelocity": -0.6389723890392618, + "velocityX": 1.3894085793213427, + "velocityY": -1.1706607007051708, + "timestamp": 0.9030069432090803 + }, + { + "x": 2.472232464576315, + "y": 4.389105550793566, + "heading": -0.6756681804644412, + "angularVelocity": -0.500092905027661, + "velocityX": 1.1755817687008607, + "velocityY": -0.9239423907952048, + "timestamp": 0.9782575218098369 + }, + { + "x": 2.5446045532635506, + "y": 4.33814345127288, + "heading": -0.7028480646839942, + "angularVelocity": -0.36119169745865176, + "velocityX": 0.9617479364681578, + "velocityY": -0.6772319956645083, + "timestamp": 1.0535081004105937 + }, + { + "x": 2.6008851981181857, + "y": 4.305746065234092, + "heading": -0.7195737326656834, + "angularVelocity": -0.2222663040297333, + "velocityX": 0.7479097955259911, + "velocityY": -0.43052673668740377, + "timestamp": 1.1287586790113504 + }, + { + "x": 2.641074232135465, + "y": 4.291913170423193, + "heading": -0.7258433490393724, + "angularVelocity": -0.0833165204875385, + "velocityX": 0.5340694352744676, + "velocityY": -0.1838244312283474, + "timestamp": 1.204009257612107 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05565716597974986, + "velocityX": 0.3202286486370387, + "velocityY": 0.06287682191127338, + "timestamp": 1.2792598362128638 + }, + { + "x": 2.673109423896828, + "y": 4.320288496439454, + "heading": -0.7067986875650104, + "angularVelocity": 0.19580508899656304, + "velocityX": 0.10461880978884638, + "velocityY": 0.3116212207833284, + "timestamp": 1.3551333878515968 + }, + { + "x": 2.664688214670906, + "y": 4.3628057108082405, + "heading": -0.6813129138970213, + "angularVelocity": 0.3358979923509836, + "velocityX": -0.1109900491548022, + "velocityY": 0.5603693704926264, + "timestamp": 1.4310069394903298 + }, + { + "x": 2.6399081472529042, + "y": 4.424196735750699, + "heading": -0.6452019329979721, + "angularVelocity": 0.47593634565822673, + "velocityX": -0.3265969087089792, + "velocityY": 0.8091228579205605, + "timestamp": 1.5068804911290627 + }, + { + "x": 2.5987694501527234, + "y": 4.5044620989064414, + "heading": -0.5984691831252384, + "angularVelocity": 0.6159293833409661, + "velocityX": -0.542200756545013, + "velocityY": 1.05788329954391, + "timestamp": 1.5827540427677957 + }, + { + "x": 2.5412724247726852, + "y": 4.603602430919982, + "heading": -0.5411158658534186, + "angularVelocity": 0.7559065844828777, + "velocityX": -0.7578006319478658, + "velocityY": 1.3066520529527457, + "timestamp": 1.6586275944065287 + }, + { + "x": 2.467417405939932, + "y": 4.7216183787073325, + "heading": -0.473137956706953, + "angularVelocity": 0.895936827501342, + "velocityX": -0.9733960943919171, + "velocityY": 1.5554293325984934, + "timestamp": 1.7345011460452617 + }, + { + "x": 2.37720453797495, + "y": 4.858510302970022, + "heading": -0.3945211456551798, + "angularVelocity": 1.0361556741946414, + "velocityX": -1.1889896547134935, + "velocityY": 1.8042113662279782, + "timestamp": 1.8103746976839947 + }, + { + "x": 2.2706323270523923, + "y": 5.014276850913776, + "heading": -0.3052336725466659, + "angularVelocity": 1.1767931140702435, + "velocityX": -1.4046029033952228, + "velocityY": 2.052975570268153, + "timestamp": 1.8862482493227277 + }, + { + "x": 2.156211312441972, + "y": 5.146461367106831, + "heading": -0.22910323737059946, + "angularVelocity": 1.0033856796180658, + "velocityX": -1.5080487487284289, + "velocityY": 1.742168559900677, + "timestamp": 1.9621218009614607 + }, + { + "x": 2.058134458607592, + "y": 5.259759379647075, + "heading": -0.16374337318870086, + "angularVelocity": 0.8614314576060107, + "velocityX": -1.2926355985202171, + "velocityY": 1.4932477799347985, + "timestamp": 2.0379953526001935 + }, + { + "x": 1.976403642015832, + "y": 5.354173274627719, + "heading": -0.10920518354656349, + "angularVelocity": 0.7188036998955512, + "velocityX": -1.0771977168121305, + "velocityY": 1.2443584482531382, + "timestamp": 2.1138689042389265 + }, + { + "x": 1.9110189688101253, + "y": 5.429703861532151, + "heading": -0.06553401442898629, + "angularVelocity": 0.5755782901203135, + "velocityX": -0.8617584361561702, + "velocityY": 0.9954797854217321, + "timestamp": 2.1897424558776595 + }, + { + "x": 1.8619803269894504, + "y": 5.486351581272176, + "heading": -0.032764600706343905, + "angularVelocity": 0.4318950808929029, + "velocityX": -0.6463206316489477, + "velocityY": 0.7466069337268786, + "timestamp": 2.2656160075163925 + }, + { + "x": 1.829287687543546, + "y": 5.524116685802078, + "heading": -0.01091803536445235, + "angularVelocity": 0.2879338698406078, + "velocityX": -0.4308832094957455, + "velocityY": 0.4977374027489312, + "timestamp": 2.3414895591551255 + }, + { + "x": 1.812941193580627, + "y": 5.542999267578124, + "heading": 1.0528400336269664e-20, + "angularVelocity": 0.14389777634817838, + "velocityX": -0.2154439011994184, + "velocityY": 0.248869090323821, + "timestamp": 2.4173631107938585 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 4.797022054850397e-21, + "angularVelocity": -9.847000764445998e-21, + "velocityX": -1.972173923637389e-19, + "velocityY": -3.0551788776993856e-19, + "timestamp": 2.4932366624325915 + }, + { + "x": 1.850755756182451, + "y": 5.5430527114919315, + "heading": -8.297424237870081e-19, + "angularVelocity": -8.963996805344217e-18, + "velocityX": 0.4061756697719998, + "velocityY": 0.0005740544380748758, + "timestamp": 2.586335696707713 + }, + { + "x": 1.9263848800936074, + "y": 5.543159599317717, + "heading": -2.6749716281445808e-18, + "angularVelocity": -1.9820068153291573e-17, + "velocityX": 0.8123513256610282, + "velocityY": 0.001148108856528731, + "timestamp": 2.6794347309828344 + }, + { + "x": 2.039828562987612, + "y": 5.543319931052195, + "heading": -5.8149048269782575e-18, + "angularVelocity": -3.372680740742886e-17, + "velocityX": 1.218526956560709, + "velocityY": 0.00172216323966475, + "timestamp": 2.772533765257956 + }, + { + "x": 2.191086799436003, + "y": 5.543533706687691, + "heading": -1.0854467146744384e-17, + "angularVelocity": -5.413119866372016e-17, + "velocityX": 1.6247025291519184, + "velocityY": 0.0022962175403924926, + "timestamp": 2.8656327995330773 + }, + { + "x": 2.3801595622964795, + "y": 5.5438009261858445, + "heading": -4.1769885430235806e-18, + "angularVelocity": 7.172446691559558e-17, + "velocityX": 2.0308778102008924, + "velocityY": 0.0028702714290790255, + "timestamp": 2.9587318338081987 + }, + { + "x": 2.53141779874487, + "y": 5.54401470182134, + "heading": -1.8296127242246427e-18, + "angularVelocity": 2.5213750465779628e-17, + "velocityX": 1.6247025291519184, + "velocityY": 0.0022962175403924926, + "timestamp": 3.05183086808332 + }, + { + "x": 2.644861481638875, + "y": 5.544175033555818, + "heading": -7.228112419808573e-19, + "angularVelocity": 1.1888431398676619e-17, + "velocityX": 1.218526956560709, + "velocityY": 0.0017221632396647501, + "timestamp": 3.1449299023584416 + }, + { + "x": 2.7204906055500317, + "y": 5.544281921381604, + "heading": -1.9384912404248153e-19, + "angularVelocity": 5.681714338671363e-18, + "velocityX": 0.8123513256610282, + "velocityY": 0.0011481088565287313, + "timestamp": 3.238028936633563 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 9.777796865534126e-30, + "angularVelocity": 2.0821818999587538e-18, + "velocityX": 0.4061756697719998, + "velocityY": 0.0005740544380748761, + "timestamp": 3.3311279709086845 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.1644118306851733e-29, + "angularVelocity": -2.560461213964373e-29, + "velocityX": -2.249094171598222e-22, + "velocityY": 3.2281317136840683e-24, + "timestamp": 3.424227005183806 + }, + { + "x": 2.7765323826419794, + "y": 5.537624192846842, + "heading": -2.642342456596521e-18, + "angularVelocity": -3.960137512120418e-17, + "velocityX": 0.2731753246622511, + "velocityY": -0.1005818367636714, + "timestamp": 3.4909505078012537 + }, + { + "x": 2.812967100767047, + "y": 5.524148555527004, + "heading": -5.7083699786826614e-18, + "angularVelocity": -4.595123759665632e-17, + "velocityX": 0.5460552383462592, + "velocityY": -0.20196237894013302, + "timestamp": 3.5576740104187015 + }, + { + "x": 2.867585856008562, + "y": 5.5038456232928015, + "heading": -9.193458656984461e-18, + "angularVelocity": -5.223180051341883e-17, + "velocityX": 0.8185834540891116, + "velocityY": -0.3042845689712529, + "timestamp": 3.624397513036149 + }, + { + "x": 2.9403602449806896, + "y": 5.476640219670028, + "heading": -1.3713804964383516e-17, + "angularVelocity": -6.774743726038035e-17, + "velocityX": 1.0906859819600914, + "velocityY": -0.40773344557094127, + "timestamp": 3.691121015653597 + }, + { + "x": 3.0312551955793734, + "y": 5.442440791714766, + "heading": -1.9689161108713677e-17, + "angularVelocity": -8.955399386867554e-17, + "velocityX": 1.3622628763933429, + "velocityY": -0.51255444653948, + "timestamp": 3.7578445182710447 + }, + { + "x": 3.1402263232984726, + "y": 5.401133415459366, + "heading": -2.6276370451665866e-17, + "angularVelocity": -9.872397407984698e-17, + "velocityX": 1.6331745703440297, + "velocityY": -0.6190828513939344, + "timestamp": 3.8245680208884925 + }, + { + "x": 3.2672156894222453, + "y": 5.352572476953852, + "heading": -3.586672627631812e-17, + "angularVelocity": -1.4373279951454157e-16, + "velocityX": 1.9032179238529063, + "velocityY": -0.7277936049600628, + "timestamp": 3.89129152350594 + }, + { + "x": 3.4121445511915924, + "y": 5.2965653622743645, + "heading": -4.6442314764739977e-17, + "angularVelocity": -1.5849870096036839e-16, + "velocityX": 2.172081142086939, + "velocityY": -0.8393911063182322, + "timestamp": 3.958015026123388 + }, + { + "x": 3.5748999018873118, + "y": 5.232845447526237, + "heading": -5.766195301574919e-17, + "angularVelocity": -1.6815121824921553e-16, + "velocityX": 2.439250703441936, + "velocityY": -0.9549845593907098, + "timestamp": 4.024738528740836 + }, + { + "x": 3.7553064852503644, + "y": 5.161019676773233, + "heading": -6.954653481566044e-17, + "angularVelocity": -1.7811687536878263e-16, + "velocityX": 2.70379366019489, + "velocityY": -1.0764688293540257, + "timestamp": 4.091462031358284 + }, + { + "x": 3.953058182659589, + "y": 5.080452177894761, + "heading": -7.943549000582077e-17, + "angularVelocity": -1.4820797473458394e-16, + "velocityX": 2.9637487489680097, + "velocityY": -1.2074830564635766, + "timestamp": 4.158185533975732 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -8.307291105750155e-17, + "angularVelocity": -5.451483973353195e-17, + "velocityX": 3.2138732879335503, + "velocityY": -1.3564093381868512, + "timestamp": 4.224909036593179 + }, + { + "x": 4.2829175575872735, + "y": 4.93941307796568, + "heading": -8.597616346613658e-17, + "angularVelocity": -8.369192817411254e-17, + "velocityX": 3.3271637455782836, + "velocityY": -1.45676206741062, + "timestamp": 4.259598790694249 + }, + { + "x": 4.40183327548322, + "y": 4.884962607883731, + "heading": -8.781389044285282e-17, + "angularVelocity": -5.2976073895804537e-17, + "velocityX": 3.4279781156557663, + "velocityY": -1.5696412814950174, + "timestamp": 4.294288544795319 + }, + { + "x": 4.518840747701827, + "y": 4.825621207784163, + "heading": -9.315781416713264e-17, + "angularVelocity": -1.5404905173670211e-16, + "velocityX": 3.3729692023097777, + "velocityY": -1.7106319037798774, + "timestamp": 4.328978298896389 + }, + { + "x": 4.633380419289881, + "y": 4.7616461011987, + "heading": -9.908185769391999e-17, + "angularVelocity": -1.7077213950707558e-16, + "velocityX": 3.301830023191876, + "velocityY": -1.8442075547457024, + "timestamp": 4.3636680529974585 + }, + { + "x": 4.745269100236443, + "y": 4.693140088610349, + "heading": -1.025376712876981e-16, + "angularVelocity": -9.9620584908128e-17, + "velocityX": 3.2254100337687888, + "velocityY": -1.9748197807559171, + "timestamp": 4.398357807098528 + }, + { + "x": 4.854330012145195, + "y": 4.620216373671222, + "heading": -1.025310951791751e-16, + "angularVelocity": 1.8956918365515503e-19, + "velocityX": 3.1438940613704167, + "velocityY": -2.1021686901170322, + "timestamp": 4.433047561199598 + }, + { + "x": 4.962829609437795, + "y": 4.5464600987106305, + "heading": -9.93623379907482e-17, + "angularVelocity": 9.134562265751682e-17, + "velocityX": 3.127713069873129, + "velocityY": -2.1261688608602203, + "timestamp": 4.467737315300668 + }, + { + "x": 5.073866943405842, + "y": 4.47658262622743, + "heading": -9.505129174841953e-17, + "angularVelocity": 1.2427433845795495e-16, + "velocityX": 3.2008682922495537, + "velocityY": -2.014354794202651, + "timestamp": 4.502427069401738 + }, + { + "x": 5.187610621724133, + "y": 4.411202764699172, + "heading": -8.900790995033302e-17, + "angularVelocity": 1.7421229853948998e-16, + "velocityX": 3.278883960575102, + "velocityY": -1.8847023630600785, + "timestamp": 4.537116823502807 + }, + { + "x": 5.303878904131373, + "y": 4.350425819404775, + "heading": -8.318308330059428e-17, + "angularVelocity": 1.6791201899662455e-16, + "velocityX": 3.3516606104641187, + "velocityY": -1.7520143013214118, + "timestamp": 4.571806577603877 + }, + { + "x": 5.422485694048398, + "y": 4.294349265955072, + "heading": -7.684855050558625e-17, + "angularVelocity": 1.8260529539962743e-16, + "velocityX": 3.4190726625349193, + "velocityY": -1.616516314480737, + "timestamp": 4.606496331704947 + }, + { + "x": 5.543241089708319, + "y": 4.243062964356387, + "heading": -6.896458750545335e-17, + "angularVelocity": 2.2727065107078374e-16, + "velocityX": 3.4810104248100697, + "velocityY": -1.4784279372307074, + "timestamp": 4.641186085806017 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -6.104113455464306e-17, + "angularVelocity": 2.2840902612270297e-16, + "velocityX": 3.5373741409339847, + "velocityY": -1.33797113887621, + "timestamp": 4.6758758399070866 + }, + { + "x": 5.795129656406628, + "y": 4.153827521880981, + "heading": -5.26502063293767e-17, + "angularVelocity": 2.331839018689876e-16, + "velocityX": 3.589854707589498, + "velocityY": -1.1900109819379716, + "timestamp": 4.711860005871443 + }, + { + "x": 5.925973456913311, + "y": 4.116403968556151, + "heading": -4.449297326372924e-17, + "angularVelocity": 2.2668951320100914e-16, + "velocityX": 3.636149317349419, + "velocityY": -1.0400005758616107, + "timestamp": 4.747844171835799 + }, + { + "x": 6.058257665524998, + "y": 4.08444288734655, + "heading": -3.6710199664402193e-17, + "angularVelocity": 2.162832843430151e-16, + "velocityX": 3.6761782597023362, + "velocityY": -0.8881984715516176, + "timestamp": 4.783828337800155 + }, + { + "x": 6.191754337287333, + "y": 4.057999339254895, + "heading": -3.0635067113443973e-17, + "angularVelocity": 1.6882793828813163e-16, + "velocityX": 3.7098726115972305, + "velocityY": -0.7348662219335074, + "timestamp": 4.819812503764511 + }, + { + "x": 6.326233440381154, + "y": 4.037118883128164, + "heading": -2.7100684489157953e-17, + "angularVelocity": 9.822049586045954e-17, + "velocityX": 3.7371743790595944, + "velocityY": -0.5802678919226052, + "timestamp": 4.855796669728867 + }, + { + "x": 6.461463250762805, + "y": 4.021837500768739, + "heading": -2.6741166877102065e-17, + "angularVelocity": 9.990994712261855e-18, + "velocityX": 3.758036535169413, + "velocityY": -0.42466962759569293, + "timestamp": 4.891780835693223 + }, + { + "x": 6.596863600046523, + "y": 4.012202729774147, + "heading": -2.0941092472280878e-17, + "angularVelocity": 1.6118407219507027e-16, + "velocityX": 3.7627758113898246, + "velocityY": -0.26775029339668854, + "timestamp": 4.927765001657579 + }, + { + "x": 6.727539041313674, + "y": 4.005664584143169, + "heading": -1.5390888357792948e-17, + "angularVelocity": 1.5424017663331785e-16, + "velocityX": 3.631470613952549, + "velocityY": -0.1816950721451956, + "timestamp": 4.963749167621935 + }, + { + "x": 6.852985008513406, + "y": 4.001263402089072, + "heading": -1.0075825367590821e-17, + "angularVelocity": 1.4770560460742425e-16, + "velocityX": 3.4861435255715034, + "velocityY": -0.12230885268972179, + "timestamp": 4.999733333586291 + }, + { + "x": 6.973061816658764, + "y": 3.998618980784619, + "heading": -5.1577923312514375e-18, + "angularVelocity": 1.3667214188229055e-16, + "velocityX": 3.336934591294929, + "velocityY": -0.07348847009744364, + "timestamp": 5.035717499550647 + }, + { + "x": 7.087708032779949, + "y": 3.9975310881250836, + "heading": -2.6946740234793898e-18, + "angularVelocity": 6.845005967538265e-17, + "velocityX": 3.186018434740021, + "velocityY": -0.030232537850478983, + "timestamp": 5.071701665515003 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -6.591539659972594e-30, + "angularVelocity": 7.488499319392298e-17, + "velocityX": 3.034163544252643, + "velocityY": 0.009603001769728973, + "timestamp": 5.107685831479359 + }, + { + "x": 7.373388400468218, + "y": 4.002743214942896, + "heading": -4.953200001247003e-18, + "angularVelocity": -7.75569723773575e-17, + "velocityX": 2.7636055642205424, + "velocityY": 0.07620053655266033, + "timestamp": 5.17155114041995 + }, + { + "x": 7.532268457334727, + "y": 4.010110424631325, + "heading": -1.3097129093463054e-17, + "angularVelocity": -1.2751725823370097e-16, + "velocityX": 2.48773644881765, + "velocityY": 0.1153554223824672, + "timestamp": 5.2354164493605415 + }, + { + "x": 7.673406960142625, + "y": 4.018856353361859, + "heading": -1.8281069995800668e-17, + "angularVelocity": -8.116990254124082e-17, + "velocityX": 2.209940030810585, + "velocityY": 0.13694334022043622, + "timestamp": 5.299281758301133 + }, + { + "x": 7.796760795699071, + "y": 4.0282099476371425, + "heading": -2.4884139524870184e-17, + "angularVelocity": -1.03390551752411e-16, + "velocityX": 1.9314685484602037, + "velocityY": 0.1464581387053742, + "timestamp": 5.363147067241724 + }, + { + "x": 7.902319649761861, + "y": 4.0376115999719895, + "heading": -2.509945215126813e-17, + "angularVelocity": -3.3713549672899342e-18, + "velocityX": 1.6528355661910739, + "velocityY": 0.14721062953900424, + "timestamp": 5.427012376182315 + }, + { + "x": 7.9900874176780725, + "y": 4.046637816644616, + "heading": -1.900477135412208e-17, + "angularVelocity": 9.543022492350273e-17, + "velocityX": 1.3742635770830707, + "velocityY": 0.14133207561907785, + "timestamp": 5.490877685122906 + }, + { + "x": 8.060074171241288, + "y": 4.054957461114281, + "heading": -1.4433757000062975e-17, + "angularVelocity": 7.157272750648916e-17, + "velocityX": 1.0958492916446645, + "velocityY": 0.13026860133729024, + "timestamp": 5.554742994063497 + }, + { + "x": 8.112292461453118, + "y": 4.062304768239738, + "heading": -9.3901869246783e-18, + "angularVelocity": 7.897198273854564e-17, + "velocityX": 0.8176315291985099, + "velocityY": 0.11504378898864956, + "timestamp": 5.6186083030040885 + }, + { + "x": 8.146755551516593, + "y": 4.0684618614995625, + "heading": -4.677400275001561e-18, + "angularVelocity": 7.379259143526985e-17, + "velocityX": 0.5396214413607953, + "velocityY": 0.09640747632727756, + "timestamp": 5.68247361194468 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -3.026226030610745e-28, + "angularVelocity": 7.323851324219789e-17, + "velocityX": 0.2618168025924992, + "velocityY": 0.07492478234891005, + "timestamp": 5.746338920885271 + }, + { + "x": 8.160404735497007, + "y": 4.076664969326324, + "heading": 5.687417788003061e-18, + "angularVelocity": 8.091717809987449e-17, + "velocityX": -0.04370411704181345, + "velocityY": 0.04862945078972498, + "timestamp": 5.816625823147878 + }, + { + "x": 8.13585178429954, + "y": 4.078318050056324, + "heading": 9.715319347453537e-18, + "angularVelocity": 5.730657391283969e-17, + "velocityX": -0.34932470214341615, + "velocityY": 0.02351904375900896, + "timestamp": 5.886912725410485 + }, + { + "x": 8.089809855979007, + "y": 4.078304537416186, + "heading": 1.421739507206778e-17, + "angularVelocity": 6.405284028537325e-17, + "velocityX": -0.6550570140153344, + "velocityY": -0.00019224976064422702, + "timestamp": 5.957199627673092 + }, + { + "x": 8.022270134239134, + "y": 4.076742304699779, + "heading": 2.3560062445993e-17, + "angularVelocity": 1.3292188264586088e-16, + "velocityX": -0.9609147588768534, + "velocityY": -0.022226512566613944, + "timestamp": 6.0274865299356994 + }, + { + "x": 7.933222739684713, + "y": 4.073775206509657, + "heading": 3.0510865621673614e-17, + "angularVelocity": 9.889186963848529e-17, + "velocityX": -1.2669130618635012, + "velocityY": -0.04221409814073001, + "timestamp": 6.097773432198307 + }, + { + "x": 7.822656702045201, + "y": 4.069582705061331, + "heading": 3.6286017590367637e-17, + "angularVelocity": 8.21654075361425e-17, + "velocityX": -1.573067443297094, + "velocityY": -0.05964840266629161, + "timestamp": 6.168060334460914 + }, + { + "x": 7.690560152164254, + "y": 4.0643949017183925, + "heading": 4.582493898384423e-17, + "angularVelocity": 1.3571406743753222e-16, + "velocityX": -1.8793906919870382, + "velocityY": -0.07380896263653505, + "timestamp": 6.238347236723521 + }, + { + "x": 7.53692113699896, + "y": 4.058517364536531, + "heading": 5.495539333254171e-17, + "angularVelocity": 1.2990264266804977e-16, + "velocityX": -2.1858840014212815, + "velocityY": -0.0836220831002289, + "timestamp": 6.308634138986128 + }, + { + "x": 7.361730243304987, + "y": 4.052375214932341, + "heading": 6.396812526564607e-17, + "angularVelocity": 1.2822775856015118e-16, + "velocityX": -2.4925112368648836, + "velocityY": -0.08738683035483485, + "timestamp": 6.378921041248735 + }, + { + "x": 7.164988961620082, + "y": 4.046599375673031, + "heading": 7.356788600543696e-17, + "angularVelocity": 1.3657965326299643e-16, + "velocityX": -2.799117265829076, + "velocityY": -0.08217518589352116, + "timestamp": 6.449207943511342 + }, + { + "x": 6.946739460744683, + "y": 4.042219881595439, + "heading": 8.53416334460989e-17, + "angularVelocity": 1.6750983557133948e-16, + "velocityX": -3.105123342325879, + "velocityY": -0.06230882193712492, + "timestamp": 6.519494845773949 + }, + { + "x": 6.707199372096715, + "y": 4.041196560014842, + "heading": 9.976814238544392e-17, + "angularVelocity": 2.052517393014489e-16, + "velocityX": -3.408033089194812, + "velocityY": -0.014559207301144003, + "timestamp": 6.589781748036557 + }, + { + "x": 6.447738133610024, + "y": 4.048401251612496, + "heading": 1.0469202046971274e-16, + "angularVelocity": 7.00539919377632e-17, + "velocityX": -3.691459292334275, + "velocityY": 0.10250404222876178, + "timestamp": 6.660068650299164 + }, + { + "x": 6.18341657905221, + "y": 4.076604236955095, + "heading": 9.517171884835342e-17, + "angularVelocity": -1.354491564534057e-16, + "velocityX": -3.7606089619691896, + "velocityY": 0.4012552045219841, + "timestamp": 6.730355552561771 + }, + { + "x": 5.922248770654031, + "y": 4.126128704367341, + "heading": 8.827514914451601e-17, + "angularVelocity": -9.812026823816515e-17, + "velocityX": -3.7157393481704415, + "velocityY": 0.7046044969688833, + "timestamp": 6.800642454824378 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 8.108309692704171e-17, + "angularVelocity": -1.0232421668622016e-16, + "velocityX": -3.6464409951607806, + "velocityY": 1.003321641969972, + "timestamp": 6.870929357086985 + }, + { + "x": 5.541094252914905, + "y": 4.236435293416023, + "heading": 7.779103781629071e-17, + "angularVelocity": -9.500999479304098e-17, + "velocityX": -3.603431085871871, + "velocityY": 1.1482444026273093, + "timestamp": 6.905578965454807 + }, + { + "x": 5.417926513725, + "y": 4.281179467366055, + "heading": 7.404581584331534e-17, + "angularVelocity": -1.0808843589526725e-16, + "velocityX": -3.5546646842995244, + "velocityY": 1.291332746825043, + "timestamp": 6.940228573822629 + }, + { + "x": 5.2966452974499845, + "y": 4.330810104553132, + "heading": 7.048745543918544e-17, + "angularVelocity": -1.0269554467404476e-16, + "velocityX": -3.5002189631570437, + "velocityY": 1.4323578108076789, + "timestamp": 6.974878182190451 + }, + { + "x": 5.177444375077077, + "y": 4.385247904729644, + "heading": 6.777735008198806e-17, + "angularVelocity": -7.821460284920669e-17, + "velocityX": -3.440180942524166, + "velocityY": 1.5710942414883695, + "timestamp": 7.0095277905582725 + }, + { + "x": 5.060514192105533, + "y": 4.444405885109906, + "heading": 6.539767218111103e-17, + "angularVelocity": -6.867834914008308e-17, + "velocityX": -3.3746465971642725, + "velocityY": 1.7073203180905416, + "timestamp": 7.044177398926094 + }, + { + "x": 4.946041562758086, + "y": 4.5081895174709326, + "heading": 6.339841419086817e-17, + "angularVelocity": -5.769929544659045e-17, + "velocityX": -3.3037207270069695, + "velocityY": 1.8408182766146604, + "timestamp": 7.078827007293916 + }, + { + "x": 4.834209367445105, + "y": 4.5764968740602265, + "heading": 6.206624882969642e-17, + "angularVelocity": -3.844676532404254e-17, + "velocityX": -3.2275168632738827, + "velocityY": 1.9713745640117823, + "timestamp": 7.113476615661738 + }, + { + "x": 4.725196244706173, + "y": 4.649218769978707, + "heading": 6.152068396615488e-17, + "angularVelocity": -1.574519567617046e-17, + "velocityX": -3.1461574278620072, + "velocityY": 2.098779736454826, + "timestamp": 7.14812622402956 + }, + { + "x": 4.619176114911752, + "y": 4.726238684753922, + "heading": 5.2556937935057745e-17, + "angularVelocity": -2.58696893087623e-16, + "velocityX": -3.0597785888073585, + "velocityY": 2.2228220866918083, + "timestamp": 7.182775832397382 + }, + { + "x": 4.51016287630692, + "y": 4.798960406862416, + "heading": 6.64273126982982e-17, + "angularVelocity": 4.003039403991449e-16, + "velocityX": -3.1461607717930216, + "velocityY": 2.0987747202369076, + "timestamp": 7.217425440765203 + }, + { + "x": 4.398330563422939, + "y": 4.8672675708300295, + "heading": 7.26949465762174e-17, + "angularVelocity": 1.8088613906315835e-16, + "velocityX": -3.2275202564146483, + "velocityY": 1.9713690048817056, + "timestamp": 7.252075049133025 + }, + { + "x": 4.283857826423259, + "y": 4.9310509990316564, + "heading": 7.854869048227426e-17, + "angularVelocity": 1.6894112751752813e-16, + "velocityX": -3.303723833888627, + "velocityY": 1.840812384501881, + "timestamp": 7.286724657500847 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 7.967166003614947e-17, + "angularVelocity": 3.240930002777194e-17, + "velocityX": -3.358155156874672, + "velocityY": 1.6997824682762939, + "timestamp": 7.321374265868669 + }, + { + "x": 3.941212613699701, + "y": 5.086809869285441, + "heading": 7.443579239948039e-17, + "angularVelocity": -7.609481799868211e-17, + "velocityX": -3.288705435486243, + "velocityY": 1.4077326545547513, + "timestamp": 7.3901814169311395 + }, + { + "x": 3.7303194477377524, + "y": 5.1698988992560615, + "heading": 6.400702034661796e-17, + "angularVelocity": -1.515652354772208e-16, + "velocityX": -3.0649890702563414, + "velocityY": 1.2075638750859479, + "timestamp": 7.45898856799361 + }, + { + "x": 3.5371029822412163, + "y": 5.24230220948227, + "heading": 4.92329505445724e-17, + "angularVelocity": -2.1471706900158835e-16, + "velocityX": -2.8080869867888283, + "velocityY": 1.0522643229404085, + "timestamp": 7.527795719056081 + }, + { + "x": 3.362390045884083, + "y": 5.305525654232102, + "heading": 3.1750019865336384e-17, + "angularVelocity": -2.5408595487118374e-16, + "velocityX": -2.5391682936924758, + "velocityY": 0.9188499127428014, + "timestamp": 7.596602870118551 + }, + { + "x": 3.2065873745181794, + "y": 5.36043931571324, + "heading": 2.169929247164939e-17, + "angularVelocity": -1.4607097138783564e-16, + "velocityX": -2.2643383566985644, + "velocityY": 0.7980807319181392, + "timestamp": 7.665410021181022 + }, + { + "x": 3.069932459879016, + "y": 5.407606100670878, + "heading": 1.5380181020622108e-17, + "angularVelocity": -9.183800453716929e-17, + "velocityX": -1.9860568636985672, + "velocityY": 0.6854924848554687, + "timestamp": 7.7342171722434925 + }, + { + "x": 2.952579630826552, + "y": 5.447418960174395, + "heading": 8.435159362125091e-18, + "angularVelocity": -1.0093459112453562e-16, + "velocityX": -1.7055324517929655, + "velocityY": 0.5786151423035916, + "timestamp": 7.803024323305963 + }, + { + "x": 2.8546367166241713, + "y": 5.480167404246393, + "heading": 5.0453569918778545e-18, + "angularVelocity": -4.926526267025923e-17, + "velocityX": -1.423440916968907, + "velocityY": 0.4759453569334053, + "timestamp": 7.871831474368434 + }, + { + "x": 2.7761830906702163, + "y": 5.506073449022424, + "heading": 3.1520224912430142e-18, + "angularVelocity": -2.7516536752894226e-17, + "velocityX": -1.1401958189305985, + "velocityY": 0.3765022149007732, + "timestamp": 7.940638625430904 + }, + { + "x": 2.7172795158320118, + "y": 5.5253126875621055, + "heading": 4.0055040762053705e-18, + "angularVelocity": 1.2403966322481763e-17, + "velocityX": -0.8560676314693773, + "velocityY": 0.2796110323215353, + "timestamp": 8.009445776493374 + }, + { + "x": 2.677973947928258, + "y": 5.538027438078825, + "heading": 2.2657574428082367e-18, + "angularVelocity": -2.5284386970987473e-17, + "velocityX": -0.571242484201505, + "velocityY": 0.18478821343985707, + "timestamp": 8.078252927555845 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -8.520572182632915e-28, + "angularVelocity": -3.292909834387365e-17, + "velocityX": -0.2858537153870097, + "velocityY": 0.09167545987855215, + "timestamp": 8.147060078618315 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -3.9086161010585418e-28, + "angularVelocity": 3.4350134480026537e-28, + "velocityX": 1.6920853023952865e-26, + "velocityY": 2.9127538584633355e-26, + "timestamp": 8.215867229680786 + }, + { + "x": 2.675073555498551, + "y": 5.538716972612474, + "heading": 1.6968135653440987e-18, + "angularVelocity": 2.665146289407491e-17, + "velocityX": 0.2633772278822265, + "velocityY": -0.08824680986851689, + "timestamp": 8.279534037481625 + }, + { + "x": 2.7085920648883794, + "y": 5.5274259655637525, + "heading": 3.725095749658628e-18, + "angularVelocity": 3.185776473374715e-17, + "velocityX": 0.5264675668156642, + "velocityY": -0.17734526731797434, + "timestamp": 8.343200845282464 + }, + { + "x": 2.7588390752001515, + "y": 5.510398898078488, + "heading": 7.631145981641487e-18, + "angularVelocity": 6.13514383217542e-17, + "velocityX": 0.7892183077397765, + "velocityY": -0.267440257701116, + "timestamp": 8.406867653083303 + }, + { + "x": 2.8257885977384563, + "y": 5.48756053072635, + "heading": 1.178722114456003e-17, + "angularVelocity": 6.527852277238055e-17, + "velocityX": 1.0515608501644154, + "velocityY": -0.3587170166216, + "timestamp": 8.470534460884142 + }, + { + "x": 2.909408815467416, + "y": 5.4588202121142055, + "heading": 1.707663826690775e-17, + "angularVelocity": 8.307966592022717e-17, + "velocityX": 1.3134036496778398, + "velocityY": -0.45141761625695104, + "timestamp": 8.534201268684981 + }, + { + "x": 3.0096598952002043, + "y": 5.424066614979104, + "heading": 2.1547525099461292e-17, + "angularVelocity": 7.022319772168498e-17, + "velocityX": 1.5746207984290834, + "velocityY": -0.5458668077692407, + "timestamp": 8.59786807648582 + }, + { + "x": 3.1264905609256615, + "y": 5.383159775106569, + "heading": 2.7044139747893618e-17, + "angularVelocity": 8.633407011044114e-17, + "velocityX": 1.835032566591432, + "velocityY": -0.6425143852114948, + "timestamp": 8.66153488428666 + }, + { + "x": 3.259832418594633, + "y": 5.335918459700519, + "heading": 3.2480422722951275e-17, + "angularVelocity": 8.538645430467616e-17, + "velocityX": 2.0943700850541704, + "velocityY": -0.7420085447636868, + "timestamp": 8.725201692087499 + }, + { + "x": 3.409589859132952, + "y": 5.282098873954419, + "heading": 3.770995869084488e-17, + "angularVelocity": 8.213912631218793e-17, + "velocityX": 2.3522058936390553, + "velocityY": -0.84533193362635, + "timestamp": 8.788868499888338 + }, + { + "x": 3.575620300185956, + "y": 5.221355820749434, + "heading": 4.453751126086424e-17, + "angularVelocity": 1.0723880787820308e-16, + "velocityX": 2.6078021937643934, + "velocityY": -0.9540772547447327, + "timestamp": 8.852535307689177 + }, + { + "x": 3.7576900290728465, + "y": 5.153163867625262, + "heading": 5.330339940674851e-17, + "angularVelocity": 1.3768380178976122e-16, + "velocityX": 2.8597276222240717, + "velocityY": -1.0710754234370974, + "timestamp": 8.916202115490016 + }, + { + "x": 3.955353840328095, + "y": 5.076631092230983, + "heading": 6.003878998986586e-17, + "angularVelocity": 1.0579124061222185e-16, + "velocityX": 3.10466031018196, + "velocityY": -1.2020828126594152, + "timestamp": 8.979868923290855 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 6.251629909739218e-17, + "angularVelocity": 3.89136693523207e-17, + "velocityX": 3.3321165674695394, + "velocityY": -1.361514725761388, + "timestamp": 9.043535731091694 + }, + { + "x": 4.285701078795114, + "y": 4.9383884935900895, + "heading": 6.191867349467079e-17, + "angularVelocity": -1.7257822842751934e-17, + "velocityX": 3.4133567862477086, + "velocityY": -1.4888942181949925, + "timestamp": 9.078164989267158 + }, + { + "x": 4.403715151512854, + "y": 4.881600775413148, + "heading": 5.87936361182216e-17, + "angularVelocity": -9.024268901925555e-17, + "velocityX": 3.4079295640631644, + "velocityY": -1.6398768315856658, + "timestamp": 9.112794247442622 + }, + { + "x": 4.5193670084290005, + "y": 4.8201449553533005, + "heading": 6.393419644700586e-17, + "angularVelocity": 1.484455804018553e-16, + "velocityX": 3.3397151140271815, + "velocityY": -1.7746790805755832, + "timestamp": 9.147423505618086 + }, + { + "x": 4.63247211701513, + "y": 4.7541191698650405, + "heading": 6.969863736825022e-17, + "angularVelocity": 1.664615768552329e-16, + "velocityX": 3.2661718600217573, + "velocityY": -1.9066474122463712, + "timestamp": 9.18205276379355 + }, + { + "x": 4.742850086360494, + "y": 4.6836289301368605, + "heading": 7.573971746769133e-17, + "angularVelocity": 1.7445017357345818e-16, + "velocityX": 3.187419400845542, + "velocityY": -2.035568864080495, + "timestamp": 9.216682021969014 + }, + { + "x": 4.850325224446504, + "y": 4.608787332408603, + "heading": 7.95254691851129e-17, + "angularVelocity": 1.0932234523778094e-16, + "velocityX": 3.1035934278880775, + "velocityY": -2.161224400160131, + "timestamp": 9.251311280144478 + }, + { + "x": 4.959315852672914, + "y": 4.536170440415771, + "heading": 8.132510178394684e-17, + "angularVelocity": 5.1968557625797656e-17, + "velocityX": 3.1473567142028136, + "velocityY": -2.0969808716342646, + "timestamp": 9.285940538319942 + }, + { + "x": 5.071119482307337, + "y": 4.467964000951313, + "heading": 8.196310550599571e-17, + "angularVelocity": 1.842383452283251e-17, + "velocityX": 3.2285886422377668, + "velocityY": -1.969618844240326, + "timestamp": 9.320569796495406 + }, + { + "x": 5.185557951100297, + "y": 4.404277214638869, + "heading": 8.211438156325687e-17, + "angularVelocity": 4.3684463753045605e-18, + "velocityX": 3.3046757228557744, + "velocityY": -1.8391033960284853, + "timestamp": 9.35519905467087 + }, + { + "x": 5.302448704050349, + "y": 4.345211813287043, + "heading": 8.111338797415481e-17, + "angularVelocity": -2.8906007282049283e-17, + "velocityX": 3.37549110517396, + "velocityY": -1.7056502063239702, + "timestamp": 9.389828312846333 + }, + { + "x": 5.421605231445948, + "y": 4.290862097784136, + "heading": 7.868835126918448e-17, + "angularVelocity": -7.002854905419881e-17, + "velocityX": 3.4409205877827787, + "velocityY": -1.5694738601537552, + "timestamp": 9.424457571021797 + }, + { + "x": 5.542837393133187, + "y": 4.2413148237692955, + "heading": 7.613874923845377e-17, + "angularVelocity": -7.362566122075624e-17, + "velocityX": 3.5008593332540854, + "velocityY": -1.4307922440552452, + "timestamp": 9.459086829197261 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 7.348031788343025e-17, + "angularVelocity": -7.676835991047679e-17, + "velocityX": 3.5552114649352746, + "velocityY": -1.289826914239228, + "timestamp": 9.493716087372725 + }, + { + "x": 5.934272394454851, + "y": 4.1243379326499765, + "heading": 6.301578755269389e-17, + "angularVelocity": -1.4241558943688294e-16, + "velocityX": 3.6516732759640362, + "velocityY": -0.9841085621303537, + "timestamp": 9.567194913476962 + }, + { + "x": 6.207753015610448, + "y": 4.075010136678792, + "heading": 7.880077877117389e-17, + "angularVelocity": 2.148236717340586e-16, + "velocityX": 3.721896982508106, + "velocityY": -0.6713198697704936, + "timestamp": 9.6406737395812 + }, + { + "x": 6.484428622538185, + "y": 4.049019979746833, + "heading": 4.7380289374867153e-17, + "angularVelocity": -4.2761283844936273e-16, + "velocityX": 3.7653787029102097, + "velocityY": -0.3537094740066765, + "timestamp": 9.714152565685437 + }, + { + "x": 6.743277361498475, + "y": 4.038426387270388, + "heading": 3.80588957590445e-17, + "angularVelocity": -1.2685822719245393e-16, + "velocityX": 3.5227663897772135, + "velocityY": -0.1441720429966888, + "timestamp": 9.787631391789674 + }, + { + "x": 6.978672820957151, + "y": 4.030025293070568, + "heading": 3.301536102704029e-17, + "angularVelocity": -6.863929378502764e-17, + "velocityX": 3.20358220101046, + "velocityY": -0.11433353858840364, + "timestamp": 9.861110217893911 + }, + { + "x": 7.190552691043286, + "y": 4.022995753598098, + "heading": 2.761165208046759e-17, + "angularVelocity": -7.354103532969467e-17, + "velocityX": 2.8835500146064224, + "velocityY": -0.09566755275184706, + "timestamp": 9.934589043998148 + }, + { + "x": 7.378901756507518, + "y": 4.0170458446186466, + "heading": 2.2049007606542194e-17, + "angularVelocity": -7.570404657768667e-17, + "velocityX": 2.563310758354254, + "velocityY": -0.08097446972017466, + "timestamp": 10.008067870102385 + }, + { + "x": 7.543713628095823, + "y": 4.012025993134882, + "heading": 1.7040082343642412e-17, + "angularVelocity": -6.816828096488101e-17, + "velocityX": 2.2429845484262843, + "velocityY": -0.06831697986905093, + "timestamp": 10.081546696206622 + }, + { + "x": 7.684984887163916, + "y": 4.007845275924109, + "heading": 1.249754397163312e-17, + "angularVelocity": -6.182105257790516e-17, + "velocityX": 1.922611812928072, + "velocityY": -0.05689689713935573, + "timestamp": 10.15502552231086 + }, + { + "x": 7.802713433535344, + "y": 4.004442580511056, + "heading": 8.52630628004598e-18, + "angularVelocity": -5.4046014370140334e-17, + "velocityX": 1.6022104953666365, + "velocityY": -0.046308516255087515, + "timestamp": 10.228504348415097 + }, + { + "x": 7.8968978566663255, + "y": 4.001774009946332, + "heading": 5.220002438781615e-18, + "angularVelocity": -4.499668838635962e-17, + "velocityX": 1.2817899812031746, + "velocityY": -0.03631754487937096, + "timestamp": 10.301983174519334 + }, + { + "x": 7.967537148396796, + "y": 3.9998065067615447, + "heading": 2.6237181268422074e-18, + "angularVelocity": -3.5333775041531244e-17, + "velocityX": 0.9613557466236943, + "velocityY": -0.026776464583087325, + "timestamp": 10.375462000623571 + }, + { + "x": 8.014630554418627, + "y": 3.998514279441312, + "heading": 9.593365131903801e-19, + "angularVelocity": -2.265117315883944e-17, + "velocityX": 0.6409112463912332, + "velocityY": -0.01758639037590862, + "timestamp": 10.448940826727808 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 1.1585643597990433e-28, + "angularVelocity": -1.3055958619386531e-17, + "velocityX": 0.3204587915210466, + "velocityY": -0.008677810201891931, + "timestamp": 10.522419652832046 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 5.650895976930055e-29, + "angularVelocity": -3.8630399964154185e-29, + "velocityX": 1.5845610208926219e-27, + "velocityY": -6.207718033855077e-27, + "timestamp": 10.595898478936283 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.1.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.1.traj deleted file mode 100644 index 2132ff88..00000000 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.1.traj +++ /dev/null @@ -1,337 +0,0 @@ -{ - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -1.748186257820146e-28, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.3063204215499635, - "y": 5.5750430661668995, - "heading": -0.008999106947679492, - "angularVelocity": -0.11745777195159221, - "velocityX": 0.21335020071190533, - "velocityY": -0.20767599777855084, - "timestamp": 0.07661567898070912 - }, - { - "x": 1.3390123980907098, - "y": 5.543220800833044, - "heading": -0.026994200234242757, - "angularVelocity": -0.23487481322228163, - "velocityX": 0.42670086570142546, - "velocityY": -0.41534925701509284, - "timestamp": 0.15323135796141824 - }, - { - "x": 1.3880505863886679, - "y": 5.4954877356301095, - "heading": -0.053973713874731634, - "angularVelocity": -0.3521408934222409, - "velocityX": 0.6400542155377871, - "velocityY": -0.6230195417923181, - "timestamp": 0.22984703694212735 - }, - { - "x": 1.4534353620178047, - "y": 5.431844149183211, - "heading": -0.08991902767117105, - "angularVelocity": -0.4691639398438398, - "velocityX": 0.853412467425134, - "velocityY": -0.8306861899103606, - "timestamp": 0.30646271592283647 - }, - { - "x": 1.5351672628403068, - "y": 5.352290408430599, - "heading": -0.13480641974722582, - "angularVelocity": -0.5858773645717358, - "velocityX": 1.066777739575042, - "velocityY": -1.0383480486921168, - "timestamp": 0.38307839490354556 - }, - { - "x": 1.6332469708393256, - "y": 5.256827004871127, - "heading": -0.1886095129083549, - "angularVelocity": -0.7022465099717149, - "velocityX": 1.2801519130772017, - "velocityY": -1.2460034923528938, - "timestamp": 0.45969407388425465 - }, - { - "x": 1.7476752821470238, - "y": 5.145454581666917, - "heading": -0.25130200812811576, - "angularVelocity": -0.818272396306973, - "velocityX": 1.493536477822563, - "velocityY": -1.4536505413596468, - "timestamp": 0.5363097528649637 - }, - { - "x": 1.8784530688655205, - "y": 5.018173944363308, - "heading": -0.3228603099280309, - "angularVelocity": -0.9339903107164581, - "velocityX": 1.7069324253829399, - "velocityY": -1.66128707626095, - "timestamp": 0.6129254318456728 - }, - { - "x": 2.0255812507970647, - "y": 4.874986059010151, - "heading": -0.4032653297049151, - "angularVelocity": -1.049459077659022, - "velocityX": 1.9203403783475084, - "velocityY": -1.8689110021266238, - "timestamp": 0.6895411108263819 - }, - { - "x": 2.156432157972857, - "y": 4.747720771234375, - "heading": -0.47395861648198423, - "angularVelocity": -0.9226999968520704, - "velocityX": 1.7078868044203455, - "velocityY": -1.6610867310239321, - "timestamp": 0.766156789807091 - }, - { - "x": 2.270931566035856, - "y": 4.636361300878039, - "heading": -0.5358188421760707, - "angularVelocity": -0.8074094819002076, - "velocityX": 1.494464443178203, - "velocityY": -1.4534814779709395, - "timestamp": 0.8427724687878001 - }, - { - "x": 2.369078961091203, - "y": 4.540907071428243, - "heading": -0.5888499820546531, - "angularVelocity": -0.6921708527593945, - "velocityX": 1.2810353744995546, - "velocityY": -1.2458837496554307, - "timestamp": 0.9193881477685092 - }, - { - "x": 2.4508739613497577, - "y": 4.461357622382182, - "heading": -0.6330536039339079, - "angularVelocity": -0.5769526870004522, - "velocityX": 1.0676013226487002, - "velocityY": -1.0382920318954914, - "timestamp": 0.9960038267492183 - }, - { - "x": 2.516316276718558, - "y": 4.3977125949651334, - "heading": -0.6684300724749328, - "angularVelocity": -0.4617392808541994, - "velocityX": 0.8541634844585878, - "velocityY": -0.8307049969200896, - "timestamp": 1.0726195057299275 - }, - { - "x": 2.5654056965461667, - "y": 4.349971723161198, - "heading": -0.6949791200920894, - "angularVelocity": -0.34652238213663356, - "velocityX": 0.6407228968125727, - "velocityY": -0.6231214340263868, - "timestamp": 1.1492351847106366 - }, - { - "x": 2.5981420853275283, - "y": 4.3181348275927744, - "heading": -0.7127001553640687, - "angularVelocity": -0.2312977640935645, - "velocityX": 0.4272805405331163, - "velocityY": -0.41554021239755634, - "timestamp": 1.2258508636913457 - }, - { - "x": 2.6145253815163745, - "y": 4.302201811854818, - "heading": -0.7215924300890252, - "angularVelocity": -0.11606338040076232, - "velocityX": 0.21383738094203186, - "velocityY": -0.20796024906265667, - "timestamp": 1.3024665426720548 - }, - { - "x": 2.614555597305298, - "y": 4.302172660827637, - "heading": -0.7216551150961179, - "angularVelocity": -0.0008181742381802744, - "velocityX": 0.0003943804906386705, - "velocityY": -0.0003804830723595261, - "timestamp": 1.3790822216527638 - }, - { - "x": 2.5982259689280283, - "y": 4.318054102166664, - "heading": -0.712883613809901, - "angularVelocity": 0.11446303533186522, - "velocityX": -0.213092236881309, - "velocityY": 0.20724365439564615, - "timestamp": 1.4557139589685733 - }, - { - "x": 2.565536606931669, - "y": 4.349846333909782, - "heading": -0.695279424271666, - "angularVelocity": 0.2297245257202117, - "velocityX": -0.426577330596217, - "velocityY": 0.4148702979331944, - "timestamp": 1.5323456962843827 - }, - { - "x": 2.5164876982610407, - "y": 4.39754963766033, - "heading": -0.6688439584276578, - "angularVelocity": 0.34496759102452434, - "velocityX": -0.640059984788952, - "velocityY": 0.6225006162319799, - "timestamp": 1.608977433600192 - }, - { - "x": 2.4510795013292355, - "y": 4.461164391501365, - "heading": -0.6335783197571896, - "angularVelocity": 0.4601962565606419, - "velocityX": -0.8535392668397416, - "velocityY": 0.8301358682967486, - "timestamp": 1.6856091709160015 - }, - { - "x": 2.369312351807588, - "y": 4.540691078953442, - "heading": -0.5894828757685662, - "angularVelocity": 0.5754201263263005, - "velocityX": -1.0670141685730552, - "velocityY": 1.0377774300260596, - "timestamp": 1.762240908231811 - }, - { - "x": 2.2711866760947044, - "y": 4.636130298345392, - "heading": -0.5365564475452359, - "angularVelocity": 0.6906593802750026, - "velocityX": -1.28048350605026, - "velocityY": 1.2454267996017627, - "timestamp": 1.8388726455476203 - }, - { - "x": 2.1567030158826075, - "y": 4.747482768262345, - "heading": -0.47479485869558546, - "angularVelocity": 0.8059531337295586, - "velocityX": -1.4939457747586862, - "velocityY": 1.4530855462992425, - "timestamp": 1.9155043828634297 - }, - { - "x": 2.0258620666320337, - "y": 4.874749320539599, - "heading": -0.4041884988158368, - "angularVelocity": 0.9213722974191146, - "velocityX": -1.7073989681051045, - "velocityY": 1.6607551479679548, - "timestamp": 1.9921361201792391 - }, - { - "x": 1.8786647355617998, - "y": 5.01793086600198, - "heading": -0.3247184712590107, - "angularVelocity": 1.0370380514754407, - "velocityX": -1.9208403227681095, - "velocityY": 1.8684366353759239, - "timestamp": 2.0687678574950485 - }, - { - "x": 1.7478414567124814, - "y": 5.145267849282916, - "heading": -0.25271823102965235, - "angularVelocity": 0.9395616327984198, - "velocityX": -1.7071683803424498, - "velocityY": 1.6616742327261775, - "timestamp": 2.145399594810858 - }, - { - "x": 1.6333728466995314, - "y": 5.256688511191179, - "heading": -0.18963629669305965, - "angularVelocity": 0.8231828815865951, - "velocityX": -1.4937493791208316, - "velocityY": 1.4539754132374219, - "timestamp": 2.222031332126668 - }, - { - "x": 1.535258100629332, - "y": 5.352192443129829, - "heading": -0.13550580636159482, - "angularVelocity": 0.70637169709355, - "velocityX": -1.2803408812977723, - "velocityY": 1.2462712617938094, - "timestamp": 2.2986630694424774 - }, - { - "x": 1.4534965485667528, - "y": 5.431779376588513, - "heading": -0.09035761814526168, - "angularVelocity": 0.5891578320466108, - "velocityX": -1.0669411257422734, - "velocityY": 1.0385636064447687, - "timestamp": 2.375294806758287 - }, - { - "x": 1.3880876748821938, - "y": 5.495449138780266, - "heading": -0.05421785896865615, - "angularVelocity": 0.4716030256909087, - "velocityX": -0.8535480988559686, - "velocityY": 0.8308536963253964, - "timestamp": 2.4519265440740967 - }, - { - "x": 1.339031128559986, - "y": 5.543201612325557, - "heading": -0.02710586002264422, - "angularVelocity": 0.35379595843554007, - "velocityX": -0.6401596525218931, - "velocityY": 0.6231422544707841, - "timestamp": 2.5285582813899063 - }, - { - "x": 1.3063267261975056, - "y": 5.575036700785004, - "heading": -0.009032648967396358, - "angularVelocity": 0.23584498657035882, - "velocityX": -0.4267735994864031, - "velocityY": 0.4154295541377912, - "timestamp": 2.605190018705716 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -2.143037766767562e-26, - "angularVelocity": 0.11787086245509872, - "velocityX": -0.21338776476374371, - "velocityY": 0.20771554341884355, - "timestamp": 2.6818217560215256 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.6489738584934847e-26, - "angularVelocity": 6.447188733213427e-26, - "velocityX": -2.3207888161938065e-29, - "velocityY": -7.774803790845182e-27, - "timestamp": 2.7584534933373352 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.2.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.2.traj deleted file mode 100644 index 094d0061..00000000 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.2.traj +++ /dev/null @@ -1,229 +0,0 @@ -{ - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.6489738584934847e-26, - "angularVelocity": 6.447188733213427e-26, - "velocityX": -2.3207888161938065e-29, - "velocityY": -7.774803790845182e-27, - "timestamp": 0 - }, - { - "x": 1.3242756726096836, - "y": 5.5904571777752485, - "heading": -6.485537692324558e-18, - "angularVelocity": -6.918907801272556e-17, - "velocityX": 0.3659270961745246, - "velocityY": -0.005303363934441593, - "timestamp": 0.09373785630439047 - }, - { - "x": 1.3928781149651148, - "y": 5.58946292585347, - "heading": -1.1202216291674618e-17, - "angularVelocity": -5.031804212956606e-17, - "velocityX": 0.7318541845207394, - "velocityY": -0.010606727755427862, - "timestamp": 0.18747571260878093 - }, - { - "x": 1.4957817770305903, - "y": 5.587971547992073, - "heading": -1.4372763095426617e-17, - "angularVelocity": -3.382497812599112e-17, - "velocityX": 1.0977812611239222, - "velocityY": -0.015910091406222935, - "timestamp": 0.2812135689131714 - }, - { - "x": 1.6329866569713576, - "y": 5.585983044217648, - "heading": -1.5623455517606444e-17, - "angularVelocity": -1.3343083564783124e-17, - "velocityX": 1.4637083181538741, - "velocityY": -0.021213454773344104, - "timestamp": 0.37495142521756186 - }, - { - "x": 1.8044927511173796, - "y": 5.583497414583386, - "heading": -1.5212462606875703e-17, - "angularVelocity": 4.38274958491921e-18, - "velocityX": 1.8296353360316953, - "velocityY": -0.02651681757303531, - "timestamp": 0.46868928152195233 - }, - { - "x": 2.0103000484542934, - "y": 5.580514659248916, - "heading": -1.3020782793974884e-17, - "angularVelocity": 2.337996988615278e-17, - "velocityX": 2.1955622364077554, - "velocityY": -0.031820178669779105, - "timestamp": 0.5624271378263428 - }, - { - "x": 2.181845878009789, - "y": 5.578028453731261, - "heading": -1.0808447718251693e-17, - "angularVelocity": 2.360324840579071e-17, - "velocityX": 1.8300592334557981, - "velocityY": -0.026522961098165376, - "timestamp": 0.6561649941307333 - }, - { - "x": 2.3190905043488854, - "y": 5.576039373914184, - "heading": -1.0180400900611551e-17, - "angularVelocity": 6.701278506879843e-18, - "velocityX": 1.4641323328076266, - "velocityY": -0.021219599997477887, - "timestamp": 0.7499028504351237 - }, - { - "x": 2.4220339164763502, - "y": 5.574547419957035, - "heading": -1.1310869476605041e-17, - "angularVelocity": -1.2059406429055699e-17, - "velocityX": 1.0982053148617765, - "velocityY": -0.015916237196800742, - "timestamp": 0.8436407067395142 - }, - { - "x": 2.490676110725689, - "y": 5.573552591912956, - "heading": -5.948170117055629e-18, - "angularVelocity": 5.721104182574728e-17, - "velocityX": 0.7322782578015892, - "velocityY": -0.010612873829241372, - "timestamp": 0.9373785630439047 - }, - { - "x": 2.5250170852633893, - "y": 5.573054889808518, - "heading": -2.1807385415410986e-18, - "angularVelocity": 4.0191949957409276e-17, - "velocityX": 0.36635118118139864, - "velocityY": -0.0053095101781998125, - "timestamp": 1.0311164193482951 - }, - { - "x": 2.525056838989258, - "y": 5.573054313659668, - "heading": -2.8826489417868528e-33, - "angularVelocity": 2.326435027113688e-17, - "velocityX": 0.0004240928242990202, - "velocityY": -0.000006146357055788897, - "timestamp": 1.1248542756526856 - }, - { - "x": 2.4907821260381073, - "y": 5.573551055438095, - "heading": 8.600966160221058e-18, - "angularVelocity": 9.173661299705183e-17, - "velocityX": -0.3655736854951678, - "velocityY": 0.00529824197033788, - "timestamp": 1.2186102382722352 - }, - { - "x": 2.422192939812278, - "y": 5.57454511523942, - "heading": 1.5616353080474726e-17, - "angularVelocity": 7.482552449285854e-17, - "velocityX": -0.7315714559878368, - "velocityY": 0.010602630184298141, - "timestamp": 1.3123662008917854 - }, - { - "x": 2.3192892814125363, - "y": 5.57603649304769, - "heading": 2.109129083859237e-17, - "angularVelocity": 5.839395174918292e-17, - "velocityX": -1.0975692147397424, - "velocityY": 0.015907018228100082, - "timestamp": 1.4061221635113355 - }, - { - "x": 2.182071152673635, - "y": 5.578025188836313, - "heading": 2.4503256173748732e-17, - "angularVelocity": 3.639115358132533e-17, - "velocityX": -1.4635669539221987, - "velocityY": 0.021211405988282927, - "timestamp": 1.4998781261308856 - }, - { - "x": 2.010538557265611, - "y": 5.5805112025521, - "heading": 2.631777380152167e-17, - "angularVelocity": 1.9353493618514127e-17, - "velocityX": -1.8295646539600892, - "velocityY": 0.026515793181145452, - "timestamp": 1.5936340887504357 - }, - { - "x": 1.8046915062028241, - "y": 5.58349453403542, - "heading": 2.6479996589742425e-17, - "angularVelocity": 1.7308836974551526e-18, - "velocityX": -2.1955622365189336, - "velocityY": 0.03182017867138977, - "timestamp": 1.6873900513699858 - }, - { - "x": 1.6331191680538633, - "y": 5.585981123740855, - "heading": 1.776473605055726e-17, - "angularVelocity": -9.295501651565233e-17, - "velocityX": -1.82998855142187, - "velocityY": 0.026521936706821576, - "timestamp": 1.7811460139895359 - }, - { - "x": 1.495861285585166, - "y": 5.587970395678386, - "heading": 1.0702828905507157e-17, - "angularVelocity": -7.532110300046076e-17, - "velocityX": -1.4639909685910264, - "velocityY": 0.021217551212635195, - "timestamp": 1.874901976609086 - }, - { - "x": 1.3929178697919677, - "y": 5.589462349688662, - "heading": 5.2389574400340815e-18, - "angularVelocity": -5.827717130868218e-17, - "velocityX": -1.097993268485135, - "velocityY": 0.01591316401878714, - "timestamp": 1.968657939228636 - }, - { - "x": 1.3242889243407625, - "y": 5.590456985718543, - "heading": 1.764781469065966e-18, - "angularVelocity": -3.705591096010782e-17, - "velocityX": -0.7319955292724558, - "velocityY": 0.010608776258166279, - "timestamp": 2.062413901848186 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -1.882241835148467e-17, - "velocityX": -0.3659977705035494, - "velocityY": 0.00530438821411795, - "timestamp": 2.1561698644677363 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.5626059218246172e-29, - "velocityY": -2.5406263809505943e-29, - "timestamp": 2.2499258270872864 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.3.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.3.traj deleted file mode 100644 index 0ce184f8..00000000 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.3.traj +++ /dev/null @@ -1,337 +0,0 @@ -{ - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.5626059218246172e-29, - "velocityY": -2.5406263809505943e-29, - "timestamp": 0 - }, - { - "x": 1.3072041550588713, - "y": 5.60554242334907, - "heading": 0.009976426632135488, - "angularVelocity": 0.13081140582840126, - "velocityX": 0.22591674199036743, - "velocityY": 0.19128015517323346, - "timestamp": 0.07626572447004332 - }, - { - "x": 1.341663665671124, - "y": 5.634718250195267, - "heading": 0.02992724315217698, - "angularVelocity": 0.26159610576452386, - "velocityX": 0.4518348296001005, - "velocityY": 0.3825549033430322, - "timestamp": 0.15253144894008663 - }, - { - "x": 1.3933533134136329, - "y": 5.678481259628573, - "heading": 0.05984264331407307, - "angularVelocity": 0.3922522257112071, - "velocityX": 0.6777572508972288, - "velocityY": 0.5738227722565116, - "timestamp": 0.22879717341012995 - }, - { - "x": 1.4622736330550405, - "y": 5.7368307766764, - "heading": 0.09970739234331524, - "angularVelocity": 0.5227085865313741, - "velocityX": 0.9036866841075589, - "velocityY": 0.7650817908955898, - "timestamp": 0.30506289788017327 - }, - { - "x": 1.548425323078623, - "y": 5.809765941188483, - "heading": 0.1495038423997354, - "angularVelocity": 0.652933547987283, - "velocityX": 1.1296252758655412, - "velocityY": 0.9563295312013731, - "timestamp": 0.3813286223502166 - }, - { - "x": 1.6518091876527043, - "y": 5.897285689503854, - "heading": 0.20921528267987785, - "angularVelocity": 0.7829393965514132, - "velocityX": 1.3555744119188056, - "velocityY": 1.1475633247313204, - "timestamp": 0.4575943468202599 - }, - { - "x": 1.7724260670196361, - "y": 5.999388766272788, - "heading": 0.27882903202196435, - "angularVelocity": 0.9127789690876109, - "velocityX": 1.5815345652717379, - "velocityY": 1.3387806580529145, - "timestamp": 0.5338600712903032 - }, - { - "x": 1.910276772460734, - "y": 6.11607377416316, - "heading": 0.3583384554068513, - "angularVelocity": 1.042531542921605, - "velocityX": 1.807505356185654, - "velocityY": 1.5299796694859533, - "timestamp": 0.6101257957603465 - }, - { - "x": 2.065362059261307, - "y": 6.247339244509118, - "heading": 0.4477430189105342, - "angularVelocity": 1.172277115920654, - "velocityX": 2.033486050437564, - "velocityY": 1.7211594236996055, - "timestamp": 0.6863915202303899 - }, - { - "x": 2.203284027987246, - "y": 6.363982908806699, - "heading": 0.5270469936467461, - "angularVelocity": 1.0398376893804102, - "velocityX": 1.8084397630070208, - "velocityY": 1.5294375694163855, - "timestamp": 0.7626572447004332 - }, - { - "x": 2.323970561231782, - "y": 6.4660479017472055, - "heading": 0.5964497874037734, - "angularVelocity": 0.9100129083298684, - "velocityX": 1.5824478694623139, - "velocityY": 1.3382813001795226, - "timestamp": 0.8389229691704765 - }, - { - "x": 2.427421209360281, - "y": 6.553534871396325, - "heading": 0.6559501239234673, - "angularVelocity": 0.7801713929154328, - "velocityX": 1.3564500802865749, - "velocityY": 1.147133528411577, - "timestamp": 0.9151886936405198 - }, - { - "x": 2.5136355947658133, - "y": 6.6264442966918295, - "heading": 0.7055475231333362, - "angularVelocity": 0.6503235827654283, - "velocityX": 1.1304473399101456, - "velocityY": 0.955992036499409, - "timestamp": 0.9914544181105631 - }, - { - "x": 2.582613392420247, - "y": 6.6847765351095845, - "heading": 0.7452421346528295, - "angularVelocity": 0.5204777344326409, - "velocityX": 0.9044403377486854, - "velocityY": 0.7648552318285594, - "timestamp": 1.0677201425806064 - }, - { - "x": 2.634354338508759, - "y": 6.728531844563373, - "heading": 0.7750344345644219, - "angularVelocity": 0.3906381285168185, - "velocityX": 0.6784298764341418, - "velocityY": 0.5737218089360453, - "timestamp": 1.1439858670506498 - }, - { - "x": 2.6688582435593817, - "y": 6.757710392622863, - "heading": 0.7949250057401149, - "angularVelocity": 0.26080616524767886, - "velocityX": 0.4524169307537305, - "velocityY": 0.3825905832253757, - "timestamp": 1.220251591520693 - }, - { - "x": 2.686125003210816, - "y": 6.772312259170968, - "heading": 0.8049144291757789, - "angularVelocity": 0.13098182002535327, - "velocityX": 0.22640261668277428, - "velocityY": 0.19146040494338912, - "timestamp": 1.2965173159907364 - }, - { - "x": 2.68615460395813, - "y": 6.772337436676025, - "heading": 0.8050032485420815, - "angularVelocity": 0.0011646034399446482, - "velocityX": 0.0003881255345023293, - "velocityY": 0.00033012790587399484, - "timestamp": 1.3727830404607797 - }, - { - "x": 2.668940238370334, - "y": 6.757780008152215, - "heading": 0.7951880324492147, - "angularVelocity": -0.12867188925922995, - "velocityX": -0.22567052234080015, - "velocityY": -0.1908395916273825, - "timestamp": 1.4490640068049814 - }, - { - "x": 2.6344820969778255, - "y": 6.728639813103283, - "heading": 0.7754676749092158, - "angularVelocity": -0.2585226492096862, - "velocityX": -0.4517265968129291, - "velocityY": -0.3820113508147273, - "timestamp": 1.525344973149183 - }, - { - "x": 2.582780444824575, - "y": 6.6849166026467195, - "heading": 0.7458414179763078, - "angularVelocity": -0.3883833453977379, - "velocityX": -0.6777791967096236, - "velocityY": -0.5731863727099495, - "timestamp": 1.6016259394933847 - }, - { - "x": 2.513835599527289, - "y": 6.6266100250455136, - "heading": 0.7063091354285856, - "angularVelocity": -0.518245696283539, - "velocityX": -0.9038276328322399, - "velocityY": -0.7643660056769808, - "timestamp": 1.6779069058375864 - }, - { - "x": 2.427647916545295, - "y": 6.553719605348423, - "heading": 0.6568717276359699, - "angularVelocity": -0.6480962438334487, - "velocityX": -1.1298714090855582, - "velocityY": -0.9555518649841811, - "timestamp": 1.754187872181788 - }, - { - "x": 2.3242177842468106, - "y": 6.466244710155765, - "heading": 0.5975314416664378, - "angularVelocity": -0.7779173336036685, - "velocityX": -1.3559100940527828, - "velocityY": -1.1467460278003494, - "timestamp": 1.8304688385259897 - }, - { - "x": 2.2035456412981804, - "y": 6.364184497615964, - "heading": 0.5282916867852259, - "angularVelocity": -0.907693729851007, - "velocityX": -1.5819430283540434, - "velocityY": -1.3379512268110498, - "timestamp": 1.9067498048701914 - }, - { - "x": 2.0656320318873616, - "y": 6.247537861954298, - "heading": 0.4491555706593054, - "angularVelocity": -1.0374293863583475, - "velocityX": -1.8079688286610023, - "velocityY": -1.529170921898447, - "timestamp": 1.983030771214393 - }, - { - "x": 1.9104777180009005, - "y": 6.1163033962265265, - "heading": 0.3601220361949963, - "angularVelocity": -1.1671789007706412, - "velocityX": -2.0339846397576693, - "velocityY": -1.7204090612237575, - "timestamp": 2.0593117375585948 - }, - { - "x": 1.7725837401718476, - "y": 5.99956344062706, - "heading": 0.2802286748074377, - "angularVelocity": -1.047356440677276, - "velocityX": -1.8077114707660742, - "velocityY": -1.530394293900403, - "timestamp": 2.1355927039027964 - }, - { - "x": 1.6519287666788485, - "y": 5.897413817352657, - "heading": 0.21025683762829658, - "angularVelocity": -0.9172909120831881, - "velocityX": -1.581717947527036, - "velocityY": -1.3391233513887155, - "timestamp": 2.211873670246998 - }, - { - "x": 1.5485118574908003, - "y": 5.80985551317875, - "heading": 0.15022948194911198, - "angularVelocity": -0.7869244264698967, - "velocityX": -1.3557367475609723, - "velocityY": -1.1478394728127326, - "timestamp": 2.2881546365911998 - }, - { - "x": 1.4623321603791597, - "y": 5.736889288541215, - "heading": 0.10017126259617518, - "angularVelocity": -0.6562347301302999, - "velocityX": -1.1297667198257733, - "velocityY": -0.9565456253555378, - "timestamp": 2.3644356029354014 - }, - { - "x": 1.393388970887201, - "y": 5.678515706716561, - "heading": 0.06010509802987578, - "angularVelocity": -0.5252445858334249, - "velocityX": -0.9038059271169646, - "velocityY": -0.7652443936641671, - "timestamp": 2.440716569279603 - }, - { - "x": 1.3416817772407035, - "y": 5.634735172181737, - "heading": 0.030048985724868114, - "angularVelocity": -0.39401850489866874, - "velocityX": -0.6778518434766467, - "velocityY": -0.5739378595104004, - "timestamp": 2.516997535623805 - }, - { - "x": 1.307210288791871, - "y": 5.605547971029586, - "heading": 0.010013480371388656, - "angularVelocity": -0.2626540579888315, - "velocityX": -0.4519015700633713, - "velocityY": -0.38262757484247345, - "timestamp": 2.5932785019680065 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -6.199521898254585e-29, - "angularVelocity": -0.13127102150595574, - "velocityX": -0.22595201079473404, - "velocityY": -0.19131466197828328, - "timestamp": 2.669559468312208 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.3170947337368488e-29, - "angularVelocity": 6.400644992589977e-28, - "velocityX": 2.2164409811256884e-29, - "velocityY": 1.733201459680474e-29, - "timestamp": 2.74584043465641 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.4.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.4.traj deleted file mode 100644 index 9d967ddd..00000000 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubAS.4.traj +++ /dev/null @@ -1,508 +0,0 @@ -{ - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.3170947337368488e-29, - "angularVelocity": 6.400644992589977e-28, - "velocityX": 2.2164409811256884e-29, - "velocityY": 1.733201459680474e-29, - "timestamp": 0 - }, - { - "x": 1.3080029680398912, - "y": 5.59126500545377, - "heading": 8.890962339325397e-19, - "angularVelocity": 1.3082751589925716e-17, - "velocityX": 0.2652835543681239, - "velocityY": 0.004571871036655683, - "timestamp": 0.06795942182608883 - }, - { - "x": 1.344062678768715, - "y": 5.591577976323001, - "heading": 2.6666437965292957e-18, - "angularVelocity": 2.6156013615677146e-17, - "velocityX": 0.5306064966400389, - "velocityY": 0.004605260916318704, - "timestamp": 0.13591884365217766 - }, - { - "x": 1.398150210579935, - "y": 5.5915422166660145, - "heading": 8.768948399955272e-19, - "angularVelocity": -2.6335553017414816e-17, - "velocityX": 0.7958798111854469, - "velocityY": -0.000526191306891862, - "timestamp": 0.20387826547826648 - }, - { - "x": 1.4702532783568034, - "y": 5.590755190471311, - "heading": -4.372782060498752e-19, - "angularVelocity": -1.9337613692601342e-17, - "velocityX": 1.060972354376158, - "velocityY": -0.011580825344825147, - "timestamp": 0.2718376873043553 - }, - { - "x": 1.560346433856685, - "y": 5.58875133578056, - "heading": -1.4651998990372215e-18, - "angularVelocity": -1.512552145628718e-17, - "velocityX": 1.3256904352487537, - "velocityY": -0.029486046774778202, - "timestamp": 0.33979710913044414 - }, - { - "x": 1.6683846538857667, - "y": 5.584987203919277, - "heading": -2.5518211074829943e-18, - "angularVelocity": -1.5989265053292237e-17, - "velocityX": 1.5897460149904699, - "velocityY": -0.05538793238877104, - "timestamp": 0.40775653095653297 - }, - { - "x": 1.7942934964417667, - "y": 5.578822081824432, - "heading": -4.145349651168243e-18, - "angularVelocity": -2.3448235739309385e-17, - "velocityX": 1.852706205744448, - "velocityY": -0.0907176949006653, - "timestamp": 0.4757159527826218 - }, - { - "x": 1.937953698284471, - "y": 5.569492606449359, - "heading": -1.0979652144550452e-17, - "angularVelocity": -1.0056445905133748e-16, - "velocityX": 2.1139114780925645, - "velocityY": -0.13728008750497092, - "timestamp": 0.5436753746087106 - }, - { - "x": 2.099176605031056, - "y": 5.556079595057618, - "heading": -1.979270285045548e-17, - "angularVelocity": -1.2968107245612037e-16, - "velocityX": 2.3723407647457755, - "velocityY": -0.19736794444875114, - "timestamp": 0.6116347964347995 - }, - { - "x": 2.2776642824157327, - "y": 5.537465473885052, - "heading": -2.5427652949352827e-17, - "angularVelocity": -8.291639256904635e-17, - "velocityX": 2.6263860490372255, - "velocityY": -0.2739005228767256, - "timestamp": 0.6795942182608883 - }, - { - "x": 2.472944009712466, - "y": 5.512282419629533, - "heading": -2.829850545817078e-17, - "angularVelocity": -4.224362761891433e-17, - "velocityX": 2.8734754070813326, - "velocityY": -0.3705601604434455, - "timestamp": 0.7475536400869771 - }, - { - "x": 2.6842610977752166, - "y": 5.47885759062817, - "heading": -2.887338329485776e-17, - "angularVelocity": -8.459133718914728e-18, - "velocityX": 3.109459768558945, - "velocityY": -0.49183509958190536, - "timestamp": 0.8155130619130659 - }, - { - "x": 2.9104101060267347, - "y": 5.435179084259705, - "heading": -3.3223671712936815e-17, - "angularVelocity": -6.40130286748416e-17, - "velocityX": 3.32770647799568, - "velocityY": -0.6427145080816032, - "timestamp": 0.8834724837391548 - }, - { - "x": 3.1494985869220757, - "y": 5.378942960028073, - "heading": -3.8165211155711695e-17, - "angularVelocity": -7.271308833998824e-17, - "velocityX": 3.5181064592806273, - "velocityY": -0.8274956248971932, - "timestamp": 0.9514319055652436 - }, - { - "x": 3.3987095123055417, - "y": 5.307785046252999, - "heading": -4.255972014068572e-17, - "angularVelocity": -6.466371942097715e-17, - "velocityX": 3.667054820172052, - "velocityY": -1.0470647316154307, - "timestamp": 1.0193913273913324 - }, - { - "x": 3.6542758900932353, - "y": 5.21975313201646, - "heading": -4.248444848952633e-17, - "angularVelocity": 1.107596991510937e-18, - "velocityX": 3.7605731614624154, - "velocityY": -1.2953599643889915, - "timestamp": 1.0873507492174213 - }, - { - "x": 3.9119052041412368, - "y": 5.113808430044301, - "heading": -4.228369470192057e-17, - "angularVelocity": 2.9540243605887717e-18, - "velocityX": 3.790928573631551, - "velocityY": -1.5589406019856407, - "timestamp": 1.15531017104351 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -4.208890664344976e-17, - "angularVelocity": 2.8662406659266305e-18, - "velocityX": 3.7609775714691667, - "velocityY": -1.8225675093785412, - "timestamp": 1.223269592869599 - }, - { - "x": 4.282012195193535, - "y": 4.930397905958738, - "heading": -4.1602494438105564e-17, - "angularVelocity": 1.582195542444361e-17, - "velocityX": 3.7248687742898503, - "velocityY": -1.9370313765192848, - "timestamp": 1.2540124557013765 - }, - { - "x": 4.3952613097643365, - "y": 4.867381383869913, - "heading": -4.188915157617957e-17, - "angularVelocity": -9.32434756133803e-18, - "velocityX": 3.6837530450723373, - "velocityY": -2.049793554804778, - "timestamp": 1.2847553185331542 - }, - { - "x": 4.506961712126711, - "y": 4.801015720721852, - "heading": -4.2434270081658926e-17, - "angularVelocity": -1.7731546618224465e-17, - "velocityX": 3.6333767279120854, - "velocityY": -2.158733996609517, - "timestamp": 1.3154981813649318 - }, - { - "x": 4.616614691776683, - "y": 4.73158032272052, - "heading": -4.19288728234691e-17, - "angularVelocity": 1.6439498850683494e-17, - "velocityX": 3.5667784178065634, - "velocityY": -2.258585948266306, - "timestamp": 1.3462410441967094 - }, - { - "x": 4.722598987733627, - "y": 4.661750165167655, - "heading": -3.986850310029329e-17, - "angularVelocity": 6.701944885386403e-17, - "velocityX": 3.4474439331457423, - "velocityY": -2.271426637622177, - "timestamp": 1.376983907028487 - }, - { - "x": 4.827229210309334, - "y": 4.595352451522488, - "heading": -3.770620807844877e-17, - "angularVelocity": 7.033486223050351e-17, - "velocityX": 3.4033988034307483, - "velocityY": -2.1597765311737476, - "timestamp": 1.4077267698602647 - }, - { - "x": 4.9314726269952285, - "y": 4.532624292130925, - "heading": -3.5259240937147126e-17, - "angularVelocity": 7.959464135435417e-17, - "velocityX": 3.390816829789258, - "velocityY": -2.04041372902732, - "timestamp": 1.4384696326920423 - }, - { - "x": 5.035668651391688, - "y": 4.473585715832336, - "heading": -3.400468055587437e-17, - "angularVelocity": 4.080818328915737e-17, - "velocityX": 3.389275259321544, - "velocityY": -1.9203994312970292, - "timestamp": 1.46921249552382 - }, - { - "x": 5.139985480442779, - "y": 4.418235050432876, - "heading": -3.272140280940892e-17, - "angularVelocity": 4.1742298155096073e-17, - "velocityX": 3.3932047780294075, - "velocityY": -1.8004395264792032, - "timestamp": 1.4999553583555976 - }, - { - "x": 5.244522953443759, - "y": 4.366567672079215, - "heading": -3.123998369139628e-17, - "angularVelocity": 4.8187415925402766e-17, - "velocityX": 3.400381856855714, - "velocityY": -1.680630025784517, - "timestamp": 1.5306982211873752 - }, - { - "x": 5.3493470515644415, - "y": 4.31857903504105, - "heading": -3.003359756723782e-17, - "angularVelocity": 3.924117707448193e-17, - "velocityX": 3.409705162927411, - "velocityY": -1.5609683880370886, - "timestamp": 1.5614410840191528 - }, - { - "x": 5.454504582553393, - "y": 4.274265192865899, - "heading": -2.87901792623276e-17, - "angularVelocity": 4.044575522175447e-17, - "velocityX": 3.4205510255946114, - "velocityY": -1.4414351200027105, - "timestamp": 1.5921839468509305 - }, - { - "x": 5.560030462239174, - "y": 4.233622810392966, - "heading": -2.7523978224966667e-17, - "angularVelocity": 4.118682909556349e-17, - "velocityX": 3.432532625969479, - "velocityY": -1.3220103376619752, - "timestamp": 1.622926809682708 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -2.6172162518713254e-17, - "angularVelocity": 4.3971692345317756e-17, - "velocityX": 3.4453937215027324, - "velocityY": -1.2026770584391975, - "timestamp": 1.6536696725144857 - }, - { - "x": 5.792835589324764, - "y": 4.157776846380009, - "heading": -2.4556205828219047e-17, - "angularVelocity": 4.408549337244093e-17, - "velocityX": 3.461564053183983, - "velocityY": -1.060487182383484, - "timestamp": 1.6903247389972957 - }, - { - "x": 5.920107057755266, - "y": 4.124135852213366, - "heading": -2.3047234390121686e-17, - "angularVelocity": 4.1166790375489185e-17, - "velocityX": 3.472138523884214, - "velocityY": -0.917772013383704, - "timestamp": 1.7269798054801058 - }, - { - "x": 6.047516391811881, - "y": 4.095738620024666, - "heading": -2.1456366912742873e-17, - "angularVelocity": 4.3401025561499594e-17, - "velocityX": 3.475899685419091, - "velocityY": -0.7747150643422807, - "timestamp": 1.7636348719629158 - }, - { - "x": 6.174755293992793, - "y": 4.0725841920992965, - "heading": -1.9852024700254233e-17, - "angularVelocity": 4.3768634637209445e-17, - "velocityX": 3.4712500723626603, - "velocityY": -0.631684243056213, - "timestamp": 1.8002899384457258 - }, - { - "x": 6.30143822417369, - "y": 4.054645790921562, - "heading": -1.7231399343635e-17, - "angularVelocity": 7.149421916471826e-17, - "velocityX": 3.4560824010592928, - "velocityY": -0.48938394876864877, - "timestamp": 1.8369450049285359 - }, - { - "x": 6.427078572160238, - "y": 4.041848309685929, - "heading": -1.4890621747726543e-17, - "angularVelocity": 6.385959215232344e-17, - "velocityX": 3.427639342721413, - "velocityY": -0.3491326701490028, - "timestamp": 1.8736000714113459 - }, - { - "x": 6.551062514369357, - "y": 4.034028011406828, - "heading": -1.2144086622031307e-17, - "angularVelocity": 7.492920868069778e-17, - "velocityX": 3.382450343317836, - "velocityY": -0.21334835888971543, - "timestamp": 1.910255137894156 - }, - { - "x": 6.672633087813251, - "y": 4.030865145435154, - "heading": -9.61251747475391e-18, - "angularVelocity": 6.906464481423604e-17, - "velocityX": 3.316610365470415, - "velocityY": -0.08628727963586283, - "timestamp": 1.946910204376966 - }, - { - "x": 6.790917094390862, - "y": 4.031790616181229, - "heading": -6.811165774297211e-18, - "angularVelocity": 7.642467929421218e-17, - "velocityX": 3.2269483574141518, - "velocityY": 0.025248098963559784, - "timestamp": 1.983565270859776 - }, - { - "x": 6.905041403639001, - "y": 4.0359118549290525, - "heading": -4.3348375779027234e-18, - "angularVelocity": 6.755759664372386e-17, - "velocityX": 3.113466164402112, - "velocityY": 0.11243299066875824, - "timestamp": 2.020220337342586 - }, - { - "x": 7.014327663163953, - "y": 4.042060231899537, - "heading": -2.4762499879984938e-18, - "angularVelocity": 5.070479385914931e-17, - "velocityX": 2.9814775967247074, - "velocityY": 0.16773607472154842, - "timestamp": 2.056875403825396 - }, - { - "x": 7.1184276572510345, - "y": 4.048995163672738, - "heading": -1.0562416558517327e-18, - "angularVelocity": 3.873975601197627e-17, - "velocityX": 2.8399892313904544, - "velocityY": 0.1891943580692654, - "timestamp": 2.093530470308206 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 3.646325715989205e-37, - "angularVelocity": 2.881570700048058e-17, - "velocityX": 2.697135958773559, - "velocityY": 0.18068777649551113, - "timestamp": 2.130185536791016 - }, - { - "x": 7.386231209731221, - "y": 4.063934540767167, - "heading": 6.43663529397786e-19, - "angularVelocity": 9.269678756083754e-18, - "velocityX": 2.432976407238177, - "velocityY": 0.11976600412078256, - "timestamp": 2.199623059528829 - }, - { - "x": 7.536591060669406, - "y": 4.069229370335429, - "heading": -1.8193245120818273e-19, - "angularVelocity": -1.1889767204445143e-17, - "velocityX": 2.1653976842740037, - "velocityY": 0.07625314613043933, - "timestamp": 2.2690605822666416 - }, - { - "x": 7.668251510768197, - "y": 4.072361333543041, - "heading": -2.313295063693691e-18, - "angularVelocity": -3.069468103770411e-17, - "velocityX": 1.8960994705401897, - "velocityY": 0.04510476589779892, - "timestamp": 2.3384981050044544 - }, - { - "x": 7.78114979643099, - "y": 4.073967912827302, - "heading": -5.036275587489351e-18, - "angularVelocity": -3.9214828185580025e-17, - "velocityX": 1.6258973709226552, - "velocityY": 0.023137047822506123, - "timestamp": 2.407935627742267 - }, - { - "x": 7.875252481646468, - "y": 4.074539583547341, - "heading": -6.689419008155875e-18, - "angularVelocity": -2.380763822621624e-17, - "velocityX": 1.3552137447471682, - "velocityY": 0.008232878960810142, - "timestamp": 2.47737315048008 - }, - { - "x": 7.950542203811188, - "y": 4.074464670768735, - "heading": -5.15013780810139e-18, - "angularVelocity": 2.2167858808364967e-17, - "velocityX": 1.0842800721592976, - "velocityY": -0.001078851543840728, - "timestamp": 2.546810673217893 - }, - { - "x": 8.007010780474163, - "y": 4.074057877238601, - "heading": -4.959999016913383e-18, - "angularVelocity": 2.738271523682432e-18, - "velocityX": 0.8132285605319336, - "velocityY": -0.005858410756812446, - "timestamp": 2.6162481959557056 - }, - { - "x": 8.044655422701718, - "y": 4.073579204097417, - "heading": -1.6663567301752781e-18, - "angularVelocity": 4.7433176715915545e-17, - "velocityX": 0.5421368842563122, - "velocityY": -0.006893580333953516, - "timestamp": 2.6856857186935184 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 4.9523840537519984e-39, - "angularVelocity": 2.3997928849898842e-17, - "velocityX": 0.27105142948932054, - "velocityY": -0.004784851370482414, - "timestamp": 2.755123241431331 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": -7.535317609356433e-39, - "angularVelocity": -1.798408291474874e-37, - "velocityX": 0, - "velocityY": 1.827412307849029e-40, - "timestamp": 2.824560764169144 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.1.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.1.traj index 5044b6d7..7c45b816 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.1.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.1.traj @@ -1,319 +1,320 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -6.079995614896932e-33, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.306947763000364, - "y": 5.574973108257302, - "heading": -0.008994433988091095, - "angularVelocity": -0.12277149964907928, - "velocityX": 0.23168094435597542, - "velocityY": -0.21813883323536412, - "timestamp": 0.07326157955062942 - }, - { - "x": 1.3408944736698363, - "y": 5.543010933982932, - "heading": -0.026978879063552612, - "angularVelocity": -0.24548262794460876, - "velocityX": 0.463363073492191, - "velocityY": -0.4362747086592839, - "timestamp": 0.14652315910125885 - }, - { - "x": 1.3918148440551168, - "y": 5.495068027805583, - "heading": -0.05394057952659419, - "angularVelocity": -0.3680196445178875, - "velocityX": 0.695048764954495, - "velocityY": -0.6544072141417159, - "timestamp": 0.21978473865188827 - }, - { - "x": 1.4597093144768722, - "y": 5.431144700814373, - "heading": -0.08985971213865897, - "angularVelocity": -0.4902860794482582, - "velocityX": 0.9267404666703231, - "velocityY": -0.8725354733449758, - "timestamp": 0.2930463182025177 - }, - { - "x": 1.5445785095442424, - "y": 5.351241370164591, - "heading": -0.13471107898519388, - "angularVelocity": -0.6122085699167754, - "velocityX": 1.1584406941256162, - "velocityY": -1.0906580384956333, - "timestamp": 0.3663078977531471 - }, - { - "x": 1.6464232512218273, - "y": 5.2553586089581215, - "heading": -0.1884659824129613, - "angularVelocity": -0.733739345472597, - "velocityX": 1.390152141167011, - "velocityY": -1.3087727809664234, - "timestamp": 0.4395694773037765 - }, - { - "x": 1.7652446226105383, - "y": 5.143497220947543, - "heading": -0.25109340653071877, - "angularVelocity": -0.8548467628175698, - "velocityX": 1.6218783722318773, - "velocityY": -1.5268765524400676, - "timestamp": 0.5128310568544059 - }, - { - "x": 1.9010443771824763, - "y": 5.015658495720495, - "heading": -0.32255561727496634, - "angularVelocity": -0.9754391207858365, - "velocityX": 1.8536285376988066, - "velocityY": -1.7449627214043517, - "timestamp": 0.5860926364050353 - }, - { - "x": 2.053859595250169, - "y": 4.871864096281773, - "heading": -0.4022671776466062, - "angularVelocity": -1.0880404280193408, - "velocityX": 2.0858848390251463, - "velocityY": -1.9627531964329257, - "timestamp": 0.6593542159556647 - }, - { - "x": 2.1896989198822134, - "y": 4.744047518400832, - "heading": -0.47311179344304544, - "angularVelocity": -0.9670091230763074, - "velocityX": 1.8541686579139196, - "velocityY": -1.7446604163456465, - "timestamp": 0.7326157955062941 - }, - { - "x": 2.308560686135844, - "y": 4.632207510590712, - "heading": -0.535112719027594, - "angularVelocity": -0.846295233666115, - "velocityX": 1.6224297508012064, - "velocityY": -1.5265847187041606, - "timestamp": 0.8058773750569235 - }, - { - "x": 2.410444090432368, - "y": 4.536343322874329, - "heading": -0.5882784511543253, - "angularVelocity": -0.72569732256441, - "velocityX": 1.3906798750648661, - "velocityY": -1.3085192580393974, - "timestamp": 0.8791389546075529 - }, - { - "x": 2.49534860091136, - "y": 4.4564544045860375, - "heading": -0.6326127919617512, - "angularVelocity": -0.6051513095863228, - "velocityX": 1.158922739582985, - "velocityY": -1.090461313806123, - "timestamp": 0.9524005341581823 - }, - { - "x": 2.5632738362111045, - "y": 4.3925403400214185, - "heading": -0.6681173722205684, - "angularVelocity": -0.48462755617056985, - "velocityX": 0.9271603986207089, - "velocityY": -0.8724090438215177, - "timestamp": 1.0256621137088118 - }, - { - "x": 2.6142195241645787, - "y": 4.344600823178412, - "heading": -0.6947927082053813, - "angularVelocity": -0.36411084975827024, - "velocityX": 0.6953943426549579, - "velocityY": -0.6543609506791417, - "timestamp": 1.0989236932594413 - }, - { - "x": 2.6481854848919673, - "y": 4.312635645188571, - "heading": -0.7126387384821254, - "angularVelocity": -0.24359330478823768, - "velocityX": 0.4636258313802147, - "velocityY": -0.43631570853248003, - "timestamp": 1.1721852728100708 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": -0.12307101033443453, - "velocityX": 0.23185602115327572, - "velocityY": -0.2182720825031122, - "timestamp": 1.2454468523607003 - }, - { - "x": 2.662943710448623, - "y": 4.298728558024191, - "heading": -0.72070100113944, - "angularVelocity": 0.011651349233264986, - "velocityX": -0.027206592771411266, - "velocityY": 0.025447590705983056, - "timestamp": 1.3273355622067569 - }, - { - "x": 2.639501593010729, - "y": 4.320770481747221, - "heading": -0.7087160822503835, - "angularVelocity": 0.14635618159801306, - "velocityX": -0.2862680030246291, - "velocityY": 0.26916926355864296, - "timestamp": 1.4092242720528134 - }, - { - "x": 2.594845480264057, - "y": 4.36277075661854, - "heading": -0.6857017147672647, - "angularVelocity": 0.2810444507720759, - "velocityX": -0.5453268567867454, - "velocityY": 0.5128945730159264, - "timestamp": 1.49111298189887 - }, - { - "x": 2.5289756911084322, - "y": 4.424729822216665, - "heading": -0.6516589308540864, - "angularVelocity": 0.4157201154734962, - "velocityX": -0.8043818162412647, - "velocityY": 0.7566252504722968, - "timestamp": 1.5730016917449265 - }, - { - "x": 2.4418926576481637, - "y": 4.5066482714289275, - "heading": -0.6065878922273316, - "angularVelocity": 0.550393805342452, - "velocityX": -1.0634314989694753, - "velocityY": 1.000363168088272, - "timestamp": 1.654890401590983 - }, - { - "x": 2.3335969392795284, - "y": 4.608526864652899, - "heading": -0.5504868101397262, - "angularVelocity": 0.6850893388486693, - "velocityX": -1.322474350520624, - "velocityY": 1.2441103714479649, - "timestamp": 1.7367791114370397 - }, - { - "x": 2.2040892544412327, - "y": 4.730366540742225, - "heading": -0.48334990238896663, - "angularVelocity": 0.8198554828494793, - "velocityX": -1.5815084287169574, - "velocityY": 1.4878690398025962, - "timestamp": 1.8186678212830962 - }, - { - "x": 2.0533705390200705, - "y": 4.8721684079929855, - "heading": -0.4051637468918412, - "angularVelocity": 0.9547855332451622, - "velocityX": -1.840531078148622, - "velocityY": 1.7316412423316419, - "timestamp": 1.9005565311291528 - }, - { - "x": 1.8837222142414454, - "y": 5.031895792612386, - "heading": -0.31535335280985394, - "angularVelocity": 1.0967371967493782, - "velocityX": -2.071693705976669, - "velocityY": 1.9505422044097782, - "timestamp": 1.9824452409752094 - }, - { - "x": 1.7352824931464605, - "y": 5.171658722106698, - "heading": -0.2366561880408508, - "angularVelocity": 0.9610258229363577, - "velocityX": -1.8127006930996745, - "velocityY": 1.706742355045694, - "timestamp": 2.0643339508212657 - }, - { - "x": 1.6080502362813274, - "y": 5.291456457157948, - "heading": -0.16911546139257225, - "angularVelocity": 0.8247867963147685, - "velocityX": -1.5537215948855285, - "velocityY": 1.4629334773555265, - "timestamp": 2.146222660667322 - }, - { - "x": 1.5020244624691528, - "y": 5.391288489879848, - "heading": -0.11277393629567076, - "angularVelocity": 0.6880255556940454, - "velocityX": -1.294754478504957, - "velocityY": 1.219118397512621, - "timestamp": 2.2281113705133784 - }, - { - "x": 1.4172044018013137, - "y": 5.471154488105965, - "heading": -0.06766925261840594, - "angularVelocity": 0.550804668458663, - "velocityX": -1.0357967640141517, - "velocityY": 0.9752992613543129, - "timestamp": 2.3100000803594347 - }, - { - "x": 1.3535895239653573, - "y": 5.531054232218378, - "heading": -0.03382997301748837, - "angularVelocity": 0.4132349827532073, - "velocityX": -0.7768455255376048, - "velocityY": 0.7314774432888099, - "timestamp": 2.391888790205491 - }, - { - "x": 1.3111795494900627, - "y": 5.570987556283822, - "heading": -0.011272655480386088, - "angularVelocity": 0.27546309594458174, - "velocityX": -0.5178976998785506, - "velocityY": 0.48765359889678683, - "timestamp": 2.4737775000515474 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0.13765823764445279, - "velocityX": -0.2589502077253713, - "velocityY": 0.24382784263139995, - "timestamp": 2.5556662098976037 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.169901203643428e-30, - "velocityY": 4.744348570353821e-29, - "timestamp": 2.63755491974366 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 4.7251350507187945e-25, + "angularVelocity": 2.5513158875858104e-25, + "velocityX": 5.636670431941991e-25, + "velocityY": -5.06363269097065e-25, + "timestamp": 0 + }, + { + "x": 1.3069477629998796, + "y": 5.574973108256567, + "heading": -0.008994433993404482, + "angularVelocity": -0.12277149941396481, + "velocityX": 0.23168094376881843, + "velocityY": -0.2181388326987634, + "timestamp": 0.07326157973420823 + }, + { + "x": 1.3408944736683446, + "y": 5.5430109339807085, + "heading": -0.02697887908003383, + "angularVelocity": -0.2454826274819163, + "velocityX": 0.46336307231734836, + "velocityY": -0.4362747075863933, + "timestamp": 0.14652315946841646 + }, + { + "x": 1.3918148440520437, + "y": 5.495068027801086, + "heading": -0.05394057956084241, + "angularVelocity": -0.3680196438382188, + "velocityX": 0.6950487631912567, + "velocityY": -0.6544072125329379, + "timestamp": 0.2197847392026247 + }, + { + "x": 1.4597093144715716, + "y": 5.431144700806779, + "heading": -0.0898597121983473, + "angularVelocity": -0.4902860785669502, + "velocityX": 0.9267404643176941, + "velocityY": -0.8725354712008585, + "timestamp": 0.2930463189368329 + }, + { + "x": 1.5445785095359612, + "y": 5.351241370153018, + "heading": -0.13471107907964566, + "angularVelocity": -0.6122085688572148, + "velocityX": 1.1584406911821108, + "velocityY": -1.090658035816974, + "timestamp": 0.36630789867104113 + }, + { + "x": 1.646423251209635, + "y": 5.255358608941595, + "heading": -0.18846598255428795, + "angularVelocity": -0.733739344273822, + "velocityX": 1.390152137630185, + "velocityY": -1.308772777754508, + "timestamp": 0.43956947840524935 + }, + { + "x": 1.7652446225931595, + "y": 5.143497220924912, + "heading": -0.2510934067365697, + "angularVelocity": -0.854846761556235, + "velocityX": 1.6218783680969795, + "velocityY": -1.5268765486973532, + "timestamp": 0.5128310581394576 + }, + { + "x": 1.9010443771576746, + "y": 5.0156584956901185, + "heading": -0.3225556175785802, + "angularVelocity": -0.975439119676018, + "velocityX": 1.8536285329526645, + "velocityY": -1.7449627171375512, + "timestamp": 0.5860926378736658 + }, + { + "x": 2.053859595231987, + "y": 4.871864096254743, + "heading": -0.40226717784175786, + "angularVelocity": -1.0880404238124515, + "velocityX": 2.0858848338886964, + "velocityY": -1.9627531914689686, + "timestamp": 0.659354217607874 + }, + { + "x": 2.189698919868216, + "y": 4.744047518376405, + "heading": -0.4731117935721764, + "angularVelocity": -0.9670091197520122, + "velocityX": 1.8541686533248656, + "velocityY": -1.7446604119383569, + "timestamp": 0.7326157973420823 + }, + { + "x": 2.3085606861249848, + "y": 4.632207510568855, + "heading": -0.5351127191107611, + "angularVelocity": -0.8462952309180741, + "velocityX": 1.6224297467785629, + "velocityY": -1.5265847148437626, + "timestamp": 0.8058773770762905 + }, + { + "x": 2.410444090424028, + "y": 4.5363433228552275, + "heading": -0.5882784512046502, + "angularVelocity": -0.7256973202976708, + "velocityX": 1.3906798716144855, + "velocityY": -1.3085192547228937, + "timestamp": 0.8791389568104987 + }, + { + "x": 2.495348600905127, + "y": 4.456454404569983, + "heading": -0.6326127919889597, + "angularVelocity": -0.6051513077544012, + "velocityX": 1.158922736707715, + "velocityY": -1.0904613110320627, + "timestamp": 0.9524005365447069 + }, + { + "x": 2.563273836206689, + "y": 4.392540340008763, + "heading": -0.6681173722323603, + "angularVelocity": -0.4846275547457583, + "velocityX": 0.927160396322236, + "velocityY": -0.8724090415890471, + "timestamp": 1.0256621162789152 + }, + { + "x": 2.6142195241617716, + "y": 4.344600823169547, + "heading": -0.6947927082081052, + "angularVelocity": -0.364110848722108, + "velocityX": 0.6953943409343986, + "velocityY": -0.6543609489877007, + "timestamp": 1.0989236960131235 + }, + { + "x": 2.6481854848906172, + "y": 4.312635645183918, + "heading": -0.712638738481162, + "angularVelocity": -0.24359330412750974, + "velocityX": 0.46362583023836135, + "velocityY": -0.4363157073816743, + "timestamp": 1.1721852757473317 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": -0.12307101003919331, + "velocityX": 0.2318560205907196, + "velocityY": -0.2182720818926514, + "timestamp": 1.2454468554815399 + }, + { + "x": 2.6629437104502958, + "y": 4.2987285580297225, + "heading": -0.7207010011457456, + "angularVelocity": 0.011651349127194545, + "velocityX": -0.027206592683106, + "velocityY": 0.025447590710046028, + "timestamp": 1.3273355655318961 + }, + { + "x": 2.6395015930145194, + "y": 4.320770481758526, + "heading": -0.7087160822692053, + "angularVelocity": 0.14635618108003276, + "velocityX": -0.28626800228457533, + "velocityY": 0.2691692629576105, + "timestamp": 1.4092242755822524 + }, + { + "x": 2.594845480270507, + "y": 4.362770756636002, + "heading": -0.6857017148059629, + "angularVelocity": 0.2810444498281884, + "velocityX": -0.545326855393761, + "velocityY": 0.5128945718115293, + "timestamp": 1.4911129856326086 + }, + { + "x": 2.52897569111822, + "y": 4.424729822240869, + "heading": -0.6516589309216121, + "angularVelocity": 0.415720114084306, + "velocityX": -0.8043818141936927, + "velocityY": 0.7566252486669522, + "timestamp": 1.5730016956829649 + }, + { + "x": 2.4418926576621733, + "y": 4.506648271460754, + "heading": -0.6065878923349923, + "angularVelocity": 0.550393803479186, + "velocityX": -1.0634314962648228, + "velocityY": 1.0003631656856, + "timestamp": 1.6548904057333211 + }, + { + "x": 2.333596939298986, + "y": 4.608526864693725, + "heading": -0.5504868103027082, + "angularVelocity": 0.6850893364639103, + "velocityX": -1.322474347154724, + "velocityY": 1.2441103684539934, + "timestamp": 1.7367791157836774 + }, + { + "x": 2.2040892544680513, + "y": 4.7303665407944155, + "heading": -0.48334990263012423, + "angularVelocity": 0.8198554798494097, + "velocityX": -1.5815084246814508, + "velocityY": 1.4878690362293745, + "timestamp": 1.8186678258340336 + }, + { + "x": 2.0533705390582235, + "y": 4.872168408061871, + "heading": -0.40516374725682197, + "angularVelocity": 0.9547855293510289, + "velocityX": -1.8405310734183675, + "velocityY": 1.7316412382153215, + "timestamp": 1.9005565358843899 + }, + { + "x": 1.8837222142672279, + "y": 5.0318957926604115, + "heading": -0.31535335305240253, + "angularVelocity": 1.0967371955082925, + "velocityX": -2.0716937009591816, + "velocityY": 1.9505421992887515, + "timestamp": 1.9824452459347461 + }, + { + "x": 1.7352824931641453, + "y": 5.171658722140381, + "heading": -0.23665618820579565, + "angularVelocity": 0.9610258214864199, + "velocityX": -1.8127006886761512, + "velocityY": 1.7067423506124824, + "timestamp": 2.064333955985102 + }, + { + "x": 1.608050236293127, + "y": 5.291456457180838, + "heading": -0.1691154615021004, + "angularVelocity": 0.8247867949337834, + "velocityX": -1.5537215910810984, + "velocityY": 1.4629334735739428, + "timestamp": 2.146222666035458 + }, + { + "x": 1.5020244624765908, + "y": 5.391288489894511, + "heading": -0.11277393636454519, + "angularVelocity": 0.6880255544739787, + "velocityX": -1.2947544753280062, + "velocityY": 1.2191183943706372, + "timestamp": 2.228111376085814 + }, + { + "x": 1.4172044018055678, + "y": 5.471154488114474, + "heading": -0.06766925265776652, + "angularVelocity": 0.5508046674449045, + "velocityX": -1.03579676146888, + "velocityY": 0.9752992588459369, + "timestamp": 2.3100000861361702 + }, + { + "x": 1.353589523967397, + "y": 5.531054232222511, + "heading": -0.033829973036366284, + "angularVelocity": 0.4132349819723778, + "velocityX": -0.7768455236265391, + "velocityY": 0.7314774414104627, + "timestamp": 2.3918887961865263 + }, + { + "x": 1.3111795494907175, + "y": 5.570987556285165, + "heading": -0.011272655486454928, + "angularVelocity": 0.27546309541376424, + "velocityX": -0.517897698603385, + "velocityY": 0.4876535976460953, + "timestamp": 2.4737775062368823 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -7.670908759441255e-25, + "angularVelocity": 0.13765823737512747, + "velocityX": -0.2589502070873256, + "velocityY": 0.24382784200668048, + "timestamp": 2.5556662162872383 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -3.41171038639136e-25, + "angularVelocity": 8.93245678861437e-25, + "velocityX": 7.394213704779868e-19, + "velocityY": -2.5191717110574877e-20, + "timestamp": 2.6375549263375944 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.2.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.2.traj index d6a536f1..ce9a8fd4 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.2.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.2.traj @@ -1,229 +1,230 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.169901203643428e-30, - "velocityY": 4.744348570353821e-29, - "timestamp": 0 - }, - { - "x": 1.3279764392732873, - "y": 5.589659578477318, - "heading": -3.8286967101155775e-19, - "angularVelocity": -4.101142681562708e-18, - "velocityX": 0.40706315074964167, - "velocityY": -0.013868615044214471, - "timestamp": 0.0933564930640447 - }, - { - "x": 1.4039804148734387, - "y": 5.5870701279768555, - "heading": -4.0712541473530154e-19, - "angularVelocity": -2.5982712070316094e-19, - "velocityX": 0.814126292755424, - "velocityY": -0.027737229790526226, - "timestamp": 0.1867129861280894 - }, - { - "x": 1.517986376641016, - "y": 5.583185952281787, - "heading": 4.833998363566258e-19, - "angularVelocity": 9.538831821902836e-18, - "velocityX": 1.2211894216447965, - "velocityY": -0.04160584408996275, - "timestamp": 0.2800694791921341 - }, - { - "x": 1.6699943225350276, - "y": 5.578007051461648, - "heading": 1.3619244496265011e-18, - "angularVelocity": 9.410228943340493e-18, - "velocityX": 1.6282525286718303, - "velocityY": -0.05547445764455082, - "timestamp": 0.3734259722561788 - }, - { - "x": 1.8600042484729113, - "y": 5.571533425655531, - "heading": 3.711758987250246e-18, - "angularVelocity": 2.5170201581763002e-17, - "velocityX": 2.0353155919679775, - "velocityY": -0.06934306970923045, - "timestamp": 0.4667824653202235 - }, - { - "x": 2.0880161422023416, - "y": 5.563765075280871, - "heading": -9.287085834288012e-18, - "angularVelocity": -1.392382283532996e-16, - "velocityX": 2.4423785240217755, - "velocityY": -0.08321167730249175, - "timestamp": 0.5601389583842682 - }, - { - "x": 2.2780692584306133, - "y": 5.5572899779843725, - "heading": -5.547928961434018e-18, - "angularVelocity": 4.0052703333031833e-17, - "velocityX": 2.03577823010496, - "velocityY": -0.06935883176043627, - "timestamp": 0.6534954514483129 - }, - { - "x": 2.430120406839514, - "y": 5.552109605257364, - "heading": -3.2776580295073633e-18, - "angularVelocity": 2.4318713794052425e-17, - "velocityX": 1.6287152977531212, - "velocityY": -0.05549022415702071, - "timestamp": 0.7468519445123576 - }, - { - "x": 2.5441695751975866, - "y": 5.548223957516569, - "heading": -1.9211838299437728e-18, - "angularVelocity": 1.4530579976060665e-17, - "velocityX": 1.2216522343824612, - "velocityY": -0.04162161208980244, - "timestamp": 0.8402084375764023 - }, - { - "x": 2.6202167594261327, - "y": 5.545633034900951, - "heading": -1.8132356413213059e-19, - "angularVelocity": 1.8636270440618356e-17, - "velocityX": 0.8145891273223099, - "velocityY": -0.02775299853408606, - "timestamp": 0.933564930640447 - }, - { - "x": 2.6582619574855144, - "y": 5.5443368374799995, - "heading": 8.843883513334677e-20, - "angularVelocity": 2.8892814546014257e-18, - "velocityX": 0.40752599841430864, - "velocityY": -0.013884384234014852, - "timestamp": 1.0269214237044917 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 1.7598923552723236e-34, - "angularVelocity": -9.476131898054967e-19, - "velocityX": 0.0004628563966037854, - "velocityY": -0.000015769487296898133, - "timestamp": 1.1202779167685364 - }, - { - "x": 2.620331993851312, - "y": 5.545629108871589, - "heading": 2.945836613273263e-19, - "angularVelocity": 3.1551525697239727e-18, - "velocityX": -0.4066774370940851, - "velocityY": 0.013855473805082683, - "timestamp": 1.2136521018644193 - }, - { - "x": 2.5443424272440374, - "y": 5.548218068458602, - "heading": 2.309166242357916e-19, - "angularVelocity": -6.815922301609038e-19, - "velocityX": -0.8138177218425701, - "velocityY": 0.027726716799615945, - "timestamp": 1.3070262869603022 - }, - { - "x": 2.4303364695545335, - "y": 5.552102244014731, - "heading": -7.469284722560017e-19, - "angularVelocity": -1.047218864069531e-17, - "velocityX": -1.220957993477128, - "velocityY": 0.041597959347358555, - "timestamp": 1.4004004720561851 - }, - { - "x": 2.278314122823792, - "y": 5.55728163547044, - "heading": -1.7117540142209262e-18, - "angularVelocity": -1.0332835908519937e-17, - "velocityX": -1.628098243253484, - "velocityY": 0.055469201150393666, - "timestamp": 1.4937746571520676 - }, - { - "x": 2.0882753911343768, - "y": 5.563756242686637, - "heading": -3.8779782019319956e-18, - "angularVelocity": -2.3199468985970604e-17, - "velocityX": -2.035238449307225, - "velocityY": 0.06934044146380212, - "timestamp": 1.58714884224795 - }, - { - "x": 1.8602202867386202, - "y": 5.571526065245884, - "heading": -6.955816575429636e-18, - "angularVelocity": -3.296264638305206e-17, - "velocityX": -2.4423785241434, - "velocityY": 0.08321167730663664, - "timestamp": 1.6805230273438325 - }, - { - "x": 1.6701383566026529, - "y": 5.578002144230341, - "heading": -1.819668166700424e-18, - "angularVelocity": 5.5006116981871155e-17, - "velocityX": -2.035701087485777, - "velocityY": 0.0693562035164242, - "timestamp": 1.773897212439715 - }, - { - "x": 1.518072799200871, - "y": 5.5831830078708, - "heading": 4.234959937878501e-18, - "angularVelocity": 6.484281229150569e-17, - "velocityX": -1.6285610123513967, - "velocityY": 0.05548496766342984, - "timestamp": 1.8672713975355975 - }, - { - "x": 1.4040236267647241, - "y": 5.587068655750534, - "heading": 2.524941481520431e-18, - "angularVelocity": -1.831387557299539e-17, - "velocityX": -1.221420806223103, - "velocityY": 0.041613727347481366, - "timestamp": 1.96064558263148 - }, - { - "x": 1.327990843372909, - "y": 5.589659087730582, - "heading": 8.045224971714285e-19, - "angularVelocity": -1.842525382897137e-17, - "velocityX": -0.814280556413611, - "velocityY": 0.027742485543317336, - "timestamp": 2.0540197677273624 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 6.74699155537553e-28, - "angularVelocity": -8.616220285509517e-18, - "velocityX": -0.40714028476041414, - "velocityY": 0.013871242994939689, - "timestamp": 2.147393952823245 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -7.225786160817796e-27, - "velocityX": -2.979354413794432e-29, - "velocityY": 7.541641997510845e-28, - "timestamp": 2.2407681379191273 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -3.41171038639136e-25, + "angularVelocity": 8.93245678861437e-25, + "velocityX": 7.394213704779868e-19, + "velocityY": -2.5191717110574877e-20, + "timestamp": 0 + }, + { + "x": 1.3279764533229341, + "y": 5.589659577998647, + "heading": -1.0733184537140863e-18, + "angularVelocity": -1.1496982275828825e-17, + "velocityX": 0.4070632249719813, + "velocityY": -0.013868617572964747, + "timestamp": 0.09335651055308736 + }, + { + "x": 1.4039804570223795, + "y": 5.587070126540843, + "heading": -3.1026019980884896e-18, + "angularVelocity": -2.173692641491287e-17, + "velocityX": 0.8141264412001065, + "velocityY": -0.02773723484802693, + "timestamp": 0.18671302110617471 + }, + { + "x": 1.5179864609388978, + "y": 5.5831859494097635, + "heading": -5.833562477390286e-18, + "angularVelocity": -2.9253027311160176e-17, + "velocityX": 1.2211896443118286, + "velocityY": -0.041605851676214084, + "timestamp": 0.28006953165926207 + }, + { + "x": 1.669994463031499, + "y": 5.578007046674942, + "heading": -9.265282118286952e-18, + "angularVelocity": -3.6759297272081945e-17, + "velocityX": 1.628252825561226, + "velocityY": -0.05547446775955324, + "timestamp": 0.37342604221234943 + }, + { + "x": 1.8600044592176233, + "y": 5.571533418475472, + "heading": -1.3542122810335315e-17, + "angularVelocity": -4.5811917888604e-17, + "velocityX": 2.035315963079774, + "velocityY": -0.0693430823529852, + "timestamp": 0.4667825527654368 + }, + { + "x": 2.08801643724496, + "y": 5.563765065228789, + "heading": -1.806715455043103e-17, + "angularVelocity": -4.8470447549846784e-17, + "velocityX": 2.442378969356152, + "velocityY": -0.08321169247500493, + "timestamp": 0.5601390633185241 + }, + { + "x": 2.278069539423553, + "y": 5.557289968410962, + "heading": -2.8931134060980015e-18, + "angularVelocity": 1.6253865304001022e-16, + "velocityX": 2.0357776983375815, + "velocityY": -0.06935881364315644, + "timestamp": 0.6534955738716115 + }, + { + "x": 2.430120659733151, + "y": 5.552109596641294, + "heading": -3.268581465491534e-18, + "angularVelocity": -4.0218733262966905e-18, + "velocityX": 1.6287146917636088, + "velocityY": -0.05549020351099765, + "timestamp": 0.7468520844246989 + }, + { + "x": 2.5441697859422803, + "y": 5.548223950336512, + "heading": -3.490008764973647e-18, + "angularVelocity": -2.371846303430449e-18, + "velocityX": 1.2216515541706623, + "velocityY": -0.04162158891503094, + "timestamp": 0.8402085949777862 + }, + { + "x": 2.62021691397224, + "y": 5.545633029635576, + "heading": -3.003302345401883e-18, + "angularVelocity": 5.213417288227865e-18, + "velocityX": 0.8145883728881999, + "velocityY": -0.027752972830565246, + "timestamp": 0.9335651055308736 + }, + { + "x": 2.6582620417833907, + "y": 5.544336834607976, + "heading": -2.0773852345449385e-18, + "angularVelocity": 9.91807795339057e-18, + "velocityX": 0.4075251697578793, + "velocityY": -0.013884356001744477, + "timestamp": 1.026921616083961 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 6.735492092706879e-29, + "angularVelocity": 2.2252173720073512e-17, + "velocityX": 0.00046195351785172767, + "velocityY": -0.00001573872627686245, + "timestamp": 1.1202781266370483 + }, + { + "x": 2.6203319236037554, + "y": 5.545629111264917, + "heading": 2.4136302284603065e-18, + "angularVelocity": 2.584901393114583e-17, + "velocityX": -0.40667826371536864, + "velocityY": 0.013855501968015705, + "timestamp": 1.2136522947107977 + }, + { + "x": 2.544342300798434, + "y": 5.548218072766594, + "heading": 5.5338149846267035e-18, + "angularVelocity": 3.341593018331809e-17, + "velocityX": -0.8138184722063877, + "velocityY": 0.02772674236446206, + "timestamp": 1.3070264627845471 + }, + { + "x": 2.4303363009603927, + "y": 5.552102249758721, + "heading": 9.360515374591917e-18, + "angularVelocity": 4.0982429991846804e-17, + "velocityX": -1.220958667583486, + "velocityY": 0.04159798231411795, + "timestamp": 1.4004006308582961 + }, + { + "x": 2.278313926130621, + "y": 5.557281642171763, + "heading": 1.3522883806487307e-17, + "angularVelocity": 4.457730047824434e-17, + "velocityX": -1.6280988411023956, + "velocityY": 0.055469221519066796, + "timestamp": 1.4937747989320456 + }, + { + "x": 2.088275180391678, + "y": 5.563756249866626, + "heading": 1.8245422432915976e-17, + "angularVelocity": 5.057649913145605e-17, + "velocityX": -2.03523897089873, + "velocityY": 0.06934045923439035, + "timestamp": 1.587148967005795 + }, + { + "x": 1.860220075995873, + "y": 5.571526072425875, + "heading": 6.8469520838322655e-18, + "angularVelocity": -1.2207305633045037e-16, + "velocityX": -2.4423789694777343, + "velocityY": 0.08321169247914799, + "timestamp": 1.6805231350795444 + }, + { + "x": 1.6701382161075125, + "y": 5.578002149017001, + "heading": -6.786620946288625e-18, + "angularVelocity": -1.460101118846944e-16, + "velocityX": -2.035700706197767, + "velocityY": 0.06935619052596638, + "timestamp": 1.7738973031532939 + }, + { + "x": 1.5180727149037914, + "y": 5.583183010742795, + "heading": -4.3005972299368415e-18, + "angularVelocity": 2.6624319168550005e-17, + "velocityX": -1.6285607073212787, + "velocityY": 0.05548495727107347, + "timestamp": 1.8672714712270433 + }, + { + "x": 1.4040235846161857, + "y": 5.5870686571865305, + "heading": -1.9657241654870744e-18, + "angularVelocity": 2.5005556186566034e-17, + "velocityX": -1.221420577450572, + "velocityY": 0.04161371955321608, + "timestamp": 1.9606456393007927 + }, + { + "x": 1.3279908293233964, + "y": 5.589659088209248, + "heading": -7.083875411773879e-19, + "angularVelocity": 1.3465572054358342e-17, + "velocityX": -0.8142804038986078, + "velocityY": 0.027742480347141066, + "timestamp": 2.054019807374542 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -9.307960730995794e-22, + "angularVelocity": 7.576578596147462e-18, + "velocityX": -0.40714020850291777, + "velocityY": 0.013871240396851729, + "timestamp": 2.1473939754482916 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -2.878097861680603e-22, + "angularVelocity": 6.886148795301622e-21, + "velocityX": 9.386048457152016e-19, + "velocityY": 1.5189228236971111e-19, + "timestamp": 2.240768143522041 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.3.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.3.traj index 0cf979fd..2b355ec2 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.3.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.3.traj @@ -1,337 +1,338 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -7.225786160817796e-27, - "velocityX": -2.979354413794432e-29, - "velocityY": 7.541641997510845e-28, - "timestamp": 0 - }, - { - "x": 1.3078091735186845, - "y": 5.606755773559667, - "heading": 0.009999668662888749, - "angularVelocity": 0.1349541802858926, - "velocityX": 0.24069501005494717, - "velocityY": 0.21325450657673967, - "timestamp": 0.07409676856345904 - }, - { - "x": 1.3434786990122702, - "y": 5.638358364996936, - "heading": 0.029996210094567113, - "angularVelocity": 0.2698706275619814, - "velocityX": 0.48139110769232624, - "velocityY": 0.42650431390462895, - "timestamp": 0.14819353712691807 - }, - { - "x": 1.3969833303762573, - "y": 5.6857616519517675, - "heading": 0.05997803368561842, - "angularVelocity": 0.4046306495359042, - "velocityX": 0.7220912922884387, - "velocityY": 0.6397483706242104, - "timestamp": 0.2222903056903771 - }, - { - "x": 1.468323578276302, - "y": 5.748965089703653, - "heading": 0.09992693775422122, - "angularVelocity": 0.5391450240657001, - "velocityX": 0.9627983687594002, - "velocityY": 0.8529850758692316, - "timestamp": 0.29638707425383615 - }, - { - "x": 1.5575001323238937, - "y": 5.827967973754339, - "heading": 0.14982100646683655, - "angularVelocity": 0.6733636254605087, - "velocityX": 1.203514752134039, - "velocityY": 1.0662122733008166, - "timestamp": 0.3704838428172952 - }, - { - "x": 1.6645138155479717, - "y": 5.922769410023865, - "heading": 0.20963796260623796, - "angularVelocity": 0.8072815765348765, - "velocityX": 1.444242242996271, - "velocityY": 1.279427404349801, - "timestamp": 0.4445806113807542 - }, - { - "x": 1.789365525357909, - "y": 6.033368309242042, - "heading": 0.27935847839394334, - "angularVelocity": 0.9409386825166515, - "velocityX": 1.6849818451743819, - "velocityY": 1.4926278347551774, - "timestamp": 0.5186773799442133 - }, - { - "x": 1.9320561732696155, - "y": 6.159763414266322, - "heading": 0.35896870362448313, - "angularVelocity": 1.074408867993172, - "velocityX": 1.925733749048249, - "velocityY": 1.7058112989342795, - "timestamp": 0.5927741485076723 - }, - { - "x": 2.092586653477802, - "y": 6.301953347709479, - "heading": 0.44846110031344777, - "angularVelocity": 1.2077773218503376, - "velocityX": 2.1664977208531133, - "velocityY": 1.9189761740812865, - "timestamp": 0.6668709170711313 - }, - { - "x": 2.2353547101625333, - "y": 6.428313752108457, - "heading": 0.5276057235865483, - "angularVelocity": 1.068125166619866, - "velocityX": 1.9267784472209708, - "velocityY": 1.7053429837972895, - "timestamp": 0.7409676856345904 - }, - { - "x": 2.360281889838346, - "y": 6.538881119368732, - "heading": 0.5968684037316069, - "angularVelocity": 0.9347597942868423, - "velocityX": 1.686000376116786, - "velocityY": 1.4922022835269577, - "timestamp": 0.8150644541980494 - }, - { - "x": 2.4673676751793194, - "y": 6.633656126085495, - "heading": 0.6562491552260078, - "angularVelocity": 0.8013946173366472, - "velocityX": 1.4452153232519684, - "velocityY": 1.279070714493898, - "timestamp": 0.8891612227615084 - }, - { - "x": 2.5566116515465542, - "y": 6.712639287159663, - "heading": 0.7057476703680625, - "angularVelocity": 0.668025287685273, - "velocityX": 1.2044246744765545, - "velocityY": 1.0659460944941062, - "timestamp": 0.9632579913249675 - }, - { - "x": 2.6280134773537864, - "y": 6.7758309931109295, - "heading": 0.7453637127245453, - "angularVelocity": 0.5346527671626246, - "velocityX": 0.9636294156990158, - "velocityY": 0.8528267448954308, - "timestamp": 1.0373547598884265 - }, - { - "x": 2.6815728846604703, - "y": 6.823231527897903, - "heading": 0.7750971751130776, - "angularVelocity": 0.4012788001208449, - "velocityX": 0.7228305408320154, - "velocityY": 0.6397112275638166, - "timestamp": 1.1114515284518856 - }, - { - "x": 2.7172896860156874, - "y": 6.854841077120622, - "heading": 0.7949480902508361, - "angularVelocity": 0.26790527462553493, - "velocityX": 0.4820291361527109, - "velocityY": 0.42659821506532714, - "timestamp": 1.1855482970153446 - }, - { - "x": 2.7351637810076403, - "y": 6.870659730936303, - "heading": 0.8049166539682622, - "angularVelocity": 0.13453439203137166, - "velocityX": 0.2412263763293308, - "velocityY": 0.2134864193164304, - "timestamp": 1.2596450655788036 - }, - { - "x": 2.73519515991211, - "y": 6.870687484741211, - "heading": 0.8050032485420815, - "angularVelocity": 0.0011686685922978992, - "velocityX": 0.00042348526922062564, - "velocityY": 0.00037456143614395893, - "timestamp": 1.3337418341422627 - }, - { - "x": 2.71737666825008, - "y": 6.854917835875481, - "heading": 0.7952044521096777, - "angularVelocity": -0.13221640762197384, - "velocityX": -0.2404271762292292, - "velocityY": -0.21278187956754013, - "timestamp": 1.4078536376738002 - }, - { - "x": 2.6817084896089662, - "y": 6.823350601971179, - "heading": 0.775519887263024, - "angularVelocity": -0.2656063393700981, - "velocityX": -0.4812752750383339, - "velocityY": -0.4259407056452727, - "timestamp": 1.4819654412053378 - }, - { - "x": 2.628190888379515, - "y": 6.7759855045171085, - "heading": 0.7459495915696174, - "angularVelocity": -0.398995764215558, - "velocityX": -0.7221198063894422, - "velocityY": -0.6391032897100266, - "timestamp": 1.5560772447368754 - }, - { - "x": 2.5568241932220688, - "y": 6.712822154407591, - "heading": 0.7064940900377142, - "angularVelocity": -0.532378105058251, - "velocityX": -0.9629599030846991, - "velocityY": -0.8522711240272193, - "timestamp": 1.630189048268413 - }, - { - "x": 2.467608789693866, - "y": 6.633860035088962, - "heading": 0.657154460511053, - "angularVelocity": -0.6657459024188197, - "velocityX": -1.2037947974070313, - "velocityY": -1.0654459282687603, - "timestamp": 1.7043008517999505 - }, - { - "x": 2.3605451226537615, - "y": 6.539098475645005, - "heading": 0.597932194997477, - "angularVelocity": -0.7990935679285951, - "velocityX": -1.4446236891921902, - "velocityY": -1.2786297852682258, - "timestamp": 1.778412655331488 - }, - { - "x": 2.2356337183339816, - "y": 6.428536615850716, - "heading": 0.528828452368811, - "angularVelocity": -0.9324255966349123, - "velocityX": -1.685445480458886, - "velocityY": -1.4918252493931579, - "timestamp": 1.8525244588630256 - }, - { - "x": 2.092875237629583, - "y": 6.30217337339218, - "heading": 0.4498420161681241, - "angularVelocity": -1.0657740391526649, - "velocityX": -1.9262583541843843, - "velocityY": -1.7050353173174484, - "timestamp": 1.9266362623945632 - }, - { - "x": 1.9322705754719112, - "y": 6.16000743597858, - "heading": 0.3609649957464903, - "angularVelocity": -1.1992289512105054, - "velocityX": -2.167059152487097, - "velocityY": -1.9182630921487032, - "timestamp": 2.0007480659261008 - }, - { - "x": 1.789533871896463, - "y": 6.033554554877999, - "heading": 0.28090803782331497, - "angularVelocity": -1.0802187250553423, - "velocityX": -1.9259645128083558, - "velocityY": -1.7062448231010843, - "timestamp": 2.0748598694576383 - }, - { - "x": 1.6646414904257845, - "y": 5.922906567233019, - "heading": 0.2107799682947157, - "angularVelocity": -0.9462469699444712, - "velocityX": -1.6851888029309197, - "velocityY": -1.4929873836216447, - "timestamp": 2.148971672989176 - }, - { - "x": 1.5575924667954766, - "y": 5.828064283268442, - "heading": 0.15061002754742392, - "angularVelocity": -0.8118806705635195, - "velocityX": -1.4444261039959587, - "velocityY": -1.2797190116759578, - "timestamp": 2.2230834765207135 - }, - { - "x": 1.4683859542310973, - "y": 5.7490283016453585, - "heading": 0.10042779042808991, - "angularVelocity": -0.6771153140084615, - "velocityX": -1.203674830696421, - "velocityY": -1.066442561920003, - "timestamp": 2.297195280052251 - }, - { - "x": 1.3970212709065528, - "y": 5.685799049936121, - "heading": 0.06025974538395147, - "angularVelocity": -0.5419925454610606, - "velocityX": -0.9629327573631904, - "velocityY": -0.8531603428758896, - "timestamp": 2.3713070835837886 - }, - { - "x": 1.343497933013845, - "y": 5.638376829349959, - "heading": 0.03012622810608221, - "angularVelocity": -0.40659538484416025, - "velocityX": -0.7221972121128929, - "velocityY": -0.639874059577299, - "timestamp": 2.445418887115326 - }, - { - "x": 1.3078156736079332, - "y": 5.606761857880495, - "heading": 0.010039060501896319, - "angularVelocity": -0.2710387097248632, - "velocityX": -0.4814652687860333, - "velocityY": -0.4265848348630388, - "timestamp": 2.5195306906468637 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -5.584258539451151e-28, - "angularVelocity": -0.1354583213971952, - "velocityX": -0.24073388709378457, - "velocityY": -0.21329334041956155, - "timestamp": 2.5936424941784013 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 7.53493083126559e-27, - "velocityX": -3.871179527516382e-30, - "velocityY": -3.125557960458497e-29, - "timestamp": 2.667754297709939 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -2.878097861680603e-22, + "angularVelocity": 6.886148795301622e-21, + "velocityX": 9.386048457152016e-19, + "velocityY": 1.5189228236971111e-19, + "timestamp": 0 + }, + { + "x": 1.3078091854990117, + "y": 5.606755784251363, + "heading": 0.009999675661372767, + "angularVelocity": 0.13495422890571762, + "velocityX": 0.24069508999867756, + "velocityY": 0.21325457844806686, + "timestamp": 0.07409679372373557 + }, + { + "x": 1.3434787349490442, + "y": 5.638358397074998, + "heading": 0.0299962311234221, + "angularVelocity": 0.26987072526518463, + "velocityX": 0.48139126752263756, + "velocityY": 0.426504457688991, + "timestamp": 0.14819358744747113 + }, + { + "x": 1.3969834022405148, + "y": 5.685761716114703, + "heading": 0.0599780758079259, + "angularVelocity": 0.40463079679653674, + "velocityX": 0.7220915319353576, + "velocityY": 0.6397485863753444, + "timestamp": 0.2222903811712067 + }, + { + "x": 1.4683236980324899, + "y": 5.748965196655092, + "heading": 0.09992700807120888, + "angularVelocity": 0.5391452214819116, + "velocityX": 0.9627986881316648, + "velocityY": 0.8529853636587607, + "timestamp": 0.29638717489494226 + }, + { + "x": 1.5575003119272133, + "y": 5.82796813420515, + "heading": 0.14982111213912733, + "angularVelocity": 0.673363873934206, + "velocityX": 1.2035151511037259, + "velocityY": 1.066212633229638, + "timestamp": 0.37048396861867783 + }, + { + "x": 1.6645140669394536, + "y": 5.922769634696042, + "heading": 0.20963811089831047, + "angularVelocity": 0.8072818775695793, + "velocityX": 1.444242721368388, + "velocityY": 1.2794278365721479, + "timestamp": 0.4445807623424134 + }, + { + "x": 1.7893658604543141, + "y": 6.033368608876843, + "heading": 0.27935867676309706, + "angularVelocity": 0.9409390388028722, + "velocityX": 1.684982402617327, + "velocityY": 1.4926283395359905, + "timestamp": 0.518677556066149 + }, + { + "x": 1.9320566039380949, + "y": 6.1597637996451535, + "heading": 0.35896895992945943, + "angularVelocity": 1.074409285011486, + "velocityX": 1.925734384888401, + "velocityY": 1.705811876821096, + "timestamp": 0.5927743497898845 + }, + { + "x": 2.0925871914348457, + "y": 6.3019538297380615, + "heading": 0.4484614236084208, + "angularVelocity": 1.207777815766594, + "velocityX": 2.1664984330533477, + "velocityY": 1.9189768267578884, + "timestamp": 0.6668711435136201 + }, + { + "x": 2.235355236977834, + "y": 6.4283142227315375, + "heading": 0.5276060335900633, + "angularVelocity": 1.0681246246190785, + "velocityX": 1.926777642704601, + "velocityY": 1.7053422509022635, + "timestamp": 0.7409679372373557 + }, + { + "x": 2.3602823931901473, + "y": 6.538881568185102, + "heading": 0.5968686960571826, + "angularVelocity": 0.9347592383735241, + "velocityX": 1.685999487077598, + "velocityY": 1.4922014826418244, + "timestamp": 0.8150647309610912 + }, + { + "x": 2.467368142896788, + "y": 6.633656542567922, + "heading": 0.6562494243147645, + "angularVelocity": 0.8013940316902027, + "velocityX": 1.4452143517289322, + "velocityY": 1.2790698439150068, + "timestamp": 0.8891615246848268 + }, + { + "x": 2.556612071509118, + "y": 6.712639660738819, + "heading": 0.7057479102710464, + "angularVelocity": 0.6680246670433977, + "velocityX": 1.2044236211497805, + "velocityY": 1.0659451536510254, + "timestamp": 0.9632583184085624 + }, + { + "x": 2.62801383746604, + "y": 6.7758313131963215, + "heading": 0.7453639172984617, + "angularVelocity": 0.5346521089039379, + "velocityX": 0.9636282809096683, + "velocityY": 0.8528257335008133, + "timestamp": 1.037355112132298 + }, + { + "x": 2.681573172842232, + "y": 6.823231783886196, + "heading": 0.7750973380985307, + "angularVelocity": 0.4012781026791483, + "velocityX": 0.7228293247867619, + "velocityY": 0.6397101454430356, + "timestamp": 1.1114519058560335 + }, + { + "x": 2.7172898901971134, + "y": 6.854841258399732, + "heading": 0.7949482053115821, + "angularVelocity": 0.2679045369636881, + "velocityX": 0.48202783899191026, + "velocityY": 0.42659706209947756, + "timestamp": 1.185548699579769 + }, + { + "x": 2.735163889126497, + "y": 6.870659826887679, + "heading": 0.8049167147138492, + "angularVelocity": 0.1345336134169838, + "velocityX": 0.24122499815612897, + "velocityY": 0.2134851954178351, + "timestamp": 1.2596454933035046 + }, + { + "x": 2.73519515991211, + "y": 6.870687484741211, + "heading": 0.8050032485420815, + "angularVelocity": 0.001167848484172869, + "velocityX": 0.00042202616390890065, + "velocityY": 0.0003732665361331451, + "timestamp": 1.3337422870272402 + }, + { + "x": 2.717376572124918, + "y": 6.854917750576811, + "heading": 0.7952043981716957, + "angularVelocity": -0.13221717973766972, + "velocityX": -0.24042855386900916, + "velocityY": -0.2127831018556256, + "timestamp": 1.4078540657675482 + }, + { + "x": 2.6817083093652894, + "y": 6.823350442039261, + "heading": 0.7755197862739446, + "angularVelocity": -0.26560706317314603, + "velocityX": -0.4812765712264476, + "velocityY": -0.42594185531780887, + "timestamp": 1.4819658445078563 + }, + { + "x": 2.6281906360223433, + "y": 6.775985280617999, + "heading": 0.7459494504462719, + "angularVelocity": -0.39899643930135414, + "velocityX": -0.722121021146653, + "velocityY": -0.6391043667597266, + "timestamp": 1.5560776232481643 + }, + { + "x": 2.5568238807540884, + "y": 6.712821877208373, + "heading": 0.7064939157358233, + "angularVelocity": -0.5323787308992164, + "velocityX": -0.9629610364410259, + "velocityY": -0.8522721284420127, + "timestamp": 1.6301894019884724 + }, + { + "x": 2.4676084291144194, + "y": 6.633859715258306, + "heading": 0.6571542600389383, + "angularVelocity": -0.6657464782996794, + "velocityX": -1.203795849405873, + "velocityY": -1.0654468600295754, + "timestamp": 1.7043011807287805 + }, + { + "x": 2.360544725957208, + "y": 6.5390981238541, + "heading": 0.5979319754390301, + "angularVelocity": -0.7990940928219623, + "velocityX": -1.4446246598987866, + "velocityY": -1.2786306443440876, + "timestamp": 1.7784129594690885 + }, + { + "x": 2.235633297506525, + "y": 6.4285362427751105, + "heading": 0.528828220928822, + "angularVelocity": -0.9324260689026471, + "velocityX": -1.6854463699809454, + "velocityY": -1.4918260357291568, + "timestamp": 1.8525247382093966 + }, + { + "x": 2.092874804641344, + "y": 6.302172989716305, + "heading": 0.44984178028878635, + "angularVelocity": -1.0657744555937454, + "velocityX": -1.9262591627360037, + "velocityY": -1.7050360307986958, + "timestamp": 1.9266365169497046 + }, + { + "x": 1.9322701422450308, + "y": 6.160007052413367, + "heading": 0.360964763590885, + "angularVelocity": -1.1992293021239147, + "velocityX": -2.1670598807118275, + "velocityY": -1.9182637324236342, + "timestamp": 2.0007482956900127 + }, + { + "x": 1.7895335350373793, + "y": 6.033554256495722, + "heading": 0.28090785571409216, + "angularVelocity": -1.0802184111289128, + "velocityX": -1.9259638566739734, + "velocityY": -1.7062442443966088, + "timestamp": 2.0748600744303207 + }, + { + "x": 1.6646412378227142, + "y": 5.922906343422477, + "heading": 0.21077983102217354, + "angularVelocity": -0.9462466814843488, + "velocityX": -1.6851882296914678, + "velocityY": -1.4929868767684242, + "timestamp": 2.1489718531706288 + }, + { + "x": 1.5575922863855105, + "y": 5.828064123391515, + "heading": 0.15060992914364188, + "angularVelocity": -0.8118804176778789, + "velocityX": -1.4444256129961441, + "velocityY": -1.279718577033419, + "timestamp": 2.223083631910937 + }, + { + "x": 1.468385833968275, + "y": 5.749028195054374, + "heading": 0.1004277246519939, + "angularVelocity": -0.67711510025268, + "velocityX": -1.2036744217112965, + "velocityY": -1.0664421996142908, + "timestamp": 2.297195410651245 + }, + { + "x": 1.397021198753748, + "y": 5.685798985978408, + "heading": 0.06025970584312968, + "angularVelocity": -0.5419923727591991, + "velocityX": -0.9629324302766086, + "velocityY": -0.853160052972471, + "timestamp": 2.371307189391553 + }, + { + "x": 1.3434978969393505, + "y": 5.638376797369784, + "heading": 0.030126208310740164, + "angularVelocity": -0.4065952544193969, + "velocityX": -0.7221969668538984, + "velocityY": -0.6398738421161722, + "timestamp": 2.445418968131861 + }, + { + "x": 1.307815661583592, + "y": 5.6067618472200556, + "heading": 0.010039053898985358, + "angularVelocity": -0.2710386223779815, + "velocityX": -0.4814651053079036, + "velocityY": -0.4265846898710802, + "timestamp": 2.519530746872169 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.285773257756444e-22, + "angularVelocity": -0.13545827761283144, + "velocityX": -0.24073380536507133, + "velocityY": -0.21329326791617506, + "timestamp": 2.593642525612477 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 2.570159716602358e-22, + "angularVelocity": 4.6040759154316275e-21, + "velocityX": 1.4250190224081253e-19, + "velocityY": 1.259493468984856e-19, + "timestamp": 2.667754304352785 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.4.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.4.traj index 5eac92d4..efccfdea 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.4.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.4.traj @@ -1,985 +1,878 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 7.53493083126559e-27, - "velocityX": -3.871179527516382e-30, - "velocityY": -3.125557960458497e-29, - "timestamp": 0 - }, - { - "x": 1.310726436274015, - "y": 5.58769762128994, - "heading": -1.5579970000423894e-18, - "angularVelocity": -2.2453280466561467e-17, - "velocityX": 0.299069985980355, - "velocityY": -0.04693411089639644, - "timestamp": 0.06938839128549112 - }, - { - "x": 1.3522304064004054, - "y": 5.581184256432646, - "heading": -2.2539666819865276e-18, - "angularVelocity": -1.003005957705595e-17, - "velocityX": 0.598139967759544, - "velocityY": -0.09386822113363098, - "timestamp": 0.13877678257098225 - }, - { - "x": 1.4144863610944205, - "y": 5.571414209224462, - "heading": -1.1138395195686094e-18, - "angularVelocity": 1.6431093577313864e-17, - "velocityX": 0.8972099444973367, - "velocityY": -0.1408023305798717, - "timestamp": 0.20816517385647337 - }, - { - "x": 1.49749429992851, - "y": 5.558387479732467, - "heading": 1.8047013110476196e-18, - "angularVelocity": 4.206093786115289e-17, - "velocityX": 1.1962799150734271, - "velocityY": -0.18773643905934295, - "timestamp": 0.2775535651419645 - }, - { - "x": 1.6012542223682356, - "y": 5.5421040680405165, - "heading": 7.934655780307681e-18, - "angularVelocity": 8.834265128972616e-17, - "velocityX": 1.4953498779473966, - "velocityY": -0.23467054633035336, - "timestamp": 0.3469419564274556 - }, - { - "x": 1.725766127726464, - "y": 5.522563974256421, - "heading": 1.6517348693937324e-17, - "angularVelocity": 1.236906166709818e-16, - "velocityX": 1.7944198309186505, - "velocityY": -0.28160465204763024, - "timestamp": 0.41633034771294675 - }, - { - "x": 1.8710300150870187, - "y": 5.499767198523928, - "heading": 2.797873673898563e-17, - "angularVelocity": 1.6517731335259598e-16, - "velocityX": 2.0934897706863036, - "velocityY": -0.3285387556932655, - "timestamp": 0.48571873899843876 - }, - { - "x": 2.0370458831672553, - "y": 5.4737137410442855, - "heading": 4.626535556608686e-17, - "angularVelocity": 2.635400319271784e-16, - "velocityX": 2.3925596919689536, - "velocityY": -0.3754728564386086, - "timestamp": 0.5551071302839299 - }, - { - "x": 2.223813730043212, - "y": 5.444403602119362, - "heading": 6.626560991799023e-17, - "angularVelocity": 2.882363175884601e-16, - "velocityX": 2.691629585524184, - "velocityY": -0.42240695283352697, - "timestamp": 0.624495521569421 - }, - { - "x": 2.4313335525083035, - "y": 5.411836782252268, - "heading": 9.137945639838438e-17, - "angularVelocity": 3.6193152862799336e-16, - "velocityX": 2.99069943286728, - "velocityY": -0.46934104197777365, - "timestamp": 0.6938839128549121 - }, - { - "x": 2.65960534414942, - "y": 5.376013282449221, - "heading": 1.1821641145167503e-16, - "angularVelocity": 3.8676433675745036e-16, - "velocityX": 3.2897691877869866, - "velocityY": -0.5162751166208155, - "timestamp": 0.7632723041404033 - }, - { - "x": 2.9086290857277297, - "y": 5.336933105728788, - "heading": 1.4538386371763974e-16, - "angularVelocity": 3.915273420502118e-16, - "velocityX": 3.5888386654437228, - "velocityY": -0.5632091477613738, - "timestamp": 0.8326606954258944 - }, - { - "x": 3.1678798362557536, - "y": 5.296247694105415, - "heading": 1.2970532145326214e-16, - "angularVelocity": -2.259533930819528e-16, - "velocityX": 3.73622656074219, - "velocityY": -0.5863432033749367, - "timestamp": 0.9020490867113855 - }, - { - "x": 3.426192624956805, - "y": 5.24997937308727, - "heading": 1.1963113847649017e-16, - "angularVelocity": -1.451854235288838e-16, - "velocityX": 3.7227089995248686, - "velocityY": -0.6668020422577368, - "timestamp": 0.9714374779968766 - }, - { - "x": 3.6799776944966163, - "y": 5.18320068927956, - "heading": 1.442058714433213e-16, - "angularVelocity": 3.5416202208408226e-16, - "velocityX": 3.657457174581828, - "velocityY": -0.9623898547086362, - "timestamp": 1.0408258692823678 - }, - { - "x": 3.9276087380924305, - "y": 5.096338039673236, - "heading": 1.2678863352315073e-16, - "angularVelocity": -2.510108333324681e-16, - "velocityX": 3.568767613835609, - "velocityY": -1.251832590395956, - "timestamp": 1.1102142605678589 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 9.872947837544061e-17, - "angularVelocity": -4.0437823428221097e-16, - "velocityX": 3.457211254829872, - "velocityY": -1.5332571030157462, - "timestamp": 1.17960265185335 - }, - { - "x": 4.284983330328794, - "y": 4.932123965200671, - "heading": 8.520616401533837e-17, - "angularVelocity": -3.905855769726077e-16, - "velocityX": 3.3932258158637794, - "velocityY": -1.6700901615188843, - "timestamp": 1.2142258308885827 - }, - { - "x": 4.400064822274762, - "y": 4.86965478591225, - "heading": 8.38275920998652e-17, - "angularVelocity": -3.9816445337684444e-17, - "velocityX": 3.3238280005674867, - "velocityY": -1.8042589106232223, - "timestamp": 1.2488490099238154 - }, - { - "x": 4.512559967686789, - "y": 4.802639932227702, - "heading": 8.775922770148411e-17, - "angularVelocity": 1.1355501462237473e-16, - "velocityX": 3.2491281432462653, - "velocityY": -1.935548830347224, - "timestamp": 1.2834721889590481 - }, - { - "x": 4.622289335106214, - "y": 4.731186349760053, - "heading": 1.0501758472421099e-16, - "angularVelocity": 4.984625185678289e-16, - "velocityX": 3.169245877386488, - "velocityY": -2.06374990566114, - "timestamp": 1.3180953679942808 - }, - { - "x": 4.729077970033436, - "y": 4.65540814642046, - "heading": 1.123300173637084e-16, - "angularVelocity": 2.11200497564256e-16, - "velocityX": 3.0843105082451987, - "velocityY": -2.188655272309947, - "timestamp": 1.3527185470295136 - }, - { - "x": 4.83750746703025, - "y": 4.5819970246859665, - "heading": 1.0947895650457139e-16, - "angularVelocity": -8.234543847737888e-17, - "velocityX": 3.131702518895681, - "velocityY": -2.120288309163928, - "timestamp": 1.3873417260647463 - }, - { - "x": 4.94878188699609, - "y": 4.512974188419722, - "heading": 1.0086901564782932e-16, - "angularVelocity": -2.486756299293195e-16, - "velocityX": 3.213870680465465, - "velocityY": -1.9935441571094976, - "timestamp": 1.421964905099979 - }, - { - "x": 5.062723794471585, - "y": 4.448449851931572, - "heading": 9.759628790443595e-17, - "angularVelocity": -9.452418393074315e-17, - "velocityX": 3.2909140827174443, - "velocityY": -1.8636167528836842, - "timestamp": 1.4565880841352117 - }, - { - "x": 5.179151445124142, - "y": 4.388526983063662, - "heading": 9.598506413460085e-17, - "angularVelocity": -4.6535985854895585e-17, - "velocityX": 3.362708275114807, - "velocityY": -1.7307153917591276, - "timestamp": 1.4912112631704444 - }, - { - "x": 5.297879115709132, - "y": 4.333301191879142, - "heading": 9.429439507749846e-17, - "angularVelocity": -4.883055525842942e-17, - "velocityX": 3.429138337186562, - "velocityY": -1.5950525839444993, - "timestamp": 1.525834442205677 - }, - { - "x": 5.418717408418424, - "y": 4.28286058857882, - "heading": 9.044194331783773e-17, - "angularVelocity": -1.112679963830143e-16, - "velocityX": 3.4900981387736523, - "velocityY": -1.4568449433541117, - "timestamp": 1.5604576212409098 - }, - { - "x": 5.54147355571485, - "y": 4.237285646363086, - "heading": 8.606851716017984e-17, - "angularVelocity": -1.2631497971943828e-16, - "velocityX": 3.545490354063358, - "velocityY": -1.3163130447772213, - "timestamp": 1.5950808002761425 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 8.12244185045661e-17, - "angularVelocity": -1.3990912419348855e-16, - "velocityX": 3.5952265671295462, - "velocityY": -1.1736811275270398, - "timestamp": 1.6297039793113752 - }, - { - "x": 5.800884886979515, - "y": 4.158897293130728, - "heading": 7.604329686230843e-17, - "angularVelocity": -1.3984796334066921e-16, - "velocityX": 3.642093094613744, - "velocityY": -1.0189897302407367, - "timestamp": 1.6667522245520763 - }, - { - "x": 5.9373078989205395, - "y": 4.126945510909642, - "heading": 7.099587364491845e-17, - "angularVelocity": -1.3623919795923125e-16, - "velocityX": 3.682306977150727, - "velocityY": -0.8624371279529385, - "timestamp": 1.7038004697927773 - }, - { - "x": 6.074971576496844, - "y": 4.100852086793861, - "heading": 6.753360793468667e-17, - "angularVelocity": -9.345289332160286e-17, - "velocityX": 3.7157948151635507, - "velocityY": -0.7043093119863827, - "timestamp": 1.7408487150334784 - }, - { - "x": 6.213624465766887, - "y": 4.080664678033529, - "heading": 6.709732769415932e-17, - "angularVelocity": -1.177600282260163e-17, - "velocityX": 3.742495450708113, - "velocityY": -0.5448951395450716, - "timestamp": 1.7778969602741794 - }, - { - "x": 6.353013304056433, - "y": 4.066420153436822, - "heading": 4.721163336657454e-17, - "angularVelocity": -5.367513143575007e-16, - "velocityX": 3.7623600627760427, - "velocityY": -0.38448581043883095, - "timestamp": 1.8149452055148805 - }, - { - "x": 6.4925719820191485, - "y": 4.0581614935185435, - "heading": 3.328638124108273e-17, - "angularVelocity": -3.758680611067538e-16, - "velocityX": 3.766944346648758, - "velocityY": -0.22291635851097177, - "timestamp": 1.8519934507555815 - }, - { - "x": 6.627754792512696, - "y": 4.053990782696842, - "heading": 2.356760119630702e-17, - "angularVelocity": -2.6232767457636995e-16, - "velocityX": 3.6488316684169346, - "velocityY": -0.1125751245330268, - "timestamp": 1.8890416959962826 - }, - { - "x": 6.757419046645761, - "y": 4.0521449831028535, - "heading": 1.6377013350687517e-17, - "angularVelocity": -1.940871361679193e-16, - "velocityX": 3.4998757239551144, - "velocityY": -0.049821511977060404, - "timestamp": 1.9260899412369836 - }, - { - "x": 6.881278674219902, - "y": 4.051770935128315, - "heading": 1.0749401519445311e-17, - "angularVelocity": -1.51899550746631e-16, - "velocityX": 3.3431982208450166, - "velocityY": -0.010096239973268065, - "timestamp": 1.9631381864776847 - }, - { - "x": 6.999234961376727, - "y": 4.052402049117112, - "heading": 6.6780180524147395e-18, - "angularVelocity": -1.0989409673710028e-16, - "velocityX": 3.183856249883563, - "velocityY": 0.017034922563732322, - "timestamp": 2.0001864317183857 - }, - { - "x": 7.111245945109509, - "y": 4.053749520879657, - "heading": 3.4043054479450288e-18, - "angularVelocity": -8.836349922350792e-17, - "velocityX": 3.0233816205072856, - "velocityY": 0.03637073102354394, - "timestamp": 2.0372346769590868 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, - "angularVelocity": -9.188843930234661e-17, - "velocityX": 2.862359859004765, - "velocityY": 0.0504413971839862, - "timestamp": 2.074282922199788 - }, - { - "x": 7.386141990779784, - "y": 4.060115199818634, - "heading": -6.285690223777642e-18, - "angularVelocity": -9.593548047553597e-17, - "velocityX": 2.5770864103628295, - "velocityY": 0.06863424057552284, - "timestamp": 2.1398028966048974 - }, - { - "x": 7.536265872308939, - "y": 4.064907354049287, - "heading": -1.0202276969724947e-17, - "angularVelocity": -5.97769883515376e-17, - "velocityX": 2.2912689281735665, - "velocityY": 0.07314035565737474, - "timestamp": 2.205322871010007 - }, - { - "x": 7.667662893519374, - "y": 4.069410994150776, - "heading": -1.2426686282698196e-17, - "angularVelocity": -3.395009435762481e-17, - "velocityX": 2.005449825087065, - "velocityY": 0.06873690263742, - "timestamp": 2.2708428454151166 - }, - { - "x": 7.780343830854776, - "y": 4.073216939396029, - "heading": -1.3104250722395789e-17, - "angularVelocity": -1.03413416429204e-17, - "velocityX": 1.7197951977010522, - "velocityY": 0.05808832008572811, - "timestamp": 2.336362819820226 - }, - { - "x": 7.874322400829606, - "y": 4.07602285690707, - "heading": -1.2397928827700396e-17, - "angularVelocity": 1.0780252923201902e-17, - "velocityX": 1.4343499189081053, - "velocityY": 0.04282537556703236, - "timestamp": 2.401882794225336 - }, - { - "x": 7.949612474283929, - "y": 4.07759641137373, - "heading": -1.096542742535041e-17, - "angularVelocity": 2.1863583068378234e-17, - "velocityX": 1.1491163441060581, - "velocityY": 0.024016408445632246, - "timestamp": 2.4674027686304454 - }, - { - "x": 8.006227098004604, - "y": 4.077753556862197, - "heading": -9.354890651543402e-18, - "angularVelocity": 2.458085169441397e-17, - "velocityX": 0.8640818961654191, - "velocityY": 0.0023984363530761407, - "timestamp": 2.532922743035555 - }, - { - "x": 8.044178200157203, - "y": 4.076344939580228, - "heading": -5.6627659945325885e-18, - "angularVelocity": 5.635113094762732e-17, - "velocityX": 0.579229502104909, - "velocityY": -0.021499051163544333, - "timestamp": 2.5984427174406646 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 8.642808619399734e-17, - "velocityX": 0.2945416648589691, - "velocityY": -0.04728304210698771, - "timestamp": 2.663962691845774 - }, - { - "x": 8.06151161206475, - "y": 4.067466722329622, - "heading": 7.499406153547556e-18, - "angularVelocity": 1.0142249804726799e-16, - "velocityX": -0.026574128354115305, - "velocityY": -0.07817228633622807, - "timestamp": 2.7379049265285085 - }, - { - "x": 8.0357781795929, - "y": 4.059672922438978, - "heading": 2.0171548175080756e-17, - "angularVelocity": 1.7137894295700475e-16, - "velocityX": -0.34802075677407873, - "velocityY": -0.10540389973451367, - "timestamp": 2.811847161211243 - }, - { - "x": 7.986251299714503, - "y": 4.050185891028673, - "heading": 3.259640252688688e-17, - "angularVelocity": 1.6803460705902343e-16, - "velocityX": -0.6698050186189494, - "velocityY": -0.12830328229881344, - "timestamp": 2.885789395893977 - }, - { - "x": 7.9129066919285, - "y": 4.039390810897977, - "heading": 4.5982022945133473e-17, - "angularVelocity": 1.81028075140569e-16, - "velocityX": -0.9919176516736913, - "velocityY": -0.1459934255032282, - "timestamp": 2.9597316305767114 - }, - { - "x": 7.8157231380567715, - "y": 4.027759201442648, - "heading": 5.916672321533067e-17, - "angularVelocity": 1.783108168799044e-16, - "velocityX": -1.314317240866671, - "velocityY": -0.1573067071239705, - "timestamp": 3.0336738652594457 - }, - { - "x": 7.694687242510101, - "y": 4.015880811506709, - "heading": 7.241021515042493e-17, - "angularVelocity": 1.7910591950468916e-16, - "velocityX": -1.6368979929535623, - "velocityY": -0.16064418375919837, - "timestamp": 3.10761609994218 - }, - { - "x": 7.549803190746082, - "y": 4.0045128268534045, - "heading": 8.516404983291186e-17, - "angularVelocity": 1.7248376026751216e-16, - "velocityX": -1.9594221406165937, - "velocityY": -0.1537414266972227, - "timestamp": 3.1815583346249143 - }, - { - "x": 7.381113647590609, - "y": 3.9946593063008904, - "heading": 9.839417244429125e-17, - "angularVelocity": 1.7892511182976314e-16, - "velocityX": -2.281369286163345, - "velocityY": -0.13325970732143547, - "timestamp": 3.2555005693076486 - }, - { - "x": 7.188747477257485, - "y": 3.987706179482555, - "heading": 1.110446332225851e-16, - "angularVelocity": 1.7108572429643452e-16, - "velocityX": -2.6015736629885633, - "velocityY": -0.09403457777777122, - "timestamp": 3.329442803990383 - }, - { - "x": 6.973038504473885, - "y": 3.9856630874408094, - "heading": 1.1423152127106693e-16, - "angularVelocity": 4.309969887337601e-17, - "velocityX": -2.91726337064529, - "velocityY": -0.027630920954873416, - "timestamp": 3.4033850386731173 - }, - { - "x": 6.734853840508573, - "y": 3.9916098664093003, - "heading": 1.1357543736188786e-16, - "angularVelocity": -8.872925078647214e-18, - "velocityX": -3.221226204310621, - "velocityY": 0.0804246584378634, - "timestamp": 3.4773272733558516 - }, - { - "x": 6.47658832080339, - "y": 4.010431184705039, - "heading": 1.1486840283254658e-16, - "angularVelocity": 1.7486156277295078e-17, - "velocityX": -3.4928011144554563, - "velocityY": 0.25454083686402557, - "timestamp": 3.551269508038586 - }, - { - "x": 6.204860010324235, - "y": 4.048943809403862, - "heading": 1.1487825258491134e-16, - "angularVelocity": 1.3320874674868913e-19, - "velocityX": -3.6748728469603784, - "velocityY": 0.5208474542873691, - "timestamp": 3.62521174272132 - }, - { - "x": 5.932253242931649, - "y": 4.111293853023339, - "heading": 1.0773394940295777e-16, - "angularVelocity": -9.662006041077537e-17, - "velocityX": -3.686753160250877, - "velocityY": 0.8432263899921877, - "timestamp": 3.6991539774040545 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 9.686455716091523e-17, - "angularVelocity": -1.4699842774133078e-16, - "velocityX": -3.6014804699029255, - "velocityY": 1.1543500395591086, - "timestamp": 3.773096212086789 - }, - { - "x": 5.543077137245883, - "y": 4.241512673300566, - "heading": 9.161999350902719e-17, - "angularVelocity": -1.5163138697871951e-16, - "velocityX": -3.552563374503243, - "velocityY": 1.2971011802444727, - "timestamp": 3.8076837981821674 - }, - { - "x": 5.422090048254055, - "y": 4.291242278322373, - "heading": 8.675864551293739e-17, - "angularVelocity": -1.4055181482408883e-16, - "velocityX": -3.4979916973158245, - "velocityY": 1.4377876757479933, - "timestamp": 3.842271384277546 - }, - { - "x": 5.303183074500814, - "y": 4.34575871765036, - "heading": 8.202370382364629e-17, - "angularVelocity": -1.368971421201274e-16, - "velocityX": -3.437851182367712, - "velocityY": 1.5761851427750155, - "timestamp": 3.8768589703729246 - }, - { - "x": 5.186545516053187, - "y": 4.404975197881964, - "heading": 7.882281033080331e-17, - "angularVelocity": -9.254457608045447e-17, - "velocityX": -3.372237603572297, - "velocityY": 1.712073229635336, - "timestamp": 3.911446556468303 - }, - { - "x": 5.072363058490256, - "y": 4.468797441531519, - "heading": 7.597641724828609e-17, - "angularVelocity": -8.229522218370685e-17, - "velocityX": -3.3012554633926476, - "velocityY": 1.845235555715285, - "timestamp": 3.946034142563682 - }, - { - "x": 4.960817475466504, - "y": 4.537123834649348, - "heading": 7.51892087087232e-17, - "angularVelocity": -2.2759857753359948e-17, - "velocityX": -3.225017863812662, - "velocityY": 1.975460008380286, - "timestamp": 3.9806217286590604 - }, - { - "x": 4.852086330068725, - "y": 4.609845576159071, - "heading": 7.00186010980759e-17, - "angularVelocity": -1.4949316197981958e-16, - "velocityX": -3.143646541216975, - "velocityY": 2.1025387926519685, - "timestamp": 4.015209314754439 - }, - { - "x": 4.744615410553515, - "y": 4.6844171206048495, - "heading": 6.888474965661026e-17, - "angularVelocity": -3.278203452357036e-17, - "velocityX": -3.10721075529382, - "velocityY": 2.1560204935996916, - "timestamp": 4.049796900849818 - }, - { - "x": 4.634255259438232, - "y": 4.75464211813236, - "heading": 6.027771904333573e-17, - "angularVelocity": -2.4884739251649313e-16, - "velocityX": -3.190744529293135, - "velocityY": 2.0303526627692405, - "timestamp": 4.084384486945196 - }, - { - "x": 4.521181551584758, - "y": 4.820408744704174, - "heading": 5.1990889261648227e-17, - "angularVelocity": -2.395897117201502e-16, - "velocityX": -3.269199172838061, - "velocityY": 1.9014517633713166, - "timestamp": 4.118972073040575 - }, - { - "x": 4.405574295139685, - "y": 4.881612290783718, - "heading": 4.5533169228459885e-17, - "angularVelocity": -1.867062944312063e-16, - "velocityX": -3.3424494015360238, - "velocityY": 1.7695234906179804, - "timestamp": 4.153559659135953 - }, - { - "x": 4.287617535211261, - "y": 4.938155315408326, - "heading": 4.203003409755996e-17, - "angularVelocity": -1.0128301874666137e-16, - "velocityX": -3.410378498318693, - "velocityY": 1.6347779942979137, - "timestamp": 4.188147245231332 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 4.0833427337575637e-17, - "angularVelocity": -3.459642302543387e-17, - "velocityX": -3.4728780863994273, - "velocityY": 1.4974297517257, - "timestamp": 4.222734831326711 - }, - { - "x": 3.9262086603019655, - "y": 5.072405406382895, - "heading": 4.69185026085022e-17, - "angularVelocity": 9.025220132506432e-17, - "velocityX": -3.5787544523378267, - "velocityY": 1.2229891223429616, - "timestamp": 4.290157843525713 - }, - { - "x": 3.6792395049411635, - "y": 5.135860556463708, - "heading": 5.846977032472703e-17, - "angularVelocity": 1.713252988776416e-16, - "velocityX": -3.6629801503359496, - "velocityY": 0.9411497352494828, - "timestamp": 4.3575808557247155 - }, - { - "x": 3.4280856473907875, - "y": 5.179929383329658, - "heading": 6.25257434115025e-17, - "angularVelocity": 6.01571029606942e-17, - "velocityX": -3.7250465287591408, - "velocityY": 0.6536169985387049, - "timestamp": 4.425003867923718 - }, - { - "x": 3.174266449354605, - "y": 5.204345315411431, - "heading": 8.981508430066906e-17, - "angularVelocity": 4.0474817127156825e-16, - "velocityX": -3.7645781426529163, - "velocityY": 0.3621305439411197, - "timestamp": 4.4924268801227205 - }, - { - "x": 2.919317391668449, - "y": 5.208960723831647, - "heading": 3.637405660157601e-17, - "angularVelocity": -7.926229629337798e-16, - "velocityX": -3.781335917381794, - "velocityY": 0.06845449750291784, - "timestamp": 4.559849892321723 - }, - { - "x": 2.6778199582724502, - "y": 5.213921183762201, - "heading": 2.780922480909488e-17, - "angularVelocity": -1.270312837495845e-16, - "velocityX": -3.581825040435896, - "velocityY": 0.07357220878699586, - "timestamp": 4.6272729045207255 - }, - { - "x": 2.4555441151084385, - "y": 5.223767512233129, - "heading": 2.258282570608997e-17, - "angularVelocity": -7.751654714759439e-17, - "velocityX": -3.2967355790624753, - "velocityY": 0.14603809811799462, - "timestamp": 4.694695916719728 - }, - { - "x": 2.2524898779476, - "y": 5.238499719208596, - "heading": 1.8179159660483545e-17, - "angularVelocity": -6.531399151152503e-17, - "velocityX": -3.0116458837750555, - "velocityY": 0.2185041352347682, - "timestamp": 4.7621189289187305 - }, - { - "x": 2.068657252210669, - "y": 5.258117807366149, - "heading": 1.4559970052520395e-17, - "angularVelocity": -5.367884777868486e-17, - "velocityX": -2.726556108088765, - "velocityY": 0.29097021206423906, - "timestamp": 4.829541941117733 - }, - { - "x": 1.9040462406637386, - "y": 5.282621777825342, - "heading": 1.0447672435760193e-17, - "angularVelocity": -6.099249326267761e-17, - "velocityX": -2.441466291376519, - "velocityY": 0.3634363054985942, - "timestamp": 4.896964953316735 - }, - { - "x": 1.7586568449915738, - "y": 5.312011631159126, - "heading": 6.39006439635328e-18, - "angularVelocity": -6.018135214654068e-17, - "velocityX": -2.156376449676296, - "velocityY": 0.4359024074308457, - "timestamp": 4.964387965515738 - }, - { - "x": 1.6324890663305314, - "y": 5.34628736769762, - "heading": 2.9527164791572072e-18, - "angularVelocity": -5.0981820681006015e-17, - "velocityX": -1.8712865911219376, - "velocityY": 0.508368514259319, - "timestamp": 5.03181097771474 - }, - { - "x": 1.5255429054998768, - "y": 5.385448987646794, - "heading": 6.750997693764207e-19, - "angularVelocity": -3.378099903552108e-17, - "velocityX": -1.5861967204164489, - "velocityY": 0.5808346241426653, - "timestamp": 5.099233989913743 - }, - { - "x": 1.437818363118664, - "y": 5.4294964911430075, - "heading": 3.7918321325869045e-19, - "angularVelocity": -4.388954848629936e-18, - "velocityX": -1.301106840529317, - "velocityY": 0.6533007360484864, - "timestamp": 5.166657002112745 - }, - { - "x": 1.3693154396712461, - "y": 5.478429878281045, - "heading": 1.633332658934955e-18, - "angularVelocity": 1.8601207579968088e-17, - "velocityX": -1.0160169534583994, - "velocityY": 0.7257668493601166, - "timestamp": 5.234080014311748 - }, - { - "x": 1.3200341355468752, - "y": 5.532249149129769, - "heading": 3.8843995459700536e-19, - "angularVelocity": -1.8463914088315652e-17, - "velocityX": -0.7309270606141895, - "velocityY": 0.798232963693069, - "timestamp": 5.30150302651075 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -5.761237033299429e-18, - "velocityX": -0.4458371630310619, - "velocityY": 0.8706990788013996, - "timestamp": 5.368926038709753 - }, - { - "x": 1.2868335136673428, - "y": 5.68644048793425, - "heading": -1.3391788419363526e-18, - "angularVelocity": -1.3686362473728894e-17, - "velocityX": -0.03210027395244838, - "velocityY": 0.9758655723245745, - "timestamp": 5.466773720706915 - }, - { - "x": 1.3242854311298142, - "y": 5.791775499822582, - "heading": 2.339658407779995e-18, - "angularVelocity": 3.759759224271494e-17, - "velocityX": 0.3827573295354856, - "velocityY": 1.076520258204863, - "timestamp": 5.564621402704078 - }, - { - "x": 1.4024881161679177, - "y": 5.906284066156856, - "heading": 5.091444284753208e-18, - "angularVelocity": 2.812315857202659e-17, - "velocityX": 0.7992287956333111, - "velocityY": 1.1702736743175466, - "timestamp": 5.66246908470124 - }, - { - "x": 1.5216854983005004, - "y": 6.028805982329465, - "heading": 6.6702155316476646e-18, - "angularVelocity": 1.6134988737668432e-17, - "velocityX": 1.218193213162059, - "velocityY": 1.2521698385881244, - "timestamp": 5.760316766698403 - }, - { - "x": 1.682281091295048, - "y": 6.156892671597042, - "heading": 8.737231811568005e-18, - "angularVelocity": 2.1124836460875052e-17, - "velocityX": 1.6412815277443633, - "velocityY": 1.3090416313724325, - "timestamp": 5.8581644486955655 - }, - { - "x": 1.884561264278339, - "y": 6.2823011062588545, - "heading": 4.474450456421642e-18, - "angularVelocity": -4.3565481249463154e-17, - "velocityX": 2.06729652511499, - "velocityY": 1.2816699599020622, - "timestamp": 5.956012130692728 - }, - { - "x": 2.074597433027926, - "y": 6.367773862583125, - "heading": 1.3148438373880832e-18, - "angularVelocity": -3.229107274750916e-17, - "velocityX": 1.9421632160391744, - "velocityY": 0.8735286782445029, - "timestamp": 6.053859812689891 - }, - { - "x": 2.2289686427209534, - "y": 6.431502913374048, - "heading": 4.376002899474807e-18, - "angularVelocity": 3.128494207400489e-17, - "velocityX": 1.5776685409625097, - "velocityY": 0.6513087432441247, - "timestamp": 6.151707494687053 - }, - { - "x": 2.345509714236833, - "y": 6.4775215317101145, - "heading": 6.666442968036081e-18, - "angularVelocity": 2.3408220019295113e-17, - "velocityX": 1.1910458085175606, - "velocityY": 0.4703087226675544, - "timestamp": 6.249555176684216 - }, - { - "x": 2.4235057296596247, - "y": 6.507444880215651, - "heading": 4.695809387720839e-18, - "angularVelocity": -2.013980853353183e-17, - "velocityX": 0.7971166391560869, - "velocityY": 0.30581560947355857, - "timestamp": 6.347402858681378 - }, - { - "x": 2.462606906890869, - "y": 6.522137641906738, - "heading": 1.18409559006069e-37, - "angularVelocity": -4.799101310235197e-17, - "velocityX": 0.3996127085808573, - "velocityY": 0.15015952745322653, - "timestamp": 6.445250540678541 - }, - { - "x": 2.462606906890869, - "y": 6.522137641906738, - "heading": 7.445601745662792e-38, - "angularVelocity": -4.492041573884033e-37, - "velocityX": -6.88570869510922e-34, - "velocityY": -3.3118407683240263e-34, - "timestamp": 6.543098222675703 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 2.570159716602358e-22, + "angularVelocity": 4.6040759154316275e-21, + "velocityX": 1.4250190224081253e-19, + "velocityY": 1.259493468984856e-19, + "timestamp": 0 + }, + { + "x": 1.310734152886531, + "y": 5.587725049797117, + "heading": -4.624423370992563e-19, + "angularVelocity": -6.667715053540946e-18, + "velocityX": 0.2991570561378951, + "velocityY": -0.04653506643871116, + "timestamp": 0.06939399019857362 + }, + { + "x": 1.3522535562378788, + "y": 5.581266541953786, + "heading": -1.1759601942239705e-18, + "angularVelocity": -1.0282127531279589e-17, + "velocityX": 0.5983141080738859, + "velocityY": -0.09307013222398468, + "timestamp": 0.13878798039714724 + }, + { + "x": 1.4145326607692024, + "y": 5.571578780265876, + "heading": 1.4139400808476166e-18, + "angularVelocity": 3.732167969674453e-17, + "velocityX": 0.8974711549675912, + "velocityY": -0.13960519722513298, + "timestamp": 0.20818197059572086 + }, + { + "x": 1.4975714660528414, + "y": 5.55866176479989, + "heading": 7.102195539243054e-18, + "angularVelocity": 8.197043349311479e-17, + "velocityX": 1.196628195698503, + "velocityY": -0.18614026126790603, + "timestamp": 0.2775759607942945 + }, + { + "x": 1.6013699715542196, + "y": 5.542515495638963, + "heading": 1.3318887961611829e-17, + "angularVelocity": 8.958545840321423e-17, + "velocityX": 1.4957852287259228, + "velocityY": -0.23267532411270994, + "timestamp": 0.3469699509928681 + }, + { + "x": 1.7259281765860246, + "y": 5.523139972889977, + "heading": 1.841155729782659e-17, + "angularVelocity": 7.338775766662924e-17, + "velocityX": 1.7949422518488518, + "velocityY": -0.27921038541726767, + "timestamp": 0.4163639411914417 + }, + { + "x": 1.8712460802318407, + "y": 5.500535196695444, + "heading": 2.3529721894456916e-17, + "angularVelocity": 7.37551563468695e-17, + "velocityX": 2.094099261765793, + "velocityY": -0.32574544466816385, + "timestamp": 0.4857579313900162 + }, + { + "x": 2.037323681208685, + "y": 5.4747011672548815, + "heading": 2.8673364146250235e-17, + "angularVelocity": 7.412230132714935e-17, + "velocityX": 2.3932562531943504, + "velocityY": -0.37228050104393373, + "timestamp": 0.5551519215885898 + }, + { + "x": 2.2241609775920828, + "y": 5.445637884867564, + "heading": 3.384245946066316e-17, + "angularVelocity": 7.448909191732528e-17, + "velocityX": 2.6924132168903303, + "velocityY": -0.4188155531070138, + "timestamp": 0.6245459117871635 + }, + { + "x": 2.431757966174578, + "y": 5.413345350032281, + "heading": 3.903697176098764e-17, + "angularVelocity": 7.485536262518973e-17, + "velocityX": 2.9915701343653476, + "velocityY": -0.4653505979822776, + "timestamp": 0.6939399019857371 + }, + { + "x": 2.660114640541256, + "y": 5.377823563746616, + "heading": 4.4256799680232527e-17, + "angularVelocity": 7.522017258708922e-17, + "velocityX": 3.29072695939845, + "velocityY": -0.5118856284819102, + "timestamp": 0.7633338921843107 + }, + { + "x": 2.909230981447395, + "y": 5.339072529003315, + "heading": 4.9501702917687895e-17, + "angularVelocity": 7.558151970286575e-17, + "velocityX": 3.589883507106047, + "velocityY": -0.5584206158546873, + "timestamp": 0.8327278823828843 + }, + { + "x": 3.168557198602556, + "y": 5.29873296059366, + "heading": 6.319291433274611e-17, + "angularVelocity": 1.9729678861063994e-16, + "velocityX": 3.737012620445782, + "velocityY": -0.5813121322786239, + "timestamp": 0.9021218725814579 + }, + { + "x": 3.4267765970934825, + "y": 5.251827631481489, + "heading": 8.420269956089665e-17, + "angularVelocity": 3.0276087551661663e-16, + "velocityX": 3.7210628435116764, + "velocityY": -0.6759278285907664, + "timestamp": 0.9715158627800315 + }, + { + "x": 3.6804171256627907, + "y": 5.18441954809062, + "heading": 4.866640802350699e-17, + "angularVelocity": -5.120946559749006e-16, + "velocityX": 3.6550791767919044, + "velocityY": -0.9713821499236162, + "timestamp": 1.0409098529786052 + }, + { + "x": 3.9278531799158563, + "y": 5.096939458942259, + "heading": 2.4335451025021366e-17, + "angularVelocity": -3.5062052101080536e-16, + "velocityX": 3.5656697870379945, + "velocityY": -1.260629182700587, + "timestamp": 1.1103038431771788 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 1.2553482249572005e-18, + "angularVelocity": -3.325951243636478e-16, + "velocityX": 3.4534097952511087, + "velocityY": -1.541800129495057, + "timestamp": 1.1796978333757524 + }, + { + "x": 4.284849298076782, + "y": 4.931828595221009, + "heading": -1.3377953270770433e-17, + "angularVelocity": -4.226105068324817e-16, + "velocityX": 3.38908081155215, + "velocityY": -1.6784855317357408, + "timestamp": 1.214323809917123 + }, + { + "x": 4.399784866536065, + "y": 4.869069261630718, + "heading": -2.6846630837218187e-17, + "angularVelocity": -3.8897610729556746e-16, + "velocityX": 3.3193451835780268, + "velocityY": -1.8124928120166828, + "timestamp": 1.2489497864584935 + }, + { + "x": 4.512122399892567, + "y": 4.801769947661279, + "heading": -3.804014031225808e-17, + "angularVelocity": -3.2326913470628567e-16, + "velocityX": 3.2443137949419496, + "velocityY": -1.9436076810434628, + "timestamp": 1.283575762999864 + }, + { + "x": 4.621682688033729, + "y": 4.730038068451707, + "heading": -4.7868809258190414e-17, + "angularVelocity": -2.838524693725381e-16, + "velocityX": 3.1641068089520323, + "velocityY": -2.0716203952795396, + "timestamp": 1.3182017395412347 + }, + { + "x": 4.728291014117835, + "y": 4.653988188924879, + "heading": -5.969081362943227e-17, + "angularVelocity": -3.414200999257287e-16, + "velocityX": 3.0788539915035598, + "velocityY": -2.1963244685955665, + "timestamp": 1.3528277160826052 + }, + { + "x": 4.836842403001111, + "y": 4.580738511169008, + "heading": -6.391442093146582e-17, + "angularVelocity": -1.2197799812053293e-16, + "velocityX": 3.134969746010814, + "velocityY": -2.1154544960878003, + "timestamp": 1.3874536926239758 + }, + { + "x": 4.948232400240648, + "y": 4.5118822283902675, + "heading": -5.985061600801175e-17, + "angularVelocity": 1.173628971657168e-16, + "velocityX": 3.216948902695972, + "velocityY": -1.9885730210806443, + "timestamp": 1.4220796691653463 + }, + { + "x": 5.062283352257089, + "y": 4.447529300627276, + "heading": -5.566760825345324e-17, + "angularVelocity": 1.2080548110971404e-16, + "velocityX": 3.293797414787035, + "velocityY": -1.8585158944500793, + "timestamp": 1.456705645706717 + }, + { + "x": 5.178813309821618, + "y": 4.387782436951599, + "heading": -4.4683720245590447e-17, + "angularVelocity": 3.1721525586676e-16, + "velocityX": 3.36539122370464, + "velocityY": -1.7254925245008748, + "timestamp": 1.4913316222480875 + }, + { + "x": 5.297636355743312, + "y": 4.332736981184801, + "heading": -3.4831170249496923e-17, + "angularVelocity": 2.845421553553544e-16, + "velocityX": 3.431615734497117, + "velocityY": -1.5897156200354856, + "timestamp": 1.525957598789458 + }, + { + "x": 5.418562909052227, + "y": 4.282480769562024, + "heading": -3.435849457009512e-17, + "angularVelocity": 1.3650898164143384e-17, + "velocityX": 3.4923651370362165, + "velocityY": -1.4514020005394612, + "timestamp": 1.5605835753308286 + }, + { + "x": 5.541400030004997, + "y": 4.2370939937981875, + "heading": -3.7514977433318736e-17, + "angularVelocity": -9.115938894395056e-17, + "velocityX": 3.5475424297711617, + "velocityY": -1.3107724401537064, + "timestamp": 1.5952095518721991 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -3.82351344872691e-17, + "angularVelocity": -2.0798173101011824e-17, + "velocityX": 3.597059527461787, + "velocityY": -1.1680513673144106, + "timestamp": 1.6298355284135697 + }, + { + "x": 5.800859276314264, + "y": 4.159128243363963, + "heading": -3.121153141892538e-17, + "angularVelocity": 1.896973231484086e-16, + "velocityX": 3.643657020852143, + "velocityY": -1.013383183783887, + "timestamp": 1.6668608430140948 + }, + { + "x": 5.937245993628074, + "y": 4.127402498051012, + "heading": -2.332987946719607e-17, + "angularVelocity": 2.1287197791179265e-16, + "velocityX": 3.6836072504803297, + "velocityY": -0.8568663265997021, + "timestamp": 1.7038861576146198 + }, + { + "x": 6.074863067137857, + "y": 4.101529712913805, + "heading": -3.4147305540686986e-17, + "angularVelocity": -2.921629752561486e-16, + "velocityX": 3.7168373853015195, + "velocityY": -0.6987863686333223, + "timestamp": 1.7409114722151449 + }, + { + "x": 6.213459439027469, + "y": 4.081557083562315, + "heading": -1.42164023960539e-17, + "angularVelocity": 5.383047614782771e-16, + "velocityX": 3.743286812953797, + "velocityY": -0.5394317257524714, + "timestamp": 1.77793678681567 + }, + { + "x": 6.352782264616147, + "y": 4.067521040771664, + "heading": -1.2952725040414258e-17, + "angularVelocity": 3.413009097467113e-17, + "velocityX": 3.762907272817643, + "velocityY": -0.3790931405199291, + "timestamp": 1.814962101416195 + }, + { + "x": 6.492463118765251, + "y": 4.059455134364433, + "heading": -1.1051006489340503e-17, + "angularVelocity": 5.13626574588434e-17, + "velocityX": 3.772577104506811, + "velocityY": -0.2178484232816345, + "timestamp": 1.85198741601672 + }, + { + "x": 6.627732984606863, + "y": 4.055428289013381, + "heading": -8.35420674945777e-18, + "angularVelocity": 7.283664619836627e-17, + "velocityX": 3.653442713480481, + "velocityY": -0.10875924740947829, + "timestamp": 1.889012730617245 + }, + { + "x": 6.7574379860569564, + "y": 4.05359307719316, + "heading": -5.856926112032921e-18, + "angularVelocity": 6.744792487052549e-17, + "velocityX": 3.503143804435185, + "velocityY": -0.04956640720064634, + "timestamp": 1.9260380452177701 + }, + { + "x": 6.881310147885468, + "y": 4.053080371999493, + "heading": -3.474844730669908e-18, + "angularVelocity": 6.433656019193302e-17, + "velocityX": 3.345607273428941, + "velocityY": -0.013847423018504562, + "timestamp": 1.9630633598182952 + }, + { + "x": 6.999262823921184, + "y": 4.05342251600114, + "heading": -1.8665913885819445e-18, + "angularVelocity": 4.3436588169438206e-17, + "velocityX": 3.1857305551170585, + "velocityY": 0.009240812815188005, + "timestamp": 2.00008867441882 + }, + { + "x": 7.111261640539779, + "y": 4.0543326919625615, + "heading": -7.554572779591287e-19, + "angularVelocity": 3.001011936481945e-17, + "velocityX": 3.0249254551103046, + "velocityY": 0.024582531471829573, + "timestamp": 2.0371139890193453 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": -4.589044952591168e-28, + "angularVelocity": 2.0403804416247763e-17, + "velocityX": 2.8637086743828277, + "velocityY": 0.034722032320904934, + "timestamp": 2.0741393036198703 + }, + { + "x": 7.386460306360812, + "y": 4.05864347722903, + "heading": 4.974033033156732e-19, + "angularVelocity": 7.57885321670836e-18, + "velocityX": 2.5775997849542525, + "velocityY": 0.04609434451454496, + "timestamp": 2.1397697215828186 + }, + { + "x": 7.53683861526572, + "y": 4.0619164310557965, + "heading": 2.2229573505485046e-18, + "angularVelocity": 2.6291986266563396e-17, + "velocityX": 2.291289825242349, + "velocityY": 0.04986946492729605, + "timestamp": 2.205400139545767 + }, + { + "x": 7.6684247981223, + "y": 4.0651140489216235, + "heading": 6.3469540781667075e-18, + "angularVelocity": 6.283666714516732e-17, + "velocityX": 2.0049572582467055, + "velocityY": 0.048721583148094445, + "timestamp": 2.2710305575087153 + }, + { + "x": 7.781221122380145, + "y": 4.068010125801229, + "heading": 5.162171035103576e-18, + "angularVelocity": -1.8052346447254954e-17, + "velocityX": 1.718659240011596, + "velocityY": 0.04412705220375746, + "timestamp": 2.3366609754716636 + }, + { + "x": 7.87523101347, + "y": 4.070437514376607, + "heading": 7.951055539821409e-18, + "angularVelocity": 4.2493779436390584e-17, + "velocityX": 1.432413414507389, + "velocityY": 0.03698572477092609, + "timestamp": 2.402291393434612 + }, + { + "x": 7.950458117127661, + "y": 4.072267695467205, + "heading": 6.860884770186536e-18, + "angularVelocity": -1.661075463106874e-17, + "velocityX": 1.1462231384863357, + "velocityY": 0.027886171494901118, + "timestamp": 2.4679218113975603 + }, + { + "x": 8.006905948924626, + "y": 4.073398787704802, + "heading": 2.4339380746534332e-18, + "angularVelocity": -6.745266649855045e-17, + "velocityX": 0.8600864286561777, + "velocityY": 0.01723426838811251, + "timestamp": 2.5335522293605086 + }, + { + "x": 8.044577771228491, + "y": 4.0737480502725605, + "heading": 2.9083264970800702e-18, + "angularVelocity": 7.228179214282684e-18, + "velocityX": 0.5739994269902885, + "velocityY": 0.0053216569176217, + "timestamp": 2.599182647323457 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 9.97684240054841e-28, + "angularVelocity": -4.4313697609032606e-17, + "velocityX": 0.2879578076461169, + "velocityY": -0.007635093856342742, + "timestamp": 2.6648130652864053 + }, + { + "x": 8.061114768714358, + "y": 4.071548552312887, + "heading": -6.576009001115218e-18, + "angularVelocity": -8.952107660855974e-17, + "velocityX": -0.032151769003766255, + "velocityY": -0.023120849595889583, + "timestamp": 2.7382707274562543 + }, + { + "x": 8.03523325714352, + "y": 4.068826117392824, + "heading": -1.7302069231102504e-17, + "angularVelocity": -1.4601690162241075e-16, + "velocityX": -0.3523323613402589, + "velocityY": -0.037061279104803016, + "timestamp": 2.8117283896261034 + }, + { + "x": 7.985826542335116, + "y": 4.0652145468753025, + "heading": -2.1436247637285606e-17, + "angularVelocity": -5.627974379451221e-17, + "velocityX": -0.6725876286964619, + "velocityY": -0.04916533430060717, + "timestamp": 2.8851860517959524 + }, + { + "x": 7.912889031757725, + "y": 4.060876766115487, + "heading": -3.018602234671886e-17, + "angularVelocity": -1.1911316602591952e-16, + "velocityX": -0.9929190287698493, + "velocityY": -0.05905144040367088, + "timestamp": 2.9586437139658015 + }, + { + "x": 7.816415389029205, + "y": 4.056013446336911, + "heading": -4.386893441840666e-17, + "angularVelocity": -1.8626936480131915e-16, + "velocityX": -1.3133230745276494, + "velocityY": -0.06620575219684201, + "timestamp": 3.0321013761356506 + }, + { + "x": 7.696401323527335, + "y": 4.050877776360081, + "heading": -5.3011403841257886e-17, + "angularVelocity": -1.244590305935936e-16, + "velocityX": -1.6337855297433015, + "velocityY": -0.06991333272974812, + "timestamp": 3.1055590383054996 + }, + { + "x": 7.552845329490783, + "y": 4.04579904746213, + "heading": -5.8407144486754e-17, + "angularVelocity": -7.345374854837963e-17, + "velocityX": -1.9542684833152193, + "velocityY": -0.06913817766494233, + "timestamp": 3.1790167004753487 + }, + { + "x": 7.385752700395874, + "y": 4.04122271365356, + "heading": -6.122941827410045e-17, + "angularVelocity": -3.8420413942670663e-17, + "velocityX": -2.2746793752918006, + "velocityY": -0.062298930749920316, + "timestamp": 3.2524743626451977 + }, + { + "x": 7.195145586336795, + "y": 4.037784147568392, + "heading": -6.335567714222456e-17, + "angularVelocity": -2.894536534102561e-17, + "velocityX": -2.5947887317507625, + "velocityY": -0.046810175869971896, + "timestamp": 3.325932024815047 + }, + { + "x": 6.981091630995715, + "y": 4.036460065314617, + "heading": -6.194065422525818e-17, + "angularVelocity": 1.926310849935096e-17, + "velocityX": -2.9139772355692886, + "velocityY": -0.01802510745187768, + "timestamp": 3.399389686984896 + }, + { + "x": 6.743803602394158, + "y": 4.0389311192423705, + "heading": -6.317374994659029e-17, + "angularVelocity": -1.6786481973259e-17, + "velocityX": -3.230269267934209, + "velocityY": 0.03363915832279882, + "timestamp": 3.472847349154745 + }, + { + "x": 6.484122272478982, + "y": 4.04866611767419, + "heading": -6.348618330459737e-17, + "angularVelocity": -4.253243967450325e-18, + "velocityX": -3.535115633202991, + "velocityY": 0.13252529612649763, + "timestamp": 3.546305011324594 + }, + { + "x": 6.20753972881787, + "y": 4.074789307976153, + "heading": -6.404970424521136e-17, + "angularVelocity": -7.671370479922628e-18, + "velocityX": -3.76519665193805, + "velocityY": 0.35562240248758487, + "timestamp": 3.619762673494443 + }, + { + "x": 5.9341618042346855, + "y": 4.124235317195696, + "heading": -6.716116670118769e-17, + "angularVelocity": -4.235722135085759e-17, + "velocityX": -3.7215712630641393, + "velocityY": 0.6731225546657573, + "timestamp": 3.693220335664292 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -6.707406307437602e-17, + "angularVelocity": 1.1857663806024648e-18, + "velocityX": -3.651219865855907, + "velocityY": 0.9857890275804273, + "timestamp": 3.766677997834141 + }, + { + "x": 5.540922192391165, + "y": 4.235828574357528, + "heading": -6.659781323672848e-17, + "angularVelocity": 1.3746706587182516e-17, + "velocityX": -3.608913259332814, + "velocityY": 1.130896109600127, + "timestamp": 3.801322647745386 + }, + { + "x": 5.417558030914266, + "y": 4.279972683638325, + "heading": -6.705658889375764e-17, + "angularVelocity": -1.3242323379839645e-17, + "velocityX": -3.560843067917901, + "velocityY": 1.2741970085969618, + "timestamp": 3.835967297656631 + }, + { + "x": 5.296056287935261, + "y": 4.329010889435292, + "heading": -6.943855501790622e-17, + "angularVelocity": -6.875422700655523e-17, + "velocityX": -3.507085315922588, + "velocityY": 1.4154625872276303, + "timestamp": 3.870611947567876 + }, + { + "x": 5.176611031188361, + "y": 4.382864860494658, + "heading": -7.367311041415583e-17, + "angularVelocity": -1.2222826345940313e-16, + "velocityX": -3.4477258985991424, + "velocityY": 1.55446717450842, + "timestamp": 3.905256597479121 + }, + { + "x": 5.059413042015962, + "y": 4.441448571672919, + "heading": -7.769450380917627e-17, + "angularVelocity": -1.160754519341825e-16, + "velocityX": -3.3828596759570035, + "velocityY": 1.6909886902694797, + "timestamp": 3.939901247390366 + }, + { + "x": 4.94464950941122, + "y": 4.504668439881924, + "heading": -7.988620676495281e-17, + "angularVelocity": -6.326237851534012e-17, + "velocityX": -3.312590339309282, + "velocityY": 1.8248089783261385, + "timestamp": 3.974545897301611 + }, + { + "x": 4.8325037281342125, + "y": 4.572423470089542, + "heading": -7.896813531381333e-17, + "angularVelocity": 2.6499660209021626e-17, + "velocityX": -3.237030293690655, + "velocityY": 1.9557140967276965, + "timestamp": 4.009190547212856 + }, + { + "x": 4.723154797252288, + "y": 4.644605406017416, + "heading": -6.685297337107051e-17, + "angularVelocity": 3.496979179591587e-16, + "velocityX": -3.1563006456137206, + "velocityY": 2.083494453336746, + "timestamp": 4.043835197124101 + }, + { + "x": 4.616777290879426, + "y": 4.721098849146534, + "heading": -4.689132664597989e-17, + "angularVelocity": 5.761826653182846e-16, + "velocityX": -3.070532005530056, + "velocityY": 2.207943891050558, + "timestamp": 4.078479847035346 + }, + { + "x": 4.50857855839689, + "y": 4.794993740354878, + "heading": -5.0771067360923566e-17, + "angularVelocity": -1.1198672017249538e-16, + "velocityX": -3.1231007604269903, + "velocityY": 2.1329380264384965, + "timestamp": 4.1131244969465905 + }, + { + "x": 4.397513666551277, + "y": 4.8645064031640235, + "heading": -6.128696485502522e-17, + "angularVelocity": -3.035359722499063e-16, + "velocityX": -3.2058309762155544, + "velocityY": 2.006447257721705, + "timestamp": 4.1477691468578355 + }, + { + "x": 4.283759944670474, + "y": 4.929525716016982, + "heading": -7.10068611169889e-17, + "angularVelocity": -2.8055980610512555e-16, + "velocityX": -3.2834426721651013, + "velocityY": 1.8767490224184165, + "timestamp": 4.18241379676908 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -7.867131264216922e-17, + "angularVelocity": -2.2123045100748588e-16, + "velocityX": -3.355810480670139, + "velocityY": 1.74405225643013, + "timestamp": 4.217058446680325 + }, + { + "x": 3.910732016936242, + "y": 5.096825920979702, + "heading": -8.738685068125367e-17, + "angularVelocity": -1.1851522353036446e-16, + "velocityX": -3.4915577224647327, + "velocityY": 1.453345144287359, + "timestamp": 4.290597843383089 + }, + { + "x": 3.6458301860416573, + "y": 5.181556502689957, + "heading": -6.150568630518277e-17, + "angularVelocity": 3.51936044311529e-16, + "velocityX": -3.6021757421438494, + "velocityY": 1.1521794508693837, + "timestamp": 4.364137240085852 + }, + { + "x": 3.3747001133794594, + "y": 5.243530170834085, + "heading": -3.504475258762766e-17, + "angularVelocity": 3.598198367759973e-16, + "velocityX": -3.68686832934017, + "velocityY": 0.8427274484534926, + "timestamp": 4.437676636788615 + }, + { + "x": 3.1003610694822004, + "y": 5.289250417175462, + "heading": -4.352123055745274e-17, + "angularVelocity": -1.1526444802732673e-16, + "velocityX": -3.7305044125682025, + "velocityY": 0.621710924909727, + "timestamp": 4.511216033491379 + }, + { + "x": 2.826021831554975, + "y": 5.334969499245887, + "heading": -7.949559303016089e-17, + "angularVelocity": -4.891849006802281e-16, + "velocityX": -3.7305070510174576, + "velocityY": 0.6216950929746646, + "timestamp": 4.584755430194142 + }, + { + "x": 2.5700139771360067, + "y": 5.3776336262742275, + "heading": -7.076803890272921e-17, + "angularVelocity": 1.186786201586086e-16, + "velocityX": -3.481234085366803, + "velocityY": 0.5801533455704441, + "timestamp": 4.658294826896905 + }, + { + "x": 2.337279538147283, + "y": 5.41641920061303, + "heading": -5.893119866883478e-17, + "angularVelocity": 1.6095916971261216e-16, + "velocityX": -3.1647586113523465, + "velocityY": 0.5274121909861365, + "timestamp": 4.731834223599669 + }, + { + "x": 2.12781853242598, + "y": 5.451326219289701, + "heading": -5.026414725549793e-17, + "angularVelocity": 1.178558949690138e-16, + "velocityX": -2.8482828947852163, + "velocityY": 0.4746709959800321, + "timestamp": 4.805373620302432 + }, + { + "x": 1.9416309659178494, + "y": 5.482354681313371, + "heading": -4.204375061179442e-17, + "angularVelocity": 1.1178221487834326e-16, + "velocityX": -2.531807097366853, + "velocityY": 0.42192978749993854, + "timestamp": 4.878913017005195 + }, + { + "x": 1.7787168415957708, + "y": 5.509504586188606, + "heading": -3.165703866093114e-17, + "angularVelocity": 1.4124010281718074e-16, + "velocityX": -2.2153312595228143, + "velocityY": 0.36918857228284047, + "timestamp": 4.952452413707959 + }, + { + "x": 1.6390761612434732, + "y": 5.532775933618142, + "heading": -2.0086106192649845e-17, + "angularVelocity": 1.5734331511581504e-16, + "velocityX": -1.8988553974233522, + "velocityY": 0.3164473530235367, + "timestamp": 5.025991810410722 + }, + { + "x": 1.5227089260501099, + "y": 5.552168723403807, + "heading": -1.374168138388034e-17, + "angularVelocity": 8.627246202414872e-17, + "velocityX": -1.5823795191536005, + "velocityY": 0.2637061310694281, + "timestamp": 5.099531207113485 + }, + { + "x": 1.4296151368650767, + "y": 5.567682955404048, + "heading": -6.859284014118712e-18, + "angularVelocity": 9.358789545320571e-17, + "velocityX": -1.2659036293336394, + "velocityY": 0.21096490719045813, + "timestamp": 5.173070603816249 + }, + { + "x": 1.35979479432542, + "y": 5.579318629512699, + "heading": -3.246170104472464e-18, + "angularVelocity": 4.913167733478094e-17, + "velocityX": -0.949427730851019, + "velocityY": 0.15822368186784203, + "timestamp": 5.246610000519012 + }, + { + "x": 1.3132478989266207, + "y": 5.587075745647187, + "heading": -2.3462842211621804e-18, + "angularVelocity": 1.223678632415003e-17, + "velocityX": -0.6329518256307737, + "velocityY": 0.10548245542238979, + "timestamp": 5.320149397221775 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.1322625168775542e-27, + "angularVelocity": 3.1905132868116714e-17, + "velocityX": -0.31647591502042816, + "velocityY": 0.05274122807866861, + "timestamp": 5.393688793924539 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 5.483370217924037e-28, + "angularVelocity": -4.839375204095192e-28, + "velocityX": -1.0823834640575863e-26, + "velocityY": -1.7334223684022012e-26, + "timestamp": 5.467228190627302 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.traj index 59b728c7..1515ebd3 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubFSub.traj @@ -1,1831 +1,1724 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -6.079995614896932e-33, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.306947763000364, - "y": 5.574973108257302, - "heading": -0.008994433988091095, - "angularVelocity": -0.12277149964907928, - "velocityX": 0.23168094435597542, - "velocityY": -0.21813883323536412, - "timestamp": 0.07326157955062942 - }, - { - "x": 1.3408944736698363, - "y": 5.543010933982932, - "heading": -0.026978879063552612, - "angularVelocity": -0.24548262794460876, - "velocityX": 0.463363073492191, - "velocityY": -0.4362747086592839, - "timestamp": 0.14652315910125885 - }, - { - "x": 1.3918148440551168, - "y": 5.495068027805583, - "heading": -0.05394057952659419, - "angularVelocity": -0.3680196445178875, - "velocityX": 0.695048764954495, - "velocityY": -0.6544072141417159, - "timestamp": 0.21978473865188827 - }, - { - "x": 1.4597093144768722, - "y": 5.431144700814373, - "heading": -0.08985971213865897, - "angularVelocity": -0.4902860794482582, - "velocityX": 0.9267404666703231, - "velocityY": -0.8725354733449758, - "timestamp": 0.2930463182025177 - }, - { - "x": 1.5445785095442424, - "y": 5.351241370164591, - "heading": -0.13471107898519388, - "angularVelocity": -0.6122085699167754, - "velocityX": 1.1584406941256162, - "velocityY": -1.0906580384956333, - "timestamp": 0.3663078977531471 - }, - { - "x": 1.6464232512218273, - "y": 5.2553586089581215, - "heading": -0.1884659824129613, - "angularVelocity": -0.733739345472597, - "velocityX": 1.390152141167011, - "velocityY": -1.3087727809664234, - "timestamp": 0.4395694773037765 - }, - { - "x": 1.7652446226105383, - "y": 5.143497220947543, - "heading": -0.25109340653071877, - "angularVelocity": -0.8548467628175698, - "velocityX": 1.6218783722318773, - "velocityY": -1.5268765524400676, - "timestamp": 0.5128310568544059 - }, - { - "x": 1.9010443771824763, - "y": 5.015658495720495, - "heading": -0.32255561727496634, - "angularVelocity": -0.9754391207858365, - "velocityX": 1.8536285376988066, - "velocityY": -1.7449627214043517, - "timestamp": 0.5860926364050353 - }, - { - "x": 2.053859595250169, - "y": 4.871864096281773, - "heading": -0.4022671776466062, - "angularVelocity": -1.0880404280193408, - "velocityX": 2.0858848390251463, - "velocityY": -1.9627531964329257, - "timestamp": 0.6593542159556647 - }, - { - "x": 2.1896989198822134, - "y": 4.744047518400832, - "heading": -0.47311179344304544, - "angularVelocity": -0.9670091230763074, - "velocityX": 1.8541686579139196, - "velocityY": -1.7446604163456465, - "timestamp": 0.7326157955062941 - }, - { - "x": 2.308560686135844, - "y": 4.632207510590712, - "heading": -0.535112719027594, - "angularVelocity": -0.846295233666115, - "velocityX": 1.6224297508012064, - "velocityY": -1.5265847187041606, - "timestamp": 0.8058773750569235 - }, - { - "x": 2.410444090432368, - "y": 4.536343322874329, - "heading": -0.5882784511543253, - "angularVelocity": -0.72569732256441, - "velocityX": 1.3906798750648661, - "velocityY": -1.3085192580393974, - "timestamp": 0.8791389546075529 - }, - { - "x": 2.49534860091136, - "y": 4.4564544045860375, - "heading": -0.6326127919617512, - "angularVelocity": -0.6051513095863228, - "velocityX": 1.158922739582985, - "velocityY": -1.090461313806123, - "timestamp": 0.9524005341581823 - }, - { - "x": 2.5632738362111045, - "y": 4.3925403400214185, - "heading": -0.6681173722205684, - "angularVelocity": -0.48462755617056985, - "velocityX": 0.9271603986207089, - "velocityY": -0.8724090438215177, - "timestamp": 1.0256621137088118 - }, - { - "x": 2.6142195241645787, - "y": 4.344600823178412, - "heading": -0.6947927082053813, - "angularVelocity": -0.36411084975827024, - "velocityX": 0.6953943426549579, - "velocityY": -0.6543609506791417, - "timestamp": 1.0989236932594413 - }, - { - "x": 2.6481854848919673, - "y": 4.312635645188571, - "heading": -0.7126387384821254, - "angularVelocity": -0.24359330478823768, - "velocityX": 0.4636258313802147, - "velocityY": -0.43631570853248003, - "timestamp": 1.1721852728100708 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": -0.12307101033443453, - "velocityX": 0.23185602115327572, - "velocityY": -0.2182720825031122, - "timestamp": 1.2454468523607003 - }, - { - "x": 2.662943710448623, - "y": 4.298728558024191, - "heading": -0.72070100113944, - "angularVelocity": 0.011651349233264986, - "velocityX": -0.027206592771411266, - "velocityY": 0.025447590705983056, - "timestamp": 1.3273355622067569 - }, - { - "x": 2.639501593010729, - "y": 4.320770481747221, - "heading": -0.7087160822503835, - "angularVelocity": 0.14635618159801306, - "velocityX": -0.2862680030246291, - "velocityY": 0.26916926355864296, - "timestamp": 1.4092242720528134 - }, - { - "x": 2.594845480264057, - "y": 4.36277075661854, - "heading": -0.6857017147672647, - "angularVelocity": 0.2810444507720759, - "velocityX": -0.5453268567867454, - "velocityY": 0.5128945730159264, - "timestamp": 1.49111298189887 - }, - { - "x": 2.5289756911084322, - "y": 4.424729822216665, - "heading": -0.6516589308540864, - "angularVelocity": 0.4157201154734962, - "velocityX": -0.8043818162412647, - "velocityY": 0.7566252504722968, - "timestamp": 1.5730016917449265 - }, - { - "x": 2.4418926576481637, - "y": 4.5066482714289275, - "heading": -0.6065878922273316, - "angularVelocity": 0.550393805342452, - "velocityX": -1.0634314989694753, - "velocityY": 1.000363168088272, - "timestamp": 1.654890401590983 - }, - { - "x": 2.3335969392795284, - "y": 4.608526864652899, - "heading": -0.5504868101397262, - "angularVelocity": 0.6850893388486693, - "velocityX": -1.322474350520624, - "velocityY": 1.2441103714479649, - "timestamp": 1.7367791114370397 - }, - { - "x": 2.2040892544412327, - "y": 4.730366540742225, - "heading": -0.48334990238896663, - "angularVelocity": 0.8198554828494793, - "velocityX": -1.5815084287169574, - "velocityY": 1.4878690398025962, - "timestamp": 1.8186678212830962 - }, - { - "x": 2.0533705390200705, - "y": 4.8721684079929855, - "heading": -0.4051637468918412, - "angularVelocity": 0.9547855332451622, - "velocityX": -1.840531078148622, - "velocityY": 1.7316412423316419, - "timestamp": 1.9005565311291528 - }, - { - "x": 1.8837222142414454, - "y": 5.031895792612386, - "heading": -0.31535335280985394, - "angularVelocity": 1.0967371967493782, - "velocityX": -2.071693705976669, - "velocityY": 1.9505422044097782, - "timestamp": 1.9824452409752094 - }, - { - "x": 1.7352824931464605, - "y": 5.171658722106698, - "heading": -0.2366561880408508, - "angularVelocity": 0.9610258229363577, - "velocityX": -1.8127006930996745, - "velocityY": 1.706742355045694, - "timestamp": 2.0643339508212657 - }, - { - "x": 1.6080502362813274, - "y": 5.291456457157948, - "heading": -0.16911546139257225, - "angularVelocity": 0.8247867963147685, - "velocityX": -1.5537215948855285, - "velocityY": 1.4629334773555265, - "timestamp": 2.146222660667322 - }, - { - "x": 1.5020244624691528, - "y": 5.391288489879848, - "heading": -0.11277393629567076, - "angularVelocity": 0.6880255556940454, - "velocityX": -1.294754478504957, - "velocityY": 1.219118397512621, - "timestamp": 2.2281113705133784 - }, - { - "x": 1.4172044018013137, - "y": 5.471154488105965, - "heading": -0.06766925261840594, - "angularVelocity": 0.550804668458663, - "velocityX": -1.0357967640141517, - "velocityY": 0.9752992613543129, - "timestamp": 2.3100000803594347 - }, - { - "x": 1.3535895239653573, - "y": 5.531054232218378, - "heading": -0.03382997301748837, - "angularVelocity": 0.4132349827532073, - "velocityX": -0.7768455255376048, - "velocityY": 0.7314774432888099, - "timestamp": 2.391888790205491 - }, - { - "x": 1.3111795494900627, - "y": 5.570987556283822, - "heading": -0.011272655480386088, - "angularVelocity": 0.27546309594458174, - "velocityX": -0.5178976998785506, - "velocityY": 0.48765359889678683, - "timestamp": 2.4737775000515474 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0.13765823764445279, - "velocityX": -0.2589502077253713, - "velocityY": 0.24382784263139995, - "timestamp": 2.5556662098976037 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.169901203643428e-30, - "velocityY": 4.744348570353821e-29, - "timestamp": 2.63755491974366 - }, - { - "x": 1.3279764392732873, - "y": 5.589659578477318, - "heading": -3.8286967101155775e-19, - "angularVelocity": -4.101142681562708e-18, - "velocityX": 0.40706315074964167, - "velocityY": -0.013868615044214471, - "timestamp": 2.7309114128077048 - }, - { - "x": 1.4039804148734387, - "y": 5.5870701279768555, - "heading": -4.0712541473530154e-19, - "angularVelocity": -2.5982712070316094e-19, - "velocityX": 0.814126292755424, - "velocityY": -0.027737229790526226, - "timestamp": 2.8242679058717495 - }, - { - "x": 1.517986376641016, - "y": 5.583185952281787, - "heading": 4.833998363566258e-19, - "angularVelocity": 9.538831821902836e-18, - "velocityX": 1.2211894216447965, - "velocityY": -0.04160584408996275, - "timestamp": 2.917624398935794 - }, - { - "x": 1.6699943225350276, - "y": 5.578007051461648, - "heading": 1.3619244496265011e-18, - "angularVelocity": 9.410228943340493e-18, - "velocityX": 1.6282525286718303, - "velocityY": -0.05547445764455082, - "timestamp": 3.010980891999839 - }, - { - "x": 1.8600042484729113, - "y": 5.571533425655531, - "heading": 3.711758987250246e-18, - "angularVelocity": 2.5170201581763002e-17, - "velocityX": 2.0353155919679775, - "velocityY": -0.06934306970923045, - "timestamp": 3.1043373850638836 - }, - { - "x": 2.0880161422023416, - "y": 5.563765075280871, - "heading": -9.287085834288012e-18, - "angularVelocity": -1.392382283532996e-16, - "velocityX": 2.4423785240217755, - "velocityY": -0.08321167730249175, - "timestamp": 3.1976938781279283 - }, - { - "x": 2.2780692584306133, - "y": 5.5572899779843725, - "heading": -5.547928961434018e-18, - "angularVelocity": 4.0052703333031833e-17, - "velocityX": 2.03577823010496, - "velocityY": -0.06935883176043627, - "timestamp": 3.291050371191973 - }, - { - "x": 2.430120406839514, - "y": 5.552109605257364, - "heading": -3.2776580295073633e-18, - "angularVelocity": 2.4318713794052425e-17, - "velocityX": 1.6287152977531212, - "velocityY": -0.05549022415702071, - "timestamp": 3.3844068642560177 - }, - { - "x": 2.5441695751975866, - "y": 5.548223957516569, - "heading": -1.9211838299437728e-18, - "angularVelocity": 1.4530579976060665e-17, - "velocityX": 1.2216522343824612, - "velocityY": -0.04162161208980244, - "timestamp": 3.4777633573200624 - }, - { - "x": 2.6202167594261327, - "y": 5.545633034900951, - "heading": -1.8132356413213059e-19, - "angularVelocity": 1.8636270440618356e-17, - "velocityX": 0.8145891273223099, - "velocityY": -0.02775299853408606, - "timestamp": 3.571119850384107 - }, - { - "x": 2.6582619574855144, - "y": 5.5443368374799995, - "heading": 8.843883513334677e-20, - "angularVelocity": 2.8892814546014257e-18, - "velocityX": 0.40752599841430864, - "velocityY": -0.013884384234014852, - "timestamp": 3.6644763434481518 - }, - { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 1.7598923552723236e-34, - "angularVelocity": -9.476131898054967e-19, - "velocityX": 0.0004628563966037854, - "velocityY": -0.000015769487296898133, - "timestamp": 3.7578328365121965 - }, - { - "x": 2.620331993851312, - "y": 5.545629108871589, - "heading": 2.945836613273263e-19, - "angularVelocity": 3.1551525697239727e-18, - "velocityX": -0.4066774370940851, - "velocityY": 0.013855473805082683, - "timestamp": 3.8512070216080794 - }, - { - "x": 2.5443424272440374, - "y": 5.548218068458602, - "heading": 2.309166242357916e-19, - "angularVelocity": -6.815922301609038e-19, - "velocityX": -0.8138177218425701, - "velocityY": 0.027726716799615945, - "timestamp": 3.9445812067039623 - }, - { - "x": 2.4303364695545335, - "y": 5.552102244014731, - "heading": -7.469284722560017e-19, - "angularVelocity": -1.047218864069531e-17, - "velocityX": -1.220957993477128, - "velocityY": 0.041597959347358555, - "timestamp": 4.037955391799845 - }, - { - "x": 2.278314122823792, - "y": 5.55728163547044, - "heading": -1.7117540142209262e-18, - "angularVelocity": -1.0332835908519937e-17, - "velocityX": -1.628098243253484, - "velocityY": 0.055469201150393666, - "timestamp": 4.131329576895728 - }, - { - "x": 2.0882753911343768, - "y": 5.563756242686637, - "heading": -3.8779782019319956e-18, - "angularVelocity": -2.3199468985970604e-17, - "velocityX": -2.035238449307225, - "velocityY": 0.06934044146380212, - "timestamp": 4.22470376199161 - }, - { - "x": 1.8602202867386202, - "y": 5.571526065245884, - "heading": -6.955816575429636e-18, - "angularVelocity": -3.296264638305206e-17, - "velocityX": -2.4423785241434, - "velocityY": 0.08321167730663664, - "timestamp": 4.318077947087493 - }, - { - "x": 1.6701383566026529, - "y": 5.578002144230341, - "heading": -1.819668166700424e-18, - "angularVelocity": 5.5006116981871155e-17, - "velocityX": -2.035701087485777, - "velocityY": 0.0693562035164242, - "timestamp": 4.411452132183375 - }, - { - "x": 1.518072799200871, - "y": 5.5831830078708, - "heading": 4.234959937878501e-18, - "angularVelocity": 6.484281229150569e-17, - "velocityX": -1.6285610123513967, - "velocityY": 0.05548496766342984, - "timestamp": 4.5048263172792575 - }, - { - "x": 1.4040236267647241, - "y": 5.587068655750534, - "heading": 2.524941481520431e-18, - "angularVelocity": -1.831387557299539e-17, - "velocityX": -1.221420806223103, - "velocityY": 0.041613727347481366, - "timestamp": 4.59820050237514 - }, - { - "x": 1.327990843372909, - "y": 5.589659087730582, - "heading": 8.045224971714285e-19, - "angularVelocity": -1.842525382897137e-17, - "velocityX": -0.814280556413611, - "velocityY": 0.027742485543317336, - "timestamp": 4.6915746874710225 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 6.74699155537553e-28, - "angularVelocity": -8.616220285509517e-18, - "velocityX": -0.40714028476041414, - "velocityY": 0.013871242994939689, - "timestamp": 4.784948872566905 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -7.225786160817796e-27, - "velocityX": -2.979354413794432e-29, - "velocityY": 7.541641997510845e-28, - "timestamp": 4.878323057662787 - }, - { - "x": 1.3078091735186845, - "y": 5.606755773559667, - "heading": 0.009999668662888749, - "angularVelocity": 0.1349541802858926, - "velocityX": 0.24069501005494717, - "velocityY": 0.21325450657673967, - "timestamp": 4.952419826226246 - }, - { - "x": 1.3434786990122702, - "y": 5.638358364996936, - "heading": 0.029996210094567113, - "angularVelocity": 0.2698706275619814, - "velocityX": 0.48139110769232624, - "velocityY": 0.42650431390462895, - "timestamp": 5.0265165947897055 - }, - { - "x": 1.3969833303762573, - "y": 5.6857616519517675, - "heading": 0.05997803368561842, - "angularVelocity": 0.4046306495359042, - "velocityX": 0.7220912922884387, - "velocityY": 0.6397483706242104, - "timestamp": 5.1006133633531645 - }, - { - "x": 1.468323578276302, - "y": 5.748965089703653, - "heading": 0.09992693775422122, - "angularVelocity": 0.5391450240657001, - "velocityX": 0.9627983687594002, - "velocityY": 0.8529850758692316, - "timestamp": 5.1747101319166235 - }, - { - "x": 1.5575001323238937, - "y": 5.827967973754339, - "heading": 0.14982100646683655, - "angularVelocity": 0.6733636254605087, - "velocityX": 1.203514752134039, - "velocityY": 1.0662122733008166, - "timestamp": 5.248806900480083 - }, - { - "x": 1.6645138155479717, - "y": 5.922769410023865, - "heading": 0.20963796260623796, - "angularVelocity": 0.8072815765348765, - "velocityX": 1.444242242996271, - "velocityY": 1.279427404349801, - "timestamp": 5.322903669043542 - }, - { - "x": 1.789365525357909, - "y": 6.033368309242042, - "heading": 0.27935847839394334, - "angularVelocity": 0.9409386825166515, - "velocityX": 1.6849818451743819, - "velocityY": 1.4926278347551774, - "timestamp": 5.397000437607001 - }, - { - "x": 1.9320561732696155, - "y": 6.159763414266322, - "heading": 0.35896870362448313, - "angularVelocity": 1.074408867993172, - "velocityX": 1.925733749048249, - "velocityY": 1.7058112989342795, - "timestamp": 5.47109720617046 - }, - { - "x": 2.092586653477802, - "y": 6.301953347709479, - "heading": 0.44846110031344777, - "angularVelocity": 1.2077773218503376, - "velocityX": 2.1664977208531133, - "velocityY": 1.9189761740812865, - "timestamp": 5.545193974733919 - }, - { - "x": 2.2353547101625333, - "y": 6.428313752108457, - "heading": 0.5276057235865483, - "angularVelocity": 1.068125166619866, - "velocityX": 1.9267784472209708, - "velocityY": 1.7053429837972895, - "timestamp": 5.619290743297378 - }, - { - "x": 2.360281889838346, - "y": 6.538881119368732, - "heading": 0.5968684037316069, - "angularVelocity": 0.9347597942868423, - "velocityX": 1.686000376116786, - "velocityY": 1.4922022835269577, - "timestamp": 5.693387511860837 - }, - { - "x": 2.4673676751793194, - "y": 6.633656126085495, - "heading": 0.6562491552260078, - "angularVelocity": 0.8013946173366472, - "velocityX": 1.4452153232519684, - "velocityY": 1.279070714493898, - "timestamp": 5.767484280424296 - }, - { - "x": 2.5566116515465542, - "y": 6.712639287159663, - "heading": 0.7057476703680625, - "angularVelocity": 0.668025287685273, - "velocityX": 1.2044246744765545, - "velocityY": 1.0659460944941062, - "timestamp": 5.841581048987755 - }, - { - "x": 2.6280134773537864, - "y": 6.7758309931109295, - "heading": 0.7453637127245453, - "angularVelocity": 0.5346527671626246, - "velocityX": 0.9636294156990158, - "velocityY": 0.8528267448954308, - "timestamp": 5.915677817551214 - }, - { - "x": 2.6815728846604703, - "y": 6.823231527897903, - "heading": 0.7750971751130776, - "angularVelocity": 0.4012788001208449, - "velocityX": 0.7228305408320154, - "velocityY": 0.6397112275638166, - "timestamp": 5.989774586114673 - }, - { - "x": 2.7172896860156874, - "y": 6.854841077120622, - "heading": 0.7949480902508361, - "angularVelocity": 0.26790527462553493, - "velocityX": 0.4820291361527109, - "velocityY": 0.42659821506532714, - "timestamp": 6.063871354678132 - }, - { - "x": 2.7351637810076403, - "y": 6.870659730936303, - "heading": 0.8049166539682622, - "angularVelocity": 0.13453439203137166, - "velocityX": 0.2412263763293308, - "velocityY": 0.2134864193164304, - "timestamp": 6.137968123241591 - }, - { - "x": 2.73519515991211, - "y": 6.870687484741211, - "heading": 0.8050032485420815, - "angularVelocity": 0.0011686685922978992, - "velocityX": 0.00042348526922062564, - "velocityY": 0.00037456143614395893, - "timestamp": 6.21206489180505 - }, - { - "x": 2.71737666825008, - "y": 6.854917835875481, - "heading": 0.7952044521096777, - "angularVelocity": -0.13221640762197384, - "velocityX": -0.2404271762292292, - "velocityY": -0.21278187956754013, - "timestamp": 6.286176695336588 - }, - { - "x": 2.6817084896089662, - "y": 6.823350601971179, - "heading": 0.775519887263024, - "angularVelocity": -0.2656063393700981, - "velocityX": -0.4812752750383339, - "velocityY": -0.4259407056452727, - "timestamp": 6.360288498868125 - }, - { - "x": 2.628190888379515, - "y": 6.7759855045171085, - "heading": 0.7459495915696174, - "angularVelocity": -0.398995764215558, - "velocityX": -0.7221198063894422, - "velocityY": -0.6391032897100266, - "timestamp": 6.434400302399663 - }, - { - "x": 2.5568241932220688, - "y": 6.712822154407591, - "heading": 0.7064940900377142, - "angularVelocity": -0.532378105058251, - "velocityX": -0.9629599030846991, - "velocityY": -0.8522711240272193, - "timestamp": 6.5085121059312 - }, - { - "x": 2.467608789693866, - "y": 6.633860035088962, - "heading": 0.657154460511053, - "angularVelocity": -0.6657459024188197, - "velocityX": -1.2037947974070313, - "velocityY": -1.0654459282687603, - "timestamp": 6.582623909462738 - }, - { - "x": 2.3605451226537615, - "y": 6.539098475645005, - "heading": 0.597932194997477, - "angularVelocity": -0.7990935679285951, - "velocityX": -1.4446236891921902, - "velocityY": -1.2786297852682258, - "timestamp": 6.6567357129942755 - }, - { - "x": 2.2356337183339816, - "y": 6.428536615850716, - "heading": 0.528828452368811, - "angularVelocity": -0.9324255966349123, - "velocityX": -1.685445480458886, - "velocityY": -1.4918252493931579, - "timestamp": 6.730847516525813 - }, - { - "x": 2.092875237629583, - "y": 6.30217337339218, - "heading": 0.4498420161681241, - "angularVelocity": -1.0657740391526649, - "velocityX": -1.9262583541843843, - "velocityY": -1.7050353173174484, - "timestamp": 6.804959320057351 - }, - { - "x": 1.9322705754719112, - "y": 6.16000743597858, - "heading": 0.3609649957464903, - "angularVelocity": -1.1992289512105054, - "velocityX": -2.167059152487097, - "velocityY": -1.9182630921487032, - "timestamp": 6.879071123588888 - }, - { - "x": 1.789533871896463, - "y": 6.033554554877999, - "heading": 0.28090803782331497, - "angularVelocity": -1.0802187250553423, - "velocityX": -1.9259645128083558, - "velocityY": -1.7062448231010843, - "timestamp": 6.953182927120426 - }, - { - "x": 1.6646414904257845, - "y": 5.922906567233019, - "heading": 0.2107799682947157, - "angularVelocity": -0.9462469699444712, - "velocityX": -1.6851888029309197, - "velocityY": -1.4929873836216447, - "timestamp": 7.027294730651963 - }, - { - "x": 1.5575924667954766, - "y": 5.828064283268442, - "heading": 0.15061002754742392, - "angularVelocity": -0.8118806705635195, - "velocityX": -1.4444261039959587, - "velocityY": -1.2797190116759578, - "timestamp": 7.101406534183501 - }, - { - "x": 1.4683859542310973, - "y": 5.7490283016453585, - "heading": 0.10042779042808991, - "angularVelocity": -0.6771153140084615, - "velocityX": -1.203674830696421, - "velocityY": -1.066442561920003, - "timestamp": 7.175518337715038 - }, - { - "x": 1.3970212709065528, - "y": 5.685799049936121, - "heading": 0.06025974538395147, - "angularVelocity": -0.5419925454610606, - "velocityX": -0.9629327573631904, - "velocityY": -0.8531603428758896, - "timestamp": 7.249630141246576 - }, - { - "x": 1.343497933013845, - "y": 5.638376829349959, - "heading": 0.03012622810608221, - "angularVelocity": -0.40659538484416025, - "velocityX": -0.7221972121128929, - "velocityY": -0.639874059577299, - "timestamp": 7.323741944778114 - }, - { - "x": 1.3078156736079332, - "y": 5.606761857880495, - "heading": 0.010039060501896319, - "angularVelocity": -0.2710387097248632, - "velocityX": -0.4814652687860333, - "velocityY": -0.4265848348630388, - "timestamp": 7.397853748309651 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -5.584258539451151e-28, - "angularVelocity": -0.1354583213971952, - "velocityX": -0.24073388709378457, - "velocityY": -0.21329334041956155, - "timestamp": 7.471965551841189 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": 7.53493083126559e-27, - "velocityX": -3.871179527516382e-30, - "velocityY": -3.125557960458497e-29, - "timestamp": 7.546077355372726 - }, - { - "x": 1.310726436274015, - "y": 5.58769762128994, - "heading": -1.5579970000423894e-18, - "angularVelocity": -2.2453280466561467e-17, - "velocityX": 0.299069985980355, - "velocityY": -0.04693411089639644, - "timestamp": 7.615465746658217 - }, - { - "x": 1.3522304064004054, - "y": 5.581184256432646, - "heading": -2.2539666819865276e-18, - "angularVelocity": -1.003005957705595e-17, - "velocityX": 0.598139967759544, - "velocityY": -0.09386822113363098, - "timestamp": 7.6848541379437085 - }, - { - "x": 1.4144863610944205, - "y": 5.571414209224462, - "heading": -1.1138395195686094e-18, - "angularVelocity": 1.6431093577313864e-17, - "velocityX": 0.8972099444973367, - "velocityY": -0.1408023305798717, - "timestamp": 7.7542425292292 - }, - { - "x": 1.49749429992851, - "y": 5.558387479732467, - "heading": 1.8047013110476196e-18, - "angularVelocity": 4.206093786115289e-17, - "velocityX": 1.1962799150734271, - "velocityY": -0.18773643905934295, - "timestamp": 7.823630920514691 - }, - { - "x": 1.6012542223682356, - "y": 5.5421040680405165, - "heading": 7.934655780307681e-18, - "angularVelocity": 8.834265128972616e-17, - "velocityX": 1.4953498779473966, - "velocityY": -0.23467054633035336, - "timestamp": 7.893019311800182 - }, - { - "x": 1.725766127726464, - "y": 5.522563974256421, - "heading": 1.6517348693937324e-17, - "angularVelocity": 1.236906166709818e-16, - "velocityX": 1.7944198309186505, - "velocityY": -0.28160465204763024, - "timestamp": 7.962407703085673 - }, - { - "x": 1.8710300150870187, - "y": 5.499767198523928, - "heading": 2.797873673898563e-17, - "angularVelocity": 1.6517731335259598e-16, - "velocityX": 2.0934897706863036, - "velocityY": -0.3285387556932655, - "timestamp": 8.031796094371165 - }, - { - "x": 2.0370458831672553, - "y": 5.4737137410442855, - "heading": 4.626535556608686e-17, - "angularVelocity": 2.635400319271784e-16, - "velocityX": 2.3925596919689536, - "velocityY": -0.3754728564386086, - "timestamp": 8.101184485656656 - }, - { - "x": 2.223813730043212, - "y": 5.444403602119362, - "heading": 6.626560991799023e-17, - "angularVelocity": 2.882363175884601e-16, - "velocityX": 2.691629585524184, - "velocityY": -0.42240695283352697, - "timestamp": 8.170572876942147 - }, - { - "x": 2.4313335525083035, - "y": 5.411836782252268, - "heading": 9.137945639838438e-17, - "angularVelocity": 3.6193152862799336e-16, - "velocityX": 2.99069943286728, - "velocityY": -0.46934104197777365, - "timestamp": 8.239961268227638 - }, - { - "x": 2.65960534414942, - "y": 5.376013282449221, - "heading": 1.1821641145167503e-16, - "angularVelocity": 3.8676433675745036e-16, - "velocityX": 3.2897691877869866, - "velocityY": -0.5162751166208155, - "timestamp": 8.30934965951313 - }, - { - "x": 2.9086290857277297, - "y": 5.336933105728788, - "heading": 1.4538386371763974e-16, - "angularVelocity": 3.915273420502118e-16, - "velocityX": 3.5888386654437228, - "velocityY": -0.5632091477613738, - "timestamp": 8.37873805079862 - }, - { - "x": 3.1678798362557536, - "y": 5.296247694105415, - "heading": 1.2970532145326214e-16, - "angularVelocity": -2.259533930819528e-16, - "velocityX": 3.73622656074219, - "velocityY": -0.5863432033749367, - "timestamp": 8.448126442084112 - }, - { - "x": 3.426192624956805, - "y": 5.24997937308727, - "heading": 1.1963113847649017e-16, - "angularVelocity": -1.451854235288838e-16, - "velocityX": 3.7227089995248686, - "velocityY": -0.6668020422577368, - "timestamp": 8.517514833369603 - }, - { - "x": 3.6799776944966163, - "y": 5.18320068927956, - "heading": 1.442058714433213e-16, - "angularVelocity": 3.5416202208408226e-16, - "velocityX": 3.657457174581828, - "velocityY": -0.9623898547086362, - "timestamp": 8.586903224655094 - }, - { - "x": 3.9276087380924305, - "y": 5.096338039673236, - "heading": 1.2678863352315073e-16, - "angularVelocity": -2.510108333324681e-16, - "velocityX": 3.568767613835609, - "velocityY": -1.251832590395956, - "timestamp": 8.656291615940585 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 9.872947837544061e-17, - "angularVelocity": -4.0437823428221097e-16, - "velocityX": 3.457211254829872, - "velocityY": -1.5332571030157462, - "timestamp": 8.725680007226076 - }, - { - "x": 4.284983330328794, - "y": 4.932123965200671, - "heading": 8.520616401533837e-17, - "angularVelocity": -3.905855769726077e-16, - "velocityX": 3.3932258158637794, - "velocityY": -1.6700901615188843, - "timestamp": 8.760303186261309 - }, - { - "x": 4.400064822274762, - "y": 4.86965478591225, - "heading": 8.38275920998652e-17, - "angularVelocity": -3.9816445337684444e-17, - "velocityX": 3.3238280005674867, - "velocityY": -1.8042589106232223, - "timestamp": 8.794926365296542 - }, - { - "x": 4.512559967686789, - "y": 4.802639932227702, - "heading": 8.775922770148411e-17, - "angularVelocity": 1.1355501462237473e-16, - "velocityX": 3.2491281432462653, - "velocityY": -1.935548830347224, - "timestamp": 8.829549544331774 - }, - { - "x": 4.622289335106214, - "y": 4.731186349760053, - "heading": 1.0501758472421099e-16, - "angularVelocity": 4.984625185678289e-16, - "velocityX": 3.169245877386488, - "velocityY": -2.06374990566114, - "timestamp": 8.864172723367007 - }, - { - "x": 4.729077970033436, - "y": 4.65540814642046, - "heading": 1.123300173637084e-16, - "angularVelocity": 2.11200497564256e-16, - "velocityX": 3.0843105082451987, - "velocityY": -2.188655272309947, - "timestamp": 8.89879590240224 - }, - { - "x": 4.83750746703025, - "y": 4.5819970246859665, - "heading": 1.0947895650457139e-16, - "angularVelocity": -8.234543847737888e-17, - "velocityX": 3.131702518895681, - "velocityY": -2.120288309163928, - "timestamp": 8.933419081437473 - }, - { - "x": 4.94878188699609, - "y": 4.512974188419722, - "heading": 1.0086901564782932e-16, - "angularVelocity": -2.486756299293195e-16, - "velocityX": 3.213870680465465, - "velocityY": -1.9935441571094976, - "timestamp": 8.968042260472705 - }, - { - "x": 5.062723794471585, - "y": 4.448449851931572, - "heading": 9.759628790443595e-17, - "angularVelocity": -9.452418393074315e-17, - "velocityX": 3.2909140827174443, - "velocityY": -1.8636167528836842, - "timestamp": 9.002665439507938 - }, - { - "x": 5.179151445124142, - "y": 4.388526983063662, - "heading": 9.598506413460085e-17, - "angularVelocity": -4.6535985854895585e-17, - "velocityX": 3.362708275114807, - "velocityY": -1.7307153917591276, - "timestamp": 9.03728861854317 - }, - { - "x": 5.297879115709132, - "y": 4.333301191879142, - "heading": 9.429439507749846e-17, - "angularVelocity": -4.883055525842942e-17, - "velocityX": 3.429138337186562, - "velocityY": -1.5950525839444993, - "timestamp": 9.071911797578403 - }, - { - "x": 5.418717408418424, - "y": 4.28286058857882, - "heading": 9.044194331783773e-17, - "angularVelocity": -1.112679963830143e-16, - "velocityX": 3.4900981387736523, - "velocityY": -1.4568449433541117, - "timestamp": 9.106534976613636 - }, - { - "x": 5.54147355571485, - "y": 4.237285646363086, - "heading": 8.606851716017984e-17, - "angularVelocity": -1.2631497971943828e-16, - "velocityX": 3.545490354063358, - "velocityY": -1.3163130447772213, - "timestamp": 9.141158155648869 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 8.12244185045661e-17, - "angularVelocity": -1.3990912419348855e-16, - "velocityX": 3.5952265671295462, - "velocityY": -1.1736811275270398, - "timestamp": 9.175781334684102 - }, - { - "x": 5.800884886979515, - "y": 4.158897293130728, - "heading": 7.604329686230843e-17, - "angularVelocity": -1.3984796334066921e-16, - "velocityX": 3.642093094613744, - "velocityY": -1.0189897302407367, - "timestamp": 9.212829579924803 - }, - { - "x": 5.9373078989205395, - "y": 4.126945510909642, - "heading": 7.099587364491845e-17, - "angularVelocity": -1.3623919795923125e-16, - "velocityX": 3.682306977150727, - "velocityY": -0.8624371279529385, - "timestamp": 9.249877825165504 - }, - { - "x": 6.074971576496844, - "y": 4.100852086793861, - "heading": 6.753360793468667e-17, - "angularVelocity": -9.345289332160286e-17, - "velocityX": 3.7157948151635507, - "velocityY": -0.7043093119863827, - "timestamp": 9.286926070406205 - }, - { - "x": 6.213624465766887, - "y": 4.080664678033529, - "heading": 6.709732769415932e-17, - "angularVelocity": -1.177600282260163e-17, - "velocityX": 3.742495450708113, - "velocityY": -0.5448951395450716, - "timestamp": 9.323974315646906 - }, - { - "x": 6.353013304056433, - "y": 4.066420153436822, - "heading": 4.721163336657454e-17, - "angularVelocity": -5.367513143575007e-16, - "velocityX": 3.7623600627760427, - "velocityY": -0.38448581043883095, - "timestamp": 9.361022560887607 - }, - { - "x": 6.4925719820191485, - "y": 4.0581614935185435, - "heading": 3.328638124108273e-17, - "angularVelocity": -3.758680611067538e-16, - "velocityX": 3.766944346648758, - "velocityY": -0.22291635851097177, - "timestamp": 9.398070806128308 - }, - { - "x": 6.627754792512696, - "y": 4.053990782696842, - "heading": 2.356760119630702e-17, - "angularVelocity": -2.6232767457636995e-16, - "velocityX": 3.6488316684169346, - "velocityY": -0.1125751245330268, - "timestamp": 9.435119051369009 - }, - { - "x": 6.757419046645761, - "y": 4.0521449831028535, - "heading": 1.6377013350687517e-17, - "angularVelocity": -1.940871361679193e-16, - "velocityX": 3.4998757239551144, - "velocityY": -0.049821511977060404, - "timestamp": 9.47216729660971 - }, - { - "x": 6.881278674219902, - "y": 4.051770935128315, - "heading": 1.0749401519445311e-17, - "angularVelocity": -1.51899550746631e-16, - "velocityX": 3.3431982208450166, - "velocityY": -0.010096239973268065, - "timestamp": 9.509215541850411 - }, - { - "x": 6.999234961376727, - "y": 4.052402049117112, - "heading": 6.6780180524147395e-18, - "angularVelocity": -1.0989409673710028e-16, - "velocityX": 3.183856249883563, - "velocityY": 0.017034922563732322, - "timestamp": 9.546263787091112 - }, - { - "x": 7.111245945109509, - "y": 4.053749520879657, - "heading": 3.4043054479450288e-18, - "angularVelocity": -8.836349922350792e-17, - "velocityX": 3.0233816205072856, - "velocityY": 0.03637073102354394, - "timestamp": 9.583312032331813 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, - "angularVelocity": -9.188843930234661e-17, - "velocityX": 2.862359859004765, - "velocityY": 0.0504413971839862, - "timestamp": 9.620360277572514 - }, - { - "x": 7.386141990779784, - "y": 4.060115199818634, - "heading": -6.285690223777642e-18, - "angularVelocity": -9.593548047553597e-17, - "velocityX": 2.5770864103628295, - "velocityY": 0.06863424057552284, - "timestamp": 9.685880251977624 - }, - { - "x": 7.536265872308939, - "y": 4.064907354049287, - "heading": -1.0202276969724947e-17, - "angularVelocity": -5.97769883515376e-17, - "velocityX": 2.2912689281735665, - "velocityY": 0.07314035565737474, - "timestamp": 9.751400226382733 - }, - { - "x": 7.667662893519374, - "y": 4.069410994150776, - "heading": -1.2426686282698196e-17, - "angularVelocity": -3.395009435762481e-17, - "velocityX": 2.005449825087065, - "velocityY": 0.06873690263742, - "timestamp": 9.816920200787843 - }, - { - "x": 7.780343830854776, - "y": 4.073216939396029, - "heading": -1.3104250722395789e-17, - "angularVelocity": -1.03413416429204e-17, - "velocityX": 1.7197951977010522, - "velocityY": 0.05808832008572811, - "timestamp": 9.882440175192952 - }, - { - "x": 7.874322400829606, - "y": 4.07602285690707, - "heading": -1.2397928827700396e-17, - "angularVelocity": 1.0780252923201902e-17, - "velocityX": 1.4343499189081053, - "velocityY": 0.04282537556703236, - "timestamp": 9.947960149598062 - }, - { - "x": 7.949612474283929, - "y": 4.07759641137373, - "heading": -1.096542742535041e-17, - "angularVelocity": 2.1863583068378234e-17, - "velocityX": 1.1491163441060581, - "velocityY": 0.024016408445632246, - "timestamp": 10.013480124003172 - }, - { - "x": 8.006227098004604, - "y": 4.077753556862197, - "heading": -9.354890651543402e-18, - "angularVelocity": 2.458085169441397e-17, - "velocityX": 0.8640818961654191, - "velocityY": 0.0023984363530761407, - "timestamp": 10.079000098408281 - }, - { - "x": 8.044178200157203, - "y": 4.076344939580228, - "heading": -5.6627659945325885e-18, - "angularVelocity": 5.635113094762732e-17, - "velocityX": 0.579229502104909, - "velocityY": -0.021499051163544333, - "timestamp": 10.14452007281339 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "angularVelocity": 8.642808619399734e-17, - "velocityX": 0.2945416648589691, - "velocityY": -0.04728304210698771, - "timestamp": 10.2100400472185 - }, - { - "x": 8.06151161206475, - "y": 4.067466722329622, - "heading": 7.499406153547556e-18, - "angularVelocity": 1.0142249804726799e-16, - "velocityX": -0.026574128354115305, - "velocityY": -0.07817228633622807, - "timestamp": 10.283982281901235 - }, - { - "x": 8.0357781795929, - "y": 4.059672922438978, - "heading": 2.0171548175080756e-17, - "angularVelocity": 1.7137894295700475e-16, - "velocityX": -0.34802075677407873, - "velocityY": -0.10540389973451367, - "timestamp": 10.357924516583969 - }, - { - "x": 7.986251299714503, - "y": 4.050185891028673, - "heading": 3.259640252688688e-17, - "angularVelocity": 1.6803460705902343e-16, - "velocityX": -0.6698050186189494, - "velocityY": -0.12830328229881344, - "timestamp": 10.431866751266703 - }, - { - "x": 7.9129066919285, - "y": 4.039390810897977, - "heading": 4.5982022945133473e-17, - "angularVelocity": 1.81028075140569e-16, - "velocityX": -0.9919176516736913, - "velocityY": -0.1459934255032282, - "timestamp": 10.505808985949438 - }, - { - "x": 7.8157231380567715, - "y": 4.027759201442648, - "heading": 5.916672321533067e-17, - "angularVelocity": 1.783108168799044e-16, - "velocityX": -1.314317240866671, - "velocityY": -0.1573067071239705, - "timestamp": 10.579751220632172 - }, - { - "x": 7.694687242510101, - "y": 4.015880811506709, - "heading": 7.241021515042493e-17, - "angularVelocity": 1.7910591950468916e-16, - "velocityX": -1.6368979929535623, - "velocityY": -0.16064418375919837, - "timestamp": 10.653693455314906 - }, - { - "x": 7.549803190746082, - "y": 4.0045128268534045, - "heading": 8.516404983291186e-17, - "angularVelocity": 1.7248376026751216e-16, - "velocityX": -1.9594221406165937, - "velocityY": -0.1537414266972227, - "timestamp": 10.72763568999764 - }, - { - "x": 7.381113647590609, - "y": 3.9946593063008904, - "heading": 9.839417244429125e-17, - "angularVelocity": 1.7892511182976314e-16, - "velocityX": -2.281369286163345, - "velocityY": -0.13325970732143547, - "timestamp": 10.801577924680375 - }, - { - "x": 7.188747477257485, - "y": 3.987706179482555, - "heading": 1.110446332225851e-16, - "angularVelocity": 1.7108572429643452e-16, - "velocityX": -2.6015736629885633, - "velocityY": -0.09403457777777122, - "timestamp": 10.87552015936311 - }, - { - "x": 6.973038504473885, - "y": 3.9856630874408094, - "heading": 1.1423152127106693e-16, - "angularVelocity": 4.309969887337601e-17, - "velocityX": -2.91726337064529, - "velocityY": -0.027630920954873416, - "timestamp": 10.949462394045844 - }, - { - "x": 6.734853840508573, - "y": 3.9916098664093003, - "heading": 1.1357543736188786e-16, - "angularVelocity": -8.872925078647214e-18, - "velocityX": -3.221226204310621, - "velocityY": 0.0804246584378634, - "timestamp": 11.023404628728578 - }, - { - "x": 6.47658832080339, - "y": 4.010431184705039, - "heading": 1.1486840283254658e-16, - "angularVelocity": 1.7486156277295078e-17, - "velocityX": -3.4928011144554563, - "velocityY": 0.25454083686402557, - "timestamp": 11.097346863411312 - }, - { - "x": 6.204860010324235, - "y": 4.048943809403862, - "heading": 1.1487825258491134e-16, - "angularVelocity": 1.3320874674868913e-19, - "velocityX": -3.6748728469603784, - "velocityY": 0.5208474542873691, - "timestamp": 11.171289098094046 - }, - { - "x": 5.932253242931649, - "y": 4.111293853023339, - "heading": 1.0773394940295777e-16, - "angularVelocity": -9.662006041077537e-17, - "velocityX": -3.686753160250877, - "velocityY": 0.8432263899921877, - "timestamp": 11.24523133277678 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 9.686455716091523e-17, - "angularVelocity": -1.4699842774133078e-16, - "velocityX": -3.6014804699029255, - "velocityY": 1.1543500395591086, - "timestamp": 11.319173567459515 - }, - { - "x": 5.543077137245883, - "y": 4.241512673300566, - "heading": 9.161999350902719e-17, - "angularVelocity": -1.5163138697871951e-16, - "velocityX": -3.552563374503243, - "velocityY": 1.2971011802444727, - "timestamp": 11.353761153554894 - }, - { - "x": 5.422090048254055, - "y": 4.291242278322373, - "heading": 8.675864551293739e-17, - "angularVelocity": -1.4055181482408883e-16, - "velocityX": -3.4979916973158245, - "velocityY": 1.4377876757479933, - "timestamp": 11.388348739650272 - }, - { - "x": 5.303183074500814, - "y": 4.34575871765036, - "heading": 8.202370382364629e-17, - "angularVelocity": -1.368971421201274e-16, - "velocityX": -3.437851182367712, - "velocityY": 1.5761851427750155, - "timestamp": 11.42293632574565 - }, - { - "x": 5.186545516053187, - "y": 4.404975197881964, - "heading": 7.882281033080331e-17, - "angularVelocity": -9.254457608045447e-17, - "velocityX": -3.372237603572297, - "velocityY": 1.712073229635336, - "timestamp": 11.45752391184103 - }, - { - "x": 5.072363058490256, - "y": 4.468797441531519, - "heading": 7.597641724828609e-17, - "angularVelocity": -8.229522218370685e-17, - "velocityX": -3.3012554633926476, - "velocityY": 1.845235555715285, - "timestamp": 11.492111497936408 - }, - { - "x": 4.960817475466504, - "y": 4.537123834649348, - "heading": 7.51892087087232e-17, - "angularVelocity": -2.2759857753359948e-17, - "velocityX": -3.225017863812662, - "velocityY": 1.975460008380286, - "timestamp": 11.526699084031787 - }, - { - "x": 4.852086330068725, - "y": 4.609845576159071, - "heading": 7.00186010980759e-17, - "angularVelocity": -1.4949316197981958e-16, - "velocityX": -3.143646541216975, - "velocityY": 2.1025387926519685, - "timestamp": 11.561286670127165 - }, - { - "x": 4.744615410553515, - "y": 4.6844171206048495, - "heading": 6.888474965661026e-17, - "angularVelocity": -3.278203452357036e-17, - "velocityX": -3.10721075529382, - "velocityY": 2.1560204935996916, - "timestamp": 11.595874256222544 - }, - { - "x": 4.634255259438232, - "y": 4.75464211813236, - "heading": 6.027771904333573e-17, - "angularVelocity": -2.4884739251649313e-16, - "velocityX": -3.190744529293135, - "velocityY": 2.0303526627692405, - "timestamp": 11.630461842317922 - }, - { - "x": 4.521181551584758, - "y": 4.820408744704174, - "heading": 5.1990889261648227e-17, - "angularVelocity": -2.395897117201502e-16, - "velocityX": -3.269199172838061, - "velocityY": 1.9014517633713166, - "timestamp": 11.665049428413301 - }, - { - "x": 4.405574295139685, - "y": 4.881612290783718, - "heading": 4.5533169228459885e-17, - "angularVelocity": -1.867062944312063e-16, - "velocityX": -3.3424494015360238, - "velocityY": 1.7695234906179804, - "timestamp": 11.69963701450868 - }, - { - "x": 4.287617535211261, - "y": 4.938155315408326, - "heading": 4.203003409755996e-17, - "angularVelocity": -1.0128301874666137e-16, - "velocityX": -3.410378498318693, - "velocityY": 1.6347779942979137, - "timestamp": 11.734224600604058 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 4.0833427337575637e-17, - "angularVelocity": -3.459642302543387e-17, - "velocityX": -3.4728780863994273, - "velocityY": 1.4974297517257, - "timestamp": 11.768812186699437 - }, - { - "x": 3.9262086603019655, - "y": 5.072405406382895, - "heading": 4.69185026085022e-17, - "angularVelocity": 9.025220132506432e-17, - "velocityX": -3.5787544523378267, - "velocityY": 1.2229891223429616, - "timestamp": 11.83623519889844 - }, - { - "x": 3.6792395049411635, - "y": 5.135860556463708, - "heading": 5.846977032472703e-17, - "angularVelocity": 1.713252988776416e-16, - "velocityX": -3.6629801503359496, - "velocityY": 0.9411497352494828, - "timestamp": 11.903658211097442 - }, - { - "x": 3.4280856473907875, - "y": 5.179929383329658, - "heading": 6.25257434115025e-17, - "angularVelocity": 6.01571029606942e-17, - "velocityX": -3.7250465287591408, - "velocityY": 0.6536169985387049, - "timestamp": 11.971081223296444 - }, - { - "x": 3.174266449354605, - "y": 5.204345315411431, - "heading": 8.981508430066906e-17, - "angularVelocity": 4.0474817127156825e-16, - "velocityX": -3.7645781426529163, - "velocityY": 0.3621305439411197, - "timestamp": 12.038504235495447 - }, - { - "x": 2.919317391668449, - "y": 5.208960723831647, - "heading": 3.637405660157601e-17, - "angularVelocity": -7.926229629337798e-16, - "velocityX": -3.781335917381794, - "velocityY": 0.06845449750291784, - "timestamp": 12.10592724769445 - }, - { - "x": 2.6778199582724502, - "y": 5.213921183762201, - "heading": 2.780922480909488e-17, - "angularVelocity": -1.270312837495845e-16, - "velocityX": -3.581825040435896, - "velocityY": 0.07357220878699586, - "timestamp": 12.173350259893452 - }, - { - "x": 2.4555441151084385, - "y": 5.223767512233129, - "heading": 2.258282570608997e-17, - "angularVelocity": -7.751654714759439e-17, - "velocityX": -3.2967355790624753, - "velocityY": 0.14603809811799462, - "timestamp": 12.240773272092454 - }, - { - "x": 2.2524898779476, - "y": 5.238499719208596, - "heading": 1.8179159660483545e-17, - "angularVelocity": -6.531399151152503e-17, - "velocityX": -3.0116458837750555, - "velocityY": 0.2185041352347682, - "timestamp": 12.308196284291457 - }, - { - "x": 2.068657252210669, - "y": 5.258117807366149, - "heading": 1.4559970052520395e-17, - "angularVelocity": -5.367884777868486e-17, - "velocityX": -2.726556108088765, - "velocityY": 0.29097021206423906, - "timestamp": 12.37561929649046 - }, - { - "x": 1.9040462406637386, - "y": 5.282621777825342, - "heading": 1.0447672435760193e-17, - "angularVelocity": -6.099249326267761e-17, - "velocityX": -2.441466291376519, - "velocityY": 0.3634363054985942, - "timestamp": 12.443042308689462 - }, - { - "x": 1.7586568449915738, - "y": 5.312011631159126, - "heading": 6.39006439635328e-18, - "angularVelocity": -6.018135214654068e-17, - "velocityX": -2.156376449676296, - "velocityY": 0.4359024074308457, - "timestamp": 12.510465320888464 - }, - { - "x": 1.6324890663305314, - "y": 5.34628736769762, - "heading": 2.9527164791572072e-18, - "angularVelocity": -5.0981820681006015e-17, - "velocityX": -1.8712865911219376, - "velocityY": 0.508368514259319, - "timestamp": 12.577888333087467 - }, - { - "x": 1.5255429054998768, - "y": 5.385448987646794, - "heading": 6.750997693764207e-19, - "angularVelocity": -3.378099903552108e-17, - "velocityX": -1.5861967204164489, - "velocityY": 0.5808346241426653, - "timestamp": 12.64531134528647 - }, - { - "x": 1.437818363118664, - "y": 5.4294964911430075, - "heading": 3.7918321325869045e-19, - "angularVelocity": -4.388954848629936e-18, - "velocityX": -1.301106840529317, - "velocityY": 0.6533007360484864, - "timestamp": 12.712734357485472 - }, - { - "x": 1.3693154396712461, - "y": 5.478429878281045, - "heading": 1.633332658934955e-18, - "angularVelocity": 1.8601207579968088e-17, - "velocityX": -1.0160169534583994, - "velocityY": 0.7257668493601166, - "timestamp": 12.780157369684474 - }, - { - "x": 1.3200341355468752, - "y": 5.532249149129769, - "heading": 3.8843995459700536e-19, - "angularVelocity": -1.8463914088315652e-17, - "velocityX": -0.7309270606141895, - "velocityY": 0.798232963693069, - "timestamp": 12.847580381883477 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -5.761237033299429e-18, - "velocityX": -0.4458371630310619, - "velocityY": 0.8706990788013996, - "timestamp": 12.915003394082479 - }, - { - "x": 1.2868335136673428, - "y": 5.68644048793425, - "heading": -1.3391788419363526e-18, - "angularVelocity": -1.3686362473728894e-17, - "velocityX": -0.03210027395244838, - "velocityY": 0.9758655723245745, - "timestamp": 13.012851076079642 - }, - { - "x": 1.3242854311298142, - "y": 5.791775499822582, - "heading": 2.339658407779995e-18, - "angularVelocity": 3.759759224271494e-17, - "velocityX": 0.3827573295354856, - "velocityY": 1.076520258204863, - "timestamp": 13.110698758076804 - }, - { - "x": 1.4024881161679177, - "y": 5.906284066156856, - "heading": 5.091444284753208e-18, - "angularVelocity": 2.812315857202659e-17, - "velocityX": 0.7992287956333111, - "velocityY": 1.1702736743175466, - "timestamp": 13.208546440073967 - }, - { - "x": 1.5216854983005004, - "y": 6.028805982329465, - "heading": 6.6702155316476646e-18, - "angularVelocity": 1.6134988737668432e-17, - "velocityX": 1.218193213162059, - "velocityY": 1.2521698385881244, - "timestamp": 13.30639412207113 - }, - { - "x": 1.682281091295048, - "y": 6.156892671597042, - "heading": 8.737231811568005e-18, - "angularVelocity": 2.1124836460875052e-17, - "velocityX": 1.6412815277443633, - "velocityY": 1.3090416313724325, - "timestamp": 13.404241804068292 - }, - { - "x": 1.884561264278339, - "y": 6.2823011062588545, - "heading": 4.474450456421642e-18, - "angularVelocity": -4.3565481249463154e-17, - "velocityX": 2.06729652511499, - "velocityY": 1.2816699599020622, - "timestamp": 13.502089486065454 - }, - { - "x": 2.074597433027926, - "y": 6.367773862583125, - "heading": 1.3148438373880832e-18, - "angularVelocity": -3.229107274750916e-17, - "velocityX": 1.9421632160391744, - "velocityY": 0.8735286782445029, - "timestamp": 13.599937168062617 - }, - { - "x": 2.2289686427209534, - "y": 6.431502913374048, - "heading": 4.376002899474807e-18, - "angularVelocity": 3.128494207400489e-17, - "velocityX": 1.5776685409625097, - "velocityY": 0.6513087432441247, - "timestamp": 13.69778485005978 - }, - { - "x": 2.345509714236833, - "y": 6.4775215317101145, - "heading": 6.666442968036081e-18, - "angularVelocity": 2.3408220019295113e-17, - "velocityX": 1.1910458085175606, - "velocityY": 0.4703087226675544, - "timestamp": 13.795632532056942 - }, - { - "x": 2.4235057296596247, - "y": 6.507444880215651, - "heading": 4.695809387720839e-18, - "angularVelocity": -2.013980853353183e-17, - "velocityX": 0.7971166391560869, - "velocityY": 0.30581560947355857, - "timestamp": 13.893480214054104 - }, - { - "x": 2.462606906890869, - "y": 6.522137641906738, - "heading": 1.18409559006069e-37, - "angularVelocity": -4.799101310235197e-17, - "velocityX": 0.3996127085808573, - "velocityY": 0.15015952745322653, - "timestamp": 13.991327896051267 - }, - { - "x": 2.462606906890869, - "y": 6.522137641906738, - "heading": 7.445601745662792e-38, - "angularVelocity": -4.492041573884033e-37, - "velocityX": -6.88570869510922e-34, - "velocityY": -3.3118407683240263e-34, - "timestamp": 14.08917557804843 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 4.7251350507187945e-25, + "angularVelocity": 2.5513158875858104e-25, + "velocityX": 5.636670431941991e-25, + "velocityY": -5.06363269097065e-25, + "timestamp": 0 + }, + { + "x": 1.3069477629998796, + "y": 5.574973108256567, + "heading": -0.008994433993404482, + "angularVelocity": -0.12277149941396481, + "velocityX": 0.23168094376881843, + "velocityY": -0.2181388326987634, + "timestamp": 0.07326157973420823 + }, + { + "x": 1.3408944736683446, + "y": 5.5430109339807085, + "heading": -0.02697887908003383, + "angularVelocity": -0.2454826274819163, + "velocityX": 0.46336307231734836, + "velocityY": -0.4362747075863933, + "timestamp": 0.14652315946841646 + }, + { + "x": 1.3918148440520437, + "y": 5.495068027801086, + "heading": -0.05394057956084241, + "angularVelocity": -0.3680196438382188, + "velocityX": 0.6950487631912567, + "velocityY": -0.6544072125329379, + "timestamp": 0.2197847392026247 + }, + { + "x": 1.4597093144715716, + "y": 5.431144700806779, + "heading": -0.0898597121983473, + "angularVelocity": -0.4902860785669502, + "velocityX": 0.9267404643176941, + "velocityY": -0.8725354712008585, + "timestamp": 0.2930463189368329 + }, + { + "x": 1.5445785095359612, + "y": 5.351241370153018, + "heading": -0.13471107907964566, + "angularVelocity": -0.6122085688572148, + "velocityX": 1.1584406911821108, + "velocityY": -1.090658035816974, + "timestamp": 0.36630789867104113 + }, + { + "x": 1.646423251209635, + "y": 5.255358608941595, + "heading": -0.18846598255428795, + "angularVelocity": -0.733739344273822, + "velocityX": 1.390152137630185, + "velocityY": -1.308772777754508, + "timestamp": 0.43956947840524935 + }, + { + "x": 1.7652446225931595, + "y": 5.143497220924912, + "heading": -0.2510934067365697, + "angularVelocity": -0.854846761556235, + "velocityX": 1.6218783680969795, + "velocityY": -1.5268765486973532, + "timestamp": 0.5128310581394576 + }, + { + "x": 1.9010443771576746, + "y": 5.0156584956901185, + "heading": -0.3225556175785802, + "angularVelocity": -0.975439119676018, + "velocityX": 1.8536285329526645, + "velocityY": -1.7449627171375512, + "timestamp": 0.5860926378736658 + }, + { + "x": 2.053859595231987, + "y": 4.871864096254743, + "heading": -0.40226717784175786, + "angularVelocity": -1.0880404238124515, + "velocityX": 2.0858848338886964, + "velocityY": -1.9627531914689686, + "timestamp": 0.659354217607874 + }, + { + "x": 2.189698919868216, + "y": 4.744047518376405, + "heading": -0.4731117935721764, + "angularVelocity": -0.9670091197520122, + "velocityX": 1.8541686533248656, + "velocityY": -1.7446604119383569, + "timestamp": 0.7326157973420823 + }, + { + "x": 2.3085606861249848, + "y": 4.632207510568855, + "heading": -0.5351127191107611, + "angularVelocity": -0.8462952309180741, + "velocityX": 1.6224297467785629, + "velocityY": -1.5265847148437626, + "timestamp": 0.8058773770762905 + }, + { + "x": 2.410444090424028, + "y": 4.5363433228552275, + "heading": -0.5882784512046502, + "angularVelocity": -0.7256973202976708, + "velocityX": 1.3906798716144855, + "velocityY": -1.3085192547228937, + "timestamp": 0.8791389568104987 + }, + { + "x": 2.495348600905127, + "y": 4.456454404569983, + "heading": -0.6326127919889597, + "angularVelocity": -0.6051513077544012, + "velocityX": 1.158922736707715, + "velocityY": -1.0904613110320627, + "timestamp": 0.9524005365447069 + }, + { + "x": 2.563273836206689, + "y": 4.392540340008763, + "heading": -0.6681173722323603, + "angularVelocity": -0.4846275547457583, + "velocityX": 0.927160396322236, + "velocityY": -0.8724090415890471, + "timestamp": 1.0256621162789152 + }, + { + "x": 2.6142195241617716, + "y": 4.344600823169547, + "heading": -0.6947927082081052, + "angularVelocity": -0.364110848722108, + "velocityX": 0.6953943409343986, + "velocityY": -0.6543609489877007, + "timestamp": 1.0989236960131235 + }, + { + "x": 2.6481854848906172, + "y": 4.312635645183918, + "heading": -0.712638738481162, + "angularVelocity": -0.24359330412750974, + "velocityX": 0.46362583023836135, + "velocityY": -0.4363157073816743, + "timestamp": 1.1721852757473317 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": -0.12307101003919331, + "velocityX": 0.2318560205907196, + "velocityY": -0.2182720818926514, + "timestamp": 1.2454468554815399 + }, + { + "x": 2.6629437104502958, + "y": 4.2987285580297225, + "heading": -0.7207010011457456, + "angularVelocity": 0.011651349127194545, + "velocityX": -0.027206592683106, + "velocityY": 0.025447590710046028, + "timestamp": 1.3273355655318961 + }, + { + "x": 2.6395015930145194, + "y": 4.320770481758526, + "heading": -0.7087160822692053, + "angularVelocity": 0.14635618108003276, + "velocityX": -0.28626800228457533, + "velocityY": 0.2691692629576105, + "timestamp": 1.4092242755822524 + }, + { + "x": 2.594845480270507, + "y": 4.362770756636002, + "heading": -0.6857017148059629, + "angularVelocity": 0.2810444498281884, + "velocityX": -0.545326855393761, + "velocityY": 0.5128945718115293, + "timestamp": 1.4911129856326086 + }, + { + "x": 2.52897569111822, + "y": 4.424729822240869, + "heading": -0.6516589309216121, + "angularVelocity": 0.415720114084306, + "velocityX": -0.8043818141936927, + "velocityY": 0.7566252486669522, + "timestamp": 1.5730016956829649 + }, + { + "x": 2.4418926576621733, + "y": 4.506648271460754, + "heading": -0.6065878923349923, + "angularVelocity": 0.550393803479186, + "velocityX": -1.0634314962648228, + "velocityY": 1.0003631656856, + "timestamp": 1.6548904057333211 + }, + { + "x": 2.333596939298986, + "y": 4.608526864693725, + "heading": -0.5504868103027082, + "angularVelocity": 0.6850893364639103, + "velocityX": -1.322474347154724, + "velocityY": 1.2441103684539934, + "timestamp": 1.7367791157836774 + }, + { + "x": 2.2040892544680513, + "y": 4.7303665407944155, + "heading": -0.48334990263012423, + "angularVelocity": 0.8198554798494097, + "velocityX": -1.5815084246814508, + "velocityY": 1.4878690362293745, + "timestamp": 1.8186678258340336 + }, + { + "x": 2.0533705390582235, + "y": 4.872168408061871, + "heading": -0.40516374725682197, + "angularVelocity": 0.9547855293510289, + "velocityX": -1.8405310734183675, + "velocityY": 1.7316412382153215, + "timestamp": 1.9005565358843899 + }, + { + "x": 1.8837222142672279, + "y": 5.0318957926604115, + "heading": -0.31535335305240253, + "angularVelocity": 1.0967371955082925, + "velocityX": -2.0716937009591816, + "velocityY": 1.9505421992887515, + "timestamp": 1.9824452459347461 + }, + { + "x": 1.7352824931641453, + "y": 5.171658722140381, + "heading": -0.23665618820579565, + "angularVelocity": 0.9610258214864199, + "velocityX": -1.8127006886761512, + "velocityY": 1.7067423506124824, + "timestamp": 2.064333955985102 + }, + { + "x": 1.608050236293127, + "y": 5.291456457180838, + "heading": -0.1691154615021004, + "angularVelocity": 0.8247867949337834, + "velocityX": -1.5537215910810984, + "velocityY": 1.4629334735739428, + "timestamp": 2.146222666035458 + }, + { + "x": 1.5020244624765908, + "y": 5.391288489894511, + "heading": -0.11277393636454519, + "angularVelocity": 0.6880255544739787, + "velocityX": -1.2947544753280062, + "velocityY": 1.2191183943706372, + "timestamp": 2.228111376085814 + }, + { + "x": 1.4172044018055678, + "y": 5.471154488114474, + "heading": -0.06766925265776652, + "angularVelocity": 0.5508046674449045, + "velocityX": -1.03579676146888, + "velocityY": 0.9752992588459369, + "timestamp": 2.3100000861361702 + }, + { + "x": 1.353589523967397, + "y": 5.531054232222511, + "heading": -0.033829973036366284, + "angularVelocity": 0.4132349819723778, + "velocityX": -0.7768455236265391, + "velocityY": 0.7314774414104627, + "timestamp": 2.3918887961865263 + }, + { + "x": 1.3111795494907175, + "y": 5.570987556285165, + "heading": -0.011272655486454928, + "angularVelocity": 0.27546309541376424, + "velocityX": -0.517897698603385, + "velocityY": 0.4876535976460953, + "timestamp": 2.4737775062368823 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -7.670908759441255e-25, + "angularVelocity": 0.13765823737512747, + "velocityX": -0.2589502070873256, + "velocityY": 0.24382784200668048, + "timestamp": 2.5556662162872383 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -3.41171038639136e-25, + "angularVelocity": 8.93245678861437e-25, + "velocityX": 7.394213704779868e-19, + "velocityY": -2.5191717110574877e-20, + "timestamp": 2.6375549263375944 + }, + { + "x": 1.3279764533229341, + "y": 5.589659577998647, + "heading": -1.0733184537140863e-18, + "angularVelocity": -1.1496982275828825e-17, + "velocityX": 0.4070632249719813, + "velocityY": -0.013868617572964747, + "timestamp": 2.7309114368906817 + }, + { + "x": 1.4039804570223795, + "y": 5.587070126540843, + "heading": -3.1026019980884896e-18, + "angularVelocity": -2.173692641491287e-17, + "velocityX": 0.8141264412001065, + "velocityY": -0.02773723484802693, + "timestamp": 2.824267947443769 + }, + { + "x": 1.5179864609388978, + "y": 5.5831859494097635, + "heading": -5.833562477390286e-18, + "angularVelocity": -2.9253027311160176e-17, + "velocityX": 1.2211896443118286, + "velocityY": -0.041605851676214084, + "timestamp": 2.9176244579968564 + }, + { + "x": 1.669994463031499, + "y": 5.578007046674942, + "heading": -9.265282118286952e-18, + "angularVelocity": -3.6759297272081945e-17, + "velocityX": 1.628252825561226, + "velocityY": -0.05547446775955324, + "timestamp": 3.010980968549944 + }, + { + "x": 1.8600044592176233, + "y": 5.571533418475472, + "heading": -1.3542122810335315e-17, + "angularVelocity": -4.5811917888604e-17, + "velocityX": 2.035315963079774, + "velocityY": -0.0693430823529852, + "timestamp": 3.104337479103031 + }, + { + "x": 2.08801643724496, + "y": 5.563765065228789, + "heading": -1.806715455043103e-17, + "angularVelocity": -4.8470447549846784e-17, + "velocityX": 2.442378969356152, + "velocityY": -0.08321169247500493, + "timestamp": 3.1976939896561185 + }, + { + "x": 2.278069539423553, + "y": 5.557289968410962, + "heading": -2.8931134060980015e-18, + "angularVelocity": 1.6253865304001022e-16, + "velocityX": 2.0357776983375815, + "velocityY": -0.06935881364315644, + "timestamp": 3.291050500209206 + }, + { + "x": 2.430120659733151, + "y": 5.552109596641294, + "heading": -3.268581465491534e-18, + "angularVelocity": -4.0218733262966905e-18, + "velocityX": 1.6287146917636088, + "velocityY": -0.05549020351099765, + "timestamp": 3.384407010762293 + }, + { + "x": 2.5441697859422803, + "y": 5.548223950336512, + "heading": -3.490008764973647e-18, + "angularVelocity": -2.371846303430449e-18, + "velocityX": 1.2216515541706623, + "velocityY": -0.04162158891503094, + "timestamp": 3.4777635213153806 + }, + { + "x": 2.62021691397224, + "y": 5.545633029635576, + "heading": -3.003302345401883e-18, + "angularVelocity": 5.213417288227865e-18, + "velocityX": 0.8145883728881999, + "velocityY": -0.027752972830565246, + "timestamp": 3.571120031868468 + }, + { + "x": 2.6582620417833907, + "y": 5.544336834607976, + "heading": -2.0773852345449385e-18, + "angularVelocity": 9.91807795339057e-18, + "velocityX": 0.4075251697578793, + "velocityY": -0.013884356001744477, + "timestamp": 3.6644765424215553 + }, + { + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 6.735492092706879e-29, + "angularVelocity": 2.2252173720073512e-17, + "velocityX": 0.00046195351785172767, + "velocityY": -0.00001573872627686245, + "timestamp": 3.7578330529746427 + }, + { + "x": 2.6203319236037554, + "y": 5.545629111264917, + "heading": 2.4136302284603065e-18, + "angularVelocity": 2.584901393114583e-17, + "velocityX": -0.40667826371536864, + "velocityY": 0.013855501968015705, + "timestamp": 3.851207221048392 + }, + { + "x": 2.544342300798434, + "y": 5.548218072766594, + "heading": 5.5338149846267035e-18, + "angularVelocity": 3.341593018331809e-17, + "velocityX": -0.8138184722063877, + "velocityY": 0.02772674236446206, + "timestamp": 3.9445813891221415 + }, + { + "x": 2.4303363009603927, + "y": 5.552102249758721, + "heading": 9.360515374591917e-18, + "angularVelocity": 4.0982429991846804e-17, + "velocityX": -1.220958667583486, + "velocityY": 0.04159798231411795, + "timestamp": 4.0379555571958905 + }, + { + "x": 2.278313926130621, + "y": 5.557281642171763, + "heading": 1.3522883806487307e-17, + "angularVelocity": 4.457730047824434e-17, + "velocityX": -1.6280988411023956, + "velocityY": 0.055469221519066796, + "timestamp": 4.13132972526964 + }, + { + "x": 2.088275180391678, + "y": 5.563756249866626, + "heading": 1.8245422432915976e-17, + "angularVelocity": 5.057649913145605e-17, + "velocityX": -2.03523897089873, + "velocityY": 0.06934045923439035, + "timestamp": 4.224703893343389 + }, + { + "x": 1.860220075995873, + "y": 5.571526072425875, + "heading": 6.8469520838322655e-18, + "angularVelocity": -1.2207305633045037e-16, + "velocityX": -2.4423789694777343, + "velocityY": 0.08321169247914799, + "timestamp": 4.318078061417139 + }, + { + "x": 1.6701382161075125, + "y": 5.578002149017001, + "heading": -6.786620946288625e-18, + "angularVelocity": -1.460101118846944e-16, + "velocityX": -2.035700706197767, + "velocityY": 0.06935619052596638, + "timestamp": 4.411452229490888 + }, + { + "x": 1.5180727149037914, + "y": 5.583183010742795, + "heading": -4.3005972299368415e-18, + "angularVelocity": 2.6624319168550005e-17, + "velocityX": -1.6285607073212787, + "velocityY": 0.05548495727107347, + "timestamp": 4.504826397564638 + }, + { + "x": 1.4040235846161857, + "y": 5.5870686571865305, + "heading": -1.9657241654870744e-18, + "angularVelocity": 2.5005556186566034e-17, + "velocityX": -1.221420577450572, + "velocityY": 0.04161371955321608, + "timestamp": 4.598200565638387 + }, + { + "x": 1.3279908293233964, + "y": 5.589659088209248, + "heading": -7.083875411773879e-19, + "angularVelocity": 1.3465572054358342e-17, + "velocityX": -0.8142804038986078, + "velocityY": 0.027742480347141066, + "timestamp": 4.6915747337121365 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -9.307960730995794e-22, + "angularVelocity": 7.576578596147462e-18, + "velocityX": -0.40714020850291777, + "velocityY": 0.013871240396851729, + "timestamp": 4.784948901785886 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -2.878097861680603e-22, + "angularVelocity": 6.886148795301622e-21, + "velocityX": 9.386048457152016e-19, + "velocityY": 1.5189228236971111e-19, + "timestamp": 4.878323069859635 + }, + { + "x": 1.3078091854990117, + "y": 5.606755784251363, + "heading": 0.009999675661372767, + "angularVelocity": 0.13495422890571762, + "velocityX": 0.24069508999867756, + "velocityY": 0.21325457844806686, + "timestamp": 4.952419863583371 + }, + { + "x": 1.3434787349490442, + "y": 5.638358397074998, + "heading": 0.0299962311234221, + "angularVelocity": 0.26987072526518463, + "velocityX": 0.48139126752263756, + "velocityY": 0.426504457688991, + "timestamp": 5.0265166573071065 + }, + { + "x": 1.3969834022405148, + "y": 5.685761716114703, + "heading": 0.0599780758079259, + "angularVelocity": 0.40463079679653674, + "velocityX": 0.7220915319353576, + "velocityY": 0.6397485863753444, + "timestamp": 5.100613451030842 + }, + { + "x": 1.4683236980324899, + "y": 5.748965196655092, + "heading": 0.09992700807120888, + "angularVelocity": 0.5391452214819116, + "velocityX": 0.9627986881316648, + "velocityY": 0.8529853636587607, + "timestamp": 5.174710244754578 + }, + { + "x": 1.5575003119272133, + "y": 5.82796813420515, + "heading": 0.14982111213912733, + "angularVelocity": 0.673363873934206, + "velocityX": 1.2035151511037259, + "velocityY": 1.066212633229638, + "timestamp": 5.248807038478313 + }, + { + "x": 1.6645140669394536, + "y": 5.922769634696042, + "heading": 0.20963811089831047, + "angularVelocity": 0.8072818775695793, + "velocityX": 1.444242721368388, + "velocityY": 1.2794278365721479, + "timestamp": 5.322903832202049 + }, + { + "x": 1.7893658604543141, + "y": 6.033368608876843, + "heading": 0.27935867676309706, + "angularVelocity": 0.9409390388028722, + "velocityX": 1.684982402617327, + "velocityY": 1.4926283395359905, + "timestamp": 5.397000625925784 + }, + { + "x": 1.9320566039380949, + "y": 6.1597637996451535, + "heading": 0.35896895992945943, + "angularVelocity": 1.074409285011486, + "velocityX": 1.925734384888401, + "velocityY": 1.705811876821096, + "timestamp": 5.47109741964952 + }, + { + "x": 2.0925871914348457, + "y": 6.3019538297380615, + "heading": 0.4484614236084208, + "angularVelocity": 1.207777815766594, + "velocityX": 2.1664984330533477, + "velocityY": 1.9189768267578884, + "timestamp": 5.5451942133732555 + }, + { + "x": 2.235355236977834, + "y": 6.4283142227315375, + "heading": 0.5276060335900633, + "angularVelocity": 1.0681246246190785, + "velocityX": 1.926777642704601, + "velocityY": 1.7053422509022635, + "timestamp": 5.619291007096991 + }, + { + "x": 2.3602823931901473, + "y": 6.538881568185102, + "heading": 0.5968686960571826, + "angularVelocity": 0.9347592383735241, + "velocityX": 1.685999487077598, + "velocityY": 1.4922014826418244, + "timestamp": 5.693387800820727 + }, + { + "x": 2.467368142896788, + "y": 6.633656542567922, + "heading": 0.6562494243147645, + "angularVelocity": 0.8013940316902027, + "velocityX": 1.4452143517289322, + "velocityY": 1.2790698439150068, + "timestamp": 5.767484594544462 + }, + { + "x": 2.556612071509118, + "y": 6.712639660738819, + "heading": 0.7057479102710464, + "angularVelocity": 0.6680246670433977, + "velocityX": 1.2044236211497805, + "velocityY": 1.0659451536510254, + "timestamp": 5.841581388268198 + }, + { + "x": 2.62801383746604, + "y": 6.7758313131963215, + "heading": 0.7453639172984617, + "angularVelocity": 0.5346521089039379, + "velocityX": 0.9636282809096683, + "velocityY": 0.8528257335008133, + "timestamp": 5.915678181991933 + }, + { + "x": 2.681573172842232, + "y": 6.823231783886196, + "heading": 0.7750973380985307, + "angularVelocity": 0.4012781026791483, + "velocityX": 0.7228293247867619, + "velocityY": 0.6397101454430356, + "timestamp": 5.989774975715669 + }, + { + "x": 2.7172898901971134, + "y": 6.854841258399732, + "heading": 0.7949482053115821, + "angularVelocity": 0.2679045369636881, + "velocityX": 0.48202783899191026, + "velocityY": 0.42659706209947756, + "timestamp": 6.063871769439404 + }, + { + "x": 2.735163889126497, + "y": 6.870659826887679, + "heading": 0.8049167147138492, + "angularVelocity": 0.1345336134169838, + "velocityX": 0.24122499815612897, + "velocityY": 0.2134851954178351, + "timestamp": 6.13796856316314 + }, + { + "x": 2.73519515991211, + "y": 6.870687484741211, + "heading": 0.8050032485420815, + "angularVelocity": 0.001167848484172869, + "velocityX": 0.00042202616390890065, + "velocityY": 0.0003732665361331451, + "timestamp": 6.2120653568868756 + }, + { + "x": 2.717376572124918, + "y": 6.854917750576811, + "heading": 0.7952043981716957, + "angularVelocity": -0.13221717973766972, + "velocityX": -0.24042855386900916, + "velocityY": -0.2127831018556256, + "timestamp": 6.286177135627184 + }, + { + "x": 2.6817083093652894, + "y": 6.823350442039261, + "heading": 0.7755197862739446, + "angularVelocity": -0.26560706317314603, + "velocityX": -0.4812765712264476, + "velocityY": -0.42594185531780887, + "timestamp": 6.360288914367492 + }, + { + "x": 2.6281906360223433, + "y": 6.775985280617999, + "heading": 0.7459494504462719, + "angularVelocity": -0.39899643930135414, + "velocityX": -0.722121021146653, + "velocityY": -0.6391043667597266, + "timestamp": 6.4344006931078 + }, + { + "x": 2.5568238807540884, + "y": 6.712821877208373, + "heading": 0.7064939157358233, + "angularVelocity": -0.5323787308992164, + "velocityX": -0.9629610364410259, + "velocityY": -0.8522721284420127, + "timestamp": 6.508512471848108 + }, + { + "x": 2.4676084291144194, + "y": 6.633859715258306, + "heading": 0.6571542600389383, + "angularVelocity": -0.6657464782996794, + "velocityX": -1.203795849405873, + "velocityY": -1.0654468600295754, + "timestamp": 6.582624250588416 + }, + { + "x": 2.360544725957208, + "y": 6.5390981238541, + "heading": 0.5979319754390301, + "angularVelocity": -0.7990940928219623, + "velocityX": -1.4446246598987866, + "velocityY": -1.2786306443440876, + "timestamp": 6.656736029328724 + }, + { + "x": 2.235633297506525, + "y": 6.4285362427751105, + "heading": 0.528828220928822, + "angularVelocity": -0.9324260689026471, + "velocityX": -1.6854463699809454, + "velocityY": -1.4918260357291568, + "timestamp": 6.730847808069032 + }, + { + "x": 2.092874804641344, + "y": 6.302172989716305, + "heading": 0.44984178028878635, + "angularVelocity": -1.0657744555937454, + "velocityX": -1.9262591627360037, + "velocityY": -1.7050360307986958, + "timestamp": 6.80495958680934 + }, + { + "x": 1.9322701422450308, + "y": 6.160007052413367, + "heading": 0.360964763590885, + "angularVelocity": -1.1992293021239147, + "velocityX": -2.1670598807118275, + "velocityY": -1.9182637324236342, + "timestamp": 6.879071365549648 + }, + { + "x": 1.7895335350373793, + "y": 6.033554256495722, + "heading": 0.28090785571409216, + "angularVelocity": -1.0802184111289128, + "velocityX": -1.9259638566739734, + "velocityY": -1.7062442443966088, + "timestamp": 6.953183144289956 + }, + { + "x": 1.6646412378227142, + "y": 5.922906343422477, + "heading": 0.21077983102217354, + "angularVelocity": -0.9462466814843488, + "velocityX": -1.6851882296914678, + "velocityY": -1.4929868767684242, + "timestamp": 7.027294923030264 + }, + { + "x": 1.5575922863855105, + "y": 5.828064123391515, + "heading": 0.15060992914364188, + "angularVelocity": -0.8118804176778789, + "velocityX": -1.4444256129961441, + "velocityY": -1.279718577033419, + "timestamp": 7.101406701770572 + }, + { + "x": 1.468385833968275, + "y": 5.749028195054374, + "heading": 0.1004277246519939, + "angularVelocity": -0.67711510025268, + "velocityX": -1.2036744217112965, + "velocityY": -1.0664421996142908, + "timestamp": 7.17551848051088 + }, + { + "x": 1.397021198753748, + "y": 5.685798985978408, + "heading": 0.06025970584312968, + "angularVelocity": -0.5419923727591991, + "velocityX": -0.9629324302766086, + "velocityY": -0.853160052972471, + "timestamp": 7.249630259251188 + }, + { + "x": 1.3434978969393505, + "y": 5.638376797369784, + "heading": 0.030126208310740164, + "angularVelocity": -0.4065952544193969, + "velocityX": -0.7221969668538984, + "velocityY": -0.6398738421161722, + "timestamp": 7.323742037991496 + }, + { + "x": 1.307815661583592, + "y": 5.6067618472200556, + "heading": 0.010039053898985358, + "angularVelocity": -0.2710386223779815, + "velocityX": -0.4814651053079036, + "velocityY": -0.4265846898710802, + "timestamp": 7.397853816731804 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.285773257756444e-22, + "angularVelocity": -0.13545827761283144, + "velocityX": -0.24073380536507133, + "velocityY": -0.21329326791617506, + "timestamp": 7.4719655954721125 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 2.570159716602358e-22, + "angularVelocity": 4.6040759154316275e-21, + "velocityX": 1.4250190224081253e-19, + "velocityY": 1.259493468984856e-19, + "timestamp": 7.5460773742124205 + }, + { + "x": 1.310734152886531, + "y": 5.587725049797117, + "heading": -4.624423370992563e-19, + "angularVelocity": -6.667715053540946e-18, + "velocityX": 0.2991570561378951, + "velocityY": -0.04653506643871116, + "timestamp": 7.615471364410994 + }, + { + "x": 1.3522535562378788, + "y": 5.581266541953786, + "heading": -1.1759601942239705e-18, + "angularVelocity": -1.0282127531279589e-17, + "velocityX": 0.5983141080738859, + "velocityY": -0.09307013222398468, + "timestamp": 7.684865354609568 + }, + { + "x": 1.4145326607692024, + "y": 5.571578780265876, + "heading": 1.4139400808476166e-18, + "angularVelocity": 3.732167969674453e-17, + "velocityX": 0.8974711549675912, + "velocityY": -0.13960519722513298, + "timestamp": 7.754259344808141 + }, + { + "x": 1.4975714660528414, + "y": 5.55866176479989, + "heading": 7.102195539243054e-18, + "angularVelocity": 8.197043349311479e-17, + "velocityX": 1.196628195698503, + "velocityY": -0.18614026126790603, + "timestamp": 7.823653335006715 + }, + { + "x": 1.6013699715542196, + "y": 5.542515495638963, + "heading": 1.3318887961611829e-17, + "angularVelocity": 8.958545840321423e-17, + "velocityX": 1.4957852287259228, + "velocityY": -0.23267532411270994, + "timestamp": 7.893047325205289 + }, + { + "x": 1.7259281765860246, + "y": 5.523139972889977, + "heading": 1.841155729782659e-17, + "angularVelocity": 7.338775766662924e-17, + "velocityX": 1.7949422518488518, + "velocityY": -0.27921038541726767, + "timestamp": 7.962441315403862 + }, + { + "x": 1.8712460802318407, + "y": 5.500535196695444, + "heading": 2.3529721894456916e-17, + "angularVelocity": 7.37551563468695e-17, + "velocityX": 2.094099261765793, + "velocityY": -0.32574544466816385, + "timestamp": 8.031835305602437 + }, + { + "x": 2.037323681208685, + "y": 5.4747011672548815, + "heading": 2.8673364146250235e-17, + "angularVelocity": 7.412230132714935e-17, + "velocityX": 2.3932562531943504, + "velocityY": -0.37228050104393373, + "timestamp": 8.10122929580101 + }, + { + "x": 2.2241609775920828, + "y": 5.445637884867564, + "heading": 3.384245946066316e-17, + "angularVelocity": 7.448909191732528e-17, + "velocityX": 2.6924132168903303, + "velocityY": -0.4188155531070138, + "timestamp": 8.170623285999584 + }, + { + "x": 2.431757966174578, + "y": 5.413345350032281, + "heading": 3.903697176098764e-17, + "angularVelocity": 7.485536262518973e-17, + "velocityX": 2.9915701343653476, + "velocityY": -0.4653505979822776, + "timestamp": 8.240017276198158 + }, + { + "x": 2.660114640541256, + "y": 5.377823563746616, + "heading": 4.4256799680232527e-17, + "angularVelocity": 7.522017258708922e-17, + "velocityX": 3.29072695939845, + "velocityY": -0.5118856284819102, + "timestamp": 8.309411266396731 + }, + { + "x": 2.909230981447395, + "y": 5.339072529003315, + "heading": 4.9501702917687895e-17, + "angularVelocity": 7.558151970286575e-17, + "velocityX": 3.589883507106047, + "velocityY": -0.5584206158546873, + "timestamp": 8.378805256595305 + }, + { + "x": 3.168557198602556, + "y": 5.29873296059366, + "heading": 6.319291433274611e-17, + "angularVelocity": 1.9729678861063994e-16, + "velocityX": 3.737012620445782, + "velocityY": -0.5813121322786239, + "timestamp": 8.448199246793878 + }, + { + "x": 3.4267765970934825, + "y": 5.251827631481489, + "heading": 8.420269956089665e-17, + "angularVelocity": 3.0276087551661663e-16, + "velocityX": 3.7210628435116764, + "velocityY": -0.6759278285907664, + "timestamp": 8.517593236992452 + }, + { + "x": 3.6804171256627907, + "y": 5.18441954809062, + "heading": 4.866640802350699e-17, + "angularVelocity": -5.120946559749006e-16, + "velocityX": 3.6550791767919044, + "velocityY": -0.9713821499236162, + "timestamp": 8.586987227191026 + }, + { + "x": 3.9278531799158563, + "y": 5.096939458942259, + "heading": 2.4335451025021366e-17, + "angularVelocity": -3.5062052101080536e-16, + "velocityX": 3.5656697870379945, + "velocityY": -1.260629182700587, + "timestamp": 8.6563812173896 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 1.2553482249572005e-18, + "angularVelocity": -3.325951243636478e-16, + "velocityX": 3.4534097952511087, + "velocityY": -1.541800129495057, + "timestamp": 8.725775207588173 + }, + { + "x": 4.284849298076782, + "y": 4.931828595221009, + "heading": -1.3377953270770433e-17, + "angularVelocity": -4.226105068324817e-16, + "velocityX": 3.38908081155215, + "velocityY": -1.6784855317357408, + "timestamp": 8.760401184129543 + }, + { + "x": 4.399784866536065, + "y": 4.869069261630718, + "heading": -2.6846630837218187e-17, + "angularVelocity": -3.8897610729556746e-16, + "velocityX": 3.3193451835780268, + "velocityY": -1.8124928120166828, + "timestamp": 8.795027160670914 + }, + { + "x": 4.512122399892567, + "y": 4.801769947661279, + "heading": -3.804014031225808e-17, + "angularVelocity": -3.2326913470628567e-16, + "velocityX": 3.2443137949419496, + "velocityY": -1.9436076810434628, + "timestamp": 8.829653137212285 + }, + { + "x": 4.621682688033729, + "y": 4.730038068451707, + "heading": -4.7868809258190414e-17, + "angularVelocity": -2.838524693725381e-16, + "velocityX": 3.1641068089520323, + "velocityY": -2.0716203952795396, + "timestamp": 8.864279113753655 + }, + { + "x": 4.728291014117835, + "y": 4.653988188924879, + "heading": -5.969081362943227e-17, + "angularVelocity": -3.414200999257287e-16, + "velocityX": 3.0788539915035598, + "velocityY": -2.1963244685955665, + "timestamp": 8.898905090295026 + }, + { + "x": 4.836842403001111, + "y": 4.580738511169008, + "heading": -6.391442093146582e-17, + "angularVelocity": -1.2197799812053293e-16, + "velocityX": 3.134969746010814, + "velocityY": -2.1154544960878003, + "timestamp": 8.933531066836396 + }, + { + "x": 4.948232400240648, + "y": 4.5118822283902675, + "heading": -5.985061600801175e-17, + "angularVelocity": 1.173628971657168e-16, + "velocityX": 3.216948902695972, + "velocityY": -1.9885730210806443, + "timestamp": 8.968157043377767 + }, + { + "x": 5.062283352257089, + "y": 4.447529300627276, + "heading": -5.566760825345324e-17, + "angularVelocity": 1.2080548110971404e-16, + "velocityX": 3.293797414787035, + "velocityY": -1.8585158944500793, + "timestamp": 9.002783019919137 + }, + { + "x": 5.178813309821618, + "y": 4.387782436951599, + "heading": -4.4683720245590447e-17, + "angularVelocity": 3.1721525586676e-16, + "velocityX": 3.36539122370464, + "velocityY": -1.7254925245008748, + "timestamp": 9.037408996460508 + }, + { + "x": 5.297636355743312, + "y": 4.332736981184801, + "heading": -3.4831170249496923e-17, + "angularVelocity": 2.845421553553544e-16, + "velocityX": 3.431615734497117, + "velocityY": -1.5897156200354856, + "timestamp": 9.072034973001879 + }, + { + "x": 5.418562909052227, + "y": 4.282480769562024, + "heading": -3.435849457009512e-17, + "angularVelocity": 1.3650898164143384e-17, + "velocityX": 3.4923651370362165, + "velocityY": -1.4514020005394612, + "timestamp": 9.10666094954325 + }, + { + "x": 5.541400030004997, + "y": 4.2370939937981875, + "heading": -3.7514977433318736e-17, + "angularVelocity": -9.115938894395056e-17, + "velocityX": 3.5475424297711617, + "velocityY": -1.3107724401537064, + "timestamp": 9.14128692608462 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -3.82351344872691e-17, + "angularVelocity": -2.0798173101011824e-17, + "velocityX": 3.597059527461787, + "velocityY": -1.1680513673144106, + "timestamp": 9.17591290262599 + }, + { + "x": 5.800859276314264, + "y": 4.159128243363963, + "heading": -3.121153141892538e-17, + "angularVelocity": 1.896973231484086e-16, + "velocityX": 3.643657020852143, + "velocityY": -1.013383183783887, + "timestamp": 9.212938217226515 + }, + { + "x": 5.937245993628074, + "y": 4.127402498051012, + "heading": -2.332987946719607e-17, + "angularVelocity": 2.1287197791179265e-16, + "velocityX": 3.6836072504803297, + "velocityY": -0.8568663265997021, + "timestamp": 9.24996353182704 + }, + { + "x": 6.074863067137857, + "y": 4.101529712913805, + "heading": -3.4147305540686986e-17, + "angularVelocity": -2.921629752561486e-16, + "velocityX": 3.7168373853015195, + "velocityY": -0.6987863686333223, + "timestamp": 9.286988846427565 + }, + { + "x": 6.213459439027469, + "y": 4.081557083562315, + "heading": -1.42164023960539e-17, + "angularVelocity": 5.383047614782771e-16, + "velocityX": 3.743286812953797, + "velocityY": -0.5394317257524714, + "timestamp": 9.32401416102809 + }, + { + "x": 6.352782264616147, + "y": 4.067521040771664, + "heading": -1.2952725040414258e-17, + "angularVelocity": 3.413009097467113e-17, + "velocityX": 3.762907272817643, + "velocityY": -0.3790931405199291, + "timestamp": 9.361039475628615 + }, + { + "x": 6.492463118765251, + "y": 4.059455134364433, + "heading": -1.1051006489340503e-17, + "angularVelocity": 5.13626574588434e-17, + "velocityX": 3.772577104506811, + "velocityY": -0.2178484232816345, + "timestamp": 9.39806479022914 + }, + { + "x": 6.627732984606863, + "y": 4.055428289013381, + "heading": -8.35420674945777e-18, + "angularVelocity": 7.283664619836627e-17, + "velocityX": 3.653442713480481, + "velocityY": -0.10875924740947829, + "timestamp": 9.435090104829666 + }, + { + "x": 6.7574379860569564, + "y": 4.05359307719316, + "heading": -5.856926112032921e-18, + "angularVelocity": 6.744792487052549e-17, + "velocityX": 3.503143804435185, + "velocityY": -0.04956640720064634, + "timestamp": 9.47211541943019 + }, + { + "x": 6.881310147885468, + "y": 4.053080371999493, + "heading": -3.474844730669908e-18, + "angularVelocity": 6.433656019193302e-17, + "velocityX": 3.345607273428941, + "velocityY": -0.013847423018504562, + "timestamp": 9.509140734030716 + }, + { + "x": 6.999262823921184, + "y": 4.05342251600114, + "heading": -1.8665913885819445e-18, + "angularVelocity": 4.3436588169438206e-17, + "velocityX": 3.1857305551170585, + "velocityY": 0.009240812815188005, + "timestamp": 9.54616604863124 + }, + { + "x": 7.111261640539779, + "y": 4.0543326919625615, + "heading": -7.554572779591287e-19, + "angularVelocity": 3.001011936481945e-17, + "velocityX": 3.0249254551103046, + "velocityY": 0.024582531471829573, + "timestamp": 9.583191363231766 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": -4.589044952591168e-28, + "angularVelocity": 2.0403804416247763e-17, + "velocityX": 2.8637086743828277, + "velocityY": 0.034722032320904934, + "timestamp": 9.62021667783229 + }, + { + "x": 7.386460306360812, + "y": 4.05864347722903, + "heading": 4.974033033156732e-19, + "angularVelocity": 7.57885321670836e-18, + "velocityX": 2.5775997849542525, + "velocityY": 0.04609434451454496, + "timestamp": 9.68584709579524 + }, + { + "x": 7.53683861526572, + "y": 4.0619164310557965, + "heading": 2.2229573505485046e-18, + "angularVelocity": 2.6291986266563396e-17, + "velocityX": 2.291289825242349, + "velocityY": 0.04986946492729605, + "timestamp": 9.751477513758187 + }, + { + "x": 7.6684247981223, + "y": 4.0651140489216235, + "heading": 6.3469540781667075e-18, + "angularVelocity": 6.283666714516732e-17, + "velocityX": 2.0049572582467055, + "velocityY": 0.048721583148094445, + "timestamp": 9.817107931721136 + }, + { + "x": 7.781221122380145, + "y": 4.068010125801229, + "heading": 5.162171035103576e-18, + "angularVelocity": -1.8052346447254954e-17, + "velocityX": 1.718659240011596, + "velocityY": 0.04412705220375746, + "timestamp": 9.882738349684084 + }, + { + "x": 7.87523101347, + "y": 4.070437514376607, + "heading": 7.951055539821409e-18, + "angularVelocity": 4.2493779436390584e-17, + "velocityX": 1.432413414507389, + "velocityY": 0.03698572477092609, + "timestamp": 9.948368767647032 + }, + { + "x": 7.950458117127661, + "y": 4.072267695467205, + "heading": 6.860884770186536e-18, + "angularVelocity": -1.661075463106874e-17, + "velocityX": 1.1462231384863357, + "velocityY": 0.027886171494901118, + "timestamp": 10.01399918560998 + }, + { + "x": 8.006905948924626, + "y": 4.073398787704802, + "heading": 2.4339380746534332e-18, + "angularVelocity": -6.745266649855045e-17, + "velocityX": 0.8600864286561777, + "velocityY": 0.01723426838811251, + "timestamp": 10.07962960357293 + }, + { + "x": 8.044577771228491, + "y": 4.0737480502725605, + "heading": 2.9083264970800702e-18, + "angularVelocity": 7.228179214282684e-18, + "velocityX": 0.5739994269902885, + "velocityY": 0.0053216569176217, + "timestamp": 10.145260021535877 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": 9.97684240054841e-28, + "angularVelocity": -4.4313697609032606e-17, + "velocityX": 0.2879578076461169, + "velocityY": -0.007635093856342742, + "timestamp": 10.210890439498826 + }, + { + "x": 8.061114768714358, + "y": 4.071548552312887, + "heading": -6.576009001115218e-18, + "angularVelocity": -8.952107660855974e-17, + "velocityX": -0.032151769003766255, + "velocityY": -0.023120849595889583, + "timestamp": 10.284348101668675 + }, + { + "x": 8.03523325714352, + "y": 4.068826117392824, + "heading": -1.7302069231102504e-17, + "angularVelocity": -1.4601690162241075e-16, + "velocityX": -0.3523323613402589, + "velocityY": -0.037061279104803016, + "timestamp": 10.357805763838524 + }, + { + "x": 7.985826542335116, + "y": 4.0652145468753025, + "heading": -2.1436247637285606e-17, + "angularVelocity": -5.627974379451221e-17, + "velocityX": -0.6725876286964619, + "velocityY": -0.04916533430060717, + "timestamp": 10.431263426008373 + }, + { + "x": 7.912889031757725, + "y": 4.060876766115487, + "heading": -3.018602234671886e-17, + "angularVelocity": -1.1911316602591952e-16, + "velocityX": -0.9929190287698493, + "velocityY": -0.05905144040367088, + "timestamp": 10.504721088178222 + }, + { + "x": 7.816415389029205, + "y": 4.056013446336911, + "heading": -4.386893441840666e-17, + "angularVelocity": -1.8626936480131915e-16, + "velocityX": -1.3133230745276494, + "velocityY": -0.06620575219684201, + "timestamp": 10.578178750348071 + }, + { + "x": 7.696401323527335, + "y": 4.050877776360081, + "heading": -5.3011403841257886e-17, + "angularVelocity": -1.244590305935936e-16, + "velocityX": -1.6337855297433015, + "velocityY": -0.06991333272974812, + "timestamp": 10.65163641251792 + }, + { + "x": 7.552845329490783, + "y": 4.04579904746213, + "heading": -5.8407144486754e-17, + "angularVelocity": -7.345374854837963e-17, + "velocityX": -1.9542684833152193, + "velocityY": -0.06913817766494233, + "timestamp": 10.72509407468777 + }, + { + "x": 7.385752700395874, + "y": 4.04122271365356, + "heading": -6.122941827410045e-17, + "angularVelocity": -3.8420413942670663e-17, + "velocityX": -2.2746793752918006, + "velocityY": -0.062298930749920316, + "timestamp": 10.798551736857618 + }, + { + "x": 7.195145586336795, + "y": 4.037784147568392, + "heading": -6.335567714222456e-17, + "angularVelocity": -2.894536534102561e-17, + "velocityX": -2.5947887317507625, + "velocityY": -0.046810175869971896, + "timestamp": 10.872009399027467 + }, + { + "x": 6.981091630995715, + "y": 4.036460065314617, + "heading": -6.194065422525818e-17, + "angularVelocity": 1.926310849935096e-17, + "velocityX": -2.9139772355692886, + "velocityY": -0.01802510745187768, + "timestamp": 10.945467061197316 + }, + { + "x": 6.743803602394158, + "y": 4.0389311192423705, + "heading": -6.317374994659029e-17, + "angularVelocity": -1.6786481973259e-17, + "velocityX": -3.230269267934209, + "velocityY": 0.03363915832279882, + "timestamp": 11.018924723367165 + }, + { + "x": 6.484122272478982, + "y": 4.04866611767419, + "heading": -6.348618330459737e-17, + "angularVelocity": -4.253243967450325e-18, + "velocityX": -3.535115633202991, + "velocityY": 0.13252529612649763, + "timestamp": 11.092382385537014 + }, + { + "x": 6.20753972881787, + "y": 4.074789307976153, + "heading": -6.404970424521136e-17, + "angularVelocity": -7.671370479922628e-18, + "velocityX": -3.76519665193805, + "velocityY": 0.35562240248758487, + "timestamp": 11.165840047706864 + }, + { + "x": 5.9341618042346855, + "y": 4.124235317195696, + "heading": -6.716116670118769e-17, + "angularVelocity": -4.235722135085759e-17, + "velocityX": -3.7215712630641393, + "velocityY": 0.6731225546657573, + "timestamp": 11.239297709876713 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -6.707406307437602e-17, + "angularVelocity": 1.1857663806024648e-18, + "velocityX": -3.651219865855907, + "velocityY": 0.9857890275804273, + "timestamp": 11.312755372046562 + }, + { + "x": 5.540922192391165, + "y": 4.235828574357528, + "heading": -6.659781323672848e-17, + "angularVelocity": 1.3746706587182516e-17, + "velocityX": -3.608913259332814, + "velocityY": 1.130896109600127, + "timestamp": 11.347400021957807 + }, + { + "x": 5.417558030914266, + "y": 4.279972683638325, + "heading": -6.705658889375764e-17, + "angularVelocity": -1.3242323379839645e-17, + "velocityX": -3.560843067917901, + "velocityY": 1.2741970085969618, + "timestamp": 11.382044671869052 + }, + { + "x": 5.296056287935261, + "y": 4.329010889435292, + "heading": -6.943855501790622e-17, + "angularVelocity": -6.875422700655523e-17, + "velocityX": -3.507085315922588, + "velocityY": 1.4154625872276303, + "timestamp": 11.416689321780296 + }, + { + "x": 5.176611031188361, + "y": 4.382864860494658, + "heading": -7.367311041415583e-17, + "angularVelocity": -1.2222826345940313e-16, + "velocityX": -3.4477258985991424, + "velocityY": 1.55446717450842, + "timestamp": 11.451333971691541 + }, + { + "x": 5.059413042015962, + "y": 4.441448571672919, + "heading": -7.769450380917627e-17, + "angularVelocity": -1.160754519341825e-16, + "velocityX": -3.3828596759570035, + "velocityY": 1.6909886902694797, + "timestamp": 11.485978621602786 + }, + { + "x": 4.94464950941122, + "y": 4.504668439881924, + "heading": -7.988620676495281e-17, + "angularVelocity": -6.326237851534012e-17, + "velocityX": -3.312590339309282, + "velocityY": 1.8248089783261385, + "timestamp": 11.520623271514031 + }, + { + "x": 4.8325037281342125, + "y": 4.572423470089542, + "heading": -7.896813531381333e-17, + "angularVelocity": 2.6499660209021626e-17, + "velocityX": -3.237030293690655, + "velocityY": 1.9557140967276965, + "timestamp": 11.555267921425276 + }, + { + "x": 4.723154797252288, + "y": 4.644605406017416, + "heading": -6.685297337107051e-17, + "angularVelocity": 3.496979179591587e-16, + "velocityX": -3.1563006456137206, + "velocityY": 2.083494453336746, + "timestamp": 11.589912571336521 + }, + { + "x": 4.616777290879426, + "y": 4.721098849146534, + "heading": -4.689132664597989e-17, + "angularVelocity": 5.761826653182846e-16, + "velocityX": -3.070532005530056, + "velocityY": 2.207943891050558, + "timestamp": 11.624557221247766 + }, + { + "x": 4.50857855839689, + "y": 4.794993740354878, + "heading": -5.0771067360923566e-17, + "angularVelocity": -1.1198672017249538e-16, + "velocityX": -3.1231007604269903, + "velocityY": 2.1329380264384965, + "timestamp": 11.659201871159011 + }, + { + "x": 4.397513666551277, + "y": 4.8645064031640235, + "heading": -6.128696485502522e-17, + "angularVelocity": -3.035359722499063e-16, + "velocityX": -3.2058309762155544, + "velocityY": 2.006447257721705, + "timestamp": 11.693846521070256 + }, + { + "x": 4.283759944670474, + "y": 4.929525716016982, + "heading": -7.10068611169889e-17, + "angularVelocity": -2.8055980610512555e-16, + "velocityX": -3.2834426721651013, + "velocityY": 1.8767490224184165, + "timestamp": 11.728491170981501 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -7.867131264216922e-17, + "angularVelocity": -2.2123045100748588e-16, + "velocityX": -3.355810480670139, + "velocityY": 1.74405225643013, + "timestamp": 11.763135820892746 + }, + { + "x": 3.910732016936242, + "y": 5.096825920979702, + "heading": -8.738685068125367e-17, + "angularVelocity": -1.1851522353036446e-16, + "velocityX": -3.4915577224647327, + "velocityY": 1.453345144287359, + "timestamp": 11.83667521759551 + }, + { + "x": 3.6458301860416573, + "y": 5.181556502689957, + "heading": -6.150568630518277e-17, + "angularVelocity": 3.51936044311529e-16, + "velocityX": -3.6021757421438494, + "velocityY": 1.1521794508693837, + "timestamp": 11.910214614298273 + }, + { + "x": 3.3747001133794594, + "y": 5.243530170834085, + "heading": -3.504475258762766e-17, + "angularVelocity": 3.598198367759973e-16, + "velocityX": -3.68686832934017, + "velocityY": 0.8427274484534926, + "timestamp": 11.983754011001036 + }, + { + "x": 3.1003610694822004, + "y": 5.289250417175462, + "heading": -4.352123055745274e-17, + "angularVelocity": -1.1526444802732673e-16, + "velocityX": -3.7305044125682025, + "velocityY": 0.621710924909727, + "timestamp": 12.0572934077038 + }, + { + "x": 2.826021831554975, + "y": 5.334969499245887, + "heading": -7.949559303016089e-17, + "angularVelocity": -4.891849006802281e-16, + "velocityX": -3.7305070510174576, + "velocityY": 0.6216950929746646, + "timestamp": 12.130832804406563 + }, + { + "x": 2.5700139771360067, + "y": 5.3776336262742275, + "heading": -7.076803890272921e-17, + "angularVelocity": 1.186786201586086e-16, + "velocityX": -3.481234085366803, + "velocityY": 0.5801533455704441, + "timestamp": 12.204372201109326 + }, + { + "x": 2.337279538147283, + "y": 5.41641920061303, + "heading": -5.893119866883478e-17, + "angularVelocity": 1.6095916971261216e-16, + "velocityX": -3.1647586113523465, + "velocityY": 0.5274121909861365, + "timestamp": 12.27791159781209 + }, + { + "x": 2.12781853242598, + "y": 5.451326219289701, + "heading": -5.026414725549793e-17, + "angularVelocity": 1.178558949690138e-16, + "velocityX": -2.8482828947852163, + "velocityY": 0.4746709959800321, + "timestamp": 12.351450994514853 + }, + { + "x": 1.9416309659178494, + "y": 5.482354681313371, + "heading": -4.204375061179442e-17, + "angularVelocity": 1.1178221487834326e-16, + "velocityX": -2.531807097366853, + "velocityY": 0.42192978749993854, + "timestamp": 12.424990391217616 + }, + { + "x": 1.7787168415957708, + "y": 5.509504586188606, + "heading": -3.165703866093114e-17, + "angularVelocity": 1.4124010281718074e-16, + "velocityX": -2.2153312595228143, + "velocityY": 0.36918857228284047, + "timestamp": 12.49852978792038 + }, + { + "x": 1.6390761612434732, + "y": 5.532775933618142, + "heading": -2.0086106192649845e-17, + "angularVelocity": 1.5734331511581504e-16, + "velocityX": -1.8988553974233522, + "velocityY": 0.3164473530235367, + "timestamp": 12.572069184623142 + }, + { + "x": 1.5227089260501099, + "y": 5.552168723403807, + "heading": -1.374168138388034e-17, + "angularVelocity": 8.627246202414872e-17, + "velocityX": -1.5823795191536005, + "velocityY": 0.2637061310694281, + "timestamp": 12.645608581325906 + }, + { + "x": 1.4296151368650767, + "y": 5.567682955404048, + "heading": -6.859284014118712e-18, + "angularVelocity": 9.358789545320571e-17, + "velocityX": -1.2659036293336394, + "velocityY": 0.21096490719045813, + "timestamp": 12.71914797802867 + }, + { + "x": 1.35979479432542, + "y": 5.579318629512699, + "heading": -3.246170104472464e-18, + "angularVelocity": 4.913167733478094e-17, + "velocityX": -0.949427730851019, + "velocityY": 0.15822368186784203, + "timestamp": 12.792687374731432 + }, + { + "x": 1.3132478989266207, + "y": 5.587075745647187, + "heading": -2.3462842211621804e-18, + "angularVelocity": 1.223678632415003e-17, + "velocityX": -0.6329518256307737, + "velocityY": 0.10548245542238979, + "timestamp": 12.866226771434196 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.1322625168775542e-27, + "angularVelocity": 3.1905132868116714e-17, + "velocityX": -0.31647591502042816, + "velocityY": 0.05274122807866861, + "timestamp": 12.93976616813696 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 5.483370217924037e-28, + "angularVelocity": -4.839375204095192e-28, + "velocityX": -1.0823834640575863e-26, + "velocityY": -1.7334223684022012e-26, + "timestamp": 13.013305564839722 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.1.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.1.traj index 8d73ed2f..d4ab4cb4 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.1.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.1.traj @@ -1,319 +1,320 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -7.2072188294937895e-31, - "velocityX": -1.1048793733131498e-32, - "velocityY": -5.425421859025623e-32, - "timestamp": 0 - }, - { - "x": 1.3063416372973582, - "y": 5.5750227907482355, - "heading": -0.009007895171544168, - "angularVelocity": -0.12420787311314503, - "velocityX": 0.22568350897133957, - "velocityY": -0.21967610709029223, - "timestamp": 0.07252273906455099 - }, - { - "x": 1.339076097606074, - "y": 5.5431599730454195, - "heading": -0.02701891548605667, - "angularVelocity": -0.24834997335784384, - "velocityX": 0.4513682292059063, - "velocityY": -0.43934934220379995, - "timestamp": 0.14504547812910198 - }, - { - "x": 1.388178099815531, - "y": 5.4953660820311345, - "heading": -0.05401968965283247, - "angularVelocity": -0.3723076998340179, - "velocityX": 0.677056642410746, - "velocityY": -0.6590193866197215, - "timestamp": 0.217568217193653 - }, - { - "x": 1.4536480974194865, - "y": 5.431641407654605, - "heading": -0.08998953758011774, - "angularVelocity": -0.49598027310141063, - "velocityX": 0.9027513087406556, - "velocityY": -0.8786854329899796, - "timestamp": 0.29009095625820397 - }, - { - "x": 1.5354867341790108, - "y": 5.351986341420191, - "heading": -0.13490221177586337, - "angularVelocity": -0.6192909255093908, - "velocityX": 1.1284548517495026, - "velocityY": -1.0983460809932237, - "timestamp": 0.36261369532275495 - }, - { - "x": 1.6336948546750298, - "y": 5.2564014251813616, - "heading": -0.18872783549465025, - "angularVelocity": -0.7421896140863778, - "velocityX": 1.3541700404978287, - "velocityY": -1.317999257498918, - "timestamp": 0.4351364343873059 - }, - { - "x": 1.7482735602297292, - "y": 5.144887415721353, - "heading": -0.25143410509925246, - "angularVelocity": -0.8646428749583058, - "velocityX": 1.579900415134063, - "velocityY": -1.5376419988880228, - "timestamp": 0.507659173451857 - }, - { - "x": 1.87922458951427, - "y": 5.017445479597787, - "heading": -0.32298143653104017, - "angularVelocity": -0.9865503200055588, - "velocityX": 1.8056547639221738, - "velocityY": -1.7572686548722531, - "timestamp": 0.5801819125164079 - }, - { - "x": 2.026417113452321, - "y": 4.874266246183284, - "heading": -0.4026826239248217, - "angularVelocity": -1.0989820354525424, - "velocityX": 2.0296051395270776, - "velocityY": -1.9742667646213359, - "timestamp": 0.6527046515809589 - }, - { - "x": 2.1572394289562786, - "y": 4.747014797378468, - "heading": -0.4735071808339005, - "angularVelocity": -0.9765841420579419, - "velocityX": 1.8038799580839464, - "velocityY": -1.7546420673865035, - "timestamp": 0.7252273906455099 - }, - { - "x": 2.27168995904481, - "y": 4.635690037684074, - "heading": -0.535478944048102, - "angularVelocity": -0.8545149288832774, - "velocityX": 1.578133031994185, - "velocityY": -1.5350324757486038, - "timestamp": 0.7977501297100609 - }, - { - "x": 2.369767924446142, - "y": 4.540291261559593, - "heading": -0.5886064902849578, - "angularVelocity": -0.732563978168669, - "velocityX": 1.35237536069838, - "velocityY": -1.3154326126526381, - "timestamp": 0.8702728687746119 - }, - { - "x": 2.4514728066095985, - "y": 4.460817939321001, - "heading": -0.6328935848970949, - "angularVelocity": -0.6106649470681784, - "velocityX": 1.1266105392220715, - "velocityY": -1.0958400532524892, - "timestamp": 0.9427956078391628 - }, - { - "x": 2.5168042340609067, - "y": 4.397269668318764, - "heading": -0.6683418026498548, - "angularVelocity": -0.48878763005849607, - "velocityX": 0.9008405955692155, - "velocityY": -0.8762530458976289, - "timestamp": 1.015318346903714 - }, - { - "x": 2.5657619428441123, - "y": 4.349646151783931, - "heading": -0.6949516025618677, - "angularVelocity": -0.36691664235512766, - "velocityX": 0.6750670122861077, - "velocityY": -0.6566701306241847, - "timestamp": 1.087841085968265 - }, - { - "x": 2.5983457601971756, - "y": 4.317947187379743, - "heading": -0.7127228546140986, - "angularVelocity": -0.24504386184805346, - "velocityX": 0.4492910468254882, - "velocityY": -0.4370900053275183, - "timestamp": 1.160363825032816 - }, - { - "x": 2.614555597305298, - "y": 4.302172660827637, - "heading": -0.7216551150961179, - "angularVelocity": -0.12316496311606469, - "velocityX": 0.22351385671724336, - "velocityY": -0.21751145579268175, - "timestamp": 1.2328865640973672 - }, - { - "x": 2.6122418058900085, - "y": 4.3044120467003895, - "heading": -0.7206083682746678, - "angularVelocity": 0.01292691365951148, - "velocityX": -0.028574418607166113, - "velocityY": 0.027655539271243604, - "timestamp": 1.3138607895750445 - }, - { - "x": 2.589515477392911, - "y": 4.326503828401164, - "heading": -0.7085432954922883, - "angularVelocity": 0.148998927882974, - "velocityX": -0.2806612642842681, - "velocityY": 0.2728248596434546, - "timestamp": 1.3948350150527218 - }, - { - "x": 2.54637684601469, - "y": 4.3684483333856905, - "heading": -0.6854613533337227, - "angularVelocity": 0.2850529538548107, - "velocityX": -0.5327452176745692, - "velocityY": 0.5179982239657722, - "timestamp": 1.475809240530399 - }, - { - "x": 2.4828262685453337, - "y": 4.430246041385043, - "heading": -0.6513636060932391, - "angularVelocity": 0.42109383620888585, - "velocityX": -0.784824764848103, - "velocityY": 0.7631775127791823, - "timestamp": 1.5567834660080764 - }, - { - "x": 2.398864238908538, - "y": 4.511897605460961, - "heading": -0.6062501465002338, - "angularVelocity": 0.5571335733911974, - "velocityX": -1.0368982122572787, - "velocityY": 1.0083648666494986, - "timestamp": 1.6377576914857537 - }, - { - "x": 2.2944914266815206, - "y": 4.613403885366389, - "heading": -0.5501188636580988, - "angularVelocity": 0.6931993793206396, - "velocityX": -1.2889633906498084, - "velocityY": 1.2535628381323076, - "timestamp": 1.718731916963431 - }, - { - "x": 2.1697087770291006, - "y": 4.73476601037198, - "heading": -0.48296289489208905, - "angularVelocity": 0.8293499365978624, - "velocityX": -1.5410168966266635, - "velocityY": 1.4987747556657698, - "timestamp": 1.7997061424411083 - }, - { - "x": 2.0245178868987646, - "y": 4.87598561985106, - "heading": -0.4047647421109401, - "angularVelocity": 0.9657165884553096, - "velocityX": -1.7930506809269309, - "velocityY": 1.7440069188183338, - "timestamp": 1.8806803679187856 - }, - { - "x": 1.8612798092271896, - "y": 5.034863868368553, - "heading": -0.31505899294827, - "angularVelocity": 1.1078309009257712, - "velocityX": -2.0159263853239904, - "velocityY": 1.9620842012417825, - "timestamp": 1.9616545933964629 - }, - { - "x": 1.7184500483895917, - "y": 5.1738844559195085, - "heading": -0.23644393225782434, - "angularVelocity": 0.9708652380027345, - "velocityX": -1.7638916580562496, - "velocityY": 1.7168498584691285, - "timestamp": 2.04262881887414 - }, - { - "x": 1.5960267405860193, - "y": 5.293046114041946, - "heading": -0.168968478820667, - "angularVelocity": 0.8332954472751476, - "velocityX": -1.5118799479886125, - "velocityY": 1.4715998506873489, - "timestamp": 2.1236030443518175 - }, - { - "x": 1.4940086408037947, - "y": 5.392348179003114, - "heading": -0.1126782173092116, - "angularVelocity": 0.6951627036802206, - "velocityX": -1.2598836133403888, - "velocityY": 1.2263416460657963, - "timestamp": 2.2045772698294948 - }, - { - "x": 1.4123948378953157, - "y": 5.471790254952654, - "heading": -0.06761272810355828, - "angularVelocity": 0.5565411578785837, - "velocityX": -1.0078985310085185, - "velocityY": 0.9810785528472173, - "timestamp": 2.285551495307172 - }, - { - "x": 1.3511847156576737, - "y": 5.531372091014257, - "heading": -0.03380194743073204, - "angularVelocity": 0.417549911387026, - "velocityX": -0.7559210585414546, - "velocityY": 0.7358123614043836, - "timestamp": 2.3665257207848494 - }, - { - "x": 1.3103779403872662, - "y": 5.571093500047757, - "heading": -0.011263323026729973, - "angularVelocity": 0.27834319218308196, - "velocityX": -0.50394770718331, - "velocityY": 0.4905438588537352, - "timestamp": 2.4474999462625266 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 4.4390879396333275e-31, - "angularVelocity": 0.13909763212046558, - "velocityX": -0.25197510938629825, - "velocityY": 0.24527315422336363, - "timestamp": 2.528474171740204 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 2.494908675164405e-31, - "angularVelocity": -2.400971097599731e-30, - "velocityX": 9.117669624782265e-34, - "velocityY": -1.69916768015881e-32, - "timestamp": 2.6094483972178812 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -4.063364103945615e-24, + "angularVelocity": -2.020629205800752e-24, + "velocityX": -1.3550816656042328e-24, + "velocityY": 1.2971572394891071e-24, + "timestamp": 0 + }, + { + "x": 1.3063227037698195, + "y": 5.575041446046491, + "heading": -0.008997103572259477, + "angularVelocity": -0.12413133595120028, + "velocityX": 0.22555374987191792, + "velocityY": -0.2195466872880714, + "timestamp": 0.0724805183422541 + }, + { + "x": 1.3390193004314572, + "y": 5.543215943390215, + "heading": -0.026986588243377234, + "angularVelocity": -0.2481975168302625, + "velocityX": 0.45110875873215955, + "velocityY": -0.4390904395301923, + "timestamp": 0.1449610366845082 + }, + { + "x": 1.3880645131173623, + "y": 5.495478033090953, + "heading": -0.053955156680249416, + "angularVelocity": -0.3720802369200259, + "velocityX": 0.6766675212546471, + "velocityY": -0.6586309175362454, + "timestamp": 0.21744155502676227 + }, + { + "x": 1.4534588011433731, + "y": 5.431828013300948, + "heading": -0.0898822289659018, + "angularVelocity": -0.4956790197885205, + "velocityX": 0.9022326208708659, + "velocityY": -0.878167281992224, + "timestamp": 0.2899220733690164 + }, + { + "x": 1.535202817150975, + "y": 5.352266287723616, + "heading": -0.1347416873383059, + "angularVelocity": -0.6189174608351592, + "velocityX": 1.1278067248581949, + "velocityY": -1.0976980766285507, + "timestamp": 0.3624025917112705 + }, + { + "x": 1.6332974209934847, + "y": 5.25679341851918, + "heading": -0.1885038314662501, + "angularVelocity": -0.7417461320306592, + "velocityX": 1.3533926920790726, + "velocityY": -1.3172211152465878, + "timestamp": 0.4348831100535246 + }, + { + "x": 1.7477437450196447, + "y": 5.145410203108038, + "heading": -0.2511366541198319, + "angularVelocity": -0.8641332055301907, + "velocityX": 1.5789942821013316, + "velocityY": -1.5367331520062975, + "timestamp": 0.5073636283957786 + }, + { + "x": 1.8785436154915462, + "y": 5.018117926270065, + "heading": -0.3226014402327865, + "angularVelocity": -0.9859861345844242, + "velocityX": 1.8046210687161843, + "velocityY": -1.7562274629010988, + "timestamp": 0.5798441467380328 + }, + { + "x": 2.0257354638148333, + "y": 4.874937893317761, + "heading": -0.40230013407560183, + "angularVelocity": -1.0995878018763188, + "velocityX": 2.0307780861644047, + "velocityY": -1.975427828429777, + "timestamp": 0.6523246650802869 + }, + { + "x": 2.156576334610242, + "y": 4.74766733108212, + "heading": -0.4731338136564732, + "angularVelocity": -0.9772788771514256, + "velocityX": 1.8051867424233425, + "velocityY": -1.7559278706406076, + "timestamp": 0.724805183422541 + }, + { + "x": 2.271064512399631, + "y": 4.636304989038449, + "heading": -0.5351259426845023, + "angularVelocity": -0.8552936767822404, + "velocityX": 1.579571730554888, + "velocityY": -1.5364451661040166, + "timestamp": 0.7972857017647951 + }, + { + "x": 2.3691991759335136, + "y": 4.540850112955004, + "heading": -0.5882849412728991, + "angularVelocity": -0.7334246471221314, + "velocityX": 1.3539453880626124, + "velocityY": -1.3169728675601617, + "timestamp": 0.8697662201070492 + }, + { + "x": 2.4509797863223954, + "y": 4.461302149238075, + "heading": -0.6326144930208853, + "angularVelocity": -0.611606439383635, + "velocityX": 1.1283116106139404, + "velocityY": -1.0975082068439748, + "timestamp": 0.9422467384493033 + }, + { + "x": 2.5164059599659825, + "y": 4.397660680894242, + "heading": -0.6681161230461814, + "angularVelocity": -0.4898092734058101, + "velocityX": 0.9026725407045777, + "velocityY": -0.8780492993071322, + "timestamp": 1.0147272567915573 + }, + { + "x": 2.5654774247056453, + "y": 4.349925401446345, + "heading": -0.6947902571446234, + "angularVelocity": -0.3680179820525904, + "velocityX": 0.677029715874088, + "velocityY": -0.6585946201776504, + "timestamp": 1.0872077751338114 + }, + { + "x": 2.598194001702078, + "y": 4.3180961013994725, + "heading": -0.7126367415892834, + "angularVelocity": -0.24622456975802207, + "velocityX": 0.45138442363152775, + "velocityY": -0.4391428314098807, + "timestamp": 1.1596882934760655 + }, + { + "x": 2.614555597305298, + "y": 4.302172660827637, + "heading": -0.7216551150961179, + "angularVelocity": -0.1244247932147739, + "velocityX": 0.2257378393178725, + "velocityY": -0.21969269723824078, + "timestamp": 1.2321688118183196 + }, + { + "x": 2.6124101268407562, + "y": 4.3042467200349375, + "heading": -0.7207049042678578, + "angularVelocity": 0.011728656954985593, + "velocityX": -0.026482004137690758, + "velocityY": 0.025600559605606452, + "timestamp": 1.3131849786230068 + }, + { + "x": 2.5898308635318203, + "y": 4.326193667394344, + "heading": -0.7087258164674446, + "angularVelocity": 0.14786046134831782, + "velocityX": -0.27870071122187107, + "velocityY": 0.2708958992384371, + "timestamp": 1.394201145427694 + }, + { + "x": 2.546818010460629, + "y": 4.368013804819267, + "heading": -0.6857195036998125, + "angularVelocity": 0.28397187468885515, + "velocityX": -0.5309169116194552, + "velocityY": 0.51619496545352, + "timestamp": 1.475217312232381 + }, + { + "x": 2.4833718812656413, + "y": 4.429707576791875, + "heading": -0.6516873068635105, + "angularVelocity": 0.4200667370297391, + "velocityX": -0.7831292407099776, + "velocityY": 0.7614995180078072, + "timestamp": 1.5562334790370682 + }, + { + "x": 2.3994929054836645, + "y": 4.511275584027978, + "heading": -0.6066297376902362, + "angularVelocity": 0.5561552829560362, + "velocityX": -1.035336267935158, + "velocityY": 1.0068114853266914, + "timestamp": 1.6372496458417554 + }, + { + "x": 2.295181645720894, + "y": 4.612718599682289, + "heading": -0.5505453877273064, + "angularVelocity": 0.6922612137172227, + "velocityX": -1.2875363507908548, + "velocityY": 1.2521329958607974, + "timestamp": 1.7182658126464425 + }, + { + "x": 2.1704388340864518, + "y": 4.734037581039087, + "heading": -0.4834287968254485, + "angularVelocity": 0.8284345402771498, + "velocityX": -1.539727397065957, + "velocityY": 1.4974663223609717, + "timestamp": 1.7992819794511297 + }, + { + "x": 2.025265436304345, + "y": 4.875233658266282, + "heading": -0.405266607510017, + "angularVelocity": 0.9647727410241949, + "velocityX": -1.7919065232015794, + "velocityY": 1.7428135987671272, + "timestamp": 1.8802981462558168 + }, + { + "x": 1.8618624811004039, + "y": 5.034280009168128, + "heading": -0.3154414224064508, + "angularVelocity": 1.10873161056997, + "velocityX": -2.0169178776116525, + "velocityY": 1.9631433721774778, + "timestamp": 1.961314313060504 + }, + { + "x": 1.7188875745497745, + "y": 5.1734469779831365, + "heading": -0.2367274347449033, + "angularVelocity": 0.9715837068829752, + "velocityX": -1.7647700723154625, + "velocityY": 1.7177678765092634, + "timestamp": 2.0423304798651913 + }, + { + "x": 1.5963395221215884, + "y": 5.292733837393614, + "heading": -0.1691692747677602, + "angularVelocity": 0.8338849224996224, + "velocityX": -1.5126370113711174, + "velocityY": 1.4723834034020926, + "timestamp": 2.1233466466698787 + }, + { + "x": 1.4942172977681851, + "y": 5.392140100481127, + "heading": -0.11281115760570573, + "angularVelocity": 0.695640381233068, + "velocityX": -1.2605166151540885, + "velocityY": 1.2269929201558054, + "timestamp": 2.204362813474566 + }, + { + "x": 1.41252009894491, + "y": 5.47166545898136, + "heading": -0.06769200462657954, + "angularVelocity": 0.5569154251385268, + "velocityX": -1.008406124918618, + "velocityY": 0.9815986319366667, + "timestamp": 2.2853789802792535 + }, + { + "x": 1.3512473746231104, + "y": 5.531309714267714, + "heading": -0.033841358591587725, + "angularVelocity": 0.4178258163780882, + "velocityX": -0.7563024361485172, + "velocityY": 0.7362018920265989, + "timestamp": 2.366395147083941 + }, + { + "x": 1.3103988349917597, + "y": 5.571072713807996, + "heading": -0.011276386726511467, + "angularVelocity": 0.2785243088515359, + "velocityX": -0.5042023245783488, + "velocityY": 0.4908032693788472, + "timestamp": 2.447411313888628 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 7.044896847536941e-24, + "angularVelocity": 0.1391868706118427, + "velocityX": -0.2521025707860859, + "velocityY": 0.24540274759468042, + "timestamp": 2.5284274806933156 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 3.2679300421363754e-24, + "angularVelocity": -5.4036172137189415e-24, + "velocityX": -5.375446023780689e-25, + "velocityY": 8.028108369127909e-25, + "timestamp": 2.609443647498003 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.2.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.2.traj index 0587ab22..39b8bd6e 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.2.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.2.traj @@ -1,211 +1,212 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 2.494908675164405e-31, - "angularVelocity": -2.400971097599731e-30, - "velocityX": 9.117669624782265e-34, - "velocityY": -1.69916768015881e-32, - "timestamp": 0 - }, - { - "x": 1.3242823472826057, - "y": 5.590457081039532, - "heading": -3.1525835471969304e-19, - "angularVelocity": -3.554932323947037e-18, - "velocityX": 0.3868644475376546, - "velocityY": -0.00560680796813751, - "timestamp": 0.08868195678333235 - }, - { - "x": 1.3928981385918453, - "y": 5.589462635652003, - "heading": -9.247304393859629e-19, - "angularVelocity": -6.8725601776619685e-18, - "velocityX": 0.7737288823800034, - "velocityY": -0.011213615752282564, - "timestamp": 0.1773639135666647 - }, - { - "x": 1.4958218231349187, - "y": 5.587970967605794, - "heading": -1.5461935946313141e-18, - "angularVelocity": -7.007774498748477e-18, - "velocityX": 1.1605932962726162, - "velocityY": -0.016820423232804093, - "timestamp": 0.26604587034999705 - }, - { - "x": 1.6330533972641115, - "y": 5.585982076953771, - "heading": -2.4826620193785056e-18, - "angularVelocity": -1.0559852970262818e-17, - "velocityX": 1.5474576690326838, - "velocityY": -0.022427230117193615, - "timestamp": 0.3547278271333294 - }, - { - "x": 1.8045928505515578, - "y": 5.5834959638470645, - "heading": -3.548549667861495e-18, - "angularVelocity": -1.201921661569969e-17, - "velocityX": 1.934321924205522, - "velocityY": -0.028034035297397054, - "timestamp": 0.44340978391666175 - }, - { - "x": 2.0104398981448357, - "y": 5.5805126324140275, - "heading": -4.504226548304647e-18, - "angularVelocity": -1.0776452337176765e-17, - "velocityX": 2.321182967310963, - "velocityY": -0.033640793925260974, - "timestamp": 0.5320917406999941 - }, - { - "x": 2.1819790701409207, - "y": 5.578026523384064, - "heading": -4.574305172786567e-18, - "angularVelocity": -7.902241563425968e-19, - "velocityX": 1.934318752293539, - "velocityY": -0.02803398932702808, - "timestamp": 0.6207736974833264 - }, - { - "x": 2.31921035343729, - "y": 5.576037636947067, - "heading": -3.929985868072649e-18, - "angularVelocity": 7.2655061760547e-18, - "velocityX": 1.547454389528782, - "velocityY": -0.022427182587500143, - "timestamp": 0.7094556542666588 - }, - { - "x": 2.422133743893467, - "y": 5.574545973163046, - "heading": -2.934544233389305e-18, - "angularVelocity": 1.1224849685211679e-17, - "velocityX": 1.1605899800749648, - "velocityY": -0.016820375171309878, - "timestamp": 0.7981376110499911 - }, - { - "x": 2.490749239479307, - "y": 5.573551532061422, - "heading": -1.705878005512682e-18, - "angularVelocity": 1.385474872728055e-17, - "velocityX": 0.7737255477287375, - "velocityY": -0.011213567423341012, - "timestamp": 0.8868195678333235 - }, - { - "x": 2.525056838989258, - "y": 5.573054313659668, - "heading": 0, - "angularVelocity": 1.9235908491289617e-17, - "velocityX": 0.3868611017883933, - "velocityY": -0.005606759478353257, - "timestamp": 0.9755015246166558 - }, - { - "x": 2.5176787496862993, - "y": 5.573161243955685, - "heading": 2.6679509724403924e-18, - "angularVelocity": 2.5453436150449368e-17, - "velocityX": -0.07039024589473145, - "velocityY": 0.0010201624731186972, - "timestamp": 1.0803184521493767 - }, - { - "x": 2.4623729801708616, - "y": 5.573962787832815, - "heading": 6.066089004278022e-18, - "angularVelocity": 3.2419744709429903e-17, - "velocityX": -0.5276415824931802, - "velocityY": 0.007647084263940974, - "timestamp": 1.1851353796820976 - }, - { - "x": 2.3591395323793796, - "y": 5.575458945262994, - "heading": 1.0152873197492895e-17, - "angularVelocity": 3.898973466799139e-17, - "velocityX": -0.9848929006171766, - "velocityY": 0.014274005787013901, - "timestamp": 1.2899523072148185 - }, - { - "x": 2.207978410184721, - "y": 5.577649716190093, - "heading": 1.5606892400662525e-17, - "angularVelocity": 5.2033763358184983e-17, - "velocityX": -1.4421441817922973, - "velocityY": 0.020900926774588543, - "timestamp": 1.3947692347475398 - }, - { - "x": 2.0088896252054647, - "y": 5.580535100445723, - "heading": 1.7204191476600093e-18, - "angularVelocity": -1.324831168006478e-16, - "velocityX": -1.8993953521210223, - "velocityY": 0.02752784615567168, - "timestamp": 1.499586162280261 - }, - { - "x": 1.769251248155021, - "y": 5.584008168003288, - "heading": 6.282149498472273e-20, - "angularVelocity": -1.5814217146918653e-17, - "velocityX": -2.2862564539075523, - "velocityY": 0.03313460563399779, - "timestamp": 1.6044030898129824 - }, - { - "x": 1.577540533889042, - "y": 5.586786622232322, - "heading": -2.476430565711122e-19, - "angularVelocity": -2.961969587010803e-18, - "velocityX": -1.829005283580098, - "velocityY": 0.02650768625293313, - "timestamp": 1.7092200173457037 - }, - { - "x": 1.4337574940262061, - "y": 5.588870462964437, - "heading": -3.350775678748249e-19, - "angularVelocity": -8.341640359227092e-19, - "velocityX": -1.3717540024052963, - "velocityY": 0.01988076526536314, - "timestamp": 1.814036944878425 - }, - { - "x": 1.3379021324393994, - "y": 5.590259690143503, - "heading": -2.0793104080618583e-19, - "angularVelocity": 1.2130342880824037e-18, - "velocityX": -0.914502684281442, - "velocityY": 0.013253843742292293, - "timestamp": 1.9188538724111464 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.4049323971929142e-31, - "angularVelocity": 1.9837543963605952e-18, - "velocityX": -0.4572513476830733, - "velocityY": 0.006626921951471195, - "timestamp": 2.0236707999438677 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 4.571690374498227e-31, - "angularVelocity": 5.701965460392164e-30, - "velocityX": 1.9882633246635272e-32, - "velocityY": -1.2353765948799823e-31, - "timestamp": 2.128487727476589 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 3.2679300421363754e-24, + "angularVelocity": -5.4036172137189415e-24, + "velocityX": -5.375446023780689e-25, + "velocityY": 8.028108369127909e-25, + "timestamp": 0 + }, + { + "x": 1.3242823472826726, + "y": 5.590457081039531, + "heading": -2.9445272594890385e-20, + "angularVelocity": -3.320691324750262e-19, + "velocityX": 0.3868644465708696, + "velocityY": -0.005606807954126726, + "timestamp": 0.08868195700512382 + }, + { + "x": 1.3928981385920456, + "y": 5.5894626356520005, + "heading": 1.967498349802273e-20, + "angularVelocity": 5.538923333201553e-19, + "velocityX": 0.7737288804464331, + "velocityY": -0.01121361572426099, + "timestamp": 0.17736391401024765 + }, + { + "x": 1.495821823135319, + "y": 5.587970967605789, + "heading": -2.85375470863899e-20, + "angularVelocity": -5.436565928013428e-19, + "velocityX": 1.1605932933722591, + "velocityY": -0.016820423190771712, + "timestamp": 0.26604587101537147 + }, + { + "x": 1.6330533972647785, + "y": 5.585982076953762, + "heading": -1.8597210244417365e-22, + "angularVelocity": 3.1969947368616956e-19, + "velocityX": 1.5474576651655374, + "velocityY": -0.022427230061150386, + "timestamp": 0.3547278280204953 + }, + { + "x": 1.8045928505525564, + "y": 5.583495963847051, + "heading": 1.7052075155994419e-19, + "angularVelocity": 1.9249318511157463e-18, + "velocityX": 1.9343219193715702, + "velocityY": -0.028034035227342758, + "timestamp": 0.4434097850256191 + }, + { + "x": 2.010439898145835, + "y": 5.580512632414013, + "heading": 4.48101102281287e-19, + "angularVelocity": 3.1300656874471152e-18, + "velocityX": 2.321182961505746, + "velocityY": -0.03364079384113171, + "timestamp": 0.5320917420307429 + }, + { + "x": 2.1819790701418538, + "y": 5.57802652338405, + "heading": 3.8109031414693246e-19, + "angularVelocity": -7.556304619487216e-19, + "velocityX": 1.934318747455112, + "velocityY": -0.028033989256909718, + "timestamp": 0.6207736990358668 + }, + { + "x": 2.3192103534380903, + "y": 5.5760376369470555, + "heading": 2.513466977645824e-19, + "angularVelocity": -1.4630215747133308e-18, + "velocityX": 1.54745438565714, + "velocityY": -0.022427182531392593, + "timestamp": 0.7094556560409906 + }, + { + "x": 2.4221337438940673, + "y": 5.574545973163037, + "heading": 2.3569701876978443e-19, + "angularVelocity": -1.7646970733669464e-19, + "velocityX": 1.1605899771701071, + "velocityY": -0.01682037512921312, + "timestamp": 0.7981376130461144 + }, + { + "x": 2.4907492394796407, + "y": 5.573551532061417, + "heading": 1.521167829061186e-19, + "angularVelocity": -9.424717122873263e-19, + "velocityX": 0.773725545790664, + "velocityY": -0.011213567395255041, + "timestamp": 0.8868195700512382 + }, + { + "x": 2.525056838989258, + "y": 5.573054313659668, + "heading": -7.785018050329552e-30, + "angularVelocity": -1.7153070145831324e-18, + "velocityX": 0.386861100817104, + "velocityY": -0.005606759464278068, + "timestamp": 0.9755015270563621 + }, + { + "x": 2.5176787496859108, + "y": 5.57316124395569, + "heading": -2.760990246718961e-19, + "angularVelocity": -2.634107204526249e-18, + "velocityX": -0.0703902457225223, + "velocityY": 0.0010201624706221363, + "timestamp": 1.0803184548510405 + }, + { + "x": 2.4623729801701617, + "y": 5.573962787832825, + "heading": -5.185614830886611e-19, + "angularVelocity": -2.3131994374789403e-18, + "velocityX": -0.5276415811774727, + "velocityY": 0.007647084244872661, + "timestamp": 1.185135382645719 + }, + { + "x": 2.359139532378446, + "y": 5.575458945263007, + "heading": -6.8142886513825e-19, + "angularVelocity": -1.5538270914101072e-18, + "velocityX": -0.9848928981579708, + "velocityY": 0.014274005751373834, + "timestamp": 1.2899523104403974 + }, + { + "x": 2.2079784101836313, + "y": 5.577649716190108, + "heading": -5.427283873734921e-19, + "angularVelocity": 1.3232641013891109e-18, + "velocityX": -1.442144178189593, + "velocityY": 0.02090092672237671, + "timestamp": 1.3947692382350758 + }, + { + "x": 2.0088896252042967, + "y": 5.58053510044574, + "heading": -5.007294376921598e-19, + "angularVelocity": 4.006886155587225e-19, + "velocityX": -1.8993953473748186, + "velocityY": 0.027527846086888057, + "timestamp": 1.4995861660297547 + }, + { + "x": 1.7692512481542437, + "y": 5.584008168003299, + "heading": -1.0777156334696855e-19, + "angularVelocity": 3.7489924825765976e-18, + "velocityX": -2.286256448190035, + "velocityY": 0.033134605551138764, + "timestamp": 1.6044030938244336 + }, + { + "x": 1.5775405338885757, + "y": 5.586786622232329, + "heading": 2.4679892039672843e-19, + "angularVelocity": 3.382759743178975e-18, + "velocityX": -1.8290052790060969, + "velocityY": 0.02650768618664608, + "timestamp": 1.7092200216191125 + }, + { + "x": 1.433757494025973, + "y": 5.58887046296444, + "heading": 3.0327550884617235e-19, + "angularVelocity": 5.388117133514026e-19, + "velocityX": -1.3717539989747978, + "velocityY": 0.019880765215647883, + "timestamp": 1.8140369494137913 + }, + { + "x": 1.3379021324393217, + "y": 5.590259690143504, + "heading": 1.0372328795673318e-19, + "angularVelocity": -1.9038167315540544e-18, + "velocityX": -0.9145026819944435, + "velocityY": 0.0132538437091488, + "timestamp": 1.9188538772084702 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.6477091007835168e-22, + "angularVelocity": -9.879942030515793e-19, + "velocityX": -0.45725134653957433, + "velocityY": 0.00662692193489945, + "timestamp": 2.023670805003149 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.5500454228160796e-22, + "angularVelocity": -9.317544566447768e-23, + "velocityX": -6.472519788584152e-21, + "velocityY": -2.347379322977946e-21, + "timestamp": 2.128487732797828 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.3.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.3.traj index caa7ef28..47997dc5 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.3.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.3.traj @@ -1,319 +1,320 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 4.571690374498227e-31, - "angularVelocity": 5.701965460392164e-30, - "velocityX": 1.9882633246635272e-32, - "velocityY": -1.2353765948799823e-31, - "timestamp": 0 - }, - { - "x": 1.3072266031673745, - "y": 5.605560947932051, - "heading": 0.009988018032587274, - "angularVelocity": 0.1383542475734168, - "velocityX": 0.2389771940088317, - "velocityY": 0.20233155967170455, - "timestamp": 0.07219162553929692 - }, - { - "x": 1.3417310637547577, - "y": 5.63477381293564, - "heading": 0.029960721397497427, - "angularVelocity": 0.2766623305087681, - "velocityX": 0.4779565542351512, - "velocityY": 0.40465725470744984, - "timestamp": 0.14438325107859384 - }, - { - "x": 1.393488225984334, - "y": 5.678592353413093, - "heading": 0.05990698047800166, - "angularVelocity": 0.4148162457460571, - "velocityX": 0.7169413604826331, - "velocityY": 0.606975395693467, - "timestamp": 0.21657487661789077 - }, - { - "x": 1.4624987017850304, - "y": 5.737015858733095, - "heading": 0.09981017735525757, - "angularVelocity": 0.5527399692025371, - "velocityX": 0.9559346431831991, - "velocityY": 0.8092836929987566, - "timestamp": 0.2887665021571877 - }, - { - "x": 1.54876329298479, - "y": 5.810043407637518, - "heading": 0.1496511403376383, - "angularVelocity": 0.6903981259608908, - "velocityX": 1.1949390328222467, - "velocityY": 1.0115792290159562, - "timestamp": 0.3609581276964846 - }, - { - "x": 1.6522829608961203, - "y": 5.897673825591776, - "heading": 0.20941128647918397, - "angularVelocity": 0.8277988713391718, - "velocityX": 1.4339567385834835, - "velocityY": 1.2138584953540306, - "timestamp": 0.43314975323578153 - }, - { - "x": 1.7730588392541233, - "y": 5.999905624129209, - "heading": 0.2790750774652724, - "angularVelocity": 0.9649843796382769, - "velocityX": 1.6729901488685341, - "velocityY": 1.4161171434188378, - "timestamp": 0.5053413787750785 - }, - { - "x": 1.911092570994838, - "y": 6.1167366962116585, - "heading": 0.35862892849655154, - "angularVelocity": 1.1019817109951129, - "velocityX": 1.9120463171400826, - "velocityY": 1.6183466047443176, - "timestamp": 0.5775330043143754 - }, - { - "x": 2.0662405750494774, - "y": 6.247971693585017, - "heading": 0.4478813931179114, - "angularVelocity": 1.2363271218036194, - "velocityX": 2.1491135972568896, - "velocityY": 1.8178700977147564, - "timestamp": 0.6497246298536723 - }, - { - "x": 2.204132997305319, - "y": 6.36460684022272, - "heading": 0.5272152358459444, - "angularVelocity": 1.0989341510922093, - "velocityX": 1.9100888950161639, - "velocityY": 1.6156326411317472, - "timestamp": 0.7219162553929692 - }, - { - "x": 2.3247684211138937, - "y": 6.466643771235888, - "heading": 0.5966353508762373, - "angularVelocity": 0.9616089748867341, - "velocityX": 1.6710445693293852, - "velocityY": 1.4134178341442696, - "timestamp": 0.7941078809322661 - }, - { - "x": 2.428146136331826, - "y": 6.5540833745423095, - "heading": 0.6561424196084651, - "angularVelocity": 0.8242932374446845, - "velocityX": 1.4319904067208047, - "velocityY": 1.2112153266137329, - "timestamp": 0.8662995064715631 - }, - { - "x": 2.51426563036811, - "y": 6.626926253781894, - "heading": 0.705736875442574, - "angularVelocity": 0.686983503468318, - "velocityX": 1.192929143690227, - "velocityY": 1.0090211807166112, - "timestamp": 0.93849113201086 - }, - { - "x": 2.5831264968268224, - "y": 6.6851728424603465, - "heading": 0.7454193798278042, - "angularVelocity": 0.5496829319015093, - "velocityX": 0.95386225125429, - "velocityY": 0.8068330397501333, - "timestamp": 1.010682757550157 - }, - { - "x": 2.6347284176402783, - "y": 6.72882344947858, - "heading": 0.7751907373365541, - "angularVelocity": 0.4123935052894287, - "velocityX": 0.7147909529377248, - "velocityY": 0.6046491776859777, - "timestamp": 1.0828743830894538 - }, - { - "x": 2.669071164543137, - "y": 6.75787827866861, - "heading": 0.7950517689437672, - "angularVelocity": 0.27511545084049754, - "velocityX": 0.4757164926845912, - "velocityY": 0.4024681391074233, - "timestamp": 1.1550660086287508 - }, - { - "x": 2.68615460395813, - "y": 6.772337436676025, - "heading": 0.8050032485420815, - "angularVelocity": 0.13784811636956903, - "velocityX": 0.23664018211616192, - "velocityY": 0.20028857778593961, - "timestamp": 1.2272576341680477 - }, - { - "x": 2.6837128162746895, - "y": 6.770286154899203, - "heading": 0.8037616945531193, - "angularVelocity": -0.015403237566514301, - "velocityX": -0.03029383829428867, - "velocityY": -0.025449058845075586, - "timestamp": 1.3078610776675168 - }, - { - "x": 2.6597554401054357, - "y": 6.750039490992799, - "heading": 0.790165886718463, - "angularVelocity": -0.1686752729700538, - "velocityX": -0.29722521928375617, - "velocityY": -0.2511885724392666, - "timestamp": 1.388464521166986 - }, - { - "x": 2.614282818196885, - "y": 6.71159714288196, - "heading": 0.7642144593793307, - "angularVelocity": -0.32196425130707784, - "velocityX": -0.5641523480167241, - "velocityY": -0.4769318337003116, - "timestamp": 1.469067964666455 - }, - { - "x": 2.547295402718566, - "y": 6.65495864191724, - "heading": 0.7259067765041173, - "angularVelocity": -0.4752611205179619, - "velocityX": -0.8310738669455855, - "velocityY": -0.702680909222201, - "timestamp": 1.5496714081659242 - }, - { - "x": 2.45879373609576, - "y": 6.580123315301835, - "heading": 0.6752436075608932, - "angularVelocity": -0.6285484433864407, - "velocityX": -1.0979886563197567, - "velocityY": -0.9284383317423857, - "timestamp": 1.6302748516653933 - }, - { - "x": 2.3487784541804713, - "y": 6.487090205965796, - "heading": 0.612227929672179, - "angularVelocity": -0.7817988308289465, - "velocityX": -1.3648955570482664, - "velocityY": -1.1542076280722422, - "timestamp": 1.7108782951648625 - }, - { - "x": 2.2172503640031413, - "y": 6.3758579034961445, - "heading": 0.536865388228733, - "angularVelocity": -0.934979179194793, - "velocityX": -1.6317924454199069, - "velocityY": -1.3799944225748537, - "timestamp": 1.7914817386643316 - }, - { - "x": 2.0642108270205712, - "y": 6.2464240090225065, - "heading": 0.4491644340858314, - "angularVelocity": -1.0880546827189126, - "velocityX": -1.8986724429895898, - "velocityY": -1.605810978465707, - "timestamp": 1.8720851821638007 - }, - { - "x": 1.892150582189426, - "y": 6.100772712319997, - "heading": 0.34952884105977006, - "angularVelocity": -1.236120799562961, - "velocityX": -2.134651292315194, - "velocityY": -1.8070108469188693, - "timestamp": 1.9526886256632698 - }, - { - "x": 1.7416020056028705, - "y": 5.973323101720471, - "heading": 0.2622670318762804, - "angularVelocity": -1.0826064668575972, - "velocityX": -1.8677685474764083, - "velocityY": -1.5811931235965593, - "timestamp": 2.033292069162739 - }, - { - "x": 1.6125631547118695, - "y": 5.864077706390992, - "heading": 0.18740079602910192, - "angularVelocity": -0.9288218045882161, - "velocityX": -1.600909902712647, - "velocityY": -1.3553440223708513, - "timestamp": 2.113895512662208 - }, - { - "x": 1.5050325714338102, - "y": 5.7730380802152625, - "heading": 0.12496122653203612, - "angularVelocity": -0.7746513893974771, - "velocityX": -1.3340693475310352, - "velocityY": -1.1294756430146329, - "timestamp": 2.1944989561616772 - }, - { - "x": 1.4190090841759289, - "y": 5.700205295684954, - "heading": 0.0749803520329015, - "angularVelocity": -0.6200836134188114, - "velocityX": -1.0672433276187945, - "velocityY": -0.9035939578786114, - "timestamp": 2.2751023996611464 - }, - { - "x": 1.3544918382019193, - "y": 5.645580092417221, - "heading": 0.037484747068995454, - "angularVelocity": -0.4651861426263309, - "velocityX": -0.8004279119223182, - "velocityY": -0.6777030967431115, - "timestamp": 2.3557058431606155 - }, - { - "x": 1.3114803343182175, - "y": 5.609162978300701, - "heading": 0.012490529490997385, - "angularVelocity": -0.31008870704351826, - "velocityX": -0.5336186894297898, - "velocityY": -0.45180593452897394, - "timestamp": 2.4363092866600846 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 1.0757846254441862e-30, - "angularVelocity": -0.1549627280016075, - "velocityX": -0.2668109738177861, - "velocityY": -0.22590442503082891, - "timestamp": 2.5169127301595537 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 2.1112287936414426e-30, - "angularVelocity": 1.2846097126015132e-29, - "velocityX": 1.1473496118122496e-30, - "velocityY": -2.480854756847108e-31, - "timestamp": 2.597516173659023 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.5500454228160796e-22, + "angularVelocity": -9.317544566447768e-23, + "velocityX": -6.472519788584152e-21, + "velocityY": -2.347379322977946e-21, + "timestamp": 0 + }, + { + "x": 1.3072065943694386, + "y": 5.605543739625148, + "heading": 0.00997794612171538, + "angularVelocity": 0.1382953520830163, + "velocityX": 0.23883926575200254, + "velocityY": 0.20221107105767083, + "timestamp": 0.07214954061309165 + }, + { + "x": 1.3416710394123585, + "y": 5.634722180998455, + "heading": 0.029930634172052512, + "angularVelocity": 0.2765462937225786, + "velocityX": 0.47768072741777556, + "velocityY": 0.4044161768094943, + "timestamp": 0.1442990812261833 + }, + { + "x": 1.3933681818707737, + "y": 5.678489073214057, + "heading": 0.05984711285989078, + "angularVelocity": 0.414645449348986, + "velocityX": 0.7165276732064818, + "velocityY": 0.6066135950928104, + "timestamp": 0.21644862183927494 + }, + { + "x": 1.4622986372338835, + "y": 5.736843692636249, + "heading": 0.09971100751764889, + "angularVelocity": 0.5525176504107191, + "velocityX": 0.9553831497383417, + "velocityY": 0.8088009837113753, + "timestamp": 0.2885981624523666 + }, + { + "x": 1.548463213011665, + "y": 5.809785098412347, + "heading": 0.14950349033581017, + "angularVelocity": 0.6901288961100662, + "velocityX": 1.194249818440935, + "velocityY": 1.0109753320156587, + "timestamp": 0.3607477030654582 + }, + { + "x": 1.6518628806196247, + "y": 5.897312083044383, + "heading": 0.20920651705409263, + "angularVelocity": 0.8274900465194276, + "velocityX": 1.4331299510616444, + "velocityY": 1.2131329442748184, + "timestamp": 0.43289724367854987 + }, + { + "x": 1.772498794450995, + "y": 5.999423091967104, + "heading": 0.2788055813251054, + "angularVelocity": 0.9646501374727254, + "velocityX": 1.6720260836904182, + "velocityY": 1.4152690101008414, + "timestamp": 0.5050467842916415 + }, + { + "x": 1.9103726539115997, + "y": 6.1161158275998355, + "heading": 0.35829010832272556, + "angularVelocity": 1.1016636602561762, + "velocityX": 1.9109457702574424, + "velocityY": 1.6173732312241136, + "timestamp": 0.5771963249047332 + }, + { + "x": 2.0655201843500692, + "y": 6.247352096059746, + "heading": 0.44752582385757206, + "angularVelocity": 1.2368161290642319, + "velocityX": 2.1503606138043514, + "velocityY": 1.8189480812314087, + "timestamp": 0.6493458655178248 + }, + { + "x": 2.203432352747445, + "y": 6.364005171819918, + "heading": 0.526860073391937, + "angularVelocity": 1.0995807992708087, + "velocityX": 1.911476736032763, + "velocityY": 1.6168235413408145, + "timestamp": 0.7214954061309165 + }, + { + "x": 2.3241076430309073, + "y": 6.466076944016547, + "heading": 0.596294464302791, + "angularVelocity": 0.9623677478863277, + "velocityX": 1.6725718453371032, + "velocityY": 1.4147251850707963, + "timestamp": 0.7936449467440081 + }, + { + "x": 2.427545315940862, + "y": 6.553568380245639, + "heading": 0.6558285941122223, + "angularVelocity": 0.8251491181168898, + "velocityX": 1.4336567084279637, + "velocityY": 1.2126402397802263, + "timestamp": 0.8657944873570997 + }, + { + "x": 2.51374484513646, + "y": 6.626480123164196, + "heading": 0.7054623538170143, + "angularVelocity": 0.687928977551743, + "velocityX": 1.1947342763809239, + "velocityY": 1.0105642017813024, + "timestamp": 0.9379440279701914 + }, + { + "x": 2.582705816133726, + "y": 6.684812629515298, + "heading": 0.7451960796056276, + "angularVelocity": 0.5507134965929852, + "velocityX": 0.9558060995436645, + "velocityY": 0.8084944942881265, + "timestamp": 1.010093568583283 + }, + { + "x": 2.6344279053527613, + "y": 6.728566223750801, + "heading": 0.7750303601927481, + "angularVelocity": 0.41350617527988826, + "velocityX": 0.7168734378559617, + "velocityY": 0.6064292837307924, + "timestamp": 1.0822431091963747 + }, + { + "x": 2.6689108803199226, + "y": 6.757741121003901, + "heading": 0.7949658633330718, + "angularVelocity": 0.2763081091150633, + "velocityX": 0.477937554059832, + "velocityY": 0.4043670549415301, + "timestamp": 1.1543926498094663 + }, + { + "x": 2.68615460395813, + "y": 6.772337436676025, + "heading": 0.8050032485420815, + "angularVelocity": 0.13911918390216763, + "velocityX": 0.23899977036136932, + "velocityY": 0.2023064256278295, + "timestamp": 1.226542190422558 + }, + { + "x": 2.6838905941834166, + "y": 6.770438484115006, + "heading": 0.8038568906266841, + "angularVelocity": -0.014214822664441809, + "velocityX": -0.028073690621275776, + "velocityY": -0.023546986103133997, + "timestamp": 1.3071874395725835 + }, + { + "x": 2.6600885687415188, + "y": 6.7503254094883065, + "heading": 0.7903430319109747, + "angularVelocity": -0.16757166551211813, + "velocityX": -0.2951447939309862, + "velocityY": -0.24940185365765316, + "timestamp": 1.3878326887226091 + }, + { + "x": 2.614748840901014, + "y": 6.711997955351817, + "heading": 0.7644600728285338, + "angularVelocity": -0.3209483429617853, + "velocityX": -0.562212012714593, + "velocityY": -0.47525991351563107, + "timestamp": 1.4684779378726347 + }, + { + "x": 2.547871822089795, + "y": 6.655455715047437, + "heading": 0.7262070502709054, + "angularVelocity": -0.47433696294329564, + "velocityX": -0.8292741297978391, + "velocityY": -0.7011230159286398, + "timestamp": 1.5491231870226603 + }, + { + "x": 2.45945799388215, + "y": 6.580698108278845, + "heading": 0.6755842481215987, + "angularVelocity": -0.627722062773128, + "velocityX": -1.0963302753664865, + "velocityY": -0.9269933140081175, + "timestamp": 1.6297684361726859 + }, + { + "x": 2.349507890636972, + "y": 6.487724331378611, + "heading": 0.6125938511497702, + "angularVelocity": -0.781080071494933, + "velocityX": -1.3633797948920177, + "velocityY": -1.152873577552878, + "timestamp": 1.7104136853227114 + }, + { + "x": 2.218022116383117, + "y": 6.3765332785035795, + "heading": 0.5372399641012893, + "angularVelocity": -0.9343871814234115, + "velocityX": -1.6304218244679243, + "velocityY": -1.3787675535378683, + "timestamp": 1.791058934472737 + }, + { + "x": 2.065001429825468, + "y": 6.2471234504060735, + "heading": 0.4495265554466888, + "angularVelocity": -1.0876450823708927, + "velocityX": -1.897454446113545, + "velocityY": -1.6046801201737573, + "timestamp": 1.8717041836227626 + }, + { + "x": 1.892766685520709, + "y": 6.101314959564872, + "heading": 0.34981930117759164, + "angularVelocity": -1.2363686059622758, + "velocityX": -2.1357085026093356, + "velocityY": -1.8080233166612498, + "timestamp": 1.9523494327727882 + }, + { + "x": 1.7420645817615519, + "y": 5.973729053643758, + "heading": 0.26248852330737293, + "angularVelocity": -1.08290046581363, + "velocityX": -1.868704050734679, + "velocityY": -1.582063509826458, + "timestamp": 2.0329946819228137 + }, + { + "x": 1.612893816606675, + "y": 5.8643673067305935, + "heading": 0.18756078557963143, + "angularVelocity": -0.929102935603218, + "velocityX": -1.6017157429147117, + "velocityY": -1.3560841843233018, + "timestamp": 2.1136399310728393 + }, + { + "x": 1.5052531418262654, + "y": 5.773230960685921, + "heading": 0.12506875746451004, + "angularVelocity": -0.7749003044043764, + "velocityX": -1.3347429131276467, + "velocityY": -1.1300894597663134, + "timestamp": 2.194285180222865 + }, + { + "x": 1.4191414897869865, + "y": 5.700320933784421, + "heading": 0.0750452611365071, + "angularVelocity": -0.6202906786851591, + "velocityX": -1.0677833219794879, + "velocityY": -0.9040833486156766, + "timestamp": 2.2749304293728905 + }, + { + "x": 1.3545580679213909, + "y": 5.645637873878687, + "heading": 0.03751734547660986, + "angularVelocity": -0.46534564720711064, + "velocityX": -0.8008335586570275, + "velocityY": -0.6780692041015978, + "timestamp": 2.355575678522916 + }, + { + "x": 1.311502418725051, + "y": 5.609182228096023, + "heading": 0.01250142936060969, + "angularVelocity": -0.31019702189105725, + "velocityX": -0.533889468383214, + "velocityY": -0.4520495152150475, + "timestamp": 2.4362209276729416 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741457, + "heading": 2.7832801817325185e-21, + "angularVelocity": -0.1550175551860858, + "velocityX": -0.2669465081562177, + "velocityY": -0.22602601575026499, + "timestamp": 2.516866176822967 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.3491121023361147e-21, + "angularVelocity": -9.888514076614425e-22, + "velocityX": -3.220372602008045e-20, + "velocityY": -2.3512590671688983e-20, + "timestamp": 2.597511425972993 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.4.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.4.traj index f1c1499b..189e89b2 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.4.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.4.traj @@ -1,472 +1,473 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 2.1112287936414426e-30, - "angularVelocity": 1.2846097126015132e-29, - "velocityX": 1.1473496118122496e-30, - "velocityY": -2.480854756847108e-31, - "timestamp": 0 - }, - { - "x": 1.3107332172473851, - "y": 5.587721554520521, - "heading": -5.6112723310966165e-9, - "angularVelocity": -8.086181485047517e-8, - "velocityX": 0.2991463288589595, - "velocityY": -0.04658586416314624, - "timestamp": 0.06939335094467669 - }, - { - "x": 1.3522507006814268, - "y": 5.581255743814154, - "heading": -1.6136427820687844e-8, - "angularVelocity": -1.516738315771432e-7, - "velocityX": 0.5982919525984769, - "velocityY": -0.09317622824587016, - "timestamp": 0.13878670188935338 - }, - { - "x": 1.414526842614822, - "y": 5.571556496717738, - "heading": -3.074141680025046e-8, - "angularVelocity": -2.1046669010163338e-7, - "velocityX": 0.8974367296809845, - "velocityY": -0.13977199493000497, - "timestamp": 0.20818005283403007 - }, - { - "x": 1.4975615711863124, - "y": 5.558623354737258, - "heading": -4.841098342837698e-8, - "angularVelocity": -2.546291003036043e-7, - "velocityX": 1.196580471199459, - "velocityY": -0.1863743687891775, - "timestamp": 0.27757340377870676 - }, - { - "x": 1.6013547964876313, - "y": 5.542455744326475, - "heading": -6.788238796631325e-8, - "angularVelocity": -2.8059467012358587e-7, - "velocityX": 1.4957229170856399, - "velocityY": -0.2329850077952488, - "timestamp": 0.34696675472338345 - }, - { - "x": 1.7259064027890496, - "y": 5.5230529273691795, - "heading": -8.754228960685774e-8, - "angularVelocity": -2.8331102694259857e-7, - "velocityX": 1.7948636952366863, - "velocityY": -0.27960628350061884, - "timestamp": 0.41636010566806014 - }, - { - "x": 1.8712162355508042, - "y": 5.500413918486819, - "heading": -1.0525703637468966e-7, - "angularVelocity": -2.552801733617599e-7, - "velocityX": 2.0940022463766295, - "velocityY": -0.3262417591047122, - "timestamp": 0.48575345661273683 - }, - { - "x": 2.0372840779688866, - "y": 5.474537335801765, - "heading": -1.1807218385471473e-7, - "angularVelocity": -1.8467399582926014e-7, - "velocityX": 2.393137673240176, - "velocityY": -0.3728971483980547, - "timestamp": 0.5551468075574135 - }, - { - "x": 2.2241096038561166, - "y": 5.445421101366719, - "heading": -1.2162497814922265e-7, - "angularVelocity": -5.119790624873975e-8, - "velocityX": 2.692268399549916, - "velocityY": -0.41958248216406707, - "timestamp": 0.6245401585020902 - }, - { - "x": 2.431692266948392, - "y": 5.413061738269229, - "heading": -1.0881824696224333e-7, - "angularVelocity": 1.845527118847122e-7, - "velocityX": 2.99139125386478, - "velocityY": -0.46631792033351455, - "timestamp": 0.6939335094467669 - }, - { - "x": 2.660030964874599, - "y": 5.377452241496065, - "heading": -6.605232487729304e-8, - "angularVelocity": 6.162827019703426e-7, - "velocityX": 3.290498222347668, - "velocityY": -0.5131543049644982, - "timestamp": 0.7633268603914427 - }, - { - "x": 2.9091223110729234, - "y": 5.338571186157936, - "heading": 4.364254321616404e-8, - "angularVelocity": 0.0000015807691363697316, - "velocityX": 3.5895563884342283, - "velocityY": -0.5602994351597471, - "timestamp": 0.8327202113361185 - }, - { - "x": 3.168557402823767, - "y": 5.298953853406429, - "heading": 4.3641894183803e-8, - "angularVelocity": -9.352947371519385e-12, - "velocityX": 3.738615994458649, - "velocityY": -0.5709096363289035, - "timestamp": 0.9021135622807943 - }, - { - "x": 3.4266802039415243, - "y": 5.251533194096367, - "heading": 4.3642354317485314e-8, - "angularVelocity": 6.630803609714539e-12, - "velocityX": 3.719705095716507, - "velocityY": -0.6833602739240744, - "timestamp": 0.9715069132254701 - }, - { - "x": 3.6803449026923616, - "y": 5.18422554387417, - "heading": 4.364233337760302e-8, - "angularVelocity": -3.0175631893268995e-13, - "velocityX": 3.6554611543845263, - "velocityY": -0.9699437958523844, - "timestamp": 1.040900264170146 - }, - { - "x": 3.9278131468583686, - "y": 5.096843813495861, - "heading": 4.364231130349348e-8, - "angularVelocity": -3.1810121673972006e-13, - "velocityX": 3.5661665101502296, - "velocityY": -1.2592233865168847, - "timestamp": 1.1102936151148217 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 4.364228801520247e-8, - "angularVelocity": -3.3559830331814666e-13, - "velocityX": 3.45401850865928, - "velocityY": -1.5404360240963577, - "timestamp": 1.1796869660594975 - }, - { - "x": 4.284862555065841, - "y": 4.931880235034004, - "heading": 4.364226510492993e-8, - "angularVelocity": -6.617055985872344e-13, - "velocityX": 3.3897491580338537, - "velocityY": -1.67713541924223, - "timestamp": 1.2143100264085245 - }, - { - "x": 4.399813643173519, - "y": 4.869172357324464, - "heading": 4.364224321511573e-8, - "angularVelocity": -6.322322259809295e-13, - "velocityX": 3.3200730076683276, - "velocityY": -1.81115929895842, - "timestamp": 1.2489330867575514 - }, - { - "x": 4.512168964860153, - "y": 4.801924216205689, - "heading": 4.364222211690104e-8, - "angularVelocity": -6.093688654191888e-13, - "velocityX": 3.245100824537311, - "velocityY": -1.9422933859936342, - "timestamp": 1.2835561471065784 - }, - { - "x": 4.621749311428581, - "y": 4.730243126546711, - "heading": 4.364220167371938e-8, - "angularVelocity": -5.904498836065766e-13, - "velocityX": 3.164952649008305, - "velocityY": -2.070327952999433, - "timestamp": 1.3181792074556054 - }, - { - "x": 4.728650036507715, - "y": 4.654623908914118, - "heading": 4.364241447304788e-8, - "angularVelocity": 6.146173210402922e-12, - "velocityX": 3.0875585231776617, - "velocityY": -2.184070872715801, - "timestamp": 1.3528022678046323 - }, - { - "x": 4.8372727310201835, - "y": 4.581499759061361, - "heading": 4.3642159997006347e-8, - "angularVelocity": -7.349900240613306e-12, - "velocityX": 3.137293278452735, - "velocityY": -2.112007116517435, - "timestamp": 1.3874253281536593 - }, - { - "x": 4.948612503037568, - "y": 4.5125832438429105, - "heading": 4.3642137671211746e-8, - "angularVelocity": -6.448244222387315e-13, - "velocityX": 3.2157692270698366, - "velocityY": -1.9904801748811223, - "timestamp": 1.4220483885026862 - }, - { - "x": 5.062589095284458, - "y": 4.448121106677303, - "heading": 4.364211455765666e-8, - "angularVelocity": -6.675769025667281e-13, - "velocityX": 3.2919271461828314, - "velocityY": -1.8618266703110185, - "timestamp": 1.4566714488517132 - }, - { - "x": 5.17904891222934, - "y": 4.388261758970167, - "heading": 4.3642090495999965e-8, - "angularVelocity": -6.94960432877984e-13, - "velocityX": 3.3636488447548265, - "velocityY": -1.728886675634883, - "timestamp": 1.4912945092007401 - }, - { - "x": 5.297806179962019, - "y": 4.333100707972697, - "heading": 4.364206516158606e-8, - "angularVelocity": -7.317208234311228e-13, - "velocityX": 3.4300049312658345, - "velocityY": -1.5931881942671235, - "timestamp": 1.525917569549767 - }, - { - "x": 5.418671454426997, - "y": 4.282725959570945, - "heading": 4.364203830903825e-8, - "angularVelocity": -7.755683028589255e-13, - "velocityX": 3.4908894028015367, - "velocityY": -1.454947884269515, - "timestamp": 1.560540629898794 - }, - { - "x": 5.541451926168614, - "y": 4.237217881100571, - "heading": 4.364200955008555e-8, - "angularVelocity": -8.306300130132497e-13, - "velocityX": 3.546205058244276, - "velocityY": -1.3143863659542425, - "timestamp": 1.595163690247821 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 4.364197825891865e-8, - "angularVelocity": -9.037666511803314e-13, - "velocityX": 3.5958636064268057, - "velocityY": -1.171727921713515, - "timestamp": 1.629786750596848 - }, - { - "x": 5.80048142988227, - "y": 4.159069716430897, - "heading": 4.364194761543742e-8, - "angularVelocity": -8.296994367988738e-13, - "velocityX": 3.642511096804761, - "velocityY": -1.0174944855729342, - "timestamp": 1.6667199808564561 - }, - { - "x": 5.936489765010674, - "y": 4.127254911109845, - "heading": 4.364191906311607e-8, - "angularVelocity": -7.730794282349154e-13, - "velocityX": 3.6825464269543904, - "velocityY": -0.8614141004570673, - "timestamp": 1.7036532111160643 - }, - { - "x": 6.073729843507157, - "y": 4.101262406827453, - "heading": 4.3641892220156324e-8, - "angularVelocity": -7.267969466454947e-13, - "velocityX": 3.7158969722335238, - "velocityY": -0.7037701305758657, - "timestamp": 1.7405864413756724 - }, - { - "x": 6.211952539114824, - "y": 4.081139381676687, - "heading": 4.3641866716107116e-8, - "angularVelocity": -6.905447558160659e-13, - "velocityX": 3.742502202923576, - "velocityY": -0.5448487719411382, - "timestamp": 1.7775196716352806 - }, - { - "x": 6.350906942398405, - "y": 4.066922357709394, - "heading": 4.364184226449404e-8, - "angularVelocity": -6.620491141311732e-13, - "velocityX": 3.7623138378867345, - "velocityY": -0.38493854632702584, - "timestamp": 1.8144529018948887 - }, - { - "x": 6.490340706014696, - "y": 4.058637143370339, - "heading": 4.364062068850958e-8, - "angularVelocity": -3.3075253340927824e-11, - "velocityX": 3.775292944489131, - "velocityY": -0.22432953415709472, - "timestamp": 1.8513861321544969 - }, - { - "x": 6.625895301013595, - "y": 4.054865050461972, - "heading": 3.8021532173824235e-8, - "angularVelocity": -1.521418068678583e-7, - "velocityX": 3.6702610101003303, - "velocityY": -0.10213276450108619, - "timestamp": 1.888319362414105 - }, - { - "x": 6.755934035567496, - "y": 4.053327166371606, - "heading": 3.0189413996037156e-8, - "angularVelocity": -2.1206154817940447e-7, - "velocityX": 3.52091419136215, - "velocityY": -0.04163957713841639, - "timestamp": 1.9252525926737132 - }, - { - "x": 6.880153762983055, - "y": 4.0530365385095966, - "heading": 2.1734826824788128e-8, - "angularVelocity": -2.2891544824669977e-7, - "velocityX": 3.3633594067565498, - "velocityY": -0.007869007394285612, - "timestamp": 1.9621858229333213 - }, - { - "x": 6.998470106919647, - "y": 4.0534986388519805, - "heading": 1.3540571224920633e-8, - "angularVelocity": -2.2186673987685247e-7, - "velocityX": 3.2035200578159904, - "velocityY": 0.012511777040293628, - "timestamp": 1.9991190531929295 - }, - { - "x": 7.110853220415867, - "y": 4.054422625435363, - "heading": 6.1666659449803414e-9, - "angularVelocity": -1.9965502711211782e-7, - "velocityX": 3.042872575896168, - "velocityY": 0.025017757095422304, - "timestamp": 2.0360522834525376 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -3.6086824530888626e-33, - "angularVelocity": -1.6696795107584883e-7, - "velocityX": 2.8819069972765123, - "velocityY": 0.03237357493636809, - "timestamp": 2.072985513712146 - }, - { - "x": 7.405345242374078, - "y": 4.058539790558069, - "heading": -6.074177156590073e-9, - "angularVelocity": -8.274407475492158e-8, - "velocityX": 2.561720622548375, - "velocityY": 0.03979751891783301, - "timestamp": 2.146394723750806 - }, - { - "x": 7.569888594263639, - "y": 4.061600743201228, - "heading": -8.336905285122374e-9, - "angularVelocity": -3.0823491082343837e-8, - "velocityX": 2.241453787650163, - "velocityY": 0.04169712004183413, - "timestamp": 2.219803933789466 - }, - { - "x": 7.71092123884848, - "y": 4.064554734769034, - "heading": -8.259320686300401e-9, - "angularVelocity": 1.056878269798436e-9, - "velocityX": 1.9211846103583805, - "velocityY": 0.04024006750993344, - "timestamp": 2.293213143828126 - }, - { - "x": 7.828444510796415, - "y": 4.067236236264996, - "heading": -6.843181194470064e-9, - "angularVelocity": 1.929103299991646e-8, - "velocityX": 1.600933614270509, - "velocityY": 0.03652813447454146, - "timestamp": 2.366622353866786 - }, - { - "x": 7.922460087151161, - "y": 4.069526407614099, - "heading": -4.8146064610859125e-9, - "angularVelocity": 2.7633790562623428e-8, - "velocityX": 1.2807054633231172, - "velocityY": 0.031197329979384493, - "timestamp": 2.440031563905446 - }, - { - "x": 7.992969626740666, - "y": 4.071335791937442, - "heading": -2.7240893137969417e-9, - "angularVelocity": 2.8477586783070736e-8, - "velocityX": 0.9604999093761202, - "velocityY": 0.024647919823550288, - "timestamp": 2.5134407739441063 - }, - { - "x": 8.039974659720452, - "y": 4.072594619838684, - "heading": -1.0033424996203534e-9, - "angularVelocity": 2.3440475863128373e-8, - "velocityX": 0.6403151996190951, - "velocityY": 0.01714809218869153, - "timestamp": 2.5868499839827663 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 1.0901032811452175e-35, - "angularVelocity": 1.3667801358727972e-8, - "velocityX": 0.32014923968219366, - "velocityY": 0.00888629686322534, - "timestamp": 2.6602591940214264 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 2.780011652171749e-35, - "angularVelocity": 2.3020443776927775e-34, - "velocityX": -1.517814230631444e-35, - "velocityY": 4.908282159537256e-34, - "timestamp": 2.7336684040600865 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.3491121023361147e-21, + "angularVelocity": -9.888514076614425e-22, + "velocityX": -3.220372602008045e-20, + "velocityY": -2.3512590671688983e-20, + "timestamp": 0 + }, + { + "x": 1.310729419094693, + "y": 5.587708246254127, + "heading": 1.3104272140971465e-18, + "angularVelocity": 1.8865367753568018e-17, + "velocityX": 0.2991037005434092, + "velocityY": -0.04677953756663729, + "timestamp": 0.06939054245040044 + }, + { + "x": 1.3522393548624092, + "y": 5.581216131325057, + "heading": -6.810029035561697e-19, + "angularVelocity": -2.869886943346906e-17, + "velocityX": 0.5982073968853444, + "velocityY": -0.0935590744763348, + "timestamp": 0.13878108490080088 + }, + { + "x": 1.4145042580183609, + "y": 5.571477959008947, + "heading": -1.7830392147175414e-18, + "angularVelocity": -1.5881650038949302e-17, + "velocityX": 0.8973110881855106, + "velocityY": -0.1403386105977046, + "timestamp": 0.20817162735120132 + }, + { + "x": 1.4975241281349527, + "y": 5.558493729372655, + "heading": -6.515620811670549e-18, + "angularVelocity": -6.820211270699198e-17, + "velocityX": 1.1964147733235146, + "velocityY": -0.18711814575556276, + "timestamp": 0.27756216980160175 + }, + { + "x": 1.6012989646776896, + "y": 5.542263442499756, + "heading": -1.6376922566585543e-17, + "angularVelocity": -1.4211305181843934e-16, + "velocityX": 1.4955184507588155, + "velocityY": -0.23389767970903125, + "timestamp": 0.3469527122520022 + }, + { + "x": 1.725828766959364, + "y": 5.522787098497699, + "heading": -2.5308091780963488e-17, + "angularVelocity": -1.2870873895834211e-16, + "velocityX": 1.7946221182906406, + "velocityY": -0.28067721211399865, + "timestamp": 0.41634325470240263 + }, + { + "x": 1.8711135340636993, + "y": 5.500064697509755, + "heading": -3.3349995940615036e-17, + "angularVelocity": -1.1589337502943322e-16, + "velocityX": 2.0937257726178307, + "velocityY": -0.3274567424542979, + "timestamp": 0.48573379715280307 + }, + { + "x": 2.0371532647079076, + "y": 5.474096239736498, + "heading": -4.30035583427312e-17, + "angularVelocity": -1.3911928140733344e-16, + "velocityX": 2.3928294084585304, + "velocityY": -0.37423626990406167, + "timestamp": 0.5551243396032035 + }, + { + "x": 2.223947956967808, + "y": 5.444881725478791, + "heading": -5.0761776746981987e-17, + "angularVelocity": -1.118051268996076e-16, + "velocityX": 2.691933016569494, + "velocityY": -0.4210157930180218, + "timestamp": 0.624514882053604 + }, + { + "x": 2.4314976076364307, + "y": 5.412421155238076, + "heading": -5.64791774774602e-17, + "angularVelocity": -8.239452421940524e-17, + "velocityX": 2.991036578464229, + "velocityY": -0.4677953089056427, + "timestamp": 0.6939054245040053 + }, + { + "x": 2.659802210299839, + "y": 5.37671453001723, + "heading": -6.06625739671826e-17, + "angularVelocity": -6.028770408822991e-17, + "velocityX": 3.290140047926519, + "velocityY": -0.5145748103405868, + "timestamp": 0.7632959669544066 + }, + { + "x": 2.9088617457162362, + "y": 5.337761852824889, + "heading": -6.241000651317059e-17, + "angularVelocity": -2.5182575093519166e-17, + "velocityX": 3.589243240091715, + "velocityY": -0.5613542684175394, + "timestamp": 0.8326865094048079 + }, + { + "x": 3.168141716720517, + "y": 5.297210399977503, + "heading": -4.3453663669154356e-17, + "angularVelocity": 2.7318337880868624e-16, + "velocityX": 3.7365318363033526, + "velocityY": -0.5843945214345647, + "timestamp": 0.9020770518552093 + }, + { + "x": 3.426418453660738, + "y": 5.250695339800734, + "heading": -4.238140416902354e-17, + "angularVelocity": 1.5452530881159203e-17, + "velocityX": 3.7220740438055158, + "velocityY": -0.6703371746952049, + "timestamp": 0.9714675943056106 + }, + { + "x": 3.680147658052144, + "y": 5.183672848816506, + "heading": -4.587291099772401e-17, + "angularVelocity": -5.0316753627269466e-17, + "velocityX": 3.65653870731402, + "velocityY": -0.9658735703375574, + "timestamp": 1.040858136756012 + }, + { + "x": 3.9277032968256567, + "y": 5.096571019436482, + "heading": -4.576332276708217e-17, + "angularVelocity": 1.5792963566594492e-18, + "velocityX": 3.567570306147443, + "velocityY": -1.2552406466959498, + "timestamp": 1.1102486792064132 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -5.405308439751368e-17, + "angularVelocity": -1.194652950873824e-16, + "velocityX": 3.455741374913111, + "velocityY": -1.5365670854176536, + "timestamp": 1.1796392216568146 + }, + { + "x": 4.284931451916926, + "y": 4.932009555387415, + "heading": -5.371492234038301e-17, + "angularVelocity": 9.766625728969003e-18, + "velocityX": 3.3916228655411733, + "velocityY": -1.6733429936143844, + "timestamp": 1.2142634682706896 + }, + { + "x": 4.39995646079623, + "y": 4.869427987028382, + "heading": -6.433424938599782e-17, + "angularVelocity": -3.067020392759585e-16, + "velocityX": 3.32209420069255, + "velocityY": -1.8074492437895704, + "timestamp": 1.2488877148845647 + }, + { + "x": 4.51239059708215, + "y": 4.8023029501801755, + "heading": -5.362012153015504e-17, + "angularVelocity": 3.0944002843229445e-16, + "velocityX": 3.2472659272494777, + "velocityY": -1.9386714055261611, + "timestamp": 1.2835119614984398 + }, + { + "x": 4.622054515133808, + "y": 4.730741572190097, + "heading": -5.247217419715393e-17, + "angularVelocity": 3.315443500231424e-17, + "velocityX": 3.1672578836043357, + "velocityY": -2.06679956933421, + "timestamp": 1.318136208112315 + }, + { + "x": 4.728773352551376, + "y": 4.65485813780801, + "heading": -6.448401171604869e-17, + "angularVelocity": -3.469198233269833e-16, + "velocityX": 3.08219955246049, + "velocityY": -2.19162701872845, + "timestamp": 1.35276045472619 + }, + { + "x": 4.837250041803269, + "y": 4.581509560989999, + "heading": -7.180181418195748e-17, + "angularVelocity": -2.1134907415213478e-16, + "velocityX": 3.1329689411472432, + "velocityY": -2.1184165430654605, + "timestamp": 1.387384701340065 + }, + { + "x": 4.948569206649129, + "y": 4.512551245909545, + "heading": -5.993451372549524e-17, + "angularVelocity": 3.427453769485116e-16, + "velocityX": 3.2150638853539646, + "velocityY": -1.9916192213354724, + "timestamp": 1.4220089479539402 + }, + { + "x": 5.062553327275946, + "y": 4.4480933081859515, + "heading": -6.273480456472046e-17, + "angularVelocity": -8.08765854062184e-17, + "velocityX": 3.2920317919968585, + "velocityY": -1.8616415959145531, + "timestamp": 1.4566331945678153 + }, + { + "x": 5.17902058017923, + "y": 4.38823861530867, + "heading": -5.454489606023899e-17, + "angularVelocity": 2.365368002574791e-16, + "velocityX": 3.3637483640325954, + "velocityY": -1.728693003627544, + "timestamp": 1.4912574411816903 + }, + { + "x": 5.297785167201245, + "y": 4.333082674122234, + "heading": -4.169311661554588e-17, + "angularVelocity": 3.711786017787193e-16, + "velocityX": 3.43009880753392, + "velocityY": -1.5929860309027757, + "timestamp": 1.5258816877955654 + }, + { + "x": 5.4186576198068925, + "y": 4.282713488635075, + "heading": -3.1063280252894355e-17, + "angularVelocity": 3.0700556411606523e-16, + "velocityX": 3.490977116516063, + "velocityY": -1.4547373708623472, + "timestamp": 1.5605059344094405 + }, + { + "x": 5.541445103981359, + "y": 4.23721142295766, + "heading": -1.7411383468509857e-17, + "angularVelocity": 3.9428718659707233e-16, + "velocityX": 3.5462860908939025, + "velocityY": -1.3141676751799676, + "timestamp": 1.5951301810233156 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -2.060214519362369e-17, + "angularVelocity": -9.215396827963617e-17, + "velocityX": 3.5959374431427245, + "velocityY": -1.1715012562024099, + "timestamp": 1.6297544276371907 + }, + { + "x": 5.800482065965206, + "y": 4.159078601267801, + "heading": -2.3136950753163445e-17, + "angularVelocity": -6.863298410721474e-17, + "velocityX": 3.6425746580693774, + "velocityY": -1.0172668618134628, + "timestamp": 1.6666871880533387 + }, + { + "x": 5.936490640801473, + "y": 4.1272726257790175, + "heading": -1.8598714484333613e-17, + "angularVelocity": 1.228783394072173e-16, + "velocityX": 3.682599765188514, + "velocityY": -0.8611859804250445, + "timestamp": 1.7036199484694867 + }, + { + "x": 6.073730568489753, + "y": 4.101288878730342, + "heading": -4.2719996436236496e-17, + "angularVelocity": -6.531134331502948e-16, + "velocityX": 3.7159401610359515, + "velocityY": -0.7035419707570875, + "timestamp": 1.7405527088856347 + }, + { + "x": 6.2119527294046275, + "y": 4.081174520992199, + "heading": -2.525874984240029e-17, + "angularVelocity": 4.727847688027027e-16, + "velocityX": 3.742535336038484, + "velocityY": -0.5446210224066904, + "timestamp": 1.7774854693017828 + }, + { + "x": 6.350906221496031, + "y": 4.066966057794483, + "heading": 5.651500811291553e-19, + "angularVelocity": 6.992139128857502e-16, + "velocityX": 3.7623370288523534, + "velocityY": -0.3847116499719891, + "timestamp": 1.8144182297179308 + }, + { + "x": 6.49033871531031, + "y": 4.058689280403353, + "heading": 9.896194803608596e-18, + "angularVelocity": 2.526495343979898e-16, + "velocityX": 3.7753065907664864, + "velocityY": -0.2241039472238093, + "timestamp": 1.8513509901340788 + }, + { + "x": 6.62590091042267, + "y": 4.054933032848218, + "heading": 1.2997488502950548e-17, + "angularVelocity": 8.397134860988979e-17, + "velocityX": 3.67051348409614, + "velocityY": -0.10170503132750916, + "timestamp": 1.8882837505502268 + }, + { + "x": 6.7559406325562135, + "y": 4.053394192007746, + "heading": 1.2355813206346684e-17, + "angularVelocity": -1.7374149364595927e-17, + "velocityX": 3.520985722927149, + "velocityY": -0.04166601204816814, + "timestamp": 1.9252165109663748 + }, + { + "x": 6.880159033617416, + "y": 4.05309103302126, + "heading": 1.0199232759050705e-17, + "angularVelocity": -5.839207311638201e-17, + "velocityX": 3.3633662813596223, + "velocityY": -0.008208403137756505, + "timestamp": 1.9621492713825228 + }, + { + "x": 6.998473495645727, + "y": 4.053535026725706, + "heading": 6.484491281135395e-18, + "angularVelocity": -1.0058120312082129e-16, + "velocityX": 3.2035098567011917, + "velocityY": 0.012021676675224401, + "timestamp": 1.9990820317986708 + }, + { + "x": 7.1108547909012785, + "y": 4.054439771564095, + "heading": 3.84540621087932e-18, + "angularVelocity": -7.14564803668238e-17, + "velocityX": 3.0428620549689076, + "velocityY": 0.024497081404008324, + "timestamp": 2.036014792214819 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 9.032556666486843e-28, + "angularVelocity": -1.0411911175769075e-16, + "velocityX": 2.881901136889931, + "velocityY": 0.03190973421532392, + "timestamp": 2.072947552630967 + }, + { + "x": 7.405345313483542, + "y": 4.0585235652542355, + "heading": -6.2741861661918545e-18, + "angularVelocity": -8.546860903842953e-17, + "velocityX": 2.5617203276716056, + "velocityY": 0.039576473944975675, + "timestamp": 2.14635679887816 + }, + { + "x": 7.569888777644686, + "y": 4.061578730558428, + "heading": -8.568202082154757e-18, + "angularVelocity": -3.124968629224788e-17, + "velocityX": 2.241454211463674, + "velocityY": 0.04161826282625519, + "timestamp": 2.219766045125353 + }, + { + "x": 7.710921484094839, + "y": 4.064533122417797, + "heading": -8.450454397008985e-18, + "angularVelocity": 1.603989837150655e-18, + "velocityX": 1.9211845054947108, + "velocityY": 0.040245500538451925, + "timestamp": 2.2931752913725463 + }, + { + "x": 7.828444755273268, + "y": 4.067218459024374, + "heading": -7.03981174142149e-18, + "angularVelocity": 1.921614411657278e-17, + "velocityX": 1.6009328141401953, + "velocityY": 0.03658035933967648, + "timestamp": 2.3665845376197394 + }, + { + "x": 7.922460282697814, + "y": 4.069513975215649, + "heading": -4.798172497228405e-18, + "angularVelocity": 3.053619753065246e-17, + "velocityX": 1.2807041650852087, + "velocityY": 0.03127012343303199, + "timestamp": 2.4399937838669326 + }, + { + "x": 7.992969748455868, + "y": 4.071328793464346, + "heading": -2.7225824385142056e-18, + "angularVelocity": 2.8274231988604325e-17, + "velocityX": 0.9604984298657211, + "velocityY": 0.024721930022069162, + "timestamp": 2.5134030301141257 + }, + { + "x": 8.039974708011181, + "y": 4.0725920534308, + "heading": -1.0566305754795318e-18, + "angularVelocity": 2.2694033081505876e-17, + "velocityX": 0.6403138835812464, + "velocityY": 0.01720845848492492, + "timestamp": 2.586812276361319 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": -4.292440471277117e-28, + "angularVelocity": 1.4393698734895493e-17, + "velocityX": 0.32014842394214194, + "velocityY": 0.0089212527612154, + "timestamp": 2.660221522608512 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": -2.12372396116075e-28, + "angularVelocity": 6.129002987458633e-29, + "velocityX": -2.550409184277031e-23, + "velocityY": -6.242614501745333e-23, + "timestamp": 2.733630768855705 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.traj b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.traj index 9a2599ac..0ad670b8 100644 --- a/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.traj +++ b/src/main/deploy/choreo/CenterLanePSubCSubBSubASubSprint.traj @@ -1,1282 +1,1283 @@ { - "samples": [ - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 0, - "angularVelocity": -7.2072188294937895e-31, - "velocityX": -1.1048793733131498e-32, - "velocityY": -5.425421859025623e-32, - "timestamp": 0 - }, - { - "x": 1.3063416372973582, - "y": 5.5750227907482355, - "heading": -0.009007895171544168, - "angularVelocity": -0.12420787311314503, - "velocityX": 0.22568350897133957, - "velocityY": -0.21967610709029223, - "timestamp": 0.07252273906455099 - }, - { - "x": 1.339076097606074, - "y": 5.5431599730454195, - "heading": -0.02701891548605667, - "angularVelocity": -0.24834997335784384, - "velocityX": 0.4513682292059063, - "velocityY": -0.43934934220379995, - "timestamp": 0.14504547812910198 - }, - { - "x": 1.388178099815531, - "y": 5.4953660820311345, - "heading": -0.05401968965283247, - "angularVelocity": -0.3723076998340179, - "velocityX": 0.677056642410746, - "velocityY": -0.6590193866197215, - "timestamp": 0.217568217193653 - }, - { - "x": 1.4536480974194865, - "y": 5.431641407654605, - "heading": -0.08998953758011774, - "angularVelocity": -0.49598027310141063, - "velocityX": 0.9027513087406556, - "velocityY": -0.8786854329899796, - "timestamp": 0.29009095625820397 - }, - { - "x": 1.5354867341790108, - "y": 5.351986341420191, - "heading": -0.13490221177586337, - "angularVelocity": -0.6192909255093908, - "velocityX": 1.1284548517495026, - "velocityY": -1.0983460809932237, - "timestamp": 0.36261369532275495 - }, - { - "x": 1.6336948546750298, - "y": 5.2564014251813616, - "heading": -0.18872783549465025, - "angularVelocity": -0.7421896140863778, - "velocityX": 1.3541700404978287, - "velocityY": -1.317999257498918, - "timestamp": 0.4351364343873059 - }, - { - "x": 1.7482735602297292, - "y": 5.144887415721353, - "heading": -0.25143410509925246, - "angularVelocity": -0.8646428749583058, - "velocityX": 1.579900415134063, - "velocityY": -1.5376419988880228, - "timestamp": 0.507659173451857 - }, - { - "x": 1.87922458951427, - "y": 5.017445479597787, - "heading": -0.32298143653104017, - "angularVelocity": -0.9865503200055588, - "velocityX": 1.8056547639221738, - "velocityY": -1.7572686548722531, - "timestamp": 0.5801819125164079 - }, - { - "x": 2.026417113452321, - "y": 4.874266246183284, - "heading": -0.4026826239248217, - "angularVelocity": -1.0989820354525424, - "velocityX": 2.0296051395270776, - "velocityY": -1.9742667646213359, - "timestamp": 0.6527046515809589 - }, - { - "x": 2.1572394289562786, - "y": 4.747014797378468, - "heading": -0.4735071808339005, - "angularVelocity": -0.9765841420579419, - "velocityX": 1.8038799580839464, - "velocityY": -1.7546420673865035, - "timestamp": 0.7252273906455099 - }, - { - "x": 2.27168995904481, - "y": 4.635690037684074, - "heading": -0.535478944048102, - "angularVelocity": -0.8545149288832774, - "velocityX": 1.578133031994185, - "velocityY": -1.5350324757486038, - "timestamp": 0.7977501297100609 - }, - { - "x": 2.369767924446142, - "y": 4.540291261559593, - "heading": -0.5886064902849578, - "angularVelocity": -0.732563978168669, - "velocityX": 1.35237536069838, - "velocityY": -1.3154326126526381, - "timestamp": 0.8702728687746119 - }, - { - "x": 2.4514728066095985, - "y": 4.460817939321001, - "heading": -0.6328935848970949, - "angularVelocity": -0.6106649470681784, - "velocityX": 1.1266105392220715, - "velocityY": -1.0958400532524892, - "timestamp": 0.9427956078391628 - }, - { - "x": 2.5168042340609067, - "y": 4.397269668318764, - "heading": -0.6683418026498548, - "angularVelocity": -0.48878763005849607, - "velocityX": 0.9008405955692155, - "velocityY": -0.8762530458976289, - "timestamp": 1.015318346903714 - }, - { - "x": 2.5657619428441123, - "y": 4.349646151783931, - "heading": -0.6949516025618677, - "angularVelocity": -0.36691664235512766, - "velocityX": 0.6750670122861077, - "velocityY": -0.6566701306241847, - "timestamp": 1.087841085968265 - }, - { - "x": 2.5983457601971756, - "y": 4.317947187379743, - "heading": -0.7127228546140986, - "angularVelocity": -0.24504386184805346, - "velocityX": 0.4492910468254882, - "velocityY": -0.4370900053275183, - "timestamp": 1.160363825032816 - }, - { - "x": 2.614555597305298, - "y": 4.302172660827637, - "heading": -0.7216551150961179, - "angularVelocity": -0.12316496311606469, - "velocityX": 0.22351385671724336, - "velocityY": -0.21751145579268175, - "timestamp": 1.2328865640973672 - }, - { - "x": 2.6122418058900085, - "y": 4.3044120467003895, - "heading": -0.7206083682746678, - "angularVelocity": 0.01292691365951148, - "velocityX": -0.028574418607166113, - "velocityY": 0.027655539271243604, - "timestamp": 1.3138607895750445 - }, - { - "x": 2.589515477392911, - "y": 4.326503828401164, - "heading": -0.7085432954922883, - "angularVelocity": 0.148998927882974, - "velocityX": -0.2806612642842681, - "velocityY": 0.2728248596434546, - "timestamp": 1.3948350150527218 - }, - { - "x": 2.54637684601469, - "y": 4.3684483333856905, - "heading": -0.6854613533337227, - "angularVelocity": 0.2850529538548107, - "velocityX": -0.5327452176745692, - "velocityY": 0.5179982239657722, - "timestamp": 1.475809240530399 - }, - { - "x": 2.4828262685453337, - "y": 4.430246041385043, - "heading": -0.6513636060932391, - "angularVelocity": 0.42109383620888585, - "velocityX": -0.784824764848103, - "velocityY": 0.7631775127791823, - "timestamp": 1.5567834660080764 - }, - { - "x": 2.398864238908538, - "y": 4.511897605460961, - "heading": -0.6062501465002338, - "angularVelocity": 0.5571335733911974, - "velocityX": -1.0368982122572787, - "velocityY": 1.0083648666494986, - "timestamp": 1.6377576914857537 - }, - { - "x": 2.2944914266815206, - "y": 4.613403885366389, - "heading": -0.5501188636580988, - "angularVelocity": 0.6931993793206396, - "velocityX": -1.2889633906498084, - "velocityY": 1.2535628381323076, - "timestamp": 1.718731916963431 - }, - { - "x": 2.1697087770291006, - "y": 4.73476601037198, - "heading": -0.48296289489208905, - "angularVelocity": 0.8293499365978624, - "velocityX": -1.5410168966266635, - "velocityY": 1.4987747556657698, - "timestamp": 1.7997061424411083 - }, - { - "x": 2.0245178868987646, - "y": 4.87598561985106, - "heading": -0.4047647421109401, - "angularVelocity": 0.9657165884553096, - "velocityX": -1.7930506809269309, - "velocityY": 1.7440069188183338, - "timestamp": 1.8806803679187856 - }, - { - "x": 1.8612798092271896, - "y": 5.034863868368553, - "heading": -0.31505899294827, - "angularVelocity": 1.1078309009257712, - "velocityX": -2.0159263853239904, - "velocityY": 1.9620842012417825, - "timestamp": 1.9616545933964629 - }, - { - "x": 1.7184500483895917, - "y": 5.1738844559195085, - "heading": -0.23644393225782434, - "angularVelocity": 0.9708652380027345, - "velocityX": -1.7638916580562496, - "velocityY": 1.7168498584691285, - "timestamp": 2.04262881887414 - }, - { - "x": 1.5960267405860193, - "y": 5.293046114041946, - "heading": -0.168968478820667, - "angularVelocity": 0.8332954472751476, - "velocityX": -1.5118799479886125, - "velocityY": 1.4715998506873489, - "timestamp": 2.1236030443518175 - }, - { - "x": 1.4940086408037947, - "y": 5.392348179003114, - "heading": -0.1126782173092116, - "angularVelocity": 0.6951627036802206, - "velocityX": -1.2598836133403888, - "velocityY": 1.2263416460657963, - "timestamp": 2.2045772698294948 - }, - { - "x": 1.4123948378953157, - "y": 5.471790254952654, - "heading": -0.06761272810355828, - "angularVelocity": 0.5565411578785837, - "velocityX": -1.0078985310085185, - "velocityY": 0.9810785528472173, - "timestamp": 2.285551495307172 - }, - { - "x": 1.3511847156576737, - "y": 5.531372091014257, - "heading": -0.03380194743073204, - "angularVelocity": 0.417549911387026, - "velocityX": -0.7559210585414546, - "velocityY": 0.7358123614043836, - "timestamp": 2.3665257207848494 - }, - { - "x": 1.3103779403872662, - "y": 5.571093500047757, - "heading": -0.011263323026729973, - "angularVelocity": 0.27834319218308196, - "velocityX": -0.50394770718331, - "velocityY": 0.4905438588537352, - "timestamp": 2.4474999462625266 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 4.4390879396333275e-31, - "angularVelocity": 0.13909763212046558, - "velocityX": -0.25197510938629825, - "velocityY": 0.24527315422336363, - "timestamp": 2.528474171740204 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 2.494908675164405e-31, - "angularVelocity": -2.400971097599731e-30, - "velocityX": 9.117669624782265e-34, - "velocityY": -1.69916768015881e-32, - "timestamp": 2.6094483972178812 - }, - { - "x": 1.3242823472826057, - "y": 5.590457081039532, - "heading": -3.1525835471969304e-19, - "angularVelocity": -3.554932323947037e-18, - "velocityX": 0.3868644475376546, - "velocityY": -0.00560680796813751, - "timestamp": 2.6981303540012136 - }, - { - "x": 1.3928981385918453, - "y": 5.589462635652003, - "heading": -9.247304393859629e-19, - "angularVelocity": -6.8725601776619685e-18, - "velocityX": 0.7737288823800034, - "velocityY": -0.011213615752282564, - "timestamp": 2.786812310784546 - }, - { - "x": 1.4958218231349187, - "y": 5.587970967605794, - "heading": -1.5461935946313141e-18, - "angularVelocity": -7.007774498748477e-18, - "velocityX": 1.1605932962726162, - "velocityY": -0.016820423232804093, - "timestamp": 2.8754942675678783 - }, - { - "x": 1.6330533972641115, - "y": 5.585982076953771, - "heading": -2.4826620193785056e-18, - "angularVelocity": -1.0559852970262818e-17, - "velocityX": 1.5474576690326838, - "velocityY": -0.022427230117193615, - "timestamp": 2.9641762243512106 - }, - { - "x": 1.8045928505515578, - "y": 5.5834959638470645, - "heading": -3.548549667861495e-18, - "angularVelocity": -1.201921661569969e-17, - "velocityX": 1.934321924205522, - "velocityY": -0.028034035297397054, - "timestamp": 3.052858181134543 - }, - { - "x": 2.0104398981448357, - "y": 5.5805126324140275, - "heading": -4.504226548304647e-18, - "angularVelocity": -1.0776452337176765e-17, - "velocityX": 2.321182967310963, - "velocityY": -0.033640793925260974, - "timestamp": 3.1415401379178753 - }, - { - "x": 2.1819790701409207, - "y": 5.578026523384064, - "heading": -4.574305172786567e-18, - "angularVelocity": -7.902241563425968e-19, - "velocityX": 1.934318752293539, - "velocityY": -0.02803398932702808, - "timestamp": 3.2302220947012077 - }, - { - "x": 2.31921035343729, - "y": 5.576037636947067, - "heading": -3.929985868072649e-18, - "angularVelocity": 7.2655061760547e-18, - "velocityX": 1.547454389528782, - "velocityY": -0.022427182587500143, - "timestamp": 3.31890405148454 - }, - { - "x": 2.422133743893467, - "y": 5.574545973163046, - "heading": -2.934544233389305e-18, - "angularVelocity": 1.1224849685211679e-17, - "velocityX": 1.1605899800749648, - "velocityY": -0.016820375171309878, - "timestamp": 3.4075860082678724 - }, - { - "x": 2.490749239479307, - "y": 5.573551532061422, - "heading": -1.705878005512682e-18, - "angularVelocity": 1.385474872728055e-17, - "velocityX": 0.7737255477287375, - "velocityY": -0.011213567423341012, - "timestamp": 3.4962679650512047 - }, - { - "x": 2.525056838989258, - "y": 5.573054313659668, - "heading": 0, - "angularVelocity": 1.9235908491289617e-17, - "velocityX": 0.3868611017883933, - "velocityY": -0.005606759478353257, - "timestamp": 3.584949921834537 - }, - { - "x": 2.5176787496862993, - "y": 5.573161243955685, - "heading": 2.6679509724403924e-18, - "angularVelocity": 2.5453436150449368e-17, - "velocityX": -0.07039024589473145, - "velocityY": 0.0010201624731186972, - "timestamp": 3.689766849367258 - }, - { - "x": 2.4623729801708616, - "y": 5.573962787832815, - "heading": 6.066089004278022e-18, - "angularVelocity": 3.2419744709429903e-17, - "velocityX": -0.5276415824931802, - "velocityY": 0.007647084263940974, - "timestamp": 3.794583776899979 - }, - { - "x": 2.3591395323793796, - "y": 5.575458945262994, - "heading": 1.0152873197492895e-17, - "angularVelocity": 3.898973466799139e-17, - "velocityX": -0.9848929006171766, - "velocityY": 0.014274005787013901, - "timestamp": 3.8994007044326997 - }, - { - "x": 2.207978410184721, - "y": 5.577649716190093, - "heading": 1.5606892400662525e-17, - "angularVelocity": 5.2033763358184983e-17, - "velocityX": -1.4421441817922973, - "velocityY": 0.020900926774588543, - "timestamp": 4.004217631965421 - }, - { - "x": 2.0088896252054647, - "y": 5.580535100445723, - "heading": 1.7204191476600093e-18, - "angularVelocity": -1.324831168006478e-16, - "velocityX": -1.8993953521210223, - "velocityY": 0.02752784615567168, - "timestamp": 4.109034559498142 - }, - { - "x": 1.769251248155021, - "y": 5.584008168003288, - "heading": 6.282149498472273e-20, - "angularVelocity": -1.5814217146918653e-17, - "velocityX": -2.2862564539075523, - "velocityY": 0.03313460563399779, - "timestamp": 4.213851487030864 - }, - { - "x": 1.577540533889042, - "y": 5.586786622232322, - "heading": -2.476430565711122e-19, - "angularVelocity": -2.961969587010803e-18, - "velocityX": -1.829005283580098, - "velocityY": 0.02650768625293313, - "timestamp": 4.318668414563585 - }, - { - "x": 1.4337574940262061, - "y": 5.588870462964437, - "heading": -3.350775678748249e-19, - "angularVelocity": -8.341640359227092e-19, - "velocityX": -1.3717540024052963, - "velocityY": 0.01988076526536314, - "timestamp": 4.423485342096306 - }, - { - "x": 1.3379021324393994, - "y": 5.590259690143503, - "heading": -2.0793104080618583e-19, - "angularVelocity": 1.2130342880824037e-18, - "velocityX": -0.914502684281442, - "velocityY": 0.013253843742292293, - "timestamp": 4.528302269629028 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -1.4049323971929142e-31, - "angularVelocity": 1.9837543963605952e-18, - "velocityX": -0.4572513476830733, - "velocityY": 0.006626921951471195, - "timestamp": 4.633119197161749 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 4.571690374498227e-31, - "angularVelocity": 5.701965460392164e-30, - "velocityX": 1.9882633246635272e-32, - "velocityY": -1.2353765948799823e-31, - "timestamp": 4.73793612469447 - }, - { - "x": 1.3072266031673745, - "y": 5.605560947932051, - "heading": 0.009988018032587274, - "angularVelocity": 0.1383542475734168, - "velocityX": 0.2389771940088317, - "velocityY": 0.20233155967170455, - "timestamp": 4.810127750233767 - }, - { - "x": 1.3417310637547577, - "y": 5.63477381293564, - "heading": 0.029960721397497427, - "angularVelocity": 0.2766623305087681, - "velocityX": 0.4779565542351512, - "velocityY": 0.40465725470744984, - "timestamp": 4.882319375773064 - }, - { - "x": 1.393488225984334, - "y": 5.678592353413093, - "heading": 0.05990698047800166, - "angularVelocity": 0.4148162457460571, - "velocityX": 0.7169413604826331, - "velocityY": 0.606975395693467, - "timestamp": 4.954511001312361 - }, - { - "x": 1.4624987017850304, - "y": 5.737015858733095, - "heading": 0.09981017735525757, - "angularVelocity": 0.5527399692025371, - "velocityX": 0.9559346431831991, - "velocityY": 0.8092836929987566, - "timestamp": 5.026702626851658 - }, - { - "x": 1.54876329298479, - "y": 5.810043407637518, - "heading": 0.1496511403376383, - "angularVelocity": 0.6903981259608908, - "velocityX": 1.1949390328222467, - "velocityY": 1.0115792290159562, - "timestamp": 5.098894252390955 - }, - { - "x": 1.6522829608961203, - "y": 5.897673825591776, - "heading": 0.20941128647918397, - "angularVelocity": 0.8277988713391718, - "velocityX": 1.4339567385834835, - "velocityY": 1.2138584953540306, - "timestamp": 5.171085877930252 - }, - { - "x": 1.7730588392541233, - "y": 5.999905624129209, - "heading": 0.2790750774652724, - "angularVelocity": 0.9649843796382769, - "velocityX": 1.6729901488685341, - "velocityY": 1.4161171434188378, - "timestamp": 5.243277503469549 - }, - { - "x": 1.911092570994838, - "y": 6.1167366962116585, - "heading": 0.35862892849655154, - "angularVelocity": 1.1019817109951129, - "velocityX": 1.9120463171400826, - "velocityY": 1.6183466047443176, - "timestamp": 5.315469129008846 - }, - { - "x": 2.0662405750494774, - "y": 6.247971693585017, - "heading": 0.4478813931179114, - "angularVelocity": 1.2363271218036194, - "velocityX": 2.1491135972568896, - "velocityY": 1.8178700977147564, - "timestamp": 5.3876607545481425 - }, - { - "x": 2.204132997305319, - "y": 6.36460684022272, - "heading": 0.5272152358459444, - "angularVelocity": 1.0989341510922093, - "velocityX": 1.9100888950161639, - "velocityY": 1.6156326411317472, - "timestamp": 5.4598523800874394 - }, - { - "x": 2.3247684211138937, - "y": 6.466643771235888, - "heading": 0.5966353508762373, - "angularVelocity": 0.9616089748867341, - "velocityX": 1.6710445693293852, - "velocityY": 1.4134178341442696, - "timestamp": 5.532044005626736 - }, - { - "x": 2.428146136331826, - "y": 6.5540833745423095, - "heading": 0.6561424196084651, - "angularVelocity": 0.8242932374446845, - "velocityX": 1.4319904067208047, - "velocityY": 1.2112153266137329, - "timestamp": 5.604235631166033 - }, - { - "x": 2.51426563036811, - "y": 6.626926253781894, - "heading": 0.705736875442574, - "angularVelocity": 0.686983503468318, - "velocityX": 1.192929143690227, - "velocityY": 1.0090211807166112, - "timestamp": 5.67642725670533 - }, - { - "x": 2.5831264968268224, - "y": 6.6851728424603465, - "heading": 0.7454193798278042, - "angularVelocity": 0.5496829319015093, - "velocityX": 0.95386225125429, - "velocityY": 0.8068330397501333, - "timestamp": 5.748618882244627 - }, - { - "x": 2.6347284176402783, - "y": 6.72882344947858, - "heading": 0.7751907373365541, - "angularVelocity": 0.4123935052894287, - "velocityX": 0.7147909529377248, - "velocityY": 0.6046491776859777, - "timestamp": 5.820810507783924 - }, - { - "x": 2.669071164543137, - "y": 6.75787827866861, - "heading": 0.7950517689437672, - "angularVelocity": 0.27511545084049754, - "velocityX": 0.4757164926845912, - "velocityY": 0.4024681391074233, - "timestamp": 5.893002133323221 - }, - { - "x": 2.68615460395813, - "y": 6.772337436676025, - "heading": 0.8050032485420815, - "angularVelocity": 0.13784811636956903, - "velocityX": 0.23664018211616192, - "velocityY": 0.20028857778593961, - "timestamp": 5.965193758862518 - }, - { - "x": 2.6837128162746895, - "y": 6.770286154899203, - "heading": 0.8037616945531193, - "angularVelocity": -0.015403237566514301, - "velocityX": -0.03029383829428867, - "velocityY": -0.025449058845075586, - "timestamp": 6.045797202361987 - }, - { - "x": 2.6597554401054357, - "y": 6.750039490992799, - "heading": 0.790165886718463, - "angularVelocity": -0.1686752729700538, - "velocityX": -0.29722521928375617, - "velocityY": -0.2511885724392666, - "timestamp": 6.126400645861456 - }, - { - "x": 2.614282818196885, - "y": 6.71159714288196, - "heading": 0.7642144593793307, - "angularVelocity": -0.32196425130707784, - "velocityX": -0.5641523480167241, - "velocityY": -0.4769318337003116, - "timestamp": 6.207004089360925 - }, - { - "x": 2.547295402718566, - "y": 6.65495864191724, - "heading": 0.7259067765041173, - "angularVelocity": -0.4752611205179619, - "velocityX": -0.8310738669455855, - "velocityY": -0.702680909222201, - "timestamp": 6.287607532860394 - }, - { - "x": 2.45879373609576, - "y": 6.580123315301835, - "heading": 0.6752436075608932, - "angularVelocity": -0.6285484433864407, - "velocityX": -1.0979886563197567, - "velocityY": -0.9284383317423857, - "timestamp": 6.3682109763598636 - }, - { - "x": 2.3487784541804713, - "y": 6.487090205965796, - "heading": 0.612227929672179, - "angularVelocity": -0.7817988308289465, - "velocityX": -1.3648955570482664, - "velocityY": -1.1542076280722422, - "timestamp": 6.448814419859333 - }, - { - "x": 2.2172503640031413, - "y": 6.3758579034961445, - "heading": 0.536865388228733, - "angularVelocity": -0.934979179194793, - "velocityX": -1.6317924454199069, - "velocityY": -1.3799944225748537, - "timestamp": 6.529417863358802 - }, - { - "x": 2.0642108270205712, - "y": 6.2464240090225065, - "heading": 0.4491644340858314, - "angularVelocity": -1.0880546827189126, - "velocityX": -1.8986724429895898, - "velocityY": -1.605810978465707, - "timestamp": 6.610021306858271 - }, - { - "x": 1.892150582189426, - "y": 6.100772712319997, - "heading": 0.34952884105977006, - "angularVelocity": -1.236120799562961, - "velocityX": -2.134651292315194, - "velocityY": -1.8070108469188693, - "timestamp": 6.69062475035774 - }, - { - "x": 1.7416020056028705, - "y": 5.973323101720471, - "heading": 0.2622670318762804, - "angularVelocity": -1.0826064668575972, - "velocityX": -1.8677685474764083, - "velocityY": -1.5811931235965593, - "timestamp": 6.771228193857209 - }, - { - "x": 1.6125631547118695, - "y": 5.864077706390992, - "heading": 0.18740079602910192, - "angularVelocity": -0.9288218045882161, - "velocityX": -1.600909902712647, - "velocityY": -1.3553440223708513, - "timestamp": 6.851831637356678 - }, - { - "x": 1.5050325714338102, - "y": 5.7730380802152625, - "heading": 0.12496122653203612, - "angularVelocity": -0.7746513893974771, - "velocityX": -1.3340693475310352, - "velocityY": -1.1294756430146329, - "timestamp": 6.9324350808561475 - }, - { - "x": 1.4190090841759289, - "y": 5.700205295684954, - "heading": 0.0749803520329015, - "angularVelocity": -0.6200836134188114, - "velocityX": -1.0672433276187945, - "velocityY": -0.9035939578786114, - "timestamp": 7.013038524355617 - }, - { - "x": 1.3544918382019193, - "y": 5.645580092417221, - "heading": 0.037484747068995454, - "angularVelocity": -0.4651861426263309, - "velocityX": -0.8004279119223182, - "velocityY": -0.6777030967431115, - "timestamp": 7.093641967855086 - }, - { - "x": 1.3114803343182175, - "y": 5.609162978300701, - "heading": 0.012490529490997385, - "angularVelocity": -0.31008870704351826, - "velocityX": -0.5336186894297898, - "velocityY": -0.45180593452897394, - "timestamp": 7.174245411354555 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 1.0757846254441862e-30, - "angularVelocity": -0.1549627280016075, - "velocityX": -0.2668109738177861, - "velocityY": -0.22590442503082891, - "timestamp": 7.254848854854024 - }, - { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": 2.1112287936414426e-30, - "angularVelocity": 1.2846097126015132e-29, - "velocityX": 1.1473496118122496e-30, - "velocityY": -2.480854756847108e-31, - "timestamp": 7.335452298353493 - }, - { - "x": 1.3107332172473851, - "y": 5.587721554520521, - "heading": -5.6112723310966165e-9, - "angularVelocity": -8.086181485047517e-8, - "velocityX": 0.2991463288589595, - "velocityY": -0.04658586416314624, - "timestamp": 7.40484564929817 - }, - { - "x": 1.3522507006814268, - "y": 5.581255743814154, - "heading": -1.6136427820687844e-8, - "angularVelocity": -1.516738315771432e-7, - "velocityX": 0.5982919525984769, - "velocityY": -0.09317622824587016, - "timestamp": 7.4742390002428465 - }, - { - "x": 1.414526842614822, - "y": 5.571556496717738, - "heading": -3.074141680025046e-8, - "angularVelocity": -2.1046669010163338e-7, - "velocityX": 0.8974367296809845, - "velocityY": -0.13977199493000497, - "timestamp": 7.543632351187523 - }, - { - "x": 1.4975615711863124, - "y": 5.558623354737258, - "heading": -4.841098342837698e-8, - "angularVelocity": -2.546291003036043e-7, - "velocityX": 1.196580471199459, - "velocityY": -0.1863743687891775, - "timestamp": 7.6130257021322 - }, - { - "x": 1.6013547964876313, - "y": 5.542455744326475, - "heading": -6.788238796631325e-8, - "angularVelocity": -2.8059467012358587e-7, - "velocityX": 1.4957229170856399, - "velocityY": -0.2329850077952488, - "timestamp": 7.682419053076877 - }, - { - "x": 1.7259064027890496, - "y": 5.5230529273691795, - "heading": -8.754228960685774e-8, - "angularVelocity": -2.8331102694259857e-7, - "velocityX": 1.7948636952366863, - "velocityY": -0.27960628350061884, - "timestamp": 7.751812404021553 - }, - { - "x": 1.8712162355508042, - "y": 5.500413918486819, - "heading": -1.0525703637468966e-7, - "angularVelocity": -2.552801733617599e-7, - "velocityX": 2.0940022463766295, - "velocityY": -0.3262417591047122, - "timestamp": 7.82120575496623 - }, - { - "x": 2.0372840779688866, - "y": 5.474537335801765, - "heading": -1.1807218385471473e-7, - "angularVelocity": -1.8467399582926014e-7, - "velocityX": 2.393137673240176, - "velocityY": -0.3728971483980547, - "timestamp": 7.890599105910907 - }, - { - "x": 2.2241096038561166, - "y": 5.445421101366719, - "heading": -1.2162497814922265e-7, - "angularVelocity": -5.119790624873975e-8, - "velocityX": 2.692268399549916, - "velocityY": -0.41958248216406707, - "timestamp": 7.959992456855583 - }, - { - "x": 2.431692266948392, - "y": 5.413061738269229, - "heading": -1.0881824696224333e-7, - "angularVelocity": 1.845527118847122e-7, - "velocityX": 2.99139125386478, - "velocityY": -0.46631792033351455, - "timestamp": 8.02938580780026 - }, - { - "x": 2.660030964874599, - "y": 5.377452241496065, - "heading": -6.605232487729304e-8, - "angularVelocity": 6.162827019703426e-7, - "velocityX": 3.290498222347668, - "velocityY": -0.5131543049644982, - "timestamp": 8.098779158744936 - }, - { - "x": 2.9091223110729234, - "y": 5.338571186157936, - "heading": 4.364254321616404e-8, - "angularVelocity": 0.0000015807691363697316, - "velocityX": 3.5895563884342283, - "velocityY": -0.5602994351597471, - "timestamp": 8.168172509689612 - }, - { - "x": 3.168557402823767, - "y": 5.298953853406429, - "heading": 4.3641894183803e-8, - "angularVelocity": -9.352947371519385e-12, - "velocityX": 3.738615994458649, - "velocityY": -0.5709096363289035, - "timestamp": 8.237565860634287 - }, - { - "x": 3.4266802039415243, - "y": 5.251533194096367, - "heading": 4.3642354317485314e-8, - "angularVelocity": 6.630803609714539e-12, - "velocityX": 3.719705095716507, - "velocityY": -0.6833602739240744, - "timestamp": 8.306959211578963 - }, - { - "x": 3.6803449026923616, - "y": 5.18422554387417, - "heading": 4.364233337760302e-8, - "angularVelocity": -3.0175631893268995e-13, - "velocityX": 3.6554611543845263, - "velocityY": -0.9699437958523844, - "timestamp": 8.376352562523639 - }, - { - "x": 3.9278131468583686, - "y": 5.096843813495861, - "heading": 4.364231130349348e-8, - "angularVelocity": -3.1810121673972006e-13, - "velocityX": 3.5661665101502296, - "velocityY": -1.2592233865168847, - "timestamp": 8.445745913468315 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 4.364228801520247e-8, - "angularVelocity": -3.3559830331814666e-13, - "velocityX": 3.45401850865928, - "velocityY": -1.5404360240963577, - "timestamp": 8.51513926441299 - }, - { - "x": 4.284862555065841, - "y": 4.931880235034004, - "heading": 4.364226510492993e-8, - "angularVelocity": -6.617055985872344e-13, - "velocityX": 3.3897491580338537, - "velocityY": -1.67713541924223, - "timestamp": 8.549762324762018 - }, - { - "x": 4.399813643173519, - "y": 4.869172357324464, - "heading": 4.364224321511573e-8, - "angularVelocity": -6.322322259809295e-13, - "velocityX": 3.3200730076683276, - "velocityY": -1.81115929895842, - "timestamp": 8.584385385111045 - }, - { - "x": 4.512168964860153, - "y": 4.801924216205689, - "heading": 4.364222211690104e-8, - "angularVelocity": -6.093688654191888e-13, - "velocityX": 3.245100824537311, - "velocityY": -1.9422933859936342, - "timestamp": 8.619008445460071 - }, - { - "x": 4.621749311428581, - "y": 4.730243126546711, - "heading": 4.364220167371938e-8, - "angularVelocity": -5.904498836065766e-13, - "velocityX": 3.164952649008305, - "velocityY": -2.070327952999433, - "timestamp": 8.653631505809098 - }, - { - "x": 4.728650036507715, - "y": 4.654623908914118, - "heading": 4.364241447304788e-8, - "angularVelocity": 6.146173210402922e-12, - "velocityX": 3.0875585231776617, - "velocityY": -2.184070872715801, - "timestamp": 8.688254566158125 - }, - { - "x": 4.8372727310201835, - "y": 4.581499759061361, - "heading": 4.3642159997006347e-8, - "angularVelocity": -7.349900240613306e-12, - "velocityX": 3.137293278452735, - "velocityY": -2.112007116517435, - "timestamp": 8.722877626507152 - }, - { - "x": 4.948612503037568, - "y": 4.5125832438429105, - "heading": 4.3642137671211746e-8, - "angularVelocity": -6.448244222387315e-13, - "velocityX": 3.2157692270698366, - "velocityY": -1.9904801748811223, - "timestamp": 8.75750068685618 - }, - { - "x": 5.062589095284458, - "y": 4.448121106677303, - "heading": 4.364211455765666e-8, - "angularVelocity": -6.675769025667281e-13, - "velocityX": 3.2919271461828314, - "velocityY": -1.8618266703110185, - "timestamp": 8.792123747205206 - }, - { - "x": 5.17904891222934, - "y": 4.388261758970167, - "heading": 4.3642090495999965e-8, - "angularVelocity": -6.94960432877984e-13, - "velocityX": 3.3636488447548265, - "velocityY": -1.728886675634883, - "timestamp": 8.826746807554233 - }, - { - "x": 5.297806179962019, - "y": 4.333100707972697, - "heading": 4.364206516158606e-8, - "angularVelocity": -7.317208234311228e-13, - "velocityX": 3.4300049312658345, - "velocityY": -1.5931881942671235, - "timestamp": 8.86136986790326 - }, - { - "x": 5.418671454426997, - "y": 4.282725959570945, - "heading": 4.364203830903825e-8, - "angularVelocity": -7.755683028589255e-13, - "velocityX": 3.4908894028015367, - "velocityY": -1.454947884269515, - "timestamp": 8.895992928252287 - }, - { - "x": 5.541451926168614, - "y": 4.237217881100571, - "heading": 4.364200955008555e-8, - "angularVelocity": -8.306300130132497e-13, - "velocityX": 3.546205058244276, - "velocityY": -1.3143863659542425, - "timestamp": 8.930615988601314 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 4.364197825891865e-8, - "angularVelocity": -9.037666511803314e-13, - "velocityX": 3.5958636064268057, - "velocityY": -1.171727921713515, - "timestamp": 8.965239048950341 - }, - { - "x": 5.80048142988227, - "y": 4.159069716430897, - "heading": 4.364194761543742e-8, - "angularVelocity": -8.296994367988738e-13, - "velocityX": 3.642511096804761, - "velocityY": -1.0174944855729342, - "timestamp": 9.00217227920995 - }, - { - "x": 5.936489765010674, - "y": 4.127254911109845, - "heading": 4.364191906311607e-8, - "angularVelocity": -7.730794282349154e-13, - "velocityX": 3.6825464269543904, - "velocityY": -0.8614141004570673, - "timestamp": 9.039105509469557 - }, - { - "x": 6.073729843507157, - "y": 4.101262406827453, - "heading": 4.3641892220156324e-8, - "angularVelocity": -7.267969466454947e-13, - "velocityX": 3.7158969722335238, - "velocityY": -0.7037701305758657, - "timestamp": 9.076038739729166 - }, - { - "x": 6.211952539114824, - "y": 4.081139381676687, - "heading": 4.3641866716107116e-8, - "angularVelocity": -6.905447558160659e-13, - "velocityX": 3.742502202923576, - "velocityY": -0.5448487719411382, - "timestamp": 9.112971969988774 - }, - { - "x": 6.350906942398405, - "y": 4.066922357709394, - "heading": 4.364184226449404e-8, - "angularVelocity": -6.620491141311732e-13, - "velocityX": 3.7623138378867345, - "velocityY": -0.38493854632702584, - "timestamp": 9.149905200248382 - }, - { - "x": 6.490340706014696, - "y": 4.058637143370339, - "heading": 4.364062068850958e-8, - "angularVelocity": -3.3075253340927824e-11, - "velocityX": 3.775292944489131, - "velocityY": -0.22432953415709472, - "timestamp": 9.18683843050799 - }, - { - "x": 6.625895301013595, - "y": 4.054865050461972, - "heading": 3.8021532173824235e-8, - "angularVelocity": -1.521418068678583e-7, - "velocityX": 3.6702610101003303, - "velocityY": -0.10213276450108619, - "timestamp": 9.223771660767598 - }, - { - "x": 6.755934035567496, - "y": 4.053327166371606, - "heading": 3.0189413996037156e-8, - "angularVelocity": -2.1206154817940447e-7, - "velocityX": 3.52091419136215, - "velocityY": -0.04163957713841639, - "timestamp": 9.260704891027206 - }, - { - "x": 6.880153762983055, - "y": 4.0530365385095966, - "heading": 2.1734826824788128e-8, - "angularVelocity": -2.2891544824669977e-7, - "velocityX": 3.3633594067565498, - "velocityY": -0.007869007394285612, - "timestamp": 9.297638121286814 - }, - { - "x": 6.998470106919647, - "y": 4.0534986388519805, - "heading": 1.3540571224920633e-8, - "angularVelocity": -2.2186673987685247e-7, - "velocityX": 3.2035200578159904, - "velocityY": 0.012511777040293628, - "timestamp": 9.334571351546423 - }, - { - "x": 7.110853220415867, - "y": 4.054422625435363, - "heading": 6.1666659449803414e-9, - "angularVelocity": -1.9965502711211782e-7, - "velocityX": 3.042872575896168, - "velocityY": 0.025017757095422304, - "timestamp": 9.37150458180603 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -3.6086824530888626e-33, - "angularVelocity": -1.6696795107584883e-7, - "velocityX": 2.8819069972765123, - "velocityY": 0.03237357493636809, - "timestamp": 9.408437812065639 - }, - { - "x": 7.405345242374078, - "y": 4.058539790558069, - "heading": -6.074177156590073e-9, - "angularVelocity": -8.274407475492158e-8, - "velocityX": 2.561720622548375, - "velocityY": 0.03979751891783301, - "timestamp": 9.481847022104299 - }, - { - "x": 7.569888594263639, - "y": 4.061600743201228, - "heading": -8.336905285122374e-9, - "angularVelocity": -3.0823491082343837e-8, - "velocityX": 2.241453787650163, - "velocityY": 0.04169712004183413, - "timestamp": 9.555256232142959 - }, - { - "x": 7.71092123884848, - "y": 4.064554734769034, - "heading": -8.259320686300401e-9, - "angularVelocity": 1.056878269798436e-9, - "velocityX": 1.9211846103583805, - "velocityY": 0.04024006750993344, - "timestamp": 9.62866544218162 - }, - { - "x": 7.828444510796415, - "y": 4.067236236264996, - "heading": -6.843181194470064e-9, - "angularVelocity": 1.929103299991646e-8, - "velocityX": 1.600933614270509, - "velocityY": 0.03652813447454146, - "timestamp": 9.70207465222028 - }, - { - "x": 7.922460087151161, - "y": 4.069526407614099, - "heading": -4.8146064610859125e-9, - "angularVelocity": 2.7633790562623428e-8, - "velocityX": 1.2807054633231172, - "velocityY": 0.031197329979384493, - "timestamp": 9.77548386225894 - }, - { - "x": 7.992969626740666, - "y": 4.071335791937442, - "heading": -2.7240893137969417e-9, - "angularVelocity": 2.8477586783070736e-8, - "velocityX": 0.9604999093761202, - "velocityY": 0.024647919823550288, - "timestamp": 9.8488930722976 - }, - { - "x": 8.039974659720452, - "y": 4.072594619838684, - "heading": -1.0033424996203534e-9, - "angularVelocity": 2.3440475863128373e-8, - "velocityX": 0.6403151996190951, - "velocityY": 0.01714809218869153, - "timestamp": 9.92230228233626 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 1.0901032811452175e-35, - "angularVelocity": 1.3667801358727972e-8, - "velocityX": 0.32014923968219366, - "velocityY": 0.00888629686322534, - "timestamp": 9.99571149237492 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 2.780011652171749e-35, - "angularVelocity": 2.3020443776927775e-34, - "velocityX": -1.517814230631444e-35, - "velocityY": 4.908282159537256e-34, - "timestamp": 10.06912070241358 - } - ] + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -4.063364103945615e-24, + "angularVelocity": -2.020629205800752e-24, + "velocityX": -1.3550816656042328e-24, + "velocityY": 1.2971572394891071e-24, + "timestamp": 0 + }, + { + "x": 1.3063227037698195, + "y": 5.575041446046491, + "heading": -0.008997103572259477, + "angularVelocity": -0.12413133595120028, + "velocityX": 0.22555374987191792, + "velocityY": -0.2195466872880714, + "timestamp": 0.0724805183422541 + }, + { + "x": 1.3390193004314572, + "y": 5.543215943390215, + "heading": -0.026986588243377234, + "angularVelocity": -0.2481975168302625, + "velocityX": 0.45110875873215955, + "velocityY": -0.4390904395301923, + "timestamp": 0.1449610366845082 + }, + { + "x": 1.3880645131173623, + "y": 5.495478033090953, + "heading": -0.053955156680249416, + "angularVelocity": -0.3720802369200259, + "velocityX": 0.6766675212546471, + "velocityY": -0.6586309175362454, + "timestamp": 0.21744155502676227 + }, + { + "x": 1.4534588011433731, + "y": 5.431828013300948, + "heading": -0.0898822289659018, + "angularVelocity": -0.4956790197885205, + "velocityX": 0.9022326208708659, + "velocityY": -0.878167281992224, + "timestamp": 0.2899220733690164 + }, + { + "x": 1.535202817150975, + "y": 5.352266287723616, + "heading": -0.1347416873383059, + "angularVelocity": -0.6189174608351592, + "velocityX": 1.1278067248581949, + "velocityY": -1.0976980766285507, + "timestamp": 0.3624025917112705 + }, + { + "x": 1.6332974209934847, + "y": 5.25679341851918, + "heading": -0.1885038314662501, + "angularVelocity": -0.7417461320306592, + "velocityX": 1.3533926920790726, + "velocityY": -1.3172211152465878, + "timestamp": 0.4348831100535246 + }, + { + "x": 1.7477437450196447, + "y": 5.145410203108038, + "heading": -0.2511366541198319, + "angularVelocity": -0.8641332055301907, + "velocityX": 1.5789942821013316, + "velocityY": -1.5367331520062975, + "timestamp": 0.5073636283957786 + }, + { + "x": 1.8785436154915462, + "y": 5.018117926270065, + "heading": -0.3226014402327865, + "angularVelocity": -0.9859861345844242, + "velocityX": 1.8046210687161843, + "velocityY": -1.7562274629010988, + "timestamp": 0.5798441467380328 + }, + { + "x": 2.0257354638148333, + "y": 4.874937893317761, + "heading": -0.40230013407560183, + "angularVelocity": -1.0995878018763188, + "velocityX": 2.0307780861644047, + "velocityY": -1.975427828429777, + "timestamp": 0.6523246650802869 + }, + { + "x": 2.156576334610242, + "y": 4.74766733108212, + "heading": -0.4731338136564732, + "angularVelocity": -0.9772788771514256, + "velocityX": 1.8051867424233425, + "velocityY": -1.7559278706406076, + "timestamp": 0.724805183422541 + }, + { + "x": 2.271064512399631, + "y": 4.636304989038449, + "heading": -0.5351259426845023, + "angularVelocity": -0.8552936767822404, + "velocityX": 1.579571730554888, + "velocityY": -1.5364451661040166, + "timestamp": 0.7972857017647951 + }, + { + "x": 2.3691991759335136, + "y": 4.540850112955004, + "heading": -0.5882849412728991, + "angularVelocity": -0.7334246471221314, + "velocityX": 1.3539453880626124, + "velocityY": -1.3169728675601617, + "timestamp": 0.8697662201070492 + }, + { + "x": 2.4509797863223954, + "y": 4.461302149238075, + "heading": -0.6326144930208853, + "angularVelocity": -0.611606439383635, + "velocityX": 1.1283116106139404, + "velocityY": -1.0975082068439748, + "timestamp": 0.9422467384493033 + }, + { + "x": 2.5164059599659825, + "y": 4.397660680894242, + "heading": -0.6681161230461814, + "angularVelocity": -0.4898092734058101, + "velocityX": 0.9026725407045777, + "velocityY": -0.8780492993071322, + "timestamp": 1.0147272567915573 + }, + { + "x": 2.5654774247056453, + "y": 4.349925401446345, + "heading": -0.6947902571446234, + "angularVelocity": -0.3680179820525904, + "velocityX": 0.677029715874088, + "velocityY": -0.6585946201776504, + "timestamp": 1.0872077751338114 + }, + { + "x": 2.598194001702078, + "y": 4.3180961013994725, + "heading": -0.7126367415892834, + "angularVelocity": -0.24622456975802207, + "velocityX": 0.45138442363152775, + "velocityY": -0.4391428314098807, + "timestamp": 1.1596882934760655 + }, + { + "x": 2.614555597305298, + "y": 4.302172660827637, + "heading": -0.7216551150961179, + "angularVelocity": -0.1244247932147739, + "velocityX": 0.2257378393178725, + "velocityY": -0.21969269723824078, + "timestamp": 1.2321688118183196 + }, + { + "x": 2.6124101268407562, + "y": 4.3042467200349375, + "heading": -0.7207049042678578, + "angularVelocity": 0.011728656954985593, + "velocityX": -0.026482004137690758, + "velocityY": 0.025600559605606452, + "timestamp": 1.3131849786230068 + }, + { + "x": 2.5898308635318203, + "y": 4.326193667394344, + "heading": -0.7087258164674446, + "angularVelocity": 0.14786046134831782, + "velocityX": -0.27870071122187107, + "velocityY": 0.2708958992384371, + "timestamp": 1.394201145427694 + }, + { + "x": 2.546818010460629, + "y": 4.368013804819267, + "heading": -0.6857195036998125, + "angularVelocity": 0.28397187468885515, + "velocityX": -0.5309169116194552, + "velocityY": 0.51619496545352, + "timestamp": 1.475217312232381 + }, + { + "x": 2.4833718812656413, + "y": 4.429707576791875, + "heading": -0.6516873068635105, + "angularVelocity": 0.4200667370297391, + "velocityX": -0.7831292407099776, + "velocityY": 0.7614995180078072, + "timestamp": 1.5562334790370682 + }, + { + "x": 2.3994929054836645, + "y": 4.511275584027978, + "heading": -0.6066297376902362, + "angularVelocity": 0.5561552829560362, + "velocityX": -1.035336267935158, + "velocityY": 1.0068114853266914, + "timestamp": 1.6372496458417554 + }, + { + "x": 2.295181645720894, + "y": 4.612718599682289, + "heading": -0.5505453877273064, + "angularVelocity": 0.6922612137172227, + "velocityX": -1.2875363507908548, + "velocityY": 1.2521329958607974, + "timestamp": 1.7182658126464425 + }, + { + "x": 2.1704388340864518, + "y": 4.734037581039087, + "heading": -0.4834287968254485, + "angularVelocity": 0.8284345402771498, + "velocityX": -1.539727397065957, + "velocityY": 1.4974663223609717, + "timestamp": 1.7992819794511297 + }, + { + "x": 2.025265436304345, + "y": 4.875233658266282, + "heading": -0.405266607510017, + "angularVelocity": 0.9647727410241949, + "velocityX": -1.7919065232015794, + "velocityY": 1.7428135987671272, + "timestamp": 1.8802981462558168 + }, + { + "x": 1.8618624811004039, + "y": 5.034280009168128, + "heading": -0.3154414224064508, + "angularVelocity": 1.10873161056997, + "velocityX": -2.0169178776116525, + "velocityY": 1.9631433721774778, + "timestamp": 1.961314313060504 + }, + { + "x": 1.7188875745497745, + "y": 5.1734469779831365, + "heading": -0.2367274347449033, + "angularVelocity": 0.9715837068829752, + "velocityX": -1.7647700723154625, + "velocityY": 1.7177678765092634, + "timestamp": 2.0423304798651913 + }, + { + "x": 1.5963395221215884, + "y": 5.292733837393614, + "heading": -0.1691692747677602, + "angularVelocity": 0.8338849224996224, + "velocityX": -1.5126370113711174, + "velocityY": 1.4723834034020926, + "timestamp": 2.1233466466698787 + }, + { + "x": 1.4942172977681851, + "y": 5.392140100481127, + "heading": -0.11281115760570573, + "angularVelocity": 0.695640381233068, + "velocityX": -1.2605166151540885, + "velocityY": 1.2269929201558054, + "timestamp": 2.204362813474566 + }, + { + "x": 1.41252009894491, + "y": 5.47166545898136, + "heading": -0.06769200462657954, + "angularVelocity": 0.5569154251385268, + "velocityX": -1.008406124918618, + "velocityY": 0.9815986319366667, + "timestamp": 2.2853789802792535 + }, + { + "x": 1.3512473746231104, + "y": 5.531309714267714, + "heading": -0.033841358591587725, + "angularVelocity": 0.4178258163780882, + "velocityX": -0.7563024361485172, + "velocityY": 0.7362018920265989, + "timestamp": 2.366395147083941 + }, + { + "x": 1.3103988349917597, + "y": 5.571072713807996, + "heading": -0.011276386726511467, + "angularVelocity": 0.2785243088515359, + "velocityX": -0.5042023245783488, + "velocityY": 0.4908032693788472, + "timestamp": 2.447411313888628 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 7.044896847536941e-24, + "angularVelocity": 0.1391868706118427, + "velocityX": -0.2521025707860859, + "velocityY": 0.24540274759468042, + "timestamp": 2.5284274806933156 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 3.2679300421363754e-24, + "angularVelocity": -5.4036172137189415e-24, + "velocityX": -5.375446023780689e-25, + "velocityY": 8.028108369127909e-25, + "timestamp": 2.609443647498003 + }, + { + "x": 1.3242823472826726, + "y": 5.590457081039531, + "heading": -2.9445272594890385e-20, + "angularVelocity": -3.320691324750262e-19, + "velocityX": 0.3868644465708696, + "velocityY": -0.005606807954126726, + "timestamp": 2.6981256045031268 + }, + { + "x": 1.3928981385920456, + "y": 5.5894626356520005, + "heading": 1.967498349802273e-20, + "angularVelocity": 5.538923333201553e-19, + "velocityX": 0.7737288804464331, + "velocityY": -0.01121361572426099, + "timestamp": 2.7868075615082506 + }, + { + "x": 1.495821823135319, + "y": 5.587970967605789, + "heading": -2.85375470863899e-20, + "angularVelocity": -5.436565928013428e-19, + "velocityX": 1.1605932933722591, + "velocityY": -0.016820423190771712, + "timestamp": 2.8754895185133744 + }, + { + "x": 1.6330533972647785, + "y": 5.585982076953762, + "heading": -1.8597210244417365e-22, + "angularVelocity": 3.1969947368616956e-19, + "velocityX": 1.5474576651655374, + "velocityY": -0.022427230061150386, + "timestamp": 2.9641714755184982 + }, + { + "x": 1.8045928505525564, + "y": 5.583495963847051, + "heading": 1.7052075155994419e-19, + "angularVelocity": 1.9249318511157463e-18, + "velocityX": 1.9343219193715702, + "velocityY": -0.028034035227342758, + "timestamp": 3.052853432523622 + }, + { + "x": 2.010439898145835, + "y": 5.580512632414013, + "heading": 4.48101102281287e-19, + "angularVelocity": 3.1300656874471152e-18, + "velocityX": 2.321182961505746, + "velocityY": -0.03364079384113171, + "timestamp": 3.141535389528746 + }, + { + "x": 2.1819790701418538, + "y": 5.57802652338405, + "heading": 3.8109031414693246e-19, + "angularVelocity": -7.556304619487216e-19, + "velocityX": 1.934318747455112, + "velocityY": -0.028033989256909718, + "timestamp": 3.2302173465338697 + }, + { + "x": 2.3192103534380903, + "y": 5.5760376369470555, + "heading": 2.513466977645824e-19, + "angularVelocity": -1.4630215747133308e-18, + "velocityX": 1.54745438565714, + "velocityY": -0.022427182531392593, + "timestamp": 3.3188993035389935 + }, + { + "x": 2.4221337438940673, + "y": 5.574545973163037, + "heading": 2.3569701876978443e-19, + "angularVelocity": -1.7646970733669464e-19, + "velocityX": 1.1605899771701071, + "velocityY": -0.01682037512921312, + "timestamp": 3.4075812605441174 + }, + { + "x": 2.4907492394796407, + "y": 5.573551532061417, + "heading": 1.521167829061186e-19, + "angularVelocity": -9.424717122873263e-19, + "velocityX": 0.773725545790664, + "velocityY": -0.011213567395255041, + "timestamp": 3.496263217549241 + }, + { + "x": 2.525056838989258, + "y": 5.573054313659668, + "heading": -7.785018050329552e-30, + "angularVelocity": -1.7153070145831324e-18, + "velocityX": 0.386861100817104, + "velocityY": -0.005606759464278068, + "timestamp": 3.584945174554365 + }, + { + "x": 2.5176787496859108, + "y": 5.57316124395569, + "heading": -2.760990246718961e-19, + "angularVelocity": -2.634107204526249e-18, + "velocityX": -0.0703902457225223, + "velocityY": 0.0010201624706221363, + "timestamp": 3.6897621023490434 + }, + { + "x": 2.4623729801701617, + "y": 5.573962787832825, + "heading": -5.185614830886611e-19, + "angularVelocity": -2.3131994374789403e-18, + "velocityX": -0.5276415811774727, + "velocityY": 0.007647084244872661, + "timestamp": 3.794579030143722 + }, + { + "x": 2.359139532378446, + "y": 5.575458945263007, + "heading": -6.8142886513825e-19, + "angularVelocity": -1.5538270914101072e-18, + "velocityX": -0.9848928981579708, + "velocityY": 0.014274005751373834, + "timestamp": 3.8993959579384003 + }, + { + "x": 2.2079784101836313, + "y": 5.577649716190108, + "heading": -5.427283873734921e-19, + "angularVelocity": 1.3232641013891109e-18, + "velocityX": -1.442144178189593, + "velocityY": 0.02090092672237671, + "timestamp": 4.004212885733079 + }, + { + "x": 2.0088896252042967, + "y": 5.58053510044574, + "heading": -5.007294376921598e-19, + "angularVelocity": 4.006886155587225e-19, + "velocityX": -1.8993953473748186, + "velocityY": 0.027527846086888057, + "timestamp": 4.109029813527758 + }, + { + "x": 1.7692512481542437, + "y": 5.584008168003299, + "heading": -1.0777156334696855e-19, + "angularVelocity": 3.7489924825765976e-18, + "velocityX": -2.286256448190035, + "velocityY": 0.033134605551138764, + "timestamp": 4.2138467413224365 + }, + { + "x": 1.5775405338885757, + "y": 5.586786622232329, + "heading": 2.4679892039672843e-19, + "angularVelocity": 3.382759743178975e-18, + "velocityX": -1.8290052790060969, + "velocityY": 0.02650768618664608, + "timestamp": 4.318663669117115 + }, + { + "x": 1.433757494025973, + "y": 5.58887046296444, + "heading": 3.0327550884617235e-19, + "angularVelocity": 5.388117133514026e-19, + "velocityX": -1.3717539989747978, + "velocityY": 0.019880765215647883, + "timestamp": 4.423480596911794 + }, + { + "x": 1.3379021324393217, + "y": 5.590259690143504, + "heading": 1.0372328795673318e-19, + "angularVelocity": -1.9038167315540544e-18, + "velocityX": -0.9145026819944435, + "velocityY": 0.0132538437091488, + "timestamp": 4.528297524706473 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.6477091007835168e-22, + "angularVelocity": -9.879942030515793e-19, + "velocityX": -0.45725134653957433, + "velocityY": 0.00662692193489945, + "timestamp": 4.633114452501152 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.5500454228160796e-22, + "angularVelocity": -9.317544566447768e-23, + "velocityX": -6.472519788584152e-21, + "velocityY": -2.347379322977946e-21, + "timestamp": 4.737931380295831 + }, + { + "x": 1.3072065943694386, + "y": 5.605543739625148, + "heading": 0.00997794612171538, + "angularVelocity": 0.1382953520830163, + "velocityX": 0.23883926575200254, + "velocityY": 0.20221107105767083, + "timestamp": 4.810080920908923 + }, + { + "x": 1.3416710394123585, + "y": 5.634722180998455, + "heading": 0.029930634172052512, + "angularVelocity": 0.2765462937225786, + "velocityX": 0.47768072741777556, + "velocityY": 0.4044161768094943, + "timestamp": 4.882230461522014 + }, + { + "x": 1.3933681818707737, + "y": 5.678489073214057, + "heading": 0.05984711285989078, + "angularVelocity": 0.414645449348986, + "velocityX": 0.7165276732064818, + "velocityY": 0.6066135950928104, + "timestamp": 4.954380002135106 + }, + { + "x": 1.4622986372338835, + "y": 5.736843692636249, + "heading": 0.09971100751764889, + "angularVelocity": 0.5525176504107191, + "velocityX": 0.9553831497383417, + "velocityY": 0.8088009837113753, + "timestamp": 5.0265295427481975 + }, + { + "x": 1.548463213011665, + "y": 5.809785098412347, + "heading": 0.14950349033581017, + "angularVelocity": 0.6901288961100662, + "velocityX": 1.194249818440935, + "velocityY": 1.0109753320156587, + "timestamp": 5.098679083361289 + }, + { + "x": 1.6518628806196247, + "y": 5.897312083044383, + "heading": 0.20920651705409263, + "angularVelocity": 0.8274900465194276, + "velocityX": 1.4331299510616444, + "velocityY": 1.2131329442748184, + "timestamp": 5.170828623974381 + }, + { + "x": 1.772498794450995, + "y": 5.999423091967104, + "heading": 0.2788055813251054, + "angularVelocity": 0.9646501374727254, + "velocityX": 1.6720260836904182, + "velocityY": 1.4152690101008414, + "timestamp": 5.2429781645874725 + }, + { + "x": 1.9103726539115997, + "y": 6.1161158275998355, + "heading": 0.35829010832272556, + "angularVelocity": 1.1016636602561762, + "velocityX": 1.9109457702574424, + "velocityY": 1.6173732312241136, + "timestamp": 5.315127705200564 + }, + { + "x": 2.0655201843500692, + "y": 6.247352096059746, + "heading": 0.44752582385757206, + "angularVelocity": 1.2368161290642319, + "velocityX": 2.1503606138043514, + "velocityY": 1.8189480812314087, + "timestamp": 5.387277245813656 + }, + { + "x": 2.203432352747445, + "y": 6.364005171819918, + "heading": 0.526860073391937, + "angularVelocity": 1.0995807992708087, + "velocityX": 1.911476736032763, + "velocityY": 1.6168235413408145, + "timestamp": 5.459426786426747 + }, + { + "x": 2.3241076430309073, + "y": 6.466076944016547, + "heading": 0.596294464302791, + "angularVelocity": 0.9623677478863277, + "velocityX": 1.6725718453371032, + "velocityY": 1.4147251850707963, + "timestamp": 5.531576327039839 + }, + { + "x": 2.427545315940862, + "y": 6.553568380245639, + "heading": 0.6558285941122223, + "angularVelocity": 0.8251491181168898, + "velocityX": 1.4336567084279637, + "velocityY": 1.2126402397802263, + "timestamp": 5.603725867652931 + }, + { + "x": 2.51374484513646, + "y": 6.626480123164196, + "heading": 0.7054623538170143, + "angularVelocity": 0.687928977551743, + "velocityX": 1.1947342763809239, + "velocityY": 1.0105642017813024, + "timestamp": 5.675875408266022 + }, + { + "x": 2.582705816133726, + "y": 6.684812629515298, + "heading": 0.7451960796056276, + "angularVelocity": 0.5507134965929852, + "velocityX": 0.9558060995436645, + "velocityY": 0.8084944942881265, + "timestamp": 5.748024948879114 + }, + { + "x": 2.6344279053527613, + "y": 6.728566223750801, + "heading": 0.7750303601927481, + "angularVelocity": 0.41350617527988826, + "velocityX": 0.7168734378559617, + "velocityY": 0.6064292837307924, + "timestamp": 5.820174489492206 + }, + { + "x": 2.6689108803199226, + "y": 6.757741121003901, + "heading": 0.7949658633330718, + "angularVelocity": 0.2763081091150633, + "velocityX": 0.477937554059832, + "velocityY": 0.4043670549415301, + "timestamp": 5.892324030105297 + }, + { + "x": 2.68615460395813, + "y": 6.772337436676025, + "heading": 0.8050032485420815, + "angularVelocity": 0.13911918390216763, + "velocityX": 0.23899977036136932, + "velocityY": 0.2023064256278295, + "timestamp": 5.964473570718389 + }, + { + "x": 2.6838905941834166, + "y": 6.770438484115006, + "heading": 0.8038568906266841, + "angularVelocity": -0.014214822664441809, + "velocityX": -0.028073690621275776, + "velocityY": -0.023546986103133997, + "timestamp": 6.0451188198684145 + }, + { + "x": 2.6600885687415188, + "y": 6.7503254094883065, + "heading": 0.7903430319109747, + "angularVelocity": -0.16757166551211813, + "velocityX": -0.2951447939309862, + "velocityY": -0.24940185365765316, + "timestamp": 6.12576406901844 + }, + { + "x": 2.614748840901014, + "y": 6.711997955351817, + "heading": 0.7644600728285338, + "angularVelocity": -0.3209483429617853, + "velocityX": -0.562212012714593, + "velocityY": -0.47525991351563107, + "timestamp": 6.206409318168466 + }, + { + "x": 2.547871822089795, + "y": 6.655455715047437, + "heading": 0.7262070502709054, + "angularVelocity": -0.47433696294329564, + "velocityX": -0.8292741297978391, + "velocityY": -0.7011230159286398, + "timestamp": 6.287054567318491 + }, + { + "x": 2.45945799388215, + "y": 6.580698108278845, + "heading": 0.6755842481215987, + "angularVelocity": -0.627722062773128, + "velocityX": -1.0963302753664865, + "velocityY": -0.9269933140081175, + "timestamp": 6.367699816468517 + }, + { + "x": 2.349507890636972, + "y": 6.487724331378611, + "heading": 0.6125938511497702, + "angularVelocity": -0.781080071494933, + "velocityX": -1.3633797948920177, + "velocityY": -1.152873577552878, + "timestamp": 6.448345065618542 + }, + { + "x": 2.218022116383117, + "y": 6.3765332785035795, + "heading": 0.5372399641012893, + "angularVelocity": -0.9343871814234115, + "velocityX": -1.6304218244679243, + "velocityY": -1.3787675535378683, + "timestamp": 6.528990314768568 + }, + { + "x": 2.065001429825468, + "y": 6.2471234504060735, + "heading": 0.4495265554466888, + "angularVelocity": -1.0876450823708927, + "velocityX": -1.897454446113545, + "velocityY": -1.6046801201737573, + "timestamp": 6.6096355639185935 + }, + { + "x": 1.892766685520709, + "y": 6.101314959564872, + "heading": 0.34981930117759164, + "angularVelocity": -1.2363686059622758, + "velocityX": -2.1357085026093356, + "velocityY": -1.8080233166612498, + "timestamp": 6.690280813068619 + }, + { + "x": 1.7420645817615519, + "y": 5.973729053643758, + "heading": 0.26248852330737293, + "angularVelocity": -1.08290046581363, + "velocityX": -1.868704050734679, + "velocityY": -1.582063509826458, + "timestamp": 6.770926062218645 + }, + { + "x": 1.612893816606675, + "y": 5.8643673067305935, + "heading": 0.18756078557963143, + "angularVelocity": -0.929102935603218, + "velocityX": -1.6017157429147117, + "velocityY": -1.3560841843233018, + "timestamp": 6.85157131136867 + }, + { + "x": 1.5052531418262654, + "y": 5.773230960685921, + "heading": 0.12506875746451004, + "angularVelocity": -0.7749003044043764, + "velocityX": -1.3347429131276467, + "velocityY": -1.1300894597663134, + "timestamp": 6.932216560518696 + }, + { + "x": 1.4191414897869865, + "y": 5.700320933784421, + "heading": 0.0750452611365071, + "angularVelocity": -0.6202906786851591, + "velocityX": -1.0677833219794879, + "velocityY": -0.9040833486156766, + "timestamp": 7.012861809668721 + }, + { + "x": 1.3545580679213909, + "y": 5.645637873878687, + "heading": 0.03751734547660986, + "angularVelocity": -0.46534564720711064, + "velocityX": -0.8008335586570275, + "velocityY": -0.6780692041015978, + "timestamp": 7.093507058818747 + }, + { + "x": 1.311502418725051, + "y": 5.609182228096023, + "heading": 0.01250142936060969, + "angularVelocity": -0.31019702189105725, + "velocityX": -0.533889468383214, + "velocityY": -0.4520495152150475, + "timestamp": 7.174152307968773 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741457, + "heading": 2.7832801817325185e-21, + "angularVelocity": -0.1550175551860858, + "velocityX": -0.2669465081562177, + "velocityY": -0.22602601575026499, + "timestamp": 7.254797557118798 + }, + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": 1.3491121023361147e-21, + "angularVelocity": -9.888514076614425e-22, + "velocityX": -3.220372602008045e-20, + "velocityY": -2.3512590671688983e-20, + "timestamp": 7.335442806268824 + }, + { + "x": 1.310729419094693, + "y": 5.587708246254127, + "heading": 1.3104272140971465e-18, + "angularVelocity": 1.8865367753568018e-17, + "velocityX": 0.2991037005434092, + "velocityY": -0.04677953756663729, + "timestamp": 7.404833348719224 + }, + { + "x": 1.3522393548624092, + "y": 5.581216131325057, + "heading": -6.810029035561697e-19, + "angularVelocity": -2.869886943346906e-17, + "velocityX": 0.5982073968853444, + "velocityY": -0.0935590744763348, + "timestamp": 7.474223891169625 + }, + { + "x": 1.4145042580183609, + "y": 5.571477959008947, + "heading": -1.7830392147175414e-18, + "angularVelocity": -1.5881650038949302e-17, + "velocityX": 0.8973110881855106, + "velocityY": -0.1403386105977046, + "timestamp": 7.543614433620025 + }, + { + "x": 1.4975241281349527, + "y": 5.558493729372655, + "heading": -6.515620811670549e-18, + "angularVelocity": -6.820211270699198e-17, + "velocityX": 1.1964147733235146, + "velocityY": -0.18711814575556276, + "timestamp": 7.6130049760704255 + }, + { + "x": 1.6012989646776896, + "y": 5.542263442499756, + "heading": -1.6376922566585543e-17, + "angularVelocity": -1.4211305181843934e-16, + "velocityX": 1.4955184507588155, + "velocityY": -0.23389767970903125, + "timestamp": 7.682395518520826 + }, + { + "x": 1.725828766959364, + "y": 5.522787098497699, + "heading": -2.5308091780963488e-17, + "angularVelocity": -1.2870873895834211e-16, + "velocityX": 1.7946221182906406, + "velocityY": -0.28067721211399865, + "timestamp": 7.751786060971226 + }, + { + "x": 1.8711135340636993, + "y": 5.500064697509755, + "heading": -3.3349995940615036e-17, + "angularVelocity": -1.1589337502943322e-16, + "velocityX": 2.0937257726178307, + "velocityY": -0.3274567424542979, + "timestamp": 7.821176603421627 + }, + { + "x": 2.0371532647079076, + "y": 5.474096239736498, + "heading": -4.30035583427312e-17, + "angularVelocity": -1.3911928140733344e-16, + "velocityX": 2.3928294084585304, + "velocityY": -0.37423626990406167, + "timestamp": 7.890567145872027 + }, + { + "x": 2.223947956967808, + "y": 5.444881725478791, + "heading": -5.0761776746981987e-17, + "angularVelocity": -1.118051268996076e-16, + "velocityX": 2.691933016569494, + "velocityY": -0.4210157930180218, + "timestamp": 7.959957688322428 + }, + { + "x": 2.4314976076364307, + "y": 5.412421155238076, + "heading": -5.64791774774602e-17, + "angularVelocity": -8.239452421940524e-17, + "velocityX": 2.991036578464229, + "velocityY": -0.4677953089056427, + "timestamp": 8.029348230772829 + }, + { + "x": 2.659802210299839, + "y": 5.37671453001723, + "heading": -6.06625739671826e-17, + "angularVelocity": -6.028770408822991e-17, + "velocityX": 3.290140047926519, + "velocityY": -0.5145748103405868, + "timestamp": 8.09873877322323 + }, + { + "x": 2.9088617457162362, + "y": 5.337761852824889, + "heading": -6.241000651317059e-17, + "angularVelocity": -2.5182575093519166e-17, + "velocityX": 3.589243240091715, + "velocityY": -0.5613542684175394, + "timestamp": 8.168129315673632 + }, + { + "x": 3.168141716720517, + "y": 5.297210399977503, + "heading": -4.3453663669154356e-17, + "angularVelocity": 2.7318337880868624e-16, + "velocityX": 3.7365318363033526, + "velocityY": -0.5843945214345647, + "timestamp": 8.237519858124033 + }, + { + "x": 3.426418453660738, + "y": 5.250695339800734, + "heading": -4.238140416902354e-17, + "angularVelocity": 1.5452530881159203e-17, + "velocityX": 3.7220740438055158, + "velocityY": -0.6703371746952049, + "timestamp": 8.306910400574434 + }, + { + "x": 3.680147658052144, + "y": 5.183672848816506, + "heading": -4.587291099772401e-17, + "angularVelocity": -5.0316753627269466e-17, + "velocityX": 3.65653870731402, + "velocityY": -0.9658735703375574, + "timestamp": 8.376300943024836 + }, + { + "x": 3.9277032968256567, + "y": 5.096571019436482, + "heading": -4.576332276708217e-17, + "angularVelocity": 1.5792963566594492e-18, + "velocityX": 3.567570306147443, + "velocityY": -1.2552406466959498, + "timestamp": 8.445691485475237 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -5.405308439751368e-17, + "angularVelocity": -1.194652950873824e-16, + "velocityX": 3.455741374913111, + "velocityY": -1.5365670854176536, + "timestamp": 8.515082027925638 + }, + { + "x": 4.284931451916926, + "y": 4.932009555387415, + "heading": -5.371492234038301e-17, + "angularVelocity": 9.766625728969003e-18, + "velocityX": 3.3916228655411733, + "velocityY": -1.6733429936143844, + "timestamp": 8.549706274539513 + }, + { + "x": 4.39995646079623, + "y": 4.869427987028382, + "heading": -6.433424938599782e-17, + "angularVelocity": -3.067020392759585e-16, + "velocityX": 3.32209420069255, + "velocityY": -1.8074492437895704, + "timestamp": 8.584330521153388 + }, + { + "x": 4.51239059708215, + "y": 4.8023029501801755, + "heading": -5.362012153015504e-17, + "angularVelocity": 3.0944002843229445e-16, + "velocityX": 3.2472659272494777, + "velocityY": -1.9386714055261611, + "timestamp": 8.618954767767264 + }, + { + "x": 4.622054515133808, + "y": 4.730741572190097, + "heading": -5.247217419715393e-17, + "angularVelocity": 3.315443500231424e-17, + "velocityX": 3.1672578836043357, + "velocityY": -2.06679956933421, + "timestamp": 8.653579014381139 + }, + { + "x": 4.728773352551376, + "y": 4.65485813780801, + "heading": -6.448401171604869e-17, + "angularVelocity": -3.469198233269833e-16, + "velocityX": 3.08219955246049, + "velocityY": -2.19162701872845, + "timestamp": 8.688203260995014 + }, + { + "x": 4.837250041803269, + "y": 4.581509560989999, + "heading": -7.180181418195748e-17, + "angularVelocity": -2.1134907415213478e-16, + "velocityX": 3.1329689411472432, + "velocityY": -2.1184165430654605, + "timestamp": 8.722827507608889 + }, + { + "x": 4.948569206649129, + "y": 4.512551245909545, + "heading": -5.993451372549524e-17, + "angularVelocity": 3.427453769485116e-16, + "velocityX": 3.2150638853539646, + "velocityY": -1.9916192213354724, + "timestamp": 8.757451754222764 + }, + { + "x": 5.062553327275946, + "y": 4.4480933081859515, + "heading": -6.273480456472046e-17, + "angularVelocity": -8.08765854062184e-17, + "velocityX": 3.2920317919968585, + "velocityY": -1.8616415959145531, + "timestamp": 8.792076000836639 + }, + { + "x": 5.17902058017923, + "y": 4.38823861530867, + "heading": -5.454489606023899e-17, + "angularVelocity": 2.365368002574791e-16, + "velocityX": 3.3637483640325954, + "velocityY": -1.728693003627544, + "timestamp": 8.826700247450514 + }, + { + "x": 5.297785167201245, + "y": 4.333082674122234, + "heading": -4.169311661554588e-17, + "angularVelocity": 3.711786017787193e-16, + "velocityX": 3.43009880753392, + "velocityY": -1.5929860309027757, + "timestamp": 8.86132449406439 + }, + { + "x": 5.4186576198068925, + "y": 4.282713488635075, + "heading": -3.1063280252894355e-17, + "angularVelocity": 3.0700556411606523e-16, + "velocityX": 3.490977116516063, + "velocityY": -1.4547373708623472, + "timestamp": 8.895948740678264 + }, + { + "x": 5.541445103981359, + "y": 4.23721142295766, + "heading": -1.7411383468509857e-17, + "angularVelocity": 3.9428718659707233e-16, + "velocityX": 3.5462860908939025, + "velocityY": -1.3141676751799676, + "timestamp": 8.93057298729214 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -2.060214519362369e-17, + "angularVelocity": -9.215396827963617e-17, + "velocityX": 3.5959374431427245, + "velocityY": -1.1715012562024099, + "timestamp": 8.965197233906014 + }, + { + "x": 5.800482065965206, + "y": 4.159078601267801, + "heading": -2.3136950753163445e-17, + "angularVelocity": -6.863298410721474e-17, + "velocityX": 3.6425746580693774, + "velocityY": -1.0172668618134628, + "timestamp": 9.002129994322162 + }, + { + "x": 5.936490640801473, + "y": 4.1272726257790175, + "heading": -1.8598714484333613e-17, + "angularVelocity": 1.228783394072173e-16, + "velocityX": 3.682599765188514, + "velocityY": -0.8611859804250445, + "timestamp": 9.03906275473831 + }, + { + "x": 6.073730568489753, + "y": 4.101288878730342, + "heading": -4.2719996436236496e-17, + "angularVelocity": -6.531134331502948e-16, + "velocityX": 3.7159401610359515, + "velocityY": -0.7035419707570875, + "timestamp": 9.075995515154458 + }, + { + "x": 6.2119527294046275, + "y": 4.081174520992199, + "heading": -2.525874984240029e-17, + "angularVelocity": 4.727847688027027e-16, + "velocityX": 3.742535336038484, + "velocityY": -0.5446210224066904, + "timestamp": 9.112928275570606 + }, + { + "x": 6.350906221496031, + "y": 4.066966057794483, + "heading": 5.651500811291553e-19, + "angularVelocity": 6.992139128857502e-16, + "velocityX": 3.7623370288523534, + "velocityY": -0.3847116499719891, + "timestamp": 9.149861035986754 + }, + { + "x": 6.49033871531031, + "y": 4.058689280403353, + "heading": 9.896194803608596e-18, + "angularVelocity": 2.526495343979898e-16, + "velocityX": 3.7753065907664864, + "velocityY": -0.2241039472238093, + "timestamp": 9.186793796402903 + }, + { + "x": 6.62590091042267, + "y": 4.054933032848218, + "heading": 1.2997488502950548e-17, + "angularVelocity": 8.397134860988979e-17, + "velocityX": 3.67051348409614, + "velocityY": -0.10170503132750916, + "timestamp": 9.22372655681905 + }, + { + "x": 6.7559406325562135, + "y": 4.053394192007746, + "heading": 1.2355813206346684e-17, + "angularVelocity": -1.7374149364595927e-17, + "velocityX": 3.520985722927149, + "velocityY": -0.04166601204816814, + "timestamp": 9.260659317235199 + }, + { + "x": 6.880159033617416, + "y": 4.05309103302126, + "heading": 1.0199232759050705e-17, + "angularVelocity": -5.839207311638201e-17, + "velocityX": 3.3633662813596223, + "velocityY": -0.008208403137756505, + "timestamp": 9.297592077651347 + }, + { + "x": 6.998473495645727, + "y": 4.053535026725706, + "heading": 6.484491281135395e-18, + "angularVelocity": -1.0058120312082129e-16, + "velocityX": 3.2035098567011917, + "velocityY": 0.012021676675224401, + "timestamp": 9.334524838067495 + }, + { + "x": 7.1108547909012785, + "y": 4.054439771564095, + "heading": 3.84540621087932e-18, + "angularVelocity": -7.14564803668238e-17, + "velocityX": 3.0428620549689076, + "velocityY": 0.024497081404008324, + "timestamp": 9.371457598483643 + }, + { + "x": 7.217291355133057, + "y": 4.0556182861328125, + "heading": 9.032556666486843e-28, + "angularVelocity": -1.0411911175769075e-16, + "velocityX": 2.881901136889931, + "velocityY": 0.03190973421532392, + "timestamp": 9.40839035889979 + }, + { + "x": 7.405345313483542, + "y": 4.0585235652542355, + "heading": -6.2741861661918545e-18, + "angularVelocity": -8.546860903842953e-17, + "velocityX": 2.5617203276716056, + "velocityY": 0.039576473944975675, + "timestamp": 9.481799605146984 + }, + { + "x": 7.569888777644686, + "y": 4.061578730558428, + "heading": -8.568202082154757e-18, + "angularVelocity": -3.124968629224788e-17, + "velocityX": 2.241454211463674, + "velocityY": 0.04161826282625519, + "timestamp": 9.555208851394177 + }, + { + "x": 7.710921484094839, + "y": 4.064533122417797, + "heading": -8.450454397008985e-18, + "angularVelocity": 1.603989837150655e-18, + "velocityX": 1.9211845054947108, + "velocityY": 0.040245500538451925, + "timestamp": 9.62861809764137 + }, + { + "x": 7.828444755273268, + "y": 4.067218459024374, + "heading": -7.03981174142149e-18, + "angularVelocity": 1.921614411657278e-17, + "velocityX": 1.6009328141401953, + "velocityY": 0.03658035933967648, + "timestamp": 9.702027343888563 + }, + { + "x": 7.922460282697814, + "y": 4.069513975215649, + "heading": -4.798172497228405e-18, + "angularVelocity": 3.053619753065246e-17, + "velocityX": 1.2807041650852087, + "velocityY": 0.03127012343303199, + "timestamp": 9.775436590135756 + }, + { + "x": 7.992969748455868, + "y": 4.071328793464346, + "heading": -2.7225824385142056e-18, + "angularVelocity": 2.8274231988604325e-17, + "velocityX": 0.9604984298657211, + "velocityY": 0.024721930022069162, + "timestamp": 9.84884583638295 + }, + { + "x": 8.039974708011181, + "y": 4.0725920534308, + "heading": -1.0566305754795318e-18, + "angularVelocity": 2.2694033081505876e-17, + "velocityX": 0.6403138835812464, + "velocityY": 0.01720845848492492, + "timestamp": 9.922255082630143 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": -4.292440471277117e-28, + "angularVelocity": 1.4393698734895493e-17, + "velocityX": 0.32014842394214194, + "velocityY": 0.0089212527612154, + "timestamp": 9.995664328877336 + }, + { + "x": 8.0634765625, + "y": 4.073246955871582, + "heading": -2.12372396116075e-28, + "angularVelocity": 6.129002987458633e-29, + "velocityX": -2.550409184277031e-23, + "velocityY": -6.242614501745333e-23, + "timestamp": 10.069073575124529 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneSprint.1.traj b/src/main/deploy/choreo/CenterLaneSprint.1.traj index b15ef766..fd842a4b 100644 --- a/src/main/deploy/choreo/CenterLaneSprint.1.traj +++ b/src/main/deploy/choreo/CenterLaneSprint.1.traj @@ -1,409 +1,410 @@ { - "samples": [ - { - "x": 1.2955970764160156, - "y": 5.586942195892334, - "heading": 0, - "angularVelocity": -1.1592130893720681e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.3207451593695585, - "y": 5.599086645701892, - "heading": 2.231825504271677e-18, - "angularVelocity": 2.789547782680952e-17, - "velocityX": 0.3143242018214309, - "velocityY": 0.15179266348424167, - "timestamp": 0.08000682991577572 - }, - { - "x": 1.3713809631464564, - "y": 5.62264555119355, - "heading": 4.51507105043022e-18, - "angularVelocity": 2.853816960464376e-17, - "velocityX": 0.6328935145938026, - "velocityY": 0.2944611793396252, - "timestamp": 0.16001365983155144 - }, - { - "x": 1.4479102774130088, - "y": 5.656665804231403, - "heading": 6.004653783100575e-18, - "angularVelocity": 1.8618332407574406e-17, - "velocityX": 0.9565347651837746, - "velocityY": 0.4252168605416788, - "timestamp": 0.24002048974732715 - }, - { - "x": 1.5508175668842004, - "y": 5.699857537973527, - "heading": 7.57580271600204e-18, - "angularVelocity": 1.963779951822175e-17, - "velocityX": 1.2862313077471652, - "velocityY": 0.5398505826146306, - "timestamp": 0.3200273196631029 - }, - { - "x": 1.6806682557862314, - "y": 5.750395481752386, - "heading": 9.251664351913254e-18, - "angularVelocity": 2.0946531612097715e-17, - "velocityX": 1.6229950497837156, - "velocityY": 0.6316703690584408, - "timestamp": 0.4000341495788786 - }, - { - "x": 1.8380612948775077, - "y": 5.805552130674172, - "heading": 1.2501573477685283e-17, - "angularVelocity": 4.062041901915404e-17, - "velocityX": 1.9672450371704346, - "velocityY": 0.6893992548031757, - "timestamp": 0.48004097949465435 - }, - { - "x": 2.0233797317677533, - "y": 5.861001231409725, - "heading": 1.6054120788359745e-17, - "angularVelocity": 4.440302445729553e-17, - "velocityX": 2.316282710935199, - "velocityY": 0.6930545904033714, - "timestamp": 0.5600478094104301 - }, - { - "x": 2.235784948485807, - "y": 5.909651835150332, - "heading": 2.213686509511287e-17, - "angularVelocity": 7.602767686101118e-17, - "velocityX": 2.6548385549342215, - "velocityY": 0.6080806325227727, - "timestamp": 0.6400546393262058 - }, - { - "x": 2.4702985640404234, - "y": 5.941239855854083, - "heading": 2.864477544319077e-17, - "angularVelocity": 8.134175236038742e-17, - "velocityX": 2.9311699488835434, - "velocityY": 0.39481655174808844, - "timestamp": 0.7200614692419814 - }, - { - "x": 2.715877888033922, - "y": 5.947186832463751, - "heading": 3.119004227784184e-17, - "angularVelocity": 3.1813081703857335e-17, - "velocityX": 3.0694794963261316, - "velocityY": 0.07433086169371976, - "timestamp": 0.8000682991577571 - }, - { - "x": 2.961603624456153, - "y": 5.925207266559582, - "heading": 3.402854212881078e-17, - "angularVelocity": 3.5478294907307905e-17, - "velocityX": 3.0713094954425415, - "velocityY": -0.2747211197853527, - "timestamp": 0.8800751290735328 - }, - { - "x": 3.2005883629277774, - "y": 5.876126550770939, - "heading": 3.9287160074963035e-17, - "angularVelocity": 6.572712324724996e-17, - "velocityX": 2.987054214261441, - "velocityY": -0.6134565741588879, - "timestamp": 0.9600819589893085 - }, - { - "x": 3.428817982052201, - "y": 5.801272963497273, - "heading": 4.077287726704947e-17, - "angularVelocity": 1.856989731461567e-17, - "velocityX": 2.8526266990269726, - "velocityY": -0.9355899659232499, - "timestamp": 1.0400887889050843 - }, - { - "x": 3.6438287356883494, - "y": 5.701819067812634, - "heading": 4.10496280483666e-17, - "angularVelocity": 3.459161663925654e-18, - "velocityX": 2.6874049860525546, - "velocityY": -1.243067570484904, - "timestamp": 1.12009561882086 - }, - { - "x": 3.8440012077207406, - "y": 5.578706367507132, - "heading": 4.4578411240497065e-17, - "angularVelocity": 4.41060783161696e-17, - "velocityX": 2.5019422996871006, - "velocityY": -1.538777382333539, - "timestamp": 1.2001024487366356 - }, - { - "x": 4.028205871582031, - "y": 5.432682037353516, - "heading": 4.825304787734528e-17, - "angularVelocity": 4.592915652788645e-17, - "velocityX": 2.302361736529145, - "velocityY": -1.825148306820832, - "timestamp": 1.2801092786524113 - }, - { - "x": 4.11697825249087, - "y": 5.353217174122665, - "heading": 5.003311226824959e-17, - "angularVelocity": 4.404060645079727e-17, - "velocityX": 2.1963179706677165, - "velocityY": -1.9660406239470811, - "timestamp": 1.3205280076771686 - }, - { - "x": 4.201472100752748, - "y": 5.268051894541109, - "heading": 5.106612036315502e-17, - "angularVelocity": 2.555768903480938e-17, - "velocityX": 2.0904627706110244, - "velocityY": -2.1070746566406697, - "timestamp": 1.360946736701926 - }, - { - "x": 4.281698714286708, - "y": 5.177177737015351, - "heading": 5.2015291988095576e-17, - "angularVelocity": 2.3483498091432662e-17, - "velocityX": 1.9848870924323019, - "velocityY": -2.2483180376661425, - "timestamp": 1.4013654657266832 - }, - { - "x": 4.357676572309284, - "y": 5.080580936550585, - "heading": 5.3034148842228453e-17, - "angularVelocity": 2.5207570315775117e-17, - "velocityX": 1.8797686086529983, - "velocityY": -2.3899019785048825, - "timestamp": 1.4417841947514405 - }, - { - "x": 4.429441369664381, - "y": 4.978235165876808, - "heading": 5.4156772615538654e-17, - "angularVelocity": 2.7774872934374724e-17, - "velocityX": 1.7755332512056952, - "velocityY": -2.5321372824774984, - "timestamp": 1.4822029237761978 - }, - { - "x": 4.497091014290783, - "y": 4.870069946485337, - "heading": 5.562432201646814e-17, - "angularVelocity": 3.6308680200224936e-17, - "velocityX": 1.6737202346179283, - "velocityY": -2.6761162956262776, - "timestamp": 1.522621652800955 - }, - { - "x": 4.561997626649892, - "y": 4.755326331353034, - "heading": 5.3639840433255375e-17, - "angularVelocity": -4.9098048541470276e-17, - "velocityX": 1.6058548580143424, - "velocityY": -2.8388724212920464, - "timestamp": 1.5630403818257124 - }, - { - "x": 4.631511261933969, - "y": 4.646021074802578, - "heading": 5.11321781778172e-17, - "angularVelocity": -6.20420508556326e-17, - "velocityX": 1.7198372378713005, - "velocityY": -2.704321961318929, - "timestamp": 1.6034591108504697 - }, - { - "x": 4.705480207686577, - "y": 4.542279156484067, - "heading": 4.966571246690934e-17, - "angularVelocity": -3.628181222560111e-17, - "velocityX": 1.8300660989972783, - "velocityY": -2.566679379125897, - "timestamp": 1.643877839875227 - }, - { - "x": 4.783858959652748, - "y": 4.44413671743755, - "heading": 4.8920670981753807e-17, - "angularVelocity": -1.843305932153545e-17, - "velocityX": 1.939169139097368, - "velocityY": -2.4281426312595698, - "timestamp": 1.6842965688999842 - }, - { - "x": 4.86662561684807, - "y": 4.3516109378583465, - "heading": 4.74522093373318e-17, - "angularVelocity": -3.633120321770328e-17, - "velocityX": 2.0477303268199805, - "velocityY": -2.2891808280849295, - "timestamp": 1.7247152979247415 - }, - { - "x": 4.9537672996521, - "y": 4.264711856842041, - "heading": 4.682424401401172e-17, - "angularVelocity": -1.5536488446652234e-17, - "velocityX": 2.1559728597813255, - "velocityY": -2.1499706475905587, - "timestamp": 1.7651340269494988 - }, - { - "x": 5.149371577077341, - "y": 4.111086251410353, - "heading": 4.245818870652888e-17, - "angularVelocity": -5.3034667663063016e-17, - "velocityX": 2.376013169452415, - "velocityY": -1.8660965214215293, - "timestamp": 1.847458603234601 - }, - { - "x": 5.361995748730517, - "y": 3.981639454553548, - "heading": 3.8311842606234285e-17, - "angularVelocity": -5.0365753698371845e-17, - "velocityX": 2.582754521741994, - "velocityY": -1.5723955433397911, - "timestamp": 1.9297831795197031 - }, - { - "x": 5.590089166249063, - "y": 3.8773917441987824, - "heading": 3.443521699897252e-17, - "angularVelocity": -4.708934879277126e-17, - "velocityX": 2.7706600848603538, - "velocityY": -1.2663012074886832, - "timestamp": 2.0121077558048053 - }, - { - "x": 5.831330212934442, - "y": 3.799628582145428, - "heading": 2.9264449434168703e-17, - "angularVelocity": -6.280941218977205e-17, - "velocityX": 2.9303648748534723, - "velocityY": -0.9445923145111385, - "timestamp": 2.0944323320899074 - }, - { - "x": 6.082008486894108, - "y": 3.749887377260942, - "heading": 2.126679829101086e-17, - "angularVelocity": -9.714783202978127e-17, - "velocityX": 3.044999261088646, - "velocityY": -0.6042084530518639, - "timestamp": 2.1767569083750096 - }, - { - "x": 6.335839306220575, - "y": 3.729546047320075, - "heading": 1.712845100944366e-17, - "angularVelocity": -5.02686298415513e-17, - "velocityX": 3.0832933587884717, - "velocityY": -0.2470869679456106, - "timestamp": 2.2590814846601117 - }, - { - "x": 6.582471637063415, - "y": 3.7378835035769398, - "heading": 1.3745368058462902e-17, - "angularVelocity": -4.109437406088766e-17, - "velocityX": 2.9958530243527477, - "velocityY": 0.10127542263348481, - "timestamp": 2.341406060945214 - }, - { - "x": 6.809467380291713, - "y": 3.768327415425299, - "heading": 1.1162328016061889e-17, - "angularVelocity": -3.137619156324536e-17, - "velocityX": 2.757326590330181, - "velocityY": 0.36980344415440275, - "timestamp": 2.423730637230316 - }, - { - "x": 7.009276028148806, - "y": 3.8103968563967796, - "heading": 8.26924402312721e-18, - "angularVelocity": -3.5142311707247735e-17, - "velocityX": 2.427083829324394, - "velocityY": 0.5110192225827506, - "timestamp": 2.506055213515418 - }, - { - "x": 7.179701211541178, - "y": 3.8557683077640763, - "heading": 5.91482335399215e-18, - "angularVelocity": -2.8599155256386034e-17, - "velocityX": 2.07016168297691, - "velocityY": 0.5511288781087693, - "timestamp": 2.5883797898005203 - }, - { - "x": 7.320624454413459, - "y": 3.899158119195988, - "heading": 3.995408556984211e-18, - "angularVelocity": -2.3315109304438273e-17, - "velocityX": 1.7118004031318457, - "velocityY": 0.5270578166492144, - "timestamp": 2.6707043660856224 - }, - { - "x": 7.432462903898932, - "y": 3.9372219303084806, - "heading": 2.438823399699254e-18, - "angularVelocity": -1.890781257081445e-17, - "velocityX": 1.35850622660075, - "velocityY": 0.4623626726238319, - "timestamp": 2.7530289423707246 - }, - { - "x": 7.515713681888788, - "y": 3.967733484062364, - "heading": 1.1936770855475268e-18, - "angularVelocity": -1.5124745408572555e-17, - "velocityX": 1.0112506100436387, - "velocityY": 0.37062509315884895, - "timestamp": 2.8353535186558267 - }, - { - "x": 7.570835923487974, - "y": 3.989130376969442, - "heading": 2.2208685009288135e-19, - "angularVelocity": -1.1801843204007997e-17, - "velocityX": 0.6695721264120479, - "velocityY": 0.25990893452889574, - "timestamp": 2.917678094940929 - }, - { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -1.415925420811752e-32, - "angularVelocity": -2.6977014897899104e-18, - "velocityX": 0.3327266129215478, - "velocityY": 0.13525998955023022, - "timestamp": 3.000002671226031 - }, - { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -2.7014243414553336e-34, - "angularVelocity": 1.6871197561712535e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 3.082327247511133 - } - ] + "samples": [ + { + "x": 1.2955970764160156, + "y": 5.586942195892334, + "heading": -1.15998018681278e-25, + "angularVelocity": 7.739922918144294e-25, + "velocityX": 9.282187758357099e-21, + "velocityY": -2.0893158073718473e-20, + "timestamp": 0 + }, + { + "x": 1.320745159351232, + "y": 5.599086645641333, + "heading": -1.049092437408357e-17, + "angularVelocity": -1.311255590411248e-16, + "velocityX": 0.3143242010468075, + "velocityY": 0.15179266246022666, + "timestamp": 0.08000683005465262 + }, + { + "x": 1.371380963088836, + "y": 5.622645551014504, + "heading": -4.3101779756853274e-17, + "angularVelocity": -4.0760132341640306e-16, + "velocityX": 0.6328935130040201, + "velocityY": 0.2944611773403811, + "timestamp": 0.16001366010930523 + }, + { + "x": 1.4479102772917398, + "y": 5.656665803879934, + "heading": -1.0786579481334514e-16, + "angularVelocity": -8.094817731725291e-16, + "velocityX": 0.9565347627274442, + "velocityY": 0.4252168576383191, + "timestamp": 0.24002049016395785 + }, + { + "x": 1.5508175666705484, + "y": 5.699857537402172, + "heading": -2.0334854319560128e-16, + "angularVelocity": -1.1934334700391475e-15, + "velocityX": 1.286231304358806, + "velocityY": 0.539850578916034, + "timestamp": 0.32002732021861047 + }, + { + "x": 1.680668255445947, + "y": 5.7503954809246025, + "heading": -3.285516720695047e-16, + "angularVelocity": -1.564906883941498e-15, + "velocityX": 1.6229950453817308, + "velocityY": 0.6316703647414047, + "timestamp": 0.40003415027326306 + }, + { + "x": 1.838061294370242, + "y": 5.80555212957311, + "heading": -4.764293851393569e-16, + "angularVelocity": -1.8483154501568934e-15, + "velocityX": 1.9672450316651602, + "velocityY": 0.689399250174457, + "timestamp": 0.48004098032791565 + }, + { + "x": 2.0233797310506665, + "y": 5.8610012300559005, + "heading": -6.647867232817025e-16, + "angularVelocity": -2.354268154652633e-15, + "velocityX": 2.316282704287029, + "velocityY": 0.6930545860260591, + "timestamp": 0.5600478103825682 + }, + { + "x": 2.2357849475361156, + "y": 5.909651833631349, + "heading": -8.83507324298293e-16, + "angularVelocity": -2.7337773116326468e-15, + "velocityX": 2.6548385474134464, + "velocityY": 0.6080806293941452, + "timestamp": 0.6400546404372208 + }, + { + "x": 2.470298562920597, + "y": 5.9412398543265486, + "heading": -1.1401611802382058e-15, + "angularVelocity": -3.2079035041983383e-15, + "velocityX": 2.931169941669824, + "velocityY": 0.39481655095687995, + "timestamp": 0.7200614704918734 + }, + { + "x": 2.715877886892817, + "y": 5.947186831043097, + "heading": -1.4273357911268484e-15, + "angularVelocity": -3.589381409909839e-15, + "velocityX": 3.069479490744277, + "velocityY": 0.07433086290896584, + "timestamp": 0.800068300546526 + }, + { + "x": 2.961603623411278, + "y": 5.925207265290794, + "heading": -1.7286845121883707e-15, + "angularVelocity": -3.766543528450414e-15, + "velocityX": 3.0713094913347376, + "velocityY": -0.2747211174005281, + "timestamp": 0.8800751306011786 + }, + { + "x": 3.200588362045183, + "y": 5.876126549681864, + "heading": -2.0448770370213645e-15, + "angularVelocity": -3.952075909922286e-15, + "velocityX": 2.9870542111299168, + "velocityY": -0.6134565708379317, + "timestamp": 0.9600819606558312 + }, + { + "x": 3.4288179813677098, + "y": 5.801272962619079, + "heading": -2.3674444528927447e-15, + "angularVelocity": -4.031755779497343e-15, + "velocityX": 2.8526266965785654, + "velocityY": -0.9355899616526571, + "timestamp": 1.0400887907104839 + }, + { + "x": 3.6438287352220136, + "y": 5.701819067182799, + "heading": -2.6977753991710968e-15, + "angularVelocity": -4.128792062826012e-15, + "velocityX": 2.687404984142338, + "velocityY": -1.2430675652106578, + "timestamp": 1.1200956207651365 + }, + { + "x": 3.844001207484155, + "y": 5.578706367168648, + "heading": -3.0187544149547513e-15, + "angularVelocity": -4.0119032807509685e-15, + "velocityX": 2.5019422982438493, + "velocityY": -1.538777376007428, + "timestamp": 1.200102450819789 + }, + { + "x": 4.028205871582031, + "y": 5.432682037353516, + "heading": -3.3335153284372285e-15, + "angularVelocity": -3.934183957133696e-15, + "velocityX": 2.302361735517395, + "velocityY": -1.8251482994061918, + "timestamp": 1.2801092808744416 + }, + { + "x": 4.116978252667536, + "y": 5.353217174251384, + "heading": -3.489150469085252e-15, + "angularVelocity": -3.850570993843687e-15, + "velocityX": 2.1963179697950594, + "velocityY": -1.9660406160607158, + "timestamp": 1.3205280099957935 + }, + { + "x": 4.201472101100227, + "y": 5.268051894804914, + "heading": -3.639541652943779e-15, + "angularVelocity": -3.720830252304448e-15, + "velocityX": 2.090462769846413, + "velocityY": -2.107074648259462, + "timestamp": 1.3609467391171453 + }, + { + "x": 4.281698714797313, + "y": 5.17717773742197, + "heading": -3.7839469401200426e-15, + "angularVelocity": -3.572733177528651e-15, + "velocityX": 1.9848870917295764, + "velocityY": -2.248318028756004, + "timestamp": 1.401365468238497 + }, + { + "x": 4.357676572972308, + "y": 5.080580937109945, + "heading": -3.920017225673807e-15, + "angularVelocity": -3.366516893435145e-15, + "velocityX": 1.8797686079362514, + "velocityY": -2.3899019690105905, + "timestamp": 1.4417841973598489 + }, + { + "x": 4.429441370463213, + "y": 4.978235166602986, + "heading": -4.042506986372987e-15, + "angularVelocity": -3.0305211076940933e-15, + "velocityX": 1.7755332503265975, + "velocityY": -2.5321372722947273, + "timestamp": 1.4822029264812007 + }, + { + "x": 4.4970910151920265, + "y": 4.870069947402846, + "heading": -4.139505406670724e-15, + "angularVelocity": -2.3998398161117503e-15, + "velocityX": 1.6737202331548666, + "velocityY": -2.6761162844925224, + "timestamp": 1.5226216556025525 + }, + { + "x": 4.561997627464952, + "y": 4.755326332071665, + "heading": -4.1031671192864916e-15, + "angularVelocity": 8.99043679267757e-16, + "velocityX": 1.6058548520428109, + "velocityY": -2.8388724194339274, + "timestamp": 1.5630403847239043 + }, + { + "x": 4.6315112626192185, + "y": 4.646021075355723, + "heading": -3.9748504414902894e-15, + "angularVelocity": 3.174685126320571e-15, + "velocityX": 1.71983723054629, + "velocityY": -2.7043219589554015, + "timestamp": 1.603459113845256 + }, + { + "x": 4.705480208220003, + "y": 4.542279156886889, + "heading": -3.831832674451909e-15, + "angularVelocity": 3.5384047912600144e-15, + "velocityX": 1.8300660908635011, + "velocityY": -2.566679376715867, + "timestamp": 1.6438778429666079 + }, + { + "x": 4.783858960019311, + "y": 4.444136717699662, + "heading": -3.678608964011542e-15, + "angularVelocity": 3.7909100509561506e-15, + "velocityX": 1.9391691303303216, + "velocityY": -2.4281426289422314, + "timestamp": 1.6842965720879597 + }, + { + "x": 4.86662561703615, + "y": 4.351610937986713, + "heading": -3.518654034373506e-15, + "angularVelocity": 3.957447150613264e-15, + "velocityX": 2.047730317505612, + "velocityY": -2.2891808259273896, + "timestamp": 1.7247153012093115 + }, + { + "x": 4.9537672996521, + "y": 4.264711856842041, + "heading": -3.353665589188468e-15, + "angularVelocity": 4.081981361358232e-15, + "velocityX": 2.155972849970579, + "velocityY": -2.149970645632314, + "timestamp": 1.7651340303306633 + }, + { + "x": 5.149371576843732, + "y": 4.1110862510516375, + "heading": -3.00177635471192e-15, + "angularVelocity": 4.274403041302851e-15, + "velocityX": 2.3760131588886937, + "velocityY": -1.8660965196719834, + "timestamp": 1.8474586068844063 + }, + { + "x": 5.361995748264174, + "y": 3.9816394538906854, + "heading": -2.6353623927870383e-15, + "angularVelocity": 4.450835884022828e-15, + "velocityX": 2.582754510515282, + "velocityY": -1.5723955418881683, + "timestamp": 1.9297831834381494 + }, + { + "x": 5.590089165557832, + "y": 3.8773917432800733, + "heading": -2.261979917940497e-15, + "angularVelocity": 4.535483155952485e-15, + "velocityX": 2.7706600731162854, + "velocityY": -1.2663012064517525, + "timestamp": 2.0121077599918924 + }, + { + "x": 5.831330212039324, + "y": 3.799628581011004, + "heading": -1.9000074489203716e-15, + "angularVelocity": 4.396885915313072e-15, + "velocityX": 2.9303648628426733, + "velocityY": -0.9445923140375594, + "timestamp": 2.0944323365456357 + }, + { + "x": 6.082008485841778, + "y": 3.749887375942779, + "heading": -1.5532288164739933e-15, + "angularVelocity": 4.21232648197744e-15, + "velocityX": 3.044999249268032, + "velocityY": -0.6042084533014725, + "timestamp": 2.176756913099379 + }, + { + "x": 6.335839305106152, + "y": 3.7295460458524454, + "heading": -1.2319901327901992e-15, + "angularVelocity": 3.9020926184765074e-15, + "velocityX": 3.0832933479915057, + "velocityY": -0.2470869689447997, + "timestamp": 2.259081489653122 + }, + { + "x": 6.582471636042754, + "y": 3.7378835020415453, + "heading": -9.36500814934657e-16, + "angularVelocity": 3.589315196126135e-15, + "velocityX": 2.995853015722435, + "velocityY": 0.10127542148557588, + "timestamp": 2.3414060662068654 + }, + { + "x": 6.8094673794894955, + "y": 3.7683274139859995, + "heading": -6.953816130946725e-16, + "angularVelocity": 2.928880396529573e-15, + "velocityX": 2.757326583982553, + "velocityY": 0.3698034441094732, + "timestamp": 2.4237306427606087 + }, + { + "x": 7.00927602757565, + "y": 3.81039685518788, + "heading": -4.96986443618591e-16, + "angularVelocity": 2.4099108474923954e-15, + "velocityX": 2.427083824181171, + "velocityY": 0.5110192236997284, + "timestamp": 2.506055219314352 + }, + { + "x": 7.179701211157642, + "y": 3.855768306837493, + "heading": -3.462577830221023e-16, + "angularVelocity": 1.8309046086233712e-15, + "velocityX": 2.070161678520555, + "velocityY": 0.5511288797213748, + "timestamp": 2.588379795868095 + }, + { + "x": 7.320624454174356, + "y": 3.899158118548539, + "heading": -2.3018244299305313e-16, + "angularVelocity": 1.409969937133298e-15, + "velocityX": 1.711800399297762, + "velocityY": 0.5270578183020111, + "timestamp": 2.6707043724218384 + }, + { + "x": 7.432462903764418, + "y": 3.93722192990686, + "heading": -1.4070305310406466e-16, + "angularVelocity": 1.0869083608873546e-15, + "velocityX": 1.358506223436839, + "velocityY": 0.46236267408521664, + "timestamp": 2.7530289489755817 + }, + { + "x": 7.515713681825484, + "y": 3.9677334838565543, + "heading": -7.487411399750651e-17, + "angularVelocity": 7.996258056579044e-16, + "velocityX": 1.0112506076082748, + "velocityY": 0.3706250943148165, + "timestamp": 2.835353525529325 + }, + { + "x": 7.570835923468037, + "y": 3.9891303768994915, + "heading": -2.4456304849391743e-17, + "angularVelocity": 6.12426558799873e-16, + "velocityX": 0.6695721247538555, + "velocityY": 0.25990893532273207, + "timestamp": 2.917678102083068 + }, + { + "x": 7.598227500915527, + "y": 4.000265598297119, + "heading": 3.2179463374057155e-25, + "angularVelocity": 2.9707144199274434e-16, + "velocityX": 0.33272661207810256, + "velocityY": 0.13525998995380084, + "timestamp": 3.0000026786368115 + }, + { + "x": 7.598227500915527, + "y": 4.000265598297119, + "heading": 1.144163224349134e-25, + "angularVelocity": -1.1292130842830807e-24, + "velocityX": -1.2177734406642243e-20, + "velocityY": 2.8518350164068296e-20, + "timestamp": 3.0823272551905547 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneSprint.traj b/src/main/deploy/choreo/CenterLaneSprint.traj index b15ef766..fd842a4b 100644 --- a/src/main/deploy/choreo/CenterLaneSprint.traj +++ b/src/main/deploy/choreo/CenterLaneSprint.traj @@ -1,409 +1,410 @@ { - "samples": [ - { - "x": 1.2955970764160156, - "y": 5.586942195892334, - "heading": 0, - "angularVelocity": -1.1592130893720681e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.3207451593695585, - "y": 5.599086645701892, - "heading": 2.231825504271677e-18, - "angularVelocity": 2.789547782680952e-17, - "velocityX": 0.3143242018214309, - "velocityY": 0.15179266348424167, - "timestamp": 0.08000682991577572 - }, - { - "x": 1.3713809631464564, - "y": 5.62264555119355, - "heading": 4.51507105043022e-18, - "angularVelocity": 2.853816960464376e-17, - "velocityX": 0.6328935145938026, - "velocityY": 0.2944611793396252, - "timestamp": 0.16001365983155144 - }, - { - "x": 1.4479102774130088, - "y": 5.656665804231403, - "heading": 6.004653783100575e-18, - "angularVelocity": 1.8618332407574406e-17, - "velocityX": 0.9565347651837746, - "velocityY": 0.4252168605416788, - "timestamp": 0.24002048974732715 - }, - { - "x": 1.5508175668842004, - "y": 5.699857537973527, - "heading": 7.57580271600204e-18, - "angularVelocity": 1.963779951822175e-17, - "velocityX": 1.2862313077471652, - "velocityY": 0.5398505826146306, - "timestamp": 0.3200273196631029 - }, - { - "x": 1.6806682557862314, - "y": 5.750395481752386, - "heading": 9.251664351913254e-18, - "angularVelocity": 2.0946531612097715e-17, - "velocityX": 1.6229950497837156, - "velocityY": 0.6316703690584408, - "timestamp": 0.4000341495788786 - }, - { - "x": 1.8380612948775077, - "y": 5.805552130674172, - "heading": 1.2501573477685283e-17, - "angularVelocity": 4.062041901915404e-17, - "velocityX": 1.9672450371704346, - "velocityY": 0.6893992548031757, - "timestamp": 0.48004097949465435 - }, - { - "x": 2.0233797317677533, - "y": 5.861001231409725, - "heading": 1.6054120788359745e-17, - "angularVelocity": 4.440302445729553e-17, - "velocityX": 2.316282710935199, - "velocityY": 0.6930545904033714, - "timestamp": 0.5600478094104301 - }, - { - "x": 2.235784948485807, - "y": 5.909651835150332, - "heading": 2.213686509511287e-17, - "angularVelocity": 7.602767686101118e-17, - "velocityX": 2.6548385549342215, - "velocityY": 0.6080806325227727, - "timestamp": 0.6400546393262058 - }, - { - "x": 2.4702985640404234, - "y": 5.941239855854083, - "heading": 2.864477544319077e-17, - "angularVelocity": 8.134175236038742e-17, - "velocityX": 2.9311699488835434, - "velocityY": 0.39481655174808844, - "timestamp": 0.7200614692419814 - }, - { - "x": 2.715877888033922, - "y": 5.947186832463751, - "heading": 3.119004227784184e-17, - "angularVelocity": 3.1813081703857335e-17, - "velocityX": 3.0694794963261316, - "velocityY": 0.07433086169371976, - "timestamp": 0.8000682991577571 - }, - { - "x": 2.961603624456153, - "y": 5.925207266559582, - "heading": 3.402854212881078e-17, - "angularVelocity": 3.5478294907307905e-17, - "velocityX": 3.0713094954425415, - "velocityY": -0.2747211197853527, - "timestamp": 0.8800751290735328 - }, - { - "x": 3.2005883629277774, - "y": 5.876126550770939, - "heading": 3.9287160074963035e-17, - "angularVelocity": 6.572712324724996e-17, - "velocityX": 2.987054214261441, - "velocityY": -0.6134565741588879, - "timestamp": 0.9600819589893085 - }, - { - "x": 3.428817982052201, - "y": 5.801272963497273, - "heading": 4.077287726704947e-17, - "angularVelocity": 1.856989731461567e-17, - "velocityX": 2.8526266990269726, - "velocityY": -0.9355899659232499, - "timestamp": 1.0400887889050843 - }, - { - "x": 3.6438287356883494, - "y": 5.701819067812634, - "heading": 4.10496280483666e-17, - "angularVelocity": 3.459161663925654e-18, - "velocityX": 2.6874049860525546, - "velocityY": -1.243067570484904, - "timestamp": 1.12009561882086 - }, - { - "x": 3.8440012077207406, - "y": 5.578706367507132, - "heading": 4.4578411240497065e-17, - "angularVelocity": 4.41060783161696e-17, - "velocityX": 2.5019422996871006, - "velocityY": -1.538777382333539, - "timestamp": 1.2001024487366356 - }, - { - "x": 4.028205871582031, - "y": 5.432682037353516, - "heading": 4.825304787734528e-17, - "angularVelocity": 4.592915652788645e-17, - "velocityX": 2.302361736529145, - "velocityY": -1.825148306820832, - "timestamp": 1.2801092786524113 - }, - { - "x": 4.11697825249087, - "y": 5.353217174122665, - "heading": 5.003311226824959e-17, - "angularVelocity": 4.404060645079727e-17, - "velocityX": 2.1963179706677165, - "velocityY": -1.9660406239470811, - "timestamp": 1.3205280076771686 - }, - { - "x": 4.201472100752748, - "y": 5.268051894541109, - "heading": 5.106612036315502e-17, - "angularVelocity": 2.555768903480938e-17, - "velocityX": 2.0904627706110244, - "velocityY": -2.1070746566406697, - "timestamp": 1.360946736701926 - }, - { - "x": 4.281698714286708, - "y": 5.177177737015351, - "heading": 5.2015291988095576e-17, - "angularVelocity": 2.3483498091432662e-17, - "velocityX": 1.9848870924323019, - "velocityY": -2.2483180376661425, - "timestamp": 1.4013654657266832 - }, - { - "x": 4.357676572309284, - "y": 5.080580936550585, - "heading": 5.3034148842228453e-17, - "angularVelocity": 2.5207570315775117e-17, - "velocityX": 1.8797686086529983, - "velocityY": -2.3899019785048825, - "timestamp": 1.4417841947514405 - }, - { - "x": 4.429441369664381, - "y": 4.978235165876808, - "heading": 5.4156772615538654e-17, - "angularVelocity": 2.7774872934374724e-17, - "velocityX": 1.7755332512056952, - "velocityY": -2.5321372824774984, - "timestamp": 1.4822029237761978 - }, - { - "x": 4.497091014290783, - "y": 4.870069946485337, - "heading": 5.562432201646814e-17, - "angularVelocity": 3.6308680200224936e-17, - "velocityX": 1.6737202346179283, - "velocityY": -2.6761162956262776, - "timestamp": 1.522621652800955 - }, - { - "x": 4.561997626649892, - "y": 4.755326331353034, - "heading": 5.3639840433255375e-17, - "angularVelocity": -4.9098048541470276e-17, - "velocityX": 1.6058548580143424, - "velocityY": -2.8388724212920464, - "timestamp": 1.5630403818257124 - }, - { - "x": 4.631511261933969, - "y": 4.646021074802578, - "heading": 5.11321781778172e-17, - "angularVelocity": -6.20420508556326e-17, - "velocityX": 1.7198372378713005, - "velocityY": -2.704321961318929, - "timestamp": 1.6034591108504697 - }, - { - "x": 4.705480207686577, - "y": 4.542279156484067, - "heading": 4.966571246690934e-17, - "angularVelocity": -3.628181222560111e-17, - "velocityX": 1.8300660989972783, - "velocityY": -2.566679379125897, - "timestamp": 1.643877839875227 - }, - { - "x": 4.783858959652748, - "y": 4.44413671743755, - "heading": 4.8920670981753807e-17, - "angularVelocity": -1.843305932153545e-17, - "velocityX": 1.939169139097368, - "velocityY": -2.4281426312595698, - "timestamp": 1.6842965688999842 - }, - { - "x": 4.86662561684807, - "y": 4.3516109378583465, - "heading": 4.74522093373318e-17, - "angularVelocity": -3.633120321770328e-17, - "velocityX": 2.0477303268199805, - "velocityY": -2.2891808280849295, - "timestamp": 1.7247152979247415 - }, - { - "x": 4.9537672996521, - "y": 4.264711856842041, - "heading": 4.682424401401172e-17, - "angularVelocity": -1.5536488446652234e-17, - "velocityX": 2.1559728597813255, - "velocityY": -2.1499706475905587, - "timestamp": 1.7651340269494988 - }, - { - "x": 5.149371577077341, - "y": 4.111086251410353, - "heading": 4.245818870652888e-17, - "angularVelocity": -5.3034667663063016e-17, - "velocityX": 2.376013169452415, - "velocityY": -1.8660965214215293, - "timestamp": 1.847458603234601 - }, - { - "x": 5.361995748730517, - "y": 3.981639454553548, - "heading": 3.8311842606234285e-17, - "angularVelocity": -5.0365753698371845e-17, - "velocityX": 2.582754521741994, - "velocityY": -1.5723955433397911, - "timestamp": 1.9297831795197031 - }, - { - "x": 5.590089166249063, - "y": 3.8773917441987824, - "heading": 3.443521699897252e-17, - "angularVelocity": -4.708934879277126e-17, - "velocityX": 2.7706600848603538, - "velocityY": -1.2663012074886832, - "timestamp": 2.0121077558048053 - }, - { - "x": 5.831330212934442, - "y": 3.799628582145428, - "heading": 2.9264449434168703e-17, - "angularVelocity": -6.280941218977205e-17, - "velocityX": 2.9303648748534723, - "velocityY": -0.9445923145111385, - "timestamp": 2.0944323320899074 - }, - { - "x": 6.082008486894108, - "y": 3.749887377260942, - "heading": 2.126679829101086e-17, - "angularVelocity": -9.714783202978127e-17, - "velocityX": 3.044999261088646, - "velocityY": -0.6042084530518639, - "timestamp": 2.1767569083750096 - }, - { - "x": 6.335839306220575, - "y": 3.729546047320075, - "heading": 1.712845100944366e-17, - "angularVelocity": -5.02686298415513e-17, - "velocityX": 3.0832933587884717, - "velocityY": -0.2470869679456106, - "timestamp": 2.2590814846601117 - }, - { - "x": 6.582471637063415, - "y": 3.7378835035769398, - "heading": 1.3745368058462902e-17, - "angularVelocity": -4.109437406088766e-17, - "velocityX": 2.9958530243527477, - "velocityY": 0.10127542263348481, - "timestamp": 2.341406060945214 - }, - { - "x": 6.809467380291713, - "y": 3.768327415425299, - "heading": 1.1162328016061889e-17, - "angularVelocity": -3.137619156324536e-17, - "velocityX": 2.757326590330181, - "velocityY": 0.36980344415440275, - "timestamp": 2.423730637230316 - }, - { - "x": 7.009276028148806, - "y": 3.8103968563967796, - "heading": 8.26924402312721e-18, - "angularVelocity": -3.5142311707247735e-17, - "velocityX": 2.427083829324394, - "velocityY": 0.5110192225827506, - "timestamp": 2.506055213515418 - }, - { - "x": 7.179701211541178, - "y": 3.8557683077640763, - "heading": 5.91482335399215e-18, - "angularVelocity": -2.8599155256386034e-17, - "velocityX": 2.07016168297691, - "velocityY": 0.5511288781087693, - "timestamp": 2.5883797898005203 - }, - { - "x": 7.320624454413459, - "y": 3.899158119195988, - "heading": 3.995408556984211e-18, - "angularVelocity": -2.3315109304438273e-17, - "velocityX": 1.7118004031318457, - "velocityY": 0.5270578166492144, - "timestamp": 2.6707043660856224 - }, - { - "x": 7.432462903898932, - "y": 3.9372219303084806, - "heading": 2.438823399699254e-18, - "angularVelocity": -1.890781257081445e-17, - "velocityX": 1.35850622660075, - "velocityY": 0.4623626726238319, - "timestamp": 2.7530289423707246 - }, - { - "x": 7.515713681888788, - "y": 3.967733484062364, - "heading": 1.1936770855475268e-18, - "angularVelocity": -1.5124745408572555e-17, - "velocityX": 1.0112506100436387, - "velocityY": 0.37062509315884895, - "timestamp": 2.8353535186558267 - }, - { - "x": 7.570835923487974, - "y": 3.989130376969442, - "heading": 2.2208685009288135e-19, - "angularVelocity": -1.1801843204007997e-17, - "velocityX": 0.6695721264120479, - "velocityY": 0.25990893452889574, - "timestamp": 2.917678094940929 - }, - { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -1.415925420811752e-32, - "angularVelocity": -2.6977014897899104e-18, - "velocityX": 0.3327266129215478, - "velocityY": 0.13525998955023022, - "timestamp": 3.000002671226031 - }, - { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -2.7014243414553336e-34, - "angularVelocity": 1.6871197561712535e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 3.082327247511133 - } - ] + "samples": [ + { + "x": 1.2955970764160156, + "y": 5.586942195892334, + "heading": -1.15998018681278e-25, + "angularVelocity": 7.739922918144294e-25, + "velocityX": 9.282187758357099e-21, + "velocityY": -2.0893158073718473e-20, + "timestamp": 0 + }, + { + "x": 1.320745159351232, + "y": 5.599086645641333, + "heading": -1.049092437408357e-17, + "angularVelocity": -1.311255590411248e-16, + "velocityX": 0.3143242010468075, + "velocityY": 0.15179266246022666, + "timestamp": 0.08000683005465262 + }, + { + "x": 1.371380963088836, + "y": 5.622645551014504, + "heading": -4.3101779756853274e-17, + "angularVelocity": -4.0760132341640306e-16, + "velocityX": 0.6328935130040201, + "velocityY": 0.2944611773403811, + "timestamp": 0.16001366010930523 + }, + { + "x": 1.4479102772917398, + "y": 5.656665803879934, + "heading": -1.0786579481334514e-16, + "angularVelocity": -8.094817731725291e-16, + "velocityX": 0.9565347627274442, + "velocityY": 0.4252168576383191, + "timestamp": 0.24002049016395785 + }, + { + "x": 1.5508175666705484, + "y": 5.699857537402172, + "heading": -2.0334854319560128e-16, + "angularVelocity": -1.1934334700391475e-15, + "velocityX": 1.286231304358806, + "velocityY": 0.539850578916034, + "timestamp": 0.32002732021861047 + }, + { + "x": 1.680668255445947, + "y": 5.7503954809246025, + "heading": -3.285516720695047e-16, + "angularVelocity": -1.564906883941498e-15, + "velocityX": 1.6229950453817308, + "velocityY": 0.6316703647414047, + "timestamp": 0.40003415027326306 + }, + { + "x": 1.838061294370242, + "y": 5.80555212957311, + "heading": -4.764293851393569e-16, + "angularVelocity": -1.8483154501568934e-15, + "velocityX": 1.9672450316651602, + "velocityY": 0.689399250174457, + "timestamp": 0.48004098032791565 + }, + { + "x": 2.0233797310506665, + "y": 5.8610012300559005, + "heading": -6.647867232817025e-16, + "angularVelocity": -2.354268154652633e-15, + "velocityX": 2.316282704287029, + "velocityY": 0.6930545860260591, + "timestamp": 0.5600478103825682 + }, + { + "x": 2.2357849475361156, + "y": 5.909651833631349, + "heading": -8.83507324298293e-16, + "angularVelocity": -2.7337773116326468e-15, + "velocityX": 2.6548385474134464, + "velocityY": 0.6080806293941452, + "timestamp": 0.6400546404372208 + }, + { + "x": 2.470298562920597, + "y": 5.9412398543265486, + "heading": -1.1401611802382058e-15, + "angularVelocity": -3.2079035041983383e-15, + "velocityX": 2.931169941669824, + "velocityY": 0.39481655095687995, + "timestamp": 0.7200614704918734 + }, + { + "x": 2.715877886892817, + "y": 5.947186831043097, + "heading": -1.4273357911268484e-15, + "angularVelocity": -3.589381409909839e-15, + "velocityX": 3.069479490744277, + "velocityY": 0.07433086290896584, + "timestamp": 0.800068300546526 + }, + { + "x": 2.961603623411278, + "y": 5.925207265290794, + "heading": -1.7286845121883707e-15, + "angularVelocity": -3.766543528450414e-15, + "velocityX": 3.0713094913347376, + "velocityY": -0.2747211174005281, + "timestamp": 0.8800751306011786 + }, + { + "x": 3.200588362045183, + "y": 5.876126549681864, + "heading": -2.0448770370213645e-15, + "angularVelocity": -3.952075909922286e-15, + "velocityX": 2.9870542111299168, + "velocityY": -0.6134565708379317, + "timestamp": 0.9600819606558312 + }, + { + "x": 3.4288179813677098, + "y": 5.801272962619079, + "heading": -2.3674444528927447e-15, + "angularVelocity": -4.031755779497343e-15, + "velocityX": 2.8526266965785654, + "velocityY": -0.9355899616526571, + "timestamp": 1.0400887907104839 + }, + { + "x": 3.6438287352220136, + "y": 5.701819067182799, + "heading": -2.6977753991710968e-15, + "angularVelocity": -4.128792062826012e-15, + "velocityX": 2.687404984142338, + "velocityY": -1.2430675652106578, + "timestamp": 1.1200956207651365 + }, + { + "x": 3.844001207484155, + "y": 5.578706367168648, + "heading": -3.0187544149547513e-15, + "angularVelocity": -4.0119032807509685e-15, + "velocityX": 2.5019422982438493, + "velocityY": -1.538777376007428, + "timestamp": 1.200102450819789 + }, + { + "x": 4.028205871582031, + "y": 5.432682037353516, + "heading": -3.3335153284372285e-15, + "angularVelocity": -3.934183957133696e-15, + "velocityX": 2.302361735517395, + "velocityY": -1.8251482994061918, + "timestamp": 1.2801092808744416 + }, + { + "x": 4.116978252667536, + "y": 5.353217174251384, + "heading": -3.489150469085252e-15, + "angularVelocity": -3.850570993843687e-15, + "velocityX": 2.1963179697950594, + "velocityY": -1.9660406160607158, + "timestamp": 1.3205280099957935 + }, + { + "x": 4.201472101100227, + "y": 5.268051894804914, + "heading": -3.639541652943779e-15, + "angularVelocity": -3.720830252304448e-15, + "velocityX": 2.090462769846413, + "velocityY": -2.107074648259462, + "timestamp": 1.3609467391171453 + }, + { + "x": 4.281698714797313, + "y": 5.17717773742197, + "heading": -3.7839469401200426e-15, + "angularVelocity": -3.572733177528651e-15, + "velocityX": 1.9848870917295764, + "velocityY": -2.248318028756004, + "timestamp": 1.401365468238497 + }, + { + "x": 4.357676572972308, + "y": 5.080580937109945, + "heading": -3.920017225673807e-15, + "angularVelocity": -3.366516893435145e-15, + "velocityX": 1.8797686079362514, + "velocityY": -2.3899019690105905, + "timestamp": 1.4417841973598489 + }, + { + "x": 4.429441370463213, + "y": 4.978235166602986, + "heading": -4.042506986372987e-15, + "angularVelocity": -3.0305211076940933e-15, + "velocityX": 1.7755332503265975, + "velocityY": -2.5321372722947273, + "timestamp": 1.4822029264812007 + }, + { + "x": 4.4970910151920265, + "y": 4.870069947402846, + "heading": -4.139505406670724e-15, + "angularVelocity": -2.3998398161117503e-15, + "velocityX": 1.6737202331548666, + "velocityY": -2.6761162844925224, + "timestamp": 1.5226216556025525 + }, + { + "x": 4.561997627464952, + "y": 4.755326332071665, + "heading": -4.1031671192864916e-15, + "angularVelocity": 8.99043679267757e-16, + "velocityX": 1.6058548520428109, + "velocityY": -2.8388724194339274, + "timestamp": 1.5630403847239043 + }, + { + "x": 4.6315112626192185, + "y": 4.646021075355723, + "heading": -3.9748504414902894e-15, + "angularVelocity": 3.174685126320571e-15, + "velocityX": 1.71983723054629, + "velocityY": -2.7043219589554015, + "timestamp": 1.603459113845256 + }, + { + "x": 4.705480208220003, + "y": 4.542279156886889, + "heading": -3.831832674451909e-15, + "angularVelocity": 3.5384047912600144e-15, + "velocityX": 1.8300660908635011, + "velocityY": -2.566679376715867, + "timestamp": 1.6438778429666079 + }, + { + "x": 4.783858960019311, + "y": 4.444136717699662, + "heading": -3.678608964011542e-15, + "angularVelocity": 3.7909100509561506e-15, + "velocityX": 1.9391691303303216, + "velocityY": -2.4281426289422314, + "timestamp": 1.6842965720879597 + }, + { + "x": 4.86662561703615, + "y": 4.351610937986713, + "heading": -3.518654034373506e-15, + "angularVelocity": 3.957447150613264e-15, + "velocityX": 2.047730317505612, + "velocityY": -2.2891808259273896, + "timestamp": 1.7247153012093115 + }, + { + "x": 4.9537672996521, + "y": 4.264711856842041, + "heading": -3.353665589188468e-15, + "angularVelocity": 4.081981361358232e-15, + "velocityX": 2.155972849970579, + "velocityY": -2.149970645632314, + "timestamp": 1.7651340303306633 + }, + { + "x": 5.149371576843732, + "y": 4.1110862510516375, + "heading": -3.00177635471192e-15, + "angularVelocity": 4.274403041302851e-15, + "velocityX": 2.3760131588886937, + "velocityY": -1.8660965196719834, + "timestamp": 1.8474586068844063 + }, + { + "x": 5.361995748264174, + "y": 3.9816394538906854, + "heading": -2.6353623927870383e-15, + "angularVelocity": 4.450835884022828e-15, + "velocityX": 2.582754510515282, + "velocityY": -1.5723955418881683, + "timestamp": 1.9297831834381494 + }, + { + "x": 5.590089165557832, + "y": 3.8773917432800733, + "heading": -2.261979917940497e-15, + "angularVelocity": 4.535483155952485e-15, + "velocityX": 2.7706600731162854, + "velocityY": -1.2663012064517525, + "timestamp": 2.0121077599918924 + }, + { + "x": 5.831330212039324, + "y": 3.799628581011004, + "heading": -1.9000074489203716e-15, + "angularVelocity": 4.396885915313072e-15, + "velocityX": 2.9303648628426733, + "velocityY": -0.9445923140375594, + "timestamp": 2.0944323365456357 + }, + { + "x": 6.082008485841778, + "y": 3.749887375942779, + "heading": -1.5532288164739933e-15, + "angularVelocity": 4.21232648197744e-15, + "velocityX": 3.044999249268032, + "velocityY": -0.6042084533014725, + "timestamp": 2.176756913099379 + }, + { + "x": 6.335839305106152, + "y": 3.7295460458524454, + "heading": -1.2319901327901992e-15, + "angularVelocity": 3.9020926184765074e-15, + "velocityX": 3.0832933479915057, + "velocityY": -0.2470869689447997, + "timestamp": 2.259081489653122 + }, + { + "x": 6.582471636042754, + "y": 3.7378835020415453, + "heading": -9.36500814934657e-16, + "angularVelocity": 3.589315196126135e-15, + "velocityX": 2.995853015722435, + "velocityY": 0.10127542148557588, + "timestamp": 2.3414060662068654 + }, + { + "x": 6.8094673794894955, + "y": 3.7683274139859995, + "heading": -6.953816130946725e-16, + "angularVelocity": 2.928880396529573e-15, + "velocityX": 2.757326583982553, + "velocityY": 0.3698034441094732, + "timestamp": 2.4237306427606087 + }, + { + "x": 7.00927602757565, + "y": 3.81039685518788, + "heading": -4.96986443618591e-16, + "angularVelocity": 2.4099108474923954e-15, + "velocityX": 2.427083824181171, + "velocityY": 0.5110192236997284, + "timestamp": 2.506055219314352 + }, + { + "x": 7.179701211157642, + "y": 3.855768306837493, + "heading": -3.462577830221023e-16, + "angularVelocity": 1.8309046086233712e-15, + "velocityX": 2.070161678520555, + "velocityY": 0.5511288797213748, + "timestamp": 2.588379795868095 + }, + { + "x": 7.320624454174356, + "y": 3.899158118548539, + "heading": -2.3018244299305313e-16, + "angularVelocity": 1.409969937133298e-15, + "velocityX": 1.711800399297762, + "velocityY": 0.5270578183020111, + "timestamp": 2.6707043724218384 + }, + { + "x": 7.432462903764418, + "y": 3.93722192990686, + "heading": -1.4070305310406466e-16, + "angularVelocity": 1.0869083608873546e-15, + "velocityX": 1.358506223436839, + "velocityY": 0.46236267408521664, + "timestamp": 2.7530289489755817 + }, + { + "x": 7.515713681825484, + "y": 3.9677334838565543, + "heading": -7.487411399750651e-17, + "angularVelocity": 7.996258056579044e-16, + "velocityX": 1.0112506076082748, + "velocityY": 0.3706250943148165, + "timestamp": 2.835353525529325 + }, + { + "x": 7.570835923468037, + "y": 3.9891303768994915, + "heading": -2.4456304849391743e-17, + "angularVelocity": 6.12426558799873e-16, + "velocityX": 0.6695721247538555, + "velocityY": 0.25990893532273207, + "timestamp": 2.917678102083068 + }, + { + "x": 7.598227500915527, + "y": 4.000265598297119, + "heading": 3.2179463374057155e-25, + "angularVelocity": 2.9707144199274434e-16, + "velocityX": 0.33272661207810256, + "velocityY": 0.13525998995380084, + "timestamp": 3.0000026786368115 + }, + { + "x": 7.598227500915527, + "y": 4.000265598297119, + "heading": 1.144163224349134e-25, + "angularVelocity": -1.1292130842830807e-24, + "velocityX": -1.2177734406642243e-20, + "velocityY": 2.8518350164068296e-20, + "timestamp": 3.0823272551905547 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneSprintBonus.1.traj b/src/main/deploy/choreo/CenterLaneSprintBonus.1.traj deleted file mode 100644 index b577b8a2..00000000 --- a/src/main/deploy/choreo/CenterLaneSprintBonus.1.traj +++ /dev/null @@ -1,499 +0,0 @@ -{ - "samples": [ - { - "x": 1.3954176902770996, - "y": 5.518980503082275, - "heading": -4.154669365501339e-31, - "angularVelocity": -1.673896254624468e-30, - "velocityX": -8.167537270046595e-18, - "velocityY": 1.3237751910388058e-17, - "timestamp": 0 - }, - { - "x": 1.4086689901208764, - "y": 5.531099909602429, - "heading": 5.4865363809901704e-18, - "angularVelocity": 8.089770653155719e-17, - "velocityX": 0.19538732001957015, - "velocityY": 0.1786978174305604, - "timestamp": 0.06782067455784145 - }, - { - "x": 1.435302968552215, - "y": 5.555193491205879, - "heading": 8.977091372122931e-18, - "angularVelocity": 5.146742470079397e-17, - "velocityX": 0.39271178891950187, - "velocityY": 0.35525423125810013, - "timestamp": 0.1356413491156829 - }, - { - "x": 1.4754682380563253, - "y": 5.591093050497596, - "heading": 1.4654590389523967e-17, - "angularVelocity": 8.37134055043427e-17, - "velocityX": 0.5922275141904324, - "velocityY": 0.5293306137953979, - "timestamp": 0.20346202367352434 - }, - { - "x": 1.5293341329115746, - "y": 5.638601649509276, - "heading": 2.0391035972700556e-17, - "angularVelocity": 8.458255853655089e-17, - "velocityX": 0.7942400338307883, - "velocityY": 0.7005031920047591, - "timestamp": 0.2712826982313658 - }, - { - "x": 1.597095145056665, - "y": 5.697485776780742, - "heading": 2.0873484642332193e-17, - "angularVelocity": 7.113597162801294e-18, - "velocityX": 0.9991202916641198, - "velocityY": 0.8682326982939436, - "timestamp": 0.33910337278920727 - }, - { - "x": 1.6789766146521206, - "y": 5.767464479151507, - "heading": 2.2177946405449375e-17, - "angularVelocity": 1.9233987436627017e-17, - "velocityX": 1.2073231375134688, - "velocityY": 1.031819615876666, - "timestamp": 0.40692404734704873 - }, - { - "x": 1.7752420754207336, - "y": 5.848193888395741, - "heading": 2.5659685154236302e-17, - "angularVelocity": 5.133742988390828e-17, - "velocityX": 1.4194117265305242, - "velocityY": 1.1903362768146415, - "timestamp": 0.4747447219048902 - }, - { - "x": 1.8862027378041668, - "y": 5.939244534569037, - "heading": 2.687644733119201e-17, - "angularVelocity": 1.7940873991424383e-17, - "velocityX": 1.6360890407953306, - "velocityY": 1.3425205037682877, - "timestamp": 0.5425653964627316 - }, - { - "x": 2.012229564399117, - "y": 6.040066954163349, - "heading": 2.5975996915224922e-17, - "angularVelocity": -1.3276929351019487e-17, - "velocityX": 1.8582361118727206, - "velocityY": 1.486603019678443, - "timestamp": 0.6103860710205731 - }, - { - "x": 2.1537679046461733, - "y": 6.149937565607546, - "heading": 2.3459157805274306e-17, - "angularVelocity": -3.711020537728529e-17, - "velocityX": 2.0869497563953514, - "velocityY": 1.6200164943876396, - "timestamp": 0.6782067455784145 - }, - { - "x": 2.311352553455493, - "y": 6.2678699868034595, - "heading": 2.1605195532052067e-17, - "angularVelocity": -2.7336238682566866e-17, - "velocityX": 2.3235488269129725, - "velocityY": 1.7388859955283122, - "timestamp": 0.746027420136256 - }, - { - "x": 2.485613520095621, - "y": 6.392464027187378, - "heading": 1.967921307517928e-17, - "angularVelocity": -2.839815790860674e-17, - "velocityX": 2.5694372368931395, - "velocityY": 1.8371100139627066, - "timestamp": 0.8138480946940975 - }, - { - "x": 2.6772369323815792, - "y": 6.5216430608077145, - "heading": 1.8045458271823762e-17, - "angularVelocity": -2.4089331717730728e-17, - "velocityX": 2.825442441191459, - "velocityY": 1.9047146679462823, - "timestamp": 0.8816687692519389 - }, - { - "x": 2.886763562184598, - "y": 6.652219019276088, - "heading": 1.683660040648187e-17, - "angularVelocity": -1.7824326418575357e-17, - "velocityX": 3.0894212003792894, - "velocityY": 1.9253119984379097, - "timestamp": 0.9494894438097804 - }, - { - "x": 3.113920279000272, - "y": 6.779380811677554, - "heading": 1.476992837613865e-17, - "angularVelocity": -3.0472596800531746e-17, - "velocityX": 3.3493727140967953, - "velocityY": 1.87497091750402, - "timestamp": 1.0173101183676219 - }, - { - "x": 3.356286094849282, - "y": 6.896994843265456, - "heading": 1.2661893348816599e-17, - "angularVelocity": -3.108248574533372e-17, - "velocityX": 3.5736273257250715, - "velocityY": 1.7341914151500553, - "timestamp": 1.0851307929254632 - }, - { - "x": 3.6089329789773132, - "y": 6.999885557033016, - "heading": 1.0426963344280672e-17, - "angularVelocity": -3.295352531869201e-17, - "velocityX": 3.725219275319372, - "velocityY": 1.5170995340042566, - "timestamp": 1.1529514674833046 - }, - { - "x": 3.866603136062622, - "y": 7.085535526275635, - "heading": 5.130524196392721e-18, - "angularVelocity": -7.809476623133778e-17, - "velocityX": 3.7992862613826874, - "velocityY": 1.262888784297902, - "timestamp": 1.220772142041146 - }, - { - "x": 4.03577288165141, - "y": 7.133931780153134, - "heading": 1.53240025945702e-18, - "angularVelocity": -8.102449650854585e-17, - "velocityX": 3.809456142072583, - "velocityY": 1.0898131101710078, - "timestamp": 1.2651799890617301 - }, - { - "x": 4.204491733807284, - "y": 7.174642072343406, - "heading": -1.8107357967918406e-18, - "angularVelocity": -7.528254097814744e-17, - "velocityX": 3.7993026790437296, - "velocityY": 0.9167364536138055, - "timestamp": 1.3095878360823143 - }, - { - "x": 4.372041951477231, - "y": 7.207742396297824, - "heading": -3.82115164020192e-18, - "angularVelocity": -4.5271625051994066e-17, - "velocityX": 3.7729867334545513, - "velocityY": 0.7453710588373391, - "timestamp": 1.3539956831028985 - }, - { - "x": 4.537853181541874, - "y": 7.2333425032674095, - "heading": -5.87160481310551e-18, - "angularVelocity": -4.617321158640688e-17, - "velocityX": 3.7338272667842607, - "velocityY": 0.5764771022950116, - "timestamp": 1.3984035301234827 - }, - { - "x": 4.701468659536594, - "y": 7.251563178434466, - "heading": -5.388451020675662e-18, - "angularVelocity": 1.0879921481471181e-17, - "velocityX": 3.684382130006832, - "velocityY": 0.410303052039688, - "timestamp": 1.442811377144067 - }, - { - "x": 4.8625183641051555, - "y": 7.262524779655412, - "heading": -5.872012470604977e-18, - "angularVelocity": -1.0889098042063212e-17, - "velocityX": 3.626604651513686, - "velocityY": 0.24683928531516408, - "timestamp": 1.4872192241646511 - }, - { - "x": 5.020698725587354, - "y": 7.266341856818374, - "heading": -7.150563317225696e-18, - "angularVelocity": -2.879110223357202e-17, - "velocityX": 3.561991226660372, - "velocityY": 0.08595501513940547, - "timestamp": 1.5316270711852353 - }, - { - "x": 5.175757540528759, - "y": 7.263120943781581, - "heading": -8.096150179100186e-18, - "angularVelocity": -2.1293241253900183e-17, - "velocityX": 3.4916985475456466, - "velocityY": -0.07253026779929446, - "timestamp": 1.5760349182058195 - }, - { - "x": 5.327482769552672, - "y": 7.252959960990395, - "heading": -8.592025684797133e-18, - "angularVelocity": -1.116639798884869e-17, - "velocityX": 3.41663105066957, - "velocityY": -0.22881052500644172, - "timestamp": 1.6204427652264037 - }, - { - "x": 5.475694166399093, - "y": 7.235948405191675, - "heading": -8.833226442519028e-18, - "angularVelocity": -5.431493574531005e-18, - "velocityX": 3.3375046706884697, - "velocityY": -0.38307544589665543, - "timestamp": 1.664850612246988 - }, - { - "x": 5.620236960766286, - "y": 7.212167896274621, - "heading": -9.572307077519199e-18, - "angularVelocity": -1.6643020598536764e-17, - "velocityX": 3.254893089056846, - "velocityY": -0.5355024058255178, - "timestamp": 1.7092584592675721 - }, - { - "x": 5.7609770366577315, - "y": 7.1816928593209, - "heading": -1.1397932886275862e-17, - "angularVelocity": -4.111043115967244e-17, - "velocityX": 3.169261410628764, - "velocityY": -0.6862534213738285, - "timestamp": 1.7536663062881563 - }, - { - "x": 5.897797208582785, - "y": 7.144591229805976, - "heading": -1.3242662306872499e-17, - "angularVelocity": -4.154061444960465e-17, - "velocityX": 3.080990885724137, - "velocityY": -0.8354746290160561, - "timestamp": 1.7980741533087405 - }, - { - "x": 6.030594311960715, - "y": 7.100925127890339, - "heading": -1.2969314838489408e-17, - "angularVelocity": 6.1553878370471014e-18, - "velocityX": 2.990397244801673, - "velocityY": -0.9832969811708759, - "timestamp": 1.8424820003293247 - }, - { - "x": 6.159276904080374, - "y": 7.050751478229476, - "heading": -1.2712775294894754e-17, - "angularVelocity": 5.77689752908346e-18, - "velocityX": 2.8977444472822667, - "velocityY": -1.1298374730395475, - "timestamp": 1.886889847349909 - }, - { - "x": 6.283763428080075, - "y": 6.994122567551955, - "heading": -1.1520483238842908e-17, - "angularVelocity": 2.6848678729359197e-17, - "velocityX": 2.8032551080892625, - "velocityY": -1.27520054397759, - "timestamp": 1.9312976943704931 - }, - { - "x": 6.403980731964111, - "y": 6.931086540222168, - "heading": -1.0278212817380173e-17, - "angularVelocity": 2.797411882250772e-17, - "velocityX": 2.707118492556311, - "velocityY": -1.4194794739895686, - "timestamp": 1.9757055413910773 - }, - { - "x": 6.60729389364524, - "y": 6.796210534013358, - "heading": -8.076224683373957e-18, - "angularVelocity": 2.7406592243829792e-17, - "velocityX": 2.5304956120267805, - "velocityY": -1.6787065778573356, - "timestamp": 2.0560507357138165 - }, - { - "x": 6.795704830022524, - "y": 6.641009825084134, - "heading": -5.924369506882037e-18, - "angularVelocity": 2.6782622499519642e-17, - "velocityX": 2.3450181179530984, - "velocityY": -1.9316738261382134, - "timestamp": 2.1363959300365556 - }, - { - "x": 6.968323808401379, - "y": 6.4661677340473895, - "heading": 1.813427886027723e-18, - "angularVelocity": 9.630691182046325e-17, - "velocityX": 2.148466748184869, - "velocityY": -2.176136264409528, - "timestamp": 2.2167411243592947 - }, - { - "x": 7.12401657856057, - "y": 6.272652848277024, - "heading": 5.514219590575789e-18, - "angularVelocity": 4.606114294192909e-17, - "velocityX": 1.9377981654234078, - "velocityY": -2.4085433783759767, - "timestamp": 2.297086318682034 - }, - { - "x": 7.261313462923536, - "y": 6.061911206598393, - "heading": 9.2916222876539e-18, - "angularVelocity": 4.7014669947754546e-17, - "velocityX": 1.7088375418131223, - "velocityY": -2.622952666367338, - "timestamp": 2.377431513004773 - }, - { - "x": 7.378307598032396, - "y": 5.836237450824618, - "heading": 1.3317359546362108e-17, - "angularVelocity": 5.010551054488734e-17, - "velocityX": 1.4561435328528385, - "velocityY": -2.8088021651582906, - "timestamp": 2.457776707327512 - }, - { - "x": 7.472656845707877, - "y": 5.59950120754036, - "heading": 1.7242988626530655e-17, - "angularVelocity": 4.8859538047431847e-17, - "velocityX": 1.1742985809019115, - "velocityY": -2.946489149473078, - "timestamp": 2.538121901650251 - }, - { - "x": 7.542192569709091, - "y": 5.358353787993265, - "heading": 1.7116315403461932e-17, - "angularVelocity": -1.5766120825832005e-18, - "velocityX": 0.8654621422893939, - "velocityY": -3.001391951065907, - "timestamp": 2.6184670959729903 - }, - { - "x": 7.5871412458956256, - "y": 5.1227422896934245, - "heading": 1.6858435017814408e-17, - "angularVelocity": -3.209652588562192e-18, - "velocityX": 0.5594444890627913, - "velocityY": -2.932490241462491, - "timestamp": 2.6988122902957294 - }, - { - "x": 7.612106015705454, - "y": 4.902486990914375, - "heading": 1.5879880365887054e-17, - "angularVelocity": -1.2179381518025204e-17, - "velocityX": 0.3107188926514608, - "velocityY": -2.741362450307933, - "timestamp": 2.7791574846184686 - }, - { - "x": 7.623350532741401, - "y": 4.703372273008907, - "heading": 1.7045873071267902e-17, - "angularVelocity": 1.45122904961207e-17, - "velocityX": 0.13995257750920653, - "velocityY": -2.4782405417509445, - "timestamp": 2.8595026789412077 - }, - { - "x": 7.625915530957838, - "y": 4.527918399643226, - "heading": 1.7155064370003262e-17, - "angularVelocity": 1.3590256508962654e-18, - "velocityX": 0.03192472478357754, - "velocityY": -2.1837506878246002, - "timestamp": 2.939847873263947 - }, - { - "x": 7.6233153599672425, - "y": 4.377132131065689, - "heading": 1.6703747348721444e-17, - "angularVelocity": -5.617224306630994e-18, - "velocityX": -0.032362495510961514, - "velocityY": -1.8767303987322825, - "timestamp": 3.020193067586686 - }, - { - "x": 7.6179855647139085, - "y": 4.251400181756933, - "heading": 1.5614701617946926e-17, - "angularVelocity": -1.355458489040667e-17, - "velocityX": -0.06633620465122089, - "velocityY": -1.5648969470868832, - "timestamp": 3.100538261909425 - }, - { - "x": 7.611666193625904, - "y": 4.150851372626415, - "heading": 1.448138270462699e-17, - "angularVelocity": -1.4105621493743314e-17, - "velocityX": -0.07865275753296536, - "velocityY": -1.251460152384776, - "timestamp": 3.180883456232164 - }, - { - "x": 7.605645305191298, - "y": 4.075503372077343, - "heading": 1.0097210956722963e-17, - "angularVelocity": -5.456669753373492e-17, - "velocityX": -0.07493775434062062, - "velocityY": -0.9378034515220308, - "timestamp": 3.2612286505549033 - }, - { - "x": 7.600907710154459, - "y": 4.0253252549341, - "heading": 5.681220340527481e-18, - "angularVelocity": -5.496272423373191e-17, - "velocityX": -0.058965505986702256, - "velocityY": -0.624531654521636, - "timestamp": 3.3415738448776424 - }, - { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -1.0462507114010796e-31, - "angularVelocity": -7.071014853118142e-17, - "velocityX": -0.03335867517061736, - "velocityY": -0.3118998821051884, - "timestamp": 3.4219190392003815 - }, - { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -7.6740532227992184e-31, - "angularVelocity": -3.211824120166074e-30, - "velocityX": 1.9649506717636078e-19, - "velocityY": 1.3301278158953467e-20, - "timestamp": 3.5022642335231207 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneSprintBonus.2.traj b/src/main/deploy/choreo/CenterLaneSprintBonus.2.traj deleted file mode 100644 index 7eec2248..00000000 --- a/src/main/deploy/choreo/CenterLaneSprintBonus.2.traj +++ /dev/null @@ -1,391 +0,0 @@ -{ - "samples": [ - { - "x": 7.598227500915527, - "y": 4.000265598297119, - "heading": -7.6740532227992184e-31, - "angularVelocity": -3.211824120166074e-30, - "velocityX": 1.9649506717636078e-19, - "velocityY": 1.3301278158953467e-20, - "timestamp": 0 - }, - { - "x": 7.601080260023373, - "y": 4.024833528540064, - "heading": -2.306467806437822e-19, - "angularVelocity": -2.8978205760100555e-18, - "velocityX": 0.035841753611820425, - "velocityY": 0.308668790189073, - "timestamp": 0.07959317891483986 - }, - { - "x": 7.6061588306375265, - "y": 4.074034103309106, - "heading": -6.659912056470629e-19, - "angularVelocity": -5.469619135125979e-18, - "velocityX": 0.06380660608606212, - "velocityY": 0.6181506435581036, - "timestamp": 0.15918635782967971 - }, - { - "x": 7.612681282533822, - "y": 4.147925498197174, - "heading": -1.328067367399e-18, - "angularVelocity": -8.318252856515268e-18, - "velocityX": 0.08194737269231386, - "velocityY": 0.9283634087178491, - "timestamp": 0.23877953674451957 - }, - { - "x": 7.619648669627935, - "y": 4.2465458921824055, - "heading": -2.2445697547086026e-18, - "angularVelocity": -1.151483703155595e-17, - "velocityX": 0.0875374898843103, - "velocityY": 1.2390558503857954, - "timestamp": 0.31837271565935943 - }, - { - "x": 7.625747861911206, - "y": 4.369884044272492, - "heading": -3.6731106086224716e-18, - "angularVelocity": -1.7948034238446692e-17, - "velocityX": 0.07662958517843273, - "velocityY": 1.5496070614550481, - "timestamp": 0.3979658945741993 - }, - { - "x": 7.629194369652072, - "y": 4.517812530748991, - "heading": -5.438708340419596e-18, - "angularVelocity": -2.2182783637629958e-17, - "velocityX": 0.04330154653780812, - "velocityY": 1.8585573348537896, - "timestamp": 0.47755907348903914 - }, - { - "x": 7.627474126240345, - "y": 4.689928325847814, - "heading": -7.604279409389531e-18, - "angularVelocity": -2.7208006214780384e-17, - "velocityX": -0.021612950194643966, - "velocityY": 2.1624440366049984, - "timestamp": 0.557152252403879 - }, - { - "x": 7.616940876905995, - "y": 4.885153687224004, - "heading": -1.141400124068985e-17, - "angularVelocity": -4.7864941206348986e-17, - "velocityX": -0.1323385933059981, - "velocityY": 2.452790101336185, - "timestamp": 0.6367454313187184 - }, - { - "x": 7.592385072354184, - "y": 5.10075278062736, - "heading": -1.6698122492004076e-17, - "angularVelocity": -6.638915655253322e-17, - "velocityX": -0.3085164443310098, - "velocityY": 2.708763443586594, - "timestamp": 0.7163386102335578 - }, - { - "x": 7.547559201834021, - "y": 5.330523864977108, - "heading": -1.75098708974119e-17, - "angularVelocity": -1.0198737275031467e-17, - "velocityX": -0.5631873375496935, - "velocityY": 2.88681878877579, - "timestamp": 0.7959317891483972 - }, - { - "x": 7.478361687755516, - "y": 5.564507297546682, - "heading": -1.8964828080994495e-17, - "angularVelocity": -1.827994481974364e-17, - "velocityX": -0.8693900032884319, - "velocityY": 2.93974227138131, - "timestamp": 0.8755249680632367 - }, - { - "x": 7.384986158389392, - "y": 5.79328068583031, - "heading": -2.0967248801545294e-17, - "angularVelocity": -2.515821870147438e-17, - "velocityX": -1.173159944598191, - "velocityY": 2.8742838444534518, - "timestamp": 0.9551181469780761 - }, - { - "x": 7.269611510863885, - "y": 6.010750981441168, - "heading": -2.341183721366507e-17, - "angularVelocity": -3.071356740399464e-17, - "velocityX": -1.4495544605518746, - "velocityY": 2.732273023590656, - "timestamp": 1.0347113258929155 - }, - { - "x": 7.134451807687368, - "y": 6.213379687795212, - "heading": -1.8377494488650744e-17, - "angularVelocity": 6.325092910440118e-17, - "velocityX": -1.6981317371571643, - "velocityY": 2.545804918419267, - "timestamp": 1.114304504807755 - }, - { - "x": 6.981289024355567, - "y": 6.399049314780469, - "heading": -1.3636102854748774e-17, - "angularVelocity": 5.957033241098293e-17, - "velocityX": -1.9243204684119384, - "velocityY": 2.332732898932091, - "timestamp": 1.1938976837225943 - }, - { - "x": 6.8114962888064765, - "y": 6.566411402116164, - "heading": -1.0217857426728479e-17, - "angularVelocity": 4.294646070658522e-17, - "velocityX": -2.1332573703427147, - "velocityY": 2.1027189718699257, - "timestamp": 1.2734908626374337 - }, - { - "x": 6.626136187340535, - "y": 6.714554267951228, - "heading": 1.5840075957918408e-18, - "angularVelocity": 1.4827738452159993e-16, - "velocityX": -2.328844054140172, - "velocityY": 1.8612507736817756, - "timestamp": 1.3530840415522731 - }, - { - "x": 6.426044940948486, - "y": 6.842829704284668, - "heading": 1.3036775744196506e-17, - "angularVelocity": 1.4389137918114227e-16, - "velocityX": -2.513924548812549, - "velocityY": 1.6116385610211605, - "timestamp": 1.4326772204671125 - }, - { - "x": 6.196280073025626, - "y": 6.956629849882612, - "heading": 2.0458443333295487e-17, - "angularVelocity": 8.729748510308141e-17, - "velocityX": -2.702613571061956, - "velocityY": 1.338576348343764, - "timestamp": 1.5176930253098964 - }, - { - "x": 5.950838280027335, - "y": 7.046967618713607, - "heading": 2.793959259584985e-17, - "angularVelocity": 8.799713979563742e-17, - "velocityX": -2.887013696478903, - "velocityY": 1.0625997012916502, - "timestamp": 1.6027088301526802 - }, - { - "x": 5.6901710071441105, - "y": 7.113547571189235, - "heading": 3.9506385392959274e-17, - "angularVelocity": 1.360545935414649e-16, - "velocityX": -3.0661036893700855, - "velocityY": 0.783147940536072, - "timestamp": 1.687724634995464 - }, - { - "x": 5.414850837059096, - "y": 7.156012349089107, - "heading": 4.700202282082566e-17, - "angularVelocity": 8.816755094375023e-17, - "velocityX": -3.238458667704845, - "velocityY": 0.4994927470064634, - "timestamp": 1.7727404398382478 - }, - { - "x": 5.12562598325124, - "y": 7.173922926375987, - "heading": 5.0881573025403224e-17, - "angularVelocity": 4.563326938276916e-17, - "velocityX": -3.402012770951351, - "velocityY": 0.21067350147440403, - "timestamp": 1.8577562446810316 - }, - { - "x": 4.82351161445262, - "y": 7.166731524808041, - "heading": 5.499719430693057e-17, - "angularVelocity": 4.841005753815757e-17, - "velocityX": -3.5536259329345636, - "velocityY": -0.0845889959076519, - "timestamp": 1.9427720495238154 - }, - { - "x": 4.5099531855637025, - "y": 7.1337470554057925, - "heading": 6.306907946999819e-17, - "angularVelocity": 9.494568681598964e-17, - "velocityX": -3.688236904523418, - "velocityY": -0.387980440380867, - "timestamp": 2.0277878543665993 - }, - { - "x": 4.187141754472217, - "y": 7.074104928642276, - "heading": 6.708348504544881e-17, - "angularVelocity": 4.721950773543392e-17, - "velocityX": -3.797075516587084, - "velocityY": -0.7015416353912106, - "timestamp": 2.112803659209383 - }, - { - "x": 3.8586724966260957, - "y": 6.986817954786897, - "heading": 7.185623411631233e-17, - "angularVelocity": 5.613953431429229e-17, - "velocityX": -3.8636258099718415, - "velocityY": -1.0267146681348074, - "timestamp": 2.197819464052167 - }, - { - "x": 3.5309501359202713, - "y": 6.871322995246275, - "heading": 7.621484395675117e-17, - "angularVelocity": 5.126820454622351e-17, - "velocityX": -3.8548404183416, - "velocityY": -1.3585116291517334, - "timestamp": 2.2828352688949507 - }, - { - "x": 3.2151594653612365, - "y": 6.73025690445887, - "heading": 7.966221541391077e-17, - "angularVelocity": 4.054976338417916e-17, - "velocityX": -3.714493689061746, - "velocityY": -1.6592925403490721, - "timestamp": 2.3678510737377345 - }, - { - "x": 2.9234333026279216, - "y": 6.574454892414151, - "heading": 7.863320324730858e-17, - "angularVelocity": -1.2103771141102572e-17, - "velocityX": -3.4314344641303904, - "velocityY": -1.83262409069529, - "timestamp": 2.4528668785805183 - }, - { - "x": 2.659851278869089, - "y": 6.416614338182214, - "heading": 7.578110084515079e-17, - "angularVelocity": -3.354789840476959e-17, - "velocityX": -3.1003885012470134, - "velocityY": -1.856602481430881, - "timestamp": 2.537882683423302 - }, - { - "x": 2.4239206377137292, - "y": 6.264399576901125, - "heading": 7.251198491757422e-17, - "angularVelocity": -3.845302447267169e-17, - "velocityX": -2.775138594425478, - "velocityY": -1.7904289862642684, - "timestamp": 2.622898488266086 - }, - { - "x": 2.2144464260119667, - "y": 6.121998379809068, - "heading": 6.761005309686019e-17, - "angularVelocity": -5.765904670925165e-17, - "velocityX": -2.4639443464556736, - "velocityY": -1.6749967533143706, - "timestamp": 2.7079142931088698 - }, - { - "x": 2.030377404257279, - "y": 5.9918782719805765, - "heading": 5.994279183003849e-17, - "angularVelocity": -9.018628912533282e-17, - "velocityX": -2.1651153229105353, - "velocityY": -1.530540210365924, - "timestamp": 2.7929300979516536 - }, - { - "x": 1.8708849952976385, - "y": 5.875623192871665, - "heading": 4.994168872232487e-17, - "angularVelocity": -1.176381435863843e-16, - "velocityX": -1.8760324536664574, - "velocityY": -1.3674525498394738, - "timestamp": 2.8779459027944374 - }, - { - "x": 1.735321954953519, - "y": 5.774322521108853, - "heading": 3.80276915684229e-17, - "angularVelocity": -1.4013859419532884e-16, - "velocityX": -1.5945628062312347, - "velocityY": -1.1915510527736544, - "timestamp": 2.962961707637221 - }, - { - "x": 1.6231760146266, - "y": 5.6887663231669565, - "heading": 2.9027956357036794e-17, - "angularVelocity": -1.0585954002347058e-16, - "velocityX": -1.3191187277977714, - "velocityY": -1.0063563839702132, - "timestamp": 3.047977512480005 - }, - { - "x": 1.5340345289934116, - "y": 5.619551610175038, - "heading": 2.339141109649791e-17, - "angularVelocity": -6.629995787223342e-17, - "velocityX": -1.048528397725959, - "velocityY": -0.8141393605567464, - "timestamp": 3.132993317322789 - }, - { - "x": 1.467559291615285, - "y": 5.567144344510584, - "heading": 1.436014992750064e-17, - "angularVelocity": -1.0623036243813761e-16, - "velocityX": -0.7819162272362828, - "velocityY": -0.6164414459331455, - "timestamp": 3.2180091221655727 - }, - { - "x": 1.4234687066815632, - "y": 5.531917778907625, - "heading": 5.2334973544914086e-18, - "angularVelocity": -1.0735240881555591e-16, - "velocityX": -0.5186163327543208, - "velocityY": -0.414353139020498, - "timestamp": 3.3030249270083565 - }, - { - "x": 1.4015250205993652, - "y": 5.514177322387695, - "heading": 1.924648421034324e-30, - "angularVelocity": -6.15591051597521e-17, - "velocityX": -0.2581130193706609, - "velocityY": -0.20867245276024773, - "timestamp": 3.3880407318511403 - }, - { - "x": 1.4015250205993652, - "y": 5.514177322387695, - "heading": 1.0779182942243083e-30, - "angularVelocity": 2.7193521636113197e-30, - "velocityX": -1.8832789283152474e-18, - "velocityY": 1.3879124381036463e-18, - "timestamp": 3.473056536693924 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneTaxi.1.traj b/src/main/deploy/choreo/CenterLaneTaxi.1.traj deleted file mode 100644 index cf8ad366..00000000 --- a/src/main/deploy/choreo/CenterLaneTaxi.1.traj +++ /dev/null @@ -1,139 +0,0 @@ -{ - "samples": [ - { - "x": 1.44908344745636, - "y": 5.489123821258545, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -2.0318283346913183e-41, - "timestamp": 0 - }, - { - "x": 1.4872695756258638, - "y": 5.489123821258545, - "heading": 2.754950983259061e-24, - "angularVelocity": 2.9447241753048086e-23, - "velocityX": 0.40816653670481284, - "velocityY": 5.876249720718061e-34, - "timestamp": 0.09355526417669109 - }, - { - "x": 1.5636418311637772, - "y": 5.489123821258545, - "heading": 4.052554305374892e-24, - "angularVelocity": 1.3869873668873725e-23, - "velocityX": 0.8163330648468328, - "velocityY": 2.351179297170128e-34, - "timestamp": 0.18711052835338218 - }, - { - "x": 1.6782002129129638, - "y": 5.489123821258545, - "heading": -2.072394992605464e-24, - "angularVelocity": -6.546869835666046e-23, - "velocityX": 1.2244995806203742, - "velocityY": -1.9003523177361593e-34, - "timestamp": 0.2806657925300733 - }, - { - "x": 1.8309447190550667, - "y": 5.489123821258545, - "heading": -2.4578798145232008e-23, - "angularVelocity": -2.4056824153148397e-22, - "velocityX": 1.632666076957736, - "velocityY": 3.860759321784845e-34, - "timestamp": 0.37422105670676437 - }, - { - "x": 2.0218753463170436, - "y": 5.489123821258545, - "heading": -7.973182418935717e-23, - "angularVelocity": -5.895236107850817e-22, - "velocityX": 2.0408325383099775, - "velocityY": 1.2704796254278893e-34, - "timestamp": 0.46777632088345544 - }, - { - "x": 2.250992087061798, - "y": 5.489123821258545, - "heading": -2.0707507158653019e-22, - "angularVelocity": -1.3611552826942191e-21, - "velocityX": 2.4489989180302882, - "velocityY": -1.7750165055103215e-35, - "timestamp": 0.5613315850601466 - }, - { - "x": 2.5182949031039126, - "y": 5.489123821258545, - "heading": -2.160490651108523e-22, - "angularVelocity": -9.592193197806397e-23, - "velocityX": 2.8571648895916577, - "velocityY": 2.8715559113727715e-34, - "timestamp": 0.6548868492368377 - }, - { - "x": 2.747411643848667, - "y": 5.489123821258545, - "heading": -1.6667699647956365e-22, - "angularVelocity": 5.277318832756414e-22, - "velocityX": 2.448998918030288, - "velocityY": 2.947986532686387e-35, - "timestamp": 0.7484421134135288 - }, - { - "x": 2.938342271110644, - "y": 5.489123821258545, - "heading": -1.1298656729823459e-22, - "angularVelocity": 5.738899818074355e-22, - "velocityX": 2.0408325383099775, - "velocityY": -2.8596027354894936e-34, - "timestamp": 0.84199737759022 - }, - { - "x": 3.0910867772527473, - "y": 5.489123821258545, - "heading": -6.615705102278205e-23, - "angularVelocity": 5.00554598927309e-22, - "velocityX": 1.632666076957736, - "velocityY": 3.1581487960600027e-34, - "timestamp": 0.9355526417669111 - }, - { - "x": 3.2056451590019335, - "y": 5.489123821258545, - "heading": -3.1148929262698523e-23, - "angularVelocity": 3.7419731471853073e-22, - "velocityX": 1.2244995806203742, - "velocityY": 5.822081879901906e-34, - "timestamp": 1.0291079059436021 - }, - { - "x": 3.2820174145398466, - "y": 5.489123821258545, - "heading": -1.119480593141529e-23, - "angularVelocity": 2.1328705390671606e-22, - "velocityX": 0.8163330648468328, - "velocityY": 1.1078164340378654e-33, - "timestamp": 1.1226631701202932 - }, - { - "x": 3.3202035427093506, - "y": 5.489123821258545, - "heading": 3.079038293636101e-40, - "angularVelocity": 1.1965981728795564e-22, - "velocityX": 0.40816653670481284, - "velocityY": 4.557991704503881e-34, - "timestamp": 1.2162184342969842 - }, - { - "x": 3.3202035427093506, - "y": 5.489123821258545, - "heading": 1.1605365063908143e-41, - "angularVelocity": -3.167017137026152e-39, - "velocityX": -4.508017624963005e-40, - "velocityY": -1.70250903211491e-50, - "timestamp": 1.3097736984736752 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLaneTaxi.traj b/src/main/deploy/choreo/CenterLaneTaxi.traj deleted file mode 100644 index cf8ad366..00000000 --- a/src/main/deploy/choreo/CenterLaneTaxi.traj +++ /dev/null @@ -1,139 +0,0 @@ -{ - "samples": [ - { - "x": 1.44908344745636, - "y": 5.489123821258545, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -2.0318283346913183e-41, - "timestamp": 0 - }, - { - "x": 1.4872695756258638, - "y": 5.489123821258545, - "heading": 2.754950983259061e-24, - "angularVelocity": 2.9447241753048086e-23, - "velocityX": 0.40816653670481284, - "velocityY": 5.876249720718061e-34, - "timestamp": 0.09355526417669109 - }, - { - "x": 1.5636418311637772, - "y": 5.489123821258545, - "heading": 4.052554305374892e-24, - "angularVelocity": 1.3869873668873725e-23, - "velocityX": 0.8163330648468328, - "velocityY": 2.351179297170128e-34, - "timestamp": 0.18711052835338218 - }, - { - "x": 1.6782002129129638, - "y": 5.489123821258545, - "heading": -2.072394992605464e-24, - "angularVelocity": -6.546869835666046e-23, - "velocityX": 1.2244995806203742, - "velocityY": -1.9003523177361593e-34, - "timestamp": 0.2806657925300733 - }, - { - "x": 1.8309447190550667, - "y": 5.489123821258545, - "heading": -2.4578798145232008e-23, - "angularVelocity": -2.4056824153148397e-22, - "velocityX": 1.632666076957736, - "velocityY": 3.860759321784845e-34, - "timestamp": 0.37422105670676437 - }, - { - "x": 2.0218753463170436, - "y": 5.489123821258545, - "heading": -7.973182418935717e-23, - "angularVelocity": -5.895236107850817e-22, - "velocityX": 2.0408325383099775, - "velocityY": 1.2704796254278893e-34, - "timestamp": 0.46777632088345544 - }, - { - "x": 2.250992087061798, - "y": 5.489123821258545, - "heading": -2.0707507158653019e-22, - "angularVelocity": -1.3611552826942191e-21, - "velocityX": 2.4489989180302882, - "velocityY": -1.7750165055103215e-35, - "timestamp": 0.5613315850601466 - }, - { - "x": 2.5182949031039126, - "y": 5.489123821258545, - "heading": -2.160490651108523e-22, - "angularVelocity": -9.592193197806397e-23, - "velocityX": 2.8571648895916577, - "velocityY": 2.8715559113727715e-34, - "timestamp": 0.6548868492368377 - }, - { - "x": 2.747411643848667, - "y": 5.489123821258545, - "heading": -1.6667699647956365e-22, - "angularVelocity": 5.277318832756414e-22, - "velocityX": 2.448998918030288, - "velocityY": 2.947986532686387e-35, - "timestamp": 0.7484421134135288 - }, - { - "x": 2.938342271110644, - "y": 5.489123821258545, - "heading": -1.1298656729823459e-22, - "angularVelocity": 5.738899818074355e-22, - "velocityX": 2.0408325383099775, - "velocityY": -2.8596027354894936e-34, - "timestamp": 0.84199737759022 - }, - { - "x": 3.0910867772527473, - "y": 5.489123821258545, - "heading": -6.615705102278205e-23, - "angularVelocity": 5.00554598927309e-22, - "velocityX": 1.632666076957736, - "velocityY": 3.1581487960600027e-34, - "timestamp": 0.9355526417669111 - }, - { - "x": 3.2056451590019335, - "y": 5.489123821258545, - "heading": -3.1148929262698523e-23, - "angularVelocity": 3.7419731471853073e-22, - "velocityX": 1.2244995806203742, - "velocityY": 5.822081879901906e-34, - "timestamp": 1.0291079059436021 - }, - { - "x": 3.2820174145398466, - "y": 5.489123821258545, - "heading": -1.119480593141529e-23, - "angularVelocity": 2.1328705390671606e-22, - "velocityX": 0.8163330648468328, - "velocityY": 1.1078164340378654e-33, - "timestamp": 1.1226631701202932 - }, - { - "x": 3.3202035427093506, - "y": 5.489123821258545, - "heading": 3.079038293636101e-40, - "angularVelocity": 1.1965981728795564e-22, - "velocityX": 0.40816653670481284, - "velocityY": 4.557991704503881e-34, - "timestamp": 1.2162184342969842 - }, - { - "x": 3.3202035427093506, - "y": 5.489123821258545, - "heading": 1.1605365063908143e-41, - "angularVelocity": -3.167017137026152e-39, - "velocityX": -4.508017624963005e-40, - "velocityY": -1.70250903211491e-50, - "timestamp": 1.3097736984736752 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/New Path (1).1.traj b/src/main/deploy/choreo/New Path (1).1.traj deleted file mode 100644 index 71f93ae5..00000000 --- a/src/main/deploy/choreo/New Path (1).1.traj +++ /dev/null @@ -1,373 +0,0 @@ -{ - "samples": [ - { - "x": 0.6769854426383971, - "y": 6.664690017700195, - "heading": 0.9928942822119466, - "angularVelocity": -5.3547730079964474e-15, - "velocityX": 1.777659960068706e-13, - "velocityY": 2.5367271903138425e-14, - "timestamp": 0 - }, - { - "x": 0.6967203266965981, - "y": 6.667499765770204, - "heading": 0.9828991063244013, - "angularVelocity": -0.13936372596521804, - "velocityX": 0.2751654402864449, - "velocityY": 0.039176595236385496, - "timestamp": 0.07172006788957132 - }, - { - "x": 0.736189875765086, - "y": 6.673119555664161, - "heading": 0.962913929932916, - "angularVelocity": -0.2786552910444675, - "velocityX": 0.5503278263661738, - "velocityY": 0.07835728631299656, - "timestamp": 0.14344013577914264 - }, - { - "x": 0.7953940363099642, - "y": 6.68154997032341, - "heading": 0.9329558173157279, - "angularVelocity": -0.41770892720413644, - "velocityX": 0.8254894660175004, - "velocityY": 0.11754610539842206, - "timestamp": 0.21516020366871397 - }, - { - "x": 0.8743328535848786, - "y": 6.692791927925672, - "heading": 0.8930510076798603, - "angularVelocity": -0.556396707504797, - "velocityX": 1.1006517366448958, - "velocityY": 0.15674772672662765, - "timestamp": 0.28688027155828527 - }, - { - "x": 0.9730063811921398, - "y": 6.706846714905386, - "heading": 0.8432310042825357, - "angularVelocity": -0.6946452347739902, - "velocityX": 1.3758147546546173, - "velocityY": 0.19596728493742963, - "timestamp": 0.3586003394478566 - }, - { - "x": 1.0914145748709998, - "y": 6.723715985001068, - "heading": 0.7835276099569957, - "angularVelocity": -0.8324503319937329, - "velocityX": 1.650977155532356, - "velocityY": 0.23520990138791184, - "timestamp": 0.4303204073374279 - }, - { - "x": 1.2295571875726545, - "y": 6.743401704195523, - "heading": 0.7139674415159935, - "angularVelocity": -0.9698843083607023, - "velocityX": 1.9261361117841878, - "velocityY": 0.2744799297310955, - "timestamp": 0.5020404752269992 - }, - { - "x": 1.3874336932373434, - "y": 6.765906031767598, - "heading": 0.6345669689838316, - "angularVelocity": -1.1070886415499044, - "velocityX": 2.2012877331327485, - "velocityY": 0.31378006511279205, - "timestamp": 0.5737605431165705 - }, - { - "x": 1.5650432732397428, - "y": 6.791231155876165, - "heading": 0.5453296116241021, - "angularVelocity": -1.2442452996087745, - "velocityX": 2.4764279403074436, - "velocityY": 0.3531107102100525, - "timestamp": 0.6454806110061418 - }, - { - "x": 1.7623848877857018, - "y": 6.819379197530148, - "heading": 0.4462466633333773, - "angularVelocity": -1.3815233477289681, - "velocityX": 2.751553649529441, - "velocityY": 0.3924709287445149, - "timestamp": 0.7172006788957132 - }, - { - "x": 1.9400745450841772, - "y": 6.8448322679157085, - "heading": 0.35718421546109663, - "angularVelocity": -1.2418065193344912, - "velocityX": 2.4775444659639825, - "velocityY": 0.35489467779856054, - "timestamp": 0.7889207467852845 - }, - { - "x": 2.098033534005446, - "y": 6.867461086203729, - "heading": 0.27799286687198344, - "angularVelocity": -1.1041728057357585, - "velocityX": 2.202437805334493, - "velocityY": 0.31551585147236794, - "timestamp": 0.8606408146748558 - }, - { - "x": 2.23626220483818, - "y": 6.887264797429327, - "heading": 0.20867854151035828, - "angularVelocity": -0.966456493995732, - "velocityX": 1.9273360288146049, - "velocityY": 0.2761251042862373, - "timestamp": 0.9323608825644271 - }, - { - "x": 2.3547607178176464, - "y": 6.904242831122056, - "heading": 0.14924336776360744, - "angularVelocity": -0.828710505940115, - "velocityX": 1.6522364864708916, - "velocityY": 0.23672640297420225, - "timestamp": 1.0040809504539985 - }, - { - "x": 2.4535291063549773, - "y": 6.918394729540127, - "heading": 0.09968662041578268, - "angularVelocity": -0.6909746296424073, - "velocityX": 1.3771374099819131, - "velocityY": 0.19732131932313765, - "timestamp": 1.0758010183435698 - }, - { - "x": 2.5325673341580632, - "y": 6.9297201080760455, - "heading": 0.06000605576339949, - "angularVelocity": -0.5532700375244937, - "velocityX": 1.1020378274684377, - "velocityY": 0.1579108730516656, - "timestamp": 1.147521086233141 - }, - { - "x": 2.591875338416447, - "y": 6.938218655413305, - "heading": 0.03019902804734761, - "angularVelocity": -0.41560233548771447, - "velocityX": 0.8269373691825658, - "velocityY": 0.11849608606337403, - "timestamp": 1.2192411541227124 - }, - { - "x": 2.631453058021563, - "y": 6.943890147754217, - "heading": 0.010263230834592196, - "angularVelocity": -0.2779667922750285, - "velocityX": 0.5518360588539131, - "velocityY": 0.07907817864172664, - "timestamp": 1.2909612220122837 - }, - { - "x": 2.6513004494759884, - "y": 6.946734463961241, - "heading": 0.0001971035258016319, - "angularVelocity": -0.14035300865333888, - "velocityX": 0.2767341420363792, - "velocityY": 0.03965858218734756, - "timestamp": 1.362681289901855 - }, - { - "x": 2.6514174938201904, - "y": 6.946751594543457, - "heading": 3.5288237270675047e-17, - "angularVelocity": -0.002748233960481834, - "velocityX": 0.0016319608642038723, - "velocityY": 0.00023885340011574744, - "timestamp": 1.4344013577914263 - }, - { - "x": 2.6317801098986524, - "y": 6.943938190404806, - "heading": 0.009684154332091924, - "angularVelocity": 0.13494453162004436, - "velocityX": -0.27363851137312334, - "velocityY": -0.03920357841114835, - "timestamp": 1.5061653204835006 - }, - { - "x": 2.5923883043342277, - "y": 6.938294461304913, - "heading": 0.029252560696534473, - "angularVelocity": 0.27267733875192557, - "velocityX": -0.5489078931331371, - "velocityY": -0.07864294122038361, - "timestamp": 1.577929283175575 - }, - { - "x": 2.5332421534292355, - "y": 6.929820712084714, - "heading": 0.0587082954705244, - "angularVelocity": 0.4104530138790504, - "velocityX": -0.8241762116530991, - "velocityY": -0.11807805620208979, - "timestamp": 1.6496932458676492 - }, - { - "x": 2.4543417024167087, - "y": 6.918517314710032, - "heading": 0.09805385643909013, - "angularVelocity": 0.5482634945680552, - "velocityX": -1.0994438998701832, - "velocityY": -0.15750798799974947, - "timestamp": 1.7214572085597235 - }, - { - "x": 2.355686921634183, - "y": 6.904384700424061, - "heading": 0.1472900028023496, - "angularVelocity": 0.6860845543848878, - "velocityX": -1.3747120014209526, - "velocityY": -0.19693191060866902, - "timestamp": 1.7932211712517978 - }, - { - "x": 2.2372776488297954, - "y": 6.887423376597064, - "heading": 0.20641433914724072, - "angularVelocity": 0.8238722351496915, - "velocityX": -1.6499823638817643, - "velocityY": -0.2363487632308513, - "timestamp": 1.864985133943872 - }, - { - "x": 2.0991135263220966, - "y": 6.86763398616501, - "heading": 0.2754201028747637, - "angularVelocity": 0.9615656820019856, - "velocityX": -1.9252577104799466, - "velocityY": -0.2757566568163505, - "timestamp": 1.9367490966359464 - }, - { - "x": 1.9411939543423418, - "y": 6.845017425150902, - "heading": 0.35429610618962826, - "angularVelocity": 1.0991032317319345, - "velocityX": -2.20054141459801, - "velocityY": -0.31515206470608825, - "timestamp": 2.0085130593280205 - }, - { - "x": 1.7635180979436822, - "y": 6.819575024064961, - "heading": 0.44302933587147275, - "angularVelocity": 1.2364594478251314, - "velocityX": -2.475836753341819, - "velocityY": -0.35452893250725576, - "timestamp": 2.080277022020095 - }, - { - "x": 1.5660850056843711, - "y": 6.791308809931096, - "heading": 0.5416123153359749, - "angularVelocity": 1.3737114809270297, - "velocityX": -2.751145350017745, - "velocityY": -0.3938775546007301, - "timestamp": 2.152040984712169 - }, - { - "x": 1.3882635137794659, - "y": 6.765974194228533, - "heading": 0.6315635240675321, - "angularVelocity": 1.253431462737045, - "velocityX": -2.477866121580597, - "velocityY": -0.35302698948771166, - "timestamp": 2.2238049474042434 - }, - { - "x": 1.2302005137524743, - "y": 6.74345974499157, - "heading": 0.7116455869416606, - "angularVelocity": 1.1159091536702253, - "velocityX": -2.202540022843384, - "velocityY": -0.3137291809520671, - "timestamp": 2.2955689100963177 - }, - { - "x": 1.0918960564219613, - "y": 6.723763317887349, - "heading": 0.7818240823983815, - "angularVelocity": 0.9779071949479434, - "velocityX": -1.9272132159713127, - "velocityY": -0.27446125277387184, - "timestamp": 2.367332872788392 - }, - { - "x": 0.9733499584724181, - "y": 6.706883152850624, - "heading": 0.8420589120511408, - "angularVelocity": 0.8393464824299417, - "velocityX": -1.6518889635221348, - "velocityY": -0.23521785034898035, - "timestamp": 2.4390968354804663 - }, - { - "x": 0.8745619102633201, - "y": 6.692817866530404, - "heading": 0.8923094188839037, - "angularVelocity": 0.7002192318544365, - "velocityX": -1.3765690257806886, - "velocityY": -0.19599372432601517, - "timestamp": 2.5108607981725406 - }, - { - "x": 0.7955315888183172, - "y": 6.681566445166433, - "heading": 0.9325395236678284, - "angularVelocity": 0.5605892327200797, - "velocityX": -1.1012535885759673, - "velocityY": -0.1567837246168418, - "timestamp": 2.582624760864615 - }, - { - "x": 0.7362587558475776, - "y": 6.6731282114678185, - "heading": 0.9627218856336621, - "angularVelocity": 0.42057825170849084, - "velocityX": -0.8259414718377056, - "velocityY": -0.1175831626653607, - "timestamp": 2.654388723556689 - }, - { - "x": 0.6967433316791286, - "y": 6.667502780132869, - "heading": 0.9828408569879545, - "angularVelocity": 0.280349225415856, - "velocityX": -0.5506304652941507, - "velocityY": -0.07838796972746226, - "timestamp": 2.7261526862487635 - }, - { - "x": 0.6769854426384101, - "y": 6.664690017700197, - "heading": 0.9928942822119466, - "angularVelocity": 0.14009016289563994, - "velocityX": -0.27531769845983267, - "velocityY": -0.03919463651668921, - "timestamp": 2.797916648940838 - }, - { - "x": 0.6769854426383973, - "y": 6.664690017700195, - "heading": 0.9928942822119468, - "angularVelocity": 5.283302360613496e-15, - "velocityX": -1.780224799783019e-13, - "velocityY": -2.5618525952842936e-14, - "timestamp": 2.869680611632912 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/New Path (1).2.traj b/src/main/deploy/choreo/New Path (1).2.traj deleted file mode 100644 index 553877d1..00000000 --- a/src/main/deploy/choreo/New Path (1).2.traj +++ /dev/null @@ -1,679 +0,0 @@ -{ - "samples": [ - { - "x": 0.6769854426383973, - "y": 6.664690017700195, - "heading": 0.9928942822119468, - "angularVelocity": 5.283302360613496e-15, - "velocityX": -1.780224799783019e-13, - "velocityY": -2.5618525952842936e-14, - "timestamp": 0 - }, - { - "x": 0.6957215109471526, - "y": 6.667163242266952, - "heading": 0.9848121033534688, - "angularVelocity": -0.11585302381685599, - "velocityX": 0.26856992477116176, - "velocityY": 0.0354521410196639, - "timestamp": 0.06976234708603224 - }, - { - "x": 0.7331963593306902, - "y": 6.672111373588403, - "heading": 0.9687702469334057, - "angularVelocity": -0.2299500674809579, - "velocityX": 0.5371787210272998, - "velocityY": 0.07092839516063441, - "timestamp": 0.13952469417206448 - }, - { - "x": 0.7894132097701914, - "y": 6.6795364163430575, - "heading": 0.9449181659353634, - "angularVelocity": -0.34190479527054096, - "velocityX": 0.8058337023863809, - "velocityY": 0.10643338512532331, - "timestamp": 0.20928704125809672 - }, - { - "x": 0.8643758534690954, - "y": 6.6894407506151765, - "heading": 0.9134357837569461, - "angularVelocity": -0.45128043268946305, - "velocityX": 1.0745430282966044, - "velocityY": 0.14197249212249724, - "timestamp": 0.27904938834412896 - }, - { - "x": 0.9580887334203075, - "y": 6.701827196189313, - "heading": 0.8745385889366635, - "angularVelocity": -0.5575671754895775, - "velocityX": 1.3433160417559256, - "velocityY": 0.17755201898327602, - "timestamp": 0.3488117354301612 - }, - { - "x": 1.0705570645194333, - "y": 6.716699092539546, - "heading": 0.8284853967881356, - "angularVelocity": -0.660143961207815, - "velocityX": 1.6121638075110551, - "velocityY": 0.21317941513480868, - "timestamp": 0.41857408251619344 - }, - { - "x": 1.201787009609373, - "y": 6.734060406742245, - "heading": 0.7755905988270219, - "angularVelocity": -0.7582141394394705, - "velocityX": 1.8810999137988282, - "velocityY": 0.24886367686696464, - "timestamp": 0.4883364296022257 - }, - { - "x": 1.3517859328741344, - "y": 6.753915895330644, - "heading": 0.716244001874149, - "angularVelocity": -0.8506966785347023, - "velocityX": 2.150141581099324, - "velocityY": 0.28461612055444174, - "timestamp": 0.5580987766882579 - }, - { - "x": 1.5205627482860895, - "y": 6.77627136903214, - "heading": 0.6509438113145414, - "angularVelocity": -0.9360377522716965, - "velocityX": 2.419311024668021, - "velocityY": 0.3204518574171048, - "timestamp": 0.6278611237742902 - }, - { - "x": 1.7081283513605638, - "y": 6.801134148413729, - "heading": 0.5803536222663421, - "angularVelocity": -1.0118666013508102, - "velocityX": 2.6886366486948075, - "velocityY": 0.3563925300696616, - "timestamp": 0.6976234708603224 - }, - { - "x": 1.9144959874302385, - "y": 6.82851387237959, - "heading": 0.5054075318506537, - "angularVelocity": -1.0743057472430404, - "velocityX": 2.958152136354839, - "velocityY": 0.3924713704384897, - "timestamp": 0.7673858179463546 - }, - { - "x": 2.13968076674591, - "y": 6.85842398368424, - "heading": 0.4275265166505411, - "angularVelocity": -1.11637607467633, - "velocityX": 3.2278842200932387, - "velocityY": 0.4287429043602019, - "timestamp": 0.8371481650323869 - }, - { - "x": 2.3836937722904747, - "y": 6.890884605781558, - "heading": 0.34915382162474085, - "angularVelocity": -1.1234240001866527, - "velocityX": 3.4977751715211443, - "velocityY": 0.46530289551880577, - "timestamp": 0.9069105121184191 - }, - { - "x": 2.64649224375029, - "y": 6.9259280275443755, - "heading": 0.2756166637480684, - "angularVelocity": -1.0541095726894745, - "velocityX": 3.767053180359977, - "velocityY": 0.5023257276536467, - "timestamp": 0.9766728592044513 - }, - { - "x": 2.9267468937804706, - "y": 6.9635991004394935, - "heading": 0.23196812994939403, - "angularVelocity": -0.625674674403458, - "velocityX": 4.017276679131286, - "velocityY": 0.5399914777617958, - "timestamp": 1.0464352062904836 - }, - { - "x": 3.21904907143058, - "y": 7.002175049325539, - "heading": 0.2319681107304229, - "angularVelocity": -2.754920374409833e-7, - "velocityX": 4.189970519335255, - "velocityY": 0.5529623141617016, - "timestamp": 1.1161975533765158 - }, - { - "x": 3.5113512743695896, - "y": 7.040750806599912, - "heading": 0.23196809151475145, - "angularVelocity": -2.7544473830670244e-7, - "velocityX": 4.1899708818359604, - "velocityY": 0.5529595675271776, - "timestamp": 1.1859599004625485 - }, - { - "x": 3.803653477296151, - "y": 7.0793265639686105, - "heading": 0.2319680722990802, - "angularVelocity": -2.7544473555538935e-7, - "velocityX": 4.189970881657522, - "velocityY": 0.5529595688792672, - "timestamp": 1.2557222475485812 - }, - { - "x": 4.0959556802087445, - "y": 7.117902321443152, - "heading": 0.23196805308340882, - "angularVelocity": -2.7544473764822676e-7, - "velocityX": 4.189970881457295, - "velocityY": 0.5529595703964558, - "timestamp": 1.3254845946346139 - }, - { - "x": 4.388257883107509, - "y": 7.156478079022474, - "heading": 0.2319680338677376, - "angularVelocity": -2.7544473482866674e-7, - "velocityX": 4.189970881259077, - "velocityY": 0.5529595718984206, - "timestamp": 1.3952469417206466 - }, - { - "x": 4.680560085992557, - "y": 7.1950538367057355, - "heading": 0.23196801465206635, - "angularVelocity": -2.754447357156584e-7, - "velocityX": 4.189970881062449, - "velocityY": 0.5529595733883419, - "timestamp": 1.4650092888066792 - }, - { - "x": 4.972862288863969, - "y": 7.233629594492314, - "heading": 0.23196799543639512, - "angularVelocity": -2.754447351402482e-7, - "velocityX": 4.189970880867005, - "velocityY": 0.552959574869303, - "timestamp": 1.534771635892712 - }, - { - "x": 5.265164491731846, - "y": 7.272205352305692, - "heading": 0.23196797622072332, - "angularVelocity": -2.754447432544447e-7, - "velocityX": 4.189970880816297, - "velocityY": 0.5529595752534879, - "timestamp": 1.6045339829787446 - }, - { - "x": 5.557466846082908, - "y": 7.310779962258838, - "heading": 0.23196795700465916, - "angularVelocity": -2.754503678098783e-7, - "velocityX": 4.189973052233876, - "velocityY": 0.5529431213886082, - "timestamp": 1.6742963300647773 - }, - { - "x": 5.847709486737357, - "y": 7.33828039753147, - "heading": 0.2201839132118726, - "angularVelocity": -0.1689169628747471, - "velocityX": 4.1604483332035205, - "velocityY": 0.39420169219245776, - "timestamp": 1.74405867715081 - }, - { - "x": 6.119935125713526, - "y": 7.363080305190302, - "heading": 0.18597451932436596, - "angularVelocity": -0.49037045507254595, - "velocityX": 3.9021857828328765, - "velocityY": 0.3554913028979438, - "timestamp": 1.8138210242368427 - }, - { - "x": 6.3733637004610815, - "y": 7.38526380063412, - "heading": 0.14808578647657544, - "angularVelocity": -0.5431114982565781, - "velocityX": 3.6327415193617596, - "velocityY": 0.317986655702123, - "timestamp": 1.8835833713228753 - }, - { - "x": 6.607967245147389, - "y": 7.404876822517165, - "heading": 0.11092485728672742, - "angularVelocity": -0.5326788839833663, - "velocityX": 3.3628963830157526, - "velocityY": 0.2811405106376069, - "timestamp": 1.953345718408908 - }, - { - "x": 6.8237543582288405, - "y": 7.421943493844908, - "heading": 0.0765344238630646, - "angularVelocity": -0.49296554459745734, - "velocityX": 3.093174500211964, - "velocityY": 0.2446401539027297, - "timestamp": 2.0231080654949407 - }, - { - "x": 7.020736321734685, - "y": 7.436478377864456, - "heading": 0.046102416314533726, - "angularVelocity": -0.43622396349425363, - "velocityX": 2.823614338303762, - "velocityY": 0.20834855228743585, - "timestamp": 2.0928704125809734 - }, - { - "x": 7.198923021458236, - "y": 7.448491159770216, - "heading": 0.02040756023105561, - "angularVelocity": -0.3683198337892883, - "velocityX": 2.55419588311451, - "velocityY": 0.17219578193012572, - "timestamp": 2.162632759667006 - }, - { - "x": 7.358322620391846, - "y": 7.457988739013672, - "heading": 2.9766228702944588e-18, - "angularVelocity": -0.2925297253243008, - "velocityX": 2.284894439360468, - "velocityY": 0.1361419109328858, - "timestamp": 2.2323951067530388 - }, - { - "x": 7.493848441478855, - "y": 7.46478597681802, - "heading": -0.01432691080555686, - "angularVelocity": -0.21426769116327873, - "velocityX": 2.026871331261308, - "velocityY": 0.10165683798776044, - "timestamp": 2.2992596476820735 - }, - { - "x": 7.61209224143662, - "y": 7.4694363121343565, - "heading": -0.023920550077542048, - "angularVelocity": -0.14347872786814114, - "velocityX": 1.7684081624557801, - "velocityY": 0.0695486015715211, - "timestamp": 2.366124188611108 - }, - { - "x": 7.713032323106061, - "y": 7.472066323918879, - "heading": -0.02917711146701418, - "angularVelocity": -0.07861508232070352, - "velocityX": 1.5096204994000009, - "velocityY": 0.03933343066414966, - "timestamp": 2.432988729540143 - }, - { - "x": 7.796652130626234, - "y": 7.472779182444933, - "heading": -0.03041864164215527, - "angularVelocity": -0.018567841159019625, - "velocityX": 1.250585233343933, - "velocityY": 0.010661234133205896, - "timestamp": 2.4998532704691776 - }, - { - "x": 7.862938677721557, - "y": 7.471660580208865, - "heading": -0.02791222023833076, - "angularVelocity": 0.0374850611250684, - "velocityX": 0.9913557496143449, - "velocityY": -0.016729378838559458, - "timestamp": 2.5667178113982123 - }, - { - "x": 7.911881544220642, - "y": 7.468782819370691, - "heading": -0.02188301743139942, - "angularVelocity": 0.0901704060651562, - "velocityX": 0.7319704258648793, - "velocityY": -0.0430386688996642, - "timestamp": 2.633582352327247 - }, - { - "x": 7.943472208864585, - "y": 7.464207719041449, - "heading": -0.012523483940745138, - "angularVelocity": 0.1399775330931801, - "velocityX": 0.4724576614901361, - "velocityY": -0.06842341644279334, - "timestamp": 2.7004468932562817 - }, - { - "x": 7.957703590393066, - "y": 7.457988739013672, - "heading": -9.58263346460075e-19, - "angularVelocity": 0.1872963422277382, - "velocityX": 0.21283899254742225, - "velocityY": -0.09300864017561207, - "timestamp": 2.7673114341853164 - }, - { - "x": 7.948008333447891, - "y": 7.447597139427541, - "heading": 0.020611106108452282, - "angularVelocity": 0.244265653865981, - "velocityX": -0.1149001060229738, - "velocityY": -0.12315257872447491, - "timestamp": 2.8516913123312664 - }, - { - "x": 7.91065728802642, - "y": 7.434662339323929, - "heading": 0.04591932681578526, - "angularVelocity": 0.29993194187313127, - "velocityX": -0.4426534648090588, - "velocityY": -0.1532924719473842, - "timestamp": 2.9360711904772163 - }, - { - "x": 7.84564906711384, - "y": 7.419184732847116, - "heading": 0.07579509604586687, - "angularVelocity": 0.35406272071649775, - "velocityX": -0.7704232613388857, - "velocityY": -0.18342769410074916, - "timestamp": 3.0204510686231663 - }, - { - "x": 7.75298205837521, - "y": 7.401164773089521, - "heading": 0.11008354939762832, - "angularVelocity": 0.4063581757306341, - "velocityX": -1.098212165918826, - "velocityY": -0.2135575465803072, - "timestamp": 3.104830946769116 - }, - { - "x": 7.632654368476224, - "y": 7.380602982041041, - "heading": 0.1485961240808659, - "angularVelocity": 0.4564189416891932, - "velocityX": -1.4260235087191944, - "velocityY": -0.24368121287061745, - "timestamp": 3.189210824915066 - }, - { - "x": 7.484663748059663, - "y": 7.357499967812027, - "heading": 0.19109766369241152, - "angularVelocity": 0.5036928299189015, - "velocityX": -1.7538615090268934, - "velocityY": -0.2737976723438022, - "timestamp": 3.273590703061016 - }, - { - "x": 7.309007491620081, - "y": 7.3318564566519955, - "heading": 0.23728570485850448, - "angularVelocity": 0.5473821742928157, - "velocityX": -2.081731572730565, - "velocityY": -0.3039055249128834, - "timestamp": 3.357970581206966 - }, - { - "x": 7.105682311132841, - "y": 7.303673357340031, - "heading": 0.28675523972532424, - "angularVelocity": 0.5862717030860531, - "velocityX": -2.409640603362243, - "velocityY": -0.33400260738960413, - "timestamp": 3.442350459352916 - }, - { - "x": 6.874684211782645, - "y": 7.27295190361209, - "heading": 0.33893402592445626, - "angularVelocity": 0.6183794921921966, - "velocityX": -2.7375969772158726, - "velocityY": -0.36408506865586426, - "timestamp": 3.526730337498866 - }, - { - "x": 6.616008551028164, - "y": 7.239694012609553, - "heading": 0.39295049539372623, - "angularVelocity": 0.6401581829241193, - "velocityX": -3.065608370600574, - "velocityY": -0.39414480956007225, - "timestamp": 3.611110215644816 - }, - { - "x": 6.3296513432839285, - "y": 7.203903380962924, - "heading": 0.4473174650872342, - "angularVelocity": 0.6443120195015049, - "velocityX": -3.3936669978229808, - "velocityY": -0.42416074108002433, - "timestamp": 3.6954900937907658 - }, - { - "x": 6.015619899578595, - "y": 7.16559014212447, - "heading": 0.4989432747445404, - "angularVelocity": 0.6118260750271552, - "velocityX": -3.721638980826211, - "velocityY": -0.4540565793681812, - "timestamp": 3.7798699719367157 - }, - { - "x": 5.674085608477763, - "y": 7.124807041946436, - "heading": 0.537674756341675, - "angularVelocity": 0.459013244012295, - "velocityX": -4.047579809372173, - "velocityY": -0.4833273177699157, - "timestamp": 3.8642498500826656 - }, - { - "x": 5.318973699251008, - "y": 7.092101853594614, - "heading": 0.5376747660084731, - "angularVelocity": 1.1456283510085914e-7, - "velocityX": -4.208490424844254, - "velocityY": -0.3875946383242225, - "timestamp": 3.9486297282286156 - }, - { - "x": 4.963861733606133, - "y": 7.0593972778332645, - "heading": 0.5376747756750203, - "angularVelocity": 1.145598610125558e-7, - "velocityX": -4.208491093464797, - "velocityY": -0.3875873784124347, - "timestamp": 4.0330096063745655 - }, - { - "x": 4.608749767865936, - "y": 7.0266927031069315, - "heading": 0.5376747853415674, - "angularVelocity": 1.1455986164882247e-7, - "velocityX": -4.208491094594466, - "velocityY": -0.3875873661462843, - "timestamp": 4.1173894845205155 - }, - { - "x": 4.253637802014652, - "y": 6.993988129586792, - "heading": 0.5376747950081144, - "angularVelocity": 1.1455986002003145e-7, - "velocityX": -4.208491095910967, - "velocityY": -0.38758735185148996, - "timestamp": 4.201769362666465 - }, - { - "x": 3.898525836033987, - "y": 6.96128355747151, - "heading": 0.5376748046746616, - "angularVelocity": 1.1455986028634167e-7, - "velocityX": -4.208491097444301, - "velocityY": -0.3875873352022801, - "timestamp": 4.286149240812415 - }, - { - "x": 3.543413869905308, - "y": 6.928578986963385, - "heading": 0.5376748143412087, - "angularVelocity": 1.1455986139039553e-7, - "velocityX": -4.208491099198433, - "velocityY": -0.3875873161555974, - "timestamp": 4.370529118958365 - }, - { - "x": 3.1883019078060477, - "y": 6.895874372703153, - "heading": 0.5376748240077555, - "angularVelocity": 1.1455985744024431e-7, - "velocityX": -4.208491051445117, - "velocityY": -0.3875878346691152, - "timestamp": 4.454908997104315 - }, - { - "x": 2.8333041011808957, - "y": 6.861953115949488, - "heading": 0.5376748448822255, - "angularVelocity": 2.473868229878406e-7, - "velocityX": -4.207138175894495, - "velocityY": -0.402006467643765, - "timestamp": 4.539288875250265 - }, - { - "x": 2.501488957166729, - "y": 6.831694059731981, - "heading": 0.6004577434905676, - "angularVelocity": 0.7440505958037514, - "velocityX": -3.932396577300496, - "velocityY": -0.3586051186891842, - "timestamp": 4.623668753396215 - }, - { - "x": 2.1973597821297983, - "y": 6.803903744187767, - "heading": 0.6620822870251231, - "angularVelocity": 0.7303227367544295, - "velocityX": -3.604285544367408, - "velocityY": -0.32934766149039085, - "timestamp": 4.708048631542165 - }, - { - "x": 1.9208992514020038, - "y": 6.778618023494544, - "heading": 0.7199271131913362, - "angularVelocity": 0.6855286762343996, - "velocityX": -3.2763798289635693, - "velocityY": -0.2996652904557198, - "timestamp": 4.792428509688115 - }, - { - "x": 1.6720976847443298, - "y": 6.755848746048995, - "heading": 0.7730511242177031, - "angularVelocity": 0.6295815091659593, - "velocityX": -2.948588835685773, - "velocityY": -0.26984250209706107, - "timestamp": 4.876808387834065 - }, - { - "x": 1.4509493593220482, - "y": 6.73560176909627, - "heading": 0.8209603900337609, - "angularVelocity": 0.5677806945062207, - "velocityX": -2.620865664676194, - "velocityY": -0.23995029854989505, - "timestamp": 4.961188265980015 - }, - { - "x": 1.2574505142738839, - "y": 6.717880578554631, - "heading": 0.863346182621816, - "angularVelocity": 0.5023210926512751, - "velocityX": -2.2931870642604335, - "velocityY": -0.210016782804394, - "timestamp": 5.045568144125965 - }, - { - "x": 1.091598491304912, - "y": 6.702687492349894, - "heading": 0.8999957411768414, - "angularVelocity": 0.4343400270338572, - "velocityX": -1.9655399677409033, - "velocityY": -0.18005579693369408, - "timestamp": 5.129948022271915 - }, - { - "x": 0.9533913181145974, - "y": 6.690024171143223, - "heading": 0.9307536905218414, - "angularVelocity": 0.3645175842965618, - "velocityX": -1.6379162452837475, - "velocityY": -0.1500751302907371, - "timestamp": 5.2143279004178655 - }, - { - "x": 0.8428274846256089, - "y": 6.6798918692543365, - "heading": 0.9555026788907879, - "angularVelocity": 0.2933043862203576, - "velocityX": -1.310310419004739, - "velocityY": -0.12007959849576923, - "timestamp": 5.298707778563816 - }, - { - "x": 0.7599058126783065, - "y": 6.672291570775605, - "heading": 0.9741525918595235, - "angularVelocity": 0.22102322708356353, - "velocityX": -0.9827185552919921, - "velocityY": -0.09007240405805265, - "timestamp": 5.383087656709767 - }, - { - "x": 0.7046253750904827, - "y": 6.667224069253472, - "heading": 0.9866340557493223, - "angularVelocity": 0.14791990891726337, - "velocityX": -0.6551376797701268, - "velocityY": -0.06005580516918964, - "timestamp": 5.467467534855718 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": 0.07419098723745464, - "velocityX": -0.32756544640035345, - "velocityY": -0.03003146732321222, - "timestamp": 5.551847413001669 - }, - { - "x": 0.6769854426383972, - "y": 6.664690017700195, - "heading": 0.9928942822119464, - "angularVelocity": -2.359956805895598e-20, - "velocityX": 8.905145998935021e-20, - "velocityY": 2.4738732069207315e-17, - "timestamp": 5.63622729114762 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SQUARFE (1).1.traj b/src/main/deploy/choreo/SQUARFE (1).1.traj deleted file mode 100644 index 3872bee3..00000000 --- a/src/main/deploy/choreo/SQUARFE (1).1.traj +++ /dev/null @@ -1,211 +0,0 @@ -{ - "samples": [ - { - "x": 0, - "y": 2, - "heading": 6.834778036374476e-42, - "angularVelocity": 0, - "velocityX": 6.685663635549999e-41, - "velocityY": 1.8473219086701058e-41, - "timestamp": 0 - }, - { - "x": 0.008264463615126694, - "y": 2, - "heading": 1.7039918296957192e-26, - "angularVelocity": 1.739180661511921e-25, - "velocityX": 0.08434788783387884, - "velocityY": -2.7623927453648295e-18, - "timestamp": 0.09798068247308528 - }, - { - "x": 0.024793390752145244, - "y": 2, - "heading": 7.683593549129969e-26, - "angularVelocity": 6.102971583946312e-25, - "velocityX": 0.16869577471619418, - "velocityY": -5.524785459565961e-18, - "timestamp": 0.19596136494617056 - }, - { - "x": 0.04958678129588319, - "y": 2, - "heading": 5.293972470528129e-26, - "angularVelocity": -2.438680388678047e-25, - "velocityX": 0.2530436604230487, - "velocityY": -8.287178135270759e-18, - "timestamp": 0.29394204741925584 - }, - { - "x": 0.08264463510045543, - "y": 2, - "heading": -7.599657248983579e-26, - "angularVelocity": -1.3159126117680205e-24, - "velocityX": 0.33739154464098625, - "velocityY": -1.1049570762213534e-17, - "timestamp": 0.3919227298923411 - }, - { - "x": 0.12396695197508915, - "y": 2, - "heading": -6.356500212054052e-25, - "angularVelocity": -5.711849121394279e-24, - "velocityX": 0.42173942691187855, - "velocityY": -1.3811963325390594e-17, - "timestamp": 0.4899034123654264 - }, - { - "x": 0.1735537316596396, - "y": 2, - "heading": -1.7701677559872392e-24, - "angularVelocity": -1.1578935811088608e-23, - "velocityX": 0.5060873065277093, - "velocityY": -1.6574355801614406e-17, - "timestamp": 0.5878840948385117 - }, - { - "x": 0.23140497377834218, - "y": 2, - "heading": -2.69685519918364e-24, - "angularVelocity": -9.45777203938324e-24, - "velocityX": 0.5904351823084512, - "velocityY": -1.9336748152239084e-17, - "timestamp": 0.6858647773115969 - }, - { - "x": 0.29752067774070967, - "y": 2, - "heading": -3.3710373572859254e-24, - "angularVelocity": -6.88078025268394e-24, - "velocityX": 0.6747830520626255, - "velocityY": -2.2099140305493712e-17, - "timestamp": 0.7838454597846822 - }, - { - "x": 0.37190084248386523, - "y": 2, - "heading": -3.785377470906936e-24, - "angularVelocity": -4.228794191414106e-24, - "velocityX": 0.7591309109689794, - "velocityY": -2.4861532103482302e-17, - "timestamp": 0.8818261422577676 - }, - { - "x": 0.454545465527764, - "y": 2, - "heading": -3.6811470837306276e-24, - "angularVelocity": 1.0637753265934456e-24, - "velocityX": 0.8434787445637631, - "velocityY": -2.7623923072517135e-17, - "timestamp": 0.9798068247308529 - }, - { - "x": 0.5454545344722358, - "y": 2, - "heading": -3.223140769415779e-24, - "angularVelocity": 4.674291630935048e-24, - "velocityX": 0.9278264516012495, - "velocityY": -3.038630989680133e-17, - "timestamp": 1.0777875072039382 - }, - { - "x": 0.6280991575161347, - "y": 2, - "heading": -3.145189007341093e-24, - "angularVelocity": 7.953315368066196e-25, - "velocityX": 0.8434787445637631, - "velocityY": -2.7623923072517132e-17, - "timestamp": 1.1757681896770236 - }, - { - "x": 0.7024793222592902, - "y": 2, - "heading": -2.7590363290180994e-24, - "angularVelocity": 3.940934765433298e-24, - "velocityX": 0.7591309109689796, - "velocityY": -2.48615321034823e-17, - "timestamp": 1.273748872150109 - }, - { - "x": 0.7685950262216578, - "y": 2, - "heading": -2.36523859209739e-24, - "angularVelocity": 4.0191634044675645e-24, - "velocityX": 0.6747830520626255, - "velocityY": -2.209914030549371e-17, - "timestamp": 1.3717295546231942 - }, - { - "x": 0.8264462683403603, - "y": 2, - "heading": -1.912083934393757e-24, - "angularVelocity": 4.624982515888689e-24, - "velocityX": 0.5904351823084512, - "velocityY": -1.9336748152239084e-17, - "timestamp": 1.4697102370962796 - }, - { - "x": 0.8760330480249108, - "y": 2, - "heading": -1.4947650795710447e-24, - "angularVelocity": 4.259255952533442e-24, - "velocityX": 0.5060873065277093, - "velocityY": -1.6574355801614403e-17, - "timestamp": 1.567690919569365 - }, - { - "x": 0.9173553648995445, - "y": 2, - "heading": -1.0599672683839614e-24, - "angularVelocity": 4.4376648384860465e-24, - "velocityX": 0.42173942691187855, - "velocityY": -1.3811963325390594e-17, - "timestamp": 1.6656716020424502 - }, - { - "x": 0.9504132187041168, - "y": 2, - "heading": -6.474555283607888e-25, - "angularVelocity": 4.210197162464856e-24, - "velocityX": 0.33739154464098625, - "velocityY": -1.1049570762213536e-17, - "timestamp": 1.7636522845155356 - }, - { - "x": 0.9752066092478547, - "y": 2, - "heading": -3.2958736625896835e-25, - "angularVelocity": 3.2442420699516962e-24, - "velocityX": 0.2530436604230487, - "velocityY": -8.287178135270759e-18, - "timestamp": 1.861632966988621 - }, - { - "x": 0.9917355363848733, - "y": 2, - "heading": -1.1216781384407191e-25, - "angularVelocity": 2.219040288078826e-24, - "velocityX": 0.16869577471619418, - "velocityY": -5.524785459565961e-18, - "timestamp": 1.9596136494617062 - }, - { - "x": 1, - "y": 2, - "heading": 1.4025423606350947e-41, - "angularVelocity": 1.1448173504163706e-24, - "velocityX": 0.08434788783387884, - "velocityY": -2.7623927453648302e-18, - "timestamp": 2.0575943319347916 - }, - { - "x": 1, - "y": 2, - "heading": 4.353206627192277e-42, - "angularVelocity": -9.871558911369574e-41, - "velocityX": -6.752513577763152e-40, - "velocityY": -2.739881155941097e-39, - "timestamp": 2.155575014407877 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SQUARFE (1).2.traj b/src/main/deploy/choreo/SQUARFE (1).2.traj deleted file mode 100644 index 4cbeb364..00000000 --- a/src/main/deploy/choreo/SQUARFE (1).2.traj +++ /dev/null @@ -1,211 +0,0 @@ -{ - "samples": [ - { - "x": 1, - "y": 2, - "heading": 4.353206627192277e-42, - "angularVelocity": -9.871558911369574e-41, - "velocityX": -6.752513577763152e-40, - "velocityY": -2.739881155941097e-39, - "timestamp": 0 - }, - { - "x": 1, - "y": 1.9917355363848732, - "heading": 2.678019676818953e-25, - "angularVelocity": 2.73325426399564e-24, - "velocityX": -7.221388058205676e-18, - "velocityY": -0.08434788783387888, - "timestamp": 0.09798068247308533 - }, - { - "x": 1, - "y": 1.9752066092478548, - "heading": 5.868915118029391e-25, - "angularVelocity": 3.2567428149199723e-24, - "velocityX": -1.4442776034943873e-17, - "velocityY": -0.16869577471619424, - "timestamp": 0.19596136494617067 - }, - { - "x": 1, - "y": 1.9504132187041168, - "heading": 7.292918744038093e-25, - "angularVelocity": 1.4534178819580077e-24, - "velocityX": -2.1664163911045784e-17, - "velocityY": -0.2530436604230488, - "timestamp": 0.293942047419256 - }, - { - "x": 1, - "y": 1.9173553648995445, - "heading": 6.385291298407452e-25, - "angularVelocity": -9.262537240715945e-25, - "velocityX": -2.888555165967506e-17, - "velocityY": -0.33739154464098636, - "timestamp": 0.39192272989234134 - }, - { - "x": 1, - "y": 1.8760330480249108, - "heading": 1.3873456572049932e-25, - "angularVelocity": -5.1008572041181364e-24, - "velocityX": -3.6106939241609364e-17, - "velocityY": -0.4217394269118787, - "timestamp": 0.48990341236542667 - }, - { - "x": 1, - "y": 1.8264462683403602, - "heading": -1.4606320867425958e-24, - "angularVelocity": -1.6323178027645798e-23, - "velocityX": -4.332832659623233e-17, - "velocityY": -0.5060873065277094, - "timestamp": 0.587884094838512 - }, - { - "x": 1, - "y": 1.7685950262216577, - "heading": -2.249822541344819e-24, - "angularVelocity": -8.054428449773172e-24, - "velocityX": -5.054971362251674e-17, - "velocityY": -0.5904351823084514, - "timestamp": 0.6858647773115973 - }, - { - "x": 1, - "y": 1.7024793222592902, - "heading": -1.6693794150187738e-24, - "angularVelocity": 5.924134836898355e-24, - "velocityX": -5.777110013284059e-17, - "velocityY": -0.6747830520626257, - "timestamp": 0.7838454597846827 - }, - { - "x": 1, - "y": 1.6280991575161345, - "heading": -4.17992965329782e-24, - "angularVelocity": -2.5622809982354645e-23, - "velocityX": -6.499248571443555e-17, - "velocityY": -0.7591309109689798, - "timestamp": 0.881826142257768 - }, - { - "x": 1, - "y": 1.545454534472236, - "heading": -1.2224446341565232e-23, - "angularVelocity": -8.210320576787122e-23, - "velocityX": -7.221386912899733e-17, - "velocityY": -0.8434787445637635, - "timestamp": 0.9798068247308533 - }, - { - "x": 1, - "y": 1.454545465527764, - "heading": -1.4447403666940808e-23, - "angularVelocity": -2.2688260164068862e-23, - "velocityX": -7.943524170844061e-17, - "velocityY": -0.9278264516012499, - "timestamp": 1.0777875072039387 - }, - { - "x": 1, - "y": 1.3719008424838655, - "heading": -1.4038421369615485e-23, - "angularVelocity": 4.174104041070465e-24, - "velocityX": -7.221386912899733e-17, - "velocityY": -0.8434787445637635, - "timestamp": 1.175768189677024 - }, - { - "x": 1, - "y": 1.2975206777407098, - "heading": -1.2608666390151249e-23, - "angularVelocity": 1.4592214374369727e-23, - "velocityX": -6.499248571443555e-17, - "velocityY": -0.7591309109689798, - "timestamp": 1.2737488721501093 - }, - { - "x": 1, - "y": 1.2314049737783423, - "heading": -9.223958699370151e-24, - "angularVelocity": 3.4544645258213173e-23, - "velocityX": -5.777110013284059e-17, - "velocityY": -0.6747830520626257, - "timestamp": 1.3717295546231947 - }, - { - "x": 1, - "y": 1.1735537316596398, - "heading": -6.510980866302344e-24, - "angularVelocity": 2.768890658272266e-23, - "velocityX": -5.054971362251675e-17, - "velocityY": -0.5904351823084515, - "timestamp": 1.46971023709628 - }, - { - "x": 1, - "y": 1.1239669519750892, - "heading": -4.570495528808751e-24, - "angularVelocity": 1.980477298357616e-23, - "velocityX": -4.332832659623233e-17, - "velocityY": -0.5060873065277095, - "timestamp": 1.5676909195693653 - }, - { - "x": 1, - "y": 1.0826446351004555, - "heading": -2.7074347601637494e-24, - "angularVelocity": 1.9014567030490677e-23, - "velocityX": -3.610693924160936e-17, - "velocityY": -0.42173942691187877, - "timestamp": 1.6656716020424507 - }, - { - "x": 1, - "y": 1.0495867812958832, - "heading": -1.1867771133422862e-24, - "angularVelocity": 1.551993357249157e-23, - "velocityX": -2.888555165967506e-17, - "velocityY": -0.33739154464098636, - "timestamp": 1.763652284515536 - }, - { - "x": 1, - "y": 1.0247933907521452, - "heading": -6.71373293397456e-25, - "angularVelocity": 5.260245478860946e-24, - "velocityX": -2.166416391104578e-17, - "velocityY": -0.2530436604230488, - "timestamp": 1.8616329669886214 - }, - { - "x": 1, - "y": 1.0082644636151268, - "heading": -2.683389851754243e-25, - "angularVelocity": 4.1133864703848926e-24, - "velocityX": -1.444277603494387e-17, - "velocityY": -0.16869577471619424, - "timestamp": 1.9596136494617062 - }, - { - "x": 1, - "y": 1, - "heading": 1.3844887180664726e-41, - "angularVelocity": 2.738667729952811e-24, - "velocityX": -7.221388058205674e-18, - "velocityY": -0.08434788783387888, - "timestamp": 2.057594331934791 - }, - { - "x": 1, - "y": 1, - "heading": -1.2548656701086555e-41, - "angularVelocity": -9.97631116732387e-41, - "velocityX": 4.048108670644253e-37, - "velocityY": -1.1182831125559368e-40, - "timestamp": 2.155575014407876 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SQUARFE (1).3.traj b/src/main/deploy/choreo/SQUARFE (1).3.traj deleted file mode 100644 index 7c2d50f8..00000000 --- a/src/main/deploy/choreo/SQUARFE (1).3.traj +++ /dev/null @@ -1,211 +0,0 @@ -{ - "samples": [ - { - "x": 1, - "y": 1, - "heading": -1.2548656701086555e-41, - "angularVelocity": -9.97631116732387e-41, - "velocityX": 4.048108670644253e-37, - "velocityY": -1.1182831125559368e-40, - "timestamp": 0 - }, - { - "x": 0.9917355363848733, - "y": 1, - "heading": -1.1243665350537136e-24, - "angularVelocity": -1.1475372005589736e-23, - "velocityX": -0.08434788783387885, - "velocityY": 1.1036137138901194e-18, - "timestamp": 0.09798068247308489 - }, - { - "x": 0.9752066092478547, - "y": 1, - "heading": -3.378137956306648e-24, - "angularVelocity": -2.3002166920042362e-23, - "velocityX": -0.16869577471619418, - "velocityY": 2.207227415329915e-18, - "timestamp": 0.19596136494616978 - }, - { - "x": 0.9504132187041168, - "y": 1, - "heading": -6.761085388842748e-24, - "angularVelocity": -3.4526627166894346e-23, - "velocityX": -0.2530436604230487, - "velocityY": 3.3108411013898984e-18, - "timestamp": 0.29394204741925467 - }, - { - "x": 0.9173553648995445, - "y": 1, - "heading": -1.1133132425652307e-23, - "angularVelocity": -4.46214562056447e-23, - "velocityX": -0.33739154464098625, - "velocityY": 4.414454767968787e-18, - "timestamp": 0.39192272989233956 - }, - { - "x": 0.8760330480249108, - "y": 1, - "heading": -1.6100751630919242e-23, - "angularVelocity": -5.0699909707958593e-23, - "velocityX": -0.42173942691187855, - "velocityY": 5.518068409072398e-18, - "timestamp": 0.48990341236542445 - }, - { - "x": 0.8264462683403604, - "y": 1, - "heading": -2.1677679346560742e-23, - "angularVelocity": -5.691865345905286e-23, - "velocityX": -0.5060873065277093, - "velocityY": 6.621682015436997e-18, - "timestamp": 0.5878840948385093 - }, - { - "x": 0.7685950262216578, - "y": 1, - "heading": -2.70563073072413e-23, - "angularVelocity": -5.489487623439359e-23, - "velocityX": -0.5904351823084512, - "velocityY": 7.725295571623022e-18, - "timestamp": 0.6858647773115942 - }, - { - "x": 0.7024793222592902, - "y": 1, - "heading": -3.2234788403103226e-23, - "angularVelocity": -5.285225361991524e-23, - "velocityX": -0.6747830520626255, - "velocityY": 8.828909048957011e-18, - "timestamp": 0.7838454597846791 - }, - { - "x": 0.6280991575161347, - "y": 1, - "heading": -3.5244044696367074e-23, - "angularVelocity": -3.071298695388889e-23, - "velocityX": -0.7591309109689796, - "velocityY": 9.932522384357355e-18, - "timestamp": 0.881826142257764 - }, - { - "x": 0.5454545344722359, - "y": 1, - "heading": -3.185198887062514e-23, - "angularVelocity": 3.4619333983565953e-23, - "velocityX": -0.8434787445637631, - "velocityY": 1.1036135388579327e-17, - "timestamp": 0.9798068247308489 - }, - { - "x": 0.4545454655277641, - "y": 1, - "heading": -1.1536119074148839e-23, - "angularVelocity": 2.07345235284118e-22, - "velocityX": -0.9278264516012495, - "velocityY": 1.2139746736916687e-17, - "timestamp": 1.0777875072039338 - }, - { - "x": 0.3719008424838653, - "y": 1, - "heading": -1.7080978536028252e-24, - "angularVelocity": 1.0030590652108129e-22, - "velocityX": -0.8434787445637631, - "velocityY": 1.1036135388579327e-17, - "timestamp": 1.1757681896770187 - }, - { - "x": 0.2975206777407097, - "y": 1, - "heading": 3.9485980786107975e-24, - "angularVelocity": 5.773286957268272e-23, - "velocityX": -0.7591309109689796, - "velocityY": 9.932522384357355e-18, - "timestamp": 1.2737488721501036 - }, - { - "x": 0.23140497377834224, - "y": 1, - "heading": 6.2504476669030164e-24, - "angularVelocity": 2.3493130652605514e-23, - "velocityX": -0.6747830520626255, - "velocityY": 8.828909048957011e-18, - "timestamp": 1.3717295546231885 - }, - { - "x": 0.17355373165963964, - "y": 1, - "heading": 6.985240619947539e-24, - "angularVelocity": 7.499550312311534e-24, - "velocityX": -0.5904351823084512, - "velocityY": 7.725295571623022e-18, - "timestamp": 1.4697102370962734 - }, - { - "x": 0.12396695197508918, - "y": 1, - "heading": 6.271694392407826e-24, - "angularVelocity": -7.282389940082428e-24, - "velocityX": -0.5060873065277093, - "velocityY": 6.621682015436996e-18, - "timestamp": 1.5676909195693582 - }, - { - "x": 0.08264463510045544, - "y": 1, - "heading": 4.81506621376513e-24, - "angularVelocity": -1.486641000682138e-23, - "velocityX": -0.4217394269118786, - "velocityY": 5.5180684090723976e-18, - "timestamp": 1.6656716020424431 - }, - { - "x": 0.04958678129588319, - "y": 1, - "heading": 3.1680135607204943e-24, - "angularVelocity": -1.6809957018793716e-23, - "velocityX": -0.33739154464098625, - "velocityY": 4.414454767968786e-18, - "timestamp": 1.763652284515528 - }, - { - "x": 0.02479339075214524, - "y": 1, - "heading": 1.7613266611845955e-24, - "angularVelocity": -1.4356757129038765e-23, - "velocityX": -0.2530436604230487, - "velocityY": 3.310841101389898e-18, - "timestamp": 1.861632966988613 - }, - { - "x": 0.008264463615126694, - "y": 1, - "heading": 6.558100798178395e-25, - "angularVelocity": -1.1282980364373314e-23, - "velocityX": -0.16869577471619418, - "velocityY": 2.207227415329915e-18, - "timestamp": 1.9596136494616978 - }, - { - "x": -2.582873329443503e-41, - "y": 1, - "heading": 0, - "angularVelocity": -6.693229593886659e-24, - "velocityX": -0.08434788783387885, - "velocityY": 1.1036137138901196e-18, - "timestamp": 2.0575943319347827 - }, - { - "x": 0, - "y": 1, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.5254761443447583e-40, - "velocityY": -1.330214233321333e-39, - "timestamp": 2.1555750144078676 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SQUARFE (1).4.traj b/src/main/deploy/choreo/SQUARFE (1).4.traj deleted file mode 100644 index 66933dd1..00000000 --- a/src/main/deploy/choreo/SQUARFE (1).4.traj +++ /dev/null @@ -1,211 +0,0 @@ -{ - "samples": [ - { - "x": 0, - "y": 1, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.5254761443447583e-40, - "velocityY": -1.330214233321333e-39, - "timestamp": 0 - }, - { - "x": 1.117551892465105e-37, - "y": 1.0082644636151268, - "heading": 5.777226208013994e-25, - "angularVelocity": 5.8962777231441345e-24, - "velocityX": 1.1405739034082113e-36, - "velocityY": 0.08434788783387888, - "timestamp": 0.09798068247308489 - }, - { - "x": 2.6897755866899145e-37, - "y": 1.0247933907521452, - "heading": 1.878956892361082e-24, - "angularVelocity": 1.3280490783242518e-23, - "velocityX": 1.604618665494541e-36, - "velocityY": 0.16869577471619424, - "timestamp": 0.19596136494616978 - }, - { - "x": 4.617201760898304e-37, - "y": 1.0495867812958832, - "heading": 3.957176037517467e-24, - "angularVelocity": 2.121045567762179e-23, - "velocityX": 1.9672540809484087e-36, - "velocityY": 0.2530436604230488, - "timestamp": 0.29394204741925467 - }, - { - "x": 7.822490165552593e-37, - "y": 1.0826446351004555, - "heading": 6.68318309457519e-24, - "angularVelocity": 2.7821884471994474e-23, - "velocityX": 3.2713410852659234e-36, - "velocityY": 0.33739154464098636, - "timestamp": 0.39192272989233956 - }, - { - "x": 1.1548715422163086e-36, - "y": 1.1239669519750892, - "heading": 1.0156145042962959e-23, - "angularVelocity": 3.544538859114664e-23, - "velocityX": 3.802953813650495e-36, - "velocityY": 0.42173942691187877, - "timestamp": 0.48990341236542445 - }, - { - "x": 1.590303807530914e-36, - "y": 1.1735537316596398, - "heading": 1.604994022058709e-23, - "angularVelocity": 6.015264831157614e-23, - "velocityX": 4.4441607272626095e-36, - "velocityY": 0.5060873065277095, - "timestamp": 0.5878840948385093 - }, - { - "x": 2.0969602398345516e-36, - "y": 1.2314049737783423, - "heading": 2.689554282848633e-23, - "angularVelocity": 1.106911690454712e-22, - "velocityX": 5.170849268642284e-36, - "velocityY": 0.5904351823084514, - "timestamp": 0.6858647773115942 - }, - { - "x": 2.6970018922938464e-36, - "y": 1.2975206777407098, - "heading": 4.114017114855317e-23, - "angularVelocity": 1.4538184971887797e-22, - "velocityX": 6.124058670873409e-36, - "velocityY": 0.6747830520626257, - "timestamp": 0.7838454597846791 - }, - { - "x": 3.420493410463521e-36, - "y": 1.3719008424838655, - "heading": 5.947449866584252e-23, - "angularVelocity": 1.8712166065083759e-22, - "velocityX": 7.384122751685e-36, - "velocityY": 0.7591309109689798, - "timestamp": 0.881826142257764 - }, - { - "x": 1.9831416217837412e-36, - "y": 1.454545465527764, - "heading": 8.166369448762052e-23, - "angularVelocity": 2.2646486737171235e-22, - "velocityX": -1.466983946444783e-35, - "velocityY": 0.8434787445637635, - "timestamp": 0.9798068247308489 - }, - { - "x": 1.0861613719346016e-36, - "y": 1.545454534472236, - "heading": 9.281000307136208e-23, - "angularVelocity": 1.1376045389293476e-22, - "velocityX": -9.154736228879551e-36, - "velocityY": 0.9278264516012499, - "timestamp": 1.0777875072039338 - }, - { - "x": 6.8567251290811176e-37, - "y": 1.6280991575161345, - "heading": 7.9681702937135e-23, - "angularVelocity": -1.3398862607068113e-22, - "velocityX": -4.087402783562853e-36, - "velocityY": 0.8434787445637635, - "timestamp": 1.1757681896770187 - }, - { - "x": 4.416311613052699e-37, - "y": 1.7024793222592902, - "heading": 6.631726452847393e-23, - "angularVelocity": -1.3639872730018692e-22, - "velocityX": -2.490693450174919e-36, - "velocityY": 0.7591309109689798, - "timestamp": 1.2737488721501036 - }, - { - "x": 2.8341582058058063e-37, - "y": 1.7685950262216577, - "heading": 5.520332634625129e-23, - "angularVelocity": -1.1342981734081153e-22, - "velocityX": -1.6148009261310582e-36, - "velocityY": 0.6747830520626257, - "timestamp": 1.3717295546231885 - }, - { - "x": 1.7315008828738255e-37, - "y": 1.8264462683403602, - "heading": 4.288088447351943e-23, - "angularVelocity": -1.2576387583905343e-22, - "velocityX": -1.125317731608965e-36, - "velocityY": 0.5904351823084514, - "timestamp": 1.4697102370962734 - }, - { - "x": 9.563231995227316e-38, - "y": 1.8760330480249108, - "heading": 3.053579758781607e-23, - "angularVelocity": -1.25995006403652e-22, - "velocityX": -7.911627994010943e-37, - "velocityY": 0.5060873065277095, - "timestamp": 1.5676909195693582 - }, - { - "x": 4.825237858288704e-38, - "y": 1.9173553648995445, - "heading": 2.0171064793054303e-23, - "angularVelocity": -1.0578336973328714e-22, - "velocityX": -4.835712844569225e-37, - "velocityY": 0.4217394269118787, - "timestamp": 1.665671602042444 - }, - { - "x": 1.9830009986802493e-38, - "y": 1.9504132187041168, - "heading": 1.190680907263644e-23, - "angularVelocity": -8.434574430528315e-23, - "velocityX": -2.900681543335251e-37, - "velocityY": 0.33739154464098636, - "timestamp": 1.7636522845155298 - }, - { - "x": 7.503247022033376e-39, - "y": 1.9752066092478548, - "heading": 5.867706563960283e-24, - "angularVelocity": -6.163563013249786e-23, - "velocityX": -1.2580315029942821e-37, - "velocityY": 0.2530436604230488, - "timestamp": 1.8616329669886156 - }, - { - "x": 2.19006134392397e-39, - "y": 1.9917355363848732, - "heading": 1.9081673701003045e-24, - "angularVelocity": -4.041141844838844e-23, - "velocityX": -5.422599062203887e-38, - "velocityY": 0.16869577471619424, - "timestamp": 1.9596136494617014 - }, - { - "x": 1.871946327660505e-43, - "y": 2, - "heading": 3.8918994248492545e-43, - "angularVelocity": -1.9474930664723247e-23, - "velocityX": -2.235046387745111e-38, - "velocityY": 0.08434788783387888, - "timestamp": 2.057594331934787 - }, - { - "x": 0, - "y": 2, - "heading": -2.5991331833497144e-43, - "angularVelocity": -6.625057575927064e-42, - "velocityX": -1.9101859645151015e-42, - "velocityY": 4.591774807899561e-41, - "timestamp": 2.155575014407873 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SQUARFE.1.traj b/src/main/deploy/choreo/SQUARFE.1.traj deleted file mode 100644 index c0295972..00000000 --- a/src/main/deploy/choreo/SQUARFE.1.traj +++ /dev/null @@ -1,805 +0,0 @@ -{ - "samples": [ - { - "x": 0, - "y": 2, - "heading": 1.066025235117479e-35, - "angularVelocity": 0, - "velocityX": -3.176387274976944e-34, - "velocityY": -5.951320349991925e-34, - "timestamp": 0 - }, - { - "x": 0.005019255586936127, - "y": 2.002260189779503, - "heading": -2.3828228381393305e-18, - "angularVelocity": -2.979840326637856e-17, - "velocityX": 0.0627683285032655, - "velocityY": 0.028264815788379395, - "timestamp": 0.07996478011478358 - }, - { - "x": 0.015089028014644117, - "y": 2.006709823197785, - "heading": -4.5974330969027984e-18, - "angularVelocity": -2.769481997390876e-17, - "velocityX": 0.12592759478927565, - "velocityY": 0.055644915322655746, - "timestamp": 0.15992956022956717 - }, - { - "x": 0.030245129831638854, - "y": 2.0132643642969383, - "heading": -6.642004718817398e-18, - "angularVelocity": -2.5568400908806152e-17, - "velocityX": 0.18953471509881312, - "velocityY": 0.08196784996775677, - "timestamp": 0.23989434034435075 - }, - { - "x": 0.050528852743670936, - "y": 2.0218211251492875, - "heading": -7.257064379564418e-18, - "angularVelocity": -7.691631736143833e-18, - "velocityX": 0.2536582090629937, - "velocityY": 0.10700662016536089, - "timestamp": 0.31985912045913434 - }, - { - "x": 0.07598806396033884, - "y": 2.032252906444552, - "heading": -7.528860856473167e-18, - "angularVelocity": -3.3989522858672543e-18, - "velocityX": 0.31838030668155515, - "velocityY": 0.13045469868472426, - "timestamp": 0.3998239005739179 - }, - { - "x": 0.1066783911012953, - "y": 2.044398398790724, - "heading": -7.622887589894659e-18, - "angularVelocity": -1.1758518205026792e-18, - "velocityX": 0.38379805580535253, - "velocityY": 0.151885521710158, - "timestamp": 0.4797886806887015 - }, - { - "x": 0.14266416316439484, - "y": 2.0580471012266317, - "heading": -7.535437739911399e-18, - "angularVelocity": 1.0936045537373189e-18, - "velocityX": 0.4500202715676151, - "velocityY": 0.17068392380140238, - "timestamp": 0.5597534608034851 - }, - { - "x": 0.18401799909562916, - "y": 2.0729145182854087, - "heading": -7.261962767216934e-18, - "angularVelocity": 3.4199427139841472e-18, - "velocityX": 0.5171506239606228, - "velocityY": 0.1859245662582475, - "timestamp": 0.6397182409182687 - }, - { - "x": 0.23081547811659442, - "y": 2.088599322388309, - "heading": -5.496441495113289e-18, - "angularVelocity": 2.2078735582384192e-17, - "velocityX": 0.5852261327273193, - "velocityY": 0.19614640445938436, - "timestamp": 0.7196830210330523 - }, - { - "x": 0.2831131435217446, - "y": 2.1045062001347263, - "heading": -3.615528657766951e-18, - "angularVelocity": 2.352176542156873e-17, - "velocityX": 0.6540087439755441, - "velocityY": 0.19892354763665143, - "timestamp": 0.7996478011478358 - }, - { - "x": 0.3408701135805606, - "y": 2.1197078453164817, - "heading": -1.7358822624727144e-18, - "angularVelocity": 2.350592756240704e-17, - "velocityX": 0.7222801085166501, - "velocityY": 0.19010425789872726, - "timestamp": 0.8796125812626194 - }, - { - "x": 0.403692097997754, - "y": 2.13275380476459, - "heading": -9.4361640863198e-19, - "angularVelocity": 9.907684705793237e-18, - "velocityX": 0.7856206735892604, - "velocityY": 0.16314631803378768, - "timestamp": 0.959577361377403 - }, - { - "x": 0.4702607204262372, - "y": 2.1417668933259137, - "heading": -6.362257579471842e-19, - "angularVelocity": 3.844075127130267e-18, - "velocityX": 0.8324742759616, - "velocityY": 0.11271322885382787, - "timestamp": 1.0395421414921866 - }, - { - "x": 0.5382796795158337, - "y": 2.145469815282364, - "heading": -2.2167383910431224e-19, - "angularVelocity": 5.184181008606903e-18, - "velocityX": 0.850611469098773, - "velocityY": 0.04630691100676233, - "timestamp": 1.1195069216069702 - }, - { - "x": 0.6057260502322735, - "y": 2.143697931570498, - "heading": 3.6199909161643833e-19, - "angularVelocity": 7.299124814607158e-18, - "velocityX": 0.8434509620313541, - "velocityY": -0.022158301558787858, - "timestamp": 1.1994717017217538 - }, - { - "x": 0.6713533698714674, - "y": 2.136730625262623, - "heading": 1.6921521064800653e-18, - "angularVelocity": 1.6634235500642562e-17, - "velocityX": 0.8207028087239234, - "velocityY": -0.08712968756837854, - "timestamp": 1.2794364818365374 - }, - { - "x": 0.734431350428031, - "y": 2.1248845651211443, - "heading": 2.0691864465641914e-18, - "angularVelocity": 4.7150049781463826e-18, - "velocityX": 0.7888220347260337, - "velocityY": -0.14814097061824272, - "timestamp": 1.359401261951321 - }, - { - "x": 0.7945085091672697, - "y": 2.10842369823011, - "heading": 1.8471511862263127e-18, - "angularVelocity": -2.7766630484638473e-18, - "velocityX": 0.7512952408923314, - "velocityY": -0.20585146194869802, - "timestamp": 1.4393660420661045 - }, - { - "x": 0.851286615700003, - "y": 2.0875562872881996, - "heading": 1.5362209056083268e-18, - "angularVelocity": -3.8883400349055174e-18, - "velocityX": 0.7100389252772585, - "velocityY": -0.260957522948938, - "timestamp": 1.519330822180888 - }, - { - "x": 0.9045570425926258, - "y": 2.0624465230223628, - "heading": 1.13056857408644e-18, - "angularVelocity": -5.0728873391071146e-18, - "velocityX": 0.6661736181373479, - "velocityY": -0.3140102959051894, - "timestamp": 1.5992956022956717 - }, - { - "x": 0.9541669202924247, - "y": 2.0332255838005793, - "heading": 6.201980366388928e-19, - "angularVelocity": -6.382441609832398e-18, - "velocityX": 0.6203965999604764, - "velocityY": -0.36542261705514717, - "timestamp": 1.6792603824104553 - }, - { - "x": 1, - "y": 2, - "heading": 0, - "angularVelocity": -7.755889945091827e-18, - "velocityX": 0.5731658317797576, - "velocityY": -0.41550272198442756, - "timestamp": 1.7592251625252389 - }, - { - "x": 1.0360682198258142, - "y": 1.9690252357457936, - "heading": -9.890855738376694e-19, - "angularVelocity": -1.4593472195699707e-17, - "velocityX": 0.5321688938798576, - "velocityY": -0.45701745500488516, - "timestamp": 1.8270010514218133 - }, - { - "x": 1.0692669632152427, - "y": 1.9353295083481672, - "heading": -2.0665277353015157e-18, - "angularVelocity": -1.5897129978073518e-17, - "velocityX": 0.4898311763950358, - "velocityY": -0.4971639316903078, - "timestamp": 1.8947769403183878 - }, - { - "x": 1.0995000427990536, - "y": 1.8990179878910807, - "heading": -2.6852884598983727e-18, - "angularVelocity": -9.129510523370085e-18, - "velocityX": 0.4460742614522743, - "velocityY": -0.5357586753675423, - "timestamp": 1.9625528292149623 - }, - { - "x": 1.1266661316570161, - "y": 1.860210249756036, - "heading": -3.1299706826939245e-18, - "angularVelocity": -6.561067790129639e-18, - "velocityX": 0.40082231749727, - "velocityY": -0.5725891429364366, - "timestamp": 2.030328718111537 - }, - { - "x": 1.1506592197210015, - "y": 1.8190425115321425, - "heading": -3.75029364893705e-18, - "angularVelocity": -9.152560722079088e-18, - "velocityX": 0.3540062469796368, - "velocityY": -0.6074097867859044, - "timestamp": 2.098104607008111 - }, - { - "x": 1.1713694688316625, - "y": 1.77567011857456, - "heading": -4.734849315417307e-18, - "angularVelocity": -1.452663543228679e-17, - "velocityX": 0.3055695682909384, - "velocityY": -0.6399383861090825, - "timestamp": 2.1658804959046853 - }, - { - "x": 1.1886846128926962, - "y": 1.7302702159613776, - "heading": -4.365253326644485e-18, - "angularVelocity": 5.453207120625985e-18, - "velocityX": 0.25547645841217265, - "velocityY": -0.6698532966858195, - "timestamp": 2.2336563848012596 - }, - { - "x": 1.2024920726171215, - "y": 1.6830444754865612, - "heading": -3.8567761493787476e-18, - "angularVelocity": 7.502330767109902e-18, - "velocityX": 0.20372229636848332, - "velocityY": -0.6967926388524772, - "timestamp": 2.301432273697834 - }, - { - "x": 1.2126819581312744, - "y": 1.6342216471211168, - "heading": -3.4963000828679734e-18, - "angularVelocity": 5.318647051824287e-18, - "velocityX": 0.1503467631343443, - "velocityY": -0.720356887387306, - "timestamp": 2.369208162594408 - }, - { - "x": 1.2191510957893172, - "y": 1.5840595772212502, - "heading": -3.2938741554653124e-18, - "angularVelocity": 2.986694853995746e-18, - "velocityX": 0.0954489533573625, - "velocityY": -0.7401167393970102, - "timestamp": 2.4369840514909824 - }, - { - "x": 1.2218081103679437, - "y": 1.5328461987368172, - "heading": -3.259573872067373e-18, - "angularVelocity": 5.060835243168827e-19, - "velocityX": 0.03920294697544978, - "velocityY": -0.7556282819482338, - "timestamp": 2.5047599403875567 - }, - { - "x": 1.220579396344816, - "y": 1.4808988958720275, - "heading": -3.3453058153274248e-18, - "angularVelocity": -1.2649331292104237e-18, - "velocityX": -0.018129072788743095, - "velocityY": -0.7664569762273552, - "timestamp": 2.572535829284131 - }, - { - "x": 1.2154155233457054, - "y": 1.4285616503210614, - "heading": -3.618440035746402e-18, - "angularVelocity": -4.029961768868618e-18, - "velocityX": -0.07619041348156665, - "velocityY": -0.7722103893145882, - "timestamp": 2.640311718180705 - }, - { - "x": 1.2062972964706975, - "y": 1.3761995720847502, - "heading": -4.087599432286243e-18, - "angularVelocity": -6.9222164854494395e-18, - "velocityX": -0.13453496550848654, - "velocityY": -0.7725767834076737, - "timestamp": 2.7080876070772795 - }, - { - "x": 1.1932404566060013, - "y": 1.3241908475189814, - "heading": -3.555359678860336e-18, - "angularVelocity": 7.852936361221584e-18, - "velocityX": -0.19264726847951472, - "velocityY": -0.7673632233010427, - "timestamp": 2.7758634959738537 - }, - { - "x": 1.1762980179515987, - "y": 1.2729167338053733, - "heading": -3.2328805268101095e-18, - "angularVelocity": 4.7580215665228305e-18, - "velocityX": -0.24997737293061145, - "velocityY": -0.7565244004671337, - "timestamp": 2.843639384870428 - }, - { - "x": 1.155559596070278, - "y": 1.2227507908644146, - "heading": -3.1247964605575482e-18, - "angularVelocity": 1.5947273141037222e-18, - "velocityX": -0.3059852437046957, - "velocityY": -0.7401738842187882, - "timestamp": 2.9114152737670023 - }, - { - "x": 1.131147710143389, - "y": 1.1740488128086175, - "heading": -3.234068776048195e-18, - "angularVelocity": -1.612259410180699e-18, - "velocityX": -0.36018540404746696, - "velocityY": -0.7185738003394676, - "timestamp": 2.9791911626635765 - }, - { - "x": 1.1032117091845473, - "y": 1.127140753581476, - "heading": -2.267931270616541e-18, - "angularVelocity": 1.4254885098319373e-17, - "velocityX": -0.4121819929425308, - "velocityY": -0.6921054078497251, - "timestamp": 3.046967051560151 - }, - { - "x": 1.0719204023306301, - "y": 1.0823254089032228, - "heading": -1.2961485271050903e-18, - "angularVelocity": 1.4338177791253964e-17, - "velocityX": -0.4616878858153752, - "velocityY": -0.6612284310522442, - "timestamp": 3.114742940456725 - }, - { - "x": 1.0374545343277675, - "y": 1.0398679644991287, - "heading": -5.410966813185065e-19, - "angularVelocity": 1.1140419716282834e-17, - "velocityX": -0.5085269786052852, - "velocityY": -0.6264387689386715, - "timestamp": 3.1825188293532993 - }, - { - "x": 1, - "y": 1, - "heading": 0, - "angularVelocity": 7.983616269770876e-18, - "velocityX": -0.5526232844385539, - "velocityY": -0.5882322629507188, - "timestamp": 3.2502947182498736 - }, - { - "x": 0.9601002931841937, - "y": 0.9632066842447741, - "heading": 3.2977160442109246e-19, - "angularVelocity": 4.906429958430405e-18, - "velocityX": -0.5936384674356385, - "velocityY": -0.547420753682207, - "timestamp": 3.3175068498830584 - }, - { - "x": 0.9176402744830485, - "y": 0.9293405687986124, - "heading": 1.3352918677177412e-18, - "angularVelocity": 1.4960397928012193e-17, - "velocityX": -0.6317314697423027, - "velocityY": -0.5038690876669165, - "timestamp": 3.3847189815162433 - }, - { - "x": 0.8728479847439143, - "y": 0.8985863996924396, - "heading": 2.1525838940809656e-18, - "angularVelocity": 1.2159888120347238e-17, - "velocityX": -0.6664316195711695, - "velocityY": -0.4575687209865014, - "timestamp": 3.451931113149428 - }, - { - "x": 0.8259849242635205, - "y": 0.8711239788151253, - "heading": 2.7942768738430748e-18, - "angularVelocity": 9.547278761162172e-18, - "velocityX": -0.6972410983206457, - "velocityY": -0.4085932139035895, - "timestamp": 3.519143244782613 - }, - { - "x": 0.7773460963851867, - "y": 0.8471213769660901, - "heading": 3.275531455850251e-18, - "angularVelocity": 7.160233562268939e-18, - "velocityX": -0.7236614387382273, - "velocityY": -0.35711710469221697, - "timestamp": 3.586355376415798 - }, - { - "x": 0.7272575825238243, - "y": 0.8267273909004392, - "heading": 3.656107749997039e-18, - "angularVelocity": 5.662315468533709e-18, - "velocityX": -0.7452302529954558, - "velocityY": -0.3034271577183785, - "timestamp": 3.6535675080489827 - }, - { - "x": 0.6760712290601476, - "y": 0.8100641481084726, - "heading": 3.9141657523662e-18, - "angularVelocity": 3.839455683091591e-18, - "velocityX": -0.7615642030672349, - "velocityY": -0.2479201654086435, - "timestamp": 3.7207796396821675 - }, - { - "x": 0.6241565565906713, - "y": 0.7972210163794645, - "heading": 3.9620361753801646e-18, - "angularVelocity": 7.122289175726647e-19, - "velocityX": -0.7724003272624098, - "velocityY": -0.19108353532216987, - "timestamp": 3.7879917713153524 - }, - { - "x": 0.5718906668220771, - "y": 0.7882509123495861, - "heading": 3.933061205617742e-18, - "angularVelocity": -4.310973365379806e-19, - "velocityX": -0.777625831804279, - "velocityY": -0.1334595974255566, - "timestamp": 3.855203902948537 - }, - { - "x": 0.5196474628149044, - "y": 0.7831696619611915, - "heading": 3.841828847804768e-18, - "angularVelocity": -1.3573793776640569e-18, - "velocityX": -0.7772883070022795, - "velocityY": -0.07560019694250741, - "timestamp": 3.922416034581722 - }, - { - "x": 0.46778766247295284, - "y": 0.7819583856222608, - "heading": 3.2741304171392934e-18, - "angularVelocity": -8.446368348915311e-18, - "velocityX": -0.7715839251309572, - "velocityY": -0.018021692059127074, - "timestamp": 3.989628166214907 - }, - { - "x": 0.41665080107502755, - "y": 0.784568242242575, - "heading": 2.3398005355380153e-18, - "angularVelocity": -1.390120864955836e-17, - "velocityX": -0.760827846928116, - "velocityY": 0.03883014207252925, - "timestamp": 4.056840297848092 - }, - { - "x": 0.3665498423457967, - "y": 0.7909265123693979, - "heading": 1.4204312415958456e-18, - "angularVelocity": -1.3678621115538104e-17, - "velocityX": -0.7454154110549615, - "velocityY": 0.09460003681364573, - "timestamp": 4.124052429481277 - }, - { - "x": 0.3177684131174127, - "y": 0.8009429979668188, - "heading": 2.911000402406795e-19, - "angularVelocity": -1.6802490157769785e-17, - "velocityX": -0.7257830996138033, - "velocityY": 0.14902794114738804, - "timestamp": 4.191264561114462 - }, - { - "x": 0.2705602477366799, - "y": 0.8145159621306034, - "heading": -8.90875593988358e-21, - "angularVelocity": -4.4636106745506625e-18, - "velocityX": -0.7023756609651165, - "velocityY": 0.201942176716847, - "timestamp": 4.258476692747647 - }, - { - "x": 0.22515023866998407, - "y": 0.8315371625385247, - "heading": 8.015184743221423e-21, - "angularVelocity": 2.5179924008277626e-19, - "velocityX": -0.6756222122893548, - "velocityY": 0.25324595418005347, - "timestamp": 4.325688824380832 - }, - { - "x": 0.1817364922151659, - "y": 0.8518958223545926, - "heading": -1.0221415362403761e-19, - "angularVelocity": -1.6400213737530966e-18, - "velocityX": -0.6459212853380655, - "velocityY": 0.30290156436603854, - "timestamp": 4.392900956014016 - }, - { - "x": 0.14049289628323774, - "y": 0.8754815778933299, - "heading": 1.279799371183032e-19, - "angularVelocity": 3.4248888768950546e-18, - "velocityX": -0.6136332077223501, - "velocityY": 0.35091515423820036, - "timestamp": 4.460113087647201 - }, - { - "x": 0.1015718469138536, - "y": 0.9021865445377336, - "heading": 2.2387831921490547e-19, - "angularVelocity": 1.4268016689731864e-18, - "velocityX": -0.5790777412297899, - "velocityY": 0.39732360803772554, - "timestamp": 4.527325219280386 - }, - { - "x": 0.06510690798404553, - "y": 0.931906676568605, - "heading": -1.120819147896167e-19, - "angularVelocity": -4.998505735207955e-18, - "velocityX": -0.542535075792835, - "velocityY": 0.4421840419088505, - "timestamp": 4.594537350913571 - }, - { - "x": 0.031215277251384486, - "y": 0.9645425908347474, - "heading": -3.927086503171987e-19, - "angularVelocity": -4.1752392884434696e-18, - "velocityX": -0.5042487108968212, - "velocityY": 0.4855658267804262, - "timestamp": 4.661749482546756 - }, - { - "x": 0, - "y": 1, - "heading": 0, - "angularVelocity": 5.84282387256885e-18, - "velocityX": -0.46442921081188193, - "velocityY": 0.527544779546109, - "timestamp": 4.728961614179941 - }, - { - "x": -0.034523228131504415, - "y": 1.0482920245716583, - "heading": 6.961793842243958e-19, - "angularVelocity": 8.333545907574826e-18, - "velocityX": -0.4132568741634686, - "velocityY": 0.5780748846976158, - "timestamp": 4.81250100648016 - }, - { - "x": -0.06466937577608543, - "y": 1.1006992615641917, - "heading": 2.448822269391661e-18, - "angularVelocity": 2.0979837004191778e-17, - "velocityX": -0.3608614668424137, - "velocityY": 0.6273356263377532, - "timestamp": 4.896040398780379 - }, - { - "x": -0.09031335806620472, - "y": 1.1570844769783872, - "heading": 3.582191703035754e-18, - "angularVelocity": 1.3566885979268263e-17, - "velocityX": -0.30696874353552017, - "velocityY": 0.6749536220177591, - "timestamp": 4.979579791080598 - }, - { - "x": -0.11129933776190112, - "y": 1.217264005888493, - "heading": 4.527037428957119e-18, - "angularVelocity": 1.1310181537234104e-17, - "velocityX": -0.2512105860224372, - "velocityY": 0.7203730749421273, - "timestamp": 5.063119183380817 - }, - { - "x": -0.12742973186409806, - "y": 1.2809814846007999, - "heading": 5.3660105917774544e-18, - "angularVelocity": 1.0042844899524535e-17, - "velocityX": -0.19308728083906038, - "velocityY": 0.7627237517280782, - "timestamp": 5.146658575681036 - }, - { - "x": -0.13845089687734538, - "y": 1.3478595921295984, - "heading": 5.804477769789897e-18, - "angularVelocity": 5.248628037181788e-18, - "velocityX": -0.13192776138040377, - "velocityY": 0.8005577451228699, - "timestamp": 5.230197967981255 - }, - { - "x": -0.14404059723021467, - "y": 1.41730538004403, - "heading": 5.60459161147457e-18, - "angularVelocity": -2.3927171307864518e-18, - "velocityX": -0.06691095301221847, - "velocityY": 0.8312939082063359, - "timestamp": 5.313737360281474 - }, - { - "x": -0.14383032525510003, - "y": 1.488317725972261, - "heading": 4.431739846980368e-18, - "angularVelocity": -1.40395049470865e-17, - "velocityX": 0.002517039797931534, - "velocityY": 0.8500462353500342, - "timestamp": 5.397276752581693 - }, - { - "x": -0.13761503207018486, - "y": 1.559147061402284, - "heading": 3.662869249400604e-18, - "angularVelocity": -9.203688693849832e-18, - "velocityX": 0.07439954988634583, - "velocityY": 0.8478555263543226, - "timestamp": 5.480816144881912 - }, - { - "x": -0.12607515499835087, - "y": 1.627193849249961, - "heading": 1.3243020605488167e-18, - "angularVelocity": -2.7993585531762966e-17, - "velocityX": 0.1381369525691848, - "velocityY": 0.8145473168290962, - "timestamp": 5.564355537182131 - }, - { - "x": -0.11116381180741976, - "y": 1.690268013573467, - "heading": 2.379879606606124e-19, - "angularVelocity": -1.3003614752838011e-17, - "velocityX": 0.17849475295850228, - "velocityY": 0.7550230207185785, - "timestamp": 5.64789492948235 - }, - { - "x": -0.09482890835601043, - "y": 1.747505461760242, - "heading": -2.2107636185603513e-19, - "angularVelocity": -5.4951837817255995e-18, - "velocityX": 0.1955353396958621, - "velocityY": 0.6851551897945106, - "timestamp": 5.731434321782569 - }, - { - "x": -0.07835937233604903, - "y": 1.7987366080747376, - "heading": -5.60981270871949e-19, - "angularVelocity": -4.068797903654375e-18, - "velocityX": 0.19714694548859107, - "velocityY": 0.6132573496630682, - "timestamp": 5.814973714082788 - }, - { - "x": -0.06255809380104226, - "y": 1.843997224564569, - "heading": -8.808563650504602e-19, - "angularVelocity": -3.8290331719286885e-18, - "velocityX": 0.18914763562345505, - "velocityY": 0.5417877152754039, - "timestamp": 5.898513106383007 - }, - { - "x": -0.04794693255938144, - "y": 1.8833690873915618, - "heading": -1.2134869024792098e-18, - "angularVelocity": -3.981720931889883e-18, - "velocityX": 0.17490145474307478, - "velocityY": 0.4712969743124334, - "timestamp": 5.982052498683226 - }, - { - "x": -0.034884553374127325, - "y": 1.9169362042125617, - "heading": -1.5815565915485662e-18, - "angularVelocity": -4.405941868057666e-18, - "velocityX": 0.1563619129321798, - "velocityY": 0.40181183866370773, - "timestamp": 6.065591890983445 - }, - { - "x": -0.0236298825386545, - "y": 1.9447739236006856, - "heading": -7.689111560877529e-19, - "angularVelocity": 9.727690984238907e-18, - "velocityX": 0.13472291963803598, - "velocityY": 0.3332286556273034, - "timestamp": 6.149131283283664 - }, - { - "x": -0.01437751052078003, - "y": 1.9669473155185608, - "heading": -4.414002390832506e-19, - "angularVelocity": 3.9204366617451644e-18, - "velocityX": 0.11075460047188025, - "velocityY": 0.26542438611702496, - "timestamp": 6.232670675583883 - }, - { - "x": -0.007278507844366605, - "y": 1.9835120705105598, - "heading": -6.316031204453377e-19, - "angularVelocity": -2.2768046075674146e-18, - "velocityX": 0.08497790660125235, - "velocityY": 0.19828675473804738, - "timestamp": 6.316210067884102 - }, - { - "x": -0.0024533007608353937, - "y": 1.9945159206791379, - "heading": -4.823898230322713e-19, - "angularVelocity": 1.786142874866039e-18, - "velocityX": 0.05775966224641234, - "velocityY": 0.13172049575166966, - "timestamp": 6.399749460184321 - }, - { - "x": -9.534714777782856e-35, - "y": 2, - "heading": -4.51332949370556e-37, - "angularVelocity": 5.774399435321978e-18, - "velocityX": 0.029366993142814022, - "velocityY": 0.06564662693683221, - "timestamp": 6.48328885248454 - }, - { - "x": 0, - "y": 2, - "heading": -6.22058063415872e-36, - "angularVelocity": -6.906021002654526e-35, - "velocityX": 1.1413436685370076e-33, - "velocityY": -9.106343514179098e-35, - "timestamp": 6.566828244784759 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGCSprint.1.traj b/src/main/deploy/choreo/SourceLanePGCSprint.1.traj index 87faabce..a5800136 100644 --- a/src/main/deploy/choreo/SourceLanePGCSprint.1.traj +++ b/src/main/deploy/choreo/SourceLanePGCSprint.1.traj @@ -1,184 +1,185 @@ { - "samples": [ - { - "x": 0.433241993188858, - "y": 4.121134281158447, - "heading": 9.759657159056985e-29, - "angularVelocity": 1.5485227916478622e-29, - "velocityX": -1.3617494152189479e-27, - "velocityY": -5.3557690609839536e-27, - "timestamp": 0 - }, - { - "x": 0.4546918803213756, - "y": 4.109218526997661, - "heading": -0.008937300145995024, - "angularVelocity": -0.11893237262991248, - "velocityX": 0.28544257523423766, - "velocityY": -0.15856789979825608, - "timestamp": 0.07514606787342623 - }, - { - "x": 0.4975916635116252, - "y": 4.0853873108981436, - "heading": -0.02681241329663613, - "angularVelocity": -0.23787157008333967, - "velocityX": 0.5708852692400183, - "velocityY": -0.3171319108760033, - "timestamp": 0.15029213574685246 - }, - { - "x": 0.5619414537480726, - "y": 4.049641027140498, - "heading": -0.05362171709802378, - "angularVelocity": -0.3567625633658495, - "velocityX": 0.8563294402155045, - "velocityY": -0.4756906750976654, - "timestamp": 0.2254382036202787 - }, - { - "x": 0.6477414523773108, - "y": 4.001980177860296, - "heading": -0.08935853104911338, - "angularVelocity": -0.4755646564406219, - "velocityX": 1.1417762905939028, - "velocityY": -0.6342427571923064, - "timestamp": 0.3005842714937049 - }, - { - "x": 0.7549919358777417, - "y": 3.9424053758532884, - "heading": -0.13401442200652888, - "angularVelocity": -0.5942545261667251, - "velocityX": 1.4272268201854599, - "velocityY": -0.7927866845588466, - "timestamp": 0.3757303393671312 - }, - { - "x": 0.8836932393269187, - "y": 3.8709173416225426, - "heading": -0.18758057158075359, - "angularVelocity": -0.7128270459134325, - "velocityX": 1.7126818087934759, - "velocityY": -0.9513210238912064, - "timestamp": 0.45087640724055744 - }, - { - "x": 1.0338457421402332, - "y": 3.7875168932975942, - "heading": -0.2500489995023012, - "angularVelocity": -0.8312933688928013, - "velocityX": 1.9981418464400145, - "velocityY": -1.1098444760333412, - "timestamp": 0.5260224751139837 - }, - { - "x": 1.2054498603190875, - "y": 3.692204931169302, - "heading": -0.3214134496146474, - "angularVelocity": -0.9496764385935714, - "velocityX": 2.283607419990346, - "velocityY": -1.268355947630322, - "timestamp": 0.60116854298741 - }, - { - "x": 1.3985060453231266, - "y": 3.584982424979261, - "heading": -0.40166985798592575, - "angularVelocity": -1.0680054278616389, - "velocityX": 2.569079001302066, - "velocityY": -1.4268545144723166, - "timestamp": 0.6763146108608362 - }, - { - "x": 1.5915937569821017, - "y": 3.4778198154599655, - "heading": -0.48176859267037747, - "angularVelocity": -1.0659071984893158, - "velocityX": 2.5694985396202785, - "velocityY": -1.4260574445464886, - "timestamp": 0.7514606787342625 - }, - { - "x": 1.7632284858900344, - "y": 3.382566036520045, - "heading": -0.552976547680448, - "angularVelocity": -0.9475938931363788, - "velocityX": 2.2840147697019764, - "velocityY": -1.2675816797275836, - "timestamp": 0.8266067466076887 - }, - { - "x": 1.9134098098562773, - "y": 3.299220400739088, - "heading": -0.6152918418017485, - "angularVelocity": -0.8292555536806328, - "velocityX": 1.9985253815170176, - "velocityY": -1.1091150626981823, - "timestamp": 0.901752814481115 - }, - { - "x": 2.04213735776467, - "y": 3.2277823059512354, - "heading": -0.6687115003742338, - "angularVelocity": -0.7108776291856513, - "velocityX": 1.7130310547348613, - "velocityY": -0.9506564589404838, - "timestamp": 0.9768988823545413 - }, - { - "x": 2.1494108084857926, - "y": 3.1682512394298143, - "heading": -0.7132320418113052, - "angularVelocity": -0.5924533737688112, - "velocityX": 1.4275324545498602, - "velocityY": -0.7922046782500088, - "timestamp": 1.0520449502279674 - }, - { - "x": 2.235229893671302, - "y": 3.120626784362015, - "heading": -0.7488501306819726, - "angularVelocity": -0.4739847323836216, - "velocityX": 1.142030283341778, - "velocityY": -0.6337584442610695, - "timestamp": 1.1271910181013935 - }, - { - "x": 2.2995943987852447, - "y": 3.0849086278583613, - "heading": -0.7755632624607874, - "angularVelocity": -0.35548276223593744, - "velocityX": 0.8565252572144713, - "velocityY": -0.4753163740226105, - "timestamp": 1.2023370859748197 - }, - { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080856, - "angularVelocity": -0.2369668826490278, - "velocityX": 0.5710180813549185, - "velocityY": -0.31687698560575434, - "timestamp": 1.2774831538482458 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550668, - "velocityX": 0.28550943973314763, - "velocityY": -0.15843872456938438, - "timestamp": 1.352629221721672 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -1.820153695476826e-18, - "velocityX": -5.7963239407122954e-18, - "velocityY": -1.1530643297922074e-18, - "timestamp": 1.4277752895950981 - } - ] + "samples": [ + { + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": 7.738396447124189e-29, + "angularVelocity": 1.1248344329069217e-28, + "velocityX": -3.4263332588426173e-28, + "velocityY": 2.738469239483736e-27, + "timestamp": 0 + }, + { + "x": 0.4546918803218127, + "y": 4.109218526998405, + "heading": -0.00893730014505659, + "angularVelocity": -0.11893237232009289, + "velocityX": 0.28544257452644645, + "velocityY": -0.1585678993919355, + "timestamp": 0.07514606806129173 + }, + { + "x": 0.49759166351299144, + "y": 4.085387310900469, + "heading": -0.026812413293685552, + "angularVelocity": -0.23787156946188326, + "velocityX": 0.5708852678251672, + "velocityY": -0.3171319100621368, + "timestamp": 0.15029213612258346 + }, + { + "x": 0.5619414537509306, + "y": 4.049641027145361, + "heading": -0.05362171709181762, + "angularVelocity": -0.35676256243061816, + "velocityX": 0.8563294380945293, + "velocityY": -0.47569067387467856, + "timestamp": 0.2254382041838752 + }, + { + "x": 0.6477414523823173, + "y": 4.001980177868811, + "heading": -0.08935853103818607, + "angularVelocity": -0.4755646551888817, + "velocityX": 1.1417762877680475, + "velocityY": -0.634242755558099, + "timestamp": 0.30058427224516693 + }, + { + "x": 0.7549919358856848, + "y": 3.942405375866793, + "heading": -0.13401442198910787, + "angularVelocity": -0.5942545245946725, + "velocityX": 1.4272268166564648, + "velocityY": -0.7927866825104752, + "timestamp": 0.3757303403064587 + }, + { + "x": 0.8836932393387835, + "y": 3.870917341642709, + "heading": -0.18758057155461047, + "angularVelocity": -0.7128270440152931, + "velocityX": 1.7126818045639527, + "velocityY": -0.9513210214242487, + "timestamp": 0.45087640836775045 + }, + { + "x": 1.0338457421573335, + "y": 3.7875168933266514, + "heading": -0.2500489994644535, + "angularVelocity": -0.8312933666588067, + "velocityX": 1.998141841514319, + "velocityY": -1.1098444731404118, + "timestamp": 0.5260224764290422 + }, + { + "x": 1.2054498603433927, + "y": 3.692204931210594, + "heading": -0.3214134495606155, + "angularVelocity": -0.9496764360040053, + "velocityX": 2.2836074143771987, + "velocityY": -1.2683559442966215, + "timestamp": 0.601168544490334 + }, + { + "x": 1.3985060453585743, + "y": 3.5849824250394744, + "heading": -0.4016698579067676, + "angularVelocity": -1.068005424857256, + "velocityX": 2.569078995027635, + "velocityY": -1.4268545106533708, + "timestamp": 0.6763146125516257 + }, + { + "x": 1.5915937570064054, + "y": 3.477819815501255, + "heading": -0.4817685926160567, + "angularVelocity": -1.0659071961550632, + "velocityX": 2.5694985330482245, + "velocityY": -1.426057441233171, + "timestamp": 0.7514606806129175 + }, + { + "x": 1.7632284859071354, + "y": 3.3825660365490977, + "heading": -0.5529765476421505, + "angularVelocity": -0.9475938909806194, + "velocityX": 2.2840147638960824, + "velocityY": -1.2675816767214638, + "timestamp": 0.8266067486742092 + }, + { + "x": 1.9134098098681454, + "y": 3.2992204007592485, + "heading": -0.6152918417750989, + "angularVelocity": -0.8292555517624935, + "velocityX": 1.9985253764510553, + "velocityY": -1.1091150600437194, + "timestamp": 0.901752816735501 + }, + { + "x": 2.042137357772617, + "y": 3.227782305964734, + "heading": -0.6687115003563293, + "angularVelocity": -0.7108776275248302, + "velocityX": 1.7130310504001012, + "velocityY": -0.9506564566524899, + "timestamp": 0.9768988847967928 + }, + { + "x": 2.1494108084908032, + "y": 3.168251239438324, + "heading": -0.7132320417999736, + "angularVelocity": -0.5924533723751416, + "velocityX": 1.4275324509419445, + "velocityY": -0.792204676335884, + "timestamp": 1.0520449528580844 + }, + { + "x": 2.2352298936741635, + "y": 3.120626784366874, + "heading": -0.7488501306754739, + "angularVelocity": -0.4739847312629708, + "velocityX": 1.1420302804580957, + "velocityY": -0.6337584427252518, + "timestamp": 1.127191020919376 + }, + { + "x": 2.299594398786613, + "y": 3.0849086278606843, + "heading": -0.7755632624576652, + "angularVelocity": -0.3554827613921602, + "velocityX": 0.8565252550532846, + "velocityY": -0.475316372868058, + "timestamp": 1.2023370889806677 + }, + { + "x": 2.3425041622841327, + "y": 3.061096568391248, + "heading": -0.7933703919070813, + "angularVelocity": -0.2369668820847947, + "velocityX": 0.5710180799149875, + "velocityY": -0.31687698483458104, + "timestamp": 1.2774831570419594 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288571271453, + "velocityX": 0.2855094390135433, + "velocityY": -0.1584387241831784, + "timestamp": 1.352629225103251 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 1.9944978930198404e-24, + "velocityX": 7.05107927160795e-24, + "velocityY": 9.089761159537798e-24, + "timestamp": 1.4277752931645427 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGCSprint.2.traj b/src/main/deploy/choreo/SourceLanePGCSprint.2.traj index 38e969e1..9de49f5f 100644 --- a/src/main/deploy/choreo/SourceLanePGCSprint.2.traj +++ b/src/main/deploy/choreo/SourceLanePGCSprint.2.traj @@ -1,841 +1,842 @@ { - "samples": [ - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -1.820153695476826e-18, - "velocityX": -5.7963239407122954e-18, - "velocityY": -1.1530643297922074e-18, - "timestamp": 0 - }, - { - "x": 2.37735740952237, - "y": 3.0408026970784268, - "heading": -0.7860869247798871, - "angularVelocity": 0.2648554274741457, - "velocityX": 0.21924714610297322, - "velocityY": -0.1372563412236913, - "timestamp": 0.061110649511906034 - }, - { - "x": 2.404179793787175, - "y": 3.024019444447085, - "heading": -0.7541504516362038, - "angularVelocity": 0.5226007806947081, - "velocityX": 0.4389150578342217, - "velocityY": -0.2746371175137346, - "timestamp": 0.12222129902381207 - }, - { - "x": 2.4444554974169383, - "y": 2.998832316814256, - "heading": -0.7069723408476191, - "angularVelocity": 0.7720112806098135, - "velocityX": 0.6590619466729163, - "velocityY": -0.41215611082519754, - "timestamp": 0.1833319485357181 - }, - { - "x": 2.4982188560395806, - "y": 2.96523040511413, - "heading": -0.645190074686331, - "angularVelocity": 1.0109901736398696, - "velocityX": 0.8797706954852063, - "velocityY": -0.5498536174710421, - "timestamp": 0.24444259804762414 - }, - { - "x": 2.5655102024042766, - "y": 2.923198127539445, - "heading": -0.5696285601591381, - "angularVelocity": 1.2364704864161304, - "velocityX": 1.101139439723772, - "velocityY": -0.6878061010707456, - "timestamp": 0.30555324755953017 - }, - { - "x": 2.646375841205288, - "y": 2.8727129664624207, - "heading": -0.48135684674506, - "angularVelocity": 1.4444571301255718, - "velocityX": 1.3232659028645477, - "velocityY": -0.8261270577264651, - "timestamp": 0.3666638970714362 - }, - { - "x": 2.740867207337065, - "y": 2.813744425609022, - "heading": -0.38173698900245784, - "angularVelocity": 1.6301554399809315, - "velocityX": 1.5462340342720005, - "velocityY": -0.9649470480903679, - "timestamp": 0.42777454658334224 - }, - { - "x": 2.849040211493812, - "y": 2.746255741762665, - "heading": -0.2724850390795677, - "angularVelocity": 1.7877726843928354, - "velocityX": 1.7701170748589676, - "velocityY": -1.1043686229060403, - "timestamp": 0.4888851960952483 - }, - { - "x": 2.9709557136972373, - "y": 2.6702088910620962, - "heading": -0.155828372393994, - "angularVelocity": 1.9089416921161393, - "velocityX": 1.994996014232719, - "velocityY": -1.2444124110602532, - "timestamp": 0.5499958456071543 - }, - { - "x": 3.1066769296559538, - "y": 2.585572206493352, - "heading": -0.03496320783780511, - "angularVelocity": 1.977808541089754, - "velocityX": 2.2209094002882996, - "velocityY": -1.3849743906298195, - "timestamp": 0.6111064951190603 - }, - { - "x": 3.2562355055408387, - "y": 2.4923383100856262, - "heading": 0.08475597327951304, - "angularVelocity": 1.959055943170691, - "velocityX": 2.4473406366879895, - "velocityY": -1.5256571015427038, - "timestamp": 0.6722171446309666 - }, - { - "x": 3.4194188611023764, - "y": 2.3906298265628587, - "heading": 0.19293288100296513, - "angularVelocity": 1.7701809518876104, - "velocityX": 2.6702932609110017, - "velocityY": -1.6643332109070865, - "timestamp": 0.7333277941428729 - }, - { - "x": 3.5932872633720114, - "y": 2.2801851899216916, - "heading": 0.26578166446591744, - "angularVelocity": 1.192080006427692, - "velocityX": 2.845140800471438, - "velocityY": -1.8072895235657866, - "timestamp": 0.7944384436547791 - }, - { - "x": 3.7772765002319786, - "y": 2.1608722925529302, - "heading": 0.30020319888903974, - "angularVelocity": 0.5632657269731189, - "velocityX": 3.0107557083666587, - "velocityY": -1.9524076134310657, - "timestamp": 0.8555490931666854 - }, - { - "x": 3.9732507395108834, - "y": 2.038358982543431, - "heading": 0.30020324669419357, - "angularVelocity": 7.822720631834703e-7, - "velocityX": 3.2068754111462736, - "velocityY": -2.0047783976771947, - "timestamp": 0.9166597426785916 - }, - { - "x": 4.174667645417682, - "y": 1.9250159473015482, - "heading": 0.3002032939445946, - "angularVelocity": 7.731942209741845e-7, - "velocityX": 3.2959379014218597, - "velocityY": -1.8547182225546543, - "timestamp": 0.9777703921904979 - }, - { - "x": 4.383569049898422, - "y": 1.8261442295125712, - "heading": 0.30020334201253857, - "angularVelocity": 7.865722968351813e-7, - "velocityX": 3.4184124395542623, - "velocityY": -1.6179130573585891, - "timestamp": 1.0388810417024041 - }, - { - "x": 4.59891708073721, - "y": 1.7422357169202292, - "heading": 0.3002033920988207, - "angularVelocity": 8.195998987503142e-7, - "velocityX": 3.52390348587004, - "velocityY": -1.3730587591937538, - "timestamp": 1.0999916912143104 - }, - { - "x": 4.819641568305972, - "y": 1.6737075280608862, - "heading": 0.30020344564176327, - "angularVelocity": 8.761638607213102e-7, - "velocityX": 3.6118825332687328, - "velocityY": -1.1213788334223447, - "timestamp": 1.1611023407262167 - }, - { - "x": 5.044645579459654, - "y": 1.6209002757550093, - "heading": 0.30020350452029126, - "angularVelocity": 9.634740995599556e-7, - "velocityX": 3.681911629982681, - "velocityY": -0.8641252012153561, - "timestamp": 1.222212990238123 - }, - { - "x": 5.272810899090047, - "y": 1.584076421741111, - "heading": 0.3002035713939988, - "angularVelocity": 0.0000010943052979537505, - "velocityX": 3.733642523075129, - "velocityY": -0.6025767081190017, - "timestamp": 1.2833236397500292 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.3002036452391599, - "angularVelocity": 0.0000012083844913604929, - "velocityX": 3.7668180588495743, - "velocityY": -0.33803334595452383, - "timestamp": 1.3444342892619354 - }, - { - "x": 5.642203177619129, - "y": 1.5568731039887571, - "heading": 0.3002037251666547, - "angularVelocity": 0.0000021691766198010436, - "velocityX": 3.7777797970827858, - "velocityY": -0.177650642031358, - "timestamp": 1.3812812173086941 - }, - { - "x": 5.781555187145968, - "y": 1.5562486567630798, - "heading": 0.30020379895395954, - "angularVelocity": 0.0000020025361337225055, - "velocityX": 3.78191661866635, - "velocityY": -0.016947063399286564, - "timestamp": 1.4181281453554528 - }, - { - "x": 5.9208078464820115, - "y": 1.5615467685843438, - "heading": 0.30020386782175307, - "angularVelocity": 0.0000018690240183201108, - "velocityX": 3.7792203235866277, - "velocityY": 0.14378706997068585, - "timestamp": 1.4549750734022115 - }, - { - "x": 6.059709556234372, - "y": 1.5727578639536914, - "heading": 0.3002039327408759, - "angularVelocity": 0.0000017618598429234572, - "velocityX": 3.7696957959723445, - "velocityY": 0.30426133096145186, - "timestamp": 1.4918220014489703 - }, - { - "x": 6.198009351638548, - "y": 1.5898616832743162, - "heading": 0.30020399449987484, - "angularVelocity": 0.0000016760962775678482, - "velocityX": 3.753360259196494, - "velocityY": 0.46418576058553174, - "timestamp": 1.528668929495729 - }, - { - "x": 6.335457356092555, - "y": 1.6128273194464042, - "heading": 0.30020405375523124, - "angularVelocity": 0.0000016081491612309129, - "velocityX": 3.730243245233006, - "velocityY": 0.6232713930166671, - "timestamp": 1.5655158575424877 - }, - { - "x": 6.471805232718414, - "y": 1.6416132738069629, - "heading": 0.3002041110675489, - "angularVelocity": 0.0000015554164405322294, - "velocityX": 3.700386541120455, - "velocityY": 0.7812307805966762, - "timestamp": 1.6023627855892464 - }, - { - "x": 6.60680663253268, - "y": 1.676167531284266, - "heading": 0.3002041691027596, - "angularVelocity": 0.0000015750352547385845, - "velocityX": 3.6638440969339445, - "velocityY": 0.937778515306732, - "timestamp": 1.639209713636005 - }, - { - "x": 6.740176057879373, - "y": 1.7164157557118485, - "heading": 0.3003755003099619, - "angularVelocity": 0.004649809801927131, - "velocityX": 3.6195534449289073, - "velocityY": 1.0923088181600296, - "timestamp": 1.6760566416827638 - }, - { - "x": 6.869837525657242, - "y": 1.7607930294165437, - "heading": 0.3093430143729343, - "angularVelocity": 0.2433720947264084, - "velocityX": 3.5189220554106844, - "velocityY": 1.2043683437702197, - "timestamp": 1.7129035697295225 - }, - { - "x": 6.995165288352465, - "y": 1.807928452248917, - "heading": 0.3301850856624388, - "angularVelocity": 0.5656393190514163, - "velocityX": 3.4013083135772337, - "velocityY": 1.2792225927914125, - "timestamp": 1.7497504977762812 - }, - { - "x": 7.114963240628345, - "y": 1.8557254140007795, - "heading": 0.35927322814233364, - "angularVelocity": 0.7894319559824341, - "velocityX": 3.2512331048020355, - "velocityY": 1.2971762989633377, - "timestamp": 1.78659742582304 - }, - { - "x": 7.228886146559884, - "y": 1.9032191557267804, - "heading": 0.3911508310575271, - "angularVelocity": 0.8651359721153189, - "velocityX": 3.091788433135363, - "velocityY": 1.2889471183522156, - "timestamp": 1.8234443538697986 - }, - { - "x": 7.336942556890349, - "y": 1.949894399055616, - "heading": 0.423151819771819, - "angularVelocity": 0.8684845768875223, - "velocityX": 2.9325758226938556, - "velocityY": 1.2667336411221237, - "timestamp": 1.8602912819165573 - }, - { - "x": 7.43919043499382, - "y": 1.9954619855368936, - "heading": 0.45375826572504047, - "angularVelocity": 0.8306376562622595, - "velocityX": 2.7749362979111796, - "velocityY": 1.2366726046592738, - "timestamp": 1.897138209963316 - }, - { - "x": 7.535687560612936, - "y": 2.0397400223416313, - "heading": 0.48197582868140687, - "angularVelocity": 0.7658050332054913, - "velocityX": 2.618864875157601, - "velocityY": 1.201675123325048, - "timestamp": 1.9339851380100748 - }, - { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.6818119458710672, - "velocityX": 2.464149969521805, - "velocityY": 1.163303151067446, - "timestamp": 1.9708320660568335 - }, - { - "x": 7.766065583230135, - "y": 2.1519617180113952, - "heading": 0.5396264377138885, - "angularVelocity": 0.5126079654708053, - "velocityX": 2.1996686083558368, - "velocityY": 1.0930061650047849, - "timestamp": 2.0342878363776813 - }, - { - "x": 7.889178097615864, - "y": 2.215679412144434, - "heading": 0.5626262771579723, - "angularVelocity": 0.3624546566496878, - "velocityX": 1.9401311143689866, - "velocityY": 1.004127659484183, - "timestamp": 2.097743606698529 - }, - { - "x": 7.996082408378713, - "y": 2.2729729622929304, - "heading": 0.5769453477724344, - "angularVelocity": 0.22565435014123394, - "velocityX": 1.6847059018008188, - "velocityY": 0.90288952224213, - "timestamp": 2.161199377019377 - }, - { - "x": 8.086990111713977, - "y": 2.3232868849538337, - "heading": 0.5832040110121259, - "angularVelocity": 0.09863032483958853, - "velocityX": 1.432615235393288, - "velocityY": 0.7928975159627525, - "timestamp": 2.2246551473402247 - }, - { - "x": 8.16207340516984, - "y": 2.3662086171292933, - "heading": 0.581874089552229, - "angularVelocity": -0.020958243090781536, - "velocityX": 1.183238231546559, - "velocityY": 0.676403926048603, - "timestamp": 2.2881109176610726 - }, - { - "x": 8.221474033046453, - "y": 2.401420292157025, - "heading": 0.5733258497363348, - "angularVelocity": -0.1347117807044565, - "velocityX": 0.9360949772773745, - "velocityY": 0.5549010728211649, - "timestamp": 2.3515666879819204 - }, - { - "x": 8.265310204045273, - "y": 2.4286697945211704, - "heading": 0.5578573342178689, - "angularVelocity": -0.24376846172150818, - "velocityX": 0.6908145748948221, - "velocityY": 0.4294251291311274, - "timestamp": 2.415022458302768 - }, - { - "x": 8.293681756385627, - "y": 2.447752418352113, - "heading": 0.5357134031725193, - "angularVelocity": -0.34896638924064666, - "velocityX": 0.44710752382171043, - "velocityY": 0.30072322397878726, - "timestamp": 2.478478228623616 - }, - { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.45094242234392296, - "velocityX": 0.20474492941705758, - "velocityY": 0.16935099752034305, - "timestamp": 2.541933998944464 - }, - { - "x": 8.303680612059921, - "y": 2.4605220907329786, - "heading": 0.4706360870687623, - "angularVelocity": -0.5539055745477068, - "velocityX": -0.04547302080206286, - "velocityY": 0.030737357256443772, - "timestamp": 2.6077618566137937 - }, - { - "x": 8.28417079095848, - "y": 2.4534909416021047, - "heading": 0.42765554839737563, - "angularVelocity": -0.6529232485020114, - "velocityX": -0.2963763639315537, - "velocityY": -0.1068111492583101, - "timestamp": 2.6735897142831235 - }, - { - "x": 8.248092493080108, - "y": 2.437487576077448, - "heading": 0.3784580606750378, - "angularVelocity": -0.7473657728536394, - "velocityX": -0.5480703634561933, - "velocityY": -0.24310931710774963, - "timestamp": 2.7394175719524534 - }, - { - "x": 8.195385029545998, - "y": 2.412609981570135, - "heading": 0.3233968459430461, - "angularVelocity": -0.8364424528074197, - "velocityX": -0.8006862960492162, - "velocityY": -0.37791894477683524, - "timestamp": 2.8052454296217832 - }, - { - "x": 8.125976752501963, - "y": 2.378976757579925, - "heading": 0.2628923564512202, - "angularVelocity": -0.9191319850595123, - "velocityX": -1.0543906410063166, - "velocityY": -0.5109269112046505, - "timestamp": 2.871073287291113 - }, - { - "x": 8.039781850826214, - "y": 2.336734306251694, - "heading": 0.19745501482460134, - "angularVelocity": -0.9940676173198235, - "velocityX": -1.3093985544650038, - "velocityY": -0.641710862602064, - "timestamp": 2.936901144960443 - }, - { - "x": 7.936695821305385, - "y": 2.286067717588345, - "heading": 0.12772072746128918, - "angularVelocity": -1.0593431084086873, - "velocityX": -1.565993990548153, - "velocityY": -0.7696830864200519, - "timestamp": 3.002729002629773 - }, - { - "x": 7.816588930914964, - "y": 2.2272180395232652, - "heading": 0.05450854772010129, - "angularVelocity": -1.1121762477665882, - "velocityX": -1.8245602187716414, - "velocityY": -0.8939935180740173, - "timestamp": 3.0685568602991027 - }, - { - "x": 7.679296617994528, - "y": 2.1605113660425403, - "heading": -0.02108172308249024, - "angularVelocity": -1.1483021547245422, - "velocityX": -2.085626325712903, - "velocityY": -1.01335021133165, - "timestamp": 3.1343847179684325 - }, - { - "x": 7.52460540494712, - "y": 2.086411774056454, - "heading": -0.09749530920005847, - "angularVelocity": -1.16080925041513, - "velocityX": -2.34993540006212, - "velocityY": -1.1256570486967377, - "timestamp": 3.2002125756377624 - }, - { - "x": 7.352233710537335, - "y": 2.0056280451301003, - "heading": -0.17238692358324137, - "angularVelocity": -1.1376887693867086, - "velocityX": -2.6185220135167313, - "velocityY": -1.2271966882493952, - "timestamp": 3.2660404333070923 - }, - { - "x": 7.161817946345765, - "y": 1.9193611743859953, - "heading": -0.24189742475427997, - "angularVelocity": -1.055943541717672, - "velocityX": -2.8926319484385736, - "velocityY": -1.3104918464375106, - "timestamp": 3.331868290976422 - }, - { - "x": 6.953014973362115, - "y": 1.8300106922230066, - "heading": -0.298719434373675, - "angularVelocity": -0.8631909290566133, - "velocityX": -3.1719545550536123, - "velocityY": -1.3573354097564532, - "timestamp": 3.397696148645752 - }, - { - "x": 6.727431268978415, - "y": 1.7436413956339059, - "heading": -0.3201876383615551, - "angularVelocity": -0.32612642653085444, - "velocityX": -3.4268729436231227, - "velocityY": -1.3120478114745338, - "timestamp": 3.463524006315082 - }, - { - "x": 6.489449879790782, - "y": 1.6705324422161696, - "heading": -0.3201878815696182, - "angularVelocity": -0.0000036946069898928346, - "velocityX": -3.61520787115807, - "velocityY": -1.1106081225517261, - "timestamp": 3.5293518639844117 - }, - { - "x": 6.246606826324383, - "y": 1.6156931769447658, - "heading": -0.3201879117478121, - "angularVelocity": -4.584410758613646e-7, - "velocityX": -3.689062078949349, - "velocityY": -0.8330707881589553, - "timestamp": 3.5951797216537416 - }, - { - "x": 6.0003025647982255, - "y": 1.5794398654466013, - "heading": -0.32018794628515007, - "angularVelocity": -5.246614307457641e-7, - "velocityX": -3.741641764546064, - "velocityY": -0.5507290193199733, - "timestamp": 3.6610075793230714 - }, - { - "x": 5.7519574472803, - "y": 1.5619815744715873, - "heading": -0.32018798749762734, - "angularVelocity": -6.260643854910128e-7, - "velocityX": -3.772644687381865, - "velocityY": -0.26521128885450773, - "timestamp": 3.7268354369924013 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.32018803965163717, - "angularVelocity": -7.922787054710825e-7, - "velocityX": -3.7818920261895626, - "velocityY": 0.021835891254127007, - "timestamp": 3.792663294661731 - }, - { - "x": 5.250083628410863, - "y": 1.584512642850435, - "heading": -0.3201880864656727, - "angularVelocity": -6.975962896264183e-7, - "velocityX": -3.768870382202017, - "velocityY": 0.31432577229262343, - "timestamp": 3.859770927241254 - }, - { - "x": 4.999553273526972, - "y": 1.6251081857259102, - "heading": -0.32018812296137045, - "angularVelocity": -5.438382542161688e-7, - "velocityX": -3.7332617059171653, - "velocityY": 0.6049318283932773, - "timestamp": 3.926878559820777 - }, - { - "x": 4.752913977063861, - "y": 1.6849623155206188, - "heading": -0.3201881529794151, - "angularVelocity": -4.473119302496649e-7, - "velocityX": -3.675279353221748, - "velocityY": 0.8919124024198194, - "timestamp": 3.9939861924003 - }, - { - "x": 4.511643860653374, - "y": 1.7637163133941143, - "heading": -0.3201881787520496, - "angularVelocity": -3.8404922716455346e-7, - "velocityX": -3.595270867655492, - "velocityY": 1.1735475510952014, - "timestamp": 4.061093824979823 - }, - { - "x": 4.277188862383835, - "y": 1.86089818593956, - "heading": -0.3201882016908444, - "angularVelocity": -3.41820951724987e-7, - "velocityX": -3.4937158301883047, - "velocityY": 1.4481493208732221, - "timestamp": 4.128201457559346 - }, - { - "x": 4.050954064298834, - "y": 1.9759254853688604, - "heading": -0.32018822275897413, - "angularVelocity": -3.1394535808439923e-7, - "velocityX": -3.3712230544999895, - "velocityY": 1.7140717830120589, - "timestamp": 4.195309090138869 - }, - { - "x": 3.834295246075004, - "y": 2.1081087693328064, - "heading": -0.32018824266587215, - "angularVelocity": -2.966413400864343e-7, - "velocityX": -3.2285272165887804, - "velocityY": 1.969720565053586, - "timestamp": 4.262416722718392 - }, - { - "x": 3.6285105963453295, - "y": 2.256655537498403, - "heading": -0.32018826197928, - "angularVelocity": -2.8779748460931035e-7, - "velocityX": -3.066486505626901, - "velocityY": 2.213559955189428, - "timestamp": 4.329524355297915 - }, - { - "x": 3.4323361610843617, - "y": 2.417680552436319, - "heading": -0.3201882811600919, - "angularVelocity": -2.858216149634079e-7, - "velocityX": -2.9232805229494647, - "velocityY": 2.399503733753405, - "timestamp": 4.396631987877438 - }, - { - "x": 3.2362856931678534, - "y": 2.578856476556199, - "heading": -0.32018830034052725, - "angularVelocity": -2.858160039511642e-7, - "velocityX": -2.921433231669861, - "velocityY": 2.401752497063368, - "timestamp": 4.463739620456961 - }, - { - "x": 3.0402351711109183, - "y": 2.7400323348209366, - "heading": -0.3201883195209977, - "angularVelocity": -2.858165259418001e-7, - "velocityX": -2.9214340384398776, - "velocityY": 2.401751515727269, - "timestamp": 4.530847253036484 - }, - { - "x": 2.8448193652926044, - "y": 2.899781496680513, - "heading": -0.32456406822795664, - "angularVelocity": -0.06520493331028634, - "velocityX": -2.911975855902028, - "velocityY": 2.380491692510139, - "timestamp": 4.597954885616007 - }, - { - "x": 2.66318575993582, - "y": 3.04911523833719, - "heading": -0.3692876119349906, - "angularVelocity": -0.6664449629337832, - "velocityX": -2.706601296678858, - "velocityY": 2.2252869892216163, - "timestamp": 4.66506251819553 - }, - { - "x": 2.496643428698106, - "y": 3.186035937240345, - "heading": -0.4222622822262224, - "angularVelocity": -0.7893985863449552, - "velocityX": -2.4817196619231154, - "velocityY": 2.040314844081317, - "timestamp": 4.732170150775053 - }, - { - "x": 2.3452748100493945, - "y": 3.3104804771184444, - "heading": -0.4767233164983405, - "angularVelocity": -0.8115475420412368, - "velocityX": -2.255609575696765, - "velocityY": 1.8544021759467144, - "timestamp": 4.799277783354576 - }, - { - "x": 2.209079907655855, - "y": 3.4224492823331634, - "heading": -0.5296433021172221, - "angularVelocity": -0.7885837062747133, - "velocityX": -2.0294994348392206, - "velocityY": 1.6684958314098723, - "timestamp": 4.866385415934099 - }, - { - "x": 2.088048432415951, - "y": 3.5219507247047837, - "heading": -0.5792907495482946, - "angularVelocity": -0.7398181923977142, - "velocityX": -1.803542616355617, - "velocityY": 1.4827142390056927, - "timestamp": 4.933493048513622 - }, - { - "x": 1.9821698412647601, - "y": 3.6089936285690154, - "heading": -0.624538681260696, - "angularVelocity": -0.6742590965160707, - "velocityX": -1.5777429046647484, - "velocityY": 1.297064141863228, - "timestamp": 5.000600681093145 - }, - { - "x": 1.8914349570352331, - "y": 3.6835858822586136, - "heading": -0.6645923745497577, - "angularVelocity": -0.5968574922025148, - "velocityX": -1.3520799459287431, - "velocityY": 1.1115315921956592, - "timestamp": 5.067708313672668 - }, - { - "x": 1.815836039628641, - "y": 3.7457342610555044, - "heading": -0.698860244587487, - "angularVelocity": -0.5106404252470358, - "velocityX": -1.1265323257679698, - "velocityY": 0.926100003948804, - "timestamp": 5.134815946252191 - }, - { - "x": 1.7553665690239355, - "y": 3.7954445247907986, - "heading": -0.7268845476309118, - "angularVelocity": -0.41760231982876106, - "velocityX": -0.9010818632746272, - "velocityY": 0.7407542454487245, - "timestamp": 5.201923578831714 - }, - { - "x": 1.7100210087875185, - "y": 3.8327215653417337, - "heading": -0.7483006908429537, - "angularVelocity": -0.31913125808251036, - "velocityX": -0.6757139015846285, - "velocityY": 0.5554813829374979, - "timestamp": 5.269031211411237 - }, - { - "x": 1.6797946046172731, - "y": 3.8575695457400823, - "heading": -0.7628116421295846, - "angularVelocity": -0.21623399200434476, - "velocityX": -0.4504167858168276, - "velocityY": 0.37027055557210875, - "timestamp": 5.33613884399076 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -0.10966416437475777, - "velocityX": -0.22518126874279432, - "velocityY": 0.1851126545280661, - "timestamp": 5.403246476570283 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -1.211900873027185e-19, - "velocityX": -5.462077711506373e-19, - "velocityY": -3.512888080115249e-19, - "timestamp": 5.470354109149806 - } - ] + "samples": [ + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 1.9944978930198404e-24, + "velocityX": 7.05107927160795e-24, + "velocityY": 9.089761159537798e-24, + "timestamp": 0 + }, + { + "x": 2.3773573652919286, + "y": 3.040801892787579, + "heading": -0.7860925568420652, + "angularVelocity": 0.2647627943666689, + "velocityX": 0.2192460319725613, + "velocityY": -0.13726925804658435, + "timestamp": 0.06111075831566115 + }, + { + "x": 2.4041795588293358, + "y": 3.024017002068538, + "heading": -0.754166261219504, + "angularVelocity": 0.5224333080216316, + "velocityX": 0.43891115536253184, + "velocityY": -0.27466343376629077, + "timestamp": 0.1222215166313223 + }, + { + "x": 2.4444548115194884, + "y": 2.998827368062219, + "heading": -0.7070016065582421, + "angularVelocity": 0.7717897136480927, + "velocityX": 0.6590533942013143, + "velocityY": -0.4121963906290397, + "timestamp": 0.18333227494698345 + }, + { + "x": 2.4982173316537715, + "y": 2.9652220428664715, + "heading": -0.6452345488674253, + "angularVelocity": 1.010739506320076, + "velocityX": 0.8797554083125275, + "velocityY": -0.5499084960157551, + "timestamp": 0.2444430332626446 + }, + { + "x": 2.565507310796468, + "y": 2.9231853998386073, + "heading": -0.5696881555564717, + "angularVelocity": 1.2362208454479753, + "velocityX": 1.1011151063633902, + "velocityY": -0.687876311577224, + "timestamp": 0.30555379157830576 + }, + { + "x": 2.646370899659449, + "y": 2.872694862845298, + "heading": -0.4814293119182948, + "angularVelocity": 1.444243960814317, + "velocityX": 1.3232300022409869, + "velocityY": -0.8262135569077015, + "timestamp": 0.3666645498939669 + }, + { + "x": 2.740859365850477, + "y": 2.8137198450437673, + "heading": -0.38181759469062915, + "angularVelocity": 1.6300193283992952, + "velocityX": 1.5461838274524098, + "velocityY": -0.9650513171003614, + "timestamp": 0.42777530820962806 + }, + { + "x": 2.8490284341196963, + "y": 2.7462234286806457, + "heading": -0.27256619156885004, + "angularVelocity": 1.787760553672932, + "velocityX": 1.7700495174758504, + "velocityY": -1.1044931894720724, + "timestamp": 0.4888860665252892 + }, + { + "x": 2.97093875412634, + "y": 2.6701673256371343, + "heading": -0.15589887958558987, + "angularVelocity": 1.909112490154803, + "velocityX": 1.994907662198022, + "velocityY": -1.2445615983138625, + "timestamp": 0.5499968248409504 + }, + { + "x": 3.1066533293644194, + "y": 2.5855194210396055, + "heading": -0.03500682401650803, + "angularVelocity": 1.9782450570262398, + "velocityX": 2.220796779137645, + "velocityY": -1.3851555263033848, + "timestamp": 0.6111075831566113 + }, + { + "x": 3.2562037135860464, + "y": 2.492271514146211, + "heading": 0.08476246781347457, + "angularVelocity": 1.959872453412, + "velocityX": 2.4472022331835617, + "velocityY": -1.5258836490251428, + "timestamp": 0.6722183414722724 + }, + { + "x": 3.419377549511949, + "y": 2.390544477111736, + "heading": 0.19301716737769203, + "angularVelocity": 1.7714507649379716, + "velocityX": 2.6701327298713187, + "velocityY": -1.6646338523409365, + "timestamp": 0.7333290997879336 + }, + { + "x": 3.5932192698224203, + "y": 2.280069793657614, + "heading": 0.2658871078344446, + "angularVelocity": 1.1924240913580317, + "velocityX": 2.8446991184843475, + "velocityY": -1.8077779837631391, + "timestamp": 0.7944398581035947 + }, + { + "x": 3.777176249725073, + "y": 2.160717630889141, + "heading": 0.3003277545805106, + "angularVelocity": 0.5635774730231045, + "velocityX": 3.010222503743814, + "velocityY": -1.9530466657273664, + "timestamp": 0.8555506164192559 + }, + { + "x": 3.9732480656956923, + "y": 2.03835976913752, + "heading": 0.3003278017850349, + "angularVelocity": 7.724421305582541e-7, + "velocityX": 3.208466420230474, + "velocityY": -2.0022311148487826, + "timestamp": 0.916661374734917 + }, + { + "x": 4.174665289116757, + "y": 1.9250164613959089, + "heading": 0.3003278489820802, + "angularVelocity": 7.723197446110156e-7, + "velocityX": 3.295937228935466, + "velocityY": -1.8547193794609556, + "timestamp": 0.9777721330505782 + }, + { + "x": 4.383567042240086, + "y": 1.82614452102736, + "heading": 0.30032789699541584, + "angularVelocity": 7.856773019778577e-7, + "velocityX": 3.4184120583853495, + "velocityY": -1.6179138190011713, + "timestamp": 1.0388828913662393 + }, + { + "x": 4.598915447104952, + "y": 1.7422358380933087, + "heading": 0.30032794702414056, + "angularVelocity": 8.186565851278932e-7, + "velocityX": 3.5239033322497164, + "velocityY": -1.3730591019772607, + "timestamp": 1.0999936496819005 + }, + { + "x": 4.819640328386772, + "y": 1.6737075334125113, + "heading": 0.30032800050450253, + "angularVelocity": 8.751382481764384e-7, + "velocityX": 3.611882545159867, + "velocityY": -1.1213787321509214, + "timestamp": 1.1611044079975616 + }, + { + "x": 5.044644747071726, + "y": 1.620900221581336, + "heading": 0.30032805931258105, + "angularVelocity": 9.623195678675265e-7, + "velocityX": 3.681911743309069, + "velocityY": -0.8641246367522506, + "timestamp": 1.2222151663132228 + }, + { + "x": 5.27281048204291, + "y": 1.5840763655637535, + "heading": 0.3003281261036848, + "angularVelocity": 0.0000010929516437587844, + "velocityX": 3.7336426720908507, + "velocityY": -0.6025756680578687, + "timestamp": 1.283325924628884 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3003281998542592, + "angularVelocity": 0.0000012068345473522654, + "velocityX": 3.7668181767205913, + "velocityY": -0.3380318248368492, + "timestamp": 1.344436682944545 + }, + { + "x": 5.642202615777798, + "y": 1.556873162710076, + "heading": 0.30032827970972786, + "angularVelocity": 0.0000021672306415301637, + "velocityX": 3.777779819586101, + "velocityY": -0.17764976646888633, + "timestamp": 1.3812834620491905 + }, + { + "x": 5.781554061367138, + "y": 1.556248726330051, + "heading": 0.3003283534295918, + "angularVelocity": 0.0000020007139214038515, + "velocityX": 3.781916601002728, + "velocityY": -0.016946837558083135, + "timestamp": 1.418130241153836 + }, + { + "x": 5.920806157723901, + "y": 1.5615468010933287, + "heading": 0.30032842223361694, + "angularVelocity": 0.000001867300937043874, + "velocityX": 3.7792203210295474, + "velocityY": 0.14378664545497588, + "timestamp": 1.4549770202584813 + }, + { + "x": 6.059707308506225, + "y": 1.5727578116463863, + "heading": 0.3003284870919326, + "angularVelocity": 0.000001760216693496236, + "velocityX": 3.7696958637237334, + "velocityY": 0.3042602589826913, + "timestamp": 1.4918237993631267 + }, + { + "x": 6.198006551988053, + "y": 1.5898614987106987, + "heading": 0.3003285487925156, + "angularVelocity": 0.0000016745176784450775, + "velocityX": 3.753360452186466, + "velocityY": 0.4641840475591571, + "timestamp": 1.528670578467772 + }, + { + "x": 6.335454014584064, + "y": 1.6128269556768218, + "heading": 0.30032860799137623, + "angularVelocity": 0.0000016066223984499512, + "velocityX": 3.730243617919986, + "velocityY": 0.6232690488604353, + "timestamp": 1.5655173575724175 + }, + { + "x": 6.47180136240203, + "y": 1.641612684542814, + "heading": 0.30032866524871465, + "angularVelocity": 0.0000015539306228254143, + "velocityX": 3.700387147292773, + "velocityY": 0.7812278186986222, + "timestamp": 1.602364136677063 + }, + { + "x": 6.606802249418837, + "y": 1.6761666710700263, + "heading": 0.30032872317249165, + "angularVelocity": 0.0000015720173768370867, + "velocityX": 3.6638449899081134, + "velocityY": 0.937774952569887, + "timestamp": 1.6392109157817083 + }, + { + "x": 6.740172242433044, + "y": 1.7164148916635715, + "heading": 0.30049565594915995, + "angularVelocity": 0.004530457769298999, + "velocityX": 3.619583482057768, + "velocityY": 1.0923131294390627, + "timestamp": 1.6760576948863537 + }, + { + "x": 6.869834708516544, + "y": 1.760792645305141, + "heading": 0.3094576493704555, + "angularVelocity": 0.2432232515043802, + "velocityX": 3.5189633730334187, + "velocityY": 1.2043862372756071, + "timestamp": 1.712904473990999 + }, + { + "x": 6.995163652975955, + "y": 1.807928879541571, + "heading": 0.3302942112120631, + "angularVelocity": 0.5654920822911383, + "velocityX": 3.4013541347392664, + "velocityY": 1.2792497846979316, + "timestamp": 1.7497512530956445 + }, + { + "x": 7.114961828122759, + "y": 1.8557263226321454, + "heading": 0.3593656658524349, + "angularVelocity": 0.7889822488366823, + "velocityX": 3.2512522955283503, + "velocityY": 1.2971946056622388, + "timestamp": 1.78659803220029 + }, + { + "x": 7.2288849177114916, + "y": 1.9032201895276672, + "heading": 0.39122292476577963, + "angularVelocity": 0.8645873448767251, + "velocityX": 3.091805915116442, + "velocityY": 1.2889557255639152, + "timestamp": 1.8234448113049353 + }, + { + "x": 7.3369415857594324, + "y": 1.9498953701157864, + "heading": 0.4232031564001013, + "angularVelocity": 0.8679247524864317, + "velocityX": 2.932594671058162, + "velocityY": 1.2667370587687046, + "timestamp": 1.8602915904095807 + }, + { + "x": 7.4391897785347085, + "y": 1.9954627576139183, + "heading": 0.4537900488962921, + "angularVelocity": 0.8301103444977762, + "velocityX": 2.774956054771842, + "velocityY": 1.236672203253364, + "timestamp": 1.897138369514226 + }, + { + "x": 7.535687239808996, + "y": 2.0397404710658504, + "heading": 0.48199026779702103, + "angularVelocity": 0.7653374212340174, + "velocityX": 2.618884570622394, + "velocityY": 1.2016712051325453, + "timestamp": 1.9339851486188715 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.681422832753116, + "velocityX": 2.464168636543981, + "velocityY": 1.1632956752609667, + "timestamp": 1.970831927723517 + }, + { + "x": 7.766066221949162, + "y": 2.1519606772611084, + "heading": 0.5396117040985706, + "angularVelocity": 0.5123769157726334, + "velocityX": 2.199683557182638, + "velocityY": 1.0929921902308588, + "timestamp": 2.034287557173969 + }, + { + "x": 7.88917922885163, + "y": 2.215677284676183, + "heading": 0.5626038110324001, + "angularVelocity": 0.36233360433028805, + "velocityX": 1.9401431830189138, + "velocityY": 1.0041127629949167, + "timestamp": 2.097743186624421 + }, + { + "x": 7.996083890855478, + "y": 2.2729699125828824, + "heading": 0.576920084072235, + "angularVelocity": 0.2256107639908224, + "velocityX": 1.6847151770407296, + "velocityY": 0.9028769929929541, + "timestamp": 2.161198816074873 + }, + { + "x": 8.086991794921751, + "y": 2.3232832101942007, + "heading": 0.5831794167828934, + "angularVelocity": 0.09864109401902421, + "velocityX": 1.4326215791028423, + "velocityY": 0.792889425998124, + "timestamp": 2.224654445525325 + }, + { + "x": 8.162075125847174, + "y": 2.3662047030478557, + "heading": 0.5818525424229896, + "angularVelocity": -0.02091027023756688, + "velocityX": 1.1832414487992695, + "velocityY": 0.6764016561709427, + "timestamp": 2.2881100749757772 + }, + { + "x": 8.2214756151654, + "y": 2.4014165863300603, + "heading": 0.5733088893147004, + "angularVelocity": -0.1346397976393925, + "velocityX": 0.936094871844373, + "velocityY": 0.5549055865831228, + "timestamp": 2.3515657044262293 + }, + { + "x": 8.265311459891254, + "y": 2.4286667893871807, + "heading": 0.5578458366246501, + "angularVelocity": -0.24368291393475716, + "velocityX": 0.6908109667414808, + "velocityY": 0.4294371246982685, + "timestamp": 2.4150213338766813 + }, + { + "x": 8.29368248788513, + "y": 2.4477506399099855, + "heading": 0.5357077069480222, + "angularVelocity": -0.34887573992019155, + "velocityX": 0.44710025319389435, + "velocityY": 0.30074322306905504, + "timestamp": 2.4784769633271333 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.45085365638085945, + "velocityX": 0.20473385621503526, + "velocityY": 0.16937940002276766, + "timestamp": 2.5419325927775853 + }, + { + "x": 8.303679627930654, + "y": 2.4605245534000177, + "heading": 0.47064149696897567, + "angularVelocity": -0.5538247976382693, + "velocityX": -0.04548808629479942, + "velocityY": 0.03077484607623031, + "timestamp": 2.6077602833736515 + }, + { + "x": 8.284168636347566, + "y": 2.453496432112781, + "heading": 0.4276658140977222, + "angularVelocity": -0.6528511403348756, + "velocityX": -0.29639489713850375, + "velocityY": -0.10676542384515514, + "timestamp": 2.6735879739697186 + }, + { + "x": 8.248089021230637, + "y": 2.4374965938671473, + "heading": 0.378472576604384, + "angularVelocity": -0.7473031037226994, + "velocityX": -0.5480917648823836, + "velocityY": -0.24305635061409855, + "timestamp": 2.7394156645657857 + }, + { + "x": 8.195380139344671, + "y": 2.412622948042365, + "heading": 0.3234149472418141, + "angularVelocity": -0.8363901097551141, + "velocityX": -0.8007098746544274, + "velocityY": -0.37785991882067893, + "timestamp": 2.8052433551618527 + }, + { + "x": 8.125970395948434, + "y": 2.3789940000965495, + "heading": 0.2629133084829053, + "angularVelocity": -0.9190910118685506, + "velocityX": -1.0544155927047596, + "velocityY": -0.5108632498164021, + "timestamp": 2.87107104575792 + }, + { + "x": 8.039774042344641, + "y": 2.3367560367748186, + "heading": 0.19747799875428093, + "angularVelocity": -0.9940392733834453, + "velocityX": -1.3094239342636282, + "velocityY": -0.6416443131950763, + "timestamp": 2.936898736353987 + }, + { + "x": 7.936686649427289, + "y": 2.286094003355545, + "heading": 0.12774482040942284, + "angularVelocity": -1.0593289497691225, + "velocityX": -1.5660186766982322, + "velocityY": -0.7696158403937725, + "timestamp": 3.002726426950054 + }, + { + "x": 7.816578572926189, + "y": 2.2272487613470173, + "heading": 0.05453269620309629, + "angularVelocity": -1.1121782268737388, + "velocityX": -1.8245828679925904, + "velocityY": -0.8939283981510958, + "timestamp": 3.068554117546121 + }, + { + "x": 7.679285357941873, + "y": 2.160546156413331, + "heading": -0.02105873619647849, + "angularVelocity": -1.1483227151841096, + "velocityX": -2.085645322525112, + "velocityY": -1.0132909772422172, + "timestamp": 3.134381808142188 + }, + { + "x": 7.52459365410468, + "y": 2.086449920391606, + "heading": -0.09747489817686252, + "angularVelocity": -1.16085132697865, + "velocityX": -2.349948819964157, + "velocityY": -1.1256089246149548, + "timestamp": 3.200209498738255 + }, + { + "x": 7.352222023674252, + "y": 2.0056683293260935, + "heading": -0.17237070551570732, + "angularVelocity": -1.1377553528107507, + "velocityX": -2.6185276875067363, + "velocityY": -1.2271673263035494, + "timestamp": 3.2660371893343223 + }, + { + "x": 7.1618070022795735, + "y": 1.9194015924783232, + "heading": -0.24188707800247594, + "angularVelocity": -1.0560354139314483, + "velocityX": -2.8926280060941036, + "velocityY": -1.3104931384745382, + "timestamp": 3.3318648799303894 + }, + { + "x": 6.953005373134873, + "y": 1.8300479988886789, + "heading": -0.29871595232882114, + "angularVelocity": -0.8632974028370489, + "velocityX": -3.1719421911054577, + "velocityY": -1.3573861209553544, + "timestamp": 3.3976925705264565 + }, + { + "x": 6.727423586928556, + "y": 1.7436722229358965, + "heading": -0.32018076563144104, + "angularVelocity": -0.3260757457577048, + "velocityX": -3.426852501791936, + "velocityY": -1.3121495706541393, + "timestamp": 3.4635202611225235 + }, + { + "x": 6.489444642291127, + "y": 1.670557467554106, + "heading": -0.32018100817910516, + "angularVelocity": -0.0000036845841301644418, + "velocityX": -3.615179911106431, + "velocityY": -1.1106990799728806, + "timestamp": 3.5293479517185906 + }, + { + "x": 6.246603596847642, + "y": 1.615712184721808, + "heading": -0.3201810383505521, + "angularVelocity": -4.583397455824824e-7, + "velocityX": -3.6890409377052413, + "velocityY": -0.8331643163488851, + "timestamp": 3.5951756423146577 + }, + { + "x": 6.0003008879136885, + "y": 1.5794526731151552, + "heading": -0.3201810728782718, + "angularVelocity": -5.245166498428474e-7, + "velocityX": -3.7416276752791178, + "velocityY": -0.5508246040279513, + "timestamp": 3.6610033329107248 + }, + { + "x": 5.751956851390291, + "y": 1.561988034181555, + "heading": -0.3201811140764657, + "angularVelocity": -6.258489930223753e-7, + "velocityX": -3.7726378409245966, + "velocityY": -0.265308394924062, + "timestamp": 3.726831023506792 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.3201811662076607, + "angularVelocity": -7.919341312199672e-7, + "velocityX": -3.7818925725102797, + "velocityY": 0.02173781608673755, + "timestamp": 3.792658714102859 + }, + { + "x": 5.250082401017072, + "y": 1.5845061941743261, + "heading": -0.3201812130071943, + "angularVelocity": -6.973782997664008e-7, + "velocityX": -3.7688784475631176, + "velocityY": 0.3142288252976883, + "timestamp": 3.859766528738267 + }, + { + "x": 4.999550332100038, + "y": 1.6250954552906307, + "heading": -0.3201812494932628, + "angularVelocity": -5.436932891401309e-7, + "velocityX": -3.7332771194853676, + "velocityY": 0.6048365803121931, + "timestamp": 3.926874343373675 + }, + { + "x": 4.752908853379521, + "y": 1.6849435065641751, + "heading": -0.32018127950441017, + "angularVelocity": -4.4720793763185246e-7, + "velocityX": -3.6753019012838326, + "velocityY": 0.8918194043226525, + "timestamp": 3.993982158009083 + }, + { + "x": 4.5116361075787825, + "y": 1.7636916636244688, + "heading": -0.32018130527180205, + "angularVelocity": -3.839700641654236e-7, + "velocityX": -3.595300295674319, + "velocityY": 1.1734573311338945, + "timestamp": 4.061089972644491 + }, + { + "x": 4.277178056371613, + "y": 1.8608679654762732, + "heading": -0.32018132820641354, + "angularVelocity": -3.4175768691264823e-7, + "velocityX": -3.493751845160869, + "velocityY": 1.4480623811065336, + "timestamp": 4.128197787279899 + }, + { + "x": 4.05093980766007, + "y": 1.975889994456223, + "heading": -0.3201813492710647, + "angularVelocity": -3.138926699011739e-7, + "velocityX": -3.3712653279007916, + "velocityY": 1.7139885958864325, + "timestamp": 4.195305601915307 + }, + { + "x": 3.834277168836918, + "y": 2.1080683355859073, + "heading": -0.3201813691749636, + "angularVelocity": -2.965958441785772e-7, + "velocityX": -3.2285753902174523, + "velocityY": 1.9696415663034246, + "timestamp": 4.262413416550715 + }, + { + "x": 3.6284883544416036, + "y": 2.2566105084469403, + "heading": -0.32018138848592925, + "angularVelocity": -2.877603113639349e-7, + "velocityX": -3.0665402459214466, + "velocityY": 2.2134854736076965, + "timestamp": 4.329521231186123 + }, + { + "x": 3.4324267312993486, + "y": 2.4177739449564437, + "heading": -0.3201814076661106, + "angularVelocity": -2.8581144314704067e-7, + "velocityX": -2.921591534569326, + "velocityY": 2.4015598985765383, + "timestamp": 4.396629045821531 + }, + { + "x": 3.2363665756420468, + "y": 2.578939166702229, + "heading": -0.3201814268462876, + "angularVelocity": -2.858113772828912e-7, + "velocityX": -2.921569666997558, + "velocityY": 2.401586501086132, + "timestamp": 4.463736860456939 + }, + { + "x": 3.04030639781219, + "y": 2.740104361474713, + "heading": -0.3201814460264912, + "angularVelocity": -2.8581177503839375e-7, + "velocityX": -2.9215699973995357, + "velocityY": 2.4015860991463303, + "timestamp": 4.5308446750923475 + }, + { + "x": 2.8448806552051593, + "y": 2.899843193504081, + "heading": -0.32455581469161493, + "angularVelocity": -0.06518419186929905, + "velocityX": -2.912116028047787, + "velocityY": 2.380331305634352, + "timestamp": 4.5979524897277555 + }, + { + "x": 2.663237615437101, + "y": 3.049167443178603, + "heading": -0.36928029407596225, + "angularVelocity": -0.6664570978407269, + "velocityX": -2.706734539858174, + "velocityY": 2.225139508502705, + "timestamp": 4.6650603043631635 + }, + { + "x": 2.4966866332566564, + "y": 3.186079438683932, + "heading": -0.4222560068283227, + "angularVelocity": -0.7894119789203976, + "velocityX": -2.481841840407185, + "velocityY": 2.0401796161767862, + "timestamp": 4.732168118998572 + }, + { + "x": 2.3453101524500037, + "y": 3.3105160667275593, + "heading": -0.47671806974270053, + "angularVelocity": -0.8115606686086642, + "velocityX": -2.2557206135987404, + "velocityY": 1.8542792477997239, + "timestamp": 4.79927593363398 + }, + { + "x": 2.209108176535601, + "y": 3.4224777520191383, + "heading": -0.5296390322123383, + "angularVelocity": -0.7885961233748087, + "velocityX": -2.029599334360373, + "velocityY": 1.6683852081886403, + "timestamp": 4.866383748269388 + }, + { + "x": 2.0880704157388417, + "y": 3.5219728662741354, + "heading": -0.579287382494677, + "angularVelocity": -0.7398296390975455, + "velocityX": -1.8036313871096692, + "velocityY": 1.4826159188098624, + "timestamp": 4.933491562904796 + }, + { + "x": 1.9821863263109802, + "y": 3.609010233643184, + "heading": -0.6245361276970436, + "angularVelocity": -0.6742693894623133, + "velocityX": -1.577820556415411, + "velocityY": 1.2969781215781884, + "timestamp": 5.000599377540204 + }, + { + "x": 1.8914467304783316, + "y": 3.6835977422656017, + "heading": -0.664590534011598, + "angularVelocity": -0.5968664980698198, + "velocityX": -1.3521464873447422, + "velocityY": 1.1114578686200178, + "timestamp": 5.067707192175612 + }, + { + "x": 1.815843887624038, + "y": 3.745742167237612, + "heading": -0.6988590085071317, + "angularVelocity": -0.510648047201536, + "velocityX": -1.1265877642572446, + "velocityY": 0.9260385740414423, + "timestamp": 5.13481500681102 + }, + { + "x": 1.7553712772903527, + "y": 3.7954492682162915, + "heading": -0.7268838015342108, + "angularVelocity": -0.41760848836064807, + "velocityX": -0.9011262050810134, + "velocityY": 0.7407051063834291, + "timestamp": 5.201922821446428 + }, + { + "x": 1.710023362676749, + "y": 3.8327239369169632, + "heading": -0.7483003159780411, + "angularVelocity": -0.3191359241868454, + "velocityX": -0.675747151952063, + "velocityY": 0.5554445321037842, + "timestamp": 5.269030636081836 + }, + { + "x": 1.679795389171883, + "y": 3.8575703362206024, + "heading": -0.7628115166816798, + "angularVelocity": -0.21623712204721673, + "velocityX": -0.4504389491610239, + "velocityY": 0.37024599055457114, + "timestamp": 5.336138450717244 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.10966573621700788, + "velocityX": -0.22519234880908606, + "velocityY": 0.1851003730766039, + "timestamp": 5.403246265352652 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": 8.092112436868569e-26, + "velocityX": -3.5782911869768004e-26, + "velocityY": 1.7219402564933353e-26, + "timestamp": 5.47035407998806 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGCSprint.3.traj b/src/main/deploy/choreo/SourceLanePGCSprint.3.traj index d9e2ca2a..3ae15e80 100644 --- a/src/main/deploy/choreo/SourceLanePGCSprint.3.traj +++ b/src/main/deploy/choreo/SourceLanePGCSprint.3.traj @@ -1,319 +1,320 @@ { - "samples": [ - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -1.211900873027185e-19, - "velocityX": -5.462077711506373e-19, - "velocityY": -3.512888080115249e-19, - "timestamp": 0 - }, - { - "x": 1.659892349205253, - "y": 3.8800826826438297, - "heading": -0.7264777443526473, - "angularVelocity": 0.7466474211356967, - "velocityX": -0.08186842286768115, - "velocityY": 0.1724334423754288, - "timestamp": 0.05851918722290517 - }, - { - "x": 1.6562452238185605, - "y": 3.90221669421699, - "heading": -0.6460681306884017, - "angularVelocity": 1.3740726329290587, - "velocityX": -0.06232358239700476, - "velocityY": 0.3782351160970439, - "timestamp": 0.11703837444581033 - }, - { - "x": 1.6553319250883793, - "y": 3.9350953760295733, - "heading": -0.5183992679116819, - "angularVelocity": 2.1816581677803772, - "velocityX": -0.01560682527428515, - "velocityY": 0.5618444714097892, - "timestamp": 0.1755575616687155 - }, - { - "x": 1.6623538133314129, - "y": 3.9788519148323376, - "heading": -0.3621106955703112, - "angularVelocity": 2.670723565350507, - "velocityX": 0.11999292157436166, - "velocityY": 0.7477297768352748, - "timestamp": 0.23407674889162067 - }, - { - "x": 1.6832034751641156, - "y": 4.019420947962781, - "heading": -0.22540240918746318, - "angularVelocity": 2.3361275655130602, - "velocityX": 0.35628761816674753, - "velocityY": 0.6932603656286723, - "timestamp": 0.29259593611452583 - }, - { - "x": 1.7146647081648767, - "y": 4.052413452071288, - "heading": -0.11878752772286896, - "angularVelocity": 1.8218790541038834, - "velocityX": 0.5376225216683612, - "velocityY": 0.5637895137339223, - "timestamp": 0.351115123337431 - }, - { - "x": 1.7541026264736177, - "y": 4.074977116543675, - "heading": -0.0408151972626467, - "angularVelocity": 1.332423332593777, - "velocityX": 0.6739314091721454, - "velocityY": 0.38557720199428364, - "timestamp": 0.40963431056033617 - }, - { - "x": 1.799090355046403, - "y": 4.0866639858943925, - "heading": 1.7934233852716553e-24, - "angularVelocity": 0.6974669198185859, - "velocityX": 0.7687688552717376, - "velocityY": 0.199710042215758, - "timestamp": 0.46815349778324133 - }, - { - "x": 1.848745584487915, - "y": 4.087520122528076, - "heading": 7.573339964499894e-25, - "angularVelocity": -5.2358045608260825e-25, - "velocityX": 0.848529034628775, - "velocityY": 0.014630015800164336, - "timestamp": 0.5266726850061465 - }, - { - "x": 1.9382598731728313, - "y": 4.089063493944204, - "heading": 6.819777064270339e-25, - "angularVelocity": -2.006050179968467e-26, - "velocityX": 1.1795649340806893, - "velocityY": 0.02033761122914328, - "timestamp": 0.6025602314214531 - }, - { - "x": 2.0528956622357724, - "y": 4.091040000742217, - "heading": 6.067470023829544e-25, - "angularVelocity": -1.8954686232133377e-26, - "velocityX": 1.510600809724148, - "velocityY": 0.026045206247625866, - "timestamp": 0.6784477778367597 - }, - { - "x": 2.1926529480236727, - "y": 4.09344964285913, - "heading": 5.320081952060173e-25, - "angularVelocity": -2.1711320260805291e-26, - "velocityX": 1.8416366372297248, - "velocityY": 0.03175280043613335, - "timestamp": 0.7543353242520663 - }, - { - "x": 2.3575317192504395, - "y": 4.096292420100353, - "heading": 4.5695624647021355e-25, - "angularVelocity": -2.0004263459154622e-26, - "velocityX": 2.1726723160140353, - "velocityY": 0.03746039206044517, - "timestamp": 0.8302228706673729 - }, - { - "x": 2.4972894766259857, - "y": 4.098702070348201, - "heading": 3.822235688125773e-25, - "angularVelocity": -1.9197661326309743e-26, - "velocityX": 1.8416428515253338, - "velocityY": 0.0317529075806614, - "timestamp": 0.9061104170826795 - }, - { - "x": 2.611925746748375, - "y": 4.100678585440459, - "heading": 3.063572284762821e-25, - "angularVelocity": -2.4686338901390002e-26, - "velocityX": 1.5106071488334032, - "velocityY": 0.026045315544143462, - "timestamp": 0.9819979634979861 - }, - { - "x": 2.701440519692856, - "y": 4.102221965206007, - "heading": 2.3114445870748474e-25, - "angularVelocity": -1.5210807516831905e-26, - "velocityX": 1.1795713153591425, - "velocityY": 0.020337721252726194, - "timestamp": 1.0578855099132927 - }, - { - "x": 2.7658337920580536, - "y": 4.1033322095862, - "heading": 1.561656112047754e-25, - "angularVelocity": -1.9040979941775662e-26, - "velocityX": 0.8485354370636105, - "velocityY": 0.01463012618851756, - "timestamp": 1.1337730563286001 - }, - { - "x": 2.805105562125262, - "y": 4.104009318551404, - "heading": 8.077058611592956e-26, - "angularVelocity": -3.263981497829794e-26, - "velocityX": 0.5174995361200375, - "velocityY": 0.008922530733819996, - "timestamp": 1.2096606027439076 - }, - { - "x": 2.819255828857422, - "y": 4.10425329208374, - "heading": 3.4975515409752656e-27, - "angularVelocity": -3.9242842268499737e-26, - "velocityX": 0.18646362151070262, - "velocityY": 0.0032149350435025517, - "timestamp": 1.2855481491592151 - }, - { - "x": 2.810470941680652, - "y": 4.100290874791341, - "heading": -0.013665728423960572, - "angularVelocity": -0.18841669161843644, - "velocityX": -0.12112192828199068, - "velocityY": -0.05463196207942443, - "timestamp": 1.3580774368065907 - }, - { - "x": 2.779376565530904, - "y": 4.092132450200435, - "heading": -0.040981035402949714, - "angularVelocity": -0.3766107163742046, - "velocityX": -0.4287147600418107, - "velocityY": -0.11248455424752818, - "timestamp": 1.4306067244539662 - }, - { - "x": 2.7259718800290944, - "y": 4.079777209778572, - "heading": -0.08192049181497908, - "angularVelocity": -0.5644541362527821, - "velocityX": -0.7363189028058957, - "velocityY": -0.17034829408406268, - "timestamp": 1.5031360121013417 - }, - { - "x": 2.650255783412156, - "y": 4.06322370149856, - "heading": -0.13644943849416394, - "angularVelocity": -0.7518196917125987, - "velocityX": -1.0439382361654468, - "velocityY": -0.22823205379447506, - "timestamp": 1.5756652997487173 - }, - { - "x": 2.5522270104067304, - "y": 4.04246950295157, - "heading": -0.20452759410435262, - "angularVelocity": -0.9386298668914527, - "velocityX": -1.3515750145241086, - "velocityY": -0.2861492125483705, - "timestamp": 1.6481945873960928 - }, - { - "x": 2.4318844389859766, - "y": 4.01751088765649, - "heading": -0.2861194089559698, - "angularVelocity": -1.1249498995261227, - "velocityX": -1.6592272628656817, - "velocityY": -0.34411775028624336, - "timestamp": 1.7207238750434684 - }, - { - "x": 2.2892276526666033, - "y": 3.9883426531177015, - "heading": -0.3812142224775482, - "angularVelocity": -1.311122949172083, - "velocityX": -1.9668852534846832, - "velocityY": -0.40215801760798864, - "timestamp": 1.793253162690844 - }, - { - "x": 2.1330829846519728, - "y": 3.9587634030266514, - "heading": -0.4780632841326763, - "angularVelocity": -1.3353097044877917, - "velocityX": -2.1528498773319837, - "velocityY": -0.40782490839919366, - "timestamp": 1.8657824503382194 - }, - { - "x": 1.9992497997330843, - "y": 3.9334042162180256, - "heading": -0.5612966415288853, - "angularVelocity": -1.1475827227322806, - "velocityX": -1.8452295515373094, - "velocityY": -0.349640643541367, - "timestamp": 1.938311737985595 - }, - { - "x": 1.8877256900718635, - "y": 3.9122685100016006, - "heading": -0.6308164435957174, - "angularVelocity": -0.9585066160421192, - "velocityX": -1.537642423891296, - "velocityY": -0.29140926240973475, - "timestamp": 2.0108410256329705 - }, - { - "x": 1.7985083705984093, - "y": 3.895358481595443, - "heading": -0.6865270710591681, - "angularVelocity": -0.768112155386183, - "velocityX": -1.2300868017236275, - "velocityY": -0.23314758705988012, - "timestamp": 2.083370313280346 - }, - { - "x": 1.731596065924516, - "y": 3.88267537699245, - "heading": -0.7283506709725182, - "angularVelocity": -0.5766442946012179, - "velocityX": -0.9225556577807338, - "velocityY": -0.17486873254092689, - "timestamp": 2.1558996009277216 - }, - { - "x": 1.6869877424298634, - "y": 3.8742198088507074, - "heading": -0.7562373778990242, - "angularVelocity": -0.3844889124250871, - "velocityX": -0.6150387649128701, - "velocityY": -0.11658143097795469, - "timestamp": 2.228428888575097 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -0.19210952061606446, - "velocityX": -0.3075243171780863, - "velocityY": -0.05829081246861152, - "timestamp": 2.3009581762224727 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -5.999453085593573e-20, - "velocityX": -1.6973677435282043e-18, - "velocityY": -2.503873223513922e-18, - "timestamp": 2.373487463869848 - } - ] + "samples": [ + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": 8.092112436868569e-26, + "velocityX": -3.5782911869768004e-26, + "velocityY": 1.7219402564933353e-26, + "timestamp": 0 + }, + { + "x": 1.659892350481847, + "y": 3.8800826841683307, + "heading": -0.726477756364864, + "angularVelocity": 0.7466472297695131, + "velocityX": -0.08186840257721437, + "velocityY": 0.1724334716376635, + "timestamp": 0.058519186133202616 + }, + { + "x": 1.6562452284597964, + "y": 3.902216695654719, + "heading": -0.6460681483712878, + "angularVelocity": 1.3740725616133136, + "velocityX": -0.06232352606116078, + "velocityY": 0.3782351216574767, + "timestamp": 0.11703837226640523 + }, + { + "x": 1.655331933364656, + "y": 3.9350953770599486, + "heading": -0.5183992946484082, + "angularVelocity": 2.1816580536898917, + "velocityX": -0.015606763447832819, + "velocityY": 0.5618444749110212, + "timestamp": 0.17555755839960785 + }, + { + "x": 1.6623538228794565, + "y": 3.9788519154462327, + "heading": -0.3621107234214173, + "angularVelocity": 2.67072359603982, + "velocityX": 0.11999294554126883, + "velocityY": 0.7477297836419755, + "timestamp": 0.23407674453281047 + }, + { + "x": 1.6832034861301444, + "y": 4.019420949275189, + "heading": -0.22540243054151132, + "angularVelocity": 2.336127720039156, + "velocityX": 0.35628764903239357, + "velocityY": 0.6932603904745416, + "timestamp": 0.2925959306660131 + }, + { + "x": 1.7146647196939016, + "y": 4.052413453417189, + "heading": -0.11878753895047192, + "angularVelocity": 1.8218792610744803, + "velocityX": 0.537622541300291, + "velocityY": 0.5637895248047576, + "timestamp": 0.3511151167992157 + }, + { + "x": 1.7541026360793814, + "y": 4.074977117129141, + "heading": -0.04081520050411857, + "angularVelocity": 1.3324234938755106, + "velocityX": 0.6739313888561295, + "velocityY": 0.3855771961795887, + "timestamp": 0.4096343029324183 + }, + { + "x": 1.7990903606675779, + "y": 4.0866639859913105, + "heading": 4.0044035166870527e-26, + "angularVelocity": 0.6974669881979226, + "velocityX": 0.7687688014969033, + "velocityY": 0.1997100375861074, + "timestamp": 0.46815348906562093 + }, + { + "x": 1.848745584487915, + "y": 4.087520122528076, + "heading": 1.8053103807695408e-26, + "angularVelocity": -4.864634969888421e-27, + "velocityX": 0.848528954372517, + "velocityY": 0.014630014416416372, + "timestamp": 0.5266726751988235 + }, + { + "x": 1.938259872491009, + "y": 4.089063493932448, + "heading": 1.6974407662610403e-26, + "angularVelocity": -2.7257088903490675e-29, + "velocityX": 1.1795648681505524, + "velocityY": 0.020337610092400867, + "timestamp": 0.6025602252777302 + }, + { + "x": 2.052895663172084, + "y": 4.09104000075836, + "heading": 1.589827161160837e-26, + "angularVelocity": 6.478274543003658e-30, + "velocityX": 1.510600758120114, + "velocityY": 0.02604520535788869, + "timestamp": 0.6784477753566369 + }, + { + "x": 2.1926529528780687, + "y": 4.093449642942828, + "heading": 1.4819575466507505e-26, + "angularVelocity": -2.7257089112437415e-29, + "velocityX": 1.8416365999517403, + "velocityY": 0.03175279979340049, + "timestamp": 0.7543353254355436 + }, + { + "x": 2.3575317303228487, + "y": 4.096292420291259, + "heading": 1.3740875703299928e-26, + "angularVelocity": -2.7304766323100225e-29, + "velocityX": 2.1726722930617965, + "velocityY": 0.03746039166471138, + "timestamp": 0.8302228755144503 + }, + { + "x": 2.497289491602916, + "y": 4.0987020706064285, + "heading": 1.2662183176306477e-26, + "angularVelocity": -2.720941189252962e-29, + "velocityX": 1.8416428140683072, + "velocityY": 0.031752906934841556, + "timestamp": 0.906110425593357 + }, + { + "x": 2.611925763329844, + "y": 4.10067858572635, + "heading": 1.1583487031205877e-26, + "angularVelocity": -2.725708910890731e-29, + "velocityX": 1.510607097049927, + "velocityY": 0.02604531465131241, + "timestamp": 0.9819979756722637 + }, + { + "x": 2.7014405355788997, + "y": 4.1022219654799095, + "heading": 1.0506887852514804e-26, + "angularVelocity": 3.7545940988338324e-31, + "velocityX": 1.1795712492494417, + "velocityY": 0.02033772011288781, + "timestamp": 1.0578855257511703 + }, + { + "x": 2.7658338049487097, + "y": 4.103332209808455, + "heading": 9.428654835122248e-27, + "angularVelocity": -2.1154273725241504e-29, + "velocityX": 0.8485353566277292, + "velocityY": 0.014630124801672608, + "timestamp": 1.133773075830078 + }, + { + "x": 2.8051055697205705, + "y": 4.10400931868236, + "heading": 8.349263998410887e-27, + "angularVelocity": -3.641131282577579e-29, + "velocityX": 0.5174994413579915, + "velocityY": 0.008922529099968745, + "timestamp": 1.2096606259089846 + }, + { + "x": 2.819255828857422, + "y": 4.10425329208374, + "heading": 5.817602179940264e-27, + "angularVelocity": -1.9173555419457397e-26, + "velocityX": 0.18646351242249948, + "velocityY": 0.0032149331626451292, + "timestamp": 1.2855481759878913 + }, + { + "x": 2.810470934258171, + "y": 4.100290875532643, + "heading": -0.013665725456507695, + "angularVelocity": -0.18841665432335325, + "velocityX": -0.12112203294600554, + "velocityY": -0.054631952907971924, + "timestamp": 1.3580774622422531 + }, + { + "x": 2.7793765513907402, + "y": 4.0921324527065925, + "heading": -0.04098102601248858, + "angularVelocity": -0.37661063505003184, + "velocityX": -0.4287148608960741, + "velocityY": -0.11248453207493378, + "timestamp": 1.430606748496615 + }, + { + "x": 2.7259718598142455, + "y": 4.079777215271425, + "heading": -0.08192047187969453, + "angularVelocity": -0.5644540017067066, + "velocityX": -0.7363190007027361, + "velocityY": -0.17034825617665642, + "timestamp": 1.5031360347509768 + }, + { + "x": 2.650255757673548, + "y": 4.0632237114983365, + "heading": -0.13644940291380203, + "angularVelocity": -0.7518194904451844, + "velocityX": -1.0439383323745826, + "velocityY": -0.22823199603860284, + "timestamp": 1.5756653210053386 + }, + { + "x": 2.5522269795422527, + "y": 4.04246951947779, + "heading": -0.20452753616691674, + "angularVelocity": -0.9386295766700757, + "velocityX": -1.3515751111558711, + "velocityY": -0.28614912806063464, + "timestamp": 1.6481946072597005 + }, + { + "x": 2.4318844030868068, + "y": 4.017510913730928, + "heading": -0.28611931872413315, + "angularVelocity": -1.1249494758720198, + "velocityX": -1.6592273641491695, + "velocityY": -0.344117625249086, + "timestamp": 1.7207238935140623 + }, + { + "x": 2.2892276108991876, + "y": 3.988342694775403, + "heading": -0.38121408025371734, + "angularVelocity": -1.3111222575124248, + "velocityX": -1.9668853721697708, + "velocityY": -0.40215781047715543, + "timestamp": 1.7932531797684241 + }, + { + "x": 2.133082954891742, + "y": 3.958763429178337, + "heading": -0.47806319392600216, + "angularVelocity": -1.335310447322373, + "velocityX": -2.1528497531306416, + "velocityY": -0.4078251300216943, + "timestamp": 1.865782466022786 + }, + { + "x": 1.999249779098319, + "y": 3.933404232877465, + "heading": -0.561296583651025, + "angularVelocity": -1.1475831905076381, + "velocityX": -1.8452294611595452, + "velocityY": -0.3496407811313142, + "timestamp": 1.9383117522771478 + }, + { + "x": 1.8877256766061599, + "y": 3.91226852016899, + "heading": -0.6308164080928251, + "angularVelocity": -0.9585069429470503, + "velocityX": -1.5376423545799296, + "velocityY": -0.29140935751595565, + "timestamp": 2.0108410385315096 + }, + { + "x": 1.7985083626535399, + "y": 3.8953584872647626, + "heading": -0.6865270511993388, + "angularVelocity": -0.7681123858179831, + "velocityX": -1.2300867492302674, + "velocityY": -0.2331476535550589, + "timestamp": 2.0833703247858715 + }, + { + "x": 1.7315960620068493, + "y": 3.8826753796533127, + "heading": -0.7283506616380105, + "angularVelocity": -0.57664445079461, + "velocityX": -0.9225556199743571, + "velocityY": -0.17486877737869985, + "timestamp": 2.1558996110402333 + }, + { + "x": 1.68698774113937, + "y": 3.874219809689345, + "heading": -0.7562373749577054, + "angularVelocity": -0.38448900795597707, + "velocityX": -0.6150387405032061, + "velocityY": -0.11658145834103056, + "timestamp": 2.228428897294595 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.1921095648592989, + "velocityX": -0.3075243052917468, + "velocityY": -0.05829082515090241, + "timestamp": 2.300958183548957 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": 4.278722998474624e-26, + "velocityX": 4.546149419559626e-26, + "velocityY": -6.368263045423934e-27, + "timestamp": 2.373487469803319 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGCSprint.4.traj b/src/main/deploy/choreo/SourceLanePGCSprint.4.traj index 0cd0adae..02f19754 100644 --- a/src/main/deploy/choreo/SourceLanePGCSprint.4.traj +++ b/src/main/deploy/choreo/SourceLanePGCSprint.4.traj @@ -1,391 +1,392 @@ { - "samples": [ - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -5.999453085593573e-20, - "velocityX": -1.6973677435282043e-18, - "velocityY": -2.503873223513922e-18, - "timestamp": 0 - }, - { - "x": 1.6770247995246623, - "y": 3.8595873316999896, - "heading": -0.7649878221642326, - "angularVelocity": 0.08507451391469989, - "velocityX": 0.20257164680788678, - "velocityY": -0.1707799926111949, - "timestamp": 0.06092450226104873 - }, - { - "x": 1.70170978110981, - "y": 3.8387765844705157, - "heading": -0.7547381408402699, - "angularVelocity": 0.1682357827076709, - "velocityX": 0.405173299231529, - "velocityY": -0.3415825563958536, - "timestamp": 0.12184900452209746 - }, - { - "x": 1.7387402457793653, - "y": 3.80755819428449, - "heading": -0.7395583157876386, - "angularVelocity": 0.24915796583103744, - "velocityX": 0.607809063599532, - "velocityY": -0.5124110830197925, - "timestamp": 0.1827735067831462 - }, - { - "x": 1.7881185850906471, - "y": 3.7659303409499723, - "heading": -0.7196099497583256, - "angularVelocity": 0.3274276405876679, - "velocityX": 0.8104840824091846, - "velocityY": -0.6832694858326719, - "timestamp": 0.24369800904419492 - }, - { - "x": 1.8498475856412335, - "y": 3.713890927230462, - "heading": -0.6950873259687543, - "angularVelocity": 0.4025083977624826, - "velocityX": 1.0132048397554583, - "velocityY": -0.8541623121766772, - "timestamp": 0.30462251130524365 - }, - { - "x": 1.9239305360498986, - "y": 3.6514375329357844, - "heading": -0.6662283152580175, - "angularVelocity": 0.47368480069122665, - "velocityX": 1.2159795756924812, - "velocityY": -1.025094862935087, - "timestamp": 0.3655470135662924 - }, - { - "x": 2.010371367313072, - "y": 3.5785673636651287, - "heading": -0.6333309419257536, - "angularVelocity": 0.539969505065564, - "velocityX": 1.4188188340512586, - "velocityY": -1.1960732803104883, - "timestamp": 0.4264715158273411 - }, - { - "x": 2.1091748328759063, - "y": 3.4952772039974476, - "heading": -0.5967798759520212, - "angularVelocity": 0.5999403297070707, - "velocityX": 1.6217361143055882, - "velocityY": -1.3671044748269063, - "timestamp": 0.48739601808838984 - }, - { - "x": 2.220346716936752, - "y": 3.40156341112366, - "heading": -0.5570916262340567, - "angularVelocity": 0.6514333026129457, - "velocityX": 1.82474833498841, - "velocityY": -1.5381954615278515, - "timestamp": 0.5483205203494386 - }, - { - "x": 2.3438939700884585, - "y": 3.297422079154605, - "heading": -0.5149984011781104, - "angularVelocity": 0.6909079843703304, - "velocityX": 2.027874641016087, - "velocityY": -1.7093505585459128, - "timestamp": 0.6092450226104873 - }, - { - "x": 2.4798242349395756, - "y": 3.182849894813871, - "heading": -0.4716228537300689, - "angularVelocity": 0.7119557130263738, - "velocityX": 2.2311263909664074, - "velocityY": -1.8805600388792227, - "timestamp": 0.670169524871536 - }, - { - "x": 2.628141721858615, - "y": 3.057848265310819, - "heading": -0.42891150330345956, - "angularVelocity": 0.7010537442489162, - "velocityX": 2.4344472488840574, - "velocityY": -2.0517464216194523, - "timestamp": 0.7310940271325848 - }, - { - "x": 2.788815804109156, - "y": 2.922450520332472, - "heading": -0.3910899170501554, - "angularVelocity": 0.6207943413513182, - "velocityX": 2.6372654069800667, - "velocityY": -2.2223857389625743, - "timestamp": 0.7920185293936335 - }, - { - "x": 2.9613127738384817, - "y": 2.777334038124512, - "heading": -0.375202514467059, - "angularVelocity": 0.2607719717597751, - "velocityX": 2.831323413857593, - "velocityY": -2.3819067341111335, - "timestamp": 0.8529430316546822 - }, - { - "x": 3.137446263833113, - "y": 2.628782733735228, - "heading": -0.3752024981016849, - "angularVelocity": 2.6861728103084975e-7, - "velocityX": 2.891012375282727, - "velocityY": -2.438285072117188, - "timestamp": 0.913867533915731 - }, - { - "x": 3.313579787334416, - "y": 2.480231469074176, - "heading": -0.37520248173599324, - "angularVelocity": 2.686224924493493e-7, - "velocityX": 2.8910129252531243, - "velocityY": -2.4382844200276295, - "timestamp": 0.9747920361767797 - }, - { - "x": 3.489713311593346, - "y": 2.3316802053114225, - "heading": -0.37520246537030155, - "angularVelocity": 2.6862249292188543e-7, - "velocityX": 2.8910129376886133, - "velocityY": -2.4382844052831705, - "timestamp": 1.0357165384378284 - }, - { - "x": 3.6658468653792484, - "y": 2.183128976558075, - "heading": -0.37520244900461003, - "angularVelocity": 2.6862249035440315e-7, - "velocityX": 2.89101342233718, - "velocityY": -2.4382838306472756, - "timestamp": 1.0966410406988771 - }, - { - "x": 3.8419803666744587, - "y": 2.034577685567575, - "heading": -0.37520243263891834, - "angularVelocity": 2.686224937536833e-7, - "velocityX": 2.8910125607676815, - "velocityY": -2.4382848521928207, - "timestamp": 1.1575655429599259 - }, - { - "x": 4.017984969789346, - "y": 1.8858736976718657, - "heading": -0.37520241627306933, - "angularVelocity": 2.6862507525069377e-7, - "velocityX": 2.8888968573061926, - "velocityY": -2.440791182151078, - "timestamp": 1.2184900452209746 - }, - { - "x": 4.198449630537114, - "y": 1.742615181247194, - "heading": -0.37520239987870785, - "angularVelocity": 2.6909307205451447e-7, - "velocityX": 2.9621031612948867, - "velocityY": -2.3514105344815093, - "timestamp": 1.2794145474820233 - }, - { - "x": 4.38853011399539, - "y": 1.6123849775845955, - "heading": -0.3752023832167796, - "angularVelocity": 2.734848475989746e-7, - "velocityX": 3.1199349424936167, - "velocityY": -2.137566969436863, - "timestamp": 1.340339049743072 - }, - { - "x": 4.587288186636938, - "y": 1.495827162172114, - "heading": -0.3752023659192684, - "angularVelocity": 2.8391715306138947e-7, - "velocityX": 3.2623667861891117, - "velocityY": -1.9131517055823732, - "timestamp": 1.4012635520041208 - }, - { - "x": 4.7937421798706055, - "y": 1.3935176134109497, - "heading": -0.3752023473605566, - "angularVelocity": 3.046181942517293e-7, - "velocityX": 3.3886857597794955, - "velocityY": -1.6792841133570497, - "timestamp": 1.4621880542651695 - }, - { - "x": 5.0647852667187605, - "y": 1.2879733857720441, - "heading": -0.3752023309051681, - "angularVelocity": 2.139583629256744e-7, - "velocityX": 3.524191190248399, - "velocityY": -1.3723206946612971, - "timestamp": 1.539097356915418 - }, - { - "x": 5.34411641028699, - "y": 1.2068681953777092, - "heading": -0.37520231550400196, - "angularVelocity": 2.0025101753440068e-7, - "velocityX": 3.6319552244350577, - "velocityY": -1.0545563098285724, - "timestamp": 1.6160066595656666 - }, - { - "x": 5.629536746845857, - "y": 1.1508398853670567, - "heading": -0.375202300582326, - "angularVelocity": 1.940165285663064e-7, - "velocityX": 3.711128910592765, - "velocityY": -0.7284984791169633, - "timestamp": 1.6929159622159151 - }, - { - "x": 5.917306487549604, - "y": 1.108501764833761, - "heading": -0.37520228573814113, - "angularVelocity": 1.9300896478374464e-7, - "velocityX": 3.741676634521091, - "velocityY": -0.5504941414672958, - "timestamp": 1.7698252648661636 - }, - { - "x": 6.2050577552774975, - "y": 1.0660382742134262, - "heading": -0.37520227089383906, - "angularVelocity": 1.9301048840600142e-7, - "velocityX": 3.7414364428249387, - "velocityY": -0.5521242444940722, - "timestamp": 1.8467345675164122 - }, - { - "x": 6.492809022919495, - "y": 1.0235747830116217, - "heading": -0.3752022560493111, - "angularVelocity": 1.9301342582848665e-7, - "velocityX": 3.7414364417080774, - "velocityY": -0.5521242520545259, - "timestamp": 1.9236438701666607 - }, - { - "x": 6.772658036083171, - "y": 0.9819575118333774, - "heading": -0.34826612342726787, - "angularVelocity": 0.3502324386496837, - "velocityX": 3.638688734915609, - "velocityY": -0.541121421520385, - "timestamp": 2.0005531728169093 - }, - { - "x": 7.027520568845826, - "y": 0.9441505058911935, - "heading": -0.3001058813779335, - "angularVelocity": 0.6261952766409414, - "velocityX": 3.313806314454086, - "velocityY": -0.49157910212908845, - "timestamp": 2.077462475467158 - }, - { - "x": 7.256873768560855, - "y": 0.9101570740557017, - "heading": -0.24860478708038097, - "angularVelocity": 0.6696341342705694, - "velocityX": 2.9821255922450605, - "velocityY": -0.44199375971564203, - "timestamp": 2.1543717781174063 - }, - { - "x": 7.460704020800595, - "y": 0.8799617502558256, - "heading": -0.1984971865955797, - "angularVelocity": 0.6515154702763284, - "velocityX": 2.65026785077839, - "velocityY": -0.3926095122353607, - "timestamp": 2.231281080767655 - }, - { - "x": 7.6390235862814775, - "y": 0.8535543484261158, - "heading": -0.15201891065632164, - "angularVelocity": 0.6043258011403606, - "velocityX": 2.318569527171569, - "velocityY": -0.3433577073218239, - "timestamp": 2.3081903834179034 - }, - { - "x": 7.7918452312093995, - "y": 0.8309283282540864, - "heading": -0.11047967273973482, - "angularVelocity": 0.5401068074364143, - "velocityX": 1.9870371939645617, - "velocityY": -0.29419094169820903, - "timestamp": 2.385099686068152 - }, - { - "x": 7.9191796079863, - "y": 0.812079256076005, - "heading": -0.07474204501348639, - "angularVelocity": 0.46467236725273836, - "velocityX": 1.6556433667844261, - "velocityY": -0.2450818240258775, - "timestamp": 2.4620089887184005 - }, - { - "x": 8.021035341477521, - "y": 0.7970039548993032, - "heading": -0.04541767666824434, - "angularVelocity": 0.3812850635065168, - "velocityX": 1.3243616829347478, - "velocityY": -0.1960140146538302, - "timestamp": 2.538918291368649 - }, - { - "x": 8.097419437322872, - "y": 0.7857000289910334, - "heading": -0.022962750583734995, - "angularVelocity": 0.29196632020738633, - "velocityX": 0.9931710887135993, - "velocityY": -0.14697735538800996, - "timestamp": 2.6158275940188975 - }, - { - "x": 8.148337646767295, - "y": 0.7781655823556067, - "heading": -0.0077300757248004864, - "angularVelocity": 0.19806023893112515, - "velocityX": 0.662055274067148, - "velocityY": -0.09796534847923602, - "timestamp": 2.692736896669146 - }, - { - "x": 8.173794746398926, - "y": 0.7743990421295166, - "heading": 1.1088837243155707e-19, - "angularVelocity": 0.10050898211824409, - "velocityX": 0.3310015661876223, - "velocityY": -0.04897379245809494, - "timestamp": 2.7696461993193946 - }, - { - "x": 8.173794746398926, - "y": 0.7743990421295166, - "heading": 5.633915553103528e-20, - "angularVelocity": 2.8365160942051004e-20, - "velocityX": -1.2324505157130932e-18, - "velocityY": 3.669165812220002e-18, - "timestamp": 2.846555501969643 - } - ] + "samples": [ + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": 4.278722998474624e-26, + "velocityX": 4.546149419559626e-26, + "velocityY": -6.368263045423934e-27, + "timestamp": 0 + }, + { + "x": 1.6770242080176545, + "y": 3.8595866692257377, + "heading": -0.7649877966744867, + "angularVelocity": 0.08507499669287022, + "velocityX": 0.20256209128031585, + "velocityY": -0.17079099557937222, + "timestamp": 0.060924456145574624 + }, + { + "x": 1.7017080065350787, + "y": 3.838774596868545, + "heading": -0.7547380668498572, + "angularVelocity": 0.16823670612895278, + "velocityX": 0.40515418731755265, + "velocityY": -0.3416045652908843, + "timestamp": 0.12184891229114925 + }, + { + "x": 1.738736696509759, + "y": 3.8075542186994835, + "heading": -0.739558172921761, + "angularVelocity": 0.2491592849319001, + "velocityX": 0.6077803942345016, + "velocityY": -0.5124441011744508, + "timestamp": 0.18277336843672387 + }, + { + "x": 1.7881126694173817, + "y": 3.76592371429674, + "heading": -0.7196097205107205, + "angularVelocity": 0.32742930627685823, + "velocityX": 0.8104458542829184, + "velocityY": -0.6833135170426513, + "timestamp": 0.2436978245822985 + }, + { + "x": 1.849838711755514, + "y": 3.713880986159023, + "heading": -0.6950869960421026, + "angularVelocity": 0.4025103549553654, + "velocityX": 1.0131570512610228, + "velocityY": -0.8542173608142635, + "timestamp": 0.3046222807278731 + }, + { + "x": 1.923918112020319, + "y": 3.6514236137874088, + "heading": -0.6662278740694468, + "angularVelocity": 0.4736869854644038, + "velocityX": 1.2159222248582278, + "velocityY": -1.025160934098061, + "timestamp": 0.36554673687344774 + }, + { + "x": 2.010354801057128, + "y": 3.578548802416206, + "heading": -0.6333303833823517, + "angularVelocity": 0.5399718400192002, + "velocityX": 1.4187519184459274, + "velocityY": -1.1961503800226454, + "timestamp": 0.42647119301902237 + }, + { + "x": 2.109153532122565, + "y": 3.4952533361865883, + "heading": -0.5967791996957547, + "angularVelocity": 0.5999427159310254, + "velocityX": 1.6216596308937807, + "velocityY": -1.367192610313814, + "timestamp": 0.487395649164597 + }, + { + "x": 2.2203200891736463, + "y": 3.4015335717559703, + "heading": -0.5570908397473109, + "angularVelocity": 0.6514356049992648, + "velocityX": 1.824662279880775, + "velocityY": -1.5382946415915537, + "timestamp": 0.5483201053101716 + }, + { + "x": 2.3438614224783176, + "y": 3.2973856025788515, + "heading": -0.5149975235515339, + "angularVelocity": 0.690910003286656, + "velocityX": 2.0277790089660783, + "velocityY": -1.7094607940079831, + "timestamp": 0.6092445614557462 + }, + { + "x": 2.4797851741645616, + "y": 3.182806114592359, + "heading": -0.47162192283324345, + "angularVelocity": 0.7119571262917361, + "velocityX": 2.231021174181074, + "velocityY": -1.8806813426895879, + "timestamp": 0.6701690176013209 + }, + { + "x": 2.628095553772068, + "y": 3.057796514272803, + "heading": -0.4289105902248852, + "angularVelocity": 0.7010539824319861, + "velocityX": 2.4343324338116106, + "velocityY": -2.0518788057927626, + "timestamp": 0.7310934737468955 + }, + { + "x": 2.788761933089725, + "y": 2.9223901324293977, + "heading": -0.3910891505574595, + "angularVelocity": 0.6207924052215403, + "velocityX": 2.6371409690347543, + "velocityY": -2.222529184665362, + "timestamp": 0.7920179298924701 + }, + { + "x": 2.9612506555206113, + "y": 2.7772643724590593, + "heading": -0.37520163462976347, + "angularVelocity": 0.2607740295577497, + "velocityX": 2.8311901877094443, + "velocityY": -2.382060820101041, + "timestamp": 0.8529423860380447 + }, + { + "x": 3.1373758975337274, + "y": 2.6287035614867915, + "heading": -0.37520161826362025, + "angularVelocity": 2.686301076874653e-7, + "velocityX": 2.8908791831030336, + "velocityY": -2.4384429565902384, + "timestamp": 0.9138668421836194 + }, + { + "x": 3.313501125191111, + "y": 2.480142733495068, + "heading": -0.3752016018976461, + "angularVelocity": 2.686273330621961e-7, + "velocityX": 2.8908789474713403, + "velocityY": -2.4384432359436565, + "timestamp": 0.974791298329194 + }, + { + "x": 3.4896263528475178, + "y": 2.3315819055021865, + "heading": -0.37520158553167193, + "angularVelocity": 2.686273331449891e-7, + "velocityX": 2.890878947455303, + "velocityY": -2.4384432359626693, + "timestamp": 1.0357157544747686 + }, + { + "x": 3.665751580503926, + "y": 2.183021077509307, + "heading": -0.3752015691656978, + "angularVelocity": 2.6862733313245716e-7, + "velocityX": 2.8908789474553287, + "velocityY": -2.438443235962638, + "timestamp": 1.0966402106203432 + }, + { + "x": 3.841876808185214, + "y": 2.0344602495459236, + "heading": -0.37520155279972356, + "angularVelocity": 2.6862733360962026e-7, + "velocityX": 2.890878947863701, + "velocityY": -2.4384432354784953, + "timestamp": 1.1575646667659178 + }, + { + "x": 4.018002401412412, + "y": 1.8858998549539654, + "heading": -0.3752015364337498, + "angularVelocity": 2.686273261605019e-7, + "velocityX": 2.8908849478501355, + "velocityY": -2.4384361222196733, + "timestamp": 1.2184891229114925 + }, + { + "x": 4.198462016558864, + "y": 1.7426352651038108, + "heading": -0.375201520037625, + "angularVelocity": 2.691222177389301e-7, + "velocityX": 2.962022586057329, + "velocityY": -2.351512002139716, + "timestamp": 1.279413579057067 + }, + { + "x": 4.3885379093915455, + "y": 1.6123986719437928, + "heading": -0.3752015033732071, + "angularVelocity": 2.7352591969451763e-7, + "velocityX": 3.119861954590255, + "velocityY": -2.1376734631627445, + "timestamp": 1.3403380352026417 + }, + { + "x": 4.587291846391845, + "y": 1.49583415162556, + "heading": -0.37520148607296444, + "angularVelocity": 2.839622014925957e-7, + "velocityX": 3.2623013741048372, + "velocityY": -1.9132632064816424, + "timestamp": 1.4012624913482163 + }, + { + "x": 4.7937421798706055, + "y": 1.3935176134109497, + "heading": -0.3752014675107609, + "angularVelocity": 3.0467573669290407e-7, + "velocityX": 3.3886282543985438, + "velocityY": -1.679400107735582, + "timestamp": 1.462186947493791 + }, + { + "x": 5.064781872639773, + "y": 1.2879640439263618, + "heading": -0.3752014510522667, + "angularVelocity": 2.1399857496737658e-7, + "velocityX": 3.52414428888234, + "velocityY": -1.3724410814878705, + "timestamp": 1.53909631060478 + }, + { + "x": 5.344110469349436, + "y": 1.2068492677876004, + "heading": -0.3752014356488163, + "angularVelocity": 2.0028056132299027e-7, + "velocityX": 3.631919254181875, + "velocityY": -1.0546801177082226, + "timestamp": 1.6160056737157689 + }, + { + "x": 5.629529124295685, + "y": 1.1508112126364383, + "heading": -0.37520142072574625, + "angularVelocity": 1.940345031855091e-7, + "velocityX": 3.711104128301771, + "velocityY": -0.7286246158389513, + "timestamp": 1.6929150368267578 + }, + { + "x": 5.917283115305387, + "y": 1.1083646233041369, + "heading": -0.3752014058811779, + "angularVelocity": 1.9301379957946635e-7, + "velocityX": 3.7414689105466086, + "velocityY": -0.5519040545303511, + "timestamp": 1.7698243999377468 + }, + { + "x": 6.205037197250251, + "y": 1.0659186504442004, + "heading": -0.37520139103661015, + "angularVelocity": 1.9301379056768152e-7, + "velocityX": 3.7414700929145033, + "velocityY": -0.5518960389605939, + "timestamp": 1.8467337630487357 + }, + { + "x": 6.492791277445097, + "y": 1.0234726657206588, + "heading": -0.37520137619194305, + "angularVelocity": 1.9301508362380675e-7, + "velocityX": 3.741470070160208, + "velocityY": -0.5518961932149626, + "timestamp": 1.9236431261597247 + }, + { + "x": 6.772643212230185, + "y": 0.9818724107444464, + "heading": -0.34826678356511775, + "angularVelocity": 0.35021213981392485, + "velocityX": 3.638723862284922, + "velocityY": -0.5408997460579521, + "timestamp": 2.0005524892707136 + }, + { + "x": 7.027508446069354, + "y": 0.9440808910715871, + "heading": -0.30010699502671095, + "angularVelocity": 0.6261888876768776, + "velocityX": 3.313838829628223, + "velocityY": -0.49137735828500057, + "timestamp": 2.0774618523817026 + }, + { + "x": 7.256864076564165, + "y": 0.9101013911553985, + "heading": -0.24860599276707973, + "angularVelocity": 0.66963241114492, + "velocityX": 2.9821548536792237, + "velocityY": -0.44181226500540305, + "timestamp": 2.1543712154926915 + }, + { + "x": 7.460696487188184, + "y": 0.8799184472229736, + "heading": -0.19849831332672846, + "angularVelocity": 0.6515159847057984, + "velocityX": 2.6502938313228213, + "velocityY": -0.39244823661935213, + "timestamp": 2.2312805786036805 + }, + { + "x": 7.639017939225139, + "y": 0.8535218749551334, + "heading": -0.15201987225869848, + "angularVelocity": 0.6043274731186726, + "velocityX": 2.3185922340771317, + "velocityY": -0.3432166279903691, + "timestamp": 2.3081899417146694 + }, + { + "x": 7.7918411996459325, + "y": 0.8309051352911735, + "heading": -0.11048043087448678, + "angularVelocity": 0.5401090283931487, + "velocityX": 1.987056637047598, + "velocityY": -0.29407004230839406, + "timestamp": 2.3850993048256584 + }, + { + "x": 7.919176921526017, + "y": 0.8120637954987597, + "heading": -0.0747425922933183, + "angularVelocity": 0.46467474356269306, + "velocityX": 1.655659554693303, + "velocityY": -0.24498109242204588, + "timestamp": 2.4620086679366473 + }, + { + "x": 8.021033730291848, + "y": 0.7969946793008421, + "heading": -0.045418027645302235, + "angularVelocity": 0.3812873161580816, + "velocityX": 1.3243746228770636, + "velocityY": -0.19593344150011263, + "timestamp": 2.5389180310476362 + }, + { + "x": 8.097418632050843, + "y": 0.7856953915277307, + "heading": -0.022962936429777186, + "angularVelocity": 0.29196823776995817, + "velocityX": 0.9931807866977714, + "velocityY": -0.14691693333626094, + "timestamp": 2.615827394158625 + }, + { + "x": 8.148337378443449, + "y": 0.7781640366357435, + "heading": -0.007730140883461889, + "angularVelocity": 0.198061652445786, + "velocityX": 0.6620617351768197, + "velocityY": -0.09792507163423873, + "timestamp": 2.692736757269614 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": -6.501598853172508e-27, + "angularVelocity": 0.10050975031878098, + "velocityX": 0.33100479480942824, + "velocityY": -0.04895365601706529, + "timestamp": 2.769646120380603 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": -3.1768871176909477e-27, + "angularVelocity": 1.9243307208194474e-27, + "velocityX": 5.67400379522632e-27, + "velocityY": -1.0775303089806919e-26, + "timestamp": 2.846555483491592 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGCSprint.traj b/src/main/deploy/choreo/SourceLanePGCSprint.traj index 32aa2f01..13e0a900 100644 --- a/src/main/deploy/choreo/SourceLanePGCSprint.traj +++ b/src/main/deploy/choreo/SourceLanePGCSprint.traj @@ -1,1696 +1,1697 @@ { - "samples": [ - { - "x": 0.433241993188858, - "y": 4.121134281158447, - "heading": 9.759657159056985e-29, - "angularVelocity": 1.5485227916478622e-29, - "velocityX": -1.3617494152189479e-27, - "velocityY": -5.3557690609839536e-27, - "timestamp": 0 - }, - { - "x": 0.4546918803213756, - "y": 4.109218526997661, - "heading": -0.008937300145995024, - "angularVelocity": -0.11893237262991248, - "velocityX": 0.28544257523423766, - "velocityY": -0.15856789979825608, - "timestamp": 0.07514606787342623 - }, - { - "x": 0.4975916635116252, - "y": 4.0853873108981436, - "heading": -0.02681241329663613, - "angularVelocity": -0.23787157008333967, - "velocityX": 0.5708852692400183, - "velocityY": -0.3171319108760033, - "timestamp": 0.15029213574685246 - }, - { - "x": 0.5619414537480726, - "y": 4.049641027140498, - "heading": -0.05362171709802378, - "angularVelocity": -0.3567625633658495, - "velocityX": 0.8563294402155045, - "velocityY": -0.4756906750976654, - "timestamp": 0.2254382036202787 - }, - { - "x": 0.6477414523773108, - "y": 4.001980177860296, - "heading": -0.08935853104911338, - "angularVelocity": -0.4755646564406219, - "velocityX": 1.1417762905939028, - "velocityY": -0.6342427571923064, - "timestamp": 0.3005842714937049 - }, - { - "x": 0.7549919358777417, - "y": 3.9424053758532884, - "heading": -0.13401442200652888, - "angularVelocity": -0.5942545261667251, - "velocityX": 1.4272268201854599, - "velocityY": -0.7927866845588466, - "timestamp": 0.3757303393671312 - }, - { - "x": 0.8836932393269187, - "y": 3.8709173416225426, - "heading": -0.18758057158075359, - "angularVelocity": -0.7128270459134325, - "velocityX": 1.7126818087934759, - "velocityY": -0.9513210238912064, - "timestamp": 0.45087640724055744 - }, - { - "x": 1.0338457421402332, - "y": 3.7875168932975942, - "heading": -0.2500489995023012, - "angularVelocity": -0.8312933688928013, - "velocityX": 1.9981418464400145, - "velocityY": -1.1098444760333412, - "timestamp": 0.5260224751139837 - }, - { - "x": 1.2054498603190875, - "y": 3.692204931169302, - "heading": -0.3214134496146474, - "angularVelocity": -0.9496764385935714, - "velocityX": 2.283607419990346, - "velocityY": -1.268355947630322, - "timestamp": 0.60116854298741 - }, - { - "x": 1.3985060453231266, - "y": 3.584982424979261, - "heading": -0.40166985798592575, - "angularVelocity": -1.0680054278616389, - "velocityX": 2.569079001302066, - "velocityY": -1.4268545144723166, - "timestamp": 0.6763146108608362 - }, - { - "x": 1.5915937569821017, - "y": 3.4778198154599655, - "heading": -0.48176859267037747, - "angularVelocity": -1.0659071984893158, - "velocityX": 2.5694985396202785, - "velocityY": -1.4260574445464886, - "timestamp": 0.7514606787342625 - }, - { - "x": 1.7632284858900344, - "y": 3.382566036520045, - "heading": -0.552976547680448, - "angularVelocity": -0.9475938931363788, - "velocityX": 2.2840147697019764, - "velocityY": -1.2675816797275836, - "timestamp": 0.8266067466076887 - }, - { - "x": 1.9134098098562773, - "y": 3.299220400739088, - "heading": -0.6152918418017485, - "angularVelocity": -0.8292555536806328, - "velocityX": 1.9985253815170176, - "velocityY": -1.1091150626981823, - "timestamp": 0.901752814481115 - }, - { - "x": 2.04213735776467, - "y": 3.2277823059512354, - "heading": -0.6687115003742338, - "angularVelocity": -0.7108776291856513, - "velocityX": 1.7130310547348613, - "velocityY": -0.9506564589404838, - "timestamp": 0.9768988823545413 - }, - { - "x": 2.1494108084857926, - "y": 3.1682512394298143, - "heading": -0.7132320418113052, - "angularVelocity": -0.5924533737688112, - "velocityX": 1.4275324545498602, - "velocityY": -0.7922046782500088, - "timestamp": 1.0520449502279674 - }, - { - "x": 2.235229893671302, - "y": 3.120626784362015, - "heading": -0.7488501306819726, - "angularVelocity": -0.4739847323836216, - "velocityX": 1.142030283341778, - "velocityY": -0.6337584442610695, - "timestamp": 1.1271910181013935 - }, - { - "x": 2.2995943987852447, - "y": 3.0849086278583613, - "heading": -0.7755632624607874, - "angularVelocity": -0.35548276223593744, - "velocityX": 0.8565252572144713, - "velocityY": -0.4753163740226105, - "timestamp": 1.2023370859748197 - }, - { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080856, - "angularVelocity": -0.2369668826490278, - "velocityX": 0.5710180813549185, - "velocityY": -0.31687698560575434, - "timestamp": 1.2774831538482458 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550668, - "velocityX": 0.28550943973314763, - "velocityY": -0.15843872456938438, - "timestamp": 1.352629221721672 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -1.820153695476826e-18, - "velocityX": -5.7963239407122954e-18, - "velocityY": -1.1530643297922074e-18, - "timestamp": 1.4277752895950981 - }, - { - "x": 2.37735740952237, - "y": 3.0408026970784268, - "heading": -0.7860869247798871, - "angularVelocity": 0.2648554274741457, - "velocityX": 0.21924714610297322, - "velocityY": -0.1372563412236913, - "timestamp": 1.4888859391070042 - }, - { - "x": 2.404179793787175, - "y": 3.024019444447085, - "heading": -0.7541504516362038, - "angularVelocity": 0.5226007806947081, - "velocityX": 0.4389150578342217, - "velocityY": -0.2746371175137346, - "timestamp": 1.5499965886189102 - }, - { - "x": 2.4444554974169383, - "y": 2.998832316814256, - "heading": -0.7069723408476191, - "angularVelocity": 0.7720112806098135, - "velocityX": 0.6590619466729163, - "velocityY": -0.41215611082519754, - "timestamp": 1.6111072381308162 - }, - { - "x": 2.4982188560395806, - "y": 2.96523040511413, - "heading": -0.645190074686331, - "angularVelocity": 1.0109901736398696, - "velocityX": 0.8797706954852063, - "velocityY": -0.5498536174710421, - "timestamp": 1.6722178876427223 - }, - { - "x": 2.5655102024042766, - "y": 2.923198127539445, - "heading": -0.5696285601591381, - "angularVelocity": 1.2364704864161304, - "velocityX": 1.101139439723772, - "velocityY": -0.6878061010707456, - "timestamp": 1.7333285371546283 - }, - { - "x": 2.646375841205288, - "y": 2.8727129664624207, - "heading": -0.48135684674506, - "angularVelocity": 1.4444571301255718, - "velocityX": 1.3232659028645477, - "velocityY": -0.8261270577264651, - "timestamp": 1.7944391866665343 - }, - { - "x": 2.740867207337065, - "y": 2.813744425609022, - "heading": -0.38173698900245784, - "angularVelocity": 1.6301554399809315, - "velocityX": 1.5462340342720005, - "velocityY": -0.9649470480903679, - "timestamp": 1.8555498361784404 - }, - { - "x": 2.849040211493812, - "y": 2.746255741762665, - "heading": -0.2724850390795677, - "angularVelocity": 1.7877726843928354, - "velocityX": 1.7701170748589676, - "velocityY": -1.1043686229060403, - "timestamp": 1.9166604856903464 - }, - { - "x": 2.9709557136972373, - "y": 2.6702088910620962, - "heading": -0.155828372393994, - "angularVelocity": 1.9089416921161393, - "velocityX": 1.994996014232719, - "velocityY": -1.2444124110602532, - "timestamp": 1.9777711352022525 - }, - { - "x": 3.1066769296559538, - "y": 2.585572206493352, - "heading": -0.03496320783780511, - "angularVelocity": 1.977808541089754, - "velocityX": 2.2209094002882996, - "velocityY": -1.3849743906298195, - "timestamp": 2.0388817847141585 - }, - { - "x": 3.2562355055408387, - "y": 2.4923383100856262, - "heading": 0.08475597327951304, - "angularVelocity": 1.959055943170691, - "velocityX": 2.4473406366879895, - "velocityY": -1.5256571015427038, - "timestamp": 2.0999924342260647 - }, - { - "x": 3.4194188611023764, - "y": 2.3906298265628587, - "heading": 0.19293288100296513, - "angularVelocity": 1.7701809518876104, - "velocityX": 2.6702932609110017, - "velocityY": -1.6643332109070865, - "timestamp": 2.161103083737971 - }, - { - "x": 3.5932872633720114, - "y": 2.2801851899216916, - "heading": 0.26578166446591744, - "angularVelocity": 1.192080006427692, - "velocityX": 2.845140800471438, - "velocityY": -1.8072895235657866, - "timestamp": 2.2222137332498773 - }, - { - "x": 3.7772765002319786, - "y": 2.1608722925529302, - "heading": 0.30020319888903974, - "angularVelocity": 0.5632657269731189, - "velocityX": 3.0107557083666587, - "velocityY": -1.9524076134310657, - "timestamp": 2.2833243827617835 - }, - { - "x": 3.9732507395108834, - "y": 2.038358982543431, - "heading": 0.30020324669419357, - "angularVelocity": 7.822720631834703e-7, - "velocityX": 3.2068754111462736, - "velocityY": -2.0047783976771947, - "timestamp": 2.3444350322736898 - }, - { - "x": 4.174667645417682, - "y": 1.9250159473015482, - "heading": 0.3002032939445946, - "angularVelocity": 7.731942209741845e-7, - "velocityX": 3.2959379014218597, - "velocityY": -1.8547182225546543, - "timestamp": 2.405545681785596 - }, - { - "x": 4.383569049898422, - "y": 1.8261442295125712, - "heading": 0.30020334201253857, - "angularVelocity": 7.865722968351813e-7, - "velocityX": 3.4184124395542623, - "velocityY": -1.6179130573585891, - "timestamp": 2.4666563312975023 - }, - { - "x": 4.59891708073721, - "y": 1.7422357169202292, - "heading": 0.3002033920988207, - "angularVelocity": 8.195998987503142e-7, - "velocityX": 3.52390348587004, - "velocityY": -1.3730587591937538, - "timestamp": 2.5277669808094085 - }, - { - "x": 4.819641568305972, - "y": 1.6737075280608862, - "heading": 0.30020344564176327, - "angularVelocity": 8.761638607213102e-7, - "velocityX": 3.6118825332687328, - "velocityY": -1.1213788334223447, - "timestamp": 2.588877630321315 - }, - { - "x": 5.044645579459654, - "y": 1.6209002757550093, - "heading": 0.30020350452029126, - "angularVelocity": 9.634740995599556e-7, - "velocityX": 3.681911629982681, - "velocityY": -0.8641252012153561, - "timestamp": 2.649988279833221 - }, - { - "x": 5.272810899090047, - "y": 1.584076421741111, - "heading": 0.3002035713939988, - "angularVelocity": 0.0000010943052979537505, - "velocityX": 3.733642523075129, - "velocityY": -0.6025767081190017, - "timestamp": 2.7110989293451273 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.3002036452391599, - "angularVelocity": 0.0000012083844913604929, - "velocityX": 3.7668180588495743, - "velocityY": -0.33803334595452383, - "timestamp": 2.7722095788570336 - }, - { - "x": 5.642203177619129, - "y": 1.5568731039887571, - "heading": 0.3002037251666547, - "angularVelocity": 0.0000021691766198010436, - "velocityX": 3.7777797970827858, - "velocityY": -0.177650642031358, - "timestamp": 2.8090565069037923 - }, - { - "x": 5.781555187145968, - "y": 1.5562486567630798, - "heading": 0.30020379895395954, - "angularVelocity": 0.0000020025361337225055, - "velocityX": 3.78191661866635, - "velocityY": -0.016947063399286564, - "timestamp": 2.845903434950551 - }, - { - "x": 5.9208078464820115, - "y": 1.5615467685843438, - "heading": 0.30020386782175307, - "angularVelocity": 0.0000018690240183201108, - "velocityX": 3.7792203235866277, - "velocityY": 0.14378706997068585, - "timestamp": 2.8827503629973097 - }, - { - "x": 6.059709556234372, - "y": 1.5727578639536914, - "heading": 0.3002039327408759, - "angularVelocity": 0.0000017618598429234572, - "velocityX": 3.7696957959723445, - "velocityY": 0.30426133096145186, - "timestamp": 2.9195972910440684 - }, - { - "x": 6.198009351638548, - "y": 1.5898616832743162, - "heading": 0.30020399449987484, - "angularVelocity": 0.0000016760962775678482, - "velocityX": 3.753360259196494, - "velocityY": 0.46418576058553174, - "timestamp": 2.956444219090827 - }, - { - "x": 6.335457356092555, - "y": 1.6128273194464042, - "heading": 0.30020405375523124, - "angularVelocity": 0.0000016081491612309129, - "velocityX": 3.730243245233006, - "velocityY": 0.6232713930166671, - "timestamp": 2.993291147137586 - }, - { - "x": 6.471805232718414, - "y": 1.6416132738069629, - "heading": 0.3002041110675489, - "angularVelocity": 0.0000015554164405322294, - "velocityX": 3.700386541120455, - "velocityY": 0.7812307805966762, - "timestamp": 3.0301380751843445 - }, - { - "x": 6.60680663253268, - "y": 1.676167531284266, - "heading": 0.3002041691027596, - "angularVelocity": 0.0000015750352547385845, - "velocityX": 3.6638440969339445, - "velocityY": 0.937778515306732, - "timestamp": 3.0669850032311032 - }, - { - "x": 6.740176057879373, - "y": 1.7164157557118485, - "heading": 0.3003755003099619, - "angularVelocity": 0.004649809801927131, - "velocityX": 3.6195534449289073, - "velocityY": 1.0923088181600296, - "timestamp": 3.103831931277862 - }, - { - "x": 6.869837525657242, - "y": 1.7607930294165437, - "heading": 0.3093430143729343, - "angularVelocity": 0.2433720947264084, - "velocityX": 3.5189220554106844, - "velocityY": 1.2043683437702197, - "timestamp": 3.1406788593246207 - }, - { - "x": 6.995165288352465, - "y": 1.807928452248917, - "heading": 0.3301850856624388, - "angularVelocity": 0.5656393190514163, - "velocityX": 3.4013083135772337, - "velocityY": 1.2792225927914125, - "timestamp": 3.1775257873713794 - }, - { - "x": 7.114963240628345, - "y": 1.8557254140007795, - "heading": 0.35927322814233364, - "angularVelocity": 0.7894319559824341, - "velocityX": 3.2512331048020355, - "velocityY": 1.2971762989633377, - "timestamp": 3.214372715418138 - }, - { - "x": 7.228886146559884, - "y": 1.9032191557267804, - "heading": 0.3911508310575271, - "angularVelocity": 0.8651359721153189, - "velocityX": 3.091788433135363, - "velocityY": 1.2889471183522156, - "timestamp": 3.251219643464897 - }, - { - "x": 7.336942556890349, - "y": 1.949894399055616, - "heading": 0.423151819771819, - "angularVelocity": 0.8684845768875223, - "velocityX": 2.9325758226938556, - "velocityY": 1.2667336411221237, - "timestamp": 3.2880665715116555 - }, - { - "x": 7.43919043499382, - "y": 1.9954619855368936, - "heading": 0.45375826572504047, - "angularVelocity": 0.8306376562622595, - "velocityX": 2.7749362979111796, - "velocityY": 1.2366726046592738, - "timestamp": 3.324913499558414 - }, - { - "x": 7.535687560612936, - "y": 2.0397400223416313, - "heading": 0.48197582868140687, - "angularVelocity": 0.7658050332054913, - "velocityX": 2.618864875157601, - "velocityY": 1.201675123325048, - "timestamp": 3.361760427605173 - }, - { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.6818119458710672, - "velocityX": 2.464149969521805, - "velocityY": 1.163303151067446, - "timestamp": 3.3986073556519316 - }, - { - "x": 7.766065583230135, - "y": 2.1519617180113952, - "heading": 0.5396264377138885, - "angularVelocity": 0.5126079654708053, - "velocityX": 2.1996686083558368, - "velocityY": 1.0930061650047849, - "timestamp": 3.4620631259727794 - }, - { - "x": 7.889178097615864, - "y": 2.215679412144434, - "heading": 0.5626262771579723, - "angularVelocity": 0.3624546566496878, - "velocityX": 1.9401311143689866, - "velocityY": 1.004127659484183, - "timestamp": 3.5255188962936272 - }, - { - "x": 7.996082408378713, - "y": 2.2729729622929304, - "heading": 0.5769453477724344, - "angularVelocity": 0.22565435014123394, - "velocityX": 1.6847059018008188, - "velocityY": 0.90288952224213, - "timestamp": 3.588974666614475 - }, - { - "x": 8.086990111713977, - "y": 2.3232868849538337, - "heading": 0.5832040110121259, - "angularVelocity": 0.09863032483958853, - "velocityX": 1.432615235393288, - "velocityY": 0.7928975159627525, - "timestamp": 3.652430436935323 - }, - { - "x": 8.16207340516984, - "y": 2.3662086171292933, - "heading": 0.581874089552229, - "angularVelocity": -0.020958243090781536, - "velocityX": 1.183238231546559, - "velocityY": 0.676403926048603, - "timestamp": 3.7158862072561707 - }, - { - "x": 8.221474033046453, - "y": 2.401420292157025, - "heading": 0.5733258497363348, - "angularVelocity": -0.1347117807044565, - "velocityX": 0.9360949772773745, - "velocityY": 0.5549010728211649, - "timestamp": 3.7793419775770185 - }, - { - "x": 8.265310204045273, - "y": 2.4286697945211704, - "heading": 0.5578573342178689, - "angularVelocity": -0.24376846172150818, - "velocityX": 0.6908145748948221, - "velocityY": 0.4294251291311274, - "timestamp": 3.8427977478978663 - }, - { - "x": 8.293681756385627, - "y": 2.447752418352113, - "heading": 0.5357134031725193, - "angularVelocity": -0.34896638924064666, - "velocityX": 0.44710752382171043, - "velocityY": 0.30072322397878726, - "timestamp": 3.906253518218714 - }, - { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.45094242234392296, - "velocityX": 0.20474492941705758, - "velocityY": 0.16935099752034305, - "timestamp": 3.969709288539562 - }, - { - "x": 8.303680612059921, - "y": 2.4605220907329786, - "heading": 0.4706360870687623, - "angularVelocity": -0.5539055745477068, - "velocityX": -0.04547302080206286, - "velocityY": 0.030737357256443772, - "timestamp": 4.035537146208892 - }, - { - "x": 8.28417079095848, - "y": 2.4534909416021047, - "heading": 0.42765554839737563, - "angularVelocity": -0.6529232485020114, - "velocityX": -0.2963763639315537, - "velocityY": -0.1068111492583101, - "timestamp": 4.101365003878222 - }, - { - "x": 8.248092493080108, - "y": 2.437487576077448, - "heading": 0.3784580606750378, - "angularVelocity": -0.7473657728536394, - "velocityX": -0.5480703634561933, - "velocityY": -0.24310931710774963, - "timestamp": 4.1671928615475515 - }, - { - "x": 8.195385029545998, - "y": 2.412609981570135, - "heading": 0.3233968459430461, - "angularVelocity": -0.8364424528074197, - "velocityX": -0.8006862960492162, - "velocityY": -0.37791894477683524, - "timestamp": 4.233020719216881 - }, - { - "x": 8.125976752501963, - "y": 2.378976757579925, - "heading": 0.2628923564512202, - "angularVelocity": -0.9191319850595123, - "velocityX": -1.0543906410063166, - "velocityY": -0.5109269112046505, - "timestamp": 4.298848576886211 - }, - { - "x": 8.039781850826214, - "y": 2.336734306251694, - "heading": 0.19745501482460134, - "angularVelocity": -0.9940676173198235, - "velocityX": -1.3093985544650038, - "velocityY": -0.641710862602064, - "timestamp": 4.364676434555541 - }, - { - "x": 7.936695821305385, - "y": 2.286067717588345, - "heading": 0.12772072746128918, - "angularVelocity": -1.0593431084086873, - "velocityX": -1.565993990548153, - "velocityY": -0.7696830864200519, - "timestamp": 4.430504292224871 - }, - { - "x": 7.816588930914964, - "y": 2.2272180395232652, - "heading": 0.05450854772010129, - "angularVelocity": -1.1121762477665882, - "velocityX": -1.8245602187716414, - "velocityY": -0.8939935180740173, - "timestamp": 4.496332149894201 - }, - { - "x": 7.679296617994528, - "y": 2.1605113660425403, - "heading": -0.02108172308249024, - "angularVelocity": -1.1483021547245422, - "velocityX": -2.085626325712903, - "velocityY": -1.01335021133165, - "timestamp": 4.562160007563531 - }, - { - "x": 7.52460540494712, - "y": 2.086411774056454, - "heading": -0.09749530920005847, - "angularVelocity": -1.16080925041513, - "velocityX": -2.34993540006212, - "velocityY": -1.1256570486967377, - "timestamp": 4.6279878652328605 - }, - { - "x": 7.352233710537335, - "y": 2.0056280451301003, - "heading": -0.17238692358324137, - "angularVelocity": -1.1376887693867086, - "velocityX": -2.6185220135167313, - "velocityY": -1.2271966882493952, - "timestamp": 4.69381572290219 - }, - { - "x": 7.161817946345765, - "y": 1.9193611743859953, - "heading": -0.24189742475427997, - "angularVelocity": -1.055943541717672, - "velocityX": -2.8926319484385736, - "velocityY": -1.3104918464375106, - "timestamp": 4.75964358057152 - }, - { - "x": 6.953014973362115, - "y": 1.8300106922230066, - "heading": -0.298719434373675, - "angularVelocity": -0.8631909290566133, - "velocityX": -3.1719545550536123, - "velocityY": -1.3573354097564532, - "timestamp": 4.82547143824085 - }, - { - "x": 6.727431268978415, - "y": 1.7436413956339059, - "heading": -0.3201876383615551, - "angularVelocity": -0.32612642653085444, - "velocityX": -3.4268729436231227, - "velocityY": -1.3120478114745338, - "timestamp": 4.89129929591018 - }, - { - "x": 6.489449879790782, - "y": 1.6705324422161696, - "heading": -0.3201878815696182, - "angularVelocity": -0.0000036946069898928346, - "velocityX": -3.61520787115807, - "velocityY": -1.1106081225517261, - "timestamp": 4.95712715357951 - }, - { - "x": 6.246606826324383, - "y": 1.6156931769447658, - "heading": -0.3201879117478121, - "angularVelocity": -4.584410758613646e-7, - "velocityX": -3.689062078949349, - "velocityY": -0.8330707881589553, - "timestamp": 5.02295501124884 - }, - { - "x": 6.0003025647982255, - "y": 1.5794398654466013, - "heading": -0.32018794628515007, - "angularVelocity": -5.246614307457641e-7, - "velocityX": -3.741641764546064, - "velocityY": -0.5507290193199733, - "timestamp": 5.0887828689181696 - }, - { - "x": 5.7519574472803, - "y": 1.5619815744715873, - "heading": -0.32018798749762734, - "angularVelocity": -6.260643854910128e-7, - "velocityX": -3.772644687381865, - "velocityY": -0.26521128885450773, - "timestamp": 5.154610726587499 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.32018803965163717, - "angularVelocity": -7.922787054710825e-7, - "velocityX": -3.7818920261895626, - "velocityY": 0.021835891254127007, - "timestamp": 5.220438584256829 - }, - { - "x": 5.250083628410863, - "y": 1.584512642850435, - "heading": -0.3201880864656727, - "angularVelocity": -6.975962896264183e-7, - "velocityX": -3.768870382202017, - "velocityY": 0.31432577229262343, - "timestamp": 5.287546216836352 - }, - { - "x": 4.999553273526972, - "y": 1.6251081857259102, - "heading": -0.32018812296137045, - "angularVelocity": -5.438382542161688e-7, - "velocityX": -3.7332617059171653, - "velocityY": 0.6049318283932773, - "timestamp": 5.354653849415875 - }, - { - "x": 4.752913977063861, - "y": 1.6849623155206188, - "heading": -0.3201881529794151, - "angularVelocity": -4.473119302496649e-7, - "velocityX": -3.675279353221748, - "velocityY": 0.8919124024198194, - "timestamp": 5.421761481995398 - }, - { - "x": 4.511643860653374, - "y": 1.7637163133941143, - "heading": -0.3201881787520496, - "angularVelocity": -3.8404922716455346e-7, - "velocityX": -3.595270867655492, - "velocityY": 1.1735475510952014, - "timestamp": 5.488869114574921 - }, - { - "x": 4.277188862383835, - "y": 1.86089818593956, - "heading": -0.3201882016908444, - "angularVelocity": -3.41820951724987e-7, - "velocityX": -3.4937158301883047, - "velocityY": 1.4481493208732221, - "timestamp": 5.555976747154444 - }, - { - "x": 4.050954064298834, - "y": 1.9759254853688604, - "heading": -0.32018822275897413, - "angularVelocity": -3.1394535808439923e-7, - "velocityX": -3.3712230544999895, - "velocityY": 1.7140717830120589, - "timestamp": 5.623084379733967 - }, - { - "x": 3.834295246075004, - "y": 2.1081087693328064, - "heading": -0.32018824266587215, - "angularVelocity": -2.966413400864343e-7, - "velocityX": -3.2285272165887804, - "velocityY": 1.969720565053586, - "timestamp": 5.69019201231349 - }, - { - "x": 3.6285105963453295, - "y": 2.256655537498403, - "heading": -0.32018826197928, - "angularVelocity": -2.8779748460931035e-7, - "velocityX": -3.066486505626901, - "velocityY": 2.213559955189428, - "timestamp": 5.757299644893013 - }, - { - "x": 3.4323361610843617, - "y": 2.417680552436319, - "heading": -0.3201882811600919, - "angularVelocity": -2.858216149634079e-7, - "velocityX": -2.9232805229494647, - "velocityY": 2.399503733753405, - "timestamp": 5.824407277472536 - }, - { - "x": 3.2362856931678534, - "y": 2.578856476556199, - "heading": -0.32018830034052725, - "angularVelocity": -2.858160039511642e-7, - "velocityX": -2.921433231669861, - "velocityY": 2.401752497063368, - "timestamp": 5.891514910052059 - }, - { - "x": 3.0402351711109183, - "y": 2.7400323348209366, - "heading": -0.3201883195209977, - "angularVelocity": -2.858165259418001e-7, - "velocityX": -2.9214340384398776, - "velocityY": 2.401751515727269, - "timestamp": 5.958622542631582 - }, - { - "x": 2.8448193652926044, - "y": 2.899781496680513, - "heading": -0.32456406822795664, - "angularVelocity": -0.06520493331028634, - "velocityX": -2.911975855902028, - "velocityY": 2.380491692510139, - "timestamp": 6.025730175211105 - }, - { - "x": 2.66318575993582, - "y": 3.04911523833719, - "heading": -0.3692876119349906, - "angularVelocity": -0.6664449629337832, - "velocityX": -2.706601296678858, - "velocityY": 2.2252869892216163, - "timestamp": 6.0928378077906284 - }, - { - "x": 2.496643428698106, - "y": 3.186035937240345, - "heading": -0.4222622822262224, - "angularVelocity": -0.7893985863449552, - "velocityX": -2.4817196619231154, - "velocityY": 2.040314844081317, - "timestamp": 6.1599454403701515 - }, - { - "x": 2.3452748100493945, - "y": 3.3104804771184444, - "heading": -0.4767233164983405, - "angularVelocity": -0.8115475420412368, - "velocityX": -2.255609575696765, - "velocityY": 1.8544021759467144, - "timestamp": 6.2270530729496745 - }, - { - "x": 2.209079907655855, - "y": 3.4224492823331634, - "heading": -0.5296433021172221, - "angularVelocity": -0.7885837062747133, - "velocityX": -2.0294994348392206, - "velocityY": 1.6684958314098723, - "timestamp": 6.2941607055291975 - }, - { - "x": 2.088048432415951, - "y": 3.5219507247047837, - "heading": -0.5792907495482946, - "angularVelocity": -0.7398181923977142, - "velocityX": -1.803542616355617, - "velocityY": 1.4827142390056927, - "timestamp": 6.3612683381087205 - }, - { - "x": 1.9821698412647601, - "y": 3.6089936285690154, - "heading": -0.624538681260696, - "angularVelocity": -0.6742590965160707, - "velocityX": -1.5777429046647484, - "velocityY": 1.297064141863228, - "timestamp": 6.4283759706882435 - }, - { - "x": 1.8914349570352331, - "y": 3.6835858822586136, - "heading": -0.6645923745497577, - "angularVelocity": -0.5968574922025148, - "velocityX": -1.3520799459287431, - "velocityY": 1.1115315921956592, - "timestamp": 6.4954836032677665 - }, - { - "x": 1.815836039628641, - "y": 3.7457342610555044, - "heading": -0.698860244587487, - "angularVelocity": -0.5106404252470358, - "velocityX": -1.1265323257679698, - "velocityY": 0.926100003948804, - "timestamp": 6.5625912358472895 - }, - { - "x": 1.7553665690239355, - "y": 3.7954445247907986, - "heading": -0.7268845476309118, - "angularVelocity": -0.41760231982876106, - "velocityX": -0.9010818632746272, - "velocityY": 0.7407542454487245, - "timestamp": 6.6296988684268126 - }, - { - "x": 1.7100210087875185, - "y": 3.8327215653417337, - "heading": -0.7483006908429537, - "angularVelocity": -0.31913125808251036, - "velocityX": -0.6757139015846285, - "velocityY": 0.5554813829374979, - "timestamp": 6.696806501006336 - }, - { - "x": 1.6797946046172731, - "y": 3.8575695457400823, - "heading": -0.7628116421295846, - "angularVelocity": -0.21623399200434476, - "velocityX": -0.4504167858168276, - "velocityY": 0.37027055557210875, - "timestamp": 6.763914133585859 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -0.10966416437475777, - "velocityX": -0.22518126874279432, - "velocityY": 0.1851126545280661, - "timestamp": 6.831021766165382 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -1.211900873027185e-19, - "velocityX": -5.462077711506373e-19, - "velocityY": -3.512888080115249e-19, - "timestamp": 6.898129398744905 - }, - { - "x": 1.659892349205253, - "y": 3.8800826826438297, - "heading": -0.7264777443526473, - "angularVelocity": 0.7466474211356967, - "velocityX": -0.08186842286768115, - "velocityY": 0.1724334423754288, - "timestamp": 6.95664858596781 - }, - { - "x": 1.6562452238185605, - "y": 3.90221669421699, - "heading": -0.6460681306884017, - "angularVelocity": 1.3740726329290587, - "velocityX": -0.06232358239700476, - "velocityY": 0.3782351160970439, - "timestamp": 7.015167773190715 - }, - { - "x": 1.6553319250883793, - "y": 3.9350953760295733, - "heading": -0.5183992679116819, - "angularVelocity": 2.1816581677803772, - "velocityX": -0.01560682527428515, - "velocityY": 0.5618444714097892, - "timestamp": 7.07368696041362 - }, - { - "x": 1.6623538133314129, - "y": 3.9788519148323376, - "heading": -0.3621106955703112, - "angularVelocity": 2.670723565350507, - "velocityX": 0.11999292157436166, - "velocityY": 0.7477297768352748, - "timestamp": 7.132206147636525 - }, - { - "x": 1.6832034751641156, - "y": 4.019420947962781, - "heading": -0.22540240918746318, - "angularVelocity": 2.3361275655130602, - "velocityX": 0.35628761816674753, - "velocityY": 0.6932603656286723, - "timestamp": 7.19072533485943 - }, - { - "x": 1.7146647081648767, - "y": 4.052413452071288, - "heading": -0.11878752772286896, - "angularVelocity": 1.8218790541038834, - "velocityX": 0.5376225216683612, - "velocityY": 0.5637895137339223, - "timestamp": 7.249244522082336 - }, - { - "x": 1.7541026264736177, - "y": 4.074977116543675, - "heading": -0.0408151972626467, - "angularVelocity": 1.332423332593777, - "velocityX": 0.6739314091721454, - "velocityY": 0.38557720199428364, - "timestamp": 7.307763709305241 - }, - { - "x": 1.799090355046403, - "y": 4.0866639858943925, - "heading": 1.7934233852716553e-24, - "angularVelocity": 0.6974669198185859, - "velocityX": 0.7687688552717376, - "velocityY": 0.199710042215758, - "timestamp": 7.366282896528146 - }, - { - "x": 1.848745584487915, - "y": 4.087520122528076, - "heading": 7.573339964499894e-25, - "angularVelocity": -5.2358045608260825e-25, - "velocityX": 0.848529034628775, - "velocityY": 0.014630015800164336, - "timestamp": 7.424802083751051 - }, - { - "x": 1.9382598731728313, - "y": 4.089063493944204, - "heading": 6.819777064270339e-25, - "angularVelocity": -2.006050179968467e-26, - "velocityX": 1.1795649340806893, - "velocityY": 0.02033761122914328, - "timestamp": 7.500689630166358 - }, - { - "x": 2.0528956622357724, - "y": 4.091040000742217, - "heading": 6.067470023829544e-25, - "angularVelocity": -1.8954686232133377e-26, - "velocityX": 1.510600809724148, - "velocityY": 0.026045206247625866, - "timestamp": 7.576577176581664 - }, - { - "x": 2.1926529480236727, - "y": 4.09344964285913, - "heading": 5.320081952060173e-25, - "angularVelocity": -2.1711320260805291e-26, - "velocityX": 1.8416366372297248, - "velocityY": 0.03175280043613335, - "timestamp": 7.652464722996971 - }, - { - "x": 2.3575317192504395, - "y": 4.096292420100353, - "heading": 4.5695624647021355e-25, - "angularVelocity": -2.0004263459154622e-26, - "velocityX": 2.1726723160140353, - "velocityY": 0.03746039206044517, - "timestamp": 7.7283522694122775 - }, - { - "x": 2.4972894766259857, - "y": 4.098702070348201, - "heading": 3.822235688125773e-25, - "angularVelocity": -1.9197661326309743e-26, - "velocityX": 1.8416428515253338, - "velocityY": 0.0317529075806614, - "timestamp": 7.804239815827584 - }, - { - "x": 2.611925746748375, - "y": 4.100678585440459, - "heading": 3.063572284762821e-25, - "angularVelocity": -2.4686338901390002e-26, - "velocityX": 1.5106071488334032, - "velocityY": 0.026045315544143462, - "timestamp": 7.880127362242891 - }, - { - "x": 2.701440519692856, - "y": 4.102221965206007, - "heading": 2.3114445870748474e-25, - "angularVelocity": -1.5210807516831905e-26, - "velocityX": 1.1795713153591425, - "velocityY": 0.020337721252726194, - "timestamp": 7.956014908658197 - }, - { - "x": 2.7658337920580536, - "y": 4.1033322095862, - "heading": 1.561656112047754e-25, - "angularVelocity": -1.9040979941775662e-26, - "velocityX": 0.8485354370636105, - "velocityY": 0.01463012618851756, - "timestamp": 8.031902455073505 - }, - { - "x": 2.805105562125262, - "y": 4.104009318551404, - "heading": 8.077058611592956e-26, - "angularVelocity": -3.263981497829794e-26, - "velocityX": 0.5174995361200375, - "velocityY": 0.008922530733819996, - "timestamp": 8.107790001488812 - }, - { - "x": 2.819255828857422, - "y": 4.10425329208374, - "heading": 3.4975515409752656e-27, - "angularVelocity": -3.9242842268499737e-26, - "velocityX": 0.18646362151070262, - "velocityY": 0.0032149350435025517, - "timestamp": 8.18367754790412 - }, - { - "x": 2.810470941680652, - "y": 4.100290874791341, - "heading": -0.013665728423960572, - "angularVelocity": -0.18841669161843644, - "velocityX": -0.12112192828199068, - "velocityY": -0.05463196207942443, - "timestamp": 8.256206835551495 - }, - { - "x": 2.779376565530904, - "y": 4.092132450200435, - "heading": -0.040981035402949714, - "angularVelocity": -0.3766107163742046, - "velocityX": -0.4287147600418107, - "velocityY": -0.11248455424752818, - "timestamp": 8.32873612319887 - }, - { - "x": 2.7259718800290944, - "y": 4.079777209778572, - "heading": -0.08192049181497908, - "angularVelocity": -0.5644541362527821, - "velocityX": -0.7363189028058957, - "velocityY": -0.17034829408406268, - "timestamp": 8.401265410846246 - }, - { - "x": 2.650255783412156, - "y": 4.06322370149856, - "heading": -0.13644943849416394, - "angularVelocity": -0.7518196917125987, - "velocityX": -1.0439382361654468, - "velocityY": -0.22823205379447506, - "timestamp": 8.473794698493622 - }, - { - "x": 2.5522270104067304, - "y": 4.04246950295157, - "heading": -0.20452759410435262, - "angularVelocity": -0.9386298668914527, - "velocityX": -1.3515750145241086, - "velocityY": -0.2861492125483705, - "timestamp": 8.546323986140997 - }, - { - "x": 2.4318844389859766, - "y": 4.01751088765649, - "heading": -0.2861194089559698, - "angularVelocity": -1.1249498995261227, - "velocityX": -1.6592272628656817, - "velocityY": -0.34411775028624336, - "timestamp": 8.618853273788373 - }, - { - "x": 2.2892276526666033, - "y": 3.9883426531177015, - "heading": -0.3812142224775482, - "angularVelocity": -1.311122949172083, - "velocityX": -1.9668852534846832, - "velocityY": -0.40215801760798864, - "timestamp": 8.691382561435748 - }, - { - "x": 2.1330829846519728, - "y": 3.9587634030266514, - "heading": -0.4780632841326763, - "angularVelocity": -1.3353097044877917, - "velocityX": -2.1528498773319837, - "velocityY": -0.40782490839919366, - "timestamp": 8.763911849083124 - }, - { - "x": 1.9992497997330843, - "y": 3.9334042162180256, - "heading": -0.5612966415288853, - "angularVelocity": -1.1475827227322806, - "velocityX": -1.8452295515373094, - "velocityY": -0.349640643541367, - "timestamp": 8.8364411367305 - }, - { - "x": 1.8877256900718635, - "y": 3.9122685100016006, - "heading": -0.6308164435957174, - "angularVelocity": -0.9585066160421192, - "velocityX": -1.537642423891296, - "velocityY": -0.29140926240973475, - "timestamp": 8.908970424377875 - }, - { - "x": 1.7985083705984093, - "y": 3.895358481595443, - "heading": -0.6865270710591681, - "angularVelocity": -0.768112155386183, - "velocityX": -1.2300868017236275, - "velocityY": -0.23314758705988012, - "timestamp": 8.98149971202525 - }, - { - "x": 1.731596065924516, - "y": 3.88267537699245, - "heading": -0.7283506709725182, - "angularVelocity": -0.5766442946012179, - "velocityX": -0.9225556577807338, - "velocityY": -0.17486873254092689, - "timestamp": 9.054028999672626 - }, - { - "x": 1.6869877424298634, - "y": 3.8742198088507074, - "heading": -0.7562373778990242, - "angularVelocity": -0.3844889124250871, - "velocityX": -0.6150387649128701, - "velocityY": -0.11658143097795469, - "timestamp": 9.126558287320002 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -0.19210952061606446, - "velocityX": -0.3075243171780863, - "velocityY": -0.05829081246861152, - "timestamp": 9.199087574967377 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -5.999453085593573e-20, - "velocityX": -1.6973677435282043e-18, - "velocityY": -2.503873223513922e-18, - "timestamp": 9.271616862614753 - }, - { - "x": 1.6770247995246623, - "y": 3.8595873316999896, - "heading": -0.7649878221642326, - "angularVelocity": 0.08507451391469989, - "velocityX": 0.20257164680788678, - "velocityY": -0.1707799926111949, - "timestamp": 9.332541364875802 - }, - { - "x": 1.70170978110981, - "y": 3.8387765844705157, - "heading": -0.7547381408402699, - "angularVelocity": 0.1682357827076709, - "velocityX": 0.405173299231529, - "velocityY": -0.3415825563958536, - "timestamp": 9.39346586713685 - }, - { - "x": 1.7387402457793653, - "y": 3.80755819428449, - "heading": -0.7395583157876386, - "angularVelocity": 0.24915796583103744, - "velocityX": 0.607809063599532, - "velocityY": -0.5124110830197925, - "timestamp": 9.454390369397899 - }, - { - "x": 1.7881185850906471, - "y": 3.7659303409499723, - "heading": -0.7196099497583256, - "angularVelocity": 0.3274276405876679, - "velocityX": 0.8104840824091846, - "velocityY": -0.6832694858326719, - "timestamp": 9.515314871658948 - }, - { - "x": 1.8498475856412335, - "y": 3.713890927230462, - "heading": -0.6950873259687543, - "angularVelocity": 0.4025083977624826, - "velocityX": 1.0132048397554583, - "velocityY": -0.8541623121766772, - "timestamp": 9.576239373919996 - }, - { - "x": 1.9239305360498986, - "y": 3.6514375329357844, - "heading": -0.6662283152580175, - "angularVelocity": 0.47368480069122665, - "velocityX": 1.2159795756924812, - "velocityY": -1.025094862935087, - "timestamp": 9.637163876181045 - }, - { - "x": 2.010371367313072, - "y": 3.5785673636651287, - "heading": -0.6333309419257536, - "angularVelocity": 0.539969505065564, - "velocityX": 1.4188188340512586, - "velocityY": -1.1960732803104883, - "timestamp": 9.698088378442094 - }, - { - "x": 2.1091748328759063, - "y": 3.4952772039974476, - "heading": -0.5967798759520212, - "angularVelocity": 0.5999403297070707, - "velocityX": 1.6217361143055882, - "velocityY": -1.3671044748269063, - "timestamp": 9.759012880703143 - }, - { - "x": 2.220346716936752, - "y": 3.40156341112366, - "heading": -0.5570916262340567, - "angularVelocity": 0.6514333026129457, - "velocityX": 1.82474833498841, - "velocityY": -1.5381954615278515, - "timestamp": 9.819937382964191 - }, - { - "x": 2.3438939700884585, - "y": 3.297422079154605, - "heading": -0.5149984011781104, - "angularVelocity": 0.6909079843703304, - "velocityX": 2.027874641016087, - "velocityY": -1.7093505585459128, - "timestamp": 9.88086188522524 - }, - { - "x": 2.4798242349395756, - "y": 3.182849894813871, - "heading": -0.4716228537300689, - "angularVelocity": 0.7119557130263738, - "velocityX": 2.2311263909664074, - "velocityY": -1.8805600388792227, - "timestamp": 9.941786387486289 - }, - { - "x": 2.628141721858615, - "y": 3.057848265310819, - "heading": -0.42891150330345956, - "angularVelocity": 0.7010537442489162, - "velocityX": 2.4344472488840574, - "velocityY": -2.0517464216194523, - "timestamp": 10.002710889747338 - }, - { - "x": 2.788815804109156, - "y": 2.922450520332472, - "heading": -0.3910899170501554, - "angularVelocity": 0.6207943413513182, - "velocityX": 2.6372654069800667, - "velocityY": -2.2223857389625743, - "timestamp": 10.063635392008386 - }, - { - "x": 2.9613127738384817, - "y": 2.777334038124512, - "heading": -0.375202514467059, - "angularVelocity": 0.2607719717597751, - "velocityX": 2.831323413857593, - "velocityY": -2.3819067341111335, - "timestamp": 10.124559894269435 - }, - { - "x": 3.137446263833113, - "y": 2.628782733735228, - "heading": -0.3752024981016849, - "angularVelocity": 2.6861728103084975e-7, - "velocityX": 2.891012375282727, - "velocityY": -2.438285072117188, - "timestamp": 10.185484396530484 - }, - { - "x": 3.313579787334416, - "y": 2.480231469074176, - "heading": -0.37520248173599324, - "angularVelocity": 2.686224924493493e-7, - "velocityX": 2.8910129252531243, - "velocityY": -2.4382844200276295, - "timestamp": 10.246408898791532 - }, - { - "x": 3.489713311593346, - "y": 2.3316802053114225, - "heading": -0.37520246537030155, - "angularVelocity": 2.6862249292188543e-7, - "velocityX": 2.8910129376886133, - "velocityY": -2.4382844052831705, - "timestamp": 10.307333401052581 - }, - { - "x": 3.6658468653792484, - "y": 2.183128976558075, - "heading": -0.37520244900461003, - "angularVelocity": 2.6862249035440315e-7, - "velocityX": 2.89101342233718, - "velocityY": -2.4382838306472756, - "timestamp": 10.36825790331363 - }, - { - "x": 3.8419803666744587, - "y": 2.034577685567575, - "heading": -0.37520243263891834, - "angularVelocity": 2.686224937536833e-7, - "velocityX": 2.8910125607676815, - "velocityY": -2.4382848521928207, - "timestamp": 10.429182405574679 - }, - { - "x": 4.017984969789346, - "y": 1.8858736976718657, - "heading": -0.37520241627306933, - "angularVelocity": 2.6862507525069377e-7, - "velocityX": 2.8888968573061926, - "velocityY": -2.440791182151078, - "timestamp": 10.490106907835727 - }, - { - "x": 4.198449630537114, - "y": 1.742615181247194, - "heading": -0.37520239987870785, - "angularVelocity": 2.6909307205451447e-7, - "velocityX": 2.9621031612948867, - "velocityY": -2.3514105344815093, - "timestamp": 10.551031410096776 - }, - { - "x": 4.38853011399539, - "y": 1.6123849775845955, - "heading": -0.3752023832167796, - "angularVelocity": 2.734848475989746e-7, - "velocityX": 3.1199349424936167, - "velocityY": -2.137566969436863, - "timestamp": 10.611955912357825 - }, - { - "x": 4.587288186636938, - "y": 1.495827162172114, - "heading": -0.3752023659192684, - "angularVelocity": 2.8391715306138947e-7, - "velocityX": 3.2623667861891117, - "velocityY": -1.9131517055823732, - "timestamp": 10.672880414618874 - }, - { - "x": 4.7937421798706055, - "y": 1.3935176134109497, - "heading": -0.3752023473605566, - "angularVelocity": 3.046181942517293e-7, - "velocityX": 3.3886857597794955, - "velocityY": -1.6792841133570497, - "timestamp": 10.733804916879922 - }, - { - "x": 5.0647852667187605, - "y": 1.2879733857720441, - "heading": -0.3752023309051681, - "angularVelocity": 2.139583629256744e-7, - "velocityX": 3.524191190248399, - "velocityY": -1.3723206946612971, - "timestamp": 10.81071421953017 - }, - { - "x": 5.34411641028699, - "y": 1.2068681953777092, - "heading": -0.37520231550400196, - "angularVelocity": 2.0025101753440068e-7, - "velocityX": 3.6319552244350577, - "velocityY": -1.0545563098285724, - "timestamp": 10.88762352218042 - }, - { - "x": 5.629536746845857, - "y": 1.1508398853670567, - "heading": -0.375202300582326, - "angularVelocity": 1.940165285663064e-7, - "velocityX": 3.711128910592765, - "velocityY": -0.7284984791169633, - "timestamp": 10.964532824830668 - }, - { - "x": 5.917306487549604, - "y": 1.108501764833761, - "heading": -0.37520228573814113, - "angularVelocity": 1.9300896478374464e-7, - "velocityX": 3.741676634521091, - "velocityY": -0.5504941414672958, - "timestamp": 11.041442127480916 - }, - { - "x": 6.2050577552774975, - "y": 1.0660382742134262, - "heading": -0.37520227089383906, - "angularVelocity": 1.9301048840600142e-7, - "velocityX": 3.7414364428249387, - "velocityY": -0.5521242444940722, - "timestamp": 11.118351430131165 - }, - { - "x": 6.492809022919495, - "y": 1.0235747830116217, - "heading": -0.3752022560493111, - "angularVelocity": 1.9301342582848665e-7, - "velocityX": 3.7414364417080774, - "velocityY": -0.5521242520545259, - "timestamp": 11.195260732781414 - }, - { - "x": 6.772658036083171, - "y": 0.9819575118333774, - "heading": -0.34826612342726787, - "angularVelocity": 0.3502324386496837, - "velocityX": 3.638688734915609, - "velocityY": -0.541121421520385, - "timestamp": 11.272170035431662 - }, - { - "x": 7.027520568845826, - "y": 0.9441505058911935, - "heading": -0.3001058813779335, - "angularVelocity": 0.6261952766409414, - "velocityX": 3.313806314454086, - "velocityY": -0.49157910212908845, - "timestamp": 11.34907933808191 - }, - { - "x": 7.256873768560855, - "y": 0.9101570740557017, - "heading": -0.24860478708038097, - "angularVelocity": 0.6696341342705694, - "velocityX": 2.9821255922450605, - "velocityY": -0.44199375971564203, - "timestamp": 11.42598864073216 - }, - { - "x": 7.460704020800595, - "y": 0.8799617502558256, - "heading": -0.1984971865955797, - "angularVelocity": 0.6515154702763284, - "velocityX": 2.65026785077839, - "velocityY": -0.3926095122353607, - "timestamp": 11.502897943382408 - }, - { - "x": 7.6390235862814775, - "y": 0.8535543484261158, - "heading": -0.15201891065632164, - "angularVelocity": 0.6043258011403606, - "velocityX": 2.318569527171569, - "velocityY": -0.3433577073218239, - "timestamp": 11.579807246032656 - }, - { - "x": 7.7918452312093995, - "y": 0.8309283282540864, - "heading": -0.11047967273973482, - "angularVelocity": 0.5401068074364143, - "velocityX": 1.9870371939645617, - "velocityY": -0.29419094169820903, - "timestamp": 11.656716548682905 - }, - { - "x": 7.9191796079863, - "y": 0.812079256076005, - "heading": -0.07474204501348639, - "angularVelocity": 0.46467236725273836, - "velocityX": 1.6556433667844261, - "velocityY": -0.2450818240258775, - "timestamp": 11.733625851333153 - }, - { - "x": 8.021035341477521, - "y": 0.7970039548993032, - "heading": -0.04541767666824434, - "angularVelocity": 0.3812850635065168, - "velocityX": 1.3243616829347478, - "velocityY": -0.1960140146538302, - "timestamp": 11.810535153983402 - }, - { - "x": 8.097419437322872, - "y": 0.7857000289910334, - "heading": -0.022962750583734995, - "angularVelocity": 0.29196632020738633, - "velocityX": 0.9931710887135993, - "velocityY": -0.14697735538800996, - "timestamp": 11.88744445663365 - }, - { - "x": 8.148337646767295, - "y": 0.7781655823556067, - "heading": -0.0077300757248004864, - "angularVelocity": 0.19806023893112515, - "velocityX": 0.662055274067148, - "velocityY": -0.09796534847923602, - "timestamp": 11.964353759283899 - }, - { - "x": 8.173794746398926, - "y": 0.7743990421295166, - "heading": 1.1088837243155707e-19, - "angularVelocity": 0.10050898211824409, - "velocityX": 0.3310015661876223, - "velocityY": -0.04897379245809494, - "timestamp": 12.041263061934147 - }, - { - "x": 8.173794746398926, - "y": 0.7743990421295166, - "heading": 5.633915553103528e-20, - "angularVelocity": 2.8365160942051004e-20, - "velocityX": -1.2324505157130932e-18, - "velocityY": 3.669165812220002e-18, - "timestamp": 12.118172364584396 - } - ] + "samples": [ + { + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": 7.738396447124189e-29, + "angularVelocity": 1.1248344329069217e-28, + "velocityX": -3.4263332588426173e-28, + "velocityY": 2.738469239483736e-27, + "timestamp": 0 + }, + { + "x": 0.4546918803218127, + "y": 4.109218526998405, + "heading": -0.00893730014505659, + "angularVelocity": -0.11893237232009289, + "velocityX": 0.28544257452644645, + "velocityY": -0.1585678993919355, + "timestamp": 0.07514606806129173 + }, + { + "x": 0.49759166351299144, + "y": 4.085387310900469, + "heading": -0.026812413293685552, + "angularVelocity": -0.23787156946188326, + "velocityX": 0.5708852678251672, + "velocityY": -0.3171319100621368, + "timestamp": 0.15029213612258346 + }, + { + "x": 0.5619414537509306, + "y": 4.049641027145361, + "heading": -0.05362171709181762, + "angularVelocity": -0.35676256243061816, + "velocityX": 0.8563294380945293, + "velocityY": -0.47569067387467856, + "timestamp": 0.2254382041838752 + }, + { + "x": 0.6477414523823173, + "y": 4.001980177868811, + "heading": -0.08935853103818607, + "angularVelocity": -0.4755646551888817, + "velocityX": 1.1417762877680475, + "velocityY": -0.634242755558099, + "timestamp": 0.30058427224516693 + }, + { + "x": 0.7549919358856848, + "y": 3.942405375866793, + "heading": -0.13401442198910787, + "angularVelocity": -0.5942545245946725, + "velocityX": 1.4272268166564648, + "velocityY": -0.7927866825104752, + "timestamp": 0.3757303403064587 + }, + { + "x": 0.8836932393387835, + "y": 3.870917341642709, + "heading": -0.18758057155461047, + "angularVelocity": -0.7128270440152931, + "velocityX": 1.7126818045639527, + "velocityY": -0.9513210214242487, + "timestamp": 0.45087640836775045 + }, + { + "x": 1.0338457421573335, + "y": 3.7875168933266514, + "heading": -0.2500489994644535, + "angularVelocity": -0.8312933666588067, + "velocityX": 1.998141841514319, + "velocityY": -1.1098444731404118, + "timestamp": 0.5260224764290422 + }, + { + "x": 1.2054498603433927, + "y": 3.692204931210594, + "heading": -0.3214134495606155, + "angularVelocity": -0.9496764360040053, + "velocityX": 2.2836074143771987, + "velocityY": -1.2683559442966215, + "timestamp": 0.601168544490334 + }, + { + "x": 1.3985060453585743, + "y": 3.5849824250394744, + "heading": -0.4016698579067676, + "angularVelocity": -1.068005424857256, + "velocityX": 2.569078995027635, + "velocityY": -1.4268545106533708, + "timestamp": 0.6763146125516257 + }, + { + "x": 1.5915937570064054, + "y": 3.477819815501255, + "heading": -0.4817685926160567, + "angularVelocity": -1.0659071961550632, + "velocityX": 2.5694985330482245, + "velocityY": -1.426057441233171, + "timestamp": 0.7514606806129175 + }, + { + "x": 1.7632284859071354, + "y": 3.3825660365490977, + "heading": -0.5529765476421505, + "angularVelocity": -0.9475938909806194, + "velocityX": 2.2840147638960824, + "velocityY": -1.2675816767214638, + "timestamp": 0.8266067486742092 + }, + { + "x": 1.9134098098681454, + "y": 3.2992204007592485, + "heading": -0.6152918417750989, + "angularVelocity": -0.8292555517624935, + "velocityX": 1.9985253764510553, + "velocityY": -1.1091150600437194, + "timestamp": 0.901752816735501 + }, + { + "x": 2.042137357772617, + "y": 3.227782305964734, + "heading": -0.6687115003563293, + "angularVelocity": -0.7108776275248302, + "velocityX": 1.7130310504001012, + "velocityY": -0.9506564566524899, + "timestamp": 0.9768988847967928 + }, + { + "x": 2.1494108084908032, + "y": 3.168251239438324, + "heading": -0.7132320417999736, + "angularVelocity": -0.5924533723751416, + "velocityX": 1.4275324509419445, + "velocityY": -0.792204676335884, + "timestamp": 1.0520449528580844 + }, + { + "x": 2.2352298936741635, + "y": 3.120626784366874, + "heading": -0.7488501306754739, + "angularVelocity": -0.4739847312629708, + "velocityX": 1.1420302804580957, + "velocityY": -0.6337584427252518, + "timestamp": 1.127191020919376 + }, + { + "x": 2.299594398786613, + "y": 3.0849086278606843, + "heading": -0.7755632624576652, + "angularVelocity": -0.3554827613921602, + "velocityX": 0.8565252550532846, + "velocityY": -0.475316372868058, + "timestamp": 1.2023370889806677 + }, + { + "x": 2.3425041622841327, + "y": 3.061096568391248, + "heading": -0.7933703919070813, + "angularVelocity": -0.2369668820847947, + "velocityX": 0.5710180799149875, + "velocityY": -0.31687698483458104, + "timestamp": 1.2774831570419594 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288571271453, + "velocityX": 0.2855094390135433, + "velocityY": -0.1584387241831784, + "timestamp": 1.352629225103251 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 1.9944978930198404e-24, + "velocityX": 7.05107927160795e-24, + "velocityY": 9.089761159537798e-24, + "timestamp": 1.4277752931645427 + }, + { + "x": 2.3773573652919286, + "y": 3.040801892787579, + "heading": -0.7860925568420652, + "angularVelocity": 0.2647627943666689, + "velocityX": 0.2192460319725613, + "velocityY": -0.13726925804658435, + "timestamp": 1.4888860514802038 + }, + { + "x": 2.4041795588293358, + "y": 3.024017002068538, + "heading": -0.754166261219504, + "angularVelocity": 0.5224333080216316, + "velocityX": 0.43891115536253184, + "velocityY": -0.27466343376629077, + "timestamp": 1.549996809795865 + }, + { + "x": 2.4444548115194884, + "y": 2.998827368062219, + "heading": -0.7070016065582421, + "angularVelocity": 0.7717897136480927, + "velocityX": 0.6590533942013143, + "velocityY": -0.4121963906290397, + "timestamp": 1.6111075681115261 + }, + { + "x": 2.4982173316537715, + "y": 2.9652220428664715, + "heading": -0.6452345488674253, + "angularVelocity": 1.010739506320076, + "velocityX": 0.8797554083125275, + "velocityY": -0.5499084960157551, + "timestamp": 1.6722183264271873 + }, + { + "x": 2.565507310796468, + "y": 2.9231853998386073, + "heading": -0.5696881555564717, + "angularVelocity": 1.2362208454479753, + "velocityX": 1.1011151063633902, + "velocityY": -0.687876311577224, + "timestamp": 1.7333290847428484 + }, + { + "x": 2.646370899659449, + "y": 2.872694862845298, + "heading": -0.4814293119182948, + "angularVelocity": 1.444243960814317, + "velocityX": 1.3232300022409869, + "velocityY": -0.8262135569077015, + "timestamp": 1.7944398430585096 + }, + { + "x": 2.740859365850477, + "y": 2.8137198450437673, + "heading": -0.38181759469062915, + "angularVelocity": 1.6300193283992952, + "velocityX": 1.5461838274524098, + "velocityY": -0.9650513171003614, + "timestamp": 1.8555506013741707 + }, + { + "x": 2.8490284341196963, + "y": 2.7462234286806457, + "heading": -0.27256619156885004, + "angularVelocity": 1.787760553672932, + "velocityX": 1.7700495174758504, + "velocityY": -1.1044931894720724, + "timestamp": 1.9166613596898319 + }, + { + "x": 2.97093875412634, + "y": 2.6701673256371343, + "heading": -0.15589887958558987, + "angularVelocity": 1.909112490154803, + "velocityX": 1.994907662198022, + "velocityY": -1.2445615983138625, + "timestamp": 1.977772118005493 + }, + { + "x": 3.1066533293644194, + "y": 2.5855194210396055, + "heading": -0.03500682401650803, + "angularVelocity": 1.9782450570262398, + "velocityX": 2.220796779137645, + "velocityY": -1.3851555263033848, + "timestamp": 2.038882876321154 + }, + { + "x": 3.2562037135860464, + "y": 2.492271514146211, + "heading": 0.08476246781347457, + "angularVelocity": 1.959872453412, + "velocityX": 2.4472022331835617, + "velocityY": -1.5258836490251428, + "timestamp": 2.099993634636815 + }, + { + "x": 3.419377549511949, + "y": 2.390544477111736, + "heading": 0.19301716737769203, + "angularVelocity": 1.7714507649379716, + "velocityX": 2.6701327298713187, + "velocityY": -1.6646338523409365, + "timestamp": 2.1611043929524762 + }, + { + "x": 3.5932192698224203, + "y": 2.280069793657614, + "heading": 0.2658871078344446, + "angularVelocity": 1.1924240913580317, + "velocityX": 2.8446991184843475, + "velocityY": -1.8077779837631391, + "timestamp": 2.2222151512681374 + }, + { + "x": 3.777176249725073, + "y": 2.160717630889141, + "heading": 0.3003277545805106, + "angularVelocity": 0.5635774730231045, + "velocityX": 3.010222503743814, + "velocityY": -1.9530466657273664, + "timestamp": 2.2833259095837986 + }, + { + "x": 3.9732480656956923, + "y": 2.03835976913752, + "heading": 0.3003278017850349, + "angularVelocity": 7.724421305582541e-7, + "velocityX": 3.208466420230474, + "velocityY": -2.0022311148487826, + "timestamp": 2.3444366678994597 + }, + { + "x": 4.174665289116757, + "y": 1.9250164613959089, + "heading": 0.3003278489820802, + "angularVelocity": 7.723197446110156e-7, + "velocityX": 3.295937228935466, + "velocityY": -1.8547193794609556, + "timestamp": 2.405547426215121 + }, + { + "x": 4.383567042240086, + "y": 1.82614452102736, + "heading": 0.30032789699541584, + "angularVelocity": 7.856773019778577e-7, + "velocityX": 3.4184120583853495, + "velocityY": -1.6179138190011713, + "timestamp": 2.466658184530782 + }, + { + "x": 4.598915447104952, + "y": 1.7422358380933087, + "heading": 0.30032794702414056, + "angularVelocity": 8.186565851278932e-7, + "velocityX": 3.5239033322497164, + "velocityY": -1.3730591019772607, + "timestamp": 2.527768942846443 + }, + { + "x": 4.819640328386772, + "y": 1.6737075334125113, + "heading": 0.30032800050450253, + "angularVelocity": 8.751382481764384e-7, + "velocityX": 3.611882545159867, + "velocityY": -1.1213787321509214, + "timestamp": 2.5888797011621043 + }, + { + "x": 5.044644747071726, + "y": 1.620900221581336, + "heading": 0.30032805931258105, + "angularVelocity": 9.623195678675265e-7, + "velocityX": 3.681911743309069, + "velocityY": -0.8641246367522506, + "timestamp": 2.6499904594777655 + }, + { + "x": 5.27281048204291, + "y": 1.5840763655637535, + "heading": 0.3003281261036848, + "angularVelocity": 0.0000010929516437587844, + "velocityX": 3.7336426720908507, + "velocityY": -0.6025756680578687, + "timestamp": 2.7111012177934266 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3003281998542592, + "angularVelocity": 0.0000012068345473522654, + "velocityX": 3.7668181767205913, + "velocityY": -0.3380318248368492, + "timestamp": 2.7722119761090878 + }, + { + "x": 5.642202615777798, + "y": 1.556873162710076, + "heading": 0.30032827970972786, + "angularVelocity": 0.0000021672306415301637, + "velocityX": 3.777779819586101, + "velocityY": -0.17764976646888633, + "timestamp": 2.809058755213733 + }, + { + "x": 5.781554061367138, + "y": 1.556248726330051, + "heading": 0.3003283534295918, + "angularVelocity": 0.0000020007139214038515, + "velocityX": 3.781916601002728, + "velocityY": -0.016946837558083135, + "timestamp": 2.8459055343183786 + }, + { + "x": 5.920806157723901, + "y": 1.5615468010933287, + "heading": 0.30032842223361694, + "angularVelocity": 0.000001867300937043874, + "velocityX": 3.7792203210295474, + "velocityY": 0.14378664545497588, + "timestamp": 2.882752313423024 + }, + { + "x": 6.059707308506225, + "y": 1.5727578116463863, + "heading": 0.3003284870919326, + "angularVelocity": 0.000001760216693496236, + "velocityX": 3.7696958637237334, + "velocityY": 0.3042602589826913, + "timestamp": 2.9195990925276694 + }, + { + "x": 6.198006551988053, + "y": 1.5898614987106987, + "heading": 0.3003285487925156, + "angularVelocity": 0.0000016745176784450775, + "velocityX": 3.753360452186466, + "velocityY": 0.4641840475591571, + "timestamp": 2.9564458716323148 + }, + { + "x": 6.335454014584064, + "y": 1.6128269556768218, + "heading": 0.30032860799137623, + "angularVelocity": 0.0000016066223984499512, + "velocityX": 3.730243617919986, + "velocityY": 0.6232690488604353, + "timestamp": 2.99329265073696 + }, + { + "x": 6.47180136240203, + "y": 1.641612684542814, + "heading": 0.30032866524871465, + "angularVelocity": 0.0000015539306228254143, + "velocityX": 3.700387147292773, + "velocityY": 0.7812278186986222, + "timestamp": 3.0301394298416056 + }, + { + "x": 6.606802249418837, + "y": 1.6761666710700263, + "heading": 0.30032872317249165, + "angularVelocity": 0.0000015720173768370867, + "velocityX": 3.6638449899081134, + "velocityY": 0.937774952569887, + "timestamp": 3.066986208946251 + }, + { + "x": 6.740172242433044, + "y": 1.7164148916635715, + "heading": 0.30049565594915995, + "angularVelocity": 0.004530457769298999, + "velocityX": 3.619583482057768, + "velocityY": 1.0923131294390627, + "timestamp": 3.1038329880508964 + }, + { + "x": 6.869834708516544, + "y": 1.760792645305141, + "heading": 0.3094576493704555, + "angularVelocity": 0.2432232515043802, + "velocityX": 3.5189633730334187, + "velocityY": 1.2043862372756071, + "timestamp": 3.1406797671555418 + }, + { + "x": 6.995163652975955, + "y": 1.807928879541571, + "heading": 0.3302942112120631, + "angularVelocity": 0.5654920822911383, + "velocityX": 3.4013541347392664, + "velocityY": 1.2792497846979316, + "timestamp": 3.177526546260187 + }, + { + "x": 7.114961828122759, + "y": 1.8557263226321454, + "heading": 0.3593656658524349, + "angularVelocity": 0.7889822488366823, + "velocityX": 3.2512522955283503, + "velocityY": 1.2971946056622388, + "timestamp": 3.2143733253648326 + }, + { + "x": 7.2288849177114916, + "y": 1.9032201895276672, + "heading": 0.39122292476577963, + "angularVelocity": 0.8645873448767251, + "velocityX": 3.091805915116442, + "velocityY": 1.2889557255639152, + "timestamp": 3.251220104469478 + }, + { + "x": 7.3369415857594324, + "y": 1.9498953701157864, + "heading": 0.4232031564001013, + "angularVelocity": 0.8679247524864317, + "velocityX": 2.932594671058162, + "velocityY": 1.2667370587687046, + "timestamp": 3.2880668835741234 + }, + { + "x": 7.4391897785347085, + "y": 1.9954627576139183, + "heading": 0.4537900488962921, + "angularVelocity": 0.8301103444977762, + "velocityX": 2.774956054771842, + "velocityY": 1.236672203253364, + "timestamp": 3.3249136626787688 + }, + { + "x": 7.535687239808996, + "y": 2.0397404710658504, + "heading": 0.48199026779702103, + "angularVelocity": 0.7653374212340174, + "velocityX": 2.618884570622394, + "velocityY": 1.2016712051325453, + "timestamp": 3.361760441783414 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.681422832753116, + "velocityX": 2.464168636543981, + "velocityY": 1.1632956752609667, + "timestamp": 3.3986072208880596 + }, + { + "x": 7.766066221949162, + "y": 2.1519606772611084, + "heading": 0.5396117040985706, + "angularVelocity": 0.5123769157726334, + "velocityX": 2.199683557182638, + "velocityY": 1.0929921902308588, + "timestamp": 3.4620628503385116 + }, + { + "x": 7.88917922885163, + "y": 2.215677284676183, + "heading": 0.5626038110324001, + "angularVelocity": 0.36233360433028805, + "velocityX": 1.9401431830189138, + "velocityY": 1.0041127629949167, + "timestamp": 3.5255184797889636 + }, + { + "x": 7.996083890855478, + "y": 2.2729699125828824, + "heading": 0.576920084072235, + "angularVelocity": 0.2256107639908224, + "velocityX": 1.6847151770407296, + "velocityY": 0.9028769929929541, + "timestamp": 3.5889741092394156 + }, + { + "x": 8.086991794921751, + "y": 2.3232832101942007, + "heading": 0.5831794167828934, + "angularVelocity": 0.09864109401902421, + "velocityX": 1.4326215791028423, + "velocityY": 0.792889425998124, + "timestamp": 3.6524297386898676 + }, + { + "x": 8.162075125847174, + "y": 2.3662047030478557, + "heading": 0.5818525424229896, + "angularVelocity": -0.02091027023756688, + "velocityX": 1.1832414487992695, + "velocityY": 0.6764016561709427, + "timestamp": 3.7158853681403197 + }, + { + "x": 8.2214756151654, + "y": 2.4014165863300603, + "heading": 0.5733088893147004, + "angularVelocity": -0.1346397976393925, + "velocityX": 0.936094871844373, + "velocityY": 0.5549055865831228, + "timestamp": 3.7793409975907717 + }, + { + "x": 8.265311459891254, + "y": 2.4286667893871807, + "heading": 0.5578458366246501, + "angularVelocity": -0.24368291393475716, + "velocityX": 0.6908109667414808, + "velocityY": 0.4294371246982685, + "timestamp": 3.8427966270412237 + }, + { + "x": 8.29368248788513, + "y": 2.4477506399099855, + "heading": 0.5357077069480222, + "angularVelocity": -0.34887573992019155, + "velocityX": 0.44710025319389435, + "velocityY": 0.30074322306905504, + "timestamp": 3.9062522564916757 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.45085365638085945, + "velocityX": 0.20473385621503526, + "velocityY": 0.16937940002276766, + "timestamp": 3.9697078859421278 + }, + { + "x": 8.303679627930654, + "y": 2.4605245534000177, + "heading": 0.47064149696897567, + "angularVelocity": -0.5538247976382693, + "velocityX": -0.04548808629479942, + "velocityY": 0.03077484607623031, + "timestamp": 4.035535576538194 + }, + { + "x": 8.284168636347566, + "y": 2.453496432112781, + "heading": 0.4276658140977222, + "angularVelocity": -0.6528511403348756, + "velocityX": -0.29639489713850375, + "velocityY": -0.10676542384515514, + "timestamp": 4.1013632671342615 + }, + { + "x": 8.248089021230637, + "y": 2.4374965938671473, + "heading": 0.378472576604384, + "angularVelocity": -0.7473031037226994, + "velocityX": -0.5480917648823836, + "velocityY": -0.24305635061409855, + "timestamp": 4.1671909577303285 + }, + { + "x": 8.195380139344671, + "y": 2.412622948042365, + "heading": 0.3234149472418141, + "angularVelocity": -0.8363901097551141, + "velocityX": -0.8007098746544274, + "velocityY": -0.37785991882067893, + "timestamp": 4.233018648326396 + }, + { + "x": 8.125970395948434, + "y": 2.3789940000965495, + "heading": 0.2629133084829053, + "angularVelocity": -0.9190910118685506, + "velocityX": -1.0544155927047596, + "velocityY": -0.5108632498164021, + "timestamp": 4.298846338922463 + }, + { + "x": 8.039774042344641, + "y": 2.3367560367748186, + "heading": 0.19747799875428093, + "angularVelocity": -0.9940392733834453, + "velocityX": -1.3094239342636282, + "velocityY": -0.6416443131950763, + "timestamp": 4.36467402951853 + }, + { + "x": 7.936686649427289, + "y": 2.286094003355545, + "heading": 0.12774482040942284, + "angularVelocity": -1.0593289497691225, + "velocityX": -1.5660186766982322, + "velocityY": -0.7696158403937725, + "timestamp": 4.430501720114597 + }, + { + "x": 7.816578572926189, + "y": 2.2272487613470173, + "heading": 0.05453269620309629, + "angularVelocity": -1.1121782268737388, + "velocityX": -1.8245828679925904, + "velocityY": -0.8939283981510958, + "timestamp": 4.496329410710664 + }, + { + "x": 7.679285357941873, + "y": 2.160546156413331, + "heading": -0.02105873619647849, + "angularVelocity": -1.1483227151841096, + "velocityX": -2.085645322525112, + "velocityY": -1.0132909772422172, + "timestamp": 4.562157101306731 + }, + { + "x": 7.52459365410468, + "y": 2.086449920391606, + "heading": -0.09747489817686252, + "angularVelocity": -1.16085132697865, + "velocityX": -2.349948819964157, + "velocityY": -1.1256089246149548, + "timestamp": 4.627984791902798 + }, + { + "x": 7.352222023674252, + "y": 2.0056683293260935, + "heading": -0.17237070551570732, + "angularVelocity": -1.1377553528107507, + "velocityX": -2.6185276875067363, + "velocityY": -1.2271673263035494, + "timestamp": 4.693812482498865 + }, + { + "x": 7.1618070022795735, + "y": 1.9194015924783232, + "heading": -0.24188707800247594, + "angularVelocity": -1.0560354139314483, + "velocityX": -2.8926280060941036, + "velocityY": -1.3104931384745382, + "timestamp": 4.759640173094932 + }, + { + "x": 6.953005373134873, + "y": 1.8300479988886789, + "heading": -0.29871595232882114, + "angularVelocity": -0.8632974028370489, + "velocityX": -3.1719421911054577, + "velocityY": -1.3573861209553544, + "timestamp": 4.825467863690999 + }, + { + "x": 6.727423586928556, + "y": 1.7436722229358965, + "heading": -0.32018076563144104, + "angularVelocity": -0.3260757457577048, + "velocityX": -3.426852501791936, + "velocityY": -1.3121495706541393, + "timestamp": 4.891295554287066 + }, + { + "x": 6.489444642291127, + "y": 1.670557467554106, + "heading": -0.32018100817910516, + "angularVelocity": -0.0000036845841301644418, + "velocityX": -3.615179911106431, + "velocityY": -1.1106990799728806, + "timestamp": 4.9571232448831335 + }, + { + "x": 6.246603596847642, + "y": 1.615712184721808, + "heading": -0.3201810383505521, + "angularVelocity": -4.583397455824824e-7, + "velocityX": -3.6890409377052413, + "velocityY": -0.8331643163488851, + "timestamp": 5.022950935479201 + }, + { + "x": 6.0003008879136885, + "y": 1.5794526731151552, + "heading": -0.3201810728782718, + "angularVelocity": -5.245166498428474e-7, + "velocityX": -3.7416276752791178, + "velocityY": -0.5508246040279513, + "timestamp": 5.088778626075268 + }, + { + "x": 5.751956851390291, + "y": 1.561988034181555, + "heading": -0.3201811140764657, + "angularVelocity": -6.258489930223753e-7, + "velocityX": -3.7726378409245966, + "velocityY": -0.265308394924062, + "timestamp": 5.154606316671335 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.3201811662076607, + "angularVelocity": -7.919341312199672e-7, + "velocityX": -3.7818925725102797, + "velocityY": 0.02173781608673755, + "timestamp": 5.220434007267402 + }, + { + "x": 5.250082401017072, + "y": 1.5845061941743261, + "heading": -0.3201812130071943, + "angularVelocity": -6.973782997664008e-7, + "velocityX": -3.7688784475631176, + "velocityY": 0.3142288252976883, + "timestamp": 5.28754182190281 + }, + { + "x": 4.999550332100038, + "y": 1.6250954552906307, + "heading": -0.3201812494932628, + "angularVelocity": -5.436932891401309e-7, + "velocityX": -3.7332771194853676, + "velocityY": 0.6048365803121931, + "timestamp": 5.354649636538218 + }, + { + "x": 4.752908853379521, + "y": 1.6849435065641751, + "heading": -0.32018127950441017, + "angularVelocity": -4.4720793763185246e-7, + "velocityX": -3.6753019012838326, + "velocityY": 0.8918194043226525, + "timestamp": 5.421757451173626 + }, + { + "x": 4.5116361075787825, + "y": 1.7636916636244688, + "heading": -0.32018130527180205, + "angularVelocity": -3.839700641654236e-7, + "velocityX": -3.595300295674319, + "velocityY": 1.1734573311338945, + "timestamp": 5.488865265809034 + }, + { + "x": 4.277178056371613, + "y": 1.8608679654762732, + "heading": -0.32018132820641354, + "angularVelocity": -3.4175768691264823e-7, + "velocityX": -3.493751845160869, + "velocityY": 1.4480623811065336, + "timestamp": 5.555973080444442 + }, + { + "x": 4.05093980766007, + "y": 1.975889994456223, + "heading": -0.3201813492710647, + "angularVelocity": -3.138926699011739e-7, + "velocityX": -3.3712653279007916, + "velocityY": 1.7139885958864325, + "timestamp": 5.62308089507985 + }, + { + "x": 3.834277168836918, + "y": 2.1080683355859073, + "heading": -0.3201813691749636, + "angularVelocity": -2.965958441785772e-7, + "velocityX": -3.2285753902174523, + "velocityY": 1.9696415663034246, + "timestamp": 5.690188709715258 + }, + { + "x": 3.6284883544416036, + "y": 2.2566105084469403, + "heading": -0.32018138848592925, + "angularVelocity": -2.877603113639349e-7, + "velocityX": -3.0665402459214466, + "velocityY": 2.2134854736076965, + "timestamp": 5.757296524350666 + }, + { + "x": 3.4324267312993486, + "y": 2.4177739449564437, + "heading": -0.3201814076661106, + "angularVelocity": -2.8581144314704067e-7, + "velocityX": -2.921591534569326, + "velocityY": 2.4015598985765383, + "timestamp": 5.824404338986074 + }, + { + "x": 3.2363665756420468, + "y": 2.578939166702229, + "heading": -0.3201814268462876, + "angularVelocity": -2.858113772828912e-7, + "velocityX": -2.921569666997558, + "velocityY": 2.401586501086132, + "timestamp": 5.891512153621482 + }, + { + "x": 3.04030639781219, + "y": 2.740104361474713, + "heading": -0.3201814460264912, + "angularVelocity": -2.8581177503839375e-7, + "velocityX": -2.9215699973995357, + "velocityY": 2.4015860991463303, + "timestamp": 5.95861996825689 + }, + { + "x": 2.8448806552051593, + "y": 2.899843193504081, + "heading": -0.32455581469161493, + "angularVelocity": -0.06518419186929905, + "velocityX": -2.912116028047787, + "velocityY": 2.380331305634352, + "timestamp": 6.025727782892298 + }, + { + "x": 2.663237615437101, + "y": 3.049167443178603, + "heading": -0.36928029407596225, + "angularVelocity": -0.6664570978407269, + "velocityX": -2.706734539858174, + "velocityY": 2.225139508502705, + "timestamp": 6.092835597527706 + }, + { + "x": 2.4966866332566564, + "y": 3.186079438683932, + "heading": -0.4222560068283227, + "angularVelocity": -0.7894119789203976, + "velocityX": -2.481841840407185, + "velocityY": 2.0401796161767862, + "timestamp": 6.1599434121631145 + }, + { + "x": 2.3453101524500037, + "y": 3.3105160667275593, + "heading": -0.47671806974270053, + "angularVelocity": -0.8115606686086642, + "velocityX": -2.2557206135987404, + "velocityY": 1.8542792477997239, + "timestamp": 6.2270512267985225 + }, + { + "x": 2.209108176535601, + "y": 3.4224777520191383, + "heading": -0.5296390322123383, + "angularVelocity": -0.7885961233748087, + "velocityX": -2.029599334360373, + "velocityY": 1.6683852081886403, + "timestamp": 6.294159041433931 + }, + { + "x": 2.0880704157388417, + "y": 3.5219728662741354, + "heading": -0.579287382494677, + "angularVelocity": -0.7398296390975455, + "velocityX": -1.8036313871096692, + "velocityY": 1.4826159188098624, + "timestamp": 6.361266856069339 + }, + { + "x": 1.9821863263109802, + "y": 3.609010233643184, + "heading": -0.6245361276970436, + "angularVelocity": -0.6742693894623133, + "velocityX": -1.577820556415411, + "velocityY": 1.2969781215781884, + "timestamp": 6.428374670704747 + }, + { + "x": 1.8914467304783316, + "y": 3.6835977422656017, + "heading": -0.664590534011598, + "angularVelocity": -0.5968664980698198, + "velocityX": -1.3521464873447422, + "velocityY": 1.1114578686200178, + "timestamp": 6.495482485340155 + }, + { + "x": 1.815843887624038, + "y": 3.745742167237612, + "heading": -0.6988590085071317, + "angularVelocity": -0.510648047201536, + "velocityX": -1.1265877642572446, + "velocityY": 0.9260385740414423, + "timestamp": 6.562590299975563 + }, + { + "x": 1.7553712772903527, + "y": 3.7954492682162915, + "heading": -0.7268838015342108, + "angularVelocity": -0.41760848836064807, + "velocityX": -0.9011262050810134, + "velocityY": 0.7407051063834291, + "timestamp": 6.629698114610971 + }, + { + "x": 1.710023362676749, + "y": 3.8327239369169632, + "heading": -0.7483003159780411, + "angularVelocity": -0.3191359241868454, + "velocityX": -0.675747151952063, + "velocityY": 0.5554445321037842, + "timestamp": 6.696805929246379 + }, + { + "x": 1.679795389171883, + "y": 3.8575703362206024, + "heading": -0.7628115166816798, + "angularVelocity": -0.21623712204721673, + "velocityX": -0.4504389491610239, + "velocityY": 0.37024599055457114, + "timestamp": 6.763913743881787 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.10966573621700788, + "velocityX": -0.22519234880908606, + "velocityY": 0.1851003730766039, + "timestamp": 6.831021558517195 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": 8.092112436868569e-26, + "velocityX": -3.5782911869768004e-26, + "velocityY": 1.7219402564933353e-26, + "timestamp": 6.898129373152603 + }, + { + "x": 1.659892350481847, + "y": 3.8800826841683307, + "heading": -0.726477756364864, + "angularVelocity": 0.7466472297695131, + "velocityX": -0.08186840257721437, + "velocityY": 0.1724334716376635, + "timestamp": 6.956648559285806 + }, + { + "x": 1.6562452284597964, + "y": 3.902216695654719, + "heading": -0.6460681483712878, + "angularVelocity": 1.3740725616133136, + "velocityX": -0.06232352606116078, + "velocityY": 0.3782351216574767, + "timestamp": 7.015167745419008 + }, + { + "x": 1.655331933364656, + "y": 3.9350953770599486, + "heading": -0.5183992946484082, + "angularVelocity": 2.1816580536898917, + "velocityX": -0.015606763447832819, + "velocityY": 0.5618444749110212, + "timestamp": 7.073686931552211 + }, + { + "x": 1.6623538228794565, + "y": 3.9788519154462327, + "heading": -0.3621107234214173, + "angularVelocity": 2.67072359603982, + "velocityX": 0.11999294554126883, + "velocityY": 0.7477297836419755, + "timestamp": 7.1322061176854135 + }, + { + "x": 1.6832034861301444, + "y": 4.019420949275189, + "heading": -0.22540243054151132, + "angularVelocity": 2.336127720039156, + "velocityX": 0.35628764903239357, + "velocityY": 0.6932603904745416, + "timestamp": 7.190725303818616 + }, + { + "x": 1.7146647196939016, + "y": 4.052413453417189, + "heading": -0.11878753895047192, + "angularVelocity": 1.8218792610744803, + "velocityX": 0.537622541300291, + "velocityY": 0.5637895248047576, + "timestamp": 7.249244489951819 + }, + { + "x": 1.7541026360793814, + "y": 4.074977117129141, + "heading": -0.04081520050411857, + "angularVelocity": 1.3324234938755106, + "velocityX": 0.6739313888561295, + "velocityY": 0.3855771961795887, + "timestamp": 7.307763676085021 + }, + { + "x": 1.7990903606675779, + "y": 4.0866639859913105, + "heading": 4.0044035166870527e-26, + "angularVelocity": 0.6974669881979226, + "velocityX": 0.7687688014969033, + "velocityY": 0.1997100375861074, + "timestamp": 7.366282862218224 + }, + { + "x": 1.848745584487915, + "y": 4.087520122528076, + "heading": 1.8053103807695408e-26, + "angularVelocity": -4.864634969888421e-27, + "velocityX": 0.848528954372517, + "velocityY": 0.014630014416416372, + "timestamp": 7.4248020483514265 + }, + { + "x": 1.938259872491009, + "y": 4.089063493932448, + "heading": 1.6974407662610403e-26, + "angularVelocity": -2.7257088903490675e-29, + "velocityX": 1.1795648681505524, + "velocityY": 0.020337610092400867, + "timestamp": 7.500689598430333 + }, + { + "x": 2.052895663172084, + "y": 4.09104000075836, + "heading": 1.589827161160837e-26, + "angularVelocity": 6.478274543003658e-30, + "velocityX": 1.510600758120114, + "velocityY": 0.02604520535788869, + "timestamp": 7.57657714850924 + }, + { + "x": 2.1926529528780687, + "y": 4.093449642942828, + "heading": 1.4819575466507505e-26, + "angularVelocity": -2.7257089112437415e-29, + "velocityX": 1.8416365999517403, + "velocityY": 0.03175279979340049, + "timestamp": 7.652464698588147 + }, + { + "x": 2.3575317303228487, + "y": 4.096292420291259, + "heading": 1.3740875703299928e-26, + "angularVelocity": -2.7304766323100225e-29, + "velocityX": 2.1726722930617965, + "velocityY": 0.03746039166471138, + "timestamp": 7.728352248667053 + }, + { + "x": 2.497289491602916, + "y": 4.0987020706064285, + "heading": 1.2662183176306477e-26, + "angularVelocity": -2.720941189252962e-29, + "velocityX": 1.8416428140683072, + "velocityY": 0.031752906934841556, + "timestamp": 7.80423979874596 + }, + { + "x": 2.611925763329844, + "y": 4.10067858572635, + "heading": 1.1583487031205877e-26, + "angularVelocity": -2.725708910890731e-29, + "velocityX": 1.510607097049927, + "velocityY": 0.02604531465131241, + "timestamp": 7.880127348824867 + }, + { + "x": 2.7014405355788997, + "y": 4.1022219654799095, + "heading": 1.0506887852514804e-26, + "angularVelocity": 3.7545940988338324e-31, + "velocityX": 1.1795712492494417, + "velocityY": 0.02033772011288781, + "timestamp": 7.956014898903773 + }, + { + "x": 2.7658338049487097, + "y": 4.103332209808455, + "heading": 9.428654835122248e-27, + "angularVelocity": -2.1154273725241504e-29, + "velocityX": 0.8485353566277292, + "velocityY": 0.014630124801672608, + "timestamp": 8.031902448982681 + }, + { + "x": 2.8051055697205705, + "y": 4.10400931868236, + "heading": 8.349263998410887e-27, + "angularVelocity": -3.641131282577579e-29, + "velocityX": 0.5174994413579915, + "velocityY": 0.008922529099968745, + "timestamp": 8.107789999061588 + }, + { + "x": 2.819255828857422, + "y": 4.10425329208374, + "heading": 5.817602179940264e-27, + "angularVelocity": -1.9173555419457397e-26, + "velocityX": 0.18646351242249948, + "velocityY": 0.0032149331626451292, + "timestamp": 8.183677549140494 + }, + { + "x": 2.810470934258171, + "y": 4.100290875532643, + "heading": -0.013665725456507695, + "angularVelocity": -0.18841665432335325, + "velocityX": -0.12112203294600554, + "velocityY": -0.054631952907971924, + "timestamp": 8.256206835394856 + }, + { + "x": 2.7793765513907402, + "y": 4.0921324527065925, + "heading": -0.04098102601248858, + "angularVelocity": -0.37661063505003184, + "velocityX": -0.4287148608960741, + "velocityY": -0.11248453207493378, + "timestamp": 8.328736121649218 + }, + { + "x": 2.7259718598142455, + "y": 4.079777215271425, + "heading": -0.08192047187969453, + "angularVelocity": -0.5644540017067066, + "velocityX": -0.7363190007027361, + "velocityY": -0.17034825617665642, + "timestamp": 8.40126540790358 + }, + { + "x": 2.650255757673548, + "y": 4.0632237114983365, + "heading": -0.13644940291380203, + "angularVelocity": -0.7518194904451844, + "velocityX": -1.0439383323745826, + "velocityY": -0.22823199603860284, + "timestamp": 8.473794694157942 + }, + { + "x": 2.5522269795422527, + "y": 4.04246951947779, + "heading": -0.20452753616691674, + "angularVelocity": -0.9386295766700757, + "velocityX": -1.3515751111558711, + "velocityY": -0.28614912806063464, + "timestamp": 8.546323980412303 + }, + { + "x": 2.4318844030868068, + "y": 4.017510913730928, + "heading": -0.28611931872413315, + "angularVelocity": -1.1249494758720198, + "velocityX": -1.6592273641491695, + "velocityY": -0.344117625249086, + "timestamp": 8.618853266666665 + }, + { + "x": 2.2892276108991876, + "y": 3.988342694775403, + "heading": -0.38121408025371734, + "angularVelocity": -1.3111222575124248, + "velocityX": -1.9668853721697708, + "velocityY": -0.40215781047715543, + "timestamp": 8.691382552921027 + }, + { + "x": 2.133082954891742, + "y": 3.958763429178337, + "heading": -0.47806319392600216, + "angularVelocity": -1.335310447322373, + "velocityX": -2.1528497531306416, + "velocityY": -0.4078251300216943, + "timestamp": 8.763911839175389 + }, + { + "x": 1.999249779098319, + "y": 3.933404232877465, + "heading": -0.561296583651025, + "angularVelocity": -1.1475831905076381, + "velocityX": -1.8452294611595452, + "velocityY": -0.3496407811313142, + "timestamp": 8.83644112542975 + }, + { + "x": 1.8877256766061599, + "y": 3.91226852016899, + "heading": -0.6308164080928251, + "angularVelocity": -0.9585069429470503, + "velocityX": -1.5376423545799296, + "velocityY": -0.29140935751595565, + "timestamp": 8.908970411684113 + }, + { + "x": 1.7985083626535399, + "y": 3.8953584872647626, + "heading": -0.6865270511993388, + "angularVelocity": -0.7681123858179831, + "velocityX": -1.2300867492302674, + "velocityY": -0.2331476535550589, + "timestamp": 8.981499697938474 + }, + { + "x": 1.7315960620068493, + "y": 3.8826753796533127, + "heading": -0.7283506616380105, + "angularVelocity": -0.57664445079461, + "velocityX": -0.9225556199743571, + "velocityY": -0.17486877737869985, + "timestamp": 9.054028984192836 + }, + { + "x": 1.68698774113937, + "y": 3.874219809689345, + "heading": -0.7562373749577054, + "angularVelocity": -0.38448900795597707, + "velocityX": -0.6150387405032061, + "velocityY": -0.11658145834103056, + "timestamp": 9.126558270447198 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.1921095648592989, + "velocityX": -0.3075243052917468, + "velocityY": -0.05829082515090241, + "timestamp": 9.19908755670156 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": 4.278722998474624e-26, + "velocityX": 4.546149419559626e-26, + "velocityY": -6.368263045423934e-27, + "timestamp": 9.271616842955922 + }, + { + "x": 1.6770242080176545, + "y": 3.8595866692257377, + "heading": -0.7649877966744867, + "angularVelocity": 0.08507499669287022, + "velocityX": 0.20256209128031585, + "velocityY": -0.17079099557937222, + "timestamp": 9.332541299101496 + }, + { + "x": 1.7017080065350787, + "y": 3.838774596868545, + "heading": -0.7547380668498572, + "angularVelocity": 0.16823670612895278, + "velocityX": 0.40515418731755265, + "velocityY": -0.3416045652908843, + "timestamp": 9.393465755247071 + }, + { + "x": 1.738736696509759, + "y": 3.8075542186994835, + "heading": -0.739558172921761, + "angularVelocity": 0.2491592849319001, + "velocityX": 0.6077803942345016, + "velocityY": -0.5124441011744508, + "timestamp": 9.454390211392646 + }, + { + "x": 1.7881126694173817, + "y": 3.76592371429674, + "heading": -0.7196097205107205, + "angularVelocity": 0.32742930627685823, + "velocityX": 0.8104458542829184, + "velocityY": -0.6833135170426513, + "timestamp": 9.51531466753822 + }, + { + "x": 1.849838711755514, + "y": 3.713880986159023, + "heading": -0.6950869960421026, + "angularVelocity": 0.4025103549553654, + "velocityX": 1.0131570512610228, + "velocityY": -0.8542173608142635, + "timestamp": 9.576239123683795 + }, + { + "x": 1.923918112020319, + "y": 3.6514236137874088, + "heading": -0.6662278740694468, + "angularVelocity": 0.4736869854644038, + "velocityX": 1.2159222248582278, + "velocityY": -1.025160934098061, + "timestamp": 9.63716357982937 + }, + { + "x": 2.010354801057128, + "y": 3.578548802416206, + "heading": -0.6333303833823517, + "angularVelocity": 0.5399718400192002, + "velocityX": 1.4187519184459274, + "velocityY": -1.1961503800226454, + "timestamp": 9.698088035974944 + }, + { + "x": 2.109153532122565, + "y": 3.4952533361865883, + "heading": -0.5967791996957547, + "angularVelocity": 0.5999427159310254, + "velocityX": 1.6216596308937807, + "velocityY": -1.367192610313814, + "timestamp": 9.759012492120519 + }, + { + "x": 2.2203200891736463, + "y": 3.4015335717559703, + "heading": -0.5570908397473109, + "angularVelocity": 0.6514356049992648, + "velocityX": 1.824662279880775, + "velocityY": -1.5382946415915537, + "timestamp": 9.819936948266093 + }, + { + "x": 2.3438614224783176, + "y": 3.2973856025788515, + "heading": -0.5149975235515339, + "angularVelocity": 0.690910003286656, + "velocityX": 2.0277790089660783, + "velocityY": -1.7094607940079831, + "timestamp": 9.880861404411668 + }, + { + "x": 2.4797851741645616, + "y": 3.182806114592359, + "heading": -0.47162192283324345, + "angularVelocity": 0.7119571262917361, + "velocityX": 2.231021174181074, + "velocityY": -1.8806813426895879, + "timestamp": 9.941785860557243 + }, + { + "x": 2.628095553772068, + "y": 3.057796514272803, + "heading": -0.4289105902248852, + "angularVelocity": 0.7010539824319861, + "velocityX": 2.4343324338116106, + "velocityY": -2.0518788057927626, + "timestamp": 10.002710316702817 + }, + { + "x": 2.788761933089725, + "y": 2.9223901324293977, + "heading": -0.3910891505574595, + "angularVelocity": 0.6207924052215403, + "velocityX": 2.6371409690347543, + "velocityY": -2.222529184665362, + "timestamp": 10.063634772848392 + }, + { + "x": 2.9612506555206113, + "y": 2.7772643724590593, + "heading": -0.37520163462976347, + "angularVelocity": 0.2607740295577497, + "velocityX": 2.8311901877094443, + "velocityY": -2.382060820101041, + "timestamp": 10.124559228993967 + }, + { + "x": 3.1373758975337274, + "y": 2.6287035614867915, + "heading": -0.37520161826362025, + "angularVelocity": 2.686301076874653e-7, + "velocityX": 2.8908791831030336, + "velocityY": -2.4384429565902384, + "timestamp": 10.185483685139541 + }, + { + "x": 3.313501125191111, + "y": 2.480142733495068, + "heading": -0.3752016018976461, + "angularVelocity": 2.686273330621961e-7, + "velocityX": 2.8908789474713403, + "velocityY": -2.4384432359436565, + "timestamp": 10.246408141285116 + }, + { + "x": 3.4896263528475178, + "y": 2.3315819055021865, + "heading": -0.37520158553167193, + "angularVelocity": 2.686273331449891e-7, + "velocityX": 2.890878947455303, + "velocityY": -2.4384432359626693, + "timestamp": 10.30733259743069 + }, + { + "x": 3.665751580503926, + "y": 2.183021077509307, + "heading": -0.3752015691656978, + "angularVelocity": 2.6862733313245716e-7, + "velocityX": 2.8908789474553287, + "velocityY": -2.438443235962638, + "timestamp": 10.368257053576265 + }, + { + "x": 3.841876808185214, + "y": 2.0344602495459236, + "heading": -0.37520155279972356, + "angularVelocity": 2.6862733360962026e-7, + "velocityX": 2.890878947863701, + "velocityY": -2.4384432354784953, + "timestamp": 10.42918150972184 + }, + { + "x": 4.018002401412412, + "y": 1.8858998549539654, + "heading": -0.3752015364337498, + "angularVelocity": 2.686273261605019e-7, + "velocityX": 2.8908849478501355, + "velocityY": -2.4384361222196733, + "timestamp": 10.490105965867414 + }, + { + "x": 4.198462016558864, + "y": 1.7426352651038108, + "heading": -0.375201520037625, + "angularVelocity": 2.691222177389301e-7, + "velocityX": 2.962022586057329, + "velocityY": -2.351512002139716, + "timestamp": 10.551030422012989 + }, + { + "x": 4.3885379093915455, + "y": 1.6123986719437928, + "heading": -0.3752015033732071, + "angularVelocity": 2.7352591969451763e-7, + "velocityX": 3.119861954590255, + "velocityY": -2.1376734631627445, + "timestamp": 10.611954878158564 + }, + { + "x": 4.587291846391845, + "y": 1.49583415162556, + "heading": -0.37520148607296444, + "angularVelocity": 2.839622014925957e-7, + "velocityX": 3.2623013741048372, + "velocityY": -1.9132632064816424, + "timestamp": 10.672879334304138 + }, + { + "x": 4.7937421798706055, + "y": 1.3935176134109497, + "heading": -0.3752014675107609, + "angularVelocity": 3.0467573669290407e-7, + "velocityX": 3.3886282543985438, + "velocityY": -1.679400107735582, + "timestamp": 10.733803790449713 + }, + { + "x": 5.064781872639773, + "y": 1.2879640439263618, + "heading": -0.3752014510522667, + "angularVelocity": 2.1399857496737658e-7, + "velocityX": 3.52414428888234, + "velocityY": -1.3724410814878705, + "timestamp": 10.810713153560702 + }, + { + "x": 5.344110469349436, + "y": 1.2068492677876004, + "heading": -0.3752014356488163, + "angularVelocity": 2.0028056132299027e-7, + "velocityX": 3.631919254181875, + "velocityY": -1.0546801177082226, + "timestamp": 10.88762251667169 + }, + { + "x": 5.629529124295685, + "y": 1.1508112126364383, + "heading": -0.37520142072574625, + "angularVelocity": 1.940345031855091e-7, + "velocityX": 3.711104128301771, + "velocityY": -0.7286246158389513, + "timestamp": 10.96453187978268 + }, + { + "x": 5.917283115305387, + "y": 1.1083646233041369, + "heading": -0.3752014058811779, + "angularVelocity": 1.9301379957946635e-7, + "velocityX": 3.7414689105466086, + "velocityY": -0.5519040545303511, + "timestamp": 11.041441242893669 + }, + { + "x": 6.205037197250251, + "y": 1.0659186504442004, + "heading": -0.37520139103661015, + "angularVelocity": 1.9301379056768152e-7, + "velocityX": 3.7414700929145033, + "velocityY": -0.5518960389605939, + "timestamp": 11.118350606004658 + }, + { + "x": 6.492791277445097, + "y": 1.0234726657206588, + "heading": -0.37520137619194305, + "angularVelocity": 1.9301508362380675e-7, + "velocityX": 3.741470070160208, + "velocityY": -0.5518961932149626, + "timestamp": 11.195259969115646 + }, + { + "x": 6.772643212230185, + "y": 0.9818724107444464, + "heading": -0.34826678356511775, + "angularVelocity": 0.35021213981392485, + "velocityX": 3.638723862284922, + "velocityY": -0.5408997460579521, + "timestamp": 11.272169332226635 + }, + { + "x": 7.027508446069354, + "y": 0.9440808910715871, + "heading": -0.30010699502671095, + "angularVelocity": 0.6261888876768776, + "velocityX": 3.313838829628223, + "velocityY": -0.49137735828500057, + "timestamp": 11.349078695337624 + }, + { + "x": 7.256864076564165, + "y": 0.9101013911553985, + "heading": -0.24860599276707973, + "angularVelocity": 0.66963241114492, + "velocityX": 2.9821548536792237, + "velocityY": -0.44181226500540305, + "timestamp": 11.425988058448613 + }, + { + "x": 7.460696487188184, + "y": 0.8799184472229736, + "heading": -0.19849831332672846, + "angularVelocity": 0.6515159847057984, + "velocityX": 2.6502938313228213, + "velocityY": -0.39244823661935213, + "timestamp": 11.502897421559602 + }, + { + "x": 7.639017939225139, + "y": 0.8535218749551334, + "heading": -0.15201987225869848, + "angularVelocity": 0.6043274731186726, + "velocityX": 2.3185922340771317, + "velocityY": -0.3432166279903691, + "timestamp": 11.579806784670591 + }, + { + "x": 7.7918411996459325, + "y": 0.8309051352911735, + "heading": -0.11048043087448678, + "angularVelocity": 0.5401090283931487, + "velocityX": 1.987056637047598, + "velocityY": -0.29407004230839406, + "timestamp": 11.65671614778158 + }, + { + "x": 7.919176921526017, + "y": 0.8120637954987597, + "heading": -0.0747425922933183, + "angularVelocity": 0.46467474356269306, + "velocityX": 1.655659554693303, + "velocityY": -0.24498109242204588, + "timestamp": 11.733625510892569 + }, + { + "x": 8.021033730291848, + "y": 0.7969946793008421, + "heading": -0.045418027645302235, + "angularVelocity": 0.3812873161580816, + "velocityX": 1.3243746228770636, + "velocityY": -0.19593344150011263, + "timestamp": 11.810534874003558 + }, + { + "x": 8.097418632050843, + "y": 0.7856953915277307, + "heading": -0.022962936429777186, + "angularVelocity": 0.29196823776995817, + "velocityX": 0.9931807866977714, + "velocityY": -0.14691693333626094, + "timestamp": 11.887444237114547 + }, + { + "x": 8.148337378443449, + "y": 0.7781640366357435, + "heading": -0.007730140883461889, + "angularVelocity": 0.198061652445786, + "velocityX": 0.6620617351768197, + "velocityY": -0.09792507163423873, + "timestamp": 11.964353600225536 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": -6.501598853172508e-27, + "angularVelocity": 0.10050975031878098, + "velocityX": 0.33100479480942824, + "velocityY": -0.04895365601706529, + "timestamp": 12.041262963336525 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": -3.1768871176909477e-27, + "angularVelocity": 1.9243307208194474e-27, + "velocityX": 5.67400379522632e-27, + "velocityY": -1.0775303089806919e-26, + "timestamp": 12.118172326447514 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.1.traj b/src/main/deploy/choreo/SourceLanePGHSprint.1.traj index 161fc9d2..8fbee08c 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.1.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.1.traj @@ -1,175 +1,185 @@ { - "samples": [ - { - "x": 0.433241993188858, - "y": 4.121134281158447, - "heading": 8.991694014476798e-34, - "angularVelocity": 0, - "velocityX": -4.6784915292932193e-35, - "velocityY": -2.119592179507727e-34, - "timestamp": 0 - }, - { - "x": 0.4570749914043126, - "y": 4.107894224868064, - "heading": -0.009932712149027413, - "angularVelocity": -0.1373640763265399, - "velocityX": 0.3295975698116412, - "velocityY": -0.18310287014791135, - "timestamp": 0.07230938695656872 - }, - { - "x": 0.5047410210186508, - "y": 4.0814144720383245, - "heading": -0.029797898632204542, - "angularVelocity": -0.27472486380099975, - "velocityX": 0.6591955985322896, - "velocityY": -0.36620076513225003, - "timestamp": 0.14461877391313743 - }, - { - "x": 0.576240240509544, - "y": 4.041695512105355, - "heading": -0.059590394556189516, - "angularVelocity": -0.41201422357348, - "velocityX": 0.9887958189140486, - "velocityY": -0.5492918914777362, - "timestamp": 0.21692816086970615 - }, - { - "x": 0.6715729183897807, - "y": 3.9887379727274364, - "heading": -0.09930156463679939, - "angularVelocity": -0.5491841620018386, - "velocityX": 1.3183997526835154, - "velocityY": -0.7323743376461849, - "timestamp": 0.28923754782627487 - }, - { - "x": 0.7907394144382421, - "y": 3.9225426248725555, - "heading": -0.148921029894937, - "angularVelocity": -0.686210564721571, - "velocityX": 1.648008662001748, - "velocityY": -0.9154461217412811, - "timestamp": 0.3615469347828436 - }, - { - "x": 0.9337401624459083, - "y": 3.8431103831358566, - "heading": -0.20843840181811515, - "angularVelocity": -0.8230932998910107, - "velocityX": 1.9776235704163385, - "velocityY": -1.0985052574765801, - "timestamp": 0.43385632173941235 - }, - { - "x": 1.100575665552893, - "y": 3.7504423098204516, - "heading": -0.2778446966612386, - "angularVelocity": -0.9598517946889951, - "velocityX": 2.307245436988583, - "velocityY": -1.2815497021301627, - "timestamp": 0.5061657086959811 - }, - { - "x": 1.2912465471408558, - "y": 3.6445396941775194, - "heading": -0.3571330045970933, - "angularVelocity": -1.0965147302863703, - "velocityX": 2.636875924594487, - "velocityY": -1.464576317132112, - "timestamp": 0.5784750956525498 - }, - { - "x": 1.505785276048115, - "y": 3.525464237325283, - "heading": -0.4461366707355329, - "angularVelocity": -1.2308729182270346, - "velocityX": 2.966955438802405, - "velocityY": -1.6467496387952614, - "timestamp": 0.6507844826091186 - }, - { - "x": 1.6964880033537257, - "y": 3.4196219693446848, - "heading": -0.5252603846379951, - "angularVelocity": -1.094238483172127, - "velocityX": 2.637316333772171, - "velocityY": -1.463741741361608, - "timestamp": 0.7230938695656873 - }, - { - "x": 1.8633540969095699, - "y": 3.3270118414974243, - "heading": -0.5945029830335408, - "angularVelocity": -0.9575879607046729, - "velocityX": 2.3076684864728434, - "velocityY": -1.280748347415596, - "timestamp": 0.7954032565222561 - }, - { - "x": 2.0063830653266717, - "y": 3.247633047534605, - "heading": -0.6538614517041024, - "angularVelocity": -0.8208957532196227, - "velocityX": 1.9780138435278007, - "velocityY": -1.0977661034589712, - "timestamp": 0.8677126434788248 - }, - { - "x": 2.125574496423577, - "y": 3.1814849244321213, - "heading": -0.7033318821965574, - "angularVelocity": -0.6841494939262661, - "velocityX": 1.648353500334547, - "velocityY": -0.9147930287697538, - "timestamp": 0.9400220304353936 - }, - { - "x": 2.220928049802523, - "y": 3.128566939134693, - "heading": -0.7429103365332542, - "angularVelocity": -0.5473487745162161, - "velocityX": 1.318688449622979, - "velocityY": -0.7318273259488806, - "timestamp": 1.0123314173919622 - }, - { - "x": 2.292443455008997, - "y": 3.0888786919590703, - "heading": -0.7725937492005902, - "angularVelocity": -0.4105056606988637, - "velocityX": 0.989019658670723, - "velocityY": -0.5488671505327598, - "timestamp": 1.084640804348531 - }, - { - "x": 2.34012050914709, - "y": 3.0624199236271457, - "heading": -0.7923807693651485, - "angularVelocity": -0.273643865580592, - "velocityX": 0.6593480617769434, - "velocityY": -0.3659105607936737, - "timestamp": 1.1569501913050997 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.13679610671265519, - "velocityX": 0.3296745537009426, - "velocityY": -0.18295553238277804, - "timestamp": 1.2292595782616684 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 2.834394108236562e-32, - "velocityX": -3.2904683025944325e-34, - "velocityY": 0, - "timestamp": 1.3015689652182372 - } - ] + "samples": [ + { + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": -1.13547667502199e-28, + "angularVelocity": -4.993890068315335e-29, + "velocityX": 1.9809218936022493e-27, + "velocityY": 1.6056222243913227e-27, + "timestamp": 0 + }, + { + "x": 0.4546918803218126, + "y": 4.109218526998405, + "heading": -0.008937300145056587, + "angularVelocity": -0.11893237232009286, + "velocityX": 0.28544257452644645, + "velocityY": -0.15856789939193544, + "timestamp": 0.07514606806129172 + }, + { + "x": 0.4975916635129914, + "y": 4.085387310900469, + "heading": -0.02681241329368555, + "angularVelocity": -0.2378715694618833, + "velocityX": 0.5708852678251674, + "velocityY": -0.31713191006213676, + "timestamp": 0.15029213612258344 + }, + { + "x": 0.5619414537509306, + "y": 4.049641027145361, + "heading": -0.05362171709181763, + "angularVelocity": -0.3567625624306182, + "velocityX": 0.8563294380945293, + "velocityY": -0.47569067387467845, + "timestamp": 0.22543820418387517 + }, + { + "x": 0.6477414523823173, + "y": 4.001980177868811, + "heading": -0.08935853103818607, + "angularVelocity": -0.47556465518888186, + "velocityX": 1.1417762877680475, + "velocityY": -0.6342427555580988, + "timestamp": 0.3005842722451669 + }, + { + "x": 0.7549919358856847, + "y": 3.942405375866793, + "heading": -0.1340144219891079, + "angularVelocity": -0.5942545245946725, + "velocityX": 1.4272268166564648, + "velocityY": -0.7927866825104751, + "timestamp": 0.3757303403064586 + }, + { + "x": 0.8836932393387834, + "y": 3.870917341642709, + "heading": -0.1875805715546105, + "angularVelocity": -0.7128270440152931, + "velocityX": 1.7126818045639527, + "velocityY": -0.9513210214242486, + "timestamp": 0.4508764083677503 + }, + { + "x": 1.0338457421573333, + "y": 3.7875168933266514, + "heading": -0.2500489994644535, + "angularVelocity": -0.8312933666588067, + "velocityX": 1.998141841514319, + "velocityY": -1.1098444731404116, + "timestamp": 0.526022476429042 + }, + { + "x": 1.2054498603433927, + "y": 3.692204931210594, + "heading": -0.32141344956061546, + "angularVelocity": -0.9496764360040055, + "velocityX": 2.2836074143771987, + "velocityY": -1.2683559442966215, + "timestamp": 0.6011685444903337 + }, + { + "x": 1.3985060453585743, + "y": 3.5849824250394744, + "heading": -0.4016698579067676, + "angularVelocity": -1.0680054248572561, + "velocityX": 2.5690789950276347, + "velocityY": -1.4268545106533708, + "timestamp": 0.6763146125516255 + }, + { + "x": 1.5915937570064054, + "y": 3.477819815501255, + "heading": -0.48176859261605665, + "angularVelocity": -1.0659071961550632, + "velocityX": 2.5694985330482245, + "velocityY": -1.426057441233171, + "timestamp": 0.7514606806129173 + }, + { + "x": 1.7632284859071354, + "y": 3.3825660365490977, + "heading": -0.5529765476421505, + "angularVelocity": -0.9475938909806194, + "velocityX": 2.2840147638960824, + "velocityY": -1.2675816767214638, + "timestamp": 0.826606748674209 + }, + { + "x": 1.9134098098681451, + "y": 3.2992204007592485, + "heading": -0.6152918417750989, + "angularVelocity": -0.8292555517624935, + "velocityX": 1.9985253764510553, + "velocityY": -1.1091150600437196, + "timestamp": 0.9017528167355008 + }, + { + "x": 2.042137357772617, + "y": 3.227782305964734, + "heading": -0.6687115003563293, + "angularVelocity": -0.7108776275248302, + "velocityX": 1.7130310504001012, + "velocityY": -0.95065645665249, + "timestamp": 0.9768988847967925 + }, + { + "x": 2.1494108084908032, + "y": 3.168251239438324, + "heading": -0.7132320417999736, + "angularVelocity": -0.5924533723751417, + "velocityX": 1.4275324509419445, + "velocityY": -0.7922046763358841, + "timestamp": 1.0520449528580842 + }, + { + "x": 2.2352298936741635, + "y": 3.120626784366874, + "heading": -0.7488501306754739, + "angularVelocity": -0.47398473126297086, + "velocityX": 1.1420302804580957, + "velocityY": -0.6337584427252518, + "timestamp": 1.1271910209193758 + }, + { + "x": 2.299594398786613, + "y": 3.0849086278606843, + "heading": -0.7755632624576652, + "angularVelocity": -0.3554827613921603, + "velocityX": 0.8565252550532846, + "velocityY": -0.475316372868058, + "timestamp": 1.2023370889806675 + }, + { + "x": 2.3425041622841327, + "y": 3.061096568391248, + "heading": -0.7933703919070813, + "angularVelocity": -0.23696688208479472, + "velocityX": 0.5710180799149875, + "velocityY": -0.31687698483458104, + "timestamp": 1.2774831570419591 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288571271456, + "velocityX": 0.28550943901354336, + "velocityY": -0.15843872418317836, + "timestamp": 1.3526292251032508 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.4441811570154594e-25, + "velocityX": -6.270976814566572e-25, + "velocityY": -1.3160668498175877e-24, + "timestamp": 1.4277752931645424 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.2.traj b/src/main/deploy/choreo/SourceLanePGHSprint.2.traj index 0c672467..3a141514 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.2.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.2.traj @@ -1,769 +1,815 @@ { - "samples": [ - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 2.834394108236562e-32, - "velocityX": -3.2904683025944325e-34, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 2.379804145625828, - "y": 3.040219511725313, - "heading": -0.7770894823147659, - "angularVelocity": 0.41593022428853316, - "velocityX": 0.2617028389642093, - "velocityY": -0.14816838427057621, - "timestamp": 0.06054604400989727 - }, - { - "x": 2.411575732183553, - "y": 3.0222750861201635, - "heading": -0.7276303412852089, - "angularVelocity": 0.8168847665996504, - "velocityX": 0.524750825215457, - "velocityY": -0.2963765163949562, - "timestamp": 0.12109208801979454 - }, - { - "x": 2.459369710281825, - "y": 2.995353279300503, - "heading": -0.6550403019178791, - "angularVelocity": 1.1989229115524662, - "velocityX": 0.789382343303212, - "velocityY": -0.4446501379224719, - "timestamp": 0.1816381320296918 - }, - { - "x": 2.5233031088128075, - "y": 2.959437775047564, - "heading": -0.5609434444834801, - "angularVelocity": 1.5541371690447268, - "velocityX": 1.0559467522028614, - "velocityY": -0.5931932439230606, - "timestamp": 0.24218417603958908 - }, - { - "x": 2.6035132843463873, - "y": 2.9144863047853526, - "heading": -0.44765744633251, - "angularVelocity": 1.8710718429836914, - "velocityX": 1.3247797910705466, - "velocityY": -0.7424344727603186, - "timestamp": 0.30273022004948635 - }, - { - "x": 2.700148811858614, - "y": 2.8604246981568315, - "heading": -0.31830060076747974, - "angularVelocity": 2.136503675514867, - "velocityX": 1.5960667470930034, - "velocityY": -0.8929007255979282, - "timestamp": 0.3632762640593836 - }, - { - "x": 2.8133605147902916, - "y": 2.7971622097455233, - "heading": -0.1768724119476308, - "angularVelocity": 2.3358782746686186, - "velocityX": 1.869844756713935, - "velocityY": -1.0448657620135586, - "timestamp": 0.4238223080692809 - }, - { - "x": 2.943296329434224, - "y": 2.7246257074465063, - "heading": -0.02888673141070084, - "angularVelocity": 2.4441841404657145, - "velocityX": 2.1460661347699634, - "velocityY": -1.1980386742882738, - "timestamp": 0.48436835207917817 - }, - { - "x": 3.0900160520490236, - "y": 2.6428091950168, - "heading": 0.11617462075416056, - "angularVelocity": 2.3958848928453333, - "velocityX": 2.4232751291036574, - "velocityY": -1.3513106226450158, - "timestamp": 0.5449143960890754 - }, - { - "x": 3.252842131476166, - "y": 2.55207108446101, - "heading": 0.23929724530787766, - "angularVelocity": 2.0335370636864525, - "velocityX": 2.6892934474880965, - "velocityY": -1.4986629108411686, - "timestamp": 0.6054604400989727 - }, - { - "x": 3.4280030327225406, - "y": 2.451410973839388, - "heading": 0.3174619447233944, - "angularVelocity": 1.2909959798981987, - "velocityX": 2.8930197523349617, - "velocityY": -1.662538193332126, - "timestamp": 0.66600648410887 - }, - { - "x": 3.6156107870917813, - "y": 2.340964520507575, - "heading": 0.3503075182755731, - "angularVelocity": 0.5424891764490772, - "velocityX": 3.098596406043856, - "velocityY": -1.8241729106819762, - "timestamp": 0.726552528118767 - }, - { - "x": 3.8155697764375462, - "y": 2.229387834544172, - "heading": 0.3503075829776931, - "angularVelocity": 0.0000010686432293648565, - "velocityX": 3.302593796434966, - "velocityY": -1.8428402348659505, - "timestamp": 0.7870985721286641 - }, - { - "x": 4.015529033815858, - "y": 2.117811628928195, - "heading": 0.35030764767858236, - "angularVelocity": 0.000001068622902276308, - "velocityX": 3.3025982233558384, - "velocityY": -1.8428323012769912, - "timestamp": 0.8476446161385611 - }, - { - "x": 4.215488291220884, - "y": 2.0062354233600974, - "heading": 0.35030771237947195, - "angularVelocity": 0.0000010686229082180607, - "velocityX": 3.302598223797092, - "velocityY": -1.8428323004862048, - "timestamp": 0.9081906601484582 - }, - { - "x": 4.415447878546615, - "y": 1.8946598090554148, - "heading": 0.350307777080342, - "angularVelocity": 0.0000010686225853054014, - "velocityX": 3.3026036728847936, - "velocityY": -1.842822534969312, - "timestamp": 0.9687367041583552 - }, - { - "x": 4.620289311400001, - "y": 1.7923224180578192, - "heading": 0.35030784179343544, - "angularVelocity": 0.00000106882447086681, - "velocityX": 3.383233970164951, - "velocityY": -1.6902407526553997, - "timestamp": 1.0292827481682523 - }, - { - "x": 4.832980772388526, - "y": 1.7074973259478416, - "heading": 0.35030790812662205, - "angularVelocity": 0.0000010955825050486418, - "velocityX": 3.512887827217186, - "velocityY": -1.401001394841118, - "timestamp": 1.0898287921781493 - }, - { - "x": 5.052028438396142, - "y": 1.6407809635052637, - "heading": 0.3503079787205065, - "angularVelocity": 0.0000011659537069164618, - "velocityX": 3.617869170309609, - "velocityY": -1.101911174108616, - "timestamp": 1.1503748361880464 - }, - { - "x": 5.275893582425239, - "y": 1.5926421132936275, - "heading": 0.3503080570287918, - "angularVelocity": 0.0000012933674954586782, - "velocityX": 3.697436350961301, - "velocityY": -0.7950783738037029, - "timestamp": 1.2109208801979434 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.35030814446785413, - "angularVelocity": 0.0000014441746575542642, - "velocityX": 3.751029791428781, - "velocityY": -0.4826595916936126, - "timestamp": 1.2714669242078405 - }, - { - "x": 5.645916996346935, - "y": 1.5526078041590061, - "heading": 0.35030823517092025, - "angularVelocity": 0.0000023934601751232044, - "velocityX": 3.7711793420471578, - "velocityY": -0.28528395446410554, - "timestamp": 1.3093631328272466 - }, - { - "x": 5.78920070535826, - "y": 1.5493061559809091, - "heading": 0.350308318678533, - "angularVelocity": 0.000002203587530623072, - "velocityX": 3.7809510299653186, - "velocityY": -0.08712344317225716, - "timestamp": 1.3472593414466527 - }, - { - "x": 5.932460399917018, - "y": 1.5535231156733713, - "heading": 0.3503083968024594, - "angularVelocity": 0.0000020615235464554193, - "velocityX": 3.7803173398564307, - "velocityY": 0.11127655895113732, - "timestamp": 1.3851555500660588 - }, - { - "x": 6.075301824148574, - "y": 1.5652470604158988, - "heading": 0.3503084709306375, - "angularVelocity": 0.0000019560842841988308, - "velocityX": 3.769280079337772, - "velocityY": 0.3093698596678062, - "timestamp": 1.423051758685465 - }, - { - "x": 6.217331878166829, - "y": 1.5844456932271112, - "heading": 0.3503085421651287, - "angularVelocity": 0.0000018797260658699983, - "velocityX": 3.747869752477631, - "velocityY": 0.5066109120314745, - "timestamp": 1.460947967304871 - }, - { - "x": 6.358159707247505, - "y": 1.6110661098029424, - "heading": 0.3503086114142786, - "angularVelocity": 0.000001827337150563986, - "velocityX": 3.716145604300894, - "velocityY": 0.7024559328132687, - "timestamp": 1.4988441759242772 - }, - { - "x": 6.497397807591943, - "y": 1.6450348544134032, - "heading": 0.35030867945721406, - "angularVelocity": 0.0000017955077283623068, - "velocityX": 3.6741960585771425, - "velocityY": 0.8963626137804634, - "timestamp": 1.5367403845436833 - }, - { - "x": 6.634663343210931, - "y": 1.6862573848132372, - "heading": 0.3503087469924593, - "angularVelocity": 0.000001782110868887289, - "velocityX": 3.622144288827235, - "velocityY": 1.0877745268355006, - "timestamp": 1.5746365931630895 - }, - { - "x": 6.770832471288569, - "y": 1.7309686010151983, - "heading": 0.350308814465375, - "angularVelocity": 0.0000017804661250723641, - "velocityX": 3.593212435713354, - "velocityY": 1.179833493397679, - "timestamp": 1.6125328017824956 - }, - { - "x": 6.907000934216078, - "y": 1.775681842897667, - "heading": 0.35030888193834014, - "angularVelocity": 0.0000017804674286764754, - "velocityX": 3.5931948838222003, - "velocityY": 1.1798869467794602, - "timestamp": 1.6504290104019017 - }, - { - "x": 7.041712141501968, - "y": 1.8246105114710787, - "heading": 0.3503089535974655, - "angularVelocity": 0.0000018909312562539825, - "velocityX": 3.5547410201057947, - "velocityY": 1.2911230530949709, - "timestamp": 1.6883252190213078 - }, - { - "x": 7.171770756302622, - "y": 1.8790616059317657, - "heading": 0.3592109050640362, - "angularVelocity": 0.2349034848307258, - "velocityX": 3.43196904225537, - "velocityY": 1.43684807648023, - "timestamp": 1.726221427640714 - }, - { - "x": 7.296132052444026, - "y": 1.9323306109051834, - "heading": 0.3900794027691138, - "angularVelocity": 0.8145537200064432, - "velocityX": 3.281628972184848, - "velocityY": 1.4056552598282668, - "timestamp": 1.76411763626012 - }, - { - "x": 7.41350002407911, - "y": 1.9843017273758428, - "heading": 0.4310938047294686, - "angularVelocity": 1.0822824618754054, - "velocityX": 3.097090076050037, - "velocityY": 1.371406754501705, - "timestamp": 1.8020138448795262 - }, - { - "x": 7.523578248846203, - "y": 2.0344435812093704, - "heading": 0.471270328487765, - "angularVelocity": 1.0601726458124487, - "velocityX": 2.904729226942307, - "velocityY": 1.3231364207724636, - "timestamp": 1.8399100534989323 - }, - { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.9454290339278092, - "velocityX": 2.715460784576445, - "velocityY": 1.2708550641540455, - "timestamp": 1.8778062621183385 - }, - { - "x": 7.782116606596641, - "y": 2.1591418247265515, - "heading": 0.5502904833862925, - "angularVelocity": 0.6648323287723413, - "velocityX": 2.395575421885603, - "velocityY": 1.1781054843634051, - "timestamp": 1.9427729869972457 - }, - { - "x": 7.917274495842872, - "y": 2.2282082387650113, - "heading": 0.5787513876779874, - "angularVelocity": 0.4380843323215672, - "velocityX": 2.0804171596791274, - "velocityY": 1.0631044456557224, - "timestamp": 2.0077397118761526 - }, - { - "x": 8.032169166662662, - "y": 2.2890615062609574, - "heading": 0.5943003276772478, - "angularVelocity": 0.2393369840982844, - "velocityX": 1.768515667581298, - "velocityY": 0.9366836270316962, - "timestamp": 2.07270643675506 - }, - { - "x": 8.12694730000964, - "y": 2.3412528312827354, - "heading": 0.5980433212943994, - "angularVelocity": 0.05761401123618554, - "velocityX": 1.458871961356148, - "velocityY": 0.8033547191898265, - "timestamp": 2.137673161633967 - }, - { - "x": 8.201715109739398, - "y": 2.3844820330172567, - "heading": 0.590722775552934, - "angularVelocity": -0.11268146509017382, - "velocityX": 1.1508631513920338, - "velocityY": 0.6654052796273325, - "timestamp": 2.2026398865128742 - }, - { - "x": 8.256552657549896, - "y": 2.418534329374767, - "heading": 0.572871769156374, - "angularVelocity": -0.27477153003838295, - "velocityX": 0.8440866907283819, - "velocityY": 0.5241498077820845, - "timestamp": 2.2676066113917814 - }, - { - "x": 8.291522317600203, - "y": 2.443248428942356, - "heading": 0.5448922111974887, - "angularVelocity": -0.4306752112106213, - "velocityX": 0.538270324007952, - "velocityY": 0.3804116586399298, - "timestamp": 2.3325733362706886 - }, - { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.5817394500892022, - "velocityX": 0.23322225384012837, - "velocityY": 0.2347399755865949, - "timestamp": 2.397540061149596 - }, - { - "x": 8.301908465997242, - "y": 2.464162051150854, - "heading": 0.4594182049840147, - "angularVelocity": -0.7297490572088752, - "velocityX": -0.07293676039464714, - "velocityY": 0.08667758549500083, - "timestamp": 2.4628780001812522 - }, - { - "x": 8.277129357589926, - "y": 2.4601442119160213, - "heading": 0.4025234718451046, - "angularVelocity": -0.8707763664131557, - "velocityX": -0.3792453324141557, - "velocityY": -0.061493204321711674, - "timestamp": 2.5282159392129087 - }, - { - "x": 8.232325554908503, - "y": 2.44643699593842, - "heading": 0.33696231020008877, - "angularVelocity": -1.0034164318107932, - "velocityX": -0.6857241496355521, - "velocityY": -0.20978953699413314, - "timestamp": 2.593553878244565 - }, - { - "x": 8.167484390379387, - "y": 2.42303097033958, - "heading": 0.26340235513425153, - "angularVelocity": -1.1258383131766259, - "velocityX": -0.992396844621899, - "velocityY": -0.35823024028199546, - "timestamp": 2.6588918172762215 - }, - { - "x": 8.082591447673355, - "y": 2.3899154191246006, - "heading": 0.18267464694130434, - "angularVelocity": -1.235541086685248, - "velocityX": -1.2992901821543714, - "velocityY": -0.5068349523381112, - "timestamp": 2.724229756307878 - }, - { - "x": 7.9776304347285665, - "y": 2.347078349358033, - "heading": 0.09584952732096214, - "angularVelocity": -1.328862233904795, - "velocityX": -1.6064328704022224, - "velocityY": -0.6556232167931427, - "timestamp": 2.7895676953395343 - }, - { - "x": 7.852583598052917, - "y": 2.2945065473278667, - "heading": 0.004378357289613106, - "angularVelocity": -1.3999702376138785, - "velocityX": -1.9138472766192256, - "velocityY": -0.8046137176854637, - "timestamp": 2.8549056343711907 - }, - { - "x": 7.707434523289432, - "y": 2.232185897198679, - "heading": -0.08962534429630348, - "angularVelocity": -1.4387307432573238, - "velocityX": -2.221512905283988, - "velocityY": -0.9538202620531743, - "timestamp": 2.920243573402847 - }, - { - "x": 7.542180705960218, - "y": 2.1601037497785995, - "heading": -0.1827828763211059, - "angularVelocity": -1.42578008130418, - "velocityX": -2.5292168650919318, - "velocityY": -1.1032204028528767, - "timestamp": 2.9855815124345035 - }, - { - "x": 7.35689136818444, - "y": 2.078264678726032, - "heading": -0.2689709068831343, - "angularVelocity": -1.3191115581449608, - "velocityX": -2.835861377353881, - "velocityY": -1.252550543611691, - "timestamp": 3.05091945146616 - }, - { - "x": 7.152059282385439, - "y": 1.9867952525471149, - "heading": -0.3348300100346558, - "angularVelocity": -1.007976439532523, - "velocityX": -3.134963986234081, - "velocityY": -1.3999435478765458, - "timestamp": 3.1162573904978164 - }, - { - "x": 6.932416862636984, - "y": 1.8843357350106482, - "heading": -0.3485403575636041, - "angularVelocity": -0.20983746552376023, - "velocityX": -3.3616367917885555, - "velocityY": -1.5681473743275887, - "timestamp": 3.1815953295294728 - }, - { - "x": 6.70958980281502, - "y": 1.7775224962078198, - "heading": -0.34854038203919246, - "angularVelocity": -3.7459994478515855e-7, - "velocityX": -3.410377846690442, - "velocityY": -1.6347812677571678, - "timestamp": 3.246933268561129 - }, - { - "x": 6.478027967608038, - "y": 1.6912665812341474, - "heading": -0.3485404067374628, - "angularVelocity": -3.780081020278718e-7, - "velocityX": -3.5440639640437577, - "velocityY": -1.3201505320191125, - "timestamp": 3.3122712075927856 - }, - { - "x": 6.239619726654668, - "y": 1.6262859956866853, - "heading": -0.34854043255776096, - "angularVelocity": -3.951807866461482e-7, - "velocityX": -3.6488485019072954, - "velocityY": -0.9945306893745133, - "timestamp": 3.377609146624442 - }, - { - "x": 5.996315346781838, - "y": 1.5831125822372847, - "heading": -0.3485404605967219, - "angularVelocity": -4.291375179720768e-7, - "velocityX": -3.7237841211206324, - "velocityY": -0.6607709714945885, - "timestamp": 3.4429470856560984 - }, - { - "x": 5.7501052372367845, - "y": 1.5620996014320112, - "heading": -0.34854049237288, - "angularVelocity": -4.863354823397662e-7, - "velocityX": -3.7682564401941674, - "velocityY": -0.3216045855853207, - "timestamp": 3.508285024687755 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.348540530032263, - "angularVelocity": -5.763784949174497e-7, - "velocityX": -3.7819013522532754, - "velocityY": 0.02019321393800792, - "timestamp": 3.5736229637194112 - }, - { - "x": 5.265773365103949, - "y": 1.585443267719361, - "heading": -0.3485405619317938, - "angularVelocity": -5.063689324990554e-7, - "velocityX": -3.7657613238076486, - "velocityY": 0.34961055977354083, - "timestamp": 3.636619582930451 - }, - { - "x": 5.031364041570911, - "y": 1.6280522215500413, - "heading": -0.3485405890146467, - "angularVelocity": -4.299096234955573e-7, - "velocityX": -3.720982593490641, - "velocityY": 0.6763688966853497, - "timestamp": 3.699616202141491 - }, - { - "x": 4.801558310439742, - "y": 1.6909217775230576, - "heading": -0.3485406130254673, - "angularVelocity": -3.811445901784285e-7, - "velocityX": -3.6479057766785146, - "velocityY": 0.9979830149678419, - "timestamp": 3.7626128213525307 - }, - { - "x": 4.57810382416931, - "y": 1.7735737496127562, - "heading": -0.34854063513060685, - "angularVelocity": -3.5089406173482316e-7, - "velocityX": -3.5470869559183034, - "velocityY": 1.3120064715347763, - "timestamp": 3.8256094405635706 - }, - { - "x": 4.3626998372206165, - "y": 1.8753793427019887, - "heading": -0.34854065619045194, - "angularVelocity": -3.3430119484907394e-7, - "velocityX": -3.4192943946259384, - "velocityY": 1.6160485175907584, - "timestamp": 3.8886060597746104 - }, - { - "x": 4.156979523974483, - "y": 1.9955559179970916, - "heading": -0.34854067691382595, - "angularVelocity": -3.2896009785914497e-7, - "velocityX": -3.265577039253029, - "velocityY": 1.907667058965506, - "timestamp": 3.95160267898565 - }, - { - "x": 3.9516956402233028, - "y": 2.116476485340398, - "heading": -0.3485406976311522, - "angularVelocity": -3.288640962267947e-7, - "velocityX": -3.258649215182055, - "velocityY": 1.9194770903216498, - "timestamp": 4.01459929819669 - }, - { - "x": 3.746411773108691, - "y": 2.2373970809271935, - "heading": -0.34854071834847844, - "angularVelocity": -3.288640958736373e-7, - "velocityX": -3.2586489510953776, - "velocityY": 1.9194775386550607, - "timestamp": 4.07759591740773 - }, - { - "x": 3.5411278789507334, - "y": 2.3583176306028646, - "heading": -0.34854073906621164, - "angularVelocity": -3.2887055643589624e-7, - "velocityX": -3.2586493803778747, - "velocityY": 1.919476809867947, - "timestamp": 4.14059253661877 - }, - { - "x": 3.345073520020233, - "y": 2.47307412374708, - "heading": -0.3873766723080469, - "angularVelocity": -0.6164764669629881, - "velocityX": -3.1121409590840567, - "velocityY": 1.821629391884994, - "timestamp": 4.2035891558298095 - }, - { - "x": 3.1664636949700515, - "y": 2.5778338770119946, - "heading": -0.44780580157610866, - "angularVelocity": -0.9592440042793773, - "velocityX": -2.835228735876023, - "velocityY": 1.6629424654356146, - "timestamp": 4.266585775040849 - }, - { - "x": 3.0057996639738764, - "y": 2.672135804524931, - "heading": -0.5103083460228613, - "angularVelocity": -0.9921571225491842, - "velocityX": -2.5503595749788768, - "velocityY": 1.496936322837019, - "timestamp": 4.329582394251889 - }, - { - "x": 2.8630598597209174, - "y": 2.7559513152021253, - "heading": -0.5701467016897592, - "angularVelocity": -0.9498661422835671, - "velocityX": -2.265832770720234, - "velocityY": 1.3304763291568347, - "timestamp": 4.392579013462929 - }, - { - "x": 2.738214483951598, - "y": 2.829279302871158, - "heading": -0.6251012943842897, - "angularVelocity": -0.8723419348970285, - "velocityX": -1.9817789800923042, - "velocityY": 1.1639987762419943, - "timestamp": 4.455575632673969 - }, - { - "x": 2.6312405711995455, - "y": 2.892122971419519, - "heading": -0.6738749327170909, - "angularVelocity": -0.7742262829916016, - "velocityX": -1.698089740239665, - "velocityY": 0.9975720814133604, - "timestamp": 4.518572251885009 - }, - { - "x": 2.5421208919645824, - "y": 2.9444859859038597, - "heading": -0.7156160888836572, - "angularVelocity": -0.6625935913597552, - "velocityX": -1.414673999193671, - "velocityY": 0.8312035652091746, - "timestamp": 4.5815688710960485 - }, - { - "x": 2.4708422721118457, - "y": 2.98637166116498, - "heading": -0.749723844710458, - "angularVelocity": -0.5414220041322991, - "velocityX": -1.1314673826217794, - "velocityY": 0.6648876683493641, - "timestamp": 4.644565490307088 - }, - { - "x": 2.4173943837507443, - "y": 3.0177828179574866, - "heading": -0.775752501403198, - "angularVelocity": -0.4131754532023893, - "velocityX": -0.84842470961893, - "velocityY": 0.4986165477750299, - "timestamp": 4.707562109518128 - }, - { - "x": 2.3817689275168203, - "y": 3.038721799552771, - "heading": -0.7933591587514436, - "angularVelocity": -0.2794857496917854, - "velocityX": -0.565513779629629, - "velocityY": 0.3323826239808009, - "timestamp": 4.770558728729168 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.14148780267529462, - "velocityX": -0.28271125846882317, - "velocityY": 0.16617910323716242, - "timestamp": 4.833555347940208 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 1.2572572851920672e-28, - "velocityX": 2.1335040537014396e-30, - "velocityY": 1.8634721866519574e-29, - "timestamp": 4.896551967151248 - } - ] + "samples": [ + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.4441811570154594e-25, + "velocityX": -6.270976814566572e-25, + "velocityY": -1.3160668498175877e-24, + "timestamp": 0 + }, + { + "x": 2.3773546784477104, + "y": 3.0407954087369538, + "heading": -0.78607704068416, + "angularVelocity": 0.26500042843772503, + "velocityX": 0.21918860936679116, + "velocityY": -0.13736692846187587, + "timestamp": 0.061114509855337396 + }, + { + "x": 2.4041715280717404, + "y": 3.023997545091338, + "heading": -0.7541197912958612, + "angularVelocity": 0.5229077262329992, + "velocityX": 0.4387967716260398, + "velocityY": -0.27485884588418563, + "timestamp": 0.12222901971067479 + }, + { + "x": 2.444438816560299, + "y": 2.9987884456278273, + "heading": -0.7069088629247025, + "angularVelocity": 0.772499501066286, + "velocityX": 0.6588826218826678, + "velocityY": -0.41248959573074695, + "timestamp": 0.1833435295660122 + }, + { + "x": 2.498190797810618, + "y": 2.965157151416224, + "heading": -0.6450804742614368, + "angularVelocity": 1.0116810035721142, + "velocityX": 0.8795289592856741, + "velocityY": -0.5502996635530664, + "timestamp": 0.24445803942134958 + }, + { + "x": 2.565467715954461, + "y": 2.92308800675928, + "heading": -0.5694581744936127, + "angularVelocity": 1.2373869960967963, + "velocityX": 1.1008338004034173, + "velocityY": -0.6883659012650978, + "timestamp": 0.305572549276687 + }, + { + "x": 2.6463157744818333, + "y": 2.8725583790255045, + "heading": -0.4811095208345568, + "angularVelocity": 1.4456248420904245, + "velocityX": 1.3228946565839355, + "velocityY": -0.8268024705325063, + "timestamp": 0.3666870591320244 + }, + { + "x": 2.7407862852871125, + "y": 2.8135375953698065, + "heading": -0.3813947607246218, + "angularVelocity": 1.6316053314665737, + "velocityX": 1.5457951152500125, + "velocityY": -0.9657409311700984, + "timestamp": 0.42780156898736177 + }, + { + "x": 2.8489350075595548, + "y": 2.74598863201195, + "heading": -0.27202744081575525, + "angularVelocity": 1.7895475259107383, + "velocityX": 1.7696079462706686, + "velocityY": -1.1052852017916852, + "timestamp": 0.48891607884269916 + }, + { + "x": 2.97082263377999, + "y": 2.6698730886283255, + "heading": -0.15523118772045078, + "angularVelocity": 1.9111051266183738, + "velocityX": 1.994413871745882, + "velocityY": -1.2454578063997521, + "timestamp": 0.5500305886980366 + }, + { + "x": 3.1065122610280413, + "y": 2.5851587283166806, + "heading": -0.034196755068831956, + "angularVelocity": 1.9804532988666075, + "velocityX": 2.220252237467639, + "velocityY": -1.3861578946173312, + "timestamp": 0.611145098553374 + }, + { + "x": 3.2560356540344637, + "y": 2.4918371862270576, + "heading": 0.08572827567170235, + "angularVelocity": 1.9623004589974744, + "velocityX": 2.4466103607859297, + "velocityY": -1.5269948545856247, + "timestamp": 0.6722596084087114 + }, + { + "x": 3.419180856935893, + "y": 2.390029020511174, + "heading": 0.1941501841545546, + "angularVelocity": 1.7740780174707271, + "velocityX": 2.669500308316405, + "velocityY": -1.6658591545096526, + "timestamp": 0.7333741182640487 + }, + { + "x": 3.5929644614197396, + "y": 2.2794553379158704, + "heading": 0.2670576621771365, + "angularVelocity": 1.1929651108248978, + "velocityX": 2.843573562075607, + "velocityY": -1.8092869084124177, + "timestamp": 0.7944886281193861 + }, + { + "x": 3.7768655447565394, + "y": 2.1600020217160374, + "heading": 0.3015223680601122, + "angularVelocity": 0.5639365506580378, + "velocityX": 3.009123099769713, + "velocityY": -1.9545819230586587, + "timestamp": 0.8556031379747235 + }, + { + "x": 3.972896711212453, + "y": 2.037552262177189, + "heading": 0.301522415266864, + "angularVelocity": 7.724311622068364e-7, + "velocityX": 3.2076043302962565, + "velocityY": -2.003611905400141, + "timestamp": 0.9167176478300609 + }, + { + "x": 4.174386731425729, + "y": 1.9243094542990702, + "heading": 0.30152246246678105, + "angularVelocity": 7.723193244877141e-7, + "velocityX": 3.29692606044323, + "velocityY": -1.8529610749750456, + "timestamp": 0.9778321576853983 + }, + { + "x": 4.383354455500062, + "y": 1.8255438133936408, + "heading": 0.30152251049528017, + "angularVelocity": 7.858771882784632e-7, + "velocityX": 3.419281682352931, + "velocityY": -1.6160751536617886, + "timestamp": 1.0389466675407357 + }, + { + "x": 4.5987615422573755, + "y": 1.741746749013425, + "heading": 0.30152256055333204, + "angularVelocity": 8.190862036393596e-7, + "velocityX": 3.52464721172105, + "velocityY": -1.3711484323210577, + "timestamp": 1.1000611773960731 + }, + { + "x": 4.819537392304322, + "y": 1.6733348767579859, + "heading": 0.30152261408094844, + "angularVelocity": 8.758577384965909e-7, + "velocityX": 3.61249481619895, + "velocityY": -1.1194047439368322, + "timestamp": 1.1611756872514105 + }, + { + "x": 5.0445846783405734, + "y": 1.620648273878739, + "heading": 0.30152267296096713, + "angularVelocity": 9.634376323324065e-7, + "velocityX": 3.682387154359157, + "velocityY": -0.8620964645541634, + "timestamp": 1.222290197106748 + }, + { + "x": 5.272784828780489, + "y": 1.5839488354188542, + "heading": 0.3015227398607896, + "angularVelocity": 0.0000010946634868711088, + "velocityX": 3.733976611774902, + "velocityY": -0.6005028682510115, + "timestamp": 1.2834047069620853 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.30152281374195866, + "angularVelocity": 0.0000012088973518937203, + "velocityX": 3.7670067063284516, + "velocityY": -0.33592433375155795, + "timestamp": 1.3445192168174227 + }, + { + "x": 5.642161233112151, + "y": 1.5569512201034212, + "heading": 0.3015228937182471, + "angularVelocity": 0.0000021712104790563405, + "velocityX": 3.7778762073788132, + "velocityY": -0.17558801391627427, + "timestamp": 1.381354101872902 + }, + { + "x": 5.781468008752501, + "y": 1.5564011001804379, + "heading": 0.3015229674749775, + "angularVelocity": 0.0000020023608117977538, + "velocityX": 3.7819250808175866, + "velocityY": -0.01493475335010743, + "timestamp": 1.4181889869283815 + }, + { + "x": 5.920672391044722, + "y": 1.5617696155329066, + "heading": 0.3015230362484881, + "angularVelocity": 0.00000186707547817187, + "velocityX": 3.7791452880212755, + "velocityY": 0.14574540803868186, + "timestamp": 1.4550238719838609 + }, + { + "x": 6.059523032201408, + "y": 1.573047069761596, + "heading": 0.3015231010202909, + "angularVelocity": 0.0000017584364021609749, + "velocityX": 3.769541860862453, + "velocityY": 0.3061623298594123, + "timestamp": 1.4918587570393402 + }, + { + "x": 6.197769223699856, + "y": 1.5902130966223555, + "heading": 0.3015231625861422, + "angularVelocity": 0.0000016714006640348176, + "velocityX": 3.753132154212656, + "velocityY": 0.46602634526766634, + "timestamp": 1.5286936420948196 + }, + { + "x": 6.335161349065868, + "y": 1.6132366967697473, + "heading": 0.30152322160724626, + "angularVelocity": 0.0000016023154129591626, + "velocityX": 3.729945815198736, + "velocityY": 0.6250487849416221, + "timestamp": 1.565528527150299 + }, + { + "x": 6.471451334685286, + "y": 1.642076293800604, + "heading": 0.30152327864702066, + "angularVelocity": 0.0000015485259219178396, + "velocityX": 3.700024729658922, + "velocityY": 0.7829425010399578, + "timestamp": 1.6023634122057784 + }, + { + "x": 6.606393097769013, + "y": 1.6766798096014064, + "heading": 0.30152333422462596, + "angularVelocity": 0.0000015088306981556567, + "velocityX": 3.6634229448655047, + "velocityY": 0.9394223912653392, + "timestamp": 1.6391982972612578 + }, + { + "x": 6.739742462814537, + "y": 1.7169846115821055, + "heading": 0.301525563266411, + "angularVelocity": 0.000060514422176811634, + "velocityX": 3.62019224017336, + "velocityY": 1.094201920814829, + "timestamp": 1.6760331823167371 + }, + { + "x": 6.86942078984101, + "y": 1.7614703146147381, + "heading": 0.31012052274281793, + "angularVelocity": 0.23333748601255083, + "velocityX": 3.520530248191535, + "velocityY": 1.2077057649461913, + "timestamp": 1.7128680673722165 + }, + { + "x": 6.994764102260858, + "y": 1.8086931054779125, + "heading": 0.330618059674586, + "angularVelocity": 0.5564707722284291, + "velocityX": 3.4028425019125397, + "velocityY": 1.2820127113753435, + "timestamp": 1.749702952427696 + }, + { + "x": 7.114597005035733, + "y": 1.8565129182538003, + "heading": 0.35962770978990066, + "angularVelocity": 0.7875591323719712, + "velocityX": 3.2532449224257225, + "velocityY": 1.2982207682707054, + "timestamp": 1.7865378374831753 + }, + { + "x": 7.228563598521064, + "y": 1.9039637598434371, + "heading": 0.3914629491729737, + "angularVelocity": 0.8642687315332709, + "velocityX": 3.093985316193586, + "velocityY": 1.2882038729907241, + "timestamp": 1.8233727225386547 + }, + { + "x": 7.3366766140353254, + "y": 1.9505339209655184, + "heading": 0.42341393018553436, + "angularVelocity": 0.8674109058420351, + "velocityX": 2.935071342055907, + "velocityY": 1.2642950032812363, + "timestamp": 1.860207607594134 + }, + { + "x": 7.4389964644321305, + "y": 1.9959395099807475, + "heading": 0.45395278398085526, + "angularVelocity": 0.8290742254068196, + "velocityX": 2.777797466795224, + "velocityY": 1.232678992939458, + "timestamp": 1.8970424926496134 + }, + { + "x": 7.535582073900071, + "y": 2.0400031621127974, + "heading": 0.48208291398817593, + "angularVelocity": 0.7636817643098928, + "velocityX": 2.6221232758692414, + "velocityY": 1.1962478521565303, + "timestamp": 1.9338773777050928 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6791276901362222, + "velocityX": 2.467819383699554, + "velocityY": 1.1565397222936757, + "timestamp": 1.9707122627605722 + }, + { + "x": 7.766333854980875, + "y": 2.1513911603861824, + "heading": 0.5393931722207971, + "angularVelocity": 0.5089507410927826, + "velocityX": 2.2039777536941085, + "velocityY": 1.084054804314438, + "timestamp": 2.034165688054972 + }, + { + "x": 7.889716821195031, + "y": 2.214535023307618, + "heading": 0.562169089136497, + "angularVelocity": 0.35893912440547393, + "velocityX": 1.9444650252008764, + "velocityY": 0.9951214237603884, + "timestamp": 2.0976191133493716 + }, + { + "x": 7.996856912790919, + "y": 2.2713427786511677, + "heading": 0.5763089078520277, + "angularVelocity": 0.22283775304370615, + "velocityX": 1.6884839722804246, + "velocityY": 0.8952669628153768, + "timestamp": 2.1610725386437712 + }, + { + "x": 8.087934078342826, + "y": 2.32132355324964, + "heading": 0.5824533262443143, + "angularVelocity": 0.09683351787202944, + "velocityX": 1.4353388352062089, + "velocityY": 0.7876765417561041, + "timestamp": 2.224525963938171 + }, + { + "x": 8.163093938645574, + "y": 2.3641124623938485, + "heading": 0.5810872040851076, + "angularVelocity": -0.021529525835184894, + "velocityX": 1.184488622230158, + "velocityY": 0.6743356870915145, + "timestamp": 2.2879793892325706 + }, + { + "x": 8.222455939868006, + "y": 2.399428067413051, + "heading": 0.5725898823037127, + "angularVelocity": -0.13391431182746497, + "velocityX": 0.9355208319647886, + "velocityY": 0.5565594742183169, + "timestamp": 2.3514328145269703 + }, + { + "x": 8.2661194627332, + "y": 2.427046891477821, + "heading": 0.5572661390575091, + "angularVelocity": -0.24149591885871216, + "velocityX": 0.6881192412011335, + "velocityY": 0.4352613580217183, + "timestamp": 2.41488623982137 + }, + { + "x": 8.294168308225482, + "y": 2.446787281552408, + "heading": 0.535366098523775, + "angularVelocity": -0.3451356712758413, + "velocityX": 0.44203831963592227, + "velocityY": 0.31110046436420713, + "timestamp": 2.4783396651157696 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.4454857086167899, + "velocityX": 0.19708463834018525, + "velocityY": 0.18456741693022066, + "timestamp": 2.5417930904101693 + }, + { + "x": 8.303024530421867, + "y": 2.461881127507041, + "heading": 0.4711910316636515, + "angularVelocity": -0.5465136629689635, + "velocityX": -0.05554517774464489, + "velocityY": 0.05148047936095934, + "timestamp": 2.607495880956368 + }, + { + "x": 8.282750419128863, + "y": 2.4565585196128312, + "heading": 0.42887568027171213, + "angularVelocity": -0.644041920292346, + "velocityX": -0.3085730624903528, + "velocityY": -0.0810103779453237, + "timestamp": 2.673198671502566 + }, + { + "x": 8.245821395574854, + "y": 2.44257692607346, + "heading": 0.38041954518186105, + "angularVelocity": -0.7375049779016557, + "velocityX": -0.5620617213821911, + "velocityY": -0.2128006044056815, + "timestamp": 2.7389014620487644 + }, + { + "x": 8.19220201373393, + "y": 2.41999124764574, + "heading": 0.3261364097301619, + "angularVelocity": -0.8261922362875965, + "velocityX": -0.8160898706916083, + "velocityY": -0.3437552383994849, + "timestamp": 2.8046042525949626 + }, + { + "x": 8.12185021393602, + "y": 2.3888680897109964, + "heading": 0.26640039291910717, + "angularVelocity": -0.9091853833674209, + "velocityX": -1.070758170437869, + "velocityY": -0.4736961349131665, + "timestamp": 2.870307043141161 + }, + { + "x": 8.034715295170983, + "y": 2.349289919572908, + "heading": 0.2016664425434797, + "angularVelocity": -0.9852542005823995, + "velocityX": -1.3261981422808646, + "velocityY": -0.6023818746368115, + "timestamp": 2.936009833687359 + }, + { + "x": 7.93073498478249, + "y": 2.3013613956679024, + "heading": 0.13250258081588168, + "angularVelocity": -1.0526776892218304, + "velocityX": -1.582585907296906, + "velocityY": -0.7294747073384187, + "timestamp": 3.0017126242335572 + }, + { + "x": 7.809831081003324, + "y": 2.2452194823487295, + "heading": 0.0596429891895938, + "angularVelocity": -1.1089268967207304, + "velocityX": -1.8401639074089793, + "velocityY": -0.8544829352369329, + "timestamp": 3.0674154147797554 + }, + { + "x": 7.671902779277316, + "y": 2.181050669281034, + "heading": -0.015920266023457082, + "angularVelocity": -1.1500768016834724, + "velocityX": -2.099276158278614, + "velocityY": -0.9766527804108355, + "timestamp": 3.1331182053259536 + }, + { + "x": 7.516816184413173, + "y": 2.1091228610813926, + "heading": -0.09276871504476969, + "angularVelocity": -1.1696375204532188, + "velocityX": -2.360426301149195, + "velocityY": -1.09474510293541, + "timestamp": 3.198820995872152 + }, + { + "x": 7.34438786737911, + "y": 2.029851654552736, + "heading": -0.16871930466642973, + "angularVelocity": -1.1559720521802803, + "velocityX": -2.6243682437326825, + "velocityY": -1.2065120198040524, + "timestamp": 3.26452378641835 + }, + { + "x": 7.154363406661781, + "y": 1.9439629810404226, + "heading": -0.24004853733783288, + "angularVelocity": -1.0856347512553308, + "velocityX": -2.892182495410387, + "velocityY": -1.307230222617737, + "timestamp": 3.3302265769645483 + }, + { + "x": 6.946444181928406, + "y": 1.853016234879977, + "heading": -0.2992772561370167, + "angularVelocity": -0.9014642803875683, + "velocityX": -3.1645417645872604, + "velocityY": -1.3842143599136347, + "timestamp": 3.3959293675107465 + }, + { + "x": 6.722123267905806, + "y": 1.762675189068849, + "heading": -0.3185532311699482, + "angularVelocity": -0.29338137501751665, + "velocityX": -3.4141763562518888, + "velocityY": -1.3749955680741657, + "timestamp": 3.4616321580569447 + }, + { + "x": 6.485759743484643, + "y": 1.6860130566353695, + "heading": -0.3185533048985424, + "angularVelocity": -0.0000011221531618495692, + "velocityX": -3.597465533141494, + "velocityY": -1.166801771982201, + "timestamp": 3.527334948603143 + }, + { + "x": 6.244268768249415, + "y": 1.6274732084557726, + "heading": -0.31855333054380797, + "angularVelocity": -3.903223189148686e-7, + "velocityX": -3.675505609848113, + "velocityY": -0.890979632568803, + "timestamp": 3.593037739149341 + }, + { + "x": 5.999037658090566, + "y": 1.5873919626414805, + "heading": -0.31855335901995885, + "angularVelocity": -4.334085455089385e-7, + "velocityX": -3.732430664210775, + "velocityY": -0.6100387134410885, + "timestamp": 3.6587405296955393 + }, + { + "x": 5.751475205097474, + "y": 1.5659995874731991, + "heading": -0.31855339177165976, + "angularVelocity": -4.984826465003964e-7, + "velocityX": -3.7679138273285506, + "velocityY": -0.3255930987168572, + "timestamp": 3.7244433202417375 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.31855343077931203, + "angularVelocity": -5.936985618685712e-7, + "velocityX": -3.781751212884059, + "velocityY": -0.03927691713851364, + "timestamp": 3.7901461107879357 + }, + { + "x": 5.26265149115341, + "y": 1.5786031057335383, + "heading": -0.31855346389778566, + "angularVelocity": -5.200844258760406e-7, + "velocityX": -3.774430808061432, + "velocityY": 0.2384477350064305, + "timestamp": 3.853825144350498 + }, + { + "x": 5.0240625557725105, + "y": 1.6113905209820258, + "heading": -0.3185534914433618, + "angularVelocity": -4.3256900426079536e-7, + "velocityX": -3.7467424053553953, + "velocityY": 0.5148855661616695, + "timestamp": 3.91750417791306 + }, + { + "x": 4.788524292077237, + "y": 1.6616042903739598, + "heading": -0.31855351524845343, + "angularVelocity": -3.738293489638866e-7, + "velocityX": -3.698835401826056, + "velocityY": 0.7885447781270322, + "timestamp": 3.981183211475622 + }, + { + "x": 4.557307734562994, + "y": 1.7289734302363957, + "heading": -0.31855353649931417, + "angularVelocity": -3.337183294186801e-7, + "velocityX": -3.63096838282074, + "velocityY": 1.0579485286353907, + "timestamp": 4.044862245038184 + }, + { + "x": 4.331660587990519, + "y": 1.8131343686528576, + "heading": -0.31855355601779706, + "angularVelocity": -3.06513492201463e-7, + "velocityX": -3.543507712798239, + "velocityY": 1.3216428345096847, + "timestamp": 4.108541278600747 + }, + { + "x": 4.1128004797347275, + "y": 1.9136328855171, + "heading": -0.31855357441317517, + "angularVelocity": -2.888765267316508e-7, + "velocityX": -3.4369257196840386, + "velocityY": 1.5782041787036154, + "timestamp": 4.172220312163308 + }, + { + "x": 3.9019083136875623, + "y": 2.0299264516040245, + "heading": -0.3185535921713155, + "angularVelocity": -2.7886950177034126e-7, + "velocityX": -3.3117991000911333, + "velocityY": 1.826245776369568, + "timestamp": 4.235899345725871 + }, + { + "x": 3.700120312567306, + "y": 2.1613848174245227, + "heading": -0.31855360971358787, + "angularVelocity": -2.7547956295773415e-7, + "velocityX": -3.1688295162647098, + "velocityY": 2.064390089892713, + "timestamp": 4.299578379288432 + }, + { + "x": 3.5092854089981618, + "y": 2.2879634490751353, + "heading": -0.3579155948331453, + "angularVelocity": -0.6181310066662056, + "velocityX": -2.9968247458036084, + "velocityY": 1.987759935556414, + "timestamp": 4.363257412850995 + }, + { + "x": 3.3329097587868497, + "y": 2.4050971281852065, + "heading": -0.4129449306411103, + "angularVelocity": -0.8641672577191554, + "velocityX": -2.7697601603521713, + "velocityY": 1.8394387062264796, + "timestamp": 4.426936446413556 + }, + { + "x": 3.171287728623564, + "y": 2.5124799864208414, + "heading": -0.469920470896881, + "angularVelocity": -0.8947299773291117, + "velocityX": -2.5380729122482584, + "velocityY": 1.6863141952387881, + "timestamp": 4.490615479976119 + }, + { + "x": 3.0244088088448944, + "y": 2.6100923711609356, + "heading": -0.525279921939494, + "angularVelocity": -0.8693513067880482, + "velocityX": -2.3065507053332985, + "velocityY": 1.5328810642861672, + "timestamp": 4.5542945135386805 + }, + { + "x": 2.892255080791877, + "y": 2.6979336459530643, + "heading": -0.5773440276114954, + "angularVelocity": -0.817602007430762, + "velocityX": -2.075309888665035, + "velocityY": 1.379437938640018, + "timestamp": 4.6179735471012435 + }, + { + "x": 2.7748123827126636, + "y": 2.776006231552379, + "heading": -0.6251288383809169, + "angularVelocity": -0.7504010047902983, + "velocityX": -1.8442914646911481, + "velocityY": 1.226032827941903, + "timestamp": 4.681652580663805 + }, + { + "x": 2.6720699827794823, + "y": 2.8443129171584167, + "heading": -0.6679860389317067, + "angularVelocity": -0.6730190166702934, + "velocityX": -1.6134415707211014, + "velocityY": 1.0726715181525133, + "timestamp": 4.745331614226368 + }, + { + "x": 2.58401962501764, + "y": 2.9028562720875715, + "heading": -0.7054558020691595, + "angularVelocity": -0.5884160145213319, + "velocityX": -1.3827213265624843, + "velocityY": 0.9193505562806369, + "timestamp": 4.809010647788929 + }, + { + "x": 2.5106548115569836, + "y": 2.9516385308148143, + "heading": -0.7371951289783589, + "angularVelocity": -0.49842664270363046, + "velocityX": -1.152103123370717, + "velocityY": 0.7660646840583188, + "timestamp": 4.872689681351492 + }, + { + "x": 2.451970312872832, + "y": 2.9906615991432, + "heading": -0.762938657381878, + "angularVelocity": -0.4042700864520384, + "velocityX": -0.921567043358115, + "velocityY": 0.6128087401019892, + "timestamp": 4.936368714914053 + }, + { + "x": 2.4079618328973456, + "y": 3.019927088676532, + "heading": -0.7824753669485657, + "angularVelocity": -0.306799718426846, + "velocityX": -0.6910984277462318, + "velocityY": 0.45957810437841673, + "timestamp": 5.000047748476616 + }, + { + "x": 2.378625774942972, + "y": 3.0394363533387363, + "heading": -0.7956338335546038, + "angularVelocity": -0.20663734780319193, + "velocityX": -0.4606862936377973, + "velocityY": 0.30636873034571754, + "timestamp": 5.063726782039177 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.10425061521167653, + "velocityX": -0.23032229137361054, + "velocityY": 0.15317707188371738, + "timestamp": 5.12740581560174 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 1.7497535778176193e-25, + "velocityX": -1.4086622042761887e-23, + "velocityY": 7.921157765160188e-25, + "timestamp": 5.191084849164302 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.3.traj b/src/main/deploy/choreo/SourceLanePGHSprint.3.traj index 81315e26..6dfd5232 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.3.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.3.traj @@ -1,724 +1,788 @@ { - "samples": [ - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 1.2572572851920672e-28, - "velocityX": 2.1335040537014396e-30, - "velocityY": 1.8634721866519574e-29, - "timestamp": 0 - }, - { - "x": 2.384240228496088, - "y": 3.0394763007756747, - "heading": -0.7921549333865273, - "angularVelocity": 0.15389773911589558, - "velocityX": 0.30849818872977186, - "velocityY": -0.14776374894381028, - "timestamp": 0.06574156742770221 - }, - { - "x": 2.4248103079231944, - "y": 3.020044385403153, - "heading": -0.7722377725963315, - "angularVelocity": 0.3029614529969845, - "velocityX": 0.6171145747585456, - "velocityY": -0.2955803479114183, - "timestamp": 0.13148313485540442 - }, - { - "x": 2.485678594891513, - "y": 2.9908905714242224, - "heading": -0.7429157303412026, - "angularVelocity": 0.4460198228065572, - "velocityX": 0.9258721589693997, - "velocityY": -0.4434608896569423, - "timestamp": 0.19722470228310662 - }, - { - "x": 2.5668563689761195, - "y": 2.952009793968192, - "heading": -0.7046902372675247, - "angularVelocity": 0.581450892781281, - "velocityX": 1.2348013176576613, - "velocityY": -0.5914184735371368, - "timestamp": 0.26296626971080883 - }, - { - "x": 2.668357540158173, - "y": 2.903395952391573, - "heading": -0.6582160423239786, - "angularVelocity": 0.706922526522589, - "velocityX": 1.5439420621310505, - "velocityY": -0.7394688547710901, - "timestamp": 0.32870783713851104 - }, - { - "x": 2.790199449693154, - "y": 2.8450417291439147, - "heading": -0.604379415056612, - "angularVelocity": 0.8189130465526614, - "velocityX": 1.8533465857651596, - "velocityY": -0.8876305438234711, - "timestamp": 0.39444940456621325 - }, - { - "x": 2.9324037360004773, - "y": 2.7769385796688133, - "heading": -0.5444410242237507, - "angularVelocity": 0.9117274378767651, - "velocityX": 2.1630802530485744, - "velocityY": -1.0359222047754857, - "timestamp": 0.46019097199391545 - }, - { - "x": 3.094996415116772, - "y": 2.6990776165764148, - "heading": -0.4803328988113282, - "angularVelocity": 0.975153588829843, - "velocityX": 2.473209652250889, - "velocityY": -1.1843490524929314, - "timestamp": 0.5259325394216177 - }, - { - "x": 3.2780012401054357, - "y": 2.6114549065561117, - "heading": -0.4154109268291881, - "angularVelocity": 0.9875330711202905, - "velocityX": 2.783700361113535, - "velocityY": -1.3328357300982285, - "timestamp": 0.5916741068493199 - }, - { - "x": 3.48136962382921, - "y": 2.514109658078373, - "heading": -0.3571766392011983, - "angularVelocity": 0.8858061939583661, - "velocityX": 3.0934520073228393, - "velocityY": -1.4807260046665736, - "timestamp": 0.6574156742770221 - }, - { - "x": 3.702896130222067, - "y": 2.408427187890042, - "heading": -0.3455558213194451, - "angularVelocity": 0.17676514778162136, - "velocityX": 3.3696565971974572, - "velocityY": -1.6075441204615863, - "timestamp": 0.7231572417047243 - }, - { - "x": 3.927079400169571, - "y": 2.300911734169414, - "heading": -0.34555579773751244, - "angularVelocity": 3.5870657763866696e-7, - "velocityX": 3.4100688304100117, - "velocityY": -1.6354257728775006, - "timestamp": 0.7888988091324265 - }, - { - "x": 4.15126266204063, - "y": 2.1933962636083315, - "heading": -0.3455557741556376, - "angularVelocity": 3.587056984423895e-7, - "velocityX": 3.4100687075585983, - "velocityY": -1.6354260290389449, - "timestamp": 0.8546403765601287 - }, - { - "x": 4.375445923911402, - "y": 2.0858807930466483, - "heading": -0.3455557505737627, - "angularVelocity": 3.5870569914452407e-7, - "velocityX": 3.4100687075542147, - "velocityY": -1.6354260290480847, - "timestamp": 0.9203819439878309 - }, - { - "x": 4.599629185782173, - "y": 1.978365322484965, - "heading": -0.34555572699188775, - "angularVelocity": 3.5870569932883336e-7, - "velocityX": 3.4100687075542147, - "velocityY": -1.6354260290480853, - "timestamp": 0.9861235114155331 - }, - { - "x": 4.823812447652946, - "y": 1.8708498519232832, - "heading": -0.3455557034100129, - "angularVelocity": 3.5870569853758494e-7, - "velocityX": 3.4100687075542258, - "velocityY": -1.6354260290480622, - "timestamp": 1.0518650788432353 - }, - { - "x": 5.047995709543858, - "y": 1.7633343814035949, - "heading": -0.34555567982813806, - "angularVelocity": 3.5870569820699746e-7, - "velocityX": 3.410068707860571, - "velocityY": -1.6354260284092945, - "timestamp": 1.1176066462709375 - }, - { - "x": 5.27217953587615, - "y": 1.65582008782303, - "heading": -0.34555565624625534, - "angularVelocity": 3.587058178727361e-7, - "velocityX": 3.410077293621499, - "velocityY": -1.6354081258984543, - "timestamp": 1.1833482136986397 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, - "angularVelocity": 3.617497277100986e-7, - "velocityX": 3.511082415812739, - "velocityY": -1.405520236667608, - "timestamp": 1.249089781126342 - }, - { - "x": 5.6599765905642645, - "y": 1.5113593672660246, - "heading": -0.34555563704903824, - "angularVelocity": -1.0484559207434358e-7, - "velocityX": 3.589689630249378, - "velocityY": -1.1905096787253004, - "timestamp": 1.2928186294317703 - }, - { - "x": 5.819811381479902, - "y": 1.4688909784273936, - "heading": -0.3455556415836398, - "angularVelocity": -1.0369817036745698e-7, - "velocityX": 3.6551337871799947, - "velocityY": -0.9711755622285476, - "timestamp": 1.3365474777371986 - }, - { - "x": 5.98037051193366, - "y": 1.4292489543347602, - "heading": -0.34555564611341977, - "angularVelocity": -1.0358790889899858e-7, - "velocityX": 3.6716981277969567, - "velocityY": -0.9065416910994355, - "timestamp": 1.380276326042627 - }, - { - "x": 6.140929731813359, - "y": 1.389607292439096, - "heading": -0.3455556506431997, - "angularVelocity": -1.0358790900922708e-7, - "velocityX": 3.6717001728071876, - "velocityY": -0.9065334083071072, - "timestamp": 1.4240051743480553 - }, - { - "x": 6.30148895170346, - "y": 1.3499656305855623, - "heading": -0.34555565517297965, - "angularVelocity": -1.0358790856487398e-7, - "velocityX": 3.6717001730450596, - "velocityY": -0.9065334073436624, - "timestamp": 1.4677340226534836 - }, - { - "x": 6.462048171593562, - "y": 1.3103239687320334, - "heading": -0.34555565970275964, - "angularVelocity": -1.0358791005302326e-7, - "velocityX": 3.671700173045087, - "velocityY": -0.9065334073435504, - "timestamp": 1.511462870958912 - }, - { - "x": 6.622607391483664, - "y": 1.2706823068785016, - "heading": -0.34555566423253964, - "angularVelocity": -1.0358790893463982e-7, - "velocityX": 3.6717001730450707, - "velocityY": -0.9065334073436169, - "timestamp": 1.5551917192643403 - }, - { - "x": 6.783166611367605, - "y": 1.2310406450000155, - "heading": -0.3455556687623196, - "angularVelocity": -1.0358790885289903e-7, - "velocityX": 3.6717001729041776, - "velocityY": -0.9065334079142718, - "timestamp": 1.5989205675697686 - }, - { - "x": 6.943725778284007, - "y": 1.1913987685896004, - "heading": -0.34555567329209946, - "angularVelocity": -1.0358790829702175e-7, - "velocityX": 3.6716989616319724, - "velocityY": -0.9065383138730976, - "timestamp": 1.642649415875197 - }, - { - "x": 7.103836002524736, - "y": 1.1499808922348331, - "heading": -0.34555567791225333, - "angularVelocity": -1.0565459632350647e-7, - "velocityX": 3.661432451237353, - "velocityY": -0.9471522338178334, - "timestamp": 1.6863782641806253 - }, - { - "x": 7.257955684496521, - "y": 1.1005509138662795, - "heading": -0.346747423550466, - "angularVelocity": -0.02725307627333064, - "velocityX": 3.524439539210384, - "velocityY": -1.1303745761449258, - "timestamp": 1.7301071124860536 - }, - { - "x": 7.402334567868656, - "y": 1.0534126604700325, - "heading": -0.34675046354447403, - "angularVelocity": -0.00006951918758018307, - "velocityX": 3.3016850195483918, - "velocityY": -1.07796695369166, - "timestamp": 1.773835960791482 - }, - { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, - "angularVelocity": 0.027323634774631837, - "velocityX": 3.079221948168643, - "velocityY": -1.024340556060407, - "timestamp": 1.8175648090969103 - }, - { - "x": 7.709533523799992, - "y": 0.9494390432841444, - "heading": -0.3413965784412158, - "angularVelocity": 0.06656216442856697, - "velocityX": 2.7614877570011993, - "velocityY": -0.9471323179117597, - "timestamp": 1.880048564827372 - }, - { - "x": 7.862380855125707, - "y": 0.8957055369328039, - "heading": -0.33682515317576994, - "angularVelocity": 0.07316181961221624, - "velocityX": 2.4461930871290236, - "velocityY": -0.8599596122731851, - "timestamp": 1.9425323205578335 - }, - { - "x": 7.99561550345175, - "y": 0.8477270543434197, - "heading": -0.33285887044966567, - "angularVelocity": 0.06347702182330028, - "velocityX": 2.1323085779411386, - "velocityY": -0.7678552934037809, - "timestamp": 2.005016076288295 - }, - { - "x": 8.109294083490143, - "y": 0.8056872786590918, - "heading": -0.33010810532148027, - "angularVelocity": 0.04402368417243478, - "velocityX": 1.8193301396409471, - "velocityY": -0.6728112801937814, - "timestamp": 2.0674998320187568 - }, - { - "x": 8.20345588526635, - "y": 0.7697081195865146, - "heading": -0.32897987990878186, - "angularVelocity": 0.018056299585531694, - "velocityX": 1.5069805051795295, - "velocityY": -0.5758162045793389, - "timestamp": 2.1299835877492184 - }, - { - "x": 8.278129725528439, - "y": 0.7398763726381793, - "heading": -0.32976503867911666, - "angularVelocity": -0.012565806282865027, - "velocityX": 1.1950920585537632, - "velocityY": -0.4774320397291953, - "timestamp": 2.19246734347968 - }, - { - "x": 8.333337625588607, - "y": 0.7162569714576368, - "heading": -0.33268181216540116, - "angularVelocity": -0.04668050843272986, - "velocityX": 0.883556044523325, - "velocityY": -0.37800866648333437, - "timestamp": 2.2549510992101416 - }, - { - "x": 8.369096953381794, - "y": 0.698900317224484, - "heading": -0.33790002427933163, - "angularVelocity": -0.08351309957167831, - "velocityX": 0.572297989695798, - "velocityY": -0.2777786647144699, - "timestamp": 2.317434854940603 - }, - { - "x": 8.385421752929688, - "y": 0.6878466606140137, - "heading": -0.34555563246426124, - "angularVelocity": -0.12252157533477633, - "velocityX": 0.26126469763300175, - "velocityY": -0.1769044847136382, - "timestamp": 2.379918610671065 - }, - { - "x": 8.379560991493735, - "y": 0.6833892008568203, - "heading": -0.35721868503670856, - "angularVelocity": -0.1678763417451177, - "velocityX": -0.08435897751441682, - "velocityY": -0.06416005011256522, - "timestamp": 2.449392679805565 - }, - { - "x": 8.349687812290387, - "y": 0.6867654591427064, - "heading": -0.37193021085772177, - "angularVelocity": -0.2117556378126066, - "velocityX": -0.42999034856465757, - "velocityY": 0.048597387887987537, - "timestamp": 2.5188667489400656 - }, - { - "x": 8.295801597591089, - "y": 0.6979765367761911, - "heading": -0.38956490915625874, - "angularVelocity": -0.253831372168465, - "velocityX": -0.7756306111129838, - "velocityY": 0.1613706779112114, - "timestamp": 2.588340818074566 - }, - { - "x": 8.217901630671626, - "y": 0.7170238074049998, - "heading": -0.40996601054380316, - "angularVelocity": -0.2936505899495876, - "velocityX": -1.121281190089058, - "velocityY": 0.2741637400269978, - "timestamp": 2.6578148872090663 - }, - { - "x": 8.115987079646073, - "y": 0.7439090305870278, - "heading": -0.4329315185089462, - "angularVelocity": -0.3305622982969707, - "velocityX": -1.4669437431144206, - "velocityY": 0.3869821289721611, - "timestamp": 2.7272889563435667 - }, - { - "x": 7.990056990056933, - "y": 0.778634533730576, - "heading": -0.45819105339643357, - "angularVelocity": -0.36358219983610734, - "velocityX": -1.812620034466978, - "velocityY": 0.4998340183057432, - "timestamp": 2.796763025478067 - }, - { - "x": 7.840110319883288, - "y": 0.8212035218059903, - "heading": -0.4853637226253089, - "angularVelocity": -0.3911195870256229, - "velocityX": -2.1583113245224226, - "velocityY": 0.6127320395326444, - "timestamp": 2.8662370946125675 - }, - { - "x": 7.6661461501938755, - "y": 0.8716206459377261, - "heading": -0.5138727598830971, - "angularVelocity": -0.4103550808661484, - "velocityX": -2.504015841545443, - "velocityY": 0.7256970083921437, - "timestamp": 2.935711163747068 - }, - { - "x": 7.468164698581641, - "y": 0.9298931437973682, - "heading": -0.5427426487462225, - "angularVelocity": -0.41554912822558204, - "velocityX": -2.8497172265661783, - "velocityY": 0.8387661552805812, - "timestamp": 3.0051852328815682 - }, - { - "x": 7.24617329762414, - "y": 0.9960332266921984, - "heading": -0.5699703376985038, - "angularVelocity": -0.391911533201964, - "velocityX": -3.1953130674947268, - "velocityY": 0.9520110700121011, - "timestamp": 3.0746593020160686 - }, - { - "x": 7.00025853984304, - "y": 1.0700564595014088, - "heading": -0.589089272180988, - "angularVelocity": -0.2751952594783323, - "velocityX": -3.53966250782022, - "velocityY": 1.065480023430083, - "timestamp": 3.144133371150569 - }, - { - "x": 6.750442302486931, - "y": 1.1514706650489683, - "heading": -0.5890892788822688, - "angularVelocity": -9.645729514768673e-8, - "velocityX": -3.5958198572257243, - "velocityY": 1.171864647656428, - "timestamp": 3.2136074402850694 - }, - { - "x": 6.500626129709104, - "y": 1.232885068752249, - "heading": -0.5890892855834843, - "angularVelocity": -9.645635490096592e-8, - "velocityX": -3.5958189276949932, - "velocityY": 1.1718674998820673, - "timestamp": 3.2830815094195698 - }, - { - "x": 6.250809956933241, - "y": 1.3142994724615544, - "heading": -0.5890892922846998, - "angularVelocity": -9.645635424551899e-8, - "velocityX": -3.595818927666732, - "velocityY": 1.1718674999687875, - "timestamp": 3.35255557855407 - }, - { - "x": 6.000993784158921, - "y": 1.395713876175591, - "heading": -0.589089298985915, - "angularVelocity": -9.645635348778811e-8, - "velocityX": -3.595818927644538, - "velocityY": 1.1718675000368866, - "timestamp": 3.4220296476885705 - }, - { - "x": 5.751177662095486, - "y": 1.477128435493372, - "heading": -0.5890893056871302, - "angularVelocity": -9.64563521650664e-8, - "velocityX": -3.5958181977191597, - "velocityY": 1.1718697397753421, - "timestamp": 3.491503716823071 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.5890893123960679, - "angularVelocity": -9.65675052838285e-8, - "velocityX": -3.5721826564600034, - "velocityY": 1.242054049730676, - "timestamp": 3.5609777859575713 - }, - { - "x": 5.287967104202961, - "y": 1.6596756394253018, - "heading": -0.5890893181956361, - "angularVelocity": -9.309834476919773e-8, - "velocityX": -3.4519020760085812, - "velocityY": 1.5451728334255468, - "timestamp": 3.6232728593175922 - }, - { - "x": 5.074932733784246, - "y": 1.7602857483579184, - "heading": -0.5890893239791856, - "angularVelocity": -9.284120212486628e-8, - "velocityX": -3.419762734486735, - "velocityY": 1.6150572349624415, - "timestamp": 3.685567932677613 - }, - { - "x": 4.861898448797989, - "y": 1.860896038187167, - "heading": -0.5890893297627348, - "angularVelocity": -9.284119839047264e-8, - "velocityX": -3.4197613630707817, - "velocityY": 1.615060138829829, - "timestamp": 3.747863006037634 - }, - { - "x": 4.648864163815087, - "y": 1.9615063280235205, - "heading": -0.5890893355462841, - "angularVelocity": -9.28411976539737e-8, - "velocityX": -3.419761363016917, - "velocityY": 1.6150601389438841, - "timestamp": 3.810158079397655 - }, - { - "x": 4.435829878832186, - "y": 2.0621166178598744, - "heading": -0.5890893413298333, - "angularVelocity": -9.28411982771023e-8, - "velocityX": -3.419761363016914, - "velocityY": 1.6150601389438883, - "timestamp": 3.872453152757676 - }, - { - "x": 4.222795593849285, - "y": 2.162726907696228, - "heading": -0.5890893471133825, - "angularVelocity": -9.284119843074624e-8, - "velocityX": -3.4197613630169146, - "velocityY": 1.6150601389438883, - "timestamp": 3.934748226117697 - }, - { - "x": 4.009761308866383, - "y": 2.2633371975325822, - "heading": -0.5890893528969318, - "angularVelocity": -9.284119832193768e-8, - "velocityX": -3.419761363016914, - "velocityY": 1.6150601389438886, - "timestamp": 3.997043299477718 - }, - { - "x": 3.7967270238835944, - "y": 2.363947487369175, - "heading": -0.589089358680481, - "angularVelocity": -9.284119741513914e-8, - "velocityX": -3.4197613630151045, - "velocityY": 1.615060138947721, - "timestamp": 4.059338372837739 - }, - { - "x": 3.583692741771847, - "y": 2.464557783284562, - "heading": -0.5890893644643718, - "angularVelocity": -9.284668184194445e-8, - "velocityX": -3.419761316927328, - "velocityY": 1.615060236528384, - "timestamp": 4.12163344619776 - }, - { - "x": 3.381763406617466, - "y": 2.559912098840239, - "heading": -0.6245815423960026, - "angularVelocity": -0.5697429349912143, - "velocityX": -3.241497670086629, - "velocityY": 1.530687908570203, - "timestamp": 4.183928519557781 - }, - { - "x": 3.1981911985495857, - "y": 2.646597580557341, - "heading": -0.6568615726262876, - "angularVelocity": -0.5181795042399261, - "velocityX": -2.9468174314037126, - "velocityY": 1.3915302935133123, - "timestamp": 4.246223592917802 - }, - { - "x": 3.032976151851517, - "y": 2.7246143065734305, - "heading": -0.6859252496642307, - "angularVelocity": -0.46654856428174046, - "velocityX": -2.652136642383354, - "velocityY": 1.2523739327700791, - "timestamp": 4.3085186662778225 - }, - { - "x": 2.8861182875087947, - "y": 2.793962348685928, - "heading": -0.7117686923384959, - "angularVelocity": -0.414855321301394, - "velocityX": -2.3574555164898965, - "velocityY": 1.1132187245645664, - "timestamp": 4.3708137396378435 - }, - { - "x": 2.7576176211174075, - "y": 2.854641769350887, - "heading": -0.7343883425051251, - "angularVelocity": -0.3631049607390947, - "velocityX": -2.0627741402397475, - "velocityY": 0.9740645189431933, - "timestamp": 4.433108812997864 - }, - { - "x": 2.6474741650844584, - "y": 2.906652621199817, - "heading": -0.7537810666899133, - "angularVelocity": -0.31130429966287193, - "velocityX": -1.7680925648228973, - "velocityY": 0.8349111581961731, - "timestamp": 4.495403886357885 - }, - { - "x": 2.5556879298075064, - "y": 2.9499949470036255, - "heading": -0.7699442582183806, - "angularVelocity": -0.2594617945957906, - "velocityX": -1.473410822497863, - "velocityY": 0.6957584840348696, - "timestamp": 4.557698959717906 - }, - { - "x": 2.482258924484474, - "y": 2.9846687797700127, - "heading": -0.7828759248842103, - "angularVelocity": -0.20758730937026382, - "velocityX": -1.1787289325219326, - "velocityY": 0.556606339734085, - "timestamp": 4.619994033077927 - }, - { - "x": 2.4271871577101587, - "y": 3.010674142880251, - "heading": -0.7925747589157716, - "angularVelocity": -0.15569183096565328, - "velocityX": -0.8840469045768797, - "velocityY": 0.4174545707643066, - "timestamp": 4.682289106437948 - }, - { - "x": 2.3904726379075623, - "y": 3.0280110502230517, - "heading": -0.7990401886427835, - "angularVelocity": -0.10378717574737188, - "velocityX": -0.5893647414204509, - "velocityY": 0.27830302474492646, - "timestamp": 4.744584179797969 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -0.05188569757551244, - "velocityX": -0.29468244125856385, - "velocityY": 0.13915155101802326, - "timestamp": 4.80687925315799 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -7.343237406242101e-30, - "velocityX": 6.07048254686404e-33, - "velocityY": -1.403576741314563e-31, - "timestamp": 4.869174326518011 - } - ] + "samples": [ + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 1.7497535778176193e-25, + "velocityX": -1.4086622042761887e-23, + "velocityY": 7.921157765160188e-25, + "timestamp": 0 + }, + { + "x": 2.3794831363952245, + "y": 3.0417249906399815, + "heading": -0.7962438376212186, + "angularVelocity": 0.09576161859408154, + "velocityX": 0.24659384653798558, + "velocityY": -0.11858712382831015, + "timestamp": 0.06295397307267159 + }, + { + "x": 2.41053316098515, + "y": 3.026792718508752, + "heading": -0.7842933803747459, + "angularVelocity": 0.1898284836237042, + "velocityX": 0.4932178713181911, + "velocityY": -0.23719348283216615, + "timestamp": 0.12590794614534317 + }, + { + "x": 2.4571113467410304, + "y": 3.0043922677761175, + "heading": -0.766548054431847, + "angularVelocity": 0.2818777763623355, + "velocityX": 0.7398768255994334, + "velocityY": -0.3558226691550744, + "timestamp": 0.18886191921801476 + }, + { + "x": 2.5192202681818534, + "y": 2.974521939997295, + "heading": -0.7431604845322959, + "angularVelocity": 0.3715026829609895, + "velocityX": 0.9865766751389508, + "velocityY": -0.4744788346295768, + "timestamp": 0.25181589229068635 + }, + { + "x": 2.5968629760285724, + "y": 2.937179727170992, + "heading": -0.7143164433853058, + "angularVelocity": 0.4581766604896231, + "velocityX": 1.2333249842244594, + "velocityY": -0.593166896443484, + "timestamp": 0.31476986536335794 + }, + { + "x": 2.6900431315277755, + "y": 2.8923632450565475, + "heading": -0.6802459924925017, + "angularVelocity": 0.5411961982681314, + "velocityX": 1.4801314508242465, + "velocityY": -0.7118928310165661, + "timestamp": 0.3777238384360295 + }, + { + "x": 2.7987651872562824, + "y": 2.8400696406927834, + "heading": -0.6412406418470571, + "angularVelocity": 0.6195852102998923, + "velocityX": 1.72700864491909, + "velocityY": -0.8306640837965626, + "timestamp": 0.4406778115087011 + }, + { + "x": 2.9230346291738636, + "y": 2.780295465724823, + "heading": -0.597681173837793, + "angularVelocity": 0.6919256384181025, + "velocityX": 1.9739729814054823, + "velocityY": -0.9494901123867101, + "timestamp": 0.5036317845813727 + }, + { + "x": 3.0628582843459875, + "y": 2.713036511288724, + "heading": -0.5500859220162175, + "angularVelocity": 0.7560325345412855, + "velocityX": 2.2210457632390876, + "velocityY": -1.0683829971852359, + "timestamp": 0.5665857576540443 + }, + { + "x": 3.2182446187737406, + "y": 2.638287632787952, + "heading": -0.49920268501328596, + "angularVelocity": 0.8082609328595405, + "velocityX": 2.468253024290973, + "velocityY": -1.1873576019496896, + "timestamp": 0.6295397307267159 + }, + { + "x": 3.3892034703211182, + "y": 2.556042786579892, + "heading": -0.4462086227940175, + "angularVelocity": 0.8417905913276443, + "velocityX": 2.7156165560834484, + "velocityY": -1.3064282076862779, + "timestamp": 0.6924937037993875 + }, + { + "x": 3.5757413262062396, + "y": 2.4662968513988717, + "heading": -0.3932458767459955, + "angularVelocity": 0.8412931458175688, + "velocityX": 2.9630831348768174, + "velocityY": -1.4255801627868938, + "timestamp": 0.755447676872059 + }, + { + "x": 3.777810773280146, + "y": 2.3690669294508315, + "heading": -0.3455559373140241, + "angularVelocity": 0.7575366113417542, + "velocityX": 3.2097965737705954, + "velocityY": -1.5444604558286192, + "timestamp": 0.8184016499447306 + }, + { + "x": 3.9925130219343306, + "y": 2.2661623700337894, + "heading": -0.34555574915683335, + "angularVelocity": 0.000002988805655755853, + "velocityX": 3.410463838499, + "velocityY": -1.6345999210924134, + "timestamp": 0.8813556230174022 + }, + { + "x": 4.207067930960627, + "y": 2.1629508436660116, + "heading": -0.34555573251477295, + "angularVelocity": 2.6435282147725544e-7, + "velocityX": 3.408123404358042, + "velocityY": -1.6394759747511882, + "timestamp": 0.9443095960900738 + }, + { + "x": 4.421622830783708, + "y": 2.0597392981666993, + "heading": -0.34555571587271255, + "angularVelocity": 2.643528217260083e-7, + "velocityX": 3.408123258168449, + "velocityY": -1.6394762786483632, + "timestamp": 1.0072635691627454 + }, + { + "x": 4.6361777306062155, + "y": 1.956527752666195, + "heading": -0.34555569923065216, + "angularVelocity": 2.643528215615007e-7, + "velocityX": 3.408123258159341, + "velocityY": -1.6394762786672963, + "timestamp": 1.070217542235417 + }, + { + "x": 4.850732630432354, + "y": 1.85331620717324, + "heading": -0.3455556825885917, + "angularVelocity": 2.643528216339363e-7, + "velocityX": 3.4081232582170267, + "velocityY": -1.6394762785473795, + "timestamp": 1.1331715153080886 + }, + { + "x": 5.065287588526139, + "y": 1.750104782806442, + "heading": -0.34555566594653137, + "angularVelocity": 2.6435282065070603e-7, + "velocityX": 3.408124183776485, + "velocityY": -1.6394743545042916, + "timestamp": 1.1961254883807602 + }, + { + "x": 5.280754782593808, + "y": 1.6488115627171362, + "heading": -0.34555564929394994, + "angularVelocity": 2.645199441921782e-7, + "velocityX": 3.4226147064450507, + "velocityY": -1.6090044066381253, + "timestamp": 1.2590794614534317 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 2.6733322523330606e-7, + "velocityX": 3.530338179119512, + "velocityY": -1.3564287389044698, + "timestamp": 1.3220334345261033 + }, + { + "x": 5.652794667941403, + "y": 1.5139935171410897, + "heading": -0.3455556356136128, + "angularVelocity": -7.55109994192204e-8, + "velocityX": 3.591492784420599, + "velocityY": -1.1850586838463792, + "timestamp": 1.3637406241748353 + }, + { + "x": 5.804789449400159, + "y": 1.47182921760144, + "heading": -0.34555563872486544, + "angularVelocity": -7.459751255727934e-8, + "velocityX": 3.644330455705477, + "velocityY": -1.0109599782379923, + "timestamp": 1.4054478138235664 + }, + { + "x": 5.9583966935302906, + "y": 1.435981535401865, + "heading": -0.3455556418269747, + "angularVelocity": -7.43782867600124e-8, + "velocityX": 3.682991959512248, + "velocityY": -0.8595084564913873, + "timestamp": 1.4471550034722975 + }, + { + "x": 6.112005098303726, + "y": 1.4001388269201716, + "heading": -0.34555564492908214, + "angularVelocity": -7.437824177042706e-8, + "velocityX": 3.683019787887031, + "velocityY": -0.8593892032421666, + "timestamp": 1.4888621931210286 + }, + { + "x": 6.265613503315511, + "y": 1.3642961194599532, + "heading": -0.34555564803118954, + "angularVelocity": -7.437824172842392e-8, + "velocityX": 3.683019793601852, + "velocityY": -0.8593891787505857, + "timestamp": 1.5305693827697597 + }, + { + "x": 6.419221908327331, + "y": 1.3284534119998854, + "heading": -0.345555651133297, + "angularVelocity": -7.437824199449142e-8, + "velocityX": 3.683019793602695, + "velocityY": -0.8593891787469733, + "timestamp": 1.5722765724184908 + }, + { + "x": 6.572830313271966, + "y": 1.2926107042518886, + "heading": -0.34555565423540446, + "angularVelocity": -7.437824209379189e-8, + "velocityX": 3.6830197919918253, + "velocityY": -0.8593891856505578, + "timestamp": 1.613983762067222 + }, + { + "x": 6.726438391075464, + "y": 1.2567665945285837, + "heading": -0.3455556573375121, + "angularVelocity": -7.437824552269796e-8, + "velocityX": 3.6830119482329913, + "velocityY": -0.8594228003659058, + "timestamp": 1.655690951715953 + }, + { + "x": 6.878955915134789, + "y": 1.2165341904124123, + "heading": -0.3455556607967783, + "angularVelocity": -8.29417253790897e-8, + "velocityX": 3.6568640885148347, + "velocityY": -0.9646395370922597, + "timestamp": 1.6973981413646841 + }, + { + "x": 7.025398158916378, + "y": 1.1717644348314602, + "heading": -0.3467458392299039, + "angularVelocity": -0.028536529148800397, + "velocityX": 3.5111990286317973, + "velocityY": -1.0734301677483917, + "timestamp": 1.7391053310134152 + }, + { + "x": 7.164406767305733, + "y": 1.1285160001941774, + "heading": -0.3473601505437228, + "angularVelocity": -0.014729146676936172, + "velocityX": 3.3329651208849276, + "velocityY": -1.0369539401127719, + "timestamp": 1.7808125206621463 + }, + { + "x": 7.296003097467406, + "y": 1.0868892243180075, + "heading": -0.34737182264197447, + "angularVelocity": -0.0002798581815252442, + "velocityX": 3.1552432870689695, + "velocityY": -0.9980719445918297, + "timestamp": 1.8225197103108774 + }, + { + "x": 7.4201946902403915, + "y": 1.0469180189360368, + "heading": -0.3467718237478745, + "angularVelocity": 0.014385982348686153, + "velocityX": 2.9777022575473837, + "velocityY": -0.958376858249577, + "timestamp": 1.8642268999596086 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.02916023097831232, + "velocityX": 2.8002535793496506, + "velocityY": -0.918273122155384, + "timestamp": 1.9059340896083397 + }, + { + "x": 7.694109553786587, + "y": 0.9554552205250534, + "heading": -0.3423846615894022, + "angularVelocity": 0.051196248581704275, + "velocityX": 2.536815281233286, + "velocityY": -0.8583516129975453, + "timestamp": 1.9678716526662745 + }, + { + "x": 7.835030505907911, + "y": 0.9064832909135739, + "heading": -0.3388644821779997, + "angularVelocity": 0.056834322140020786, + "velocityX": 2.2752098268623953, + "velocityY": -0.7906660706956153, + "timestamp": 2.0298092157242094 + }, + { + "x": 7.959827165183218, + "y": 0.8619979100061141, + "heading": -0.33562038589869275, + "angularVelocity": 0.052376879540328165, + "velocityX": 2.014878421331732, + "velocityY": -0.7182294347914383, + "timestamp": 2.091746778782144 + }, + { + "x": 8.068556862392501, + "y": 0.8221974820041511, + "heading": -0.33307610796483417, + "angularVelocity": 0.04107810847318481, + "velocityX": 1.7554726379463939, + "velocityY": -0.642589505252806, + "timestamp": 2.153684341840079 + }, + { + "x": 8.161262931310752, + "y": 0.7872247537071302, + "heading": -0.33153778511771786, + "angularVelocity": 0.024836670530246144, + "velocityX": 1.4967664909827672, + "velocityY": -0.5646448870503377, + "timestamp": 2.215621904898014 + }, + { + "x": 8.237979201929296, + "y": 0.7571873231462991, + "heading": -0.3312369649013586, + "angularVelocity": 0.004856830031849059, + "velocityX": 1.2386065390849326, + "velocityY": -0.4849630672865623, + "timestamp": 2.2775594679559488 + }, + { + "x": 8.298732785621018, + "y": 0.7321691840482208, + "heading": -0.33235491812017326, + "angularVelocity": -0.018049680413951164, + "velocityX": 0.9808843082007279, + "velocityY": -0.40392514433731974, + "timestamp": 2.3394970310138836 + }, + { + "x": 8.34354588005975, + "y": 0.7122377172112425, + "heading": -0.33503742038505996, + "angularVelocity": -0.043309780566883505, + "velocityX": 0.7235204652274613, + "velocityY": -0.3217993387685419, + "timestamp": 2.4014345940718185 + }, + { + "x": 8.37243698558101, + "y": 0.697448170361649, + "heading": -0.33940425947903785, + "angularVelocity": -0.07050388937474399, + "velocityX": 0.4664553155608451, + "velocityY": -0.2387815425634291, + "timestamp": 2.4633721571297533 + }, + { + "x": 8.385421752929688, + "y": 0.6878466606140137, + "heading": -0.34555563246426124, + "angularVelocity": -0.0993157089417533, + "velocityX": 0.20964285173008823, + "velocityY": -0.15501917210811605, + "timestamp": 2.525309720187688 + }, + { + "x": 8.381221613409881, + "y": 0.6835499744193162, + "heading": -0.3542287504471472, + "angularVelocity": -0.13146726396270184, + "velocityX": -0.06366578340339453, + "velocityY": -0.06512923947740829, + "timestamp": 2.591281413227155 + }, + { + "x": 8.358990441245284, + "y": 0.6851839703239002, + "heading": -0.3649454511800031, + "angularVelocity": -0.16244392464575347, + "velocityX": -0.3369804705678509, + "velocityY": 0.024768136594682598, + "timestamp": 2.657253106266622 + }, + { + "x": 8.31872779484583, + "y": 0.6927492166918153, + "heading": -0.3776152319611175, + "angularVelocity": -0.19204874389891546, + "velocityX": -0.6103018513617493, + "velocityY": 0.11467412793831581, + "timestamp": 2.723224799306089 + }, + { + "x": 8.26043318618489, + "y": 0.7062463795602301, + "heading": -0.3921309983002572, + "angularVelocity": -0.2200302231209342, + "velocityX": -0.8836306296711994, + "velocityY": 0.20459021508422195, + "timestamp": 2.789196492345556 + }, + { + "x": 8.184106078073826, + "y": 0.72567624991864, + "heading": -0.40836398748744607, + "angularVelocity": -0.24605991508324035, + "velocityX": -1.1569675506948445, + "velocityY": 0.294518292061811, + "timestamp": 2.855168185385023 + }, + { + "x": 8.089745885570975, + "y": 0.7510397809748904, + "heading": -0.426156376289344, + "angularVelocity": -0.26969731989830587, + "velocityX": -1.430313338273756, + "velocityY": 0.3844608177794827, + "timestamp": 2.9211398784244897 + }, + { + "x": 7.9773519881013435, + "y": 0.7823381401243138, + "heading": -0.4453100949140704, + "angularVelocity": -0.2903323795747961, + "velocityX": -1.7036685325385499, + "velocityY": 0.47442103889465864, + "timestamp": 2.9871115714639567 + }, + { + "x": 7.846923768615211, + "y": 0.8195727828481538, + "heading": -0.46556907619150867, + "angularVelocity": -0.3070859689066713, + "velocityX": -1.9770330800530667, + "velocityY": 0.5644033222182843, + "timestamp": 3.0530832645034236 + }, + { + "x": 7.698460723132396, + "y": 0.8627455590379989, + "heading": -0.48658932670817834, + "angularVelocity": -0.3186253004617381, + "velocityX": -2.2504052669074097, + "velocityY": 0.6544136462288057, + "timestamp": 3.1190549575428905 + }, + { + "x": 7.531962767971369, + "y": 0.9118588631154407, + "heading": -0.50788425226007, + "angularVelocity": -0.3227888291293674, + "velocityX": -2.52377872220773, + "velocityY": 0.7444602649208999, + "timestamp": 3.1850266505823575 + }, + { + "x": 7.347431178555123, + "y": 0.9669158145351346, + "heading": -0.528713009661178, + "angularVelocity": -0.31572264469016176, + "velocityX": -2.79713284462553, + "velocityY": 0.8345541683575805, + "timestamp": 3.2509983436218244 + }, + { + "x": 7.144872018682828, + "y": 1.0279202336148925, + "heading": -0.5478112276567515, + "angularVelocity": -0.28949110013211704, + "velocityX": -3.070395051876507, + "velocityY": 0.9247059802340185, + "timestamp": 3.3169700366612913 + }, + { + "x": 6.9243138547651855, + "y": 1.094874091293601, + "heading": -0.5625298674499013, + "angularVelocity": -0.22310538224848228, + "velocityX": -3.343224249007752, + "velocityY": 1.0148876676342686, + "timestamp": 3.3829417297007582 + }, + { + "x": 6.6860270471357435, + "y": 1.1677363105602094, + "heading": -0.5635939187210077, + "angularVelocity": -0.016128906536775855, + "velocityX": -3.6119553197898044, + "velocityY": 1.1044467090304817, + "timestamp": 3.448913422740225 + }, + { + "x": 6.44899650553119, + "y": 1.2456322116060934, + "heading": -0.5635939252708948, + "angularVelocity": -9.928329560968507e-8, + "velocityX": -3.59291281887752, + "velocityY": 1.1807473396094794, + "timestamp": 3.514885115779692 + }, + { + "x": 6.2119660599131885, + "y": 1.3235284047302205, + "heading": -0.5635939318207752, + "angularVelocity": -9.928319425999079e-8, + "velocityX": -3.592911363911807, + "velocityY": 1.1807517669362584, + "timestamp": 3.580856808819159 + }, + { + "x": 5.974935614307485, + "y": 1.4014245978917717, + "heading": -0.5635939383706557, + "angularVelocity": -9.928319467077491e-8, + "velocityX": -3.5929113637253804, + "velocityY": 1.1807517675035346, + "timestamp": 3.646828501858626 + }, + { + "x": 5.737905296446339, + "y": 1.4793211797663546, + "heading": -0.5635939449205359, + "angularVelocity": -9.928319168961564e-8, + "velocityX": -3.5929094273712967, + "velocityY": 1.1807576596220166, + "timestamp": 3.712800194898093 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.563593951480198, + "angularVelocity": -9.943146555012959e-8, + "velocityX": -3.5606437907587183, + "velocityY": 1.2747558956305969, + "timestamp": 3.77877188793756 + }, + { + "x": 5.285962555889255, + "y": 1.6593525613363396, + "heading": -0.5635939570041091, + "angularVelocity": -8.80379800734208e-8, + "velocityX": -3.4591170542763976, + "velocityY": 1.5289526345693891, + "timestamp": 3.8415165242030325 + }, + { + "x": 5.071426961149029, + "y": 1.7607652993502259, + "heading": -0.5635939625107921, + "angularVelocity": -8.776340606411008e-8, + "velocityX": -3.419186204738301, + "velocityY": 1.6162774071206454, + "timestamp": 3.904261160468505 + }, + { + "x": 4.856891538791384, + "y": 1.862178402033475, + "heading": -0.5635939680174745, + "angularVelocity": -8.776339871909346e-8, + "velocityX": -3.4191834573706834, + "velocityY": 1.6162832190813905, + "timestamp": 3.967005796733978 + }, + { + "x": 4.642356116443861, + "y": 1.9635915047381367, + "heading": -0.5635939735241571, + "angularVelocity": -8.776339868323175e-8, + "velocityX": -3.4191834572093653, + "velocityY": 1.6162832194226513, + "timestamp": 4.0297504329994505 + }, + { + "x": 4.427820694096338, + "y": 2.0650046074427992, + "heading": -0.5635939790308396, + "angularVelocity": -8.776339874733319e-8, + "velocityX": -3.4191834572093556, + "velocityY": 1.6162832194226713, + "timestamp": 4.092495069264923 + }, + { + "x": 4.213285271748816, + "y": 2.166417710147462, + "heading": -0.5635939845375221, + "angularVelocity": -8.776339865248036e-8, + "velocityX": -3.419183457209353, + "velocityY": 1.616283219422678, + "timestamp": 4.155239705530396 + }, + { + "x": 3.9987498494044535, + "y": 2.2678308128588087, + "heading": -0.5635939900442046, + "angularVelocity": -8.776339914853593e-8, + "velocityX": -3.4191834571589985, + "velocityY": 1.6162832195291985, + "timestamp": 4.2179843417958685 + }, + { + "x": 3.784214480868839, + "y": 2.3692440293975783, + "heading": -0.5635939955531623, + "angularVelocity": -8.779965948091874e-8, + "velocityX": -3.419182599575763, + "velocityY": 1.6162850336670995, + "timestamp": 4.280728978061341 + }, + { + "x": 3.5824862821613, + "y": 2.464592718536819, + "heading": -0.5976475454093522, + "angularVelocity": -0.5427324450827832, + "velocityX": -3.2150668282469037, + "velocityY": 1.519630916909295, + "timestamp": 4.343473614326814 + }, + { + "x": 3.3962755579097226, + "y": 2.5526066544868016, + "heading": -0.6290952782619788, + "angularVelocity": -0.5012019309438841, + "velocityX": -2.967755259010819, + "velocityY": 1.4027324276388398, + "timestamp": 4.4062182505922864 + }, + { + "x": 3.2255823355920983, + "y": 2.6332858939778268, + "heading": -0.6579338280606744, + "angularVelocity": -0.45961776998243625, + "velocityX": -2.7204432518410133, + "velocityY": 1.2858348425142074, + "timestamp": 4.468962886857759 + }, + { + "x": 3.070406633071231, + "y": 2.70663049336622, + "heading": -0.6841601724081167, + "angularVelocity": -0.41798543920915443, + "velocityX": -2.4731309599806814, + "velocityY": 1.1689381555751166, + "timestamp": 4.531707523123232 + }, + { + "x": 2.930748464054276, + "y": 2.772640503904357, + "heading": -0.7077714566039787, + "angularVelocity": -0.3763076113145772, + "velocityX": -2.225818449660962, + "velocityY": 1.0520422854767753, + "timestamp": 4.594452159388704 + }, + { + "x": 2.8066078395848333, + "y": 2.8313159708307802, + "heading": -0.7287650312356323, + "angularVelocity": -0.33458755809548013, + "velocityX": -1.9785057633325538, + "velocityY": 0.9351471363730166, + "timestamp": 4.657196795654177 + }, + { + "x": 2.697984768845601, + "y": 2.8826569331257605, + "heading": -0.7471385161808816, + "angularVelocity": -0.2928295713997149, + "velocityX": -1.731192930653829, + "velocityY": 0.8182526085218894, + "timestamp": 4.71994143191965 + }, + { + "x": 2.6048792597285715, + "y": 2.926663423472239, + "heading": -0.7628898638650828, + "angularVelocity": -0.2510389512428944, + "velocityX": -1.483879972195546, + "velocityY": 0.7013586015589744, + "timestamp": 4.782686068185122 + }, + { + "x": 2.5272913192824404, + "y": 2.9633354682979096, + "heading": -0.7760174151917997, + "angularVelocity": -0.20922188904202302, + "velocityX": -1.236566901397856, + "velocityY": 0.5844650157905298, + "timestamp": 4.845430704450595 + }, + { + "x": 2.465220954071045, + "y": 2.992673087849862, + "heading": -0.7865199462233702, + "angularVelocity": -0.16738532019110303, + "velocityX": -0.9892537259882359, + "velocityY": 0.4675717527124547, + "timestamp": 4.908175340716068 + }, + { + "x": 2.4186681704546857, + "y": 3.014676296277141, + "heading": -0.7943967050876205, + "angularVelocity": -0.12553676828922553, + "velocityX": -0.7419404492105731, + "velocityY": 0.35067871513643956, + "timestamp": 4.97091997698154 + }, + { + "x": 2.3876329747987564, + "y": 3.0293451017064936, + "heading": -0.7996474390669244, + "angularVelocity": -0.08368418867053315, + "velocityX": -0.4946270709837142, + "velocityY": 0.23378580708140337, + "timestamp": 5.033664613247013 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": -0.04183581368700928, + "velocityX": -0.2473135890317597, + "velocityY": 0.11689293351474017, + "timestamp": 5.096409249512486 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": 8.260301472108595e-27, + "velocityX": 2.4653400544228422e-26, + "velocityY": -1.932180439483193e-25, + "timestamp": 5.159153885777958 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.4.traj b/src/main/deploy/choreo/SourceLanePGHSprint.4.traj index f736ac99..01b876ca 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.4.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.4.traj @@ -1,337 +1,356 @@ { - "samples": [ - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -7.343237406242101e-30, - "velocityX": 6.07048254686404e-33, - "velocityY": -1.403576741314563e-31, - "timestamp": 0 - }, - { - "x": 2.3887780433959134, - "y": 3.0288255570872233, - "heading": -0.7958553483876907, - "angularVelocity": 0.1079808929405059, - "velocityX": 0.2803852472906352, - "velocityY": -0.13215958314273293, - "timestamp": 0.059427769276288345 - }, - { - "x": 2.422106198393558, - "y": 3.0131153644204134, - "heading": -0.7831901230540868, - "angularVelocity": 0.2131196490779077, - "velocityX": 0.5608178702232047, - "velocityY": -0.26435777176444086, - "timestamp": 0.11885553855257669 - }, - { - "x": 2.47210314120033, - "y": 2.9895461805025576, - "heading": -0.7644823111559433, - "angularVelocity": 0.3147991608305137, - "velocityX": 0.8413060664338303, - "velocityY": -0.3966021980108423, - "timestamp": 0.17828330782886503 - }, - { - "x": 2.5387727913774722, - "y": 2.9581147004806385, - "heading": -0.7399860035384287, - "angularVelocity": 0.41220304776557914, - "velocityX": 1.1218602176902186, - "velocityY": -0.5289022355153943, - "timestamp": 0.23771107710515338 - }, - { - "x": 2.6221198536798296, - "y": 2.9188169151709924, - "heading": -0.7100217609714737, - "angularVelocity": 0.5042128104749645, - "velocityX": 1.4024935365630693, - "velocityY": -0.6612697361556471, - "timestamp": 0.2971388463814417 - }, - { - "x": 2.7221500309650937, - "y": 2.871647898493915, - "heading": -0.6750052177423963, - "angularVelocity": 0.5892286326010091, - "velocityX": 1.6832228182788898, - "velocityY": -0.7937201286790226, - "timestamp": 0.35656661565773007 - }, - { - "x": 2.8388702680559588, - "y": 2.816601499667555, - "heading": -0.6354955831852485, - "angularVelocity": 0.6648345552665271, - "velocityX": 1.9640689615693319, - "velocityY": -0.9262740213325141, - "timestamp": 0.4159943849340184 - }, - { - "x": 2.9722889163933544, - "y": 2.753669910841413, - "heading": -0.5922850618953597, - "angularVelocity": 0.7271099322127873, - "velocityX": 2.2450556358107243, - "velocityY": -1.0589592978591336, - "timestamp": 0.47542215421030676 - }, - { - "x": 3.122415239721002, - "y": 2.6828431673634756, - "heading": -0.5465844996458821, - "angularVelocity": 0.7690102254582669, - "velocityX": 2.5261981924591717, - "velocityY": -1.191812251081979, - "timestamp": 0.5348499234865951 - }, - { - "x": 3.289254937095535, - "y": 2.6041094136623544, - "heading": -0.5004858972625192, - "angularVelocity": 0.7757081065791619, - "velocityX": 2.8074366479896216, - "velocityY": -1.3248646997853857, - "timestamp": 0.5942776927628834 - }, - { - "x": 3.4727741175032825, - "y": 2.5174653392644726, - "heading": -0.45856185133391086, - "angularVelocity": 0.7054622180728715, - "velocityX": 3.0881048143423393, - "velocityY": -1.4579728543249981, - "timestamp": 0.6537054620391718 - }, - { - "x": 3.672087548521264, - "y": 2.4232619622496343, - "heading": -0.44156909021440377, - "angularVelocity": 0.2859397437680458, - "velocityX": 3.353877041746403, - "velocityY": -1.5851743749110756, - "timestamp": 0.7131332313154601 - }, - { - "x": 3.87540622002593, - "y": 2.327473100540026, - "heading": -0.4415690736830568, - "angularVelocity": 2.781754584465502e-7, - "velocityX": 3.4212738250264785, - "velocityY": -1.6118535640173974, - "timestamp": 0.7725610005917485 - }, - { - "x": 4.078724897772698, - "y": 2.231684252079618, - "heading": -0.4415690571518241, - "angularVelocity": 2.781735362772021e-7, - "velocityX": 3.4212739300632746, - "velocityY": -1.6118533410711124, - "timestamp": 0.8319887698680368 - }, - { - "x": 4.282043575519764, - "y": 2.135895403619844, - "heading": -0.4415690406205913, - "angularVelocity": 2.7817353604412086e-7, - "velocityX": 3.4212739300683004, - "velocityY": -1.611853341060444, - "timestamp": 0.8914165391443252 - }, - { - "x": 4.485362253266831, - "y": 2.0401065551600697, - "heading": -0.44156902408935866, - "angularVelocity": 2.781735361125895e-7, - "velocityX": 3.421273930068301, - "velocityY": -1.611853341060444, - "timestamp": 0.9508443084206135 - }, - { - "x": 4.688680931013898, - "y": 1.9443177067002955, - "heading": -0.4415690075581259, - "angularVelocity": 2.781735368678374e-7, - "velocityX": 3.421273930068301, - "velocityY": -1.611853341060444, - "timestamp": 1.0102720776969019 - }, - { - "x": 4.8919996087609645, - "y": 1.8485288582405217, - "heading": -0.4415689910268932, - "angularVelocity": 2.7817353639391735e-7, - "velocityX": 3.4212739300683026, - "velocityY": -1.6118533410604396, - "timestamp": 1.0696998469731902 - }, - { - "x": 5.0953182865105076, - "y": 1.7527400097860029, - "heading": -0.4415689744956604, - "angularVelocity": 2.781735367341045e-7, - "velocityX": 3.4212739301099617, - "velocityY": -1.611853340972016, - "timestamp": 1.1291276162494785 - }, - { - "x": 5.298637015997747, - "y": 1.656951271148522, - "heading": -0.4415689579644276, - "angularVelocity": 2.7817353744823303e-7, - "velocityX": 3.4212748007079674, - "velocityY": -1.6118514930642196, - "timestamp": 1.188555385525767 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.44156894141194364, - "angularVelocity": 2.7853113329051353e-7, - "velocityX": 3.4389071599112104, - "velocityY": -1.5738818379758603, - "timestamp": 1.2479831548020552 - }, - { - "x": 5.76887189670182, - "y": 1.4734081866468696, - "heading": -0.441568927295876, - "angularVelocity": 1.9019557057481577e-7, - "velocityX": 3.5822280134028937, - "velocityY": -1.2127779127617127, - "timestamp": 1.3222018528993154 - }, - { - "x": 6.03546847764148, - "y": 1.3855778991398175, - "heading": -0.4415689131909676, - "angularVelocity": 1.9004521532739413e-7, - "velocityX": 3.5920406551769672, - "velocityY": -1.1833983855651649, - "timestamp": 1.3964205509965755 - }, - { - "x": 6.302065075021444, - "y": 1.2977476615350296, - "heading": -0.441568899086059, - "angularVelocity": 1.900452165326357e-7, - "velocityX": 3.592040876688587, - "velocityY": -1.1833977131973445, - "timestamp": 1.4706392490938356 - }, - { - "x": 6.568661672398624, - "y": 1.2099174239217916, - "heading": -0.44156888498115054, - "angularVelocity": 1.9004521568233082e-7, - "velocityX": 3.5920408766510765, - "velocityY": -1.1833977133112008, - "timestamp": 1.5448579471910957 - }, - { - "x": 6.835258130689826, - "y": 1.1220867641390964, - "heading": -0.44156887087145064, - "angularVelocity": 1.9010977386064987e-7, - "velocityX": 3.5920390026492015, - "velocityY": -1.1834034014931274, - "timestamp": 1.6190766452883558 - }, - { - "x": 7.080885784636739, - "y": 1.0408339813622511, - "heading": -0.35950242791659653, - "angularVelocity": 1.105738109919598, - "velocityX": 3.3095117570646315, - "velocityY": -1.0947751019611969, - "timestamp": 1.693295343385616 - }, - { - "x": 7.299180381748869, - "y": 0.9686950138524031, - "heading": -0.28235870642192235, - "angularVelocity": 1.03941086912835, - "velocityX": 2.9412345232203254, - "velocityY": -0.9719783472261578, - "timestamp": 1.767514041482876 - }, - { - "x": 7.490166378274822, - "y": 0.9056069788063349, - "heading": -0.2132225753720476, - "angularVelocity": 0.9315190487378978, - "velocityX": 2.5732868053771534, - "velocityY": -0.8500288561164988, - "timestamp": 1.8417327395801362 - }, - { - "x": 7.653856153896351, - "y": 0.8515488890315335, - "heading": -0.15310137838681676, - "angularVelocity": 0.8100545890297152, - "velocityX": 2.2055058875732163, - "velocityY": -0.7283621400089914, - "timestamp": 1.9159514376773963 - }, - { - "x": 7.790256705792751, - "y": 0.8065104939114175, - "heading": -0.10249736767748478, - "angularVelocity": 0.6818229369001992, - "velocityX": 1.8378192476193804, - "velocityY": -0.6068335375688586, - "timestamp": 1.9901701357746564 - }, - { - "x": 7.899372493751606, - "y": 0.7704858089539702, - "heading": -0.06171206576746063, - "angularVelocity": 0.5495286626665922, - "velocityX": 1.470192697477151, - "velocityY": -0.48538556834071367, - "timestamp": 2.0643888338719165 - }, - { - "x": 7.981206582030799, - "y": 0.7434709210905415, - "heading": -0.030946027709333555, - "angularVelocity": 0.4145321710976964, - "velocityX": 1.1026074342068202, - "velocityY": -0.3639903226007307, - "timestamp": 2.1386075319691766 - }, - { - "x": 8.035761170601306, - "y": 0.7254630347979746, - "heading": -0.010340878696429398, - "angularVelocity": 0.2776274650631065, - "velocityX": 0.7350518126714517, - "velocityY": -0.24263274288330566, - "timestamp": 2.2128262300664368 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": -7.30119772009808e-38, - "angularVelocity": 0.13932983145219902, - "velocityX": 0.3675179222006851, - "velocityY": -0.12130427346551735, - "timestamp": 2.287044928163697 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": 1.121139660303252e-31, - "angularVelocity": 1.5105915295246559e-30, - "velocityX": -7.247567295016868e-32, - "velocityY": -3.7967728194239397e-31, - "timestamp": 2.361263626260957 - } - ] + "samples": [ + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": 8.260301472108595e-27, + "velocityX": 2.4653400544228422e-26, + "velocityY": -1.932180439483193e-25, + "timestamp": 0 + }, + { + "x": 2.3862784269914368, + "y": 3.0299921206081337, + "heading": -0.7979445021330078, + "angularVelocity": 0.07214760810661107, + "velocityX": 0.23610252086471115, + "velocityY": -0.11148080699315335, + "timestamp": 0.05998687912401479 + }, + { + "x": 2.4146057122945543, + "y": 3.016617241069024, + "heading": -0.7893555779678016, + "angularVelocity": 0.14318004688074568, + "velocityX": 0.47222468841152654, + "velocityY": -0.22296341690754115, + "timestamp": 0.11997375824802958 + }, + { + "x": 2.4570985950819932, + "y": 2.9965547338415663, + "heading": -0.7765849361695806, + "angularVelocity": 0.21289058515312126, + "velocityX": 0.7083696202896328, + "velocityY": -0.3344482580262399, + "timestamp": 0.17996063737204437 + }, + { + "x": 2.5137586742181073, + "y": 2.9698044451299115, + "heading": -0.7597273140396543, + "angularVelocity": 0.28102182304025686, + "velocityX": 0.9445412057356202, + "velocityY": -0.445935662969764, + "timestamp": 0.23994751649605917 + }, + { + "x": 2.5845878441848784, + "y": 2.9363662103913186, + "heading": -0.7388971765018356, + "angularVelocity": 0.34724489491702143, + "velocityX": 1.1807443727876086, + "velocityY": -0.5574258108921492, + "timestamp": 0.29993439562007396 + }, + { + "x": 2.669588380905788, + "y": 2.8962398697740044, + "heading": -0.7142349758160352, + "angularVelocity": 0.41112658377867206, + "velocityX": 1.416985480194453, + "velocityY": -0.6689186235936444, + "timestamp": 0.35992127474408875 + }, + { + "x": 2.7687630627135538, + "y": 2.8494252954239245, + "heading": -0.6859166587732123, + "angularVelocity": 0.47207518471295223, + "velocityX": 1.65327290327499, + "velocityY": -0.7804135676619703, + "timestamp": 0.41990815386810354 + }, + { + "x": 2.8821153447700505, + "y": 2.7959224429728664, + "heading": -0.6541688089081689, + "angularVelocity": 0.5292465673936607, + "velocityX": 1.8896179249824876, + "velocityY": -0.8919092513622485, + "timestamp": 0.47989503299211833 + }, + { + "x": 3.00964961510041, + "y": 2.7357314565429642, + "heading": -0.6192942625032292, + "angularVelocity": 0.5813695747171842, + "velocityX": 2.126036096438681, + "velocityY": -1.00340253250157, + "timestamp": 0.5398819121161331 + }, + { + "x": 3.151371568939876, + "y": 2.668852905662324, + "heading": -0.5817190532214994, + "angularVelocity": 0.6263904678896166, + "velocityX": 2.3625492092441416, + "velocityY": -1.114886319429574, + "timestamp": 0.5998687912401479 + }, + { + "x": 3.307288700965784, + "y": 2.595288399732038, + "heading": -0.5420886206685631, + "angularVelocity": 0.6606516813619456, + "velocityX": 2.5991872606602855, + "velocityY": -1.2263432771390068, + "timestamp": 0.6598556703641627 + }, + { + "x": 3.477410464476997, + "y": 2.5150425548921422, + "heading": -0.5015011072339337, + "angularVelocity": 0.6766065184141348, + "velocityX": 2.8359829015193223, + "velocityY": -1.3377232823531002, + "timestamp": 0.7198425494881775 + }, + { + "x": 3.661742704023134, + "y": 2.428132077104721, + "heading": -0.46226256504462326, + "angularVelocity": 0.6541187466710837, + "velocityX": 3.072875972844897, + "velocityY": -1.44882479396444, + "timestamp": 0.7798294286121923 + }, + { + "x": 3.8601464032626667, + "y": 2.3346891273821493, + "heading": -0.4327535625874362, + "angularVelocity": 0.4919242822448071, + "velocityX": 3.307451598363016, + "velocityY": -1.55772314024523, + "timestamp": 0.8398163077362071 + }, + { + "x": 4.065242172431025, + "y": 2.2377114042955792, + "heading": -0.4327535491582794, + "angularVelocity": 2.2386823508713805e-7, + "velocityX": 3.4190104930171277, + "velocityY": -1.6166489156083852, + "timestamp": 0.8998031868602219 + }, + { + "x": 4.270337923318267, + "y": 2.140733642546332, + "heading": -0.4327535357295662, + "angularVelocity": 2.238608404824225e-7, + "velocityX": 3.4190101882652333, + "velocityY": -1.6166495601272812, + "timestamp": 0.9597900659842367 + }, + { + "x": 4.475433674204211, + "y": 2.043755880794341, + "heading": -0.43275352230085307, + "angularVelocity": 2.2386084074280025e-7, + "velocityX": 3.4190101882436053, + "velocityY": -1.6166495601730213, + "timestamp": 1.0197769451082515 + }, + { + "x": 4.680529425090156, + "y": 1.9467781190423497, + "heading": -0.43275350887213987, + "angularVelocity": 2.238608397454811e-7, + "velocityX": 3.4190101882436035, + "velocityY": -1.6166495601730246, + "timestamp": 1.0797638242322662 + }, + { + "x": 4.885625175976101, + "y": 1.8498003572903603, + "heading": -0.4327534954434267, + "angularVelocity": 2.2386084091908683e-7, + "velocityX": 3.4190101882436177, + "velocityY": -1.6166495601729944, + "timestamp": 1.139750703356281 + }, + { + "x": 5.090720926874051, + "y": 1.7528225955637593, + "heading": -0.43275348201471353, + "angularVelocity": 2.238608408563689e-7, + "velocityX": 3.41901018844374, + "velocityY": -1.616649559749761, + "timestamp": 1.1997375824802958 + }, + { + "x": 5.295816846922337, + "y": 1.6558451915696206, + "heading": -0.4327534685859999, + "angularVelocity": 2.2386084788003102e-7, + "velocityX": 3.41901300823263, + "velocityY": -1.6166435962379466, + "timestamp": 1.2597244616043106 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.4327534551249964, + "angularVelocity": 2.2439912995828436e-7, + "velocityX": 3.453867801804683, + "velocityY": -1.5407737242905175, + "timestamp": 1.3197113407283254 + }, + { + "x": 5.767637731173938, + "y": 1.4715486411010592, + "heading": -0.43275344331162413, + "angularVelocity": 1.594904372832459e-7, + "velocityX": 3.572782848206104, + "velocityY": -1.2403267182093187, + "timestamp": 1.3937808112664776 + }, + { + "x": 6.033774187788168, + "y": 1.3841256962215103, + "heading": -0.43275343151544077, + "angularVelocity": 1.5925837367602458e-7, + "velocityX": 3.5930654651722995, + "velocityY": -1.1802831077956568, + "timestamp": 1.4678502818046297 + }, + { + "x": 6.29991069823436, + "y": 1.2967029152195577, + "heading": -0.43275341971925746, + "angularVelocity": 1.5925837245370656e-7, + "velocityX": 3.593066191948934, + "velocityY": -1.1802808953106085, + "timestamp": 1.541919752342782 + }, + { + "x": 6.566047156059249, + "y": 1.2092799740277729, + "heading": -0.4327534079212868, + "angularVelocity": 1.5928250285484768e-7, + "velocityX": 3.5930654815171974, + "velocityY": -1.180283058007746, + "timestamp": 1.615989222880934 + }, + { + "x": 6.815604164652681, + "y": 1.1270295176704332, + "heading": -0.3680065962902011, + "angularVelocity": 0.8741362826096571, + "velocityX": 3.369229005962575, + "velocityY": -1.1104501727870977, + "timestamp": 1.6900586934190862 + }, + { + "x": 7.042448674730911, + "y": 1.0523256933844904, + "heading": -0.3045889928856297, + "angularVelocity": 0.8561908562841154, + "velocityX": 3.0625912191634557, + "velocityY": -1.0085643078475075, + "timestamp": 1.7641281639572384 + }, + { + "x": 7.24659057217413, + "y": 0.9851213013692786, + "heading": -0.24567984884817082, + "angularVelocity": 0.7953228720207434, + "velocityX": 2.7560869000416006, + "velocityY": -0.9073156798197457, + "timestamp": 1.8381976344953905 + }, + { + "x": 7.4280385469425845, + "y": 0.925399905884711, + "heading": -0.19230648340236473, + "angularVelocity": 0.7205852162574101, + "velocityX": 2.4496999026743933, + "velocityY": -0.8062889480734987, + "timestamp": 1.9122671050335427 + }, + { + "x": 7.586797948314142, + "y": 0.8731533384819596, + "heading": -0.1449799977839161, + "angularVelocity": 0.6389472649743265, + "velocityX": 2.1433851250466707, + "velocityY": -0.7053724972401416, + "timestamp": 1.9863365755716949 + }, + { + "x": 7.722872326109614, + "y": 0.8283767850935353, + "heading": -0.10400779947540247, + "angularVelocity": 0.5531590547472527, + "velocityX": 1.8371182729783764, + "velocityY": -0.6045210403571151, + "timestamp": 2.060406046109847 + }, + { + "x": 7.836264193936779, + "y": 0.7910670987596001, + "heading": -0.06959571066012236, + "angularVelocity": 0.46459207235125305, + "velocityX": 1.530885356724122, + "velocityY": -0.5037120700723456, + "timestamp": 2.134475516647999 + }, + { + "x": 7.92697541388319, + "y": 0.7612220649081012, + "heading": -0.04189104486357632, + "angularVelocity": 0.37403623375808837, + "velocityX": 1.2246775802141858, + "velocityY": -0.40293299836842056, + "timestamp": 2.2085449871861513 + }, + { + "x": 7.995007406321081, + "y": 0.7388400303082948, + "heading": -0.02100380767783558, + "angularVelocity": 0.2819952273721474, + "velocityX": 0.918488979921211, + "velocityY": -0.3021762466666727, + "timestamp": 2.2826144577243035 + }, + { + "x": 8.04036127275019, + "y": 0.7239196955099017, + "heading": -0.007018297514014948, + "angularVelocity": 0.1888161217058643, + "velocityX": 0.6123152507988858, + "velocityY": -0.2014370386339925, + "timestamp": 2.3566839282624557 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -1.7184723296669235e-26, + "angularVelocity": 0.09475290511763465, + "velocityX": 0.30615312084056384, + "velocityY": -0.10071228953000028, + "timestamp": 2.430753398800608 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -8.404488237973452e-27, + "angularVelocity": 4.825075090660763e-27, + "velocityX": -1.1513118466109779e-26, + "velocityY": -4.361967999563915e-27, + "timestamp": 2.50482286933876 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGHSprint.traj b/src/main/deploy/choreo/SourceLanePGHSprint.traj index 1540ab71..b2199188 100644 --- a/src/main/deploy/choreo/SourceLanePGHSprint.traj +++ b/src/main/deploy/choreo/SourceLanePGHSprint.traj @@ -1,1966 +1,2102 @@ { - "samples": [ - { - "x": 0.433241993188858, - "y": 4.121134281158447, - "heading": 8.991694014476798e-34, - "angularVelocity": 0, - "velocityX": -4.6784915292932193e-35, - "velocityY": -2.119592179507727e-34, - "timestamp": 0 - }, - { - "x": 0.4570749914043126, - "y": 4.107894224868064, - "heading": -0.009932712149027413, - "angularVelocity": -0.1373640763265399, - "velocityX": 0.3295975698116412, - "velocityY": -0.18310287014791135, - "timestamp": 0.07230938695656872 - }, - { - "x": 0.5047410210186508, - "y": 4.0814144720383245, - "heading": -0.029797898632204542, - "angularVelocity": -0.27472486380099975, - "velocityX": 0.6591955985322896, - "velocityY": -0.36620076513225003, - "timestamp": 0.14461877391313743 - }, - { - "x": 0.576240240509544, - "y": 4.041695512105355, - "heading": -0.059590394556189516, - "angularVelocity": -0.41201422357348, - "velocityX": 0.9887958189140486, - "velocityY": -0.5492918914777362, - "timestamp": 0.21692816086970615 - }, - { - "x": 0.6715729183897807, - "y": 3.9887379727274364, - "heading": -0.09930156463679939, - "angularVelocity": -0.5491841620018386, - "velocityX": 1.3183997526835154, - "velocityY": -0.7323743376461849, - "timestamp": 0.28923754782627487 - }, - { - "x": 0.7907394144382421, - "y": 3.9225426248725555, - "heading": -0.148921029894937, - "angularVelocity": -0.686210564721571, - "velocityX": 1.648008662001748, - "velocityY": -0.9154461217412811, - "timestamp": 0.3615469347828436 - }, - { - "x": 0.9337401624459083, - "y": 3.8431103831358566, - "heading": -0.20843840181811515, - "angularVelocity": -0.8230932998910107, - "velocityX": 1.9776235704163385, - "velocityY": -1.0985052574765801, - "timestamp": 0.43385632173941235 - }, - { - "x": 1.100575665552893, - "y": 3.7504423098204516, - "heading": -0.2778446966612386, - "angularVelocity": -0.9598517946889951, - "velocityX": 2.307245436988583, - "velocityY": -1.2815497021301627, - "timestamp": 0.5061657086959811 - }, - { - "x": 1.2912465471408558, - "y": 3.6445396941775194, - "heading": -0.3571330045970933, - "angularVelocity": -1.0965147302863703, - "velocityX": 2.636875924594487, - "velocityY": -1.464576317132112, - "timestamp": 0.5784750956525498 - }, - { - "x": 1.505785276048115, - "y": 3.525464237325283, - "heading": -0.4461366707355329, - "angularVelocity": -1.2308729182270346, - "velocityX": 2.966955438802405, - "velocityY": -1.6467496387952614, - "timestamp": 0.6507844826091186 - }, - { - "x": 1.6964880033537257, - "y": 3.4196219693446848, - "heading": -0.5252603846379951, - "angularVelocity": -1.094238483172127, - "velocityX": 2.637316333772171, - "velocityY": -1.463741741361608, - "timestamp": 0.7230938695656873 - }, - { - "x": 1.8633540969095699, - "y": 3.3270118414974243, - "heading": -0.5945029830335408, - "angularVelocity": -0.9575879607046729, - "velocityX": 2.3076684864728434, - "velocityY": -1.280748347415596, - "timestamp": 0.7954032565222561 - }, - { - "x": 2.0063830653266717, - "y": 3.247633047534605, - "heading": -0.6538614517041024, - "angularVelocity": -0.8208957532196227, - "velocityX": 1.9780138435278007, - "velocityY": -1.0977661034589712, - "timestamp": 0.8677126434788248 - }, - { - "x": 2.125574496423577, - "y": 3.1814849244321213, - "heading": -0.7033318821965574, - "angularVelocity": -0.6841494939262661, - "velocityX": 1.648353500334547, - "velocityY": -0.9147930287697538, - "timestamp": 0.9400220304353936 - }, - { - "x": 2.220928049802523, - "y": 3.128566939134693, - "heading": -0.7429103365332542, - "angularVelocity": -0.5473487745162161, - "velocityX": 1.318688449622979, - "velocityY": -0.7318273259488806, - "timestamp": 1.0123314173919622 - }, - { - "x": 2.292443455008997, - "y": 3.0888786919590703, - "heading": -0.7725937492005902, - "angularVelocity": -0.4105056606988637, - "velocityX": 0.989019658670723, - "velocityY": -0.5488671505327598, - "timestamp": 1.084640804348531 - }, - { - "x": 2.34012050914709, - "y": 3.0624199236271457, - "heading": -0.7923807693651485, - "angularVelocity": -0.273643865580592, - "velocityX": 0.6593480617769434, - "velocityY": -0.3659105607936737, - "timestamp": 1.1569501913050997 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.13679610671265519, - "velocityX": 0.3296745537009426, - "velocityY": -0.18295553238277804, - "timestamp": 1.2292595782616684 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 2.834394108236562e-32, - "velocityX": -3.2904683025944325e-34, - "velocityY": 0, - "timestamp": 1.3015689652182372 - }, - { - "x": 2.379804145625828, - "y": 3.040219511725313, - "heading": -0.7770894823147659, - "angularVelocity": 0.41593022428853316, - "velocityX": 0.2617028389642093, - "velocityY": -0.14816838427057621, - "timestamp": 1.3621150092281344 - }, - { - "x": 2.411575732183553, - "y": 3.0222750861201635, - "heading": -0.7276303412852089, - "angularVelocity": 0.8168847665996504, - "velocityX": 0.524750825215457, - "velocityY": -0.2963765163949562, - "timestamp": 1.4226610532380317 - }, - { - "x": 2.459369710281825, - "y": 2.995353279300503, - "heading": -0.6550403019178791, - "angularVelocity": 1.1989229115524662, - "velocityX": 0.789382343303212, - "velocityY": -0.4446501379224719, - "timestamp": 1.483207097247929 - }, - { - "x": 2.5233031088128075, - "y": 2.959437775047564, - "heading": -0.5609434444834801, - "angularVelocity": 1.5541371690447268, - "velocityX": 1.0559467522028614, - "velocityY": -0.5931932439230606, - "timestamp": 1.5437531412578263 - }, - { - "x": 2.6035132843463873, - "y": 2.9144863047853526, - "heading": -0.44765744633251, - "angularVelocity": 1.8710718429836914, - "velocityX": 1.3247797910705466, - "velocityY": -0.7424344727603186, - "timestamp": 1.6042991852677235 - }, - { - "x": 2.700148811858614, - "y": 2.8604246981568315, - "heading": -0.31830060076747974, - "angularVelocity": 2.136503675514867, - "velocityX": 1.5960667470930034, - "velocityY": -0.8929007255979282, - "timestamp": 1.6648452292776208 - }, - { - "x": 2.8133605147902916, - "y": 2.7971622097455233, - "heading": -0.1768724119476308, - "angularVelocity": 2.3358782746686186, - "velocityX": 1.869844756713935, - "velocityY": -1.0448657620135586, - "timestamp": 1.725391273287518 - }, - { - "x": 2.943296329434224, - "y": 2.7246257074465063, - "heading": -0.02888673141070084, - "angularVelocity": 2.4441841404657145, - "velocityX": 2.1460661347699634, - "velocityY": -1.1980386742882738, - "timestamp": 1.7859373172974153 - }, - { - "x": 3.0900160520490236, - "y": 2.6428091950168, - "heading": 0.11617462075416056, - "angularVelocity": 2.3958848928453333, - "velocityX": 2.4232751291036574, - "velocityY": -1.3513106226450158, - "timestamp": 1.8464833613073126 - }, - { - "x": 3.252842131476166, - "y": 2.55207108446101, - "heading": 0.23929724530787766, - "angularVelocity": 2.0335370636864525, - "velocityX": 2.6892934474880965, - "velocityY": -1.4986629108411686, - "timestamp": 1.9070294053172099 - }, - { - "x": 3.4280030327225406, - "y": 2.451410973839388, - "heading": 0.3174619447233944, - "angularVelocity": 1.2909959798981987, - "velocityX": 2.8930197523349617, - "velocityY": -1.662538193332126, - "timestamp": 1.9675754493271072 - }, - { - "x": 3.6156107870917813, - "y": 2.340964520507575, - "heading": 0.3503075182755731, - "angularVelocity": 0.5424891764490772, - "velocityX": 3.098596406043856, - "velocityY": -1.8241729106819762, - "timestamp": 2.028121493337004 - }, - { - "x": 3.8155697764375462, - "y": 2.229387834544172, - "heading": 0.3503075829776931, - "angularVelocity": 0.0000010686432293648565, - "velocityX": 3.302593796434966, - "velocityY": -1.8428402348659505, - "timestamp": 2.0886675373469012 - }, - { - "x": 4.015529033815858, - "y": 2.117811628928195, - "heading": 0.35030764767858236, - "angularVelocity": 0.000001068622902276308, - "velocityX": 3.3025982233558384, - "velocityY": -1.8428323012769912, - "timestamp": 2.1492135813567983 - }, - { - "x": 4.215488291220884, - "y": 2.0062354233600974, - "heading": 0.35030771237947195, - "angularVelocity": 0.0000010686229082180607, - "velocityX": 3.302598223797092, - "velocityY": -1.8428323004862048, - "timestamp": 2.2097596253666953 - }, - { - "x": 4.415447878546615, - "y": 1.8946598090554148, - "heading": 0.350307777080342, - "angularVelocity": 0.0000010686225853054014, - "velocityX": 3.3026036728847936, - "velocityY": -1.842822534969312, - "timestamp": 2.2703056693765924 - }, - { - "x": 4.620289311400001, - "y": 1.7923224180578192, - "heading": 0.35030784179343544, - "angularVelocity": 0.00000106882447086681, - "velocityX": 3.383233970164951, - "velocityY": -1.6902407526553997, - "timestamp": 2.3308517133864894 - }, - { - "x": 4.832980772388526, - "y": 1.7074973259478416, - "heading": 0.35030790812662205, - "angularVelocity": 0.0000010955825050486418, - "velocityX": 3.512887827217186, - "velocityY": -1.401001394841118, - "timestamp": 2.3913977573963865 - }, - { - "x": 5.052028438396142, - "y": 1.6407809635052637, - "heading": 0.3503079787205065, - "angularVelocity": 0.0000011659537069164618, - "velocityX": 3.617869170309609, - "velocityY": -1.101911174108616, - "timestamp": 2.4519438014062835 - }, - { - "x": 5.275893582425239, - "y": 1.5926421132936275, - "heading": 0.3503080570287918, - "angularVelocity": 0.0000012933674954586782, - "velocityX": 3.697436350961301, - "velocityY": -0.7950783738037029, - "timestamp": 2.5124898454161806 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.35030814446785413, - "angularVelocity": 0.0000014441746575542642, - "velocityX": 3.751029791428781, - "velocityY": -0.4826595916936126, - "timestamp": 2.5730358894260776 - }, - { - "x": 5.645916996346935, - "y": 1.5526078041590061, - "heading": 0.35030823517092025, - "angularVelocity": 0.0000023934601751232044, - "velocityX": 3.7711793420471578, - "velocityY": -0.28528395446410554, - "timestamp": 2.6109320980454838 - }, - { - "x": 5.78920070535826, - "y": 1.5493061559809091, - "heading": 0.350308318678533, - "angularVelocity": 0.000002203587530623072, - "velocityX": 3.7809510299653186, - "velocityY": -0.08712344317225716, - "timestamp": 2.64882830666489 - }, - { - "x": 5.932460399917018, - "y": 1.5535231156733713, - "heading": 0.3503083968024594, - "angularVelocity": 0.0000020615235464554193, - "velocityX": 3.7803173398564307, - "velocityY": 0.11127655895113732, - "timestamp": 2.686724515284296 - }, - { - "x": 6.075301824148574, - "y": 1.5652470604158988, - "heading": 0.3503084709306375, - "angularVelocity": 0.0000019560842841988308, - "velocityX": 3.769280079337772, - "velocityY": 0.3093698596678062, - "timestamp": 2.724620723903702 - }, - { - "x": 6.217331878166829, - "y": 1.5844456932271112, - "heading": 0.3503085421651287, - "angularVelocity": 0.0000018797260658699983, - "velocityX": 3.747869752477631, - "velocityY": 0.5066109120314745, - "timestamp": 2.7625169325231083 - }, - { - "x": 6.358159707247505, - "y": 1.6110661098029424, - "heading": 0.3503086114142786, - "angularVelocity": 0.000001827337150563986, - "velocityX": 3.716145604300894, - "velocityY": 0.7024559328132687, - "timestamp": 2.8004131411425144 - }, - { - "x": 6.497397807591943, - "y": 1.6450348544134032, - "heading": 0.35030867945721406, - "angularVelocity": 0.0000017955077283623068, - "velocityX": 3.6741960585771425, - "velocityY": 0.8963626137804634, - "timestamp": 2.8383093497619205 - }, - { - "x": 6.634663343210931, - "y": 1.6862573848132372, - "heading": 0.3503087469924593, - "angularVelocity": 0.000001782110868887289, - "velocityX": 3.622144288827235, - "velocityY": 1.0877745268355006, - "timestamp": 2.8762055583813266 - }, - { - "x": 6.770832471288569, - "y": 1.7309686010151983, - "heading": 0.350308814465375, - "angularVelocity": 0.0000017804661250723641, - "velocityX": 3.593212435713354, - "velocityY": 1.179833493397679, - "timestamp": 2.9141017670007328 - }, - { - "x": 6.907000934216078, - "y": 1.775681842897667, - "heading": 0.35030888193834014, - "angularVelocity": 0.0000017804674286764754, - "velocityX": 3.5931948838222003, - "velocityY": 1.1798869467794602, - "timestamp": 2.951997975620139 - }, - { - "x": 7.041712141501968, - "y": 1.8246105114710787, - "heading": 0.3503089535974655, - "angularVelocity": 0.0000018909312562539825, - "velocityX": 3.5547410201057947, - "velocityY": 1.2911230530949709, - "timestamp": 2.989894184239545 - }, - { - "x": 7.171770756302622, - "y": 1.8790616059317657, - "heading": 0.3592109050640362, - "angularVelocity": 0.2349034848307258, - "velocityX": 3.43196904225537, - "velocityY": 1.43684807648023, - "timestamp": 3.027790392858951 - }, - { - "x": 7.296132052444026, - "y": 1.9323306109051834, - "heading": 0.3900794027691138, - "angularVelocity": 0.8145537200064432, - "velocityX": 3.281628972184848, - "velocityY": 1.4056552598282668, - "timestamp": 3.0656866014783573 - }, - { - "x": 7.41350002407911, - "y": 1.9843017273758428, - "heading": 0.4310938047294686, - "angularVelocity": 1.0822824618754054, - "velocityX": 3.097090076050037, - "velocityY": 1.371406754501705, - "timestamp": 3.1035828100977634 - }, - { - "x": 7.523578248846203, - "y": 2.0344435812093704, - "heading": 0.471270328487765, - "angularVelocity": 1.0601726458124487, - "velocityX": 2.904729226942307, - "velocityY": 1.3231364207724636, - "timestamp": 3.1414790187171695 - }, - { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.9454290339278092, - "velocityX": 2.715460784576445, - "velocityY": 1.2708550641540455, - "timestamp": 3.1793752273365756 - }, - { - "x": 7.782116606596641, - "y": 2.1591418247265515, - "heading": 0.5502904833862925, - "angularVelocity": 0.6648323287723413, - "velocityX": 2.395575421885603, - "velocityY": 1.1781054843634051, - "timestamp": 3.244341952215483 - }, - { - "x": 7.917274495842872, - "y": 2.2282082387650113, - "heading": 0.5787513876779874, - "angularVelocity": 0.4380843323215672, - "velocityX": 2.0804171596791274, - "velocityY": 1.0631044456557224, - "timestamp": 3.30930867709439 - }, - { - "x": 8.032169166662662, - "y": 2.2890615062609574, - "heading": 0.5943003276772478, - "angularVelocity": 0.2393369840982844, - "velocityX": 1.768515667581298, - "velocityY": 0.9366836270316962, - "timestamp": 3.3742754019732972 - }, - { - "x": 8.12694730000964, - "y": 2.3412528312827354, - "heading": 0.5980433212943994, - "angularVelocity": 0.05761401123618554, - "velocityX": 1.458871961356148, - "velocityY": 0.8033547191898265, - "timestamp": 3.4392421268522044 - }, - { - "x": 8.201715109739398, - "y": 2.3844820330172567, - "heading": 0.590722775552934, - "angularVelocity": -0.11268146509017382, - "velocityX": 1.1508631513920338, - "velocityY": 0.6654052796273325, - "timestamp": 3.5042088517311116 - }, - { - "x": 8.256552657549896, - "y": 2.418534329374767, - "heading": 0.572871769156374, - "angularVelocity": -0.27477153003838295, - "velocityX": 0.8440866907283819, - "velocityY": 0.5241498077820845, - "timestamp": 3.569175576610019 - }, - { - "x": 8.291522317600203, - "y": 2.443248428942356, - "heading": 0.5448922111974887, - "angularVelocity": -0.4306752112106213, - "velocityX": 0.538270324007952, - "velocityY": 0.3804116586399298, - "timestamp": 3.634142301488926 - }, - { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.5817394500892022, - "velocityX": 0.23322225384012837, - "velocityY": 0.2347399755865949, - "timestamp": 3.6991090263678332 - }, - { - "x": 8.301908465997242, - "y": 2.464162051150854, - "heading": 0.4594182049840147, - "angularVelocity": -0.7297490572088752, - "velocityX": -0.07293676039464714, - "velocityY": 0.08667758549500083, - "timestamp": 3.7644469653994896 - }, - { - "x": 8.277129357589926, - "y": 2.4601442119160213, - "heading": 0.4025234718451046, - "angularVelocity": -0.8707763664131557, - "velocityX": -0.3792453324141557, - "velocityY": -0.061493204321711674, - "timestamp": 3.829784904431146 - }, - { - "x": 8.232325554908503, - "y": 2.44643699593842, - "heading": 0.33696231020008877, - "angularVelocity": -1.0034164318107932, - "velocityX": -0.6857241496355521, - "velocityY": -0.20978953699413314, - "timestamp": 3.8951228434628025 - }, - { - "x": 8.167484390379387, - "y": 2.42303097033958, - "heading": 0.26340235513425153, - "angularVelocity": -1.1258383131766259, - "velocityX": -0.992396844621899, - "velocityY": -0.35823024028199546, - "timestamp": 3.960460782494459 - }, - { - "x": 8.082591447673355, - "y": 2.3899154191246006, - "heading": 0.18267464694130434, - "angularVelocity": -1.235541086685248, - "velocityX": -1.2992901821543714, - "velocityY": -0.5068349523381112, - "timestamp": 4.025798721526115 - }, - { - "x": 7.9776304347285665, - "y": 2.347078349358033, - "heading": 0.09584952732096214, - "angularVelocity": -1.328862233904795, - "velocityX": -1.6064328704022224, - "velocityY": -0.6556232167931427, - "timestamp": 4.091136660557772 - }, - { - "x": 7.852583598052917, - "y": 2.2945065473278667, - "heading": 0.004378357289613106, - "angularVelocity": -1.3999702376138785, - "velocityX": -1.9138472766192256, - "velocityY": -0.8046137176854637, - "timestamp": 4.156474599589428 - }, - { - "x": 7.707434523289432, - "y": 2.232185897198679, - "heading": -0.08962534429630348, - "angularVelocity": -1.4387307432573238, - "velocityX": -2.221512905283988, - "velocityY": -0.9538202620531743, - "timestamp": 4.2218125386210845 - }, - { - "x": 7.542180705960218, - "y": 2.1601037497785995, - "heading": -0.1827828763211059, - "angularVelocity": -1.42578008130418, - "velocityX": -2.5292168650919318, - "velocityY": -1.1032204028528767, - "timestamp": 4.287150477652741 - }, - { - "x": 7.35689136818444, - "y": 2.078264678726032, - "heading": -0.2689709068831343, - "angularVelocity": -1.3191115581449608, - "velocityX": -2.835861377353881, - "velocityY": -1.252550543611691, - "timestamp": 4.352488416684397 - }, - { - "x": 7.152059282385439, - "y": 1.9867952525471149, - "heading": -0.3348300100346558, - "angularVelocity": -1.007976439532523, - "velocityX": -3.134963986234081, - "velocityY": -1.3999435478765458, - "timestamp": 4.417826355716054 - }, - { - "x": 6.932416862636984, - "y": 1.8843357350106482, - "heading": -0.3485403575636041, - "angularVelocity": -0.20983746552376023, - "velocityX": -3.3616367917885555, - "velocityY": -1.5681473743275887, - "timestamp": 4.48316429474771 - }, - { - "x": 6.70958980281502, - "y": 1.7775224962078198, - "heading": -0.34854038203919246, - "angularVelocity": -3.7459994478515855e-7, - "velocityX": -3.410377846690442, - "velocityY": -1.6347812677571678, - "timestamp": 4.548502233779367 - }, - { - "x": 6.478027967608038, - "y": 1.6912665812341474, - "heading": -0.3485404067374628, - "angularVelocity": -3.780081020278718e-7, - "velocityX": -3.5440639640437577, - "velocityY": -1.3201505320191125, - "timestamp": 4.613840172811023 - }, - { - "x": 6.239619726654668, - "y": 1.6262859956866853, - "heading": -0.34854043255776096, - "angularVelocity": -3.951807866461482e-7, - "velocityX": -3.6488485019072954, - "velocityY": -0.9945306893745133, - "timestamp": 4.679178111842679 - }, - { - "x": 5.996315346781838, - "y": 1.5831125822372847, - "heading": -0.3485404605967219, - "angularVelocity": -4.291375179720768e-7, - "velocityX": -3.7237841211206324, - "velocityY": -0.6607709714945885, - "timestamp": 4.744516050874336 - }, - { - "x": 5.7501052372367845, - "y": 1.5620996014320112, - "heading": -0.34854049237288, - "angularVelocity": -4.863354823397662e-7, - "velocityX": -3.7682564401941674, - "velocityY": -0.3216045855853207, - "timestamp": 4.809853989905992 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.348540530032263, - "angularVelocity": -5.763784949174497e-7, - "velocityX": -3.7819013522532754, - "velocityY": 0.02019321393800792, - "timestamp": 4.875191928937649 - }, - { - "x": 5.265773365103949, - "y": 1.585443267719361, - "heading": -0.3485405619317938, - "angularVelocity": -5.063689324990554e-7, - "velocityX": -3.7657613238076486, - "velocityY": 0.34961055977354083, - "timestamp": 4.9381885481486885 - }, - { - "x": 5.031364041570911, - "y": 1.6280522215500413, - "heading": -0.3485405890146467, - "angularVelocity": -4.299096234955573e-7, - "velocityX": -3.720982593490641, - "velocityY": 0.6763688966853497, - "timestamp": 5.001185167359728 - }, - { - "x": 4.801558310439742, - "y": 1.6909217775230576, - "heading": -0.3485406130254673, - "angularVelocity": -3.811445901784285e-7, - "velocityX": -3.6479057766785146, - "velocityY": 0.9979830149678419, - "timestamp": 5.064181786570768 - }, - { - "x": 4.57810382416931, - "y": 1.7735737496127562, - "heading": -0.34854063513060685, - "angularVelocity": -3.5089406173482316e-7, - "velocityX": -3.5470869559183034, - "velocityY": 1.3120064715347763, - "timestamp": 5.127178405781808 - }, - { - "x": 4.3626998372206165, - "y": 1.8753793427019887, - "heading": -0.34854065619045194, - "angularVelocity": -3.3430119484907394e-7, - "velocityX": -3.4192943946259384, - "velocityY": 1.6160485175907584, - "timestamp": 5.190175024992848 - }, - { - "x": 4.156979523974483, - "y": 1.9955559179970916, - "heading": -0.34854067691382595, - "angularVelocity": -3.2896009785914497e-7, - "velocityX": -3.265577039253029, - "velocityY": 1.907667058965506, - "timestamp": 5.253171644203888 - }, - { - "x": 3.9516956402233028, - "y": 2.116476485340398, - "heading": -0.3485406976311522, - "angularVelocity": -3.288640962267947e-7, - "velocityX": -3.258649215182055, - "velocityY": 1.9194770903216498, - "timestamp": 5.316168263414927 - }, - { - "x": 3.746411773108691, - "y": 2.2373970809271935, - "heading": -0.34854071834847844, - "angularVelocity": -3.288640958736373e-7, - "velocityX": -3.2586489510953776, - "velocityY": 1.9194775386550607, - "timestamp": 5.379164882625967 - }, - { - "x": 3.5411278789507334, - "y": 2.3583176306028646, - "heading": -0.34854073906621164, - "angularVelocity": -3.2887055643589624e-7, - "velocityX": -3.2586493803778747, - "velocityY": 1.919476809867947, - "timestamp": 5.442161501837007 - }, - { - "x": 3.345073520020233, - "y": 2.47307412374708, - "heading": -0.3873766723080469, - "angularVelocity": -0.6164764669629881, - "velocityX": -3.1121409590840567, - "velocityY": 1.821629391884994, - "timestamp": 5.505158121048047 - }, - { - "x": 3.1664636949700515, - "y": 2.5778338770119946, - "heading": -0.44780580157610866, - "angularVelocity": -0.9592440042793773, - "velocityX": -2.835228735876023, - "velocityY": 1.6629424654356146, - "timestamp": 5.568154740259087 - }, - { - "x": 3.0057996639738764, - "y": 2.672135804524931, - "heading": -0.5103083460228613, - "angularVelocity": -0.9921571225491842, - "velocityX": -2.5503595749788768, - "velocityY": 1.496936322837019, - "timestamp": 5.631151359470127 - }, - { - "x": 2.8630598597209174, - "y": 2.7559513152021253, - "heading": -0.5701467016897592, - "angularVelocity": -0.9498661422835671, - "velocityX": -2.265832770720234, - "velocityY": 1.3304763291568347, - "timestamp": 5.694147978681166 - }, - { - "x": 2.738214483951598, - "y": 2.829279302871158, - "heading": -0.6251012943842897, - "angularVelocity": -0.8723419348970285, - "velocityX": -1.9817789800923042, - "velocityY": 1.1639987762419943, - "timestamp": 5.757144597892206 - }, - { - "x": 2.6312405711995455, - "y": 2.892122971419519, - "heading": -0.6738749327170909, - "angularVelocity": -0.7742262829916016, - "velocityX": -1.698089740239665, - "velocityY": 0.9975720814133604, - "timestamp": 5.820141217103246 - }, - { - "x": 2.5421208919645824, - "y": 2.9444859859038597, - "heading": -0.7156160888836572, - "angularVelocity": -0.6625935913597552, - "velocityX": -1.414673999193671, - "velocityY": 0.8312035652091746, - "timestamp": 5.883137836314286 - }, - { - "x": 2.4708422721118457, - "y": 2.98637166116498, - "heading": -0.749723844710458, - "angularVelocity": -0.5414220041322991, - "velocityX": -1.1314673826217794, - "velocityY": 0.6648876683493641, - "timestamp": 5.946134455525326 - }, - { - "x": 2.4173943837507443, - "y": 3.0177828179574866, - "heading": -0.775752501403198, - "angularVelocity": -0.4131754532023893, - "velocityX": -0.84842470961893, - "velocityY": 0.4986165477750299, - "timestamp": 6.0091310747363655 - }, - { - "x": 2.3817689275168203, - "y": 3.038721799552771, - "heading": -0.7933591587514436, - "angularVelocity": -0.2794857496917854, - "velocityX": -0.565513779629629, - "velocityY": 0.3323826239808009, - "timestamp": 6.072127693947405 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.14148780267529462, - "velocityX": -0.28271125846882317, - "velocityY": 0.16617910323716242, - "timestamp": 6.135124313158445 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 1.2572572851920672e-28, - "velocityX": 2.1335040537014396e-30, - "velocityY": 1.8634721866519574e-29, - "timestamp": 6.198120932369485 - }, - { - "x": 2.384240228496088, - "y": 3.0394763007756747, - "heading": -0.7921549333865273, - "angularVelocity": 0.15389773911589558, - "velocityX": 0.30849818872977186, - "velocityY": -0.14776374894381028, - "timestamp": 6.263862499797187 - }, - { - "x": 2.4248103079231944, - "y": 3.020044385403153, - "heading": -0.7722377725963315, - "angularVelocity": 0.3029614529969845, - "velocityX": 0.6171145747585456, - "velocityY": -0.2955803479114183, - "timestamp": 6.329604067224889 - }, - { - "x": 2.485678594891513, - "y": 2.9908905714242224, - "heading": -0.7429157303412026, - "angularVelocity": 0.4460198228065572, - "velocityX": 0.9258721589693997, - "velocityY": -0.4434608896569423, - "timestamp": 6.395345634652592 - }, - { - "x": 2.5668563689761195, - "y": 2.952009793968192, - "heading": -0.7046902372675247, - "angularVelocity": 0.581450892781281, - "velocityX": 1.2348013176576613, - "velocityY": -0.5914184735371368, - "timestamp": 6.461087202080294 - }, - { - "x": 2.668357540158173, - "y": 2.903395952391573, - "heading": -0.6582160423239786, - "angularVelocity": 0.706922526522589, - "velocityX": 1.5439420621310505, - "velocityY": -0.7394688547710901, - "timestamp": 6.526828769507996 - }, - { - "x": 2.790199449693154, - "y": 2.8450417291439147, - "heading": -0.604379415056612, - "angularVelocity": 0.8189130465526614, - "velocityX": 1.8533465857651596, - "velocityY": -0.8876305438234711, - "timestamp": 6.592570336935698 - }, - { - "x": 2.9324037360004773, - "y": 2.7769385796688133, - "heading": -0.5444410242237507, - "angularVelocity": 0.9117274378767651, - "velocityX": 2.1630802530485744, - "velocityY": -1.0359222047754857, - "timestamp": 6.6583119043634005 - }, - { - "x": 3.094996415116772, - "y": 2.6990776165764148, - "heading": -0.4803328988113282, - "angularVelocity": 0.975153588829843, - "velocityX": 2.473209652250889, - "velocityY": -1.1843490524929314, - "timestamp": 6.724053471791103 - }, - { - "x": 3.2780012401054357, - "y": 2.6114549065561117, - "heading": -0.4154109268291881, - "angularVelocity": 0.9875330711202905, - "velocityX": 2.783700361113535, - "velocityY": -1.3328357300982285, - "timestamp": 6.789795039218805 - }, - { - "x": 3.48136962382921, - "y": 2.514109658078373, - "heading": -0.3571766392011983, - "angularVelocity": 0.8858061939583661, - "velocityX": 3.0934520073228393, - "velocityY": -1.4807260046665736, - "timestamp": 6.855536606646507 - }, - { - "x": 3.702896130222067, - "y": 2.408427187890042, - "heading": -0.3455558213194451, - "angularVelocity": 0.17676514778162136, - "velocityX": 3.3696565971974572, - "velocityY": -1.6075441204615863, - "timestamp": 6.921278174074209 - }, - { - "x": 3.927079400169571, - "y": 2.300911734169414, - "heading": -0.34555579773751244, - "angularVelocity": 3.5870657763866696e-7, - "velocityX": 3.4100688304100117, - "velocityY": -1.6354257728775006, - "timestamp": 6.9870197415019115 - }, - { - "x": 4.15126266204063, - "y": 2.1933962636083315, - "heading": -0.3455557741556376, - "angularVelocity": 3.587056984423895e-7, - "velocityX": 3.4100687075585983, - "velocityY": -1.6354260290389449, - "timestamp": 7.052761308929614 - }, - { - "x": 4.375445923911402, - "y": 2.0858807930466483, - "heading": -0.3455557505737627, - "angularVelocity": 3.5870569914452407e-7, - "velocityX": 3.4100687075542147, - "velocityY": -1.6354260290480847, - "timestamp": 7.118502876357316 - }, - { - "x": 4.599629185782173, - "y": 1.978365322484965, - "heading": -0.34555572699188775, - "angularVelocity": 3.5870569932883336e-7, - "velocityX": 3.4100687075542147, - "velocityY": -1.6354260290480853, - "timestamp": 7.184244443785018 - }, - { - "x": 4.823812447652946, - "y": 1.8708498519232832, - "heading": -0.3455557034100129, - "angularVelocity": 3.5870569853758494e-7, - "velocityX": 3.4100687075542258, - "velocityY": -1.6354260290480622, - "timestamp": 7.24998601121272 - }, - { - "x": 5.047995709543858, - "y": 1.7633343814035949, - "heading": -0.34555567982813806, - "angularVelocity": 3.5870569820699746e-7, - "velocityX": 3.410068707860571, - "velocityY": -1.6354260284092945, - "timestamp": 7.315727578640423 - }, - { - "x": 5.27217953587615, - "y": 1.65582008782303, - "heading": -0.34555565624625534, - "angularVelocity": 3.587058178727361e-7, - "velocityX": 3.410077293621499, - "velocityY": -1.6354081258984543, - "timestamp": 7.381469146068125 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, - "angularVelocity": 3.617497277100986e-7, - "velocityX": 3.511082415812739, - "velocityY": -1.405520236667608, - "timestamp": 7.447210713495827 - }, - { - "x": 5.6599765905642645, - "y": 1.5113593672660246, - "heading": -0.34555563704903824, - "angularVelocity": -1.0484559207434358e-7, - "velocityX": 3.589689630249378, - "velocityY": -1.1905096787253004, - "timestamp": 7.490939561801255 - }, - { - "x": 5.819811381479902, - "y": 1.4688909784273936, - "heading": -0.3455556415836398, - "angularVelocity": -1.0369817036745698e-7, - "velocityX": 3.6551337871799947, - "velocityY": -0.9711755622285476, - "timestamp": 7.534668410106684 - }, - { - "x": 5.98037051193366, - "y": 1.4292489543347602, - "heading": -0.34555564611341977, - "angularVelocity": -1.0358790889899858e-7, - "velocityX": 3.6716981277969567, - "velocityY": -0.9065416910994355, - "timestamp": 7.578397258412112 - }, - { - "x": 6.140929731813359, - "y": 1.389607292439096, - "heading": -0.3455556506431997, - "angularVelocity": -1.0358790900922708e-7, - "velocityX": 3.6717001728071876, - "velocityY": -0.9065334083071072, - "timestamp": 7.62212610671754 - }, - { - "x": 6.30148895170346, - "y": 1.3499656305855623, - "heading": -0.34555565517297965, - "angularVelocity": -1.0358790856487398e-7, - "velocityX": 3.6717001730450596, - "velocityY": -0.9065334073436624, - "timestamp": 7.665854955022969 - }, - { - "x": 6.462048171593562, - "y": 1.3103239687320334, - "heading": -0.34555565970275964, - "angularVelocity": -1.0358791005302326e-7, - "velocityX": 3.671700173045087, - "velocityY": -0.9065334073435504, - "timestamp": 7.709583803328397 - }, - { - "x": 6.622607391483664, - "y": 1.2706823068785016, - "heading": -0.34555566423253964, - "angularVelocity": -1.0358790893463982e-7, - "velocityX": 3.6717001730450707, - "velocityY": -0.9065334073436169, - "timestamp": 7.753312651633825 - }, - { - "x": 6.783166611367605, - "y": 1.2310406450000155, - "heading": -0.3455556687623196, - "angularVelocity": -1.0358790885289903e-7, - "velocityX": 3.6717001729041776, - "velocityY": -0.9065334079142718, - "timestamp": 7.797041499939254 - }, - { - "x": 6.943725778284007, - "y": 1.1913987685896004, - "heading": -0.34555567329209946, - "angularVelocity": -1.0358790829702175e-7, - "velocityX": 3.6716989616319724, - "velocityY": -0.9065383138730976, - "timestamp": 7.840770348244682 - }, - { - "x": 7.103836002524736, - "y": 1.1499808922348331, - "heading": -0.34555567791225333, - "angularVelocity": -1.0565459632350647e-7, - "velocityX": 3.661432451237353, - "velocityY": -0.9471522338178334, - "timestamp": 7.88449919655011 - }, - { - "x": 7.257955684496521, - "y": 1.1005509138662795, - "heading": -0.346747423550466, - "angularVelocity": -0.02725307627333064, - "velocityX": 3.524439539210384, - "velocityY": -1.1303745761449258, - "timestamp": 7.928228044855539 - }, - { - "x": 7.402334567868656, - "y": 1.0534126604700325, - "heading": -0.34675046354447403, - "angularVelocity": -0.00006951918758018307, - "velocityX": 3.3016850195483918, - "velocityY": -1.07796695369166, - "timestamp": 7.971956893160967 - }, - { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, - "angularVelocity": 0.027323634774631837, - "velocityX": 3.079221948168643, - "velocityY": -1.024340556060407, - "timestamp": 8.015685741466395 - }, - { - "x": 7.709533523799992, - "y": 0.9494390432841444, - "heading": -0.3413965784412158, - "angularVelocity": 0.06656216442856697, - "velocityX": 2.7614877570011993, - "velocityY": -0.9471323179117597, - "timestamp": 8.078169497196857 - }, - { - "x": 7.862380855125707, - "y": 0.8957055369328039, - "heading": -0.33682515317576994, - "angularVelocity": 0.07316181961221624, - "velocityX": 2.4461930871290236, - "velocityY": -0.8599596122731851, - "timestamp": 8.140653252927319 - }, - { - "x": 7.99561550345175, - "y": 0.8477270543434197, - "heading": -0.33285887044966567, - "angularVelocity": 0.06347702182330028, - "velocityX": 2.1323085779411386, - "velocityY": -0.7678552934037809, - "timestamp": 8.20313700865778 - }, - { - "x": 8.109294083490143, - "y": 0.8056872786590918, - "heading": -0.33010810532148027, - "angularVelocity": 0.04402368417243478, - "velocityX": 1.8193301396409471, - "velocityY": -0.6728112801937814, - "timestamp": 8.265620764388242 - }, - { - "x": 8.20345588526635, - "y": 0.7697081195865146, - "heading": -0.32897987990878186, - "angularVelocity": 0.018056299585531694, - "velocityX": 1.5069805051795295, - "velocityY": -0.5758162045793389, - "timestamp": 8.328104520118703 - }, - { - "x": 8.278129725528439, - "y": 0.7398763726381793, - "heading": -0.32976503867911666, - "angularVelocity": -0.012565806282865027, - "velocityX": 1.1950920585537632, - "velocityY": -0.4774320397291953, - "timestamp": 8.390588275849165 - }, - { - "x": 8.333337625588607, - "y": 0.7162569714576368, - "heading": -0.33268181216540116, - "angularVelocity": -0.04668050843272986, - "velocityX": 0.883556044523325, - "velocityY": -0.37800866648333437, - "timestamp": 8.453072031579627 - }, - { - "x": 8.369096953381794, - "y": 0.698900317224484, - "heading": -0.33790002427933163, - "angularVelocity": -0.08351309957167831, - "velocityX": 0.572297989695798, - "velocityY": -0.2777786647144699, - "timestamp": 8.515555787310088 - }, - { - "x": 8.385421752929688, - "y": 0.6878466606140137, - "heading": -0.34555563246426124, - "angularVelocity": -0.12252157533477633, - "velocityX": 0.26126469763300175, - "velocityY": -0.1769044847136382, - "timestamp": 8.57803954304055 - }, - { - "x": 8.379560991493735, - "y": 0.6833892008568203, - "heading": -0.35721868503670856, - "angularVelocity": -0.1678763417451177, - "velocityX": -0.08435897751441682, - "velocityY": -0.06416005011256522, - "timestamp": 8.64751361217505 - }, - { - "x": 8.349687812290387, - "y": 0.6867654591427064, - "heading": -0.37193021085772177, - "angularVelocity": -0.2117556378126066, - "velocityX": -0.42999034856465757, - "velocityY": 0.048597387887987537, - "timestamp": 8.71698768130955 - }, - { - "x": 8.295801597591089, - "y": 0.6979765367761911, - "heading": -0.38956490915625874, - "angularVelocity": -0.253831372168465, - "velocityX": -0.7756306111129838, - "velocityY": 0.1613706779112114, - "timestamp": 8.786461750444051 - }, - { - "x": 8.217901630671626, - "y": 0.7170238074049998, - "heading": -0.40996601054380316, - "angularVelocity": -0.2936505899495876, - "velocityX": -1.121281190089058, - "velocityY": 0.2741637400269978, - "timestamp": 8.855935819578551 - }, - { - "x": 8.115987079646073, - "y": 0.7439090305870278, - "heading": -0.4329315185089462, - "angularVelocity": -0.3305622982969707, - "velocityX": -1.4669437431144206, - "velocityY": 0.3869821289721611, - "timestamp": 8.925409888713052 - }, - { - "x": 7.990056990056933, - "y": 0.778634533730576, - "heading": -0.45819105339643357, - "angularVelocity": -0.36358219983610734, - "velocityX": -1.812620034466978, - "velocityY": 0.4998340183057432, - "timestamp": 8.994883957847552 - }, - { - "x": 7.840110319883288, - "y": 0.8212035218059903, - "heading": -0.4853637226253089, - "angularVelocity": -0.3911195870256229, - "velocityX": -2.1583113245224226, - "velocityY": 0.6127320395326444, - "timestamp": 9.064358026982053 - }, - { - "x": 7.6661461501938755, - "y": 0.8716206459377261, - "heading": -0.5138727598830971, - "angularVelocity": -0.4103550808661484, - "velocityX": -2.504015841545443, - "velocityY": 0.7256970083921437, - "timestamp": 9.133832096116553 - }, - { - "x": 7.468164698581641, - "y": 0.9298931437973682, - "heading": -0.5427426487462225, - "angularVelocity": -0.41554912822558204, - "velocityX": -2.8497172265661783, - "velocityY": 0.8387661552805812, - "timestamp": 9.203306165251053 - }, - { - "x": 7.24617329762414, - "y": 0.9960332266921984, - "heading": -0.5699703376985038, - "angularVelocity": -0.391911533201964, - "velocityX": -3.1953130674947268, - "velocityY": 0.9520110700121011, - "timestamp": 9.272780234385554 - }, - { - "x": 7.00025853984304, - "y": 1.0700564595014088, - "heading": -0.589089272180988, - "angularVelocity": -0.2751952594783323, - "velocityX": -3.53966250782022, - "velocityY": 1.065480023430083, - "timestamp": 9.342254303520054 - }, - { - "x": 6.750442302486931, - "y": 1.1514706650489683, - "heading": -0.5890892788822688, - "angularVelocity": -9.645729514768673e-8, - "velocityX": -3.5958198572257243, - "velocityY": 1.171864647656428, - "timestamp": 9.411728372654554 - }, - { - "x": 6.500626129709104, - "y": 1.232885068752249, - "heading": -0.5890892855834843, - "angularVelocity": -9.645635490096592e-8, - "velocityX": -3.5958189276949932, - "velocityY": 1.1718674998820673, - "timestamp": 9.481202441789055 - }, - { - "x": 6.250809956933241, - "y": 1.3142994724615544, - "heading": -0.5890892922846998, - "angularVelocity": -9.645635424551899e-8, - "velocityX": -3.595818927666732, - "velocityY": 1.1718674999687875, - "timestamp": 9.550676510923555 - }, - { - "x": 6.000993784158921, - "y": 1.395713876175591, - "heading": -0.589089298985915, - "angularVelocity": -9.645635348778811e-8, - "velocityX": -3.595818927644538, - "velocityY": 1.1718675000368866, - "timestamp": 9.620150580058056 - }, - { - "x": 5.751177662095486, - "y": 1.477128435493372, - "heading": -0.5890893056871302, - "angularVelocity": -9.64563521650664e-8, - "velocityX": -3.5958181977191597, - "velocityY": 1.1718697397753421, - "timestamp": 9.689624649192556 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.5890893123960679, - "angularVelocity": -9.65675052838285e-8, - "velocityX": -3.5721826564600034, - "velocityY": 1.242054049730676, - "timestamp": 9.759098718327056 - }, - { - "x": 5.287967104202961, - "y": 1.6596756394253018, - "heading": -0.5890893181956361, - "angularVelocity": -9.309834476919773e-8, - "velocityX": -3.4519020760085812, - "velocityY": 1.5451728334255468, - "timestamp": 9.821393791687077 - }, - { - "x": 5.074932733784246, - "y": 1.7602857483579184, - "heading": -0.5890893239791856, - "angularVelocity": -9.284120212486628e-8, - "velocityX": -3.419762734486735, - "velocityY": 1.6150572349624415, - "timestamp": 9.883688865047098 - }, - { - "x": 4.861898448797989, - "y": 1.860896038187167, - "heading": -0.5890893297627348, - "angularVelocity": -9.284119839047264e-8, - "velocityX": -3.4197613630707817, - "velocityY": 1.615060138829829, - "timestamp": 9.94598393840712 - }, - { - "x": 4.648864163815087, - "y": 1.9615063280235205, - "heading": -0.5890893355462841, - "angularVelocity": -9.28411976539737e-8, - "velocityX": -3.419761363016917, - "velocityY": 1.6150601389438841, - "timestamp": 10.00827901176714 - }, - { - "x": 4.435829878832186, - "y": 2.0621166178598744, - "heading": -0.5890893413298333, - "angularVelocity": -9.28411982771023e-8, - "velocityX": -3.419761363016914, - "velocityY": 1.6150601389438883, - "timestamp": 10.070574085127161 - }, - { - "x": 4.222795593849285, - "y": 2.162726907696228, - "heading": -0.5890893471133825, - "angularVelocity": -9.284119843074624e-8, - "velocityX": -3.4197613630169146, - "velocityY": 1.6150601389438883, - "timestamp": 10.132869158487182 - }, - { - "x": 4.009761308866383, - "y": 2.2633371975325822, - "heading": -0.5890893528969318, - "angularVelocity": -9.284119832193768e-8, - "velocityX": -3.419761363016914, - "velocityY": 1.6150601389438886, - "timestamp": 10.195164231847203 - }, - { - "x": 3.7967270238835944, - "y": 2.363947487369175, - "heading": -0.589089358680481, - "angularVelocity": -9.284119741513914e-8, - "velocityX": -3.4197613630151045, - "velocityY": 1.615060138947721, - "timestamp": 10.257459305207224 - }, - { - "x": 3.583692741771847, - "y": 2.464557783284562, - "heading": -0.5890893644643718, - "angularVelocity": -9.284668184194445e-8, - "velocityX": -3.419761316927328, - "velocityY": 1.615060236528384, - "timestamp": 10.319754378567245 - }, - { - "x": 3.381763406617466, - "y": 2.559912098840239, - "heading": -0.6245815423960026, - "angularVelocity": -0.5697429349912143, - "velocityX": -3.241497670086629, - "velocityY": 1.530687908570203, - "timestamp": 10.382049451927266 - }, - { - "x": 3.1981911985495857, - "y": 2.646597580557341, - "heading": -0.6568615726262876, - "angularVelocity": -0.5181795042399261, - "velocityX": -2.9468174314037126, - "velocityY": 1.3915302935133123, - "timestamp": 10.444344525287287 - }, - { - "x": 3.032976151851517, - "y": 2.7246143065734305, - "heading": -0.6859252496642307, - "angularVelocity": -0.46654856428174046, - "velocityX": -2.652136642383354, - "velocityY": 1.2523739327700791, - "timestamp": 10.506639598647308 - }, - { - "x": 2.8861182875087947, - "y": 2.793962348685928, - "heading": -0.7117686923384959, - "angularVelocity": -0.414855321301394, - "velocityX": -2.3574555164898965, - "velocityY": 1.1132187245645664, - "timestamp": 10.568934672007329 - }, - { - "x": 2.7576176211174075, - "y": 2.854641769350887, - "heading": -0.7343883425051251, - "angularVelocity": -0.3631049607390947, - "velocityX": -2.0627741402397475, - "velocityY": 0.9740645189431933, - "timestamp": 10.63122974536735 - }, - { - "x": 2.6474741650844584, - "y": 2.906652621199817, - "heading": -0.7537810666899133, - "angularVelocity": -0.31130429966287193, - "velocityX": -1.7680925648228973, - "velocityY": 0.8349111581961731, - "timestamp": 10.69352481872737 - }, - { - "x": 2.5556879298075064, - "y": 2.9499949470036255, - "heading": -0.7699442582183806, - "angularVelocity": -0.2594617945957906, - "velocityX": -1.473410822497863, - "velocityY": 0.6957584840348696, - "timestamp": 10.755819892087391 - }, - { - "x": 2.482258924484474, - "y": 2.9846687797700127, - "heading": -0.7828759248842103, - "angularVelocity": -0.20758730937026382, - "velocityX": -1.1787289325219326, - "velocityY": 0.556606339734085, - "timestamp": 10.818114965447412 - }, - { - "x": 2.4271871577101587, - "y": 3.010674142880251, - "heading": -0.7925747589157716, - "angularVelocity": -0.15569183096565328, - "velocityX": -0.8840469045768797, - "velocityY": 0.4174545707643066, - "timestamp": 10.880410038807433 - }, - { - "x": 2.3904726379075623, - "y": 3.0280110502230517, - "heading": -0.7990401886427835, - "angularVelocity": -0.10378717574737188, - "velocityX": -0.5893647414204509, - "velocityY": 0.27830302474492646, - "timestamp": 10.942705112167454 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -0.05188569757551244, - "velocityX": -0.29468244125856385, - "velocityY": 0.13915155101802326, - "timestamp": 11.005000185527475 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -7.343237406242101e-30, - "velocityX": 6.07048254686404e-33, - "velocityY": -1.403576741314563e-31, - "timestamp": 11.067295258887496 - }, - { - "x": 2.3887780433959134, - "y": 3.0288255570872233, - "heading": -0.7958553483876907, - "angularVelocity": 0.1079808929405059, - "velocityX": 0.2803852472906352, - "velocityY": -0.13215958314273293, - "timestamp": 11.126723028163784 - }, - { - "x": 2.422106198393558, - "y": 3.0131153644204134, - "heading": -0.7831901230540868, - "angularVelocity": 0.2131196490779077, - "velocityX": 0.5608178702232047, - "velocityY": -0.26435777176444086, - "timestamp": 11.186150797440073 - }, - { - "x": 2.47210314120033, - "y": 2.9895461805025576, - "heading": -0.7644823111559433, - "angularVelocity": 0.3147991608305137, - "velocityX": 0.8413060664338303, - "velocityY": -0.3966021980108423, - "timestamp": 11.245578566716361 - }, - { - "x": 2.5387727913774722, - "y": 2.9581147004806385, - "heading": -0.7399860035384287, - "angularVelocity": 0.41220304776557914, - "velocityX": 1.1218602176902186, - "velocityY": -0.5289022355153943, - "timestamp": 11.30500633599265 - }, - { - "x": 2.6221198536798296, - "y": 2.9188169151709924, - "heading": -0.7100217609714737, - "angularVelocity": 0.5042128104749645, - "velocityX": 1.4024935365630693, - "velocityY": -0.6612697361556471, - "timestamp": 11.364434105268938 - }, - { - "x": 2.7221500309650937, - "y": 2.871647898493915, - "heading": -0.6750052177423963, - "angularVelocity": 0.5892286326010091, - "velocityX": 1.6832228182788898, - "velocityY": -0.7937201286790226, - "timestamp": 11.423861874545226 - }, - { - "x": 2.8388702680559588, - "y": 2.816601499667555, - "heading": -0.6354955831852485, - "angularVelocity": 0.6648345552665271, - "velocityX": 1.9640689615693319, - "velocityY": -0.9262740213325141, - "timestamp": 11.483289643821514 - }, - { - "x": 2.9722889163933544, - "y": 2.753669910841413, - "heading": -0.5922850618953597, - "angularVelocity": 0.7271099322127873, - "velocityX": 2.2450556358107243, - "velocityY": -1.0589592978591336, - "timestamp": 11.542717413097803 - }, - { - "x": 3.122415239721002, - "y": 2.6828431673634756, - "heading": -0.5465844996458821, - "angularVelocity": 0.7690102254582669, - "velocityX": 2.5261981924591717, - "velocityY": -1.191812251081979, - "timestamp": 11.602145182374091 - }, - { - "x": 3.289254937095535, - "y": 2.6041094136623544, - "heading": -0.5004858972625192, - "angularVelocity": 0.7757081065791619, - "velocityX": 2.8074366479896216, - "velocityY": -1.3248646997853857, - "timestamp": 11.66157295165038 - }, - { - "x": 3.4727741175032825, - "y": 2.5174653392644726, - "heading": -0.45856185133391086, - "angularVelocity": 0.7054622180728715, - "velocityX": 3.0881048143423393, - "velocityY": -1.4579728543249981, - "timestamp": 11.721000720926668 - }, - { - "x": 3.672087548521264, - "y": 2.4232619622496343, - "heading": -0.44156909021440377, - "angularVelocity": 0.2859397437680458, - "velocityX": 3.353877041746403, - "velocityY": -1.5851743749110756, - "timestamp": 11.780428490202956 - }, - { - "x": 3.87540622002593, - "y": 2.327473100540026, - "heading": -0.4415690736830568, - "angularVelocity": 2.781754584465502e-7, - "velocityX": 3.4212738250264785, - "velocityY": -1.6118535640173974, - "timestamp": 11.839856259479244 - }, - { - "x": 4.078724897772698, - "y": 2.231684252079618, - "heading": -0.4415690571518241, - "angularVelocity": 2.781735362772021e-7, - "velocityX": 3.4212739300632746, - "velocityY": -1.6118533410711124, - "timestamp": 11.899284028755533 - }, - { - "x": 4.282043575519764, - "y": 2.135895403619844, - "heading": -0.4415690406205913, - "angularVelocity": 2.7817353604412086e-7, - "velocityX": 3.4212739300683004, - "velocityY": -1.611853341060444, - "timestamp": 11.958711798031821 - }, - { - "x": 4.485362253266831, - "y": 2.0401065551600697, - "heading": -0.44156902408935866, - "angularVelocity": 2.781735361125895e-7, - "velocityX": 3.421273930068301, - "velocityY": -1.611853341060444, - "timestamp": 12.01813956730811 - }, - { - "x": 4.688680931013898, - "y": 1.9443177067002955, - "heading": -0.4415690075581259, - "angularVelocity": 2.781735368678374e-7, - "velocityX": 3.421273930068301, - "velocityY": -1.611853341060444, - "timestamp": 12.077567336584398 - }, - { - "x": 4.8919996087609645, - "y": 1.8485288582405217, - "heading": -0.4415689910268932, - "angularVelocity": 2.7817353639391735e-7, - "velocityX": 3.4212739300683026, - "velocityY": -1.6118533410604396, - "timestamp": 12.136995105860686 - }, - { - "x": 5.0953182865105076, - "y": 1.7527400097860029, - "heading": -0.4415689744956604, - "angularVelocity": 2.781735367341045e-7, - "velocityX": 3.4212739301099617, - "velocityY": -1.611853340972016, - "timestamp": 12.196422875136975 - }, - { - "x": 5.298637015997747, - "y": 1.656951271148522, - "heading": -0.4415689579644276, - "angularVelocity": 2.7817353744823303e-7, - "velocityX": 3.4212748007079674, - "velocityY": -1.6118514930642196, - "timestamp": 12.255850644413263 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.44156894141194364, - "angularVelocity": 2.7853113329051353e-7, - "velocityX": 3.4389071599112104, - "velocityY": -1.5738818379758603, - "timestamp": 12.315278413689551 - }, - { - "x": 5.76887189670182, - "y": 1.4734081866468696, - "heading": -0.441568927295876, - "angularVelocity": 1.9019557057481577e-7, - "velocityX": 3.5822280134028937, - "velocityY": -1.2127779127617127, - "timestamp": 12.389497111786811 - }, - { - "x": 6.03546847764148, - "y": 1.3855778991398175, - "heading": -0.4415689131909676, - "angularVelocity": 1.9004521532739413e-7, - "velocityX": 3.5920406551769672, - "velocityY": -1.1833983855651649, - "timestamp": 12.463715809884071 - }, - { - "x": 6.302065075021444, - "y": 1.2977476615350296, - "heading": -0.441568899086059, - "angularVelocity": 1.900452165326357e-7, - "velocityX": 3.592040876688587, - "velocityY": -1.1833977131973445, - "timestamp": 12.537934507981332 - }, - { - "x": 6.568661672398624, - "y": 1.2099174239217916, - "heading": -0.44156888498115054, - "angularVelocity": 1.9004521568233082e-7, - "velocityX": 3.5920408766510765, - "velocityY": -1.1833977133112008, - "timestamp": 12.612153206078592 - }, - { - "x": 6.835258130689826, - "y": 1.1220867641390964, - "heading": -0.44156887087145064, - "angularVelocity": 1.9010977386064987e-7, - "velocityX": 3.5920390026492015, - "velocityY": -1.1834034014931274, - "timestamp": 12.686371904175852 - }, - { - "x": 7.080885784636739, - "y": 1.0408339813622511, - "heading": -0.35950242791659653, - "angularVelocity": 1.105738109919598, - "velocityX": 3.3095117570646315, - "velocityY": -1.0947751019611969, - "timestamp": 12.760590602273112 - }, - { - "x": 7.299180381748869, - "y": 0.9686950138524031, - "heading": -0.28235870642192235, - "angularVelocity": 1.03941086912835, - "velocityX": 2.9412345232203254, - "velocityY": -0.9719783472261578, - "timestamp": 12.834809300370372 - }, - { - "x": 7.490166378274822, - "y": 0.9056069788063349, - "heading": -0.2132225753720476, - "angularVelocity": 0.9315190487378978, - "velocityX": 2.5732868053771534, - "velocityY": -0.8500288561164988, - "timestamp": 12.909027998467632 - }, - { - "x": 7.653856153896351, - "y": 0.8515488890315335, - "heading": -0.15310137838681676, - "angularVelocity": 0.8100545890297152, - "velocityX": 2.2055058875732163, - "velocityY": -0.7283621400089914, - "timestamp": 12.983246696564892 - }, - { - "x": 7.790256705792751, - "y": 0.8065104939114175, - "heading": -0.10249736767748478, - "angularVelocity": 0.6818229369001992, - "velocityX": 1.8378192476193804, - "velocityY": -0.6068335375688586, - "timestamp": 13.057465394662152 - }, - { - "x": 7.899372493751606, - "y": 0.7704858089539702, - "heading": -0.06171206576746063, - "angularVelocity": 0.5495286626665922, - "velocityX": 1.470192697477151, - "velocityY": -0.48538556834071367, - "timestamp": 13.131684092759413 - }, - { - "x": 7.981206582030799, - "y": 0.7434709210905415, - "heading": -0.030946027709333555, - "angularVelocity": 0.4145321710976964, - "velocityX": 1.1026074342068202, - "velocityY": -0.3639903226007307, - "timestamp": 13.205902790856673 - }, - { - "x": 8.035761170601306, - "y": 0.7254630347979746, - "heading": -0.010340878696429398, - "angularVelocity": 0.2776274650631065, - "velocityX": 0.7350518126714517, - "velocityY": -0.24263274288330566, - "timestamp": 13.280121488953933 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": -7.30119772009808e-38, - "angularVelocity": 0.13932983145219902, - "velocityX": 0.3675179222006851, - "velocityY": -0.12130427346551735, - "timestamp": 13.354340187051193 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": 1.121139660303252e-31, - "angularVelocity": 1.5105915295246559e-30, - "velocityX": -7.247567295016868e-32, - "velocityY": -3.7967728194239397e-31, - "timestamp": 13.428558885148453 - } - ] + "samples": [ + { + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": -1.13547667502199e-28, + "angularVelocity": -4.993890068315335e-29, + "velocityX": 1.9809218936022493e-27, + "velocityY": 1.6056222243913227e-27, + "timestamp": 0 + }, + { + "x": 0.4546918803218126, + "y": 4.109218526998405, + "heading": -0.008937300145056587, + "angularVelocity": -0.11893237232009286, + "velocityX": 0.28544257452644645, + "velocityY": -0.15856789939193544, + "timestamp": 0.07514606806129172 + }, + { + "x": 0.4975916635129914, + "y": 4.085387310900469, + "heading": -0.02681241329368555, + "angularVelocity": -0.2378715694618833, + "velocityX": 0.5708852678251674, + "velocityY": -0.31713191006213676, + "timestamp": 0.15029213612258344 + }, + { + "x": 0.5619414537509306, + "y": 4.049641027145361, + "heading": -0.05362171709181763, + "angularVelocity": -0.3567625624306182, + "velocityX": 0.8563294380945293, + "velocityY": -0.47569067387467845, + "timestamp": 0.22543820418387517 + }, + { + "x": 0.6477414523823173, + "y": 4.001980177868811, + "heading": -0.08935853103818607, + "angularVelocity": -0.47556465518888186, + "velocityX": 1.1417762877680475, + "velocityY": -0.6342427555580988, + "timestamp": 0.3005842722451669 + }, + { + "x": 0.7549919358856847, + "y": 3.942405375866793, + "heading": -0.1340144219891079, + "angularVelocity": -0.5942545245946725, + "velocityX": 1.4272268166564648, + "velocityY": -0.7927866825104751, + "timestamp": 0.3757303403064586 + }, + { + "x": 0.8836932393387834, + "y": 3.870917341642709, + "heading": -0.1875805715546105, + "angularVelocity": -0.7128270440152931, + "velocityX": 1.7126818045639527, + "velocityY": -0.9513210214242486, + "timestamp": 0.4508764083677503 + }, + { + "x": 1.0338457421573333, + "y": 3.7875168933266514, + "heading": -0.2500489994644535, + "angularVelocity": -0.8312933666588067, + "velocityX": 1.998141841514319, + "velocityY": -1.1098444731404116, + "timestamp": 0.526022476429042 + }, + { + "x": 1.2054498603433927, + "y": 3.692204931210594, + "heading": -0.32141344956061546, + "angularVelocity": -0.9496764360040055, + "velocityX": 2.2836074143771987, + "velocityY": -1.2683559442966215, + "timestamp": 0.6011685444903337 + }, + { + "x": 1.3985060453585743, + "y": 3.5849824250394744, + "heading": -0.4016698579067676, + "angularVelocity": -1.0680054248572561, + "velocityX": 2.5690789950276347, + "velocityY": -1.4268545106533708, + "timestamp": 0.6763146125516255 + }, + { + "x": 1.5915937570064054, + "y": 3.477819815501255, + "heading": -0.48176859261605665, + "angularVelocity": -1.0659071961550632, + "velocityX": 2.5694985330482245, + "velocityY": -1.426057441233171, + "timestamp": 0.7514606806129173 + }, + { + "x": 1.7632284859071354, + "y": 3.3825660365490977, + "heading": -0.5529765476421505, + "angularVelocity": -0.9475938909806194, + "velocityX": 2.2840147638960824, + "velocityY": -1.2675816767214638, + "timestamp": 0.826606748674209 + }, + { + "x": 1.9134098098681451, + "y": 3.2992204007592485, + "heading": -0.6152918417750989, + "angularVelocity": -0.8292555517624935, + "velocityX": 1.9985253764510553, + "velocityY": -1.1091150600437196, + "timestamp": 0.9017528167355008 + }, + { + "x": 2.042137357772617, + "y": 3.227782305964734, + "heading": -0.6687115003563293, + "angularVelocity": -0.7108776275248302, + "velocityX": 1.7130310504001012, + "velocityY": -0.95065645665249, + "timestamp": 0.9768988847967925 + }, + { + "x": 2.1494108084908032, + "y": 3.168251239438324, + "heading": -0.7132320417999736, + "angularVelocity": -0.5924533723751417, + "velocityX": 1.4275324509419445, + "velocityY": -0.7922046763358841, + "timestamp": 1.0520449528580842 + }, + { + "x": 2.2352298936741635, + "y": 3.120626784366874, + "heading": -0.7488501306754739, + "angularVelocity": -0.47398473126297086, + "velocityX": 1.1420302804580957, + "velocityY": -0.6337584427252518, + "timestamp": 1.1271910209193758 + }, + { + "x": 2.299594398786613, + "y": 3.0849086278606843, + "heading": -0.7755632624576652, + "angularVelocity": -0.3554827613921603, + "velocityX": 0.8565252550532846, + "velocityY": -0.475316372868058, + "timestamp": 1.2023370889806675 + }, + { + "x": 2.3425041622841327, + "y": 3.061096568391248, + "heading": -0.7933703919070813, + "angularVelocity": -0.23696688208479472, + "velocityX": 0.5710180799149875, + "velocityY": -0.31687698483458104, + "timestamp": 1.2774831570419591 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288571271456, + "velocityX": 0.28550943901354336, + "velocityY": -0.15843872418317836, + "timestamp": 1.3526292251032508 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -1.4441811570154594e-25, + "velocityX": -6.270976814566572e-25, + "velocityY": -1.3160668498175877e-24, + "timestamp": 1.4277752931645424 + }, + { + "x": 2.3773546784477104, + "y": 3.0407954087369538, + "heading": -0.78607704068416, + "angularVelocity": 0.26500042843772503, + "velocityX": 0.21918860936679116, + "velocityY": -0.13736692846187587, + "timestamp": 1.4888898030198798 + }, + { + "x": 2.4041715280717404, + "y": 3.023997545091338, + "heading": -0.7541197912958612, + "angularVelocity": 0.5229077262329992, + "velocityX": 0.4387967716260398, + "velocityY": -0.27485884588418563, + "timestamp": 1.5500043128752172 + }, + { + "x": 2.444438816560299, + "y": 2.9987884456278273, + "heading": -0.7069088629247025, + "angularVelocity": 0.772499501066286, + "velocityX": 0.6588826218826678, + "velocityY": -0.41248959573074695, + "timestamp": 1.6111188227305546 + }, + { + "x": 2.498190797810618, + "y": 2.965157151416224, + "heading": -0.6450804742614368, + "angularVelocity": 1.0116810035721142, + "velocityX": 0.8795289592856741, + "velocityY": -0.5502996635530664, + "timestamp": 1.672233332585892 + }, + { + "x": 2.565467715954461, + "y": 2.92308800675928, + "heading": -0.5694581744936127, + "angularVelocity": 1.2373869960967963, + "velocityX": 1.1008338004034173, + "velocityY": -0.6883659012650978, + "timestamp": 1.7333478424412294 + }, + { + "x": 2.6463157744818333, + "y": 2.8725583790255045, + "heading": -0.4811095208345568, + "angularVelocity": 1.4456248420904245, + "velocityX": 1.3228946565839355, + "velocityY": -0.8268024705325063, + "timestamp": 1.7944623522965668 + }, + { + "x": 2.7407862852871125, + "y": 2.8135375953698065, + "heading": -0.3813947607246218, + "angularVelocity": 1.6316053314665737, + "velocityX": 1.5457951152500125, + "velocityY": -0.9657409311700984, + "timestamp": 1.8555768621519042 + }, + { + "x": 2.8489350075595548, + "y": 2.74598863201195, + "heading": -0.27202744081575525, + "angularVelocity": 1.7895475259107383, + "velocityX": 1.7696079462706686, + "velocityY": -1.1052852017916852, + "timestamp": 1.9166913720072416 + }, + { + "x": 2.97082263377999, + "y": 2.6698730886283255, + "heading": -0.15523118772045078, + "angularVelocity": 1.9111051266183738, + "velocityX": 1.994413871745882, + "velocityY": -1.2454578063997521, + "timestamp": 1.977805881862579 + }, + { + "x": 3.1065122610280413, + "y": 2.5851587283166806, + "heading": -0.034196755068831956, + "angularVelocity": 1.9804532988666075, + "velocityX": 2.220252237467639, + "velocityY": -1.3861578946173312, + "timestamp": 2.0389203917179164 + }, + { + "x": 3.2560356540344637, + "y": 2.4918371862270576, + "heading": 0.08572827567170235, + "angularVelocity": 1.9623004589974744, + "velocityX": 2.4466103607859297, + "velocityY": -1.5269948545856247, + "timestamp": 2.100034901573254 + }, + { + "x": 3.419180856935893, + "y": 2.390029020511174, + "heading": 0.1941501841545546, + "angularVelocity": 1.7740780174707271, + "velocityX": 2.669500308316405, + "velocityY": -1.6658591545096526, + "timestamp": 2.161149411428591 + }, + { + "x": 3.5929644614197396, + "y": 2.2794553379158704, + "heading": 0.2670576621771365, + "angularVelocity": 1.1929651108248978, + "velocityX": 2.843573562075607, + "velocityY": -1.8092869084124177, + "timestamp": 2.2222639212839286 + }, + { + "x": 3.7768655447565394, + "y": 2.1600020217160374, + "heading": 0.3015223680601122, + "angularVelocity": 0.5639365506580378, + "velocityX": 3.009123099769713, + "velocityY": -1.9545819230586587, + "timestamp": 2.283378431139266 + }, + { + "x": 3.972896711212453, + "y": 2.037552262177189, + "heading": 0.301522415266864, + "angularVelocity": 7.724311622068364e-7, + "velocityX": 3.2076043302962565, + "velocityY": -2.003611905400141, + "timestamp": 2.3444929409946034 + }, + { + "x": 4.174386731425729, + "y": 1.9243094542990702, + "heading": 0.30152246246678105, + "angularVelocity": 7.723193244877141e-7, + "velocityX": 3.29692606044323, + "velocityY": -1.8529610749750456, + "timestamp": 2.4056074508499408 + }, + { + "x": 4.383354455500062, + "y": 1.8255438133936408, + "heading": 0.30152251049528017, + "angularVelocity": 7.858771882784632e-7, + "velocityX": 3.419281682352931, + "velocityY": -1.6160751536617886, + "timestamp": 2.466721960705278 + }, + { + "x": 4.5987615422573755, + "y": 1.741746749013425, + "heading": 0.30152256055333204, + "angularVelocity": 8.190862036393596e-7, + "velocityX": 3.52464721172105, + "velocityY": -1.3711484323210577, + "timestamp": 2.5278364705606156 + }, + { + "x": 4.819537392304322, + "y": 1.6733348767579859, + "heading": 0.30152261408094844, + "angularVelocity": 8.758577384965909e-7, + "velocityX": 3.61249481619895, + "velocityY": -1.1194047439368322, + "timestamp": 2.588950980415953 + }, + { + "x": 5.0445846783405734, + "y": 1.620648273878739, + "heading": 0.30152267296096713, + "angularVelocity": 9.634376323324065e-7, + "velocityX": 3.682387154359157, + "velocityY": -0.8620964645541634, + "timestamp": 2.6500654902712903 + }, + { + "x": 5.272784828780489, + "y": 1.5839488354188542, + "heading": 0.3015227398607896, + "angularVelocity": 0.0000010946634868711088, + "velocityX": 3.733976611774902, + "velocityY": -0.6005028682510115, + "timestamp": 2.7111800001266277 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.30152281374195866, + "angularVelocity": 0.0000012088973518937203, + "velocityX": 3.7670067063284516, + "velocityY": -0.33592433375155795, + "timestamp": 2.772294509981965 + }, + { + "x": 5.642161233112151, + "y": 1.5569512201034212, + "heading": 0.3015228937182471, + "angularVelocity": 0.0000021712104790563405, + "velocityX": 3.7778762073788132, + "velocityY": -0.17558801391627427, + "timestamp": 2.8091293950374445 + }, + { + "x": 5.781468008752501, + "y": 1.5564011001804379, + "heading": 0.3015229674749775, + "angularVelocity": 0.0000020023608117977538, + "velocityX": 3.7819250808175866, + "velocityY": -0.01493475335010743, + "timestamp": 2.845964280092924 + }, + { + "x": 5.920672391044722, + "y": 1.5617696155329066, + "heading": 0.3015230362484881, + "angularVelocity": 0.00000186707547817187, + "velocityX": 3.7791452880212755, + "velocityY": 0.14574540803868186, + "timestamp": 2.8827991651484033 + }, + { + "x": 6.059523032201408, + "y": 1.573047069761596, + "heading": 0.3015231010202909, + "angularVelocity": 0.0000017584364021609749, + "velocityX": 3.769541860862453, + "velocityY": 0.3061623298594123, + "timestamp": 2.9196340502038827 + }, + { + "x": 6.197769223699856, + "y": 1.5902130966223555, + "heading": 0.3015231625861422, + "angularVelocity": 0.0000016714006640348176, + "velocityX": 3.753132154212656, + "velocityY": 0.46602634526766634, + "timestamp": 2.956468935259362 + }, + { + "x": 6.335161349065868, + "y": 1.6132366967697473, + "heading": 0.30152322160724626, + "angularVelocity": 0.0000016023154129591626, + "velocityX": 3.729945815198736, + "velocityY": 0.6250487849416221, + "timestamp": 2.9933038203148414 + }, + { + "x": 6.471451334685286, + "y": 1.642076293800604, + "heading": 0.30152327864702066, + "angularVelocity": 0.0000015485259219178396, + "velocityX": 3.700024729658922, + "velocityY": 0.7829425010399578, + "timestamp": 3.030138705370321 + }, + { + "x": 6.606393097769013, + "y": 1.6766798096014064, + "heading": 0.30152333422462596, + "angularVelocity": 0.0000015088306981556567, + "velocityX": 3.6634229448655047, + "velocityY": 0.9394223912653392, + "timestamp": 3.0669735904258 + }, + { + "x": 6.739742462814537, + "y": 1.7169846115821055, + "heading": 0.301525563266411, + "angularVelocity": 0.000060514422176811634, + "velocityX": 3.62019224017336, + "velocityY": 1.094201920814829, + "timestamp": 3.1038084754812796 + }, + { + "x": 6.86942078984101, + "y": 1.7614703146147381, + "heading": 0.31012052274281793, + "angularVelocity": 0.23333748601255083, + "velocityX": 3.520530248191535, + "velocityY": 1.2077057649461913, + "timestamp": 3.140643360536759 + }, + { + "x": 6.994764102260858, + "y": 1.8086931054779125, + "heading": 0.330618059674586, + "angularVelocity": 0.5564707722284291, + "velocityX": 3.4028425019125397, + "velocityY": 1.2820127113753435, + "timestamp": 3.1774782455922383 + }, + { + "x": 7.114597005035733, + "y": 1.8565129182538003, + "heading": 0.35962770978990066, + "angularVelocity": 0.7875591323719712, + "velocityX": 3.2532449224257225, + "velocityY": 1.2982207682707054, + "timestamp": 3.2143131306477177 + }, + { + "x": 7.228563598521064, + "y": 1.9039637598434371, + "heading": 0.3914629491729737, + "angularVelocity": 0.8642687315332709, + "velocityX": 3.093985316193586, + "velocityY": 1.2882038729907241, + "timestamp": 3.251148015703197 + }, + { + "x": 7.3366766140353254, + "y": 1.9505339209655184, + "heading": 0.42341393018553436, + "angularVelocity": 0.8674109058420351, + "velocityX": 2.935071342055907, + "velocityY": 1.2642950032812363, + "timestamp": 3.2879829007586765 + }, + { + "x": 7.4389964644321305, + "y": 1.9959395099807475, + "heading": 0.45395278398085526, + "angularVelocity": 0.8290742254068196, + "velocityX": 2.777797466795224, + "velocityY": 1.232678992939458, + "timestamp": 3.324817785814156 + }, + { + "x": 7.535582073900071, + "y": 2.0400031621127974, + "heading": 0.48208291398817593, + "angularVelocity": 0.7636817643098928, + "velocityX": 2.6221232758692414, + "velocityY": 1.1962478521565303, + "timestamp": 3.3616526708696353 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.6791276901362222, + "velocityX": 2.467819383699554, + "velocityY": 1.1565397222936757, + "timestamp": 3.3984875559251146 + }, + { + "x": 7.766333854980875, + "y": 2.1513911603861824, + "heading": 0.5393931722207971, + "angularVelocity": 0.5089507410927826, + "velocityX": 2.2039777536941085, + "velocityY": 1.084054804314438, + "timestamp": 3.4619409812195143 + }, + { + "x": 7.889716821195031, + "y": 2.214535023307618, + "heading": 0.562169089136497, + "angularVelocity": 0.35893912440547393, + "velocityX": 1.9444650252008764, + "velocityY": 0.9951214237603884, + "timestamp": 3.525394406513914 + }, + { + "x": 7.996856912790919, + "y": 2.2713427786511677, + "heading": 0.5763089078520277, + "angularVelocity": 0.22283775304370615, + "velocityX": 1.6884839722804246, + "velocityY": 0.8952669628153768, + "timestamp": 3.5888478318083137 + }, + { + "x": 8.087934078342826, + "y": 2.32132355324964, + "heading": 0.5824533262443143, + "angularVelocity": 0.09683351787202944, + "velocityX": 1.4353388352062089, + "velocityY": 0.7876765417561041, + "timestamp": 3.6523012571027134 + }, + { + "x": 8.163093938645574, + "y": 2.3641124623938485, + "heading": 0.5810872040851076, + "angularVelocity": -0.021529525835184894, + "velocityX": 1.184488622230158, + "velocityY": 0.6743356870915145, + "timestamp": 3.715754682397113 + }, + { + "x": 8.222455939868006, + "y": 2.399428067413051, + "heading": 0.5725898823037127, + "angularVelocity": -0.13391431182746497, + "velocityX": 0.9355208319647886, + "velocityY": 0.5565594742183169, + "timestamp": 3.7792081076915127 + }, + { + "x": 8.2661194627332, + "y": 2.427046891477821, + "heading": 0.5572661390575091, + "angularVelocity": -0.24149591885871216, + "velocityX": 0.6881192412011335, + "velocityY": 0.4352613580217183, + "timestamp": 3.8426615329859124 + }, + { + "x": 8.294168308225482, + "y": 2.446787281552408, + "heading": 0.535366098523775, + "angularVelocity": -0.3451356712758413, + "velocityX": 0.44203831963592227, + "velocityY": 0.31110046436420713, + "timestamp": 3.906114958280312 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.4454857086167899, + "velocityX": 0.19708463834018525, + "velocityY": 0.18456741693022066, + "timestamp": 3.9695683835747118 + }, + { + "x": 8.303024530421867, + "y": 2.461881127507041, + "heading": 0.4711910316636515, + "angularVelocity": -0.5465136629689635, + "velocityX": -0.05554517774464489, + "velocityY": 0.05148047936095934, + "timestamp": 4.03527117412091 + }, + { + "x": 8.282750419128863, + "y": 2.4565585196128312, + "heading": 0.42887568027171213, + "angularVelocity": -0.644041920292346, + "velocityX": -0.3085730624903528, + "velocityY": -0.0810103779453237, + "timestamp": 4.100973964667109 + }, + { + "x": 8.245821395574854, + "y": 2.44257692607346, + "heading": 0.38041954518186105, + "angularVelocity": -0.7375049779016557, + "velocityX": -0.5620617213821911, + "velocityY": -0.2128006044056815, + "timestamp": 4.166676755213307 + }, + { + "x": 8.19220201373393, + "y": 2.41999124764574, + "heading": 0.3261364097301619, + "angularVelocity": -0.8261922362875965, + "velocityX": -0.8160898706916083, + "velocityY": -0.3437552383994849, + "timestamp": 4.232379545759505 + }, + { + "x": 8.12185021393602, + "y": 2.3888680897109964, + "heading": 0.26640039291910717, + "angularVelocity": -0.9091853833674209, + "velocityX": -1.070758170437869, + "velocityY": -0.4736961349131665, + "timestamp": 4.298082336305703 + }, + { + "x": 8.034715295170983, + "y": 2.349289919572908, + "heading": 0.2016664425434797, + "angularVelocity": -0.9852542005823995, + "velocityX": -1.3261981422808646, + "velocityY": -0.6023818746368115, + "timestamp": 4.3637851268519015 + }, + { + "x": 7.93073498478249, + "y": 2.3013613956679024, + "heading": 0.13250258081588168, + "angularVelocity": -1.0526776892218304, + "velocityX": -1.582585907296906, + "velocityY": -0.7294747073384187, + "timestamp": 4.4294879173981 + }, + { + "x": 7.809831081003324, + "y": 2.2452194823487295, + "heading": 0.0596429891895938, + "angularVelocity": -1.1089268967207304, + "velocityX": -1.8401639074089793, + "velocityY": -0.8544829352369329, + "timestamp": 4.495190707944298 + }, + { + "x": 7.671902779277316, + "y": 2.181050669281034, + "heading": -0.015920266023457082, + "angularVelocity": -1.1500768016834724, + "velocityX": -2.099276158278614, + "velocityY": -0.9766527804108355, + "timestamp": 4.560893498490496 + }, + { + "x": 7.516816184413173, + "y": 2.1091228610813926, + "heading": -0.09276871504476969, + "angularVelocity": -1.1696375204532188, + "velocityX": -2.360426301149195, + "velocityY": -1.09474510293541, + "timestamp": 4.626596289036694 + }, + { + "x": 7.34438786737911, + "y": 2.029851654552736, + "heading": -0.16871930466642973, + "angularVelocity": -1.1559720521802803, + "velocityX": -2.6243682437326825, + "velocityY": -1.2065120198040524, + "timestamp": 4.6922990795828925 + }, + { + "x": 7.154363406661781, + "y": 1.9439629810404226, + "heading": -0.24004853733783288, + "angularVelocity": -1.0856347512553308, + "velocityX": -2.892182495410387, + "velocityY": -1.307230222617737, + "timestamp": 4.758001870129091 + }, + { + "x": 6.946444181928406, + "y": 1.853016234879977, + "heading": -0.2992772561370167, + "angularVelocity": -0.9014642803875683, + "velocityX": -3.1645417645872604, + "velocityY": -1.3842143599136347, + "timestamp": 4.823704660675289 + }, + { + "x": 6.722123267905806, + "y": 1.762675189068849, + "heading": -0.3185532311699482, + "angularVelocity": -0.29338137501751665, + "velocityX": -3.4141763562518888, + "velocityY": -1.3749955680741657, + "timestamp": 4.889407451221487 + }, + { + "x": 6.485759743484643, + "y": 1.6860130566353695, + "heading": -0.3185533048985424, + "angularVelocity": -0.0000011221531618495692, + "velocityX": -3.597465533141494, + "velocityY": -1.166801771982201, + "timestamp": 4.955110241767685 + }, + { + "x": 6.244268768249415, + "y": 1.6274732084557726, + "heading": -0.31855333054380797, + "angularVelocity": -3.903223189148686e-7, + "velocityX": -3.675505609848113, + "velocityY": -0.890979632568803, + "timestamp": 5.0208130323138835 + }, + { + "x": 5.999037658090566, + "y": 1.5873919626414805, + "heading": -0.31855335901995885, + "angularVelocity": -4.334085455089385e-7, + "velocityX": -3.732430664210775, + "velocityY": -0.6100387134410885, + "timestamp": 5.086515822860082 + }, + { + "x": 5.751475205097474, + "y": 1.5659995874731991, + "heading": -0.31855339177165976, + "angularVelocity": -4.984826465003964e-7, + "velocityX": -3.7679138273285506, + "velocityY": -0.3255930987168572, + "timestamp": 5.15221861340628 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.31855343077931203, + "angularVelocity": -5.936985618685712e-7, + "velocityX": -3.781751212884059, + "velocityY": -0.03927691713851364, + "timestamp": 5.217921403952478 + }, + { + "x": 5.26265149115341, + "y": 1.5786031057335383, + "heading": -0.31855346389778566, + "angularVelocity": -5.200844258760406e-7, + "velocityX": -3.774430808061432, + "velocityY": 0.2384477350064305, + "timestamp": 5.28160043751504 + }, + { + "x": 5.0240625557725105, + "y": 1.6113905209820258, + "heading": -0.3185534914433618, + "angularVelocity": -4.3256900426079536e-7, + "velocityX": -3.7467424053553953, + "velocityY": 0.5148855661616695, + "timestamp": 5.345279471077602 + }, + { + "x": 4.788524292077237, + "y": 1.6616042903739598, + "heading": -0.31855351524845343, + "angularVelocity": -3.738293489638866e-7, + "velocityX": -3.698835401826056, + "velocityY": 0.7885447781270322, + "timestamp": 5.4089585046401645 + }, + { + "x": 4.557307734562994, + "y": 1.7289734302363957, + "heading": -0.31855353649931417, + "angularVelocity": -3.337183294186801e-7, + "velocityX": -3.63096838282074, + "velocityY": 1.0579485286353907, + "timestamp": 5.472637538202727 + }, + { + "x": 4.331660587990519, + "y": 1.8131343686528576, + "heading": -0.31855355601779706, + "angularVelocity": -3.06513492201463e-7, + "velocityX": -3.543507712798239, + "velocityY": 1.3216428345096847, + "timestamp": 5.536316571765289 + }, + { + "x": 4.1128004797347275, + "y": 1.9136328855171, + "heading": -0.31855357441317517, + "angularVelocity": -2.888765267316508e-7, + "velocityX": -3.4369257196840386, + "velocityY": 1.5782041787036154, + "timestamp": 5.599995605327851 + }, + { + "x": 3.9019083136875623, + "y": 2.0299264516040245, + "heading": -0.3185535921713155, + "angularVelocity": -2.7886950177034126e-7, + "velocityX": -3.3117991000911333, + "velocityY": 1.826245776369568, + "timestamp": 5.663674638890413 + }, + { + "x": 3.700120312567306, + "y": 2.1613848174245227, + "heading": -0.31855360971358787, + "angularVelocity": -2.7547956295773415e-7, + "velocityX": -3.1688295162647098, + "velocityY": 2.064390089892713, + "timestamp": 5.727353672452975 + }, + { + "x": 3.5092854089981618, + "y": 2.2879634490751353, + "heading": -0.3579155948331453, + "angularVelocity": -0.6181310066662056, + "velocityX": -2.9968247458036084, + "velocityY": 1.987759935556414, + "timestamp": 5.791032706015537 + }, + { + "x": 3.3329097587868497, + "y": 2.4050971281852065, + "heading": -0.4129449306411103, + "angularVelocity": -0.8641672577191554, + "velocityX": -2.7697601603521713, + "velocityY": 1.8394387062264796, + "timestamp": 5.854711739578099 + }, + { + "x": 3.171287728623564, + "y": 2.5124799864208414, + "heading": -0.469920470896881, + "angularVelocity": -0.8947299773291117, + "velocityX": -2.5380729122482584, + "velocityY": 1.6863141952387881, + "timestamp": 5.918390773140661 + }, + { + "x": 3.0244088088448944, + "y": 2.6100923711609356, + "heading": -0.525279921939494, + "angularVelocity": -0.8693513067880482, + "velocityX": -2.3065507053332985, + "velocityY": 1.5328810642861672, + "timestamp": 5.982069806703223 + }, + { + "x": 2.892255080791877, + "y": 2.6979336459530643, + "heading": -0.5773440276114954, + "angularVelocity": -0.817602007430762, + "velocityX": -2.075309888665035, + "velocityY": 1.379437938640018, + "timestamp": 6.0457488402657855 + }, + { + "x": 2.7748123827126636, + "y": 2.776006231552379, + "heading": -0.6251288383809169, + "angularVelocity": -0.7504010047902983, + "velocityX": -1.8442914646911481, + "velocityY": 1.226032827941903, + "timestamp": 6.109427873828348 + }, + { + "x": 2.6720699827794823, + "y": 2.8443129171584167, + "heading": -0.6679860389317067, + "angularVelocity": -0.6730190166702934, + "velocityX": -1.6134415707211014, + "velocityY": 1.0726715181525133, + "timestamp": 6.17310690739091 + }, + { + "x": 2.58401962501764, + "y": 2.9028562720875715, + "heading": -0.7054558020691595, + "angularVelocity": -0.5884160145213319, + "velocityX": -1.3827213265624843, + "velocityY": 0.9193505562806369, + "timestamp": 6.236785940953472 + }, + { + "x": 2.5106548115569836, + "y": 2.9516385308148143, + "heading": -0.7371951289783589, + "angularVelocity": -0.49842664270363046, + "velocityX": -1.152103123370717, + "velocityY": 0.7660646840583188, + "timestamp": 6.300464974516034 + }, + { + "x": 2.451970312872832, + "y": 2.9906615991432, + "heading": -0.762938657381878, + "angularVelocity": -0.4042700864520384, + "velocityX": -0.921567043358115, + "velocityY": 0.6128087401019892, + "timestamp": 6.364144008078596 + }, + { + "x": 2.4079618328973456, + "y": 3.019927088676532, + "heading": -0.7824753669485657, + "angularVelocity": -0.306799718426846, + "velocityX": -0.6910984277462318, + "velocityY": 0.45957810437841673, + "timestamp": 6.427823041641158 + }, + { + "x": 2.378625774942972, + "y": 3.0394363533387363, + "heading": -0.7956338335546038, + "angularVelocity": -0.20663734780319193, + "velocityX": -0.4606862936377973, + "velocityY": 0.30636873034571754, + "timestamp": 6.49150207520372 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.10425061521167653, + "velocityX": -0.23032229137361054, + "velocityY": 0.15317707188371738, + "timestamp": 6.555181108766282 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 1.7497535778176193e-25, + "velocityX": -1.4086622042761887e-23, + "velocityY": 7.921157765160188e-25, + "timestamp": 6.6188601423288445 + }, + { + "x": 2.3794831363952245, + "y": 3.0417249906399815, + "heading": -0.7962438376212186, + "angularVelocity": 0.09576161859408154, + "velocityX": 0.24659384653798558, + "velocityY": -0.11858712382831015, + "timestamp": 6.681814115401516 + }, + { + "x": 2.41053316098515, + "y": 3.026792718508752, + "heading": -0.7842933803747459, + "angularVelocity": 0.1898284836237042, + "velocityX": 0.4932178713181911, + "velocityY": -0.23719348283216615, + "timestamp": 6.744768088474188 + }, + { + "x": 2.4571113467410304, + "y": 3.0043922677761175, + "heading": -0.766548054431847, + "angularVelocity": 0.2818777763623355, + "velocityX": 0.7398768255994334, + "velocityY": -0.3558226691550744, + "timestamp": 6.807722061546859 + }, + { + "x": 2.5192202681818534, + "y": 2.974521939997295, + "heading": -0.7431604845322959, + "angularVelocity": 0.3715026829609895, + "velocityX": 0.9865766751389508, + "velocityY": -0.4744788346295768, + "timestamp": 6.870676034619531 + }, + { + "x": 2.5968629760285724, + "y": 2.937179727170992, + "heading": -0.7143164433853058, + "angularVelocity": 0.4581766604896231, + "velocityX": 1.2333249842244594, + "velocityY": -0.593166896443484, + "timestamp": 6.933630007692202 + }, + { + "x": 2.6900431315277755, + "y": 2.8923632450565475, + "heading": -0.6802459924925017, + "angularVelocity": 0.5411961982681314, + "velocityX": 1.4801314508242465, + "velocityY": -0.7118928310165661, + "timestamp": 6.996583980764874 + }, + { + "x": 2.7987651872562824, + "y": 2.8400696406927834, + "heading": -0.6412406418470571, + "angularVelocity": 0.6195852102998923, + "velocityX": 1.72700864491909, + "velocityY": -0.8306640837965626, + "timestamp": 7.059537953837546 + }, + { + "x": 2.9230346291738636, + "y": 2.780295465724823, + "heading": -0.597681173837793, + "angularVelocity": 0.6919256384181025, + "velocityX": 1.9739729814054823, + "velocityY": -0.9494901123867101, + "timestamp": 7.122491926910217 + }, + { + "x": 3.0628582843459875, + "y": 2.713036511288724, + "heading": -0.5500859220162175, + "angularVelocity": 0.7560325345412855, + "velocityX": 2.2210457632390876, + "velocityY": -1.0683829971852359, + "timestamp": 7.185445899982889 + }, + { + "x": 3.2182446187737406, + "y": 2.638287632787952, + "heading": -0.49920268501328596, + "angularVelocity": 0.8082609328595405, + "velocityX": 2.468253024290973, + "velocityY": -1.1873576019496896, + "timestamp": 7.24839987305556 + }, + { + "x": 3.3892034703211182, + "y": 2.556042786579892, + "heading": -0.4462086227940175, + "angularVelocity": 0.8417905913276443, + "velocityX": 2.7156165560834484, + "velocityY": -1.3064282076862779, + "timestamp": 7.311353846128232 + }, + { + "x": 3.5757413262062396, + "y": 2.4662968513988717, + "heading": -0.3932458767459955, + "angularVelocity": 0.8412931458175688, + "velocityX": 2.9630831348768174, + "velocityY": -1.4255801627868938, + "timestamp": 7.3743078192009035 + }, + { + "x": 3.777810773280146, + "y": 2.3690669294508315, + "heading": -0.3455559373140241, + "angularVelocity": 0.7575366113417542, + "velocityX": 3.2097965737705954, + "velocityY": -1.5444604558286192, + "timestamp": 7.437261792273575 + }, + { + "x": 3.9925130219343306, + "y": 2.2661623700337894, + "heading": -0.34555574915683335, + "angularVelocity": 0.000002988805655755853, + "velocityX": 3.410463838499, + "velocityY": -1.6345999210924134, + "timestamp": 7.500215765346247 + }, + { + "x": 4.207067930960627, + "y": 2.1629508436660116, + "heading": -0.34555573251477295, + "angularVelocity": 2.6435282147725544e-7, + "velocityX": 3.408123404358042, + "velocityY": -1.6394759747511882, + "timestamp": 7.563169738418918 + }, + { + "x": 4.421622830783708, + "y": 2.0597392981666993, + "heading": -0.34555571587271255, + "angularVelocity": 2.643528217260083e-7, + "velocityX": 3.408123258168449, + "velocityY": -1.6394762786483632, + "timestamp": 7.62612371149159 + }, + { + "x": 4.6361777306062155, + "y": 1.956527752666195, + "heading": -0.34555569923065216, + "angularVelocity": 2.643528215615007e-7, + "velocityX": 3.408123258159341, + "velocityY": -1.6394762786672963, + "timestamp": 7.689077684564261 + }, + { + "x": 4.850732630432354, + "y": 1.85331620717324, + "heading": -0.3455556825885917, + "angularVelocity": 2.643528216339363e-7, + "velocityX": 3.4081232582170267, + "velocityY": -1.6394762785473795, + "timestamp": 7.752031657636933 + }, + { + "x": 5.065287588526139, + "y": 1.750104782806442, + "heading": -0.34555566594653137, + "angularVelocity": 2.6435282065070603e-7, + "velocityX": 3.408124183776485, + "velocityY": -1.6394743545042916, + "timestamp": 7.814985630709605 + }, + { + "x": 5.280754782593808, + "y": 1.6488115627171362, + "heading": -0.34555564929394994, + "angularVelocity": 2.645199441921782e-7, + "velocityX": 3.4226147064450507, + "velocityY": -1.6090044066381253, + "timestamp": 7.877939603782276 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 2.6733322523330606e-7, + "velocityX": 3.530338179119512, + "velocityY": -1.3564287389044698, + "timestamp": 7.940893576854948 + }, + { + "x": 5.652794667941403, + "y": 1.5139935171410897, + "heading": -0.3455556356136128, + "angularVelocity": -7.55109994192204e-8, + "velocityX": 3.591492784420599, + "velocityY": -1.1850586838463792, + "timestamp": 7.98260076650368 + }, + { + "x": 5.804789449400159, + "y": 1.47182921760144, + "heading": -0.34555563872486544, + "angularVelocity": -7.459751255727934e-8, + "velocityX": 3.644330455705477, + "velocityY": -1.0109599782379923, + "timestamp": 8.02430795615241 + }, + { + "x": 5.9583966935302906, + "y": 1.435981535401865, + "heading": -0.3455556418269747, + "angularVelocity": -7.43782867600124e-8, + "velocityX": 3.682991959512248, + "velocityY": -0.8595084564913873, + "timestamp": 8.066015145801142 + }, + { + "x": 6.112005098303726, + "y": 1.4001388269201716, + "heading": -0.34555564492908214, + "angularVelocity": -7.437824177042706e-8, + "velocityX": 3.683019787887031, + "velocityY": -0.8593892032421666, + "timestamp": 8.107722335449873 + }, + { + "x": 6.265613503315511, + "y": 1.3642961194599532, + "heading": -0.34555564803118954, + "angularVelocity": -7.437824172842392e-8, + "velocityX": 3.683019793601852, + "velocityY": -0.8593891787505857, + "timestamp": 8.149429525098604 + }, + { + "x": 6.419221908327331, + "y": 1.3284534119998854, + "heading": -0.345555651133297, + "angularVelocity": -7.437824199449142e-8, + "velocityX": 3.683019793602695, + "velocityY": -0.8593891787469733, + "timestamp": 8.191136714747335 + }, + { + "x": 6.572830313271966, + "y": 1.2926107042518886, + "heading": -0.34555565423540446, + "angularVelocity": -7.437824209379189e-8, + "velocityX": 3.6830197919918253, + "velocityY": -0.8593891856505578, + "timestamp": 8.232843904396066 + }, + { + "x": 6.726438391075464, + "y": 1.2567665945285837, + "heading": -0.3455556573375121, + "angularVelocity": -7.437824552269796e-8, + "velocityX": 3.6830119482329913, + "velocityY": -0.8594228003659058, + "timestamp": 8.274551094044797 + }, + { + "x": 6.878955915134789, + "y": 1.2165341904124123, + "heading": -0.3455556607967783, + "angularVelocity": -8.29417253790897e-8, + "velocityX": 3.6568640885148347, + "velocityY": -0.9646395370922597, + "timestamp": 8.316258283693529 + }, + { + "x": 7.025398158916378, + "y": 1.1717644348314602, + "heading": -0.3467458392299039, + "angularVelocity": -0.028536529148800397, + "velocityX": 3.5111990286317973, + "velocityY": -1.0734301677483917, + "timestamp": 8.35796547334226 + }, + { + "x": 7.164406767305733, + "y": 1.1285160001941774, + "heading": -0.3473601505437228, + "angularVelocity": -0.014729146676936172, + "velocityX": 3.3329651208849276, + "velocityY": -1.0369539401127719, + "timestamp": 8.39967266299099 + }, + { + "x": 7.296003097467406, + "y": 1.0868892243180075, + "heading": -0.34737182264197447, + "angularVelocity": -0.0002798581815252442, + "velocityX": 3.1552432870689695, + "velocityY": -0.9980719445918297, + "timestamp": 8.441379852639722 + }, + { + "x": 7.4201946902403915, + "y": 1.0469180189360368, + "heading": -0.3467718237478745, + "angularVelocity": 0.014385982348686153, + "velocityX": 2.9777022575473837, + "velocityY": -0.958376858249577, + "timestamp": 8.483087042288453 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.02916023097831232, + "velocityX": 2.8002535793496506, + "velocityY": -0.918273122155384, + "timestamp": 8.524794231937184 + }, + { + "x": 7.694109553786587, + "y": 0.9554552205250534, + "heading": -0.3423846615894022, + "angularVelocity": 0.051196248581704275, + "velocityX": 2.536815281233286, + "velocityY": -0.8583516129975453, + "timestamp": 8.586731794995119 + }, + { + "x": 7.835030505907911, + "y": 0.9064832909135739, + "heading": -0.3388644821779997, + "angularVelocity": 0.056834322140020786, + "velocityX": 2.2752098268623953, + "velocityY": -0.7906660706956153, + "timestamp": 8.648669358053054 + }, + { + "x": 7.959827165183218, + "y": 0.8619979100061141, + "heading": -0.33562038589869275, + "angularVelocity": 0.052376879540328165, + "velocityX": 2.014878421331732, + "velocityY": -0.7182294347914383, + "timestamp": 8.710606921110989 + }, + { + "x": 8.068556862392501, + "y": 0.8221974820041511, + "heading": -0.33307610796483417, + "angularVelocity": 0.04107810847318481, + "velocityX": 1.7554726379463939, + "velocityY": -0.642589505252806, + "timestamp": 8.772544484168924 + }, + { + "x": 8.161262931310752, + "y": 0.7872247537071302, + "heading": -0.33153778511771786, + "angularVelocity": 0.024836670530246144, + "velocityX": 1.4967664909827672, + "velocityY": -0.5646448870503377, + "timestamp": 8.834482047226858 + }, + { + "x": 8.237979201929296, + "y": 0.7571873231462991, + "heading": -0.3312369649013586, + "angularVelocity": 0.004856830031849059, + "velocityX": 1.2386065390849326, + "velocityY": -0.4849630672865623, + "timestamp": 8.896419610284793 + }, + { + "x": 8.298732785621018, + "y": 0.7321691840482208, + "heading": -0.33235491812017326, + "angularVelocity": -0.018049680413951164, + "velocityX": 0.9808843082007279, + "velocityY": -0.40392514433731974, + "timestamp": 8.958357173342728 + }, + { + "x": 8.34354588005975, + "y": 0.7122377172112425, + "heading": -0.33503742038505996, + "angularVelocity": -0.043309780566883505, + "velocityX": 0.7235204652274613, + "velocityY": -0.3217993387685419, + "timestamp": 9.020294736400663 + }, + { + "x": 8.37243698558101, + "y": 0.697448170361649, + "heading": -0.33940425947903785, + "angularVelocity": -0.07050388937474399, + "velocityX": 0.4664553155608451, + "velocityY": -0.2387815425634291, + "timestamp": 9.082232299458598 + }, + { + "x": 8.385421752929688, + "y": 0.6878466606140137, + "heading": -0.34555563246426124, + "angularVelocity": -0.0993157089417533, + "velocityX": 0.20964285173008823, + "velocityY": -0.15501917210811605, + "timestamp": 9.144169862516533 + }, + { + "x": 8.381221613409881, + "y": 0.6835499744193162, + "heading": -0.3542287504471472, + "angularVelocity": -0.13146726396270184, + "velocityX": -0.06366578340339453, + "velocityY": -0.06512923947740829, + "timestamp": 9.210141555556 + }, + { + "x": 8.358990441245284, + "y": 0.6851839703239002, + "heading": -0.3649454511800031, + "angularVelocity": -0.16244392464575347, + "velocityX": -0.3369804705678509, + "velocityY": 0.024768136594682598, + "timestamp": 9.276113248595466 + }, + { + "x": 8.31872779484583, + "y": 0.6927492166918153, + "heading": -0.3776152319611175, + "angularVelocity": -0.19204874389891546, + "velocityX": -0.6103018513617493, + "velocityY": 0.11467412793831581, + "timestamp": 9.342084941634933 + }, + { + "x": 8.26043318618489, + "y": 0.7062463795602301, + "heading": -0.3921309983002572, + "angularVelocity": -0.2200302231209342, + "velocityX": -0.8836306296711994, + "velocityY": 0.20459021508422195, + "timestamp": 9.4080566346744 + }, + { + "x": 8.184106078073826, + "y": 0.72567624991864, + "heading": -0.40836398748744607, + "angularVelocity": -0.24605991508324035, + "velocityX": -1.1569675506948445, + "velocityY": 0.294518292061811, + "timestamp": 9.474028327713867 + }, + { + "x": 8.089745885570975, + "y": 0.7510397809748904, + "heading": -0.426156376289344, + "angularVelocity": -0.26969731989830587, + "velocityX": -1.430313338273756, + "velocityY": 0.3844608177794827, + "timestamp": 9.540000020753334 + }, + { + "x": 7.9773519881013435, + "y": 0.7823381401243138, + "heading": -0.4453100949140704, + "angularVelocity": -0.2903323795747961, + "velocityX": -1.7036685325385499, + "velocityY": 0.47442103889465864, + "timestamp": 9.605971713792801 + }, + { + "x": 7.846923768615211, + "y": 0.8195727828481538, + "heading": -0.46556907619150867, + "angularVelocity": -0.3070859689066713, + "velocityX": -1.9770330800530667, + "velocityY": 0.5644033222182843, + "timestamp": 9.671943406832268 + }, + { + "x": 7.698460723132396, + "y": 0.8627455590379989, + "heading": -0.48658932670817834, + "angularVelocity": -0.3186253004617381, + "velocityX": -2.2504052669074097, + "velocityY": 0.6544136462288057, + "timestamp": 9.737915099871735 + }, + { + "x": 7.531962767971369, + "y": 0.9118588631154407, + "heading": -0.50788425226007, + "angularVelocity": -0.3227888291293674, + "velocityX": -2.52377872220773, + "velocityY": 0.7444602649208999, + "timestamp": 9.803886792911202 + }, + { + "x": 7.347431178555123, + "y": 0.9669158145351346, + "heading": -0.528713009661178, + "angularVelocity": -0.31572264469016176, + "velocityX": -2.79713284462553, + "velocityY": 0.8345541683575805, + "timestamp": 9.869858485950669 + }, + { + "x": 7.144872018682828, + "y": 1.0279202336148925, + "heading": -0.5478112276567515, + "angularVelocity": -0.28949110013211704, + "velocityX": -3.070395051876507, + "velocityY": 0.9247059802340185, + "timestamp": 9.935830178990136 + }, + { + "x": 6.9243138547651855, + "y": 1.094874091293601, + "heading": -0.5625298674499013, + "angularVelocity": -0.22310538224848228, + "velocityX": -3.343224249007752, + "velocityY": 1.0148876676342686, + "timestamp": 10.001801872029603 + }, + { + "x": 6.6860270471357435, + "y": 1.1677363105602094, + "heading": -0.5635939187210077, + "angularVelocity": -0.016128906536775855, + "velocityX": -3.6119553197898044, + "velocityY": 1.1044467090304817, + "timestamp": 10.06777356506907 + }, + { + "x": 6.44899650553119, + "y": 1.2456322116060934, + "heading": -0.5635939252708948, + "angularVelocity": -9.928329560968507e-8, + "velocityX": -3.59291281887752, + "velocityY": 1.1807473396094794, + "timestamp": 10.133745258108537 + }, + { + "x": 6.2119660599131885, + "y": 1.3235284047302205, + "heading": -0.5635939318207752, + "angularVelocity": -9.928319425999079e-8, + "velocityX": -3.592911363911807, + "velocityY": 1.1807517669362584, + "timestamp": 10.199716951148003 + }, + { + "x": 5.974935614307485, + "y": 1.4014245978917717, + "heading": -0.5635939383706557, + "angularVelocity": -9.928319467077491e-8, + "velocityX": -3.5929113637253804, + "velocityY": 1.1807517675035346, + "timestamp": 10.26568864418747 + }, + { + "x": 5.737905296446339, + "y": 1.4793211797663546, + "heading": -0.5635939449205359, + "angularVelocity": -9.928319168961564e-8, + "velocityX": -3.5929094273712967, + "velocityY": 1.1807576596220166, + "timestamp": 10.331660337226937 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.563593951480198, + "angularVelocity": -9.943146555012959e-8, + "velocityX": -3.5606437907587183, + "velocityY": 1.2747558956305969, + "timestamp": 10.397632030266404 + }, + { + "x": 5.285962555889255, + "y": 1.6593525613363396, + "heading": -0.5635939570041091, + "angularVelocity": -8.80379800734208e-8, + "velocityX": -3.4591170542763976, + "velocityY": 1.5289526345693891, + "timestamp": 10.460376666531877 + }, + { + "x": 5.071426961149029, + "y": 1.7607652993502259, + "heading": -0.5635939625107921, + "angularVelocity": -8.776340606411008e-8, + "velocityX": -3.419186204738301, + "velocityY": 1.6162774071206454, + "timestamp": 10.52312130279735 + }, + { + "x": 4.856891538791384, + "y": 1.862178402033475, + "heading": -0.5635939680174745, + "angularVelocity": -8.776339871909346e-8, + "velocityX": -3.4191834573706834, + "velocityY": 1.6162832190813905, + "timestamp": 10.585865939062822 + }, + { + "x": 4.642356116443861, + "y": 1.9635915047381367, + "heading": -0.5635939735241571, + "angularVelocity": -8.776339868323175e-8, + "velocityX": -3.4191834572093653, + "velocityY": 1.6162832194226513, + "timestamp": 10.648610575328295 + }, + { + "x": 4.427820694096338, + "y": 2.0650046074427992, + "heading": -0.5635939790308396, + "angularVelocity": -8.776339874733319e-8, + "velocityX": -3.4191834572093556, + "velocityY": 1.6162832194226713, + "timestamp": 10.711355211593768 + }, + { + "x": 4.213285271748816, + "y": 2.166417710147462, + "heading": -0.5635939845375221, + "angularVelocity": -8.776339865248036e-8, + "velocityX": -3.419183457209353, + "velocityY": 1.616283219422678, + "timestamp": 10.77409984785924 + }, + { + "x": 3.9987498494044535, + "y": 2.2678308128588087, + "heading": -0.5635939900442046, + "angularVelocity": -8.776339914853593e-8, + "velocityX": -3.4191834571589985, + "velocityY": 1.6162832195291985, + "timestamp": 10.836844484124713 + }, + { + "x": 3.784214480868839, + "y": 2.3692440293975783, + "heading": -0.5635939955531623, + "angularVelocity": -8.779965948091874e-8, + "velocityX": -3.419182599575763, + "velocityY": 1.6162850336670995, + "timestamp": 10.899589120390186 + }, + { + "x": 3.5824862821613, + "y": 2.464592718536819, + "heading": -0.5976475454093522, + "angularVelocity": -0.5427324450827832, + "velocityX": -3.2150668282469037, + "velocityY": 1.519630916909295, + "timestamp": 10.962333756655658 + }, + { + "x": 3.3962755579097226, + "y": 2.5526066544868016, + "heading": -0.6290952782619788, + "angularVelocity": -0.5012019309438841, + "velocityX": -2.967755259010819, + "velocityY": 1.4027324276388398, + "timestamp": 11.025078392921131 + }, + { + "x": 3.2255823355920983, + "y": 2.6332858939778268, + "heading": -0.6579338280606744, + "angularVelocity": -0.45961776998243625, + "velocityX": -2.7204432518410133, + "velocityY": 1.2858348425142074, + "timestamp": 11.087823029186604 + }, + { + "x": 3.070406633071231, + "y": 2.70663049336622, + "heading": -0.6841601724081167, + "angularVelocity": -0.41798543920915443, + "velocityX": -2.4731309599806814, + "velocityY": 1.1689381555751166, + "timestamp": 11.150567665452076 + }, + { + "x": 2.930748464054276, + "y": 2.772640503904357, + "heading": -0.7077714566039787, + "angularVelocity": -0.3763076113145772, + "velocityX": -2.225818449660962, + "velocityY": 1.0520422854767753, + "timestamp": 11.213312301717549 + }, + { + "x": 2.8066078395848333, + "y": 2.8313159708307802, + "heading": -0.7287650312356323, + "angularVelocity": -0.33458755809548013, + "velocityX": -1.9785057633325538, + "velocityY": 0.9351471363730166, + "timestamp": 11.276056937983022 + }, + { + "x": 2.697984768845601, + "y": 2.8826569331257605, + "heading": -0.7471385161808816, + "angularVelocity": -0.2928295713997149, + "velocityX": -1.731192930653829, + "velocityY": 0.8182526085218894, + "timestamp": 11.338801574248494 + }, + { + "x": 2.6048792597285715, + "y": 2.926663423472239, + "heading": -0.7628898638650828, + "angularVelocity": -0.2510389512428944, + "velocityX": -1.483879972195546, + "velocityY": 0.7013586015589744, + "timestamp": 11.401546210513967 + }, + { + "x": 2.5272913192824404, + "y": 2.9633354682979096, + "heading": -0.7760174151917997, + "angularVelocity": -0.20922188904202302, + "velocityX": -1.236566901397856, + "velocityY": 0.5844650157905298, + "timestamp": 11.46429084677944 + }, + { + "x": 2.465220954071045, + "y": 2.992673087849862, + "heading": -0.7865199462233702, + "angularVelocity": -0.16738532019110303, + "velocityX": -0.9892537259882359, + "velocityY": 0.4675717527124547, + "timestamp": 11.527035483044912 + }, + { + "x": 2.4186681704546857, + "y": 3.014676296277141, + "heading": -0.7943967050876205, + "angularVelocity": -0.12553676828922553, + "velocityX": -0.7419404492105731, + "velocityY": 0.35067871513643956, + "timestamp": 11.589780119310385 + }, + { + "x": 2.3876329747987564, + "y": 3.0293451017064936, + "heading": -0.7996474390669244, + "angularVelocity": -0.08368418867053315, + "velocityX": -0.4946270709837142, + "velocityY": 0.23378580708140337, + "timestamp": 11.652524755575858 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": -0.04183581368700928, + "velocityX": -0.2473135890317597, + "velocityY": 0.11689293351474017, + "timestamp": 11.71526939184133 + }, + { + "x": 2.37211537361145, + "y": 3.03667950630188, + "heading": -0.8022724119795859, + "angularVelocity": 8.260301472108595e-27, + "velocityX": 2.4653400544228422e-26, + "velocityY": -1.932180439483193e-25, + "timestamp": 11.778014028106803 + }, + { + "x": 2.3862784269914368, + "y": 3.0299921206081337, + "heading": -0.7979445021330078, + "angularVelocity": 0.07214760810661107, + "velocityX": 0.23610252086471115, + "velocityY": -0.11148080699315335, + "timestamp": 11.838000907230818 + }, + { + "x": 2.4146057122945543, + "y": 3.016617241069024, + "heading": -0.7893555779678016, + "angularVelocity": 0.14318004688074568, + "velocityX": 0.47222468841152654, + "velocityY": -0.22296341690754115, + "timestamp": 11.897987786354832 + }, + { + "x": 2.4570985950819932, + "y": 2.9965547338415663, + "heading": -0.7765849361695806, + "angularVelocity": 0.21289058515312126, + "velocityX": 0.7083696202896328, + "velocityY": -0.3344482580262399, + "timestamp": 11.957974665478847 + }, + { + "x": 2.5137586742181073, + "y": 2.9698044451299115, + "heading": -0.7597273140396543, + "angularVelocity": 0.28102182304025686, + "velocityX": 0.9445412057356202, + "velocityY": -0.445935662969764, + "timestamp": 12.017961544602862 + }, + { + "x": 2.5845878441848784, + "y": 2.9363662103913186, + "heading": -0.7388971765018356, + "angularVelocity": 0.34724489491702143, + "velocityX": 1.1807443727876086, + "velocityY": -0.5574258108921492, + "timestamp": 12.077948423726877 + }, + { + "x": 2.669588380905788, + "y": 2.8962398697740044, + "heading": -0.7142349758160352, + "angularVelocity": 0.41112658377867206, + "velocityX": 1.416985480194453, + "velocityY": -0.6689186235936444, + "timestamp": 12.137935302850892 + }, + { + "x": 2.7687630627135538, + "y": 2.8494252954239245, + "heading": -0.6859166587732123, + "angularVelocity": 0.47207518471295223, + "velocityX": 1.65327290327499, + "velocityY": -0.7804135676619703, + "timestamp": 12.197922181974906 + }, + { + "x": 2.8821153447700505, + "y": 2.7959224429728664, + "heading": -0.6541688089081689, + "angularVelocity": 0.5292465673936607, + "velocityX": 1.8896179249824876, + "velocityY": -0.8919092513622485, + "timestamp": 12.257909061098921 + }, + { + "x": 3.00964961510041, + "y": 2.7357314565429642, + "heading": -0.6192942625032292, + "angularVelocity": 0.5813695747171842, + "velocityX": 2.126036096438681, + "velocityY": -1.00340253250157, + "timestamp": 12.317895940222936 + }, + { + "x": 3.151371568939876, + "y": 2.668852905662324, + "heading": -0.5817190532214994, + "angularVelocity": 0.6263904678896166, + "velocityX": 2.3625492092441416, + "velocityY": -1.114886319429574, + "timestamp": 12.37788281934695 + }, + { + "x": 3.307288700965784, + "y": 2.595288399732038, + "heading": -0.5420886206685631, + "angularVelocity": 0.6606516813619456, + "velocityX": 2.5991872606602855, + "velocityY": -1.2263432771390068, + "timestamp": 12.437869698470966 + }, + { + "x": 3.477410464476997, + "y": 2.5150425548921422, + "heading": -0.5015011072339337, + "angularVelocity": 0.6766065184141348, + "velocityX": 2.8359829015193223, + "velocityY": -1.3377232823531002, + "timestamp": 12.49785657759498 + }, + { + "x": 3.661742704023134, + "y": 2.428132077104721, + "heading": -0.46226256504462326, + "angularVelocity": 0.6541187466710837, + "velocityX": 3.072875972844897, + "velocityY": -1.44882479396444, + "timestamp": 12.557843456718995 + }, + { + "x": 3.8601464032626667, + "y": 2.3346891273821493, + "heading": -0.4327535625874362, + "angularVelocity": 0.4919242822448071, + "velocityX": 3.307451598363016, + "velocityY": -1.55772314024523, + "timestamp": 12.61783033584301 + }, + { + "x": 4.065242172431025, + "y": 2.2377114042955792, + "heading": -0.4327535491582794, + "angularVelocity": 2.2386823508713805e-7, + "velocityX": 3.4190104930171277, + "velocityY": -1.6166489156083852, + "timestamp": 12.677817214967025 + }, + { + "x": 4.270337923318267, + "y": 2.140733642546332, + "heading": -0.4327535357295662, + "angularVelocity": 2.238608404824225e-7, + "velocityX": 3.4190101882652333, + "velocityY": -1.6166495601272812, + "timestamp": 12.73780409409104 + }, + { + "x": 4.475433674204211, + "y": 2.043755880794341, + "heading": -0.43275352230085307, + "angularVelocity": 2.2386084074280025e-7, + "velocityX": 3.4190101882436053, + "velocityY": -1.6166495601730213, + "timestamp": 12.797790973215054 + }, + { + "x": 4.680529425090156, + "y": 1.9467781190423497, + "heading": -0.43275350887213987, + "angularVelocity": 2.238608397454811e-7, + "velocityX": 3.4190101882436035, + "velocityY": -1.6166495601730246, + "timestamp": 12.85777785233907 + }, + { + "x": 4.885625175976101, + "y": 1.8498003572903603, + "heading": -0.4327534954434267, + "angularVelocity": 2.2386084091908683e-7, + "velocityX": 3.4190101882436177, + "velocityY": -1.6166495601729944, + "timestamp": 12.917764731463084 + }, + { + "x": 5.090720926874051, + "y": 1.7528225955637593, + "heading": -0.43275348201471353, + "angularVelocity": 2.238608408563689e-7, + "velocityX": 3.41901018844374, + "velocityY": -1.616649559749761, + "timestamp": 12.977751610587099 + }, + { + "x": 5.295816846922337, + "y": 1.6558451915696206, + "heading": -0.4327534685859999, + "angularVelocity": 2.2386084788003102e-7, + "velocityX": 3.41901300823263, + "velocityY": -1.6166435962379466, + "timestamp": 13.037738489711113 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.4327534551249964, + "angularVelocity": 2.2439912995828436e-7, + "velocityX": 3.453867801804683, + "velocityY": -1.5407737242905175, + "timestamp": 13.097725368835128 + }, + { + "x": 5.767637731173938, + "y": 1.4715486411010592, + "heading": -0.43275344331162413, + "angularVelocity": 1.594904372832459e-7, + "velocityX": 3.572782848206104, + "velocityY": -1.2403267182093187, + "timestamp": 13.17179483937328 + }, + { + "x": 6.033774187788168, + "y": 1.3841256962215103, + "heading": -0.43275343151544077, + "angularVelocity": 1.5925837367602458e-7, + "velocityX": 3.5930654651722995, + "velocityY": -1.1802831077956568, + "timestamp": 13.245864309911433 + }, + { + "x": 6.29991069823436, + "y": 1.2967029152195577, + "heading": -0.43275341971925746, + "angularVelocity": 1.5925837245370656e-7, + "velocityX": 3.593066191948934, + "velocityY": -1.1802808953106085, + "timestamp": 13.319933780449585 + }, + { + "x": 6.566047156059249, + "y": 1.2092799740277729, + "heading": -0.4327534079212868, + "angularVelocity": 1.5928250285484768e-7, + "velocityX": 3.5930654815171974, + "velocityY": -1.180283058007746, + "timestamp": 13.394003250987737 + }, + { + "x": 6.815604164652681, + "y": 1.1270295176704332, + "heading": -0.3680065962902011, + "angularVelocity": 0.8741362826096571, + "velocityX": 3.369229005962575, + "velocityY": -1.1104501727870977, + "timestamp": 13.468072721525889 + }, + { + "x": 7.042448674730911, + "y": 1.0523256933844904, + "heading": -0.3045889928856297, + "angularVelocity": 0.8561908562841154, + "velocityX": 3.0625912191634557, + "velocityY": -1.0085643078475075, + "timestamp": 13.542142192064041 + }, + { + "x": 7.24659057217413, + "y": 0.9851213013692786, + "heading": -0.24567984884817082, + "angularVelocity": 0.7953228720207434, + "velocityX": 2.7560869000416006, + "velocityY": -0.9073156798197457, + "timestamp": 13.616211662602193 + }, + { + "x": 7.4280385469425845, + "y": 0.925399905884711, + "heading": -0.19230648340236473, + "angularVelocity": 0.7205852162574101, + "velocityX": 2.4496999026743933, + "velocityY": -0.8062889480734987, + "timestamp": 13.690281133140346 + }, + { + "x": 7.586797948314142, + "y": 0.8731533384819596, + "heading": -0.1449799977839161, + "angularVelocity": 0.6389472649743265, + "velocityX": 2.1433851250466707, + "velocityY": -0.7053724972401416, + "timestamp": 13.764350603678498 + }, + { + "x": 7.722872326109614, + "y": 0.8283767850935353, + "heading": -0.10400779947540247, + "angularVelocity": 0.5531590547472527, + "velocityX": 1.8371182729783764, + "velocityY": -0.6045210403571151, + "timestamp": 13.83842007421665 + }, + { + "x": 7.836264193936779, + "y": 0.7910670987596001, + "heading": -0.06959571066012236, + "angularVelocity": 0.46459207235125305, + "velocityX": 1.530885356724122, + "velocityY": -0.5037120700723456, + "timestamp": 13.912489544754802 + }, + { + "x": 7.92697541388319, + "y": 0.7612220649081012, + "heading": -0.04189104486357632, + "angularVelocity": 0.37403623375808837, + "velocityX": 1.2246775802141858, + "velocityY": -0.40293299836842056, + "timestamp": 13.986559015292954 + }, + { + "x": 7.995007406321081, + "y": 0.7388400303082948, + "heading": -0.02100380767783558, + "angularVelocity": 0.2819952273721474, + "velocityX": 0.918488979921211, + "velocityY": -0.3021762466666727, + "timestamp": 14.060628485831106 + }, + { + "x": 8.04036127275019, + "y": 0.7239196955099017, + "heading": -0.007018297514014948, + "angularVelocity": 0.1888161217058643, + "velocityX": 0.6123152507988858, + "velocityY": -0.2014370386339925, + "timestamp": 14.134697956369259 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -1.7184723296669235e-26, + "angularVelocity": 0.09475290511763465, + "velocityX": 0.30615312084056384, + "velocityY": -0.10071228953000028, + "timestamp": 14.20876742690741 + }, + { + "x": 8.063037872314453, + "y": 0.7164599895477295, + "heading": -8.404488237973452e-27, + "angularVelocity": 4.825075090660763e-27, + "velocityX": -1.1513118466109779e-26, + "velocityY": -4.361967999563915e-27, + "timestamp": 14.282836897445563 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHG.1.traj b/src/main/deploy/choreo/SourceLanePHG.1.traj deleted file mode 100644 index af0c38e9..00000000 --- a/src/main/deploy/choreo/SourceLanePHG.1.traj +++ /dev/null @@ -1,760 +0,0 @@ -{ - "samples": [ - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": -2.4067139628401462e-30, - "velocityY": 4.141136343307707e-31, - "timestamp": 0 - }, - { - "x": 0.6685138112284881, - "y": 4.399313979606576, - "heading": -1.0421267461853634, - "angularVelocity": 0.09932567105471685, - "velocityX": 0.09267630701206256, - "velocityY": -0.1889696524677149, - "timestamp": 0.054279568525224545 - }, - { - "x": 0.6786484579557129, - "y": 4.378832133160022, - "heading": -1.0314692860511485, - "angularVelocity": 0.19634386241041998, - "velocityX": 0.18671199868723143, - "velocityY": -0.37733989055266326, - "timestamp": 0.10855913705044909 - }, - { - "x": 0.693968761358233, - "y": 4.348162371795097, - "heading": -1.0156843576080679, - "angularVelocity": 0.2908079204009033, - "velocityX": 0.28224806900226374, - "velocityY": -0.5650332564945059, - "timestamp": 0.16283870557567365 - }, - { - "x": 0.7145650551769096, - "y": 4.307346481900655, - "heading": -0.9949261504953751, - "angularVelocity": 0.38243132133675484, - "velocityX": 0.3794483703219895, - "velocityY": -0.7519567860137538, - "timestamp": 0.21711827410089818 - }, - { - "x": 0.7405380857475481, - "y": 4.256432369487947, - "heading": -0.9693670660541108, - "angularVelocity": 0.4708785485165361, - "velocityX": 0.47850473532355586, - "velocityY": -0.937997736460398, - "timestamp": 0.2713978426261227 - }, - { - "x": 0.7720008885174507, - "y": 4.195475448437783, - "heading": -0.9392010256611868, - "angularVelocity": 0.555753135342305, - "velocityX": 0.57964356800814, - "velocityY": -1.123017789314901, - "timestamp": 0.32567741115134724 - }, - { - "x": 0.8090811298857963, - "y": 4.124540462550696, - "heading": -0.9046476631972737, - "angularVelocity": 0.6365813768002293, - "velocityX": 0.6831344164262224, - "velocityY": -1.3068450581754785, - "timestamp": 0.37995697967657177 - }, - { - "x": 0.8519240607624827, - "y": 4.043703916517851, - "heading": -0.8659577584025531, - "angularVelocity": 0.7127894684118136, - "velocityX": 0.7893012424514297, - "velocityY": -1.4892628705270368, - "timestamp": 0.4342365482017963 - }, - { - "x": 0.9006962826465369, - "y": 3.9530573795094304, - "heading": -0.8234204483791939, - "angularVelocity": 0.7836707471907769, - "velocityX": 0.8985373909409342, - "velocityY": -1.66999369138861, - "timestamp": 0.48851611672702083 - }, - { - "x": 0.9555906032345497, - "y": 3.8527120690461056, - "heading": -0.7773730211474894, - "angularVelocity": 0.8483381221113144, - "velocityX": 1.0113256622978322, - "velocityY": -1.848675536480972, - "timestamp": 0.5427956852522454 - }, - { - "x": 1.016832364065682, - "y": 3.7428053636391674, - "heading": -0.7282144900332294, - "angularVelocity": 0.9056544193311374, - "velocityX": 1.1282654320045469, - "velocityY": -2.024826438254813, - "timestamp": 0.59707525377747 - }, - { - "x": 1.0846877631000658, - "y": 3.6235103077486457, - "heading": -0.6764247389027994, - "angularVelocity": 0.954129749693199, - "velocityX": 1.2501094035567064, - "velocityY": -2.1977893180024854, - "timestamp": 0.6513548223026945 - }, - { - "x": 1.1594748616088653, - "y": 3.4950499100869274, - "heading": -0.6225919295344664, - "angularVelocity": 0.9917692942476846, - "velocityX": 1.3778130619082676, - "velocityY": -2.3666436773907185, - "timestamp": 0.705634390827919 - }, - { - "x": 1.2415780909962442, - "y": 3.357719385453135, - "heading": -0.5674522655491329, - "angularVelocity": 1.0158456576475523, - "velocityX": 1.5125991532011505, - "velocityY": -2.530059253694571, - "timestamp": 0.7599139593531435 - }, - { - "x": 1.3314668957233695, - "y": 3.2119220175526286, - "heading": -0.5119484539522454, - "angularVelocity": 1.0225543994715707, - "velocityX": 1.6560338847452667, - "velocityY": -2.6860450785041534, - "timestamp": 0.8141935278783681 - }, - { - "x": 1.4297177382603383, - "y": 3.0582290891859683, - "heading": -0.45731709920597674, - "angularVelocity": 1.0064810062165677, - "velocityX": 1.8100888641240536, - "velocityY": -2.8315060812474724, - "timestamp": 0.8684730964035926 - }, - { - "x": 1.5370329600645323, - "y": 2.8974829642202957, - "heading": -0.4052238817010545, - "angularVelocity": 0.9597205526922217, - "velocityX": 1.9770831773344253, - "velocityY": -2.961448097933455, - "timestamp": 0.9227526649288171 - }, - { - "x": 1.65422980763682, - "y": 2.7309746521881917, - "heading": -0.3579907998094356, - "angularVelocity": 0.8701816019350151, - "velocityX": 2.1591337358885414, - "velocityY": -3.067605667401865, - "timestamp": 0.9770322334540417 - }, - { - "x": 1.7821106756566756, - "y": 2.5607173715442637, - "heading": -0.3189850796711697, - "angularVelocity": 0.7186077781760888, - "velocityX": 2.355966922626252, - "velocityY": -3.1366734347714873, - "timestamp": 1.0313118019792662 - }, - { - "x": 1.9210512751868398, - "y": 2.3896521178847574, - "heading": -0.29256562311844797, - "angularVelocity": 0.4867293029502528, - "velocityX": 2.5597218862488753, - "velocityY": -3.1515588334128015, - "timestamp": 1.0855913705044908 - }, - { - "x": 2.0706237625196056, - "y": 2.2212859540907277, - "heading": -0.28021240209206266, - "angularVelocity": 0.22758509991937323, - "velocityX": 2.755594626056317, - "velocityY": -3.1018331274277675, - "timestamp": 1.1398709390297155 - }, - { - "x": 2.2301805683211944, - "y": 2.0582298433884807, - "heading": -0.2764430998476646, - "angularVelocity": 0.06944237669582931, - "velocityX": 2.9395371064425038, - "velocityY": -3.0040052847226777, - "timestamp": 1.1941505075549401 - }, - { - "x": 2.3985885826985385, - "y": 1.9024633766120895, - "heading": -0.27643829138574205, - "angularVelocity": 0.00008858695974109528, - "velocityX": 3.102604146513756, - "velocityY": -2.869707166223975, - "timestamp": 1.2484300760801648 - }, - { - "x": 2.574594080208038, - "y": 1.7553334006090828, - "heading": -0.2764382175735448, - "angularVelocity": 0.0000013598523209833, - "velocityX": 3.24257362929679, - "velocityY": -2.710595901930033, - "timestamp": 1.3027096446053894 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.2764381275977681, - "angularVelocity": 0.0000016576361814365047, - "velocityX": 3.3743689745660044, - "velocityY": -2.5446505810771214, - "timestamp": 1.356989213130614 - }, - { - "x": 2.954408298235258, - "y": 1.4843272313598128, - "heading": -0.2764380461537353, - "angularVelocity": 0.0000014502572240295733, - "velocityX": 3.501794020880456, - "velocityY": -2.3662316687928113, - "timestamp": 1.4131475493634884 - }, - { - "x": 3.1576899513687753, - "y": 1.3618209313025427, - "heading": -0.2764379800793587, - "angularVelocity": 0.0000011765729015146578, - "velocityX": 3.6197947939653696, - "velocityY": -2.1814446131251213, - "timestamp": 1.4693058855963628 - }, - { - "x": 3.3670512430786275, - "y": 1.2500216621052442, - "heading": -0.27643792471439105, - "angularVelocity": 9.858726483276036e-7, - "velocityX": 3.7280536738425174, - "velocityY": -1.9907867058899023, - "timestamp": 1.5254642218292371 - }, - { - "x": 3.581928723423017, - "y": 1.1492303042976313, - "heading": -0.27643787707795237, - "angularVelocity": 8.482523145865807e-7, - "velocityX": 3.826279315921067, - "velocityY": -1.79477108063992, - "timestamp": 1.5816225580621115 - }, - { - "x": 3.801744097262885, - "y": 1.0597181118411938, - "heading": -0.27643783516424925, - "angularVelocity": 7.463487333007502e-7, - "velocityX": 3.914207374811615, - "velocityY": -1.5939252916122084, - "timestamp": 1.6377808942949859 - }, - { - "x": 4.0259057805385385, - "y": 0.9817259817488578, - "heading": -0.27643779756498593, - "angularVelocity": 6.695223869720139e-7, - "velocityX": 3.991601217424077, - "velocityY": -1.3887898987770135, - "timestamp": 1.6939392305278602 - }, - { - "x": 4.253810492376224, - "y": 0.9154638050144658, - "heading": -0.2764377632538633, - "angularVelocity": 6.1097113708303e-7, - "velocityX": 4.058252560984372, - "velocityY": -1.1799170199705118, - "timestamp": 1.7500975667607346 - }, - { - "x": 4.484844878745894, - "y": 0.8611099002652811, - "heading": -0.2764377314569093, - "angularVelocity": 5.662018528839179e-7, - "velocityX": 4.113982034859944, - "velocityY": -0.9678688578628118, - "timestamp": 1.806255902993609 - }, - { - "x": 4.718387163351893, - "y": 0.8188105308042397, - "heading": -0.27643770157079633, - "angularVelocity": 5.321758963604942e-7, - "velocityX": 4.15863966549077, - "velocityY": -0.7532162150538033, - "timestamp": 1.8624142392264833 - }, - { - "x": 4.953808821482174, - "y": 0.7886795038935095, - "heading": -0.276437673109168, - "angularVelocity": 5.068103905597141e-7, - "velocityX": 4.1921052852073, - "velocityY": -0.5365370296190328, - "timestamp": 1.9185725754593577 - }, - { - "x": 5.190476272916541, - "y": 0.7707978448607309, - "heading": -0.2764376456658509, - "angularVelocity": 4.886775314219463e-7, - "velocityX": 4.21428887161057, - "velocityY": -0.3184150427575166, - "timestamp": 1.974730911692232 - }, - { - "x": 5.4277525922783605, - "y": 0.7652135064035146, - "heading": -0.2764376188884457, - "angularVelocity": 4.7681977394585435e-7, - "velocityX": 4.22513085818452, - "velocityY": -0.09943917202358898, - "timestamp": 2.0308892479251064 - }, - { - "x": 5.664999258887886, - "y": 0.7719407565233619, - "heading": -0.2764375924579199, - "angularVelocity": 4.7064296385211095e-7, - "velocityX": 4.224602837693134, - "velocityY": 0.11979076609300059, - "timestamp": 2.0870475841579808 - }, - { - "x": 5.898877486323667, - "y": 0.7737442479561557, - "heading": -0.2651531848557105, - "angularVelocity": 0.20093913671902439, - "velocityX": 4.164621730707031, - "velocityY": 0.03211440284345818, - "timestamp": 2.143205920390855 - }, - { - "x": 6.12191109577276, - "y": 0.7749441189167442, - "heading": -0.22893250990124014, - "angularVelocity": 0.6449741460337541, - "velocityX": 3.9715138376648005, - "velocityY": 0.021365856631144577, - "timestamp": 2.1993642566237295 - }, - { - "x": 6.332710871841552, - "y": 0.775181013221915, - "heading": -0.1886770321809241, - "angularVelocity": 0.7168210531272046, - "velocityX": 3.7536684704236807, - "velocityY": 0.004218328409775849, - "timestamp": 2.255522592856604 - }, - { - "x": 6.5312320452807215, - "y": 0.7745053331977244, - "heading": -0.14872047736784255, - "angularVelocity": 0.711498194095287, - "velocityX": 3.535025906322294, - "velocityY": -0.012031695906790239, - "timestamp": 2.311680929089478 - }, - { - "x": 6.717485391758682, - "y": 0.7729410233871945, - "heading": -0.11100536134349584, - "angularVelocity": 0.6715853523144963, - "velocityX": 3.316575222343054, - "velocityY": -0.02785534464622499, - "timestamp": 2.3678392653223526 - }, - { - "x": 6.891485719484664, - "y": 0.7705023127908537, - "heading": -0.07664662804260018, - "angularVelocity": 0.6118189320713212, - "velocityX": 3.0983882251149053, - "velocityY": -0.043425620485337525, - "timestamp": 2.423997601555227 - }, - { - "x": 7.053246047425731, - "y": 0.7671987747352785, - "heading": -0.046371963828299605, - "angularVelocity": 0.5390947496870709, - "velocityX": 2.880433053968835, - "velocityY": -0.058825426057371275, - "timestamp": 2.4801559377881013 - }, - { - "x": 7.2027771487227, - "y": 0.7630373158459373, - "heading": -0.02069540493420602, - "angularVelocity": 0.45721722929294106, - "velocityX": 2.6626697179364744, - "velocityY": -0.07410224676318732, - "timestamp": 2.5363142740209756 - }, - { - "x": 7.340087890625, - "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": 0.36851884016630077, - "velocityX": 2.44506427919992, - "velocityY": -0.08928635297360961, - "timestamp": 2.59247261025385 - }, - { - "x": 7.477669598107483, - "y": 0.7513983472276828, - "heading": 0.016486572692662255, - "angularVelocity": 0.2640172810255479, - "velocityX": 2.203244361670493, - "velocityY": -0.10609000128911963, - "timestamp": 2.65491766168617 - }, - { - "x": 7.600090715062493, - "y": 0.7448611589208545, - "heading": 0.02742791207243882, - "angularVelocity": 0.17521547550705094, - "velocityX": 1.9604614640712232, - "velocityY": -0.1046870513656922, - "timestamp": 2.71736271311849 - }, - { - "x": 7.707366712764208, - "y": 0.7393129750529779, - "heading": 0.03361109082005568, - "angularVelocity": 0.09901791424286414, - "velocityX": 1.7179263246821832, - "velocityY": -0.08884905593984434, - "timestamp": 2.77980776455081 - }, - { - "x": 7.799550704086465, - "y": 0.7354784548909429, - "heading": 0.035675107933675586, - "angularVelocity": 0.03305333355128957, - "velocityX": 1.4762417390618812, - "velocityY": -0.06140630961271657, - "timestamp": 2.84225281598313 - }, - { - "x": 7.8767131917501745, - "y": 0.7339488795899547, - "heading": 0.034146712742278346, - "angularVelocity": -0.024475841661426835, - "velocityX": 1.2356861895988753, - "velocityY": -0.02449474002989417, - "timestamp": 2.90469786741545 - }, - { - "x": 7.938931274791619, - "y": 0.7352136619846016, - "heading": 0.02946667853743436, - "angularVelocity": -0.07494643846864563, - "velocityX": 0.9963653102100188, - "velocityY": 0.0202543254531208, - "timestamp": 2.9671429188477703 - }, - { - "x": 7.986282927383124, - "y": 0.739683296755121, - "heading": 0.022008823943331524, - "angularVelocity": -0.11943067421740607, - "velocityX": 0.7582931153932394, - "velocityY": 0.07157708526133728, - "timestamp": 3.0295879702800903 - }, - { - "x": 8.018844028188742, - "y": 0.7477062429275226, - "heading": 0.012093915253346623, - "angularVelocity": -0.1587781331356692, - "velocityX": 0.5214360475130281, - "velocityY": 0.1284800955140119, - "timestamp": 3.0920330217124103 - }, - { - "x": 8.036686897277832, - "y": 0.7595815062522888, - "heading": 3.7467909104623897e-28, - "angularVelocity": -0.19367291684360474, - "velocityX": 0.28573711895216997, - "velocityY": 0.19017140753959036, - "timestamp": 3.1544780731447304 - }, - { - "x": 8.033078547677619, - "y": 0.7841378633723926, - "heading": -0.020629184334771507, - "angularVelocity": -0.23689147615093625, - "velocityX": -0.041435824577044006, - "velocityY": 0.28198844863030675, - "timestamp": 3.241560920826543 - }, - { - "x": 8.000977407549723, - "y": 0.8166882948302093, - "heading": -0.04487258538731522, - "angularVelocity": -0.27839467470248114, - "velocityX": -0.3686275883534489, - "velocityY": 0.37378694340303564, - "timestamp": 3.328643768508355 - }, - { - "x": 7.940381596505895, - "y": 0.8572308856330779, - "heading": -0.0725538114207906, - "angularVelocity": -0.31787231091269064, - "velocityX": -0.6958409452253523, - "velocityY": 0.46556344770677466, - "timestamp": 3.4157266161901676 - }, - { - "x": 7.851288941262752, - "y": 0.9057633293863456, - "heading": -0.10346158740058668, - "angularVelocity": -0.35492380879330676, - "velocityX": -1.0230792586006634, - "velocityY": 0.5573134669481409, - "timestamp": 3.50280946387198 - }, - { - "x": 7.733696909453058, - "y": 0.9622827924317745, - "heading": -0.1373381842539412, - "angularVelocity": -0.38901572186907185, - "velocityX": -1.3503466519533016, - "velocityY": 0.649030946391906, - "timestamp": 3.5898923115537924 - }, - { - "x": 7.587602525764421, - "y": 1.026785702749848, - "heading": -0.173861756071492, - "angularVelocity": -0.41941177613991887, - "velocityX": -1.6776482117631761, - "velocityY": 0.7407074072009858, - "timestamp": 3.676975159235605 - }, - { - "x": 7.413002271708086, - "y": 1.0992674032204544, - "heading": -0.2126180954789335, - "angularVelocity": -0.4450513555671891, - "velocityX": -2.004990175497003, - "velocityY": 0.8323303888205812, - "timestamp": 3.764058006917417 - }, - { - "x": 7.209891989182915, - "y": 1.179721540530717, - "heading": -0.25305269619381565, - "angularVelocity": -0.46432336322560563, - "velocityX": -2.332379888026919, - "velocityY": 0.9238804133304188, - "timestamp": 3.8511408545992296 - }, - { - "x": 6.978266885073543, - "y": 1.2681388833729486, - "heading": -0.2943826693816009, - "angularVelocity": -0.47460520972854375, - "velocityX": -2.6598246414230293, - "velocityY": 1.0153244318019483, - "timestamp": 3.938223702281042 - }, - { - "x": 6.718122073357289, - "y": 1.3645047215826278, - "heading": -0.3354158360182677, - "angularVelocity": -0.4711968858275734, - "velocityX": -2.9873255025695, - "velocityY": 1.1065995287818986, - "timestamp": 4.025306549962854 - }, - { - "x": 6.429455909714376, - "y": 1.4687919159078104, - "heading": -0.3741111996126966, - "angularVelocity": -0.44435115093865635, - "velocityX": -3.314845245962294, - "velocityY": 1.1975629771115504, - "timestamp": 4.112389397644667 - }, - { - "x": 6.11229273419799, - "y": 1.5809350912859041, - "heading": -0.4061539482737321, - "angularVelocity": -0.3679570605926316, - "velocityX": -3.642085484793208, - "velocityY": 1.287775702832415, - "timestamp": 4.199472245326479 - }, - { - "x": 5.7670373625706794, - "y": 1.70063842953662, - "heading": -0.41585493727254547, - "angularVelocity": -0.11139953799236638, - "velocityX": -3.964677095641404, - "velocityY": 1.3745914544284772, - "timestamp": 4.286555093008292 - }, - { - "x": 5.412761186901786, - "y": 1.8003410365894708, - "heading": -0.415854945416732, - "angularVelocity": -9.352228080597314e-8, - "velocityX": -4.0682658537231795, - "velocityY": 1.1449167052638152, - "timestamp": 4.373637940690104 - }, - { - "x": 5.071022372606716, - "y": 1.8966285846840993, - "heading": -0.4602634606072584, - "angularVelocity": -0.5099570853813654, - "velocityX": -3.924295350833406, - "velocityY": 1.1057004985235321, - "timestamp": 4.460720788371916 - }, - { - "x": 4.757762067487565, - "y": 1.9848574922818703, - "heading": -0.5077454026581931, - "angularVelocity": -0.5452502222300594, - "velocityX": -3.597267584355503, - "velocityY": 1.0131605700372426, - "timestamp": 4.547803636053729 - }, - { - "x": 4.472993355873205, - "y": 2.0650467750158894, - "heading": -0.5536705784011021, - "angularVelocity": -0.5273733802403061, - "velocityX": -3.2700895663732, - "velocityY": 0.9208390041058229, - "timestamp": 4.634886483735541 - }, - { - "x": 4.216711388833469, - "y": 2.137205651490315, - "heading": -0.5965489946241969, - "angularVelocity": -0.4923864729339809, - "velocityX": -2.942967230196159, - "velocityY": 0.8286232983340568, - "timestamp": 4.721969331417354 - }, - { - "x": 3.9889121588577114, - "y": 2.2013393045187435, - "heading": -0.6356392489676235, - "angularVelocity": -0.44888580683829243, - "velocityX": -2.6158909135367425, - "velocityY": 0.7364671084570362, - "timestamp": 4.809052179099166 - }, - { - "x": 3.7895927667556286, - "y": 2.257451059564497, - "heading": -0.67049482646752, - "angularVelocity": -0.4002576675863153, - "velocityX": -2.28884788920048, - "velocityY": 0.6443491059316062, - "timestamp": 4.8961350267809784 - }, - { - "x": 3.6187510751100724, - "y": 2.3055432379933247, - "heading": -0.7008161056508533, - "angularVelocity": -0.34818887979091523, - "velocityX": -1.961829409504224, - "velocityY": 0.5522577603864, - "timestamp": 4.983217874462791 - }, - { - "x": 3.4763854585684455, - "y": 2.3456175525628025, - "heading": -0.7263878572428285, - "angularVelocity": -0.2936485458699124, - "velocityX": -1.6348295942481088, - "velocityY": 0.46018608298046837, - "timestamp": 5.070300722144603 - }, - { - "x": 3.3624946473645725, - "y": 2.3776753150411416, - "heading": -0.7470483845555387, - "angularVelocity": -0.2372513975220547, - "velocityX": -1.3078443601202956, - "velocityY": 0.3681294690255582, - "timestamp": 5.157383569826416 - }, - { - "x": 3.27707762853477, - "y": 2.401717555655836, - "heading": -0.7626725767311283, - "angularVelocity": -0.17941756145455898, - "velocityX": -0.9808707581762147, - "velocityY": 0.2760846855001934, - "timestamp": 5.244466417508228 - }, - { - "x": 3.220133581536875, - "y": 2.417745096695034, - "heading": -0.7731618414519866, - "angularVelocity": -0.12045155848812429, - "velocityX": -0.653906578778407, - "velocityY": 0.184049344570815, - "timestamp": 5.3315492651900405 - }, - { - "x": 3.191661834716797, - "y": 2.425758600234986, - "heading": -0.7784377394795863, - "angularVelocity": -0.06058481283107604, - "velocityX": -0.3269501121978633, - "velocityY": 0.09202160647332173, - "timestamp": 5.418632112871853 - }, - { - "x": 3.191661834716797, - "y": 2.425758600234986, - "heading": -0.7784377394795863, - "angularVelocity": -2.2213424468321708e-29, - "velocityX": 4.80853199627901e-31, - "velocityY": 5.887004206119992e-31, - "timestamp": 5.505714960553665 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHG.2.traj b/src/main/deploy/choreo/SourceLanePHG.2.traj deleted file mode 100644 index 1a6f478e..00000000 --- a/src/main/deploy/choreo/SourceLanePHG.2.traj +++ /dev/null @@ -1,751 +0,0 @@ -{ - "samples": [ - { - "x": 3.191661834716797, - "y": 2.425758600234986, - "heading": -0.7784377394795863, - "angularVelocity": -2.2213424468321708e-29, - "velocityX": 4.80853199627901e-31, - "velocityY": 5.887004206119992e-31, - "timestamp": 0 - }, - { - "x": 3.2018718927349297, - "y": 2.4192860517445967, - "heading": -0.775629671378581, - "angularVelocity": 0.05042136367130771, - "velocityX": 0.18333068498342892, - "velocityY": -0.11622037271718683, - "timestamp": 0.05569203005517309 - }, - { - "x": 3.2224010944618575, - "y": 2.406516889109746, - "heading": -0.7699926416822923, - "angularVelocity": 0.10121788864051368, - "velocityX": 0.3686201006964548, - "velocityY": -0.22928168756284123, - "timestamp": 0.11138406011034618 - }, - { - "x": 3.2533689407857556, - "y": 2.3876518965280447, - "heading": -0.7615043709895374, - "angularVelocity": 0.15241446010040724, - "velocityX": 0.5560552612863737, - "velocityY": -0.3387377433182489, - "timestamp": 0.16707609016551928 - }, - { - "x": 3.2949065410422427, - "y": 2.3629220473113715, - "heading": -0.7501411239102694, - "angularVelocity": 0.2040372216277724, - "velocityX": 0.7458446067657482, - "velocityY": -0.4440464675497314, - "timestamp": 0.22276812022069237 - }, - { - "x": 3.3471578282370227, - "y": 2.3325953799076995, - "heading": -0.7358776871721558, - "angularVelocity": 0.2561127099152785, - "velocityX": 0.9382183975519481, - "velocityY": -0.5445423227278213, - "timestamp": 0.27846015027586546 - }, - { - "x": 3.41028066593418, - "y": 2.2969859549710607, - "heading": -0.7186874476189649, - "angularVelocity": 0.3086660611250982, - "velocityX": 1.1334267692275264, - "velocityY": -0.6393989391545056, - "timestamp": 0.33415218033103855 - }, - { - "x": 3.4844475827329466, - "y": 2.256465654452846, - "heading": -0.6985426607798245, - "angularVelocity": 0.3617175890191665, - "velocityX": 1.331733045559483, - "velocityY": -0.727578083938971, - "timestamp": 0.38984421038621164 - }, - { - "x": 3.569845617335609, - "y": 2.2114798809831826, - "heading": -0.6754150695245975, - "angularVelocity": 0.41527649885117823, - "velocityX": 1.5333977683711546, - "velocityY": -0.8077596278156253, - "timestamp": 0.44553624044138473 - }, - { - "x": 3.666674280148722, - "y": 2.1625685982008305, - "heading": -0.6492771543323811, - "angularVelocity": 0.4693295461903955, - "velocityX": 1.738644878220203, - "velocityY": -0.87824564365666, - "timestamp": 0.5012282704965578 - }, - { - "x": 3.775139743986139, - "y": 2.1103945662614825, - "heading": -0.6201045118785193, - "angularVelocity": 0.5238207769578656, - "velocityX": 1.9475940045633349, - "velocityY": -0.9368312106356999, - "timestamp": 0.5569203005517309 - }, - { - "x": 3.895441739801367, - "y": 2.055780830444415, - "heading": -0.5878802296523478, - "angularVelocity": 0.5786156869169138, - "velocityX": 2.1601294780608042, - "velocityY": -0.9806382666058729, - "timestamp": 0.612612330606904 - }, - { - "x": 4.027746802463347, - "y": 1.9997587552569576, - "heading": -0.5526027278249325, - "angularVelocity": 0.6334389641114943, - "velocityX": 2.3756552334491494, - "velocityY": -1.0059262542945027, - "timestamp": 0.6683043606620771 - }, - { - "x": 4.172137292392089, - "y": 1.9436241693843643, - "heading": -0.51429939372606, - "angularVelocity": 0.6877704774080914, - "velocityX": 2.5926598435305, - "velocityY": -1.0079464838502272, - "timestamp": 0.7239963907172502 - }, - { - "x": 4.3285218208265555, - "y": 1.8889883430033667, - "heading": -0.4730491306307316, - "angularVelocity": 0.7406852121293169, - "velocityX": 2.808023487014891, - "velocityY": -0.9810349223555088, - "timestamp": 0.7796884207724233 - }, - { - "x": 4.49649793430264, - "y": 1.8377877666662683, - "heading": -0.4290162836436476, - "angularVelocity": 0.7906489841268347, - "velocityX": 3.0161607201187084, - "velocityY": -0.9193519483196195, - "timestamp": 0.8353804508275964 - }, - { - "x": 4.675191879236026, - "y": 1.7921900095553935, - "heading": -0.38249210396514205, - "angularVelocity": 0.8353830814286095, - "velocityX": 3.2086089294348805, - "velocityY": -0.8187483391376037, - "timestamp": 0.8910724808827695 - }, - { - "x": 4.863169718330775, - "y": 1.7543508609872742, - "heading": -0.33392159403718297, - "angularVelocity": 0.8721267635573858, - "velocityX": 3.375309517511239, - "velocityY": -0.6794356127911404, - "timestamp": 0.9467645109379426 - }, - { - "x": 5.0585395666429065, - "y": 1.7260962541621017, - "heading": -0.2838765417100418, - "angularVelocity": 0.8986034855896275, - "velocityX": 3.5080396264704503, - "velocityY": -0.5073366296251212, - "timestamp": 1.0024565409931157 - }, - { - "x": 5.259232445364841, - "y": 1.708716063515423, - "heading": -0.232969871935678, - "angularVelocity": 0.9140745942270582, - "velocityX": 3.603619378268493, - "velocityY": -0.312076802182647, - "timestamp": 1.0581485710482887 - }, - { - "x": 5.4632887840271, - "y": 1.7029682397842407, - "heading": -0.18176811165848733, - "angularVelocity": 0.919373207018413, - "velocityX": 3.6640132970570867, - "velocityY": -0.10320729421226303, - "timestamp": 1.1138406011034618 - }, - { - "x": 5.615161187166233, - "y": 1.7052796689747005, - "heading": -0.14398910637879186, - "angularVelocity": 0.9169644687547894, - "velocityX": 3.6862166283090447, - "velocityY": 0.05610254753936902, - "timestamp": 1.1550406765338632 - }, - { - "x": 5.767420990442743, - "y": 1.7142063964817436, - "heading": -0.10652675388990283, - "angularVelocity": 0.9092787354764283, - "velocityX": 3.695619527049639, - "velocityY": 0.21666774669193956, - "timestamp": 1.1962407519642646 - }, - { - "x": 5.919386924300179, - "y": 1.7297521629533545, - "heading": -0.0696570985444518, - "angularVelocity": 0.8948929088184353, - "velocityX": 3.6884867872183342, - "velocityY": 0.3773237381050978, - "timestamp": 1.237440827394666 - }, - { - "x": 6.070179219983336, - "y": 1.751816393631274, - "heading": -0.03372721993940781, - "angularVelocity": 0.8720828355214922, - "velocityX": 3.66000047591873, - "velocityY": 0.5355385990783469, - "timestamp": 1.2786409028250674 - }, - { - "x": 6.218691660613551, - "y": 1.7800947313312476, - "heading": 0.0008467859316837714, - "angularVelocity": 0.8391733633958196, - "velocityX": 3.6046642895373915, - "velocityY": 0.6863661632790825, - "timestamp": 1.3198409782554688 - }, - { - "x": 6.363637018199303, - "y": 1.8139414045223912, - "heading": 0.03362320983867217, - "angularVelocity": 0.7955428130794786, - "velocityX": 3.5180847625049867, - "velocityY": 0.821519689892803, - "timestamp": 1.3610410536858701 - }, - { - "x": 6.5037346457874685, - "y": 1.8522755217573394, - "heading": 0.06422742113546837, - "angularVelocity": 0.7428193025640265, - "velocityX": 3.4004216284707804, - "velocityY": 0.9304380352338247, - "timestamp": 1.4022411291162715 - }, - { - "x": 6.63798940408483, - "y": 1.8936799802395343, - "heading": 0.09242547454575481, - "angularVelocity": 0.684417519038789, - "velocityX": 3.2586046723180027, - "velocityY": 1.004960744602, - "timestamp": 1.443441204546673 - }, - { - "x": 6.765843552605767, - "y": 1.936686507443115, - "heading": 0.1181284172233344, - "angularVelocity": 0.6238566898014307, - "velocityX": 3.1032503505222433, - "velocityY": 1.0438458365502405, - "timestamp": 1.4846412799770743 - }, - { - "x": 6.887107406175693, - "y": 1.9800193223462106, - "heading": 0.14135707235090567, - "angularVelocity": 0.5638012766945281, - "velocityX": 2.943292028063759, - "velocityY": 1.0517654264079415, - "timestamp": 1.5258413554074757 - }, - { - "x": 7.0018061689137365, - "y": 2.022672952124899, - "heading": 0.16219156310473354, - "angularVelocity": 0.505690597315122, - "velocityX": 2.7839454549494933, - "velocityY": 1.0352803807542028, - "timestamp": 1.567041430837877 - }, - { - "x": 7.110062105457345, - "y": 2.063884242340868, - "heading": 0.18073107684316084, - "angularVelocity": 0.44998737368201414, - "velocityX": 2.627566464689694, - "velocityY": 1.000272203034835, - "timestamp": 1.6082415062682784 - }, - { - "x": 7.212029936432938, - "y": 2.1030762783452954, - "heading": 0.19707316527143803, - "angularVelocity": 0.39665190555011326, - "velocityX": 2.4749428225645964, - "velocityY": 0.951261268213787, - "timestamp": 1.6494415816986798 - }, - { - "x": 7.307866875073466, - "y": 2.139807899939946, - "heading": 0.21130608363236952, - "angularVelocity": 0.3454585510401522, - "velocityX": 2.3261350286220495, - "velocityY": 0.89154258119504, - "timestamp": 1.6906416571290812 - }, - { - "x": 7.397720405161271, - "y": 2.173735875930798, - "heading": 0.22350713861237825, - "angularVelocity": 0.2961415689789137, - "velocityX": 2.1809069315805827, - "velocityY": 0.8234930552048578, - "timestamp": 1.7318417325594826 - }, - { - "x": 7.481724262237549, - "y": 2.2045881748199463, - "heading": 0.2337432969576531, - "angularVelocity": 0.248449990402726, - "velocityX": 2.0389248368776234, - "velocityY": 0.7488408350432911, - "timestamp": 1.773041807989884 - }, - { - "x": 7.595794373586248, - "y": 2.243752616633879, - "heading": 0.24487156374946684, - "angularVelocity": 0.17836312173475907, - "velocityX": 1.8283081756947666, - "velocityY": 0.6277250746783941, - "timestamp": 1.8354328823531283 - }, - { - "x": 7.696474893293651, - "y": 2.2757924724177725, - "heading": 0.2521501090469708, - "angularVelocity": 0.1166600410681812, - "velocityX": 1.613700689320329, - "velocityY": 0.5135326825333361, - "timestamp": 1.8978239567163726 - }, - { - "x": 7.783579008927407, - "y": 2.3010567107975834, - "heading": 0.25599476725502607, - "angularVelocity": 0.06162192665046021, - "velocityX": 1.396098985675918, - "velocityY": 0.4049335363696611, - "timestamp": 1.960215031079617 - }, - { - "x": 7.856962202651826, - "y": 2.3198324584355756, - "heading": 0.25674317704545796, - "angularVelocity": 0.011995462461098809, - "velocityX": 1.17618095975041, - "velocityY": 0.3009364372967597, - "timestamp": 2.0226061054428612 - }, - { - "x": 7.91650981703959, - "y": 2.3323597342047413, - "heading": 0.2546745141891388, - "angularVelocity": -0.03315639099713727, - "velocityX": 0.9544252121878052, - "velocityY": 0.2007863447939911, - "timestamp": 2.0849971798061055 - }, - { - "x": 7.962128947539202, - "y": 2.3388419529288034, - "heading": 0.2500232368325733, - "angularVelocity": -0.07455036484041708, - "velocityX": 0.731180396638371, - "velocityY": 0.10389657158846945, - "timestamp": 2.14738825416935 - }, - { - "x": 7.993742963409653, - "y": 2.339453579074058, - "heading": 0.24298889583795466, - "angularVelocity": -0.11274595070545464, - "velocityX": 0.5067073486568353, - "velocityY": 0.009803103272327605, - "timestamp": 2.209779328532594 - }, - { - "x": 8.011287689208984, - "y": 2.334345817565918, - "heading": 0.2337432969576531, - "angularVelocity": -0.14818784537148968, - "velocityX": 0.28120570094987624, - "velocityY": -0.08186686253232739, - "timestamp": 2.2721704028958385 - }, - { - "x": 8.013057457291007, - "y": 2.3214137467214835, - "heading": 0.22064239230319685, - "angularVelocity": -0.1855819214437055, - "velocityX": 0.02506979249403061, - "velocityY": -0.18319029249174376, - "timestamp": 2.3427640499091886 - }, - { - "x": 7.99666998493903, - "y": 2.3015233616034343, - "heading": 0.20489547559952764, - "angularVelocity": -0.22306421852225894, - "velocityX": -0.232138061217864, - "velocityY": -0.28175885450835847, - "timestamp": 2.4133576969225388 - }, - { - "x": 7.962040754952174, - "y": 2.2748995096887543, - "heading": 0.186495589242378, - "angularVelocity": -0.2606450741051799, - "velocityX": -0.4905431501549616, - "velocityY": -0.37714232145627924, - "timestamp": 2.483951343935889 - }, - { - "x": 7.9090749698086515, - "y": 2.2418049871920993, - "heading": 0.16543502379528896, - "angularVelocity": -0.29833513833201103, - "velocityX": -0.7502911010321619, - "velocityY": -0.4688031274315115, - "timestamp": 2.55454499094924 - }, - { - "x": 7.83766590050416, - "y": 2.202550806257141, - "heading": 0.14170533667281937, - "angularVelocity": -0.3361447966837854, - "velocityX": -1.0115509302272958, - "velocityY": -0.5560582658030753, - "timestamp": 2.62513863796259 - }, - { - "x": 7.747693073646214, - "y": 2.1575104269397727, - "heading": 0.11529745438529491, - "angularVelocity": -0.37408298628529985, - "velocityX": -1.2745173349795946, - "velocityY": -0.6380231256341062, - "timestamp": 2.69573228497594 - }, - { - "x": 7.639020495122075, - "y": 2.1071400026715006, - "heading": 0.08620194014422251, - "angularVelocity": -0.4121548534752705, - "velocityX": -1.5394101753035547, - "velocityY": -0.7135263072432857, - "timestamp": 2.7663259319892903 - }, - { - "x": 7.511495469210905, - "y": 2.052008041294553, - "heading": 0.054409589277409784, - "angularVelocity": -0.4503571101915203, - "velocityX": -1.8064660391756437, - "velocityY": -0.7809762451644056, - "timestamp": 2.8369195790026405 - }, - { - "x": 7.364949479960618, - "y": 1.9928403305635423, - "heading": 0.01991269014230069, - "angularVelocity": -0.4886686068023256, - "velocityX": -2.075909029357446, - "velocityY": -0.8381449781142655, - "timestamp": 2.9075132260159906 - }, - { - "x": 7.199204960601857, - "y": 1.9305905238043142, - "heading": -0.01729230931714009, - "angularVelocity": -0.5270304203493653, - "velocityX": -2.347867355931005, - "velocityY": -0.8818046579666796, - "timestamp": 2.9781068730293407 - }, - { - "x": 7.014098188628079, - "y": 1.8665552887004158, - "heading": -0.05719900816860063, - "angularVelocity": -0.5653015609735185, - "velocityX": -2.6221449068748512, - "velocityY": -0.9070962871742325, - "timestamp": 3.048700520042691 - }, - { - "x": 6.809546840003655, - "y": 1.8025678316240845, - "heading": -0.09977788205082659, - "angularVelocity": -0.6031544718772441, - "velocityX": -2.89758862558468, - "velocityY": -0.9064194836715306, - "timestamp": 3.119294167056041 - }, - { - "x": 6.5857441999493975, - "y": 1.7413205913498453, - "heading": -0.14494480394528797, - "angularVelocity": -0.6398156747153125, - "velocityX": -3.1702943469110325, - "velocityY": -0.8676027215686496, - "timestamp": 3.189887814069391 - }, - { - "x": 6.343702784750113, - "y": 1.6868213690291616, - "heading": -0.19248452084491505, - "angularVelocity": -0.6734276937220217, - "velocityX": -3.42865718714759, - "velocityY": -0.77201312903379, - "timestamp": 3.2604814610827413 - }, - { - "x": 6.0865152782234535, - "y": 1.6445255287826241, - "heading": -0.24188828313696017, - "angularVelocity": -0.699832979059746, - "velocityX": -3.6432103653466354, - "velocityY": -0.5991451360848178, - "timestamp": 3.3310751080960914 - }, - { - "x": 5.820539703788856, - "y": 1.6195865880270663, - "heading": -0.292193130463703, - "angularVelocity": -0.7125973717894101, - "velocityX": -3.767698450036126, - "velocityY": -0.35327457654711547, - "timestamp": 3.4016687551094416 - }, - { - "x": 5.553319270967186, - "y": 1.61406380660166, - "heading": -0.3422542788791351, - "angularVelocity": -0.7091452352074223, - "velocityX": -3.7853325919134613, - "velocityY": -0.07823340568255462, - "timestamp": 3.4722624021227917 - }, - { - "x": 5.290481661115068, - "y": 1.6274952853330582, - "heading": -0.39122534936664, - "angularVelocity": -0.693703648406827, - "velocityX": -3.72324735967261, - "velocityY": 0.19026469519074493, - "timestamp": 3.542856049136142 - }, - { - "x": 5.035514831542969, - "y": 1.6587157249450684, - "heading": -0.43859871161755776, - "angularVelocity": -0.6710711835295708, - "velocityX": -3.6117531868537824, - "velocityY": 0.44225565518814863, - "timestamp": 3.613449696149492 - }, - { - "x": 4.840429456104478, - "y": 1.693764115895758, - "heading": -0.47482255342493607, - "angularVelocity": -0.6498265287292678, - "velocityX": -3.4996744133643607, - "velocityY": 0.628739887672373, - "timestamp": 3.66919356040541 - }, - { - "x": 4.653407145998877, - "y": 1.737869407208374, - "heading": -0.5094986634723291, - "angularVelocity": -0.6220614682935635, - "velocityX": -3.355029519428204, - "velocityY": 0.7912133810840754, - "timestamp": 3.724937424661328 - }, - { - "x": 4.476082725221748, - "y": 1.7892498770756833, - "heading": -0.542310474310624, - "angularVelocity": -0.5886174429468579, - "velocityX": -3.1810572005385227, - "velocityY": 0.9217242211882539, - "timestamp": 3.780681288917246 - }, - { - "x": 4.309697720613771, - "y": 1.8458506852049763, - "heading": -0.5730328703053699, - "angularVelocity": -0.5511350245419085, - "velocityX": -2.984812890690747, - "velocityY": 1.0153728824654409, - "timestamp": 3.836425153173164 - }, - { - "x": 4.155016964634681, - "y": 1.905597573014691, - "heading": -0.601541437642242, - "angularVelocity": -0.5114207225747871, - "velocityX": -2.774848102904345, - "velocityY": 1.071811016463078, - "timestamp": 3.892169017429082 - }, - { - "x": 4.012389650881917, - "y": 1.966603698419971, - "heading": -0.6277908857969042, - "angularVelocity": -0.47089394510133564, - "velocityX": -2.5586190633998585, - "velocityY": 1.0944007240905271, - "timestamp": 3.947912881685 - }, - { - "x": 3.8818763982479205, - "y": 2.02726549778738, - "heading": -0.6517856604184045, - "angularVelocity": -0.43044691898899096, - "velocityX": -2.3413025698185628, - "velocityY": 1.0882237924682305, - "timestamp": 4.003656745940918 - }, - { - "x": 3.763368379878263, - "y": 2.0862691727799794, - "heading": -0.6735568165305625, - "angularVelocity": -0.39055699497630253, - "velocityX": -2.1259383423006524, - "velocityY": 1.0584783774895203, - "timestamp": 4.059400610196836 - }, - { - "x": 3.65667108287158, - "y": 2.142554070867054, - "heading": -0.6931474445007211, - "angularVelocity": -0.35144007742661887, - "velocityX": -1.914063519472572, - "velocityY": 1.009705710904315, - "timestamp": 4.115144474452754 - }, - { - "x": 3.5615549777894695, - "y": 2.1952651611601923, - "heading": -0.7106044963543238, - "angularVelocity": -0.3131654413741073, - "velocityX": -1.7063062697884914, - "velocityY": 0.9455944792622217, - "timestamp": 4.170888338708672 - }, - { - "x": 3.4777834556344756, - "y": 2.2437091911618374, - "heading": -0.7259745587811693, - "angularVelocity": -0.2757265329917277, - "velocityX": -1.5027935948323077, - "velocityY": 0.8690468565157425, - "timestamp": 4.22663220296459 - }, - { - "x": 3.4051270953206445, - "y": 2.2873188436103873, - "heading": -0.7393018511038773, - "angularVelocity": -0.23908088362017574, - "velocityX": -1.3033965492644939, - "velocityY": 0.7823220193049412, - "timestamp": 4.2823760672205085 - }, - { - "x": 3.3433702555456972, - "y": 2.325624884061789, - "heading": -0.7506274140344208, - "angularVelocity": -0.20317147154615087, - "velocityX": -1.1078679348712595, - "velocityY": 0.6871794943303605, - "timestamp": 4.3381199314764265 - }, - { - "x": 3.2923135378300334, - "y": 2.358234947831376, - "heading": -0.7599889096515458, - "angularVelocity": -0.16793768681243185, - "velocityX": -0.9159163685040683, - "velocityY": 0.5849982631249935, - "timestamp": 4.3938637957323445 - }, - { - "x": 3.251774113497663, - "y": 2.384817462963455, - "heading": -0.7674207191873286, - "angularVelocity": -0.13332067367385547, - "velocityX": -0.7272446012399799, - "velocityY": 0.4768688982529085, - "timestamp": 4.449607659988263 - }, - { - "x": 3.2215850043710765, - "y": 2.405089430282806, - "heading": -0.7729541729065691, - "angularVelocity": -0.0992657002362922, - "velocityX": -0.5415682879103932, - "velocityY": 0.36366275625032934, - "timestamp": 4.505351524244181 - }, - { - "x": 3.2015939022173066, - "y": 2.4188070703406934, - "heading": -0.7766178262935338, - "angularVelocity": -0.06572298917321301, - "velocityX": -0.3586242615329249, - "velocityY": 0.24608340740265958, - "timestamp": 4.561095388500099 - }, - { - "x": 3.191661834716797, - "y": 2.425758600234986, - "heading": -0.7784377394795863, - "angularVelocity": -0.03264777586457403, - "velocityX": -0.17817328656858386, - "velocityY": 0.124704843969519, - "timestamp": 4.616839252756017 - }, - { - "x": 3.191661834716797, - "y": 2.425758600234986, - "heading": -0.7784377394795863, - "angularVelocity": -3.751194855513336e-35, - "velocityX": 1.1060996014173732e-35, - "velocityY": 1.8550525441659548e-35, - "timestamp": 4.672583117011935 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHG.traj b/src/main/deploy/choreo/SourceLanePHG.traj deleted file mode 100644 index 711137fc..00000000 --- a/src/main/deploy/choreo/SourceLanePHG.traj +++ /dev/null @@ -1,1498 +0,0 @@ -{ - "samples": [ - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": -2.4067139628401462e-30, - "velocityY": 4.141136343307707e-31, - "timestamp": 0 - }, - { - "x": 0.6685138112284881, - "y": 4.399313979606576, - "heading": -1.0421267461853634, - "angularVelocity": 0.09932567105471685, - "velocityX": 0.09267630701206256, - "velocityY": -0.1889696524677149, - "timestamp": 0.054279568525224545 - }, - { - "x": 0.6786484579557129, - "y": 4.378832133160022, - "heading": -1.0314692860511485, - "angularVelocity": 0.19634386241041998, - "velocityX": 0.18671199868723143, - "velocityY": -0.37733989055266326, - "timestamp": 0.10855913705044909 - }, - { - "x": 0.693968761358233, - "y": 4.348162371795097, - "heading": -1.0156843576080679, - "angularVelocity": 0.2908079204009033, - "velocityX": 0.28224806900226374, - "velocityY": -0.5650332564945059, - "timestamp": 0.16283870557567365 - }, - { - "x": 0.7145650551769096, - "y": 4.307346481900655, - "heading": -0.9949261504953751, - "angularVelocity": 0.38243132133675484, - "velocityX": 0.3794483703219895, - "velocityY": -0.7519567860137538, - "timestamp": 0.21711827410089818 - }, - { - "x": 0.7405380857475481, - "y": 4.256432369487947, - "heading": -0.9693670660541108, - "angularVelocity": 0.4708785485165361, - "velocityX": 0.47850473532355586, - "velocityY": -0.937997736460398, - "timestamp": 0.2713978426261227 - }, - { - "x": 0.7720008885174507, - "y": 4.195475448437783, - "heading": -0.9392010256611868, - "angularVelocity": 0.555753135342305, - "velocityX": 0.57964356800814, - "velocityY": -1.123017789314901, - "timestamp": 0.32567741115134724 - }, - { - "x": 0.8090811298857963, - "y": 4.124540462550696, - "heading": -0.9046476631972737, - "angularVelocity": 0.6365813768002293, - "velocityX": 0.6831344164262224, - "velocityY": -1.3068450581754785, - "timestamp": 0.37995697967657177 - }, - { - "x": 0.8519240607624827, - "y": 4.043703916517851, - "heading": -0.8659577584025531, - "angularVelocity": 0.7127894684118136, - "velocityX": 0.7893012424514297, - "velocityY": -1.4892628705270368, - "timestamp": 0.4342365482017963 - }, - { - "x": 0.9006962826465369, - "y": 3.9530573795094304, - "heading": -0.8234204483791939, - "angularVelocity": 0.7836707471907769, - "velocityX": 0.8985373909409342, - "velocityY": -1.66999369138861, - "timestamp": 0.48851611672702083 - }, - { - "x": 0.9555906032345497, - "y": 3.8527120690461056, - "heading": -0.7773730211474894, - "angularVelocity": 0.8483381221113144, - "velocityX": 1.0113256622978322, - "velocityY": -1.848675536480972, - "timestamp": 0.5427956852522454 - }, - { - "x": 1.016832364065682, - "y": 3.7428053636391674, - "heading": -0.7282144900332294, - "angularVelocity": 0.9056544193311374, - "velocityX": 1.1282654320045469, - "velocityY": -2.024826438254813, - "timestamp": 0.59707525377747 - }, - { - "x": 1.0846877631000658, - "y": 3.6235103077486457, - "heading": -0.6764247389027994, - "angularVelocity": 0.954129749693199, - "velocityX": 1.2501094035567064, - "velocityY": -2.1977893180024854, - "timestamp": 0.6513548223026945 - }, - { - "x": 1.1594748616088653, - "y": 3.4950499100869274, - "heading": -0.6225919295344664, - "angularVelocity": 0.9917692942476846, - "velocityX": 1.3778130619082676, - "velocityY": -2.3666436773907185, - "timestamp": 0.705634390827919 - }, - { - "x": 1.2415780909962442, - "y": 3.357719385453135, - "heading": -0.5674522655491329, - "angularVelocity": 1.0158456576475523, - "velocityX": 1.5125991532011505, - "velocityY": -2.530059253694571, - "timestamp": 0.7599139593531435 - }, - { - "x": 1.3314668957233695, - "y": 3.2119220175526286, - "heading": -0.5119484539522454, - "angularVelocity": 1.0225543994715707, - "velocityX": 1.6560338847452667, - "velocityY": -2.6860450785041534, - "timestamp": 0.8141935278783681 - }, - { - "x": 1.4297177382603383, - "y": 3.0582290891859683, - "heading": -0.45731709920597674, - "angularVelocity": 1.0064810062165677, - "velocityX": 1.8100888641240536, - "velocityY": -2.8315060812474724, - "timestamp": 0.8684730964035926 - }, - { - "x": 1.5370329600645323, - "y": 2.8974829642202957, - "heading": -0.4052238817010545, - "angularVelocity": 0.9597205526922217, - "velocityX": 1.9770831773344253, - "velocityY": -2.961448097933455, - "timestamp": 0.9227526649288171 - }, - { - "x": 1.65422980763682, - "y": 2.7309746521881917, - "heading": -0.3579907998094356, - "angularVelocity": 0.8701816019350151, - "velocityX": 2.1591337358885414, - "velocityY": -3.067605667401865, - "timestamp": 0.9770322334540417 - }, - { - "x": 1.7821106756566756, - "y": 2.5607173715442637, - "heading": -0.3189850796711697, - "angularVelocity": 0.7186077781760888, - "velocityX": 2.355966922626252, - "velocityY": -3.1366734347714873, - "timestamp": 1.0313118019792662 - }, - { - "x": 1.9210512751868398, - "y": 2.3896521178847574, - "heading": -0.29256562311844797, - "angularVelocity": 0.4867293029502528, - "velocityX": 2.5597218862488753, - "velocityY": -3.1515588334128015, - "timestamp": 1.0855913705044908 - }, - { - "x": 2.0706237625196056, - "y": 2.2212859540907277, - "heading": -0.28021240209206266, - "angularVelocity": 0.22758509991937323, - "velocityX": 2.755594626056317, - "velocityY": -3.1018331274277675, - "timestamp": 1.1398709390297155 - }, - { - "x": 2.2301805683211944, - "y": 2.0582298433884807, - "heading": -0.2764430998476646, - "angularVelocity": 0.06944237669582931, - "velocityX": 2.9395371064425038, - "velocityY": -3.0040052847226777, - "timestamp": 1.1941505075549401 - }, - { - "x": 2.3985885826985385, - "y": 1.9024633766120895, - "heading": -0.27643829138574205, - "angularVelocity": 0.00008858695974109528, - "velocityX": 3.102604146513756, - "velocityY": -2.869707166223975, - "timestamp": 1.2484300760801648 - }, - { - "x": 2.574594080208038, - "y": 1.7553334006090828, - "heading": -0.2764382175735448, - "angularVelocity": 0.0000013598523209833, - "velocityX": 3.24257362929679, - "velocityY": -2.710595901930033, - "timestamp": 1.3027096446053894 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.2764381275977681, - "angularVelocity": 0.0000016576361814365047, - "velocityX": 3.3743689745660044, - "velocityY": -2.5446505810771214, - "timestamp": 1.356989213130614 - }, - { - "x": 2.954408298235258, - "y": 1.4843272313598128, - "heading": -0.2764380461537353, - "angularVelocity": 0.0000014502572240295733, - "velocityX": 3.501794020880456, - "velocityY": -2.3662316687928113, - "timestamp": 1.4131475493634884 - }, - { - "x": 3.1576899513687753, - "y": 1.3618209313025427, - "heading": -0.2764379800793587, - "angularVelocity": 0.0000011765729015146578, - "velocityX": 3.6197947939653696, - "velocityY": -2.1814446131251213, - "timestamp": 1.4693058855963628 - }, - { - "x": 3.3670512430786275, - "y": 1.2500216621052442, - "heading": -0.27643792471439105, - "angularVelocity": 9.858726483276036e-7, - "velocityX": 3.7280536738425174, - "velocityY": -1.9907867058899023, - "timestamp": 1.5254642218292371 - }, - { - "x": 3.581928723423017, - "y": 1.1492303042976313, - "heading": -0.27643787707795237, - "angularVelocity": 8.482523145865807e-7, - "velocityX": 3.826279315921067, - "velocityY": -1.79477108063992, - "timestamp": 1.5816225580621115 - }, - { - "x": 3.801744097262885, - "y": 1.0597181118411938, - "heading": -0.27643783516424925, - "angularVelocity": 7.463487333007502e-7, - "velocityX": 3.914207374811615, - "velocityY": -1.5939252916122084, - "timestamp": 1.6377808942949859 - }, - { - "x": 4.0259057805385385, - "y": 0.9817259817488578, - "heading": -0.27643779756498593, - "angularVelocity": 6.695223869720139e-7, - "velocityX": 3.991601217424077, - "velocityY": -1.3887898987770135, - "timestamp": 1.6939392305278602 - }, - { - "x": 4.253810492376224, - "y": 0.9154638050144658, - "heading": -0.2764377632538633, - "angularVelocity": 6.1097113708303e-7, - "velocityX": 4.058252560984372, - "velocityY": -1.1799170199705118, - "timestamp": 1.7500975667607346 - }, - { - "x": 4.484844878745894, - "y": 0.8611099002652811, - "heading": -0.2764377314569093, - "angularVelocity": 5.662018528839179e-7, - "velocityX": 4.113982034859944, - "velocityY": -0.9678688578628118, - "timestamp": 1.806255902993609 - }, - { - "x": 4.718387163351893, - "y": 0.8188105308042397, - "heading": -0.27643770157079633, - "angularVelocity": 5.321758963604942e-7, - "velocityX": 4.15863966549077, - "velocityY": -0.7532162150538033, - "timestamp": 1.8624142392264833 - }, - { - "x": 4.953808821482174, - "y": 0.7886795038935095, - "heading": -0.276437673109168, - "angularVelocity": 5.068103905597141e-7, - "velocityX": 4.1921052852073, - "velocityY": -0.5365370296190328, - "timestamp": 1.9185725754593577 - }, - { - "x": 5.190476272916541, - "y": 0.7707978448607309, - "heading": -0.2764376456658509, - "angularVelocity": 4.886775314219463e-7, - "velocityX": 4.21428887161057, - "velocityY": -0.3184150427575166, - "timestamp": 1.974730911692232 - }, - { - "x": 5.4277525922783605, - "y": 0.7652135064035146, - "heading": -0.2764376188884457, - "angularVelocity": 4.7681977394585435e-7, - "velocityX": 4.22513085818452, - "velocityY": -0.09943917202358898, - "timestamp": 2.0308892479251064 - }, - { - "x": 5.664999258887886, - "y": 0.7719407565233619, - "heading": -0.2764375924579199, - "angularVelocity": 4.7064296385211095e-7, - "velocityX": 4.224602837693134, - "velocityY": 0.11979076609300059, - "timestamp": 2.0870475841579808 - }, - { - "x": 5.898877486323667, - "y": 0.7737442479561557, - "heading": -0.2651531848557105, - "angularVelocity": 0.20093913671902439, - "velocityX": 4.164621730707031, - "velocityY": 0.03211440284345818, - "timestamp": 2.143205920390855 - }, - { - "x": 6.12191109577276, - "y": 0.7749441189167442, - "heading": -0.22893250990124014, - "angularVelocity": 0.6449741460337541, - "velocityX": 3.9715138376648005, - "velocityY": 0.021365856631144577, - "timestamp": 2.1993642566237295 - }, - { - "x": 6.332710871841552, - "y": 0.775181013221915, - "heading": -0.1886770321809241, - "angularVelocity": 0.7168210531272046, - "velocityX": 3.7536684704236807, - "velocityY": 0.004218328409775849, - "timestamp": 2.255522592856604 - }, - { - "x": 6.5312320452807215, - "y": 0.7745053331977244, - "heading": -0.14872047736784255, - "angularVelocity": 0.711498194095287, - "velocityX": 3.535025906322294, - "velocityY": -0.012031695906790239, - "timestamp": 2.311680929089478 - }, - { - "x": 6.717485391758682, - "y": 0.7729410233871945, - "heading": -0.11100536134349584, - "angularVelocity": 0.6715853523144963, - "velocityX": 3.316575222343054, - "velocityY": -0.02785534464622499, - "timestamp": 2.3678392653223526 - }, - { - "x": 6.891485719484664, - "y": 0.7705023127908537, - "heading": -0.07664662804260018, - "angularVelocity": 0.6118189320713212, - "velocityX": 3.0983882251149053, - "velocityY": -0.043425620485337525, - "timestamp": 2.423997601555227 - }, - { - "x": 7.053246047425731, - "y": 0.7671987747352785, - "heading": -0.046371963828299605, - "angularVelocity": 0.5390947496870709, - "velocityX": 2.880433053968835, - "velocityY": -0.058825426057371275, - "timestamp": 2.4801559377881013 - }, - { - "x": 7.2027771487227, - "y": 0.7630373158459373, - "heading": -0.02069540493420602, - "angularVelocity": 0.45721722929294106, - "velocityX": 2.6626697179364744, - "velocityY": -0.07410224676318732, - "timestamp": 2.5363142740209756 - }, - { - "x": 7.340087890625, - "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": 0.36851884016630077, - "velocityX": 2.44506427919992, - "velocityY": -0.08928635297360961, - "timestamp": 2.59247261025385 - }, - { - "x": 7.477669598107483, - "y": 0.7513983472276828, - "heading": 0.016486572692662255, - "angularVelocity": 0.2640172810255479, - "velocityX": 2.203244361670493, - "velocityY": -0.10609000128911963, - "timestamp": 2.65491766168617 - }, - { - "x": 7.600090715062493, - "y": 0.7448611589208545, - "heading": 0.02742791207243882, - "angularVelocity": 0.17521547550705094, - "velocityX": 1.9604614640712232, - "velocityY": -0.1046870513656922, - "timestamp": 2.71736271311849 - }, - { - "x": 7.707366712764208, - "y": 0.7393129750529779, - "heading": 0.03361109082005568, - "angularVelocity": 0.09901791424286414, - "velocityX": 1.7179263246821832, - "velocityY": -0.08884905593984434, - "timestamp": 2.77980776455081 - }, - { - "x": 7.799550704086465, - "y": 0.7354784548909429, - "heading": 0.035675107933675586, - "angularVelocity": 0.03305333355128957, - "velocityX": 1.4762417390618812, - "velocityY": -0.06140630961271657, - "timestamp": 2.84225281598313 - }, - { - "x": 7.8767131917501745, - "y": 0.7339488795899547, - "heading": 0.034146712742278346, - "angularVelocity": -0.024475841661426835, - "velocityX": 1.2356861895988753, - "velocityY": -0.02449474002989417, - "timestamp": 2.90469786741545 - }, - { - "x": 7.938931274791619, - "y": 0.7352136619846016, - "heading": 0.02946667853743436, - "angularVelocity": -0.07494643846864563, - "velocityX": 0.9963653102100188, - "velocityY": 0.0202543254531208, - "timestamp": 2.9671429188477703 - }, - { - "x": 7.986282927383124, - "y": 0.739683296755121, - "heading": 0.022008823943331524, - "angularVelocity": -0.11943067421740607, - "velocityX": 0.7582931153932394, - "velocityY": 0.07157708526133728, - "timestamp": 3.0295879702800903 - }, - { - "x": 8.018844028188742, - "y": 0.7477062429275226, - "heading": 0.012093915253346623, - "angularVelocity": -0.1587781331356692, - "velocityX": 0.5214360475130281, - "velocityY": 0.1284800955140119, - "timestamp": 3.0920330217124103 - }, - { - "x": 8.036686897277832, - "y": 0.7595815062522888, - "heading": 3.7467909104623897e-28, - "angularVelocity": -0.19367291684360474, - "velocityX": 0.28573711895216997, - "velocityY": 0.19017140753959036, - "timestamp": 3.1544780731447304 - }, - { - "x": 8.033078547677619, - "y": 0.7841378633723926, - "heading": -0.020629184334771507, - "angularVelocity": -0.23689147615093625, - "velocityX": -0.041435824577044006, - "velocityY": 0.28198844863030675, - "timestamp": 3.241560920826543 - }, - { - "x": 8.000977407549723, - "y": 0.8166882948302093, - "heading": -0.04487258538731522, - "angularVelocity": -0.27839467470248114, - "velocityX": -0.3686275883534489, - "velocityY": 0.37378694340303564, - "timestamp": 3.328643768508355 - }, - { - "x": 7.940381596505895, - "y": 0.8572308856330779, - "heading": -0.0725538114207906, - "angularVelocity": -0.31787231091269064, - "velocityX": -0.6958409452253523, - "velocityY": 0.46556344770677466, - "timestamp": 3.4157266161901676 - }, - { - "x": 7.851288941262752, - "y": 0.9057633293863456, - "heading": -0.10346158740058668, - "angularVelocity": -0.35492380879330676, - "velocityX": -1.0230792586006634, - "velocityY": 0.5573134669481409, - "timestamp": 3.50280946387198 - }, - { - "x": 7.733696909453058, - "y": 0.9622827924317745, - "heading": -0.1373381842539412, - "angularVelocity": -0.38901572186907185, - "velocityX": -1.3503466519533016, - "velocityY": 0.649030946391906, - "timestamp": 3.5898923115537924 - }, - { - "x": 7.587602525764421, - "y": 1.026785702749848, - "heading": -0.173861756071492, - "angularVelocity": -0.41941177613991887, - "velocityX": -1.6776482117631761, - "velocityY": 0.7407074072009858, - "timestamp": 3.676975159235605 - }, - { - "x": 7.413002271708086, - "y": 1.0992674032204544, - "heading": -0.2126180954789335, - "angularVelocity": -0.4450513555671891, - "velocityX": -2.004990175497003, - "velocityY": 0.8323303888205812, - "timestamp": 3.764058006917417 - }, - { - "x": 7.209891989182915, - "y": 1.179721540530717, - "heading": -0.25305269619381565, - "angularVelocity": -0.46432336322560563, - "velocityX": -2.332379888026919, - "velocityY": 0.9238804133304188, - "timestamp": 3.8511408545992296 - }, - { - "x": 6.978266885073543, - "y": 1.2681388833729486, - "heading": -0.2943826693816009, - "angularVelocity": -0.47460520972854375, - "velocityX": -2.6598246414230293, - "velocityY": 1.0153244318019483, - "timestamp": 3.938223702281042 - }, - { - "x": 6.718122073357289, - "y": 1.3645047215826278, - "heading": -0.3354158360182677, - "angularVelocity": -0.4711968858275734, - "velocityX": -2.9873255025695, - "velocityY": 1.1065995287818986, - "timestamp": 4.025306549962854 - }, - { - "x": 6.429455909714376, - "y": 1.4687919159078104, - "heading": -0.3741111996126966, - "angularVelocity": -0.44435115093865635, - "velocityX": -3.314845245962294, - "velocityY": 1.1975629771115504, - "timestamp": 4.112389397644667 - }, - { - "x": 6.11229273419799, - "y": 1.5809350912859041, - "heading": -0.4061539482737321, - "angularVelocity": -0.3679570605926316, - "velocityX": -3.642085484793208, - "velocityY": 1.287775702832415, - "timestamp": 4.199472245326479 - }, - { - "x": 5.7670373625706794, - "y": 1.70063842953662, - "heading": -0.41585493727254547, - "angularVelocity": -0.11139953799236638, - "velocityX": -3.964677095641404, - "velocityY": 1.3745914544284772, - "timestamp": 4.286555093008292 - }, - { - "x": 5.412761186901786, - "y": 1.8003410365894708, - "heading": -0.415854945416732, - "angularVelocity": -9.352228080597314e-8, - "velocityX": -4.0682658537231795, - "velocityY": 1.1449167052638152, - "timestamp": 4.373637940690104 - }, - { - "x": 5.071022372606716, - "y": 1.8966285846840993, - "heading": -0.4602634606072584, - "angularVelocity": -0.5099570853813654, - "velocityX": -3.924295350833406, - "velocityY": 1.1057004985235321, - "timestamp": 4.460720788371916 - }, - { - "x": 4.757762067487565, - "y": 1.9848574922818703, - "heading": -0.5077454026581931, - "angularVelocity": -0.5452502222300594, - "velocityX": -3.597267584355503, - "velocityY": 1.0131605700372426, - "timestamp": 4.547803636053729 - }, - { - "x": 4.472993355873205, - "y": 2.0650467750158894, - "heading": -0.5536705784011021, - "angularVelocity": -0.5273733802403061, - "velocityX": -3.2700895663732, - "velocityY": 0.9208390041058229, - "timestamp": 4.634886483735541 - }, - { - "x": 4.216711388833469, - "y": 2.137205651490315, - "heading": -0.5965489946241969, - "angularVelocity": -0.4923864729339809, - "velocityX": -2.942967230196159, - "velocityY": 0.8286232983340568, - "timestamp": 4.721969331417354 - }, - { - "x": 3.9889121588577114, - "y": 2.2013393045187435, - "heading": -0.6356392489676235, - "angularVelocity": -0.44888580683829243, - "velocityX": -2.6158909135367425, - "velocityY": 0.7364671084570362, - "timestamp": 4.809052179099166 - }, - { - "x": 3.7895927667556286, - "y": 2.257451059564497, - "heading": -0.67049482646752, - "angularVelocity": -0.4002576675863153, - "velocityX": -2.28884788920048, - "velocityY": 0.6443491059316062, - "timestamp": 4.8961350267809784 - }, - { - "x": 3.6187510751100724, - "y": 2.3055432379933247, - "heading": -0.7008161056508533, - "angularVelocity": -0.34818887979091523, - "velocityX": -1.961829409504224, - "velocityY": 0.5522577603864, - "timestamp": 4.983217874462791 - }, - { - "x": 3.4763854585684455, - "y": 2.3456175525628025, - "heading": -0.7263878572428285, - "angularVelocity": -0.2936485458699124, - "velocityX": -1.6348295942481088, - "velocityY": 0.46018608298046837, - "timestamp": 5.070300722144603 - }, - { - "x": 3.3624946473645725, - "y": 2.3776753150411416, - "heading": -0.7470483845555387, - "angularVelocity": -0.2372513975220547, - "velocityX": -1.3078443601202956, - "velocityY": 0.3681294690255582, - "timestamp": 5.157383569826416 - }, - { - "x": 3.27707762853477, - "y": 2.401717555655836, - "heading": -0.7626725767311283, - "angularVelocity": -0.17941756145455898, - "velocityX": -0.9808707581762147, - "velocityY": 0.2760846855001934, - "timestamp": 5.244466417508228 - }, - { - "x": 3.220133581536875, - "y": 2.417745096695034, - "heading": -0.7731618414519866, - "angularVelocity": -0.12045155848812429, - "velocityX": -0.653906578778407, - "velocityY": 0.184049344570815, - "timestamp": 5.3315492651900405 - }, - { - "x": 3.191661834716797, - "y": 2.425758600234986, - "heading": -0.7784377394795863, - "angularVelocity": -0.06058481283107604, - "velocityX": -0.3269501121978633, - "velocityY": 0.09202160647332173, - "timestamp": 5.418632112871853 - }, - { - "x": 3.191661834716797, - "y": 2.425758600234986, - "heading": -0.7784377394795863, - "angularVelocity": -2.2213424468321708e-29, - "velocityX": 4.80853199627901e-31, - "velocityY": 5.887004206119992e-31, - "timestamp": 5.505714960553665 - }, - { - "x": 3.2018718927349297, - "y": 2.4192860517445967, - "heading": -0.775629671378581, - "angularVelocity": 0.05042136367130771, - "velocityX": 0.18333068498342892, - "velocityY": -0.11622037271718683, - "timestamp": 5.561406990608838 - }, - { - "x": 3.2224010944618575, - "y": 2.406516889109746, - "heading": -0.7699926416822923, - "angularVelocity": 0.10121788864051368, - "velocityX": 0.3686201006964548, - "velocityY": -0.22928168756284123, - "timestamp": 5.6170990206640115 - }, - { - "x": 3.2533689407857556, - "y": 2.3876518965280447, - "heading": -0.7615043709895374, - "angularVelocity": 0.15241446010040724, - "velocityX": 0.5560552612863737, - "velocityY": -0.3387377433182489, - "timestamp": 5.6727910507191845 - }, - { - "x": 3.2949065410422427, - "y": 2.3629220473113715, - "heading": -0.7501411239102694, - "angularVelocity": 0.2040372216277724, - "velocityX": 0.7458446067657482, - "velocityY": -0.4440464675497314, - "timestamp": 5.728483080774358 - }, - { - "x": 3.3471578282370227, - "y": 2.3325953799076995, - "heading": -0.7358776871721558, - "angularVelocity": 0.2561127099152785, - "velocityX": 0.9382183975519481, - "velocityY": -0.5445423227278213, - "timestamp": 5.784175110829531 - }, - { - "x": 3.41028066593418, - "y": 2.2969859549710607, - "heading": -0.7186874476189649, - "angularVelocity": 0.3086660611250982, - "velocityX": 1.1334267692275264, - "velocityY": -0.6393989391545056, - "timestamp": 5.839867140884704 - }, - { - "x": 3.4844475827329466, - "y": 2.256465654452846, - "heading": -0.6985426607798245, - "angularVelocity": 0.3617175890191665, - "velocityX": 1.331733045559483, - "velocityY": -0.727578083938971, - "timestamp": 5.895559170939877 - }, - { - "x": 3.569845617335609, - "y": 2.2114798809831826, - "heading": -0.6754150695245975, - "angularVelocity": 0.41527649885117823, - "velocityX": 1.5333977683711546, - "velocityY": -0.8077596278156253, - "timestamp": 5.95125120099505 - }, - { - "x": 3.666674280148722, - "y": 2.1625685982008305, - "heading": -0.6492771543323811, - "angularVelocity": 0.4693295461903955, - "velocityX": 1.738644878220203, - "velocityY": -0.87824564365666, - "timestamp": 6.006943231050223 - }, - { - "x": 3.775139743986139, - "y": 2.1103945662614825, - "heading": -0.6201045118785193, - "angularVelocity": 0.5238207769578656, - "velocityX": 1.9475940045633349, - "velocityY": -0.9368312106356999, - "timestamp": 6.062635261105396 - }, - { - "x": 3.895441739801367, - "y": 2.055780830444415, - "heading": -0.5878802296523478, - "angularVelocity": 0.5786156869169138, - "velocityX": 2.1601294780608042, - "velocityY": -0.9806382666058729, - "timestamp": 6.118327291160569 - }, - { - "x": 4.027746802463347, - "y": 1.9997587552569576, - "heading": -0.5526027278249325, - "angularVelocity": 0.6334389641114943, - "velocityX": 2.3756552334491494, - "velocityY": -1.0059262542945027, - "timestamp": 6.174019321215742 - }, - { - "x": 4.172137292392089, - "y": 1.9436241693843643, - "heading": -0.51429939372606, - "angularVelocity": 0.6877704774080914, - "velocityX": 2.5926598435305, - "velocityY": -1.0079464838502272, - "timestamp": 6.2297113512709155 - }, - { - "x": 4.3285218208265555, - "y": 1.8889883430033667, - "heading": -0.4730491306307316, - "angularVelocity": 0.7406852121293169, - "velocityX": 2.808023487014891, - "velocityY": -0.9810349223555088, - "timestamp": 6.285403381326089 - }, - { - "x": 4.49649793430264, - "y": 1.8377877666662683, - "heading": -0.4290162836436476, - "angularVelocity": 0.7906489841268347, - "velocityX": 3.0161607201187084, - "velocityY": -0.9193519483196195, - "timestamp": 6.341095411381262 - }, - { - "x": 4.675191879236026, - "y": 1.7921900095553935, - "heading": -0.38249210396514205, - "angularVelocity": 0.8353830814286095, - "velocityX": 3.2086089294348805, - "velocityY": -0.8187483391376037, - "timestamp": 6.396787441436435 - }, - { - "x": 4.863169718330775, - "y": 1.7543508609872742, - "heading": -0.33392159403718297, - "angularVelocity": 0.8721267635573858, - "velocityX": 3.375309517511239, - "velocityY": -0.6794356127911404, - "timestamp": 6.452479471491608 - }, - { - "x": 5.0585395666429065, - "y": 1.7260962541621017, - "heading": -0.2838765417100418, - "angularVelocity": 0.8986034855896275, - "velocityX": 3.5080396264704503, - "velocityY": -0.5073366296251212, - "timestamp": 6.508171501546781 - }, - { - "x": 5.259232445364841, - "y": 1.708716063515423, - "heading": -0.232969871935678, - "angularVelocity": 0.9140745942270582, - "velocityX": 3.603619378268493, - "velocityY": -0.312076802182647, - "timestamp": 6.563863531601954 - }, - { - "x": 5.4632887840271, - "y": 1.7029682397842407, - "heading": -0.18176811165848733, - "angularVelocity": 0.919373207018413, - "velocityX": 3.6640132970570867, - "velocityY": -0.10320729421226303, - "timestamp": 6.619555561657127 - }, - { - "x": 5.615161187166233, - "y": 1.7052796689747005, - "heading": -0.14398910637879186, - "angularVelocity": 0.9169644687547894, - "velocityX": 3.6862166283090447, - "velocityY": 0.05610254753936902, - "timestamp": 6.6607556370875285 - }, - { - "x": 5.767420990442743, - "y": 1.7142063964817436, - "heading": -0.10652675388990283, - "angularVelocity": 0.9092787354764283, - "velocityX": 3.695619527049639, - "velocityY": 0.21666774669193956, - "timestamp": 6.70195571251793 - }, - { - "x": 5.919386924300179, - "y": 1.7297521629533545, - "heading": -0.0696570985444518, - "angularVelocity": 0.8948929088184353, - "velocityX": 3.6884867872183342, - "velocityY": 0.3773237381050978, - "timestamp": 6.743155787948331 - }, - { - "x": 6.070179219983336, - "y": 1.751816393631274, - "heading": -0.03372721993940781, - "angularVelocity": 0.8720828355214922, - "velocityX": 3.66000047591873, - "velocityY": 0.5355385990783469, - "timestamp": 6.784355863378733 - }, - { - "x": 6.218691660613551, - "y": 1.7800947313312476, - "heading": 0.0008467859316837714, - "angularVelocity": 0.8391733633958196, - "velocityX": 3.6046642895373915, - "velocityY": 0.6863661632790825, - "timestamp": 6.825555938809134 - }, - { - "x": 6.363637018199303, - "y": 1.8139414045223912, - "heading": 0.03362320983867217, - "angularVelocity": 0.7955428130794786, - "velocityX": 3.5180847625049867, - "velocityY": 0.821519689892803, - "timestamp": 6.866756014239535 - }, - { - "x": 6.5037346457874685, - "y": 1.8522755217573394, - "heading": 0.06422742113546837, - "angularVelocity": 0.7428193025640265, - "velocityX": 3.4004216284707804, - "velocityY": 0.9304380352338247, - "timestamp": 6.907956089669937 - }, - { - "x": 6.63798940408483, - "y": 1.8936799802395343, - "heading": 0.09242547454575481, - "angularVelocity": 0.684417519038789, - "velocityX": 3.2586046723180027, - "velocityY": 1.004960744602, - "timestamp": 6.949156165100338 - }, - { - "x": 6.765843552605767, - "y": 1.936686507443115, - "heading": 0.1181284172233344, - "angularVelocity": 0.6238566898014307, - "velocityX": 3.1032503505222433, - "velocityY": 1.0438458365502405, - "timestamp": 6.99035624053074 - }, - { - "x": 6.887107406175693, - "y": 1.9800193223462106, - "heading": 0.14135707235090567, - "angularVelocity": 0.5638012766945281, - "velocityX": 2.943292028063759, - "velocityY": 1.0517654264079415, - "timestamp": 7.031556315961141 - }, - { - "x": 7.0018061689137365, - "y": 2.022672952124899, - "heading": 0.16219156310473354, - "angularVelocity": 0.505690597315122, - "velocityX": 2.7839454549494933, - "velocityY": 1.0352803807542028, - "timestamp": 7.072756391391542 - }, - { - "x": 7.110062105457345, - "y": 2.063884242340868, - "heading": 0.18073107684316084, - "angularVelocity": 0.44998737368201414, - "velocityX": 2.627566464689694, - "velocityY": 1.000272203034835, - "timestamp": 7.113956466821944 - }, - { - "x": 7.212029936432938, - "y": 2.1030762783452954, - "heading": 0.19707316527143803, - "angularVelocity": 0.39665190555011326, - "velocityX": 2.4749428225645964, - "velocityY": 0.951261268213787, - "timestamp": 7.155156542252345 - }, - { - "x": 7.307866875073466, - "y": 2.139807899939946, - "heading": 0.21130608363236952, - "angularVelocity": 0.3454585510401522, - "velocityX": 2.3261350286220495, - "velocityY": 0.89154258119504, - "timestamp": 7.1963566176827465 - }, - { - "x": 7.397720405161271, - "y": 2.173735875930798, - "heading": 0.22350713861237825, - "angularVelocity": 0.2961415689789137, - "velocityX": 2.1809069315805827, - "velocityY": 0.8234930552048578, - "timestamp": 7.237556693113148 - }, - { - "x": 7.481724262237549, - "y": 2.2045881748199463, - "heading": 0.2337432969576531, - "angularVelocity": 0.248449990402726, - "velocityX": 2.0389248368776234, - "velocityY": 0.7488408350432911, - "timestamp": 7.278756768543549 - }, - { - "x": 7.595794373586248, - "y": 2.243752616633879, - "heading": 0.24487156374946684, - "angularVelocity": 0.17836312173475907, - "velocityX": 1.8283081756947666, - "velocityY": 0.6277250746783941, - "timestamp": 7.341147842906794 - }, - { - "x": 7.696474893293651, - "y": 2.2757924724177725, - "heading": 0.2521501090469708, - "angularVelocity": 0.1166600410681812, - "velocityX": 1.613700689320329, - "velocityY": 0.5135326825333361, - "timestamp": 7.403538917270038 - }, - { - "x": 7.783579008927407, - "y": 2.3010567107975834, - "heading": 0.25599476725502607, - "angularVelocity": 0.06162192665046021, - "velocityX": 1.396098985675918, - "velocityY": 0.4049335363696611, - "timestamp": 7.465929991633282 - }, - { - "x": 7.856962202651826, - "y": 2.3198324584355756, - "heading": 0.25674317704545796, - "angularVelocity": 0.011995462461098809, - "velocityX": 1.17618095975041, - "velocityY": 0.3009364372967597, - "timestamp": 7.5283210659965265 - }, - { - "x": 7.91650981703959, - "y": 2.3323597342047413, - "heading": 0.2546745141891388, - "angularVelocity": -0.03315639099713727, - "velocityX": 0.9544252121878052, - "velocityY": 0.2007863447939911, - "timestamp": 7.590712140359771 - }, - { - "x": 7.962128947539202, - "y": 2.3388419529288034, - "heading": 0.2500232368325733, - "angularVelocity": -0.07455036484041708, - "velocityX": 0.731180396638371, - "velocityY": 0.10389657158846945, - "timestamp": 7.653103214723015 - }, - { - "x": 7.993742963409653, - "y": 2.339453579074058, - "heading": 0.24298889583795466, - "angularVelocity": -0.11274595070545464, - "velocityX": 0.5067073486568353, - "velocityY": 0.009803103272327605, - "timestamp": 7.7154942890862594 - }, - { - "x": 8.011287689208984, - "y": 2.334345817565918, - "heading": 0.2337432969576531, - "angularVelocity": -0.14818784537148968, - "velocityX": 0.28120570094987624, - "velocityY": -0.08186686253232739, - "timestamp": 7.777885363449504 - }, - { - "x": 8.013057457291007, - "y": 2.3214137467214835, - "heading": 0.22064239230319685, - "angularVelocity": -0.1855819214437055, - "velocityX": 0.02506979249403061, - "velocityY": -0.18319029249174376, - "timestamp": 7.848479010462854 - }, - { - "x": 7.99666998493903, - "y": 2.3015233616034343, - "heading": 0.20489547559952764, - "angularVelocity": -0.22306421852225894, - "velocityX": -0.232138061217864, - "velocityY": -0.28175885450835847, - "timestamp": 7.919072657476204 - }, - { - "x": 7.962040754952174, - "y": 2.2748995096887543, - "heading": 0.186495589242378, - "angularVelocity": -0.2606450741051799, - "velocityX": -0.4905431501549616, - "velocityY": -0.37714232145627924, - "timestamp": 7.989666304489554 - }, - { - "x": 7.9090749698086515, - "y": 2.2418049871920993, - "heading": 0.16543502379528896, - "angularVelocity": -0.29833513833201103, - "velocityX": -0.7502911010321619, - "velocityY": -0.4688031274315115, - "timestamp": 8.060259951502905 - }, - { - "x": 7.83766590050416, - "y": 2.202550806257141, - "heading": 0.14170533667281937, - "angularVelocity": -0.3361447966837854, - "velocityX": -1.0115509302272958, - "velocityY": -0.5560582658030753, - "timestamp": 8.130853598516255 - }, - { - "x": 7.747693073646214, - "y": 2.1575104269397727, - "heading": 0.11529745438529491, - "angularVelocity": -0.37408298628529985, - "velocityX": -1.2745173349795946, - "velocityY": -0.6380231256341062, - "timestamp": 8.201447245529605 - }, - { - "x": 7.639020495122075, - "y": 2.1071400026715006, - "heading": 0.08620194014422251, - "angularVelocity": -0.4121548534752705, - "velocityX": -1.5394101753035547, - "velocityY": -0.7135263072432857, - "timestamp": 8.272040892542956 - }, - { - "x": 7.511495469210905, - "y": 2.052008041294553, - "heading": 0.054409589277409784, - "angularVelocity": -0.4503571101915203, - "velocityX": -1.8064660391756437, - "velocityY": -0.7809762451644056, - "timestamp": 8.342634539556306 - }, - { - "x": 7.364949479960618, - "y": 1.9928403305635423, - "heading": 0.01991269014230069, - "angularVelocity": -0.4886686068023256, - "velocityX": -2.075909029357446, - "velocityY": -0.8381449781142655, - "timestamp": 8.413228186569656 - }, - { - "x": 7.199204960601857, - "y": 1.9305905238043142, - "heading": -0.01729230931714009, - "angularVelocity": -0.5270304203493653, - "velocityX": -2.347867355931005, - "velocityY": -0.8818046579666796, - "timestamp": 8.483821833583006 - }, - { - "x": 7.014098188628079, - "y": 1.8665552887004158, - "heading": -0.05719900816860063, - "angularVelocity": -0.5653015609735185, - "velocityX": -2.6221449068748512, - "velocityY": -0.9070962871742325, - "timestamp": 8.554415480596356 - }, - { - "x": 6.809546840003655, - "y": 1.8025678316240845, - "heading": -0.09977788205082659, - "angularVelocity": -0.6031544718772441, - "velocityX": -2.89758862558468, - "velocityY": -0.9064194836715306, - "timestamp": 8.625009127609706 - }, - { - "x": 6.5857441999493975, - "y": 1.7413205913498453, - "heading": -0.14494480394528797, - "angularVelocity": -0.6398156747153125, - "velocityX": -3.1702943469110325, - "velocityY": -0.8676027215686496, - "timestamp": 8.695602774623056 - }, - { - "x": 6.343702784750113, - "y": 1.6868213690291616, - "heading": -0.19248452084491505, - "angularVelocity": -0.6734276937220217, - "velocityX": -3.42865718714759, - "velocityY": -0.77201312903379, - "timestamp": 8.766196421636407 - }, - { - "x": 6.0865152782234535, - "y": 1.6445255287826241, - "heading": -0.24188828313696017, - "angularVelocity": -0.699832979059746, - "velocityX": -3.6432103653466354, - "velocityY": -0.5991451360848178, - "timestamp": 8.836790068649757 - }, - { - "x": 5.820539703788856, - "y": 1.6195865880270663, - "heading": -0.292193130463703, - "angularVelocity": -0.7125973717894101, - "velocityX": -3.767698450036126, - "velocityY": -0.35327457654711547, - "timestamp": 8.907383715663107 - }, - { - "x": 5.553319270967186, - "y": 1.61406380660166, - "heading": -0.3422542788791351, - "angularVelocity": -0.7091452352074223, - "velocityX": -3.7853325919134613, - "velocityY": -0.07823340568255462, - "timestamp": 8.977977362676457 - }, - { - "x": 5.290481661115068, - "y": 1.6274952853330582, - "heading": -0.39122534936664, - "angularVelocity": -0.693703648406827, - "velocityX": -3.72324735967261, - "velocityY": 0.19026469519074493, - "timestamp": 9.048571009689807 - }, - { - "x": 5.035514831542969, - "y": 1.6587157249450684, - "heading": -0.43859871161755776, - "angularVelocity": -0.6710711835295708, - "velocityX": -3.6117531868537824, - "velocityY": 0.44225565518814863, - "timestamp": 9.119164656703157 - }, - { - "x": 4.840429456104478, - "y": 1.693764115895758, - "heading": -0.47482255342493607, - "angularVelocity": -0.6498265287292678, - "velocityX": -3.4996744133643607, - "velocityY": 0.628739887672373, - "timestamp": 9.174908520959075 - }, - { - "x": 4.653407145998877, - "y": 1.737869407208374, - "heading": -0.5094986634723291, - "angularVelocity": -0.6220614682935635, - "velocityX": -3.355029519428204, - "velocityY": 0.7912133810840754, - "timestamp": 9.230652385214993 - }, - { - "x": 4.476082725221748, - "y": 1.7892498770756833, - "heading": -0.542310474310624, - "angularVelocity": -0.5886174429468579, - "velocityX": -3.1810572005385227, - "velocityY": 0.9217242211882539, - "timestamp": 9.286396249470911 - }, - { - "x": 4.309697720613771, - "y": 1.8458506852049763, - "heading": -0.5730328703053699, - "angularVelocity": -0.5511350245419085, - "velocityX": -2.984812890690747, - "velocityY": 1.0153728824654409, - "timestamp": 9.34214011372683 - }, - { - "x": 4.155016964634681, - "y": 1.905597573014691, - "heading": -0.601541437642242, - "angularVelocity": -0.5114207225747871, - "velocityX": -2.774848102904345, - "velocityY": 1.071811016463078, - "timestamp": 9.397883977982747 - }, - { - "x": 4.012389650881917, - "y": 1.966603698419971, - "heading": -0.6277908857969042, - "angularVelocity": -0.47089394510133564, - "velocityX": -2.5586190633998585, - "velocityY": 1.0944007240905271, - "timestamp": 9.453627842238665 - }, - { - "x": 3.8818763982479205, - "y": 2.02726549778738, - "heading": -0.6517856604184045, - "angularVelocity": -0.43044691898899096, - "velocityX": -2.3413025698185628, - "velocityY": 1.0882237924682305, - "timestamp": 9.509371706494584 - }, - { - "x": 3.763368379878263, - "y": 2.0862691727799794, - "heading": -0.6735568165305625, - "angularVelocity": -0.39055699497630253, - "velocityX": -2.1259383423006524, - "velocityY": 1.0584783774895203, - "timestamp": 9.565115570750502 - }, - { - "x": 3.65667108287158, - "y": 2.142554070867054, - "heading": -0.6931474445007211, - "angularVelocity": -0.35144007742661887, - "velocityX": -1.914063519472572, - "velocityY": 1.009705710904315, - "timestamp": 9.62085943500642 - }, - { - "x": 3.5615549777894695, - "y": 2.1952651611601923, - "heading": -0.7106044963543238, - "angularVelocity": -0.3131654413741073, - "velocityX": -1.7063062697884914, - "velocityY": 0.9455944792622217, - "timestamp": 9.676603299262338 - }, - { - "x": 3.4777834556344756, - "y": 2.2437091911618374, - "heading": -0.7259745587811693, - "angularVelocity": -0.2757265329917277, - "velocityX": -1.5027935948323077, - "velocityY": 0.8690468565157425, - "timestamp": 9.732347163518256 - }, - { - "x": 3.4051270953206445, - "y": 2.2873188436103873, - "heading": -0.7393018511038773, - "angularVelocity": -0.23908088362017574, - "velocityX": -1.3033965492644939, - "velocityY": 0.7823220193049412, - "timestamp": 9.788091027774174 - }, - { - "x": 3.3433702555456972, - "y": 2.325624884061789, - "heading": -0.7506274140344208, - "angularVelocity": -0.20317147154615087, - "velocityX": -1.1078679348712595, - "velocityY": 0.6871794943303605, - "timestamp": 9.843834892030092 - }, - { - "x": 3.2923135378300334, - "y": 2.358234947831376, - "heading": -0.7599889096515458, - "angularVelocity": -0.16793768681243185, - "velocityX": -0.9159163685040683, - "velocityY": 0.5849982631249935, - "timestamp": 9.89957875628601 - }, - { - "x": 3.251774113497663, - "y": 2.384817462963455, - "heading": -0.7674207191873286, - "angularVelocity": -0.13332067367385547, - "velocityX": -0.7272446012399799, - "velocityY": 0.4768688982529085, - "timestamp": 9.955322620541928 - }, - { - "x": 3.2215850043710765, - "y": 2.405089430282806, - "heading": -0.7729541729065691, - "angularVelocity": -0.0992657002362922, - "velocityX": -0.5415682879103932, - "velocityY": 0.36366275625032934, - "timestamp": 10.011066484797846 - }, - { - "x": 3.2015939022173066, - "y": 2.4188070703406934, - "heading": -0.7766178262935338, - "angularVelocity": -0.06572298917321301, - "velocityX": -0.3586242615329249, - "velocityY": 0.24608340740265958, - "timestamp": 10.066810349053764 - }, - { - "x": 3.191661834716797, - "y": 2.425758600234986, - "heading": -0.7784377394795863, - "angularVelocity": -0.03264777586457403, - "velocityX": -0.17817328656858386, - "velocityY": 0.124704843969519, - "timestamp": 10.122554213309682 - }, - { - "x": 3.191661834716797, - "y": 2.425758600234986, - "heading": -0.7784377394795863, - "angularVelocity": -3.751194855513336e-35, - "velocityX": 1.1060996014173732e-35, - "velocityY": 1.8550525441659548e-35, - "timestamp": 10.1782980775656 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGFSprint.1.traj b/src/main/deploy/choreo/SourceLanePHGFSprint.1.traj deleted file mode 100644 index 77d7b28a..00000000 --- a/src/main/deploy/choreo/SourceLanePHGFSprint.1.traj +++ /dev/null @@ -1,427 +0,0 @@ -{ - "samples": [ - { - "x": 1.433172345161438, - "y": 2.136303424835205, - "heading": 2.8770383139324086e-29, - "angularVelocity": 0, - "velocityX": 1.701446192977867e-30, - "velocityY": -1.266914834653595e-29, - "timestamp": 0 - }, - { - "x": 1.4678872089966617, - "y": 2.127700564422531, - "heading": 0.004117315336523342, - "angularVelocity": 0.04300890670031375, - "velocityX": 0.3626266675757136, - "velocityY": -0.08986429034885272, - "timestamp": 0.09573169030115974 - }, - { - "x": 1.537317326749348, - "y": 2.110494823513309, - "heading": 0.0122988440120433, - "angularVelocity": 0.0854631172768069, - "velocityX": 0.7252574098965585, - "velocityY": -0.17972879049106513, - "timestamp": 0.1914633806023195 - }, - { - "x": 1.6414631627627712, - "y": 2.0846861763424815, - "heading": 0.024480626754227467, - "angularVelocity": 0.1272492181380779, - "velocityX": 1.0878930026830405, - "velocityY": -0.26959355977151667, - "timestamp": 0.28719507090347923 - }, - { - "x": 1.780325283643077, - "y": 2.0502745948695433, - "heading": 0.04058342242477801, - "angularVelocity": 0.16820757702525263, - "velocityX": 1.450534514158978, - "velocityY": -0.35945862195920303, - "timestamp": 0.382926761204639 - }, - { - "x": 1.9539044005410915, - "y": 2.0072600541327383, - "heading": 0.060505710064861905, - "angularVelocity": 0.20810546201361826, - "velocityX": 1.8131834542181169, - "velocityY": -0.44932394489394517, - "timestamp": 0.4786584515057987 - }, - { - "x": 2.1622014359513058, - "y": 1.9556425412505571, - "heading": 0.08411192731013951, - "angularVelocity": 0.2465872812881091, - "velocityX": 2.175842030527155, - "velocityY": -0.5391894023871501, - "timestamp": 0.5743901418069585 - }, - { - "x": 2.4052176349352066, - "y": 1.8954220721835344, - "heading": 0.11121111812581518, - "angularVelocity": 0.28307440022634617, - "velocityX": 2.5385136125763252, - "velocityY": -0.6290546931588193, - "timestamp": 0.6701218321081182 - }, - { - "x": 2.682954761527793, - "y": 1.8265987279247267, - "heading": 0.1415138814765405, - "angularVelocity": 0.3165384759644085, - "velocityX": 2.9012036214911956, - "velocityY": -0.7189191378882197, - "timestamp": 0.765853522409278 - }, - { - "x": 2.9954154621622653, - "y": 1.749172751537101, - "heading": 0.17453107861154096, - "angularVelocity": 0.3448930759444131, - "velocityX": 3.2639212746813424, - "velocityY": -0.8087810436154711, - "timestamp": 0.8615852127104378 - }, - { - "x": 3.3426038443567645, - "y": 1.6631449161907195, - "heading": 0.20926651570903568, - "angularVelocity": 0.36284157300443215, - "velocityX": 3.6266818344380534, - "velocityY": -0.8986348729151596, - "timestamp": 0.9573169030115976 - }, - { - "x": 3.724521896188089, - "y": 1.5685193860859707, - "heading": 0.24264187046069935, - "angularVelocity": 0.3486343408669774, - "velocityX": 3.989463161370044, - "velocityY": -0.9884452035533644, - "timestamp": 1.0530485933127574 - }, - { - "x": 4.117233745301501, - "y": 1.4711990601083857, - "heading": 0.24264187604727303, - "angularVelocity": 5.835657624563612e-8, - "velocityX": 4.102213675301073, - "velocityY": -1.0165946686272007, - "timestamp": 1.1487802836139172 - }, - { - "x": 4.509945594349689, - "y": 1.373878733867449, - "heading": 0.24264188163378278, - "angularVelocity": 5.835590832713292e-8, - "velocityX": 4.102213674619734, - "velocityY": -1.0165946713781402, - "timestamp": 1.244511973915077 - }, - { - "x": 4.902657443397878, - "y": 1.27655840762652, - "heading": 0.24264188722029242, - "angularVelocity": 5.835590717483892e-8, - "velocityX": 4.102213674619755, - "velocityY": -1.0165946713780565, - "timestamp": 1.3402436642162368 - }, - { - "x": 5.295369292588233, - "y": 1.1792380819592594, - "heading": 0.24264189280680207, - "angularVelocity": 5.8355907494596246e-8, - "velocityX": 4.102213676104793, - "velocityY": -1.0165946653856088, - "timestamp": 1.4359753545173966 - }, - { - "x": 5.688086680864775, - "y": 1.0819401068348615, - "heading": 0.242641898394392, - "angularVelocity": 5.83671917480604e-8, - "velocityX": 4.102271536518705, - "velocityY": -1.0163611951659655, - "timestamp": 1.5317070448185564 - }, - { - "x": 6.057380727938785, - "y": 1.0111000403294017, - "heading": 0.24430232120402645, - "angularVelocity": 0.017344547115249777, - "velocityX": 3.8575945532093883, - "velocityY": -0.7399855394762825, - "timestamp": 1.6274387351197162 - }, - { - "x": 6.391965385523026, - "y": 0.9488653299205841, - "heading": 0.24119901543014724, - "angularVelocity": -0.03241670299487622, - "velocityX": 3.495025069879339, - "velocityY": -0.6500951795986862, - "timestamp": 1.723170425420876 - }, - { - "x": 6.691840373882702, - "y": 0.895234975869734, - "heading": 0.2333347965847716, - "angularVelocity": -0.08214854265046012, - "velocityX": 3.1324526644833015, - "velocityY": -0.5602152628325792, - "timestamp": 1.8189021157220358 - }, - { - "x": 6.957005576380099, - "y": 0.8502086242600702, - "heading": 0.2207147600128324, - "angularVelocity": -0.1318271570356455, - "velocityX": 2.7698790407075005, - "velocityY": -0.4703390430254154, - "timestamp": 1.9146338060231956 - }, - { - "x": 7.18746090816696, - "y": 0.8137860741035156, - "heading": 0.20334602215443712, - "angularVelocity": -0.18143143407058088, - "velocityX": 2.4073045306193745, - "velocityY": -0.38046492270850996, - "timestamp": 2.010365496324355 - }, - { - "x": 7.383206290326008, - "y": 0.7859671775168647, - "heading": 0.18123751114726283, - "angularVelocity": -0.2309424490133121, - "velocityX": 2.0447291961727903, - "velocityY": -0.29059234715707755, - "timestamp": 2.1060971866255147 - }, - { - "x": 7.544241641769053, - "y": 0.7667518051986745, - "heading": 0.15439971280375697, - "angularVelocity": -0.28034393060874496, - "velocityX": 1.6821530146905204, - "velocityY": -0.2007211222692816, - "timestamp": 2.2018288769266743 - }, - { - "x": 7.670566876398333, - "y": 0.7561398304498085, - "heading": 0.12284435298340343, - "angularVelocity": -0.32962292549710004, - "velocityX": 1.31957593384315, - "velocityY": -0.11085122086217061, - "timestamp": 2.297560567227834 - }, - { - "x": 7.762181902442319, - "y": 0.7541311201311741, - "heading": 0.08658400694043104, - "angularVelocity": -0.37877056101837964, - "velocityX": 0.9569978943632882, - "velocityY": -0.020982710206882894, - "timestamp": 2.3932922575289934 - }, - { - "x": 7.81908662296772, - "y": 0.7607255287810747, - "heading": 0.045631629218029345, - "angularVelocity": -0.42778287517258756, - "velocityX": 0.5944188423317792, - "velocityY": 0.06888428097620071, - "timestamp": 2.489023947830153 - }, - { - "x": 7.841280937194824, - "y": 0.7759228944778442, - "heading": 0, - "angularVelocity": -0.4766616892938097, - "velocityX": 0.2318387375694728, - "velocityY": 0.15874958074840423, - "timestamp": 2.5847556381313126 - }, - { - "x": 7.835403168803218, - "y": 0.7947828328368709, - "heading": -0.04166817387070667, - "angularVelocity": -0.5176676769900772, - "velocityX": -0.0730228955314616, - "velocityY": 0.2343078558429211, - "timestamp": 2.6652477679723754 - }, - { - "x": 7.804986392936784, - "y": 0.8197245003639376, - "heading": -0.0866323351495184, - "angularVelocity": -0.5586156232617521, - "velocityX": -0.3778850916668647, - "velocityY": 0.30986467339008555, - "timestamp": 2.745739897813438 - }, - { - "x": 7.750030563561768, - "y": 0.8507477655512394, - "heading": -0.13488866933437027, - "angularVelocity": -0.5995161797814921, - "velocityX": -0.6827478597060769, - "velocityY": 0.38541985714018384, - "timestamp": 2.826232027654501 - }, - { - "x": 7.6705356351825085, - "y": 0.8878524818601358, - "heading": -0.18643456038885645, - "angularVelocity": -0.640384235777116, - "velocityX": -0.9876111929640158, - "velocityY": 0.4609732203565098, - "timestamp": 2.906724157495564 - }, - { - "x": 7.5665015641225395, - "y": 0.9310384864662067, - "heading": -0.24126895121607964, - "angularVelocity": -0.6812391588502915, - "velocityX": -1.292475068839824, - "velocityY": 0.5365245607114574, - "timestamp": 2.9872162873366266 - }, - { - "x": 7.437928309633229, - "y": 0.9803055976729939, - "heading": -0.29939271538639584, - "angularVelocity": -0.7221049347901444, - "velocityX": -1.5973394509593273, - "velocityY": 0.6120736437468451, - "timestamp": 3.0677084171776894 - }, - { - "x": 7.284815834214756, - "y": 1.0356536080910796, - "heading": -0.36080902567537027, - "angularVelocity": -0.7630101279446713, - "velocityX": -1.9022042989606411, - "velocityY": 0.6876201502642577, - "timestamp": 3.148200547018752 - }, - { - "x": 7.107164100687513, - "y": 1.0970822617404554, - "heading": -0.4255236836274993, - "angularVelocity": -0.8039873970214162, - "velocityX": -2.207069608865607, - "velocityY": 0.763163476594831, - "timestamp": 3.228692676859815 - }, - { - "x": 6.904973048833601, - "y": 1.1645911316931183, - "heading": -0.49354522243909305, - "angularVelocity": -0.8450706789009879, - "velocityX": -2.5119356668921826, - "velocityY": 0.8387014989532872, - "timestamp": 3.309184806700878 - }, - { - "x": 6.684120642089045, - "y": 1.2193164102467027, - "heading": -0.5235123583408667, - "angularVelocity": -0.3722989559393653, - "velocityX": -2.7437764062379517, - "velocityY": 0.6798835943938059, - "timestamp": 3.3896769365419406 - }, - { - "x": 6.487807289631828, - "y": 1.267960625172924, - "heading": -0.5501610284793903, - "angularVelocity": -0.3310717481395217, - "velocityX": -2.4389136285238937, - "velocityY": 0.6043350452407918, - "timestamp": 3.4701690663830034 - }, - { - "x": 6.316033066880454, - "y": 1.310524076323902, - "heading": -0.5734871955472854, - "angularVelocity": -0.2897943825495106, - "velocityX": -2.134049913842353, - "velocityY": 0.5287902213231489, - "timestamp": 3.550661196224066 - }, - { - "x": 6.168797996381023, - "y": 1.3470068919007985, - "heading": -0.5934869971532756, - "angularVelocity": -0.2484690322524527, - "velocityX": -1.8291859190576596, - "velocityY": 0.45324699012227004, - "timestamp": 3.631153326065129 - }, - { - "x": 6.046102088284196, - "y": 1.3774091505678683, - "heading": -0.6101571315963537, - "angularVelocity": -0.20710266302584004, - "velocityX": -1.5243217981654285, - "velocityY": 0.3777047362126793, - "timestamp": 3.7116454559061918 - }, - { - "x": 5.947945348853021, - "y": 1.401730906160479, - "heading": -0.6234950110327551, - "angularVelocity": -0.16570414352854168, - "velocityX": -1.219457599463783, - "velocityY": 0.30216315113081255, - "timestamp": 3.7921375857472546 - }, - { - "x": 5.874327783550542, - "y": 1.4199721961221465, - "heading": -0.6334988572334701, - "angularVelocity": -0.12428353207005839, - "velocityX": -0.914593332891598, - "velocityY": 0.2266220312312794, - "timestamp": 3.8726297155883174 - }, - { - "x": 5.825249398499615, - "y": 1.4321330452450582, - "heading": -0.6401677660826286, - "angularVelocity": -0.08285168826039573, - "velocityX": -0.6097289902516339, - "velocityY": 0.15108121933934396, - "timestamp": 3.95312184542938 - }, - { - "x": 5.800710201263428, - "y": 1.4382134675979614, - "heading": -0.6435017481374367, - "angularVelocity": -0.04141997560898668, - "velocityX": -0.30486455364984805, - "velocityY": 0.07554058223155366, - "timestamp": 4.033613975270443 - }, - { - "x": 5.800710201263428, - "y": 1.4382134675979614, - "heading": -0.6435017481374367, - "angularVelocity": -1.8014229783540224e-27, - "velocityX": -3.376915136198235e-29, - "velocityY": 5.736032048058663e-30, - "timestamp": 4.114106105111506 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGFSprint.2.traj b/src/main/deploy/choreo/SourceLanePHGFSprint.2.traj deleted file mode 100644 index 72fbdf6b..00000000 --- a/src/main/deploy/choreo/SourceLanePHGFSprint.2.traj +++ /dev/null @@ -1,400 +0,0 @@ -{ - "samples": [ - { - "x": 5.800710201263428, - "y": 1.4382134675979614, - "heading": -0.6435017481374367, - "angularVelocity": -1.8014229783540224e-27, - "velocityX": -3.376915136198235e-29, - "velocityY": 5.736032048058663e-30, - "timestamp": 0 - }, - { - "x": 5.818547555700618, - "y": 1.4390540038628774, - "heading": -0.6376275065659812, - "angularVelocity": 0.08671691701318579, - "velocityX": 0.26331916480604495, - "velocityY": 0.012408191363030088, - "timestamp": 0.0677404337444587 - }, - { - "x": 5.854212795257727, - "y": 1.4409016567123936, - "heading": -0.6258375764635414, - "angularVelocity": 0.1740456836594175, - "velocityX": 0.5264985413534782, - "velocityY": 0.0272754800550328, - "timestamp": 0.1354808674889174 - }, - { - "x": 5.907692159338857, - "y": 1.443961759263086, - "heading": -0.6080852705803181, - "angularVelocity": 0.2620636583195098, - "velocityX": 0.7894747807915273, - "velocityY": 0.04517394385510083, - "timestamp": 0.20322130123337612 - }, - { - "x": 5.978964930251624, - "y": 1.4484936039085805, - "heading": -0.5843154945280162, - "angularVelocity": 0.3508949491225589, - "velocityX": 1.0521451808477307, - "velocityY": 0.06690014213062502, - "timestamp": 0.2709617349778348 - }, - { - "x": 6.067998493941586, - "y": 1.4548346984588374, - "heading": -0.5544599081943509, - "angularVelocity": 0.44073509251935744, - "velocityX": 1.31433412466517, - "velocityY": 0.09360870900499038, - "timestamp": 0.33870216872229353 - }, - { - "x": 6.174738434598293, - "y": 1.4634418593409828, - "heading": -0.5184292628909575, - "angularVelocity": 0.5318927457611795, - "velocityX": 1.575719769663251, - "velocityY": 0.12706090596665875, - "timestamp": 0.40644260246675223 - }, - { - "x": 6.2990864932887405, - "y": 1.4749664049356752, - "heading": -0.47610067620246677, - "angularVelocity": 0.6248644177297312, - "velocityX": 1.8356548934943828, - "velocityY": 0.1701280159818153, - "timestamp": 0.47418303621121094 - }, - { - "x": 6.440844047185778, - "y": 1.490406322856265, - "heading": -0.42729541631250667, - "angularVelocity": 0.7204745702407376, - "velocityX": 2.0926579010668633, - "velocityY": 0.22792765069729165, - "timestamp": 0.5419234699556696 - }, - { - "x": 6.59953463470863, - "y": 1.5114574086941215, - "heading": -0.371739955153997, - "angularVelocity": 0.8201226075416767, - "velocityX": 2.3426272722358106, - "velocityY": 0.31076101338926715, - "timestamp": 0.6096639037001284 - }, - { - "x": 6.773654182639981, - "y": 1.5414451590022467, - "heading": -0.3090282649921381, - "angularVelocity": 0.9257645204698558, - "velocityX": 2.570393165597269, - "velocityY": 0.4426861277748774, - "timestamp": 0.6774043374445871 - }, - { - "x": 6.956422392745004, - "y": 1.5870356897360083, - "heading": -0.23940243254396396, - "angularVelocity": 1.027832693112474, - "velocityX": 2.698066723258537, - "velocityY": 0.6730179925588539, - "timestamp": 0.7451447711890458 - }, - { - "x": 7.13098273232138, - "y": 1.6485480325873274, - "heading": -0.1686125340995527, - "angularVelocity": 1.0450169054342962, - "velocityX": 2.576900234133126, - "velocityY": 0.9080594772003634, - "timestamp": 0.8128852049335045 - }, - { - "x": 7.290659982684113, - "y": 1.7200152367861081, - "heading": -0.1002049626024793, - "angularVelocity": 1.0098484422926985, - "velocityX": 2.357192618002618, - "velocityY": 1.0550154501280693, - "timestamp": 0.8806256386779632 - }, - { - "x": 7.433825813884328, - "y": 1.798376978611666, - "heading": -0.035376660914373755, - "angularVelocity": 0.9570104309142923, - "velocityX": 2.1134472173633743, - "velocityY": 1.1567942142379297, - "timestamp": 0.9483660724224219 - }, - { - "x": 7.559908602192434, - "y": 1.882036919563645, - "heading": 0.025328755206427432, - "angularVelocity": 0.8961474375821659, - "velocityX": 1.8612633746004037, - "velocityY": 1.235007459024761, - "timestamp": 1.0161065061668806 - }, - { - "x": 7.6686462166126725, - "y": 1.9700426296704405, - "heading": 0.08160707257360432, - "angularVelocity": 0.8307935786103601, - "velocityX": 1.6052098932586671, - "velocityY": 1.2991607115889543, - "timestamp": 1.0838469399113393 - }, - { - "x": 7.759896555956456, - "y": 2.061767068594048, - "heading": 0.13326449164713097, - "angularVelocity": 0.762578805863526, - "velocityX": 1.347058681200838, - "velocityY": 1.354057449198286, - "timestamp": 1.151587373655798 - }, - { - "x": 7.833573521261895, - "y": 2.156767827393036, - "heading": 0.1801664809201526, - "angularVelocity": 0.6923780477986421, - "velocityX": 1.0876364562910028, - "velocityY": 1.4024232433669601, - "timestamp": 1.2193278074002567 - }, - { - "x": 7.8896206787548335, - "y": 2.2547166504175005, - "heading": 0.22221394940579448, - "angularVelocity": 0.6207144855945275, - "velocityX": 0.8273811429133712, - "velocityY": 1.4459432514701964, - "timestamp": 1.2870682411447154 - }, - { - "x": 7.9279988186375325, - "y": 2.3553605545122482, - "heading": 0.2593308440003242, - "angularVelocity": 0.547928209827353, - "velocityX": 0.566547005404121, - "velocityY": 1.485728663539606, - "timestamp": 1.3548086748891741 - }, - { - "x": 7.948679447174072, - "y": 2.45849871635437, - "heading": 0.2914571610744201, - "angularVelocity": 0.47425614656215387, - "velocityX": 0.30529223675411965, - "velocityY": 1.5225494751214026, - "timestamp": 1.4225491086336328 - }, - { - "x": 7.946831051825066, - "y": 2.5911250937356964, - "heading": 0.3237506602012709, - "angularVelocity": 0.38120746378091935, - "velocityX": -0.02181931726540404, - "velocityY": 1.5655833625644262, - "timestamp": 1.5072628201820768 - }, - { - "x": 7.917248965751211, - "y": 2.7272096797694707, - "heading": 0.34809099666884863, - "angularVelocity": 0.28732463756659415, - "velocityX": -0.34920068467238213, - "velocityY": 1.6064056638098445, - "timestamp": 1.5919765317305208 - }, - { - "x": 7.859906635586575, - "y": 2.8665210553288647, - "heading": 0.3644006813152481, - "angularVelocity": 0.19252709329200723, - "velocityX": -0.6768955003446485, - "velocityY": 1.6444961861897385, - "timestamp": 1.6766902432789648 - }, - { - "x": 7.774773151306567, - "y": 3.008766263060034, - "heading": 0.37259167904917256, - "angularVelocity": 0.09669034190811442, - "velocityX": -1.004955192304655, - "velocityY": 1.679128503888361, - "timestamp": 1.7614039548274087 - }, - { - "x": 7.6618127476379465, - "y": 3.1535627303797917, - "heading": 0.3725589270974435, - "angularVelocity": -0.00038661925124551516, - "velocityX": -1.333437074162696, - "velocityY": 1.7092447571129605, - "timestamp": 1.8461176663758527 - }, - { - "x": 7.520985197062152, - "y": 3.300390177076741, - "heading": 0.3641689647090754, - "angularVelocity": -0.09903901310675314, - "velocityX": -1.6623938203352226, - "velocityY": 1.7332193810559793, - "timestamp": 1.9308313779242967 - }, - { - "x": 7.352249696631812, - "y": 3.448501059595182, - "heading": 0.34723906580696273, - "angularVelocity": -0.19984839045130479, - "velocityX": -1.991832223451176, - "velocityY": 1.748369653639153, - "timestamp": 2.0155450894727407 - }, - { - "x": 7.155582548717868, - "y": 3.5967336644018815, - "heading": 0.32149579845629134, - "angularVelocity": -0.30388548536148036, - "velocityX": -2.3215503643879303, - "velocityY": 1.7498065200688502, - "timestamp": 2.1002588010211847 - }, - { - "x": 6.931058620807543, - "y": 3.7430527877883204, - "heading": 0.28648334175757967, - "angularVelocity": -0.4133033018945168, - "velocityX": -2.6503847347300935, - "velocityY": 1.7272188965863609, - "timestamp": 2.1849725125696287 - }, - { - "x": 6.679342452608786, - "y": 3.8831106231055927, - "heading": 0.24134794483523736, - "angularVelocity": -0.5327991903238861, - "velocityX": -2.9713745696859344, - "velocityY": 1.6533077438966797, - "timestamp": 2.2696862241180726 - }, - { - "x": 6.406788011208051, - "y": 4.004645601729296, - "heading": 0.18499930261907258, - "angularVelocity": -0.6651655462403078, - "velocityX": -3.2173592257833326, - "velocityY": 1.4346553397581023, - "timestamp": 2.3543999356665166 - }, - { - "x": 6.145744610358307, - "y": 4.100645232567065, - "heading": 0.1258988121859892, - "angularVelocity": -0.6976496408056244, - "velocityX": -3.0814775563276378, - "velocityY": 1.1332242335158502, - "timestamp": 2.4391136472149606 - }, - { - "x": 5.907416819572644, - "y": 4.18026386145866, - "heading": 0.07010067718233673, - "angularVelocity": -0.6586671033973509, - "velocityX": -2.8133319439011144, - "velocityY": 0.9398552776909607, - "timestamp": 2.5238273587634046 - }, - { - "x": 5.694140881693529, - "y": 4.247396241924432, - "heading": 0.01923390264073781, - "angularVelocity": -0.6004550339234085, - "velocityX": -2.517608235794895, - "velocityY": 0.7924618015040228, - "timestamp": 2.6085410703118486 - }, - { - "x": 5.50676824724975, - "y": 4.303942898252954, - "heading": -0.02601095760308067, - "angularVelocity": -0.5340913462154806, - "velocityX": -2.2118336101544562, - "velocityY": 0.6675029967986442, - "timestamp": 2.6932547818602925 - }, - { - "x": 5.345718150174197, - "y": 4.351008976671829, - "heading": -0.06525584873836304, - "angularVelocity": -0.4632649239177752, - "velocityX": -1.901110152439194, - "velocityY": 0.5555898514960005, - "timestamp": 2.7779684934087365 - }, - { - "x": 5.21123496648668, - "y": 4.389313273102533, - "heading": -0.09826172817354481, - "angularVelocity": -0.38961673183575596, - "velocityX": -1.5875019666752757, - "velocityY": 0.45216170712576903, - "timestamp": 2.8626822049571805 - }, - { - "x": 5.103477083411047, - "y": 4.419359601001064, - "heading": -0.1248629898593969, - "angularVelocity": -0.3140136490258714, - "velocityX": -1.2720241045513758, - "velocityY": 0.3546808108076878, - "timestamp": 2.9473959165056245 - }, - { - "x": 5.022554847546882, - "y": 4.44152029736655, - "heading": -0.14493751186900491, - "angularVelocity": -0.23696898226597282, - "velocityX": -0.9552436599108176, - "velocityY": 0.26159515337505695, - "timestamp": 3.0321096280540685 - }, - { - "x": 4.968549251002919, - "y": 4.456081570178744, - "heading": -0.15839111246886314, - "angularVelocity": -0.15881255057706564, - "velocityX": -0.6375071467985308, - "velocityY": 0.17188802787689772, - "timestamp": 3.1168233396025125 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": -0.07976943545791065, - "velocityX": -0.3190407913838158, - "velocityY": 0.08485777647782772, - "timestamp": 3.2015370511509564 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": -5.429647481375564e-36, - "velocityX": -2.0130702505219768e-38, - "velocityY": 9.290884594011316e-38, - "timestamp": 3.2862507626994004 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGFSprint.3.traj b/src/main/deploy/choreo/SourceLanePHGFSprint.3.traj deleted file mode 100644 index 23ed34a4..00000000 --- a/src/main/deploy/choreo/SourceLanePHGFSprint.3.traj +++ /dev/null @@ -1,355 +0,0 @@ -{ - "samples": [ - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": -5.429647481375564e-36, - "velocityX": -2.0130702505219768e-38, - "velocityY": 9.290884594011316e-38, - "timestamp": 0 - }, - { - "x": 4.970877788406757, - "y": 4.459511277906968, - "heading": -0.16349815793624287, - "angularVelocity": 0.018956158928001787, - "velocityX": 0.33714881644675093, - "velocityY": -0.04317094482113791, - "timestamp": 0.08707035452977951 - }, - { - "x": 5.029589121543177, - "y": 4.45199345668782, - "heading": -0.16019712382497864, - "angularVelocity": 0.03791226220556215, - "velocityX": 0.6742976234964045, - "velocityY": -0.0863419157961063, - "timestamp": 0.17414070905955903 - }, - { - "x": 5.117656119781506, - "y": 4.440716720346877, - "heading": -0.15524559648557837, - "angularVelocity": 0.05686811965037711, - "velocityX": 1.0114464184042, - "velocityY": -0.12951292551686924, - "timestamp": 0.26121106358933854 - }, - { - "x": 5.235078781735307, - "y": 4.425681064380957, - "heading": -0.14864360395133913, - "angularVelocity": 0.0758236551337492, - "velocityX": 1.3485951973888195, - "velocityY": -0.17268398695652012, - "timestamp": 0.34828141811911806 - }, - { - "x": 5.381857105533103, - "y": 4.406886483098539, - "heading": -0.14039116676025787, - "angularVelocity": 0.09477895473893824, - "velocityX": 1.6857439548795636, - "velocityY": -0.21585511376308925, - "timestamp": 0.4353517726488976 - }, - { - "x": 5.557991088523465, - "y": 4.3843329695031, - "heading": -0.13048827839881783, - "angularVelocity": 0.11373432915164189, - "velocityX": 2.0228926819187647, - "velocityY": -0.25902632092448025, - "timestamp": 0.5224221271786771 - }, - { - "x": 5.763480726622518, - "y": 4.3580205150215345, - "heading": -0.11893487664373759, - "angularVelocity": 0.13269041819657243, - "velocityX": 2.360041362054773, - "velocityY": -0.30219762654769566, - "timestamp": 0.6094924817084557 - }, - { - "x": 5.998326012444723, - "y": 4.327949108694172, - "heading": -0.10573079137043012, - "angularVelocity": 0.15164846111648134, - "velocityX": 2.697189957368156, - "velocityY": -0.34536905804233253, - "timestamp": 0.6965628362382343 - }, - { - "x": 6.262526926435642, - "y": 4.2941187332410315, - "heading": -0.09087557162555815, - "angularVelocity": 0.17061168321981837, - "velocityX": 3.034338328099486, - "velocityY": -0.38854068799696395, - "timestamp": 0.783633190768013 - }, - { - "x": 6.556082683402125, - "y": 4.256529028141766, - "heading": -0.0743557876801526, - "angularVelocity": 0.18972914529428694, - "velocityX": 3.371477680914724, - "velocityY": -0.4317164585152746, - "timestamp": 0.8707035452977916 - }, - { - "x": 6.820282808642692, - "y": 4.222698267435462, - "heading": -0.05948639298755931, - "angularVelocity": 0.17077448200245657, - "velocityX": 3.0343292693290502, - "velocityY": -0.3885451126161881, - "timestamp": 0.9577738998275702 - }, - { - "x": 7.055127288351479, - "y": 4.192626473165152, - "heading": -0.04626808716200777, - "angularVelocity": 0.15181178366548068, - "velocityX": 2.6971806991835345, - "velocityY": -0.3453735135536204, - "timestamp": 1.0448442543573488 - }, - { - "x": 7.260616119038252, - "y": 4.166313656843632, - "heading": -0.034701331761684315, - "angularVelocity": 0.13284378434875296, - "velocityX": 2.360032088952761, - "velocityY": -0.3022017822670142, - "timestamp": 1.1319146088871275 - }, - { - "x": 7.436749299548261, - "y": 4.1437598266696405, - "heading": -0.02478650882076325, - "angularVelocity": 0.11387139738279192, - "velocityX": 2.022883465459762, - "velocityY": -0.25902995681816454, - "timestamp": 1.218984963416906 - }, - { - "x": 7.583526829501114, - "y": 4.124964988926465, - "heading": -0.016523936768266155, - "angularVelocity": 0.09489535327056917, - "velocityX": 1.68573483759795, - "velocityY": -0.2158580592059934, - "timestamp": 1.3060553179466847 - }, - { - "x": 7.70094870876185, - "y": 4.10992914844881, - "heading": -0.009913866219354965, - "angularVelocity": 0.07591643085190908, - "velocityX": 1.3485862081861149, - "velocityY": -0.1726861060673939, - "timestamp": 1.3931256724764634 - }, - { - "x": 7.789014937206106, - "y": 4.09865230882672, - "heading": -0.004956472404176458, - "angularVelocity": 0.05693549592109463, - "velocityX": 1.011437577345994, - "velocityY": -0.12951411169725838, - "timestamp": 1.480196027006242 - }, - { - "x": 7.847725514598055, - "y": 4.091134472510321, - "heading": -0.0016518483286155007, - "angularVelocity": 0.03795349282092027, - "velocityX": 0.6742889437973931, - "velocityY": -0.08634208918752465, - "timestamp": 1.5672663815360206 - }, - { - "x": 7.87708044052124, - "y": 4.087375640869141, - "heading": 7.60993628191321e-37, - "angularVelocity": 0.01897142072679338, - "velocityX": 0.3371403054657971, - "velocityY": -0.043170050948797965, - "timestamp": 1.6543367360657992 - }, - { - "x": 7.87366387419082, - "y": 4.087813222475328, - "heading": -0.0001932265358713677, - "angularVelocity": -0.002007825984855413, - "velocityX": -0.035501700769334996, - "velocityY": 0.004546930965948054, - "timestamp": 1.7505734305447191 - }, - { - "x": 7.834385473616069, - "y": 4.092842926972847, - "heading": -0.0024054804575218995, - "angularVelocity": -0.02298763412052877, - "velocityX": -0.4081437001491553, - "velocityY": 0.05226389502209074, - "timestamp": 1.846810125023639 - }, - { - "x": 7.759245239147118, - "y": 4.102464751168547, - "heading": -0.006636690667772024, - "angularVelocity": -0.04396670348207906, - "velocityX": -0.780785695890768, - "velocityY": 0.09998082589804004, - "timestamp": 1.943046819502559 - }, - { - "x": 7.6482431709074055, - "y": 4.116678690374747, - "heading": -0.012886671317606013, - "angularVelocity": -0.06494384167780186, - "velocityX": -1.153427690349718, - "velocityY": 0.14769770806408986, - "timestamp": 2.039283513981479 - }, - { - "x": 7.501379268940608, - "y": 4.135484738377977, - "heading": -0.02115513791608259, - "angularVelocity": -0.08591802371482843, - "velocityX": -1.526069684354818, - "velocityY": 0.19541452566566683, - "timestamp": 2.135520208460399 - }, - { - "x": 7.318653533488854, - "y": 4.158882887388937, - "heading": -0.031441729230877964, - "angularVelocity": -0.10688845216985864, - "velocityX": -1.8987116758439624, - "velocityY": 0.24313126232826374, - "timestamp": 2.2317569029393187 - }, - { - "x": 7.100065965603603, - "y": 4.1868731279492994, - "heading": -0.04374603503260887, - "angularVelocity": -0.12785461791215239, - "velocityX": -2.2713536564073333, - "velocityY": 0.2908479007089456, - "timestamp": 2.3279935974182386 - }, - { - "x": 6.845616568893901, - "y": 4.219455448693913, - "heading": -0.05806762969505058, - "angularVelocity": -0.14881636095240902, - "velocityX": -2.643995599468943, - "velocityY": 0.3385644209938051, - "timestamp": 2.4242302918971586 - }, - { - "x": 6.555305358042951, - "y": 4.256629835248091, - "heading": -0.07440611137609496, - "angularVelocity": -0.16977392843249786, - "velocityX": -3.016637389956695, - "velocityY": 0.38628079191062686, - "timestamp": 2.5204669863760785 - }, - { - "x": 6.23254875309289, - "y": 4.297957965433844, - "heading": -0.09255419315316445, - "angularVelocity": -0.18857756779088714, - "velocityX": -3.353778999763519, - "velocityY": 0.4294425365451959, - "timestamp": 2.6167036808549984 - }, - { - "x": 5.945653966420611, - "y": 4.334694055593217, - "heading": -0.10868589302504747, - "angularVelocity": -0.16762524896796518, - "velocityX": -2.981137166292844, - "velocityY": 0.3817264335426585, - "timestamp": 2.7129403753339183 - }, - { - "x": 5.694621016414327, - "y": 4.366838114244224, - "heading": -0.12280128203638066, - "angularVelocity": -0.14667366837319035, - "velocityX": -2.6084951417493896, - "velocityY": 0.3340104190512136, - "timestamp": 2.809177069812838 - }, - { - "x": 5.479449909856628, - "y": 4.394390149838104, - "heading": -0.13490034942614496, - "angularVelocity": -0.12572197596016224, - "velocityX": -2.2358530467277298, - "velocityY": 0.28629449237696436, - "timestamp": 2.905413764291758 - }, - { - "x": 5.300140650438581, - "y": 4.417350169666037, - "heading": -0.1449830377404426, - "angularVelocity": -0.10476968654099145, - "velocityX": -1.863210913351998, - "velocityY": 0.23857864146571758, - "timestamp": 3.001650458770678 - }, - { - "x": 5.156693240503607, - "y": 4.435718179635196, - "heading": -0.1530492718346854, - "angularVelocity": -0.08381661629088484, - "velocityX": -1.4905687556256995, - "velocityY": 0.19086285193619654, - "timestamp": 3.097887153249598 - }, - { - "x": 5.04910768165416, - "y": 4.449494184192496, - "heading": -0.1590989820260221, - "angularVelocity": -0.06286282196300745, - "velocityX": -1.117926581248191, - "velocityY": 0.14314710861477242, - "timestamp": 3.194123847728518 - }, - { - "x": 4.97738397502577, - "y": 4.458678186291814, - "heading": -0.16313212141892855, - "angularVelocity": -0.0419085403415415, - "velocityX": -0.7452843950713719, - "velocityY": 0.09543139598720737, - "timestamp": 3.290360542207438 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": -0.020954127805584536, - "velocityX": -0.37264220046732904, - "velocityY": 0.047715698372427014, - "timestamp": 3.3865972366863577 - }, - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": 0, - "velocityX": 6.569525640446563e-39, - "velocityY": 1.8902971795210484e-36, - "timestamp": 3.4828339311652776 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGFSprint.4.traj b/src/main/deploy/choreo/SourceLanePHGFSprint.4.traj deleted file mode 100644 index 1fdf2927..00000000 --- a/src/main/deploy/choreo/SourceLanePHGFSprint.4.traj +++ /dev/null @@ -1,193 +0,0 @@ -{ - "samples": [ - { - "x": 4.941522121429443, - "y": 4.46327018737793, - "heading": -0.16514867741462683, - "angularVelocity": 0, - "velocityX": 6.569525640446563e-39, - "velocityY": 1.8902971795210484e-36, - "timestamp": 0 - }, - { - "x": 4.970161732141451, - "y": 4.445728451981636, - "heading": -0.16350561334854938, - "angularVelocity": 0.017714526683228992, - "velocityX": 0.3087750250459319, - "velocityY": -0.18912442074741204, - "timestamp": 0.09275235491518963 - }, - { - "x": 5.027440952562264, - "y": 4.4106449800780645, - "heading": -0.1602193291584108, - "angularVelocity": 0.03543073588960003, - "velocityX": 0.6175500392759685, - "velocityY": -0.378248853472785, - "timestamp": 0.18550470983037926 - }, - { - "x": 5.113359780805984, - "y": 4.358019770281013, - "heading": -0.1552893983395244, - "angularVelocity": 0.053151543412500564, - "velocityX": 0.9263250331733618, - "velocityY": -0.5673733011433544, - "timestamp": 0.2782570647455689 - }, - { - "x": 5.227918214012291, - "y": 4.287852820966911, - "heading": -0.14871510376483507, - "angularVelocity": 0.07088008256718281, - "velocityX": 1.2350999962325, - "velocityY": -0.756497766318278, - "timestamp": 0.3710094196607585 - }, - { - "x": 5.371116248190762, - "y": 4.200144130331831, - "heading": -0.1404954056786583, - "angularVelocity": 0.0886198317411194, - "velocityX": 1.5438749162693188, - "velocityY": -0.9456222509420716, - "timestamp": 0.46376177457594814 - }, - { - "x": 5.5429538779355045, - "y": 4.094893696480259, - "heading": -0.1306288879973164, - "angularVelocity": 0.1063748482759657, - "velocityX": 1.8526497780230582, - "velocityY": -1.1347467560021554, - "timestamp": 0.5565141294911378 - }, - { - "x": 5.7434310958415224, - "y": 3.9721015175775665, - "heading": -0.11911365753726154, - "angularVelocity": 0.12415027597503053, - "velocityX": 2.1614245599405897, - "velocityY": -1.3238712808420907, - "timestamp": 0.6492664844063274 - }, - { - "x": 5.972547891098849, - "y": 3.83176759216481, - "heading": -0.10594711713280619, - "angularVelocity": 0.1419537047495384, - "velocityX": 2.470199225311589, - "velocityY": -1.5129958214114747, - "timestamp": 0.742018839321517 - }, - { - "x": 6.230304245042205, - "y": 3.673891920053355, - "heading": -0.091125277264113, - "angularVelocity": 0.15980014612292848, - "velocityX": 2.7789736894447628, - "velocityY": -1.702120364014608, - "timestamp": 0.8347711942367066 - }, - { - "x": 6.5167001058009015, - "y": 3.498474507164359, - "heading": -0.07463891609789546, - "angularVelocity": 0.17774601174592225, - "velocityX": 3.087747594339454, - "velocityY": -1.8912448427793878, - "timestamp": 0.9275235491518963 - }, - { - "x": 6.774456304633716, - "y": 3.340598685286401, - "heading": -0.059730001301582306, - "angularVelocity": 0.16073893552293816, - "velocityX": 2.7789720171363874, - "velocityY": -1.7021219787067958, - "timestamp": 1.020275904067086 - }, - { - "x": 7.003572909925525, - "y": 3.200264603321266, - "heading": -0.04646689079252882, - "angularVelocity": 0.14299486542612352, - "velocityX": 2.4701971772178397, - "velocityY": -1.512997509264889, - "timestamp": 1.1130282589822755 - }, - { - "x": 7.204049930227038, - "y": 3.0774722725779653, - "heading": -0.03485583277229418, - "angularVelocity": 0.12518343098513846, - "velocityX": 2.161422429487887, - "velocityY": -1.3238729178960327, - "timestamp": 1.2057806138974652 - }, - { - "x": 7.375887370453985, - "y": 2.9722216978386684, - "heading": -0.024899933451460868, - "angularVelocity": 0.107338507253393, - "velocityX": 1.8526477347563841, - "velocityY": -1.1347482749687028, - "timestamp": 1.2985329688126548 - }, - { - "x": 7.519085234016694, - "y": 2.884512881951178, - "heading": -0.016601225810711995, - "angularVelocity": 0.0894716651489553, - "velocityX": 1.5438730767929685, - "velocityY": -0.9456236013380901, - "timestamp": 1.3912853237278444 - }, - { - "x": 7.633643523301954, - "y": 2.814345826878192, - "heading": -0.009961123896120593, - "angularVelocity": 0.07158957765183391, - "velocityX": 1.235098444562514, - "velocityY": -0.7564989065468437, - "timestamp": 1.484037678643034 - }, - { - "x": 7.7195622398456, - "y": 2.761720534080977, - "heading": -0.004980577858606377, - "angularVelocity": 0.05369724620003789, - "velocityX": 0.92632382889046, - "velocityY": -0.5673741960011052, - "timestamp": 1.5767900335582237 - }, - { - "x": 7.776841384411153, - "y": 2.726637004697944, - "heading": -0.0016601403967374856, - "angularVelocity": 0.035798955885325216, - "velocityX": 0.6175492214502578, - "velocityY": -0.37824947318170576, - "timestamp": 1.6695423884734133 - }, - { - "x": 7.80548095703125, - "y": 2.7090952396392822, - "heading": 2.2544024955772584e-32, - "angularVelocity": 0.017898633390553583, - "velocityX": 0.3087746143619144, - "velocityY": -0.1891247405491907, - "timestamp": 1.762294743388603 - }, - { - "x": 7.80548095703125, - "y": 2.7090952396392822, - "heading": 1.2738822120559367e-32, - "angularVelocity": -1.0571378529733318e-31, - "velocityX": -6.060794460308241e-34, - "velocityY": -2.7880656703838183e-33, - "timestamp": 1.8550470983037926 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint (1).1.traj b/src/main/deploy/choreo/SourceLanePHGSprint (1).1.traj deleted file mode 100644 index b65ea667..00000000 --- a/src/main/deploy/choreo/SourceLanePHGSprint (1).1.traj +++ /dev/null @@ -1,184 +0,0 @@ -{ - "samples": [ - { - "x": 0.433241993188858, - "y": 4.121134281158447, - "heading": -3.528930169589175e-36, - "angularVelocity": 0, - "velocityX": -3.4629926489463697e-37, - "velocityY": -7.866244288467422e-37, - "timestamp": 0 - }, - { - "x": 0.45469188032137553, - "y": 4.109218526997661, - "heading": -0.008937300145995173, - "angularVelocity": -0.1189323726299145, - "velocityX": 0.2854425752342375, - "velocityY": -0.15856789979825622, - "timestamp": 0.07514606787342622 - }, - { - "x": 0.49759166351162515, - "y": 4.0853873108981436, - "heading": -0.026812413296636604, - "angularVelocity": -0.237871570083344, - "velocityX": 0.5708852692400179, - "velocityY": -0.3171319108760035, - "timestamp": 0.15029213574685243 - }, - { - "x": 0.5619414537480725, - "y": 4.049641027140498, - "heading": -0.053621717098024775, - "angularVelocity": -0.3567625633658565, - "velocityX": 0.856329440215504, - "velocityY": -0.4756906750976659, - "timestamp": 0.22543820362027867 - }, - { - "x": 0.6477414523773106, - "y": 4.001980177860296, - "heading": -0.08935853104911512, - "angularVelocity": -0.47556465644063195, - "velocityX": 1.141776290593902, - "velocityY": -0.6342427571923072, - "timestamp": 0.30058427149370487 - }, - { - "x": 0.7549919358777414, - "y": 3.942405375853288, - "heading": -0.1340144220065316, - "angularVelocity": -0.5942545261667388, - "velocityX": 1.4272268201854585, - "velocityY": -0.7927866845588477, - "timestamp": 0.37573033936713107 - }, - { - "x": 0.8836932393269182, - "y": 3.8709173416225418, - "heading": -0.18758057158075767, - "angularVelocity": -0.7128270459134506, - "velocityX": 1.712681808793474, - "velocityY": -0.951321023891208, - "timestamp": 0.4508764072405573 - }, - { - "x": 1.0338457421402325, - "y": 3.787516893297593, - "heading": -0.2500489995023071, - "angularVelocity": -0.8312933688928251, - "velocityX": 1.9981418464400118, - "velocityY": -1.1098444760333435, - "timestamp": 0.5260224751139835 - }, - { - "x": 1.2054498603190866, - "y": 3.6922049311693006, - "heading": -0.32141344961465573, - "angularVelocity": -0.9496764385936043, - "velocityX": 2.283607419990343, - "velocityY": -1.268355947630325, - "timestamp": 0.6011685429874097 - }, - { - "x": 1.3985060453231255, - "y": 3.584982424979259, - "heading": -0.4016698579859379, - "angularVelocity": -1.0680054278616897, - "velocityX": 2.5690790013020615, - "velocityY": -1.4268545144723217, - "timestamp": 0.676314610860836 - }, - { - "x": 1.5915937569821008, - "y": 3.4778198154599647, - "heading": -0.48176859267038585, - "angularVelocity": -1.065907198489266, - "velocityX": 2.5694985396202825, - "velocityY": -1.4260574445464829, - "timestamp": 0.7514606787342623 - }, - { - "x": 1.7632284858900336, - "y": 3.3825660365200445, - "heading": -0.5529765476804539, - "angularVelocity": -0.9475938931363466, - "velocityX": 2.2840147697019786, - "velocityY": -1.2675816797275798, - "timestamp": 0.8266067466076885 - }, - { - "x": 1.9134098098562764, - "y": 3.2992204007390877, - "heading": -0.6152918418017526, - "angularVelocity": -0.8292555536806094, - "velocityX": 1.9985253815170194, - "velocityY": -1.1091150626981794, - "timestamp": 0.9017528144811148 - }, - { - "x": 2.0421373577646693, - "y": 3.2277823059512354, - "heading": -0.6687115003742365, - "angularVelocity": -0.7108776291856339, - "velocityX": 1.7130310547348626, - "velocityY": -0.9506564589404817, - "timestamp": 0.976898882354541 - }, - { - "x": 2.149410808485792, - "y": 3.1682512394298143, - "heading": -0.7132320418113071, - "angularVelocity": -0.5924533737687983, - "velocityX": 1.4275324545498613, - "velocityY": -0.7922046782500071, - "timestamp": 1.0520449502279672 - }, - { - "x": 2.2352298936713018, - "y": 3.120626784362015, - "heading": -0.7488501306819737, - "angularVelocity": -0.4739847323836121, - "velocityX": 1.1420302833417786, - "velocityY": -0.6337584442610683, - "timestamp": 1.1271910181013933 - }, - { - "x": 2.2995943987852443, - "y": 3.0849086278583613, - "heading": -0.775563262460788, - "angularVelocity": -0.3554827622359309, - "velocityX": 0.8565252572144717, - "velocityY": -0.4753163740226096, - "timestamp": 1.2023370859748195 - }, - { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080858, - "angularVelocity": -0.23696688264902366, - "velocityX": 0.5710180813549187, - "velocityY": -0.3168769856057537, - "timestamp": 1.2774831538482456 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550472, - "velocityX": 0.28550943973314774, - "velocityY": -0.15843872456938407, - "timestamp": 1.3526292217216718 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 9.576319315825878e-33, - "velocityX": -3.340074147697442e-33, - "velocityY": 1.4972964833121403e-33, - "timestamp": 1.427775289595098 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint (1).2.traj b/src/main/deploy/choreo/SourceLanePHGSprint (1).2.traj deleted file mode 100644 index a125ab60..00000000 --- a/src/main/deploy/choreo/SourceLanePHGSprint (1).2.traj +++ /dev/null @@ -1,814 +0,0 @@ -{ - "samples": [ - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 9.576319315825878e-33, - "velocityX": -3.340074147697442e-33, - "velocityY": 1.4972964833121403e-33, - "timestamp": 0 - }, - { - "x": 2.3773546785315864, - "y": 3.0407954088560687, - "heading": -0.7860770412156382, - "angularVelocity": 0.26500042058499174, - "velocityX": 0.21918861143707746, - "velocityY": -0.13736692695017713, - "timestamp": 0.06111450966076326 - }, - { - "x": 2.4041715283150715, - "y": 3.0239975454455843, - "heading": -0.7541197928104101, - "angularVelocity": 0.5229077118120982, - "velocityX": 0.438796775632191, - "velocityY": -0.27485884291187945, - "timestamp": 0.12222901932152652 - }, - { - "x": 2.4444388170293236, - "y": 2.998788446329585, - "heading": -0.7069088657797715, - "angularVelocity": 0.7724994815911764, - "velocityX": 0.6588826276733535, - "velocityY": -0.41248959135778174, - "timestamp": 0.18334352898228978 - }, - { - "x": 2.498190798561039, - "y": 2.965157152573787, - "heading": -0.6450804787000076, - "angularVelocity": 1.0116809808826634, - "velocityX": 0.8795289666903056, - "velocityY": -0.5502996578468836, - "timestamp": 0.24445803864305304 - }, - { - "x": 2.5654677170302604, - "y": 2.923088008476279, - "heading": -0.5694581806203519, - "angularVelocity": 1.2373869724133075, - "velocityX": 1.1008338092322851, - "velocityY": -0.6883658943027918, - "timestamp": 0.3055725483038163 - }, - { - "x": 2.646315775914165, - "y": 2.8725583813996622, - "heading": -0.48110952859275347, - "angularVelocity": 1.4456248199978636, - "velocityX": 1.3228946666295571, - "velocityY": -0.8268024624119353, - "timestamp": 0.36668705796457957 - }, - { - "x": 2.7407862870930626, - "y": 2.813537598490058, - "heading": -0.3813947698765895, - "angularVelocity": 1.631605313855327, - "velocityX": 1.5457951262848801, - "velocityY": -0.965740922036666, - "timestamp": 0.4278015676253428 - }, - { - "x": 2.84893500974016, - "y": 2.7459886359527177, - "heading": -0.2720274509201168, - "angularVelocity": 1.7895475160244767, - "velocityX": 1.7696079580350594, - "velocityY": -1.105285191884772, - "timestamp": 0.4889160772861061 - }, - { - "x": 2.9708226363170516, - "y": 2.669873093439706, - "heading": -0.15523119808259803, - "angularVelocity": 1.9111051284848068, - "velocityX": 1.9944138839282306, - "velocityY": -1.2454577961194009, - "timestamp": 0.5500305869468693 - }, - { - "x": 3.1065122638812066, - "y": 2.5851587340080315, - "heading": -0.03419676462946552, - "angularVelocity": 1.9804533182868547, - "velocityX": 2.220252249708728, - "velocityY": -1.3861578846318265, - "timestamp": 0.6111450966076324 - }, - { - "x": 3.2560356571448454, - "y": 2.4918371927335334, - "heading": 0.08572826850512158, - "angularVelocity": 1.9623005044182085, - "velocityX": 2.446610372784124, - "velocityY": -1.5269948461095475, - "timestamp": 0.6722596062683956 - }, - { - "x": 3.419180860242083, - "y": 2.390029027608911, - "heading": 0.19415018142326163, - "angularVelocity": 1.7740780956923754, - "velocityX": 2.6695003200194134, - "velocityY": -1.6658591501387063, - "timestamp": 0.7333741159291589 - }, - { - "x": 3.5929644641848295, - "y": 2.279455345020229, - "heading": 0.2670576620012504, - "angularVelocity": 1.1929651564364419, - "velocityX": 2.8435735622750062, - "velocityY": -1.809286914064415, - "timestamp": 0.7944886255899222 - }, - { - "x": 3.776865546474961, - "y": 2.1600020280366277, - "heading": 0.3015223704483068, - "angularVelocity": 0.5639365944088309, - "velocityX": 3.0091230922237155, - "velocityY": -1.9545819421061736, - "timestamp": 0.8556031352506854 - }, - { - "x": 3.9728967066647076, - "y": 2.0375522576737963, - "heading": 0.30152241765502197, - "angularVelocity": 7.724305629887772e-7, - "velocityX": 3.2076042379769354, - "velocityY": -2.003612088889046, - "timestamp": 0.9167176449114487 - }, - { - "x": 4.174386727718979, - "y": 1.9243094504352285, - "heading": 0.30152246485493484, - "angularVelocity": 7.72319260587173e-7, - "velocityX": 3.2969260847008353, - "velocityY": -1.8529610704096402, - "timestamp": 0.977832154572212 - }, - { - "x": 4.383354452565465, - "y": 1.8255438101811148, - "heading": 0.3015225128834295, - "angularVelocity": 7.858771167686507e-7, - "velocityX": 3.4192817058736558, - "velocityY": -1.6160751481496929, - "timestamp": 1.0389466642329752 - }, - { - "x": 4.598761540028333, - "y": 1.7417467464566943, - "heading": 0.30152256294147645, - "angularVelocity": 8.190861254028443e-7, - "velocityX": 3.524647234487482, - "velocityY": -1.3711484259558735, - "timestamp": 1.1000611738937385 - }, - { - "x": 4.819537390717514, - "y": 1.6733348748555603, - "heading": 0.3015226164690873, - "angularVelocity": 8.758576513463982e-7, - "velocityX": 3.612494838208991, - "velocityY": -1.1194047367945394, - "timestamp": 1.1611756835545017 - }, - { - "x": 5.04458467733647, - "y": 1.620648272623701, - "heading": 0.30152267534909954, - "angularVelocity": 9.634375302003616e-7, - "velocityX": 3.682387175617667, - "velocityY": -0.8620964567058493, - "timestamp": 1.222290193215265 - }, - { - "x": 5.27278482830379, - "y": 1.5839488347993986, - "heading": 0.3015227422489141, - "angularVelocity": 0.0000010946633609592033, - "velocityX": 3.7339766322927472, - "velocityY": -0.6005028597630111, - "timestamp": 1.2834047028760283 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.3015228161300745, - "angularVelocity": 0.0000012088972118666571, - "velocityX": 3.7670067261218194, - "velocityY": -0.33592432468507805, - "timestamp": 1.3445192125367915 - }, - { - "x": 5.642161233224445, - "y": 1.5569512204704454, - "heading": 0.30152289610635336, - "angularVelocity": 0.0000021712102287950276, - "velocityX": 3.777876226736954, - "velocityY": -0.1755880047102645, - "timestamp": 1.38135409743325 - }, - { - "x": 5.781468008961295, - "y": 1.556401100893367, - "heading": 0.3015229698630755, - "angularVelocity": 0.0000020023605971612715, - "velocityX": 3.7819250997644014, - "velocityY": -0.01493474402390838, - "timestamp": 1.4181889823297087 - }, - { - "x": 5.920672391336217, - "y": 1.5617696165699877, - "heading": 0.3015230386365789, - "angularVelocity": 0.0000018670752917486593, - "velocityX": 3.779145306581512, - "velocityY": 0.14574541746801997, - "timestamp": 1.4550238672261673 - }, - { - "x": 6.0595230325638285, - "y": 1.5730470711005764, - "heading": 0.30152310340837535, - "angularVelocity": 0.000001758436237719359, - "velocityX": 3.769541879061509, - "velocityY": 0.3061623393771694, - "timestamp": 1.4918587521226259 - }, - { - "x": 6.197769224123459, - "y": 1.5902130982406153, - "heading": 0.3015231649742211, - "angularVelocity": 0.0000016714005183062998, - "velocityX": 3.753132172076425, - "velocityY": 0.4660263548614807, - "timestamp": 1.5286936370190845 - }, - { - "x": 6.335161349542959, - "y": 1.6132366986444322, - "heading": 0.3015232239953201, - "angularVelocity": 0.0000016023152826544925, - "velocityX": 3.7299458327534505, - "velocityY": 0.6250487946015111, - "timestamp": 1.565528521915543 - }, - { - "x": 6.471451335210211, - "y": 1.6420762959087596, - "heading": 0.3015232810350899, - "angularVelocity": 0.000001548525804727782, - "velocityX": 3.7000247469309935, - "velocityY": 0.7829425107583309, - "timestamp": 1.6023634068120016 - }, - { - "x": 6.606393098338155, - "y": 1.6766798119201123, - "heading": 0.3015233366126912, - "angularVelocity": 0.0000015088305949550033, - "velocityX": 3.6634229618813627, - "velocityY": 0.9394224010369983, - "timestamp": 1.6391982917084602 - }, - { - "x": 6.739742463422568, - "y": 1.7169846140875678, - "heading": 0.30152556567084704, - "angularVelocity": 0.00006051486687687375, - "velocityX": 3.6201922568579414, - "velocityY": 1.0942019306087243, - "timestamp": 1.6760331766049188 - }, - { - "x": 6.8694207904091344, - "y": 1.7614703171513668, - "heading": 0.3101205256410533, - "angularVelocity": 0.23333750042565127, - "velocityX": 3.520530262306672, - "velocityY": 1.2077057710061154, - "timestamp": 1.7128680615013774 - }, - { - "x": 6.994764102779472, - "y": 1.8086931078781319, - "heading": 0.3306180633638748, - "angularVelocity": 0.5564707961064402, - "velocityX": 3.4028425152588877, - "velocityY": 1.2820127132066854, - "timestamp": 1.749702946397836 - }, - { - "x": 7.114597005425528, - "y": 1.8565129203183168, - "heading": 0.359627713397502, - "angularVelocity": 0.7875591335542955, - "velocityX": 3.2532449329732183, - "velocityY": 1.298220764761567, - "timestamp": 1.7865378312942946 - }, - { - "x": 7.228563598795877, - "y": 1.903963761474251, - "heading": 0.39146295222083616, - "angularVelocity": 0.8642687200685345, - "velocityX": 3.0939853264291135, - "velocityY": 1.288203866777826, - "timestamp": 1.8233727161907531 - }, - { - "x": 7.336676614220082, - "y": 1.9505339221399112, - "heading": 0.42341393247502157, - "angularVelocity": 0.8674108889982476, - "velocityX": 2.9350713522821095, - "velocityY": 1.2642949963483492, - "timestamp": 1.8602076010872117 - }, - { - "x": 7.438996464546078, - "y": 1.9959395107165516, - "heading": 0.45395278545537515, - "angularVelocity": 0.8290742068611577, - "velocityX": 2.777797476865021, - "velocityY": 1.2326789863542045, - "timestamp": 1.8970424859836703 - }, - { - "x": 7.535582073954796, - "y": 2.0400031624519186, - "heading": 0.4820829146791016, - "angularVelocity": 0.7636817463336469, - "velocityX": 2.622123285581467, - "velocityY": 1.1962478465516526, - "timestamp": 1.933877370880129 - }, - { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.6791276743107278, - "velocityX": 2.4678193928677445, - "velocityY": 1.1565397180800696, - "timestamp": 1.9707122557765875 - }, - { - "x": 7.766333855095899, - "y": 2.151391160116789, - "heading": 0.5393931714770351, - "angularVelocity": 0.508950730683683, - "velocityX": 2.2039777611896034, - "velocityY": 1.0840548028640593, - "timestamp": 2.0341656809073783 - }, - { - "x": 7.8897168213730975, - "y": 2.2145350229010927, - "heading": 0.5621690879226864, - "angularVelocity": 0.3589391179231936, - "velocityX": 1.9444650312080196, - "velocityY": 0.9951214241650663, - "timestamp": 2.0976191060381684 - }, - { - "x": 7.996856912991746, - "y": 2.2713427782005033, - "heading": 0.5763089063837279, - "angularVelocity": 0.22283774960762856, - "velocityX": 1.6884839769927564, - "velocityY": 0.8952669644281364, - "timestamp": 2.1610725311689594 - }, - { - "x": 8.08793407853681, - "y": 2.3213235528188796, - "heading": 0.5824533246990756, - "angularVelocity": 0.09683351690917823, - "velocityX": 1.4353388387992243, - "velocityY": 0.7876765441007405, - "timestamp": 2.2245259562997495 - }, - { - "x": 8.16309393881241, - "y": 2.364112462024999, - "heading": 0.5810872026138336, - "angularVelocity": -0.021529524725041688, - "velocityX": 1.1844886248564515, - "velocityY": 0.6743356898059116, - "timestamp": 2.2879793814305405 - }, - { - "x": 8.222455939995415, - "y": 2.3994280671308665, - "heading": 0.5725898810376202, - "angularVelocity": -0.13391430893917458, - "velocityX": 0.9355208337555886, - "velocityY": 0.5565594770191679, - "timestamp": 2.3514328065613306 - }, - { - "x": 8.266119462815809, - "y": 2.427046891293226, - "heading": 0.5572661381126698, - "angularVelocity": -0.24149591441856877, - "velocityX": 0.6881192422693246, - "velocityY": 0.4352613606819736, - "timestamp": 2.4148862316921216 - }, - { - "x": 8.294168308263885, - "y": 2.446787281465013, - "heading": 0.5353660980042331, - "angularVelocity": -0.3451356654632322, - "velocityX": 0.4420383200790658, - "velocityY": 0.31110046669817776, - "timestamp": 2.4783396568229117 - }, - { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.4454857015776663, - "velocityX": 0.19708463824313482, - "velocityY": 0.18456741878341767, - "timestamp": 2.5417930819537027 - }, - { - "x": 8.303024530407173, - "y": 2.4618811275821986, - "heading": 0.4711910323232683, - "angularVelocity": -0.5465136547157353, - "velocityX": -0.055545178149817624, - "velocityY": 0.05148048067311967, - "timestamp": 2.607495872285164 - }, - { - "x": 8.282750419132642, - "y": 2.456558519757899, - "heading": 0.42887568169046636, - "angularVelocity": -0.6440419108431609, - "velocityX": -0.30857306321773753, - "velocityY": -0.08101037714605691, - "timestamp": 2.6731986626166258 - }, - { - "x": 8.245821395629305, - "y": 2.4425769262851884, - "heading": 0.3804195474569322, - "angularVelocity": -0.7375049672788591, - "velocityX": -0.5620617224479451, - "velocityY": -0.2128006040866038, - "timestamp": 2.7389014529480873 - }, - { - "x": 8.19220201387014, - "y": 2.4199912479232255, - "heading": 0.32613641295591106, - "angularVelocity": -0.8261922245184766, - "velocityX": -0.8160898721144464, - "velocityY": -0.3437552385221621, - "timestamp": 2.804604243279549 - }, - { - "x": 8.121850214183777, - "y": 2.388868090056124, - "heading": 0.2664003971864744, - "angularVelocity": -0.9091853704854365, - "velocityX": -1.0707581722397086, - "velocityY": -0.4736961354318394, - "timestamp": 2.8703070336110104 - }, - { - "x": 8.034715295558527, - "y": 2.3492899199909276, - "heading": 0.20166644793914107, - "angularVelocity": -0.9852541866298157, - "velocityX": -1.32619814448768, - "velocityY": -0.6023818754961511, - "timestamp": 2.936009823942472 - }, - { - "x": 7.930734985336223, - "y": 2.3013613961682027, - "heading": 0.13250258742090648, - "angularVelocity": -1.0526776742557293, - "velocityX": -1.582585909939886, - "velocityY": -0.7294747084702521, - "timestamp": 3.0017126142739334 - }, - { - "x": 7.809831081747402, - "y": 2.2452194829459002, - "heading": 0.05964299707705179, - "angularVelocity": -1.1089268808263428, - "velocityX": -1.8401639105261383, - "velocityY": -0.8544829365552755, - "timestamp": 3.067415404605395 - }, - { - "x": 7.671902780233126, - "y": 2.1810506699963903, - "heading": -0.015920256793081004, - "angularVelocity": -1.1500767850029923, - "velocityX": -2.099276161917122, - "velocityY": -0.976652781804051, - "timestamp": 3.1331181949368565 - }, - { - "x": 7.516816185598605, - "y": 2.109122861945235, - "heading": -0.09276870443286901, - "angularVelocity": -1.1696375032490753, - "velocityX": -2.360426305368932, - "velocityY": -1.0947451042534073, - "timestamp": 3.198820985268318 - }, - { - "x": 7.344387868807497, - "y": 2.0298516556077795, - "heading": -0.16871929267657423, - "angularVelocity": -1.155972034985807, - "velocityX": -2.624368248612144, - "velocityY": -1.2065120208372122, - "timestamp": 3.2645237755997796 - }, - { - "x": 7.154363408340053, - "y": 1.943962982346676, - "heading": -0.2400485240661189, - "angularVelocity": -1.0856347352935647, - "velocityX": -2.892182501059652, - "velocityY": -1.3072302230667407, - "timestamp": 3.330226565931241 - }, - { - "x": 6.946444183851081, - "y": 1.8530162365161487, - "heading": -0.2992772419214472, - "angularVelocity": -0.9014642689682999, - "velocityX": -3.164541771210153, - "velocityY": -1.3842143594162997, - "timestamp": 3.3959293562627026 - }, - { - "x": 6.72212326982476, - "y": 1.762675190878268, - "heading": -0.31855321839260486, - "angularVelocity": -0.29338139786625583, - "velocityX": -3.4141763674670904, - "velocityY": -1.3749955699312393, - "timestamp": 3.461632146594164 - }, - { - "x": 6.4857597450479005, - "y": 1.6860130581467423, - "heading": -0.3185532921212083, - "angularVelocity": -0.000001122153306519187, - "velocityX": -3.5974655503128186, - "velocityY": -1.1668017803319486, - "timestamp": 3.5273349369256257 - }, - { - "x": 6.24426876944993, - "y": 1.6274732096370486, - "heading": -0.3185533177664737, - "angularVelocity": -3.903223175651228e-7, - "velocityX": -3.6755056273817623, - "velocityY": -0.8909796405048931, - "timestamp": 3.5930377272570873 - }, - { - "x": 5.999037658914315, - "y": 1.5873919634602989, - "heading": -0.3185533462426245, - "angularVelocity": -4.3340854500246066e-7, - "velocityX": -3.732430682143934, - "velocityY": -0.6100387209515066, - "timestamp": 3.658740517588549 - }, - { - "x": 5.751475205523387, - "y": 1.5659995878977724, - "heading": -0.3185533789943253, - "angularVelocity": -4.984826460154038e-7, - "velocityX": -3.7679138456983283, - "velocityY": -0.32559310578142847, - "timestamp": 3.7244433079200103 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.3185534180019773, - "angularVelocity": -5.936985598886166e-7, - "velocityX": -3.781751231726404, - "velocityY": -0.03927692372891183, - "timestamp": 3.790146098251472 - }, - { - "x": 5.26265149067387, - "y": 1.5786031053003824, - "heading": -0.31855345112044986, - "angularVelocity": -5.200844119271939e-7, - "velocityX": -3.774430827391544, - "velocityY": 0.23844772894968422, - "timestamp": 3.853825131614962 - }, - { - "x": 5.0240625547753925, - "y": 1.61139052009726, - "heading": -0.3185534786660252, - "angularVelocity": -4.325689925822067e-7, - "velocityX": -3.746742425196293, - "velocityY": 0.5148855606793217, - "timestamp": 3.9175041649784523 - }, - { - "x": 4.788524290519247, - "y": 1.6616042890226763, - "heading": -0.31855350247111613, - "angularVelocity": -3.7382933976434193e-7, - "velocityX": -3.698835422197071, - "velocityY": 0.7885447732660757, - "timestamp": 3.9811831983419426 - }, - { - "x": 4.55730773239592, - "y": 1.7289734284079032, - "heading": -0.3185535237219762, - "angularVelocity": -3.3371831981217076e-7, - "velocityX": -3.6309684037367047, - "velocityY": 1.0579485244487483, - "timestamp": 4.044862231705433 - }, - { - "x": 4.33166058516164, - "y": 1.8131343663413289, - "heading": -0.31855354324045854, - "angularVelocity": -3.06513484100539e-7, - "velocityX": -3.5435077342686676, - "velocityY": 1.3216428310558945, - "timestamp": 4.108541265068923 - }, - { - "x": 4.112800476187319, - "y": 1.9136328827222213, - "heading": -0.31855356163583604, - "angularVelocity": -2.8887651909639526e-7, - "velocityX": -3.4369257417120753, - "velocityY": 1.5782041760469425, - "timestamp": 4.172220298432413 - }, - { - "x": 3.9019083093617053, - "y": 2.029926448332014, - "heading": -0.3185535793939757, - "angularVelocity": -2.788694925452677e-7, - "velocityX": -3.3117991226689654, - "velocityY": 1.8262457745859704, - "timestamp": 4.2358993317959035 - }, - { - "x": 3.700120307432907, - "y": 2.161384813738153, - "heading": -0.3185535969362317, - "angularVelocity": -2.7547930719470604e-7, - "velocityX": -3.1688295388681516, - "velocityY": 2.0643900898393666, - "timestamp": 4.299578365159394 - }, - { - "x": 3.5092854049648876, - "y": 2.2879634464070207, - "heading": -0.3579155843505842, - "angularVelocity": -0.618131044635493, - "velocityX": -2.996824737880426, - "velocityY": 1.9877599577609333, - "timestamp": 4.363257398522884 - }, - { - "x": 3.3329097555386666, - "y": 2.4050971261724468, - "heading": -0.41294492327597715, - "angularVelocity": -0.8641673093760206, - "velocityX": -2.7697601566820413, - "velocityY": 1.8394387222684179, - "timestamp": 4.426936431886374 - }, - { - "x": 3.171287726021972, - "y": 2.512479984884094, - "heading": -0.46992046584984914, - "angularVelocity": -0.8947300165290927, - "velocityX": -2.538072910028812, - "velocityY": 1.686314207985683, - "timestamp": 4.490615465249864 - }, - { - "x": 3.024408806786741, - "y": 2.6100923699912792, - "heading": -0.5252799185719805, - "angularVelocity": -0.8693513358805383, - "velocityX": -2.306550704009953, - "velocityY": 1.532881074842923, - "timestamp": 4.554294498613355 - }, - { - "x": 2.892255079193046, - "y": 2.6979336450739244, - "heading": -0.5773440254447586, - "angularVelocity": -0.8176020288434342, - "velocityX": -2.0753098879397363, - "velocityY": 1.3794379475145806, - "timestamp": 4.617973531976845 - }, - { - "x": 2.7748123815012584, - "y": 2.776006230905406, - "heading": -0.625128837055519, - "angularVelocity": -0.7504010203483642, - "velocityX": -1.8442914643726742, - "velocityY": 1.226032835420591, - "timestamp": 4.681652565340335 - }, - { - "x": 2.6720699818921556, - "y": 2.8443129166968544, - "heading": -0.6679860381773632, - "angularVelocity": -0.6730190277419648, - "velocityX": -1.6134415706757543, - "velocityY": 1.0726715244175118, - "timestamp": 4.745331598703825 - }, - { - "x": 2.5840196243973645, - "y": 2.9028562717726505, - "heading": -0.7054558016839699, - "angularVelocity": -0.5884160221579269, - "velocityX": -1.382721326691407, - "velocityY": 0.9193505614575074, - "timestamp": 4.809010632067316 - }, - { - "x": 2.510654811151598, - "y": 2.95163853061359, - "heading": -0.7371951288142529, - "angularVelocity": -0.4984266477336411, - "velocityX": -1.1521031235978156, - "velocityY": 0.7660646882386302, - "timestamp": 4.872689665430806 - }, - { - "x": 2.4519703126340358, - "y": 2.9906615990271557, - "heading": -0.7629386573341135, - "angularVelocity": -0.4042700895428554, - "velocityX": -0.9215670436230067, - "velocityY": 0.6128087433553877, - "timestamp": 4.936368698794296 - }, - { - "x": 2.4079618327799786, - "y": 3.0199270886206313, - "heading": -0.7824753669479414, - "angularVelocity": -0.30679972012623624, - "velocityX": -0.6910984279998401, - "velocityY": 0.45957810675962124, - "timestamp": 5.000047732157786 - }, - { - "x": 2.378625774904473, - "y": 3.039436353320747, - "heading": -0.795633833561549, - "angularVelocity": -0.2066373485680448, - "velocityX": -0.4606862938394533, - "velocityY": 0.3063687318988397, - "timestamp": 5.0637267655212765 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.10425061542851646, - "velocityX": -0.2303222914890624, - "velocityY": 0.15317707264507013, - "timestamp": 5.127405798884767 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -7.869331036874185e-31, - "timestamp": 5.191084832248257 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint (1).3.traj b/src/main/deploy/choreo/SourceLanePHGSprint (1).3.traj deleted file mode 100644 index e900750d..00000000 --- a/src/main/deploy/choreo/SourceLanePHGSprint (1).3.traj +++ /dev/null @@ -1,787 +0,0 @@ -{ - "samples": [ - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -7.869331036874185e-31, - "timestamp": 0 - }, - { - "x": 2.3794831364602356, - "y": 3.04172499089676, - "heading": -0.7962438371233982, - "angularVelocity": 0.09576162685715454, - "velocityX": 0.24659384848580665, - "velocityY": -0.11858712018957836, - "timestamp": 0.0629539728390398 - }, - { - "x": 2.4105331611816494, - "y": 3.0267927192788293, - "heading": -0.7842933789401895, - "angularVelocity": 0.1898284992078828, - "velocityX": 0.49321787523723476, - "velocityY": -0.23719347555886855, - "timestamp": 0.1259079456780796 - }, - { - "x": 2.457111347137164, - "y": 3.0043922693157348, - "heading": -0.7665480516906409, - "angularVelocity": 0.2818777981640571, - "velocityX": 0.7398768315163422, - "velocityY": -0.3558226582517257, - "timestamp": 0.18886191851711942 - }, - { - "x": 2.5192202688476826, - "y": 2.9745219425624034, - "heading": -0.7431604801963628, - "angularVelocity": 0.37150270967132, - "velocityX": 0.9865766830842898, - "velocityY": -0.4744788201008906, - "timestamp": 0.2518158913561592 - }, - { - "x": 2.5968629770363694, - "y": 2.9371797310172574, - "heading": -0.7143164372650311, - "angularVelocity": 0.45817669053357035, - "velocityX": 1.2333249942335256, - "velocityY": -0.5931668782941151, - "timestamp": 0.31476986419519903 - }, - { - "x": 2.690043132952377, - "y": 2.892363250439385, - "heading": -0.6802459845187668, - "angularVelocity": 0.5411962297181008, - "velocityX": 1.4801314629379994, - "velocityY": -0.711892809250637, - "timestamp": 0.37772383703423884 - }, - { - "x": 2.798765189175504, - "y": 2.840069647867451, - "heading": -0.6412406321012497, - "angularVelocity": 0.6195852407479587, - "velocityX": 1.7270086591851181, - "velocityY": -0.8306640584167341, - "timestamp": 0.44067780987327865 - }, - { - "x": 2.9230346316689584, - "y": 2.7802954749466373, - "heading": -0.5976811625941267, - "angularVelocity": 0.6919256647788576, - "velocityX": 1.9739729978787044, - "velocityY": -0.9494900833922507, - "timestamp": 0.5036317827123185 - }, - { - "x": 3.0628582875020656, - "y": 2.7130365228135442, - "heading": -0.550085909804202, - "angularVelocity": 0.7560325527288918, - "velocityX": 2.2210457819811675, - "velocityY": -1.0683829645677803, - "timestamp": 0.5665857555513583 - }, - { - "x": 3.2182446226798445, - "y": 2.6382876468732976, - "heading": -0.4992026727158649, - "angularVelocity": 0.8082609372157451, - "velocityX": 2.4682530453648863, - "velocityY": -1.1873575656831603, - "timestamp": 0.6295397283903981 - }, - { - "x": 3.3892034750687743, - "y": 2.5560428034877645, - "heading": -0.4462086118156311, - "angularVelocity": 0.8417905734992818, - "velocityX": 2.7156165795292284, - "velocityY": -1.306428167699852, - "timestamp": 0.6924937012294379 - }, - { - "x": 3.57574133188106, - "y": 2.4662968714042055, - "heading": -0.3932458693377865, - "angularVelocity": 0.8412930922288103, - "velocityX": 2.963083160600891, - "velocityY": -1.4255801188754254, - "timestamp": 0.7554476740684777 - }, - { - "x": 3.777810779911522, - "y": 2.369066952878771, - "heading": -0.3455559373144148, - "angularVelocity": 0.7575364964702819, - "velocityX": 3.209796600877132, - "velocityY": -1.5444604071935277, - "timestamp": 0.8184016469075175 - }, - { - "x": 3.9925130304181535, - "y": 2.2661623966169606, - "heading": -0.3455557491568548, - "angularVelocity": 0.0000029888115329808544, - "velocityX": 3.4104638805811245, - "velocityY": -1.63459987703898, - "timestamp": 0.8813556197465573 - }, - { - "x": 4.207067940824861, - "y": 2.162950872410893, - "heading": -0.34555573251479127, - "angularVelocity": 2.643528717349897e-7, - "velocityX": 3.408123438933375, - "velocityY": -1.6394759464975768, - "timestamp": 0.9443095925855971 - }, - { - "x": 4.421622842445454, - "y": 2.0597393299403546, - "heading": -0.34555571587272765, - "angularVelocity": 2.6435287281597733e-7, - "velocityX": 3.408123299369276, - "velocityY": -1.6394762366217663, - "timestamp": 1.007263565424637 - }, - { - "x": 4.636177744065525, - "y": 1.9565277874687297, - "heading": -0.3455556992306641, - "angularVelocity": 2.6435287314542613e-7, - "velocityX": 3.4081232993609745, - "velocityY": -1.6394762366390223, - "timestamp": 1.0702175382636767 - }, - { - "x": 4.850732645688907, - "y": 1.8533162450039902, - "heading": -0.34555568258860053, - "angularVelocity": 2.643528725209065e-7, - "velocityX": 3.408123299413585, - "velocityY": -1.6394762365296556, - "timestamp": 1.1331715111027165 - }, - { - "x": 5.065287602977393, - "y": 1.7501048182552732, - "heading": -0.345555665946537, - "angularVelocity": 2.6435287138683083e-7, - "velocityX": 3.4081241836326663, - "velocityY": -1.63947439842449, - "timestamp": 1.1961254839417563 - }, - { - "x": 5.280754789180012, - "y": 1.6488115807144432, - "heading": -0.34555564929395255, - "angularVelocity": 2.6451999265849417e-7, - "velocityX": 3.4226145942134942, - "velocityY": -1.6090046898202166, - "timestamp": 1.2590794567807961 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, - "angularVelocity": 2.673332686258823e-7, - "velocityX": 3.530338087601756, - "velocityY": -1.3564290298187671, - "timestamp": 1.322033429619836 - }, - { - "x": 5.652794664229654, - "y": 1.5139935049725783, - "heading": -0.34555563561361163, - "angularVelocity": -7.551097078688917e-8, - "velocityX": 3.591492706822241, - "velocityY": -1.185058979367509, - "timestamp": 1.363740619136216 - }, - { - "x": 5.804789442560329, - "y": 1.4718291930788845, - "heading": -0.345555638724863, - "angularVelocity": -7.459748339352162e-8, - "velocityX": 3.644330392269195, - "velocityY": -1.010960277655107, - "timestamp": 1.405447808652596 - }, - { - "x": 5.958396699024851, - "y": 1.435981562464388, - "heading": -0.3455556418269712, - "angularVelocity": -7.437825995471752e-8, - "velocityX": 3.6829922669374113, - "velocityY": -0.8595072223799174, - "timestamp": 1.4471549981689762 - }, - { - "x": 6.112005102436535, - "y": 1.4001388468787674, - "heading": -0.3455556449290776, - "angularVelocity": -7.437821592181375e-8, - "velocityX": 3.6830197669242586, - "velocityY": -0.8593893762979032, - "timestamp": 1.4888621876853563 - }, - { - "x": 6.2656135060730795, - "y": 1.3642961322568121, - "heading": -0.3455556480311839, - "angularVelocity": -7.437821455888137e-8, - "velocityX": 3.683019772315654, - "velocityY": -0.8593893531924107, - "timestamp": 1.5305693772017364 - }, - { - "x": 6.419221909709655, - "y": 1.3284534176349936, - "heading": -0.3455556511332902, - "angularVelocity": -7.437821520024811e-8, - "velocityX": 3.6830197723164173, - "velocityY": -0.8593893531891351, - "timestamp": 1.5722765667181164 - }, - { - "x": 6.572830313283965, - "y": 1.292610702746327, - "heading": -0.3455556542353965, - "angularVelocity": -7.437821564310945e-8, - "velocityX": 3.6830197708234897, - "velocityY": -0.859389359587264, - "timestamp": 1.6139837562344965 - }, - { - "x": 6.72643839927336, - "y": 1.2567666268353574, - "heading": -0.34555565733750304, - "angularVelocity": -7.43782201062097e-8, - "velocityX": 3.6830121561911366, - "velocityY": -0.8594219923855618, - "timestamp": 1.6556909457508766 - }, - { - "x": 6.87895592150965, - "y": 1.216534214678565, - "heading": -0.3455556607967647, - "angularVelocity": -8.294161584911855e-8, - "velocityX": 3.656864056409029, - "velocityY": -0.9646397329408027, - "timestamp": 1.6973981352672567 - }, - { - "x": 7.025398163354816, - "y": 1.1717644512232448, - "heading": -0.34674583940477344, - "angularVelocity": -0.02853653343247435, - "velocityX": 3.5111989933450576, - "velocityY": -1.073430359955979, - "timestamp": 1.7391053247836368 - }, - { - "x": 7.164406770259058, - "y": 1.1285160108597303, - "heading": -0.3473601506358553, - "angularVelocity": -0.014729144739916763, - "velocityX": 3.3329650958535093, - "velocityY": -1.0369540806994204, - "timestamp": 1.7808125143000169 - }, - { - "x": 7.296003099222487, - "y": 1.086889230520204, - "heading": -0.3473718226301098, - "angularVelocity": -0.0002798556889061875, - "velocityX": 3.155243268351752, - "velocityY": -0.9980720547755352, - "timestamp": 1.822519703816397 - }, - { - "x": 7.420194691021883, - "y": 1.0469180216341771, - "heading": -0.34677182369168125, - "angularVelocity": 0.014385983457189529, - "velocityX": 2.977702243653219, - "velocityY": -0.9583769453064839, - "timestamp": 1.864226893332777 - }, - { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, - "angularVelocity": 0.02916022972351966, - "velocityX": 2.8002535694982806, - "velocityY": -0.9182731897618387, - "timestamp": 1.905934082849157 - }, - { - "x": 7.694109552956691, - "y": 0.9554552181085981, - "heading": -0.3423846619503678, - "angularVelocity": 0.05119624293404516, - "velocityX": 2.536815276765165, - "velocityY": -0.8583516550337287, - "timestamp": 1.9678716456890424 - }, - { - "x": 7.835030504559327, - "y": 0.9064832871477253, - "heading": -0.33886448301682054, - "angularVelocity": 0.05683431462499228, - "velocityX": 2.2752098264978664, - "velocityY": -0.7906660952654867, - "timestamp": 2.0298092085289277 - }, - { - "x": 7.959827163561028, - "y": 0.861997905644216, - "heading": -0.3356203872211221, - "angularVelocity": 0.05237687191671972, - "velocityX": 2.014878424007598, - "velocityY": -0.7182294469433418, - "timestamp": 2.091746771368813 - }, - { - "x": 8.068556860689503, - "y": 0.8221974775860657, - "heading": -0.3330761096963967, - "angularVelocity": 0.04107810201222393, - "velocityX": 1.7554726428218221, - "velocityY": -0.6425895084221868, - "timestamp": 2.153684334208698 - }, - { - "x": 8.161262929678218, - "y": 0.7872247496182815, - "heading": -0.33153778712366017, - "angularVelocity": 0.02483666618774324, - "velocityX": 1.4967664973897774, - "velocityY": -0.5646448837225355, - "timestamp": 2.2156218970485835 - }, - { - "x": 8.237979200485006, - "y": 0.7571873196552872, - "heading": -0.33123696700010097, - "angularVelocity": 0.0048568285506627, - "velocityX": 1.2386065464846836, - "velocityY": -0.48496305934161354, - "timestamp": 2.2775594598884688 - }, - { - "x": 8.298732784455282, - "y": 0.7321691813322279, - "heading": -0.3323549200927614, - "angularVelocity": -0.018049678440697953, - "velocityX": 0.9808843161512628, - "velocityY": -0.4039251332464228, - "timestamp": 2.339497022728354 - }, - { - "x": 8.34354587924, - "y": 0.712237715373984, - "heading": -0.3350374219820423, - "angularVelocity": -0.04330977465508917, - "velocityX": 0.7235204733606144, - "velocityY": -0.3217993257140046, - "timestamp": 2.4014345855682393 - }, - { - "x": 8.372436985155346, - "y": 0.6974481694465224, - "heading": -0.33940426042561933, - "angularVelocity": -0.07050387912204041, - "velocityX": 0.46645532356563796, - "velocityY": -0.23878152851596665, - "timestamp": 2.4633721484081246 - }, - { - "x": 8.385421752929688, - "y": 0.6878466606140137, - "heading": -0.34555563246426124, - "angularVelocity": -0.09931569400855825, - "velocityX": 0.20964285934061647, - "velocityY": -0.15501915787887396, - "timestamp": 2.52530971124801 - }, - { - "x": 8.381221613866826, - "y": 0.6835499753387894, - "heading": -0.35422874908994917, - "angularVelocity": -0.13146724375807764, - "velocityX": -0.06366577665514053, - "velocityY": -0.06512922572222897, - "timestamp": 2.591281404102908 - }, - { - "x": 8.358990442151228, - "y": 0.6851839721097847, - "heading": -0.36494544812778174, - "angularVelocity": -0.16244389940702994, - "velocityX": -0.33698046470466403, - "velocityY": 0.024768149797052455, - "timestamp": 2.657253096957806 - }, - { - "x": 8.318727796191062, - "y": 0.692749219284977, - "heading": -0.37761522689458643, - "angularVelocity": -0.1920487139032685, - "velocityX": -0.6103018464104298, - "velocityY": 0.1146741404958606, - "timestamp": 2.723224789812704 - }, - { - "x": 8.260433187957593, - "y": 0.7062463828943455, - "heading": -0.3921309909220783, - "angularVelocity": -0.22003018869651583, - "velocityX": -0.8836306256637128, - "velocityY": 0.20459022688799033, - "timestamp": 2.7891964826676023 - }, - { - "x": 8.184106080259626, - "y": 0.7256762539187386, - "heading": -0.40836397752669645, - "angularVelocity": -0.24605987662499226, - "velocityX": -1.1569675476699668, - "velocityY": 0.2945183029807682, - "timestamp": 2.8551681755225005 - }, - { - "x": 8.089745888152331, - "y": 0.7510397855554056, - "heading": -0.4261563635075344, - "angularVelocity": -0.26969727789116266, - "velocityX": -1.4303133362794993, - "velocityY": 0.3844608276530502, - "timestamp": 2.9211398683773986 - }, - { - "x": 7.977351991056692, - "y": 0.7823381451863918, - "heading": -0.445310079113529, - "angularVelocity": -0.2903323346290704, - "velocityX": -1.7036685316358853, - "velocityY": 0.47442104752147907, - "timestamp": 2.9871115612322967 - }, - { - "x": 7.846923771917719, - "y": 0.8195727882757919, - "heading": -0.4655690572275494, - "angularVelocity": -0.30708592181466465, - "velocityX": -1.9770330803219518, - "velocityY": 0.5644033293384785, - "timestamp": 3.053083254087195 - }, - { - "x": 7.6984607267480145, - "y": 0.8627455646922209, - "heading": -0.48658930450770715, - "angularVelocity": -0.31862525229405353, - "velocityX": -2.2504052684572193, - "velocityY": 0.6544136514942158, - "timestamp": 3.119054946942093 - }, - { - "x": 7.531962771855591, - "y": 0.9118588688249081, - "heading": -0.5078842268520795, - "angularVelocity": -0.32278878141280287, - "velocityX": -2.5237787251969923, - "velocityY": 0.7444602678410827, - "timestamp": 3.185026639796991 - }, - { - "x": 7.347431182646945, - "y": 0.9669158200795512, - "heading": -0.5287129812318903, - "angularVelocity": -0.31572259977658373, - "velocityX": -2.7971328493042473, - "velocityY": 0.8345541681905657, - "timestamp": 3.250998332651889 - }, - { - "x": 7.144872022891467, - "y": 1.0279202386913098, - "heading": -0.5478111966657127, - "angularVelocity": -0.28949106211095804, - "velocityX": -3.0703950586958326, - "velocityY": 0.9247059757271185, - "timestamp": 3.3169700255067873 - }, - { - "x": 6.92431385893063, - "y": 1.0948740954299652, - "heading": -0.56252983495188, - "angularVelocity": -0.2231053600297971, - "velocityX": -3.343224259015806, - "velocityY": 1.0148876562242728, - "timestamp": 3.3829417183616854 - }, - { - "x": 6.686027050772059, - "y": 1.1677363127465568, - "heading": -0.5635938882487441, - "angularVelocity": -0.016128937288368943, - "velocityX": -3.611955337915506, - "velocityY": 1.1044466825619976, - "timestamp": 3.4489134112165836 - }, - { - "x": 6.448996512762522, - "y": 1.2456322264917639, - "heading": -0.5635938947986299, - "angularVelocity": -9.928327608034937e-8, - "velocityX": -3.592912774436053, - "velocityY": 1.180747535409401, - "timestamp": 3.5148851040714817 - }, - { - "x": 6.2119660667132655, - "y": 1.3235284200636117, - "heading": -0.5635939013485092, - "angularVelocity": -9.928317804375559e-8, - "velocityX": -3.5929113805006425, - "velocityY": 1.180751777026201, - "timestamp": 3.58085679692638 - }, - { - "x": 5.97493562067527, - "y": 1.4014246136697293, - "heading": -0.5635939078983885, - "angularVelocity": -9.928317794724392e-8, - "velocityX": -3.5929113803299297, - "velocityY": 1.180751777545662, - "timestamp": 3.646828489781278 - }, - { - "x": 5.737905297280568, - "y": 1.4793211804663164, - "heading": -0.5635939144482677, - "angularVelocity": -9.928317524609207e-8, - "velocityX": -3.5929095213008893, - "velocityY": 1.180757434372905, - "timestamp": 3.712800182636176 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.5635939210079286, - "angularVelocity": -9.943144757674115e-8, - "velocityX": -3.5606438133655525, - "velocityY": 1.2747558885869388, - "timestamp": 3.778771875491074 - }, - { - "x": 5.2859625551559315, - "y": 1.6593525606269757, - "heading": -0.5635939265318385, - "angularVelocity": -8.803796196510374e-8, - "velocityX": -3.459117077662582, - "velocityY": 1.5289526284347386, - "timestamp": 3.841516511544345 - }, - { - "x": 5.071426966449666, - "y": 1.7607653123039275, - "heading": -0.5635939320385204, - "angularVelocity": -8.776338927441567e-8, - "velocityX": -3.4191861201350386, - "velocityY": 1.6162776303436093, - "timestamp": 3.904261147597616 - }, - { - "x": 4.856891543272159, - "y": 1.8621784141511948, - "heading": -0.5635939375452019, - "angularVelocity": -8.776338338833754e-8, - "velocityX": -3.419183482000995, - "velocityY": 1.6162832112241017, - "timestamp": 3.9670057836508867 - }, - { - "x": 4.642356120103932, - "y": 1.9635915160180906, - "heading": -0.5635939430518834, - "angularVelocity": -8.776338232016452e-8, - "velocityX": -3.419183481853115, - "velocityY": 1.6162832115369357, - "timestamp": 4.029750419704158 - }, - { - "x": 4.427820696935705, - "y": 2.0650046178849877, - "heading": -0.5635939485585649, - "angularVelocity": -8.776338320403535e-8, - "velocityX": -3.419183481853107, - "velocityY": 1.616283211536953, - "timestamp": 4.0924950557574284 - }, - { - "x": 4.213285273767479, - "y": 2.1664177197518852, - "heading": -0.5635939540652464, - "angularVelocity": -8.776338205784925e-8, - "velocityX": -3.419183481853105, - "velocityY": 1.616283211536959, - "timestamp": 4.155239691810699 - }, - { - "x": 3.99874985060226, - "y": 2.2678308216251453, - "heading": -0.5635939595719279, - "angularVelocity": -8.776338204240658e-8, - "velocityX": -3.4191834818051703, - "velocityY": 1.616283211638361, - "timestamp": 4.21798432786397 - }, - { - "x": 3.7842144810928517, - "y": 2.369244037002313, - "heading": -0.5635939650808715, - "angularVelocity": -8.779943360299573e-8, - "velocityX": -3.4191826266593917, - "velocityY": 1.6162850206202093, - "timestamp": 4.280728963917241 - }, - { - "x": 3.582486282432141, - "y": 2.464592725052892, - "heading": -0.5976475269249973, - "angularVelocity": -0.5427326379774409, - "velocityX": -3.21506683837393, - "velocityY": 1.5196309046980254, - "timestamp": 4.343473599970512 - }, - { - "x": 3.3962755581787456, - "y": 2.5526066599990593, - "heading": -0.6290952664889575, - "angularVelocity": -0.5012020396016188, - "velocityX": -2.9677552690767404, - "velocityY": 1.4027324163844592, - "timestamp": 4.406218236023783 - }, - { - "x": 3.2255823358404876, - "y": 2.63328589857046, - "heading": -0.6579338206032609, - "angularVelocity": -0.4596178403173682, - "velocityX": -2.7204432613704155, - "velocityY": 1.2858348322062758, - "timestamp": 4.468962872077054 - }, - { - "x": 3.0704066332901094, - "y": 2.7066304971231805, - "heading": -0.684160167831089, - "angularVelocity": -0.4179854865292692, - "velocityX": -2.4731309688151453, - "velocityY": 1.168938146209836, - "timestamp": 4.531707508130324 - }, - { - "x": 2.930748464239733, - "y": 2.7726405069094633, - "heading": -0.7077714539525848, - "angularVelocity": -0.3763076432772636, - "velocityX": -2.22581845772133, - "velocityY": 1.05204227705202, - "timestamp": 4.594452144183595 - }, - { - "x": 2.8066078397359373, - "y": 2.83131597316776, - "heading": -0.7287650298436226, - "angularVelocity": -0.3345875792986393, - "velocityX": -1.978505770571369, - "velocityY": 0.9351471288873412, - "timestamp": 4.657196780236866 - }, - { - "x": 2.697984768963403, - "y": 2.882656934878273, - "heading": -0.7471385155745514, - "angularVelocity": -0.2928295849119221, - "velocityX": -1.7311929370394774, - "velocityY": 0.8182526019742015, - "timestamp": 4.719941416290137 - }, - { - "x": 2.604879259815539, - "y": 2.926663424723888, - "heading": -0.7628898637084475, - "angularVelocity": -0.25103895925897446, - "velocityX": -1.4838799777054614, - "velocityY": 0.7013585959483968, - "timestamp": 4.782686052343408 - }, - { - "x": 2.5272913193421016, - "y": 2.963335469132253, - "heading": -0.7760174152523261, - "angularVelocity": -0.20922189321065382, - "velocityX": -1.236566906015123, - "velocityY": 0.584465011116332, - "timestamp": 4.845430688396679 - }, - { - "x": 2.465220954107753, - "y": 2.9926730883504185, - "heading": -0.7865199463491176, - "angularVelocity": -0.1673853217966667, - "velocityX": -0.9892537296997038, - "velocityY": 0.46757174897401693, - "timestamp": 4.9081753244499495 - }, - { - "x": 2.418668170473455, - "y": 3.014676296527397, - "heading": -0.7943967051912195, - "angularVelocity": -0.12553676836079783, - "velocityX": -0.7419404520057251, - "velocityY": 0.3506787123332304, - "timestamp": 4.97091996050322 - }, - { - "x": 2.38763297480514, - "y": 3.0293451017899056, - "heading": -0.7996474391139023, - "angularVelocity": -0.08368418805115034, - "velocityX": -0.49462707285394797, - "velocityY": 0.23378580521296682, - "timestamp": 5.033664596556491 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -0.0418358130797821, - "velocityX": -0.24731358996990807, - "velocityY": 0.11689293258068652, - "timestamp": 5.096409232609762 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": 1.2905382194087526e-36, - "velocityX": 1.2188676912146853e-38, - "velocityY": -1.9456328527917923e-39, - "timestamp": 5.159153868663033 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint (1).4.traj b/src/main/deploy/choreo/SourceLanePHGSprint (1).4.traj deleted file mode 100644 index 274b8c90..00000000 --- a/src/main/deploy/choreo/SourceLanePHGSprint (1).4.traj +++ /dev/null @@ -1,355 +0,0 @@ -{ - "samples": [ - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": 1.2905382194087526e-36, - "velocityX": 1.2188676912146853e-38, - "velocityY": -1.9456328527917923e-39, - "timestamp": 0 - }, - { - "x": 2.3862784269967885, - "y": 3.0299921206795677, - "heading": -0.7979445022798209, - "angularVelocity": 0.07214760590413372, - "velocityX": 0.23610252175550234, - "velocityY": -0.11148080618080955, - "timestamp": 0.059986878920357256 - }, - { - "x": 2.414605712310544, - "y": 3.016617241283919, - "heading": -0.7893555783917804, - "angularVelocity": 0.14318004274641105, - "velocityX": 0.4722246901920793, - "velocityY": -0.22296341527296173, - "timestamp": 0.11997375784071451 - }, - { - "x": 2.457098595113841, - "y": 2.996554734272641, - "heading": -0.7765849369819305, - "angularVelocity": 0.21289057940162145, - "velocityX": 0.7083696229589316, - "velocityY": -0.3344482555579276, - "timestamp": 0.17996063676107177 - }, - { - "x": 2.5137586742709717, - "y": 2.9698044458506936, - "heading": -0.7597273153290622, - "angularVelocity": 0.28102181604163, - "velocityX": 0.9445412092927212, - "velocityY": -0.44593565965421605, - "timestamp": 0.23994751568142902 - }, - { - "x": 2.5845878442638655, - "y": 2.9363662114763, - "heading": -0.7388971783301495, - "angularVelocity": 0.34724488711219914, - "velocityX": 1.180744377231747, - "velocityY": -0.5574258067133014, - "timestamp": 0.2999343946017863 - }, - { - "x": 2.6695883810159726, - "y": 2.8962398712988433, - "heading": -0.7142349782126007, - "angularVelocity": 0.4111265757015274, - "velocityX": 1.4169854855252446, - "velocityY": -0.6689186185320777, - "timestamp": 0.35992127352214354 - }, - { - "x": 2.7687630628600255, - "y": 2.849425297465716, - "heading": -0.6859166617272401, - "angularVelocity": 0.4720751770225938, - "velocityX": 1.653272909492815, - "velocityY": -0.7804135616937368, - "timestamp": 0.4199081524425008 - }, - { - "x": 2.882115344958, - "y": 2.795922445610523, - "heading": -0.6541688123580988, - "angularVelocity": 0.529246560923628, - "velocityX": 1.8896179320892517, - "velocityY": -0.8919092444570627, - "timestamp": 0.47989503136285805 - }, - { - "x": 3.009649615335316, - "y": 2.73573145985777, - "heading": -0.6192942663212875, - "angularVelocity": 0.5813695705541366, - "velocityX": 2.1260361044394105, - "velocityY": -1.0034025246198652, - "timestamp": 0.5398819102832153 - }, - { - "x": 3.1513715692279023, - "y": 2.6688529097388005, - "heading": -0.5817190571900686, - "angularVelocity": 0.626390467507171, - "velocityX": 2.3625492181506127, - "velocityY": -1.114886310517361, - "timestamp": 0.5998687892035726 - }, - { - "x": 3.3072887013147403, - "y": 2.5952884046594, - "heading": -0.5420886244414693, - "angularVelocity": 0.6606516868666416, - "velocityX": 2.5991872705003334, - "velocityY": -1.2263432671179573, - "timestamp": 0.6598556681239298 - }, - { - "x": 3.4774104648990076, - "y": 2.5150425607670805, - "heading": -0.50150111026719, - "angularVelocity": 0.6766065330414328, - "velocityX": 2.8359829123654157, - "velocityY": -1.33772327109832, - "timestamp": 0.7198425470442871 - }, - { - "x": 3.6617427045443476, - "y": 2.428132084038069, - "heading": -0.4622625664613271, - "angularVelocity": 0.6541187758402723, - "velocityX": 3.0728759849311564, - "velocityY": -1.4488247812392312, - "timestamp": 0.7798294259646443 - }, - { - "x": 3.8601464039752953, - "y": 2.3346891355318427, - "heading": -0.4327535611710116, - "angularVelocity": 0.49192433114404066, - "velocityX": 3.3074516127828706, - "velocityY": -1.5577231252568986, - "timestamp": 0.8398163048850016 - }, - { - "x": 4.065242173401204, - "y": 2.237711412138144, - "heading": -0.4327535477418676, - "angularVelocity": 2.2386802351026997e-7, - "velocityX": 3.4190105089182294, - "velocityY": -1.6166489262168848, - "timestamp": 0.8998031838053588 - }, - { - "x": 4.2703379249938225, - "y": 2.140733651028875, - "heading": -0.43275353431315516, - "angularVelocity": 2.2386082807599726e-7, - "velocityX": 3.419010211631728, - "velocityY": -1.6166495549472233, - "timestamp": 0.9597900627257161 - }, - { - "x": 4.475433676585232, - "y": 2.0437558899170503, - "heading": -0.43275352088444274, - "angularVelocity": 2.2386082918741598e-7, - "velocityX": 3.4190102116115857, - "velocityY": -1.616649554989821, - "timestamp": 1.0197769416460734 - }, - { - "x": 4.6805294281766425, - "y": 1.9467781288052255, - "heading": -0.4327535074557304, - "angularVelocity": 2.2386082819075723e-7, - "velocityX": 3.4190102116115852, - "velocityY": -1.616649554989824, - "timestamp": 1.0797638205664306 - }, - { - "x": 4.885625179768054, - "y": 1.8498003676934025, - "heading": -0.43275349402701796, - "angularVelocity": 2.2386082903608912e-7, - "velocityX": 3.4190102116115972, - "velocityY": -1.6166495549897977, - "timestamp": 1.1397506994867879 - }, - { - "x": 5.090720931370459, - "y": 1.7528226066048298, - "heading": -0.43275348059830554, - "angularVelocity": 2.2386082830289002e-7, - "velocityX": 3.4190102117948658, - "velocityY": -1.6166495546022077, - "timestamp": 1.1997375784071451 - }, - { - "x": 5.29581684523517, - "y": 1.6558451886813434, - "heading": -0.43275346716959256, - "angularVelocity": 2.2386083719980442e-7, - "velocityX": 3.4190129167581613, - "velocityY": -1.6166438339330798, - "timestamp": 1.2597244573275024 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.43275345370859003, - "angularVelocity": 2.243991161112281e-7, - "velocityX": 3.453867841656271, - "velocityY": -1.5407736813730113, - "timestamp": 1.3197113362478596 - }, - { - "x": 5.767637732978587, - "y": 1.4715486447521433, - "heading": -0.4327534418952186, - "angularVelocity": 1.5949042666645356e-7, - "velocityX": 3.572782883962456, - "velocityY": -1.2403266728714897, - "timestamp": 1.3937808065498363 - }, - { - "x": 6.033774192023134, - "y": 1.384125705645109, - "heading": -0.432753430099036, - "angularVelocity": 1.5925836332865082e-7, - "velocityX": 3.593065509440347, - "velocityY": -1.1802830336252828, - "timestamp": 1.467850276851813 - }, - { - "x": 6.2999107025526015, - "y": 1.2967029232707166, - "heading": -0.43275341830285335, - "angularVelocity": 1.5925836332521055e-7, - "velocityX": 3.5930662045299844, - "velocityY": -1.1802809176031086, - "timestamp": 1.5419197471537895 - }, - { - "x": 6.566047160708243, - "y": 1.2092799814598572, - "heading": -0.43275340650489624, - "angularVelocity": 1.5928232033441407e-7, - "velocityX": 3.593065497439389, - "velocityY": -1.1802830701291938, - "timestamp": 1.6159892174557662 - }, - { - "x": 6.815604168443372, - "y": 1.1270295237235333, - "heading": -0.36800659105834704, - "angularVelocity": 0.8741363369088632, - "velocityX": 3.3692290051177802, - "velocityY": -1.110450194945293, - "timestamp": 1.6900586877577428 - }, - { - "x": 7.0424486777806585, - "y": 1.0523256982782683, - "heading": -0.304588986464792, - "angularVelocity": 0.8561908750664192, - "velocityX": 3.0625912189253843, - "velocityY": -1.0085643267152118, - "timestamp": 1.7641281580597195 - }, - { - "x": 7.246590574581559, - "y": 0.9851213052520007, - "heading": -0.24567984244132185, - "angularVelocity": 0.7953228743678282, - "velocityX": 2.7560869001577597, - "velocityY": -0.9073156963628868, - "timestamp": 1.838197628361696 - }, - { - "x": 7.428038548794457, - "y": 0.9253999088853845, - "heading": -0.19230647763371986, - "angularVelocity": 0.7205852099387576, - "velocityX": 2.4496999029849618, - "velocityY": -0.8062889625528061, - "timestamp": 1.9122670986636727 - }, - { - "x": 7.586797949690041, - "y": 0.8731533407207944, - "heading": -0.1449799929692073, - "angularVelocity": 0.6389472541327181, - "velocityX": 2.1433851254549694, - "velocityY": -0.705372509774734, - "timestamp": 1.9863365689656494 - }, - { - "x": 7.722872327084393, - "y": 0.8283767866857153, - "heading": -0.10400779573923778, - "angularVelocity": 0.5531590419497899, - "velocityX": 1.8371182734206921, - "velocityY": -0.6045210510150558, - "timestamp": 2.060406039267626 - }, - { - "x": 7.836264194581954, - "y": 0.7910670998170554, - "heading": -0.06959570799723012, - "angularVelocity": 0.46459205934255987, - "velocityX": 1.530885357155523, - "velocityY": -0.5037120788977025, - "timestamp": 2.1344755095696026 - }, - { - "x": 7.926975414267802, - "y": 0.7612220655404804, - "heading": -0.04189104317473826, - "angularVelocity": 0.3740362218001811, - "velocityX": 1.224677580601353, - "velocityY": -0.4029330053920843, - "timestamp": 2.2085449798715793 - }, - { - "x": 7.995007406512269, - "y": 0.7388400306235591, - "heading": -0.021003806792755122, - "angularVelocity": 0.28199521741990763, - "velocityX": 0.9184889802384877, - "velocityY": -0.3021762519115013, - "timestamp": 2.282614450173556 - }, - { - "x": 8.040361272813582, - "y": 0.7239196956147146, - "heading": -0.0070182972067734635, - "angularVelocity": 0.18881611450660718, - "velocityX": 0.612315251025934, - "velocityY": -0.20143704211755914, - "timestamp": 2.3566839204755325 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": -5.172058523180806e-42, - "angularVelocity": 0.09475290127174331, - "velocityX": 0.30615312096089364, - "velocityY": -0.10071229126619016, - "timestamp": 2.430753390777509 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": -2.2197990868657932e-42, - "angularVelocity": 3.9858008032330975e-41, - "velocityX": -4.443522904196121e-42, - "velocityY": 7.940802598890531e-42, - "timestamp": 2.504822861079486 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint (1).traj b/src/main/deploy/choreo/SourceLanePHGSprint (1).traj deleted file mode 100644 index 6d244255..00000000 --- a/src/main/deploy/choreo/SourceLanePHGSprint (1).traj +++ /dev/null @@ -1,2101 +0,0 @@ -{ - "samples": [ - { - "x": 0.433241993188858, - "y": 4.121134281158447, - "heading": -3.528930169589175e-36, - "angularVelocity": 0, - "velocityX": -3.4629926489463697e-37, - "velocityY": -7.866244288467422e-37, - "timestamp": 0 - }, - { - "x": 0.45469188032137553, - "y": 4.109218526997661, - "heading": -0.008937300145995173, - "angularVelocity": -0.1189323726299145, - "velocityX": 0.2854425752342375, - "velocityY": -0.15856789979825622, - "timestamp": 0.07514606787342622 - }, - { - "x": 0.49759166351162515, - "y": 4.0853873108981436, - "heading": -0.026812413296636604, - "angularVelocity": -0.237871570083344, - "velocityX": 0.5708852692400179, - "velocityY": -0.3171319108760035, - "timestamp": 0.15029213574685243 - }, - { - "x": 0.5619414537480725, - "y": 4.049641027140498, - "heading": -0.053621717098024775, - "angularVelocity": -0.3567625633658565, - "velocityX": 0.856329440215504, - "velocityY": -0.4756906750976659, - "timestamp": 0.22543820362027867 - }, - { - "x": 0.6477414523773106, - "y": 4.001980177860296, - "heading": -0.08935853104911512, - "angularVelocity": -0.47556465644063195, - "velocityX": 1.141776290593902, - "velocityY": -0.6342427571923072, - "timestamp": 0.30058427149370487 - }, - { - "x": 0.7549919358777414, - "y": 3.942405375853288, - "heading": -0.1340144220065316, - "angularVelocity": -0.5942545261667388, - "velocityX": 1.4272268201854585, - "velocityY": -0.7927866845588477, - "timestamp": 0.37573033936713107 - }, - { - "x": 0.8836932393269182, - "y": 3.8709173416225418, - "heading": -0.18758057158075767, - "angularVelocity": -0.7128270459134506, - "velocityX": 1.712681808793474, - "velocityY": -0.951321023891208, - "timestamp": 0.4508764072405573 - }, - { - "x": 1.0338457421402325, - "y": 3.787516893297593, - "heading": -0.2500489995023071, - "angularVelocity": -0.8312933688928251, - "velocityX": 1.9981418464400118, - "velocityY": -1.1098444760333435, - "timestamp": 0.5260224751139835 - }, - { - "x": 1.2054498603190866, - "y": 3.6922049311693006, - "heading": -0.32141344961465573, - "angularVelocity": -0.9496764385936043, - "velocityX": 2.283607419990343, - "velocityY": -1.268355947630325, - "timestamp": 0.6011685429874097 - }, - { - "x": 1.3985060453231255, - "y": 3.584982424979259, - "heading": -0.4016698579859379, - "angularVelocity": -1.0680054278616897, - "velocityX": 2.5690790013020615, - "velocityY": -1.4268545144723217, - "timestamp": 0.676314610860836 - }, - { - "x": 1.5915937569821008, - "y": 3.4778198154599647, - "heading": -0.48176859267038585, - "angularVelocity": -1.065907198489266, - "velocityX": 2.5694985396202825, - "velocityY": -1.4260574445464829, - "timestamp": 0.7514606787342623 - }, - { - "x": 1.7632284858900336, - "y": 3.3825660365200445, - "heading": -0.5529765476804539, - "angularVelocity": -0.9475938931363466, - "velocityX": 2.2840147697019786, - "velocityY": -1.2675816797275798, - "timestamp": 0.8266067466076885 - }, - { - "x": 1.9134098098562764, - "y": 3.2992204007390877, - "heading": -0.6152918418017526, - "angularVelocity": -0.8292555536806094, - "velocityX": 1.9985253815170194, - "velocityY": -1.1091150626981794, - "timestamp": 0.9017528144811148 - }, - { - "x": 2.0421373577646693, - "y": 3.2277823059512354, - "heading": -0.6687115003742365, - "angularVelocity": -0.7108776291856339, - "velocityX": 1.7130310547348626, - "velocityY": -0.9506564589404817, - "timestamp": 0.976898882354541 - }, - { - "x": 2.149410808485792, - "y": 3.1682512394298143, - "heading": -0.7132320418113071, - "angularVelocity": -0.5924533737687983, - "velocityX": 1.4275324545498613, - "velocityY": -0.7922046782500071, - "timestamp": 1.0520449502279672 - }, - { - "x": 2.2352298936713018, - "y": 3.120626784362015, - "heading": -0.7488501306819737, - "angularVelocity": -0.4739847323836121, - "velocityX": 1.1420302833417786, - "velocityY": -0.6337584442610683, - "timestamp": 1.1271910181013933 - }, - { - "x": 2.2995943987852443, - "y": 3.0849086278583613, - "heading": -0.775563262460788, - "angularVelocity": -0.3554827622359309, - "velocityX": 0.8565252572144717, - "velocityY": -0.4753163740226096, - "timestamp": 1.2023370859748195 - }, - { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080858, - "angularVelocity": -0.23696688264902366, - "velocityX": 0.5710180813549187, - "velocityY": -0.3168769856057537, - "timestamp": 1.2774831538482456 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550472, - "velocityX": 0.28550943973314774, - "velocityY": -0.15843872456938407, - "timestamp": 1.3526292217216718 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 9.576319315825878e-33, - "velocityX": -3.340074147697442e-33, - "velocityY": 1.4972964833121403e-33, - "timestamp": 1.427775289595098 - }, - { - "x": 2.3773546785315864, - "y": 3.0407954088560687, - "heading": -0.7860770412156382, - "angularVelocity": 0.26500042058499174, - "velocityX": 0.21918861143707746, - "velocityY": -0.13736692695017713, - "timestamp": 1.4888897992558612 - }, - { - "x": 2.4041715283150715, - "y": 3.0239975454455843, - "heading": -0.7541197928104101, - "angularVelocity": 0.5229077118120982, - "velocityX": 0.438796775632191, - "velocityY": -0.27485884291187945, - "timestamp": 1.5500043089166244 - }, - { - "x": 2.4444388170293236, - "y": 2.998788446329585, - "heading": -0.7069088657797715, - "angularVelocity": 0.7724994815911764, - "velocityX": 0.6588826276733535, - "velocityY": -0.41248959135778174, - "timestamp": 1.6111188185773877 - }, - { - "x": 2.498190798561039, - "y": 2.965157152573787, - "heading": -0.6450804787000076, - "angularVelocity": 1.0116809808826634, - "velocityX": 0.8795289666903056, - "velocityY": -0.5502996578468836, - "timestamp": 1.672233328238151 - }, - { - "x": 2.5654677170302604, - "y": 2.923088008476279, - "heading": -0.5694581806203519, - "angularVelocity": 1.2373869724133075, - "velocityX": 1.1008338092322851, - "velocityY": -0.6883658943027918, - "timestamp": 1.7333478378989142 - }, - { - "x": 2.646315775914165, - "y": 2.8725583813996622, - "heading": -0.48110952859275347, - "angularVelocity": 1.4456248199978636, - "velocityX": 1.3228946666295571, - "velocityY": -0.8268024624119353, - "timestamp": 1.7944623475596775 - }, - { - "x": 2.7407862870930626, - "y": 2.813537598490058, - "heading": -0.3813947698765895, - "angularVelocity": 1.631605313855327, - "velocityX": 1.5457951262848801, - "velocityY": -0.965740922036666, - "timestamp": 1.8555768572204407 - }, - { - "x": 2.84893500974016, - "y": 2.7459886359527177, - "heading": -0.2720274509201168, - "angularVelocity": 1.7895475160244767, - "velocityX": 1.7696079580350594, - "velocityY": -1.105285191884772, - "timestamp": 1.916691366881204 - }, - { - "x": 2.9708226363170516, - "y": 2.669873093439706, - "heading": -0.15523119808259803, - "angularVelocity": 1.9111051284848068, - "velocityX": 1.9944138839282306, - "velocityY": -1.2454577961194009, - "timestamp": 1.9778058765419673 - }, - { - "x": 3.1065122638812066, - "y": 2.5851587340080315, - "heading": -0.03419676462946552, - "angularVelocity": 1.9804533182868547, - "velocityX": 2.220252249708728, - "velocityY": -1.3861578846318265, - "timestamp": 2.0389203862027303 - }, - { - "x": 3.2560356571448454, - "y": 2.4918371927335334, - "heading": 0.08572826850512158, - "angularVelocity": 1.9623005044182085, - "velocityX": 2.446610372784124, - "velocityY": -1.5269948461095475, - "timestamp": 2.1000348958634936 - }, - { - "x": 3.419180860242083, - "y": 2.390029027608911, - "heading": 0.19415018142326163, - "angularVelocity": 1.7740780956923754, - "velocityX": 2.6695003200194134, - "velocityY": -1.6658591501387063, - "timestamp": 2.161149405524257 - }, - { - "x": 3.5929644641848295, - "y": 2.279455345020229, - "heading": 0.2670576620012504, - "angularVelocity": 1.1929651564364419, - "velocityX": 2.8435735622750062, - "velocityY": -1.809286914064415, - "timestamp": 2.22226391518502 - }, - { - "x": 3.776865546474961, - "y": 2.1600020280366277, - "heading": 0.3015223704483068, - "angularVelocity": 0.5639365944088309, - "velocityX": 3.0091230922237155, - "velocityY": -1.9545819421061736, - "timestamp": 2.2833784248457833 - }, - { - "x": 3.9728967066647076, - "y": 2.0375522576737963, - "heading": 0.30152241765502197, - "angularVelocity": 7.724305629887772e-7, - "velocityX": 3.2076042379769354, - "velocityY": -2.003612088889046, - "timestamp": 2.3444929345065466 - }, - { - "x": 4.174386727718979, - "y": 1.9243094504352285, - "heading": 0.30152246485493484, - "angularVelocity": 7.72319260587173e-7, - "velocityX": 3.2969260847008353, - "velocityY": -1.8529610704096402, - "timestamp": 2.40560744416731 - }, - { - "x": 4.383354452565465, - "y": 1.8255438101811148, - "heading": 0.3015225128834295, - "angularVelocity": 7.858771167686507e-7, - "velocityX": 3.4192817058736558, - "velocityY": -1.6160751481496929, - "timestamp": 2.466721953828073 - }, - { - "x": 4.598761540028333, - "y": 1.7417467464566943, - "heading": 0.30152256294147645, - "angularVelocity": 8.190861254028443e-7, - "velocityX": 3.524647234487482, - "velocityY": -1.3711484259558735, - "timestamp": 2.5278364634888364 - }, - { - "x": 4.819537390717514, - "y": 1.6733348748555603, - "heading": 0.3015226164690873, - "angularVelocity": 8.758576513463982e-7, - "velocityX": 3.612494838208991, - "velocityY": -1.1194047367945394, - "timestamp": 2.5889509731495997 - }, - { - "x": 5.04458467733647, - "y": 1.620648272623701, - "heading": 0.30152267534909954, - "angularVelocity": 9.634375302003616e-7, - "velocityX": 3.682387175617667, - "velocityY": -0.8620964567058493, - "timestamp": 2.650065482810363 - }, - { - "x": 5.27278482830379, - "y": 1.5839488347993986, - "heading": 0.3015227422489141, - "angularVelocity": 0.0000010946633609592033, - "velocityX": 3.7339766322927472, - "velocityY": -0.6005028597630111, - "timestamp": 2.711179992471126 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.3015228161300745, - "angularVelocity": 0.0000012088972118666571, - "velocityX": 3.7670067261218194, - "velocityY": -0.33592432468507805, - "timestamp": 2.7722945021318894 - }, - { - "x": 5.642161233224445, - "y": 1.5569512204704454, - "heading": 0.30152289610635336, - "angularVelocity": 0.0000021712102287950276, - "velocityX": 3.777876226736954, - "velocityY": -0.1755880047102645, - "timestamp": 2.809129387028348 - }, - { - "x": 5.781468008961295, - "y": 1.556401100893367, - "heading": 0.3015229698630755, - "angularVelocity": 0.0000020023605971612715, - "velocityX": 3.7819250997644014, - "velocityY": -0.01493474402390838, - "timestamp": 2.8459642719248066 - }, - { - "x": 5.920672391336217, - "y": 1.5617696165699877, - "heading": 0.3015230386365789, - "angularVelocity": 0.0000018670752917486593, - "velocityX": 3.779145306581512, - "velocityY": 0.14574541746801997, - "timestamp": 2.882799156821265 - }, - { - "x": 6.0595230325638285, - "y": 1.5730470711005764, - "heading": 0.30152310340837535, - "angularVelocity": 0.000001758436237719359, - "velocityX": 3.769541879061509, - "velocityY": 0.3061623393771694, - "timestamp": 2.919634041717724 - }, - { - "x": 6.197769224123459, - "y": 1.5902130982406153, - "heading": 0.3015231649742211, - "angularVelocity": 0.0000016714005183062998, - "velocityX": 3.753132172076425, - "velocityY": 0.4660263548614807, - "timestamp": 2.9564689266141824 - }, - { - "x": 6.335161349542959, - "y": 1.6132366986444322, - "heading": 0.3015232239953201, - "angularVelocity": 0.0000016023152826544925, - "velocityX": 3.7299458327534505, - "velocityY": 0.6250487946015111, - "timestamp": 2.993303811510641 - }, - { - "x": 6.471451335210211, - "y": 1.6420762959087596, - "heading": 0.3015232810350899, - "angularVelocity": 0.000001548525804727782, - "velocityX": 3.7000247469309935, - "velocityY": 0.7829425107583309, - "timestamp": 3.0301386964070995 - }, - { - "x": 6.606393098338155, - "y": 1.6766798119201123, - "heading": 0.3015233366126912, - "angularVelocity": 0.0000015088305949550033, - "velocityX": 3.6634229618813627, - "velocityY": 0.9394224010369983, - "timestamp": 3.066973581303558 - }, - { - "x": 6.739742463422568, - "y": 1.7169846140875678, - "heading": 0.30152556567084704, - "angularVelocity": 0.00006051486687687375, - "velocityX": 3.6201922568579414, - "velocityY": 1.0942019306087243, - "timestamp": 3.1038084662000167 - }, - { - "x": 6.8694207904091344, - "y": 1.7614703171513668, - "heading": 0.3101205256410533, - "angularVelocity": 0.23333750042565127, - "velocityX": 3.520530262306672, - "velocityY": 1.2077057710061154, - "timestamp": 3.1406433510964753 - }, - { - "x": 6.994764102779472, - "y": 1.8086931078781319, - "heading": 0.3306180633638748, - "angularVelocity": 0.5564707961064402, - "velocityX": 3.4028425152588877, - "velocityY": 1.2820127132066854, - "timestamp": 3.177478235992934 - }, - { - "x": 7.114597005425528, - "y": 1.8565129203183168, - "heading": 0.359627713397502, - "angularVelocity": 0.7875591335542955, - "velocityX": 3.2532449329732183, - "velocityY": 1.298220764761567, - "timestamp": 3.2143131208893925 - }, - { - "x": 7.228563598795877, - "y": 1.903963761474251, - "heading": 0.39146295222083616, - "angularVelocity": 0.8642687200685345, - "velocityX": 3.0939853264291135, - "velocityY": 1.288203866777826, - "timestamp": 3.251148005785851 - }, - { - "x": 7.336676614220082, - "y": 1.9505339221399112, - "heading": 0.42341393247502157, - "angularVelocity": 0.8674108889982476, - "velocityX": 2.9350713522821095, - "velocityY": 1.2642949963483492, - "timestamp": 3.2879828906823096 - }, - { - "x": 7.438996464546078, - "y": 1.9959395107165516, - "heading": 0.45395278545537515, - "angularVelocity": 0.8290742068611577, - "velocityX": 2.777797476865021, - "velocityY": 1.2326789863542045, - "timestamp": 3.3248177755787682 - }, - { - "x": 7.535582073954796, - "y": 2.0400031624519186, - "heading": 0.4820829146791016, - "angularVelocity": 0.7636817463336469, - "velocityX": 2.622123285581467, - "velocityY": 1.1962478465516526, - "timestamp": 3.361652660475227 - }, - { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.6791276743107278, - "velocityX": 2.4678193928677445, - "velocityY": 1.1565397180800696, - "timestamp": 3.3984875453716854 - }, - { - "x": 7.766333855095899, - "y": 2.151391160116789, - "heading": 0.5393931714770351, - "angularVelocity": 0.508950730683683, - "velocityX": 2.2039777611896034, - "velocityY": 1.0840548028640593, - "timestamp": 3.461940970502476 - }, - { - "x": 7.8897168213730975, - "y": 2.2145350229010927, - "heading": 0.5621690879226864, - "angularVelocity": 0.3589391179231936, - "velocityX": 1.9444650312080196, - "velocityY": 0.9951214241650663, - "timestamp": 3.5253943956332665 - }, - { - "x": 7.996856912991746, - "y": 2.2713427782005033, - "heading": 0.5763089063837279, - "angularVelocity": 0.22283774960762856, - "velocityX": 1.6884839769927564, - "velocityY": 0.8952669644281364, - "timestamp": 3.588847820764057 - }, - { - "x": 8.08793407853681, - "y": 2.3213235528188796, - "heading": 0.5824533246990756, - "angularVelocity": 0.09683351690917823, - "velocityX": 1.4353388387992243, - "velocityY": 0.7876765441007405, - "timestamp": 3.6523012458948476 - }, - { - "x": 8.16309393881241, - "y": 2.364112462024999, - "heading": 0.5810872026138336, - "angularVelocity": -0.021529524725041688, - "velocityX": 1.1844886248564515, - "velocityY": 0.6743356898059116, - "timestamp": 3.715754671025638 - }, - { - "x": 8.222455939995415, - "y": 2.3994280671308665, - "heading": 0.5725898810376202, - "angularVelocity": -0.13391430893917458, - "velocityX": 0.9355208337555886, - "velocityY": 0.5565594770191679, - "timestamp": 3.7792080961564287 - }, - { - "x": 8.266119462815809, - "y": 2.427046891293226, - "heading": 0.5572661381126698, - "angularVelocity": -0.24149591441856877, - "velocityX": 0.6881192422693246, - "velocityY": 0.4352613606819736, - "timestamp": 3.8426615212872193 - }, - { - "x": 8.294168308263885, - "y": 2.446787281465013, - "heading": 0.5353660980042331, - "angularVelocity": -0.3451356654632322, - "velocityX": 0.4420383200790658, - "velocityY": 0.31110046669817776, - "timestamp": 3.90611494641801 - }, - { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.4454857015776663, - "velocityX": 0.19708463824313482, - "velocityY": 0.18456741878341767, - "timestamp": 3.9695683715488004 - }, - { - "x": 8.303024530407173, - "y": 2.4618811275821986, - "heading": 0.4711910323232683, - "angularVelocity": -0.5465136547157353, - "velocityX": -0.055545178149817624, - "velocityY": 0.05148048067311967, - "timestamp": 4.035271161880262 - }, - { - "x": 8.282750419132642, - "y": 2.456558519757899, - "heading": 0.42887568169046636, - "angularVelocity": -0.6440419108431609, - "velocityX": -0.30857306321773753, - "velocityY": -0.08101037714605691, - "timestamp": 4.1009739522117235 - }, - { - "x": 8.245821395629305, - "y": 2.4425769262851884, - "heading": 0.3804195474569322, - "angularVelocity": -0.7375049672788591, - "velocityX": -0.5620617224479451, - "velocityY": -0.2128006040866038, - "timestamp": 4.166676742543185 - }, - { - "x": 8.19220201387014, - "y": 2.4199912479232255, - "heading": 0.32613641295591106, - "angularVelocity": -0.8261922245184766, - "velocityX": -0.8160898721144464, - "velocityY": -0.3437552385221621, - "timestamp": 4.2323795328746465 - }, - { - "x": 8.121850214183777, - "y": 2.388868090056124, - "heading": 0.2664003971864744, - "angularVelocity": -0.9091853704854365, - "velocityX": -1.0707581722397086, - "velocityY": -0.4736961354318394, - "timestamp": 4.298082323206108 - }, - { - "x": 8.034715295558527, - "y": 2.3492899199909276, - "heading": 0.20166644793914107, - "angularVelocity": -0.9852541866298157, - "velocityX": -1.32619814448768, - "velocityY": -0.6023818754961511, - "timestamp": 4.36378511353757 - }, - { - "x": 7.930734985336223, - "y": 2.3013613961682027, - "heading": 0.13250258742090648, - "angularVelocity": -1.0526776742557293, - "velocityX": -1.582585909939886, - "velocityY": -0.7294747084702521, - "timestamp": 4.429487903869031 - }, - { - "x": 7.809831081747402, - "y": 2.2452194829459002, - "heading": 0.05964299707705179, - "angularVelocity": -1.1089268808263428, - "velocityX": -1.8401639105261383, - "velocityY": -0.8544829365552755, - "timestamp": 4.495190694200493 - }, - { - "x": 7.671902780233126, - "y": 2.1810506699963903, - "heading": -0.015920256793081004, - "angularVelocity": -1.1500767850029923, - "velocityX": -2.099276161917122, - "velocityY": -0.976652781804051, - "timestamp": 4.560893484531954 - }, - { - "x": 7.516816185598605, - "y": 2.109122861945235, - "heading": -0.09276870443286901, - "angularVelocity": -1.1696375032490753, - "velocityX": -2.360426305368932, - "velocityY": -1.0947451042534073, - "timestamp": 4.626596274863416 - }, - { - "x": 7.344387868807497, - "y": 2.0298516556077795, - "heading": -0.16871929267657423, - "angularVelocity": -1.155972034985807, - "velocityX": -2.624368248612144, - "velocityY": -1.2065120208372122, - "timestamp": 4.692299065194877 - }, - { - "x": 7.154363408340053, - "y": 1.943962982346676, - "heading": -0.2400485240661189, - "angularVelocity": -1.0856347352935647, - "velocityX": -2.892182501059652, - "velocityY": -1.3072302230667407, - "timestamp": 4.758001855526339 - }, - { - "x": 6.946444183851081, - "y": 1.8530162365161487, - "heading": -0.2992772419214472, - "angularVelocity": -0.9014642689682999, - "velocityX": -3.164541771210153, - "velocityY": -1.3842143594162997, - "timestamp": 4.8237046458578 - }, - { - "x": 6.72212326982476, - "y": 1.762675190878268, - "heading": -0.31855321839260486, - "angularVelocity": -0.29338139786625583, - "velocityX": -3.4141763674670904, - "velocityY": -1.3749955699312393, - "timestamp": 4.889407436189262 - }, - { - "x": 6.4857597450479005, - "y": 1.6860130581467423, - "heading": -0.3185532921212083, - "angularVelocity": -0.000001122153306519187, - "velocityX": -3.5974655503128186, - "velocityY": -1.1668017803319486, - "timestamp": 4.955110226520723 - }, - { - "x": 6.24426876944993, - "y": 1.6274732096370486, - "heading": -0.3185533177664737, - "angularVelocity": -3.903223175651228e-7, - "velocityX": -3.6755056273817623, - "velocityY": -0.8909796405048931, - "timestamp": 5.020813016852185 - }, - { - "x": 5.999037658914315, - "y": 1.5873919634602989, - "heading": -0.3185533462426245, - "angularVelocity": -4.3340854500246066e-7, - "velocityX": -3.732430682143934, - "velocityY": -0.6100387209515066, - "timestamp": 5.0865158071836465 - }, - { - "x": 5.751475205523387, - "y": 1.5659995878977724, - "heading": -0.3185533789943253, - "angularVelocity": -4.984826460154038e-7, - "velocityX": -3.7679138456983283, - "velocityY": -0.32559310578142847, - "timestamp": 5.152218597515108 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.3185534180019773, - "angularVelocity": -5.936985598886166e-7, - "velocityX": -3.781751231726404, - "velocityY": -0.03927692372891183, - "timestamp": 5.21792138784657 - }, - { - "x": 5.26265149067387, - "y": 1.5786031053003824, - "heading": -0.31855345112044986, - "angularVelocity": -5.200844119271939e-7, - "velocityX": -3.774430827391544, - "velocityY": 0.23844772894968422, - "timestamp": 5.28160042121006 - }, - { - "x": 5.0240625547753925, - "y": 1.61139052009726, - "heading": -0.3185534786660252, - "angularVelocity": -4.325689925822067e-7, - "velocityX": -3.746742425196293, - "velocityY": 0.5148855606793217, - "timestamp": 5.34527945457355 - }, - { - "x": 4.788524290519247, - "y": 1.6616042890226763, - "heading": -0.31855350247111613, - "angularVelocity": -3.7382933976434193e-7, - "velocityX": -3.698835422197071, - "velocityY": 0.7885447732660757, - "timestamp": 5.40895848793704 - }, - { - "x": 4.55730773239592, - "y": 1.7289734284079032, - "heading": -0.3185535237219762, - "angularVelocity": -3.3371831981217076e-7, - "velocityX": -3.6309684037367047, - "velocityY": 1.0579485244487483, - "timestamp": 5.4726375213005305 - }, - { - "x": 4.33166058516164, - "y": 1.8131343663413289, - "heading": -0.31855354324045854, - "angularVelocity": -3.06513484100539e-7, - "velocityX": -3.5435077342686676, - "velocityY": 1.3216428310558945, - "timestamp": 5.536316554664021 - }, - { - "x": 4.112800476187319, - "y": 1.9136328827222213, - "heading": -0.31855356163583604, - "angularVelocity": -2.8887651909639526e-7, - "velocityX": -3.4369257417120753, - "velocityY": 1.5782041760469425, - "timestamp": 5.599995588027511 - }, - { - "x": 3.9019083093617053, - "y": 2.029926448332014, - "heading": -0.3185535793939757, - "angularVelocity": -2.788694925452677e-7, - "velocityX": -3.3117991226689654, - "velocityY": 1.8262457745859704, - "timestamp": 5.663674621391001 - }, - { - "x": 3.700120307432907, - "y": 2.161384813738153, - "heading": -0.3185535969362317, - "angularVelocity": -2.7547930719470604e-7, - "velocityX": -3.1688295388681516, - "velocityY": 2.0643900898393666, - "timestamp": 5.727353654754491 - }, - { - "x": 3.5092854049648876, - "y": 2.2879634464070207, - "heading": -0.3579155843505842, - "angularVelocity": -0.618131044635493, - "velocityX": -2.996824737880426, - "velocityY": 1.9877599577609333, - "timestamp": 5.791032688117982 - }, - { - "x": 3.3329097555386666, - "y": 2.4050971261724468, - "heading": -0.41294492327597715, - "angularVelocity": -0.8641673093760206, - "velocityX": -2.7697601566820413, - "velocityY": 1.8394387222684179, - "timestamp": 5.854711721481472 - }, - { - "x": 3.171287726021972, - "y": 2.512479984884094, - "heading": -0.46992046584984914, - "angularVelocity": -0.8947300165290927, - "velocityX": -2.538072910028812, - "velocityY": 1.686314207985683, - "timestamp": 5.918390754844962 - }, - { - "x": 3.024408806786741, - "y": 2.6100923699912792, - "heading": -0.5252799185719805, - "angularVelocity": -0.8693513358805383, - "velocityX": -2.306550704009953, - "velocityY": 1.532881074842923, - "timestamp": 5.982069788208452 - }, - { - "x": 2.892255079193046, - "y": 2.6979336450739244, - "heading": -0.5773440254447586, - "angularVelocity": -0.8176020288434342, - "velocityX": -2.0753098879397363, - "velocityY": 1.3794379475145806, - "timestamp": 6.045748821571943 - }, - { - "x": 2.7748123815012584, - "y": 2.776006230905406, - "heading": -0.625128837055519, - "angularVelocity": -0.7504010203483642, - "velocityX": -1.8442914643726742, - "velocityY": 1.226032835420591, - "timestamp": 6.109427854935433 - }, - { - "x": 2.6720699818921556, - "y": 2.8443129166968544, - "heading": -0.6679860381773632, - "angularVelocity": -0.6730190277419648, - "velocityX": -1.6134415706757543, - "velocityY": 1.0726715244175118, - "timestamp": 6.173106888298923 - }, - { - "x": 2.5840196243973645, - "y": 2.9028562717726505, - "heading": -0.7054558016839699, - "angularVelocity": -0.5884160221579269, - "velocityX": -1.382721326691407, - "velocityY": 0.9193505614575074, - "timestamp": 6.236785921662413 - }, - { - "x": 2.510654811151598, - "y": 2.95163853061359, - "heading": -0.7371951288142529, - "angularVelocity": -0.4984266477336411, - "velocityX": -1.1521031235978156, - "velocityY": 0.7660646882386302, - "timestamp": 6.3004649550259035 - }, - { - "x": 2.4519703126340358, - "y": 2.9906615990271557, - "heading": -0.7629386573341135, - "angularVelocity": -0.4042700895428554, - "velocityX": -0.9215670436230067, - "velocityY": 0.6128087433553877, - "timestamp": 6.364143988389394 - }, - { - "x": 2.4079618327799786, - "y": 3.0199270886206313, - "heading": -0.7824753669479414, - "angularVelocity": -0.30679972012623624, - "velocityX": -0.6910984279998401, - "velocityY": 0.45957810675962124, - "timestamp": 6.427823021752884 - }, - { - "x": 2.378625774904473, - "y": 3.039436353320747, - "heading": -0.795633833561549, - "angularVelocity": -0.2066373485680448, - "velocityX": -0.4606862938394533, - "velocityY": 0.3063687318988397, - "timestamp": 6.491502055116374 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.10425061542851646, - "velocityX": -0.2303222914890624, - "velocityY": 0.15317707264507013, - "timestamp": 6.5551810884798645 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -7.869331036874185e-31, - "timestamp": 6.618860121843355 - }, - { - "x": 2.3794831364602356, - "y": 3.04172499089676, - "heading": -0.7962438371233982, - "angularVelocity": 0.09576162685715454, - "velocityX": 0.24659384848580665, - "velocityY": -0.11858712018957836, - "timestamp": 6.6818140946823945 - }, - { - "x": 2.4105331611816494, - "y": 3.0267927192788293, - "heading": -0.7842933789401895, - "angularVelocity": 0.1898284992078828, - "velocityX": 0.49321787523723476, - "velocityY": -0.23719347555886855, - "timestamp": 6.744768067521434 - }, - { - "x": 2.457111347137164, - "y": 3.0043922693157348, - "heading": -0.7665480516906409, - "angularVelocity": 0.2818777981640571, - "velocityX": 0.7398768315163422, - "velocityY": -0.3558226582517257, - "timestamp": 6.807722040360474 - }, - { - "x": 2.5192202688476826, - "y": 2.9745219425624034, - "heading": -0.7431604801963628, - "angularVelocity": 0.37150270967132, - "velocityX": 0.9865766830842898, - "velocityY": -0.4744788201008906, - "timestamp": 6.870676013199514 - }, - { - "x": 2.5968629770363694, - "y": 2.9371797310172574, - "heading": -0.7143164372650311, - "angularVelocity": 0.45817669053357035, - "velocityX": 1.2333249942335256, - "velocityY": -0.5931668782941151, - "timestamp": 6.933629986038554 - }, - { - "x": 2.690043132952377, - "y": 2.892363250439385, - "heading": -0.6802459845187668, - "angularVelocity": 0.5411962297181008, - "velocityX": 1.4801314629379994, - "velocityY": -0.711892809250637, - "timestamp": 6.9965839588775935 - }, - { - "x": 2.798765189175504, - "y": 2.840069647867451, - "heading": -0.6412406321012497, - "angularVelocity": 0.6195852407479587, - "velocityX": 1.7270086591851181, - "velocityY": -0.8306640584167341, - "timestamp": 7.059537931716633 - }, - { - "x": 2.9230346316689584, - "y": 2.7802954749466373, - "heading": -0.5976811625941267, - "angularVelocity": 0.6919256647788576, - "velocityX": 1.9739729978787044, - "velocityY": -0.9494900833922507, - "timestamp": 7.122491904555673 - }, - { - "x": 3.0628582875020656, - "y": 2.7130365228135442, - "heading": -0.550085909804202, - "angularVelocity": 0.7560325527288918, - "velocityX": 2.2210457819811675, - "velocityY": -1.0683829645677803, - "timestamp": 7.185445877394713 - }, - { - "x": 3.2182446226798445, - "y": 2.6382876468732976, - "heading": -0.4992026727158649, - "angularVelocity": 0.8082609372157451, - "velocityX": 2.4682530453648863, - "velocityY": -1.1873575656831603, - "timestamp": 7.248399850233753 - }, - { - "x": 3.3892034750687743, - "y": 2.5560428034877645, - "heading": -0.4462086118156311, - "angularVelocity": 0.8417905734992818, - "velocityX": 2.7156165795292284, - "velocityY": -1.306428167699852, - "timestamp": 7.311353823072793 - }, - { - "x": 3.57574133188106, - "y": 2.4662968714042055, - "heading": -0.3932458693377865, - "angularVelocity": 0.8412930922288103, - "velocityX": 2.963083160600891, - "velocityY": -1.4255801188754254, - "timestamp": 7.374307795911832 - }, - { - "x": 3.777810779911522, - "y": 2.369066952878771, - "heading": -0.3455559373144148, - "angularVelocity": 0.7575364964702819, - "velocityX": 3.209796600877132, - "velocityY": -1.5444604071935277, - "timestamp": 7.437261768750872 - }, - { - "x": 3.9925130304181535, - "y": 2.2661623966169606, - "heading": -0.3455557491568548, - "angularVelocity": 0.0000029888115329808544, - "velocityX": 3.4104638805811245, - "velocityY": -1.63459987703898, - "timestamp": 7.500215741589912 - }, - { - "x": 4.207067940824861, - "y": 2.162950872410893, - "heading": -0.34555573251479127, - "angularVelocity": 2.643528717349897e-7, - "velocityX": 3.408123438933375, - "velocityY": -1.6394759464975768, - "timestamp": 7.563169714428952 - }, - { - "x": 4.421622842445454, - "y": 2.0597393299403546, - "heading": -0.34555571587272765, - "angularVelocity": 2.6435287281597733e-7, - "velocityX": 3.408123299369276, - "velocityY": -1.6394762366217663, - "timestamp": 7.626123687267992 - }, - { - "x": 4.636177744065525, - "y": 1.9565277874687297, - "heading": -0.3455556992306641, - "angularVelocity": 2.6435287314542613e-7, - "velocityX": 3.4081232993609745, - "velocityY": -1.6394762366390223, - "timestamp": 7.689077660107031 - }, - { - "x": 4.850732645688907, - "y": 1.8533162450039902, - "heading": -0.34555568258860053, - "angularVelocity": 2.643528725209065e-7, - "velocityX": 3.408123299413585, - "velocityY": -1.6394762365296556, - "timestamp": 7.752031632946071 - }, - { - "x": 5.065287602977393, - "y": 1.7501048182552732, - "heading": -0.345555665946537, - "angularVelocity": 2.6435287138683083e-7, - "velocityX": 3.4081241836326663, - "velocityY": -1.63947439842449, - "timestamp": 7.814985605785111 - }, - { - "x": 5.280754789180012, - "y": 1.6488115807144432, - "heading": -0.34555564929395255, - "angularVelocity": 2.6451999265849417e-7, - "velocityX": 3.4226145942134942, - "velocityY": -1.6090046898202166, - "timestamp": 7.877939578624151 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, - "angularVelocity": 2.673332686258823e-7, - "velocityX": 3.530338087601756, - "velocityY": -1.3564290298187671, - "timestamp": 7.940893551463191 - }, - { - "x": 5.652794664229654, - "y": 1.5139935049725783, - "heading": -0.34555563561361163, - "angularVelocity": -7.551097078688917e-8, - "velocityX": 3.591492706822241, - "velocityY": -1.185058979367509, - "timestamp": 7.982600740979571 - }, - { - "x": 5.804789442560329, - "y": 1.4718291930788845, - "heading": -0.345555638724863, - "angularVelocity": -7.459748339352162e-8, - "velocityX": 3.644330392269195, - "velocityY": -1.010960277655107, - "timestamp": 8.02430793049595 - }, - { - "x": 5.958396699024851, - "y": 1.435981562464388, - "heading": -0.3455556418269712, - "angularVelocity": -7.437825995471752e-8, - "velocityX": 3.6829922669374113, - "velocityY": -0.8595072223799174, - "timestamp": 8.066015120012331 - }, - { - "x": 6.112005102436535, - "y": 1.4001388468787674, - "heading": -0.3455556449290776, - "angularVelocity": -7.437821592181375e-8, - "velocityX": 3.6830197669242586, - "velocityY": -0.8593893762979032, - "timestamp": 8.107722309528711 - }, - { - "x": 6.2656135060730795, - "y": 1.3642961322568121, - "heading": -0.3455556480311839, - "angularVelocity": -7.437821455888137e-8, - "velocityX": 3.683019772315654, - "velocityY": -0.8593893531924107, - "timestamp": 8.149429499045091 - }, - { - "x": 6.419221909709655, - "y": 1.3284534176349936, - "heading": -0.3455556511332902, - "angularVelocity": -7.437821520024811e-8, - "velocityX": 3.6830197723164173, - "velocityY": -0.8593893531891351, - "timestamp": 8.191136688561471 - }, - { - "x": 6.572830313283965, - "y": 1.292610702746327, - "heading": -0.3455556542353965, - "angularVelocity": -7.437821564310945e-8, - "velocityX": 3.6830197708234897, - "velocityY": -0.859389359587264, - "timestamp": 8.232843878077851 - }, - { - "x": 6.72643839927336, - "y": 1.2567666268353574, - "heading": -0.34555565733750304, - "angularVelocity": -7.43782201062097e-8, - "velocityX": 3.6830121561911366, - "velocityY": -0.8594219923855618, - "timestamp": 8.274551067594231 - }, - { - "x": 6.87895592150965, - "y": 1.216534214678565, - "heading": -0.3455556607967647, - "angularVelocity": -8.294161584911855e-8, - "velocityX": 3.656864056409029, - "velocityY": -0.9646397329408027, - "timestamp": 8.316258257110611 - }, - { - "x": 7.025398163354816, - "y": 1.1717644512232448, - "heading": -0.34674583940477344, - "angularVelocity": -0.02853653343247435, - "velocityX": 3.5111989933450576, - "velocityY": -1.073430359955979, - "timestamp": 8.357965446626991 - }, - { - "x": 7.164406770259058, - "y": 1.1285160108597303, - "heading": -0.3473601506358553, - "angularVelocity": -0.014729144739916763, - "velocityX": 3.3329650958535093, - "velocityY": -1.0369540806994204, - "timestamp": 8.399672636143372 - }, - { - "x": 7.296003099222487, - "y": 1.086889230520204, - "heading": -0.3473718226301098, - "angularVelocity": -0.0002798556889061875, - "velocityX": 3.155243268351752, - "velocityY": -0.9980720547755352, - "timestamp": 8.441379825659752 - }, - { - "x": 7.420194691021883, - "y": 1.0469180216341771, - "heading": -0.34677182369168125, - "angularVelocity": 0.014385983457189529, - "velocityX": 2.977702243653219, - "velocityY": -0.9583769453064839, - "timestamp": 8.483087015176132 - }, - { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, - "angularVelocity": 0.02916022972351966, - "velocityX": 2.8002535694982806, - "velocityY": -0.9182731897618387, - "timestamp": 8.524794204692512 - }, - { - "x": 7.694109552956691, - "y": 0.9554552181085981, - "heading": -0.3423846619503678, - "angularVelocity": 0.05119624293404516, - "velocityX": 2.536815276765165, - "velocityY": -0.8583516550337287, - "timestamp": 8.586731767532397 - }, - { - "x": 7.835030504559327, - "y": 0.9064832871477253, - "heading": -0.33886448301682054, - "angularVelocity": 0.05683431462499228, - "velocityX": 2.2752098264978664, - "velocityY": -0.7906660952654867, - "timestamp": 8.648669330372282 - }, - { - "x": 7.959827163561028, - "y": 0.861997905644216, - "heading": -0.3356203872211221, - "angularVelocity": 0.05237687191671972, - "velocityX": 2.014878424007598, - "velocityY": -0.7182294469433418, - "timestamp": 8.710606893212168 - }, - { - "x": 8.068556860689503, - "y": 0.8221974775860657, - "heading": -0.3330761096963967, - "angularVelocity": 0.04107810201222393, - "velocityX": 1.7554726428218221, - "velocityY": -0.6425895084221868, - "timestamp": 8.772544456052053 - }, - { - "x": 8.161262929678218, - "y": 0.7872247496182815, - "heading": -0.33153778712366017, - "angularVelocity": 0.02483666618774324, - "velocityX": 1.4967664973897774, - "velocityY": -0.5646448837225355, - "timestamp": 8.834482018891938 - }, - { - "x": 8.237979200485006, - "y": 0.7571873196552872, - "heading": -0.33123696700010097, - "angularVelocity": 0.0048568285506627, - "velocityX": 1.2386065464846836, - "velocityY": -0.48496305934161354, - "timestamp": 8.896419581731823 - }, - { - "x": 8.298732784455282, - "y": 0.7321691813322279, - "heading": -0.3323549200927614, - "angularVelocity": -0.018049678440697953, - "velocityX": 0.9808843161512628, - "velocityY": -0.4039251332464228, - "timestamp": 8.958357144571709 - }, - { - "x": 8.34354587924, - "y": 0.712237715373984, - "heading": -0.3350374219820423, - "angularVelocity": -0.04330977465508917, - "velocityX": 0.7235204733606144, - "velocityY": -0.3217993257140046, - "timestamp": 9.020294707411594 - }, - { - "x": 8.372436985155346, - "y": 0.6974481694465224, - "heading": -0.33940426042561933, - "angularVelocity": -0.07050387912204041, - "velocityX": 0.46645532356563796, - "velocityY": -0.23878152851596665, - "timestamp": 9.08223227025148 - }, - { - "x": 8.385421752929688, - "y": 0.6878466606140137, - "heading": -0.34555563246426124, - "angularVelocity": -0.09931569400855825, - "velocityX": 0.20964285934061647, - "velocityY": -0.15501915787887396, - "timestamp": 9.144169833091365 - }, - { - "x": 8.381221613866826, - "y": 0.6835499753387894, - "heading": -0.35422874908994917, - "angularVelocity": -0.13146724375807764, - "velocityX": -0.06366577665514053, - "velocityY": -0.06512922572222897, - "timestamp": 9.210141525946263 - }, - { - "x": 8.358990442151228, - "y": 0.6851839721097847, - "heading": -0.36494544812778174, - "angularVelocity": -0.16244389940702994, - "velocityX": -0.33698046470466403, - "velocityY": 0.024768149797052455, - "timestamp": 9.27611321880116 - }, - { - "x": 8.318727796191062, - "y": 0.692749219284977, - "heading": -0.37761522689458643, - "angularVelocity": -0.1920487139032685, - "velocityX": -0.6103018464104298, - "velocityY": 0.1146741404958606, - "timestamp": 9.342084911656059 - }, - { - "x": 8.260433187957593, - "y": 0.7062463828943455, - "heading": -0.3921309909220783, - "angularVelocity": -0.22003018869651583, - "velocityX": -0.8836306256637128, - "velocityY": 0.20459022688799033, - "timestamp": 9.408056604510957 - }, - { - "x": 8.184106080259626, - "y": 0.7256762539187386, - "heading": -0.40836397752669645, - "angularVelocity": -0.24605987662499226, - "velocityX": -1.1569675476699668, - "velocityY": 0.2945183029807682, - "timestamp": 9.474028297365855 - }, - { - "x": 8.089745888152331, - "y": 0.7510397855554056, - "heading": -0.4261563635075344, - "angularVelocity": -0.26969727789116266, - "velocityX": -1.4303133362794993, - "velocityY": 0.3844608276530502, - "timestamp": 9.539999990220753 - }, - { - "x": 7.977351991056692, - "y": 0.7823381451863918, - "heading": -0.445310079113529, - "angularVelocity": -0.2903323346290704, - "velocityX": -1.7036685316358853, - "velocityY": 0.47442104752147907, - "timestamp": 9.605971683075651 - }, - { - "x": 7.846923771917719, - "y": 0.8195727882757919, - "heading": -0.4655690572275494, - "angularVelocity": -0.30708592181466465, - "velocityX": -1.9770330803219518, - "velocityY": 0.5644033293384785, - "timestamp": 9.67194337593055 - }, - { - "x": 7.6984607267480145, - "y": 0.8627455646922209, - "heading": -0.48658930450770715, - "angularVelocity": -0.31862525229405353, - "velocityX": -2.2504052684572193, - "velocityY": 0.6544136514942158, - "timestamp": 9.737915068785448 - }, - { - "x": 7.531962771855591, - "y": 0.9118588688249081, - "heading": -0.5078842268520795, - "angularVelocity": -0.32278878141280287, - "velocityX": -2.5237787251969923, - "velocityY": 0.7444602678410827, - "timestamp": 9.803886761640346 - }, - { - "x": 7.347431182646945, - "y": 0.9669158200795512, - "heading": -0.5287129812318903, - "angularVelocity": -0.31572259977658373, - "velocityX": -2.7971328493042473, - "velocityY": 0.8345541681905657, - "timestamp": 9.869858454495244 - }, - { - "x": 7.144872022891467, - "y": 1.0279202386913098, - "heading": -0.5478111966657127, - "angularVelocity": -0.28949106211095804, - "velocityX": -3.0703950586958326, - "velocityY": 0.9247059757271185, - "timestamp": 9.935830147350142 - }, - { - "x": 6.92431385893063, - "y": 1.0948740954299652, - "heading": -0.56252983495188, - "angularVelocity": -0.2231053600297971, - "velocityX": -3.343224259015806, - "velocityY": 1.0148876562242728, - "timestamp": 10.00180184020504 - }, - { - "x": 6.686027050772059, - "y": 1.1677363127465568, - "heading": -0.5635938882487441, - "angularVelocity": -0.016128937288368943, - "velocityX": -3.611955337915506, - "velocityY": 1.1044466825619976, - "timestamp": 10.067773533059938 - }, - { - "x": 6.448996512762522, - "y": 1.2456322264917639, - "heading": -0.5635938947986299, - "angularVelocity": -9.928327608034937e-8, - "velocityX": -3.592912774436053, - "velocityY": 1.180747535409401, - "timestamp": 10.133745225914836 - }, - { - "x": 6.2119660667132655, - "y": 1.3235284200636117, - "heading": -0.5635939013485092, - "angularVelocity": -9.928317804375559e-8, - "velocityX": -3.5929113805006425, - "velocityY": 1.180751777026201, - "timestamp": 10.199716918769735 - }, - { - "x": 5.97493562067527, - "y": 1.4014246136697293, - "heading": -0.5635939078983885, - "angularVelocity": -9.928317794724392e-8, - "velocityX": -3.5929113803299297, - "velocityY": 1.180751777545662, - "timestamp": 10.265688611624633 - }, - { - "x": 5.737905297280568, - "y": 1.4793211804663164, - "heading": -0.5635939144482677, - "angularVelocity": -9.928317524609207e-8, - "velocityX": -3.5929095213008893, - "velocityY": 1.180757434372905, - "timestamp": 10.33166030447953 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.5635939210079286, - "angularVelocity": -9.943144757674115e-8, - "velocityX": -3.5606438133655525, - "velocityY": 1.2747558885869388, - "timestamp": 10.397631997334429 - }, - { - "x": 5.2859625551559315, - "y": 1.6593525606269757, - "heading": -0.5635939265318385, - "angularVelocity": -8.803796196510374e-8, - "velocityX": -3.459117077662582, - "velocityY": 1.5289526284347386, - "timestamp": 10.4603766333877 - }, - { - "x": 5.071426966449666, - "y": 1.7607653123039275, - "heading": -0.5635939320385204, - "angularVelocity": -8.776338927441567e-8, - "velocityX": -3.4191861201350386, - "velocityY": 1.6162776303436093, - "timestamp": 10.52312126944097 - }, - { - "x": 4.856891543272159, - "y": 1.8621784141511948, - "heading": -0.5635939375452019, - "angularVelocity": -8.776338338833754e-8, - "velocityX": -3.419183482000995, - "velocityY": 1.6162832112241017, - "timestamp": 10.585865905494241 - }, - { - "x": 4.642356120103932, - "y": 1.9635915160180906, - "heading": -0.5635939430518834, - "angularVelocity": -8.776338232016452e-8, - "velocityX": -3.419183481853115, - "velocityY": 1.6162832115369357, - "timestamp": 10.648610541547512 - }, - { - "x": 4.427820696935705, - "y": 2.0650046178849877, - "heading": -0.5635939485585649, - "angularVelocity": -8.776338320403535e-8, - "velocityX": -3.419183481853107, - "velocityY": 1.616283211536953, - "timestamp": 10.711355177600783 - }, - { - "x": 4.213285273767479, - "y": 2.1664177197518852, - "heading": -0.5635939540652464, - "angularVelocity": -8.776338205784925e-8, - "velocityX": -3.419183481853105, - "velocityY": 1.616283211536959, - "timestamp": 10.774099813654054 - }, - { - "x": 3.99874985060226, - "y": 2.2678308216251453, - "heading": -0.5635939595719279, - "angularVelocity": -8.776338204240658e-8, - "velocityX": -3.4191834818051703, - "velocityY": 1.616283211638361, - "timestamp": 10.836844449707325 - }, - { - "x": 3.7842144810928517, - "y": 2.369244037002313, - "heading": -0.5635939650808715, - "angularVelocity": -8.779943360299573e-8, - "velocityX": -3.4191826266593917, - "velocityY": 1.6162850206202093, - "timestamp": 10.899589085760596 - }, - { - "x": 3.582486282432141, - "y": 2.464592725052892, - "heading": -0.5976475269249973, - "angularVelocity": -0.5427326379774409, - "velocityX": -3.21506683837393, - "velocityY": 1.5196309046980254, - "timestamp": 10.962333721813867 - }, - { - "x": 3.3962755581787456, - "y": 2.5526066599990593, - "heading": -0.6290952664889575, - "angularVelocity": -0.5012020396016188, - "velocityX": -2.9677552690767404, - "velocityY": 1.4027324163844592, - "timestamp": 11.025078357867137 - }, - { - "x": 3.2255823358404876, - "y": 2.63328589857046, - "heading": -0.6579338206032609, - "angularVelocity": -0.4596178403173682, - "velocityX": -2.7204432613704155, - "velocityY": 1.2858348322062758, - "timestamp": 11.087822993920408 - }, - { - "x": 3.0704066332901094, - "y": 2.7066304971231805, - "heading": -0.684160167831089, - "angularVelocity": -0.4179854865292692, - "velocityX": -2.4731309688151453, - "velocityY": 1.168938146209836, - "timestamp": 11.15056762997368 - }, - { - "x": 2.930748464239733, - "y": 2.7726405069094633, - "heading": -0.7077714539525848, - "angularVelocity": -0.3763076432772636, - "velocityX": -2.22581845772133, - "velocityY": 1.05204227705202, - "timestamp": 11.21331226602695 - }, - { - "x": 2.8066078397359373, - "y": 2.83131597316776, - "heading": -0.7287650298436226, - "angularVelocity": -0.3345875792986393, - "velocityX": -1.978505770571369, - "velocityY": 0.9351471288873412, - "timestamp": 11.27605690208022 - }, - { - "x": 2.697984768963403, - "y": 2.882656934878273, - "heading": -0.7471385155745514, - "angularVelocity": -0.2928295849119221, - "velocityX": -1.7311929370394774, - "velocityY": 0.8182526019742015, - "timestamp": 11.338801538133492 - }, - { - "x": 2.604879259815539, - "y": 2.926663424723888, - "heading": -0.7628898637084475, - "angularVelocity": -0.25103895925897446, - "velocityX": -1.4838799777054614, - "velocityY": 0.7013585959483968, - "timestamp": 11.401546174186763 - }, - { - "x": 2.5272913193421016, - "y": 2.963335469132253, - "heading": -0.7760174152523261, - "angularVelocity": -0.20922189321065382, - "velocityX": -1.236566906015123, - "velocityY": 0.584465011116332, - "timestamp": 11.464290810240033 - }, - { - "x": 2.465220954107753, - "y": 2.9926730883504185, - "heading": -0.7865199463491176, - "angularVelocity": -0.1673853217966667, - "velocityX": -0.9892537296997038, - "velocityY": 0.46757174897401693, - "timestamp": 11.527035446293304 - }, - { - "x": 2.418668170473455, - "y": 3.014676296527397, - "heading": -0.7943967051912195, - "angularVelocity": -0.12553676836079783, - "velocityX": -0.7419404520057251, - "velocityY": 0.3506787123332304, - "timestamp": 11.589780082346575 - }, - { - "x": 2.38763297480514, - "y": 3.0293451017899056, - "heading": -0.7996474391139023, - "angularVelocity": -0.08368418805115034, - "velocityX": -0.49462707285394797, - "velocityY": 0.23378580521296682, - "timestamp": 11.652524718399846 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": -0.0418358130797821, - "velocityX": -0.24731358996990807, - "velocityY": 0.11689293258068652, - "timestamp": 11.715269354453117 - }, - { - "x": 2.37211537361145, - "y": 3.03667950630188, - "heading": -0.8022724119795859, - "angularVelocity": 1.2905382194087526e-36, - "velocityX": 1.2188676912146853e-38, - "velocityY": -1.9456328527917923e-39, - "timestamp": 11.778013990506388 - }, - { - "x": 2.3862784269967885, - "y": 3.0299921206795677, - "heading": -0.7979445022798209, - "angularVelocity": 0.07214760590413372, - "velocityX": 0.23610252175550234, - "velocityY": -0.11148080618080955, - "timestamp": 11.838000869426745 - }, - { - "x": 2.414605712310544, - "y": 3.016617241283919, - "heading": -0.7893555783917804, - "angularVelocity": 0.14318004274641105, - "velocityX": 0.4722246901920793, - "velocityY": -0.22296341527296173, - "timestamp": 11.897987748347102 - }, - { - "x": 2.457098595113841, - "y": 2.996554734272641, - "heading": -0.7765849369819305, - "angularVelocity": 0.21289057940162145, - "velocityX": 0.7083696229589316, - "velocityY": -0.3344482555579276, - "timestamp": 11.95797462726746 - }, - { - "x": 2.5137586742709717, - "y": 2.9698044458506936, - "heading": -0.7597273153290622, - "angularVelocity": 0.28102181604163, - "velocityX": 0.9445412092927212, - "velocityY": -0.44593565965421605, - "timestamp": 12.017961506187817 - }, - { - "x": 2.5845878442638655, - "y": 2.9363662114763, - "heading": -0.7388971783301495, - "angularVelocity": 0.34724488711219914, - "velocityX": 1.180744377231747, - "velocityY": -0.5574258067133014, - "timestamp": 12.077948385108174 - }, - { - "x": 2.6695883810159726, - "y": 2.8962398712988433, - "heading": -0.7142349782126007, - "angularVelocity": 0.4111265757015274, - "velocityX": 1.4169854855252446, - "velocityY": -0.6689186185320777, - "timestamp": 12.137935264028531 - }, - { - "x": 2.7687630628600255, - "y": 2.849425297465716, - "heading": -0.6859166617272401, - "angularVelocity": 0.4720751770225938, - "velocityX": 1.653272909492815, - "velocityY": -0.7804135616937368, - "timestamp": 12.197922142948888 - }, - { - "x": 2.882115344958, - "y": 2.795922445610523, - "heading": -0.6541688123580988, - "angularVelocity": 0.529246560923628, - "velocityX": 1.8896179320892517, - "velocityY": -0.8919092444570627, - "timestamp": 12.257909021869246 - }, - { - "x": 3.009649615335316, - "y": 2.73573145985777, - "heading": -0.6192942663212875, - "angularVelocity": 0.5813695705541366, - "velocityX": 2.1260361044394105, - "velocityY": -1.0034025246198652, - "timestamp": 12.317895900789603 - }, - { - "x": 3.1513715692279023, - "y": 2.6688529097388005, - "heading": -0.5817190571900686, - "angularVelocity": 0.626390467507171, - "velocityX": 2.3625492181506127, - "velocityY": -1.114886310517361, - "timestamp": 12.37788277970996 - }, - { - "x": 3.3072887013147403, - "y": 2.5952884046594, - "heading": -0.5420886244414693, - "angularVelocity": 0.6606516868666416, - "velocityX": 2.5991872705003334, - "velocityY": -1.2263432671179573, - "timestamp": 12.437869658630317 - }, - { - "x": 3.4774104648990076, - "y": 2.5150425607670805, - "heading": -0.50150111026719, - "angularVelocity": 0.6766065330414328, - "velocityX": 2.8359829123654157, - "velocityY": -1.33772327109832, - "timestamp": 12.497856537550675 - }, - { - "x": 3.6617427045443476, - "y": 2.428132084038069, - "heading": -0.4622625664613271, - "angularVelocity": 0.6541187758402723, - "velocityX": 3.0728759849311564, - "velocityY": -1.4488247812392312, - "timestamp": 12.557843416471032 - }, - { - "x": 3.8601464039752953, - "y": 2.3346891355318427, - "heading": -0.4327535611710116, - "angularVelocity": 0.49192433114404066, - "velocityX": 3.3074516127828706, - "velocityY": -1.5577231252568986, - "timestamp": 12.61783029539139 - }, - { - "x": 4.065242173401204, - "y": 2.237711412138144, - "heading": -0.4327535477418676, - "angularVelocity": 2.2386802351026997e-7, - "velocityX": 3.4190105089182294, - "velocityY": -1.6166489262168848, - "timestamp": 12.677817174311746 - }, - { - "x": 4.2703379249938225, - "y": 2.140733651028875, - "heading": -0.43275353431315516, - "angularVelocity": 2.2386082807599726e-7, - "velocityX": 3.419010211631728, - "velocityY": -1.6166495549472233, - "timestamp": 12.737804053232104 - }, - { - "x": 4.475433676585232, - "y": 2.0437558899170503, - "heading": -0.43275352088444274, - "angularVelocity": 2.2386082918741598e-7, - "velocityX": 3.4190102116115857, - "velocityY": -1.616649554989821, - "timestamp": 12.797790932152461 - }, - { - "x": 4.6805294281766425, - "y": 1.9467781288052255, - "heading": -0.4327535074557304, - "angularVelocity": 2.2386082819075723e-7, - "velocityX": 3.4190102116115852, - "velocityY": -1.616649554989824, - "timestamp": 12.857777811072818 - }, - { - "x": 4.885625179768054, - "y": 1.8498003676934025, - "heading": -0.43275349402701796, - "angularVelocity": 2.2386082903608912e-7, - "velocityX": 3.4190102116115972, - "velocityY": -1.6166495549897977, - "timestamp": 12.917764689993176 - }, - { - "x": 5.090720931370459, - "y": 1.7528226066048298, - "heading": -0.43275348059830554, - "angularVelocity": 2.2386082830289002e-7, - "velocityX": 3.4190102117948658, - "velocityY": -1.6166495546022077, - "timestamp": 12.977751568913533 - }, - { - "x": 5.29581684523517, - "y": 1.6558451886813434, - "heading": -0.43275346716959256, - "angularVelocity": 2.2386083719980442e-7, - "velocityX": 3.4190129167581613, - "velocityY": -1.6166438339330798, - "timestamp": 13.03773844783389 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.43275345370859003, - "angularVelocity": 2.243991161112281e-7, - "velocityX": 3.453867841656271, - "velocityY": -1.5407736813730113, - "timestamp": 13.097725326754247 - }, - { - "x": 5.767637732978587, - "y": 1.4715486447521433, - "heading": -0.4327534418952186, - "angularVelocity": 1.5949042666645356e-7, - "velocityX": 3.572782883962456, - "velocityY": -1.2403266728714897, - "timestamp": 13.171794797056224 - }, - { - "x": 6.033774192023134, - "y": 1.384125705645109, - "heading": -0.432753430099036, - "angularVelocity": 1.5925836332865082e-7, - "velocityX": 3.593065509440347, - "velocityY": -1.1802830336252828, - "timestamp": 13.2458642673582 - }, - { - "x": 6.2999107025526015, - "y": 1.2967029232707166, - "heading": -0.43275341830285335, - "angularVelocity": 1.5925836332521055e-7, - "velocityX": 3.5930662045299844, - "velocityY": -1.1802809176031086, - "timestamp": 13.319933737660177 - }, - { - "x": 6.566047160708243, - "y": 1.2092799814598572, - "heading": -0.43275340650489624, - "angularVelocity": 1.5928232033441407e-7, - "velocityX": 3.593065497439389, - "velocityY": -1.1802830701291938, - "timestamp": 13.394003207962154 - }, - { - "x": 6.815604168443372, - "y": 1.1270295237235333, - "heading": -0.36800659105834704, - "angularVelocity": 0.8741363369088632, - "velocityX": 3.3692290051177802, - "velocityY": -1.110450194945293, - "timestamp": 13.46807267826413 - }, - { - "x": 7.0424486777806585, - "y": 1.0523256982782683, - "heading": -0.304588986464792, - "angularVelocity": 0.8561908750664192, - "velocityX": 3.0625912189253843, - "velocityY": -1.0085643267152118, - "timestamp": 13.542142148566107 - }, - { - "x": 7.246590574581559, - "y": 0.9851213052520007, - "heading": -0.24567984244132185, - "angularVelocity": 0.7953228743678282, - "velocityX": 2.7560869001577597, - "velocityY": -0.9073156963628868, - "timestamp": 13.616211618868084 - }, - { - "x": 7.428038548794457, - "y": 0.9253999088853845, - "heading": -0.19230647763371986, - "angularVelocity": 0.7205852099387576, - "velocityX": 2.4496999029849618, - "velocityY": -0.8062889625528061, - "timestamp": 13.69028108917006 - }, - { - "x": 7.586797949690041, - "y": 0.8731533407207944, - "heading": -0.1449799929692073, - "angularVelocity": 0.6389472541327181, - "velocityX": 2.1433851254549694, - "velocityY": -0.705372509774734, - "timestamp": 13.764350559472037 - }, - { - "x": 7.722872327084393, - "y": 0.8283767866857153, - "heading": -0.10400779573923778, - "angularVelocity": 0.5531590419497899, - "velocityX": 1.8371182734206921, - "velocityY": -0.6045210510150558, - "timestamp": 13.838420029774014 - }, - { - "x": 7.836264194581954, - "y": 0.7910670998170554, - "heading": -0.06959570799723012, - "angularVelocity": 0.46459205934255987, - "velocityX": 1.530885357155523, - "velocityY": -0.5037120788977025, - "timestamp": 13.91248950007599 - }, - { - "x": 7.926975414267802, - "y": 0.7612220655404804, - "heading": -0.04189104317473826, - "angularVelocity": 0.3740362218001811, - "velocityX": 1.224677580601353, - "velocityY": -0.4029330053920843, - "timestamp": 13.986558970377967 - }, - { - "x": 7.995007406512269, - "y": 0.7388400306235591, - "heading": -0.021003806792755122, - "angularVelocity": 0.28199521741990763, - "velocityX": 0.9184889802384877, - "velocityY": -0.3021762519115013, - "timestamp": 14.060628440679944 - }, - { - "x": 8.040361272813582, - "y": 0.7239196956147146, - "heading": -0.0070182972067734635, - "angularVelocity": 0.18881611450660718, - "velocityX": 0.612315251025934, - "velocityY": -0.20143704211755914, - "timestamp": 14.13469791098192 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": -5.172058523180806e-42, - "angularVelocity": 0.09475290127174331, - "velocityX": 0.30615312096089364, - "velocityY": -0.10071229126619016, - "timestamp": 14.208767381283897 - }, - { - "x": 8.063037872314453, - "y": 0.7164599895477295, - "heading": -2.2197990868657932e-42, - "angularVelocity": 3.9858008032330975e-41, - "velocityX": -4.443522904196121e-42, - "velocityY": 7.940802598890531e-42, - "timestamp": 14.282836851585873 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint.1.traj b/src/main/deploy/choreo/SourceLanePHGSprint.1.traj deleted file mode 100644 index 87faabce..00000000 --- a/src/main/deploy/choreo/SourceLanePHGSprint.1.traj +++ /dev/null @@ -1,184 +0,0 @@ -{ - "samples": [ - { - "x": 0.433241993188858, - "y": 4.121134281158447, - "heading": 9.759657159056985e-29, - "angularVelocity": 1.5485227916478622e-29, - "velocityX": -1.3617494152189479e-27, - "velocityY": -5.3557690609839536e-27, - "timestamp": 0 - }, - { - "x": 0.4546918803213756, - "y": 4.109218526997661, - "heading": -0.008937300145995024, - "angularVelocity": -0.11893237262991248, - "velocityX": 0.28544257523423766, - "velocityY": -0.15856789979825608, - "timestamp": 0.07514606787342623 - }, - { - "x": 0.4975916635116252, - "y": 4.0853873108981436, - "heading": -0.02681241329663613, - "angularVelocity": -0.23787157008333967, - "velocityX": 0.5708852692400183, - "velocityY": -0.3171319108760033, - "timestamp": 0.15029213574685246 - }, - { - "x": 0.5619414537480726, - "y": 4.049641027140498, - "heading": -0.05362171709802378, - "angularVelocity": -0.3567625633658495, - "velocityX": 0.8563294402155045, - "velocityY": -0.4756906750976654, - "timestamp": 0.2254382036202787 - }, - { - "x": 0.6477414523773108, - "y": 4.001980177860296, - "heading": -0.08935853104911338, - "angularVelocity": -0.4755646564406219, - "velocityX": 1.1417762905939028, - "velocityY": -0.6342427571923064, - "timestamp": 0.3005842714937049 - }, - { - "x": 0.7549919358777417, - "y": 3.9424053758532884, - "heading": -0.13401442200652888, - "angularVelocity": -0.5942545261667251, - "velocityX": 1.4272268201854599, - "velocityY": -0.7927866845588466, - "timestamp": 0.3757303393671312 - }, - { - "x": 0.8836932393269187, - "y": 3.8709173416225426, - "heading": -0.18758057158075359, - "angularVelocity": -0.7128270459134325, - "velocityX": 1.7126818087934759, - "velocityY": -0.9513210238912064, - "timestamp": 0.45087640724055744 - }, - { - "x": 1.0338457421402332, - "y": 3.7875168932975942, - "heading": -0.2500489995023012, - "angularVelocity": -0.8312933688928013, - "velocityX": 1.9981418464400145, - "velocityY": -1.1098444760333412, - "timestamp": 0.5260224751139837 - }, - { - "x": 1.2054498603190875, - "y": 3.692204931169302, - "heading": -0.3214134496146474, - "angularVelocity": -0.9496764385935714, - "velocityX": 2.283607419990346, - "velocityY": -1.268355947630322, - "timestamp": 0.60116854298741 - }, - { - "x": 1.3985060453231266, - "y": 3.584982424979261, - "heading": -0.40166985798592575, - "angularVelocity": -1.0680054278616389, - "velocityX": 2.569079001302066, - "velocityY": -1.4268545144723166, - "timestamp": 0.6763146108608362 - }, - { - "x": 1.5915937569821017, - "y": 3.4778198154599655, - "heading": -0.48176859267037747, - "angularVelocity": -1.0659071984893158, - "velocityX": 2.5694985396202785, - "velocityY": -1.4260574445464886, - "timestamp": 0.7514606787342625 - }, - { - "x": 1.7632284858900344, - "y": 3.382566036520045, - "heading": -0.552976547680448, - "angularVelocity": -0.9475938931363788, - "velocityX": 2.2840147697019764, - "velocityY": -1.2675816797275836, - "timestamp": 0.8266067466076887 - }, - { - "x": 1.9134098098562773, - "y": 3.299220400739088, - "heading": -0.6152918418017485, - "angularVelocity": -0.8292555536806328, - "velocityX": 1.9985253815170176, - "velocityY": -1.1091150626981823, - "timestamp": 0.901752814481115 - }, - { - "x": 2.04213735776467, - "y": 3.2277823059512354, - "heading": -0.6687115003742338, - "angularVelocity": -0.7108776291856513, - "velocityX": 1.7130310547348613, - "velocityY": -0.9506564589404838, - "timestamp": 0.9768988823545413 - }, - { - "x": 2.1494108084857926, - "y": 3.1682512394298143, - "heading": -0.7132320418113052, - "angularVelocity": -0.5924533737688112, - "velocityX": 1.4275324545498602, - "velocityY": -0.7922046782500088, - "timestamp": 1.0520449502279674 - }, - { - "x": 2.235229893671302, - "y": 3.120626784362015, - "heading": -0.7488501306819726, - "angularVelocity": -0.4739847323836216, - "velocityX": 1.142030283341778, - "velocityY": -0.6337584442610695, - "timestamp": 1.1271910181013935 - }, - { - "x": 2.2995943987852447, - "y": 3.0849086278583613, - "heading": -0.7755632624607874, - "angularVelocity": -0.35548276223593744, - "velocityX": 0.8565252572144713, - "velocityY": -0.4753163740226105, - "timestamp": 1.2023370859748197 - }, - { - "x": 2.342504162283695, - "y": 3.0610965683905045, - "heading": -0.7933703919080856, - "angularVelocity": -0.2369668826490278, - "velocityX": 0.5710180813549185, - "velocityY": -0.31687698560575434, - "timestamp": 1.2774831538482458 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.11846288599550668, - "velocityX": 0.28550943973314763, - "velocityY": -0.15843872456938438, - "timestamp": 1.352629221721672 - }, - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -1.820153695476826e-18, - "velocityX": -5.7963239407122954e-18, - "velocityY": -1.1530643297922074e-18, - "timestamp": 1.4277752895950981 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint.2.traj b/src/main/deploy/choreo/SourceLanePHGSprint.2.traj deleted file mode 100644 index 38e969e1..00000000 --- a/src/main/deploy/choreo/SourceLanePHGSprint.2.traj +++ /dev/null @@ -1,841 +0,0 @@ -{ - "samples": [ - { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -1.820153695476826e-18, - "velocityX": -5.7963239407122954e-18, - "velocityY": -1.1530643297922074e-18, - "timestamp": 0 - }, - { - "x": 2.37735740952237, - "y": 3.0408026970784268, - "heading": -0.7860869247798871, - "angularVelocity": 0.2648554274741457, - "velocityX": 0.21924714610297322, - "velocityY": -0.1372563412236913, - "timestamp": 0.061110649511906034 - }, - { - "x": 2.404179793787175, - "y": 3.024019444447085, - "heading": -0.7541504516362038, - "angularVelocity": 0.5226007806947081, - "velocityX": 0.4389150578342217, - "velocityY": -0.2746371175137346, - "timestamp": 0.12222129902381207 - }, - { - "x": 2.4444554974169383, - "y": 2.998832316814256, - "heading": -0.7069723408476191, - "angularVelocity": 0.7720112806098135, - "velocityX": 0.6590619466729163, - "velocityY": -0.41215611082519754, - "timestamp": 0.1833319485357181 - }, - { - "x": 2.4982188560395806, - "y": 2.96523040511413, - "heading": -0.645190074686331, - "angularVelocity": 1.0109901736398696, - "velocityX": 0.8797706954852063, - "velocityY": -0.5498536174710421, - "timestamp": 0.24444259804762414 - }, - { - "x": 2.5655102024042766, - "y": 2.923198127539445, - "heading": -0.5696285601591381, - "angularVelocity": 1.2364704864161304, - "velocityX": 1.101139439723772, - "velocityY": -0.6878061010707456, - "timestamp": 0.30555324755953017 - }, - { - "x": 2.646375841205288, - "y": 2.8727129664624207, - "heading": -0.48135684674506, - "angularVelocity": 1.4444571301255718, - "velocityX": 1.3232659028645477, - "velocityY": -0.8261270577264651, - "timestamp": 0.3666638970714362 - }, - { - "x": 2.740867207337065, - "y": 2.813744425609022, - "heading": -0.38173698900245784, - "angularVelocity": 1.6301554399809315, - "velocityX": 1.5462340342720005, - "velocityY": -0.9649470480903679, - "timestamp": 0.42777454658334224 - }, - { - "x": 2.849040211493812, - "y": 2.746255741762665, - "heading": -0.2724850390795677, - "angularVelocity": 1.7877726843928354, - "velocityX": 1.7701170748589676, - "velocityY": -1.1043686229060403, - "timestamp": 0.4888851960952483 - }, - { - "x": 2.9709557136972373, - "y": 2.6702088910620962, - "heading": -0.155828372393994, - "angularVelocity": 1.9089416921161393, - "velocityX": 1.994996014232719, - "velocityY": -1.2444124110602532, - "timestamp": 0.5499958456071543 - }, - { - "x": 3.1066769296559538, - "y": 2.585572206493352, - "heading": -0.03496320783780511, - "angularVelocity": 1.977808541089754, - "velocityX": 2.2209094002882996, - "velocityY": -1.3849743906298195, - "timestamp": 0.6111064951190603 - }, - { - "x": 3.2562355055408387, - "y": 2.4923383100856262, - "heading": 0.08475597327951304, - "angularVelocity": 1.959055943170691, - "velocityX": 2.4473406366879895, - "velocityY": -1.5256571015427038, - "timestamp": 0.6722171446309666 - }, - { - "x": 3.4194188611023764, - "y": 2.3906298265628587, - "heading": 0.19293288100296513, - "angularVelocity": 1.7701809518876104, - "velocityX": 2.6702932609110017, - "velocityY": -1.6643332109070865, - "timestamp": 0.7333277941428729 - }, - { - "x": 3.5932872633720114, - "y": 2.2801851899216916, - "heading": 0.26578166446591744, - "angularVelocity": 1.192080006427692, - "velocityX": 2.845140800471438, - "velocityY": -1.8072895235657866, - "timestamp": 0.7944384436547791 - }, - { - "x": 3.7772765002319786, - "y": 2.1608722925529302, - "heading": 0.30020319888903974, - "angularVelocity": 0.5632657269731189, - "velocityX": 3.0107557083666587, - "velocityY": -1.9524076134310657, - "timestamp": 0.8555490931666854 - }, - { - "x": 3.9732507395108834, - "y": 2.038358982543431, - "heading": 0.30020324669419357, - "angularVelocity": 7.822720631834703e-7, - "velocityX": 3.2068754111462736, - "velocityY": -2.0047783976771947, - "timestamp": 0.9166597426785916 - }, - { - "x": 4.174667645417682, - "y": 1.9250159473015482, - "heading": 0.3002032939445946, - "angularVelocity": 7.731942209741845e-7, - "velocityX": 3.2959379014218597, - "velocityY": -1.8547182225546543, - "timestamp": 0.9777703921904979 - }, - { - "x": 4.383569049898422, - "y": 1.8261442295125712, - "heading": 0.30020334201253857, - "angularVelocity": 7.865722968351813e-7, - "velocityX": 3.4184124395542623, - "velocityY": -1.6179130573585891, - "timestamp": 1.0388810417024041 - }, - { - "x": 4.59891708073721, - "y": 1.7422357169202292, - "heading": 0.3002033920988207, - "angularVelocity": 8.195998987503142e-7, - "velocityX": 3.52390348587004, - "velocityY": -1.3730587591937538, - "timestamp": 1.0999916912143104 - }, - { - "x": 4.819641568305972, - "y": 1.6737075280608862, - "heading": 0.30020344564176327, - "angularVelocity": 8.761638607213102e-7, - "velocityX": 3.6118825332687328, - "velocityY": -1.1213788334223447, - "timestamp": 1.1611023407262167 - }, - { - "x": 5.044645579459654, - "y": 1.6209002757550093, - "heading": 0.30020350452029126, - "angularVelocity": 9.634740995599556e-7, - "velocityX": 3.681911629982681, - "velocityY": -0.8641252012153561, - "timestamp": 1.222212990238123 - }, - { - "x": 5.272810899090047, - "y": 1.584076421741111, - "heading": 0.3002035713939988, - "angularVelocity": 0.0000010943052979537505, - "velocityX": 3.733642523075129, - "velocityY": -0.6025767081190017, - "timestamp": 1.2833236397500292 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.3002036452391599, - "angularVelocity": 0.0000012083844913604929, - "velocityX": 3.7668180588495743, - "velocityY": -0.33803334595452383, - "timestamp": 1.3444342892619354 - }, - { - "x": 5.642203177619129, - "y": 1.5568731039887571, - "heading": 0.3002037251666547, - "angularVelocity": 0.0000021691766198010436, - "velocityX": 3.7777797970827858, - "velocityY": -0.177650642031358, - "timestamp": 1.3812812173086941 - }, - { - "x": 5.781555187145968, - "y": 1.5562486567630798, - "heading": 0.30020379895395954, - "angularVelocity": 0.0000020025361337225055, - "velocityX": 3.78191661866635, - "velocityY": -0.016947063399286564, - "timestamp": 1.4181281453554528 - }, - { - "x": 5.9208078464820115, - "y": 1.5615467685843438, - "heading": 0.30020386782175307, - "angularVelocity": 0.0000018690240183201108, - "velocityX": 3.7792203235866277, - "velocityY": 0.14378706997068585, - "timestamp": 1.4549750734022115 - }, - { - "x": 6.059709556234372, - "y": 1.5727578639536914, - "heading": 0.3002039327408759, - "angularVelocity": 0.0000017618598429234572, - "velocityX": 3.7696957959723445, - "velocityY": 0.30426133096145186, - "timestamp": 1.4918220014489703 - }, - { - "x": 6.198009351638548, - "y": 1.5898616832743162, - "heading": 0.30020399449987484, - "angularVelocity": 0.0000016760962775678482, - "velocityX": 3.753360259196494, - "velocityY": 0.46418576058553174, - "timestamp": 1.528668929495729 - }, - { - "x": 6.335457356092555, - "y": 1.6128273194464042, - "heading": 0.30020405375523124, - "angularVelocity": 0.0000016081491612309129, - "velocityX": 3.730243245233006, - "velocityY": 0.6232713930166671, - "timestamp": 1.5655158575424877 - }, - { - "x": 6.471805232718414, - "y": 1.6416132738069629, - "heading": 0.3002041110675489, - "angularVelocity": 0.0000015554164405322294, - "velocityX": 3.700386541120455, - "velocityY": 0.7812307805966762, - "timestamp": 1.6023627855892464 - }, - { - "x": 6.60680663253268, - "y": 1.676167531284266, - "heading": 0.3002041691027596, - "angularVelocity": 0.0000015750352547385845, - "velocityX": 3.6638440969339445, - "velocityY": 0.937778515306732, - "timestamp": 1.639209713636005 - }, - { - "x": 6.740176057879373, - "y": 1.7164157557118485, - "heading": 0.3003755003099619, - "angularVelocity": 0.004649809801927131, - "velocityX": 3.6195534449289073, - "velocityY": 1.0923088181600296, - "timestamp": 1.6760566416827638 - }, - { - "x": 6.869837525657242, - "y": 1.7607930294165437, - "heading": 0.3093430143729343, - "angularVelocity": 0.2433720947264084, - "velocityX": 3.5189220554106844, - "velocityY": 1.2043683437702197, - "timestamp": 1.7129035697295225 - }, - { - "x": 6.995165288352465, - "y": 1.807928452248917, - "heading": 0.3301850856624388, - "angularVelocity": 0.5656393190514163, - "velocityX": 3.4013083135772337, - "velocityY": 1.2792225927914125, - "timestamp": 1.7497504977762812 - }, - { - "x": 7.114963240628345, - "y": 1.8557254140007795, - "heading": 0.35927322814233364, - "angularVelocity": 0.7894319559824341, - "velocityX": 3.2512331048020355, - "velocityY": 1.2971762989633377, - "timestamp": 1.78659742582304 - }, - { - "x": 7.228886146559884, - "y": 1.9032191557267804, - "heading": 0.3911508310575271, - "angularVelocity": 0.8651359721153189, - "velocityX": 3.091788433135363, - "velocityY": 1.2889471183522156, - "timestamp": 1.8234443538697986 - }, - { - "x": 7.336942556890349, - "y": 1.949894399055616, - "heading": 0.423151819771819, - "angularVelocity": 0.8684845768875223, - "velocityX": 2.9325758226938556, - "velocityY": 1.2667336411221237, - "timestamp": 1.8602912819165573 - }, - { - "x": 7.43919043499382, - "y": 1.9954619855368936, - "heading": 0.45375826572504047, - "angularVelocity": 0.8306376562622595, - "velocityX": 2.7749362979111796, - "velocityY": 1.2366726046592738, - "timestamp": 1.897138209963316 - }, - { - "x": 7.535687560612936, - "y": 2.0397400223416313, - "heading": 0.48197582868140687, - "angularVelocity": 0.7658050332054913, - "velocityX": 2.618864875157601, - "velocityY": 1.201675123325048, - "timestamp": 1.9339851380100748 - }, - { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.6818119458710672, - "velocityX": 2.464149969521805, - "velocityY": 1.163303151067446, - "timestamp": 1.9708320660568335 - }, - { - "x": 7.766065583230135, - "y": 2.1519617180113952, - "heading": 0.5396264377138885, - "angularVelocity": 0.5126079654708053, - "velocityX": 2.1996686083558368, - "velocityY": 1.0930061650047849, - "timestamp": 2.0342878363776813 - }, - { - "x": 7.889178097615864, - "y": 2.215679412144434, - "heading": 0.5626262771579723, - "angularVelocity": 0.3624546566496878, - "velocityX": 1.9401311143689866, - "velocityY": 1.004127659484183, - "timestamp": 2.097743606698529 - }, - { - "x": 7.996082408378713, - "y": 2.2729729622929304, - "heading": 0.5769453477724344, - "angularVelocity": 0.22565435014123394, - "velocityX": 1.6847059018008188, - "velocityY": 0.90288952224213, - "timestamp": 2.161199377019377 - }, - { - "x": 8.086990111713977, - "y": 2.3232868849538337, - "heading": 0.5832040110121259, - "angularVelocity": 0.09863032483958853, - "velocityX": 1.432615235393288, - "velocityY": 0.7928975159627525, - "timestamp": 2.2246551473402247 - }, - { - "x": 8.16207340516984, - "y": 2.3662086171292933, - "heading": 0.581874089552229, - "angularVelocity": -0.020958243090781536, - "velocityX": 1.183238231546559, - "velocityY": 0.676403926048603, - "timestamp": 2.2881109176610726 - }, - { - "x": 8.221474033046453, - "y": 2.401420292157025, - "heading": 0.5733258497363348, - "angularVelocity": -0.1347117807044565, - "velocityX": 0.9360949772773745, - "velocityY": 0.5549010728211649, - "timestamp": 2.3515666879819204 - }, - { - "x": 8.265310204045273, - "y": 2.4286697945211704, - "heading": 0.5578573342178689, - "angularVelocity": -0.24376846172150818, - "velocityX": 0.6908145748948221, - "velocityY": 0.4294251291311274, - "timestamp": 2.415022458302768 - }, - { - "x": 8.293681756385627, - "y": 2.447752418352113, - "heading": 0.5357134031725193, - "angularVelocity": -0.34896638924064666, - "velocityX": 0.44710752382171043, - "velocityY": 0.30072322397878726, - "timestamp": 2.478478228623616 - }, - { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.45094242234392296, - "velocityX": 0.20474492941705758, - "velocityY": 0.16935099752034305, - "timestamp": 2.541933998944464 - }, - { - "x": 8.303680612059921, - "y": 2.4605220907329786, - "heading": 0.4706360870687623, - "angularVelocity": -0.5539055745477068, - "velocityX": -0.04547302080206286, - "velocityY": 0.030737357256443772, - "timestamp": 2.6077618566137937 - }, - { - "x": 8.28417079095848, - "y": 2.4534909416021047, - "heading": 0.42765554839737563, - "angularVelocity": -0.6529232485020114, - "velocityX": -0.2963763639315537, - "velocityY": -0.1068111492583101, - "timestamp": 2.6735897142831235 - }, - { - "x": 8.248092493080108, - "y": 2.437487576077448, - "heading": 0.3784580606750378, - "angularVelocity": -0.7473657728536394, - "velocityX": -0.5480703634561933, - "velocityY": -0.24310931710774963, - "timestamp": 2.7394175719524534 - }, - { - "x": 8.195385029545998, - "y": 2.412609981570135, - "heading": 0.3233968459430461, - "angularVelocity": -0.8364424528074197, - "velocityX": -0.8006862960492162, - "velocityY": -0.37791894477683524, - "timestamp": 2.8052454296217832 - }, - { - "x": 8.125976752501963, - "y": 2.378976757579925, - "heading": 0.2628923564512202, - "angularVelocity": -0.9191319850595123, - "velocityX": -1.0543906410063166, - "velocityY": -0.5109269112046505, - "timestamp": 2.871073287291113 - }, - { - "x": 8.039781850826214, - "y": 2.336734306251694, - "heading": 0.19745501482460134, - "angularVelocity": -0.9940676173198235, - "velocityX": -1.3093985544650038, - "velocityY": -0.641710862602064, - "timestamp": 2.936901144960443 - }, - { - "x": 7.936695821305385, - "y": 2.286067717588345, - "heading": 0.12772072746128918, - "angularVelocity": -1.0593431084086873, - "velocityX": -1.565993990548153, - "velocityY": -0.7696830864200519, - "timestamp": 3.002729002629773 - }, - { - "x": 7.816588930914964, - "y": 2.2272180395232652, - "heading": 0.05450854772010129, - "angularVelocity": -1.1121762477665882, - "velocityX": -1.8245602187716414, - "velocityY": -0.8939935180740173, - "timestamp": 3.0685568602991027 - }, - { - "x": 7.679296617994528, - "y": 2.1605113660425403, - "heading": -0.02108172308249024, - "angularVelocity": -1.1483021547245422, - "velocityX": -2.085626325712903, - "velocityY": -1.01335021133165, - "timestamp": 3.1343847179684325 - }, - { - "x": 7.52460540494712, - "y": 2.086411774056454, - "heading": -0.09749530920005847, - "angularVelocity": -1.16080925041513, - "velocityX": -2.34993540006212, - "velocityY": -1.1256570486967377, - "timestamp": 3.2002125756377624 - }, - { - "x": 7.352233710537335, - "y": 2.0056280451301003, - "heading": -0.17238692358324137, - "angularVelocity": -1.1376887693867086, - "velocityX": -2.6185220135167313, - "velocityY": -1.2271966882493952, - "timestamp": 3.2660404333070923 - }, - { - "x": 7.161817946345765, - "y": 1.9193611743859953, - "heading": -0.24189742475427997, - "angularVelocity": -1.055943541717672, - "velocityX": -2.8926319484385736, - "velocityY": -1.3104918464375106, - "timestamp": 3.331868290976422 - }, - { - "x": 6.953014973362115, - "y": 1.8300106922230066, - "heading": -0.298719434373675, - "angularVelocity": -0.8631909290566133, - "velocityX": -3.1719545550536123, - "velocityY": -1.3573354097564532, - "timestamp": 3.397696148645752 - }, - { - "x": 6.727431268978415, - "y": 1.7436413956339059, - "heading": -0.3201876383615551, - "angularVelocity": -0.32612642653085444, - "velocityX": -3.4268729436231227, - "velocityY": -1.3120478114745338, - "timestamp": 3.463524006315082 - }, - { - "x": 6.489449879790782, - "y": 1.6705324422161696, - "heading": -0.3201878815696182, - "angularVelocity": -0.0000036946069898928346, - "velocityX": -3.61520787115807, - "velocityY": -1.1106081225517261, - "timestamp": 3.5293518639844117 - }, - { - "x": 6.246606826324383, - "y": 1.6156931769447658, - "heading": -0.3201879117478121, - "angularVelocity": -4.584410758613646e-7, - "velocityX": -3.689062078949349, - "velocityY": -0.8330707881589553, - "timestamp": 3.5951797216537416 - }, - { - "x": 6.0003025647982255, - "y": 1.5794398654466013, - "heading": -0.32018794628515007, - "angularVelocity": -5.246614307457641e-7, - "velocityX": -3.741641764546064, - "velocityY": -0.5507290193199733, - "timestamp": 3.6610075793230714 - }, - { - "x": 5.7519574472803, - "y": 1.5619815744715873, - "heading": -0.32018798749762734, - "angularVelocity": -6.260643854910128e-7, - "velocityX": -3.772644687381865, - "velocityY": -0.26521128885450773, - "timestamp": 3.7268354369924013 - }, - { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.32018803965163717, - "angularVelocity": -7.922787054710825e-7, - "velocityX": -3.7818920261895626, - "velocityY": 0.021835891254127007, - "timestamp": 3.792663294661731 - }, - { - "x": 5.250083628410863, - "y": 1.584512642850435, - "heading": -0.3201880864656727, - "angularVelocity": -6.975962896264183e-7, - "velocityX": -3.768870382202017, - "velocityY": 0.31432577229262343, - "timestamp": 3.859770927241254 - }, - { - "x": 4.999553273526972, - "y": 1.6251081857259102, - "heading": -0.32018812296137045, - "angularVelocity": -5.438382542161688e-7, - "velocityX": -3.7332617059171653, - "velocityY": 0.6049318283932773, - "timestamp": 3.926878559820777 - }, - { - "x": 4.752913977063861, - "y": 1.6849623155206188, - "heading": -0.3201881529794151, - "angularVelocity": -4.473119302496649e-7, - "velocityX": -3.675279353221748, - "velocityY": 0.8919124024198194, - "timestamp": 3.9939861924003 - }, - { - "x": 4.511643860653374, - "y": 1.7637163133941143, - "heading": -0.3201881787520496, - "angularVelocity": -3.8404922716455346e-7, - "velocityX": -3.595270867655492, - "velocityY": 1.1735475510952014, - "timestamp": 4.061093824979823 - }, - { - "x": 4.277188862383835, - "y": 1.86089818593956, - "heading": -0.3201882016908444, - "angularVelocity": -3.41820951724987e-7, - "velocityX": -3.4937158301883047, - "velocityY": 1.4481493208732221, - "timestamp": 4.128201457559346 - }, - { - "x": 4.050954064298834, - "y": 1.9759254853688604, - "heading": -0.32018822275897413, - "angularVelocity": -3.1394535808439923e-7, - "velocityX": -3.3712230544999895, - "velocityY": 1.7140717830120589, - "timestamp": 4.195309090138869 - }, - { - "x": 3.834295246075004, - "y": 2.1081087693328064, - "heading": -0.32018824266587215, - "angularVelocity": -2.966413400864343e-7, - "velocityX": -3.2285272165887804, - "velocityY": 1.969720565053586, - "timestamp": 4.262416722718392 - }, - { - "x": 3.6285105963453295, - "y": 2.256655537498403, - "heading": -0.32018826197928, - "angularVelocity": -2.8779748460931035e-7, - "velocityX": -3.066486505626901, - "velocityY": 2.213559955189428, - "timestamp": 4.329524355297915 - }, - { - "x": 3.4323361610843617, - "y": 2.417680552436319, - "heading": -0.3201882811600919, - "angularVelocity": -2.858216149634079e-7, - "velocityX": -2.9232805229494647, - "velocityY": 2.399503733753405, - "timestamp": 4.396631987877438 - }, - { - "x": 3.2362856931678534, - "y": 2.578856476556199, - "heading": -0.32018830034052725, - "angularVelocity": -2.858160039511642e-7, - "velocityX": -2.921433231669861, - "velocityY": 2.401752497063368, - "timestamp": 4.463739620456961 - }, - { - "x": 3.0402351711109183, - "y": 2.7400323348209366, - "heading": -0.3201883195209977, - "angularVelocity": -2.858165259418001e-7, - "velocityX": -2.9214340384398776, - "velocityY": 2.401751515727269, - "timestamp": 4.530847253036484 - }, - { - "x": 2.8448193652926044, - "y": 2.899781496680513, - "heading": -0.32456406822795664, - "angularVelocity": -0.06520493331028634, - "velocityX": -2.911975855902028, - "velocityY": 2.380491692510139, - "timestamp": 4.597954885616007 - }, - { - "x": 2.66318575993582, - "y": 3.04911523833719, - "heading": -0.3692876119349906, - "angularVelocity": -0.6664449629337832, - "velocityX": -2.706601296678858, - "velocityY": 2.2252869892216163, - "timestamp": 4.66506251819553 - }, - { - "x": 2.496643428698106, - "y": 3.186035937240345, - "heading": -0.4222622822262224, - "angularVelocity": -0.7893985863449552, - "velocityX": -2.4817196619231154, - "velocityY": 2.040314844081317, - "timestamp": 4.732170150775053 - }, - { - "x": 2.3452748100493945, - "y": 3.3104804771184444, - "heading": -0.4767233164983405, - "angularVelocity": -0.8115475420412368, - "velocityX": -2.255609575696765, - "velocityY": 1.8544021759467144, - "timestamp": 4.799277783354576 - }, - { - "x": 2.209079907655855, - "y": 3.4224492823331634, - "heading": -0.5296433021172221, - "angularVelocity": -0.7885837062747133, - "velocityX": -2.0294994348392206, - "velocityY": 1.6684958314098723, - "timestamp": 4.866385415934099 - }, - { - "x": 2.088048432415951, - "y": 3.5219507247047837, - "heading": -0.5792907495482946, - "angularVelocity": -0.7398181923977142, - "velocityX": -1.803542616355617, - "velocityY": 1.4827142390056927, - "timestamp": 4.933493048513622 - }, - { - "x": 1.9821698412647601, - "y": 3.6089936285690154, - "heading": -0.624538681260696, - "angularVelocity": -0.6742590965160707, - "velocityX": -1.5777429046647484, - "velocityY": 1.297064141863228, - "timestamp": 5.000600681093145 - }, - { - "x": 1.8914349570352331, - "y": 3.6835858822586136, - "heading": -0.6645923745497577, - "angularVelocity": -0.5968574922025148, - "velocityX": -1.3520799459287431, - "velocityY": 1.1115315921956592, - "timestamp": 5.067708313672668 - }, - { - "x": 1.815836039628641, - "y": 3.7457342610555044, - "heading": -0.698860244587487, - "angularVelocity": -0.5106404252470358, - "velocityX": -1.1265323257679698, - "velocityY": 0.926100003948804, - "timestamp": 5.134815946252191 - }, - { - "x": 1.7553665690239355, - "y": 3.7954445247907986, - "heading": -0.7268845476309118, - "angularVelocity": -0.41760231982876106, - "velocityX": -0.9010818632746272, - "velocityY": 0.7407542454487245, - "timestamp": 5.201923578831714 - }, - { - "x": 1.7100210087875185, - "y": 3.8327215653417337, - "heading": -0.7483006908429537, - "angularVelocity": -0.31913125808251036, - "velocityX": -0.6757139015846285, - "velocityY": 0.5554813829374979, - "timestamp": 5.269031211411237 - }, - { - "x": 1.6797946046172731, - "y": 3.8575695457400823, - "heading": -0.7628116421295846, - "angularVelocity": -0.21623399200434476, - "velocityX": -0.4504167858168276, - "velocityY": 0.37027055557210875, - "timestamp": 5.33613884399076 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -0.10966416437475777, - "velocityX": -0.22518126874279432, - "velocityY": 0.1851126545280661, - "timestamp": 5.403246476570283 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -1.211900873027185e-19, - "velocityX": -5.462077711506373e-19, - "velocityY": -3.512888080115249e-19, - "timestamp": 5.470354109149806 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint.3.traj b/src/main/deploy/choreo/SourceLanePHGSprint.3.traj deleted file mode 100644 index d9e2ca2a..00000000 --- a/src/main/deploy/choreo/SourceLanePHGSprint.3.traj +++ /dev/null @@ -1,319 +0,0 @@ -{ - "samples": [ - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -1.211900873027185e-19, - "velocityX": -5.462077711506373e-19, - "velocityY": -3.512888080115249e-19, - "timestamp": 0 - }, - { - "x": 1.659892349205253, - "y": 3.8800826826438297, - "heading": -0.7264777443526473, - "angularVelocity": 0.7466474211356967, - "velocityX": -0.08186842286768115, - "velocityY": 0.1724334423754288, - "timestamp": 0.05851918722290517 - }, - { - "x": 1.6562452238185605, - "y": 3.90221669421699, - "heading": -0.6460681306884017, - "angularVelocity": 1.3740726329290587, - "velocityX": -0.06232358239700476, - "velocityY": 0.3782351160970439, - "timestamp": 0.11703837444581033 - }, - { - "x": 1.6553319250883793, - "y": 3.9350953760295733, - "heading": -0.5183992679116819, - "angularVelocity": 2.1816581677803772, - "velocityX": -0.01560682527428515, - "velocityY": 0.5618444714097892, - "timestamp": 0.1755575616687155 - }, - { - "x": 1.6623538133314129, - "y": 3.9788519148323376, - "heading": -0.3621106955703112, - "angularVelocity": 2.670723565350507, - "velocityX": 0.11999292157436166, - "velocityY": 0.7477297768352748, - "timestamp": 0.23407674889162067 - }, - { - "x": 1.6832034751641156, - "y": 4.019420947962781, - "heading": -0.22540240918746318, - "angularVelocity": 2.3361275655130602, - "velocityX": 0.35628761816674753, - "velocityY": 0.6932603656286723, - "timestamp": 0.29259593611452583 - }, - { - "x": 1.7146647081648767, - "y": 4.052413452071288, - "heading": -0.11878752772286896, - "angularVelocity": 1.8218790541038834, - "velocityX": 0.5376225216683612, - "velocityY": 0.5637895137339223, - "timestamp": 0.351115123337431 - }, - { - "x": 1.7541026264736177, - "y": 4.074977116543675, - "heading": -0.0408151972626467, - "angularVelocity": 1.332423332593777, - "velocityX": 0.6739314091721454, - "velocityY": 0.38557720199428364, - "timestamp": 0.40963431056033617 - }, - { - "x": 1.799090355046403, - "y": 4.0866639858943925, - "heading": 1.7934233852716553e-24, - "angularVelocity": 0.6974669198185859, - "velocityX": 0.7687688552717376, - "velocityY": 0.199710042215758, - "timestamp": 0.46815349778324133 - }, - { - "x": 1.848745584487915, - "y": 4.087520122528076, - "heading": 7.573339964499894e-25, - "angularVelocity": -5.2358045608260825e-25, - "velocityX": 0.848529034628775, - "velocityY": 0.014630015800164336, - "timestamp": 0.5266726850061465 - }, - { - "x": 1.9382598731728313, - "y": 4.089063493944204, - "heading": 6.819777064270339e-25, - "angularVelocity": -2.006050179968467e-26, - "velocityX": 1.1795649340806893, - "velocityY": 0.02033761122914328, - "timestamp": 0.6025602314214531 - }, - { - "x": 2.0528956622357724, - "y": 4.091040000742217, - "heading": 6.067470023829544e-25, - "angularVelocity": -1.8954686232133377e-26, - "velocityX": 1.510600809724148, - "velocityY": 0.026045206247625866, - "timestamp": 0.6784477778367597 - }, - { - "x": 2.1926529480236727, - "y": 4.09344964285913, - "heading": 5.320081952060173e-25, - "angularVelocity": -2.1711320260805291e-26, - "velocityX": 1.8416366372297248, - "velocityY": 0.03175280043613335, - "timestamp": 0.7543353242520663 - }, - { - "x": 2.3575317192504395, - "y": 4.096292420100353, - "heading": 4.5695624647021355e-25, - "angularVelocity": -2.0004263459154622e-26, - "velocityX": 2.1726723160140353, - "velocityY": 0.03746039206044517, - "timestamp": 0.8302228706673729 - }, - { - "x": 2.4972894766259857, - "y": 4.098702070348201, - "heading": 3.822235688125773e-25, - "angularVelocity": -1.9197661326309743e-26, - "velocityX": 1.8416428515253338, - "velocityY": 0.0317529075806614, - "timestamp": 0.9061104170826795 - }, - { - "x": 2.611925746748375, - "y": 4.100678585440459, - "heading": 3.063572284762821e-25, - "angularVelocity": -2.4686338901390002e-26, - "velocityX": 1.5106071488334032, - "velocityY": 0.026045315544143462, - "timestamp": 0.9819979634979861 - }, - { - "x": 2.701440519692856, - "y": 4.102221965206007, - "heading": 2.3114445870748474e-25, - "angularVelocity": -1.5210807516831905e-26, - "velocityX": 1.1795713153591425, - "velocityY": 0.020337721252726194, - "timestamp": 1.0578855099132927 - }, - { - "x": 2.7658337920580536, - "y": 4.1033322095862, - "heading": 1.561656112047754e-25, - "angularVelocity": -1.9040979941775662e-26, - "velocityX": 0.8485354370636105, - "velocityY": 0.01463012618851756, - "timestamp": 1.1337730563286001 - }, - { - "x": 2.805105562125262, - "y": 4.104009318551404, - "heading": 8.077058611592956e-26, - "angularVelocity": -3.263981497829794e-26, - "velocityX": 0.5174995361200375, - "velocityY": 0.008922530733819996, - "timestamp": 1.2096606027439076 - }, - { - "x": 2.819255828857422, - "y": 4.10425329208374, - "heading": 3.4975515409752656e-27, - "angularVelocity": -3.9242842268499737e-26, - "velocityX": 0.18646362151070262, - "velocityY": 0.0032149350435025517, - "timestamp": 1.2855481491592151 - }, - { - "x": 2.810470941680652, - "y": 4.100290874791341, - "heading": -0.013665728423960572, - "angularVelocity": -0.18841669161843644, - "velocityX": -0.12112192828199068, - "velocityY": -0.05463196207942443, - "timestamp": 1.3580774368065907 - }, - { - "x": 2.779376565530904, - "y": 4.092132450200435, - "heading": -0.040981035402949714, - "angularVelocity": -0.3766107163742046, - "velocityX": -0.4287147600418107, - "velocityY": -0.11248455424752818, - "timestamp": 1.4306067244539662 - }, - { - "x": 2.7259718800290944, - "y": 4.079777209778572, - "heading": -0.08192049181497908, - "angularVelocity": -0.5644541362527821, - "velocityX": -0.7363189028058957, - "velocityY": -0.17034829408406268, - "timestamp": 1.5031360121013417 - }, - { - "x": 2.650255783412156, - "y": 4.06322370149856, - "heading": -0.13644943849416394, - "angularVelocity": -0.7518196917125987, - "velocityX": -1.0439382361654468, - "velocityY": -0.22823205379447506, - "timestamp": 1.5756652997487173 - }, - { - "x": 2.5522270104067304, - "y": 4.04246950295157, - "heading": -0.20452759410435262, - "angularVelocity": -0.9386298668914527, - "velocityX": -1.3515750145241086, - "velocityY": -0.2861492125483705, - "timestamp": 1.6481945873960928 - }, - { - "x": 2.4318844389859766, - "y": 4.01751088765649, - "heading": -0.2861194089559698, - "angularVelocity": -1.1249498995261227, - "velocityX": -1.6592272628656817, - "velocityY": -0.34411775028624336, - "timestamp": 1.7207238750434684 - }, - { - "x": 2.2892276526666033, - "y": 3.9883426531177015, - "heading": -0.3812142224775482, - "angularVelocity": -1.311122949172083, - "velocityX": -1.9668852534846832, - "velocityY": -0.40215801760798864, - "timestamp": 1.793253162690844 - }, - { - "x": 2.1330829846519728, - "y": 3.9587634030266514, - "heading": -0.4780632841326763, - "angularVelocity": -1.3353097044877917, - "velocityX": -2.1528498773319837, - "velocityY": -0.40782490839919366, - "timestamp": 1.8657824503382194 - }, - { - "x": 1.9992497997330843, - "y": 3.9334042162180256, - "heading": -0.5612966415288853, - "angularVelocity": -1.1475827227322806, - "velocityX": -1.8452295515373094, - "velocityY": -0.349640643541367, - "timestamp": 1.938311737985595 - }, - { - "x": 1.8877256900718635, - "y": 3.9122685100016006, - "heading": -0.6308164435957174, - "angularVelocity": -0.9585066160421192, - "velocityX": -1.537642423891296, - "velocityY": -0.29140926240973475, - "timestamp": 2.0108410256329705 - }, - { - "x": 1.7985083705984093, - "y": 3.895358481595443, - "heading": -0.6865270710591681, - "angularVelocity": -0.768112155386183, - "velocityX": -1.2300868017236275, - "velocityY": -0.23314758705988012, - "timestamp": 2.083370313280346 - }, - { - "x": 1.731596065924516, - "y": 3.88267537699245, - "heading": -0.7283506709725182, - "angularVelocity": -0.5766442946012179, - "velocityX": -0.9225556577807338, - "velocityY": -0.17486873254092689, - "timestamp": 2.1558996009277216 - }, - { - "x": 1.6869877424298634, - "y": 3.8742198088507074, - "heading": -0.7562373778990242, - "angularVelocity": -0.3844889124250871, - "velocityX": -0.6150387649128701, - "velocityY": -0.11658143097795469, - "timestamp": 2.228428888575097 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -0.19210952061606446, - "velocityX": -0.3075243171780863, - "velocityY": -0.05829081246861152, - "timestamp": 2.3009581762224727 - }, - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -5.999453085593573e-20, - "velocityX": -1.6973677435282043e-18, - "velocityY": -2.503873223513922e-18, - "timestamp": 2.373487463869848 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGSprint.4.traj b/src/main/deploy/choreo/SourceLanePHGSprint.4.traj deleted file mode 100644 index 0cd0adae..00000000 --- a/src/main/deploy/choreo/SourceLanePHGSprint.4.traj +++ /dev/null @@ -1,391 +0,0 @@ -{ - "samples": [ - { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -5.999453085593573e-20, - "velocityX": -1.6973677435282043e-18, - "velocityY": -2.503873223513922e-18, - "timestamp": 0 - }, - { - "x": 1.6770247995246623, - "y": 3.8595873316999896, - "heading": -0.7649878221642326, - "angularVelocity": 0.08507451391469989, - "velocityX": 0.20257164680788678, - "velocityY": -0.1707799926111949, - "timestamp": 0.06092450226104873 - }, - { - "x": 1.70170978110981, - "y": 3.8387765844705157, - "heading": -0.7547381408402699, - "angularVelocity": 0.1682357827076709, - "velocityX": 0.405173299231529, - "velocityY": -0.3415825563958536, - "timestamp": 0.12184900452209746 - }, - { - "x": 1.7387402457793653, - "y": 3.80755819428449, - "heading": -0.7395583157876386, - "angularVelocity": 0.24915796583103744, - "velocityX": 0.607809063599532, - "velocityY": -0.5124110830197925, - "timestamp": 0.1827735067831462 - }, - { - "x": 1.7881185850906471, - "y": 3.7659303409499723, - "heading": -0.7196099497583256, - "angularVelocity": 0.3274276405876679, - "velocityX": 0.8104840824091846, - "velocityY": -0.6832694858326719, - "timestamp": 0.24369800904419492 - }, - { - "x": 1.8498475856412335, - "y": 3.713890927230462, - "heading": -0.6950873259687543, - "angularVelocity": 0.4025083977624826, - "velocityX": 1.0132048397554583, - "velocityY": -0.8541623121766772, - "timestamp": 0.30462251130524365 - }, - { - "x": 1.9239305360498986, - "y": 3.6514375329357844, - "heading": -0.6662283152580175, - "angularVelocity": 0.47368480069122665, - "velocityX": 1.2159795756924812, - "velocityY": -1.025094862935087, - "timestamp": 0.3655470135662924 - }, - { - "x": 2.010371367313072, - "y": 3.5785673636651287, - "heading": -0.6333309419257536, - "angularVelocity": 0.539969505065564, - "velocityX": 1.4188188340512586, - "velocityY": -1.1960732803104883, - "timestamp": 0.4264715158273411 - }, - { - "x": 2.1091748328759063, - "y": 3.4952772039974476, - "heading": -0.5967798759520212, - "angularVelocity": 0.5999403297070707, - "velocityX": 1.6217361143055882, - "velocityY": -1.3671044748269063, - "timestamp": 0.48739601808838984 - }, - { - "x": 2.220346716936752, - "y": 3.40156341112366, - "heading": -0.5570916262340567, - "angularVelocity": 0.6514333026129457, - "velocityX": 1.82474833498841, - "velocityY": -1.5381954615278515, - "timestamp": 0.5483205203494386 - }, - { - "x": 2.3438939700884585, - "y": 3.297422079154605, - "heading": -0.5149984011781104, - "angularVelocity": 0.6909079843703304, - "velocityX": 2.027874641016087, - "velocityY": -1.7093505585459128, - "timestamp": 0.6092450226104873 - }, - { - "x": 2.4798242349395756, - "y": 3.182849894813871, - "heading": -0.4716228537300689, - "angularVelocity": 0.7119557130263738, - "velocityX": 2.2311263909664074, - "velocityY": -1.8805600388792227, - "timestamp": 0.670169524871536 - }, - { - "x": 2.628141721858615, - "y": 3.057848265310819, - "heading": -0.42891150330345956, - "angularVelocity": 0.7010537442489162, - "velocityX": 2.4344472488840574, - "velocityY": -2.0517464216194523, - "timestamp": 0.7310940271325848 - }, - { - "x": 2.788815804109156, - "y": 2.922450520332472, - "heading": -0.3910899170501554, - "angularVelocity": 0.6207943413513182, - "velocityX": 2.6372654069800667, - "velocityY": -2.2223857389625743, - "timestamp": 0.7920185293936335 - }, - { - "x": 2.9613127738384817, - "y": 2.777334038124512, - "heading": -0.375202514467059, - "angularVelocity": 0.2607719717597751, - "velocityX": 2.831323413857593, - "velocityY": -2.3819067341111335, - "timestamp": 0.8529430316546822 - }, - { - "x": 3.137446263833113, - "y": 2.628782733735228, - "heading": -0.3752024981016849, - "angularVelocity": 2.6861728103084975e-7, - "velocityX": 2.891012375282727, - "velocityY": -2.438285072117188, - "timestamp": 0.913867533915731 - }, - { - "x": 3.313579787334416, - "y": 2.480231469074176, - "heading": -0.37520248173599324, - "angularVelocity": 2.686224924493493e-7, - "velocityX": 2.8910129252531243, - "velocityY": -2.4382844200276295, - "timestamp": 0.9747920361767797 - }, - { - "x": 3.489713311593346, - "y": 2.3316802053114225, - "heading": -0.37520246537030155, - "angularVelocity": 2.6862249292188543e-7, - "velocityX": 2.8910129376886133, - "velocityY": -2.4382844052831705, - "timestamp": 1.0357165384378284 - }, - { - "x": 3.6658468653792484, - "y": 2.183128976558075, - "heading": -0.37520244900461003, - "angularVelocity": 2.6862249035440315e-7, - "velocityX": 2.89101342233718, - "velocityY": -2.4382838306472756, - "timestamp": 1.0966410406988771 - }, - { - "x": 3.8419803666744587, - "y": 2.034577685567575, - "heading": -0.37520243263891834, - "angularVelocity": 2.686224937536833e-7, - "velocityX": 2.8910125607676815, - "velocityY": -2.4382848521928207, - "timestamp": 1.1575655429599259 - }, - { - "x": 4.017984969789346, - "y": 1.8858736976718657, - "heading": -0.37520241627306933, - "angularVelocity": 2.6862507525069377e-7, - "velocityX": 2.8888968573061926, - "velocityY": -2.440791182151078, - "timestamp": 1.2184900452209746 - }, - { - "x": 4.198449630537114, - "y": 1.742615181247194, - "heading": -0.37520239987870785, - "angularVelocity": 2.6909307205451447e-7, - "velocityX": 2.9621031612948867, - "velocityY": -2.3514105344815093, - "timestamp": 1.2794145474820233 - }, - { - "x": 4.38853011399539, - "y": 1.6123849775845955, - "heading": -0.3752023832167796, - "angularVelocity": 2.734848475989746e-7, - "velocityX": 3.1199349424936167, - "velocityY": -2.137566969436863, - "timestamp": 1.340339049743072 - }, - { - "x": 4.587288186636938, - "y": 1.495827162172114, - "heading": -0.3752023659192684, - "angularVelocity": 2.8391715306138947e-7, - "velocityX": 3.2623667861891117, - "velocityY": -1.9131517055823732, - "timestamp": 1.4012635520041208 - }, - { - "x": 4.7937421798706055, - "y": 1.3935176134109497, - "heading": -0.3752023473605566, - "angularVelocity": 3.046181942517293e-7, - "velocityX": 3.3886857597794955, - "velocityY": -1.6792841133570497, - "timestamp": 1.4621880542651695 - }, - { - "x": 5.0647852667187605, - "y": 1.2879733857720441, - "heading": -0.3752023309051681, - "angularVelocity": 2.139583629256744e-7, - "velocityX": 3.524191190248399, - "velocityY": -1.3723206946612971, - "timestamp": 1.539097356915418 - }, - { - "x": 5.34411641028699, - "y": 1.2068681953777092, - "heading": -0.37520231550400196, - "angularVelocity": 2.0025101753440068e-7, - "velocityX": 3.6319552244350577, - "velocityY": -1.0545563098285724, - "timestamp": 1.6160066595656666 - }, - { - "x": 5.629536746845857, - "y": 1.1508398853670567, - "heading": -0.375202300582326, - "angularVelocity": 1.940165285663064e-7, - "velocityX": 3.711128910592765, - "velocityY": -0.7284984791169633, - "timestamp": 1.6929159622159151 - }, - { - "x": 5.917306487549604, - "y": 1.108501764833761, - "heading": -0.37520228573814113, - "angularVelocity": 1.9300896478374464e-7, - "velocityX": 3.741676634521091, - "velocityY": -0.5504941414672958, - "timestamp": 1.7698252648661636 - }, - { - "x": 6.2050577552774975, - "y": 1.0660382742134262, - "heading": -0.37520227089383906, - "angularVelocity": 1.9301048840600142e-7, - "velocityX": 3.7414364428249387, - "velocityY": -0.5521242444940722, - "timestamp": 1.8467345675164122 - }, - { - "x": 6.492809022919495, - "y": 1.0235747830116217, - "heading": -0.3752022560493111, - "angularVelocity": 1.9301342582848665e-7, - "velocityX": 3.7414364417080774, - "velocityY": -0.5521242520545259, - "timestamp": 1.9236438701666607 - }, - { - "x": 6.772658036083171, - "y": 0.9819575118333774, - "heading": -0.34826612342726787, - "angularVelocity": 0.3502324386496837, - "velocityX": 3.638688734915609, - "velocityY": -0.541121421520385, - "timestamp": 2.0005531728169093 - }, - { - "x": 7.027520568845826, - "y": 0.9441505058911935, - "heading": -0.3001058813779335, - "angularVelocity": 0.6261952766409414, - "velocityX": 3.313806314454086, - "velocityY": -0.49157910212908845, - "timestamp": 2.077462475467158 - }, - { - "x": 7.256873768560855, - "y": 0.9101570740557017, - "heading": -0.24860478708038097, - "angularVelocity": 0.6696341342705694, - "velocityX": 2.9821255922450605, - "velocityY": -0.44199375971564203, - "timestamp": 2.1543717781174063 - }, - { - "x": 7.460704020800595, - "y": 0.8799617502558256, - "heading": -0.1984971865955797, - "angularVelocity": 0.6515154702763284, - "velocityX": 2.65026785077839, - "velocityY": -0.3926095122353607, - "timestamp": 2.231281080767655 - }, - { - "x": 7.6390235862814775, - "y": 0.8535543484261158, - "heading": -0.15201891065632164, - "angularVelocity": 0.6043258011403606, - "velocityX": 2.318569527171569, - "velocityY": -0.3433577073218239, - "timestamp": 2.3081903834179034 - }, - { - "x": 7.7918452312093995, - "y": 0.8309283282540864, - "heading": -0.11047967273973482, - "angularVelocity": 0.5401068074364143, - "velocityX": 1.9870371939645617, - "velocityY": -0.29419094169820903, - "timestamp": 2.385099686068152 - }, - { - "x": 7.9191796079863, - "y": 0.812079256076005, - "heading": -0.07474204501348639, - "angularVelocity": 0.46467236725273836, - "velocityX": 1.6556433667844261, - "velocityY": -0.2450818240258775, - "timestamp": 2.4620089887184005 - }, - { - "x": 8.021035341477521, - "y": 0.7970039548993032, - "heading": -0.04541767666824434, - "angularVelocity": 0.3812850635065168, - "velocityX": 1.3243616829347478, - "velocityY": -0.1960140146538302, - "timestamp": 2.538918291368649 - }, - { - "x": 8.097419437322872, - "y": 0.7857000289910334, - "heading": -0.022962750583734995, - "angularVelocity": 0.29196632020738633, - "velocityX": 0.9931710887135993, - "velocityY": -0.14697735538800996, - "timestamp": 2.6158275940188975 - }, - { - "x": 8.148337646767295, - "y": 0.7781655823556067, - "heading": -0.0077300757248004864, - "angularVelocity": 0.19806023893112515, - "velocityX": 0.662055274067148, - "velocityY": -0.09796534847923602, - "timestamp": 2.692736896669146 - }, - { - "x": 8.173794746398926, - "y": 0.7743990421295166, - "heading": 1.1088837243155707e-19, - "angularVelocity": 0.10050898211824409, - "velocityX": 0.3310015661876223, - "velocityY": -0.04897379245809494, - "timestamp": 2.7696461993193946 - }, - { - "x": 8.173794746398926, - "y": 0.7743990421295166, - "heading": 5.633915553103528e-20, - "angularVelocity": 2.8365160942051004e-20, - "velocityX": -1.2324505157130932e-18, - "velocityY": 3.669165812220002e-18, - "timestamp": 2.846555501969643 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubGSub.1.traj b/src/main/deploy/choreo/SourceLanePSubHSubGSub.1.traj index 1d70bb19..8b631cb8 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubGSub.1.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubGSub.1.traj @@ -1,904 +1,905 @@ { - "samples": [ - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": -1.7128958192175968e-32, - "velocityY": -9.692960229070848e-33, - "timestamp": 0 - }, - { - "x": 0.6705320996855912, - "y": 4.398099342118358, - "heading": -1.0386114127976835, - "angularVelocity": 0.15927551181553445, - "velocityX": 0.1260500242755718, - "velocityY": -0.20514712032687513, - "timestamp": 0.05592000838348859 - }, - { - "x": 0.6846335258864488, - "y": 4.375148415401169, - "heading": -1.0210111049652701, - "angularVelocity": 0.31474079388031545, - "velocityX": 0.25217138924859694, - "velocityY": -0.41042423598715305, - "timestamp": 0.11184001676697718 - }, - { - "x": 0.7057920861320249, - "y": 4.340710150429397, - "heading": -0.99496347376924, - "angularVelocity": 0.4658016325283906, - "velocityX": 0.3783719076090773, - "velocityY": -0.6158487090273924, - "timestamp": 0.16776002515046576 - }, - { - "x": 0.7340126708459819, - "y": 4.29477510174039, - "heading": -0.9607567098648512, - "angularVelocity": 0.6117088479280167, - "velocityX": 0.5046598798845959, - "velocityY": -0.8214420923185946, - "timestamp": 0.22368003353395435 - }, - { - "x": 0.7693006604451206, - "y": 4.237332365746024, - "heading": -0.918731573784226, - "angularVelocity": 0.7515223494321578, - "velocityX": 0.6310440684690269, - "velocityY": -1.0272304610620717, - "timestamp": 0.2796000419174429 - }, - { - "x": 0.8116619611973517, - "y": 4.168369327121192, - "heading": -0.8692948358062459, - "angularVelocity": 0.8840617054087768, - "velocityX": 0.7575338769931039, - "velocityY": -1.2332444257142754, - "timestamp": 0.3355200503009315 - }, - { - "x": 0.8611030812908155, - "y": 4.087871426229515, - "heading": -0.8129373545696883, - "angularVelocity": 1.0078231900479873, - "velocityX": 0.8841400694078274, - "velocityY": -1.4395187557848361, - "timestamp": 0.3914400586844201 - }, - { - "x": 0.9176312901451578, - "y": 3.995821968070834, - "heading": -0.7502611156342615, - "angularVelocity": 1.120819555419329, - "velocityX": 1.0108762585778373, - "velocityY": -1.6460916373156267, - "timestamp": 0.4473600670679087 - }, - { - "x": 0.9812548770105831, - "y": 3.8922019971306105, - "heading": -0.6820246305048665, - "angularVelocity": 1.2202516970570252, - "velocityX": 1.1377606818136925, - "velocityY": -1.8530034943775027, - "timestamp": 0.5032800754513973 - }, - { - "x": 1.0519833618359966, - "y": 3.77699034023748, - "heading": -0.6092267013635411, - "angularVelocity": 1.3018225720227254, - "velocityX": 1.2648153473148886, - "velocityY": -2.060293984633039, - "timestamp": 0.5592000838348858 - }, - { - "x": 1.129826908295346, - "y": 3.6501643798944907, - "heading": -0.5332724156478236, - "angularVelocity": 1.3582667083101592, - "velocityX": 1.3920517666147918, - "velocityY": -2.26798893650447, - "timestamp": 0.6151200922183744 - }, - { - "x": 1.214791954900265, - "y": 3.511704401985981, - "heading": -0.45632702327380104, - "angularVelocity": 1.3759903583409006, - "velocityX": 1.5194033238021893, - "velocityY": -2.4760364297333126, - "timestamp": 0.6710401006018629 - }, - { - "x": 1.306860167939052, - "y": 3.361616595640326, - "heading": -0.38215505844470404, - "angularVelocity": 1.3263940219829728, - "velocityX": 1.6464270249639614, - "velocityY": -2.6839732447173734, - "timestamp": 0.7269601089853515 - }, - { - "x": 1.4058792704184802, - "y": 3.2000739494751453, - "heading": -0.3184323584737964, - "angularVelocity": 1.1395330904442935, - "velocityX": 1.770727604337509, - "velocityY": -2.888816558419535, - "timestamp": 0.78288011736884 - }, - { - "x": 1.5121691709784744, - "y": 3.0300749520186443, - "heading": -0.28662541962876287, - "angularVelocity": 0.5687935278354703, - "velocityX": 1.9007490097476158, - "velocityY": -3.040038840671852, - "timestamp": 0.8388001257523285 - }, - { - "x": 1.6244650109400063, - "y": 2.8508645326617055, - "heading": -0.2866253125429205, - "angularVelocity": 0.0000019149825875479786, - "velocityX": 2.00815134345883, - "velocityY": -3.2047638141959642, - "timestamp": 0.8947201341358171 - }, - { - "x": 1.735288205529194, - "y": 2.670739688820617, - "heading": -0.28662527835532886, - "angularVelocity": 6.113659969296124e-7, - "velocityX": 1.9818164873864772, - "velocityY": -3.2211161809173365, - "timestamp": 0.9506401425193056 - }, - { - "x": 1.8506362591615513, - "y": 2.493478394875359, - "heading": -0.2866252441530595, - "angularVelocity": 6.116284727574992e-7, - "velocityX": 2.062733124811472, - "velocityY": -3.1699082147776885, - "timestamp": 1.0065601509027942 - }, - { - "x": 1.9771720781105007, - "y": 2.324022128559085, - "heading": -0.2866252095630549, - "angularVelocity": 6.185622213406466e-7, - "velocityX": 2.2628004288052175, - "velocityY": -3.0303333496335663, - "timestamp": 1.0624801592862827 - }, - { - "x": 2.1143701177449694, - "y": 2.1630766803808767, - "heading": -0.28662517392184667, - "angularVelocity": 6.373605666632096e-7, - "velocityX": 2.4534695827223683, - "velocityY": -2.878137053815794, - "timestamp": 1.1184001676697712 - }, - { - "x": 2.261659606829455, - "y": 2.011311910787294, - "heading": -0.2866251364727607, - "angularVelocity": 6.696902775127292e-7, - "velocityX": 2.633931813357441, - "velocityY": -2.7139618533818717, - "timestamp": 1.1743201760532598 - }, - { - "x": 2.418427669547749, - "y": 1.8693594092228303, - "heading": -0.2866250962912339, - "angularVelocity": 7.185536621676126e-7, - "velocityX": 2.803434177677683, - "velocityY": -2.538492136678184, - "timestamp": 1.2302401844367483 - }, - { - "x": 2.5840219554137547, - "y": 1.7378099138847216, - "heading": -0.28662505217416817, - "angularVelocity": 7.889316720181228e-7, - "velocityX": 2.961270762521925, - "velocityY": -2.3524584337678864, - "timestamp": 1.2861601928202369 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.2866250016379229, - "angularVelocity": 9.037238488587988e-7, - "velocityX": 3.1067845266977008, - "velocityY": -2.1566350283233997, - "timestamp": 1.3420802012037254 - }, - { - "x": 2.964172022537395, - "y": 1.4952512181839386, - "heading": -0.28662495601958665, - "angularVelocity": 7.195928192095116e-7, - "velocityX": 3.256089340971223, - "velocityY": -1.9238160187086888, - "timestamp": 1.4054748531095118 - }, - { - "x": 3.1789518158723995, - "y": 1.3887033110695066, - "heading": -0.2866249156918973, - "angularVelocity": 6.361370893313497e-7, - "velocityX": 3.3879796935268014, - "velocityY": -1.6807081340674839, - "timestamp": 1.4688695050152982 - }, - { - "x": 3.4009440542760783, - "y": 1.2981369637223683, - "heading": -0.28662487900587236, - "angularVelocity": 5.786927430417384e-7, - "velocityX": 3.501750253847766, - "velocityY": -1.4286117933375984, - "timestamp": 1.5322641569210846 - }, - { - "x": 3.6289614625982565, - "y": 1.2240364953903842, - "heading": -0.2866248447654741, - "angularVelocity": 5.401149339588049e-7, - "velocityX": 3.596792496960864, - "velocityY": -1.168875703302355, - "timestamp": 1.595658808826871 - }, - { - "x": 3.8617845298582507, - "y": 1.1667980667810436, - "heading": -0.2866248120410035, - "angularVelocity": 5.162023862741612e-7, - "velocityX": 3.672597928386812, - "velocityY": -0.9028904945231835, - "timestamp": 1.6590534607326575 - }, - { - "x": 4.0981679647792335, - "y": 1.1267270015307347, - "heading": -0.28662478005254766, - "angularVelocity": 5.045923416827531e-7, - "velocityX": 3.728759884544863, - "velocityY": -0.6320890492444092, - "timestamp": 1.7224481126384439 - }, - { - "x": 4.3358126512762265, - "y": 1.0949807955483843, - "heading": -0.28662474820722456, - "angularVelocity": 5.023345368252209e-7, - "velocityX": 3.7486551207847545, - "velocityY": -0.5007710434238176, - "timestamp": 1.7858427645442303 - }, - { - "x": 4.573457419312962, - "y": 1.06323519995946, - "heading": -0.28662471636190573, - "angularVelocity": 5.023344693474633e-7, - "velocityX": 3.7486564070090496, - "velocityY": -0.5007614149550472, - "timestamp": 1.8492374164500167 - }, - { - "x": 4.811102187354235, - "y": 1.0314896044045172, - "heading": -0.28662468451658685, - "angularVelocity": 5.02334469511756e-7, - "velocityX": 3.748656407080655, - "velocityY": -0.5007614144190154, - "timestamp": 1.9126320683558031 - }, - { - "x": 5.04874695539551, - "y": 0.9997440088495764, - "heading": -0.286624652671268, - "angularVelocity": 5.023344701827313e-7, - "velocityX": 3.748656407080659, - "velocityY": -0.5007614144189856, - "timestamp": 1.9760267202615895 - }, - { - "x": 5.286391723436784, - "y": 0.9679984132946354, - "heading": -0.2866246208259492, - "angularVelocity": 5.023344694262712e-7, - "velocityX": 3.7486564070806585, - "velocityY": -0.500761414418986, - "timestamp": 2.039421372167376 - }, - { - "x": 5.524036491478058, - "y": 0.9362528177396949, - "heading": -0.28662458898063026, - "angularVelocity": 5.023344704301764e-7, - "velocityX": 3.74865640708066, - "velocityY": -0.5007614144189806, - "timestamp": 2.1028160240731624 - }, - { - "x": 5.761681259520114, - "y": 0.9045072221906024, - "heading": -0.2866245571353113, - "angularVelocity": 5.02334470827004e-7, - "velocityX": 3.748656407092983, - "velocityY": -0.5007614143267314, - "timestamp": 2.166210675978949 - }, - { - "x": 5.99932604159473, - "y": 0.8727617316882137, - "heading": -0.28662452528999255, - "angularVelocity": 5.023344683092169e-7, - "velocityX": 3.748656628445413, - "velocityY": -0.5007597572988834, - "timestamp": 2.229605327884735 - }, - { - "x": 6.237212534637086, - "y": 0.8428813906316717, - "heading": -0.286624491558187, - "angularVelocity": 5.320922916664829e-7, - "velocityX": 3.7524694259050357, - "velocityY": -0.47133851450037906, - "timestamp": 2.2929999797905216 - }, - { - "x": 6.4638245505635705, - "y": 0.8220107849523657, - "heading": -0.24424436180847475, - "angularVelocity": 0.6685126974542769, - "velocityX": 3.5746235544169, - "velocityY": -0.32921713507194117, - "timestamp": 2.356394631696308 - }, - { - "x": 6.673647733933235, - "y": 0.8041360764949451, - "heading": -0.18514255243378552, - "angularVelocity": 0.9322838377994884, - "velocityX": 3.309793130207448, - "velocityY": -0.2819592492436893, - "timestamp": 2.4197892836020944 - }, - { - "x": 6.866141134149738, - "y": 0.7888964331390943, - "heading": -0.12766912670980685, - "angularVelocity": 0.9065973863125305, - "velocityX": 3.0364296424022634, - "velocityY": -0.24039320191392657, - "timestamp": 2.483183935507881 - }, - { - "x": 7.04134978279503, - "y": 0.7761674362974819, - "heading": -0.07636429138531158, - "angularVelocity": 0.8092927996630003, - "velocityX": 2.7637764918352543, - "velocityY": -0.20078975842520036, - "timestamp": 2.5465785874136673 - }, - { - "x": 7.19932046714385, - "y": 0.765888430525111, - "heading": -0.03338684229285238, - "angularVelocity": 0.6779349330023905, - "velocityX": 2.4918613731579065, - "velocityY": -0.16214310613530766, - "timestamp": 2.6099732393194537 - }, - { - "x": 7.340087890625, - "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": 0.5266507708326882, - "velocityX": 2.220493673351972, - "velocityY": -0.12406863156475333, - "timestamp": 2.67336789122524 - }, - { - "x": 7.464023522460179, - "y": 0.7525372143128384, - "heading": 0.023010119360387892, - "angularVelocity": 0.36178963616626764, - "velocityX": 1.9486481772395672, - "velocityY": -0.08625561847872552, - "timestamp": 2.736968716290925 - }, - { - "x": 7.57058231953554, - "y": 0.7491648354718851, - "heading": 0.03698370022131657, - "angularVelocity": 0.21970754068830903, - "velocityX": 1.6754310492876554, - "velocityY": -0.05302413667543652, - "timestamp": 2.8005695413566096 - }, - { - "x": 7.659708944521439, - "y": 0.7476928551810322, - "heading": 0.04295335113137195, - "angularVelocity": 0.09386121805637215, - "velocityX": 1.4013438488235506, - "velocityY": -0.02314404395434409, - "timestamp": 2.8641703664222944 - }, - { - "x": 7.731365798341117, - "y": 0.7479588271083956, - "heading": 0.041693436231508264, - "angularVelocity": -0.019809725716020507, - "velocityX": 1.1266654755763426, - "velocityY": 0.004181894292859409, - "timestamp": 2.927771191487979 - }, - { - "x": 7.785525986892143, - "y": 0.7498349370538775, - "heading": 0.03380693253041854, - "angularVelocity": -0.1240000219013676, - "velocityX": 0.8515642445690118, - "velocityY": 0.029498201376229376, - "timestamp": 2.991372016553664 - }, - { - "x": 7.822169492500932, - "y": 0.7532180353995639, - "heading": 0.01977708435408761, - "angularVelocity": -0.22059223542841888, - "velocityX": 0.5761482743493486, - "velocityY": 0.05319268016086978, - "timestamp": 3.0549728416193487 - }, - { - "x": 7.841280937194824, - "y": 0.7580231428146362, - "heading": 2.046055081284657e-30, - "angularVelocity": -0.310956411865142, - "velocityX": 0.30049051524338544, - "velocityY": 0.07555102327854848, - "timestamp": 3.1185736666850334 - }, - { - "x": 7.837626456422163, - "y": 0.7661832239405314, - "heading": -0.033360531548023535, - "angularVelocity": -0.41786111882102345, - "velocityX": -0.045774613098587845, - "velocityY": 0.10221002096529477, - "timestamp": 3.1984100765480727 - }, - { - "x": 7.806322741144082, - "y": 0.7764707746744537, - "heading": -0.07501113915617402, - "angularVelocity": -0.5216994060680163, - "velocityX": -0.3920982335225698, - "velocityY": 0.12885788265743264, - "timestamp": 3.278246486411112 - }, - { - "x": 7.74736425619444, - "y": 0.7888847549403224, - "heading": -0.12465356813987538, - "angularVelocity": -0.6218018704606533, - "velocityX": -0.738491185297363, - "velocityY": 0.15549271675874554, - "timestamp": 3.3580828962741514 - }, - { - "x": 7.660744366778727, - "y": 0.8034239689194149, - "heading": -0.18191677738707485, - "angularVelocity": -0.7172568173523243, - "velocityX": -1.084967241942757, - "velocityY": 0.18211257249711932, - "timestamp": 3.4379193061371907 - }, - { - "x": 7.5464550145644145, - "y": 0.8200870575241468, - "heading": -0.24632578527266594, - "angularVelocity": -0.8067623280666757, - "velocityX": -1.4315442341455176, - "velocityY": 0.20871540482992282, - "timestamp": 3.51775571600023 - }, - { - "x": 7.404486266909035, - "y": 0.8388724767291542, - "heading": -0.3172478637527482, - "angularVelocity": -0.8883425319569115, - "velocityX": -1.7782456387872365, - "velocityY": 0.23529889729803788, - "timestamp": 3.5975921258632693 - }, - { - "x": 7.234825726560123, - "y": 0.8597784078771571, - "heading": -0.3937913075224082, - "angularVelocity": -0.958753579989022, - "velocityX": -2.1251023266197273, - "velocityY": 0.2618596099682753, - "timestamp": 3.6774285357263086 - }, - { - "x": 7.037458048630302, - "y": 0.8828023770067691, - "heading": -0.474592846440749, - "angularVelocity": -1.0120888333650895, - "velocityX": -2.472151218578179, - "velocityY": 0.28838933475478196, - "timestamp": 3.757264945589348 - }, - { - "x": 6.812366694532574, - "y": 0.9079395818644894, - "heading": -0.5572950195818104, - "angularVelocity": -1.0358954427301792, - "velocityX": -2.8194072664825045, - "velocityY": 0.3148589083707988, - "timestamp": 3.837101355452387 - }, - { - "x": 6.559557151686243, - "y": 0.9351738572850871, - "heading": -0.6368741954392075, - "angularVelocity": -0.996777986308708, - "velocityX": -3.1665945810943685, - "velocityY": 0.3411260033776375, - "timestamp": 3.9169377653154265 - }, - { - "x": 6.2794552371906684, - "y": 0.9644007497338529, - "heading": -0.6963848347023115, - "angularVelocity": -0.7454072567290441, - "velocityX": -3.5084482753682624, - "velocityY": 0.3660847537972336, - "timestamp": 3.996774175178466 - }, - { - "x": 5.978414020607044, - "y": 0.9876514129078968, - "heading": -0.6963848502753711, - "angularVelocity": -1.950621226343756e-7, - "velocityX": -3.770725876828189, - "velocityY": 0.2912288167006853, - "timestamp": 4.0766105850415055 - }, - { - "x": 5.677372687056552, - "y": 1.010900561597095, - "heading": -0.6963848658464019, - "angularVelocity": -1.9503671222417687e-7, - "velocityX": -3.770727341909935, - "velocityY": 0.29120984684910667, - "timestamp": 4.156446994904545 - }, - { - "x": 5.376331353502739, - "y": 1.034149710243266, - "heading": -0.6963848814174328, - "angularVelocity": -1.9503671215554748e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.2912098463101618, - "timestamp": 4.236283404767585 - }, - { - "x": 5.075290019948924, - "y": 1.0573988588894354, - "heading": -0.6963848969884637, - "angularVelocity": -1.9503671305455908e-7, - "velocityX": -3.770727341951559, - "velocityY": 0.2912098463101465, - "timestamp": 4.316119814630625 - }, - { - "x": 4.774248686395111, - "y": 1.0806480075356062, - "heading": -0.6963849125594946, - "angularVelocity": -1.95036712978891e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.29120984631016333, - "timestamp": 4.3959562244936645 - }, - { - "x": 4.473207352844907, - "y": 1.1038971562285251, - "heading": -0.6963849281305255, - "angularVelocity": -1.9503671242042344e-7, - "velocityX": -3.770727341906336, - "velocityY": 0.29120984689571133, - "timestamp": 4.475792634356704 - }, - { - "x": 4.172166146375442, - "y": 1.1271479503662634, - "heading": -0.6963849437015556, - "angularVelocity": -1.9503670063864683e-7, - "velocityX": -3.770725750142127, - "velocityY": 0.2912304571012814, - "timestamp": 4.555629044219744 - }, - { - "x": 3.8735860999949274, - "y": 1.1720518831992321, - "heading": -0.696384959370356, - "angularVelocity": -1.9626133629188135e-7, - "velocityX": -3.7398982105123864, - "velocityY": 0.5624492998871312, - "timestamp": 4.635465454082784 - }, - { - "x": 3.580403442801602, - "y": 1.244234542261993, - "heading": -0.6963849756495779, - "angularVelocity": -2.0390723854783083e-7, - "velocityX": -3.672292600535076, - "velocityY": 0.9041320769131684, - "timestamp": 4.7153018639458235 - }, - { - "x": 3.295105029858622, - "y": 1.3430840304472003, - "heading": -0.6963849932202171, - "angularVelocity": -2.2008303364523734e-7, - "velocityX": -3.573537605616451, - "velocityY": 1.2381504673717785, - "timestamp": 4.795138273808863 - }, - { - "x": 3.020110821637561, - "y": 1.4677619739384067, - "heading": -0.6963850130039023, - "angularVelocity": -2.4780279138256264e-7, - "velocityX": -3.4444711215448884, - "velocityY": 1.5616677115753714, - "timestamp": 4.874974683671903 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.6963850355061774, - "angularVelocity": -2.8185479639389566e-7, - "velocityX": -3.2861879673103584, - "velocityY": 1.8719390230438342, - "timestamp": 4.954811093534943 - }, - { - "x": 2.5741841985169076, - "y": 1.73878733799422, - "heading": -0.6963850589279512, - "angularVelocity": -4.0231077616908087e-7, - "velocityX": -3.153128257888759, - "velocityY": 2.0882929565548642, - "timestamp": 5.0130292054104135 - }, - { - "x": 2.3991894720533877, - "y": 1.872411165939459, - "heading": -0.6963850794155536, - "angularVelocity": -3.519111451084776e-7, - "velocityX": -3.0058468202788267, - "velocityY": 2.2952277846293376, - "timestamp": 5.071247317285884 - }, - { - "x": 2.2335584908852413, - "y": 2.017479635272907, - "heading": -0.6963850978369928, - "angularVelocity": -3.164211012595542e-7, - "velocityX": -2.845007779064253, - "velocityY": 2.491809930967045, - "timestamp": 5.129465429161355 - }, - { - "x": 2.0780383093475274, - "y": 2.173338406648706, - "heading": -0.6963851148088384, - "angularVelocity": -2.9152174806156023e-7, - "velocityX": -2.6713367460348776, - "velocityY": 2.6771526309404154, - "timestamp": 5.187683541036826 - }, - { - "x": 1.9333303562340614, - "y": 2.339284459115452, - "heading": -0.6963851307944149, - "angularVelocity": -2.745808111537906e-7, - "velocityX": -2.4856174213103777, - "velocityY": 2.850419691069807, - "timestamp": 5.245901652912297 - }, - { - "x": 1.8000872249814166, - "y": 2.514569234135199, - "heading": -0.6963851461633945, - "angularVelocity": -2.639896616028141e-7, - "velocityX": -2.288688639330214, - "velocityY": 3.0108289220145816, - "timestamp": 5.304119764787767 - }, - { - "x": 1.6789094155739503, - "y": 2.6984018293834224, - "heading": -0.6963851612308567, - "angularVelocity": -2.588105615203023e-7, - "velocityX": -2.0814451981312545, - "velocityY": 3.1576529936498847, - "timestamp": 5.362337876663238 - }, - { - "x": 1.5665736982883989, - "y": 2.8877670089616614, - "heading": -0.6963851762457395, - "angularVelocity": -2.5790741626665006e-7, - "velocityX": -1.9295664814042677, - "velocityY": 3.2526850060560912, - "timestamp": 5.420555988538709 - }, - { - "x": 1.4540456075762054, - "y": 3.0767463777174253, - "heading": -0.6971250426595835, - "angularVelocity": -0.012708526436355243, - "velocityX": -1.9328708384238367, - "velocityY": 3.246058016446739, - "timestamp": 5.47877410041418 - }, - { - "x": 1.3486317039814464, - "y": 3.2544802087721734, - "heading": -0.7253513181524348, - "angularVelocity": -0.48483666995638414, - "velocityX": -1.8106719747325546, - "velocityY": 3.052895831368142, - "timestamp": 5.53699221228965 - }, - { - "x": 1.2507109072486164, - "y": 3.4195717896549707, - "heading": -0.7601787682651484, - "angularVelocity": -0.598223628193407, - "velocityX": -1.6819644879978937, - "velocityY": 2.8357426162485435, - "timestamp": 5.595210324165121 - }, - { - "x": 1.160328320301069, - "y": 3.57195134860905, - "heading": -0.7969704460064531, - "angularVelocity": -0.6319627441714823, - "velocityX": -1.5524822780387875, - "velocityY": 2.617390946652855, - "timestamp": 5.653428436040592 - }, - { - "x": 1.0774888591394964, - "y": 3.71161238889488, - "heading": -0.8336344675120992, - "angularVelocity": -0.6297700204374731, - "velocityX": -1.422915626991949, - "velocityY": 2.398927683957996, - "timestamp": 5.711646547916063 - }, - { - "x": 1.0021908033919844, - "y": 3.838558158350536, - "heading": -0.8689739346571361, - "angularVelocity": -0.6070184347549549, - "velocityX": -1.2933785264038766, - "velocityY": 2.1805202086799973, - "timestamp": 5.7698646597915335 - }, - { - "x": 0.9344313462846833, - "y": 3.952793536595294, - "heading": -0.9022118432658017, - "angularVelocity": -0.5709204152783613, - "velocityX": -1.1638896371672067, - "velocityY": 1.9621965495739466, - "timestamp": 5.828082771667004 - }, - { - "x": 0.8742077702690397, - "y": 4.05432328906152, - "heading": -0.932802055793642, - "angularVelocity": -0.5254415085338586, - "velocityX": -1.0344474266781911, - "velocityY": 1.74395474527581, - "timestamp": 5.886300883542475 - }, - { - "x": 0.8215176865940929, - "y": 4.143151679881797, - "heading": -0.9603390876580661, - "angularVelocity": -0.47299768022924404, - "velocityX": -0.9050462472512313, - "velocityY": 1.5257861850670003, - "timestamp": 5.944518995417946 - }, - { - "x": 0.7763590444591775, - "y": 4.21928243186123, - "heading": -0.9845096003709082, - "angularVelocity": -0.4151717040316119, - "velocityX": -0.7756802939866984, - "velocityY": 1.307681570681621, - "timestamp": 6.002737107293417 - }, - { - "x": 0.7387300854642734, - "y": 4.282718776761074, - "heading": -1.0050639989905084, - "angularVelocity": -0.35305848914451915, - "velocityX": -0.6463445443815323, - "velocityY": 1.0896324675649904, - "timestamp": 6.060955219168887 - }, - { - "x": 0.7086292911290722, - "y": 4.333463522832413, - "heading": -1.0217986916878445, - "angularVelocity": -0.28744822115035373, - "velocityX": -0.5170348773863939, - "velocityY": 0.871631601173907, - "timestamp": 6.119173331044358 - }, - { - "x": 0.6860553360496396, - "y": 4.371519118466469, - "heading": -1.0345444282293665, - "angularVelocity": -0.21893077825652071, - "velocityX": -0.3877479765698806, - "velocityY": 0.6536727902728674, - "timestamp": 6.177391442919829 - }, - { - "x": 0.6710070489868055, - "y": 4.396887706764773, - "heading": -1.0431583071392123, - "angularVelocity": -0.14795874741302198, - "velocityX": -0.2584811938769611, - "velocityY": 0.43575079096636016, - "timestamp": 6.2356095547953 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -0.07488723824993972, - "velocityX": -0.12923242394972312, - "velocityY": 0.2178611369128848, - "timestamp": 6.29382766667077 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -7.364462273467936e-32, - "velocityX": -8.426727085890477e-35, - "velocityY": -7.327805300908389e-34, - "timestamp": 6.352045778546241 - } - ] + "samples": [ + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -7.591443414914956e-27, + "velocityX": -1.3210318643006072e-26, + "velocityY": -4.725695657383892e-27, + "timestamp": 0 + }, + { + "x": 0.6705320996373237, + "y": 4.398099342072708, + "heading": -1.0386114123729513, + "angularVelocity": 0.159275518837918, + "velocityX": 0.1260500229589734, + "velocityY": -0.20514712040522684, + "timestamp": 0.055920008584652126 + }, + { + "x": 0.6846335257451809, + "y": 4.375148415264061, + "heading": -1.0210111037335994, + "angularVelocity": 0.3147408071783192, + "velocityX": 0.25217138667835376, + "velocityY": -0.41042423614624113, + "timestamp": 0.11184001716930425 + }, + { + "x": 0.7057920858570316, + "y": 4.3407101501549405, + "heading": -0.9949634713965649, + "angularVelocity": 0.4658016512569599, + "velocityX": 0.37837190385657166, + "velocityY": -0.6158487092681121, + "timestamp": 0.16776002575395638 + }, + { + "x": 0.7340126704011022, + "y": 4.2947751012827355, + "heading": -0.9607567060719514, + "angularVelocity": 0.6117088711249232, + "velocityX": 0.5046598750311373, + "velocityY": -0.8214420926396748, + "timestamp": 0.2236800343386085 + }, + { + "x": 0.769300659799434, + "y": 4.23733236505955, + "heading": -0.9187315683545035, + "angularVelocity": 0.7515223759994586, + "velocityX": 0.6310440626079745, + "velocityY": -1.0272304614586787, + "timestamp": 0.2796000429232606 + }, + { + "x": 0.8116619603260306, + "y": 4.168369326160844, + "heading": -0.8692948285945653, + "angularVelocity": 0.8840617340947026, + "velocityX": 0.7575338702330436, + "velocityY": -1.233244426175458, + "timestamp": 0.33552005150791275 + }, + { + "x": 0.8611030801762559, + "y": 4.087871424951383, + "heading": -0.8129373455124671, + "angularVelocity": 1.0078232194257235, + "velocityX": 0.8841400618775134, + "velocityY": -1.439518756289218, + "timestamp": 0.3914400600925649 + }, + { + "x": 0.9176312887785422, + "y": 3.995821966433115, + "heading": -0.7502611047615702, + "angularVelocity": 1.1208195838528383, + "velocityX": 1.0108762504339304, + "velocityY": -1.646091637824456, + "timestamp": 0.447360068677217 + }, + { + "x": 0.9812548753941115, + "y": 3.892201995095196, + "heading": -0.6820246179563085, + "angularVelocity": 1.2202517226363587, + "velocityX": 1.1377606732526766, + "velocityY": -1.8530034948234757, + "timestamp": 0.5032800772618691 + }, + { + "x": 1.051983359986124, + "y": 3.776990337772613, + "heading": -0.6092266874108444, + "angularVelocity": 1.3018225924493927, + "velocityX": 1.2648153385910723, + "velocityY": -2.060293984901216, + "timestamp": 0.5592000858465211 + }, + { + "x": 1.1298269062475699, + "y": 3.6501643769794776, + "heading": -0.5332724007265813, + "angularVelocity": 1.358266720744203, + "velocityX": 1.3920517580680556, + "velocityY": -2.267988936395559, + "timestamp": 0.6151200944311732 + }, + { + "x": 1.2147919527166744, + "y": 3.511704398620439, + "heading": -0.4563270080164221, + "angularVelocity": 1.375990359402014, + "velocityX": 1.5194033159076472, + "velocityY": -2.476036428882822, + "timestamp": 0.6710401030158253 + }, + { + "x": 1.306860165723401, + "y": 3.3616165918639007, + "heading": -0.3821550436425617, + "angularVelocity": 1.3263940090706232, + "velocityX": 1.6464270184678684, + "velocityY": -2.6839732424098903, + "timestamp": 0.7269601116004774 + }, + { + "x": 1.4058792683735797, + "y": 3.2000739453958573, + "heading": -0.3184323444941022, + "angularVelocity": 1.1395330716374203, + "velocityX": 1.7707276010210737, + "velocityY": -2.888816553443462, + "timestamp": 0.7828801201851294 + }, + { + "x": 1.51216916955334, + "y": 3.030074948039413, + "heading": -0.28662540722206215, + "angularVelocity": 0.5687934976599732, + "velocityX": 1.9007490139930476, + "velocityY": -3.040038827946505, + "timestamp": 0.8388001287697815 + }, + { + "x": 1.6244649894914296, + "y": 2.850864516485835, + "heading": -0.28662530013608456, + "angularVelocity": 0.000001914984999387773, + "velocityX": 2.0081509781618743, + "velocityY": -3.2047640207760013, + "timestamp": 0.8947201373544336 + }, + { + "x": 1.7352882065392865, + "y": 2.6707396868109132, + "heading": -0.2866252659484935, + "angularVelocity": 6.113659837473025e-7, + "velocityX": 1.9818168818785473, + "velocityY": -3.221115916000747, + "timestamp": 0.9506401459390856 + }, + { + "x": 1.8506362599745876, + "y": 2.4934783930913484, + "heading": -0.28662523174622423, + "angularVelocity": 6.116284691333173e-7, + "velocityX": 2.06273311386722, + "velocityY": -3.169908199338448, + "timestamp": 1.0065601545237377 + }, + { + "x": 1.977172078744517, + "y": 2.3240221270116197, + "heading": -0.2866251971562196, + "angularVelocity": 6.18562220740454e-7, + "velocityX": 2.262800417463786, + "velocityY": -3.0303333345023407, + "timestamp": 1.0624801631083898 + }, + { + "x": 2.11437011822214, + "y": 2.163076679089509, + "heading": -0.2866251615150111, + "angularVelocity": 6.373605678250823e-7, + "velocityX": 2.4534695710915724, + "velocityY": -2.8781370388824357, + "timestamp": 1.1184001716930418 + }, + { + "x": 2.2616596071687796, + "y": 2.011311909775525, + "heading": -0.28662512406592494, + "angularVelocity": 6.696902799491419e-7, + "velocityX": 2.633931801417216, + "velocityY": -2.7139618386188316, + "timestamp": 1.174320180277694 + }, + { + "x": 2.4184276697644314, + "y": 1.869359408517574, + "heading": -0.2866250838843977, + "angularVelocity": 7.185536662845936e-7, + "velocityX": 2.803434165399588, + "velocityY": -2.5384921220650836, + "timestamp": 1.230240188862346 + }, + { + "x": 2.5840219555188257, + "y": 1.7378099135158744, + "heading": -0.2866250397673314, + "angularVelocity": 7.889316803983354e-7, + "velocityX": 2.9612707498732993, + "velocityY": -2.352458419289386, + "timestamp": 1.286160197446998 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.2866249892310852, + "angularVelocity": 9.037238633691851e-7, + "velocityX": 3.1067845136425762, + "velocityY": -2.1566350139692623, + "timestamp": 1.3420802060316501 + }, + { + "x": 2.9641720225317068, + "y": 1.495251218591247, + "heading": -0.2866249436127499, + "angularVelocity": 7.195928005652006e-7, + "velocityX": 3.256089327481367, + "velocityY": -1.9238160043664425, + "timestamp": 1.4054748581983314 + }, + { + "x": 3.1789518158633587, + "y": 1.3887033119472996, + "heading": -0.28662490328506157, + "angularVelocity": 6.361370710101161e-7, + "velocityX": 3.3879796795310004, + "velocityY": -1.6807081197291671, + "timestamp": 1.4688695103650127 + }, + { + "x": 3.4009440542565814, + "y": 1.2981369651359904, + "heading": -0.2866248665990377, + "angularVelocity": 5.78692723773973e-7, + "velocityX": 3.501750239271706, + "velocityY": -1.4286117790059998, + "timestamp": 1.532264162531694 + }, + { + "x": 3.628961462551479, + "y": 1.2240364974062796, + "heading": -0.28662483235864045, + "angularVelocity": 5.401149161418495e-7, + "velocityX": 3.596792481728267, + "velocityY": -1.168875688991578, + "timestamp": 1.5956588146983752 + }, + { + "x": 3.8617845297574473, + "y": 1.16679806946556, + "heading": -0.28662479963417076, + "angularVelocity": 5.162023699881254e-7, + "velocityX": 3.672597912420367, + "velocityY": -0.9028904802604593, + "timestamp": 1.6590534668650565 + }, + { + "x": 4.098167964587261, + "y": 1.1267270049464149, + "heading": -0.286624767645716, + "angularVelocity": 5.045923223989343e-7, + "velocityX": 3.7287598677613913, + "velocityY": -0.6320890351095811, + "timestamp": 1.7224481190317378 + }, + { + "x": 4.335812648113985, + "y": 1.0949807783310654, + "heading": -0.28662473580039416, + "angularVelocity": 5.023345147080502e-7, + "velocityX": 3.7486550585038954, + "velocityY": -0.500771366832023, + "timestamp": 1.785842771198419 + }, + { + "x": 4.573457416127468, + "y": 1.063235184169841, + "heading": -0.28662470395507683, + "angularVelocity": 5.023344436089823e-7, + "velocityX": 3.74865639121502, + "velocityY": -0.5007613903733815, + "timestamp": 1.8492374233651003 + }, + { + "x": 4.811102184145877, + "y": 1.031489590045499, + "heading": -0.2866246721097595, + "angularVelocity": 5.023344446091564e-7, + "velocityX": 3.748656391292737, + "velocityY": -0.5007613897915972, + "timestamp": 1.9126320755317816 + }, + { + "x": 5.048746952164287, + "y": 0.9997439959211589, + "heading": -0.2866246402644421, + "angularVelocity": 5.023344444502557e-7, + "velocityX": 3.7486563912927418, + "velocityY": -0.5007613897915636, + "timestamp": 1.9760267276984629 + }, + { + "x": 5.2863917201826975, + "y": 0.9679984017968188, + "heading": -0.2866246084191247, + "angularVelocity": 5.023344456533015e-7, + "velocityX": 3.748656391292741, + "velocityY": -0.5007613897915631, + "timestamp": 2.039421379865144 + }, + { + "x": 5.524036488201108, + "y": 0.9362528076724792, + "heading": -0.2866245765738072, + "angularVelocity": 5.023344456596004e-7, + "velocityX": 3.748656391292742, + "velocityY": -0.5007613897915576, + "timestamp": 2.1028160320318254 + }, + { + "x": 5.761681256220374, + "y": 0.904507213554553, + "heading": -0.28662454472848975, + "angularVelocity": 5.023344455217454e-7, + "velocityX": 3.7486563913062563, + "velocityY": -0.500761389690393, + "timestamp": 2.1662106841985067 + }, + { + "x": 5.999326038930599, + "y": 0.8727617294120621, + "heading": -0.28662451288317237, + "angularVelocity": 5.023344437091617e-7, + "velocityX": 3.748656623044388, + "velocityY": -0.5007596549157438, + "timestamp": 2.229605336365188 + }, + { + "x": 6.23721253164048, + "y": 0.8428813874103429, + "heading": -0.28662447915136585, + "angularVelocity": 5.320923033369603e-7, + "velocityX": 3.7524694052175755, + "velocityY": -0.4713385274700439, + "timestamp": 2.2929999885318693 + }, + { + "x": 6.46382454823366, + "y": 0.8220107822516065, + "heading": -0.24424435205194292, + "angularVelocity": 0.6685126528968796, + "velocityX": 3.57462355022247, + "velocityY": -0.32921712550550913, + "timestamp": 2.3563946406985505 + }, + { + "x": 6.673647732212354, + "y": 0.804136074402253, + "heading": -0.1851425455819509, + "angularVelocity": 0.932283788143483, + "velocityX": 3.3097931261932545, + "velocityY": -0.28195923849154, + "timestamp": 2.419789292865232 + }, + { + "x": 6.866141132976172, + "y": 0.7888964316418866, + "heading": -0.12766912233999997, + "angularVelocity": 0.9065973434295085, + "velocityX": 3.036429638539596, + "velocityY": -0.24039319153132044, + "timestamp": 2.483183945031913 + }, + { + "x": 7.0413497820944775, + "y": 0.7761674353552135, + "heading": -0.07636428899971823, + "angularVelocity": 0.8092927650330481, + "velocityX": 2.763776487922594, + "velocityY": -0.2007897488451438, + "timestamp": 2.5465785971985944 + }, + { + "x": 7.199320466835695, + "y": 0.7658884300843406, + "heading": -0.03338684136743644, + "angularVelocity": 0.6779349071792803, + "velocityX": 2.4918613690926352, + "velocityY": -0.16214309755729506, + "timestamp": 2.6099732493652756 + }, + { + "x": 7.340087890625, + "y": 0.7580231428146362, + "heading": 2.6436628934070276e-25, + "angularVelocity": 0.5266507540676091, + "velocityX": 2.2204936690746413, + "velocityY": -0.1240686241013575, + "timestamp": 2.673367901531957 + }, + { + "x": 7.46402352318357, + "y": 0.752537214674761, + "heading": 0.023010119033746678, + "angularVelocity": 0.36178962784078056, + "velocityX": 1.9486481714334611, + "velocityY": -0.08625561202772893, + "timestamp": 2.736968727158371 + }, + { + "x": 7.570582320737423, + "y": 0.749164836112825, + "heading": 0.036983699836062915, + "angularVelocity": 0.2197075378297117, + "velocityX": 1.6754310420397782, + "velocityY": -0.05302413182094548, + "timestamp": 2.800569552784785 + }, + { + "x": 7.659708945959918, + "y": 0.7476928559896275, + "heading": 0.042953350833826356, + "angularVelocity": 0.09386121860789494, + "velocityX": 1.4013438401887508, + "velocityY": -0.023144041114242904, + "timestamp": 2.864170378411199 + }, + { + "x": 7.731365799776358, + "y": 0.7479588279518706, + "heading": 0.041693436078494114, + "angularVelocity": -0.019809723268890925, + "velocityX": 1.1266654655923272, + "velocityY": 0.00418189480440736, + "timestamp": 2.927771204037613 + }, + { + "x": 7.785525988085828, + "y": 0.7498349377827495, + "heading": 0.03380693250994457, + "angularVelocity": -0.12400001872419436, + "velocityX": 0.8515642332632949, + "velocityY": 0.029498199314249407, + "timestamp": 2.991372029664027 + }, + { + "x": 7.822169493215899, + "y": 0.7532180358509439, + "heading": 0.01977708439947626, + "angularVelocity": -0.22059223244802648, + "velocityX": 0.5761482617428887, + "velocityY": 0.05319267532887989, + "timestamp": 3.054972855290441 + }, + { + "x": 7.841280937194824, + "y": 0.7580231428146362, + "heading": -3.944931722509772e-25, + "angularVelocity": -0.31095640983727535, + "velocityX": 0.30049050135267197, + "velocityY": 0.07555101551538161, + "timestamp": 3.118573680916855 + }, + { + "x": 7.8376264553511055, + "y": 0.7661832230205181, + "heading": -0.033360531574413245, + "angularVelocity": -0.41786111810448695, + "velocityX": -0.04577462639954243, + "velocityY": 0.10221000918544447, + "timestamp": 3.19841009097995 + }, + { + "x": 7.806322738977973, + "y": 0.7764707725075406, + "heading": -0.07501113928889683, + "angularVelocity": -0.52169940609262, + "velocityX": -0.3920982462562205, + "velocityY": 0.12885786671635327, + "timestamp": 3.2782465010430455 + }, + { + "x": 7.747364252907164, + "y": 0.788884751185466, + "heading": -0.12465356840345063, + "angularVelocity": -0.6218018705415377, + "velocityX": -0.7384911974901414, + "velocityY": 0.15549269647914413, + "timestamp": 3.3580829111061408 + }, + { + "x": 7.660744362341635, + "y": 0.8034239632178009, + "heading": -0.18191677773821885, + "angularVelocity": -0.7172568166518608, + "velocityX": -1.0849672536261623, + "velocityY": 0.1821125476564444, + "timestamp": 3.437919321169236 + }, + { + "x": 7.5464550089458, + "y": 0.820087049493991, + "heading": -0.2463257855831559, + "angularVelocity": -0.8067623255358589, + "velocityX": -1.4315442453576273, + "velocityY": 0.2087153751405061, + "timestamp": 3.5177557312323313 + }, + { + "x": 7.404486260073429, + "y": 0.8388724659578224, + "heading": -0.3172478637844774, + "angularVelocity": -0.8883425262392362, + "velocityX": -1.7782456495748211, + "velocityY": 0.23529886237351122, + "timestamp": 3.5975921412954266 + }, + { + "x": 7.234825718467376, + "y": 0.8597783939083566, + "heading": -0.39379130688880964, + "angularVelocity": -0.9587535692529209, + "velocityX": -2.125102337041081, + "velocityY": 0.26185956926184595, + "timestamp": 3.677428551358522 + }, + { + "x": 7.037458039234421, + "y": 0.8828023593175933, + "heading": -0.4745928445431507, + "angularVelocity": -1.0120888149966063, + "velocityX": -2.4721512287059744, + "velocityY": 0.2883892874321469, + "timestamp": 3.757264961421617 + }, + { + "x": 6.812366683780793, + "y": 0.9079395598178155, + "heading": -0.5572950154989933, + "angularVelocity": -1.035895412763206, + "velocityX": -2.819407276401054, + "velocityY": 0.31485885300148303, + "timestamp": 3.8371013714847124 + }, + { + "x": 6.5595571395215275, + "y": 0.9351738300036528, + "heading": -0.6368741877579803, + "angularVelocity": -0.9967779387386686, + "velocityX": -3.16659459085733, + "velocityY": 0.34112593695425386, + "timestamp": 3.9169377815478077 + }, + { + "x": 6.279455223533108, + "y": 0.9644007155460906, + "heading": -0.6963848224506938, + "angularVelocity": -0.7454071976142457, + "velocityX": -3.5084482852755343, + "velocityY": 0.36608466637389636, + "timestamp": 3.996774191610903 + }, + { + "x": 5.978414008635392, + "y": 0.98765139076885, + "heading": -0.6963848380237664, + "angularVelocity": -1.9506228543282429e-7, + "velocityX": -3.7707258462623967, + "velocityY": 0.29122896688846905, + "timestamp": 4.076610601673998 + }, + { + "x": 5.677372675666473, + "y": 1.0109005372081505, + "heading": -0.6963848535947982, + "angularVelocity": -1.9503672258290444e-7, + "velocityX": -3.7707273251766207, + "velocityY": 0.2912098179380391, + "timestamp": 4.156447011737093 + }, + { + "x": 5.376331342694041, + "y": 1.0341496836019548, + "heading": -0.69638486916583, + "angularVelocity": -1.9503672329128174e-7, + "velocityX": -3.7707273252206313, + "velocityY": 0.291209817368169, + "timestamp": 4.236283421800188 + }, + { + "x": 5.075290009721609, + "y": 1.0573988299957575, + "heading": -0.6963848847368618, + "angularVelocity": -1.9503672272562427e-7, + "velocityX": -3.770727325220632, + "velocityY": 0.29120981736815194, + "timestamp": 4.316119831863283 + }, + { + "x": 4.774248676749177, + "y": 1.080647976389562, + "heading": -0.6963849003078936, + "angularVelocity": -1.9503672313945863e-7, + "velocityX": -3.770727325220631, + "velocityY": 0.29120981736817053, + "timestamp": 4.395956241926378 + }, + { + "x": 4.473207343780594, + "y": 1.1038971228332064, + "heading": -0.6963849158789254, + "angularVelocity": -1.950367233647788e-7, + "velocityX": -3.770727325172418, + "velocityY": 0.2912098179924497, + "timestamp": 4.475792651989472 + }, + { + "x": 4.1721661401549985, + "y": 1.1271479440124157, + "heading": -0.6963849314499564, + "angularVelocity": -1.9503671206893558e-7, + "velocityX": -3.77072570507217, + "velocityY": 0.2912307950825182, + "timestamp": 4.555629062052567 + }, + { + "x": 3.8735860947396503, + "y": 1.1720518781992464, + "heading": -0.6963849471187581, + "angularVelocity": -1.9626135080534044e-7, + "velocityX": -3.739898189051575, + "velocityY": 0.5624493154356899, + "timestamp": 4.635465472115662 + }, + { + "x": 3.5804034386486023, + "y": 1.2442345385889582, + "heading": -0.6963849633979813, + "angularVelocity": -2.0390725479039878e-7, + "velocityX": -3.6722925775262745, + "velocityY": 0.9041320912684484, + "timestamp": 4.715301882178757 + }, + { + "x": 3.295105026948141, + "y": 1.3430840280599874, + "heading": -0.6963849809686221, + "angularVelocity": -2.2008305287856005e-7, + "velocityX": -3.573537581098501, + "velocityY": 1.238150480374907, + "timestamp": 4.795138292241852 + }, + { + "x": 3.020110820111431, + "y": 1.467761972780782, + "heading": -0.6963850007523091, + "angularVelocity": -2.47802813909356e-7, + "velocityX": -3.444471095573825, + "velocityY": 1.561667723063461, + "timestamp": 4.874974702304947 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.6963850232545865, + "angularVelocity": -2.8185482385848004e-7, + "velocityX": -3.286187939960048, + "velocityY": 1.871939032853051, + "timestamp": 4.954811112368041 + }, + { + "x": 2.5741841996274863, + "y": 1.7387873388514183, + "heading": -0.6963850466763624, + "angularVelocity": -4.023108112528609e-7, + "velocityX": -3.153128229528534, + "velocityY": 2.088292965130013, + "timestamp": 5.013029224414929 + }, + { + "x": 2.399189474356015, + "y": 1.8724111676120014, + "heading": -0.6963850671639665, + "angularVelocity": -3.519111736959287e-7, + "velocityX": -3.0058467909528623, + "velocityY": 2.295227791876272, + "timestamp": 5.071247336461816 + }, + { + "x": 2.233558494460658, + "y": 2.017479637711712, + "heading": -0.6963850855854071, + "angularVelocity": -3.164211270787494e-7, + "velocityX": -2.845007748824996, + "velocityY": 2.4918099367921025, + "timestamp": 5.129465448508704 + }, + { + "x": 2.0780383142751306, + "y": 2.1733384097973816, + "heading": -0.6963851025572542, + "angularVelocity": -2.9152176872861595e-7, + "velocityX": -2.671336714943196, + "velocityY": 2.6771526352511037, + "timestamp": 5.187683560555591 + }, + { + "x": 1.93333036259127, + "y": 2.339284462910256, + "heading": -0.6963851185428317, + "angularVelocity": -2.745808302376555e-7, + "velocityX": -2.4856173894357143, + "velocityY": 2.850419693775459, + "timestamp": 5.2459016726024785 + }, + { + "x": 1.800087232842991, + "y": 2.514569238505031, + "heading": -0.6963851339118123, + "angularVelocity": -2.639896791762275e-7, + "velocityX": -2.2886886067512457, + "velocityY": 3.0108289230266427, + "timestamp": 5.304119784649366 + }, + { + "x": 1.6789094250105836, + "y": 2.698401834249409, + "heading": -0.6963851489792755, + "angularVelocity": -2.5881057704594286e-7, + "velocityX": -2.0814451649482146, + "velocityY": 3.157652992874852, + "timestamp": 5.362337896696253 + }, + { + "x": 1.5665736783823851, + "y": 2.8877669958946757, + "heading": -0.6963851639941595, + "angularVelocity": -2.5790743404475406e-7, + "velocityX": -1.9295669797352, + "velocityY": 3.2526846884480856, + "timestamp": 5.420556008743141 + }, + { + "x": 1.4540455902705487, + "y": 3.076746366139197, + "heading": -0.6971250291266823, + "angularVelocity": -0.012708504389955933, + "velocityX": -1.9328707880669334, + "velocityY": 3.2460580324611277, + "timestamp": 5.478774120790028 + }, + { + "x": 1.348631689048656, + "y": 3.2544801987728227, + "heading": -0.7253513053237864, + "angularVelocity": -0.48483668062563473, + "velocityX": -1.8106719286430082, + "velocityY": 3.0528958494992513, + "timestamp": 5.536992232836916 + }, + { + "x": 1.250710894485043, + "y": 3.4195717811136617, + "heading": -0.7601787566205287, + "angularVelocity": -0.5982236467698062, + "velocityX": -1.681964445785358, + "velocityY": 2.8357426329434743, + "timestamp": 5.595210344883803 + }, + { + "x": 1.1603283095237442, + "y": 3.5719513414037465, + "heading": -0.7969704357290219, + "angularVelocity": -0.6319627657946384, + "velocityX": -1.5524822393502977, + "velocityY": 2.617390961894501, + "timestamp": 5.6534284569306905 + }, + { + "x": 1.0774888501734665, + "y": 3.7116123829065053, + "heading": -0.8336344586576223, + "angularVelocity": -0.6297700430249606, + "velocityX": -1.4229155916901037, + "velocityY": 2.398927697797529, + "timestamp": 5.711646568977578 + }, + { + "x": 1.0021907960664256, + "y": 3.838558153462584, + "heading": -0.8689739272109267, + "angularVelocity": -0.6070184571571581, + "velocityX": -1.2933784944176396, + "velocityY": 2.1805202211614, + "timestamp": 5.769864681024465 + }, + { + "x": 0.9344313404312847, + "y": 3.9527935326932564, + "heading": -0.9022118371683866, + "angularVelocity": -0.5709204367652957, + "velocityX": -1.1638896084532742, + "velocityY": 1.962196560731307, + "timestamp": 5.828082793071353 + }, + { + "x": 0.8742077657211834, + "y": 4.054323286032448, + "heading": -0.9328020509544388, + "angularVelocity": -0.5254415285987893, + "velocityX": -1.0344474012073521, + "velocityY": 1.7439547551356767, + "timestamp": 5.88630090511824 + }, + { + "x": 0.8215176831863799, + "y": 4.143151677613985, + "heading": -0.9603390839635252, + "angularVelocity": -0.47299769849816853, + "velocityX": -0.9050462250024212, + "velocityY": 1.5257861936504657, + "timestamp": 5.944519017165128 + }, + { + "x": 0.7763590420271295, + "y": 4.219282430243976, + "heading": -0.9845095976897603, + "angularVelocity": -0.4151717202160187, + "velocityX": -0.7756802749439998, + "velocityY": 1.3076815780057875, + "timestamp": 6.002737129212015 + }, + { + "x": 0.7387300838441316, + "y": 4.282718775684509, + "heading": -1.0050639971773583, + "angularVelocity": -0.3530585030143863, + "velocityX": -0.6463445285325005, + "velocityY": 1.0896324736439786, + "timestamp": 6.0609552412589025 + }, + { + "x": 0.7086292901576546, + "y": 4.3334635221873645, + "heading": -1.0217986905857463, + "angularVelocity": -0.2874482325175743, + "velocityX": -0.517034864721044, + "velocityY": 0.8716316060195561, + "timestamp": 6.11917335330579 + }, + { + "x": 0.6860553355642359, + "y": 4.371519118144359, + "heading": -1.0345444276717368, + "angularVelocity": -0.21893078696412344, + "velocityX": -0.3877479670800402, + "velocityY": 0.6536727938952351, + "timestamp": 6.177391465352677 + }, + { + "x": 0.6710070488250978, + "y": 4.396887706657532, + "heading": -1.0431583069512933, + "angularVelocity": -0.14795875332781483, + "velocityX": -0.2584811875558354, + "velocityY": 0.435750793374088, + "timestamp": 6.235609577399565 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -0.074887241257288, + "velocityX": -0.12923242079159178, + "velocityY": 0.21786113811346933, + "timestamp": 6.293827689446452 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": 1.6934356345077546e-25, + "velocityX": 7.869408079103222e-27, + "velocityY": 8.248206791285532e-26, + "timestamp": 6.35204580149334 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubGSub.2.traj b/src/main/deploy/choreo/SourceLanePSubHSubGSub.2.traj index 88eac2d5..7e6dd0e0 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubGSub.2.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubGSub.2.traj @@ -1,931 +1,932 @@ { - "samples": [ - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -7.364462273467936e-32, - "velocityX": -8.426727085890477e-35, - "velocityY": -7.327805300908389e-34, - "timestamp": 0 - }, - { - "x": 0.6687542994396359, - "y": 4.396916362187497, - "heading": -1.0350125956919525, - "angularVelocity": 0.2204003026897336, - "velocityX": 0.09289604490221867, - "velocityY": -0.22303166776742134, - "timestamp": 0.05673996319027008 - }, - { - "x": 0.6793565646010321, - "y": 4.371607473302922, - "heading": -1.0104451675948027, - "angularVelocity": 0.4329827993501856, - "velocityX": 0.18685710327027932, - "velocityY": -0.4460504988292779, - "timestamp": 0.11347992638054016 - }, - { - "x": 0.6953596015342451, - "y": 4.333646306767502, - "heading": -0.974331857670356, - "angularVelocity": 0.6364704503481122, - "velocityX": 0.2820417221553139, - "velocityY": -0.6690375601429702, - "timestamp": 0.17021988957081025 - }, - { - "x": 0.7168434967725004, - "y": 4.283036502515157, - "heading": -0.9272707047547418, - "angularVelocity": 0.8294181079709282, - "velocityX": 0.3786378071168583, - "velocityY": -0.8919604703061096, - "timestamp": 0.22695985276108033 - }, - { - "x": 0.7439015517455688, - "y": 4.2197848874550585, - "heading": -0.8699519201955506, - "angularVelocity": 1.0102012996903078, - "velocityX": 0.4768782609592592, - "velocityY": -1.114763061230607, - "timestamp": 0.2836998159513504 - }, - { - "x": 0.7766442528059767, - "y": 4.143903696796417, - "heading": -0.8031709855452435, - "angularVelocity": 1.1769647157923862, - "velocityX": 0.5770659552705253, - "velocityY": -1.3373500156174554, - "timestamp": 0.3404397791416205 - }, - { - "x": 0.8152053931020986, - "y": 4.055414010177326, - "heading": -0.7278520306671347, - "angularVelocity": 1.3274410246890034, - "velocityX": 0.6796116551364678, - "velocityY": -1.559565456931146, - "timestamp": 0.3971797423318906 - }, - { - "x": 0.8597512927978146, - "y": 3.9543509740987406, - "heading": -0.6450962807712558, - "angularVelocity": 1.4585090515192565, - "velocityX": 0.7850886252135366, - "velocityY": -1.7811614670894944, - "timestamp": 0.45391970552216065 - }, - { - "x": 0.9104943772436287, - "y": 3.840772171273386, - "heading": -0.5562797497368643, - "angularVelocity": 1.5653258486713548, - "velocityX": 0.8943094354089278, - "velocityY": -2.0017426244088754, - "timestamp": 0.5106596687124307 - }, - { - "x": 0.9677131149809225, - "y": 3.7147727983516208, - "heading": -0.46323645073147074, - "angularVelocity": 1.6398195165087535, - "velocityX": 1.0084380482486026, - "velocityY": -2.2206460109824513, - "timestamp": 0.5673996319027008 - }, - { - "x": 1.0317820620609501, - "y": 3.576517638239885, - "heading": -0.3685846112186535, - "angularVelocity": 1.6681688564973882, - "velocityX": 1.1291679352201949, - "velocityY": -2.436645220373414, - "timestamp": 0.6241395950929709 - }, - { - "x": 1.1032183238971194, - "y": 3.4263179326907856, - "heading": -0.2763023679431359, - "angularVelocity": 1.6264064706221368, - "velocityX": 1.2590114236876868, - "velocityY": -2.64715902344566, - "timestamp": 0.680879558283241 - }, - { - "x": 1.1827463587467486, - "y": 3.2648378432524847, - "heading": -0.1927401719055838, - "angularVelocity": 1.4727220699339676, - "velocityX": 1.4016229545821517, - "velocityY": -2.8459674691151595, - "timestamp": 0.7376195214735111 - }, - { - "x": 1.2713975131080564, - "y": 3.0937718159398226, - "heading": -0.12815970051626668, - "angularVelocity": 1.1381831738726118, - "velocityX": 1.562411206789599, - "velocityY": -3.014912553591437, - "timestamp": 0.7943594846637811 - }, - { - "x": 1.3706976001434645, - "y": 2.9168916321975784, - "heading": -0.09351951840137682, - "angularVelocity": 0.610507659279375, - "velocityX": 1.7500907905494714, - "velocityY": -3.117382772158287, - "timestamp": 0.8510994478540512 - }, - { - "x": 1.4825331259415389, - "y": 2.737551336103052, - "heading": -0.084031813825935, - "angularVelocity": 0.1672137950394149, - "velocityX": 1.9710186526390192, - "velocityY": -3.1607404377956887, - "timestamp": 0.9078394110443213 - }, - { - "x": 1.6073609242840539, - "y": 2.563006200971128, - "heading": -0.08403157292142222, - "angularVelocity": 0.000004245764347455466, - "velocityX": 2.199997873173106, - "velocityY": -3.0762292627263235, - "timestamp": 0.9645793742345914 - }, - { - "x": 1.7433400400906298, - "y": 2.3970011200993575, - "heading": -0.08403150231700539, - "angularVelocity": 0.0000012443507689652163, - "velocityX": 2.3965316182984986, - "velocityY": -2.925717105509803, - "timestamp": 1.0213193374248615 - }, - { - "x": 1.889887864003564, - "y": 2.2402473630588187, - "heading": -0.08403142284388228, - "angularVelocity": 0.0000014006551755862827, - "velocityX": 2.5827973032253166, - "velocityY": -2.7626693467333516, - "timestamp": 1.0780593006151316 - }, - { - "x": 2.046376539489404, - "y": 2.0934165181715283, - "heading": -0.08403133019563887, - "angularVelocity": 0.0000016328569530158415, - "velocityX": 2.757997479855154, - "velocityY": -2.58778533914292, - "timestamp": 1.1347992638054016 - }, - { - "x": 2.2121356179273546, - "y": 1.9571376609651308, - "heading": -0.08403121735664319, - "angularVelocity": 0.00000198870406896459, - "velocityX": 2.921381494064391, - "velocityY": -2.401814339381988, - "timestamp": 1.1915392269956717 - }, - { - "x": 2.3864549305613383, - "y": 1.8319946594051495, - "heading": -0.08403107170612155, - "angularVelocity": 0.0000025669830126630976, - "velocityX": 3.0722493077661266, - "velocityY": -2.2055530973879893, - "timestamp": 1.2482791901859418 - }, - { - "x": 2.5685876291674634, - "y": 1.7185236734546225, - "heading": -0.08403086720089675, - "angularVelocity": 0.000003604253744603396, - "velocityX": 3.2099544723948052, - "velocityY": -1.9998424315154402, - "timestamp": 1.3050191533762119 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.08403051576382121, - "angularVelocity": 0.000006193819237400721, - "velocityX": 3.3339066927233554, - "velocityY": -1.7855635206200389, - "timestamp": 1.361759116566482 - }, - { - "x": 2.9656624873675215, - "y": 1.523823440642391, - "heading": -0.08403020563552344, - "angularVelocity": 0.0000051460695851667095, - "velocityX": 3.449910187925088, - "velocityY": -1.5496108793286638, - "timestamp": 1.4220241970612824 - }, - { - "x": 3.179557704009846, - "y": 1.4451070785194118, - "heading": -0.08403001029754076, - "angularVelocity": 0.0000032413128973800796, - "velocityX": 3.5492397070767696, - "velocityY": -1.306168704607808, - "timestamp": 1.4822892775560828 - }, - { - "x": 3.3984052007772028, - "y": 1.3814422389869163, - "heading": -0.0840298717822531, - "angularVelocity": 0.0000022984336290020716, - "velocityX": 3.631414659542976, - "velocityY": -1.0564134156924954, - "timestamp": 1.5425543580508831 - }, - { - "x": 3.6211472313814954, - "y": 1.3331366287340103, - "heading": -0.08402976608446186, - "angularVelocity": 0.000001753881192375512, - "velocityX": 3.6960380501525916, - "velocityY": -0.8015522398094749, - "timestamp": 1.6028194385456835 - }, - { - "x": 3.846707230492903, - "y": 1.3004237167488812, - "heading": -0.08402968105378755, - "angularVelocity": 0.0000014109443416028125, - "velocityX": 3.7427976078264336, - "velocityY": -0.5428170296387713, - "timestamp": 1.6630845190404848 - }, - { - "x": 4.073995015295796, - "y": 1.2834616061839175, - "heading": -0.08402960975207145, - "angularVelocity": 0.0000011831348355128416, - "velocityX": 3.7714673727599504, - "velocityY": -0.2814583574052856, - "timestamp": 1.723349599535286 - }, - { - "x": 4.301912054355543, - "y": 1.282332269070522, - "heading": -0.08402954787131725, - "angularVelocity": 0.0000010268094506712622, - "velocityX": 3.78190881333695, - "velocityY": -0.018739493984294066, - "timestamp": 1.7836146800300874 - }, - { - "x": 4.529356777936335, - "y": 1.2970411469267544, - "heading": -0.08402949255215464, - "angularVelocity": 9.179306183548953e-7, - "velocityX": 3.7740715139410477, - "velocityY": 0.2440696624889034, - "timestamp": 1.8438797605248887 - }, - { - "x": 4.755229905146536, - "y": 1.32751711580047, - "heading": -0.08402944177914254, - "angularVelocity": 8.424947198098048e-7, - "velocityX": 3.7479934541809494, - "velocityY": 0.5056986338273413, - "timestamp": 1.90414484101969 - }, - { - "x": 4.978439767588294, - "y": 1.373612800948183, - "heading": -0.08402939404329389, - "angularVelocity": 7.920979819424461e-7, - "velocityX": 3.7038009508842378, - "velocityY": 0.7648821634228096, - "timestamp": 1.9644099215144912 - }, - { - "x": 5.197907642868858, - "y": 1.435105133457297, - "heading": -0.08402934813807361, - "angularVelocity": 7.617217119968608e-7, - "velocityX": 3.6417088217363065, - "velocityY": 1.0203642308985088, - "timestamp": 2.0246750020092925 - }, - { - "x": 5.41257465992254, - "y": 1.51169186312203, - "heading": -0.08402930302434115, - "angularVelocity": 7.485882719571497e-7, - "velocityX": 3.562046466895548, - "velocityY": 1.2708309527826855, - "timestamp": 2.084940082504094 - }, - { - "x": 5.626738902818376, - "y": 1.589673479175761, - "heading": -0.0840292579538456, - "angularVelocity": 7.478708262765028e-7, - "velocityX": 3.5537037557647206, - "velocityY": 1.2939768007189265, - "timestamp": 2.145205162998895 - }, - { - "x": 5.8409031120271635, - "y": 1.6676551877456105, - "heading": -0.0840292128833514, - "angularVelocity": 7.478708040156826e-7, - "velocityX": 3.5537031967834927, - "velocityY": 1.2939783358719235, - "timestamp": 2.2054702434936964 - }, - { - "x": 6.055067321252931, - "y": 1.745636896268829, - "heading": -0.08402916781285703, - "angularVelocity": 7.478708068018038e-7, - "velocityX": 3.5537031970652393, - "velocityY": 1.2939783350981522, - "timestamp": 2.2657353239884976 - }, - { - "x": 6.269231822230628, - "y": 1.8236178035375321, - "heading": -0.08402912274221092, - "angularVelocity": 7.478733249018271e-7, - "velocityX": 3.5537080382091872, - "velocityY": 1.2939650395958724, - "timestamp": 2.326000404483299 - }, - { - "x": 6.484552473362613, - "y": 1.8927933712391167, - "heading": -0.0785738032137305, - "angularVelocity": 0.09052206491205224, - "velocityX": 3.572892450555367, - "velocityY": 1.1478548959633867, - "timestamp": 2.3862654849781 - }, - { - "x": 6.6874523544155435, - "y": 1.9584473392764923, - "heading": -0.032553192185028265, - "angularVelocity": 0.7636364317587296, - "velocityX": 3.3667901774467204, - "velocityY": 1.0894197352485022, - "timestamp": 2.4465305654729015 - }, - { - "x": 6.875915672715089, - "y": 2.018652626020107, - "heading": 0.028144572860875648, - "angularVelocity": 1.0071796892586995, - "velocityX": 3.1272391366971615, - "velocityY": 0.9990078209355214, - "timestamp": 2.5067956459677028 - }, - { - "x": 7.049532373282227, - "y": 2.073323850727038, - "heading": 0.08948102802542592, - "angularVelocity": 1.0177777024597514, - "velocityX": 2.8808839072589665, - "velocityY": 0.9071791534676087, - "timestamp": 2.567060726462504 - }, - { - "x": 7.208331847820926, - "y": 2.1225088918275805, - "heading": 0.1460274817443076, - "angularVelocity": 0.9382954980664153, - "velocityX": 2.635016384859868, - "velocityY": 0.8161449498899447, - "timestamp": 2.6273258069573053 - }, - { - "x": 7.352376622997373, - "y": 2.1662517609495984, - "heading": 0.19476972653717045, - "angularVelocity": 0.8087974726436816, - "velocityX": 2.3901863897597297, - "velocityY": 0.72584104696901, - "timestamp": 2.6875908874521066 - }, - { - "x": 7.481724262237549, - "y": 2.2045881748199463, - "heading": 0.2337432969576531, - "angularVelocity": 0.6467023706015796, - "velocityX": 2.146311565141514, - "velocityY": 0.6361298044504436, - "timestamp": 2.747855967946908 - }, - { - "x": 7.603777233505286, - "y": 2.239565945400217, - "heading": 0.2627306680277325, - "angularVelocity": 0.44772760939813167, - "velocityX": 1.8851825132237892, - "velocityY": 0.540252980034664, - "timestamp": 2.8125992861198625 - }, - { - "x": 7.70877923669531, - "y": 2.2685458571337813, - "heading": 0.28047750188422, - "angularVelocity": 0.2741106628035178, - "velocityX": 1.621819921393637, - "velocityY": 0.4476123954003698, - "timestamp": 2.877342604292817 - }, - { - "x": 7.796631047750002, - "y": 2.291686366419086, - "heading": 0.28817114093339696, - "angularVelocity": 0.11883294317143692, - "velocityX": 1.3569247535321998, - "velocityY": 0.3574192663942211, - "timestamp": 2.942085922465772 - }, - { - "x": 7.867260828377704, - "y": 2.309110963970804, - "heading": 0.2867085865097153, - "angularVelocity": -0.022590044269504023, - "velocityX": 1.0909199994819943, - "velocityY": 0.2691335267242617, - "timestamp": 3.0068292406387265 - }, - { - "x": 7.920614376230513, - "y": 2.320918360587503, - "heading": 0.2767910958513383, - "angularVelocity": -0.15318168636157245, - "velocityX": 0.824078057140675, - "velocityY": 0.1823724354867997, - "timestamp": 3.071572558811681 - }, - { - "x": 7.956649448780364, - "y": 2.327189146390105, - "heading": 0.25898192892634386, - "angularVelocity": -0.27507343502876075, - "velocityX": 0.5565836532132435, - "velocityY": 0.09685610777394495, - "timestamp": 3.1363158769846358 - }, - { - "x": 7.975332260131836, - "y": 2.3279902935028076, - "heading": 0.2337432969576531, - "angularVelocity": -0.38982604971324913, - "velocityX": 0.2885674055438917, - "velocityY": 0.012374205328225838, - "timestamp": 3.2010591951575904 - }, - { - "x": 7.972208451820267, - "y": 2.320885188130861, - "heading": 0.19238106068409147, - "angularVelocity": -0.5227816803364386, - "velocityX": -0.039482143745094074, - "velocityY": -0.08980217850764118, - "timestamp": 3.2801787164211174 - }, - { - "x": 7.9431197679460075, - "y": 2.3056970040477323, - "heading": 0.14088004342725263, - "angularVelocity": -0.6509268058549238, - "velocityX": -0.3676549530345614, - "velocityY": -0.19196506551829257, - "timestamp": 3.3592982376846443 - }, - { - "x": 7.888054782175393, - "y": 2.282427289475609, - "heading": 0.07969868788669623, - "angularVelocity": -0.7732776255909873, - "velocityX": -0.695972180964121, - "velocityY": -0.2941083843849025, - "timestamp": 3.4384177589481713 - }, - { - "x": 7.806999942498959, - "y": 2.251078456184475, - "heading": 0.009399898258955654, - "angularVelocity": -0.8885138396324784, - "velocityX": -1.0244606941750847, - "velocityY": -0.39622122063553056, - "timestamp": 3.517537280211698 - }, - { - "x": 7.699938927127902, - "y": 2.2116544317191673, - "heading": -0.06930512925958275, - "angularVelocity": -0.9947611697041394, - "velocityX": -1.353155500201559, - "velocityY": -0.49828441623143704, - "timestamp": 3.596656801475225 - }, - { - "x": 7.56685172715319, - "y": 2.1641617607796966, - "heading": -0.15547855671843705, - "angularVelocity": -1.089155066697531, - "velocityX": -1.682103200946216, - "velocityY": -0.6002648926714905, - "timestamp": 3.675776322738752 - }, - { - "x": 7.407713494064462, - "y": 2.1086115226344657, - "heading": -0.24780538645569025, - "angularVelocity": -1.1669285691167814, - "velocityX": -2.011364964642267, - "velocityY": -0.702105337066011, - "timestamp": 3.754895844002279 - }, - { - "x": 7.222493943520295, - "y": 2.045023086580113, - "heading": -0.3442810817864959, - "angularVelocity": -1.2193665202986854, - "velocityX": -2.3410094953335943, - "velocityY": -0.8037009708710834, - "timestamp": 3.834015365265806 - }, - { - "x": 7.0111623875747595, - "y": 1.9734335261753837, - "heading": -0.4414827777687788, - "angularVelocity": -1.2285425193427022, - "velocityX": -2.6710418942202843, - "velocityY": -0.9048280280448349, - "timestamp": 3.913134886529333 - }, - { - "x": 6.773734962048751, - "y": 1.893931948844204, - "heading": -0.5324651154460041, - "angularVelocity": -1.1499353917244444, - "velocityX": -3.0008703507595227, - "velocityY": -1.0048288470601288, - "timestamp": 3.99225440779286 - }, - { - "x": 6.510837595414292, - "y": 1.8068437015263887, - "heading": -0.5976366951472135, - "angularVelocity": -0.8237104909184032, - "velocityX": -3.3227876311184024, - "velocityY": -1.1007175716817783, - "timestamp": 4.071373929056387 - }, - { - "x": 6.227373100797986, - "y": 1.7110085398670838, - "heading": -0.597636758184297, - "angularVelocity": -7.967323658608173e-7, - "velocityX": -3.5827377376583796, - "velocityY": -1.2112707474568936, - "timestamp": 4.150493450319914 - }, - { - "x": 5.941831354312318, - "y": 1.6215523108704284, - "heading": -0.5976367779578059, - "angularVelocity": -2.499194721356179e-7, - "velocityX": -3.6089923438060088, - "velocityY": -1.1306467426502582, - "timestamp": 4.229612971583441 - }, - { - "x": 5.656289545112644, - "y": 1.532096282055343, - "heading": -0.5976367977313131, - "angularVelocity": -2.499194483006569e-7, - "velocityX": -3.6089931364549517, - "velocityY": -1.130644212534215, - "timestamp": 4.308732492846968 - }, - { - "x": 5.370747722448539, - "y": 1.442640296218458, - "heading": -0.5976368175048198, - "angularVelocity": -2.499194443736624e-7, - "velocityX": -3.6089933066333257, - "velocityY": -1.130643669328191, - "timestamp": 4.387852014110495 - }, - { - "x": 5.084751687474933, - "y": 1.3546472631644115, - "heading": -0.5976368372766676, - "angularVelocity": -2.4989847693362694e-7, - "velocityX": -3.6147341440682292, - "velocityY": -1.112153254327249, - "timestamp": 4.466971535374022 - }, - { - "x": 4.791925527807106, - "y": 1.2930896001142946, - "heading": -0.5976368572220272, - "angularVelocity": -2.52091508667939e-7, - "velocityX": -3.7010608126974813, - "velocityY": -0.7780338160172059, - "timestamp": 4.5460910566375485 - }, - { - "x": 4.494706489260304, - "y": 1.2584868813594625, - "heading": -0.5976368780658494, - "angularVelocity": -2.63447274854176e-7, - "velocityX": -3.756582873610133, - "velocityY": -0.4373474232683845, - "timestamp": 4.6252105779010755 - }, - { - "x": 4.195570491935266, - "y": 1.2511276025347224, - "heading": -0.597636900694303, - "angularVelocity": -2.860034177860552e-7, - "velocityX": -3.780811518420208, - "velocityY": -0.09301470366874595, - "timestamp": 4.704330099164602 - }, - { - "x": 3.897009479956395, - "y": 1.2710731343763437, - "heading": -0.597636926337917, - "angularVelocity": -3.24112350243062e-7, - "velocityX": -3.7735442177972423, - "velocityY": 0.2520936871595517, - "timestamp": 4.783449620428129 - }, - { - "x": 3.6015106196507847, - "y": 1.3181573465478027, - "heading": -0.5976369569136154, - "angularVelocity": -3.8644948741127607e-7, - "velocityX": -3.7348413588269285, - "velocityY": 0.5951023390881358, - "timestamp": 4.862569141691656 - }, - { - "x": 3.311535572783971, - "y": 1.3919880157013045, - "heading": -0.5976369957684174, - "angularVelocity": -4.910899536256082e-7, - "velocityX": -3.6650252963611574, - "velocityY": 0.933153638627185, - "timestamp": 4.941688662955183 - }, - { - "x": 3.029499988052695, - "y": 1.4919500996437052, - "heading": -0.5976370495734, - "angularVelocity": -6.80046867328716e-7, - "velocityX": -3.5646775944445412, - "velocityY": 1.263431354816361, - "timestamp": 5.02080818421871 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.5976371219829064, - "angularVelocity": -9.151914125801383e-7, - "velocityX": -3.434634228323893, - "velocityY": 1.583184066038948, - "timestamp": 5.099927705482237 - }, - { - "x": 2.562615182382705, - "y": 1.7237517111402614, - "heading": -0.5976372044989885, - "angularVelocity": -0.000001403654433041047, - "velocityX": -3.319432744203681, - "velocityY": 1.8123319353797018, - "timestamp": 5.158714312968238 - }, - { - "x": 2.3751466995867516, - "y": 1.8432734201994623, - "heading": -0.5976372636960788, - "angularVelocity": -0.000001006982590926024, - "velocityX": -3.188965834447897, - "velocityY": 2.033145203823246, - "timestamp": 5.217500920454238 - }, - { - "x": 2.1962100914661193, - "y": 1.9752263102479175, - "heading": -0.5976373093137471, - "angularVelocity": -7.759874278787006e-7, - "velocityX": -3.04383286896162, - "velocityY": 2.2446080100791628, - "timestamp": 5.276287527940239 - }, - { - "x": 2.026628285101079, - "y": 2.1190035289778706, - "heading": -0.5976373463774368, - "angularVelocity": -6.304784584021761e-7, - "velocityX": -2.884701356604495, - "velocityY": 2.4457478476571914, - "timestamp": 5.33507413542624 - }, - { - "x": 1.8671811829950287, - "y": 2.2739438442752706, - "heading": -0.5976373777665476, - "angularVelocity": -5.339500279584693e-7, - "velocityX": -2.7123031745627153, - "velocityY": 2.6356396792296386, - "timestamp": 5.39386074291224 - }, - { - "x": 1.7186020763048488, - "y": 2.439334685025019, - "heading": -0.5976374052703787, - "angularVelocity": -4.678587919148875e-7, - "velocityX": -2.5274312134028762, - "velocityY": 2.813410193625064, - "timestamp": 5.452647350398241 - }, - { - "x": 1.5815742720495027, - "y": 2.6144154181152923, - "heading": -0.597637430079118, - "angularVelocity": -4.220134537614776e-7, - "velocityX": -2.330935737157126, - "velocityY": 2.978241823734581, - "timestamp": 5.5114339578842415 - }, - { - "x": 1.456727954325337, - "y": 2.7983808405830617, - "heading": -0.5976374777093088, - "angularVelocity": -8.102217978593512e-7, - "velocityX": -2.123720402710731, - "velocityY": 3.129376406209191, - "timestamp": 5.570220565370242 - }, - { - "x": 1.3455561973341574, - "y": 2.9885105140912174, - "heading": -0.6039795556256176, - "angularVelocity": -0.10788303982023785, - "velocityX": -1.8911068650739042, - "velocityY": 3.234234490456602, - "timestamp": 5.629007172856243 - }, - { - "x": 1.246887053690835, - "y": 3.1733277865183314, - "heading": -0.6383468335999772, - "angularVelocity": -0.584610669743856, - "velocityX": -1.6784289460285728, - "velocityY": 3.1438669508378774, - "timestamp": 5.687793780342243 - }, - { - "x": 1.158565022655154, - "y": 3.3474027501759167, - "heading": -0.6816254514815488, - "angularVelocity": -0.736198595775039, - "velocityX": -1.5024175541464038, - "velocityY": 2.9611330046395477, - "timestamp": 5.746580387828244 - }, - { - "x": 1.0790333486785466, - "y": 3.5092497722088574, - "heading": -0.7278331515121171, - "angularVelocity": -0.7860242665231638, - "velocityX": -1.3528876282841755, - "velocityY": 2.7531274375968073, - "timestamp": 5.805366995314245 - }, - { - "x": 1.0074427197106477, - "y": 3.6582801720807683, - "heading": -0.7741804064083878, - "angularVelocity": -0.7883981892867038, - "velocityX": -1.217805075500369, - "velocityY": 2.5351080160120225, - "timestamp": 5.864153602800245 - }, - { - "x": 0.9432732996874974, - "y": 3.7941912605500616, - "heading": -0.8190469180938782, - "angularVelocity": -0.7632097446033984, - "velocityX": -1.0915652861654275, - "velocityY": 2.3119396454653414, - "timestamp": 5.922940210286246 - }, - { - "x": 0.8861770840921692, - "y": 3.9168021213870996, - "heading": -0.8613760150853045, - "angularVelocity": -0.7200466024767106, - "velocityX": -0.971245289310589, - "velocityY": 2.085693767347223, - "timestamp": 5.9817268177722465 - }, - { - "x": 0.8359056858624994, - "y": 4.025993630774074, - "heading": -0.9004245404660403, - "angularVelocity": -0.6642418579781981, - "velocityX": -0.8551505245755496, - "velocityY": 1.857421512424882, - "timestamp": 6.040513425258247 - }, - { - "x": 0.7922732558571624, - "y": 4.121681912181887, - "heading": -0.9356409495537121, - "angularVelocity": -0.5990549649604897, - "velocityX": -0.7422171795800264, - "velocityY": 1.627722460946589, - "timestamp": 6.099300032744248 - }, - { - "x": 0.7551356458246514, - "y": 4.203804939460416, - "heading": -0.9665991512691937, - "angularVelocity": -0.5266199741642557, - "velocityX": -0.6317358939509348, - "velocityY": 1.3969683026544202, - "timestamp": 6.158086640230248 - }, - { - "x": 0.7243778479811152, - "y": 4.272315098350931, - "heading": -0.9929596716344966, - "angularVelocity": -0.44841030113160957, - "velocityX": -0.5232109686013307, - "velocityY": 1.1654041935798078, - "timestamp": 6.216873247716249 - }, - { - "x": 0.6999059919554825, - "y": 4.327174746467568, - "heading": -1.0144454156380707, - "angularVelocity": -0.3654870543208486, - "velocityX": -0.41628284182686576, - "velocityY": 0.9331997620325617, - "timestamp": 6.27565985520225 - }, - { - "x": 0.6816420121384978, - "y": 4.368353408748977, - "heading": -1.0308257653496888, - "angularVelocity": -0.27864084035669345, - "velocityX": -0.31068266392705546, - "velocityY": 0.7004769290559099, - "timestamp": 6.33444646268825 - }, - { - "x": 0.6695199611090151, - "y": 4.395825923599214, - "heading": -1.0419057024338247, - "angularVelocity": -0.1884772324508513, - "velocityX": -0.2062042963164626, - "velocityY": 0.46732608029439393, - "timestamp": 6.393233070174251 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -0.09547069579077468, - "velocityX": -0.1026863106378516, - "velocityY": 0.23381596243572259, - "timestamp": 6.452019677660251 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -7.821518056781921e-37, - "velocityX": -6.302822662985753e-37, - "velocityY": 5.041148502478119e-38, - "timestamp": 6.510806285146252 - } - ] + "samples": [ + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": 1.6934356345077546e-25, + "velocityX": 7.869408079103222e-27, + "velocityY": 8.248206791285532e-26, + "timestamp": 0 + }, + { + "x": 0.6687542994417671, + "y": 4.396916362162236, + "heading": -1.0350125954694533, + "angularVelocity": 0.22040030578546457, + "velocityX": 0.09289604459177633, + "velocityY": -0.22303166737709929, + "timestamp": 0.05673996340282805 + }, + { + "x": 0.679356564606483, + "y": 4.37160747322566, + "heading": -1.010445166940442, + "angularVelocity": 0.4329828053394013, + "velocityX": 0.18685710262878952, + "velocityY": -0.44605049807478747, + "timestamp": 0.1134799268056561 + }, + { + "x": 0.6953596015431592, + "y": 4.333646306609793, + "heading": -0.9743318563908197, + "angularVelocity": 0.6364704589820466, + "velocityX": 0.28204172115977494, + "velocityY": -0.6690375590544457, + "timestamp": 0.17021989020848416 + }, + { + "x": 0.7168434967838145, + "y": 4.283036502246609, + "heading": -0.9272707026758202, + "angularVelocity": 0.8294181189523645, + "velocityX": 0.37863780574071754, + "velocityY": -0.8919604689181454, + "timestamp": 0.2269598536113122 + }, + { + "x": 0.7439015517568099, + "y": 4.219784887043078, + "heading": -0.8699519171651382, + "angularVelocity": 1.010201312675245, + "velocityX": 0.47687825917150645, + "velocityY": -1.114763059582432, + "timestamp": 0.28369981701414027 + }, + { + "x": 0.7766442528130582, + "y": 4.143903696205975, + "heading": -0.8031709814362636, + "angularVelocity": 1.176964730392239, + "velocityX": 0.5770659530354297, + "velocityY": -1.3373500137527523, + "timestamp": 0.3404397804169683 + }, + { + "x": 0.8152053930991828, + "y": 4.055414009370765, + "heading": -0.7278520253810473, + "angularVelocity": 1.3274410404618378, + "velocityX": 0.679611652414335, + "velocityY": -1.5595654548977111, + "timestamp": 0.39717974381979637 + }, + { + "x": 0.8597512927773133, + "y": 3.954350973035596, + "heading": -0.6450962742441403, + "angularVelocity": 1.4585090679276476, + "velocityX": 0.7850886219625312, + "velocityY": -1.7811614649390577, + "timestamp": 0.4539197072226244 + }, + { + "x": 0.9104943771963883, + "y": 3.8407721699102697, + "heading": -0.556279741952017, + "angularVelocity": 1.56532586497398, + "velocityX": 0.8943094315874387, + "velocityY": -2.001742622196794, + "timestamp": 0.5106596706254525 + }, + { + "x": 0.9677131148965982, + "y": 3.714772796642351, + "heading": -0.4632364417450351, + "angularVelocity": 1.6398195315428243, + "velocityX": 1.0084380438172502, + "velocityY": -2.2206460087642403, + "timestamp": 0.5673996340282805 + }, + { + "x": 1.0317820619285505, + "y": 3.5765176361367876, + "heading": -0.36858460120873987, + "angularVelocity": 1.6681688682861946, + "velocityX": 1.1291679301428568, + "velocityY": -2.4366452181862606, + "timestamp": 0.6241395974311086 + }, + { + "x": 1.1032183237055362, + "y": 3.4263179301504665, + "heading": -0.2763023573010561, + "angularVelocity": 1.6264064756708094, + "velocityX": 1.2590114179281549, + "velocityY": -2.6471590212346836, + "timestamp": 0.6808795608339366 + }, + { + "x": 1.1827463584827187, + "y": 3.264837840257316, + "heading": -0.19274016139718564, + "angularVelocity": 1.472722062060866, + "velocityX": 1.401622948054619, + "velocityY": -2.845967466470068, + "timestamp": 0.7376195242367647 + }, + { + "x": 1.2713975127609685, + "y": 3.0937718125950266, + "heading": -0.12815969147628775, + "angularVelocity": 1.1381831437289833, + "velocityX": 1.5624111994727097, + "velocityY": -3.0149125484589985, + "timestamp": 0.7943594876395927 + }, + { + "x": 1.3706975997688013, + "y": 2.916891628537348, + "heading": -0.09351951086762286, + "angularVelocity": 0.6105076304462074, + "velocityX": 1.7500907835073471, + "velocityY": -3.117382766039356, + "timestamp": 0.8510994510424208 + }, + { + "x": 1.4825331256479723, + "y": 2.7375513323263876, + "heading": -0.08403180748655616, + "angularVelocity": 0.16721377336302404, + "velocityX": 1.971018646684527, + "velocityY": -3.160740428007094, + "timestamp": 0.9078394144452488 + }, + { + "x": 1.6073609239957887, + "y": 2.563006197529012, + "heading": -0.08403156658210396, + "angularVelocity": 0.000004245763263575802, + "velocityX": 2.199997865024975, + "velocityY": -3.0762292453060858, + "timestamp": 0.9645793778480769 + }, + { + "x": 1.743340039826477, + "y": 2.397001117024792, + "heading": -0.08403149597768683, + "angularVelocity": 0.000001244350769606544, + "velocityX": 2.3965316097456575, + "velocityY": -2.925717088071768, + "timestamp": 1.021319341250905 + }, + { + "x": 1.889887863777881, + "y": 2.2402473603885453, + "heading": -0.08403141650456336, + "angularVelocity": 0.0000014006551769644768, + "velocityX": 2.5827972942277393, + "velocityY": -2.7626693292585887, + "timestamp": 1.078059304653733 + }, + { + "x": 2.0463765393116815, + "y": 2.0934165159455875, + "heading": -0.08403132385631941, + "angularVelocity": 0.000001632856956195875, + "velocityX": 2.757997470368504, + "velocityY": -2.587785321617605, + "timestamp": 1.134799268056561 + }, + { + "x": 2.212135617801855, + "y": 1.957137659226374, + "heading": -0.08403121101732289, + "angularVelocity": 0.000001988704076477053, + "velocityX": 2.9213814840407957, + "velocityY": -2.401814321798131, + "timestamp": 1.1915392314593891 + }, + { + "x": 2.3864549304867793, + "y": 1.8319946581987157, + "heading": -0.08403106536679993, + "angularVelocity": 0.0000025669830265249184, + "velocityX": 3.072249297154761, + "velocityY": -2.2055530797438214, + "timestamp": 1.2482791948622172 + }, + { + "x": 2.5685876291367493, + "y": 1.7185236728273872, + "heading": -0.08403086086157263, + "angularVelocity": 0.000003604253774998199, + "velocityX": 3.209954461142516, + "velocityY": -1.9998424138157533, + "timestamp": 1.3050191582650452 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.0840305094244903, + "angularVelocity": 0.000006193819334023837, + "velocityX": 3.333906680775301, + "velocityY": -1.7855635028764607, + "timestamp": 1.3617591216678733 + }, + { + "x": 2.9656624873135113, + "y": 1.5238234413872127, + "heading": -0.08403019929618429, + "angularVelocity": 0.000005146069704041112, + "velocityX": 3.449910175144429, + "velocityY": -1.5496108616313848, + "timestamp": 1.4220242023702783 + }, + { + "x": 3.179557703869071, + "y": 1.4451070800547587, + "heading": -0.08403000395819739, + "angularVelocity": 0.000003241312956351986, + "velocityX": 3.549239693410422, + "velocityY": -1.306168686990774, + "timestamp": 1.4822892830726833 + }, + { + "x": 3.398405200510204, + "y": 1.3814422413572929, + "heading": -0.084029865442907, + "angularVelocity": 0.000002298433666202308, + "velocityX": 3.631414644938793, + "velocityY": -1.0564133981973587, + "timestamp": 1.5425543637750883 + }, + { + "x": 3.6211472309421358, + "y": 1.3331366319820044, + "heading": -0.08402975974511386, + "angularVelocity": 0.0000017538812179176465, + "velocityX": 3.696038034560229, + "velocityY": -0.8015522224856215, + "timestamp": 1.6028194444774932 + }, + { + "x": 3.8467072298284624, + "y": 1.3004237209144467, + "heading": -0.08402967471443804, + "angularVelocity": 0.0000014109443619153728, + "velocityX": 3.742797591198178, + "velocityY": -0.5428170125432534, + "timestamp": 1.6630845251798991 + }, + { + "x": 4.07399501434713, + "y": 1.283461611303661, + "heading": -0.08402960341272074, + "angularVelocity": 0.000001183134851133743, + "velocityX": 3.771467355051538, + "velocityY": -0.28145834060267944, + "timestamp": 1.723349605882305 + }, + { + "x": 4.301912053057315, + "y": 1.282332275177001, + "heading": -0.08402954153196555, + "angularVelocity": 0.000001026809463787284, + "velocityX": 3.7819087945083893, + "velocityY": -0.01873947754649237, + "timestamp": 1.783614686584711 + }, + { + "x": 4.529356776217307, + "y": 1.2970411540477857, + "heading": -0.08402948621280212, + "angularVelocity": 9.179306286652721e-7, + "velocityX": 3.774071493957432, + "velocityY": 0.2440696784829437, + "timestamp": 1.8438797672871168 + }, + { + "x": 4.755229902929938, + "y": 1.3275171239584524, + "heading": -0.08402943543978927, + "angularVelocity": 8.424947292982353e-7, + "velocityX": 3.747993433013281, + "velocityY": 0.505698649291784, + "timestamp": 1.9041448479895227 + }, + { + "x": 4.978439764792266, + "y": 1.3736128101593832, + "heading": -0.08402938770393992, + "angularVelocity": 7.920979912088378e-7, + "velocityX": 3.703800928510481, + "velocityY": 0.7648821782643186, + "timestamp": 1.9644099286919285 + }, + { + "x": 5.197907639407193, + "y": 1.435105143730422, + "heading": -0.08402934179871885, + "angularVelocity": 7.617217222606304e-7, + "velocityX": 3.6417087981459852, + "velocityY": 1.020364245004398, + "timestamp": 2.0246750093943344 + }, + { + "x": 5.4125746557646295, + "y": 1.5116918742918497, + "heading": -0.08402929668498581, + "angularVelocity": 7.485882790977605e-7, + "velocityX": 3.562046443071779, + "velocityY": 1.270830963284032, + "timestamp": 2.0849400900967403 + }, + { + "x": 5.626738899704859, + "y": 1.5896734864413642, + "heading": -0.08402925161448958, + "angularVelocity": 7.478708348501504e-7, + "velocityX": 3.553703760852683, + "velocityY": 1.293976731477308, + "timestamp": 2.145205170799146 + }, + { + "x": 5.840903108372873, + "y": 1.667655195460401, + "heading": -0.08402920654399476, + "angularVelocity": 7.478708115803468e-7, + "velocityX": 3.553703175568263, + "velocityY": 1.2939783388678752, + "timestamp": 2.205470251501552 + }, + { + "x": 6.055067317059206, + "y": 1.7456369044291342, + "heading": -0.08402916147349976, + "angularVelocity": 7.478708148850431e-7, + "velocityX": 3.5537031758721978, + "velocityY": 1.293978338033166, + "timestamp": 2.265735332203958 + }, + { + "x": 6.269231826783721, + "y": 1.8236177866398526, + "heading": -0.08402911640284853, + "angularVelocity": 7.47873406932242e-7, + "velocityX": 3.5537081711062477, + "velocityY": 1.2939646193422618, + "timestamp": 2.326000412906364 + }, + { + "x": 6.484552476173004, + "y": 1.8927933599363171, + "heading": -0.07857379817757901, + "angularVelocity": 0.09052204297557322, + "velocityX": 3.5728924093299965, + "velocityY": 1.1478549848470312, + "timestamp": 2.3862654936087697 + }, + { + "x": 6.687452356091927, + "y": 1.958447331788137, + "heading": -0.03255318883880569, + "angularVelocity": 0.763636401086517, + "velocityX": 3.3667901470316046, + "velocityY": 1.089419794790041, + "timestamp": 2.4465305743111756 + }, + { + "x": 6.875915673704612, + "y": 2.0186526211606592, + "heading": 0.028144575028097817, + "angularVelocity": 1.0071796662255343, + "velocityX": 3.127239114526933, + "velocityY": 0.9990078611164873, + "timestamp": 2.5067956550135815 + }, + { + "x": 7.0495323738250235, + "y": 2.0733238477692897, + "heading": 0.08948102939748631, + "angularVelocity": 1.0177776857592493, + "velocityX": 2.880883889922049, + "velocityY": 0.9071791818980943, + "timestamp": 2.5670607357159874 + }, + { + "x": 7.208331848074836, + "y": 2.122508890246591, + "heading": 0.14602748254405165, + "angularVelocity": 0.938295485337472, + "velocityX": 2.635016370989014, + "velocityY": 0.8161449699234864, + "timestamp": 2.6273258164183932 + }, + { + "x": 7.352376623078616, + "y": 2.166251760331207, + "heading": 0.19476972690009334, + "angularVelocity": 0.8087974626091599, + "velocityX": 2.39018637866075, + "velocityY": 0.7258410604413249, + "timestamp": 2.687590897120799 + }, + { + "x": 7.481724262237549, + "y": 2.2045881748199463, + "heading": 0.2337432969576531, + "angularVelocity": 0.646702362351673, + "velocityX": 2.1463115563996884, + "velocityY": 0.6361298125202616, + "timestamp": 2.747855977823205 + }, + { + "x": 7.603777232623247, + "y": 2.2395659454817083, + "heading": 0.2627306675325177, + "angularVelocity": 0.44772760396065475, + "velocityX": 1.8851825089114527, + "velocityY": 0.54025298396176, + "timestamp": 2.812599295676378 + }, + { + "x": 7.708779235262183, + "y": 2.2685458571632657, + "heading": 0.28047750108878416, + "angularVelocity": 0.27411065952030855, + "velocityX": 1.6218199208922495, + "velocityY": 0.4476123968079393, + "timestamp": 2.8773426135295512 + }, + { + "x": 7.796631046068626, + "y": 2.2916863663414535, + "heading": 0.2881711400063891, + "angularVelocity": 0.1188329417261678, + "velocityX": 1.3569247563999605, + "velocityY": 0.3574192665050968, + "timestamp": 2.9420859313827243 + }, + { + "x": 7.867260826730005, + "y": 2.309110963792019, + "heading": 0.28670858560451956, + "angularVelocity": -0.02259004404417999, + "velocityX": 1.0909200053904464, + "velocityY": 0.2691335264912042, + "timestamp": 3.0068292492358974 + }, + { + "x": 7.920614374882339, + "y": 2.320918360362405, + "heading": 0.27679109511178074, + "angularVelocity": -0.1531816845597854, + "velocityX": 0.8240780658373096, + "velocityY": 0.18237243567224945, + "timestamp": 3.0715725670890706 + }, + { + "x": 7.95664944798485, + "y": 2.3271891462135073, + "heading": 0.25898192848994617, + "angularVelocity": -0.27507343170491383, + "velocityX": 0.5565836644984964, + "velocityY": 0.09685610900144763, + "timestamp": 3.1363158849422437 + }, + { + "x": 7.975332260131836, + "y": 2.3279902935028076, + "heading": 0.2337432969576531, + "angularVelocity": -0.38982604489825085, + "velocityX": 0.28856741925638435, + "velocityY": 0.012374208116996958, + "timestamp": 3.201059202795417 + }, + { + "x": 7.972208452847698, + "y": 2.3208851884260335, + "heading": 0.1923810608861147, + "angularVelocity": -0.5227816750662158, + "velocityX": -0.039482130554085705, + "velocityY": -0.08980217431023955, + "timestamp": 3.280178724470119 + }, + { + "x": 7.943119769823989, + "y": 2.3056970047129943, + "heading": 0.14088004377458682, + "angularVelocity": -0.650926800635539, + "velocityX": -0.367654940373712, + "velocityY": -0.19196505984306744, + "timestamp": 3.359298246144821 + }, + { + "x": 7.888054784726001, + "y": 2.282427290592543, + "heading": 0.07969868827872208, + "angularVelocity": -0.7732776210074993, + "velocityX": -0.6959721688458461, + "velocityY": -0.29410837714772914, + "timestamp": 3.438417767819523 + }, + { + "x": 7.80699994554277, + "y": 2.251078457842943, + "heading": 0.009399898543879681, + "angularVelocity": -0.8885138363686558, + "velocityX": -1.0244606826174518, + "velocityY": -0.3962212117319218, + "timestamp": 3.517537289494225 + }, + { + "x": 7.6999389304832775, + "y": 2.211654434019612, + "heading": -0.0693051292941164, + "angularVelocity": -0.9947611685721511, + "velocityX": -1.3531554892314888, + "velocityY": -0.4982844055278961, + "timestamp": 3.596656811168927 + }, + { + "x": 7.566851730635094, + "y": 2.1641617638366206, + "heading": -0.15547855735771587, + "angularVelocity": -1.0891550686807683, + "velocityX": -1.6821031906053239, + "velocityY": -0.6002648799907612, + "timestamp": 3.675776332843629 + }, + { + "x": 7.407713497482348, + "y": 2.1086115265820684, + "heading": -0.24780538807324573, + "angularVelocity": -1.1669285754169436, + "velocityX": -2.011364954998582, + "velocityY": -0.70210532215988, + "timestamp": 3.754895854518331 + }, + { + "x": 7.22249394667395, + "y": 2.0450230915823275, + "heading": -0.3442810848564748, + "angularVelocity": -1.2193665323191243, + "velocityX": -2.3410094865073168, + "velocityY": -0.803700953365003, + "timestamp": 3.834015376193033 + }, + { + "x": 7.0111623902454, + "y": 1.973433532446374, + "heading": -0.441482782839164, + "angularVelocity": -1.2285425382414696, + "velocityX": -2.671041886444093, + "velocityY": -0.9048280073063681, + "timestamp": 3.913134897867735 + }, + { + "x": 6.773734963981195, + "y": 1.8939319567020383, + "heading": -0.5324651228432857, + "angularVelocity": -1.1499354151582672, + "velocityX": -3.0008703444945404, + "velocityY": -1.0048288217818704, + "timestamp": 3.992254419542437 + }, + { + "x": 6.510837596418812, + "y": 1.8068437116330571, + "heading": -0.5976367021926547, + "angularVelocity": -0.8237104821907345, + "velocityX": -3.3227876255784223, + "velocityY": -1.1007175375382399, + "timestamp": 4.071373941217139 + }, + { + "x": 6.227373099976041, + "y": 1.711008555192143, + "heading": -0.5976367652296872, + "angularVelocity": -7.967317193878993e-7, + "velocityX": -3.582737742124223, + "velocityY": -1.2112706752062792, + "timestamp": 4.150493462891841 + }, + { + "x": 5.941831356009296, + "y": 1.6215523179581028, + "heading": -0.5976367850031955, + "angularVelocity": -2.499194616123805e-7, + "velocityX": -3.6089922932135883, + "velocityY": -1.1306468408876076, + "timestamp": 4.229612984566543 + }, + { + "x": 5.656289546537612, + "y": 1.5320962898142203, + "heading": -0.5976368047767018, + "angularVelocity": -2.4991943644041576e-7, + "velocityX": -3.608993121137454, + "velocityY": -1.1306441981749955, + "timestamp": 4.308732506241245 + }, + { + "x": 5.370747722964019, + "y": 1.442640306683351, + "heading": -0.5976368245502077, + "angularVelocity": -2.4991943318222364e-7, + "velocityX": -3.6089932993729574, + "velocityY": -1.130643629250762, + "timestamp": 4.387852027915947 + }, + { + "x": 5.084751688404252, + "y": 1.3546472720839122, + "heading": -0.5976368443220548, + "angularVelocity": -2.4989846513470833e-7, + "velocityX": -3.614734120052362, + "velocityY": -1.1121532680799078, + "timestamp": 4.4669715495906495 + }, + { + "x": 4.791925529016698, + "y": 1.2930896074141898, + "heading": -0.5976368642674134, + "angularVelocity": -2.5209149670001704e-7, + "velocityX": -3.7010607899211223, + "velocityY": -0.7780338324442274, + "timestamp": 4.5460910712653515 + }, + { + "x": 4.494706490584963, + "y": 1.2584868871615749, + "heading": -0.5976368851112348, + "angularVelocity": -2.6344726295956754e-7, + "velocityX": -3.7565828526333167, + "velocityY": -0.4373474399261832, + "timestamp": 4.6252105929400535 + }, + { + "x": 4.195570493234456, + "y": 1.2511276069767714, + "heading": -0.5976369077396874, + "angularVelocity": -2.860034028815913e-7, + "velocityX": -3.7808114990937063, + "velocityY": -0.09301472037534324, + "timestamp": 4.704330114614756 + }, + { + "x": 3.8970094811156857, + "y": 1.2710731376080628, + "heading": -0.5976369333832999, + "angularVelocity": -3.2411233223341237e-7, + "velocityX": -3.7735441999548045, + "velocityY": 0.2520936705519675, + "timestamp": 4.783449636289458 + }, + { + "x": 3.6015106205827614, + "y": 1.3181573487272922, + "heading": -0.5976369639589968, + "angularVelocity": -3.8644946446935137e-7, + "velocityX": -3.734841342290479, + "velocityY": 0.5951023226962214, + "timestamp": 4.86256915796416 + }, + { + "x": 3.311535573428697, + "y": 1.3919880169913565, + "heading": -0.5976370028137967, + "angularVelocity": -4.910899244313184e-7, + "velocityX": -3.665025280945076, + "velocityY": 0.9331536225360112, + "timestamp": 4.941688679638862 + }, + { + "x": 3.0294999883777347, + "y": 1.4919501002081306, + "heading": -0.597637056618776, + "angularVelocity": -6.800468215926745e-7, + "velocityX": -3.564677579959917, + "velocityY": 1.263431339079197, + "timestamp": 5.020808201313564 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.5976371290282776, + "angularVelocity": -9.151913479184684e-7, + "velocityX": -3.4346342145827435, + "velocityY": 1.5831840506775074, + "timestamp": 5.099927722988266 + }, + { + "x": 2.5626151824790466, + "y": 1.7237517105978553, + "heading": -0.5976372115443579, + "angularVelocity": -0.0000014036543940556534, + "velocityX": -3.319432731110931, + "velocityY": 1.8123319198994405, + "timestamp": 5.158714330677112 + }, + { + "x": 2.3751466997704354, + "y": 1.8432734191532605, + "heading": -0.5976372707414468, + "angularVelocity": -0.0000010069825624449078, + "velocityX": -3.188965821958426, + "velocityY": 2.0331451882378415, + "timestamp": 5.2175009383659585 + }, + { + "x": 2.196210091733693, + "y": 1.9752263087350537, + "heading": -0.5976373163591139, + "angularVelocity": -7.759874050438566e-7, + "velocityX": -3.0438328570316586, + "velocityY": 2.244607994395761, + "timestamp": 5.276287546054805 + }, + { + "x": 2.0266282854543833, + "y": 2.1190035270334358, + "heading": -0.5976373534228027, + "angularVelocity": -6.304784410722591e-7, + "velocityX": -2.8847013451923114, + "velocityY": 2.445747831876676, + "timestamp": 5.335074153743651 + }, + { + "x": 1.867181183440892, + "y": 2.2739438419317777, + "heading": -0.5976373848119125, + "angularVelocity": -5.339500111409062e-7, + "velocityX": -2.712303163629251, + "velocityY": 2.635639663346953, + "timestamp": 5.393860761432498 + }, + { + "x": 1.718602076854733, + "y": 2.439334682311898, + "heading": -0.5976374123157431, + "angularVelocity": -4.678587798571888e-7, + "velocityX": -2.52743120291235, + "velocityY": 2.813410177629591, + "timestamp": 5.452647369121344 + }, + { + "x": 1.5815742727191044, + "y": 2.6144154150584233, + "heading": -0.5976374371244817, + "angularVelocity": -4.220134408077074e-7, + "velocityX": -2.3309357270776045, + "velocityY": 2.9782418076105714, + "timestamp": 5.51143397681019 + }, + { + "x": 1.4567279551341459, + "y": 2.7983808372043484, + "heading": -0.5976374847546617, + "angularVelocity": -8.102216113846116e-7, + "velocityX": -2.1237203930146884, + "velocityY": 3.12937638993629, + "timestamp": 5.570220584499037 + }, + { + "x": 1.345556198102205, + "y": 2.9885105109664996, + "heading": -0.6039795609021088, + "angularVelocity": -0.10788300935844368, + "velocityX": -1.891106859241893, + "velocityY": 3.2342344836173083, + "timestamp": 5.629007192187883 + }, + { + "x": 1.246887054415439, + "y": 3.17332778383869, + "heading": -0.6383468381311229, + "angularVelocity": -0.584610655047786, + "velocityX": -1.6784289409760504, + "velocityY": 3.1438669475608143, + "timestamp": 5.687793799876729 + }, + { + "x": 1.1585650233427731, + "y": 3.3474027479162674, + "heading": -0.6816254554461085, + "angularVelocity": -0.736198583596727, + "velocityX": -1.502417549591352, + "velocityY": 2.9611330015663175, + "timestamp": 5.746580407565576 + }, + { + "x": 1.079033349316087, + "y": 3.5092497703250873, + "heading": -0.7278331549531709, + "angularVelocity": -0.7860242549057459, + "velocityX": -1.352887624467824, + "velocityY": 2.753127434490931, + "timestamp": 5.805367015254422 + }, + { + "x": 1.0074427202851626, + "y": 3.6582801705311523, + "heading": -0.7741804093489261, + "angularVelocity": -0.7883981780521784, + "velocityX": -1.2178050723703648, + "velocityY": 2.535108012948657, + "timestamp": 5.8641536229432685 + }, + { + "x": 0.9432733001900041, + "y": 3.7941912592964138, + "heading": -0.8190469205570853, + "angularVelocity": -0.7632097338501684, + "velocityX": -1.0915652836238168, + "velocityY": 2.3119396425224727, + "timestamp": 5.922940230632115 + }, + { + "x": 0.886177084517944, + "y": 3.9168021203940837, + "heading": -0.861376017099095, + "angularVelocity": -0.7200465923472629, + "velocityX": -0.9712452872645088, + "velocityY": 2.0856937645839286, + "timestamp": 5.981726838320961 + }, + { + "x": 0.835905686210625, + "y": 4.025993630008512, + "heading": -0.9004245420638279, + "angularVelocity": -0.6642418486097024, + "velocityX": -0.855150522945662, + "velocityY": 1.8574215098848674, + "timestamp": 6.0405134460098076 + }, + { + "x": 0.79227325612998, + "y": 4.121681911612244, + "heading": -0.9356409507741646, + "angularVelocity": -0.599054956474685, + "velocityX": -0.7422171782999981, + "velocityY": 1.6277224586627488, + "timestamp": 6.099300053698654 + }, + { + "x": 0.7551356460272671, + "y": 4.203804939056435, + "heading": -0.9665991521557302, + "angularVelocity": -0.5266199666669827, + "velocityX": -0.6317358929652714, + "velocityY": 1.3969683006521094, + "timestamp": 6.1580866613875 + }, + { + "x": 0.7243778481209882, + "y": 4.272315098083365, + "heading": -0.9929596722347582, + "angularVelocity": -0.44841029471460875, + "velocityX": -0.5232109678632522, + "velocityY": 1.1654041918790294, + "timestamp": 6.216873269076347 + }, + { + "x": 0.6999059920420889, + "y": 4.327174746307989, + "heading": -1.0144454160034293, + "angularVelocity": -0.3654870490638522, + "velocityX": -0.4162828412965571, + "velocityY": 0.9331997606494288, + "timestamp": 6.275659876765193 + }, + { + "x": 0.6816420121830558, + "y": 4.368353408669628, + "heading": -1.0308257655348128, + "angularVelocity": -0.2786408363293121, + "velocityX": -0.3106826635702974, + "velocityY": 0.7004769280036421, + "timestamp": 6.334446484454039 + }, + { + "x": 0.6695199611242603, + "y": 4.3958259235729, + "heading": -1.0419057024962997, + "angularVelocity": -0.18847722971415903, + "velocityX": -0.2062042961035709, + "velocityY": 0.4673260795840121, + "timestamp": 6.393233092142886 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -0.0954706943986077, + "velocityX": -0.10268631054285546, + "velocityY": 0.233815962076544, + "timestamp": 6.452019699831732 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": 7.129108746129837e-28, + "velocityX": 3.4723117091122335e-27, + "velocityY": 7.64903026201397e-27, + "timestamp": 6.510806307520578 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubGSub.traj b/src/main/deploy/choreo/SourceLanePSubHSubGSub.traj index 09e98b3d..b052fe12 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubGSub.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubGSub.traj @@ -1,1822 +1,1823 @@ { - "samples": [ - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": -1.7128958192175968e-32, - "velocityY": -9.692960229070848e-33, - "timestamp": 0 - }, - { - "x": 0.6705320996855912, - "y": 4.398099342118358, - "heading": -1.0386114127976835, - "angularVelocity": 0.15927551181553445, - "velocityX": 0.1260500242755718, - "velocityY": -0.20514712032687513, - "timestamp": 0.05592000838348859 - }, - { - "x": 0.6846335258864488, - "y": 4.375148415401169, - "heading": -1.0210111049652701, - "angularVelocity": 0.31474079388031545, - "velocityX": 0.25217138924859694, - "velocityY": -0.41042423598715305, - "timestamp": 0.11184001676697718 - }, - { - "x": 0.7057920861320249, - "y": 4.340710150429397, - "heading": -0.99496347376924, - "angularVelocity": 0.4658016325283906, - "velocityX": 0.3783719076090773, - "velocityY": -0.6158487090273924, - "timestamp": 0.16776002515046576 - }, - { - "x": 0.7340126708459819, - "y": 4.29477510174039, - "heading": -0.9607567098648512, - "angularVelocity": 0.6117088479280167, - "velocityX": 0.5046598798845959, - "velocityY": -0.8214420923185946, - "timestamp": 0.22368003353395435 - }, - { - "x": 0.7693006604451206, - "y": 4.237332365746024, - "heading": -0.918731573784226, - "angularVelocity": 0.7515223494321578, - "velocityX": 0.6310440684690269, - "velocityY": -1.0272304610620717, - "timestamp": 0.2796000419174429 - }, - { - "x": 0.8116619611973517, - "y": 4.168369327121192, - "heading": -0.8692948358062459, - "angularVelocity": 0.8840617054087768, - "velocityX": 0.7575338769931039, - "velocityY": -1.2332444257142754, - "timestamp": 0.3355200503009315 - }, - { - "x": 0.8611030812908155, - "y": 4.087871426229515, - "heading": -0.8129373545696883, - "angularVelocity": 1.0078231900479873, - "velocityX": 0.8841400694078274, - "velocityY": -1.4395187557848361, - "timestamp": 0.3914400586844201 - }, - { - "x": 0.9176312901451578, - "y": 3.995821968070834, - "heading": -0.7502611156342615, - "angularVelocity": 1.120819555419329, - "velocityX": 1.0108762585778373, - "velocityY": -1.6460916373156267, - "timestamp": 0.4473600670679087 - }, - { - "x": 0.9812548770105831, - "y": 3.8922019971306105, - "heading": -0.6820246305048665, - "angularVelocity": 1.2202516970570252, - "velocityX": 1.1377606818136925, - "velocityY": -1.8530034943775027, - "timestamp": 0.5032800754513973 - }, - { - "x": 1.0519833618359966, - "y": 3.77699034023748, - "heading": -0.6092267013635411, - "angularVelocity": 1.3018225720227254, - "velocityX": 1.2648153473148886, - "velocityY": -2.060293984633039, - "timestamp": 0.5592000838348858 - }, - { - "x": 1.129826908295346, - "y": 3.6501643798944907, - "heading": -0.5332724156478236, - "angularVelocity": 1.3582667083101592, - "velocityX": 1.3920517666147918, - "velocityY": -2.26798893650447, - "timestamp": 0.6151200922183744 - }, - { - "x": 1.214791954900265, - "y": 3.511704401985981, - "heading": -0.45632702327380104, - "angularVelocity": 1.3759903583409006, - "velocityX": 1.5194033238021893, - "velocityY": -2.4760364297333126, - "timestamp": 0.6710401006018629 - }, - { - "x": 1.306860167939052, - "y": 3.361616595640326, - "heading": -0.38215505844470404, - "angularVelocity": 1.3263940219829728, - "velocityX": 1.6464270249639614, - "velocityY": -2.6839732447173734, - "timestamp": 0.7269601089853515 - }, - { - "x": 1.4058792704184802, - "y": 3.2000739494751453, - "heading": -0.3184323584737964, - "angularVelocity": 1.1395330904442935, - "velocityX": 1.770727604337509, - "velocityY": -2.888816558419535, - "timestamp": 0.78288011736884 - }, - { - "x": 1.5121691709784744, - "y": 3.0300749520186443, - "heading": -0.28662541962876287, - "angularVelocity": 0.5687935278354703, - "velocityX": 1.9007490097476158, - "velocityY": -3.040038840671852, - "timestamp": 0.8388001257523285 - }, - { - "x": 1.6244650109400063, - "y": 2.8508645326617055, - "heading": -0.2866253125429205, - "angularVelocity": 0.0000019149825875479786, - "velocityX": 2.00815134345883, - "velocityY": -3.2047638141959642, - "timestamp": 0.8947201341358171 - }, - { - "x": 1.735288205529194, - "y": 2.670739688820617, - "heading": -0.28662527835532886, - "angularVelocity": 6.113659969296124e-7, - "velocityX": 1.9818164873864772, - "velocityY": -3.2211161809173365, - "timestamp": 0.9506401425193056 - }, - { - "x": 1.8506362591615513, - "y": 2.493478394875359, - "heading": -0.2866252441530595, - "angularVelocity": 6.116284727574992e-7, - "velocityX": 2.062733124811472, - "velocityY": -3.1699082147776885, - "timestamp": 1.0065601509027942 - }, - { - "x": 1.9771720781105007, - "y": 2.324022128559085, - "heading": -0.2866252095630549, - "angularVelocity": 6.185622213406466e-7, - "velocityX": 2.2628004288052175, - "velocityY": -3.0303333496335663, - "timestamp": 1.0624801592862827 - }, - { - "x": 2.1143701177449694, - "y": 2.1630766803808767, - "heading": -0.28662517392184667, - "angularVelocity": 6.373605666632096e-7, - "velocityX": 2.4534695827223683, - "velocityY": -2.878137053815794, - "timestamp": 1.1184001676697712 - }, - { - "x": 2.261659606829455, - "y": 2.011311910787294, - "heading": -0.2866251364727607, - "angularVelocity": 6.696902775127292e-7, - "velocityX": 2.633931813357441, - "velocityY": -2.7139618533818717, - "timestamp": 1.1743201760532598 - }, - { - "x": 2.418427669547749, - "y": 1.8693594092228303, - "heading": -0.2866250962912339, - "angularVelocity": 7.185536621676126e-7, - "velocityX": 2.803434177677683, - "velocityY": -2.538492136678184, - "timestamp": 1.2302401844367483 - }, - { - "x": 2.5840219554137547, - "y": 1.7378099138847216, - "heading": -0.28662505217416817, - "angularVelocity": 7.889316720181228e-7, - "velocityX": 2.961270762521925, - "velocityY": -2.3524584337678864, - "timestamp": 1.2861601928202369 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.2866250016379229, - "angularVelocity": 9.037238488587988e-7, - "velocityX": 3.1067845266977008, - "velocityY": -2.1566350283233997, - "timestamp": 1.3420802012037254 - }, - { - "x": 2.964172022537395, - "y": 1.4952512181839386, - "heading": -0.28662495601958665, - "angularVelocity": 7.195928192095116e-7, - "velocityX": 3.256089340971223, - "velocityY": -1.9238160187086888, - "timestamp": 1.4054748531095118 - }, - { - "x": 3.1789518158723995, - "y": 1.3887033110695066, - "heading": -0.2866249156918973, - "angularVelocity": 6.361370893313497e-7, - "velocityX": 3.3879796935268014, - "velocityY": -1.6807081340674839, - "timestamp": 1.4688695050152982 - }, - { - "x": 3.4009440542760783, - "y": 1.2981369637223683, - "heading": -0.28662487900587236, - "angularVelocity": 5.786927430417384e-7, - "velocityX": 3.501750253847766, - "velocityY": -1.4286117933375984, - "timestamp": 1.5322641569210846 - }, - { - "x": 3.6289614625982565, - "y": 1.2240364953903842, - "heading": -0.2866248447654741, - "angularVelocity": 5.401149339588049e-7, - "velocityX": 3.596792496960864, - "velocityY": -1.168875703302355, - "timestamp": 1.595658808826871 - }, - { - "x": 3.8617845298582507, - "y": 1.1667980667810436, - "heading": -0.2866248120410035, - "angularVelocity": 5.162023862741612e-7, - "velocityX": 3.672597928386812, - "velocityY": -0.9028904945231835, - "timestamp": 1.6590534607326575 - }, - { - "x": 4.0981679647792335, - "y": 1.1267270015307347, - "heading": -0.28662478005254766, - "angularVelocity": 5.045923416827531e-7, - "velocityX": 3.728759884544863, - "velocityY": -0.6320890492444092, - "timestamp": 1.7224481126384439 - }, - { - "x": 4.3358126512762265, - "y": 1.0949807955483843, - "heading": -0.28662474820722456, - "angularVelocity": 5.023345368252209e-7, - "velocityX": 3.7486551207847545, - "velocityY": -0.5007710434238176, - "timestamp": 1.7858427645442303 - }, - { - "x": 4.573457419312962, - "y": 1.06323519995946, - "heading": -0.28662471636190573, - "angularVelocity": 5.023344693474633e-7, - "velocityX": 3.7486564070090496, - "velocityY": -0.5007614149550472, - "timestamp": 1.8492374164500167 - }, - { - "x": 4.811102187354235, - "y": 1.0314896044045172, - "heading": -0.28662468451658685, - "angularVelocity": 5.02334469511756e-7, - "velocityX": 3.748656407080655, - "velocityY": -0.5007614144190154, - "timestamp": 1.9126320683558031 - }, - { - "x": 5.04874695539551, - "y": 0.9997440088495764, - "heading": -0.286624652671268, - "angularVelocity": 5.023344701827313e-7, - "velocityX": 3.748656407080659, - "velocityY": -0.5007614144189856, - "timestamp": 1.9760267202615895 - }, - { - "x": 5.286391723436784, - "y": 0.9679984132946354, - "heading": -0.2866246208259492, - "angularVelocity": 5.023344694262712e-7, - "velocityX": 3.7486564070806585, - "velocityY": -0.500761414418986, - "timestamp": 2.039421372167376 - }, - { - "x": 5.524036491478058, - "y": 0.9362528177396949, - "heading": -0.28662458898063026, - "angularVelocity": 5.023344704301764e-7, - "velocityX": 3.74865640708066, - "velocityY": -0.5007614144189806, - "timestamp": 2.1028160240731624 - }, - { - "x": 5.761681259520114, - "y": 0.9045072221906024, - "heading": -0.2866245571353113, - "angularVelocity": 5.02334470827004e-7, - "velocityX": 3.748656407092983, - "velocityY": -0.5007614143267314, - "timestamp": 2.166210675978949 - }, - { - "x": 5.99932604159473, - "y": 0.8727617316882137, - "heading": -0.28662452528999255, - "angularVelocity": 5.023344683092169e-7, - "velocityX": 3.748656628445413, - "velocityY": -0.5007597572988834, - "timestamp": 2.229605327884735 - }, - { - "x": 6.237212534637086, - "y": 0.8428813906316717, - "heading": -0.286624491558187, - "angularVelocity": 5.320922916664829e-7, - "velocityX": 3.7524694259050357, - "velocityY": -0.47133851450037906, - "timestamp": 2.2929999797905216 - }, - { - "x": 6.4638245505635705, - "y": 0.8220107849523657, - "heading": -0.24424436180847475, - "angularVelocity": 0.6685126974542769, - "velocityX": 3.5746235544169, - "velocityY": -0.32921713507194117, - "timestamp": 2.356394631696308 - }, - { - "x": 6.673647733933235, - "y": 0.8041360764949451, - "heading": -0.18514255243378552, - "angularVelocity": 0.9322838377994884, - "velocityX": 3.309793130207448, - "velocityY": -0.2819592492436893, - "timestamp": 2.4197892836020944 - }, - { - "x": 6.866141134149738, - "y": 0.7888964331390943, - "heading": -0.12766912670980685, - "angularVelocity": 0.9065973863125305, - "velocityX": 3.0364296424022634, - "velocityY": -0.24039320191392657, - "timestamp": 2.483183935507881 - }, - { - "x": 7.04134978279503, - "y": 0.7761674362974819, - "heading": -0.07636429138531158, - "angularVelocity": 0.8092927996630003, - "velocityX": 2.7637764918352543, - "velocityY": -0.20078975842520036, - "timestamp": 2.5465785874136673 - }, - { - "x": 7.19932046714385, - "y": 0.765888430525111, - "heading": -0.03338684229285238, - "angularVelocity": 0.6779349330023905, - "velocityX": 2.4918613731579065, - "velocityY": -0.16214310613530766, - "timestamp": 2.6099732393194537 - }, - { - "x": 7.340087890625, - "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": 0.5266507708326882, - "velocityX": 2.220493673351972, - "velocityY": -0.12406863156475333, - "timestamp": 2.67336789122524 - }, - { - "x": 7.464023522460179, - "y": 0.7525372143128384, - "heading": 0.023010119360387892, - "angularVelocity": 0.36178963616626764, - "velocityX": 1.9486481772395672, - "velocityY": -0.08625561847872552, - "timestamp": 2.736968716290925 - }, - { - "x": 7.57058231953554, - "y": 0.7491648354718851, - "heading": 0.03698370022131657, - "angularVelocity": 0.21970754068830903, - "velocityX": 1.6754310492876554, - "velocityY": -0.05302413667543652, - "timestamp": 2.8005695413566096 - }, - { - "x": 7.659708944521439, - "y": 0.7476928551810322, - "heading": 0.04295335113137195, - "angularVelocity": 0.09386121805637215, - "velocityX": 1.4013438488235506, - "velocityY": -0.02314404395434409, - "timestamp": 2.8641703664222944 - }, - { - "x": 7.731365798341117, - "y": 0.7479588271083956, - "heading": 0.041693436231508264, - "angularVelocity": -0.019809725716020507, - "velocityX": 1.1266654755763426, - "velocityY": 0.004181894292859409, - "timestamp": 2.927771191487979 - }, - { - "x": 7.785525986892143, - "y": 0.7498349370538775, - "heading": 0.03380693253041854, - "angularVelocity": -0.1240000219013676, - "velocityX": 0.8515642445690118, - "velocityY": 0.029498201376229376, - "timestamp": 2.991372016553664 - }, - { - "x": 7.822169492500932, - "y": 0.7532180353995639, - "heading": 0.01977708435408761, - "angularVelocity": -0.22059223542841888, - "velocityX": 0.5761482743493486, - "velocityY": 0.05319268016086978, - "timestamp": 3.0549728416193487 - }, - { - "x": 7.841280937194824, - "y": 0.7580231428146362, - "heading": 2.046055081284657e-30, - "angularVelocity": -0.310956411865142, - "velocityX": 0.30049051524338544, - "velocityY": 0.07555102327854848, - "timestamp": 3.1185736666850334 - }, - { - "x": 7.837626456422163, - "y": 0.7661832239405314, - "heading": -0.033360531548023535, - "angularVelocity": -0.41786111882102345, - "velocityX": -0.045774613098587845, - "velocityY": 0.10221002096529477, - "timestamp": 3.1984100765480727 - }, - { - "x": 7.806322741144082, - "y": 0.7764707746744537, - "heading": -0.07501113915617402, - "angularVelocity": -0.5216994060680163, - "velocityX": -0.3920982335225698, - "velocityY": 0.12885788265743264, - "timestamp": 3.278246486411112 - }, - { - "x": 7.74736425619444, - "y": 0.7888847549403224, - "heading": -0.12465356813987538, - "angularVelocity": -0.6218018704606533, - "velocityX": -0.738491185297363, - "velocityY": 0.15549271675874554, - "timestamp": 3.3580828962741514 - }, - { - "x": 7.660744366778727, - "y": 0.8034239689194149, - "heading": -0.18191677738707485, - "angularVelocity": -0.7172568173523243, - "velocityX": -1.084967241942757, - "velocityY": 0.18211257249711932, - "timestamp": 3.4379193061371907 - }, - { - "x": 7.5464550145644145, - "y": 0.8200870575241468, - "heading": -0.24632578527266594, - "angularVelocity": -0.8067623280666757, - "velocityX": -1.4315442341455176, - "velocityY": 0.20871540482992282, - "timestamp": 3.51775571600023 - }, - { - "x": 7.404486266909035, - "y": 0.8388724767291542, - "heading": -0.3172478637527482, - "angularVelocity": -0.8883425319569115, - "velocityX": -1.7782456387872365, - "velocityY": 0.23529889729803788, - "timestamp": 3.5975921258632693 - }, - { - "x": 7.234825726560123, - "y": 0.8597784078771571, - "heading": -0.3937913075224082, - "angularVelocity": -0.958753579989022, - "velocityX": -2.1251023266197273, - "velocityY": 0.2618596099682753, - "timestamp": 3.6774285357263086 - }, - { - "x": 7.037458048630302, - "y": 0.8828023770067691, - "heading": -0.474592846440749, - "angularVelocity": -1.0120888333650895, - "velocityX": -2.472151218578179, - "velocityY": 0.28838933475478196, - "timestamp": 3.757264945589348 - }, - { - "x": 6.812366694532574, - "y": 0.9079395818644894, - "heading": -0.5572950195818104, - "angularVelocity": -1.0358954427301792, - "velocityX": -2.8194072664825045, - "velocityY": 0.3148589083707988, - "timestamp": 3.837101355452387 - }, - { - "x": 6.559557151686243, - "y": 0.9351738572850871, - "heading": -0.6368741954392075, - "angularVelocity": -0.996777986308708, - "velocityX": -3.1665945810943685, - "velocityY": 0.3411260033776375, - "timestamp": 3.9169377653154265 - }, - { - "x": 6.2794552371906684, - "y": 0.9644007497338529, - "heading": -0.6963848347023115, - "angularVelocity": -0.7454072567290441, - "velocityX": -3.5084482753682624, - "velocityY": 0.3660847537972336, - "timestamp": 3.996774175178466 - }, - { - "x": 5.978414020607044, - "y": 0.9876514129078968, - "heading": -0.6963848502753711, - "angularVelocity": -1.950621226343756e-7, - "velocityX": -3.770725876828189, - "velocityY": 0.2912288167006853, - "timestamp": 4.0766105850415055 - }, - { - "x": 5.677372687056552, - "y": 1.010900561597095, - "heading": -0.6963848658464019, - "angularVelocity": -1.9503671222417687e-7, - "velocityX": -3.770727341909935, - "velocityY": 0.29120984684910667, - "timestamp": 4.156446994904545 - }, - { - "x": 5.376331353502739, - "y": 1.034149710243266, - "heading": -0.6963848814174328, - "angularVelocity": -1.9503671215554748e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.2912098463101618, - "timestamp": 4.236283404767585 - }, - { - "x": 5.075290019948924, - "y": 1.0573988588894354, - "heading": -0.6963848969884637, - "angularVelocity": -1.9503671305455908e-7, - "velocityX": -3.770727341951559, - "velocityY": 0.2912098463101465, - "timestamp": 4.316119814630625 - }, - { - "x": 4.774248686395111, - "y": 1.0806480075356062, - "heading": -0.6963849125594946, - "angularVelocity": -1.95036712978891e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.29120984631016333, - "timestamp": 4.3959562244936645 - }, - { - "x": 4.473207352844907, - "y": 1.1038971562285251, - "heading": -0.6963849281305255, - "angularVelocity": -1.9503671242042344e-7, - "velocityX": -3.770727341906336, - "velocityY": 0.29120984689571133, - "timestamp": 4.475792634356704 - }, - { - "x": 4.172166146375442, - "y": 1.1271479503662634, - "heading": -0.6963849437015556, - "angularVelocity": -1.9503670063864683e-7, - "velocityX": -3.770725750142127, - "velocityY": 0.2912304571012814, - "timestamp": 4.555629044219744 - }, - { - "x": 3.8735860999949274, - "y": 1.1720518831992321, - "heading": -0.696384959370356, - "angularVelocity": -1.9626133629188135e-7, - "velocityX": -3.7398982105123864, - "velocityY": 0.5624492998871312, - "timestamp": 4.635465454082784 - }, - { - "x": 3.580403442801602, - "y": 1.244234542261993, - "heading": -0.6963849756495779, - "angularVelocity": -2.0390723854783083e-7, - "velocityX": -3.672292600535076, - "velocityY": 0.9041320769131684, - "timestamp": 4.7153018639458235 - }, - { - "x": 3.295105029858622, - "y": 1.3430840304472003, - "heading": -0.6963849932202171, - "angularVelocity": -2.2008303364523734e-7, - "velocityX": -3.573537605616451, - "velocityY": 1.2381504673717785, - "timestamp": 4.795138273808863 - }, - { - "x": 3.020110821637561, - "y": 1.4677619739384067, - "heading": -0.6963850130039023, - "angularVelocity": -2.4780279138256264e-7, - "velocityX": -3.4444711215448884, - "velocityY": 1.5616677115753714, - "timestamp": 4.874974683671903 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.6963850355061774, - "angularVelocity": -2.8185479639389566e-7, - "velocityX": -3.2861879673103584, - "velocityY": 1.8719390230438342, - "timestamp": 4.954811093534943 - }, - { - "x": 2.5741841985169076, - "y": 1.73878733799422, - "heading": -0.6963850589279512, - "angularVelocity": -4.0231077616908087e-7, - "velocityX": -3.153128257888759, - "velocityY": 2.0882929565548642, - "timestamp": 5.0130292054104135 - }, - { - "x": 2.3991894720533877, - "y": 1.872411165939459, - "heading": -0.6963850794155536, - "angularVelocity": -3.519111451084776e-7, - "velocityX": -3.0058468202788267, - "velocityY": 2.2952277846293376, - "timestamp": 5.071247317285884 - }, - { - "x": 2.2335584908852413, - "y": 2.017479635272907, - "heading": -0.6963850978369928, - "angularVelocity": -3.164211012595542e-7, - "velocityX": -2.845007779064253, - "velocityY": 2.491809930967045, - "timestamp": 5.129465429161355 - }, - { - "x": 2.0780383093475274, - "y": 2.173338406648706, - "heading": -0.6963851148088384, - "angularVelocity": -2.9152174806156023e-7, - "velocityX": -2.6713367460348776, - "velocityY": 2.6771526309404154, - "timestamp": 5.187683541036826 - }, - { - "x": 1.9333303562340614, - "y": 2.339284459115452, - "heading": -0.6963851307944149, - "angularVelocity": -2.745808111537906e-7, - "velocityX": -2.4856174213103777, - "velocityY": 2.850419691069807, - "timestamp": 5.245901652912297 - }, - { - "x": 1.8000872249814166, - "y": 2.514569234135199, - "heading": -0.6963851461633945, - "angularVelocity": -2.639896616028141e-7, - "velocityX": -2.288688639330214, - "velocityY": 3.0108289220145816, - "timestamp": 5.304119764787767 - }, - { - "x": 1.6789094155739503, - "y": 2.6984018293834224, - "heading": -0.6963851612308567, - "angularVelocity": -2.588105615203023e-7, - "velocityX": -2.0814451981312545, - "velocityY": 3.1576529936498847, - "timestamp": 5.362337876663238 - }, - { - "x": 1.5665736982883989, - "y": 2.8877670089616614, - "heading": -0.6963851762457395, - "angularVelocity": -2.5790741626665006e-7, - "velocityX": -1.9295664814042677, - "velocityY": 3.2526850060560912, - "timestamp": 5.420555988538709 - }, - { - "x": 1.4540456075762054, - "y": 3.0767463777174253, - "heading": -0.6971250426595835, - "angularVelocity": -0.012708526436355243, - "velocityX": -1.9328708384238367, - "velocityY": 3.246058016446739, - "timestamp": 5.47877410041418 - }, - { - "x": 1.3486317039814464, - "y": 3.2544802087721734, - "heading": -0.7253513181524348, - "angularVelocity": -0.48483666995638414, - "velocityX": -1.8106719747325546, - "velocityY": 3.052895831368142, - "timestamp": 5.53699221228965 - }, - { - "x": 1.2507109072486164, - "y": 3.4195717896549707, - "heading": -0.7601787682651484, - "angularVelocity": -0.598223628193407, - "velocityX": -1.6819644879978937, - "velocityY": 2.8357426162485435, - "timestamp": 5.595210324165121 - }, - { - "x": 1.160328320301069, - "y": 3.57195134860905, - "heading": -0.7969704460064531, - "angularVelocity": -0.6319627441714823, - "velocityX": -1.5524822780387875, - "velocityY": 2.617390946652855, - "timestamp": 5.653428436040592 - }, - { - "x": 1.0774888591394964, - "y": 3.71161238889488, - "heading": -0.8336344675120992, - "angularVelocity": -0.6297700204374731, - "velocityX": -1.422915626991949, - "velocityY": 2.398927683957996, - "timestamp": 5.711646547916063 - }, - { - "x": 1.0021908033919844, - "y": 3.838558158350536, - "heading": -0.8689739346571361, - "angularVelocity": -0.6070184347549549, - "velocityX": -1.2933785264038766, - "velocityY": 2.1805202086799973, - "timestamp": 5.7698646597915335 - }, - { - "x": 0.9344313462846833, - "y": 3.952793536595294, - "heading": -0.9022118432658017, - "angularVelocity": -0.5709204152783613, - "velocityX": -1.1638896371672067, - "velocityY": 1.9621965495739466, - "timestamp": 5.828082771667004 - }, - { - "x": 0.8742077702690397, - "y": 4.05432328906152, - "heading": -0.932802055793642, - "angularVelocity": -0.5254415085338586, - "velocityX": -1.0344474266781911, - "velocityY": 1.74395474527581, - "timestamp": 5.886300883542475 - }, - { - "x": 0.8215176865940929, - "y": 4.143151679881797, - "heading": -0.9603390876580661, - "angularVelocity": -0.47299768022924404, - "velocityX": -0.9050462472512313, - "velocityY": 1.5257861850670003, - "timestamp": 5.944518995417946 - }, - { - "x": 0.7763590444591775, - "y": 4.21928243186123, - "heading": -0.9845096003709082, - "angularVelocity": -0.4151717040316119, - "velocityX": -0.7756802939866984, - "velocityY": 1.307681570681621, - "timestamp": 6.002737107293417 - }, - { - "x": 0.7387300854642734, - "y": 4.282718776761074, - "heading": -1.0050639989905084, - "angularVelocity": -0.35305848914451915, - "velocityX": -0.6463445443815323, - "velocityY": 1.0896324675649904, - "timestamp": 6.060955219168887 - }, - { - "x": 0.7086292911290722, - "y": 4.333463522832413, - "heading": -1.0217986916878445, - "angularVelocity": -0.28744822115035373, - "velocityX": -0.5170348773863939, - "velocityY": 0.871631601173907, - "timestamp": 6.119173331044358 - }, - { - "x": 0.6860553360496396, - "y": 4.371519118466469, - "heading": -1.0345444282293665, - "angularVelocity": -0.21893077825652071, - "velocityX": -0.3877479765698806, - "velocityY": 0.6536727902728674, - "timestamp": 6.177391442919829 - }, - { - "x": 0.6710070489868055, - "y": 4.396887706764773, - "heading": -1.0431583071392123, - "angularVelocity": -0.14795874741302198, - "velocityX": -0.2584811938769611, - "velocityY": 0.43575079096636016, - "timestamp": 6.2356095547953 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -0.07488723824993972, - "velocityX": -0.12923242394972312, - "velocityY": 0.2178611369128848, - "timestamp": 6.29382766667077 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -7.364462273467936e-32, - "velocityX": -8.426727085890477e-35, - "velocityY": -7.327805300908389e-34, - "timestamp": 6.352045778546241 - }, - { - "x": 0.6687542994396359, - "y": 4.396916362187497, - "heading": -1.0350125956919525, - "angularVelocity": 0.2204003026897336, - "velocityX": 0.09289604490221867, - "velocityY": -0.22303166776742134, - "timestamp": 6.408785741736511 - }, - { - "x": 0.6793565646010321, - "y": 4.371607473302922, - "heading": -1.0104451675948027, - "angularVelocity": 0.4329827993501856, - "velocityX": 0.18685710327027932, - "velocityY": -0.4460504988292779, - "timestamp": 6.465525704926781 - }, - { - "x": 0.6953596015342451, - "y": 4.333646306767502, - "heading": -0.974331857670356, - "angularVelocity": 0.6364704503481122, - "velocityX": 0.2820417221553139, - "velocityY": -0.6690375601429702, - "timestamp": 6.522265668117051 - }, - { - "x": 0.7168434967725004, - "y": 4.283036502515157, - "heading": -0.9272707047547418, - "angularVelocity": 0.8294181079709282, - "velocityX": 0.3786378071168583, - "velocityY": -0.8919604703061096, - "timestamp": 6.5790056313073215 - }, - { - "x": 0.7439015517455688, - "y": 4.2197848874550585, - "heading": -0.8699519201955506, - "angularVelocity": 1.0102012996903078, - "velocityX": 0.4768782609592592, - "velocityY": -1.114763061230607, - "timestamp": 6.635745594497592 - }, - { - "x": 0.7766442528059767, - "y": 4.143903696796417, - "heading": -0.8031709855452435, - "angularVelocity": 1.1769647157923862, - "velocityX": 0.5770659552705253, - "velocityY": -1.3373500156174554, - "timestamp": 6.692485557687862 - }, - { - "x": 0.8152053931020986, - "y": 4.055414010177326, - "heading": -0.7278520306671347, - "angularVelocity": 1.3274410246890034, - "velocityX": 0.6796116551364678, - "velocityY": -1.559565456931146, - "timestamp": 6.749225520878132 - }, - { - "x": 0.8597512927978146, - "y": 3.9543509740987406, - "heading": -0.6450962807712558, - "angularVelocity": 1.4585090515192565, - "velocityX": 0.7850886252135366, - "velocityY": -1.7811614670894944, - "timestamp": 6.805965484068402 - }, - { - "x": 0.9104943772436287, - "y": 3.840772171273386, - "heading": -0.5562797497368643, - "angularVelocity": 1.5653258486713548, - "velocityX": 0.8943094354089278, - "velocityY": -2.0017426244088754, - "timestamp": 6.862705447258672 - }, - { - "x": 0.9677131149809225, - "y": 3.7147727983516208, - "heading": -0.46323645073147074, - "angularVelocity": 1.6398195165087535, - "velocityX": 1.0084380482486026, - "velocityY": -2.2206460109824513, - "timestamp": 6.919445410448942 - }, - { - "x": 1.0317820620609501, - "y": 3.576517638239885, - "heading": -0.3685846112186535, - "angularVelocity": 1.6681688564973882, - "velocityX": 1.1291679352201949, - "velocityY": -2.436645220373414, - "timestamp": 6.976185373639212 - }, - { - "x": 1.1032183238971194, - "y": 3.4263179326907856, - "heading": -0.2763023679431359, - "angularVelocity": 1.6264064706221368, - "velocityX": 1.2590114236876868, - "velocityY": -2.64715902344566, - "timestamp": 7.032925336829482 - }, - { - "x": 1.1827463587467486, - "y": 3.2648378432524847, - "heading": -0.1927401719055838, - "angularVelocity": 1.4727220699339676, - "velocityX": 1.4016229545821517, - "velocityY": -2.8459674691151595, - "timestamp": 7.089665300019752 - }, - { - "x": 1.2713975131080564, - "y": 3.0937718159398226, - "heading": -0.12815970051626668, - "angularVelocity": 1.1381831738726118, - "velocityX": 1.562411206789599, - "velocityY": -3.014912553591437, - "timestamp": 7.146405263210022 - }, - { - "x": 1.3706976001434645, - "y": 2.9168916321975784, - "heading": -0.09351951840137682, - "angularVelocity": 0.610507659279375, - "velocityX": 1.7500907905494714, - "velocityY": -3.117382772158287, - "timestamp": 7.203145226400292 - }, - { - "x": 1.4825331259415389, - "y": 2.737551336103052, - "heading": -0.084031813825935, - "angularVelocity": 0.1672137950394149, - "velocityX": 1.9710186526390192, - "velocityY": -3.1607404377956887, - "timestamp": 7.2598851895905625 - }, - { - "x": 1.6073609242840539, - "y": 2.563006200971128, - "heading": -0.08403157292142222, - "angularVelocity": 0.000004245764347455466, - "velocityX": 2.199997873173106, - "velocityY": -3.0762292627263235, - "timestamp": 7.316625152780833 - }, - { - "x": 1.7433400400906298, - "y": 2.3970011200993575, - "heading": -0.08403150231700539, - "angularVelocity": 0.0000012443507689652163, - "velocityX": 2.3965316182984986, - "velocityY": -2.925717105509803, - "timestamp": 7.373365115971103 - }, - { - "x": 1.889887864003564, - "y": 2.2402473630588187, - "heading": -0.08403142284388228, - "angularVelocity": 0.0000014006551755862827, - "velocityX": 2.5827973032253166, - "velocityY": -2.7626693467333516, - "timestamp": 7.430105079161373 - }, - { - "x": 2.046376539489404, - "y": 2.0934165181715283, - "heading": -0.08403133019563887, - "angularVelocity": 0.0000016328569530158415, - "velocityX": 2.757997479855154, - "velocityY": -2.58778533914292, - "timestamp": 7.486845042351643 - }, - { - "x": 2.2121356179273546, - "y": 1.9571376609651308, - "heading": -0.08403121735664319, - "angularVelocity": 0.00000198870406896459, - "velocityX": 2.921381494064391, - "velocityY": -2.401814339381988, - "timestamp": 7.543585005541913 - }, - { - "x": 2.3864549305613383, - "y": 1.8319946594051495, - "heading": -0.08403107170612155, - "angularVelocity": 0.0000025669830126630976, - "velocityX": 3.0722493077661266, - "velocityY": -2.2055530973879893, - "timestamp": 7.600324968732183 - }, - { - "x": 2.5685876291674634, - "y": 1.7185236734546225, - "heading": -0.08403086720089675, - "angularVelocity": 0.000003604253744603396, - "velocityX": 3.2099544723948052, - "velocityY": -1.9998424315154402, - "timestamp": 7.657064931922453 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.08403051576382121, - "angularVelocity": 0.000006193819237400721, - "velocityX": 3.3339066927233554, - "velocityY": -1.7855635206200389, - "timestamp": 7.713804895112723 - }, - { - "x": 2.9656624873675215, - "y": 1.523823440642391, - "heading": -0.08403020563552344, - "angularVelocity": 0.0000051460695851667095, - "velocityX": 3.449910187925088, - "velocityY": -1.5496108793286638, - "timestamp": 7.7740699756075236 - }, - { - "x": 3.179557704009846, - "y": 1.4451070785194118, - "heading": -0.08403001029754076, - "angularVelocity": 0.0000032413128973800796, - "velocityX": 3.5492397070767696, - "velocityY": -1.306168704607808, - "timestamp": 7.834335056102324 - }, - { - "x": 3.3984052007772028, - "y": 1.3814422389869163, - "heading": -0.0840298717822531, - "angularVelocity": 0.0000022984336290020716, - "velocityX": 3.631414659542976, - "velocityY": -1.0564134156924954, - "timestamp": 7.894600136597124 - }, - { - "x": 3.6211472313814954, - "y": 1.3331366287340103, - "heading": -0.08402976608446186, - "angularVelocity": 0.000001753881192375512, - "velocityX": 3.6960380501525916, - "velocityY": -0.8015522398094749, - "timestamp": 7.954865217091925 - }, - { - "x": 3.846707230492903, - "y": 1.3004237167488812, - "heading": -0.08402968105378755, - "angularVelocity": 0.0000014109443416028125, - "velocityX": 3.7427976078264336, - "velocityY": -0.5428170296387713, - "timestamp": 8.015130297586726 - }, - { - "x": 4.073995015295796, - "y": 1.2834616061839175, - "heading": -0.08402960975207145, - "angularVelocity": 0.0000011831348355128416, - "velocityX": 3.7714673727599504, - "velocityY": -0.2814583574052856, - "timestamp": 8.075395378081527 - }, - { - "x": 4.301912054355543, - "y": 1.282332269070522, - "heading": -0.08402954787131725, - "angularVelocity": 0.0000010268094506712622, - "velocityX": 3.78190881333695, - "velocityY": -0.018739493984294066, - "timestamp": 8.135660458576329 - }, - { - "x": 4.529356777936335, - "y": 1.2970411469267544, - "heading": -0.08402949255215464, - "angularVelocity": 9.179306183548953e-7, - "velocityX": 3.7740715139410477, - "velocityY": 0.2440696624889034, - "timestamp": 8.19592553907113 - }, - { - "x": 4.755229905146536, - "y": 1.32751711580047, - "heading": -0.08402944177914254, - "angularVelocity": 8.424947198098048e-7, - "velocityX": 3.7479934541809494, - "velocityY": 0.5056986338273413, - "timestamp": 8.256190619565931 - }, - { - "x": 4.978439767588294, - "y": 1.373612800948183, - "heading": -0.08402939404329389, - "angularVelocity": 7.920979819424461e-7, - "velocityX": 3.7038009508842378, - "velocityY": 0.7648821634228096, - "timestamp": 8.316455700060732 - }, - { - "x": 5.197907642868858, - "y": 1.435105133457297, - "heading": -0.08402934813807361, - "angularVelocity": 7.617217119968608e-7, - "velocityX": 3.6417088217363065, - "velocityY": 1.0203642308985088, - "timestamp": 8.376720780555534 - }, - { - "x": 5.41257465992254, - "y": 1.51169186312203, - "heading": -0.08402930302434115, - "angularVelocity": 7.485882719571497e-7, - "velocityX": 3.562046466895548, - "velocityY": 1.2708309527826855, - "timestamp": 8.436985861050335 - }, - { - "x": 5.626738902818376, - "y": 1.589673479175761, - "heading": -0.0840292579538456, - "angularVelocity": 7.478708262765028e-7, - "velocityX": 3.5537037557647206, - "velocityY": 1.2939768007189265, - "timestamp": 8.497250941545136 - }, - { - "x": 5.8409031120271635, - "y": 1.6676551877456105, - "heading": -0.0840292128833514, - "angularVelocity": 7.478708040156826e-7, - "velocityX": 3.5537031967834927, - "velocityY": 1.2939783358719235, - "timestamp": 8.557516022039938 - }, - { - "x": 6.055067321252931, - "y": 1.745636896268829, - "heading": -0.08402916781285703, - "angularVelocity": 7.478708068018038e-7, - "velocityX": 3.5537031970652393, - "velocityY": 1.2939783350981522, - "timestamp": 8.617781102534739 - }, - { - "x": 6.269231822230628, - "y": 1.8236178035375321, - "heading": -0.08402912274221092, - "angularVelocity": 7.478733249018271e-7, - "velocityX": 3.5537080382091872, - "velocityY": 1.2939650395958724, - "timestamp": 8.67804618302954 - }, - { - "x": 6.484552473362613, - "y": 1.8927933712391167, - "heading": -0.0785738032137305, - "angularVelocity": 0.09052206491205224, - "velocityX": 3.572892450555367, - "velocityY": 1.1478548959633867, - "timestamp": 8.738311263524341 - }, - { - "x": 6.6874523544155435, - "y": 1.9584473392764923, - "heading": -0.032553192185028265, - "angularVelocity": 0.7636364317587296, - "velocityX": 3.3667901774467204, - "velocityY": 1.0894197352485022, - "timestamp": 8.798576344019143 - }, - { - "x": 6.875915672715089, - "y": 2.018652626020107, - "heading": 0.028144572860875648, - "angularVelocity": 1.0071796892586995, - "velocityX": 3.1272391366971615, - "velocityY": 0.9990078209355214, - "timestamp": 8.858841424513944 - }, - { - "x": 7.049532373282227, - "y": 2.073323850727038, - "heading": 0.08948102802542592, - "angularVelocity": 1.0177777024597514, - "velocityX": 2.8808839072589665, - "velocityY": 0.9071791534676087, - "timestamp": 8.919106505008745 - }, - { - "x": 7.208331847820926, - "y": 2.1225088918275805, - "heading": 0.1460274817443076, - "angularVelocity": 0.9382954980664153, - "velocityX": 2.635016384859868, - "velocityY": 0.8161449498899447, - "timestamp": 8.979371585503547 - }, - { - "x": 7.352376622997373, - "y": 2.1662517609495984, - "heading": 0.19476972653717045, - "angularVelocity": 0.8087974726436816, - "velocityX": 2.3901863897597297, - "velocityY": 0.72584104696901, - "timestamp": 9.039636665998348 - }, - { - "x": 7.481724262237549, - "y": 2.2045881748199463, - "heading": 0.2337432969576531, - "angularVelocity": 0.6467023706015796, - "velocityX": 2.146311565141514, - "velocityY": 0.6361298044504436, - "timestamp": 9.099901746493149 - }, - { - "x": 7.603777233505286, - "y": 2.239565945400217, - "heading": 0.2627306680277325, - "angularVelocity": 0.44772760939813167, - "velocityX": 1.8851825132237892, - "velocityY": 0.540252980034664, - "timestamp": 9.164645064666104 - }, - { - "x": 7.70877923669531, - "y": 2.2685458571337813, - "heading": 0.28047750188422, - "angularVelocity": 0.2741106628035178, - "velocityX": 1.621819921393637, - "velocityY": 0.4476123954003698, - "timestamp": 9.229388382839058 - }, - { - "x": 7.796631047750002, - "y": 2.291686366419086, - "heading": 0.28817114093339696, - "angularVelocity": 0.11883294317143692, - "velocityX": 1.3569247535321998, - "velocityY": 0.3574192663942211, - "timestamp": 9.294131701012013 - }, - { - "x": 7.867260828377704, - "y": 2.309110963970804, - "heading": 0.2867085865097153, - "angularVelocity": -0.022590044269504023, - "velocityX": 1.0909199994819943, - "velocityY": 0.2691335267242617, - "timestamp": 9.358875019184968 - }, - { - "x": 7.920614376230513, - "y": 2.320918360587503, - "heading": 0.2767910958513383, - "angularVelocity": -0.15318168636157245, - "velocityX": 0.824078057140675, - "velocityY": 0.1823724354867997, - "timestamp": 9.423618337357922 - }, - { - "x": 7.956649448780364, - "y": 2.327189146390105, - "heading": 0.25898192892634386, - "angularVelocity": -0.27507343502876075, - "velocityX": 0.5565836532132435, - "velocityY": 0.09685610777394495, - "timestamp": 9.488361655530877 - }, - { - "x": 7.975332260131836, - "y": 2.3279902935028076, - "heading": 0.2337432969576531, - "angularVelocity": -0.38982604971324913, - "velocityX": 0.2885674055438917, - "velocityY": 0.012374205328225838, - "timestamp": 9.553104973703832 - }, - { - "x": 7.972208451820267, - "y": 2.320885188130861, - "heading": 0.19238106068409147, - "angularVelocity": -0.5227816803364386, - "velocityX": -0.039482143745094074, - "velocityY": -0.08980217850764118, - "timestamp": 9.632224494967359 - }, - { - "x": 7.9431197679460075, - "y": 2.3056970040477323, - "heading": 0.14088004342725263, - "angularVelocity": -0.6509268058549238, - "velocityX": -0.3676549530345614, - "velocityY": -0.19196506551829257, - "timestamp": 9.711344016230886 - }, - { - "x": 7.888054782175393, - "y": 2.282427289475609, - "heading": 0.07969868788669623, - "angularVelocity": -0.7732776255909873, - "velocityX": -0.695972180964121, - "velocityY": -0.2941083843849025, - "timestamp": 9.790463537494412 - }, - { - "x": 7.806999942498959, - "y": 2.251078456184475, - "heading": 0.009399898258955654, - "angularVelocity": -0.8885138396324784, - "velocityX": -1.0244606941750847, - "velocityY": -0.39622122063553056, - "timestamp": 9.86958305875794 - }, - { - "x": 7.699938927127902, - "y": 2.2116544317191673, - "heading": -0.06930512925958275, - "angularVelocity": -0.9947611697041394, - "velocityX": -1.353155500201559, - "velocityY": -0.49828441623143704, - "timestamp": 9.948702580021466 - }, - { - "x": 7.56685172715319, - "y": 2.1641617607796966, - "heading": -0.15547855671843705, - "angularVelocity": -1.089155066697531, - "velocityX": -1.682103200946216, - "velocityY": -0.6002648926714905, - "timestamp": 10.027822101284993 - }, - { - "x": 7.407713494064462, - "y": 2.1086115226344657, - "heading": -0.24780538645569025, - "angularVelocity": -1.1669285691167814, - "velocityX": -2.011364964642267, - "velocityY": -0.702105337066011, - "timestamp": 10.10694162254852 - }, - { - "x": 7.222493943520295, - "y": 2.045023086580113, - "heading": -0.3442810817864959, - "angularVelocity": -1.2193665202986854, - "velocityX": -2.3410094953335943, - "velocityY": -0.8037009708710834, - "timestamp": 10.186061143812047 - }, - { - "x": 7.0111623875747595, - "y": 1.9734335261753837, - "heading": -0.4414827777687788, - "angularVelocity": -1.2285425193427022, - "velocityX": -2.6710418942202843, - "velocityY": -0.9048280280448349, - "timestamp": 10.265180665075574 - }, - { - "x": 6.773734962048751, - "y": 1.893931948844204, - "heading": -0.5324651154460041, - "angularVelocity": -1.1499353917244444, - "velocityX": -3.0008703507595227, - "velocityY": -1.0048288470601288, - "timestamp": 10.344300186339101 - }, - { - "x": 6.510837595414292, - "y": 1.8068437015263887, - "heading": -0.5976366951472135, - "angularVelocity": -0.8237104909184032, - "velocityX": -3.3227876311184024, - "velocityY": -1.1007175716817783, - "timestamp": 10.423419707602628 - }, - { - "x": 6.227373100797986, - "y": 1.7110085398670838, - "heading": -0.597636758184297, - "angularVelocity": -7.967323658608173e-7, - "velocityX": -3.5827377376583796, - "velocityY": -1.2112707474568936, - "timestamp": 10.502539228866155 - }, - { - "x": 5.941831354312318, - "y": 1.6215523108704284, - "heading": -0.5976367779578059, - "angularVelocity": -2.499194721356179e-7, - "velocityX": -3.6089923438060088, - "velocityY": -1.1306467426502582, - "timestamp": 10.581658750129682 - }, - { - "x": 5.656289545112644, - "y": 1.532096282055343, - "heading": -0.5976367977313131, - "angularVelocity": -2.499194483006569e-7, - "velocityX": -3.6089931364549517, - "velocityY": -1.130644212534215, - "timestamp": 10.660778271393209 - }, - { - "x": 5.370747722448539, - "y": 1.442640296218458, - "heading": -0.5976368175048198, - "angularVelocity": -2.499194443736624e-7, - "velocityX": -3.6089933066333257, - "velocityY": -1.130643669328191, - "timestamp": 10.739897792656736 - }, - { - "x": 5.084751687474933, - "y": 1.3546472631644115, - "heading": -0.5976368372766676, - "angularVelocity": -2.4989847693362694e-7, - "velocityX": -3.6147341440682292, - "velocityY": -1.112153254327249, - "timestamp": 10.819017313920263 - }, - { - "x": 4.791925527807106, - "y": 1.2930896001142946, - "heading": -0.5976368572220272, - "angularVelocity": -2.52091508667939e-7, - "velocityX": -3.7010608126974813, - "velocityY": -0.7780338160172059, - "timestamp": 10.89813683518379 - }, - { - "x": 4.494706489260304, - "y": 1.2584868813594625, - "heading": -0.5976368780658494, - "angularVelocity": -2.63447274854176e-7, - "velocityX": -3.756582873610133, - "velocityY": -0.4373474232683845, - "timestamp": 10.977256356447317 - }, - { - "x": 4.195570491935266, - "y": 1.2511276025347224, - "heading": -0.597636900694303, - "angularVelocity": -2.860034177860552e-7, - "velocityX": -3.780811518420208, - "velocityY": -0.09301470366874595, - "timestamp": 11.056375877710844 - }, - { - "x": 3.897009479956395, - "y": 1.2710731343763437, - "heading": -0.597636926337917, - "angularVelocity": -3.24112350243062e-7, - "velocityX": -3.7735442177972423, - "velocityY": 0.2520936871595517, - "timestamp": 11.13549539897437 - }, - { - "x": 3.6015106196507847, - "y": 1.3181573465478027, - "heading": -0.5976369569136154, - "angularVelocity": -3.8644948741127607e-7, - "velocityX": -3.7348413588269285, - "velocityY": 0.5951023390881358, - "timestamp": 11.214614920237898 - }, - { - "x": 3.311535572783971, - "y": 1.3919880157013045, - "heading": -0.5976369957684174, - "angularVelocity": -4.910899536256082e-7, - "velocityX": -3.6650252963611574, - "velocityY": 0.933153638627185, - "timestamp": 11.293734441501424 - }, - { - "x": 3.029499988052695, - "y": 1.4919500996437052, - "heading": -0.5976370495734, - "angularVelocity": -6.80046867328716e-7, - "velocityX": -3.5646775944445412, - "velocityY": 1.263431354816361, - "timestamp": 11.372853962764951 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.5976371219829064, - "angularVelocity": -9.151914125801383e-7, - "velocityX": -3.434634228323893, - "velocityY": 1.583184066038948, - "timestamp": 11.451973484028478 - }, - { - "x": 2.562615182382705, - "y": 1.7237517111402614, - "heading": -0.5976372044989885, - "angularVelocity": -0.000001403654433041047, - "velocityX": -3.319432744203681, - "velocityY": 1.8123319353797018, - "timestamp": 11.510760091514479 - }, - { - "x": 2.3751466995867516, - "y": 1.8432734201994623, - "heading": -0.5976372636960788, - "angularVelocity": -0.000001006982590926024, - "velocityX": -3.188965834447897, - "velocityY": 2.033145203823246, - "timestamp": 11.56954669900048 - }, - { - "x": 2.1962100914661193, - "y": 1.9752263102479175, - "heading": -0.5976373093137471, - "angularVelocity": -7.759874278787006e-7, - "velocityX": -3.04383286896162, - "velocityY": 2.2446080100791628, - "timestamp": 11.62833330648648 - }, - { - "x": 2.026628285101079, - "y": 2.1190035289778706, - "heading": -0.5976373463774368, - "angularVelocity": -6.304784584021761e-7, - "velocityX": -2.884701356604495, - "velocityY": 2.4457478476571914, - "timestamp": 11.68711991397248 - }, - { - "x": 1.8671811829950287, - "y": 2.2739438442752706, - "heading": -0.5976373777665476, - "angularVelocity": -5.339500279584693e-7, - "velocityX": -2.7123031745627153, - "velocityY": 2.6356396792296386, - "timestamp": 11.745906521458481 - }, - { - "x": 1.7186020763048488, - "y": 2.439334685025019, - "heading": -0.5976374052703787, - "angularVelocity": -4.678587919148875e-7, - "velocityX": -2.5274312134028762, - "velocityY": 2.813410193625064, - "timestamp": 11.804693128944482 - }, - { - "x": 1.5815742720495027, - "y": 2.6144154181152923, - "heading": -0.597637430079118, - "angularVelocity": -4.220134537614776e-7, - "velocityX": -2.330935737157126, - "velocityY": 2.978241823734581, - "timestamp": 11.863479736430483 - }, - { - "x": 1.456727954325337, - "y": 2.7983808405830617, - "heading": -0.5976374777093088, - "angularVelocity": -8.102217978593512e-7, - "velocityX": -2.123720402710731, - "velocityY": 3.129376406209191, - "timestamp": 11.922266343916483 - }, - { - "x": 1.3455561973341574, - "y": 2.9885105140912174, - "heading": -0.6039795556256176, - "angularVelocity": -0.10788303982023785, - "velocityX": -1.8911068650739042, - "velocityY": 3.234234490456602, - "timestamp": 11.981052951402484 - }, - { - "x": 1.246887053690835, - "y": 3.1733277865183314, - "heading": -0.6383468335999772, - "angularVelocity": -0.584610669743856, - "velocityX": -1.6784289460285728, - "velocityY": 3.1438669508378774, - "timestamp": 12.039839558888485 - }, - { - "x": 1.158565022655154, - "y": 3.3474027501759167, - "heading": -0.6816254514815488, - "angularVelocity": -0.736198595775039, - "velocityX": -1.5024175541464038, - "velocityY": 2.9611330046395477, - "timestamp": 12.098626166374485 - }, - { - "x": 1.0790333486785466, - "y": 3.5092497722088574, - "heading": -0.7278331515121171, - "angularVelocity": -0.7860242665231638, - "velocityX": -1.3528876282841755, - "velocityY": 2.7531274375968073, - "timestamp": 12.157412773860486 - }, - { - "x": 1.0074427197106477, - "y": 3.6582801720807683, - "heading": -0.7741804064083878, - "angularVelocity": -0.7883981892867038, - "velocityX": -1.217805075500369, - "velocityY": 2.5351080160120225, - "timestamp": 12.216199381346486 - }, - { - "x": 0.9432732996874974, - "y": 3.7941912605500616, - "heading": -0.8190469180938782, - "angularVelocity": -0.7632097446033984, - "velocityX": -1.0915652861654275, - "velocityY": 2.3119396454653414, - "timestamp": 12.274985988832487 - }, - { - "x": 0.8861770840921692, - "y": 3.9168021213870996, - "heading": -0.8613760150853045, - "angularVelocity": -0.7200466024767106, - "velocityX": -0.971245289310589, - "velocityY": 2.085693767347223, - "timestamp": 12.333772596318488 - }, - { - "x": 0.8359056858624994, - "y": 4.025993630774074, - "heading": -0.9004245404660403, - "angularVelocity": -0.6642418579781981, - "velocityX": -0.8551505245755496, - "velocityY": 1.857421512424882, - "timestamp": 12.392559203804488 - }, - { - "x": 0.7922732558571624, - "y": 4.121681912181887, - "heading": -0.9356409495537121, - "angularVelocity": -0.5990549649604897, - "velocityX": -0.7422171795800264, - "velocityY": 1.627722460946589, - "timestamp": 12.451345811290489 - }, - { - "x": 0.7551356458246514, - "y": 4.203804939460416, - "heading": -0.9665991512691937, - "angularVelocity": -0.5266199741642557, - "velocityX": -0.6317358939509348, - "velocityY": 1.3969683026544202, - "timestamp": 12.51013241877649 - }, - { - "x": 0.7243778479811152, - "y": 4.272315098350931, - "heading": -0.9929596716344966, - "angularVelocity": -0.44841030113160957, - "velocityX": -0.5232109686013307, - "velocityY": 1.1654041935798078, - "timestamp": 12.56891902626249 - }, - { - "x": 0.6999059919554825, - "y": 4.327174746467568, - "heading": -1.0144454156380707, - "angularVelocity": -0.3654870543208486, - "velocityX": -0.41628284182686576, - "velocityY": 0.9331997620325617, - "timestamp": 12.62770563374849 - }, - { - "x": 0.6816420121384978, - "y": 4.368353408748977, - "heading": -1.0308257653496888, - "angularVelocity": -0.27864084035669345, - "velocityX": -0.31068266392705546, - "velocityY": 0.7004769290559099, - "timestamp": 12.686492241234491 - }, - { - "x": 0.6695199611090151, - "y": 4.395825923599214, - "heading": -1.0419057024338247, - "angularVelocity": -0.1884772324508513, - "velocityX": -0.2062042963164626, - "velocityY": 0.46732608029439393, - "timestamp": 12.745278848720492 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -0.09547069579077468, - "velocityX": -0.1026863106378516, - "velocityY": 0.23381596243572259, - "timestamp": 12.804065456206493 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -7.821518056781921e-37, - "velocityX": -6.302822662985753e-37, - "velocityY": 5.041148502478119e-38, - "timestamp": 12.862852063692493 - } - ] + "samples": [ + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -7.591443414914956e-27, + "velocityX": -1.3210318643006072e-26, + "velocityY": -4.725695657383892e-27, + "timestamp": 0 + }, + { + "x": 0.6705320996373237, + "y": 4.398099342072708, + "heading": -1.0386114123729513, + "angularVelocity": 0.159275518837918, + "velocityX": 0.1260500229589734, + "velocityY": -0.20514712040522684, + "timestamp": 0.055920008584652126 + }, + { + "x": 0.6846335257451809, + "y": 4.375148415264061, + "heading": -1.0210111037335994, + "angularVelocity": 0.3147408071783192, + "velocityX": 0.25217138667835376, + "velocityY": -0.41042423614624113, + "timestamp": 0.11184001716930425 + }, + { + "x": 0.7057920858570316, + "y": 4.3407101501549405, + "heading": -0.9949634713965649, + "angularVelocity": 0.4658016512569599, + "velocityX": 0.37837190385657166, + "velocityY": -0.6158487092681121, + "timestamp": 0.16776002575395638 + }, + { + "x": 0.7340126704011022, + "y": 4.2947751012827355, + "heading": -0.9607567060719514, + "angularVelocity": 0.6117088711249232, + "velocityX": 0.5046598750311373, + "velocityY": -0.8214420926396748, + "timestamp": 0.2236800343386085 + }, + { + "x": 0.769300659799434, + "y": 4.23733236505955, + "heading": -0.9187315683545035, + "angularVelocity": 0.7515223759994586, + "velocityX": 0.6310440626079745, + "velocityY": -1.0272304614586787, + "timestamp": 0.2796000429232606 + }, + { + "x": 0.8116619603260306, + "y": 4.168369326160844, + "heading": -0.8692948285945653, + "angularVelocity": 0.8840617340947026, + "velocityX": 0.7575338702330436, + "velocityY": -1.233244426175458, + "timestamp": 0.33552005150791275 + }, + { + "x": 0.8611030801762559, + "y": 4.087871424951383, + "heading": -0.8129373455124671, + "angularVelocity": 1.0078232194257235, + "velocityX": 0.8841400618775134, + "velocityY": -1.439518756289218, + "timestamp": 0.3914400600925649 + }, + { + "x": 0.9176312887785422, + "y": 3.995821966433115, + "heading": -0.7502611047615702, + "angularVelocity": 1.1208195838528383, + "velocityX": 1.0108762504339304, + "velocityY": -1.646091637824456, + "timestamp": 0.447360068677217 + }, + { + "x": 0.9812548753941115, + "y": 3.892201995095196, + "heading": -0.6820246179563085, + "angularVelocity": 1.2202517226363587, + "velocityX": 1.1377606732526766, + "velocityY": -1.8530034948234757, + "timestamp": 0.5032800772618691 + }, + { + "x": 1.051983359986124, + "y": 3.776990337772613, + "heading": -0.6092266874108444, + "angularVelocity": 1.3018225924493927, + "velocityX": 1.2648153385910723, + "velocityY": -2.060293984901216, + "timestamp": 0.5592000858465211 + }, + { + "x": 1.1298269062475699, + "y": 3.6501643769794776, + "heading": -0.5332724007265813, + "angularVelocity": 1.358266720744203, + "velocityX": 1.3920517580680556, + "velocityY": -2.267988936395559, + "timestamp": 0.6151200944311732 + }, + { + "x": 1.2147919527166744, + "y": 3.511704398620439, + "heading": -0.4563270080164221, + "angularVelocity": 1.375990359402014, + "velocityX": 1.5194033159076472, + "velocityY": -2.476036428882822, + "timestamp": 0.6710401030158253 + }, + { + "x": 1.306860165723401, + "y": 3.3616165918639007, + "heading": -0.3821550436425617, + "angularVelocity": 1.3263940090706232, + "velocityX": 1.6464270184678684, + "velocityY": -2.6839732424098903, + "timestamp": 0.7269601116004774 + }, + { + "x": 1.4058792683735797, + "y": 3.2000739453958573, + "heading": -0.3184323444941022, + "angularVelocity": 1.1395330716374203, + "velocityX": 1.7707276010210737, + "velocityY": -2.888816553443462, + "timestamp": 0.7828801201851294 + }, + { + "x": 1.51216916955334, + "y": 3.030074948039413, + "heading": -0.28662540722206215, + "angularVelocity": 0.5687934976599732, + "velocityX": 1.9007490139930476, + "velocityY": -3.040038827946505, + "timestamp": 0.8388001287697815 + }, + { + "x": 1.6244649894914296, + "y": 2.850864516485835, + "heading": -0.28662530013608456, + "angularVelocity": 0.000001914984999387773, + "velocityX": 2.0081509781618743, + "velocityY": -3.2047640207760013, + "timestamp": 0.8947201373544336 + }, + { + "x": 1.7352882065392865, + "y": 2.6707396868109132, + "heading": -0.2866252659484935, + "angularVelocity": 6.113659837473025e-7, + "velocityX": 1.9818168818785473, + "velocityY": -3.221115916000747, + "timestamp": 0.9506401459390856 + }, + { + "x": 1.8506362599745876, + "y": 2.4934783930913484, + "heading": -0.28662523174622423, + "angularVelocity": 6.116284691333173e-7, + "velocityX": 2.06273311386722, + "velocityY": -3.169908199338448, + "timestamp": 1.0065601545237377 + }, + { + "x": 1.977172078744517, + "y": 2.3240221270116197, + "heading": -0.2866251971562196, + "angularVelocity": 6.18562220740454e-7, + "velocityX": 2.262800417463786, + "velocityY": -3.0303333345023407, + "timestamp": 1.0624801631083898 + }, + { + "x": 2.11437011822214, + "y": 2.163076679089509, + "heading": -0.2866251615150111, + "angularVelocity": 6.373605678250823e-7, + "velocityX": 2.4534695710915724, + "velocityY": -2.8781370388824357, + "timestamp": 1.1184001716930418 + }, + { + "x": 2.2616596071687796, + "y": 2.011311909775525, + "heading": -0.28662512406592494, + "angularVelocity": 6.696902799491419e-7, + "velocityX": 2.633931801417216, + "velocityY": -2.7139618386188316, + "timestamp": 1.174320180277694 + }, + { + "x": 2.4184276697644314, + "y": 1.869359408517574, + "heading": -0.2866250838843977, + "angularVelocity": 7.185536662845936e-7, + "velocityX": 2.803434165399588, + "velocityY": -2.5384921220650836, + "timestamp": 1.230240188862346 + }, + { + "x": 2.5840219555188257, + "y": 1.7378099135158744, + "heading": -0.2866250397673314, + "angularVelocity": 7.889316803983354e-7, + "velocityX": 2.9612707498732993, + "velocityY": -2.352458419289386, + "timestamp": 1.286160197446998 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.2866249892310852, + "angularVelocity": 9.037238633691851e-7, + "velocityX": 3.1067845136425762, + "velocityY": -2.1566350139692623, + "timestamp": 1.3420802060316501 + }, + { + "x": 2.9641720225317068, + "y": 1.495251218591247, + "heading": -0.2866249436127499, + "angularVelocity": 7.195928005652006e-7, + "velocityX": 3.256089327481367, + "velocityY": -1.9238160043664425, + "timestamp": 1.4054748581983314 + }, + { + "x": 3.1789518158633587, + "y": 1.3887033119472996, + "heading": -0.28662490328506157, + "angularVelocity": 6.361370710101161e-7, + "velocityX": 3.3879796795310004, + "velocityY": -1.6807081197291671, + "timestamp": 1.4688695103650127 + }, + { + "x": 3.4009440542565814, + "y": 1.2981369651359904, + "heading": -0.2866248665990377, + "angularVelocity": 5.78692723773973e-7, + "velocityX": 3.501750239271706, + "velocityY": -1.4286117790059998, + "timestamp": 1.532264162531694 + }, + { + "x": 3.628961462551479, + "y": 1.2240364974062796, + "heading": -0.28662483235864045, + "angularVelocity": 5.401149161418495e-7, + "velocityX": 3.596792481728267, + "velocityY": -1.168875688991578, + "timestamp": 1.5956588146983752 + }, + { + "x": 3.8617845297574473, + "y": 1.16679806946556, + "heading": -0.28662479963417076, + "angularVelocity": 5.162023699881254e-7, + "velocityX": 3.672597912420367, + "velocityY": -0.9028904802604593, + "timestamp": 1.6590534668650565 + }, + { + "x": 4.098167964587261, + "y": 1.1267270049464149, + "heading": -0.286624767645716, + "angularVelocity": 5.045923223989343e-7, + "velocityX": 3.7287598677613913, + "velocityY": -0.6320890351095811, + "timestamp": 1.7224481190317378 + }, + { + "x": 4.335812648113985, + "y": 1.0949807783310654, + "heading": -0.28662473580039416, + "angularVelocity": 5.023345147080502e-7, + "velocityX": 3.7486550585038954, + "velocityY": -0.500771366832023, + "timestamp": 1.785842771198419 + }, + { + "x": 4.573457416127468, + "y": 1.063235184169841, + "heading": -0.28662470395507683, + "angularVelocity": 5.023344436089823e-7, + "velocityX": 3.74865639121502, + "velocityY": -0.5007613903733815, + "timestamp": 1.8492374233651003 + }, + { + "x": 4.811102184145877, + "y": 1.031489590045499, + "heading": -0.2866246721097595, + "angularVelocity": 5.023344446091564e-7, + "velocityX": 3.748656391292737, + "velocityY": -0.5007613897915972, + "timestamp": 1.9126320755317816 + }, + { + "x": 5.048746952164287, + "y": 0.9997439959211589, + "heading": -0.2866246402644421, + "angularVelocity": 5.023344444502557e-7, + "velocityX": 3.7486563912927418, + "velocityY": -0.5007613897915636, + "timestamp": 1.9760267276984629 + }, + { + "x": 5.2863917201826975, + "y": 0.9679984017968188, + "heading": -0.2866246084191247, + "angularVelocity": 5.023344456533015e-7, + "velocityX": 3.748656391292741, + "velocityY": -0.5007613897915631, + "timestamp": 2.039421379865144 + }, + { + "x": 5.524036488201108, + "y": 0.9362528076724792, + "heading": -0.2866245765738072, + "angularVelocity": 5.023344456596004e-7, + "velocityX": 3.748656391292742, + "velocityY": -0.5007613897915576, + "timestamp": 2.1028160320318254 + }, + { + "x": 5.761681256220374, + "y": 0.904507213554553, + "heading": -0.28662454472848975, + "angularVelocity": 5.023344455217454e-7, + "velocityX": 3.7486563913062563, + "velocityY": -0.500761389690393, + "timestamp": 2.1662106841985067 + }, + { + "x": 5.999326038930599, + "y": 0.8727617294120621, + "heading": -0.28662451288317237, + "angularVelocity": 5.023344437091617e-7, + "velocityX": 3.748656623044388, + "velocityY": -0.5007596549157438, + "timestamp": 2.229605336365188 + }, + { + "x": 6.23721253164048, + "y": 0.8428813874103429, + "heading": -0.28662447915136585, + "angularVelocity": 5.320923033369603e-7, + "velocityX": 3.7524694052175755, + "velocityY": -0.4713385274700439, + "timestamp": 2.2929999885318693 + }, + { + "x": 6.46382454823366, + "y": 0.8220107822516065, + "heading": -0.24424435205194292, + "angularVelocity": 0.6685126528968796, + "velocityX": 3.57462355022247, + "velocityY": -0.32921712550550913, + "timestamp": 2.3563946406985505 + }, + { + "x": 6.673647732212354, + "y": 0.804136074402253, + "heading": -0.1851425455819509, + "angularVelocity": 0.932283788143483, + "velocityX": 3.3097931261932545, + "velocityY": -0.28195923849154, + "timestamp": 2.419789292865232 + }, + { + "x": 6.866141132976172, + "y": 0.7888964316418866, + "heading": -0.12766912233999997, + "angularVelocity": 0.9065973434295085, + "velocityX": 3.036429638539596, + "velocityY": -0.24039319153132044, + "timestamp": 2.483183945031913 + }, + { + "x": 7.0413497820944775, + "y": 0.7761674353552135, + "heading": -0.07636428899971823, + "angularVelocity": 0.8092927650330481, + "velocityX": 2.763776487922594, + "velocityY": -0.2007897488451438, + "timestamp": 2.5465785971985944 + }, + { + "x": 7.199320466835695, + "y": 0.7658884300843406, + "heading": -0.03338684136743644, + "angularVelocity": 0.6779349071792803, + "velocityX": 2.4918613690926352, + "velocityY": -0.16214309755729506, + "timestamp": 2.6099732493652756 + }, + { + "x": 7.340087890625, + "y": 0.7580231428146362, + "heading": 2.6436628934070276e-25, + "angularVelocity": 0.5266507540676091, + "velocityX": 2.2204936690746413, + "velocityY": -0.1240686241013575, + "timestamp": 2.673367901531957 + }, + { + "x": 7.46402352318357, + "y": 0.752537214674761, + "heading": 0.023010119033746678, + "angularVelocity": 0.36178962784078056, + "velocityX": 1.9486481714334611, + "velocityY": -0.08625561202772893, + "timestamp": 2.736968727158371 + }, + { + "x": 7.570582320737423, + "y": 0.749164836112825, + "heading": 0.036983699836062915, + "angularVelocity": 0.2197075378297117, + "velocityX": 1.6754310420397782, + "velocityY": -0.05302413182094548, + "timestamp": 2.800569552784785 + }, + { + "x": 7.659708945959918, + "y": 0.7476928559896275, + "heading": 0.042953350833826356, + "angularVelocity": 0.09386121860789494, + "velocityX": 1.4013438401887508, + "velocityY": -0.023144041114242904, + "timestamp": 2.864170378411199 + }, + { + "x": 7.731365799776358, + "y": 0.7479588279518706, + "heading": 0.041693436078494114, + "angularVelocity": -0.019809723268890925, + "velocityX": 1.1266654655923272, + "velocityY": 0.00418189480440736, + "timestamp": 2.927771204037613 + }, + { + "x": 7.785525988085828, + "y": 0.7498349377827495, + "heading": 0.03380693250994457, + "angularVelocity": -0.12400001872419436, + "velocityX": 0.8515642332632949, + "velocityY": 0.029498199314249407, + "timestamp": 2.991372029664027 + }, + { + "x": 7.822169493215899, + "y": 0.7532180358509439, + "heading": 0.01977708439947626, + "angularVelocity": -0.22059223244802648, + "velocityX": 0.5761482617428887, + "velocityY": 0.05319267532887989, + "timestamp": 3.054972855290441 + }, + { + "x": 7.841280937194824, + "y": 0.7580231428146362, + "heading": -3.944931722509772e-25, + "angularVelocity": -0.31095640983727535, + "velocityX": 0.30049050135267197, + "velocityY": 0.07555101551538161, + "timestamp": 3.118573680916855 + }, + { + "x": 7.8376264553511055, + "y": 0.7661832230205181, + "heading": -0.033360531574413245, + "angularVelocity": -0.41786111810448695, + "velocityX": -0.04577462639954243, + "velocityY": 0.10221000918544447, + "timestamp": 3.19841009097995 + }, + { + "x": 7.806322738977973, + "y": 0.7764707725075406, + "heading": -0.07501113928889683, + "angularVelocity": -0.52169940609262, + "velocityX": -0.3920982462562205, + "velocityY": 0.12885786671635327, + "timestamp": 3.2782465010430455 + }, + { + "x": 7.747364252907164, + "y": 0.788884751185466, + "heading": -0.12465356840345063, + "angularVelocity": -0.6218018705415377, + "velocityX": -0.7384911974901414, + "velocityY": 0.15549269647914413, + "timestamp": 3.3580829111061408 + }, + { + "x": 7.660744362341635, + "y": 0.8034239632178009, + "heading": -0.18191677773821885, + "angularVelocity": -0.7172568166518608, + "velocityX": -1.0849672536261623, + "velocityY": 0.1821125476564444, + "timestamp": 3.437919321169236 + }, + { + "x": 7.5464550089458, + "y": 0.820087049493991, + "heading": -0.2463257855831559, + "angularVelocity": -0.8067623255358589, + "velocityX": -1.4315442453576273, + "velocityY": 0.2087153751405061, + "timestamp": 3.5177557312323313 + }, + { + "x": 7.404486260073429, + "y": 0.8388724659578224, + "heading": -0.3172478637844774, + "angularVelocity": -0.8883425262392362, + "velocityX": -1.7782456495748211, + "velocityY": 0.23529886237351122, + "timestamp": 3.5975921412954266 + }, + { + "x": 7.234825718467376, + "y": 0.8597783939083566, + "heading": -0.39379130688880964, + "angularVelocity": -0.9587535692529209, + "velocityX": -2.125102337041081, + "velocityY": 0.26185956926184595, + "timestamp": 3.677428551358522 + }, + { + "x": 7.037458039234421, + "y": 0.8828023593175933, + "heading": -0.4745928445431507, + "angularVelocity": -1.0120888149966063, + "velocityX": -2.4721512287059744, + "velocityY": 0.2883892874321469, + "timestamp": 3.757264961421617 + }, + { + "x": 6.812366683780793, + "y": 0.9079395598178155, + "heading": -0.5572950154989933, + "angularVelocity": -1.035895412763206, + "velocityX": -2.819407276401054, + "velocityY": 0.31485885300148303, + "timestamp": 3.8371013714847124 + }, + { + "x": 6.5595571395215275, + "y": 0.9351738300036528, + "heading": -0.6368741877579803, + "angularVelocity": -0.9967779387386686, + "velocityX": -3.16659459085733, + "velocityY": 0.34112593695425386, + "timestamp": 3.9169377815478077 + }, + { + "x": 6.279455223533108, + "y": 0.9644007155460906, + "heading": -0.6963848224506938, + "angularVelocity": -0.7454071976142457, + "velocityX": -3.5084482852755343, + "velocityY": 0.36608466637389636, + "timestamp": 3.996774191610903 + }, + { + "x": 5.978414008635392, + "y": 0.98765139076885, + "heading": -0.6963848380237664, + "angularVelocity": -1.9506228543282429e-7, + "velocityX": -3.7707258462623967, + "velocityY": 0.29122896688846905, + "timestamp": 4.076610601673998 + }, + { + "x": 5.677372675666473, + "y": 1.0109005372081505, + "heading": -0.6963848535947982, + "angularVelocity": -1.9503672258290444e-7, + "velocityX": -3.7707273251766207, + "velocityY": 0.2912098179380391, + "timestamp": 4.156447011737093 + }, + { + "x": 5.376331342694041, + "y": 1.0341496836019548, + "heading": -0.69638486916583, + "angularVelocity": -1.9503672329128174e-7, + "velocityX": -3.7707273252206313, + "velocityY": 0.291209817368169, + "timestamp": 4.236283421800188 + }, + { + "x": 5.075290009721609, + "y": 1.0573988299957575, + "heading": -0.6963848847368618, + "angularVelocity": -1.9503672272562427e-7, + "velocityX": -3.770727325220632, + "velocityY": 0.29120981736815194, + "timestamp": 4.316119831863283 + }, + { + "x": 4.774248676749177, + "y": 1.080647976389562, + "heading": -0.6963849003078936, + "angularVelocity": -1.9503672313945863e-7, + "velocityX": -3.770727325220631, + "velocityY": 0.29120981736817053, + "timestamp": 4.395956241926378 + }, + { + "x": 4.473207343780594, + "y": 1.1038971228332064, + "heading": -0.6963849158789254, + "angularVelocity": -1.950367233647788e-7, + "velocityX": -3.770727325172418, + "velocityY": 0.2912098179924497, + "timestamp": 4.475792651989472 + }, + { + "x": 4.1721661401549985, + "y": 1.1271479440124157, + "heading": -0.6963849314499564, + "angularVelocity": -1.9503671206893558e-7, + "velocityX": -3.77072570507217, + "velocityY": 0.2912307950825182, + "timestamp": 4.555629062052567 + }, + { + "x": 3.8735860947396503, + "y": 1.1720518781992464, + "heading": -0.6963849471187581, + "angularVelocity": -1.9626135080534044e-7, + "velocityX": -3.739898189051575, + "velocityY": 0.5624493154356899, + "timestamp": 4.635465472115662 + }, + { + "x": 3.5804034386486023, + "y": 1.2442345385889582, + "heading": -0.6963849633979813, + "angularVelocity": -2.0390725479039878e-7, + "velocityX": -3.6722925775262745, + "velocityY": 0.9041320912684484, + "timestamp": 4.715301882178757 + }, + { + "x": 3.295105026948141, + "y": 1.3430840280599874, + "heading": -0.6963849809686221, + "angularVelocity": -2.2008305287856005e-7, + "velocityX": -3.573537581098501, + "velocityY": 1.238150480374907, + "timestamp": 4.795138292241852 + }, + { + "x": 3.020110820111431, + "y": 1.467761972780782, + "heading": -0.6963850007523091, + "angularVelocity": -2.47802813909356e-7, + "velocityX": -3.444471095573825, + "velocityY": 1.561667723063461, + "timestamp": 4.874974702304947 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.6963850232545865, + "angularVelocity": -2.8185482385848004e-7, + "velocityX": -3.286187939960048, + "velocityY": 1.871939032853051, + "timestamp": 4.954811112368041 + }, + { + "x": 2.5741841996274863, + "y": 1.7387873388514183, + "heading": -0.6963850466763624, + "angularVelocity": -4.023108112528609e-7, + "velocityX": -3.153128229528534, + "velocityY": 2.088292965130013, + "timestamp": 5.013029224414929 + }, + { + "x": 2.399189474356015, + "y": 1.8724111676120014, + "heading": -0.6963850671639665, + "angularVelocity": -3.519111736959287e-7, + "velocityX": -3.0058467909528623, + "velocityY": 2.295227791876272, + "timestamp": 5.071247336461816 + }, + { + "x": 2.233558494460658, + "y": 2.017479637711712, + "heading": -0.6963850855854071, + "angularVelocity": -3.164211270787494e-7, + "velocityX": -2.845007748824996, + "velocityY": 2.4918099367921025, + "timestamp": 5.129465448508704 + }, + { + "x": 2.0780383142751306, + "y": 2.1733384097973816, + "heading": -0.6963851025572542, + "angularVelocity": -2.9152176872861595e-7, + "velocityX": -2.671336714943196, + "velocityY": 2.6771526352511037, + "timestamp": 5.187683560555591 + }, + { + "x": 1.93333036259127, + "y": 2.339284462910256, + "heading": -0.6963851185428317, + "angularVelocity": -2.745808302376555e-7, + "velocityX": -2.4856173894357143, + "velocityY": 2.850419693775459, + "timestamp": 5.2459016726024785 + }, + { + "x": 1.800087232842991, + "y": 2.514569238505031, + "heading": -0.6963851339118123, + "angularVelocity": -2.639896791762275e-7, + "velocityX": -2.2886886067512457, + "velocityY": 3.0108289230266427, + "timestamp": 5.304119784649366 + }, + { + "x": 1.6789094250105836, + "y": 2.698401834249409, + "heading": -0.6963851489792755, + "angularVelocity": -2.5881057704594286e-7, + "velocityX": -2.0814451649482146, + "velocityY": 3.157652992874852, + "timestamp": 5.362337896696253 + }, + { + "x": 1.5665736783823851, + "y": 2.8877669958946757, + "heading": -0.6963851639941595, + "angularVelocity": -2.5790743404475406e-7, + "velocityX": -1.9295669797352, + "velocityY": 3.2526846884480856, + "timestamp": 5.420556008743141 + }, + { + "x": 1.4540455902705487, + "y": 3.076746366139197, + "heading": -0.6971250291266823, + "angularVelocity": -0.012708504389955933, + "velocityX": -1.9328707880669334, + "velocityY": 3.2460580324611277, + "timestamp": 5.478774120790028 + }, + { + "x": 1.348631689048656, + "y": 3.2544801987728227, + "heading": -0.7253513053237864, + "angularVelocity": -0.48483668062563473, + "velocityX": -1.8106719286430082, + "velocityY": 3.0528958494992513, + "timestamp": 5.536992232836916 + }, + { + "x": 1.250710894485043, + "y": 3.4195717811136617, + "heading": -0.7601787566205287, + "angularVelocity": -0.5982236467698062, + "velocityX": -1.681964445785358, + "velocityY": 2.8357426329434743, + "timestamp": 5.595210344883803 + }, + { + "x": 1.1603283095237442, + "y": 3.5719513414037465, + "heading": -0.7969704357290219, + "angularVelocity": -0.6319627657946384, + "velocityX": -1.5524822393502977, + "velocityY": 2.617390961894501, + "timestamp": 5.6534284569306905 + }, + { + "x": 1.0774888501734665, + "y": 3.7116123829065053, + "heading": -0.8336344586576223, + "angularVelocity": -0.6297700430249606, + "velocityX": -1.4229155916901037, + "velocityY": 2.398927697797529, + "timestamp": 5.711646568977578 + }, + { + "x": 1.0021907960664256, + "y": 3.838558153462584, + "heading": -0.8689739272109267, + "angularVelocity": -0.6070184571571581, + "velocityX": -1.2933784944176396, + "velocityY": 2.1805202211614, + "timestamp": 5.769864681024465 + }, + { + "x": 0.9344313404312847, + "y": 3.9527935326932564, + "heading": -0.9022118371683866, + "angularVelocity": -0.5709204367652957, + "velocityX": -1.1638896084532742, + "velocityY": 1.962196560731307, + "timestamp": 5.828082793071353 + }, + { + "x": 0.8742077657211834, + "y": 4.054323286032448, + "heading": -0.9328020509544388, + "angularVelocity": -0.5254415285987893, + "velocityX": -1.0344474012073521, + "velocityY": 1.7439547551356767, + "timestamp": 5.88630090511824 + }, + { + "x": 0.8215176831863799, + "y": 4.143151677613985, + "heading": -0.9603390839635252, + "angularVelocity": -0.47299769849816853, + "velocityX": -0.9050462250024212, + "velocityY": 1.5257861936504657, + "timestamp": 5.944519017165128 + }, + { + "x": 0.7763590420271295, + "y": 4.219282430243976, + "heading": -0.9845095976897603, + "angularVelocity": -0.4151717202160187, + "velocityX": -0.7756802749439998, + "velocityY": 1.3076815780057875, + "timestamp": 6.002737129212015 + }, + { + "x": 0.7387300838441316, + "y": 4.282718775684509, + "heading": -1.0050639971773583, + "angularVelocity": -0.3530585030143863, + "velocityX": -0.6463445285325005, + "velocityY": 1.0896324736439786, + "timestamp": 6.0609552412589025 + }, + { + "x": 0.7086292901576546, + "y": 4.3334635221873645, + "heading": -1.0217986905857463, + "angularVelocity": -0.2874482325175743, + "velocityX": -0.517034864721044, + "velocityY": 0.8716316060195561, + "timestamp": 6.11917335330579 + }, + { + "x": 0.6860553355642359, + "y": 4.371519118144359, + "heading": -1.0345444276717368, + "angularVelocity": -0.21893078696412344, + "velocityX": -0.3877479670800402, + "velocityY": 0.6536727938952351, + "timestamp": 6.177391465352677 + }, + { + "x": 0.6710070488250978, + "y": 4.396887706657532, + "heading": -1.0431583069512933, + "angularVelocity": -0.14795875332781483, + "velocityX": -0.2584811875558354, + "velocityY": 0.435750793374088, + "timestamp": 6.235609577399565 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -0.074887241257288, + "velocityX": -0.12923242079159178, + "velocityY": 0.21786113811346933, + "timestamp": 6.293827689446452 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": 1.6934356345077546e-25, + "velocityX": 7.869408079103222e-27, + "velocityY": 8.248206791285532e-26, + "timestamp": 6.35204580149334 + }, + { + "x": 0.6687542994417671, + "y": 4.396916362162236, + "heading": -1.0350125954694533, + "angularVelocity": 0.22040030578546457, + "velocityX": 0.09289604459177633, + "velocityY": -0.22303166737709929, + "timestamp": 6.408785764896168 + }, + { + "x": 0.679356564606483, + "y": 4.37160747322566, + "heading": -1.010445166940442, + "angularVelocity": 0.4329828053394013, + "velocityX": 0.18685710262878952, + "velocityY": -0.44605049807478747, + "timestamp": 6.465525728298996 + }, + { + "x": 0.6953596015431592, + "y": 4.333646306609793, + "heading": -0.9743318563908197, + "angularVelocity": 0.6364704589820466, + "velocityX": 0.28204172115977494, + "velocityY": -0.6690375590544457, + "timestamp": 6.522265691701824 + }, + { + "x": 0.7168434967838145, + "y": 4.283036502246609, + "heading": -0.9272707026758202, + "angularVelocity": 0.8294181189523645, + "velocityX": 0.37863780574071754, + "velocityY": -0.8919604689181454, + "timestamp": 6.579005655104652 + }, + { + "x": 0.7439015517568099, + "y": 4.219784887043078, + "heading": -0.8699519171651382, + "angularVelocity": 1.010201312675245, + "velocityX": 0.47687825917150645, + "velocityY": -1.114763059582432, + "timestamp": 6.63574561850748 + }, + { + "x": 0.7766442528130582, + "y": 4.143903696205975, + "heading": -0.8031709814362636, + "angularVelocity": 1.176964730392239, + "velocityX": 0.5770659530354297, + "velocityY": -1.3373500137527523, + "timestamp": 6.692485581910308 + }, + { + "x": 0.8152053930991828, + "y": 4.055414009370765, + "heading": -0.7278520253810473, + "angularVelocity": 1.3274410404618378, + "velocityX": 0.679611652414335, + "velocityY": -1.5595654548977111, + "timestamp": 6.749225545313136 + }, + { + "x": 0.8597512927773133, + "y": 3.954350973035596, + "heading": -0.6450962742441403, + "angularVelocity": 1.4585090679276476, + "velocityX": 0.7850886219625312, + "velocityY": -1.7811614649390577, + "timestamp": 6.805965508715964 + }, + { + "x": 0.9104943771963883, + "y": 3.8407721699102697, + "heading": -0.556279741952017, + "angularVelocity": 1.56532586497398, + "velocityX": 0.8943094315874387, + "velocityY": -2.001742622196794, + "timestamp": 6.862705472118792 + }, + { + "x": 0.9677131148965982, + "y": 3.714772796642351, + "heading": -0.4632364417450351, + "angularVelocity": 1.6398195315428243, + "velocityX": 1.0084380438172502, + "velocityY": -2.2206460087642403, + "timestamp": 6.91944543552162 + }, + { + "x": 1.0317820619285505, + "y": 3.5765176361367876, + "heading": -0.36858460120873987, + "angularVelocity": 1.6681688682861946, + "velocityX": 1.1291679301428568, + "velocityY": -2.4366452181862606, + "timestamp": 6.976185398924448 + }, + { + "x": 1.1032183237055362, + "y": 3.4263179301504665, + "heading": -0.2763023573010561, + "angularVelocity": 1.6264064756708094, + "velocityX": 1.2590114179281549, + "velocityY": -2.6471590212346836, + "timestamp": 7.032925362327276 + }, + { + "x": 1.1827463584827187, + "y": 3.264837840257316, + "heading": -0.19274016139718564, + "angularVelocity": 1.472722062060866, + "velocityX": 1.401622948054619, + "velocityY": -2.845967466470068, + "timestamp": 7.089665325730104 + }, + { + "x": 1.2713975127609685, + "y": 3.0937718125950266, + "heading": -0.12815969147628775, + "angularVelocity": 1.1381831437289833, + "velocityX": 1.5624111994727097, + "velocityY": -3.0149125484589985, + "timestamp": 7.146405289132932 + }, + { + "x": 1.3706975997688013, + "y": 2.916891628537348, + "heading": -0.09351951086762286, + "angularVelocity": 0.6105076304462074, + "velocityX": 1.7500907835073471, + "velocityY": -3.117382766039356, + "timestamp": 7.20314525253576 + }, + { + "x": 1.4825331256479723, + "y": 2.7375513323263876, + "heading": -0.08403180748655616, + "angularVelocity": 0.16721377336302404, + "velocityX": 1.971018646684527, + "velocityY": -3.160740428007094, + "timestamp": 7.2598852159385885 + }, + { + "x": 1.6073609239957887, + "y": 2.563006197529012, + "heading": -0.08403156658210396, + "angularVelocity": 0.000004245763263575802, + "velocityX": 2.199997865024975, + "velocityY": -3.0762292453060858, + "timestamp": 7.3166251793414165 + }, + { + "x": 1.743340039826477, + "y": 2.397001117024792, + "heading": -0.08403149597768683, + "angularVelocity": 0.000001244350769606544, + "velocityX": 2.3965316097456575, + "velocityY": -2.925717088071768, + "timestamp": 7.373365142744245 + }, + { + "x": 1.889887863777881, + "y": 2.2402473603885453, + "heading": -0.08403141650456336, + "angularVelocity": 0.0000014006551769644768, + "velocityX": 2.5827972942277393, + "velocityY": -2.7626693292585887, + "timestamp": 7.430105106147073 + }, + { + "x": 2.0463765393116815, + "y": 2.0934165159455875, + "heading": -0.08403132385631941, + "angularVelocity": 0.000001632856956195875, + "velocityX": 2.757997470368504, + "velocityY": -2.587785321617605, + "timestamp": 7.486845069549901 + }, + { + "x": 2.212135617801855, + "y": 1.957137659226374, + "heading": -0.08403121101732289, + "angularVelocity": 0.000001988704076477053, + "velocityX": 2.9213814840407957, + "velocityY": -2.401814321798131, + "timestamp": 7.543585032952729 + }, + { + "x": 2.3864549304867793, + "y": 1.8319946581987157, + "heading": -0.08403106536679993, + "angularVelocity": 0.0000025669830265249184, + "velocityX": 3.072249297154761, + "velocityY": -2.2055530797438214, + "timestamp": 7.600324996355557 + }, + { + "x": 2.5685876291367493, + "y": 1.7185236728273872, + "heading": -0.08403086086157263, + "angularVelocity": 0.000003604253774998199, + "velocityX": 3.209954461142516, + "velocityY": -1.9998424138157533, + "timestamp": 7.657064959758385 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.0840305094244903, + "angularVelocity": 0.000006193819334023837, + "velocityX": 3.333906680775301, + "velocityY": -1.7855635028764607, + "timestamp": 7.713804923161213 + }, + { + "x": 2.9656624873135113, + "y": 1.5238234413872127, + "heading": -0.08403019929618429, + "angularVelocity": 0.000005146069704041112, + "velocityX": 3.449910175144429, + "velocityY": -1.5496108616313848, + "timestamp": 7.774070003863618 + }, + { + "x": 3.179557703869071, + "y": 1.4451070800547587, + "heading": -0.08403000395819739, + "angularVelocity": 0.000003241312956351986, + "velocityX": 3.549239693410422, + "velocityY": -1.306168686990774, + "timestamp": 7.834335084566023 + }, + { + "x": 3.398405200510204, + "y": 1.3814422413572929, + "heading": -0.084029865442907, + "angularVelocity": 0.000002298433666202308, + "velocityX": 3.631414644938793, + "velocityY": -1.0564133981973587, + "timestamp": 7.894600165268428 + }, + { + "x": 3.6211472309421358, + "y": 1.3331366319820044, + "heading": -0.08402975974511386, + "angularVelocity": 0.0000017538812179176465, + "velocityX": 3.696038034560229, + "velocityY": -0.8015522224856215, + "timestamp": 7.954865245970833 + }, + { + "x": 3.8467072298284624, + "y": 1.3004237209144467, + "heading": -0.08402967471443804, + "angularVelocity": 0.0000014109443619153728, + "velocityX": 3.742797591198178, + "velocityY": -0.5428170125432534, + "timestamp": 8.015130326673239 + }, + { + "x": 4.07399501434713, + "y": 1.283461611303661, + "heading": -0.08402960341272074, + "angularVelocity": 0.000001183134851133743, + "velocityX": 3.771467355051538, + "velocityY": -0.28145834060267944, + "timestamp": 8.075395407375645 + }, + { + "x": 4.301912053057315, + "y": 1.282332275177001, + "heading": -0.08402954153196555, + "angularVelocity": 0.000001026809463787284, + "velocityX": 3.7819087945083893, + "velocityY": -0.01873947754649237, + "timestamp": 8.13566048807805 + }, + { + "x": 4.529356776217307, + "y": 1.2970411540477857, + "heading": -0.08402948621280212, + "angularVelocity": 9.179306286652721e-7, + "velocityX": 3.774071493957432, + "velocityY": 0.2440696784829437, + "timestamp": 8.195925568780456 + }, + { + "x": 4.755229902929938, + "y": 1.3275171239584524, + "heading": -0.08402943543978927, + "angularVelocity": 8.424947292982353e-7, + "velocityX": 3.747993433013281, + "velocityY": 0.505698649291784, + "timestamp": 8.256190649482862 + }, + { + "x": 4.978439764792266, + "y": 1.3736128101593832, + "heading": -0.08402938770393992, + "angularVelocity": 7.920979912088378e-7, + "velocityX": 3.703800928510481, + "velocityY": 0.7648821782643186, + "timestamp": 8.316455730185268 + }, + { + "x": 5.197907639407193, + "y": 1.435105143730422, + "heading": -0.08402934179871885, + "angularVelocity": 7.617217222606304e-7, + "velocityX": 3.6417087981459852, + "velocityY": 1.020364245004398, + "timestamp": 8.376720810887674 + }, + { + "x": 5.4125746557646295, + "y": 1.5116918742918497, + "heading": -0.08402929668498581, + "angularVelocity": 7.485882790977605e-7, + "velocityX": 3.562046443071779, + "velocityY": 1.270830963284032, + "timestamp": 8.43698589159008 + }, + { + "x": 5.626738899704859, + "y": 1.5896734864413642, + "heading": -0.08402925161448958, + "angularVelocity": 7.478708348501504e-7, + "velocityX": 3.553703760852683, + "velocityY": 1.293976731477308, + "timestamp": 8.497250972292486 + }, + { + "x": 5.840903108372873, + "y": 1.667655195460401, + "heading": -0.08402920654399476, + "angularVelocity": 7.478708115803468e-7, + "velocityX": 3.553703175568263, + "velocityY": 1.2939783388678752, + "timestamp": 8.557516052994892 + }, + { + "x": 6.055067317059206, + "y": 1.7456369044291342, + "heading": -0.08402916147349976, + "angularVelocity": 7.478708148850431e-7, + "velocityX": 3.5537031758721978, + "velocityY": 1.293978338033166, + "timestamp": 8.617781133697298 + }, + { + "x": 6.269231826783721, + "y": 1.8236177866398526, + "heading": -0.08402911640284853, + "angularVelocity": 7.47873406932242e-7, + "velocityX": 3.5537081711062477, + "velocityY": 1.2939646193422618, + "timestamp": 8.678046214399703 + }, + { + "x": 6.484552476173004, + "y": 1.8927933599363171, + "heading": -0.07857379817757901, + "angularVelocity": 0.09052204297557322, + "velocityX": 3.5728924093299965, + "velocityY": 1.1478549848470312, + "timestamp": 8.73831129510211 + }, + { + "x": 6.687452356091927, + "y": 1.958447331788137, + "heading": -0.03255318883880569, + "angularVelocity": 0.763636401086517, + "velocityX": 3.3667901470316046, + "velocityY": 1.089419794790041, + "timestamp": 8.798576375804515 + }, + { + "x": 6.875915673704612, + "y": 2.0186526211606592, + "heading": 0.028144575028097817, + "angularVelocity": 1.0071796662255343, + "velocityX": 3.127239114526933, + "velocityY": 0.9990078611164873, + "timestamp": 8.858841456506921 + }, + { + "x": 7.0495323738250235, + "y": 2.0733238477692897, + "heading": 0.08948102939748631, + "angularVelocity": 1.0177776857592493, + "velocityX": 2.880883889922049, + "velocityY": 0.9071791818980943, + "timestamp": 8.919106537209327 + }, + { + "x": 7.208331848074836, + "y": 2.122508890246591, + "heading": 0.14602748254405165, + "angularVelocity": 0.938295485337472, + "velocityX": 2.635016370989014, + "velocityY": 0.8161449699234864, + "timestamp": 8.979371617911733 + }, + { + "x": 7.352376623078616, + "y": 2.166251760331207, + "heading": 0.19476972690009334, + "angularVelocity": 0.8087974626091599, + "velocityX": 2.39018637866075, + "velocityY": 0.7258410604413249, + "timestamp": 9.039636698614139 + }, + { + "x": 7.481724262237549, + "y": 2.2045881748199463, + "heading": 0.2337432969576531, + "angularVelocity": 0.646702362351673, + "velocityX": 2.1463115563996884, + "velocityY": 0.6361298125202616, + "timestamp": 9.099901779316545 + }, + { + "x": 7.603777232623247, + "y": 2.2395659454817083, + "heading": 0.2627306675325177, + "angularVelocity": 0.44772760396065475, + "velocityX": 1.8851825089114527, + "velocityY": 0.54025298396176, + "timestamp": 9.164645097169718 + }, + { + "x": 7.708779235262183, + "y": 2.2685458571632657, + "heading": 0.28047750108878416, + "angularVelocity": 0.27411065952030855, + "velocityX": 1.6218199208922495, + "velocityY": 0.4476123968079393, + "timestamp": 9.22938841502289 + }, + { + "x": 7.796631046068626, + "y": 2.2916863663414535, + "heading": 0.2881711400063891, + "angularVelocity": 0.1188329417261678, + "velocityX": 1.3569247563999605, + "velocityY": 0.3574192665050968, + "timestamp": 9.294131732876064 + }, + { + "x": 7.867260826730005, + "y": 2.309110963792019, + "heading": 0.28670858560451956, + "angularVelocity": -0.02259004404417999, + "velocityX": 1.0909200053904464, + "velocityY": 0.2691335264912042, + "timestamp": 9.358875050729237 + }, + { + "x": 7.920614374882339, + "y": 2.320918360362405, + "heading": 0.27679109511178074, + "angularVelocity": -0.1531816845597854, + "velocityX": 0.8240780658373096, + "velocityY": 0.18237243567224945, + "timestamp": 9.42361836858241 + }, + { + "x": 7.95664944798485, + "y": 2.3271891462135073, + "heading": 0.25898192848994617, + "angularVelocity": -0.27507343170491383, + "velocityX": 0.5565836644984964, + "velocityY": 0.09685610900144763, + "timestamp": 9.488361686435583 + }, + { + "x": 7.975332260131836, + "y": 2.3279902935028076, + "heading": 0.2337432969576531, + "angularVelocity": -0.38982604489825085, + "velocityX": 0.28856741925638435, + "velocityY": 0.012374208116996958, + "timestamp": 9.553105004288756 + }, + { + "x": 7.972208452847698, + "y": 2.3208851884260335, + "heading": 0.1923810608861147, + "angularVelocity": -0.5227816750662158, + "velocityX": -0.039482130554085705, + "velocityY": -0.08980217431023955, + "timestamp": 9.632224525963458 + }, + { + "x": 7.943119769823989, + "y": 2.3056970047129943, + "heading": 0.14088004377458682, + "angularVelocity": -0.650926800635539, + "velocityX": -0.367654940373712, + "velocityY": -0.19196505984306744, + "timestamp": 9.71134404763816 + }, + { + "x": 7.888054784726001, + "y": 2.282427290592543, + "heading": 0.07969868827872208, + "angularVelocity": -0.7732776210074993, + "velocityX": -0.6959721688458461, + "velocityY": -0.29410837714772914, + "timestamp": 9.790463569312863 + }, + { + "x": 7.80699994554277, + "y": 2.251078457842943, + "heading": 0.009399898543879681, + "angularVelocity": -0.8885138363686558, + "velocityX": -1.0244606826174518, + "velocityY": -0.3962212117319218, + "timestamp": 9.869583090987565 + }, + { + "x": 7.6999389304832775, + "y": 2.211654434019612, + "heading": -0.0693051292941164, + "angularVelocity": -0.9947611685721511, + "velocityX": -1.3531554892314888, + "velocityY": -0.4982844055278961, + "timestamp": 9.948702612662267 + }, + { + "x": 7.566851730635094, + "y": 2.1641617638366206, + "heading": -0.15547855735771587, + "angularVelocity": -1.0891550686807683, + "velocityX": -1.6821031906053239, + "velocityY": -0.6002648799907612, + "timestamp": 10.027822134336969 + }, + { + "x": 7.407713497482348, + "y": 2.1086115265820684, + "heading": -0.24780538807324573, + "angularVelocity": -1.1669285754169436, + "velocityX": -2.011364954998582, + "velocityY": -0.70210532215988, + "timestamp": 10.10694165601167 + }, + { + "x": 7.22249394667395, + "y": 2.0450230915823275, + "heading": -0.3442810848564748, + "angularVelocity": -1.2193665323191243, + "velocityX": -2.3410094865073168, + "velocityY": -0.803700953365003, + "timestamp": 10.186061177686373 + }, + { + "x": 7.0111623902454, + "y": 1.973433532446374, + "heading": -0.441482782839164, + "angularVelocity": -1.2285425382414696, + "velocityX": -2.671041886444093, + "velocityY": -0.9048280073063681, + "timestamp": 10.265180699361075 + }, + { + "x": 6.773734963981195, + "y": 1.8939319567020383, + "heading": -0.5324651228432857, + "angularVelocity": -1.1499354151582672, + "velocityX": -3.0008703444945404, + "velocityY": -1.0048288217818704, + "timestamp": 10.344300221035777 + }, + { + "x": 6.510837596418812, + "y": 1.8068437116330571, + "heading": -0.5976367021926547, + "angularVelocity": -0.8237104821907345, + "velocityX": -3.3227876255784223, + "velocityY": -1.1007175375382399, + "timestamp": 10.423419742710479 + }, + { + "x": 6.227373099976041, + "y": 1.711008555192143, + "heading": -0.5976367652296872, + "angularVelocity": -7.967317193878993e-7, + "velocityX": -3.582737742124223, + "velocityY": -1.2112706752062792, + "timestamp": 10.502539264385181 + }, + { + "x": 5.941831356009296, + "y": 1.6215523179581028, + "heading": -0.5976367850031955, + "angularVelocity": -2.499194616123805e-7, + "velocityX": -3.6089922932135883, + "velocityY": -1.1306468408876076, + "timestamp": 10.581658786059883 + }, + { + "x": 5.656289546537612, + "y": 1.5320962898142203, + "heading": -0.5976368047767018, + "angularVelocity": -2.4991943644041576e-7, + "velocityX": -3.608993121137454, + "velocityY": -1.1306441981749955, + "timestamp": 10.660778307734585 + }, + { + "x": 5.370747722964019, + "y": 1.442640306683351, + "heading": -0.5976368245502077, + "angularVelocity": -2.4991943318222364e-7, + "velocityX": -3.6089932993729574, + "velocityY": -1.130643629250762, + "timestamp": 10.739897829409287 + }, + { + "x": 5.084751688404252, + "y": 1.3546472720839122, + "heading": -0.5976368443220548, + "angularVelocity": -2.4989846513470833e-7, + "velocityX": -3.614734120052362, + "velocityY": -1.1121532680799078, + "timestamp": 10.819017351083989 + }, + { + "x": 4.791925529016698, + "y": 1.2930896074141898, + "heading": -0.5976368642674134, + "angularVelocity": -2.5209149670001704e-7, + "velocityX": -3.7010607899211223, + "velocityY": -0.7780338324442274, + "timestamp": 10.898136872758691 + }, + { + "x": 4.494706490584963, + "y": 1.2584868871615749, + "heading": -0.5976368851112348, + "angularVelocity": -2.6344726295956754e-7, + "velocityX": -3.7565828526333167, + "velocityY": -0.4373474399261832, + "timestamp": 10.977256394433393 + }, + { + "x": 4.195570493234456, + "y": 1.2511276069767714, + "heading": -0.5976369077396874, + "angularVelocity": -2.860034028815913e-7, + "velocityX": -3.7808114990937063, + "velocityY": -0.09301472037534324, + "timestamp": 11.056375916108095 + }, + { + "x": 3.8970094811156857, + "y": 1.2710731376080628, + "heading": -0.5976369333832999, + "angularVelocity": -3.2411233223341237e-7, + "velocityX": -3.7735441999548045, + "velocityY": 0.2520936705519675, + "timestamp": 11.135495437782797 + }, + { + "x": 3.6015106205827614, + "y": 1.3181573487272922, + "heading": -0.5976369639589968, + "angularVelocity": -3.8644946446935137e-7, + "velocityX": -3.734841342290479, + "velocityY": 0.5951023226962214, + "timestamp": 11.2146149594575 + }, + { + "x": 3.311535573428697, + "y": 1.3919880169913565, + "heading": -0.5976370028137967, + "angularVelocity": -4.910899244313184e-7, + "velocityX": -3.665025280945076, + "velocityY": 0.9331536225360112, + "timestamp": 11.293734481132201 + }, + { + "x": 3.0294999883777347, + "y": 1.4919501002081306, + "heading": -0.597637056618776, + "angularVelocity": -6.800468215926745e-7, + "velocityX": -3.564677579959917, + "velocityY": 1.263431339079197, + "timestamp": 11.372854002806903 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.5976371290282776, + "angularVelocity": -9.151913479184684e-7, + "velocityX": -3.4346342145827435, + "velocityY": 1.5831840506775074, + "timestamp": 11.451973524481605 + }, + { + "x": 2.5626151824790466, + "y": 1.7237517105978553, + "heading": -0.5976372115443579, + "angularVelocity": -0.0000014036543940556534, + "velocityX": -3.319432731110931, + "velocityY": 1.8123319198994405, + "timestamp": 11.510760132170452 + }, + { + "x": 2.3751466997704354, + "y": 1.8432734191532605, + "heading": -0.5976372707414468, + "angularVelocity": -0.0000010069825624449078, + "velocityX": -3.188965821958426, + "velocityY": 2.0331451882378415, + "timestamp": 11.569546739859298 + }, + { + "x": 2.196210091733693, + "y": 1.9752263087350537, + "heading": -0.5976373163591139, + "angularVelocity": -7.759874050438566e-7, + "velocityX": -3.0438328570316586, + "velocityY": 2.244607994395761, + "timestamp": 11.628333347548145 + }, + { + "x": 2.0266282854543833, + "y": 2.1190035270334358, + "heading": -0.5976373534228027, + "angularVelocity": -6.304784410722591e-7, + "velocityX": -2.8847013451923114, + "velocityY": 2.445747831876676, + "timestamp": 11.68711995523699 + }, + { + "x": 1.867181183440892, + "y": 2.2739438419317777, + "heading": -0.5976373848119125, + "angularVelocity": -5.339500111409062e-7, + "velocityX": -2.712303163629251, + "velocityY": 2.635639663346953, + "timestamp": 11.745906562925837 + }, + { + "x": 1.718602076854733, + "y": 2.439334682311898, + "heading": -0.5976374123157431, + "angularVelocity": -4.678587798571888e-7, + "velocityX": -2.52743120291235, + "velocityY": 2.813410177629591, + "timestamp": 11.804693170614684 + }, + { + "x": 1.5815742727191044, + "y": 2.6144154150584233, + "heading": -0.5976374371244817, + "angularVelocity": -4.220134408077074e-7, + "velocityX": -2.3309357270776045, + "velocityY": 2.9782418076105714, + "timestamp": 11.86347977830353 + }, + { + "x": 1.4567279551341459, + "y": 2.7983808372043484, + "heading": -0.5976374847546617, + "angularVelocity": -8.102216113846116e-7, + "velocityX": -2.1237203930146884, + "velocityY": 3.12937638993629, + "timestamp": 11.922266385992376 + }, + { + "x": 1.345556198102205, + "y": 2.9885105109664996, + "heading": -0.6039795609021088, + "angularVelocity": -0.10788300935844368, + "velocityX": -1.891106859241893, + "velocityY": 3.2342344836173083, + "timestamp": 11.981052993681223 + }, + { + "x": 1.246887054415439, + "y": 3.17332778383869, + "heading": -0.6383468381311229, + "angularVelocity": -0.584610655047786, + "velocityX": -1.6784289409760504, + "velocityY": 3.1438669475608143, + "timestamp": 12.039839601370069 + }, + { + "x": 1.1585650233427731, + "y": 3.3474027479162674, + "heading": -0.6816254554461085, + "angularVelocity": -0.736198583596727, + "velocityX": -1.502417549591352, + "velocityY": 2.9611330015663175, + "timestamp": 12.098626209058915 + }, + { + "x": 1.079033349316087, + "y": 3.5092497703250873, + "heading": -0.7278331549531709, + "angularVelocity": -0.7860242549057459, + "velocityX": -1.352887624467824, + "velocityY": 2.753127434490931, + "timestamp": 12.157412816747762 + }, + { + "x": 1.0074427202851626, + "y": 3.6582801705311523, + "heading": -0.7741804093489261, + "angularVelocity": -0.7883981780521784, + "velocityX": -1.2178050723703648, + "velocityY": 2.535108012948657, + "timestamp": 12.216199424436608 + }, + { + "x": 0.9432733001900041, + "y": 3.7941912592964138, + "heading": -0.8190469205570853, + "angularVelocity": -0.7632097338501684, + "velocityX": -1.0915652836238168, + "velocityY": 2.3119396425224727, + "timestamp": 12.274986032125454 + }, + { + "x": 0.886177084517944, + "y": 3.9168021203940837, + "heading": -0.861376017099095, + "angularVelocity": -0.7200465923472629, + "velocityX": -0.9712452872645088, + "velocityY": 2.0856937645839286, + "timestamp": 12.3337726398143 + }, + { + "x": 0.835905686210625, + "y": 4.025993630008512, + "heading": -0.9004245420638279, + "angularVelocity": -0.6642418486097024, + "velocityX": -0.855150522945662, + "velocityY": 1.8574215098848674, + "timestamp": 12.392559247503147 + }, + { + "x": 0.79227325612998, + "y": 4.121681911612244, + "heading": -0.9356409507741646, + "angularVelocity": -0.599054956474685, + "velocityX": -0.7422171782999981, + "velocityY": 1.6277224586627488, + "timestamp": 12.451345855191994 + }, + { + "x": 0.7551356460272671, + "y": 4.203804939056435, + "heading": -0.9665991521557302, + "angularVelocity": -0.5266199666669827, + "velocityX": -0.6317358929652714, + "velocityY": 1.3969683006521094, + "timestamp": 12.51013246288084 + }, + { + "x": 0.7243778481209882, + "y": 4.272315098083365, + "heading": -0.9929596722347582, + "angularVelocity": -0.44841029471460875, + "velocityX": -0.5232109678632522, + "velocityY": 1.1654041918790294, + "timestamp": 12.568919070569686 + }, + { + "x": 0.6999059920420889, + "y": 4.327174746307989, + "heading": -1.0144454160034293, + "angularVelocity": -0.3654870490638522, + "velocityX": -0.4162828412965571, + "velocityY": 0.9331997606494288, + "timestamp": 12.627705678258533 + }, + { + "x": 0.6816420121830558, + "y": 4.368353408669628, + "heading": -1.0308257655348128, + "angularVelocity": -0.2786408363293121, + "velocityX": -0.3106826635702974, + "velocityY": 0.7004769280036421, + "timestamp": 12.686492285947379 + }, + { + "x": 0.6695199611242603, + "y": 4.3958259235729, + "heading": -1.0419057024962997, + "angularVelocity": -0.18847722971415903, + "velocityX": -0.2062042961035709, + "velocityY": 0.4673260795840121, + "timestamp": 12.745278893636225 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -0.0954706943986077, + "velocityX": -0.10268631054285546, + "velocityY": 0.233815962076544, + "timestamp": 12.804065501325072 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": 7.129108746129837e-28, + "velocityX": 3.4723117091122335e-27, + "velocityY": 7.64903026201397e-27, + "timestamp": 12.862852109013918 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubSprint.1.traj b/src/main/deploy/choreo/SourceLanePSubHSubSprint.1.traj index e72c3131..5ace25e1 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubSprint.1.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubSprint.1.traj @@ -1,904 +1,905 @@ { - "samples": [ - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": 2.1342037757525677e-32, - "velocityY": 1.1024409975331184e-32, - "timestamp": 0 - }, - { - "x": 0.6705320996855912, - "y": 4.398099342118358, - "heading": -1.0386114127976833, - "angularVelocity": 0.15927551181553457, - "velocityX": 0.12605002427557185, - "velocityY": -0.20514712032687507, - "timestamp": 0.05592000838348859 - }, - { - "x": 0.6846335258864488, - "y": 4.375148415401169, - "heading": -1.0210111049652704, - "angularVelocity": 0.3147407938803157, - "velocityX": 0.25217138924859706, - "velocityY": -0.41042423598715294, - "timestamp": 0.11184001676697718 - }, - { - "x": 0.7057920861320249, - "y": 4.340710150429397, - "heading": -0.9949634737692402, - "angularVelocity": 0.4658016325283909, - "velocityX": 0.3783719076090775, - "velocityY": -0.6158487090273922, - "timestamp": 0.16776002515046576 - }, - { - "x": 0.7340126708459819, - "y": 4.294775101740391, - "heading": -0.9607567098648513, - "angularVelocity": 0.6117088479280168, - "velocityX": 0.5046598798845962, - "velocityY": -0.8214420923185943, - "timestamp": 0.22368003353395435 - }, - { - "x": 0.7693006604451206, - "y": 4.237332365746024, - "heading": -0.9187315737842261, - "angularVelocity": 0.7515223494321577, - "velocityX": 0.6310440684690272, - "velocityY": -1.0272304610620715, - "timestamp": 0.2796000419174429 - }, - { - "x": 0.8116619611973516, - "y": 4.168369327121192, - "heading": -0.8692948358062461, - "angularVelocity": 0.8840617054087766, - "velocityX": 0.7575338769931041, - "velocityY": -1.2332444257142752, - "timestamp": 0.3355200503009315 - }, - { - "x": 0.8611030812908155, - "y": 4.087871426229515, - "heading": -0.8129373545696884, - "angularVelocity": 1.0078231900479868, - "velocityX": 0.8841400694078276, - "velocityY": -1.4395187557848361, - "timestamp": 0.3914400586844201 - }, - { - "x": 0.9176312901451578, - "y": 3.995821968070834, - "heading": -0.7502611156342618, - "angularVelocity": 1.120819555419328, - "velocityX": 1.0108762585778377, - "velocityY": -1.6460916373156267, - "timestamp": 0.4473600670679087 - }, - { - "x": 0.9812548770105831, - "y": 3.8922019971306105, - "heading": -0.6820246305048668, - "angularVelocity": 1.220251697057024, - "velocityX": 1.1377606818136927, - "velocityY": -1.8530034943775027, - "timestamp": 0.5032800754513973 - }, - { - "x": 1.0519833618359966, - "y": 3.7769903402374796, - "heading": -0.6092267013635415, - "angularVelocity": 1.3018225720227237, - "velocityX": 1.2648153473148889, - "velocityY": -2.0602939846330393, - "timestamp": 0.5592000838348858 - }, - { - "x": 1.1298269082953458, - "y": 3.6501643798944903, - "heading": -0.5332724156478241, - "angularVelocity": 1.3582667083101572, - "velocityX": 1.392051766614792, - "velocityY": -2.26798893650447, - "timestamp": 0.6151200922183744 - }, - { - "x": 1.2147919549002646, - "y": 3.511704401985981, - "heading": -0.45632702327380176, - "angularVelocity": 1.375990358340898, - "velocityX": 1.5194033238021896, - "velocityY": -2.4760364297333126, - "timestamp": 0.6710401006018629 - }, - { - "x": 1.3068601679390515, - "y": 3.3616165956403266, - "heading": -0.38215505844470493, - "angularVelocity": 1.3263940219829704, - "velocityX": 1.6464270249639619, - "velocityY": -2.683973244717373, - "timestamp": 0.7269601089853515 - }, - { - "x": 1.4058792704184795, - "y": 3.2000739494751453, - "heading": -0.3184323584737973, - "angularVelocity": 1.139533090444293, - "velocityX": 1.7707276043375095, - "velocityY": -2.8888165584195344, - "timestamp": 0.78288011736884 - }, - { - "x": 1.512169170978474, - "y": 3.0300749520186443, - "heading": -0.28662541962876376, - "angularVelocity": 0.5687935278354703, - "velocityX": 1.900749009747616, - "velocityY": -3.0400388406718526, - "timestamp": 0.8388001257523285 - }, - { - "x": 1.6244650109400058, - "y": 2.850864532661705, - "heading": -0.2866253125429214, - "angularVelocity": 0.0000019149825876044375, - "velocityX": 2.008151343458836, - "velocityY": -3.2047638141959607, - "timestamp": 0.8947201341358171 - }, - { - "x": 1.7352882055291936, - "y": 2.6707396888206167, - "heading": -0.2866252783553297, - "angularVelocity": 6.113659974508405e-7, - "velocityX": 1.9818164873864779, - "velocityY": -3.221116180917336, - "timestamp": 0.9506401425193056 - }, - { - "x": 1.8506362591615508, - "y": 2.493478394875359, - "heading": -0.2866252441530604, - "angularVelocity": 6.116284725625113e-7, - "velocityX": 2.0627331248114724, - "velocityY": -3.169908214777688, - "timestamp": 1.0065601509027942 - }, - { - "x": 1.9771720781105002, - "y": 2.324022128559085, - "heading": -0.2866252095630558, - "angularVelocity": 6.185622200419991e-7, - "velocityX": 2.2628004288052184, - "velocityY": -3.0303333496335663, - "timestamp": 1.0624801592862827 - }, - { - "x": 2.114370117744969, - "y": 2.1630766803808767, - "heading": -0.28662517392184755, - "angularVelocity": 6.373605669269748e-7, - "velocityX": 2.4534695827223696, - "velocityY": -2.8781370538157938, - "timestamp": 1.1184001676697712 - }, - { - "x": 2.2616596068294545, - "y": 2.011311910787294, - "heading": -0.28662513647276167, - "angularVelocity": 6.696902771759525e-7, - "velocityX": 2.6339318133574414, - "velocityY": -2.713961853381871, - "timestamp": 1.1743201760532598 - }, - { - "x": 2.418427669547749, - "y": 1.8693594092228303, - "heading": -0.2866250962912349, - "angularVelocity": 7.185536621043778e-7, - "velocityX": 2.8034341776776834, - "velocityY": -2.538492136678183, - "timestamp": 1.2302401844367483 - }, - { - "x": 2.5840219554137547, - "y": 1.7378099138847216, - "heading": -0.28662505217416917, - "angularVelocity": 7.889316725786441e-7, - "velocityX": 2.961270762521926, - "velocityY": -2.3524584337678855, - "timestamp": 1.2861601928202369 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.28662500163792387, - "angularVelocity": 9.037238500487202e-7, - "velocityX": 3.1067845266977017, - "velocityY": -2.1566350283233993, - "timestamp": 1.3420802012037254 - }, - { - "x": 2.964172022537395, - "y": 1.4952512181839386, - "heading": -0.2866249560195876, - "angularVelocity": 7.195928187708238e-7, - "velocityX": 3.256089340971223, - "velocityY": -1.923816018708688, - "timestamp": 1.4054748531095118 - }, - { - "x": 3.1789518158724, - "y": 1.3887033110695066, - "heading": -0.28662491569189824, - "angularVelocity": 6.361370890269852e-7, - "velocityX": 3.387979693526802, - "velocityY": -1.6807081340674828, - "timestamp": 1.4688695050152982 - }, - { - "x": 3.4009440542760783, - "y": 1.2981369637223683, - "heading": -0.2866248790058733, - "angularVelocity": 5.786927429417107e-7, - "velocityX": 3.5017502538477667, - "velocityY": -1.4286117933375972, - "timestamp": 1.5322641569210846 - }, - { - "x": 3.6289614625982565, - "y": 1.2240364953903844, - "heading": -0.286624844765475, - "angularVelocity": 5.401149349395126e-7, - "velocityX": 3.5967924969608642, - "velocityY": -1.1688757033023536, - "timestamp": 1.595658808826871 - }, - { - "x": 3.861784529858251, - "y": 1.1667980667810438, - "heading": -0.2866248120410044, - "angularVelocity": 5.162023864828723e-7, - "velocityX": 3.672597928386812, - "velocityY": -0.9028904945231823, - "timestamp": 1.6590534607326575 - }, - { - "x": 4.0981679647792335, - "y": 1.1267270015307351, - "heading": -0.2866247800525485, - "angularVelocity": 5.045923424053189e-7, - "velocityX": 3.728759884544863, - "velocityY": -0.6320890492444079, - "timestamp": 1.7224481126384439 - }, - { - "x": 4.3358126512762265, - "y": 1.0949807955483848, - "heading": -0.2866247482072254, - "angularVelocity": 5.023345364444787e-7, - "velocityX": 3.7486551207847545, - "velocityY": -0.5007710434238178, - "timestamp": 1.7858427645442303 - }, - { - "x": 4.573457419312962, - "y": 1.0632351999594603, - "heading": -0.28662471636190656, - "angularVelocity": 5.023344691113163e-7, - "velocityX": 3.7486564070090496, - "velocityY": -0.5007614149550478, - "timestamp": 1.8492374164500167 - }, - { - "x": 4.811102187354235, - "y": 1.0314896044045174, - "heading": -0.28662468451658774, - "angularVelocity": 5.023344695763415e-7, - "velocityX": 3.748656407080655, - "velocityY": -0.5007614144190159, - "timestamp": 1.9126320683558031 - }, - { - "x": 5.04874695539551, - "y": 0.9997440088495765, - "heading": -0.28662465267126885, - "angularVelocity": 5.023344699297441e-7, - "velocityX": 3.7486564070806594, - "velocityY": -0.5007614144189861, - "timestamp": 1.9760267202615895 - }, - { - "x": 5.286391723436784, - "y": 0.9679984132946355, - "heading": -0.28662462082595, - "angularVelocity": 5.023344691743294e-7, - "velocityX": 3.748656407080659, - "velocityY": -0.5007614144189861, - "timestamp": 2.039421372167376 - }, - { - "x": 5.524036491478059, - "y": 0.936252817739695, - "heading": -0.28662458898063115, - "angularVelocity": 5.023344697631327e-7, - "velocityX": 3.74865640708066, - "velocityY": -0.5007614144189809, - "timestamp": 2.1028160240731624 - }, - { - "x": 5.761681259520114, - "y": 0.9045072221906025, - "heading": -0.2866245571353122, - "angularVelocity": 5.023344707942377e-7, - "velocityX": 3.748656407092983, - "velocityY": -0.5007614143267316, - "timestamp": 2.166210675978949 - }, - { - "x": 5.99932604159473, - "y": 0.8727617316882137, - "heading": -0.2866245252899934, - "angularVelocity": 5.023344694954779e-7, - "velocityX": 3.748656628445413, - "velocityY": -0.500759757298884, - "timestamp": 2.229605327884735 - }, - { - "x": 6.237212534637086, - "y": 0.8428813906316718, - "heading": -0.2866244915581877, - "angularVelocity": 5.320922919551869e-7, - "velocityX": 3.752469425905036, - "velocityY": -0.47133851450037834, - "timestamp": 2.2929999797905216 - }, - { - "x": 6.4638245505635705, - "y": 0.8220107849523658, - "heading": -0.24424436180847547, - "angularVelocity": 0.6685126974542774, - "velocityX": 3.574623554416899, - "velocityY": -0.3292171350719418, - "timestamp": 2.356394631696308 - }, - { - "x": 6.673647733933235, - "y": 0.8041360764949451, - "heading": -0.18514255243378605, - "angularVelocity": 0.9322838377994912, - "velocityX": 3.309793130207447, - "velocityY": -0.2819592492436898, - "timestamp": 2.4197892836020944 - }, - { - "x": 6.866141134149738, - "y": 0.7888964331390943, - "heading": -0.1276691267098072, - "angularVelocity": 0.9065973863125333, - "velocityX": 3.0364296424022625, - "velocityY": -0.240393201913927, - "timestamp": 2.483183935507881 - }, - { - "x": 7.04134978279503, - "y": 0.7761674362974819, - "heading": -0.07636429138531178, - "angularVelocity": 0.8092927996630026, - "velocityX": 2.7637764918352534, - "velocityY": -0.2007897584252007, - "timestamp": 2.5465785874136673 - }, - { - "x": 7.19932046714385, - "y": 0.765888430525111, - "heading": -0.03338684229285246, - "angularVelocity": 0.6779349330023924, - "velocityX": 2.4918613731579056, - "velocityY": -0.16214310613530794, - "timestamp": 2.6099732393194537 - }, - { - "x": 7.340087890625, - "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": 0.5266507708326894, - "velocityX": 2.220493673351971, - "velocityY": -0.12406863156475356, - "timestamp": 2.67336789122524 - }, - { - "x": 7.464023522460178, - "y": 0.7525372143128384, - "heading": 0.02301011936038792, - "angularVelocity": 0.3617896361662683, - "velocityX": 1.9486481772395665, - "velocityY": -0.08625561847872572, - "timestamp": 2.736968716290925 - }, - { - "x": 7.570582319535539, - "y": 0.7491648354718851, - "heading": 0.036983700221316605, - "angularVelocity": 0.21970754068830917, - "velocityX": 1.675431049287655, - "velocityY": -0.053024136675436706, - "timestamp": 2.8005695413566096 - }, - { - "x": 7.659708944521439, - "y": 0.7476928551810322, - "heading": 0.04295335113137197, - "angularVelocity": 0.09386121805637201, - "velocityX": 1.4013438488235506, - "velocityY": -0.02314404395434427, - "timestamp": 2.8641703664222944 - }, - { - "x": 7.731365798341117, - "y": 0.7479588271083956, - "heading": 0.041693436231508264, - "angularVelocity": -0.019809725716020715, - "velocityX": 1.1266654755763426, - "velocityY": 0.0041818942928592215, - "timestamp": 2.927771191487979 - }, - { - "x": 7.785525986892142, - "y": 0.7498349370538775, - "heading": 0.03380693253041853, - "angularVelocity": -0.12400002190136777, - "velocityX": 0.8515642445690119, - "velocityY": 0.029498201376229192, - "timestamp": 2.991372016553664 - }, - { - "x": 7.822169492500932, - "y": 0.7532180353995639, - "heading": 0.0197770843540876, - "angularVelocity": -0.220592235428419, - "velocityX": 0.5761482743493489, - "velocityY": 0.053192680160869574, - "timestamp": 3.0549728416193487 - }, - { - "x": 7.841280937194824, - "y": 0.7580231428146362, - "heading": -3.3329541710174495e-31, - "angularVelocity": -0.3109564118651419, - "velocityX": 0.3004905152433859, - "velocityY": 0.07555102327854826, - "timestamp": 3.1185736666850334 - }, - { - "x": 7.837626456422163, - "y": 0.7661832239405314, - "heading": -0.033360531548023514, - "angularVelocity": -0.4178611188210231, - "velocityX": -0.04577461309858743, - "velocityY": 0.10221002096529454, - "timestamp": 3.1984100765480727 - }, - { - "x": 7.806322741144082, - "y": 0.7764707746744536, - "heading": -0.07501113915617397, - "angularVelocity": -0.521699406068016, - "velocityX": -0.3920982335225694, - "velocityY": 0.12885788265743237, - "timestamp": 3.278246486411112 - }, - { - "x": 7.74736425619444, - "y": 0.7888847549403223, - "heading": -0.12465356813987527, - "angularVelocity": -0.6218018704606526, - "velocityX": -0.7384911852973625, - "velocityY": 0.15549271675874526, - "timestamp": 3.3580828962741514 - }, - { - "x": 7.660744366778727, - "y": 0.8034239689194147, - "heading": -0.18191677738707465, - "angularVelocity": -0.7172568173523234, - "velocityX": -1.0849672419427567, - "velocityY": 0.182112572497119, - "timestamp": 3.4379193061371907 - }, - { - "x": 7.5464550145644145, - "y": 0.8200870575241466, - "heading": -0.24632578527266566, - "angularVelocity": -0.8067623280666746, - "velocityX": -1.4315442341455173, - "velocityY": 0.20871540482992249, - "timestamp": 3.51775571600023 - }, - { - "x": 7.404486266909035, - "y": 0.8388724767291539, - "heading": -0.3172478637527479, - "angularVelocity": -0.8883425319569104, - "velocityX": -1.7782456387872363, - "velocityY": 0.23529889729803757, - "timestamp": 3.5975921258632693 - }, - { - "x": 7.234825726560123, - "y": 0.8597784078771568, - "heading": -0.39379130752240776, - "angularVelocity": -0.958753579989021, - "velocityX": -2.125102326619727, - "velocityY": 0.26185960996827495, - "timestamp": 3.6774285357263086 - }, - { - "x": 7.037458048630302, - "y": 0.8828023770067689, - "heading": -0.47459284644074845, - "angularVelocity": -1.0120888333650886, - "velocityX": -2.4721512185781784, - "velocityY": 0.28838933475478157, - "timestamp": 3.757264945589348 - }, - { - "x": 6.812366694532574, - "y": 0.9079395818644891, - "heading": -0.5572950195818098, - "angularVelocity": -1.0358954427301785, - "velocityX": -2.8194072664825045, - "velocityY": 0.31485890837079844, - "timestamp": 3.837101355452387 - }, - { - "x": 6.559557151686244, - "y": 0.9351738572850867, - "heading": -0.6368741954392069, - "angularVelocity": -0.9967779863087077, - "velocityX": -3.1665945810943685, - "velocityY": 0.3411260033776371, - "timestamp": 3.9169377653154265 - }, - { - "x": 6.2794552371906684, - "y": 0.9644007497338526, - "heading": -0.6963848347023109, - "angularVelocity": -0.7454072567290455, - "velocityX": -3.5084482753682624, - "velocityY": 0.36608475379723326, - "timestamp": 3.996774175178466 - }, - { - "x": 5.978414020607044, - "y": 0.9876514129078965, - "heading": -0.6963848502753706, - "angularVelocity": -1.9506212382871808e-7, - "velocityX": -3.7707258768281893, - "velocityY": 0.2912288167006853, - "timestamp": 4.0766105850415055 - }, - { - "x": 5.677372687056552, - "y": 1.0109005615970947, - "heading": -0.6963848658464015, - "angularVelocity": -1.950367123738679e-7, - "velocityX": -3.770727341909936, - "velocityY": 0.29120984684910617, - "timestamp": 4.156446994904545 - }, - { - "x": 5.376331353502739, - "y": 1.0341497102432655, - "heading": -0.6963848814174324, - "angularVelocity": -1.950367127895845e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.29120984631016156, - "timestamp": 4.236283404767585 - }, - { - "x": 5.075290019948924, - "y": 1.0573988588894352, - "heading": -0.6963848969884634, - "angularVelocity": -1.950367129001825e-7, - "velocityX": -3.770727341951559, - "velocityY": 0.2912098463101467, - "timestamp": 4.316119814630625 - }, - { - "x": 4.774248686395111, - "y": 1.0806480075356062, - "heading": -0.6963849125594943, - "angularVelocity": -1.9503671289084429e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.2912098463101629, - "timestamp": 4.3959562244936645 - }, - { - "x": 4.473207352844907, - "y": 1.1038971562285251, - "heading": -0.6963849281305252, - "angularVelocity": -1.950367124712723e-7, - "velocityX": -3.770727341906337, - "velocityY": 0.2912098468957114, - "timestamp": 4.475792634356704 - }, - { - "x": 4.172166146375442, - "y": 1.1271479503662634, - "heading": -0.6963849437015552, - "angularVelocity": -1.9503670107200537e-7, - "velocityX": -3.770725750142127, - "velocityY": 0.29123045710128126, - "timestamp": 4.555629044219744 - }, - { - "x": 3.8735860999949274, - "y": 1.1720518831992321, - "heading": -0.6963849593703558, - "angularVelocity": -1.9626133737167217e-7, - "velocityX": -3.7398982105123864, - "velocityY": 0.5624492998871315, - "timestamp": 4.635465454082784 - }, - { - "x": 3.580403442801602, - "y": 1.244234542261993, - "heading": -0.6963849756495777, - "angularVelocity": -2.0390723857242194e-7, - "velocityX": -3.672292600535076, - "velocityY": 0.9041320769131688, - "timestamp": 4.7153018639458235 - }, - { - "x": 3.295105029858622, - "y": 1.3430840304472003, - "heading": -0.6963849932202171, - "angularVelocity": -2.20083034395915e-7, - "velocityX": -3.5735376056164516, - "velocityY": 1.238150467371779, - "timestamp": 4.795138273808863 - }, - { - "x": 3.0201108216375614, - "y": 1.4677619739384065, - "heading": -0.6963850130039021, - "angularVelocity": -2.4780278968053093e-7, - "velocityX": -3.444471121544888, - "velocityY": 1.5616677115753714, - "timestamp": 4.874974683671903 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.696385035506177, - "angularVelocity": -2.8185479518318006e-7, - "velocityX": -3.286187967310359, - "velocityY": 1.8719390230438346, - "timestamp": 4.954811093534943 - }, - { - "x": 2.5741841985169076, - "y": 1.7387873379942203, - "heading": -0.6963850589279509, - "angularVelocity": -4.023107769663099e-7, - "velocityX": -3.153128257888759, - "velocityY": 2.0882929565548642, - "timestamp": 5.0130292054104135 - }, - { - "x": 2.3991894720533877, - "y": 1.872411165939459, - "heading": -0.6963850794155533, - "angularVelocity": -3.519111438487621e-7, - "velocityX": -3.0058468202788267, - "velocityY": 2.2952277846293376, - "timestamp": 5.071247317285884 - }, - { - "x": 2.2335584908852413, - "y": 2.017479635272907, - "heading": -0.6963850978369923, - "angularVelocity": -3.1642110134470593e-7, - "velocityX": -2.845007779064253, - "velocityY": 2.4918099309670456, - "timestamp": 5.129465429161355 - }, - { - "x": 2.0780383093475274, - "y": 2.173338406648706, - "heading": -0.696385114808838, - "angularVelocity": -2.9152174806319146e-7, - "velocityX": -2.671336746034877, - "velocityY": 2.6771526309404154, - "timestamp": 5.187683541036826 - }, - { - "x": 1.9333303562340614, - "y": 2.339284459115452, - "heading": -0.6963851307944144, - "angularVelocity": -2.7458081036886754e-7, - "velocityX": -2.485617421310377, - "velocityY": 2.8504196910698067, - "timestamp": 5.245901652912297 - }, - { - "x": 1.8000872249814166, - "y": 2.5145692341351995, - "heading": -0.6963851461633941, - "angularVelocity": -2.6398966215373687e-7, - "velocityX": -2.2886886393302137, - "velocityY": 3.0108289220145825, - "timestamp": 5.304119764787767 - }, - { - "x": 1.6789094155739503, - "y": 2.698401829383423, - "heading": -0.6963851612308563, - "angularVelocity": -2.588105610313673e-7, - "velocityX": -2.0814451981312536, - "velocityY": 3.157652993649885, - "timestamp": 5.362337876663238 - }, - { - "x": 1.5665736982883989, - "y": 2.8877670089616614, - "heading": -0.6963851762457391, - "angularVelocity": -2.5790741716981964e-7, - "velocityX": -1.929566481404267, - "velocityY": 3.2526850060560912, - "timestamp": 5.420555988538709 - }, - { - "x": 1.4540456075762054, - "y": 3.0767463777174253, - "heading": -0.6971250426595832, - "angularVelocity": -0.012708526436353156, - "velocityX": -1.9328708384238364, - "velocityY": 3.24605801644674, - "timestamp": 5.47877410041418 - }, - { - "x": 1.3486317039814464, - "y": 3.254480208772174, - "heading": -0.7253513181524345, - "angularVelocity": -0.48483666995638336, - "velocityX": -1.8106719747325544, - "velocityY": 3.0528958313681422, - "timestamp": 5.53699221228965 - }, - { - "x": 1.2507109072486164, - "y": 3.4195717896549707, - "heading": -0.7601787682651481, - "angularVelocity": -0.5982236281934066, - "velocityX": -1.6819644879978934, - "velocityY": 2.835742616248544, - "timestamp": 5.595210324165121 - }, - { - "x": 1.160328320301069, - "y": 3.5719513486090504, - "heading": -0.7969704460064527, - "angularVelocity": -0.631962744171482, - "velocityX": -1.5524822780387872, - "velocityY": 2.6173909466528555, - "timestamp": 5.653428436040592 - }, - { - "x": 1.0774888591394964, - "y": 3.71161238889488, - "heading": -0.8336344675120989, - "angularVelocity": -0.6297700204374729, - "velocityX": -1.422915626991949, - "velocityY": 2.3989276839579965, - "timestamp": 5.711646547916063 - }, - { - "x": 1.0021908033919844, - "y": 3.838558158350536, - "heading": -0.8689739346571358, - "angularVelocity": -0.6070184347549548, - "velocityX": -1.2933785264038764, - "velocityY": 2.1805202086799977, - "timestamp": 5.7698646597915335 - }, - { - "x": 0.9344313462846832, - "y": 3.9527935365952946, - "heading": -0.9022118432658016, - "angularVelocity": -0.5709204152783613, - "velocityX": -1.1638896371672067, - "velocityY": 1.962196549573947, - "timestamp": 5.828082771667004 - }, - { - "x": 0.8742077702690397, - "y": 4.054323289061519, - "heading": -0.9328020557936417, - "angularVelocity": -0.5254415085338586, - "velocityX": -1.034447426678191, - "velocityY": 1.7439547452758104, - "timestamp": 5.886300883542475 - }, - { - "x": 0.8215176865940929, - "y": 4.143151679881797, - "heading": -0.9603390876580659, - "angularVelocity": -0.4729976802292441, - "velocityX": -0.9050462472512312, - "velocityY": 1.5257861850670007, - "timestamp": 5.944518995417946 - }, - { - "x": 0.7763590444591775, - "y": 4.21928243186123, - "heading": -0.984509600370908, - "angularVelocity": -0.41517170403161197, - "velocityX": -0.7756802939866982, - "velocityY": 1.3076815706816212, - "timestamp": 6.002737107293417 - }, - { - "x": 0.7387300854642734, - "y": 4.282718776761074, - "heading": -1.0050639989905081, - "angularVelocity": -0.3530584891445192, - "velocityX": -0.6463445443815322, - "velocityY": 1.0896324675649907, - "timestamp": 6.060955219168887 - }, - { - "x": 0.7086292911290721, - "y": 4.333463522832413, - "heading": -1.0217986916878445, - "angularVelocity": -0.28744822115035373, - "velocityX": -0.5170348773863939, - "velocityY": 0.8716316011739073, - "timestamp": 6.119173331044358 - }, - { - "x": 0.6860553360496396, - "y": 4.371519118466469, - "heading": -1.0345444282293665, - "angularVelocity": -0.21893077825652074, - "velocityX": -0.3877479765698806, - "velocityY": 0.6536727902728675, - "timestamp": 6.177391442919829 - }, - { - "x": 0.6710070489868055, - "y": 4.396887706764773, - "heading": -1.0431583071392123, - "angularVelocity": -0.14795874741302203, - "velocityX": -0.25848119387696106, - "velocityY": 0.4357507909663603, - "timestamp": 6.2356095547953 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -0.07488723824993983, - "velocityX": -0.12923242394972312, - "velocityY": 0.2178611369128849, - "timestamp": 6.29382766667077 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -4.5881817726943837e-32, - "velocityX": 7.1951222626888875e-34, - "velocityY": 0, - "timestamp": 6.352045778546241 - } - ] + "samples": [ + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -3.050132919883851e-26, + "velocityX": -1.6533006332521264e-25, + "velocityY": 1.9877794824032622e-25, + "timestamp": 0 + }, + { + "x": 0.6705320996373239, + "y": 4.398099342072708, + "heading": -1.0386114123729497, + "angularVelocity": 0.15927551883794178, + "velocityX": 0.12605002295897502, + "velocityY": -0.20514712040522456, + "timestamp": 0.0559200085846521 + }, + { + "x": 0.6846335257451812, + "y": 4.375148415264062, + "heading": -1.0210111037335954, + "angularVelocity": 0.3147408071783644, + "velocityX": 0.25217138667835703, + "velocityY": -0.41042423614623674, + "timestamp": 0.1118400171693042 + }, + { + "x": 0.7057920858570321, + "y": 4.340710150154942, + "heading": -0.9949634713965575, + "angularVelocity": 0.4658016512570238, + "velocityX": 0.37837190385657676, + "velocityY": -0.6158487092681056, + "timestamp": 0.1677600257539563 + }, + { + "x": 0.7340126704011031, + "y": 4.294775101282737, + "heading": -0.9607567060719395, + "angularVelocity": 0.6117088711250027, + "velocityX": 0.5046598750311442, + "velocityY": -0.8214420926396665, + "timestamp": 0.2236800343386084 + }, + { + "x": 0.7693006597994352, + "y": 4.237332365059552, + "heading": -0.9187315683544864, + "angularVelocity": 0.7515223759995503, + "velocityX": 0.6310440626079833, + "velocityY": -1.0272304614586687, + "timestamp": 0.27960004292326046 + }, + { + "x": 0.8116619603260323, + "y": 4.168369326160847, + "heading": -0.8692948285945427, + "angularVelocity": 0.8840617340948025, + "velocityX": 0.7575338702330544, + "velocityY": -1.2332444261754465, + "timestamp": 0.33552005150791253 + }, + { + "x": 0.8611030801762585, + "y": 4.087871424951387, + "heading": -0.8129373455124387, + "angularVelocity": 1.0078232194258272, + "velocityX": 0.8841400618775261, + "velocityY": -1.4395187562892047, + "timestamp": 0.3914400600925646 + }, + { + "x": 0.9176312887785456, + "y": 3.99582196643312, + "heading": -0.7502611047615361, + "angularVelocity": 1.1208195838529407, + "velocityX": 1.0108762504339452, + "velocityY": -1.6460916378244417, + "timestamp": 0.44736006867721667 + }, + { + "x": 0.9812548753941158, + "y": 3.892201995095202, + "heading": -0.6820246179562691, + "angularVelocity": 1.2202517226364538, + "velocityX": 1.1377606732526935, + "velocityY": -1.85300349482346, + "timestamp": 0.5032800772618687 + }, + { + "x": 1.0519833599861292, + "y": 3.7769903377726197, + "heading": -0.6092266874108005, + "angularVelocity": 1.301822592449473, + "velocityX": 1.2648153385910914, + "velocityY": -2.060293984901199, + "timestamp": 0.5592000858465208 + }, + { + "x": 1.129826906247576, + "y": 3.6501643769794856, + "heading": -0.5332724007265343, + "angularVelocity": 1.3582667207442602, + "velocityX": 1.3920517580680767, + "velocityY": -2.267988936395541, + "timestamp": 0.6151200944311729 + }, + { + "x": 1.214791952716682, + "y": 3.5117043986204477, + "heading": -0.45632700801637377, + "angularVelocity": 1.3759903594020386, + "velocityX": 1.51940331590767, + "velocityY": -2.4760364288828027, + "timestamp": 0.671040103015825 + }, + { + "x": 1.3068601657234094, + "y": 3.3616165918639114, + "heading": -0.3821550436425142, + "angularVelocity": 1.3263940090706097, + "velocityX": 1.6464270184678926, + "velocityY": -2.68397324240987, + "timestamp": 0.726960111600477 + }, + { + "x": 1.40587926837359, + "y": 3.2000739453958693, + "heading": -0.31843234449405566, + "angularVelocity": 1.1395330716374026, + "velocityX": 1.7707276010210988, + "velocityY": -2.8888165534434402, + "timestamp": 0.7828801201851291 + }, + { + "x": 1.5121691695533515, + "y": 3.030074948039426, + "heading": -0.28662540722201535, + "angularVelocity": 0.5687934976599797, + "velocityX": 1.9007490139930723, + "velocityY": -3.0400388279464865, + "timestamp": 0.8388001287697812 + }, + { + "x": 1.624464989491448, + "y": 2.8508645164858524, + "heading": -0.2866253001360378, + "angularVelocity": 0.000001914984998408506, + "velocityX": 2.0081509781620026, + "velocityY": -3.2047640207759205, + "timestamp": 0.8947201373544332 + }, + { + "x": 1.7352882065393072, + "y": 2.6707396868109323, + "heading": -0.2866252659484468, + "angularVelocity": 6.113659826501039e-7, + "velocityX": 1.9818168818785864, + "velocityY": -3.2211159160007226, + "timestamp": 0.9506401459390853 + }, + { + "x": 1.850636259974605, + "y": 2.493478393091365, + "heading": -0.28662523174617754, + "angularVelocity": 6.116284689254095e-7, + "velocityX": 2.0627331138671594, + "velocityY": -3.1699081993384874, + "timestamp": 1.0065601545237375 + }, + { + "x": 1.977172078744531, + "y": 2.3240221270116344, + "heading": -0.28662519715617285, + "angularVelocity": 6.185622201662938e-7, + "velocityX": 2.262800417463728, + "velocityY": -3.030333334502384, + "timestamp": 1.0624801631083896 + }, + { + "x": 2.1143701182221513, + "y": 2.163076679089521, + "heading": -0.2866251615149645, + "angularVelocity": 6.373605670854621e-7, + "velocityX": 2.4534695710915173, + "velocityY": -2.8781370388824827, + "timestamp": 1.1184001716930416 + }, + { + "x": 2.2616596071687876, + "y": 2.0113119097755345, + "heading": -0.2866251240658783, + "angularVelocity": 6.696902793737695e-7, + "velocityX": 2.633931801417164, + "velocityY": -2.713961838618882, + "timestamp": 1.1743201802776937 + }, + { + "x": 2.4184276697644367, + "y": 1.8693594085175804, + "heading": -0.28662508388435115, + "angularVelocity": 7.185536668369167e-7, + "velocityX": 2.8034341653995387, + "velocityY": -2.5384921220651377, + "timestamp": 1.2302401888623458 + }, + { + "x": 2.5840219555188284, + "y": 1.7378099135158778, + "heading": -0.28662503976728476, + "angularVelocity": 7.889316805282802e-7, + "velocityX": 2.9612707498732536, + "velocityY": -2.352458419289443, + "timestamp": 1.2861601974469978 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.2866249892310386, + "angularVelocity": 9.037238643571085e-7, + "velocityX": 3.106784513642535, + "velocityY": -2.156635013969323, + "timestamp": 1.34208020603165 + }, + { + "x": 2.964172022531705, + "y": 1.4952512185912425, + "heading": -0.28662494361270324, + "angularVelocity": 7.19592801055205e-7, + "velocityX": 3.2560893274813307, + "velocityY": -1.9238160043665056, + "timestamp": 1.4054748581983312 + }, + { + "x": 3.178951815863355, + "y": 1.3887033119472907, + "heading": -0.28662490328501483, + "angularVelocity": 6.36137071526776e-7, + "velocityX": 3.3879796795309685, + "velocityY": -1.680708119729232, + "timestamp": 1.4688695103650125 + }, + { + "x": 3.4009440542565765, + "y": 1.2981369651359775, + "heading": -0.28662486659899084, + "angularVelocity": 5.786927242976968e-7, + "velocityX": 3.501750239271679, + "velocityY": -1.4286117790060657, + "timestamp": 1.5322641625316937 + }, + { + "x": 3.628961462551473, + "y": 1.224036497406262, + "heading": -0.28662483235859354, + "angularVelocity": 5.40114917287456e-7, + "velocityX": 3.596792481728244, + "velocityY": -1.168875688991645, + "timestamp": 1.595658814698375 + }, + { + "x": 3.861784529757441, + "y": 1.166798069465538, + "heading": -0.2866247996341239, + "angularVelocity": 5.162023688737337e-7, + "velocityX": 3.672597912420351, + "velocityY": -0.9028904802605274, + "timestamp": 1.6590534668650563 + }, + { + "x": 4.098167964587255, + "y": 1.1267270049463884, + "heading": -0.28662476764566913, + "angularVelocity": 5.045923229108656e-7, + "velocityX": 3.7287598677613794, + "velocityY": -0.6320890351096494, + "timestamp": 1.7224481190317376 + }, + { + "x": 4.335812648113977, + "y": 1.0949807783310244, + "heading": -0.2866247358003473, + "angularVelocity": 5.023345150165169e-7, + "velocityX": 3.748655058503865, + "velocityY": -0.500771366832249, + "timestamp": 1.7858427711984188 + }, + { + "x": 4.573457416127459, + "y": 1.0632351841697858, + "heading": -0.2866247039550299, + "angularVelocity": 5.023344440869941e-7, + "velocityX": 3.7486563912149893, + "velocityY": -0.5007613903736075, + "timestamp": 1.8492374233651 + }, + { + "x": 4.8111021841458665, + "y": 1.0314895900454293, + "heading": -0.2866246721097125, + "angularVelocity": 5.023344453927301e-7, + "velocityX": 3.7486563912927067, + "velocityY": -0.5007613897918232, + "timestamp": 1.9126320755317814 + }, + { + "x": 5.048746952164275, + "y": 0.9997439959210748, + "heading": -0.28662464026439505, + "angularVelocity": 5.023344449153729e-7, + "velocityX": 3.748656391292711, + "velocityY": -0.5007613897917893, + "timestamp": 1.9760267276984627 + }, + { + "x": 5.286391720182684, + "y": 0.9679984017967204, + "heading": -0.28662460841907766, + "angularVelocity": 5.023344443752201e-7, + "velocityX": 3.7486563912927116, + "velocityY": -0.5007613897917892, + "timestamp": 2.039421379865144 + }, + { + "x": 5.524036488201093, + "y": 0.9362528076723663, + "heading": -0.28662457657376017, + "angularVelocity": 5.023344462660274e-7, + "velocityX": 3.748656391292712, + "velocityY": -0.5007613897917833, + "timestamp": 2.1028160320318254 + }, + { + "x": 5.761681256220357, + "y": 0.9045072135544255, + "heading": -0.28662454472844273, + "angularVelocity": 5.023344454687972e-7, + "velocityX": 3.748656391306226, + "velocityY": -0.5007613896906189, + "timestamp": 2.1662106841985067 + }, + { + "x": 5.9993260389305805, + "y": 0.8727617294119203, + "heading": -0.28662451288312535, + "angularVelocity": 5.023344447486752e-7, + "velocityX": 3.748656623044358, + "velocityY": -0.5007596549159697, + "timestamp": 2.229605336365188 + }, + { + "x": 6.2372125316404645, + "y": 0.8428813874102156, + "heading": -0.28662447915131883, + "angularVelocity": 5.320923039829879e-7, + "velocityX": 3.752469405217604, + "velocityY": -0.4713385274698144, + "timestamp": 2.2929999885318693 + }, + { + "x": 6.463824548233647, + "y": 0.8220107822514932, + "heading": -0.244244352051896, + "angularVelocity": 0.6685126528968754, + "velocityX": 3.5746235502224897, + "velocityY": -0.32921712550528753, + "timestamp": 2.3563946406985505 + }, + { + "x": 6.673647732212342, + "y": 0.8041360744021581, + "heading": -0.1851425455819086, + "angularVelocity": 0.9322837881434082, + "velocityX": 3.309793126193282, + "velocityY": -0.28195923849124843, + "timestamp": 2.419789292865232 + }, + { + "x": 6.866141132976164, + "y": 0.7888964316418127, + "heading": -0.12766912233996416, + "angularVelocity": 0.9065973434294042, + "velocityX": 3.0364296385396288, + "velocityY": -0.24039319153098848, + "timestamp": 2.483183945031913 + }, + { + "x": 7.041349782094471, + "y": 0.7761674353551624, + "heading": -0.07636428899969135, + "angularVelocity": 0.8092927650329051, + "velocityX": 2.7637764879226316, + "velocityY": -0.2007897488447803, + "timestamp": 2.5465785971985944 + }, + { + "x": 7.199320466835692, + "y": 0.7658884300843142, + "heading": -0.0333868413674214, + "angularVelocity": 0.677934907179092, + "velocityX": 2.4918613690926774, + "velocityY": -0.16214309755690418, + "timestamp": 2.6099732493652756 + }, + { + "x": 7.340087890625, + "y": 0.7580231428146362, + "heading": 6.333223838299427e-25, + "angularVelocity": 0.5266507540673704, + "velocityX": 2.2204936690746884, + "velocityY": -0.12406862410094172, + "timestamp": 2.673367901531957 + }, + { + "x": 7.464023523183681, + "y": 0.7525372146747857, + "heading": 0.023010119033740877, + "angularVelocity": 0.36178962784032076, + "velocityX": 1.9486481714332364, + "velocityY": -0.08625561202725132, + "timestamp": 2.7369687271584358 + }, + { + "x": 7.57058232073761, + "y": 0.7491648361128735, + "heading": 0.03698369983604058, + "angularVelocity": 0.21970753782922778, + "velocityX": 1.675431042039261, + "velocityY": -0.05302413182051879, + "timestamp": 2.8005695527849146 + }, + { + "x": 7.659708945960142, + "y": 0.7476928559896933, + "heading": 0.042953350833786465, + "angularVelocity": 0.09386121860752329, + "velocityX": 1.4013438401879301, + "velocityY": -0.02314404111394868, + "timestamp": 2.8641703784113934 + }, + { + "x": 7.731365799776583, + "y": 0.747958827951943, + "heading": 0.041693436078442773, + "angularVelocity": -0.01980972326905067, + "velocityX": 1.1266654655911963, + "velocityY": 0.0041818948045075285, + "timestamp": 2.9277712040378723 + }, + { + "x": 7.785525988086016, + "y": 0.7498349377828146, + "heading": 0.03380693250989334, + "angularVelocity": -0.1240000187240661, + "velocityX": 0.85156423326185, + "velocityY": 0.029498199314107475, + "timestamp": 2.991372029664351 + }, + { + "x": 7.822169493216012, + "y": 0.7532180358509858, + "heading": 0.01977708439944101, + "angularVelocity": -0.2205922324475507, + "velocityX": 0.5761482617411275, + "velocityY": 0.05319267532845736, + "timestamp": 3.05497285529083 + }, + { + "x": 7.841280937194824, + "y": 0.7580231428146362, + "heading": -7.691291273987186e-25, + "angularVelocity": -0.3109564098364043, + "velocityX": 0.30049050135059296, + "velocityY": 0.07555101551464699, + "timestamp": 3.118573680917309 + }, + { + "x": 7.837626455350944, + "y": 0.7661832230204207, + "heading": -0.033360531574280844, + "angularVelocity": -0.41786111810293475, + "velocityX": -0.045774626401580844, + "velocityY": 0.10221000918425123, + "timestamp": 3.1984100909803836 + }, + { + "x": 7.806322738977659, + "y": 0.7764707725073046, + "heading": -0.07501113928857397, + "angularVelocity": -0.5216994060903668, + "velocityX": -0.39209824625822215, + "velocityY": 0.12885786671465047, + "timestamp": 3.2782465010434585 + }, + { + "x": 7.747364252906707, + "y": 0.7888847511850451, + "heading": -0.12465356840287752, + "angularVelocity": -0.6218018705385611, + "velocityX": -0.7384911974921111, + "velocityY": 0.15549269647686922, + "timestamp": 3.3580829111065333 + }, + { + "x": 7.660744362341046, + "y": 0.8034239632171427, + "heading": -0.18191677773733375, + "angularVelocity": -0.7172568166481353, + "velocityX": -1.084967253628106, + "velocityY": 0.18211254765351886, + "timestamp": 3.437919321169608 + }, + { + "x": 7.5464550089450855, + "y": 0.820087049493035, + "heading": -0.24632578558189497, + "angularVelocity": -0.8067623255313559, + "velocityX": -1.4315442453595524, + "velocityY": 0.20871537513682828, + "timestamp": 3.517755731232683 + }, + { + "x": 7.4044862600725985, + "y": 0.8388724659564971, + "heading": -0.31724786378277436, + "angularVelocity": -0.8883425262339242, + "velocityX": -1.7782456495767374, + "velocityY": 0.23529886236894457, + "timestamp": 3.597592141295758 + }, + { + "x": 7.234825718466435, + "y": 0.859778393906575, + "heading": -0.393791306886596, + "angularVelocity": -0.958753569246769, + "velocityX": -2.1251023370430016, + "velocityY": 0.261859569256197, + "timestamp": 3.6774285513588327 + }, + { + "x": 7.037458039233374, + "y": 0.8828023593152449, + "heading": -0.47459284454035716, + "angularVelocity": -1.012088814989599, + "velocityX": -2.4721512287079195, + "velocityY": 0.2883892874251208, + "timestamp": 3.7572649614219076 + }, + { + "x": 6.812366683779643, + "y": 0.9079395598147495, + "heading": -0.5572950154955562, + "angularVelocity": -1.0358954127554088, + "velocityX": -2.8194072764030573, + "velocityY": 0.3148588529925756, + "timestamp": 3.8371013714849824 + }, + { + "x": 6.559557139520272, + "y": 0.9351738299996345, + "heading": -0.6368741877538795, + "angularVelocity": -0.9967779387306077, + "velocityX": -3.166594590859465, + "velocityY": 0.3411259369424114, + "timestamp": 3.9169377815480573 + }, + { + "x": 6.279455223531701, + "y": 0.9644007155406045, + "heading": -0.6963848224463702, + "angularVelocity": -0.7454071976116461, + "velocityX": -3.5084482852782983, + "velocityY": 0.3660846663556045, + "timestamp": 3.996774191611132 + }, + { + "x": 5.978414008634455, + "y": 0.9876513907684487, + "heading": -0.696384838019443, + "angularVelocity": -1.9506228555195058e-7, + "velocityX": -3.770725846257472, + "velocityY": 0.2912289669522333, + "timestamp": 4.076610601674207 + }, + { + "x": 5.677372675665616, + "y": 1.0109005372077842, + "heading": -0.6963848535904748, + "angularVelocity": -1.950367229938273e-7, + "velocityX": -3.770727325176581, + "velocityY": 0.2912098179385523, + "timestamp": 4.156447011737281 + }, + { + "x": 5.3763313426932635, + "y": 1.0341496836016242, + "heading": -0.6963848691615065, + "angularVelocity": -1.950367226738497e-7, + "velocityX": -3.770727325220591, + "velocityY": 0.29120981736869106, + "timestamp": 4.236283421800357 + }, + { + "x": 5.075290009720911, + "y": 1.0573988299954629, + "heading": -0.6963848847325383, + "angularVelocity": -1.950367234539369e-7, + "velocityX": -3.7707273252205917, + "velocityY": 0.2912098173686746, + "timestamp": 4.316119831863432 + }, + { + "x": 4.774248676748558, + "y": 1.080647976389303, + "heading": -0.6963849003035701, + "angularVelocity": -1.9503672325222526e-7, + "velocityX": -3.7707273252205904, + "velocityY": 0.2912098173686933, + "timestamp": 4.395956241926507 + }, + { + "x": 4.473207343780055, + "y": 1.1038971228329835, + "heading": -0.696384915874602, + "angularVelocity": -1.950367229679151e-7, + "velocityX": -3.7707273251723774, + "velocityY": 0.29120981799297246, + "timestamp": 4.4757926519895825 + }, + { + "x": 4.172166140154539, + "y": 1.1271479440122283, + "heading": -0.696384931445633, + "angularVelocity": -1.9503671234983974e-7, + "velocityX": -3.770725705072129, + "velocityY": 0.2912307950830405, + "timestamp": 4.555629062052658 + }, + { + "x": 3.8735860947392786, + "y": 1.1720518781991272, + "heading": -0.6963849471144347, + "angularVelocity": -1.9626135115744237e-7, + "velocityX": -3.739898189051426, + "velocityY": 0.5624493154366842, + "timestamp": 4.635465472115733 + }, + { + "x": 3.580403438648322, + "y": 1.2442345385888913, + "heading": -0.6963849633936577, + "angularVelocity": -2.0390725406473894e-7, + "velocityX": -3.672292577526056, + "velocityY": 0.9041320912693386, + "timestamp": 4.715301882178808 + }, + { + "x": 3.2951050269479554, + "y": 1.343084028059958, + "heading": -0.6963849809642987, + "angularVelocity": -2.2008305293195764e-7, + "velocityX": -3.5735375810982304, + "velocityY": 1.2381504803756895, + "timestamp": 4.795138292241884 + }, + { + "x": 3.020110820111339, + "y": 1.4677619727807747, + "heading": -0.6963850007479858, + "angularVelocity": -2.4780281394367673e-7, + "velocityX": -3.444471095573519, + "velocityY": 1.5616677230641343, + "timestamp": 4.874974702304959 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.6963850232502631, + "angularVelocity": -2.818548242668458e-7, + "velocityX": -3.286187939959726, + "velocityY": 1.8719390328536167, + "timestamp": 4.954811112368034 + }, + { + "x": 2.574184199627506, + "y": 1.7387873388514508, + "heading": -0.696385046672039, + "angularVelocity": -4.0231081168076635e-7, + "velocityX": -3.153128229528173, + "velocityY": 2.0882929651305573, + "timestamp": 5.013029224414923 + }, + { + "x": 2.3991894743560565, + "y": 1.8724111676120654, + "heading": -0.6963850671596432, + "angularVelocity": -3.5191117316400294e-7, + "velocityX": -3.005846790952465, + "velocityY": 2.295227791876793, + "timestamp": 5.071247336461811 + }, + { + "x": 2.2335584944607234, + "y": 2.017479637711806, + "heading": -0.6963850855810837, + "angularVelocity": -3.164211261355859e-7, + "velocityX": -2.8450077488245626, + "velocityY": 2.4918099367925968, + "timestamp": 5.129465448508699 + }, + { + "x": 2.0780383142752217, + "y": 2.1733384097975033, + "heading": -0.6963851025529307, + "angularVelocity": -2.915217692304984e-7, + "velocityX": -2.6713367149427287, + "velocityY": 2.6771526352515695, + "timestamp": 5.1876835605555875 + }, + { + "x": 1.933330362591389, + "y": 2.339284462910405, + "heading": -0.6963851185385084, + "angularVelocity": -2.745808308512789e-7, + "velocityX": -2.4856173894352156, + "velocityY": 2.8504196937758937, + "timestamp": 5.245901672602476 + }, + { + "x": 1.8000872328431394, + "y": 2.5145692385052043, + "heading": -0.6963851339074891, + "angularVelocity": -2.63989679511656e-7, + "velocityX": -2.2886886067507177, + "velocityY": 3.010828923027044, + "timestamp": 5.304119784649364 + }, + { + "x": 1.6789094250107635, + "y": 2.6984018342496054, + "heading": -0.6963851489749523, + "angularVelocity": -2.5881057720147785e-7, + "velocityX": -2.0814451649476586, + "velocityY": 3.1576529928752177, + "timestamp": 5.3623378966962525 + }, + { + "x": 1.5665736783825392, + "y": 2.887766995894858, + "heading": -0.6963851639898362, + "angularVelocity": -2.5790743394160864e-7, + "velocityX": -1.9295669797356307, + "velocityY": 3.2526846884478298, + "timestamp": 5.420556008743141 + }, + { + "x": 1.454045590270684, + "y": 3.0767463661393277, + "heading": -0.6971250291224751, + "angularVelocity": -0.012708504391950055, + "velocityX": -1.9328707880672382, + "velocityY": 3.246058032460206, + "timestamp": 5.478774120790029 + }, + { + "x": 1.3486316890487728, + "y": 3.2544801987729373, + "heading": -0.7253513053199856, + "angularVelocity": -0.48483668063260893, + "velocityX": -1.8106719286433102, + "velocityY": 3.0528958494989498, + "timestamp": 5.536992232836917 + }, + { + "x": 1.250710894485142, + "y": 3.4195717811137616, + "heading": -0.7601787566171799, + "angularVelocity": -0.5982236467775679, + "velocityX": -1.6819644457856464, + "velocityY": 2.8357426329432034, + "timestamp": 5.595210344883806 + }, + { + "x": 1.1603283095238273, + "y": 3.571951341403833, + "heading": -0.796970435726129, + "angularVelocity": -0.631962765802461, + "velocityX": -1.5524822393505606, + "velocityY": 2.6173909618942433, + "timestamp": 5.653428456930694 + }, + { + "x": 1.0774888501735351, + "y": 3.7116123829065786, + "heading": -0.8336344586551708, + "angularVelocity": -0.6297700430325383, + "velocityX": -1.4229155916903404, + "velocityY": 2.398927697797285, + "timestamp": 5.711646568977582 + }, + { + "x": 1.0021907960664813, + "y": 3.8385581534626447, + "heading": -0.8689739272088923, + "angularVelocity": -0.6070184571643223, + "velocityX": -1.2933784944178508, + "velocityY": 2.1805202211611707, + "timestamp": 5.769864681024471 + }, + { + "x": 0.9344313404313289, + "y": 3.952793532693306, + "heading": -0.9022118371667394, + "angularVelocity": -0.5709204367719397, + "velocityX": -1.1638896084534611, + "velocityY": 1.9621965607310954, + "timestamp": 5.828082793071359 + }, + { + "x": 0.8742077657212176, + "y": 4.054323286032487, + "heading": -0.932802050953144, + "angularVelocity": -0.5254415286048384, + "velocityX": -1.034447401207516, + "velocityY": 1.743954755135484, + "timestamp": 5.886300905118247 + }, + { + "x": 0.8215176831864053, + "y": 4.143151677614014, + "heading": -0.9603390839625449, + "angularVelocity": -0.4729976985035671, + "velocityX": -0.9050462250025629, + "velocityY": 1.5257861936502937, + "timestamp": 5.944519017165136 + }, + { + "x": 0.7763590420271476, + "y": 4.219282430243998, + "heading": -0.9845095976890542, + "angularVelocity": -0.4151717202207242, + "velocityX": -0.7756802749441201, + "velocityY": 1.3076815780056374, + "timestamp": 6.002737129212024 + }, + { + "x": 0.7387300838441436, + "y": 4.282718775684524, + "heading": -1.005063997176884, + "angularVelocity": -0.3530585030183643, + "velocityX": -0.6463445285325996, + "velocityY": 1.0896324736438516, + "timestamp": 6.060955241258912 + }, + { + "x": 0.7086292901576616, + "y": 4.333463522187373, + "heading": -1.0217986905854597, + "angularVelocity": -0.2874482325207969, + "velocityX": -0.5170348647211226, + "velocityY": 0.871631606019453, + "timestamp": 6.119173353305801 + }, + { + "x": 0.6860553355642394, + "y": 4.371519118144364, + "heading": -1.0345444276715927, + "angularVelocity": -0.21893078696656726, + "velocityX": -0.3877479670800986, + "velocityY": 0.6536727938951569, + "timestamp": 6.177391465352689 + }, + { + "x": 0.6710070488250989, + "y": 4.396887706657534, + "heading": -1.043158306951245, + "angularVelocity": -0.1479587533294601, + "velocityX": -0.25848118755587396, + "velocityY": 0.4357507933740352, + "timestamp": 6.235609577399577 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -0.07488724125811806, + "velocityX": -0.12923242079161093, + "velocityY": 0.2178611381134427, + "timestamp": 6.2938276894464655 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": 5.982509217155958e-25, + "velocityX": 4.848331418564855e-25, + "velocityY": 2.001430584095579e-25, + "timestamp": 6.352045801493354 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubSprint.2.traj b/src/main/deploy/choreo/SourceLanePSubHSubSprint.2.traj index 739cb370..d3c0bd5a 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubSprint.2.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubSprint.2.traj @@ -1,445 +1,446 @@ { - "samples": [ - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -4.5881817726943837e-32, - "velocityX": 7.1951222626888875e-34, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 0.6704852067504524, - "y": 4.397985362145517, - "heading": -1.0417684453451177, - "angularVelocity": 0.10293302663784343, - "velocityX": 0.12534996227388928, - "velocityY": -0.20741457823419143, - "timestamp": 0.055858217681718614 - }, - { - "x": 0.684490344241533, - "y": 4.374811471822251, - "heading": -1.0303744644017594, - "angularVelocity": 0.20398038849505432, - "velocityX": 0.25072653715666804, - "velocityY": -0.4148698487895252, - "timestamp": 0.11171643536343723 - }, - { - "x": 0.7055004573054668, - "y": 4.340046908929331, - "heading": -1.0134578875635165, - "angularVelocity": 0.3028484892703545, - "velocityX": 0.3761328938859112, - "velocityY": -0.6223715029901059, - "timestamp": 0.16757465304515584 - }, - { - "x": 0.7335174330880717, - "y": 4.293688705050122, - "heading": -0.9911609198549822, - "angularVelocity": 0.3991707690278106, - "velocityX": 0.5015730351843025, - "velocityY": -0.8299262991769599, - "timestamp": 0.22343287072687446 - }, - { - "x": 0.768543442750825, - "y": 4.235733440885856, - "heading": -0.963651742055662, - "angularVelocity": 0.49248219762521406, - "velocityX": 0.6270520456333297, - "velocityY": -1.0375423092533531, - "timestamp": 0.27929108840859307 - }, - { - "x": 0.8105810218384888, - "y": 4.16617715590748, - "heading": -0.9311321668224927, - "angularVelocity": 0.5821806814257259, - "velocityX": 0.7525764485933851, - "velocityY": -1.24522922257758, - "timestamp": 0.3351493060903117 - }, - { - "x": 0.8596331801158593, - "y": 4.085015237354172, - "heading": -0.8938487217946981, - "angularVelocity": 0.667465711853477, - "velocityX": 0.8781547337738402, - "velocityY": -1.4529987157085942, - "timestamp": 0.3910075237720303 - }, - { - "x": 0.9157035549190764, - "y": 3.992242285598939, - "heading": -0.8521094876337724, - "angularVelocity": 0.7472353378469229, - "velocityX": 1.0037981362510981, - "velocityY": -1.6608648754934532, - "timestamp": 0.4468657414537489 - }, - { - "x": 0.9787966256099634, - "y": 3.88785195918136, - "heading": -0.8063112110880574, - "angularVelocity": 0.8199022175514963, - "velocityX": 1.1295217303637053, - "velocityY": -1.868844563075711, - "timestamp": 0.5027239591354675 - }, - { - "x": 1.0489179988671087, - "y": 3.7718368324625953, - "heading": -0.756986124262755, - "angularVelocity": 0.8830408285913816, - "velocityX": 1.2553456978648867, - "velocityY": -2.0769571879257267, - "timestamp": 0.5585821768171861 - }, - { - "x": 1.1260747058581024, - "y": 3.644188427808813, - "heading": -0.7048901245652565, - "angularVelocity": 0.9326470098695786, - "velocityX": 1.381295540624565, - "velocityY": -2.2852215833510225, - "timestamp": 0.6144403944989048 - }, - { - "x": 1.210275065586603, - "y": 3.504898185530833, - "heading": -0.6511892097412871, - "angularVelocity": 0.9613789528688276, - "velocityX": 1.5073943140877994, - "velocityY": -2.4936392183449136, - "timestamp": 0.6702986121806234 - }, - { - "x": 1.3015253174439423, - "y": 3.3539635614800978, - "heading": -0.5979268038301901, - "angularVelocity": 0.95352856073188, - "velocityX": 1.6336047880597582, - "velocityY": -2.7021023998790135, - "timestamp": 0.726156829862342 - }, - { - "x": 1.3998011054306767, - "y": 3.1914330516367446, - "heading": -0.5495841694257507, - "angularVelocity": 0.865452504766571, - "velocityX": 1.7593792295112722, - "velocityY": -2.909697383641142, - "timestamp": 0.7820150475440606 - }, - { - "x": 1.504860441802437, - "y": 3.018388823031747, - "heading": -0.5232691895419826, - "angularVelocity": 0.4711031066854229, - "velocityX": 1.8808214929160718, - "velocityY": -3.0979189058807544, - "timestamp": 0.8378732652257792 - }, - { - "x": 1.6140923684634172, - "y": 2.8375675393930413, - "heading": -0.5232691688026271, - "angularVelocity": 3.712856650457212e-7, - "velocityX": 1.9555211604385054, - "velocityY": -3.237147391079152, - "timestamp": 0.8937314829074978 - }, - { - "x": 1.7233256383679056, - "y": 2.6567470671976134, - "heading": -0.5232691480644303, - "angularVelocity": 3.7126492252401944e-7, - "velocityX": 1.9555452078135154, - "velocityY": -3.2371328642411785, - "timestamp": 0.9495897005892164 - }, - { - "x": 1.84078732146222, - "y": 2.4811603268646896, - "heading": -0.5232691272677354, - "angularVelocity": 3.72312181556421e-7, - "velocityX": 2.102854118325333, - "velocityY": -3.143436142796787, - "timestamp": 1.005447918270935 - }, - { - "x": 1.9693130123787856, - "y": 2.313502648911294, - "heading": -0.5232691060970931, - "angularVelocity": 3.790067651022537e-7, - "velocityX": 2.3009271733106282, - "velocityY": -3.0014863508304788, - "timestamp": 1.0613061359526537 - }, - { - "x": 2.1083695489461487, - "y": 2.154470483882732, - "heading": -0.5232690841533676, - "angularVelocity": 3.928468612451328e-7, - "velocityX": 2.4894553091491622, - "velocityY": -2.847068374697022, - "timestamp": 1.1171643536343723 - }, - { - "x": 2.2573796500994945, - "y": 2.0047242347352134, - "heading": -0.5232690609733034, - "angularVelocity": 4.1498037571099624e-7, - "velocityX": 2.6676486887284834, - "velocityY": -2.6808275552359513, - "timestamp": 1.173022571316091 - }, - { - "x": 2.4157246359676448, - "y": 1.8648857059384452, - "heading": -0.5232690359846437, - "angularVelocity": 4.47358702857604e-7, - "velocityX": 2.8347661712087695, - "velocityY": -2.5034549006481424, - "timestamp": 1.2288807889978095 - }, - { - "x": 2.582747044375024, - "y": 1.7355355500002163, - "heading": -0.523269008439205, - "angularVelocity": 4.931313565110266e-7, - "velocityX": 2.9901134575950428, - "velocityY": -2.3156871326484083, - "timestamp": 1.2847390066795281 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.5232689753097743, - "angularVelocity": 5.930985989560848e-7, - "velocityX": 3.133045325838173, - "velocityY": -2.1183039826598433, - "timestamp": 1.3405972243612467 - }, - { - "x": 3.027691141146945, - "y": 1.4695236072310565, - "heading": -0.5232689479285146, - "angularVelocity": 3.365468040910692e-7, - "velocityX": 3.3178420048715034, - "velocityY": -1.8152442667680868, - "timestamp": 1.4219566647921713 - }, - { - "x": 3.3102859882061892, - "y": 1.3477940719025636, - "heading": -0.523268924510295, - "angularVelocity": 2.8783653775450794e-7, - "velocityX": 3.473411881429686, - "velocityY": -1.4961943529078627, - "timestamp": 1.5033161052230959 - }, - { - "x": 3.6030485594101127, - "y": 1.2530945172715204, - "heading": -0.5232689034957836, - "angularVelocity": 2.582922326531371e-7, - "velocityX": 3.5983847682984376, - "velocityY": -1.1639651665432065, - "timestamp": 1.5846755456540205 - }, - { - "x": 3.9033999272897986, - "y": 1.1862590073495238, - "heading": -0.5232688838271395, - "angularVelocity": 2.4174999152857024e-7, - "velocityX": 3.6916597052396023, - "velocityY": -0.8214843854382324, - "timestamp": 1.666034986084945 - }, - { - "x": 4.2086942600952995, - "y": 1.1478754132776305, - "heading": -0.5232688646920325, - "angularVelocity": 2.3519221562308788e-7, - "velocityX": 3.752414362592638, - "velocityY": -0.471777999806691, - "timestamp": 1.7473944265158696 - }, - { - "x": 4.515097551396115, - "y": 1.1196805012022473, - "heading": -0.5232688456111896, - "angularVelocity": 2.345252478429259e-7, - "velocityX": 3.7660447229963117, - "velocityY": -0.3465475171172218, - "timestamp": 1.8287538669467942 - }, - { - "x": 4.821500870663513, - "y": 1.0914858930504614, - "heading": -0.5232688265303463, - "angularVelocity": 2.3452525108540152e-7, - "velocityX": 3.7660450667373904, - "velocityY": -0.3465437815507533, - "timestamp": 1.9101133073777188 - }, - { - "x": 5.127904189931636, - "y": 1.0632912849065406, - "heading": -0.5232688074495031, - "angularVelocity": 2.3452525107290264e-7, - "velocityX": 3.7660450667462855, - "velocityY": -0.34654378145408427, - "timestamp": 1.9914727478086434 - }, - { - "x": 5.4343075091997575, - "y": 1.0350966767626197, - "heading": -0.5232687883686599, - "angularVelocity": 2.3452525075472094e-7, - "velocityX": 3.7660450667462855, - "velocityY": -0.3465437814540822, - "timestamp": 2.072832188239568 - }, - { - "x": 5.740710828467879, - "y": 1.0069020686186991, - "heading": -0.5232687692878167, - "angularVelocity": 2.3452525040262446e-7, - "velocityX": 3.7660450667462855, - "velocityY": -0.3465437814540818, - "timestamp": 2.1541916286704925 - }, - { - "x": 6.047114147736001, - "y": 0.9787074604747784, - "heading": -0.5232687502069736, - "angularVelocity": 2.345252500146494e-7, - "velocityX": 3.766045066746286, - "velocityY": -0.34654378145408327, - "timestamp": 2.235551069101417 - }, - { - "x": 6.353517467003837, - "y": 0.9505128523277552, - "heading": -0.5232687311261306, - "angularVelocity": 2.3452524994005725e-7, - "velocityX": 3.7660450667427767, - "velocityY": -0.3465437814922157, - "timestamp": 2.3169105095323417 - }, - { - "x": 6.659920775239498, - "y": 0.9223181242925758, - "heading": -0.5232687120445318, - "angularVelocity": 2.3453453739742123e-7, - "velocityX": 3.7660449311448136, - "velocityY": -0.3465452550539253, - "timestamp": 2.3982699499632663 - }, - { - "x": 6.946350295372631, - "y": 0.8955257235290481, - "heading": -0.4609901004295649, - "angularVelocity": 0.7654749256522044, - "velocityX": 3.5205443721840264, - "velocityY": -0.3293090589318255, - "timestamp": 2.479629390394191 - }, - { - "x": 7.204305307998309, - "y": 0.8715129005451068, - "heading": -0.38283013166037505, - "angularVelocity": 0.9606748566019065, - "velocityX": 3.170560309404857, - "velocityY": -0.2951448886171808, - "timestamp": 2.5609888308251154 - }, - { - "x": 7.433510095635826, - "y": 0.8502195253793108, - "heading": -0.30543025852913497, - "angularVelocity": 0.9513324172497697, - "velocityX": 2.8171873653938824, - "velocityY": -0.2617197838752883, - "timestamp": 2.64234827125604 - }, - { - "x": 7.633994056446923, - "y": 0.8316157363547472, - "heading": -0.2335287634090791, - "angularVelocity": 0.8837511017679792, - "velocityX": 2.4641757582060952, - "velocityY": -0.22866171308489489, - "timestamp": 2.7237077116869646 - }, - { - "x": 7.805788503499911, - "y": 0.8156861724274476, - "heading": -0.16939985967365537, - "angularVelocity": 0.7882171189447914, - "velocityX": 2.1115490242198023, - "velocityY": -0.19579244698498122, - "timestamp": 2.805067152117889 - }, - { - "x": 7.948917170139319, - "y": 0.802421883536533, - "heading": -0.1143881084380515, - "angularVelocity": 0.6761569517222741, - "velocityX": 1.759214000014262, - "velocityY": -0.16303318730634864, - "timestamp": 2.8864265925488137 - }, - { - "x": 8.063397872753223, - "y": 0.7918170647898031, - "heading": -0.0693839808587397, - "angularVelocity": 0.5531518818338115, - "velocityX": 1.407097959469132, - "velocityY": -0.13034527635097967, - "timestamp": 2.9677860329797383 - }, - { - "x": 8.149244274991434, - "y": 0.7838676118817843, - "heading": -0.0350206058237791, - "angularVelocity": 0.4223649382659615, - "velocityX": 1.0551498607109575, - "velocityY": -0.09770781197503392, - "timestamp": 3.049145473410663 - }, - { - "x": 8.20646710981356, - "y": 0.7785704017832406, - "heading": -0.011770656016753542, - "angularVelocity": 0.2857683101540642, - "velocityX": 0.703333682225969, - "velocityY": -0.06510873317818848, - "timestamp": 3.1305049138415875 - }, - { - "x": 8.235074996948242, - "y": 0.7759228944778442, - "heading": -3.5413188929792264e-38, - "angularVelocity": 0.1446747415470122, - "velocityX": 0.35162345000356726, - "velocityY": -0.03254087406911382, - "timestamp": 3.211864354272512 - }, - { - "x": 8.235074996948242, - "y": 0.7759228944778442, - "heading": -7.544724392466369e-38, - "angularVelocity": -4.920640404901405e-37, - "velocityX": -6.041520920103385e-38, - "velocityY": 3.1783876166730987e-37, - "timestamp": 3.2932237947034366 - } - ] + "samples": [ + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": 5.982509217155958e-25, + "velocityX": 4.848331418564855e-25, + "velocityY": 2.001430584095579e-25, + "timestamp": 0 + }, + { + "x": 0.6704852066209848, + "y": 4.3979853620663825, + "heading": -1.0417684451600073, + "angularVelocity": 0.10293302967346842, + "velocityX": 0.1253499596171797, + "velocityY": -0.20741457909008942, + "timestamp": 0.05585821783274714 + }, + { + "x": 0.684490343853258, + "y": 4.3748114715841036, + "heading": -1.0303744638705372, + "angularVelocity": 0.20398039413979072, + "velocityX": 0.2507265318454658, + "velocityY": -0.414869850514514, + "timestamp": 0.11171643566549427 + }, + { + "x": 0.7055004565291726, + "y": 4.340046908451465, + "heading": -1.013457886552716, + "angularVelocity": 0.3028484970371536, + "velocityX": 0.37613288592242283, + "velocityY": -0.6223715055989062, + "timestamp": 0.1675746534982414 + }, + { + "x": 0.7335174317946694, + "y": 4.293688704250901, + "heading": -0.9911609182628701, + "angularVelocity": 0.39917077835544595, + "velocityX": 0.501573024570643, + "velocityY": -0.8299263026860625, + "timestamp": 0.22343287133098855 + }, + { + "x": 0.7685434408113375, + "y": 4.235733439682599, + "heading": -0.9636517398174463, + "angularVelocity": 0.4924822078604928, + "velocityX": 0.6270520323713931, + "velocityY": -1.0375423136812831, + "timestamp": 0.2792910891637357 + }, + { + "x": 0.8105810191240276, + "y": 4.166177154216333, + "heading": -0.9311321639168866, + "angularVelocity": 0.5821806917995676, + "velocityX": 0.7525764326846329, + "velocityY": -1.2452292279451722, + "timestamp": 0.3351493069964828 + }, + { + "x": 0.8596331764975813, + "y": 4.085015235089962, + "heading": -0.8938487182523878, + "angularVelocity": 0.6674657214473685, + "velocityX": 0.8781547152189431, + "velocityY": -1.4529987220392506, + "timestamp": 0.39100752482922996 + }, + { + "x": 0.9157035502681051, + "y": 3.9922422826750155, + "heading": -0.85210948354869, + "angularVelocity": 0.7472353455435148, + "velocityX": 1.0037981150492816, + "velocityY": -1.660864882813316, + "timestamp": 0.4468657426619771 + }, + { + "x": 0.9787966197972374, + "y": 3.8878519555094497, + "heading": -0.8063112066330053, + "angularVelocity": 0.8199022219580233, + "velocityX": 1.1295217065114396, + "velocityY": -1.8688445714135682, + "timestamp": 0.5027239604947242 + }, + { + "x": 1.0489179917630689, + "y": 3.7718368279526966, + "heading": -0.7569861197121166, + "angularVelocity": 0.883040827915056, + "velocityX": 1.255345671352987, + "velocityY": -2.076957197312126, + "timestamp": 0.5585821783274714 + }, + { + "x": 1.1260746973319842, + "y": 3.6441884223692846, + "heading": -0.7048901203297451, + "angularVelocity": 0.9326470017063425, + "velocityX": 1.3812955114311163, + "velocityY": -2.2852215938149305, + "timestamp": 0.6144403961602185 + }, + { + "x": 1.2102750555046553, + "y": 3.5048981790691265, + "heading": -0.6511892064235582, + "angularVelocity": 0.9613789338388887, + "velocityX": 1.5073942821589288, + "velocityY": -2.493639229902148, + "timestamp": 0.6702986139929656 + }, + { + "x": 1.301525305664333, + "y": 3.3539635539059023, + "heading": -0.5979268023129009, + "angularVelocity": 0.9535285259214237, + "velocityX": 1.6336047532505, + "velocityY": -2.7021024124893804, + "timestamp": 0.7261568318257128 + }, + { + "x": 1.3998010917877863, + "y": 3.1914330428750026, + "heading": -0.5495841709370638, + "angularVelocity": 0.8654524482071109, + "velocityX": 1.759379191396948, + "velocityY": -2.909697397033947, + "timestamp": 0.7820150496584599 + }, + { + "x": 1.504860426118042, + "y": 3.0183888128426037, + "heading": -0.5232691930367682, + "angularVelocity": 0.4711030699026016, + "velocityX": 1.880821451282748, + "velocityY": -3.0979189230586286, + "timestamp": 0.837873267491207 + }, + { + "x": 1.6140923512047893, + "y": 2.837567528819654, + "heading": -0.5232691722973926, + "angularVelocity": 3.712860260075055e-7, + "velocityX": 1.9555211269685275, + "velocityY": -3.2371473892055076, + "timestamp": 0.8937314853239542 + }, + { + "x": 1.723325651827911, + "y": 2.656747075748015, + "heading": -0.5232691515591942, + "angularVelocity": 3.712649483056031e-7, + "velocityX": 1.9555457524655004, + "velocityY": -3.2371325131256046, + "timestamp": 0.9495897031567013 + }, + { + "x": 1.840787332697237, + "y": 2.481160334510265, + "heading": -0.5232691307624974, + "angularVelocity": 3.7231221667363717e-7, + "velocityX": 2.1028540728068745, + "velocityY": -3.143436150496223, + "timestamp": 1.0054479209894485 + }, + { + "x": 1.969313021479893, + "y": 2.3135026555322407, + "heading": -0.5232691095918528, + "angularVelocity": 3.790068020002191e-7, + "velocityX": 2.300927128887148, + "velocityY": -3.0014863610584763, + "timestamp": 1.0613061388221956 + }, + { + "x": 2.108369556011401, + "y": 2.15447048936791, + "heading": -0.5232690876481252, + "angularVelocity": 3.9284689919031285e-7, + "velocityX": 2.4894552659713574, + "velocityY": -2.8470683873322, + "timestamp": 1.1171643566549427 + }, + { + "x": 2.2573796552323504, + "y": 2.004724238981821, + "heading": -0.5232690644680587, + "angularVelocity": 4.1498041678709737e-7, + "velocityX": 2.6676486469210485, + "velocityY": -2.680827570161024, + "timestamp": 1.1730225744876899 + }, + { + "x": 2.4157246392762546, + "y": 1.8648857088521948, + "heading": -0.5232690394793963, + "angularVelocity": 4.473587475801905e-7, + "velocityX": 2.8347661308856478, + "velocityY": -2.5034549177407666, + "timestamp": 1.228880792320437 + }, + { + "x": 2.582747045971562, + "y": 1.7355355514954984, + "heading": -0.5232690119339547, + "angularVelocity": 4.931314075834542e-7, + "velocityX": 2.990113418860087, + "velocityY": -2.3156871517813467, + "timestamp": 1.2847390101531841 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.5232689788045201, + "angularVelocity": 5.930986675824575e-7, + "velocityX": 3.1330452887851035, + "velocityY": -2.1183040037016325, + "timestamp": 1.3405972279859313 + }, + { + "x": 3.027691139544385, + "y": 1.469523604710322, + "heading": -0.5232689514232575, + "angularVelocity": 3.3654683913245854e-7, + "velocityX": 3.317841970698504, + "velocityY": -1.8152442898308883, + "timestamp": 1.4219566687718261 + }, + { + "x": 3.310285985294353, + "y": 1.3477940668377997, + "heading": -0.5232689280050355, + "angularVelocity": 2.8783656375516125e-7, + "velocityX": 3.473411850182739, + "velocityY": -1.4961943776489928, + "timestamp": 1.503316109557721 + }, + { + "x": 3.603048555472094, + "y": 1.2530945096711932, + "heading": -0.5232689069905222, + "angularVelocity": 2.58292255489841e-7, + "velocityX": 3.5983847399857765, + "velocityY": -1.1639651926297916, + "timestamp": 1.5846755503436158 + }, + { + "x": 3.9033999225952147, + "y": 1.1862589972516577, + "heading": -0.5232688873218765, + "angularVelocity": 2.41750010150048e-7, + "velocityX": 3.6916596798338883, + "velocityY": -0.8214844125516969, + "timestamp": 1.6660349911295107 + }, + { + "x": 4.208694254897112, + "y": 1.1478754007454768, + "heading": -0.5232688681867681, + "angularVelocity": 2.3519223109451003e-7, + "velocityX": 3.7524143400310193, + "velocityY": -0.47177802766849325, + "timestamp": 1.7473944319154064 + }, + { + "x": 4.515097545025261, + "y": 1.1196804780654097, + "heading": -0.523268849105924, + "angularVelocity": 2.3452526230425957e-7, + "velocityX": 3.7660446921516857, + "velocityY": -0.34654764594885085, + "timestamp": 1.8287538727013022 + }, + { + "x": 4.821500864274129, + "y": 1.0914858718513367, + "heading": -0.5232688300250796, + "angularVelocity": 2.3452526515688585e-7, + "velocityX": 3.766045050078413, + "velocityY": -0.34654375622209216, + "timestamp": 1.910113313487198 + }, + { + "x": 5.1279041835237855, + "y": 1.0632912656458422, + "heading": -0.523268810944235, + "angularVelocity": 2.3452526522511142e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.34654375611665356, + "timestamp": 1.9914727542730937 + }, + { + "x": 5.434307502773443, + "y": 1.035096659440348, + "heading": -0.5232687918633908, + "angularVelocity": 2.3452526444881321e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.3465437561166505, + "timestamp": 2.0728321950589894 + }, + { + "x": 5.7407108220231, + "y": 1.0069020532348534, + "heading": -0.5232687727825464, + "angularVelocity": 2.345252646023774e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.34654375611665045, + "timestamp": 2.154191635844885 + }, + { + "x": 6.047114141272757, + "y": 0.9787074470293591, + "heading": -0.5232687537017021, + "angularVelocity": 2.3452526402445505e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.3465437561166515, + "timestamp": 2.235551076630781 + }, + { + "x": 6.353517460522115, + "y": 0.9505128408206217, + "heading": -0.5232687346208578, + "angularVelocity": 2.3452526381848406e-7, + "velocityX": 3.766045050084448, + "velocityY": -0.3465437561565115, + "timestamp": 2.3169105174166766 + }, + { + "x": 6.659920768762443, + "y": 0.9223181149752775, + "heading": -0.5232687155392478, + "angularVelocity": 2.3453467400509184e-7, + "velocityX": 3.7660449147709487, + "velocityY": -0.3465452266263895, + "timestamp": 2.3982699582025724 + }, + { + "x": 6.9463502900273895, + "y": 0.8955257159158118, + "heading": -0.4609901078524047, + "angularVelocity": 0.7654748740313396, + "velocityX": 3.5205443707352013, + "velocityY": -0.3293090365501957, + "timestamp": 2.479629398988468 + }, + { + "x": 7.204305303751847, + "y": 0.8715128944520383, + "heading": -0.38283014022876516, + "angularVelocity": 0.9606748383303768, + "velocityX": 3.1705603090769854, + "velocityY": -0.29514486864487394, + "timestamp": 2.560988839774364 + }, + { + "x": 7.433510092361594, + "y": 0.8502195206373326, + "heading": -0.3054302667455531, + "angularVelocity": 0.9513324174252511, + "velocityX": 2.8171873650523347, + "velocityY": -0.2617197661269713, + "timestamp": 2.6423482805602596 + }, + { + "x": 7.6339940540118905, + "y": 0.8316157327961914, + "heading": -0.23352877050863402, + "angularVelocity": 0.8837511116397012, + "velocityX": 2.464175757769631, + "velocityY": -0.2286616975416389, + "timestamp": 2.7237077213461554 + }, + { + "x": 7.805788501774165, + "y": 0.8156861698842703, + "heading": -0.16939986530394652, + "angularVelocity": 0.7882171335647298, + "velocityX": 2.111549023725079, + "velocityY": -0.1957924336505852, + "timestamp": 2.805067162132051 + }, + { + "x": 7.948917168997056, + "y": 0.8024218818402508, + "heading": -0.11438811251079256, + "angularVelocity": 0.6761569679162687, + "velocityX": 1.7592139995104843, + "velocityY": -0.16303317618573446, + "timestamp": 2.886426602917947 + }, + { + "x": 8.063397872072363, + "y": 0.7918170637715687, + "heading": -0.06938398346942785, + "angularVelocity": 0.5531518973907075, + "velocityX": 1.4070979590011485, + "velocityY": -0.13034526744830524, + "timestamp": 2.9677860437038426 + }, + { + "x": 8.149244274653059, + "y": 0.7838676113724481, + "heading": -0.03502060720303478, + "angularVelocity": 0.422364951558892, + "velocityX": 1.0551498603168807, + "velocityY": -0.09770780529379805, + "timestamp": 3.0491454844897383 + }, + { + "x": 8.206467109701398, + "y": 0.7785704016133925, + "heading": -0.011770656498572084, + "angularVelocity": 0.2857683199377824, + "velocityX": 0.7033336819377413, + "velocityY": -0.06510872872142374, + "timestamp": 3.130504925275634 + }, + { + "x": 8.235074996948242, + "y": 0.7759228944778442, + "heading": 4.4114982490549674e-27, + "angularVelocity": 0.14467474683789522, + "velocityX": 0.35162344984804855, + "velocityY": -0.032540871839514214, + "timestamp": 3.21186436606153 + }, + { + "x": 8.235074996948242, + "y": 0.7759228944778442, + "heading": 2.1650776701546007e-27, + "angularVelocity": -8.454240815185174e-28, + "velocityX": -3.4252443089053894e-26, + "velocityY": -2.509402795665863e-26, + "timestamp": 3.2932238068474255 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePSubHSubSprint.traj b/src/main/deploy/choreo/SourceLanePSubHSubSprint.traj index 5d530312..563beaf4 100644 --- a/src/main/deploy/choreo/SourceLanePSubHSubSprint.traj +++ b/src/main/deploy/choreo/SourceLanePSubHSubSprint.traj @@ -1,1336 +1,1337 @@ { - "samples": [ - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": 0, - "velocityX": 2.1342037757525677e-32, - "velocityY": 1.1024409975331184e-32, - "timestamp": 0 - }, - { - "x": 0.6705320996855912, - "y": 4.398099342118358, - "heading": -1.0386114127976833, - "angularVelocity": 0.15927551181553457, - "velocityX": 0.12605002427557185, - "velocityY": -0.20514712032687507, - "timestamp": 0.05592000838348859 - }, - { - "x": 0.6846335258864488, - "y": 4.375148415401169, - "heading": -1.0210111049652704, - "angularVelocity": 0.3147407938803157, - "velocityX": 0.25217138924859706, - "velocityY": -0.41042423598715294, - "timestamp": 0.11184001676697718 - }, - { - "x": 0.7057920861320249, - "y": 4.340710150429397, - "heading": -0.9949634737692402, - "angularVelocity": 0.4658016325283909, - "velocityX": 0.3783719076090775, - "velocityY": -0.6158487090273922, - "timestamp": 0.16776002515046576 - }, - { - "x": 0.7340126708459819, - "y": 4.294775101740391, - "heading": -0.9607567098648513, - "angularVelocity": 0.6117088479280168, - "velocityX": 0.5046598798845962, - "velocityY": -0.8214420923185943, - "timestamp": 0.22368003353395435 - }, - { - "x": 0.7693006604451206, - "y": 4.237332365746024, - "heading": -0.9187315737842261, - "angularVelocity": 0.7515223494321577, - "velocityX": 0.6310440684690272, - "velocityY": -1.0272304610620715, - "timestamp": 0.2796000419174429 - }, - { - "x": 0.8116619611973516, - "y": 4.168369327121192, - "heading": -0.8692948358062461, - "angularVelocity": 0.8840617054087766, - "velocityX": 0.7575338769931041, - "velocityY": -1.2332444257142752, - "timestamp": 0.3355200503009315 - }, - { - "x": 0.8611030812908155, - "y": 4.087871426229515, - "heading": -0.8129373545696884, - "angularVelocity": 1.0078231900479868, - "velocityX": 0.8841400694078276, - "velocityY": -1.4395187557848361, - "timestamp": 0.3914400586844201 - }, - { - "x": 0.9176312901451578, - "y": 3.995821968070834, - "heading": -0.7502611156342618, - "angularVelocity": 1.120819555419328, - "velocityX": 1.0108762585778377, - "velocityY": -1.6460916373156267, - "timestamp": 0.4473600670679087 - }, - { - "x": 0.9812548770105831, - "y": 3.8922019971306105, - "heading": -0.6820246305048668, - "angularVelocity": 1.220251697057024, - "velocityX": 1.1377606818136927, - "velocityY": -1.8530034943775027, - "timestamp": 0.5032800754513973 - }, - { - "x": 1.0519833618359966, - "y": 3.7769903402374796, - "heading": -0.6092267013635415, - "angularVelocity": 1.3018225720227237, - "velocityX": 1.2648153473148889, - "velocityY": -2.0602939846330393, - "timestamp": 0.5592000838348858 - }, - { - "x": 1.1298269082953458, - "y": 3.6501643798944903, - "heading": -0.5332724156478241, - "angularVelocity": 1.3582667083101572, - "velocityX": 1.392051766614792, - "velocityY": -2.26798893650447, - "timestamp": 0.6151200922183744 - }, - { - "x": 1.2147919549002646, - "y": 3.511704401985981, - "heading": -0.45632702327380176, - "angularVelocity": 1.375990358340898, - "velocityX": 1.5194033238021896, - "velocityY": -2.4760364297333126, - "timestamp": 0.6710401006018629 - }, - { - "x": 1.3068601679390515, - "y": 3.3616165956403266, - "heading": -0.38215505844470493, - "angularVelocity": 1.3263940219829704, - "velocityX": 1.6464270249639619, - "velocityY": -2.683973244717373, - "timestamp": 0.7269601089853515 - }, - { - "x": 1.4058792704184795, - "y": 3.2000739494751453, - "heading": -0.3184323584737973, - "angularVelocity": 1.139533090444293, - "velocityX": 1.7707276043375095, - "velocityY": -2.8888165584195344, - "timestamp": 0.78288011736884 - }, - { - "x": 1.512169170978474, - "y": 3.0300749520186443, - "heading": -0.28662541962876376, - "angularVelocity": 0.5687935278354703, - "velocityX": 1.900749009747616, - "velocityY": -3.0400388406718526, - "timestamp": 0.8388001257523285 - }, - { - "x": 1.6244650109400058, - "y": 2.850864532661705, - "heading": -0.2866253125429214, - "angularVelocity": 0.0000019149825876044375, - "velocityX": 2.008151343458836, - "velocityY": -3.2047638141959607, - "timestamp": 0.8947201341358171 - }, - { - "x": 1.7352882055291936, - "y": 2.6707396888206167, - "heading": -0.2866252783553297, - "angularVelocity": 6.113659974508405e-7, - "velocityX": 1.9818164873864779, - "velocityY": -3.221116180917336, - "timestamp": 0.9506401425193056 - }, - { - "x": 1.8506362591615508, - "y": 2.493478394875359, - "heading": -0.2866252441530604, - "angularVelocity": 6.116284725625113e-7, - "velocityX": 2.0627331248114724, - "velocityY": -3.169908214777688, - "timestamp": 1.0065601509027942 - }, - { - "x": 1.9771720781105002, - "y": 2.324022128559085, - "heading": -0.2866252095630558, - "angularVelocity": 6.185622200419991e-7, - "velocityX": 2.2628004288052184, - "velocityY": -3.0303333496335663, - "timestamp": 1.0624801592862827 - }, - { - "x": 2.114370117744969, - "y": 2.1630766803808767, - "heading": -0.28662517392184755, - "angularVelocity": 6.373605669269748e-7, - "velocityX": 2.4534695827223696, - "velocityY": -2.8781370538157938, - "timestamp": 1.1184001676697712 - }, - { - "x": 2.2616596068294545, - "y": 2.011311910787294, - "heading": -0.28662513647276167, - "angularVelocity": 6.696902771759525e-7, - "velocityX": 2.6339318133574414, - "velocityY": -2.713961853381871, - "timestamp": 1.1743201760532598 - }, - { - "x": 2.418427669547749, - "y": 1.8693594092228303, - "heading": -0.2866250962912349, - "angularVelocity": 7.185536621043778e-7, - "velocityX": 2.8034341776776834, - "velocityY": -2.538492136678183, - "timestamp": 1.2302401844367483 - }, - { - "x": 2.5840219554137547, - "y": 1.7378099138847216, - "heading": -0.28662505217416917, - "angularVelocity": 7.889316725786441e-7, - "velocityX": 2.961270762521926, - "velocityY": -2.3524584337678855, - "timestamp": 1.2861601928202369 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.28662500163792387, - "angularVelocity": 9.037238500487202e-7, - "velocityX": 3.1067845266977017, - "velocityY": -2.1566350283233993, - "timestamp": 1.3420802012037254 - }, - { - "x": 2.964172022537395, - "y": 1.4952512181839386, - "heading": -0.2866249560195876, - "angularVelocity": 7.195928187708238e-7, - "velocityX": 3.256089340971223, - "velocityY": -1.923816018708688, - "timestamp": 1.4054748531095118 - }, - { - "x": 3.1789518158724, - "y": 1.3887033110695066, - "heading": -0.28662491569189824, - "angularVelocity": 6.361370890269852e-7, - "velocityX": 3.387979693526802, - "velocityY": -1.6807081340674828, - "timestamp": 1.4688695050152982 - }, - { - "x": 3.4009440542760783, - "y": 1.2981369637223683, - "heading": -0.2866248790058733, - "angularVelocity": 5.786927429417107e-7, - "velocityX": 3.5017502538477667, - "velocityY": -1.4286117933375972, - "timestamp": 1.5322641569210846 - }, - { - "x": 3.6289614625982565, - "y": 1.2240364953903844, - "heading": -0.286624844765475, - "angularVelocity": 5.401149349395126e-7, - "velocityX": 3.5967924969608642, - "velocityY": -1.1688757033023536, - "timestamp": 1.595658808826871 - }, - { - "x": 3.861784529858251, - "y": 1.1667980667810438, - "heading": -0.2866248120410044, - "angularVelocity": 5.162023864828723e-7, - "velocityX": 3.672597928386812, - "velocityY": -0.9028904945231823, - "timestamp": 1.6590534607326575 - }, - { - "x": 4.0981679647792335, - "y": 1.1267270015307351, - "heading": -0.2866247800525485, - "angularVelocity": 5.045923424053189e-7, - "velocityX": 3.728759884544863, - "velocityY": -0.6320890492444079, - "timestamp": 1.7224481126384439 - }, - { - "x": 4.3358126512762265, - "y": 1.0949807955483848, - "heading": -0.2866247482072254, - "angularVelocity": 5.023345364444787e-7, - "velocityX": 3.7486551207847545, - "velocityY": -0.5007710434238178, - "timestamp": 1.7858427645442303 - }, - { - "x": 4.573457419312962, - "y": 1.0632351999594603, - "heading": -0.28662471636190656, - "angularVelocity": 5.023344691113163e-7, - "velocityX": 3.7486564070090496, - "velocityY": -0.5007614149550478, - "timestamp": 1.8492374164500167 - }, - { - "x": 4.811102187354235, - "y": 1.0314896044045174, - "heading": -0.28662468451658774, - "angularVelocity": 5.023344695763415e-7, - "velocityX": 3.748656407080655, - "velocityY": -0.5007614144190159, - "timestamp": 1.9126320683558031 - }, - { - "x": 5.04874695539551, - "y": 0.9997440088495765, - "heading": -0.28662465267126885, - "angularVelocity": 5.023344699297441e-7, - "velocityX": 3.7486564070806594, - "velocityY": -0.5007614144189861, - "timestamp": 1.9760267202615895 - }, - { - "x": 5.286391723436784, - "y": 0.9679984132946355, - "heading": -0.28662462082595, - "angularVelocity": 5.023344691743294e-7, - "velocityX": 3.748656407080659, - "velocityY": -0.5007614144189861, - "timestamp": 2.039421372167376 - }, - { - "x": 5.524036491478059, - "y": 0.936252817739695, - "heading": -0.28662458898063115, - "angularVelocity": 5.023344697631327e-7, - "velocityX": 3.74865640708066, - "velocityY": -0.5007614144189809, - "timestamp": 2.1028160240731624 - }, - { - "x": 5.761681259520114, - "y": 0.9045072221906025, - "heading": -0.2866245571353122, - "angularVelocity": 5.023344707942377e-7, - "velocityX": 3.748656407092983, - "velocityY": -0.5007614143267316, - "timestamp": 2.166210675978949 - }, - { - "x": 5.99932604159473, - "y": 0.8727617316882137, - "heading": -0.2866245252899934, - "angularVelocity": 5.023344694954779e-7, - "velocityX": 3.748656628445413, - "velocityY": -0.500759757298884, - "timestamp": 2.229605327884735 - }, - { - "x": 6.237212534637086, - "y": 0.8428813906316718, - "heading": -0.2866244915581877, - "angularVelocity": 5.320922919551869e-7, - "velocityX": 3.752469425905036, - "velocityY": -0.47133851450037834, - "timestamp": 2.2929999797905216 - }, - { - "x": 6.4638245505635705, - "y": 0.8220107849523658, - "heading": -0.24424436180847547, - "angularVelocity": 0.6685126974542774, - "velocityX": 3.574623554416899, - "velocityY": -0.3292171350719418, - "timestamp": 2.356394631696308 - }, - { - "x": 6.673647733933235, - "y": 0.8041360764949451, - "heading": -0.18514255243378605, - "angularVelocity": 0.9322838377994912, - "velocityX": 3.309793130207447, - "velocityY": -0.2819592492436898, - "timestamp": 2.4197892836020944 - }, - { - "x": 6.866141134149738, - "y": 0.7888964331390943, - "heading": -0.1276691267098072, - "angularVelocity": 0.9065973863125333, - "velocityX": 3.0364296424022625, - "velocityY": -0.240393201913927, - "timestamp": 2.483183935507881 - }, - { - "x": 7.04134978279503, - "y": 0.7761674362974819, - "heading": -0.07636429138531178, - "angularVelocity": 0.8092927996630026, - "velocityX": 2.7637764918352534, - "velocityY": -0.2007897584252007, - "timestamp": 2.5465785874136673 - }, - { - "x": 7.19932046714385, - "y": 0.765888430525111, - "heading": -0.03338684229285246, - "angularVelocity": 0.6779349330023924, - "velocityX": 2.4918613731579056, - "velocityY": -0.16214310613530794, - "timestamp": 2.6099732393194537 - }, - { - "x": 7.340087890625, - "y": 0.7580231428146362, - "heading": 0, - "angularVelocity": 0.5266507708326894, - "velocityX": 2.220493673351971, - "velocityY": -0.12406863156475356, - "timestamp": 2.67336789122524 - }, - { - "x": 7.464023522460178, - "y": 0.7525372143128384, - "heading": 0.02301011936038792, - "angularVelocity": 0.3617896361662683, - "velocityX": 1.9486481772395665, - "velocityY": -0.08625561847872572, - "timestamp": 2.736968716290925 - }, - { - "x": 7.570582319535539, - "y": 0.7491648354718851, - "heading": 0.036983700221316605, - "angularVelocity": 0.21970754068830917, - "velocityX": 1.675431049287655, - "velocityY": -0.053024136675436706, - "timestamp": 2.8005695413566096 - }, - { - "x": 7.659708944521439, - "y": 0.7476928551810322, - "heading": 0.04295335113137197, - "angularVelocity": 0.09386121805637201, - "velocityX": 1.4013438488235506, - "velocityY": -0.02314404395434427, - "timestamp": 2.8641703664222944 - }, - { - "x": 7.731365798341117, - "y": 0.7479588271083956, - "heading": 0.041693436231508264, - "angularVelocity": -0.019809725716020715, - "velocityX": 1.1266654755763426, - "velocityY": 0.0041818942928592215, - "timestamp": 2.927771191487979 - }, - { - "x": 7.785525986892142, - "y": 0.7498349370538775, - "heading": 0.03380693253041853, - "angularVelocity": -0.12400002190136777, - "velocityX": 0.8515642445690119, - "velocityY": 0.029498201376229192, - "timestamp": 2.991372016553664 - }, - { - "x": 7.822169492500932, - "y": 0.7532180353995639, - "heading": 0.0197770843540876, - "angularVelocity": -0.220592235428419, - "velocityX": 0.5761482743493489, - "velocityY": 0.053192680160869574, - "timestamp": 3.0549728416193487 - }, - { - "x": 7.841280937194824, - "y": 0.7580231428146362, - "heading": -3.3329541710174495e-31, - "angularVelocity": -0.3109564118651419, - "velocityX": 0.3004905152433859, - "velocityY": 0.07555102327854826, - "timestamp": 3.1185736666850334 - }, - { - "x": 7.837626456422163, - "y": 0.7661832239405314, - "heading": -0.033360531548023514, - "angularVelocity": -0.4178611188210231, - "velocityX": -0.04577461309858743, - "velocityY": 0.10221002096529454, - "timestamp": 3.1984100765480727 - }, - { - "x": 7.806322741144082, - "y": 0.7764707746744536, - "heading": -0.07501113915617397, - "angularVelocity": -0.521699406068016, - "velocityX": -0.3920982335225694, - "velocityY": 0.12885788265743237, - "timestamp": 3.278246486411112 - }, - { - "x": 7.74736425619444, - "y": 0.7888847549403223, - "heading": -0.12465356813987527, - "angularVelocity": -0.6218018704606526, - "velocityX": -0.7384911852973625, - "velocityY": 0.15549271675874526, - "timestamp": 3.3580828962741514 - }, - { - "x": 7.660744366778727, - "y": 0.8034239689194147, - "heading": -0.18191677738707465, - "angularVelocity": -0.7172568173523234, - "velocityX": -1.0849672419427567, - "velocityY": 0.182112572497119, - "timestamp": 3.4379193061371907 - }, - { - "x": 7.5464550145644145, - "y": 0.8200870575241466, - "heading": -0.24632578527266566, - "angularVelocity": -0.8067623280666746, - "velocityX": -1.4315442341455173, - "velocityY": 0.20871540482992249, - "timestamp": 3.51775571600023 - }, - { - "x": 7.404486266909035, - "y": 0.8388724767291539, - "heading": -0.3172478637527479, - "angularVelocity": -0.8883425319569104, - "velocityX": -1.7782456387872363, - "velocityY": 0.23529889729803757, - "timestamp": 3.5975921258632693 - }, - { - "x": 7.234825726560123, - "y": 0.8597784078771568, - "heading": -0.39379130752240776, - "angularVelocity": -0.958753579989021, - "velocityX": -2.125102326619727, - "velocityY": 0.26185960996827495, - "timestamp": 3.6774285357263086 - }, - { - "x": 7.037458048630302, - "y": 0.8828023770067689, - "heading": -0.47459284644074845, - "angularVelocity": -1.0120888333650886, - "velocityX": -2.4721512185781784, - "velocityY": 0.28838933475478157, - "timestamp": 3.757264945589348 - }, - { - "x": 6.812366694532574, - "y": 0.9079395818644891, - "heading": -0.5572950195818098, - "angularVelocity": -1.0358954427301785, - "velocityX": -2.8194072664825045, - "velocityY": 0.31485890837079844, - "timestamp": 3.837101355452387 - }, - { - "x": 6.559557151686244, - "y": 0.9351738572850867, - "heading": -0.6368741954392069, - "angularVelocity": -0.9967779863087077, - "velocityX": -3.1665945810943685, - "velocityY": 0.3411260033776371, - "timestamp": 3.9169377653154265 - }, - { - "x": 6.2794552371906684, - "y": 0.9644007497338526, - "heading": -0.6963848347023109, - "angularVelocity": -0.7454072567290455, - "velocityX": -3.5084482753682624, - "velocityY": 0.36608475379723326, - "timestamp": 3.996774175178466 - }, - { - "x": 5.978414020607044, - "y": 0.9876514129078965, - "heading": -0.6963848502753706, - "angularVelocity": -1.9506212382871808e-7, - "velocityX": -3.7707258768281893, - "velocityY": 0.2912288167006853, - "timestamp": 4.0766105850415055 - }, - { - "x": 5.677372687056552, - "y": 1.0109005615970947, - "heading": -0.6963848658464015, - "angularVelocity": -1.950367123738679e-7, - "velocityX": -3.770727341909936, - "velocityY": 0.29120984684910617, - "timestamp": 4.156446994904545 - }, - { - "x": 5.376331353502739, - "y": 1.0341497102432655, - "heading": -0.6963848814174324, - "angularVelocity": -1.950367127895845e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.29120984631016156, - "timestamp": 4.236283404767585 - }, - { - "x": 5.075290019948924, - "y": 1.0573988588894352, - "heading": -0.6963848969884634, - "angularVelocity": -1.950367129001825e-7, - "velocityX": -3.770727341951559, - "velocityY": 0.2912098463101467, - "timestamp": 4.316119814630625 - }, - { - "x": 4.774248686395111, - "y": 1.0806480075356062, - "heading": -0.6963849125594943, - "angularVelocity": -1.9503671289084429e-7, - "velocityX": -3.7707273419515577, - "velocityY": 0.2912098463101629, - "timestamp": 4.3959562244936645 - }, - { - "x": 4.473207352844907, - "y": 1.1038971562285251, - "heading": -0.6963849281305252, - "angularVelocity": -1.950367124712723e-7, - "velocityX": -3.770727341906337, - "velocityY": 0.2912098468957114, - "timestamp": 4.475792634356704 - }, - { - "x": 4.172166146375442, - "y": 1.1271479503662634, - "heading": -0.6963849437015552, - "angularVelocity": -1.9503670107200537e-7, - "velocityX": -3.770725750142127, - "velocityY": 0.29123045710128126, - "timestamp": 4.555629044219744 - }, - { - "x": 3.8735860999949274, - "y": 1.1720518831992321, - "heading": -0.6963849593703558, - "angularVelocity": -1.9626133737167217e-7, - "velocityX": -3.7398982105123864, - "velocityY": 0.5624492998871315, - "timestamp": 4.635465454082784 - }, - { - "x": 3.580403442801602, - "y": 1.244234542261993, - "heading": -0.6963849756495777, - "angularVelocity": -2.0390723857242194e-7, - "velocityX": -3.672292600535076, - "velocityY": 0.9041320769131688, - "timestamp": 4.7153018639458235 - }, - { - "x": 3.295105029858622, - "y": 1.3430840304472003, - "heading": -0.6963849932202171, - "angularVelocity": -2.20083034395915e-7, - "velocityX": -3.5735376056164516, - "velocityY": 1.238150467371779, - "timestamp": 4.795138273808863 - }, - { - "x": 3.0201108216375614, - "y": 1.4677619739384065, - "heading": -0.6963850130039021, - "angularVelocity": -2.4780278968053093e-7, - "velocityX": -3.444471121544888, - "velocityY": 1.5616677115753714, - "timestamp": 4.874974683671903 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.696385035506177, - "angularVelocity": -2.8185479518318006e-7, - "velocityX": -3.286187967310359, - "velocityY": 1.8719390230438346, - "timestamp": 4.954811093534943 - }, - { - "x": 2.5741841985169076, - "y": 1.7387873379942203, - "heading": -0.6963850589279509, - "angularVelocity": -4.023107769663099e-7, - "velocityX": -3.153128257888759, - "velocityY": 2.0882929565548642, - "timestamp": 5.0130292054104135 - }, - { - "x": 2.3991894720533877, - "y": 1.872411165939459, - "heading": -0.6963850794155533, - "angularVelocity": -3.519111438487621e-7, - "velocityX": -3.0058468202788267, - "velocityY": 2.2952277846293376, - "timestamp": 5.071247317285884 - }, - { - "x": 2.2335584908852413, - "y": 2.017479635272907, - "heading": -0.6963850978369923, - "angularVelocity": -3.1642110134470593e-7, - "velocityX": -2.845007779064253, - "velocityY": 2.4918099309670456, - "timestamp": 5.129465429161355 - }, - { - "x": 2.0780383093475274, - "y": 2.173338406648706, - "heading": -0.696385114808838, - "angularVelocity": -2.9152174806319146e-7, - "velocityX": -2.671336746034877, - "velocityY": 2.6771526309404154, - "timestamp": 5.187683541036826 - }, - { - "x": 1.9333303562340614, - "y": 2.339284459115452, - "heading": -0.6963851307944144, - "angularVelocity": -2.7458081036886754e-7, - "velocityX": -2.485617421310377, - "velocityY": 2.8504196910698067, - "timestamp": 5.245901652912297 - }, - { - "x": 1.8000872249814166, - "y": 2.5145692341351995, - "heading": -0.6963851461633941, - "angularVelocity": -2.6398966215373687e-7, - "velocityX": -2.2886886393302137, - "velocityY": 3.0108289220145825, - "timestamp": 5.304119764787767 - }, - { - "x": 1.6789094155739503, - "y": 2.698401829383423, - "heading": -0.6963851612308563, - "angularVelocity": -2.588105610313673e-7, - "velocityX": -2.0814451981312536, - "velocityY": 3.157652993649885, - "timestamp": 5.362337876663238 - }, - { - "x": 1.5665736982883989, - "y": 2.8877670089616614, - "heading": -0.6963851762457391, - "angularVelocity": -2.5790741716981964e-7, - "velocityX": -1.929566481404267, - "velocityY": 3.2526850060560912, - "timestamp": 5.420555988538709 - }, - { - "x": 1.4540456075762054, - "y": 3.0767463777174253, - "heading": -0.6971250426595832, - "angularVelocity": -0.012708526436353156, - "velocityX": -1.9328708384238364, - "velocityY": 3.24605801644674, - "timestamp": 5.47877410041418 - }, - { - "x": 1.3486317039814464, - "y": 3.254480208772174, - "heading": -0.7253513181524345, - "angularVelocity": -0.48483666995638336, - "velocityX": -1.8106719747325544, - "velocityY": 3.0528958313681422, - "timestamp": 5.53699221228965 - }, - { - "x": 1.2507109072486164, - "y": 3.4195717896549707, - "heading": -0.7601787682651481, - "angularVelocity": -0.5982236281934066, - "velocityX": -1.6819644879978934, - "velocityY": 2.835742616248544, - "timestamp": 5.595210324165121 - }, - { - "x": 1.160328320301069, - "y": 3.5719513486090504, - "heading": -0.7969704460064527, - "angularVelocity": -0.631962744171482, - "velocityX": -1.5524822780387872, - "velocityY": 2.6173909466528555, - "timestamp": 5.653428436040592 - }, - { - "x": 1.0774888591394964, - "y": 3.71161238889488, - "heading": -0.8336344675120989, - "angularVelocity": -0.6297700204374729, - "velocityX": -1.422915626991949, - "velocityY": 2.3989276839579965, - "timestamp": 5.711646547916063 - }, - { - "x": 1.0021908033919844, - "y": 3.838558158350536, - "heading": -0.8689739346571358, - "angularVelocity": -0.6070184347549548, - "velocityX": -1.2933785264038764, - "velocityY": 2.1805202086799977, - "timestamp": 5.7698646597915335 - }, - { - "x": 0.9344313462846832, - "y": 3.9527935365952946, - "heading": -0.9022118432658016, - "angularVelocity": -0.5709204152783613, - "velocityX": -1.1638896371672067, - "velocityY": 1.962196549573947, - "timestamp": 5.828082771667004 - }, - { - "x": 0.8742077702690397, - "y": 4.054323289061519, - "heading": -0.9328020557936417, - "angularVelocity": -0.5254415085338586, - "velocityX": -1.034447426678191, - "velocityY": 1.7439547452758104, - "timestamp": 5.886300883542475 - }, - { - "x": 0.8215176865940929, - "y": 4.143151679881797, - "heading": -0.9603390876580659, - "angularVelocity": -0.4729976802292441, - "velocityX": -0.9050462472512312, - "velocityY": 1.5257861850670007, - "timestamp": 5.944518995417946 - }, - { - "x": 0.7763590444591775, - "y": 4.21928243186123, - "heading": -0.984509600370908, - "angularVelocity": -0.41517170403161197, - "velocityX": -0.7756802939866982, - "velocityY": 1.3076815706816212, - "timestamp": 6.002737107293417 - }, - { - "x": 0.7387300854642734, - "y": 4.282718776761074, - "heading": -1.0050639989905081, - "angularVelocity": -0.3530584891445192, - "velocityX": -0.6463445443815322, - "velocityY": 1.0896324675649907, - "timestamp": 6.060955219168887 - }, - { - "x": 0.7086292911290721, - "y": 4.333463522832413, - "heading": -1.0217986916878445, - "angularVelocity": -0.28744822115035373, - "velocityX": -0.5170348773863939, - "velocityY": 0.8716316011739073, - "timestamp": 6.119173331044358 - }, - { - "x": 0.6860553360496396, - "y": 4.371519118466469, - "heading": -1.0345444282293665, - "angularVelocity": -0.21893077825652074, - "velocityX": -0.3877479765698806, - "velocityY": 0.6536727902728675, - "timestamp": 6.177391442919829 - }, - { - "x": 0.6710070489868055, - "y": 4.396887706764773, - "heading": -1.0431583071392123, - "angularVelocity": -0.14795874741302203, - "velocityX": -0.25848119387696106, - "velocityY": 0.4357507909663603, - "timestamp": 6.2356095547953 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -0.07488723824993983, - "velocityX": -0.12923242394972312, - "velocityY": 0.2178611369128849, - "timestamp": 6.29382766667077 - }, - { - "x": 0.6634833812713623, - "y": 4.409571170806885, - "heading": -1.0475181007536924, - "angularVelocity": -4.5881817726943837e-32, - "velocityX": 7.1951222626888875e-34, - "velocityY": 0, - "timestamp": 6.352045778546241 - }, - { - "x": 0.6704852067504524, - "y": 4.397985362145517, - "heading": -1.0417684453451177, - "angularVelocity": 0.10293302663784343, - "velocityX": 0.12534996227388928, - "velocityY": -0.20741457823419143, - "timestamp": 6.40790399622796 - }, - { - "x": 0.684490344241533, - "y": 4.374811471822251, - "heading": -1.0303744644017594, - "angularVelocity": 0.20398038849505432, - "velocityX": 0.25072653715666804, - "velocityY": -0.4148698487895252, - "timestamp": 6.463762213909678 - }, - { - "x": 0.7055004573054668, - "y": 4.340046908929331, - "heading": -1.0134578875635165, - "angularVelocity": 0.3028484892703545, - "velocityX": 0.3761328938859112, - "velocityY": -0.6223715029901059, - "timestamp": 6.519620431591397 - }, - { - "x": 0.7335174330880717, - "y": 4.293688705050122, - "heading": -0.9911609198549822, - "angularVelocity": 0.3991707690278106, - "velocityX": 0.5015730351843025, - "velocityY": -0.8299262991769599, - "timestamp": 6.575478649273116 - }, - { - "x": 0.768543442750825, - "y": 4.235733440885856, - "heading": -0.963651742055662, - "angularVelocity": 0.49248219762521406, - "velocityX": 0.6270520456333297, - "velocityY": -1.0375423092533531, - "timestamp": 6.631336866954834 - }, - { - "x": 0.8105810218384888, - "y": 4.16617715590748, - "heading": -0.9311321668224927, - "angularVelocity": 0.5821806814257259, - "velocityX": 0.7525764485933851, - "velocityY": -1.24522922257758, - "timestamp": 6.687195084636553 - }, - { - "x": 0.8596331801158593, - "y": 4.085015237354172, - "heading": -0.8938487217946981, - "angularVelocity": 0.667465711853477, - "velocityX": 0.8781547337738402, - "velocityY": -1.4529987157085942, - "timestamp": 6.7430533023182715 - }, - { - "x": 0.9157035549190764, - "y": 3.992242285598939, - "heading": -0.8521094876337724, - "angularVelocity": 0.7472353378469229, - "velocityX": 1.0037981362510981, - "velocityY": -1.6608648754934532, - "timestamp": 6.79891151999999 - }, - { - "x": 0.9787966256099634, - "y": 3.88785195918136, - "heading": -0.8063112110880574, - "angularVelocity": 0.8199022175514963, - "velocityX": 1.1295217303637053, - "velocityY": -1.868844563075711, - "timestamp": 6.854769737681709 - }, - { - "x": 1.0489179988671087, - "y": 3.7718368324625953, - "heading": -0.756986124262755, - "angularVelocity": 0.8830408285913816, - "velocityX": 1.2553456978648867, - "velocityY": -2.0769571879257267, - "timestamp": 6.910627955363427 - }, - { - "x": 1.1260747058581024, - "y": 3.644188427808813, - "heading": -0.7048901245652565, - "angularVelocity": 0.9326470098695786, - "velocityX": 1.381295540624565, - "velocityY": -2.2852215833510225, - "timestamp": 6.966486173045146 - }, - { - "x": 1.210275065586603, - "y": 3.504898185530833, - "heading": -0.6511892097412871, - "angularVelocity": 0.9613789528688276, - "velocityX": 1.5073943140877994, - "velocityY": -2.4936392183449136, - "timestamp": 7.022344390726865 - }, - { - "x": 1.3015253174439423, - "y": 3.3539635614800978, - "heading": -0.5979268038301901, - "angularVelocity": 0.95352856073188, - "velocityX": 1.6336047880597582, - "velocityY": -2.7021023998790135, - "timestamp": 7.078202608408583 - }, - { - "x": 1.3998011054306767, - "y": 3.1914330516367446, - "heading": -0.5495841694257507, - "angularVelocity": 0.865452504766571, - "velocityX": 1.7593792295112722, - "velocityY": -2.909697383641142, - "timestamp": 7.134060826090302 - }, - { - "x": 1.504860441802437, - "y": 3.018388823031747, - "heading": -0.5232691895419826, - "angularVelocity": 0.4711031066854229, - "velocityX": 1.8808214929160718, - "velocityY": -3.0979189058807544, - "timestamp": 7.18991904377202 - }, - { - "x": 1.6140923684634172, - "y": 2.8375675393930413, - "heading": -0.5232691688026271, - "angularVelocity": 3.712856650457212e-7, - "velocityX": 1.9555211604385054, - "velocityY": -3.237147391079152, - "timestamp": 7.245777261453739 - }, - { - "x": 1.7233256383679056, - "y": 2.6567470671976134, - "heading": -0.5232691480644303, - "angularVelocity": 3.7126492252401944e-7, - "velocityX": 1.9555452078135154, - "velocityY": -3.2371328642411785, - "timestamp": 7.301635479135458 - }, - { - "x": 1.84078732146222, - "y": 2.4811603268646896, - "heading": -0.5232691272677354, - "angularVelocity": 3.72312181556421e-7, - "velocityX": 2.102854118325333, - "velocityY": -3.143436142796787, - "timestamp": 7.357493696817176 - }, - { - "x": 1.9693130123787856, - "y": 2.313502648911294, - "heading": -0.5232691060970931, - "angularVelocity": 3.790067651022537e-7, - "velocityX": 2.3009271733106282, - "velocityY": -3.0014863508304788, - "timestamp": 7.413351914498895 - }, - { - "x": 2.1083695489461487, - "y": 2.154470483882732, - "heading": -0.5232690841533676, - "angularVelocity": 3.928468612451328e-7, - "velocityX": 2.4894553091491622, - "velocityY": -2.847068374697022, - "timestamp": 7.4692101321806135 - }, - { - "x": 2.2573796500994945, - "y": 2.0047242347352134, - "heading": -0.5232690609733034, - "angularVelocity": 4.1498037571099624e-7, - "velocityX": 2.6676486887284834, - "velocityY": -2.6808275552359513, - "timestamp": 7.525068349862332 - }, - { - "x": 2.4157246359676448, - "y": 1.8648857059384452, - "heading": -0.5232690359846437, - "angularVelocity": 4.47358702857604e-7, - "velocityX": 2.8347661712087695, - "velocityY": -2.5034549006481424, - "timestamp": 7.580926567544051 - }, - { - "x": 2.582747044375024, - "y": 1.7355355500002163, - "heading": -0.523269008439205, - "angularVelocity": 4.931313565110266e-7, - "velocityX": 2.9901134575950428, - "velocityY": -2.3156871326484083, - "timestamp": 7.636784785225769 - }, - { - "x": 2.757753372192383, - "y": 1.617210865020752, - "heading": -0.5232689753097743, - "angularVelocity": 5.930985989560848e-7, - "velocityX": 3.133045325838173, - "velocityY": -2.1183039826598433, - "timestamp": 7.692643002907488 - }, - { - "x": 3.027691141146945, - "y": 1.4695236072310565, - "heading": -0.5232689479285146, - "angularVelocity": 3.365468040910692e-7, - "velocityX": 3.3178420048715034, - "velocityY": -1.8152442667680868, - "timestamp": 7.7740024433384125 - }, - { - "x": 3.3102859882061892, - "y": 1.3477940719025636, - "heading": -0.523268924510295, - "angularVelocity": 2.8783653775450794e-7, - "velocityX": 3.473411881429686, - "velocityY": -1.4961943529078627, - "timestamp": 7.855361883769337 - }, - { - "x": 3.6030485594101127, - "y": 1.2530945172715204, - "heading": -0.5232689034957836, - "angularVelocity": 2.582922326531371e-7, - "velocityX": 3.5983847682984376, - "velocityY": -1.1639651665432065, - "timestamp": 7.936721324200262 - }, - { - "x": 3.9033999272897986, - "y": 1.1862590073495238, - "heading": -0.5232688838271395, - "angularVelocity": 2.4174999152857024e-7, - "velocityX": 3.6916597052396023, - "velocityY": -0.8214843854382324, - "timestamp": 8.018080764631186 - }, - { - "x": 4.2086942600952995, - "y": 1.1478754132776305, - "heading": -0.5232688646920325, - "angularVelocity": 2.3519221562308788e-7, - "velocityX": 3.752414362592638, - "velocityY": -0.471777999806691, - "timestamp": 8.09944020506211 - }, - { - "x": 4.515097551396115, - "y": 1.1196805012022473, - "heading": -0.5232688456111896, - "angularVelocity": 2.345252478429259e-7, - "velocityX": 3.7660447229963117, - "velocityY": -0.3465475171172218, - "timestamp": 8.180799645493035 - }, - { - "x": 4.821500870663513, - "y": 1.0914858930504614, - "heading": -0.5232688265303463, - "angularVelocity": 2.3452525108540152e-7, - "velocityX": 3.7660450667373904, - "velocityY": -0.3465437815507533, - "timestamp": 8.26215908592396 - }, - { - "x": 5.127904189931636, - "y": 1.0632912849065406, - "heading": -0.5232688074495031, - "angularVelocity": 2.3452525107290264e-7, - "velocityX": 3.7660450667462855, - "velocityY": -0.34654378145408427, - "timestamp": 8.343518526354885 - }, - { - "x": 5.4343075091997575, - "y": 1.0350966767626197, - "heading": -0.5232687883686599, - "angularVelocity": 2.3452525075472094e-7, - "velocityX": 3.7660450667462855, - "velocityY": -0.3465437814540822, - "timestamp": 8.42487796678581 - }, - { - "x": 5.740710828467879, - "y": 1.0069020686186991, - "heading": -0.5232687692878167, - "angularVelocity": 2.3452525040262446e-7, - "velocityX": 3.7660450667462855, - "velocityY": -0.3465437814540818, - "timestamp": 8.506237407216734 - }, - { - "x": 6.047114147736001, - "y": 0.9787074604747784, - "heading": -0.5232687502069736, - "angularVelocity": 2.345252500146494e-7, - "velocityX": 3.766045066746286, - "velocityY": -0.34654378145408327, - "timestamp": 8.587596847647658 - }, - { - "x": 6.353517467003837, - "y": 0.9505128523277552, - "heading": -0.5232687311261306, - "angularVelocity": 2.3452524994005725e-7, - "velocityX": 3.7660450667427767, - "velocityY": -0.3465437814922157, - "timestamp": 8.668956288078583 - }, - { - "x": 6.659920775239498, - "y": 0.9223181242925758, - "heading": -0.5232687120445318, - "angularVelocity": 2.3453453739742123e-7, - "velocityX": 3.7660449311448136, - "velocityY": -0.3465452550539253, - "timestamp": 8.750315728509507 - }, - { - "x": 6.946350295372631, - "y": 0.8955257235290481, - "heading": -0.4609901004295649, - "angularVelocity": 0.7654749256522044, - "velocityX": 3.5205443721840264, - "velocityY": -0.3293090589318255, - "timestamp": 8.831675168940432 - }, - { - "x": 7.204305307998309, - "y": 0.8715129005451068, - "heading": -0.38283013166037505, - "angularVelocity": 0.9606748566019065, - "velocityX": 3.170560309404857, - "velocityY": -0.2951448886171808, - "timestamp": 8.913034609371357 - }, - { - "x": 7.433510095635826, - "y": 0.8502195253793108, - "heading": -0.30543025852913497, - "angularVelocity": 0.9513324172497697, - "velocityX": 2.8171873653938824, - "velocityY": -0.2617197838752883, - "timestamp": 8.994394049802281 - }, - { - "x": 7.633994056446923, - "y": 0.8316157363547472, - "heading": -0.2335287634090791, - "angularVelocity": 0.8837511017679792, - "velocityX": 2.4641757582060952, - "velocityY": -0.22866171308489489, - "timestamp": 9.075753490233206 - }, - { - "x": 7.805788503499911, - "y": 0.8156861724274476, - "heading": -0.16939985967365537, - "angularVelocity": 0.7882171189447914, - "velocityX": 2.1115490242198023, - "velocityY": -0.19579244698498122, - "timestamp": 9.15711293066413 - }, - { - "x": 7.948917170139319, - "y": 0.802421883536533, - "heading": -0.1143881084380515, - "angularVelocity": 0.6761569517222741, - "velocityX": 1.759214000014262, - "velocityY": -0.16303318730634864, - "timestamp": 9.238472371095055 - }, - { - "x": 8.063397872753223, - "y": 0.7918170647898031, - "heading": -0.0693839808587397, - "angularVelocity": 0.5531518818338115, - "velocityX": 1.407097959469132, - "velocityY": -0.13034527635097967, - "timestamp": 9.31983181152598 - }, - { - "x": 8.149244274991434, - "y": 0.7838676118817843, - "heading": -0.0350206058237791, - "angularVelocity": 0.4223649382659615, - "velocityX": 1.0551498607109575, - "velocityY": -0.09770781197503392, - "timestamp": 9.401191251956904 - }, - { - "x": 8.20646710981356, - "y": 0.7785704017832406, - "heading": -0.011770656016753542, - "angularVelocity": 0.2857683101540642, - "velocityX": 0.703333682225969, - "velocityY": -0.06510873317818848, - "timestamp": 9.482550692387829 - }, - { - "x": 8.235074996948242, - "y": 0.7759228944778442, - "heading": -3.5413188929792264e-38, - "angularVelocity": 0.1446747415470122, - "velocityX": 0.35162345000356726, - "velocityY": -0.03254087406911382, - "timestamp": 9.563910132818753 - }, - { - "x": 8.235074996948242, - "y": 0.7759228944778442, - "heading": -7.544724392466369e-38, - "angularVelocity": -4.920640404901405e-37, - "velocityX": -6.041520920103385e-38, - "velocityY": 3.1783876166730987e-37, - "timestamp": 9.645269573249678 - } - ] + "samples": [ + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -3.050132919883851e-26, + "velocityX": -1.6533006332521264e-25, + "velocityY": 1.9877794824032622e-25, + "timestamp": 0 + }, + { + "x": 0.6705320996373239, + "y": 4.398099342072708, + "heading": -1.0386114123729497, + "angularVelocity": 0.15927551883794178, + "velocityX": 0.12605002295897502, + "velocityY": -0.20514712040522456, + "timestamp": 0.0559200085846521 + }, + { + "x": 0.6846335257451812, + "y": 4.375148415264062, + "heading": -1.0210111037335954, + "angularVelocity": 0.3147408071783644, + "velocityX": 0.25217138667835703, + "velocityY": -0.41042423614623674, + "timestamp": 0.1118400171693042 + }, + { + "x": 0.7057920858570321, + "y": 4.340710150154942, + "heading": -0.9949634713965575, + "angularVelocity": 0.4658016512570238, + "velocityX": 0.37837190385657676, + "velocityY": -0.6158487092681056, + "timestamp": 0.1677600257539563 + }, + { + "x": 0.7340126704011031, + "y": 4.294775101282737, + "heading": -0.9607567060719395, + "angularVelocity": 0.6117088711250027, + "velocityX": 0.5046598750311442, + "velocityY": -0.8214420926396665, + "timestamp": 0.2236800343386084 + }, + { + "x": 0.7693006597994352, + "y": 4.237332365059552, + "heading": -0.9187315683544864, + "angularVelocity": 0.7515223759995503, + "velocityX": 0.6310440626079833, + "velocityY": -1.0272304614586687, + "timestamp": 0.27960004292326046 + }, + { + "x": 0.8116619603260323, + "y": 4.168369326160847, + "heading": -0.8692948285945427, + "angularVelocity": 0.8840617340948025, + "velocityX": 0.7575338702330544, + "velocityY": -1.2332444261754465, + "timestamp": 0.33552005150791253 + }, + { + "x": 0.8611030801762585, + "y": 4.087871424951387, + "heading": -0.8129373455124387, + "angularVelocity": 1.0078232194258272, + "velocityX": 0.8841400618775261, + "velocityY": -1.4395187562892047, + "timestamp": 0.3914400600925646 + }, + { + "x": 0.9176312887785456, + "y": 3.99582196643312, + "heading": -0.7502611047615361, + "angularVelocity": 1.1208195838529407, + "velocityX": 1.0108762504339452, + "velocityY": -1.6460916378244417, + "timestamp": 0.44736006867721667 + }, + { + "x": 0.9812548753941158, + "y": 3.892201995095202, + "heading": -0.6820246179562691, + "angularVelocity": 1.2202517226364538, + "velocityX": 1.1377606732526935, + "velocityY": -1.85300349482346, + "timestamp": 0.5032800772618687 + }, + { + "x": 1.0519833599861292, + "y": 3.7769903377726197, + "heading": -0.6092266874108005, + "angularVelocity": 1.301822592449473, + "velocityX": 1.2648153385910914, + "velocityY": -2.060293984901199, + "timestamp": 0.5592000858465208 + }, + { + "x": 1.129826906247576, + "y": 3.6501643769794856, + "heading": -0.5332724007265343, + "angularVelocity": 1.3582667207442602, + "velocityX": 1.3920517580680767, + "velocityY": -2.267988936395541, + "timestamp": 0.6151200944311729 + }, + { + "x": 1.214791952716682, + "y": 3.5117043986204477, + "heading": -0.45632700801637377, + "angularVelocity": 1.3759903594020386, + "velocityX": 1.51940331590767, + "velocityY": -2.4760364288828027, + "timestamp": 0.671040103015825 + }, + { + "x": 1.3068601657234094, + "y": 3.3616165918639114, + "heading": -0.3821550436425142, + "angularVelocity": 1.3263940090706097, + "velocityX": 1.6464270184678926, + "velocityY": -2.68397324240987, + "timestamp": 0.726960111600477 + }, + { + "x": 1.40587926837359, + "y": 3.2000739453958693, + "heading": -0.31843234449405566, + "angularVelocity": 1.1395330716374026, + "velocityX": 1.7707276010210988, + "velocityY": -2.8888165534434402, + "timestamp": 0.7828801201851291 + }, + { + "x": 1.5121691695533515, + "y": 3.030074948039426, + "heading": -0.28662540722201535, + "angularVelocity": 0.5687934976599797, + "velocityX": 1.9007490139930723, + "velocityY": -3.0400388279464865, + "timestamp": 0.8388001287697812 + }, + { + "x": 1.624464989491448, + "y": 2.8508645164858524, + "heading": -0.2866253001360378, + "angularVelocity": 0.000001914984998408506, + "velocityX": 2.0081509781620026, + "velocityY": -3.2047640207759205, + "timestamp": 0.8947201373544332 + }, + { + "x": 1.7352882065393072, + "y": 2.6707396868109323, + "heading": -0.2866252659484468, + "angularVelocity": 6.113659826501039e-7, + "velocityX": 1.9818168818785864, + "velocityY": -3.2211159160007226, + "timestamp": 0.9506401459390853 + }, + { + "x": 1.850636259974605, + "y": 2.493478393091365, + "heading": -0.28662523174617754, + "angularVelocity": 6.116284689254095e-7, + "velocityX": 2.0627331138671594, + "velocityY": -3.1699081993384874, + "timestamp": 1.0065601545237375 + }, + { + "x": 1.977172078744531, + "y": 2.3240221270116344, + "heading": -0.28662519715617285, + "angularVelocity": 6.185622201662938e-7, + "velocityX": 2.262800417463728, + "velocityY": -3.030333334502384, + "timestamp": 1.0624801631083896 + }, + { + "x": 2.1143701182221513, + "y": 2.163076679089521, + "heading": -0.2866251615149645, + "angularVelocity": 6.373605670854621e-7, + "velocityX": 2.4534695710915173, + "velocityY": -2.8781370388824827, + "timestamp": 1.1184001716930416 + }, + { + "x": 2.2616596071687876, + "y": 2.0113119097755345, + "heading": -0.2866251240658783, + "angularVelocity": 6.696902793737695e-7, + "velocityX": 2.633931801417164, + "velocityY": -2.713961838618882, + "timestamp": 1.1743201802776937 + }, + { + "x": 2.4184276697644367, + "y": 1.8693594085175804, + "heading": -0.28662508388435115, + "angularVelocity": 7.185536668369167e-7, + "velocityX": 2.8034341653995387, + "velocityY": -2.5384921220651377, + "timestamp": 1.2302401888623458 + }, + { + "x": 2.5840219555188284, + "y": 1.7378099135158778, + "heading": -0.28662503976728476, + "angularVelocity": 7.889316805282802e-7, + "velocityX": 2.9612707498732536, + "velocityY": -2.352458419289443, + "timestamp": 1.2861601974469978 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.2866249892310386, + "angularVelocity": 9.037238643571085e-7, + "velocityX": 3.106784513642535, + "velocityY": -2.156635013969323, + "timestamp": 1.34208020603165 + }, + { + "x": 2.964172022531705, + "y": 1.4952512185912425, + "heading": -0.28662494361270324, + "angularVelocity": 7.19592801055205e-7, + "velocityX": 3.2560893274813307, + "velocityY": -1.9238160043665056, + "timestamp": 1.4054748581983312 + }, + { + "x": 3.178951815863355, + "y": 1.3887033119472907, + "heading": -0.28662490328501483, + "angularVelocity": 6.36137071526776e-7, + "velocityX": 3.3879796795309685, + "velocityY": -1.680708119729232, + "timestamp": 1.4688695103650125 + }, + { + "x": 3.4009440542565765, + "y": 1.2981369651359775, + "heading": -0.28662486659899084, + "angularVelocity": 5.786927242976968e-7, + "velocityX": 3.501750239271679, + "velocityY": -1.4286117790060657, + "timestamp": 1.5322641625316937 + }, + { + "x": 3.628961462551473, + "y": 1.224036497406262, + "heading": -0.28662483235859354, + "angularVelocity": 5.40114917287456e-7, + "velocityX": 3.596792481728244, + "velocityY": -1.168875688991645, + "timestamp": 1.595658814698375 + }, + { + "x": 3.861784529757441, + "y": 1.166798069465538, + "heading": -0.2866247996341239, + "angularVelocity": 5.162023688737337e-7, + "velocityX": 3.672597912420351, + "velocityY": -0.9028904802605274, + "timestamp": 1.6590534668650563 + }, + { + "x": 4.098167964587255, + "y": 1.1267270049463884, + "heading": -0.28662476764566913, + "angularVelocity": 5.045923229108656e-7, + "velocityX": 3.7287598677613794, + "velocityY": -0.6320890351096494, + "timestamp": 1.7224481190317376 + }, + { + "x": 4.335812648113977, + "y": 1.0949807783310244, + "heading": -0.2866247358003473, + "angularVelocity": 5.023345150165169e-7, + "velocityX": 3.748655058503865, + "velocityY": -0.500771366832249, + "timestamp": 1.7858427711984188 + }, + { + "x": 4.573457416127459, + "y": 1.0632351841697858, + "heading": -0.2866247039550299, + "angularVelocity": 5.023344440869941e-7, + "velocityX": 3.7486563912149893, + "velocityY": -0.5007613903736075, + "timestamp": 1.8492374233651 + }, + { + "x": 4.8111021841458665, + "y": 1.0314895900454293, + "heading": -0.2866246721097125, + "angularVelocity": 5.023344453927301e-7, + "velocityX": 3.7486563912927067, + "velocityY": -0.5007613897918232, + "timestamp": 1.9126320755317814 + }, + { + "x": 5.048746952164275, + "y": 0.9997439959210748, + "heading": -0.28662464026439505, + "angularVelocity": 5.023344449153729e-7, + "velocityX": 3.748656391292711, + "velocityY": -0.5007613897917893, + "timestamp": 1.9760267276984627 + }, + { + "x": 5.286391720182684, + "y": 0.9679984017967204, + "heading": -0.28662460841907766, + "angularVelocity": 5.023344443752201e-7, + "velocityX": 3.7486563912927116, + "velocityY": -0.5007613897917892, + "timestamp": 2.039421379865144 + }, + { + "x": 5.524036488201093, + "y": 0.9362528076723663, + "heading": -0.28662457657376017, + "angularVelocity": 5.023344462660274e-7, + "velocityX": 3.748656391292712, + "velocityY": -0.5007613897917833, + "timestamp": 2.1028160320318254 + }, + { + "x": 5.761681256220357, + "y": 0.9045072135544255, + "heading": -0.28662454472844273, + "angularVelocity": 5.023344454687972e-7, + "velocityX": 3.748656391306226, + "velocityY": -0.5007613896906189, + "timestamp": 2.1662106841985067 + }, + { + "x": 5.9993260389305805, + "y": 0.8727617294119203, + "heading": -0.28662451288312535, + "angularVelocity": 5.023344447486752e-7, + "velocityX": 3.748656623044358, + "velocityY": -0.5007596549159697, + "timestamp": 2.229605336365188 + }, + { + "x": 6.2372125316404645, + "y": 0.8428813874102156, + "heading": -0.28662447915131883, + "angularVelocity": 5.320923039829879e-7, + "velocityX": 3.752469405217604, + "velocityY": -0.4713385274698144, + "timestamp": 2.2929999885318693 + }, + { + "x": 6.463824548233647, + "y": 0.8220107822514932, + "heading": -0.244244352051896, + "angularVelocity": 0.6685126528968754, + "velocityX": 3.5746235502224897, + "velocityY": -0.32921712550528753, + "timestamp": 2.3563946406985505 + }, + { + "x": 6.673647732212342, + "y": 0.8041360744021581, + "heading": -0.1851425455819086, + "angularVelocity": 0.9322837881434082, + "velocityX": 3.309793126193282, + "velocityY": -0.28195923849124843, + "timestamp": 2.419789292865232 + }, + { + "x": 6.866141132976164, + "y": 0.7888964316418127, + "heading": -0.12766912233996416, + "angularVelocity": 0.9065973434294042, + "velocityX": 3.0364296385396288, + "velocityY": -0.24039319153098848, + "timestamp": 2.483183945031913 + }, + { + "x": 7.041349782094471, + "y": 0.7761674353551624, + "heading": -0.07636428899969135, + "angularVelocity": 0.8092927650329051, + "velocityX": 2.7637764879226316, + "velocityY": -0.2007897488447803, + "timestamp": 2.5465785971985944 + }, + { + "x": 7.199320466835692, + "y": 0.7658884300843142, + "heading": -0.0333868413674214, + "angularVelocity": 0.677934907179092, + "velocityX": 2.4918613690926774, + "velocityY": -0.16214309755690418, + "timestamp": 2.6099732493652756 + }, + { + "x": 7.340087890625, + "y": 0.7580231428146362, + "heading": 6.333223838299427e-25, + "angularVelocity": 0.5266507540673704, + "velocityX": 2.2204936690746884, + "velocityY": -0.12406862410094172, + "timestamp": 2.673367901531957 + }, + { + "x": 7.464023523183681, + "y": 0.7525372146747857, + "heading": 0.023010119033740877, + "angularVelocity": 0.36178962784032076, + "velocityX": 1.9486481714332364, + "velocityY": -0.08625561202725132, + "timestamp": 2.7369687271584358 + }, + { + "x": 7.57058232073761, + "y": 0.7491648361128735, + "heading": 0.03698369983604058, + "angularVelocity": 0.21970753782922778, + "velocityX": 1.675431042039261, + "velocityY": -0.05302413182051879, + "timestamp": 2.8005695527849146 + }, + { + "x": 7.659708945960142, + "y": 0.7476928559896933, + "heading": 0.042953350833786465, + "angularVelocity": 0.09386121860752329, + "velocityX": 1.4013438401879301, + "velocityY": -0.02314404111394868, + "timestamp": 2.8641703784113934 + }, + { + "x": 7.731365799776583, + "y": 0.747958827951943, + "heading": 0.041693436078442773, + "angularVelocity": -0.01980972326905067, + "velocityX": 1.1266654655911963, + "velocityY": 0.0041818948045075285, + "timestamp": 2.9277712040378723 + }, + { + "x": 7.785525988086016, + "y": 0.7498349377828146, + "heading": 0.03380693250989334, + "angularVelocity": -0.1240000187240661, + "velocityX": 0.85156423326185, + "velocityY": 0.029498199314107475, + "timestamp": 2.991372029664351 + }, + { + "x": 7.822169493216012, + "y": 0.7532180358509858, + "heading": 0.01977708439944101, + "angularVelocity": -0.2205922324475507, + "velocityX": 0.5761482617411275, + "velocityY": 0.05319267532845736, + "timestamp": 3.05497285529083 + }, + { + "x": 7.841280937194824, + "y": 0.7580231428146362, + "heading": -7.691291273987186e-25, + "angularVelocity": -0.3109564098364043, + "velocityX": 0.30049050135059296, + "velocityY": 0.07555101551464699, + "timestamp": 3.118573680917309 + }, + { + "x": 7.837626455350944, + "y": 0.7661832230204207, + "heading": -0.033360531574280844, + "angularVelocity": -0.41786111810293475, + "velocityX": -0.045774626401580844, + "velocityY": 0.10221000918425123, + "timestamp": 3.1984100909803836 + }, + { + "x": 7.806322738977659, + "y": 0.7764707725073046, + "heading": -0.07501113928857397, + "angularVelocity": -0.5216994060903668, + "velocityX": -0.39209824625822215, + "velocityY": 0.12885786671465047, + "timestamp": 3.2782465010434585 + }, + { + "x": 7.747364252906707, + "y": 0.7888847511850451, + "heading": -0.12465356840287752, + "angularVelocity": -0.6218018705385611, + "velocityX": -0.7384911974921111, + "velocityY": 0.15549269647686922, + "timestamp": 3.3580829111065333 + }, + { + "x": 7.660744362341046, + "y": 0.8034239632171427, + "heading": -0.18191677773733375, + "angularVelocity": -0.7172568166481353, + "velocityX": -1.084967253628106, + "velocityY": 0.18211254765351886, + "timestamp": 3.437919321169608 + }, + { + "x": 7.5464550089450855, + "y": 0.820087049493035, + "heading": -0.24632578558189497, + "angularVelocity": -0.8067623255313559, + "velocityX": -1.4315442453595524, + "velocityY": 0.20871537513682828, + "timestamp": 3.517755731232683 + }, + { + "x": 7.4044862600725985, + "y": 0.8388724659564971, + "heading": -0.31724786378277436, + "angularVelocity": -0.8883425262339242, + "velocityX": -1.7782456495767374, + "velocityY": 0.23529886236894457, + "timestamp": 3.597592141295758 + }, + { + "x": 7.234825718466435, + "y": 0.859778393906575, + "heading": -0.393791306886596, + "angularVelocity": -0.958753569246769, + "velocityX": -2.1251023370430016, + "velocityY": 0.261859569256197, + "timestamp": 3.6774285513588327 + }, + { + "x": 7.037458039233374, + "y": 0.8828023593152449, + "heading": -0.47459284454035716, + "angularVelocity": -1.012088814989599, + "velocityX": -2.4721512287079195, + "velocityY": 0.2883892874251208, + "timestamp": 3.7572649614219076 + }, + { + "x": 6.812366683779643, + "y": 0.9079395598147495, + "heading": -0.5572950154955562, + "angularVelocity": -1.0358954127554088, + "velocityX": -2.8194072764030573, + "velocityY": 0.3148588529925756, + "timestamp": 3.8371013714849824 + }, + { + "x": 6.559557139520272, + "y": 0.9351738299996345, + "heading": -0.6368741877538795, + "angularVelocity": -0.9967779387306077, + "velocityX": -3.166594590859465, + "velocityY": 0.3411259369424114, + "timestamp": 3.9169377815480573 + }, + { + "x": 6.279455223531701, + "y": 0.9644007155406045, + "heading": -0.6963848224463702, + "angularVelocity": -0.7454071976116461, + "velocityX": -3.5084482852782983, + "velocityY": 0.3660846663556045, + "timestamp": 3.996774191611132 + }, + { + "x": 5.978414008634455, + "y": 0.9876513907684487, + "heading": -0.696384838019443, + "angularVelocity": -1.9506228555195058e-7, + "velocityX": -3.770725846257472, + "velocityY": 0.2912289669522333, + "timestamp": 4.076610601674207 + }, + { + "x": 5.677372675665616, + "y": 1.0109005372077842, + "heading": -0.6963848535904748, + "angularVelocity": -1.950367229938273e-7, + "velocityX": -3.770727325176581, + "velocityY": 0.2912098179385523, + "timestamp": 4.156447011737281 + }, + { + "x": 5.3763313426932635, + "y": 1.0341496836016242, + "heading": -0.6963848691615065, + "angularVelocity": -1.950367226738497e-7, + "velocityX": -3.770727325220591, + "velocityY": 0.29120981736869106, + "timestamp": 4.236283421800357 + }, + { + "x": 5.075290009720911, + "y": 1.0573988299954629, + "heading": -0.6963848847325383, + "angularVelocity": -1.950367234539369e-7, + "velocityX": -3.7707273252205917, + "velocityY": 0.2912098173686746, + "timestamp": 4.316119831863432 + }, + { + "x": 4.774248676748558, + "y": 1.080647976389303, + "heading": -0.6963849003035701, + "angularVelocity": -1.9503672325222526e-7, + "velocityX": -3.7707273252205904, + "velocityY": 0.2912098173686933, + "timestamp": 4.395956241926507 + }, + { + "x": 4.473207343780055, + "y": 1.1038971228329835, + "heading": -0.696384915874602, + "angularVelocity": -1.950367229679151e-7, + "velocityX": -3.7707273251723774, + "velocityY": 0.29120981799297246, + "timestamp": 4.4757926519895825 + }, + { + "x": 4.172166140154539, + "y": 1.1271479440122283, + "heading": -0.696384931445633, + "angularVelocity": -1.9503671234983974e-7, + "velocityX": -3.770725705072129, + "velocityY": 0.2912307950830405, + "timestamp": 4.555629062052658 + }, + { + "x": 3.8735860947392786, + "y": 1.1720518781991272, + "heading": -0.6963849471144347, + "angularVelocity": -1.9626135115744237e-7, + "velocityX": -3.739898189051426, + "velocityY": 0.5624493154366842, + "timestamp": 4.635465472115733 + }, + { + "x": 3.580403438648322, + "y": 1.2442345385888913, + "heading": -0.6963849633936577, + "angularVelocity": -2.0390725406473894e-7, + "velocityX": -3.672292577526056, + "velocityY": 0.9041320912693386, + "timestamp": 4.715301882178808 + }, + { + "x": 3.2951050269479554, + "y": 1.343084028059958, + "heading": -0.6963849809642987, + "angularVelocity": -2.2008305293195764e-7, + "velocityX": -3.5735375810982304, + "velocityY": 1.2381504803756895, + "timestamp": 4.795138292241884 + }, + { + "x": 3.020110820111339, + "y": 1.4677619727807747, + "heading": -0.6963850007479858, + "angularVelocity": -2.4780281394367673e-7, + "velocityX": -3.444471095573519, + "velocityY": 1.5616677230641343, + "timestamp": 4.874974702304959 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.6963850232502631, + "angularVelocity": -2.818548242668458e-7, + "velocityX": -3.286187939959726, + "velocityY": 1.8719390328536167, + "timestamp": 4.954811112368034 + }, + { + "x": 2.574184199627506, + "y": 1.7387873388514508, + "heading": -0.696385046672039, + "angularVelocity": -4.0231081168076635e-7, + "velocityX": -3.153128229528173, + "velocityY": 2.0882929651305573, + "timestamp": 5.013029224414923 + }, + { + "x": 2.3991894743560565, + "y": 1.8724111676120654, + "heading": -0.6963850671596432, + "angularVelocity": -3.5191117316400294e-7, + "velocityX": -3.005846790952465, + "velocityY": 2.295227791876793, + "timestamp": 5.071247336461811 + }, + { + "x": 2.2335584944607234, + "y": 2.017479637711806, + "heading": -0.6963850855810837, + "angularVelocity": -3.164211261355859e-7, + "velocityX": -2.8450077488245626, + "velocityY": 2.4918099367925968, + "timestamp": 5.129465448508699 + }, + { + "x": 2.0780383142752217, + "y": 2.1733384097975033, + "heading": -0.6963851025529307, + "angularVelocity": -2.915217692304984e-7, + "velocityX": -2.6713367149427287, + "velocityY": 2.6771526352515695, + "timestamp": 5.1876835605555875 + }, + { + "x": 1.933330362591389, + "y": 2.339284462910405, + "heading": -0.6963851185385084, + "angularVelocity": -2.745808308512789e-7, + "velocityX": -2.4856173894352156, + "velocityY": 2.8504196937758937, + "timestamp": 5.245901672602476 + }, + { + "x": 1.8000872328431394, + "y": 2.5145692385052043, + "heading": -0.6963851339074891, + "angularVelocity": -2.63989679511656e-7, + "velocityX": -2.2886886067507177, + "velocityY": 3.010828923027044, + "timestamp": 5.304119784649364 + }, + { + "x": 1.6789094250107635, + "y": 2.6984018342496054, + "heading": -0.6963851489749523, + "angularVelocity": -2.5881057720147785e-7, + "velocityX": -2.0814451649476586, + "velocityY": 3.1576529928752177, + "timestamp": 5.3623378966962525 + }, + { + "x": 1.5665736783825392, + "y": 2.887766995894858, + "heading": -0.6963851639898362, + "angularVelocity": -2.5790743394160864e-7, + "velocityX": -1.9295669797356307, + "velocityY": 3.2526846884478298, + "timestamp": 5.420556008743141 + }, + { + "x": 1.454045590270684, + "y": 3.0767463661393277, + "heading": -0.6971250291224751, + "angularVelocity": -0.012708504391950055, + "velocityX": -1.9328707880672382, + "velocityY": 3.246058032460206, + "timestamp": 5.478774120790029 + }, + { + "x": 1.3486316890487728, + "y": 3.2544801987729373, + "heading": -0.7253513053199856, + "angularVelocity": -0.48483668063260893, + "velocityX": -1.8106719286433102, + "velocityY": 3.0528958494989498, + "timestamp": 5.536992232836917 + }, + { + "x": 1.250710894485142, + "y": 3.4195717811137616, + "heading": -0.7601787566171799, + "angularVelocity": -0.5982236467775679, + "velocityX": -1.6819644457856464, + "velocityY": 2.8357426329432034, + "timestamp": 5.595210344883806 + }, + { + "x": 1.1603283095238273, + "y": 3.571951341403833, + "heading": -0.796970435726129, + "angularVelocity": -0.631962765802461, + "velocityX": -1.5524822393505606, + "velocityY": 2.6173909618942433, + "timestamp": 5.653428456930694 + }, + { + "x": 1.0774888501735351, + "y": 3.7116123829065786, + "heading": -0.8336344586551708, + "angularVelocity": -0.6297700430325383, + "velocityX": -1.4229155916903404, + "velocityY": 2.398927697797285, + "timestamp": 5.711646568977582 + }, + { + "x": 1.0021907960664813, + "y": 3.8385581534626447, + "heading": -0.8689739272088923, + "angularVelocity": -0.6070184571643223, + "velocityX": -1.2933784944178508, + "velocityY": 2.1805202211611707, + "timestamp": 5.769864681024471 + }, + { + "x": 0.9344313404313289, + "y": 3.952793532693306, + "heading": -0.9022118371667394, + "angularVelocity": -0.5709204367719397, + "velocityX": -1.1638896084534611, + "velocityY": 1.9621965607310954, + "timestamp": 5.828082793071359 + }, + { + "x": 0.8742077657212176, + "y": 4.054323286032487, + "heading": -0.932802050953144, + "angularVelocity": -0.5254415286048384, + "velocityX": -1.034447401207516, + "velocityY": 1.743954755135484, + "timestamp": 5.886300905118247 + }, + { + "x": 0.8215176831864053, + "y": 4.143151677614014, + "heading": -0.9603390839625449, + "angularVelocity": -0.4729976985035671, + "velocityX": -0.9050462250025629, + "velocityY": 1.5257861936502937, + "timestamp": 5.944519017165136 + }, + { + "x": 0.7763590420271476, + "y": 4.219282430243998, + "heading": -0.9845095976890542, + "angularVelocity": -0.4151717202207242, + "velocityX": -0.7756802749441201, + "velocityY": 1.3076815780056374, + "timestamp": 6.002737129212024 + }, + { + "x": 0.7387300838441436, + "y": 4.282718775684524, + "heading": -1.005063997176884, + "angularVelocity": -0.3530585030183643, + "velocityX": -0.6463445285325996, + "velocityY": 1.0896324736438516, + "timestamp": 6.060955241258912 + }, + { + "x": 0.7086292901576616, + "y": 4.333463522187373, + "heading": -1.0217986905854597, + "angularVelocity": -0.2874482325207969, + "velocityX": -0.5170348647211226, + "velocityY": 0.871631606019453, + "timestamp": 6.119173353305801 + }, + { + "x": 0.6860553355642394, + "y": 4.371519118144364, + "heading": -1.0345444276715927, + "angularVelocity": -0.21893078696656726, + "velocityX": -0.3877479670800986, + "velocityY": 0.6536727938951569, + "timestamp": 6.177391465352689 + }, + { + "x": 0.6710070488250989, + "y": 4.396887706657534, + "heading": -1.043158306951245, + "angularVelocity": -0.1479587533294601, + "velocityX": -0.25848118755587396, + "velocityY": 0.4357507933740352, + "timestamp": 6.235609577399577 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": -0.07488724125811806, + "velocityX": -0.12923242079161093, + "velocityY": 0.2178611381134427, + "timestamp": 6.2938276894464655 + }, + { + "x": 0.6634833812713623, + "y": 4.409571170806885, + "heading": -1.0475181007536924, + "angularVelocity": 5.982509217155958e-25, + "velocityX": 4.848331418564855e-25, + "velocityY": 2.001430584095579e-25, + "timestamp": 6.352045801493354 + }, + { + "x": 0.6704852066209848, + "y": 4.3979853620663825, + "heading": -1.0417684451600073, + "angularVelocity": 0.10293302967346842, + "velocityX": 0.1253499596171797, + "velocityY": -0.20741457909008942, + "timestamp": 6.407904019326101 + }, + { + "x": 0.684490343853258, + "y": 4.3748114715841036, + "heading": -1.0303744638705372, + "angularVelocity": 0.20398039413979072, + "velocityX": 0.2507265318454658, + "velocityY": -0.414869850514514, + "timestamp": 6.463762237158848 + }, + { + "x": 0.7055004565291726, + "y": 4.340046908451465, + "heading": -1.013457886552716, + "angularVelocity": 0.3028484970371536, + "velocityX": 0.37613288592242283, + "velocityY": -0.6223715055989062, + "timestamp": 6.519620454991595 + }, + { + "x": 0.7335174317946694, + "y": 4.293688704250901, + "heading": -0.9911609182628701, + "angularVelocity": 0.39917077835544595, + "velocityX": 0.501573024570643, + "velocityY": -0.8299263026860625, + "timestamp": 6.575478672824342 + }, + { + "x": 0.7685434408113375, + "y": 4.235733439682599, + "heading": -0.9636517398174463, + "angularVelocity": 0.4924822078604928, + "velocityX": 0.6270520323713931, + "velocityY": -1.0375423136812831, + "timestamp": 6.6313368906570895 + }, + { + "x": 0.8105810191240276, + "y": 4.166177154216333, + "heading": -0.9311321639168866, + "angularVelocity": 0.5821806917995676, + "velocityX": 0.7525764326846329, + "velocityY": -1.2452292279451722, + "timestamp": 6.687195108489837 + }, + { + "x": 0.8596331764975813, + "y": 4.085015235089962, + "heading": -0.8938487182523878, + "angularVelocity": 0.6674657214473685, + "velocityX": 0.8781547152189431, + "velocityY": -1.4529987220392506, + "timestamp": 6.743053326322584 + }, + { + "x": 0.9157035502681051, + "y": 3.9922422826750155, + "heading": -0.85210948354869, + "angularVelocity": 0.7472353455435148, + "velocityX": 1.0037981150492816, + "velocityY": -1.660864882813316, + "timestamp": 6.798911544155331 + }, + { + "x": 0.9787966197972374, + "y": 3.8878519555094497, + "heading": -0.8063112066330053, + "angularVelocity": 0.8199022219580233, + "velocityX": 1.1295217065114396, + "velocityY": -1.8688445714135682, + "timestamp": 6.854769761988078 + }, + { + "x": 1.0489179917630689, + "y": 3.7718368279526966, + "heading": -0.7569861197121166, + "angularVelocity": 0.883040827915056, + "velocityX": 1.255345671352987, + "velocityY": -2.076957197312126, + "timestamp": 6.910627979820825 + }, + { + "x": 1.1260746973319842, + "y": 3.6441884223692846, + "heading": -0.7048901203297451, + "angularVelocity": 0.9326470017063425, + "velocityX": 1.3812955114311163, + "velocityY": -2.2852215938149305, + "timestamp": 6.966486197653572 + }, + { + "x": 1.2102750555046553, + "y": 3.5048981790691265, + "heading": -0.6511892064235582, + "angularVelocity": 0.9613789338388887, + "velocityX": 1.5073942821589288, + "velocityY": -2.493639229902148, + "timestamp": 7.0223444154863195 + }, + { + "x": 1.301525305664333, + "y": 3.3539635539059023, + "heading": -0.5979268023129009, + "angularVelocity": 0.9535285259214237, + "velocityX": 1.6336047532505, + "velocityY": -2.7021024124893804, + "timestamp": 7.078202633319067 + }, + { + "x": 1.3998010917877863, + "y": 3.1914330428750026, + "heading": -0.5495841709370638, + "angularVelocity": 0.8654524482071109, + "velocityX": 1.759379191396948, + "velocityY": -2.909697397033947, + "timestamp": 7.134060851151814 + }, + { + "x": 1.504860426118042, + "y": 3.0183888128426037, + "heading": -0.5232691930367682, + "angularVelocity": 0.4711030699026016, + "velocityX": 1.880821451282748, + "velocityY": -3.0979189230586286, + "timestamp": 7.189919068984561 + }, + { + "x": 1.6140923512047893, + "y": 2.837567528819654, + "heading": -0.5232691722973926, + "angularVelocity": 3.712860260075055e-7, + "velocityX": 1.9555211269685275, + "velocityY": -3.2371473892055076, + "timestamp": 7.245777286817308 + }, + { + "x": 1.723325651827911, + "y": 2.656747075748015, + "heading": -0.5232691515591942, + "angularVelocity": 3.712649483056031e-7, + "velocityX": 1.9555457524655004, + "velocityY": -3.2371325131256046, + "timestamp": 7.301635504650055 + }, + { + "x": 1.840787332697237, + "y": 2.481160334510265, + "heading": -0.5232691307624974, + "angularVelocity": 3.7231221667363717e-7, + "velocityX": 2.1028540728068745, + "velocityY": -3.143436150496223, + "timestamp": 7.357493722482802 + }, + { + "x": 1.969313021479893, + "y": 2.3135026555322407, + "heading": -0.5232691095918528, + "angularVelocity": 3.790068020002191e-7, + "velocityX": 2.300927128887148, + "velocityY": -3.0014863610584763, + "timestamp": 7.4133519403155494 + }, + { + "x": 2.108369556011401, + "y": 2.15447048936791, + "heading": -0.5232690876481252, + "angularVelocity": 3.9284689919031285e-7, + "velocityX": 2.4894552659713574, + "velocityY": -2.8470683873322, + "timestamp": 7.469210158148297 + }, + { + "x": 2.2573796552323504, + "y": 2.004724238981821, + "heading": -0.5232690644680587, + "angularVelocity": 4.1498041678709737e-7, + "velocityX": 2.6676486469210485, + "velocityY": -2.680827570161024, + "timestamp": 7.525068375981044 + }, + { + "x": 2.4157246392762546, + "y": 1.8648857088521948, + "heading": -0.5232690394793963, + "angularVelocity": 4.473587475801905e-7, + "velocityX": 2.8347661308856478, + "velocityY": -2.5034549177407666, + "timestamp": 7.580926593813791 + }, + { + "x": 2.582747045971562, + "y": 1.7355355514954984, + "heading": -0.5232690119339547, + "angularVelocity": 4.931314075834542e-7, + "velocityX": 2.990113418860087, + "velocityY": -2.3156871517813467, + "timestamp": 7.636784811646538 + }, + { + "x": 2.757753372192383, + "y": 1.617210865020752, + "heading": -0.5232689788045201, + "angularVelocity": 5.930986675824575e-7, + "velocityX": 3.1330452887851035, + "velocityY": -2.1183040037016325, + "timestamp": 7.692643029479285 + }, + { + "x": 3.027691139544385, + "y": 1.469523604710322, + "heading": -0.5232689514232575, + "angularVelocity": 3.3654683913245854e-7, + "velocityX": 3.317841970698504, + "velocityY": -1.8152442898308883, + "timestamp": 7.77400247026518 + }, + { + "x": 3.310285985294353, + "y": 1.3477940668377997, + "heading": -0.5232689280050355, + "angularVelocity": 2.8783656375516125e-7, + "velocityX": 3.473411850182739, + "velocityY": -1.4961943776489928, + "timestamp": 7.855361911051075 + }, + { + "x": 3.603048555472094, + "y": 1.2530945096711932, + "heading": -0.5232689069905222, + "angularVelocity": 2.58292255489841e-7, + "velocityX": 3.5983847399857765, + "velocityY": -1.1639651926297916, + "timestamp": 7.93672135183697 + }, + { + "x": 3.9033999225952147, + "y": 1.1862589972516577, + "heading": -0.5232688873218765, + "angularVelocity": 2.41750010150048e-7, + "velocityX": 3.6916596798338883, + "velocityY": -0.8214844125516969, + "timestamp": 8.018080792622865 + }, + { + "x": 4.208694254897112, + "y": 1.1478754007454768, + "heading": -0.5232688681867681, + "angularVelocity": 2.3519223109451003e-7, + "velocityX": 3.7524143400310193, + "velocityY": -0.47177802766849325, + "timestamp": 8.09944023340876 + }, + { + "x": 4.515097545025261, + "y": 1.1196804780654097, + "heading": -0.523268849105924, + "angularVelocity": 2.3452526230425957e-7, + "velocityX": 3.7660446921516857, + "velocityY": -0.34654764594885085, + "timestamp": 8.180799674194656 + }, + { + "x": 4.821500864274129, + "y": 1.0914858718513367, + "heading": -0.5232688300250796, + "angularVelocity": 2.3452526515688585e-7, + "velocityX": 3.766045050078413, + "velocityY": -0.34654375622209216, + "timestamp": 8.262159114980552 + }, + { + "x": 5.1279041835237855, + "y": 1.0632912656458422, + "heading": -0.523268810944235, + "angularVelocity": 2.3452526522511142e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.34654375611665356, + "timestamp": 8.343518555766448 + }, + { + "x": 5.434307502773443, + "y": 1.035096659440348, + "heading": -0.5232687918633908, + "angularVelocity": 2.3452526444881321e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.3465437561166505, + "timestamp": 8.424877996552343 + }, + { + "x": 5.7407108220231, + "y": 1.0069020532348534, + "heading": -0.5232687727825464, + "angularVelocity": 2.345252646023774e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.34654375611665045, + "timestamp": 8.506237437338239 + }, + { + "x": 6.047114141272757, + "y": 0.9787074470293591, + "heading": -0.5232687537017021, + "angularVelocity": 2.3452526402445505e-7, + "velocityX": 3.7660450500881155, + "velocityY": -0.3465437561166515, + "timestamp": 8.587596878124135 + }, + { + "x": 6.353517460522115, + "y": 0.9505128408206217, + "heading": -0.5232687346208578, + "angularVelocity": 2.3452526381848406e-7, + "velocityX": 3.766045050084448, + "velocityY": -0.3465437561565115, + "timestamp": 8.66895631891003 + }, + { + "x": 6.659920768762443, + "y": 0.9223181149752775, + "heading": -0.5232687155392478, + "angularVelocity": 2.3453467400509184e-7, + "velocityX": 3.7660449147709487, + "velocityY": -0.3465452266263895, + "timestamp": 8.750315759695926 + }, + { + "x": 6.9463502900273895, + "y": 0.8955257159158118, + "heading": -0.4609901078524047, + "angularVelocity": 0.7654748740313396, + "velocityX": 3.5205443707352013, + "velocityY": -0.3293090365501957, + "timestamp": 8.831675200481822 + }, + { + "x": 7.204305303751847, + "y": 0.8715128944520383, + "heading": -0.38283014022876516, + "angularVelocity": 0.9606748383303768, + "velocityX": 3.1705603090769854, + "velocityY": -0.29514486864487394, + "timestamp": 8.913034641267718 + }, + { + "x": 7.433510092361594, + "y": 0.8502195206373326, + "heading": -0.3054302667455531, + "angularVelocity": 0.9513324174252511, + "velocityX": 2.8171873650523347, + "velocityY": -0.2617197661269713, + "timestamp": 8.994394082053613 + }, + { + "x": 7.6339940540118905, + "y": 0.8316157327961914, + "heading": -0.23352877050863402, + "angularVelocity": 0.8837511116397012, + "velocityX": 2.464175757769631, + "velocityY": -0.2286616975416389, + "timestamp": 9.07575352283951 + }, + { + "x": 7.805788501774165, + "y": 0.8156861698842703, + "heading": -0.16939986530394652, + "angularVelocity": 0.7882171335647298, + "velocityX": 2.111549023725079, + "velocityY": -0.1957924336505852, + "timestamp": 9.157112963625405 + }, + { + "x": 7.948917168997056, + "y": 0.8024218818402508, + "heading": -0.11438811251079256, + "angularVelocity": 0.6761569679162687, + "velocityX": 1.7592139995104843, + "velocityY": -0.16303317618573446, + "timestamp": 9.2384724044113 + }, + { + "x": 8.063397872072363, + "y": 0.7918170637715687, + "heading": -0.06938398346942785, + "angularVelocity": 0.5531518973907075, + "velocityX": 1.4070979590011485, + "velocityY": -0.13034526744830524, + "timestamp": 9.319831845197196 + }, + { + "x": 8.149244274653059, + "y": 0.7838676113724481, + "heading": -0.03502060720303478, + "angularVelocity": 0.422364951558892, + "velocityX": 1.0551498603168807, + "velocityY": -0.09770780529379805, + "timestamp": 9.401191285983092 + }, + { + "x": 8.206467109701398, + "y": 0.7785704016133925, + "heading": -0.011770656498572084, + "angularVelocity": 0.2857683199377824, + "velocityX": 0.7033336819377413, + "velocityY": -0.06510872872142374, + "timestamp": 9.482550726768988 + }, + { + "x": 8.235074996948242, + "y": 0.7759228944778442, + "heading": 4.4114982490549674e-27, + "angularVelocity": 0.14467474683789522, + "velocityX": 0.35162344984804855, + "velocityY": -0.032540871839514214, + "timestamp": 9.563910167554884 + }, + { + "x": 8.235074996948242, + "y": 0.7759228944778442, + "heading": 2.1650776701546007e-27, + "angularVelocity": -8.454240815185174e-28, + "velocityX": -3.4252443089053894e-26, + "velocityY": -2.509402795665863e-26, + "timestamp": 9.64526960834078 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLaneSprint.1.traj b/src/main/deploy/choreo/SourceLaneSprint.1.traj deleted file mode 100644 index a119bd81..00000000 --- a/src/main/deploy/choreo/SourceLaneSprint.1.traj +++ /dev/null @@ -1,238 +0,0 @@ -{ - "samples": [ - { - "x": 1.493931531906128, - "y": 1.5100655555725098, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.82118644197349e-37, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.5360008147282442, - "y": 1.5100655555725098, - "heading": 3.2333963174943666e-19, - "angularVelocity": 3.292766805519721e-18, - "velocityX": 0.4284174372625368, - "velocityY": 1.8392808793575992e-26, - "timestamp": 0.09819694336189172 - }, - { - "x": 1.6201393797142145, - "y": 1.5100655555725098, - "heading": 7.027380416999147e-19, - "angularVelocity": 3.863647922469736e-18, - "velocityX": 0.8568348678215871, - "velocityY": 3.963342552313322e-26, - "timestamp": 0.19639388672378344 - }, - { - "x": 1.746347225986537, - "y": 1.5100655555725098, - "heading": 8.361393183209098e-19, - "angularVelocity": 1.3585074996311424e-18, - "velocityX": 1.2852522894444904, - "velocityY": 6.468173032965746e-26, - "timestamp": 0.29459083008567516 - }, - { - "x": 1.9146243523169562, - "y": 1.5100655555725098, - "heading": 3.80235401897157e-19, - "angularVelocity": -4.642750516315599e-18, - "velocityX": 1.7136696985593167, - "velocityY": 9.506575448004094e-26, - "timestamp": 0.3927877734475669 - }, - { - "x": 2.12497075686256, - "y": 1.5100655555725098, - "heading": -1.0623970199278788e-18, - "angularVelocity": -1.4691215100259113e-17, - "velocityX": 2.1420870889066284, - "velocityY": 1.3343481989110454e-25, - "timestamp": 0.4909847168094586 - }, - { - "x": 2.3773864365383615, - "y": 1.5100655555725098, - "heading": -3.9564889545725385e-18, - "angularVelocity": -2.947232200024047e-17, - "velocityX": 2.5705044478376236, - "velocityY": 1.8500903675379378e-25, - "timestamp": 0.5891816601713503 - }, - { - "x": 2.6718713848686027, - "y": 1.5100655555725098, - "heading": -8.853646546888228e-18, - "angularVelocity": -4.987077429157769e-17, - "velocityX": 2.9989217408219764, - "velocityY": 2.625793984498987e-25, - "timestamp": 0.6873786035332421 - }, - { - "x": 3.0084255847706767, - "y": 1.5100655555725098, - "heading": -1.6413613329692498e-17, - "angularVelocity": -7.698780165331962e-17, - "velocityX": 3.4273388598436147, - "velocityY": 4.161152605292971e-25, - "timestamp": 0.7855755468951338 - }, - { - "x": 3.3798020724552167, - "y": 1.5100655555725098, - "heading": -2.58741769005412e-17, - "angularVelocity": -9.634275005875749e-17, - "velocityX": 3.781955679779987, - "velocityY": -5.487818039619291e-25, - "timestamp": 0.8837724902570254 - }, - { - "x": 3.7511785589388484, - "y": 1.5100655555725098, - "heading": 5.146033898598045e-18, - "angularVelocity": 3.158979265252523e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.295977316525143e-24, - "timestamp": 0.9819694336189171 - }, - { - "x": 4.122555045422488, - "y": 1.5100655555725098, - "heading": 4.0038826563950476e-17, - "angularVelocity": 3.553348146159671e-16, - "velocityX": 3.7819556675504797, - "velocityY": 7.623936784532972e-25, - "timestamp": 1.0801663769808088 - }, - { - "x": 4.493931531906129, - "y": 1.5100655555725098, - "heading": 6.570668459860429e-17, - "angularVelocity": 2.613916192895976e-16, - "velocityX": 3.7819556675504797, - "velocityY": 3.4278643163770276e-26, - "timestamp": 1.1783633203427004 - }, - { - "x": 4.865308018389769, - "y": 1.5100655555725098, - "heading": 6.663782988380241e-17, - "angularVelocity": 9.482426370100795e-18, - "velocityX": 3.7819556675504797, - "velocityY": 2.693381196266241e-25, - "timestamp": 1.276560263704592 - }, - { - "x": 5.236684504873409, - "y": 1.5100655555725098, - "heading": 5.2326006182614025e-17, - "angularVelocity": -1.4574612214194992e-16, - "velocityX": 3.7819556675504797, - "velocityY": 5.124149697811417e-24, - "timestamp": 1.3747572070664837 - }, - { - "x": 5.60806099135704, - "y": 1.5100655555725098, - "heading": 2.884064553605338e-17, - "angularVelocity": -2.3916590315859936e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.1014556301472372e-23, - "timestamp": 1.4729541504283754 - }, - { - "x": 5.9794374790415805, - "y": 1.5100655555725098, - "heading": 1.6502190572837753e-17, - "angularVelocity": -1.2565009195595738e-16, - "velocityX": 3.7819556797799856, - "velocityY": 6.524035237986936e-24, - "timestamp": 1.571151093790267 - }, - { - "x": 6.315991678943653, - "y": 1.5100655555725098, - "heading": 9.967918627860597e-18, - "angularVelocity": -6.65425189408073e-17, - "velocityX": 3.427338859843601, - "velocityY": -3.262675265981889e-25, - "timestamp": 1.6693480371521587 - }, - { - "x": 6.610476627273894, - "y": 1.5100655555725098, - "heading": 5.2731830670839714e-18, - "angularVelocity": -4.78093860085579e-17, - "velocityX": 2.9989217408219764, - "velocityY": -2.379324733264266e-25, - "timestamp": 1.7675449805140504 - }, - { - "x": 6.862892306949696, - "y": 1.5100655555725098, - "heading": 2.2289001785398693e-18, - "angularVelocity": -3.10018090849907e-17, - "velocityX": 2.5705044478376236, - "velocityY": -1.7314309405461854e-25, - "timestamp": 1.865741923875942 - }, - { - "x": 7.0732387114953, - "y": 1.5100655555725098, - "heading": 5.002812433628901e-19, - "angularVelocity": -1.760359206249271e-17, - "velocityX": 2.1420870889066284, - "velocityY": -1.2661949502166272e-25, - "timestamp": 1.9639388672378337 - }, - { - "x": 7.24151583782572, - "y": 1.5100655555725098, - "heading": -2.663585350994398e-19, - "angularVelocity": -7.807165345874594e-18, - "velocityX": 1.7136696985593165, - "velocityY": -9.096401200333518e-26, - "timestamp": 2.0621358105997256 - }, - { - "x": 7.367723684098041, - "y": 1.5100655555725098, - "heading": -4.029353027704155e-19, - "angularVelocity": -1.390845344762092e-18, - "velocityX": 1.2852522894444902, - "velocityY": -6.226126404979919e-26, - "timestamp": 2.1603327539616175 - }, - { - "x": 7.451862249084011, - "y": 1.5100655555725098, - "heading": -2.1995708883691042e-19, - "angularVelocity": 1.8633799906775463e-18, - "velocityX": 0.8568348678215871, - "velocityY": -3.833262935804246e-26, - "timestamp": 2.2585296973235094 - }, - { - "x": 7.493931531906128, - "y": 1.5100655555725098, - "heading": 0, - "angularVelocity": 2.2399586452459634e-18, - "velocityX": 0.42841743726253684, - "velocityY": -1.7862050890834192e-26, - "timestamp": 2.3567266406854013 - }, - { - "x": 7.493931531906128, - "y": 1.5100655555725098, - "heading": 0, - "angularVelocity": 0, - "velocityX": -2.1810930337522913e-39, - "velocityY": 0, - "timestamp": 2.454923584047293 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLaneSprint.traj b/src/main/deploy/choreo/SourceLaneSprint.traj deleted file mode 100644 index a119bd81..00000000 --- a/src/main/deploy/choreo/SourceLaneSprint.traj +++ /dev/null @@ -1,238 +0,0 @@ -{ - "samples": [ - { - "x": 1.493931531906128, - "y": 1.5100655555725098, - "heading": 0, - "angularVelocity": 0, - "velocityX": 2.82118644197349e-37, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.5360008147282442, - "y": 1.5100655555725098, - "heading": 3.2333963174943666e-19, - "angularVelocity": 3.292766805519721e-18, - "velocityX": 0.4284174372625368, - "velocityY": 1.8392808793575992e-26, - "timestamp": 0.09819694336189172 - }, - { - "x": 1.6201393797142145, - "y": 1.5100655555725098, - "heading": 7.027380416999147e-19, - "angularVelocity": 3.863647922469736e-18, - "velocityX": 0.8568348678215871, - "velocityY": 3.963342552313322e-26, - "timestamp": 0.19639388672378344 - }, - { - "x": 1.746347225986537, - "y": 1.5100655555725098, - "heading": 8.361393183209098e-19, - "angularVelocity": 1.3585074996311424e-18, - "velocityX": 1.2852522894444904, - "velocityY": 6.468173032965746e-26, - "timestamp": 0.29459083008567516 - }, - { - "x": 1.9146243523169562, - "y": 1.5100655555725098, - "heading": 3.80235401897157e-19, - "angularVelocity": -4.642750516315599e-18, - "velocityX": 1.7136696985593167, - "velocityY": 9.506575448004094e-26, - "timestamp": 0.3927877734475669 - }, - { - "x": 2.12497075686256, - "y": 1.5100655555725098, - "heading": -1.0623970199278788e-18, - "angularVelocity": -1.4691215100259113e-17, - "velocityX": 2.1420870889066284, - "velocityY": 1.3343481989110454e-25, - "timestamp": 0.4909847168094586 - }, - { - "x": 2.3773864365383615, - "y": 1.5100655555725098, - "heading": -3.9564889545725385e-18, - "angularVelocity": -2.947232200024047e-17, - "velocityX": 2.5705044478376236, - "velocityY": 1.8500903675379378e-25, - "timestamp": 0.5891816601713503 - }, - { - "x": 2.6718713848686027, - "y": 1.5100655555725098, - "heading": -8.853646546888228e-18, - "angularVelocity": -4.987077429157769e-17, - "velocityX": 2.9989217408219764, - "velocityY": 2.625793984498987e-25, - "timestamp": 0.6873786035332421 - }, - { - "x": 3.0084255847706767, - "y": 1.5100655555725098, - "heading": -1.6413613329692498e-17, - "angularVelocity": -7.698780165331962e-17, - "velocityX": 3.4273388598436147, - "velocityY": 4.161152605292971e-25, - "timestamp": 0.7855755468951338 - }, - { - "x": 3.3798020724552167, - "y": 1.5100655555725098, - "heading": -2.58741769005412e-17, - "angularVelocity": -9.634275005875749e-17, - "velocityX": 3.781955679779987, - "velocityY": -5.487818039619291e-25, - "timestamp": 0.8837724902570254 - }, - { - "x": 3.7511785589388484, - "y": 1.5100655555725098, - "heading": 5.146033898598045e-18, - "angularVelocity": 3.158979265252523e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.295977316525143e-24, - "timestamp": 0.9819694336189171 - }, - { - "x": 4.122555045422488, - "y": 1.5100655555725098, - "heading": 4.0038826563950476e-17, - "angularVelocity": 3.553348146159671e-16, - "velocityX": 3.7819556675504797, - "velocityY": 7.623936784532972e-25, - "timestamp": 1.0801663769808088 - }, - { - "x": 4.493931531906129, - "y": 1.5100655555725098, - "heading": 6.570668459860429e-17, - "angularVelocity": 2.613916192895976e-16, - "velocityX": 3.7819556675504797, - "velocityY": 3.4278643163770276e-26, - "timestamp": 1.1783633203427004 - }, - { - "x": 4.865308018389769, - "y": 1.5100655555725098, - "heading": 6.663782988380241e-17, - "angularVelocity": 9.482426370100795e-18, - "velocityX": 3.7819556675504797, - "velocityY": 2.693381196266241e-25, - "timestamp": 1.276560263704592 - }, - { - "x": 5.236684504873409, - "y": 1.5100655555725098, - "heading": 5.2326006182614025e-17, - "angularVelocity": -1.4574612214194992e-16, - "velocityX": 3.7819556675504797, - "velocityY": 5.124149697811417e-24, - "timestamp": 1.3747572070664837 - }, - { - "x": 5.60806099135704, - "y": 1.5100655555725098, - "heading": 2.884064553605338e-17, - "angularVelocity": -2.3916590315859936e-16, - "velocityX": 3.7819556675503945, - "velocityY": -1.1014556301472372e-23, - "timestamp": 1.4729541504283754 - }, - { - "x": 5.9794374790415805, - "y": 1.5100655555725098, - "heading": 1.6502190572837753e-17, - "angularVelocity": -1.2565009195595738e-16, - "velocityX": 3.7819556797799856, - "velocityY": 6.524035237986936e-24, - "timestamp": 1.571151093790267 - }, - { - "x": 6.315991678943653, - "y": 1.5100655555725098, - "heading": 9.967918627860597e-18, - "angularVelocity": -6.65425189408073e-17, - "velocityX": 3.427338859843601, - "velocityY": -3.262675265981889e-25, - "timestamp": 1.6693480371521587 - }, - { - "x": 6.610476627273894, - "y": 1.5100655555725098, - "heading": 5.2731830670839714e-18, - "angularVelocity": -4.78093860085579e-17, - "velocityX": 2.9989217408219764, - "velocityY": -2.379324733264266e-25, - "timestamp": 1.7675449805140504 - }, - { - "x": 6.862892306949696, - "y": 1.5100655555725098, - "heading": 2.2289001785398693e-18, - "angularVelocity": -3.10018090849907e-17, - "velocityX": 2.5705044478376236, - "velocityY": -1.7314309405461854e-25, - "timestamp": 1.865741923875942 - }, - { - "x": 7.0732387114953, - "y": 1.5100655555725098, - "heading": 5.002812433628901e-19, - "angularVelocity": -1.760359206249271e-17, - "velocityX": 2.1420870889066284, - "velocityY": -1.2661949502166272e-25, - "timestamp": 1.9639388672378337 - }, - { - "x": 7.24151583782572, - "y": 1.5100655555725098, - "heading": -2.663585350994398e-19, - "angularVelocity": -7.807165345874594e-18, - "velocityX": 1.7136696985593165, - "velocityY": -9.096401200333518e-26, - "timestamp": 2.0621358105997256 - }, - { - "x": 7.367723684098041, - "y": 1.5100655555725098, - "heading": -4.029353027704155e-19, - "angularVelocity": -1.390845344762092e-18, - "velocityX": 1.2852522894444902, - "velocityY": -6.226126404979919e-26, - "timestamp": 2.1603327539616175 - }, - { - "x": 7.451862249084011, - "y": 1.5100655555725098, - "heading": -2.1995708883691042e-19, - "angularVelocity": 1.8633799906775463e-18, - "velocityX": 0.8568348678215871, - "velocityY": -3.833262935804246e-26, - "timestamp": 2.2585296973235094 - }, - { - "x": 7.493931531906128, - "y": 1.5100655555725098, - "heading": 0, - "angularVelocity": 2.2399586452459634e-18, - "velocityX": 0.42841743726253684, - "velocityY": -1.7862050890834192e-26, - "timestamp": 2.3567266406854013 - }, - { - "x": 7.493931531906128, - "y": 1.5100655555725098, - "heading": 0, - "angularVelocity": 0, - "velocityX": -2.1810930337522913e-39, - "velocityY": 0, - "timestamp": 2.454923584047293 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLaneTaxi.1.traj b/src/main/deploy/choreo/SourceLaneTaxi.1.traj deleted file mode 100644 index 2a7cdf97..00000000 --- a/src/main/deploy/choreo/SourceLaneTaxi.1.traj +++ /dev/null @@ -1,139 +0,0 @@ -{ - "samples": [ - { - "x": 1.468971848487854, - "y": 1.9215065240859983, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -6.616666962933087e-41, - "timestamp": 0 - }, - { - "x": 1.5097881789229493, - "y": 1.9215065240859983, - "heading": 2.3748244992157107e-19, - "angularVelocity": 2.455567735557075e-18, - "velocityX": 0.42198942031848213, - "velocityY": 1.4827190365964012e-17, - "timestamp": 0.09672358706123588 - }, - { - "x": 1.5914208389649156, - "y": 1.9215065240859985, - "heading": 6.823321419293863e-18, - "angularVelocity": 6.808983664986858e-17, - "velocityX": 0.843978832074171, - "velocityY": 2.965467782674968e-17, - "timestamp": 0.19344717412247175 - }, - { - "x": 1.7138698274174293, - "y": 1.9215065240859985, - "heading": 1.3350406511427553e-17, - "angularVelocity": 6.748261205621051e-17, - "velocityX": 1.2659682314613812, - "velocityY": 4.448259121719168e-17, - "timestamp": 0.29017076118370766 - }, - { - "x": 1.8771351424005533, - "y": 1.9215065240859985, - "heading": 2.3094752603641018e-17, - "angularVelocity": 1.0074517896422799e-16, - "velocityX": 1.6879576114124106, - "velocityY": 5.931116840042e-17, - "timestamp": 0.3868943482449435 - }, - { - "x": 2.081216780530401, - "y": 1.9215065240859985, - "heading": 2.7994689255823954e-17, - "angularVelocity": 5.0660149601833395e-17, - "velocityX": 2.1099469563783173, - "velocityY": 7.414094299456716e-17, - "timestamp": 0.48361793530617936 - }, - { - "x": 2.326114733911239, - "y": 1.9215065240859985, - "heading": 3.194288143588596e-17, - "angularVelocity": 4.082043898429076e-17, - "velocityX": 2.531936219712287, - "velocityY": 8.89740008515513e-17, - "timestamp": 0.5803415223674152 - }, - { - "x": 2.6118289630644647, - "y": 1.9215065240859985, - "heading": 2.800063510694937e-17, - "angularVelocity": -4.0776590509092634e-17, - "velocityX": 2.953925074887259, - "velocityY": 1.036106690795216e-16, - "timestamp": 0.6770651094286511 - }, - { - "x": 2.8567269164453046, - "y": 1.9215065240859985, - "heading": 2.2632732501440496e-17, - "angularVelocity": -5.54925246223202e-17, - "velocityX": 2.531936219712306, - "velocityY": 8.894540117079268e-17, - "timestamp": 0.7737886964898869 - }, - { - "x": 3.0608085545751536, - "y": 1.9215065240859985, - "heading": 1.7892029047767983e-17, - "angularVelocity": -4.9009641288663106e-17, - "velocityX": 2.109946956378327, - "velocityY": 7.412152260849424e-17, - "timestamp": 0.8705122835511228 - }, - { - "x": 3.224073869558278, - "y": 1.9215065240859985, - "heading": 2.083425694040062e-17, - "angularVelocity": 3.042138426747003e-17, - "velocityX": 1.687957611412416, - "velocityY": 5.929781651761968e-17, - "timestamp": 0.9672358706123586 - }, - { - "x": 3.3465228580107924, - "y": 1.9215065240859985, - "heading": 1.131382345905398e-17, - "angularVelocity": -9.842749204538218e-17, - "velocityX": 1.265968231461384, - "velocityY": 4.4473700308316656e-17, - "timestamp": 1.0639594576735945 - }, - { - "x": 3.4281555180527588, - "y": 1.9215065240859983, - "heading": -4.743251660877572e-19, - "angularVelocity": -1.2187342421598578e-16, - "velocityX": 0.8439788320741727, - "velocityY": 2.964930533250184e-17, - "timestamp": 1.1606830447348304 - }, - { - "x": 3.468971848487854, - "y": 1.9215065240859983, - "heading": -3.025400677588882e-41, - "angularVelocity": 4.904504421960316e-18, - "velocityX": 0.42198942031848286, - "velocityY": 1.4824720643168207e-17, - "timestamp": 1.2574066317960664 - }, - { - "x": 3.468971848487854, - "y": 1.9215065240859983, - "heading": 4.753747394144705e-41, - "angularVelocity": 8.043397133285877e-40, - "velocityX": -3.5738155514754405e-40, - "velocityY": 2.012790081694559e-42, - "timestamp": 1.3541302188573023 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLaneTaxi.traj b/src/main/deploy/choreo/SourceLaneTaxi.traj deleted file mode 100644 index 2a7cdf97..00000000 --- a/src/main/deploy/choreo/SourceLaneTaxi.traj +++ /dev/null @@ -1,139 +0,0 @@ -{ - "samples": [ - { - "x": 1.468971848487854, - "y": 1.9215065240859983, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -6.616666962933087e-41, - "timestamp": 0 - }, - { - "x": 1.5097881789229493, - "y": 1.9215065240859983, - "heading": 2.3748244992157107e-19, - "angularVelocity": 2.455567735557075e-18, - "velocityX": 0.42198942031848213, - "velocityY": 1.4827190365964012e-17, - "timestamp": 0.09672358706123588 - }, - { - "x": 1.5914208389649156, - "y": 1.9215065240859985, - "heading": 6.823321419293863e-18, - "angularVelocity": 6.808983664986858e-17, - "velocityX": 0.843978832074171, - "velocityY": 2.965467782674968e-17, - "timestamp": 0.19344717412247175 - }, - { - "x": 1.7138698274174293, - "y": 1.9215065240859985, - "heading": 1.3350406511427553e-17, - "angularVelocity": 6.748261205621051e-17, - "velocityX": 1.2659682314613812, - "velocityY": 4.448259121719168e-17, - "timestamp": 0.29017076118370766 - }, - { - "x": 1.8771351424005533, - "y": 1.9215065240859985, - "heading": 2.3094752603641018e-17, - "angularVelocity": 1.0074517896422799e-16, - "velocityX": 1.6879576114124106, - "velocityY": 5.931116840042e-17, - "timestamp": 0.3868943482449435 - }, - { - "x": 2.081216780530401, - "y": 1.9215065240859985, - "heading": 2.7994689255823954e-17, - "angularVelocity": 5.0660149601833395e-17, - "velocityX": 2.1099469563783173, - "velocityY": 7.414094299456716e-17, - "timestamp": 0.48361793530617936 - }, - { - "x": 2.326114733911239, - "y": 1.9215065240859985, - "heading": 3.194288143588596e-17, - "angularVelocity": 4.082043898429076e-17, - "velocityX": 2.531936219712287, - "velocityY": 8.89740008515513e-17, - "timestamp": 0.5803415223674152 - }, - { - "x": 2.6118289630644647, - "y": 1.9215065240859985, - "heading": 2.800063510694937e-17, - "angularVelocity": -4.0776590509092634e-17, - "velocityX": 2.953925074887259, - "velocityY": 1.036106690795216e-16, - "timestamp": 0.6770651094286511 - }, - { - "x": 2.8567269164453046, - "y": 1.9215065240859985, - "heading": 2.2632732501440496e-17, - "angularVelocity": -5.54925246223202e-17, - "velocityX": 2.531936219712306, - "velocityY": 8.894540117079268e-17, - "timestamp": 0.7737886964898869 - }, - { - "x": 3.0608085545751536, - "y": 1.9215065240859985, - "heading": 1.7892029047767983e-17, - "angularVelocity": -4.9009641288663106e-17, - "velocityX": 2.109946956378327, - "velocityY": 7.412152260849424e-17, - "timestamp": 0.8705122835511228 - }, - { - "x": 3.224073869558278, - "y": 1.9215065240859985, - "heading": 2.083425694040062e-17, - "angularVelocity": 3.042138426747003e-17, - "velocityX": 1.687957611412416, - "velocityY": 5.929781651761968e-17, - "timestamp": 0.9672358706123586 - }, - { - "x": 3.3465228580107924, - "y": 1.9215065240859985, - "heading": 1.131382345905398e-17, - "angularVelocity": -9.842749204538218e-17, - "velocityX": 1.265968231461384, - "velocityY": 4.4473700308316656e-17, - "timestamp": 1.0639594576735945 - }, - { - "x": 3.4281555180527588, - "y": 1.9215065240859983, - "heading": -4.743251660877572e-19, - "angularVelocity": -1.2187342421598578e-16, - "velocityX": 0.8439788320741727, - "velocityY": 2.964930533250184e-17, - "timestamp": 1.1606830447348304 - }, - { - "x": 3.468971848487854, - "y": 1.9215065240859983, - "heading": -3.025400677588882e-41, - "angularVelocity": 4.904504421960316e-18, - "velocityX": 0.42198942031848286, - "velocityY": 1.4824720643168207e-17, - "timestamp": 1.2574066317960664 - }, - { - "x": 3.468971848487854, - "y": 1.9215065240859983, - "heading": 4.753747394144705e-41, - "angularVelocity": 8.043397133285877e-40, - "velocityX": -3.5738155514754405e-40, - "velocityY": 2.012790081694559e-42, - "timestamp": 1.3541302188573023 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Sprint.1.traj b/src/main/deploy/choreo/Sprint.1.traj new file mode 100644 index 00000000..90649b69 --- /dev/null +++ b/src/main/deploy/choreo/Sprint.1.traj @@ -0,0 +1,239 @@ +{ + "samples": [ + { + "x": 1.493931531906128, + "y": 1.5100655555725098, + "heading": -2.3489391793091186e-30, + "angularVelocity": -1.8438286181159006e-30, + "velocityX": -6.15674840421599e-23, + "velocityY": -1.1532892177192752e-40, + "timestamp": 0 + }, + { + "x": 1.536000814827162, + "y": 1.5100655555725098, + "heading": -8.185103677237685e-20, + "angularVelocity": -8.335395549473075e-19, + "velocityX": 0.42841743669638827, + "velocityY": -2.1388205270259975e-34, + "timestamp": 0.09819694372254935 + }, + { + "x": 1.62013938001091, + "y": 1.5100655555725098, + "heading": -2.4554391273005915e-19, + "angularVelocity": -1.6669854452684527e-18, + "velocityX": 0.8568348666886964, + "velocityY": 1.6301517969798743e-35, + "timestamp": 0.1963938874450987 + }, + { + "x": 1.7463472265796118, + "y": 1.5100655555725098, + "heading": -4.910663826735234e-19, + "angularVelocity": -2.5003066351518114e-18, + "velocityX": 1.2852522877422325, + "velocityY": -6.448425500517587e-35, + "timestamp": 0.29459083116764806 + }, + { + "x": 1.9146243533044036, + "y": 1.5100655555725098, + "heading": -8.184016386536964e-19, + "angularVelocity": -3.3334566593304657e-18, + "velocityX": 1.7136696962814904, + "velocityY": 1.7221012737740448e-34, + "timestamp": 0.3927877748901974 + }, + { + "x": 2.12497075834199, + "y": 1.5100655555725098, + "heading": -1.2275245556703786e-18, + "angularVelocity": -4.1663508201467245e-18, + "velocityX": 2.1420870860493375, + "velocityY": 1.0129663973735025e-34, + "timestamp": 0.49098471861274673 + }, + { + "x": 2.3773864386202144, + "y": 1.5100655555725098, + "heading": -1.7183931876154327e-18, + "angularVelocity": -4.9988178178910575e-18, + "velocityX": 2.570504444531517, + "velocityY": 3.8560644096510287e-35, + "timestamp": 0.5891816623352961 + }, + { + "x": 2.6718713879947695, + "y": 1.5100655555725098, + "heading": -2.290925793320508e-18, + "angularVelocity": -5.830452394938298e-18, + "velocityX": 2.9989217404424307, + "velocityY": -1.57638284621387e-34, + "timestamp": 0.6873786060578455 + }, + { + "x": 3.008425588032791, + "y": 1.5100655555725098, + "heading": -2.9448687714922995e-18, + "angularVelocity": -6.6595043937130054e-18, + "velocityX": 3.4273388486401246, + "velocityY": -2.5130241601160745e-34, + "timestamp": 0.7855755497803949 + }, + { + "x": 3.379802074000998, + "y": 1.5100655555725098, + "heading": -4.375619805389422e-18, + "angularVelocity": -1.4570219598048688e-17, + "velocityX": 3.781955648411144, + "velocityY": 2.8959618409076868e-34, + "timestamp": 0.8837724935029443 + }, + { + "x": 3.7511785599693743, + "y": 1.5100655555725098, + "heading": -4.530753712175346e-18, + "angularVelocity": -1.579824187012377e-18, + "velocityX": 3.781955648412874, + "velocityY": 1.4582574743238952e-34, + "timestamp": 0.9819694372254937 + }, + { + "x": 4.122555045937751, + "y": 1.5100655555725098, + "heading": -2.578867570214759e-18, + "angularVelocity": 1.9877259596570474e-17, + "velocityX": 3.781955648412874, + "velocityY": 2.230882066085756e-34, + "timestamp": 1.080166380948043 + }, + { + "x": 4.493931531906128, + "y": 1.5100655555725098, + "heading": 2.246937505394666e-18, + "angularVelocity": 4.9144147390644355e-17, + "velocityX": 3.781955648412874, + "velocityY": 2.057712754500175e-33, + "timestamp": 1.1783633246705925 + }, + { + "x": 4.865308017874504, + "y": 1.5100655555725098, + "heading": 5.0874648823324006e-18, + "angularVelocity": 2.8926840991771126e-17, + "velocityX": 3.781955648412874, + "velocityY": -7.440450670385348e-34, + "timestamp": 1.2765602683931419 + }, + { + "x": 5.2366845038428815, + "y": 1.5100655555725098, + "heading": 4.705898936719953e-18, + "angularVelocity": -3.885721196050614e-18, + "velocityX": 3.781955648412874, + "velocityY": -9.318220497232172e-35, + "timestamp": 1.3747572121156912 + }, + { + "x": 5.608060989811258, + "y": 1.5100655555725098, + "heading": 7.44292639457605e-18, + "angularVelocity": 2.7872837525287766e-17, + "velocityX": 3.781955648412874, + "velocityY": 9.982777944892104e-32, + "timestamp": 1.4729541558382406 + }, + { + "x": 5.979437475779465, + "y": 1.5100655555725098, + "heading": 2.983432162684063e-18, + "angularVelocity": -4.541377829935033e-17, + "velocityX": 3.781955648411144, + "velocityY": -1.946339553545498e-31, + "timestamp": 1.57115109956079 + }, + { + "x": 6.315991675817486, + "y": 1.5100655555725098, + "heading": 2.316345923938171e-18, + "angularVelocity": -6.793350316770868e-18, + "velocityX": 3.4273388486401246, + "velocityY": 3.102303151666472e-32, + "timestamp": 1.6693480432833394 + }, + { + "x": 6.610476625192041, + "y": 1.5100655555725098, + "heading": 1.7355272119353566e-18, + "angularVelocity": -5.9148349223730734e-18, + "velocityX": 2.99892174044243, + "velocityY": 1.9710968904355283e-32, + "timestamp": 1.7675449870058888 + }, + { + "x": 6.8628923054702655, + "y": 1.5100655555725098, + "heading": 1.23879193875581e-18, + "angularVelocity": -5.058561441389415e-18, + "velocityX": 2.570504444531517, + "velocityY": 1.4085581304064316e-32, + "timestamp": 1.8657419307284382 + }, + { + "x": 7.073238710507852, + "y": 1.5100655555725098, + "heading": 8.254145878831398e-19, + "angularVelocity": -4.20967634225236e-18, + "velocityX": 2.1420870860493375, + "velocityY": 1.0374368292665007e-32, + "timestamp": 1.9639388744509876 + }, + { + "x": 7.241515837232644, + "y": 1.5100655555725098, + "heading": 4.95031593430639e-19, + "angularVelocity": -3.364493658617499e-18, + "velocityX": 1.7136696962814904, + "velocityY": 7.695161857259128e-33, + "timestamp": 2.062135818173537 + }, + { + "x": 7.367723683801346, + "y": 1.5100655555725098, + "heading": 2.474248388890608e-19, + "angularVelocity": -2.521532189828829e-18, + "velocityX": 1.2852522877422325, + "velocityY": 5.4450246921373414e-33, + "timestamp": 2.160332761896086 + }, + { + "x": 7.451862248985094, + "y": 1.5100655555725098, + "heading": 8.244895136962396e-20, + "angularVelocity": -1.6800511427648084e-18, + "velocityX": 0.8568348666886965, + "velocityY": 3.3251578108171654e-33, + "timestamp": 2.2585297056186353 + }, + { + "x": 7.493931531906128, + "y": 1.5100655555725098, + "heading": 4.9021992998797275e-30, + "angularVelocity": -8.396284877798725e-19, + "velocityX": 0.4284174366963883, + "velocityY": 1.5649544358297725e-33, + "timestamp": 2.3567266493411845 + }, + { + "x": 7.493931531906128, + "y": 1.5100655555725098, + "heading": 2.348585897046796e-30, + "angularVelocity": -2.0879214567555354e-30, + "velocityX": -3.9015027280463263e-23, + "velocityY": -1.2968680331891934e-40, + "timestamp": 2.4549235930637336 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Sprint.traj b/src/main/deploy/choreo/Sprint.traj new file mode 100644 index 00000000..90649b69 --- /dev/null +++ b/src/main/deploy/choreo/Sprint.traj @@ -0,0 +1,239 @@ +{ + "samples": [ + { + "x": 1.493931531906128, + "y": 1.5100655555725098, + "heading": -2.3489391793091186e-30, + "angularVelocity": -1.8438286181159006e-30, + "velocityX": -6.15674840421599e-23, + "velocityY": -1.1532892177192752e-40, + "timestamp": 0 + }, + { + "x": 1.536000814827162, + "y": 1.5100655555725098, + "heading": -8.185103677237685e-20, + "angularVelocity": -8.335395549473075e-19, + "velocityX": 0.42841743669638827, + "velocityY": -2.1388205270259975e-34, + "timestamp": 0.09819694372254935 + }, + { + "x": 1.62013938001091, + "y": 1.5100655555725098, + "heading": -2.4554391273005915e-19, + "angularVelocity": -1.6669854452684527e-18, + "velocityX": 0.8568348666886964, + "velocityY": 1.6301517969798743e-35, + "timestamp": 0.1963938874450987 + }, + { + "x": 1.7463472265796118, + "y": 1.5100655555725098, + "heading": -4.910663826735234e-19, + "angularVelocity": -2.5003066351518114e-18, + "velocityX": 1.2852522877422325, + "velocityY": -6.448425500517587e-35, + "timestamp": 0.29459083116764806 + }, + { + "x": 1.9146243533044036, + "y": 1.5100655555725098, + "heading": -8.184016386536964e-19, + "angularVelocity": -3.3334566593304657e-18, + "velocityX": 1.7136696962814904, + "velocityY": 1.7221012737740448e-34, + "timestamp": 0.3927877748901974 + }, + { + "x": 2.12497075834199, + "y": 1.5100655555725098, + "heading": -1.2275245556703786e-18, + "angularVelocity": -4.1663508201467245e-18, + "velocityX": 2.1420870860493375, + "velocityY": 1.0129663973735025e-34, + "timestamp": 0.49098471861274673 + }, + { + "x": 2.3773864386202144, + "y": 1.5100655555725098, + "heading": -1.7183931876154327e-18, + "angularVelocity": -4.9988178178910575e-18, + "velocityX": 2.570504444531517, + "velocityY": 3.8560644096510287e-35, + "timestamp": 0.5891816623352961 + }, + { + "x": 2.6718713879947695, + "y": 1.5100655555725098, + "heading": -2.290925793320508e-18, + "angularVelocity": -5.830452394938298e-18, + "velocityX": 2.9989217404424307, + "velocityY": -1.57638284621387e-34, + "timestamp": 0.6873786060578455 + }, + { + "x": 3.008425588032791, + "y": 1.5100655555725098, + "heading": -2.9448687714922995e-18, + "angularVelocity": -6.6595043937130054e-18, + "velocityX": 3.4273388486401246, + "velocityY": -2.5130241601160745e-34, + "timestamp": 0.7855755497803949 + }, + { + "x": 3.379802074000998, + "y": 1.5100655555725098, + "heading": -4.375619805389422e-18, + "angularVelocity": -1.4570219598048688e-17, + "velocityX": 3.781955648411144, + "velocityY": 2.8959618409076868e-34, + "timestamp": 0.8837724935029443 + }, + { + "x": 3.7511785599693743, + "y": 1.5100655555725098, + "heading": -4.530753712175346e-18, + "angularVelocity": -1.579824187012377e-18, + "velocityX": 3.781955648412874, + "velocityY": 1.4582574743238952e-34, + "timestamp": 0.9819694372254937 + }, + { + "x": 4.122555045937751, + "y": 1.5100655555725098, + "heading": -2.578867570214759e-18, + "angularVelocity": 1.9877259596570474e-17, + "velocityX": 3.781955648412874, + "velocityY": 2.230882066085756e-34, + "timestamp": 1.080166380948043 + }, + { + "x": 4.493931531906128, + "y": 1.5100655555725098, + "heading": 2.246937505394666e-18, + "angularVelocity": 4.9144147390644355e-17, + "velocityX": 3.781955648412874, + "velocityY": 2.057712754500175e-33, + "timestamp": 1.1783633246705925 + }, + { + "x": 4.865308017874504, + "y": 1.5100655555725098, + "heading": 5.0874648823324006e-18, + "angularVelocity": 2.8926840991771126e-17, + "velocityX": 3.781955648412874, + "velocityY": -7.440450670385348e-34, + "timestamp": 1.2765602683931419 + }, + { + "x": 5.2366845038428815, + "y": 1.5100655555725098, + "heading": 4.705898936719953e-18, + "angularVelocity": -3.885721196050614e-18, + "velocityX": 3.781955648412874, + "velocityY": -9.318220497232172e-35, + "timestamp": 1.3747572121156912 + }, + { + "x": 5.608060989811258, + "y": 1.5100655555725098, + "heading": 7.44292639457605e-18, + "angularVelocity": 2.7872837525287766e-17, + "velocityX": 3.781955648412874, + "velocityY": 9.982777944892104e-32, + "timestamp": 1.4729541558382406 + }, + { + "x": 5.979437475779465, + "y": 1.5100655555725098, + "heading": 2.983432162684063e-18, + "angularVelocity": -4.541377829935033e-17, + "velocityX": 3.781955648411144, + "velocityY": -1.946339553545498e-31, + "timestamp": 1.57115109956079 + }, + { + "x": 6.315991675817486, + "y": 1.5100655555725098, + "heading": 2.316345923938171e-18, + "angularVelocity": -6.793350316770868e-18, + "velocityX": 3.4273388486401246, + "velocityY": 3.102303151666472e-32, + "timestamp": 1.6693480432833394 + }, + { + "x": 6.610476625192041, + "y": 1.5100655555725098, + "heading": 1.7355272119353566e-18, + "angularVelocity": -5.9148349223730734e-18, + "velocityX": 2.99892174044243, + "velocityY": 1.9710968904355283e-32, + "timestamp": 1.7675449870058888 + }, + { + "x": 6.8628923054702655, + "y": 1.5100655555725098, + "heading": 1.23879193875581e-18, + "angularVelocity": -5.058561441389415e-18, + "velocityX": 2.570504444531517, + "velocityY": 1.4085581304064316e-32, + "timestamp": 1.8657419307284382 + }, + { + "x": 7.073238710507852, + "y": 1.5100655555725098, + "heading": 8.254145878831398e-19, + "angularVelocity": -4.20967634225236e-18, + "velocityX": 2.1420870860493375, + "velocityY": 1.0374368292665007e-32, + "timestamp": 1.9639388744509876 + }, + { + "x": 7.241515837232644, + "y": 1.5100655555725098, + "heading": 4.95031593430639e-19, + "angularVelocity": -3.364493658617499e-18, + "velocityX": 1.7136696962814904, + "velocityY": 7.695161857259128e-33, + "timestamp": 2.062135818173537 + }, + { + "x": 7.367723683801346, + "y": 1.5100655555725098, + "heading": 2.474248388890608e-19, + "angularVelocity": -2.521532189828829e-18, + "velocityX": 1.2852522877422325, + "velocityY": 5.4450246921373414e-33, + "timestamp": 2.160332761896086 + }, + { + "x": 7.451862248985094, + "y": 1.5100655555725098, + "heading": 8.244895136962396e-20, + "angularVelocity": -1.6800511427648084e-18, + "velocityX": 0.8568348666886965, + "velocityY": 3.3251578108171654e-33, + "timestamp": 2.2585297056186353 + }, + { + "x": 7.493931531906128, + "y": 1.5100655555725098, + "heading": 4.9021992998797275e-30, + "angularVelocity": -8.396284877798725e-19, + "velocityX": 0.4284174366963883, + "velocityY": 1.5649544358297725e-33, + "timestamp": 2.3567266493411845 + }, + { + "x": 7.493931531906128, + "y": 1.5100655555725098, + "heading": 2.348585897046796e-30, + "angularVelocity": -2.0879214567555354e-30, + "velocityX": -3.9015027280463263e-23, + "velocityY": -1.2968680331891934e-40, + "timestamp": 2.4549235930637336 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/StraightForward.1.traj b/src/main/deploy/choreo/StraightForward.1.traj deleted file mode 100644 index 658adf9b..00000000 --- a/src/main/deploy/choreo/StraightForward.1.traj +++ /dev/null @@ -1,634 +0,0 @@ -{ - "samples": [ - { - "x": -1.1376170994794325e-18, - "y": -7.908497077335199e-28, - "heading": -7.532778409384681e-27, - "angularVelocity": -8.199365209469061e-27, - "velocityX": -6.394191701786484e-18, - "velocityY": 1.7031182398553385e-25, - "timestamp": 0 - }, - { - "x": 0.0033613448640062186, - "y": -3.207353843920882e-24, - "heading": 1.8540534582969372e-19, - "angularVelocity": 1.8765611031481612e-18, - "velocityX": 0.03402149614781158, - "velocityY": -3.300241746816345e-23, - "timestamp": 0.09880061856781212 - }, - { - "x": 0.010084034581535006, - "y": -9.734755015964764e-24, - "heading": 6.933840485443198e-19, - "angularVelocity": 5.141453424525524e-18, - "velocityX": 0.06804299218951397, - "velocityY": -6.718410081091478e-23, - "timestamp": 0.19760123713562425 - }, - { - "x": 0.02016806914144748, - "y": -1.9685611056691934e-23, - "heading": 1.6696757328056013e-18, - "angularVelocity": 9.881433945283818e-18, - "velocityX": 0.10206448811847535, - "velocityY": -1.0242470180298394e-22, - "timestamp": 0.29640185570343636 - }, - { - "x": 0.033613448531886116, - "y": -3.317040886768565e-23, - "heading": 2.0919453510145205e-18, - "angularVelocity": 4.27395869496419e-18, - "velocityX": 0.1360859839274221, - "velocityY": -1.388002418111676e-22, - "timestamp": 0.3952024742712485 - }, - { - "x": 0.050420172740202905, - "y": -5.030578939773999e-23, - "heading": 2.1256769428547757e-18, - "angularVelocity": 3.4141256618882077e-19, - "velocityX": 0.17010747960835326, - "velocityY": -1.7638794972644125e-22, - "timestamp": 0.49400309283906063 - }, - { - "x": 0.07058824175287753, - "y": -7.121786937435362e-23, - "heading": 1.9477652127659873e-18, - "angularVelocity": -1.800712610315598e-18, - "velocityX": 0.20412897515244016, - "velocityY": -2.1525580928579397e-22, - "timestamp": 0.5928037114068727 - }, - { - "x": 0.09411765555542399, - "y": -9.604275159196487e-23, - "heading": 1.7477397651697942e-18, - "angularVelocity": -2.0245340371444115e-18, - "velocityX": 0.23815047054990815, - "velocityY": -2.555345814319257e-22, - "timestamp": 0.6916043299746848 - }, - { - "x": 0.12100841413228318, - "y": -1.2492405496996858e-22, - "heading": 5.516927538791495e-19, - "angularVelocity": -1.210566046231088e-17, - "velocityX": 0.27217196578989666, - "velocityY": -2.972647097568963e-22, - "timestamp": 0.7904049485424969 - }, - { - "x": 0.1512605174666992, - "y": -1.5800976588220565e-22, - "heading": -2.4367433435029e-19, - "angularVelocity": -8.050220986716945e-18, - "velocityX": 0.30619346086029187, - "velocityY": -3.4057685817351433e-22, - "timestamp": 0.889205567110309 - }, - { - "x": 0.18487396554057559, - "y": -1.9547245627985932e-22, - "heading": -4.015998529188928e-19, - "angularVelocity": -1.598423665927227e-18, - "velocityX": 0.34021495574752625, - "velocityY": -3.856342021902462e-22, - "timestamp": 0.988006185678121 - }, - { - "x": 0.22184875833430798, - "y": -2.3748820340521924e-22, - "heading": -8.43069725371826e-19, - "angularVelocity": -4.468287727535479e-18, - "velocityX": 0.37423645043633647, - "velocityY": -4.325560385300792e-22, - "timestamp": 1.0868068042459331 - }, - { - "x": 0.2621848958265872, - "y": -2.842690964569586e-22, - "heading": -1.2892913052717086e-18, - "angularVelocity": -4.516381645740685e-18, - "velocityX": 0.4082579449094682, - "velocityY": -4.815024197737192e-22, - "timestamp": 1.1856074228137452 - }, - { - "x": 0.30588237799416684, - "y": -3.3601955996258784e-22, - "heading": -1.4361286105818133e-18, - "angularVelocity": -1.4861955425735956e-18, - "velocityX": 0.44227943914731443, - "velocityY": -5.325974076598438e-22, - "timestamp": 1.2844080413815573 - }, - { - "x": 0.35294120481158675, - "y": -3.9297468573778265e-22, - "heading": -2.1279601842953803e-18, - "angularVelocity": -7.002297360670298e-18, - "velocityX": 0.47630093312746685, - "velocityY": -5.861971021361979e-22, - "timestamp": 1.3832086599493694 - }, - { - "x": 0.4033613762508404, - "y": -4.553879204439828e-22, - "heading": -1.820906029131428e-18, - "angularVelocity": 3.10781837556822e-18, - "velocityX": 0.5103224268241561, - "velocityY": -6.423935446828857e-22, - "timestamp": 1.4820092785171814 - }, - { - "x": 0.4571428922809735, - "y": -5.235567689200076e-22, - "heading": -1.2874752262796756e-18, - "angularVelocity": 5.399065310541327e-18, - "velocityX": 0.5443439202075441, - "velocityY": -7.01517753563082e-22, - "timestamp": 1.5808098970849935 - }, - { - "x": 0.5142857528675923, - "y": -5.977742188885252e-22, - "heading": -1.2551861792254937e-18, - "angularVelocity": 3.268118073802405e-19, - "velocityX": 0.5783654132428195, - "velocityY": -7.638949232743287e-22, - "timestamp": 1.6796105156528056 - }, - { - "x": 0.5747899579722567, - "y": -6.783964811322378e-22, - "heading": -1.2211517140311946e-18, - "angularVelocity": 3.4447739303320164e-19, - "velocityX": 0.6123869058890279, - "velocityY": -8.298034991350254e-22, - "timestamp": 1.7784111342206177 - }, - { - "x": 0.6386555075517218, - "y": -7.658133530250519e-22, - "heading": -6.19635872521098e-19, - "angularVelocity": 6.0881793581750656e-18, - "velocityX": 0.6464083980975359, - "velocityY": -8.997961755349937e-22, - "timestamp": 1.8772117527884298 - }, - { - "x": 0.7058824015569765, - "y": -8.604791608615287e-22, - "heading": 1.304515627908251e-20, - "angularVelocity": 6.403613779661501e-18, - "velocityX": 0.6804298898099835, - "velocityY": -9.743947632554384e-22, - "timestamp": 1.9760123713562419 - }, - { - "x": 0.7764706399320078, - "y": -9.628899194740571e-22, - "heading": 1.4096290346844686e-18, - "angularVelocity": 1.4135374581608934e-17, - "velocityX": 0.714451380955505, - "velocityY": -1.0541932211126647e-21, - "timestamp": 2.074812989924054 - }, - { - "x": 0.8504202226121862, - "y": -1.0736383554782084e-21, - "heading": 2.060605710264541e-18, - "angularVelocity": 6.5887893423673715e-18, - "velocityX": 0.7484728714468809, - "velocityY": -1.1398796868710084e-21, - "timestamp": 2.1736136084918662 - }, - { - "x": 0.9277311495221099, - "y": -1.1933843266651194e-21, - "heading": 2.952299975389915e-18, - "angularVelocity": 9.025185671520631e-18, - "velocityX": 0.782494361175087, - "velocityY": -1.2326251827346421e-21, - "timestamp": 2.2724142270596785 - }, - { - "x": 1.0084034205726677, - "y": -1.3228608726632435e-21, - "heading": 4.072843680930929e-18, - "angularVelocity": 1.134145955927035e-17, - "velocityX": 0.8165158500013651, - "velocityY": -1.3332101260360877e-21, - "timestamp": 2.371214845627491 - }, - { - "x": 1.0924370356569266, - "y": -1.463114094622031e-21, - "heading": 5.6434310876510846e-18, - "angularVelocity": 1.5896527419660045e-17, - "velocityX": 0.8505373377453298, - "velocityY": -1.4438898247438702e-21, - "timestamp": 2.470015464195303 - }, - { - "x": 1.17983199464419, - "y": -1.6151196326348776e-21, - "heading": 7.018518492787492e-18, - "angularVelocity": 1.3917792832553574e-17, - "velocityX": 0.8845588241664668, - "velocityY": -1.5652186056968467e-21, - "timestamp": 2.5688160827631155 - }, - { - "x": 1.270588297371088, - "y": -1.7802259481782284e-21, - "heading": 7.979758659218145e-18, - "angularVelocity": 9.729079426406841e-18, - "velocityX": 0.9185803089340697, - "velocityY": -1.7002423732627545e-21, - "timestamp": 2.6676167013309278 - }, - { - "x": 1.3647059436275732, - "y": -1.960029507975355e-21, - "heading": 8.919068700069961e-18, - "angularVelocity": 9.507112968894936e-18, - "velocityX": 0.9526017915757001, - "velocityY": -1.851885594364366e-21, - "timestamp": 2.76641731989874 - }, - { - "x": 1.4621849331335823, - "y": -2.156646942783595e-21, - "heading": 9.966114158544211e-18, - "angularVelocity": 1.0597541833276519e-17, - "velocityX": 0.9866232713827009, - "velocityY": -2.0254826724633154e-21, - "timestamp": 2.8652179384665524 - }, - { - "x": 1.5630252654970274, - "y": -2.372489218605291e-21, - "heading": 1.1537140907678108e-17, - "angularVelocity": 1.5900958141426103e-17, - "velocityX": 1.0206447472212212, - "velocityY": -2.2237643073670865e-21, - "timestamp": 2.9640185570343647 - }, - { - "x": 1.6672269401297757, - "y": -2.6101775109831374e-21, - "heading": 1.3077485198475941e-17, - "angularVelocity": 1.5590402638929695e-17, - "velocityX": 1.054666217107023, - "velocityY": -2.4507675580642354e-21, - "timestamp": 3.062819175602177 - }, - { - "x": 1.7747899560516078, - "y": -2.8712245936259582e-21, - "heading": 1.4443190701622367e-17, - "angularVelocity": 1.3822805901452837e-17, - "velocityX": 1.0886876770716345, - "velocityY": -2.6936515218666954e-21, - "timestamp": 3.1616197941699893 - }, - { - "x": 1.8857143113020887, - "y": -3.1425844237227276e-21, - "heading": 1.6507544936655253e-17, - "angularVelocity": 2.0894091122735124e-17, - "velocityX": 1.122709117193911, - "velocityY": -2.8091008420292473e-21, - "timestamp": 3.2604204127378016 - }, - { - "x": 2, - "y": -3.164655249070787e-21, - "heading": 1.8228167907282414e-17, - "angularVelocity": 1.7415022670654974e-17, - "velocityX": 1.156730497790058, - "velocityY": -3.0799511941989887e-22, - "timestamp": 3.359221031305614 - }, - { - "x": 2.1142856886979113, - "y": -3.147499024006811e-21, - "heading": 1.9005172299965157e-17, - "angularVelocity": 7.864349059131218e-18, - "velocityX": 1.156730497790058, - "velocityY": 2.5626451006413984e-22, - "timestamp": 3.458021649873426 - }, - { - "x": 2.225210043948392, - "y": -2.8708197340377503e-21, - "heading": 1.976235400694375e-17, - "angularVelocity": 7.66372977455926e-18, - "velocityX": 1.122709117193911, - "velocityY": 2.8612962139149132e-21, - "timestamp": 3.5568222684412385 - }, - { - "x": 2.3327730598702243, - "y": -2.607451498389937e-21, - "heading": 2.027561255801402e-17, - "angularVelocity": 5.194893740089378e-18, - "velocityX": 1.0886876770716345, - "velocityY": 2.7166086454579756e-21, - "timestamp": 3.655622887009051 - }, - { - "x": 2.4369747345029724, - "y": -2.368709002669498e-21, - "heading": 2.0492359746230776e-17, - "angularVelocity": 2.1937892093060286e-18, - "velocityX": 1.054666217107023, - "velocityY": 2.4606042161289694e-21, - "timestamp": 3.754423505576863 - }, - { - "x": 2.5378150668664174, - "y": -2.1523875052863635e-21, - "heading": 2.0979083463355325e-17, - "angularVelocity": 4.926330516473677e-18, - "velocityX": 1.0206447472212212, - "velocityY": 2.228247008672144e-21, - "timestamp": 3.8532241241446754 - }, - { - "x": 2.6352940563724263, - "y": -1.9555762507063697e-21, - "heading": 2.089955236742629e-17, - "angularVelocity": -8.049557622963381e-19, - "velocityX": 0.9866232713827009, - "velocityY": 2.0274855500697654e-21, - "timestamp": 3.9520247427124877 - }, - { - "x": 2.7294117026289118, - "y": -1.7756973118970903e-21, - "heading": 2.048181439249147e-17, - "angularVelocity": -4.228079574039095e-18, - "velocityX": 0.9526017915757001, - "velocityY": 1.8530533449651495e-21, - "timestamp": 4.0508253612803 - }, - { - "x": 2.82016800535581, - "y": -1.610696178925322e-21, - "heading": 2.0393670363820926e-17, - "angularVelocity": -8.921285320598318e-19, - "velocityX": 0.9185803089340697, - "velocityY": 1.6996791445230123e-21, - "timestamp": 4.149625979848111 - }, - { - "x": 2.907562964343073, - "y": -1.458877869554151e-21, - "heading": 2.084174445721108e-17, - "angularVelocity": 4.53514691585319e-18, - "velocityX": 0.8845588241664668, - "velocityY": 1.5641372351679697e-21, - "timestamp": 4.248426598415923 - }, - { - "x": 2.991596579427332, - "y": -1.3188231303239282e-21, - "heading": 2.0547699398998952e-17, - "angularVelocity": -2.9761329639527713e-18, - "velocityX": 0.8505373377453298, - "velocityY": 1.4429357708430696e-21, - "timestamp": 4.347227216983735 - }, - { - "x": 3.0722688504778892, - "y": -1.1894052177930912e-21, - "heading": 2.0372841992969962e-17, - "angularVelocity": -1.769787561100399e-18, - "velocityX": 0.8165158500013651, - "velocityY": 1.3327558109827944e-21, - "timestamp": 4.446027835551547 - }, - { - "x": 3.149579777387813, - "y": -1.0697504752144254e-21, - "heading": 1.984446413733655e-17, - "angularVelocity": -5.347907206549522e-18, - "velocityX": 0.782494361175087, - "velocityY": 1.2321009132712108e-21, - "timestamp": 4.544828454119359 - }, - { - "x": 3.2235293600679915, - "y": -9.590738101698616e-22, - "heading": 1.9549867930164684e-17, - "angularVelocity": -2.981711079527443e-18, - "velocityX": 0.7484728714468809, - "velocityY": 1.139418959561847e-21, - "timestamp": 4.643629072687171 - }, - { - "x": 3.2941175984430227, - "y": -8.567435506711912e-22, - "heading": 1.880855841586028e-17, - "angularVelocity": -7.503072559944813e-18, - "velocityX": 0.714451380955505, - "velocityY": 1.053479684788194e-21, - "timestamp": 4.742429691254983 - }, - { - "x": 3.361344492448277, - "y": -7.621603632941542e-22, - "heading": 1.804651402975059e-17, - "angularVelocity": -7.71293857627177e-18, - "velocityX": 0.6804298898099835, - "velocityY": 9.736267688058442e-22, - "timestamp": 4.8412303098227945 - }, - { - "x": 3.4252100420277425, - "y": -6.74848663914946e-22, - "heading": 1.7632923835461265e-17, - "angularVelocity": -4.186096719140597e-18, - "velocityX": 0.6464083980975359, - "velocityY": 8.98840146425094e-22, - "timestamp": 4.940030928390606 - }, - { - "x": 3.4857142471324067, - "y": -5.943339267652739e-22, - "heading": 1.6713354403567037e-17, - "angularVelocity": -9.30731224773011e-18, - "velocityX": 0.6123869058890279, - "velocityY": 8.286419249206994e-22, - "timestamp": 5.038831546958418 - }, - { - "x": 3.5428571077190254, - "y": -5.202415794373983e-22, - "heading": 1.5572837882186598e-17, - "angularVelocity": -1.1543605179767327e-17, - "velocityX": 0.5783654132428195, - "velocityY": 7.624593234334322e-22, - "timestamp": 5.13763216552623 - }, - { - "x": 3.596638623749159, - "y": -4.522125741089791e-22, - "heading": 1.4464732933422248e-17, - "angularVelocity": -1.1215555338005426e-17, - "velocityX": 0.5443439202075441, - "velocityY": 6.999916566838223e-22, - "timestamp": 5.236432784094042 - }, - { - "x": 3.6470587951884124, - "y": -3.899342272220007e-22, - "heading": 1.3615733384952931e-17, - "angularVelocity": -8.593048010242285e-18, - "velocityX": 0.5103224268241561, - "velocityY": 6.4066863404930395e-22, - "timestamp": 5.335233402661854 - }, - { - "x": 3.694117622005832, - "y": -3.3313328500098465e-22, - "heading": 1.2052388332633744e-17, - "angularVelocity": -1.5823220790511242e-17, - "velocityX": 0.4763009331274668, - "velocityY": 5.842312496177269e-22, - "timestamp": 5.434034021229666 - }, - { - "x": 3.737815104173412, - "y": -2.8156433257179758e-22, - "heading": 1.1136769768708063e-17, - "angularVelocity": -9.267326418690942e-18, - "velocityX": 0.4422794391473144, - "velocityY": 5.304122695590917e-22, - "timestamp": 5.5328346397974775 - }, - { - "x": 3.7781512416656913, - "y": -2.3498113666642085e-22, - "heading": 9.859209548379765e-18, - "angularVelocity": -1.2930681107294836e-17, - "velocityX": 0.40825794490946815, - "velocityY": 4.790511156155067e-22, - "timestamp": 5.631635258365289 - }, - { - "x": 3.815126034459424, - "y": -1.9316819160355648e-22, - "heading": 8.372929242622771e-18, - "angularVelocity": -1.50432199482529e-17, - "velocityX": 0.3742364504363364, - "velocityY": 4.29918013017057e-22, - "timestamp": 5.730435876933101 - }, - { - "x": 3.8487394825333006, - "y": -1.5593072898156008e-22, - "heading": 6.818382919293722e-18, - "angularVelocity": -1.5734167867569838e-17, - "velocityX": 0.3402149557475262, - "velocityY": 3.8283016928136736e-22, - "timestamp": 5.829236495500913 - }, - { - "x": 3.8789915858677166, - "y": -1.2308917074135604e-22, - "heading": 5.324791099236139e-18, - "angularVelocity": -1.5117224017561152e-17, - "velocityX": 0.3061934608602918, - "velocityY": 3.37574987627076e-22, - "timestamp": 5.928037114068725 - }, - { - "x": 3.905882344444576, - "y": -9.448393998536425e-23, - "heading": 4.011436152983029e-18, - "angularVelocity": -1.3292976330431885e-17, - "velocityX": 0.2721719657898966, - "velocityY": 2.9401710775658265e-22, - "timestamp": 6.026837732636537 - }, - { - "x": 3.9294117582471224, - "y": -6.995384807456761e-23, - "heading": 2.9887621240859383e-18, - "angularVelocity": -1.0350880993468448e-17, - "velocityX": 0.23815047054990812, - "velocityY": 2.5206100605913985e-22, - "timestamp": 6.125638351204349 - }, - { - "x": 3.949579827259797, - "y": -4.935913599830647e-23, - "heading": 2.359326118608253e-18, - "angularVelocity": -6.3707648062754864e-18, - "velocityX": 0.20412897515244013, - "velocityY": 2.1159029061948054e-22, - "timestamp": 6.2244389697721605 - }, - { - "x": 3.966386551468114, - "y": -3.25642428787695e-23, - "heading": 2.218613212934694e-18, - "angularVelocity": -1.4242065657400621e-18, - "velocityX": 0.17010747960835323, - "velocityY": 1.725072970802276e-22, - "timestamp": 6.323239588339972 - }, - { - "x": 3.979831930858553, - "y": -1.944612833644722e-23, - "heading": 1.4782676158808526e-18, - "angularVelocity": -7.493326036397043e-18, - "velocityX": 0.1360859839274221, - "velocityY": 1.3470636183720975e-22, - "timestamp": 6.422040206907784 - }, - { - "x": 3.989915965418465, - "y": -9.887317530316752e-24, - "heading": 2.216610663986935e-19, - "angularVelocity": -1.2718607311216777e-17, - "velocityX": 0.10206448811847536, - "velocityY": 9.811604675618584e-23, - "timestamp": 6.520840825475596 - }, - { - "x": 3.996638655135994, - "y": -3.7771668643641844e-24, - "heading": -2.955699978047428e-19, - "angularVelocity": -5.2350975881402005e-18, - "velocityX": 0.06804299218951398, - "velocityY": 6.266873493281363e-23, - "timestamp": 6.619641444043408 - }, - { - "x": 4, - "y": -1.0128885732606577e-24, - "heading": 1.6809468171434698e-26, - "angularVelocity": 2.991581476965099e-18, - "velocityX": 0.03402149614781159, - "velocityY": 2.828535780318287e-23, - "timestamp": 6.71844206261122 - }, - { - "x": 4, - "y": -1.4975010105831042e-24, - "heading": 7.569049534824136e-27, - "angularVelocity": -1.729958968573892e-26, - "velocityX": -1.9187470113508293e-19, - "velocityY": -5.092260525072701e-24, - "timestamp": 6.817242681179032 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/StraightForward.2.traj b/src/main/deploy/choreo/StraightForward.2.traj deleted file mode 100644 index ccbea4c4..00000000 --- a/src/main/deploy/choreo/StraightForward.2.traj +++ /dev/null @@ -1,634 +0,0 @@ -{ - "samples": [ - { - "x": 4, - "y": -1.4975010105831042e-24, - "heading": 7.569049534824136e-27, - "angularVelocity": -1.729958968573892e-26, - "velocityX": -1.9187470113508293e-19, - "velocityY": -5.092260525072701e-24, - "timestamp": 0 - }, - { - "x": 3.9966386551359934, - "y": 1.622382271786415e-20, - "heading": 3.3801616172272874e-20, - "angularVelocity": 3.421193955466234e-19, - "velocityX": -0.034021496147811595, - "velocityY": 1.6425392695916164e-19, - "timestamp": 0.09880061856781186 - }, - { - "x": 3.989915965418465, - "y": 4.867644146454126e-20, - "heading": 1.0140943353587003e-19, - "angularVelocity": 6.842853514451412e-19, - "velocityX": -0.06804299218951398, - "velocityY": 3.285129050412422e-19, - "timestamp": 0.19760123713562372 - }, - { - "x": 3.9798319308585524, - "y": 9.735635325747916e-20, - "heading": 2.0282865518075705e-19, - "angularVelocity": 1.0265038932822472e-18, - "velocityX": -0.10206448811847536, - "velocityY": 4.927718248471109e-19, - "timestamp": 0.2964018557034356 - }, - { - "x": 3.966386551468114, - "y": 1.6226355072631345e-19, - "heading": 3.3806475714931607e-19, - "angularVelocity": 1.368777860282825e-18, - "velocityX": -0.1360859839274221, - "velocityY": 6.570306670773842e-19, - "timestamp": 0.39520247427124744 - }, - { - "x": 3.949579827259797, - "y": 2.4339802438242795e-19, - "heading": 5.071235363962782e-19, - "angularVelocity": 1.7111105005542439e-18, - "velocityX": -0.17010747960835323, - "velocityY": 8.212894100973546e-19, - "timestamp": 0.4940030928390593 - }, - { - "x": 3.9294117582471224, - "y": 3.4075976233216677e-19, - "heading": 7.100112314845174e-19, - "angularVelocity": 2.0535062837733624e-18, - "velocityX": -0.2041289751524401, - "velocityY": 9.855480293458379e-19, - "timestamp": 0.5928037114068712 - }, - { - "x": 3.9058823444445765, - "y": 4.543487499464003e-19, - "heading": 9.467344638444388e-19, - "angularVelocity": 2.3959690850953575e-18, - "velocityX": -0.23815047054990812, - "velocityY": 1.1498064968884804e-18, - "timestamp": 0.691604329974683 - }, - { - "x": 3.8789915858677175, - "y": 5.84164969472011e-19, - "heading": 1.2173004091506322e-18, - "angularVelocity": 2.7385045142480053e-18, - "velocityX": -0.2721719657898966, - "velocityY": 1.3140647808442577e-18, - "timestamp": 0.7904049485424949 - }, - { - "x": 3.8487394825333014, - "y": 7.302083995695225e-19, - "heading": 1.5217167130383014e-18, - "angularVelocity": 3.0811173278628614e-18, - "velocityX": -0.30619346086029176, - "velocityY": 1.478322844607657e-18, - "timestamp": 0.8892055671103067 - }, - { - "x": 3.815126034459425, - "y": 8.9247901475715e-19, - "heading": 1.859991687922629e-18, - "angularVelocity": 3.42381427473618e-18, - "velocityX": -0.3402149557475262, - "velocityY": 1.6425806459233442e-18, - "timestamp": 0.9880061856781186 - }, - { - "x": 3.7781512416656926, - "y": 1.0709767847438179e-18, - "heading": 2.232134281072569e-18, - "angularVelocity": 3.7666017804132156e-18, - "velocityX": -0.3742364504363364, - "velocityY": 1.8068381357194375e-18, - "timestamp": 1.0868068042459305 - }, - { - "x": 3.7378151041734133, - "y": 1.2657016736040118e-18, - "heading": 2.638154283334348e-18, - "angularVelocity": 4.1094883831216016e-18, - "velocityX": -0.40825794490946815, - "velocityY": 1.9710952565763296e-18, - "timestamp": 1.1856074228137423 - }, - { - "x": 3.694117622005834, - "y": 1.476653638799985e-18, - "heading": 3.078062355478974e-18, - "angularVelocity": 4.452482887685102e-18, - "velocityX": -0.4422794391473143, - "velocityY": 2.135351941018014e-18, - "timestamp": 1.284408041381555 - }, - { - "x": 3.6470587951884137, - "y": 1.7038326299290477e-18, - "heading": 3.551870199581173e-18, - "angularVelocity": 4.795595833539573e-18, - "velocityX": -0.47630093312746674, - "velocityY": 2.299608108984575e-18, - "timestamp": 1.3832086599493678 - }, - { - "x": 3.5966386237491603, - "y": 1.9472385872002766e-18, - "heading": 4.059590654442284e-18, - "angularVelocity": 5.138838726011681e-18, - "velocityX": -0.5103224268241561, - "velocityY": 2.4638636646972548e-18, - "timestamp": 1.4820092785171806 - }, - { - "x": 3.542857107719027, - "y": 2.206871439463151e-18, - "heading": 3.423761751446479e-18, - "angularVelocity": -6.435474798217317e-18, - "velocityX": -0.5443439202075441, - "velocityY": 2.62811849254691e-18, - "timestamp": 1.5808098970849933 - }, - { - "x": 3.4857142471324085, - "y": 2.4827311017573516e-18, - "heading": 2.8218754348898888e-18, - "angularVelocity": -6.091928510129713e-18, - "velocityX": -0.5783654132428195, - "velocityY": 2.7923724518212657e-18, - "timestamp": 1.679610515652806 - }, - { - "x": 3.425210042027744, - "y": 2.7748174720594974e-18, - "heading": 2.2539492010975555e-18, - "angularVelocity": -5.748205134856798e-18, - "velocityX": -0.6123869058890279, - "velocityY": 2.956625368836816e-18, - "timestamp": 1.7784111342206188 - }, - { - "x": 3.3613444924482785, - "y": 3.0831304270690512e-18, - "heading": 1.7200028141341935e-18, - "angularVelocity": -5.4042817196730335e-18, - "velocityX": -0.6464083980975359, - "velocityY": 3.1208770270543982e-18, - "timestamp": 1.8772117527884316 - }, - { - "x": 3.2941175984430235, - "y": 3.407669816567964e-18, - "heading": 1.2200587363363886e-18, - "angularVelocity": -5.0601309543877636e-18, - "velocityX": -0.6804298898099835, - "velocityY": 3.2851271525026197e-18, - "timestamp": 1.9760123713562443 - }, - { - "x": 3.2235293600679924, - "y": 3.748435455691416e-18, - "heading": 7.541425169569603e-19, - "angularVelocity": -4.715721595037053e-18, - "velocityX": -0.714451380955505, - "velocityY": 3.4493753928478014e-18, - "timestamp": 2.074812989924057 - }, - { - "x": 3.149579777387814, - "y": 4.105427114281349e-18, - "heading": 3.2228393306074877e-19, - "angularVelocity": -4.371010850288281e-18, - "velocityX": -0.7484728714468808, - "velocityY": 3.61362128746775e-18, - "timestamp": 2.17361360849187 - }, - { - "x": 3.0722688504778906, - "y": 4.478644501633054e-18, - "heading": -7.548243860660323e-20, - "angularVelocity": -4.025950140734063e-18, - "velocityX": -0.782494361175087, - "velocityY": 3.7778642208016786e-18, - "timestamp": 2.2724142270596825 - }, - { - "x": 2.9915965794273327, - "y": 4.868087244242979e-18, - "heading": -4.391158104077786e-19, - "angularVelocity": -3.680476603421153e-18, - "velocityX": -0.8165158500013651, - "velocityY": 3.942103351400059e-18, - "timestamp": 2.3712148456274953 - }, - { - "x": 2.907562964343074, - "y": 5.2737548521275295e-18, - "heading": -7.685671750620224e-19, - "angularVelocity": -3.334507044128586e-18, - "velocityX": -0.8505373377453298, - "velocityY": 4.1063374959021146e-18, - "timestamp": 2.470015464195308 - }, - { - "x": 2.8201680053558102, - "y": 5.695646665833697e-18, - "heading": -1.0637766148592117e-18, - "angularVelocity": -2.9879310340772823e-18, - "velocityX": -0.8845588241664668, - "velocityY": 4.270564932988105e-18, - "timestamp": 2.568816082763121 - }, - { - "x": 2.729411702628912, - "y": 6.1337617692699354e-18, - "heading": -1.3246696022153147e-18, - "angularVelocity": -2.6406007010458607e-18, - "velocityX": -0.9185803089340697, - "velocityY": 4.434783057031529e-18, - "timestamp": 2.6676167013309335 - }, - { - "x": 2.6352940563724268, - "y": 6.588098837620084e-18, - "heading": -1.5511502459600477e-18, - "angularVelocity": -2.2922998157391772e-18, - "velocityX": -0.9526017915757001, - "velocityY": 4.59898771937502e-18, - "timestamp": 2.7664173198987463 - }, - { - "x": 2.5378150668664174, - "y": 7.058655852182776e-18, - "heading": -1.7430903085127116e-18, - "angularVelocity": -1.9427009874027487e-18, - "velocityX": -0.9866232713827009, - "velocityY": 4.763171879327714e-18, - "timestamp": 2.865217938466559 - }, - { - "x": 2.4369747345029724, - "y": 7.545429512244077e-18, - "heading": -1.9003111994243256e-18, - "angularVelocity": -1.591294574936401e-18, - "velocityX": -1.0206447472212212, - "velocityY": 4.927322533296983e-18, - "timestamp": 2.964018557034372 - }, - { - "x": 2.3327730598702243, - "y": 8.04841385541656e-18, - "heading": -2.022543272012013e-18, - "angularVelocity": -1.2371589623518452e-18, - "velocityX": -1.054666217107023, - "velocityY": 5.091412692060407e-18, - "timestamp": 3.0628191756021845 - }, - { - "x": 2.225210043948392, - "y": 8.567596345936824e-18, - "heading": -2.1093375884193087e-18, - "angularVelocity": -8.784794728630001e-19, - "velocityX": -1.0886876770716345, - "velocityY": 5.255375725743804e-18, - "timestamp": 3.1616197941699973 - }, - { - "x": 2.1142856886979113, - "y": 9.102942798044225e-18, - "heading": -2.159797685075532e-18, - "angularVelocity": -5.107265229524464e-19, - "velocityX": -1.122709117193911, - "velocityY": 5.418992260996229e-18, - "timestamp": 3.26042041273781 - }, - { - "x": 2, - "y": 9.654290125645511e-18, - "heading": -2.171249723825065e-18, - "angularVelocity": -1.1591060473957685e-19, - "velocityX": -1.156730497790058, - "velocityY": 5.580956667058148e-18, - "timestamp": 3.3592210313056228 - }, - { - "x": 1.8857143113020887, - "y": 9.61185739696908e-18, - "heading": -2.2270648319021466e-18, - "angularVelocity": -5.649266874918011e-19, - "velocityX": -1.1567304977900583, - "velocityY": -4.2949032487006095e-19, - "timestamp": 3.4580216498734355 - }, - { - "x": 1.7747899560516078, - "y": 9.553513369992968e-18, - "heading": -2.3111613029527945e-18, - "angularVelocity": -8.511735047427319e-19, - "velocityX": -1.1227091171939112, - "velocityY": -5.905538912325038e-19, - "timestamp": 3.5568222684412483 - }, - { - "x": 1.6672269401297757, - "y": 9.479013543281256e-18, - "heading": -2.4262130872250256e-18, - "angularVelocity": -1.1644844250684416e-18, - "velocityX": -1.0886876770716345, - "velocityY": -7.540907241750166e-19, - "timestamp": 3.655622887009061 - }, - { - "x": 1.5630252654970274, - "y": 9.388316488623372e-18, - "heading": -2.5731140577432874e-18, - "angularVelocity": -1.4868425859374024e-18, - "velocityX": -1.054666217107023, - "velocityY": -9.180463912283675e-19, - "timestamp": 3.7544235055768738 - }, - { - "x": 1.4621849331335826, - "y": 9.281408255613739e-18, - "heading": -2.7523119274054557e-18, - "angularVelocity": -1.8137322254046327e-18, - "velocityX": -1.0206447472212212, - "velocityY": -1.082143017049038e-18, - "timestamp": 3.8532241241446865 - }, - { - "x": 1.3647059436275732, - "y": 9.158282543727594e-18, - "heading": -2.964075868359904e-18, - "angularVelocity": -2.143346262102346e-18, - "velocityX": -0.986623271382701, - "velocityY": -1.246303269806804e-18, - "timestamp": 3.9520247427124993 - }, - { - "x": 1.2705882973710878, - "y": 9.018935994247637e-18, - "heading": -3.2085845705889515e-18, - "angularVelocity": -2.4747688902482226e-18, - "velocityX": -0.9526017915757001, - "velocityY": -1.4104974218850865e-18, - "timestamp": 4.050825361280312 - }, - { - "x": 1.17983199464419, - "y": 8.86336661456802e-18, - "heading": -3.485966591694146e-18, - "angularVelocity": -2.8074927005328834e-18, - "velocityX": -0.9185803089340697, - "velocityY": -1.5747116736319965e-18, - "timestamp": 4.149625979848125 - }, - { - "x": 1.0924370356569264, - "y": 8.69157313096237e-18, - "heading": -3.796317507759278e-18, - "angularVelocity": -3.141183873969326e-18, - "velocityX": -0.8845588241664669, - "velocityY": -1.7389387660362588e-18, - "timestamp": 4.2484265984159375 - }, - { - "x": 1.0084034205726677, - "y": 8.503554683319867e-18, - "heading": -4.139711774475018e-18, - "angularVelocity": -3.475628642739219e-18, - "velocityX": -0.8505373377453298, - "velocityY": -1.903174524206938e-18, - "timestamp": 4.34722721698375 - }, - { - "x": 0.9277311495221099, - "y": 8.299310665997281e-18, - "heading": -4.516208836106112e-18, - "angularVelocity": -3.810675070331667e-18, - "velocityX": -0.8165158500013652, - "velocityY": -2.067416380452275e-18, - "timestamp": 4.446027835551563 - }, - { - "x": 0.8504202226121863, - "y": 8.0788406383874e-18, - "heading": -4.9258577031240064e-18, - "angularVelocity": -4.1462175521934945e-18, - "velocityX": -0.782494361175087, - "velocityY": -2.2316626702442053e-18, - "timestamp": 4.544828454119376 - }, - { - "x": 0.7764706399320079, - "y": 7.842144271446874e-18, - "heading": -5.368698751428868e-18, - "angularVelocity": -4.482168694424708e-18, - "velocityX": -0.7484728714468809, - "velocityY": -2.395912268619746e-18, - "timestamp": 4.6436290726871885 - }, - { - "x": 0.7058824015569765, - "y": 7.589221314273757e-18, - "heading": -5.844766537688045e-18, - "angularVelocity": -4.8184695983098995e-18, - "velocityX": -0.714451380955505, - "velocityY": -2.5601643880906105e-18, - "timestamp": 4.742429691255001 - }, - { - "x": 0.6386555075517218, - "y": 7.320071572246599e-18, - "heading": -6.3540907090448196e-18, - "angularVelocity": -5.155070572582827e-18, - "velocityX": -0.6804298898099835, - "velocityY": -2.7244184616393568e-18, - "timestamp": 4.841230309822814 - }, - { - "x": 0.5747899579722568, - "y": 7.034694892326384e-18, - "heading": -5.719220693211024e-18, - "angularVelocity": 6.4257695231037726e-18, - "velocityX": -0.6464083980975359, - "velocityY": -2.888674070537039e-18, - "timestamp": 4.940030928390627 - }, - { - "x": 0.5142857528675924, - "y": 6.733091152832895e-18, - "heading": -5.1176550171087195e-18, - "angularVelocity": 6.0886831811172465e-18, - "velocityX": -0.6123869058890279, - "velocityY": -3.0529308990878334e-18, - "timestamp": 5.0388315469584395 - }, - { - "x": 0.4571428922809735, - "y": 6.415260256155639e-18, - "heading": -4.549413483741888e-18, - "angularVelocity": 5.751396405133998e-18, - "velocityX": -0.5783654132428195, - "velocityY": -3.2171887049931132e-18, - "timestamp": 5.137632165526252 - }, - { - "x": 0.4033613762508404, - "y": 6.081202123490567e-18, - "heading": -4.014513636196958e-18, - "angularVelocity": 5.413932068682268e-18, - "velocityX": -0.5443439202075441, - "velocityY": -3.381447299080878e-18, - "timestamp": 5.236432784094065 - }, - { - "x": 0.35294120481158675, - "y": 5.730916690946256e-18, - "heading": -3.512971170353614e-18, - "angularVelocity": 5.076308868051237e-18, - "velocityX": -0.5103224268241561, - "velocityY": -3.545706531606395e-18, - "timestamp": 5.335233402661878 - }, - { - "x": 0.3058823779941669, - "y": 5.364403906543085e-18, - "heading": -3.0448003053157668e-18, - "angularVelocity": 4.738541750260109e-18, - "velocityX": -0.47630093312746674, - "velocityY": -3.709966282517656e-18, - "timestamp": 5.4340340212296905 - }, - { - "x": 0.2621848958265872, - "y": 4.981663728010729e-18, - "heading": -2.6100139125080014e-18, - "angularVelocity": 4.400644355727739e-18, - "velocityX": -0.4422794391473143, - "velocityY": -3.874226454538646e-18, - "timestamp": 5.532834639797503 - }, - { - "x": 0.22184875833430798, - "y": 4.582696121005342e-18, - "heading": -2.208623621378623e-18, - "angularVelocity": 4.062629254983044e-18, - "velocityX": -0.4082579449094682, - "velocityY": -4.038486967950237e-18, - "timestamp": 5.631635258365316 - }, - { - "x": 0.18487396554057556, - "y": 4.167501057752707e-18, - "heading": -1.8406400335009087e-18, - "angularVelocity": 3.724506851511534e-18, - "velocityX": -0.3742364504363364, - "velocityY": -4.202747756784724e-18, - "timestamp": 5.730435876933129 - }, - { - "x": 0.15126051746669916, - "y": 3.736078515959616e-18, - "heading": -1.506072956037892e-18, - "angularVelocity": 3.3862851859073145e-18, - "velocityX": -0.3402149557475262, - "velocityY": -4.367008766061658e-18, - "timestamp": 5.8292364955009415 - }, - { - "x": 0.12100841413228317, - "y": 3.2884284779303413e-18, - "heading": -1.2049313957041987e-18, - "angularVelocity": 3.0479723597881684e-18, - "velocityX": -0.3061934608602918, - "velocityY": -4.531269949657919e-18, - "timestamp": 5.928037114068754 - }, - { - "x": 0.09411765555542398, - "y": 2.824550929875594e-18, - "heading": -9.372236288478572e-19, - "angularVelocity": 2.7095757655060014e-18, - "velocityX": -0.2721719657898966, - "velocityY": -4.695531268401831e-18, - "timestamp": 6.026837732636567 - }, - { - "x": 0.07058824175287753, - "y": 2.3444458613366558e-18, - "heading": -7.029573657694057e-19, - "angularVelocity": 2.371101132177252e-18, - "velocityX": -0.23815047054990812, - "velocityY": -4.859792688975063e-18, - "timestamp": 6.12563835120438 - }, - { - "x": 0.050420172740202905, - "y": 1.8481132646899755e-18, - "heading": -5.021397823279256e-19, - "angularVelocity": 2.032553869215878e-18, - "velocityX": -0.2041289751524401, - "velocityY": -5.024054182861873e-18, - "timestamp": 6.2244389697721925 - }, - { - "x": 0.03361344853188612, - "y": 1.3355531347764025e-18, - "heading": -3.3477751504246894e-19, - "angularVelocity": 1.6939394354207508e-18, - "velocityX": -0.17010747960835323, - "velocityY": -5.188315725440333e-18, - "timestamp": 6.323239588340005 - }, - { - "x": 0.020168069141447482, - "y": 8.067654685706554e-19, - "heading": -2.008767863019259e-19, - "angularVelocity": 1.355262022365565e-18, - "velocityX": -0.1360859839274221, - "velocityY": -5.352577295354402e-18, - "timestamp": 6.422040206907818 - }, - { - "x": 0.010084034581535006, - "y": 2.6175026490285327e-19, - "heading": -1.0044337813369068e-19, - "angularVelocity": 1.016526087149893e-18, - "velocityX": -0.10206448811847536, - "velocityY": -5.516838874018515e-18, - "timestamp": 6.520840825475631 - }, - { - "x": 0.0033613448640062178, - "y": -2.9949247577400035e-19, - "heading": -3.3482745638057493e-20, - "angularVelocity": 6.777349387241571e-19, - "velocityX": -0.06804299218951398, - "velocityY": -5.6811004451979164e-18, - "timestamp": 6.6196414440434435 - }, - { - "x": -2.9258798480024608e-18, - "y": -8.769627515656533e-19, - "heading": -1.2196216569957611e-26, - "angularVelocity": 3.388919345415541e-19, - "velocityX": -0.03402149614781159, - "velocityY": -5.845361994494316e-18, - "timestamp": 6.718442062611256 - }, - { - "x": -1.1376141000811762e-18, - "y": -1.4706605593137037e-18, - "heading": -9.263715163460755e-27, - "angularVelocity": 2.929800407564323e-26, - "velocityX": 6.5860729270801154e-18, - "velocityY": -6.0096235092764735e-18, - "timestamp": 6.817242681179069 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/StraightForward.3.traj b/src/main/deploy/choreo/StraightForward.3.traj deleted file mode 100644 index 79704ecd..00000000 --- a/src/main/deploy/choreo/StraightForward.3.traj +++ /dev/null @@ -1,454 +0,0 @@ -{ - "samples": [ - { - "x": -1.1376141000811762e-18, - "y": -1.4706605593137037e-18, - "heading": -9.263715163460755e-27, - "angularVelocity": 2.929800407564323e-26, - "velocityX": 6.5860729270801154e-18, - "velocityY": -6.0096235092764735e-18, - "timestamp": 0 - }, - { - "x": -5.135794547435122e-19, - "y": 0.0033333336521360404, - "heading": -2.1647079011876355e-19, - "angularVelocity": -2.2001741500814918e-18, - "velocityX": 6.3431154311892524e-18, - "velocityY": 0.03387944331015331, - "timestamp": 0.09838808806923538 - }, - { - "x": 8.655817806035818e-20, - "y": 0.010000000941341826, - "heading": -5.697678805519529e-19, - "angularVelocity": -3.590855508881197e-18, - "velocityX": 6.100208278091576e-18, - "velocityY": 0.06775888646717527, - "timestamp": 0.19677617613847076 - }, - { - "x": 6.628042064573855e-19, - "y": 0.020000001851181393, - "heading": -9.256562256435773e-19, - "angularVelocity": -3.617194481532914e-18, - "velocityX": 5.857356086283696e-18, - "velocityY": 0.10163832945714488, - "timestamp": 0.29516426420770614 - }, - { - "x": 1.2151645581634748e-18, - "y": 0.033333336363653446, - "heading": -1.117382623336341e-18, - "angularVelocity": -1.9486820947092033e-18, - "velocityX": 5.614564136214341e-18, - "velocityY": 0.13551777226415238, - "timestamp": 0.3935523522769415 - }, - { - "x": 1.7436457586956917e-18, - "y": 0.05000000445895656, - "heading": -2.3967423989417355e-19, - "angularVelocity": 8.920870961137473e-18, - "velocityX": 5.371838503153778e-18, - "velocityY": 0.16939721486990153, - "timestamp": 0.4919404403461769 - }, - { - "x": 2.2482550261017366e-18, - "y": 0.07000000611520496, - "heading": 3.6125235146058906e-19, - "angularVelocity": 6.107704969814862e-18, - "velocityX": 5.129186225139142e-18, - "velocityY": 0.20327665725320723, - "timestamp": 0.5903285284154123 - }, - { - "x": 2.7290003868346387e-18, - "y": 0.09333334130808112, - "heading": 2.3536802574991945e-18, - "angularVelocity": 2.0250687712038572e-17, - "velocityX": 4.886615517778109e-18, - "velocityY": 0.23715609938935361, - "timestamp": 0.6887166164846477 - }, - { - "x": 3.185890819000824e-18, - "y": 0.12000001001040657, - "heading": 3.40725878345287e-18, - "angularVelocity": 1.070837766681137e-17, - "velocityX": 4.64413605242649e-18, - "velocityY": 0.27103554124926305, - "timestamp": 0.787104704553883 - }, - { - "x": 3.618936431576692e-18, - "y": 0.15000001219160564, - "heading": 4.731756190632852e-18, - "angularVelocity": 1.3461948548984505e-17, - "velocityX": 4.4017593218063314e-18, - "velocityY": 0.30491498279840595, - "timestamp": 0.8854927926231184 - }, - { - "x": 4.028148691625798e-18, - "y": 0.18333334781702582, - "heading": 5.109272897761843e-18, - "angularVelocity": 3.836992583636047e-18, - "velocityX": 4.1594991279037615e-18, - "velocityY": 0.3387944239953468, - "timestamp": 0.9838808806923538 - }, - { - "x": 4.413540716810058e-18, - "y": 0.22000001684706427, - "heading": 6.997707395181324e-18, - "angularVelocity": 1.919370316508542e-17, - "velocityX": 3.91737224524324e-18, - "velocityY": 0.37267386478977105, - "timestamp": 1.0822689687615892 - }, - { - "x": 4.775127658373826e-18, - "y": 0.2600000192360254, - "heading": 7.882590747581164e-18, - "angularVelocity": 8.993773931256457e-18, - "velocityX": 3.6753993405321354e-18, - "velocityY": 0.406553305119753, - "timestamp": 1.1806570568308246 - }, - { - "x": 5.112927212518244e-18, - "y": 0.3033333549305977, - "heading": 8.987118747688736e-18, - "angularVelocity": 1.1226200578912163e-17, - "velocityX": 3.433606277613366e-18, - "velocityY": 0.4404327449078857, - "timestamp": 1.27904514490006 - }, - { - "x": 5.426960319003839e-18, - "y": 0.3500000238677766, - "heading": 1.0530500974465385e-17, - "angularVelocity": 1.5686635916726856e-17, - "velocityX": 3.1920260198981374e-18, - "velocityY": 0.47431218405565095, - "timestamp": 1.3774332329692953 - }, - { - "x": 5.7172521411776675e-18, - "y": 0.4000000259719568, - "heading": 1.2181097094013063e-17, - "angularVelocity": 1.6776333979129697e-17, - "velocityX": 2.9507014905610136e-18, - "velocityY": 0.5081916224349754, - "timestamp": 1.4758213210385307 - }, - { - "x": 5.983833485064665e-18, - "y": 0.45333336115073153, - "heading": 1.4802873414250385e-17, - "angularVelocity": 2.6647239353050688e-17, - "velocityX": 2.7096900347694933e-18, - "velocityY": 0.5420710598750945, - "timestamp": 1.574209409107766 - }, - { - "x": 6.226742934460706e-18, - "y": 0.5100000292885927, - "heading": 1.7753206373705575e-17, - "angularVelocity": 2.9986626187410924e-17, - "velocityX": 2.469070695949251e-18, - "velocityY": 0.575950496141207, - "timestamp": 1.6725974971770015 - }, - { - "x": 6.446030218838556e-18, - "y": 0.5700000302370294, - "heading": 2.0426163520318335e-17, - "angularVelocity": 2.7167415779249895e-17, - "velocityX": 2.22895674498737e-18, - "velocityY": 0.6098299308978823, - "timestamp": 1.7709855852462368 - }, - { - "x": 6.6417618540851514e-18, - "y": 0.6333333637980277, - "heading": 2.266086393224807e-17, - "angularVelocity": 2.2713036643856925e-17, - "velocityX": 1.9895187779423867e-18, - "velocityY": 0.643709363641975, - "timestamp": 1.8693736733154722 - }, - { - "x": 6.814031362443908e-18, - "y": 0.7000000296943679, - "heading": 2.4278538293185775e-17, - "angularVelocity": 1.6441673304388865e-17, - "velocityX": 1.7510312541171544e-18, - "velocityY": 0.6775887935684523, - "timestamp": 1.9677617613847076 - }, - { - "x": 6.962979904521674e-18, - "y": 0.7700000275102201, - "heading": 2.5576668981263964e-17, - "angularVelocity": 1.3193866152308666e-17, - "velocityX": 1.5139783111446211e-18, - "velocityY": 0.7114682192685073, - "timestamp": 2.066149849453943 - }, - { - "x": 7.08884511898635e-18, - "y": 0.8433333565525355, - "heading": 2.687208646774119e-17, - "angularVelocity": 1.3166263419656072e-17, - "velocityX": 1.2793404346458061e-18, - "velocityY": 0.7453476379245281, - "timestamp": 2.1645379375231784 - }, - { - "x": 7.192111469900735e-18, - "y": 0.9200000154352173, - "heading": 2.792648282176678e-17, - "angularVelocity": 1.07165211280523e-17, - "velocityX": 1.0496260639054993e-18, - "velocityY": 0.7792270424924943, - "timestamp": 2.2629260255924137 - }, - { - "x": 7.274312354830099e-18, - "y": 1, - "heading": 3.000827595607593e-17, - "angularVelocity": 2.1158704872256568e-17, - "velocityX": 8.354953731965381e-19, - "velocityY": 0.813106404796549, - "timestamp": 2.361314113661649 - }, - { - "x": 6.687149573241866e-18, - "y": 1.0799999845647827, - "heading": 2.930546078718981e-17, - "angularVelocity": -7.143039017142222e-18, - "velocityX": -5.968361601177678e-18, - "velocityY": 0.813106404796549, - "timestamp": 2.4597022017308863 - }, - { - "x": 6.126769558515328e-18, - "y": 1.1566666434474644, - "heading": 2.90297401967108e-17, - "angularVelocity": -2.8021955879817803e-18, - "velocityX": -5.696128244278018e-18, - "velocityY": 0.7792270424924943, - "timestamp": 2.5580902898001217 - }, - { - "x": 5.591823986937553e-18, - "y": 1.2299999724897799, - "heading": 2.857601134904138e-17, - "angularVelocity": -4.611479301985519e-18, - "velocityX": -5.437597031717557e-18, - "velocityY": 0.745347637924528, - "timestamp": 2.656478377869357 - }, - { - "x": 5.081849281816435e-18, - "y": 1.299999970305632, - "heading": 2.647194627599618e-17, - "angularVelocity": -2.1385244054143357e-17, - "velocityX": -5.183776870340226e-18, - "velocityY": 0.7114682192685073, - "timestamp": 2.7548664659385924 - }, - { - "x": 4.596610451039836e-18, - "y": 1.3666666362019722, - "heading": 2.5047603581040707e-17, - "angularVelocity": -1.4476678683571795e-17, - "velocityX": -4.932344796891426e-18, - "velocityY": 0.6775887935684523, - "timestamp": 2.853254554007828 - }, - { - "x": 4.135965398247791e-18, - "y": 1.4299999697629706, - "heading": 2.302762325152366e-17, - "angularVelocity": -2.0530653971935406e-17, - "velocityX": -4.682356767449444e-18, - "velocityY": 0.6437093636419748, - "timestamp": 2.951642642077063 - }, - { - "x": 3.699818913378491e-18, - "y": 1.4899999707114076, - "heading": 2.142223093747466e-17, - "angularVelocity": -1.6316863078396375e-17, - "velocityX": -4.433336304057429e-18, - "velocityY": 0.6098299308978822, - "timestamp": 3.0500307301462986 - }, - { - "x": 3.2881027461238596e-18, - "y": 1.5466666388492685, - "heading": 2.064122419774981e-17, - "angularVelocity": -7.937956363543176e-18, - "velocityX": -4.185009430290064e-18, - "velocityY": 0.5759504961412069, - "timestamp": 3.148418818215534 - }, - { - "x": 2.9007655712244052e-18, - "y": 1.5999999740280433, - "heading": 1.9969619559651016e-17, - "angularVelocity": -6.826019925002285e-18, - "velocityX": -3.937204146623221e-18, - "velocityY": 0.5420710598750945, - "timestamp": 3.2468069062847693 - }, - { - "x": 2.5377673846695666e-18, - "y": 1.6499999761322235, - "heading": 1.9374646420177126e-17, - "angularVelocity": -6.047157286094339e-18, - "velocityX": -3.689805401521427e-18, - "velocityY": 0.5081916224349754, - "timestamp": 3.3451949943540047 - }, - { - "x": 2.1990761279741485e-18, - "y": 1.6966666450694023, - "heading": 1.8916317262571595e-17, - "angularVelocity": -4.65833693251108e-18, - "velocityX": -3.4427324491167103e-18, - "velocityY": 0.4743121840556509, - "timestamp": 3.44358308242324 - }, - { - "x": 1.8846655332659204e-18, - "y": 1.7399999807639748, - "heading": 1.7548485280288527e-17, - "angularVelocity": -1.3902376050525728e-17, - "velocityX": -3.1959264426763933e-18, - "velocityY": 0.44043274490788564, - "timestamp": 3.5419711704924755 - }, - { - "x": 1.5945136831032818e-18, - "y": 1.7799999831529358, - "heading": 1.5621235661695857e-17, - "angularVelocity": -1.9588207602447508e-17, - "velocityX": -2.9493431711978625e-18, - "velocityY": 0.40655330511975296, - "timestamp": 3.640359258561711 - }, - { - "x": 1.3286020112835303e-18, - "y": 1.8166666521829742, - "heading": 1.3232319504175871e-17, - "angularVelocity": -2.4280513008707978e-17, - "velocityX": -2.7029485778613663e-18, - "velocityY": 0.37267386478977105, - "timestamp": 3.738747346630946 - }, - { - "x": 1.086914587899011e-18, - "y": 1.8499999878083944, - "heading": 1.2132419497554097e-17, - "angularVelocity": -1.1179173608616076e-17, - "velocityX": -2.4567158715211472e-18, - "velocityY": 0.3387944239953468, - "timestamp": 3.8371354347001816 - }, - { - "x": 8.694375942744545e-19, - "y": 1.8799999899895934, - "heading": 1.0540845375160962e-17, - "angularVelocity": -1.617647020627435e-17, - "velocityX": -2.21062359685979e-18, - "velocityY": 0.304914982798406, - "timestamp": 3.935523522769417 - }, - { - "x": 6.761589286442026e-19, - "y": 1.9066666586919188, - "heading": 8.883338993825269e-18, - "angularVelocity": -1.6846597854180293e-17, - "velocityX": -1.964654305865992e-18, - "velocityY": 0.27103554124926305, - "timestamp": 4.033911610838652 - }, - { - "x": 5.070679042760459e-19, - "y": 1.929999993884795, - "heading": 6.529061416277828e-18, - "angularVelocity": -2.392846609653831e-17, - "velocityX": -1.7187936183501359e-18, - "velocityY": 0.2371560993893536, - "timestamp": 4.132299698907888 - }, - { - "x": 3.6215501445535984e-19, - "y": 1.9499999955410434, - "heading": 5.175466168401875e-18, - "angularVelocity": -1.3757701954247557e-17, - "velocityX": -1.4730295424471154e-18, - "velocityY": 0.20327665725320723, - "timestamp": 4.230687786977123 - }, - { - "x": 2.414117467704845e-19, - "y": 1.9666666636363466, - "heading": 2.4887675690357e-18, - "angularVelocity": -2.7307142780637332e-17, - "velocityX": -1.2273519735815864e-18, - "velocityY": 0.1693972148699015, - "timestamp": 4.3290758750463585 - }, - { - "x": 1.4483043443847653e-19, - "y": 1.9799999981488186, - "heading": 8.601988500829106e-19, - "angularVelocity": -1.655249097200501e-17, - "velocityX": -9.817523181071624e-19, - "velocityY": 0.13551777226415235, - "timestamp": 4.427463963115594 - }, - { - "x": 7.240413585822682e-20, - "y": 1.989999999058658, - "heading": 3.266565799202321e-19, - "angularVelocity": -5.4228284055470716e-18, - "velocityX": -7.3622320639297e-19, - "velocityY": 0.10163832945714488, - "timestamp": 4.525852051184829 - }, - { - "x": 2.4126536001171076e-20, - "y": 1.9966666663478638, - "heading": 2.6734077366946317e-19, - "angularVelocity": -6.028723729689646e-19, - "velocityX": -4.907582709261484e-19, - "velocityY": 0.06775888646717529, - "timestamp": 4.624240139254065 - }, - { - "x": -8.135093947234693e-24, - "y": 1.9999999999999998, - "heading": -1.6954777581594528e-26, - "angularVelocity": -2.7172052030925932e-18, - "velocityX": -2.4535197248634874e-19, - "velocityY": 0.033879443310153315, - "timestamp": 4.7226282273233 - }, - { - "x": -5.008838848500651e-24, - "y": 2, - "heading": -9.913062195911202e-27, - "angularVelocity": -2.918386915613901e-26, - "velocityX": 2.1783781061156283e-24, - "velocityY": -2.5283992697715324e-19, - "timestamp": 4.821016315392535 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/StraightForward.4.traj b/src/main/deploy/choreo/StraightForward.4.traj deleted file mode 100644 index 44c351a6..00000000 --- a/src/main/deploy/choreo/StraightForward.4.traj +++ /dev/null @@ -1,454 +0,0 @@ -{ - "samples": [ - { - "x": -5.008838848500651e-24, - "y": 2, - "heading": -9.913062195911202e-27, - "angularVelocity": -2.918386915613901e-26, - "velocityX": 2.1783781061156283e-24, - "velocityY": -2.5283992697715324e-19, - "timestamp": 0 - }, - { - "x": 2.0753539310053916e-20, - "y": 1.9966666663478638, - "heading": -3.3924267442427026e-25, - "angularVelocity": -3.3472516337821726e-24, - "velocityX": 2.1102334506622982e-19, - "velocityY": -0.03387944331015331, - "timestamp": 0.09838808806923538 - }, - { - "x": 6.227260339405634e-20, - "y": 1.989999999058658, - "heading": -9.415127970314296e-25, - "angularVelocity": -6.1213762440798274e-24, - "velocityX": 4.2204537503841897e-19, - "velocityY": -0.06775888646717527, - "timestamp": 0.19677617613847076 - }, - { - "x": 1.2455227342885217e-19, - "y": 1.9799999981488186, - "heading": -1.7962723582885153e-24, - "angularVelocity": -8.687657753919161e-24, - "velocityX": 6.330683169707924e-19, - "velocityY": -0.10163832945714488, - "timestamp": 0.29516426420770614 - }, - { - "x": 2.075926447807237e-19, - "y": 1.9666666636363466, - "heading": -2.8366172681319197e-24, - "angularVelocity": -1.0573910042856395e-23, - "velocityX": 8.440922249651074e-19, - "velocityY": -0.13551777226415238, - "timestamp": 0.3935523522769415 - }, - { - "x": 3.1139381878486925e-19, - "y": 1.9499999955410434, - "heading": -3.972041686262479e-24, - "angularVelocity": -1.1540279609110701e-23, - "velocityX": 1.0551171593656732e-18, - "velocityY": -0.1693972148699015, - "timestamp": 0.4919404403461769 - }, - { - "x": 4.359559034453201e-19, - "y": 1.929999993884795, - "heading": -5.159478281485765e-24, - "angularVelocity": -1.20689097210531e-23, - "velocityX": 1.2661431875802598e-18, - "velocityY": -0.20327665725320723, - "timestamp": 0.5903285284154123 - }, - { - "x": 5.812790142432268e-19, - "y": 1.9066666586919188, - "heading": -6.324264281794437e-24, - "angularVelocity": -1.1838680158880217e-23, - "velocityX": 1.4771703851722756e-18, - "velocityY": -0.2371560993893536, - "timestamp": 0.6887166164846477 - }, - { - "x": 7.473632750557841e-19, - "y": 1.8799999899895934, - "heading": -7.433822752952983e-24, - "angularVelocity": -1.1277360292414862e-23, - "velocityX": 1.6881988369545231e-18, - "velocityY": -0.27103554124926305, - "timestamp": 0.787104704553883 - }, - { - "x": 9.342088191957516e-19, - "y": 1.8499999878083944, - "heading": -8.289463274563344e-24, - "angularVelocity": -8.696581833507738e-24, - "velocityX": 1.899228638237177e-18, - "velocityY": -0.304914982798406, - "timestamp": 0.8854927926231184 - }, - { - "x": 1.1418157905706857e-18, - "y": 1.8166666521829742, - "heading": -8.791094438531766e-24, - "angularVelocity": -5.098477457853364e-24, - "velocityX": 2.1102598959558546e-18, - "velocityY": -0.33879442399534687, - "timestamp": 0.9838808806923538 - }, - { - "x": 1.3701843449632601e-18, - "y": 1.7799999831529358, - "heading": -8.958538336930905e-24, - "angularVelocity": -1.7018687746695977e-24, - "velocityX": 2.3212927299416833e-18, - "velocityY": -0.37267386478977105, - "timestamp": 1.0822689687615892 - }, - { - "x": 1.6193146513766686e-18, - "y": 1.7399999807639748, - "heading": -8.662387950504875e-24, - "angularVelocity": 3.010025947478922e-24, - "velocityX": 2.532327273455358e-18, - "velocityY": -0.406553305119753, - "timestamp": 1.1806570568308246 - }, - { - "x": 1.8892068933023243e-18, - "y": 1.6966666450694023, - "heading": -7.763746676879998e-24, - "angularVelocity": 9.133641008650935e-24, - "velocityX": 2.743363672401436e-18, - "velocityY": -0.44043274490788564, - "timestamp": 1.27904514490006 - }, - { - "x": 2.179861269683059e-18, - "y": 1.6499999761322235, - "heading": -5.979891084403687e-24, - "angularVelocity": 1.8130823090544997e-23, - "velocityX": 2.9544020820640314e-18, - "velocityY": -0.47431218405565095, - "timestamp": 1.3774332329692953 - }, - { - "x": 2.491277994923378e-18, - "y": 1.5999999740280433, - "heading": -3.1318625511206987e-24, - "angularVelocity": 2.8946883296905834e-23, - "velocityX": 3.1654426573706926e-18, - "velocityY": -0.5081916224349754, - "timestamp": 1.4758213210385307 - }, - { - "x": 2.8234572966317407e-18, - "y": 1.5466666388492685, - "heading": 1.2746740644139744e-24, - "angularVelocity": 4.47872963412253e-23, - "velocityX": 3.376485529499522e-18, - "velocityY": -0.5420710598750945, - "timestamp": 1.574209409107766 - }, - { - "x": 3.1763994081437363e-18, - "y": 1.4899999707114076, - "heading": 7.137353173597414e-24, - "angularVelocity": 5.958725558661365e-23, - "velocityX": 3.587530752531668e-18, - "velocityY": -0.575950496141207, - "timestamp": 1.6725974971770015 - }, - { - "x": 3.550104548721676e-18, - "y": 1.4299999697629708, - "heading": 1.4784368588518567e-23, - "angularVelocity": 7.772294744741366e-23, - "velocityX": 3.798578177501792e-18, - "velocityY": -0.6098299308978822, - "timestamp": 1.7709855852462368 - }, - { - "x": 3.944572873439836e-18, - "y": 1.3666666362019724, - "heading": 2.4685522368597076e-23, - "angularVelocity": 1.0063362932558388e-22, - "velocityX": 4.009627143145663e-18, - "velocityY": -0.643709363641975, - "timestamp": 1.8693736733154722 - }, - { - "x": 4.35980434250334e-18, - "y": 1.2999999703056322, - "heading": 3.740510491747365e-23, - "angularVelocity": 1.2927969167310106e-22, - "velocityX": 4.220675654736974e-18, - "velocityY": -0.6775887935684523, - "timestamp": 1.9677617613847076 - }, - { - "x": 4.7957983473533305e-18, - "y": 1.22999997248978, - "heading": 5.318650036063457e-23, - "angularVelocity": 1.6039947437582512e-22, - "velocityX": 4.431717906486655e-18, - "velocityY": -0.7114682192685073, - "timestamp": 2.066149849453943 - }, - { - "x": 5.252552446679876e-18, - "y": 1.1566666434474648, - "heading": 7.27694765189339e-23, - "angularVelocity": 1.9903820911278035e-22, - "velocityX": 4.642735221235561e-18, - "velocityY": -0.7453476379245281, - "timestamp": 2.1645379375231784 - }, - { - "x": 5.730056595973869e-18, - "y": 1.079999984564783, - "heading": 1.0096630665085409e-22, - "angularVelocity": 2.865878607555212e-22, - "velocityX": 4.853650197261181e-18, - "velocityY": -0.7792270424924945, - "timestamp": 2.2629260255924137 - }, - { - "x": 6.2282427168358e-18, - "y": 1, - "heading": 1.3642695798435716e-22, - "angularVelocity": 3.6041629217015064e-22, - "velocityX": 5.063872492830655e-18, - "velocityY": -0.8131064047965492, - "timestamp": 2.361314113661649 - }, - { - "x": 6.183163776953066e-18, - "y": 0.9200000154352174, - "heading": 1.0949873427318893e-22, - "angularVelocity": -2.7369386218952554e-22, - "velocityX": -4.58180081175704e-19, - "velocityY": -0.8131064047965493, - "timestamp": 2.4597022017308845 - }, - { - "x": 6.117523635853445e-18, - "y": 0.8433333565525356, - "heading": 9.207244155443994e-23, - "angularVelocity": -1.771178319990209e-22, - "velocityX": -6.671778045636001e-19, - "velocityY": -0.7792270424924946, - "timestamp": 2.55809028980012 - }, - { - "x": 6.031184241871937e-18, - "y": 0.7700000275102202, - "heading": 7.870338506115835e-23, - "angularVelocity": -1.3588087365283406e-22, - "velocityX": -8.775779476826745e-19, - "velocityY": -0.7453476379245282, - "timestamp": 2.6564783778693553 - }, - { - "x": 5.924117435021876e-18, - "y": 0.7000000296943679, - "heading": 6.771187556227619e-23, - "angularVelocity": -1.1171597807921007e-22, - "velocityX": -1.0882640657264913e-18, - "velocityY": -0.7114682192685075, - "timestamp": 2.7548664659385906 - }, - { - "x": 5.796312293836418e-18, - "y": 0.6333333637980278, - "heading": 5.866324558507834e-23, - "angularVelocity": -9.196882191601766e-23, - "velocityX": -1.2990610684817378e-18, - "velocityY": -0.6775887935684524, - "timestamp": 2.853254554007826 - }, - { - "x": 5.647763255268458e-18, - "y": 0.5700000302370295, - "heading": 5.1400871049015666e-23, - "angularVelocity": -7.381359399345974e-23, - "velocityX": -1.5099145415853494e-18, - "velocityY": -0.6437093636419751, - "timestamp": 2.9516426420770614 - }, - { - "x": 5.4784670203584905e-18, - "y": 0.5100000292885928, - "heading": 4.54530402227702e-23, - "angularVelocity": -6.045276480742016e-23, - "velocityX": -1.7208014970199695e-18, - "velocityY": -0.6098299308978823, - "timestamp": 3.0500307301462968 - }, - { - "x": 5.288421433894345e-18, - "y": 0.45333336115073164, - "heading": 4.030799828317159e-23, - "angularVelocity": -5.2293407257414914e-23, - "velocityX": -1.9317103236247645e-18, - "velocityY": -0.5759504961412071, - "timestamp": 3.148418818215532 - }, - { - "x": 5.077624990656839e-18, - "y": 0.4000000259719569, - "heading": 3.58579938101029e-23, - "angularVelocity": -4.5229107826418413e-23, - "velocityX": -2.1426344234041997e-18, - "velocityY": -0.5420710598750946, - "timestamp": 3.2468069062847675 - }, - { - "x": 4.846076586985391e-18, - "y": 0.3500000238677767, - "heading": 3.180712014523294e-23, - "angularVelocity": -4.1172381156419403e-23, - "velocityX": -2.353569720709197e-18, - "velocityY": -0.5081916224349755, - "timestamp": 3.345194994354003 - }, - { - "x": 4.593775383282716e-18, - "y": 0.3033333549305977, - "heading": 2.770876605483693e-23, - "angularVelocity": -4.165496316836929e-23, - "velocityX": -2.564513535686176e-18, - "velocityY": -0.474312184055651, - "timestamp": 3.4435830824232383 - }, - { - "x": 4.320720722178402e-18, - "y": 0.26000001923602545, - "heading": 2.3786765227246354e-23, - "angularVelocity": -3.986256457760449e-23, - "velocityX": -2.7754640190164052e-18, - "velocityY": -0.4404327449078857, - "timestamp": 3.5419711704924737 - }, - { - "x": 4.02691207701313e-18, - "y": 0.22000001684706427, - "heading": 2.020229881065505e-23, - "angularVelocity": -3.6431920742617227e-23, - "velocityX": -2.9864198443486014e-18, - "velocityY": -0.4065533051197531, - "timestamp": 3.640359258561709 - }, - { - "x": 3.712349017892417e-18, - "y": 0.18333334781702584, - "heading": 1.6882521656893888e-23, - "angularVelocity": -3.374167614218178e-23, - "velocityX": -3.197380029916969e-18, - "velocityY": -0.37267386478977116, - "timestamp": 3.7387473466309444 - }, - { - "x": 3.377031188434149e-18, - "y": 0.15000001219160564, - "heading": 1.3838247028470944e-23, - "angularVelocity": -3.0941513829444673e-23, - "velocityX": -3.4083438298267012e-18, - "velocityY": -0.33879442399534687, - "timestamp": 3.83713543470018 - }, - { - "x": 3.0209582893555436e-18, - "y": 0.12000001001040657, - "heading": 1.1063576902336286e-23, - "angularVelocity": -2.8201330931181093e-23, - "velocityX": -3.619310664732038e-18, - "velocityY": -0.304914982798406, - "timestamp": 3.935523522769415 - }, - { - "x": 2.6441300665755467e-18, - "y": 0.09333334130808112, - "heading": 8.606488657282892e-24, - "angularVelocity": -2.4973453877481673e-23, - "velocityX": -3.83028007615491e-18, - "velocityY": -0.27103554124926305, - "timestamp": 4.033911610838651 - }, - { - "x": 2.2465463023701263e-18, - "y": 0.07000000611520496, - "heading": 6.4325429496626686e-24, - "angularVelocity": -2.2095612525455652e-23, - "velocityX": -4.041251695195559e-18, - "velocityY": -0.23715609938935361, - "timestamp": 4.132299698907886 - }, - { - "x": 1.8282068086937753e-18, - "y": 0.05000000445895656, - "heading": 4.560020147095621e-24, - "angularVelocity": -1.9031993197120398e-23, - "velocityX": -4.252225220837285e-18, - "velocityY": -0.20327665725320723, - "timestamp": 4.230687786977121 - }, - { - "x": 1.3891114220101059e-18, - "y": 0.033333336363653446, - "heading": 3.049320875422628e-24, - "angularVelocity": -1.5354485508607965e-23, - "velocityX": -4.463200404415354e-18, - "velocityY": -0.16939721486990153, - "timestamp": 4.329075875046357 - }, - { - "x": 9.292599992741813e-19, - "y": 0.02000000185118139, - "heading": 1.8599640085541418e-24, - "angularVelocity": -1.2088421222550768e-23, - "velocityX": -4.674177038146959e-18, - "velocityY": -0.13551777226415238, - "timestamp": 4.427463963115592 - }, - { - "x": 4.48652414721063e-19, - "y": 0.010000000941341822, - "heading": 9.448880778978016e-25, - "angularVelocity": -9.300681800302624e-24, - "velocityX": -4.885154946696719e-18, - "velocityY": -0.10163832945714489, - "timestamp": 4.5258520511848275 - }, - { - "x": -5.271144269665744e-20, - "y": 0.0033333336521360374, - "heading": 3.2591541895178886e-25, - "angularVelocity": -6.291130188677765e-24, - "velocityX": -5.096133980824773e-18, - "velocityY": -0.06775888646717527, - "timestamp": 4.624240139254063 - }, - { - "x": -5.748316713988906e-19, - "y": -4.132250866556525e-18, - "heading": -1.2567862526727601e-29, - "angularVelocity": -3.3126782172732724e-24, - "velocityX": -5.307114012445354e-18, - "velocityY": -0.03387944331015331, - "timestamp": 4.722628227323298 - }, - { - "x": -1.1177083588923526e-18, - "y": -2.588370317815696e-18, - "heading": -1.4472783150300173e-29, - "angularVelocity": -1.9354755320059638e-29, - "velocityX": -5.51809493087181e-18, - "velocityY": 7.443696375437466e-19, - "timestamp": 4.821016315392534 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/StraightForward.5.traj b/src/main/deploy/choreo/StraightForward.5.traj deleted file mode 100644 index c769963a..00000000 --- a/src/main/deploy/choreo/StraightForward.5.traj +++ /dev/null @@ -1,535 +0,0 @@ -{ - "samples": [ - { - "x": -1.1177083588923526e-18, - "y": -2.588370317815696e-18, - "heading": -1.4472783150300173e-29, - "angularVelocity": -1.9354755320059638e-29, - "velocityX": -5.51809493087181e-18, - "velocityY": 7.443696375437466e-19, - "timestamp": 0 - }, - { - "x": 0.0023781215211549377, - "y": 0.0023781215211549364, - "heading": -5.418034508365842e-21, - "angularVelocity": -5.482324599836796e-20, - "velocityX": 0.0240633670172817, - "velocityY": 0.024063367017281703, - "timestamp": 0.0988274633157964 - }, - { - "x": 0.007134364554539963, - "y": 0.007134364554539962, - "heading": 2.784928879289317e-19, - "angularVelocity": 2.872793614299252e-18, - "velocityX": 0.04812673394425598, - "velocityY": 0.04812673394425598, - "timestamp": 0.1976549266315928 - }, - { - "x": 0.01426872909055665, - "y": 0.014268729090556648, - "heading": 2.6290919679734843e-19, - "angularVelocity": -1.5768594792595913e-19, - "velocityX": 0.07219010077410719, - "velocityY": 0.07219010077410717, - "timestamp": 0.2964823899473892 - }, - { - "x": 0.02378121511885375, - "y": 0.023781215118853744, - "heading": 2.4261266154684275e-19, - "angularVelocity": -2.053734849348351e-19, - "velocityX": 0.09625346749921782, - "velocityY": 0.0962534674992178, - "timestamp": 0.3953098532631856 - }, - { - "x": 0.03567182262823502, - "y": 0.03567182262823501, - "heading": -1.6278307756301597e-19, - "angularVelocity": -4.102055402193371e-18, - "velocityX": 0.12031683411103763, - "velocityY": 0.12031683411103761, - "timestamp": 0.49413731657898197 - }, - { - "x": 0.049940551606551344, - "y": 0.04994055160655134, - "heading": -8.668505353601748e-19, - "angularVelocity": -7.124208314248421e-18, - "velocityX": 0.14438020059992485, - "velocityY": 0.14438020059992482, - "timestamp": 0.5929647798947784 - }, - { - "x": 0.06658740204057369, - "y": 0.06658740204057367, - "heading": -1.2802085811795093e-18, - "angularVelocity": -4.182623038241345e-18, - "velocityX": 0.16844356695495213, - "velocityY": 0.1684435669549521, - "timestamp": 0.6917922432105748 - }, - { - "x": 0.0856123739158424, - "y": 0.08561237391584238, - "heading": -1.394541841050903e-18, - "angularVelocity": -1.1568975558424374e-18, - "velocityX": 0.1925069331636674, - "velocityY": 0.19250693316366735, - "timestamp": 0.7906197065263711 - }, - { - "x": 0.10701546721648714, - "y": 0.10701546721648711, - "heading": -1.5119540621698801e-18, - "angularVelocity": -1.1880524815792804e-18, - "velocityX": 0.21657029921179627, - "velocityY": 0.21657029921179627, - "timestamp": 0.8894471698421675 - }, - { - "x": 0.13079668192500984, - "y": 0.1307966819250098, - "heading": -1.9265469652509166e-18, - "angularVelocity": -4.195118191853065e-18, - "velocityX": 0.2406336650828684, - "velocityY": 0.2406336650828684, - "timestamp": 0.9882746331579639 - }, - { - "x": 0.15695601802202072, - "y": 0.15695601802202067, - "heading": -2.1496546222771247e-18, - "angularVelocity": -2.2575471033153812e-18, - "velocityX": 0.26469703075774254, - "velocityY": 0.2646970307577425, - "timestamp": 1.0871020964737603 - }, - { - "x": 0.1854934754859141, - "y": 0.18549347548591405, - "heading": -2.374412701432123e-18, - "angularVelocity": -2.274247218263183e-18, - "velocityX": 0.28876039621399596, - "velocityY": 0.2887603962139959, - "timestamp": 1.1859295597895567 - }, - { - "x": 0.21640905429246535, - "y": 0.2164090542924653, - "heading": -2.8947586701185075e-18, - "angularVelocity": -5.265196027705901e-18, - "velocityX": 0.3128237614251293, - "velocityY": 0.31282376142512924, - "timestamp": 1.2847570231053531 - }, - { - "x": 0.24970275441432413, - "y": 0.24970275441432405, - "heading": -3.4154953875764575e-18, - "angularVelocity": -5.2691499554024646e-18, - "velocityX": 0.33688712635951407, - "velocityY": 0.33688712635951407, - "timestamp": 1.3835844864211495 - }, - { - "x": 0.2853745758203676, - "y": 0.28537457582036757, - "heading": -3.908799891042607e-18, - "angularVelocity": -4.9915731422981935e-18, - "velocityX": 0.36095049097897813, - "velocityY": 0.3609504909789781, - "timestamp": 1.482411949736946 - }, - { - "x": 0.3234245184748622, - "y": 0.32342451847486214, - "heading": -4.106333508592265e-18, - "angularVelocity": -1.998772747832259e-18, - "velocityX": 0.38501385523686654, - "velocityY": 0.38501385523686654, - "timestamp": 1.5812394130527423 - }, - { - "x": 0.3638525823363569, - "y": 0.36385258233635687, - "heading": -4.301612868352148e-18, - "angularVelocity": -1.9759627937070713e-18, - "velocityX": 0.4090772190753272, - "velocityY": 0.4090772190753271, - "timestamp": 1.6800668763685387 - }, - { - "x": 0.4066587673561919, - "y": 0.40665876735619183, - "heading": -4.4934499230572835e-18, - "angularVelocity": -1.9411313422842306e-18, - "velocityX": 0.4331405824214162, - "velocityY": 0.4331405824214161, - "timestamp": 1.778894339684335 - }, - { - "x": 0.45184307347643876, - "y": 0.4518430734764387, - "heading": -4.38590045106494e-18, - "angularVelocity": 1.0882545061626265e-18, - "velocityX": 0.4572039451813483, - "velocityY": 0.45720394518134827, - "timestamp": 1.8777218030001315 - }, - { - "x": 0.49940550062697375, - "y": 0.49940550062697364, - "heading": -4.075459454626922e-18, - "angularVelocity": 3.1412417917185753e-18, - "velocityX": 0.48126730723172234, - "velocityY": 0.48126730723172234, - "timestamp": 1.9765492663159279 - }, - { - "x": 0.5493460487211731, - "y": 0.549346048721173, - "heading": -3.461804526195924e-18, - "angularVelocity": 6.209355725483015e-18, - "velocityX": 0.5053306684055835, - "velocityY": 0.5053306684055834, - "timestamp": 2.0753767296317243 - }, - { - "x": 0.6016647176493138, - "y": 0.6016647176493137, - "heading": -3.1313136201207807e-18, - "angularVelocity": 3.3441196837755998e-18, - "velocityX": 0.5293940284691947, - "velocityY": 0.5293940284691947, - "timestamp": 2.1742041929475207 - }, - { - "x": 0.6563615072679119, - "y": 0.6563615072679118, - "heading": -2.785953977666397e-18, - "angularVelocity": 3.494571203940914e-18, - "velocityX": 0.5534573870809412, - "velocityY": 0.5534573870809412, - "timestamp": 2.273031656263317 - }, - { - "x": 0.7134364173813069, - "y": 0.7134364173813067, - "heading": -2.715478020612081e-18, - "angularVelocity": 7.131208814821819e-19, - "velocityX": 0.5775207437128721, - "velocityY": 0.577520743712872, - "timestamp": 2.3718591195791134 - }, - { - "x": 0.772889447706879, - "y": 0.7728894477068786, - "heading": -2.308002641013774e-18, - "angularVelocity": 4.123098255203255e-18, - "velocityX": 0.60158409748507, - "velocityY": 0.60158409748507, - "timestamp": 2.47068658289491 - }, - { - "x": 0.8347205978005107, - "y": 0.8347205978005103, - "heading": -2.1882520138105614e-18, - "angularVelocity": 1.2117138211518718e-18, - "velocityX": 0.6256474467634036, - "velocityY": 0.6256474467634034, - "timestamp": 2.5695140462107062 - }, - { - "x": 0.8989298668627916, - "y": 0.8989298668627914, - "heading": -2.600558892907419e-18, - "angularVelocity": -4.171986778366762e-18, - "velocityX": 0.6497107879527877, - "velocityY": 0.6497107879527876, - "timestamp": 2.6683415095265026 - }, - { - "x": 0.9655172530284372, - "y": 0.9655172530284369, - "heading": -2.9104158106151282e-18, - "angularVelocity": -3.1353319319610485e-18, - "velocityX": 0.6737741102680166, - "velocityY": 0.6737741102680165, - "timestamp": 2.767168972842299 - }, - { - "x": 1.034482746971563, - "y": 1.0344827469715627, - "heading": -2.8870355286032125e-18, - "angularVelocity": 2.365768387844758e-19, - "velocityX": 0.6978373382179309, - "velocityY": 0.6978373382179308, - "timestamp": 2.8659964361580954 - }, - { - "x": 1.1010701331372088, - "y": 1.1010701331372084, - "heading": -2.5044778062473253e-18, - "angularVelocity": 3.870965844879766e-18, - "velocityX": 0.6737741102680164, - "velocityY": 0.6737741102680164, - "timestamp": 2.964823899473892 - }, - { - "x": 1.1652794021994897, - "y": 1.1652794021994894, - "heading": -1.9025969173015906e-18, - "angularVelocity": 6.090218909244497e-18, - "velocityX": 0.6497107879527875, - "velocityY": 0.6497107879527875, - "timestamp": 3.063651362789688 - }, - { - "x": 1.2271105522931214, - "y": 1.2271105522931212, - "heading": -9.264679815140483e-19, - "angularVelocity": 9.877101951914267e-18, - "velocityX": 0.6256474467634033, - "velocityY": 0.6256474467634033, - "timestamp": 3.1624788261054846 - }, - { - "x": 1.2865635826186934, - "y": 1.2865635826186932, - "heading": -1.846677135454343e-19, - "angularVelocity": 7.506013398090202e-18, - "velocityX": 0.60158409748507, - "velocityY": 0.6015840974850699, - "timestamp": 3.261306289421281 - }, - { - "x": 1.3436384927320884, - "y": 1.3436384927320884, - "heading": 2.985033438459339e-19, - "angularVelocity": 4.889036286936694e-18, - "velocityX": 0.5775207437128721, - "velocityY": 0.577520743712872, - "timestamp": 3.3601337527370774 - }, - { - "x": 1.3983352823506863, - "y": 1.3983352823506863, - "heading": 5.055652462787692e-19, - "angularVelocity": 2.095185898822513e-18, - "velocityX": 0.5534573870809412, - "velocityY": 0.553457387080941, - "timestamp": 3.4589612160528738 - }, - { - "x": 1.450653951278827, - "y": 1.450653951278827, - "heading": 5.279093823877594e-19, - "angularVelocity": 2.260923516848411e-19, - "velocityX": 0.5293940284691947, - "velocityY": 0.5293940284691946, - "timestamp": 3.55778867936867 - }, - { - "x": 1.5005944993730265, - "y": 1.5005944993730265, - "heading": 5.844300351339039e-19, - "angularVelocity": 5.719124628116133e-19, - "velocityX": 0.5053306684055834, - "velocityY": 0.5053306684055834, - "timestamp": 3.6566161426844666 - }, - { - "x": 1.5481569265235613, - "y": 1.5481569265235613, - "heading": 3.779197885193655e-19, - "angularVelocity": -2.0896037166931965e-18, - "velocityX": 0.4812673072317223, - "velocityY": 0.4812673072317223, - "timestamp": 3.755443606000263 - }, - { - "x": 1.5933412326438083, - "y": 1.5933412326438083, - "heading": 2.039493487749352e-20, - "angularVelocity": -3.6176667234042734e-18, - "velocityX": 0.45720394518134827, - "velocityY": 0.45720394518134827, - "timestamp": 3.8542710693160593 - }, - { - "x": 1.6361474176636432, - "y": 1.6361474176636432, - "heading": -6.968095113053582e-21, - "angularVelocity": -2.7687653315843575e-19, - "velocityX": 0.43314058242141606, - "velocityY": 0.43314058242141606, - "timestamp": 3.9530985326318557 - }, - { - "x": 1.676575481525138, - "y": 1.676575481525138, - "heading": 1.028814691598036e-19, - "angularVelocity": 1.111528889365859e-18, - "velocityX": 0.4090772190753271, - "velocityY": 0.4090772190753271, - "timestamp": 4.051925995947652 - }, - { - "x": 1.7146254241796324, - "y": 1.7146254241796324, - "heading": -5.672999077177864e-20, - "angularVelocity": -1.6150514296064684e-18, - "velocityX": 0.3850138552368665, - "velocityY": 0.3850138552368665, - "timestamp": 4.1507534592634485 - }, - { - "x": 1.7502972455856758, - "y": 1.750297245585676, - "heading": -1.922855653141426e-19, - "angularVelocity": -1.371638460144624e-18, - "velocityX": 0.36095049097897813, - "velocityY": 0.3609504909789781, - "timestamp": 4.249580922579245 - }, - { - "x": 1.7835909457075345, - "y": 1.7835909457075347, - "heading": -3.046581816575936e-19, - "angularVelocity": -1.1370583307666246e-18, - "velocityX": 0.33688712635951407, - "velocityY": 0.336887126359514, - "timestamp": 4.348408385895041 - }, - { - "x": 1.814506524514086, - "y": 1.8145065245140861, - "heading": -1.0007879634751174e-19, - "angularVelocity": 2.0700663400412595e-18, - "velocityX": 0.3128237614251293, - "velocityY": 0.3128237614251292, - "timestamp": 4.447235849210838 - }, - { - "x": 1.8430439819779791, - "y": 1.8430439819779794, - "heading": 4.207844233499018e-19, - "angularVelocity": 5.2704300636110826e-18, - "velocityX": 0.28876039621399596, - "velocityY": 0.2887603962139959, - "timestamp": 4.546063312526634 - }, - { - "x": 1.86920331807499, - "y": 1.8692033180749903, - "heading": 1.25733988172306e-18, - "angularVelocity": 8.46480757118372e-18, - "velocityX": 0.2646970307577425, - "velocityY": 0.2646970307577425, - "timestamp": 4.6448907758424305 - }, - { - "x": 1.8929845327835126, - "y": 1.8929845327835129, - "heading": 1.52547262188251e-18, - "angularVelocity": 2.713140055177162e-18, - "velocityX": 0.24063366508286838, - "velocityY": 0.24063366508286835, - "timestamp": 4.743718239158227 - }, - { - "x": 1.9143876260841575, - "y": 1.9143876260841575, - "heading": 1.5351626707181141e-18, - "angularVelocity": 9.805025681151685e-20, - "velocityX": 0.21657029921179624, - "velocityY": 0.21657029921179624, - "timestamp": 4.842545702474023 - }, - { - "x": 1.9334125979594263, - "y": 1.9334125979594263, - "heading": 1.5661943887692378e-18, - "angularVelocity": 3.139989977967844e-19, - "velocityX": 0.19250693316366735, - "velocityY": 0.19250693316366735, - "timestamp": 4.94137316578982 - }, - { - "x": 1.9500594483934486, - "y": 1.9500594483934488, - "heading": 1.9111016259033982e-18, - "angularVelocity": 3.48999383435869e-18, - "velocityX": 0.1684435669549521, - "velocityY": 0.1684435669549521, - "timestamp": 5.040200629105616 - }, - { - "x": 1.9643281773717647, - "y": 1.9643281773717651, - "heading": 1.8525436426975768e-18, - "angularVelocity": -5.925273851352638e-19, - "velocityX": 0.14438020059992485, - "velocityY": 0.14438020059992482, - "timestamp": 5.1390280924214125 - }, - { - "x": 1.976218784881146, - "y": 1.9762187848811463, - "heading": 1.5181268192693785e-18, - "angularVelocity": -3.383844909664733e-18, - "velocityX": 0.12031683411103762, - "velocityY": 0.12031683411103762, - "timestamp": 5.237855555737209 - }, - { - "x": 1.9857312709094432, - "y": 1.9857312709094435, - "heading": 1.218007859467631e-18, - "angularVelocity": -3.03679700352538e-18, - "velocityX": 0.09625346749921782, - "velocityY": 0.09625346749921782, - "timestamp": 5.336683019053005 - }, - { - "x": 1.9928656354454601, - "y": 1.99286563544546, - "heading": 6.414597240846549e-19, - "angularVelocity": -5.833885501885998e-18, - "velocityX": 0.07219010077410719, - "velocityY": 0.07219010077410719, - "timestamp": 5.435510482368802 - }, - { - "x": 1.997621878478845, - "y": 1.9976218784788449, - "heading": 3.1192356934373525e-19, - "angularVelocity": -3.3344590469642965e-18, - "velocityX": 0.04812673394425598, - "velocityY": 0.04812673394425598, - "timestamp": 5.534337945684598 - }, - { - "x": 2, - "y": 2, - "heading": 3.095704809390742e-29, - "angularVelocity": -3.1562436810261706e-18, - "velocityX": 0.024063367017281703, - "velocityY": 0.0240633670172817, - "timestamp": 5.633165409000394 - }, - { - "x": 2, - "y": 2, - "heading": 9.24554742891244e-30, - "angularVelocity": -7.339590862305549e-29, - "velocityX": -1.9532349903527946e-19, - "velocityY": -1.953180778215209e-19, - "timestamp": 5.731992872316191 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/StraightForward.6.traj b/src/main/deploy/choreo/StraightForward.6.traj deleted file mode 100644 index d75b68e4..00000000 --- a/src/main/deploy/choreo/StraightForward.6.traj +++ /dev/null @@ -1,535 +0,0 @@ -{ - "samples": [ - { - "x": 2, - "y": 2, - "heading": 9.24554742891244e-30, - "angularVelocity": -7.339590862305549e-29, - "velocityX": -1.9532349903527946e-19, - "velocityY": -1.953180778215209e-19, - "timestamp": 0 - }, - { - "x": 1.997621878478845, - "y": 1.997621878478845, - "heading": 2.9176550698701047e-19, - "angularVelocity": 2.952271403840556e-18, - "velocityX": -0.024063367017281706, - "velocityY": -0.0240633670172817, - "timestamp": 0.0988274633157964 - }, - { - "x": 1.99286563544546, - "y": 1.99286563544546, - "heading": 5.967411588481832e-19, - "angularVelocity": 3.0859402387521106e-18, - "velocityX": -0.04812673394425599, - "velocityY": -0.04812673394425598, - "timestamp": 0.1976549266315928 - }, - { - "x": 1.9857312709094432, - "y": 1.9857312709094432, - "heading": 8.990553363945518e-19, - "angularVelocity": 3.0590097309406617e-18, - "velocityX": -0.07219010077410719, - "velocityY": -0.07219010077410717, - "timestamp": 0.2964823899473892 - }, - { - "x": 1.9762187848811459, - "y": 1.9762187848811459, - "heading": 1.4932940433223978e-18, - "angularVelocity": 6.01289029503833e-18, - "velocityX": -0.09625346749921782, - "velocityY": -0.0962534674992178, - "timestamp": 0.3953098532631856 - }, - { - "x": 1.9643281773717647, - "y": 1.9643281773717647, - "heading": 2.301350529247808e-18, - "angularVelocity": 8.176436550349542e-18, - "velocityX": -0.12031683411103763, - "velocityY": -0.1203168341110376, - "timestamp": 0.49413731657898197 - }, - { - "x": 1.9500594483934484, - "y": 1.9500594483934484, - "heading": 3.1069311821375085e-18, - "angularVelocity": 8.151384411515975e-18, - "velocityX": -0.14438020059992485, - "velocityY": -0.1443802005999248, - "timestamp": 0.5929647798947784 - }, - { - "x": 1.933412597959426, - "y": 1.933412597959426, - "heading": 3.615579822445298e-18, - "angularVelocity": 5.146834914364682e-18, - "velocityX": -0.1684435669549521, - "velocityY": -0.16844356695495205, - "timestamp": 0.6917922432105748 - }, - { - "x": 1.9143876260841575, - "y": 1.9143876260841575, - "heading": 4.121905172237148e-18, - "angularVelocity": 5.123326299163994e-18, - "velocityX": -0.19250693316366735, - "velocityY": -0.1925069331636673, - "timestamp": 0.7906197065263711 - }, - { - "x": 1.8929845327835126, - "y": 1.8929845327835126, - "heading": 4.625994654512789e-18, - "angularVelocity": 5.100702286243361e-18, - "velocityX": -0.2165702992117962, - "velocityY": -0.21657029921179619, - "timestamp": 0.8894471698421675 - }, - { - "x": 1.8692033180749899, - "y": 1.8692033180749899, - "heading": 5.127944634100554e-18, - "angularVelocity": 5.079053493036733e-18, - "velocityX": -0.24063366508286835, - "velocityY": -0.24063366508286832, - "timestamp": 0.9882746331579639 - }, - { - "x": 1.8430439819779791, - "y": 1.8430439819779791, - "heading": 5.333332886477307e-18, - "angularVelocity": 2.0782507129575468e-18, - "velocityX": -0.2646970307577425, - "velocityY": -0.2646970307577424, - "timestamp": 1.0871020964737603 - }, - { - "x": 1.8145065245140857, - "y": 1.8145065245140857, - "heading": 5.2582030051247135e-18, - "angularVelocity": -7.602127045758556e-19, - "velocityX": -0.2887603962139959, - "velocityY": -0.28876039621399585, - "timestamp": 1.1859295597895567 - }, - { - "x": 1.7835909457075345, - "y": 1.7835909457075345, - "heading": 5.181292957950482e-18, - "angularVelocity": -7.782256292200617e-19, - "velocityX": -0.3128237614251292, - "velocityY": -0.3128237614251292, - "timestamp": 1.2847570231053531 - }, - { - "x": 1.7502972455856758, - "y": 1.7502972455856758, - "heading": 5.1027534114594344e-18, - "angularVelocity": -7.947139485337703e-19, - "velocityX": -0.336887126359514, - "velocityY": -0.33688712635951396, - "timestamp": 1.3835844864211495 - }, - { - "x": 1.7146254241796322, - "y": 1.7146254241796322, - "heading": 5.022755773866078e-18, - "angularVelocity": -8.094678491276822e-19, - "velocityX": -0.360950490978978, - "velocityY": -0.360950490978978, - "timestamp": 1.482411949736946 - }, - { - "x": 1.6765754815251377, - "y": 1.6765754815251377, - "heading": 4.646967759716646e-18, - "angularVelocity": -3.802465605838998e-18, - "velocityX": -0.38501385523686643, - "velocityY": -0.38501385523686643, - "timestamp": 1.5812394130527423 - }, - { - "x": 1.636147417663643, - "y": 1.6361474176636428, - "heading": 4.5646761078242135e-18, - "angularVelocity": -8.326802921818095e-19, - "velocityX": -0.40907721907532707, - "velocityY": -0.40907721907532707, - "timestamp": 1.6800668763685387 - }, - { - "x": 1.5933412326438081, - "y": 1.593341232643808, - "heading": 4.481619773950942e-18, - "angularVelocity": -8.404179011597725e-19, - "velocityX": -0.433140582421416, - "velocityY": -0.433140582421416, - "timestamp": 1.778894339684335 - }, - { - "x": 1.548156926523561, - "y": 1.548156926523561, - "heading": 4.69264716566721e-18, - "angularVelocity": 2.135310765625931e-18, - "velocityX": -0.4572039451813482, - "velocityY": -0.4572039451813482, - "timestamp": 1.8777218030001315 - }, - { - "x": 1.5005944993730262, - "y": 1.5005944993730262, - "heading": 5.1981444287811744e-18, - "angularVelocity": 5.114946741660759e-18, - "velocityX": -0.48126730723172223, - "velocityY": -0.48126730723172223, - "timestamp": 1.9765492663159279 - }, - { - "x": 1.450653951278827, - "y": 1.4506539512788268, - "heading": 5.998588984822106e-18, - "angularVelocity": 8.099413645686185e-18, - "velocityX": -0.5053306684055833, - "velocityY": -0.5053306684055833, - "timestamp": 2.0753767296317243 - }, - { - "x": 1.3983352823506863, - "y": 1.398335282350686, - "heading": 6.800057219972362e-18, - "angularVelocity": 8.109771959924199e-18, - "velocityX": -0.5293940284691946, - "velocityY": -0.5293940284691946, - "timestamp": 2.1742041929475207 - }, - { - "x": 1.3436384927320881, - "y": 1.3436384927320881, - "heading": 7.308809795922512e-18, - "angularVelocity": 5.147886314223004e-18, - "velocityX": -0.553457387080941, - "velocityY": -0.553457387080941, - "timestamp": 2.273031656263317 - }, - { - "x": 1.2865635826186932, - "y": 1.286563582618693, - "heading": 7.52592489681066e-18, - "angularVelocity": 2.1969102927676096e-18, - "velocityX": -0.5775207437128719, - "velocityY": -0.577520743712872, - "timestamp": 2.3718591195791134 - }, - { - "x": 1.2271105522931212, - "y": 1.2271105522931212, - "heading": 8.042018199779756e-18, - "angularVelocity": 5.222164534781162e-18, - "velocityX": -0.6015840974850699, - "velocityY": -0.6015840974850699, - "timestamp": 2.47068658289491 - }, - { - "x": 1.1652794021994897, - "y": 1.1652794021994894, - "heading": 8.270479544084847e-18, - "angularVelocity": 2.3117189802704113e-18, - "velocityX": -0.6256474467634033, - "velocityY": -0.6256474467634034, - "timestamp": 2.5695140462107062 - }, - { - "x": 1.1010701331372086, - "y": 1.1010701331372084, - "heading": 8.510243075879087e-18, - "angularVelocity": 2.4260818071067678e-18, - "velocityX": -0.6497107879527876, - "velocityY": -0.6497107879527876, - "timestamp": 2.6683415095265026 - }, - { - "x": 1.0344827469715632, - "y": 1.034482746971563, - "heading": 8.487156635946621e-18, - "angularVelocity": -2.3360355366177763e-19, - "velocityX": -0.6737741102680165, - "velocityY": -0.6737741102680165, - "timestamp": 2.767168972842299 - }, - { - "x": 0.9655172530284372, - "y": 0.965517253028437, - "heading": 8.5480561716338e-18, - "angularVelocity": 6.162207686069967e-19, - "velocityX": -0.6978373382179308, - "velocityY": -0.6978373382179308, - "timestamp": 2.8659964361580954 - }, - { - "x": 0.8989298668627916, - "y": 0.8989298668627914, - "heading": 8.406703773978591e-18, - "angularVelocity": -1.4302945801324534e-18, - "velocityX": -0.6737741102680165, - "velocityY": -0.6737741102680164, - "timestamp": 2.964823899473892 - }, - { - "x": 0.8347205978005106, - "y": 0.8347205978005104, - "heading": 8.29516939176197e-18, - "angularVelocity": -1.1285766777045508e-18, - "velocityX": -0.6497107879527876, - "velocityY": -0.6497107879527875, - "timestamp": 3.063651362789688 - }, - { - "x": 0.7728894477068788, - "y": 0.7728894477068787, - "heading": 8.497700879239251e-18, - "angularVelocity": 2.0493442633946093e-18, - "velocityX": -0.6256474467634034, - "velocityY": -0.6256474467634033, - "timestamp": 3.162478826105481 - }, - { - "x": 0.7134364173813069, - "y": 0.7134364173813067, - "heading": 8.715363483429085e-18, - "angularVelocity": 2.2024506538239383e-18, - "velocityX": -0.6015840974850699, - "velocityY": -0.6015840974850699, - "timestamp": 3.261306289421274 - }, - { - "x": 0.6563615072679118, - "y": 0.6563615072679116, - "heading": 8.945709750816389e-18, - "angularVelocity": 2.330792062318582e-18, - "velocityX": -0.577520743712872, - "velocityY": -0.577520743712872, - "timestamp": 3.3601337527370667 - }, - { - "x": 0.6016647176493137, - "y": 0.6016647176493136, - "heading": 9.481711280544972e-18, - "angularVelocity": 5.4236090257494665e-18, - "velocityX": -0.553457387080941, - "velocityY": -0.553457387080941, - "timestamp": 3.4589612160528596 - }, - { - "x": 0.549346048721173, - "y": 0.5493460487211729, - "heading": 9.733232170684809e-18, - "angularVelocity": 2.5450506077548204e-18, - "velocityX": -0.5293940284691947, - "velocityY": -0.5293940284691946, - "timestamp": 3.5577886793686524 - }, - { - "x": 0.4994055006269736, - "y": 0.4994055006269735, - "heading": 9.699481252660535e-18, - "angularVelocity": -3.415135021970597e-19, - "velocityX": -0.5053306684055833, - "velocityY": -0.5053306684055833, - "timestamp": 3.6566161426844452 - }, - { - "x": 0.45184307347643865, - "y": 0.45184307347643854, - "heading": 9.37985428465038e-18, - "angularVelocity": -3.234191651445345e-18, - "velocityX": -0.4812673072317223, - "velocityY": -0.48126730723172223, - "timestamp": 3.755443606000238 - }, - { - "x": 0.40665876735619183, - "y": 0.4066587673561917, - "heading": 9.362931683815324e-18, - "angularVelocity": -1.712336792194834e-19, - "velocityX": -0.45720394518134827, - "velocityY": -0.4572039451813482, - "timestamp": 3.854271069316031 - }, - { - "x": 0.3638525823363568, - "y": 0.36385258233635676, - "heading": 9.059269008897951e-18, - "angularVelocity": -3.072654581500084e-18, - "velocityX": -0.43314058242141606, - "velocityY": -0.433140582421416, - "timestamp": 3.9530985326318238 - }, - { - "x": 0.32342451847486214, - "y": 0.32342451847486203, - "heading": 8.763075814779251e-18, - "angularVelocity": -2.9970734945597385e-18, - "velocityX": -0.40907721907532707, - "velocityY": -0.409077219075327, - "timestamp": 4.051925995947617 - }, - { - "x": 0.28537457582036757, - "y": 0.28537457582036746, - "heading": 8.179554689955981e-18, - "angularVelocity": -5.9044427782956105e-18, - "velocityX": -0.3850138552368665, - "velocityY": -0.38501385523686643, - "timestamp": 4.150753459263409 - }, - { - "x": 0.2497027544143241, - "y": 0.24970275441432402, - "heading": 7.603006050284837e-18, - "angularVelocity": -5.8338906138842846e-18, - "velocityX": -0.3609504909789781, - "velocityY": -0.360950490978978, - "timestamp": 4.249580922579202 - }, - { - "x": 0.21640905429246532, - "y": 0.21640905429246526, - "heading": 7.033233234769523e-18, - "angularVelocity": -5.765328461225564e-18, - "velocityX": -0.336887126359514, - "velocityY": -0.33688712635951396, - "timestamp": 4.348408385894995 - }, - { - "x": 0.18549347548591408, - "y": 0.18549347548591402, - "heading": 6.175535578468071e-18, - "angularVelocity": -8.67873752787532e-18, - "velocityX": -0.31282376142512924, - "velocityY": -0.3128237614251292, - "timestamp": 4.447235849210788 - }, - { - "x": 0.15695601802202072, - "y": 0.15695601802202067, - "heading": 5.3242912654674056e-18, - "angularVelocity": -8.613438444393486e-18, - "velocityX": -0.2887603962139959, - "velocityY": -0.2887603962139959, - "timestamp": 4.546063312526581 - }, - { - "x": 0.13079668192500982, - "y": 0.13079668192500976, - "heading": 4.479366707374499e-18, - "angularVelocity": -8.549491094513284e-18, - "velocityX": -0.26469703075774254, - "velocityY": -0.2646970307577425, - "timestamp": 4.644890775842374 - }, - { - "x": 0.10701546721648711, - "y": 0.10701546721648708, - "heading": 3.64064272114218e-18, - "angularVelocity": -8.486749715566848e-18, - "velocityX": -0.2406336650828684, - "velocityY": -0.24063366508286835, - "timestamp": 4.7437182391581665 - }, - { - "x": 0.08561237391584237, - "y": 0.08561237391584235, - "heading": 3.1105036447238866e-18, - "angularVelocity": -5.364288719895572e-18, - "velocityX": -0.2165702992117963, - "velocityY": -0.21657029921179624, - "timestamp": 4.842545702473959 - }, - { - "x": 0.06658740204057367, - "y": 0.06658740204057366, - "heading": 2.5863618381776226e-18, - "angularVelocity": -5.303604412403694e-18, - "velocityX": -0.19250693316366738, - "velocityY": -0.19250693316366735, - "timestamp": 4.941373165789752 - }, - { - "x": 0.04994055160655133, - "y": 0.04994055160655132, - "heading": 2.06812977982204e-18, - "angularVelocity": -5.2438058439965976e-18, - "velocityX": -0.1684435669549521, - "velocityY": -0.16844356695495208, - "timestamp": 5.040200629105545 - }, - { - "x": 0.03567182262823501, - "y": 0.035671822628235005, - "heading": 1.5557277971478494e-18, - "angularVelocity": -5.184813384293766e-18, - "velocityX": -0.14438020059992485, - "velocityY": -0.1443802005999248, - "timestamp": 5.139028092421338 - }, - { - "x": 0.023781215118853744, - "y": 0.02378121511885374, - "heading": 1.2333207349325143e-18, - "angularVelocity": -3.2623223165160492e-18, - "velocityX": -0.12031683411103762, - "velocityY": -0.12031683411103758, - "timestamp": 5.237855555737131 - }, - { - "x": 0.014268729090556646, - "y": 0.014268729090556643, - "heading": 9.166038261970496e-19, - "angularVelocity": -3.2047456798497867e-18, - "velocityX": -0.09625346749921782, - "velocityY": -0.09625346749921779, - "timestamp": 5.3366830190529235 - }, - { - "x": 0.0071343645545399615, - "y": 0.00713436455453996, - "heading": 6.055153925645065e-19, - "angularVelocity": -3.1477932139121737e-18, - "velocityX": -0.07219010077410719, - "velocityY": -0.07219010077410716, - "timestamp": 5.435510482368716 - }, - { - "x": 0.0023781215211549364, - "y": 0.0023781215211549356, - "heading": 2.99998430529326e-19, - "angularVelocity": -3.0914174770892124e-18, - "velocityX": -0.04812673394425598, - "velocityY": -0.04812673394425597, - "timestamp": 5.534337945684509 - }, - { - "x": -2.5177140124632005e-18, - "y": -2.5177139656237954e-18, - "heading": 1.1418576549630119e-29, - "angularVelocity": -3.0355774220989457e-18, - "velocityX": -0.024063367017281703, - "velocityY": -0.024063367017281696, - "timestamp": 5.633165409000302 - }, - { - "x": -1.1177094777187861e-18, - "y": -1.1177094535843756e-18, - "heading": 5.214297458751026e-30, - "angularVelocity": -1.0017271530534466e-29, - "velocityX": 2.856704072481815e-18, - "velocityY": 2.856704086919547e-18, - "timestamp": 5.731992872316095 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Tag14.1.traj b/src/main/deploy/choreo/Tag14.1.traj index d6ae0344..8dc833fe 100644 --- a/src/main/deploy/choreo/Tag14.1.traj +++ b/src/main/deploy/choreo/Tag14.1.traj @@ -1,157 +1,77 @@ { - "samples": [ - { - "x": 6.224338054656982, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.4108248462198856e-52, - "timestamp": 0 - }, - { - "x": 6.21723902185644, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 2.4668199846059922e-26, - "velocityX": -0.0744521368751352, - "velocityY": -9.854529719176074e-36, - "timestamp": 0.09535028944096227 - }, - { - "x": 6.203040956377823, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 4.073373060497981e-25, - "velocityX": -0.14890427246588678, - "velocityY": -3.4838944147190658e-37, - "timestamp": 0.19070057888192454 - }, - { - "x": 6.181743858388128, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 4.6563923109407e-25, - "velocityX": -0.22335640630520623, - "velocityY": 9.60043551478312e-36, - "timestamp": 0.28605086832288684 - }, - { - "x": 6.153347728128578, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 1.4742520302800545e-24, - "velocityX": -0.29780853761467935, - "velocityY": -1.788338902588745e-35, - "timestamp": 0.3814011577638491 - }, - { - "x": 6.117852565978237, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 6.442632745673608e-25, - "velocityX": -0.3722606649486799, - "velocityY": -6.290456308064992e-36, - "timestamp": 0.47675144720481133 - }, - { - "x": 6.075258372619414, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 4.4423067176817646e-24, - "velocityX": -0.4467127851268304, - "velocityY": 5.242130261899161e-35, - "timestamp": 0.5721017366457736 - }, - { - "x": 6.025565149644175, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 8.105662994909866e-24, - "velocityX": -0.5211648886080018, - "velocityY": 7.004285681194871e-35, - "timestamp": 0.6674520260867358 - }, - { - "x": 5.968772905012807, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -2.0725270792413912e-24, - "velocityX": -0.5956169086044655, - "velocityY": -4.3523966027072635e-35, - "timestamp": 0.7628023155276981 - }, - { - "x": 5.919079682037568, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -8.006174965180316e-24, - "velocityX": -0.5211648886080017, - "velocityY": -1.8945235189214832e-35, - "timestamp": 0.8581526049686603 - }, - { - "x": 5.876485488678745, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.383666380682825e-24, - "velocityX": -0.4467127851268303, - "velocityY": -1.5466989905142068e-35, - "timestamp": 0.9535028944096225 - }, - { - "x": 5.840990326528404, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -2.1969937395424362e-24, - "velocityX": -0.3722606649486798, - "velocityY": -1.620842864899481e-35, - "timestamp": 1.0488531838505848 - }, - { - "x": 5.812594196268854, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.3032170138675753e-24, - "velocityX": -0.29780853761467935, - "velocityY": 2.776053837220692e-37, - "timestamp": 1.1442034732915471 - }, - { - "x": 5.791297098279159, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -5.244503893696962e-25, - "velocityX": -0.22335640630520623, - "velocityY": -3.2771501301056225e-36, - "timestamp": 1.2395537627325095 - }, - { - "x": 5.777099032800542, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.4115566589891501e-25, - "velocityX": -0.14890427246588678, - "velocityY": 2.0466110465352638e-36, - "timestamp": 1.3349040521734719 - }, - { - "x": 5.77, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 6.406778527596553e-26, - "velocityX": -0.0744521368751352, - "velocityY": -2.5902780242718716e-36, - "timestamp": 1.4302543416144342 - }, - { - "x": 5.77, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -7.3977584585344e-40, - "velocityX": -2.3244728289151072e-40, - "velocityY": 1.0431484672638798e-42, - "timestamp": 1.5256046310553966 - } - ] + "samples": [ + { + "x": 6.224338054656982, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -1.9390670879884598e-35, + "velocityX": 1.7766873980807548e-19, + "velocityY": 8.904182726292149e-48, + "timestamp": 0 + }, + { + "x": 6.186476547774305, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -2.4996122271018033e-24, + "velocityX": -0.40642791397744565, + "velocityY": 1.1300565278043194e-37, + "timestamp": 0.0931567581373812 + }, + { + "x": 6.110753536336873, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -7.642010441615522e-24, + "velocityX": -0.8128558029655188, + "velocityY": -2.827834467819816e-37, + "timestamp": 0.1863135162747624 + }, + { + "x": 5.99716902732849, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -1.6489729233438956e-24, + "velocityX": -1.2192836169854868, + "velocityY": -1.22969459333099e-36, + "timestamp": 0.27947027441214356 + }, + { + "x": 5.88358451832009, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 1.0379573373635505e-23, + "velocityX": -1.2192836169854868, + "velocityY": -2.8845016149269677e-37, + "timestamp": 0.3726270325495248 + }, + { + "x": 5.807861506882689, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -6.829975200213146e-25, + "velocityX": -0.8128558029655188, + "velocityY": 9.593634209674488e-37, + "timestamp": 0.465783790686906 + }, + { + "x": 5.769999999999999, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 2.101401640366878e-24, + "velocityX": -0.4064279139774457, + "velocityY": 7.2855825720271415e-37, + "timestamp": 0.5589405488242872 + }, + { + "x": 5.77, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -1.8956839170817245e-35, + "velocityX": 1.3726397507122414e-19, + "velocityY": 1.1843965226752523e-47, + "timestamp": 0.6520973069616685 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Tag14.2.traj b/src/main/deploy/choreo/Tag14.2.traj index d7effa7e..bbb51a92 100644 --- a/src/main/deploy/choreo/Tag14.2.traj +++ b/src/main/deploy/choreo/Tag14.2.traj @@ -1,67 +1,41 @@ { - "samples": [ - { - "x": 5.77, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -7.3977584585344e-40, - "velocityX": -2.3244728289151072e-40, - "velocityY": 1.0431484672638798e-42, - "timestamp": 0 - }, - { - "x": 5.776222222781161, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 7.967001188899933e-26, - "velocityX": 0.06970282002598219, - "velocityY": 6.68920621834722e-35, - "timestamp": 0.08926787723712959 - }, - { - "x": 5.788666667784544, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 1.2807830939495395e-24, - "velocityX": 0.13940563379059512, - "velocityY": -9.825179674996197e-36, - "timestamp": 0.17853575447425918 - }, - { - "x": 5.807333332215455, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.0883251323550264e-24, - "velocityX": 0.209108416248375, - "velocityY": -9.687396006109793e-36, - "timestamp": 0.26780363171138877 - }, - { - "x": 5.819777777218838, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -2.360522661586125e-25, - "velocityX": 0.13940563379059512, - "velocityY": -9.74036123267109e-36, - "timestamp": 0.35707150894851836 - }, - { - "x": 5.826, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -3.6074338636642e-26, - "velocityX": 0.06970282002598219, - "velocityY": -4.2689399061332323e-35, - "timestamp": 0.44633938618564795 - }, - { - "x": 5.826, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 3.7086661645988916e-41, - "velocityX": -1.1448525627762707e-40, - "velocityY": 6.561918971393292e-42, - "timestamp": 0.5356072634227778 - } - ] + "samples": [ + { + "x": 5.77, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -1.8956839170817245e-35, + "velocityX": 1.3726397507122414e-19, + "velocityY": 1.1843965226752523e-47, + "timestamp": 0 + }, + { + "x": 5.797999999999997, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -4.600113928387426e-28, + "velocityX": 0.3495132037100882, + "velocityY": 8.714533783663232e-44, + "timestamp": 0.08011142269527904 + }, + { + "x": 5.826, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -4.607782485444709e-28, + "velocityX": 0.34951320371008815, + "velocityY": 8.023846057264112e-44, + "timestamp": 0.1602228453905581 + }, + { + "x": 5.826, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 9.888109688615658e-44, + "velocityX": -2.1136492085418514e-19, + "velocityY": -4.40493861881828e-55, + "timestamp": 0.24033426808583713 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Tag14.traj b/src/main/deploy/choreo/Tag14.traj index af311346..f789a808 100644 --- a/src/main/deploy/choreo/Tag14.traj +++ b/src/main/deploy/choreo/Tag14.traj @@ -1,211 +1,104 @@ { - "samples": [ - { - "x": 6.224338054656982, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -1.4108248462198856e-52, - "timestamp": 0 - }, - { - "x": 6.21723902185644, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 2.4668199846059922e-26, - "velocityX": -0.0744521368751352, - "velocityY": -9.854529719176074e-36, - "timestamp": 0.09535028944096227 - }, - { - "x": 6.203040956377823, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 4.073373060497981e-25, - "velocityX": -0.14890427246588678, - "velocityY": -3.4838944147190658e-37, - "timestamp": 0.19070057888192454 - }, - { - "x": 6.181743858388128, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 4.6563923109407e-25, - "velocityX": -0.22335640630520623, - "velocityY": 9.60043551478312e-36, - "timestamp": 0.28605086832288684 - }, - { - "x": 6.153347728128578, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 1.4742520302800545e-24, - "velocityX": -0.29780853761467935, - "velocityY": -1.788338902588745e-35, - "timestamp": 0.3814011577638491 - }, - { - "x": 6.117852565978237, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 6.442632745673608e-25, - "velocityX": -0.3722606649486799, - "velocityY": -6.290456308064992e-36, - "timestamp": 0.47675144720481133 - }, - { - "x": 6.075258372619414, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 4.4423067176817646e-24, - "velocityX": -0.4467127851268304, - "velocityY": 5.242130261899161e-35, - "timestamp": 0.5721017366457736 - }, - { - "x": 6.025565149644175, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 8.105662994909866e-24, - "velocityX": -0.5211648886080018, - "velocityY": 7.004285681194871e-35, - "timestamp": 0.6674520260867358 - }, - { - "x": 5.968772905012807, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -2.0725270792413912e-24, - "velocityX": -0.5956169086044655, - "velocityY": -4.3523966027072635e-35, - "timestamp": 0.7628023155276981 - }, - { - "x": 5.919079682037568, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -8.006174965180316e-24, - "velocityX": -0.5211648886080017, - "velocityY": -1.8945235189214832e-35, - "timestamp": 0.8581526049686603 - }, - { - "x": 5.876485488678745, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.383666380682825e-24, - "velocityX": -0.4467127851268303, - "velocityY": -1.5466989905142068e-35, - "timestamp": 0.9535028944096225 - }, - { - "x": 5.840990326528404, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -2.1969937395424362e-24, - "velocityX": -0.3722606649486798, - "velocityY": -1.620842864899481e-35, - "timestamp": 1.0488531838505848 - }, - { - "x": 5.812594196268854, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.3032170138675753e-24, - "velocityX": -0.29780853761467935, - "velocityY": 2.776053837220692e-37, - "timestamp": 1.1442034732915471 - }, - { - "x": 5.791297098279159, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -5.244503893696962e-25, - "velocityX": -0.22335640630520623, - "velocityY": -3.2771501301056225e-36, - "timestamp": 1.2395537627325095 - }, - { - "x": 5.777099032800542, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.4115566589891501e-25, - "velocityX": -0.14890427246588678, - "velocityY": 2.0466110465352638e-36, - "timestamp": 1.3349040521734719 - }, - { - "x": 5.77, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 6.406778527596553e-26, - "velocityX": -0.0744521368751352, - "velocityY": -2.5902780242718716e-36, - "timestamp": 1.4302543416144342 - }, - { - "x": 5.77, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -7.3977584585344e-40, - "velocityX": -2.3244728289151072e-40, - "velocityY": 1.0431484672638798e-42, - "timestamp": 1.5256046310553966 - }, - { - "x": 5.776222222781161, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 7.967001188899933e-26, - "velocityX": 0.06970282002598219, - "velocityY": 6.68920621834722e-35, - "timestamp": 1.6148725082925262 - }, - { - "x": 5.788666667784544, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 1.2807830939495395e-24, - "velocityX": 0.13940563379059512, - "velocityY": -9.825179674996197e-36, - "timestamp": 1.7041403855296557 - }, - { - "x": 5.807333332215455, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.0883251323550264e-24, - "velocityX": 0.209108416248375, - "velocityY": -9.687396006109793e-36, - "timestamp": 1.7934082627667853 - }, - { - "x": 5.819777777218838, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -2.360522661586125e-25, - "velocityX": 0.13940563379059512, - "velocityY": -9.74036123267109e-36, - "timestamp": 1.882676140003915 - }, - { - "x": 5.826, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -3.6074338636642e-26, - "velocityX": 0.06970282002598219, - "velocityY": -4.2689399061332323e-35, - "timestamp": 1.9719440172410445 - }, - { - "x": 5.826, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 3.7086661645988916e-41, - "velocityX": -1.1448525627762707e-40, - "velocityY": 6.561918971393292e-42, - "timestamp": 2.0612118944781743 - } - ] + "samples": [ + { + "x": 6.224338054656982, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -1.9390670879884598e-35, + "velocityX": 1.7766873980807548e-19, + "velocityY": 8.904182726292149e-48, + "timestamp": 0 + }, + { + "x": 6.186476547774305, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -2.4996122271018033e-24, + "velocityX": -0.40642791397744565, + "velocityY": 1.1300565278043194e-37, + "timestamp": 0.0931567581373812 + }, + { + "x": 6.110753536336873, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -7.642010441615522e-24, + "velocityX": -0.8128558029655188, + "velocityY": -2.827834467819816e-37, + "timestamp": 0.1863135162747624 + }, + { + "x": 5.99716902732849, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -1.6489729233438956e-24, + "velocityX": -1.2192836169854868, + "velocityY": -1.22969459333099e-36, + "timestamp": 0.27947027441214356 + }, + { + "x": 5.88358451832009, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 1.0379573373635505e-23, + "velocityX": -1.2192836169854868, + "velocityY": -2.8845016149269677e-37, + "timestamp": 0.3726270325495248 + }, + { + "x": 5.807861506882689, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -6.829975200213146e-25, + "velocityX": -0.8128558029655188, + "velocityY": 9.593634209674488e-37, + "timestamp": 0.465783790686906 + }, + { + "x": 5.769999999999999, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 2.101401640366878e-24, + "velocityX": -0.4064279139774457, + "velocityY": 7.2855825720271415e-37, + "timestamp": 0.5589405488242872 + }, + { + "x": 5.77, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -1.8956839170817245e-35, + "velocityX": 1.3726397507122414e-19, + "velocityY": 1.1843965226752523e-47, + "timestamp": 0.6520973069616685 + }, + { + "x": 5.797999999999997, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -4.600113928387426e-28, + "velocityX": 0.3495132037100882, + "velocityY": 8.714533783663232e-44, + "timestamp": 0.7322087296569475 + }, + { + "x": 5.826, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -4.607782485444709e-28, + "velocityX": 0.34951320371008815, + "velocityY": 8.023846057264112e-44, + "timestamp": 0.8123201523522265 + }, + { + "x": 5.826, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 9.888109688615658e-44, + "velocityX": -2.1136492085418514e-19, + "velocityY": -4.40493861881828e-55, + "timestamp": 0.8924315750475056 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Tag15.1.traj b/src/main/deploy/choreo/Tag15.1.traj index 5e319415..463bae2a 100644 --- a/src/main/deploy/choreo/Tag15.1.traj +++ b/src/main/deploy/choreo/Tag15.1.traj @@ -1,157 +1,77 @@ { - "samples": [ - { - "x": 4.189568967671509, - "y": 5.279922480109792, - "heading": -1.0471975512, - "angularVelocity": 8.220687424023247e-35, - "velocityX": -1.8495013172516143e-33, - "velocityY": -1.5997840520692296e-33, - "timestamp": 0 - }, - { - "x": 4.19311848407178, - "y": 5.273774537362223, - "heading": -1.0471975512, - "angularVelocity": 7.45114074725589e-17, - "velocityX": 0.03722606843756754, - "velocityY": -0.06447744189990307, - "timestamp": 0.09535028944096205 - }, - { - "x": 4.200217516811089, - "y": 5.261478651973145, - "heading": -1.0471975512, - "angularVelocity": 1.61376783975804e-16, - "velocityX": 0.07445213623294329, - "velocityY": -0.1289548826874973, - "timestamp": 0.1907005788819241 - }, - { - "x": 4.210866065805936, - "y": 5.243034824087183, - "heading": -1.0471975512, - "angularVelocity": 2.4319200253214066e-16, - "velocityX": 0.11167820315260298, - "velocityY": -0.1934323219583068, - "timestamp": 0.2860508683228862 - }, - { - "x": 4.225064130935711, - "y": 5.218443053913242, - "heading": -1.0471975512, - "angularVelocity": 3.169122315246911e-16, - "velocityX": 0.14890426880733948, - "velocityY": -0.2579097590382051, - "timestamp": 0.3814011577638482 - }, - { - "x": 4.242811712010882, - "y": 5.1877033417795975, - "heading": -1.0471975512, - "angularVelocity": 3.710180313583571e-16, - "velocityX": 0.1861303324743397, - "velocityY": -0.3223871926752432, - "timestamp": 0.4767514472048102 - }, - { - "x": 4.264108808690293, - "y": 5.150815688277151, - "heading": -1.0471975512, - "angularVelocity": 4.524900551124174e-16, - "velocityX": 0.22335639256341489, - "velocityY": -0.38686462011513334, - "timestamp": 0.5721017366457722 - }, - { - "x": 4.2889554201779125, - "y": 5.10778009478467, - "heading": -1.0471975512, - "angularVelocity": 5.323391559336802e-16, - "velocityX": 0.2605824443040005, - "velocityY": -0.4513420330950154, - "timestamp": 0.6674520260867343 - }, - { - "x": 4.317351542493596, - "y": 5.0585965681959655, - "heading": -1.0471975512, - "angularVelocity": 5.803718444019021e-16, - "velocityX": 0.2978084543022323, - "velocityY": -0.5158193737750199, - "timestamp": 0.7628023155276963 - }, - { - "x": 4.342198153981216, - "y": 5.015560974703484, - "heading": -1.0471975512, - "angularVelocity": 5.035003607050494e-16, - "velocityX": 0.2605824443040005, - "velocityY": -0.45134203309501547, - "timestamp": 0.8581526049686583 - }, - { - "x": 4.363495250660627, - "y": 4.978673321201038, - "heading": -1.0471975512, - "angularVelocity": 4.229927708554212e-16, - "velocityX": 0.22335639256341489, - "velocityY": -0.3868646201151334, - "timestamp": 0.9535028944096203 - }, - { - "x": 4.381242831735798, - "y": 4.947933609067394, - "heading": -1.0471975512, - "angularVelocity": 3.364084361912933e-16, - "velocityX": 0.1861303324743397, - "velocityY": -0.32238719267524324, - "timestamp": 1.0488531838505823 - }, - { - "x": 4.395440896865573, - "y": 4.923341838893452, - "heading": -1.0471975512, - "angularVelocity": 2.5077961884006393e-16, - "velocityX": 0.14890426880733948, - "velocityY": -0.2579097590382052, - "timestamp": 1.1442034732915445 - }, - { - "x": 4.406089445860419, - "y": 4.90489801100749, - "heading": -1.0471975512, - "angularVelocity": 1.7002215965852864e-16, - "velocityX": 0.11167820315260296, - "velocityY": -0.19343232195830684, - "timestamp": 1.2395537627325066 - }, - { - "x": 4.413188478599729, - "y": 4.892602125618413, - "heading": -1.0471975512, - "angularVelocity": 1.1674982705623592e-16, - "velocityX": 0.07445213623294329, - "velocityY": -0.12895488268749733, - "timestamp": 1.3349040521734687 - }, - { - "x": 4.416737995, - "y": 4.886454182870843, - "heading": -1.0471975512, - "angularVelocity": 6.969332401174355e-17, - "velocityX": 0.037226068437567546, - "velocityY": -0.06447744189990308, - "timestamp": 1.4302543416144309 - }, - { - "x": 4.416737995, - "y": 4.886454182870843, - "heading": -1.0471975512, - "angularVelocity": 7.023845548835228e-34, - "velocityX": 1.047712734399832e-33, - "velocityY": 1.3991585734206156e-34, - "timestamp": 1.525604631055393 - } - ] + "samples": [ + { + "x": 4.189568967671509, + "y": 5.279922480109792, + "heading": -1.0471975512, + "angularVelocity": -8.682191393976642e-30, + "velocityX": -8.883974143721003e-20, + "velocityY": 1.5386233886481345e-19, + "timestamp": 0 + }, + { + "x": 4.208499721112847, + "y": 5.247133453323834, + "heading": -1.0471975512, + "angularVelocity": -9.594003781408746e-18, + "velocityX": 0.2032139569887226, + "velocityY": -0.35197689831158363, + "timestamp": 0.09315675813738096 + }, + { + "x": 4.246361226831558, + "y": 5.181555401767972, + "heading": -1.0471975512, + "angularVelocity": -1.757511495393616e-18, + "velocityX": 0.4064279014827589, + "velocityY": -0.7039537749817357, + "timestamp": 0.18631351627476192 + }, + { + "x": 4.303153481335755, + "y": 5.083188331490316, + "heading": -1.0471975512, + "angularVelocity": -4.4708927110588736e-18, + "velocityX": 0.6096418084927427, + "velocityY": -1.0559305867276043, + "timestamp": 0.2794702744121429 + }, + { + "x": 4.35994573583996, + "y": 4.984821261212667, + "heading": -1.0471975512, + "angularVelocity": -2.3203957666119342e-17, + "velocityX": 0.6096418084927429, + "velocityY": -1.055930586727604, + "timestamp": 0.37262703254952384 + }, + { + "x": 4.397807241558656, + "y": 4.919243209656798, + "heading": -1.0471975512, + "angularVelocity": 3.512195891842984e-17, + "velocityX": 0.406427901482759, + "velocityY": -0.7039537749817355, + "timestamp": 0.4657837906869048 + }, + { + "x": 4.416737995000001, + "y": 4.8864541828708425, + "heading": -1.0471975512, + "angularVelocity": 3.9044172518056536e-18, + "velocityX": 0.20321395698872258, + "velocityY": -0.35197689831158346, + "timestamp": 0.5589405488242858 + }, + { + "x": 4.416737995, + "y": 4.886454182870843, + "heading": -1.0471975512, + "angularVelocity": -6.644908206829601e-29, + "velocityX": -6.863436538799463e-20, + "velocityY": 1.1889252306500327e-19, + "timestamp": 0.6520973069616668 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Tag15.2.traj b/src/main/deploy/choreo/Tag15.2.traj index bcad5eea..193d2a2d 100644 --- a/src/main/deploy/choreo/Tag15.2.traj +++ b/src/main/deploy/choreo/Tag15.2.traj @@ -1,67 +1,41 @@ { - "samples": [ - { - "x": 4.416737995, - "y": 4.886454182870843, - "heading": -1.0471975512, - "angularVelocity": 7.023845548835228e-34, - "velocityX": 1.047712734399832e-33, - "velocityY": 1.3991585734206156e-34, - "timestamp": 0 - }, - { - "x": 4.413626883609419, - "y": 4.891842785867335, - "heading": -1.0471975512, - "angularVelocity": 3.2593553049819007e-18, - "velocityX": -0.03485141001299148, - "velocityY": 0.060364412857915745, - "timestamp": 0.08926787723713048 - }, - { - "x": 4.407404661107727, - "y": 4.902619991376263, - "heading": -1.0471975512, - "angularVelocity": -2.840644301428943e-19, - "velocityX": -0.06970281689529834, - "velocityY": 0.12072882029332666, - "timestamp": 0.17853575447426095 - }, - { - "x": 4.398071328892272, - "y": 4.918785796977351, - "heading": -1.0471975512, - "angularVelocity": 7.063784329206859e-19, - "velocityX": -0.10455420812418867, - "velocityY": 0.1810932006162248, - "timestamp": 0.26780363171139143 - }, - { - "x": 4.3918491063905805, - "y": 4.9295630024862795, - "heading": -1.0471975512, - "angularVelocity": -2.9031033307640906e-18, - "velocityX": -0.06970281689529835, - "velocityY": 0.12072882029332664, - "timestamp": 0.3570715089485219 - }, - { - "x": 4.388737995, - "y": 4.934951605482771, - "heading": -1.0471975512, - "angularVelocity": -7.785659769863438e-19, - "velocityX": -0.03485141001299149, - "velocityY": 0.06036441285791574, - "timestamp": 0.4463393861856524 - }, - { - "x": 4.388737995, - "y": 4.934951605482771, - "heading": -1.0471975512, - "angularVelocity": 0, - "velocityX": 2.2438461971651043e-34, - "velocityY": -1.8871387146521994e-34, - "timestamp": 0.5356072634227826 - } - ] + "samples": [ + { + "x": 4.416737995, + "y": 4.886454182870843, + "heading": -1.0471975512, + "angularVelocity": -6.644908206829601e-29, + "velocityX": -6.863436538799463e-20, + "velocityY": 1.1889252306500327e-19, + "timestamp": 0 + }, + { + "x": 4.402737994999999, + "y": 4.910702894176807, + "heading": -1.0471975512, + "angularVelocity": 2.5156332234400082e-17, + "velocityX": -0.1747566018550504, + "velocityY": 0.30268731337101645, + "timestamp": 0.08011142269527871 + }, + { + "x": 4.388737995, + "y": 4.934951605482771, + "heading": -1.0471975512, + "angularVelocity": -2.5156340257881864e-17, + "velocityX": -0.1747566018550504, + "velocityY": 0.3026873133710164, + "timestamp": 0.1602228453905573 + }, + { + "x": 4.388737995, + "y": 4.934951605482771, + "heading": -1.0471975512, + "angularVelocity": -1.0978097374367874e-28, + "velocityX": 1.0569070815560555e-19, + "velocityY": -1.830330405016386e-19, + "timestamp": 0.2403342680858359 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Tag15.traj b/src/main/deploy/choreo/Tag15.traj index f9b9c715..0ced3c35 100644 --- a/src/main/deploy/choreo/Tag15.traj +++ b/src/main/deploy/choreo/Tag15.traj @@ -1,211 +1,104 @@ { - "samples": [ - { - "x": 4.189568967671509, - "y": 5.279922480109792, - "heading": -1.0471975512, - "angularVelocity": 8.220687424023247e-35, - "velocityX": -1.8495013172516143e-33, - "velocityY": -1.5997840520692296e-33, - "timestamp": 0 - }, - { - "x": 4.19311848407178, - "y": 5.273774537362223, - "heading": -1.0471975512, - "angularVelocity": 7.45114074725589e-17, - "velocityX": 0.03722606843756754, - "velocityY": -0.06447744189990307, - "timestamp": 0.09535028944096205 - }, - { - "x": 4.200217516811089, - "y": 5.261478651973145, - "heading": -1.0471975512, - "angularVelocity": 1.61376783975804e-16, - "velocityX": 0.07445213623294329, - "velocityY": -0.1289548826874973, - "timestamp": 0.1907005788819241 - }, - { - "x": 4.210866065805936, - "y": 5.243034824087183, - "heading": -1.0471975512, - "angularVelocity": 2.4319200253214066e-16, - "velocityX": 0.11167820315260298, - "velocityY": -0.1934323219583068, - "timestamp": 0.2860508683228862 - }, - { - "x": 4.225064130935711, - "y": 5.218443053913242, - "heading": -1.0471975512, - "angularVelocity": 3.169122315246911e-16, - "velocityX": 0.14890426880733948, - "velocityY": -0.2579097590382051, - "timestamp": 0.3814011577638482 - }, - { - "x": 4.242811712010882, - "y": 5.1877033417795975, - "heading": -1.0471975512, - "angularVelocity": 3.710180313583571e-16, - "velocityX": 0.1861303324743397, - "velocityY": -0.3223871926752432, - "timestamp": 0.4767514472048102 - }, - { - "x": 4.264108808690293, - "y": 5.150815688277151, - "heading": -1.0471975512, - "angularVelocity": 4.524900551124174e-16, - "velocityX": 0.22335639256341489, - "velocityY": -0.38686462011513334, - "timestamp": 0.5721017366457722 - }, - { - "x": 4.2889554201779125, - "y": 5.10778009478467, - "heading": -1.0471975512, - "angularVelocity": 5.323391559336802e-16, - "velocityX": 0.2605824443040005, - "velocityY": -0.4513420330950154, - "timestamp": 0.6674520260867343 - }, - { - "x": 4.317351542493596, - "y": 5.0585965681959655, - "heading": -1.0471975512, - "angularVelocity": 5.803718444019021e-16, - "velocityX": 0.2978084543022323, - "velocityY": -0.5158193737750199, - "timestamp": 0.7628023155276963 - }, - { - "x": 4.342198153981216, - "y": 5.015560974703484, - "heading": -1.0471975512, - "angularVelocity": 5.035003607050494e-16, - "velocityX": 0.2605824443040005, - "velocityY": -0.45134203309501547, - "timestamp": 0.8581526049686583 - }, - { - "x": 4.363495250660627, - "y": 4.978673321201038, - "heading": -1.0471975512, - "angularVelocity": 4.229927708554212e-16, - "velocityX": 0.22335639256341489, - "velocityY": -0.3868646201151334, - "timestamp": 0.9535028944096203 - }, - { - "x": 4.381242831735798, - "y": 4.947933609067394, - "heading": -1.0471975512, - "angularVelocity": 3.364084361912933e-16, - "velocityX": 0.1861303324743397, - "velocityY": -0.32238719267524324, - "timestamp": 1.0488531838505823 - }, - { - "x": 4.395440896865573, - "y": 4.923341838893452, - "heading": -1.0471975512, - "angularVelocity": 2.5077961884006393e-16, - "velocityX": 0.14890426880733948, - "velocityY": -0.2579097590382052, - "timestamp": 1.1442034732915445 - }, - { - "x": 4.406089445860419, - "y": 4.90489801100749, - "heading": -1.0471975512, - "angularVelocity": 1.7002215965852864e-16, - "velocityX": 0.11167820315260296, - "velocityY": -0.19343232195830684, - "timestamp": 1.2395537627325066 - }, - { - "x": 4.413188478599729, - "y": 4.892602125618413, - "heading": -1.0471975512, - "angularVelocity": 1.1674982705623592e-16, - "velocityX": 0.07445213623294329, - "velocityY": -0.12895488268749733, - "timestamp": 1.3349040521734687 - }, - { - "x": 4.416737995, - "y": 4.886454182870843, - "heading": -1.0471975512, - "angularVelocity": 6.969332401174355e-17, - "velocityX": 0.037226068437567546, - "velocityY": -0.06447744189990308, - "timestamp": 1.4302543416144309 - }, - { - "x": 4.416737995, - "y": 4.886454182870843, - "heading": -1.0471975512, - "angularVelocity": 7.023845548835228e-34, - "velocityX": 1.047712734399832e-33, - "velocityY": 1.3991585734206156e-34, - "timestamp": 1.525604631055393 - }, - { - "x": 4.413626883609419, - "y": 4.891842785867335, - "heading": -1.0471975512, - "angularVelocity": 3.2593553049819007e-18, - "velocityX": -0.03485141001299148, - "velocityY": 0.060364412857915745, - "timestamp": 1.6148725082925235 - }, - { - "x": 4.407404661107727, - "y": 4.902619991376263, - "heading": -1.0471975512, - "angularVelocity": -2.840644301428943e-19, - "velocityX": -0.06970281689529834, - "velocityY": 0.12072882029332666, - "timestamp": 1.704140385529654 - }, - { - "x": 4.398071328892272, - "y": 4.918785796977351, - "heading": -1.0471975512, - "angularVelocity": 7.063784329206859e-19, - "velocityX": -0.10455420812418867, - "velocityY": 0.1810932006162248, - "timestamp": 1.7934082627667844 - }, - { - "x": 4.3918491063905805, - "y": 4.9295630024862795, - "heading": -1.0471975512, - "angularVelocity": -2.9031033307640906e-18, - "velocityX": -0.06970281689529835, - "velocityY": 0.12072882029332664, - "timestamp": 1.882676140003915 - }, - { - "x": 4.388737995, - "y": 4.934951605482771, - "heading": -1.0471975512, - "angularVelocity": -7.785659769863438e-19, - "velocityX": -0.03485141001299149, - "velocityY": 0.06036441285791574, - "timestamp": 1.9719440172410454 - }, - { - "x": 4.388737995, - "y": 4.934951605482771, - "heading": -1.0471975512, - "angularVelocity": 0, - "velocityX": 2.2438461971651043e-34, - "velocityY": -1.8871387146521994e-34, - "timestamp": 2.0612118944781757 - } - ] + "samples": [ + { + "x": 4.189568967671509, + "y": 5.279922480109792, + "heading": -1.0471975512, + "angularVelocity": -8.682191393976642e-30, + "velocityX": -8.883974143721003e-20, + "velocityY": 1.5386233886481345e-19, + "timestamp": 0 + }, + { + "x": 4.208499721112847, + "y": 5.247133453323834, + "heading": -1.0471975512, + "angularVelocity": -9.594003781408746e-18, + "velocityX": 0.2032139569887226, + "velocityY": -0.35197689831158363, + "timestamp": 0.09315675813738096 + }, + { + "x": 4.246361226831558, + "y": 5.181555401767972, + "heading": -1.0471975512, + "angularVelocity": -1.757511495393616e-18, + "velocityX": 0.4064279014827589, + "velocityY": -0.7039537749817357, + "timestamp": 0.18631351627476192 + }, + { + "x": 4.303153481335755, + "y": 5.083188331490316, + "heading": -1.0471975512, + "angularVelocity": -4.4708927110588736e-18, + "velocityX": 0.6096418084927427, + "velocityY": -1.0559305867276043, + "timestamp": 0.2794702744121429 + }, + { + "x": 4.35994573583996, + "y": 4.984821261212667, + "heading": -1.0471975512, + "angularVelocity": -2.3203957666119342e-17, + "velocityX": 0.6096418084927429, + "velocityY": -1.055930586727604, + "timestamp": 0.37262703254952384 + }, + { + "x": 4.397807241558656, + "y": 4.919243209656798, + "heading": -1.0471975512, + "angularVelocity": 3.512195891842984e-17, + "velocityX": 0.406427901482759, + "velocityY": -0.7039537749817355, + "timestamp": 0.4657837906869048 + }, + { + "x": 4.416737995000001, + "y": 4.8864541828708425, + "heading": -1.0471975512, + "angularVelocity": 3.9044172518056536e-18, + "velocityX": 0.20321395698872258, + "velocityY": -0.35197689831158346, + "timestamp": 0.5589405488242858 + }, + { + "x": 4.416737995, + "y": 4.886454182870843, + "heading": -1.0471975512, + "angularVelocity": -6.644908206829601e-29, + "velocityX": -6.863436538799463e-20, + "velocityY": 1.1889252306500327e-19, + "timestamp": 0.6520973069616668 + }, + { + "x": 4.402737994999999, + "y": 4.910702894176807, + "heading": -1.0471975512, + "angularVelocity": 2.5156332234400082e-17, + "velocityX": -0.1747566018550504, + "velocityY": 0.30268731337101645, + "timestamp": 0.7322087296569455 + }, + { + "x": 4.388737995, + "y": 4.934951605482771, + "heading": -1.0471975512, + "angularVelocity": -2.5156340257881864e-17, + "velocityX": -0.1747566018550504, + "velocityY": 0.3026873133710164, + "timestamp": 0.8123201523522241 + }, + { + "x": 4.388737995, + "y": 4.934951605482771, + "heading": -1.0471975512, + "angularVelocity": -1.0978097374367874e-28, + "velocityX": 1.0569070815560555e-19, + "velocityY": -1.830330405016386e-19, + "timestamp": 0.8924315750475027 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Tag16.1.traj b/src/main/deploy/choreo/Tag16.1.traj index a1adaf54..565d5958 100644 --- a/src/main/deploy/choreo/Tag16.1.traj +++ b/src/main/deploy/choreo/Tag16.1.traj @@ -1,157 +1,77 @@ { - "samples": [ - { - "x": 4.189568967671509, - "y": 2.930373519890208, - "heading": 1.0471975512, - "angularVelocity": 1.1450146397244414e-36, - "velocityX": 3.551739543404917e-34, - "velocityY": -2.349922304718694e-34, - "timestamp": 0 - }, - { - "x": 4.19311848407178, - "y": 2.9365214626377765, - "heading": 1.0471975512, - "angularVelocity": 2.0807829607167273e-17, - "velocityX": 0.03722606843756739, - "velocityY": 0.0644774418999032, - "timestamp": 0.0953502894409621 - }, - { - "x": 4.200217516811089, - "y": 2.948817348026855, - "heading": 1.0471975512, - "angularVelocity": 6.456177548720404e-17, - "velocityX": 0.07445213623294297, - "velocityY": 0.1289548826874976, - "timestamp": 0.1907005788819242 - }, - { - "x": 4.210866065805936, - "y": 2.967261175912817, - "heading": 1.0471975512, - "angularVelocity": 1.2599339745541e-16, - "velocityX": 0.11167820315260249, - "velocityY": 0.19343232195830726, - "timestamp": 0.2860508683228863 - }, - { - "x": 4.225064130935711, - "y": 2.991852946086759, - "heading": 1.0471975512, - "angularVelocity": 1.4686476867569292e-16, - "velocityX": 0.1489042688073388, - "velocityY": 0.2579097590382057, - "timestamp": 0.3814011577638484 - }, - { - "x": 4.242811712010882, - "y": 3.0225926582204026, - "heading": 1.0471975512, - "angularVelocity": 1.7718661382769725e-16, - "velocityX": 0.18613033247433888, - "velocityY": 0.32238719267524385, - "timestamp": 0.47675144720481055 - }, - { - "x": 4.264108808690293, - "y": 3.059480311722848, - "heading": 1.0471975512, - "angularVelocity": 2.0209928205397637e-16, - "velocityX": 0.2233563925634139, - "velocityY": 0.3868646201151341, - "timestamp": 0.5721017366457727 - }, - { - "x": 4.2889554201779125, - "y": 3.1025159052153306, - "heading": 1.0471975512, - "angularVelocity": 2.386366328775348e-16, - "velocityX": 0.2605824443039994, - "velocityY": 0.45134203309501636, - "timestamp": 0.6674520260867348 - }, - { - "x": 4.317351542493596, - "y": 3.1516994318040346, - "heading": 1.0471975512, - "angularVelocity": 2.693541370963743e-16, - "velocityX": 0.297808454302231, - "velocityY": 0.515819373775021, - "timestamp": 0.762802315527697 - }, - { - "x": 4.342198153981216, - "y": 3.194735025296517, - "heading": 1.0471975512, - "angularVelocity": 2.483620297451855e-16, - "velocityX": 0.2605824443039994, - "velocityY": 0.45134203309501647, - "timestamp": 0.8581526049686591 - }, - { - "x": 4.363495250660627, - "y": 3.2316226787989626, - "heading": 1.0471975512, - "angularVelocity": 2.1567920172697482e-16, - "velocityX": 0.22335639256341389, - "velocityY": 0.38686462011513423, - "timestamp": 0.9535028944096212 - }, - { - "x": 4.381242831735798, - "y": 3.2623623909326063, - "heading": 1.0471975512, - "angularVelocity": 1.7766713056448412e-16, - "velocityX": 0.18613033247433886, - "velocityY": 0.32238719267524396, - "timestamp": 1.0488531838505832 - }, - { - "x": 4.395440896865573, - "y": 3.2869541611065483, - "heading": 1.0471975512, - "angularVelocity": 1.4244354178358303e-16, - "velocityX": 0.1489042688073388, - "velocityY": 0.25790975903820573, - "timestamp": 1.1442034732915454 - }, - { - "x": 4.406089445860419, - "y": 3.3053979889925102, - "heading": 1.0471975512, - "angularVelocity": 1.228103737524146e-16, - "velocityX": 0.11167820315260248, - "velocityY": 0.19343232195830726, - "timestamp": 1.2395537627325075 - }, - { - "x": 4.413188478599729, - "y": 3.3176938743815887, - "heading": 1.0471975512, - "angularVelocity": 1.00554048636381e-16, - "velocityX": 0.07445213623294296, - "velocityY": 0.1289548826874976, - "timestamp": 1.3349040521734696 - }, - { - "x": 4.416737995, - "y": 3.323841817129157, - "heading": 1.0471975512, - "angularVelocity": 4.815824152485883e-17, - "velocityX": 0.03722606843756737, - "velocityY": 0.06447744189990322, - "timestamp": 1.4302543416144318 - }, - { - "x": 4.416737995, - "y": 3.323841817129157, - "heading": 1.0471975512, - "angularVelocity": 5.809967250206149e-34, - "velocityX": -3.837623152452288e-34, - "velocityY": -8.025233082712604e-35, - "timestamp": 1.525604631055394 - } - ] + "samples": [ + { + "x": 4.189568967671509, + "y": 2.930373519890208, + "heading": 1.0471975512, + "angularVelocity": -2.002499149483344e-30, + "velocityX": -8.884646650147337e-20, + "velocityY": -1.5387579641415467e-19, + "timestamp": 0 + }, + { + "x": 4.208499721112847, + "y": 2.9631625466761657, + "heading": 1.0471975512, + "angularVelocity": -7.928469907031793e-18, + "velocityX": 0.20321395698872408, + "velocityY": 0.35197689831158385, + "timestamp": 0.0931567581373812 + }, + { + "x": 4.2463612268315565, + "y": 3.0287405982320337, + "heading": 1.0471975512, + "angularVelocity": -2.0524140454745182e-17, + "velocityX": 0.40642790148276187, + "velocityY": 0.7039537749817363, + "timestamp": 0.1863135162747624 + }, + { + "x": 4.303153481335761, + "y": 3.127107668509687, + "heading": 1.0471975512, + "angularVelocity": 5.366441184028338e-17, + "velocityX": 0.6096418084927471, + "velocityY": 1.055930586727605, + "timestamp": 0.27947027441214356 + }, + { + "x": 4.3599457358399505, + "y": 3.225474738787313, + "heading": 1.0471975512, + "angularVelocity": -4.609538572640182e-17, + "velocityX": 0.6096418084927472, + "velocityY": 1.0559305867276048, + "timestamp": 0.3726270325495248 + }, + { + "x": 4.397807241558657, + "y": 3.291052790343219, + "heading": 1.0471975512, + "angularVelocity": 9.988825598390611e-18, + "velocityX": 0.4064279014827619, + "velocityY": 0.7039537749817361, + "timestamp": 0.465783790686906 + }, + { + "x": 4.416737994999997, + "y": 3.323841817129139, + "heading": 1.0471975512, + "angularVelocity": 1.0894764403980658e-17, + "velocityX": 0.20321395698872408, + "velocityY": 0.35197689831158374, + "timestamp": 0.5589405488242872 + }, + { + "x": 4.416737995, + "y": 3.323841817129157, + "heading": 1.0471975512, + "angularVelocity": -2.786574070680907e-28, + "velocityX": -6.865066631576706e-20, + "velocityY": -1.1890733865081739e-19, + "timestamp": 0.6520973069616685 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Tag16.2.traj b/src/main/deploy/choreo/Tag16.2.traj index c29e0073..c8eda2ba 100644 --- a/src/main/deploy/choreo/Tag16.2.traj +++ b/src/main/deploy/choreo/Tag16.2.traj @@ -1,67 +1,41 @@ { - "samples": [ - { - "x": 4.416737995, - "y": 3.323841817129157, - "heading": 1.0471975512, - "angularVelocity": 5.809967250206149e-34, - "velocityX": -3.837623152452288e-34, - "velocityY": -8.025233082712604e-35, - "timestamp": 0 - }, - { - "x": 4.413626883609419, - "y": 3.3184532141326653, - "heading": 1.0471975512, - "angularVelocity": -1.5491376688497814e-18, - "velocityX": -0.03485141001299182, - "velocityY": -0.06036441285791556, - "timestamp": 0.08926787723713048 - }, - { - "x": 4.407404661107727, - "y": 3.307676008623737, - "heading": 1.0471975512, - "angularVelocity": -2.100497225592737e-18, - "velocityX": -0.06970281689529902, - "velocityY": -0.12072882029332627, - "timestamp": 0.17853575447426095 - }, - { - "x": 4.398071328892272, - "y": 3.291510203022649, - "heading": 1.0471975512, - "angularVelocity": 2.337039351447479e-18, - "velocityX": -0.10455420812418968, - "velocityY": -0.18109320061622425, - "timestamp": 0.26780363171139143 - }, - { - "x": 4.3918491063905805, - "y": 3.280732997513721, - "heading": 1.0471975512, - "angularVelocity": 5.855066236692621e-18, - "velocityX": -0.06970281689529902, - "velocityY": -0.12072882029332627, - "timestamp": 0.3570715089485219 - }, - { - "x": 4.388737995, - "y": 3.275344394517229, - "heading": 1.0471975512, - "angularVelocity": -4.542470693700315e-18, - "velocityX": -0.03485141001299182, - "velocityY": -0.06036441285791555, - "timestamp": 0.4463393861856524 - }, - { - "x": 4.388737995, - "y": 3.275344394517229, - "heading": 1.0471975512, - "angularVelocity": 0, - "velocityX": 8.236649016847915e-35, - "velocityY": -4.795525817613878e-35, - "timestamp": 0.5356072634227826 - } - ] + "samples": [ + { + "x": 4.416737995, + "y": 3.323841817129157, + "heading": 1.0471975512, + "angularVelocity": -2.786574070680907e-28, + "velocityX": -6.865066631576706e-20, + "velocityY": -1.1890733865081739e-19, + "timestamp": 0 + }, + { + "x": 4.402737994999996, + "y": 3.299593105823197, + "heading": 1.0471975512, + "angularVelocity": 1.2203521575276725e-17, + "velocityX": -0.17475660185504371, + "velocityY": -0.30268731337102, + "timestamp": 0.0801114226952786 + }, + { + "x": 4.388737995, + "y": 3.275344394517229, + "heading": 1.0471975512, + "angularVelocity": -1.2203535086762994e-17, + "velocityX": -0.17475660185504374, + "velocityY": -0.30268731337102006, + "timestamp": 0.1602228453905572 + }, + { + "x": 4.388737995, + "y": 3.275344394517229, + "heading": 1.0471975512, + "angularVelocity": -5.52100607399031e-28, + "velocityX": 1.0568999281101467e-19, + "velocityY": 1.8303406915772078e-19, + "timestamp": 0.2403342680858358 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Tag16.traj b/src/main/deploy/choreo/Tag16.traj index bb1f7b10..60c8c0e0 100644 --- a/src/main/deploy/choreo/Tag16.traj +++ b/src/main/deploy/choreo/Tag16.traj @@ -1,211 +1,104 @@ { - "samples": [ - { - "x": 4.189568967671509, - "y": 2.930373519890208, - "heading": 1.0471975512, - "angularVelocity": 1.1450146397244414e-36, - "velocityX": 3.551739543404917e-34, - "velocityY": -2.349922304718694e-34, - "timestamp": 0 - }, - { - "x": 4.19311848407178, - "y": 2.9365214626377765, - "heading": 1.0471975512, - "angularVelocity": 2.0807829607167273e-17, - "velocityX": 0.03722606843756739, - "velocityY": 0.0644774418999032, - "timestamp": 0.0953502894409621 - }, - { - "x": 4.200217516811089, - "y": 2.948817348026855, - "heading": 1.0471975512, - "angularVelocity": 6.456177548720404e-17, - "velocityX": 0.07445213623294297, - "velocityY": 0.1289548826874976, - "timestamp": 0.1907005788819242 - }, - { - "x": 4.210866065805936, - "y": 2.967261175912817, - "heading": 1.0471975512, - "angularVelocity": 1.2599339745541e-16, - "velocityX": 0.11167820315260249, - "velocityY": 0.19343232195830726, - "timestamp": 0.2860508683228863 - }, - { - "x": 4.225064130935711, - "y": 2.991852946086759, - "heading": 1.0471975512, - "angularVelocity": 1.4686476867569292e-16, - "velocityX": 0.1489042688073388, - "velocityY": 0.2579097590382057, - "timestamp": 0.3814011577638484 - }, - { - "x": 4.242811712010882, - "y": 3.0225926582204026, - "heading": 1.0471975512, - "angularVelocity": 1.7718661382769725e-16, - "velocityX": 0.18613033247433888, - "velocityY": 0.32238719267524385, - "timestamp": 0.47675144720481055 - }, - { - "x": 4.264108808690293, - "y": 3.059480311722848, - "heading": 1.0471975512, - "angularVelocity": 2.0209928205397637e-16, - "velocityX": 0.2233563925634139, - "velocityY": 0.3868646201151341, - "timestamp": 0.5721017366457727 - }, - { - "x": 4.2889554201779125, - "y": 3.1025159052153306, - "heading": 1.0471975512, - "angularVelocity": 2.386366328775348e-16, - "velocityX": 0.2605824443039994, - "velocityY": 0.45134203309501636, - "timestamp": 0.6674520260867348 - }, - { - "x": 4.317351542493596, - "y": 3.1516994318040346, - "heading": 1.0471975512, - "angularVelocity": 2.693541370963743e-16, - "velocityX": 0.297808454302231, - "velocityY": 0.515819373775021, - "timestamp": 0.762802315527697 - }, - { - "x": 4.342198153981216, - "y": 3.194735025296517, - "heading": 1.0471975512, - "angularVelocity": 2.483620297451855e-16, - "velocityX": 0.2605824443039994, - "velocityY": 0.45134203309501647, - "timestamp": 0.8581526049686591 - }, - { - "x": 4.363495250660627, - "y": 3.2316226787989626, - "heading": 1.0471975512, - "angularVelocity": 2.1567920172697482e-16, - "velocityX": 0.22335639256341389, - "velocityY": 0.38686462011513423, - "timestamp": 0.9535028944096212 - }, - { - "x": 4.381242831735798, - "y": 3.2623623909326063, - "heading": 1.0471975512, - "angularVelocity": 1.7766713056448412e-16, - "velocityX": 0.18613033247433886, - "velocityY": 0.32238719267524396, - "timestamp": 1.0488531838505832 - }, - { - "x": 4.395440896865573, - "y": 3.2869541611065483, - "heading": 1.0471975512, - "angularVelocity": 1.4244354178358303e-16, - "velocityX": 0.1489042688073388, - "velocityY": 0.25790975903820573, - "timestamp": 1.1442034732915454 - }, - { - "x": 4.406089445860419, - "y": 3.3053979889925102, - "heading": 1.0471975512, - "angularVelocity": 1.228103737524146e-16, - "velocityX": 0.11167820315260248, - "velocityY": 0.19343232195830726, - "timestamp": 1.2395537627325075 - }, - { - "x": 4.413188478599729, - "y": 3.3176938743815887, - "heading": 1.0471975512, - "angularVelocity": 1.00554048636381e-16, - "velocityX": 0.07445213623294296, - "velocityY": 0.1289548826874976, - "timestamp": 1.3349040521734696 - }, - { - "x": 4.416737995, - "y": 3.323841817129157, - "heading": 1.0471975512, - "angularVelocity": 4.815824152485883e-17, - "velocityX": 0.03722606843756737, - "velocityY": 0.06447744189990322, - "timestamp": 1.4302543416144318 - }, - { - "x": 4.416737995, - "y": 3.323841817129157, - "heading": 1.0471975512, - "angularVelocity": 5.809967250206149e-34, - "velocityX": -3.837623152452288e-34, - "velocityY": -8.025233082712604e-35, - "timestamp": 1.525604631055394 - }, - { - "x": 4.413626883609419, - "y": 3.3184532141326653, - "heading": 1.0471975512, - "angularVelocity": -1.5491376688497814e-18, - "velocityX": -0.03485141001299182, - "velocityY": -0.06036441285791556, - "timestamp": 1.6148725082925244 - }, - { - "x": 4.407404661107727, - "y": 3.307676008623737, - "heading": 1.0471975512, - "angularVelocity": -2.100497225592737e-18, - "velocityX": -0.06970281689529902, - "velocityY": -0.12072882029332627, - "timestamp": 1.7041403855296549 - }, - { - "x": 4.398071328892272, - "y": 3.291510203022649, - "heading": 1.0471975512, - "angularVelocity": 2.337039351447479e-18, - "velocityX": -0.10455420812418968, - "velocityY": -0.18109320061622425, - "timestamp": 1.7934082627667853 - }, - { - "x": 4.3918491063905805, - "y": 3.280732997513721, - "heading": 1.0471975512, - "angularVelocity": 5.855066236692621e-18, - "velocityX": -0.06970281689529902, - "velocityY": -0.12072882029332627, - "timestamp": 1.8826761400039158 - }, - { - "x": 4.388737995, - "y": 3.275344394517229, - "heading": 1.0471975512, - "angularVelocity": -4.542470693700315e-18, - "velocityX": -0.03485141001299182, - "velocityY": -0.06036441285791555, - "timestamp": 1.9719440172410463 - }, - { - "x": 4.388737995, - "y": 3.275344394517229, - "heading": 1.0471975512, - "angularVelocity": 0, - "velocityX": 8.236649016847915e-35, - "velocityY": -4.795525817613878e-35, - "timestamp": 2.0612118944781765 - } - ] + "samples": [ + { + "x": 4.189568967671509, + "y": 2.930373519890208, + "heading": 1.0471975512, + "angularVelocity": -2.002499149483344e-30, + "velocityX": -8.884646650147337e-20, + "velocityY": -1.5387579641415467e-19, + "timestamp": 0 + }, + { + "x": 4.208499721112847, + "y": 2.9631625466761657, + "heading": 1.0471975512, + "angularVelocity": -7.928469907031793e-18, + "velocityX": 0.20321395698872408, + "velocityY": 0.35197689831158385, + "timestamp": 0.0931567581373812 + }, + { + "x": 4.2463612268315565, + "y": 3.0287405982320337, + "heading": 1.0471975512, + "angularVelocity": -2.0524140454745182e-17, + "velocityX": 0.40642790148276187, + "velocityY": 0.7039537749817363, + "timestamp": 0.1863135162747624 + }, + { + "x": 4.303153481335761, + "y": 3.127107668509687, + "heading": 1.0471975512, + "angularVelocity": 5.366441184028338e-17, + "velocityX": 0.6096418084927471, + "velocityY": 1.055930586727605, + "timestamp": 0.27947027441214356 + }, + { + "x": 4.3599457358399505, + "y": 3.225474738787313, + "heading": 1.0471975512, + "angularVelocity": -4.609538572640182e-17, + "velocityX": 0.6096418084927472, + "velocityY": 1.0559305867276048, + "timestamp": 0.3726270325495248 + }, + { + "x": 4.397807241558657, + "y": 3.291052790343219, + "heading": 1.0471975512, + "angularVelocity": 9.988825598390611e-18, + "velocityX": 0.4064279014827619, + "velocityY": 0.7039537749817361, + "timestamp": 0.465783790686906 + }, + { + "x": 4.416737994999997, + "y": 3.323841817129139, + "heading": 1.0471975512, + "angularVelocity": 1.0894764403980658e-17, + "velocityX": 0.20321395698872408, + "velocityY": 0.35197689831158374, + "timestamp": 0.5589405488242872 + }, + { + "x": 4.416737995, + "y": 3.323841817129157, + "heading": 1.0471975512, + "angularVelocity": -2.786574070680907e-28, + "velocityX": -6.865066631576706e-20, + "velocityY": -1.1890733865081739e-19, + "timestamp": 0.6520973069616685 + }, + { + "x": 4.402737994999996, + "y": 3.299593105823197, + "heading": 1.0471975512, + "angularVelocity": 1.2203521575276725e-17, + "velocityX": -0.17475660185504371, + "velocityY": -0.30268731337102, + "timestamp": 0.732208729656947 + }, + { + "x": 4.388737995, + "y": 3.275344394517229, + "heading": 1.0471975512, + "angularVelocity": -1.2203535086762994e-17, + "velocityX": -0.17475660185504374, + "velocityY": -0.30268731337102006, + "timestamp": 0.8123201523522257 + }, + { + "x": 4.388737995, + "y": 3.275344394517229, + "heading": 1.0471975512, + "angularVelocity": -5.52100607399031e-28, + "velocityX": 1.0568999281101467e-19, + "velocityY": 1.8303406915772078e-19, + "timestamp": 0.8924315750475043 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Taxi.1.traj b/src/main/deploy/choreo/Taxi.1.traj new file mode 100644 index 00000000..26e6bb6e --- /dev/null +++ b/src/main/deploy/choreo/Taxi.1.traj @@ -0,0 +1,140 @@ +{ + "samples": [ + { + "x": 1.468971848487854, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": -2.4295253407291496e-19, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.5097881789229493, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.4219894192635095, + "velocityY": 0, + "timestamp": 0.09672358730304476 + }, + { + "x": 1.5914208389651578, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.8439788299642234, + "velocityY": 0, + "timestamp": 0.1934471746060895 + }, + { + "x": 1.7138698274174076, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.2659682282964586, + "velocityY": 0, + "timestamp": 0.29017076190913427 + }, + { + "x": 1.877135142400635, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.6879576071925144, + "velocityY": 0, + "timestamp": 0.386894349212179 + }, + { + "x": 2.0812167805304824, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.109946951103447, + "velocityY": 0, + "timestamp": 0.4836179365152238 + }, + { + "x": 2.326114733911371, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.53193621338244, + "velocityY": 0, + "timestamp": 0.5803415238182685 + }, + { + "x": 2.611828963064364, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.9539250675023654, + "velocityY": 0, + "timestamp": 0.6770651111213133 + }, + { + "x": 2.856726916445444, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.5319362133824383, + "velocityY": 0, + "timestamp": 0.773788698424358 + }, + { + "x": 3.0608085545750643, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.109946951103445, + "velocityY": 0, + "timestamp": 0.8705122857274028 + }, + { + "x": 3.2240738695580573, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.6879576071925126, + "velocityY": 0, + "timestamp": 0.9672358730304476 + }, + { + "x": 3.3465228580110096, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.265968228296457, + "velocityY": 0, + "timestamp": 1.0639594603334923 + }, + { + "x": 3.428155518052592, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.8439788299642216, + "velocityY": 0, + "timestamp": 1.160683047636537 + }, + { + "x": 3.468971848487854, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.42198941926350725, + "velocityY": 0, + "timestamp": 1.2574066349395818 + }, + { + "x": 3.468971848487854, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": -3.3284599459506804e-19, + "velocityY": 0, + "timestamp": 1.3541302222426266 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Taxi.traj b/src/main/deploy/choreo/Taxi.traj new file mode 100644 index 00000000..26e6bb6e --- /dev/null +++ b/src/main/deploy/choreo/Taxi.traj @@ -0,0 +1,140 @@ +{ + "samples": [ + { + "x": 1.468971848487854, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": -2.4295253407291496e-19, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.5097881789229493, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.4219894192635095, + "velocityY": 0, + "timestamp": 0.09672358730304476 + }, + { + "x": 1.5914208389651578, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.8439788299642234, + "velocityY": 0, + "timestamp": 0.1934471746060895 + }, + { + "x": 1.7138698274174076, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.2659682282964586, + "velocityY": 0, + "timestamp": 0.29017076190913427 + }, + { + "x": 1.877135142400635, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.6879576071925144, + "velocityY": 0, + "timestamp": 0.386894349212179 + }, + { + "x": 2.0812167805304824, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.109946951103447, + "velocityY": 0, + "timestamp": 0.4836179365152238 + }, + { + "x": 2.326114733911371, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.53193621338244, + "velocityY": 0, + "timestamp": 0.5803415238182685 + }, + { + "x": 2.611828963064364, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.9539250675023654, + "velocityY": 0, + "timestamp": 0.6770651111213133 + }, + { + "x": 2.856726916445444, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.5319362133824383, + "velocityY": 0, + "timestamp": 0.773788698424358 + }, + { + "x": 3.0608085545750643, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.109946951103445, + "velocityY": 0, + "timestamp": 0.8705122857274028 + }, + { + "x": 3.2240738695580573, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.6879576071925126, + "velocityY": 0, + "timestamp": 0.9672358730304476 + }, + { + "x": 3.3465228580110096, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.265968228296457, + "velocityY": 0, + "timestamp": 1.0639594603334923 + }, + { + "x": 3.428155518052592, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.8439788299642216, + "velocityY": 0, + "timestamp": 1.160683047636537 + }, + { + "x": 3.468971848487854, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0.42198941926350725, + "velocityY": 0, + "timestamp": 1.2574066349395818 + }, + { + "x": 3.468971848487854, + "y": 1.9215065240859983, + "heading": 0, + "angularVelocity": 0, + "velocityX": -3.3284599459506804e-19, + "velocityY": 0, + "timestamp": 1.3541302222426266 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index e2f50b5d..bab0da93 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.540513081026162,"y":8.016256032512064},"nodeSizeMeters":0.2,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 1d14b117..443084b9 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -1,5 +1,6 @@ package org.team1540.robot2024; +import com.pathplanner.lib.util.GeometryUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -8,6 +9,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import org.team1540.robot2024.commands.FeedForwardCharacterization; @@ -106,27 +108,6 @@ public RobotContainer() { } } - // Set up FF characterization routines - AutoManager.getInstance().add( - new AutoCommand( - "Drive FF Characterization", - new FeedForwardCharacterization( - drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity - ) - ) - ); - - AutoManager.getInstance().add( - new AutoCommand( - "Flywheels FF Characterization", - new FeedForwardCharacterization( - shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60 - ) - ) - ); - - AutoManager.getInstance().addDefault(new AmpLanePABCSprint(drivetrain, shooter, indexer)); - AutoManager.getInstance().add(new SourceLanePHGFSprint(drivetrain)); configureAutoRoutines(); // Configure the button bindings @@ -145,12 +126,25 @@ private void configureLedBindings() { private void configureButtonBindings() { Command manualPivotCommand = new ManualPivotCommand(shooter, copilot); + Command defaultShooterCommand = new ConditionalCommand( + Commands.either( + new LeadingShootPrepare(drivetrain, shooter, 7000*0.75, 3000*0.75), + new LeadingShootPrepare(drivetrain, shooter, 0, 0), + indexer::isNoteStaged + ), + new OverStageShootPrepare(drivetrain::getPose, shooter, 0, 0), + ()->{ + Pose2d pose = DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Blue ? drivetrain.getPose() : GeometryUtil.flipFieldPose(drivetrain.getPose()); + return pose.getTranslation().getX() < Units.inchesToMeters(231.2); + } + + ); drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); // drivetrain.setDefaultCommand(new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter)); // drivetrain.setDefaultCommand(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter)); // drivetrain.setDefaultCommand(new DriveWithAmpSideLock(drivetrain, driver.getHID())); elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); - shooter.setDefaultCommand(manualPivotCommand); + shooter.setDefaultCommand(defaultShooterCommand); driver.b().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); // driver.y().toggleOnTrue(new DriveWithSpeakerTargetingCommand(drivetrain, driver)); driver.y().onTrue(Commands.runOnce(() -> { @@ -268,11 +262,10 @@ private void configureAutoRoutines() { } autos.addDefault(new AutoCommand("Dwayne :skull:")); autos.add(new AutoCommand("SubwooferShot", new ShootSequence(shooter, indexer))); - autos.add(new DriveSinglePath("AmpLaneTaxi", drivetrain)); - autos.add(new DriveSinglePath("AmpLaneSprint", drivetrain)); - autos.add(new AmpLanePADESprint(drivetrain, shooter, indexer)); - autos.add(new DriveSinglePath("CenterLaneTaxi", drivetrain)); + autos.add(new DriveSinglePath("Taxi", drivetrain)); + autos.add(new DriveSinglePath("Sprint", drivetrain)); autos.add(new DriveSinglePath("CenterLaneSprint", drivetrain, true, true)); + autos.add(new AmpLanePADESprint(drivetrain, shooter, indexer)); autos.add(new CenterLanePSubSprint(drivetrain, shooter, indexer)); autos.add(new CenterLanePCBADSprint(drivetrain, shooter, indexer)); autos.add(new CenterLanePCBAFSprint(drivetrain, shooter, indexer)); @@ -282,10 +275,8 @@ private void configureAutoRoutines() { autos.add(new CenterLanePSubCSubBSubASubFSub(drivetrain, shooter, indexer)); // autos.add(new CenterLanePSubCSubBSubFSub(drivetrain, shooter, indexer)); autos.add(new CenterLanePSubCSubBSubASub(drivetrain, shooter, indexer)); - autos.add(new DriveSinglePath("SourceLaneTaxi", drivetrain)); - autos.add(new DriveSinglePath("SourceLaneSprint", drivetrain)); autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer)); - autos.add(new SourceLanePHG(drivetrain, shooter, indexer)); +// autos.addDefault(new ATestAuto(drivetrain, shooter, indexer)); } diff --git a/src/main/java/org/team1540/robot2024/commands/autos/ATestAuto.java b/src/main/java/org/team1540/robot2024/commands/autos/ATestAuto.java new file mode 100644 index 00000000..a2146f03 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/ATestAuto.java @@ -0,0 +1,58 @@ +package org.team1540.robot2024.commands.autos; + +import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.wpilibj2.command.*; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.commands.shooter.ShootSequence; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class ATestAuto extends AutoCommand { + + public ATestAuto(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("!ATestAuto"); + addPath( + PathHelper.fromChoreoPath("CenterLaneSprint.1", true, true), + PathHelper.fromChoreoPath("CenterLaneSprint.1", false, true) + ); + + addCommands( + ShootSequence.forAutoSubwoofer(shooter, indexer), + Commands.parallel( + new AutoShootPrepare(drivetrain, shooter), + Commands.sequence( + drivetrain.commandCopyVisionPose(), +// Commands.deadline( + Commands.race( + getPath(0).getCommand(drivetrain), + Commands.sequence( + Commands.waitSeconds(1), + Commands.either( +// Commands.waitSeconds(0), + Commands.runOnce(()->{}), + Commands.waitSeconds(100), + ()->false + ) + ) + ), +// Commands.print("YO YO DIGGITY DAWG"), +// Commands.runOnce(drivetrain::stop) +// Commands.sequence( +// Commands.waitSeconds(1) +// Commands.runOnce(()->CommandScheduler.getInstance().cancelAll()) +// ) +// ), +// Commands.runOnce(drivetrain::stop) + drivetrain.commandStop(), + getPath(1).getCommand(drivetrain,true) + + ) + ) + ); + + addRequirements(drivetrain, shooter, indexer); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java deleted file mode 100644 index ddcf9f85..00000000 --- a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.team1540.robot2024.commands.autos; - -import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.commands.indexer.IntakeAndFeed; -import org.team1540.robot2024.commands.shooter.AutoShootPrepare; -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.subsystems.indexer.Indexer; -import org.team1540.robot2024.subsystems.shooter.Shooter; -import org.team1540.robot2024.util.auto.AutoCommand; -import org.team1540.robot2024.util.auto.PathHelper; - -public class AmpLanePABCSprint extends AutoCommand { - public AmpLanePABCSprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ - super("!AmpLanePABCSprint"); - - addPath( - PathHelper.fromChoreoPath("AmpLanePABCSprint.1", true, true), - PathHelper.fromChoreoPath("AmpLanePABCSprint.2"), - PathHelper.fromChoreoPath("AmpLanePABCSprint.3"), - PathHelper.fromChoreoPath("AmpLanePABCSprint.4") -// PathHelper.fromChoreoPath("AmpLanePABCSprint.4") - ); - - addCommands( - Commands.parallel( - new AutoShootPrepare(drivetrain, shooter), - Commands.sequence( - createSegmentSequence(drivetrain, shooter, indexer, 0), - IntakeAndFeed.withDefaults(indexer).withTimeout(2), - createSegmentSequence(drivetrain, shooter, indexer, 1), - createSegmentSequence(drivetrain, shooter, indexer, 2) - ) - ), - getPath(3).getCommand(drivetrain) - ); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePSprint.java deleted file mode 100644 index a9c3e5b1..00000000 --- a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePSprint.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.team1540.robot2024.commands.autos; - -import org.team1540.robot2024.commands.shooter.ShootSequence; -import org.team1540.robot2024.subsystems.indexer.Indexer; -import org.team1540.robot2024.util.auto.AutoCommand; -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.subsystems.shooter.Shooter; -import org.team1540.robot2024.util.auto.PathHelper; - -public class AmpLanePSprint extends AutoCommand { - public AmpLanePSprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { - super("AmpLanePSprint"); - - addPath( - PathHelper.fromChoreoPath("AmpLanePSprint.1", true, true), - PathHelper.fromChoreoPath("AmpLanePSprint.2")); - - addCommands( - getPath(0).getCommand(drivetrain), - new ShootSequence(shooter, indexer), - getPath(1).getCommand(drivetrain) - ); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLaneSprintBonus.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLaneSprintBonus.java deleted file mode 100644 index 5fba986d..00000000 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLaneSprintBonus.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.team1540.robot2024.commands.autos; - -import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.util.auto.AutoCommand; -import org.team1540.robot2024.util.auto.PathHelper; - -public class CenterLaneSprintBonus extends AutoCommand { - public CenterLaneSprintBonus(Drivetrain drivetrain){ - super("CenterLaneSprintBonus"); - - addPath( - PathHelper.fromChoreoPath("CenterLaneSprintBonus.1", true, true), - PathHelper.fromChoreoPath("CenterLaneSprintBonus.2") - ); - - addCommands( - getPath(0).getCommand(drivetrain), - Commands.waitSeconds(2), - drivetrain.commandCopyVisionPose(), - Commands.waitSeconds(2), - getPath(1).getCommand(drivetrain), - Commands.runOnce(drivetrain::stop) - ); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/PathVisualising.java b/src/main/java/org/team1540/robot2024/commands/autos/PathVisualising.java deleted file mode 100644 index 5c074e6b..00000000 --- a/src/main/java/org/team1540/robot2024/commands/autos/PathVisualising.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.team1540.robot2024.commands.autos; - -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.subsystems.indexer.Indexer; -import org.team1540.robot2024.subsystems.shooter.Shooter; -import org.team1540.robot2024.util.auto.AutoCommand; -import org.team1540.robot2024.util.auto.PathHelper; - -public class PathVisualising extends AutoCommand { - - public PathVisualising(Drivetrain drivetrain){ - super("PathVisualising"); - - addPath( - PathHelper.fromChoreoPath("AmpLanePADESprint", true, true) - ); - addCommands( - getPath(0).getCommand(drivetrain) - ); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java deleted file mode 100644 index 5bf93935..00000000 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHG.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.team1540.robot2024.commands.autos; - -import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.commands.shooter.AutoShootPrepare; -import org.team1540.robot2024.commands.shooter.ShootSequence; -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.subsystems.indexer.Indexer; -import org.team1540.robot2024.subsystems.shooter.Shooter; -import org.team1540.robot2024.util.auto.AutoCommand; -import org.team1540.robot2024.util.auto.PathHelper; - -public class SourceLanePHG extends AutoCommand { - public SourceLanePHG(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ - super("!SourceLanePHG"); - - addPath( - PathHelper.fromChoreoPath("SourceLanePHG.1", true, true), - PathHelper.fromChoreoPath("SourceLanePHG.2") - ); - - addCommands( - ShootSequence.forAutoSubwoofer(shooter, indexer), - Commands.parallel( - new AutoShootPrepare(drivetrain, shooter), - Commands.sequence( - createSegmentSequence(drivetrain, shooter, indexer, 0), - createSegmentSequence(drivetrain, shooter, indexer, 1) - ) - ) - ); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGFSprint.java deleted file mode 100644 index 07141c4e..00000000 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGFSprint.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.team1540.robot2024.commands.autos; - -import edu.wpi.first.wpilibj2.command.PrintCommand; -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.util.auto.AutoCommand; -import org.team1540.robot2024.util.auto.PathHelper; - -public class SourceLanePHGFSprint extends AutoCommand { - public SourceLanePHGFSprint(Drivetrain drivetrain){ - super("SourceLanePHGFSprint"); - - addPath( - PathHelper.fromChoreoPath("SourceLanePHGFSprint.1", true, true), - PathHelper.fromChoreoPath("SourceLanePHGFSprint.2"), - PathHelper.fromChoreoPath("SourceLanePHGFSprint.3"), - PathHelper.fromChoreoPath("SourceLanePHGFSprint.4") - ); - - addCommands( - getPath(0).getCommand(drivetrain, true), - getPath(1).getCommand(drivetrain), - getPath(2).getCommand(drivetrain), - getPath(3).getCommand(drivetrain) - ); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/StraightForward.java b/src/main/java/org/team1540/robot2024/commands/autos/StraightForward.java deleted file mode 100644 index a5bf78ac..00000000 --- a/src/main/java/org/team1540/robot2024/commands/autos/StraightForward.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.team1540.robot2024.commands.autos; - -import edu.wpi.first.wpilibj2.command.Commands; -import org.team1540.robot2024.subsystems.drive.Drivetrain; -import org.team1540.robot2024.util.auto.AutoCommand; -import org.team1540.robot2024.util.auto.PathHelper; - -public class StraightForward extends AutoCommand { - public StraightForward(Drivetrain drivetrain){ - super("StraightForward"); - - addPath( - PathHelper.fromChoreoPath("StraightForward.1", true, true), - PathHelper.fromChoreoPath("StraightForward.2", true, true), - PathHelper.fromChoreoPath("StraightForward.3"), - PathHelper.fromChoreoPath("StraightForward.4"), - PathHelper.fromChoreoPath("StraightForward.5"), - PathHelper.fromChoreoPath("StraightForward.6") - ); - - addCommands( -// getPath(0).getCommand(drivetrain) -// Commands.waitSeconds(35), - getPath(1).getCommand(drivetrain) -// getPath(2).getCommand(drivetrain), -// getPath(3).getCommand(drivetrain), -// getPath(4).getCommand(drivetrain), -// getPath(5).getCommand(drivetrain), -// Commands.runOnce(drivetrain::stop) - ); - } -} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java index dced6d9a..74ff9556 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java @@ -20,7 +20,7 @@ public AutoShootPrepare(Drivetrain drivetrain, Shooter shooter) { double C = -0.4942; double D = 1.491; - public AutoShootPrepare(Supplier positionSupplier, Shooter shooter) { + public AutoShootPrepare(Supplier positionSupplier, Shooter shooter, double leftSetpoint, double rightSetpoint) { addCommands( new PrepareShooterCommand(shooter, () -> //new ShooterSetpoint( // Rotation2d.fromRadians( @@ -36,10 +36,14 @@ public AutoShootPrepare(Supplier positionSupplier, Shooter shooter) { Rotation2d.fromRadians( A * Math.atan(B * (positionSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation())) + C) + D ), - 7000, - 3000 + leftSetpoint, + rightSetpoint ) ) ); } + + public AutoShootPrepare(Supplier positionSupplier, Shooter shooter){ + this(positionSupplier, shooter, 7000, 3000); + } } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/LeadingShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/LeadingShootPrepare.java new file mode 100644 index 00000000..2154d5b9 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/shooter/LeadingShootPrepare.java @@ -0,0 +1,43 @@ +package org.team1540.robot2024.commands.shooter; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.team1540.robot2024.Constants; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.vision.AprilTagsCrescendo; + +import java.util.function.Supplier; + +import static org.team1540.robot2024.Constants.Targeting.SPEAKER_CENTER_HEIGHT; + +public class LeadingShootPrepare extends SequentialCommandGroup { + + + public LeadingShootPrepare(Drivetrain drivetrain, Shooter shooter, double leftSetpoint, double rightSetpoint){ + Supplier modifiedPosition = ()->{ + Pose3d speakerPose3d = new Pose3d( + AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getX(), + AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getY(), + SPEAKER_CENTER_HEIGHT, + new Rotation3d()); + double flightDistance = speakerPose3d.getTranslation().getDistance(new Translation3d(drivetrain.getPose().getX(), drivetrain.getPose().getY(), Constants.Shooter.Pivot.PIVOT_HEIGHT)); +// double NOTE_VELOCITY = 6000.0*1.5*2.0*Math.PI*2.54/100.0/60.0; + double NOTE_VELOCITY = 12.18; + double time = flightDistance/NOTE_VELOCITY; + Translation2d modifier = new Translation2d(drivetrain.getChassisSpeeds().vxMetersPerSecond * time,drivetrain.getChassisSpeeds().vyMetersPerSecond*time); + return modifier; + + }; + addCommands( + new AutoShootPrepare(()-> + drivetrain.getPose().plus(new Transform2d(modifiedPosition.get().getX(), modifiedPosition.get().getY(), new Rotation2d())), + shooter, leftSetpoint, rightSetpoint) + ); + + } + + public LeadingShootPrepare(Drivetrain drivetrain, Shooter shooter){ + this(drivetrain, shooter, 7000, 3000); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java index 09291aa7..09be37ee 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java @@ -15,7 +15,7 @@ public class OverStageShootPrepare extends SequentialCommandGroup { public OverStageShootPrepare(Drivetrain drivetrain, Shooter shooter) { this(drivetrain::getPose, shooter); } - public OverStageShootPrepare(Supplier positionSupplier, Shooter shooter) { + public OverStageShootPrepare(Supplier positionSupplier, Shooter shooter, double leftSetpoint, double rightSetpoint) { addCommands( new PrepareShooterCommand(shooter, () -> new ShooterSetpoint( Rotation2d.fromRadians( @@ -25,8 +25,12 @@ public OverStageShootPrepare(Supplier positionSupplier, Shooter shooter) ) ) ).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), - 3500, 3500) + leftSetpoint, rightSetpoint) ) ); } + + public OverStageShootPrepare(Supplier positionSupplier, Shooter shooter){ + this(positionSupplier, shooter, 3500, 3500); + } } From 816158523c84a35ce595d488f31027fa7f012b70 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Wed, 27 Mar 2024 16:55:03 -0700 Subject: [PATCH 59/75] feat: flippers, new but commented climb alignment, reverting --- paths.chor | 428 +++++++----------- src/main/deploy/choreo/Tag14.1.traj | 73 +-- src/main/deploy/choreo/Tag14.2.traj | 56 ++- src/main/deploy/choreo/Tag14.traj | 105 ++--- .../org/team1540/robot2024/Constants.java | 4 + .../team1540/robot2024/RobotContainer.java | 16 +- .../commands/climb/ClimbAlignment.java | 13 +- .../shooter/DefaultShooterCommand.java | 41 ++ .../subsystems/elevator/Elevator.java | 4 + .../subsystems/elevator/ElevatorIO.java | 3 + .../subsystems/elevator/ElevatorIOSim.java | 8 + .../elevator/ElevatorIOTalonFX.java | 11 +- 12 files changed, 328 insertions(+), 434 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/commands/shooter/DefaultShooterCommand.java diff --git a/paths.chor b/paths.chor index f4fa6b27..ef0fb7a5 100644 --- a/paths.chor +++ b/paths.chor @@ -200,15 +200,13 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -216,7 +214,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "Sprint": { "waypoints": [ @@ -504,15 +502,13 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -520,7 +516,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "CenterLaneSprint": { "waypoints": [ @@ -1019,15 +1015,13 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -1035,30 +1029,30 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "Tag14": { "waypoints": [ { - "x": 6.224338054656982, + "x": 5.65, "y": 4.105148, "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 2 }, { - "x": 5.77, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 3 + "controlIntervalCount": 5 }, { - "x": 5.826, + "x": 5.87, "y": 4.105148, "heading": 3.141592653589793, "isInitialGuess": false, @@ -1069,132 +1063,105 @@ ], "trajectory": [ { - "x": 6.224338054656982, + "x": 5.65, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.9390670879884598e-35, - "velocityX": 1.7766873980807548e-19, - "velocityY": 8.904182726292149e-48, + "angularVelocity": 1.9898168855172235e-35, + "velocityX": -4.1906548586207977e-19, + "velocityY": -8.603933715604811e-45, "timestamp": 0 }, { - "x": 6.186476547774305, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -2.4996122271018033e-24, - "velocityX": -0.40642791397744565, - "velocityY": 1.1300565278043194e-37, - "timestamp": 0.0931567581373812 - }, - { - "x": 6.110753536336873, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -7.642010441615522e-24, - "velocityX": -0.8128558029655188, - "velocityY": -2.827834467819816e-37, - "timestamp": 0.1863135162747624 - }, - { - "x": 5.99716902732849, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.6489729233438956e-24, - "velocityX": -1.2192836169854868, - "velocityY": -1.22969459333099e-36, - "timestamp": 0.27947027441214356 + "angularVelocity": -1.9404859008459245e-25, + "velocityX": 0.2953925521078346, + "velocityY": 2.5211469248369934e-41, + "timestamp": 0.06770651411921333 }, { - "x": 5.88358451832009, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.0379573373635505e-23, - "velocityX": -1.2192836169854868, - "velocityY": -2.8845016149269677e-37, - "timestamp": 0.3726270325495248 + "angularVelocity": 1.9894322566378973e-35, + "velocityX": -9.732091066591102e-19, + "velocityY": -8.603933345990491e-45, + "timestamp": 0.13541302823842666 }, { - "x": 5.807861506882689, + "x": 5.7033333348623145, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -6.829975200213146e-25, - "velocityX": -0.8128558029655188, - "velocityY": 9.593634209674488e-37, - "timestamp": 0.465783790686906 + "angularVelocity": 6.224455028031798e-27, + "velocityX": 0.3813501839335422, + "velocityY": -7.87167491563539e-43, + "timestamp": 0.22282175718804903 }, { - "x": 5.769999999999999, + "x": 5.770000000000059, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 2.101401640366878e-24, - "velocityX": -0.4064279139774457, - "velocityY": 7.2855825720271415e-37, - "timestamp": 0.5589405488242872 + "angularVelocity": 1.1939294645306622e-26, + "velocityX": 0.7627003153894066, + "velocityY": -1.6493125386550898e-42, + "timestamp": 0.3102304861376714 }, { - "x": 5.77, + "x": 5.836666665137633, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.8956839170817245e-35, - "velocityX": 1.3726397507122414e-19, - "velocityY": 1.1843965226752523e-47, - "timestamp": 0.6520973069616685 + "angularVelocity": 1.2599779930508123e-26, + "velocityX": 0.7627003153894064, + "velocityY": -1.6423599321913863e-42, + "timestamp": 0.3976392150872938 }, { - "x": 5.797999999999997, + "x": 5.87, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -4.600113928387426e-28, - "velocityX": 0.3495132037100882, - "velocityY": 8.714533783663232e-44, - "timestamp": 0.7322087296569475 + "angularVelocity": 6.4603054830567416e-27, + "velocityX": 0.38135018393354214, + "velocityY": -7.88271785574287e-43, + "timestamp": 0.48504794403691615 }, { - "x": 5.826, + "x": 5.87, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -4.607782485444709e-28, - "velocityX": 0.34951320371008815, - "velocityY": 8.023846057264112e-44, - "timestamp": 0.8123201523522265 - }, - { - "x": 5.826, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 9.888109688615658e-44, - "velocityX": -2.1136492085418514e-19, - "velocityY": -4.40493861881828e-55, - "timestamp": 0.8924315750475056 + "angularVelocity": -8.84678881735324e-46, + "velocityX": -2.5654038297220854e-19, + "velocityY": -1.971878449457756e-53, + "timestamp": 0.5724566729865386 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 6.224338054656982, + "x": 5.65, "y": 4.105148, "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 2 }, { - "timestamp": 0.6520973069616685, + "timestamp": 0.13541302823842666, "isStopPoint": true, - "x": 5.77, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 3 + "controlIntervalCount": 5 }, { - "timestamp": 0.8924315750475056, + "timestamp": 0.5724566729865386, "isStopPoint": true, - "x": 5.826, + "x": 5.87, "y": 4.105148, "heading": 3.141592653589793, "isInitialGuess": false, @@ -1208,22 +1175,19 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 1 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -1404,22 +1368,19 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 1 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -1427,7 +1388,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "Tag16": { "waypoints": [ @@ -1600,22 +1561,19 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 1 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -1623,7 +1581,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "SourceLanePSubHSubSprint": { "waypoints": [ @@ -3129,22 +3087,19 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 5 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -3152,7 +3107,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "CenterLanePSubCSubBSubASubSprint": { "waypoints": [ @@ -4664,36 +4619,31 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 4 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 6 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -4701,7 +4651,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "CenterLanePSubCSubBSubASubFSub": { "waypoints": [ @@ -6714,36 +6664,31 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 4 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 6 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -6751,7 +6696,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "SourceLanePSubHSubGSub": { "waypoints": [ @@ -8803,22 +8748,19 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 5 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -8826,7 +8768,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "AmpLanePSubASubDSubESub": { "waypoints": [ @@ -10885,29 +10827,25 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 5 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -10915,7 +10853,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "CenterLanePCBAFSprint": { "waypoints": [ @@ -13067,43 +13005,37 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 3 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 5 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 12 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ @@ -13118,7 +13050,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "CenterLanePCBADSprint": { "waypoints": [ @@ -14574,51 +14506,44 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 3 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 5 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 4, 5 ], - "type": "StraightLine", - "direction": 0 + "type": "StraightLine" }, { "scope": [ 8 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -14626,7 +14551,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "SourceLanePGHSprint": { "waypoints": [ @@ -17017,36 +16942,31 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 6 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 1 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 11 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -17054,7 +16974,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "AmpLanePADESprint": { "waypoints": [ @@ -18883,43 +18803,37 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 1 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 5 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 10 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -18927,7 +18841,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "CenterLanePBDA": { "waypoints": [ @@ -19963,29 +19877,25 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 4 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 1 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -19993,7 +19903,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "CenterLanePCBA": { "waypoints": [ @@ -20694,44 +20604,38 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 3 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 5 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 4, 5 ], - "type": "StraightLine", - "direction": 0 + "type": "StraightLine" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -20739,7 +20643,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "CenterLanePCBAFE": { "waypoints": [ @@ -23037,43 +22941,37 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 3 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 5 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 11 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -23081,7 +22979,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "CenterLanePCBFSprint": { "waypoints": [ @@ -24941,36 +24839,31 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 3 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 10 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -24978,7 +24871,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "SourceLanePGCSprint": { "waypoints": [ @@ -26924,52 +26817,45 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 6 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 1 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 9 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 7, 8 ], - "type": "ZeroAngularVelocity", - "direction": 0 + "type": "ZeroAngularVelocity" }, { "scope": [ 7, 8 ], - "type": "StraightLine", - "direction": 0 + "type": "StraightLine" } ], "usesControlIntervalGuessing": true, @@ -26977,7 +26863,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/deploy/choreo/Tag14.1.traj b/src/main/deploy/choreo/Tag14.1.traj index 8dc833fe..26298b3d 100644 --- a/src/main/deploy/choreo/Tag14.1.traj +++ b/src/main/deploy/choreo/Tag14.1.traj @@ -1,76 +1,31 @@ { "samples": [ { - "x": 6.224338054656982, + "x": 5.65, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.9390670879884598e-35, - "velocityX": 1.7766873980807548e-19, - "velocityY": 8.904182726292149e-48, + "angularVelocity": 1.9898168855172235e-35, + "velocityX": -4.1906548586207977e-19, + "velocityY": -8.603933715604811e-45, "timestamp": 0 }, { - "x": 6.186476547774305, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -2.4996122271018033e-24, - "velocityX": -0.40642791397744565, - "velocityY": 1.1300565278043194e-37, - "timestamp": 0.0931567581373812 + "angularVelocity": -1.9404859008459245e-25, + "velocityX": 0.2953925521078346, + "velocityY": 2.5211469248369934e-41, + "timestamp": 0.06770651411921333 }, { - "x": 6.110753536336873, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -7.642010441615522e-24, - "velocityX": -0.8128558029655188, - "velocityY": -2.827834467819816e-37, - "timestamp": 0.1863135162747624 - }, - { - "x": 5.99716902732849, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.6489729233438956e-24, - "velocityX": -1.2192836169854868, - "velocityY": -1.22969459333099e-36, - "timestamp": 0.27947027441214356 - }, - { - "x": 5.88358451832009, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 1.0379573373635505e-23, - "velocityX": -1.2192836169854868, - "velocityY": -2.8845016149269677e-37, - "timestamp": 0.3726270325495248 - }, - { - "x": 5.807861506882689, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -6.829975200213146e-25, - "velocityX": -0.8128558029655188, - "velocityY": 9.593634209674488e-37, - "timestamp": 0.465783790686906 - }, - { - "x": 5.769999999999999, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 2.101401640366878e-24, - "velocityX": -0.4064279139774457, - "velocityY": 7.2855825720271415e-37, - "timestamp": 0.5589405488242872 - }, - { - "x": 5.77, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -1.8956839170817245e-35, - "velocityX": 1.3726397507122414e-19, - "velocityY": 1.1843965226752523e-47, - "timestamp": 0.6520973069616685 + "angularVelocity": 1.9894322566378973e-35, + "velocityX": -9.732091066591102e-19, + "velocityY": -8.603933345990491e-45, + "timestamp": 0.13541302823842666 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Tag14.2.traj b/src/main/deploy/choreo/Tag14.2.traj index bbb51a92..297c4300 100644 --- a/src/main/deploy/choreo/Tag14.2.traj +++ b/src/main/deploy/choreo/Tag14.2.traj @@ -1,40 +1,58 @@ { "samples": [ { - "x": 5.77, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.8956839170817245e-35, - "velocityX": 1.3726397507122414e-19, - "velocityY": 1.1843965226752523e-47, + "angularVelocity": 1.9894322566378973e-35, + "velocityX": -9.732091066591102e-19, + "velocityY": -8.603933345990491e-45, "timestamp": 0 }, { - "x": 5.797999999999997, + "x": 5.7033333348623145, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -4.600113928387426e-28, - "velocityX": 0.3495132037100882, - "velocityY": 8.714533783663232e-44, - "timestamp": 0.08011142269527904 + "angularVelocity": 6.224455028031798e-27, + "velocityX": 0.3813501839335422, + "velocityY": -7.87167491563539e-43, + "timestamp": 0.08740872894962237 }, { - "x": 5.826, + "x": 5.770000000000059, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -4.607782485444709e-28, - "velocityX": 0.34951320371008815, - "velocityY": 8.023846057264112e-44, - "timestamp": 0.1602228453905581 + "angularVelocity": 1.1939294645306622e-26, + "velocityX": 0.7627003153894066, + "velocityY": -1.6493125386550898e-42, + "timestamp": 0.17481745789924474 }, { - "x": 5.826, + "x": 5.836666665137633, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 9.888109688615658e-44, - "velocityX": -2.1136492085418514e-19, - "velocityY": -4.40493861881828e-55, - "timestamp": 0.24033426808583713 + "angularVelocity": 1.2599779930508123e-26, + "velocityX": 0.7627003153894064, + "velocityY": -1.6423599321913863e-42, + "timestamp": 0.2622261868488671 + }, + { + "x": 5.87, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 6.4603054830567416e-27, + "velocityX": 0.38135018393354214, + "velocityY": -7.88271785574287e-43, + "timestamp": 0.3496349157984895 + }, + { + "x": 5.87, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -8.84678881735324e-46, + "velocityX": -2.5654038297220854e-19, + "velocityY": -1.971878449457756e-53, + "timestamp": 0.4370436447481119 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Tag14.traj b/src/main/deploy/choreo/Tag14.traj index f789a808..19f94d02 100644 --- a/src/main/deploy/choreo/Tag14.traj +++ b/src/main/deploy/choreo/Tag14.traj @@ -1,103 +1,76 @@ { "samples": [ { - "x": 6.224338054656982, + "x": 5.65, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.9390670879884598e-35, - "velocityX": 1.7766873980807548e-19, - "velocityY": 8.904182726292149e-48, + "angularVelocity": 1.9898168855172235e-35, + "velocityX": -4.1906548586207977e-19, + "velocityY": -8.603933715604811e-45, "timestamp": 0 }, { - "x": 6.186476547774305, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -2.4996122271018033e-24, - "velocityX": -0.40642791397744565, - "velocityY": 1.1300565278043194e-37, - "timestamp": 0.0931567581373812 + "angularVelocity": -1.9404859008459245e-25, + "velocityX": 0.2953925521078346, + "velocityY": 2.5211469248369934e-41, + "timestamp": 0.06770651411921333 }, { - "x": 6.110753536336873, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -7.642010441615522e-24, - "velocityX": -0.8128558029655188, - "velocityY": -2.827834467819816e-37, - "timestamp": 0.1863135162747624 + "angularVelocity": 1.9894322566378973e-35, + "velocityX": -9.732091066591102e-19, + "velocityY": -8.603933345990491e-45, + "timestamp": 0.13541302823842666 }, { - "x": 5.99716902732849, + "x": 5.7033333348623145, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.6489729233438956e-24, - "velocityX": -1.2192836169854868, - "velocityY": -1.22969459333099e-36, - "timestamp": 0.27947027441214356 + "angularVelocity": 6.224455028031798e-27, + "velocityX": 0.3813501839335422, + "velocityY": -7.87167491563539e-43, + "timestamp": 0.22282175718804903 }, { - "x": 5.88358451832009, + "x": 5.770000000000059, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.0379573373635505e-23, - "velocityX": -1.2192836169854868, - "velocityY": -2.8845016149269677e-37, - "timestamp": 0.3726270325495248 + "angularVelocity": 1.1939294645306622e-26, + "velocityX": 0.7627003153894066, + "velocityY": -1.6493125386550898e-42, + "timestamp": 0.3102304861376714 }, { - "x": 5.807861506882689, + "x": 5.836666665137633, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -6.829975200213146e-25, - "velocityX": -0.8128558029655188, - "velocityY": 9.593634209674488e-37, - "timestamp": 0.465783790686906 + "angularVelocity": 1.2599779930508123e-26, + "velocityX": 0.7627003153894064, + "velocityY": -1.6423599321913863e-42, + "timestamp": 0.3976392150872938 }, { - "x": 5.769999999999999, + "x": 5.87, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 2.101401640366878e-24, - "velocityX": -0.4064279139774457, - "velocityY": 7.2855825720271415e-37, - "timestamp": 0.5589405488242872 + "angularVelocity": 6.4603054830567416e-27, + "velocityX": 0.38135018393354214, + "velocityY": -7.88271785574287e-43, + "timestamp": 0.48504794403691615 }, { - "x": 5.77, + "x": 5.87, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.8956839170817245e-35, - "velocityX": 1.3726397507122414e-19, - "velocityY": 1.1843965226752523e-47, - "timestamp": 0.6520973069616685 - }, - { - "x": 5.797999999999997, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -4.600113928387426e-28, - "velocityX": 0.3495132037100882, - "velocityY": 8.714533783663232e-44, - "timestamp": 0.7322087296569475 - }, - { - "x": 5.826, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -4.607782485444709e-28, - "velocityX": 0.34951320371008815, - "velocityY": 8.023846057264112e-44, - "timestamp": 0.8123201523522265 - }, - { - "x": 5.826, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": 9.888109688615658e-44, - "velocityX": -2.1136492085418514e-19, - "velocityY": -4.40493861881828e-55, - "timestamp": 0.8924315750475056 + "angularVelocity": -8.84678881735324e-46, + "velocityX": -2.5654038297220854e-19, + "velocityY": -1.971878449457756e-53, + "timestamp": 0.5724566729865386 } ], "eventMarkers": [] diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 431f79b8..86a5153c 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -209,6 +209,10 @@ public static class Elevator { public static final double GEAR_RATIO = 11.571; public static final int LEADER_ID = 7; public static final int FOLLOWER_ID = 8; + + public static final int LEFT_FLIPPER_ID = 1; + + public static final int RIGHT_FLIPPER_ID = 0; public static final double KS = 0.03178; public static final double KV = 0.82983; public static final double KA = 0.00; diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 443084b9..35e2853c 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -126,25 +126,12 @@ private void configureLedBindings() { private void configureButtonBindings() { Command manualPivotCommand = new ManualPivotCommand(shooter, copilot); - Command defaultShooterCommand = new ConditionalCommand( - Commands.either( - new LeadingShootPrepare(drivetrain, shooter, 7000*0.75, 3000*0.75), - new LeadingShootPrepare(drivetrain, shooter, 0, 0), - indexer::isNoteStaged - ), - new OverStageShootPrepare(drivetrain::getPose, shooter, 0, 0), - ()->{ - Pose2d pose = DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Blue ? drivetrain.getPose() : GeometryUtil.flipFieldPose(drivetrain.getPose()); - return pose.getTranslation().getX() < Units.inchesToMeters(231.2); - } - - ); drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); // drivetrain.setDefaultCommand(new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter)); // drivetrain.setDefaultCommand(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter)); // drivetrain.setDefaultCommand(new DriveWithAmpSideLock(drivetrain, driver.getHID())); elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); - shooter.setDefaultCommand(defaultShooterCommand); + shooter.setDefaultCommand(manualPivotCommand); driver.b().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); // driver.y().toggleOnTrue(new DriveWithSpeakerTargetingCommand(drivetrain, driver)); driver.y().onTrue(Commands.runOnce(() -> { @@ -179,6 +166,7 @@ private void configureButtonBindings() { driver.a().whileTrue(new TuneShooterCommand(shooter, indexer, drivetrain::getPose)); } + copilot.povLeft().onTrue(Commands.runOnce(()->elevator.setFlipper(true))).onFalse(Commands.runOnce(()->elevator.setFlipper(false))); driver.povDown().and(() -> !DriverStation.isFMSAttached()).onTrue(Commands.runOnce(() -> drivetrain.setPose(new Pose2d(Units.inchesToMeters(260), Units.inchesToMeters(161.62), Rotation2d.fromRadians(0)))).ignoringDisable(true)); driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java index ba5e67c1..49b8128d 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java @@ -27,21 +27,26 @@ public class ClimbAlignment extends ParallelRaceGroup { public ClimbAlignment(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Indexer indexer, Shooter shooter){ this.drivetrain = drivetrain; addCommands( + new PrepareShooterCommand(shooter, ()->new ShooterSetpoint(0,0,0)), new SequentialCommandGroup( new ParallelCommandGroup( new SequentialCommandGroup( new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.BOTTOM), new StageTrampCommand(tramp, indexer).onlyIf(indexer::isNoteStaged) - ), - new PrepareShooterCommand(shooter, ()->new ShooterSetpoint(0,0,0)) + ) // new ProxyCommand(() -> climbPath(drivetrain::getPose, 1)) ), - Commands.runOnce(() -> drivetrain.setBrakeMode(false)) +// Commands.runOnce(() -> drivetrain.setBrakeMode(false)) // Commands.waitSeconds(5), //Confirm that nothing will break +// Commands.runOnce(()->elevator.setFlipper(true)), // new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.TOP), // Commands.runOnce(() -> drivetrain.setBrakeMode(true)), // Commands.waitSeconds(5), //Confirm that nothing will break -// new ProxyCommand(() -> climbPath(drivetrain::getPose, 2)) +// new ProxyCommand(() -> climbPath(drivetrain::getPose, 2)), + Commands.parallel( +// Commands.runOnce(()->elevator.setFlipper(false)), +// new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.TOP) + ) ), new StartEndCommand(()->{}, ()->{ drivetrain.setBrakeMode(true); diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/DefaultShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/DefaultShooterCommand.java new file mode 100644 index 00000000..17b92360 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/shooter/DefaultShooterCommand.java @@ -0,0 +1,41 @@ +package org.team1540.robot2024.commands.shooter; + +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; + +import java.util.function.BooleanSupplier; + +public class DefaultShooterCommand extends SequentialCommandGroup { + + private boolean hasNote = false; + private boolean shouldShootPrepare = false; + + private BooleanSupplier shootPrepareSupplier; + + public DefaultShooterCommand(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ + shootPrepareSupplier = ()->{ + Pose2d pose = DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Blue ? drivetrain.getPose() : GeometryUtil.flipFieldPose(drivetrain.getPose()); + return pose.getTranslation().getX() < Units.inchesToMeters(231.2); + }; + addCommands( + Commands.runOnce(()->shouldShootPrepare = shootPrepareSupplier.getAsBoolean()), + Commands.runOnce(()->hasNote = indexer.isNoteStaged()), + Commands.either( + Commands.either( + new LeadingShootPrepare(drivetrain, shooter, 7000*0.75, 3000*0.75).alongWith(Commands.runOnce(()->hasNote = true)), + new LeadingShootPrepare(drivetrain, shooter, 0, 0).alongWith(Commands.runOnce(()->hasNote = true)), + indexer::isNoteStaged + ).alongWith(Commands.runOnce(()-> shouldShootPrepare = true)), + new OverStageShootPrepare(drivetrain::getPose, shooter, 0, 0).alongWith(Commands.runOnce(()-> shouldShootPrepare = false)), + shootPrepareSupplier + ).until(()-> shootPrepareSupplier.getAsBoolean() != shouldShootPrepare || indexer.isNoteStaged() != hasNote).repeatedly() + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java index cde6eb84..d9bc820b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java @@ -76,6 +76,10 @@ public void setElevatorPosition(double positionMeters) { positionFilter.clear(); } + public void setFlipper(boolean flipped){ + io.setFlipper(flipped); + } + public boolean isAtSetpoint() { return MathUtil.isNear(setpointMeters, positionFilter.getAverage(), POS_ERR_TOLERANCE_METERS) || (inputs.atLowerLimit && setpointMeters <= 0); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java index 76a9bf7f..385a6395 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java @@ -12,6 +12,7 @@ class ElevatorIOInputs { public double[] tempCelsius = new double[]{}; public boolean atUpperLimit = false; public boolean atLowerLimit = false; + public double flipperAngleDegrees = 0.0; } default void updateInputs(ElevatorIOInputs inputs) {} @@ -22,6 +23,8 @@ default void setVoltage(double voltage) {} default void setBrakeMode(boolean isBrakeMode) {} + default void setFlipper(boolean flipped) {} + default void configPID(double kP, double kI, double kD) {} } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java index 93733e69..8c6e0242 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java @@ -31,6 +31,8 @@ public class ElevatorIOSim implements ElevatorIO { private boolean isClosedLoop; private TrapezoidProfile.State setpoint; + private double servo; + @Override public void updateInputs(ElevatorIOInputs inputs) { if (isClosedLoop) { @@ -49,6 +51,7 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.currentAmps = new double[]{elevatorSim.getCurrentDrawAmps()}; inputs.atUpperLimit = elevatorSim.hasHitUpperLimit(); inputs.atLowerLimit = elevatorSim.hasHitLowerLimit(); + inputs.flipperAngleDegrees = servo; } @Override @@ -63,6 +66,11 @@ public void setSetpointMeters(double position) { setpoint = new TrapezoidProfile.State(position, 0.0); } + @Override + public void setFlipper(boolean flipped) { + servo = flipped ? 90 : 0; + } + @Override public void configPID(double kP, double kI, double kD) { controller.setPID(kP, kI, kD); diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java index 53bcec6f..b3abbc8d 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.*; +import edu.wpi.first.wpilibj.Servo; import org.team1540.robot2024.Constants; public class ElevatorIOTalonFX implements ElevatorIO { @@ -16,7 +17,8 @@ public class ElevatorIOTalonFX implements ElevatorIO { private final MotionMagicVoltage motionMagicOut = new MotionMagicVoltage(0).withEnableFOC(true); private final TalonFX leader = new TalonFX(Constants.Elevator.LEADER_ID); private final TalonFX follower = new TalonFX(Constants.Elevator.FOLLOWER_ID); - + private final Servo leftFlipper = new Servo(Constants.Elevator.LEFT_FLIPPER_ID); + private final Servo rightFlipper = new Servo(Constants.Elevator.RIGHT_FLIPPER_ID); private final StatusSignal leaderPosition = leader.getPosition(); private final StatusSignal leaderVelocity = leader.getVelocity(); private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); @@ -99,6 +101,7 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.tempCelsius = new double[]{leaderTemp.getValueAsDouble(), followerCurrent.getValueAsDouble()}; inputs.atUpperLimit = topLimitStatus.getValue() == ForwardLimitValue.ClosedToGround; inputs.atLowerLimit = bottomLimitStatus.getValue() == ReverseLimitValue.ClosedToGround; + inputs.flipperAngleDegrees = leftFlipper.getAngle(); } @Override @@ -117,6 +120,12 @@ public void setBrakeMode(boolean isBrakeMode) { follower.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); } + @Override + public void setFlipper(boolean flipped) { + leftFlipper.set(flipped ? 0.63 : 1); + rightFlipper.set(flipped ? 1 : 0); + } + @Override public void configPID(double kP, double kI, double kD) { Slot0Configs configs = new Slot0Configs(); From 972d523d28dad63f0f4e54ed887d82088011cb9f Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Thu, 28 Mar 2024 22:27:24 -0700 Subject: [PATCH 60/75] fix: stop feeding to shooter when flywheels aren't spun up (still needs led indicators --- src/main/java/org/team1540/robot2024/Constants.java | 2 +- .../java/org/team1540/robot2024/RobotContainer.java | 12 ++++-------- .../robot2024/subsystems/shooter/Shooter.java | 2 +- 3 files changed, 6 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 86a5153c..963f0292 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -148,7 +148,7 @@ public static class Flywheels { public static final double KS = 0.26925; public static final double KV = 0.07485; - public static final double ERROR_TOLERANCE_RPM = 100; + public static final double ERROR_TOLERANCE_RPM = 1000; } public static class Pivot { diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 35e2853c..ac5b23c7 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -127,13 +127,9 @@ private void configureLedBindings() { private void configureButtonBindings() { Command manualPivotCommand = new ManualPivotCommand(shooter, copilot); drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); -// drivetrain.setDefaultCommand(new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter)); -// drivetrain.setDefaultCommand(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter)); -// drivetrain.setDefaultCommand(new DriveWithAmpSideLock(drivetrain, driver.getHID())); elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); shooter.setDefaultCommand(manualPivotCommand); driver.b().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); -// driver.y().toggleOnTrue(new DriveWithSpeakerTargetingCommand(drivetrain, driver)); driver.y().onTrue(Commands.runOnce(() -> { drivetrain.zeroFieldOrientationManual(); drivetrain.setBrakeMode(true); @@ -164,9 +160,8 @@ private void configureButtonBindings() { if (isTuningMode()) { driver.leftTrigger().whileTrue(new AutoShootPrepareWhileMoving(driver.getHID(), drivetrain, shooter).alongWith(leds.commandShowPattern(new LedPatternWave("#00ff00"), Leds.PatternLevel.DRIVER_LOCK))); driver.a().whileTrue(new TuneShooterCommand(shooter, indexer, drivetrain::getPose)); - } - copilot.povLeft().onTrue(Commands.runOnce(()->elevator.setFlipper(true))).onFalse(Commands.runOnce(()->elevator.setFlipper(false))); + driver.povDown().and(() -> !DriverStation.isFMSAttached()).onTrue(Commands.runOnce(() -> drivetrain.setPose(new Pose2d(Units.inchesToMeters(260), Units.inchesToMeters(161.62), Rotation2d.fromRadians(0)))).ignoringDisable(true)); driver.rightTrigger(0.95).toggleOnTrue(autoShooterCommand); @@ -182,7 +177,8 @@ private void configureButtonBindings() { copilot.povDown().whileTrue(indexer.commandRunIntake(-1)); copilot.povUp().whileTrue(indexer.commandRunIntake(1)); - copilot.povRight().whileTrue(tramp.commandRun(1)); + copilot.povRight().whileTrue(IntakeAndFeed.withDefaults(indexer)).onFalse(cancelAlignment); + copilot.povLeft().onTrue(Commands.runOnce(()->elevator.setFlipper(true))).onFalse(Commands.runOnce(()->elevator.setFlipper(false))); copilot.rightTrigger(0.95).whileTrue(tramp.commandRun(1)); @@ -191,7 +187,7 @@ private void configureButtonBindings() { copilot.x().whileTrue(new ShootSequence(shooter, indexer)); copilot.a().whileTrue(new AmpScoreStageSequence(indexer, tramp, elevator).alongWith(ampLock)); - copilot.b().whileTrue(IntakeAndFeed.withDefaults(indexer)) + copilot.b().whileTrue(IntakeAndFeed.withDefaults(indexer).onlyIf(shooter::areFlywheelsSpunUp)) .onFalse(cancelAlignment); copilot.y().whileTrue(new StageTrampCommand(tramp, indexer)); diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index 13cb201e..ea2cc2e7 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -49,7 +49,7 @@ public class Shooter extends SubsystemBase { private boolean flipper = false; - private Rotation2d angleOffset = new Rotation2d(); + private final Rotation2d angleOffset = new Rotation2d(); public final ShooterLerp lerp = new ShooterLerp().put( new Pair<>(1.386, new ShooterSetpoint(Rotation2d.fromRadians(1.06184))), From d5deda4c6c85af1337cf1a58e78bd0dac884c371 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 29 Mar 2024 15:32:35 -0700 Subject: [PATCH 61/75] feat: shoot to better location of speaker and led indicators for shooting --- .../org/team1540/robot2024/Constants.java | 21 +++++++++++-- .../team1540/robot2024/RobotContainer.java | 19 +++++++----- .../drivetrain/DriveWithTargetingCommand.java | 11 ++----- .../commands/shooter/AutoShootPrepare.java | 12 +------- .../shooter/AutoShootPrepareWhileMoving.java | 27 ++++++++++------- .../commands/shooter/LeadingShootPrepare.java | 11 ++++--- .../shooter/OverStageShootPrepare.java | 1 + .../OverStageShootPrepareWithTargeting.java | 7 ++--- .../commands/shooter/TuneShooterCommand.java | 5 ++-- .../robot2024/subsystems/led/Leds.java | 2 +- .../led/patterns/LedPatternProgressBar.java | 30 +++++++++++++++++++ .../robot2024/subsystems/shooter/Shooter.java | 5 ++++ 12 files changed, 99 insertions(+), 52 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternProgressBar.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 963f0292..d8a03584 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -1,6 +1,7 @@ package org.team1540.robot2024; import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.util.GeometryUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -267,10 +268,26 @@ public static class Targeting { public static final double ROT_KI = 0.0; public static final double ROT_KD = 0.0; - public static final Pose2d SPEAKER_POSE = - new Pose2d(Units.inchesToMeters(8.861), Units.inchesToMeters(218), new Rotation2d()); + public static final double SPEAKER_CENTER_HEIGHT = Units.inchesToMeters(80.4375); public static final double STAGE_MAX_HEIGHT = Units.feetToMeters(7.365); + + private static final Pose2d SPEAKER_POSE = + new Pose2d(Units.inchesToMeters(8.861), Units.inchesToMeters(218), new Rotation2d()); + private static final Pose2d SHUFFLE_POSE = + new Pose2d(SPEAKER_POSE.getX(), SPEAKER_POSE.getY() + 2, new Rotation2d()); + + public static Pose2d getSpeakerPose() { + return DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red + ? GeometryUtil.flipFieldPose(SPEAKER_POSE) + : SPEAKER_POSE; + } + + public static Pose2d getShufflePose() { + return DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red + ? GeometryUtil.flipFieldPose(SHUFFLE_POSE) + : SHUFFLE_POSE; + } } public static boolean isTuningMode() { diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index ac5b23c7..6e0b2160 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -1,6 +1,5 @@ package org.team1540.robot2024; -import com.pathplanner.lib.util.GeometryUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -9,7 +8,6 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import org.team1540.robot2024.commands.FeedForwardCharacterization; @@ -21,7 +19,6 @@ import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; import org.team1540.robot2024.commands.indexer.ContinuousIntakeCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; -import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.commands.indexer.StageTrampCommand; import org.team1540.robot2024.commands.shooter.*; import org.team1540.robot2024.commands.tramp.AmpScoreSequence; @@ -30,6 +27,7 @@ import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.led.Leds; +import org.team1540.robot2024.subsystems.led.patterns.LedPatternProgressBar; import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState; import org.team1540.robot2024.subsystems.led.patterns.LedPatternWave; import org.team1540.robot2024.subsystems.led.patterns.SimpleLedPattern; @@ -135,13 +133,18 @@ private void configureButtonBindings() { drivetrain.setBrakeMode(true); }).ignoringDisable(true)); - Command targetDrive = new AutoShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter) - .alongWith(leds.commandShowPattern(new LedPatternWave("#00a9ff"), Leds.PatternLevel.DRIVER_LOCK)); + .alongWith(leds.commandShowPattern( + new LedPatternProgressBar(shooter::getSpinUpPercent, "#00a9ff", 33), + Leds.PatternLevel.DRIVER_LOCK)); Command overstageTargetDrive = new OverStageShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter) - .alongWith(leds.commandShowPattern(new LedPatternWave("#f700ff"), Leds.PatternLevel.DRIVER_LOCK)); + .alongWith(leds.commandShowPattern( + new LedPatternProgressBar(shooter::getSpinUpPercent, "#f700ff", 33), + Leds.PatternLevel.DRIVER_LOCK)); Command autoShooterCommand = new AutoShootPrepare(drivetrain, shooter) - .alongWith(leds.commandShowPattern(new LedPatternWave("#00ffbc"), Leds.PatternLevel.DRIVER_LOCK)); + .alongWith(leds.commandShowPattern( + new LedPatternProgressBar(shooter::getSpinUpPercent, "#00ffbc", 33), + Leds.PatternLevel.DRIVER_LOCK)); Command ampLock = new DriveWithAmpSideLock(drivetrain, driver.getHID()) .alongWith(leds.commandShowPattern(new LedPatternWave("#ffffff"), Leds.PatternLevel.DRIVER_LOCK)); Command cancelAlignment = Commands.runOnce(() -> { @@ -187,7 +190,7 @@ private void configureButtonBindings() { copilot.x().whileTrue(new ShootSequence(shooter, indexer)); copilot.a().whileTrue(new AmpScoreStageSequence(indexer, tramp, elevator).alongWith(ampLock)); - copilot.b().whileTrue(IntakeAndFeed.withDefaults(indexer).onlyIf(shooter::areFlywheelsSpunUp)) + copilot.b().and(shooter::areFlywheelsSpunUp).whileTrue(IntakeAndFeed.withDefaults(indexer)) .onFalse(cancelAlignment); copilot.y().whileTrue(new StageTrampCommand(tramp, indexer)); diff --git a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java index 6d54a8eb..04317c5e 100644 --- a/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/drivetrain/DriveWithTargetingCommand.java @@ -1,19 +1,15 @@ package org.team1540.robot2024.commands.drivetrain; -import com.pathplanner.lib.util.GeometryUtil; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.littletonrobotics.junction.Logger; import org.team1540.robot2024.Constants; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.util.LoggedTunableNumber; import org.team1540.robot2024.util.math.JoystickUtils; -import org.team1540.robot2024.util.vision.AprilTagsCrescendo; import java.util.function.Supplier; @@ -31,12 +27,12 @@ public class DriveWithTargetingCommand extends Command { private boolean isFlipped; - private Supplier target; + private final Supplier target; private final double deadzone = 0.03; public DriveWithTargetingCommand(Drivetrain drivetrain, XboxController controller){ - this(drivetrain, controller, ()-> AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d()); + this(drivetrain, controller, Constants.Targeting::getSpeakerPose); } public DriveWithTargetingCommand(Drivetrain drivetrain, XboxController controller, Supplier target) { this.drivetrain = drivetrain; @@ -58,9 +54,8 @@ public void execute() { drivetrain.getPose() .minus(target.get()).getTranslation().getAngle() .rotateBy(isFlipped ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0)); -// Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); drivetrain.setTargetPose(new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); - Logger.recordOutput("Targeting/rotErrorDegrees", Math.abs(targetRot.minus(drivetrain.getRotation()).getDegrees())); + Logger.recordOutput("Targeting/rotError", targetRot.minus(drivetrain.getRotation())); Logger.recordOutput("Targeting/target", target.get()); double linearMagnitude = 0; diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java index 74ff9556..a475193e 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepare.java @@ -3,7 +3,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import org.team1540.robot2024.Constants; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.util.shooter.ShooterSetpoint; @@ -22,16 +21,7 @@ public AutoShootPrepare(Drivetrain drivetrain, Shooter shooter) { public AutoShootPrepare(Supplier positionSupplier, Shooter shooter, double leftSetpoint, double rightSetpoint) { addCommands( - new PrepareShooterCommand(shooter, () -> //new ShooterSetpoint( -// Rotation2d.fromRadians( -// Math.atan2(Constants.Targeting.SPEAKER_CENTER_HEIGHT - Constants.Shooter.Pivot.PIVOT_HEIGHT, -// positionSupplier.get().getTranslation().getDistance( -// AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation() -// ) -// ) -// ).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), -// 8000, 6000) -// shooter.lerp.get(positionSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation())) + new PrepareShooterCommand(shooter, () -> new ShooterSetpoint( Rotation2d.fromRadians( A * Math.atan(B * (positionSupplier.get().getTranslation().getDistance(AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getTranslation())) + C) + D diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java index 4bfc6108..5550bc11 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/AutoShootPrepareWhileMoving.java @@ -1,7 +1,6 @@ package org.team1540.robot2024.commands.shooter; import edu.wpi.first.math.geometry.*; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import org.team1540.robot2024.Constants; @@ -16,27 +15,35 @@ public class AutoShootPrepareWhileMoving extends ParallelCommandGroup { public AutoShootPrepareWhileMoving(XboxController controller, Drivetrain drivetrain, Shooter shooter) { - Supplier modifiedPosition = ()->{ + Supplier modifiedPosition = () -> { Pose3d speakerPose3d = new Pose3d( AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getX(), AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().getY(), SPEAKER_CENTER_HEIGHT, new Rotation3d()); double flightDistance = speakerPose3d.getTranslation().getDistance(new Translation3d(drivetrain.getPose().getX(), drivetrain.getPose().getY(), Constants.Shooter.Pivot.PIVOT_HEIGHT)); -// double NOTE_VELOCITY = 6000.0*1.5*2.0*Math.PI*2.54/100.0/60.0; double NOTE_VELOCITY = 12.18; double time = flightDistance/NOTE_VELOCITY; - Translation2d modifier = new Translation2d(drivetrain.getChassisSpeeds().vxMetersPerSecond * time,drivetrain.getChassisSpeeds().vyMetersPerSecond*time); + Translation2d modifier = new Translation2d( + drivetrain.getChassisSpeeds().vxMetersPerSecond * time, + drivetrain.getChassisSpeeds().vyMetersPerSecond * time); return modifier; - }; addCommands( - new DriveWithTargetingCommand(drivetrain,controller, ()-> - AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().plus(new Transform2d(-modifiedPosition.get().getX(), -modifiedPosition.get().getY(), new Rotation2d())) - ), - new AutoShootPrepare(()-> - drivetrain.getPose().plus(new Transform2d(modifiedPosition.get().getX(), modifiedPosition.get().getY(), new Rotation2d())), + new DriveWithTargetingCommand( + drivetrain, + controller, + () -> Constants.Targeting.getSpeakerPose() + .plus(new Transform2d( + -modifiedPosition.get().getX(), + -modifiedPosition.get().getY(), + new Rotation2d()))), + new AutoShootPrepare( + () -> drivetrain.getPose().plus(new Transform2d( + modifiedPosition.get().getX(), + modifiedPosition.get().getY(), + new Rotation2d())), shooter) ); } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/LeadingShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/LeadingShootPrepare.java index 2154d5b9..e2268608 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/LeadingShootPrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/LeadingShootPrepare.java @@ -22,16 +22,19 @@ public LeadingShootPrepare(Drivetrain drivetrain, Shooter shooter, double leftSe SPEAKER_CENTER_HEIGHT, new Rotation3d()); double flightDistance = speakerPose3d.getTranslation().getDistance(new Translation3d(drivetrain.getPose().getX(), drivetrain.getPose().getY(), Constants.Shooter.Pivot.PIVOT_HEIGHT)); -// double NOTE_VELOCITY = 6000.0*1.5*2.0*Math.PI*2.54/100.0/60.0; double NOTE_VELOCITY = 12.18; double time = flightDistance/NOTE_VELOCITY; Translation2d modifier = new Translation2d(drivetrain.getChassisSpeeds().vxMetersPerSecond * time,drivetrain.getChassisSpeeds().vyMetersPerSecond*time); return modifier; - }; + addCommands( - new AutoShootPrepare(()-> - drivetrain.getPose().plus(new Transform2d(modifiedPosition.get().getX(), modifiedPosition.get().getY(), new Rotation2d())), + new AutoShootPrepare( + ()-> drivetrain.getPose().plus( + new Transform2d( + modifiedPosition.get().getX(), + modifiedPosition.get().getY(), + new Rotation2d())), shooter, leftSetpoint, rightSetpoint) ); diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java index 09be37ee..ce738058 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepare.java @@ -15,6 +15,7 @@ public class OverStageShootPrepare extends SequentialCommandGroup { public OverStageShootPrepare(Drivetrain drivetrain, Shooter shooter) { this(drivetrain::getPose, shooter); } + public OverStageShootPrepare(Supplier positionSupplier, Shooter shooter, double leftSetpoint, double rightSetpoint) { addCommands( new PrepareShooterCommand(shooter, () -> new ShooterSetpoint( diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java index 5d4c888f..1d31f2b6 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/OverStageShootPrepareWithTargeting.java @@ -1,19 +1,16 @@ package org.team1540.robot2024.commands.shooter; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import org.team1540.robot2024.Constants; import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.shooter.Shooter; -import org.team1540.robot2024.util.vision.AprilTagsCrescendo; public class OverStageShootPrepareWithTargeting extends ParallelCommandGroup { public OverStageShootPrepareWithTargeting(XboxController controller, Drivetrain drivetrain, Shooter shooter) { addCommands( - new DriveWithTargetingCommand(drivetrain,controller, ()-> AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.SPEAKER_CENTER).toPose2d().plus(new Transform2d(0,-2 * (DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Blue ? -1 : 1),new Rotation2d()))), + new DriveWithTargetingCommand(drivetrain,controller, Constants.Targeting::getShufflePose), new OverStageShootPrepare(drivetrain, shooter) ); } diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java index f5f820bc..74e56eed 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -31,8 +30,8 @@ public TuneShooterCommand(Shooter shooter, Indexer indexer, Supplier pos pivotRotationSupplier = shooter::getPivotPosition; addCommands( - new IntakeAndFeed(indexer, ()->1, ()->1), - new PrepareShooterCommand(shooter, ()-> + new IntakeAndFeed(indexer, () -> 1, () -> 1), + new PrepareShooterCommand(shooter, () -> new ShooterSetpoint( Rotation2d.fromDegrees(angleSetpoint.get()), leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get() )), diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index 323a4381..b85c6502 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -24,7 +24,7 @@ public Leds() { strip.setData(ledBuffer); strip.start(); - buffers[Zone.MAIN.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 41, false); + buffers[Zone.MAIN.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 0, 41, false); buffers[Zone.TOP.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 32, 41, false); for (int i = 0; i < ZONE_COUNT;i++) { patterns[i] = new LedTriager(); diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternProgressBar.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternProgressBar.java new file mode 100644 index 00000000..f61de9e7 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternProgressBar.java @@ -0,0 +1,30 @@ +package org.team1540.robot2024.subsystems.led.patterns; + +import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer; + +import edu.wpi.first.wpilibj.util.Color; +import java.util.function.DoubleSupplier; + +public class LedPatternProgressBar extends LedPattern { + private final DoubleSupplier percent; + private final Color color; + private final int maxLength; + + public LedPatternProgressBar(DoubleSupplier percent, String hex, int maxLength) { + super(true); + this.percent = percent; + this.color = new Color(hex); + this.maxLength = maxLength; + } + + @Override + public void apply(ZonedAddressableLEDBuffer buffer) { + int barLength = (int) (maxLength * percent.getAsDouble()); + for (int i = 0; i < buffer.getLength(); i++) { + buffer.setRGB(i, 0, 0, 0); + } + for (int i = 0; i < barLength; i++) { + buffer.setLED(i, color); + } + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index ea2cc2e7..7194a2ad 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -209,6 +209,10 @@ public double getRightFlywheelSpeed() { return flywheelInputs.rightVelocityRPM; } + public double getSpinUpPercent() { + return (getRightFlywheelSpeed() + getLeftFlywheelSpeed()) / (getRightFlywheelSetpointRPM() + getLeftFlywheelSetpointRPM()); + } + /** * Gets the position of the pivot */ @@ -287,6 +291,7 @@ public Rotation2d getAbsolutePivotSetpoint(){ public void zeroPivot() { pivotIO.setEncoderPosition(0); } + public void zeroPivotToCancoder(){ pivotIO.setEncoderPosition(pivotInputs.absolutePosition.getRotations()); flipper = !flipper; From d1217e0bdbcd97fa57843b8d4b22be31d9dbdd6a Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 29 Mar 2024 21:25:44 -0700 Subject: [PATCH 62/75] feat: climb align (-ish) --- .../org/team1540/robot2024/Constants.java | 4 +- .../commands/climb/ClimbAlignment.java | 15 +- .../elevator/ElevatorSetpointCommand.java | 4 + .../subsystems/elevator/Elevator.java | 1 + .../vision/AprilTagVisionIOLimelight.java | 2 +- .../util/vision/EstimatedVisionPose.java | 2 +- .../util/vision/LimelightHelpers.java | 174 +++++++++++++++--- 7 files changed, 168 insertions(+), 34 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index d8a03584..afb1d318 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -83,7 +83,7 @@ public static class Auto { Constants.Drivetrain.MAX_ANGULAR_SPEED * ANGULAR_ACCEL_TIME_SECS); public static final PathConstraints STAGE_PATH_CONSTRAINTS = new PathConstraints( - 3.0, 1, + 3.0, 3.0, 1, 0.3); } @@ -204,7 +204,7 @@ public static class Elevator { public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25); public static final double MINIMUM_HEIGHT = Units.inchesToMeters(-2); //TODO: Does this make it angry? public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0); - public static final double MAX_HEIGHT = MINIMUM_HEIGHT + Units.inchesToMeters(21.0); //TODO: Fix these constants to be more accurate + public static final double MAX_HEIGHT = 0.5; //TODO: Fix these constants to be more accurate public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + MAX_HEIGHT - MINIMUM_HEIGHT; public static final double GEAR_RATIO = 11.571; diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java index 49b8128d..9f2e5022 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java @@ -33,19 +33,22 @@ public ClimbAlignment(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Ind new SequentialCommandGroup( new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.BOTTOM), new StageTrampCommand(tramp, indexer).onlyIf(indexer::isNoteStaged) - ) -// new ProxyCommand(() -> climbPath(drivetrain::getPose, 1)) + ), + new ProxyCommand(() -> climbPath(drivetrain::getPose, 1)) ), // Commands.runOnce(() -> drivetrain.setBrakeMode(false)) // Commands.waitSeconds(5), //Confirm that nothing will break -// Commands.runOnce(()->elevator.setFlipper(true)), + Commands.runOnce(()->elevator.setFlipper(true)), // new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.TOP), // Commands.runOnce(() -> drivetrain.setBrakeMode(true)), // Commands.waitSeconds(5), //Confirm that nothing will break -// new ProxyCommand(() -> climbPath(drivetrain::getPose, 2)), + new ProxyCommand(() -> climbPath(drivetrain::getPose, 2)), + new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.AMP), + new WaitCommand(5), + new RepeatCommand(new PrintCommand("elevator things")), Commands.parallel( -// Commands.runOnce(()->elevator.setFlipper(false)), -// new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.TOP) + Commands.runOnce(()->elevator.setFlipper(false)), + new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.TOP) ) ), new StartEndCommand(()->{}, ()->{ diff --git a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorSetpointCommand.java b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorSetpointCommand.java index ac717d11..55ba7073 100644 --- a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorSetpointCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorSetpointCommand.java @@ -24,4 +24,8 @@ public boolean isFinished() { return elevator.isAtSetpoint(); } + @Override + public void end(boolean interrupted) { + elevator.holdPosition(); + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java index d9bc820b..1b9adbdf 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java @@ -80,6 +80,7 @@ public void setFlipper(boolean flipped){ io.setFlipper(flipped); } + @AutoLogOutput(key = "Elevator/isAtSetpoint") public boolean isAtSetpoint() { return MathUtil.isNear(setpointMeters, positionFilter.getAverage(), POS_ERR_TOLERANCE_METERS) || (inputs.atLowerLimit && setpointMeters <= 0); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java index 97fcf898..470bbbd2 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -21,7 +21,7 @@ public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) { public void updateInputs(AprilTagVisionIOInputs inputs) { LimelightHelpers.PoseEstimate measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); if (measurement.tagCount > 1) { - inputs.estimatedPoseMeters = new Pose3d(measurement.pose); + inputs.estimatedPoseMeters = LimelightHelpers.getBotPose3d_wpiBlue(name); inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds; } inputs.numTagsSeen = measurement.tagCount; diff --git a/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java b/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java index 628c79f2..79034e29 100644 --- a/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java +++ b/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java @@ -23,6 +23,6 @@ public Matrix getStdDevs() { Constants.Vision.ROT_STD_DEV_COEFF * Math.pow(avgDistance, 2.0) / numTags; - return VecBuilder.fill(xyStdDev, xyStdDev, numTags > 1 ? rotStdDev : Double.POSITIVE_INFINITY); + return VecBuilder.fill(xyStdDev, xyStdDev, rotStdDev/*numTags > 1 ? rotStdDev : Double.POSITIVE_INFINITY*/); } } diff --git a/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java b/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java index a0664278..da31dc6b 100644 --- a/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java +++ b/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.3.1 (March 4, 2024) +//LimelightHelpers v1.5.0 (March 27, 2024) package org.team1540.robot2024.util.vision; @@ -179,7 +179,7 @@ public Pose2d getTargetPose_RobotSpace2D() { return toPose2D(targetPose_RobotSpace); } - + @JsonProperty("ta") public double ta; @@ -197,7 +197,7 @@ public Pose2d getTargetPose_RobotSpace2D() @JsonProperty("ts") public double ts; - + public LimelightTarget_Fiducial() { cameraPose_TargetSpace = new double[6]; robotPose_FieldSpace = new double[6]; @@ -305,13 +305,13 @@ public static class Results { @JsonProperty("botpose_tagcount") public double botpose_tagcount; - + @JsonProperty("botpose_span") public double botpose_span; - + @JsonProperty("botpose_avgdist") public double botpose_avgdist; - + @JsonProperty("botpose_avgarea") public double botpose_avgarea; @@ -321,11 +321,11 @@ public static class Results { public Pose3d getBotPose3d() { return toPose3D(botpose); } - + public Pose3d getBotPose3d_wpiRed() { return toPose3D(botpose_wpired); } - + public Pose3d getBotPose3d_wpiBlue() { return toPose3D(botpose_wpiblue); } @@ -333,11 +333,11 @@ public Pose3d getBotPose3d_wpiBlue() { public Pose2d getBotPose2d() { return toPose2D(botpose); } - + public Pose2d getBotPose2d_wpiRed() { return toPose2D(botpose_wpired); } - + public Pose2d getBotPose2d_wpiBlue() { return toPose2D(botpose_wpiblue); } @@ -374,7 +374,7 @@ public Results() { public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; - + public String error; public LimelightResults() { @@ -385,6 +385,27 @@ public LimelightResults() { } + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + public static class PoseEstimate { public Pose2d pose; public double timestampSeconds; @@ -393,9 +414,12 @@ public static class PoseEstimate { public double tagSpan; public double avgTagDist; public double avgTagArea; + public RawFiducial[] rawFiducials; + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int tagCount, double tagSpan, double avgTagDist, double avgTagArea) { this.pose = pose; this.timestampSeconds = timestampSeconds; this.latency = latency; @@ -403,6 +427,7 @@ public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int ta this.tagSpan = tagSpan; this.avgTagDist = avgTagDist; this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -427,9 +452,9 @@ private static Pose3d toPose3D(double[] inData){ return new Pose3d(); } return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); } private static Pose2d toPose2D(double[] inData){ @@ -462,7 +487,65 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr double tagArea = extractBotPoseEntry(poseArray,10); //getlastchange() in microseconds, ll latency in milliseconds var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); - return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } } public static NetworkTable getLimelightNTTable(String tableName) { @@ -537,7 +620,7 @@ public static String getJSONDump(String limelightName) { /** * Switch to getBotPose - * + * * @param limelightName * @return */ @@ -548,7 +631,7 @@ public static double[] getBotpose(String limelightName) { /** * Switch to getBotPose_wpiRed - * + * * @param limelightName * @return */ @@ -559,7 +642,7 @@ public static double[] getBotpose_wpiRed(String limelightName) { /** * Switch to getBotPose_wpiBlue - * + * * @param limelightName * @return */ @@ -654,7 +737,7 @@ public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) - * + * * @param limelightName * @return */ @@ -667,7 +750,7 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { /** * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE * alliance - * + * * @param limelightName * @return */ @@ -675,10 +758,21 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpiblue"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) - * + * * @param limelightName * @return */ @@ -699,10 +793,20 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpired"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) - * + * * @param limelightName * @return */ @@ -724,7 +828,7 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } - + public static void setPriorityTagID(String limelightName, int ID) { setLimelightNTDouble(limelightName, "priorityid", ID); } @@ -782,6 +886,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; From 011308017f6492ff0921820e728eaf34f80ab32d Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 29 Mar 2024 21:28:24 -0700 Subject: [PATCH 63/75] fix: only feed to shooter if targeting is scheduled --- src/main/java/org/team1540/robot2024/RobotContainer.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 6e0b2160..4a63ffad 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -190,8 +190,13 @@ private void configureButtonBindings() { copilot.x().whileTrue(new ShootSequence(shooter, indexer)); copilot.a().whileTrue(new AmpScoreStageSequence(indexer, tramp, elevator).alongWith(ampLock)); - copilot.b().and(shooter::areFlywheelsSpunUp).whileTrue(IntakeAndFeed.withDefaults(indexer)) - .onFalse(cancelAlignment); + copilot.b() + .and(shooter::areFlywheelsSpunUp) + .and(() -> targetDrive.isScheduled() + || overstageTargetDrive.isScheduled() + || autoShooterCommand.isScheduled()) + .whileTrue(IntakeAndFeed.withDefaults(indexer)) + .onFalse(cancelAlignment); copilot.y().whileTrue(new StageTrampCommand(tramp, indexer)); // copilot.leftTrigger(0.5).whileTrue(new ElevatorSetpointCommand(elevator, ElevatorState.CLIMB)); From 2498fceda35c181edd55e92492f3246076694c9a Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 29 Mar 2024 21:38:12 -0700 Subject: [PATCH 64/75] fix: stop climb align getting canceled by requirements --- src/main/java/org/team1540/robot2024/RobotContainer.java | 2 +- .../team1540/robot2024/commands/climb/ClimbAlignment.java | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 4a63ffad..ec98aade 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -185,7 +185,7 @@ private void configureButtonBindings() { copilot.rightTrigger(0.95).whileTrue(tramp.commandRun(1)); - copilot.leftTrigger(0.95).whileTrue(new ClimbAlignment(drivetrain, elevator, tramp, indexer, shooter)); + copilot.leftTrigger(0.95).whileTrue(new ClimbAlignment(drivetrain, elevator, tramp, indexer)); copilot.x().whileTrue(new ShootSequence(shooter, indexer)); diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java index 9f2e5022..dc93164d 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java @@ -24,10 +24,9 @@ public class ClimbAlignment extends ParallelRaceGroup { private final Drivetrain drivetrain; - public ClimbAlignment(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Indexer indexer, Shooter shooter){ + public ClimbAlignment(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Indexer indexer){ this.drivetrain = drivetrain; addCommands( - new PrepareShooterCommand(shooter, ()->new ShooterSetpoint(0,0,0)), new SequentialCommandGroup( new ParallelCommandGroup( new SequentialCommandGroup( @@ -44,8 +43,6 @@ public ClimbAlignment(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Ind // Commands.waitSeconds(5), //Confirm that nothing will break new ProxyCommand(() -> climbPath(drivetrain::getPose, 2)), new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.AMP), - new WaitCommand(5), - new RepeatCommand(new PrintCommand("elevator things")), Commands.parallel( Commands.runOnce(()->elevator.setFlipper(false)), new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.TOP) From a1bd7acea15cf95c5966d67409717709ea4f7e0c Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sat, 30 Mar 2024 14:05:58 -0700 Subject: [PATCH 65/75] fix: stop cancelling climb sequence, deal with stupid limelight offset --- .../java/org/team1540/robot2024/Robot.java | 4 ++++ .../commands/climb/ClimbSequence.java | 8 ++------ .../commands/climb/TrapAndClimbSequence.java | 4 ++-- .../subsystems/drive/Drivetrain.java | 19 +++++++++++++++++++ .../vision/AprilTagVisionIOLimelight.java | 6 +++++- 5 files changed, 32 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index a2297b87..50f54fc4 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -19,6 +19,7 @@ import org.team1540.robot2024.util.MechanismVisualiser; import org.team1540.robot2024.util.auto.AutoManager; import org.team1540.robot2024.util.vision.AprilTagsCrescendo; +import org.team1540.robot2024.util.vision.LimelightHelpers; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -105,6 +106,9 @@ public void robotInit() { PathfindingCommand.warmupCommand().schedule(); AprilTagsCrescendo.getInstance().getTag(1); + + // Init driver cam + LimelightHelpers.setCameraMode_Driver("limelight-driver"); } /** diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java index bdfcfe19..1a01db20 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java @@ -14,14 +14,10 @@ public class ClimbSequence extends ParallelRaceGroup { - public ClimbSequence(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Indexer indexer, Shooter shooter, CommandXboxController controller) { + public ClimbSequence(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Indexer indexer, CommandXboxController controller) { addCommands( - Commands.startEnd(() -> { - shooter.stopPivot(); - shooter.setPivotBrakeMode(false); - }, () -> shooter.setPivotBrakeMode(true), shooter), Commands.sequence( - new ClimbAlignment(drivetrain, elevator, tramp, indexer, shooter), + new ClimbAlignment(drivetrain, elevator, tramp, indexer), Commands.waitUntil(controller.a()), new ElevatorSetpointCommand(elevator, ElevatorState.TOP), Commands.waitSeconds(5), // Confirm that nothing will break. Also might need to be tuned if chain does weird things diff --git a/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java index c7dd478a..9b025c60 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java @@ -13,9 +13,9 @@ public class TrapAndClimbSequence extends SequentialCommandGroup { - public TrapAndClimbSequence(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Indexer indexer, Shooter shooter, CommandXboxController controller) { + public TrapAndClimbSequence(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Indexer indexer, CommandXboxController controller) { addCommands( - new ClimbSequence(drivetrain, elevator, tramp, indexer, shooter, controller),//Confirm that nothing will break + new ClimbSequence(drivetrain, elevator, tramp, indexer, controller),//Confirm that nothing will break Commands.waitUntil(controller.a()), new ElevatorSetpointCommand(elevator, ElevatorState.TOP), Commands.runOnce(()->tramp.setDistanceToGo(3)) diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 8bbd7fb2..77cce713 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -1,6 +1,8 @@ package org.team1540.robot2024.subsystems.drive; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; @@ -34,6 +36,7 @@ import org.team1540.robot2024.util.vision.VisionPoseAcceptor; import java.util.Arrays; +import java.util.Optional; import java.util.function.Supplier; import static org.team1540.robot2024.Constants.Drivetrain.*; @@ -54,6 +57,7 @@ public class Drivetrain extends SubsystemBase { private static boolean hasInstance = false; private boolean blockTags = false; + private boolean overrideTargetRot = false; private boolean isCharacterizingWheels = false; private double characterizationInput = 0.0; @@ -102,6 +106,7 @@ private Drivetrain( new HolonomicPathFollowerConfig(new PIDConstants(5.0, 0.0, 0.0),new PIDConstants(7.0, 0.0, 0.0),MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), () -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red, this); + PPHolonomicDriveController.setRotationTargetOverride(this::getOverrideTargetRotationToSpeaker); Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( (activePath) -> Logger.recordOutput("Pathplanner/ActivePath", activePath.toArray(new Pose2d[activePath.size()]))); @@ -381,6 +386,20 @@ public SwerveModulePosition[] getModulePositions() { }; } + public void setPathRotationOverride(boolean shouldOverride) { + overrideTargetRot = shouldOverride; + } + + private Optional getOverrideTargetRotationToSpeaker() { + if (overrideTargetRot) + return Optional.of(getPose() + .minus(Constants.Targeting.getSpeakerPose()).getTranslation().getAngle() + .rotateBy(DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red + ? Rotation2d.fromDegrees(180) + : Rotation2d.fromDegrees(0))); + else return Optional.empty(); + } + /** * Returns the maximum linear speed in meters per sec. */ diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java index 470bbbd2..2e5b088c 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -1,7 +1,9 @@ package org.team1540.robot2024.subsystems.vision; +import com.pathplanner.lib.util.GeometryUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Timer; @@ -21,7 +23,9 @@ public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) { public void updateInputs(AprilTagVisionIOInputs inputs) { LimelightHelpers.PoseEstimate measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); if (measurement.tagCount > 1) { - inputs.estimatedPoseMeters = LimelightHelpers.getBotPose3d_wpiBlue(name); + inputs.estimatedPoseMeters = + LimelightHelpers.getBotPose3d_wpiBlue(name) + .transformBy(new Transform3d(0, 0.105, 0, new Rotation3d())); inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds; } inputs.numTagsSeen = measurement.tagCount; From c79397900afd9766060152fc9200e9190dff9bd3 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Sun, 31 Mar 2024 20:46:30 -0700 Subject: [PATCH 66/75] feat: camas3 --- paths.chor | 10807 +++++++++++++++- src/main/deploy/choreo/AmpLanePADEF.1.traj | 185 + src/main/deploy/choreo/AmpLanePADEF.2.traj | 86 + src/main/deploy/choreo/AmpLanePADEF.3.traj | 446 + src/main/deploy/choreo/AmpLanePADEF.4.traj | 554 + src/main/deploy/choreo/AmpLanePADEF.5.traj | 473 + src/main/deploy/choreo/AmpLanePADEF.traj | 1688 +++ src/main/deploy/choreo/AmpLanePdChaos.2.traj | 599 + src/main/deploy/choreo/AmpLanePdChaos.traj | 770 ++ src/main/deploy/choreo/AmpLanepdChaos.1.traj | 185 + .../deploy/choreo/CenterLanePDEABC.1.traj | 599 + .../deploy/choreo/CenterLanePDEABC.2.traj | 707 + .../deploy/choreo/CenterLanePDEABC.3.traj | 77 + .../deploy/choreo/CenterLanePDEABC.4.traj | 248 + .../deploy/choreo/CenterLanePDEABC.5.traj | 212 + src/main/deploy/choreo/CenterLanePDEABC.traj | 1787 +++ src/main/deploy/choreo/SourceLanePHGF.1.traj | 230 + src/main/deploy/choreo/SourceLanePHGF.2.traj | 635 + src/main/deploy/choreo/SourceLanePHGF.3.traj | 680 + src/main/deploy/choreo/SourceLanePHGF.4.traj | 446 + src/main/deploy/choreo/SourceLanePHGF.traj | 1949 +++ .../deploy/choreo/SourceLanephChaos.1.traj | 1481 +++ src/main/deploy/choreo/SourceLanephChaos.traj | 1481 +++ src/main/deploy/choreo/Tag14.1.traj | 60 +- src/main/deploy/choreo/Tag14.2.traj | 55 +- src/main/deploy/choreo/Tag14.traj | 103 +- src/main/deploy/choreo/Tag15.1.traj | 91 +- src/main/deploy/choreo/Tag15.2.traj | 55 +- src/main/deploy/choreo/Tag15.traj | 130 +- src/main/deploy/choreo/Tag16.1.traj | 91 +- src/main/deploy/choreo/Tag16.2.traj | 55 +- src/main/deploy/choreo/Tag16.traj | 130 +- .../org/team1540/robot2024/Constants.java | 8 +- .../java/org/team1540/robot2024/Robot.java | 5 +- .../team1540/robot2024/RobotContainer.java | 33 +- .../commands/autos/AmpLanePADEF.java | 74 + .../commands/autos/AmpLanepdChaos.java | 35 + .../commands/autos/CenterLanePDEABC.java | 72 + .../commands/autos/SourceLanePGHF.java | 35 + .../commands/autos/SoureLanephChaos.java | 35 + .../commands/climb/ClimbAlignment.java | 2 +- .../subsystems/drive/Drivetrain.java | 3 +- .../subsystems/elevator/Elevator.java | 3 + .../robot2024/subsystems/indexer/Indexer.java | 8 - .../robot2024/util/auto/AutoCommand.java | 7 +- 45 files changed, 26808 insertions(+), 607 deletions(-) create mode 100644 src/main/deploy/choreo/AmpLanePADEF.1.traj create mode 100644 src/main/deploy/choreo/AmpLanePADEF.2.traj create mode 100644 src/main/deploy/choreo/AmpLanePADEF.3.traj create mode 100644 src/main/deploy/choreo/AmpLanePADEF.4.traj create mode 100644 src/main/deploy/choreo/AmpLanePADEF.5.traj create mode 100644 src/main/deploy/choreo/AmpLanePADEF.traj create mode 100644 src/main/deploy/choreo/AmpLanePdChaos.2.traj create mode 100644 src/main/deploy/choreo/AmpLanePdChaos.traj create mode 100644 src/main/deploy/choreo/AmpLanepdChaos.1.traj create mode 100644 src/main/deploy/choreo/CenterLanePDEABC.1.traj create mode 100644 src/main/deploy/choreo/CenterLanePDEABC.2.traj create mode 100644 src/main/deploy/choreo/CenterLanePDEABC.3.traj create mode 100644 src/main/deploy/choreo/CenterLanePDEABC.4.traj create mode 100644 src/main/deploy/choreo/CenterLanePDEABC.5.traj create mode 100644 src/main/deploy/choreo/CenterLanePDEABC.traj create mode 100644 src/main/deploy/choreo/SourceLanePHGF.1.traj create mode 100644 src/main/deploy/choreo/SourceLanePHGF.2.traj create mode 100644 src/main/deploy/choreo/SourceLanePHGF.3.traj create mode 100644 src/main/deploy/choreo/SourceLanePHGF.4.traj create mode 100644 src/main/deploy/choreo/SourceLanePHGF.traj create mode 100644 src/main/deploy/choreo/SourceLanephChaos.1.traj create mode 100644 src/main/deploy/choreo/SourceLanephChaos.traj create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADEF.java create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/AmpLanepdChaos.java create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHF.java create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/SoureLanephChaos.java diff --git a/paths.chor b/paths.chor index ef0fb7a5..99ff7769 100644 --- a/paths.chor +++ b/paths.chor @@ -10,7 +10,7 @@ "trackWidth": 0.5016497291091463, "bumperLength": 0.8635995336562519, "bumperWidth": 0.8000995679462333, - "wheelRadius": 0.0500888 + "wheelRadius": 0.04933448212790457 }, "paths": { "Taxi": { @@ -1034,13 +1034,13 @@ "Tag14": { "waypoints": [ { - "x": 5.65, + "x": 6, "y": 4.105148, "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 2 + "controlIntervalCount": 6 }, { "x": 5.67, @@ -1049,10 +1049,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 4 }, { - "x": 5.87, + "x": 5.82, "y": 4.105148, "heading": 3.141592653589793, "isInitialGuess": false, @@ -1063,92 +1063,119 @@ ], "trajectory": [ { - "x": 5.65, + "x": 6, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.9898168855172235e-35, - "velocityX": -4.1906548586207977e-19, - "velocityY": -8.603933715604811e-45, + "angularVelocity": -5.012651915780492e-33, + "velocityX": 8.02537183018205e-19, + "velocityY": -7.958388468544976e-47, "timestamp": 0 }, { - "x": 5.67, + "x": 5.9633333301031834, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -6.117923804140079e-22, + "velocityX": -0.40300955210174366, + "velocityY": -2.1307554430713835e-36, + "timestamp": 0.09098213604515144 + }, + { + "x": 5.889999993541086, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -7.100860879914842e-22, + "velocityX": -0.8060190686851338, + "velocityY": 3.483301093527144e-37, + "timestamp": 0.1819642720903029 + }, + { + "x": 5.780000006458914, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 1.1538730689460168e-21, + "velocityX": -1.2090284078138351, + "velocityY": 9.187359888346366e-37, + "timestamp": 0.27294640813545434 + }, + { + "x": 5.7066666698968165, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.9404859008459245e-25, - "velocityX": 0.2953925521078346, - "velocityY": 2.5211469248369934e-41, - "timestamp": 0.06770651411921333 + "angularVelocity": 3.07203086725026e-22, + "velocityX": -0.8060190686851337, + "velocityY": -9.782309229551151e-38, + "timestamp": 0.3639285441806058 }, { "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.9894322566378973e-35, - "velocityX": -9.732091066591102e-19, - "velocityY": -8.603933345990491e-45, - "timestamp": 0.13541302823842666 + "angularVelocity": -1.39204485684163e-22, + "velocityX": -0.40300955210174366, + "velocityY": 9.615122274536806e-37, + "timestamp": 0.4549106802257572 }, { - "x": 5.7033333348623145, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 6.224455028031798e-27, - "velocityX": 0.3813501839335422, - "velocityY": -7.87167491563539e-43, - "timestamp": 0.22282175718804903 + "angularVelocity": -4.017860840401767e-33, + "velocityX": 6.500049543311854e-19, + "velocityY": -4.971127395478411e-47, + "timestamp": 0.5458928162709087 }, { - "x": 5.770000000000059, + "x": 5.707500002722882, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.1939294645306622e-26, - "velocityX": 0.7627003153894066, - "velocityY": -1.6493125386550898e-42, - "timestamp": 0.3102304861376714 + "angularVelocity": 1.9645071480983067e-28, + "velocityX": 0.4075634710831348, + "velocityY": 3.190135159299058e-45, + "timestamp": 0.6379030315711736 }, { - "x": 5.836666665137633, + "x": 5.782499997277117, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.2599779930508123e-26, - "velocityX": 0.7627003153894064, - "velocityY": -1.6423599321913863e-42, - "timestamp": 0.3976392150872938 + "angularVelocity": 4.103505651639184e-28, + "velocityX": 0.8151268237932148, + "velocityY": 1.0316108499249623e-44, + "timestamp": 0.7299132468714384 }, { - "x": 5.87, + "x": 5.82, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 6.4603054830567416e-27, - "velocityX": 0.38135018393354214, - "velocityY": -7.88271785574287e-43, - "timestamp": 0.48504794403691615 + "angularVelocity": 1.95973609278201e-28, + "velocityX": 0.40756347108313484, + "velocityY": 5.478051332325139e-45, + "timestamp": 0.8219234621717033 }, { - "x": 5.87, + "x": 5.82, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -8.84678881735324e-46, - "velocityX": -2.5654038297220854e-19, - "velocityY": -1.971878449457756e-53, - "timestamp": 0.5724566729865386 + "angularVelocity": -1.4408058549587707e-43, + "velocityX": -8.120067364727127e-19, + "velocityY": -3.194160598149635e-55, + "timestamp": 0.9139336774719682 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 5.65, + "x": 6, "y": 4.105148, "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 2 + "controlIntervalCount": 6 }, { - "timestamp": 0.13541302823842666, + "timestamp": 0.5458928162709087, "isStopPoint": true, "x": 5.67, "y": 4.105148, @@ -1156,12 +1183,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 4 }, { - "timestamp": 0.5724566729865386, + "timestamp": 0.9139336774719682, "isStopPoint": true, - "x": 5.87, + "x": 5.82, "y": 4.105148, "heading": 3.141592653589793, "isInitialGuess": false, @@ -1200,26 +1227,26 @@ "Tag15": { "waypoints": [ { - "x": 4.189568967671509, - "y": 5.279922480109792, + "x": 4.301737995, + "y": 5.085640025741264, "heading": -1.0471975512, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { - "x": 4.416737995, - "y": 4.886454182870843, + "x": 4.466737995, + "y": 4.799851642492399, "heading": -1.0471975512, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 3 + "controlIntervalCount": 4 }, { - "x": 4.388737995, - "y": 4.934951605482771, + "x": 4.391737995, + "y": 4.929755453060065, "heading": -1.0471975512, "isInitialGuess": false, "translationConstrained": true, @@ -1229,133 +1256,133 @@ ], "trajectory": [ { - "x": 4.189568967671509, - "y": 5.279922480109792, + "x": 4.301737995, + "y": 5.085640025741264, "heading": -1.0471975512, - "angularVelocity": -8.682191393976642e-30, - "velocityX": -8.883974143721003e-20, - "velocityY": 1.5386233886481345e-19, + "angularVelocity": 2.89796352109676e-29, + "velocityX": -4.014735819374218e-19, + "velocityY": 6.953992905350176e-19, "timestamp": 0 }, { - "x": 4.208499721112847, - "y": 5.247133453323834, + "x": 4.3200713299484095, + "y": 5.053885758138444, "heading": -1.0471975512, - "angularVelocity": -9.594003781408746e-18, - "velocityX": 0.2032139569887226, - "velocityY": -0.35197689831158363, - "timestamp": 0.09315675813738096 + "angularVelocity": -2.7521954694009315e-18, + "velocityX": 0.20150477605089573, + "velocityY": -0.3490165100878853, + "timestamp": 0.09098213604515161 }, { - "x": 4.246361226831558, - "y": 5.181555401767972, + "x": 4.356737998229468, + "y": 4.9903772257313985, "heading": -1.0471975512, - "angularVelocity": -1.757511495393616e-18, - "velocityX": 0.4064279014827589, - "velocityY": -0.7039537749817357, - "timestamp": 0.18631351627476192 + "angularVelocity": -2.0944138451736214e-17, + "velocityX": 0.4030095343426707, + "velocityY": -0.6980329894159418, + "timestamp": 0.18196427209030322 }, { - "x": 4.303153481335755, - "y": 5.083188331490316, + "x": 4.411737991770512, + "y": 4.895114442502254, "heading": -1.0471975512, - "angularVelocity": -4.4708927110588736e-18, - "velocityX": 0.6096418084927427, - "velocityY": -1.0559305867276043, - "timestamp": 0.2794702744121429 + "angularVelocity": -1.0707290083549787e-17, + "velocityX": 0.6045142039064659, + "velocityY": -1.0470493150640965, + "timestamp": 0.27294640813545484 }, { - "x": 4.35994573583996, - "y": 4.984821261212667, + "x": 4.448404660051591, + "y": 4.83160591009522, "heading": -1.0471975512, - "angularVelocity": -2.3203957666119342e-17, - "velocityX": 0.6096418084927429, - "velocityY": -1.055930586727604, - "timestamp": 0.37262703254952384 + "angularVelocity": 2.3522281076555332e-17, + "velocityX": 0.40300953434289255, + "velocityY": -0.6980329894158137, + "timestamp": 0.36392854418060644 }, { - "x": 4.397807241558656, - "y": 4.919243209656798, + "x": 4.466737995, + "y": 4.799851642492399, "heading": -1.0471975512, - "angularVelocity": 3.512195891842984e-17, - "velocityX": 0.406427901482759, - "velocityY": -0.7039537749817355, - "timestamp": 0.4657837906869048 + "angularVelocity": 1.0881053432643224e-17, + "velocityX": 0.20150477605087666, + "velocityY": -0.34901651008789636, + "timestamp": 0.45491068022575804 }, { - "x": 4.416737995000001, - "y": 4.8864541828708425, + "x": 4.466737995, + "y": 4.799851642492399, "heading": -1.0471975512, - "angularVelocity": 3.9044172518056536e-18, - "velocityX": 0.20321395698872258, - "velocityY": -0.35197689831158346, - "timestamp": 0.5589405488242858 + "angularVelocity": -5.284169622642589e-29, + "velocityX": -3.257038644202628e-19, + "velocityY": 5.641870908245426e-19, + "timestamp": 0.5458928162709097 }, { - "x": 4.416737995, - "y": 4.886454182870843, + "x": 4.4479879936385585, + "y": 4.832327597492402, "heading": -1.0471975512, - "angularVelocity": -6.644908206829601e-29, - "velocityX": -6.863436538799463e-20, - "velocityY": 1.1889252306500327e-19, - "timestamp": 0.6520973069616668 + "angularVelocity": 1.6934506076200052e-18, + "velocityX": -0.2037817355415684, + "velocityY": 0.35296031961255897, + "timestamp": 0.6379030315711746 }, { - "x": 4.402737994999999, - "y": 4.910702894176807, + "x": 4.410487996361441, + "y": 4.897279498060063, "heading": -1.0471975512, - "angularVelocity": 2.5156332234400082e-17, - "velocityX": -0.1747566018550504, - "velocityY": 0.30268731337101645, - "timestamp": 0.7322087296569455 + "angularVelocity": -5.718462154811666e-17, + "velocityX": -0.40756341189660933, + "velocityY": 0.7059205367110453, + "timestamp": 0.7299132468714394 }, { - "x": 4.388737995, - "y": 4.934951605482771, + "x": 4.391737995, + "y": 4.929755453060065, "heading": -1.0471975512, - "angularVelocity": -2.5156340257881864e-17, - "velocityX": -0.1747566018550504, - "velocityY": 0.3026873133710164, - "timestamp": 0.8123201523522241 + "angularVelocity": 5.550395167887549e-17, + "velocityX": -0.20378173554156837, + "velocityY": 0.35296031961255897, + "timestamp": 0.8219234621717043 }, { - "x": 4.388737995, - "y": 4.934951605482771, + "x": 4.391737995, + "y": 4.929755453060065, "heading": -1.0471975512, - "angularVelocity": -1.0978097374367874e-28, - "velocityX": 1.0569070815560555e-19, - "velocityY": -1.830330405016386e-19, - "timestamp": 0.8924315750475027 + "angularVelocity": -6.837213333867933e-28, + "velocityX": 4.0578533921404514e-19, + "velocityY": -7.0284075232843425e-19, + "timestamp": 0.9139336774719692 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 4.189568967671509, - "y": 5.279922480109792, + "x": 4.301737995, + "y": 5.085640025741264, "heading": -1.0471975512, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { - "timestamp": 0.6520973069616668, + "timestamp": 0.5458928162709097, "isStopPoint": true, - "x": 4.416737995, - "y": 4.886454182870843, + "x": 4.466737995, + "y": 4.799851642492399, "heading": -1.0471975512, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 3 + "controlIntervalCount": 4 }, { - "timestamp": 0.8924315750475027, + "timestamp": 0.9139336774719692, "isStopPoint": true, - "x": 4.388737995, - "y": 4.934951605482771, + "x": 4.391737995, + "y": 4.929755453060065, "heading": -1.0471975512, "isInitialGuess": false, "translationConstrained": true, @@ -1388,31 +1415,31 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": true + "isTrajectoryStale": false }, "Tag16": { "waypoints": [ { - "x": 4.189568967671509, - "y": 2.930373519890208, + "x": 4.301737995, + "y": 3.124655974258736, "heading": 1.0471975512, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { - "x": 4.416737995, - "y": 3.323841817129157, + "x": 4.466737995, + "y": 3.410444357507601, "heading": 1.0471975512, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 3 + "controlIntervalCount": 4 }, { - "x": 4.388737995, - "y": 3.275344394517229, + "x": 4.391737995, + "y": 3.280540546939935, "heading": 1.0471975512, "isInitialGuess": false, "translationConstrained": true, @@ -1422,133 +1449,133 @@ ], "trajectory": [ { - "x": 4.189568967671509, - "y": 2.930373519890208, + "x": 4.301737995, + "y": 3.124655974258736, "heading": 1.0471975512, - "angularVelocity": -2.002499149483344e-30, - "velocityX": -8.884646650147337e-20, - "velocityY": -1.5387579641415467e-19, + "angularVelocity": -7.335351200507843e-30, + "velocityX": -4.0149214769441807e-19, + "velocityY": -6.95484705410363e-19, "timestamp": 0 }, { - "x": 4.208499721112847, - "y": 2.9631625466761657, + "x": 4.320071329948398, + "y": 3.1564102418615634, "heading": 1.0471975512, - "angularVelocity": -7.928469907031793e-18, - "velocityX": 0.20321395698872408, - "velocityY": 0.35197689831158385, - "timestamp": 0.0931567581373812 + "angularVelocity": 5.394453296191308e-18, + "velocityX": 0.20150477605075734, + "velocityY": 0.34901651008796514, + "timestamp": 0.09098213604515158 }, { - "x": 4.2463612268315565, - "y": 3.0287405982320337, + "x": 4.356737998229433, + "y": 3.2199187742686215, "heading": 1.0471975512, - "angularVelocity": -2.0524140454745182e-17, - "velocityX": 0.40642790148276187, - "velocityY": 0.7039537749817363, - "timestamp": 0.1863135162747624 + "angularVelocity": 3.284063714220586e-17, + "velocityX": 0.40300953434242154, + "velocityY": 0.6980329894160855, + "timestamp": 0.18196427209030316 }, { - "x": 4.303153481335761, - "y": 3.127107668509687, + "x": 4.411737991770526, + "y": 3.315181557497739, "heading": 1.0471975512, - "angularVelocity": 5.366441184028338e-17, - "velocityX": 0.6096418084927471, - "velocityY": 1.055930586727605, - "timestamp": 0.27947027441214356 + "angularVelocity": -7.726708537325842e-18, + "velocityX": 0.6045142039069931, + "velocityY": 1.0470493150637918, + "timestamp": 0.27294640813545473 }, { - "x": 4.3599457358399505, - "y": 3.225474738787313, + "x": 4.448404660051587, + "y": 3.3786900899047825, "heading": 1.0471975512, - "angularVelocity": -4.609538572640182e-17, - "velocityX": 0.6096418084927472, - "velocityY": 1.0559305867276048, - "timestamp": 0.3726270325495248 + "angularVelocity": -3.8700184388956635e-17, + "velocityX": 0.40300953434269476, + "velocityY": 0.6980329894159277, + "timestamp": 0.36392854418060633 }, { - "x": 4.397807241558657, - "y": 3.291052790343219, + "x": 4.466737995, + "y": 3.410444357507601, "heading": 1.0471975512, - "angularVelocity": 9.988825598390611e-18, - "velocityX": 0.4064279014827619, - "velocityY": 0.7039537749817361, - "timestamp": 0.465783790686906 + "angularVelocity": 8.191735638164562e-18, + "velocityX": 0.20150477605093248, + "velocityY": 0.34901651008786405, + "timestamp": 0.4549106802257579 }, { - "x": 4.416737994999997, - "y": 3.323841817129139, + "x": 4.466737995, + "y": 3.410444357507601, "heading": 1.0471975512, - "angularVelocity": 1.0894764403980658e-17, - "velocityX": 0.20321395698872408, - "velocityY": 0.35197689831158374, - "timestamp": 0.5589405488242872 + "angularVelocity": -8.684839191459878e-29, + "velocityX": -3.2590018149845075e-19, + "velocityY": -5.6430407567783995e-19, + "timestamp": 0.5458928162709095 }, { - "x": 4.416737995, - "y": 3.323841817129157, + "x": 4.4479879936385585, + "y": 3.377968402507599, "heading": 1.0471975512, - "angularVelocity": -2.786574070680907e-28, - "velocityX": -6.865066631576706e-20, - "velocityY": -1.1890733865081739e-19, - "timestamp": 0.6520973069616685 + "angularVelocity": 1.3525551603206105e-18, + "velocityX": -0.2037817355415677, + "velocityY": -0.3529603196125604, + "timestamp": 0.6379030315711746 }, { - "x": 4.402737994999996, - "y": 3.299593105823197, + "x": 4.410487996361441, + "y": 3.3130165019399365, "heading": 1.0471975512, - "angularVelocity": 1.2203521575276725e-17, - "velocityX": -0.17475660185504371, - "velocityY": -0.30268731337102, - "timestamp": 0.732208729656947 + "angularVelocity": 1.8398786975520478e-17, + "velocityX": -0.40756341189660816, + "velocityY": -0.7059205367110482, + "timestamp": 0.7299132468714397 }, { - "x": 4.388737995, - "y": 3.275344394517229, + "x": 4.391737995, + "y": 3.280540546939935, "heading": 1.0471975512, - "angularVelocity": -1.2203535086762994e-17, - "velocityX": -0.17475660185504374, - "velocityY": -0.30268731337102006, - "timestamp": 0.8123201523522257 + "angularVelocity": -1.974689064536144e-17, + "velocityX": -0.20378173554156773, + "velocityY": -0.35296031961256047, + "timestamp": 0.8219234621717048 }, { - "x": 4.388737995, - "y": 3.275344394517229, + "x": 4.391737995, + "y": 3.280540546939935, "heading": 1.0471975512, - "angularVelocity": -5.52100607399031e-28, - "velocityX": 1.0568999281101467e-19, - "velocityY": 1.8303406915772078e-19, - "timestamp": 0.8924315750475043 + "angularVelocity": -5.432293012393194e-29, + "velocityX": 4.058597366698449e-19, + "velocityY": 7.02969590180103e-19, + "timestamp": 0.9139336774719699 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 4.189568967671509, - "y": 2.930373519890208, + "x": 4.301737995, + "y": 3.124655974258736, "heading": 1.0471975512, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { - "timestamp": 0.6520973069616685, + "timestamp": 0.5458928162709095, "isStopPoint": true, - "x": 4.416737995, - "y": 3.323841817129157, + "x": 4.466737995, + "y": 3.410444357507601, "heading": 1.0471975512, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 3 + "controlIntervalCount": 4 }, { - "timestamp": 0.8924315750475043, + "timestamp": 0.9139336774719699, "isStopPoint": true, - "x": 4.388737995, - "y": 3.275344394517229, + "x": 4.391737995, + "y": 3.280540546939935, "heading": 1.0471975512, "isInitialGuess": false, "translationConstrained": true, @@ -1581,7 +1608,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": true + "isTrajectoryStale": false }, "SourceLanePSubHSubSprint": { "waypoints": [ @@ -26864,6 +26891,10352 @@ "circleObstacles": [], "eventMarkers": [], "isTrajectoryStale": true + }, + "AmpLanePADEF": { + "waypoints": [ + { + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 + }, + { + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": -2.0223823950436125e-24, + "angularVelocity": -1.0530930101357375e-37, + "velocityX": 9.679119214488622e-24, + "velocityY": 2.8468005585003605e-23, + "timestamp": 0 + }, + { + "x": 0.45358539197474673, + "y": 6.976442037280005, + "heading": -4.051643418810716e-24, + "angularVelocity": -1.1805501930230392e-25, + "velocityX": 0.3537634154949626, + "velocityY": -0.0735607445986989, + "timestamp": 0.05826629620784151 + }, + { + "x": 0.49483104636042347, + "y": 6.967970532727312, + "heading": -6.080913911985301e-24, + "angularVelocity": -1.1821753877066713e-25, + "velocityX": 0.7078818643036704, + "velocityY": -0.1453928789719994, + "timestamp": 0.11653259241568302 + }, + { + "x": 0.5567354919848906, + "y": 6.95544199101986, + "heading": -8.110182775739579e-24, + "angularVelocity": -1.1818957371343575e-25, + "velocityX": 1.0624400322898162, + "velocityY": -0.21502210579442033, + "timestamp": 0.17479888862352452 + }, + { + "x": 0.6393312280569997, + "y": 6.9390255615072824, + "heading": -1.013944960655158e-23, + "angularVelocity": -1.1815468317795835e-25, + "velocityX": 1.4175559705645637, + "velocityY": -0.28174829328466466, + "timestamp": 0.23306518483136604 + }, + { + "x": 0.7426606798558012, + "y": 6.918954250671649, + "heading": -1.2168715161261342e-23, + "angularVelocity": -1.181327819705339e-25, + "velocityX": 1.7734000361068998, + "velocityY": -0.3444754882657602, + "timestamp": 0.29133148103920753 + }, + { + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": -1.4479605554027417e-23, + "angularVelocity": -4.951541740242581e-24, + "velocityX": 2.130226970895328, + "velocityY": -0.4013456783802647, + "timestamp": 0.34959777724704905 + }, + { + "x": 1.0109741912909123, + "y": 6.869549473554426, + "heading": 0.010039096014815363, + "angularVelocity": 0.17297490390919232, + "velocityX": 2.484465074043925, + "velocityY": -0.44832534814487107, + "timestamp": 0.4076356531928316 + }, + { + "x": 1.1757265724080805, + "y": 6.8408027772938045, + "heading": 0.030114435462749452, + "angularVelocity": 0.3459006574721642, + "velocityX": 2.83870452583543, + "velocityY": -0.49530924059791875, + "timestamp": 0.4656735291386142 + }, + { + "x": 1.3610383679326326, + "y": 6.809328854359183, + "heading": 0.060221992176283975, + "angularVelocity": 0.5187570396556241, + "velocityX": 3.1929458565586635, + "velocityY": -0.5422997038007265, + "timestamp": 0.5237114050843967 + }, + { + "x": 1.546079908472426, + "y": 6.784713461447441, + "heading": 0.1506024575679624, + "angularVelocity": 1.5572669385094218, + "velocityX": 3.188289328724817, + "velocityY": -0.4241263573246083, + "timestamp": 0.5817492810301793 + }, + { + "x": 1.710561311165979, + "y": 6.762828401312947, + "heading": 0.2309691373975153, + "angularVelocity": 1.3847281369261228, + "velocityX": 2.834035533057883, + "velocityY": -0.37708237556692, + "timestamp": 0.6397871569759619 + }, + { + "x": 1.8544824093337007, + "y": 6.743675274338349, + "heading": 0.3013204880160867, + "angularVelocity": 1.2121627380762827, + "velocityX": 2.4797788654803434, + "velocityY": -0.3300108190122258, + "timestamp": 0.6978250329217445 + }, + { + "x": 1.9778431850692357, + "y": 6.727255600598568, + "heading": 0.36164875379241124, + "angularVelocity": 1.0394637087112153, + "velocityX": 2.12552188937403, + "velocityY": -0.28291307137290855, + "timestamp": 0.755862908867527 + }, + { + "x": 2.0806437247377927, + "y": 6.713570715572485, + "heading": 0.4119419860379565, + "angularVelocity": 0.8665588019197638, + "velocityX": 1.7712664013512611, + "velocityY": -0.23579231326223515, + "timestamp": 0.8139007848133096 + }, + { + "x": 2.16288415503368, + "y": 6.702621710000965, + "heading": 0.45218713038219516, + "angularVelocity": 0.6934289666602311, + "velocityX": 1.4170130962875669, + "velocityY": -0.18865276154743782, + "timestamp": 0.8719386607590922 + }, + { + "x": 2.2245645766627815, + "y": 6.694409403887936, + "heading": 0.48237322491294377, + "angularVelocity": 0.5201102562565806, + "velocityX": 1.062761526399103, + "velocityY": -0.14149908106046907, + "timestamp": 0.9299765367048748 + }, + { + "x": 2.265685008078599, + "y": 6.688934342765243, + "heading": 0.5024941236633007, + "angularVelocity": 0.3466856500598576, + "velocityX": 0.7085102744668164, + "velocityY": -0.09433600099024221, + "timestamp": 0.9880144126506574 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.1732712607555392, + "velocityX": 0.35425724418212445, + "velocityY": -0.04716814104583245, + "timestamp": 1.0460522885964398 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -7.633423863817013e-25, + "velocityX": -5.0744692988574966e-24, + "velocityY": 1.0116864366107383e-24, + "timestamp": 1.1040901645422223 + }, + { + "x": 2.3347330882331176, + "y": 6.713955255875249, + "heading": 0.5125504196, + "angularVelocity": -3.160328817220775e-17, + "velocityX": 0.51083603498144, + "velocityY": 0.292445406541253, + "timestamp": 1.199008570534359 + }, + { + "x": 2.4317085702199166, + "y": 6.7694721581924355, + "heading": 0.5125504196, + "angularVelocity": -2.914232468557734e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645907, + "timestamp": 1.2939269765264956 + }, + { + "x": 2.577171786567949, + "y": 6.852747507871406, + "heading": 0.5125504196, + "angularVelocity": -1.252738056302556e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461287, + "timestamp": 1.3888453825186322 + }, + { + "x": 2.771122709968306, + "y": 6.963781289278252, + "heading": 0.5125504196, + "angularVelocity": 1.2751776994126907e-16, + "velocityX": 2.0433436631502695, + "velocityY": 1.1697813532187185, + "timestamp": 1.4837637885107688 + }, + { + "x": 2.9165859263163383, + "y": 7.047056638957223, + "heading": 0.5125504196, + "angularVelocity": 7.51589809363944e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461287, + "timestamp": 1.5786821945029055 + }, + { + "x": 3.0135614083031372, + "y": 7.102573541274409, + "heading": 0.5125504196, + "angularVelocity": 6.994323112372533e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645907, + "timestamp": 1.6736006004950421 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": -4.84539672491667e-17, + "velocityX": 0.51083603498144, + "velocityY": 0.29244540654125295, + "timestamp": 1.7685190064871787 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": 9.145149251733134e-29, + "velocityX": 3.330473944863186e-25, + "velocityY": 2.452283341762288e-25, + "timestamp": 1.8634374124793154 + }, + { + "x": 3.0904722904343656, + "y": 7.132620894810973, + "heading": 0.4951032659572505, + "angularVelocity": -0.25584290647295194, + "velocityX": 0.41679341451670293, + "velocityY": 0.03356417198946019, + "timestamp": 1.9316322038975173 + }, + { + "x": 3.1473436628615548, + "y": 7.13720088972522, + "heading": 0.4609074027399865, + "angularVelocity": -0.5014439153799775, + "velocityX": 0.8339547822417649, + "velocityY": 0.06716047984016284, + "timestamp": 1.9998269953157193 + }, + { + "x": 3.232694908233173, + "y": 7.144074686546732, + "heading": 0.4108880311553055, + "angularVelocity": -0.7334778880389725, + "velocityX": 1.2515801221269964, + "velocityY": 0.10079650774733914, + "timestamp": 2.068021786733921 + }, + { + "x": 3.3465670290823137, + "y": 7.153245490430184, + "heading": 0.3463288656587193, + "angularVelocity": -0.9466876304478969, + "velocityX": 1.6698067180941178, + "velocityY": 0.1344795356468125, + "timestamp": 2.1362165781521227 + }, + { + "x": 3.489013918523337, + "y": 7.164717369527402, + "heading": 0.26910560921909454, + "angularVelocity": -1.132392296151419, + "velocityX": 2.0888235960349633, + "velocityY": 0.16822221842234344, + "timestamp": 2.2044113695703245 + }, + { + "x": 3.6601053694274515, + "y": 7.178495513325157, + "heading": 0.18214628797045526, + "angularVelocity": -1.2751607482067742, + "velocityX": 2.5088639080205257, + "velocityY": 0.2020409991909967, + "timestamp": 2.272606160988526 + }, + { + "x": 3.8599204497819497, + "y": 7.19458405385975, + "heading": 0.0905625121816468, + "angularVelocity": -1.3429731785111632, + "velocityX": 2.9300636632076498, + "velocityY": 0.23592037162970983, + "timestamp": 2.340800952406728 + }, + { + "x": 4.088431686194158, + "y": 7.212973174468024, + "heading": 0.005865325294296566, + "angularVelocity": -1.2419890892831975, + "velocityX": 3.350860551957296, + "velocityY": 0.26965579373215276, + "timestamp": 2.4089957438249296 + }, + { + "x": 4.340105997195649, + "y": 7.232705863139766, + "heading": 3.5066831801673635e-7, + "angularVelocity": -0.08600326365120493, + "velocityX": 3.6905210173326544, + "velocityY": 0.2893577098979943, + "timestamp": 2.4771905352431314 + }, + { + "x": 4.593304664542687, + "y": 7.253186623137088, + "heading": 3.187896158571913e-7, + "angularVelocity": -4.67465351775184e-7, + "velocityX": 3.712873990541412, + "velocityY": 0.30032733543716505, + "timestamp": 2.545385326661333 + }, + { + "x": 4.846503330689328, + "y": 7.273667397974732, + "heading": 2.869109349284145e-7, + "angularVelocity": -4.674650404498194e-7, + "velocityX": 3.7128739729389193, + "velocityY": 0.30032755305381253, + "timestamp": 2.613580118079535 + }, + { + "x": 5.0997019968359405, + "y": 7.294148172812728, + "heading": 2.550322540852e-7, + "angularVelocity": -4.674650391951449e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589851, + "timestamp": 2.6817749094977366 + }, + { + "x": 5.352900662982552, + "y": 7.314628947650725, + "heading": 2.2315357326181484e-7, + "angularVelocity": -4.674650389043701e-7, + "velocityX": 3.712873972938501, + "velocityY": 0.30032755305898534, + "timestamp": 2.7499697009159383 + }, + { + "x": 5.606099329129164, + "y": 7.335109722488721, + "heading": 1.9127489242002956e-7, + "angularVelocity": -4.6746503917418713e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.30032755305898523, + "timestamp": 2.81816449233414 + }, + { + "x": 5.859297995275776, + "y": 7.355590497326717, + "heading": 1.593962115667357e-7, + "angularVelocity": -4.6746503934294765e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.30032755305898523, + "timestamp": 2.8863592837523417 + }, + { + "x": 6.112496661422388, + "y": 7.3760712721647135, + "heading": 1.2751753069996381e-7, + "angularVelocity": -4.674650395405871e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589853, + "timestamp": 2.9545540751705435 + }, + { + "x": 6.365695327569, + "y": 7.39655204700271, + "heading": 9.56388499052934e-8, + "angularVelocity": -4.674650384833005e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589854, + "timestamp": 3.022748866588745 + }, + { + "x": 6.618893993715612, + "y": 7.41703282184071, + "heading": 6.376016904700571e-8, + "angularVelocity": -4.6746503941617634e-7, + "velocityX": 3.7128739729384974, + "velocityY": 0.300327553059032, + "timestamp": 3.090943658006947 + }, + { + "x": 6.872092659851402, + "y": 7.437513596812488, + "heading": 3.188148827519539e-8, + "angularVelocity": -4.674650381480829e-7, + "velocityX": 3.7128739727798186, + "velocityY": 0.3003275550207428, + "timestamp": 3.1591384494251487 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": 2.875044920599443e-26, + "angularVelocity": -4.675062070310236e-7, + "velocityX": 3.7128672960134566, + "velocityY": 0.300410086481236, + "timestamp": 3.2273332408433504 + }, + { + "x": 7.327398255820751, + "y": 7.485125041057045, + "heading": -0.0034915599283927, + "angularVelocity": -0.05859476692705873, + "velocityX": 3.3917318820879983, + "velocityY": 0.4552078415437867, + "timestamp": 3.2869214961006694 + }, + { + "x": 7.508458450228073, + "y": 7.505782624333376, + "heading": -0.006110359017821738, + "angularVelocity": -0.04394824245348874, + "velocityX": 3.038521494302052, + "velocityY": 0.3466720612497382, + "timestamp": 3.3465097513579884 + }, + { + "x": 7.668471533847549, + "y": 7.519972433195907, + "heading": -0.00785620988870458, + "angularVelocity": -0.029298573407523957, + "velocityX": 2.68531244837584, + "velocityY": 0.23813096727293456, + "timestamp": 3.4060980066153075 + }, + { + "x": 7.807437533254427, + "y": 7.527694362359166, + "heading": -0.008729041834027805, + "angularVelocity": -0.01464771776844422, + "velocityX": 2.332103848430922, + "velocityY": 0.12958810641313134, + "timestamp": 3.4656862618726265 + }, + { + "x": 7.925356461622809, + "y": 7.528948359353389, + "heading": -0.008728854851904898, + "angularVelocity": 0.0000031379022946504295, + "velocityX": 1.9788954695715617, + "velocityY": 0.021044365014674563, + "timestamp": 3.5252745171299456 + }, + { + "x": 8.022228326704038, + "y": 7.523734392879376, + "heading": -0.007855710298573708, + "angularVelocity": 0.0146529639030494, + "velocityX": 1.6256872207939095, + "velocityY": -0.0874999016416258, + "timestamp": 3.5848627723872646 + }, + { + "x": 8.098053133481649, + "y": 7.512052442276777, + "heading": -0.006109727375299004, + "angularVelocity": 0.029300789488382373, + "velocityX": 1.272479055649084, + "velocityY": -0.19604451501648842, + "timestamp": 3.6444510276445836 + }, + { + "x": 8.152830885308017, + "y": 7.493902493017176, + "heading": -0.0034910810079803226, + "angularVelocity": 0.04394567949691789, + "velocityX": 0.919270946763285, + "velocityY": -0.30458937220470644, + "timestamp": 3.7040392829019027 + }, + { + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 2.1197622073376246e-26, + "angularVelocity": 0.058586729765871357, + "velocityX": 0.5660628762997177, + "velocityY": -0.41313440805613916, + "timestamp": 3.7636275381592217 + }, + { + "x": 8.185948757189772, + "y": 7.412401444181256, + "heading": 0.007947733329215964, + "angularVelocity": 0.08230133185775951, + "velocityX": -0.0063460233869197455, + "velocityY": -0.5890426736452052, + "timestamp": 3.8601962467063924 + }, + { + "x": 8.130028903552427, + "y": 7.338629187384324, + "heading": 0.018106821496109617, + "angularVelocity": 0.10520062160644154, + "velocityX": -0.5790680488393326, + "velocityY": -0.7639354186961682, + "timestamp": 3.956764955253563 + }, + { + "x": 8.018757435307506, + "y": 7.248113553962043, + "heading": 0.03036067277368815, + "angularVelocity": 0.12689256656666278, + "velocityX": -1.1522517999768782, + "velocityY": -0.9373184625128009, + "timestamp": 4.053333663800734 + }, + { + "x": 7.852062035550644, + "y": 7.141094085596084, + "heading": 0.044518821199191705, + "angularVelocity": 0.14661217529473056, + "velocityX": -1.7261844158911652, + "velocityY": -1.1082209752622316, + "timestamp": 4.1499023723479045 + }, + { + "x": 7.629805209867524, + "y": 7.0180373220836785, + "heading": 0.060212869798815634, + "angularVelocity": 0.16251691501039256, + "velocityX": -2.301540830636187, + "velocityY": -1.2742923185339627, + "timestamp": 4.246471080895075 + }, + { + "x": 7.351625362652527, + "y": 6.880249252941204, + "heading": 0.07642107570328951, + "angularVelocity": 0.16784117907672624, + "velocityX": -2.8806416840411067, + "velocityY": -1.4268397208104806, + "timestamp": 4.343039789442246 + }, + { + "x": 7.016267448829526, + "y": 6.750124387193528, + "heading": 0.07642107964742734, + "angularVelocity": 4.0842814169097235e-8, + "velocityX": -3.4727389323964073, + "velocityY": -1.347484787829737, + "timestamp": 4.439608497989417 + }, + { + "x": 6.673392879085966, + "y": 6.641338156398843, + "heading": 0.0764210813675651, + "angularVelocity": 1.7812579245729294e-8, + "velocityX": -3.5505763192025745, + "velocityY": -1.1265163677895376, + "timestamp": 4.536177206536587 + }, + { + "x": 6.330518244592264, + "y": 6.532552129685128, + "heading": 0.0764210830877029, + "angularVelocity": 1.7812579496548032e-8, + "velocityX": -3.550576989711091, + "velocityY": -1.1265142544655247, + "timestamp": 4.632745915083758 + }, + { + "x": 5.987643610098044, + "y": 6.42376610297305, + "heading": 0.07642108480784073, + "angularVelocity": 1.7812579676147597e-8, + "velocityX": -3.550576989716468, + "velocityY": -1.1265142544485776, + "timestamp": 4.729314623630929 + }, + { + "x": 5.6447689756038235, + "y": 6.314980076260972, + "heading": 0.0764210865279785, + "angularVelocity": 1.7812579243167278e-8, + "velocityX": -3.550576989716468, + "velocityY": -1.1265142544485782, + "timestamp": 4.825883332178099 + }, + { + "x": 5.3018943411135915, + "y": 6.20619404953636, + "heading": 0.07642108824812432, + "angularVelocity": 1.7812662732207566e-8, + "velocityX": -3.5505769896751667, + "velocityY": -1.1265142545783664, + "timestamp": 4.92245204072527 + }, + { + "x": 4.971178879857472, + "y": 6.101264896222856, + "heading": 0.09246038898135349, + "angularVelocity": 0.16609211176718244, + "velocityX": -3.4246648446642007, + "velocityY": -1.0865750913739254, + "timestamp": 5.019020749272441 + }, + { + "x": 4.695582635189411, + "y": 6.01382394107076, + "heading": 0.10582487967853092, + "angularVelocity": 0.13839359455293215, + "velocityX": -2.8538876496773393, + "velocityY": -0.9054791812752113, + "timestamp": 5.1155894578196115 + }, + { + "x": 4.475105628641342, + "y": 5.943871182253239, + "heading": 0.1165156132920598, + "angularVelocity": 0.11070598099908092, + "velocityX": -2.2831102317203804, + "velocityY": -0.7243832900939244, + "timestamp": 5.212158166366782 + }, + { + "x": 4.309747868917504, + "y": 5.8914066159860985, + "heading": 0.12453333874296012, + "angularVelocity": 0.08302612276298497, + "velocityX": -1.7123327236282266, + "velocityY": -0.5432874380991931, + "timestamp": 5.308726874913953 + }, + { + "x": 4.19950936065589, + "y": 5.856430239360431, + "heading": 0.12987847932741303, + "angularVelocity": 0.055350647894830074, + "velocityX": -1.1415551675081796, + "velocityY": -0.36219161622714885, + "timestamp": 5.405295583461124 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.027676321443494054, + "velocityX": -0.5707775871082961, + "velocityY": -0.18109580929158178, + "timestamp": 5.501864292008294 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 2.6946819475075084e-27, + "velocityX": -6.238393680699965e-27, + "velocityY": 1.2085111651652188e-26, + "timestamp": 5.598433000555465 + }, + { + "x": 4.164497272188891, + "y": 5.856383810972102, + "heading": 0.11953980585413508, + "angularVelocity": -0.19788617668780903, + "velocityX": 0.30580479589931786, + "velocityY": 0.26526731175760876, + "timestamp": 5.664184637734952 + }, + { + "x": 4.205280500846038, + "y": 5.8906190238483225, + "heading": 0.09417342379390335, + "angularVelocity": -0.38579088138881684, + "velocityX": 0.6202617973727179, + "velocityY": 0.520674683472388, + "timestamp": 5.729936274914438 + }, + { + "x": 4.267438293143164, + "y": 5.9407879010107, + "heading": 0.05730532907136487, + "angularVelocity": -0.5607175167653599, + "velocityX": 0.9453421232303373, + "velocityY": 0.7630057488215584, + "timestamp": 5.795687912093925 + }, + { + "x": 4.351839190303838, + "y": 6.005705720740894, + "heading": 0.010093847833175575, + "angularVelocity": -0.7180274630928684, + "velocityX": 1.283631872622101, + "velocityY": 0.9873186815559245, + "timestamp": 5.8614395492734115 + }, + { + "x": 4.459568016776304, + "y": 6.083661828301211, + "heading": -0.04581026085444041, + "angularVelocity": -0.850231432793241, + "velocityX": 1.6384204423441482, + "velocityY": 1.1856146995627632, + "timestamp": 5.927191186452898 + }, + { + "x": 4.591948294079918, + "y": 6.172045469361953, + "heading": -0.1079292379524144, + "angularVelocity": -0.944751792695351, + "velocityX": 2.013338115707256, + "velocityY": 1.344204416073704, + "timestamp": 5.992942823632385 + }, + { + "x": 4.7504027579860635, + "y": 6.266641095517782, + "heading": -0.17238119577808125, + "angularVelocity": -0.9802335058171755, + "velocityX": 2.409893817147125, + "velocityY": 1.438680924363996, + "timestamp": 6.058694460811871 + }, + { + "x": 4.935641035495618, + "y": 6.360501248168881, + "heading": -0.23308994891442564, + "angularVelocity": -0.9233040535648408, + "velocityX": 2.8172420559490887, + "velocityY": 1.4274952940697678, + "timestamp": 6.124446097991358 + }, + { + "x": 5.145232187341836, + "y": 6.443576909061307, + "heading": -0.2814508136896339, + "angularVelocity": -0.7355081462564137, + "velocityX": 3.1876187550141757, + "velocityY": 1.2634766897993692, + "timestamp": 6.190197735170845 + }, + { + "x": 5.371831569570122, + "y": 6.506507878011031, + "heading": -0.310075861000899, + "angularVelocity": -0.4353510960210094, + "velocityX": 3.446292624010618, + "velocityY": 0.9571011711531484, + "timestamp": 6.255949372350331 + }, + { + "x": 5.608723728071605, + "y": 6.544912225542492, + "heading": -0.32489986784082275, + "angularVelocity": -0.22545456624080287, + "velocityX": 3.6028328519763595, + "velocityY": 0.5840819967208853, + "timestamp": 6.321701009529818 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.3320371023810657, + "angularVelocity": -0.10854839280670678, + "velocityX": 3.6853903618544206, + "velocityY": 0.18577139494688796, + "timestamp": 6.3874526467093045 + }, + { + "x": 5.9711094207782285, + "y": 6.556647574574296, + "heading": -0.33458795200315017, + "angularVelocity": -0.07861324883762467, + "velocityX": 3.700225495398522, + "velocityY": -0.014775117904477024, + "timestamp": 6.419900734783533 + }, + { + "x": 6.091317843363461, + "y": 6.549645373760147, + "heading": -0.3360778340908156, + "angularVelocity": -0.045915866730181235, + "velocityX": 3.704638076371226, + "velocityY": -0.21579702317530708, + "timestamp": 6.452348822857761 + }, + { + "x": 6.211267508828441, + "y": 6.536122994121453, + "heading": -0.33661432797946356, + "angularVelocity": -0.016533913721531207, + "velocityX": 3.6966635812434836, + "velocityY": -0.4167388724956589, + "timestamp": 6.48479691093199 + }, + { + "x": 6.330469045455405, + "y": 6.51611559729621, + "heading": -0.33661493169123347, + "angularVelocity": -0.000018605465091905695, + "velocityX": 3.6736074049811314, + "velocityY": -0.6165970943950513, + "timestamp": 6.517244999006218 + }, + { + "x": 6.448416412815129, + "y": 6.489700510555554, + "heading": -0.3366150129853366, + "angularVelocity": -0.0000025053588043472207, + "velocityX": 3.6349558436203777, + "velocityY": -0.8140722091307351, + "timestamp": 6.549693087080446 + }, + { + "x": 6.564765294299518, + "y": 6.456954843497344, + "heading": -0.3366150728513962, + "angularVelocity": -0.000001844979570705055, + "velocityX": 3.5856929757534926, + "velocityY": -1.0091709250573346, + "timestamp": 6.582141175154675 + }, + { + "x": 6.679176170718401, + "y": 6.417974154303709, + "heading": -0.33661511946704875, + "angularVelocity": -0.0000014366224727676693, + "velocityX": 3.525966650397334, + "velocityY": -1.2013246852776633, + "timestamp": 6.614589263228903 + }, + { + "x": 6.791315180749945, + "y": 6.372872197072731, + "heading": -0.33661515729887875, + "angularVelocity": -0.000001165918618828322, + "velocityX": 3.4559512343227956, + "velocityY": -1.389972719742485, + "timestamp": 6.647037351303132 + }, + { + "x": 6.900855096060357, + "y": 6.3217805921945684, + "heading": -0.3366151890219745, + "angularVelocity": -9.776568561253354e-7, + "velocityX": 3.375851145985221, + "velocityY": -1.5745644169014148, + "timestamp": 6.67948543937736 + }, + { + "x": 7.007476282450098, + "y": 6.264848452420557, + "heading": -0.33661521634397956, + "angularVelocity": -8.420220318148558e-7, + "velocityX": 3.285900424882802, + "velocityY": -1.7545606891775674, + "timestamp": 6.711933527451588 + }, + { + "x": 7.110867767687471, + "y": 6.2022421991346945, + "heading": -0.33661529971413134, + "angularVelocity": -0.0000025693394187906504, + "velocityX": 3.186366019497228, + "velocityY": -1.9294281112232177, + "timestamp": 6.744381615525817 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.25801763236442515, + "velocityX": 3.2027040434173877, + "velocityY": -1.73803251899956, + "timestamp": 6.776829703600045 + }, + { + "x": 7.403072746116238, + "y": 6.061144053185023, + "heading": -0.35924792619770074, + "angularVelocity": -0.23258228867402195, + "velocityX": 3.0708274318237563, + "velocityY": -1.3814614025635412, + "timestamp": 6.8381432615033795 + }, + { + "x": 7.5736331381530135, + "y": 5.991566896909843, + "heading": -0.3703276689612211, + "angularVelocity": -0.18070624413915942, + "velocityX": 2.781772871600061, + "velocityY": -1.1347760373794369, + "timestamp": 6.899456819406714 + }, + { + "x": 7.72439550755076, + "y": 5.934268464416171, + "heading": -0.37789043666294847, + "angularVelocity": -0.12334576495545502, + "velocityX": 2.4588749136926022, + "velocityY": -0.934514884685174, + "timestamp": 6.960770377310048 + }, + { + "x": 7.854564778976688, + "y": 5.88786008036463, + "heading": -0.38180542343665536, + "angularVelocity": -0.06385189357106222, + "velocityX": 2.1230095900021, + "velocityY": -0.7569024802753538, + "timestamp": 7.022083935213383 + }, + { + "x": 7.963733931476252, + "y": 5.851532970517725, + "heading": -0.38200457597908144, + "angularVelocity": -0.0032480995922642706, + "velocityX": 1.7805059147224407, + "velocityY": -0.592480865393233, + "timestamp": 7.083397493116717 + }, + { + "x": 8.05165855711374, + "y": 5.8247603198900775, + "heading": -0.3784464795942301, + "angularVelocity": 0.0580311517798547, + "velocityX": 1.434016042195889, + "velocityY": -0.43665139559925853, + "timestamp": 7.144711051020051 + }, + { + "x": 8.118176532906231, + "y": 5.80717243118701, + "heading": -0.3711033743685625, + "angularVelocity": 0.11976315641712676, + "velocityX": 1.084882007619954, + "velocityY": -0.2868515431904276, + "timestamp": 7.206024608923386 + }, + { + "x": 8.16317279255834, + "y": 5.798495818445154, + "heading": -0.3599553749598749, + "angularVelocity": 0.18181948316004298, + "velocityX": 0.7338712870495722, + "velocityY": -0.1415121392161809, + "timestamp": 7.26733816682672 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.24412049957493756, + "velocityX": 0.38146199167222045, + "velocityY": 0.0003958300819017007, + "timestamp": 7.328651724730054 + }, + { + "x": 8.186562507619959, + "y": 5.808483646148046, + "heading": -0.3243738625034631, + "angularVelocity": 0.3115588640226202, + "velocityX": 0.000013952657501305156, + "velocityY": 0.1505914724827301, + "timestamp": 7.394814554457269 + }, + { + "x": 8.161321884729395, + "y": 5.828379592770816, + "heading": -0.29947359998774176, + "angularVelocity": 0.37634820968788424, + "velocityX": -0.38149249351379677, + "velocityY": 0.30071184537904955, + "timestamp": 7.460977384184483 + }, + { + "x": 8.110834888181197, + "y": 5.858201520747478, + "heading": -0.2705090334908383, + "angularVelocity": 0.43777702096967036, + "velocityX": -0.7630719054241456, + "velocityY": 0.45073537663996555, + "timestamp": 7.527140213911697 + }, + { + "x": 8.035095319337403, + "y": 5.897940888331321, + "heading": -0.23777201719034075, + "angularVelocity": 0.494794682081624, + "velocityX": -1.144745004953147, + "velocityY": 0.600629806005663, + "timestamp": 7.593303043638912 + }, + { + "x": 7.934094943360084, + "y": 5.947585769746669, + "heading": -0.20166340599285526, + "angularVelocity": 0.5457537313074324, + "velocityX": -1.526542567688504, + "velocityY": 0.7503439864956146, + "timestamp": 7.659465873366126 + }, + { + "x": 7.8078223399404525, + "y": 6.0071184189660025, + "heading": -0.1627692568761995, + "angularVelocity": 0.587854982578802, + "velocityX": -1.9085127395585555, + "velocityY": 0.8997899495046319, + "timestamp": 7.72562870309334 + }, + { + "x": 7.656260799272679, + "y": 6.076509799690328, + "heading": -0.12202859757436733, + "angularVelocity": 0.6157635559090158, + "velocityX": -2.290735467220094, + "velocityY": 1.0487970513114737, + "timestamp": 7.791791532820555 + }, + { + "x": 7.479384511526562, + "y": 6.155704239729163, + "heading": -0.08118783305769839, + "angularVelocity": 0.6172765688083961, + "velocityX": -2.6733482905034087, + "velocityY": 1.1969627110168721, + "timestamp": 7.857954362547769 + }, + { + "x": 7.277158908396328, + "y": 6.244553850532033, + "heading": -0.04460973018212138, + "angularVelocity": 0.5528497349098663, + "velocityX": -3.0564835869928735, + "velocityY": 1.342893149661115, + "timestamp": 7.924117192274983 + }, + { + "x": 7.0505381507723195, + "y": 6.341424218836914, + "heading": -0.04460966986271364, + "angularVelocity": 9.116811959714027e-7, + "velocityX": -3.425197479587153, + "velocityY": 1.464120695929603, + "timestamp": 7.990280022002198 + }, + { + "x": 6.821387916460742, + "y": 6.432149176298842, + "heading": -0.04460966140291972, + "angularVelocity": 1.278632421205442e-7, + "velocityX": -3.4634285633844892, + "velocityY": 1.371237563991483, + "timestamp": 8.056442851729411 + }, + { + "x": 6.585463067264655, + "y": 6.503425491915177, + "heading": -0.044609652839645464, + "angularVelocity": 1.2942726746927926e-7, + "velocityX": -3.5658216277748638, + "velocityY": 1.0772863843672305, + "timestamp": 8.122605681456625 + }, + { + "x": 6.34313048184035, + "y": 6.548322764309462, + "heading": -0.044609643757262056, + "angularVelocity": 1.3727320078677108e-7, + "velocityX": -3.662669605024884, + "velocityY": 0.6785875480144041, + "timestamp": 8.188768511183838 + }, + { + "x": 6.097330137085011, + "y": 6.566295976512413, + "heading": -0.04460963356805428, + "angularVelocity": 1.5400199501081406e-7, + "velocityX": -3.715082105296292, + "velocityY": 0.27165120169518303, + "timestamp": 8.254931340911051 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.04460962128616813, + "angularVelocity": 1.8563121019327388e-7, + "velocityX": -3.722421790171331, + "velocityY": -0.13858200516588748, + "timestamp": 8.321094170638265 + }, + { + "x": 5.594131845425269, + "y": 6.517387984060631, + "heading": -0.0446096110934515, + "angularVelocity": 1.4604851016203596e-7, + "velocityX": -3.6812230501933914, + "velocityY": -0.5694089339833474, + "timestamp": 8.390884109764682 + }, + { + "x": 5.343562863816817, + "y": 6.448118054894418, + "heading": -0.04460960236123058, + "angularVelocity": 1.2512148631224918e-7, + "velocityX": -3.590330995339737, + "velocityY": -0.9925489265829068, + "timestamp": 8.4606740488911 + }, + { + "x": 5.102719675691794, + "y": 6.350252358061884, + "heading": -0.04460958274491264, + "angularVelocity": 2.810765876662287e-7, + "velocityX": -3.4509728929374397, + "velocityY": -1.4022894711981222, + "timestamp": 8.530463988017518 + }, + { + "x": 4.88810249039924, + "y": 6.239912108360582, + "heading": -0.010754389574110686, + "angularVelocity": 0.4851013426086585, + "velocityX": -3.0751880282313135, + "velocityY": -1.5810337576227271, + "timestamp": 8.600253927143935 + }, + { + "x": 4.7014054819057804, + "y": 6.141094298946399, + "heading": 0.022653281130584686, + "angularVelocity": 0.47868892168225635, + "velocityX": -2.6751278311803, + "velocityY": -1.415932019014718, + "timestamp": 8.670043866270353 + }, + { + "x": 4.541859398687963, + "y": 6.055486217406872, + "heading": 0.05282297144244177, + "angularVelocity": 0.43229283030620586, + "velocityX": -2.286090018345113, + "velocityY": -1.2266536210105516, + "timestamp": 8.73983380539677 + }, + { + "x": 4.409161403799561, + "y": 5.983676432778196, + "heading": 0.07876234420420766, + "angularVelocity": 0.37167782471881955, + "velocityX": -1.9013914691633662, + "velocityY": -1.0289417862737966, + "timestamp": 8.809623744523188 + }, + { + "x": 4.303150560780497, + "y": 5.925963754570679, + "heading": 0.09996559761955173, + "angularVelocity": 0.3038153304151236, + "velocityX": -1.5189989323107926, + "velocityY": -0.8269483958565483, + "timestamp": 8.879413683649606 + }, + { + "x": 4.223727158834788, + "y": 5.882528846842074, + "heading": 0.11612624955874849, + "angularVelocity": 0.23156134167022593, + "velocityX": -1.1380351228253833, + "velocityY": -0.6223663220271061, + "timestamp": 8.949203622776023 + }, + { + "x": 4.170823380513197, + "y": 5.853492711046949, + "heading": 0.1270379483840901, + "angularVelocity": 0.15635059955527503, + "velocityX": -0.7580430500986731, + "velocityY": -0.41605045309653765, + "timestamp": 9.018993561902441 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.0789970249504139, + "velocityX": -0.37875479822591473, + "velocityY": -0.2084922310471305, + "timestamp": 9.088783501028859 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.1741097503964344e-26, + "velocityX": -8.106670406309328e-26, + "velocityY": -1.6795580659596768e-26, + "timestamp": 9.158573440155276 + }, + { + "x": 4.149229218776726, + "y": 5.815100726386144, + "heading": 0.12918022174703248, + "angularVelocity": -0.05380546988265158, + "velocityX": 0.07724016042252664, + "velocityY": -0.38054657832352085, + "timestamp": 9.221223654099454 + }, + { + "x": 4.159717289478366, + "y": 5.767595641795523, + "heading": 0.12259706469587855, + "angularVelocity": -0.1050779660069413, + "velocityX": 0.16740678189839092, + "velocityY": -0.758258936401222, + "timestamp": 9.283873868043631 + }, + { + "x": 4.176867312970839, + "y": 5.696690246498273, + "heading": 0.11300853276979762, + "angularVelocity": -0.1530486701070873, + "velocityX": 0.2737424569332407, + "velocityY": -1.1317662116912734, + "timestamp": 9.346524081987809 + }, + { + "x": 4.201974040585746, + "y": 5.602790592825652, + "heading": 0.10069220099176118, + "angularVelocity": -0.19658882233044642, + "velocityX": 0.4007444832874458, + "velocityY": -1.4987922268914513, + "timestamp": 9.409174295931987 + }, + { + "x": 4.236730391014469, + "y": 5.486551757326023, + "heading": 0.08603276386217854, + "angularVelocity": -0.2339886204162749, + "velocityX": 0.5547682639949316, + "velocityY": -1.8553621477366304, + "timestamp": 9.471824509876164 + }, + { + "x": 4.283393708275682, + "y": 5.349087774330418, + "heading": 0.06958159023755481, + "angularVelocity": -0.2625876687233969, + "velocityX": 0.7448229514872947, + "velocityY": -2.194150256503298, + "timestamp": 9.534474723820342 + }, + { + "x": 4.344983097190331, + "y": 5.19239819137041, + "heading": 0.0521501823955064, + "angularVelocity": -0.27823381190014773, + "velocityX": 0.983067495500113, + "velocityY": -2.501022312543423, + "timestamp": 9.59712493776452 + }, + { + "x": 4.425324635342364, + "y": 5.020190178284826, + "heading": 0.03492497946543372, + "angularVelocity": -0.2749424438585406, + "velocityX": 1.2823825026937383, + "velocityY": -2.748721867718171, + "timestamp": 9.659775151708697 + }, + { + "x": 4.5282801412548075, + "y": 4.838988810812342, + "heading": 0.019478906624106312, + "angularVelocity": -0.24654461443802958, + "velocityX": 1.6433384569792093, + "velocityY": -2.892270529737325, + "timestamp": 9.722425365652875 + }, + { + "x": 4.655561045937701, + "y": 4.65789803685685, + "heading": 0.007446897298022715, + "angularVelocity": -0.1920505704386623, + "velocityX": 2.031611652536461, + "velocityY": -2.890505276116785, + "timestamp": 9.785075579597052 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -3.0154063280737095e-24, + "angularVelocity": -0.11886467466269138, + "velocityX": 2.394177429870417, + "velocityY": -2.752084488523512, + "timestamp": 9.84772579354123 + }, + { + "x": 5.0265563719767545, + "y": 4.290478604211027, + "heading": -0.0011711585949722948, + "angularVelocity": -0.014781576411206173, + "velocityX": 2.789308345003365, + "velocityY": -2.461168374898756, + "timestamp": 9.926956760685195 + }, + { + "x": 5.271648004487176, + "y": 4.1260554019145586, + "heading": -0.0011711658442560332, + "angularVelocity": -9.149558562553705e-8, + "velocityX": 3.0933818094771004, + "velocityY": -2.075239117019812, + "timestamp": 10.006187727829161 + }, + { + "x": 5.5362482788476575, + "y": 3.9953205544154557, + "heading": -0.0011711670074451403, + "angularVelocity": -1.4680990893194224e-8, + "velocityX": 3.3396067711718436, + "velocityY": -1.650047351581014, + "timestamp": 10.085418694973127 + }, + { + "x": 5.815753541872799, + "y": 3.900548632610082, + "heading": -0.0011711678822933615, + "angularVelocity": -1.1041746080675712e-8, + "velocityX": 3.5277275174146143, + "velocityY": -1.196147481491295, + "timestamp": 10.164649662117093 + }, + { + "x": 6.105300821187424, + "y": 3.8433884686557884, + "heading": -0.001171168612339158, + "angularVelocity": -9.214147233371614e-9, + "velocityX": 3.654471095733393, + "velocityY": -0.7214371604278238, + "timestamp": 10.243880629261058 + }, + { + "x": 6.399852439542986, + "y": 3.8248343191470044, + "heading": -0.0011711692753051952, + "angularVelocity": -8.367511605727905e-9, + "velocityX": 3.717632498671273, + "velocityY": -0.2341780010721126, + "timestamp": 10.323111596405024 + }, + { + "x": 6.694725372224164, + "y": 3.8372788433696505, + "heading": -0.0011711699217424045, + "angularVelocity": -8.15889585625018e-9, + "velocityX": 3.721687912068309, + "velocityY": 0.15706641823560874, + "timestamp": 10.40234256354899 + }, + { + "x": 6.9895982402615005, + "y": 3.8497248992335154, + "heading": -0.0011711705681793068, + "angularVelocity": -8.15889197720244e-9, + "velocityX": 3.7216870961771913, + "velocityY": 0.157085749581342, + "timestamp": 10.481573530692955 + }, + { + "x": 7.283138211598938, + "y": 3.880370995696298, + "heading": -0.0011711735014489986, + "angularVelocity": -3.702175800028382e-8, + "velocityX": 3.7048641701427636, + "velocityY": 0.386794426061941, + "timestamp": 10.560804497836921 + }, + { + "x": 7.537795284833578, + "y": 3.9128988141490386, + "heading": -0.0022060584164621646, + "angularVelocity": -0.01306162164009358, + "velocityX": 3.2141103714147428, + "velocityY": 0.4105442559300831, + "timestamp": 10.640035464980887 + }, + { + "x": 7.753528037916665, + "y": 3.9448039763834073, + "heading": -0.003047966804873468, + "angularVelocity": -0.010626001660203437, + "velocityX": 2.722833771435503, + "velocityY": 0.40268550775602213, + "timestamp": 10.719266432124853 + }, + { + "x": 7.930345641638023, + "y": 3.9756628799518334, + "heading": -0.0034891865809292344, + "angularVelocity": -0.005568779379583401, + "velocityX": 2.23167291899988, + "velocityY": 0.3894803342783207, + "timestamp": 10.798497399268818 + }, + { + "x": 8.068253246603454, + "y": 4.005300685276172, + "heading": -0.003443969014949724, + "angularVelocity": 0.0005707057178457502, + "velocityX": 1.7405770740479334, + "velocityY": 0.3740684531905991, + "timestamp": 10.877728366412784 + }, + { + "x": 8.167254001972685, + "y": 4.033621928989228, + "heading": -0.0028654925288464014, + "angularVelocity": 0.007301141295577129, + "velocityX": 1.2495209756728338, + "velocityY": 0.3574516976625533, + "timestamp": 10.95695933355675 + }, + { + "x": 8.227350013878981, + "y": 4.060566470670501, + "heading": -0.0017242628170792867, + "angularVelocity": 0.014403834168696394, + "velocityX": 0.7584914594958708, + "velocityY": 0.340075890179585, + "timestamp": 11.036190300700715 + }, + { + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 1.0796890687236353e-23, + "angularVelocity": 0.021762486048494588, + "velocityX": 0.2674809172408869, + "velocityY": 0.32217804683224427, + "timestamp": 11.115421267844681 + }, + { + "x": 8.231523012343859, + "y": 4.109892958457586, + "heading": 0.0022863134326364947, + "angularVelocity": 0.029215203502289427, + "velocityX": -0.21748380315842067, + "velocityY": 0.3041237094875908, + "timestamp": 11.193678926822047 + }, + { + "x": 8.176551036095814, + "y": 4.132280075994507, + "heading": 0.005155847293198806, + "angularVelocity": 0.03666777026121207, + "velocityX": -0.7024485138757453, + "velocityY": 0.2860693487316991, + "timestamp": 11.271936585799413 + }, + { + "x": 8.083626857911096, + "y": 4.153254299063425, + "heading": 0.008608587113368522, + "angularVelocity": 0.04412015213959263, + "velocityX": -1.1874132116780545, + "velocityY": 0.26801495652947127, + "timestamp": 11.350194244776779 + }, + { + "x": 7.952750479206828, + "y": 4.172815624183239, + "heading": 0.012644515987955914, + "angularVelocity": 0.051572317998353566, + "velocityX": -1.6723778913719014, + "velocityY": 0.24996051984474485, + "timestamp": 11.428451903754144 + }, + { + "x": 7.783921902112919, + "y": 4.190964046074225, + "heading": 0.017263614398582115, + "angularVelocity": 0.059024234445374815, + "velocityX": -2.1573425438491505, + "velocityY": 0.2319060156940586, + "timestamp": 11.50670956273151 + }, + { + "x": 7.57714113018582, + "y": 4.207699555855796, + "heading": 0.022465858708652038, + "angularVelocity": 0.06647584885684583, + "velocityX": -2.642307150880996, + "velocityY": 0.2138513980645823, + "timestamp": 11.584967221708876 + }, + { + "x": 7.332408170546637, + "y": 4.223022135701224, + "heading": 0.028251214754351595, + "angularVelocity": 0.07392702671278313, + "velocityX": -3.1272716669171876, + "velocityY": 0.19579655263979157, + "timestamp": 11.663224880686242 + }, + { + "x": 7.049723044551417, + "y": 4.23693173264099, + "heading": 0.03461960291604749, + "angularVelocity": 0.08137718716500116, + "velocityX": -3.6122359100593893, + "velocityY": 0.1777410303544707, + "timestamp": 11.741482539663608 + }, + { + "x": 6.758414971904035, + "y": 4.226087741393702, + "heading": 0.03461960351130125, + "angularVelocity": 7.606332300457526e-9, + "velocityX": -3.7224225264857154, + "velocityY": -0.13856779501191718, + "timestamp": 11.819740198640973 + }, + { + "x": 6.467106926605873, + "y": 4.215243015473263, + "heading": 0.03461960410653643, + "angularVelocity": 7.606094897280975e-9, + "velocityX": -3.7224221770091352, + "velocityY": -0.13857718288730186, + "timestamp": 11.89799785761834 + }, + { + "x": 6.175798881305749, + "y": 4.204398289605515, + "heading": 0.03461960470177156, + "angularVelocity": 7.606094342014038e-9, + "velocityX": -3.7224221770342005, + "velocityY": -0.1385771822140065, + "timestamp": 11.976255516595705 + }, + { + "x": 5.8844906838954465, + "y": 4.193557650451513, + "heading": 0.03461960529700769, + "angularVelocity": 7.606107036408581e-9, + "velocityX": -3.7224241207439843, + "velocityY": -0.13852496095158084, + "timestamp": 12.05451317557307 + }, + { + "x": 5.594009037113428, + "y": 4.218019822631225, + "heading": 0.034619605911650746, + "angularVelocity": 7.854094608871324e-9, + "velocityX": -3.7118622071998786, + "velocityY": 0.31258502361778046, + "timestamp": 12.132770834550437 + }, + { + "x": 5.309172831526295, + "y": 4.280038516789021, + "heading": 0.034619599952821836, + "angularVelocity": -7.614371543607384e-8, + "velocityX": -3.639723054704678, + "velocityY": 0.7924936034150137, + "timestamp": 12.211028493527802 + }, + { + "x": 5.03935943827081, + "y": 4.376755129785837, + "heading": 0.01971114929830447, + "angularVelocity": -0.19050468477250748, + "velocityX": -3.4477570218849003, + "velocityY": 1.235874089011402, + "timestamp": 12.289286152505168 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -1.0813807424036439e-23, + "angularVelocity": -0.25187501844395294, + "velocityX": -2.987601050001106, + "velocityY": 1.389310982379468, + "timestamp": 12.367543811482534 + }, + { + "x": 4.627457882261689, + "y": 4.587679677136334, + "heading": -0.019778853338755203, + "angularVelocity": -0.2848710655643308, + "velocityX": -2.5651244910957094, + "velocityY": 1.4719718180672714, + "timestamp": 12.43697470752518 + }, + { + "x": 4.478940134439646, + "y": 4.685562428002016, + "heading": -0.03941675041867698, + "angularVelocity": -0.28284089935782175, + "velocityX": -2.1390728953118847, + "velocityY": 1.4097866575934486, + "timestamp": 12.506405603567828 + }, + { + "x": 4.358424662792702, + "y": 4.772986561558444, + "heading": -0.05736759927865439, + "angularVelocity": -0.2585426644782356, + "velocityX": -1.7357614335398788, + "velocityY": 1.2591531802027813, + "timestamp": 12.575836499610475 + }, + { + "x": 4.264251963016929, + "y": 4.846292896298891, + "heading": -0.07266630938102059, + "angularVelocity": -0.220344414005101, + "velocityX": -1.3563514968599655, + "velocityY": 1.0558172070171679, + "timestamp": 12.645267395653121 + }, + { + "x": 4.19504667897931, + "y": 4.903178017800909, + "heading": -0.0846829764598402, + "angularVelocity": -0.17307377210627528, + "velocityX": -0.9967505531703156, + "velocityY": 0.8193055936809015, + "timestamp": 12.714698291695768 + }, + { + "x": 4.149718350868284, + "y": 4.942098244876492, + "heading": -0.09298341563379672, + "angularVelocity": -0.11954964788093829, + "velocityX": -0.6528552948990191, + "velocityY": 0.5605606335784127, + "timestamp": 12.784129187738415 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.06149221603273053, + "velocityX": -0.32142062647019837, + "velocityY": 0.2860902346287965, + "timestamp": 12.853560083781062 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -7.045263803296892e-25, + "velocityX": -1.4084469220925396e-25, + "velocityY": -2.7940612123980634e-25, + "timestamp": 12.922990979823709 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 + }, + { + "timestamp": 0.34959777724704905, + "isStopPoint": false, + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 1.1040901645422223, + "isStopPoint": true, + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 1.8634374124793154, + "isStopPoint": true, + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "timestamp": 3.2273332408433504, + "isStopPoint": false, + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 3.7636275381592217, + "isStopPoint": false, + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 5.598433000555465, + "isStopPoint": true, + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 6.3874526467093045, + "isStopPoint": false, + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "timestamp": 6.776829703600045, + "isStopPoint": false, + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 7.328651724730054, + "isStopPoint": false, + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 8.321094170638265, + "isStopPoint": false, + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "timestamp": 9.158573440155276, + "isStopPoint": true, + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 9.84772579354123, + "isStopPoint": false, + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 11.115421267844681, + "isStopPoint": false, + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 12.367543811482534, + "isStopPoint": false, + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 12.922990979823709, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint" + }, + { + "scope": [ + 11 + ], + "type": "StopPoint" + }, + { + "scope": [ + 0, + 1 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 11 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "CenterLanePDEABC": { + "waypoints": [ + { + "x": 1.3350272178649902, + "y": 5.601006507873535, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.3368515968322754, + "y": 6.133185386657715, + "heading": 0.2525545797921912, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 5.747424125671387, + "y": 7.119814872741699, + "heading": 0.2525545797921912, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 3.405486822128296, + "y": 6.266934394836426, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.205486822128296, + "y": 6.266934394836426, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 2.256009101867676, + "y": 6.624549388885498, + "heading": 0.3930579718029317, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 2.8562917709350586, + "y": 6.879989147186279, + "heading": 0.3930579718029317, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 1.7400015592575073, + "y": 6.167843341827393, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 7 + }, + { + "x": 2.0785037517547607, + "y": 5.548515796661377, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 2.0932393074035645, + "y": 4.9619622230529785, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "x": 2.2881293296813965, + "y": 4.354815483093262, + "heading": -0.3347371673945201, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.3350272178649902, + "y": 5.601006507873535, + "heading": 1.3464399983715043e-25, + "angularVelocity": 2.380601151660865e-26, + "velocityX": -3.6373884256645e-26, + "velocityY": 3.3932882505305813e-26, + "timestamp": 0 + }, + { + "x": 1.3513037651461417, + "y": 5.614995878157814, + "heading": 0.006549885022004973, + "angularVelocity": 0.11118193396003606, + "velocityX": 0.27628851481066846, + "velocityY": 0.23746451088284293, + "timestamp": 0.058911414730016334 + }, + { + "x": 1.3843789066016257, + "y": 5.642351283609904, + "heading": 0.01935493583583066, + "angularVelocity": 0.21736111537143754, + "velocityX": 0.5614385871916894, + "velocityY": 0.4643481331666494, + "timestamp": 0.11782282946003267 + }, + { + "x": 1.4348748758418, + "y": 5.682261570187258, + "heading": 0.038033490581189856, + "angularVelocity": 0.31706172447157616, + "velocityX": 0.8571508505030987, + "velocityY": 0.6774627083776117, + "timestamp": 0.17673424419004902 + }, + { + "x": 1.503535032987812, + "y": 5.733639392412991, + "heading": 0.06208045701046981, + "angularVelocity": 0.408188574989825, + "velocityX": 1.1654813835429523, + "velocityY": 0.8721199866136559, + "timestamp": 0.23564565892006534 + }, + { + "x": 1.5912385139518157, + "y": 5.794977395067809, + "heading": 0.09080789039897333, + "angularVelocity": 0.48763781213127805, + "velocityX": 1.4887349313530773, + "velocityY": 1.041190454106761, + "timestamp": 0.29455707365008166 + }, + { + "x": 1.698981116852805, + "y": 5.864111533799201, + "heading": 0.12324878614145561, + "angularVelocity": 0.550672495154884, + "velocityX": 1.828891792783458, + "velocityY": 1.1735270498633465, + "timestamp": 0.353468488380098 + }, + { + "x": 1.8277399268235341, + "y": 5.937848492694958, + "heading": 0.15800938596992012, + "angularVelocity": 0.5900486346791701, + "velocityX": 2.1856343216474206, + "velocityY": 1.2516582606899718, + "timestamp": 0.4123799031101143 + }, + { + "x": 1.9780206143757966, + "y": 6.011501208422765, + "heading": 0.1930700705634149, + "angularVelocity": 0.5951424652450383, + "velocityX": 2.5509604249190097, + "velocityY": 1.2502282633229538, + "timestamp": 0.4712913178401306 + }, + { + "x": 2.1488551922208914, + "y": 6.0788007358405975, + "heading": 0.22564028873153014, + "angularVelocity": 0.5528676966489513, + "velocityX": 2.8998552933757997, + "velocityY": 1.1423851850487312, + "timestamp": 0.5302027325701469 + }, + { + "x": 2.3368515968322754, + "y": 6.133185386657715, + "heading": 0.2525545797921912, + "angularVelocity": 0.456860375599634, + "velocityX": 3.1911711078905185, + "velocityY": 0.9231598165882628, + "timestamp": 0.5891141473001633 + }, + { + "x": 2.5473350968821267, + "y": 6.194075230145087, + "heading": 0.25255484664471606, + "angularVelocity": 0.0000044758460677296985, + "velocityX": 3.530383481883881, + "velocityY": 1.0212890683184164, + "timestamp": 0.6487347242516318 + }, + { + "x": 2.7606743654688723, + "y": 6.255791206318425, + "heading": 0.25255482885764124, + "angularVelocity": -2.983378514405921e-7, + "velocityX": 3.578282524176262, + "velocityY": 1.0351455710261694, + "timestamp": 0.7083553012031003 + }, + { + "x": 2.9740136340556216, + "y": 6.317507182491763, + "heading": 0.25255481107057365, + "angularVelocity": -2.9833773077503677e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.7679758781545688 + }, + { + "x": 3.187352902642371, + "y": 6.379223158665101, + "heading": 0.25255479328350594, + "angularVelocity": -2.9833773187848623e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.8275964551060373 + }, + { + "x": 3.40069217122912, + "y": 6.44093913483844, + "heading": 0.2525547754964383, + "angularVelocity": -2.983377307846883e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 0.8872170320575058 + }, + { + "x": 3.614031439815869, + "y": 6.502655111011779, + "heading": 0.25255475770937064, + "angularVelocity": -2.9833773102796393e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.9468376090089743 + }, + { + "x": 3.827370708402618, + "y": 6.564371087185117, + "heading": 0.25255473992230293, + "angularVelocity": -2.983377313896792e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.006458185960443 + }, + { + "x": 4.040709976989367, + "y": 6.626087063358456, + "heading": 0.2525547221352353, + "angularVelocity": -2.9833773109308854e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.0660787629119115 + }, + { + "x": 4.254049245576117, + "y": 6.6878030395317944, + "heading": 0.2525547043481677, + "angularVelocity": -2.983377300262124e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.12569933986338 + }, + { + "x": 4.467388514162867, + "y": 6.749519015705133, + "heading": 0.2525546865611001, + "angularVelocity": -2.9833773003639016e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.1853199168148487 + }, + { + "x": 4.680727782749616, + "y": 6.811234991878472, + "heading": 0.2525546687740325, + "angularVelocity": -2.9833772985092435e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.2449404937663173 + }, + { + "x": 4.894067051336365, + "y": 6.872950968051811, + "heading": 0.25255465098696483, + "angularVelocity": -2.9833773088728923e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.304561070717786 + }, + { + "x": 5.107406319923115, + "y": 6.934666944225149, + "heading": 0.2525546331998971, + "angularVelocity": -2.9833773138995595e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.3641816476692545 + }, + { + "x": 5.3207455885098645, + "y": 6.996382920398487, + "heading": 0.2525546154128296, + "angularVelocity": -2.9833772971803955e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4238022246207231 + }, + { + "x": 5.534084857096614, + "y": 7.058098896571825, + "heading": 0.252554597625762, + "angularVelocity": -2.983377299147757e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4834228015721918 + }, + { + "x": 5.747424125671387, + "y": 7.119814872741699, + "heading": 0.2525545797921912, + "angularVelocity": -2.9911771592583556e-7, + "velocityX": 3.5782825239754317, + "velocityY": 1.035145570968072, + "timestamp": 1.5430433785236604 + }, + { + "x": 5.874238789453484, + "y": 7.151633771807316, + "heading": 0.23428972710167786, + "angularVelocity": -0.5023194013498574, + "velocityX": 3.4876528747747586, + "velocityY": 0.8750823563200211, + "timestamp": 1.579404412243304 + }, + { + "x": 6.000467771954814, + "y": 7.182688874971163, + "heading": 0.21343765731660022, + "angularVelocity": -0.5734729641036735, + "velocityX": 3.471545486704275, + "velocityY": 0.8540764655727967, + "timestamp": 1.6157654459629476 + }, + { + "x": 6.126526412325093, + "y": 7.213769689844044, + "heading": 0.19236113767335114, + "angularVelocity": -0.5796457770083356, + "velocityX": 3.4668607428005407, + "velocityY": 0.8547835881819195, + "timestamp": 1.6521264796825912 + }, + { + "x": 6.252399106037461, + "y": 7.244861395063954, + "heading": 0.17099373366442527, + "angularVelocity": -0.5876456696384406, + "velocityX": 3.4617468436923633, + "velocityY": 0.8550830941616943, + "timestamp": 1.6884875134022348 + }, + { + "x": 6.378079158099059, + "y": 7.275966592245878, + "heading": 0.1493195497494227, + "angularVelocity": -0.596082720918173, + "velocityX": 3.456448819102211, + "velocityY": 0.8554541496744962, + "timestamp": 1.7248485471218784 + }, + { + "x": 6.503558906617952, + "y": 7.3070873984764315, + "heading": 0.12731920059935156, + "angularVelocity": -0.6050529069030769, + "velocityX": 3.4509400774022723, + "velocityY": 0.8558834292365232, + "timestamp": 1.761209580841522 + }, + { + "x": 6.628826049826062, + "y": 7.3382208951944605, + "heading": 0.10495362803069447, + "angularVelocity": -0.6150972698164627, + "velocityX": 3.44509301286545, + "velocityY": 0.8562324426219444, + "timestamp": 1.7975706145611656 + }, + { + "x": 6.7538656809780315, + "y": 7.369363297624379, + "heading": 0.08217584529224368, + "angularVelocity": -0.6264338608763309, + "velocityX": 3.438835983489081, + "velocityY": 0.8564773672288275, + "timestamp": 1.8339316482808092 + }, + { + "x": 6.8786958169095165, + "y": 7.400552514729427, + "heading": 0.05908376270904082, + "angularVelocity": -0.6350777252718116, + "velocityX": 3.4330744525573853, + "velocityY": 0.8577648629444327, + "timestamp": 1.8702926820004528 + }, + { + "x": 7.003224052672837, + "y": 7.431696922152255, + "heading": 0.03530389927777457, + "angularVelocity": -0.6539930524147748, + "velocityX": 3.4247716036754445, + "velocityY": 0.8565325084804077, + "timestamp": 1.9066537157200965 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": -8.902558475497447e-21, + "angularVelocity": -0.9709267219953129, + "velocityX": 3.3570777699788383, + "velocityY": 0.7233864155389959, + "timestamp": 1.94301474943974 + }, + { + "x": 7.338537269448379, + "y": 7.482023887493991, + "heading": -0.008039547554639858, + "angularVelocity": -0.1278866817794853, + "velocityX": 3.39215287381437, + "velocityY": 0.38215275600640763, + "timestamp": 2.0058793686696177 + }, + { + "x": 7.527981936001141, + "y": 7.500234795903286, + "heading": -0.013861641566031323, + "angularVelocity": -0.09261320728121766, + "velocityX": 3.013533985786477, + "velocityY": 0.2896845416768293, + "timestamp": 2.068743987899495 + }, + { + "x": 7.693596991003447, + "y": 7.512743640283509, + "heading": -0.017657163586421762, + "angularVelocity": -0.060376123595234456, + "velocityX": 2.6344716158496166, + "velocityY": 0.1989806751947781, + "timestamp": 2.1316086071293725 + }, + { + "x": 7.835373269936495, + "y": 7.519587451824941, + "heading": -0.01948976746028864, + "angularVelocity": -0.02915159427221823, + "velocityX": 2.255263464089624, + "velocityY": 0.10886587122090353, + "timestamp": 2.19447322635925 + }, + { + "x": 7.953306214016821, + "y": 7.520784758027623, + "heading": -0.01939144141260055, + "angularVelocity": 0.0015640919947123182, + "velocityX": 1.875982794854424, + "velocityY": 0.01904578787480972, + "timestamp": 2.2573378455891273 + }, + { + "x": 8.04739309462225, + "y": 7.516346680416715, + "heading": -0.01738162754383806, + "angularVelocity": 0.031970508902840686, + "velocityX": 1.4966587208836901, + "velocityY": -0.07059738315886452, + "timestamp": 2.3202024648190047 + }, + { + "x": 8.117632094556283, + "y": 7.5062806367543455, + "heading": -0.0134735913597302, + "angularVelocity": 0.06216590877322135, + "velocityX": 1.1173057404068745, + "velocityY": -0.16012255837517314, + "timestamp": 2.383067084048882 + }, + { + "x": 8.164021915899584, + "y": 7.490591928437994, + "heading": -0.007677144931297963, + "angularVelocity": 0.09220522607854079, + "velocityX": 0.7379321136689944, + "velocityY": -0.24956340320748793, + "timestamp": 2.4459317032787595 + }, + { + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 2.2979832887228624e-21, + "angularVelocity": 0.12212187117884055, + "velocityX": 0.358542990464167, + "velocityY": -0.33894095350730474, + "timestamp": 2.508796322508637 + }, + { + "x": 8.169433855540824, + "y": 7.426853893988931, + "heading": 0.014976899045789983, + "angularVelocity": 0.16512821061392627, + "velocityX": -0.18884224442902273, + "velocityY": -0.46782018853404506, + "timestamp": 2.599494931729361 + }, + { + "x": 8.102655131439361, + "y": 7.372743106744783, + "heading": 0.03346799984598468, + "angularVelocity": 0.20387413830342968, + "velocityX": -0.7362706515041357, + "velocityY": -0.5965999667366941, + "timestamp": 2.6901935409500854 + }, + { + "x": 7.986220220035368, + "y": 7.306964887600374, + "heading": 0.05493172658414638, + "angularVelocity": 0.23664890699621968, + "velocityX": -1.2837563045827718, + "velocityY": -0.7252395566985038, + "timestamp": 2.7808921501708097 + }, + { + "x": 7.820121969665864, + "y": 7.229538517591083, + "heading": 0.07855370015299508, + "angularVelocity": 0.26044471653762835, + "velocityX": -1.8313208085174242, + "velocityY": -0.8536665630766948, + "timestamp": 2.871590759391534 + }, + { + "x": 7.6043501738014525, + "y": 7.14049675145262, + "heading": 0.10296815130417637, + "angularVelocity": 0.2691821998258671, + "velocityX": -2.3789978448215092, + "velocityY": -0.9817324312192064, + "timestamp": 2.9622893686122582 + }, + { + "x": 7.338891049230189, + "y": 7.039907645727491, + "heading": 0.1254011174832251, + "angularVelocity": 0.24733528299707236, + "velocityX": -2.9268268483063693, + "velocityY": -1.1090479400884177, + "timestamp": 3.0529879778329825 + }, + { + "x": 7.023762471374015, + "y": 6.927998022527093, + "heading": 0.1370927000479183, + "angularVelocity": 0.12890586377394736, + "velocityX": -3.4744587658370385, + "velocityY": -1.2338626155562706, + "timestamp": 3.1436865870537067 + }, + { + "x": 6.694988070852044, + "y": 6.85020567874946, + "heading": 0.13709270182779565, + "angularVelocity": 1.9624086279332165e-8, + "velocityX": -3.624911157368082, + "velocityY": -0.8577016168827462, + "timestamp": 3.234385196274431 + }, + { + "x": 6.366213601226777, + "y": 6.7724136270242035, + "heading": 0.13709270360766948, + "angularVelocity": 1.962404782489554e-8, + "velocityX": -3.624911919268354, + "velocityY": -0.8576983968512897, + "timestamp": 3.3250838054951553 + }, + { + "x": 6.0374391323544945, + "y": 6.69462157211659, + "heading": 0.13709270538754328, + "angularVelocity": 1.962404757872295e-8, + "velocityX": -3.6249119109663046, + "velocityY": -0.857698431938445, + "timestamp": 3.4157824147158795 + }, + { + "x": 5.708664663238629, + "y": 6.616829518238441, + "heading": 0.13709270716741703, + "angularVelocity": 1.9624046615308717e-8, + "velocityX": -3.6249119136519434, + "velocityY": -0.8576984205880644, + "timestamp": 3.5064810239366038 + }, + { + "x": 5.3798901940960535, + "y": 6.539037464473215, + "heading": 0.13709270894730027, + "angularVelocity": 1.9624151634826213e-8, + "velocityX": -3.6249119139464265, + "velocityY": -0.8576984193430164, + "timestamp": 3.597179633157328 + }, + { + "x": 5.082161936855116, + "y": 6.468591654596992, + "heading": 0.17334127091967155, + "angularVelocity": 0.3996595127953577, + "velocityX": -3.2826110543369467, + "velocityY": -0.7767022061472455, + "timestamp": 3.6878782423780523 + }, + { + "x": 4.834055003719788, + "y": 6.409886994261807, + "heading": 0.20354833113507975, + "angularVelocity": 0.33304876971041714, + "velocityX": -2.735509786390822, + "velocityY": -0.647249840318068, + "timestamp": 3.7785768515987765 + }, + { + "x": 4.635569429511693, + "y": 6.362923375586849, + "heading": 0.22771434771261193, + "angularVelocity": 0.2664430776300183, + "velocityX": -2.1884081345179167, + "velocityY": -0.5177986639317362, + "timestamp": 3.869275460819501 + }, + { + "x": 4.486705235532264, + "y": 6.327700716475024, + "heading": 0.24583917728696456, + "angularVelocity": 0.19983580487153885, + "velocityX": -1.6413062477854834, + "velocityY": -0.38834839270916643, + "timestamp": 3.959974070040225 + }, + { + "x": 4.387462434805768, + "y": 6.304218961949016, + "heading": 0.25792250356094953, + "angularVelocity": 0.13322504476974903, + "velocityX": -1.0942042174536384, + "velocityY": -0.25889872764050514, + "timestamp": 4.050672679260949 + }, + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": 0.06661212070370816, + "velocityX": -0.54710211431646, + "velocityY": -0.12944936516318697, + "timestamp": 4.141371288481674 + }, + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": 4.2322185448004084e-23, + "velocityX": 1.750098119560174e-21, + "velocityY": 1.2842584640589406e-21, + "timestamp": 4.232069897702398 + }, + { + "x": 4.357336303204376, + "y": 6.297583237177048, + "heading": 0.24752122945545674, + "angularVelocity": -0.28561960466886005, + "velocityX": 0.3386404361144792, + "velocityY": 0.08867849340285801, + "timestamp": 4.289639128041561 + }, + { + "x": 4.396392552566799, + "y": 6.3076902844788645, + "heading": 0.21545182139905172, + "angularVelocity": -0.5570581344838518, + "velocityX": 0.6784222949017583, + "velocityY": 0.17556335636714013, + "timestamp": 4.3472083583807235 + }, + { + "x": 4.455088009056865, + "y": 6.322669909558741, + "heading": 0.16876627146696008, + "angularVelocity": -0.8109462234782202, + "velocityX": 1.0195629878021513, + "velocityY": 0.26020193411697123, + "timestamp": 4.404777588719886 + }, + { + "x": 4.533516711715102, + "y": 6.342355579726992, + "heading": 0.10873908045256343, + "angularVelocity": -1.042695736259688, + "velocityX": 1.3623371755394689, + "velocityY": 0.3419477740500502, + "timestamp": 4.462346819059049 + }, + { + "x": 4.631792826161861, + "y": 6.366523725829109, + "heading": 0.03701265289139425, + "angularVelocity": -1.2459160412359371, + "velocityX": 1.7070944646606674, + "velocityY": 0.4198101305112548, + "timestamp": 4.519916049398212 + }, + { + "x": 4.750056312234907, + "y": 6.394856813133717, + "heading": -0.04421826686802747, + "angularVelocity": -1.4110127802796573, + "velocityX": 2.054282910789491, + "velocityY": 0.492156784756161, + "timestamp": 4.577485279737375 + }, + { + "x": 4.888478272202705, + "y": 6.42687003155547, + "heading": -0.13181310680866934, + "angularVelocity": -1.5215565576365515, + "velocityX": 2.4044434701020125, + "velocityY": 0.5560820985993948, + "timestamp": 4.635054510076538 + }, + { + "x": 5.047249483224003, + "y": 6.461748830996926, + "heading": -0.22073093757153267, + "angularVelocity": -1.5445374245758388, + "velocityX": 2.757917903121444, + "velocityY": 0.6058583593348625, + "timestamp": 4.6926237404157005 + }, + { + "x": 5.226429045018527, + "y": 6.497908973529479, + "heading": -0.30132047165929504, + "angularVelocity": -1.399871660833024, + "velocityX": 3.1124189213388544, + "velocityY": 0.6281157889295987, + "timestamp": 4.750192970754863 + }, + { + "x": 5.423903541491299, + "y": 6.531166091125145, + "heading": -0.3435649109260108, + "angularVelocity": -0.7338023978753491, + "velocityX": 3.430209077129816, + "velocityY": 0.5776891127384427, + "timestamp": 4.807762201094026 + }, + { + "x": 5.636616267297259, + "y": 6.55438559892435, + "heading": -0.3449872384479477, + "angularVelocity": -0.024706384184006707, + "velocityX": 3.6949030680588684, + "velocityY": 0.40333191294048215, + "timestamp": 4.865331431433189 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.3449872632321153, + "angularVelocity": -4.3051066415640403e-7, + "velocityX": 3.7246964993017384, + "velocityY": 0.047619187556732175, + "timestamp": 4.922900661772352 + }, + { + "x": 5.9707478293428995, + "y": 6.552244535149851, + "heading": -0.34498728566747394, + "angularVelocity": -6.975751659806049e-7, + "velocityX": 3.7219059383782973, + "velocityY": -0.151808826449267, + "timestamp": 4.9550625845387355 + }, + { + "x": 6.090018567999844, + "y": 6.540962083011693, + "heading": -0.3449873062012292, + "angularVelocity": -6.384492454517859e-7, + "velocityX": 3.7084455280643134, + "velocityY": -0.3508015431822894, + "timestamp": 4.987224507305119 + }, + { + "x": 6.208514463680753, + "y": 6.523311991997161, + "heading": -0.3449873252853557, + "angularVelocity": -5.933764159352289e-7, + "velocityX": 3.6843535923407127, + "velocityY": -0.5487884273193968, + "timestamp": 5.019386430071503 + }, + { + "x": 6.325895808833114, + "y": 6.499344869625537, + "heading": -0.34498734327239833, + "angularVelocity": -5.592651525230829e-7, + "velocityX": 3.6496992423305046, + "velocityY": -0.7452017886404377, + "timestamp": 5.051548352837886 + }, + { + "x": 6.441826094112058, + "y": 6.46912943973789, + "heading": -0.34498736044751604, + "angularVelocity": -5.340202407325154e-7, + "velocityX": 3.6045819188434196, + "velocityY": -0.9394783423592116, + "timestamp": 5.08371027560427 + }, + { + "x": 6.555972977983021, + "y": 6.432752355963861, + "heading": -0.3449873770504224, + "angularVelocity": -5.162286594877017e-7, + "velocityX": 3.5491312102233143, + "velocityY": -1.131060603505082, + "timestamp": 5.115872198370654 + }, + { + "x": 6.66800926163723, + "y": 6.390318000635929, + "heading": -0.3449873932911425, + "angularVelocity": -5.049673254841841e-7, + "velocityX": 3.4835070175379053, + "velocityY": -1.319397339399292, + "timestamp": 5.148034121137037 + }, + { + "x": 6.777614079975414, + "y": 6.34194872129089, + "heading": -0.344987409362088, + "angularVelocity": -4.996885790126847e-7, + "velocityX": 3.407906272716571, + "velocityY": -1.5039299639011887, + "timestamp": 5.180196043903421 + }, + { + "x": 6.886436638977974, + "y": 6.291844289930693, + "heading": -0.3449874254080791, + "angularVelocity": -4.98912679463648e-7, + "velocityX": 3.3835837425834847, + "velocityY": -1.557880470149199, + "timestamp": 5.2123579666698046 + }, + { + "x": 6.995259181110964, + "y": 6.241739821931245, + "heading": -0.34498744145406995, + "angularVelocity": -4.989126736688575e-7, + "velocityX": 3.383583218063541, + "velocityY": -1.5578816093613586, + "timestamp": 5.244519889436188 + }, + { + "x": 7.104082434838663, + "y": 6.191636899482441, + "heading": -0.34498745750007104, + "angularVelocity": -4.98912992332448e-7, + "velocityX": 3.3836053434418156, + "velocityY": -1.5578335540676167, + "timestamp": 5.276681812202572 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -6.552383423892292e-7, + "velocityX": 3.442174665036408, + "velocityY": -1.4237498464482023, + "timestamp": 5.3088437349689555 + }, + { + "x": 7.41054811335349, + "y": 6.075273519082507, + "heading": -0.3626370912094401, + "angularVelocity": -0.2874236730044668, + "velocityX": 3.187927819628061, + "velocityY": -1.1492777517419182, + "timestamp": 5.370249994415684 + }, + { + "x": 7.584442418775818, + "y": 6.012933970935389, + "heading": -0.3763799731717047, + "angularVelocity": -0.22380262347988858, + "velocityX": 2.831866115752982, + "velocityY": -1.0151985922737843, + "timestamp": 5.431656253862413 + }, + { + "x": 7.73639717593505, + "y": 5.958610534020796, + "heading": -0.3857543878763611, + "angularVelocity": -0.15266220071243664, + "velocityX": 2.4745809063822897, + "velocityY": -0.8846563429208651, + "timestamp": 5.493062513309142 + }, + { + "x": 7.866387295940067, + "y": 5.912228690600519, + "heading": -0.39060211670074774, + "angularVelocity": -0.07894519008427879, + "velocityX": 2.1168871248017687, + "velocityY": -0.7553276137999276, + "timestamp": 5.554468772755871 + }, + { + "x": 7.974400233400663, + "y": 5.873750785403185, + "heading": -0.39084333039715696, + "angularVelocity": -0.003928161372840292, + "velocityX": 1.7589890417327878, + "velocityY": -0.62661210019988, + "timestamp": 5.6158750322026 + }, + { + "x": 8.060428467430407, + "y": 5.843154117743718, + "heading": -0.3864300742351692, + "angularVelocity": 0.0718698093932331, + "velocityX": 1.4009684811427807, + "velocityY": -0.49826626691061593, + "timestamp": 5.6772812916493285 + }, + { + "x": 8.124466994034947, + "y": 5.820423531988022, + "heading": -0.37733049959220943, + "angularVelocity": 0.14818643449294983, + "velocityX": 1.0428664305809843, + "velocityY": -0.3701672428918256, + "timestamp": 5.738687551096057 + }, + { + "x": 8.16651225164957, + "y": 5.805548215416436, + "heading": -0.3635219686266917, + "angularVelocity": 0.22487171649816598, + "velocityX": 0.6847063799920386, + "velocityY": -0.24224430384806347, + "timestamp": 5.800093810542786 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.3018338882695626, + "velocityX": 0.32650307971422843, + "velocityY": -0.11445294476423894, + "timestamp": 5.861500069989515 + }, + { + "x": 8.182873431203555, + "y": 5.799961031790675, + "heading": -0.31977664522832494, + "angularVelocity": 0.3842917157740494, + "velocityX": -0.056218956684159486, + "velocityY": 0.021964473717296987, + "timestamp": 5.927103444485088 + }, + { + "x": 8.154073485915251, + "y": 5.810352807071496, + "heading": -0.2894961331766682, + "angularVelocity": 0.46156942816562346, + "velocityX": -0.4390009738027679, + "velocityY": 0.15840306021945175, + "timestamp": 5.99270681898066 + }, + { + "x": 8.100157148407472, + "y": 5.829696959053818, + "heading": -0.25457167500592204, + "angularVelocity": 0.5323576483569935, + "velocityX": -0.8218531123184383, + "velocityY": 0.29486519757649204, + "timestamp": 6.058310193476233 + }, + { + "x": 8.021119032382456, + "y": 5.857995174413058, + "heading": -0.21555305979528627, + "angularVelocity": 0.5947653685599519, + "velocityX": -1.2047873548082333, + "velocityY": 0.4313530451265237, + "timestamp": 6.1239135679718055 + }, + { + "x": 7.916952910031998, + "y": 5.895249222659617, + "heading": -0.17317929559353756, + "angularVelocity": 0.6459083016317765, + "velocityX": -1.5878165285154446, + "velocityY": 0.567867865532928, + "timestamp": 6.189516942467378 + }, + { + "x": 7.787651985971443, + "y": 5.941460762829762, + "heading": -0.12849926666272526, + "angularVelocity": 0.6810629677872386, + "velocityX": -1.9709492850749835, + "velocityY": 0.7044079748255081, + "timestamp": 6.255120316962951 + }, + { + "x": 7.633210581172123, + "y": 5.996630683324096, + "heading": -0.08312168761624714, + "angularVelocity": 0.6916958067384257, + "velocityX": -2.354168607740487, + "velocityY": 0.8409616261135585, + "timestamp": 6.320723691458523 + }, + { + "x": 7.453632538523732, + "y": 6.060756399754721, + "heading": -0.03982832718204048, + "angularVelocity": 0.6599258158149841, + "velocityX": -2.7373293527836737, + "velocityY": 0.9774758832711061, + "timestamp": 6.386327065954096 + }, + { + "x": 7.248984101269758, + "y": 6.133814967695316, + "heading": -0.004587884016795663, + "angularVelocity": 0.5371742450172758, + "velocityX": -3.1194803442281134, + "velocityY": 1.11364039582332, + "timestamp": 6.451930440449669 + }, + { + "x": 7.020152981307638, + "y": 6.215344107160959, + "heading": -7.996010360253712e-8, + "angularVelocity": 0.06993244009119806, + "velocityX": -3.488099838180771, + "velocityY": 1.242758319865779, + "timestamp": 6.517533814945241 + }, + { + "x": 6.789970042208637, + "y": 6.297403837687721, + "heading": -7.492708758217055e-8, + "angularVelocity": 7.671885873349635e-8, + "velocityX": -3.5087057772391836, + "velocityY": 1.2508461821929457, + "timestamp": 6.583137189440814 + }, + { + "x": 6.5597871223902215, + "y": 6.379463622297776, + "heading": -6.98940737576559e-8, + "angularVelocity": 7.671882526186561e-8, + "velocityX": -3.5087054833429296, + "velocityY": 1.2508470065910036, + "timestamp": 6.648740563936387 + }, + { + "x": 6.3296039103658925, + "y": 6.46152258724702, + "heading": -6.48610593666683e-8, + "angularVelocity": 7.671883389668008e-8, + "velocityX": -3.5087099374722537, + "velocityY": 1.2508345124042644, + "timestamp": 6.714343938431959 + }, + { + "x": 6.092938315579126, + "y": 6.522410853761584, + "heading": -5.975605245251985e-8, + "angularVelocity": 7.781622444578512e-8, + "velocityX": -3.6075216649524458, + "velocityY": 0.9281270511271241, + "timestamp": 6.779947312927532 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -5.436468625345948e-8, + "angularVelocity": 8.218123290935499e-8, + "velocityX": -3.6872209612695626, + "velocityY": 0.5291823081772375, + "timestamp": 6.8455506874231045 + }, + { + "x": 5.6615808215647085, + "y": 6.568071618356437, + "heading": -4.94825886082418e-8, + "angularVelocity": 9.582618856188379e-8, + "velocityX": -3.71880135129745, + "velocityY": 0.2148218335343848, + "timestamp": 6.896498113366742 + }, + { + "x": 5.471871500054035, + "y": 6.562921703037617, + "heading": -4.486806140713326e-8, + "angularVelocity": 9.057429527869525e-8, + "velocityX": -3.7236291725620743, + "velocityY": -0.10108293448460899, + "timestamp": 6.94744553931038 + }, + { + "x": 5.283280913941195, + "y": 6.541714647593256, + "heading": -4.0367954549807895e-8, + "angularVelocity": 8.832844395914553e-8, + "velocityX": -3.7016705480170033, + "velocityY": -0.4162537174659499, + "timestamp": 6.998392965254017 + }, + { + "x": 5.095501482386873, + "y": 6.514236775268743, + "heading": -3.5882626212892094e-8, + "angularVelocity": 8.803837002241881e-8, + "velocityX": -3.68574914387354, + "velocityY": -0.539337794119592, + "timestamp": 7.049340391197655 + }, + { + "x": 4.907722071138919, + "y": 6.486758764174086, + "heading": -3.139729791987603e-8, + "angularVelocity": 8.803836916075124e-8, + "velocityX": -3.6857487452985973, + "velocityY": -0.5393405179106829, + "timestamp": 7.100287817141292 + }, + { + "x": 4.7199426574500984, + "y": 6.459280769759847, + "heading": -2.6911969659920007e-8, + "angularVelocity": 8.803836851184644e-8, + "velocityX": -3.6857487932081243, + "velocityY": -0.5393401905061415, + "timestamp": 7.15123524308493 + }, + { + "x": 4.532163244787761, + "y": 6.431802768330805, + "heading": -2.2426641378076926e-8, + "angularVelocity": 8.803836894144722e-8, + "velocityX": -3.6857487730602023, + "velocityY": -0.5393403281932087, + "timestamp": 7.202182669028567 + }, + { + "x": 4.3443838326647874, + "y": 6.404324763215868, + "heading": -1.7941313081549045e-8, + "angularVelocity": 8.803836922968166e-8, + "velocityX": -3.6857487624735623, + "velocityY": -0.5393404005402795, + "timestamp": 7.253130094972205 + }, + { + "x": 4.156604420793327, + "y": 6.376846756382128, + "heading": -1.3455984799695103e-8, + "angularVelocity": 8.80383689416604e-8, + "velocityX": -3.685748757536816, + "velocityY": -0.5393404342770572, + "timestamp": 7.304077520915842 + }, + { + "x": 3.968825009218295, + "y": 6.349368747522659, + "heading": -8.970656536814864e-9, + "angularVelocity": 8.803836856924311e-8, + "velocityX": -3.685748751718508, + "velocityY": -0.5393404740382505, + "timestamp": 7.35502494685948 + }, + { + "x": 3.781045597297714, + "y": 6.321890741024598, + "heading": -4.485328240150933e-9, + "angularVelocity": 8.803836923235202e-8, + "velocityX": -3.6857487585009534, + "velocityY": -0.5393404276883234, + "timestamp": 7.4059723728031175 + }, + { + "x": 3.5932661777631214, + "y": 6.2944127865592865, + "heading": -1.5290485245544054e-22, + "angularVelocity": 8.803836812310754e-8, + "velocityX": -3.6857489079493813, + "velocityY": -0.5393394063855248, + "timestamp": 7.456919798746755 + }, + { + "x": 3.405486822128296, + "y": 6.266934394836426, + "heading": -7.242717716527151e-23, + "angularVelocity": 4.066531625418711e-24, + "velocityX": -3.6857476537198193, + "velocityY": -0.5393479889103638, + "timestamp": 7.507867224690393 + }, + { + "x": 3.1943671277625394, + "y": 6.2230448572371815, + "heading": -6.454692239159141e-23, + "angularVelocity": 6.1636286209902435e-25, + "velocityX": -3.5183764765296694, + "velocityY": -0.7314330248480798, + "timestamp": 7.5678720865276885 + }, + { + "x": 3.002694024502071, + "y": 6.190127687410396, + "heading": -5.666660567021065e-23, + "angularVelocity": 6.173952401211252e-25, + "velocityX": -3.1942928854697095, + "velocityY": -0.5485750457361542, + "timestamp": 7.627876948364984 + }, + { + "x": 2.830467511330891, + "y": 6.168182901091022, + "heading": -4.87863785909476e-23, + "angularVelocity": 6.159013258773547e-25, + "velocityX": -2.870209311341694, + "velocityY": -0.36571680439623283, + "timestamp": 7.68788181020228 + }, + { + "x": 2.6776875881237743, + "y": 6.157210503145755, + "heading": -4.0906073219636964e-23, + "angularVelocity": 6.172060876090466e-25, + "velocityX": -2.5461257393006025, + "velocityY": -0.1828584819513171, + "timestamp": 7.747886672039576 + }, + { + "x": 2.5443542548814535, + "y": 6.157210495895674, + "heading": -3.3025815924682597e-23, + "angularVelocity": 6.164048799254959e-25, + "velocityX": -2.2220421672473036, + "velocityY": -1.208249057531058e-7, + "timestamp": 7.807891533876872 + }, + { + "x": 2.430467511630842, + "y": 6.168182880686502, + "heading": -2.5145589184649632e-23, + "angularVelocity": 6.158956724948031e-25, + "velocityX": -1.8979585947454753, + "velocityY": 0.18285826272844113, + "timestamp": 7.867896395714168 + }, + { + "x": 2.3360273584029394, + "y": 6.1901276583922495, + "heading": -1.726521486745078e-23, + "angularVelocity": 6.183550926375638e-25, + "velocityX": -1.5738750217270354, + "velocityY": 0.3657166608474323, + "timestamp": 7.927901257551464 + }, + { + "x": 2.261033795226874, + "y": 6.223044829624826, + "heading": -9.385003241848346e-24, + "angularVelocity": 6.156437857286479e-25, + "velocityX": -1.2497914482231562, + "velocityY": 0.5485750691641013, + "timestamp": 7.98790611938876 + }, + { + "x": 2.205486822128296, + "y": 6.266934394836426, + "heading": -1.5067646314431248e-24, + "angularVelocity": 5.827628634700505e-25, + "velocityX": -0.9257078742918208, + "velocityY": 0.7314334850167038, + "timestamp": 8.047910981226057 + }, + { + "x": 2.166583654686104, + "y": 6.330345399138925, + "heading": 0.03190215483964595, + "angularVelocity": 0.46789281830201823, + "velocityX": -0.5705731398677051, + "velocityY": 0.9300172249677102, + "timestamp": 8.116093593977881 + }, + { + "x": 2.1528025694390287, + "y": 6.405563401029407, + "heading": 0.09569947573294339, + "angularVelocity": 0.9356831356039661, + "velocityX": -0.20212022817660624, + "velocityY": 1.1031845048863924, + "timestamp": 8.184276206729706 + }, + { + "x": 2.1659059898168422, + "y": 6.488336785914272, + "heading": 0.18898264963043915, + "angularVelocity": 1.3681372733109376, + "velocityX": 0.1921812592531237, + "velocityY": 1.2139954974466904, + "timestamp": 8.25245881948153 + }, + { + "x": 2.2052860593561716, + "y": 6.55939873181835, + "heading": 0.2899151707587667, + "angularVelocity": 1.4803263919455447, + "velocityX": 0.5775676224475003, + "velocityY": 1.0422297274341026, + "timestamp": 8.320641432233355 + }, + { + "x": 2.238286501729519, + "y": 6.603471711362833, + "heading": 0.35830069991198743, + "angularVelocity": 1.0029760725382513, + "velocityX": 0.4840008477449354, + "velocityY": 0.6463961670829911, + "timestamp": 8.38882404498518 + }, + { + "x": 2.256009101867676, + "y": 6.624549388885498, + "heading": 0.3930579718029317, + "angularVelocity": 0.5097673803944184, + "velocityX": 0.2599284395666251, + "velocityY": 0.3091356677601188, + "timestamp": 8.457006657737004 + }, + { + "x": 2.256009101867676, + "y": 6.624549388885498, + "heading": 0.3930579718029317, + "angularVelocity": 1.058900443902291e-24, + "velocityX": 2.5593812661454674e-24, + "velocityY": -7.939073774690169e-24, + "timestamp": 8.525189270488829 + }, + { + "x": 2.306032658844481, + "y": 6.645836035930219, + "heading": 0.3930579718029317, + "angularVelocity": 7.690085231355673e-17, + "velocityX": 0.5342706205732553, + "velocityY": 0.22734948919726436, + "timestamp": 8.618818893849236 + }, + { + "x": 2.406079771576901, + "y": 6.688409329500005, + "heading": 0.3930579718029317, + "angularVelocity": 7.285372137244723e-17, + "velocityX": 1.0685412281037379, + "velocityY": 0.45469897284440564, + "timestamp": 8.712448517209642 + }, + { + "x": 2.556150436401367, + "y": 6.752269268035889, + "heading": 0.3930579718029317, + "angularVelocity": 1.0864711023830975e-16, + "velocityX": 1.6028117965059043, + "velocityY": 0.6820484398411784, + "timestamp": 8.80607814057005 + }, + { + "x": 2.7062211012258333, + "y": 6.816129206571772, + "heading": 0.3930579718029317, + "angularVelocity": 1.1973301038102625e-17, + "velocityX": 1.6028117965059043, + "velocityY": 0.6820484398411785, + "timestamp": 8.899707763930456 + }, + { + "x": 2.8062682139582535, + "y": 6.8587025001415585, + "heading": 0.3930579718029317, + "angularVelocity": -9.389335874202783e-18, + "velocityX": 1.0685412281037379, + "velocityY": 0.45469897284440564, + "timestamp": 8.993337387290863 + }, + { + "x": 2.8562917709350586, + "y": 6.879989147186279, + "heading": 0.3930579718029317, + "angularVelocity": 6.118834146220853e-17, + "velocityX": 0.5342706205732553, + "velocityY": 0.22734948919726436, + "timestamp": 9.08696701065127 + }, + { + "x": 2.8562917709350586, + "y": 6.879989147186279, + "heading": 0.3930579718029317, + "angularVelocity": 9.527092238533276e-28, + "velocityX": 5.413823231130942e-26, + "velocityY": -7.426199884271778e-26, + "timestamp": 9.180596634011676 + }, + { + "x": 2.8345596275672933, + "y": 6.874297000721419, + "heading": 0.3901475474656487, + "angularVelocity": -0.04834267054248828, + "velocityX": -0.36097480135517596, + "velocityY": -0.09454757428506516, + "timestamp": 9.240800679518948 + }, + { + "x": 2.791210221547733, + "y": 6.8624934986264625, + "heading": 0.38424126669366737, + "angularVelocity": -0.0981043835545563, + "velocityX": -0.7200414134017654, + "velocityY": -0.19605828803531558, + "timestamp": 9.30100472502622 + }, + { + "x": 2.7264146334756165, + "y": 6.84400633385399, + "heading": 0.3752232336055894, + "angularVelocity": -0.14979114795514328, + "velocityX": -1.076266345993141, + "velocityY": -0.3070751245485542, + "timestamp": 9.36120877053349 + }, + { + "x": 2.6404493956081283, + "y": 6.818009888574526, + "heading": 0.36292730942366375, + "angularVelocity": -0.20423750726918255, + "velocityX": -1.4278980281666382, + "velocityY": -0.431805621373474, + "timestamp": 9.421412816040762 + }, + { + "x": 2.5338184232966747, + "y": 6.783217894913327, + "heading": 0.3470968619442176, + "angularVelocity": -0.2629465735410421, + "velocityX": -1.7711595859214113, + "velocityY": -0.5779012584294861, + "timestamp": 9.481616861548034 + }, + { + "x": 2.407629795142463, + "y": 6.737392802929253, + "heading": 0.3272924409497442, + "angularVelocity": -0.3289549867887417, + "velocityX": -2.0960157592560678, + "velocityY": -0.761163001555094, + "timestamp": 9.541820907055305 + }, + { + "x": 2.2652115507191506, + "y": 6.676063122618794, + "heading": 0.30267146092102903, + "angularVelocity": -0.40895889671967695, + "velocityX": -2.3655925980274817, + "velocityY": -1.0186969960855932, + "timestamp": 9.602024952562576 + }, + { + "x": 2.1204311340072723, + "y": 6.592422966921264, + "heading": 0.27232018505584993, + "angularVelocity": -0.5041401389133051, + "velocityX": -2.4048287036523215, + "velocityY": -1.3892779960680068, + "timestamp": 9.662228998069848 + }, + { + "x": 1.992685360010843, + "y": 6.494128334362165, + "heading": 0.23976877399587285, + "angularVelocity": -0.5406847793317353, + "velocityX": -2.1218802311382734, + "velocityY": -1.6326914866082545, + "timestamp": 9.72243304357712 + }, + { + "x": 1.886238292214546, + "y": 6.388651713547729, + "heading": 0.20716928495643328, + "angularVelocity": -0.5414833631986058, + "velocityX": -1.7681048989214407, + "velocityY": -1.7519856003978374, + "timestamp": 9.78263708908439 + }, + { + "x": 1.801932900713492, + "y": 6.279311808551523, + "heading": 0.17534566664181037, + "angularVelocity": -0.5285960112228498, + "velocityX": -1.4003276821467197, + "velocityY": -1.8161554439559784, + "timestamp": 9.842841134591662 + }, + { + "x": 1.740001559257507, + "y": 6.167843341827393, + "heading": 0.1447051188724413, + "angularVelocity": -0.5089449971541862, + "velocityX": -1.0286906956859625, + "velocityY": -1.8515112362451833, + "timestamp": 9.903045180098934 + }, + { + "x": 1.69740928285499, + "y": 6.020477623351315, + "heading": 0.10712758929116097, + "angularVelocity": -0.47810334665219445, + "velocityX": -0.5419065626848166, + "velocityY": -1.874951439604909, + "timestamp": 9.98164226552912 + }, + { + "x": 1.692516979833633, + "y": 5.879848304102219, + "heading": 0.07335850742026521, + "angularVelocity": -0.429648016667122, + "velocityX": -0.06224534910652133, + "velocityY": -1.7892434366922005, + "timestamp": 10.060239350959305 + }, + { + "x": 1.7224083305171312, + "y": 5.755218848404405, + "heading": 0.044801662501508355, + "angularVelocity": -0.3633321103760591, + "velocityX": 0.38031118482184595, + "velocityY": -1.585675283195029, + "timestamp": 10.138836436389491 + }, + { + "x": 1.7819268218986297, + "y": 5.6548248770557, + "heading": 0.022621166422217082, + "angularVelocity": -0.282205071064539, + "velocityX": 0.757260794795832, + "velocityY": -1.277324353685865, + "timestamp": 10.217433521819677 + }, + { + "x": 1.8649022251310534, + "y": 5.584669337944561, + "heading": 0.0075685613575453385, + "angularVelocity": -0.19151607190373818, + "velocityX": 1.055705854463611, + "velocityY": -0.8925972092623536, + "timestamp": 10.296030607249863 + }, + { + "x": 1.9654334653384358, + "y": 5.548515796661377, + "heading": 4.814116720601663e-28, + "angularVelocity": -0.09629569997564517, + "velocityX": 1.2790708415858416, + "velocityY": -0.4599857753669162, + "timestamp": 10.374627692680049 + }, + { + "x": 2.0785037517547607, + "y": 5.548515796661377, + "heading": 4.711208229171849e-28, + "angularVelocity": -4.224112944688565e-28, + "velocityX": 1.43860660732464, + "velocityY": -1.1518576201030206e-26, + "timestamp": 10.453224778110235 + }, + { + "x": 2.2115594481070073, + "y": 5.548515796661377, + "heading": 9.919579688456685e-28, + "angularVelocity": -1.4478744000102224e-30, + "velocityX": 1.8779766683240853, + "velocityY": 6.343973751493874e-16, + "timestamp": 10.52407533690391 + }, + { + "x": 2.367207543861562, + "y": 5.548515796661377, + "heading": 1.5127951147748595e-27, + "angularVelocity": -1.4478743900360266e-30, + "velocityX": 2.196850644577389, + "velocityY": 3.1719689645397288e-15, + "timestamp": 10.594925895697587 + }, + { + "x": 2.4917260252771096, + "y": 5.548515796661377, + "heading": 2.033632260703789e-27, + "angularVelocity": -1.4478743937385766e-30, + "velocityX": 1.757480583578164, + "velocityY": 2.537576634676114e-15, + "timestamp": 10.665776454491263 + }, + { + "x": 2.585114887925518, + "y": 5.548515796661377, + "heading": 2.5544694066327203e-27, + "angularVelocity": -1.4478743937382235e-30, + "velocityX": 1.3181104600793105, + "velocityY": 1.903182958104165e-15, + "timestamp": 10.73662701328494 + }, + { + "x": 2.6473741303307423, + "y": 5.548515796661377, + "heading": 3.0753065525616534e-27, + "angularVelocity": -1.4478743937387e-30, + "velocityX": 0.8787403157472581, + "velocityY": 1.2687888330822125e-15, + "timestamp": 10.807477572078616 + }, + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": 3.596143698490385e-27, + "angularVelocity": -1.4478743965967547e-30, + "velocityX": 0.439370160998608, + "velocityY": 6.343944838152068e-16, + "timestamp": 10.878328130872292 + }, + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": 4.090371729515312e-27, + "angularVelocity": -3.7701463571595157e-28, + "velocityX": -1.25991204352499e-25, + "velocityY": -5.479637988962548e-26, + "timestamp": 10.949178689665969 + }, + { + "x": 2.6551487281624913, + "y": 5.535337362195577, + "heading": -0.005358266388786186, + "angularVelocity": -0.0814332978786503, + "velocityX": -0.3549425234125069, + "velocityY": -0.20028182653884274, + "timestamp": 11.014978141835657 + }, + { + "x": 2.6089441252483114, + "y": 5.508126235802502, + "heading": -0.016249929633628903, + "angularVelocity": -0.16552817517013077, + "velocityX": -0.7022034590048662, + "velocityY": -0.41354639736059473, + "timestamp": 11.080777594005346 + }, + { + "x": 2.5407349136089206, + "y": 5.4655968271259, + "heading": -0.03293816472196892, + "angularVelocity": -0.2536227056313986, + "velocityX": -1.036622789251932, + "velocityY": -0.6463489782091849, + "timestamp": 11.146577046175034 + }, + { + "x": 2.4521626111212957, + "y": 5.405634188324023, + "heading": -0.055847587039488555, + "angularVelocity": -0.3481704111827463, + "velocityX": -1.3460948315984265, + "velocityY": -0.9112938911290818, + "timestamp": 11.212376498344723 + }, + { + "x": 2.347299393995622, + "y": 5.324395116461008, + "heading": -0.08569447081932854, + "angularVelocity": -0.45360383400866283, + "velocityX": -1.5936791822406984, + "velocityY": -1.2346466297851597, + "timestamp": 11.278175950514411 + }, + { + "x": 2.239880929566337, + "y": 5.216483135728542, + "heading": -0.1227261468780359, + "angularVelocity": -0.5627961151288575, + "velocityX": -1.6325130512069794, + "velocityY": -1.6400133614209302, + "timestamp": 11.3439754026841 + }, + { + "x": 2.153374403298574, + "y": 5.091739159361965, + "heading": -0.1618030539678993, + "angularVelocity": -0.5938789123819666, + "velocityX": -1.3146997948352968, + "velocityY": -1.8958208959685574, + "timestamp": 11.409774854853788 + }, + { + "x": 2.0932393074035645, + "y": 4.9619622230529785, + "heading": -0.19963034769256285, + "angularVelocity": -0.5748876696892882, + "velocityX": -0.9139148414173566, + "velocityY": -1.9723102857195454, + "timestamp": 11.475574307023477 + }, + { + "x": 2.0629011107007105, + "y": 4.862373019847467, + "heading": -0.22719939375272252, + "angularVelocity": -0.5460591677053324, + "velocityX": -0.600907641312322, + "velocityY": -1.9725599970405534, + "timestamp": 11.52606159428213 + }, + { + "x": 2.048028599586168, + "y": 4.7660067703914, + "heading": -0.2526003668147138, + "angularVelocity": -0.5031162187791786, + "velocityX": -0.29457932723437075, + "velocityY": -1.908723060567117, + "timestamp": 11.576548881540782 + }, + { + "x": 2.047740775609064, + "y": 4.675697634358311, + "heading": -0.2752320074015959, + "angularVelocity": -0.44826414362367223, + "velocityX": -0.00570091983015821, + "velocityY": -1.788750018800283, + "timestamp": 11.627036168799435 + }, + { + "x": 2.0608710405337267, + "y": 4.5937072504707785, + "heading": -0.29463334748351117, + "angularVelocity": -0.3842816902108431, + "velocityX": 0.2600707155723077, + "velocityY": -1.62398077495204, + "timestamp": 11.677523456058088 + }, + { + "x": 2.0861920181883997, + "y": 4.5217424317614965, + "heading": -0.3104741156384499, + "angularVelocity": -0.31375756185482273, + "velocityX": 0.5015317524380102, + "velocityY": -1.4254047427939316, + "timestamp": 11.72801074331674 + }, + { + "x": 2.122546344803851, + "y": 4.461060494351438, + "heading": -0.3225286157246152, + "angularVelocity": -0.23876307761216561, + "velocityX": 0.7200689240680361, + "velocityY": -1.2019250925322578, + "timestamp": 11.778498030575394 + }, + { + "x": 2.1689002066225895, + "y": 4.412584630951134, + "heading": -0.3306480218329806, + "angularVelocity": -0.16082080359692716, + "velocityX": 0.9181293813878815, + "velocityY": -0.9601597953155385, + "timestamp": 11.828985317834046 + }, + { + "x": 2.2243532862123785, + "y": 4.376998478332531, + "heading": -0.3347371673945201, + "angularVelocity": -0.0809935685510361, + "velocityX": 1.098357281620145, + "velocityY": -0.7048537275590006, + "timestamp": 11.8794726050927 + }, + { + "x": 2.2881293296813965, + "y": 4.354815483093262, + "heading": -0.3347371673945201, + "angularVelocity": 5.296142385201319e-27, + "velocityX": 1.2632099471355256, + "velocityY": -0.43937784031895555, + "timestamp": 11.929959892351352 + }, + { + "x": 2.4097151173059284, + "y": 4.31443319431648, + "heading": -0.3347371673945201, + "angularVelocity": 4.260827770605265e-29, + "velocityX": 1.6917387541138205, + "velocityY": -0.5618772081689671, + "timestamp": 12.001830201717304 + }, + { + "x": 2.501147235462863, + "y": 4.284859003573599, + "heading": -0.3347371673945201, + "angularVelocity": 4.260827800538464e-29, + "velocityX": 1.2721820590944748, + "velocityY": -0.41149385613875655, + "timestamp": 12.073700511083256 + }, + { + "x": 2.562139216145648, + "y": 4.265257651838079, + "heading": -0.3347371673945201, + "angularVelocity": 4.260827801548558e-29, + "velocityX": 0.8486394621209058, + "velocityY": -0.27273225770760434, + "timestamp": 12.145570820449208 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": 1.3873453317425255e-27, + "velocityX": 0.4244517046157105, + "velocityY": -0.13595549514112237, + "timestamp": 12.21744112981516 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": -3.25532193935464e-28, + "velocityX": 2.1483187881556724e-26, + "velocityY": -1.7096603516619137e-26, + "timestamp": 12.289311439181112 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.3350272178649902, + "y": 5.601006507873535, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 0.5891141473001633, + "isStopPoint": false, + "x": 2.3368515968322754, + "y": 6.133185386657715, + "heading": 0.2525545797921912, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 1.5430433785236604, + "isStopPoint": false, + "x": 5.747424125671387, + "y": 7.119814872741699, + "heading": 0.2525545797921912, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 1.94301474943974, + "isStopPoint": false, + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 2.508796322508637, + "isStopPoint": false, + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 4.232069897702398, + "isStopPoint": true, + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 4.922900661772352, + "isStopPoint": false, + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "timestamp": 5.3088437349689555, + "isStopPoint": false, + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 5.861500069989515, + "isStopPoint": false, + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 6.8455506874231045, + "isStopPoint": false, + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 7.507867224690393, + "isStopPoint": false, + "x": 3.405486822128296, + "y": 6.266934394836426, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 8.047910981226057, + "isStopPoint": false, + "x": 2.205486822128296, + "y": 6.266934394836426, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 8.525189270488829, + "isStopPoint": true, + "x": 2.256009101867676, + "y": 6.624549388885498, + "heading": 0.3930579718029317, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 9.180596634011676, + "isStopPoint": true, + "x": 2.8562917709350586, + "y": 6.879989147186279, + "heading": 0.3930579718029317, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 9.903045180098934, + "isStopPoint": false, + "x": 1.7400015592575073, + "y": 6.167843341827393, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 7 + }, + { + "timestamp": 10.453224778110235, + "isStopPoint": false, + "x": 2.0785037517547607, + "y": 5.548515796661377, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 10.949178689665969, + "isStopPoint": true, + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 11.475574307023477, + "isStopPoint": false, + "x": 2.0932393074035645, + "y": 4.9619622230529785, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "timestamp": 11.929959892351352, + "isStopPoint": false, + "x": 2.2881293296813965, + "y": 4.354815483093262, + "heading": -0.3347371673945201, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "timestamp": 12.289311439181112, + "isStopPoint": true, + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 12 + ], + "type": "StopPoint" + }, + { + "scope": [ + 1, + 2 + ], + "type": "StraightLine" + }, + { + "scope": [ + 13 + ], + "type": "StopPoint" + }, + { + "scope": [ + 10, + 11 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 16 + ], + "type": "StopPoint" + }, + { + "scope": [ + 19 + ], + "type": "StopPoint" + }, + { + "scope": [ + 15, + 16 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 18, + 19 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 18 + ], + "type": "WptVelocityDirection", + "direction": -0.3347371673945201 + }, + { + "scope": [ + 15 + ], + "type": "WptVelocityDirection", + "direction": 0 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "SourceLanePHGF": { + "waypoints": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 8.208122253417969, + "y": 0.763107419013977, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 19 + }, + { + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": 5.0336126459058196e-26, + "angularVelocity": 1.4969503738616677e-26, + "velocityX": 6.632423953121246e-26, + "velocityY": 2.3403167387234696e-25, + "timestamp": 0 + }, + { + "x": 0.40051040035567387, + "y": 4.115348381940811, + "heading": -0.015277315995300885, + "angularVelocity": -0.30891158633599436, + "velocityX": 0.2731840597647725, + "velocityY": -0.11699249437857151, + "timestamp": 0.04945530265311636 + }, + { + "x": 0.42755206280004615, + "y": 4.103759116150659, + "heading": -0.04541480648334463, + "angularVelocity": -0.6093884552568736, + "velocityX": 0.5467899495842703, + "velocityY": -0.23433818354000846, + "timestamp": 0.09891060530623272 + }, + { + "x": 0.46814907636931, + "y": 4.086345867978605, + "heading": -0.08991129386238127, + "angularVelocity": -0.8997313734209399, + "velocityX": 0.8208829264278229, + "velocityY": -0.3521007301116199, + "timestamp": 0.14836590795934906 + }, + { + "x": 0.5223311730580029, + "y": 4.063083084813687, + "heading": -0.14811250055404074, + "angularVelocity": -1.1768446166407593, + "velocityX": 1.095577092485525, + "velocityY": -0.4703799575969681, + "timestamp": 0.19782121061246544 + }, + { + "x": 0.5901369333153729, + "y": 4.033939247984461, + "heading": -0.2191152852440613, + "angularVelocity": -1.4356960908325656, + "velocityX": 1.3710513659771775, + "velocityY": -0.5892965014013483, + "timestamp": 0.2472765132655818 + }, + { + "x": 0.6716171763309451, + "y": 3.9988769782710625, + "heading": -0.3016414776381226, + "angularVelocity": -1.6687026055205227, + "velocityX": 1.6475532176414214, + "velocityY": -0.7089688634468179, + "timestamp": 0.2967318159186982 + }, + { + "x": 0.766836765570499, + "y": 3.957853748642466, + "heading": -0.39387111054068624, + "angularVelocity": -1.864908876394308, + "velocityX": 1.9253666266574487, + "velocityY": -0.8295011339094811, + "timestamp": 0.34618711857181456 + }, + { + "x": 0.8758711660110932, + "y": 3.910821247404655, + "heading": -0.49317423957562295, + "angularVelocity": -2.007936939168226, + "velocityX": 2.2047059585373607, + "velocityY": -0.9510102802868479, + "timestamp": 0.39564242122493093 + }, + { + "x": 0.998787530743033, + "y": 3.857723681334368, + "heading": -0.5954683580210677, + "angularVelocity": -2.0684155784657574, + "velocityX": 2.4854031446149563, + "velocityY": -1.0736475811849282, + "timestamp": 0.4450977238780473 + }, + { + "x": 1.1355416361282653, + "y": 3.7985302060221593, + "heading": -0.6929936795382847, + "angularVelocity": -1.9719891757870285, + "velocityX": 2.7652061164085326, + "velocityY": -1.1969085646365696, + "timestamp": 0.4945530265311637 + }, + { + "x": 1.284369196894048, + "y": 3.7333344256591525, + "heading": -0.760822351237353, + "angularVelocity": -1.3715146417124207, + "velocityX": 3.009334748382224, + "velocityY": -1.3182768452615752, + "timestamp": 0.54400832918428 + }, + { + "x": 1.444068666194049, + "y": 3.662157825629303, + "heading": -0.7899998870158123, + "angularVelocity": -0.589977903544803, + "velocityX": 3.229167768320948, + "velocityY": -1.4392106854362665, + "timestamp": 0.5934636318373964 + }, + { + "x": 1.6133635999437081, + "y": 3.589517549083241, + "heading": -0.7899999435088091, + "angularVelocity": -0.0000011423041352586315, + "velocityX": 3.42319073319818, + "velocityY": -1.4688066324367013, + "timestamp": 0.6429189344905127 + }, + { + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "angularVelocity": -0.000001142267622119268, + "velocityX": 3.4231940186144136, + "velocityY": -1.4687989754737478, + "timestamp": 0.6923742371436291 + }, + { + "x": 2.023972333671043, + "y": 3.4134441767348855, + "heading": -0.79, + "angularVelocity": -1.7456316689050396e-17, + "velocityX": 3.4237473592821543, + "velocityY": -1.4675096226857507, + "timestamp": 0.7628565514540566 + }, + { + "x": 2.250495583067251, + "y": 3.3163502598603043, + "heading": -0.79, + "angularVelocity": -2.7198862157697797e-16, + "velocityX": 3.2139019782824447, + "velocityY": -1.37756425600538, + "timestamp": 0.8333388657644841 + }, + { + "x": 2.4487034476982825, + "y": 3.2313930734184306, + "heading": -0.79, + "angularVelocity": -2.2318862582403906e-16, + "velocityX": 2.8121645347520525, + "velocityY": -1.2053688542021173, + "timestamp": 0.9038211800749116 + }, + { + "x": 2.6185959113515174, + "y": 3.1585726243584236, + "heading": -0.79, + "angularVelocity": -1.6717248939922967e-16, + "velocityX": 2.410426861197717, + "velocityY": -1.0331733538044932, + "timestamp": 0.9743034943853391 + }, + { + "x": 2.760172968622745, + "y": 3.0978889149966715, + "heading": -0.79, + "angularVelocity": -1.1384117019167077e-16, + "velocityX": 2.0086891109686777, + "velocityY": -0.8609778205420586, + "timestamp": 1.0447858086957666 + }, + { + "x": 2.8734346168098592, + "y": 3.049341946491368, + "heading": -0.79, + "angularVelocity": -1.4459082642914294e-16, + "velocityX": 1.6069513224022822, + "velocityY": -0.6887822708472163, + "timestamp": 1.1152681230061943 + }, + { + "x": 2.958380854291598, + "y": 3.0129317195374297, + "heading": -0.79, + "angularVelocity": -1.2018162111442988e-16, + "velocityX": 1.2052135108334716, + "velocityY": -0.5165867112929291, + "timestamp": 1.1857504373166219 + }, + { + "x": 3.015011679987118, + "y": 2.9886582345981343, + "heading": -0.79, + "angularVelocity": -6.358045799066146e-17, + "velocityX": 0.8034756839297176, + "velocityY": -0.3443911451656786, + "timestamp": 1.2562327516270495 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -1.168085544952961e-16, + "velocityX": 0.40173784607243274, + "velocityY": -0.17219557434345445, + "timestamp": 1.3267150659374771 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 1.4132283846579156e-28, + "velocityX": -7.2110997365083e-27, + "velocityY": -2.0111921052200978e-27, + "timestamp": 1.3971973802479047 + }, + { + "x": 3.065434104608151, + "y": 2.9635744703134126, + "heading": -0.7762472311002773, + "angularVelocity": 0.2130317791406229, + "velocityX": 0.34243984045734815, + "velocityY": -0.2005506735052959, + "timestamp": 1.4617547385688177 + }, + { + "x": 3.109663922209824, + "y": 2.9376745027807694, + "heading": -0.7492642309726533, + "angularVelocity": 0.41796939697396296, + "velocityX": 0.6851243413927804, + "velocityY": -0.40119311270289487, + "timestamp": 1.5263120968897306 + }, + { + "x": 3.176036274163731, + "y": 2.8988141772022082, + "heading": -0.7097339632553362, + "angularVelocity": 0.612327839079368, + "velocityX": 1.028114434670201, + "velocityY": -0.6019503676929793, + "timestamp": 1.5908694552106435 + }, + { + "x": 3.2645764628811422, + "y": 2.8469841623353034, + "heading": -0.6585853448584378, + "angularVelocity": 0.7922972644363798, + "velocityX": 1.3714964648534713, + "velocityY": -0.8028521645706009, + "timestamp": 1.6554268135315564 + }, + { + "x": 3.3753176768637694, + "y": 2.782172830859228, + "heading": -0.5971442411061407, + "angularVelocity": 0.9517289020234527, + "velocityX": 1.7153925882799501, + "velocityY": -1.0039340698220625, + "timestamp": 1.7199841718524693 + }, + { + "x": 3.5083041287580397, + "y": 2.7043667248342107, + "heading": -0.5274363257086371, + "angularVelocity": 1.0797826492680733, + "velocityX": 2.0599735700645976, + "velocityY": -1.2052244399196936, + "timestamp": 1.7845415301733822 + }, + { + "x": 3.663593064192348, + "y": 2.6135556042232486, + "heading": -0.45293092753819847, + "angularVelocity": 1.1540961419157545, + "velocityX": 2.405441292414274, + "velocityY": -1.4066734292246417, + "timestamp": 1.8490988884942952 + }, + { + "x": 3.8412236037881105, + "y": 2.50976866496977, + "heading": -0.3811103115245424, + "angularVelocity": 1.1125085951726474, + "velocityX": 2.751514997140456, + "velocityY": -1.6076701704173844, + "timestamp": 1.913656246815208 + }, + { + "x": 4.040074211349249, + "y": 2.394026373110605, + "heading": -0.3455558271366132, + "angularVelocity": 0.5507425537951661, + "velocityX": 3.0802159929261363, + "velocityY": -1.7928597896433947, + "timestamp": 1.978213605136121 + }, + { + "x": 4.2474603616320605, + "y": 2.2722898239651945, + "heading": -0.3455557994031658, + "angularVelocity": 4.2959390119270946e-7, + "velocityX": 3.2124324116842065, + "velocityY": -1.8857114403637907, + "timestamp": 2.042770963457034 + }, + { + "x": 4.454846495549661, + "y": 2.150553246940426, + "heading": -0.34555577166993406, + "angularVelocity": 4.2959055996150303e-7, + "velocityX": 3.21243215818542, + "velocityY": -1.8857118722178698, + "timestamp": 2.1073283217779473 + }, + { + "x": 4.662232629466809, + "y": 2.0288166699148875, + "heading": -0.34555574393670246, + "angularVelocity": 4.295905587004531e-7, + "velocityX": 3.2124321581784225, + "velocityY": -1.885711872229791, + "timestamp": 2.1718856800988604 + }, + { + "x": 4.86961876338396, + "y": 1.9070800928893532, + "heading": -0.34555571620347075, + "angularVelocity": 4.295905595925436e-7, + "velocityX": 3.2124321581784563, + "velocityY": -1.8857118722297332, + "timestamp": 2.2364430384197735 + }, + { + "x": 5.0770048973810855, + "y": 1.7853435160000617, + "heading": -0.3455556884702391, + "angularVelocity": 4.2959055900802054e-7, + "velocityX": 3.2124321594172818, + "velocityY": -1.8857118701193136, + "timestamp": 2.3010003967406867 + }, + { + "x": 5.284393928450512, + "y": 1.6636118745954684, + "heading": -0.3455556607369369, + "angularVelocity": 4.295916530112558e-7, + "velocityX": 3.212477035359784, + "velocityY": -1.885635418962908, + "timestamp": 2.3655577550616 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 4.37946601168607e-7, + "velocityX": 3.386285847111441, + "velocityY": -1.5519979873442957, + "timestamp": 2.430115113382513 + }, + { + "x": 5.67114193645051, + "y": 1.502092704427615, + "heading": -0.3455556386074096, + "angularVelocity": -1.278584041845331e-7, + "velocityX": 3.49949220171166, + "velocityY": -1.2763944237939575, + "timestamp": 2.478161609702393 + }, + { + "x": 5.843643430464205, + "y": 1.4543994299893364, + "heading": -0.3455556446255623, + "angularVelocity": -1.252568492070864e-7, + "velocityX": 3.590303294234579, + "velocityY": -0.9926483321645406, + "timestamp": 2.526208106022273 + }, + { + "x": 6.017301477098486, + "y": 1.4111062813327742, + "heading": -0.3455556506300644, + "angularVelocity": -1.2497273485529655e-7, + "velocityX": 3.6143748230487924, + "velocityY": -0.9010677567065171, + "timestamp": 2.5742546023421533 + }, + { + "x": 6.190959602710417, + "y": 1.3678134494736272, + "heading": -0.3455556566345664, + "angularVelocity": -1.249727336927334e-7, + "velocityX": 3.6143764668242215, + "velocityY": -0.9010611631473742, + "timestamp": 2.6223010986620334 + }, + { + "x": 6.364617728327444, + "y": 1.32452061763492, + "heading": -0.34555566263906834, + "angularVelocity": -1.2497273274023636e-7, + "velocityX": 3.6143764669302776, + "velocityY": -0.9010611627219542, + "timestamp": 2.6703475949819135 + }, + { + "x": 6.53827585394447, + "y": 1.2812277857962142, + "heading": -0.3455556686435703, + "angularVelocity": -1.2497273262525817e-7, + "velocityX": 3.6143764669302847, + "velocityY": -0.9010611627219266, + "timestamp": 2.7183940913017937 + }, + { + "x": 6.711933979561497, + "y": 1.2379349539575075, + "heading": -0.3455556746480723, + "angularVelocity": -1.249727344246102e-7, + "velocityX": 3.614376466930281, + "velocityY": -0.9010611627219414, + "timestamp": 2.7664405876216738 + }, + { + "x": 6.885592105175778, + "y": 1.1946421221077876, + "heading": -0.3455556806525743, + "angularVelocity": -1.2497273402138388e-7, + "velocityX": 3.6143764668731366, + "velocityY": -0.9010611629511635, + "timestamp": 2.814487083941554 + }, + { + "x": 7.059250188235952, + "y": 1.1513491195635046, + "heading": -0.34555568665707626, + "angularVelocity": -1.2497273336829244e-7, + "velocityX": 3.614375581187171, + "velocityY": -0.901064715646495, + "timestamp": 2.862533580261434 + }, + { + "x": 7.232252247020901, + "y": 1.105504804591487, + "heading": -0.3455556930363148, + "angularVelocity": -1.3277218955785758e-7, + "velocityX": 3.600721635000194, + "velocityY": -0.954165620460628, + "timestamp": 2.910580076581314 + }, + { + "x": 7.3915823056650956, + "y": 1.0554297346464303, + "heading": -0.34668228326838396, + "angularVelocity": -0.02344791646343344, + "velocityX": 3.3161639421825697, + "velocityY": -1.0422210521173303, + "timestamp": 2.9586265729011942 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.023449177160012234, + "velocityX": 3.0262995808418314, + "velocityY": -0.9742709781335815, + "timestamp": 3.0066730692210744 + }, + { + "x": 7.706721170015621, + "y": 0.9519776263380658, + "heading": -0.3399354575219549, + "angularVelocity": 0.0873776196257354, + "velocityX": 2.6389050045733367, + "velocityY": -0.8806177429462266, + "timestamp": 3.0709936007986016 + }, + { + "x": 7.851727537144501, + "y": 0.9021680166584882, + "heading": -0.3343317435174853, + "angularVelocity": 0.0871216991998166, + "velocityX": 2.254433593324705, + "velocityY": -0.7743967355049113, + "timestamp": 3.135314132376129 + }, + { + "x": 7.9720996028837305, + "y": 0.8595135021783036, + "heading": -0.3304150780634631, + "angularVelocity": 0.06089292731204182, + "velocityX": 1.8714407792812162, + "velocityY": -0.6631555031346718, + "timestamp": 3.199634663953656 + }, + { + "x": 8.067893377913451, + "y": 0.8241876261341342, + "heading": -0.3290917427033041, + "angularVelocity": 0.020574073747570358, + "velocityX": 1.4893187708540474, + "velocityY": -0.5492161706031644, + "timestamp": 3.2639551955311834 + }, + { + "x": 8.139145592381213, + "y": 0.7962986910106811, + "heading": -0.33093102223192583, + "angularVelocity": -0.028595527485726017, + "velocityX": 1.107767810996388, + "velocityY": -0.4335930447004768, + "timestamp": 3.3282757271087107 + }, + { + "x": 8.185882143295247, + "y": 0.7759207035915239, + "heading": -0.33632389468863655, + "angularVelocity": -0.08384371715290594, + "velocityX": 0.726619475426766, + "velocityY": -0.3168193253284159, + "timestamp": 3.392596258686238 + }, + { + "x": 8.208122253417969, + "y": 0.763107419013977, + "heading": -0.34555563246426124, + "angularVelocity": -0.14352707524653271, + "velocityX": 0.34576999874318054, + "velocityY": -0.19920986756931033, + "timestamp": 3.456916790263765 + }, + { + "x": 8.204294589663224, + "y": 0.758059174501978, + "heading": -0.3598356424553756, + "angularVelocity": -0.21015280737857786, + "velocityX": -0.056330092504229386, + "velocityY": -0.07429285814157252, + "timestamp": 3.5248673932999157 + }, + { + "x": 8.173143956099947, + "y": 0.7614990375664682, + "heading": -0.3786432717750789, + "angularVelocity": -0.2767838470792884, + "velocityX": -0.4584305682571252, + "velocityY": 0.05062299539358275, + "timestamp": 3.5928179963360662 + }, + { + "x": 8.114670323431806, + "y": 0.7734269104252286, + "heading": -0.4019794984677993, + "angularVelocity": -0.34342928024207936, + "velocityX": -0.8605314751516402, + "velocityY": 0.17553740990958688, + "timestamp": 3.6607685993722168 + }, + { + "x": 8.028873660690506, + "y": 0.7938426763627194, + "heading": -0.42984618674257646, + "angularVelocity": -0.41010214817301655, + "velocityX": -1.2626328377933065, + "velocityY": 0.3004501067728478, + "timestamp": 3.7287192024083673 + }, + { + "x": 7.915753937180706, + "y": 0.8227462003684911, + "heading": -0.46224640751329055, + "angularVelocity": -0.47682020943179554, + "velocityX": -1.6647346521651787, + "velocityY": 0.4253608167449919, + "timestamp": 3.796669805444518 + }, + { + "x": 7.7753111250087, + "y": 0.8601373303576515, + "heading": -0.4991848061620223, + "angularVelocity": -0.5436066347943985, + "velocityX": -2.0668368770368284, + "velocityY": 0.5502692885487409, + "timestamp": 3.8646204084806683 + }, + { + "x": 7.607545202545143, + "y": 0.9060158995198092, + "heading": -0.5406680081845634, + "angularVelocity": -0.6104905647485055, + "velocityX": -2.468939420218282, + "velocityY": 0.6751753054752103, + "timestamp": 3.932571011516819 + }, + { + "x": 7.412456160128696, + "y": 0.9603817316039739, + "heading": -0.5867050376005867, + "angularVelocity": -0.6775072973455604, + "velocityX": -2.8710421055815774, + "velocityY": 0.800078728591143, + "timestamp": 4.000521614552969 + }, + { + "x": 7.190044017003328, + "y": 1.023234660815804, + "heading": -0.6373076187752672, + "angularVelocity": -0.7446965724168686, + "velocityX": -3.273144507739573, + "velocityY": 0.9249797117825663, + "timestamp": 4.06847221758912 + }, + { + "x": 6.9483240964505395, + "y": 1.0983293400926897, + "heading": -0.6373076247970214, + "angularVelocity": -8.861958477756183e-8, + "velocityX": -3.5572888208834765, + "velocityY": 1.1051363184655567, + "timestamp": 4.1364228206252704 + }, + { + "x": 6.70660438555466, + "y": 1.1734246942257325, + "heading": -0.6373076308180073, + "angularVelocity": -8.86082771208124e-8, + "velocityX": -3.5572857354522953, + "velocityY": 1.1051462500353575, + "timestamp": 4.204373423661421 + }, + { + "x": 6.464884674663689, + "y": 1.2485200483745762, + "heading": -0.6373076368389932, + "angularVelocity": -8.860827701986505e-8, + "velocityX": -3.557285735380054, + "velocityY": 1.105146250267891, + "timestamp": 4.2723240266975715 + }, + { + "x": 6.223164963772718, + "y": 1.3236154025234206, + "heading": -0.637307642859979, + "angularVelocity": -8.860827626589147e-8, + "velocityX": -3.5572857353800513, + "velocityY": 1.1051462502679001, + "timestamp": 4.340274629733722 + }, + { + "x": 5.98144525288525, + "y": 1.39871075668354, + "heading": -0.6373076488809649, + "angularVelocity": -8.860827719404325e-8, + "velocityX": -3.5572857353285006, + "velocityY": 1.1051462504338314, + "timestamp": 4.4082252327698725 + }, + { + "x": 5.739725691606158, + "y": 1.473806592406946, + "heading": -0.6373076549019507, + "angularVelocity": -8.860827643379068e-8, + "velocityX": -3.557283533605943, + "velocityY": 1.1051533373950213, + "timestamp": 4.476175835806023 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.6373076609624853, + "angularVelocity": -8.919029793694062e-8, + "velocityX": -3.483737947413037, + "velocityY": 1.3187872955082738, + "timestamp": 4.544126438842174 + }, + { + "x": 5.300661081332848, + "y": 1.6644259130191759, + "heading": -0.6373076662501911, + "angularVelocity": -8.709485563097327e-8, + "velocityX": -3.332823841916901, + "velocityY": 1.6637052194159954, + "timestamp": 4.6048384732983045 + }, + { + "x": 5.105140298152244, + "y": 1.7780767678650242, + "heading": -0.6373076714738495, + "angularVelocity": -8.603991577037157e-8, + "velocityX": -3.2204617244688527, + "velocityY": 1.8719658443989302, + "timestamp": 4.6655505077544355 + }, + { + "x": 4.909619881391782, + "y": 1.8917282530843815, + "heading": -0.6373076766975064, + "angularVelocity": -8.60398930609077e-8, + "velocityX": -3.220455689089781, + "velocityY": 1.8719762274064293, + "timestamp": 4.726262542210566 + }, + { + "x": 4.714099464642575, + "y": 2.0053797383231027, + "heading": -0.6373076819211634, + "angularVelocity": -8.603989205375444e-8, + "velocityX": -3.2204556889043876, + "velocityY": 1.8719762277253713, + "timestamp": 4.786974576666697 + }, + { + "x": 4.518579047893369, + "y": 2.1190312235618243, + "heading": -0.6373076871448202, + "angularVelocity": -8.60398922147823e-8, + "velocityX": -3.220455688904382, + "velocityY": 1.8719762277253813, + "timestamp": 4.847686611122828 + }, + { + "x": 4.323058631144163, + "y": 2.232682708800546, + "heading": -0.6373076923684772, + "angularVelocity": -8.603989163870152e-8, + "velocityX": -3.2204556889043805, + "velocityY": 1.8719762277253815, + "timestamp": 4.908398645578959 + }, + { + "x": 4.12753821439536, + "y": 2.3463341940399602, + "heading": -0.637307697592134, + "angularVelocity": -8.60398917504913e-8, + "velocityX": -3.2204556888977502, + "velocityY": 1.8719762277367886, + "timestamp": 4.96911068003509 + }, + { + "x": 3.9320178107531194, + "y": 2.4599857018249596, + "heading": -0.637307702817472, + "angularVelocity": -8.606758041499758e-8, + "velocityX": -3.2204554730169304, + "velocityY": 1.8719765990896107, + "timestamp": 5.029822714491221 + }, + { + "x": 3.754279851907353, + "y": 2.5632932295685515, + "heading": -0.6678287625734391, + "angularVelocity": -0.5027184483172089, + "velocityX": -2.9275572864255675, + "velocityY": 1.7015988455837243, + "timestamp": 5.090534748947352 + }, + { + "x": 3.5962904713608705, + "y": 2.6551219777359605, + "heading": -0.6949668036285394, + "angularVelocity": -0.4469960741425893, + "velocityX": -2.6022745236884077, + "velocityY": 1.5125295831382852, + "timestamp": 5.151246783403483 + }, + { + "x": 3.4580497056890147, + "y": 2.735472006369499, + "heading": -0.7187186173095553, + "angularVelocity": -0.3912208492729469, + "velocityX": -2.276991158511501, + "velocityY": 1.3234613096617127, + "timestamp": 5.211958817859614 + }, + { + "x": 3.3395575797280896, + "y": 2.804343370444312, + "heading": -0.7390815132457978, + "angularVelocity": -0.33540131077235125, + "velocityX": -1.9517073842508859, + "velocityY": 1.1343939416917022, + "timestamp": 5.272670852315745 + }, + { + "x": 3.2408141126366585, + "y": 2.861736116018487, + "heading": -0.7560532042537318, + "angularVelocity": -0.2795440996166463, + "velocityX": -1.6264232944257633, + "velocityY": 0.9453273323536148, + "timestamp": 5.333382886771876 + }, + { + "x": 3.16181931959339, + "y": 2.907650279470937, + "heading": -0.7696318707216167, + "angularVelocity": -0.22365691727389694, + "velocityX": -1.3011389545897718, + "velocityY": 0.756261322219841, + "timestamp": 5.394094921228007 + }, + { + "x": 3.1025732126750043, + "y": 2.942085887277494, + "heading": -0.7798162350397829, + "angularVelocity": -0.16774869116806188, + "velocityX": -0.9758544158357173, + "velocityY": 0.5671957481747656, + "timestamp": 5.454806955684138 + }, + { + "x": 3.0630758014096395, + "y": 2.9650429559458633, + "heading": -0.7866056220675054, + "angularVelocity": -0.11182934468500333, + "velocityX": -0.6505697201418033, + "velocityY": 0.37813044603137397, + "timestamp": 5.515518990140269 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -0.05590947433901855, + "velocityX": -0.3252849037618716, + "velocityY": 0.18906525141774544, + "timestamp": 5.5762310245963995 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 6.3035161532873835e-28, + "velocityX": -3.172311091955017e-27, + "velocityY": -4.980348575504513e-27, + "timestamp": 5.6369430590525305 + }, + { + "x": 3.0599792847904204, + "y": 2.966059333110192, + "heading": -0.7574164237892916, + "angularVelocity": 0.5582463314772692, + "velocityX": 0.2852978705745262, + "velocityY": -0.17924557403559396, + "timestamp": 5.695310798119411 + }, + { + "x": 3.0934694098196505, + "y": 2.9451270864719556, + "heading": -0.6941025427131003, + "angularVelocity": 1.0847410245519962, + "velocityX": 0.5737780075883294, + "velocityY": -0.35862699108922125, + "timestamp": 5.753678537186291 + }, + { + "x": 3.1440142929618213, + "y": 2.9136974289532884, + "heading": -0.6025701359612278, + "angularVelocity": 1.5682020276130737, + "velocityX": 0.8659729492734756, + "velocityY": -0.5384765286634409, + "timestamp": 5.812046276253171 + }, + { + "x": 3.2118603630472147, + "y": 2.8716921302721974, + "heading": -0.4864204343113424, + "angularVelocity": 1.9899640367566072, + "velocityX": 1.162389895000939, + "velocityY": -0.7196663662603013, + "timestamp": 5.870414015320051 + }, + { + "x": 3.297261323571818, + "y": 2.8189697938755005, + "heading": -0.35051552340624004, + "angularVelocity": 2.3284251382322116, + "velocityX": 1.463153479814372, + "velocityY": -0.9032787159407621, + "timestamp": 5.9287817543869314 + }, + { + "x": 3.400457830317495, + "y": 2.755367474678837, + "heading": -0.2010210241745889, + "angularVelocity": 2.561252185224339, + "velocityX": 1.768040160463131, + "velocityY": -1.0896827633461208, + "timestamp": 5.987149493453812 + }, + { + "x": 3.521660389826238, + "y": 2.6807799872839175, + "heading": -0.04666004939322163, + "angularVelocity": 2.644628304078967, + "velocityX": 2.076533397496587, + "velocityY": -1.277888926097578, + "timestamp": 6.045517232520692 + }, + { + "x": 3.6607588635338297, + "y": 2.5953087461465243, + "heading": 0.09647781625085058, + "angularVelocity": 2.4523455582211025, + "velocityX": 2.383139657820331, + "velocityY": -1.4643575801258442, + "timestamp": 6.103884971587572 + }, + { + "x": 3.8159145666935697, + "y": 2.50056098976987, + "heading": 0.19948059626334666, + "angularVelocity": 1.7647210883819062, + "velocityX": 2.6582441883170422, + "velocityY": -1.6232898154250606, + "timestamp": 6.162252710654452 + }, + { + "x": 3.984110357979779, + "y": 2.3943443694149815, + "heading": 0.2530307730268392, + "angularVelocity": 0.9174618996657108, + "velocityX": 2.8816567846406262, + "velocityY": -1.8197830180329746, + "timestamp": 6.220620449721332 + }, + { + "x": 4.165386797993929, + "y": 2.276764205392624, + "heading": 0.25694414020151846, + "angularVelocity": 0.06704674940715359, + "velocityX": 3.105764295691424, + "velocityY": -2.0144717938728, + "timestamp": 6.278988188788213 + }, + { + "x": 4.35049005488602, + "y": 2.162710813518879, + "heading": 0.2569442210228932, + "angularVelocity": 0.0000013846925722227273, + "velocityX": 3.171328200326413, + "velocityY": -1.9540484811833736, + "timestamp": 6.337355927855093 + }, + { + "x": 4.5355934176880135, + "y": 2.0486575935320284, + "heading": 0.2569443018441972, + "angularVelocity": 0.0000013846913602153038, + "velocityX": 3.171330014854514, + "velocityY": -1.9540455362878368, + "timestamp": 6.395723666921973 + }, + { + "x": 4.72069678050449, + "y": 1.9346043735686815, + "heading": 0.25694438266550157, + "angularVelocity": 0.0000013846913664886619, + "velocityX": 3.171330015102631, + "velocityY": -1.9540455358851527, + "timestamp": 6.454091405988853 + }, + { + "x": 4.905800420839664, + "y": 1.8205516040072125, + "heading": 0.2569444634867951, + "angularVelocity": 0.0000013846911811343512, + "velocityX": 3.171334769761663, + "velocityY": -1.9540378192614634, + "timestamp": 6.512459145055733 + }, + { + "x": 5.096209165426906, + "y": 1.7155949376896535, + "heading": 0.2569445446080598, + "angularVelocity": 0.0000013898305123819847, + "velocityX": 3.262225805407059, + "velocityY": -1.7981965379418772, + "timestamp": 6.570826884122614 + }, + { + "x": 5.295905298565615, + "y": 1.6296132789604225, + "heading": 0.2569446291955456, + "angularVelocity": 0.000001449216418667715, + "velocityX": 3.4213443304680435, + "velocityY": -1.473102438158684, + "timestamp": 6.629194623189494 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.256944722079518, + "angularVelocity": 0.0000015913580667498199, + "velocityX": 3.548163797412206, + "velocityY": -1.1340904342967109, + "timestamp": 6.687562362256374 + }, + { + "x": 5.694707875531734, + "y": 1.520245371460122, + "heading": 0.25694480525285635, + "angularVelocity": 0.0000015766499744480022, + "velocityX": 3.6339835774201763, + "velocityY": -0.8184074026058323, + "timestamp": 6.740315566608581 + }, + { + "x": 5.889460824583175, + "y": 1.4940579532692961, + "heading": 0.2569448839753191, + "angularVelocity": 0.0000014922783134293427, + "velocityX": 3.691774773550607, + "velocityY": -0.49641379158667054, + "timestamp": 6.793068770960788 + }, + { + "x": 6.085760337593024, + "y": 1.4850583071038423, + "heading": 0.25694496145000295, + "angularVelocity": 0.0000014686251728049365, + "velocityX": 3.721091740688451, + "velocityY": -0.17059904276842675, + "timestamp": 6.845821975312996 + }, + { + "x": 6.282233415675307, + "y": 1.4886391007137698, + "heading": 0.2569450396472057, + "angularVelocity": 0.0000014823213810882118, + "velocityX": 3.7243818739526797, + "velocityY": 0.06787821998490172, + "timestamp": 6.898575179665203 + }, + { + "x": 6.478706465105997, + "y": 1.4922214660505555, + "heading": 0.2569451178445693, + "angularVelocity": 0.000001482324430715833, + "velocityX": 3.7243813308274873, + "velocityY": 0.06790801394485714, + "timestamp": 6.95132838401741 + }, + { + "x": 6.6750148942106335, + "y": 1.5010244818843792, + "heading": 0.2569451969790633, + "angularVelocity": 0.0000015000888565600604, + "velocityX": 3.7212607559150324, + "velocityY": 0.16687167996564178, + "timestamp": 7.004081588369617 + }, + { + "x": 6.869794082781354, + "y": 1.5270160132207926, + "heading": 0.25694528055290106, + "angularVelocity": 0.000001584241920357872, + "velocityX": 3.692272174980594, + "velocityY": 0.4927005222826037, + "timestamp": 7.0568347927218245 + }, + { + "x": 7.061541469504156, + "y": 1.5699966670376497, + "heading": 0.25694631305508775, + "angularVelocity": 0.000019572312228907657, + "velocityX": 3.6348007495923222, + "velocityY": 0.814749631698122, + "timestamp": 7.109587997074032 + }, + { + "x": 7.242025004547745, + "y": 1.623544327324134, + "heading": 0.2892009263238574, + "angularVelocity": 0.611424721300743, + "velocityX": 3.421280986811508, + "velocityY": 1.0150598611787165, + "timestamp": 7.162341201426239 + }, + { + "x": 7.4101475255884415, + "y": 1.6859098332100053, + "heading": 0.3552181316610267, + "angularVelocity": 1.2514349819662982, + "velocityX": 3.186963201670605, + "velocityY": 1.1822126570641514, + "timestamp": 7.215094405778446 + }, + { + "x": 7.5650279020019084, + "y": 1.7562974315324296, + "heading": 0.45280543116466554, + "angularVelocity": 1.8498838260534038, + "velocityX": 2.935942533071662, + "velocityY": 1.3342810012540722, + "timestamp": 7.267847610130653 + }, + { + "x": 7.70378035758304, + "y": 1.8320280138991303, + "heading": 0.5628377984659659, + "angularVelocity": 2.0857949512728657, + "velocityX": 2.6302185295655014, + "velocityY": 1.4355636457850884, + "timestamp": 7.3206008144828605 + }, + { + "x": 7.825662907370156, + "y": 1.911322826381737, + "heading": 0.6698617562914626, + "angularVelocity": 2.0287669562392776, + "velocityX": 2.3104293148405795, + "velocityY": 1.5031278849564027, + "timestamp": 7.373354018835068 + }, + { + "x": 7.930693514616294, + "y": 1.9932244953323592, + "heading": 0.7659443680912281, + "angularVelocity": 1.8213606733397365, + "velocityX": 1.9909806150333516, + "velocityY": 1.5525439630890379, + "timestamp": 7.426107223187275 + }, + { + "x": 8.019011714129077, + "y": 2.0771819587981266, + "heading": 0.8464048765673222, + "angularVelocity": 1.5252250448882378, + "velocityX": 1.6741769641730917, + "velocityY": 1.5915140036844804, + "timestamp": 7.478860427539482 + }, + { + "x": 8.090768492192002, + "y": 2.1628615391483916, + "heading": 0.9080474489075738, + "angularVelocity": 1.1685085881929373, + "velocityX": 1.360235438663432, + "velocityY": 1.6241587862269713, + "timestamp": 7.531613631891689 + }, + { + "x": 8.146101418716428, + "y": 2.2500495157538962, + "heading": 0.9485346126333066, + "angularVelocity": 0.7674825486508784, + "velocityX": 1.048901715145006, + "velocityY": 1.6527522389615, + "timestamp": 7.5843668362438965 + }, + { + "x": 8.185122291410421, + "y": 2.338598291571523, + "heading": 0.9662103754330295, + "angularVelocity": 0.3350651968307046, + "velocityX": 0.7396872507207403, + "velocityY": 1.6785478134452219, + "timestamp": 7.637120040596104 + }, + { + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "angularVelocity": -0.11638132008356272, + "velocityX": 0.4319241942959219, + "velocityY": 1.7021818924733116, + "timestamp": 7.689873244948311 + }, + { + "x": 8.211423054828662, + "y": 2.540472392253658, + "heading": 0.9162466453352144, + "angularVelocity": -0.6758725051077806, + "velocityX": 0.054215367725083355, + "velocityY": 1.7285138763859442, + "timestamp": 7.754714235231942 + }, + { + "x": 8.190202888383633, + "y": 2.6538474230997973, + "heading": 0.8391734249667565, + "angularVelocity": -1.1886496494165113, + "velocityX": -0.3272646878495706, + "velocityY": 1.7485086262595175, + "timestamp": 7.819555225515574 + }, + { + "x": 8.144008306424501, + "y": 2.768019013990823, + "heading": 0.7319233890258415, + "angularVelocity": -1.6540468532601917, + "velocityX": -0.7124286929774685, + "velocityY": 1.760793448582603, + "timestamp": 7.884396215799205 + }, + { + "x": 8.072604152106418, + "y": 2.8824539716664033, + "heading": 0.5977397407756817, + "angularVelocity": -2.0694262635904495, + "velocityX": -1.1012193676521893, + "velocityY": 1.764855181498803, + "timestamp": 7.949237206082836 + }, + { + "x": 7.975691382967155, + "y": 2.9965380091631473, + "heading": 0.44171220804424666, + "angularVelocity": -2.406310145001336, + "velocityX": -1.4946219777850651, + "velocityY": 1.7594431700951976, + "timestamp": 8.014078196366468 + }, + { + "x": 7.852920805151493, + "y": 3.1093104190449847, + "heading": 0.2733111632783997, + "angularVelocity": -2.5971386931201565, + "velocityX": -1.8934099753663647, + "velocityY": 1.7392147989804203, + "timestamp": 8.078919186650099 + }, + { + "x": 7.704278478364254, + "y": 3.219028342207882, + "heading": 0.10912005253192102, + "angularVelocity": -2.5322116461865276, + "velocityX": -2.292412964963042, + "velocityY": 1.6921074567640455, + "timestamp": 8.14376017693373 + }, + { + "x": 7.532421373794059, + "y": 3.323057914868166, + "heading": -0.010926874391562237, + "angularVelocity": -1.851404896784686, + "velocityX": -2.650439233245006, + "velocityY": 1.6043797635605452, + "timestamp": 8.208601167217362 + }, + { + "x": 7.337707687800787, + "y": 3.422034456346363, + "heading": -0.08211008235538154, + "angularVelocity": -1.0978118571669786, + "velocityX": -3.0029412743627657, + "velocityY": 1.5264501829051038, + "timestamp": 8.273442157500993 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": -0.09725138680843892, + "angularVelocity": -0.2335143924672555, + "velocityX": -3.3462025777705335, + "velocityY": 1.4627042932775656, + "timestamp": 8.338283147784624 + }, + { + "x": 7.010606209661033, + "y": 3.566708155420985, + "heading": -0.09725146950362613, + "angularVelocity": -0.000002548325484883459, + "velocityX": -3.39376555274088, + "velocityY": 1.5355711508400656, + "timestamp": 8.370733942512778 + }, + { + "x": 6.900475999605217, + "y": 3.616539055790813, + "heading": -0.09725155219704654, + "angularVelocity": -0.0000025482710392560423, + "velocityX": -3.3937600289421765, + "velocityY": 1.5355833589676455, + "timestamp": 8.403184737240933 + }, + { + "x": 6.790345789587328, + "y": 3.6663699562444614, + "heading": -0.09725163489046874, + "angularVelocity": -0.0000025482710945290222, + "velocityX": -3.3937600277734288, + "velocityY": 1.5355833615506584, + "timestamp": 8.435635531969087 + }, + { + "x": 6.680215579569448, + "y": 3.7162008566981277, + "heading": -0.0972517175838927, + "angularVelocity": -0.0000025482711487845157, + "velocityX": -3.393760027773178, + "velocityY": 1.5355833615512018, + "timestamp": 8.468086326697241 + }, + { + "x": 6.570085369551568, + "y": 3.7660317571517936, + "heading": -0.09725180027731849, + "angularVelocity": -0.0000025482712051303815, + "velocityX": -3.3937600277731743, + "velocityY": 1.5355833615511987, + "timestamp": 8.500537121425396 + }, + { + "x": 6.4599551595336875, + "y": 3.8158626576054595, + "heading": -0.09725188297074605, + "angularVelocity": -0.0000025482712597994537, + "velocityX": -3.3937600277731703, + "velocityY": 1.5355833615511956, + "timestamp": 8.53298791615355 + }, + { + "x": 6.349824949515808, + "y": 3.8656935580591254, + "heading": -0.0972519656641754, + "angularVelocity": -0.0000025482713151989977, + "velocityX": -3.393760027773166, + "velocityY": 1.5355833615511918, + "timestamp": 8.565438710881704 + }, + { + "x": 6.239694739497929, + "y": 3.9155244585127913, + "heading": -0.09725204835760656, + "angularVelocity": -0.00000254827137059863, + "velocityX": -3.3937600277731623, + "velocityY": 1.5355833615511885, + "timestamp": 8.597889505609858 + }, + { + "x": 6.129564529480049, + "y": 3.9653553589664567, + "heading": -0.09725213105103951, + "angularVelocity": -0.000002548271425616177, + "velocityX": -3.3937600277731588, + "velocityY": 1.535583361551185, + "timestamp": 8.630340300338013 + }, + { + "x": 6.019434319462169, + "y": 4.015186259420122, + "heading": -0.09725221374447425, + "angularVelocity": -0.0000025482714812860075, + "velocityX": -3.393760027773155, + "velocityY": 1.5355833615511818, + "timestamp": 8.662791095066167 + }, + { + "x": 5.90930410944429, + "y": 4.065017159873787, + "heading": -0.09725229643791082, + "angularVelocity": -0.0000025482715370955637, + "velocityX": -3.3937600277731512, + "velocityY": 1.5355833615511787, + "timestamp": 8.695241889794321 + }, + { + "x": 5.79917389942641, + "y": 4.114848060327453, + "heading": -0.09725237913134915, + "angularVelocity": -0.0000025482715921357367, + "velocityX": -3.3937600277731477, + "velocityY": 1.5355833615511751, + "timestamp": 8.727692684522475 + }, + { + "x": 5.689043689408531, + "y": 4.164678960781118, + "heading": -0.09725246182478928, + "angularVelocity": -0.0000025482716465928655, + "velocityX": -3.3937600277731437, + "velocityY": 1.5355833615511723, + "timestamp": 8.76014347925063 + }, + { + "x": 5.5789134793906525, + "y": 4.2145098612347835, + "heading": -0.09725254451823122, + "angularVelocity": -0.0000025482717030707046, + "velocityX": -3.39376002777314, + "velocityY": 1.535583361551169, + "timestamp": 8.792594273978784 + }, + { + "x": 5.468783269372774, + "y": 4.264340761688448, + "heading": -0.09725262721167492, + "angularVelocity": -0.000002548271757487181, + "velocityX": -3.3937600277731357, + "velocityY": 1.535583361551166, + "timestamp": 8.825045068706938 + }, + { + "x": 5.358653059354908, + "y": 4.314171662142141, + "heading": -0.09725270990512043, + "angularVelocity": -0.000002548271812724623, + "velocityX": -3.3937600277727378, + "velocityY": 1.5355833615520338, + "timestamp": 8.857495863435092 + }, + { + "x": 5.248522849397515, + "y": 4.364002562729485, + "heading": -0.09725279259856773, + "angularVelocity": -0.000002548271867828041, + "velocityX": -3.393760025909188, + "velocityY": 1.5355833656706142, + "timestamp": 8.889946658163247 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.000002548289809515153, + "velocityX": -3.3937512180408453, + "velocityY": 1.5356028315849852, + "timestamp": 8.922397452891401 + }, + { + "x": 4.9137282295128415, + "y": 4.535640155229253, + "heading": -0.09725287529313524, + "angularVelocity": -7.495790555066974e-12, + "velocityX": -3.1302347498710463, + "velocityY": 1.6971138308958609, + "timestamp": 8.994169928025691 + }, + { + "x": 4.717146626097375, + "y": 4.642220517268037, + "heading": -0.09725287529322811, + "angularVelocity": -1.2941727290342862e-12, + "velocityX": -2.738955331380901, + "velocityY": 1.4849754288028527, + "timestamp": 9.065942403159982 + }, + { + "x": 4.5486481109529855, + "y": 4.733575136181824, + "heading": -0.09725287529316204, + "angularVelocity": 9.207220397125275e-13, + "velocityX": -2.347675969501116, + "velocityY": 1.2728364006237491, + "timestamp": 9.137714878294272 + }, + { + "x": 4.408232682725307, + "y": 4.809703996992033, + "heading": -0.09725287529303236, + "angularVelocity": 1.806653851545192e-12, + "velocityX": -1.9563966264916148, + "velocityY": 1.0606971637493046, + "timestamp": 9.209487353428562 + }, + { + "x": 4.2959003407371545, + "y": 4.870607092209374, + "heading": -0.09725287529288679, + "angularVelocity": 2.028301137290635e-12, + "velocityX": -1.5651172929172836, + "velocityY": 0.8485578225272127, + "timestamp": 9.281259828562852 + }, + { + "x": 4.211651084582215, + "y": 4.916284417340276, + "heading": -0.09725287529275393, + "angularVelocity": 1.850997831523441e-12, + "velocityX": -1.1738379650040627, + "velocityY": 0.6364184186965437, + "timestamp": 9.353032303697143 + }, + { + "x": 4.155484913989614, + "y": 4.946735969389024, + "heading": -0.09725287529265289, + "angularVelocity": 1.4078899342405594e-12, + "velocityX": -0.7825586408649186, + "velocityY": 0.424278973126829, + "timestamp": 9.424804778831433 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 7.752391774433126e-13, + "velocityX": -0.3912793194215449, + "velocityY": 0.2121394977435137, + "timestamp": 9.496577253965723 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -8.979320165657004e-28, + "velocityX": -3.317577006729804e-27, + "velocityY": -2.7701124132227202e-27, + "timestamp": 9.568349729100014 + }, + { + "x": 4.149345287192321, + "y": 4.948730090302555, + "heading": -0.09725287542061174, + "angularVelocity": -1.9914932525911006e-9, + "velocityX": 0.34136955009475556, + "velocityY": -0.20584195701235133, + "timestamp": 9.632630384537388 + }, + { + "x": 4.193331867161711, + "y": 4.922433486987429, + "heading": -0.09725287565526207, + "angularVelocity": -3.6504037180002943e-9, + "velocityX": 0.6842895373437039, + "velocityY": -0.4090904664272751, + "timestamp": 9.696911039974761 + }, + { + "x": 4.259487364434434, + "y": 4.883286547964955, + "heading": -0.09725287596919822, + "angularVelocity": -4.883835550017299e-9, + "velocityX": 1.02916650153289, + "velocityY": -0.6090003089749592, + "timestamp": 9.761191695412135 + }, + { + "x": 4.347975400700828, + "y": 4.831575784808056, + "heading": -0.09725287632620161, + "angularVelocity": -5.55382285919441e-9, + "velocityX": 1.3765888923239984, + "velocityY": -0.8044529540816497, + "timestamp": 9.825472350849509 + }, + { + "x": 4.45901715765165, + "y": 4.76770271955414, + "heading": -0.09725287667606397, + "angularVelocity": -5.442731637489258e-9, + "velocityX": 1.7274521579669766, + "velocityY": -0.9936592092802461, + "timestamp": 9.889753006286883 + }, + { + "x": 4.592927185358519, + "y": 4.69226963680943, + "heading": -0.09725287694462323, + "angularVelocity": -4.177917358984547e-9, + "velocityX": 2.083208809800217, + "velocityY": -1.1734958555019974, + "timestamp": 9.954033661724257 + }, + { + "x": 4.750184246245984, + "y": 4.606276833638444, + "heading": -0.09725287701156489, + "angularVelocity": -1.0413967703177253e-9, + "velocityX": 2.446413463233511, + "velocityY": -1.3377711005881947, + "timestamp": 10.01831431716163 + }, + { + "x": 4.931581310093376, + "y": 4.511690334370361, + "heading": -0.0972528766497097, + "angularVelocity": 5.629301248871626e-9, + "velocityX": 2.821954172886779, + "velocityY": -1.4714613381662662, + "timestamp": 10.082594972599004 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 2.1112299822496666e-8, + "velocityX": 3.2173227507078654, + "velocityY": -1.5223279648179473, + "timestamp": 10.146875628036378 + }, + { + "x": 5.398868955921396, + "y": 4.32613897098341, + "heading": -0.09725286997797288, + "angularVelocity": 7.20303645148975e-8, + "velocityX": 3.5302934161705415, + "velocityY": -1.1885528129676433, + "timestamp": 10.220658738943204 + }, + { + "x": 5.6681314885139065, + "y": 4.271040046607528, + "heading": -0.09725286670603911, + "angularVelocity": 4.4345294143454955e-8, + "velocityX": 3.6493789606207083, + "velocityY": -0.7467687889368465, + "timestamp": 10.29444184985003 + }, + { + "x": 5.942117714678701, + "y": 4.249366302589977, + "heading": -0.0972528635577222, + "angularVelocity": 4.26698858409276e-8, + "velocityX": 3.713400299843557, + "velocityY": -0.29374939266142885, + "timestamp": 10.368224960756855 + }, + { + "x": 6.216382330790907, + "y": 4.231558335942029, + "heading": -0.09725286041527656, + "angularVelocity": 4.259031080949294e-8, + "velocityX": 3.7171733848218165, + "velocityY": -0.24135559519084493, + "timestamp": 10.44200807166368 + }, + { + "x": 6.4906469514050675, + "y": 4.21375043862986, + "heading": -0.09725285727283085, + "angularVelocity": 4.259031210936323e-8, + "velocityX": 3.717173445837876, + "velocityY": -0.241354655466577, + "timestamp": 10.515791182570506 + }, + { + "x": 6.764911572019293, + "y": 4.195942541318692, + "heading": -0.09725285413038519, + "angularVelocity": 4.259031138372869e-8, + "velocityX": 3.717173445838756, + "velocityY": -0.24135465545301274, + "timestamp": 10.589574293477332 + }, + { + "x": 7.039176191804645, + "y": 4.178134631242245, + "heading": -0.09725285098790416, + "angularVelocity": 4.2590790558228534e-8, + "velocityX": 3.717173434604845, + "velocityY": -0.24135482846385797, + "timestamp": 10.663357404384158 + }, + { + "x": 7.303151625715878, + "y": 4.160355738175747, + "heading": -0.07493794888540253, + "angularVelocity": 0.3024391602392781, + "velocityX": 3.5777216583424782, + "velocityY": -0.24096155404655678, + "timestamp": 10.737140515290983 + }, + { + "x": 7.533445930864736, + "y": 4.144763761571347, + "heading": -0.05552477724704528, + "angularVelocity": 0.26311131910489216, + "velocityX": 3.1212333326481616, + "velocityY": -0.21132175660214023, + "timestamp": 10.810923626197809 + }, + { + "x": 7.730059092320318, + "y": 4.131358730965977, + "heading": -0.03901370974052792, + "angularVelocity": 0.22377841356361258, + "velocityX": 2.664744804591801, + "velocityY": -0.1816815588366564, + "timestamp": 10.884706737104635 + }, + { + "x": 7.892991106532594, + "y": 4.120140669647355, + "heading": -0.02540508118930727, + "angularVelocity": 0.18444097008061852, + "velocityX": 2.20825622842103, + "velocityY": -0.15204104544723035, + "timestamp": 10.95848984801146 + }, + { + "x": 8.02224197249394, + "y": 4.111109595697515, + "heading": -0.014699203415963554, + "angularVelocity": 0.14509930039224503, + "velocityX": 1.7517676385937064, + "velocityY": -0.12240028698767837, + "timestamp": 11.032272958918286 + }, + { + "x": 8.117811689845258, + "y": 4.104265522227424, + "heading": -0.0068963232026660135, + "angularVelocity": 0.10575428600660669, + "velocityX": 1.2952790438994197, + "velocityY": -0.09275935083211628, + "timestamp": 11.106056069825112 + }, + { + "x": 8.179700258175622, + "y": 4.099608457473234, + "heading": -0.001996583897562948, + "angularVelocity": 0.06640732878951901, + "velocityX": 0.8387904436357886, + "velocityY": -0.06311830305000711, + "timestamp": 11.179839180731937 + }, + { + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": 5.455800259144416e-28, + "angularVelocity": 0.027060175059361254, + "velocityX": 0.38230183268875023, + "velocityY": -0.03347720903449647, + "timestamp": 11.253622291638763 + }, + { + "x": 8.200639571345327, + "y": 4.096943687462525, + "heading": -0.0010802899918473188, + "angularVelocity": -0.0140223298346035, + "velocityX": -0.0943411225502494, + "velocityY": -0.002527461513954009, + "timestamp": 11.330662983491656 + }, + { + "x": 8.15665056358688, + "y": 4.099133352364744, + "heading": -0.0053255986809540795, + "angularVelocity": -0.05510475810903991, + "velocityX": -0.5709840695932866, + "velocityY": 0.028422186373926028, + "timestamp": 11.40770367534455 + }, + { + "x": 8.075940653456616, + "y": 4.103707386175576, + "heading": -0.012735784669566218, + "angularVelocity": -0.09618535101893501, + "velocityX": -1.0476270161796533, + "velocityY": 0.05937166062275022, + "timestamp": 11.484744367197443 + }, + { + "x": 7.958509840643352, + "y": 4.110665769785427, + "heading": -0.023310616019513113, + "angularVelocity": -0.13726293333579098, + "velocityX": -1.5242699668052715, + "velocityY": 0.0903208868261235, + "timestamp": 11.561785059050337 + }, + { + "x": 7.804358124937036, + "y": 4.120008478318513, + "heading": -0.037049836652876506, + "angularVelocity": -0.17833719172197993, + "velocityX": -2.000912920157348, + "velocityY": 0.12126979013798393, + "timestamp": 11.63882575090323 + }, + { + "x": 7.613485507099167, + "y": 4.131735481105862, + "heading": -0.05395325351914213, + "angularVelocity": -0.21940894428287547, + "velocityX": -2.477555863625089, + "velocityY": 0.15221829536189968, + "timestamp": 11.715866442756123 + }, + { + "x": 7.385891991055361, + "y": 4.145846741676364, + "heading": -0.07402084166347275, + "angularVelocity": -0.2604803729261552, + "velocityX": -2.954198756137707, + "velocityY": 0.18316632718520298, + "timestamp": 11.792907134609017 + }, + { + "x": 7.121577593711165, + "y": 4.162342217760912, + "heading": -0.09725284930983928, + "angularVelocity": -0.30155502355466035, + "velocityX": -3.4308414292137566, + "velocityY": 0.214113810349038, + "timestamp": 11.86994782646191 + }, + { + "x": 6.835203978246458, + "y": 4.180936324910101, + "heading": -0.09725285259123628, + "angularVelocity": -4.2593036391257635e-8, + "velocityX": -3.717173464790893, + "velocityY": 0.24135436354456943, + "timestamp": 11.946988518314804 + }, + { + "x": 6.5488303639215975, + "y": 4.199530449615619, + "heading": -0.09725285587256693, + "angularVelocity": -4.259217541458275e-8, + "velocityX": -3.717173449995519, + "velocityY": 0.2413545914284215, + "timestamp": 12.024029210167697 + }, + { + "x": 6.262456749596831, + "y": 4.218124574322583, + "heading": -0.0972528591538976, + "angularVelocity": -4.259217545578449e-8, + "velocityX": -3.7171734499942994, + "velocityY": 0.2413545914471941, + "timestamp": 12.10106990202059 + }, + { + "x": 5.976083139919176, + "y": 4.236718770600949, + "heading": -0.09725286243522821, + "angularVelocity": -4.259217464314882e-8, + "velocityX": -3.717173389674077, + "velocityY": 0.24135552045497397, + "timestamp": 12.178110593873484 + }, + { + "x": 5.690024921889411, + "y": 4.259659658304738, + "heading": -0.09725286572349409, + "angularVelocity": -4.268219562622653e-8, + "velocityX": -3.71307955769639, + "velocityY": 0.29777624203574676, + "timestamp": 12.255151285726377 + }, + { + "x": 5.4092552652272206, + "y": 4.319022844462834, + "heading": -0.09725286915389227, + "angularVelocity": -4.452709474069528e-8, + "velocityX": -3.6444332197627625, + "velocityY": 0.7705432639604031, + "timestamp": 12.33219197757927 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -7.968133230975529e-8, + "velocityX": -3.5158347290283003, + "velocityY": 1.2306645781352268, + "timestamp": 12.409232669432164 + }, + { + "x": 4.909018653493503, + "y": 4.524275328875486, + "heading": -0.09725287687080802, + "angularVelocity": -2.1964896173308048e-8, + "velocityX": -3.192337867898694, + "velocityY": 1.5370761958403907, + "timestamp": 12.481084171767012 + }, + { + "x": 4.71108590609518, + "y": 4.628682093882472, + "heading": -0.09725287720527087, + "angularVelocity": -4.654917836145884e-9, + "velocityX": -2.754747513502237, + "velocityY": 1.4530909113134631, + "timestamp": 12.55293567410186 + }, + { + "x": 4.54294597125426, + "y": 4.721368217002082, + "heading": -0.09725287703485491, + "angularVelocity": 2.3717800410462983e-9, + "velocityX": -2.3401032598781417, + "velocityY": 1.2899677822694202, + "timestamp": 12.624787176436708 + }, + { + "x": 4.40369795049572, + "y": 4.80026148816612, + "heading": -0.0972528766496691, + "angularVelocity": 5.3608595378330946e-9, + "velocityX": -1.9379973449908554, + "velocityY": 1.0980044759032759, + "timestamp": 12.696638678771556 + }, + { + "x": 4.292814205558175, + "y": 4.864307082758427, + "heading": -0.09725287620265412, + "angularVelocity": 6.221372869321198e-9, + "velocityX": -1.5432348849269113, + "velocityY": 0.8913605493429585, + "timestamp": 12.768490181106404 + }, + { + "x": 4.209952469660844, + "y": 4.91286872561946, + "heading": -0.09725287578795568, + "angularVelocity": 5.771604384474185e-9, + "velocityX": -1.1532359547775561, + "velocityY": 0.6758612037744437, + "timestamp": 12.840341683441252 + }, + { + "x": 4.154873707750821, + "y": 4.945521497298995, + "heading": -0.09725287546928488, + "angularVelocity": 4.4351304984872785e-9, + "velocityX": -0.7665638173206214, + "velocityY": 0.4544480020384788, + "timestamp": 12.9121931857761 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 2.459066716778208e-9, + "velocityX": -0.38234244368231685, + "velocityY": 0.22880870103743695, + "timestamp": 12.984044688110949 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -3.871307616700285e-29, + "velocityX": 3.757481365144797e-28, + "velocityY": -1.0154842203400723e-27, + "timestamp": 13.055896190445797 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.387, + "y": 4.121134281158447, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 0.6923742371436291, + "isStopPoint": false, + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 1.3971973802479047, + "isStopPoint": true, + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 2.430115113382513, + "isStopPoint": false, + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 3.0066730692210744, + "isStopPoint": false, + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 3.456916790263765, + "isStopPoint": false, + "x": 8.208122253417969, + "y": 0.763107419013977, + "heading": -0.34555563246426124, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 4.544126438842174, + "isStopPoint": false, + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 + }, + { + "timestamp": 5.6369430590525305, + "isStopPoint": true, + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 6.687562362256374, + "isStopPoint": false, + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 19 + }, + { + "timestamp": 7.689873244948311, + "isStopPoint": false, + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 8.338283147784624, + "isStopPoint": false, + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 + }, + { + "timestamp": 8.922397452891401, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 9.568349729100014, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 10.146875628036378, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 11.253622291638763, + "isStopPoint": false, + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 12.409232669432164, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 13.055896190445797, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 16 + ], + "type": "StopPoint" + }, + { + "scope": [ + 12 + ], + "type": "StopPoint" + }, + { + "scope": [ + 7 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "AmpLanepdChaos": { + "waypoints": [ + { + "x": 0.4269569218158722, + "y": 6.9908647537231445, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 1.5002212524414062, + "y": 6.694304943084717, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 3.8162126541137695, + "y": 6.581329345703125, + "heading": 0.2985001179522956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 8.264610290527344, + "y": 7.456887245178223, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 8.377585411071777, + "y": 6.553085803985596, + "heading": 1.0808392342145412, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.4269569218158722, + "y": 6.9908647537231445, + "heading": -4.078401909973915e-26, + "angularVelocity": -1.0353470347432896e-26, + "velocityX": -4.552508560212021e-27, + "velocityY": 2.1159417555564544e-25, + "timestamp": 0 + }, + { + "x": 0.4542226293224775, + "y": 6.990998038288888, + "heading": -0.0026449196556089733, + "angularVelocity": -0.03988238422563954, + "velocityX": 0.41113589997196526, + "velocityY": 0.002009779862709322, + "timestamp": 0.06631799244109911 + }, + { + "x": 0.5087389045980273, + "y": 6.990153139430081, + "heading": -0.007565779067559187, + "angularVelocity": -0.07420097066901893, + "velocityX": 0.8220435098961852, + "velocityY": -0.012740115128746688, + "timestamp": 0.13263598488219822 + }, + { + "x": 0.5904087421733784, + "y": 6.986785246694186, + "heading": -0.014243760110711712, + "angularVelocity": -0.10069636907485748, + "velocityX": 1.2314883875275162, + "velocityY": -0.050783997101327, + "timestamp": 0.19895397732329734 + }, + { + "x": 0.698928204332834, + "y": 6.978630694988513, + "heading": -0.02190403949445005, + "angularVelocity": -0.11550831232627977, + "velocityX": 1.6363502296279249, + "velocityY": -0.12296137753138935, + "timestamp": 0.26527196976439643 + }, + { + "x": 0.8334233004336311, + "y": 6.962161301124643, + "heading": -0.029295015883550983, + "angularVelocity": -0.1114475290497566, + "velocityX": 2.028033285540875, + "velocityY": -0.24833975302400757, + "timestamp": 0.3315899622054955 + }, + { + "x": 0.9912778011031088, + "y": 6.931626408492899, + "heading": -0.03421092005184831, + "angularVelocity": -0.07412625122305122, + "velocityX": 2.3802665741077376, + "velocityY": -0.46043149841822084, + "timestamp": 0.3979079546465946 + }, + { + "x": 1.1646969055280092, + "y": 6.878746569678396, + "heading": -0.03283456831141496, + "angularVelocity": 0.020753820943174132, + "velocityX": 2.6149631199847305, + "velocityY": -0.7973679067783651, + "timestamp": 0.4642259470876937 + }, + { + "x": 1.3384870937620608, + "y": 6.798699012794488, + "heading": -0.021425411587726872, + "angularVelocity": 0.17203712452274886, + "velocityX": 2.6205586423383487, + "velocityY": -1.2070262373367564, + "timestamp": 0.5305439395287929 + }, + { + "x": 1.5002212524414062, + "y": 6.694304943084717, + "heading": 2.0169023598696312e-25, + "angularVelocity": 0.3230708711026805, + "velocityX": 2.438767410261869, + "velocityY": -1.5741439972340743, + "timestamp": 0.596861931969892 + }, + { + "x": 1.618361440068333, + "y": 6.597461102589199, + "heading": 0.022914254950403036, + "angularVelocity": 0.43061521013807685, + "velocityX": 2.2201447016642413, + "velocityY": -1.8199339588312498, + "timestamp": 0.6500747642408552 + }, + { + "x": 1.720748899087832, + "y": 6.493018845204374, + "heading": 0.052155873821770504, + "angularVelocity": 0.5495219409195748, + "velocityX": 1.9241121859128945, + "velocityY": -1.9627269011541884, + "timestamp": 0.7032875965118184 + }, + { + "x": 1.8070404982970254, + "y": 6.395496343697618, + "heading": 0.0861590476515803, + "angularVelocity": 0.6390032700507929, + "velocityX": 1.6216313908981774, + "velocityY": -1.8326876684587161, + "timestamp": 0.7565004287827816 + }, + { + "x": 1.884907399599166, + "y": 6.313378100766396, + "heading": 0.12134509518920418, + "angularVelocity": 0.6612323764022606, + "velocityX": 1.4633105959411692, + "velocityY": -1.5432037616992567, + "timestamp": 0.8097132610537447 + }, + { + "x": 1.9587492444943766, + "y": 6.248351408561538, + "heading": 0.15586853154597385, + "angularVelocity": 0.6487802825636894, + "velocityX": 1.3876698860756558, + "velocityY": -1.2220114853826567, + "timestamp": 0.8629260933247079 + }, + { + "x": 2.0307407284297647, + "y": 6.200783008350587, + "heading": 0.18886810446406907, + "angularVelocity": 0.6201431404752012, + "velocityX": 1.3528970525155128, + "velocityY": -0.8939272386165963, + "timestamp": 0.9161389255956711 + }, + { + "x": 2.102117652739723, + "y": 6.170757252045742, + "heading": 0.21987485065465232, + "angularVelocity": 0.5826930247331111, + "velocityX": 1.341347965590374, + "velocityY": -0.5642578119494339, + "timestamp": 0.9693517578666343 + }, + { + "x": 2.173665780366198, + "y": 6.158282055577678, + "heading": 0.24860100716919956, + "angularVelocity": 0.5398351354100418, + "velocityX": 1.3445652969973032, + "velocityY": -0.23443962547489916, + "timestamp": 1.0225645901375975 + }, + { + "x": 2.2459257714568976, + "y": 6.1633423033591725, + "heading": 0.2748547098138841, + "angularVelocity": 0.49337164597815464, + "velocityX": 1.3579429623807138, + "velocityY": 0.09509450193755564, + "timestamp": 1.0757774224085606 + }, + { + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "angularVelocity": 0.4443553768761555, + "velocityX": 1.3787197305196603, + "velocityY": 0.42422324606752454, + "timestamp": 1.1289902546795236 + }, + { + "x": 2.405902969061742, + "y": 6.211704806035944, + "heading": 0.3149900926576246, + "angularVelocity": 0.32101497976485666, + "velocityX": 1.6860927079542964, + "velocityY": 0.5020296968516651, + "timestamp": 1.1803584952438984 + }, + { + "x": 2.5083069945465017, + "y": 6.241490500500008, + "heading": 0.32528072647931233, + "angularVelocity": 0.20033066557519108, + "velocityX": 1.9935279923871816, + "velocityY": 0.5798464992535075, + "timestamp": 1.231726735808273 + }, + { + "x": 2.626507788844443, + "y": 6.275274316431955, + "heading": 0.3295638724656478, + "angularVelocity": 0.08338120868609038, + "velocityX": 2.301048137901694, + "velocityY": 0.6576790554001625, + "timestamp": 1.2830949763726478 + }, + { + "x": 2.760511656599945, + "y": 6.31305748289524, + "heading": 0.32812899320961725, + "angularVelocity": -0.027933198417265603, + "velocityX": 2.6086910177032148, + "velocityY": 0.7355355380711394, + "timestamp": 1.3344632169370225 + }, + { + "x": 2.910328504957554, + "y": 6.354841933969171, + "heading": 0.32146257997595, + "angularVelocity": -0.12977694311552027, + "velocityX": 2.9165267626766083, + "velocityY": 0.8134296720084567, + "timestamp": 1.3858314575013972 + }, + { + "x": 3.0759758332063107, + "y": 6.400630905749405, + "heading": 0.31055238687433734, + "angularVelocity": -0.21239180049275855, + "velocityX": 3.2247031712360825, + "velocityY": 0.8913868039309617, + "timestamp": 1.437199698065772 + }, + { + "x": 3.2574854985287596, + "y": 6.450428350805819, + "heading": 0.2985001681214898, + "angularVelocity": -0.23462393534276735, + "velocityX": 3.5334997525364127, + "velocityY": 0.9694208816439399, + "timestamp": 1.4885679386301467 + }, + { + "x": 3.442976613157377, + "y": 6.497402957699113, + "heading": 0.298500151417318, + "angularVelocity": -3.251848140670727e-7, + "velocityX": 3.6110077470175392, + "velocityY": 0.9144678964510323, + "timestamp": 1.5399361791945214 + }, + { + "x": 3.628468017220463, + "y": 6.544376421672586, + "heading": 0.29850013471325804, + "angularVelocity": -3.251826373487328e-7, + "velocityX": 3.6110133815198173, + "velocityY": 0.9144456469091344, + "timestamp": 1.5913044197588961 + }, + { + "x": 3.8162126541137695, + "y": 6.581329345703125, + "heading": 0.2985001179522956, + "angularVelocity": -3.2629037443593046e-7, + "velocityX": 3.654877699344691, + "velocityY": 0.7193729749071157, + "timestamp": 1.6426726603232709 + }, + { + "x": 4.069115062985546, + "y": 6.6311069780063585, + "heading": 0.29850007681370144, + "angularVelocity": -5.945239460622418e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.711868517582718 + }, + { + "x": 4.322017471857322, + "y": 6.68088461030959, + "heading": 0.2985000356751073, + "angularVelocity": -5.945239466780917e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.781064374842165 + }, + { + "x": 4.574919880729098, + "y": 6.730662242612822, + "heading": 0.29849999453651316, + "angularVelocity": -5.9452394681569e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020963, + "timestamp": 1.8502602321016122 + }, + { + "x": 4.827822289600874, + "y": 6.780439874916056, + "heading": 0.2984999533979189, + "angularVelocity": -5.945239480024863e-7, + "velocityX": 3.6548778913675166, + "velocityY": 0.7193730127020963, + "timestamp": 1.9194560893610593 + }, + { + "x": 5.08072469847265, + "y": 6.830217507219288, + "heading": 0.2984999122593246, + "angularVelocity": -5.945239490427592e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.9886519466205064 + }, + { + "x": 5.333627107344427, + "y": 6.8799951395225225, + "heading": 0.29849987112073023, + "angularVelocity": -5.945239498975491e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020962, + "timestamp": 2.0578478038799535 + }, + { + "x": 5.586529516216203, + "y": 6.929772771825754, + "heading": 0.2984998299821357, + "angularVelocity": -5.945239517809028e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.1270436611394006 + }, + { + "x": 5.839431925087979, + "y": 6.979550404128987, + "heading": 0.29849978884354117, + "angularVelocity": -5.94523951964415e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.1962395183988477 + }, + { + "x": 6.092334333959755, + "y": 7.0293280364322195, + "heading": 0.29849974770494664, + "angularVelocity": -5.945239518467797e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.265435375658295 + }, + { + "x": 6.345236742831531, + "y": 7.079105668735451, + "heading": 0.2984997065663521, + "angularVelocity": -5.945239526667429e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.334631232917742 + }, + { + "x": 6.5981391517033074, + "y": 7.128883301038686, + "heading": 0.29849966542775747, + "angularVelocity": -5.945239534790066e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020961, + "timestamp": 2.403827090177189 + }, + { + "x": 6.851041560575084, + "y": 7.178660933341917, + "heading": 0.2984996242891628, + "angularVelocity": -5.945239543358476e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.473022947436636 + }, + { + "x": 7.103943969446842, + "y": 7.228438565645149, + "heading": 0.29849958315051817, + "angularVelocity": -5.94524676762677e-7, + "velocityX": 3.654877891367282, + "velocityY": 0.7193730127020501, + "timestamp": 2.542218804696083 + }, + { + "x": 7.353433038122888, + "y": 7.277544365703102, + "heading": 0.285740099482384, + "angularVelocity": -0.18439664126557498, + "velocityX": 3.605549212875508, + "velocityY": 0.7096638729950698, + "timestamp": 2.6114146619555303 + }, + { + "x": 7.577591625127541, + "y": 7.321664482119973, + "heading": 0.20789341833488353, + "angularVelocity": -1.1250193903316694, + "velocityX": 3.2394798747008724, + "velocityY": 0.6376121080694793, + "timestamp": 2.6806105192149774 + }, + { + "x": 7.772618972429793, + "y": 7.360050828957848, + "heading": 0.13154587005742394, + "angularVelocity": -1.1033543235283216, + "velocityX": 2.818482999220663, + "velocityY": 0.5547492054899759, + "timestamp": 2.7498063764744245 + }, + { + "x": 7.938688452208001, + "y": 7.392737530275035, + "heading": 0.06829117903160017, + "angularVelocity": -0.9141398565040214, + "velocityX": 2.399991652036878, + "velocityY": 0.47237945466343967, + "timestamp": 2.8190022337338716 + }, + { + "x": 8.075968311155869, + "y": 7.419757701303976, + "heading": 0.02342953211486201, + "angularVelocity": -0.648328508288165, + "velocityX": 1.9839317610177574, + "velocityY": 0.3904882763086567, + "timestamp": 2.8881980909933187 + }, + { + "x": 8.184579349560671, + "y": 7.441135118616469, + "heading": -3.788367805348886e-26, + "angularVelocity": -0.33859732421572447, + "velocityX": 1.569617643402696, + "velocityY": 0.3089407106026408, + "timestamp": 2.957393948252766 + }, + { + "x": 8.264610290527344, + "y": 7.456887245178223, + "heading": -1.8720614918263104e-26, + "angularVelocity": 4.317468010745751e-27, + "velocityX": 1.1565857283420875, + "velocityY": 0.22764551500088193, + "timestamp": 3.026589805512213 + }, + { + "x": 8.309090579989254, + "y": 7.457165429033815, + "heading": 0.013618338743486997, + "angularVelocity": 0.312683368289724, + "velocityX": 1.021288058215154, + "velocityY": 0.006387230234839926, + "timestamp": 3.070142933451108 + }, + { + "x": 8.347694581932577, + "y": 7.447770213119958, + "heading": 0.040485197139557096, + "angularVelocity": 0.6168755188780982, + "velocityX": 0.8863657737162873, + "velocityY": -0.21571851112597287, + "timestamp": 3.1136960613900033 + }, + { + "x": 8.38043908621251, + "y": 7.428658664796905, + "heading": 0.08013490839874948, + "angularVelocity": 0.910375744190425, + "velocityX": 0.7518289920731597, + "velocityY": -0.43881000579032564, + "timestamp": 3.1572491893288985 + }, + { + "x": 8.407344118270583, + "y": 7.3997816960160065, + "heading": 0.13202626739741974, + "angularVelocity": 1.1914496490693778, + "velocityX": 0.6177520038473495, + "velocityY": -0.6630285847990722, + "timestamp": 3.2008023172677937 + }, + { + "x": 8.42843706760286, + "y": 7.361082970163005, + "heading": 0.19556012614559412, + "angularVelocity": 1.4587668384533072, + "velocityX": 0.4843038911435242, + "velocityY": -0.8885406785775672, + "timestamp": 3.244355445206689 + }, + { + "x": 8.44375820295339, + "y": 7.312496178717057, + "heading": 0.2700814705675387, + "angularVelocity": 1.7110446011248115, + "velocityX": 0.35178036746348396, + "velocityY": -1.1155752467220192, + "timestamp": 3.287908573145584 + }, + { + "x": 8.453367300002943, + "y": 7.253940317433788, + "heading": 0.3548489824478594, + "angularVelocity": 1.9463013540439453, + "velocityX": 0.22062932111410738, + "velocityY": -1.3444697098546627, + "timestamp": 3.3314617010844794 + }, + { + "x": 8.457351243336406, + "y": 7.185313206736467, + "heading": 0.4489562663311582, + "angularVelocity": 2.1607468472834177, + "velocityX": 0.09147318509597613, + "velocityY": -1.575710263419076, + "timestamp": 3.3750148290233746 + }, + { + "x": 8.455833781713947, + "y": 7.106484187241007, + "heading": 0.5511965710397227, + "angularVelocity": 2.3474847742740175, + "velocityX": -0.034841622043504016, + "velocityY": -1.8099508169896936, + "timestamp": 3.41856795696227 + }, + { + "x": 8.448992060952785, + "y": 7.01728730637676, + "heading": 0.6598709993298483, + "angularVelocity": 2.4952152332800406, + "velocityX": -0.157089079130233, + "velocityY": -2.048001718484822, + "timestamp": 3.462121084901165 + }, + { + "x": 8.437091795531538, + "y": 6.917516589235633, + "heading": 0.7725277452001952, + "angularVelocity": 2.5866510903282087, + "velocityX": -0.27323560865576907, + "velocityY": -2.2907818993185733, + "timestamp": 3.50567421284006 + }, + { + "x": 8.420568790056167, + "y": 6.8069293867126985, + "heading": 0.8855178727469111, + "angularVelocity": 2.5943056881067363, + "velocityX": -0.3793758624765617, + "velocityY": -2.539133416045067, + "timestamp": 3.5492273407789554 + }, + { + "x": 8.400226551820241, + "y": 6.685307877393556, + "heading": 0.9927665754284469, + "angularVelocity": 2.4624799126254615, + "velocityX": -0.46706721649169625, + "velocityY": -2.7924862133846577, + "timestamp": 3.5927804687178506 + }, + { + "x": 8.377585411071777, + "y": 6.553085803985596, + "heading": 1.0808392342145412, + "angularVelocity": 2.022189058605845, + "velocityX": -0.5198510834911644, + "velocityY": -3.035880077166161, + "timestamp": 3.636333596656746 + }, + { + "x": 8.33115655720319, + "y": 6.248001091638859, + "heading": 1.1393178439925082, + "angularVelocity": 0.6668090360059423, + "velocityX": -0.529410316156976, + "velocityY": -3.478763324102234, + "timestamp": 3.724032782810915 + }, + { + "x": 8.325096274978753, + "y": 5.9213777688010065, + "heading": 1.139317843108129, + "angularVelocity": -1.0084235369622052e-8, + "velocityX": -0.06910306115935677, + "velocityY": -3.7243597935295676, + "timestamp": 3.811731968965084 + }, + { + "x": 8.328579577191661, + "y": 5.594716799276763, + "heading": 1.1393178410314753, + "angularVelocity": -2.3679281998093647e-8, + "velocityX": 0.03971875185687836, + "velocityY": -3.7247890641766865, + "timestamp": 3.8994311551192533 + }, + { + "x": 8.332062981879394, + "y": 5.2680558308452605, + "heading": 1.1393178389548215, + "angularVelocity": -2.3679282006774464e-8, + "velocityX": 0.03971992033781079, + "velocityY": -3.7247890517165767, + "timestamp": 3.9871303412734225 + }, + { + "x": 8.335546386568186, + "y": 4.94139486241377, + "heading": 1.1393178368781678, + "angularVelocity": -2.367928278808999e-8, + "velocityX": 0.039719920349857205, + "velocityY": -3.7247890517164484, + "timestamp": 4.074829527427592 + }, + { + "x": 8.339029791256978, + "y": 4.614733893982279, + "heading": 1.139317834801514, + "angularVelocity": -2.3679282128211672e-8, + "velocityX": 0.039719920349857364, + "velocityY": -3.724789051716449, + "timestamp": 4.162528713581761 + }, + { + "x": 8.34251319594577, + "y": 4.288072925550788, + "heading": 1.1393178327248603, + "angularVelocity": -2.3679283665933347e-8, + "velocityX": 0.03971992034985746, + "velocityY": -3.724789051716448, + "timestamp": 4.25022789973593 + }, + { + "x": 8.345996600634562, + "y": 3.9614119571192963, + "heading": 1.1393178306482066, + "angularVelocity": -2.367928231652133e-8, + "velocityX": 0.039719920349857295, + "velocityY": -3.724789051716449, + "timestamp": 4.337927085890099 + }, + { + "x": 8.349480005323354, + "y": 3.634750988687805, + "heading": 1.1393178285715528, + "angularVelocity": -2.3679282106335388e-8, + "velocityX": 0.03971992034985752, + "velocityY": -3.7247890517164484, + "timestamp": 4.425626272044268 + }, + { + "x": 8.352963410012144, + "y": 3.308090020256313, + "heading": 1.139317826494899, + "angularVelocity": -2.367928328328809e-8, + "velocityX": 0.03971992034985731, + "velocityY": -3.724789051716449, + "timestamp": 4.5133254581984374 + }, + { + "x": 8.356446814700936, + "y": 2.9814290518248217, + "heading": 1.1393178244182454, + "angularVelocity": -2.3679282343675033e-8, + "velocityX": 0.03971992034985722, + "velocityY": -3.7247890517164484, + "timestamp": 4.601024644352607 + }, + { + "x": 8.359930219389728, + "y": 2.6547680833933303, + "heading": 1.1393178223415916, + "angularVelocity": -2.3679282346751086e-8, + "velocityX": 0.03971992034985726, + "velocityY": -3.7247890517164484, + "timestamp": 4.688723830506776 + }, + { + "x": 8.363413624078524, + "y": 2.3281071149618393, + "heading": 1.1393178202649379, + "angularVelocity": -2.36792827125054e-8, + "velocityX": 0.039719920349909635, + "velocityY": -3.724789051716448, + "timestamp": 4.776423016660945 + }, + { + "x": 8.366897029211264, + "y": 2.001446146535253, + "heading": 1.139317818188205, + "angularVelocity": -2.368018344998753e-8, + "velocityX": 0.03971992541203934, + "velocityY": -3.7247890516605238, + "timestamp": 4.864122202815114 + }, + { + "x": 8.369950836003104, + "y": 1.7153186940356544, + "heading": 1.1226153865660347, + "angularVelocity": -0.19045138677579856, + "velocityX": 0.03482138119813859, + "velocityY": -3.262601000613699, + "timestamp": 4.951821388969283 + }, + { + "x": 8.372495686328458, + "y": 1.4768791360484632, + "heading": 1.108692502981797, + "angularVelocity": -0.1587572723851997, + "velocityX": 0.02901794688128086, + "velocityY": -2.718834329523098, + "timestamp": 5.039520575123452 + }, + { + "x": 8.374531573132332, + "y": 1.2861274855011322, + "heading": 1.0975518982602839, + "angularVelocity": -0.1270320194525941, + "velocityX": 0.023214432119068158, + "velocityY": -2.175067511025735, + "timestamp": 5.127219761277622 + }, + { + "x": 8.376058491392183, + "y": 1.1430637450377987, + "heading": 1.089195515779546, + "angularVelocity": -0.09528460692950849, + "velocityX": 0.01741086008672813, + "velocityY": -1.6313006623782995, + "timestamp": 5.214918947431791 + }, + { + "x": 8.377076437882632, + "y": 1.0476879159528336, + "heading": 1.083624509980256, + "angularVelocity": -0.06352403076462655, + "velocityX": 0.011607251276645152, + "velocityY": -1.0875337989716474, + "timestamp": 5.30261813358596 + }, + { + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "angularVelocity": -0.03175942546169668, + "velocityX": 0.005803624998872994, + "velocityY": -0.5437669155675123, + "timestamp": 5.390317319740129 + }, + { + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "angularVelocity": -2.228121369995549e-27, + "velocityX": -4.3209221674019054e-27, + "velocityY": -1.4392739239520783e-27, + "timestamp": 5.478016505894298 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.4269569218158722, + "y": 6.9908647537231445, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 0.596861931969892, + "isStopPoint": false, + "x": 1.5002212524414062, + "y": 6.694304943084717, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 1.1289902546795236, + "isStopPoint": false, + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 1.6426726603232709, + "isStopPoint": false, + "x": 3.8162126541137695, + "y": 6.581329345703125, + "heading": 0.2985001179522956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "timestamp": 3.026589805512213, + "isStopPoint": false, + "x": 8.264610290527344, + "y": 7.456887245178223, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 3.636333596656746, + "isStopPoint": false, + "x": 8.377585411071777, + "y": 6.553085803985596, + "heading": 1.0808392342145412, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "timestamp": 5.478016505894298, + "isStopPoint": true, + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 4, + 4 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 3, + 4 + ], + "type": "StraightLine" + }, + { + "scope": [ + 2 + ], + "type": "WptVelocityDirection", + "direction": 0.2985001179522956 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "SourceLanephChaos": { + "waypoints": [ + { + "x": 1.4578553438186646, + "y": 1.4268364906311035, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 3.095994472503662, + "y": 1.2997395992279053, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 6.202812194824219, + "y": 1.003179669380188, + "heading": 0.7165423509110156, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 8.25048828125, + "y": 0.7348634600639343, + "heading": 0.7165423509110156, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "x": 8.377585411071777, + "y": 2.1329314708709717, + "heading": 3.7458973568177814, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 8.34934139251709, + "y": 2.556588649749756, + "heading": 5.290290323982317, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 8.377585411071777, + "y": 3.7993156909942627, + "heading": 3.770388538466339, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 8.179879188537598, + "y": 4.208850860595703, + "heading": 5.281700753397833, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 8.44819450378418, + "y": 5.38096809387207, + "heading": 3.7759750997570256, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 8.34934139251709, + "y": 5.974088191986084, + "heading": 5.326548029576795, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 8.44819450378418, + "y": 6.821402072906494, + "heading": 3.7101172753280305, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 8.321097373962402, + "y": 7.527496814727783, + "heading": 3.7090325363569563, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.4578553438186646, + "y": 1.4268364906311035, + "heading": 1.9268906266143656e-30, + "angularVelocity": 1.1190631191470915e-30, + "velocityX": -2.439449667203012e-26, + "velocityY": -3.284743628951617e-25, + "timestamp": 0 + }, + { + "x": 1.4836061256801238, + "y": 1.4248385832903434, + "heading": -1.6036305967396036e-15, + "angularVelocity": -2.484855550358542e-14, + "velocityX": 0.3990131727631876, + "velocityY": -0.030957947265928064, + "timestamp": 0.0645361697788919 + }, + { + "x": 1.5351076891841593, + "y": 1.420842768625806, + "heading": -4.81143470802221e-15, + "angularVelocity": -4.970552361990032e-14, + "velocityX": 0.7980263421347389, + "velocityY": -0.061915894268711684, + "timestamp": 0.1290723395577838 + }, + { + "x": 1.6123600340493498, + "y": 1.4148490466593255, + "heading": -9.625413675640745e-15, + "angularVelocity": -7.459350290095871e-14, + "velocityX": 1.1970395071456144, + "velocityY": -0.09287384093316672, + "timestamp": 0.1936085093366757 + }, + { + "x": 1.7153631599004666, + "y": 1.4068574174200144, + "heading": -1.6044392495578035e-14, + "angularVelocity": -9.946327527539101e-14, + "velocityX": 1.5960526663422558, + "velocityY": -0.12383178714651694, + "timestamp": 0.2581446791155676 + }, + { + "x": 1.8441170662121902, + "y": 1.39686788094863, + "heading": -2.407162986647994e-14, + "angularVelocity": -1.2438354179375857e-13, + "velocityX": 1.9950658173989684, + "velocityY": -0.15478973272832033, + "timestamp": 0.32268084889445947 + }, + { + "x": 1.9986217521965406, + "y": 1.3848804373063093, + "heading": -3.3704756925123664e-14, + "angularVelocity": -1.4926710233420884e-13, + "velocityX": 2.3940789562457856, + "velocityY": -0.1857476773628035, + "timestamp": 0.38721701867335134 + }, + { + "x": 2.1788772165402177, + "y": 1.3708950865949459, + "heading": -4.494558790872857e-14, + "angularVelocity": -1.7417877481910134e-13, + "velocityX": 2.793092074742775, + "velocityY": -0.2167056204184192, + "timestamp": 0.4517531884522432 + }, + { + "x": 2.3848834566166217, + "y": 1.3549118290183289, + "heading": -5.77927518216841e-14, + "angularVelocity": -1.9906920347103564e-13, + "velocityX": 3.1921051525401003, + "velocityY": -0.2476635603162993, + "timestamp": 0.5162893582311351 + }, + { + "x": 2.6166404645459465, + "y": 1.3369306651878214, + "heading": -7.224642245471218e-14, + "angularVelocity": -2.239623250426539e-13, + "velocityX": 3.5911181082383807, + "velocityY": -0.27862149074096887, + "timestamp": 0.580825528010027 + }, + { + "x": 2.856317459743021, + "y": 1.3183350190196794, + "heading": -7.226675860496505e-14, + "angularVelocity": -3.151124450451814e-16, + "velocityX": 3.7138397896595787, + "velocityY": -0.2881430092900255, + "timestamp": 0.6453616977889188 + }, + { + "x": 3.095994472503662, + "y": 1.2997395992279053, + "heading": 8.152675815601398e-24, + "angularVelocity": 1.119786917331966e-12, + "velocityX": 3.7138400618103087, + "velocityY": -0.28813950154594287, + "timestamp": 0.7098978675678107 + }, + { + "x": 3.27382254607751, + "y": 1.289661887920689, + "heading": 0.031144225689139507, + "angularVelocity": 0.6214983848668802, + "velocityX": 3.548646918156489, + "velocityY": -0.20110569975653228, + "timestamp": 0.7600093826537434 + }, + { + "x": 3.4512411724559873, + "y": 1.2794797717459663, + "heading": 0.06298682604506026, + "angularVelocity": 0.6354347958012462, + "velocityX": 3.5404761974215813, + "velocityY": -0.20318915038314128, + "timestamp": 0.8101208977396761 + }, + { + "x": 3.62822231090223, + "y": 1.2691838802186675, + "heading": 0.09560610924482929, + "angularVelocity": 0.6509338850328615, + "velocityX": 3.5317459099520483, + "velocityY": -0.20545959366110889, + "timestamp": 0.8602324128256088 + }, + { + "x": 3.8047300835293036, + "y": 1.258765647314116, + "heading": 0.12910425796921293, + "angularVelocity": 0.6684720800586451, + "velocityX": 3.5222996615526942, + "velocityY": -0.20790097618653766, + "timestamp": 0.9103439279115415 + }, + { + "x": 3.9807203074364863, + "y": 1.248215472268555, + "heading": 0.16360694737365913, + "angularVelocity": 0.6885181847980438, + "velocityX": 3.5119717215771487, + "velocityY": -0.21053394668808642, + "timestamp": 0.9604554429974742 + }, + { + "x": 4.156137440439902, + "y": 1.2375224650571883, + "heading": 0.19927204657899403, + "angularVelocity": 0.7117146457091811, + "velocityX": 3.50053540992732, + "velocityY": -0.2133842330057237, + "timestamp": 1.010566958083407 + }, + { + "x": 4.330909871790797, + "y": 1.2266740963817457, + "heading": 0.23630304667549967, + "angularVelocity": 0.7389718716946344, + "velocityX": 3.487670070465641, + "velocityY": -0.21648454765016575, + "timestamp": 1.0606784731693397 + }, + { + "x": 4.504942288680689, + "y": 1.2156556997827002, + "heading": 0.27497084629517315, + "angularVelocity": 0.7716350134966949, + "velocityX": 3.4729027168996156, + "velocityY": -0.21987753872839294, + "timestamp": 1.1107899882552725 + }, + { + "x": 4.678102490303239, + "y": 1.204449734224157, + "heading": 0.31565141892272963, + "angularVelocity": 0.8118008916273269, + "velocityX": 3.4554972310378296, + "velocityY": -0.22362056982965403, + "timestamp": 1.1609015033412053 + }, + { + "x": 4.850196619661247, + "y": 1.1930346345230565, + "heading": 0.3588966354181107, + "angularVelocity": 0.8629796249668915, + "velocityX": 3.4342232331809583, + "velocityY": -0.22779394479542386, + "timestamp": 1.2110130184271382 + }, + { + "x": 5.020916905370504, + "y": 1.1813828932131683, + "heading": 0.40558384210290876, + "angularVelocity": 0.931666236886622, + "velocityX": 3.4068075055503675, + "velocityY": -0.23251624481733327, + "timestamp": 1.261124533513071 + }, + { + "x": 5.189710400965398, + "y": 1.16945753417408, + "heading": 0.45729195406784345, + "angularVelocity": 1.0318608782086127, + "velocityX": 3.3683574584692115, + "velocityY": -0.2379764215597622, + "timestamp": 1.3112360485990038 + }, + { + "x": 5.35533172203286, + "y": 1.1572047491345996, + "heading": 0.5175844077279528, + "angularVelocity": 1.2031656507834203, + "velocityX": 3.3050551511653112, + "velocityY": -0.244510368893633, + "timestamp": 1.3613475636849366 + }, + { + "x": 5.512680992157926, + "y": 1.1445116485154183, + "heading": 0.6010844246558354, + "angularVelocity": 1.6662840224386364, + "velocityX": 3.1399822945931812, + "velocityY": -0.25329708346305213, + "timestamp": 1.4114590787708694 + }, + { + "x": 5.678954750693719, + "y": 1.128816110929195, + "heading": 0.6579086700766517, + "angularVelocity": 1.1339558447469087, + "velocityX": 3.3180748626470646, + "velocityY": -0.3132121940298074, + "timestamp": 1.4615705938568022 + }, + { + "x": 5.854338980055963, + "y": 1.1023762612022525, + "heading": 0.6848690400817262, + "angularVelocity": 0.5380074810917644, + "velocityX": 3.4998788015387303, + "velocityY": -0.5276202422058656, + "timestamp": 1.511682108942735 + }, + { + "x": 6.029328591050374, + "y": 1.0604680679443284, + "heading": 0.7046293616016649, + "angularVelocity": 0.39432696229705316, + "velocityX": 3.4920039973713513, + "velocityY": -0.8362986668046034, + "timestamp": 1.5617936240286678 + }, + { + "x": 6.202812194824219, + "y": 1.003179669380188, + "heading": 0.7165423509110156, + "angularVelocity": 0.2377295775017328, + "velocityX": 3.4619508804782524, + "velocityY": -1.1432182496558028, + "timestamp": 1.6119051391146006 + }, + { + "x": 6.402547568535553, + "y": 0.9154521762914801, + "heading": 0.7165423052284949, + "angularVelocity": -7.80040343805035e-7, + "velocityX": 3.4105309321224193, + "velocityY": -1.4979686533092873, + "timestamp": 1.6704694441118393 + }, + { + "x": 6.592790671542091, + "y": 0.8086912505756066, + "heading": 0.7165422622385289, + "angularVelocity": -7.34064308321193e-7, + "velocityX": 3.2484480609051705, + "velocityY": -1.8229692253823835, + "timestamp": 1.729033749109078 + }, + { + "x": 6.792526057186022, + "y": 0.7209637957417591, + "heading": 0.7165421996040524, + "angularVelocity": -0.0000010694991862536609, + "velocityX": 3.410531135874468, + "velocityY": -1.4979680000980806, + "timestamp": 1.7875980541063166 + }, + { + "x": 6.995358135542775, + "y": 0.6540115645265273, + "heading": 0.702913894528627, + "angularVelocity": -0.23270668158817906, + "velocityX": 3.4634079302455083, + "velocityY": -1.1432259158268625, + "timestamp": 1.8461623591035552 + }, + { + "x": 7.192981165480996, + "y": 0.6064556135405038, + "heading": 0.6594934468717042, + "angularVelocity": -0.7414148884541547, + "velocityX": 3.3744621394813583, + "velocityY": -0.8120296311595578, + "timestamp": 1.9047266641007938 + }, + { + "x": 7.3826916988169575, + "y": 0.5778864686152094, + "heading": 0.5928355536169142, + "angularVelocity": -1.138200022316206, + "velocityX": 3.239354301991779, + "velocityY": -0.4878252192464604, + "timestamp": 1.9632909690980325 + }, + { + "x": 7.56045020150032, + "y": 0.5667919314464598, + "heading": 0.5348000913331561, + "angularVelocity": -0.9909698798012667, + "velocityX": 3.0352704209798658, + "velocityY": -0.18944196758201154, + "timestamp": 2.021855274095271 + }, + { + "x": 7.725161762075186, + "y": 0.5716432246000058, + "heading": 0.4994377275755845, + "angularVelocity": -0.6038211118400366, + "velocityX": 2.812490655914593, + "velocityY": 0.08283703108531969, + "timestamp": 2.0804195790925095 + }, + { + "x": 7.876544251370993, + "y": 0.5914886747454028, + "heading": 0.49476791383000396, + "angularVelocity": -0.07973822528587567, + "velocityX": 2.5848934654470064, + "velocityY": 0.3388659721366544, + "timestamp": 2.138983884089748 + }, + { + "x": 8.014557697135269, + "y": 0.6256824372518744, + "heading": 0.5263224795620095, + "angularVelocity": 0.5388020182855987, + "velocityX": 2.35661373887698, + "velocityY": 0.5838669562984516, + "timestamp": 2.197548189086987 + }, + { + "x": 8.139220711960105, + "y": 0.6736709753190644, + "heading": 0.5988363569407734, + "angularVelocity": 1.238192400339819, + "velocityX": 2.128651826922783, + "velocityY": 0.8194161626173599, + "timestamp": 2.2561124940842254 + }, + { + "x": 8.25048828125, + "y": 0.7348634600639343, + "heading": 0.7165423509110156, + "angularVelocity": 2.0098589742641386, + "velocityX": 1.8999212796112497, + "velocityY": 1.044876819553399, + "timestamp": 2.314676799081464 + }, + { + "x": 8.302330173893822, + "y": 0.7683170831186208, + "heading": 0.7866903105882892, + "angularVelocity": 2.4170681239593663, + "velocityX": 1.7863012234653046, + "velocityY": 1.152701893090581, + "timestamp": 2.343698719887885 + }, + { + "x": 8.350761252955918, + "y": 0.804862507674631, + "heading": 0.8681048778617475, + "angularVelocity": 2.805278389962577, + "velocityX": 1.6687757983055662, + "velocityY": 1.2592352105076432, + "timestamp": 2.372720640694306 + }, + { + "x": 8.395659126629422, + "y": 0.8444668269434722, + "heading": 0.9600582095499061, + "angularVelocity": 3.168409572250435, + "velocityX": 1.5470331537659474, + "velocityY": 1.36463466815329, + "timestamp": 2.4017425615007273 + }, + { + "x": 8.436895225190787, + "y": 0.8871049250141315, + "heading": 1.0616235787520496, + "angularVelocity": 3.499608791561171, + "velocityX": 1.420860419143676, + "velocityY": 1.469168714057877, + "timestamp": 2.4307644823071484 + }, + { + "x": 8.474338814577639, + "y": 0.9327584709704907, + "heading": 1.1716968936719283, + "angularVelocity": 3.792764636568266, + "velocityX": 1.2901830184364116, + "velocityY": 1.5730711368442005, + "timestamp": 2.4597864031135694 + }, + { + "x": 8.50785937445302, + "y": 0.9814068916097549, + "heading": 1.2890700861945108, + "angularVelocity": 4.0442944250820885, + "velocityX": 1.1550083159196811, + "velocityY": 1.676264674683452, + "timestamp": 2.4888083239199905 + }, + { + "x": 8.537323038411628, + "y": 1.0330097209040148, + "heading": 1.4125130240648722, + "angularVelocity": 4.253437899363618, + "velocityX": 1.0152210170765539, + "velocityY": 1.7780638862070914, + "timestamp": 2.5178302447264116 + }, + { + "x": 8.56258232091714, + "y": 1.0874855013153344, + "heading": 1.5407726955584362, + "angularVelocity": 4.419406708090351, + "velocityX": 0.870351851415892, + "velocityY": 1.877056338713015, + "timestamp": 2.5468521655328327 + }, + { + "x": 8.583466101858683, + "y": 1.1446918380868547, + "heading": 1.6724319994758665, + "angularVelocity": 4.536546867301084, + "velocityX": 0.7195864491822832, + "velocityY": 1.9711423359291682, + "timestamp": 2.5758740863392537 + }, + { + "x": 8.599781102269066, + "y": 1.2044006523964297, + "heading": 1.8056614857101974, + "angularVelocity": 4.590650188972121, + "velocityX": 0.5621612890203437, + "velocityY": 2.057369486597329, + "timestamp": 2.604896007145675 + }, + { + "x": 8.61133776585303, + "y": 1.2662515535930832, + "heading": 1.9379574799883315, + "angularVelocity": 4.558485124418899, + "velocityX": 0.3982046419686604, + "velocityY": 2.131178759986453, + "timestamp": 2.633917927952096 + }, + { + "x": 8.61803446841016, + "y": 1.329807234083184, + "heading": 2.0667068830619315, + "angularVelocity": 4.43628124865926, + "velocityX": 0.23074635899526613, + "velocityY": 2.189919851067852, + "timestamp": 2.662939848758517 + }, + { + "x": 8.619867506604743, + "y": 1.394964606545626, + "heading": 2.191275336084359, + "angularVelocity": 4.292219452093143, + "velocityX": 0.06316047124545049, + "velocityY": 2.245108891897538, + "timestamp": 2.691961769564938 + }, + { + "x": 8.616839586460362, + "y": 1.4616264782251567, + "heading": 2.311063053299335, + "angularVelocity": 4.12749100977746, + "velocityX": -0.10433217582589409, + "velocityY": 2.2969489898401783, + "timestamp": 2.720983690371359 + }, + { + "x": 8.608949954788589, + "y": 1.5296853158108266, + "heading": 2.4255531996922417, + "angularVelocity": 3.9449541316223122, + "velocityX": -0.2718507752949118, + "velocityY": 2.345083843334451, + "timestamp": 2.75000561117778 + }, + { + "x": 8.596173618614296, + "y": 1.5989753133446671, + "heading": 2.534426417053211, + "angularVelocity": 3.7514132192408356, + "velocityX": -0.44023055053838084, + "velocityY": 2.3875055684980784, + "timestamp": 2.7790275319842013 + }, + { + "x": 8.57842938977896, + "y": 1.6691611767333963, + "heading": 2.6378090883600067, + "angularVelocity": 3.5622270488699845, + "velocityX": -0.6114078028705309, + "velocityY": 2.4183741612719314, + "timestamp": 2.8080494527906223 + }, + { + "x": 8.555564514046702, + "y": 1.7394263904616403, + "heading": 2.737024931725787, + "angularVelocity": 3.4186518538024746, + "velocityX": -0.7878484640892429, + "velocityY": 2.421108313158164, + "timestamp": 2.8370713735970434 + }, + { + "x": 8.527959255725655, + "y": 1.8075255209054102, + "heading": 2.8376495221341176, + "angularVelocity": 3.4671926465345364, + "velocityX": -0.9511864671252386, + "velocityY": 2.3464722027875915, + "timestamp": 2.8660932944034645 + }, + { + "x": 8.499577272044599, + "y": 1.8711589675729616, + "heading": 2.9502161532820788, + "angularVelocity": 3.878676118606718, + "velocityX": -0.9779498700435901, + "velocityY": 2.1925994179362642, + "timestamp": 2.8951152152098856 + }, + { + "x": 8.472091457883874, + "y": 1.93067768486564, + "heading": 3.076977426653027, + "angularVelocity": 4.3677768338097795, + "velocityX": -0.9470708139567483, + "velocityY": 2.0508193682173372, + "timestamp": 2.9241371360163066 + }, + { + "x": 8.445935000391486, + "y": 1.9864273544904458, + "heading": 3.219502150557654, + "angularVelocity": 4.910933526946052, + "velocityX": -0.9012655525750104, + "velocityY": 1.920950373914328, + "timestamp": 2.9531590568227277 + }, + { + "x": 8.421236309719301, + "y": 2.038741873704351, + "heading": 3.37984910584556, + "angularVelocity": 5.52502904123526, + "velocityX": -0.8510356994262128, + "velocityY": 1.8025863816129806, + "timestamp": 2.982180977629149 + }, + { + "x": 8.398072066125595, + "y": 2.08798012126862, + "heading": 3.5600291132631146, + "angularVelocity": 6.208410829158125, + "velocityX": -0.7981636966145051, + "velocityY": 1.6965881718406162, + "timestamp": 3.01120289843557 + }, + { + "x": 8.377585411071777, + "y": 2.1329314708709717, + "heading": 3.745897356817782, + "angularVelocity": 6.404408750007486, + "velocityX": -0.7059027963884773, + "velocityY": 1.5488757585061539, + "timestamp": 3.040224819241991 + }, + { + "x": 8.364981537655835, + "y": 2.1618201695949115, + "heading": 3.87540454879606, + "angularVelocity": 6.50944417189865, + "velocityX": -0.6335108428921614, + "velocityY": 1.4520380580394667, + "timestamp": 3.0601200957906958 + }, + { + "x": 8.354284801682704, + "y": 2.189254221668137, + "heading": 4.007147516190271, + "angularVelocity": 6.621821369092149, + "velocityX": -0.5376520375047442, + "velocityY": 1.3789228818240207, + "timestamp": 3.0800153723394006 + }, + { + "x": 8.345932812421994, + "y": 2.2160755919920243, + "heading": 4.140533212375188, + "angularVelocity": 6.704390153028566, + "velocityX": -0.41979759568875646, + "velocityY": 1.3481275446574927, + "timestamp": 3.0999106488881054 + }, + { + "x": 8.339986678909588, + "y": 2.2433741294975276, + "heading": 4.274319407548544, + "angularVelocity": 6.724520508465367, + "velocityX": -0.2988716189920965, + "velocityY": 1.3721114878033795, + "timestamp": 3.1198059254368102 + }, + { + "x": 8.33607844515623, + "y": 2.2720346987144775, + "heading": 4.407573079371785, + "angularVelocity": 6.697754187886208, + "velocityX": -0.1964402828877463, + "velocityY": 1.4405715420334793, + "timestamp": 3.139701201985515 + }, + { + "x": 8.333720429129459, + "y": 2.3025901920416527, + "heading": 4.540110627397886, + "angularVelocity": 6.661759523756392, + "velocityX": -0.11852139984079614, + "velocityY": 1.5358164664046496, + "timestamp": 3.15959647853422 + }, + { + "x": 8.332603722599888, + "y": 2.3352255445188432, + "heading": 4.671035459898787, + "angularVelocity": 6.580699302188081, + "velocityX": -0.05612922880648972, + "velocityY": 1.640356815211755, + "timestamp": 3.1794917550829247 + }, + { + "x": 8.332608537566271, + "y": 2.3698002053105385, + "heading": 4.797912823563958, + "angularVelocity": 6.377260620357163, + "velocityX": 0.0002420155543911525, + "velocityY": 1.7378326311300352, + "timestamp": 3.1993870316316295 + }, + { + "x": 8.33396980572602, + "y": 2.4053616174482886, + "heading": 4.91661874197118, + "angularVelocity": 5.966537741590232, + "velocityX": 0.06842167568854002, + "velocityY": 1.787429898282315, + "timestamp": 3.2192823081803343 + }, + { + "x": 8.336666983434197, + "y": 2.4421571036885257, + "heading": 5.027955549931603, + "angularVelocity": 5.596142767247511, + "velocityX": 0.13556874676122466, + "velocityY": 1.8494583953211146, + "timestamp": 3.239177584729039 + }, + { + "x": 8.340246531495488, + "y": 2.479675325798265, + "heading": 5.127878006964601, + "angularVelocity": 5.02242111530254, + "velocityX": 0.17991949257545845, + "velocityY": 1.8857854032786403, + "timestamp": 3.259072861277744 + }, + { + "x": 8.344506054620522, + "y": 2.517833586551145, + "heading": 5.215460170451448, + "angularVelocity": 4.402158636621092, + "velocityX": 0.2140972061688483, + "velocityY": 1.9179557851063929, + "timestamp": 3.278968137826449 + }, + { + "x": 8.34934139251709, + "y": 2.556588649749756, + "heading": 5.290290323982317, + "angularVelocity": 3.7612019791572497, + "velocityX": 0.24303949154619145, + "velocityY": 1.9479529778707136, + "timestamp": 3.2988634143751536 + }, + { + "x": 8.360489719138714, + "y": 2.633118600966112, + "heading": 5.386097875540369, + "angularVelocity": 2.5082503010878967, + "velocityX": 0.2918641918155103, + "velocityY": 2.0035609934605225, + "timestamp": 3.3370603804102004 + }, + { + "x": 8.3734961558476, + "y": 2.711837506326731, + "heading": 5.434143906525109, + "angularVelocity": 1.2578494046008082, + "velocityX": 0.3405096807153113, + "velocityY": 2.0608680094746767, + "timestamp": 3.375257346445247 + }, + { + "x": 8.388354991269559, + "y": 2.792730047490086, + "heading": 5.4343775006036195, + "angularVelocity": 0.006115513946711611, + "velocityX": 0.3890056453259781, + "velocityY": 2.117773989932431, + "timestamp": 3.413454312480294 + }, + { + "x": 8.405064352616025, + "y": 2.875700202856259, + "heading": 5.386603131855514, + "angularVelocity": -1.2507372628567064, + "velocityX": 0.43745258016393523, + "velocityY": 2.1721661162838264, + "timestamp": 3.4516512785153406 + }, + { + "x": 8.423595902184939, + "y": 2.960570562063504, + "heading": 5.290443702807007, + "angularVelocity": -2.51746248537848, + "velocityX": 0.4851576314179031, + "velocityY": 2.221913623437374, + "timestamp": 3.4898482445503873 + }, + { + "x": 8.443874909764538, + "y": 3.0470824289690306, + "heading": 5.145320206366309, + "angularVelocity": -3.799346165544723, + "velocityX": 0.5309062390188924, + "velocityY": 2.264888442347729, + "timestamp": 3.528045210585434 + }, + { + "x": 8.46822613284771, + "y": 3.1277535966232044, + "heading": 4.9755072400440925, + "angularVelocity": -4.445718703585181, + "velocityX": 0.6375172064929964, + "velocityY": 2.1119784115878613, + "timestamp": 3.566242176620481 + }, + { + "x": 8.485889521575103, + "y": 3.2052285744044853, + "heading": 4.782579464036899, + "angularVelocity": -5.050866496312229, + "velocityX": 0.4624291026461906, + "velocityY": 2.02830187377173, + "timestamp": 3.6044391426555276 + }, + { + "x": 8.496466363696742, + "y": 3.2817681103248395, + "heading": 4.564012968163081, + "angularVelocity": -5.722090484183368, + "velocityX": 0.2769026762998172, + "velocityY": 2.0038119218716672, + "timestamp": 3.6426361086905743 + }, + { + "x": 8.498864361133254, + "y": 3.3553108945925123, + "heading": 4.355441183612215, + "angularVelocity": -5.460428044455047, + "velocityX": 0.06277978817249102, + "velocityY": 1.9253566945656204, + "timestamp": 3.680833074725621 + }, + { + "x": 8.49244817179446, + "y": 3.427344053675241, + "heading": 4.152515229757634, + "angularVelocity": -5.312619689961475, + "velocityX": -0.16797641291473847, + "velocityY": 1.8858345716944014, + "timestamp": 3.719030040760668 + }, + { + "x": 8.480949544401803, + "y": 3.4955676666301225, + "heading": 3.980239273291126, + "angularVelocity": -4.5102000066822105, + "velocityX": -0.3010350974500928, + "velocityY": 1.7861003120584202, + "timestamp": 3.7572270067957145 + }, + { + "x": 8.465880541095904, + "y": 3.5608797001839374, + "heading": 3.848658427278945, + "angularVelocity": -3.4447983615099473, + "velocityX": -0.39450785939576005, + "velocityY": 1.7098749019461128, + "timestamp": 3.7954239728307613 + }, + { + "x": 8.447723716276025, + "y": 3.6237520788842326, + "heading": 3.7606807673271616, + "angularVelocity": -2.3032630358929707, + "velocityX": -0.4753473038465876, + "velocityY": 1.6460045188564885, + "timestamp": 3.833620938865808 + }, + { + "x": 8.426810965043074, + "y": 3.6844175741296494, + "heading": 3.717839244299634, + "angularVelocity": -1.1215949190367396, + "velocityX": -0.5474977047591038, + "velocityY": 1.5882281118807744, + "timestamp": 3.8718179049008548 + }, + { + "x": 8.403377538803907, + "y": 3.7429507831754503, + "heading": 3.7209535641751312, + "angularVelocity": 0.08153317393427642, + "velocityX": -0.6134892027200903, + "velocityY": 1.5324046677449423, + "timestamp": 3.9100148709359015 + }, + { + "x": 8.377585411071777, + "y": 3.7993156909942623, + "heading": 3.770388538466339, + "angularVelocity": 1.2942120650590392, + "velocityX": -0.6752402195625785, + "velocityY": 1.475638346959163, + "timestamp": 3.9482118369709482 + }, + { + "x": 8.359121603366816, + "y": 3.8364193104219853, + "heading": 3.8251043907760733, + "angularVelocity": 2.1181774191421603, + "velocityX": -0.7147767767674121, + "velocityY": 1.4363670768638492, + "timestamp": 3.974043408875767 + }, + { + "x": 8.339698595709613, + "y": 3.872452168004382, + "heading": 3.901119172271844, + "angularVelocity": 2.9427083173978534, + "velocityX": -0.7519096293779934, + "velocityY": 1.3949154048838457, + "timestamp": 3.999874980780586 + }, + { + "x": 8.31940846552165, + "y": 3.907347005013746, + "heading": 3.9984628782143856, + "angularVelocity": 3.7684004016953554, + "velocityX": -0.7854779516602693, + "velocityY": 1.3508599917163486, + "timestamp": 4.025706552685405 + }, + { + "x": 8.29838582091067, + "y": 3.941024765667179, + "heading": 4.1171731868845285, + "angularVelocity": 4.595551099544112, + "velocityX": -0.8138352822058595, + "velocityY": 1.3037441460211887, + "timestamp": 4.051538124590223 + }, + { + "x": 8.276832604055052, + "y": 3.9733768568490917, + "heading": 4.2572182344953795, + "angularVelocity": 5.421468276374085, + "velocityX": -0.8343749631278609, + "velocityY": 1.2524244092121188, + "timestamp": 4.077369696495042 + }, + { + "x": 8.255105530373472, + "y": 4.004178929302719, + "heading": 4.418037307026107, + "angularVelocity": 6.225678914287337, + "velocityX": -0.841105363685868, + "velocityY": 1.1924195928580044, + "timestamp": 4.103201268399861 + }, + { + "x": 8.236226871427698, + "y": 4.03234435965831, + "heading": 4.584751742520452, + "angularVelocity": 6.45390207412214, + "velocityX": -0.7308366295065699, + "velocityY": 1.0903490681625025, + "timestamp": 4.12903284030468 + }, + { + "x": 8.22111995531124, + "y": 4.060012965907288, + "heading": 4.744248346923677, + "angularVelocity": 6.174483108922656, + "velocityX": -0.5848237254830212, + "velocityY": 1.0711158558576532, + "timestamp": 4.154864412209498 + }, + { + "x": 8.208790974794852, + "y": 4.088137716132201, + "heading": 4.888562116667079, + "angularVelocity": 5.5867204007233076, + "velocityX": -0.4772834019477146, + "velocityY": 1.0887742460483674, + "timestamp": 4.180695984114317 + }, + { + "x": 8.198725383178298, + "y": 4.116963981676649, + "heading": 5.015233890568161, + "angularVelocity": 4.903757865290863, + "velocityX": -0.3896623733794405, + "velocityY": 1.115931529473434, + "timestamp": 4.206527556019136 + }, + { + "x": 8.190656129284886, + "y": 4.146632428651468, + "heading": 5.123285713831358, + "angularVelocity": 4.182936433807978, + "velocityX": -0.31237951461661634, + "velocityY": 1.148534323971415, + "timestamp": 4.232359127923955 + }, + { + "x": 8.184413831367804, + "y": 4.177239551429493, + "heading": 5.212211909192924, + "angularVelocity": 3.4425390637949445, + "velocityX": -0.241653815729148, + "velocityY": 1.184872639218488, + "timestamp": 4.2581906998287735 + }, + { + "x": 8.179879188537598, + "y": 4.208850860595703, + "heading": 5.281700753397833, + "angularVelocity": 2.6900741643192534, + "velocityX": -0.17554653069182086, + "velocityY": 1.223747020997694, + "timestamp": 4.284022271733592 + }, + { + "x": 8.176711633108404, + "y": 4.259079542498045, + "heading": 5.341834535732932, + "angularVelocity": 1.5389696629172125, + "velocityX": -0.08106544311437222, + "velocityY": 1.2854740655637256, + "timestamp": 4.323096324838129 + }, + { + "x": 8.177255357527843, + "y": 4.31178579369655, + "heading": 5.357219600158119, + "angularVelocity": 0.3937411965947536, + "velocityX": 0.013915229576633022, + "velocityY": 1.3488810863182197, + "timestamp": 4.362170377942666 + }, + { + "x": 8.181626457446923, + "y": 4.366954653737556, + "heading": 5.328216932710745, + "angularVelocity": -0.7422487595484647, + "velocityX": 0.11186707218176441, + "velocityY": 1.411905232697765, + "timestamp": 4.401244431047203 + }, + { + "x": 8.190025046391563, + "y": 4.424472904744573, + "heading": 5.255283063236909, + "angularVelocity": -1.8665550071990427, + "velocityX": 0.21494030635039485, + "velocityY": 1.4720318584083123, + "timestamp": 4.4403184841517405 + }, + { + "x": 8.202703493077177, + "y": 4.484103126840556, + "heading": 5.138917873952012, + "angularVelocity": -2.978068053845803, + "velocityX": 0.3244722694032999, + "velocityY": 1.526082332345974, + "timestamp": 4.4793925372562775 + }, + { + "x": 8.219928011035833, + "y": 4.545471721328882, + "heading": 4.979694935972509, + "angularVelocity": -4.074902021388053, + "velocityX": 0.44081728385262037, + "velocityY": 1.570571507494814, + "timestamp": 4.5184665903608145 + }, + { + "x": 8.242002423270923, + "y": 4.608108641713836, + "heading": 4.778755843149729, + "angularVelocity": -5.142519827292898, + "velocityX": 0.5649378674905671, + "velocityY": 1.6030310502311427, + "timestamp": 4.557540643465352 + }, + { + "x": 8.271074244498946, + "y": 4.672945765445566, + "heading": 4.551673202567828, + "angularVelocity": -5.811596764082065, + "velocityX": 0.7440185728940091, + "velocityY": 1.6593396021207916, + "timestamp": 4.596614696569889 + }, + { + "x": 8.307044890775797, + "y": 4.743652286364594, + "heading": 4.33581324080306, + "angularVelocity": -5.524381133108038, + "velocityX": 0.9205762755303734, + "velocityY": 1.8095517434514046, + "timestamp": 4.635688749674426 + }, + { + "x": 8.343074596799024, + "y": 4.821913482728032, + "heading": 4.143267038130374, + "angularVelocity": -4.927725366947583, + "velocityX": 0.9220877580023329, + "velocityY": 2.0028942519492667, + "timestamp": 4.674762802778963 + }, + { + "x": 8.37162830890395, + "y": 4.904380364973872, + "heading": 3.9684903103687414, + "angularVelocity": -4.472961310003866, + "velocityX": 0.7307589010163777, + "velocityY": 2.1105279768446303, + "timestamp": 4.7138368558835 + }, + { + "x": 8.39410907934667, + "y": 4.988084023708393, + "heading": 3.8243195102940244, + "angularVelocity": -3.6896812237268355, + "velocityX": 0.5753375617977339, + "velocityY": 2.142180093541445, + "timestamp": 4.752910908988037 + }, + { + "x": 8.412533295857541, + "y": 5.0701333500179695, + "heading": 3.7241871610957364, + "angularVelocity": -2.5626302173055078, + "velocityX": 0.47152048602634916, + "velocityY": 2.099841705442333, + "timestamp": 4.791984962092574 + }, + { + "x": 8.427022508757116, + "y": 5.150508503599114, + "heading": 3.668854256125942, + "angularVelocity": -1.4161035411852148, + "velocityX": 0.370814178421956, + "velocityY": 2.0569955557492743, + "timestamp": 4.831059015197111 + }, + { + "x": 8.437687476391535, + "y": 5.229173684079572, + "heading": 3.658771745830396, + "angularVelocity": -0.25803594698946425, + "velocityX": 0.27294244612622637, + "velocityY": 2.013233187506785, + "timestamp": 4.870133068301648 + }, + { + "x": 8.444672138848645, + "y": 5.306037054642886, + "heading": 3.694319619572902, + "angularVelocity": 0.9097564987026582, + "velocityX": 0.17875449056755022, + "velocityY": 1.9671204918946052, + "timestamp": 4.909207121406185 + }, + { + "x": 8.44819450378418, + "y": 5.38096809387207, + "heading": 3.7759750997570256, + "angularVelocity": 2.0897622257323873, + "velocityX": 0.09014588085116931, + "velocityY": 1.9176674359508388, + "timestamp": 4.948281174510722 + }, + { + "x": 8.449106377133136, + "y": 5.428397890121427, + "heading": 3.847877974347001, + "angularVelocity": 2.856844558406781, + "velocityX": 0.03623054613292597, + "velocityY": 1.8844803645751356, + "timestamp": 4.973449808337294 + }, + { + "x": 8.448712080913069, + "y": 5.474928555589069, + "heading": 3.9390652949611624, + "angularVelocity": 3.623054045861213, + "velocityX": -0.015666174921664548, + "velocityY": 1.848756106043276, + "timestamp": 4.998618442163867 + }, + { + "x": 8.447075648100045, + "y": 5.520495348927803, + "heading": 4.049507624303278, + "angularVelocity": 4.388093930847932, + "velocityX": -0.06501873817623247, + "velocityY": 1.8104595447141671, + "timestamp": 5.023787075990439 + }, + { + "x": 8.444260693041544, + "y": 5.565024011584617, + "heading": 4.179082223329722, + "angularVelocity": 5.148257148929752, + "velocityX": -0.11184377657913579, + "velocityY": 1.7692125430265968, + "timestamp": 5.048955709817012 + }, + { + "x": 8.440260130020729, + "y": 5.60821579730256, + "heading": 4.326669444296132, + "angularVelocity": 5.86393452991448, + "velocityX": -0.15895034463857272, + "velocityY": 1.7160957569474122, + "timestamp": 5.074124343643584 + }, + { + "x": 8.433318308538809, + "y": 5.649473049584276, + "heading": 4.481200094717335, + "angularVelocity": 6.1398108250935115, + "velocityX": -0.275812407211048, + "velocityY": 1.639232886695548, + "timestamp": 5.0992929774701565 + }, + { + "x": 8.423269771904357, + "y": 5.688774009331345, + "heading": 4.641460146861477, + "angularVelocity": 6.367451378109614, + "velocityX": -0.3992483940007152, + "velocityY": 1.5615054840830123, + "timestamp": 5.124461611296729 + }, + { + "x": 8.409301302318001, + "y": 5.727844142412751, + "heading": 4.801252594095005, + "angularVelocity": 6.3488725027588, + "velocityX": -0.5549951452513271, + "velocityY": 1.552334280462891, + "timestamp": 5.149630245123301 + }, + { + "x": 8.396208121715038, + "y": 5.767480930820443, + "heading": 4.939805836150058, + "angularVelocity": 5.504996536950433, + "velocityX": -0.5202181688995966, + "velocityY": 1.5748486263026762, + "timestamp": 5.174798878949874 + }, + { + "x": 8.384341280819799, + "y": 5.8076306100276724, + "heading": 5.057844576112943, + "angularVelocity": 4.689914469583332, + "velocityX": -0.4714932473891813, + "velocityY": 1.5952268003057297, + "timestamp": 5.199967512776446 + }, + { + "x": 8.373739781221703, + "y": 5.848339028395987, + "heading": 5.1555398013279, + "angularVelocity": 3.881626070296041, + "velocityX": -0.42121871497465463, + "velocityY": 1.6174266211198407, + "timestamp": 5.225136146603019 + }, + { + "x": 8.364388866933238, + "y": 5.889641090063779, + "heading": 5.232909954944061, + "angularVelocity": 3.07407045409338, + "velocityX": -0.37153046736263995, + "velocityY": 1.6410132529397037, + "timestamp": 5.250304780429591 + }, + { + "x": 8.356262991610826, + "y": 5.931555344979487, + "heading": 5.289924578574465, + "angularVelocity": 2.2653046654526636, + "velocityX": -0.32285722691204854, + "velocityY": 1.665336911193694, + "timestamp": 5.2754734142561635 + }, + { + "x": 8.34934139251709, + "y": 5.974088191986084, + "heading": 5.326548029576795, + "angularVelocity": 1.4551227235728712, + "velocityX": -0.27500893141166805, + "velocityY": 1.689914808236125, + "timestamp": 5.300642048082736 + }, + { + "x": 8.343221165925502, + "y": 6.021143146137171, + "heading": 5.342226898752842, + "angularVelocity": 0.5719929961678595, + "velocityX": -0.22327673673661108, + "velocityY": 1.7166483058956983, + "timestamp": 5.328052994162652 + }, + { + "x": 8.338553091700128, + "y": 6.068939036747707, + "heading": 5.3338112220665685, + "angularVelocity": -0.3070188333426256, + "velocityX": -0.17029963912102522, + "velocityY": 1.7436789839791385, + "timestamp": 5.3554639402425686 + }, + { + "x": 8.335401457891965, + "y": 6.11747737229338, + "heading": 5.301487591207973, + "angularVelocity": -1.1792234665799601, + "velocityX": -0.1149771992171805, + "velocityY": 1.7707646939350166, + "timestamp": 5.382874886322485 + }, + { + "x": 8.333863965332327, + "y": 6.166745972097111, + "heading": 5.245527796242896, + "angularVelocity": -2.0415127154650743, + "velocityX": -0.05609045945204774, + "velocityY": 1.7974060311558808, + "timestamp": 5.410285832402401 + }, + { + "x": 8.334074611539513, + "y": 6.216707641134325, + "heading": 5.16630466616314, + "angularVelocity": -2.890200500514666, + "velocityX": 0.007684747785513327, + "velocityY": 1.8226904278148732, + "timestamp": 5.437696778482318 + }, + { + "x": 8.336209337170322, + "y": 6.2672871852177625, + "heading": 5.064334139872564, + "angularVelocity": -3.7200659179468194, + "velocityX": 0.07787858268680638, + "velocityY": 1.845231606963613, + "timestamp": 5.465107724562234 + }, + { + "x": 8.34050875980612, + "y": 6.318358761986823, + "heading": 4.9404202596625035, + "angularVelocity": -4.520598444460426, + "velocityX": 0.1568505743385533, + "velocityY": 1.8631818332778705, + "timestamp": 5.49251867064215 + }, + { + "x": 8.347373076329852, + "y": 6.369731891617242, + "heading": 4.796226953445604, + "angularVelocity": -5.260427925271259, + "velocityX": 0.2504224591051665, + "velocityY": 1.874183017274954, + "timestamp": 5.519929616722067 + }, + { + "x": 8.357734742408361, + "y": 6.421155881623346, + "heading": 4.637107940656009, + "angularVelocity": -5.804944211910156, + "velocityX": 0.37801198281516796, + "velocityY": 1.876038494117501, + "timestamp": 5.547340562801983 + }, + { + "x": 8.372714822209472, + "y": 6.472180587497517, + "heading": 4.476004426648119, + "angularVelocity": -5.877342341201648, + "velocityX": 0.546499918588575, + "velocityY": 1.8614718997808002, + "timestamp": 5.574751508881899 + }, + { + "x": 8.392163518645132, + "y": 6.523832787745315, + "heading": 4.319730445846118, + "angularVelocity": -5.701152391689906, + "velocityX": 0.7095229905220347, + "velocityY": 1.8843640090788911, + "timestamp": 5.602162454961816 + }, + { + "x": 8.409770328909872, + "y": 6.578150492835037, + "heading": 4.178613291017239, + "angularVelocity": -5.148204458811868, + "velocityX": 0.642327711469996, + "velocityY": 1.9816063601511025, + "timestamp": 5.629573401041732 + }, + { + "x": 8.424485615502517, + "y": 6.635136223401266, + "heading": 4.049096201405813, + "angularVelocity": -4.725013475777802, + "velocityX": 0.5368397920211534, + "velocityY": 2.0789406684499903, + "timestamp": 5.656984347121648 + }, + { + "x": 8.435886735748374, + "y": 6.694743962812389, + "heading": 3.9283767340305045, + "angularVelocity": -4.404060590369644, + "velocityX": 0.41593311710644926, + "velocityY": 2.1745962082934467, + "timestamp": 5.684395293201565 + }, + { + "x": 8.4437690723566, + "y": 6.756967833759458, + "heading": 3.814495508904072, + "angularVelocity": -4.154589367124023, + "velocityX": 0.28756163998295975, + "velocityY": 2.2700373334672657, + "timestamp": 5.711806239281481 + }, + { + "x": 8.44819450378418, + "y": 6.821402072906494, + "heading": 3.7101172753280305, + "angularVelocity": -3.8079033562623543, + "velocityX": 0.16144759887806323, + "velocityY": 2.350675491432431, + "timestamp": 5.739217185361397 + }, + { + "x": 8.442296007576482, + "y": 7.005937519917937, + "heading": 3.5641292983287713, + "angularVelocity": -1.9075537985667739, + "velocityX": -0.07707277734850891, + "velocityY": 2.41123481640327, + "timestamp": 5.815748697662097 + }, + { + "x": 8.41659744566655, + "y": 7.180425726585724, + "heading": 3.5626950047371615, + "angularVelocity": -0.01874121585334014, + "velocityX": -0.3357905931475581, + "velocityY": 2.279952419889553, + "timestamp": 5.892280209962797 + }, + { + "x": 8.38385523357143, + "y": 7.320664328992628, + "heading": 3.601572492187995, + "angularVelocity": 0.5079931949871856, + "velocityX": -0.4278265398241839, + "velocityY": 1.8324295207429526, + "timestamp": 5.968811722263497 + }, + { + "x": 8.354355980334272, + "y": 7.424774646283286, + "heading": 3.6482652707450245, + "angularVelocity": 0.6101117977855888, + "velocityX": -0.38545237576453223, + "velocityY": 1.3603588137863938, + "timestamp": 6.045343234564196 + }, + { + "x": 8.33265202622526, + "y": 7.4934610231290915, + "heading": 3.6869536728033228, + "angularVelocity": 0.5055225082484697, + "velocityX": -0.2835949984071486, + "velocityY": 0.8974914356315113, + "timestamp": 6.121874746864896 + }, + { + "x": 8.321097373962402, + "y": 7.527496814727783, + "heading": 3.709032536356956, + "angularVelocity": 0.2884937575371967, + "velocityX": -0.15097901394471921, + "velocityY": 0.44472911321759667, + "timestamp": 6.198406259165596 + }, + { + "x": 8.321097373962402, + "y": 7.527496814727783, + "heading": 3.709032536356956, + "angularVelocity": -5.082729725441752e-29, + "velocityX": 4.680465584518782e-28, + "velocityY": 3.556470741173404e-29, + "timestamp": 6.274937771466296 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.4578553438186646, + "y": 1.4268364906311035, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 0.7098978675678107, + "isStopPoint": false, + "x": 3.095994472503662, + "y": 1.2997395992279053, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 1.6119051391146006, + "isStopPoint": false, + "x": 6.202812194824219, + "y": 1.003179669380188, + "heading": 0.7165423509110156, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 2.314676799081464, + "isStopPoint": false, + "x": 8.25048828125, + "y": 0.7348634600639343, + "heading": 0.7165423509110156, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "timestamp": 3.040224819241991, + "isStopPoint": false, + "x": 8.377585411071777, + "y": 2.1329314708709717, + "heading": 3.7458973568177814, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 3.2988634143751536, + "isStopPoint": false, + "x": 8.34934139251709, + "y": 2.556588649749756, + "heading": 5.290290323982317, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 3.9482118369709482, + "isStopPoint": false, + "x": 8.377585411071777, + "y": 3.7993156909942627, + "heading": 3.770388538466339, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 4.284022271733592, + "isStopPoint": false, + "x": 8.179879188537598, + "y": 4.208850860595703, + "heading": 5.281700753397833, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 4.948281174510722, + "isStopPoint": false, + "x": 8.44819450378418, + "y": 5.38096809387207, + "heading": 3.7759750997570256, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 5.300642048082736, + "isStopPoint": false, + "x": 8.34934139251709, + "y": 5.974088191986084, + "heading": 5.326548029576795, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 5.739217185361397, + "isStopPoint": false, + "x": 8.44819450378418, + "y": 6.821402072906494, + "heading": 3.7101172753280305, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 6.274937771466296, + "isStopPoint": true, + "x": 8.321097373962402, + "y": 7.527496814727783, + "heading": 3.7090325363569563, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "direction": 0 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "AmpLanePdChaos": { + "waypoints": [ + { + "x": 0.4269569218158722, + "y": 6.9908647537231445, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 1.5002212524414062, + "y": 6.694304943084717, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 3.8162126541137695, + "y": 6.581329345703125, + "heading": 0.2985001179522956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 8.264610290527344, + "y": 7.456887245178223, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 8.377585411071777, + "y": 6.553085803985596, + "heading": 1.0808392342145412, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.4269569218158722, + "y": 6.9908647537231445, + "heading": 1.0147582935939444e-26, + "angularVelocity": 4.802793352791378e-28, + "velocityX": 7.569777584926927e-27, + "velocityY": 3.719655610214095e-26, + "timestamp": 0 + }, + { + "x": 0.45154320453389124, + "y": 6.9860595473156515, + "heading": -0.0020588646196638234, + "angularVelocity": -0.03238986683838223, + "velocityX": 0.38678911458368936, + "velocityY": -0.07559506058978287, + "timestamp": 0.06356508441164857 + }, + { + "x": 0.500665330018184, + "y": 6.976191418092733, + "heading": -0.005903955941845083, + "angularVelocity": -0.06049061930415103, + "velocityX": 0.7727847125345899, + "velocityY": -0.15524449175608063, + "timestamp": 0.12713016882329714 + }, + { + "x": 0.5742528189740612, + "y": 6.960923422240335, + "heading": -0.01117745758299664, + "angularVelocity": -0.08296223768067817, + "velocityX": 1.1576715367720332, + "velocityY": -0.24019469168832944, + "timestamp": 0.1906952532349457 + }, + { + "x": 0.6722013148089163, + "y": 6.9397965653748726, + "heading": -0.017389498609317183, + "angularVelocity": -0.09772725205698236, + "velocityX": 1.5409166327938608, + "velocityY": -0.3323657486025543, + "timestamp": 0.2542603376465943 + }, + { + "x": 0.7943433521763879, + "y": 6.912149808159037, + "heading": -0.023829197654655073, + "angularVelocity": -0.10130874685282076, + "velocityX": 1.9215271795515552, + "velocityY": -0.43493621493200585, + "timestamp": 0.3178254220582428 + }, + { + "x": 0.940376377090631, + "y": 6.876952520080898, + "heading": -0.029373547858602904, + "angularVelocity": -0.08722320209696448, + "velocityX": 2.2973779751243755, + "velocityY": -0.553720464684682, + "timestamp": 0.38139050646989137 + }, + { + "x": 1.109639548430514, + "y": 6.832391950283203, + "heading": -0.03199489164843748, + "angularVelocity": -0.04123873686470252, + "velocityX": 2.6628324796004614, + "velocityY": -0.7010227424401742, + "timestamp": 0.4449555908815399 + }, + { + "x": 1.3001020816519337, + "y": 6.774603331599785, + "heading": -0.02710028887606378, + "angularVelocity": 0.07700143589326758, + "velocityX": 2.9963388703770333, + "velocityY": -0.909125178048654, + "timestamp": 0.5085206752931885 + }, + { + "x": 1.5002212524414062, + "y": 6.694304943084717, + "heading": 4.6927585573960014e-26, + "angularVelocity": 0.426339225801414, + "velocityX": 3.148256195075543, + "velocityY": -1.2632467849024598, + "timestamp": 0.572085759704837 + }, + { + "x": 1.6698109310260505, + "y": 6.606230489693705, + "heading": 0.043636088677614475, + "angularVelocity": 0.7393180152975136, + "velocityX": 2.8733259186555666, + "velocityY": -1.492228842060727, + "timestamp": 0.6311078419140127 + }, + { + "x": 1.8178691248742131, + "y": 6.519462655469564, + "heading": 0.09197841408379424, + "angularVelocity": 0.8190548960108419, + "velocityX": 2.508522036268403, + "velocityY": -1.4700910401065466, + "timestamp": 0.6901299241231884 + }, + { + "x": 1.945478769087034, + "y": 6.439654918114978, + "heading": 0.13899529896762525, + "angularVelocity": 0.7965982073828233, + "velocityX": 2.1620661189242583, + "velocityY": -1.3521674323814197, + "timestamp": 0.7491520063323641 + }, + { + "x": 2.05361266916171, + "y": 6.369142283614203, + "heading": 0.1819641027220741, + "angularVelocity": 0.7280123327768478, + "velocityX": 1.8320922615275888, + "velocityY": -1.1946822589361732, + "timestamp": 0.8081740885415398 + }, + { + "x": 2.1429230510824855, + "y": 6.309142790648472, + "heading": 0.2193838977387365, + "angularVelocity": 0.6339965249623981, + "velocityX": 1.5131689458914253, + "velocityY": -1.0165600859876827, + "timestamp": 0.8671961707507155 + }, + { + "x": 2.2138592843219276, + "y": 6.2603940232890976, + "heading": 0.25031075711822803, + "angularVelocity": 0.5239879418331249, + "velocityX": 1.2018592124222665, + "velocityY": -0.8259411653185552, + "timestamp": 0.9262182529598912 + }, + { + "x": 2.2667458111920182, + "y": 6.2233878173394785, + "heading": 0.2740979405057225, + "angularVelocity": 0.4030217589273135, + "velocityX": 0.8960464438150413, + "velocityY": -0.6269891634535774, + "timestamp": 0.985240335169067 + }, + { + "x": 2.301826622003525, + "y": 6.198474614172422, + "heading": 0.2902775915680301, + "angularVelocity": 0.27412877446387834, + "velocityX": 0.5943675569963707, + "velocityY": -0.4220996995457338, + "timestamp": 1.0442624173782427 + }, + { + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "angularVelocity": 0.1393127127424044, + "velocityX": 0.2959016450874765, + "velocityY": -0.2127710494914169, + "timestamp": 1.1032844995874185 + }, + { + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "angularVelocity": -6.615842884891707e-27, + "velocityX": 2.924635996734034e-23, + "velocityY": -9.88114513956692e-23, + "timestamp": 1.162306581796594 + }, + { + "x": 2.347136097791038, + "y": 6.1936465997058665, + "heading": 0.2985001179522956, + "angularVelocity": 1.0620750257524312e-16, + "velocityX": 0.4079003982651746, + "velocityY": 0.11324010619816775, + "timestamp": 1.2305701693874758 + }, + { + "x": 2.402825586267742, + "y": 6.209106951340729, + "heading": 0.2985001179522956, + "angularVelocity": 1.523734708742185e-16, + "velocityX": 0.8158007869504784, + "velocityY": 0.22648020973522806, + "timestamp": 1.2988337569783575 + }, + { + "x": 2.486359817783878, + "y": 6.2322974784599845, + "heading": 0.2985001179522956, + "angularVelocity": 1.8006542596605785e-16, + "velocityX": 1.2237011628626233, + "velocityY": 0.3397203097241459, + "timestamp": 1.3670973445692391 + }, + { + "x": 2.5977387911187284, + "y": 6.263218180724542, + "heading": 0.2985001179522956, + "angularVelocity": 1.3885125036469104e-16, + "velocityX": 1.6316015208923507, + "velocityY": 0.45296040474566596, + "timestamp": 1.4353609321601208 + }, + { + "x": 2.7369625044412174, + "y": 6.301869057625762, + "heading": 0.2985001179522956, + "angularVelocity": 1.7915030698796464e-16, + "velocityX": 2.0395018520984625, + "velocityY": 0.5662004923160927, + "timestamp": 1.5036245197510025 + }, + { + "x": 2.904030954699552, + "y": 6.348250108315915, + "heading": 0.2985001179522956, + "angularVelocity": 1.7115442013810006e-16, + "velocityX": 2.447402138598578, + "velocityY": 0.6794405674680408, + "timestamp": 1.5718881073418842 + }, + { + "x": 3.098944135790158, + "y": 6.402361331099544, + "heading": 0.2985001179522956, + "angularVelocity": 1.731300305768192e-16, + "velocityX": 2.8553023356868215, + "velocityY": 0.7926806177830709, + "timestamp": 1.640151694932766 + }, + { + "x": 3.3217020294023807, + "y": 6.464202720890301, + "heading": 0.2985001179522956, + "angularVelocity": -4.291109894147179e-17, + "velocityX": 3.263202264540478, + "velocityY": 0.9059205935876805, + "timestamp": 1.7084152825236476 + }, + { + "x": 3.5667175760590744, + "y": 6.53222236284647, + "heading": 0.2985001179522956, + "angularVelocity": 2.176874790341123e-16, + "velocityX": 3.589256810308954, + "velocityY": 0.9964264164348671, + "timestamp": 1.7766788701145293 + }, + { + "x": 3.8162126541137695, + "y": 6.581329345703125, + "heading": 0.2985001179522956, + "angularVelocity": -3.979347495805123e-16, + "velocityX": 3.6548779057727367, + "velocityY": 0.7193730155374103, + "timestamp": 1.844942457705411 + }, + { + "x": 4.06911506298554, + "y": 6.631106978006357, + "heading": 0.2985000768137015, + "angularVelocity": -5.945239455017304e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.914138314964857 + }, + { + "x": 4.322017471857311, + "y": 6.6808846103095885, + "heading": 0.2985000356751073, + "angularVelocity": -5.945239469422448e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.983334172224303 + }, + { + "x": 4.574919880729083, + "y": 6.730662242612821, + "heading": 0.29849999453651305, + "angularVelocity": -5.945239484047347e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.0525300294837487 + }, + { + "x": 4.827822289600854, + "y": 6.780439874916052, + "heading": 0.29849995339791874, + "angularVelocity": -5.945239490070907e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.1217258867431945 + }, + { + "x": 5.080724698472625, + "y": 6.830217507219284, + "heading": 0.29849991225932443, + "angularVelocity": -5.945239490087136e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.1909217440026403 + }, + { + "x": 5.3336271073443955, + "y": 6.879995139522516, + "heading": 0.29849987112073, + "angularVelocity": -5.945239499004978e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020961, + "timestamp": 2.260117601262086 + }, + { + "x": 5.586529516216166, + "y": 6.929772771825747, + "heading": 0.2984998299821356, + "angularVelocity": -5.945239509711758e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020961, + "timestamp": 2.329313458521532 + }, + { + "x": 5.839431925087938, + "y": 6.979550404128979, + "heading": 0.2984997888435411, + "angularVelocity": -5.945239507812541e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.3985093157809776 + }, + { + "x": 6.092334333959709, + "y": 7.029328036432211, + "heading": 0.2984997477049465, + "angularVelocity": -5.945239524665447e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.4677051730404234 + }, + { + "x": 6.34523674283148, + "y": 7.079105668735443, + "heading": 0.29849970656635194, + "angularVelocity": -5.945239530479041e-7, + "velocityX": 3.6548778913675166, + "velocityY": 0.7193730127020963, + "timestamp": 2.536901030299869 + }, + { + "x": 6.598139151703251, + "y": 7.128883301038674, + "heading": 0.29849966542775724, + "angularVelocity": -5.945239544565078e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020962, + "timestamp": 2.606096887559315 + }, + { + "x": 6.8510415605750214, + "y": 7.178660933341907, + "heading": 0.29849962428916255, + "angularVelocity": -5.945239549845622e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.6752927448187607 + }, + { + "x": 7.103943969446777, + "y": 7.228438565645134, + "heading": 0.29849958315051783, + "angularVelocity": -5.945246772244776e-7, + "velocityX": 3.654877891367282, + "velocityY": 0.7193730127020502, + "timestamp": 2.7444886020782064 + }, + { + "x": 7.353433038122826, + "y": 7.27754436570309, + "heading": 0.2857400994824136, + "angularVelocity": -0.18439664126514535, + "velocityX": 3.605549212875623, + "velocityY": 0.7096638729950924, + "timestamp": 2.813684459337652 + }, + { + "x": 7.577591625127487, + "y": 7.32166448211996, + "heading": 0.20789341833491348, + "angularVelocity": -1.1250193903316845, + "velocityX": 3.239479874701043, + "velocityY": 0.6376121080695129, + "timestamp": 2.882880316597098 + }, + { + "x": 7.772618972429747, + "y": 7.360050828957839, + "heading": 0.13154587005744633, + "angularVelocity": -1.1033543235284506, + "velocityX": 2.818482999220841, + "velocityY": 0.5547492054900108, + "timestamp": 2.9520761738565438 + }, + { + "x": 7.938688452207964, + "y": 7.392737530275028, + "heading": 0.06829117903161311, + "angularVelocity": -0.9141398565041748, + "velocityX": 2.3999916520370626, + "velocityY": 0.47237945466347603, + "timestamp": 3.0212720311159895 + }, + { + "x": 8.075968311155844, + "y": 7.41975770130397, + "heading": 0.023429532114866793, + "angularVelocity": -0.6483285082882946, + "velocityX": 1.9839317610179508, + "velocityY": 0.3904882763086947, + "timestamp": 3.0904678883754353 + }, + { + "x": 8.184579349560657, + "y": 7.441135118616467, + "heading": 4.707584249891564e-24, + "angularVelocity": -0.33859732421579963, + "velocityX": 1.5696176434028992, + "velocityY": 0.3089407106026808, + "timestamp": 3.159663745634881 + }, + { + "x": 8.264610290527344, + "y": 7.456887245178223, + "heading": 4.305949741153467e-24, + "angularVelocity": 2.0877324046258187e-24, + "velocityX": 1.156585728342302, + "velocityY": 0.22764551500092414, + "timestamp": 3.228859602894327 + }, + { + "x": 8.309090579989265, + "y": 7.457165429033817, + "heading": 0.013618338743485297, + "angularVelocity": 0.3126833682896741, + "velocityX": 1.021288058215364, + "velocityY": 0.006387230234869135, + "timestamp": 3.2724127308332234 + }, + { + "x": 8.347694581932599, + "y": 7.44777021311996, + "heading": 0.040485197139552155, + "angularVelocity": 0.6168755188780024, + "velocityX": 0.886365773716491, + "velocityY": -0.2157185111259548, + "timestamp": 3.31596585877212 + }, + { + "x": 8.38043908621254, + "y": 7.428658664796906, + "heading": 0.08013490839873996, + "angularVelocity": 0.9103757441902883, + "velocityX": 0.7518289920733539, + "velocityY": -0.4388100057903166, + "timestamp": 3.3595189867110165 + }, + { + "x": 8.407344118270622, + "y": 7.3997816960160065, + "heading": 0.13202626739740458, + "angularVelocity": 1.1914496490692066, + "velocityX": 0.6177520038475312, + "velocityY": -0.6630285847990697, + "timestamp": 3.403072114649913 + }, + { + "x": 8.428437067602909, + "y": 7.361082970163004, + "heading": 0.19556012614557247, + "angularVelocity": 1.4587668384531083, + "velocityX": 0.4843038911436893, + "velocityY": -0.8885406785775682, + "timestamp": 3.4466252425888095 + }, + { + "x": 8.443758202953443, + "y": 7.312496178717053, + "heading": 0.2700814705675102, + "angularVelocity": 1.7110446011245937, + "velocityX": 0.35178036746362773, + "velocityY": -1.1155752467220204, + "timestamp": 3.490178370527706 + }, + { + "x": 8.453367300003004, + "y": 7.253940317433782, + "heading": 0.354848982447824, + "angularVelocity": 1.9463013540437195, + "velocityX": 0.22062932111422345, + "velocityY": -1.3444697098546596, + "timestamp": 3.5337314984666026 + }, + { + "x": 8.45735124333647, + "y": 7.18531320673646, + "heading": 0.4489562663311165, + "angularVelocity": 2.1607468472831988, + "velocityX": 0.09147318509605634, + "velocityY": -1.5757102634190636, + "timestamp": 3.577284626405499 + }, + { + "x": 8.455833781714013, + "y": 7.106484187240998, + "heading": 0.5511965710396762, + "angularVelocity": 2.347484774273826, + "velocityX": -0.03484162204347062, + "velocityY": -1.8099508169896654, + "timestamp": 3.6208377543443957 + }, + { + "x": 8.448992060952849, + "y": 7.01728730637675, + "heading": 0.6598709993297996, + "angularVelocity": 2.4952152332799042, + "velocityX": -0.15708907913026152, + "velocityY": -2.0480017184847705, + "timestamp": 3.6643908822832922 + }, + { + "x": 8.437091795531597, + "y": 6.917516589235624, + "heading": 0.7725277452001486, + "angularVelocity": 2.5866510903281643, + "velocityX": -0.2732356086558815, + "velocityY": -2.2907818993184903, + "timestamp": 3.707944010222189 + }, + { + "x": 8.420568790056215, + "y": 6.806929386712691, + "heading": 0.8855178727468727, + "angularVelocity": 2.594305688106834, + "velocityX": -0.37937586247679217, + "velocityY": -2.539133416044943, + "timestamp": 3.7514971381610853 + }, + { + "x": 8.40022655182027, + "y": 6.685307877393552, + "heading": 0.992766575428426, + "angularVelocity": 2.462479912625774, + "velocityX": -0.46706721649209976, + "velocityY": -2.7924862133844903, + "timestamp": 3.795050266099982 + }, + { + "x": 8.377585411071777, + "y": 6.553085803985596, + "heading": 1.0808392342145412, + "angularVelocity": 2.0221890586062603, + "velocityX": -0.519851083491793, + "velocityY": -3.035880077165969, + "timestamp": 3.8386033940388784 + }, + { + "x": 8.331156557203096, + "y": 6.248001091638873, + "heading": 1.1393178439925227, + "angularVelocity": 0.6668090360061052, + "velocityX": -0.5294103161580237, + "velocityY": -3.478763324102056, + "timestamp": 3.9263025801930476 + }, + { + "x": 8.32509627497856, + "y": 5.9213777688010225, + "heading": 1.1393178431081437, + "angularVelocity": -1.0084234488389344e-8, + "velocityX": -0.06910306116049933, + "velocityY": -3.7243597935295467, + "timestamp": 4.014001766347217 + }, + { + "x": 8.328579577192095, + "y": 5.594716799276784, + "heading": 1.13931784103149, + "angularVelocity": -2.3679282154855844e-8, + "velocityX": 0.03971875186402571, + "velocityY": -3.7247890641766106, + "timestamp": 4.101700952501386 + }, + { + "x": 8.33206298187959, + "y": 5.268055830845277, + "heading": 1.139317838954836, + "angularVelocity": -2.3679282513207347e-8, + "velocityX": 0.03971992033507496, + "velocityY": -3.724789051716606, + "timestamp": 4.1894001386555555 + }, + { + "x": 8.335546386568366, + "y": 4.941394862413785, + "heading": 1.1393178368781822, + "angularVelocity": -2.3679282515836517e-8, + "velocityX": 0.0397199203496886, + "velocityY": -3.7247890517164506, + "timestamp": 4.277099324809725 + }, + { + "x": 8.339029791257143, + "y": 4.614733893982293, + "heading": 1.1393178348015287, + "angularVelocity": -2.3679282075329618e-8, + "velocityX": 0.039719920349696757, + "velocityY": -3.7247890517164497, + "timestamp": 4.364798510963894 + }, + { + "x": 8.34251319594592, + "y": 4.2880729255508, + "heading": 1.139317832724875, + "angularVelocity": -2.3679282013341874e-8, + "velocityX": 0.03971992034969426, + "velocityY": -3.72478905171645, + "timestamp": 4.452497697118063 + }, + { + "x": 8.345996600634695, + "y": 3.9614119571193074, + "heading": 1.139317830648221, + "angularVelocity": -2.3679283402849396e-8, + "velocityX": 0.039719920349694765, + "velocityY": -3.7247890517164497, + "timestamp": 4.540196883272232 + }, + { + "x": 8.349480005323471, + "y": 3.6347509886878155, + "heading": 1.139317828571567, + "angularVelocity": -2.3679282271688366e-8, + "velocityX": 0.03971992034969471, + "velocityY": -3.7247890517164506, + "timestamp": 4.627896069426401 + }, + { + "x": 8.352963410012249, + "y": 3.3080900202563233, + "heading": 1.139317826494913, + "angularVelocity": -2.367928303185141e-8, + "velocityX": 0.03971992034969481, + "velocityY": -3.72478905171645, + "timestamp": 4.7155952555805705 + }, + { + "x": 8.356446814701025, + "y": 2.981429051824831, + "heading": 1.1393178244182591, + "angularVelocity": -2.3679282848596997e-8, + "velocityX": 0.03971992034969499, + "velocityY": -3.72478905171645, + "timestamp": 4.80329444173474 + }, + { + "x": 8.3599302193898, + "y": 2.6547680833933387, + "heading": 1.1393178223416056, + "angularVelocity": -2.3679282487737324e-8, + "velocityX": 0.0397199203496951, + "velocityY": -3.72478905171645, + "timestamp": 4.890993627888909 + }, + { + "x": 8.363413624078582, + "y": 2.3281071149618464, + "heading": 1.1393178202649519, + "angularVelocity": -2.3679282618768576e-8, + "velocityX": 0.039719920349732145, + "velocityY": -3.7247890517164497, + "timestamp": 4.978692814043078 + }, + { + "x": 8.366897029211325, + "y": 2.001446146535259, + "heading": 1.139317818188219, + "angularVelocity": -2.3680183399714216e-8, + "velocityX": 0.03971992541207313, + "velocityY": -3.724789051660523, + "timestamp": 5.066392000197247 + }, + { + "x": 8.369950836003147, + "y": 1.715318694035658, + "heading": 1.1226153865662416, + "angularVelocity": -0.190451386773599, + "velocityX": 0.03482138119793182, + "velocityY": -3.262601000613718, + "timestamp": 5.154091186351416 + }, + { + "x": 8.372495686328486, + "y": 1.4768791360484652, + "heading": 1.1086925029820063, + "angularVelocity": -0.15875727238517048, + "velocityX": 0.029017946881124475, + "velocityY": -2.718834329523107, + "timestamp": 5.241790372505585 + }, + { + "x": 8.374531573132348, + "y": 1.2861274855011333, + "heading": 1.0975518982604389, + "angularVelocity": -0.12703201945321188, + "velocityX": 0.02321443211894871, + "velocityY": -2.1750675110257403, + "timestamp": 5.329489558659755 + }, + { + "x": 8.376058491392191, + "y": 1.1430637450377992, + "heading": 1.0891955157796347, + "angularVelocity": -0.09528460693026461, + "velocityX": 0.017410860086641132, + "velocityY": -1.6313006623783024, + "timestamp": 5.417188744813924 + }, + { + "x": 8.377076437882634, + "y": 1.0476879159528336, + "heading": 1.0836245099802884, + "angularVelocity": -0.06352403076526633, + "velocityX": 0.01160725127658835, + "velocityY": -1.0875337989716487, + "timestamp": 5.504887930968093 + }, + { + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "angularVelocity": -0.03175942546206756, + "velocityX": 0.005803624998845044, + "velocityY": -0.5437669155675129, + "timestamp": 5.592587117122262 + }, + { + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "angularVelocity": 4.871302260765609e-25, + "velocityX": -2.093358596809338e-24, + "velocityY": 4.245353600645279e-24, + "timestamp": 5.680286303276431 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.4269569218158722, + "y": 6.9908647537231445, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 0.572085759704837, + "isStopPoint": false, + "x": 1.5002212524414062, + "y": 6.694304943084717, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 1.162306581796594, + "isStopPoint": true, + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 1.844942457705411, + "isStopPoint": false, + "x": 3.8162126541137695, + "y": 6.581329345703125, + "heading": 0.2985001179522956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "timestamp": 3.228859602894327, + "isStopPoint": false, + "x": 8.264610290527344, + "y": 7.456887245178223, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 3.8386033940388784, + "isStopPoint": false, + "x": 8.377585411071777, + "y": 6.553085803985596, + "heading": 1.0808392342145412, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "timestamp": 5.680286303276431, + "isStopPoint": true, + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + 4, + 4 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 3, + 4 + ], + "type": "StraightLine" + }, + { + "scope": [ + 2 + ], + "type": "WptVelocityDirection", + "direction": 0.2985001179522956 + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/deploy/choreo/AmpLanePADEF.1.traj b/src/main/deploy/choreo/AmpLanePADEF.1.traj new file mode 100644 index 00000000..9b708483 --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePADEF.1.traj @@ -0,0 +1,185 @@ +{ + "samples": [ + { + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": -2.0223823950436125e-24, + "angularVelocity": -1.0530930101357375e-37, + "velocityX": 9.679119214488622e-24, + "velocityY": 2.8468005585003605e-23, + "timestamp": 0 + }, + { + "x": 0.45358539197474673, + "y": 6.976442037280005, + "heading": -4.051643418810716e-24, + "angularVelocity": -1.1805501930230392e-25, + "velocityX": 0.3537634154949626, + "velocityY": -0.0735607445986989, + "timestamp": 0.05826629620784151 + }, + { + "x": 0.49483104636042347, + "y": 6.967970532727312, + "heading": -6.080913911985301e-24, + "angularVelocity": -1.1821753877066713e-25, + "velocityX": 0.7078818643036704, + "velocityY": -0.1453928789719994, + "timestamp": 0.11653259241568302 + }, + { + "x": 0.5567354919848906, + "y": 6.95544199101986, + "heading": -8.110182775739579e-24, + "angularVelocity": -1.1818957371343575e-25, + "velocityX": 1.0624400322898162, + "velocityY": -0.21502210579442033, + "timestamp": 0.17479888862352452 + }, + { + "x": 0.6393312280569997, + "y": 6.9390255615072824, + "heading": -1.013944960655158e-23, + "angularVelocity": -1.1815468317795835e-25, + "velocityX": 1.4175559705645637, + "velocityY": -0.28174829328466466, + "timestamp": 0.23306518483136604 + }, + { + "x": 0.7426606798558012, + "y": 6.918954250671649, + "heading": -1.2168715161261342e-23, + "angularVelocity": -1.181327819705339e-25, + "velocityX": 1.7734000361068998, + "velocityY": -0.3444754882657602, + "timestamp": 0.29133148103920753 + }, + { + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": -1.4479605554027417e-23, + "angularVelocity": -4.951541740242581e-24, + "velocityX": 2.130226970895328, + "velocityY": -0.4013456783802647, + "timestamp": 0.34959777724704905 + }, + { + "x": 1.0109741912909123, + "y": 6.869549473554426, + "heading": 0.010039096014815363, + "angularVelocity": 0.17297490390919232, + "velocityX": 2.484465074043925, + "velocityY": -0.44832534814487107, + "timestamp": 0.4076356531928316 + }, + { + "x": 1.1757265724080805, + "y": 6.8408027772938045, + "heading": 0.030114435462749452, + "angularVelocity": 0.3459006574721642, + "velocityX": 2.83870452583543, + "velocityY": -0.49530924059791875, + "timestamp": 0.4656735291386142 + }, + { + "x": 1.3610383679326326, + "y": 6.809328854359183, + "heading": 0.060221992176283975, + "angularVelocity": 0.5187570396556241, + "velocityX": 3.1929458565586635, + "velocityY": -0.5422997038007265, + "timestamp": 0.5237114050843967 + }, + { + "x": 1.546079908472426, + "y": 6.784713461447441, + "heading": 0.1506024575679624, + "angularVelocity": 1.5572669385094218, + "velocityX": 3.188289328724817, + "velocityY": -0.4241263573246083, + "timestamp": 0.5817492810301793 + }, + { + "x": 1.710561311165979, + "y": 6.762828401312947, + "heading": 0.2309691373975153, + "angularVelocity": 1.3847281369261228, + "velocityX": 2.834035533057883, + "velocityY": -0.37708237556692, + "timestamp": 0.6397871569759619 + }, + { + "x": 1.8544824093337007, + "y": 6.743675274338349, + "heading": 0.3013204880160867, + "angularVelocity": 1.2121627380762827, + "velocityX": 2.4797788654803434, + "velocityY": -0.3300108190122258, + "timestamp": 0.6978250329217445 + }, + { + "x": 1.9778431850692357, + "y": 6.727255600598568, + "heading": 0.36164875379241124, + "angularVelocity": 1.0394637087112153, + "velocityX": 2.12552188937403, + "velocityY": -0.28291307137290855, + "timestamp": 0.755862908867527 + }, + { + "x": 2.0806437247377927, + "y": 6.713570715572485, + "heading": 0.4119419860379565, + "angularVelocity": 0.8665588019197638, + "velocityX": 1.7712664013512611, + "velocityY": -0.23579231326223515, + "timestamp": 0.8139007848133096 + }, + { + "x": 2.16288415503368, + "y": 6.702621710000965, + "heading": 0.45218713038219516, + "angularVelocity": 0.6934289666602311, + "velocityX": 1.4170130962875669, + "velocityY": -0.18865276154743782, + "timestamp": 0.8719386607590922 + }, + { + "x": 2.2245645766627815, + "y": 6.694409403887936, + "heading": 0.48237322491294377, + "angularVelocity": 0.5201102562565806, + "velocityX": 1.062761526399103, + "velocityY": -0.14149908106046907, + "timestamp": 0.9299765367048748 + }, + { + "x": 2.265685008078599, + "y": 6.688934342765243, + "heading": 0.5024941236633007, + "angularVelocity": 0.3466856500598576, + "velocityX": 0.7085102744668164, + "velocityY": -0.09433600099024221, + "timestamp": 0.9880144126506574 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.1732712607555392, + "velocityX": 0.35425724418212445, + "velocityY": -0.04716814104583245, + "timestamp": 1.0460522885964398 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -7.633423863817013e-25, + "velocityX": -5.0744692988574966e-24, + "velocityY": 1.0116864366107383e-24, + "timestamp": 1.1040901645422223 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEF.2.traj b/src/main/deploy/choreo/AmpLanePADEF.2.traj new file mode 100644 index 00000000..0346e120 --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePADEF.2.traj @@ -0,0 +1,86 @@ +{ + "samples": [ + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -7.633423863817013e-25, + "velocityX": -5.0744692988574966e-24, + "velocityY": 1.0116864366107383e-24, + "timestamp": 0 + }, + { + "x": 2.3347330882331176, + "y": 6.713955255875249, + "heading": 0.5125504196, + "angularVelocity": -3.160328817220775e-17, + "velocityX": 0.51083603498144, + "velocityY": 0.292445406541253, + "timestamp": 0.09491840599213663 + }, + { + "x": 2.4317085702199166, + "y": 6.7694721581924355, + "heading": 0.5125504196, + "angularVelocity": -2.914232468557734e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645907, + "timestamp": 0.18983681198427327 + }, + { + "x": 2.577171786567949, + "y": 6.852747507871406, + "heading": 0.5125504196, + "angularVelocity": -1.252738056302556e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461287, + "timestamp": 0.2847552179764099 + }, + { + "x": 2.771122709968306, + "y": 6.963781289278252, + "heading": 0.5125504196, + "angularVelocity": 1.2751776994126907e-16, + "velocityX": 2.0433436631502695, + "velocityY": 1.1697813532187185, + "timestamp": 0.37967362396854654 + }, + { + "x": 2.9165859263163383, + "y": 7.047056638957223, + "heading": 0.5125504196, + "angularVelocity": 7.51589809363944e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461287, + "timestamp": 0.47459202996068317 + }, + { + "x": 3.0135614083031372, + "y": 7.102573541274409, + "heading": 0.5125504196, + "angularVelocity": 6.994323112372533e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645907, + "timestamp": 0.5695104359528198 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": -4.84539672491667e-17, + "velocityX": 0.51083603498144, + "velocityY": 0.29244540654125295, + "timestamp": 0.6644288419449564 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": 9.145149251733134e-29, + "velocityX": 3.330473944863186e-25, + "velocityY": 2.452283341762288e-25, + "timestamp": 0.7593472479370931 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEF.3.traj b/src/main/deploy/choreo/AmpLanePADEF.3.traj new file mode 100644 index 00000000..6b4d0668 --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePADEF.3.traj @@ -0,0 +1,446 @@ +{ + "samples": [ + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": 9.145149251733134e-29, + "velocityX": 3.330473944863186e-25, + "velocityY": 2.452283341762288e-25, + "timestamp": 0 + }, + { + "x": 3.0904722904343656, + "y": 7.132620894810973, + "heading": 0.4951032659572505, + "angularVelocity": -0.25584290647295194, + "velocityX": 0.41679341451670293, + "velocityY": 0.03356417198946019, + "timestamp": 0.06819479141820195 + }, + { + "x": 3.1473436628615548, + "y": 7.13720088972522, + "heading": 0.4609074027399865, + "angularVelocity": -0.5014439153799775, + "velocityX": 0.8339547822417649, + "velocityY": 0.06716047984016284, + "timestamp": 0.1363895828364039 + }, + { + "x": 3.232694908233173, + "y": 7.144074686546732, + "heading": 0.4108880311553055, + "angularVelocity": -0.7334778880389725, + "velocityX": 1.2515801221269964, + "velocityY": 0.10079650774733914, + "timestamp": 0.20458437425460563 + }, + { + "x": 3.3465670290823137, + "y": 7.153245490430184, + "heading": 0.3463288656587193, + "angularVelocity": -0.9466876304478969, + "velocityX": 1.6698067180941178, + "velocityY": 0.1344795356468125, + "timestamp": 0.27277916567280736 + }, + { + "x": 3.489013918523337, + "y": 7.164717369527402, + "heading": 0.26910560921909454, + "angularVelocity": -1.132392296151419, + "velocityX": 2.0888235960349633, + "velocityY": 0.16822221842234344, + "timestamp": 0.3409739570910091 + }, + { + "x": 3.6601053694274515, + "y": 7.178495513325157, + "heading": 0.18214628797045526, + "angularVelocity": -1.2751607482067742, + "velocityX": 2.5088639080205257, + "velocityY": 0.2020409991909967, + "timestamp": 0.4091687485092108 + }, + { + "x": 3.8599204497819497, + "y": 7.19458405385975, + "heading": 0.0905625121816468, + "angularVelocity": -1.3429731785111632, + "velocityX": 2.9300636632076498, + "velocityY": 0.23592037162970983, + "timestamp": 0.47736353992741254 + }, + { + "x": 4.088431686194158, + "y": 7.212973174468024, + "heading": 0.005865325294296566, + "angularVelocity": -1.2419890892831975, + "velocityX": 3.350860551957296, + "velocityY": 0.26965579373215276, + "timestamp": 0.5455583313456143 + }, + { + "x": 4.340105997195649, + "y": 7.232705863139766, + "heading": 3.5066831801673635e-7, + "angularVelocity": -0.08600326365120493, + "velocityX": 3.6905210173326544, + "velocityY": 0.2893577098979943, + "timestamp": 0.613753122763816 + }, + { + "x": 4.593304664542687, + "y": 7.253186623137088, + "heading": 3.187896158571913e-7, + "angularVelocity": -4.67465351775184e-7, + "velocityX": 3.712873990541412, + "velocityY": 0.30032733543716505, + "timestamp": 0.6819479141820177 + }, + { + "x": 4.846503330689328, + "y": 7.273667397974732, + "heading": 2.869109349284145e-7, + "angularVelocity": -4.674650404498194e-7, + "velocityX": 3.7128739729389193, + "velocityY": 0.30032755305381253, + "timestamp": 0.7501427056002195 + }, + { + "x": 5.0997019968359405, + "y": 7.294148172812728, + "heading": 2.550322540852e-7, + "angularVelocity": -4.674650391951449e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589851, + "timestamp": 0.8183374970184212 + }, + { + "x": 5.352900662982552, + "y": 7.314628947650725, + "heading": 2.2315357326181484e-7, + "angularVelocity": -4.674650389043701e-7, + "velocityX": 3.712873972938501, + "velocityY": 0.30032755305898534, + "timestamp": 0.8865322884366229 + }, + { + "x": 5.606099329129164, + "y": 7.335109722488721, + "heading": 1.9127489242002956e-7, + "angularVelocity": -4.6746503917418713e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.30032755305898523, + "timestamp": 0.9547270798548246 + }, + { + "x": 5.859297995275776, + "y": 7.355590497326717, + "heading": 1.593962115667357e-7, + "angularVelocity": -4.6746503934294765e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.30032755305898523, + "timestamp": 1.0229218712730264 + }, + { + "x": 6.112496661422388, + "y": 7.3760712721647135, + "heading": 1.2751753069996381e-7, + "angularVelocity": -4.674650395405871e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589853, + "timestamp": 1.091116662691228 + }, + { + "x": 6.365695327569, + "y": 7.39655204700271, + "heading": 9.56388499052934e-8, + "angularVelocity": -4.674650384833005e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589854, + "timestamp": 1.1593114541094298 + }, + { + "x": 6.618893993715612, + "y": 7.41703282184071, + "heading": 6.376016904700571e-8, + "angularVelocity": -4.6746503941617634e-7, + "velocityX": 3.7128739729384974, + "velocityY": 0.300327553059032, + "timestamp": 1.2275062455276315 + }, + { + "x": 6.872092659851402, + "y": 7.437513596812488, + "heading": 3.188148827519539e-8, + "angularVelocity": -4.674650381480829e-7, + "velocityX": 3.7128739727798186, + "velocityY": 0.3003275550207428, + "timestamp": 1.2957010369458333 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": 2.875044920599443e-26, + "angularVelocity": -4.675062070310236e-7, + "velocityX": 3.7128672960134566, + "velocityY": 0.300410086481236, + "timestamp": 1.363895828364035 + }, + { + "x": 7.327398255820751, + "y": 7.485125041057045, + "heading": -0.0034915599283927, + "angularVelocity": -0.05859476692705873, + "velocityX": 3.3917318820879983, + "velocityY": 0.4552078415437867, + "timestamp": 1.423484083621354 + }, + { + "x": 7.508458450228073, + "y": 7.505782624333376, + "heading": -0.006110359017821738, + "angularVelocity": -0.04394824245348874, + "velocityX": 3.038521494302052, + "velocityY": 0.3466720612497382, + "timestamp": 1.483072338878673 + }, + { + "x": 7.668471533847549, + "y": 7.519972433195907, + "heading": -0.00785620988870458, + "angularVelocity": -0.029298573407523957, + "velocityX": 2.68531244837584, + "velocityY": 0.23813096727293456, + "timestamp": 1.542660594135992 + }, + { + "x": 7.807437533254427, + "y": 7.527694362359166, + "heading": -0.008729041834027805, + "angularVelocity": -0.01464771776844422, + "velocityX": 2.332103848430922, + "velocityY": 0.12958810641313134, + "timestamp": 1.6022488493933111 + }, + { + "x": 7.925356461622809, + "y": 7.528948359353389, + "heading": -0.008728854851904898, + "angularVelocity": 0.0000031379022946504295, + "velocityX": 1.9788954695715617, + "velocityY": 0.021044365014674563, + "timestamp": 1.6618371046506302 + }, + { + "x": 8.022228326704038, + "y": 7.523734392879376, + "heading": -0.007855710298573708, + "angularVelocity": 0.0146529639030494, + "velocityX": 1.6256872207939095, + "velocityY": -0.0874999016416258, + "timestamp": 1.7214253599079492 + }, + { + "x": 8.098053133481649, + "y": 7.512052442276777, + "heading": -0.006109727375299004, + "angularVelocity": 0.029300789488382373, + "velocityX": 1.272479055649084, + "velocityY": -0.19604451501648842, + "timestamp": 1.7810136151652682 + }, + { + "x": 8.152830885308017, + "y": 7.493902493017176, + "heading": -0.0034910810079803226, + "angularVelocity": 0.04394567949691789, + "velocityX": 0.919270946763285, + "velocityY": -0.30458937220470644, + "timestamp": 1.8406018704225873 + }, + { + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 2.1197622073376246e-26, + "angularVelocity": 0.058586729765871357, + "velocityX": 0.5660628762997177, + "velocityY": -0.41313440805613916, + "timestamp": 1.9001901256799063 + }, + { + "x": 8.185948757189772, + "y": 7.412401444181256, + "heading": 0.007947733329215964, + "angularVelocity": 0.08230133185775951, + "velocityX": -0.0063460233869197455, + "velocityY": -0.5890426736452052, + "timestamp": 1.996758834227077 + }, + { + "x": 8.130028903552427, + "y": 7.338629187384324, + "heading": 0.018106821496109617, + "angularVelocity": 0.10520062160644154, + "velocityX": -0.5790680488393326, + "velocityY": -0.7639354186961682, + "timestamp": 2.0933275427742477 + }, + { + "x": 8.018757435307506, + "y": 7.248113553962043, + "heading": 0.03036067277368815, + "angularVelocity": 0.12689256656666278, + "velocityX": -1.1522517999768782, + "velocityY": -0.9373184625128009, + "timestamp": 2.1898962513214184 + }, + { + "x": 7.852062035550644, + "y": 7.141094085596084, + "heading": 0.044518821199191705, + "angularVelocity": 0.14661217529473056, + "velocityX": -1.7261844158911652, + "velocityY": -1.1082209752622316, + "timestamp": 2.286464959868589 + }, + { + "x": 7.629805209867524, + "y": 7.0180373220836785, + "heading": 0.060212869798815634, + "angularVelocity": 0.16251691501039256, + "velocityX": -2.301540830636187, + "velocityY": -1.2742923185339627, + "timestamp": 2.38303366841576 + }, + { + "x": 7.351625362652527, + "y": 6.880249252941204, + "heading": 0.07642107570328951, + "angularVelocity": 0.16784117907672624, + "velocityX": -2.8806416840411067, + "velocityY": -1.4268397208104806, + "timestamp": 2.4796023769629305 + }, + { + "x": 7.016267448829526, + "y": 6.750124387193528, + "heading": 0.07642107964742734, + "angularVelocity": 4.0842814169097235e-8, + "velocityX": -3.4727389323964073, + "velocityY": -1.347484787829737, + "timestamp": 2.576171085510101 + }, + { + "x": 6.673392879085966, + "y": 6.641338156398843, + "heading": 0.0764210813675651, + "angularVelocity": 1.7812579245729294e-8, + "velocityX": -3.5505763192025745, + "velocityY": -1.1265163677895376, + "timestamp": 2.672739794057272 + }, + { + "x": 6.330518244592264, + "y": 6.532552129685128, + "heading": 0.0764210830877029, + "angularVelocity": 1.7812579496548032e-8, + "velocityX": -3.550576989711091, + "velocityY": -1.1265142544655247, + "timestamp": 2.7693085026044426 + }, + { + "x": 5.987643610098044, + "y": 6.42376610297305, + "heading": 0.07642108480784073, + "angularVelocity": 1.7812579676147597e-8, + "velocityX": -3.550576989716468, + "velocityY": -1.1265142544485776, + "timestamp": 2.8658772111516133 + }, + { + "x": 5.6447689756038235, + "y": 6.314980076260972, + "heading": 0.0764210865279785, + "angularVelocity": 1.7812579243167278e-8, + "velocityX": -3.550576989716468, + "velocityY": -1.1265142544485782, + "timestamp": 2.962445919698784 + }, + { + "x": 5.3018943411135915, + "y": 6.20619404953636, + "heading": 0.07642108824812432, + "angularVelocity": 1.7812662732207566e-8, + "velocityX": -3.5505769896751667, + "velocityY": -1.1265142545783664, + "timestamp": 3.0590146282459547 + }, + { + "x": 4.971178879857472, + "y": 6.101264896222856, + "heading": 0.09246038898135349, + "angularVelocity": 0.16609211176718244, + "velocityX": -3.4246648446642007, + "velocityY": -1.0865750913739254, + "timestamp": 3.1555833367931254 + }, + { + "x": 4.695582635189411, + "y": 6.01382394107076, + "heading": 0.10582487967853092, + "angularVelocity": 0.13839359455293215, + "velocityX": -2.8538876496773393, + "velocityY": -0.9054791812752113, + "timestamp": 3.252152045340296 + }, + { + "x": 4.475105628641342, + "y": 5.943871182253239, + "heading": 0.1165156132920598, + "angularVelocity": 0.11070598099908092, + "velocityX": -2.2831102317203804, + "velocityY": -0.7243832900939244, + "timestamp": 3.348720753887467 + }, + { + "x": 4.309747868917504, + "y": 5.8914066159860985, + "heading": 0.12453333874296012, + "angularVelocity": 0.08302612276298497, + "velocityX": -1.7123327236282266, + "velocityY": -0.5432874380991931, + "timestamp": 3.4452894624346375 + }, + { + "x": 4.19950936065589, + "y": 5.856430239360431, + "heading": 0.12987847932741303, + "angularVelocity": 0.055350647894830074, + "velocityX": -1.1415551675081796, + "velocityY": -0.36219161622714885, + "timestamp": 3.541858170981808 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.027676321443494054, + "velocityX": -0.5707775871082961, + "velocityY": -0.18109580929158178, + "timestamp": 3.638426879528979 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 2.6946819475075084e-27, + "velocityX": -6.238393680699965e-27, + "velocityY": 1.2085111651652188e-26, + "timestamp": 3.7349955880761496 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEF.4.traj b/src/main/deploy/choreo/AmpLanePADEF.4.traj new file mode 100644 index 00000000..bd3a1d20 --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePADEF.4.traj @@ -0,0 +1,554 @@ +{ + "samples": [ + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 2.6946819475075084e-27, + "velocityX": -6.238393680699965e-27, + "velocityY": 1.2085111651652188e-26, + "timestamp": 0 + }, + { + "x": 4.164497272188891, + "y": 5.856383810972102, + "heading": 0.11953980585413508, + "angularVelocity": -0.19788617668780903, + "velocityX": 0.30580479589931786, + "velocityY": 0.26526731175760876, + "timestamp": 0.06575163717948662 + }, + { + "x": 4.205280500846038, + "y": 5.8906190238483225, + "heading": 0.09417342379390335, + "angularVelocity": -0.38579088138881684, + "velocityX": 0.6202617973727179, + "velocityY": 0.520674683472388, + "timestamp": 0.13150327435897324 + }, + { + "x": 4.267438293143164, + "y": 5.9407879010107, + "heading": 0.05730532907136487, + "angularVelocity": -0.5607175167653599, + "velocityX": 0.9453421232303373, + "velocityY": 0.7630057488215584, + "timestamp": 0.19725491153845987 + }, + { + "x": 4.351839190303838, + "y": 6.005705720740894, + "heading": 0.010093847833175575, + "angularVelocity": -0.7180274630928684, + "velocityX": 1.283631872622101, + "velocityY": 0.9873186815559245, + "timestamp": 0.2630065487179465 + }, + { + "x": 4.459568016776304, + "y": 6.083661828301211, + "heading": -0.04581026085444041, + "angularVelocity": -0.850231432793241, + "velocityX": 1.6384204423441482, + "velocityY": 1.1856146995627632, + "timestamp": 0.3287581858974331 + }, + { + "x": 4.591948294079918, + "y": 6.172045469361953, + "heading": -0.1079292379524144, + "angularVelocity": -0.944751792695351, + "velocityX": 2.013338115707256, + "velocityY": 1.344204416073704, + "timestamp": 0.39450982307691973 + }, + { + "x": 4.7504027579860635, + "y": 6.266641095517782, + "heading": -0.17238119577808125, + "angularVelocity": -0.9802335058171755, + "velocityX": 2.409893817147125, + "velocityY": 1.438680924363996, + "timestamp": 0.46026146025640635 + }, + { + "x": 4.935641035495618, + "y": 6.360501248168881, + "heading": -0.23308994891442564, + "angularVelocity": -0.9233040535648408, + "velocityX": 2.8172420559490887, + "velocityY": 1.4274952940697678, + "timestamp": 0.526013097435893 + }, + { + "x": 5.145232187341836, + "y": 6.443576909061307, + "heading": -0.2814508136896339, + "angularVelocity": -0.7355081462564137, + "velocityX": 3.1876187550141757, + "velocityY": 1.2634766897993692, + "timestamp": 0.5917647346153796 + }, + { + "x": 5.371831569570122, + "y": 6.506507878011031, + "heading": -0.310075861000899, + "angularVelocity": -0.4353510960210094, + "velocityX": 3.446292624010618, + "velocityY": 0.9571011711531484, + "timestamp": 0.6575163717948662 + }, + { + "x": 5.608723728071605, + "y": 6.544912225542492, + "heading": -0.32489986784082275, + "angularVelocity": -0.22545456624080287, + "velocityX": 3.6028328519763595, + "velocityY": 0.5840819967208853, + "timestamp": 0.7232680089743528 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.3320371023810657, + "angularVelocity": -0.10854839280670678, + "velocityX": 3.6853903618544206, + "velocityY": 0.18577139494688796, + "timestamp": 0.7890196461538395 + }, + { + "x": 5.9711094207782285, + "y": 6.556647574574296, + "heading": -0.33458795200315017, + "angularVelocity": -0.07861324883762467, + "velocityX": 3.700225495398522, + "velocityY": -0.014775117904477024, + "timestamp": 0.8214677342280678 + }, + { + "x": 6.091317843363461, + "y": 6.549645373760147, + "heading": -0.3360778340908156, + "angularVelocity": -0.045915866730181235, + "velocityX": 3.704638076371226, + "velocityY": -0.21579702317530708, + "timestamp": 0.8539158223022962 + }, + { + "x": 6.211267508828441, + "y": 6.536122994121453, + "heading": -0.33661432797946356, + "angularVelocity": -0.016533913721531207, + "velocityX": 3.6966635812434836, + "velocityY": -0.4167388724956589, + "timestamp": 0.8863639103765246 + }, + { + "x": 6.330469045455405, + "y": 6.51611559729621, + "heading": -0.33661493169123347, + "angularVelocity": -0.000018605465091905695, + "velocityX": 3.6736074049811314, + "velocityY": -0.6165970943950513, + "timestamp": 0.918811998450753 + }, + { + "x": 6.448416412815129, + "y": 6.489700510555554, + "heading": -0.3366150129853366, + "angularVelocity": -0.0000025053588043472207, + "velocityX": 3.6349558436203777, + "velocityY": -0.8140722091307351, + "timestamp": 0.9512600865249814 + }, + { + "x": 6.564765294299518, + "y": 6.456954843497344, + "heading": -0.3366150728513962, + "angularVelocity": -0.000001844979570705055, + "velocityX": 3.5856929757534926, + "velocityY": -1.0091709250573346, + "timestamp": 0.9837081745992098 + }, + { + "x": 6.679176170718401, + "y": 6.417974154303709, + "heading": -0.33661511946704875, + "angularVelocity": -0.0000014366224727676693, + "velocityX": 3.525966650397334, + "velocityY": -1.2013246852776633, + "timestamp": 1.0161562626734382 + }, + { + "x": 6.791315180749945, + "y": 6.372872197072731, + "heading": -0.33661515729887875, + "angularVelocity": -0.000001165918618828322, + "velocityX": 3.4559512343227956, + "velocityY": -1.389972719742485, + "timestamp": 1.0486043507476666 + }, + { + "x": 6.900855096060357, + "y": 6.3217805921945684, + "heading": -0.3366151890219745, + "angularVelocity": -9.776568561253354e-7, + "velocityX": 3.375851145985221, + "velocityY": -1.5745644169014148, + "timestamp": 1.081052438821895 + }, + { + "x": 7.007476282450098, + "y": 6.264848452420557, + "heading": -0.33661521634397956, + "angularVelocity": -8.420220318148558e-7, + "velocityX": 3.285900424882802, + "velocityY": -1.7545606891775674, + "timestamp": 1.1135005268961233 + }, + { + "x": 7.110867767687471, + "y": 6.2022421991346945, + "heading": -0.33661529971413134, + "angularVelocity": -0.0000025693394187906504, + "velocityX": 3.186366019497228, + "velocityY": -1.9294281112232177, + "timestamp": 1.1459486149703517 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.25801763236442515, + "velocityX": 3.2027040434173877, + "velocityY": -1.73803251899956, + "timestamp": 1.17839670304458 + }, + { + "x": 7.403072746116238, + "y": 6.061144053185023, + "heading": -0.35924792619770074, + "angularVelocity": -0.23258228867402195, + "velocityX": 3.0708274318237563, + "velocityY": -1.3814614025635412, + "timestamp": 1.2397102609479145 + }, + { + "x": 7.5736331381530135, + "y": 5.991566896909843, + "heading": -0.3703276689612211, + "angularVelocity": -0.18070624413915942, + "velocityX": 2.781772871600061, + "velocityY": -1.1347760373794369, + "timestamp": 1.3010238188512488 + }, + { + "x": 7.72439550755076, + "y": 5.934268464416171, + "heading": -0.37789043666294847, + "angularVelocity": -0.12334576495545502, + "velocityX": 2.4588749136926022, + "velocityY": -0.934514884685174, + "timestamp": 1.3623373767545832 + }, + { + "x": 7.854564778976688, + "y": 5.88786008036463, + "heading": -0.38180542343665536, + "angularVelocity": -0.06385189357106222, + "velocityX": 2.1230095900021, + "velocityY": -0.7569024802753538, + "timestamp": 1.4236509346579176 + }, + { + "x": 7.963733931476252, + "y": 5.851532970517725, + "heading": -0.38200457597908144, + "angularVelocity": -0.0032480995922642706, + "velocityX": 1.7805059147224407, + "velocityY": -0.592480865393233, + "timestamp": 1.484964492561252 + }, + { + "x": 8.05165855711374, + "y": 5.8247603198900775, + "heading": -0.3784464795942301, + "angularVelocity": 0.0580311517798547, + "velocityX": 1.434016042195889, + "velocityY": -0.43665139559925853, + "timestamp": 1.5462780504645863 + }, + { + "x": 8.118176532906231, + "y": 5.80717243118701, + "heading": -0.3711033743685625, + "angularVelocity": 0.11976315641712676, + "velocityX": 1.084882007619954, + "velocityY": -0.2868515431904276, + "timestamp": 1.6075916083679207 + }, + { + "x": 8.16317279255834, + "y": 5.798495818445154, + "heading": -0.3599553749598749, + "angularVelocity": 0.18181948316004298, + "velocityX": 0.7338712870495722, + "velocityY": -0.1415121392161809, + "timestamp": 1.668905166271255 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.24412049957493756, + "velocityX": 0.38146199167222045, + "velocityY": 0.0003958300819017007, + "timestamp": 1.7302187241745894 + }, + { + "x": 8.186562507619959, + "y": 5.808483646148046, + "heading": -0.3243738625034631, + "angularVelocity": 0.3115588640226202, + "velocityX": 0.000013952657501305156, + "velocityY": 0.1505914724827301, + "timestamp": 1.7963815539018038 + }, + { + "x": 8.161321884729395, + "y": 5.828379592770816, + "heading": -0.29947359998774176, + "angularVelocity": 0.37634820968788424, + "velocityX": -0.38149249351379677, + "velocityY": 0.30071184537904955, + "timestamp": 1.862544383629018 + }, + { + "x": 8.110834888181197, + "y": 5.858201520747478, + "heading": -0.2705090334908383, + "angularVelocity": 0.43777702096967036, + "velocityX": -0.7630719054241456, + "velocityY": 0.45073537663996555, + "timestamp": 1.9287072133562324 + }, + { + "x": 8.035095319337403, + "y": 5.897940888331321, + "heading": -0.23777201719034075, + "angularVelocity": 0.494794682081624, + "velocityX": -1.144745004953147, + "velocityY": 0.600629806005663, + "timestamp": 1.9948700430834467 + }, + { + "x": 7.934094943360084, + "y": 5.947585769746669, + "heading": -0.20166340599285526, + "angularVelocity": 0.5457537313074324, + "velocityX": -1.526542567688504, + "velocityY": 0.7503439864956146, + "timestamp": 2.061032872810661 + }, + { + "x": 7.8078223399404525, + "y": 6.0071184189660025, + "heading": -0.1627692568761995, + "angularVelocity": 0.587854982578802, + "velocityX": -1.9085127395585555, + "velocityY": 0.8997899495046319, + "timestamp": 2.1271957025378754 + }, + { + "x": 7.656260799272679, + "y": 6.076509799690328, + "heading": -0.12202859757436733, + "angularVelocity": 0.6157635559090158, + "velocityX": -2.290735467220094, + "velocityY": 1.0487970513114737, + "timestamp": 2.1933585322650897 + }, + { + "x": 7.479384511526562, + "y": 6.155704239729163, + "heading": -0.08118783305769839, + "angularVelocity": 0.6172765688083961, + "velocityX": -2.6733482905034087, + "velocityY": 1.1969627110168721, + "timestamp": 2.259521361992304 + }, + { + "x": 7.277158908396328, + "y": 6.244553850532033, + "heading": -0.04460973018212138, + "angularVelocity": 0.5528497349098663, + "velocityX": -3.0564835869928735, + "velocityY": 1.342893149661115, + "timestamp": 2.3256841917195183 + }, + { + "x": 7.0505381507723195, + "y": 6.341424218836914, + "heading": -0.04460966986271364, + "angularVelocity": 9.116811959714027e-7, + "velocityX": -3.425197479587153, + "velocityY": 1.464120695929603, + "timestamp": 2.3918470214467327 + }, + { + "x": 6.821387916460742, + "y": 6.432149176298842, + "heading": -0.04460966140291972, + "angularVelocity": 1.278632421205442e-7, + "velocityX": -3.4634285633844892, + "velocityY": 1.371237563991483, + "timestamp": 2.458009851173946 + }, + { + "x": 6.585463067264655, + "y": 6.503425491915177, + "heading": -0.044609652839645464, + "angularVelocity": 1.2942726746927926e-7, + "velocityX": -3.5658216277748638, + "velocityY": 1.0772863843672305, + "timestamp": 2.5241726809011595 + }, + { + "x": 6.34313048184035, + "y": 6.548322764309462, + "heading": -0.044609643757262056, + "angularVelocity": 1.3727320078677108e-7, + "velocityX": -3.662669605024884, + "velocityY": 0.6785875480144041, + "timestamp": 2.590335510628373 + }, + { + "x": 6.097330137085011, + "y": 6.566295976512413, + "heading": -0.04460963356805428, + "angularVelocity": 1.5400199501081406e-7, + "velocityX": -3.715082105296292, + "velocityY": 0.27165120169518303, + "timestamp": 2.6564983403555864 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.04460962128616813, + "angularVelocity": 1.8563121019327388e-7, + "velocityX": -3.722421790171331, + "velocityY": -0.13858200516588748, + "timestamp": 2.7226611700828 + }, + { + "x": 5.594131845425269, + "y": 6.517387984060631, + "heading": -0.0446096110934515, + "angularVelocity": 1.4604851016203596e-7, + "velocityX": -3.6812230501933914, + "velocityY": -0.5694089339833474, + "timestamp": 2.7924511092092175 + }, + { + "x": 5.343562863816817, + "y": 6.448118054894418, + "heading": -0.04460960236123058, + "angularVelocity": 1.2512148631224918e-7, + "velocityX": -3.590330995339737, + "velocityY": -0.9925489265829068, + "timestamp": 2.862241048335635 + }, + { + "x": 5.102719675691794, + "y": 6.350252358061884, + "heading": -0.04460958274491264, + "angularVelocity": 2.810765876662287e-7, + "velocityX": -3.4509728929374397, + "velocityY": -1.4022894711981222, + "timestamp": 2.9320309874620527 + }, + { + "x": 4.88810249039924, + "y": 6.239912108360582, + "heading": -0.010754389574110686, + "angularVelocity": 0.4851013426086585, + "velocityX": -3.0751880282313135, + "velocityY": -1.5810337576227271, + "timestamp": 3.0018209265884703 + }, + { + "x": 4.7014054819057804, + "y": 6.141094298946399, + "heading": 0.022653281130584686, + "angularVelocity": 0.47868892168225635, + "velocityX": -2.6751278311803, + "velocityY": -1.415932019014718, + "timestamp": 3.071610865714888 + }, + { + "x": 4.541859398687963, + "y": 6.055486217406872, + "heading": 0.05282297144244177, + "angularVelocity": 0.43229283030620586, + "velocityX": -2.286090018345113, + "velocityY": -1.2266536210105516, + "timestamp": 3.1414008048413056 + }, + { + "x": 4.409161403799561, + "y": 5.983676432778196, + "heading": 0.07876234420420766, + "angularVelocity": 0.37167782471881955, + "velocityX": -1.9013914691633662, + "velocityY": -1.0289417862737966, + "timestamp": 3.211190743967723 + }, + { + "x": 4.303150560780497, + "y": 5.925963754570679, + "heading": 0.09996559761955173, + "angularVelocity": 0.3038153304151236, + "velocityX": -1.5189989323107926, + "velocityY": -0.8269483958565483, + "timestamp": 3.280980683094141 + }, + { + "x": 4.223727158834788, + "y": 5.882528846842074, + "heading": 0.11612624955874849, + "angularVelocity": 0.23156134167022593, + "velocityX": -1.1380351228253833, + "velocityY": -0.6223663220271061, + "timestamp": 3.3507706222205584 + }, + { + "x": 4.170823380513197, + "y": 5.853492711046949, + "heading": 0.1270379483840901, + "angularVelocity": 0.15635059955527503, + "velocityX": -0.7580430500986731, + "velocityY": -0.41605045309653765, + "timestamp": 3.420560561346976 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.0789970249504139, + "velocityX": -0.37875479822591473, + "velocityY": -0.2084922310471305, + "timestamp": 3.4903505004733937 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.1741097503964344e-26, + "velocityX": -8.106670406309328e-26, + "velocityY": -1.6795580659596768e-26, + "timestamp": 3.5601404395998113 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEF.5.traj b/src/main/deploy/choreo/AmpLanePADEF.5.traj new file mode 100644 index 00000000..1d21628a --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePADEF.5.traj @@ -0,0 +1,473 @@ +{ + "samples": [ + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.1741097503964344e-26, + "velocityX": -8.106670406309328e-26, + "velocityY": -1.6795580659596768e-26, + "timestamp": 0 + }, + { + "x": 4.149229218776726, + "y": 5.815100726386144, + "heading": 0.12918022174703248, + "angularVelocity": -0.05380546988265158, + "velocityX": 0.07724016042252664, + "velocityY": -0.38054657832352085, + "timestamp": 0.06265021394417758 + }, + { + "x": 4.159717289478366, + "y": 5.767595641795523, + "heading": 0.12259706469587855, + "angularVelocity": -0.1050779660069413, + "velocityX": 0.16740678189839092, + "velocityY": -0.758258936401222, + "timestamp": 0.12530042788835516 + }, + { + "x": 4.176867312970839, + "y": 5.696690246498273, + "heading": 0.11300853276979762, + "angularVelocity": -0.1530486701070873, + "velocityX": 0.2737424569332407, + "velocityY": -1.1317662116912734, + "timestamp": 0.18795064183253274 + }, + { + "x": 4.201974040585746, + "y": 5.602790592825652, + "heading": 0.10069220099176118, + "angularVelocity": -0.19658882233044642, + "velocityX": 0.4007444832874458, + "velocityY": -1.4987922268914513, + "timestamp": 0.2506008557767103 + }, + { + "x": 4.236730391014469, + "y": 5.486551757326023, + "heading": 0.08603276386217854, + "angularVelocity": -0.2339886204162749, + "velocityX": 0.5547682639949316, + "velocityY": -1.8553621477366304, + "timestamp": 0.3132510697208879 + }, + { + "x": 4.283393708275682, + "y": 5.349087774330418, + "heading": 0.06958159023755481, + "angularVelocity": -0.2625876687233969, + "velocityX": 0.7448229514872947, + "velocityY": -2.194150256503298, + "timestamp": 0.3759012836650655 + }, + { + "x": 4.344983097190331, + "y": 5.19239819137041, + "heading": 0.0521501823955064, + "angularVelocity": -0.27823381190014773, + "velocityX": 0.983067495500113, + "velocityY": -2.501022312543423, + "timestamp": 0.43855149760924306 + }, + { + "x": 4.425324635342364, + "y": 5.020190178284826, + "heading": 0.03492497946543372, + "angularVelocity": -0.2749424438585406, + "velocityX": 1.2823825026937383, + "velocityY": -2.748721867718171, + "timestamp": 0.5012017115534206 + }, + { + "x": 4.5282801412548075, + "y": 4.838988810812342, + "heading": 0.019478906624106312, + "angularVelocity": -0.24654461443802958, + "velocityX": 1.6433384569792093, + "velocityY": -2.892270529737325, + "timestamp": 0.5638519254975982 + }, + { + "x": 4.655561045937701, + "y": 4.65789803685685, + "heading": 0.007446897298022715, + "angularVelocity": -0.1920505704386623, + "velocityX": 2.031611652536461, + "velocityY": -2.890505276116785, + "timestamp": 0.6265021394417758 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -3.0154063280737095e-24, + "angularVelocity": -0.11886467466269138, + "velocityX": 2.394177429870417, + "velocityY": -2.752084488523512, + "timestamp": 0.6891523533859534 + }, + { + "x": 5.0265563719767545, + "y": 4.290478604211027, + "heading": -0.0011711585949722948, + "angularVelocity": -0.014781576411206173, + "velocityX": 2.789308345003365, + "velocityY": -2.461168374898756, + "timestamp": 0.7683833205299191 + }, + { + "x": 5.271648004487176, + "y": 4.1260554019145586, + "heading": -0.0011711658442560332, + "angularVelocity": -9.149558562553705e-8, + "velocityX": 3.0933818094771004, + "velocityY": -2.075239117019812, + "timestamp": 0.8476142876738848 + }, + { + "x": 5.5362482788476575, + "y": 3.9953205544154557, + "heading": -0.0011711670074451403, + "angularVelocity": -1.4680990893194224e-8, + "velocityX": 3.3396067711718436, + "velocityY": -1.650047351581014, + "timestamp": 0.9268452548178505 + }, + { + "x": 5.815753541872799, + "y": 3.900548632610082, + "heading": -0.0011711678822933615, + "angularVelocity": -1.1041746080675712e-8, + "velocityX": 3.5277275174146143, + "velocityY": -1.196147481491295, + "timestamp": 1.0060762219618162 + }, + { + "x": 6.105300821187424, + "y": 3.8433884686557884, + "heading": -0.001171168612339158, + "angularVelocity": -9.214147233371614e-9, + "velocityX": 3.654471095733393, + "velocityY": -0.7214371604278238, + "timestamp": 1.085307189105782 + }, + { + "x": 6.399852439542986, + "y": 3.8248343191470044, + "heading": -0.0011711692753051952, + "angularVelocity": -8.367511605727905e-9, + "velocityX": 3.717632498671273, + "velocityY": -0.2341780010721126, + "timestamp": 1.1645381562497477 + }, + { + "x": 6.694725372224164, + "y": 3.8372788433696505, + "heading": -0.0011711699217424045, + "angularVelocity": -8.15889585625018e-9, + "velocityX": 3.721687912068309, + "velocityY": 0.15706641823560874, + "timestamp": 1.2437691233937134 + }, + { + "x": 6.9895982402615005, + "y": 3.8497248992335154, + "heading": -0.0011711705681793068, + "angularVelocity": -8.15889197720244e-9, + "velocityX": 3.7216870961771913, + "velocityY": 0.157085749581342, + "timestamp": 1.323000090537679 + }, + { + "x": 7.283138211598938, + "y": 3.880370995696298, + "heading": -0.0011711735014489986, + "angularVelocity": -3.702175800028382e-8, + "velocityX": 3.7048641701427636, + "velocityY": 0.386794426061941, + "timestamp": 1.4022310576816448 + }, + { + "x": 7.537795284833578, + "y": 3.9128988141490386, + "heading": -0.0022060584164621646, + "angularVelocity": -0.01306162164009358, + "velocityX": 3.2141103714147428, + "velocityY": 0.4105442559300831, + "timestamp": 1.4814620248256105 + }, + { + "x": 7.753528037916665, + "y": 3.9448039763834073, + "heading": -0.003047966804873468, + "angularVelocity": -0.010626001660203437, + "velocityX": 2.722833771435503, + "velocityY": 0.40268550775602213, + "timestamp": 1.5606929919695762 + }, + { + "x": 7.930345641638023, + "y": 3.9756628799518334, + "heading": -0.0034891865809292344, + "angularVelocity": -0.005568779379583401, + "velocityX": 2.23167291899988, + "velocityY": 0.3894803342783207, + "timestamp": 1.639923959113542 + }, + { + "x": 8.068253246603454, + "y": 4.005300685276172, + "heading": -0.003443969014949724, + "angularVelocity": 0.0005707057178457502, + "velocityX": 1.7405770740479334, + "velocityY": 0.3740684531905991, + "timestamp": 1.7191549262575077 + }, + { + "x": 8.167254001972685, + "y": 4.033621928989228, + "heading": -0.0028654925288464014, + "angularVelocity": 0.007301141295577129, + "velocityX": 1.2495209756728338, + "velocityY": 0.3574516976625533, + "timestamp": 1.7983858934014734 + }, + { + "x": 8.227350013878981, + "y": 4.060566470670501, + "heading": -0.0017242628170792867, + "angularVelocity": 0.014403834168696394, + "velocityX": 0.7584914594958708, + "velocityY": 0.340075890179585, + "timestamp": 1.877616860545439 + }, + { + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 1.0796890687236353e-23, + "angularVelocity": 0.021762486048494588, + "velocityX": 0.2674809172408869, + "velocityY": 0.32217804683224427, + "timestamp": 1.9568478276894048 + }, + { + "x": 8.231523012343859, + "y": 4.109892958457586, + "heading": 0.0022863134326364947, + "angularVelocity": 0.029215203502289427, + "velocityX": -0.21748380315842067, + "velocityY": 0.3041237094875908, + "timestamp": 2.0351054866667706 + }, + { + "x": 8.176551036095814, + "y": 4.132280075994507, + "heading": 0.005155847293198806, + "angularVelocity": 0.03666777026121207, + "velocityX": -0.7024485138757453, + "velocityY": 0.2860693487316991, + "timestamp": 2.1133631456441364 + }, + { + "x": 8.083626857911096, + "y": 4.153254299063425, + "heading": 0.008608587113368522, + "angularVelocity": 0.04412015213959263, + "velocityX": -1.1874132116780545, + "velocityY": 0.26801495652947127, + "timestamp": 2.191620804621502 + }, + { + "x": 7.952750479206828, + "y": 4.172815624183239, + "heading": 0.012644515987955914, + "angularVelocity": 0.051572317998353566, + "velocityX": -1.6723778913719014, + "velocityY": 0.24996051984474485, + "timestamp": 2.269878463598868 + }, + { + "x": 7.783921902112919, + "y": 4.190964046074225, + "heading": 0.017263614398582115, + "angularVelocity": 0.059024234445374815, + "velocityX": -2.1573425438491505, + "velocityY": 0.2319060156940586, + "timestamp": 2.348136122576234 + }, + { + "x": 7.57714113018582, + "y": 4.207699555855796, + "heading": 0.022465858708652038, + "angularVelocity": 0.06647584885684583, + "velocityX": -2.642307150880996, + "velocityY": 0.2138513980645823, + "timestamp": 2.4263937815535996 + }, + { + "x": 7.332408170546637, + "y": 4.223022135701224, + "heading": 0.028251214754351595, + "angularVelocity": 0.07392702671278313, + "velocityX": -3.1272716669171876, + "velocityY": 0.19579655263979157, + "timestamp": 2.5046514405309654 + }, + { + "x": 7.049723044551417, + "y": 4.23693173264099, + "heading": 0.03461960291604749, + "angularVelocity": 0.08137718716500116, + "velocityX": -3.6122359100593893, + "velocityY": 0.1777410303544707, + "timestamp": 2.5829090995083313 + }, + { + "x": 6.758414971904035, + "y": 4.226087741393702, + "heading": 0.03461960351130125, + "angularVelocity": 7.606332300457526e-9, + "velocityX": -3.7224225264857154, + "velocityY": -0.13856779501191718, + "timestamp": 2.661166758485697 + }, + { + "x": 6.467106926605873, + "y": 4.215243015473263, + "heading": 0.03461960410653643, + "angularVelocity": 7.606094897280975e-9, + "velocityX": -3.7224221770091352, + "velocityY": -0.13857718288730186, + "timestamp": 2.739424417463063 + }, + { + "x": 6.175798881305749, + "y": 4.204398289605515, + "heading": 0.03461960470177156, + "angularVelocity": 7.606094342014038e-9, + "velocityX": -3.7224221770342005, + "velocityY": -0.1385771822140065, + "timestamp": 2.8176820764404287 + }, + { + "x": 5.8844906838954465, + "y": 4.193557650451513, + "heading": 0.03461960529700769, + "angularVelocity": 7.606107036408581e-9, + "velocityX": -3.7224241207439843, + "velocityY": -0.13852496095158084, + "timestamp": 2.8959397354177945 + }, + { + "x": 5.594009037113428, + "y": 4.218019822631225, + "heading": 0.034619605911650746, + "angularVelocity": 7.854094608871324e-9, + "velocityX": -3.7118622071998786, + "velocityY": 0.31258502361778046, + "timestamp": 2.9741973943951603 + }, + { + "x": 5.309172831526295, + "y": 4.280038516789021, + "heading": 0.034619599952821836, + "angularVelocity": -7.614371543607384e-8, + "velocityX": -3.639723054704678, + "velocityY": 0.7924936034150137, + "timestamp": 3.052455053372526 + }, + { + "x": 5.03935943827081, + "y": 4.376755129785837, + "heading": 0.01971114929830447, + "angularVelocity": -0.19050468477250748, + "velocityX": -3.4477570218849003, + "velocityY": 1.235874089011402, + "timestamp": 3.130712712349892 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -1.0813807424036439e-23, + "angularVelocity": -0.25187501844395294, + "velocityX": -2.987601050001106, + "velocityY": 1.389310982379468, + "timestamp": 3.2089703713272577 + }, + { + "x": 4.627457882261689, + "y": 4.587679677136334, + "heading": -0.019778853338755203, + "angularVelocity": -0.2848710655643308, + "velocityX": -2.5651244910957094, + "velocityY": 1.4719718180672714, + "timestamp": 3.2784012673699046 + }, + { + "x": 4.478940134439646, + "y": 4.685562428002016, + "heading": -0.03941675041867698, + "angularVelocity": -0.28284089935782175, + "velocityX": -2.1390728953118847, + "velocityY": 1.4097866575934486, + "timestamp": 3.3478321634125514 + }, + { + "x": 4.358424662792702, + "y": 4.772986561558444, + "heading": -0.05736759927865439, + "angularVelocity": -0.2585426644782356, + "velocityX": -1.7357614335398788, + "velocityY": 1.2591531802027813, + "timestamp": 3.4172630594551983 + }, + { + "x": 4.264251963016929, + "y": 4.846292896298891, + "heading": -0.07266630938102059, + "angularVelocity": -0.220344414005101, + "velocityX": -1.3563514968599655, + "velocityY": 1.0558172070171679, + "timestamp": 3.486693955497845 + }, + { + "x": 4.19504667897931, + "y": 4.903178017800909, + "heading": -0.0846829764598402, + "angularVelocity": -0.17307377210627528, + "velocityX": -0.9967505531703156, + "velocityY": 0.8193055936809015, + "timestamp": 3.556124851540492 + }, + { + "x": 4.149718350868284, + "y": 4.942098244876492, + "heading": -0.09298341563379672, + "angularVelocity": -0.11954964788093829, + "velocityX": -0.6528552948990191, + "velocityY": 0.5605606335784127, + "timestamp": 3.625555747583139 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.06149221603273053, + "velocityX": -0.32142062647019837, + "velocityY": 0.2860902346287965, + "timestamp": 3.6949866436257857 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -7.045263803296892e-25, + "velocityX": -1.4084469220925396e-25, + "velocityY": -2.7940612123980634e-25, + "timestamp": 3.7644175396684325 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePADEF.traj b/src/main/deploy/choreo/AmpLanePADEF.traj new file mode 100644 index 00000000..cefc2a7e --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePADEF.traj @@ -0,0 +1,1688 @@ +{ + "samples": [ + { + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": -2.0223823950436125e-24, + "angularVelocity": -1.0530930101357375e-37, + "velocityX": 9.679119214488622e-24, + "velocityY": 2.8468005585003605e-23, + "timestamp": 0 + }, + { + "x": 0.45358539197474673, + "y": 6.976442037280005, + "heading": -4.051643418810716e-24, + "angularVelocity": -1.1805501930230392e-25, + "velocityX": 0.3537634154949626, + "velocityY": -0.0735607445986989, + "timestamp": 0.05826629620784151 + }, + { + "x": 0.49483104636042347, + "y": 6.967970532727312, + "heading": -6.080913911985301e-24, + "angularVelocity": -1.1821753877066713e-25, + "velocityX": 0.7078818643036704, + "velocityY": -0.1453928789719994, + "timestamp": 0.11653259241568302 + }, + { + "x": 0.5567354919848906, + "y": 6.95544199101986, + "heading": -8.110182775739579e-24, + "angularVelocity": -1.1818957371343575e-25, + "velocityX": 1.0624400322898162, + "velocityY": -0.21502210579442033, + "timestamp": 0.17479888862352452 + }, + { + "x": 0.6393312280569997, + "y": 6.9390255615072824, + "heading": -1.013944960655158e-23, + "angularVelocity": -1.1815468317795835e-25, + "velocityX": 1.4175559705645637, + "velocityY": -0.28174829328466466, + "timestamp": 0.23306518483136604 + }, + { + "x": 0.7426606798558012, + "y": 6.918954250671649, + "heading": -1.2168715161261342e-23, + "angularVelocity": -1.181327819705339e-25, + "velocityX": 1.7734000361068998, + "velocityY": -0.3444754882657602, + "timestamp": 0.29133148103920753 + }, + { + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": -1.4479605554027417e-23, + "angularVelocity": -4.951541740242581e-24, + "velocityX": 2.130226970895328, + "velocityY": -0.4013456783802647, + "timestamp": 0.34959777724704905 + }, + { + "x": 1.0109741912909123, + "y": 6.869549473554426, + "heading": 0.010039096014815363, + "angularVelocity": 0.17297490390919232, + "velocityX": 2.484465074043925, + "velocityY": -0.44832534814487107, + "timestamp": 0.4076356531928316 + }, + { + "x": 1.1757265724080805, + "y": 6.8408027772938045, + "heading": 0.030114435462749452, + "angularVelocity": 0.3459006574721642, + "velocityX": 2.83870452583543, + "velocityY": -0.49530924059791875, + "timestamp": 0.4656735291386142 + }, + { + "x": 1.3610383679326326, + "y": 6.809328854359183, + "heading": 0.060221992176283975, + "angularVelocity": 0.5187570396556241, + "velocityX": 3.1929458565586635, + "velocityY": -0.5422997038007265, + "timestamp": 0.5237114050843967 + }, + { + "x": 1.546079908472426, + "y": 6.784713461447441, + "heading": 0.1506024575679624, + "angularVelocity": 1.5572669385094218, + "velocityX": 3.188289328724817, + "velocityY": -0.4241263573246083, + "timestamp": 0.5817492810301793 + }, + { + "x": 1.710561311165979, + "y": 6.762828401312947, + "heading": 0.2309691373975153, + "angularVelocity": 1.3847281369261228, + "velocityX": 2.834035533057883, + "velocityY": -0.37708237556692, + "timestamp": 0.6397871569759619 + }, + { + "x": 1.8544824093337007, + "y": 6.743675274338349, + "heading": 0.3013204880160867, + "angularVelocity": 1.2121627380762827, + "velocityX": 2.4797788654803434, + "velocityY": -0.3300108190122258, + "timestamp": 0.6978250329217445 + }, + { + "x": 1.9778431850692357, + "y": 6.727255600598568, + "heading": 0.36164875379241124, + "angularVelocity": 1.0394637087112153, + "velocityX": 2.12552188937403, + "velocityY": -0.28291307137290855, + "timestamp": 0.755862908867527 + }, + { + "x": 2.0806437247377927, + "y": 6.713570715572485, + "heading": 0.4119419860379565, + "angularVelocity": 0.8665588019197638, + "velocityX": 1.7712664013512611, + "velocityY": -0.23579231326223515, + "timestamp": 0.8139007848133096 + }, + { + "x": 2.16288415503368, + "y": 6.702621710000965, + "heading": 0.45218713038219516, + "angularVelocity": 0.6934289666602311, + "velocityX": 1.4170130962875669, + "velocityY": -0.18865276154743782, + "timestamp": 0.8719386607590922 + }, + { + "x": 2.2245645766627815, + "y": 6.694409403887936, + "heading": 0.48237322491294377, + "angularVelocity": 0.5201102562565806, + "velocityX": 1.062761526399103, + "velocityY": -0.14149908106046907, + "timestamp": 0.9299765367048748 + }, + { + "x": 2.265685008078599, + "y": 6.688934342765243, + "heading": 0.5024941236633007, + "angularVelocity": 0.3466856500598576, + "velocityX": 0.7085102744668164, + "velocityY": -0.09433600099024221, + "timestamp": 0.9880144126506574 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.1732712607555392, + "velocityX": 0.35425724418212445, + "velocityY": -0.04716814104583245, + "timestamp": 1.0460522885964398 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -7.633423863817013e-25, + "velocityX": -5.0744692988574966e-24, + "velocityY": 1.0116864366107383e-24, + "timestamp": 1.1040901645422223 + }, + { + "x": 2.3347330882331176, + "y": 6.713955255875249, + "heading": 0.5125504196, + "angularVelocity": -3.160328817220775e-17, + "velocityX": 0.51083603498144, + "velocityY": 0.292445406541253, + "timestamp": 1.199008570534359 + }, + { + "x": 2.4317085702199166, + "y": 6.7694721581924355, + "heading": 0.5125504196, + "angularVelocity": -2.914232468557734e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645907, + "timestamp": 1.2939269765264956 + }, + { + "x": 2.577171786567949, + "y": 6.852747507871406, + "heading": 0.5125504196, + "angularVelocity": -1.252738056302556e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461287, + "timestamp": 1.3888453825186322 + }, + { + "x": 2.771122709968306, + "y": 6.963781289278252, + "heading": 0.5125504196, + "angularVelocity": 1.2751776994126907e-16, + "velocityX": 2.0433436631502695, + "velocityY": 1.1697813532187185, + "timestamp": 1.4837637885107688 + }, + { + "x": 2.9165859263163383, + "y": 7.047056638957223, + "heading": 0.5125504196, + "angularVelocity": 7.51589809363944e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461287, + "timestamp": 1.5786821945029055 + }, + { + "x": 3.0135614083031372, + "y": 7.102573541274409, + "heading": 0.5125504196, + "angularVelocity": 6.994323112372533e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645907, + "timestamp": 1.6736006004950421 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": -4.84539672491667e-17, + "velocityX": 0.51083603498144, + "velocityY": 0.29244540654125295, + "timestamp": 1.7685190064871787 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": 9.145149251733134e-29, + "velocityX": 3.330473944863186e-25, + "velocityY": 2.452283341762288e-25, + "timestamp": 1.8634374124793154 + }, + { + "x": 3.0904722904343656, + "y": 7.132620894810973, + "heading": 0.4951032659572505, + "angularVelocity": -0.25584290647295194, + "velocityX": 0.41679341451670293, + "velocityY": 0.03356417198946019, + "timestamp": 1.9316322038975173 + }, + { + "x": 3.1473436628615548, + "y": 7.13720088972522, + "heading": 0.4609074027399865, + "angularVelocity": -0.5014439153799775, + "velocityX": 0.8339547822417649, + "velocityY": 0.06716047984016284, + "timestamp": 1.9998269953157193 + }, + { + "x": 3.232694908233173, + "y": 7.144074686546732, + "heading": 0.4108880311553055, + "angularVelocity": -0.7334778880389725, + "velocityX": 1.2515801221269964, + "velocityY": 0.10079650774733914, + "timestamp": 2.068021786733921 + }, + { + "x": 3.3465670290823137, + "y": 7.153245490430184, + "heading": 0.3463288656587193, + "angularVelocity": -0.9466876304478969, + "velocityX": 1.6698067180941178, + "velocityY": 0.1344795356468125, + "timestamp": 2.1362165781521227 + }, + { + "x": 3.489013918523337, + "y": 7.164717369527402, + "heading": 0.26910560921909454, + "angularVelocity": -1.132392296151419, + "velocityX": 2.0888235960349633, + "velocityY": 0.16822221842234344, + "timestamp": 2.2044113695703245 + }, + { + "x": 3.6601053694274515, + "y": 7.178495513325157, + "heading": 0.18214628797045526, + "angularVelocity": -1.2751607482067742, + "velocityX": 2.5088639080205257, + "velocityY": 0.2020409991909967, + "timestamp": 2.272606160988526 + }, + { + "x": 3.8599204497819497, + "y": 7.19458405385975, + "heading": 0.0905625121816468, + "angularVelocity": -1.3429731785111632, + "velocityX": 2.9300636632076498, + "velocityY": 0.23592037162970983, + "timestamp": 2.340800952406728 + }, + { + "x": 4.088431686194158, + "y": 7.212973174468024, + "heading": 0.005865325294296566, + "angularVelocity": -1.2419890892831975, + "velocityX": 3.350860551957296, + "velocityY": 0.26965579373215276, + "timestamp": 2.4089957438249296 + }, + { + "x": 4.340105997195649, + "y": 7.232705863139766, + "heading": 3.5066831801673635e-7, + "angularVelocity": -0.08600326365120493, + "velocityX": 3.6905210173326544, + "velocityY": 0.2893577098979943, + "timestamp": 2.4771905352431314 + }, + { + "x": 4.593304664542687, + "y": 7.253186623137088, + "heading": 3.187896158571913e-7, + "angularVelocity": -4.67465351775184e-7, + "velocityX": 3.712873990541412, + "velocityY": 0.30032733543716505, + "timestamp": 2.545385326661333 + }, + { + "x": 4.846503330689328, + "y": 7.273667397974732, + "heading": 2.869109349284145e-7, + "angularVelocity": -4.674650404498194e-7, + "velocityX": 3.7128739729389193, + "velocityY": 0.30032755305381253, + "timestamp": 2.613580118079535 + }, + { + "x": 5.0997019968359405, + "y": 7.294148172812728, + "heading": 2.550322540852e-7, + "angularVelocity": -4.674650391951449e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589851, + "timestamp": 2.6817749094977366 + }, + { + "x": 5.352900662982552, + "y": 7.314628947650725, + "heading": 2.2315357326181484e-7, + "angularVelocity": -4.674650389043701e-7, + "velocityX": 3.712873972938501, + "velocityY": 0.30032755305898534, + "timestamp": 2.7499697009159383 + }, + { + "x": 5.606099329129164, + "y": 7.335109722488721, + "heading": 1.9127489242002956e-7, + "angularVelocity": -4.6746503917418713e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.30032755305898523, + "timestamp": 2.81816449233414 + }, + { + "x": 5.859297995275776, + "y": 7.355590497326717, + "heading": 1.593962115667357e-7, + "angularVelocity": -4.6746503934294765e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.30032755305898523, + "timestamp": 2.8863592837523417 + }, + { + "x": 6.112496661422388, + "y": 7.3760712721647135, + "heading": 1.2751753069996381e-7, + "angularVelocity": -4.674650395405871e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589853, + "timestamp": 2.9545540751705435 + }, + { + "x": 6.365695327569, + "y": 7.39655204700271, + "heading": 9.56388499052934e-8, + "angularVelocity": -4.674650384833005e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589854, + "timestamp": 3.022748866588745 + }, + { + "x": 6.618893993715612, + "y": 7.41703282184071, + "heading": 6.376016904700571e-8, + "angularVelocity": -4.6746503941617634e-7, + "velocityX": 3.7128739729384974, + "velocityY": 0.300327553059032, + "timestamp": 3.090943658006947 + }, + { + "x": 6.872092659851402, + "y": 7.437513596812488, + "heading": 3.188148827519539e-8, + "angularVelocity": -4.674650381480829e-7, + "velocityX": 3.7128739727798186, + "velocityY": 0.3003275550207428, + "timestamp": 3.1591384494251487 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": 2.875044920599443e-26, + "angularVelocity": -4.675062070310236e-7, + "velocityX": 3.7128672960134566, + "velocityY": 0.300410086481236, + "timestamp": 3.2273332408433504 + }, + { + "x": 7.327398255820751, + "y": 7.485125041057045, + "heading": -0.0034915599283927, + "angularVelocity": -0.05859476692705873, + "velocityX": 3.3917318820879983, + "velocityY": 0.4552078415437867, + "timestamp": 3.2869214961006694 + }, + { + "x": 7.508458450228073, + "y": 7.505782624333376, + "heading": -0.006110359017821738, + "angularVelocity": -0.04394824245348874, + "velocityX": 3.038521494302052, + "velocityY": 0.3466720612497382, + "timestamp": 3.3465097513579884 + }, + { + "x": 7.668471533847549, + "y": 7.519972433195907, + "heading": -0.00785620988870458, + "angularVelocity": -0.029298573407523957, + "velocityX": 2.68531244837584, + "velocityY": 0.23813096727293456, + "timestamp": 3.4060980066153075 + }, + { + "x": 7.807437533254427, + "y": 7.527694362359166, + "heading": -0.008729041834027805, + "angularVelocity": -0.01464771776844422, + "velocityX": 2.332103848430922, + "velocityY": 0.12958810641313134, + "timestamp": 3.4656862618726265 + }, + { + "x": 7.925356461622809, + "y": 7.528948359353389, + "heading": -0.008728854851904898, + "angularVelocity": 0.0000031379022946504295, + "velocityX": 1.9788954695715617, + "velocityY": 0.021044365014674563, + "timestamp": 3.5252745171299456 + }, + { + "x": 8.022228326704038, + "y": 7.523734392879376, + "heading": -0.007855710298573708, + "angularVelocity": 0.0146529639030494, + "velocityX": 1.6256872207939095, + "velocityY": -0.0874999016416258, + "timestamp": 3.5848627723872646 + }, + { + "x": 8.098053133481649, + "y": 7.512052442276777, + "heading": -0.006109727375299004, + "angularVelocity": 0.029300789488382373, + "velocityX": 1.272479055649084, + "velocityY": -0.19604451501648842, + "timestamp": 3.6444510276445836 + }, + { + "x": 8.152830885308017, + "y": 7.493902493017176, + "heading": -0.0034910810079803226, + "angularVelocity": 0.04394567949691789, + "velocityX": 0.919270946763285, + "velocityY": -0.30458937220470644, + "timestamp": 3.7040392829019027 + }, + { + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 2.1197622073376246e-26, + "angularVelocity": 0.058586729765871357, + "velocityX": 0.5660628762997177, + "velocityY": -0.41313440805613916, + "timestamp": 3.7636275381592217 + }, + { + "x": 8.185948757189772, + "y": 7.412401444181256, + "heading": 0.007947733329215964, + "angularVelocity": 0.08230133185775951, + "velocityX": -0.0063460233869197455, + "velocityY": -0.5890426736452052, + "timestamp": 3.8601962467063924 + }, + { + "x": 8.130028903552427, + "y": 7.338629187384324, + "heading": 0.018106821496109617, + "angularVelocity": 0.10520062160644154, + "velocityX": -0.5790680488393326, + "velocityY": -0.7639354186961682, + "timestamp": 3.956764955253563 + }, + { + "x": 8.018757435307506, + "y": 7.248113553962043, + "heading": 0.03036067277368815, + "angularVelocity": 0.12689256656666278, + "velocityX": -1.1522517999768782, + "velocityY": -0.9373184625128009, + "timestamp": 4.053333663800734 + }, + { + "x": 7.852062035550644, + "y": 7.141094085596084, + "heading": 0.044518821199191705, + "angularVelocity": 0.14661217529473056, + "velocityX": -1.7261844158911652, + "velocityY": -1.1082209752622316, + "timestamp": 4.1499023723479045 + }, + { + "x": 7.629805209867524, + "y": 7.0180373220836785, + "heading": 0.060212869798815634, + "angularVelocity": 0.16251691501039256, + "velocityX": -2.301540830636187, + "velocityY": -1.2742923185339627, + "timestamp": 4.246471080895075 + }, + { + "x": 7.351625362652527, + "y": 6.880249252941204, + "heading": 0.07642107570328951, + "angularVelocity": 0.16784117907672624, + "velocityX": -2.8806416840411067, + "velocityY": -1.4268397208104806, + "timestamp": 4.343039789442246 + }, + { + "x": 7.016267448829526, + "y": 6.750124387193528, + "heading": 0.07642107964742734, + "angularVelocity": 4.0842814169097235e-8, + "velocityX": -3.4727389323964073, + "velocityY": -1.347484787829737, + "timestamp": 4.439608497989417 + }, + { + "x": 6.673392879085966, + "y": 6.641338156398843, + "heading": 0.0764210813675651, + "angularVelocity": 1.7812579245729294e-8, + "velocityX": -3.5505763192025745, + "velocityY": -1.1265163677895376, + "timestamp": 4.536177206536587 + }, + { + "x": 6.330518244592264, + "y": 6.532552129685128, + "heading": 0.0764210830877029, + "angularVelocity": 1.7812579496548032e-8, + "velocityX": -3.550576989711091, + "velocityY": -1.1265142544655247, + "timestamp": 4.632745915083758 + }, + { + "x": 5.987643610098044, + "y": 6.42376610297305, + "heading": 0.07642108480784073, + "angularVelocity": 1.7812579676147597e-8, + "velocityX": -3.550576989716468, + "velocityY": -1.1265142544485776, + "timestamp": 4.729314623630929 + }, + { + "x": 5.6447689756038235, + "y": 6.314980076260972, + "heading": 0.0764210865279785, + "angularVelocity": 1.7812579243167278e-8, + "velocityX": -3.550576989716468, + "velocityY": -1.1265142544485782, + "timestamp": 4.825883332178099 + }, + { + "x": 5.3018943411135915, + "y": 6.20619404953636, + "heading": 0.07642108824812432, + "angularVelocity": 1.7812662732207566e-8, + "velocityX": -3.5505769896751667, + "velocityY": -1.1265142545783664, + "timestamp": 4.92245204072527 + }, + { + "x": 4.971178879857472, + "y": 6.101264896222856, + "heading": 0.09246038898135349, + "angularVelocity": 0.16609211176718244, + "velocityX": -3.4246648446642007, + "velocityY": -1.0865750913739254, + "timestamp": 5.019020749272441 + }, + { + "x": 4.695582635189411, + "y": 6.01382394107076, + "heading": 0.10582487967853092, + "angularVelocity": 0.13839359455293215, + "velocityX": -2.8538876496773393, + "velocityY": -0.9054791812752113, + "timestamp": 5.1155894578196115 + }, + { + "x": 4.475105628641342, + "y": 5.943871182253239, + "heading": 0.1165156132920598, + "angularVelocity": 0.11070598099908092, + "velocityX": -2.2831102317203804, + "velocityY": -0.7243832900939244, + "timestamp": 5.212158166366782 + }, + { + "x": 4.309747868917504, + "y": 5.8914066159860985, + "heading": 0.12453333874296012, + "angularVelocity": 0.08302612276298497, + "velocityX": -1.7123327236282266, + "velocityY": -0.5432874380991931, + "timestamp": 5.308726874913953 + }, + { + "x": 4.19950936065589, + "y": 5.856430239360431, + "heading": 0.12987847932741303, + "angularVelocity": 0.055350647894830074, + "velocityX": -1.1415551675081796, + "velocityY": -0.36219161622714885, + "timestamp": 5.405295583461124 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.027676321443494054, + "velocityX": -0.5707775871082961, + "velocityY": -0.18109580929158178, + "timestamp": 5.501864292008294 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 2.6946819475075084e-27, + "velocityX": -6.238393680699965e-27, + "velocityY": 1.2085111651652188e-26, + "timestamp": 5.598433000555465 + }, + { + "x": 4.164497272188891, + "y": 5.856383810972102, + "heading": 0.11953980585413508, + "angularVelocity": -0.19788617668780903, + "velocityX": 0.30580479589931786, + "velocityY": 0.26526731175760876, + "timestamp": 5.664184637734952 + }, + { + "x": 4.205280500846038, + "y": 5.8906190238483225, + "heading": 0.09417342379390335, + "angularVelocity": -0.38579088138881684, + "velocityX": 0.6202617973727179, + "velocityY": 0.520674683472388, + "timestamp": 5.729936274914438 + }, + { + "x": 4.267438293143164, + "y": 5.9407879010107, + "heading": 0.05730532907136487, + "angularVelocity": -0.5607175167653599, + "velocityX": 0.9453421232303373, + "velocityY": 0.7630057488215584, + "timestamp": 5.795687912093925 + }, + { + "x": 4.351839190303838, + "y": 6.005705720740894, + "heading": 0.010093847833175575, + "angularVelocity": -0.7180274630928684, + "velocityX": 1.283631872622101, + "velocityY": 0.9873186815559245, + "timestamp": 5.8614395492734115 + }, + { + "x": 4.459568016776304, + "y": 6.083661828301211, + "heading": -0.04581026085444041, + "angularVelocity": -0.850231432793241, + "velocityX": 1.6384204423441482, + "velocityY": 1.1856146995627632, + "timestamp": 5.927191186452898 + }, + { + "x": 4.591948294079918, + "y": 6.172045469361953, + "heading": -0.1079292379524144, + "angularVelocity": -0.944751792695351, + "velocityX": 2.013338115707256, + "velocityY": 1.344204416073704, + "timestamp": 5.992942823632385 + }, + { + "x": 4.7504027579860635, + "y": 6.266641095517782, + "heading": -0.17238119577808125, + "angularVelocity": -0.9802335058171755, + "velocityX": 2.409893817147125, + "velocityY": 1.438680924363996, + "timestamp": 6.058694460811871 + }, + { + "x": 4.935641035495618, + "y": 6.360501248168881, + "heading": -0.23308994891442564, + "angularVelocity": -0.9233040535648408, + "velocityX": 2.8172420559490887, + "velocityY": 1.4274952940697678, + "timestamp": 6.124446097991358 + }, + { + "x": 5.145232187341836, + "y": 6.443576909061307, + "heading": -0.2814508136896339, + "angularVelocity": -0.7355081462564137, + "velocityX": 3.1876187550141757, + "velocityY": 1.2634766897993692, + "timestamp": 6.190197735170845 + }, + { + "x": 5.371831569570122, + "y": 6.506507878011031, + "heading": -0.310075861000899, + "angularVelocity": -0.4353510960210094, + "velocityX": 3.446292624010618, + "velocityY": 0.9571011711531484, + "timestamp": 6.255949372350331 + }, + { + "x": 5.608723728071605, + "y": 6.544912225542492, + "heading": -0.32489986784082275, + "angularVelocity": -0.22545456624080287, + "velocityX": 3.6028328519763595, + "velocityY": 0.5840819967208853, + "timestamp": 6.321701009529818 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.3320371023810657, + "angularVelocity": -0.10854839280670678, + "velocityX": 3.6853903618544206, + "velocityY": 0.18577139494688796, + "timestamp": 6.3874526467093045 + }, + { + "x": 5.9711094207782285, + "y": 6.556647574574296, + "heading": -0.33458795200315017, + "angularVelocity": -0.07861324883762467, + "velocityX": 3.700225495398522, + "velocityY": -0.014775117904477024, + "timestamp": 6.419900734783533 + }, + { + "x": 6.091317843363461, + "y": 6.549645373760147, + "heading": -0.3360778340908156, + "angularVelocity": -0.045915866730181235, + "velocityX": 3.704638076371226, + "velocityY": -0.21579702317530708, + "timestamp": 6.452348822857761 + }, + { + "x": 6.211267508828441, + "y": 6.536122994121453, + "heading": -0.33661432797946356, + "angularVelocity": -0.016533913721531207, + "velocityX": 3.6966635812434836, + "velocityY": -0.4167388724956589, + "timestamp": 6.48479691093199 + }, + { + "x": 6.330469045455405, + "y": 6.51611559729621, + "heading": -0.33661493169123347, + "angularVelocity": -0.000018605465091905695, + "velocityX": 3.6736074049811314, + "velocityY": -0.6165970943950513, + "timestamp": 6.517244999006218 + }, + { + "x": 6.448416412815129, + "y": 6.489700510555554, + "heading": -0.3366150129853366, + "angularVelocity": -0.0000025053588043472207, + "velocityX": 3.6349558436203777, + "velocityY": -0.8140722091307351, + "timestamp": 6.549693087080446 + }, + { + "x": 6.564765294299518, + "y": 6.456954843497344, + "heading": -0.3366150728513962, + "angularVelocity": -0.000001844979570705055, + "velocityX": 3.5856929757534926, + "velocityY": -1.0091709250573346, + "timestamp": 6.582141175154675 + }, + { + "x": 6.679176170718401, + "y": 6.417974154303709, + "heading": -0.33661511946704875, + "angularVelocity": -0.0000014366224727676693, + "velocityX": 3.525966650397334, + "velocityY": -1.2013246852776633, + "timestamp": 6.614589263228903 + }, + { + "x": 6.791315180749945, + "y": 6.372872197072731, + "heading": -0.33661515729887875, + "angularVelocity": -0.000001165918618828322, + "velocityX": 3.4559512343227956, + "velocityY": -1.389972719742485, + "timestamp": 6.647037351303132 + }, + { + "x": 6.900855096060357, + "y": 6.3217805921945684, + "heading": -0.3366151890219745, + "angularVelocity": -9.776568561253354e-7, + "velocityX": 3.375851145985221, + "velocityY": -1.5745644169014148, + "timestamp": 6.67948543937736 + }, + { + "x": 7.007476282450098, + "y": 6.264848452420557, + "heading": -0.33661521634397956, + "angularVelocity": -8.420220318148558e-7, + "velocityX": 3.285900424882802, + "velocityY": -1.7545606891775674, + "timestamp": 6.711933527451588 + }, + { + "x": 7.110867767687471, + "y": 6.2022421991346945, + "heading": -0.33661529971413134, + "angularVelocity": -0.0000025693394187906504, + "velocityX": 3.186366019497228, + "velocityY": -1.9294281112232177, + "timestamp": 6.744381615525817 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.25801763236442515, + "velocityX": 3.2027040434173877, + "velocityY": -1.73803251899956, + "timestamp": 6.776829703600045 + }, + { + "x": 7.403072746116238, + "y": 6.061144053185023, + "heading": -0.35924792619770074, + "angularVelocity": -0.23258228867402195, + "velocityX": 3.0708274318237563, + "velocityY": -1.3814614025635412, + "timestamp": 6.8381432615033795 + }, + { + "x": 7.5736331381530135, + "y": 5.991566896909843, + "heading": -0.3703276689612211, + "angularVelocity": -0.18070624413915942, + "velocityX": 2.781772871600061, + "velocityY": -1.1347760373794369, + "timestamp": 6.899456819406714 + }, + { + "x": 7.72439550755076, + "y": 5.934268464416171, + "heading": -0.37789043666294847, + "angularVelocity": -0.12334576495545502, + "velocityX": 2.4588749136926022, + "velocityY": -0.934514884685174, + "timestamp": 6.960770377310048 + }, + { + "x": 7.854564778976688, + "y": 5.88786008036463, + "heading": -0.38180542343665536, + "angularVelocity": -0.06385189357106222, + "velocityX": 2.1230095900021, + "velocityY": -0.7569024802753538, + "timestamp": 7.022083935213383 + }, + { + "x": 7.963733931476252, + "y": 5.851532970517725, + "heading": -0.38200457597908144, + "angularVelocity": -0.0032480995922642706, + "velocityX": 1.7805059147224407, + "velocityY": -0.592480865393233, + "timestamp": 7.083397493116717 + }, + { + "x": 8.05165855711374, + "y": 5.8247603198900775, + "heading": -0.3784464795942301, + "angularVelocity": 0.0580311517798547, + "velocityX": 1.434016042195889, + "velocityY": -0.43665139559925853, + "timestamp": 7.144711051020051 + }, + { + "x": 8.118176532906231, + "y": 5.80717243118701, + "heading": -0.3711033743685625, + "angularVelocity": 0.11976315641712676, + "velocityX": 1.084882007619954, + "velocityY": -0.2868515431904276, + "timestamp": 7.206024608923386 + }, + { + "x": 8.16317279255834, + "y": 5.798495818445154, + "heading": -0.3599553749598749, + "angularVelocity": 0.18181948316004298, + "velocityX": 0.7338712870495722, + "velocityY": -0.1415121392161809, + "timestamp": 7.26733816682672 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.24412049957493756, + "velocityX": 0.38146199167222045, + "velocityY": 0.0003958300819017007, + "timestamp": 7.328651724730054 + }, + { + "x": 8.186562507619959, + "y": 5.808483646148046, + "heading": -0.3243738625034631, + "angularVelocity": 0.3115588640226202, + "velocityX": 0.000013952657501305156, + "velocityY": 0.1505914724827301, + "timestamp": 7.394814554457269 + }, + { + "x": 8.161321884729395, + "y": 5.828379592770816, + "heading": -0.29947359998774176, + "angularVelocity": 0.37634820968788424, + "velocityX": -0.38149249351379677, + "velocityY": 0.30071184537904955, + "timestamp": 7.460977384184483 + }, + { + "x": 8.110834888181197, + "y": 5.858201520747478, + "heading": -0.2705090334908383, + "angularVelocity": 0.43777702096967036, + "velocityX": -0.7630719054241456, + "velocityY": 0.45073537663996555, + "timestamp": 7.527140213911697 + }, + { + "x": 8.035095319337403, + "y": 5.897940888331321, + "heading": -0.23777201719034075, + "angularVelocity": 0.494794682081624, + "velocityX": -1.144745004953147, + "velocityY": 0.600629806005663, + "timestamp": 7.593303043638912 + }, + { + "x": 7.934094943360084, + "y": 5.947585769746669, + "heading": -0.20166340599285526, + "angularVelocity": 0.5457537313074324, + "velocityX": -1.526542567688504, + "velocityY": 0.7503439864956146, + "timestamp": 7.659465873366126 + }, + { + "x": 7.8078223399404525, + "y": 6.0071184189660025, + "heading": -0.1627692568761995, + "angularVelocity": 0.587854982578802, + "velocityX": -1.9085127395585555, + "velocityY": 0.8997899495046319, + "timestamp": 7.72562870309334 + }, + { + "x": 7.656260799272679, + "y": 6.076509799690328, + "heading": -0.12202859757436733, + "angularVelocity": 0.6157635559090158, + "velocityX": -2.290735467220094, + "velocityY": 1.0487970513114737, + "timestamp": 7.791791532820555 + }, + { + "x": 7.479384511526562, + "y": 6.155704239729163, + "heading": -0.08118783305769839, + "angularVelocity": 0.6172765688083961, + "velocityX": -2.6733482905034087, + "velocityY": 1.1969627110168721, + "timestamp": 7.857954362547769 + }, + { + "x": 7.277158908396328, + "y": 6.244553850532033, + "heading": -0.04460973018212138, + "angularVelocity": 0.5528497349098663, + "velocityX": -3.0564835869928735, + "velocityY": 1.342893149661115, + "timestamp": 7.924117192274983 + }, + { + "x": 7.0505381507723195, + "y": 6.341424218836914, + "heading": -0.04460966986271364, + "angularVelocity": 9.116811959714027e-7, + "velocityX": -3.425197479587153, + "velocityY": 1.464120695929603, + "timestamp": 7.990280022002198 + }, + { + "x": 6.821387916460742, + "y": 6.432149176298842, + "heading": -0.04460966140291972, + "angularVelocity": 1.278632421205442e-7, + "velocityX": -3.4634285633844892, + "velocityY": 1.371237563991483, + "timestamp": 8.056442851729411 + }, + { + "x": 6.585463067264655, + "y": 6.503425491915177, + "heading": -0.044609652839645464, + "angularVelocity": 1.2942726746927926e-7, + "velocityX": -3.5658216277748638, + "velocityY": 1.0772863843672305, + "timestamp": 8.122605681456625 + }, + { + "x": 6.34313048184035, + "y": 6.548322764309462, + "heading": -0.044609643757262056, + "angularVelocity": 1.3727320078677108e-7, + "velocityX": -3.662669605024884, + "velocityY": 0.6785875480144041, + "timestamp": 8.188768511183838 + }, + { + "x": 6.097330137085011, + "y": 6.566295976512413, + "heading": -0.04460963356805428, + "angularVelocity": 1.5400199501081406e-7, + "velocityX": -3.715082105296292, + "velocityY": 0.27165120169518303, + "timestamp": 8.254931340911051 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.04460962128616813, + "angularVelocity": 1.8563121019327388e-7, + "velocityX": -3.722421790171331, + "velocityY": -0.13858200516588748, + "timestamp": 8.321094170638265 + }, + { + "x": 5.594131845425269, + "y": 6.517387984060631, + "heading": -0.0446096110934515, + "angularVelocity": 1.4604851016203596e-7, + "velocityX": -3.6812230501933914, + "velocityY": -0.5694089339833474, + "timestamp": 8.390884109764682 + }, + { + "x": 5.343562863816817, + "y": 6.448118054894418, + "heading": -0.04460960236123058, + "angularVelocity": 1.2512148631224918e-7, + "velocityX": -3.590330995339737, + "velocityY": -0.9925489265829068, + "timestamp": 8.4606740488911 + }, + { + "x": 5.102719675691794, + "y": 6.350252358061884, + "heading": -0.04460958274491264, + "angularVelocity": 2.810765876662287e-7, + "velocityX": -3.4509728929374397, + "velocityY": -1.4022894711981222, + "timestamp": 8.530463988017518 + }, + { + "x": 4.88810249039924, + "y": 6.239912108360582, + "heading": -0.010754389574110686, + "angularVelocity": 0.4851013426086585, + "velocityX": -3.0751880282313135, + "velocityY": -1.5810337576227271, + "timestamp": 8.600253927143935 + }, + { + "x": 4.7014054819057804, + "y": 6.141094298946399, + "heading": 0.022653281130584686, + "angularVelocity": 0.47868892168225635, + "velocityX": -2.6751278311803, + "velocityY": -1.415932019014718, + "timestamp": 8.670043866270353 + }, + { + "x": 4.541859398687963, + "y": 6.055486217406872, + "heading": 0.05282297144244177, + "angularVelocity": 0.43229283030620586, + "velocityX": -2.286090018345113, + "velocityY": -1.2266536210105516, + "timestamp": 8.73983380539677 + }, + { + "x": 4.409161403799561, + "y": 5.983676432778196, + "heading": 0.07876234420420766, + "angularVelocity": 0.37167782471881955, + "velocityX": -1.9013914691633662, + "velocityY": -1.0289417862737966, + "timestamp": 8.809623744523188 + }, + { + "x": 4.303150560780497, + "y": 5.925963754570679, + "heading": 0.09996559761955173, + "angularVelocity": 0.3038153304151236, + "velocityX": -1.5189989323107926, + "velocityY": -0.8269483958565483, + "timestamp": 8.879413683649606 + }, + { + "x": 4.223727158834788, + "y": 5.882528846842074, + "heading": 0.11612624955874849, + "angularVelocity": 0.23156134167022593, + "velocityX": -1.1380351228253833, + "velocityY": -0.6223663220271061, + "timestamp": 8.949203622776023 + }, + { + "x": 4.170823380513197, + "y": 5.853492711046949, + "heading": 0.1270379483840901, + "angularVelocity": 0.15635059955527503, + "velocityX": -0.7580430500986731, + "velocityY": -0.41605045309653765, + "timestamp": 9.018993561902441 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.0789970249504139, + "velocityX": -0.37875479822591473, + "velocityY": -0.2084922310471305, + "timestamp": 9.088783501028859 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.1741097503964344e-26, + "velocityX": -8.106670406309328e-26, + "velocityY": -1.6795580659596768e-26, + "timestamp": 9.158573440155276 + }, + { + "x": 4.149229218776726, + "y": 5.815100726386144, + "heading": 0.12918022174703248, + "angularVelocity": -0.05380546988265158, + "velocityX": 0.07724016042252664, + "velocityY": -0.38054657832352085, + "timestamp": 9.221223654099454 + }, + { + "x": 4.159717289478366, + "y": 5.767595641795523, + "heading": 0.12259706469587855, + "angularVelocity": -0.1050779660069413, + "velocityX": 0.16740678189839092, + "velocityY": -0.758258936401222, + "timestamp": 9.283873868043631 + }, + { + "x": 4.176867312970839, + "y": 5.696690246498273, + "heading": 0.11300853276979762, + "angularVelocity": -0.1530486701070873, + "velocityX": 0.2737424569332407, + "velocityY": -1.1317662116912734, + "timestamp": 9.346524081987809 + }, + { + "x": 4.201974040585746, + "y": 5.602790592825652, + "heading": 0.10069220099176118, + "angularVelocity": -0.19658882233044642, + "velocityX": 0.4007444832874458, + "velocityY": -1.4987922268914513, + "timestamp": 9.409174295931987 + }, + { + "x": 4.236730391014469, + "y": 5.486551757326023, + "heading": 0.08603276386217854, + "angularVelocity": -0.2339886204162749, + "velocityX": 0.5547682639949316, + "velocityY": -1.8553621477366304, + "timestamp": 9.471824509876164 + }, + { + "x": 4.283393708275682, + "y": 5.349087774330418, + "heading": 0.06958159023755481, + "angularVelocity": -0.2625876687233969, + "velocityX": 0.7448229514872947, + "velocityY": -2.194150256503298, + "timestamp": 9.534474723820342 + }, + { + "x": 4.344983097190331, + "y": 5.19239819137041, + "heading": 0.0521501823955064, + "angularVelocity": -0.27823381190014773, + "velocityX": 0.983067495500113, + "velocityY": -2.501022312543423, + "timestamp": 9.59712493776452 + }, + { + "x": 4.425324635342364, + "y": 5.020190178284826, + "heading": 0.03492497946543372, + "angularVelocity": -0.2749424438585406, + "velocityX": 1.2823825026937383, + "velocityY": -2.748721867718171, + "timestamp": 9.659775151708697 + }, + { + "x": 4.5282801412548075, + "y": 4.838988810812342, + "heading": 0.019478906624106312, + "angularVelocity": -0.24654461443802958, + "velocityX": 1.6433384569792093, + "velocityY": -2.892270529737325, + "timestamp": 9.722425365652875 + }, + { + "x": 4.655561045937701, + "y": 4.65789803685685, + "heading": 0.007446897298022715, + "angularVelocity": -0.1920505704386623, + "velocityX": 2.031611652536461, + "velocityY": -2.890505276116785, + "timestamp": 9.785075579597052 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -3.0154063280737095e-24, + "angularVelocity": -0.11886467466269138, + "velocityX": 2.394177429870417, + "velocityY": -2.752084488523512, + "timestamp": 9.84772579354123 + }, + { + "x": 5.0265563719767545, + "y": 4.290478604211027, + "heading": -0.0011711585949722948, + "angularVelocity": -0.014781576411206173, + "velocityX": 2.789308345003365, + "velocityY": -2.461168374898756, + "timestamp": 9.926956760685195 + }, + { + "x": 5.271648004487176, + "y": 4.1260554019145586, + "heading": -0.0011711658442560332, + "angularVelocity": -9.149558562553705e-8, + "velocityX": 3.0933818094771004, + "velocityY": -2.075239117019812, + "timestamp": 10.006187727829161 + }, + { + "x": 5.5362482788476575, + "y": 3.9953205544154557, + "heading": -0.0011711670074451403, + "angularVelocity": -1.4680990893194224e-8, + "velocityX": 3.3396067711718436, + "velocityY": -1.650047351581014, + "timestamp": 10.085418694973127 + }, + { + "x": 5.815753541872799, + "y": 3.900548632610082, + "heading": -0.0011711678822933615, + "angularVelocity": -1.1041746080675712e-8, + "velocityX": 3.5277275174146143, + "velocityY": -1.196147481491295, + "timestamp": 10.164649662117093 + }, + { + "x": 6.105300821187424, + "y": 3.8433884686557884, + "heading": -0.001171168612339158, + "angularVelocity": -9.214147233371614e-9, + "velocityX": 3.654471095733393, + "velocityY": -0.7214371604278238, + "timestamp": 10.243880629261058 + }, + { + "x": 6.399852439542986, + "y": 3.8248343191470044, + "heading": -0.0011711692753051952, + "angularVelocity": -8.367511605727905e-9, + "velocityX": 3.717632498671273, + "velocityY": -0.2341780010721126, + "timestamp": 10.323111596405024 + }, + { + "x": 6.694725372224164, + "y": 3.8372788433696505, + "heading": -0.0011711699217424045, + "angularVelocity": -8.15889585625018e-9, + "velocityX": 3.721687912068309, + "velocityY": 0.15706641823560874, + "timestamp": 10.40234256354899 + }, + { + "x": 6.9895982402615005, + "y": 3.8497248992335154, + "heading": -0.0011711705681793068, + "angularVelocity": -8.15889197720244e-9, + "velocityX": 3.7216870961771913, + "velocityY": 0.157085749581342, + "timestamp": 10.481573530692955 + }, + { + "x": 7.283138211598938, + "y": 3.880370995696298, + "heading": -0.0011711735014489986, + "angularVelocity": -3.702175800028382e-8, + "velocityX": 3.7048641701427636, + "velocityY": 0.386794426061941, + "timestamp": 10.560804497836921 + }, + { + "x": 7.537795284833578, + "y": 3.9128988141490386, + "heading": -0.0022060584164621646, + "angularVelocity": -0.01306162164009358, + "velocityX": 3.2141103714147428, + "velocityY": 0.4105442559300831, + "timestamp": 10.640035464980887 + }, + { + "x": 7.753528037916665, + "y": 3.9448039763834073, + "heading": -0.003047966804873468, + "angularVelocity": -0.010626001660203437, + "velocityX": 2.722833771435503, + "velocityY": 0.40268550775602213, + "timestamp": 10.719266432124853 + }, + { + "x": 7.930345641638023, + "y": 3.9756628799518334, + "heading": -0.0034891865809292344, + "angularVelocity": -0.005568779379583401, + "velocityX": 2.23167291899988, + "velocityY": 0.3894803342783207, + "timestamp": 10.798497399268818 + }, + { + "x": 8.068253246603454, + "y": 4.005300685276172, + "heading": -0.003443969014949724, + "angularVelocity": 0.0005707057178457502, + "velocityX": 1.7405770740479334, + "velocityY": 0.3740684531905991, + "timestamp": 10.877728366412784 + }, + { + "x": 8.167254001972685, + "y": 4.033621928989228, + "heading": -0.0028654925288464014, + "angularVelocity": 0.007301141295577129, + "velocityX": 1.2495209756728338, + "velocityY": 0.3574516976625533, + "timestamp": 10.95695933355675 + }, + { + "x": 8.227350013878981, + "y": 4.060566470670501, + "heading": -0.0017242628170792867, + "angularVelocity": 0.014403834168696394, + "velocityX": 0.7584914594958708, + "velocityY": 0.340075890179585, + "timestamp": 11.036190300700715 + }, + { + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 1.0796890687236353e-23, + "angularVelocity": 0.021762486048494588, + "velocityX": 0.2674809172408869, + "velocityY": 0.32217804683224427, + "timestamp": 11.115421267844681 + }, + { + "x": 8.231523012343859, + "y": 4.109892958457586, + "heading": 0.0022863134326364947, + "angularVelocity": 0.029215203502289427, + "velocityX": -0.21748380315842067, + "velocityY": 0.3041237094875908, + "timestamp": 11.193678926822047 + }, + { + "x": 8.176551036095814, + "y": 4.132280075994507, + "heading": 0.005155847293198806, + "angularVelocity": 0.03666777026121207, + "velocityX": -0.7024485138757453, + "velocityY": 0.2860693487316991, + "timestamp": 11.271936585799413 + }, + { + "x": 8.083626857911096, + "y": 4.153254299063425, + "heading": 0.008608587113368522, + "angularVelocity": 0.04412015213959263, + "velocityX": -1.1874132116780545, + "velocityY": 0.26801495652947127, + "timestamp": 11.350194244776779 + }, + { + "x": 7.952750479206828, + "y": 4.172815624183239, + "heading": 0.012644515987955914, + "angularVelocity": 0.051572317998353566, + "velocityX": -1.6723778913719014, + "velocityY": 0.24996051984474485, + "timestamp": 11.428451903754144 + }, + { + "x": 7.783921902112919, + "y": 4.190964046074225, + "heading": 0.017263614398582115, + "angularVelocity": 0.059024234445374815, + "velocityX": -2.1573425438491505, + "velocityY": 0.2319060156940586, + "timestamp": 11.50670956273151 + }, + { + "x": 7.57714113018582, + "y": 4.207699555855796, + "heading": 0.022465858708652038, + "angularVelocity": 0.06647584885684583, + "velocityX": -2.642307150880996, + "velocityY": 0.2138513980645823, + "timestamp": 11.584967221708876 + }, + { + "x": 7.332408170546637, + "y": 4.223022135701224, + "heading": 0.028251214754351595, + "angularVelocity": 0.07392702671278313, + "velocityX": -3.1272716669171876, + "velocityY": 0.19579655263979157, + "timestamp": 11.663224880686242 + }, + { + "x": 7.049723044551417, + "y": 4.23693173264099, + "heading": 0.03461960291604749, + "angularVelocity": 0.08137718716500116, + "velocityX": -3.6122359100593893, + "velocityY": 0.1777410303544707, + "timestamp": 11.741482539663608 + }, + { + "x": 6.758414971904035, + "y": 4.226087741393702, + "heading": 0.03461960351130125, + "angularVelocity": 7.606332300457526e-9, + "velocityX": -3.7224225264857154, + "velocityY": -0.13856779501191718, + "timestamp": 11.819740198640973 + }, + { + "x": 6.467106926605873, + "y": 4.215243015473263, + "heading": 0.03461960410653643, + "angularVelocity": 7.606094897280975e-9, + "velocityX": -3.7224221770091352, + "velocityY": -0.13857718288730186, + "timestamp": 11.89799785761834 + }, + { + "x": 6.175798881305749, + "y": 4.204398289605515, + "heading": 0.03461960470177156, + "angularVelocity": 7.606094342014038e-9, + "velocityX": -3.7224221770342005, + "velocityY": -0.1385771822140065, + "timestamp": 11.976255516595705 + }, + { + "x": 5.8844906838954465, + "y": 4.193557650451513, + "heading": 0.03461960529700769, + "angularVelocity": 7.606107036408581e-9, + "velocityX": -3.7224241207439843, + "velocityY": -0.13852496095158084, + "timestamp": 12.05451317557307 + }, + { + "x": 5.594009037113428, + "y": 4.218019822631225, + "heading": 0.034619605911650746, + "angularVelocity": 7.854094608871324e-9, + "velocityX": -3.7118622071998786, + "velocityY": 0.31258502361778046, + "timestamp": 12.132770834550437 + }, + { + "x": 5.309172831526295, + "y": 4.280038516789021, + "heading": 0.034619599952821836, + "angularVelocity": -7.614371543607384e-8, + "velocityX": -3.639723054704678, + "velocityY": 0.7924936034150137, + "timestamp": 12.211028493527802 + }, + { + "x": 5.03935943827081, + "y": 4.376755129785837, + "heading": 0.01971114929830447, + "angularVelocity": -0.19050468477250748, + "velocityX": -3.4477570218849003, + "velocityY": 1.235874089011402, + "timestamp": 12.289286152505168 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -1.0813807424036439e-23, + "angularVelocity": -0.25187501844395294, + "velocityX": -2.987601050001106, + "velocityY": 1.389310982379468, + "timestamp": 12.367543811482534 + }, + { + "x": 4.627457882261689, + "y": 4.587679677136334, + "heading": -0.019778853338755203, + "angularVelocity": -0.2848710655643308, + "velocityX": -2.5651244910957094, + "velocityY": 1.4719718180672714, + "timestamp": 12.43697470752518 + }, + { + "x": 4.478940134439646, + "y": 4.685562428002016, + "heading": -0.03941675041867698, + "angularVelocity": -0.28284089935782175, + "velocityX": -2.1390728953118847, + "velocityY": 1.4097866575934486, + "timestamp": 12.506405603567828 + }, + { + "x": 4.358424662792702, + "y": 4.772986561558444, + "heading": -0.05736759927865439, + "angularVelocity": -0.2585426644782356, + "velocityX": -1.7357614335398788, + "velocityY": 1.2591531802027813, + "timestamp": 12.575836499610475 + }, + { + "x": 4.264251963016929, + "y": 4.846292896298891, + "heading": -0.07266630938102059, + "angularVelocity": -0.220344414005101, + "velocityX": -1.3563514968599655, + "velocityY": 1.0558172070171679, + "timestamp": 12.645267395653121 + }, + { + "x": 4.19504667897931, + "y": 4.903178017800909, + "heading": -0.0846829764598402, + "angularVelocity": -0.17307377210627528, + "velocityX": -0.9967505531703156, + "velocityY": 0.8193055936809015, + "timestamp": 12.714698291695768 + }, + { + "x": 4.149718350868284, + "y": 4.942098244876492, + "heading": -0.09298341563379672, + "angularVelocity": -0.11954964788093829, + "velocityX": -0.6528552948990191, + "velocityY": 0.5605606335784127, + "timestamp": 12.784129187738415 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.06149221603273053, + "velocityX": -0.32142062647019837, + "velocityY": 0.2860902346287965, + "timestamp": 12.853560083781062 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -7.045263803296892e-25, + "velocityX": -1.4084469220925396e-25, + "velocityY": -2.7940612123980634e-25, + "timestamp": 12.922990979823709 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePdChaos.2.traj b/src/main/deploy/choreo/AmpLanePdChaos.2.traj new file mode 100644 index 00000000..ef7edf1d --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePdChaos.2.traj @@ -0,0 +1,599 @@ +{ + "samples": [ + { + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "angularVelocity": -6.615842884891707e-27, + "velocityX": 2.924635996734034e-23, + "velocityY": -9.88114513956692e-23, + "timestamp": 0 + }, + { + "x": 2.347136097791038, + "y": 6.1936465997058665, + "heading": 0.2985001179522956, + "angularVelocity": 1.0620750257524312e-16, + "velocityX": 0.4079003982651746, + "velocityY": 0.11324010619816775, + "timestamp": 0.06826358759088169 + }, + { + "x": 2.402825586267742, + "y": 6.209106951340729, + "heading": 0.2985001179522956, + "angularVelocity": 1.523734708742185e-16, + "velocityX": 0.8158007869504784, + "velocityY": 0.22648020973522806, + "timestamp": 0.13652717518176338 + }, + { + "x": 2.486359817783878, + "y": 6.2322974784599845, + "heading": 0.2985001179522956, + "angularVelocity": 1.8006542596605785e-16, + "velocityX": 1.2237011628626233, + "velocityY": 0.3397203097241459, + "timestamp": 0.20479076277264507 + }, + { + "x": 2.5977387911187284, + "y": 6.263218180724542, + "heading": 0.2985001179522956, + "angularVelocity": 1.3885125036469104e-16, + "velocityX": 1.6316015208923507, + "velocityY": 0.45296040474566596, + "timestamp": 0.27305435036352677 + }, + { + "x": 2.7369625044412174, + "y": 6.301869057625762, + "heading": 0.2985001179522956, + "angularVelocity": 1.7915030698796464e-16, + "velocityX": 2.0395018520984625, + "velocityY": 0.5662004923160927, + "timestamp": 0.34131793795440846 + }, + { + "x": 2.904030954699552, + "y": 6.348250108315915, + "heading": 0.2985001179522956, + "angularVelocity": 1.7115442013810006e-16, + "velocityX": 2.447402138598578, + "velocityY": 0.6794405674680408, + "timestamp": 0.40958152554529015 + }, + { + "x": 3.098944135790158, + "y": 6.402361331099544, + "heading": 0.2985001179522956, + "angularVelocity": 1.731300305768192e-16, + "velocityX": 2.8553023356868215, + "velocityY": 0.7926806177830709, + "timestamp": 0.47784511313617184 + }, + { + "x": 3.3217020294023807, + "y": 6.464202720890301, + "heading": 0.2985001179522956, + "angularVelocity": -4.291109894147179e-17, + "velocityX": 3.263202264540478, + "velocityY": 0.9059205935876805, + "timestamp": 0.5461087007270535 + }, + { + "x": 3.5667175760590744, + "y": 6.53222236284647, + "heading": 0.2985001179522956, + "angularVelocity": 2.176874790341123e-16, + "velocityX": 3.589256810308954, + "velocityY": 0.9964264164348671, + "timestamp": 0.6143722883179352 + }, + { + "x": 3.8162126541137695, + "y": 6.581329345703125, + "heading": 0.2985001179522956, + "angularVelocity": -3.979347495805123e-16, + "velocityX": 3.6548779057727367, + "velocityY": 0.7193730155374103, + "timestamp": 0.6826358759088169 + }, + { + "x": 4.06911506298554, + "y": 6.631106978006357, + "heading": 0.2985000768137015, + "angularVelocity": -5.945239455017304e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 0.7518317331682629 + }, + { + "x": 4.322017471857311, + "y": 6.6808846103095885, + "heading": 0.2985000356751073, + "angularVelocity": -5.945239469422448e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 0.8210275904277089 + }, + { + "x": 4.574919880729083, + "y": 6.730662242612821, + "heading": 0.29849999453651305, + "angularVelocity": -5.945239484047347e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 0.8902234476871547 + }, + { + "x": 4.827822289600854, + "y": 6.780439874916052, + "heading": 0.29849995339791874, + "angularVelocity": -5.945239490070907e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 0.9594193049466004 + }, + { + "x": 5.080724698472625, + "y": 6.830217507219284, + "heading": 0.29849991225932443, + "angularVelocity": -5.945239490087136e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.0286151622060462 + }, + { + "x": 5.3336271073443955, + "y": 6.879995139522516, + "heading": 0.29849987112073, + "angularVelocity": -5.945239499004978e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020961, + "timestamp": 1.097811019465492 + }, + { + "x": 5.586529516216166, + "y": 6.929772771825747, + "heading": 0.2984998299821356, + "angularVelocity": -5.945239509711758e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020961, + "timestamp": 1.1670068767249377 + }, + { + "x": 5.839431925087938, + "y": 6.979550404128979, + "heading": 0.2984997888435411, + "angularVelocity": -5.945239507812541e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.2362027339843835 + }, + { + "x": 6.092334333959709, + "y": 7.029328036432211, + "heading": 0.2984997477049465, + "angularVelocity": -5.945239524665447e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.3053985912438293 + }, + { + "x": 6.34523674283148, + "y": 7.079105668735443, + "heading": 0.29849970656635194, + "angularVelocity": -5.945239530479041e-7, + "velocityX": 3.6548778913675166, + "velocityY": 0.7193730127020963, + "timestamp": 1.374594448503275 + }, + { + "x": 6.598139151703251, + "y": 7.128883301038674, + "heading": 0.29849966542775724, + "angularVelocity": -5.945239544565078e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020962, + "timestamp": 1.4437903057627208 + }, + { + "x": 6.8510415605750214, + "y": 7.178660933341907, + "heading": 0.29849962428916255, + "angularVelocity": -5.945239549845622e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.5129861630221666 + }, + { + "x": 7.103943969446777, + "y": 7.228438565645134, + "heading": 0.29849958315051783, + "angularVelocity": -5.945246772244776e-7, + "velocityX": 3.654877891367282, + "velocityY": 0.7193730127020502, + "timestamp": 1.5821820202816124 + }, + { + "x": 7.353433038122826, + "y": 7.27754436570309, + "heading": 0.2857400994824136, + "angularVelocity": -0.18439664126514535, + "velocityX": 3.605549212875623, + "velocityY": 0.7096638729950924, + "timestamp": 1.6513778775410581 + }, + { + "x": 7.577591625127487, + "y": 7.32166448211996, + "heading": 0.20789341833491348, + "angularVelocity": -1.1250193903316845, + "velocityX": 3.239479874701043, + "velocityY": 0.6376121080695129, + "timestamp": 1.720573734800504 + }, + { + "x": 7.772618972429747, + "y": 7.360050828957839, + "heading": 0.13154587005744633, + "angularVelocity": -1.1033543235284506, + "velocityX": 2.818482999220841, + "velocityY": 0.5547492054900108, + "timestamp": 1.7897695920599497 + }, + { + "x": 7.938688452207964, + "y": 7.392737530275028, + "heading": 0.06829117903161311, + "angularVelocity": -0.9141398565041748, + "velocityX": 2.3999916520370626, + "velocityY": 0.47237945466347603, + "timestamp": 1.8589654493193954 + }, + { + "x": 8.075968311155844, + "y": 7.41975770130397, + "heading": 0.023429532114866793, + "angularVelocity": -0.6483285082882946, + "velocityX": 1.9839317610179508, + "velocityY": 0.3904882763086947, + "timestamp": 1.9281613065788412 + }, + { + "x": 8.184579349560657, + "y": 7.441135118616467, + "heading": 4.707584249891564e-24, + "angularVelocity": -0.33859732421579963, + "velocityX": 1.5696176434028992, + "velocityY": 0.3089407106026808, + "timestamp": 1.997357163838287 + }, + { + "x": 8.264610290527344, + "y": 7.456887245178223, + "heading": 4.305949741153467e-24, + "angularVelocity": 2.0877324046258187e-24, + "velocityX": 1.156585728342302, + "velocityY": 0.22764551500092414, + "timestamp": 2.0665530210977328 + }, + { + "x": 8.309090579989265, + "y": 7.457165429033817, + "heading": 0.013618338743485297, + "angularVelocity": 0.3126833682896741, + "velocityX": 1.021288058215364, + "velocityY": 0.006387230234869135, + "timestamp": 2.1101061490366293 + }, + { + "x": 8.347694581932599, + "y": 7.44777021311996, + "heading": 0.040485197139552155, + "angularVelocity": 0.6168755188780024, + "velocityX": 0.886365773716491, + "velocityY": -0.2157185111259548, + "timestamp": 2.153659276975526 + }, + { + "x": 8.38043908621254, + "y": 7.428658664796906, + "heading": 0.08013490839873996, + "angularVelocity": 0.9103757441902883, + "velocityX": 0.7518289920733539, + "velocityY": -0.4388100057903166, + "timestamp": 2.1972124049144224 + }, + { + "x": 8.407344118270622, + "y": 7.3997816960160065, + "heading": 0.13202626739740458, + "angularVelocity": 1.1914496490692066, + "velocityX": 0.6177520038475312, + "velocityY": -0.6630285847990697, + "timestamp": 2.240765532853319 + }, + { + "x": 8.428437067602909, + "y": 7.361082970163004, + "heading": 0.19556012614557247, + "angularVelocity": 1.4587668384531083, + "velocityX": 0.4843038911436893, + "velocityY": -0.8885406785775682, + "timestamp": 2.2843186607922155 + }, + { + "x": 8.443758202953443, + "y": 7.312496178717053, + "heading": 0.2700814705675102, + "angularVelocity": 1.7110446011245937, + "velocityX": 0.35178036746362773, + "velocityY": -1.1155752467220204, + "timestamp": 2.327871788731112 + }, + { + "x": 8.453367300003004, + "y": 7.253940317433782, + "heading": 0.354848982447824, + "angularVelocity": 1.9463013540437195, + "velocityX": 0.22062932111422345, + "velocityY": -1.3444697098546596, + "timestamp": 2.3714249166700085 + }, + { + "x": 8.45735124333647, + "y": 7.18531320673646, + "heading": 0.4489562663311165, + "angularVelocity": 2.1607468472831988, + "velocityX": 0.09147318509605634, + "velocityY": -1.5757102634190636, + "timestamp": 2.414978044608905 + }, + { + "x": 8.455833781714013, + "y": 7.106484187240998, + "heading": 0.5511965710396762, + "angularVelocity": 2.347484774273826, + "velocityX": -0.03484162204347062, + "velocityY": -1.8099508169896654, + "timestamp": 2.4585311725478016 + }, + { + "x": 8.448992060952849, + "y": 7.01728730637675, + "heading": 0.6598709993297996, + "angularVelocity": 2.4952152332799042, + "velocityX": -0.15708907913026152, + "velocityY": -2.0480017184847705, + "timestamp": 2.502084300486698 + }, + { + "x": 8.437091795531597, + "y": 6.917516589235624, + "heading": 0.7725277452001486, + "angularVelocity": 2.5866510903281643, + "velocityX": -0.2732356086558815, + "velocityY": -2.2907818993184903, + "timestamp": 2.5456374284255947 + }, + { + "x": 8.420568790056215, + "y": 6.806929386712691, + "heading": 0.8855178727468727, + "angularVelocity": 2.594305688106834, + "velocityX": -0.37937586247679217, + "velocityY": -2.539133416044943, + "timestamp": 2.5891905563644912 + }, + { + "x": 8.40022655182027, + "y": 6.685307877393552, + "heading": 0.992766575428426, + "angularVelocity": 2.462479912625774, + "velocityX": -0.46706721649209976, + "velocityY": -2.7924862133844903, + "timestamp": 2.632743684303388 + }, + { + "x": 8.377585411071777, + "y": 6.553085803985596, + "heading": 1.0808392342145412, + "angularVelocity": 2.0221890586062603, + "velocityX": -0.519851083491793, + "velocityY": -3.035880077165969, + "timestamp": 2.6762968122422843 + }, + { + "x": 8.331156557203096, + "y": 6.248001091638873, + "heading": 1.1393178439925227, + "angularVelocity": 0.6668090360061052, + "velocityX": -0.5294103161580237, + "velocityY": -3.478763324102056, + "timestamp": 2.7639959983964535 + }, + { + "x": 8.32509627497856, + "y": 5.9213777688010225, + "heading": 1.1393178431081437, + "angularVelocity": -1.0084234488389344e-8, + "velocityX": -0.06910306116049933, + "velocityY": -3.7243597935295467, + "timestamp": 2.851695184550623 + }, + { + "x": 8.328579577192095, + "y": 5.594716799276784, + "heading": 1.13931784103149, + "angularVelocity": -2.3679282154855844e-8, + "velocityX": 0.03971875186402571, + "velocityY": -3.7247890641766106, + "timestamp": 2.9393943707047923 + }, + { + "x": 8.33206298187959, + "y": 5.268055830845277, + "heading": 1.139317838954836, + "angularVelocity": -2.3679282513207347e-8, + "velocityX": 0.03971992033507496, + "velocityY": -3.724789051716606, + "timestamp": 3.0270935568589614 + }, + { + "x": 8.335546386568366, + "y": 4.941394862413785, + "heading": 1.1393178368781822, + "angularVelocity": -2.3679282515836517e-8, + "velocityX": 0.0397199203496886, + "velocityY": -3.7247890517164506, + "timestamp": 3.1147927430131306 + }, + { + "x": 8.339029791257143, + "y": 4.614733893982293, + "heading": 1.1393178348015287, + "angularVelocity": -2.3679282075329618e-8, + "velocityX": 0.039719920349696757, + "velocityY": -3.7247890517164497, + "timestamp": 3.2024919291672997 + }, + { + "x": 8.34251319594592, + "y": 4.2880729255508, + "heading": 1.139317832724875, + "angularVelocity": -2.3679282013341874e-8, + "velocityX": 0.03971992034969426, + "velocityY": -3.72478905171645, + "timestamp": 3.290191115321469 + }, + { + "x": 8.345996600634695, + "y": 3.9614119571193074, + "heading": 1.139317830648221, + "angularVelocity": -2.3679283402849396e-8, + "velocityX": 0.039719920349694765, + "velocityY": -3.7247890517164497, + "timestamp": 3.377890301475638 + }, + { + "x": 8.349480005323471, + "y": 3.6347509886878155, + "heading": 1.139317828571567, + "angularVelocity": -2.3679282271688366e-8, + "velocityX": 0.03971992034969471, + "velocityY": -3.7247890517164506, + "timestamp": 3.4655894876298072 + }, + { + "x": 8.352963410012249, + "y": 3.3080900202563233, + "heading": 1.139317826494913, + "angularVelocity": -2.367928303185141e-8, + "velocityX": 0.03971992034969481, + "velocityY": -3.72478905171645, + "timestamp": 3.5532886737839764 + }, + { + "x": 8.356446814701025, + "y": 2.981429051824831, + "heading": 1.1393178244182591, + "angularVelocity": -2.3679282848596997e-8, + "velocityX": 0.03971992034969499, + "velocityY": -3.72478905171645, + "timestamp": 3.6409878599381456 + }, + { + "x": 8.3599302193898, + "y": 2.6547680833933387, + "heading": 1.1393178223416056, + "angularVelocity": -2.3679282487737324e-8, + "velocityX": 0.0397199203496951, + "velocityY": -3.72478905171645, + "timestamp": 3.7286870460923147 + }, + { + "x": 8.363413624078582, + "y": 2.3281071149618464, + "heading": 1.1393178202649519, + "angularVelocity": -2.3679282618768576e-8, + "velocityX": 0.039719920349732145, + "velocityY": -3.7247890517164497, + "timestamp": 3.816386232246484 + }, + { + "x": 8.366897029211325, + "y": 2.001446146535259, + "heading": 1.139317818188219, + "angularVelocity": -2.3680183399714216e-8, + "velocityX": 0.03971992541207313, + "velocityY": -3.724789051660523, + "timestamp": 3.904085418400653 + }, + { + "x": 8.369950836003147, + "y": 1.715318694035658, + "heading": 1.1226153865662416, + "angularVelocity": -0.190451386773599, + "velocityX": 0.03482138119793182, + "velocityY": -3.262601000613718, + "timestamp": 3.991784604554822 + }, + { + "x": 8.372495686328486, + "y": 1.4768791360484652, + "heading": 1.1086925029820063, + "angularVelocity": -0.15875727238517048, + "velocityX": 0.029017946881124475, + "velocityY": -2.718834329523107, + "timestamp": 4.079483790708991 + }, + { + "x": 8.374531573132348, + "y": 1.2861274855011333, + "heading": 1.0975518982604389, + "angularVelocity": -0.12703201945321188, + "velocityX": 0.02321443211894871, + "velocityY": -2.1750675110257403, + "timestamp": 4.1671829768631605 + }, + { + "x": 8.376058491392191, + "y": 1.1430637450377992, + "heading": 1.0891955157796347, + "angularVelocity": -0.09528460693026461, + "velocityX": 0.017410860086641132, + "velocityY": -1.6313006623783024, + "timestamp": 4.25488216301733 + }, + { + "x": 8.377076437882634, + "y": 1.0476879159528336, + "heading": 1.0836245099802884, + "angularVelocity": -0.06352403076526633, + "velocityX": 0.01160725127658835, + "velocityY": -1.0875337989716487, + "timestamp": 4.342581349171499 + }, + { + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "angularVelocity": -0.03175942546206756, + "velocityX": 0.005803624998845044, + "velocityY": -0.5437669155675129, + "timestamp": 4.430280535325668 + }, + { + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "angularVelocity": 4.871302260765609e-25, + "velocityX": -2.093358596809338e-24, + "velocityY": 4.245353600645279e-24, + "timestamp": 4.517979721479837 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePdChaos.traj b/src/main/deploy/choreo/AmpLanePdChaos.traj new file mode 100644 index 00000000..f4f10ac8 --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePdChaos.traj @@ -0,0 +1,770 @@ +{ + "samples": [ + { + "x": 0.4269569218158722, + "y": 6.9908647537231445, + "heading": 1.0147582935939444e-26, + "angularVelocity": 4.802793352791378e-28, + "velocityX": 7.569777584926927e-27, + "velocityY": 3.719655610214095e-26, + "timestamp": 0 + }, + { + "x": 0.45154320453389124, + "y": 6.9860595473156515, + "heading": -0.0020588646196638234, + "angularVelocity": -0.03238986683838223, + "velocityX": 0.38678911458368936, + "velocityY": -0.07559506058978287, + "timestamp": 0.06356508441164857 + }, + { + "x": 0.500665330018184, + "y": 6.976191418092733, + "heading": -0.005903955941845083, + "angularVelocity": -0.06049061930415103, + "velocityX": 0.7727847125345899, + "velocityY": -0.15524449175608063, + "timestamp": 0.12713016882329714 + }, + { + "x": 0.5742528189740612, + "y": 6.960923422240335, + "heading": -0.01117745758299664, + "angularVelocity": -0.08296223768067817, + "velocityX": 1.1576715367720332, + "velocityY": -0.24019469168832944, + "timestamp": 0.1906952532349457 + }, + { + "x": 0.6722013148089163, + "y": 6.9397965653748726, + "heading": -0.017389498609317183, + "angularVelocity": -0.09772725205698236, + "velocityX": 1.5409166327938608, + "velocityY": -0.3323657486025543, + "timestamp": 0.2542603376465943 + }, + { + "x": 0.7943433521763879, + "y": 6.912149808159037, + "heading": -0.023829197654655073, + "angularVelocity": -0.10130874685282076, + "velocityX": 1.9215271795515552, + "velocityY": -0.43493621493200585, + "timestamp": 0.3178254220582428 + }, + { + "x": 0.940376377090631, + "y": 6.876952520080898, + "heading": -0.029373547858602904, + "angularVelocity": -0.08722320209696448, + "velocityX": 2.2973779751243755, + "velocityY": -0.553720464684682, + "timestamp": 0.38139050646989137 + }, + { + "x": 1.109639548430514, + "y": 6.832391950283203, + "heading": -0.03199489164843748, + "angularVelocity": -0.04123873686470252, + "velocityX": 2.6628324796004614, + "velocityY": -0.7010227424401742, + "timestamp": 0.4449555908815399 + }, + { + "x": 1.3001020816519337, + "y": 6.774603331599785, + "heading": -0.02710028887606378, + "angularVelocity": 0.07700143589326758, + "velocityX": 2.9963388703770333, + "velocityY": -0.909125178048654, + "timestamp": 0.5085206752931885 + }, + { + "x": 1.5002212524414062, + "y": 6.694304943084717, + "heading": 4.6927585573960014e-26, + "angularVelocity": 0.426339225801414, + "velocityX": 3.148256195075543, + "velocityY": -1.2632467849024598, + "timestamp": 0.572085759704837 + }, + { + "x": 1.6698109310260505, + "y": 6.606230489693705, + "heading": 0.043636088677614475, + "angularVelocity": 0.7393180152975136, + "velocityX": 2.8733259186555666, + "velocityY": -1.492228842060727, + "timestamp": 0.6311078419140127 + }, + { + "x": 1.8178691248742131, + "y": 6.519462655469564, + "heading": 0.09197841408379424, + "angularVelocity": 0.8190548960108419, + "velocityX": 2.508522036268403, + "velocityY": -1.4700910401065466, + "timestamp": 0.6901299241231884 + }, + { + "x": 1.945478769087034, + "y": 6.439654918114978, + "heading": 0.13899529896762525, + "angularVelocity": 0.7965982073828233, + "velocityX": 2.1620661189242583, + "velocityY": -1.3521674323814197, + "timestamp": 0.7491520063323641 + }, + { + "x": 2.05361266916171, + "y": 6.369142283614203, + "heading": 0.1819641027220741, + "angularVelocity": 0.7280123327768478, + "velocityX": 1.8320922615275888, + "velocityY": -1.1946822589361732, + "timestamp": 0.8081740885415398 + }, + { + "x": 2.1429230510824855, + "y": 6.309142790648472, + "heading": 0.2193838977387365, + "angularVelocity": 0.6339965249623981, + "velocityX": 1.5131689458914253, + "velocityY": -1.0165600859876827, + "timestamp": 0.8671961707507155 + }, + { + "x": 2.2138592843219276, + "y": 6.2603940232890976, + "heading": 0.25031075711822803, + "angularVelocity": 0.5239879418331249, + "velocityX": 1.2018592124222665, + "velocityY": -0.8259411653185552, + "timestamp": 0.9262182529598912 + }, + { + "x": 2.2667458111920182, + "y": 6.2233878173394785, + "heading": 0.2740979405057225, + "angularVelocity": 0.4030217589273135, + "velocityX": 0.8960464438150413, + "velocityY": -0.6269891634535774, + "timestamp": 0.985240335169067 + }, + { + "x": 2.301826622003525, + "y": 6.198474614172422, + "heading": 0.2902775915680301, + "angularVelocity": 0.27412877446387834, + "velocityX": 0.5943675569963707, + "velocityY": -0.4220996995457338, + "timestamp": 1.0442624173782427 + }, + { + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "angularVelocity": 0.1393127127424044, + "velocityX": 0.2959016450874765, + "velocityY": -0.2127710494914169, + "timestamp": 1.1032844995874185 + }, + { + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "angularVelocity": -6.615842884891707e-27, + "velocityX": 2.924635996734034e-23, + "velocityY": -9.88114513956692e-23, + "timestamp": 1.162306581796594 + }, + { + "x": 2.347136097791038, + "y": 6.1936465997058665, + "heading": 0.2985001179522956, + "angularVelocity": 1.0620750257524312e-16, + "velocityX": 0.4079003982651746, + "velocityY": 0.11324010619816775, + "timestamp": 1.2305701693874758 + }, + { + "x": 2.402825586267742, + "y": 6.209106951340729, + "heading": 0.2985001179522956, + "angularVelocity": 1.523734708742185e-16, + "velocityX": 0.8158007869504784, + "velocityY": 0.22648020973522806, + "timestamp": 1.2988337569783575 + }, + { + "x": 2.486359817783878, + "y": 6.2322974784599845, + "heading": 0.2985001179522956, + "angularVelocity": 1.8006542596605785e-16, + "velocityX": 1.2237011628626233, + "velocityY": 0.3397203097241459, + "timestamp": 1.3670973445692391 + }, + { + "x": 2.5977387911187284, + "y": 6.263218180724542, + "heading": 0.2985001179522956, + "angularVelocity": 1.3885125036469104e-16, + "velocityX": 1.6316015208923507, + "velocityY": 0.45296040474566596, + "timestamp": 1.4353609321601208 + }, + { + "x": 2.7369625044412174, + "y": 6.301869057625762, + "heading": 0.2985001179522956, + "angularVelocity": 1.7915030698796464e-16, + "velocityX": 2.0395018520984625, + "velocityY": 0.5662004923160927, + "timestamp": 1.5036245197510025 + }, + { + "x": 2.904030954699552, + "y": 6.348250108315915, + "heading": 0.2985001179522956, + "angularVelocity": 1.7115442013810006e-16, + "velocityX": 2.447402138598578, + "velocityY": 0.6794405674680408, + "timestamp": 1.5718881073418842 + }, + { + "x": 3.098944135790158, + "y": 6.402361331099544, + "heading": 0.2985001179522956, + "angularVelocity": 1.731300305768192e-16, + "velocityX": 2.8553023356868215, + "velocityY": 0.7926806177830709, + "timestamp": 1.640151694932766 + }, + { + "x": 3.3217020294023807, + "y": 6.464202720890301, + "heading": 0.2985001179522956, + "angularVelocity": -4.291109894147179e-17, + "velocityX": 3.263202264540478, + "velocityY": 0.9059205935876805, + "timestamp": 1.7084152825236476 + }, + { + "x": 3.5667175760590744, + "y": 6.53222236284647, + "heading": 0.2985001179522956, + "angularVelocity": 2.176874790341123e-16, + "velocityX": 3.589256810308954, + "velocityY": 0.9964264164348671, + "timestamp": 1.7766788701145293 + }, + { + "x": 3.8162126541137695, + "y": 6.581329345703125, + "heading": 0.2985001179522956, + "angularVelocity": -3.979347495805123e-16, + "velocityX": 3.6548779057727367, + "velocityY": 0.7193730155374103, + "timestamp": 1.844942457705411 + }, + { + "x": 4.06911506298554, + "y": 6.631106978006357, + "heading": 0.2985000768137015, + "angularVelocity": -5.945239455017304e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.914138314964857 + }, + { + "x": 4.322017471857311, + "y": 6.6808846103095885, + "heading": 0.2985000356751073, + "angularVelocity": -5.945239469422448e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 1.983334172224303 + }, + { + "x": 4.574919880729083, + "y": 6.730662242612821, + "heading": 0.29849999453651305, + "angularVelocity": -5.945239484047347e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.0525300294837487 + }, + { + "x": 4.827822289600854, + "y": 6.780439874916052, + "heading": 0.29849995339791874, + "angularVelocity": -5.945239490070907e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.1217258867431945 + }, + { + "x": 5.080724698472625, + "y": 6.830217507219284, + "heading": 0.29849991225932443, + "angularVelocity": -5.945239490087136e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.1909217440026403 + }, + { + "x": 5.3336271073443955, + "y": 6.879995139522516, + "heading": 0.29849987112073, + "angularVelocity": -5.945239499004978e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020961, + "timestamp": 2.260117601262086 + }, + { + "x": 5.586529516216166, + "y": 6.929772771825747, + "heading": 0.2984998299821356, + "angularVelocity": -5.945239509711758e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020961, + "timestamp": 2.329313458521532 + }, + { + "x": 5.839431925087938, + "y": 6.979550404128979, + "heading": 0.2984997888435411, + "angularVelocity": -5.945239507812541e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.3985093157809776 + }, + { + "x": 6.092334333959709, + "y": 7.029328036432211, + "heading": 0.2984997477049465, + "angularVelocity": -5.945239524665447e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.4677051730404234 + }, + { + "x": 6.34523674283148, + "y": 7.079105668735443, + "heading": 0.29849970656635194, + "angularVelocity": -5.945239530479041e-7, + "velocityX": 3.6548778913675166, + "velocityY": 0.7193730127020963, + "timestamp": 2.536901030299869 + }, + { + "x": 6.598139151703251, + "y": 7.128883301038674, + "heading": 0.29849966542775724, + "angularVelocity": -5.945239544565078e-7, + "velocityX": 3.6548778913675157, + "velocityY": 0.7193730127020962, + "timestamp": 2.606096887559315 + }, + { + "x": 6.8510415605750214, + "y": 7.178660933341907, + "heading": 0.29849962428916255, + "angularVelocity": -5.945239549845622e-7, + "velocityX": 3.654877891367516, + "velocityY": 0.7193730127020962, + "timestamp": 2.6752927448187607 + }, + { + "x": 7.103943969446777, + "y": 7.228438565645134, + "heading": 0.29849958315051783, + "angularVelocity": -5.945246772244776e-7, + "velocityX": 3.654877891367282, + "velocityY": 0.7193730127020502, + "timestamp": 2.7444886020782064 + }, + { + "x": 7.353433038122826, + "y": 7.27754436570309, + "heading": 0.2857400994824136, + "angularVelocity": -0.18439664126514535, + "velocityX": 3.605549212875623, + "velocityY": 0.7096638729950924, + "timestamp": 2.813684459337652 + }, + { + "x": 7.577591625127487, + "y": 7.32166448211996, + "heading": 0.20789341833491348, + "angularVelocity": -1.1250193903316845, + "velocityX": 3.239479874701043, + "velocityY": 0.6376121080695129, + "timestamp": 2.882880316597098 + }, + { + "x": 7.772618972429747, + "y": 7.360050828957839, + "heading": 0.13154587005744633, + "angularVelocity": -1.1033543235284506, + "velocityX": 2.818482999220841, + "velocityY": 0.5547492054900108, + "timestamp": 2.9520761738565438 + }, + { + "x": 7.938688452207964, + "y": 7.392737530275028, + "heading": 0.06829117903161311, + "angularVelocity": -0.9141398565041748, + "velocityX": 2.3999916520370626, + "velocityY": 0.47237945466347603, + "timestamp": 3.0212720311159895 + }, + { + "x": 8.075968311155844, + "y": 7.41975770130397, + "heading": 0.023429532114866793, + "angularVelocity": -0.6483285082882946, + "velocityX": 1.9839317610179508, + "velocityY": 0.3904882763086947, + "timestamp": 3.0904678883754353 + }, + { + "x": 8.184579349560657, + "y": 7.441135118616467, + "heading": 4.707584249891564e-24, + "angularVelocity": -0.33859732421579963, + "velocityX": 1.5696176434028992, + "velocityY": 0.3089407106026808, + "timestamp": 3.159663745634881 + }, + { + "x": 8.264610290527344, + "y": 7.456887245178223, + "heading": 4.305949741153467e-24, + "angularVelocity": 2.0877324046258187e-24, + "velocityX": 1.156585728342302, + "velocityY": 0.22764551500092414, + "timestamp": 3.228859602894327 + }, + { + "x": 8.309090579989265, + "y": 7.457165429033817, + "heading": 0.013618338743485297, + "angularVelocity": 0.3126833682896741, + "velocityX": 1.021288058215364, + "velocityY": 0.006387230234869135, + "timestamp": 3.2724127308332234 + }, + { + "x": 8.347694581932599, + "y": 7.44777021311996, + "heading": 0.040485197139552155, + "angularVelocity": 0.6168755188780024, + "velocityX": 0.886365773716491, + "velocityY": -0.2157185111259548, + "timestamp": 3.31596585877212 + }, + { + "x": 8.38043908621254, + "y": 7.428658664796906, + "heading": 0.08013490839873996, + "angularVelocity": 0.9103757441902883, + "velocityX": 0.7518289920733539, + "velocityY": -0.4388100057903166, + "timestamp": 3.3595189867110165 + }, + { + "x": 8.407344118270622, + "y": 7.3997816960160065, + "heading": 0.13202626739740458, + "angularVelocity": 1.1914496490692066, + "velocityX": 0.6177520038475312, + "velocityY": -0.6630285847990697, + "timestamp": 3.403072114649913 + }, + { + "x": 8.428437067602909, + "y": 7.361082970163004, + "heading": 0.19556012614557247, + "angularVelocity": 1.4587668384531083, + "velocityX": 0.4843038911436893, + "velocityY": -0.8885406785775682, + "timestamp": 3.4466252425888095 + }, + { + "x": 8.443758202953443, + "y": 7.312496178717053, + "heading": 0.2700814705675102, + "angularVelocity": 1.7110446011245937, + "velocityX": 0.35178036746362773, + "velocityY": -1.1155752467220204, + "timestamp": 3.490178370527706 + }, + { + "x": 8.453367300003004, + "y": 7.253940317433782, + "heading": 0.354848982447824, + "angularVelocity": 1.9463013540437195, + "velocityX": 0.22062932111422345, + "velocityY": -1.3444697098546596, + "timestamp": 3.5337314984666026 + }, + { + "x": 8.45735124333647, + "y": 7.18531320673646, + "heading": 0.4489562663311165, + "angularVelocity": 2.1607468472831988, + "velocityX": 0.09147318509605634, + "velocityY": -1.5757102634190636, + "timestamp": 3.577284626405499 + }, + { + "x": 8.455833781714013, + "y": 7.106484187240998, + "heading": 0.5511965710396762, + "angularVelocity": 2.347484774273826, + "velocityX": -0.03484162204347062, + "velocityY": -1.8099508169896654, + "timestamp": 3.6208377543443957 + }, + { + "x": 8.448992060952849, + "y": 7.01728730637675, + "heading": 0.6598709993297996, + "angularVelocity": 2.4952152332799042, + "velocityX": -0.15708907913026152, + "velocityY": -2.0480017184847705, + "timestamp": 3.6643908822832922 + }, + { + "x": 8.437091795531597, + "y": 6.917516589235624, + "heading": 0.7725277452001486, + "angularVelocity": 2.5866510903281643, + "velocityX": -0.2732356086558815, + "velocityY": -2.2907818993184903, + "timestamp": 3.707944010222189 + }, + { + "x": 8.420568790056215, + "y": 6.806929386712691, + "heading": 0.8855178727468727, + "angularVelocity": 2.594305688106834, + "velocityX": -0.37937586247679217, + "velocityY": -2.539133416044943, + "timestamp": 3.7514971381610853 + }, + { + "x": 8.40022655182027, + "y": 6.685307877393552, + "heading": 0.992766575428426, + "angularVelocity": 2.462479912625774, + "velocityX": -0.46706721649209976, + "velocityY": -2.7924862133844903, + "timestamp": 3.795050266099982 + }, + { + "x": 8.377585411071777, + "y": 6.553085803985596, + "heading": 1.0808392342145412, + "angularVelocity": 2.0221890586062603, + "velocityX": -0.519851083491793, + "velocityY": -3.035880077165969, + "timestamp": 3.8386033940388784 + }, + { + "x": 8.331156557203096, + "y": 6.248001091638873, + "heading": 1.1393178439925227, + "angularVelocity": 0.6668090360061052, + "velocityX": -0.5294103161580237, + "velocityY": -3.478763324102056, + "timestamp": 3.9263025801930476 + }, + { + "x": 8.32509627497856, + "y": 5.9213777688010225, + "heading": 1.1393178431081437, + "angularVelocity": -1.0084234488389344e-8, + "velocityX": -0.06910306116049933, + "velocityY": -3.7243597935295467, + "timestamp": 4.014001766347217 + }, + { + "x": 8.328579577192095, + "y": 5.594716799276784, + "heading": 1.13931784103149, + "angularVelocity": -2.3679282154855844e-8, + "velocityX": 0.03971875186402571, + "velocityY": -3.7247890641766106, + "timestamp": 4.101700952501386 + }, + { + "x": 8.33206298187959, + "y": 5.268055830845277, + "heading": 1.139317838954836, + "angularVelocity": -2.3679282513207347e-8, + "velocityX": 0.03971992033507496, + "velocityY": -3.724789051716606, + "timestamp": 4.1894001386555555 + }, + { + "x": 8.335546386568366, + "y": 4.941394862413785, + "heading": 1.1393178368781822, + "angularVelocity": -2.3679282515836517e-8, + "velocityX": 0.0397199203496886, + "velocityY": -3.7247890517164506, + "timestamp": 4.277099324809725 + }, + { + "x": 8.339029791257143, + "y": 4.614733893982293, + "heading": 1.1393178348015287, + "angularVelocity": -2.3679282075329618e-8, + "velocityX": 0.039719920349696757, + "velocityY": -3.7247890517164497, + "timestamp": 4.364798510963894 + }, + { + "x": 8.34251319594592, + "y": 4.2880729255508, + "heading": 1.139317832724875, + "angularVelocity": -2.3679282013341874e-8, + "velocityX": 0.03971992034969426, + "velocityY": -3.72478905171645, + "timestamp": 4.452497697118063 + }, + { + "x": 8.345996600634695, + "y": 3.9614119571193074, + "heading": 1.139317830648221, + "angularVelocity": -2.3679283402849396e-8, + "velocityX": 0.039719920349694765, + "velocityY": -3.7247890517164497, + "timestamp": 4.540196883272232 + }, + { + "x": 8.349480005323471, + "y": 3.6347509886878155, + "heading": 1.139317828571567, + "angularVelocity": -2.3679282271688366e-8, + "velocityX": 0.03971992034969471, + "velocityY": -3.7247890517164506, + "timestamp": 4.627896069426401 + }, + { + "x": 8.352963410012249, + "y": 3.3080900202563233, + "heading": 1.139317826494913, + "angularVelocity": -2.367928303185141e-8, + "velocityX": 0.03971992034969481, + "velocityY": -3.72478905171645, + "timestamp": 4.7155952555805705 + }, + { + "x": 8.356446814701025, + "y": 2.981429051824831, + "heading": 1.1393178244182591, + "angularVelocity": -2.3679282848596997e-8, + "velocityX": 0.03971992034969499, + "velocityY": -3.72478905171645, + "timestamp": 4.80329444173474 + }, + { + "x": 8.3599302193898, + "y": 2.6547680833933387, + "heading": 1.1393178223416056, + "angularVelocity": -2.3679282487737324e-8, + "velocityX": 0.0397199203496951, + "velocityY": -3.72478905171645, + "timestamp": 4.890993627888909 + }, + { + "x": 8.363413624078582, + "y": 2.3281071149618464, + "heading": 1.1393178202649519, + "angularVelocity": -2.3679282618768576e-8, + "velocityX": 0.039719920349732145, + "velocityY": -3.7247890517164497, + "timestamp": 4.978692814043078 + }, + { + "x": 8.366897029211325, + "y": 2.001446146535259, + "heading": 1.139317818188219, + "angularVelocity": -2.3680183399714216e-8, + "velocityX": 0.03971992541207313, + "velocityY": -3.724789051660523, + "timestamp": 5.066392000197247 + }, + { + "x": 8.369950836003147, + "y": 1.715318694035658, + "heading": 1.1226153865662416, + "angularVelocity": -0.190451386773599, + "velocityX": 0.03482138119793182, + "velocityY": -3.262601000613718, + "timestamp": 5.154091186351416 + }, + { + "x": 8.372495686328486, + "y": 1.4768791360484652, + "heading": 1.1086925029820063, + "angularVelocity": -0.15875727238517048, + "velocityX": 0.029017946881124475, + "velocityY": -2.718834329523107, + "timestamp": 5.241790372505585 + }, + { + "x": 8.374531573132348, + "y": 1.2861274855011333, + "heading": 1.0975518982604389, + "angularVelocity": -0.12703201945321188, + "velocityX": 0.02321443211894871, + "velocityY": -2.1750675110257403, + "timestamp": 5.329489558659755 + }, + { + "x": 8.376058491392191, + "y": 1.1430637450377992, + "heading": 1.0891955157796347, + "angularVelocity": -0.09528460693026461, + "velocityX": 0.017410860086641132, + "velocityY": -1.6313006623783024, + "timestamp": 5.417188744813924 + }, + { + "x": 8.377076437882634, + "y": 1.0476879159528336, + "heading": 1.0836245099802884, + "angularVelocity": -0.06352403076526633, + "velocityX": 0.01160725127658835, + "velocityY": -1.0875337989716487, + "timestamp": 5.504887930968093 + }, + { + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "angularVelocity": -0.03175942546206756, + "velocityX": 0.005803624998845044, + "velocityY": -0.5437669155675129, + "timestamp": 5.592587117122262 + }, + { + "x": 8.377585411071777, + "y": 1, + "heading": 1.0808392342145412, + "angularVelocity": 4.871302260765609e-25, + "velocityX": -2.093358596809338e-24, + "velocityY": 4.245353600645279e-24, + "timestamp": 5.680286303276431 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanepdChaos.1.traj b/src/main/deploy/choreo/AmpLanepdChaos.1.traj new file mode 100644 index 00000000..e5387d12 --- /dev/null +++ b/src/main/deploy/choreo/AmpLanepdChaos.1.traj @@ -0,0 +1,185 @@ +{ + "samples": [ + { + "x": 0.4269569218158722, + "y": 6.9908647537231445, + "heading": 1.0147582935939444e-26, + "angularVelocity": 4.802793352791378e-28, + "velocityX": 7.569777584926927e-27, + "velocityY": 3.719655610214095e-26, + "timestamp": 0 + }, + { + "x": 0.45154320453389124, + "y": 6.9860595473156515, + "heading": -0.0020588646196638234, + "angularVelocity": -0.03238986683838223, + "velocityX": 0.38678911458368936, + "velocityY": -0.07559506058978287, + "timestamp": 0.06356508441164857 + }, + { + "x": 0.500665330018184, + "y": 6.976191418092733, + "heading": -0.005903955941845083, + "angularVelocity": -0.06049061930415103, + "velocityX": 0.7727847125345899, + "velocityY": -0.15524449175608063, + "timestamp": 0.12713016882329714 + }, + { + "x": 0.5742528189740612, + "y": 6.960923422240335, + "heading": -0.01117745758299664, + "angularVelocity": -0.08296223768067817, + "velocityX": 1.1576715367720332, + "velocityY": -0.24019469168832944, + "timestamp": 0.1906952532349457 + }, + { + "x": 0.6722013148089163, + "y": 6.9397965653748726, + "heading": -0.017389498609317183, + "angularVelocity": -0.09772725205698236, + "velocityX": 1.5409166327938608, + "velocityY": -0.3323657486025543, + "timestamp": 0.2542603376465943 + }, + { + "x": 0.7943433521763879, + "y": 6.912149808159037, + "heading": -0.023829197654655073, + "angularVelocity": -0.10130874685282076, + "velocityX": 1.9215271795515552, + "velocityY": -0.43493621493200585, + "timestamp": 0.3178254220582428 + }, + { + "x": 0.940376377090631, + "y": 6.876952520080898, + "heading": -0.029373547858602904, + "angularVelocity": -0.08722320209696448, + "velocityX": 2.2973779751243755, + "velocityY": -0.553720464684682, + "timestamp": 0.38139050646989137 + }, + { + "x": 1.109639548430514, + "y": 6.832391950283203, + "heading": -0.03199489164843748, + "angularVelocity": -0.04123873686470252, + "velocityX": 2.6628324796004614, + "velocityY": -0.7010227424401742, + "timestamp": 0.4449555908815399 + }, + { + "x": 1.3001020816519337, + "y": 6.774603331599785, + "heading": -0.02710028887606378, + "angularVelocity": 0.07700143589326758, + "velocityX": 2.9963388703770333, + "velocityY": -0.909125178048654, + "timestamp": 0.5085206752931885 + }, + { + "x": 1.5002212524414062, + "y": 6.694304943084717, + "heading": 4.6927585573960014e-26, + "angularVelocity": 0.426339225801414, + "velocityX": 3.148256195075543, + "velocityY": -1.2632467849024598, + "timestamp": 0.572085759704837 + }, + { + "x": 1.6698109310260505, + "y": 6.606230489693705, + "heading": 0.043636088677614475, + "angularVelocity": 0.7393180152975136, + "velocityX": 2.8733259186555666, + "velocityY": -1.492228842060727, + "timestamp": 0.6311078419140127 + }, + { + "x": 1.8178691248742131, + "y": 6.519462655469564, + "heading": 0.09197841408379424, + "angularVelocity": 0.8190548960108419, + "velocityX": 2.508522036268403, + "velocityY": -1.4700910401065466, + "timestamp": 0.6901299241231884 + }, + { + "x": 1.945478769087034, + "y": 6.439654918114978, + "heading": 0.13899529896762525, + "angularVelocity": 0.7965982073828233, + "velocityX": 2.1620661189242583, + "velocityY": -1.3521674323814197, + "timestamp": 0.7491520063323641 + }, + { + "x": 2.05361266916171, + "y": 6.369142283614203, + "heading": 0.1819641027220741, + "angularVelocity": 0.7280123327768478, + "velocityX": 1.8320922615275888, + "velocityY": -1.1946822589361732, + "timestamp": 0.8081740885415398 + }, + { + "x": 2.1429230510824855, + "y": 6.309142790648472, + "heading": 0.2193838977387365, + "angularVelocity": 0.6339965249623981, + "velocityX": 1.5131689458914253, + "velocityY": -1.0165600859876827, + "timestamp": 0.8671961707507155 + }, + { + "x": 2.2138592843219276, + "y": 6.2603940232890976, + "heading": 0.25031075711822803, + "angularVelocity": 0.5239879418331249, + "velocityX": 1.2018592124222665, + "velocityY": -0.8259411653185552, + "timestamp": 0.9262182529598912 + }, + { + "x": 2.2667458111920182, + "y": 6.2233878173394785, + "heading": 0.2740979405057225, + "angularVelocity": 0.4030217589273135, + "velocityX": 0.8960464438150413, + "velocityY": -0.6269891634535774, + "timestamp": 0.985240335169067 + }, + { + "x": 2.301826622003525, + "y": 6.198474614172422, + "heading": 0.2902775915680301, + "angularVelocity": 0.27412877446387834, + "velocityX": 0.5943675569963707, + "velocityY": -0.4220996995457338, + "timestamp": 1.0442624173782427 + }, + { + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "angularVelocity": 0.1393127127424044, + "velocityX": 0.2959016450874765, + "velocityY": -0.2127710494914169, + "timestamp": 1.1032844995874185 + }, + { + "x": 2.319291353225708, + "y": 6.185916423797607, + "heading": 0.2985001179522956, + "angularVelocity": -6.615842884891707e-27, + "velocityX": 2.924635996734034e-23, + "velocityY": -9.88114513956692e-23, + "timestamp": 1.162306581796594 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePDEABC.1.traj b/src/main/deploy/choreo/CenterLanePDEABC.1.traj new file mode 100644 index 00000000..cb8830a2 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePDEABC.1.traj @@ -0,0 +1,599 @@ +{ + "samples": [ + { + "x": 1.3350272178649902, + "y": 5.601006507873535, + "heading": 1.3464399983715043e-25, + "angularVelocity": 2.380601151660865e-26, + "velocityX": -3.6373884256645e-26, + "velocityY": 3.3932882505305813e-26, + "timestamp": 0 + }, + { + "x": 1.3513037651461417, + "y": 5.614995878157814, + "heading": 0.006549885022004973, + "angularVelocity": 0.11118193396003606, + "velocityX": 0.27628851481066846, + "velocityY": 0.23746451088284293, + "timestamp": 0.058911414730016334 + }, + { + "x": 1.3843789066016257, + "y": 5.642351283609904, + "heading": 0.01935493583583066, + "angularVelocity": 0.21736111537143754, + "velocityX": 0.5614385871916894, + "velocityY": 0.4643481331666494, + "timestamp": 0.11782282946003267 + }, + { + "x": 1.4348748758418, + "y": 5.682261570187258, + "heading": 0.038033490581189856, + "angularVelocity": 0.31706172447157616, + "velocityX": 0.8571508505030987, + "velocityY": 0.6774627083776117, + "timestamp": 0.17673424419004902 + }, + { + "x": 1.503535032987812, + "y": 5.733639392412991, + "heading": 0.06208045701046981, + "angularVelocity": 0.408188574989825, + "velocityX": 1.1654813835429523, + "velocityY": 0.8721199866136559, + "timestamp": 0.23564565892006534 + }, + { + "x": 1.5912385139518157, + "y": 5.794977395067809, + "heading": 0.09080789039897333, + "angularVelocity": 0.48763781213127805, + "velocityX": 1.4887349313530773, + "velocityY": 1.041190454106761, + "timestamp": 0.29455707365008166 + }, + { + "x": 1.698981116852805, + "y": 5.864111533799201, + "heading": 0.12324878614145561, + "angularVelocity": 0.550672495154884, + "velocityX": 1.828891792783458, + "velocityY": 1.1735270498633465, + "timestamp": 0.353468488380098 + }, + { + "x": 1.8277399268235341, + "y": 5.937848492694958, + "heading": 0.15800938596992012, + "angularVelocity": 0.5900486346791701, + "velocityX": 2.1856343216474206, + "velocityY": 1.2516582606899718, + "timestamp": 0.4123799031101143 + }, + { + "x": 1.9780206143757966, + "y": 6.011501208422765, + "heading": 0.1930700705634149, + "angularVelocity": 0.5951424652450383, + "velocityX": 2.5509604249190097, + "velocityY": 1.2502282633229538, + "timestamp": 0.4712913178401306 + }, + { + "x": 2.1488551922208914, + "y": 6.0788007358405975, + "heading": 0.22564028873153014, + "angularVelocity": 0.5528676966489513, + "velocityX": 2.8998552933757997, + "velocityY": 1.1423851850487312, + "timestamp": 0.5302027325701469 + }, + { + "x": 2.3368515968322754, + "y": 6.133185386657715, + "heading": 0.2525545797921912, + "angularVelocity": 0.456860375599634, + "velocityX": 3.1911711078905185, + "velocityY": 0.9231598165882628, + "timestamp": 0.5891141473001633 + }, + { + "x": 2.5473350968821267, + "y": 6.194075230145087, + "heading": 0.25255484664471606, + "angularVelocity": 0.0000044758460677296985, + "velocityX": 3.530383481883881, + "velocityY": 1.0212890683184164, + "timestamp": 0.6487347242516318 + }, + { + "x": 2.7606743654688723, + "y": 6.255791206318425, + "heading": 0.25255482885764124, + "angularVelocity": -2.983378514405921e-7, + "velocityX": 3.578282524176262, + "velocityY": 1.0351455710261694, + "timestamp": 0.7083553012031003 + }, + { + "x": 2.9740136340556216, + "y": 6.317507182491763, + "heading": 0.25255481107057365, + "angularVelocity": -2.9833773077503677e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.7679758781545688 + }, + { + "x": 3.187352902642371, + "y": 6.379223158665101, + "heading": 0.25255479328350594, + "angularVelocity": -2.9833773187848623e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.8275964551060373 + }, + { + "x": 3.40069217122912, + "y": 6.44093913483844, + "heading": 0.2525547754964383, + "angularVelocity": -2.983377307846883e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 0.8872170320575058 + }, + { + "x": 3.614031439815869, + "y": 6.502655111011779, + "heading": 0.25255475770937064, + "angularVelocity": -2.9833773102796393e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.9468376090089743 + }, + { + "x": 3.827370708402618, + "y": 6.564371087185117, + "heading": 0.25255473992230293, + "angularVelocity": -2.983377313896792e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.006458185960443 + }, + { + "x": 4.040709976989367, + "y": 6.626087063358456, + "heading": 0.2525547221352353, + "angularVelocity": -2.9833773109308854e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.0660787629119115 + }, + { + "x": 4.254049245576117, + "y": 6.6878030395317944, + "heading": 0.2525547043481677, + "angularVelocity": -2.983377300262124e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.12569933986338 + }, + { + "x": 4.467388514162867, + "y": 6.749519015705133, + "heading": 0.2525546865611001, + "angularVelocity": -2.9833773003639016e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.1853199168148487 + }, + { + "x": 4.680727782749616, + "y": 6.811234991878472, + "heading": 0.2525546687740325, + "angularVelocity": -2.9833772985092435e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.2449404937663173 + }, + { + "x": 4.894067051336365, + "y": 6.872950968051811, + "heading": 0.25255465098696483, + "angularVelocity": -2.9833773088728923e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.304561070717786 + }, + { + "x": 5.107406319923115, + "y": 6.934666944225149, + "heading": 0.2525546331998971, + "angularVelocity": -2.9833773138995595e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.3641816476692545 + }, + { + "x": 5.3207455885098645, + "y": 6.996382920398487, + "heading": 0.2525546154128296, + "angularVelocity": -2.9833772971803955e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4238022246207231 + }, + { + "x": 5.534084857096614, + "y": 7.058098896571825, + "heading": 0.252554597625762, + "angularVelocity": -2.983377299147757e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4834228015721918 + }, + { + "x": 5.747424125671387, + "y": 7.119814872741699, + "heading": 0.2525545797921912, + "angularVelocity": -2.9911771592583556e-7, + "velocityX": 3.5782825239754317, + "velocityY": 1.035145570968072, + "timestamp": 1.5430433785236604 + }, + { + "x": 5.874238789453484, + "y": 7.151633771807316, + "heading": 0.23428972710167786, + "angularVelocity": -0.5023194013498574, + "velocityX": 3.4876528747747586, + "velocityY": 0.8750823563200211, + "timestamp": 1.579404412243304 + }, + { + "x": 6.000467771954814, + "y": 7.182688874971163, + "heading": 0.21343765731660022, + "angularVelocity": -0.5734729641036735, + "velocityX": 3.471545486704275, + "velocityY": 0.8540764655727967, + "timestamp": 1.6157654459629476 + }, + { + "x": 6.126526412325093, + "y": 7.213769689844044, + "heading": 0.19236113767335114, + "angularVelocity": -0.5796457770083356, + "velocityX": 3.4668607428005407, + "velocityY": 0.8547835881819195, + "timestamp": 1.6521264796825912 + }, + { + "x": 6.252399106037461, + "y": 7.244861395063954, + "heading": 0.17099373366442527, + "angularVelocity": -0.5876456696384406, + "velocityX": 3.4617468436923633, + "velocityY": 0.8550830941616943, + "timestamp": 1.6884875134022348 + }, + { + "x": 6.378079158099059, + "y": 7.275966592245878, + "heading": 0.1493195497494227, + "angularVelocity": -0.596082720918173, + "velocityX": 3.456448819102211, + "velocityY": 0.8554541496744962, + "timestamp": 1.7248485471218784 + }, + { + "x": 6.503558906617952, + "y": 7.3070873984764315, + "heading": 0.12731920059935156, + "angularVelocity": -0.6050529069030769, + "velocityX": 3.4509400774022723, + "velocityY": 0.8558834292365232, + "timestamp": 1.761209580841522 + }, + { + "x": 6.628826049826062, + "y": 7.3382208951944605, + "heading": 0.10495362803069447, + "angularVelocity": -0.6150972698164627, + "velocityX": 3.44509301286545, + "velocityY": 0.8562324426219444, + "timestamp": 1.7975706145611656 + }, + { + "x": 6.7538656809780315, + "y": 7.369363297624379, + "heading": 0.08217584529224368, + "angularVelocity": -0.6264338608763309, + "velocityX": 3.438835983489081, + "velocityY": 0.8564773672288275, + "timestamp": 1.8339316482808092 + }, + { + "x": 6.8786958169095165, + "y": 7.400552514729427, + "heading": 0.05908376270904082, + "angularVelocity": -0.6350777252718116, + "velocityX": 3.4330744525573853, + "velocityY": 0.8577648629444327, + "timestamp": 1.8702926820004528 + }, + { + "x": 7.003224052672837, + "y": 7.431696922152255, + "heading": 0.03530389927777457, + "angularVelocity": -0.6539930524147748, + "velocityX": 3.4247716036754445, + "velocityY": 0.8565325084804077, + "timestamp": 1.9066537157200965 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": -8.902558475497447e-21, + "angularVelocity": -0.9709267219953129, + "velocityX": 3.3570777699788383, + "velocityY": 0.7233864155389959, + "timestamp": 1.94301474943974 + }, + { + "x": 7.338537269448379, + "y": 7.482023887493991, + "heading": -0.008039547554639858, + "angularVelocity": -0.1278866817794853, + "velocityX": 3.39215287381437, + "velocityY": 0.38215275600640763, + "timestamp": 2.0058793686696177 + }, + { + "x": 7.527981936001141, + "y": 7.500234795903286, + "heading": -0.013861641566031323, + "angularVelocity": -0.09261320728121766, + "velocityX": 3.013533985786477, + "velocityY": 0.2896845416768293, + "timestamp": 2.068743987899495 + }, + { + "x": 7.693596991003447, + "y": 7.512743640283509, + "heading": -0.017657163586421762, + "angularVelocity": -0.060376123595234456, + "velocityX": 2.6344716158496166, + "velocityY": 0.1989806751947781, + "timestamp": 2.1316086071293725 + }, + { + "x": 7.835373269936495, + "y": 7.519587451824941, + "heading": -0.01948976746028864, + "angularVelocity": -0.02915159427221823, + "velocityX": 2.255263464089624, + "velocityY": 0.10886587122090353, + "timestamp": 2.19447322635925 + }, + { + "x": 7.953306214016821, + "y": 7.520784758027623, + "heading": -0.01939144141260055, + "angularVelocity": 0.0015640919947123182, + "velocityX": 1.875982794854424, + "velocityY": 0.01904578787480972, + "timestamp": 2.2573378455891273 + }, + { + "x": 8.04739309462225, + "y": 7.516346680416715, + "heading": -0.01738162754383806, + "angularVelocity": 0.031970508902840686, + "velocityX": 1.4966587208836901, + "velocityY": -0.07059738315886452, + "timestamp": 2.3202024648190047 + }, + { + "x": 8.117632094556283, + "y": 7.5062806367543455, + "heading": -0.0134735913597302, + "angularVelocity": 0.06216590877322135, + "velocityX": 1.1173057404068745, + "velocityY": -0.16012255837517314, + "timestamp": 2.383067084048882 + }, + { + "x": 8.164021915899584, + "y": 7.490591928437994, + "heading": -0.007677144931297963, + "angularVelocity": 0.09220522607854079, + "velocityX": 0.7379321136689944, + "velocityY": -0.24956340320748793, + "timestamp": 2.4459317032787595 + }, + { + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 2.2979832887228624e-21, + "angularVelocity": 0.12212187117884055, + "velocityX": 0.358542990464167, + "velocityY": -0.33894095350730474, + "timestamp": 2.508796322508637 + }, + { + "x": 8.169433855540824, + "y": 7.426853893988931, + "heading": 0.014976899045789983, + "angularVelocity": 0.16512821061392627, + "velocityX": -0.18884224442902273, + "velocityY": -0.46782018853404506, + "timestamp": 2.599494931729361 + }, + { + "x": 8.102655131439361, + "y": 7.372743106744783, + "heading": 0.03346799984598468, + "angularVelocity": 0.20387413830342968, + "velocityX": -0.7362706515041357, + "velocityY": -0.5965999667366941, + "timestamp": 2.6901935409500854 + }, + { + "x": 7.986220220035368, + "y": 7.306964887600374, + "heading": 0.05493172658414638, + "angularVelocity": 0.23664890699621968, + "velocityX": -1.2837563045827718, + "velocityY": -0.7252395566985038, + "timestamp": 2.7808921501708097 + }, + { + "x": 7.820121969665864, + "y": 7.229538517591083, + "heading": 0.07855370015299508, + "angularVelocity": 0.26044471653762835, + "velocityX": -1.8313208085174242, + "velocityY": -0.8536665630766948, + "timestamp": 2.871590759391534 + }, + { + "x": 7.6043501738014525, + "y": 7.14049675145262, + "heading": 0.10296815130417637, + "angularVelocity": 0.2691821998258671, + "velocityX": -2.3789978448215092, + "velocityY": -0.9817324312192064, + "timestamp": 2.9622893686122582 + }, + { + "x": 7.338891049230189, + "y": 7.039907645727491, + "heading": 0.1254011174832251, + "angularVelocity": 0.24733528299707236, + "velocityX": -2.9268268483063693, + "velocityY": -1.1090479400884177, + "timestamp": 3.0529879778329825 + }, + { + "x": 7.023762471374015, + "y": 6.927998022527093, + "heading": 0.1370927000479183, + "angularVelocity": 0.12890586377394736, + "velocityX": -3.4744587658370385, + "velocityY": -1.2338626155562706, + "timestamp": 3.1436865870537067 + }, + { + "x": 6.694988070852044, + "y": 6.85020567874946, + "heading": 0.13709270182779565, + "angularVelocity": 1.9624086279332165e-8, + "velocityX": -3.624911157368082, + "velocityY": -0.8577016168827462, + "timestamp": 3.234385196274431 + }, + { + "x": 6.366213601226777, + "y": 6.7724136270242035, + "heading": 0.13709270360766948, + "angularVelocity": 1.962404782489554e-8, + "velocityX": -3.624911919268354, + "velocityY": -0.8576983968512897, + "timestamp": 3.3250838054951553 + }, + { + "x": 6.0374391323544945, + "y": 6.69462157211659, + "heading": 0.13709270538754328, + "angularVelocity": 1.962404757872295e-8, + "velocityX": -3.6249119109663046, + "velocityY": -0.857698431938445, + "timestamp": 3.4157824147158795 + }, + { + "x": 5.708664663238629, + "y": 6.616829518238441, + "heading": 0.13709270716741703, + "angularVelocity": 1.9624046615308717e-8, + "velocityX": -3.6249119136519434, + "velocityY": -0.8576984205880644, + "timestamp": 3.5064810239366038 + }, + { + "x": 5.3798901940960535, + "y": 6.539037464473215, + "heading": 0.13709270894730027, + "angularVelocity": 1.9624151634826213e-8, + "velocityX": -3.6249119139464265, + "velocityY": -0.8576984193430164, + "timestamp": 3.597179633157328 + }, + { + "x": 5.082161936855116, + "y": 6.468591654596992, + "heading": 0.17334127091967155, + "angularVelocity": 0.3996595127953577, + "velocityX": -3.2826110543369467, + "velocityY": -0.7767022061472455, + "timestamp": 3.6878782423780523 + }, + { + "x": 4.834055003719788, + "y": 6.409886994261807, + "heading": 0.20354833113507975, + "angularVelocity": 0.33304876971041714, + "velocityX": -2.735509786390822, + "velocityY": -0.647249840318068, + "timestamp": 3.7785768515987765 + }, + { + "x": 4.635569429511693, + "y": 6.362923375586849, + "heading": 0.22771434771261193, + "angularVelocity": 0.2664430776300183, + "velocityX": -2.1884081345179167, + "velocityY": -0.5177986639317362, + "timestamp": 3.869275460819501 + }, + { + "x": 4.486705235532264, + "y": 6.327700716475024, + "heading": 0.24583917728696456, + "angularVelocity": 0.19983580487153885, + "velocityX": -1.6413062477854834, + "velocityY": -0.38834839270916643, + "timestamp": 3.959974070040225 + }, + { + "x": 4.387462434805768, + "y": 6.304218961949016, + "heading": 0.25792250356094953, + "angularVelocity": 0.13322504476974903, + "velocityX": -1.0942042174536384, + "velocityY": -0.25889872764050514, + "timestamp": 4.050672679260949 + }, + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": 0.06661212070370816, + "velocityX": -0.54710211431646, + "velocityY": -0.12944936516318697, + "timestamp": 4.141371288481674 + }, + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": 4.2322185448004084e-23, + "velocityX": 1.750098119560174e-21, + "velocityY": 1.2842584640589406e-21, + "timestamp": 4.232069897702398 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePDEABC.2.traj b/src/main/deploy/choreo/CenterLanePDEABC.2.traj new file mode 100644 index 00000000..99522444 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePDEABC.2.traj @@ -0,0 +1,707 @@ +{ + "samples": [ + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": 4.2322185448004084e-23, + "velocityX": 1.750098119560174e-21, + "velocityY": 1.2842584640589406e-21, + "timestamp": 0 + }, + { + "x": 4.357336303204376, + "y": 6.297583237177048, + "heading": 0.24752122945545674, + "angularVelocity": -0.28561960466886005, + "velocityX": 0.3386404361144792, + "velocityY": 0.08867849340285801, + "timestamp": 0.057569230339162836 + }, + { + "x": 4.396392552566799, + "y": 6.3076902844788645, + "heading": 0.21545182139905172, + "angularVelocity": -0.5570581344838518, + "velocityX": 0.6784222949017583, + "velocityY": 0.17556335636714013, + "timestamp": 0.11513846067832567 + }, + { + "x": 4.455088009056865, + "y": 6.322669909558741, + "heading": 0.16876627146696008, + "angularVelocity": -0.8109462234782202, + "velocityX": 1.0195629878021513, + "velocityY": 0.26020193411697123, + "timestamp": 0.1727076910174885 + }, + { + "x": 4.533516711715102, + "y": 6.342355579726992, + "heading": 0.10873908045256343, + "angularVelocity": -1.042695736259688, + "velocityX": 1.3623371755394689, + "velocityY": 0.3419477740500502, + "timestamp": 0.23027692135665134 + }, + { + "x": 4.631792826161861, + "y": 6.366523725829109, + "heading": 0.03701265289139425, + "angularVelocity": -1.2459160412359371, + "velocityX": 1.7070944646606674, + "velocityY": 0.4198101305112548, + "timestamp": 0.2878461516958142 + }, + { + "x": 4.750056312234907, + "y": 6.394856813133717, + "heading": -0.04421826686802747, + "angularVelocity": -1.4110127802796573, + "velocityX": 2.054282910789491, + "velocityY": 0.492156784756161, + "timestamp": 0.345415382034977 + }, + { + "x": 4.888478272202705, + "y": 6.42687003155547, + "heading": -0.13181310680866934, + "angularVelocity": -1.5215565576365515, + "velocityX": 2.4044434701020125, + "velocityY": 0.5560820985993948, + "timestamp": 0.40298461237413985 + }, + { + "x": 5.047249483224003, + "y": 6.461748830996926, + "heading": -0.22073093757153267, + "angularVelocity": -1.5445374245758388, + "velocityX": 2.757917903121444, + "velocityY": 0.6058583593348625, + "timestamp": 0.4605538427133027 + }, + { + "x": 5.226429045018527, + "y": 6.497908973529479, + "heading": -0.30132047165929504, + "angularVelocity": -1.399871660833024, + "velocityX": 3.1124189213388544, + "velocityY": 0.6281157889295987, + "timestamp": 0.5181230730524655 + }, + { + "x": 5.423903541491299, + "y": 6.531166091125145, + "heading": -0.3435649109260108, + "angularVelocity": -0.7338023978753491, + "velocityX": 3.430209077129816, + "velocityY": 0.5776891127384427, + "timestamp": 0.5756923033916284 + }, + { + "x": 5.636616267297259, + "y": 6.55438559892435, + "heading": -0.3449872384479477, + "angularVelocity": -0.024706384184006707, + "velocityX": 3.6949030680588684, + "velocityY": 0.40333191294048215, + "timestamp": 0.6332615337307912 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.3449872632321153, + "angularVelocity": -4.3051066415640403e-7, + "velocityX": 3.7246964993017384, + "velocityY": 0.047619187556732175, + "timestamp": 0.690830764069954 + }, + { + "x": 5.9707478293428995, + "y": 6.552244535149851, + "heading": -0.34498728566747394, + "angularVelocity": -6.975751659806049e-7, + "velocityX": 3.7219059383782973, + "velocityY": -0.151808826449267, + "timestamp": 0.7229926868363377 + }, + { + "x": 6.090018567999844, + "y": 6.540962083011693, + "heading": -0.3449873062012292, + "angularVelocity": -6.384492454517859e-7, + "velocityX": 3.7084455280643134, + "velocityY": -0.3508015431822894, + "timestamp": 0.7551546096027213 + }, + { + "x": 6.208514463680753, + "y": 6.523311991997161, + "heading": -0.3449873252853557, + "angularVelocity": -5.933764159352289e-7, + "velocityX": 3.6843535923407127, + "velocityY": -0.5487884273193968, + "timestamp": 0.7873165323691049 + }, + { + "x": 6.325895808833114, + "y": 6.499344869625537, + "heading": -0.34498734327239833, + "angularVelocity": -5.592651525230829e-7, + "velocityX": 3.6496992423305046, + "velocityY": -0.7452017886404377, + "timestamp": 0.8194784551354886 + }, + { + "x": 6.441826094112058, + "y": 6.46912943973789, + "heading": -0.34498736044751604, + "angularVelocity": -5.340202407325154e-7, + "velocityX": 3.6045819188434196, + "velocityY": -0.9394783423592116, + "timestamp": 0.8516403779018722 + }, + { + "x": 6.555972977983021, + "y": 6.432752355963861, + "heading": -0.3449873770504224, + "angularVelocity": -5.162286594877017e-7, + "velocityX": 3.5491312102233143, + "velocityY": -1.131060603505082, + "timestamp": 0.8838023006682558 + }, + { + "x": 6.66800926163723, + "y": 6.390318000635929, + "heading": -0.3449873932911425, + "angularVelocity": -5.049673254841841e-7, + "velocityX": 3.4835070175379053, + "velocityY": -1.319397339399292, + "timestamp": 0.9159642234346395 + }, + { + "x": 6.777614079975414, + "y": 6.34194872129089, + "heading": -0.344987409362088, + "angularVelocity": -4.996885790126847e-7, + "velocityX": 3.407906272716571, + "velocityY": -1.5039299639011887, + "timestamp": 0.9481261462010231 + }, + { + "x": 6.886436638977974, + "y": 6.291844289930693, + "heading": -0.3449874254080791, + "angularVelocity": -4.98912679463648e-7, + "velocityX": 3.3835837425834847, + "velocityY": -1.557880470149199, + "timestamp": 0.9802880689674067 + }, + { + "x": 6.995259181110964, + "y": 6.241739821931245, + "heading": -0.34498744145406995, + "angularVelocity": -4.989126736688575e-7, + "velocityX": 3.383583218063541, + "velocityY": -1.5578816093613586, + "timestamp": 1.0124499917337904 + }, + { + "x": 7.104082434838663, + "y": 6.191636899482441, + "heading": -0.34498745750007104, + "angularVelocity": -4.98912992332448e-7, + "velocityX": 3.3836053434418156, + "velocityY": -1.5578335540676167, + "timestamp": 1.044611914500174 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -6.552383423892292e-7, + "velocityX": 3.442174665036408, + "velocityY": -1.4237498464482023, + "timestamp": 1.0767738372665576 + }, + { + "x": 7.41054811335349, + "y": 6.075273519082507, + "heading": -0.3626370912094401, + "angularVelocity": -0.2874236730044668, + "velocityX": 3.187927819628061, + "velocityY": -1.1492777517419182, + "timestamp": 1.1381800967132865 + }, + { + "x": 7.584442418775818, + "y": 6.012933970935389, + "heading": -0.3763799731717047, + "angularVelocity": -0.22380262347988858, + "velocityX": 2.831866115752982, + "velocityY": -1.0151985922737843, + "timestamp": 1.1995863561600153 + }, + { + "x": 7.73639717593505, + "y": 5.958610534020796, + "heading": -0.3857543878763611, + "angularVelocity": -0.15266220071243664, + "velocityX": 2.4745809063822897, + "velocityY": -0.8846563429208651, + "timestamp": 1.2609926156067441 + }, + { + "x": 7.866387295940067, + "y": 5.912228690600519, + "heading": -0.39060211670074774, + "angularVelocity": -0.07894519008427879, + "velocityX": 2.1168871248017687, + "velocityY": -0.7553276137999276, + "timestamp": 1.322398875053473 + }, + { + "x": 7.974400233400663, + "y": 5.873750785403185, + "heading": -0.39084333039715696, + "angularVelocity": -0.003928161372840292, + "velocityX": 1.7589890417327878, + "velocityY": -0.62661210019988, + "timestamp": 1.3838051345002018 + }, + { + "x": 8.060428467430407, + "y": 5.843154117743718, + "heading": -0.3864300742351692, + "angularVelocity": 0.0718698093932331, + "velocityX": 1.4009684811427807, + "velocityY": -0.49826626691061593, + "timestamp": 1.4452113939469307 + }, + { + "x": 8.124466994034947, + "y": 5.820423531988022, + "heading": -0.37733049959220943, + "angularVelocity": 0.14818643449294983, + "velocityX": 1.0428664305809843, + "velocityY": -0.3701672428918256, + "timestamp": 1.5066176533936595 + }, + { + "x": 8.16651225164957, + "y": 5.805548215416436, + "heading": -0.3635219686266917, + "angularVelocity": 0.22487171649816598, + "velocityX": 0.6847063799920386, + "velocityY": -0.24224430384806347, + "timestamp": 1.5680239128403883 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.3018338882695626, + "velocityX": 0.32650307971422843, + "velocityY": -0.11445294476423894, + "timestamp": 1.6294301722871172 + }, + { + "x": 8.182873431203555, + "y": 5.799961031790675, + "heading": -0.31977664522832494, + "angularVelocity": 0.3842917157740494, + "velocityX": -0.056218956684159486, + "velocityY": 0.021964473717296987, + "timestamp": 1.6950335467826898 + }, + { + "x": 8.154073485915251, + "y": 5.810352807071496, + "heading": -0.2894961331766682, + "angularVelocity": 0.46156942816562346, + "velocityX": -0.4390009738027679, + "velocityY": 0.15840306021945175, + "timestamp": 1.7606369212782624 + }, + { + "x": 8.100157148407472, + "y": 5.829696959053818, + "heading": -0.25457167500592204, + "angularVelocity": 0.5323576483569935, + "velocityX": -0.8218531123184383, + "velocityY": 0.29486519757649204, + "timestamp": 1.826240295773835 + }, + { + "x": 8.021119032382456, + "y": 5.857995174413058, + "heading": -0.21555305979528627, + "angularVelocity": 0.5947653685599519, + "velocityX": -1.2047873548082333, + "velocityY": 0.4313530451265237, + "timestamp": 1.8918436702694077 + }, + { + "x": 7.916952910031998, + "y": 5.895249222659617, + "heading": -0.17317929559353756, + "angularVelocity": 0.6459083016317765, + "velocityX": -1.5878165285154446, + "velocityY": 0.567867865532928, + "timestamp": 1.9574470447649803 + }, + { + "x": 7.787651985971443, + "y": 5.941460762829762, + "heading": -0.12849926666272526, + "angularVelocity": 0.6810629677872386, + "velocityX": -1.9709492850749835, + "velocityY": 0.7044079748255081, + "timestamp": 2.023050419260553 + }, + { + "x": 7.633210581172123, + "y": 5.996630683324096, + "heading": -0.08312168761624714, + "angularVelocity": 0.6916958067384257, + "velocityX": -2.354168607740487, + "velocityY": 0.8409616261135585, + "timestamp": 2.0886537937561256 + }, + { + "x": 7.453632538523732, + "y": 6.060756399754721, + "heading": -0.03982832718204048, + "angularVelocity": 0.6599258158149841, + "velocityX": -2.7373293527836737, + "velocityY": 0.9774758832711061, + "timestamp": 2.154257168251698 + }, + { + "x": 7.248984101269758, + "y": 6.133814967695316, + "heading": -0.004587884016795663, + "angularVelocity": 0.5371742450172758, + "velocityX": -3.1194803442281134, + "velocityY": 1.11364039582332, + "timestamp": 2.219860542747271 + }, + { + "x": 7.020152981307638, + "y": 6.215344107160959, + "heading": -7.996010360253712e-8, + "angularVelocity": 0.06993244009119806, + "velocityX": -3.488099838180771, + "velocityY": 1.242758319865779, + "timestamp": 2.2854639172428435 + }, + { + "x": 6.789970042208637, + "y": 6.297403837687721, + "heading": -7.492708758217055e-8, + "angularVelocity": 7.671885873349635e-8, + "velocityX": -3.5087057772391836, + "velocityY": 1.2508461821929457, + "timestamp": 2.351067291738416 + }, + { + "x": 6.5597871223902215, + "y": 6.379463622297776, + "heading": -6.98940737576559e-8, + "angularVelocity": 7.671882526186561e-8, + "velocityX": -3.5087054833429296, + "velocityY": 1.2508470065910036, + "timestamp": 2.4166706662339887 + }, + { + "x": 6.3296039103658925, + "y": 6.46152258724702, + "heading": -6.48610593666683e-8, + "angularVelocity": 7.671883389668008e-8, + "velocityX": -3.5087099374722537, + "velocityY": 1.2508345124042644, + "timestamp": 2.4822740407295614 + }, + { + "x": 6.092938315579126, + "y": 6.522410853761584, + "heading": -5.975605245251985e-8, + "angularVelocity": 7.781622444578512e-8, + "velocityX": -3.6075216649524458, + "velocityY": 0.9281270511271241, + "timestamp": 2.547877415225134 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -5.436468625345948e-8, + "angularVelocity": 8.218123290935499e-8, + "velocityX": -3.6872209612695626, + "velocityY": 0.5291823081772375, + "timestamp": 2.6134807897207066 + }, + { + "x": 5.6615808215647085, + "y": 6.568071618356437, + "heading": -4.94825886082418e-8, + "angularVelocity": 9.582618856188379e-8, + "velocityX": -3.71880135129745, + "velocityY": 0.2148218335343848, + "timestamp": 2.664428215664344 + }, + { + "x": 5.471871500054035, + "y": 6.562921703037617, + "heading": -4.486806140713326e-8, + "angularVelocity": 9.057429527869525e-8, + "velocityX": -3.7236291725620743, + "velocityY": -0.10108293448460899, + "timestamp": 2.7153756416079817 + }, + { + "x": 5.283280913941195, + "y": 6.541714647593256, + "heading": -4.0367954549807895e-8, + "angularVelocity": 8.832844395914553e-8, + "velocityX": -3.7016705480170033, + "velocityY": -0.4162537174659499, + "timestamp": 2.7663230675516193 + }, + { + "x": 5.095501482386873, + "y": 6.514236775268743, + "heading": -3.5882626212892094e-8, + "angularVelocity": 8.803837002241881e-8, + "velocityX": -3.68574914387354, + "velocityY": -0.539337794119592, + "timestamp": 2.817270493495257 + }, + { + "x": 4.907722071138919, + "y": 6.486758764174086, + "heading": -3.139729791987603e-8, + "angularVelocity": 8.803836916075124e-8, + "velocityX": -3.6857487452985973, + "velocityY": -0.5393405179106829, + "timestamp": 2.8682179194388944 + }, + { + "x": 4.7199426574500984, + "y": 6.459280769759847, + "heading": -2.6911969659920007e-8, + "angularVelocity": 8.803836851184644e-8, + "velocityX": -3.6857487932081243, + "velocityY": -0.5393401905061415, + "timestamp": 2.919165345382532 + }, + { + "x": 4.532163244787761, + "y": 6.431802768330805, + "heading": -2.2426641378076926e-8, + "angularVelocity": 8.803836894144722e-8, + "velocityX": -3.6857487730602023, + "velocityY": -0.5393403281932087, + "timestamp": 2.9701127713261695 + }, + { + "x": 4.3443838326647874, + "y": 6.404324763215868, + "heading": -1.7941313081549045e-8, + "angularVelocity": 8.803836922968166e-8, + "velocityX": -3.6857487624735623, + "velocityY": -0.5393404005402795, + "timestamp": 3.021060197269807 + }, + { + "x": 4.156604420793327, + "y": 6.376846756382128, + "heading": -1.3455984799695103e-8, + "angularVelocity": 8.80383689416604e-8, + "velocityX": -3.685748757536816, + "velocityY": -0.5393404342770572, + "timestamp": 3.0720076232134446 + }, + { + "x": 3.968825009218295, + "y": 6.349368747522659, + "heading": -8.970656536814864e-9, + "angularVelocity": 8.803836856924311e-8, + "velocityX": -3.685748751718508, + "velocityY": -0.5393404740382505, + "timestamp": 3.122955049157082 + }, + { + "x": 3.781045597297714, + "y": 6.321890741024598, + "heading": -4.485328240150933e-9, + "angularVelocity": 8.803836923235202e-8, + "velocityX": -3.6857487585009534, + "velocityY": -0.5393404276883234, + "timestamp": 3.1739024751007197 + }, + { + "x": 3.5932661777631214, + "y": 6.2944127865592865, + "heading": -1.5290485245544054e-22, + "angularVelocity": 8.803836812310754e-8, + "velocityX": -3.6857489079493813, + "velocityY": -0.5393394063855248, + "timestamp": 3.224849901044357 + }, + { + "x": 3.405486822128296, + "y": 6.266934394836426, + "heading": -7.242717716527151e-23, + "angularVelocity": 4.066531625418711e-24, + "velocityX": -3.6857476537198193, + "velocityY": -0.5393479889103638, + "timestamp": 3.2757973269879948 + }, + { + "x": 3.1943671277625394, + "y": 6.2230448572371815, + "heading": -6.454692239159141e-23, + "angularVelocity": 6.1636286209902435e-25, + "velocityX": -3.5183764765296694, + "velocityY": -0.7314330248480798, + "timestamp": 3.3358021888252907 + }, + { + "x": 3.002694024502071, + "y": 6.190127687410396, + "heading": -5.666660567021065e-23, + "angularVelocity": 6.173952401211252e-25, + "velocityX": -3.1942928854697095, + "velocityY": -0.5485750457361542, + "timestamp": 3.3958070506625866 + }, + { + "x": 2.830467511330891, + "y": 6.168182901091022, + "heading": -4.87863785909476e-23, + "angularVelocity": 6.159013258773547e-25, + "velocityX": -2.870209311341694, + "velocityY": -0.36571680439623283, + "timestamp": 3.4558119124998825 + }, + { + "x": 2.6776875881237743, + "y": 6.157210503145755, + "heading": -4.0906073219636964e-23, + "angularVelocity": 6.172060876090466e-25, + "velocityX": -2.5461257393006025, + "velocityY": -0.1828584819513171, + "timestamp": 3.5158167743371784 + }, + { + "x": 2.5443542548814535, + "y": 6.157210495895674, + "heading": -3.3025815924682597e-23, + "angularVelocity": 6.164048799254959e-25, + "velocityX": -2.2220421672473036, + "velocityY": -1.208249057531058e-7, + "timestamp": 3.5758216361744743 + }, + { + "x": 2.430467511630842, + "y": 6.168182880686502, + "heading": -2.5145589184649632e-23, + "angularVelocity": 6.158956724948031e-25, + "velocityX": -1.8979585947454753, + "velocityY": 0.18285826272844113, + "timestamp": 3.6358264980117703 + }, + { + "x": 2.3360273584029394, + "y": 6.1901276583922495, + "heading": -1.726521486745078e-23, + "angularVelocity": 6.183550926375638e-25, + "velocityX": -1.5738750217270354, + "velocityY": 0.3657166608474323, + "timestamp": 3.695831359849066 + }, + { + "x": 2.261033795226874, + "y": 6.223044829624826, + "heading": -9.385003241848346e-24, + "angularVelocity": 6.156437857286479e-25, + "velocityX": -1.2497914482231562, + "velocityY": 0.5485750691641013, + "timestamp": 3.755836221686362 + }, + { + "x": 2.205486822128296, + "y": 6.266934394836426, + "heading": -1.5067646314431248e-24, + "angularVelocity": 5.827628634700505e-25, + "velocityX": -0.9257078742918208, + "velocityY": 0.7314334850167038, + "timestamp": 3.815841083523659 + }, + { + "x": 2.166583654686104, + "y": 6.330345399138925, + "heading": 0.03190215483964595, + "angularVelocity": 0.46789281830201823, + "velocityX": -0.5705731398677051, + "velocityY": 0.9300172249677102, + "timestamp": 3.8840236962754835 + }, + { + "x": 2.1528025694390287, + "y": 6.405563401029407, + "heading": 0.09569947573294339, + "angularVelocity": 0.9356831356039661, + "velocityX": -0.20212022817660624, + "velocityY": 1.1031845048863924, + "timestamp": 3.952206309027308 + }, + { + "x": 2.1659059898168422, + "y": 6.488336785914272, + "heading": 0.18898264963043915, + "angularVelocity": 1.3681372733109376, + "velocityX": 0.1921812592531237, + "velocityY": 1.2139954974466904, + "timestamp": 4.020388921779133 + }, + { + "x": 2.2052860593561716, + "y": 6.55939873181835, + "heading": 0.2899151707587667, + "angularVelocity": 1.4803263919455447, + "velocityX": 0.5775676224475003, + "velocityY": 1.0422297274341026, + "timestamp": 4.088571534530957 + }, + { + "x": 2.238286501729519, + "y": 6.603471711362833, + "heading": 0.35830069991198743, + "angularVelocity": 1.0029760725382513, + "velocityX": 0.4840008477449354, + "velocityY": 0.6463961670829911, + "timestamp": 4.156754147282782 + }, + { + "x": 2.256009101867676, + "y": 6.624549388885498, + "heading": 0.3930579718029317, + "angularVelocity": 0.5097673803944184, + "velocityX": 0.2599284395666251, + "velocityY": 0.3091356677601188, + "timestamp": 4.224936760034606 + }, + { + "x": 2.256009101867676, + "y": 6.624549388885498, + "heading": 0.3930579718029317, + "angularVelocity": 1.058900443902291e-24, + "velocityX": 2.5593812661454674e-24, + "velocityY": -7.939073774690169e-24, + "timestamp": 4.293119372786431 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePDEABC.3.traj b/src/main/deploy/choreo/CenterLanePDEABC.3.traj new file mode 100644 index 00000000..f386d29d --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePDEABC.3.traj @@ -0,0 +1,77 @@ +{ + "samples": [ + { + "x": 2.256009101867676, + "y": 6.624549388885498, + "heading": 0.3930579718029317, + "angularVelocity": 1.058900443902291e-24, + "velocityX": 2.5593812661454674e-24, + "velocityY": -7.939073774690169e-24, + "timestamp": 0 + }, + { + "x": 2.306032658844481, + "y": 6.645836035930219, + "heading": 0.3930579718029317, + "angularVelocity": 7.690085231355673e-17, + "velocityX": 0.5342706205732553, + "velocityY": 0.22734948919726436, + "timestamp": 0.09362962336040681 + }, + { + "x": 2.406079771576901, + "y": 6.688409329500005, + "heading": 0.3930579718029317, + "angularVelocity": 7.285372137244723e-17, + "velocityX": 1.0685412281037379, + "velocityY": 0.45469897284440564, + "timestamp": 0.18725924672081362 + }, + { + "x": 2.556150436401367, + "y": 6.752269268035889, + "heading": 0.3930579718029317, + "angularVelocity": 1.0864711023830975e-16, + "velocityX": 1.6028117965059043, + "velocityY": 0.6820484398411784, + "timestamp": 0.28088887008122043 + }, + { + "x": 2.7062211012258333, + "y": 6.816129206571772, + "heading": 0.3930579718029317, + "angularVelocity": 1.1973301038102625e-17, + "velocityX": 1.6028117965059043, + "velocityY": 0.6820484398411785, + "timestamp": 0.37451849344162724 + }, + { + "x": 2.8062682139582535, + "y": 6.8587025001415585, + "heading": 0.3930579718029317, + "angularVelocity": -9.389335874202783e-18, + "velocityX": 1.0685412281037379, + "velocityY": 0.45469897284440564, + "timestamp": 0.46814811680203405 + }, + { + "x": 2.8562917709350586, + "y": 6.879989147186279, + "heading": 0.3930579718029317, + "angularVelocity": 6.118834146220853e-17, + "velocityX": 0.5342706205732553, + "velocityY": 0.22734948919726436, + "timestamp": 0.5617777401624409 + }, + { + "x": 2.8562917709350586, + "y": 6.879989147186279, + "heading": 0.3930579718029317, + "angularVelocity": 9.527092238533276e-28, + "velocityX": 5.413823231130942e-26, + "velocityY": -7.426199884271778e-26, + "timestamp": 0.6554073635228477 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePDEABC.4.traj b/src/main/deploy/choreo/CenterLanePDEABC.4.traj new file mode 100644 index 00000000..164ec04b --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePDEABC.4.traj @@ -0,0 +1,248 @@ +{ + "samples": [ + { + "x": 2.8562917709350586, + "y": 6.879989147186279, + "heading": 0.3930579718029317, + "angularVelocity": 9.527092238533276e-28, + "velocityX": 5.413823231130942e-26, + "velocityY": -7.426199884271778e-26, + "timestamp": 0 + }, + { + "x": 2.8345596275672933, + "y": 6.874297000721419, + "heading": 0.3901475474656487, + "angularVelocity": -0.04834267054248828, + "velocityX": -0.36097480135517596, + "velocityY": -0.09454757428506516, + "timestamp": 0.06020404550727143 + }, + { + "x": 2.791210221547733, + "y": 6.8624934986264625, + "heading": 0.38424126669366737, + "angularVelocity": -0.0981043835545563, + "velocityX": -0.7200414134017654, + "velocityY": -0.19605828803531558, + "timestamp": 0.12040809101454286 + }, + { + "x": 2.7264146334756165, + "y": 6.84400633385399, + "heading": 0.3752232336055894, + "angularVelocity": -0.14979114795514328, + "velocityX": -1.076266345993141, + "velocityY": -0.3070751245485542, + "timestamp": 0.1806121365218143 + }, + { + "x": 2.6404493956081283, + "y": 6.818009888574526, + "heading": 0.36292730942366375, + "angularVelocity": -0.20423750726918255, + "velocityX": -1.4278980281666382, + "velocityY": -0.431805621373474, + "timestamp": 0.24081618202908572 + }, + { + "x": 2.5338184232966747, + "y": 6.783217894913327, + "heading": 0.3470968619442176, + "angularVelocity": -0.2629465735410421, + "velocityX": -1.7711595859214113, + "velocityY": -0.5779012584294861, + "timestamp": 0.30102022753635715 + }, + { + "x": 2.407629795142463, + "y": 6.737392802929253, + "heading": 0.3272924409497442, + "angularVelocity": -0.3289549867887417, + "velocityX": -2.0960157592560678, + "velocityY": -0.761163001555094, + "timestamp": 0.3612242730436286 + }, + { + "x": 2.2652115507191506, + "y": 6.676063122618794, + "heading": 0.30267146092102903, + "angularVelocity": -0.40895889671967695, + "velocityX": -2.3655925980274817, + "velocityY": -1.0186969960855932, + "timestamp": 0.4214283185509 + }, + { + "x": 2.1204311340072723, + "y": 6.592422966921264, + "heading": 0.27232018505584993, + "angularVelocity": -0.5041401389133051, + "velocityX": -2.4048287036523215, + "velocityY": -1.3892779960680068, + "timestamp": 0.48163236405817145 + }, + { + "x": 1.992685360010843, + "y": 6.494128334362165, + "heading": 0.23976877399587285, + "angularVelocity": -0.5406847793317353, + "velocityX": -2.1218802311382734, + "velocityY": -1.6326914866082545, + "timestamp": 0.5418364095654429 + }, + { + "x": 1.886238292214546, + "y": 6.388651713547729, + "heading": 0.20716928495643328, + "angularVelocity": -0.5414833631986058, + "velocityX": -1.7681048989214407, + "velocityY": -1.7519856003978374, + "timestamp": 0.6020404550727143 + }, + { + "x": 1.801932900713492, + "y": 6.279311808551523, + "heading": 0.17534566664181037, + "angularVelocity": -0.5285960112228498, + "velocityX": -1.4003276821467197, + "velocityY": -1.8161554439559784, + "timestamp": 0.6622445005799857 + }, + { + "x": 1.740001559257507, + "y": 6.167843341827393, + "heading": 0.1447051188724413, + "angularVelocity": -0.5089449971541862, + "velocityX": -1.0286906956859625, + "velocityY": -1.8515112362451833, + "timestamp": 0.7224485460872572 + }, + { + "x": 1.69740928285499, + "y": 6.020477623351315, + "heading": 0.10712758929116097, + "angularVelocity": -0.47810334665219445, + "velocityX": -0.5419065626848166, + "velocityY": -1.874951439604909, + "timestamp": 0.801045631517443 + }, + { + "x": 1.692516979833633, + "y": 5.879848304102219, + "heading": 0.07335850742026521, + "angularVelocity": -0.429648016667122, + "velocityX": -0.06224534910652133, + "velocityY": -1.7892434366922005, + "timestamp": 0.8796427169476289 + }, + { + "x": 1.7224083305171312, + "y": 5.755218848404405, + "heading": 0.044801662501508355, + "angularVelocity": -0.3633321103760591, + "velocityX": 0.38031118482184595, + "velocityY": -1.585675283195029, + "timestamp": 0.9582398023778147 + }, + { + "x": 1.7819268218986297, + "y": 5.6548248770557, + "heading": 0.022621166422217082, + "angularVelocity": -0.282205071064539, + "velocityX": 0.757260794795832, + "velocityY": -1.277324353685865, + "timestamp": 1.0368368878080005 + }, + { + "x": 1.8649022251310534, + "y": 5.584669337944561, + "heading": 0.0075685613575453385, + "angularVelocity": -0.19151607190373818, + "velocityX": 1.055705854463611, + "velocityY": -0.8925972092623536, + "timestamp": 1.1154339732381864 + }, + { + "x": 1.9654334653384358, + "y": 5.548515796661377, + "heading": 4.814116720601663e-28, + "angularVelocity": -0.09629569997564517, + "velocityX": 1.2790708415858416, + "velocityY": -0.4599857753669162, + "timestamp": 1.1940310586683722 + }, + { + "x": 2.0785037517547607, + "y": 5.548515796661377, + "heading": 4.711208229171849e-28, + "angularVelocity": -4.224112944688565e-28, + "velocityX": 1.43860660732464, + "velocityY": -1.1518576201030206e-26, + "timestamp": 1.272628144098558 + }, + { + "x": 2.2115594481070073, + "y": 5.548515796661377, + "heading": 9.919579688456685e-28, + "angularVelocity": -1.4478744000102224e-30, + "velocityX": 1.8779766683240853, + "velocityY": 6.343973751493874e-16, + "timestamp": 1.3434787028922344 + }, + { + "x": 2.367207543861562, + "y": 5.548515796661377, + "heading": 1.5127951147748595e-27, + "angularVelocity": -1.4478743900360266e-30, + "velocityX": 2.196850644577389, + "velocityY": 3.1719689645397288e-15, + "timestamp": 1.4143292616859107 + }, + { + "x": 2.4917260252771096, + "y": 5.548515796661377, + "heading": 2.033632260703789e-27, + "angularVelocity": -1.4478743937385766e-30, + "velocityX": 1.757480583578164, + "velocityY": 2.537576634676114e-15, + "timestamp": 1.485179820479587 + }, + { + "x": 2.585114887925518, + "y": 5.548515796661377, + "heading": 2.5544694066327203e-27, + "angularVelocity": -1.4478743937382235e-30, + "velocityX": 1.3181104600793105, + "velocityY": 1.903182958104165e-15, + "timestamp": 1.5560303792732633 + }, + { + "x": 2.6473741303307423, + "y": 5.548515796661377, + "heading": 3.0753065525616534e-27, + "angularVelocity": -1.4478743937387e-30, + "velocityX": 0.8787403157472581, + "velocityY": 1.2687888330822125e-15, + "timestamp": 1.6268809380669396 + }, + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": 3.596143698490385e-27, + "angularVelocity": -1.4478743965967547e-30, + "velocityX": 0.439370160998608, + "velocityY": 6.343944838152068e-16, + "timestamp": 1.6977314968606159 + }, + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": 4.090371729515312e-27, + "angularVelocity": -3.7701463571595157e-28, + "velocityX": -1.25991204352499e-25, + "velocityY": -5.479637988962548e-26, + "timestamp": 1.7685820556542922 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePDEABC.5.traj b/src/main/deploy/choreo/CenterLanePDEABC.5.traj new file mode 100644 index 00000000..1862a3b9 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePDEABC.5.traj @@ -0,0 +1,212 @@ +{ + "samples": [ + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": 4.090371729515312e-27, + "angularVelocity": -3.7701463571595157e-28, + "velocityX": -1.25991204352499e-25, + "velocityY": -5.479637988962548e-26, + "timestamp": 0 + }, + { + "x": 2.6551487281624913, + "y": 5.535337362195577, + "heading": -0.005358266388786186, + "angularVelocity": -0.0814332978786503, + "velocityX": -0.3549425234125069, + "velocityY": -0.20028182653884274, + "timestamp": 0.06579945216968852 + }, + { + "x": 2.6089441252483114, + "y": 5.508126235802502, + "heading": -0.016249929633628903, + "angularVelocity": -0.16552817517013077, + "velocityX": -0.7022034590048662, + "velocityY": -0.41354639736059473, + "timestamp": 0.13159890433937704 + }, + { + "x": 2.5407349136089206, + "y": 5.4655968271259, + "heading": -0.03293816472196892, + "angularVelocity": -0.2536227056313986, + "velocityX": -1.036622789251932, + "velocityY": -0.6463489782091849, + "timestamp": 0.19739835650906556 + }, + { + "x": 2.4521626111212957, + "y": 5.405634188324023, + "heading": -0.055847587039488555, + "angularVelocity": -0.3481704111827463, + "velocityX": -1.3460948315984265, + "velocityY": -0.9112938911290818, + "timestamp": 0.2631978086787541 + }, + { + "x": 2.347299393995622, + "y": 5.324395116461008, + "heading": -0.08569447081932854, + "angularVelocity": -0.45360383400866283, + "velocityX": -1.5936791822406984, + "velocityY": -1.2346466297851597, + "timestamp": 0.3289972608484426 + }, + { + "x": 2.239880929566337, + "y": 5.216483135728542, + "heading": -0.1227261468780359, + "angularVelocity": -0.5627961151288575, + "velocityX": -1.6325130512069794, + "velocityY": -1.6400133614209302, + "timestamp": 0.39479671301813113 + }, + { + "x": 2.153374403298574, + "y": 5.091739159361965, + "heading": -0.1618030539678993, + "angularVelocity": -0.5938789123819666, + "velocityX": -1.3146997948352968, + "velocityY": -1.8958208959685574, + "timestamp": 0.46059616518781965 + }, + { + "x": 2.0932393074035645, + "y": 4.9619622230529785, + "heading": -0.19963034769256285, + "angularVelocity": -0.5748876696892882, + "velocityX": -0.9139148414173566, + "velocityY": -1.9723102857195454, + "timestamp": 0.5263956173575082 + }, + { + "x": 2.0629011107007105, + "y": 4.862373019847467, + "heading": -0.22719939375272252, + "angularVelocity": -0.5460591677053324, + "velocityX": -0.600907641312322, + "velocityY": -1.9725599970405534, + "timestamp": 0.576882904616161 + }, + { + "x": 2.048028599586168, + "y": 4.7660067703914, + "heading": -0.2526003668147138, + "angularVelocity": -0.5031162187791786, + "velocityX": -0.29457932723437075, + "velocityY": -1.908723060567117, + "timestamp": 0.6273701918748138 + }, + { + "x": 2.047740775609064, + "y": 4.675697634358311, + "heading": -0.2752320074015959, + "angularVelocity": -0.44826414362367223, + "velocityX": -0.00570091983015821, + "velocityY": -1.788750018800283, + "timestamp": 0.6778574791334666 + }, + { + "x": 2.0608710405337267, + "y": 4.5937072504707785, + "heading": -0.29463334748351117, + "angularVelocity": -0.3842816902108431, + "velocityX": 0.2600707155723077, + "velocityY": -1.62398077495204, + "timestamp": 0.7283447663921194 + }, + { + "x": 2.0861920181883997, + "y": 4.5217424317614965, + "heading": -0.3104741156384499, + "angularVelocity": -0.31375756185482273, + "velocityX": 0.5015317524380102, + "velocityY": -1.4254047427939316, + "timestamp": 0.7788320536507722 + }, + { + "x": 2.122546344803851, + "y": 4.461060494351438, + "heading": -0.3225286157246152, + "angularVelocity": -0.23876307761216561, + "velocityX": 0.7200689240680361, + "velocityY": -1.2019250925322578, + "timestamp": 0.829319340909425 + }, + { + "x": 2.1689002066225895, + "y": 4.412584630951134, + "heading": -0.3306480218329806, + "angularVelocity": -0.16082080359692716, + "velocityX": 0.9181293813878815, + "velocityY": -0.9601597953155385, + "timestamp": 0.8798066281680779 + }, + { + "x": 2.2243532862123785, + "y": 4.376998478332531, + "heading": -0.3347371673945201, + "angularVelocity": -0.0809935685510361, + "velocityX": 1.098357281620145, + "velocityY": -0.7048537275590006, + "timestamp": 0.9302939154267307 + }, + { + "x": 2.2881293296813965, + "y": 4.354815483093262, + "heading": -0.3347371673945201, + "angularVelocity": 5.296142385201319e-27, + "velocityX": 1.2632099471355256, + "velocityY": -0.43937784031895555, + "timestamp": 0.9807812026853835 + }, + { + "x": 2.4097151173059284, + "y": 4.31443319431648, + "heading": -0.3347371673945201, + "angularVelocity": 4.260827770605265e-29, + "velocityX": 1.6917387541138205, + "velocityY": -0.5618772081689671, + "timestamp": 1.0526515120513356 + }, + { + "x": 2.501147235462863, + "y": 4.284859003573599, + "heading": -0.3347371673945201, + "angularVelocity": 4.260827800538464e-29, + "velocityX": 1.2721820590944748, + "velocityY": -0.41149385613875655, + "timestamp": 1.1245218214172876 + }, + { + "x": 2.562139216145648, + "y": 4.265257651838079, + "heading": -0.3347371673945201, + "angularVelocity": 4.260827801548558e-29, + "velocityX": 0.8486394621209058, + "velocityY": -0.27273225770760434, + "timestamp": 1.1963921307832397 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": 1.3873453317425255e-27, + "velocityX": 0.4244517046157105, + "velocityY": -0.13595549514112237, + "timestamp": 1.2682624401491918 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": -3.25532193935464e-28, + "velocityX": 2.1483187881556724e-26, + "velocityY": -1.7096603516619137e-26, + "timestamp": 1.3401327495151438 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePDEABC.traj b/src/main/deploy/choreo/CenterLanePDEABC.traj new file mode 100644 index 00000000..bd4a810d --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePDEABC.traj @@ -0,0 +1,1787 @@ +{ + "samples": [ + { + "x": 1.3350272178649902, + "y": 5.601006507873535, + "heading": 1.3464399983715043e-25, + "angularVelocity": 2.380601151660865e-26, + "velocityX": -3.6373884256645e-26, + "velocityY": 3.3932882505305813e-26, + "timestamp": 0 + }, + { + "x": 1.3513037651461417, + "y": 5.614995878157814, + "heading": 0.006549885022004973, + "angularVelocity": 0.11118193396003606, + "velocityX": 0.27628851481066846, + "velocityY": 0.23746451088284293, + "timestamp": 0.058911414730016334 + }, + { + "x": 1.3843789066016257, + "y": 5.642351283609904, + "heading": 0.01935493583583066, + "angularVelocity": 0.21736111537143754, + "velocityX": 0.5614385871916894, + "velocityY": 0.4643481331666494, + "timestamp": 0.11782282946003267 + }, + { + "x": 1.4348748758418, + "y": 5.682261570187258, + "heading": 0.038033490581189856, + "angularVelocity": 0.31706172447157616, + "velocityX": 0.8571508505030987, + "velocityY": 0.6774627083776117, + "timestamp": 0.17673424419004902 + }, + { + "x": 1.503535032987812, + "y": 5.733639392412991, + "heading": 0.06208045701046981, + "angularVelocity": 0.408188574989825, + "velocityX": 1.1654813835429523, + "velocityY": 0.8721199866136559, + "timestamp": 0.23564565892006534 + }, + { + "x": 1.5912385139518157, + "y": 5.794977395067809, + "heading": 0.09080789039897333, + "angularVelocity": 0.48763781213127805, + "velocityX": 1.4887349313530773, + "velocityY": 1.041190454106761, + "timestamp": 0.29455707365008166 + }, + { + "x": 1.698981116852805, + "y": 5.864111533799201, + "heading": 0.12324878614145561, + "angularVelocity": 0.550672495154884, + "velocityX": 1.828891792783458, + "velocityY": 1.1735270498633465, + "timestamp": 0.353468488380098 + }, + { + "x": 1.8277399268235341, + "y": 5.937848492694958, + "heading": 0.15800938596992012, + "angularVelocity": 0.5900486346791701, + "velocityX": 2.1856343216474206, + "velocityY": 1.2516582606899718, + "timestamp": 0.4123799031101143 + }, + { + "x": 1.9780206143757966, + "y": 6.011501208422765, + "heading": 0.1930700705634149, + "angularVelocity": 0.5951424652450383, + "velocityX": 2.5509604249190097, + "velocityY": 1.2502282633229538, + "timestamp": 0.4712913178401306 + }, + { + "x": 2.1488551922208914, + "y": 6.0788007358405975, + "heading": 0.22564028873153014, + "angularVelocity": 0.5528676966489513, + "velocityX": 2.8998552933757997, + "velocityY": 1.1423851850487312, + "timestamp": 0.5302027325701469 + }, + { + "x": 2.3368515968322754, + "y": 6.133185386657715, + "heading": 0.2525545797921912, + "angularVelocity": 0.456860375599634, + "velocityX": 3.1911711078905185, + "velocityY": 0.9231598165882628, + "timestamp": 0.5891141473001633 + }, + { + "x": 2.5473350968821267, + "y": 6.194075230145087, + "heading": 0.25255484664471606, + "angularVelocity": 0.0000044758460677296985, + "velocityX": 3.530383481883881, + "velocityY": 1.0212890683184164, + "timestamp": 0.6487347242516318 + }, + { + "x": 2.7606743654688723, + "y": 6.255791206318425, + "heading": 0.25255482885764124, + "angularVelocity": -2.983378514405921e-7, + "velocityX": 3.578282524176262, + "velocityY": 1.0351455710261694, + "timestamp": 0.7083553012031003 + }, + { + "x": 2.9740136340556216, + "y": 6.317507182491763, + "heading": 0.25255481107057365, + "angularVelocity": -2.9833773077503677e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.7679758781545688 + }, + { + "x": 3.187352902642371, + "y": 6.379223158665101, + "heading": 0.25255479328350594, + "angularVelocity": -2.9833773187848623e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.8275964551060373 + }, + { + "x": 3.40069217122912, + "y": 6.44093913483844, + "heading": 0.2525547754964383, + "angularVelocity": -2.983377307846883e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 0.8872170320575058 + }, + { + "x": 3.614031439815869, + "y": 6.502655111011779, + "heading": 0.25255475770937064, + "angularVelocity": -2.9833773102796393e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.9468376090089743 + }, + { + "x": 3.827370708402618, + "y": 6.564371087185117, + "heading": 0.25255473992230293, + "angularVelocity": -2.983377313896792e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.006458185960443 + }, + { + "x": 4.040709976989367, + "y": 6.626087063358456, + "heading": 0.2525547221352353, + "angularVelocity": -2.9833773109308854e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.0660787629119115 + }, + { + "x": 4.254049245576117, + "y": 6.6878030395317944, + "heading": 0.2525547043481677, + "angularVelocity": -2.983377300262124e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.12569933986338 + }, + { + "x": 4.467388514162867, + "y": 6.749519015705133, + "heading": 0.2525546865611001, + "angularVelocity": -2.9833773003639016e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.1853199168148487 + }, + { + "x": 4.680727782749616, + "y": 6.811234991878472, + "heading": 0.2525546687740325, + "angularVelocity": -2.9833772985092435e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.2449404937663173 + }, + { + "x": 4.894067051336365, + "y": 6.872950968051811, + "heading": 0.25255465098696483, + "angularVelocity": -2.9833773088728923e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.304561070717786 + }, + { + "x": 5.107406319923115, + "y": 6.934666944225149, + "heading": 0.2525546331998971, + "angularVelocity": -2.9833773138995595e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.3641816476692545 + }, + { + "x": 5.3207455885098645, + "y": 6.996382920398487, + "heading": 0.2525546154128296, + "angularVelocity": -2.9833772971803955e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4238022246207231 + }, + { + "x": 5.534084857096614, + "y": 7.058098896571825, + "heading": 0.252554597625762, + "angularVelocity": -2.983377299147757e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4834228015721918 + }, + { + "x": 5.747424125671387, + "y": 7.119814872741699, + "heading": 0.2525545797921912, + "angularVelocity": -2.9911771592583556e-7, + "velocityX": 3.5782825239754317, + "velocityY": 1.035145570968072, + "timestamp": 1.5430433785236604 + }, + { + "x": 5.874238789453484, + "y": 7.151633771807316, + "heading": 0.23428972710167786, + "angularVelocity": -0.5023194013498574, + "velocityX": 3.4876528747747586, + "velocityY": 0.8750823563200211, + "timestamp": 1.579404412243304 + }, + { + "x": 6.000467771954814, + "y": 7.182688874971163, + "heading": 0.21343765731660022, + "angularVelocity": -0.5734729641036735, + "velocityX": 3.471545486704275, + "velocityY": 0.8540764655727967, + "timestamp": 1.6157654459629476 + }, + { + "x": 6.126526412325093, + "y": 7.213769689844044, + "heading": 0.19236113767335114, + "angularVelocity": -0.5796457770083356, + "velocityX": 3.4668607428005407, + "velocityY": 0.8547835881819195, + "timestamp": 1.6521264796825912 + }, + { + "x": 6.252399106037461, + "y": 7.244861395063954, + "heading": 0.17099373366442527, + "angularVelocity": -0.5876456696384406, + "velocityX": 3.4617468436923633, + "velocityY": 0.8550830941616943, + "timestamp": 1.6884875134022348 + }, + { + "x": 6.378079158099059, + "y": 7.275966592245878, + "heading": 0.1493195497494227, + "angularVelocity": -0.596082720918173, + "velocityX": 3.456448819102211, + "velocityY": 0.8554541496744962, + "timestamp": 1.7248485471218784 + }, + { + "x": 6.503558906617952, + "y": 7.3070873984764315, + "heading": 0.12731920059935156, + "angularVelocity": -0.6050529069030769, + "velocityX": 3.4509400774022723, + "velocityY": 0.8558834292365232, + "timestamp": 1.761209580841522 + }, + { + "x": 6.628826049826062, + "y": 7.3382208951944605, + "heading": 0.10495362803069447, + "angularVelocity": -0.6150972698164627, + "velocityX": 3.44509301286545, + "velocityY": 0.8562324426219444, + "timestamp": 1.7975706145611656 + }, + { + "x": 6.7538656809780315, + "y": 7.369363297624379, + "heading": 0.08217584529224368, + "angularVelocity": -0.6264338608763309, + "velocityX": 3.438835983489081, + "velocityY": 0.8564773672288275, + "timestamp": 1.8339316482808092 + }, + { + "x": 6.8786958169095165, + "y": 7.400552514729427, + "heading": 0.05908376270904082, + "angularVelocity": -0.6350777252718116, + "velocityX": 3.4330744525573853, + "velocityY": 0.8577648629444327, + "timestamp": 1.8702926820004528 + }, + { + "x": 7.003224052672837, + "y": 7.431696922152255, + "heading": 0.03530389927777457, + "angularVelocity": -0.6539930524147748, + "velocityX": 3.4247716036754445, + "velocityY": 0.8565325084804077, + "timestamp": 1.9066537157200965 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": -8.902558475497447e-21, + "angularVelocity": -0.9709267219953129, + "velocityX": 3.3570777699788383, + "velocityY": 0.7233864155389959, + "timestamp": 1.94301474943974 + }, + { + "x": 7.338537269448379, + "y": 7.482023887493991, + "heading": -0.008039547554639858, + "angularVelocity": -0.1278866817794853, + "velocityX": 3.39215287381437, + "velocityY": 0.38215275600640763, + "timestamp": 2.0058793686696177 + }, + { + "x": 7.527981936001141, + "y": 7.500234795903286, + "heading": -0.013861641566031323, + "angularVelocity": -0.09261320728121766, + "velocityX": 3.013533985786477, + "velocityY": 0.2896845416768293, + "timestamp": 2.068743987899495 + }, + { + "x": 7.693596991003447, + "y": 7.512743640283509, + "heading": -0.017657163586421762, + "angularVelocity": -0.060376123595234456, + "velocityX": 2.6344716158496166, + "velocityY": 0.1989806751947781, + "timestamp": 2.1316086071293725 + }, + { + "x": 7.835373269936495, + "y": 7.519587451824941, + "heading": -0.01948976746028864, + "angularVelocity": -0.02915159427221823, + "velocityX": 2.255263464089624, + "velocityY": 0.10886587122090353, + "timestamp": 2.19447322635925 + }, + { + "x": 7.953306214016821, + "y": 7.520784758027623, + "heading": -0.01939144141260055, + "angularVelocity": 0.0015640919947123182, + "velocityX": 1.875982794854424, + "velocityY": 0.01904578787480972, + "timestamp": 2.2573378455891273 + }, + { + "x": 8.04739309462225, + "y": 7.516346680416715, + "heading": -0.01738162754383806, + "angularVelocity": 0.031970508902840686, + "velocityX": 1.4966587208836901, + "velocityY": -0.07059738315886452, + "timestamp": 2.3202024648190047 + }, + { + "x": 8.117632094556283, + "y": 7.5062806367543455, + "heading": -0.0134735913597302, + "angularVelocity": 0.06216590877322135, + "velocityX": 1.1173057404068745, + "velocityY": -0.16012255837517314, + "timestamp": 2.383067084048882 + }, + { + "x": 8.164021915899584, + "y": 7.490591928437994, + "heading": -0.007677144931297963, + "angularVelocity": 0.09220522607854079, + "velocityX": 0.7379321136689944, + "velocityY": -0.24956340320748793, + "timestamp": 2.4459317032787595 + }, + { + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 2.2979832887228624e-21, + "angularVelocity": 0.12212187117884055, + "velocityX": 0.358542990464167, + "velocityY": -0.33894095350730474, + "timestamp": 2.508796322508637 + }, + { + "x": 8.169433855540824, + "y": 7.426853893988931, + "heading": 0.014976899045789983, + "angularVelocity": 0.16512821061392627, + "velocityX": -0.18884224442902273, + "velocityY": -0.46782018853404506, + "timestamp": 2.599494931729361 + }, + { + "x": 8.102655131439361, + "y": 7.372743106744783, + "heading": 0.03346799984598468, + "angularVelocity": 0.20387413830342968, + "velocityX": -0.7362706515041357, + "velocityY": -0.5965999667366941, + "timestamp": 2.6901935409500854 + }, + { + "x": 7.986220220035368, + "y": 7.306964887600374, + "heading": 0.05493172658414638, + "angularVelocity": 0.23664890699621968, + "velocityX": -1.2837563045827718, + "velocityY": -0.7252395566985038, + "timestamp": 2.7808921501708097 + }, + { + "x": 7.820121969665864, + "y": 7.229538517591083, + "heading": 0.07855370015299508, + "angularVelocity": 0.26044471653762835, + "velocityX": -1.8313208085174242, + "velocityY": -0.8536665630766948, + "timestamp": 2.871590759391534 + }, + { + "x": 7.6043501738014525, + "y": 7.14049675145262, + "heading": 0.10296815130417637, + "angularVelocity": 0.2691821998258671, + "velocityX": -2.3789978448215092, + "velocityY": -0.9817324312192064, + "timestamp": 2.9622893686122582 + }, + { + "x": 7.338891049230189, + "y": 7.039907645727491, + "heading": 0.1254011174832251, + "angularVelocity": 0.24733528299707236, + "velocityX": -2.9268268483063693, + "velocityY": -1.1090479400884177, + "timestamp": 3.0529879778329825 + }, + { + "x": 7.023762471374015, + "y": 6.927998022527093, + "heading": 0.1370927000479183, + "angularVelocity": 0.12890586377394736, + "velocityX": -3.4744587658370385, + "velocityY": -1.2338626155562706, + "timestamp": 3.1436865870537067 + }, + { + "x": 6.694988070852044, + "y": 6.85020567874946, + "heading": 0.13709270182779565, + "angularVelocity": 1.9624086279332165e-8, + "velocityX": -3.624911157368082, + "velocityY": -0.8577016168827462, + "timestamp": 3.234385196274431 + }, + { + "x": 6.366213601226777, + "y": 6.7724136270242035, + "heading": 0.13709270360766948, + "angularVelocity": 1.962404782489554e-8, + "velocityX": -3.624911919268354, + "velocityY": -0.8576983968512897, + "timestamp": 3.3250838054951553 + }, + { + "x": 6.0374391323544945, + "y": 6.69462157211659, + "heading": 0.13709270538754328, + "angularVelocity": 1.962404757872295e-8, + "velocityX": -3.6249119109663046, + "velocityY": -0.857698431938445, + "timestamp": 3.4157824147158795 + }, + { + "x": 5.708664663238629, + "y": 6.616829518238441, + "heading": 0.13709270716741703, + "angularVelocity": 1.9624046615308717e-8, + "velocityX": -3.6249119136519434, + "velocityY": -0.8576984205880644, + "timestamp": 3.5064810239366038 + }, + { + "x": 5.3798901940960535, + "y": 6.539037464473215, + "heading": 0.13709270894730027, + "angularVelocity": 1.9624151634826213e-8, + "velocityX": -3.6249119139464265, + "velocityY": -0.8576984193430164, + "timestamp": 3.597179633157328 + }, + { + "x": 5.082161936855116, + "y": 6.468591654596992, + "heading": 0.17334127091967155, + "angularVelocity": 0.3996595127953577, + "velocityX": -3.2826110543369467, + "velocityY": -0.7767022061472455, + "timestamp": 3.6878782423780523 + }, + { + "x": 4.834055003719788, + "y": 6.409886994261807, + "heading": 0.20354833113507975, + "angularVelocity": 0.33304876971041714, + "velocityX": -2.735509786390822, + "velocityY": -0.647249840318068, + "timestamp": 3.7785768515987765 + }, + { + "x": 4.635569429511693, + "y": 6.362923375586849, + "heading": 0.22771434771261193, + "angularVelocity": 0.2664430776300183, + "velocityX": -2.1884081345179167, + "velocityY": -0.5177986639317362, + "timestamp": 3.869275460819501 + }, + { + "x": 4.486705235532264, + "y": 6.327700716475024, + "heading": 0.24583917728696456, + "angularVelocity": 0.19983580487153885, + "velocityX": -1.6413062477854834, + "velocityY": -0.38834839270916643, + "timestamp": 3.959974070040225 + }, + { + "x": 4.387462434805768, + "y": 6.304218961949016, + "heading": 0.25792250356094953, + "angularVelocity": 0.13322504476974903, + "velocityX": -1.0942042174536384, + "velocityY": -0.25889872764050514, + "timestamp": 4.050672679260949 + }, + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": 0.06661212070370816, + "velocityX": -0.54710211431646, + "velocityY": -0.12944936516318697, + "timestamp": 4.141371288481674 + }, + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": 4.2322185448004084e-23, + "velocityX": 1.750098119560174e-21, + "velocityY": 1.2842584640589406e-21, + "timestamp": 4.232069897702398 + }, + { + "x": 4.357336303204376, + "y": 6.297583237177048, + "heading": 0.24752122945545674, + "angularVelocity": -0.28561960466886005, + "velocityX": 0.3386404361144792, + "velocityY": 0.08867849340285801, + "timestamp": 4.289639128041561 + }, + { + "x": 4.396392552566799, + "y": 6.3076902844788645, + "heading": 0.21545182139905172, + "angularVelocity": -0.5570581344838518, + "velocityX": 0.6784222949017583, + "velocityY": 0.17556335636714013, + "timestamp": 4.3472083583807235 + }, + { + "x": 4.455088009056865, + "y": 6.322669909558741, + "heading": 0.16876627146696008, + "angularVelocity": -0.8109462234782202, + "velocityX": 1.0195629878021513, + "velocityY": 0.26020193411697123, + "timestamp": 4.404777588719886 + }, + { + "x": 4.533516711715102, + "y": 6.342355579726992, + "heading": 0.10873908045256343, + "angularVelocity": -1.042695736259688, + "velocityX": 1.3623371755394689, + "velocityY": 0.3419477740500502, + "timestamp": 4.462346819059049 + }, + { + "x": 4.631792826161861, + "y": 6.366523725829109, + "heading": 0.03701265289139425, + "angularVelocity": -1.2459160412359371, + "velocityX": 1.7070944646606674, + "velocityY": 0.4198101305112548, + "timestamp": 4.519916049398212 + }, + { + "x": 4.750056312234907, + "y": 6.394856813133717, + "heading": -0.04421826686802747, + "angularVelocity": -1.4110127802796573, + "velocityX": 2.054282910789491, + "velocityY": 0.492156784756161, + "timestamp": 4.577485279737375 + }, + { + "x": 4.888478272202705, + "y": 6.42687003155547, + "heading": -0.13181310680866934, + "angularVelocity": -1.5215565576365515, + "velocityX": 2.4044434701020125, + "velocityY": 0.5560820985993948, + "timestamp": 4.635054510076538 + }, + { + "x": 5.047249483224003, + "y": 6.461748830996926, + "heading": -0.22073093757153267, + "angularVelocity": -1.5445374245758388, + "velocityX": 2.757917903121444, + "velocityY": 0.6058583593348625, + "timestamp": 4.6926237404157005 + }, + { + "x": 5.226429045018527, + "y": 6.497908973529479, + "heading": -0.30132047165929504, + "angularVelocity": -1.399871660833024, + "velocityX": 3.1124189213388544, + "velocityY": 0.6281157889295987, + "timestamp": 4.750192970754863 + }, + { + "x": 5.423903541491299, + "y": 6.531166091125145, + "heading": -0.3435649109260108, + "angularVelocity": -0.7338023978753491, + "velocityX": 3.430209077129816, + "velocityY": 0.5776891127384427, + "timestamp": 4.807762201094026 + }, + { + "x": 5.636616267297259, + "y": 6.55438559892435, + "heading": -0.3449872384479477, + "angularVelocity": -0.024706384184006707, + "velocityX": 3.6949030680588684, + "velocityY": 0.40333191294048215, + "timestamp": 4.865331431433189 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.3449872632321153, + "angularVelocity": -4.3051066415640403e-7, + "velocityX": 3.7246964993017384, + "velocityY": 0.047619187556732175, + "timestamp": 4.922900661772352 + }, + { + "x": 5.9707478293428995, + "y": 6.552244535149851, + "heading": -0.34498728566747394, + "angularVelocity": -6.975751659806049e-7, + "velocityX": 3.7219059383782973, + "velocityY": -0.151808826449267, + "timestamp": 4.9550625845387355 + }, + { + "x": 6.090018567999844, + "y": 6.540962083011693, + "heading": -0.3449873062012292, + "angularVelocity": -6.384492454517859e-7, + "velocityX": 3.7084455280643134, + "velocityY": -0.3508015431822894, + "timestamp": 4.987224507305119 + }, + { + "x": 6.208514463680753, + "y": 6.523311991997161, + "heading": -0.3449873252853557, + "angularVelocity": -5.933764159352289e-7, + "velocityX": 3.6843535923407127, + "velocityY": -0.5487884273193968, + "timestamp": 5.019386430071503 + }, + { + "x": 6.325895808833114, + "y": 6.499344869625537, + "heading": -0.34498734327239833, + "angularVelocity": -5.592651525230829e-7, + "velocityX": 3.6496992423305046, + "velocityY": -0.7452017886404377, + "timestamp": 5.051548352837886 + }, + { + "x": 6.441826094112058, + "y": 6.46912943973789, + "heading": -0.34498736044751604, + "angularVelocity": -5.340202407325154e-7, + "velocityX": 3.6045819188434196, + "velocityY": -0.9394783423592116, + "timestamp": 5.08371027560427 + }, + { + "x": 6.555972977983021, + "y": 6.432752355963861, + "heading": -0.3449873770504224, + "angularVelocity": -5.162286594877017e-7, + "velocityX": 3.5491312102233143, + "velocityY": -1.131060603505082, + "timestamp": 5.115872198370654 + }, + { + "x": 6.66800926163723, + "y": 6.390318000635929, + "heading": -0.3449873932911425, + "angularVelocity": -5.049673254841841e-7, + "velocityX": 3.4835070175379053, + "velocityY": -1.319397339399292, + "timestamp": 5.148034121137037 + }, + { + "x": 6.777614079975414, + "y": 6.34194872129089, + "heading": -0.344987409362088, + "angularVelocity": -4.996885790126847e-7, + "velocityX": 3.407906272716571, + "velocityY": -1.5039299639011887, + "timestamp": 5.180196043903421 + }, + { + "x": 6.886436638977974, + "y": 6.291844289930693, + "heading": -0.3449874254080791, + "angularVelocity": -4.98912679463648e-7, + "velocityX": 3.3835837425834847, + "velocityY": -1.557880470149199, + "timestamp": 5.2123579666698046 + }, + { + "x": 6.995259181110964, + "y": 6.241739821931245, + "heading": -0.34498744145406995, + "angularVelocity": -4.989126736688575e-7, + "velocityX": 3.383583218063541, + "velocityY": -1.5578816093613586, + "timestamp": 5.244519889436188 + }, + { + "x": 7.104082434838663, + "y": 6.191636899482441, + "heading": -0.34498745750007104, + "angularVelocity": -4.98912992332448e-7, + "velocityX": 3.3836053434418156, + "velocityY": -1.5578335540676167, + "timestamp": 5.276681812202572 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -6.552383423892292e-7, + "velocityX": 3.442174665036408, + "velocityY": -1.4237498464482023, + "timestamp": 5.3088437349689555 + }, + { + "x": 7.41054811335349, + "y": 6.075273519082507, + "heading": -0.3626370912094401, + "angularVelocity": -0.2874236730044668, + "velocityX": 3.187927819628061, + "velocityY": -1.1492777517419182, + "timestamp": 5.370249994415684 + }, + { + "x": 7.584442418775818, + "y": 6.012933970935389, + "heading": -0.3763799731717047, + "angularVelocity": -0.22380262347988858, + "velocityX": 2.831866115752982, + "velocityY": -1.0151985922737843, + "timestamp": 5.431656253862413 + }, + { + "x": 7.73639717593505, + "y": 5.958610534020796, + "heading": -0.3857543878763611, + "angularVelocity": -0.15266220071243664, + "velocityX": 2.4745809063822897, + "velocityY": -0.8846563429208651, + "timestamp": 5.493062513309142 + }, + { + "x": 7.866387295940067, + "y": 5.912228690600519, + "heading": -0.39060211670074774, + "angularVelocity": -0.07894519008427879, + "velocityX": 2.1168871248017687, + "velocityY": -0.7553276137999276, + "timestamp": 5.554468772755871 + }, + { + "x": 7.974400233400663, + "y": 5.873750785403185, + "heading": -0.39084333039715696, + "angularVelocity": -0.003928161372840292, + "velocityX": 1.7589890417327878, + "velocityY": -0.62661210019988, + "timestamp": 5.6158750322026 + }, + { + "x": 8.060428467430407, + "y": 5.843154117743718, + "heading": -0.3864300742351692, + "angularVelocity": 0.0718698093932331, + "velocityX": 1.4009684811427807, + "velocityY": -0.49826626691061593, + "timestamp": 5.6772812916493285 + }, + { + "x": 8.124466994034947, + "y": 5.820423531988022, + "heading": -0.37733049959220943, + "angularVelocity": 0.14818643449294983, + "velocityX": 1.0428664305809843, + "velocityY": -0.3701672428918256, + "timestamp": 5.738687551096057 + }, + { + "x": 8.16651225164957, + "y": 5.805548215416436, + "heading": -0.3635219686266917, + "angularVelocity": 0.22487171649816598, + "velocityX": 0.6847063799920386, + "velocityY": -0.24224430384806347, + "timestamp": 5.800093810542786 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.3018338882695626, + "velocityX": 0.32650307971422843, + "velocityY": -0.11445294476423894, + "timestamp": 5.861500069989515 + }, + { + "x": 8.182873431203555, + "y": 5.799961031790675, + "heading": -0.31977664522832494, + "angularVelocity": 0.3842917157740494, + "velocityX": -0.056218956684159486, + "velocityY": 0.021964473717296987, + "timestamp": 5.927103444485088 + }, + { + "x": 8.154073485915251, + "y": 5.810352807071496, + "heading": -0.2894961331766682, + "angularVelocity": 0.46156942816562346, + "velocityX": -0.4390009738027679, + "velocityY": 0.15840306021945175, + "timestamp": 5.99270681898066 + }, + { + "x": 8.100157148407472, + "y": 5.829696959053818, + "heading": -0.25457167500592204, + "angularVelocity": 0.5323576483569935, + "velocityX": -0.8218531123184383, + "velocityY": 0.29486519757649204, + "timestamp": 6.058310193476233 + }, + { + "x": 8.021119032382456, + "y": 5.857995174413058, + "heading": -0.21555305979528627, + "angularVelocity": 0.5947653685599519, + "velocityX": -1.2047873548082333, + "velocityY": 0.4313530451265237, + "timestamp": 6.1239135679718055 + }, + { + "x": 7.916952910031998, + "y": 5.895249222659617, + "heading": -0.17317929559353756, + "angularVelocity": 0.6459083016317765, + "velocityX": -1.5878165285154446, + "velocityY": 0.567867865532928, + "timestamp": 6.189516942467378 + }, + { + "x": 7.787651985971443, + "y": 5.941460762829762, + "heading": -0.12849926666272526, + "angularVelocity": 0.6810629677872386, + "velocityX": -1.9709492850749835, + "velocityY": 0.7044079748255081, + "timestamp": 6.255120316962951 + }, + { + "x": 7.633210581172123, + "y": 5.996630683324096, + "heading": -0.08312168761624714, + "angularVelocity": 0.6916958067384257, + "velocityX": -2.354168607740487, + "velocityY": 0.8409616261135585, + "timestamp": 6.320723691458523 + }, + { + "x": 7.453632538523732, + "y": 6.060756399754721, + "heading": -0.03982832718204048, + "angularVelocity": 0.6599258158149841, + "velocityX": -2.7373293527836737, + "velocityY": 0.9774758832711061, + "timestamp": 6.386327065954096 + }, + { + "x": 7.248984101269758, + "y": 6.133814967695316, + "heading": -0.004587884016795663, + "angularVelocity": 0.5371742450172758, + "velocityX": -3.1194803442281134, + "velocityY": 1.11364039582332, + "timestamp": 6.451930440449669 + }, + { + "x": 7.020152981307638, + "y": 6.215344107160959, + "heading": -7.996010360253712e-8, + "angularVelocity": 0.06993244009119806, + "velocityX": -3.488099838180771, + "velocityY": 1.242758319865779, + "timestamp": 6.517533814945241 + }, + { + "x": 6.789970042208637, + "y": 6.297403837687721, + "heading": -7.492708758217055e-8, + "angularVelocity": 7.671885873349635e-8, + "velocityX": -3.5087057772391836, + "velocityY": 1.2508461821929457, + "timestamp": 6.583137189440814 + }, + { + "x": 6.5597871223902215, + "y": 6.379463622297776, + "heading": -6.98940737576559e-8, + "angularVelocity": 7.671882526186561e-8, + "velocityX": -3.5087054833429296, + "velocityY": 1.2508470065910036, + "timestamp": 6.648740563936387 + }, + { + "x": 6.3296039103658925, + "y": 6.46152258724702, + "heading": -6.48610593666683e-8, + "angularVelocity": 7.671883389668008e-8, + "velocityX": -3.5087099374722537, + "velocityY": 1.2508345124042644, + "timestamp": 6.714343938431959 + }, + { + "x": 6.092938315579126, + "y": 6.522410853761584, + "heading": -5.975605245251985e-8, + "angularVelocity": 7.781622444578512e-8, + "velocityX": -3.6075216649524458, + "velocityY": 0.9281270511271241, + "timestamp": 6.779947312927532 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -5.436468625345948e-8, + "angularVelocity": 8.218123290935499e-8, + "velocityX": -3.6872209612695626, + "velocityY": 0.5291823081772375, + "timestamp": 6.8455506874231045 + }, + { + "x": 5.6615808215647085, + "y": 6.568071618356437, + "heading": -4.94825886082418e-8, + "angularVelocity": 9.582618856188379e-8, + "velocityX": -3.71880135129745, + "velocityY": 0.2148218335343848, + "timestamp": 6.896498113366742 + }, + { + "x": 5.471871500054035, + "y": 6.562921703037617, + "heading": -4.486806140713326e-8, + "angularVelocity": 9.057429527869525e-8, + "velocityX": -3.7236291725620743, + "velocityY": -0.10108293448460899, + "timestamp": 6.94744553931038 + }, + { + "x": 5.283280913941195, + "y": 6.541714647593256, + "heading": -4.0367954549807895e-8, + "angularVelocity": 8.832844395914553e-8, + "velocityX": -3.7016705480170033, + "velocityY": -0.4162537174659499, + "timestamp": 6.998392965254017 + }, + { + "x": 5.095501482386873, + "y": 6.514236775268743, + "heading": -3.5882626212892094e-8, + "angularVelocity": 8.803837002241881e-8, + "velocityX": -3.68574914387354, + "velocityY": -0.539337794119592, + "timestamp": 7.049340391197655 + }, + { + "x": 4.907722071138919, + "y": 6.486758764174086, + "heading": -3.139729791987603e-8, + "angularVelocity": 8.803836916075124e-8, + "velocityX": -3.6857487452985973, + "velocityY": -0.5393405179106829, + "timestamp": 7.100287817141292 + }, + { + "x": 4.7199426574500984, + "y": 6.459280769759847, + "heading": -2.6911969659920007e-8, + "angularVelocity": 8.803836851184644e-8, + "velocityX": -3.6857487932081243, + "velocityY": -0.5393401905061415, + "timestamp": 7.15123524308493 + }, + { + "x": 4.532163244787761, + "y": 6.431802768330805, + "heading": -2.2426641378076926e-8, + "angularVelocity": 8.803836894144722e-8, + "velocityX": -3.6857487730602023, + "velocityY": -0.5393403281932087, + "timestamp": 7.202182669028567 + }, + { + "x": 4.3443838326647874, + "y": 6.404324763215868, + "heading": -1.7941313081549045e-8, + "angularVelocity": 8.803836922968166e-8, + "velocityX": -3.6857487624735623, + "velocityY": -0.5393404005402795, + "timestamp": 7.253130094972205 + }, + { + "x": 4.156604420793327, + "y": 6.376846756382128, + "heading": -1.3455984799695103e-8, + "angularVelocity": 8.80383689416604e-8, + "velocityX": -3.685748757536816, + "velocityY": -0.5393404342770572, + "timestamp": 7.304077520915842 + }, + { + "x": 3.968825009218295, + "y": 6.349368747522659, + "heading": -8.970656536814864e-9, + "angularVelocity": 8.803836856924311e-8, + "velocityX": -3.685748751718508, + "velocityY": -0.5393404740382505, + "timestamp": 7.35502494685948 + }, + { + "x": 3.781045597297714, + "y": 6.321890741024598, + "heading": -4.485328240150933e-9, + "angularVelocity": 8.803836923235202e-8, + "velocityX": -3.6857487585009534, + "velocityY": -0.5393404276883234, + "timestamp": 7.4059723728031175 + }, + { + "x": 3.5932661777631214, + "y": 6.2944127865592865, + "heading": -1.5290485245544054e-22, + "angularVelocity": 8.803836812310754e-8, + "velocityX": -3.6857489079493813, + "velocityY": -0.5393394063855248, + "timestamp": 7.456919798746755 + }, + { + "x": 3.405486822128296, + "y": 6.266934394836426, + "heading": -7.242717716527151e-23, + "angularVelocity": 4.066531625418711e-24, + "velocityX": -3.6857476537198193, + "velocityY": -0.5393479889103638, + "timestamp": 7.507867224690393 + }, + { + "x": 3.1943671277625394, + "y": 6.2230448572371815, + "heading": -6.454692239159141e-23, + "angularVelocity": 6.1636286209902435e-25, + "velocityX": -3.5183764765296694, + "velocityY": -0.7314330248480798, + "timestamp": 7.5678720865276885 + }, + { + "x": 3.002694024502071, + "y": 6.190127687410396, + "heading": -5.666660567021065e-23, + "angularVelocity": 6.173952401211252e-25, + "velocityX": -3.1942928854697095, + "velocityY": -0.5485750457361542, + "timestamp": 7.627876948364984 + }, + { + "x": 2.830467511330891, + "y": 6.168182901091022, + "heading": -4.87863785909476e-23, + "angularVelocity": 6.159013258773547e-25, + "velocityX": -2.870209311341694, + "velocityY": -0.36571680439623283, + "timestamp": 7.68788181020228 + }, + { + "x": 2.6776875881237743, + "y": 6.157210503145755, + "heading": -4.0906073219636964e-23, + "angularVelocity": 6.172060876090466e-25, + "velocityX": -2.5461257393006025, + "velocityY": -0.1828584819513171, + "timestamp": 7.747886672039576 + }, + { + "x": 2.5443542548814535, + "y": 6.157210495895674, + "heading": -3.3025815924682597e-23, + "angularVelocity": 6.164048799254959e-25, + "velocityX": -2.2220421672473036, + "velocityY": -1.208249057531058e-7, + "timestamp": 7.807891533876872 + }, + { + "x": 2.430467511630842, + "y": 6.168182880686502, + "heading": -2.5145589184649632e-23, + "angularVelocity": 6.158956724948031e-25, + "velocityX": -1.8979585947454753, + "velocityY": 0.18285826272844113, + "timestamp": 7.867896395714168 + }, + { + "x": 2.3360273584029394, + "y": 6.1901276583922495, + "heading": -1.726521486745078e-23, + "angularVelocity": 6.183550926375638e-25, + "velocityX": -1.5738750217270354, + "velocityY": 0.3657166608474323, + "timestamp": 7.927901257551464 + }, + { + "x": 2.261033795226874, + "y": 6.223044829624826, + "heading": -9.385003241848346e-24, + "angularVelocity": 6.156437857286479e-25, + "velocityX": -1.2497914482231562, + "velocityY": 0.5485750691641013, + "timestamp": 7.98790611938876 + }, + { + "x": 2.205486822128296, + "y": 6.266934394836426, + "heading": -1.5067646314431248e-24, + "angularVelocity": 5.827628634700505e-25, + "velocityX": -0.9257078742918208, + "velocityY": 0.7314334850167038, + "timestamp": 8.047910981226057 + }, + { + "x": 2.166583654686104, + "y": 6.330345399138925, + "heading": 0.03190215483964595, + "angularVelocity": 0.46789281830201823, + "velocityX": -0.5705731398677051, + "velocityY": 0.9300172249677102, + "timestamp": 8.116093593977881 + }, + { + "x": 2.1528025694390287, + "y": 6.405563401029407, + "heading": 0.09569947573294339, + "angularVelocity": 0.9356831356039661, + "velocityX": -0.20212022817660624, + "velocityY": 1.1031845048863924, + "timestamp": 8.184276206729706 + }, + { + "x": 2.1659059898168422, + "y": 6.488336785914272, + "heading": 0.18898264963043915, + "angularVelocity": 1.3681372733109376, + "velocityX": 0.1921812592531237, + "velocityY": 1.2139954974466904, + "timestamp": 8.25245881948153 + }, + { + "x": 2.2052860593561716, + "y": 6.55939873181835, + "heading": 0.2899151707587667, + "angularVelocity": 1.4803263919455447, + "velocityX": 0.5775676224475003, + "velocityY": 1.0422297274341026, + "timestamp": 8.320641432233355 + }, + { + "x": 2.238286501729519, + "y": 6.603471711362833, + "heading": 0.35830069991198743, + "angularVelocity": 1.0029760725382513, + "velocityX": 0.4840008477449354, + "velocityY": 0.6463961670829911, + "timestamp": 8.38882404498518 + }, + { + "x": 2.256009101867676, + "y": 6.624549388885498, + "heading": 0.3930579718029317, + "angularVelocity": 0.5097673803944184, + "velocityX": 0.2599284395666251, + "velocityY": 0.3091356677601188, + "timestamp": 8.457006657737004 + }, + { + "x": 2.256009101867676, + "y": 6.624549388885498, + "heading": 0.3930579718029317, + "angularVelocity": 1.058900443902291e-24, + "velocityX": 2.5593812661454674e-24, + "velocityY": -7.939073774690169e-24, + "timestamp": 8.525189270488829 + }, + { + "x": 2.306032658844481, + "y": 6.645836035930219, + "heading": 0.3930579718029317, + "angularVelocity": 7.690085231355673e-17, + "velocityX": 0.5342706205732553, + "velocityY": 0.22734948919726436, + "timestamp": 8.618818893849236 + }, + { + "x": 2.406079771576901, + "y": 6.688409329500005, + "heading": 0.3930579718029317, + "angularVelocity": 7.285372137244723e-17, + "velocityX": 1.0685412281037379, + "velocityY": 0.45469897284440564, + "timestamp": 8.712448517209642 + }, + { + "x": 2.556150436401367, + "y": 6.752269268035889, + "heading": 0.3930579718029317, + "angularVelocity": 1.0864711023830975e-16, + "velocityX": 1.6028117965059043, + "velocityY": 0.6820484398411784, + "timestamp": 8.80607814057005 + }, + { + "x": 2.7062211012258333, + "y": 6.816129206571772, + "heading": 0.3930579718029317, + "angularVelocity": 1.1973301038102625e-17, + "velocityX": 1.6028117965059043, + "velocityY": 0.6820484398411785, + "timestamp": 8.899707763930456 + }, + { + "x": 2.8062682139582535, + "y": 6.8587025001415585, + "heading": 0.3930579718029317, + "angularVelocity": -9.389335874202783e-18, + "velocityX": 1.0685412281037379, + "velocityY": 0.45469897284440564, + "timestamp": 8.993337387290863 + }, + { + "x": 2.8562917709350586, + "y": 6.879989147186279, + "heading": 0.3930579718029317, + "angularVelocity": 6.118834146220853e-17, + "velocityX": 0.5342706205732553, + "velocityY": 0.22734948919726436, + "timestamp": 9.08696701065127 + }, + { + "x": 2.8562917709350586, + "y": 6.879989147186279, + "heading": 0.3930579718029317, + "angularVelocity": 9.527092238533276e-28, + "velocityX": 5.413823231130942e-26, + "velocityY": -7.426199884271778e-26, + "timestamp": 9.180596634011676 + }, + { + "x": 2.8345596275672933, + "y": 6.874297000721419, + "heading": 0.3901475474656487, + "angularVelocity": -0.04834267054248828, + "velocityX": -0.36097480135517596, + "velocityY": -0.09454757428506516, + "timestamp": 9.240800679518948 + }, + { + "x": 2.791210221547733, + "y": 6.8624934986264625, + "heading": 0.38424126669366737, + "angularVelocity": -0.0981043835545563, + "velocityX": -0.7200414134017654, + "velocityY": -0.19605828803531558, + "timestamp": 9.30100472502622 + }, + { + "x": 2.7264146334756165, + "y": 6.84400633385399, + "heading": 0.3752232336055894, + "angularVelocity": -0.14979114795514328, + "velocityX": -1.076266345993141, + "velocityY": -0.3070751245485542, + "timestamp": 9.36120877053349 + }, + { + "x": 2.6404493956081283, + "y": 6.818009888574526, + "heading": 0.36292730942366375, + "angularVelocity": -0.20423750726918255, + "velocityX": -1.4278980281666382, + "velocityY": -0.431805621373474, + "timestamp": 9.421412816040762 + }, + { + "x": 2.5338184232966747, + "y": 6.783217894913327, + "heading": 0.3470968619442176, + "angularVelocity": -0.2629465735410421, + "velocityX": -1.7711595859214113, + "velocityY": -0.5779012584294861, + "timestamp": 9.481616861548034 + }, + { + "x": 2.407629795142463, + "y": 6.737392802929253, + "heading": 0.3272924409497442, + "angularVelocity": -0.3289549867887417, + "velocityX": -2.0960157592560678, + "velocityY": -0.761163001555094, + "timestamp": 9.541820907055305 + }, + { + "x": 2.2652115507191506, + "y": 6.676063122618794, + "heading": 0.30267146092102903, + "angularVelocity": -0.40895889671967695, + "velocityX": -2.3655925980274817, + "velocityY": -1.0186969960855932, + "timestamp": 9.602024952562576 + }, + { + "x": 2.1204311340072723, + "y": 6.592422966921264, + "heading": 0.27232018505584993, + "angularVelocity": -0.5041401389133051, + "velocityX": -2.4048287036523215, + "velocityY": -1.3892779960680068, + "timestamp": 9.662228998069848 + }, + { + "x": 1.992685360010843, + "y": 6.494128334362165, + "heading": 0.23976877399587285, + "angularVelocity": -0.5406847793317353, + "velocityX": -2.1218802311382734, + "velocityY": -1.6326914866082545, + "timestamp": 9.72243304357712 + }, + { + "x": 1.886238292214546, + "y": 6.388651713547729, + "heading": 0.20716928495643328, + "angularVelocity": -0.5414833631986058, + "velocityX": -1.7681048989214407, + "velocityY": -1.7519856003978374, + "timestamp": 9.78263708908439 + }, + { + "x": 1.801932900713492, + "y": 6.279311808551523, + "heading": 0.17534566664181037, + "angularVelocity": -0.5285960112228498, + "velocityX": -1.4003276821467197, + "velocityY": -1.8161554439559784, + "timestamp": 9.842841134591662 + }, + { + "x": 1.740001559257507, + "y": 6.167843341827393, + "heading": 0.1447051188724413, + "angularVelocity": -0.5089449971541862, + "velocityX": -1.0286906956859625, + "velocityY": -1.8515112362451833, + "timestamp": 9.903045180098934 + }, + { + "x": 1.69740928285499, + "y": 6.020477623351315, + "heading": 0.10712758929116097, + "angularVelocity": -0.47810334665219445, + "velocityX": -0.5419065626848166, + "velocityY": -1.874951439604909, + "timestamp": 9.98164226552912 + }, + { + "x": 1.692516979833633, + "y": 5.879848304102219, + "heading": 0.07335850742026521, + "angularVelocity": -0.429648016667122, + "velocityX": -0.06224534910652133, + "velocityY": -1.7892434366922005, + "timestamp": 10.060239350959305 + }, + { + "x": 1.7224083305171312, + "y": 5.755218848404405, + "heading": 0.044801662501508355, + "angularVelocity": -0.3633321103760591, + "velocityX": 0.38031118482184595, + "velocityY": -1.585675283195029, + "timestamp": 10.138836436389491 + }, + { + "x": 1.7819268218986297, + "y": 5.6548248770557, + "heading": 0.022621166422217082, + "angularVelocity": -0.282205071064539, + "velocityX": 0.757260794795832, + "velocityY": -1.277324353685865, + "timestamp": 10.217433521819677 + }, + { + "x": 1.8649022251310534, + "y": 5.584669337944561, + "heading": 0.0075685613575453385, + "angularVelocity": -0.19151607190373818, + "velocityX": 1.055705854463611, + "velocityY": -0.8925972092623536, + "timestamp": 10.296030607249863 + }, + { + "x": 1.9654334653384358, + "y": 5.548515796661377, + "heading": 4.814116720601663e-28, + "angularVelocity": -0.09629569997564517, + "velocityX": 1.2790708415858416, + "velocityY": -0.4599857753669162, + "timestamp": 10.374627692680049 + }, + { + "x": 2.0785037517547607, + "y": 5.548515796661377, + "heading": 4.711208229171849e-28, + "angularVelocity": -4.224112944688565e-28, + "velocityX": 1.43860660732464, + "velocityY": -1.1518576201030206e-26, + "timestamp": 10.453224778110235 + }, + { + "x": 2.2115594481070073, + "y": 5.548515796661377, + "heading": 9.919579688456685e-28, + "angularVelocity": -1.4478744000102224e-30, + "velocityX": 1.8779766683240853, + "velocityY": 6.343973751493874e-16, + "timestamp": 10.52407533690391 + }, + { + "x": 2.367207543861562, + "y": 5.548515796661377, + "heading": 1.5127951147748595e-27, + "angularVelocity": -1.4478743900360266e-30, + "velocityX": 2.196850644577389, + "velocityY": 3.1719689645397288e-15, + "timestamp": 10.594925895697587 + }, + { + "x": 2.4917260252771096, + "y": 5.548515796661377, + "heading": 2.033632260703789e-27, + "angularVelocity": -1.4478743937385766e-30, + "velocityX": 1.757480583578164, + "velocityY": 2.537576634676114e-15, + "timestamp": 10.665776454491263 + }, + { + "x": 2.585114887925518, + "y": 5.548515796661377, + "heading": 2.5544694066327203e-27, + "angularVelocity": -1.4478743937382235e-30, + "velocityX": 1.3181104600793105, + "velocityY": 1.903182958104165e-15, + "timestamp": 10.73662701328494 + }, + { + "x": 2.6473741303307423, + "y": 5.548515796661377, + "heading": 3.0753065525616534e-27, + "angularVelocity": -1.4478743937387e-30, + "velocityX": 0.8787403157472581, + "velocityY": 1.2687888330822125e-15, + "timestamp": 10.807477572078616 + }, + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": 3.596143698490385e-27, + "angularVelocity": -1.4478743965967547e-30, + "velocityX": 0.439370160998608, + "velocityY": 6.343944838152068e-16, + "timestamp": 10.878328130872292 + }, + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": 4.090371729515312e-27, + "angularVelocity": -3.7701463571595157e-28, + "velocityX": -1.25991204352499e-25, + "velocityY": -5.479637988962548e-26, + "timestamp": 10.949178689665969 + }, + { + "x": 2.6551487281624913, + "y": 5.535337362195577, + "heading": -0.005358266388786186, + "angularVelocity": -0.0814332978786503, + "velocityX": -0.3549425234125069, + "velocityY": -0.20028182653884274, + "timestamp": 11.014978141835657 + }, + { + "x": 2.6089441252483114, + "y": 5.508126235802502, + "heading": -0.016249929633628903, + "angularVelocity": -0.16552817517013077, + "velocityX": -0.7022034590048662, + "velocityY": -0.41354639736059473, + "timestamp": 11.080777594005346 + }, + { + "x": 2.5407349136089206, + "y": 5.4655968271259, + "heading": -0.03293816472196892, + "angularVelocity": -0.2536227056313986, + "velocityX": -1.036622789251932, + "velocityY": -0.6463489782091849, + "timestamp": 11.146577046175034 + }, + { + "x": 2.4521626111212957, + "y": 5.405634188324023, + "heading": -0.055847587039488555, + "angularVelocity": -0.3481704111827463, + "velocityX": -1.3460948315984265, + "velocityY": -0.9112938911290818, + "timestamp": 11.212376498344723 + }, + { + "x": 2.347299393995622, + "y": 5.324395116461008, + "heading": -0.08569447081932854, + "angularVelocity": -0.45360383400866283, + "velocityX": -1.5936791822406984, + "velocityY": -1.2346466297851597, + "timestamp": 11.278175950514411 + }, + { + "x": 2.239880929566337, + "y": 5.216483135728542, + "heading": -0.1227261468780359, + "angularVelocity": -0.5627961151288575, + "velocityX": -1.6325130512069794, + "velocityY": -1.6400133614209302, + "timestamp": 11.3439754026841 + }, + { + "x": 2.153374403298574, + "y": 5.091739159361965, + "heading": -0.1618030539678993, + "angularVelocity": -0.5938789123819666, + "velocityX": -1.3146997948352968, + "velocityY": -1.8958208959685574, + "timestamp": 11.409774854853788 + }, + { + "x": 2.0932393074035645, + "y": 4.9619622230529785, + "heading": -0.19963034769256285, + "angularVelocity": -0.5748876696892882, + "velocityX": -0.9139148414173566, + "velocityY": -1.9723102857195454, + "timestamp": 11.475574307023477 + }, + { + "x": 2.0629011107007105, + "y": 4.862373019847467, + "heading": -0.22719939375272252, + "angularVelocity": -0.5460591677053324, + "velocityX": -0.600907641312322, + "velocityY": -1.9725599970405534, + "timestamp": 11.52606159428213 + }, + { + "x": 2.048028599586168, + "y": 4.7660067703914, + "heading": -0.2526003668147138, + "angularVelocity": -0.5031162187791786, + "velocityX": -0.29457932723437075, + "velocityY": -1.908723060567117, + "timestamp": 11.576548881540782 + }, + { + "x": 2.047740775609064, + "y": 4.675697634358311, + "heading": -0.2752320074015959, + "angularVelocity": -0.44826414362367223, + "velocityX": -0.00570091983015821, + "velocityY": -1.788750018800283, + "timestamp": 11.627036168799435 + }, + { + "x": 2.0608710405337267, + "y": 4.5937072504707785, + "heading": -0.29463334748351117, + "angularVelocity": -0.3842816902108431, + "velocityX": 0.2600707155723077, + "velocityY": -1.62398077495204, + "timestamp": 11.677523456058088 + }, + { + "x": 2.0861920181883997, + "y": 4.5217424317614965, + "heading": -0.3104741156384499, + "angularVelocity": -0.31375756185482273, + "velocityX": 0.5015317524380102, + "velocityY": -1.4254047427939316, + "timestamp": 11.72801074331674 + }, + { + "x": 2.122546344803851, + "y": 4.461060494351438, + "heading": -0.3225286157246152, + "angularVelocity": -0.23876307761216561, + "velocityX": 0.7200689240680361, + "velocityY": -1.2019250925322578, + "timestamp": 11.778498030575394 + }, + { + "x": 2.1689002066225895, + "y": 4.412584630951134, + "heading": -0.3306480218329806, + "angularVelocity": -0.16082080359692716, + "velocityX": 0.9181293813878815, + "velocityY": -0.9601597953155385, + "timestamp": 11.828985317834046 + }, + { + "x": 2.2243532862123785, + "y": 4.376998478332531, + "heading": -0.3347371673945201, + "angularVelocity": -0.0809935685510361, + "velocityX": 1.098357281620145, + "velocityY": -0.7048537275590006, + "timestamp": 11.8794726050927 + }, + { + "x": 2.2881293296813965, + "y": 4.354815483093262, + "heading": -0.3347371673945201, + "angularVelocity": 5.296142385201319e-27, + "velocityX": 1.2632099471355256, + "velocityY": -0.43937784031895555, + "timestamp": 11.929959892351352 + }, + { + "x": 2.4097151173059284, + "y": 4.31443319431648, + "heading": -0.3347371673945201, + "angularVelocity": 4.260827770605265e-29, + "velocityX": 1.6917387541138205, + "velocityY": -0.5618772081689671, + "timestamp": 12.001830201717304 + }, + { + "x": 2.501147235462863, + "y": 4.284859003573599, + "heading": -0.3347371673945201, + "angularVelocity": 4.260827800538464e-29, + "velocityX": 1.2721820590944748, + "velocityY": -0.41149385613875655, + "timestamp": 12.073700511083256 + }, + { + "x": 2.562139216145648, + "y": 4.265257651838079, + "heading": -0.3347371673945201, + "angularVelocity": 4.260827801548558e-29, + "velocityX": 0.8486394621209058, + "velocityY": -0.27273225770760434, + "timestamp": 12.145570820449208 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": 1.3873453317425255e-27, + "velocityX": 0.4244517046157105, + "velocityY": -0.13595549514112237, + "timestamp": 12.21744112981516 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": -3.25532193935464e-28, + "velocityX": 2.1483187881556724e-26, + "velocityY": -1.7096603516619137e-26, + "timestamp": 12.289311439181112 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGF.1.traj b/src/main/deploy/choreo/SourceLanePHGF.1.traj new file mode 100644 index 00000000..15aee3f3 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePHGF.1.traj @@ -0,0 +1,230 @@ +{ + "samples": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": 5.0336126459058196e-26, + "angularVelocity": 1.4969503738616677e-26, + "velocityX": 6.632423953121246e-26, + "velocityY": 2.3403167387234696e-25, + "timestamp": 0 + }, + { + "x": 0.40051040035567387, + "y": 4.115348381940811, + "heading": -0.015277315995300885, + "angularVelocity": -0.30891158633599436, + "velocityX": 0.2731840597647725, + "velocityY": -0.11699249437857151, + "timestamp": 0.04945530265311636 + }, + { + "x": 0.42755206280004615, + "y": 4.103759116150659, + "heading": -0.04541480648334463, + "angularVelocity": -0.6093884552568736, + "velocityX": 0.5467899495842703, + "velocityY": -0.23433818354000846, + "timestamp": 0.09891060530623272 + }, + { + "x": 0.46814907636931, + "y": 4.086345867978605, + "heading": -0.08991129386238127, + "angularVelocity": -0.8997313734209399, + "velocityX": 0.8208829264278229, + "velocityY": -0.3521007301116199, + "timestamp": 0.14836590795934906 + }, + { + "x": 0.5223311730580029, + "y": 4.063083084813687, + "heading": -0.14811250055404074, + "angularVelocity": -1.1768446166407593, + "velocityX": 1.095577092485525, + "velocityY": -0.4703799575969681, + "timestamp": 0.19782121061246544 + }, + { + "x": 0.5901369333153729, + "y": 4.033939247984461, + "heading": -0.2191152852440613, + "angularVelocity": -1.4356960908325656, + "velocityX": 1.3710513659771775, + "velocityY": -0.5892965014013483, + "timestamp": 0.2472765132655818 + }, + { + "x": 0.6716171763309451, + "y": 3.9988769782710625, + "heading": -0.3016414776381226, + "angularVelocity": -1.6687026055205227, + "velocityX": 1.6475532176414214, + "velocityY": -0.7089688634468179, + "timestamp": 0.2967318159186982 + }, + { + "x": 0.766836765570499, + "y": 3.957853748642466, + "heading": -0.39387111054068624, + "angularVelocity": -1.864908876394308, + "velocityX": 1.9253666266574487, + "velocityY": -0.8295011339094811, + "timestamp": 0.34618711857181456 + }, + { + "x": 0.8758711660110932, + "y": 3.910821247404655, + "heading": -0.49317423957562295, + "angularVelocity": -2.007936939168226, + "velocityX": 2.2047059585373607, + "velocityY": -0.9510102802868479, + "timestamp": 0.39564242122493093 + }, + { + "x": 0.998787530743033, + "y": 3.857723681334368, + "heading": -0.5954683580210677, + "angularVelocity": -2.0684155784657574, + "velocityX": 2.4854031446149563, + "velocityY": -1.0736475811849282, + "timestamp": 0.4450977238780473 + }, + { + "x": 1.1355416361282653, + "y": 3.7985302060221593, + "heading": -0.6929936795382847, + "angularVelocity": -1.9719891757870285, + "velocityX": 2.7652061164085326, + "velocityY": -1.1969085646365696, + "timestamp": 0.4945530265311637 + }, + { + "x": 1.284369196894048, + "y": 3.7333344256591525, + "heading": -0.760822351237353, + "angularVelocity": -1.3715146417124207, + "velocityX": 3.009334748382224, + "velocityY": -1.3182768452615752, + "timestamp": 0.54400832918428 + }, + { + "x": 1.444068666194049, + "y": 3.662157825629303, + "heading": -0.7899998870158123, + "angularVelocity": -0.589977903544803, + "velocityX": 3.229167768320948, + "velocityY": -1.4392106854362665, + "timestamp": 0.5934636318373964 + }, + { + "x": 1.6133635999437081, + "y": 3.589517549083241, + "heading": -0.7899999435088091, + "angularVelocity": -0.0000011423041352586315, + "velocityX": 3.42319073319818, + "velocityY": -1.4688066324367013, + "timestamp": 0.6429189344905127 + }, + { + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "angularVelocity": -0.000001142267622119268, + "velocityX": 3.4231940186144136, + "velocityY": -1.4687989754737478, + "timestamp": 0.6923742371436291 + }, + { + "x": 2.023972333671043, + "y": 3.4134441767348855, + "heading": -0.79, + "angularVelocity": -1.7456316689050396e-17, + "velocityX": 3.4237473592821543, + "velocityY": -1.4675096226857507, + "timestamp": 0.7628565514540566 + }, + { + "x": 2.250495583067251, + "y": 3.3163502598603043, + "heading": -0.79, + "angularVelocity": -2.7198862157697797e-16, + "velocityX": 3.2139019782824447, + "velocityY": -1.37756425600538, + "timestamp": 0.8333388657644841 + }, + { + "x": 2.4487034476982825, + "y": 3.2313930734184306, + "heading": -0.79, + "angularVelocity": -2.2318862582403906e-16, + "velocityX": 2.8121645347520525, + "velocityY": -1.2053688542021173, + "timestamp": 0.9038211800749116 + }, + { + "x": 2.6185959113515174, + "y": 3.1585726243584236, + "heading": -0.79, + "angularVelocity": -1.6717248939922967e-16, + "velocityX": 2.410426861197717, + "velocityY": -1.0331733538044932, + "timestamp": 0.9743034943853391 + }, + { + "x": 2.760172968622745, + "y": 3.0978889149966715, + "heading": -0.79, + "angularVelocity": -1.1384117019167077e-16, + "velocityX": 2.0086891109686777, + "velocityY": -0.8609778205420586, + "timestamp": 1.0447858086957666 + }, + { + "x": 2.8734346168098592, + "y": 3.049341946491368, + "heading": -0.79, + "angularVelocity": -1.4459082642914294e-16, + "velocityX": 1.6069513224022822, + "velocityY": -0.6887822708472163, + "timestamp": 1.1152681230061943 + }, + { + "x": 2.958380854291598, + "y": 3.0129317195374297, + "heading": -0.79, + "angularVelocity": -1.2018162111442988e-16, + "velocityX": 1.2052135108334716, + "velocityY": -0.5165867112929291, + "timestamp": 1.1857504373166219 + }, + { + "x": 3.015011679987118, + "y": 2.9886582345981343, + "heading": -0.79, + "angularVelocity": -6.358045799066146e-17, + "velocityX": 0.8034756839297176, + "velocityY": -0.3443911451656786, + "timestamp": 1.2562327516270495 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -1.168085544952961e-16, + "velocityX": 0.40173784607243274, + "velocityY": -0.17219557434345445, + "timestamp": 1.3267150659374771 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 1.4132283846579156e-28, + "velocityX": -7.2110997365083e-27, + "velocityY": -2.0111921052200978e-27, + "timestamp": 1.3971973802479047 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGF.2.traj b/src/main/deploy/choreo/SourceLanePHGF.2.traj new file mode 100644 index 00000000..9b5629a3 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePHGF.2.traj @@ -0,0 +1,635 @@ +{ + "samples": [ + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 1.4132283846579156e-28, + "velocityX": -7.2110997365083e-27, + "velocityY": -2.0111921052200978e-27, + "timestamp": 0 + }, + { + "x": 3.065434104608151, + "y": 2.9635744703134126, + "heading": -0.7762472311002773, + "angularVelocity": 0.2130317791406229, + "velocityX": 0.34243984045734815, + "velocityY": -0.2005506735052959, + "timestamp": 0.06455735832091292 + }, + { + "x": 3.109663922209824, + "y": 2.9376745027807694, + "heading": -0.7492642309726533, + "angularVelocity": 0.41796939697396296, + "velocityX": 0.6851243413927804, + "velocityY": -0.40119311270289487, + "timestamp": 0.12911471664182583 + }, + { + "x": 3.176036274163731, + "y": 2.8988141772022082, + "heading": -0.7097339632553362, + "angularVelocity": 0.612327839079368, + "velocityX": 1.028114434670201, + "velocityY": -0.6019503676929793, + "timestamp": 0.19367207496273875 + }, + { + "x": 3.2645764628811422, + "y": 2.8469841623353034, + "heading": -0.6585853448584378, + "angularVelocity": 0.7922972644363798, + "velocityX": 1.3714964648534713, + "velocityY": -0.8028521645706009, + "timestamp": 0.25822943328365167 + }, + { + "x": 3.3753176768637694, + "y": 2.782172830859228, + "heading": -0.5971442411061407, + "angularVelocity": 0.9517289020234527, + "velocityX": 1.7153925882799501, + "velocityY": -1.0039340698220625, + "timestamp": 0.3227867916045646 + }, + { + "x": 3.5083041287580397, + "y": 2.7043667248342107, + "heading": -0.5274363257086371, + "angularVelocity": 1.0797826492680733, + "velocityX": 2.0599735700645976, + "velocityY": -1.2052244399196936, + "timestamp": 0.3873441499254775 + }, + { + "x": 3.663593064192348, + "y": 2.6135556042232486, + "heading": -0.45293092753819847, + "angularVelocity": 1.1540961419157545, + "velocityX": 2.405441292414274, + "velocityY": -1.4066734292246417, + "timestamp": 0.4519015082463904 + }, + { + "x": 3.8412236037881105, + "y": 2.50976866496977, + "heading": -0.3811103115245424, + "angularVelocity": 1.1125085951726474, + "velocityX": 2.751514997140456, + "velocityY": -1.6076701704173844, + "timestamp": 0.5164588665673033 + }, + { + "x": 4.040074211349249, + "y": 2.394026373110605, + "heading": -0.3455558271366132, + "angularVelocity": 0.5507425537951661, + "velocityX": 3.0802159929261363, + "velocityY": -1.7928597896433947, + "timestamp": 0.5810162248882162 + }, + { + "x": 4.2474603616320605, + "y": 2.2722898239651945, + "heading": -0.3455557994031658, + "angularVelocity": 4.2959390119270946e-7, + "velocityX": 3.2124324116842065, + "velocityY": -1.8857114403637907, + "timestamp": 0.6455735832091294 + }, + { + "x": 4.454846495549661, + "y": 2.150553246940426, + "heading": -0.34555577166993406, + "angularVelocity": 4.2959055996150303e-7, + "velocityX": 3.21243215818542, + "velocityY": -1.8857118722178698, + "timestamp": 0.7101309415300425 + }, + { + "x": 4.662232629466809, + "y": 2.0288166699148875, + "heading": -0.34555574393670246, + "angularVelocity": 4.295905587004531e-7, + "velocityX": 3.2124321581784225, + "velocityY": -1.885711872229791, + "timestamp": 0.7746882998509557 + }, + { + "x": 4.86961876338396, + "y": 1.9070800928893532, + "heading": -0.34555571620347075, + "angularVelocity": 4.295905595925436e-7, + "velocityX": 3.2124321581784563, + "velocityY": -1.8857118722297332, + "timestamp": 0.8392456581718688 + }, + { + "x": 5.0770048973810855, + "y": 1.7853435160000617, + "heading": -0.3455556884702391, + "angularVelocity": 4.2959055900802054e-7, + "velocityX": 3.2124321594172818, + "velocityY": -1.8857118701193136, + "timestamp": 0.9038030164927819 + }, + { + "x": 5.284393928450512, + "y": 1.6636118745954684, + "heading": -0.3455556607369369, + "angularVelocity": 4.295916530112558e-7, + "velocityX": 3.212477035359784, + "velocityY": -1.885635418962908, + "timestamp": 0.9683603748136951 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 4.37946601168607e-7, + "velocityX": 3.386285847111441, + "velocityY": -1.5519979873442957, + "timestamp": 1.0329177331346082 + }, + { + "x": 5.67114193645051, + "y": 1.502092704427615, + "heading": -0.3455556386074096, + "angularVelocity": -1.278584041845331e-7, + "velocityX": 3.49949220171166, + "velocityY": -1.2763944237939575, + "timestamp": 1.0809642294544883 + }, + { + "x": 5.843643430464205, + "y": 1.4543994299893364, + "heading": -0.3455556446255623, + "angularVelocity": -1.252568492070864e-7, + "velocityX": 3.590303294234579, + "velocityY": -0.9926483321645406, + "timestamp": 1.1290107257743685 + }, + { + "x": 6.017301477098486, + "y": 1.4111062813327742, + "heading": -0.3455556506300644, + "angularVelocity": -1.2497273485529655e-7, + "velocityX": 3.6143748230487924, + "velocityY": -0.9010677567065171, + "timestamp": 1.1770572220942486 + }, + { + "x": 6.190959602710417, + "y": 1.3678134494736272, + "heading": -0.3455556566345664, + "angularVelocity": -1.249727336927334e-7, + "velocityX": 3.6143764668242215, + "velocityY": -0.9010611631473742, + "timestamp": 1.2251037184141287 + }, + { + "x": 6.364617728327444, + "y": 1.32452061763492, + "heading": -0.34555566263906834, + "angularVelocity": -1.2497273274023636e-7, + "velocityX": 3.6143764669302776, + "velocityY": -0.9010611627219542, + "timestamp": 1.2731502147340088 + }, + { + "x": 6.53827585394447, + "y": 1.2812277857962142, + "heading": -0.3455556686435703, + "angularVelocity": -1.2497273262525817e-7, + "velocityX": 3.6143764669302847, + "velocityY": -0.9010611627219266, + "timestamp": 1.321196711053889 + }, + { + "x": 6.711933979561497, + "y": 1.2379349539575075, + "heading": -0.3455556746480723, + "angularVelocity": -1.249727344246102e-7, + "velocityX": 3.614376466930281, + "velocityY": -0.9010611627219414, + "timestamp": 1.369243207373769 + }, + { + "x": 6.885592105175778, + "y": 1.1946421221077876, + "heading": -0.3455556806525743, + "angularVelocity": -1.2497273402138388e-7, + "velocityX": 3.6143764668731366, + "velocityY": -0.9010611629511635, + "timestamp": 1.4172897036936491 + }, + { + "x": 7.059250188235952, + "y": 1.1513491195635046, + "heading": -0.34555568665707626, + "angularVelocity": -1.2497273336829244e-7, + "velocityX": 3.614375581187171, + "velocityY": -0.901064715646495, + "timestamp": 1.4653362000135293 + }, + { + "x": 7.232252247020901, + "y": 1.105504804591487, + "heading": -0.3455556930363148, + "angularVelocity": -1.3277218955785758e-7, + "velocityX": 3.600721635000194, + "velocityY": -0.954165620460628, + "timestamp": 1.5133826963334094 + }, + { + "x": 7.3915823056650956, + "y": 1.0554297346464303, + "heading": -0.34668228326838396, + "angularVelocity": -0.02344791646343344, + "velocityX": 3.3161639421825697, + "velocityY": -1.0422210521173303, + "timestamp": 1.5614291926532895 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.023449177160012234, + "velocityX": 3.0262995808418314, + "velocityY": -0.9742709781335815, + "timestamp": 1.6094756889731696 + }, + { + "x": 7.706721170015621, + "y": 0.9519776263380658, + "heading": -0.3399354575219549, + "angularVelocity": 0.0873776196257354, + "velocityX": 2.6389050045733367, + "velocityY": -0.8806177429462266, + "timestamp": 1.6737962205506969 + }, + { + "x": 7.851727537144501, + "y": 0.9021680166584882, + "heading": -0.3343317435174853, + "angularVelocity": 0.0871216991998166, + "velocityX": 2.254433593324705, + "velocityY": -0.7743967355049113, + "timestamp": 1.7381167521282241 + }, + { + "x": 7.9720996028837305, + "y": 0.8595135021783036, + "heading": -0.3304150780634631, + "angularVelocity": 0.06089292731204182, + "velocityX": 1.8714407792812162, + "velocityY": -0.6631555031346718, + "timestamp": 1.8024372837057514 + }, + { + "x": 8.067893377913451, + "y": 0.8241876261341342, + "heading": -0.3290917427033041, + "angularVelocity": 0.020574073747570358, + "velocityX": 1.4893187708540474, + "velocityY": -0.5492161706031644, + "timestamp": 1.8667578152832787 + }, + { + "x": 8.139145592381213, + "y": 0.7962986910106811, + "heading": -0.33093102223192583, + "angularVelocity": -0.028595527485726017, + "velocityX": 1.107767810996388, + "velocityY": -0.4335930447004768, + "timestamp": 1.931078346860806 + }, + { + "x": 8.185882143295247, + "y": 0.7759207035915239, + "heading": -0.33632389468863655, + "angularVelocity": -0.08384371715290594, + "velocityX": 0.726619475426766, + "velocityY": -0.3168193253284159, + "timestamp": 1.9953988784383332 + }, + { + "x": 8.208122253417969, + "y": 0.763107419013977, + "heading": -0.34555563246426124, + "angularVelocity": -0.14352707524653271, + "velocityX": 0.34576999874318054, + "velocityY": -0.19920986756931033, + "timestamp": 2.0597194100158607 + }, + { + "x": 8.204294589663224, + "y": 0.758059174501978, + "heading": -0.3598356424553756, + "angularVelocity": -0.21015280737857786, + "velocityX": -0.056330092504229386, + "velocityY": -0.07429285814157252, + "timestamp": 2.127670013052011 + }, + { + "x": 8.173143956099947, + "y": 0.7614990375664682, + "heading": -0.3786432717750789, + "angularVelocity": -0.2767838470792884, + "velocityX": -0.4584305682571252, + "velocityY": 0.05062299539358275, + "timestamp": 2.1956206160881617 + }, + { + "x": 8.114670323431806, + "y": 0.7734269104252286, + "heading": -0.4019794984677993, + "angularVelocity": -0.34342928024207936, + "velocityX": -0.8605314751516402, + "velocityY": 0.17553740990958688, + "timestamp": 2.2635712191243123 + }, + { + "x": 8.028873660690506, + "y": 0.7938426763627194, + "heading": -0.42984618674257646, + "angularVelocity": -0.41010214817301655, + "velocityX": -1.2626328377933065, + "velocityY": 0.3004501067728478, + "timestamp": 2.3315218221604628 + }, + { + "x": 7.915753937180706, + "y": 0.8227462003684911, + "heading": -0.46224640751329055, + "angularVelocity": -0.47682020943179554, + "velocityX": -1.6647346521651787, + "velocityY": 0.4253608167449919, + "timestamp": 2.3994724251966133 + }, + { + "x": 7.7753111250087, + "y": 0.8601373303576515, + "heading": -0.4991848061620223, + "angularVelocity": -0.5436066347943985, + "velocityX": -2.0668368770368284, + "velocityY": 0.5502692885487409, + "timestamp": 2.467423028232764 + }, + { + "x": 7.607545202545143, + "y": 0.9060158995198092, + "heading": -0.5406680081845634, + "angularVelocity": -0.6104905647485055, + "velocityX": -2.468939420218282, + "velocityY": 0.6751753054752103, + "timestamp": 2.5353736312689144 + }, + { + "x": 7.412456160128696, + "y": 0.9603817316039739, + "heading": -0.5867050376005867, + "angularVelocity": -0.6775072973455604, + "velocityX": -2.8710421055815774, + "velocityY": 0.800078728591143, + "timestamp": 2.603324234305065 + }, + { + "x": 7.190044017003328, + "y": 1.023234660815804, + "heading": -0.6373076187752672, + "angularVelocity": -0.7446965724168686, + "velocityX": -3.273144507739573, + "velocityY": 0.9249797117825663, + "timestamp": 2.6712748373412154 + }, + { + "x": 6.9483240964505395, + "y": 1.0983293400926897, + "heading": -0.6373076247970214, + "angularVelocity": -8.861958477756183e-8, + "velocityX": -3.5572888208834765, + "velocityY": 1.1051363184655567, + "timestamp": 2.739225440377366 + }, + { + "x": 6.70660438555466, + "y": 1.1734246942257325, + "heading": -0.6373076308180073, + "angularVelocity": -8.86082771208124e-8, + "velocityX": -3.5572857354522953, + "velocityY": 1.1051462500353575, + "timestamp": 2.8071760434135165 + }, + { + "x": 6.464884674663689, + "y": 1.2485200483745762, + "heading": -0.6373076368389932, + "angularVelocity": -8.860827701986505e-8, + "velocityX": -3.557285735380054, + "velocityY": 1.105146250267891, + "timestamp": 2.875126646449667 + }, + { + "x": 6.223164963772718, + "y": 1.3236154025234206, + "heading": -0.637307642859979, + "angularVelocity": -8.860827626589147e-8, + "velocityX": -3.5572857353800513, + "velocityY": 1.1051462502679001, + "timestamp": 2.9430772494858175 + }, + { + "x": 5.98144525288525, + "y": 1.39871075668354, + "heading": -0.6373076488809649, + "angularVelocity": -8.860827719404325e-8, + "velocityX": -3.5572857353285006, + "velocityY": 1.1051462504338314, + "timestamp": 3.011027852521968 + }, + { + "x": 5.739725691606158, + "y": 1.473806592406946, + "heading": -0.6373076549019507, + "angularVelocity": -8.860827643379068e-8, + "velocityX": -3.557283533605943, + "velocityY": 1.1051533373950213, + "timestamp": 3.0789784555581186 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.6373076609624853, + "angularVelocity": -8.919029793694062e-8, + "velocityX": -3.483737947413037, + "velocityY": 1.3187872955082738, + "timestamp": 3.146929058594269 + }, + { + "x": 5.300661081332848, + "y": 1.6644259130191759, + "heading": -0.6373076662501911, + "angularVelocity": -8.709485563097327e-8, + "velocityX": -3.332823841916901, + "velocityY": 1.6637052194159954, + "timestamp": 3.2076410930504 + }, + { + "x": 5.105140298152244, + "y": 1.7780767678650242, + "heading": -0.6373076714738495, + "angularVelocity": -8.603991577037157e-8, + "velocityX": -3.2204617244688527, + "velocityY": 1.8719658443989302, + "timestamp": 3.268353127506531 + }, + { + "x": 4.909619881391782, + "y": 1.8917282530843815, + "heading": -0.6373076766975064, + "angularVelocity": -8.60398930609077e-8, + "velocityX": -3.220455689089781, + "velocityY": 1.8719762274064293, + "timestamp": 3.329065161962662 + }, + { + "x": 4.714099464642575, + "y": 2.0053797383231027, + "heading": -0.6373076819211634, + "angularVelocity": -8.603989205375444e-8, + "velocityX": -3.2204556889043876, + "velocityY": 1.8719762277253713, + "timestamp": 3.389777196418793 + }, + { + "x": 4.518579047893369, + "y": 2.1190312235618243, + "heading": -0.6373076871448202, + "angularVelocity": -8.60398922147823e-8, + "velocityX": -3.220455688904382, + "velocityY": 1.8719762277253813, + "timestamp": 3.4504892308749238 + }, + { + "x": 4.323058631144163, + "y": 2.232682708800546, + "heading": -0.6373076923684772, + "angularVelocity": -8.603989163870152e-8, + "velocityX": -3.2204556889043805, + "velocityY": 1.8719762277253815, + "timestamp": 3.5112012653310547 + }, + { + "x": 4.12753821439536, + "y": 2.3463341940399602, + "heading": -0.637307697592134, + "angularVelocity": -8.60398917504913e-8, + "velocityX": -3.2204556888977502, + "velocityY": 1.8719762277367886, + "timestamp": 3.5719132997871856 + }, + { + "x": 3.9320178107531194, + "y": 2.4599857018249596, + "heading": -0.637307702817472, + "angularVelocity": -8.606758041499758e-8, + "velocityX": -3.2204554730169304, + "velocityY": 1.8719765990896107, + "timestamp": 3.6326253342433166 + }, + { + "x": 3.754279851907353, + "y": 2.5632932295685515, + "heading": -0.6678287625734391, + "angularVelocity": -0.5027184483172089, + "velocityX": -2.9275572864255675, + "velocityY": 1.7015988455837243, + "timestamp": 3.6933373686994475 + }, + { + "x": 3.5962904713608705, + "y": 2.6551219777359605, + "heading": -0.6949668036285394, + "angularVelocity": -0.4469960741425893, + "velocityX": -2.6022745236884077, + "velocityY": 1.5125295831382852, + "timestamp": 3.7540494031555784 + }, + { + "x": 3.4580497056890147, + "y": 2.735472006369499, + "heading": -0.7187186173095553, + "angularVelocity": -0.3912208492729469, + "velocityX": -2.276991158511501, + "velocityY": 1.3234613096617127, + "timestamp": 3.8147614376117094 + }, + { + "x": 3.3395575797280896, + "y": 2.804343370444312, + "heading": -0.7390815132457978, + "angularVelocity": -0.33540131077235125, + "velocityX": -1.9517073842508859, + "velocityY": 1.1343939416917022, + "timestamp": 3.8754734720678403 + }, + { + "x": 3.2408141126366585, + "y": 2.861736116018487, + "heading": -0.7560532042537318, + "angularVelocity": -0.2795440996166463, + "velocityX": -1.6264232944257633, + "velocityY": 0.9453273323536148, + "timestamp": 3.9361855065239713 + }, + { + "x": 3.16181931959339, + "y": 2.907650279470937, + "heading": -0.7696318707216167, + "angularVelocity": -0.22365691727389694, + "velocityX": -1.3011389545897718, + "velocityY": 0.756261322219841, + "timestamp": 3.996897540980102 + }, + { + "x": 3.1025732126750043, + "y": 2.942085887277494, + "heading": -0.7798162350397829, + "angularVelocity": -0.16774869116806188, + "velocityX": -0.9758544158357173, + "velocityY": 0.5671957481747656, + "timestamp": 4.057609575436233 + }, + { + "x": 3.0630758014096395, + "y": 2.9650429559458633, + "heading": -0.7866056220675054, + "angularVelocity": -0.11182934468500333, + "velocityX": -0.6505697201418033, + "velocityY": 0.37813044603137397, + "timestamp": 4.118321609892364 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -0.05590947433901855, + "velocityX": -0.3252849037618716, + "velocityY": 0.18906525141774544, + "timestamp": 4.179033644348495 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 6.3035161532873835e-28, + "velocityX": -3.172311091955017e-27, + "velocityY": -4.980348575504513e-27, + "timestamp": 4.239745678804626 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGF.3.traj b/src/main/deploy/choreo/SourceLanePHGF.3.traj new file mode 100644 index 00000000..7799c807 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePHGF.3.traj @@ -0,0 +1,680 @@ +{ + "samples": [ + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 6.3035161532873835e-28, + "velocityX": -3.172311091955017e-27, + "velocityY": -4.980348575504513e-27, + "timestamp": 0 + }, + { + "x": 3.0599792847904204, + "y": 2.966059333110192, + "heading": -0.7574164237892916, + "angularVelocity": 0.5582463314772692, + "velocityX": 0.2852978705745262, + "velocityY": -0.17924557403559396, + "timestamp": 0.0583677390668802 + }, + { + "x": 3.0934694098196505, + "y": 2.9451270864719556, + "heading": -0.6941025427131003, + "angularVelocity": 1.0847410245519962, + "velocityX": 0.5737780075883294, + "velocityY": -0.35862699108922125, + "timestamp": 0.1167354781337604 + }, + { + "x": 3.1440142929618213, + "y": 2.9136974289532884, + "heading": -0.6025701359612278, + "angularVelocity": 1.5682020276130737, + "velocityX": 0.8659729492734756, + "velocityY": -0.5384765286634409, + "timestamp": 0.1751032172006406 + }, + { + "x": 3.2118603630472147, + "y": 2.8716921302721974, + "heading": -0.4864204343113424, + "angularVelocity": 1.9899640367566072, + "velocityX": 1.162389895000939, + "velocityY": -0.7196663662603013, + "timestamp": 0.2334709562675208 + }, + { + "x": 3.297261323571818, + "y": 2.8189697938755005, + "heading": -0.35051552340624004, + "angularVelocity": 2.3284251382322116, + "velocityX": 1.463153479814372, + "velocityY": -0.9032787159407621, + "timestamp": 0.291838695334401 + }, + { + "x": 3.400457830317495, + "y": 2.755367474678837, + "heading": -0.2010210241745889, + "angularVelocity": 2.561252185224339, + "velocityX": 1.768040160463131, + "velocityY": -1.0896827633461208, + "timestamp": 0.3502064344012812 + }, + { + "x": 3.521660389826238, + "y": 2.6807799872839175, + "heading": -0.04666004939322163, + "angularVelocity": 2.644628304078967, + "velocityX": 2.076533397496587, + "velocityY": -1.277888926097578, + "timestamp": 0.4085741734681614 + }, + { + "x": 3.6607588635338297, + "y": 2.5953087461465243, + "heading": 0.09647781625085058, + "angularVelocity": 2.4523455582211025, + "velocityX": 2.383139657820331, + "velocityY": -1.4643575801258442, + "timestamp": 0.4669419125350416 + }, + { + "x": 3.8159145666935697, + "y": 2.50056098976987, + "heading": 0.19948059626334666, + "angularVelocity": 1.7647210883819062, + "velocityX": 2.6582441883170422, + "velocityY": -1.6232898154250606, + "timestamp": 0.5253096516019218 + }, + { + "x": 3.984110357979779, + "y": 2.3943443694149815, + "heading": 0.2530307730268392, + "angularVelocity": 0.9174618996657108, + "velocityX": 2.8816567846406262, + "velocityY": -1.8197830180329746, + "timestamp": 0.583677390668802 + }, + { + "x": 4.165386797993929, + "y": 2.276764205392624, + "heading": 0.25694414020151846, + "angularVelocity": 0.06704674940715359, + "velocityX": 3.105764295691424, + "velocityY": -2.0144717938728, + "timestamp": 0.6420451297356822 + }, + { + "x": 4.35049005488602, + "y": 2.162710813518879, + "heading": 0.2569442210228932, + "angularVelocity": 0.0000013846925722227273, + "velocityX": 3.171328200326413, + "velocityY": -1.9540484811833736, + "timestamp": 0.7004128688025624 + }, + { + "x": 4.5355934176880135, + "y": 2.0486575935320284, + "heading": 0.2569443018441972, + "angularVelocity": 0.0000013846913602153038, + "velocityX": 3.171330014854514, + "velocityY": -1.9540455362878368, + "timestamp": 0.7587806078694426 + }, + { + "x": 4.72069678050449, + "y": 1.9346043735686815, + "heading": 0.25694438266550157, + "angularVelocity": 0.0000013846913664886619, + "velocityX": 3.171330015102631, + "velocityY": -1.9540455358851527, + "timestamp": 0.8171483469363228 + }, + { + "x": 4.905800420839664, + "y": 1.8205516040072125, + "heading": 0.2569444634867951, + "angularVelocity": 0.0000013846911811343512, + "velocityX": 3.171334769761663, + "velocityY": -1.9540378192614634, + "timestamp": 0.875516086003203 + }, + { + "x": 5.096209165426906, + "y": 1.7155949376896535, + "heading": 0.2569445446080598, + "angularVelocity": 0.0000013898305123819847, + "velocityX": 3.262225805407059, + "velocityY": -1.7981965379418772, + "timestamp": 0.9338838250700832 + }, + { + "x": 5.295905298565615, + "y": 1.6296132789604225, + "heading": 0.2569446291955456, + "angularVelocity": 0.000001449216418667715, + "velocityX": 3.4213443304680435, + "velocityY": -1.473102438158684, + "timestamp": 0.9922515641369634 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.256944722079518, + "angularVelocity": 0.0000015913580667498199, + "velocityX": 3.548163797412206, + "velocityY": -1.1340904342967109, + "timestamp": 1.0506193032038436 + }, + { + "x": 5.694707875531734, + "y": 1.520245371460122, + "heading": 0.25694480525285635, + "angularVelocity": 0.0000015766499744480022, + "velocityX": 3.6339835774201763, + "velocityY": -0.8184074026058323, + "timestamp": 1.1033725075560508 + }, + { + "x": 5.889460824583175, + "y": 1.4940579532692961, + "heading": 0.2569448839753191, + "angularVelocity": 0.0000014922783134293427, + "velocityX": 3.691774773550607, + "velocityY": -0.49641379158667054, + "timestamp": 1.156125711908258 + }, + { + "x": 6.085760337593024, + "y": 1.4850583071038423, + "heading": 0.25694496145000295, + "angularVelocity": 0.0000014686251728049365, + "velocityX": 3.721091740688451, + "velocityY": -0.17059904276842675, + "timestamp": 1.2088789162604652 + }, + { + "x": 6.282233415675307, + "y": 1.4886391007137698, + "heading": 0.2569450396472057, + "angularVelocity": 0.0000014823213810882118, + "velocityX": 3.7243818739526797, + "velocityY": 0.06787821998490172, + "timestamp": 1.2616321206126724 + }, + { + "x": 6.478706465105997, + "y": 1.4922214660505555, + "heading": 0.2569451178445693, + "angularVelocity": 0.000001482324430715833, + "velocityX": 3.7243813308274873, + "velocityY": 0.06790801394485714, + "timestamp": 1.3143853249648796 + }, + { + "x": 6.6750148942106335, + "y": 1.5010244818843792, + "heading": 0.2569451969790633, + "angularVelocity": 0.0000015000888565600604, + "velocityX": 3.7212607559150324, + "velocityY": 0.16687167996564178, + "timestamp": 1.3671385293170868 + }, + { + "x": 6.869794082781354, + "y": 1.5270160132207926, + "heading": 0.25694528055290106, + "angularVelocity": 0.000001584241920357872, + "velocityX": 3.692272174980594, + "velocityY": 0.4927005222826037, + "timestamp": 1.419891733669294 + }, + { + "x": 7.061541469504156, + "y": 1.5699966670376497, + "heading": 0.25694631305508775, + "angularVelocity": 0.000019572312228907657, + "velocityX": 3.6348007495923222, + "velocityY": 0.814749631698122, + "timestamp": 1.4726449380215012 + }, + { + "x": 7.242025004547745, + "y": 1.623544327324134, + "heading": 0.2892009263238574, + "angularVelocity": 0.611424721300743, + "velocityX": 3.421280986811508, + "velocityY": 1.0150598611787165, + "timestamp": 1.5253981423737084 + }, + { + "x": 7.4101475255884415, + "y": 1.6859098332100053, + "heading": 0.3552181316610267, + "angularVelocity": 1.2514349819662982, + "velocityX": 3.186963201670605, + "velocityY": 1.1822126570641514, + "timestamp": 1.5781513467259156 + }, + { + "x": 7.5650279020019084, + "y": 1.7562974315324296, + "heading": 0.45280543116466554, + "angularVelocity": 1.8498838260534038, + "velocityX": 2.935942533071662, + "velocityY": 1.3342810012540722, + "timestamp": 1.6309045510781228 + }, + { + "x": 7.70378035758304, + "y": 1.8320280138991303, + "heading": 0.5628377984659659, + "angularVelocity": 2.0857949512728657, + "velocityX": 2.6302185295655014, + "velocityY": 1.4355636457850884, + "timestamp": 1.68365775543033 + }, + { + "x": 7.825662907370156, + "y": 1.911322826381737, + "heading": 0.6698617562914626, + "angularVelocity": 2.0287669562392776, + "velocityX": 2.3104293148405795, + "velocityY": 1.5031278849564027, + "timestamp": 1.7364109597825372 + }, + { + "x": 7.930693514616294, + "y": 1.9932244953323592, + "heading": 0.7659443680912281, + "angularVelocity": 1.8213606733397365, + "velocityX": 1.9909806150333516, + "velocityY": 1.5525439630890379, + "timestamp": 1.7891641641347444 + }, + { + "x": 8.019011714129077, + "y": 2.0771819587981266, + "heading": 0.8464048765673222, + "angularVelocity": 1.5252250448882378, + "velocityX": 1.6741769641730917, + "velocityY": 1.5915140036844804, + "timestamp": 1.8419173684869516 + }, + { + "x": 8.090768492192002, + "y": 2.1628615391483916, + "heading": 0.9080474489075738, + "angularVelocity": 1.1685085881929373, + "velocityX": 1.360235438663432, + "velocityY": 1.6241587862269713, + "timestamp": 1.8946705728391589 + }, + { + "x": 8.146101418716428, + "y": 2.2500495157538962, + "heading": 0.9485346126333066, + "angularVelocity": 0.7674825486508784, + "velocityX": 1.048901715145006, + "velocityY": 1.6527522389615, + "timestamp": 1.947423777191366 + }, + { + "x": 8.185122291410421, + "y": 2.338598291571523, + "heading": 0.9662103754330295, + "angularVelocity": 0.3350651968307046, + "velocityX": 0.7396872507207403, + "velocityY": 1.6785478134452219, + "timestamp": 2.0001769815435733 + }, + { + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "angularVelocity": -0.11638132008356272, + "velocityX": 0.4319241942959219, + "velocityY": 1.7021818924733116, + "timestamp": 2.0529301858957805 + }, + { + "x": 8.211423054828662, + "y": 2.540472392253658, + "heading": 0.9162466453352144, + "angularVelocity": -0.6758725051077806, + "velocityX": 0.054215367725083355, + "velocityY": 1.7285138763859442, + "timestamp": 2.117771176179412 + }, + { + "x": 8.190202888383633, + "y": 2.6538474230997973, + "heading": 0.8391734249667565, + "angularVelocity": -1.1886496494165113, + "velocityX": -0.3272646878495706, + "velocityY": 1.7485086262595175, + "timestamp": 2.182612166463043 + }, + { + "x": 8.144008306424501, + "y": 2.768019013990823, + "heading": 0.7319233890258415, + "angularVelocity": -1.6540468532601917, + "velocityX": -0.7124286929774685, + "velocityY": 1.760793448582603, + "timestamp": 2.2474531567466745 + }, + { + "x": 8.072604152106418, + "y": 2.8824539716664033, + "heading": 0.5977397407756817, + "angularVelocity": -2.0694262635904495, + "velocityX": -1.1012193676521893, + "velocityY": 1.764855181498803, + "timestamp": 2.312294147030306 + }, + { + "x": 7.975691382967155, + "y": 2.9965380091631473, + "heading": 0.44171220804424666, + "angularVelocity": -2.406310145001336, + "velocityX": -1.4946219777850651, + "velocityY": 1.7594431700951976, + "timestamp": 2.377135137313937 + }, + { + "x": 7.852920805151493, + "y": 3.1093104190449847, + "heading": 0.2733111632783997, + "angularVelocity": -2.5971386931201565, + "velocityX": -1.8934099753663647, + "velocityY": 1.7392147989804203, + "timestamp": 2.4419761275975684 + }, + { + "x": 7.704278478364254, + "y": 3.219028342207882, + "heading": 0.10912005253192102, + "angularVelocity": -2.5322116461865276, + "velocityX": -2.292412964963042, + "velocityY": 1.6921074567640455, + "timestamp": 2.5068171178811998 + }, + { + "x": 7.532421373794059, + "y": 3.323057914868166, + "heading": -0.010926874391562237, + "angularVelocity": -1.851404896784686, + "velocityX": -2.650439233245006, + "velocityY": 1.6043797635605452, + "timestamp": 2.571658108164831 + }, + { + "x": 7.337707687800787, + "y": 3.422034456346363, + "heading": -0.08211008235538154, + "angularVelocity": -1.0978118571669786, + "velocityX": -3.0029412743627657, + "velocityY": 1.5264501829051038, + "timestamp": 2.6364990984484624 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": -0.09725138680843892, + "angularVelocity": -0.2335143924672555, + "velocityX": -3.3462025777705335, + "velocityY": 1.4627042932775656, + "timestamp": 2.7013400887320937 + }, + { + "x": 7.010606209661033, + "y": 3.566708155420985, + "heading": -0.09725146950362613, + "angularVelocity": -0.000002548325484883459, + "velocityX": -3.39376555274088, + "velocityY": 1.5355711508400656, + "timestamp": 2.733790883460248 + }, + { + "x": 6.900475999605217, + "y": 3.616539055790813, + "heading": -0.09725155219704654, + "angularVelocity": -0.0000025482710392560423, + "velocityX": -3.3937600289421765, + "velocityY": 1.5355833589676455, + "timestamp": 2.7662416781884023 + }, + { + "x": 6.790345789587328, + "y": 3.6663699562444614, + "heading": -0.09725163489046874, + "angularVelocity": -0.0000025482710945290222, + "velocityX": -3.3937600277734288, + "velocityY": 1.5355833615506584, + "timestamp": 2.7986924729165565 + }, + { + "x": 6.680215579569448, + "y": 3.7162008566981277, + "heading": -0.0972517175838927, + "angularVelocity": -0.0000025482711487845157, + "velocityX": -3.393760027773178, + "velocityY": 1.5355833615512018, + "timestamp": 2.831143267644711 + }, + { + "x": 6.570085369551568, + "y": 3.7660317571517936, + "heading": -0.09725180027731849, + "angularVelocity": -0.0000025482712051303815, + "velocityX": -3.3937600277731743, + "velocityY": 1.5355833615511987, + "timestamp": 2.863594062372865 + }, + { + "x": 6.4599551595336875, + "y": 3.8158626576054595, + "heading": -0.09725188297074605, + "angularVelocity": -0.0000025482712597994537, + "velocityX": -3.3937600277731703, + "velocityY": 1.5355833615511956, + "timestamp": 2.8960448571010193 + }, + { + "x": 6.349824949515808, + "y": 3.8656935580591254, + "heading": -0.0972519656641754, + "angularVelocity": -0.0000025482713151989977, + "velocityX": -3.393760027773166, + "velocityY": 1.5355833615511918, + "timestamp": 2.9284956518291736 + }, + { + "x": 6.239694739497929, + "y": 3.9155244585127913, + "heading": -0.09725204835760656, + "angularVelocity": -0.00000254827137059863, + "velocityX": -3.3937600277731623, + "velocityY": 1.5355833615511885, + "timestamp": 2.960946446557328 + }, + { + "x": 6.129564529480049, + "y": 3.9653553589664567, + "heading": -0.09725213105103951, + "angularVelocity": -0.000002548271425616177, + "velocityX": -3.3937600277731588, + "velocityY": 1.535583361551185, + "timestamp": 2.993397241285482 + }, + { + "x": 6.019434319462169, + "y": 4.015186259420122, + "heading": -0.09725221374447425, + "angularVelocity": -0.0000025482714812860075, + "velocityX": -3.393760027773155, + "velocityY": 1.5355833615511818, + "timestamp": 3.0258480360136364 + }, + { + "x": 5.90930410944429, + "y": 4.065017159873787, + "heading": -0.09725229643791082, + "angularVelocity": -0.0000025482715370955637, + "velocityX": -3.3937600277731512, + "velocityY": 1.5355833615511787, + "timestamp": 3.0582988307417907 + }, + { + "x": 5.79917389942641, + "y": 4.114848060327453, + "heading": -0.09725237913134915, + "angularVelocity": -0.0000025482715921357367, + "velocityX": -3.3937600277731477, + "velocityY": 1.5355833615511751, + "timestamp": 3.090749625469945 + }, + { + "x": 5.689043689408531, + "y": 4.164678960781118, + "heading": -0.09725246182478928, + "angularVelocity": -0.0000025482716465928655, + "velocityX": -3.3937600277731437, + "velocityY": 1.5355833615511723, + "timestamp": 3.123200420198099 + }, + { + "x": 5.5789134793906525, + "y": 4.2145098612347835, + "heading": -0.09725254451823122, + "angularVelocity": -0.0000025482717030707046, + "velocityX": -3.39376002777314, + "velocityY": 1.535583361551169, + "timestamp": 3.1556512149262534 + }, + { + "x": 5.468783269372774, + "y": 4.264340761688448, + "heading": -0.09725262721167492, + "angularVelocity": -0.000002548271757487181, + "velocityX": -3.3937600277731357, + "velocityY": 1.535583361551166, + "timestamp": 3.1881020096544077 + }, + { + "x": 5.358653059354908, + "y": 4.314171662142141, + "heading": -0.09725270990512043, + "angularVelocity": -0.000002548271812724623, + "velocityX": -3.3937600277727378, + "velocityY": 1.5355833615520338, + "timestamp": 3.220552804382562 + }, + { + "x": 5.248522849397515, + "y": 4.364002562729485, + "heading": -0.09725279259856773, + "angularVelocity": -0.000002548271867828041, + "velocityX": -3.393760025909188, + "velocityY": 1.5355833656706142, + "timestamp": 3.2530035991107162 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.000002548289809515153, + "velocityX": -3.3937512180408453, + "velocityY": 1.5356028315849852, + "timestamp": 3.2854543938388705 + }, + { + "x": 4.9137282295128415, + "y": 4.535640155229253, + "heading": -0.09725287529313524, + "angularVelocity": -7.495790555066974e-12, + "velocityX": -3.1302347498710463, + "velocityY": 1.6971138308958609, + "timestamp": 3.357226868973161 + }, + { + "x": 4.717146626097375, + "y": 4.642220517268037, + "heading": -0.09725287529322811, + "angularVelocity": -1.2941727290342862e-12, + "velocityX": -2.738955331380901, + "velocityY": 1.4849754288028527, + "timestamp": 3.428999344107451 + }, + { + "x": 4.5486481109529855, + "y": 4.733575136181824, + "heading": -0.09725287529316204, + "angularVelocity": 9.207220397125275e-13, + "velocityX": -2.347675969501116, + "velocityY": 1.2728364006237491, + "timestamp": 3.5007718192417414 + }, + { + "x": 4.408232682725307, + "y": 4.809703996992033, + "heading": -0.09725287529303236, + "angularVelocity": 1.806653851545192e-12, + "velocityX": -1.9563966264916148, + "velocityY": 1.0606971637493046, + "timestamp": 3.5725442943760317 + }, + { + "x": 4.2959003407371545, + "y": 4.870607092209374, + "heading": -0.09725287529288679, + "angularVelocity": 2.028301137290635e-12, + "velocityX": -1.5651172929172836, + "velocityY": 0.8485578225272127, + "timestamp": 3.644316769510322 + }, + { + "x": 4.211651084582215, + "y": 4.916284417340276, + "heading": -0.09725287529275393, + "angularVelocity": 1.850997831523441e-12, + "velocityX": -1.1738379650040627, + "velocityY": 0.6364184186965437, + "timestamp": 3.7160892446446123 + }, + { + "x": 4.155484913989614, + "y": 4.946735969389024, + "heading": -0.09725287529265289, + "angularVelocity": 1.4078899342405594e-12, + "velocityX": -0.7825586408649186, + "velocityY": 0.424278973126829, + "timestamp": 3.7878617197789026 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 7.752391774433126e-13, + "velocityX": -0.3912793194215449, + "velocityY": 0.2121394977435137, + "timestamp": 3.859634194913193 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -8.979320165657004e-28, + "velocityX": -3.317577006729804e-27, + "velocityY": -2.7701124132227202e-27, + "timestamp": 3.9314066700474832 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGF.4.traj b/src/main/deploy/choreo/SourceLanePHGF.4.traj new file mode 100644 index 00000000..6a3211db --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePHGF.4.traj @@ -0,0 +1,446 @@ +{ + "samples": [ + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -8.979320165657004e-28, + "velocityX": -3.317577006729804e-27, + "velocityY": -2.7701124132227202e-27, + "timestamp": 0 + }, + { + "x": 4.149345287192321, + "y": 4.948730090302555, + "heading": -0.09725287542061174, + "angularVelocity": -1.9914932525911006e-9, + "velocityX": 0.34136955009475556, + "velocityY": -0.20584195701235133, + "timestamp": 0.06428065543737382 + }, + { + "x": 4.193331867161711, + "y": 4.922433486987429, + "heading": -0.09725287565526207, + "angularVelocity": -3.6504037180002943e-9, + "velocityX": 0.6842895373437039, + "velocityY": -0.4090904664272751, + "timestamp": 0.12856131087474765 + }, + { + "x": 4.259487364434434, + "y": 4.883286547964955, + "heading": -0.09725287596919822, + "angularVelocity": -4.883835550017299e-9, + "velocityX": 1.02916650153289, + "velocityY": -0.6090003089749592, + "timestamp": 0.19284196631212147 + }, + { + "x": 4.347975400700828, + "y": 4.831575784808056, + "heading": -0.09725287632620161, + "angularVelocity": -5.55382285919441e-9, + "velocityX": 1.3765888923239984, + "velocityY": -0.8044529540816497, + "timestamp": 0.2571226217494953 + }, + { + "x": 4.45901715765165, + "y": 4.76770271955414, + "heading": -0.09725287667606397, + "angularVelocity": -5.442731637489258e-9, + "velocityX": 1.7274521579669766, + "velocityY": -0.9936592092802461, + "timestamp": 0.3214032771868691 + }, + { + "x": 4.592927185358519, + "y": 4.69226963680943, + "heading": -0.09725287694462323, + "angularVelocity": -4.177917358984547e-9, + "velocityX": 2.083208809800217, + "velocityY": -1.1734958555019974, + "timestamp": 0.38568393262424294 + }, + { + "x": 4.750184246245984, + "y": 4.606276833638444, + "heading": -0.09725287701156489, + "angularVelocity": -1.0413967703177253e-9, + "velocityX": 2.446413463233511, + "velocityY": -1.3377711005881947, + "timestamp": 0.44996458806161677 + }, + { + "x": 4.931581310093376, + "y": 4.511690334370361, + "heading": -0.0972528766497097, + "angularVelocity": 5.629301248871626e-9, + "velocityX": 2.821954172886779, + "velocityY": -1.4714613381662662, + "timestamp": 0.5142452434989906 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 2.1112299822496666e-8, + "velocityX": 3.2173227507078654, + "velocityY": -1.5223279648179473, + "timestamp": 0.5785258989363644 + }, + { + "x": 5.398868955921396, + "y": 4.32613897098341, + "heading": -0.09725286997797288, + "angularVelocity": 7.20303645148975e-8, + "velocityX": 3.5302934161705415, + "velocityY": -1.1885528129676433, + "timestamp": 0.6523090098431901 + }, + { + "x": 5.6681314885139065, + "y": 4.271040046607528, + "heading": -0.09725286670603911, + "angularVelocity": 4.4345294143454955e-8, + "velocityX": 3.6493789606207083, + "velocityY": -0.7467687889368465, + "timestamp": 0.7260921207500157 + }, + { + "x": 5.942117714678701, + "y": 4.249366302589977, + "heading": -0.0972528635577222, + "angularVelocity": 4.26698858409276e-8, + "velocityX": 3.713400299843557, + "velocityY": -0.29374939266142885, + "timestamp": 0.7998752316568414 + }, + { + "x": 6.216382330790907, + "y": 4.231558335942029, + "heading": -0.09725286041527656, + "angularVelocity": 4.259031080949294e-8, + "velocityX": 3.7171733848218165, + "velocityY": -0.24135559519084493, + "timestamp": 0.873658342563667 + }, + { + "x": 6.4906469514050675, + "y": 4.21375043862986, + "heading": -0.09725285727283085, + "angularVelocity": 4.259031210936323e-8, + "velocityX": 3.717173445837876, + "velocityY": -0.241354655466577, + "timestamp": 0.9474414534704927 + }, + { + "x": 6.764911572019293, + "y": 4.195942541318692, + "heading": -0.09725285413038519, + "angularVelocity": 4.259031138372869e-8, + "velocityX": 3.717173445838756, + "velocityY": -0.24135465545301274, + "timestamp": 1.0212245643773183 + }, + { + "x": 7.039176191804645, + "y": 4.178134631242245, + "heading": -0.09725285098790416, + "angularVelocity": 4.2590790558228534e-8, + "velocityX": 3.717173434604845, + "velocityY": -0.24135482846385797, + "timestamp": 1.095007675284144 + }, + { + "x": 7.303151625715878, + "y": 4.160355738175747, + "heading": -0.07493794888540253, + "angularVelocity": 0.3024391602392781, + "velocityX": 3.5777216583424782, + "velocityY": -0.24096155404655678, + "timestamp": 1.1687907861909697 + }, + { + "x": 7.533445930864736, + "y": 4.144763761571347, + "heading": -0.05552477724704528, + "angularVelocity": 0.26311131910489216, + "velocityX": 3.1212333326481616, + "velocityY": -0.21132175660214023, + "timestamp": 1.2425738970977953 + }, + { + "x": 7.730059092320318, + "y": 4.131358730965977, + "heading": -0.03901370974052792, + "angularVelocity": 0.22377841356361258, + "velocityX": 2.664744804591801, + "velocityY": -0.1816815588366564, + "timestamp": 1.316357008004621 + }, + { + "x": 7.892991106532594, + "y": 4.120140669647355, + "heading": -0.02540508118930727, + "angularVelocity": 0.18444097008061852, + "velocityX": 2.20825622842103, + "velocityY": -0.15204104544723035, + "timestamp": 1.3901401189114466 + }, + { + "x": 8.02224197249394, + "y": 4.111109595697515, + "heading": -0.014699203415963554, + "angularVelocity": 0.14509930039224503, + "velocityX": 1.7517676385937064, + "velocityY": -0.12240028698767837, + "timestamp": 1.4639232298182723 + }, + { + "x": 8.117811689845258, + "y": 4.104265522227424, + "heading": -0.0068963232026660135, + "angularVelocity": 0.10575428600660669, + "velocityX": 1.2952790438994197, + "velocityY": -0.09275935083211628, + "timestamp": 1.537706340725098 + }, + { + "x": 8.179700258175622, + "y": 4.099608457473234, + "heading": -0.001996583897562948, + "angularVelocity": 0.06640732878951901, + "velocityX": 0.8387904436357886, + "velocityY": -0.06311830305000711, + "timestamp": 1.6114894516319236 + }, + { + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": 5.455800259144416e-28, + "angularVelocity": 0.027060175059361254, + "velocityX": 0.38230183268875023, + "velocityY": -0.03347720903449647, + "timestamp": 1.6852725625387492 + }, + { + "x": 8.200639571345327, + "y": 4.096943687462525, + "heading": -0.0010802899918473188, + "angularVelocity": -0.0140223298346035, + "velocityX": -0.0943411225502494, + "velocityY": -0.002527461513954009, + "timestamp": 1.7623132543916427 + }, + { + "x": 8.15665056358688, + "y": 4.099133352364744, + "heading": -0.0053255986809540795, + "angularVelocity": -0.05510475810903991, + "velocityX": -0.5709840695932866, + "velocityY": 0.028422186373926028, + "timestamp": 1.839353946244536 + }, + { + "x": 8.075940653456616, + "y": 4.103707386175576, + "heading": -0.012735784669566218, + "angularVelocity": -0.09618535101893501, + "velocityX": -1.0476270161796533, + "velocityY": 0.05937166062275022, + "timestamp": 1.9163946380974295 + }, + { + "x": 7.958509840643352, + "y": 4.110665769785427, + "heading": -0.023310616019513113, + "angularVelocity": -0.13726293333579098, + "velocityX": -1.5242699668052715, + "velocityY": 0.0903208868261235, + "timestamp": 1.993435329950323 + }, + { + "x": 7.804358124937036, + "y": 4.120008478318513, + "heading": -0.037049836652876506, + "angularVelocity": -0.17833719172197993, + "velocityX": -2.000912920157348, + "velocityY": 0.12126979013798393, + "timestamp": 2.0704760218032163 + }, + { + "x": 7.613485507099167, + "y": 4.131735481105862, + "heading": -0.05395325351914213, + "angularVelocity": -0.21940894428287547, + "velocityX": -2.477555863625089, + "velocityY": 0.15221829536189968, + "timestamp": 2.1475167136561097 + }, + { + "x": 7.385891991055361, + "y": 4.145846741676364, + "heading": -0.07402084166347275, + "angularVelocity": -0.2604803729261552, + "velocityX": -2.954198756137707, + "velocityY": 0.18316632718520298, + "timestamp": 2.224557405509003 + }, + { + "x": 7.121577593711165, + "y": 4.162342217760912, + "heading": -0.09725284930983928, + "angularVelocity": -0.30155502355466035, + "velocityX": -3.4308414292137566, + "velocityY": 0.214113810349038, + "timestamp": 2.3015980973618966 + }, + { + "x": 6.835203978246458, + "y": 4.180936324910101, + "heading": -0.09725285259123628, + "angularVelocity": -4.2593036391257635e-8, + "velocityX": -3.717173464790893, + "velocityY": 0.24135436354456943, + "timestamp": 2.37863878921479 + }, + { + "x": 6.5488303639215975, + "y": 4.199530449615619, + "heading": -0.09725285587256693, + "angularVelocity": -4.259217541458275e-8, + "velocityX": -3.717173449995519, + "velocityY": 0.2413545914284215, + "timestamp": 2.4556794810676834 + }, + { + "x": 6.262456749596831, + "y": 4.218124574322583, + "heading": -0.0972528591538976, + "angularVelocity": -4.259217545578449e-8, + "velocityX": -3.7171734499942994, + "velocityY": 0.2413545914471941, + "timestamp": 2.532720172920577 + }, + { + "x": 5.976083139919176, + "y": 4.236718770600949, + "heading": -0.09725286243522821, + "angularVelocity": -4.259217464314882e-8, + "velocityX": -3.717173389674077, + "velocityY": 0.24135552045497397, + "timestamp": 2.6097608647734702 + }, + { + "x": 5.690024921889411, + "y": 4.259659658304738, + "heading": -0.09725286572349409, + "angularVelocity": -4.268219562622653e-8, + "velocityX": -3.71307955769639, + "velocityY": 0.29777624203574676, + "timestamp": 2.6868015566263637 + }, + { + "x": 5.4092552652272206, + "y": 4.319022844462834, + "heading": -0.09725286915389227, + "angularVelocity": -4.452709474069528e-8, + "velocityX": -3.6444332197627625, + "velocityY": 0.7705432639604031, + "timestamp": 2.763842248479257 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -7.968133230975529e-8, + "velocityX": -3.5158347290283003, + "velocityY": 1.2306645781352268, + "timestamp": 2.8408829403321505 + }, + { + "x": 4.909018653493503, + "y": 4.524275328875486, + "heading": -0.09725287687080802, + "angularVelocity": -2.1964896173308048e-8, + "velocityX": -3.192337867898694, + "velocityY": 1.5370761958403907, + "timestamp": 2.9127344426669985 + }, + { + "x": 4.71108590609518, + "y": 4.628682093882472, + "heading": -0.09725287720527087, + "angularVelocity": -4.654917836145884e-9, + "velocityX": -2.754747513502237, + "velocityY": 1.4530909113134631, + "timestamp": 2.9845859450018466 + }, + { + "x": 4.54294597125426, + "y": 4.721368217002082, + "heading": -0.09725287703485491, + "angularVelocity": 2.3717800410462983e-9, + "velocityX": -2.3401032598781417, + "velocityY": 1.2899677822694202, + "timestamp": 3.0564374473366946 + }, + { + "x": 4.40369795049572, + "y": 4.80026148816612, + "heading": -0.0972528766496691, + "angularVelocity": 5.3608595378330946e-9, + "velocityX": -1.9379973449908554, + "velocityY": 1.0980044759032759, + "timestamp": 3.1282889496715427 + }, + { + "x": 4.292814205558175, + "y": 4.864307082758427, + "heading": -0.09725287620265412, + "angularVelocity": 6.221372869321198e-9, + "velocityX": -1.5432348849269113, + "velocityY": 0.8913605493429585, + "timestamp": 3.2001404520063907 + }, + { + "x": 4.209952469660844, + "y": 4.91286872561946, + "heading": -0.09725287578795568, + "angularVelocity": 5.771604384474185e-9, + "velocityX": -1.1532359547775561, + "velocityY": 0.6758612037744437, + "timestamp": 3.271991954341239 + }, + { + "x": 4.154873707750821, + "y": 4.945521497298995, + "heading": -0.09725287546928488, + "angularVelocity": 4.4351304984872785e-9, + "velocityX": -0.7665638173206214, + "velocityY": 0.4544480020384788, + "timestamp": 3.343843456676087 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 2.459066716778208e-9, + "velocityX": -0.38234244368231685, + "velocityY": 0.22880870103743695, + "timestamp": 3.415694959010935 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -3.871307616700285e-29, + "velocityX": 3.757481365144797e-28, + "velocityY": -1.0154842203400723e-27, + "timestamp": 3.487546461345783 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGF.traj b/src/main/deploy/choreo/SourceLanePHGF.traj new file mode 100644 index 00000000..b6b2c909 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePHGF.traj @@ -0,0 +1,1949 @@ +{ + "samples": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": 5.0336126459058196e-26, + "angularVelocity": 1.4969503738616677e-26, + "velocityX": 6.632423953121246e-26, + "velocityY": 2.3403167387234696e-25, + "timestamp": 0 + }, + { + "x": 0.40051040035567387, + "y": 4.115348381940811, + "heading": -0.015277315995300885, + "angularVelocity": -0.30891158633599436, + "velocityX": 0.2731840597647725, + "velocityY": -0.11699249437857151, + "timestamp": 0.04945530265311636 + }, + { + "x": 0.42755206280004615, + "y": 4.103759116150659, + "heading": -0.04541480648334463, + "angularVelocity": -0.6093884552568736, + "velocityX": 0.5467899495842703, + "velocityY": -0.23433818354000846, + "timestamp": 0.09891060530623272 + }, + { + "x": 0.46814907636931, + "y": 4.086345867978605, + "heading": -0.08991129386238127, + "angularVelocity": -0.8997313734209399, + "velocityX": 0.8208829264278229, + "velocityY": -0.3521007301116199, + "timestamp": 0.14836590795934906 + }, + { + "x": 0.5223311730580029, + "y": 4.063083084813687, + "heading": -0.14811250055404074, + "angularVelocity": -1.1768446166407593, + "velocityX": 1.095577092485525, + "velocityY": -0.4703799575969681, + "timestamp": 0.19782121061246544 + }, + { + "x": 0.5901369333153729, + "y": 4.033939247984461, + "heading": -0.2191152852440613, + "angularVelocity": -1.4356960908325656, + "velocityX": 1.3710513659771775, + "velocityY": -0.5892965014013483, + "timestamp": 0.2472765132655818 + }, + { + "x": 0.6716171763309451, + "y": 3.9988769782710625, + "heading": -0.3016414776381226, + "angularVelocity": -1.6687026055205227, + "velocityX": 1.6475532176414214, + "velocityY": -0.7089688634468179, + "timestamp": 0.2967318159186982 + }, + { + "x": 0.766836765570499, + "y": 3.957853748642466, + "heading": -0.39387111054068624, + "angularVelocity": -1.864908876394308, + "velocityX": 1.9253666266574487, + "velocityY": -0.8295011339094811, + "timestamp": 0.34618711857181456 + }, + { + "x": 0.8758711660110932, + "y": 3.910821247404655, + "heading": -0.49317423957562295, + "angularVelocity": -2.007936939168226, + "velocityX": 2.2047059585373607, + "velocityY": -0.9510102802868479, + "timestamp": 0.39564242122493093 + }, + { + "x": 0.998787530743033, + "y": 3.857723681334368, + "heading": -0.5954683580210677, + "angularVelocity": -2.0684155784657574, + "velocityX": 2.4854031446149563, + "velocityY": -1.0736475811849282, + "timestamp": 0.4450977238780473 + }, + { + "x": 1.1355416361282653, + "y": 3.7985302060221593, + "heading": -0.6929936795382847, + "angularVelocity": -1.9719891757870285, + "velocityX": 2.7652061164085326, + "velocityY": -1.1969085646365696, + "timestamp": 0.4945530265311637 + }, + { + "x": 1.284369196894048, + "y": 3.7333344256591525, + "heading": -0.760822351237353, + "angularVelocity": -1.3715146417124207, + "velocityX": 3.009334748382224, + "velocityY": -1.3182768452615752, + "timestamp": 0.54400832918428 + }, + { + "x": 1.444068666194049, + "y": 3.662157825629303, + "heading": -0.7899998870158123, + "angularVelocity": -0.589977903544803, + "velocityX": 3.229167768320948, + "velocityY": -1.4392106854362665, + "timestamp": 0.5934636318373964 + }, + { + "x": 1.6133635999437081, + "y": 3.589517549083241, + "heading": -0.7899999435088091, + "angularVelocity": -0.0000011423041352586315, + "velocityX": 3.42319073319818, + "velocityY": -1.4688066324367013, + "timestamp": 0.6429189344905127 + }, + { + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "angularVelocity": -0.000001142267622119268, + "velocityX": 3.4231940186144136, + "velocityY": -1.4687989754737478, + "timestamp": 0.6923742371436291 + }, + { + "x": 2.023972333671043, + "y": 3.4134441767348855, + "heading": -0.79, + "angularVelocity": -1.7456316689050396e-17, + "velocityX": 3.4237473592821543, + "velocityY": -1.4675096226857507, + "timestamp": 0.7628565514540566 + }, + { + "x": 2.250495583067251, + "y": 3.3163502598603043, + "heading": -0.79, + "angularVelocity": -2.7198862157697797e-16, + "velocityX": 3.2139019782824447, + "velocityY": -1.37756425600538, + "timestamp": 0.8333388657644841 + }, + { + "x": 2.4487034476982825, + "y": 3.2313930734184306, + "heading": -0.79, + "angularVelocity": -2.2318862582403906e-16, + "velocityX": 2.8121645347520525, + "velocityY": -1.2053688542021173, + "timestamp": 0.9038211800749116 + }, + { + "x": 2.6185959113515174, + "y": 3.1585726243584236, + "heading": -0.79, + "angularVelocity": -1.6717248939922967e-16, + "velocityX": 2.410426861197717, + "velocityY": -1.0331733538044932, + "timestamp": 0.9743034943853391 + }, + { + "x": 2.760172968622745, + "y": 3.0978889149966715, + "heading": -0.79, + "angularVelocity": -1.1384117019167077e-16, + "velocityX": 2.0086891109686777, + "velocityY": -0.8609778205420586, + "timestamp": 1.0447858086957666 + }, + { + "x": 2.8734346168098592, + "y": 3.049341946491368, + "heading": -0.79, + "angularVelocity": -1.4459082642914294e-16, + "velocityX": 1.6069513224022822, + "velocityY": -0.6887822708472163, + "timestamp": 1.1152681230061943 + }, + { + "x": 2.958380854291598, + "y": 3.0129317195374297, + "heading": -0.79, + "angularVelocity": -1.2018162111442988e-16, + "velocityX": 1.2052135108334716, + "velocityY": -0.5165867112929291, + "timestamp": 1.1857504373166219 + }, + { + "x": 3.015011679987118, + "y": 2.9886582345981343, + "heading": -0.79, + "angularVelocity": -6.358045799066146e-17, + "velocityX": 0.8034756839297176, + "velocityY": -0.3443911451656786, + "timestamp": 1.2562327516270495 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -1.168085544952961e-16, + "velocityX": 0.40173784607243274, + "velocityY": -0.17219557434345445, + "timestamp": 1.3267150659374771 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 1.4132283846579156e-28, + "velocityX": -7.2110997365083e-27, + "velocityY": -2.0111921052200978e-27, + "timestamp": 1.3971973802479047 + }, + { + "x": 3.065434104608151, + "y": 2.9635744703134126, + "heading": -0.7762472311002773, + "angularVelocity": 0.2130317791406229, + "velocityX": 0.34243984045734815, + "velocityY": -0.2005506735052959, + "timestamp": 1.4617547385688177 + }, + { + "x": 3.109663922209824, + "y": 2.9376745027807694, + "heading": -0.7492642309726533, + "angularVelocity": 0.41796939697396296, + "velocityX": 0.6851243413927804, + "velocityY": -0.40119311270289487, + "timestamp": 1.5263120968897306 + }, + { + "x": 3.176036274163731, + "y": 2.8988141772022082, + "heading": -0.7097339632553362, + "angularVelocity": 0.612327839079368, + "velocityX": 1.028114434670201, + "velocityY": -0.6019503676929793, + "timestamp": 1.5908694552106435 + }, + { + "x": 3.2645764628811422, + "y": 2.8469841623353034, + "heading": -0.6585853448584378, + "angularVelocity": 0.7922972644363798, + "velocityX": 1.3714964648534713, + "velocityY": -0.8028521645706009, + "timestamp": 1.6554268135315564 + }, + { + "x": 3.3753176768637694, + "y": 2.782172830859228, + "heading": -0.5971442411061407, + "angularVelocity": 0.9517289020234527, + "velocityX": 1.7153925882799501, + "velocityY": -1.0039340698220625, + "timestamp": 1.7199841718524693 + }, + { + "x": 3.5083041287580397, + "y": 2.7043667248342107, + "heading": -0.5274363257086371, + "angularVelocity": 1.0797826492680733, + "velocityX": 2.0599735700645976, + "velocityY": -1.2052244399196936, + "timestamp": 1.7845415301733822 + }, + { + "x": 3.663593064192348, + "y": 2.6135556042232486, + "heading": -0.45293092753819847, + "angularVelocity": 1.1540961419157545, + "velocityX": 2.405441292414274, + "velocityY": -1.4066734292246417, + "timestamp": 1.8490988884942952 + }, + { + "x": 3.8412236037881105, + "y": 2.50976866496977, + "heading": -0.3811103115245424, + "angularVelocity": 1.1125085951726474, + "velocityX": 2.751514997140456, + "velocityY": -1.6076701704173844, + "timestamp": 1.913656246815208 + }, + { + "x": 4.040074211349249, + "y": 2.394026373110605, + "heading": -0.3455558271366132, + "angularVelocity": 0.5507425537951661, + "velocityX": 3.0802159929261363, + "velocityY": -1.7928597896433947, + "timestamp": 1.978213605136121 + }, + { + "x": 4.2474603616320605, + "y": 2.2722898239651945, + "heading": -0.3455557994031658, + "angularVelocity": 4.2959390119270946e-7, + "velocityX": 3.2124324116842065, + "velocityY": -1.8857114403637907, + "timestamp": 2.042770963457034 + }, + { + "x": 4.454846495549661, + "y": 2.150553246940426, + "heading": -0.34555577166993406, + "angularVelocity": 4.2959055996150303e-7, + "velocityX": 3.21243215818542, + "velocityY": -1.8857118722178698, + "timestamp": 2.1073283217779473 + }, + { + "x": 4.662232629466809, + "y": 2.0288166699148875, + "heading": -0.34555574393670246, + "angularVelocity": 4.295905587004531e-7, + "velocityX": 3.2124321581784225, + "velocityY": -1.885711872229791, + "timestamp": 2.1718856800988604 + }, + { + "x": 4.86961876338396, + "y": 1.9070800928893532, + "heading": -0.34555571620347075, + "angularVelocity": 4.295905595925436e-7, + "velocityX": 3.2124321581784563, + "velocityY": -1.8857118722297332, + "timestamp": 2.2364430384197735 + }, + { + "x": 5.0770048973810855, + "y": 1.7853435160000617, + "heading": -0.3455556884702391, + "angularVelocity": 4.2959055900802054e-7, + "velocityX": 3.2124321594172818, + "velocityY": -1.8857118701193136, + "timestamp": 2.3010003967406867 + }, + { + "x": 5.284393928450512, + "y": 1.6636118745954684, + "heading": -0.3455556607369369, + "angularVelocity": 4.295916530112558e-7, + "velocityX": 3.212477035359784, + "velocityY": -1.885635418962908, + "timestamp": 2.3655577550616 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 4.37946601168607e-7, + "velocityX": 3.386285847111441, + "velocityY": -1.5519979873442957, + "timestamp": 2.430115113382513 + }, + { + "x": 5.67114193645051, + "y": 1.502092704427615, + "heading": -0.3455556386074096, + "angularVelocity": -1.278584041845331e-7, + "velocityX": 3.49949220171166, + "velocityY": -1.2763944237939575, + "timestamp": 2.478161609702393 + }, + { + "x": 5.843643430464205, + "y": 1.4543994299893364, + "heading": -0.3455556446255623, + "angularVelocity": -1.252568492070864e-7, + "velocityX": 3.590303294234579, + "velocityY": -0.9926483321645406, + "timestamp": 2.526208106022273 + }, + { + "x": 6.017301477098486, + "y": 1.4111062813327742, + "heading": -0.3455556506300644, + "angularVelocity": -1.2497273485529655e-7, + "velocityX": 3.6143748230487924, + "velocityY": -0.9010677567065171, + "timestamp": 2.5742546023421533 + }, + { + "x": 6.190959602710417, + "y": 1.3678134494736272, + "heading": -0.3455556566345664, + "angularVelocity": -1.249727336927334e-7, + "velocityX": 3.6143764668242215, + "velocityY": -0.9010611631473742, + "timestamp": 2.6223010986620334 + }, + { + "x": 6.364617728327444, + "y": 1.32452061763492, + "heading": -0.34555566263906834, + "angularVelocity": -1.2497273274023636e-7, + "velocityX": 3.6143764669302776, + "velocityY": -0.9010611627219542, + "timestamp": 2.6703475949819135 + }, + { + "x": 6.53827585394447, + "y": 1.2812277857962142, + "heading": -0.3455556686435703, + "angularVelocity": -1.2497273262525817e-7, + "velocityX": 3.6143764669302847, + "velocityY": -0.9010611627219266, + "timestamp": 2.7183940913017937 + }, + { + "x": 6.711933979561497, + "y": 1.2379349539575075, + "heading": -0.3455556746480723, + "angularVelocity": -1.249727344246102e-7, + "velocityX": 3.614376466930281, + "velocityY": -0.9010611627219414, + "timestamp": 2.7664405876216738 + }, + { + "x": 6.885592105175778, + "y": 1.1946421221077876, + "heading": -0.3455556806525743, + "angularVelocity": -1.2497273402138388e-7, + "velocityX": 3.6143764668731366, + "velocityY": -0.9010611629511635, + "timestamp": 2.814487083941554 + }, + { + "x": 7.059250188235952, + "y": 1.1513491195635046, + "heading": -0.34555568665707626, + "angularVelocity": -1.2497273336829244e-7, + "velocityX": 3.614375581187171, + "velocityY": -0.901064715646495, + "timestamp": 2.862533580261434 + }, + { + "x": 7.232252247020901, + "y": 1.105504804591487, + "heading": -0.3455556930363148, + "angularVelocity": -1.3277218955785758e-7, + "velocityX": 3.600721635000194, + "velocityY": -0.954165620460628, + "timestamp": 2.910580076581314 + }, + { + "x": 7.3915823056650956, + "y": 1.0554297346464303, + "heading": -0.34668228326838396, + "angularVelocity": -0.02344791646343344, + "velocityX": 3.3161639421825697, + "velocityY": -1.0422210521173303, + "timestamp": 2.9586265729011942 + }, + { + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.023449177160012234, + "velocityX": 3.0262995808418314, + "velocityY": -0.9742709781335815, + "timestamp": 3.0066730692210744 + }, + { + "x": 7.706721170015621, + "y": 0.9519776263380658, + "heading": -0.3399354575219549, + "angularVelocity": 0.0873776196257354, + "velocityX": 2.6389050045733367, + "velocityY": -0.8806177429462266, + "timestamp": 3.0709936007986016 + }, + { + "x": 7.851727537144501, + "y": 0.9021680166584882, + "heading": -0.3343317435174853, + "angularVelocity": 0.0871216991998166, + "velocityX": 2.254433593324705, + "velocityY": -0.7743967355049113, + "timestamp": 3.135314132376129 + }, + { + "x": 7.9720996028837305, + "y": 0.8595135021783036, + "heading": -0.3304150780634631, + "angularVelocity": 0.06089292731204182, + "velocityX": 1.8714407792812162, + "velocityY": -0.6631555031346718, + "timestamp": 3.199634663953656 + }, + { + "x": 8.067893377913451, + "y": 0.8241876261341342, + "heading": -0.3290917427033041, + "angularVelocity": 0.020574073747570358, + "velocityX": 1.4893187708540474, + "velocityY": -0.5492161706031644, + "timestamp": 3.2639551955311834 + }, + { + "x": 8.139145592381213, + "y": 0.7962986910106811, + "heading": -0.33093102223192583, + "angularVelocity": -0.028595527485726017, + "velocityX": 1.107767810996388, + "velocityY": -0.4335930447004768, + "timestamp": 3.3282757271087107 + }, + { + "x": 8.185882143295247, + "y": 0.7759207035915239, + "heading": -0.33632389468863655, + "angularVelocity": -0.08384371715290594, + "velocityX": 0.726619475426766, + "velocityY": -0.3168193253284159, + "timestamp": 3.392596258686238 + }, + { + "x": 8.208122253417969, + "y": 0.763107419013977, + "heading": -0.34555563246426124, + "angularVelocity": -0.14352707524653271, + "velocityX": 0.34576999874318054, + "velocityY": -0.19920986756931033, + "timestamp": 3.456916790263765 + }, + { + "x": 8.204294589663224, + "y": 0.758059174501978, + "heading": -0.3598356424553756, + "angularVelocity": -0.21015280737857786, + "velocityX": -0.056330092504229386, + "velocityY": -0.07429285814157252, + "timestamp": 3.5248673932999157 + }, + { + "x": 8.173143956099947, + "y": 0.7614990375664682, + "heading": -0.3786432717750789, + "angularVelocity": -0.2767838470792884, + "velocityX": -0.4584305682571252, + "velocityY": 0.05062299539358275, + "timestamp": 3.5928179963360662 + }, + { + "x": 8.114670323431806, + "y": 0.7734269104252286, + "heading": -0.4019794984677993, + "angularVelocity": -0.34342928024207936, + "velocityX": -0.8605314751516402, + "velocityY": 0.17553740990958688, + "timestamp": 3.6607685993722168 + }, + { + "x": 8.028873660690506, + "y": 0.7938426763627194, + "heading": -0.42984618674257646, + "angularVelocity": -0.41010214817301655, + "velocityX": -1.2626328377933065, + "velocityY": 0.3004501067728478, + "timestamp": 3.7287192024083673 + }, + { + "x": 7.915753937180706, + "y": 0.8227462003684911, + "heading": -0.46224640751329055, + "angularVelocity": -0.47682020943179554, + "velocityX": -1.6647346521651787, + "velocityY": 0.4253608167449919, + "timestamp": 3.796669805444518 + }, + { + "x": 7.7753111250087, + "y": 0.8601373303576515, + "heading": -0.4991848061620223, + "angularVelocity": -0.5436066347943985, + "velocityX": -2.0668368770368284, + "velocityY": 0.5502692885487409, + "timestamp": 3.8646204084806683 + }, + { + "x": 7.607545202545143, + "y": 0.9060158995198092, + "heading": -0.5406680081845634, + "angularVelocity": -0.6104905647485055, + "velocityX": -2.468939420218282, + "velocityY": 0.6751753054752103, + "timestamp": 3.932571011516819 + }, + { + "x": 7.412456160128696, + "y": 0.9603817316039739, + "heading": -0.5867050376005867, + "angularVelocity": -0.6775072973455604, + "velocityX": -2.8710421055815774, + "velocityY": 0.800078728591143, + "timestamp": 4.000521614552969 + }, + { + "x": 7.190044017003328, + "y": 1.023234660815804, + "heading": -0.6373076187752672, + "angularVelocity": -0.7446965724168686, + "velocityX": -3.273144507739573, + "velocityY": 0.9249797117825663, + "timestamp": 4.06847221758912 + }, + { + "x": 6.9483240964505395, + "y": 1.0983293400926897, + "heading": -0.6373076247970214, + "angularVelocity": -8.861958477756183e-8, + "velocityX": -3.5572888208834765, + "velocityY": 1.1051363184655567, + "timestamp": 4.1364228206252704 + }, + { + "x": 6.70660438555466, + "y": 1.1734246942257325, + "heading": -0.6373076308180073, + "angularVelocity": -8.86082771208124e-8, + "velocityX": -3.5572857354522953, + "velocityY": 1.1051462500353575, + "timestamp": 4.204373423661421 + }, + { + "x": 6.464884674663689, + "y": 1.2485200483745762, + "heading": -0.6373076368389932, + "angularVelocity": -8.860827701986505e-8, + "velocityX": -3.557285735380054, + "velocityY": 1.105146250267891, + "timestamp": 4.2723240266975715 + }, + { + "x": 6.223164963772718, + "y": 1.3236154025234206, + "heading": -0.637307642859979, + "angularVelocity": -8.860827626589147e-8, + "velocityX": -3.5572857353800513, + "velocityY": 1.1051462502679001, + "timestamp": 4.340274629733722 + }, + { + "x": 5.98144525288525, + "y": 1.39871075668354, + "heading": -0.6373076488809649, + "angularVelocity": -8.860827719404325e-8, + "velocityX": -3.5572857353285006, + "velocityY": 1.1051462504338314, + "timestamp": 4.4082252327698725 + }, + { + "x": 5.739725691606158, + "y": 1.473806592406946, + "heading": -0.6373076549019507, + "angularVelocity": -8.860827643379068e-8, + "velocityX": -3.557283533605943, + "velocityY": 1.1051533373950213, + "timestamp": 4.476175835806023 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.6373076609624853, + "angularVelocity": -8.919029793694062e-8, + "velocityX": -3.483737947413037, + "velocityY": 1.3187872955082738, + "timestamp": 4.544126438842174 + }, + { + "x": 5.300661081332848, + "y": 1.6644259130191759, + "heading": -0.6373076662501911, + "angularVelocity": -8.709485563097327e-8, + "velocityX": -3.332823841916901, + "velocityY": 1.6637052194159954, + "timestamp": 4.6048384732983045 + }, + { + "x": 5.105140298152244, + "y": 1.7780767678650242, + "heading": -0.6373076714738495, + "angularVelocity": -8.603991577037157e-8, + "velocityX": -3.2204617244688527, + "velocityY": 1.8719658443989302, + "timestamp": 4.6655505077544355 + }, + { + "x": 4.909619881391782, + "y": 1.8917282530843815, + "heading": -0.6373076766975064, + "angularVelocity": -8.60398930609077e-8, + "velocityX": -3.220455689089781, + "velocityY": 1.8719762274064293, + "timestamp": 4.726262542210566 + }, + { + "x": 4.714099464642575, + "y": 2.0053797383231027, + "heading": -0.6373076819211634, + "angularVelocity": -8.603989205375444e-8, + "velocityX": -3.2204556889043876, + "velocityY": 1.8719762277253713, + "timestamp": 4.786974576666697 + }, + { + "x": 4.518579047893369, + "y": 2.1190312235618243, + "heading": -0.6373076871448202, + "angularVelocity": -8.60398922147823e-8, + "velocityX": -3.220455688904382, + "velocityY": 1.8719762277253813, + "timestamp": 4.847686611122828 + }, + { + "x": 4.323058631144163, + "y": 2.232682708800546, + "heading": -0.6373076923684772, + "angularVelocity": -8.603989163870152e-8, + "velocityX": -3.2204556889043805, + "velocityY": 1.8719762277253815, + "timestamp": 4.908398645578959 + }, + { + "x": 4.12753821439536, + "y": 2.3463341940399602, + "heading": -0.637307697592134, + "angularVelocity": -8.60398917504913e-8, + "velocityX": -3.2204556888977502, + "velocityY": 1.8719762277367886, + "timestamp": 4.96911068003509 + }, + { + "x": 3.9320178107531194, + "y": 2.4599857018249596, + "heading": -0.637307702817472, + "angularVelocity": -8.606758041499758e-8, + "velocityX": -3.2204554730169304, + "velocityY": 1.8719765990896107, + "timestamp": 5.029822714491221 + }, + { + "x": 3.754279851907353, + "y": 2.5632932295685515, + "heading": -0.6678287625734391, + "angularVelocity": -0.5027184483172089, + "velocityX": -2.9275572864255675, + "velocityY": 1.7015988455837243, + "timestamp": 5.090534748947352 + }, + { + "x": 3.5962904713608705, + "y": 2.6551219777359605, + "heading": -0.6949668036285394, + "angularVelocity": -0.4469960741425893, + "velocityX": -2.6022745236884077, + "velocityY": 1.5125295831382852, + "timestamp": 5.151246783403483 + }, + { + "x": 3.4580497056890147, + "y": 2.735472006369499, + "heading": -0.7187186173095553, + "angularVelocity": -0.3912208492729469, + "velocityX": -2.276991158511501, + "velocityY": 1.3234613096617127, + "timestamp": 5.211958817859614 + }, + { + "x": 3.3395575797280896, + "y": 2.804343370444312, + "heading": -0.7390815132457978, + "angularVelocity": -0.33540131077235125, + "velocityX": -1.9517073842508859, + "velocityY": 1.1343939416917022, + "timestamp": 5.272670852315745 + }, + { + "x": 3.2408141126366585, + "y": 2.861736116018487, + "heading": -0.7560532042537318, + "angularVelocity": -0.2795440996166463, + "velocityX": -1.6264232944257633, + "velocityY": 0.9453273323536148, + "timestamp": 5.333382886771876 + }, + { + "x": 3.16181931959339, + "y": 2.907650279470937, + "heading": -0.7696318707216167, + "angularVelocity": -0.22365691727389694, + "velocityX": -1.3011389545897718, + "velocityY": 0.756261322219841, + "timestamp": 5.394094921228007 + }, + { + "x": 3.1025732126750043, + "y": 2.942085887277494, + "heading": -0.7798162350397829, + "angularVelocity": -0.16774869116806188, + "velocityX": -0.9758544158357173, + "velocityY": 0.5671957481747656, + "timestamp": 5.454806955684138 + }, + { + "x": 3.0630758014096395, + "y": 2.9650429559458633, + "heading": -0.7866056220675054, + "angularVelocity": -0.11182934468500333, + "velocityX": -0.6505697201418033, + "velocityY": 0.37813044603137397, + "timestamp": 5.515518990140269 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -0.05590947433901855, + "velocityX": -0.3252849037618716, + "velocityY": 0.18906525141774544, + "timestamp": 5.5762310245963995 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 6.3035161532873835e-28, + "velocityX": -3.172311091955017e-27, + "velocityY": -4.980348575504513e-27, + "timestamp": 5.6369430590525305 + }, + { + "x": 3.0599792847904204, + "y": 2.966059333110192, + "heading": -0.7574164237892916, + "angularVelocity": 0.5582463314772692, + "velocityX": 0.2852978705745262, + "velocityY": -0.17924557403559396, + "timestamp": 5.695310798119411 + }, + { + "x": 3.0934694098196505, + "y": 2.9451270864719556, + "heading": -0.6941025427131003, + "angularVelocity": 1.0847410245519962, + "velocityX": 0.5737780075883294, + "velocityY": -0.35862699108922125, + "timestamp": 5.753678537186291 + }, + { + "x": 3.1440142929618213, + "y": 2.9136974289532884, + "heading": -0.6025701359612278, + "angularVelocity": 1.5682020276130737, + "velocityX": 0.8659729492734756, + "velocityY": -0.5384765286634409, + "timestamp": 5.812046276253171 + }, + { + "x": 3.2118603630472147, + "y": 2.8716921302721974, + "heading": -0.4864204343113424, + "angularVelocity": 1.9899640367566072, + "velocityX": 1.162389895000939, + "velocityY": -0.7196663662603013, + "timestamp": 5.870414015320051 + }, + { + "x": 3.297261323571818, + "y": 2.8189697938755005, + "heading": -0.35051552340624004, + "angularVelocity": 2.3284251382322116, + "velocityX": 1.463153479814372, + "velocityY": -0.9032787159407621, + "timestamp": 5.9287817543869314 + }, + { + "x": 3.400457830317495, + "y": 2.755367474678837, + "heading": -0.2010210241745889, + "angularVelocity": 2.561252185224339, + "velocityX": 1.768040160463131, + "velocityY": -1.0896827633461208, + "timestamp": 5.987149493453812 + }, + { + "x": 3.521660389826238, + "y": 2.6807799872839175, + "heading": -0.04666004939322163, + "angularVelocity": 2.644628304078967, + "velocityX": 2.076533397496587, + "velocityY": -1.277888926097578, + "timestamp": 6.045517232520692 + }, + { + "x": 3.6607588635338297, + "y": 2.5953087461465243, + "heading": 0.09647781625085058, + "angularVelocity": 2.4523455582211025, + "velocityX": 2.383139657820331, + "velocityY": -1.4643575801258442, + "timestamp": 6.103884971587572 + }, + { + "x": 3.8159145666935697, + "y": 2.50056098976987, + "heading": 0.19948059626334666, + "angularVelocity": 1.7647210883819062, + "velocityX": 2.6582441883170422, + "velocityY": -1.6232898154250606, + "timestamp": 6.162252710654452 + }, + { + "x": 3.984110357979779, + "y": 2.3943443694149815, + "heading": 0.2530307730268392, + "angularVelocity": 0.9174618996657108, + "velocityX": 2.8816567846406262, + "velocityY": -1.8197830180329746, + "timestamp": 6.220620449721332 + }, + { + "x": 4.165386797993929, + "y": 2.276764205392624, + "heading": 0.25694414020151846, + "angularVelocity": 0.06704674940715359, + "velocityX": 3.105764295691424, + "velocityY": -2.0144717938728, + "timestamp": 6.278988188788213 + }, + { + "x": 4.35049005488602, + "y": 2.162710813518879, + "heading": 0.2569442210228932, + "angularVelocity": 0.0000013846925722227273, + "velocityX": 3.171328200326413, + "velocityY": -1.9540484811833736, + "timestamp": 6.337355927855093 + }, + { + "x": 4.5355934176880135, + "y": 2.0486575935320284, + "heading": 0.2569443018441972, + "angularVelocity": 0.0000013846913602153038, + "velocityX": 3.171330014854514, + "velocityY": -1.9540455362878368, + "timestamp": 6.395723666921973 + }, + { + "x": 4.72069678050449, + "y": 1.9346043735686815, + "heading": 0.25694438266550157, + "angularVelocity": 0.0000013846913664886619, + "velocityX": 3.171330015102631, + "velocityY": -1.9540455358851527, + "timestamp": 6.454091405988853 + }, + { + "x": 4.905800420839664, + "y": 1.8205516040072125, + "heading": 0.2569444634867951, + "angularVelocity": 0.0000013846911811343512, + "velocityX": 3.171334769761663, + "velocityY": -1.9540378192614634, + "timestamp": 6.512459145055733 + }, + { + "x": 5.096209165426906, + "y": 1.7155949376896535, + "heading": 0.2569445446080598, + "angularVelocity": 0.0000013898305123819847, + "velocityX": 3.262225805407059, + "velocityY": -1.7981965379418772, + "timestamp": 6.570826884122614 + }, + { + "x": 5.295905298565615, + "y": 1.6296132789604225, + "heading": 0.2569446291955456, + "angularVelocity": 0.000001449216418667715, + "velocityX": 3.4213443304680435, + "velocityY": -1.473102438158684, + "timestamp": 6.629194623189494 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.256944722079518, + "angularVelocity": 0.0000015913580667498199, + "velocityX": 3.548163797412206, + "velocityY": -1.1340904342967109, + "timestamp": 6.687562362256374 + }, + { + "x": 5.694707875531734, + "y": 1.520245371460122, + "heading": 0.25694480525285635, + "angularVelocity": 0.0000015766499744480022, + "velocityX": 3.6339835774201763, + "velocityY": -0.8184074026058323, + "timestamp": 6.740315566608581 + }, + { + "x": 5.889460824583175, + "y": 1.4940579532692961, + "heading": 0.2569448839753191, + "angularVelocity": 0.0000014922783134293427, + "velocityX": 3.691774773550607, + "velocityY": -0.49641379158667054, + "timestamp": 6.793068770960788 + }, + { + "x": 6.085760337593024, + "y": 1.4850583071038423, + "heading": 0.25694496145000295, + "angularVelocity": 0.0000014686251728049365, + "velocityX": 3.721091740688451, + "velocityY": -0.17059904276842675, + "timestamp": 6.845821975312996 + }, + { + "x": 6.282233415675307, + "y": 1.4886391007137698, + "heading": 0.2569450396472057, + "angularVelocity": 0.0000014823213810882118, + "velocityX": 3.7243818739526797, + "velocityY": 0.06787821998490172, + "timestamp": 6.898575179665203 + }, + { + "x": 6.478706465105997, + "y": 1.4922214660505555, + "heading": 0.2569451178445693, + "angularVelocity": 0.000001482324430715833, + "velocityX": 3.7243813308274873, + "velocityY": 0.06790801394485714, + "timestamp": 6.95132838401741 + }, + { + "x": 6.6750148942106335, + "y": 1.5010244818843792, + "heading": 0.2569451969790633, + "angularVelocity": 0.0000015000888565600604, + "velocityX": 3.7212607559150324, + "velocityY": 0.16687167996564178, + "timestamp": 7.004081588369617 + }, + { + "x": 6.869794082781354, + "y": 1.5270160132207926, + "heading": 0.25694528055290106, + "angularVelocity": 0.000001584241920357872, + "velocityX": 3.692272174980594, + "velocityY": 0.4927005222826037, + "timestamp": 7.0568347927218245 + }, + { + "x": 7.061541469504156, + "y": 1.5699966670376497, + "heading": 0.25694631305508775, + "angularVelocity": 0.000019572312228907657, + "velocityX": 3.6348007495923222, + "velocityY": 0.814749631698122, + "timestamp": 7.109587997074032 + }, + { + "x": 7.242025004547745, + "y": 1.623544327324134, + "heading": 0.2892009263238574, + "angularVelocity": 0.611424721300743, + "velocityX": 3.421280986811508, + "velocityY": 1.0150598611787165, + "timestamp": 7.162341201426239 + }, + { + "x": 7.4101475255884415, + "y": 1.6859098332100053, + "heading": 0.3552181316610267, + "angularVelocity": 1.2514349819662982, + "velocityX": 3.186963201670605, + "velocityY": 1.1822126570641514, + "timestamp": 7.215094405778446 + }, + { + "x": 7.5650279020019084, + "y": 1.7562974315324296, + "heading": 0.45280543116466554, + "angularVelocity": 1.8498838260534038, + "velocityX": 2.935942533071662, + "velocityY": 1.3342810012540722, + "timestamp": 7.267847610130653 + }, + { + "x": 7.70378035758304, + "y": 1.8320280138991303, + "heading": 0.5628377984659659, + "angularVelocity": 2.0857949512728657, + "velocityX": 2.6302185295655014, + "velocityY": 1.4355636457850884, + "timestamp": 7.3206008144828605 + }, + { + "x": 7.825662907370156, + "y": 1.911322826381737, + "heading": 0.6698617562914626, + "angularVelocity": 2.0287669562392776, + "velocityX": 2.3104293148405795, + "velocityY": 1.5031278849564027, + "timestamp": 7.373354018835068 + }, + { + "x": 7.930693514616294, + "y": 1.9932244953323592, + "heading": 0.7659443680912281, + "angularVelocity": 1.8213606733397365, + "velocityX": 1.9909806150333516, + "velocityY": 1.5525439630890379, + "timestamp": 7.426107223187275 + }, + { + "x": 8.019011714129077, + "y": 2.0771819587981266, + "heading": 0.8464048765673222, + "angularVelocity": 1.5252250448882378, + "velocityX": 1.6741769641730917, + "velocityY": 1.5915140036844804, + "timestamp": 7.478860427539482 + }, + { + "x": 8.090768492192002, + "y": 2.1628615391483916, + "heading": 0.9080474489075738, + "angularVelocity": 1.1685085881929373, + "velocityX": 1.360235438663432, + "velocityY": 1.6241587862269713, + "timestamp": 7.531613631891689 + }, + { + "x": 8.146101418716428, + "y": 2.2500495157538962, + "heading": 0.9485346126333066, + "angularVelocity": 0.7674825486508784, + "velocityX": 1.048901715145006, + "velocityY": 1.6527522389615, + "timestamp": 7.5843668362438965 + }, + { + "x": 8.185122291410421, + "y": 2.338598291571523, + "heading": 0.9662103754330295, + "angularVelocity": 0.3350651968307046, + "velocityX": 0.7396872507207403, + "velocityY": 1.6785478134452219, + "timestamp": 7.637120040596104 + }, + { + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "angularVelocity": -0.11638132008356272, + "velocityX": 0.4319241942959219, + "velocityY": 1.7021818924733116, + "timestamp": 7.689873244948311 + }, + { + "x": 8.211423054828662, + "y": 2.540472392253658, + "heading": 0.9162466453352144, + "angularVelocity": -0.6758725051077806, + "velocityX": 0.054215367725083355, + "velocityY": 1.7285138763859442, + "timestamp": 7.754714235231942 + }, + { + "x": 8.190202888383633, + "y": 2.6538474230997973, + "heading": 0.8391734249667565, + "angularVelocity": -1.1886496494165113, + "velocityX": -0.3272646878495706, + "velocityY": 1.7485086262595175, + "timestamp": 7.819555225515574 + }, + { + "x": 8.144008306424501, + "y": 2.768019013990823, + "heading": 0.7319233890258415, + "angularVelocity": -1.6540468532601917, + "velocityX": -0.7124286929774685, + "velocityY": 1.760793448582603, + "timestamp": 7.884396215799205 + }, + { + "x": 8.072604152106418, + "y": 2.8824539716664033, + "heading": 0.5977397407756817, + "angularVelocity": -2.0694262635904495, + "velocityX": -1.1012193676521893, + "velocityY": 1.764855181498803, + "timestamp": 7.949237206082836 + }, + { + "x": 7.975691382967155, + "y": 2.9965380091631473, + "heading": 0.44171220804424666, + "angularVelocity": -2.406310145001336, + "velocityX": -1.4946219777850651, + "velocityY": 1.7594431700951976, + "timestamp": 8.014078196366468 + }, + { + "x": 7.852920805151493, + "y": 3.1093104190449847, + "heading": 0.2733111632783997, + "angularVelocity": -2.5971386931201565, + "velocityX": -1.8934099753663647, + "velocityY": 1.7392147989804203, + "timestamp": 8.078919186650099 + }, + { + "x": 7.704278478364254, + "y": 3.219028342207882, + "heading": 0.10912005253192102, + "angularVelocity": -2.5322116461865276, + "velocityX": -2.292412964963042, + "velocityY": 1.6921074567640455, + "timestamp": 8.14376017693373 + }, + { + "x": 7.532421373794059, + "y": 3.323057914868166, + "heading": -0.010926874391562237, + "angularVelocity": -1.851404896784686, + "velocityX": -2.650439233245006, + "velocityY": 1.6043797635605452, + "timestamp": 8.208601167217362 + }, + { + "x": 7.337707687800787, + "y": 3.422034456346363, + "heading": -0.08211008235538154, + "angularVelocity": -1.0978118571669786, + "velocityX": -3.0029412743627657, + "velocityY": 1.5264501829051038, + "timestamp": 8.273442157500993 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": -0.09725138680843892, + "angularVelocity": -0.2335143924672555, + "velocityX": -3.3462025777705335, + "velocityY": 1.4627042932775656, + "timestamp": 8.338283147784624 + }, + { + "x": 7.010606209661033, + "y": 3.566708155420985, + "heading": -0.09725146950362613, + "angularVelocity": -0.000002548325484883459, + "velocityX": -3.39376555274088, + "velocityY": 1.5355711508400656, + "timestamp": 8.370733942512778 + }, + { + "x": 6.900475999605217, + "y": 3.616539055790813, + "heading": -0.09725155219704654, + "angularVelocity": -0.0000025482710392560423, + "velocityX": -3.3937600289421765, + "velocityY": 1.5355833589676455, + "timestamp": 8.403184737240933 + }, + { + "x": 6.790345789587328, + "y": 3.6663699562444614, + "heading": -0.09725163489046874, + "angularVelocity": -0.0000025482710945290222, + "velocityX": -3.3937600277734288, + "velocityY": 1.5355833615506584, + "timestamp": 8.435635531969087 + }, + { + "x": 6.680215579569448, + "y": 3.7162008566981277, + "heading": -0.0972517175838927, + "angularVelocity": -0.0000025482711487845157, + "velocityX": -3.393760027773178, + "velocityY": 1.5355833615512018, + "timestamp": 8.468086326697241 + }, + { + "x": 6.570085369551568, + "y": 3.7660317571517936, + "heading": -0.09725180027731849, + "angularVelocity": -0.0000025482712051303815, + "velocityX": -3.3937600277731743, + "velocityY": 1.5355833615511987, + "timestamp": 8.500537121425396 + }, + { + "x": 6.4599551595336875, + "y": 3.8158626576054595, + "heading": -0.09725188297074605, + "angularVelocity": -0.0000025482712597994537, + "velocityX": -3.3937600277731703, + "velocityY": 1.5355833615511956, + "timestamp": 8.53298791615355 + }, + { + "x": 6.349824949515808, + "y": 3.8656935580591254, + "heading": -0.0972519656641754, + "angularVelocity": -0.0000025482713151989977, + "velocityX": -3.393760027773166, + "velocityY": 1.5355833615511918, + "timestamp": 8.565438710881704 + }, + { + "x": 6.239694739497929, + "y": 3.9155244585127913, + "heading": -0.09725204835760656, + "angularVelocity": -0.00000254827137059863, + "velocityX": -3.3937600277731623, + "velocityY": 1.5355833615511885, + "timestamp": 8.597889505609858 + }, + { + "x": 6.129564529480049, + "y": 3.9653553589664567, + "heading": -0.09725213105103951, + "angularVelocity": -0.000002548271425616177, + "velocityX": -3.3937600277731588, + "velocityY": 1.535583361551185, + "timestamp": 8.630340300338013 + }, + { + "x": 6.019434319462169, + "y": 4.015186259420122, + "heading": -0.09725221374447425, + "angularVelocity": -0.0000025482714812860075, + "velocityX": -3.393760027773155, + "velocityY": 1.5355833615511818, + "timestamp": 8.662791095066167 + }, + { + "x": 5.90930410944429, + "y": 4.065017159873787, + "heading": -0.09725229643791082, + "angularVelocity": -0.0000025482715370955637, + "velocityX": -3.3937600277731512, + "velocityY": 1.5355833615511787, + "timestamp": 8.695241889794321 + }, + { + "x": 5.79917389942641, + "y": 4.114848060327453, + "heading": -0.09725237913134915, + "angularVelocity": -0.0000025482715921357367, + "velocityX": -3.3937600277731477, + "velocityY": 1.5355833615511751, + "timestamp": 8.727692684522475 + }, + { + "x": 5.689043689408531, + "y": 4.164678960781118, + "heading": -0.09725246182478928, + "angularVelocity": -0.0000025482716465928655, + "velocityX": -3.3937600277731437, + "velocityY": 1.5355833615511723, + "timestamp": 8.76014347925063 + }, + { + "x": 5.5789134793906525, + "y": 4.2145098612347835, + "heading": -0.09725254451823122, + "angularVelocity": -0.0000025482717030707046, + "velocityX": -3.39376002777314, + "velocityY": 1.535583361551169, + "timestamp": 8.792594273978784 + }, + { + "x": 5.468783269372774, + "y": 4.264340761688448, + "heading": -0.09725262721167492, + "angularVelocity": -0.000002548271757487181, + "velocityX": -3.3937600277731357, + "velocityY": 1.535583361551166, + "timestamp": 8.825045068706938 + }, + { + "x": 5.358653059354908, + "y": 4.314171662142141, + "heading": -0.09725270990512043, + "angularVelocity": -0.000002548271812724623, + "velocityX": -3.3937600277727378, + "velocityY": 1.5355833615520338, + "timestamp": 8.857495863435092 + }, + { + "x": 5.248522849397515, + "y": 4.364002562729485, + "heading": -0.09725279259856773, + "angularVelocity": -0.000002548271867828041, + "velocityX": -3.393760025909188, + "velocityY": 1.5355833656706142, + "timestamp": 8.889946658163247 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.000002548289809515153, + "velocityX": -3.3937512180408453, + "velocityY": 1.5356028315849852, + "timestamp": 8.922397452891401 + }, + { + "x": 4.9137282295128415, + "y": 4.535640155229253, + "heading": -0.09725287529313524, + "angularVelocity": -7.495790555066974e-12, + "velocityX": -3.1302347498710463, + "velocityY": 1.6971138308958609, + "timestamp": 8.994169928025691 + }, + { + "x": 4.717146626097375, + "y": 4.642220517268037, + "heading": -0.09725287529322811, + "angularVelocity": -1.2941727290342862e-12, + "velocityX": -2.738955331380901, + "velocityY": 1.4849754288028527, + "timestamp": 9.065942403159982 + }, + { + "x": 4.5486481109529855, + "y": 4.733575136181824, + "heading": -0.09725287529316204, + "angularVelocity": 9.207220397125275e-13, + "velocityX": -2.347675969501116, + "velocityY": 1.2728364006237491, + "timestamp": 9.137714878294272 + }, + { + "x": 4.408232682725307, + "y": 4.809703996992033, + "heading": -0.09725287529303236, + "angularVelocity": 1.806653851545192e-12, + "velocityX": -1.9563966264916148, + "velocityY": 1.0606971637493046, + "timestamp": 9.209487353428562 + }, + { + "x": 4.2959003407371545, + "y": 4.870607092209374, + "heading": -0.09725287529288679, + "angularVelocity": 2.028301137290635e-12, + "velocityX": -1.5651172929172836, + "velocityY": 0.8485578225272127, + "timestamp": 9.281259828562852 + }, + { + "x": 4.211651084582215, + "y": 4.916284417340276, + "heading": -0.09725287529275393, + "angularVelocity": 1.850997831523441e-12, + "velocityX": -1.1738379650040627, + "velocityY": 0.6364184186965437, + "timestamp": 9.353032303697143 + }, + { + "x": 4.155484913989614, + "y": 4.946735969389024, + "heading": -0.09725287529265289, + "angularVelocity": 1.4078899342405594e-12, + "velocityX": -0.7825586408649186, + "velocityY": 0.424278973126829, + "timestamp": 9.424804778831433 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 7.752391774433126e-13, + "velocityX": -0.3912793194215449, + "velocityY": 0.2121394977435137, + "timestamp": 9.496577253965723 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -8.979320165657004e-28, + "velocityX": -3.317577006729804e-27, + "velocityY": -2.7701124132227202e-27, + "timestamp": 9.568349729100014 + }, + { + "x": 4.149345287192321, + "y": 4.948730090302555, + "heading": -0.09725287542061174, + "angularVelocity": -1.9914932525911006e-9, + "velocityX": 0.34136955009475556, + "velocityY": -0.20584195701235133, + "timestamp": 9.632630384537388 + }, + { + "x": 4.193331867161711, + "y": 4.922433486987429, + "heading": -0.09725287565526207, + "angularVelocity": -3.6504037180002943e-9, + "velocityX": 0.6842895373437039, + "velocityY": -0.4090904664272751, + "timestamp": 9.696911039974761 + }, + { + "x": 4.259487364434434, + "y": 4.883286547964955, + "heading": -0.09725287596919822, + "angularVelocity": -4.883835550017299e-9, + "velocityX": 1.02916650153289, + "velocityY": -0.6090003089749592, + "timestamp": 9.761191695412135 + }, + { + "x": 4.347975400700828, + "y": 4.831575784808056, + "heading": -0.09725287632620161, + "angularVelocity": -5.55382285919441e-9, + "velocityX": 1.3765888923239984, + "velocityY": -0.8044529540816497, + "timestamp": 9.825472350849509 + }, + { + "x": 4.45901715765165, + "y": 4.76770271955414, + "heading": -0.09725287667606397, + "angularVelocity": -5.442731637489258e-9, + "velocityX": 1.7274521579669766, + "velocityY": -0.9936592092802461, + "timestamp": 9.889753006286883 + }, + { + "x": 4.592927185358519, + "y": 4.69226963680943, + "heading": -0.09725287694462323, + "angularVelocity": -4.177917358984547e-9, + "velocityX": 2.083208809800217, + "velocityY": -1.1734958555019974, + "timestamp": 9.954033661724257 + }, + { + "x": 4.750184246245984, + "y": 4.606276833638444, + "heading": -0.09725287701156489, + "angularVelocity": -1.0413967703177253e-9, + "velocityX": 2.446413463233511, + "velocityY": -1.3377711005881947, + "timestamp": 10.01831431716163 + }, + { + "x": 4.931581310093376, + "y": 4.511690334370361, + "heading": -0.0972528766497097, + "angularVelocity": 5.629301248871626e-9, + "velocityX": 2.821954172886779, + "velocityY": -1.4714613381662662, + "timestamp": 10.082594972599004 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 2.1112299822496666e-8, + "velocityX": 3.2173227507078654, + "velocityY": -1.5223279648179473, + "timestamp": 10.146875628036378 + }, + { + "x": 5.398868955921396, + "y": 4.32613897098341, + "heading": -0.09725286997797288, + "angularVelocity": 7.20303645148975e-8, + "velocityX": 3.5302934161705415, + "velocityY": -1.1885528129676433, + "timestamp": 10.220658738943204 + }, + { + "x": 5.6681314885139065, + "y": 4.271040046607528, + "heading": -0.09725286670603911, + "angularVelocity": 4.4345294143454955e-8, + "velocityX": 3.6493789606207083, + "velocityY": -0.7467687889368465, + "timestamp": 10.29444184985003 + }, + { + "x": 5.942117714678701, + "y": 4.249366302589977, + "heading": -0.0972528635577222, + "angularVelocity": 4.26698858409276e-8, + "velocityX": 3.713400299843557, + "velocityY": -0.29374939266142885, + "timestamp": 10.368224960756855 + }, + { + "x": 6.216382330790907, + "y": 4.231558335942029, + "heading": -0.09725286041527656, + "angularVelocity": 4.259031080949294e-8, + "velocityX": 3.7171733848218165, + "velocityY": -0.24135559519084493, + "timestamp": 10.44200807166368 + }, + { + "x": 6.4906469514050675, + "y": 4.21375043862986, + "heading": -0.09725285727283085, + "angularVelocity": 4.259031210936323e-8, + "velocityX": 3.717173445837876, + "velocityY": -0.241354655466577, + "timestamp": 10.515791182570506 + }, + { + "x": 6.764911572019293, + "y": 4.195942541318692, + "heading": -0.09725285413038519, + "angularVelocity": 4.259031138372869e-8, + "velocityX": 3.717173445838756, + "velocityY": -0.24135465545301274, + "timestamp": 10.589574293477332 + }, + { + "x": 7.039176191804645, + "y": 4.178134631242245, + "heading": -0.09725285098790416, + "angularVelocity": 4.2590790558228534e-8, + "velocityX": 3.717173434604845, + "velocityY": -0.24135482846385797, + "timestamp": 10.663357404384158 + }, + { + "x": 7.303151625715878, + "y": 4.160355738175747, + "heading": -0.07493794888540253, + "angularVelocity": 0.3024391602392781, + "velocityX": 3.5777216583424782, + "velocityY": -0.24096155404655678, + "timestamp": 10.737140515290983 + }, + { + "x": 7.533445930864736, + "y": 4.144763761571347, + "heading": -0.05552477724704528, + "angularVelocity": 0.26311131910489216, + "velocityX": 3.1212333326481616, + "velocityY": -0.21132175660214023, + "timestamp": 10.810923626197809 + }, + { + "x": 7.730059092320318, + "y": 4.131358730965977, + "heading": -0.03901370974052792, + "angularVelocity": 0.22377841356361258, + "velocityX": 2.664744804591801, + "velocityY": -0.1816815588366564, + "timestamp": 10.884706737104635 + }, + { + "x": 7.892991106532594, + "y": 4.120140669647355, + "heading": -0.02540508118930727, + "angularVelocity": 0.18444097008061852, + "velocityX": 2.20825622842103, + "velocityY": -0.15204104544723035, + "timestamp": 10.95848984801146 + }, + { + "x": 8.02224197249394, + "y": 4.111109595697515, + "heading": -0.014699203415963554, + "angularVelocity": 0.14509930039224503, + "velocityX": 1.7517676385937064, + "velocityY": -0.12240028698767837, + "timestamp": 11.032272958918286 + }, + { + "x": 8.117811689845258, + "y": 4.104265522227424, + "heading": -0.0068963232026660135, + "angularVelocity": 0.10575428600660669, + "velocityX": 1.2952790438994197, + "velocityY": -0.09275935083211628, + "timestamp": 11.106056069825112 + }, + { + "x": 8.179700258175622, + "y": 4.099608457473234, + "heading": -0.001996583897562948, + "angularVelocity": 0.06640732878951901, + "velocityX": 0.8387904436357886, + "velocityY": -0.06311830305000711, + "timestamp": 11.179839180731937 + }, + { + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": 5.455800259144416e-28, + "angularVelocity": 0.027060175059361254, + "velocityX": 0.38230183268875023, + "velocityY": -0.03347720903449647, + "timestamp": 11.253622291638763 + }, + { + "x": 8.200639571345327, + "y": 4.096943687462525, + "heading": -0.0010802899918473188, + "angularVelocity": -0.0140223298346035, + "velocityX": -0.0943411225502494, + "velocityY": -0.002527461513954009, + "timestamp": 11.330662983491656 + }, + { + "x": 8.15665056358688, + "y": 4.099133352364744, + "heading": -0.0053255986809540795, + "angularVelocity": -0.05510475810903991, + "velocityX": -0.5709840695932866, + "velocityY": 0.028422186373926028, + "timestamp": 11.40770367534455 + }, + { + "x": 8.075940653456616, + "y": 4.103707386175576, + "heading": -0.012735784669566218, + "angularVelocity": -0.09618535101893501, + "velocityX": -1.0476270161796533, + "velocityY": 0.05937166062275022, + "timestamp": 11.484744367197443 + }, + { + "x": 7.958509840643352, + "y": 4.110665769785427, + "heading": -0.023310616019513113, + "angularVelocity": -0.13726293333579098, + "velocityX": -1.5242699668052715, + "velocityY": 0.0903208868261235, + "timestamp": 11.561785059050337 + }, + { + "x": 7.804358124937036, + "y": 4.120008478318513, + "heading": -0.037049836652876506, + "angularVelocity": -0.17833719172197993, + "velocityX": -2.000912920157348, + "velocityY": 0.12126979013798393, + "timestamp": 11.63882575090323 + }, + { + "x": 7.613485507099167, + "y": 4.131735481105862, + "heading": -0.05395325351914213, + "angularVelocity": -0.21940894428287547, + "velocityX": -2.477555863625089, + "velocityY": 0.15221829536189968, + "timestamp": 11.715866442756123 + }, + { + "x": 7.385891991055361, + "y": 4.145846741676364, + "heading": -0.07402084166347275, + "angularVelocity": -0.2604803729261552, + "velocityX": -2.954198756137707, + "velocityY": 0.18316632718520298, + "timestamp": 11.792907134609017 + }, + { + "x": 7.121577593711165, + "y": 4.162342217760912, + "heading": -0.09725284930983928, + "angularVelocity": -0.30155502355466035, + "velocityX": -3.4308414292137566, + "velocityY": 0.214113810349038, + "timestamp": 11.86994782646191 + }, + { + "x": 6.835203978246458, + "y": 4.180936324910101, + "heading": -0.09725285259123628, + "angularVelocity": -4.2593036391257635e-8, + "velocityX": -3.717173464790893, + "velocityY": 0.24135436354456943, + "timestamp": 11.946988518314804 + }, + { + "x": 6.5488303639215975, + "y": 4.199530449615619, + "heading": -0.09725285587256693, + "angularVelocity": -4.259217541458275e-8, + "velocityX": -3.717173449995519, + "velocityY": 0.2413545914284215, + "timestamp": 12.024029210167697 + }, + { + "x": 6.262456749596831, + "y": 4.218124574322583, + "heading": -0.0972528591538976, + "angularVelocity": -4.259217545578449e-8, + "velocityX": -3.7171734499942994, + "velocityY": 0.2413545914471941, + "timestamp": 12.10106990202059 + }, + { + "x": 5.976083139919176, + "y": 4.236718770600949, + "heading": -0.09725286243522821, + "angularVelocity": -4.259217464314882e-8, + "velocityX": -3.717173389674077, + "velocityY": 0.24135552045497397, + "timestamp": 12.178110593873484 + }, + { + "x": 5.690024921889411, + "y": 4.259659658304738, + "heading": -0.09725286572349409, + "angularVelocity": -4.268219562622653e-8, + "velocityX": -3.71307955769639, + "velocityY": 0.29777624203574676, + "timestamp": 12.255151285726377 + }, + { + "x": 5.4092552652272206, + "y": 4.319022844462834, + "heading": -0.09725286915389227, + "angularVelocity": -4.452709474069528e-8, + "velocityX": -3.6444332197627625, + "velocityY": 0.7705432639604031, + "timestamp": 12.33219197757927 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -7.968133230975529e-8, + "velocityX": -3.5158347290283003, + "velocityY": 1.2306645781352268, + "timestamp": 12.409232669432164 + }, + { + "x": 4.909018653493503, + "y": 4.524275328875486, + "heading": -0.09725287687080802, + "angularVelocity": -2.1964896173308048e-8, + "velocityX": -3.192337867898694, + "velocityY": 1.5370761958403907, + "timestamp": 12.481084171767012 + }, + { + "x": 4.71108590609518, + "y": 4.628682093882472, + "heading": -0.09725287720527087, + "angularVelocity": -4.654917836145884e-9, + "velocityX": -2.754747513502237, + "velocityY": 1.4530909113134631, + "timestamp": 12.55293567410186 + }, + { + "x": 4.54294597125426, + "y": 4.721368217002082, + "heading": -0.09725287703485491, + "angularVelocity": 2.3717800410462983e-9, + "velocityX": -2.3401032598781417, + "velocityY": 1.2899677822694202, + "timestamp": 12.624787176436708 + }, + { + "x": 4.40369795049572, + "y": 4.80026148816612, + "heading": -0.0972528766496691, + "angularVelocity": 5.3608595378330946e-9, + "velocityX": -1.9379973449908554, + "velocityY": 1.0980044759032759, + "timestamp": 12.696638678771556 + }, + { + "x": 4.292814205558175, + "y": 4.864307082758427, + "heading": -0.09725287620265412, + "angularVelocity": 6.221372869321198e-9, + "velocityX": -1.5432348849269113, + "velocityY": 0.8913605493429585, + "timestamp": 12.768490181106404 + }, + { + "x": 4.209952469660844, + "y": 4.91286872561946, + "heading": -0.09725287578795568, + "angularVelocity": 5.771604384474185e-9, + "velocityX": -1.1532359547775561, + "velocityY": 0.6758612037744437, + "timestamp": 12.840341683441252 + }, + { + "x": 4.154873707750821, + "y": 4.945521497298995, + "heading": -0.09725287546928488, + "angularVelocity": 4.4351304984872785e-9, + "velocityX": -0.7665638173206214, + "velocityY": 0.4544480020384788, + "timestamp": 12.9121931857761 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 2.459066716778208e-9, + "velocityX": -0.38234244368231685, + "velocityY": 0.22880870103743695, + "timestamp": 12.984044688110949 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -3.871307616700285e-29, + "velocityX": 3.757481365144797e-28, + "velocityY": -1.0154842203400723e-27, + "timestamp": 13.055896190445797 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanephChaos.1.traj b/src/main/deploy/choreo/SourceLanephChaos.1.traj new file mode 100644 index 00000000..308ed012 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanephChaos.1.traj @@ -0,0 +1,1481 @@ +{ + "samples": [ + { + "x": 1.4578553438186646, + "y": 1.4268364906311035, + "heading": 1.9268906266143656e-30, + "angularVelocity": 1.1190631191470915e-30, + "velocityX": -2.439449667203012e-26, + "velocityY": -3.284743628951617e-25, + "timestamp": 0 + }, + { + "x": 1.4836061256801238, + "y": 1.4248385832903434, + "heading": -1.6036305967396036e-15, + "angularVelocity": -2.484855550358542e-14, + "velocityX": 0.3990131727631876, + "velocityY": -0.030957947265928064, + "timestamp": 0.0645361697788919 + }, + { + "x": 1.5351076891841593, + "y": 1.420842768625806, + "heading": -4.81143470802221e-15, + "angularVelocity": -4.970552361990032e-14, + "velocityX": 0.7980263421347389, + "velocityY": -0.061915894268711684, + "timestamp": 0.1290723395577838 + }, + { + "x": 1.6123600340493498, + "y": 1.4148490466593255, + "heading": -9.625413675640745e-15, + "angularVelocity": -7.459350290095871e-14, + "velocityX": 1.1970395071456144, + "velocityY": -0.09287384093316672, + "timestamp": 0.1936085093366757 + }, + { + "x": 1.7153631599004666, + "y": 1.4068574174200144, + "heading": -1.6044392495578035e-14, + "angularVelocity": -9.946327527539101e-14, + "velocityX": 1.5960526663422558, + "velocityY": -0.12383178714651694, + "timestamp": 0.2581446791155676 + }, + { + "x": 1.8441170662121902, + "y": 1.39686788094863, + "heading": -2.407162986647994e-14, + "angularVelocity": -1.2438354179375857e-13, + "velocityX": 1.9950658173989684, + "velocityY": -0.15478973272832033, + "timestamp": 0.32268084889445947 + }, + { + "x": 1.9986217521965406, + "y": 1.3848804373063093, + "heading": -3.3704756925123664e-14, + "angularVelocity": -1.4926710233420884e-13, + "velocityX": 2.3940789562457856, + "velocityY": -0.1857476773628035, + "timestamp": 0.38721701867335134 + }, + { + "x": 2.1788772165402177, + "y": 1.3708950865949459, + "heading": -4.494558790872857e-14, + "angularVelocity": -1.7417877481910134e-13, + "velocityX": 2.793092074742775, + "velocityY": -0.2167056204184192, + "timestamp": 0.4517531884522432 + }, + { + "x": 2.3848834566166217, + "y": 1.3549118290183289, + "heading": -5.77927518216841e-14, + "angularVelocity": -1.9906920347103564e-13, + "velocityX": 3.1921051525401003, + "velocityY": -0.2476635603162993, + "timestamp": 0.5162893582311351 + }, + { + "x": 2.6166404645459465, + "y": 1.3369306651878214, + "heading": -7.224642245471218e-14, + "angularVelocity": -2.239623250426539e-13, + "velocityX": 3.5911181082383807, + "velocityY": -0.27862149074096887, + "timestamp": 0.580825528010027 + }, + { + "x": 2.856317459743021, + "y": 1.3183350190196794, + "heading": -7.226675860496505e-14, + "angularVelocity": -3.151124450451814e-16, + "velocityX": 3.7138397896595787, + "velocityY": -0.2881430092900255, + "timestamp": 0.6453616977889188 + }, + { + "x": 3.095994472503662, + "y": 1.2997395992279053, + "heading": 8.152675815601398e-24, + "angularVelocity": 1.119786917331966e-12, + "velocityX": 3.7138400618103087, + "velocityY": -0.28813950154594287, + "timestamp": 0.7098978675678107 + }, + { + "x": 3.27382254607751, + "y": 1.289661887920689, + "heading": 0.031144225689139507, + "angularVelocity": 0.6214983848668802, + "velocityX": 3.548646918156489, + "velocityY": -0.20110569975653228, + "timestamp": 0.7600093826537434 + }, + { + "x": 3.4512411724559873, + "y": 1.2794797717459663, + "heading": 0.06298682604506026, + "angularVelocity": 0.6354347958012462, + "velocityX": 3.5404761974215813, + "velocityY": -0.20318915038314128, + "timestamp": 0.8101208977396761 + }, + { + "x": 3.62822231090223, + "y": 1.2691838802186675, + "heading": 0.09560610924482929, + "angularVelocity": 0.6509338850328615, + "velocityX": 3.5317459099520483, + "velocityY": -0.20545959366110889, + "timestamp": 0.8602324128256088 + }, + { + "x": 3.8047300835293036, + "y": 1.258765647314116, + "heading": 0.12910425796921293, + "angularVelocity": 0.6684720800586451, + "velocityX": 3.5222996615526942, + "velocityY": -0.20790097618653766, + "timestamp": 0.9103439279115415 + }, + { + "x": 3.9807203074364863, + "y": 1.248215472268555, + "heading": 0.16360694737365913, + "angularVelocity": 0.6885181847980438, + "velocityX": 3.5119717215771487, + "velocityY": -0.21053394668808642, + "timestamp": 0.9604554429974742 + }, + { + "x": 4.156137440439902, + "y": 1.2375224650571883, + "heading": 0.19927204657899403, + "angularVelocity": 0.7117146457091811, + "velocityX": 3.50053540992732, + "velocityY": -0.2133842330057237, + "timestamp": 1.010566958083407 + }, + { + "x": 4.330909871790797, + "y": 1.2266740963817457, + "heading": 0.23630304667549967, + "angularVelocity": 0.7389718716946344, + "velocityX": 3.487670070465641, + "velocityY": -0.21648454765016575, + "timestamp": 1.0606784731693397 + }, + { + "x": 4.504942288680689, + "y": 1.2156556997827002, + "heading": 0.27497084629517315, + "angularVelocity": 0.7716350134966949, + "velocityX": 3.4729027168996156, + "velocityY": -0.21987753872839294, + "timestamp": 1.1107899882552725 + }, + { + "x": 4.678102490303239, + "y": 1.204449734224157, + "heading": 0.31565141892272963, + "angularVelocity": 0.8118008916273269, + "velocityX": 3.4554972310378296, + "velocityY": -0.22362056982965403, + "timestamp": 1.1609015033412053 + }, + { + "x": 4.850196619661247, + "y": 1.1930346345230565, + "heading": 0.3588966354181107, + "angularVelocity": 0.8629796249668915, + "velocityX": 3.4342232331809583, + "velocityY": -0.22779394479542386, + "timestamp": 1.2110130184271382 + }, + { + "x": 5.020916905370504, + "y": 1.1813828932131683, + "heading": 0.40558384210290876, + "angularVelocity": 0.931666236886622, + "velocityX": 3.4068075055503675, + "velocityY": -0.23251624481733327, + "timestamp": 1.261124533513071 + }, + { + "x": 5.189710400965398, + "y": 1.16945753417408, + "heading": 0.45729195406784345, + "angularVelocity": 1.0318608782086127, + "velocityX": 3.3683574584692115, + "velocityY": -0.2379764215597622, + "timestamp": 1.3112360485990038 + }, + { + "x": 5.35533172203286, + "y": 1.1572047491345996, + "heading": 0.5175844077279528, + "angularVelocity": 1.2031656507834203, + "velocityX": 3.3050551511653112, + "velocityY": -0.244510368893633, + "timestamp": 1.3613475636849366 + }, + { + "x": 5.512680992157926, + "y": 1.1445116485154183, + "heading": 0.6010844246558354, + "angularVelocity": 1.6662840224386364, + "velocityX": 3.1399822945931812, + "velocityY": -0.25329708346305213, + "timestamp": 1.4114590787708694 + }, + { + "x": 5.678954750693719, + "y": 1.128816110929195, + "heading": 0.6579086700766517, + "angularVelocity": 1.1339558447469087, + "velocityX": 3.3180748626470646, + "velocityY": -0.3132121940298074, + "timestamp": 1.4615705938568022 + }, + { + "x": 5.854338980055963, + "y": 1.1023762612022525, + "heading": 0.6848690400817262, + "angularVelocity": 0.5380074810917644, + "velocityX": 3.4998788015387303, + "velocityY": -0.5276202422058656, + "timestamp": 1.511682108942735 + }, + { + "x": 6.029328591050374, + "y": 1.0604680679443284, + "heading": 0.7046293616016649, + "angularVelocity": 0.39432696229705316, + "velocityX": 3.4920039973713513, + "velocityY": -0.8362986668046034, + "timestamp": 1.5617936240286678 + }, + { + "x": 6.202812194824219, + "y": 1.003179669380188, + "heading": 0.7165423509110156, + "angularVelocity": 0.2377295775017328, + "velocityX": 3.4619508804782524, + "velocityY": -1.1432182496558028, + "timestamp": 1.6119051391146006 + }, + { + "x": 6.402547568535553, + "y": 0.9154521762914801, + "heading": 0.7165423052284949, + "angularVelocity": -7.80040343805035e-7, + "velocityX": 3.4105309321224193, + "velocityY": -1.4979686533092873, + "timestamp": 1.6704694441118393 + }, + { + "x": 6.592790671542091, + "y": 0.8086912505756066, + "heading": 0.7165422622385289, + "angularVelocity": -7.34064308321193e-7, + "velocityX": 3.2484480609051705, + "velocityY": -1.8229692253823835, + "timestamp": 1.729033749109078 + }, + { + "x": 6.792526057186022, + "y": 0.7209637957417591, + "heading": 0.7165421996040524, + "angularVelocity": -0.0000010694991862536609, + "velocityX": 3.410531135874468, + "velocityY": -1.4979680000980806, + "timestamp": 1.7875980541063166 + }, + { + "x": 6.995358135542775, + "y": 0.6540115645265273, + "heading": 0.702913894528627, + "angularVelocity": -0.23270668158817906, + "velocityX": 3.4634079302455083, + "velocityY": -1.1432259158268625, + "timestamp": 1.8461623591035552 + }, + { + "x": 7.192981165480996, + "y": 0.6064556135405038, + "heading": 0.6594934468717042, + "angularVelocity": -0.7414148884541547, + "velocityX": 3.3744621394813583, + "velocityY": -0.8120296311595578, + "timestamp": 1.9047266641007938 + }, + { + "x": 7.3826916988169575, + "y": 0.5778864686152094, + "heading": 0.5928355536169142, + "angularVelocity": -1.138200022316206, + "velocityX": 3.239354301991779, + "velocityY": -0.4878252192464604, + "timestamp": 1.9632909690980325 + }, + { + "x": 7.56045020150032, + "y": 0.5667919314464598, + "heading": 0.5348000913331561, + "angularVelocity": -0.9909698798012667, + "velocityX": 3.0352704209798658, + "velocityY": -0.18944196758201154, + "timestamp": 2.021855274095271 + }, + { + "x": 7.725161762075186, + "y": 0.5716432246000058, + "heading": 0.4994377275755845, + "angularVelocity": -0.6038211118400366, + "velocityX": 2.812490655914593, + "velocityY": 0.08283703108531969, + "timestamp": 2.0804195790925095 + }, + { + "x": 7.876544251370993, + "y": 0.5914886747454028, + "heading": 0.49476791383000396, + "angularVelocity": -0.07973822528587567, + "velocityX": 2.5848934654470064, + "velocityY": 0.3388659721366544, + "timestamp": 2.138983884089748 + }, + { + "x": 8.014557697135269, + "y": 0.6256824372518744, + "heading": 0.5263224795620095, + "angularVelocity": 0.5388020182855987, + "velocityX": 2.35661373887698, + "velocityY": 0.5838669562984516, + "timestamp": 2.197548189086987 + }, + { + "x": 8.139220711960105, + "y": 0.6736709753190644, + "heading": 0.5988363569407734, + "angularVelocity": 1.238192400339819, + "velocityX": 2.128651826922783, + "velocityY": 0.8194161626173599, + "timestamp": 2.2561124940842254 + }, + { + "x": 8.25048828125, + "y": 0.7348634600639343, + "heading": 0.7165423509110156, + "angularVelocity": 2.0098589742641386, + "velocityX": 1.8999212796112497, + "velocityY": 1.044876819553399, + "timestamp": 2.314676799081464 + }, + { + "x": 8.302330173893822, + "y": 0.7683170831186208, + "heading": 0.7866903105882892, + "angularVelocity": 2.4170681239593663, + "velocityX": 1.7863012234653046, + "velocityY": 1.152701893090581, + "timestamp": 2.343698719887885 + }, + { + "x": 8.350761252955918, + "y": 0.804862507674631, + "heading": 0.8681048778617475, + "angularVelocity": 2.805278389962577, + "velocityX": 1.6687757983055662, + "velocityY": 1.2592352105076432, + "timestamp": 2.372720640694306 + }, + { + "x": 8.395659126629422, + "y": 0.8444668269434722, + "heading": 0.9600582095499061, + "angularVelocity": 3.168409572250435, + "velocityX": 1.5470331537659474, + "velocityY": 1.36463466815329, + "timestamp": 2.4017425615007273 + }, + { + "x": 8.436895225190787, + "y": 0.8871049250141315, + "heading": 1.0616235787520496, + "angularVelocity": 3.499608791561171, + "velocityX": 1.420860419143676, + "velocityY": 1.469168714057877, + "timestamp": 2.4307644823071484 + }, + { + "x": 8.474338814577639, + "y": 0.9327584709704907, + "heading": 1.1716968936719283, + "angularVelocity": 3.792764636568266, + "velocityX": 1.2901830184364116, + "velocityY": 1.5730711368442005, + "timestamp": 2.4597864031135694 + }, + { + "x": 8.50785937445302, + "y": 0.9814068916097549, + "heading": 1.2890700861945108, + "angularVelocity": 4.0442944250820885, + "velocityX": 1.1550083159196811, + "velocityY": 1.676264674683452, + "timestamp": 2.4888083239199905 + }, + { + "x": 8.537323038411628, + "y": 1.0330097209040148, + "heading": 1.4125130240648722, + "angularVelocity": 4.253437899363618, + "velocityX": 1.0152210170765539, + "velocityY": 1.7780638862070914, + "timestamp": 2.5178302447264116 + }, + { + "x": 8.56258232091714, + "y": 1.0874855013153344, + "heading": 1.5407726955584362, + "angularVelocity": 4.419406708090351, + "velocityX": 0.870351851415892, + "velocityY": 1.877056338713015, + "timestamp": 2.5468521655328327 + }, + { + "x": 8.583466101858683, + "y": 1.1446918380868547, + "heading": 1.6724319994758665, + "angularVelocity": 4.536546867301084, + "velocityX": 0.7195864491822832, + "velocityY": 1.9711423359291682, + "timestamp": 2.5758740863392537 + }, + { + "x": 8.599781102269066, + "y": 1.2044006523964297, + "heading": 1.8056614857101974, + "angularVelocity": 4.590650188972121, + "velocityX": 0.5621612890203437, + "velocityY": 2.057369486597329, + "timestamp": 2.604896007145675 + }, + { + "x": 8.61133776585303, + "y": 1.2662515535930832, + "heading": 1.9379574799883315, + "angularVelocity": 4.558485124418899, + "velocityX": 0.3982046419686604, + "velocityY": 2.131178759986453, + "timestamp": 2.633917927952096 + }, + { + "x": 8.61803446841016, + "y": 1.329807234083184, + "heading": 2.0667068830619315, + "angularVelocity": 4.43628124865926, + "velocityX": 0.23074635899526613, + "velocityY": 2.189919851067852, + "timestamp": 2.662939848758517 + }, + { + "x": 8.619867506604743, + "y": 1.394964606545626, + "heading": 2.191275336084359, + "angularVelocity": 4.292219452093143, + "velocityX": 0.06316047124545049, + "velocityY": 2.245108891897538, + "timestamp": 2.691961769564938 + }, + { + "x": 8.616839586460362, + "y": 1.4616264782251567, + "heading": 2.311063053299335, + "angularVelocity": 4.12749100977746, + "velocityX": -0.10433217582589409, + "velocityY": 2.2969489898401783, + "timestamp": 2.720983690371359 + }, + { + "x": 8.608949954788589, + "y": 1.5296853158108266, + "heading": 2.4255531996922417, + "angularVelocity": 3.9449541316223122, + "velocityX": -0.2718507752949118, + "velocityY": 2.345083843334451, + "timestamp": 2.75000561117778 + }, + { + "x": 8.596173618614296, + "y": 1.5989753133446671, + "heading": 2.534426417053211, + "angularVelocity": 3.7514132192408356, + "velocityX": -0.44023055053838084, + "velocityY": 2.3875055684980784, + "timestamp": 2.7790275319842013 + }, + { + "x": 8.57842938977896, + "y": 1.6691611767333963, + "heading": 2.6378090883600067, + "angularVelocity": 3.5622270488699845, + "velocityX": -0.6114078028705309, + "velocityY": 2.4183741612719314, + "timestamp": 2.8080494527906223 + }, + { + "x": 8.555564514046702, + "y": 1.7394263904616403, + "heading": 2.737024931725787, + "angularVelocity": 3.4186518538024746, + "velocityX": -0.7878484640892429, + "velocityY": 2.421108313158164, + "timestamp": 2.8370713735970434 + }, + { + "x": 8.527959255725655, + "y": 1.8075255209054102, + "heading": 2.8376495221341176, + "angularVelocity": 3.4671926465345364, + "velocityX": -0.9511864671252386, + "velocityY": 2.3464722027875915, + "timestamp": 2.8660932944034645 + }, + { + "x": 8.499577272044599, + "y": 1.8711589675729616, + "heading": 2.9502161532820788, + "angularVelocity": 3.878676118606718, + "velocityX": -0.9779498700435901, + "velocityY": 2.1925994179362642, + "timestamp": 2.8951152152098856 + }, + { + "x": 8.472091457883874, + "y": 1.93067768486564, + "heading": 3.076977426653027, + "angularVelocity": 4.3677768338097795, + "velocityX": -0.9470708139567483, + "velocityY": 2.0508193682173372, + "timestamp": 2.9241371360163066 + }, + { + "x": 8.445935000391486, + "y": 1.9864273544904458, + "heading": 3.219502150557654, + "angularVelocity": 4.910933526946052, + "velocityX": -0.9012655525750104, + "velocityY": 1.920950373914328, + "timestamp": 2.9531590568227277 + }, + { + "x": 8.421236309719301, + "y": 2.038741873704351, + "heading": 3.37984910584556, + "angularVelocity": 5.52502904123526, + "velocityX": -0.8510356994262128, + "velocityY": 1.8025863816129806, + "timestamp": 2.982180977629149 + }, + { + "x": 8.398072066125595, + "y": 2.08798012126862, + "heading": 3.5600291132631146, + "angularVelocity": 6.208410829158125, + "velocityX": -0.7981636966145051, + "velocityY": 1.6965881718406162, + "timestamp": 3.01120289843557 + }, + { + "x": 8.377585411071777, + "y": 2.1329314708709717, + "heading": 3.745897356817782, + "angularVelocity": 6.404408750007486, + "velocityX": -0.7059027963884773, + "velocityY": 1.5488757585061539, + "timestamp": 3.040224819241991 + }, + { + "x": 8.364981537655835, + "y": 2.1618201695949115, + "heading": 3.87540454879606, + "angularVelocity": 6.50944417189865, + "velocityX": -0.6335108428921614, + "velocityY": 1.4520380580394667, + "timestamp": 3.0601200957906958 + }, + { + "x": 8.354284801682704, + "y": 2.189254221668137, + "heading": 4.007147516190271, + "angularVelocity": 6.621821369092149, + "velocityX": -0.5376520375047442, + "velocityY": 1.3789228818240207, + "timestamp": 3.0800153723394006 + }, + { + "x": 8.345932812421994, + "y": 2.2160755919920243, + "heading": 4.140533212375188, + "angularVelocity": 6.704390153028566, + "velocityX": -0.41979759568875646, + "velocityY": 1.3481275446574927, + "timestamp": 3.0999106488881054 + }, + { + "x": 8.339986678909588, + "y": 2.2433741294975276, + "heading": 4.274319407548544, + "angularVelocity": 6.724520508465367, + "velocityX": -0.2988716189920965, + "velocityY": 1.3721114878033795, + "timestamp": 3.1198059254368102 + }, + { + "x": 8.33607844515623, + "y": 2.2720346987144775, + "heading": 4.407573079371785, + "angularVelocity": 6.697754187886208, + "velocityX": -0.1964402828877463, + "velocityY": 1.4405715420334793, + "timestamp": 3.139701201985515 + }, + { + "x": 8.333720429129459, + "y": 2.3025901920416527, + "heading": 4.540110627397886, + "angularVelocity": 6.661759523756392, + "velocityX": -0.11852139984079614, + "velocityY": 1.5358164664046496, + "timestamp": 3.15959647853422 + }, + { + "x": 8.332603722599888, + "y": 2.3352255445188432, + "heading": 4.671035459898787, + "angularVelocity": 6.580699302188081, + "velocityX": -0.05612922880648972, + "velocityY": 1.640356815211755, + "timestamp": 3.1794917550829247 + }, + { + "x": 8.332608537566271, + "y": 2.3698002053105385, + "heading": 4.797912823563958, + "angularVelocity": 6.377260620357163, + "velocityX": 0.0002420155543911525, + "velocityY": 1.7378326311300352, + "timestamp": 3.1993870316316295 + }, + { + "x": 8.33396980572602, + "y": 2.4053616174482886, + "heading": 4.91661874197118, + "angularVelocity": 5.966537741590232, + "velocityX": 0.06842167568854002, + "velocityY": 1.787429898282315, + "timestamp": 3.2192823081803343 + }, + { + "x": 8.336666983434197, + "y": 2.4421571036885257, + "heading": 5.027955549931603, + "angularVelocity": 5.596142767247511, + "velocityX": 0.13556874676122466, + "velocityY": 1.8494583953211146, + "timestamp": 3.239177584729039 + }, + { + "x": 8.340246531495488, + "y": 2.479675325798265, + "heading": 5.127878006964601, + "angularVelocity": 5.02242111530254, + "velocityX": 0.17991949257545845, + "velocityY": 1.8857854032786403, + "timestamp": 3.259072861277744 + }, + { + "x": 8.344506054620522, + "y": 2.517833586551145, + "heading": 5.215460170451448, + "angularVelocity": 4.402158636621092, + "velocityX": 0.2140972061688483, + "velocityY": 1.9179557851063929, + "timestamp": 3.278968137826449 + }, + { + "x": 8.34934139251709, + "y": 2.556588649749756, + "heading": 5.290290323982317, + "angularVelocity": 3.7612019791572497, + "velocityX": 0.24303949154619145, + "velocityY": 1.9479529778707136, + "timestamp": 3.2988634143751536 + }, + { + "x": 8.360489719138714, + "y": 2.633118600966112, + "heading": 5.386097875540369, + "angularVelocity": 2.5082503010878967, + "velocityX": 0.2918641918155103, + "velocityY": 2.0035609934605225, + "timestamp": 3.3370603804102004 + }, + { + "x": 8.3734961558476, + "y": 2.711837506326731, + "heading": 5.434143906525109, + "angularVelocity": 1.2578494046008082, + "velocityX": 0.3405096807153113, + "velocityY": 2.0608680094746767, + "timestamp": 3.375257346445247 + }, + { + "x": 8.388354991269559, + "y": 2.792730047490086, + "heading": 5.4343775006036195, + "angularVelocity": 0.006115513946711611, + "velocityX": 0.3890056453259781, + "velocityY": 2.117773989932431, + "timestamp": 3.413454312480294 + }, + { + "x": 8.405064352616025, + "y": 2.875700202856259, + "heading": 5.386603131855514, + "angularVelocity": -1.2507372628567064, + "velocityX": 0.43745258016393523, + "velocityY": 2.1721661162838264, + "timestamp": 3.4516512785153406 + }, + { + "x": 8.423595902184939, + "y": 2.960570562063504, + "heading": 5.290443702807007, + "angularVelocity": -2.51746248537848, + "velocityX": 0.4851576314179031, + "velocityY": 2.221913623437374, + "timestamp": 3.4898482445503873 + }, + { + "x": 8.443874909764538, + "y": 3.0470824289690306, + "heading": 5.145320206366309, + "angularVelocity": -3.799346165544723, + "velocityX": 0.5309062390188924, + "velocityY": 2.264888442347729, + "timestamp": 3.528045210585434 + }, + { + "x": 8.46822613284771, + "y": 3.1277535966232044, + "heading": 4.9755072400440925, + "angularVelocity": -4.445718703585181, + "velocityX": 0.6375172064929964, + "velocityY": 2.1119784115878613, + "timestamp": 3.566242176620481 + }, + { + "x": 8.485889521575103, + "y": 3.2052285744044853, + "heading": 4.782579464036899, + "angularVelocity": -5.050866496312229, + "velocityX": 0.4624291026461906, + "velocityY": 2.02830187377173, + "timestamp": 3.6044391426555276 + }, + { + "x": 8.496466363696742, + "y": 3.2817681103248395, + "heading": 4.564012968163081, + "angularVelocity": -5.722090484183368, + "velocityX": 0.2769026762998172, + "velocityY": 2.0038119218716672, + "timestamp": 3.6426361086905743 + }, + { + "x": 8.498864361133254, + "y": 3.3553108945925123, + "heading": 4.355441183612215, + "angularVelocity": -5.460428044455047, + "velocityX": 0.06277978817249102, + "velocityY": 1.9253566945656204, + "timestamp": 3.680833074725621 + }, + { + "x": 8.49244817179446, + "y": 3.427344053675241, + "heading": 4.152515229757634, + "angularVelocity": -5.312619689961475, + "velocityX": -0.16797641291473847, + "velocityY": 1.8858345716944014, + "timestamp": 3.719030040760668 + }, + { + "x": 8.480949544401803, + "y": 3.4955676666301225, + "heading": 3.980239273291126, + "angularVelocity": -4.5102000066822105, + "velocityX": -0.3010350974500928, + "velocityY": 1.7861003120584202, + "timestamp": 3.7572270067957145 + }, + { + "x": 8.465880541095904, + "y": 3.5608797001839374, + "heading": 3.848658427278945, + "angularVelocity": -3.4447983615099473, + "velocityX": -0.39450785939576005, + "velocityY": 1.7098749019461128, + "timestamp": 3.7954239728307613 + }, + { + "x": 8.447723716276025, + "y": 3.6237520788842326, + "heading": 3.7606807673271616, + "angularVelocity": -2.3032630358929707, + "velocityX": -0.4753473038465876, + "velocityY": 1.6460045188564885, + "timestamp": 3.833620938865808 + }, + { + "x": 8.426810965043074, + "y": 3.6844175741296494, + "heading": 3.717839244299634, + "angularVelocity": -1.1215949190367396, + "velocityX": -0.5474977047591038, + "velocityY": 1.5882281118807744, + "timestamp": 3.8718179049008548 + }, + { + "x": 8.403377538803907, + "y": 3.7429507831754503, + "heading": 3.7209535641751312, + "angularVelocity": 0.08153317393427642, + "velocityX": -0.6134892027200903, + "velocityY": 1.5324046677449423, + "timestamp": 3.9100148709359015 + }, + { + "x": 8.377585411071777, + "y": 3.7993156909942623, + "heading": 3.770388538466339, + "angularVelocity": 1.2942120650590392, + "velocityX": -0.6752402195625785, + "velocityY": 1.475638346959163, + "timestamp": 3.9482118369709482 + }, + { + "x": 8.359121603366816, + "y": 3.8364193104219853, + "heading": 3.8251043907760733, + "angularVelocity": 2.1181774191421603, + "velocityX": -0.7147767767674121, + "velocityY": 1.4363670768638492, + "timestamp": 3.974043408875767 + }, + { + "x": 8.339698595709613, + "y": 3.872452168004382, + "heading": 3.901119172271844, + "angularVelocity": 2.9427083173978534, + "velocityX": -0.7519096293779934, + "velocityY": 1.3949154048838457, + "timestamp": 3.999874980780586 + }, + { + "x": 8.31940846552165, + "y": 3.907347005013746, + "heading": 3.9984628782143856, + "angularVelocity": 3.7684004016953554, + "velocityX": -0.7854779516602693, + "velocityY": 1.3508599917163486, + "timestamp": 4.025706552685405 + }, + { + "x": 8.29838582091067, + "y": 3.941024765667179, + "heading": 4.1171731868845285, + "angularVelocity": 4.595551099544112, + "velocityX": -0.8138352822058595, + "velocityY": 1.3037441460211887, + "timestamp": 4.051538124590223 + }, + { + "x": 8.276832604055052, + "y": 3.9733768568490917, + "heading": 4.2572182344953795, + "angularVelocity": 5.421468276374085, + "velocityX": -0.8343749631278609, + "velocityY": 1.2524244092121188, + "timestamp": 4.077369696495042 + }, + { + "x": 8.255105530373472, + "y": 4.004178929302719, + "heading": 4.418037307026107, + "angularVelocity": 6.225678914287337, + "velocityX": -0.841105363685868, + "velocityY": 1.1924195928580044, + "timestamp": 4.103201268399861 + }, + { + "x": 8.236226871427698, + "y": 4.03234435965831, + "heading": 4.584751742520452, + "angularVelocity": 6.45390207412214, + "velocityX": -0.7308366295065699, + "velocityY": 1.0903490681625025, + "timestamp": 4.12903284030468 + }, + { + "x": 8.22111995531124, + "y": 4.060012965907288, + "heading": 4.744248346923677, + "angularVelocity": 6.174483108922656, + "velocityX": -0.5848237254830212, + "velocityY": 1.0711158558576532, + "timestamp": 4.154864412209498 + }, + { + "x": 8.208790974794852, + "y": 4.088137716132201, + "heading": 4.888562116667079, + "angularVelocity": 5.5867204007233076, + "velocityX": -0.4772834019477146, + "velocityY": 1.0887742460483674, + "timestamp": 4.180695984114317 + }, + { + "x": 8.198725383178298, + "y": 4.116963981676649, + "heading": 5.015233890568161, + "angularVelocity": 4.903757865290863, + "velocityX": -0.3896623733794405, + "velocityY": 1.115931529473434, + "timestamp": 4.206527556019136 + }, + { + "x": 8.190656129284886, + "y": 4.146632428651468, + "heading": 5.123285713831358, + "angularVelocity": 4.182936433807978, + "velocityX": -0.31237951461661634, + "velocityY": 1.148534323971415, + "timestamp": 4.232359127923955 + }, + { + "x": 8.184413831367804, + "y": 4.177239551429493, + "heading": 5.212211909192924, + "angularVelocity": 3.4425390637949445, + "velocityX": -0.241653815729148, + "velocityY": 1.184872639218488, + "timestamp": 4.2581906998287735 + }, + { + "x": 8.179879188537598, + "y": 4.208850860595703, + "heading": 5.281700753397833, + "angularVelocity": 2.6900741643192534, + "velocityX": -0.17554653069182086, + "velocityY": 1.223747020997694, + "timestamp": 4.284022271733592 + }, + { + "x": 8.176711633108404, + "y": 4.259079542498045, + "heading": 5.341834535732932, + "angularVelocity": 1.5389696629172125, + "velocityX": -0.08106544311437222, + "velocityY": 1.2854740655637256, + "timestamp": 4.323096324838129 + }, + { + "x": 8.177255357527843, + "y": 4.31178579369655, + "heading": 5.357219600158119, + "angularVelocity": 0.3937411965947536, + "velocityX": 0.013915229576633022, + "velocityY": 1.3488810863182197, + "timestamp": 4.362170377942666 + }, + { + "x": 8.181626457446923, + "y": 4.366954653737556, + "heading": 5.328216932710745, + "angularVelocity": -0.7422487595484647, + "velocityX": 0.11186707218176441, + "velocityY": 1.411905232697765, + "timestamp": 4.401244431047203 + }, + { + "x": 8.190025046391563, + "y": 4.424472904744573, + "heading": 5.255283063236909, + "angularVelocity": -1.8665550071990427, + "velocityX": 0.21494030635039485, + "velocityY": 1.4720318584083123, + "timestamp": 4.4403184841517405 + }, + { + "x": 8.202703493077177, + "y": 4.484103126840556, + "heading": 5.138917873952012, + "angularVelocity": -2.978068053845803, + "velocityX": 0.3244722694032999, + "velocityY": 1.526082332345974, + "timestamp": 4.4793925372562775 + }, + { + "x": 8.219928011035833, + "y": 4.545471721328882, + "heading": 4.979694935972509, + "angularVelocity": -4.074902021388053, + "velocityX": 0.44081728385262037, + "velocityY": 1.570571507494814, + "timestamp": 4.5184665903608145 + }, + { + "x": 8.242002423270923, + "y": 4.608108641713836, + "heading": 4.778755843149729, + "angularVelocity": -5.142519827292898, + "velocityX": 0.5649378674905671, + "velocityY": 1.6030310502311427, + "timestamp": 4.557540643465352 + }, + { + "x": 8.271074244498946, + "y": 4.672945765445566, + "heading": 4.551673202567828, + "angularVelocity": -5.811596764082065, + "velocityX": 0.7440185728940091, + "velocityY": 1.6593396021207916, + "timestamp": 4.596614696569889 + }, + { + "x": 8.307044890775797, + "y": 4.743652286364594, + "heading": 4.33581324080306, + "angularVelocity": -5.524381133108038, + "velocityX": 0.9205762755303734, + "velocityY": 1.8095517434514046, + "timestamp": 4.635688749674426 + }, + { + "x": 8.343074596799024, + "y": 4.821913482728032, + "heading": 4.143267038130374, + "angularVelocity": -4.927725366947583, + "velocityX": 0.9220877580023329, + "velocityY": 2.0028942519492667, + "timestamp": 4.674762802778963 + }, + { + "x": 8.37162830890395, + "y": 4.904380364973872, + "heading": 3.9684903103687414, + "angularVelocity": -4.472961310003866, + "velocityX": 0.7307589010163777, + "velocityY": 2.1105279768446303, + "timestamp": 4.7138368558835 + }, + { + "x": 8.39410907934667, + "y": 4.988084023708393, + "heading": 3.8243195102940244, + "angularVelocity": -3.6896812237268355, + "velocityX": 0.5753375617977339, + "velocityY": 2.142180093541445, + "timestamp": 4.752910908988037 + }, + { + "x": 8.412533295857541, + "y": 5.0701333500179695, + "heading": 3.7241871610957364, + "angularVelocity": -2.5626302173055078, + "velocityX": 0.47152048602634916, + "velocityY": 2.099841705442333, + "timestamp": 4.791984962092574 + }, + { + "x": 8.427022508757116, + "y": 5.150508503599114, + "heading": 3.668854256125942, + "angularVelocity": -1.4161035411852148, + "velocityX": 0.370814178421956, + "velocityY": 2.0569955557492743, + "timestamp": 4.831059015197111 + }, + { + "x": 8.437687476391535, + "y": 5.229173684079572, + "heading": 3.658771745830396, + "angularVelocity": -0.25803594698946425, + "velocityX": 0.27294244612622637, + "velocityY": 2.013233187506785, + "timestamp": 4.870133068301648 + }, + { + "x": 8.444672138848645, + "y": 5.306037054642886, + "heading": 3.694319619572902, + "angularVelocity": 0.9097564987026582, + "velocityX": 0.17875449056755022, + "velocityY": 1.9671204918946052, + "timestamp": 4.909207121406185 + }, + { + "x": 8.44819450378418, + "y": 5.38096809387207, + "heading": 3.7759750997570256, + "angularVelocity": 2.0897622257323873, + "velocityX": 0.09014588085116931, + "velocityY": 1.9176674359508388, + "timestamp": 4.948281174510722 + }, + { + "x": 8.449106377133136, + "y": 5.428397890121427, + "heading": 3.847877974347001, + "angularVelocity": 2.856844558406781, + "velocityX": 0.03623054613292597, + "velocityY": 1.8844803645751356, + "timestamp": 4.973449808337294 + }, + { + "x": 8.448712080913069, + "y": 5.474928555589069, + "heading": 3.9390652949611624, + "angularVelocity": 3.623054045861213, + "velocityX": -0.015666174921664548, + "velocityY": 1.848756106043276, + "timestamp": 4.998618442163867 + }, + { + "x": 8.447075648100045, + "y": 5.520495348927803, + "heading": 4.049507624303278, + "angularVelocity": 4.388093930847932, + "velocityX": -0.06501873817623247, + "velocityY": 1.8104595447141671, + "timestamp": 5.023787075990439 + }, + { + "x": 8.444260693041544, + "y": 5.565024011584617, + "heading": 4.179082223329722, + "angularVelocity": 5.148257148929752, + "velocityX": -0.11184377657913579, + "velocityY": 1.7692125430265968, + "timestamp": 5.048955709817012 + }, + { + "x": 8.440260130020729, + "y": 5.60821579730256, + "heading": 4.326669444296132, + "angularVelocity": 5.86393452991448, + "velocityX": -0.15895034463857272, + "velocityY": 1.7160957569474122, + "timestamp": 5.074124343643584 + }, + { + "x": 8.433318308538809, + "y": 5.649473049584276, + "heading": 4.481200094717335, + "angularVelocity": 6.1398108250935115, + "velocityX": -0.275812407211048, + "velocityY": 1.639232886695548, + "timestamp": 5.0992929774701565 + }, + { + "x": 8.423269771904357, + "y": 5.688774009331345, + "heading": 4.641460146861477, + "angularVelocity": 6.367451378109614, + "velocityX": -0.3992483940007152, + "velocityY": 1.5615054840830123, + "timestamp": 5.124461611296729 + }, + { + "x": 8.409301302318001, + "y": 5.727844142412751, + "heading": 4.801252594095005, + "angularVelocity": 6.3488725027588, + "velocityX": -0.5549951452513271, + "velocityY": 1.552334280462891, + "timestamp": 5.149630245123301 + }, + { + "x": 8.396208121715038, + "y": 5.767480930820443, + "heading": 4.939805836150058, + "angularVelocity": 5.504996536950433, + "velocityX": -0.5202181688995966, + "velocityY": 1.5748486263026762, + "timestamp": 5.174798878949874 + }, + { + "x": 8.384341280819799, + "y": 5.8076306100276724, + "heading": 5.057844576112943, + "angularVelocity": 4.689914469583332, + "velocityX": -0.4714932473891813, + "velocityY": 1.5952268003057297, + "timestamp": 5.199967512776446 + }, + { + "x": 8.373739781221703, + "y": 5.848339028395987, + "heading": 5.1555398013279, + "angularVelocity": 3.881626070296041, + "velocityX": -0.42121871497465463, + "velocityY": 1.6174266211198407, + "timestamp": 5.225136146603019 + }, + { + "x": 8.364388866933238, + "y": 5.889641090063779, + "heading": 5.232909954944061, + "angularVelocity": 3.07407045409338, + "velocityX": -0.37153046736263995, + "velocityY": 1.6410132529397037, + "timestamp": 5.250304780429591 + }, + { + "x": 8.356262991610826, + "y": 5.931555344979487, + "heading": 5.289924578574465, + "angularVelocity": 2.2653046654526636, + "velocityX": -0.32285722691204854, + "velocityY": 1.665336911193694, + "timestamp": 5.2754734142561635 + }, + { + "x": 8.34934139251709, + "y": 5.974088191986084, + "heading": 5.326548029576795, + "angularVelocity": 1.4551227235728712, + "velocityX": -0.27500893141166805, + "velocityY": 1.689914808236125, + "timestamp": 5.300642048082736 + }, + { + "x": 8.343221165925502, + "y": 6.021143146137171, + "heading": 5.342226898752842, + "angularVelocity": 0.5719929961678595, + "velocityX": -0.22327673673661108, + "velocityY": 1.7166483058956983, + "timestamp": 5.328052994162652 + }, + { + "x": 8.338553091700128, + "y": 6.068939036747707, + "heading": 5.3338112220665685, + "angularVelocity": -0.3070188333426256, + "velocityX": -0.17029963912102522, + "velocityY": 1.7436789839791385, + "timestamp": 5.3554639402425686 + }, + { + "x": 8.335401457891965, + "y": 6.11747737229338, + "heading": 5.301487591207973, + "angularVelocity": -1.1792234665799601, + "velocityX": -0.1149771992171805, + "velocityY": 1.7707646939350166, + "timestamp": 5.382874886322485 + }, + { + "x": 8.333863965332327, + "y": 6.166745972097111, + "heading": 5.245527796242896, + "angularVelocity": -2.0415127154650743, + "velocityX": -0.05609045945204774, + "velocityY": 1.7974060311558808, + "timestamp": 5.410285832402401 + }, + { + "x": 8.334074611539513, + "y": 6.216707641134325, + "heading": 5.16630466616314, + "angularVelocity": -2.890200500514666, + "velocityX": 0.007684747785513327, + "velocityY": 1.8226904278148732, + "timestamp": 5.437696778482318 + }, + { + "x": 8.336209337170322, + "y": 6.2672871852177625, + "heading": 5.064334139872564, + "angularVelocity": -3.7200659179468194, + "velocityX": 0.07787858268680638, + "velocityY": 1.845231606963613, + "timestamp": 5.465107724562234 + }, + { + "x": 8.34050875980612, + "y": 6.318358761986823, + "heading": 4.9404202596625035, + "angularVelocity": -4.520598444460426, + "velocityX": 0.1568505743385533, + "velocityY": 1.8631818332778705, + "timestamp": 5.49251867064215 + }, + { + "x": 8.347373076329852, + "y": 6.369731891617242, + "heading": 4.796226953445604, + "angularVelocity": -5.260427925271259, + "velocityX": 0.2504224591051665, + "velocityY": 1.874183017274954, + "timestamp": 5.519929616722067 + }, + { + "x": 8.357734742408361, + "y": 6.421155881623346, + "heading": 4.637107940656009, + "angularVelocity": -5.804944211910156, + "velocityX": 0.37801198281516796, + "velocityY": 1.876038494117501, + "timestamp": 5.547340562801983 + }, + { + "x": 8.372714822209472, + "y": 6.472180587497517, + "heading": 4.476004426648119, + "angularVelocity": -5.877342341201648, + "velocityX": 0.546499918588575, + "velocityY": 1.8614718997808002, + "timestamp": 5.574751508881899 + }, + { + "x": 8.392163518645132, + "y": 6.523832787745315, + "heading": 4.319730445846118, + "angularVelocity": -5.701152391689906, + "velocityX": 0.7095229905220347, + "velocityY": 1.8843640090788911, + "timestamp": 5.602162454961816 + }, + { + "x": 8.409770328909872, + "y": 6.578150492835037, + "heading": 4.178613291017239, + "angularVelocity": -5.148204458811868, + "velocityX": 0.642327711469996, + "velocityY": 1.9816063601511025, + "timestamp": 5.629573401041732 + }, + { + "x": 8.424485615502517, + "y": 6.635136223401266, + "heading": 4.049096201405813, + "angularVelocity": -4.725013475777802, + "velocityX": 0.5368397920211534, + "velocityY": 2.0789406684499903, + "timestamp": 5.656984347121648 + }, + { + "x": 8.435886735748374, + "y": 6.694743962812389, + "heading": 3.9283767340305045, + "angularVelocity": -4.404060590369644, + "velocityX": 0.41593311710644926, + "velocityY": 2.1745962082934467, + "timestamp": 5.684395293201565 + }, + { + "x": 8.4437690723566, + "y": 6.756967833759458, + "heading": 3.814495508904072, + "angularVelocity": -4.154589367124023, + "velocityX": 0.28756163998295975, + "velocityY": 2.2700373334672657, + "timestamp": 5.711806239281481 + }, + { + "x": 8.44819450378418, + "y": 6.821402072906494, + "heading": 3.7101172753280305, + "angularVelocity": -3.8079033562623543, + "velocityX": 0.16144759887806323, + "velocityY": 2.350675491432431, + "timestamp": 5.739217185361397 + }, + { + "x": 8.442296007576482, + "y": 7.005937519917937, + "heading": 3.5641292983287713, + "angularVelocity": -1.9075537985667739, + "velocityX": -0.07707277734850891, + "velocityY": 2.41123481640327, + "timestamp": 5.815748697662097 + }, + { + "x": 8.41659744566655, + "y": 7.180425726585724, + "heading": 3.5626950047371615, + "angularVelocity": -0.01874121585334014, + "velocityX": -0.3357905931475581, + "velocityY": 2.279952419889553, + "timestamp": 5.892280209962797 + }, + { + "x": 8.38385523357143, + "y": 7.320664328992628, + "heading": 3.601572492187995, + "angularVelocity": 0.5079931949871856, + "velocityX": -0.4278265398241839, + "velocityY": 1.8324295207429526, + "timestamp": 5.968811722263497 + }, + { + "x": 8.354355980334272, + "y": 7.424774646283286, + "heading": 3.6482652707450245, + "angularVelocity": 0.6101117977855888, + "velocityX": -0.38545237576453223, + "velocityY": 1.3603588137863938, + "timestamp": 6.045343234564196 + }, + { + "x": 8.33265202622526, + "y": 7.4934610231290915, + "heading": 3.6869536728033228, + "angularVelocity": 0.5055225082484697, + "velocityX": -0.2835949984071486, + "velocityY": 0.8974914356315113, + "timestamp": 6.121874746864896 + }, + { + "x": 8.321097373962402, + "y": 7.527496814727783, + "heading": 3.709032536356956, + "angularVelocity": 0.2884937575371967, + "velocityX": -0.15097901394471921, + "velocityY": 0.44472911321759667, + "timestamp": 6.198406259165596 + }, + { + "x": 8.321097373962402, + "y": 7.527496814727783, + "heading": 3.709032536356956, + "angularVelocity": -5.082729725441752e-29, + "velocityX": 4.680465584518782e-28, + "velocityY": 3.556470741173404e-29, + "timestamp": 6.274937771466296 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanephChaos.traj b/src/main/deploy/choreo/SourceLanephChaos.traj new file mode 100644 index 00000000..308ed012 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanephChaos.traj @@ -0,0 +1,1481 @@ +{ + "samples": [ + { + "x": 1.4578553438186646, + "y": 1.4268364906311035, + "heading": 1.9268906266143656e-30, + "angularVelocity": 1.1190631191470915e-30, + "velocityX": -2.439449667203012e-26, + "velocityY": -3.284743628951617e-25, + "timestamp": 0 + }, + { + "x": 1.4836061256801238, + "y": 1.4248385832903434, + "heading": -1.6036305967396036e-15, + "angularVelocity": -2.484855550358542e-14, + "velocityX": 0.3990131727631876, + "velocityY": -0.030957947265928064, + "timestamp": 0.0645361697788919 + }, + { + "x": 1.5351076891841593, + "y": 1.420842768625806, + "heading": -4.81143470802221e-15, + "angularVelocity": -4.970552361990032e-14, + "velocityX": 0.7980263421347389, + "velocityY": -0.061915894268711684, + "timestamp": 0.1290723395577838 + }, + { + "x": 1.6123600340493498, + "y": 1.4148490466593255, + "heading": -9.625413675640745e-15, + "angularVelocity": -7.459350290095871e-14, + "velocityX": 1.1970395071456144, + "velocityY": -0.09287384093316672, + "timestamp": 0.1936085093366757 + }, + { + "x": 1.7153631599004666, + "y": 1.4068574174200144, + "heading": -1.6044392495578035e-14, + "angularVelocity": -9.946327527539101e-14, + "velocityX": 1.5960526663422558, + "velocityY": -0.12383178714651694, + "timestamp": 0.2581446791155676 + }, + { + "x": 1.8441170662121902, + "y": 1.39686788094863, + "heading": -2.407162986647994e-14, + "angularVelocity": -1.2438354179375857e-13, + "velocityX": 1.9950658173989684, + "velocityY": -0.15478973272832033, + "timestamp": 0.32268084889445947 + }, + { + "x": 1.9986217521965406, + "y": 1.3848804373063093, + "heading": -3.3704756925123664e-14, + "angularVelocity": -1.4926710233420884e-13, + "velocityX": 2.3940789562457856, + "velocityY": -0.1857476773628035, + "timestamp": 0.38721701867335134 + }, + { + "x": 2.1788772165402177, + "y": 1.3708950865949459, + "heading": -4.494558790872857e-14, + "angularVelocity": -1.7417877481910134e-13, + "velocityX": 2.793092074742775, + "velocityY": -0.2167056204184192, + "timestamp": 0.4517531884522432 + }, + { + "x": 2.3848834566166217, + "y": 1.3549118290183289, + "heading": -5.77927518216841e-14, + "angularVelocity": -1.9906920347103564e-13, + "velocityX": 3.1921051525401003, + "velocityY": -0.2476635603162993, + "timestamp": 0.5162893582311351 + }, + { + "x": 2.6166404645459465, + "y": 1.3369306651878214, + "heading": -7.224642245471218e-14, + "angularVelocity": -2.239623250426539e-13, + "velocityX": 3.5911181082383807, + "velocityY": -0.27862149074096887, + "timestamp": 0.580825528010027 + }, + { + "x": 2.856317459743021, + "y": 1.3183350190196794, + "heading": -7.226675860496505e-14, + "angularVelocity": -3.151124450451814e-16, + "velocityX": 3.7138397896595787, + "velocityY": -0.2881430092900255, + "timestamp": 0.6453616977889188 + }, + { + "x": 3.095994472503662, + "y": 1.2997395992279053, + "heading": 8.152675815601398e-24, + "angularVelocity": 1.119786917331966e-12, + "velocityX": 3.7138400618103087, + "velocityY": -0.28813950154594287, + "timestamp": 0.7098978675678107 + }, + { + "x": 3.27382254607751, + "y": 1.289661887920689, + "heading": 0.031144225689139507, + "angularVelocity": 0.6214983848668802, + "velocityX": 3.548646918156489, + "velocityY": -0.20110569975653228, + "timestamp": 0.7600093826537434 + }, + { + "x": 3.4512411724559873, + "y": 1.2794797717459663, + "heading": 0.06298682604506026, + "angularVelocity": 0.6354347958012462, + "velocityX": 3.5404761974215813, + "velocityY": -0.20318915038314128, + "timestamp": 0.8101208977396761 + }, + { + "x": 3.62822231090223, + "y": 1.2691838802186675, + "heading": 0.09560610924482929, + "angularVelocity": 0.6509338850328615, + "velocityX": 3.5317459099520483, + "velocityY": -0.20545959366110889, + "timestamp": 0.8602324128256088 + }, + { + "x": 3.8047300835293036, + "y": 1.258765647314116, + "heading": 0.12910425796921293, + "angularVelocity": 0.6684720800586451, + "velocityX": 3.5222996615526942, + "velocityY": -0.20790097618653766, + "timestamp": 0.9103439279115415 + }, + { + "x": 3.9807203074364863, + "y": 1.248215472268555, + "heading": 0.16360694737365913, + "angularVelocity": 0.6885181847980438, + "velocityX": 3.5119717215771487, + "velocityY": -0.21053394668808642, + "timestamp": 0.9604554429974742 + }, + { + "x": 4.156137440439902, + "y": 1.2375224650571883, + "heading": 0.19927204657899403, + "angularVelocity": 0.7117146457091811, + "velocityX": 3.50053540992732, + "velocityY": -0.2133842330057237, + "timestamp": 1.010566958083407 + }, + { + "x": 4.330909871790797, + "y": 1.2266740963817457, + "heading": 0.23630304667549967, + "angularVelocity": 0.7389718716946344, + "velocityX": 3.487670070465641, + "velocityY": -0.21648454765016575, + "timestamp": 1.0606784731693397 + }, + { + "x": 4.504942288680689, + "y": 1.2156556997827002, + "heading": 0.27497084629517315, + "angularVelocity": 0.7716350134966949, + "velocityX": 3.4729027168996156, + "velocityY": -0.21987753872839294, + "timestamp": 1.1107899882552725 + }, + { + "x": 4.678102490303239, + "y": 1.204449734224157, + "heading": 0.31565141892272963, + "angularVelocity": 0.8118008916273269, + "velocityX": 3.4554972310378296, + "velocityY": -0.22362056982965403, + "timestamp": 1.1609015033412053 + }, + { + "x": 4.850196619661247, + "y": 1.1930346345230565, + "heading": 0.3588966354181107, + "angularVelocity": 0.8629796249668915, + "velocityX": 3.4342232331809583, + "velocityY": -0.22779394479542386, + "timestamp": 1.2110130184271382 + }, + { + "x": 5.020916905370504, + "y": 1.1813828932131683, + "heading": 0.40558384210290876, + "angularVelocity": 0.931666236886622, + "velocityX": 3.4068075055503675, + "velocityY": -0.23251624481733327, + "timestamp": 1.261124533513071 + }, + { + "x": 5.189710400965398, + "y": 1.16945753417408, + "heading": 0.45729195406784345, + "angularVelocity": 1.0318608782086127, + "velocityX": 3.3683574584692115, + "velocityY": -0.2379764215597622, + "timestamp": 1.3112360485990038 + }, + { + "x": 5.35533172203286, + "y": 1.1572047491345996, + "heading": 0.5175844077279528, + "angularVelocity": 1.2031656507834203, + "velocityX": 3.3050551511653112, + "velocityY": -0.244510368893633, + "timestamp": 1.3613475636849366 + }, + { + "x": 5.512680992157926, + "y": 1.1445116485154183, + "heading": 0.6010844246558354, + "angularVelocity": 1.6662840224386364, + "velocityX": 3.1399822945931812, + "velocityY": -0.25329708346305213, + "timestamp": 1.4114590787708694 + }, + { + "x": 5.678954750693719, + "y": 1.128816110929195, + "heading": 0.6579086700766517, + "angularVelocity": 1.1339558447469087, + "velocityX": 3.3180748626470646, + "velocityY": -0.3132121940298074, + "timestamp": 1.4615705938568022 + }, + { + "x": 5.854338980055963, + "y": 1.1023762612022525, + "heading": 0.6848690400817262, + "angularVelocity": 0.5380074810917644, + "velocityX": 3.4998788015387303, + "velocityY": -0.5276202422058656, + "timestamp": 1.511682108942735 + }, + { + "x": 6.029328591050374, + "y": 1.0604680679443284, + "heading": 0.7046293616016649, + "angularVelocity": 0.39432696229705316, + "velocityX": 3.4920039973713513, + "velocityY": -0.8362986668046034, + "timestamp": 1.5617936240286678 + }, + { + "x": 6.202812194824219, + "y": 1.003179669380188, + "heading": 0.7165423509110156, + "angularVelocity": 0.2377295775017328, + "velocityX": 3.4619508804782524, + "velocityY": -1.1432182496558028, + "timestamp": 1.6119051391146006 + }, + { + "x": 6.402547568535553, + "y": 0.9154521762914801, + "heading": 0.7165423052284949, + "angularVelocity": -7.80040343805035e-7, + "velocityX": 3.4105309321224193, + "velocityY": -1.4979686533092873, + "timestamp": 1.6704694441118393 + }, + { + "x": 6.592790671542091, + "y": 0.8086912505756066, + "heading": 0.7165422622385289, + "angularVelocity": -7.34064308321193e-7, + "velocityX": 3.2484480609051705, + "velocityY": -1.8229692253823835, + "timestamp": 1.729033749109078 + }, + { + "x": 6.792526057186022, + "y": 0.7209637957417591, + "heading": 0.7165421996040524, + "angularVelocity": -0.0000010694991862536609, + "velocityX": 3.410531135874468, + "velocityY": -1.4979680000980806, + "timestamp": 1.7875980541063166 + }, + { + "x": 6.995358135542775, + "y": 0.6540115645265273, + "heading": 0.702913894528627, + "angularVelocity": -0.23270668158817906, + "velocityX": 3.4634079302455083, + "velocityY": -1.1432259158268625, + "timestamp": 1.8461623591035552 + }, + { + "x": 7.192981165480996, + "y": 0.6064556135405038, + "heading": 0.6594934468717042, + "angularVelocity": -0.7414148884541547, + "velocityX": 3.3744621394813583, + "velocityY": -0.8120296311595578, + "timestamp": 1.9047266641007938 + }, + { + "x": 7.3826916988169575, + "y": 0.5778864686152094, + "heading": 0.5928355536169142, + "angularVelocity": -1.138200022316206, + "velocityX": 3.239354301991779, + "velocityY": -0.4878252192464604, + "timestamp": 1.9632909690980325 + }, + { + "x": 7.56045020150032, + "y": 0.5667919314464598, + "heading": 0.5348000913331561, + "angularVelocity": -0.9909698798012667, + "velocityX": 3.0352704209798658, + "velocityY": -0.18944196758201154, + "timestamp": 2.021855274095271 + }, + { + "x": 7.725161762075186, + "y": 0.5716432246000058, + "heading": 0.4994377275755845, + "angularVelocity": -0.6038211118400366, + "velocityX": 2.812490655914593, + "velocityY": 0.08283703108531969, + "timestamp": 2.0804195790925095 + }, + { + "x": 7.876544251370993, + "y": 0.5914886747454028, + "heading": 0.49476791383000396, + "angularVelocity": -0.07973822528587567, + "velocityX": 2.5848934654470064, + "velocityY": 0.3388659721366544, + "timestamp": 2.138983884089748 + }, + { + "x": 8.014557697135269, + "y": 0.6256824372518744, + "heading": 0.5263224795620095, + "angularVelocity": 0.5388020182855987, + "velocityX": 2.35661373887698, + "velocityY": 0.5838669562984516, + "timestamp": 2.197548189086987 + }, + { + "x": 8.139220711960105, + "y": 0.6736709753190644, + "heading": 0.5988363569407734, + "angularVelocity": 1.238192400339819, + "velocityX": 2.128651826922783, + "velocityY": 0.8194161626173599, + "timestamp": 2.2561124940842254 + }, + { + "x": 8.25048828125, + "y": 0.7348634600639343, + "heading": 0.7165423509110156, + "angularVelocity": 2.0098589742641386, + "velocityX": 1.8999212796112497, + "velocityY": 1.044876819553399, + "timestamp": 2.314676799081464 + }, + { + "x": 8.302330173893822, + "y": 0.7683170831186208, + "heading": 0.7866903105882892, + "angularVelocity": 2.4170681239593663, + "velocityX": 1.7863012234653046, + "velocityY": 1.152701893090581, + "timestamp": 2.343698719887885 + }, + { + "x": 8.350761252955918, + "y": 0.804862507674631, + "heading": 0.8681048778617475, + "angularVelocity": 2.805278389962577, + "velocityX": 1.6687757983055662, + "velocityY": 1.2592352105076432, + "timestamp": 2.372720640694306 + }, + { + "x": 8.395659126629422, + "y": 0.8444668269434722, + "heading": 0.9600582095499061, + "angularVelocity": 3.168409572250435, + "velocityX": 1.5470331537659474, + "velocityY": 1.36463466815329, + "timestamp": 2.4017425615007273 + }, + { + "x": 8.436895225190787, + "y": 0.8871049250141315, + "heading": 1.0616235787520496, + "angularVelocity": 3.499608791561171, + "velocityX": 1.420860419143676, + "velocityY": 1.469168714057877, + "timestamp": 2.4307644823071484 + }, + { + "x": 8.474338814577639, + "y": 0.9327584709704907, + "heading": 1.1716968936719283, + "angularVelocity": 3.792764636568266, + "velocityX": 1.2901830184364116, + "velocityY": 1.5730711368442005, + "timestamp": 2.4597864031135694 + }, + { + "x": 8.50785937445302, + "y": 0.9814068916097549, + "heading": 1.2890700861945108, + "angularVelocity": 4.0442944250820885, + "velocityX": 1.1550083159196811, + "velocityY": 1.676264674683452, + "timestamp": 2.4888083239199905 + }, + { + "x": 8.537323038411628, + "y": 1.0330097209040148, + "heading": 1.4125130240648722, + "angularVelocity": 4.253437899363618, + "velocityX": 1.0152210170765539, + "velocityY": 1.7780638862070914, + "timestamp": 2.5178302447264116 + }, + { + "x": 8.56258232091714, + "y": 1.0874855013153344, + "heading": 1.5407726955584362, + "angularVelocity": 4.419406708090351, + "velocityX": 0.870351851415892, + "velocityY": 1.877056338713015, + "timestamp": 2.5468521655328327 + }, + { + "x": 8.583466101858683, + "y": 1.1446918380868547, + "heading": 1.6724319994758665, + "angularVelocity": 4.536546867301084, + "velocityX": 0.7195864491822832, + "velocityY": 1.9711423359291682, + "timestamp": 2.5758740863392537 + }, + { + "x": 8.599781102269066, + "y": 1.2044006523964297, + "heading": 1.8056614857101974, + "angularVelocity": 4.590650188972121, + "velocityX": 0.5621612890203437, + "velocityY": 2.057369486597329, + "timestamp": 2.604896007145675 + }, + { + "x": 8.61133776585303, + "y": 1.2662515535930832, + "heading": 1.9379574799883315, + "angularVelocity": 4.558485124418899, + "velocityX": 0.3982046419686604, + "velocityY": 2.131178759986453, + "timestamp": 2.633917927952096 + }, + { + "x": 8.61803446841016, + "y": 1.329807234083184, + "heading": 2.0667068830619315, + "angularVelocity": 4.43628124865926, + "velocityX": 0.23074635899526613, + "velocityY": 2.189919851067852, + "timestamp": 2.662939848758517 + }, + { + "x": 8.619867506604743, + "y": 1.394964606545626, + "heading": 2.191275336084359, + "angularVelocity": 4.292219452093143, + "velocityX": 0.06316047124545049, + "velocityY": 2.245108891897538, + "timestamp": 2.691961769564938 + }, + { + "x": 8.616839586460362, + "y": 1.4616264782251567, + "heading": 2.311063053299335, + "angularVelocity": 4.12749100977746, + "velocityX": -0.10433217582589409, + "velocityY": 2.2969489898401783, + "timestamp": 2.720983690371359 + }, + { + "x": 8.608949954788589, + "y": 1.5296853158108266, + "heading": 2.4255531996922417, + "angularVelocity": 3.9449541316223122, + "velocityX": -0.2718507752949118, + "velocityY": 2.345083843334451, + "timestamp": 2.75000561117778 + }, + { + "x": 8.596173618614296, + "y": 1.5989753133446671, + "heading": 2.534426417053211, + "angularVelocity": 3.7514132192408356, + "velocityX": -0.44023055053838084, + "velocityY": 2.3875055684980784, + "timestamp": 2.7790275319842013 + }, + { + "x": 8.57842938977896, + "y": 1.6691611767333963, + "heading": 2.6378090883600067, + "angularVelocity": 3.5622270488699845, + "velocityX": -0.6114078028705309, + "velocityY": 2.4183741612719314, + "timestamp": 2.8080494527906223 + }, + { + "x": 8.555564514046702, + "y": 1.7394263904616403, + "heading": 2.737024931725787, + "angularVelocity": 3.4186518538024746, + "velocityX": -0.7878484640892429, + "velocityY": 2.421108313158164, + "timestamp": 2.8370713735970434 + }, + { + "x": 8.527959255725655, + "y": 1.8075255209054102, + "heading": 2.8376495221341176, + "angularVelocity": 3.4671926465345364, + "velocityX": -0.9511864671252386, + "velocityY": 2.3464722027875915, + "timestamp": 2.8660932944034645 + }, + { + "x": 8.499577272044599, + "y": 1.8711589675729616, + "heading": 2.9502161532820788, + "angularVelocity": 3.878676118606718, + "velocityX": -0.9779498700435901, + "velocityY": 2.1925994179362642, + "timestamp": 2.8951152152098856 + }, + { + "x": 8.472091457883874, + "y": 1.93067768486564, + "heading": 3.076977426653027, + "angularVelocity": 4.3677768338097795, + "velocityX": -0.9470708139567483, + "velocityY": 2.0508193682173372, + "timestamp": 2.9241371360163066 + }, + { + "x": 8.445935000391486, + "y": 1.9864273544904458, + "heading": 3.219502150557654, + "angularVelocity": 4.910933526946052, + "velocityX": -0.9012655525750104, + "velocityY": 1.920950373914328, + "timestamp": 2.9531590568227277 + }, + { + "x": 8.421236309719301, + "y": 2.038741873704351, + "heading": 3.37984910584556, + "angularVelocity": 5.52502904123526, + "velocityX": -0.8510356994262128, + "velocityY": 1.8025863816129806, + "timestamp": 2.982180977629149 + }, + { + "x": 8.398072066125595, + "y": 2.08798012126862, + "heading": 3.5600291132631146, + "angularVelocity": 6.208410829158125, + "velocityX": -0.7981636966145051, + "velocityY": 1.6965881718406162, + "timestamp": 3.01120289843557 + }, + { + "x": 8.377585411071777, + "y": 2.1329314708709717, + "heading": 3.745897356817782, + "angularVelocity": 6.404408750007486, + "velocityX": -0.7059027963884773, + "velocityY": 1.5488757585061539, + "timestamp": 3.040224819241991 + }, + { + "x": 8.364981537655835, + "y": 2.1618201695949115, + "heading": 3.87540454879606, + "angularVelocity": 6.50944417189865, + "velocityX": -0.6335108428921614, + "velocityY": 1.4520380580394667, + "timestamp": 3.0601200957906958 + }, + { + "x": 8.354284801682704, + "y": 2.189254221668137, + "heading": 4.007147516190271, + "angularVelocity": 6.621821369092149, + "velocityX": -0.5376520375047442, + "velocityY": 1.3789228818240207, + "timestamp": 3.0800153723394006 + }, + { + "x": 8.345932812421994, + "y": 2.2160755919920243, + "heading": 4.140533212375188, + "angularVelocity": 6.704390153028566, + "velocityX": -0.41979759568875646, + "velocityY": 1.3481275446574927, + "timestamp": 3.0999106488881054 + }, + { + "x": 8.339986678909588, + "y": 2.2433741294975276, + "heading": 4.274319407548544, + "angularVelocity": 6.724520508465367, + "velocityX": -0.2988716189920965, + "velocityY": 1.3721114878033795, + "timestamp": 3.1198059254368102 + }, + { + "x": 8.33607844515623, + "y": 2.2720346987144775, + "heading": 4.407573079371785, + "angularVelocity": 6.697754187886208, + "velocityX": -0.1964402828877463, + "velocityY": 1.4405715420334793, + "timestamp": 3.139701201985515 + }, + { + "x": 8.333720429129459, + "y": 2.3025901920416527, + "heading": 4.540110627397886, + "angularVelocity": 6.661759523756392, + "velocityX": -0.11852139984079614, + "velocityY": 1.5358164664046496, + "timestamp": 3.15959647853422 + }, + { + "x": 8.332603722599888, + "y": 2.3352255445188432, + "heading": 4.671035459898787, + "angularVelocity": 6.580699302188081, + "velocityX": -0.05612922880648972, + "velocityY": 1.640356815211755, + "timestamp": 3.1794917550829247 + }, + { + "x": 8.332608537566271, + "y": 2.3698002053105385, + "heading": 4.797912823563958, + "angularVelocity": 6.377260620357163, + "velocityX": 0.0002420155543911525, + "velocityY": 1.7378326311300352, + "timestamp": 3.1993870316316295 + }, + { + "x": 8.33396980572602, + "y": 2.4053616174482886, + "heading": 4.91661874197118, + "angularVelocity": 5.966537741590232, + "velocityX": 0.06842167568854002, + "velocityY": 1.787429898282315, + "timestamp": 3.2192823081803343 + }, + { + "x": 8.336666983434197, + "y": 2.4421571036885257, + "heading": 5.027955549931603, + "angularVelocity": 5.596142767247511, + "velocityX": 0.13556874676122466, + "velocityY": 1.8494583953211146, + "timestamp": 3.239177584729039 + }, + { + "x": 8.340246531495488, + "y": 2.479675325798265, + "heading": 5.127878006964601, + "angularVelocity": 5.02242111530254, + "velocityX": 0.17991949257545845, + "velocityY": 1.8857854032786403, + "timestamp": 3.259072861277744 + }, + { + "x": 8.344506054620522, + "y": 2.517833586551145, + "heading": 5.215460170451448, + "angularVelocity": 4.402158636621092, + "velocityX": 0.2140972061688483, + "velocityY": 1.9179557851063929, + "timestamp": 3.278968137826449 + }, + { + "x": 8.34934139251709, + "y": 2.556588649749756, + "heading": 5.290290323982317, + "angularVelocity": 3.7612019791572497, + "velocityX": 0.24303949154619145, + "velocityY": 1.9479529778707136, + "timestamp": 3.2988634143751536 + }, + { + "x": 8.360489719138714, + "y": 2.633118600966112, + "heading": 5.386097875540369, + "angularVelocity": 2.5082503010878967, + "velocityX": 0.2918641918155103, + "velocityY": 2.0035609934605225, + "timestamp": 3.3370603804102004 + }, + { + "x": 8.3734961558476, + "y": 2.711837506326731, + "heading": 5.434143906525109, + "angularVelocity": 1.2578494046008082, + "velocityX": 0.3405096807153113, + "velocityY": 2.0608680094746767, + "timestamp": 3.375257346445247 + }, + { + "x": 8.388354991269559, + "y": 2.792730047490086, + "heading": 5.4343775006036195, + "angularVelocity": 0.006115513946711611, + "velocityX": 0.3890056453259781, + "velocityY": 2.117773989932431, + "timestamp": 3.413454312480294 + }, + { + "x": 8.405064352616025, + "y": 2.875700202856259, + "heading": 5.386603131855514, + "angularVelocity": -1.2507372628567064, + "velocityX": 0.43745258016393523, + "velocityY": 2.1721661162838264, + "timestamp": 3.4516512785153406 + }, + { + "x": 8.423595902184939, + "y": 2.960570562063504, + "heading": 5.290443702807007, + "angularVelocity": -2.51746248537848, + "velocityX": 0.4851576314179031, + "velocityY": 2.221913623437374, + "timestamp": 3.4898482445503873 + }, + { + "x": 8.443874909764538, + "y": 3.0470824289690306, + "heading": 5.145320206366309, + "angularVelocity": -3.799346165544723, + "velocityX": 0.5309062390188924, + "velocityY": 2.264888442347729, + "timestamp": 3.528045210585434 + }, + { + "x": 8.46822613284771, + "y": 3.1277535966232044, + "heading": 4.9755072400440925, + "angularVelocity": -4.445718703585181, + "velocityX": 0.6375172064929964, + "velocityY": 2.1119784115878613, + "timestamp": 3.566242176620481 + }, + { + "x": 8.485889521575103, + "y": 3.2052285744044853, + "heading": 4.782579464036899, + "angularVelocity": -5.050866496312229, + "velocityX": 0.4624291026461906, + "velocityY": 2.02830187377173, + "timestamp": 3.6044391426555276 + }, + { + "x": 8.496466363696742, + "y": 3.2817681103248395, + "heading": 4.564012968163081, + "angularVelocity": -5.722090484183368, + "velocityX": 0.2769026762998172, + "velocityY": 2.0038119218716672, + "timestamp": 3.6426361086905743 + }, + { + "x": 8.498864361133254, + "y": 3.3553108945925123, + "heading": 4.355441183612215, + "angularVelocity": -5.460428044455047, + "velocityX": 0.06277978817249102, + "velocityY": 1.9253566945656204, + "timestamp": 3.680833074725621 + }, + { + "x": 8.49244817179446, + "y": 3.427344053675241, + "heading": 4.152515229757634, + "angularVelocity": -5.312619689961475, + "velocityX": -0.16797641291473847, + "velocityY": 1.8858345716944014, + "timestamp": 3.719030040760668 + }, + { + "x": 8.480949544401803, + "y": 3.4955676666301225, + "heading": 3.980239273291126, + "angularVelocity": -4.5102000066822105, + "velocityX": -0.3010350974500928, + "velocityY": 1.7861003120584202, + "timestamp": 3.7572270067957145 + }, + { + "x": 8.465880541095904, + "y": 3.5608797001839374, + "heading": 3.848658427278945, + "angularVelocity": -3.4447983615099473, + "velocityX": -0.39450785939576005, + "velocityY": 1.7098749019461128, + "timestamp": 3.7954239728307613 + }, + { + "x": 8.447723716276025, + "y": 3.6237520788842326, + "heading": 3.7606807673271616, + "angularVelocity": -2.3032630358929707, + "velocityX": -0.4753473038465876, + "velocityY": 1.6460045188564885, + "timestamp": 3.833620938865808 + }, + { + "x": 8.426810965043074, + "y": 3.6844175741296494, + "heading": 3.717839244299634, + "angularVelocity": -1.1215949190367396, + "velocityX": -0.5474977047591038, + "velocityY": 1.5882281118807744, + "timestamp": 3.8718179049008548 + }, + { + "x": 8.403377538803907, + "y": 3.7429507831754503, + "heading": 3.7209535641751312, + "angularVelocity": 0.08153317393427642, + "velocityX": -0.6134892027200903, + "velocityY": 1.5324046677449423, + "timestamp": 3.9100148709359015 + }, + { + "x": 8.377585411071777, + "y": 3.7993156909942623, + "heading": 3.770388538466339, + "angularVelocity": 1.2942120650590392, + "velocityX": -0.6752402195625785, + "velocityY": 1.475638346959163, + "timestamp": 3.9482118369709482 + }, + { + "x": 8.359121603366816, + "y": 3.8364193104219853, + "heading": 3.8251043907760733, + "angularVelocity": 2.1181774191421603, + "velocityX": -0.7147767767674121, + "velocityY": 1.4363670768638492, + "timestamp": 3.974043408875767 + }, + { + "x": 8.339698595709613, + "y": 3.872452168004382, + "heading": 3.901119172271844, + "angularVelocity": 2.9427083173978534, + "velocityX": -0.7519096293779934, + "velocityY": 1.3949154048838457, + "timestamp": 3.999874980780586 + }, + { + "x": 8.31940846552165, + "y": 3.907347005013746, + "heading": 3.9984628782143856, + "angularVelocity": 3.7684004016953554, + "velocityX": -0.7854779516602693, + "velocityY": 1.3508599917163486, + "timestamp": 4.025706552685405 + }, + { + "x": 8.29838582091067, + "y": 3.941024765667179, + "heading": 4.1171731868845285, + "angularVelocity": 4.595551099544112, + "velocityX": -0.8138352822058595, + "velocityY": 1.3037441460211887, + "timestamp": 4.051538124590223 + }, + { + "x": 8.276832604055052, + "y": 3.9733768568490917, + "heading": 4.2572182344953795, + "angularVelocity": 5.421468276374085, + "velocityX": -0.8343749631278609, + "velocityY": 1.2524244092121188, + "timestamp": 4.077369696495042 + }, + { + "x": 8.255105530373472, + "y": 4.004178929302719, + "heading": 4.418037307026107, + "angularVelocity": 6.225678914287337, + "velocityX": -0.841105363685868, + "velocityY": 1.1924195928580044, + "timestamp": 4.103201268399861 + }, + { + "x": 8.236226871427698, + "y": 4.03234435965831, + "heading": 4.584751742520452, + "angularVelocity": 6.45390207412214, + "velocityX": -0.7308366295065699, + "velocityY": 1.0903490681625025, + "timestamp": 4.12903284030468 + }, + { + "x": 8.22111995531124, + "y": 4.060012965907288, + "heading": 4.744248346923677, + "angularVelocity": 6.174483108922656, + "velocityX": -0.5848237254830212, + "velocityY": 1.0711158558576532, + "timestamp": 4.154864412209498 + }, + { + "x": 8.208790974794852, + "y": 4.088137716132201, + "heading": 4.888562116667079, + "angularVelocity": 5.5867204007233076, + "velocityX": -0.4772834019477146, + "velocityY": 1.0887742460483674, + "timestamp": 4.180695984114317 + }, + { + "x": 8.198725383178298, + "y": 4.116963981676649, + "heading": 5.015233890568161, + "angularVelocity": 4.903757865290863, + "velocityX": -0.3896623733794405, + "velocityY": 1.115931529473434, + "timestamp": 4.206527556019136 + }, + { + "x": 8.190656129284886, + "y": 4.146632428651468, + "heading": 5.123285713831358, + "angularVelocity": 4.182936433807978, + "velocityX": -0.31237951461661634, + "velocityY": 1.148534323971415, + "timestamp": 4.232359127923955 + }, + { + "x": 8.184413831367804, + "y": 4.177239551429493, + "heading": 5.212211909192924, + "angularVelocity": 3.4425390637949445, + "velocityX": -0.241653815729148, + "velocityY": 1.184872639218488, + "timestamp": 4.2581906998287735 + }, + { + "x": 8.179879188537598, + "y": 4.208850860595703, + "heading": 5.281700753397833, + "angularVelocity": 2.6900741643192534, + "velocityX": -0.17554653069182086, + "velocityY": 1.223747020997694, + "timestamp": 4.284022271733592 + }, + { + "x": 8.176711633108404, + "y": 4.259079542498045, + "heading": 5.341834535732932, + "angularVelocity": 1.5389696629172125, + "velocityX": -0.08106544311437222, + "velocityY": 1.2854740655637256, + "timestamp": 4.323096324838129 + }, + { + "x": 8.177255357527843, + "y": 4.31178579369655, + "heading": 5.357219600158119, + "angularVelocity": 0.3937411965947536, + "velocityX": 0.013915229576633022, + "velocityY": 1.3488810863182197, + "timestamp": 4.362170377942666 + }, + { + "x": 8.181626457446923, + "y": 4.366954653737556, + "heading": 5.328216932710745, + "angularVelocity": -0.7422487595484647, + "velocityX": 0.11186707218176441, + "velocityY": 1.411905232697765, + "timestamp": 4.401244431047203 + }, + { + "x": 8.190025046391563, + "y": 4.424472904744573, + "heading": 5.255283063236909, + "angularVelocity": -1.8665550071990427, + "velocityX": 0.21494030635039485, + "velocityY": 1.4720318584083123, + "timestamp": 4.4403184841517405 + }, + { + "x": 8.202703493077177, + "y": 4.484103126840556, + "heading": 5.138917873952012, + "angularVelocity": -2.978068053845803, + "velocityX": 0.3244722694032999, + "velocityY": 1.526082332345974, + "timestamp": 4.4793925372562775 + }, + { + "x": 8.219928011035833, + "y": 4.545471721328882, + "heading": 4.979694935972509, + "angularVelocity": -4.074902021388053, + "velocityX": 0.44081728385262037, + "velocityY": 1.570571507494814, + "timestamp": 4.5184665903608145 + }, + { + "x": 8.242002423270923, + "y": 4.608108641713836, + "heading": 4.778755843149729, + "angularVelocity": -5.142519827292898, + "velocityX": 0.5649378674905671, + "velocityY": 1.6030310502311427, + "timestamp": 4.557540643465352 + }, + { + "x": 8.271074244498946, + "y": 4.672945765445566, + "heading": 4.551673202567828, + "angularVelocity": -5.811596764082065, + "velocityX": 0.7440185728940091, + "velocityY": 1.6593396021207916, + "timestamp": 4.596614696569889 + }, + { + "x": 8.307044890775797, + "y": 4.743652286364594, + "heading": 4.33581324080306, + "angularVelocity": -5.524381133108038, + "velocityX": 0.9205762755303734, + "velocityY": 1.8095517434514046, + "timestamp": 4.635688749674426 + }, + { + "x": 8.343074596799024, + "y": 4.821913482728032, + "heading": 4.143267038130374, + "angularVelocity": -4.927725366947583, + "velocityX": 0.9220877580023329, + "velocityY": 2.0028942519492667, + "timestamp": 4.674762802778963 + }, + { + "x": 8.37162830890395, + "y": 4.904380364973872, + "heading": 3.9684903103687414, + "angularVelocity": -4.472961310003866, + "velocityX": 0.7307589010163777, + "velocityY": 2.1105279768446303, + "timestamp": 4.7138368558835 + }, + { + "x": 8.39410907934667, + "y": 4.988084023708393, + "heading": 3.8243195102940244, + "angularVelocity": -3.6896812237268355, + "velocityX": 0.5753375617977339, + "velocityY": 2.142180093541445, + "timestamp": 4.752910908988037 + }, + { + "x": 8.412533295857541, + "y": 5.0701333500179695, + "heading": 3.7241871610957364, + "angularVelocity": -2.5626302173055078, + "velocityX": 0.47152048602634916, + "velocityY": 2.099841705442333, + "timestamp": 4.791984962092574 + }, + { + "x": 8.427022508757116, + "y": 5.150508503599114, + "heading": 3.668854256125942, + "angularVelocity": -1.4161035411852148, + "velocityX": 0.370814178421956, + "velocityY": 2.0569955557492743, + "timestamp": 4.831059015197111 + }, + { + "x": 8.437687476391535, + "y": 5.229173684079572, + "heading": 3.658771745830396, + "angularVelocity": -0.25803594698946425, + "velocityX": 0.27294244612622637, + "velocityY": 2.013233187506785, + "timestamp": 4.870133068301648 + }, + { + "x": 8.444672138848645, + "y": 5.306037054642886, + "heading": 3.694319619572902, + "angularVelocity": 0.9097564987026582, + "velocityX": 0.17875449056755022, + "velocityY": 1.9671204918946052, + "timestamp": 4.909207121406185 + }, + { + "x": 8.44819450378418, + "y": 5.38096809387207, + "heading": 3.7759750997570256, + "angularVelocity": 2.0897622257323873, + "velocityX": 0.09014588085116931, + "velocityY": 1.9176674359508388, + "timestamp": 4.948281174510722 + }, + { + "x": 8.449106377133136, + "y": 5.428397890121427, + "heading": 3.847877974347001, + "angularVelocity": 2.856844558406781, + "velocityX": 0.03623054613292597, + "velocityY": 1.8844803645751356, + "timestamp": 4.973449808337294 + }, + { + "x": 8.448712080913069, + "y": 5.474928555589069, + "heading": 3.9390652949611624, + "angularVelocity": 3.623054045861213, + "velocityX": -0.015666174921664548, + "velocityY": 1.848756106043276, + "timestamp": 4.998618442163867 + }, + { + "x": 8.447075648100045, + "y": 5.520495348927803, + "heading": 4.049507624303278, + "angularVelocity": 4.388093930847932, + "velocityX": -0.06501873817623247, + "velocityY": 1.8104595447141671, + "timestamp": 5.023787075990439 + }, + { + "x": 8.444260693041544, + "y": 5.565024011584617, + "heading": 4.179082223329722, + "angularVelocity": 5.148257148929752, + "velocityX": -0.11184377657913579, + "velocityY": 1.7692125430265968, + "timestamp": 5.048955709817012 + }, + { + "x": 8.440260130020729, + "y": 5.60821579730256, + "heading": 4.326669444296132, + "angularVelocity": 5.86393452991448, + "velocityX": -0.15895034463857272, + "velocityY": 1.7160957569474122, + "timestamp": 5.074124343643584 + }, + { + "x": 8.433318308538809, + "y": 5.649473049584276, + "heading": 4.481200094717335, + "angularVelocity": 6.1398108250935115, + "velocityX": -0.275812407211048, + "velocityY": 1.639232886695548, + "timestamp": 5.0992929774701565 + }, + { + "x": 8.423269771904357, + "y": 5.688774009331345, + "heading": 4.641460146861477, + "angularVelocity": 6.367451378109614, + "velocityX": -0.3992483940007152, + "velocityY": 1.5615054840830123, + "timestamp": 5.124461611296729 + }, + { + "x": 8.409301302318001, + "y": 5.727844142412751, + "heading": 4.801252594095005, + "angularVelocity": 6.3488725027588, + "velocityX": -0.5549951452513271, + "velocityY": 1.552334280462891, + "timestamp": 5.149630245123301 + }, + { + "x": 8.396208121715038, + "y": 5.767480930820443, + "heading": 4.939805836150058, + "angularVelocity": 5.504996536950433, + "velocityX": -0.5202181688995966, + "velocityY": 1.5748486263026762, + "timestamp": 5.174798878949874 + }, + { + "x": 8.384341280819799, + "y": 5.8076306100276724, + "heading": 5.057844576112943, + "angularVelocity": 4.689914469583332, + "velocityX": -0.4714932473891813, + "velocityY": 1.5952268003057297, + "timestamp": 5.199967512776446 + }, + { + "x": 8.373739781221703, + "y": 5.848339028395987, + "heading": 5.1555398013279, + "angularVelocity": 3.881626070296041, + "velocityX": -0.42121871497465463, + "velocityY": 1.6174266211198407, + "timestamp": 5.225136146603019 + }, + { + "x": 8.364388866933238, + "y": 5.889641090063779, + "heading": 5.232909954944061, + "angularVelocity": 3.07407045409338, + "velocityX": -0.37153046736263995, + "velocityY": 1.6410132529397037, + "timestamp": 5.250304780429591 + }, + { + "x": 8.356262991610826, + "y": 5.931555344979487, + "heading": 5.289924578574465, + "angularVelocity": 2.2653046654526636, + "velocityX": -0.32285722691204854, + "velocityY": 1.665336911193694, + "timestamp": 5.2754734142561635 + }, + { + "x": 8.34934139251709, + "y": 5.974088191986084, + "heading": 5.326548029576795, + "angularVelocity": 1.4551227235728712, + "velocityX": -0.27500893141166805, + "velocityY": 1.689914808236125, + "timestamp": 5.300642048082736 + }, + { + "x": 8.343221165925502, + "y": 6.021143146137171, + "heading": 5.342226898752842, + "angularVelocity": 0.5719929961678595, + "velocityX": -0.22327673673661108, + "velocityY": 1.7166483058956983, + "timestamp": 5.328052994162652 + }, + { + "x": 8.338553091700128, + "y": 6.068939036747707, + "heading": 5.3338112220665685, + "angularVelocity": -0.3070188333426256, + "velocityX": -0.17029963912102522, + "velocityY": 1.7436789839791385, + "timestamp": 5.3554639402425686 + }, + { + "x": 8.335401457891965, + "y": 6.11747737229338, + "heading": 5.301487591207973, + "angularVelocity": -1.1792234665799601, + "velocityX": -0.1149771992171805, + "velocityY": 1.7707646939350166, + "timestamp": 5.382874886322485 + }, + { + "x": 8.333863965332327, + "y": 6.166745972097111, + "heading": 5.245527796242896, + "angularVelocity": -2.0415127154650743, + "velocityX": -0.05609045945204774, + "velocityY": 1.7974060311558808, + "timestamp": 5.410285832402401 + }, + { + "x": 8.334074611539513, + "y": 6.216707641134325, + "heading": 5.16630466616314, + "angularVelocity": -2.890200500514666, + "velocityX": 0.007684747785513327, + "velocityY": 1.8226904278148732, + "timestamp": 5.437696778482318 + }, + { + "x": 8.336209337170322, + "y": 6.2672871852177625, + "heading": 5.064334139872564, + "angularVelocity": -3.7200659179468194, + "velocityX": 0.07787858268680638, + "velocityY": 1.845231606963613, + "timestamp": 5.465107724562234 + }, + { + "x": 8.34050875980612, + "y": 6.318358761986823, + "heading": 4.9404202596625035, + "angularVelocity": -4.520598444460426, + "velocityX": 0.1568505743385533, + "velocityY": 1.8631818332778705, + "timestamp": 5.49251867064215 + }, + { + "x": 8.347373076329852, + "y": 6.369731891617242, + "heading": 4.796226953445604, + "angularVelocity": -5.260427925271259, + "velocityX": 0.2504224591051665, + "velocityY": 1.874183017274954, + "timestamp": 5.519929616722067 + }, + { + "x": 8.357734742408361, + "y": 6.421155881623346, + "heading": 4.637107940656009, + "angularVelocity": -5.804944211910156, + "velocityX": 0.37801198281516796, + "velocityY": 1.876038494117501, + "timestamp": 5.547340562801983 + }, + { + "x": 8.372714822209472, + "y": 6.472180587497517, + "heading": 4.476004426648119, + "angularVelocity": -5.877342341201648, + "velocityX": 0.546499918588575, + "velocityY": 1.8614718997808002, + "timestamp": 5.574751508881899 + }, + { + "x": 8.392163518645132, + "y": 6.523832787745315, + "heading": 4.319730445846118, + "angularVelocity": -5.701152391689906, + "velocityX": 0.7095229905220347, + "velocityY": 1.8843640090788911, + "timestamp": 5.602162454961816 + }, + { + "x": 8.409770328909872, + "y": 6.578150492835037, + "heading": 4.178613291017239, + "angularVelocity": -5.148204458811868, + "velocityX": 0.642327711469996, + "velocityY": 1.9816063601511025, + "timestamp": 5.629573401041732 + }, + { + "x": 8.424485615502517, + "y": 6.635136223401266, + "heading": 4.049096201405813, + "angularVelocity": -4.725013475777802, + "velocityX": 0.5368397920211534, + "velocityY": 2.0789406684499903, + "timestamp": 5.656984347121648 + }, + { + "x": 8.435886735748374, + "y": 6.694743962812389, + "heading": 3.9283767340305045, + "angularVelocity": -4.404060590369644, + "velocityX": 0.41593311710644926, + "velocityY": 2.1745962082934467, + "timestamp": 5.684395293201565 + }, + { + "x": 8.4437690723566, + "y": 6.756967833759458, + "heading": 3.814495508904072, + "angularVelocity": -4.154589367124023, + "velocityX": 0.28756163998295975, + "velocityY": 2.2700373334672657, + "timestamp": 5.711806239281481 + }, + { + "x": 8.44819450378418, + "y": 6.821402072906494, + "heading": 3.7101172753280305, + "angularVelocity": -3.8079033562623543, + "velocityX": 0.16144759887806323, + "velocityY": 2.350675491432431, + "timestamp": 5.739217185361397 + }, + { + "x": 8.442296007576482, + "y": 7.005937519917937, + "heading": 3.5641292983287713, + "angularVelocity": -1.9075537985667739, + "velocityX": -0.07707277734850891, + "velocityY": 2.41123481640327, + "timestamp": 5.815748697662097 + }, + { + "x": 8.41659744566655, + "y": 7.180425726585724, + "heading": 3.5626950047371615, + "angularVelocity": -0.01874121585334014, + "velocityX": -0.3357905931475581, + "velocityY": 2.279952419889553, + "timestamp": 5.892280209962797 + }, + { + "x": 8.38385523357143, + "y": 7.320664328992628, + "heading": 3.601572492187995, + "angularVelocity": 0.5079931949871856, + "velocityX": -0.4278265398241839, + "velocityY": 1.8324295207429526, + "timestamp": 5.968811722263497 + }, + { + "x": 8.354355980334272, + "y": 7.424774646283286, + "heading": 3.6482652707450245, + "angularVelocity": 0.6101117977855888, + "velocityX": -0.38545237576453223, + "velocityY": 1.3603588137863938, + "timestamp": 6.045343234564196 + }, + { + "x": 8.33265202622526, + "y": 7.4934610231290915, + "heading": 3.6869536728033228, + "angularVelocity": 0.5055225082484697, + "velocityX": -0.2835949984071486, + "velocityY": 0.8974914356315113, + "timestamp": 6.121874746864896 + }, + { + "x": 8.321097373962402, + "y": 7.527496814727783, + "heading": 3.709032536356956, + "angularVelocity": 0.2884937575371967, + "velocityX": -0.15097901394471921, + "velocityY": 0.44472911321759667, + "timestamp": 6.198406259165596 + }, + { + "x": 8.321097373962402, + "y": 7.527496814727783, + "heading": 3.709032536356956, + "angularVelocity": -5.082729725441752e-29, + "velocityX": 4.680465584518782e-28, + "velocityY": 3.556470741173404e-29, + "timestamp": 6.274937771466296 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Tag14.1.traj b/src/main/deploy/choreo/Tag14.1.traj index 26298b3d..64a51f57 100644 --- a/src/main/deploy/choreo/Tag14.1.traj +++ b/src/main/deploy/choreo/Tag14.1.traj @@ -1,31 +1,67 @@ { "samples": [ { - "x": 5.65, + "x": 6, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.9898168855172235e-35, - "velocityX": -4.1906548586207977e-19, - "velocityY": -8.603933715604811e-45, + "angularVelocity": -5.012651915780492e-33, + "velocityX": 8.02537183018205e-19, + "velocityY": -7.958388468544976e-47, "timestamp": 0 }, + { + "x": 5.9633333301031834, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -6.117923804140079e-22, + "velocityX": -0.40300955210174366, + "velocityY": -2.1307554430713835e-36, + "timestamp": 0.09098213604515144 + }, + { + "x": 5.889999993541086, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -7.100860879914842e-22, + "velocityX": -0.8060190686851338, + "velocityY": 3.483301093527144e-37, + "timestamp": 0.1819642720903029 + }, + { + "x": 5.780000006458914, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 1.1538730689460168e-21, + "velocityX": -1.2090284078138351, + "velocityY": 9.187359888346366e-37, + "timestamp": 0.27294640813545434 + }, + { + "x": 5.7066666698968165, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 3.07203086725026e-22, + "velocityX": -0.8060190686851337, + "velocityY": -9.782309229551151e-38, + "timestamp": 0.3639285441806058 + }, { "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.9404859008459245e-25, - "velocityX": 0.2953925521078346, - "velocityY": 2.5211469248369934e-41, - "timestamp": 0.06770651411921333 + "angularVelocity": -1.39204485684163e-22, + "velocityX": -0.40300955210174366, + "velocityY": 9.615122274536806e-37, + "timestamp": 0.4549106802257572 }, { "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.9894322566378973e-35, - "velocityX": -9.732091066591102e-19, - "velocityY": -8.603933345990491e-45, - "timestamp": 0.13541302823842666 + "angularVelocity": -4.017860840401767e-33, + "velocityX": 6.500049543311854e-19, + "velocityY": -4.971127395478411e-47, + "timestamp": 0.5458928162709087 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Tag14.2.traj b/src/main/deploy/choreo/Tag14.2.traj index 297c4300..a5673de3 100644 --- a/src/main/deploy/choreo/Tag14.2.traj +++ b/src/main/deploy/choreo/Tag14.2.traj @@ -4,55 +4,46 @@ "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.9894322566378973e-35, - "velocityX": -9.732091066591102e-19, - "velocityY": -8.603933345990491e-45, + "angularVelocity": -4.017860840401767e-33, + "velocityX": 6.500049543311854e-19, + "velocityY": -4.971127395478411e-47, "timestamp": 0 }, { - "x": 5.7033333348623145, + "x": 5.707500002722882, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 6.224455028031798e-27, - "velocityX": 0.3813501839335422, - "velocityY": -7.87167491563539e-43, - "timestamp": 0.08740872894962237 + "angularVelocity": 1.9645071480983067e-28, + "velocityX": 0.4075634710831348, + "velocityY": 3.190135159299058e-45, + "timestamp": 0.09201021530026487 }, { - "x": 5.770000000000059, + "x": 5.782499997277117, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.1939294645306622e-26, - "velocityX": 0.7627003153894066, - "velocityY": -1.6493125386550898e-42, - "timestamp": 0.17481745789924474 + "angularVelocity": 4.103505651639184e-28, + "velocityX": 0.8151268237932148, + "velocityY": 1.0316108499249623e-44, + "timestamp": 0.18402043060052975 }, { - "x": 5.836666665137633, + "x": 5.82, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.2599779930508123e-26, - "velocityX": 0.7627003153894064, - "velocityY": -1.6423599321913863e-42, - "timestamp": 0.2622261868488671 + "angularVelocity": 1.95973609278201e-28, + "velocityX": 0.40756347108313484, + "velocityY": 5.478051332325139e-45, + "timestamp": 0.2760306459007946 }, { - "x": 5.87, + "x": 5.82, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 6.4603054830567416e-27, - "velocityX": 0.38135018393354214, - "velocityY": -7.88271785574287e-43, - "timestamp": 0.3496349157984895 - }, - { - "x": 5.87, - "y": 4.105148, - "heading": 3.141592653589793, - "angularVelocity": -8.84678881735324e-46, - "velocityX": -2.5654038297220854e-19, - "velocityY": -1.971878449457756e-53, - "timestamp": 0.4370436447481119 + "angularVelocity": -1.4408058549587707e-43, + "velocityX": -8.120067364727127e-19, + "velocityY": -3.194160598149635e-55, + "timestamp": 0.3680408612010595 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Tag14.traj b/src/main/deploy/choreo/Tag14.traj index 19f94d02..79ad0238 100644 --- a/src/main/deploy/choreo/Tag14.traj +++ b/src/main/deploy/choreo/Tag14.traj @@ -1,76 +1,103 @@ { "samples": [ { - "x": 5.65, + "x": 6, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.9898168855172235e-35, - "velocityX": -4.1906548586207977e-19, - "velocityY": -8.603933715604811e-45, + "angularVelocity": -5.012651915780492e-33, + "velocityX": 8.02537183018205e-19, + "velocityY": -7.958388468544976e-47, "timestamp": 0 }, { - "x": 5.67, + "x": 5.9633333301031834, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -6.117923804140079e-22, + "velocityX": -0.40300955210174366, + "velocityY": -2.1307554430713835e-36, + "timestamp": 0.09098213604515144 + }, + { + "x": 5.889999993541086, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": -7.100860879914842e-22, + "velocityX": -0.8060190686851338, + "velocityY": 3.483301093527144e-37, + "timestamp": 0.1819642720903029 + }, + { + "x": 5.780000006458914, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -1.9404859008459245e-25, - "velocityX": 0.2953925521078346, - "velocityY": 2.5211469248369934e-41, - "timestamp": 0.06770651411921333 + "angularVelocity": 1.1538730689460168e-21, + "velocityX": -1.2090284078138351, + "velocityY": 9.187359888346366e-37, + "timestamp": 0.27294640813545434 + }, + { + "x": 5.7066666698968165, + "y": 4.105148, + "heading": 3.141592653589793, + "angularVelocity": 3.07203086725026e-22, + "velocityX": -0.8060190686851337, + "velocityY": -9.782309229551151e-38, + "timestamp": 0.3639285441806058 }, { "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.9894322566378973e-35, - "velocityX": -9.732091066591102e-19, - "velocityY": -8.603933345990491e-45, - "timestamp": 0.13541302823842666 + "angularVelocity": -1.39204485684163e-22, + "velocityX": -0.40300955210174366, + "velocityY": 9.615122274536806e-37, + "timestamp": 0.4549106802257572 }, { - "x": 5.7033333348623145, + "x": 5.67, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 6.224455028031798e-27, - "velocityX": 0.3813501839335422, - "velocityY": -7.87167491563539e-43, - "timestamp": 0.22282175718804903 + "angularVelocity": -4.017860840401767e-33, + "velocityX": 6.500049543311854e-19, + "velocityY": -4.971127395478411e-47, + "timestamp": 0.5458928162709087 }, { - "x": 5.770000000000059, + "x": 5.707500002722882, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.1939294645306622e-26, - "velocityX": 0.7627003153894066, - "velocityY": -1.6493125386550898e-42, - "timestamp": 0.3102304861376714 + "angularVelocity": 1.9645071480983067e-28, + "velocityX": 0.4075634710831348, + "velocityY": 3.190135159299058e-45, + "timestamp": 0.6379030315711736 }, { - "x": 5.836666665137633, + "x": 5.782499997277117, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 1.2599779930508123e-26, - "velocityX": 0.7627003153894064, - "velocityY": -1.6423599321913863e-42, - "timestamp": 0.3976392150872938 + "angularVelocity": 4.103505651639184e-28, + "velocityX": 0.8151268237932148, + "velocityY": 1.0316108499249623e-44, + "timestamp": 0.7299132468714384 }, { - "x": 5.87, + "x": 5.82, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": 6.4603054830567416e-27, - "velocityX": 0.38135018393354214, - "velocityY": -7.88271785574287e-43, - "timestamp": 0.48504794403691615 + "angularVelocity": 1.95973609278201e-28, + "velocityX": 0.40756347108313484, + "velocityY": 5.478051332325139e-45, + "timestamp": 0.8219234621717033 }, { - "x": 5.87, + "x": 5.82, "y": 4.105148, "heading": 3.141592653589793, - "angularVelocity": -8.84678881735324e-46, - "velocityX": -2.5654038297220854e-19, - "velocityY": -1.971878449457756e-53, - "timestamp": 0.5724566729865386 + "angularVelocity": -1.4408058549587707e-43, + "velocityX": -8.120067364727127e-19, + "velocityY": -3.194160598149635e-55, + "timestamp": 0.9139336774719682 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Tag15.1.traj b/src/main/deploy/choreo/Tag15.1.traj index 463bae2a..5b7090f7 100644 --- a/src/main/deploy/choreo/Tag15.1.traj +++ b/src/main/deploy/choreo/Tag15.1.traj @@ -1,76 +1,67 @@ { "samples": [ { - "x": 4.189568967671509, - "y": 5.279922480109792, + "x": 4.301737995, + "y": 5.085640025741264, "heading": -1.0471975512, - "angularVelocity": -8.682191393976642e-30, - "velocityX": -8.883974143721003e-20, - "velocityY": 1.5386233886481345e-19, + "angularVelocity": 2.89796352109676e-29, + "velocityX": -4.014735819374218e-19, + "velocityY": 6.953992905350176e-19, "timestamp": 0 }, { - "x": 4.208499721112847, - "y": 5.247133453323834, + "x": 4.3200713299484095, + "y": 5.053885758138444, "heading": -1.0471975512, - "angularVelocity": -9.594003781408746e-18, - "velocityX": 0.2032139569887226, - "velocityY": -0.35197689831158363, - "timestamp": 0.09315675813738096 + "angularVelocity": -2.7521954694009315e-18, + "velocityX": 0.20150477605089573, + "velocityY": -0.3490165100878853, + "timestamp": 0.09098213604515161 }, { - "x": 4.246361226831558, - "y": 5.181555401767972, + "x": 4.356737998229468, + "y": 4.9903772257313985, "heading": -1.0471975512, - "angularVelocity": -1.757511495393616e-18, - "velocityX": 0.4064279014827589, - "velocityY": -0.7039537749817357, - "timestamp": 0.18631351627476192 + "angularVelocity": -2.0944138451736214e-17, + "velocityX": 0.4030095343426707, + "velocityY": -0.6980329894159418, + "timestamp": 0.18196427209030322 }, { - "x": 4.303153481335755, - "y": 5.083188331490316, + "x": 4.411737991770512, + "y": 4.895114442502254, "heading": -1.0471975512, - "angularVelocity": -4.4708927110588736e-18, - "velocityX": 0.6096418084927427, - "velocityY": -1.0559305867276043, - "timestamp": 0.2794702744121429 + "angularVelocity": -1.0707290083549787e-17, + "velocityX": 0.6045142039064659, + "velocityY": -1.0470493150640965, + "timestamp": 0.27294640813545484 }, { - "x": 4.35994573583996, - "y": 4.984821261212667, + "x": 4.448404660051591, + "y": 4.83160591009522, "heading": -1.0471975512, - "angularVelocity": -2.3203957666119342e-17, - "velocityX": 0.6096418084927429, - "velocityY": -1.055930586727604, - "timestamp": 0.37262703254952384 + "angularVelocity": 2.3522281076555332e-17, + "velocityX": 0.40300953434289255, + "velocityY": -0.6980329894158137, + "timestamp": 0.36392854418060644 }, { - "x": 4.397807241558656, - "y": 4.919243209656798, + "x": 4.466737995, + "y": 4.799851642492399, "heading": -1.0471975512, - "angularVelocity": 3.512195891842984e-17, - "velocityX": 0.406427901482759, - "velocityY": -0.7039537749817355, - "timestamp": 0.4657837906869048 + "angularVelocity": 1.0881053432643224e-17, + "velocityX": 0.20150477605087666, + "velocityY": -0.34901651008789636, + "timestamp": 0.45491068022575804 }, { - "x": 4.416737995000001, - "y": 4.8864541828708425, + "x": 4.466737995, + "y": 4.799851642492399, "heading": -1.0471975512, - "angularVelocity": 3.9044172518056536e-18, - "velocityX": 0.20321395698872258, - "velocityY": -0.35197689831158346, - "timestamp": 0.5589405488242858 - }, - { - "x": 4.416737995, - "y": 4.886454182870843, - "heading": -1.0471975512, - "angularVelocity": -6.644908206829601e-29, - "velocityX": -6.863436538799463e-20, - "velocityY": 1.1889252306500327e-19, - "timestamp": 0.6520973069616668 + "angularVelocity": -5.284169622642589e-29, + "velocityX": -3.257038644202628e-19, + "velocityY": 5.641870908245426e-19, + "timestamp": 0.5458928162709097 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Tag15.2.traj b/src/main/deploy/choreo/Tag15.2.traj index 193d2a2d..d59c10c6 100644 --- a/src/main/deploy/choreo/Tag15.2.traj +++ b/src/main/deploy/choreo/Tag15.2.traj @@ -1,40 +1,49 @@ { "samples": [ { - "x": 4.416737995, - "y": 4.886454182870843, + "x": 4.466737995, + "y": 4.799851642492399, "heading": -1.0471975512, - "angularVelocity": -6.644908206829601e-29, - "velocityX": -6.863436538799463e-20, - "velocityY": 1.1889252306500327e-19, + "angularVelocity": -5.284169622642589e-29, + "velocityX": -3.257038644202628e-19, + "velocityY": 5.641870908245426e-19, "timestamp": 0 }, { - "x": 4.402737994999999, - "y": 4.910702894176807, + "x": 4.4479879936385585, + "y": 4.832327597492402, "heading": -1.0471975512, - "angularVelocity": 2.5156332234400082e-17, - "velocityX": -0.1747566018550504, - "velocityY": 0.30268731337101645, - "timestamp": 0.08011142269527871 + "angularVelocity": 1.6934506076200052e-18, + "velocityX": -0.2037817355415684, + "velocityY": 0.35296031961255897, + "timestamp": 0.09201021530026487 }, { - "x": 4.388737995, - "y": 4.934951605482771, + "x": 4.410487996361441, + "y": 4.897279498060063, "heading": -1.0471975512, - "angularVelocity": -2.5156340257881864e-17, - "velocityX": -0.1747566018550504, - "velocityY": 0.3026873133710164, - "timestamp": 0.1602228453905573 + "angularVelocity": -5.718462154811666e-17, + "velocityX": -0.40756341189660933, + "velocityY": 0.7059205367110453, + "timestamp": 0.18402043060052975 }, { - "x": 4.388737995, - "y": 4.934951605482771, + "x": 4.391737995, + "y": 4.929755453060065, "heading": -1.0471975512, - "angularVelocity": -1.0978097374367874e-28, - "velocityX": 1.0569070815560555e-19, - "velocityY": -1.830330405016386e-19, - "timestamp": 0.2403342680858359 + "angularVelocity": 5.550395167887549e-17, + "velocityX": -0.20378173554156837, + "velocityY": 0.35296031961255897, + "timestamp": 0.2760306459007946 + }, + { + "x": 4.391737995, + "y": 4.929755453060065, + "heading": -1.0471975512, + "angularVelocity": -6.837213333867933e-28, + "velocityX": 4.0578533921404514e-19, + "velocityY": -7.0284075232843425e-19, + "timestamp": 0.3680408612010595 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Tag15.traj b/src/main/deploy/choreo/Tag15.traj index 0ced3c35..d2769b13 100644 --- a/src/main/deploy/choreo/Tag15.traj +++ b/src/main/deploy/choreo/Tag15.traj @@ -1,103 +1,103 @@ { "samples": [ { - "x": 4.189568967671509, - "y": 5.279922480109792, + "x": 4.301737995, + "y": 5.085640025741264, "heading": -1.0471975512, - "angularVelocity": -8.682191393976642e-30, - "velocityX": -8.883974143721003e-20, - "velocityY": 1.5386233886481345e-19, + "angularVelocity": 2.89796352109676e-29, + "velocityX": -4.014735819374218e-19, + "velocityY": 6.953992905350176e-19, "timestamp": 0 }, { - "x": 4.208499721112847, - "y": 5.247133453323834, + "x": 4.3200713299484095, + "y": 5.053885758138444, "heading": -1.0471975512, - "angularVelocity": -9.594003781408746e-18, - "velocityX": 0.2032139569887226, - "velocityY": -0.35197689831158363, - "timestamp": 0.09315675813738096 + "angularVelocity": -2.7521954694009315e-18, + "velocityX": 0.20150477605089573, + "velocityY": -0.3490165100878853, + "timestamp": 0.09098213604515161 }, { - "x": 4.246361226831558, - "y": 5.181555401767972, + "x": 4.356737998229468, + "y": 4.9903772257313985, "heading": -1.0471975512, - "angularVelocity": -1.757511495393616e-18, - "velocityX": 0.4064279014827589, - "velocityY": -0.7039537749817357, - "timestamp": 0.18631351627476192 + "angularVelocity": -2.0944138451736214e-17, + "velocityX": 0.4030095343426707, + "velocityY": -0.6980329894159418, + "timestamp": 0.18196427209030322 }, { - "x": 4.303153481335755, - "y": 5.083188331490316, + "x": 4.411737991770512, + "y": 4.895114442502254, "heading": -1.0471975512, - "angularVelocity": -4.4708927110588736e-18, - "velocityX": 0.6096418084927427, - "velocityY": -1.0559305867276043, - "timestamp": 0.2794702744121429 + "angularVelocity": -1.0707290083549787e-17, + "velocityX": 0.6045142039064659, + "velocityY": -1.0470493150640965, + "timestamp": 0.27294640813545484 }, { - "x": 4.35994573583996, - "y": 4.984821261212667, + "x": 4.448404660051591, + "y": 4.83160591009522, "heading": -1.0471975512, - "angularVelocity": -2.3203957666119342e-17, - "velocityX": 0.6096418084927429, - "velocityY": -1.055930586727604, - "timestamp": 0.37262703254952384 + "angularVelocity": 2.3522281076555332e-17, + "velocityX": 0.40300953434289255, + "velocityY": -0.6980329894158137, + "timestamp": 0.36392854418060644 }, { - "x": 4.397807241558656, - "y": 4.919243209656798, + "x": 4.466737995, + "y": 4.799851642492399, "heading": -1.0471975512, - "angularVelocity": 3.512195891842984e-17, - "velocityX": 0.406427901482759, - "velocityY": -0.7039537749817355, - "timestamp": 0.4657837906869048 + "angularVelocity": 1.0881053432643224e-17, + "velocityX": 0.20150477605087666, + "velocityY": -0.34901651008789636, + "timestamp": 0.45491068022575804 }, { - "x": 4.416737995000001, - "y": 4.8864541828708425, + "x": 4.466737995, + "y": 4.799851642492399, "heading": -1.0471975512, - "angularVelocity": 3.9044172518056536e-18, - "velocityX": 0.20321395698872258, - "velocityY": -0.35197689831158346, - "timestamp": 0.5589405488242858 + "angularVelocity": -5.284169622642589e-29, + "velocityX": -3.257038644202628e-19, + "velocityY": 5.641870908245426e-19, + "timestamp": 0.5458928162709097 }, { - "x": 4.416737995, - "y": 4.886454182870843, + "x": 4.4479879936385585, + "y": 4.832327597492402, "heading": -1.0471975512, - "angularVelocity": -6.644908206829601e-29, - "velocityX": -6.863436538799463e-20, - "velocityY": 1.1889252306500327e-19, - "timestamp": 0.6520973069616668 + "angularVelocity": 1.6934506076200052e-18, + "velocityX": -0.2037817355415684, + "velocityY": 0.35296031961255897, + "timestamp": 0.6379030315711746 }, { - "x": 4.402737994999999, - "y": 4.910702894176807, + "x": 4.410487996361441, + "y": 4.897279498060063, "heading": -1.0471975512, - "angularVelocity": 2.5156332234400082e-17, - "velocityX": -0.1747566018550504, - "velocityY": 0.30268731337101645, - "timestamp": 0.7322087296569455 + "angularVelocity": -5.718462154811666e-17, + "velocityX": -0.40756341189660933, + "velocityY": 0.7059205367110453, + "timestamp": 0.7299132468714394 }, { - "x": 4.388737995, - "y": 4.934951605482771, + "x": 4.391737995, + "y": 4.929755453060065, "heading": -1.0471975512, - "angularVelocity": -2.5156340257881864e-17, - "velocityX": -0.1747566018550504, - "velocityY": 0.3026873133710164, - "timestamp": 0.8123201523522241 + "angularVelocity": 5.550395167887549e-17, + "velocityX": -0.20378173554156837, + "velocityY": 0.35296031961255897, + "timestamp": 0.8219234621717043 }, { - "x": 4.388737995, - "y": 4.934951605482771, + "x": 4.391737995, + "y": 4.929755453060065, "heading": -1.0471975512, - "angularVelocity": -1.0978097374367874e-28, - "velocityX": 1.0569070815560555e-19, - "velocityY": -1.830330405016386e-19, - "timestamp": 0.8924315750475027 + "angularVelocity": -6.837213333867933e-28, + "velocityX": 4.0578533921404514e-19, + "velocityY": -7.0284075232843425e-19, + "timestamp": 0.9139336774719692 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Tag16.1.traj b/src/main/deploy/choreo/Tag16.1.traj index 565d5958..332e9878 100644 --- a/src/main/deploy/choreo/Tag16.1.traj +++ b/src/main/deploy/choreo/Tag16.1.traj @@ -1,76 +1,67 @@ { "samples": [ { - "x": 4.189568967671509, - "y": 2.930373519890208, + "x": 4.301737995, + "y": 3.124655974258736, "heading": 1.0471975512, - "angularVelocity": -2.002499149483344e-30, - "velocityX": -8.884646650147337e-20, - "velocityY": -1.5387579641415467e-19, + "angularVelocity": -7.335351200507843e-30, + "velocityX": -4.0149214769441807e-19, + "velocityY": -6.95484705410363e-19, "timestamp": 0 }, { - "x": 4.208499721112847, - "y": 2.9631625466761657, + "x": 4.320071329948398, + "y": 3.1564102418615634, "heading": 1.0471975512, - "angularVelocity": -7.928469907031793e-18, - "velocityX": 0.20321395698872408, - "velocityY": 0.35197689831158385, - "timestamp": 0.0931567581373812 + "angularVelocity": 5.394453296191308e-18, + "velocityX": 0.20150477605075734, + "velocityY": 0.34901651008796514, + "timestamp": 0.09098213604515158 }, { - "x": 4.2463612268315565, - "y": 3.0287405982320337, + "x": 4.356737998229433, + "y": 3.2199187742686215, "heading": 1.0471975512, - "angularVelocity": -2.0524140454745182e-17, - "velocityX": 0.40642790148276187, - "velocityY": 0.7039537749817363, - "timestamp": 0.1863135162747624 + "angularVelocity": 3.284063714220586e-17, + "velocityX": 0.40300953434242154, + "velocityY": 0.6980329894160855, + "timestamp": 0.18196427209030316 }, { - "x": 4.303153481335761, - "y": 3.127107668509687, + "x": 4.411737991770526, + "y": 3.315181557497739, "heading": 1.0471975512, - "angularVelocity": 5.366441184028338e-17, - "velocityX": 0.6096418084927471, - "velocityY": 1.055930586727605, - "timestamp": 0.27947027441214356 + "angularVelocity": -7.726708537325842e-18, + "velocityX": 0.6045142039069931, + "velocityY": 1.0470493150637918, + "timestamp": 0.27294640813545473 }, { - "x": 4.3599457358399505, - "y": 3.225474738787313, + "x": 4.448404660051587, + "y": 3.3786900899047825, "heading": 1.0471975512, - "angularVelocity": -4.609538572640182e-17, - "velocityX": 0.6096418084927472, - "velocityY": 1.0559305867276048, - "timestamp": 0.3726270325495248 + "angularVelocity": -3.8700184388956635e-17, + "velocityX": 0.40300953434269476, + "velocityY": 0.6980329894159277, + "timestamp": 0.36392854418060633 }, { - "x": 4.397807241558657, - "y": 3.291052790343219, + "x": 4.466737995, + "y": 3.410444357507601, "heading": 1.0471975512, - "angularVelocity": 9.988825598390611e-18, - "velocityX": 0.4064279014827619, - "velocityY": 0.7039537749817361, - "timestamp": 0.465783790686906 + "angularVelocity": 8.191735638164562e-18, + "velocityX": 0.20150477605093248, + "velocityY": 0.34901651008786405, + "timestamp": 0.4549106802257579 }, { - "x": 4.416737994999997, - "y": 3.323841817129139, + "x": 4.466737995, + "y": 3.410444357507601, "heading": 1.0471975512, - "angularVelocity": 1.0894764403980658e-17, - "velocityX": 0.20321395698872408, - "velocityY": 0.35197689831158374, - "timestamp": 0.5589405488242872 - }, - { - "x": 4.416737995, - "y": 3.323841817129157, - "heading": 1.0471975512, - "angularVelocity": -2.786574070680907e-28, - "velocityX": -6.865066631576706e-20, - "velocityY": -1.1890733865081739e-19, - "timestamp": 0.6520973069616685 + "angularVelocity": -8.684839191459878e-29, + "velocityX": -3.2590018149845075e-19, + "velocityY": -5.6430407567783995e-19, + "timestamp": 0.5458928162709095 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Tag16.2.traj b/src/main/deploy/choreo/Tag16.2.traj index c8eda2ba..25284420 100644 --- a/src/main/deploy/choreo/Tag16.2.traj +++ b/src/main/deploy/choreo/Tag16.2.traj @@ -1,40 +1,49 @@ { "samples": [ { - "x": 4.416737995, - "y": 3.323841817129157, + "x": 4.466737995, + "y": 3.410444357507601, "heading": 1.0471975512, - "angularVelocity": -2.786574070680907e-28, - "velocityX": -6.865066631576706e-20, - "velocityY": -1.1890733865081739e-19, + "angularVelocity": -8.684839191459878e-29, + "velocityX": -3.2590018149845075e-19, + "velocityY": -5.6430407567783995e-19, "timestamp": 0 }, { - "x": 4.402737994999996, - "y": 3.299593105823197, + "x": 4.4479879936385585, + "y": 3.377968402507599, "heading": 1.0471975512, - "angularVelocity": 1.2203521575276725e-17, - "velocityX": -0.17475660185504371, - "velocityY": -0.30268731337102, - "timestamp": 0.0801114226952786 + "angularVelocity": 1.3525551603206105e-18, + "velocityX": -0.2037817355415677, + "velocityY": -0.3529603196125604, + "timestamp": 0.0920102153002651 }, { - "x": 4.388737995, - "y": 3.275344394517229, + "x": 4.410487996361441, + "y": 3.3130165019399365, "heading": 1.0471975512, - "angularVelocity": -1.2203535086762994e-17, - "velocityX": -0.17475660185504374, - "velocityY": -0.30268731337102006, - "timestamp": 0.1602228453905572 + "angularVelocity": 1.8398786975520478e-17, + "velocityX": -0.40756341189660816, + "velocityY": -0.7059205367110482, + "timestamp": 0.1840204306005302 }, { - "x": 4.388737995, - "y": 3.275344394517229, + "x": 4.391737995, + "y": 3.280540546939935, "heading": 1.0471975512, - "angularVelocity": -5.52100607399031e-28, - "velocityX": 1.0568999281101467e-19, - "velocityY": 1.8303406915772078e-19, - "timestamp": 0.2403342680858358 + "angularVelocity": -1.974689064536144e-17, + "velocityX": -0.20378173554156773, + "velocityY": -0.35296031961256047, + "timestamp": 0.2760306459007953 + }, + { + "x": 4.391737995, + "y": 3.280540546939935, + "heading": 1.0471975512, + "angularVelocity": -5.432293012393194e-29, + "velocityX": 4.058597366698449e-19, + "velocityY": 7.02969590180103e-19, + "timestamp": 0.3680408612010604 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Tag16.traj b/src/main/deploy/choreo/Tag16.traj index 60c8c0e0..5f4b954e 100644 --- a/src/main/deploy/choreo/Tag16.traj +++ b/src/main/deploy/choreo/Tag16.traj @@ -1,103 +1,103 @@ { "samples": [ { - "x": 4.189568967671509, - "y": 2.930373519890208, + "x": 4.301737995, + "y": 3.124655974258736, "heading": 1.0471975512, - "angularVelocity": -2.002499149483344e-30, - "velocityX": -8.884646650147337e-20, - "velocityY": -1.5387579641415467e-19, + "angularVelocity": -7.335351200507843e-30, + "velocityX": -4.0149214769441807e-19, + "velocityY": -6.95484705410363e-19, "timestamp": 0 }, { - "x": 4.208499721112847, - "y": 2.9631625466761657, + "x": 4.320071329948398, + "y": 3.1564102418615634, "heading": 1.0471975512, - "angularVelocity": -7.928469907031793e-18, - "velocityX": 0.20321395698872408, - "velocityY": 0.35197689831158385, - "timestamp": 0.0931567581373812 + "angularVelocity": 5.394453296191308e-18, + "velocityX": 0.20150477605075734, + "velocityY": 0.34901651008796514, + "timestamp": 0.09098213604515158 }, { - "x": 4.2463612268315565, - "y": 3.0287405982320337, + "x": 4.356737998229433, + "y": 3.2199187742686215, "heading": 1.0471975512, - "angularVelocity": -2.0524140454745182e-17, - "velocityX": 0.40642790148276187, - "velocityY": 0.7039537749817363, - "timestamp": 0.1863135162747624 + "angularVelocity": 3.284063714220586e-17, + "velocityX": 0.40300953434242154, + "velocityY": 0.6980329894160855, + "timestamp": 0.18196427209030316 }, { - "x": 4.303153481335761, - "y": 3.127107668509687, + "x": 4.411737991770526, + "y": 3.315181557497739, "heading": 1.0471975512, - "angularVelocity": 5.366441184028338e-17, - "velocityX": 0.6096418084927471, - "velocityY": 1.055930586727605, - "timestamp": 0.27947027441214356 + "angularVelocity": -7.726708537325842e-18, + "velocityX": 0.6045142039069931, + "velocityY": 1.0470493150637918, + "timestamp": 0.27294640813545473 }, { - "x": 4.3599457358399505, - "y": 3.225474738787313, + "x": 4.448404660051587, + "y": 3.3786900899047825, "heading": 1.0471975512, - "angularVelocity": -4.609538572640182e-17, - "velocityX": 0.6096418084927472, - "velocityY": 1.0559305867276048, - "timestamp": 0.3726270325495248 + "angularVelocity": -3.8700184388956635e-17, + "velocityX": 0.40300953434269476, + "velocityY": 0.6980329894159277, + "timestamp": 0.36392854418060633 }, { - "x": 4.397807241558657, - "y": 3.291052790343219, + "x": 4.466737995, + "y": 3.410444357507601, "heading": 1.0471975512, - "angularVelocity": 9.988825598390611e-18, - "velocityX": 0.4064279014827619, - "velocityY": 0.7039537749817361, - "timestamp": 0.465783790686906 + "angularVelocity": 8.191735638164562e-18, + "velocityX": 0.20150477605093248, + "velocityY": 0.34901651008786405, + "timestamp": 0.4549106802257579 }, { - "x": 4.416737994999997, - "y": 3.323841817129139, + "x": 4.466737995, + "y": 3.410444357507601, "heading": 1.0471975512, - "angularVelocity": 1.0894764403980658e-17, - "velocityX": 0.20321395698872408, - "velocityY": 0.35197689831158374, - "timestamp": 0.5589405488242872 + "angularVelocity": -8.684839191459878e-29, + "velocityX": -3.2590018149845075e-19, + "velocityY": -5.6430407567783995e-19, + "timestamp": 0.5458928162709095 }, { - "x": 4.416737995, - "y": 3.323841817129157, + "x": 4.4479879936385585, + "y": 3.377968402507599, "heading": 1.0471975512, - "angularVelocity": -2.786574070680907e-28, - "velocityX": -6.865066631576706e-20, - "velocityY": -1.1890733865081739e-19, - "timestamp": 0.6520973069616685 + "angularVelocity": 1.3525551603206105e-18, + "velocityX": -0.2037817355415677, + "velocityY": -0.3529603196125604, + "timestamp": 0.6379030315711746 }, { - "x": 4.402737994999996, - "y": 3.299593105823197, + "x": 4.410487996361441, + "y": 3.3130165019399365, "heading": 1.0471975512, - "angularVelocity": 1.2203521575276725e-17, - "velocityX": -0.17475660185504371, - "velocityY": -0.30268731337102, - "timestamp": 0.732208729656947 + "angularVelocity": 1.8398786975520478e-17, + "velocityX": -0.40756341189660816, + "velocityY": -0.7059205367110482, + "timestamp": 0.7299132468714397 }, { - "x": 4.388737995, - "y": 3.275344394517229, + "x": 4.391737995, + "y": 3.280540546939935, "heading": 1.0471975512, - "angularVelocity": -1.2203535086762994e-17, - "velocityX": -0.17475660185504374, - "velocityY": -0.30268731337102006, - "timestamp": 0.8123201523522257 + "angularVelocity": -1.974689064536144e-17, + "velocityX": -0.20378173554156773, + "velocityY": -0.35296031961256047, + "timestamp": 0.8219234621717048 }, { - "x": 4.388737995, - "y": 3.275344394517229, + "x": 4.391737995, + "y": 3.280540546939935, "heading": 1.0471975512, - "angularVelocity": -5.52100607399031e-28, - "velocityX": 1.0568999281101467e-19, - "velocityY": 1.8303406915772078e-19, - "timestamp": 0.8924315750475043 + "angularVelocity": -5.432293012393194e-29, + "velocityX": 4.058597366698449e-19, + "velocityY": 7.02969590180103e-19, + "timestamp": 0.9139336774719699 } ], "eventMarkers": [] diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index afb1d318..23e383fc 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -65,7 +65,7 @@ public static class Drivetrain { public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (IS_L3 ? 16.0 / 28.0 : 17.0 / 27.0) * (45.0 / 15.0); public static final double TURN_GEAR_RATIO = 150.0 / 7.0; public static final boolean IS_TURN_MOTOR_INVERTED = true; - public static final double WHEEL_RADIUS = Units.inchesToMeters(1.972022165051841); + public static final double WHEEL_RADIUS = Units.inchesToMeters(1.9423034948238156); public static final double MAX_LINEAR_SPEED = Units.feetToMeters(IS_L3 ? 16.0 : 15.7); public static final double TRACK_WIDTH_X = Units.inchesToMeters(18.75); @@ -111,7 +111,7 @@ public static class Vision { public static final String REAR_CAMERA_NAME = "limelight-rear"; public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.086018, 0, 0.627079, new Rotation3d(0, Math.toRadians(-40.843), 0)); - public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(0, Math.toRadians(10), Math.PI)); + public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(0, Math.toRadians(10), Math.PI + 0.043)); public static final boolean TAKE_SNAPSHOTS = true; public static final double SNAPSHOT_PERIOD_SECS = 1; @@ -149,7 +149,7 @@ public static class Flywheels { public static final double KS = 0.26925; public static final double KV = 0.07485; - public static final double ERROR_TOLERANCE_RPM = 1000; + public static final double ERROR_TOLERANCE_RPM = 2000; } public static class Pivot { @@ -204,7 +204,7 @@ public static class Elevator { public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25); public static final double MINIMUM_HEIGHT = Units.inchesToMeters(-2); //TODO: Does this make it angry? public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0); - public static final double MAX_HEIGHT = 0.5; //TODO: Fix these constants to be more accurate + public static final double MAX_HEIGHT = 0.52; //TODO: Fix these constants to be more accurate public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + MAX_HEIGHT - MINIMUM_HEIGHT; public static final double GEAR_RATIO = 11.571; diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 50f54fc4..fb4843c9 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -163,14 +163,17 @@ public void enabledInit() { */ @Override public void autonomousInit() { - enabledInit(); + // robotContainer.leds.setPatternAll(LedPatternFlame::new, Leds.PatternCriticality.HIGH); robotContainer.drivetrain.blockTags(); + autonomousCommand = robotContainer.getAutonomousCommand(); + // schedule the autonomous command (example) if (autonomousCommand != null) { autonomousCommand.schedule(); } + } /** diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index ec98aade..f2c4fb5e 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -253,20 +253,25 @@ private void configureAutoRoutines() { autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.COUNTER_CLOCKWISE))); } autos.addDefault(new AutoCommand("Dwayne :skull:")); - autos.add(new AutoCommand("SubwooferShot", new ShootSequence(shooter, indexer))); - autos.add(new DriveSinglePath("Taxi", drivetrain)); - autos.add(new DriveSinglePath("Sprint", drivetrain)); - autos.add(new DriveSinglePath("CenterLaneSprint", drivetrain, true, true)); - autos.add(new AmpLanePADESprint(drivetrain, shooter, indexer)); - autos.add(new CenterLanePSubSprint(drivetrain, shooter, indexer)); - autos.add(new CenterLanePCBADSprint(drivetrain, shooter, indexer)); - autos.add(new CenterLanePCBAFSprint(drivetrain, shooter, indexer)); -// autos.add(new CenterLanePCBAFE(drivetrain, shooter, indexer)); - autos.add(new CenterLanePCBA(drivetrain, shooter, indexer)); - autos.add(new CenterLanePBDA(drivetrain, shooter, indexer)); - autos.add(new CenterLanePSubCSubBSubASubFSub(drivetrain, shooter, indexer)); -// autos.add(new CenterLanePSubCSubBSubFSub(drivetrain, shooter, indexer)); - autos.add(new CenterLanePSubCSubBSubASub(drivetrain, shooter, indexer)); + autos.add(new AmpLanePADEF(drivetrain, shooter, indexer)); + autos.add(new CenterLanePDEABC(drivetrain, shooter, indexer)); + autos.add(new SourceLanePGHF(drivetrain, shooter, indexer)); + autos.add(new AmpLanepdChaos(drivetrain, shooter, indexer)); + autos.add(new SoureLanephChaos(drivetrain, shooter, indexer)); +// autos.add(new AutoCommand("SubwooferShot", new ShootSequence(shooter, indexer))); +// autos.add(new DriveSinglePath("Taxi", drivetrain)); +// autos.add(new DriveSinglePath("Sprint", drivetrain)); +// autos.add(new DriveSinglePath("CenterLaneSprint", drivetrain, true, true)); +// autos.add(new AmpLanePADESprint(drivetrain, shooter, indexer)); +// autos.add(new CenterLanePSubSprint(drivetrain, shooter, indexer)); +// autos.add(new CenterLanePCBADSprint(drivetrain, shooter, indexer)); +// autos.add(new CenterLanePCBAFSprint(drivetrain, shooter, indexer)); +//// autos.add(new CenterLanePCBAFE(drivetrain, shooter, indexer)); +// autos.add(new CenterLanePCBA(drivetrain, shooter, indexer)); +// autos.add(new CenterLanePBDA(drivetrain, shooter, indexer)); +// autos.add(new CenterLanePSubCSubBSubASubFSub(drivetrain, shooter, indexer)); +//// autos.add(new CenterLanePSubCSubBSubFSub(drivetrain, shooter, indexer)); +// autos.add(new CenterLanePSubCSubBSubASub(drivetrain, shooter, indexer)); autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer)); // autos.addDefault(new ATestAuto(drivetrain, shooter, indexer)); } diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADEF.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADEF.java new file mode 100644 index 00000000..8254ab4a --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePADEF.java @@ -0,0 +1,74 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; +import org.team1540.robot2024.commands.indexer.IntakeAndFeed; +import org.team1540.robot2024.commands.indexer.IntakeCommand; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.commands.shooter.AutoShootPrepareWhileMoving; +import org.team1540.robot2024.commands.shooter.LeadingShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class AmpLanePADEF extends AutoCommand { + + public AmpLanePADEF(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("!AmpLanePADEF"); + + addPath( + PathHelper.fromChoreoPath("AmpLanePADEF.1", true, true), + PathHelper.fromChoreoPath("AmpLanePADEF.2"), + PathHelper.fromChoreoPath("AmpLanePADEF.3"), + PathHelper.fromChoreoPath("AmpLanePADEF.4"), + PathHelper.fromChoreoPath("AmpLanePADEF.5") + ); + + addCommands( +// Commands.runOnce(drivetrain::unblockTags), + Commands.parallel( + new LeadingShootPrepare(drivetrain, shooter), +// Commands.sequence( +// Commands.deadline( +// getPath(0).getCommand(drivetrain), +// Commands.sequence( +// Commands.waitSeconds(0.25), +// new InstantCommand(shooter::zeroPivotToCancoder), +// Commands.waitSeconds(1), +// IntakeAndFeed.withDefaults(indexer).withTimeout(2), +// new IntakeCommand(indexer, () -> false, 1) +// ) +// ), +// Commands.sequence( +// Commands.waitSeconds(0.25), +// new InstantCommand(shooter::zeroPivotToCancoder) +// ), +// drivetrain.commandCopyVisionPose(), +// Commands.parallel( +// new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4), +// Commands.sequence( +// new ParallelDeadlineGroup( +// Commands.sequence( +// Commands.waitUntil(()->!indexer.isNoteStaged()), +// Commands.waitSeconds(0.2) +// ).withTimeout(1), +// IntakeAndFeed.withDefaults(indexer) +// ) +// ) +// ), + Commands.sequence( + createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), + createSegmentSequence(drivetrain, shooter, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 2), + createSegmentSequence(drivetrain, shooter, indexer, 3), + createSegmentSequence(drivetrain, shooter, indexer, 4) + ) +// ) + ) + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanepdChaos.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanepdChaos.java new file mode 100644 index 00000000..ee213674 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanepdChaos.java @@ -0,0 +1,35 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.indexer.IntakeAndFeed; +import org.team1540.robot2024.commands.indexer.IntakeCommand; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class AmpLanepdChaos extends AutoCommand { + + public AmpLanepdChaos(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("!AmpLanepdChaos"); + + addPath( + PathHelper.fromChoreoPath("AmpLanepdChaos.1", true, true) + ); + + addCommands( + Commands.parallel( + new AutoShootPrepare(drivetrain, shooter), + getPath(0).getCommand(drivetrain), + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.runOnce(shooter::zeroPivotToCancoder), + IntakeAndFeed.withDefaults(indexer).withTimeout(0.5), + new IntakeCommand(indexer, ()->false, 1) + ) + ) + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java new file mode 100644 index 00000000..f982d442 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java @@ -0,0 +1,72 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.*; +import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; +import org.team1540.robot2024.commands.indexer.IntakeAndFeed; +import org.team1540.robot2024.commands.indexer.IntakeCommand; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.commands.shooter.AutoShootPrepareWhileMoving; +import org.team1540.robot2024.commands.shooter.LeadingShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class CenterLanePDEABC extends AutoCommand { + + public CenterLanePDEABC(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("!CenterLanePDEABC"); + + addPath( + PathHelper.fromChoreoPath("CenterLanePDEABC.1", true, true), + PathHelper.fromChoreoPath("CenterLanePDEABC.2"), + PathHelper.fromChoreoPath("CenterLanePDEABC.3"), + PathHelper.fromChoreoPath("CenterLanePDEABC.4"), + PathHelper.fromChoreoPath("CenterLanePDEABC.5") + ); + + addCommands( + Commands.parallel( + new LeadingShootPrepare(drivetrain, shooter), + Commands.sequence( + Commands.runOnce(drivetrain::unblockTags), +// drivetrain.commandCopyVisionPose(), +// Commands.deadline( +// Commands.waitSeconds(0.2).andThen(getPath(0).getCommand(drivetrain)), + Commands.sequence( +// Commands.waitSeconds(0.25), +// Commands.waitSeconds(0.05), + IntakeAndFeed.withDefaults(indexer).withTimeout(0.2) +// new IntakeCommand(indexer, () -> false, 1) + ), +// ) +// , +// Commands.sequence( +// Commands.waitSeconds(0.25), +// new InstantCommand(shooter::zeroPivotToCancoder) +// ), +// drivetrain.commandCopyVisionPose(), +// Commands.parallel( +// new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4), +// Commands.sequence( +// new ParallelDeadlineGroup( +// Commands.sequence( +// Commands.waitUntil(()->!indexer.isNoteStaged()), +// Commands.waitSeconds(0.2) +// ).withTimeout(1), +// IntakeAndFeed.withDefaults(indexer) +// ) +// ) +// ), + createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), + createSegmentSequence(drivetrain, shooter, indexer, 1), + Commands.runOnce(drivetrain::blockTags), + createSegmentSequence(drivetrain, shooter, indexer, 2), + createSegmentSequence(drivetrain, shooter, indexer, 3), + createSegmentSequence(drivetrain, shooter, indexer, 4) + ) + ) + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHF.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHF.java new file mode 100644 index 00000000..25cce318 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHF.java @@ -0,0 +1,35 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class SourceLanePGHF extends AutoCommand { + private static final double SHOT_WAIT = 0.2; + public SourceLanePGHF(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ + super("!SourceLanePHGF"); + addPath( + PathHelper.fromChoreoPath("SourceLanePHGF.1", true, true), + PathHelper.fromChoreoPath("SourceLanePHGF.2"), + PathHelper.fromChoreoPath("SourceLanePHGF.3"), + PathHelper.fromChoreoPath("SourceLanePHGF.4") + ); + + addCommands( + Commands.parallel( + new AutoShootPrepare(drivetrain, shooter), + Commands.sequence( + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0) + ) + ) + ); + } + +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SoureLanephChaos.java b/src/main/java/org/team1540/robot2024/commands/autos/SoureLanephChaos.java new file mode 100644 index 00000000..4d77198e --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/SoureLanephChaos.java @@ -0,0 +1,35 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.indexer.IntakeAndFeed; +import org.team1540.robot2024.commands.indexer.IntakeCommand; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class SoureLanephChaos extends AutoCommand { + + public SoureLanephChaos(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("!SourceLanephChaos"); + + addPath( + PathHelper.fromChoreoPath("SourceLanephChaos.1", true, true) + ); + + addCommands( + Commands.parallel( + new AutoShootPrepare(drivetrain, shooter), + getPath(0).getCommand(drivetrain), + Commands.sequence( + Commands.waitSeconds(0.25), + Commands.runOnce(shooter::zeroPivotToCancoder), + IntakeAndFeed.withDefaults(indexer).withTimeout(0.5), + new IntakeCommand(indexer, ()->false, 1) + ) + ) + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java index dc93164d..fec1bb68 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java @@ -72,6 +72,6 @@ else if(amp < source){ else{ pathCmd= AutoBuilder.pathfindThenFollowPath(PathHelper.fromChoreoPath("Tag16." + index).getPath(), STAGE_PATH_CONSTRAINTS); } - return Commands.runOnce(drivetrain::blockTags).andThen(pathCmd).andThen(Commands.runOnce(drivetrain::unblockTags)); + return Commands.runOnce(drivetrain::unblockTags).andThen(pathCmd).andThen(Commands.runOnce(drivetrain::unblockTags)); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 77cce713..7e9a7abe 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -22,6 +22,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -354,7 +355,7 @@ public void addVisionMeasurement(EstimatedVisionPose visionPose) { Matrix stdDevs = visionPose.getStdDevs(); visionPoseEstimator.setVisionMeasurementStdDevs(stdDevs); visionPoseEstimator.addVisionMeasurement(visionPose.poseMeters.toPose2d(), visionPose.timestampSecs); - if (!blockTags) { + if (!blockTags && (!RobotState.isAutonomous() || getSpeakerDistanceMeters() < 4.5)) { poseEstimator.setVisionMeasurementStdDevs(stdDevs); poseEstimator.addVisionMeasurement(visionPose.poseMeters.toPose2d(), visionPose.timestampSecs); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java index 1b9adbdf..7c9eb6a0 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java @@ -66,6 +66,9 @@ public void periodic() { if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { io.configPID(kP.get(), kI.get(), kD.get()); } + if(getPosition() > 0.31){ + setFlipper(false); + } } public void setElevatorPosition(double positionMeters) { diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index 84976249..cf17f292 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -133,14 +133,6 @@ public Command commandRunIntake(double percent) { ); } - public Command setIntakeAndFeeder(double intakePercent, double feederVelocity) { - return Commands.startEnd( - () -> {setIntakePercent(intakePercent); setFeederVelocity(feederVelocity);}, - () -> {setIntakePercent(0); setFeederVelocity(0);}, - this - ); - } - public void setIntakeNeutralMode(boolean isBrake) { io.setIntakeNeutralMode(isBrake); } diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index 3948e432..22e9f6ce 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -97,7 +97,6 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, ).onlyIf(()->shouldZeroCancoder), drivetrain.commandCopyVisionPose().onlyIf(()->shouldResetOdometry), Commands.parallel( - new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4).onlyIf(()->shouldRealignYaw), Commands.sequence( // Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10).onlyIf(()->shouldRealignYaw), new ParallelDeadlineGroup( @@ -106,9 +105,11 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Commands.waitUntil(()->!indexer.isNoteStaged()), Commands.waitSeconds(0.2) ).withTimeout(1+extraPreShotWait), - IntakeAndFeed.withDefaults(indexer) + Commands.waitSeconds(extraPreShotWait).andThen(IntakeAndFeed.withDefaults(indexer)) ) - ) + ), + new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4).onlyIf(()->shouldRealignYaw) + ) ); } From 0f32b452c6b2e6e00f7cf6fa2f348952e384c65b Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Sun, 31 Mar 2024 20:50:27 -0700 Subject: [PATCH 67/75] feat: untested but theoretically sound auto speedups --- .../robot2024/util/auto/AutoCommand.java | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index 22e9f6ce..4c392ab0 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -96,20 +96,16 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, new InstantCommand(shooter::zeroPivotToCancoder) ).onlyIf(()->shouldZeroCancoder), drivetrain.commandCopyVisionPose().onlyIf(()->shouldResetOdometry), - Commands.parallel( - Commands.sequence( -// Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10).onlyIf(()->shouldRealignYaw), - new ParallelDeadlineGroup( - Commands.sequence( - Commands.waitSeconds(extraPreShotWait), - Commands.waitUntil(()->!indexer.isNoteStaged()), - Commands.waitSeconds(0.2) - ).withTimeout(1+extraPreShotWait), - Commands.waitSeconds(extraPreShotWait).andThen(IntakeAndFeed.withDefaults(indexer)) - ) + Commands.deadline( + Commands.deadline( + Commands.sequence( + Commands.waitSeconds(extraPreShotWait), + Commands.waitUntil(()->!indexer.isNoteStaged()), + Commands.waitSeconds(0.1) + ).withTimeout(1+extraPreShotWait), + Commands.waitSeconds(extraPreShotWait).andThen(IntakeAndFeed.withDefaults(indexer)) ), new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4).onlyIf(()->shouldRealignYaw) - ) ); } From 8090624894fa8769813fa2b063e3a1352b38eb5a Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Mon, 1 Apr 2024 20:57:36 -0700 Subject: [PATCH 68/75] fix: apply weird limelight offset correctly --- .../vision/AprilTagVisionIOLimelight.java | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java index 2e5b088c..40d0f6d9 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -1,12 +1,6 @@ package org.team1540.robot2024.subsystems.vision; -import com.pathplanner.lib.util.GeometryUtil; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.Timer; import org.team1540.robot2024.util.vision.LimelightHelpers; public class AprilTagVisionIOLimelight implements AprilTagVisionIO { @@ -23,9 +17,12 @@ public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) { public void updateInputs(AprilTagVisionIOInputs inputs) { LimelightHelpers.PoseEstimate measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); if (measurement.tagCount > 1) { - inputs.estimatedPoseMeters = - LimelightHelpers.getBotPose3d_wpiBlue(name) - .transformBy(new Transform3d(0, 0.105, 0, new Rotation3d())); + Pose3d limelightPose = LimelightHelpers.getBotPose3d_wpiBlue(name); + inputs.estimatedPoseMeters = new Pose3d( + limelightPose.getX(), + limelightPose.getY() - 10.5, + limelightPose.getZ(), + limelightPose.getRotation()); inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds; } inputs.numTagsSeen = measurement.tagCount; From 70a7d2010ad31e43fa888614f45d6de888a07d56 Mon Sep 17 00:00:00 2001 From: Zach Rutman Date: Tue, 2 Apr 2024 15:28:40 -0700 Subject: [PATCH 69/75] fix brake mode (#43) * feat: add usable brake mode logic * fix: remove enabledInit, add FMS check for isPreMatch --- .../java/org/team1540/robot2024/Robot.java | 30 ++++----- .../team1540/robot2024/RobotContainer.java | 61 ++++++++++++++----- .../robot2024/subsystems/indexer/Indexer.java | 4 +- .../subsystems/indexer/IndexerIO.java | 2 +- .../subsystems/indexer/IndexerIOSparkMax.java | 2 +- .../subsystems/indexer/IndexerIOTalonFX.java | 2 +- .../robot2024/subsystems/led/Leds.java | 14 +---- 7 files changed, 65 insertions(+), 50 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index fb4843c9..8b510894 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -98,8 +98,7 @@ public void robotInit() { // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); - robotContainer.shooter.setPivotBrakeMode(false); - robotContainer.drivetrain.setBrakeMode(false); + robotContainer.disableBrakeMode(); // Pathplanner warmup (helps prevents delays at the start of auto) FollowPathCommand.warmupCommand().schedule(); @@ -134,9 +133,6 @@ public void robotPeriodic() { */ @Override public void disabledInit() { -// robotContainer.elevator.setBrakeMode(false); - robotContainer.shooter.setPivotBrakeMode(false); - robotContainer.indexer.setIntakeNeutralMode(false); robotContainer.drivetrain.unblockTags(); robotContainer.shooter.setPivotPosition(new Rotation2d()); robotContainer.leds.setPattern(Leds.Zone.MAIN, new LedPatternRainbow(1)); @@ -151,19 +147,11 @@ public void disabledPeriodic() { AutoManager.getInstance().updateSelected(); } - public void enabledInit() { - robotContainer.elevator.setBrakeMode(true); - robotContainer.shooter.setPivotBrakeMode(true); - robotContainer.drivetrain.setBrakeMode(true); - robotContainer.indexer.setIntakeNeutralMode(true); - } - /** * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - // robotContainer.leds.setPatternAll(LedPatternFlame::new, Leds.PatternCriticality.HIGH); robotContainer.drivetrain.blockTags(); @@ -176,6 +164,11 @@ public void autonomousInit() { } + @Override + public void autonomousExit() { + robotContainer.enableBrakeMode(true); + } + /** * This function is called periodically during autonomous. */ @@ -183,12 +176,14 @@ public void autonomousInit() { public void autonomousPeriodic() { } + + /** * This function is called once when teleop is enabled. */ @Override public void teleopInit() { - enabledInit(); + robotContainer.enableBrakeMode(false); // robotContainer.leds.setPatternAll(() -> new LedPatternRainbow(2), Leds.PatternCriticality.HIGH); robotContainer.drivetrain.zeroFieldOrientation();// TODO: remove this once odometry / startup zero is good @@ -197,6 +192,10 @@ public void teleopInit() { } } + @Override + public void teleopExit() { + robotContainer.enableBrakeMode(true); + } /** * This function is called periodically during operator control. */ @@ -209,9 +208,6 @@ public void teleopPeriodic() { */ @Override public void testInit() { - robotContainer.leds.setPattern(Leds.Zone.MAIN,new LedPatternTuneColor()); - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); } /** diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index f2c4fb5e..448e7be7 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -4,12 +4,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import org.littletonrobotics.junction.Logger; import org.team1540.robot2024.commands.FeedForwardCharacterization; import org.team1540.robot2024.commands.autos.*; import org.team1540.robot2024.commands.climb.ClimbAlignment; @@ -27,10 +26,7 @@ import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.led.Leds; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternProgressBar; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternWave; -import org.team1540.robot2024.subsystems.led.patterns.SimpleLedPattern; +import org.team1540.robot2024.subsystems.led.patterns.*; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.subsystems.tramp.Tramp; import org.team1540.robot2024.subsystems.vision.AprilTagVision; @@ -39,6 +35,8 @@ import org.team1540.robot2024.util.auto.AutoCommand; import org.team1540.robot2024.util.auto.AutoManager; +import java.util.function.BooleanSupplier; + import static org.team1540.robot2024.Constants.SwerveConfig; import static org.team1540.robot2024.Constants.isTuningMode; @@ -58,6 +56,9 @@ public class RobotContainer { public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS); + + + public boolean isBrakeMode; /** * The container for the robot. Contains subsystems, IO devices, and commands. */ @@ -130,7 +131,7 @@ private void configureButtonBindings() { driver.b().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); driver.y().onTrue(Commands.runOnce(() -> { drivetrain.zeroFieldOrientationManual(); - drivetrain.setBrakeMode(true); + enableBrakeMode(false); }).ignoringDisable(true)); Command targetDrive = new AutoShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter) @@ -214,17 +215,24 @@ private void configureButtonBindings() { new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4)); - new Trigger(RobotController::getUserButton).toggleOnTrue(Commands.startEnd( - () -> { - elevator.setBrakeMode(false); - leds.setPatternAll(() -> new LedPatternRSLState(Color.kMagenta), Leds.PatternLevel.ELEVATOR_STATE); - }, + + BooleanSupplier isPreMatch = () -> (!DriverStation.isDSAttached() || !DriverStation.isFMSAttached() || DriverStation.isAutonomous()) && DriverStation.isDisabled(); + Command brakeModeCommand = Commands.runOnce( () -> { - elevator.setBrakeMode(true); - leds.clearPatternAll(Leds.PatternLevel.ELEVATOR_STATE); + if (!isPreMatch.getAsBoolean()) {return;} + if (!isBrakeMode) { + enableBrakeMode(true); + } else { + disableBrakeMode(); + } } - ).ignoringDisable(true)); + ).ignoringDisable(true); + + new Trigger(tramp::isNoteStaged).debounce(0.1) + .onTrue(brakeModeCommand); + driver.start().onTrue(brakeModeCommand); + copilot.start().onTrue(brakeModeCommand); } @@ -286,4 +294,27 @@ private void configureAutoRoutines() { public Command getAutonomousCommand() { return AutoManager.getInstance().getSelected(); } + + public void enableBrakeMode(boolean requireRobotDisabled) { + if (!isBrakeMode && (!requireRobotDisabled || !DriverStation.isEnabled())) { + shooter.setPivotBrakeMode(true); + indexer.setIntakeBrakeMode(true); + elevator.setBrakeMode(true); + drivetrain.setBrakeMode(true); + leds.clearPattern(Leds.Zone.MAIN, Leds.PatternLevel.COAST_STATE); + isBrakeMode = true; + Logger.recordOutput("brakeMode", true); + } + } + public void disableBrakeMode() { + if (isBrakeMode && !DriverStation.isEnabled()) { + shooter.setPivotBrakeMode(false); + indexer.setIntakeBrakeMode(false); + elevator.setBrakeMode(false); + drivetrain.setBrakeMode(false); + leds.setPattern(Leds.Zone.MAIN, new LedPatternBreathing("#f000c7"), Leds.PatternLevel.COAST_STATE); + isBrakeMode = false; + Logger.recordOutput("brakeMode", false); + } + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index cf17f292..0acdb199 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -133,8 +133,8 @@ public Command commandRunIntake(double percent) { ); } - public void setIntakeNeutralMode(boolean isBrake) { - io.setIntakeNeutralMode(isBrake); + public void setIntakeBrakeMode(boolean isBrake) { + io.setIntakeBrakeMode(isBrake); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java index 56d0842c..3d1b8ef7 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -30,6 +30,6 @@ default void setFeederVelocity(double velocityRPM) {} default void configureFeederPID(double p, double i, double d) {} - default void setIntakeNeutralMode(boolean isBrakeMode) {} + default void setIntakeBrakeMode(boolean isBrakeMode) {} } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java index d00087c5..df790fe4 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -74,7 +74,7 @@ public void configureFeederPID(double p, double i, double d) { feederPID.setD(d); } - public void setIntakeNeutralMode(boolean isBrakeMode) { + public void setIntakeBrakeMode(boolean isBrakeMode) { intakeMotor.setIdleMode(isBrakeMode ? CANSparkBase.IdleMode.kBrake : CANSparkBase.IdleMode.kCoast); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java index cabb1293..fcac060d 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java @@ -116,7 +116,7 @@ public void configureFeederPID(double p, double i, double d) { } @Override - public void setIntakeNeutralMode(boolean isBrakeMode) { + public void setIntakeBrakeMode(boolean isBrakeMode) { intakeMotor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index b85c6502..463cf3da 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -62,19 +62,7 @@ public void clearPattern(Zone zone, PatternLevel priority) { patterns[zone.ordinal()].clearPattern(priority); } - public void setPatternAll(Supplier patternSupplier, PatternLevel priority) { - for (int i = 0; i Date: Tue, 2 Apr 2024 21:17:39 -0700 Subject: [PATCH 70/75] feat: revert to pre wilsonville auto logic and fix limelight offset --- paths.chor | 3442 +++++++++-------- .../deploy/choreo/CenterLanePDEABC.1.traj | 1145 +++--- .../deploy/choreo/CenterLanePDEABC.2.traj | 1355 +++---- .../deploy/choreo/CenterLanePDEABC.3.traj | 107 +- .../deploy/choreo/CenterLanePDEABC.4.traj | 419 +- .../deploy/choreo/CenterLanePDEABC.5.traj | 318 +- src/main/deploy/choreo/CenterLanePDEABC.traj | 3346 ++++++++-------- .../org/team1540/robot2024/Constants.java | 2 +- .../team1540/robot2024/RobotContainer.java | 3 +- .../commands/autos/CenterLanePDEABC.java | 35 +- .../vision/AprilTagVisionIOLimelight.java | 2 +- .../robot2024/util/auto/AutoCommand.java | 23 +- .../util/vision/EstimatedVisionPose.java | 2 +- 13 files changed, 5198 insertions(+), 5001 deletions(-) diff --git a/paths.chor b/paths.chor index 99ff7769..8da6a0fa 100644 --- a/paths.chor +++ b/paths.chor @@ -1,9 +1,9 @@ { - "version": "v0.3", + "version": "v0.3.1", "robotConfiguration": { "mass": 61.73998083590995, "rotationalInertia": 3.6816072891155396, - "motorMaxTorque": 0.5, + "motorMaxTorque": 0.65, "motorMaxVelocity": 4864, "gearing": 6.746, "wheelbase": 0.4762497428251389, @@ -28969,7 +28969,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 11 }, { "x": 2.3368515968322754, @@ -28978,7 +28978,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 17 }, { "x": 5.747424125671387, @@ -28987,7 +28987,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 12 }, { "x": 7.125290870666504, @@ -29050,7 +29050,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 14 }, { "x": 3.405486822128296, @@ -29059,7 +29059,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { "x": 2.205486822128296, @@ -29068,20 +29068,20 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 8 }, { - "x": 2.256009101867676, - "y": 6.624549388885498, + "x": 2.052253484725952, + "y": 6.674348831176758, "heading": 0.3930579718029317, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 8 }, { - "x": 2.8562917709350586, - "y": 6.879989147186279, + "x": 2.8644089698791504, + "y": 7.012746810913086, "heading": 0.3930579718029317, "isInitialGuess": false, "translationConstrained": true, @@ -29095,7 +29095,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 7 + "controlIntervalCount": 8 }, { "x": 2.0785037517547607, @@ -29147,1784 +29147,1856 @@ { "x": 1.3350272178649902, "y": 5.601006507873535, - "heading": 1.3464399983715043e-25, - "angularVelocity": 2.380601151660865e-26, - "velocityX": -3.6373884256645e-26, - "velocityY": 3.3932882505305813e-26, + "heading": 3.967978705267542e-25, + "angularVelocity": 7.379244608128328e-26, + "velocityX": 8.172146600807894e-26, + "velocityY": -1.420872098935511e-25, "timestamp": 0 }, { - "x": 1.3513037651461417, - "y": 5.614995878157814, - "heading": 0.006549885022004973, - "angularVelocity": 0.11118193396003606, - "velocityX": 0.27628851481066846, - "velocityY": 0.23746451088284293, - "timestamp": 0.058911414730016334 - }, - { - "x": 1.3843789066016257, - "y": 5.642351283609904, - "heading": 0.01935493583583066, - "angularVelocity": 0.21736111537143754, - "velocityX": 0.5614385871916894, - "velocityY": 0.4643481331666494, - "timestamp": 0.11782282946003267 - }, - { - "x": 1.4348748758418, - "y": 5.682261570187258, - "heading": 0.038033490581189856, - "angularVelocity": 0.31706172447157616, - "velocityX": 0.8571508505030987, - "velocityY": 0.6774627083776117, - "timestamp": 0.17673424419004902 - }, - { - "x": 1.503535032987812, - "y": 5.733639392412991, - "heading": 0.06208045701046981, - "angularVelocity": 0.408188574989825, - "velocityX": 1.1654813835429523, - "velocityY": 0.8721199866136559, - "timestamp": 0.23564565892006534 - }, - { - "x": 1.5912385139518157, - "y": 5.794977395067809, - "heading": 0.09080789039897333, - "angularVelocity": 0.48763781213127805, - "velocityX": 1.4887349313530773, - "velocityY": 1.041190454106761, - "timestamp": 0.29455707365008166 - }, - { - "x": 1.698981116852805, - "y": 5.864111533799201, - "heading": 0.12324878614145561, - "angularVelocity": 0.550672495154884, - "velocityX": 1.828891792783458, - "velocityY": 1.1735270498633465, - "timestamp": 0.353468488380098 - }, - { - "x": 1.8277399268235341, - "y": 5.937848492694958, - "heading": 0.15800938596992012, - "angularVelocity": 0.5900486346791701, - "velocityX": 2.1856343216474206, - "velocityY": 1.2516582606899718, - "timestamp": 0.4123799031101143 - }, - { - "x": 1.9780206143757966, - "y": 6.011501208422765, - "heading": 0.1930700705634149, - "angularVelocity": 0.5951424652450383, - "velocityX": 2.5509604249190097, - "velocityY": 1.2502282633229538, - "timestamp": 0.4712913178401306 - }, - { - "x": 2.1488551922208914, - "y": 6.0788007358405975, - "heading": 0.22564028873153014, - "angularVelocity": 0.5528676966489513, - "velocityX": 2.8998552933757997, - "velocityY": 1.1423851850487312, - "timestamp": 0.5302027325701469 + "x": 1.3486609048876415, + "y": 5.6125344531964885, + "heading": 0.0053952990645401815, + "angularVelocity": 0.09676162177278211, + "velocityX": 0.24451242670209422, + "velocityY": 0.20674714632375757, + "timestamp": 0.05575866718325111 + }, + { + "x": 1.3762911400128175, + "y": 5.635152571560341, + "heading": 0.015980040657686934, + "angularVelocity": 0.1898313235924372, + "velocityX": 0.4955325606038828, + "velocityY": 0.4056430956198756, + "timestamp": 0.11151733436650221 + }, + { + "x": 1.4183448247504244, + "y": 5.668306854106291, + "heading": 0.0314941432622812, + "angularVelocity": 0.2782366112447971, + "velocityX": 0.7542089304143026, + "velocityY": 0.594603210958904, + "timestamp": 0.1672760015497533 + }, + { + "x": 1.4753265664194932, + "y": 5.711278723492, + "heading": 0.05160335039247586, + "angularVelocity": 0.3606471988310923, + "velocityX": 1.0219351456482668, + "velocityY": 0.7706760501373088, + "timestamp": 0.22303466873300443 + }, + { + "x": 1.547831871808343, + "y": 5.763109379844883, + "heading": 0.07586790196130719, + "angularVelocity": 0.43517093923866185, + "velocityX": 1.3003414366157815, + "velocityY": 0.9295533586292857, + "timestamp": 0.27879333591625555 + }, + { + "x": 1.6365515186230641, + "y": 5.822478611934523, + "heading": 0.10369270639110653, + "angularVelocity": 0.499022050479668, + "velocityX": 1.5911364330704598, + "velocityY": 1.064753429175822, + "timestamp": 0.33455200309950667 + }, + { + "x": 1.7422414603963892, + "y": 5.887510284275809, + "heading": 0.1342490325748326, + "angularVelocity": 0.5480103404068561, + "velocityX": 1.8954890264140394, + "velocityY": 1.1663060762833288, + "timestamp": 0.3903106702827578 + }, + { + "x": 1.8655876183871098, + "y": 5.955479067462068, + "heading": 0.1663576179952195, + "angularVelocity": 0.57584922743691, + "velocityX": 2.212143227623125, + "velocityY": 1.2189814896916369, + "timestamp": 0.4460693374660089 + }, + { + "x": 2.0068104942426706, + "y": 6.022477551407248, + "heading": 0.1983353246164728, + "angularVelocity": 0.5735019905723076, + "velocityX": 2.532752000535291, + "velocityY": 1.201579724368033, + "timestamp": 0.50182800464926 + }, + { + "x": 2.164876110933553, + "y": 6.083435356761985, + "heading": 0.22790661577104523, + "angularVelocity": 0.5303442971006865, + "velocityX": 2.834816983185756, + "velocityY": 1.093243587663212, + "timestamp": 0.5575866718325111 }, { "x": 2.3368515968322754, "y": 6.133185386657715, "heading": 0.2525545797921912, - "angularVelocity": 0.456860375599634, - "velocityX": 3.1911711078905185, - "velocityY": 0.9231598165882628, - "timestamp": 0.5891141473001633 - }, - { - "x": 2.5473350968821267, - "y": 6.194075230145087, - "heading": 0.25255484664471606, - "angularVelocity": 0.0000044758460677296985, - "velocityX": 3.530383481883881, - "velocityY": 1.0212890683184164, - "timestamp": 0.6487347242516318 - }, - { - "x": 2.7606743654688723, - "y": 6.255791206318425, - "heading": 0.25255482885764124, - "angularVelocity": -2.983378514405921e-7, - "velocityX": 3.578282524176262, - "velocityY": 1.0351455710261694, - "timestamp": 0.7083553012031003 - }, - { - "x": 2.9740136340556216, - "y": 6.317507182491763, - "heading": 0.25255481107057365, - "angularVelocity": -2.9833773077503677e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.7679758781545688 - }, - { - "x": 3.187352902642371, - "y": 6.379223158665101, - "heading": 0.25255479328350594, - "angularVelocity": -2.9833773187848623e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.8275964551060373 - }, - { - "x": 3.40069217122912, - "y": 6.44093913483844, - "heading": 0.2525547754964383, - "angularVelocity": -2.983377307846883e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 0.8872170320575058 - }, - { - "x": 3.614031439815869, - "y": 6.502655111011779, - "heading": 0.25255475770937064, - "angularVelocity": -2.9833773102796393e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.9468376090089743 - }, - { - "x": 3.827370708402618, - "y": 6.564371087185117, - "heading": 0.25255473992230293, - "angularVelocity": -2.983377313896792e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.006458185960443 - }, - { - "x": 4.040709976989367, - "y": 6.626087063358456, - "heading": 0.2525547221352353, - "angularVelocity": -2.9833773109308854e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261857, - "timestamp": 1.0660787629119115 - }, - { - "x": 4.254049245576117, - "y": 6.6878030395317944, - "heading": 0.2525547043481677, - "angularVelocity": -2.983377300262124e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.12569933986338 - }, - { - "x": 4.467388514162867, - "y": 6.749519015705133, - "heading": 0.2525546865611001, - "angularVelocity": -2.9833773003639016e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261857, - "timestamp": 1.1853199168148487 - }, - { - "x": 4.680727782749616, - "y": 6.811234991878472, - "heading": 0.2525546687740325, - "angularVelocity": -2.9833772985092435e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.2449404937663173 - }, - { - "x": 4.894067051336365, - "y": 6.872950968051811, - "heading": 0.25255465098696483, - "angularVelocity": -2.9833773088728923e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.304561070717786 - }, - { - "x": 5.107406319923115, - "y": 6.934666944225149, - "heading": 0.2525546331998971, - "angularVelocity": -2.9833773138995595e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.3641816476692545 - }, - { - "x": 5.3207455885098645, - "y": 6.996382920398487, - "heading": 0.2525546154128296, - "angularVelocity": -2.9833772971803955e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.4238022246207231 - }, - { - "x": 5.534084857096614, - "y": 7.058098896571825, - "heading": 0.252554597625762, - "angularVelocity": -2.983377299147757e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.4834228015721918 + "angularVelocity": 0.44204722362068527, + "velocityX": 3.0842825803838596, + "velocityY": 0.8922385058492518, + "timestamp": 0.6133453390157622 + }, + { + "x": 2.5269005159766844, + "y": 6.188163800352406, + "heading": 0.252554892201178, + "angularVelocity": 0.000005553817937610827, + "velocityX": 3.378574691383036, + "velocityY": 0.9773729727982717, + "timestamp": 0.6695965504005877 + }, + { + "x": 2.7281832415838734, + "y": 6.246391992377104, + "heading": 0.2525548726806658, + "angularVelocity": -3.47023853848989e-7, + "velocityX": 3.578282505423293, + "velocityY": 1.0351455656012056, + "timestamp": 0.7258477617854133 + }, + { + "x": 2.929465967191096, + "y": 6.304620184401812, + "heading": 0.2525548531602303, + "angularVelocity": -3.4702249119339613e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 0.7820989731702389 + }, + { + "x": 3.1307486927983192, + "y": 6.36284837642652, + "heading": 0.25255483363979486, + "angularVelocity": -3.4702249016578237e-7, + "velocityX": 3.5782825054238923, + "velocityY": 1.0351455656013788, + "timestamp": 0.8383501845550645 + }, + { + "x": 3.332031418405542, + "y": 6.421076568451228, + "heading": 0.2525548141193594, + "angularVelocity": -3.470224908197631e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 0.8946013959398901 + }, + { + "x": 3.5333141440127647, + "y": 6.4793047604759355, + "heading": 0.2525547945989239, + "angularVelocity": -3.4702249080835237e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 0.9508526073247157 + }, + { + "x": 3.7345968696199874, + "y": 6.537532952500643, + "heading": 0.2525547750784885, + "angularVelocity": -3.470224894869701e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 1.0071038187095414 + }, + { + "x": 3.93587959522721, + "y": 6.595761144525352, + "heading": 0.25255475555805307, + "angularVelocity": -3.470224898418443e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 1.063355030094367 + }, + { + "x": 4.137162320834433, + "y": 6.65398933655006, + "heading": 0.2525547360376176, + "angularVelocity": -3.470224909887922e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.1196062414791927 + }, + { + "x": 4.338445046441655, + "y": 6.712217528574768, + "heading": 0.2525547165171822, + "angularVelocity": -3.4702248928646837e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 1.1758574528640184 + }, + { + "x": 4.539727772048877, + "y": 6.770445720599476, + "heading": 0.25255469699674676, + "angularVelocity": -3.470224898754089e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 1.2321086642488441 + }, + { + "x": 4.7410104976561, + "y": 6.8286739126241836, + "heading": 0.25255467747631133, + "angularVelocity": -3.470224907627516e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.2883598756336698 + }, + { + "x": 4.942293223263322, + "y": 6.886902104648891, + "heading": 0.25255465795587584, + "angularVelocity": -3.4702249046766804e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 1.3446110870184955 + }, + { + "x": 5.143575948870544, + "y": 6.945130296673599, + "heading": 0.2525546384354404, + "angularVelocity": -3.47022489425383e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.4008622984033212 + }, + { + "x": 5.344858674477766, + "y": 7.003358488698307, + "heading": 0.25255461891500497, + "angularVelocity": -3.470224896167856e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.457113509788147 + }, + { + "x": 5.546141400084989, + "y": 7.061586680723015, + "heading": 0.25255459939456953, + "angularVelocity": -3.470224897168836e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 1.5133647211729726 }, { "x": 5.747424125671387, "y": 7.119814872741699, "heading": 0.2525545797921912, - "angularVelocity": -2.9911771592583556e-7, - "velocityX": 3.5782825239754317, - "velocityY": 1.035145570968072, - "timestamp": 1.5430433785236604 - }, - { - "x": 5.874238789453484, - "y": 7.151633771807316, - "heading": 0.23428972710167786, - "angularVelocity": -0.5023194013498574, - "velocityX": 3.4876528747747586, - "velocityY": 0.8750823563200211, - "timestamp": 1.579404412243304 - }, - { - "x": 6.000467771954814, - "y": 7.182688874971163, - "heading": 0.21343765731660022, - "angularVelocity": -0.5734729641036735, - "velocityX": 3.471545486704275, - "velocityY": 0.8540764655727967, - "timestamp": 1.6157654459629476 - }, - { - "x": 6.126526412325093, - "y": 7.213769689844044, - "heading": 0.19236113767335114, - "angularVelocity": -0.5796457770083356, - "velocityX": 3.4668607428005407, - "velocityY": 0.8547835881819195, - "timestamp": 1.6521264796825912 - }, - { - "x": 6.252399106037461, - "y": 7.244861395063954, - "heading": 0.17099373366442527, - "angularVelocity": -0.5876456696384406, - "velocityX": 3.4617468436923633, - "velocityY": 0.8550830941616943, - "timestamp": 1.6884875134022348 - }, - { - "x": 6.378079158099059, - "y": 7.275966592245878, - "heading": 0.1493195497494227, - "angularVelocity": -0.596082720918173, - "velocityX": 3.456448819102211, - "velocityY": 0.8554541496744962, - "timestamp": 1.7248485471218784 - }, - { - "x": 6.503558906617952, - "y": 7.3070873984764315, - "heading": 0.12731920059935156, - "angularVelocity": -0.6050529069030769, - "velocityX": 3.4509400774022723, - "velocityY": 0.8558834292365232, - "timestamp": 1.761209580841522 - }, - { - "x": 6.628826049826062, - "y": 7.3382208951944605, - "heading": 0.10495362803069447, - "angularVelocity": -0.6150972698164627, - "velocityX": 3.44509301286545, - "velocityY": 0.8562324426219444, - "timestamp": 1.7975706145611656 - }, - { - "x": 6.7538656809780315, - "y": 7.369363297624379, - "heading": 0.08217584529224368, - "angularVelocity": -0.6264338608763309, - "velocityX": 3.438835983489081, - "velocityY": 0.8564773672288275, - "timestamp": 1.8339316482808092 - }, - { - "x": 6.8786958169095165, - "y": 7.400552514729427, - "heading": 0.05908376270904082, - "angularVelocity": -0.6350777252718116, - "velocityX": 3.4330744525573853, - "velocityY": 0.8577648629444327, - "timestamp": 1.8702926820004528 - }, - { - "x": 7.003224052672837, - "y": 7.431696922152255, - "heading": 0.03530389927777457, - "angularVelocity": -0.6539930524147748, - "velocityX": 3.4247716036754445, - "velocityY": 0.8565325084804077, - "timestamp": 1.9066537157200965 + "angularVelocity": -3.4847922161224964e-7, + "velocityX": 3.5782825050536915, + "velocityY": 1.0351455654942852, + "timestamp": 1.5696159325577983 + }, + { + "x": 5.864122316952898, + "y": 7.149764367678907, + "heading": 0.23825610881476394, + "angularVelocity": -0.4289921260648423, + "velocityX": 3.501255852098466, + "velocityY": 0.8985644358731831, + "timestamp": 1.6029463134871351 + }, + { + "x": 5.980010019619152, + "y": 7.178464848632163, + "heading": 0.21993450293637717, + "angularVelocity": -0.5496968641681629, + "velocityX": 3.4769390398491176, + "velocityY": 0.8610906972261488, + "timestamp": 1.636276694416472 + }, + { + "x": 6.095759168980239, + "y": 7.207171809244793, + "heading": 0.2014022591938407, + "angularVelocity": -0.5560165598414963, + "velocityX": 3.4727820725027265, + "velocityY": 0.8612851042264353, + "timestamp": 1.6696070753458088 + }, + { + "x": 6.211373074459108, + "y": 7.235898623678772, + "heading": 0.18268720108591044, + "angularVelocity": -0.5615014766140173, + "velocityX": 3.4687243966392267, + "velocityY": 0.8618807716264242, + "timestamp": 1.7029374562751456 + }, + { + "x": 6.326838442963843, + "y": 7.264631799858999, + "heading": 0.16373191419922642, + "angularVelocity": -0.5687089783603383, + "velocityX": 3.464267892693179, + "velocityY": 0.8620716409195424, + "timestamp": 1.7362678372044824 + }, + { + "x": 6.442151068511899, + "y": 7.29337427879422, + "heading": 0.1445283357059787, + "angularVelocity": -0.5761583863671085, + "velocityX": 3.4596851980938594, + "velocityY": 0.8623507482904867, + "timestamp": 1.7695982181338192 + }, + { + "x": 6.557303014616615, + "y": 7.322124225476267, + "heading": 0.1250513432276187, + "angularVelocity": -0.5843615324905178, + "velocityX": 3.454864387804252, + "velocityY": 0.8625748005400329, + "timestamp": 1.802928599063156 + }, + { + "x": 6.672285593632433, + "y": 7.350880202549992, + "heading": 0.10527449948643015, + "angularVelocity": -0.5933578671998123, + "velocityX": 3.449782925061388, + "velocityY": 0.8627557283156009, + "timestamp": 1.8362589799924929 + }, + { + "x": 6.787113803468559, + "y": 7.379671810584065, + "heading": 0.08527801395894485, + "angularVelocity": -0.5999477044645701, + "velocityX": 3.4451514394501603, + "velocityY": 0.8638247518116766, + "timestamp": 1.8695893609218297 + }, + { + "x": 6.901739724666763, + "y": 7.408448228453197, + "heading": 0.0648615716893241, + "angularVelocity": -0.6125475227212472, + "velocityX": 3.4390822427508825, + "velocityY": 0.8633690064971079, + "timestamp": 1.9029197418511665 + }, + { + "x": 7.014861438150348, + "y": 7.4357314738869205, + "heading": 0.038575820671149744, + "angularVelocity": -0.7886423822728691, + "velocityX": 3.3939520140322568, + "velocityY": 0.8185698654799702, + "timestamp": 1.9362501227805033 }, { "x": 7.125290870666504, "y": 7.458, - "heading": -8.902558475497447e-21, - "angularVelocity": -0.9709267219953129, - "velocityX": 3.3570777699788383, - "velocityY": 0.7233864155389959, - "timestamp": 1.94301474943974 - }, - { - "x": 7.338537269448379, - "y": 7.482023887493991, - "heading": -0.008039547554639858, - "angularVelocity": -0.1278866817794853, - "velocityX": 3.39215287381437, - "velocityY": 0.38215275600640763, - "timestamp": 2.0058793686696177 - }, - { - "x": 7.527981936001141, - "y": 7.500234795903286, - "heading": -0.013861641566031323, - "angularVelocity": -0.09261320728121766, - "velocityX": 3.013533985786477, - "velocityY": 0.2896845416768293, - "timestamp": 2.068743987899495 - }, - { - "x": 7.693596991003447, - "y": 7.512743640283509, - "heading": -0.017657163586421762, - "angularVelocity": -0.060376123595234456, - "velocityX": 2.6344716158496166, - "velocityY": 0.1989806751947781, - "timestamp": 2.1316086071293725 - }, - { - "x": 7.835373269936495, - "y": 7.519587451824941, - "heading": -0.01948976746028864, - "angularVelocity": -0.02915159427221823, - "velocityX": 2.255263464089624, - "velocityY": 0.10886587122090353, - "timestamp": 2.19447322635925 - }, - { - "x": 7.953306214016821, - "y": 7.520784758027623, - "heading": -0.01939144141260055, - "angularVelocity": 0.0015640919947123182, - "velocityX": 1.875982794854424, - "velocityY": 0.01904578787480972, - "timestamp": 2.2573378455891273 - }, - { - "x": 8.04739309462225, - "y": 7.516346680416715, - "heading": -0.01738162754383806, - "angularVelocity": 0.031970508902840686, - "velocityX": 1.4966587208836901, - "velocityY": -0.07059738315886452, - "timestamp": 2.3202024648190047 - }, - { - "x": 8.117632094556283, - "y": 7.5062806367543455, - "heading": -0.0134735913597302, - "angularVelocity": 0.06216590877322135, - "velocityX": 1.1173057404068745, - "velocityY": -0.16012255837517314, - "timestamp": 2.383067084048882 - }, - { - "x": 8.164021915899584, - "y": 7.490591928437994, - "heading": -0.007677144931297963, - "angularVelocity": 0.09220522607854079, - "velocityX": 0.7379321136689944, - "velocityY": -0.24956340320748793, - "timestamp": 2.4459317032787595 + "heading": 1.0084355524659913e-21, + "angularVelocity": -1.1573771314805474, + "velocityX": 3.313176430544709, + "velocityY": 0.668114959750724, + "timestamp": 1.9695805037098402 + }, + { + "x": 7.337738270409439, + "y": 7.481917447620179, + "heading": -0.00819620478546554, + "angularVelocity": -0.12615227280600286, + "velocityX": 3.2698941803922152, + "velocityY": 0.368126523919295, + "timestamp": 2.0345512299849795 + }, + { + "x": 7.526586631074925, + "y": 7.500039280530163, + "heading": -0.014041404226933134, + "angularVelocity": -0.08996666308937673, + "velocityX": 2.906668456586843, + "velocityY": 0.27892304656162725, + "timestamp": 2.099521956260119 + }, + { + "x": 7.6918053440422804, + "y": 7.512484823370471, + "heading": -0.017813194168849914, + "angularVelocity": -0.058053682914731695, + "velocityX": 2.5429716187515523, + "velocityY": 0.19155616004049963, + "timestamp": 2.164492682535258 + }, + { + "x": 7.833384352673449, + "y": 7.519294018241946, + "heading": -0.01960436856302971, + "angularVelocity": -0.027568945229186584, + "velocityX": 2.179119993696969, + "velocityY": 0.10480404424970338, + "timestamp": 2.2294634088103975 + }, + { + "x": 7.951318655599417, + "y": 7.520486868366768, + "heading": -0.019461534286698053, + "angularVelocity": 0.0021984405057560424, + "velocityX": 1.815191389835117, + "velocityY": 0.018359808997220107, + "timestamp": 2.294434135085537 + }, + { + "x": 8.045605259786509, + "y": 7.516075387181805, + "heading": -0.017412919986794666, + "angularVelocity": 0.031531343688969565, + "velocityX": 1.4512167185542217, + "velocityY": -0.06789952087470152, + "timestamp": 2.359404861360676 + }, + { + "x": 8.116242172260733, + "y": 7.5060675897274995, + "heading": -0.013477652900012242, + "angularVelocity": 0.06056984910584597, + "velocityX": 1.0872113723200465, + "velocityY": -0.15403548687333402, + "timestamp": 2.4243755876358155 + }, + { + "x": 8.163227969814436, + "y": 7.490469205298236, + "heading": -0.007669730702440132, + "angularVelocity": 0.08939290863052056, + "velocityX": 0.723184120718108, + "velocityY": -0.24008327016705255, + "timestamp": 2.489346313910955 }, { "x": 8.186561584472656, "y": 7.469284534454346, - "heading": 2.2979832887228624e-21, - "angularVelocity": 0.12212187117884055, - "velocityX": 0.358542990464167, - "velocityY": -0.33894095350730474, - "timestamp": 2.508796322508637 - }, - { - "x": 8.169433855540824, - "y": 7.426853893988931, - "heading": 0.014976899045789983, - "angularVelocity": 0.16512821061392627, - "velocityX": -0.18884224442902273, - "velocityY": -0.46782018853404506, - "timestamp": 2.599494931729361 - }, - { - "x": 8.102655131439361, - "y": 7.372743106744783, - "heading": 0.03346799984598468, - "angularVelocity": 0.20387413830342968, - "velocityX": -0.7362706515041357, - "velocityY": -0.5965999667366941, - "timestamp": 2.6901935409500854 - }, - { - "x": 7.986220220035368, - "y": 7.306964887600374, - "heading": 0.05493172658414638, - "angularVelocity": 0.23664890699621968, - "velocityX": -1.2837563045827718, - "velocityY": -0.7252395566985038, - "timestamp": 2.7808921501708097 - }, - { - "x": 7.820121969665864, - "y": 7.229538517591083, - "heading": 0.07855370015299508, - "angularVelocity": 0.26044471653762835, - "velocityX": -1.8313208085174242, - "velocityY": -0.8536665630766948, - "timestamp": 2.871590759391534 - }, - { - "x": 7.6043501738014525, - "y": 7.14049675145262, - "heading": 0.10296815130417637, - "angularVelocity": 0.2691821998258671, - "velocityX": -2.3789978448215092, - "velocityY": -0.9817324312192064, - "timestamp": 2.9622893686122582 - }, - { - "x": 7.338891049230189, - "y": 7.039907645727491, - "heading": 0.1254011174832251, - "angularVelocity": 0.24733528299707236, - "velocityX": -2.9268268483063693, - "velocityY": -1.1090479400884177, - "timestamp": 3.0529879778329825 - }, - { - "x": 7.023762471374015, - "y": 6.927998022527093, - "heading": 0.1370927000479183, - "angularVelocity": 0.12890586377394736, - "velocityX": -3.4744587658370385, - "velocityY": -1.2338626155562706, - "timestamp": 3.1436865870537067 - }, - { - "x": 6.694988070852044, - "y": 6.85020567874946, - "heading": 0.13709270182779565, - "angularVelocity": 1.9624086279332165e-8, - "velocityX": -3.624911157368082, - "velocityY": -0.8577016168827462, - "timestamp": 3.234385196274431 - }, - { - "x": 6.366213601226777, - "y": 6.7724136270242035, - "heading": 0.13709270360766948, - "angularVelocity": 1.962404782489554e-8, - "velocityX": -3.624911919268354, - "velocityY": -0.8576983968512897, - "timestamp": 3.3250838054951553 - }, - { - "x": 6.0374391323544945, - "y": 6.69462157211659, - "heading": 0.13709270538754328, - "angularVelocity": 1.962404757872295e-8, - "velocityX": -3.6249119109663046, - "velocityY": -0.857698431938445, - "timestamp": 3.4157824147158795 - }, - { - "x": 5.708664663238629, - "y": 6.616829518238441, - "heading": 0.13709270716741703, - "angularVelocity": 1.9624046615308717e-8, - "velocityX": -3.6249119136519434, - "velocityY": -0.8576984205880644, - "timestamp": 3.5064810239366038 - }, - { - "x": 5.3798901940960535, - "y": 6.539037464473215, - "heading": 0.13709270894730027, - "angularVelocity": 1.9624151634826213e-8, - "velocityX": -3.6249119139464265, - "velocityY": -0.8576984193430164, - "timestamp": 3.597179633157328 - }, - { - "x": 5.082161936855116, - "y": 6.468591654596992, - "heading": 0.17334127091967155, - "angularVelocity": 0.3996595127953577, - "velocityX": -3.2826110543369467, - "velocityY": -0.7767022061472455, - "timestamp": 3.6878782423780523 - }, - { - "x": 4.834055003719788, - "y": 6.409886994261807, - "heading": 0.20354833113507975, - "angularVelocity": 0.33304876971041714, - "velocityX": -2.735509786390822, - "velocityY": -0.647249840318068, - "timestamp": 3.7785768515987765 - }, - { - "x": 4.635569429511693, - "y": 6.362923375586849, - "heading": 0.22771434771261193, - "angularVelocity": 0.2664430776300183, - "velocityX": -2.1884081345179167, - "velocityY": -0.5177986639317362, - "timestamp": 3.869275460819501 - }, - { - "x": 4.486705235532264, - "y": 6.327700716475024, - "heading": 0.24583917728696456, - "angularVelocity": 0.19983580487153885, - "velocityX": -1.6413062477854834, - "velocityY": -0.38834839270916643, - "timestamp": 3.959974070040225 - }, - { - "x": 4.387462434805768, - "y": 6.304218961949016, - "heading": 0.25792250356094953, - "angularVelocity": 0.13322504476974903, - "velocityX": -1.0942042174536384, - "velocityY": -0.25889872764050514, - "timestamp": 4.050672679260949 + "heading": 7.422813602625121e-23, + "angularVelocity": 0.11804902210820577, + "velocityX": 0.3591404313291331, + "velocityY": -0.3260648611834275, + "timestamp": 2.554317040186094 + }, + { + "x": 8.171272340508109, + "y": 7.427332842662171, + "heading": 0.014840203267270782, + "angularVelocity": 0.15900903302410063, + "velocityX": -0.16382039077820704, + "velocityY": -0.44950179087577585, + "timestamp": 2.6476463497544476 + }, + { + "x": 8.107175512886487, + "y": 7.3738609015025816, + "heading": 0.03350091514476021, + "angularVelocity": 0.1999448186619506, + "velocityX": -0.6867813328746101, + "velocityY": -0.572938355666582, + "timestamp": 2.7409756593228005 + }, + { + "x": 7.994271087246123, + "y": 7.308868753063173, + "heading": 0.05597944584590409, + "angularVelocity": 0.24085178391554368, + "velocityX": -1.2097424288526986, + "velocityY": -0.6963744694994183, + "timestamp": 2.8343049688911535 + }, + { + "x": 7.832559046421551, + "y": 7.232356448582914, + "heading": 0.08227288434163177, + "angularVelocity": 0.2817275582272545, + "velocityX": -1.732703708754388, + "velocityY": -0.8198100343196232, + "timestamp": 2.9276342784595064 + }, + { + "x": 7.6220393708156715, + "y": 7.144324050552095, + "heading": 0.1123783464235267, + "angularVelocity": 0.3225724289736236, + "velocityX": -2.2556651986340626, + "velocityY": -0.9432449295721523, + "timestamp": 3.0209635880278594 + }, + { + "x": 7.362712038546165, + "y": 7.04477163823656, + "heading": 0.14629325961596634, + "angularVelocity": 0.36338973629287186, + "velocityX": -2.778626922977279, + "velocityY": -1.0666789755111628, + "timestamp": 3.1142928975962123 + }, + { + "x": 7.054577023664908, + "y": 6.933699333854541, + "heading": 0.18401565672002676, + "angularVelocity": 0.4041859655720804, + "velocityX": -3.3015889253480717, + "velocityY": -1.1901117119126619, + "timestamp": 3.2076222071645653 + }, + { + "x": 6.71622230121249, + "y": 6.85383794706396, + "heading": 0.18401565782506035, + "angularVelocity": 1.1840156036202986e-8, + "velocityX": -3.6253854659088836, + "velocityY": -0.8556946061203927, + "timestamp": 3.300951516732918 + }, + { + "x": 6.377867419030211, + "y": 6.7739772370157985, + "heading": 0.1840156589300666, + "angularVelocity": 1.1839863049890439e-8, + "velocityX": -3.6253871773740243, + "velocityY": -0.8556873549961612, + "timestamp": 3.394280826301271 + }, + { + "x": 6.0395125368493074, + "y": 6.694116526961809, + "heading": 0.18401566003507294, + "angularVelocity": 1.1839864253851599e-8, + "velocityX": -3.6253871773592854, + "velocityY": -0.8556873550586043, + "timestamp": 3.487610135869624 + }, + { + "x": 5.701157654668755, + "y": 6.614255816906336, + "heading": 0.18401566114007933, + "angularVelocity": 1.18398644204111e-8, + "velocityX": -3.625387177355538, + "velocityY": -0.855687355074483, + "timestamp": 3.580939445437977 + }, + { + "x": 5.362802772497576, + "y": 6.534395106811441, + "heading": 0.18401566224512567, + "angularVelocity": 1.1840292694239514e-8, + "velocityX": -3.6253871772550927, + "velocityY": -0.8556873554969, + "timestamp": 3.67426875500633 + }, + { + "x": 5.069956587719004, + "y": 6.4652758877562615, + "heading": 0.20685757815363412, + "angularVelocity": 0.24474536471074324, + "velocityX": -3.137772968995303, + "velocityY": -0.7405949896646158, + "timestamp": 3.767598064574683 + }, + { + "x": 4.8259180814209035, + "y": 6.407676584765678, + "heading": 0.22589278292875978, + "angularVelocity": 0.20395741555534105, + "velocityX": -2.614811010890098, + "velocityY": -0.6171619961293916, + "timestamp": 3.860927374143036 + }, + { + "x": 4.6306872671740225, + "y": 6.361597170610555, + "heading": 0.2411211921270372, + "angularVelocity": 0.16316856160951593, + "velocityX": -2.0918489073777633, + "velocityY": -0.49372929434751617, + "timestamp": 3.954256683711389 + }, + { + "x": 4.484264152022265, + "y": 6.3270376241371595, + "heading": 0.25254264332581844, + "angularVelocity": 0.12237796734600637, + "velocityX": -1.5688867283917882, + "velocityY": -0.37029681922252144, + "timestamp": 4.047585993279742 + }, + { + "x": 4.386648740184571, + "y": 6.303997931141046, + "heading": 0.26015698140700544, + "angularVelocity": 0.08158571103122086, + "velocityX": -1.0459245042009158, + "velocityY": -0.24686449629458126, + "timestamp": 4.140915302848095 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": 0.06661212070370816, - "velocityX": -0.54710211431646, - "velocityY": -0.12944936516318697, - "timestamp": 4.141371288481674 + "angularVelocity": 0.04079263927507296, + "velocityX": -0.5229622556382164, + "velocityY": -0.12343224899141036, + "timestamp": 4.234244612416448 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": 4.2322185448004084e-23, - "velocityX": 1.750098119560174e-21, - "velocityY": 1.2842584640589406e-21, - "timestamp": 4.232069897702398 - }, - { - "x": 4.357336303204376, - "y": 6.297583237177048, - "heading": 0.24752122945545674, - "angularVelocity": -0.28561960466886005, - "velocityX": 0.3386404361144792, - "velocityY": 0.08867849340285801, - "timestamp": 4.289639128041561 - }, - { - "x": 4.396392552566799, - "y": 6.3076902844788645, - "heading": 0.21545182139905172, - "angularVelocity": -0.5570581344838518, - "velocityX": 0.6784222949017583, - "velocityY": 0.17556335636714013, - "timestamp": 4.3472083583807235 - }, - { - "x": 4.455088009056865, - "y": 6.322669909558741, - "heading": 0.16876627146696008, - "angularVelocity": -0.8109462234782202, - "velocityX": 1.0195629878021513, - "velocityY": 0.26020193411697123, - "timestamp": 4.404777588719886 - }, - { - "x": 4.533516711715102, - "y": 6.342355579726992, - "heading": 0.10873908045256343, - "angularVelocity": -1.042695736259688, - "velocityX": 1.3623371755394689, - "velocityY": 0.3419477740500502, - "timestamp": 4.462346819059049 - }, - { - "x": 4.631792826161861, - "y": 6.366523725829109, - "heading": 0.03701265289139425, - "angularVelocity": -1.2459160412359371, - "velocityX": 1.7070944646606674, - "velocityY": 0.4198101305112548, - "timestamp": 4.519916049398212 - }, - { - "x": 4.750056312234907, - "y": 6.394856813133717, - "heading": -0.04421826686802747, - "angularVelocity": -1.4110127802796573, - "velocityX": 2.054282910789491, - "velocityY": 0.492156784756161, - "timestamp": 4.577485279737375 - }, - { - "x": 4.888478272202705, - "y": 6.42687003155547, - "heading": -0.13181310680866934, - "angularVelocity": -1.5215565576365515, - "velocityX": 2.4044434701020125, - "velocityY": 0.5560820985993948, - "timestamp": 4.635054510076538 - }, - { - "x": 5.047249483224003, - "y": 6.461748830996926, - "heading": -0.22073093757153267, - "angularVelocity": -1.5445374245758388, - "velocityX": 2.757917903121444, - "velocityY": 0.6058583593348625, - "timestamp": 4.6926237404157005 - }, - { - "x": 5.226429045018527, - "y": 6.497908973529479, - "heading": -0.30132047165929504, - "angularVelocity": -1.399871660833024, - "velocityX": 3.1124189213388544, - "velocityY": 0.6281157889295987, - "timestamp": 4.750192970754863 - }, - { - "x": 5.423903541491299, - "y": 6.531166091125145, - "heading": -0.3435649109260108, - "angularVelocity": -0.7338023978753491, - "velocityX": 3.430209077129816, - "velocityY": 0.5776891127384427, - "timestamp": 4.807762201094026 - }, - { - "x": 5.636616267297259, - "y": 6.55438559892435, - "heading": -0.3449872384479477, - "angularVelocity": -0.024706384184006707, - "velocityX": 3.6949030680588684, - "velocityY": 0.40333191294048215, - "timestamp": 4.865331431433189 + "angularVelocity": -3.1350638097986163e-24, + "velocityX": 6.734301517773745e-23, + "velocityY": -5.476052050130636e-23, + "timestamp": 4.327573921984801 + }, + { + "x": 4.357085465154312, + "y": 6.298335605731047, + "heading": 0.24955697801054222, + "angularVelocity": -0.24189458757929325, + "velocityX": 0.32311199814604374, + "velocityY": 0.09834717102755294, + "timestamp": 4.387133552481924 + }, + { + "x": 4.395672489468794, + "y": 6.30982325311387, + "heading": 0.2214506254853789, + "angularVelocity": -0.4719027349661102, + "velocityX": 0.6478721239942183, + "velocityY": 0.19287640448638915, + "timestamp": 4.446693182979047 + }, + { + "x": 4.453716686931878, + "y": 6.3266599763174565, + "heading": 0.18050722178606005, + "angularVelocity": -0.6874354887291741, + "velocityX": 0.9745560370104606, + "velocityY": 0.28268683104740583, + "timestamp": 4.50625281347617 + }, + { + "x": 4.531352614071991, + "y": 6.348490007100106, + "heading": 0.12779504929109745, + "angularVelocity": -0.8850318924914878, + "velocityX": 1.3034991401409508, + "velocityY": 0.366523945841208, + "timestamp": 4.565812443973293 + }, + { + "x": 4.628738799037524, + "y": 6.374849024990675, + "heading": 0.0646674309135714, + "angularVelocity": -1.0599061453306917, + "velocityX": 1.6351039143911004, + "velocityY": 0.4425651682281852, + "timestamp": 4.625372074470416 + }, + { + "x": 4.746060953896417, + "y": 6.4051070852453105, + "heading": -0.0071047467810986895, + "angularVelocity": -1.2050473969635673, + "velocityX": 1.9698267749421239, + "velocityY": 0.5080296838996932, + "timestamp": 4.684931704967539 + }, + { + "x": 4.883529200739737, + "y": 6.4383664128247835, + "heading": -0.08508741675594196, + "angularVelocity": -1.30932091626408, + "velocityX": 2.3080775635429776, + "velocityY": 0.5584206500589359, + "timestamp": 4.744491335464662 + }, + { + "x": 5.041348217191184, + "y": 6.473264113474332, + "heading": -0.1656919745439482, + "angularVelocity": -1.3533421398895296, + "velocityX": 2.6497648681529427, + "velocityY": 0.5859287634639322, + "timestamp": 4.804050965961785 + }, + { + "x": 5.219569793721533, + "y": 6.507557049328585, + "heading": -0.2431024824486723, + "angularVelocity": -1.29971437462936, + "velocityX": 2.99232172937938, + "velocityY": 0.575774825465208, + "timestamp": 4.8636105964589085 + }, + { + "x": 5.417370624809519, + "y": 6.5372140981281, + "heading": -0.30621328049041796, + "angularVelocity": -1.0596237336427745, + "velocityX": 3.3210553765530904, + "velocityY": 0.49793876409201393, + "timestamp": 4.923170226956032 + }, + { + "x": 5.630089765221455, + "y": 6.556096742386712, + "heading": -0.3316159113808185, + "angularVelocity": -0.4265075299892522, + "velocityX": 3.5715322381358874, + "velocityY": 0.31703763272213936, + "timestamp": 4.982729857453155 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -0.3449872632321153, - "angularVelocity": -4.3051066415640403e-7, - "velocityX": 3.7246964993017384, - "velocityY": 0.047619187556732175, - "timestamp": 4.922900661772352 - }, - { - "x": 5.9707478293428995, - "y": 6.552244535149851, - "heading": -0.34498728566747394, - "angularVelocity": -6.975751659806049e-7, - "velocityX": 3.7219059383782973, - "velocityY": -0.151808826449267, - "timestamp": 4.9550625845387355 - }, - { - "x": 6.090018567999844, - "y": 6.540962083011693, - "heading": -0.3449873062012292, - "angularVelocity": -6.384492454517859e-7, - "velocityX": 3.7084455280643134, - "velocityY": -0.3508015431822894, - "timestamp": 4.987224507305119 - }, - { - "x": 6.208514463680753, - "y": 6.523311991997161, - "heading": -0.3449873252853557, - "angularVelocity": -5.933764159352289e-7, - "velocityX": 3.6843535923407127, - "velocityY": -0.5487884273193968, - "timestamp": 5.019386430071503 - }, - { - "x": 6.325895808833114, - "y": 6.499344869625537, - "heading": -0.34498734327239833, - "angularVelocity": -5.592651525230829e-7, - "velocityX": 3.6496992423305046, - "velocityY": -0.7452017886404377, - "timestamp": 5.051548352837886 - }, - { - "x": 6.441826094112058, - "y": 6.46912943973789, - "heading": -0.34498736044751604, - "angularVelocity": -5.340202407325154e-7, - "velocityX": 3.6045819188434196, - "velocityY": -0.9394783423592116, - "timestamp": 5.08371027560427 - }, - { - "x": 6.555972977983021, - "y": 6.432752355963861, - "heading": -0.3449873770504224, - "angularVelocity": -5.162286594877017e-7, - "velocityX": 3.5491312102233143, - "velocityY": -1.131060603505082, - "timestamp": 5.115872198370654 - }, - { - "x": 6.66800926163723, - "y": 6.390318000635929, - "heading": -0.3449873932911425, - "angularVelocity": -5.049673254841841e-7, - "velocityX": 3.4835070175379053, - "velocityY": -1.319397339399292, - "timestamp": 5.148034121137037 - }, - { - "x": 6.777614079975414, - "y": 6.34194872129089, - "heading": -0.344987409362088, - "angularVelocity": -4.996885790126847e-7, - "velocityX": 3.407906272716571, - "velocityY": -1.5039299639011887, - "timestamp": 5.180196043903421 - }, - { - "x": 6.886436638977974, - "y": 6.291844289930693, - "heading": -0.3449874254080791, - "angularVelocity": -4.98912679463648e-7, - "velocityX": 3.3835837425834847, - "velocityY": -1.557880470149199, - "timestamp": 5.2123579666698046 - }, - { - "x": 6.995259181110964, - "y": 6.241739821931245, - "heading": -0.34498744145406995, - "angularVelocity": -4.989126736688575e-7, - "velocityX": 3.383583218063541, - "velocityY": -1.5578816093613586, - "timestamp": 5.244519889436188 - }, - { - "x": 7.104082434838663, - "y": 6.191636899482441, - "heading": -0.34498745750007104, - "angularVelocity": -4.98912992332448e-7, - "velocityX": 3.3836053434418156, - "velocityY": -1.5578335540676167, - "timestamp": 5.276681812202572 + "heading": -0.3344741491569628, + "angularVelocity": -0.04798951491618805, + "velocityX": 3.7098016045994533, + "velocityY": 0.017297899702458933, + "timestamp": 5.042289487950278 + }, + { + "x": 5.971113356144086, + "y": 6.5517128681904815, + "heading": -0.33447429817496127, + "angularVelocity": -0.000004618408354033321, + "velocityX": 3.7212182481092846, + "velocityY": -0.167796284707936, + "timestamp": 5.074555582043538 + }, + { + "x": 6.090763237120761, + "y": 6.540318338833946, + "heading": -0.33447432111787584, + "angularVelocity": -7.11053358628561e-7, + "velocityX": 3.70822327086892, + "velocityY": -0.3531425069176937, + "timestamp": 5.106821676136798 + }, + { + "x": 6.209696100342635, + "y": 6.522971764048271, + "heading": -0.3344743416762489, + "angularVelocity": -6.371509664508709e-7, + "velocityX": 3.686001251905956, + "velocityY": -0.537609998146539, + "timestamp": 5.139087770230058 + }, + { + "x": 6.327616045254045, + "y": 6.499716303685921, + "heading": -0.3344743603957426, + "angularVelocity": -5.80159891848438e-7, + "velocityX": 3.6546085984432737, + "velocityY": -0.7207398669059114, + "timestamp": 5.171353864323319 + }, + { + "x": 6.444229692072682, + "y": 6.470609819762047, + "heading": -0.3344743776866033, + "angularVelocity": -5.358832938629608e-7, + "velocityX": 3.6141234350083242, + "velocityY": -0.9020764595722645, + "timestamp": 5.203619958416579 + }, + { + "x": 6.559246912194348, + "y": 6.435724733351891, + "heading": -0.3344743938681527, + "angularVelocity": -5.015031984210286e-7, + "velocityX": 3.5646465230413518, + "velocityY": -1.081168557598769, + "timestamp": 5.235886052509839 + }, + { + "x": 6.67238155121755, + "y": 6.395147846497736, + "heading": -0.33447440919657534, + "angularVelocity": -4.750628499499889e-7, + "velocityX": 3.506301032167155, + "velocityY": -1.2575704619491082, + "timestamp": 5.268152146603099 + }, + { + "x": 6.783352144572337, + "y": 6.348980132760865, + "heading": -0.33447442388388354, + "angularVelocity": -4.551932493526412e-7, + "velocityX": 3.439232310983837, + "velocityY": -1.4308429648605667, + "timestamp": 5.300418240696359 + }, + { + "x": 6.891882636447486, + "y": 6.297336519628137, + "heading": -0.3344744381114049, + "angularVelocity": -4.409434049866837e-7, + "velocityX": 3.363607989286128, + "velocityY": -1.6005536022878626, + "timestamp": 5.33268433478962 + }, + { + "x": 6.998345027127988, + "y": 6.24155399695869, + "heading": -0.33447445218138366, + "angularVelocity": -4.360607978051808e-7, + "velocityX": 3.299512806625678, + "velocityY": -1.7288278682946643, + "timestamp": 5.36495042888288 + }, + { + "x": 7.107456437455107, + "y": 6.19114935204841, + "heading": -0.33447450198371365, + "angularVelocity": -0.00000154348803003307, + "velocityX": 3.381611979799848, + "velocityY": -1.5621551454164793, + "timestamp": 5.39721652297614 }, { "x": 7.214789390563965, "y": 6.145846366882324, "heading": -0.344987478573796, - "angularVelocity": -6.552383423892292e-7, - "velocityX": 3.442174665036408, - "velocityY": -1.4237498464482023, - "timestamp": 5.3088437349689555 - }, - { - "x": 7.41054811335349, - "y": 6.075273519082507, - "heading": -0.3626370912094401, - "angularVelocity": -0.2874236730044668, - "velocityX": 3.187927819628061, - "velocityY": -1.1492777517419182, - "timestamp": 5.370249994415684 - }, - { - "x": 7.584442418775818, - "y": 6.012933970935389, - "heading": -0.3763799731717047, - "angularVelocity": -0.22380262347988858, - "velocityX": 2.831866115752982, - "velocityY": -1.0151985922737843, - "timestamp": 5.431656253862413 - }, - { - "x": 7.73639717593505, - "y": 5.958610534020796, - "heading": -0.3857543878763611, - "angularVelocity": -0.15266220071243664, - "velocityX": 2.4745809063822897, - "velocityY": -0.8846563429208651, - "timestamp": 5.493062513309142 - }, - { - "x": 7.866387295940067, - "y": 5.912228690600519, - "heading": -0.39060211670074774, - "angularVelocity": -0.07894519008427879, - "velocityX": 2.1168871248017687, - "velocityY": -0.7553276137999276, - "timestamp": 5.554468772755871 - }, - { - "x": 7.974400233400663, - "y": 5.873750785403185, - "heading": -0.39084333039715696, - "angularVelocity": -0.003928161372840292, - "velocityX": 1.7589890417327878, - "velocityY": -0.62661210019988, - "timestamp": 5.6158750322026 - }, - { - "x": 8.060428467430407, - "y": 5.843154117743718, - "heading": -0.3864300742351692, - "angularVelocity": 0.0718698093932331, - "velocityX": 1.4009684811427807, - "velocityY": -0.49826626691061593, - "timestamp": 5.6772812916493285 - }, - { - "x": 8.124466994034947, - "y": 5.820423531988022, - "heading": -0.37733049959220943, - "angularVelocity": 0.14818643449294983, - "velocityX": 1.0428664305809843, - "velocityY": -0.3701672428918256, - "timestamp": 5.738687551096057 - }, - { - "x": 8.16651225164957, - "y": 5.805548215416436, - "heading": -0.3635219686266917, - "angularVelocity": 0.22487171649816598, - "velocityX": 0.6847063799920386, - "velocityY": -0.24224430384806347, - "timestamp": 5.800093810542786 + "angularVelocity": -0.325821171899398, + "velocityX": 3.32649352594794, + "velocityY": -1.404043050117709, + "timestamp": 5.4294826170694 + }, + { + "x": 7.408281473848404, + "y": 6.071835314929934, + "heading": -0.36045376555379516, + "angularVelocity": -0.24376616005736862, + "velocityX": 3.0496538829742375, + "velocityY": -1.1664978129250685, + "timestamp": 5.492929844430077 + }, + { + "x": 7.581043516745906, + "y": 6.008144867615469, + "heading": -0.3717285153471776, + "angularVelocity": -0.17770279746487722, + "velocityX": 2.722925021063643, + "velocityY": -1.003833421315721, + "timestamp": 5.556377071790754 + }, + { + "x": 7.732516109743219, + "y": 5.95357268553042, + "heading": -0.3790773339263405, + "angularVelocity": -0.11582568513809473, + "velocityX": 2.3873792330788404, + "velocityY": -0.8601192574550642, + "timestamp": 5.619824299151431 + }, + { + "x": 7.862474679773266, + "y": 5.907578864525407, + "heading": -0.3826203402067485, + "angularVelocity": -0.05584178265611413, + "velocityX": 2.048293919784284, + "velocityY": -0.7249145930925164, + "timestamp": 5.683271526512108 + }, + { + "x": 7.970798961808127, + "y": 5.869857366205246, + "heading": -0.38242588059176913, + "angularVelocity": 0.003064903275819629, + "velocityX": 1.7073130937475338, + "velocityY": -0.5945334396683074, + "timestamp": 5.746718753872785 + }, + { + "x": 8.057414200976034, + "y": 5.840211247118204, + "heading": -0.378538126264348, + "angularVelocity": 0.061275401450096946, + "velocityX": 1.3651540464570049, + "velocityY": -0.46725633759397367, + "timestamp": 5.810165981233462 + }, + { + "x": 8.122269484577586, + "y": 5.818503192947792, + "heading": -0.37098804864933244, + "angularVelocity": 0.11899775497037605, + "velocityX": 1.0221925574914426, + "velocityY": -0.3421434643157637, + "timestamp": 5.873613208594139 + }, + { + "x": 8.165327926639057, + "y": 5.804632020140207, + "heading": -0.3597986070827489, + "angularVelocity": 0.1763582434103087, + "velocityX": 0.6786497038979674, + "velocityY": -0.2186253581851867, + "timestamp": 5.937060435954816 }, { "x": 8.186561584472656, "y": 5.798520088195801, "heading": -0.344987478573796, - "angularVelocity": 0.3018338882695626, - "velocityX": 0.32650307971422843, - "velocityY": -0.11445294476423894, - "timestamp": 5.861500069989515 - }, - { - "x": 8.182873431203555, - "y": 5.799961031790675, - "heading": -0.31977664522832494, - "angularVelocity": 0.3842917157740494, - "velocityX": -0.056218956684159486, - "velocityY": 0.021964473717296987, - "timestamp": 5.927103444485088 - }, - { - "x": 8.154073485915251, - "y": 5.810352807071496, - "heading": -0.2894961331766682, - "angularVelocity": 0.46156942816562346, - "velocityX": -0.4390009738027679, - "velocityY": 0.15840306021945175, - "timestamp": 5.99270681898066 - }, - { - "x": 8.100157148407472, - "y": 5.829696959053818, - "heading": -0.25457167500592204, - "angularVelocity": 0.5323576483569935, - "velocityX": -0.8218531123184383, - "velocityY": 0.29486519757649204, - "timestamp": 6.058310193476233 - }, - { - "x": 8.021119032382456, - "y": 5.857995174413058, - "heading": -0.21555305979528627, - "angularVelocity": 0.5947653685599519, - "velocityX": -1.2047873548082333, - "velocityY": 0.4313530451265237, - "timestamp": 6.1239135679718055 - }, - { - "x": 7.916952910031998, - "y": 5.895249222659617, - "heading": -0.17317929559353756, - "angularVelocity": 0.6459083016317765, - "velocityX": -1.5878165285154446, - "velocityY": 0.567867865532928, - "timestamp": 6.189516942467378 - }, - { - "x": 7.787651985971443, - "y": 5.941460762829762, - "heading": -0.12849926666272526, - "angularVelocity": 0.6810629677872386, - "velocityX": -1.9709492850749835, - "velocityY": 0.7044079748255081, - "timestamp": 6.255120316962951 - }, - { - "x": 7.633210581172123, - "y": 5.996630683324096, - "heading": -0.08312168761624714, - "angularVelocity": 0.6916958067384257, - "velocityX": -2.354168607740487, - "velocityY": 0.8409616261135585, - "timestamp": 6.320723691458523 - }, - { - "x": 7.453632538523732, - "y": 6.060756399754721, - "heading": -0.03982832718204048, - "angularVelocity": 0.6599258158149841, - "velocityX": -2.7373293527836737, - "velocityY": 0.9774758832711061, - "timestamp": 6.386327065954096 - }, - { - "x": 7.248984101269758, - "y": 6.133814967695316, - "heading": -0.004587884016795663, - "angularVelocity": 0.5371742450172758, - "velocityX": -3.1194803442281134, - "velocityY": 1.11364039582332, - "timestamp": 6.451930440449669 - }, - { - "x": 7.020152981307638, - "y": 6.215344107160959, - "heading": -7.996010360253712e-8, - "angularVelocity": 0.06993244009119806, - "velocityX": -3.488099838180771, - "velocityY": 1.242758319865779, - "timestamp": 6.517533814945241 - }, - { - "x": 6.789970042208637, - "y": 6.297403837687721, - "heading": -7.492708758217055e-8, - "angularVelocity": 7.671885873349635e-8, - "velocityX": -3.5087057772391836, - "velocityY": 1.2508461821929457, - "timestamp": 6.583137189440814 - }, - { - "x": 6.5597871223902215, - "y": 6.379463622297776, - "heading": -6.98940737576559e-8, - "angularVelocity": 7.671882526186561e-8, - "velocityX": -3.5087054833429296, - "velocityY": 1.2508470065910036, - "timestamp": 6.648740563936387 - }, - { - "x": 6.3296039103658925, - "y": 6.46152258724702, - "heading": -6.48610593666683e-8, - "angularVelocity": 7.671883389668008e-8, - "velocityX": -3.5087099374722537, - "velocityY": 1.2508345124042644, - "timestamp": 6.714343938431959 - }, - { - "x": 6.092938315579126, - "y": 6.522410853761584, - "heading": -5.975605245251985e-8, - "angularVelocity": 7.781622444578512e-8, - "velocityX": -3.6075216649524458, - "velocityY": 0.9281270511271241, - "timestamp": 6.779947312927532 + "angularVelocity": 0.23344012220985313, + "velocityX": 0.3346664419690626, + "velocityY": -0.09633095406457304, + "timestamp": 6.000507663315493 + }, + { + "x": 8.184449385960491, + "y": 5.800720366343895, + "heading": -0.3251687978756523, + "angularVelocity": 0.29387941781314064, + "velocityX": -0.03132053432391204, + "velocityY": 0.03262661481041375, + "timestamp": 6.0679458011940195 + }, + { + "x": 8.157654760081511, + "y": 5.811617201116665, + "heading": -0.30136325235519174, + "angularVelocity": 0.3529982628426134, + "velocityX": -0.39732155604955366, + "velocityY": 0.1615826758502398, + "timestamp": 6.135383939072546 + }, + { + "x": 8.106176542546697, + "y": 5.831210437992555, + "heading": -0.27368179509987206, + "angularVelocity": 0.41047185058966307, + "velocityX": -0.7633398423239571, + "velocityY": 0.29053644558191305, + "timestamp": 6.2028220769510725 + }, + { + "x": 8.03001326746699, + "y": 5.859499833662039, + "heading": -0.24226677680218867, + "angularVelocity": 0.46583460466049514, + "velocityX": -1.129379865394508, + "velocityY": 0.4194866074214659, + "timestamp": 6.270260214829599 + }, + { + "x": 7.929163035221131, + "y": 5.896484993829156, + "heading": -0.2073080079914019, + "angularVelocity": 0.5183827713890392, + "velocityX": -1.495448056817871, + "velocityY": 0.5484309224809322, + "timestamp": 6.337698352708125 + }, + { + "x": 7.80362329670357, + "y": 5.942165255300411, + "heading": -0.16907207724227352, + "angularVelocity": 0.5669778548452968, + "velocityX": -1.8615540474099375, + "velocityY": 0.677365403438884, + "timestamp": 6.405136490586652 + }, + { + "x": 7.653390486781624, + "y": 5.996539431834676, + "heading": -0.12796178084950977, + "angularVelocity": 0.6096001118360348, + "velocityX": -2.227712903232215, + "velocityY": 0.8062822943333132, + "timestamp": 6.472574628465178 + }, + { + "x": 7.478459409616276, + "y": 6.059605150575439, + "heading": -0.0846568853073795, + "angularVelocity": 0.6421425161550796, + "velocityX": -2.593948805058106, + "velocityY": 0.9351639995511196, + "timestamp": 6.540012766343705 + }, + { + "x": 7.278822723764661, + "y": 6.131356463760539, + "heading": -0.04054583544960124, + "angularVelocity": 0.6540964985900647, + "velocityX": -2.9602935687698184, + "velocityY": 1.063957508944601, + "timestamp": 6.607450904222231 + }, + { + "x": 7.05448611686489, + "y": 6.21176649036125, + "heading": -5.1297661467442016e-8, + "angularVelocity": 0.6012292958766616, + "velocityX": -3.3265539938819106, + "velocityY": 1.1923524155656642, + "timestamp": 6.674889042100758 + }, + { + "x": 6.817570799613061, + "y": 6.29528987249135, + "heading": -4.834198432704978e-8, + "angularVelocity": 4.3827976770594794e-8, + "velocityX": -3.5130762014599934, + "velocityY": 1.238518511299158, + "timestamp": 6.742327179979284 + }, + { + "x": 6.580655475027505, + "y": 6.378813233819268, + "heading": -4.538633792987658e-8, + "angularVelocity": 4.382752089770155e-8, + "velocityX": -3.5130763102074556, + "velocityY": 1.23851820283599, + "timestamp": 6.809765317857811 + }, + { + "x": 6.3437395023630385, + "y": 6.4623347568384295, + "heading": -4.243069068525663e-8, + "angularVelocity": 4.382753346398488e-8, + "velocityX": -3.5130859201838094, + "velocityY": 1.2384909436497944, + "timestamp": 6.877203455736337 + }, + { + "x": 6.09986574355872, + "y": 6.5225893218228235, + "heading": -3.943141637637637e-8, + "angularVelocity": 4.447445322827149e-8, + "velocityX": -3.6162587888117317, + "velocityY": 0.8934790740059899, + "timestamp": 6.944641593614864 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -5.436468625345948e-8, - "angularVelocity": 8.218123290935499e-8, - "velocityX": -3.6872209612695626, - "velocityY": 0.5291823081772375, - "timestamp": 6.8455506874231045 - }, - { - "x": 5.6615808215647085, - "y": 6.568071618356437, - "heading": -4.94825886082418e-8, - "angularVelocity": 9.582618856188379e-8, - "velocityX": -3.71880135129745, - "velocityY": 0.2148218335343848, - "timestamp": 6.896498113366742 - }, - { - "x": 5.471871500054035, - "y": 6.562921703037617, - "heading": -4.486806140713326e-8, - "angularVelocity": 9.057429527869525e-8, - "velocityX": -3.7236291725620743, - "velocityY": -0.10108293448460899, - "timestamp": 6.94744553931038 - }, - { - "x": 5.283280913941195, - "y": 6.541714647593256, - "heading": -4.0367954549807895e-8, - "angularVelocity": 8.832844395914553e-8, - "velocityX": -3.7016705480170033, - "velocityY": -0.4162537174659499, - "timestamp": 6.998392965254017 - }, - { - "x": 5.095501482386873, - "y": 6.514236775268743, - "heading": -3.5882626212892094e-8, - "angularVelocity": 8.803837002241881e-8, - "velocityX": -3.68574914387354, - "velocityY": -0.539337794119592, - "timestamp": 7.049340391197655 - }, - { - "x": 4.907722071138919, - "y": 6.486758764174086, - "heading": -3.139729791987603e-8, - "angularVelocity": 8.803836916075124e-8, - "velocityX": -3.6857487452985973, - "velocityY": -0.5393405179106829, - "timestamp": 7.100287817141292 - }, - { - "x": 4.7199426574500984, - "y": 6.459280769759847, - "heading": -2.6911969659920007e-8, - "angularVelocity": 8.803836851184644e-8, - "velocityX": -3.6857487932081243, - "velocityY": -0.5393401905061415, - "timestamp": 7.15123524308493 - }, - { - "x": 4.532163244787761, - "y": 6.431802768330805, - "heading": -2.2426641378076926e-8, - "angularVelocity": 8.803836894144722e-8, - "velocityX": -3.6857487730602023, - "velocityY": -0.5393403281932087, - "timestamp": 7.202182669028567 - }, - { - "x": 4.3443838326647874, - "y": 6.404324763215868, - "heading": -1.7941313081549045e-8, - "angularVelocity": 8.803836922968166e-8, - "velocityX": -3.6857487624735623, - "velocityY": -0.5393404005402795, - "timestamp": 7.253130094972205 - }, - { - "x": 4.156604420793327, - "y": 6.376846756382128, - "heading": -1.3455984799695103e-8, - "angularVelocity": 8.80383689416604e-8, - "velocityX": -3.685748757536816, - "velocityY": -0.5393404342770572, - "timestamp": 7.304077520915842 - }, - { - "x": 3.968825009218295, - "y": 6.349368747522659, - "heading": -8.970656536814864e-9, - "angularVelocity": 8.803836856924311e-8, - "velocityX": -3.685748751718508, - "velocityY": -0.5393404740382505, - "timestamp": 7.35502494685948 - }, - { - "x": 3.781045597297714, - "y": 6.321890741024598, - "heading": -4.485328240150933e-9, - "angularVelocity": 8.803836923235202e-8, - "velocityX": -3.6857487585009534, - "velocityY": -0.5393404276883234, - "timestamp": 7.4059723728031175 - }, - { - "x": 3.5932661777631214, - "y": 6.2944127865592865, - "heading": -1.5290485245544054e-22, - "angularVelocity": 8.803836812310754e-8, - "velocityX": -3.6857489079493813, - "velocityY": -0.5393394063855248, - "timestamp": 7.456919798746755 + "heading": -3.6277001373267394e-8, + "angularVelocity": 4.677494222617604e-8, + "velocityX": -3.689626869559746, + "velocityY": 0.5121386527717335, + "timestamp": 7.01207973149339 + }, + { + "x": 5.675146419069591, + "y": 6.5685334564190505, + "heading": -3.325736766733182e-8, + "angularVelocity": 6.38129937324226e-8, + "velocityX": -3.7171934353117426, + "velocityY": 0.24104917118070399, + "timestamp": 7.059399776037881 + }, + { + "x": 5.498885444409021, + "y": 6.56705094919282, + "heading": -3.0392937858444436e-8, + "angularVelocity": 6.053311733876744e-8, + "velocityX": -3.724869161837878, + "velocityY": -0.03132937089348242, + "timestamp": 7.106719820582372 + }, + { + "x": 5.3232044198709785, + "y": 6.552687568221491, + "heading": -2.7611010604394772e-8, + "angularVelocity": 5.878961613051839e-8, + "velocityX": -3.7126132536258285, + "velocityY": -0.3035369283692009, + "timestamp": 7.154039865126863 + }, + { + "x": 5.148803896994517, + "y": 6.527102675424277, + "heading": -2.484990875079416e-8, + "angularVelocity": 5.8349519324830205e-8, + "velocityX": -3.6855528044249266, + "velocityY": -0.5406776989222635, + "timestamp": 7.201359909671353 + }, + { + "x": 4.974403597324897, + "y": 6.501516261174204, + "heading": -2.208880782591367e-8, + "angularVelocity": 5.834949969847243e-8, + "velocityX": -3.6855480874631454, + "velocityY": -0.5407098513192842, + "timestamp": 7.248679954215844 + }, + { + "x": 4.800003297129097, + "y": 6.475929850510635, + "heading": -1.9327706889009288e-8, + "angularVelocity": 5.8349499952569724e-8, + "velocityX": -3.6855480985827302, + "velocityY": -0.5407097755267535, + "timestamp": 7.295999998760335 + }, + { + "x": 4.625602997244748, + "y": 6.450343437724181, + "heading": -1.656660593755509e-8, + "angularVelocity": 5.8349500260046496e-8, + "velocityX": -3.685548092000944, + "velocityY": -0.5407098203890633, + "timestamp": 7.343320043304826 + }, + { + "x": 4.451202697255753, + "y": 6.4247570256510045, + "heading": -1.3805504990627123e-8, + "angularVelocity": 5.834950016439505e-8, + "velocityX": -3.685548094212386, + "velocityY": -0.5407098053155895, + "timestamp": 7.390640087849317 + }, + { + "x": 4.276802397278509, + "y": 6.3991706134977395, + "heading": -1.1044403987822918e-8, + "angularVelocity": 5.8349501345210454e-8, + "velocityX": -3.685548093964082, + "velocityY": -0.5407098070080617, + "timestamp": 7.437960132393807 + }, + { + "x": 4.102402097300047, + "y": 6.373584201352764, + "heading": -8.283303038922857e-9, + "angularVelocity": 5.834950020607066e-8, + "velocityX": -3.685548093989784, + "velocityY": -0.5407098068328745, + "timestamp": 7.485280176938298 + }, + { + "x": 3.9280017973350696, + "y": 6.347997789115884, + "heading": -5.522202092459704e-9, + "angularVelocity": 5.8349500154572286e-8, + "velocityX": -3.685548093704841, + "velocityY": -0.5407098087750815, + "timestamp": 7.532600221482789 + }, + { + "x": 3.7536014973495653, + "y": 6.322411377018916, + "heading": -2.7611011159009457e-9, + "angularVelocity": 5.834950079057347e-8, + "velocityX": -3.6855480941386265, + "velocityY": -0.5407098058183458, + "timestamp": 7.57992026602728 + }, + { + "x": 3.579201219971439, + "y": 6.296824810827607, + "heading": -5.947116132480024e-26, + "angularVelocity": 5.8349503735249215e-8, + "velocityX": -3.6855476163838365, + "velocityY": -0.5407130622468497, + "timestamp": 7.6272403105717705 }, { "x": 3.405486822128296, "y": 6.266934394836426, - "heading": -7.242717716527151e-23, - "angularVelocity": 4.066531625418711e-24, - "velocityX": -3.6857476537198193, - "velocityY": -0.5393479889103638, - "timestamp": 7.507867224690393 - }, - { - "x": 3.1943671277625394, - "y": 6.2230448572371815, - "heading": -6.454692239159141e-23, - "angularVelocity": 6.1636286209902435e-25, - "velocityX": -3.5183764765296694, - "velocityY": -0.7314330248480798, - "timestamp": 7.5678720865276885 - }, - { - "x": 3.002694024502071, - "y": 6.190127687410396, - "heading": -5.666660567021065e-23, - "angularVelocity": 6.173952401211252e-25, - "velocityX": -3.1942928854697095, - "velocityY": -0.5485750457361542, - "timestamp": 7.627876948364984 - }, - { - "x": 2.830467511330891, - "y": 6.168182901091022, - "heading": -4.87863785909476e-23, - "angularVelocity": 6.159013258773547e-25, - "velocityX": -2.870209311341694, - "velocityY": -0.36571680439623283, - "timestamp": 7.68788181020228 - }, - { - "x": 2.6776875881237743, - "y": 6.157210503145755, - "heading": -4.0906073219636964e-23, - "angularVelocity": 6.172060876090466e-25, - "velocityX": -2.5461257393006025, - "velocityY": -0.1828584819513171, - "timestamp": 7.747886672039576 - }, - { - "x": 2.5443542548814535, - "y": 6.157210495895674, - "heading": -3.3025815924682597e-23, - "angularVelocity": 6.164048799254959e-25, - "velocityX": -2.2220421672473036, - "velocityY": -1.208249057531058e-7, - "timestamp": 7.807891533876872 - }, - { - "x": 2.430467511630842, - "y": 6.168182880686502, - "heading": -2.5145589184649632e-23, - "angularVelocity": 6.158956724948031e-25, - "velocityX": -1.8979585947454753, - "velocityY": 0.18285826272844113, - "timestamp": 7.867896395714168 - }, - { - "x": 2.3360273584029394, - "y": 6.1901276583922495, - "heading": -1.726521486745078e-23, - "angularVelocity": 6.183550926375638e-25, - "velocityX": -1.5738750217270354, - "velocityY": 0.3657166608474323, - "timestamp": 7.927901257551464 - }, - { - "x": 2.261033795226874, - "y": 6.223044829624826, - "heading": -9.385003241848346e-24, - "angularVelocity": 6.156437857286479e-25, - "velocityX": -1.2497914482231562, - "velocityY": 0.5485750691641013, - "timestamp": 7.98790611938876 + "heading": -3.242812966980484e-26, + "angularVelocity": 4.141833469401844e-27, + "velocityX": -3.6710531343607253, + "velocityY": -0.6316650011408507, + "timestamp": 7.674560355116261 + }, + { + "x": 3.234879221757396, + "y": 6.227705290951873, + "heading": -3.799073171453003e-26, + "angularVelocity": 3.7193578167117204e-28, + "velocityX": -3.4320264149158155, + "velocityY": -0.7891519514521484, + "timestamp": 7.724270811780997 + }, + { + "x": 3.075654096212167, + "y": 6.19701582942553, + "heading": -4.3640191392241984e-26, + "angularVelocity": -1.3753350907836377e-27, + "velocityX": -3.2030509520179686, + "velocityY": -0.6173643049252793, + "timestamp": 7.773981268445732 + }, + { + "x": 2.9276929132783946, + "y": 6.175021768989464, + "heading": -4.923363376741485e-26, + "angularVelocity": -2.4846347668894466e-28, + "velocityX": -2.9764599414500257, + "velocityY": -0.4424433391228337, + "timestamp": 7.823691725110468 + }, + { + "x": 2.790955431257819, + "y": 6.16177499390257, + "heading": -5.486689770041805e-26, + "angularVelocity": -1.0495335231555751e-27, + "velocityX": -2.7506784526800994, + "velocityY": -0.2664786440453503, + "timestamp": 7.873402181775203 + }, + { + "x": 2.6654213921010017, + "y": 6.157301437220893, + "heading": -6.044835484657667e-26, + "angularVelocity": -7.36271402334531e-30, + "velocityX": -2.525304484798085, + "velocityY": -0.08999226685541546, + "timestamp": 7.923112638439939 + }, + { + "x": 2.551078597067916, + "y": 6.16161665554964, + "heading": -6.59367177391361e-26, + "angularVelocity": 1.865367086612129e-27, + "velocityX": -2.300175912771294, + "velocityY": 0.08680705465754052, + "timestamp": 7.972823095104674 + }, + { + "x": 2.447918895369922, + "y": 6.174731018536324, + "heading": -7.161083135132275e-26, + "angularVelocity": -1.871285764514843e-27, + "velocityX": -2.075211306018359, + "velocityY": 0.2638149771009456, + "timestamp": 8.02253355176941 + }, + { + "x": 2.355936456054869, + "y": 6.196651932350337, + "heading": -7.721743669966212e-26, + "angularVelocity": -5.132563200735982e-28, + "velocityX": -1.8503639975672366, + "velocityY": 0.44097188569106954, + "timestamp": 8.072244008434145 + }, + { + "x": 2.2751269010087887, + "y": 6.227384951214515, + "heading": -8.282082994441975e-26, + "angularVelocity": -4.486400643129525e-28, + "velocityX": -1.6256047614104199, + "velocityY": 0.618240525760057, + "timestamp": 8.12195446509888 }, { "x": 2.205486822128296, "y": 6.266934394836426, - "heading": -1.5067646314431248e-24, - "angularVelocity": 5.827628634700505e-25, - "velocityX": -0.9257078742918208, - "velocityY": 0.7314334850167038, - "timestamp": 8.047910981226057 - }, - { - "x": 2.166583654686104, - "y": 6.330345399138925, - "heading": 0.03190215483964595, - "angularVelocity": 0.46789281830201823, - "velocityX": -0.5705731398677051, - "velocityY": 0.9300172249677102, - "timestamp": 8.116093593977881 - }, - { - "x": 2.1528025694390287, - "y": 6.405563401029407, - "heading": 0.09569947573294339, - "angularVelocity": 0.9356831356039661, - "velocityX": -0.20212022817660624, - "velocityY": 1.1031845048863924, - "timestamp": 8.184276206729706 - }, - { - "x": 2.1659059898168422, - "y": 6.488336785914272, - "heading": 0.18898264963043915, - "angularVelocity": 1.3681372733109376, - "velocityX": 0.1921812592531237, - "velocityY": 1.2139954974466904, - "timestamp": 8.25245881948153 - }, - { - "x": 2.2052860593561716, - "y": 6.55939873181835, - "heading": 0.2899151707587667, - "angularVelocity": 1.4803263919455447, - "velocityX": 0.5775676224475003, - "velocityY": 1.0422297274341026, - "timestamp": 8.320641432233355 - }, - { - "x": 2.238286501729519, - "y": 6.603471711362833, - "heading": 0.35830069991198743, - "angularVelocity": 1.0029760725382513, - "velocityX": 0.4840008477449354, - "velocityY": 0.6463961670829911, - "timestamp": 8.38882404498518 - }, - { - "x": 2.256009101867676, - "y": 6.624549388885498, + "heading": -8.869624024083721e-26, + "angularVelocity": -5.920668887028623e-27, + "velocityX": -1.400914084337811, + "velocityY": 0.7955960631914043, + "timestamp": 8.171664921763616 + }, + { + "x": 2.1363371710210255, + "y": 6.327839189643039, + "heading": 0.025262557050058088, + "angularVelocity": 0.415801673405611, + "velocityX": -1.1381484696439823, + "velocityY": 1.002444667372145, + "timestamp": 8.23242118770922 + }, + { + "x": 2.0845111064453854, + "y": 6.399393278580984, + "heading": 0.0757412033730169, + "angularVelocity": 0.8308385240158422, + "velocityX": -0.8530159608893926, + "velocityY": 1.177723611289897, + "timestamp": 8.293177453654822 + }, + { + "x": 2.051957537582464, + "y": 6.477853810368175, + "heading": 0.14980799257498115, + "angularVelocity": 1.2190806668118568, + "velocityX": -0.5358059511436571, + "velocityY": 1.291398188582541, + "timestamp": 8.353933719600425 + }, + { + "x": 2.0403269766878736, + "y": 6.554154175481095, + "heading": 0.23625288977037628, + "angularVelocity": 1.422814517152715, + "velocityX": -0.19142981737889736, + "velocityY": 1.255843556633874, + "timestamp": 8.414689985546028 + }, + { + "x": 2.0424863781029257, + "y": 6.6144252836562565, + "heading": 0.31360709056854197, + "angularVelocity": 1.2731888570542382, + "velocityX": 0.03554203638823312, + "velocityY": 0.9920146874912401, + "timestamp": 8.475446251491631 + }, + { + "x": 2.048130429243297, + "y": 6.654505373712402, + "heading": 0.3666123519116142, + "angularVelocity": 0.8724246053983875, + "velocityX": 0.09289660996323934, + "velocityY": 0.659686526687304, + "timestamp": 8.536202517437234 + }, + { + "x": 2.052253484725952, + "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": 0.5097673803944184, - "velocityX": 0.2599284395666251, - "velocityY": 0.3091356677601188, - "timestamp": 8.457006657737004 + "angularVelocity": 0.4352739504266956, + "velocityX": 0.0678622265289755, + "velocityY": 0.3266075878021015, + "timestamp": 8.596958783382837 }, { - "x": 2.256009101867676, - "y": 6.624549388885498, + "x": 2.052253484725952, + "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": 1.058900443902291e-24, - "velocityX": 2.5593812661454674e-24, - "velocityY": -7.939073774690169e-24, - "timestamp": 8.525189270488829 + "angularVelocity": -3.1389023173080445e-26, + "velocityX": -4.435068013070024e-22, + "velocityY": -1.8479842669198598e-22, + "timestamp": 8.65771504932844 }, { - "x": 2.306032658844481, - "y": 6.645836035930219, + "x": 2.103013204328941, + "y": 6.695498705652326, "heading": 0.3930579718029317, - "angularVelocity": 7.690085231355673e-17, - "velocityX": 0.5342706205732553, - "velocityY": 0.22734948919726436, - "timestamp": 8.618818893849236 + "angularVelocity": 3.416438400428702e-17, + "velocityX": 0.5194333949867557, + "velocityY": 0.21643049229415012, + "timestamp": 8.755436374320036 }, { - "x": 2.406079771576901, - "y": 6.688409329500005, + "x": 2.2045326425850984, + "y": 6.737798454207702, "heading": 0.3930579718029317, - "angularVelocity": 7.285372137244723e-17, - "velocityX": 1.0685412281037379, - "velocityY": 0.45469897284440564, - "timestamp": 8.712448517209642 + "angularVelocity": 5.790454228147962e-17, + "velocityX": 1.0388667802538214, + "velocityY": 0.432860980538431, + "timestamp": 8.853157699311632 }, { - "x": 2.556150436401367, - "y": 6.752269268035889, - "heading": 0.3930579718029317, - "angularVelocity": 1.0864711023830975e-16, - "velocityX": 1.6028117965059043, - "velocityY": 0.6820484398411784, - "timestamp": 8.80607814057005 + "x": 2.356811797278175, + "y": 6.801248075919452, + "heading": 0.3930579718029318, + "angularVelocity": 1.3697605676063785e-16, + "velocityX": 1.558300142841611, + "velocityY": 0.6492914593330171, + "timestamp": 8.950879024303228 }, { - "x": 2.7062211012258333, - "y": 6.816129206571772, - "heading": 0.3930579718029317, - "angularVelocity": 1.1973301038102625e-17, - "velocityX": 1.6028117965059043, - "velocityY": 0.6820484398411785, - "timestamp": 8.899707763930456 + "x": 2.5598506573269275, + "y": 6.8858475661703915, + "heading": 0.3930579718029318, + "angularVelocity": 1.6886690536158693e-16, + "velocityX": 2.0777333920330334, + "velocityY": 0.8657218908791364, + "timestamp": 9.048600349294825 + }, + { + "x": 2.712129812020004, + "y": 6.9492971878821415, + "heading": 0.3930579718029318, + "angularVelocity": 9.579438090360999e-17, + "velocityX": 1.5583001428416108, + "velocityY": 0.649291459333017, + "timestamp": 9.14632167428642 }, { - "x": 2.8062682139582535, - "y": 6.8587025001415585, + "x": 2.8136492502761614, + "y": 6.991596936437518, "heading": 0.3930579718029317, - "angularVelocity": -9.389335874202783e-18, - "velocityX": 1.0685412281037379, - "velocityY": 0.45469897284440564, - "timestamp": 8.993337387290863 + "angularVelocity": -7.496030828989607e-17, + "velocityX": 1.0388667802538214, + "velocityY": 0.4328609805384309, + "timestamp": 9.244042999278017 }, { - "x": 2.8562917709350586, - "y": 6.879989147186279, + "x": 2.8644089698791504, + "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 6.118834146220853e-17, - "velocityX": 0.5342706205732553, - "velocityY": 0.22734948919726436, - "timestamp": 9.08696701065127 + "angularVelocity": 4.893193347075666e-17, + "velocityX": 0.5194333949867557, + "velocityY": 0.21643049229415012, + "timestamp": 9.341764324269613 }, { - "x": 2.8562917709350586, - "y": 6.879989147186279, + "x": 2.8644089698791504, + "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 9.527092238533276e-28, - "velocityX": 5.413823231130942e-26, - "velocityY": -7.426199884271778e-26, - "timestamp": 9.180596634011676 - }, - { - "x": 2.8345596275672933, - "y": 6.874297000721419, - "heading": 0.3901475474656487, - "angularVelocity": -0.04834267054248828, - "velocityX": -0.36097480135517596, - "velocityY": -0.09454757428506516, - "timestamp": 9.240800679518948 - }, - { - "x": 2.791210221547733, - "y": 6.8624934986264625, - "heading": 0.38424126669366737, - "angularVelocity": -0.0981043835545563, - "velocityX": -0.7200414134017654, - "velocityY": -0.19605828803531558, - "timestamp": 9.30100472502622 - }, - { - "x": 2.7264146334756165, - "y": 6.84400633385399, - "heading": 0.3752232336055894, - "angularVelocity": -0.14979114795514328, - "velocityX": -1.076266345993141, - "velocityY": -0.3070751245485542, - "timestamp": 9.36120877053349 - }, - { - "x": 2.6404493956081283, - "y": 6.818009888574526, - "heading": 0.36292730942366375, - "angularVelocity": -0.20423750726918255, - "velocityX": -1.4278980281666382, - "velocityY": -0.431805621373474, - "timestamp": 9.421412816040762 - }, - { - "x": 2.5338184232966747, - "y": 6.783217894913327, - "heading": 0.3470968619442176, - "angularVelocity": -0.2629465735410421, - "velocityX": -1.7711595859214113, - "velocityY": -0.5779012584294861, - "timestamp": 9.481616861548034 - }, - { - "x": 2.407629795142463, - "y": 6.737392802929253, - "heading": 0.3272924409497442, - "angularVelocity": -0.3289549867887417, - "velocityX": -2.0960157592560678, - "velocityY": -0.761163001555094, - "timestamp": 9.541820907055305 - }, - { - "x": 2.2652115507191506, - "y": 6.676063122618794, - "heading": 0.30267146092102903, - "angularVelocity": -0.40895889671967695, - "velocityX": -2.3655925980274817, - "velocityY": -1.0186969960855932, - "timestamp": 9.602024952562576 - }, - { - "x": 2.1204311340072723, - "y": 6.592422966921264, - "heading": 0.27232018505584993, - "angularVelocity": -0.5041401389133051, - "velocityX": -2.4048287036523215, - "velocityY": -1.3892779960680068, - "timestamp": 9.662228998069848 - }, - { - "x": 1.992685360010843, - "y": 6.494128334362165, - "heading": 0.23976877399587285, - "angularVelocity": -0.5406847793317353, - "velocityX": -2.1218802311382734, - "velocityY": -1.6326914866082545, - "timestamp": 9.72243304357712 - }, - { - "x": 1.886238292214546, - "y": 6.388651713547729, - "heading": 0.20716928495643328, - "angularVelocity": -0.5414833631986058, - "velocityX": -1.7681048989214407, - "velocityY": -1.7519856003978374, - "timestamp": 9.78263708908439 - }, - { - "x": 1.801932900713492, - "y": 6.279311808551523, - "heading": 0.17534566664181037, - "angularVelocity": -0.5285960112228498, - "velocityX": -1.4003276821467197, - "velocityY": -1.8161554439559784, - "timestamp": 9.842841134591662 + "angularVelocity": 3.090778525044755e-27, + "velocityX": 4.437436909321685e-22, + "velocityY": 1.850515832937087e-22, + "timestamp": 9.43948564926121 + }, + { + "x": 2.8419574642644285, + "y": 7.0048102452086525, + "heading": 0.3900130033247021, + "angularVelocity": -0.04733865202910357, + "velocityX": -0.3490426976251426, + "velocityY": -0.12338594795790273, + "timestamp": 9.503808741442997 + }, + { + "x": 2.7972074454620057, + "y": 6.988518822273239, + "heading": 0.3838409042111465, + "angularVelocity": -0.0959546393713819, + "velocityX": -0.6957068959923765, + "velocityY": -0.25327487194445947, + "timestamp": 9.568131833624784 + }, + { + "x": 2.7303816098102875, + "y": 6.963303209303571, + "heading": 0.37443012101631223, + "angularVelocity": -0.14630489417762893, + "velocityX": -1.0389089421083304, + "velocityY": -0.39201493762775413, + "timestamp": 9.632454925806572 + }, + { + "x": 2.6418300370011316, + "y": 6.928345468623928, + "heading": 0.3616207312094327, + "angularVelocity": -0.19914138721251212, + "velocityX": -1.3766684685943704, + "velocityY": -0.5434710847054101, + "timestamp": 9.69677801798836 + }, + { + "x": 2.5321688819092247, + "y": 6.882378878870811, + "heading": 0.34516587333325927, + "angularVelocity": -0.25581571591224617, + "velocityX": -1.7048489332880077, + "velocityY": -0.7146203360872027, + "timestamp": 9.761101110170147 + }, + { + "x": 2.4026942269657687, + "y": 6.823220010782376, + "heading": 0.324643183634766, + "angularVelocity": -0.3190563295758964, + "velocityX": -2.012879831360391, + "velocityY": -0.9197143060417909, + "timestamp": 9.825424202351934 + }, + { + "x": 2.2570878780964114, + "y": 6.7465695928484575, + "heading": 0.2992393598461131, + "angularVelocity": -0.3949409601898093, + "velocityX": -2.2636714736575443, + "velocityY": -1.1916469705357338, + "timestamp": 9.889747294533722 + }, + { + "x": 2.110110870224978, + "y": 6.646173604450097, + "heading": 0.26804931760848005, + "angularVelocity": -0.4848964995259366, + "velocityX": -2.2849804461522467, + "velocityY": -1.5608078684187743, + "timestamp": 9.95407038671551 + }, + { + "x": 1.9823765062843866, + "y": 6.531738038725912, + "heading": 0.2347990669991598, + "angularVelocity": -0.5169255625234794, + "velocityX": -1.9858243689465778, + "velocityY": -1.779074385926154, + "timestamp": 10.018393478897297 + }, + { + "x": 1.8778139110395455, + "y": 6.411761042169391, + "heading": 0.201692518308331, + "angularVelocity": -0.5146914983077023, + "velocityX": -1.6255840896039286, + "velocityY": -1.865224330594162, + "timestamp": 10.082716571079084 + }, + { + "x": 1.796996456842112, + "y": 6.289847592193697, + "heading": 0.16955186256906168, + "angularVelocity": -0.49967522780830353, + "velocityX": -1.2564298676598118, + "velocityY": -1.8953294352072878, + "timestamp": 10.147039663260871 }, { "x": 1.740001559257507, "y": 6.167843341827393, - "heading": 0.1447051188724413, - "angularVelocity": -0.5089449971541862, - "velocityX": -1.0286906956859625, - "velocityY": -1.8515112362451833, - "timestamp": 9.903045180098934 - }, - { - "x": 1.69740928285499, - "y": 6.020477623351315, - "heading": 0.10712758929116097, - "angularVelocity": -0.47810334665219445, - "velocityX": -0.5419065626848166, - "velocityY": -1.874951439604909, - "timestamp": 9.98164226552912 - }, - { - "x": 1.692516979833633, - "y": 5.879848304102219, - "heading": 0.07335850742026521, - "angularVelocity": -0.429648016667122, - "velocityX": -0.06224534910652133, - "velocityY": -1.7892434366922005, - "timestamp": 10.060239350959305 - }, - { - "x": 1.7224083305171312, - "y": 5.755218848404405, - "heading": 0.044801662501508355, - "angularVelocity": -0.3633321103760591, - "velocityX": 0.38031118482184595, - "velocityY": -1.585675283195029, - "timestamp": 10.138836436389491 - }, - { - "x": 1.7819268218986297, - "y": 5.6548248770557, - "heading": 0.022621166422217082, - "angularVelocity": -0.282205071064539, - "velocityX": 0.757260794795832, - "velocityY": -1.277324353685865, - "timestamp": 10.217433521819677 - }, - { - "x": 1.8649022251310534, - "y": 5.584669337944561, - "heading": 0.0075685613575453385, - "angularVelocity": -0.19151607190373818, - "velocityX": 1.055705854463611, - "velocityY": -0.8925972092623536, - "timestamp": 10.296030607249863 - }, - { - "x": 1.9654334653384358, + "heading": 0.1387804363394676, + "angularVelocity": -0.4783884789404854, + "velocityX": -0.8860721033672938, + "velocityY": -1.8967410649584446, + "timestamp": 10.211362755442659 + }, + { + "x": 1.706182040858774, + "y": 6.03669580225492, + "heading": 0.10729245974250749, + "angularVelocity": -0.45130411484825544, + "velocityX": -0.48472113692462804, + "velocityY": -1.8796833159166881, + "timestamp": 10.281133837870186 + }, + { + "x": 1.699651780790286, + "y": 5.911922041931935, + "heading": 0.07852355852866735, + "angularVelocity": -0.4123327346071031, + "velocityX": -0.09359551036449741, + "velocityY": -1.7883305802599465, + "timestamp": 10.350904920297713 + }, + { + "x": 1.7185433856985064, + "y": 5.798927064131694, + "heading": 0.053308691164350094, + "angularVelocity": -0.36139424080898114, + "velocityX": 0.2707655414095584, + "velocityY": -1.6195101733961197, + "timestamp": 10.42067600272524 + }, + { + "x": 1.7598996151781712, + "y": 5.702665752208848, + "heading": 0.032386709878439925, + "angularVelocity": -0.29986608431425904, + "velocityX": 0.592741692414218, + "velocityY": -1.3796734775160497, + "timestamp": 10.490447085152768 + }, + { + "x": 1.820114668725021, + "y": 5.6271104583982785, + "heading": 0.016317787108466864, + "angularVelocity": -0.2303092084985792, + "velocityX": 0.8630373996189019, + "velocityY": -1.082902703839363, + "timestamp": 10.560218167580295 + }, + { + "x": 1.8954858630630067, + "y": 5.575103001600796, + "heading": 0.005460341828234747, + "angularVelocity": -0.15561526211822638, + "velocityX": 1.0802640824194598, + "velocityY": -0.7454013179672614, + "timestamp": 10.629989250007823 + }, + { + "x": 1.9826012990017379, "y": 5.548515796661377, - "heading": 4.814116720601663e-28, - "angularVelocity": -0.09629569997564517, - "velocityX": 1.2790708415858416, - "velocityY": -0.4599857753669162, - "timestamp": 10.374627692680049 + "heading": -3.619508887306417e-27, + "angularVelocity": -0.07826081577430707, + "velocityX": 1.248589428567619, + "velocityY": -0.3810633863540251, + "timestamp": 10.69976033243535 }, { "x": 2.0785037517547607, "y": 5.548515796661377, - "heading": 4.711208229171849e-28, - "angularVelocity": -4.224112944688565e-28, - "velocityX": 1.43860660732464, - "velocityY": -1.1518576201030206e-26, - "timestamp": 10.453224778110235 + "heading": -1.3504694626823558e-27, + "angularVelocity": 2.3770174727505955e-28, + "velocityX": 1.3745300978043182, + "velocityY": 2.5815580956362606e-25, + "timestamp": 10.769531414862877 }, { - "x": 2.2115594481070073, + "x": 2.2108243531689626, "y": 5.548515796661377, - "heading": 9.919579688456685e-28, - "angularVelocity": -1.4478744000102224e-30, - "velocityX": 1.8779766683240853, - "velocityY": 6.343973751493874e-16, - "timestamp": 10.52407533690391 + "heading": -3.98229506769339e-28, + "angularVelocity": 6.58921025141663e-29, + "velocityX": 1.7982508177997887, + "velocityY": 1.343058267982166e-16, + "timestamp": 10.843114365641734 }, { - "x": 2.367207543861562, + "x": 2.366717480768576, "y": 5.548515796661377, - "heading": 1.5127951147748595e-27, - "angularVelocity": -1.4478743900360266e-30, - "velocityX": 2.196850644577389, - "velocityY": 3.1719689645397288e-15, - "timestamp": 10.594925895697587 + "heading": 5.540104511762132e-28, + "angularVelocity": 6.589213013653999e-29, + "velocityX": 2.118603915030903, + "velocityY": 6.71525155127488e-16, + "timestamp": 10.91669731642059 }, { - "x": 2.4917260252771096, + "x": 2.4914319874847974, "y": 5.548515796661377, - "heading": 2.033632260703789e-27, - "angularVelocity": -1.4478743937385766e-30, - "velocityX": 1.757480583578164, - "velocityY": 2.537576634676114e-15, - "timestamp": 10.665776454491263 + "heading": 1.506250408105455e-27, + "angularVelocity": 6.589211632476856e-29, + "velocityX": 1.694883195035658, + "velocityY": 5.372204471623799e-16, + "timestamp": 10.990280267199447 }, { - "x": 2.585114887925518, + "x": 2.58496786905088, "y": 5.548515796661377, - "heading": 2.5544694066327203e-27, - "angularVelocity": -1.4478743937382235e-30, - "velocityX": 1.3181104600793105, - "velocityY": 1.903182958104165e-15, - "timestamp": 10.73662701328494 + "heading": 2.458490365034698e-27, + "angularVelocity": 6.589211632476792e-29, + "velocityX": 1.2711624170548799, + "velocityY": 4.029154421641203e-16, + "timestamp": 11.063863217978303 }, { - "x": 2.6473741303307423, + "x": 2.647325124044576, "y": 5.548515796661377, - "heading": 3.0753065525616534e-27, - "angularVelocity": -1.4478743937387e-30, - "velocityX": 0.8787403157472581, - "velocityY": 1.2687888330822125e-15, - "timestamp": 10.807477572078616 + "heading": 3.4107303219639416e-27, + "angularVelocity": 6.589211632476844e-29, + "velocityX": 0.8474416197456015, + "velocityY": 2.686103379493745e-16, + "timestamp": 11.13744616875716 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 3.596143698490385e-27, - "angularVelocity": -1.4478743965967547e-30, - "velocityX": 0.439370160998608, - "velocityY": 6.343944838152068e-16, - "timestamp": 10.878328130872292 + "heading": 4.3629702788936556e-27, + "angularVelocity": 6.589211633112284e-29, + "velocityX": 0.42372081277207485, + "velocityY": 1.3430518397832439e-16, + "timestamp": 11.211029119536017 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 4.090371729515312e-27, - "angularVelocity": -3.7701463571595157e-28, - "velocityX": -1.25991204352499e-25, - "velocityY": -5.479637988962548e-26, - "timestamp": 10.949178689665969 - }, - { - "x": 2.6551487281624913, - "y": 5.535337362195577, - "heading": -0.005358266388786186, - "angularVelocity": -0.0814332978786503, - "velocityX": -0.3549425234125069, - "velocityY": -0.20028182653884274, - "timestamp": 11.014978141835657 - }, - { - "x": 2.6089441252483114, - "y": 5.508126235802502, - "heading": -0.016249929633628903, - "angularVelocity": -0.16552817517013077, - "velocityX": -0.7022034590048662, - "velocityY": -0.41354639736059473, - "timestamp": 11.080777594005346 - }, - { - "x": 2.5407349136089206, - "y": 5.4655968271259, - "heading": -0.03293816472196892, - "angularVelocity": -0.2536227056313986, - "velocityX": -1.036622789251932, - "velocityY": -0.6463489782091849, - "timestamp": 11.146577046175034 - }, - { - "x": 2.4521626111212957, - "y": 5.405634188324023, - "heading": -0.055847587039488555, - "angularVelocity": -0.3481704111827463, - "velocityX": -1.3460948315984265, - "velocityY": -0.9112938911290818, - "timestamp": 11.212376498344723 - }, - { - "x": 2.347299393995622, - "y": 5.324395116461008, - "heading": -0.08569447081932854, - "angularVelocity": -0.45360383400866283, - "velocityX": -1.5936791822406984, - "velocityY": -1.2346466297851597, - "timestamp": 11.278175950514411 - }, - { - "x": 2.239880929566337, - "y": 5.216483135728542, - "heading": -0.1227261468780359, - "angularVelocity": -0.5627961151288575, - "velocityX": -1.6325130512069794, - "velocityY": -1.6400133614209302, - "timestamp": 11.3439754026841 - }, - { - "x": 2.153374403298574, - "y": 5.091739159361965, - "heading": -0.1618030539678993, - "angularVelocity": -0.5938789123819666, - "velocityX": -1.3146997948352968, - "velocityY": -1.8958208959685574, - "timestamp": 11.409774854853788 + "heading": 5.369537146588021e-27, + "angularVelocity": 8.042005177965745e-28, + "velocityX": 2.643601734591811e-26, + "velocityY": 4.252096317345015e-26, + "timestamp": 11.284612070314873 + }, + { + "x": 2.65514872822099, + "y": 5.535337362244174, + "heading": -0.005358266322899027, + "angularVelocity": -0.07847108857121549, + "velocityX": -0.34203117386479875, + "velocityY": -0.1929963969806516, + "timestamp": 11.352895388674044 + }, + { + "x": 2.608944125410673, + "y": 5.508126235941394, + "heading": -0.01624992945284291, + "angularVelocity": -0.15950693949367814, + "velocityX": -0.676660184662962, + "velocityY": -0.39850327952208153, + "timestamp": 11.421178707033215 + }, + { + "x": 2.5407349138993025, + "y": 5.465596827387689, + "heading": -0.03293816440101749, + "angularVelocity": -0.24439695300679784, + "velocityX": -0.9989147151957237, + "velocityY": -0.622837459802409, + "timestamp": 11.489462025392386 + }, + { + "x": 2.4521626115248987, + "y": 5.405634188727449, + "heading": -0.05584758658914312, + "angularVelocity": -0.33550540217775826, + "velocityX": -1.2971294380936889, + "velocityY": -0.8781447665568274, + "timestamp": 11.557745343751558 + }, + { + "x": 2.3472993944122122, + "y": 5.324395116991271, + "heading": -0.08569447031240085, + "angularVelocity": -0.43710359192361903, + "velocityX": -1.5357076901433586, + "velocityY": -1.1897352631408427, + "timestamp": 11.626028662110729 + }, + { + "x": 2.239880929754108, + "y": 5.216483136191594, + "heading": -0.12272614650889033, + "angularVelocity": -0.542323909943895, + "velocityX": -1.5731289462688671, + "velocityY": -1.580356423688402, + "timestamp": 11.6943119804699 + }, + { + "x": 2.153374403355559, + "y": 5.091739159562662, + "heading": -0.16180305380781881, + "angularVelocity": -0.5722760439582536, + "velocityX": -1.2668764271754205, + "velocityY": -1.8268587354348766, + "timestamp": 11.762595298829071 }, { "x": 2.0932393074035645, "y": 4.9619622230529785, - "heading": -0.19963034769256285, - "angularVelocity": -0.5748876696892882, - "velocityX": -0.9139148414173566, - "velocityY": -1.9723102857195454, - "timestamp": 11.475574307023477 - }, - { - "x": 2.0629011107007105, - "y": 4.862373019847467, - "heading": -0.22719939375272252, - "angularVelocity": -0.5460591677053324, - "velocityX": -0.600907641312322, - "velocityY": -1.9725599970405534, - "timestamp": 11.52606159428213 - }, - { - "x": 2.048028599586168, - "y": 4.7660067703914, - "heading": -0.2526003668147138, - "angularVelocity": -0.5031162187791786, - "velocityX": -0.29457932723437075, - "velocityY": -1.908723060567117, - "timestamp": 11.576548881540782 - }, - { - "x": 2.047740775609064, - "y": 4.675697634358311, - "heading": -0.2752320074015959, - "angularVelocity": -0.44826414362367223, - "velocityX": -0.00570091983015821, - "velocityY": -1.788750018800283, - "timestamp": 11.627036168799435 - }, - { - "x": 2.0608710405337267, - "y": 4.5937072504707785, - "heading": -0.29463334748351117, - "angularVelocity": -0.3842816902108431, - "velocityX": 0.2600707155723077, - "velocityY": -1.62398077495204, - "timestamp": 11.677523456058088 - }, - { - "x": 2.0861920181883997, - "y": 4.5217424317614965, - "heading": -0.3104741156384499, - "angularVelocity": -0.31375756185482273, - "velocityX": 0.5015317524380102, - "velocityY": -1.4254047427939316, - "timestamp": 11.72801074331674 - }, - { - "x": 2.122546344803851, - "y": 4.461060494351438, - "heading": -0.3225286157246152, - "angularVelocity": -0.23876307761216561, - "velocityX": 0.7200689240680361, - "velocityY": -1.2019250925322578, - "timestamp": 11.778498030575394 - }, - { - "x": 2.1689002066225895, - "y": 4.412584630951134, - "heading": -0.3306480218329806, - "angularVelocity": -0.16082080359692716, - "velocityX": 0.9181293813878815, - "velocityY": -0.9601597953155385, - "timestamp": 11.828985317834046 - }, - { - "x": 2.2243532862123785, - "y": 4.376998478332531, + "heading": -0.19963034769606672, + "angularVelocity": -0.5539756238745723, + "velocityX": -0.8806703803655747, + "velocityY": -1.9005657549777475, + "timestamp": 11.830878617188242 + }, + { + "x": 2.062901110669511, + "y": 4.862373019706677, + "heading": -0.22719939385993115, + "angularVelocity": -0.526195783497447, + "velocityX": -0.5790490938819385, + "velocityY": -1.9008063815925753, + "timestamp": 11.883271751129039 + }, + { + "x": 2.0480285995415843, + "y": 4.7660067701880315, + "heading": -0.2526003669786383, + "angularVelocity": -0.4848149215011564, + "velocityX": -0.28386374338156817, + "velocityY": -1.8392915687680964, + "timestamp": 11.935664885069835 + }, + { + "x": 2.0477407755531423, + "y": 4.675697634157938, + "heading": -0.27523200757833177, + "angularVelocity": -0.43195813835581237, + "velocityX": -0.005493544035129206, + "velocityY": -1.723682651473774, + "timestamp": 11.988058019010632 + }, + { + "x": 2.060871040463269, + "y": 4.593707250314659, + "heading": -0.2946333476397457, + "angularVelocity": -0.37030310275649647, + "velocityX": 0.2506104125201524, + "velocityY": -1.5649070341149178, + "timestamp": 12.040451152951428 + }, + { + "x": 2.0861920181034543, + "y": 4.521742431666527, + "heading": -0.3104741157540841, + "angularVelocity": -0.30234435169001433, + "velocityX": 0.48328809016841023, + "velocityY": -1.373554380798257, + "timestamp": 12.092844286892225 + }, + { + "x": 2.122546344711596, + "y": 4.4610604943149115, + "heading": -0.32252861579276854, + "angularVelocity": -0.23007785814655887, + "velocityX": 0.6938757786320086, + "velocityY": -1.1582040009323555, + "timestamp": 12.145237420833022 + }, + { + "x": 2.168900206537902, + "y": 4.412584630956035, + "heading": -0.3306480218589531, + "angularVelocity": -0.15497080352855533, + "velocityX": 0.8847316115635476, + "velocityY": -0.9252331309986794, + "timestamp": 12.197630554773818 + }, + { + "x": 2.224353286156776, + "y": 4.376998478351871, "heading": -0.3347371673945201, - "angularVelocity": -0.0809935685510361, - "velocityX": 1.098357281620145, - "velocityY": -0.7048537275590006, - "timestamp": 11.8794726050927 + "angularVelocity": -0.07804735521619326, + "velocityX": 1.0584035626029547, + "velocityY": -0.6792140482448454, + "timestamp": 12.250023688714615 }, { "x": 2.2881293296813965, "y": 4.354815483093262, "heading": -0.3347371673945201, - "angularVelocity": 5.296142385201319e-27, - "velocityX": 1.2632099471355256, - "velocityY": -0.43937784031895555, - "timestamp": 11.929959892351352 + "angularVelocity": 3.779464021226463e-27, + "velocityX": 1.2172595668105288, + "velocityY": -0.4233950823341772, + "timestamp": 12.302416822655411 }, { - "x": 2.4097151173059284, - "y": 4.31443319431648, + "x": 2.4097151176038345, + "y": 4.314433194202397, "heading": -0.3347371673945201, - "angularVelocity": 4.260827770605265e-29, - "velocityX": 1.6917387541138205, - "velocityY": -0.5618772081689671, - "timestamp": 12.001830201717304 + "angularVelocity": 6.573584275341322e-29, + "velocityX": 1.6302002618641742, + "velocityY": -0.5414384283676151, + "timestamp": 12.377000167270632 }, { - "x": 2.501147235462863, - "y": 4.284859003573599, + "x": 2.5011472356587063, + "y": 4.284859003501582, "heading": -0.3347371673945201, - "angularVelocity": 4.260827800538464e-29, - "velocityX": 1.2721820590944748, - "velocityY": -0.41149385613875655, - "timestamp": 12.073700511083256 + "angularVelocity": 6.573584272217567e-29, + "velocityX": 1.225905308035939, + "velocityY": -0.396525401929742, + "timestamp": 12.451583511885852 }, { - "x": 2.562139216145648, - "y": 4.265257651838079, + "x": 2.562139216220098, + "y": 4.265257651811236, "heading": -0.3347371673945201, - "angularVelocity": 4.260827801548558e-29, - "velocityX": 0.8486394621209058, - "velocityY": -0.27273225770760434, - "timestamp": 12.145570820449208 + "angularVelocity": -6.199774132429429e-28, + "velocityX": 0.8177694480725244, + "velocityY": -0.2628113795576003, + "timestamp": 12.526166856501073 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": 1.3873453317425255e-27, - "velocityX": 0.4244517046157105, - "velocityY": -0.13595549514112237, - "timestamp": 12.21744112981516 + "angularVelocity": 7.63642932425348e-28, + "velocityX": 0.40901189675210486, + "velocityY": -0.1310099931742232, + "timestamp": 12.600750201116293 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": -3.25532193935464e-28, - "velocityX": 2.1483187881556724e-26, - "velocityY": -1.7096603516619137e-26, - "timestamp": 12.289311439181112 + "angularVelocity": 1.6433960683327454e-29, + "velocityX": 1.171390593825864e-26, + "velocityY": -5.961445669521376e-27, + "timestamp": 12.675333545731513 } ], "trajectoryWaypoints": [ @@ -30937,10 +31009,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 11 }, { - "timestamp": 0.5891141473001633, + "timestamp": 0.6133453390157622, "isStopPoint": false, "x": 2.3368515968322754, "y": 6.133185386657715, @@ -30948,10 +31020,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 17 }, { - "timestamp": 1.5430433785236604, + "timestamp": 1.5696159325577983, "isStopPoint": false, "x": 5.747424125671387, "y": 7.119814872741699, @@ -30959,10 +31031,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 12 }, { - "timestamp": 1.94301474943974, + "timestamp": 1.9695805037098402, "isStopPoint": false, "x": 7.125290870666504, "y": 7.458, @@ -30973,7 +31045,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 2.508796322508637, + "timestamp": 2.554317040186094, "isStopPoint": false, "x": 8.186561584472656, "y": 7.469284534454346, @@ -30984,7 +31056,7 @@ "controlIntervalCount": 19 }, { - "timestamp": 4.232069897702398, + "timestamp": 4.327573921984801, "isStopPoint": true, "x": 4.337841033935547, "y": 6.292478084564209, @@ -30995,7 +31067,7 @@ "controlIntervalCount": 12 }, { - "timestamp": 4.922900661772352, + "timestamp": 5.042289487950278, "isStopPoint": false, "x": 5.851044178009033, "y": 6.557126998901367, @@ -31006,7 +31078,7 @@ "controlIntervalCount": 12 }, { - "timestamp": 5.3088437349689555, + "timestamp": 5.4294826170694, "isStopPoint": false, "x": 7.214789390563965, "y": 6.145846366882324, @@ -31017,7 +31089,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 5.861500069989515, + "timestamp": 6.000507663315493, "isStopPoint": false, "x": 8.186561584472656, "y": 5.798520088195801, @@ -31028,7 +31100,7 @@ "controlIntervalCount": 15 }, { - "timestamp": 6.8455506874231045, + "timestamp": 7.01207973149339, "isStopPoint": false, "x": 5.851044178009033, "y": 6.557126998901367, @@ -31036,10 +31108,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 14 }, { - "timestamp": 7.507867224690393, + "timestamp": 7.674560355116261, "isStopPoint": false, "x": 3.405486822128296, "y": 6.266934394836426, @@ -31047,10 +31119,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { - "timestamp": 8.047910981226057, + "timestamp": 8.171664921763616, "isStopPoint": false, "x": 2.205486822128296, "y": 6.266934394836426, @@ -31058,24 +31130,24 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 8 }, { - "timestamp": 8.525189270488829, + "timestamp": 8.65771504932844, "isStopPoint": true, - "x": 2.256009101867676, - "y": 6.624549388885498, + "x": 2.052253484725952, + "y": 6.674348831176758, "heading": 0.3930579718029317, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 8 }, { - "timestamp": 9.180596634011676, + "timestamp": 9.43948564926121, "isStopPoint": true, - "x": 2.8562917709350586, - "y": 6.879989147186279, + "x": 2.8644089698791504, + "y": 7.012746810913086, "heading": 0.3930579718029317, "isInitialGuess": false, "translationConstrained": true, @@ -31083,7 +31155,7 @@ "controlIntervalCount": 12 }, { - "timestamp": 9.903045180098934, + "timestamp": 10.211362755442659, "isStopPoint": false, "x": 1.7400015592575073, "y": 6.167843341827393, @@ -31091,10 +31163,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 7 + "controlIntervalCount": 8 }, { - "timestamp": 10.453224778110235, + "timestamp": 10.769531414862877, "isStopPoint": false, "x": 2.0785037517547607, "y": 5.548515796661377, @@ -31105,7 +31177,7 @@ "controlIntervalCount": 7 }, { - "timestamp": 10.949178689665969, + "timestamp": 11.284612070314873, "isStopPoint": true, "x": 2.6785037517547607, "y": 5.548515796661377, @@ -31116,7 +31188,7 @@ "controlIntervalCount": 8 }, { - "timestamp": 11.475574307023477, + "timestamp": 11.830878617188242, "isStopPoint": false, "x": 2.0932393074035645, "y": 4.9619622230529785, @@ -31127,7 +31199,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 11.929959892351352, + "timestamp": 12.302416822655411, "isStopPoint": false, "x": 2.2881293296813965, "y": 4.354815483093262, @@ -31138,7 +31210,7 @@ "controlIntervalCount": 5 }, { - "timestamp": 12.289311439181112, + "timestamp": 12.675333545731513, "isStopPoint": true, "x": 2.592644691467285, "y": 4.255486488342285, @@ -36260,15 +36332,13 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/CenterLanePDEABC.1.traj b/src/main/deploy/choreo/CenterLanePDEABC.1.traj index cb8830a2..d1c28fcf 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.1.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.1.traj @@ -3,596 +3,623 @@ { "x": 1.3350272178649902, "y": 5.601006507873535, - "heading": 1.3464399983715043e-25, - "angularVelocity": 2.380601151660865e-26, - "velocityX": -3.6373884256645e-26, - "velocityY": 3.3932882505305813e-26, + "heading": 3.967978705267542e-25, + "angularVelocity": 7.379244608128328e-26, + "velocityX": 8.172146600807894e-26, + "velocityY": -1.420872098935511e-25, "timestamp": 0 }, { - "x": 1.3513037651461417, - "y": 5.614995878157814, - "heading": 0.006549885022004973, - "angularVelocity": 0.11118193396003606, - "velocityX": 0.27628851481066846, - "velocityY": 0.23746451088284293, - "timestamp": 0.058911414730016334 - }, - { - "x": 1.3843789066016257, - "y": 5.642351283609904, - "heading": 0.01935493583583066, - "angularVelocity": 0.21736111537143754, - "velocityX": 0.5614385871916894, - "velocityY": 0.4643481331666494, - "timestamp": 0.11782282946003267 - }, - { - "x": 1.4348748758418, - "y": 5.682261570187258, - "heading": 0.038033490581189856, - "angularVelocity": 0.31706172447157616, - "velocityX": 0.8571508505030987, - "velocityY": 0.6774627083776117, - "timestamp": 0.17673424419004902 - }, - { - "x": 1.503535032987812, - "y": 5.733639392412991, - "heading": 0.06208045701046981, - "angularVelocity": 0.408188574989825, - "velocityX": 1.1654813835429523, - "velocityY": 0.8721199866136559, - "timestamp": 0.23564565892006534 - }, - { - "x": 1.5912385139518157, - "y": 5.794977395067809, - "heading": 0.09080789039897333, - "angularVelocity": 0.48763781213127805, - "velocityX": 1.4887349313530773, - "velocityY": 1.041190454106761, - "timestamp": 0.29455707365008166 - }, - { - "x": 1.698981116852805, - "y": 5.864111533799201, - "heading": 0.12324878614145561, - "angularVelocity": 0.550672495154884, - "velocityX": 1.828891792783458, - "velocityY": 1.1735270498633465, - "timestamp": 0.353468488380098 - }, - { - "x": 1.8277399268235341, - "y": 5.937848492694958, - "heading": 0.15800938596992012, - "angularVelocity": 0.5900486346791701, - "velocityX": 2.1856343216474206, - "velocityY": 1.2516582606899718, - "timestamp": 0.4123799031101143 - }, - { - "x": 1.9780206143757966, - "y": 6.011501208422765, - "heading": 0.1930700705634149, - "angularVelocity": 0.5951424652450383, - "velocityX": 2.5509604249190097, - "velocityY": 1.2502282633229538, - "timestamp": 0.4712913178401306 - }, - { - "x": 2.1488551922208914, - "y": 6.0788007358405975, - "heading": 0.22564028873153014, - "angularVelocity": 0.5528676966489513, - "velocityX": 2.8998552933757997, - "velocityY": 1.1423851850487312, - "timestamp": 0.5302027325701469 + "x": 1.3486609048876415, + "y": 5.6125344531964885, + "heading": 0.0053952990645401815, + "angularVelocity": 0.09676162177278211, + "velocityX": 0.24451242670209422, + "velocityY": 0.20674714632375757, + "timestamp": 0.05575866718325111 + }, + { + "x": 1.3762911400128175, + "y": 5.635152571560341, + "heading": 0.015980040657686934, + "angularVelocity": 0.1898313235924372, + "velocityX": 0.4955325606038828, + "velocityY": 0.4056430956198756, + "timestamp": 0.11151733436650221 + }, + { + "x": 1.4183448247504244, + "y": 5.668306854106291, + "heading": 0.0314941432622812, + "angularVelocity": 0.2782366112447971, + "velocityX": 0.7542089304143026, + "velocityY": 0.594603210958904, + "timestamp": 0.1672760015497533 + }, + { + "x": 1.4753265664194932, + "y": 5.711278723492, + "heading": 0.05160335039247586, + "angularVelocity": 0.3606471988310923, + "velocityX": 1.0219351456482668, + "velocityY": 0.7706760501373088, + "timestamp": 0.22303466873300443 + }, + { + "x": 1.547831871808343, + "y": 5.763109379844883, + "heading": 0.07586790196130719, + "angularVelocity": 0.43517093923866185, + "velocityX": 1.3003414366157815, + "velocityY": 0.9295533586292857, + "timestamp": 0.27879333591625555 + }, + { + "x": 1.6365515186230641, + "y": 5.822478611934523, + "heading": 0.10369270639110653, + "angularVelocity": 0.499022050479668, + "velocityX": 1.5911364330704598, + "velocityY": 1.064753429175822, + "timestamp": 0.33455200309950667 + }, + { + "x": 1.7422414603963892, + "y": 5.887510284275809, + "heading": 0.1342490325748326, + "angularVelocity": 0.5480103404068561, + "velocityX": 1.8954890264140394, + "velocityY": 1.1663060762833288, + "timestamp": 0.3903106702827578 + }, + { + "x": 1.8655876183871098, + "y": 5.955479067462068, + "heading": 0.1663576179952195, + "angularVelocity": 0.57584922743691, + "velocityX": 2.212143227623125, + "velocityY": 1.2189814896916369, + "timestamp": 0.4460693374660089 + }, + { + "x": 2.0068104942426706, + "y": 6.022477551407248, + "heading": 0.1983353246164728, + "angularVelocity": 0.5735019905723076, + "velocityX": 2.532752000535291, + "velocityY": 1.201579724368033, + "timestamp": 0.50182800464926 + }, + { + "x": 2.164876110933553, + "y": 6.083435356761985, + "heading": 0.22790661577104523, + "angularVelocity": 0.5303442971006865, + "velocityX": 2.834816983185756, + "velocityY": 1.093243587663212, + "timestamp": 0.5575866718325111 }, { "x": 2.3368515968322754, "y": 6.133185386657715, "heading": 0.2525545797921912, - "angularVelocity": 0.456860375599634, - "velocityX": 3.1911711078905185, - "velocityY": 0.9231598165882628, - "timestamp": 0.5891141473001633 - }, - { - "x": 2.5473350968821267, - "y": 6.194075230145087, - "heading": 0.25255484664471606, - "angularVelocity": 0.0000044758460677296985, - "velocityX": 3.530383481883881, - "velocityY": 1.0212890683184164, - "timestamp": 0.6487347242516318 - }, - { - "x": 2.7606743654688723, - "y": 6.255791206318425, - "heading": 0.25255482885764124, - "angularVelocity": -2.983378514405921e-7, - "velocityX": 3.578282524176262, - "velocityY": 1.0351455710261694, - "timestamp": 0.7083553012031003 - }, - { - "x": 2.9740136340556216, - "y": 6.317507182491763, - "heading": 0.25255481107057365, - "angularVelocity": -2.9833773077503677e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.7679758781545688 - }, - { - "x": 3.187352902642371, - "y": 6.379223158665101, - "heading": 0.25255479328350594, - "angularVelocity": -2.9833773187848623e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.8275964551060373 - }, - { - "x": 3.40069217122912, - "y": 6.44093913483844, - "heading": 0.2525547754964383, - "angularVelocity": -2.983377307846883e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 0.8872170320575058 - }, - { - "x": 3.614031439815869, - "y": 6.502655111011779, - "heading": 0.25255475770937064, - "angularVelocity": -2.9833773102796393e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.9468376090089743 - }, - { - "x": 3.827370708402618, - "y": 6.564371087185117, - "heading": 0.25255473992230293, - "angularVelocity": -2.983377313896792e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.006458185960443 - }, - { - "x": 4.040709976989367, - "y": 6.626087063358456, - "heading": 0.2525547221352353, - "angularVelocity": -2.9833773109308854e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261857, - "timestamp": 1.0660787629119115 - }, - { - "x": 4.254049245576117, - "y": 6.6878030395317944, - "heading": 0.2525547043481677, - "angularVelocity": -2.983377300262124e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.12569933986338 - }, - { - "x": 4.467388514162867, - "y": 6.749519015705133, - "heading": 0.2525546865611001, - "angularVelocity": -2.9833773003639016e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261857, - "timestamp": 1.1853199168148487 - }, - { - "x": 4.680727782749616, - "y": 6.811234991878472, - "heading": 0.2525546687740325, - "angularVelocity": -2.9833772985092435e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.2449404937663173 - }, - { - "x": 4.894067051336365, - "y": 6.872950968051811, - "heading": 0.25255465098696483, - "angularVelocity": -2.9833773088728923e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.304561070717786 - }, - { - "x": 5.107406319923115, - "y": 6.934666944225149, - "heading": 0.2525546331998971, - "angularVelocity": -2.9833773138995595e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.3641816476692545 - }, - { - "x": 5.3207455885098645, - "y": 6.996382920398487, - "heading": 0.2525546154128296, - "angularVelocity": -2.9833772971803955e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.4238022246207231 - }, - { - "x": 5.534084857096614, - "y": 7.058098896571825, - "heading": 0.252554597625762, - "angularVelocity": -2.983377299147757e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.4834228015721918 + "angularVelocity": 0.44204722362068527, + "velocityX": 3.0842825803838596, + "velocityY": 0.8922385058492518, + "timestamp": 0.6133453390157622 + }, + { + "x": 2.5269005159766844, + "y": 6.188163800352406, + "heading": 0.252554892201178, + "angularVelocity": 0.000005553817937610827, + "velocityX": 3.378574691383036, + "velocityY": 0.9773729727982717, + "timestamp": 0.6695965504005877 + }, + { + "x": 2.7281832415838734, + "y": 6.246391992377104, + "heading": 0.2525548726806658, + "angularVelocity": -3.47023853848989e-7, + "velocityX": 3.578282505423293, + "velocityY": 1.0351455656012056, + "timestamp": 0.7258477617854133 + }, + { + "x": 2.929465967191096, + "y": 6.304620184401812, + "heading": 0.2525548531602303, + "angularVelocity": -3.4702249119339613e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 0.7820989731702389 + }, + { + "x": 3.1307486927983192, + "y": 6.36284837642652, + "heading": 0.25255483363979486, + "angularVelocity": -3.4702249016578237e-7, + "velocityX": 3.5782825054238923, + "velocityY": 1.0351455656013788, + "timestamp": 0.8383501845550645 + }, + { + "x": 3.332031418405542, + "y": 6.421076568451228, + "heading": 0.2525548141193594, + "angularVelocity": -3.470224908197631e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 0.8946013959398901 + }, + { + "x": 3.5333141440127647, + "y": 6.4793047604759355, + "heading": 0.2525547945989239, + "angularVelocity": -3.4702249080835237e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 0.9508526073247157 + }, + { + "x": 3.7345968696199874, + "y": 6.537532952500643, + "heading": 0.2525547750784885, + "angularVelocity": -3.470224894869701e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 1.0071038187095414 + }, + { + "x": 3.93587959522721, + "y": 6.595761144525352, + "heading": 0.25255475555805307, + "angularVelocity": -3.470224898418443e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 1.063355030094367 + }, + { + "x": 4.137162320834433, + "y": 6.65398933655006, + "heading": 0.2525547360376176, + "angularVelocity": -3.470224909887922e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.1196062414791927 + }, + { + "x": 4.338445046441655, + "y": 6.712217528574768, + "heading": 0.2525547165171822, + "angularVelocity": -3.4702248928646837e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 1.1758574528640184 + }, + { + "x": 4.539727772048877, + "y": 6.770445720599476, + "heading": 0.25255469699674676, + "angularVelocity": -3.470224898754089e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 1.2321086642488441 + }, + { + "x": 4.7410104976561, + "y": 6.8286739126241836, + "heading": 0.25255467747631133, + "angularVelocity": -3.470224907627516e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.2883598756336698 + }, + { + "x": 4.942293223263322, + "y": 6.886902104648891, + "heading": 0.25255465795587584, + "angularVelocity": -3.4702249046766804e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 1.3446110870184955 + }, + { + "x": 5.143575948870544, + "y": 6.945130296673599, + "heading": 0.2525546384354404, + "angularVelocity": -3.47022489425383e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.4008622984033212 + }, + { + "x": 5.344858674477766, + "y": 7.003358488698307, + "heading": 0.25255461891500497, + "angularVelocity": -3.470224896167856e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.457113509788147 + }, + { + "x": 5.546141400084989, + "y": 7.061586680723015, + "heading": 0.25255459939456953, + "angularVelocity": -3.470224897168836e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 1.5133647211729726 }, { "x": 5.747424125671387, "y": 7.119814872741699, "heading": 0.2525545797921912, - "angularVelocity": -2.9911771592583556e-7, - "velocityX": 3.5782825239754317, - "velocityY": 1.035145570968072, - "timestamp": 1.5430433785236604 - }, - { - "x": 5.874238789453484, - "y": 7.151633771807316, - "heading": 0.23428972710167786, - "angularVelocity": -0.5023194013498574, - "velocityX": 3.4876528747747586, - "velocityY": 0.8750823563200211, - "timestamp": 1.579404412243304 - }, - { - "x": 6.000467771954814, - "y": 7.182688874971163, - "heading": 0.21343765731660022, - "angularVelocity": -0.5734729641036735, - "velocityX": 3.471545486704275, - "velocityY": 0.8540764655727967, - "timestamp": 1.6157654459629476 - }, - { - "x": 6.126526412325093, - "y": 7.213769689844044, - "heading": 0.19236113767335114, - "angularVelocity": -0.5796457770083356, - "velocityX": 3.4668607428005407, - "velocityY": 0.8547835881819195, - "timestamp": 1.6521264796825912 - }, - { - "x": 6.252399106037461, - "y": 7.244861395063954, - "heading": 0.17099373366442527, - "angularVelocity": -0.5876456696384406, - "velocityX": 3.4617468436923633, - "velocityY": 0.8550830941616943, - "timestamp": 1.6884875134022348 - }, - { - "x": 6.378079158099059, - "y": 7.275966592245878, - "heading": 0.1493195497494227, - "angularVelocity": -0.596082720918173, - "velocityX": 3.456448819102211, - "velocityY": 0.8554541496744962, - "timestamp": 1.7248485471218784 - }, - { - "x": 6.503558906617952, - "y": 7.3070873984764315, - "heading": 0.12731920059935156, - "angularVelocity": -0.6050529069030769, - "velocityX": 3.4509400774022723, - "velocityY": 0.8558834292365232, - "timestamp": 1.761209580841522 - }, - { - "x": 6.628826049826062, - "y": 7.3382208951944605, - "heading": 0.10495362803069447, - "angularVelocity": -0.6150972698164627, - "velocityX": 3.44509301286545, - "velocityY": 0.8562324426219444, - "timestamp": 1.7975706145611656 - }, - { - "x": 6.7538656809780315, - "y": 7.369363297624379, - "heading": 0.08217584529224368, - "angularVelocity": -0.6264338608763309, - "velocityX": 3.438835983489081, - "velocityY": 0.8564773672288275, - "timestamp": 1.8339316482808092 - }, - { - "x": 6.8786958169095165, - "y": 7.400552514729427, - "heading": 0.05908376270904082, - "angularVelocity": -0.6350777252718116, - "velocityX": 3.4330744525573853, - "velocityY": 0.8577648629444327, - "timestamp": 1.8702926820004528 - }, - { - "x": 7.003224052672837, - "y": 7.431696922152255, - "heading": 0.03530389927777457, - "angularVelocity": -0.6539930524147748, - "velocityX": 3.4247716036754445, - "velocityY": 0.8565325084804077, - "timestamp": 1.9066537157200965 + "angularVelocity": -3.4847922161224964e-7, + "velocityX": 3.5782825050536915, + "velocityY": 1.0351455654942852, + "timestamp": 1.5696159325577983 + }, + { + "x": 5.864122316952898, + "y": 7.149764367678907, + "heading": 0.23825610881476394, + "angularVelocity": -0.4289921260648423, + "velocityX": 3.501255852098466, + "velocityY": 0.8985644358731831, + "timestamp": 1.6029463134871351 + }, + { + "x": 5.980010019619152, + "y": 7.178464848632163, + "heading": 0.21993450293637717, + "angularVelocity": -0.5496968641681629, + "velocityX": 3.4769390398491176, + "velocityY": 0.8610906972261488, + "timestamp": 1.636276694416472 + }, + { + "x": 6.095759168980239, + "y": 7.207171809244793, + "heading": 0.2014022591938407, + "angularVelocity": -0.5560165598414963, + "velocityX": 3.4727820725027265, + "velocityY": 0.8612851042264353, + "timestamp": 1.6696070753458088 + }, + { + "x": 6.211373074459108, + "y": 7.235898623678772, + "heading": 0.18268720108591044, + "angularVelocity": -0.5615014766140173, + "velocityX": 3.4687243966392267, + "velocityY": 0.8618807716264242, + "timestamp": 1.7029374562751456 + }, + { + "x": 6.326838442963843, + "y": 7.264631799858999, + "heading": 0.16373191419922642, + "angularVelocity": -0.5687089783603383, + "velocityX": 3.464267892693179, + "velocityY": 0.8620716409195424, + "timestamp": 1.7362678372044824 + }, + { + "x": 6.442151068511899, + "y": 7.29337427879422, + "heading": 0.1445283357059787, + "angularVelocity": -0.5761583863671085, + "velocityX": 3.4596851980938594, + "velocityY": 0.8623507482904867, + "timestamp": 1.7695982181338192 + }, + { + "x": 6.557303014616615, + "y": 7.322124225476267, + "heading": 0.1250513432276187, + "angularVelocity": -0.5843615324905178, + "velocityX": 3.454864387804252, + "velocityY": 0.8625748005400329, + "timestamp": 1.802928599063156 + }, + { + "x": 6.672285593632433, + "y": 7.350880202549992, + "heading": 0.10527449948643015, + "angularVelocity": -0.5933578671998123, + "velocityX": 3.449782925061388, + "velocityY": 0.8627557283156009, + "timestamp": 1.8362589799924929 + }, + { + "x": 6.787113803468559, + "y": 7.379671810584065, + "heading": 0.08527801395894485, + "angularVelocity": -0.5999477044645701, + "velocityX": 3.4451514394501603, + "velocityY": 0.8638247518116766, + "timestamp": 1.8695893609218297 + }, + { + "x": 6.901739724666763, + "y": 7.408448228453197, + "heading": 0.0648615716893241, + "angularVelocity": -0.6125475227212472, + "velocityX": 3.4390822427508825, + "velocityY": 0.8633690064971079, + "timestamp": 1.9029197418511665 + }, + { + "x": 7.014861438150348, + "y": 7.4357314738869205, + "heading": 0.038575820671149744, + "angularVelocity": -0.7886423822728691, + "velocityX": 3.3939520140322568, + "velocityY": 0.8185698654799702, + "timestamp": 1.9362501227805033 }, { "x": 7.125290870666504, "y": 7.458, - "heading": -8.902558475497447e-21, - "angularVelocity": -0.9709267219953129, - "velocityX": 3.3570777699788383, - "velocityY": 0.7233864155389959, - "timestamp": 1.94301474943974 - }, - { - "x": 7.338537269448379, - "y": 7.482023887493991, - "heading": -0.008039547554639858, - "angularVelocity": -0.1278866817794853, - "velocityX": 3.39215287381437, - "velocityY": 0.38215275600640763, - "timestamp": 2.0058793686696177 - }, - { - "x": 7.527981936001141, - "y": 7.500234795903286, - "heading": -0.013861641566031323, - "angularVelocity": -0.09261320728121766, - "velocityX": 3.013533985786477, - "velocityY": 0.2896845416768293, - "timestamp": 2.068743987899495 - }, - { - "x": 7.693596991003447, - "y": 7.512743640283509, - "heading": -0.017657163586421762, - "angularVelocity": -0.060376123595234456, - "velocityX": 2.6344716158496166, - "velocityY": 0.1989806751947781, - "timestamp": 2.1316086071293725 - }, - { - "x": 7.835373269936495, - "y": 7.519587451824941, - "heading": -0.01948976746028864, - "angularVelocity": -0.02915159427221823, - "velocityX": 2.255263464089624, - "velocityY": 0.10886587122090353, - "timestamp": 2.19447322635925 - }, - { - "x": 7.953306214016821, - "y": 7.520784758027623, - "heading": -0.01939144141260055, - "angularVelocity": 0.0015640919947123182, - "velocityX": 1.875982794854424, - "velocityY": 0.01904578787480972, - "timestamp": 2.2573378455891273 - }, - { - "x": 8.04739309462225, - "y": 7.516346680416715, - "heading": -0.01738162754383806, - "angularVelocity": 0.031970508902840686, - "velocityX": 1.4966587208836901, - "velocityY": -0.07059738315886452, - "timestamp": 2.3202024648190047 - }, - { - "x": 8.117632094556283, - "y": 7.5062806367543455, - "heading": -0.0134735913597302, - "angularVelocity": 0.06216590877322135, - "velocityX": 1.1173057404068745, - "velocityY": -0.16012255837517314, - "timestamp": 2.383067084048882 - }, - { - "x": 8.164021915899584, - "y": 7.490591928437994, - "heading": -0.007677144931297963, - "angularVelocity": 0.09220522607854079, - "velocityX": 0.7379321136689944, - "velocityY": -0.24956340320748793, - "timestamp": 2.4459317032787595 + "heading": 1.0084355524659913e-21, + "angularVelocity": -1.1573771314805474, + "velocityX": 3.313176430544709, + "velocityY": 0.668114959750724, + "timestamp": 1.9695805037098402 + }, + { + "x": 7.337738270409439, + "y": 7.481917447620179, + "heading": -0.00819620478546554, + "angularVelocity": -0.12615227280600286, + "velocityX": 3.2698941803922152, + "velocityY": 0.368126523919295, + "timestamp": 2.0345512299849795 + }, + { + "x": 7.526586631074925, + "y": 7.500039280530163, + "heading": -0.014041404226933134, + "angularVelocity": -0.08996666308937673, + "velocityX": 2.906668456586843, + "velocityY": 0.27892304656162725, + "timestamp": 2.099521956260119 + }, + { + "x": 7.6918053440422804, + "y": 7.512484823370471, + "heading": -0.017813194168849914, + "angularVelocity": -0.058053682914731695, + "velocityX": 2.5429716187515523, + "velocityY": 0.19155616004049963, + "timestamp": 2.164492682535258 + }, + { + "x": 7.833384352673449, + "y": 7.519294018241946, + "heading": -0.01960436856302971, + "angularVelocity": -0.027568945229186584, + "velocityX": 2.179119993696969, + "velocityY": 0.10480404424970338, + "timestamp": 2.2294634088103975 + }, + { + "x": 7.951318655599417, + "y": 7.520486868366768, + "heading": -0.019461534286698053, + "angularVelocity": 0.0021984405057560424, + "velocityX": 1.815191389835117, + "velocityY": 0.018359808997220107, + "timestamp": 2.294434135085537 + }, + { + "x": 8.045605259786509, + "y": 7.516075387181805, + "heading": -0.017412919986794666, + "angularVelocity": 0.031531343688969565, + "velocityX": 1.4512167185542217, + "velocityY": -0.06789952087470152, + "timestamp": 2.359404861360676 + }, + { + "x": 8.116242172260733, + "y": 7.5060675897274995, + "heading": -0.013477652900012242, + "angularVelocity": 0.06056984910584597, + "velocityX": 1.0872113723200465, + "velocityY": -0.15403548687333402, + "timestamp": 2.4243755876358155 + }, + { + "x": 8.163227969814436, + "y": 7.490469205298236, + "heading": -0.007669730702440132, + "angularVelocity": 0.08939290863052056, + "velocityX": 0.723184120718108, + "velocityY": -0.24008327016705255, + "timestamp": 2.489346313910955 }, { "x": 8.186561584472656, "y": 7.469284534454346, - "heading": 2.2979832887228624e-21, - "angularVelocity": 0.12212187117884055, - "velocityX": 0.358542990464167, - "velocityY": -0.33894095350730474, - "timestamp": 2.508796322508637 - }, - { - "x": 8.169433855540824, - "y": 7.426853893988931, - "heading": 0.014976899045789983, - "angularVelocity": 0.16512821061392627, - "velocityX": -0.18884224442902273, - "velocityY": -0.46782018853404506, - "timestamp": 2.599494931729361 - }, - { - "x": 8.102655131439361, - "y": 7.372743106744783, - "heading": 0.03346799984598468, - "angularVelocity": 0.20387413830342968, - "velocityX": -0.7362706515041357, - "velocityY": -0.5965999667366941, - "timestamp": 2.6901935409500854 - }, - { - "x": 7.986220220035368, - "y": 7.306964887600374, - "heading": 0.05493172658414638, - "angularVelocity": 0.23664890699621968, - "velocityX": -1.2837563045827718, - "velocityY": -0.7252395566985038, - "timestamp": 2.7808921501708097 - }, - { - "x": 7.820121969665864, - "y": 7.229538517591083, - "heading": 0.07855370015299508, - "angularVelocity": 0.26044471653762835, - "velocityX": -1.8313208085174242, - "velocityY": -0.8536665630766948, - "timestamp": 2.871590759391534 - }, - { - "x": 7.6043501738014525, - "y": 7.14049675145262, - "heading": 0.10296815130417637, - "angularVelocity": 0.2691821998258671, - "velocityX": -2.3789978448215092, - "velocityY": -0.9817324312192064, - "timestamp": 2.9622893686122582 - }, - { - "x": 7.338891049230189, - "y": 7.039907645727491, - "heading": 0.1254011174832251, - "angularVelocity": 0.24733528299707236, - "velocityX": -2.9268268483063693, - "velocityY": -1.1090479400884177, - "timestamp": 3.0529879778329825 - }, - { - "x": 7.023762471374015, - "y": 6.927998022527093, - "heading": 0.1370927000479183, - "angularVelocity": 0.12890586377394736, - "velocityX": -3.4744587658370385, - "velocityY": -1.2338626155562706, - "timestamp": 3.1436865870537067 - }, - { - "x": 6.694988070852044, - "y": 6.85020567874946, - "heading": 0.13709270182779565, - "angularVelocity": 1.9624086279332165e-8, - "velocityX": -3.624911157368082, - "velocityY": -0.8577016168827462, - "timestamp": 3.234385196274431 - }, - { - "x": 6.366213601226777, - "y": 6.7724136270242035, - "heading": 0.13709270360766948, - "angularVelocity": 1.962404782489554e-8, - "velocityX": -3.624911919268354, - "velocityY": -0.8576983968512897, - "timestamp": 3.3250838054951553 - }, - { - "x": 6.0374391323544945, - "y": 6.69462157211659, - "heading": 0.13709270538754328, - "angularVelocity": 1.962404757872295e-8, - "velocityX": -3.6249119109663046, - "velocityY": -0.857698431938445, - "timestamp": 3.4157824147158795 - }, - { - "x": 5.708664663238629, - "y": 6.616829518238441, - "heading": 0.13709270716741703, - "angularVelocity": 1.9624046615308717e-8, - "velocityX": -3.6249119136519434, - "velocityY": -0.8576984205880644, - "timestamp": 3.5064810239366038 - }, - { - "x": 5.3798901940960535, - "y": 6.539037464473215, - "heading": 0.13709270894730027, - "angularVelocity": 1.9624151634826213e-8, - "velocityX": -3.6249119139464265, - "velocityY": -0.8576984193430164, - "timestamp": 3.597179633157328 - }, - { - "x": 5.082161936855116, - "y": 6.468591654596992, - "heading": 0.17334127091967155, - "angularVelocity": 0.3996595127953577, - "velocityX": -3.2826110543369467, - "velocityY": -0.7767022061472455, - "timestamp": 3.6878782423780523 - }, - { - "x": 4.834055003719788, - "y": 6.409886994261807, - "heading": 0.20354833113507975, - "angularVelocity": 0.33304876971041714, - "velocityX": -2.735509786390822, - "velocityY": -0.647249840318068, - "timestamp": 3.7785768515987765 - }, - { - "x": 4.635569429511693, - "y": 6.362923375586849, - "heading": 0.22771434771261193, - "angularVelocity": 0.2664430776300183, - "velocityX": -2.1884081345179167, - "velocityY": -0.5177986639317362, - "timestamp": 3.869275460819501 - }, - { - "x": 4.486705235532264, - "y": 6.327700716475024, - "heading": 0.24583917728696456, - "angularVelocity": 0.19983580487153885, - "velocityX": -1.6413062477854834, - "velocityY": -0.38834839270916643, - "timestamp": 3.959974070040225 - }, - { - "x": 4.387462434805768, - "y": 6.304218961949016, - "heading": 0.25792250356094953, - "angularVelocity": 0.13322504476974903, - "velocityX": -1.0942042174536384, - "velocityY": -0.25889872764050514, - "timestamp": 4.050672679260949 + "heading": 7.422813602625121e-23, + "angularVelocity": 0.11804902210820577, + "velocityX": 0.3591404313291331, + "velocityY": -0.3260648611834275, + "timestamp": 2.554317040186094 + }, + { + "x": 8.171272340508109, + "y": 7.427332842662171, + "heading": 0.014840203267270782, + "angularVelocity": 0.15900903302410063, + "velocityX": -0.16382039077820704, + "velocityY": -0.44950179087577585, + "timestamp": 2.6476463497544476 + }, + { + "x": 8.107175512886487, + "y": 7.3738609015025816, + "heading": 0.03350091514476021, + "angularVelocity": 0.1999448186619506, + "velocityX": -0.6867813328746101, + "velocityY": -0.572938355666582, + "timestamp": 2.7409756593228005 + }, + { + "x": 7.994271087246123, + "y": 7.308868753063173, + "heading": 0.05597944584590409, + "angularVelocity": 0.24085178391554368, + "velocityX": -1.2097424288526986, + "velocityY": -0.6963744694994183, + "timestamp": 2.8343049688911535 + }, + { + "x": 7.832559046421551, + "y": 7.232356448582914, + "heading": 0.08227288434163177, + "angularVelocity": 0.2817275582272545, + "velocityX": -1.732703708754388, + "velocityY": -0.8198100343196232, + "timestamp": 2.9276342784595064 + }, + { + "x": 7.6220393708156715, + "y": 7.144324050552095, + "heading": 0.1123783464235267, + "angularVelocity": 0.3225724289736236, + "velocityX": -2.2556651986340626, + "velocityY": -0.9432449295721523, + "timestamp": 3.0209635880278594 + }, + { + "x": 7.362712038546165, + "y": 7.04477163823656, + "heading": 0.14629325961596634, + "angularVelocity": 0.36338973629287186, + "velocityX": -2.778626922977279, + "velocityY": -1.0666789755111628, + "timestamp": 3.1142928975962123 + }, + { + "x": 7.054577023664908, + "y": 6.933699333854541, + "heading": 0.18401565672002676, + "angularVelocity": 0.4041859655720804, + "velocityX": -3.3015889253480717, + "velocityY": -1.1901117119126619, + "timestamp": 3.2076222071645653 + }, + { + "x": 6.71622230121249, + "y": 6.85383794706396, + "heading": 0.18401565782506035, + "angularVelocity": 1.1840156036202986e-8, + "velocityX": -3.6253854659088836, + "velocityY": -0.8556946061203927, + "timestamp": 3.300951516732918 + }, + { + "x": 6.377867419030211, + "y": 6.7739772370157985, + "heading": 0.1840156589300666, + "angularVelocity": 1.1839863049890439e-8, + "velocityX": -3.6253871773740243, + "velocityY": -0.8556873549961612, + "timestamp": 3.394280826301271 + }, + { + "x": 6.0395125368493074, + "y": 6.694116526961809, + "heading": 0.18401566003507294, + "angularVelocity": 1.1839864253851599e-8, + "velocityX": -3.6253871773592854, + "velocityY": -0.8556873550586043, + "timestamp": 3.487610135869624 + }, + { + "x": 5.701157654668755, + "y": 6.614255816906336, + "heading": 0.18401566114007933, + "angularVelocity": 1.18398644204111e-8, + "velocityX": -3.625387177355538, + "velocityY": -0.855687355074483, + "timestamp": 3.580939445437977 + }, + { + "x": 5.362802772497576, + "y": 6.534395106811441, + "heading": 0.18401566224512567, + "angularVelocity": 1.1840292694239514e-8, + "velocityX": -3.6253871772550927, + "velocityY": -0.8556873554969, + "timestamp": 3.67426875500633 + }, + { + "x": 5.069956587719004, + "y": 6.4652758877562615, + "heading": 0.20685757815363412, + "angularVelocity": 0.24474536471074324, + "velocityX": -3.137772968995303, + "velocityY": -0.7405949896646158, + "timestamp": 3.767598064574683 + }, + { + "x": 4.8259180814209035, + "y": 6.407676584765678, + "heading": 0.22589278292875978, + "angularVelocity": 0.20395741555534105, + "velocityX": -2.614811010890098, + "velocityY": -0.6171619961293916, + "timestamp": 3.860927374143036 + }, + { + "x": 4.6306872671740225, + "y": 6.361597170610555, + "heading": 0.2411211921270372, + "angularVelocity": 0.16316856160951593, + "velocityX": -2.0918489073777633, + "velocityY": -0.49372929434751617, + "timestamp": 3.954256683711389 + }, + { + "x": 4.484264152022265, + "y": 6.3270376241371595, + "heading": 0.25254264332581844, + "angularVelocity": 0.12237796734600637, + "velocityX": -1.5688867283917882, + "velocityY": -0.37029681922252144, + "timestamp": 4.047585993279742 + }, + { + "x": 4.386648740184571, + "y": 6.303997931141046, + "heading": 0.26015698140700544, + "angularVelocity": 0.08158571103122086, + "velocityX": -1.0459245042009158, + "velocityY": -0.24686449629458126, + "timestamp": 4.140915302848095 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": 0.06661212070370816, - "velocityX": -0.54710211431646, - "velocityY": -0.12944936516318697, - "timestamp": 4.141371288481674 + "angularVelocity": 0.04079263927507296, + "velocityX": -0.5229622556382164, + "velocityY": -0.12343224899141036, + "timestamp": 4.234244612416448 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": 4.2322185448004084e-23, - "velocityX": 1.750098119560174e-21, - "velocityY": 1.2842584640589406e-21, - "timestamp": 4.232069897702398 + "angularVelocity": -3.1350638097986163e-24, + "velocityX": 6.734301517773745e-23, + "velocityY": -5.476052050130636e-23, + "timestamp": 4.327573921984801 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePDEABC.2.traj b/src/main/deploy/choreo/CenterLanePDEABC.2.traj index 99522444..e6b4b62c 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.2.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.2.traj @@ -4,703 +4,730 @@ "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": 4.2322185448004084e-23, - "velocityX": 1.750098119560174e-21, - "velocityY": 1.2842584640589406e-21, + "angularVelocity": -3.1350638097986163e-24, + "velocityX": 6.734301517773745e-23, + "velocityY": -5.476052050130636e-23, "timestamp": 0 }, { - "x": 4.357336303204376, - "y": 6.297583237177048, - "heading": 0.24752122945545674, - "angularVelocity": -0.28561960466886005, - "velocityX": 0.3386404361144792, - "velocityY": 0.08867849340285801, - "timestamp": 0.057569230339162836 - }, - { - "x": 4.396392552566799, - "y": 6.3076902844788645, - "heading": 0.21545182139905172, - "angularVelocity": -0.5570581344838518, - "velocityX": 0.6784222949017583, - "velocityY": 0.17556335636714013, - "timestamp": 0.11513846067832567 - }, - { - "x": 4.455088009056865, - "y": 6.322669909558741, - "heading": 0.16876627146696008, - "angularVelocity": -0.8109462234782202, - "velocityX": 1.0195629878021513, - "velocityY": 0.26020193411697123, - "timestamp": 0.1727076910174885 - }, - { - "x": 4.533516711715102, - "y": 6.342355579726992, - "heading": 0.10873908045256343, - "angularVelocity": -1.042695736259688, - "velocityX": 1.3623371755394689, - "velocityY": 0.3419477740500502, - "timestamp": 0.23027692135665134 - }, - { - "x": 4.631792826161861, - "y": 6.366523725829109, - "heading": 0.03701265289139425, - "angularVelocity": -1.2459160412359371, - "velocityX": 1.7070944646606674, - "velocityY": 0.4198101305112548, - "timestamp": 0.2878461516958142 - }, - { - "x": 4.750056312234907, - "y": 6.394856813133717, - "heading": -0.04421826686802747, - "angularVelocity": -1.4110127802796573, - "velocityX": 2.054282910789491, - "velocityY": 0.492156784756161, - "timestamp": 0.345415382034977 - }, - { - "x": 4.888478272202705, - "y": 6.42687003155547, - "heading": -0.13181310680866934, - "angularVelocity": -1.5215565576365515, - "velocityX": 2.4044434701020125, - "velocityY": 0.5560820985993948, - "timestamp": 0.40298461237413985 - }, - { - "x": 5.047249483224003, - "y": 6.461748830996926, - "heading": -0.22073093757153267, - "angularVelocity": -1.5445374245758388, - "velocityX": 2.757917903121444, - "velocityY": 0.6058583593348625, - "timestamp": 0.4605538427133027 - }, - { - "x": 5.226429045018527, - "y": 6.497908973529479, - "heading": -0.30132047165929504, - "angularVelocity": -1.399871660833024, - "velocityX": 3.1124189213388544, - "velocityY": 0.6281157889295987, - "timestamp": 0.5181230730524655 - }, - { - "x": 5.423903541491299, - "y": 6.531166091125145, - "heading": -0.3435649109260108, - "angularVelocity": -0.7338023978753491, - "velocityX": 3.430209077129816, - "velocityY": 0.5776891127384427, - "timestamp": 0.5756923033916284 - }, - { - "x": 5.636616267297259, - "y": 6.55438559892435, - "heading": -0.3449872384479477, - "angularVelocity": -0.024706384184006707, - "velocityX": 3.6949030680588684, - "velocityY": 0.40333191294048215, - "timestamp": 0.6332615337307912 + "x": 4.357085465154312, + "y": 6.298335605731047, + "heading": 0.24955697801054222, + "angularVelocity": -0.24189458757929325, + "velocityX": 0.32311199814604374, + "velocityY": 0.09834717102755294, + "timestamp": 0.059559630497123095 + }, + { + "x": 4.395672489468794, + "y": 6.30982325311387, + "heading": 0.2214506254853789, + "angularVelocity": -0.4719027349661102, + "velocityX": 0.6478721239942183, + "velocityY": 0.19287640448638915, + "timestamp": 0.11911926099424619 + }, + { + "x": 4.453716686931878, + "y": 6.3266599763174565, + "heading": 0.18050722178606005, + "angularVelocity": -0.6874354887291741, + "velocityX": 0.9745560370104606, + "velocityY": 0.28268683104740583, + "timestamp": 0.17867889149136928 + }, + { + "x": 4.531352614071991, + "y": 6.348490007100106, + "heading": 0.12779504929109745, + "angularVelocity": -0.8850318924914878, + "velocityX": 1.3034991401409508, + "velocityY": 0.366523945841208, + "timestamp": 0.23823852198849238 + }, + { + "x": 4.628738799037524, + "y": 6.374849024990675, + "heading": 0.0646674309135714, + "angularVelocity": -1.0599061453306917, + "velocityX": 1.6351039143911004, + "velocityY": 0.4425651682281852, + "timestamp": 0.2977981524856155 + }, + { + "x": 4.746060953896417, + "y": 6.4051070852453105, + "heading": -0.0071047467810986895, + "angularVelocity": -1.2050473969635673, + "velocityX": 1.9698267749421239, + "velocityY": 0.5080296838996932, + "timestamp": 0.35735778298273857 + }, + { + "x": 4.883529200739737, + "y": 6.4383664128247835, + "heading": -0.08508741675594196, + "angularVelocity": -1.30932091626408, + "velocityX": 2.3080775635429776, + "velocityY": 0.5584206500589359, + "timestamp": 0.41691741347986166 + }, + { + "x": 5.041348217191184, + "y": 6.473264113474332, + "heading": -0.1656919745439482, + "angularVelocity": -1.3533421398895296, + "velocityX": 2.6497648681529427, + "velocityY": 0.5859287634639322, + "timestamp": 0.47647704397698476 + }, + { + "x": 5.219569793721533, + "y": 6.507557049328585, + "heading": -0.2431024824486723, + "angularVelocity": -1.29971437462936, + "velocityX": 2.99232172937938, + "velocityY": 0.575774825465208, + "timestamp": 0.5360366744741079 + }, + { + "x": 5.417370624809519, + "y": 6.5372140981281, + "heading": -0.30621328049041796, + "angularVelocity": -1.0596237336427745, + "velocityX": 3.3210553765530904, + "velocityY": 0.49793876409201393, + "timestamp": 0.595596304971231 + }, + { + "x": 5.630089765221455, + "y": 6.556096742386712, + "heading": -0.3316159113808185, + "angularVelocity": -0.4265075299892522, + "velocityX": 3.5715322381358874, + "velocityY": 0.31703763272213936, + "timestamp": 0.655155935468354 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -0.3449872632321153, - "angularVelocity": -4.3051066415640403e-7, - "velocityX": 3.7246964993017384, - "velocityY": 0.047619187556732175, - "timestamp": 0.690830764069954 - }, - { - "x": 5.9707478293428995, - "y": 6.552244535149851, - "heading": -0.34498728566747394, - "angularVelocity": -6.975751659806049e-7, - "velocityX": 3.7219059383782973, - "velocityY": -0.151808826449267, - "timestamp": 0.7229926868363377 - }, - { - "x": 6.090018567999844, - "y": 6.540962083011693, - "heading": -0.3449873062012292, - "angularVelocity": -6.384492454517859e-7, - "velocityX": 3.7084455280643134, - "velocityY": -0.3508015431822894, - "timestamp": 0.7551546096027213 - }, - { - "x": 6.208514463680753, - "y": 6.523311991997161, - "heading": -0.3449873252853557, - "angularVelocity": -5.933764159352289e-7, - "velocityX": 3.6843535923407127, - "velocityY": -0.5487884273193968, - "timestamp": 0.7873165323691049 - }, - { - "x": 6.325895808833114, - "y": 6.499344869625537, - "heading": -0.34498734327239833, - "angularVelocity": -5.592651525230829e-7, - "velocityX": 3.6496992423305046, - "velocityY": -0.7452017886404377, - "timestamp": 0.8194784551354886 - }, - { - "x": 6.441826094112058, - "y": 6.46912943973789, - "heading": -0.34498736044751604, - "angularVelocity": -5.340202407325154e-7, - "velocityX": 3.6045819188434196, - "velocityY": -0.9394783423592116, - "timestamp": 0.8516403779018722 - }, - { - "x": 6.555972977983021, - "y": 6.432752355963861, - "heading": -0.3449873770504224, - "angularVelocity": -5.162286594877017e-7, - "velocityX": 3.5491312102233143, - "velocityY": -1.131060603505082, - "timestamp": 0.8838023006682558 - }, - { - "x": 6.66800926163723, - "y": 6.390318000635929, - "heading": -0.3449873932911425, - "angularVelocity": -5.049673254841841e-7, - "velocityX": 3.4835070175379053, - "velocityY": -1.319397339399292, - "timestamp": 0.9159642234346395 - }, - { - "x": 6.777614079975414, - "y": 6.34194872129089, - "heading": -0.344987409362088, - "angularVelocity": -4.996885790126847e-7, - "velocityX": 3.407906272716571, - "velocityY": -1.5039299639011887, - "timestamp": 0.9481261462010231 - }, - { - "x": 6.886436638977974, - "y": 6.291844289930693, - "heading": -0.3449874254080791, - "angularVelocity": -4.98912679463648e-7, - "velocityX": 3.3835837425834847, - "velocityY": -1.557880470149199, - "timestamp": 0.9802880689674067 - }, - { - "x": 6.995259181110964, - "y": 6.241739821931245, - "heading": -0.34498744145406995, - "angularVelocity": -4.989126736688575e-7, - "velocityX": 3.383583218063541, - "velocityY": -1.5578816093613586, - "timestamp": 1.0124499917337904 - }, - { - "x": 7.104082434838663, - "y": 6.191636899482441, - "heading": -0.34498745750007104, - "angularVelocity": -4.98912992332448e-7, - "velocityX": 3.3836053434418156, - "velocityY": -1.5578335540676167, - "timestamp": 1.044611914500174 + "heading": -0.3344741491569628, + "angularVelocity": -0.04798951491618805, + "velocityX": 3.7098016045994533, + "velocityY": 0.017297899702458933, + "timestamp": 0.7147155659654771 + }, + { + "x": 5.971113356144086, + "y": 6.5517128681904815, + "heading": -0.33447429817496127, + "angularVelocity": -0.000004618408354033321, + "velocityX": 3.7212182481092846, + "velocityY": -0.167796284707936, + "timestamp": 0.7469816600587373 + }, + { + "x": 6.090763237120761, + "y": 6.540318338833946, + "heading": -0.33447432111787584, + "angularVelocity": -7.11053358628561e-7, + "velocityX": 3.70822327086892, + "velocityY": -0.3531425069176937, + "timestamp": 0.7792477541519975 + }, + { + "x": 6.209696100342635, + "y": 6.522971764048271, + "heading": -0.3344743416762489, + "angularVelocity": -6.371509664508709e-7, + "velocityX": 3.686001251905956, + "velocityY": -0.537609998146539, + "timestamp": 0.8115138482452577 + }, + { + "x": 6.327616045254045, + "y": 6.499716303685921, + "heading": -0.3344743603957426, + "angularVelocity": -5.80159891848438e-7, + "velocityX": 3.6546085984432737, + "velocityY": -0.7207398669059114, + "timestamp": 0.8437799423385179 + }, + { + "x": 6.444229692072682, + "y": 6.470609819762047, + "heading": -0.3344743776866033, + "angularVelocity": -5.358832938629608e-7, + "velocityX": 3.6141234350083242, + "velocityY": -0.9020764595722645, + "timestamp": 0.8760460364317781 + }, + { + "x": 6.559246912194348, + "y": 6.435724733351891, + "heading": -0.3344743938681527, + "angularVelocity": -5.015031984210286e-7, + "velocityX": 3.5646465230413518, + "velocityY": -1.081168557598769, + "timestamp": 0.9083121305250383 + }, + { + "x": 6.67238155121755, + "y": 6.395147846497736, + "heading": -0.33447440919657534, + "angularVelocity": -4.750628499499889e-7, + "velocityX": 3.506301032167155, + "velocityY": -1.2575704619491082, + "timestamp": 0.9405782246182985 + }, + { + "x": 6.783352144572337, + "y": 6.348980132760865, + "heading": -0.33447442388388354, + "angularVelocity": -4.551932493526412e-7, + "velocityX": 3.439232310983837, + "velocityY": -1.4308429648605667, + "timestamp": 0.9728443187115587 + }, + { + "x": 6.891882636447486, + "y": 6.297336519628137, + "heading": -0.3344744381114049, + "angularVelocity": -4.409434049866837e-7, + "velocityX": 3.363607989286128, + "velocityY": -1.6005536022878626, + "timestamp": 1.005110412804819 + }, + { + "x": 6.998345027127988, + "y": 6.24155399695869, + "heading": -0.33447445218138366, + "angularVelocity": -4.360607978051808e-7, + "velocityX": 3.299512806625678, + "velocityY": -1.7288278682946643, + "timestamp": 1.0373765068980791 + }, + { + "x": 7.107456437455107, + "y": 6.19114935204841, + "heading": -0.33447450198371365, + "angularVelocity": -0.00000154348803003307, + "velocityX": 3.381611979799848, + "velocityY": -1.5621551454164793, + "timestamp": 1.0696426009913393 }, { "x": 7.214789390563965, "y": 6.145846366882324, "heading": -0.344987478573796, - "angularVelocity": -6.552383423892292e-7, - "velocityX": 3.442174665036408, - "velocityY": -1.4237498464482023, - "timestamp": 1.0767738372665576 - }, - { - "x": 7.41054811335349, - "y": 6.075273519082507, - "heading": -0.3626370912094401, - "angularVelocity": -0.2874236730044668, - "velocityX": 3.187927819628061, - "velocityY": -1.1492777517419182, - "timestamp": 1.1381800967132865 - }, - { - "x": 7.584442418775818, - "y": 6.012933970935389, - "heading": -0.3763799731717047, - "angularVelocity": -0.22380262347988858, - "velocityX": 2.831866115752982, - "velocityY": -1.0151985922737843, - "timestamp": 1.1995863561600153 - }, - { - "x": 7.73639717593505, - "y": 5.958610534020796, - "heading": -0.3857543878763611, - "angularVelocity": -0.15266220071243664, - "velocityX": 2.4745809063822897, - "velocityY": -0.8846563429208651, - "timestamp": 1.2609926156067441 - }, - { - "x": 7.866387295940067, - "y": 5.912228690600519, - "heading": -0.39060211670074774, - "angularVelocity": -0.07894519008427879, - "velocityX": 2.1168871248017687, - "velocityY": -0.7553276137999276, - "timestamp": 1.322398875053473 - }, - { - "x": 7.974400233400663, - "y": 5.873750785403185, - "heading": -0.39084333039715696, - "angularVelocity": -0.003928161372840292, - "velocityX": 1.7589890417327878, - "velocityY": -0.62661210019988, - "timestamp": 1.3838051345002018 - }, - { - "x": 8.060428467430407, - "y": 5.843154117743718, - "heading": -0.3864300742351692, - "angularVelocity": 0.0718698093932331, - "velocityX": 1.4009684811427807, - "velocityY": -0.49826626691061593, - "timestamp": 1.4452113939469307 - }, - { - "x": 8.124466994034947, - "y": 5.820423531988022, - "heading": -0.37733049959220943, - "angularVelocity": 0.14818643449294983, - "velocityX": 1.0428664305809843, - "velocityY": -0.3701672428918256, - "timestamp": 1.5066176533936595 - }, - { - "x": 8.16651225164957, - "y": 5.805548215416436, - "heading": -0.3635219686266917, - "angularVelocity": 0.22487171649816598, - "velocityX": 0.6847063799920386, - "velocityY": -0.24224430384806347, - "timestamp": 1.5680239128403883 + "angularVelocity": -0.325821171899398, + "velocityX": 3.32649352594794, + "velocityY": -1.404043050117709, + "timestamp": 1.1019086950845995 + }, + { + "x": 7.408281473848404, + "y": 6.071835314929934, + "heading": -0.36045376555379516, + "angularVelocity": -0.24376616005736862, + "velocityX": 3.0496538829742375, + "velocityY": -1.1664978129250685, + "timestamp": 1.1653559224452765 + }, + { + "x": 7.581043516745906, + "y": 6.008144867615469, + "heading": -0.3717285153471776, + "angularVelocity": -0.17770279746487722, + "velocityX": 2.722925021063643, + "velocityY": -1.003833421315721, + "timestamp": 1.2288031498059535 + }, + { + "x": 7.732516109743219, + "y": 5.95357268553042, + "heading": -0.3790773339263405, + "angularVelocity": -0.11582568513809473, + "velocityX": 2.3873792330788404, + "velocityY": -0.8601192574550642, + "timestamp": 1.2922503771666305 + }, + { + "x": 7.862474679773266, + "y": 5.907578864525407, + "heading": -0.3826203402067485, + "angularVelocity": -0.05584178265611413, + "velocityX": 2.048293919784284, + "velocityY": -0.7249145930925164, + "timestamp": 1.3556976045273075 + }, + { + "x": 7.970798961808127, + "y": 5.869857366205246, + "heading": -0.38242588059176913, + "angularVelocity": 0.003064903275819629, + "velocityX": 1.7073130937475338, + "velocityY": -0.5945334396683074, + "timestamp": 1.4191448318879845 + }, + { + "x": 8.057414200976034, + "y": 5.840211247118204, + "heading": -0.378538126264348, + "angularVelocity": 0.061275401450096946, + "velocityX": 1.3651540464570049, + "velocityY": -0.46725633759397367, + "timestamp": 1.4825920592486614 + }, + { + "x": 8.122269484577586, + "y": 5.818503192947792, + "heading": -0.37098804864933244, + "angularVelocity": 0.11899775497037605, + "velocityX": 1.0221925574914426, + "velocityY": -0.3421434643157637, + "timestamp": 1.5460392866093384 + }, + { + "x": 8.165327926639057, + "y": 5.804632020140207, + "heading": -0.3597986070827489, + "angularVelocity": 0.1763582434103087, + "velocityX": 0.6786497038979674, + "velocityY": -0.2186253581851867, + "timestamp": 1.6094865139700154 }, { "x": 8.186561584472656, "y": 5.798520088195801, "heading": -0.344987478573796, - "angularVelocity": 0.3018338882695626, - "velocityX": 0.32650307971422843, - "velocityY": -0.11445294476423894, - "timestamp": 1.6294301722871172 - }, - { - "x": 8.182873431203555, - "y": 5.799961031790675, - "heading": -0.31977664522832494, - "angularVelocity": 0.3842917157740494, - "velocityX": -0.056218956684159486, - "velocityY": 0.021964473717296987, - "timestamp": 1.6950335467826898 - }, - { - "x": 8.154073485915251, - "y": 5.810352807071496, - "heading": -0.2894961331766682, - "angularVelocity": 0.46156942816562346, - "velocityX": -0.4390009738027679, - "velocityY": 0.15840306021945175, - "timestamp": 1.7606369212782624 - }, - { - "x": 8.100157148407472, - "y": 5.829696959053818, - "heading": -0.25457167500592204, - "angularVelocity": 0.5323576483569935, - "velocityX": -0.8218531123184383, - "velocityY": 0.29486519757649204, - "timestamp": 1.826240295773835 - }, - { - "x": 8.021119032382456, - "y": 5.857995174413058, - "heading": -0.21555305979528627, - "angularVelocity": 0.5947653685599519, - "velocityX": -1.2047873548082333, - "velocityY": 0.4313530451265237, - "timestamp": 1.8918436702694077 - }, - { - "x": 7.916952910031998, - "y": 5.895249222659617, - "heading": -0.17317929559353756, - "angularVelocity": 0.6459083016317765, - "velocityX": -1.5878165285154446, - "velocityY": 0.567867865532928, - "timestamp": 1.9574470447649803 - }, - { - "x": 7.787651985971443, - "y": 5.941460762829762, - "heading": -0.12849926666272526, - "angularVelocity": 0.6810629677872386, - "velocityX": -1.9709492850749835, - "velocityY": 0.7044079748255081, - "timestamp": 2.023050419260553 - }, - { - "x": 7.633210581172123, - "y": 5.996630683324096, - "heading": -0.08312168761624714, - "angularVelocity": 0.6916958067384257, - "velocityX": -2.354168607740487, - "velocityY": 0.8409616261135585, - "timestamp": 2.0886537937561256 - }, - { - "x": 7.453632538523732, - "y": 6.060756399754721, - "heading": -0.03982832718204048, - "angularVelocity": 0.6599258158149841, - "velocityX": -2.7373293527836737, - "velocityY": 0.9774758832711061, - "timestamp": 2.154257168251698 - }, - { - "x": 7.248984101269758, - "y": 6.133814967695316, - "heading": -0.004587884016795663, - "angularVelocity": 0.5371742450172758, - "velocityX": -3.1194803442281134, - "velocityY": 1.11364039582332, - "timestamp": 2.219860542747271 - }, - { - "x": 7.020152981307638, - "y": 6.215344107160959, - "heading": -7.996010360253712e-8, - "angularVelocity": 0.06993244009119806, - "velocityX": -3.488099838180771, - "velocityY": 1.242758319865779, - "timestamp": 2.2854639172428435 - }, - { - "x": 6.789970042208637, - "y": 6.297403837687721, - "heading": -7.492708758217055e-8, - "angularVelocity": 7.671885873349635e-8, - "velocityX": -3.5087057772391836, - "velocityY": 1.2508461821929457, - "timestamp": 2.351067291738416 - }, - { - "x": 6.5597871223902215, - "y": 6.379463622297776, - "heading": -6.98940737576559e-8, - "angularVelocity": 7.671882526186561e-8, - "velocityX": -3.5087054833429296, - "velocityY": 1.2508470065910036, - "timestamp": 2.4166706662339887 - }, - { - "x": 6.3296039103658925, - "y": 6.46152258724702, - "heading": -6.48610593666683e-8, - "angularVelocity": 7.671883389668008e-8, - "velocityX": -3.5087099374722537, - "velocityY": 1.2508345124042644, - "timestamp": 2.4822740407295614 - }, - { - "x": 6.092938315579126, - "y": 6.522410853761584, - "heading": -5.975605245251985e-8, - "angularVelocity": 7.781622444578512e-8, - "velocityX": -3.6075216649524458, - "velocityY": 0.9281270511271241, - "timestamp": 2.547877415225134 + "angularVelocity": 0.23344012220985313, + "velocityX": 0.3346664419690626, + "velocityY": -0.09633095406457304, + "timestamp": 1.6729337413306924 + }, + { + "x": 8.184449385960491, + "y": 5.800720366343895, + "heading": -0.3251687978756523, + "angularVelocity": 0.29387941781314064, + "velocityX": -0.03132053432391204, + "velocityY": 0.03262661481041375, + "timestamp": 1.7403718792092189 + }, + { + "x": 8.157654760081511, + "y": 5.811617201116665, + "heading": -0.30136325235519174, + "angularVelocity": 0.3529982628426134, + "velocityX": -0.39732155604955366, + "velocityY": 0.1615826758502398, + "timestamp": 1.8078100170877454 + }, + { + "x": 8.106176542546697, + "y": 5.831210437992555, + "heading": -0.27368179509987206, + "angularVelocity": 0.41047185058966307, + "velocityX": -0.7633398423239571, + "velocityY": 0.29053644558191305, + "timestamp": 1.8752481549662718 + }, + { + "x": 8.03001326746699, + "y": 5.859499833662039, + "heading": -0.24226677680218867, + "angularVelocity": 0.46583460466049514, + "velocityX": -1.129379865394508, + "velocityY": 0.4194866074214659, + "timestamp": 1.9426862928447983 + }, + { + "x": 7.929163035221131, + "y": 5.896484993829156, + "heading": -0.2073080079914019, + "angularVelocity": 0.5183827713890392, + "velocityX": -1.495448056817871, + "velocityY": 0.5484309224809322, + "timestamp": 2.010124430723325 + }, + { + "x": 7.80362329670357, + "y": 5.942165255300411, + "heading": -0.16907207724227352, + "angularVelocity": 0.5669778548452968, + "velocityX": -1.8615540474099375, + "velocityY": 0.677365403438884, + "timestamp": 2.0775625686018513 + }, + { + "x": 7.653390486781624, + "y": 5.996539431834676, + "heading": -0.12796178084950977, + "angularVelocity": 0.6096001118360348, + "velocityX": -2.227712903232215, + "velocityY": 0.8062822943333132, + "timestamp": 2.1450007064803778 + }, + { + "x": 7.478459409616276, + "y": 6.059605150575439, + "heading": -0.0846568853073795, + "angularVelocity": 0.6421425161550796, + "velocityX": -2.593948805058106, + "velocityY": 0.9351639995511196, + "timestamp": 2.2124388443589043 + }, + { + "x": 7.278822723764661, + "y": 6.131356463760539, + "heading": -0.04054583544960124, + "angularVelocity": 0.6540964985900647, + "velocityX": -2.9602935687698184, + "velocityY": 1.063957508944601, + "timestamp": 2.2798769822374307 + }, + { + "x": 7.05448611686489, + "y": 6.21176649036125, + "heading": -5.1297661467442016e-8, + "angularVelocity": 0.6012292958766616, + "velocityX": -3.3265539938819106, + "velocityY": 1.1923524155656642, + "timestamp": 2.347315120115957 + }, + { + "x": 6.817570799613061, + "y": 6.29528987249135, + "heading": -4.834198432704978e-8, + "angularVelocity": 4.3827976770594794e-8, + "velocityX": -3.5130762014599934, + "velocityY": 1.238518511299158, + "timestamp": 2.4147532579944837 + }, + { + "x": 6.580655475027505, + "y": 6.378813233819268, + "heading": -4.538633792987658e-8, + "angularVelocity": 4.382752089770155e-8, + "velocityX": -3.5130763102074556, + "velocityY": 1.23851820283599, + "timestamp": 2.48219139587301 + }, + { + "x": 6.3437395023630385, + "y": 6.4623347568384295, + "heading": -4.243069068525663e-8, + "angularVelocity": 4.382753346398488e-8, + "velocityX": -3.5130859201838094, + "velocityY": 1.2384909436497944, + "timestamp": 2.5496295337515367 + }, + { + "x": 6.09986574355872, + "y": 6.5225893218228235, + "heading": -3.943141637637637e-8, + "angularVelocity": 4.447445322827149e-8, + "velocityX": -3.6162587888117317, + "velocityY": 0.8934790740059899, + "timestamp": 2.617067671630063 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -5.436468625345948e-8, - "angularVelocity": 8.218123290935499e-8, - "velocityX": -3.6872209612695626, - "velocityY": 0.5291823081772375, - "timestamp": 2.6134807897207066 - }, - { - "x": 5.6615808215647085, - "y": 6.568071618356437, - "heading": -4.94825886082418e-8, - "angularVelocity": 9.582618856188379e-8, - "velocityX": -3.71880135129745, - "velocityY": 0.2148218335343848, - "timestamp": 2.664428215664344 - }, - { - "x": 5.471871500054035, - "y": 6.562921703037617, - "heading": -4.486806140713326e-8, - "angularVelocity": 9.057429527869525e-8, - "velocityX": -3.7236291725620743, - "velocityY": -0.10108293448460899, - "timestamp": 2.7153756416079817 - }, - { - "x": 5.283280913941195, - "y": 6.541714647593256, - "heading": -4.0367954549807895e-8, - "angularVelocity": 8.832844395914553e-8, - "velocityX": -3.7016705480170033, - "velocityY": -0.4162537174659499, - "timestamp": 2.7663230675516193 - }, - { - "x": 5.095501482386873, - "y": 6.514236775268743, - "heading": -3.5882626212892094e-8, - "angularVelocity": 8.803837002241881e-8, - "velocityX": -3.68574914387354, - "velocityY": -0.539337794119592, - "timestamp": 2.817270493495257 - }, - { - "x": 4.907722071138919, - "y": 6.486758764174086, - "heading": -3.139729791987603e-8, - "angularVelocity": 8.803836916075124e-8, - "velocityX": -3.6857487452985973, - "velocityY": -0.5393405179106829, - "timestamp": 2.8682179194388944 - }, - { - "x": 4.7199426574500984, - "y": 6.459280769759847, - "heading": -2.6911969659920007e-8, - "angularVelocity": 8.803836851184644e-8, - "velocityX": -3.6857487932081243, - "velocityY": -0.5393401905061415, - "timestamp": 2.919165345382532 - }, - { - "x": 4.532163244787761, - "y": 6.431802768330805, - "heading": -2.2426641378076926e-8, - "angularVelocity": 8.803836894144722e-8, - "velocityX": -3.6857487730602023, - "velocityY": -0.5393403281932087, - "timestamp": 2.9701127713261695 - }, - { - "x": 4.3443838326647874, - "y": 6.404324763215868, - "heading": -1.7941313081549045e-8, - "angularVelocity": 8.803836922968166e-8, - "velocityX": -3.6857487624735623, - "velocityY": -0.5393404005402795, - "timestamp": 3.021060197269807 - }, - { - "x": 4.156604420793327, - "y": 6.376846756382128, - "heading": -1.3455984799695103e-8, - "angularVelocity": 8.80383689416604e-8, - "velocityX": -3.685748757536816, - "velocityY": -0.5393404342770572, - "timestamp": 3.0720076232134446 - }, - { - "x": 3.968825009218295, - "y": 6.349368747522659, - "heading": -8.970656536814864e-9, - "angularVelocity": 8.803836856924311e-8, - "velocityX": -3.685748751718508, - "velocityY": -0.5393404740382505, - "timestamp": 3.122955049157082 - }, - { - "x": 3.781045597297714, - "y": 6.321890741024598, - "heading": -4.485328240150933e-9, - "angularVelocity": 8.803836923235202e-8, - "velocityX": -3.6857487585009534, - "velocityY": -0.5393404276883234, - "timestamp": 3.1739024751007197 - }, - { - "x": 3.5932661777631214, - "y": 6.2944127865592865, - "heading": -1.5290485245544054e-22, - "angularVelocity": 8.803836812310754e-8, - "velocityX": -3.6857489079493813, - "velocityY": -0.5393394063855248, - "timestamp": 3.224849901044357 + "heading": -3.6277001373267394e-8, + "angularVelocity": 4.677494222617604e-8, + "velocityX": -3.689626869559746, + "velocityY": 0.5121386527717335, + "timestamp": 2.6845058095085896 + }, + { + "x": 5.675146419069591, + "y": 6.5685334564190505, + "heading": -3.325736766733182e-8, + "angularVelocity": 6.38129937324226e-8, + "velocityX": -3.7171934353117426, + "velocityY": 0.24104917118070399, + "timestamp": 2.7318258540530804 + }, + { + "x": 5.498885444409021, + "y": 6.56705094919282, + "heading": -3.0392937858444436e-8, + "angularVelocity": 6.053311733876744e-8, + "velocityX": -3.724869161837878, + "velocityY": -0.03132937089348242, + "timestamp": 2.779145898597571 + }, + { + "x": 5.3232044198709785, + "y": 6.552687568221491, + "heading": -2.7611010604394772e-8, + "angularVelocity": 5.878961613051839e-8, + "velocityX": -3.7126132536258285, + "velocityY": -0.3035369283692009, + "timestamp": 2.826465943142062 + }, + { + "x": 5.148803896994517, + "y": 6.527102675424277, + "heading": -2.484990875079416e-8, + "angularVelocity": 5.8349519324830205e-8, + "velocityX": -3.6855528044249266, + "velocityY": -0.5406776989222635, + "timestamp": 2.873785987686553 + }, + { + "x": 4.974403597324897, + "y": 6.501516261174204, + "heading": -2.208880782591367e-8, + "angularVelocity": 5.834949969847243e-8, + "velocityX": -3.6855480874631454, + "velocityY": -0.5407098513192842, + "timestamp": 2.9211060322310436 + }, + { + "x": 4.800003297129097, + "y": 6.475929850510635, + "heading": -1.9327706889009288e-8, + "angularVelocity": 5.8349499952569724e-8, + "velocityX": -3.6855480985827302, + "velocityY": -0.5407097755267535, + "timestamp": 2.9684260767755344 + }, + { + "x": 4.625602997244748, + "y": 6.450343437724181, + "heading": -1.656660593755509e-8, + "angularVelocity": 5.8349500260046496e-8, + "velocityX": -3.685548092000944, + "velocityY": -0.5407098203890633, + "timestamp": 3.015746121320025 + }, + { + "x": 4.451202697255753, + "y": 6.4247570256510045, + "heading": -1.3805504990627123e-8, + "angularVelocity": 5.834950016439505e-8, + "velocityX": -3.685548094212386, + "velocityY": -0.5407098053155895, + "timestamp": 3.063066165864516 + }, + { + "x": 4.276802397278509, + "y": 6.3991706134977395, + "heading": -1.1044403987822918e-8, + "angularVelocity": 5.8349501345210454e-8, + "velocityX": -3.685548093964082, + "velocityY": -0.5407098070080617, + "timestamp": 3.1103862104090068 + }, + { + "x": 4.102402097300047, + "y": 6.373584201352764, + "heading": -8.283303038922857e-9, + "angularVelocity": 5.834950020607066e-8, + "velocityX": -3.685548093989784, + "velocityY": -0.5407098068328745, + "timestamp": 3.1577062549534975 + }, + { + "x": 3.9280017973350696, + "y": 6.347997789115884, + "heading": -5.522202092459704e-9, + "angularVelocity": 5.8349500154572286e-8, + "velocityX": -3.685548093704841, + "velocityY": -0.5407098087750815, + "timestamp": 3.2050262994979883 + }, + { + "x": 3.7536014973495653, + "y": 6.322411377018916, + "heading": -2.7611011159009457e-9, + "angularVelocity": 5.834950079057347e-8, + "velocityX": -3.6855480941386265, + "velocityY": -0.5407098058183458, + "timestamp": 3.252346344042479 + }, + { + "x": 3.579201219971439, + "y": 6.296824810827607, + "heading": -5.947116132480024e-26, + "angularVelocity": 5.8349503735249215e-8, + "velocityX": -3.6855476163838365, + "velocityY": -0.5407130622468497, + "timestamp": 3.29966638858697 }, { "x": 3.405486822128296, "y": 6.266934394836426, - "heading": -7.242717716527151e-23, - "angularVelocity": 4.066531625418711e-24, - "velocityX": -3.6857476537198193, - "velocityY": -0.5393479889103638, - "timestamp": 3.2757973269879948 - }, - { - "x": 3.1943671277625394, - "y": 6.2230448572371815, - "heading": -6.454692239159141e-23, - "angularVelocity": 6.1636286209902435e-25, - "velocityX": -3.5183764765296694, - "velocityY": -0.7314330248480798, - "timestamp": 3.3358021888252907 - }, - { - "x": 3.002694024502071, - "y": 6.190127687410396, - "heading": -5.666660567021065e-23, - "angularVelocity": 6.173952401211252e-25, - "velocityX": -3.1942928854697095, - "velocityY": -0.5485750457361542, - "timestamp": 3.3958070506625866 - }, - { - "x": 2.830467511330891, - "y": 6.168182901091022, - "heading": -4.87863785909476e-23, - "angularVelocity": 6.159013258773547e-25, - "velocityX": -2.870209311341694, - "velocityY": -0.36571680439623283, - "timestamp": 3.4558119124998825 - }, - { - "x": 2.6776875881237743, - "y": 6.157210503145755, - "heading": -4.0906073219636964e-23, - "angularVelocity": 6.172060876090466e-25, - "velocityX": -2.5461257393006025, - "velocityY": -0.1828584819513171, - "timestamp": 3.5158167743371784 - }, - { - "x": 2.5443542548814535, - "y": 6.157210495895674, - "heading": -3.3025815924682597e-23, - "angularVelocity": 6.164048799254959e-25, - "velocityX": -2.2220421672473036, - "velocityY": -1.208249057531058e-7, - "timestamp": 3.5758216361744743 - }, - { - "x": 2.430467511630842, - "y": 6.168182880686502, - "heading": -2.5145589184649632e-23, - "angularVelocity": 6.158956724948031e-25, - "velocityX": -1.8979585947454753, - "velocityY": 0.18285826272844113, - "timestamp": 3.6358264980117703 - }, - { - "x": 2.3360273584029394, - "y": 6.1901276583922495, - "heading": -1.726521486745078e-23, - "angularVelocity": 6.183550926375638e-25, - "velocityX": -1.5738750217270354, - "velocityY": 0.3657166608474323, - "timestamp": 3.695831359849066 - }, - { - "x": 2.261033795226874, - "y": 6.223044829624826, - "heading": -9.385003241848346e-24, - "angularVelocity": 6.156437857286479e-25, - "velocityX": -1.2497914482231562, - "velocityY": 0.5485750691641013, - "timestamp": 3.755836221686362 + "heading": -3.242812966980484e-26, + "angularVelocity": 4.141833469401844e-27, + "velocityX": -3.6710531343607253, + "velocityY": -0.6316650011408507, + "timestamp": 3.3469864331314607 + }, + { + "x": 3.234879221757396, + "y": 6.227705290951873, + "heading": -3.799073171453003e-26, + "angularVelocity": 3.7193578167117204e-28, + "velocityX": -3.4320264149158155, + "velocityY": -0.7891519514521484, + "timestamp": 3.396696889796196 + }, + { + "x": 3.075654096212167, + "y": 6.19701582942553, + "heading": -4.3640191392241984e-26, + "angularVelocity": -1.3753350907836377e-27, + "velocityX": -3.2030509520179686, + "velocityY": -0.6173643049252793, + "timestamp": 3.4464073464609317 + }, + { + "x": 2.9276929132783946, + "y": 6.175021768989464, + "heading": -4.923363376741485e-26, + "angularVelocity": -2.4846347668894466e-28, + "velocityX": -2.9764599414500257, + "velocityY": -0.4424433391228337, + "timestamp": 3.4961178031256672 + }, + { + "x": 2.790955431257819, + "y": 6.16177499390257, + "heading": -5.486689770041805e-26, + "angularVelocity": -1.0495335231555751e-27, + "velocityX": -2.7506784526800994, + "velocityY": -0.2664786440453503, + "timestamp": 3.5458282597904027 + }, + { + "x": 2.6654213921010017, + "y": 6.157301437220893, + "heading": -6.044835484657667e-26, + "angularVelocity": -7.36271402334531e-30, + "velocityX": -2.525304484798085, + "velocityY": -0.08999226685541546, + "timestamp": 3.5955387164551382 + }, + { + "x": 2.551078597067916, + "y": 6.16161665554964, + "heading": -6.59367177391361e-26, + "angularVelocity": 1.865367086612129e-27, + "velocityX": -2.300175912771294, + "velocityY": 0.08680705465754052, + "timestamp": 3.6452491731198737 + }, + { + "x": 2.447918895369922, + "y": 6.174731018536324, + "heading": -7.161083135132275e-26, + "angularVelocity": -1.871285764514843e-27, + "velocityX": -2.075211306018359, + "velocityY": 0.2638149771009456, + "timestamp": 3.6949596297846092 + }, + { + "x": 2.355936456054869, + "y": 6.196651932350337, + "heading": -7.721743669966212e-26, + "angularVelocity": -5.132563200735982e-28, + "velocityX": -1.8503639975672366, + "velocityY": 0.44097188569106954, + "timestamp": 3.7446700864493447 + }, + { + "x": 2.2751269010087887, + "y": 6.227384951214515, + "heading": -8.282082994441975e-26, + "angularVelocity": -4.486400643129525e-28, + "velocityX": -1.6256047614104199, + "velocityY": 0.618240525760057, + "timestamp": 3.7943805431140802 }, { "x": 2.205486822128296, "y": 6.266934394836426, - "heading": -1.5067646314431248e-24, - "angularVelocity": 5.827628634700505e-25, - "velocityX": -0.9257078742918208, - "velocityY": 0.7314334850167038, - "timestamp": 3.815841083523659 - }, - { - "x": 2.166583654686104, - "y": 6.330345399138925, - "heading": 0.03190215483964595, - "angularVelocity": 0.46789281830201823, - "velocityX": -0.5705731398677051, - "velocityY": 0.9300172249677102, - "timestamp": 3.8840236962754835 - }, - { - "x": 2.1528025694390287, - "y": 6.405563401029407, - "heading": 0.09569947573294339, - "angularVelocity": 0.9356831356039661, - "velocityX": -0.20212022817660624, - "velocityY": 1.1031845048863924, - "timestamp": 3.952206309027308 - }, - { - "x": 2.1659059898168422, - "y": 6.488336785914272, - "heading": 0.18898264963043915, - "angularVelocity": 1.3681372733109376, - "velocityX": 0.1921812592531237, - "velocityY": 1.2139954974466904, - "timestamp": 4.020388921779133 - }, - { - "x": 2.2052860593561716, - "y": 6.55939873181835, - "heading": 0.2899151707587667, - "angularVelocity": 1.4803263919455447, - "velocityX": 0.5775676224475003, - "velocityY": 1.0422297274341026, - "timestamp": 4.088571534530957 - }, - { - "x": 2.238286501729519, - "y": 6.603471711362833, - "heading": 0.35830069991198743, - "angularVelocity": 1.0029760725382513, - "velocityX": 0.4840008477449354, - "velocityY": 0.6463961670829911, - "timestamp": 4.156754147282782 - }, - { - "x": 2.256009101867676, - "y": 6.624549388885498, + "heading": -8.869624024083721e-26, + "angularVelocity": -5.920668887028623e-27, + "velocityX": -1.400914084337811, + "velocityY": 0.7955960631914043, + "timestamp": 3.8440909997788157 + }, + { + "x": 2.1363371710210255, + "y": 6.327839189643039, + "heading": 0.025262557050058088, + "angularVelocity": 0.415801673405611, + "velocityX": -1.1381484696439823, + "velocityY": 1.002444667372145, + "timestamp": 3.9048472657244186 + }, + { + "x": 2.0845111064453854, + "y": 6.399393278580984, + "heading": 0.0757412033730169, + "angularVelocity": 0.8308385240158422, + "velocityX": -0.8530159608893926, + "velocityY": 1.177723611289897, + "timestamp": 3.9656035316700216 + }, + { + "x": 2.051957537582464, + "y": 6.477853810368175, + "heading": 0.14980799257498115, + "angularVelocity": 1.2190806668118568, + "velocityX": -0.5358059511436571, + "velocityY": 1.291398188582541, + "timestamp": 4.0263597976156245 + }, + { + "x": 2.0403269766878736, + "y": 6.554154175481095, + "heading": 0.23625288977037628, + "angularVelocity": 1.422814517152715, + "velocityX": -0.19142981737889736, + "velocityY": 1.255843556633874, + "timestamp": 4.087116063561227 + }, + { + "x": 2.0424863781029257, + "y": 6.6144252836562565, + "heading": 0.31360709056854197, + "angularVelocity": 1.2731888570542382, + "velocityX": 0.03554203638823312, + "velocityY": 0.9920146874912401, + "timestamp": 4.14787232950683 + }, + { + "x": 2.048130429243297, + "y": 6.654505373712402, + "heading": 0.3666123519116142, + "angularVelocity": 0.8724246053983875, + "velocityX": 0.09289660996323934, + "velocityY": 0.659686526687304, + "timestamp": 4.208628595452433 + }, + { + "x": 2.052253484725952, + "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": 0.5097673803944184, - "velocityX": 0.2599284395666251, - "velocityY": 0.3091356677601188, - "timestamp": 4.224936760034606 + "angularVelocity": 0.4352739504266956, + "velocityX": 0.0678622265289755, + "velocityY": 0.3266075878021015, + "timestamp": 4.269384861398036 }, { - "x": 2.256009101867676, - "y": 6.624549388885498, + "x": 2.052253484725952, + "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": 1.058900443902291e-24, - "velocityX": 2.5593812661454674e-24, - "velocityY": -7.939073774690169e-24, - "timestamp": 4.293119372786431 + "angularVelocity": -3.1389023173080445e-26, + "velocityX": -4.435068013070024e-22, + "velocityY": -1.8479842669198598e-22, + "timestamp": 4.330141127343639 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePDEABC.3.traj b/src/main/deploy/choreo/CenterLanePDEABC.3.traj index f386d29d..fda79d64 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.3.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.3.traj @@ -1,76 +1,85 @@ { "samples": [ { - "x": 2.256009101867676, - "y": 6.624549388885498, + "x": 2.052253484725952, + "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": 1.058900443902291e-24, - "velocityX": 2.5593812661454674e-24, - "velocityY": -7.939073774690169e-24, + "angularVelocity": -3.1389023173080445e-26, + "velocityX": -4.435068013070024e-22, + "velocityY": -1.8479842669198598e-22, "timestamp": 0 }, { - "x": 2.306032658844481, - "y": 6.645836035930219, + "x": 2.103013204328941, + "y": 6.695498705652326, "heading": 0.3930579718029317, - "angularVelocity": 7.690085231355673e-17, - "velocityX": 0.5342706205732553, - "velocityY": 0.22734948919726436, - "timestamp": 0.09362962336040681 + "angularVelocity": 3.416438400428702e-17, + "velocityX": 0.5194333949867557, + "velocityY": 0.21643049229415012, + "timestamp": 0.09772132499159625 }, { - "x": 2.406079771576901, - "y": 6.688409329500005, + "x": 2.2045326425850984, + "y": 6.737798454207702, "heading": 0.3930579718029317, - "angularVelocity": 7.285372137244723e-17, - "velocityX": 1.0685412281037379, - "velocityY": 0.45469897284440564, - "timestamp": 0.18725924672081362 + "angularVelocity": 5.790454228147962e-17, + "velocityX": 1.0388667802538214, + "velocityY": 0.432860980538431, + "timestamp": 0.1954426499831925 }, { - "x": 2.556150436401367, - "y": 6.752269268035889, - "heading": 0.3930579718029317, - "angularVelocity": 1.0864711023830975e-16, - "velocityX": 1.6028117965059043, - "velocityY": 0.6820484398411784, - "timestamp": 0.28088887008122043 + "x": 2.356811797278175, + "y": 6.801248075919452, + "heading": 0.3930579718029318, + "angularVelocity": 1.3697605676063785e-16, + "velocityX": 1.558300142841611, + "velocityY": 0.6492914593330171, + "timestamp": 0.29316397497478874 }, { - "x": 2.7062211012258333, - "y": 6.816129206571772, - "heading": 0.3930579718029317, - "angularVelocity": 1.1973301038102625e-17, - "velocityX": 1.6028117965059043, - "velocityY": 0.6820484398411785, - "timestamp": 0.37451849344162724 + "x": 2.5598506573269275, + "y": 6.8858475661703915, + "heading": 0.3930579718029318, + "angularVelocity": 1.6886690536158693e-16, + "velocityX": 2.0777333920330334, + "velocityY": 0.8657218908791364, + "timestamp": 0.390885299966385 + }, + { + "x": 2.712129812020004, + "y": 6.9492971878821415, + "heading": 0.3930579718029318, + "angularVelocity": 9.579438090360999e-17, + "velocityX": 1.5583001428416108, + "velocityY": 0.649291459333017, + "timestamp": 0.48860662495798124 }, { - "x": 2.8062682139582535, - "y": 6.8587025001415585, + "x": 2.8136492502761614, + "y": 6.991596936437518, "heading": 0.3930579718029317, - "angularVelocity": -9.389335874202783e-18, - "velocityX": 1.0685412281037379, - "velocityY": 0.45469897284440564, - "timestamp": 0.46814811680203405 + "angularVelocity": -7.496030828989607e-17, + "velocityX": 1.0388667802538214, + "velocityY": 0.4328609805384309, + "timestamp": 0.5863279499495775 }, { - "x": 2.8562917709350586, - "y": 6.879989147186279, + "x": 2.8644089698791504, + "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 6.118834146220853e-17, - "velocityX": 0.5342706205732553, - "velocityY": 0.22734948919726436, - "timestamp": 0.5617777401624409 + "angularVelocity": 4.893193347075666e-17, + "velocityX": 0.5194333949867557, + "velocityY": 0.21643049229415012, + "timestamp": 0.6840492749411737 }, { - "x": 2.8562917709350586, - "y": 6.879989147186279, + "x": 2.8644089698791504, + "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 9.527092238533276e-28, - "velocityX": 5.413823231130942e-26, - "velocityY": -7.426199884271778e-26, - "timestamp": 0.6554073635228477 + "angularVelocity": 3.090778525044755e-27, + "velocityX": 4.437436909321685e-22, + "velocityY": 1.850515832937087e-22, + "timestamp": 0.78177059993277 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePDEABC.4.traj b/src/main/deploy/choreo/CenterLanePDEABC.4.traj index 164ec04b..6fcd62ac 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.4.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.4.traj @@ -1,247 +1,256 @@ { "samples": [ { - "x": 2.8562917709350586, - "y": 6.879989147186279, + "x": 2.8644089698791504, + "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 9.527092238533276e-28, - "velocityX": 5.413823231130942e-26, - "velocityY": -7.426199884271778e-26, + "angularVelocity": 3.090778525044755e-27, + "velocityX": 4.437436909321685e-22, + "velocityY": 1.850515832937087e-22, "timestamp": 0 }, { - "x": 2.8345596275672933, - "y": 6.874297000721419, - "heading": 0.3901475474656487, - "angularVelocity": -0.04834267054248828, - "velocityX": -0.36097480135517596, - "velocityY": -0.09454757428506516, - "timestamp": 0.06020404550727143 - }, - { - "x": 2.791210221547733, - "y": 6.8624934986264625, - "heading": 0.38424126669366737, - "angularVelocity": -0.0981043835545563, - "velocityX": -0.7200414134017654, - "velocityY": -0.19605828803531558, - "timestamp": 0.12040809101454286 - }, - { - "x": 2.7264146334756165, - "y": 6.84400633385399, - "heading": 0.3752232336055894, - "angularVelocity": -0.14979114795514328, - "velocityX": -1.076266345993141, - "velocityY": -0.3070751245485542, - "timestamp": 0.1806121365218143 - }, - { - "x": 2.6404493956081283, - "y": 6.818009888574526, - "heading": 0.36292730942366375, - "angularVelocity": -0.20423750726918255, - "velocityX": -1.4278980281666382, - "velocityY": -0.431805621373474, - "timestamp": 0.24081618202908572 - }, - { - "x": 2.5338184232966747, - "y": 6.783217894913327, - "heading": 0.3470968619442176, - "angularVelocity": -0.2629465735410421, - "velocityX": -1.7711595859214113, - "velocityY": -0.5779012584294861, - "timestamp": 0.30102022753635715 - }, - { - "x": 2.407629795142463, - "y": 6.737392802929253, - "heading": 0.3272924409497442, - "angularVelocity": -0.3289549867887417, - "velocityX": -2.0960157592560678, - "velocityY": -0.761163001555094, - "timestamp": 0.3612242730436286 - }, - { - "x": 2.2652115507191506, - "y": 6.676063122618794, - "heading": 0.30267146092102903, - "angularVelocity": -0.40895889671967695, - "velocityX": -2.3655925980274817, - "velocityY": -1.0186969960855932, - "timestamp": 0.4214283185509 - }, - { - "x": 2.1204311340072723, - "y": 6.592422966921264, - "heading": 0.27232018505584993, - "angularVelocity": -0.5041401389133051, - "velocityX": -2.4048287036523215, - "velocityY": -1.3892779960680068, - "timestamp": 0.48163236405817145 - }, - { - "x": 1.992685360010843, - "y": 6.494128334362165, - "heading": 0.23976877399587285, - "angularVelocity": -0.5406847793317353, - "velocityX": -2.1218802311382734, - "velocityY": -1.6326914866082545, - "timestamp": 0.5418364095654429 - }, - { - "x": 1.886238292214546, - "y": 6.388651713547729, - "heading": 0.20716928495643328, - "angularVelocity": -0.5414833631986058, - "velocityX": -1.7681048989214407, - "velocityY": -1.7519856003978374, - "timestamp": 0.6020404550727143 - }, - { - "x": 1.801932900713492, - "y": 6.279311808551523, - "heading": 0.17534566664181037, - "angularVelocity": -0.5285960112228498, - "velocityX": -1.4003276821467197, - "velocityY": -1.8161554439559784, - "timestamp": 0.6622445005799857 + "x": 2.8419574642644285, + "y": 7.0048102452086525, + "heading": 0.3900130033247021, + "angularVelocity": -0.04733865202910357, + "velocityX": -0.3490426976251426, + "velocityY": -0.12338594795790273, + "timestamp": 0.06432309218178744 + }, + { + "x": 2.7972074454620057, + "y": 6.988518822273239, + "heading": 0.3838409042111465, + "angularVelocity": -0.0959546393713819, + "velocityX": -0.6957068959923765, + "velocityY": -0.25327487194445947, + "timestamp": 0.1286461843635749 + }, + { + "x": 2.7303816098102875, + "y": 6.963303209303571, + "heading": 0.37443012101631223, + "angularVelocity": -0.14630489417762893, + "velocityX": -1.0389089421083304, + "velocityY": -0.39201493762775413, + "timestamp": 0.19296927654536233 + }, + { + "x": 2.6418300370011316, + "y": 6.928345468623928, + "heading": 0.3616207312094327, + "angularVelocity": -0.19914138721251212, + "velocityX": -1.3766684685943704, + "velocityY": -0.5434710847054101, + "timestamp": 0.2572923687271498 + }, + { + "x": 2.5321688819092247, + "y": 6.882378878870811, + "heading": 0.34516587333325927, + "angularVelocity": -0.25581571591224617, + "velocityX": -1.7048489332880077, + "velocityY": -0.7146203360872027, + "timestamp": 0.3216154609089372 + }, + { + "x": 2.4026942269657687, + "y": 6.823220010782376, + "heading": 0.324643183634766, + "angularVelocity": -0.3190563295758964, + "velocityX": -2.012879831360391, + "velocityY": -0.9197143060417909, + "timestamp": 0.38593855309072467 + }, + { + "x": 2.2570878780964114, + "y": 6.7465695928484575, + "heading": 0.2992393598461131, + "angularVelocity": -0.3949409601898093, + "velocityX": -2.2636714736575443, + "velocityY": -1.1916469705357338, + "timestamp": 0.4502616452725121 + }, + { + "x": 2.110110870224978, + "y": 6.646173604450097, + "heading": 0.26804931760848005, + "angularVelocity": -0.4848964995259366, + "velocityX": -2.2849804461522467, + "velocityY": -1.5608078684187743, + "timestamp": 0.5145847374542996 + }, + { + "x": 1.9823765062843866, + "y": 6.531738038725912, + "heading": 0.2347990669991598, + "angularVelocity": -0.5169255625234794, + "velocityX": -1.9858243689465778, + "velocityY": -1.779074385926154, + "timestamp": 0.578907829636087 + }, + { + "x": 1.8778139110395455, + "y": 6.411761042169391, + "heading": 0.201692518308331, + "angularVelocity": -0.5146914983077023, + "velocityX": -1.6255840896039286, + "velocityY": -1.865224330594162, + "timestamp": 0.6432309218178744 + }, + { + "x": 1.796996456842112, + "y": 6.289847592193697, + "heading": 0.16955186256906168, + "angularVelocity": -0.49967522780830353, + "velocityX": -1.2564298676598118, + "velocityY": -1.8953294352072878, + "timestamp": 0.7075540139996619 }, { "x": 1.740001559257507, "y": 6.167843341827393, - "heading": 0.1447051188724413, - "angularVelocity": -0.5089449971541862, - "velocityX": -1.0286906956859625, - "velocityY": -1.8515112362451833, - "timestamp": 0.7224485460872572 - }, - { - "x": 1.69740928285499, - "y": 6.020477623351315, - "heading": 0.10712758929116097, - "angularVelocity": -0.47810334665219445, - "velocityX": -0.5419065626848166, - "velocityY": -1.874951439604909, - "timestamp": 0.801045631517443 - }, - { - "x": 1.692516979833633, - "y": 5.879848304102219, - "heading": 0.07335850742026521, - "angularVelocity": -0.429648016667122, - "velocityX": -0.06224534910652133, - "velocityY": -1.7892434366922005, - "timestamp": 0.8796427169476289 - }, - { - "x": 1.7224083305171312, - "y": 5.755218848404405, - "heading": 0.044801662501508355, - "angularVelocity": -0.3633321103760591, - "velocityX": 0.38031118482184595, - "velocityY": -1.585675283195029, - "timestamp": 0.9582398023778147 - }, - { - "x": 1.7819268218986297, - "y": 5.6548248770557, - "heading": 0.022621166422217082, - "angularVelocity": -0.282205071064539, - "velocityX": 0.757260794795832, - "velocityY": -1.277324353685865, - "timestamp": 1.0368368878080005 - }, - { - "x": 1.8649022251310534, - "y": 5.584669337944561, - "heading": 0.0075685613575453385, - "angularVelocity": -0.19151607190373818, - "velocityX": 1.055705854463611, - "velocityY": -0.8925972092623536, - "timestamp": 1.1154339732381864 - }, - { - "x": 1.9654334653384358, + "heading": 0.1387804363394676, + "angularVelocity": -0.4783884789404854, + "velocityX": -0.8860721033672938, + "velocityY": -1.8967410649584446, + "timestamp": 0.7718771061814493 + }, + { + "x": 1.706182040858774, + "y": 6.03669580225492, + "heading": 0.10729245974250749, + "angularVelocity": -0.45130411484825544, + "velocityX": -0.48472113692462804, + "velocityY": -1.8796833159166881, + "timestamp": 0.8416481886089766 + }, + { + "x": 1.699651780790286, + "y": 5.911922041931935, + "heading": 0.07852355852866735, + "angularVelocity": -0.4123327346071031, + "velocityX": -0.09359551036449741, + "velocityY": -1.7883305802599465, + "timestamp": 0.9114192710365039 + }, + { + "x": 1.7185433856985064, + "y": 5.798927064131694, + "heading": 0.053308691164350094, + "angularVelocity": -0.36139424080898114, + "velocityX": 0.2707655414095584, + "velocityY": -1.6195101733961197, + "timestamp": 0.9811903534640312 + }, + { + "x": 1.7598996151781712, + "y": 5.702665752208848, + "heading": 0.032386709878439925, + "angularVelocity": -0.29986608431425904, + "velocityX": 0.592741692414218, + "velocityY": -1.3796734775160497, + "timestamp": 1.0509614358915584 + }, + { + "x": 1.820114668725021, + "y": 5.6271104583982785, + "heading": 0.016317787108466864, + "angularVelocity": -0.2303092084985792, + "velocityX": 0.8630373996189019, + "velocityY": -1.082902703839363, + "timestamp": 1.1207325183190857 + }, + { + "x": 1.8954858630630067, + "y": 5.575103001600796, + "heading": 0.005460341828234747, + "angularVelocity": -0.15561526211822638, + "velocityX": 1.0802640824194598, + "velocityY": -0.7454013179672614, + "timestamp": 1.190503600746613 + }, + { + "x": 1.9826012990017379, "y": 5.548515796661377, - "heading": 4.814116720601663e-28, - "angularVelocity": -0.09629569997564517, - "velocityX": 1.2790708415858416, - "velocityY": -0.4599857753669162, - "timestamp": 1.1940310586683722 + "heading": -3.619508887306417e-27, + "angularVelocity": -0.07826081577430707, + "velocityX": 1.248589428567619, + "velocityY": -0.3810633863540251, + "timestamp": 1.2602746831741403 }, { "x": 2.0785037517547607, "y": 5.548515796661377, - "heading": 4.711208229171849e-28, - "angularVelocity": -4.224112944688565e-28, - "velocityX": 1.43860660732464, - "velocityY": -1.1518576201030206e-26, - "timestamp": 1.272628144098558 + "heading": -1.3504694626823558e-27, + "angularVelocity": 2.3770174727505955e-28, + "velocityX": 1.3745300978043182, + "velocityY": 2.5815580956362606e-25, + "timestamp": 1.3300457656016675 }, { - "x": 2.2115594481070073, + "x": 2.2108243531689626, "y": 5.548515796661377, - "heading": 9.919579688456685e-28, - "angularVelocity": -1.4478744000102224e-30, - "velocityX": 1.8779766683240853, - "velocityY": 6.343973751493874e-16, - "timestamp": 1.3434787028922344 + "heading": -3.98229506769339e-28, + "angularVelocity": 6.58921025141663e-29, + "velocityX": 1.7982508177997887, + "velocityY": 1.343058267982166e-16, + "timestamp": 1.403628716380524 }, { - "x": 2.367207543861562, + "x": 2.366717480768576, "y": 5.548515796661377, - "heading": 1.5127951147748595e-27, - "angularVelocity": -1.4478743900360266e-30, - "velocityX": 2.196850644577389, - "velocityY": 3.1719689645397288e-15, - "timestamp": 1.4143292616859107 + "heading": 5.540104511762132e-28, + "angularVelocity": 6.589213013653999e-29, + "velocityX": 2.118603915030903, + "velocityY": 6.71525155127488e-16, + "timestamp": 1.4772116671593807 }, { - "x": 2.4917260252771096, + "x": 2.4914319874847974, "y": 5.548515796661377, - "heading": 2.033632260703789e-27, - "angularVelocity": -1.4478743937385766e-30, - "velocityX": 1.757480583578164, - "velocityY": 2.537576634676114e-15, - "timestamp": 1.485179820479587 + "heading": 1.506250408105455e-27, + "angularVelocity": 6.589211632476856e-29, + "velocityX": 1.694883195035658, + "velocityY": 5.372204471623799e-16, + "timestamp": 1.5507946179382373 }, { - "x": 2.585114887925518, + "x": 2.58496786905088, "y": 5.548515796661377, - "heading": 2.5544694066327203e-27, - "angularVelocity": -1.4478743937382235e-30, - "velocityX": 1.3181104600793105, - "velocityY": 1.903182958104165e-15, - "timestamp": 1.5560303792732633 + "heading": 2.458490365034698e-27, + "angularVelocity": 6.589211632476792e-29, + "velocityX": 1.2711624170548799, + "velocityY": 4.029154421641203e-16, + "timestamp": 1.6243775687170938 }, { - "x": 2.6473741303307423, + "x": 2.647325124044576, "y": 5.548515796661377, - "heading": 3.0753065525616534e-27, - "angularVelocity": -1.4478743937387e-30, - "velocityX": 0.8787403157472581, - "velocityY": 1.2687888330822125e-15, - "timestamp": 1.6268809380669396 + "heading": 3.4107303219639416e-27, + "angularVelocity": 6.589211632476844e-29, + "velocityX": 0.8474416197456015, + "velocityY": 2.686103379493745e-16, + "timestamp": 1.6979605194959504 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 3.596143698490385e-27, - "angularVelocity": -1.4478743965967547e-30, - "velocityX": 0.439370160998608, - "velocityY": 6.343944838152068e-16, - "timestamp": 1.6977314968606159 + "heading": 4.3629702788936556e-27, + "angularVelocity": 6.589211633112284e-29, + "velocityX": 0.42372081277207485, + "velocityY": 1.3430518397832439e-16, + "timestamp": 1.771543470274807 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 4.090371729515312e-27, - "angularVelocity": -3.7701463571595157e-28, - "velocityX": -1.25991204352499e-25, - "velocityY": -5.479637988962548e-26, - "timestamp": 1.7685820556542922 + "heading": 5.369537146588021e-27, + "angularVelocity": 8.042005177965745e-28, + "velocityX": 2.643601734591811e-26, + "velocityY": 4.252096317345015e-26, + "timestamp": 1.8451264210536635 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePDEABC.5.traj b/src/main/deploy/choreo/CenterLanePDEABC.5.traj index 1862a3b9..81cb908c 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.5.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.5.traj @@ -3,209 +3,209 @@ { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 4.090371729515312e-27, - "angularVelocity": -3.7701463571595157e-28, - "velocityX": -1.25991204352499e-25, - "velocityY": -5.479637988962548e-26, + "heading": 5.369537146588021e-27, + "angularVelocity": 8.042005177965745e-28, + "velocityX": 2.643601734591811e-26, + "velocityY": 4.252096317345015e-26, "timestamp": 0 }, { - "x": 2.6551487281624913, - "y": 5.535337362195577, - "heading": -0.005358266388786186, - "angularVelocity": -0.0814332978786503, - "velocityX": -0.3549425234125069, - "velocityY": -0.20028182653884274, - "timestamp": 0.06579945216968852 + "x": 2.65514872822099, + "y": 5.535337362244174, + "heading": -0.005358266322899027, + "angularVelocity": -0.07847108857121549, + "velocityX": -0.34203117386479875, + "velocityY": -0.1929963969806516, + "timestamp": 0.06828331835917112 }, { - "x": 2.6089441252483114, - "y": 5.508126235802502, - "heading": -0.016249929633628903, - "angularVelocity": -0.16552817517013077, - "velocityX": -0.7022034590048662, - "velocityY": -0.41354639736059473, - "timestamp": 0.13159890433937704 + "x": 2.608944125410673, + "y": 5.508126235941394, + "heading": -0.01624992945284291, + "angularVelocity": -0.15950693949367814, + "velocityX": -0.676660184662962, + "velocityY": -0.39850327952208153, + "timestamp": 0.13656663671834224 }, { - "x": 2.5407349136089206, - "y": 5.4655968271259, - "heading": -0.03293816472196892, - "angularVelocity": -0.2536227056313986, - "velocityX": -1.036622789251932, - "velocityY": -0.6463489782091849, - "timestamp": 0.19739835650906556 + "x": 2.5407349138993025, + "y": 5.465596827387689, + "heading": -0.03293816440101749, + "angularVelocity": -0.24439695300679784, + "velocityX": -0.9989147151957237, + "velocityY": -0.622837459802409, + "timestamp": 0.20484995507751336 }, { - "x": 2.4521626111212957, - "y": 5.405634188324023, - "heading": -0.055847587039488555, - "angularVelocity": -0.3481704111827463, - "velocityX": -1.3460948315984265, - "velocityY": -0.9112938911290818, - "timestamp": 0.2631978086787541 + "x": 2.4521626115248987, + "y": 5.405634188727449, + "heading": -0.05584758658914312, + "angularVelocity": -0.33550540217775826, + "velocityX": -1.2971294380936889, + "velocityY": -0.8781447665568274, + "timestamp": 0.2731332734366845 }, { - "x": 2.347299393995622, - "y": 5.324395116461008, - "heading": -0.08569447081932854, - "angularVelocity": -0.45360383400866283, - "velocityX": -1.5936791822406984, - "velocityY": -1.2346466297851597, - "timestamp": 0.3289972608484426 + "x": 2.3472993944122122, + "y": 5.324395116991271, + "heading": -0.08569447031240085, + "angularVelocity": -0.43710359192361903, + "velocityX": -1.5357076901433586, + "velocityY": -1.1897352631408427, + "timestamp": 0.3414165917958556 }, { - "x": 2.239880929566337, - "y": 5.216483135728542, - "heading": -0.1227261468780359, - "angularVelocity": -0.5627961151288575, - "velocityX": -1.6325130512069794, - "velocityY": -1.6400133614209302, - "timestamp": 0.39479671301813113 + "x": 2.239880929754108, + "y": 5.216483136191594, + "heading": -0.12272614650889033, + "angularVelocity": -0.542323909943895, + "velocityX": -1.5731289462688671, + "velocityY": -1.580356423688402, + "timestamp": 0.4096999101550267 }, { - "x": 2.153374403298574, - "y": 5.091739159361965, - "heading": -0.1618030539678993, - "angularVelocity": -0.5938789123819666, - "velocityX": -1.3146997948352968, - "velocityY": -1.8958208959685574, - "timestamp": 0.46059616518781965 + "x": 2.153374403355559, + "y": 5.091739159562662, + "heading": -0.16180305380781881, + "angularVelocity": -0.5722760439582536, + "velocityX": -1.2668764271754205, + "velocityY": -1.8268587354348766, + "timestamp": 0.47798322851419783 }, { "x": 2.0932393074035645, "y": 4.9619622230529785, - "heading": -0.19963034769256285, - "angularVelocity": -0.5748876696892882, - "velocityX": -0.9139148414173566, - "velocityY": -1.9723102857195454, - "timestamp": 0.5263956173575082 - }, - { - "x": 2.0629011107007105, - "y": 4.862373019847467, - "heading": -0.22719939375272252, - "angularVelocity": -0.5460591677053324, - "velocityX": -0.600907641312322, - "velocityY": -1.9725599970405534, - "timestamp": 0.576882904616161 - }, - { - "x": 2.048028599586168, - "y": 4.7660067703914, - "heading": -0.2526003668147138, - "angularVelocity": -0.5031162187791786, - "velocityX": -0.29457932723437075, - "velocityY": -1.908723060567117, - "timestamp": 0.6273701918748138 - }, - { - "x": 2.047740775609064, - "y": 4.675697634358311, - "heading": -0.2752320074015959, - "angularVelocity": -0.44826414362367223, - "velocityX": -0.00570091983015821, - "velocityY": -1.788750018800283, - "timestamp": 0.6778574791334666 - }, - { - "x": 2.0608710405337267, - "y": 4.5937072504707785, - "heading": -0.29463334748351117, - "angularVelocity": -0.3842816902108431, - "velocityX": 0.2600707155723077, - "velocityY": -1.62398077495204, - "timestamp": 0.7283447663921194 - }, - { - "x": 2.0861920181883997, - "y": 4.5217424317614965, - "heading": -0.3104741156384499, - "angularVelocity": -0.31375756185482273, - "velocityX": 0.5015317524380102, - "velocityY": -1.4254047427939316, - "timestamp": 0.7788320536507722 - }, - { - "x": 2.122546344803851, - "y": 4.461060494351438, - "heading": -0.3225286157246152, - "angularVelocity": -0.23876307761216561, - "velocityX": 0.7200689240680361, - "velocityY": -1.2019250925322578, - "timestamp": 0.829319340909425 - }, - { - "x": 2.1689002066225895, - "y": 4.412584630951134, - "heading": -0.3306480218329806, - "angularVelocity": -0.16082080359692716, - "velocityX": 0.9181293813878815, - "velocityY": -0.9601597953155385, - "timestamp": 0.8798066281680779 - }, - { - "x": 2.2243532862123785, - "y": 4.376998478332531, + "heading": -0.19963034769606672, + "angularVelocity": -0.5539756238745723, + "velocityX": -0.8806703803655747, + "velocityY": -1.9005657549777475, + "timestamp": 0.546266546873369 + }, + { + "x": 2.062901110669511, + "y": 4.862373019706677, + "heading": -0.22719939385993115, + "angularVelocity": -0.526195783497447, + "velocityX": -0.5790490938819385, + "velocityY": -1.9008063815925753, + "timestamp": 0.5986596808141655 + }, + { + "x": 2.0480285995415843, + "y": 4.7660067701880315, + "heading": -0.2526003669786383, + "angularVelocity": -0.4848149215011564, + "velocityX": -0.28386374338156817, + "velocityY": -1.8392915687680964, + "timestamp": 0.6510528147549621 + }, + { + "x": 2.0477407755531423, + "y": 4.675697634157938, + "heading": -0.27523200757833177, + "angularVelocity": -0.43195813835581237, + "velocityX": -0.005493544035129206, + "velocityY": -1.723682651473774, + "timestamp": 0.7034459486957587 + }, + { + "x": 2.060871040463269, + "y": 4.593707250314659, + "heading": -0.2946333476397457, + "angularVelocity": -0.37030310275649647, + "velocityX": 0.2506104125201524, + "velocityY": -1.5649070341149178, + "timestamp": 0.7558390826365553 + }, + { + "x": 2.0861920181034543, + "y": 4.521742431666527, + "heading": -0.3104741157540841, + "angularVelocity": -0.30234435169001433, + "velocityX": 0.48328809016841023, + "velocityY": -1.373554380798257, + "timestamp": 0.8082322165773519 + }, + { + "x": 2.122546344711596, + "y": 4.4610604943149115, + "heading": -0.32252861579276854, + "angularVelocity": -0.23007785814655887, + "velocityX": 0.6938757786320086, + "velocityY": -1.1582040009323555, + "timestamp": 0.8606253505181485 + }, + { + "x": 2.168900206537902, + "y": 4.412584630956035, + "heading": -0.3306480218589531, + "angularVelocity": -0.15497080352855533, + "velocityX": 0.8847316115635476, + "velocityY": -0.9252331309986794, + "timestamp": 0.9130184844589451 + }, + { + "x": 2.224353286156776, + "y": 4.376998478351871, "heading": -0.3347371673945201, - "angularVelocity": -0.0809935685510361, - "velocityX": 1.098357281620145, - "velocityY": -0.7048537275590006, - "timestamp": 0.9302939154267307 + "angularVelocity": -0.07804735521619326, + "velocityX": 1.0584035626029547, + "velocityY": -0.6792140482448454, + "timestamp": 0.9654116183997417 }, { "x": 2.2881293296813965, "y": 4.354815483093262, "heading": -0.3347371673945201, - "angularVelocity": 5.296142385201319e-27, - "velocityX": 1.2632099471355256, - "velocityY": -0.43937784031895555, - "timestamp": 0.9807812026853835 + "angularVelocity": 3.779464021226463e-27, + "velocityX": 1.2172595668105288, + "velocityY": -0.4233950823341772, + "timestamp": 1.0178047523405382 }, { - "x": 2.4097151173059284, - "y": 4.31443319431648, + "x": 2.4097151176038345, + "y": 4.314433194202397, "heading": -0.3347371673945201, - "angularVelocity": 4.260827770605265e-29, - "velocityX": 1.6917387541138205, - "velocityY": -0.5618772081689671, - "timestamp": 1.0526515120513356 + "angularVelocity": 6.573584275341322e-29, + "velocityX": 1.6302002618641742, + "velocityY": -0.5414384283676151, + "timestamp": 1.0923880969557587 }, { - "x": 2.501147235462863, - "y": 4.284859003573599, + "x": 2.5011472356587063, + "y": 4.284859003501582, "heading": -0.3347371673945201, - "angularVelocity": 4.260827800538464e-29, - "velocityX": 1.2721820590944748, - "velocityY": -0.41149385613875655, - "timestamp": 1.1245218214172876 + "angularVelocity": 6.573584272217567e-29, + "velocityX": 1.225905308035939, + "velocityY": -0.396525401929742, + "timestamp": 1.166971441570979 }, { - "x": 2.562139216145648, - "y": 4.265257651838079, + "x": 2.562139216220098, + "y": 4.265257651811236, "heading": -0.3347371673945201, - "angularVelocity": 4.260827801548558e-29, - "velocityX": 0.8486394621209058, - "velocityY": -0.27273225770760434, - "timestamp": 1.1963921307832397 + "angularVelocity": -6.199774132429429e-28, + "velocityX": 0.8177694480725244, + "velocityY": -0.2628113795576003, + "timestamp": 1.2415547861861995 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": 1.3873453317425255e-27, - "velocityX": 0.4244517046157105, - "velocityY": -0.13595549514112237, - "timestamp": 1.2682624401491918 + "angularVelocity": 7.63642932425348e-28, + "velocityX": 0.40901189675210486, + "velocityY": -0.1310099931742232, + "timestamp": 1.31613813080142 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": -3.25532193935464e-28, - "velocityX": 2.1483187881556724e-26, - "velocityY": -1.7096603516619137e-26, - "timestamp": 1.3401327495151438 + "angularVelocity": 1.6433960683327454e-29, + "velocityX": 1.171390593825864e-26, + "velocityY": -5.961445669521376e-27, + "timestamp": 1.3907214754166404 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePDEABC.traj b/src/main/deploy/choreo/CenterLanePDEABC.traj index bd4a810d..d390d04e 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.traj @@ -3,1784 +3,1856 @@ { "x": 1.3350272178649902, "y": 5.601006507873535, - "heading": 1.3464399983715043e-25, - "angularVelocity": 2.380601151660865e-26, - "velocityX": -3.6373884256645e-26, - "velocityY": 3.3932882505305813e-26, + "heading": 3.967978705267542e-25, + "angularVelocity": 7.379244608128328e-26, + "velocityX": 8.172146600807894e-26, + "velocityY": -1.420872098935511e-25, "timestamp": 0 }, { - "x": 1.3513037651461417, - "y": 5.614995878157814, - "heading": 0.006549885022004973, - "angularVelocity": 0.11118193396003606, - "velocityX": 0.27628851481066846, - "velocityY": 0.23746451088284293, - "timestamp": 0.058911414730016334 - }, - { - "x": 1.3843789066016257, - "y": 5.642351283609904, - "heading": 0.01935493583583066, - "angularVelocity": 0.21736111537143754, - "velocityX": 0.5614385871916894, - "velocityY": 0.4643481331666494, - "timestamp": 0.11782282946003267 - }, - { - "x": 1.4348748758418, - "y": 5.682261570187258, - "heading": 0.038033490581189856, - "angularVelocity": 0.31706172447157616, - "velocityX": 0.8571508505030987, - "velocityY": 0.6774627083776117, - "timestamp": 0.17673424419004902 - }, - { - "x": 1.503535032987812, - "y": 5.733639392412991, - "heading": 0.06208045701046981, - "angularVelocity": 0.408188574989825, - "velocityX": 1.1654813835429523, - "velocityY": 0.8721199866136559, - "timestamp": 0.23564565892006534 - }, - { - "x": 1.5912385139518157, - "y": 5.794977395067809, - "heading": 0.09080789039897333, - "angularVelocity": 0.48763781213127805, - "velocityX": 1.4887349313530773, - "velocityY": 1.041190454106761, - "timestamp": 0.29455707365008166 - }, - { - "x": 1.698981116852805, - "y": 5.864111533799201, - "heading": 0.12324878614145561, - "angularVelocity": 0.550672495154884, - "velocityX": 1.828891792783458, - "velocityY": 1.1735270498633465, - "timestamp": 0.353468488380098 - }, - { - "x": 1.8277399268235341, - "y": 5.937848492694958, - "heading": 0.15800938596992012, - "angularVelocity": 0.5900486346791701, - "velocityX": 2.1856343216474206, - "velocityY": 1.2516582606899718, - "timestamp": 0.4123799031101143 - }, - { - "x": 1.9780206143757966, - "y": 6.011501208422765, - "heading": 0.1930700705634149, - "angularVelocity": 0.5951424652450383, - "velocityX": 2.5509604249190097, - "velocityY": 1.2502282633229538, - "timestamp": 0.4712913178401306 - }, - { - "x": 2.1488551922208914, - "y": 6.0788007358405975, - "heading": 0.22564028873153014, - "angularVelocity": 0.5528676966489513, - "velocityX": 2.8998552933757997, - "velocityY": 1.1423851850487312, - "timestamp": 0.5302027325701469 + "x": 1.3486609048876415, + "y": 5.6125344531964885, + "heading": 0.0053952990645401815, + "angularVelocity": 0.09676162177278211, + "velocityX": 0.24451242670209422, + "velocityY": 0.20674714632375757, + "timestamp": 0.05575866718325111 + }, + { + "x": 1.3762911400128175, + "y": 5.635152571560341, + "heading": 0.015980040657686934, + "angularVelocity": 0.1898313235924372, + "velocityX": 0.4955325606038828, + "velocityY": 0.4056430956198756, + "timestamp": 0.11151733436650221 + }, + { + "x": 1.4183448247504244, + "y": 5.668306854106291, + "heading": 0.0314941432622812, + "angularVelocity": 0.2782366112447971, + "velocityX": 0.7542089304143026, + "velocityY": 0.594603210958904, + "timestamp": 0.1672760015497533 + }, + { + "x": 1.4753265664194932, + "y": 5.711278723492, + "heading": 0.05160335039247586, + "angularVelocity": 0.3606471988310923, + "velocityX": 1.0219351456482668, + "velocityY": 0.7706760501373088, + "timestamp": 0.22303466873300443 + }, + { + "x": 1.547831871808343, + "y": 5.763109379844883, + "heading": 0.07586790196130719, + "angularVelocity": 0.43517093923866185, + "velocityX": 1.3003414366157815, + "velocityY": 0.9295533586292857, + "timestamp": 0.27879333591625555 + }, + { + "x": 1.6365515186230641, + "y": 5.822478611934523, + "heading": 0.10369270639110653, + "angularVelocity": 0.499022050479668, + "velocityX": 1.5911364330704598, + "velocityY": 1.064753429175822, + "timestamp": 0.33455200309950667 + }, + { + "x": 1.7422414603963892, + "y": 5.887510284275809, + "heading": 0.1342490325748326, + "angularVelocity": 0.5480103404068561, + "velocityX": 1.8954890264140394, + "velocityY": 1.1663060762833288, + "timestamp": 0.3903106702827578 + }, + { + "x": 1.8655876183871098, + "y": 5.955479067462068, + "heading": 0.1663576179952195, + "angularVelocity": 0.57584922743691, + "velocityX": 2.212143227623125, + "velocityY": 1.2189814896916369, + "timestamp": 0.4460693374660089 + }, + { + "x": 2.0068104942426706, + "y": 6.022477551407248, + "heading": 0.1983353246164728, + "angularVelocity": 0.5735019905723076, + "velocityX": 2.532752000535291, + "velocityY": 1.201579724368033, + "timestamp": 0.50182800464926 + }, + { + "x": 2.164876110933553, + "y": 6.083435356761985, + "heading": 0.22790661577104523, + "angularVelocity": 0.5303442971006865, + "velocityX": 2.834816983185756, + "velocityY": 1.093243587663212, + "timestamp": 0.5575866718325111 }, { "x": 2.3368515968322754, "y": 6.133185386657715, "heading": 0.2525545797921912, - "angularVelocity": 0.456860375599634, - "velocityX": 3.1911711078905185, - "velocityY": 0.9231598165882628, - "timestamp": 0.5891141473001633 - }, - { - "x": 2.5473350968821267, - "y": 6.194075230145087, - "heading": 0.25255484664471606, - "angularVelocity": 0.0000044758460677296985, - "velocityX": 3.530383481883881, - "velocityY": 1.0212890683184164, - "timestamp": 0.6487347242516318 - }, - { - "x": 2.7606743654688723, - "y": 6.255791206318425, - "heading": 0.25255482885764124, - "angularVelocity": -2.983378514405921e-7, - "velocityX": 3.578282524176262, - "velocityY": 1.0351455710261694, - "timestamp": 0.7083553012031003 - }, - { - "x": 2.9740136340556216, - "y": 6.317507182491763, - "heading": 0.25255481107057365, - "angularVelocity": -2.9833773077503677e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.7679758781545688 - }, - { - "x": 3.187352902642371, - "y": 6.379223158665101, - "heading": 0.25255479328350594, - "angularVelocity": -2.9833773187848623e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.8275964551060373 - }, - { - "x": 3.40069217122912, - "y": 6.44093913483844, - "heading": 0.2525547754964383, - "angularVelocity": -2.983377307846883e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 0.8872170320575058 - }, - { - "x": 3.614031439815869, - "y": 6.502655111011779, - "heading": 0.25255475770937064, - "angularVelocity": -2.9833773102796393e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.9468376090089743 - }, - { - "x": 3.827370708402618, - "y": 6.564371087185117, - "heading": 0.25255473992230293, - "angularVelocity": -2.983377313896792e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.006458185960443 - }, - { - "x": 4.040709976989367, - "y": 6.626087063358456, - "heading": 0.2525547221352353, - "angularVelocity": -2.9833773109308854e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261857, - "timestamp": 1.0660787629119115 - }, - { - "x": 4.254049245576117, - "y": 6.6878030395317944, - "heading": 0.2525547043481677, - "angularVelocity": -2.983377300262124e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.12569933986338 - }, - { - "x": 4.467388514162867, - "y": 6.749519015705133, - "heading": 0.2525546865611001, - "angularVelocity": -2.9833773003639016e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261857, - "timestamp": 1.1853199168148487 - }, - { - "x": 4.680727782749616, - "y": 6.811234991878472, - "heading": 0.2525546687740325, - "angularVelocity": -2.9833772985092435e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.2449404937663173 - }, - { - "x": 4.894067051336365, - "y": 6.872950968051811, - "heading": 0.25255465098696483, - "angularVelocity": -2.9833773088728923e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.304561070717786 - }, - { - "x": 5.107406319923115, - "y": 6.934666944225149, - "heading": 0.2525546331998971, - "angularVelocity": -2.9833773138995595e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.3641816476692545 - }, - { - "x": 5.3207455885098645, - "y": 6.996382920398487, - "heading": 0.2525546154128296, - "angularVelocity": -2.9833772971803955e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.4238022246207231 - }, - { - "x": 5.534084857096614, - "y": 7.058098896571825, - "heading": 0.252554597625762, - "angularVelocity": -2.983377299147757e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.4834228015721918 + "angularVelocity": 0.44204722362068527, + "velocityX": 3.0842825803838596, + "velocityY": 0.8922385058492518, + "timestamp": 0.6133453390157622 + }, + { + "x": 2.5269005159766844, + "y": 6.188163800352406, + "heading": 0.252554892201178, + "angularVelocity": 0.000005553817937610827, + "velocityX": 3.378574691383036, + "velocityY": 0.9773729727982717, + "timestamp": 0.6695965504005877 + }, + { + "x": 2.7281832415838734, + "y": 6.246391992377104, + "heading": 0.2525548726806658, + "angularVelocity": -3.47023853848989e-7, + "velocityX": 3.578282505423293, + "velocityY": 1.0351455656012056, + "timestamp": 0.7258477617854133 + }, + { + "x": 2.929465967191096, + "y": 6.304620184401812, + "heading": 0.2525548531602303, + "angularVelocity": -3.4702249119339613e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 0.7820989731702389 + }, + { + "x": 3.1307486927983192, + "y": 6.36284837642652, + "heading": 0.25255483363979486, + "angularVelocity": -3.4702249016578237e-7, + "velocityX": 3.5782825054238923, + "velocityY": 1.0351455656013788, + "timestamp": 0.8383501845550645 + }, + { + "x": 3.332031418405542, + "y": 6.421076568451228, + "heading": 0.2525548141193594, + "angularVelocity": -3.470224908197631e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 0.8946013959398901 + }, + { + "x": 3.5333141440127647, + "y": 6.4793047604759355, + "heading": 0.2525547945989239, + "angularVelocity": -3.4702249080835237e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 0.9508526073247157 + }, + { + "x": 3.7345968696199874, + "y": 6.537532952500643, + "heading": 0.2525547750784885, + "angularVelocity": -3.470224894869701e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 1.0071038187095414 + }, + { + "x": 3.93587959522721, + "y": 6.595761144525352, + "heading": 0.25255475555805307, + "angularVelocity": -3.470224898418443e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 1.063355030094367 + }, + { + "x": 4.137162320834433, + "y": 6.65398933655006, + "heading": 0.2525547360376176, + "angularVelocity": -3.470224909887922e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.1196062414791927 + }, + { + "x": 4.338445046441655, + "y": 6.712217528574768, + "heading": 0.2525547165171822, + "angularVelocity": -3.4702248928646837e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 1.1758574528640184 + }, + { + "x": 4.539727772048877, + "y": 6.770445720599476, + "heading": 0.25255469699674676, + "angularVelocity": -3.470224898754089e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 1.2321086642488441 + }, + { + "x": 4.7410104976561, + "y": 6.8286739126241836, + "heading": 0.25255467747631133, + "angularVelocity": -3.470224907627516e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.2883598756336698 + }, + { + "x": 4.942293223263322, + "y": 6.886902104648891, + "heading": 0.25255465795587584, + "angularVelocity": -3.4702249046766804e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013786, + "timestamp": 1.3446110870184955 + }, + { + "x": 5.143575948870544, + "y": 6.945130296673599, + "heading": 0.2525546384354404, + "angularVelocity": -3.47022489425383e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.4008622984033212 + }, + { + "x": 5.344858674477766, + "y": 7.003358488698307, + "heading": 0.25255461891500497, + "angularVelocity": -3.470224896167856e-7, + "velocityX": 3.578282505423892, + "velocityY": 1.0351455656013788, + "timestamp": 1.457113509788147 + }, + { + "x": 5.546141400084989, + "y": 7.061586680723015, + "heading": 0.25255459939456953, + "angularVelocity": -3.470224897168836e-7, + "velocityX": 3.5782825054238914, + "velocityY": 1.0351455656013788, + "timestamp": 1.5133647211729726 }, { "x": 5.747424125671387, "y": 7.119814872741699, "heading": 0.2525545797921912, - "angularVelocity": -2.9911771592583556e-7, - "velocityX": 3.5782825239754317, - "velocityY": 1.035145570968072, - "timestamp": 1.5430433785236604 - }, - { - "x": 5.874238789453484, - "y": 7.151633771807316, - "heading": 0.23428972710167786, - "angularVelocity": -0.5023194013498574, - "velocityX": 3.4876528747747586, - "velocityY": 0.8750823563200211, - "timestamp": 1.579404412243304 - }, - { - "x": 6.000467771954814, - "y": 7.182688874971163, - "heading": 0.21343765731660022, - "angularVelocity": -0.5734729641036735, - "velocityX": 3.471545486704275, - "velocityY": 0.8540764655727967, - "timestamp": 1.6157654459629476 - }, - { - "x": 6.126526412325093, - "y": 7.213769689844044, - "heading": 0.19236113767335114, - "angularVelocity": -0.5796457770083356, - "velocityX": 3.4668607428005407, - "velocityY": 0.8547835881819195, - "timestamp": 1.6521264796825912 - }, - { - "x": 6.252399106037461, - "y": 7.244861395063954, - "heading": 0.17099373366442527, - "angularVelocity": -0.5876456696384406, - "velocityX": 3.4617468436923633, - "velocityY": 0.8550830941616943, - "timestamp": 1.6884875134022348 - }, - { - "x": 6.378079158099059, - "y": 7.275966592245878, - "heading": 0.1493195497494227, - "angularVelocity": -0.596082720918173, - "velocityX": 3.456448819102211, - "velocityY": 0.8554541496744962, - "timestamp": 1.7248485471218784 - }, - { - "x": 6.503558906617952, - "y": 7.3070873984764315, - "heading": 0.12731920059935156, - "angularVelocity": -0.6050529069030769, - "velocityX": 3.4509400774022723, - "velocityY": 0.8558834292365232, - "timestamp": 1.761209580841522 - }, - { - "x": 6.628826049826062, - "y": 7.3382208951944605, - "heading": 0.10495362803069447, - "angularVelocity": -0.6150972698164627, - "velocityX": 3.44509301286545, - "velocityY": 0.8562324426219444, - "timestamp": 1.7975706145611656 - }, - { - "x": 6.7538656809780315, - "y": 7.369363297624379, - "heading": 0.08217584529224368, - "angularVelocity": -0.6264338608763309, - "velocityX": 3.438835983489081, - "velocityY": 0.8564773672288275, - "timestamp": 1.8339316482808092 - }, - { - "x": 6.8786958169095165, - "y": 7.400552514729427, - "heading": 0.05908376270904082, - "angularVelocity": -0.6350777252718116, - "velocityX": 3.4330744525573853, - "velocityY": 0.8577648629444327, - "timestamp": 1.8702926820004528 - }, - { - "x": 7.003224052672837, - "y": 7.431696922152255, - "heading": 0.03530389927777457, - "angularVelocity": -0.6539930524147748, - "velocityX": 3.4247716036754445, - "velocityY": 0.8565325084804077, - "timestamp": 1.9066537157200965 + "angularVelocity": -3.4847922161224964e-7, + "velocityX": 3.5782825050536915, + "velocityY": 1.0351455654942852, + "timestamp": 1.5696159325577983 + }, + { + "x": 5.864122316952898, + "y": 7.149764367678907, + "heading": 0.23825610881476394, + "angularVelocity": -0.4289921260648423, + "velocityX": 3.501255852098466, + "velocityY": 0.8985644358731831, + "timestamp": 1.6029463134871351 + }, + { + "x": 5.980010019619152, + "y": 7.178464848632163, + "heading": 0.21993450293637717, + "angularVelocity": -0.5496968641681629, + "velocityX": 3.4769390398491176, + "velocityY": 0.8610906972261488, + "timestamp": 1.636276694416472 + }, + { + "x": 6.095759168980239, + "y": 7.207171809244793, + "heading": 0.2014022591938407, + "angularVelocity": -0.5560165598414963, + "velocityX": 3.4727820725027265, + "velocityY": 0.8612851042264353, + "timestamp": 1.6696070753458088 + }, + { + "x": 6.211373074459108, + "y": 7.235898623678772, + "heading": 0.18268720108591044, + "angularVelocity": -0.5615014766140173, + "velocityX": 3.4687243966392267, + "velocityY": 0.8618807716264242, + "timestamp": 1.7029374562751456 + }, + { + "x": 6.326838442963843, + "y": 7.264631799858999, + "heading": 0.16373191419922642, + "angularVelocity": -0.5687089783603383, + "velocityX": 3.464267892693179, + "velocityY": 0.8620716409195424, + "timestamp": 1.7362678372044824 + }, + { + "x": 6.442151068511899, + "y": 7.29337427879422, + "heading": 0.1445283357059787, + "angularVelocity": -0.5761583863671085, + "velocityX": 3.4596851980938594, + "velocityY": 0.8623507482904867, + "timestamp": 1.7695982181338192 + }, + { + "x": 6.557303014616615, + "y": 7.322124225476267, + "heading": 0.1250513432276187, + "angularVelocity": -0.5843615324905178, + "velocityX": 3.454864387804252, + "velocityY": 0.8625748005400329, + "timestamp": 1.802928599063156 + }, + { + "x": 6.672285593632433, + "y": 7.350880202549992, + "heading": 0.10527449948643015, + "angularVelocity": -0.5933578671998123, + "velocityX": 3.449782925061388, + "velocityY": 0.8627557283156009, + "timestamp": 1.8362589799924929 + }, + { + "x": 6.787113803468559, + "y": 7.379671810584065, + "heading": 0.08527801395894485, + "angularVelocity": -0.5999477044645701, + "velocityX": 3.4451514394501603, + "velocityY": 0.8638247518116766, + "timestamp": 1.8695893609218297 + }, + { + "x": 6.901739724666763, + "y": 7.408448228453197, + "heading": 0.0648615716893241, + "angularVelocity": -0.6125475227212472, + "velocityX": 3.4390822427508825, + "velocityY": 0.8633690064971079, + "timestamp": 1.9029197418511665 + }, + { + "x": 7.014861438150348, + "y": 7.4357314738869205, + "heading": 0.038575820671149744, + "angularVelocity": -0.7886423822728691, + "velocityX": 3.3939520140322568, + "velocityY": 0.8185698654799702, + "timestamp": 1.9362501227805033 }, { "x": 7.125290870666504, "y": 7.458, - "heading": -8.902558475497447e-21, - "angularVelocity": -0.9709267219953129, - "velocityX": 3.3570777699788383, - "velocityY": 0.7233864155389959, - "timestamp": 1.94301474943974 - }, - { - "x": 7.338537269448379, - "y": 7.482023887493991, - "heading": -0.008039547554639858, - "angularVelocity": -0.1278866817794853, - "velocityX": 3.39215287381437, - "velocityY": 0.38215275600640763, - "timestamp": 2.0058793686696177 - }, - { - "x": 7.527981936001141, - "y": 7.500234795903286, - "heading": -0.013861641566031323, - "angularVelocity": -0.09261320728121766, - "velocityX": 3.013533985786477, - "velocityY": 0.2896845416768293, - "timestamp": 2.068743987899495 - }, - { - "x": 7.693596991003447, - "y": 7.512743640283509, - "heading": -0.017657163586421762, - "angularVelocity": -0.060376123595234456, - "velocityX": 2.6344716158496166, - "velocityY": 0.1989806751947781, - "timestamp": 2.1316086071293725 - }, - { - "x": 7.835373269936495, - "y": 7.519587451824941, - "heading": -0.01948976746028864, - "angularVelocity": -0.02915159427221823, - "velocityX": 2.255263464089624, - "velocityY": 0.10886587122090353, - "timestamp": 2.19447322635925 - }, - { - "x": 7.953306214016821, - "y": 7.520784758027623, - "heading": -0.01939144141260055, - "angularVelocity": 0.0015640919947123182, - "velocityX": 1.875982794854424, - "velocityY": 0.01904578787480972, - "timestamp": 2.2573378455891273 - }, - { - "x": 8.04739309462225, - "y": 7.516346680416715, - "heading": -0.01738162754383806, - "angularVelocity": 0.031970508902840686, - "velocityX": 1.4966587208836901, - "velocityY": -0.07059738315886452, - "timestamp": 2.3202024648190047 - }, - { - "x": 8.117632094556283, - "y": 7.5062806367543455, - "heading": -0.0134735913597302, - "angularVelocity": 0.06216590877322135, - "velocityX": 1.1173057404068745, - "velocityY": -0.16012255837517314, - "timestamp": 2.383067084048882 - }, - { - "x": 8.164021915899584, - "y": 7.490591928437994, - "heading": -0.007677144931297963, - "angularVelocity": 0.09220522607854079, - "velocityX": 0.7379321136689944, - "velocityY": -0.24956340320748793, - "timestamp": 2.4459317032787595 + "heading": 1.0084355524659913e-21, + "angularVelocity": -1.1573771314805474, + "velocityX": 3.313176430544709, + "velocityY": 0.668114959750724, + "timestamp": 1.9695805037098402 + }, + { + "x": 7.337738270409439, + "y": 7.481917447620179, + "heading": -0.00819620478546554, + "angularVelocity": -0.12615227280600286, + "velocityX": 3.2698941803922152, + "velocityY": 0.368126523919295, + "timestamp": 2.0345512299849795 + }, + { + "x": 7.526586631074925, + "y": 7.500039280530163, + "heading": -0.014041404226933134, + "angularVelocity": -0.08996666308937673, + "velocityX": 2.906668456586843, + "velocityY": 0.27892304656162725, + "timestamp": 2.099521956260119 + }, + { + "x": 7.6918053440422804, + "y": 7.512484823370471, + "heading": -0.017813194168849914, + "angularVelocity": -0.058053682914731695, + "velocityX": 2.5429716187515523, + "velocityY": 0.19155616004049963, + "timestamp": 2.164492682535258 + }, + { + "x": 7.833384352673449, + "y": 7.519294018241946, + "heading": -0.01960436856302971, + "angularVelocity": -0.027568945229186584, + "velocityX": 2.179119993696969, + "velocityY": 0.10480404424970338, + "timestamp": 2.2294634088103975 + }, + { + "x": 7.951318655599417, + "y": 7.520486868366768, + "heading": -0.019461534286698053, + "angularVelocity": 0.0021984405057560424, + "velocityX": 1.815191389835117, + "velocityY": 0.018359808997220107, + "timestamp": 2.294434135085537 + }, + { + "x": 8.045605259786509, + "y": 7.516075387181805, + "heading": -0.017412919986794666, + "angularVelocity": 0.031531343688969565, + "velocityX": 1.4512167185542217, + "velocityY": -0.06789952087470152, + "timestamp": 2.359404861360676 + }, + { + "x": 8.116242172260733, + "y": 7.5060675897274995, + "heading": -0.013477652900012242, + "angularVelocity": 0.06056984910584597, + "velocityX": 1.0872113723200465, + "velocityY": -0.15403548687333402, + "timestamp": 2.4243755876358155 + }, + { + "x": 8.163227969814436, + "y": 7.490469205298236, + "heading": -0.007669730702440132, + "angularVelocity": 0.08939290863052056, + "velocityX": 0.723184120718108, + "velocityY": -0.24008327016705255, + "timestamp": 2.489346313910955 }, { "x": 8.186561584472656, "y": 7.469284534454346, - "heading": 2.2979832887228624e-21, - "angularVelocity": 0.12212187117884055, - "velocityX": 0.358542990464167, - "velocityY": -0.33894095350730474, - "timestamp": 2.508796322508637 - }, - { - "x": 8.169433855540824, - "y": 7.426853893988931, - "heading": 0.014976899045789983, - "angularVelocity": 0.16512821061392627, - "velocityX": -0.18884224442902273, - "velocityY": -0.46782018853404506, - "timestamp": 2.599494931729361 - }, - { - "x": 8.102655131439361, - "y": 7.372743106744783, - "heading": 0.03346799984598468, - "angularVelocity": 0.20387413830342968, - "velocityX": -0.7362706515041357, - "velocityY": -0.5965999667366941, - "timestamp": 2.6901935409500854 - }, - { - "x": 7.986220220035368, - "y": 7.306964887600374, - "heading": 0.05493172658414638, - "angularVelocity": 0.23664890699621968, - "velocityX": -1.2837563045827718, - "velocityY": -0.7252395566985038, - "timestamp": 2.7808921501708097 - }, - { - "x": 7.820121969665864, - "y": 7.229538517591083, - "heading": 0.07855370015299508, - "angularVelocity": 0.26044471653762835, - "velocityX": -1.8313208085174242, - "velocityY": -0.8536665630766948, - "timestamp": 2.871590759391534 - }, - { - "x": 7.6043501738014525, - "y": 7.14049675145262, - "heading": 0.10296815130417637, - "angularVelocity": 0.2691821998258671, - "velocityX": -2.3789978448215092, - "velocityY": -0.9817324312192064, - "timestamp": 2.9622893686122582 - }, - { - "x": 7.338891049230189, - "y": 7.039907645727491, - "heading": 0.1254011174832251, - "angularVelocity": 0.24733528299707236, - "velocityX": -2.9268268483063693, - "velocityY": -1.1090479400884177, - "timestamp": 3.0529879778329825 - }, - { - "x": 7.023762471374015, - "y": 6.927998022527093, - "heading": 0.1370927000479183, - "angularVelocity": 0.12890586377394736, - "velocityX": -3.4744587658370385, - "velocityY": -1.2338626155562706, - "timestamp": 3.1436865870537067 - }, - { - "x": 6.694988070852044, - "y": 6.85020567874946, - "heading": 0.13709270182779565, - "angularVelocity": 1.9624086279332165e-8, - "velocityX": -3.624911157368082, - "velocityY": -0.8577016168827462, - "timestamp": 3.234385196274431 - }, - { - "x": 6.366213601226777, - "y": 6.7724136270242035, - "heading": 0.13709270360766948, - "angularVelocity": 1.962404782489554e-8, - "velocityX": -3.624911919268354, - "velocityY": -0.8576983968512897, - "timestamp": 3.3250838054951553 - }, - { - "x": 6.0374391323544945, - "y": 6.69462157211659, - "heading": 0.13709270538754328, - "angularVelocity": 1.962404757872295e-8, - "velocityX": -3.6249119109663046, - "velocityY": -0.857698431938445, - "timestamp": 3.4157824147158795 - }, - { - "x": 5.708664663238629, - "y": 6.616829518238441, - "heading": 0.13709270716741703, - "angularVelocity": 1.9624046615308717e-8, - "velocityX": -3.6249119136519434, - "velocityY": -0.8576984205880644, - "timestamp": 3.5064810239366038 - }, - { - "x": 5.3798901940960535, - "y": 6.539037464473215, - "heading": 0.13709270894730027, - "angularVelocity": 1.9624151634826213e-8, - "velocityX": -3.6249119139464265, - "velocityY": -0.8576984193430164, - "timestamp": 3.597179633157328 - }, - { - "x": 5.082161936855116, - "y": 6.468591654596992, - "heading": 0.17334127091967155, - "angularVelocity": 0.3996595127953577, - "velocityX": -3.2826110543369467, - "velocityY": -0.7767022061472455, - "timestamp": 3.6878782423780523 - }, - { - "x": 4.834055003719788, - "y": 6.409886994261807, - "heading": 0.20354833113507975, - "angularVelocity": 0.33304876971041714, - "velocityX": -2.735509786390822, - "velocityY": -0.647249840318068, - "timestamp": 3.7785768515987765 - }, - { - "x": 4.635569429511693, - "y": 6.362923375586849, - "heading": 0.22771434771261193, - "angularVelocity": 0.2664430776300183, - "velocityX": -2.1884081345179167, - "velocityY": -0.5177986639317362, - "timestamp": 3.869275460819501 - }, - { - "x": 4.486705235532264, - "y": 6.327700716475024, - "heading": 0.24583917728696456, - "angularVelocity": 0.19983580487153885, - "velocityX": -1.6413062477854834, - "velocityY": -0.38834839270916643, - "timestamp": 3.959974070040225 - }, - { - "x": 4.387462434805768, - "y": 6.304218961949016, - "heading": 0.25792250356094953, - "angularVelocity": 0.13322504476974903, - "velocityX": -1.0942042174536384, - "velocityY": -0.25889872764050514, - "timestamp": 4.050672679260949 + "heading": 7.422813602625121e-23, + "angularVelocity": 0.11804902210820577, + "velocityX": 0.3591404313291331, + "velocityY": -0.3260648611834275, + "timestamp": 2.554317040186094 + }, + { + "x": 8.171272340508109, + "y": 7.427332842662171, + "heading": 0.014840203267270782, + "angularVelocity": 0.15900903302410063, + "velocityX": -0.16382039077820704, + "velocityY": -0.44950179087577585, + "timestamp": 2.6476463497544476 + }, + { + "x": 8.107175512886487, + "y": 7.3738609015025816, + "heading": 0.03350091514476021, + "angularVelocity": 0.1999448186619506, + "velocityX": -0.6867813328746101, + "velocityY": -0.572938355666582, + "timestamp": 2.7409756593228005 + }, + { + "x": 7.994271087246123, + "y": 7.308868753063173, + "heading": 0.05597944584590409, + "angularVelocity": 0.24085178391554368, + "velocityX": -1.2097424288526986, + "velocityY": -0.6963744694994183, + "timestamp": 2.8343049688911535 + }, + { + "x": 7.832559046421551, + "y": 7.232356448582914, + "heading": 0.08227288434163177, + "angularVelocity": 0.2817275582272545, + "velocityX": -1.732703708754388, + "velocityY": -0.8198100343196232, + "timestamp": 2.9276342784595064 + }, + { + "x": 7.6220393708156715, + "y": 7.144324050552095, + "heading": 0.1123783464235267, + "angularVelocity": 0.3225724289736236, + "velocityX": -2.2556651986340626, + "velocityY": -0.9432449295721523, + "timestamp": 3.0209635880278594 + }, + { + "x": 7.362712038546165, + "y": 7.04477163823656, + "heading": 0.14629325961596634, + "angularVelocity": 0.36338973629287186, + "velocityX": -2.778626922977279, + "velocityY": -1.0666789755111628, + "timestamp": 3.1142928975962123 + }, + { + "x": 7.054577023664908, + "y": 6.933699333854541, + "heading": 0.18401565672002676, + "angularVelocity": 0.4041859655720804, + "velocityX": -3.3015889253480717, + "velocityY": -1.1901117119126619, + "timestamp": 3.2076222071645653 + }, + { + "x": 6.71622230121249, + "y": 6.85383794706396, + "heading": 0.18401565782506035, + "angularVelocity": 1.1840156036202986e-8, + "velocityX": -3.6253854659088836, + "velocityY": -0.8556946061203927, + "timestamp": 3.300951516732918 + }, + { + "x": 6.377867419030211, + "y": 6.7739772370157985, + "heading": 0.1840156589300666, + "angularVelocity": 1.1839863049890439e-8, + "velocityX": -3.6253871773740243, + "velocityY": -0.8556873549961612, + "timestamp": 3.394280826301271 + }, + { + "x": 6.0395125368493074, + "y": 6.694116526961809, + "heading": 0.18401566003507294, + "angularVelocity": 1.1839864253851599e-8, + "velocityX": -3.6253871773592854, + "velocityY": -0.8556873550586043, + "timestamp": 3.487610135869624 + }, + { + "x": 5.701157654668755, + "y": 6.614255816906336, + "heading": 0.18401566114007933, + "angularVelocity": 1.18398644204111e-8, + "velocityX": -3.625387177355538, + "velocityY": -0.855687355074483, + "timestamp": 3.580939445437977 + }, + { + "x": 5.362802772497576, + "y": 6.534395106811441, + "heading": 0.18401566224512567, + "angularVelocity": 1.1840292694239514e-8, + "velocityX": -3.6253871772550927, + "velocityY": -0.8556873554969, + "timestamp": 3.67426875500633 + }, + { + "x": 5.069956587719004, + "y": 6.4652758877562615, + "heading": 0.20685757815363412, + "angularVelocity": 0.24474536471074324, + "velocityX": -3.137772968995303, + "velocityY": -0.7405949896646158, + "timestamp": 3.767598064574683 + }, + { + "x": 4.8259180814209035, + "y": 6.407676584765678, + "heading": 0.22589278292875978, + "angularVelocity": 0.20395741555534105, + "velocityX": -2.614811010890098, + "velocityY": -0.6171619961293916, + "timestamp": 3.860927374143036 + }, + { + "x": 4.6306872671740225, + "y": 6.361597170610555, + "heading": 0.2411211921270372, + "angularVelocity": 0.16316856160951593, + "velocityX": -2.0918489073777633, + "velocityY": -0.49372929434751617, + "timestamp": 3.954256683711389 + }, + { + "x": 4.484264152022265, + "y": 6.3270376241371595, + "heading": 0.25254264332581844, + "angularVelocity": 0.12237796734600637, + "velocityX": -1.5688867283917882, + "velocityY": -0.37029681922252144, + "timestamp": 4.047585993279742 + }, + { + "x": 4.386648740184571, + "y": 6.303997931141046, + "heading": 0.26015698140700544, + "angularVelocity": 0.08158571103122086, + "velocityX": -1.0459245042009158, + "velocityY": -0.24686449629458126, + "timestamp": 4.140915302848095 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": 0.06661212070370816, - "velocityX": -0.54710211431646, - "velocityY": -0.12944936516318697, - "timestamp": 4.141371288481674 + "angularVelocity": 0.04079263927507296, + "velocityX": -0.5229622556382164, + "velocityY": -0.12343224899141036, + "timestamp": 4.234244612416448 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": 4.2322185448004084e-23, - "velocityX": 1.750098119560174e-21, - "velocityY": 1.2842584640589406e-21, - "timestamp": 4.232069897702398 - }, - { - "x": 4.357336303204376, - "y": 6.297583237177048, - "heading": 0.24752122945545674, - "angularVelocity": -0.28561960466886005, - "velocityX": 0.3386404361144792, - "velocityY": 0.08867849340285801, - "timestamp": 4.289639128041561 - }, - { - "x": 4.396392552566799, - "y": 6.3076902844788645, - "heading": 0.21545182139905172, - "angularVelocity": -0.5570581344838518, - "velocityX": 0.6784222949017583, - "velocityY": 0.17556335636714013, - "timestamp": 4.3472083583807235 - }, - { - "x": 4.455088009056865, - "y": 6.322669909558741, - "heading": 0.16876627146696008, - "angularVelocity": -0.8109462234782202, - "velocityX": 1.0195629878021513, - "velocityY": 0.26020193411697123, - "timestamp": 4.404777588719886 - }, - { - "x": 4.533516711715102, - "y": 6.342355579726992, - "heading": 0.10873908045256343, - "angularVelocity": -1.042695736259688, - "velocityX": 1.3623371755394689, - "velocityY": 0.3419477740500502, - "timestamp": 4.462346819059049 - }, - { - "x": 4.631792826161861, - "y": 6.366523725829109, - "heading": 0.03701265289139425, - "angularVelocity": -1.2459160412359371, - "velocityX": 1.7070944646606674, - "velocityY": 0.4198101305112548, - "timestamp": 4.519916049398212 - }, - { - "x": 4.750056312234907, - "y": 6.394856813133717, - "heading": -0.04421826686802747, - "angularVelocity": -1.4110127802796573, - "velocityX": 2.054282910789491, - "velocityY": 0.492156784756161, - "timestamp": 4.577485279737375 - }, - { - "x": 4.888478272202705, - "y": 6.42687003155547, - "heading": -0.13181310680866934, - "angularVelocity": -1.5215565576365515, - "velocityX": 2.4044434701020125, - "velocityY": 0.5560820985993948, - "timestamp": 4.635054510076538 - }, - { - "x": 5.047249483224003, - "y": 6.461748830996926, - "heading": -0.22073093757153267, - "angularVelocity": -1.5445374245758388, - "velocityX": 2.757917903121444, - "velocityY": 0.6058583593348625, - "timestamp": 4.6926237404157005 - }, - { - "x": 5.226429045018527, - "y": 6.497908973529479, - "heading": -0.30132047165929504, - "angularVelocity": -1.399871660833024, - "velocityX": 3.1124189213388544, - "velocityY": 0.6281157889295987, - "timestamp": 4.750192970754863 - }, - { - "x": 5.423903541491299, - "y": 6.531166091125145, - "heading": -0.3435649109260108, - "angularVelocity": -0.7338023978753491, - "velocityX": 3.430209077129816, - "velocityY": 0.5776891127384427, - "timestamp": 4.807762201094026 - }, - { - "x": 5.636616267297259, - "y": 6.55438559892435, - "heading": -0.3449872384479477, - "angularVelocity": -0.024706384184006707, - "velocityX": 3.6949030680588684, - "velocityY": 0.40333191294048215, - "timestamp": 4.865331431433189 + "angularVelocity": -3.1350638097986163e-24, + "velocityX": 6.734301517773745e-23, + "velocityY": -5.476052050130636e-23, + "timestamp": 4.327573921984801 + }, + { + "x": 4.357085465154312, + "y": 6.298335605731047, + "heading": 0.24955697801054222, + "angularVelocity": -0.24189458757929325, + "velocityX": 0.32311199814604374, + "velocityY": 0.09834717102755294, + "timestamp": 4.387133552481924 + }, + { + "x": 4.395672489468794, + "y": 6.30982325311387, + "heading": 0.2214506254853789, + "angularVelocity": -0.4719027349661102, + "velocityX": 0.6478721239942183, + "velocityY": 0.19287640448638915, + "timestamp": 4.446693182979047 + }, + { + "x": 4.453716686931878, + "y": 6.3266599763174565, + "heading": 0.18050722178606005, + "angularVelocity": -0.6874354887291741, + "velocityX": 0.9745560370104606, + "velocityY": 0.28268683104740583, + "timestamp": 4.50625281347617 + }, + { + "x": 4.531352614071991, + "y": 6.348490007100106, + "heading": 0.12779504929109745, + "angularVelocity": -0.8850318924914878, + "velocityX": 1.3034991401409508, + "velocityY": 0.366523945841208, + "timestamp": 4.565812443973293 + }, + { + "x": 4.628738799037524, + "y": 6.374849024990675, + "heading": 0.0646674309135714, + "angularVelocity": -1.0599061453306917, + "velocityX": 1.6351039143911004, + "velocityY": 0.4425651682281852, + "timestamp": 4.625372074470416 + }, + { + "x": 4.746060953896417, + "y": 6.4051070852453105, + "heading": -0.0071047467810986895, + "angularVelocity": -1.2050473969635673, + "velocityX": 1.9698267749421239, + "velocityY": 0.5080296838996932, + "timestamp": 4.684931704967539 + }, + { + "x": 4.883529200739737, + "y": 6.4383664128247835, + "heading": -0.08508741675594196, + "angularVelocity": -1.30932091626408, + "velocityX": 2.3080775635429776, + "velocityY": 0.5584206500589359, + "timestamp": 4.744491335464662 + }, + { + "x": 5.041348217191184, + "y": 6.473264113474332, + "heading": -0.1656919745439482, + "angularVelocity": -1.3533421398895296, + "velocityX": 2.6497648681529427, + "velocityY": 0.5859287634639322, + "timestamp": 4.804050965961785 + }, + { + "x": 5.219569793721533, + "y": 6.507557049328585, + "heading": -0.2431024824486723, + "angularVelocity": -1.29971437462936, + "velocityX": 2.99232172937938, + "velocityY": 0.575774825465208, + "timestamp": 4.8636105964589085 + }, + { + "x": 5.417370624809519, + "y": 6.5372140981281, + "heading": -0.30621328049041796, + "angularVelocity": -1.0596237336427745, + "velocityX": 3.3210553765530904, + "velocityY": 0.49793876409201393, + "timestamp": 4.923170226956032 + }, + { + "x": 5.630089765221455, + "y": 6.556096742386712, + "heading": -0.3316159113808185, + "angularVelocity": -0.4265075299892522, + "velocityX": 3.5715322381358874, + "velocityY": 0.31703763272213936, + "timestamp": 4.982729857453155 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -0.3449872632321153, - "angularVelocity": -4.3051066415640403e-7, - "velocityX": 3.7246964993017384, - "velocityY": 0.047619187556732175, - "timestamp": 4.922900661772352 - }, - { - "x": 5.9707478293428995, - "y": 6.552244535149851, - "heading": -0.34498728566747394, - "angularVelocity": -6.975751659806049e-7, - "velocityX": 3.7219059383782973, - "velocityY": -0.151808826449267, - "timestamp": 4.9550625845387355 - }, - { - "x": 6.090018567999844, - "y": 6.540962083011693, - "heading": -0.3449873062012292, - "angularVelocity": -6.384492454517859e-7, - "velocityX": 3.7084455280643134, - "velocityY": -0.3508015431822894, - "timestamp": 4.987224507305119 - }, - { - "x": 6.208514463680753, - "y": 6.523311991997161, - "heading": -0.3449873252853557, - "angularVelocity": -5.933764159352289e-7, - "velocityX": 3.6843535923407127, - "velocityY": -0.5487884273193968, - "timestamp": 5.019386430071503 - }, - { - "x": 6.325895808833114, - "y": 6.499344869625537, - "heading": -0.34498734327239833, - "angularVelocity": -5.592651525230829e-7, - "velocityX": 3.6496992423305046, - "velocityY": -0.7452017886404377, - "timestamp": 5.051548352837886 - }, - { - "x": 6.441826094112058, - "y": 6.46912943973789, - "heading": -0.34498736044751604, - "angularVelocity": -5.340202407325154e-7, - "velocityX": 3.6045819188434196, - "velocityY": -0.9394783423592116, - "timestamp": 5.08371027560427 - }, - { - "x": 6.555972977983021, - "y": 6.432752355963861, - "heading": -0.3449873770504224, - "angularVelocity": -5.162286594877017e-7, - "velocityX": 3.5491312102233143, - "velocityY": -1.131060603505082, - "timestamp": 5.115872198370654 - }, - { - "x": 6.66800926163723, - "y": 6.390318000635929, - "heading": -0.3449873932911425, - "angularVelocity": -5.049673254841841e-7, - "velocityX": 3.4835070175379053, - "velocityY": -1.319397339399292, - "timestamp": 5.148034121137037 - }, - { - "x": 6.777614079975414, - "y": 6.34194872129089, - "heading": -0.344987409362088, - "angularVelocity": -4.996885790126847e-7, - "velocityX": 3.407906272716571, - "velocityY": -1.5039299639011887, - "timestamp": 5.180196043903421 - }, - { - "x": 6.886436638977974, - "y": 6.291844289930693, - "heading": -0.3449874254080791, - "angularVelocity": -4.98912679463648e-7, - "velocityX": 3.3835837425834847, - "velocityY": -1.557880470149199, - "timestamp": 5.2123579666698046 - }, - { - "x": 6.995259181110964, - "y": 6.241739821931245, - "heading": -0.34498744145406995, - "angularVelocity": -4.989126736688575e-7, - "velocityX": 3.383583218063541, - "velocityY": -1.5578816093613586, - "timestamp": 5.244519889436188 - }, - { - "x": 7.104082434838663, - "y": 6.191636899482441, - "heading": -0.34498745750007104, - "angularVelocity": -4.98912992332448e-7, - "velocityX": 3.3836053434418156, - "velocityY": -1.5578335540676167, - "timestamp": 5.276681812202572 + "heading": -0.3344741491569628, + "angularVelocity": -0.04798951491618805, + "velocityX": 3.7098016045994533, + "velocityY": 0.017297899702458933, + "timestamp": 5.042289487950278 + }, + { + "x": 5.971113356144086, + "y": 6.5517128681904815, + "heading": -0.33447429817496127, + "angularVelocity": -0.000004618408354033321, + "velocityX": 3.7212182481092846, + "velocityY": -0.167796284707936, + "timestamp": 5.074555582043538 + }, + { + "x": 6.090763237120761, + "y": 6.540318338833946, + "heading": -0.33447432111787584, + "angularVelocity": -7.11053358628561e-7, + "velocityX": 3.70822327086892, + "velocityY": -0.3531425069176937, + "timestamp": 5.106821676136798 + }, + { + "x": 6.209696100342635, + "y": 6.522971764048271, + "heading": -0.3344743416762489, + "angularVelocity": -6.371509664508709e-7, + "velocityX": 3.686001251905956, + "velocityY": -0.537609998146539, + "timestamp": 5.139087770230058 + }, + { + "x": 6.327616045254045, + "y": 6.499716303685921, + "heading": -0.3344743603957426, + "angularVelocity": -5.80159891848438e-7, + "velocityX": 3.6546085984432737, + "velocityY": -0.7207398669059114, + "timestamp": 5.171353864323319 + }, + { + "x": 6.444229692072682, + "y": 6.470609819762047, + "heading": -0.3344743776866033, + "angularVelocity": -5.358832938629608e-7, + "velocityX": 3.6141234350083242, + "velocityY": -0.9020764595722645, + "timestamp": 5.203619958416579 + }, + { + "x": 6.559246912194348, + "y": 6.435724733351891, + "heading": -0.3344743938681527, + "angularVelocity": -5.015031984210286e-7, + "velocityX": 3.5646465230413518, + "velocityY": -1.081168557598769, + "timestamp": 5.235886052509839 + }, + { + "x": 6.67238155121755, + "y": 6.395147846497736, + "heading": -0.33447440919657534, + "angularVelocity": -4.750628499499889e-7, + "velocityX": 3.506301032167155, + "velocityY": -1.2575704619491082, + "timestamp": 5.268152146603099 + }, + { + "x": 6.783352144572337, + "y": 6.348980132760865, + "heading": -0.33447442388388354, + "angularVelocity": -4.551932493526412e-7, + "velocityX": 3.439232310983837, + "velocityY": -1.4308429648605667, + "timestamp": 5.300418240696359 + }, + { + "x": 6.891882636447486, + "y": 6.297336519628137, + "heading": -0.3344744381114049, + "angularVelocity": -4.409434049866837e-7, + "velocityX": 3.363607989286128, + "velocityY": -1.6005536022878626, + "timestamp": 5.33268433478962 + }, + { + "x": 6.998345027127988, + "y": 6.24155399695869, + "heading": -0.33447445218138366, + "angularVelocity": -4.360607978051808e-7, + "velocityX": 3.299512806625678, + "velocityY": -1.7288278682946643, + "timestamp": 5.36495042888288 + }, + { + "x": 7.107456437455107, + "y": 6.19114935204841, + "heading": -0.33447450198371365, + "angularVelocity": -0.00000154348803003307, + "velocityX": 3.381611979799848, + "velocityY": -1.5621551454164793, + "timestamp": 5.39721652297614 }, { "x": 7.214789390563965, "y": 6.145846366882324, "heading": -0.344987478573796, - "angularVelocity": -6.552383423892292e-7, - "velocityX": 3.442174665036408, - "velocityY": -1.4237498464482023, - "timestamp": 5.3088437349689555 - }, - { - "x": 7.41054811335349, - "y": 6.075273519082507, - "heading": -0.3626370912094401, - "angularVelocity": -0.2874236730044668, - "velocityX": 3.187927819628061, - "velocityY": -1.1492777517419182, - "timestamp": 5.370249994415684 - }, - { - "x": 7.584442418775818, - "y": 6.012933970935389, - "heading": -0.3763799731717047, - "angularVelocity": -0.22380262347988858, - "velocityX": 2.831866115752982, - "velocityY": -1.0151985922737843, - "timestamp": 5.431656253862413 - }, - { - "x": 7.73639717593505, - "y": 5.958610534020796, - "heading": -0.3857543878763611, - "angularVelocity": -0.15266220071243664, - "velocityX": 2.4745809063822897, - "velocityY": -0.8846563429208651, - "timestamp": 5.493062513309142 - }, - { - "x": 7.866387295940067, - "y": 5.912228690600519, - "heading": -0.39060211670074774, - "angularVelocity": -0.07894519008427879, - "velocityX": 2.1168871248017687, - "velocityY": -0.7553276137999276, - "timestamp": 5.554468772755871 - }, - { - "x": 7.974400233400663, - "y": 5.873750785403185, - "heading": -0.39084333039715696, - "angularVelocity": -0.003928161372840292, - "velocityX": 1.7589890417327878, - "velocityY": -0.62661210019988, - "timestamp": 5.6158750322026 - }, - { - "x": 8.060428467430407, - "y": 5.843154117743718, - "heading": -0.3864300742351692, - "angularVelocity": 0.0718698093932331, - "velocityX": 1.4009684811427807, - "velocityY": -0.49826626691061593, - "timestamp": 5.6772812916493285 - }, - { - "x": 8.124466994034947, - "y": 5.820423531988022, - "heading": -0.37733049959220943, - "angularVelocity": 0.14818643449294983, - "velocityX": 1.0428664305809843, - "velocityY": -0.3701672428918256, - "timestamp": 5.738687551096057 - }, - { - "x": 8.16651225164957, - "y": 5.805548215416436, - "heading": -0.3635219686266917, - "angularVelocity": 0.22487171649816598, - "velocityX": 0.6847063799920386, - "velocityY": -0.24224430384806347, - "timestamp": 5.800093810542786 + "angularVelocity": -0.325821171899398, + "velocityX": 3.32649352594794, + "velocityY": -1.404043050117709, + "timestamp": 5.4294826170694 + }, + { + "x": 7.408281473848404, + "y": 6.071835314929934, + "heading": -0.36045376555379516, + "angularVelocity": -0.24376616005736862, + "velocityX": 3.0496538829742375, + "velocityY": -1.1664978129250685, + "timestamp": 5.492929844430077 + }, + { + "x": 7.581043516745906, + "y": 6.008144867615469, + "heading": -0.3717285153471776, + "angularVelocity": -0.17770279746487722, + "velocityX": 2.722925021063643, + "velocityY": -1.003833421315721, + "timestamp": 5.556377071790754 + }, + { + "x": 7.732516109743219, + "y": 5.95357268553042, + "heading": -0.3790773339263405, + "angularVelocity": -0.11582568513809473, + "velocityX": 2.3873792330788404, + "velocityY": -0.8601192574550642, + "timestamp": 5.619824299151431 + }, + { + "x": 7.862474679773266, + "y": 5.907578864525407, + "heading": -0.3826203402067485, + "angularVelocity": -0.05584178265611413, + "velocityX": 2.048293919784284, + "velocityY": -0.7249145930925164, + "timestamp": 5.683271526512108 + }, + { + "x": 7.970798961808127, + "y": 5.869857366205246, + "heading": -0.38242588059176913, + "angularVelocity": 0.003064903275819629, + "velocityX": 1.7073130937475338, + "velocityY": -0.5945334396683074, + "timestamp": 5.746718753872785 + }, + { + "x": 8.057414200976034, + "y": 5.840211247118204, + "heading": -0.378538126264348, + "angularVelocity": 0.061275401450096946, + "velocityX": 1.3651540464570049, + "velocityY": -0.46725633759397367, + "timestamp": 5.810165981233462 + }, + { + "x": 8.122269484577586, + "y": 5.818503192947792, + "heading": -0.37098804864933244, + "angularVelocity": 0.11899775497037605, + "velocityX": 1.0221925574914426, + "velocityY": -0.3421434643157637, + "timestamp": 5.873613208594139 + }, + { + "x": 8.165327926639057, + "y": 5.804632020140207, + "heading": -0.3597986070827489, + "angularVelocity": 0.1763582434103087, + "velocityX": 0.6786497038979674, + "velocityY": -0.2186253581851867, + "timestamp": 5.937060435954816 }, { "x": 8.186561584472656, "y": 5.798520088195801, "heading": -0.344987478573796, - "angularVelocity": 0.3018338882695626, - "velocityX": 0.32650307971422843, - "velocityY": -0.11445294476423894, - "timestamp": 5.861500069989515 - }, - { - "x": 8.182873431203555, - "y": 5.799961031790675, - "heading": -0.31977664522832494, - "angularVelocity": 0.3842917157740494, - "velocityX": -0.056218956684159486, - "velocityY": 0.021964473717296987, - "timestamp": 5.927103444485088 - }, - { - "x": 8.154073485915251, - "y": 5.810352807071496, - "heading": -0.2894961331766682, - "angularVelocity": 0.46156942816562346, - "velocityX": -0.4390009738027679, - "velocityY": 0.15840306021945175, - "timestamp": 5.99270681898066 - }, - { - "x": 8.100157148407472, - "y": 5.829696959053818, - "heading": -0.25457167500592204, - "angularVelocity": 0.5323576483569935, - "velocityX": -0.8218531123184383, - "velocityY": 0.29486519757649204, - "timestamp": 6.058310193476233 - }, - { - "x": 8.021119032382456, - "y": 5.857995174413058, - "heading": -0.21555305979528627, - "angularVelocity": 0.5947653685599519, - "velocityX": -1.2047873548082333, - "velocityY": 0.4313530451265237, - "timestamp": 6.1239135679718055 - }, - { - "x": 7.916952910031998, - "y": 5.895249222659617, - "heading": -0.17317929559353756, - "angularVelocity": 0.6459083016317765, - "velocityX": -1.5878165285154446, - "velocityY": 0.567867865532928, - "timestamp": 6.189516942467378 - }, - { - "x": 7.787651985971443, - "y": 5.941460762829762, - "heading": -0.12849926666272526, - "angularVelocity": 0.6810629677872386, - "velocityX": -1.9709492850749835, - "velocityY": 0.7044079748255081, - "timestamp": 6.255120316962951 - }, - { - "x": 7.633210581172123, - "y": 5.996630683324096, - "heading": -0.08312168761624714, - "angularVelocity": 0.6916958067384257, - "velocityX": -2.354168607740487, - "velocityY": 0.8409616261135585, - "timestamp": 6.320723691458523 - }, - { - "x": 7.453632538523732, - "y": 6.060756399754721, - "heading": -0.03982832718204048, - "angularVelocity": 0.6599258158149841, - "velocityX": -2.7373293527836737, - "velocityY": 0.9774758832711061, - "timestamp": 6.386327065954096 - }, - { - "x": 7.248984101269758, - "y": 6.133814967695316, - "heading": -0.004587884016795663, - "angularVelocity": 0.5371742450172758, - "velocityX": -3.1194803442281134, - "velocityY": 1.11364039582332, - "timestamp": 6.451930440449669 - }, - { - "x": 7.020152981307638, - "y": 6.215344107160959, - "heading": -7.996010360253712e-8, - "angularVelocity": 0.06993244009119806, - "velocityX": -3.488099838180771, - "velocityY": 1.242758319865779, - "timestamp": 6.517533814945241 - }, - { - "x": 6.789970042208637, - "y": 6.297403837687721, - "heading": -7.492708758217055e-8, - "angularVelocity": 7.671885873349635e-8, - "velocityX": -3.5087057772391836, - "velocityY": 1.2508461821929457, - "timestamp": 6.583137189440814 - }, - { - "x": 6.5597871223902215, - "y": 6.379463622297776, - "heading": -6.98940737576559e-8, - "angularVelocity": 7.671882526186561e-8, - "velocityX": -3.5087054833429296, - "velocityY": 1.2508470065910036, - "timestamp": 6.648740563936387 - }, - { - "x": 6.3296039103658925, - "y": 6.46152258724702, - "heading": -6.48610593666683e-8, - "angularVelocity": 7.671883389668008e-8, - "velocityX": -3.5087099374722537, - "velocityY": 1.2508345124042644, - "timestamp": 6.714343938431959 - }, - { - "x": 6.092938315579126, - "y": 6.522410853761584, - "heading": -5.975605245251985e-8, - "angularVelocity": 7.781622444578512e-8, - "velocityX": -3.6075216649524458, - "velocityY": 0.9281270511271241, - "timestamp": 6.779947312927532 + "angularVelocity": 0.23344012220985313, + "velocityX": 0.3346664419690626, + "velocityY": -0.09633095406457304, + "timestamp": 6.000507663315493 + }, + { + "x": 8.184449385960491, + "y": 5.800720366343895, + "heading": -0.3251687978756523, + "angularVelocity": 0.29387941781314064, + "velocityX": -0.03132053432391204, + "velocityY": 0.03262661481041375, + "timestamp": 6.0679458011940195 + }, + { + "x": 8.157654760081511, + "y": 5.811617201116665, + "heading": -0.30136325235519174, + "angularVelocity": 0.3529982628426134, + "velocityX": -0.39732155604955366, + "velocityY": 0.1615826758502398, + "timestamp": 6.135383939072546 + }, + { + "x": 8.106176542546697, + "y": 5.831210437992555, + "heading": -0.27368179509987206, + "angularVelocity": 0.41047185058966307, + "velocityX": -0.7633398423239571, + "velocityY": 0.29053644558191305, + "timestamp": 6.2028220769510725 + }, + { + "x": 8.03001326746699, + "y": 5.859499833662039, + "heading": -0.24226677680218867, + "angularVelocity": 0.46583460466049514, + "velocityX": -1.129379865394508, + "velocityY": 0.4194866074214659, + "timestamp": 6.270260214829599 + }, + { + "x": 7.929163035221131, + "y": 5.896484993829156, + "heading": -0.2073080079914019, + "angularVelocity": 0.5183827713890392, + "velocityX": -1.495448056817871, + "velocityY": 0.5484309224809322, + "timestamp": 6.337698352708125 + }, + { + "x": 7.80362329670357, + "y": 5.942165255300411, + "heading": -0.16907207724227352, + "angularVelocity": 0.5669778548452968, + "velocityX": -1.8615540474099375, + "velocityY": 0.677365403438884, + "timestamp": 6.405136490586652 + }, + { + "x": 7.653390486781624, + "y": 5.996539431834676, + "heading": -0.12796178084950977, + "angularVelocity": 0.6096001118360348, + "velocityX": -2.227712903232215, + "velocityY": 0.8062822943333132, + "timestamp": 6.472574628465178 + }, + { + "x": 7.478459409616276, + "y": 6.059605150575439, + "heading": -0.0846568853073795, + "angularVelocity": 0.6421425161550796, + "velocityX": -2.593948805058106, + "velocityY": 0.9351639995511196, + "timestamp": 6.540012766343705 + }, + { + "x": 7.278822723764661, + "y": 6.131356463760539, + "heading": -0.04054583544960124, + "angularVelocity": 0.6540964985900647, + "velocityX": -2.9602935687698184, + "velocityY": 1.063957508944601, + "timestamp": 6.607450904222231 + }, + { + "x": 7.05448611686489, + "y": 6.21176649036125, + "heading": -5.1297661467442016e-8, + "angularVelocity": 0.6012292958766616, + "velocityX": -3.3265539938819106, + "velocityY": 1.1923524155656642, + "timestamp": 6.674889042100758 + }, + { + "x": 6.817570799613061, + "y": 6.29528987249135, + "heading": -4.834198432704978e-8, + "angularVelocity": 4.3827976770594794e-8, + "velocityX": -3.5130762014599934, + "velocityY": 1.238518511299158, + "timestamp": 6.742327179979284 + }, + { + "x": 6.580655475027505, + "y": 6.378813233819268, + "heading": -4.538633792987658e-8, + "angularVelocity": 4.382752089770155e-8, + "velocityX": -3.5130763102074556, + "velocityY": 1.23851820283599, + "timestamp": 6.809765317857811 + }, + { + "x": 6.3437395023630385, + "y": 6.4623347568384295, + "heading": -4.243069068525663e-8, + "angularVelocity": 4.382753346398488e-8, + "velocityX": -3.5130859201838094, + "velocityY": 1.2384909436497944, + "timestamp": 6.877203455736337 + }, + { + "x": 6.09986574355872, + "y": 6.5225893218228235, + "heading": -3.943141637637637e-8, + "angularVelocity": 4.447445322827149e-8, + "velocityX": -3.6162587888117317, + "velocityY": 0.8934790740059899, + "timestamp": 6.944641593614864 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -5.436468625345948e-8, - "angularVelocity": 8.218123290935499e-8, - "velocityX": -3.6872209612695626, - "velocityY": 0.5291823081772375, - "timestamp": 6.8455506874231045 - }, - { - "x": 5.6615808215647085, - "y": 6.568071618356437, - "heading": -4.94825886082418e-8, - "angularVelocity": 9.582618856188379e-8, - "velocityX": -3.71880135129745, - "velocityY": 0.2148218335343848, - "timestamp": 6.896498113366742 - }, - { - "x": 5.471871500054035, - "y": 6.562921703037617, - "heading": -4.486806140713326e-8, - "angularVelocity": 9.057429527869525e-8, - "velocityX": -3.7236291725620743, - "velocityY": -0.10108293448460899, - "timestamp": 6.94744553931038 - }, - { - "x": 5.283280913941195, - "y": 6.541714647593256, - "heading": -4.0367954549807895e-8, - "angularVelocity": 8.832844395914553e-8, - "velocityX": -3.7016705480170033, - "velocityY": -0.4162537174659499, - "timestamp": 6.998392965254017 - }, - { - "x": 5.095501482386873, - "y": 6.514236775268743, - "heading": -3.5882626212892094e-8, - "angularVelocity": 8.803837002241881e-8, - "velocityX": -3.68574914387354, - "velocityY": -0.539337794119592, - "timestamp": 7.049340391197655 - }, - { - "x": 4.907722071138919, - "y": 6.486758764174086, - "heading": -3.139729791987603e-8, - "angularVelocity": 8.803836916075124e-8, - "velocityX": -3.6857487452985973, - "velocityY": -0.5393405179106829, - "timestamp": 7.100287817141292 - }, - { - "x": 4.7199426574500984, - "y": 6.459280769759847, - "heading": -2.6911969659920007e-8, - "angularVelocity": 8.803836851184644e-8, - "velocityX": -3.6857487932081243, - "velocityY": -0.5393401905061415, - "timestamp": 7.15123524308493 - }, - { - "x": 4.532163244787761, - "y": 6.431802768330805, - "heading": -2.2426641378076926e-8, - "angularVelocity": 8.803836894144722e-8, - "velocityX": -3.6857487730602023, - "velocityY": -0.5393403281932087, - "timestamp": 7.202182669028567 - }, - { - "x": 4.3443838326647874, - "y": 6.404324763215868, - "heading": -1.7941313081549045e-8, - "angularVelocity": 8.803836922968166e-8, - "velocityX": -3.6857487624735623, - "velocityY": -0.5393404005402795, - "timestamp": 7.253130094972205 - }, - { - "x": 4.156604420793327, - "y": 6.376846756382128, - "heading": -1.3455984799695103e-8, - "angularVelocity": 8.80383689416604e-8, - "velocityX": -3.685748757536816, - "velocityY": -0.5393404342770572, - "timestamp": 7.304077520915842 - }, - { - "x": 3.968825009218295, - "y": 6.349368747522659, - "heading": -8.970656536814864e-9, - "angularVelocity": 8.803836856924311e-8, - "velocityX": -3.685748751718508, - "velocityY": -0.5393404740382505, - "timestamp": 7.35502494685948 - }, - { - "x": 3.781045597297714, - "y": 6.321890741024598, - "heading": -4.485328240150933e-9, - "angularVelocity": 8.803836923235202e-8, - "velocityX": -3.6857487585009534, - "velocityY": -0.5393404276883234, - "timestamp": 7.4059723728031175 - }, - { - "x": 3.5932661777631214, - "y": 6.2944127865592865, - "heading": -1.5290485245544054e-22, - "angularVelocity": 8.803836812310754e-8, - "velocityX": -3.6857489079493813, - "velocityY": -0.5393394063855248, - "timestamp": 7.456919798746755 + "heading": -3.6277001373267394e-8, + "angularVelocity": 4.677494222617604e-8, + "velocityX": -3.689626869559746, + "velocityY": 0.5121386527717335, + "timestamp": 7.01207973149339 + }, + { + "x": 5.675146419069591, + "y": 6.5685334564190505, + "heading": -3.325736766733182e-8, + "angularVelocity": 6.38129937324226e-8, + "velocityX": -3.7171934353117426, + "velocityY": 0.24104917118070399, + "timestamp": 7.059399776037881 + }, + { + "x": 5.498885444409021, + "y": 6.56705094919282, + "heading": -3.0392937858444436e-8, + "angularVelocity": 6.053311733876744e-8, + "velocityX": -3.724869161837878, + "velocityY": -0.03132937089348242, + "timestamp": 7.106719820582372 + }, + { + "x": 5.3232044198709785, + "y": 6.552687568221491, + "heading": -2.7611010604394772e-8, + "angularVelocity": 5.878961613051839e-8, + "velocityX": -3.7126132536258285, + "velocityY": -0.3035369283692009, + "timestamp": 7.154039865126863 + }, + { + "x": 5.148803896994517, + "y": 6.527102675424277, + "heading": -2.484990875079416e-8, + "angularVelocity": 5.8349519324830205e-8, + "velocityX": -3.6855528044249266, + "velocityY": -0.5406776989222635, + "timestamp": 7.201359909671353 + }, + { + "x": 4.974403597324897, + "y": 6.501516261174204, + "heading": -2.208880782591367e-8, + "angularVelocity": 5.834949969847243e-8, + "velocityX": -3.6855480874631454, + "velocityY": -0.5407098513192842, + "timestamp": 7.248679954215844 + }, + { + "x": 4.800003297129097, + "y": 6.475929850510635, + "heading": -1.9327706889009288e-8, + "angularVelocity": 5.8349499952569724e-8, + "velocityX": -3.6855480985827302, + "velocityY": -0.5407097755267535, + "timestamp": 7.295999998760335 + }, + { + "x": 4.625602997244748, + "y": 6.450343437724181, + "heading": -1.656660593755509e-8, + "angularVelocity": 5.8349500260046496e-8, + "velocityX": -3.685548092000944, + "velocityY": -0.5407098203890633, + "timestamp": 7.343320043304826 + }, + { + "x": 4.451202697255753, + "y": 6.4247570256510045, + "heading": -1.3805504990627123e-8, + "angularVelocity": 5.834950016439505e-8, + "velocityX": -3.685548094212386, + "velocityY": -0.5407098053155895, + "timestamp": 7.390640087849317 + }, + { + "x": 4.276802397278509, + "y": 6.3991706134977395, + "heading": -1.1044403987822918e-8, + "angularVelocity": 5.8349501345210454e-8, + "velocityX": -3.685548093964082, + "velocityY": -0.5407098070080617, + "timestamp": 7.437960132393807 + }, + { + "x": 4.102402097300047, + "y": 6.373584201352764, + "heading": -8.283303038922857e-9, + "angularVelocity": 5.834950020607066e-8, + "velocityX": -3.685548093989784, + "velocityY": -0.5407098068328745, + "timestamp": 7.485280176938298 + }, + { + "x": 3.9280017973350696, + "y": 6.347997789115884, + "heading": -5.522202092459704e-9, + "angularVelocity": 5.8349500154572286e-8, + "velocityX": -3.685548093704841, + "velocityY": -0.5407098087750815, + "timestamp": 7.532600221482789 + }, + { + "x": 3.7536014973495653, + "y": 6.322411377018916, + "heading": -2.7611011159009457e-9, + "angularVelocity": 5.834950079057347e-8, + "velocityX": -3.6855480941386265, + "velocityY": -0.5407098058183458, + "timestamp": 7.57992026602728 + }, + { + "x": 3.579201219971439, + "y": 6.296824810827607, + "heading": -5.947116132480024e-26, + "angularVelocity": 5.8349503735249215e-8, + "velocityX": -3.6855476163838365, + "velocityY": -0.5407130622468497, + "timestamp": 7.6272403105717705 }, { "x": 3.405486822128296, "y": 6.266934394836426, - "heading": -7.242717716527151e-23, - "angularVelocity": 4.066531625418711e-24, - "velocityX": -3.6857476537198193, - "velocityY": -0.5393479889103638, - "timestamp": 7.507867224690393 - }, - { - "x": 3.1943671277625394, - "y": 6.2230448572371815, - "heading": -6.454692239159141e-23, - "angularVelocity": 6.1636286209902435e-25, - "velocityX": -3.5183764765296694, - "velocityY": -0.7314330248480798, - "timestamp": 7.5678720865276885 - }, - { - "x": 3.002694024502071, - "y": 6.190127687410396, - "heading": -5.666660567021065e-23, - "angularVelocity": 6.173952401211252e-25, - "velocityX": -3.1942928854697095, - "velocityY": -0.5485750457361542, - "timestamp": 7.627876948364984 - }, - { - "x": 2.830467511330891, - "y": 6.168182901091022, - "heading": -4.87863785909476e-23, - "angularVelocity": 6.159013258773547e-25, - "velocityX": -2.870209311341694, - "velocityY": -0.36571680439623283, - "timestamp": 7.68788181020228 - }, - { - "x": 2.6776875881237743, - "y": 6.157210503145755, - "heading": -4.0906073219636964e-23, - "angularVelocity": 6.172060876090466e-25, - "velocityX": -2.5461257393006025, - "velocityY": -0.1828584819513171, - "timestamp": 7.747886672039576 - }, - { - "x": 2.5443542548814535, - "y": 6.157210495895674, - "heading": -3.3025815924682597e-23, - "angularVelocity": 6.164048799254959e-25, - "velocityX": -2.2220421672473036, - "velocityY": -1.208249057531058e-7, - "timestamp": 7.807891533876872 - }, - { - "x": 2.430467511630842, - "y": 6.168182880686502, - "heading": -2.5145589184649632e-23, - "angularVelocity": 6.158956724948031e-25, - "velocityX": -1.8979585947454753, - "velocityY": 0.18285826272844113, - "timestamp": 7.867896395714168 - }, - { - "x": 2.3360273584029394, - "y": 6.1901276583922495, - "heading": -1.726521486745078e-23, - "angularVelocity": 6.183550926375638e-25, - "velocityX": -1.5738750217270354, - "velocityY": 0.3657166608474323, - "timestamp": 7.927901257551464 - }, - { - "x": 2.261033795226874, - "y": 6.223044829624826, - "heading": -9.385003241848346e-24, - "angularVelocity": 6.156437857286479e-25, - "velocityX": -1.2497914482231562, - "velocityY": 0.5485750691641013, - "timestamp": 7.98790611938876 + "heading": -3.242812966980484e-26, + "angularVelocity": 4.141833469401844e-27, + "velocityX": -3.6710531343607253, + "velocityY": -0.6316650011408507, + "timestamp": 7.674560355116261 + }, + { + "x": 3.234879221757396, + "y": 6.227705290951873, + "heading": -3.799073171453003e-26, + "angularVelocity": 3.7193578167117204e-28, + "velocityX": -3.4320264149158155, + "velocityY": -0.7891519514521484, + "timestamp": 7.724270811780997 + }, + { + "x": 3.075654096212167, + "y": 6.19701582942553, + "heading": -4.3640191392241984e-26, + "angularVelocity": -1.3753350907836377e-27, + "velocityX": -3.2030509520179686, + "velocityY": -0.6173643049252793, + "timestamp": 7.773981268445732 + }, + { + "x": 2.9276929132783946, + "y": 6.175021768989464, + "heading": -4.923363376741485e-26, + "angularVelocity": -2.4846347668894466e-28, + "velocityX": -2.9764599414500257, + "velocityY": -0.4424433391228337, + "timestamp": 7.823691725110468 + }, + { + "x": 2.790955431257819, + "y": 6.16177499390257, + "heading": -5.486689770041805e-26, + "angularVelocity": -1.0495335231555751e-27, + "velocityX": -2.7506784526800994, + "velocityY": -0.2664786440453503, + "timestamp": 7.873402181775203 + }, + { + "x": 2.6654213921010017, + "y": 6.157301437220893, + "heading": -6.044835484657667e-26, + "angularVelocity": -7.36271402334531e-30, + "velocityX": -2.525304484798085, + "velocityY": -0.08999226685541546, + "timestamp": 7.923112638439939 + }, + { + "x": 2.551078597067916, + "y": 6.16161665554964, + "heading": -6.59367177391361e-26, + "angularVelocity": 1.865367086612129e-27, + "velocityX": -2.300175912771294, + "velocityY": 0.08680705465754052, + "timestamp": 7.972823095104674 + }, + { + "x": 2.447918895369922, + "y": 6.174731018536324, + "heading": -7.161083135132275e-26, + "angularVelocity": -1.871285764514843e-27, + "velocityX": -2.075211306018359, + "velocityY": 0.2638149771009456, + "timestamp": 8.02253355176941 + }, + { + "x": 2.355936456054869, + "y": 6.196651932350337, + "heading": -7.721743669966212e-26, + "angularVelocity": -5.132563200735982e-28, + "velocityX": -1.8503639975672366, + "velocityY": 0.44097188569106954, + "timestamp": 8.072244008434145 + }, + { + "x": 2.2751269010087887, + "y": 6.227384951214515, + "heading": -8.282082994441975e-26, + "angularVelocity": -4.486400643129525e-28, + "velocityX": -1.6256047614104199, + "velocityY": 0.618240525760057, + "timestamp": 8.12195446509888 }, { "x": 2.205486822128296, "y": 6.266934394836426, - "heading": -1.5067646314431248e-24, - "angularVelocity": 5.827628634700505e-25, - "velocityX": -0.9257078742918208, - "velocityY": 0.7314334850167038, - "timestamp": 8.047910981226057 - }, - { - "x": 2.166583654686104, - "y": 6.330345399138925, - "heading": 0.03190215483964595, - "angularVelocity": 0.46789281830201823, - "velocityX": -0.5705731398677051, - "velocityY": 0.9300172249677102, - "timestamp": 8.116093593977881 - }, - { - "x": 2.1528025694390287, - "y": 6.405563401029407, - "heading": 0.09569947573294339, - "angularVelocity": 0.9356831356039661, - "velocityX": -0.20212022817660624, - "velocityY": 1.1031845048863924, - "timestamp": 8.184276206729706 - }, - { - "x": 2.1659059898168422, - "y": 6.488336785914272, - "heading": 0.18898264963043915, - "angularVelocity": 1.3681372733109376, - "velocityX": 0.1921812592531237, - "velocityY": 1.2139954974466904, - "timestamp": 8.25245881948153 - }, - { - "x": 2.2052860593561716, - "y": 6.55939873181835, - "heading": 0.2899151707587667, - "angularVelocity": 1.4803263919455447, - "velocityX": 0.5775676224475003, - "velocityY": 1.0422297274341026, - "timestamp": 8.320641432233355 - }, - { - "x": 2.238286501729519, - "y": 6.603471711362833, - "heading": 0.35830069991198743, - "angularVelocity": 1.0029760725382513, - "velocityX": 0.4840008477449354, - "velocityY": 0.6463961670829911, - "timestamp": 8.38882404498518 - }, - { - "x": 2.256009101867676, - "y": 6.624549388885498, + "heading": -8.869624024083721e-26, + "angularVelocity": -5.920668887028623e-27, + "velocityX": -1.400914084337811, + "velocityY": 0.7955960631914043, + "timestamp": 8.171664921763616 + }, + { + "x": 2.1363371710210255, + "y": 6.327839189643039, + "heading": 0.025262557050058088, + "angularVelocity": 0.415801673405611, + "velocityX": -1.1381484696439823, + "velocityY": 1.002444667372145, + "timestamp": 8.23242118770922 + }, + { + "x": 2.0845111064453854, + "y": 6.399393278580984, + "heading": 0.0757412033730169, + "angularVelocity": 0.8308385240158422, + "velocityX": -0.8530159608893926, + "velocityY": 1.177723611289897, + "timestamp": 8.293177453654822 + }, + { + "x": 2.051957537582464, + "y": 6.477853810368175, + "heading": 0.14980799257498115, + "angularVelocity": 1.2190806668118568, + "velocityX": -0.5358059511436571, + "velocityY": 1.291398188582541, + "timestamp": 8.353933719600425 + }, + { + "x": 2.0403269766878736, + "y": 6.554154175481095, + "heading": 0.23625288977037628, + "angularVelocity": 1.422814517152715, + "velocityX": -0.19142981737889736, + "velocityY": 1.255843556633874, + "timestamp": 8.414689985546028 + }, + { + "x": 2.0424863781029257, + "y": 6.6144252836562565, + "heading": 0.31360709056854197, + "angularVelocity": 1.2731888570542382, + "velocityX": 0.03554203638823312, + "velocityY": 0.9920146874912401, + "timestamp": 8.475446251491631 + }, + { + "x": 2.048130429243297, + "y": 6.654505373712402, + "heading": 0.3666123519116142, + "angularVelocity": 0.8724246053983875, + "velocityX": 0.09289660996323934, + "velocityY": 0.659686526687304, + "timestamp": 8.536202517437234 + }, + { + "x": 2.052253484725952, + "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": 0.5097673803944184, - "velocityX": 0.2599284395666251, - "velocityY": 0.3091356677601188, - "timestamp": 8.457006657737004 + "angularVelocity": 0.4352739504266956, + "velocityX": 0.0678622265289755, + "velocityY": 0.3266075878021015, + "timestamp": 8.596958783382837 }, { - "x": 2.256009101867676, - "y": 6.624549388885498, + "x": 2.052253484725952, + "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": 1.058900443902291e-24, - "velocityX": 2.5593812661454674e-24, - "velocityY": -7.939073774690169e-24, - "timestamp": 8.525189270488829 + "angularVelocity": -3.1389023173080445e-26, + "velocityX": -4.435068013070024e-22, + "velocityY": -1.8479842669198598e-22, + "timestamp": 8.65771504932844 }, { - "x": 2.306032658844481, - "y": 6.645836035930219, + "x": 2.103013204328941, + "y": 6.695498705652326, "heading": 0.3930579718029317, - "angularVelocity": 7.690085231355673e-17, - "velocityX": 0.5342706205732553, - "velocityY": 0.22734948919726436, - "timestamp": 8.618818893849236 + "angularVelocity": 3.416438400428702e-17, + "velocityX": 0.5194333949867557, + "velocityY": 0.21643049229415012, + "timestamp": 8.755436374320036 }, { - "x": 2.406079771576901, - "y": 6.688409329500005, + "x": 2.2045326425850984, + "y": 6.737798454207702, "heading": 0.3930579718029317, - "angularVelocity": 7.285372137244723e-17, - "velocityX": 1.0685412281037379, - "velocityY": 0.45469897284440564, - "timestamp": 8.712448517209642 + "angularVelocity": 5.790454228147962e-17, + "velocityX": 1.0388667802538214, + "velocityY": 0.432860980538431, + "timestamp": 8.853157699311632 }, { - "x": 2.556150436401367, - "y": 6.752269268035889, - "heading": 0.3930579718029317, - "angularVelocity": 1.0864711023830975e-16, - "velocityX": 1.6028117965059043, - "velocityY": 0.6820484398411784, - "timestamp": 8.80607814057005 + "x": 2.356811797278175, + "y": 6.801248075919452, + "heading": 0.3930579718029318, + "angularVelocity": 1.3697605676063785e-16, + "velocityX": 1.558300142841611, + "velocityY": 0.6492914593330171, + "timestamp": 8.950879024303228 }, { - "x": 2.7062211012258333, - "y": 6.816129206571772, - "heading": 0.3930579718029317, - "angularVelocity": 1.1973301038102625e-17, - "velocityX": 1.6028117965059043, - "velocityY": 0.6820484398411785, - "timestamp": 8.899707763930456 + "x": 2.5598506573269275, + "y": 6.8858475661703915, + "heading": 0.3930579718029318, + "angularVelocity": 1.6886690536158693e-16, + "velocityX": 2.0777333920330334, + "velocityY": 0.8657218908791364, + "timestamp": 9.048600349294825 + }, + { + "x": 2.712129812020004, + "y": 6.9492971878821415, + "heading": 0.3930579718029318, + "angularVelocity": 9.579438090360999e-17, + "velocityX": 1.5583001428416108, + "velocityY": 0.649291459333017, + "timestamp": 9.14632167428642 }, { - "x": 2.8062682139582535, - "y": 6.8587025001415585, + "x": 2.8136492502761614, + "y": 6.991596936437518, "heading": 0.3930579718029317, - "angularVelocity": -9.389335874202783e-18, - "velocityX": 1.0685412281037379, - "velocityY": 0.45469897284440564, - "timestamp": 8.993337387290863 + "angularVelocity": -7.496030828989607e-17, + "velocityX": 1.0388667802538214, + "velocityY": 0.4328609805384309, + "timestamp": 9.244042999278017 }, { - "x": 2.8562917709350586, - "y": 6.879989147186279, + "x": 2.8644089698791504, + "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 6.118834146220853e-17, - "velocityX": 0.5342706205732553, - "velocityY": 0.22734948919726436, - "timestamp": 9.08696701065127 + "angularVelocity": 4.893193347075666e-17, + "velocityX": 0.5194333949867557, + "velocityY": 0.21643049229415012, + "timestamp": 9.341764324269613 }, { - "x": 2.8562917709350586, - "y": 6.879989147186279, + "x": 2.8644089698791504, + "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 9.527092238533276e-28, - "velocityX": 5.413823231130942e-26, - "velocityY": -7.426199884271778e-26, - "timestamp": 9.180596634011676 - }, - { - "x": 2.8345596275672933, - "y": 6.874297000721419, - "heading": 0.3901475474656487, - "angularVelocity": -0.04834267054248828, - "velocityX": -0.36097480135517596, - "velocityY": -0.09454757428506516, - "timestamp": 9.240800679518948 - }, - { - "x": 2.791210221547733, - "y": 6.8624934986264625, - "heading": 0.38424126669366737, - "angularVelocity": -0.0981043835545563, - "velocityX": -0.7200414134017654, - "velocityY": -0.19605828803531558, - "timestamp": 9.30100472502622 - }, - { - "x": 2.7264146334756165, - "y": 6.84400633385399, - "heading": 0.3752232336055894, - "angularVelocity": -0.14979114795514328, - "velocityX": -1.076266345993141, - "velocityY": -0.3070751245485542, - "timestamp": 9.36120877053349 - }, - { - "x": 2.6404493956081283, - "y": 6.818009888574526, - "heading": 0.36292730942366375, - "angularVelocity": -0.20423750726918255, - "velocityX": -1.4278980281666382, - "velocityY": -0.431805621373474, - "timestamp": 9.421412816040762 - }, - { - "x": 2.5338184232966747, - "y": 6.783217894913327, - "heading": 0.3470968619442176, - "angularVelocity": -0.2629465735410421, - "velocityX": -1.7711595859214113, - "velocityY": -0.5779012584294861, - "timestamp": 9.481616861548034 - }, - { - "x": 2.407629795142463, - "y": 6.737392802929253, - "heading": 0.3272924409497442, - "angularVelocity": -0.3289549867887417, - "velocityX": -2.0960157592560678, - "velocityY": -0.761163001555094, - "timestamp": 9.541820907055305 - }, - { - "x": 2.2652115507191506, - "y": 6.676063122618794, - "heading": 0.30267146092102903, - "angularVelocity": -0.40895889671967695, - "velocityX": -2.3655925980274817, - "velocityY": -1.0186969960855932, - "timestamp": 9.602024952562576 - }, - { - "x": 2.1204311340072723, - "y": 6.592422966921264, - "heading": 0.27232018505584993, - "angularVelocity": -0.5041401389133051, - "velocityX": -2.4048287036523215, - "velocityY": -1.3892779960680068, - "timestamp": 9.662228998069848 - }, - { - "x": 1.992685360010843, - "y": 6.494128334362165, - "heading": 0.23976877399587285, - "angularVelocity": -0.5406847793317353, - "velocityX": -2.1218802311382734, - "velocityY": -1.6326914866082545, - "timestamp": 9.72243304357712 - }, - { - "x": 1.886238292214546, - "y": 6.388651713547729, - "heading": 0.20716928495643328, - "angularVelocity": -0.5414833631986058, - "velocityX": -1.7681048989214407, - "velocityY": -1.7519856003978374, - "timestamp": 9.78263708908439 - }, - { - "x": 1.801932900713492, - "y": 6.279311808551523, - "heading": 0.17534566664181037, - "angularVelocity": -0.5285960112228498, - "velocityX": -1.4003276821467197, - "velocityY": -1.8161554439559784, - "timestamp": 9.842841134591662 + "angularVelocity": 3.090778525044755e-27, + "velocityX": 4.437436909321685e-22, + "velocityY": 1.850515832937087e-22, + "timestamp": 9.43948564926121 + }, + { + "x": 2.8419574642644285, + "y": 7.0048102452086525, + "heading": 0.3900130033247021, + "angularVelocity": -0.04733865202910357, + "velocityX": -0.3490426976251426, + "velocityY": -0.12338594795790273, + "timestamp": 9.503808741442997 + }, + { + "x": 2.7972074454620057, + "y": 6.988518822273239, + "heading": 0.3838409042111465, + "angularVelocity": -0.0959546393713819, + "velocityX": -0.6957068959923765, + "velocityY": -0.25327487194445947, + "timestamp": 9.568131833624784 + }, + { + "x": 2.7303816098102875, + "y": 6.963303209303571, + "heading": 0.37443012101631223, + "angularVelocity": -0.14630489417762893, + "velocityX": -1.0389089421083304, + "velocityY": -0.39201493762775413, + "timestamp": 9.632454925806572 + }, + { + "x": 2.6418300370011316, + "y": 6.928345468623928, + "heading": 0.3616207312094327, + "angularVelocity": -0.19914138721251212, + "velocityX": -1.3766684685943704, + "velocityY": -0.5434710847054101, + "timestamp": 9.69677801798836 + }, + { + "x": 2.5321688819092247, + "y": 6.882378878870811, + "heading": 0.34516587333325927, + "angularVelocity": -0.25581571591224617, + "velocityX": -1.7048489332880077, + "velocityY": -0.7146203360872027, + "timestamp": 9.761101110170147 + }, + { + "x": 2.4026942269657687, + "y": 6.823220010782376, + "heading": 0.324643183634766, + "angularVelocity": -0.3190563295758964, + "velocityX": -2.012879831360391, + "velocityY": -0.9197143060417909, + "timestamp": 9.825424202351934 + }, + { + "x": 2.2570878780964114, + "y": 6.7465695928484575, + "heading": 0.2992393598461131, + "angularVelocity": -0.3949409601898093, + "velocityX": -2.2636714736575443, + "velocityY": -1.1916469705357338, + "timestamp": 9.889747294533722 + }, + { + "x": 2.110110870224978, + "y": 6.646173604450097, + "heading": 0.26804931760848005, + "angularVelocity": -0.4848964995259366, + "velocityX": -2.2849804461522467, + "velocityY": -1.5608078684187743, + "timestamp": 9.95407038671551 + }, + { + "x": 1.9823765062843866, + "y": 6.531738038725912, + "heading": 0.2347990669991598, + "angularVelocity": -0.5169255625234794, + "velocityX": -1.9858243689465778, + "velocityY": -1.779074385926154, + "timestamp": 10.018393478897297 + }, + { + "x": 1.8778139110395455, + "y": 6.411761042169391, + "heading": 0.201692518308331, + "angularVelocity": -0.5146914983077023, + "velocityX": -1.6255840896039286, + "velocityY": -1.865224330594162, + "timestamp": 10.082716571079084 + }, + { + "x": 1.796996456842112, + "y": 6.289847592193697, + "heading": 0.16955186256906168, + "angularVelocity": -0.49967522780830353, + "velocityX": -1.2564298676598118, + "velocityY": -1.8953294352072878, + "timestamp": 10.147039663260871 }, { "x": 1.740001559257507, "y": 6.167843341827393, - "heading": 0.1447051188724413, - "angularVelocity": -0.5089449971541862, - "velocityX": -1.0286906956859625, - "velocityY": -1.8515112362451833, - "timestamp": 9.903045180098934 - }, - { - "x": 1.69740928285499, - "y": 6.020477623351315, - "heading": 0.10712758929116097, - "angularVelocity": -0.47810334665219445, - "velocityX": -0.5419065626848166, - "velocityY": -1.874951439604909, - "timestamp": 9.98164226552912 - }, - { - "x": 1.692516979833633, - "y": 5.879848304102219, - "heading": 0.07335850742026521, - "angularVelocity": -0.429648016667122, - "velocityX": -0.06224534910652133, - "velocityY": -1.7892434366922005, - "timestamp": 10.060239350959305 - }, - { - "x": 1.7224083305171312, - "y": 5.755218848404405, - "heading": 0.044801662501508355, - "angularVelocity": -0.3633321103760591, - "velocityX": 0.38031118482184595, - "velocityY": -1.585675283195029, - "timestamp": 10.138836436389491 - }, - { - "x": 1.7819268218986297, - "y": 5.6548248770557, - "heading": 0.022621166422217082, - "angularVelocity": -0.282205071064539, - "velocityX": 0.757260794795832, - "velocityY": -1.277324353685865, - "timestamp": 10.217433521819677 - }, - { - "x": 1.8649022251310534, - "y": 5.584669337944561, - "heading": 0.0075685613575453385, - "angularVelocity": -0.19151607190373818, - "velocityX": 1.055705854463611, - "velocityY": -0.8925972092623536, - "timestamp": 10.296030607249863 - }, - { - "x": 1.9654334653384358, + "heading": 0.1387804363394676, + "angularVelocity": -0.4783884789404854, + "velocityX": -0.8860721033672938, + "velocityY": -1.8967410649584446, + "timestamp": 10.211362755442659 + }, + { + "x": 1.706182040858774, + "y": 6.03669580225492, + "heading": 0.10729245974250749, + "angularVelocity": -0.45130411484825544, + "velocityX": -0.48472113692462804, + "velocityY": -1.8796833159166881, + "timestamp": 10.281133837870186 + }, + { + "x": 1.699651780790286, + "y": 5.911922041931935, + "heading": 0.07852355852866735, + "angularVelocity": -0.4123327346071031, + "velocityX": -0.09359551036449741, + "velocityY": -1.7883305802599465, + "timestamp": 10.350904920297713 + }, + { + "x": 1.7185433856985064, + "y": 5.798927064131694, + "heading": 0.053308691164350094, + "angularVelocity": -0.36139424080898114, + "velocityX": 0.2707655414095584, + "velocityY": -1.6195101733961197, + "timestamp": 10.42067600272524 + }, + { + "x": 1.7598996151781712, + "y": 5.702665752208848, + "heading": 0.032386709878439925, + "angularVelocity": -0.29986608431425904, + "velocityX": 0.592741692414218, + "velocityY": -1.3796734775160497, + "timestamp": 10.490447085152768 + }, + { + "x": 1.820114668725021, + "y": 5.6271104583982785, + "heading": 0.016317787108466864, + "angularVelocity": -0.2303092084985792, + "velocityX": 0.8630373996189019, + "velocityY": -1.082902703839363, + "timestamp": 10.560218167580295 + }, + { + "x": 1.8954858630630067, + "y": 5.575103001600796, + "heading": 0.005460341828234747, + "angularVelocity": -0.15561526211822638, + "velocityX": 1.0802640824194598, + "velocityY": -0.7454013179672614, + "timestamp": 10.629989250007823 + }, + { + "x": 1.9826012990017379, "y": 5.548515796661377, - "heading": 4.814116720601663e-28, - "angularVelocity": -0.09629569997564517, - "velocityX": 1.2790708415858416, - "velocityY": -0.4599857753669162, - "timestamp": 10.374627692680049 + "heading": -3.619508887306417e-27, + "angularVelocity": -0.07826081577430707, + "velocityX": 1.248589428567619, + "velocityY": -0.3810633863540251, + "timestamp": 10.69976033243535 }, { "x": 2.0785037517547607, "y": 5.548515796661377, - "heading": 4.711208229171849e-28, - "angularVelocity": -4.224112944688565e-28, - "velocityX": 1.43860660732464, - "velocityY": -1.1518576201030206e-26, - "timestamp": 10.453224778110235 + "heading": -1.3504694626823558e-27, + "angularVelocity": 2.3770174727505955e-28, + "velocityX": 1.3745300978043182, + "velocityY": 2.5815580956362606e-25, + "timestamp": 10.769531414862877 }, { - "x": 2.2115594481070073, + "x": 2.2108243531689626, "y": 5.548515796661377, - "heading": 9.919579688456685e-28, - "angularVelocity": -1.4478744000102224e-30, - "velocityX": 1.8779766683240853, - "velocityY": 6.343973751493874e-16, - "timestamp": 10.52407533690391 + "heading": -3.98229506769339e-28, + "angularVelocity": 6.58921025141663e-29, + "velocityX": 1.7982508177997887, + "velocityY": 1.343058267982166e-16, + "timestamp": 10.843114365641734 }, { - "x": 2.367207543861562, + "x": 2.366717480768576, "y": 5.548515796661377, - "heading": 1.5127951147748595e-27, - "angularVelocity": -1.4478743900360266e-30, - "velocityX": 2.196850644577389, - "velocityY": 3.1719689645397288e-15, - "timestamp": 10.594925895697587 + "heading": 5.540104511762132e-28, + "angularVelocity": 6.589213013653999e-29, + "velocityX": 2.118603915030903, + "velocityY": 6.71525155127488e-16, + "timestamp": 10.91669731642059 }, { - "x": 2.4917260252771096, + "x": 2.4914319874847974, "y": 5.548515796661377, - "heading": 2.033632260703789e-27, - "angularVelocity": -1.4478743937385766e-30, - "velocityX": 1.757480583578164, - "velocityY": 2.537576634676114e-15, - "timestamp": 10.665776454491263 + "heading": 1.506250408105455e-27, + "angularVelocity": 6.589211632476856e-29, + "velocityX": 1.694883195035658, + "velocityY": 5.372204471623799e-16, + "timestamp": 10.990280267199447 }, { - "x": 2.585114887925518, + "x": 2.58496786905088, "y": 5.548515796661377, - "heading": 2.5544694066327203e-27, - "angularVelocity": -1.4478743937382235e-30, - "velocityX": 1.3181104600793105, - "velocityY": 1.903182958104165e-15, - "timestamp": 10.73662701328494 + "heading": 2.458490365034698e-27, + "angularVelocity": 6.589211632476792e-29, + "velocityX": 1.2711624170548799, + "velocityY": 4.029154421641203e-16, + "timestamp": 11.063863217978303 }, { - "x": 2.6473741303307423, + "x": 2.647325124044576, "y": 5.548515796661377, - "heading": 3.0753065525616534e-27, - "angularVelocity": -1.4478743937387e-30, - "velocityX": 0.8787403157472581, - "velocityY": 1.2687888330822125e-15, - "timestamp": 10.807477572078616 + "heading": 3.4107303219639416e-27, + "angularVelocity": 6.589211632476844e-29, + "velocityX": 0.8474416197456015, + "velocityY": 2.686103379493745e-16, + "timestamp": 11.13744616875716 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 3.596143698490385e-27, - "angularVelocity": -1.4478743965967547e-30, - "velocityX": 0.439370160998608, - "velocityY": 6.343944838152068e-16, - "timestamp": 10.878328130872292 + "heading": 4.3629702788936556e-27, + "angularVelocity": 6.589211633112284e-29, + "velocityX": 0.42372081277207485, + "velocityY": 1.3430518397832439e-16, + "timestamp": 11.211029119536017 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 4.090371729515312e-27, - "angularVelocity": -3.7701463571595157e-28, - "velocityX": -1.25991204352499e-25, - "velocityY": -5.479637988962548e-26, - "timestamp": 10.949178689665969 - }, - { - "x": 2.6551487281624913, - "y": 5.535337362195577, - "heading": -0.005358266388786186, - "angularVelocity": -0.0814332978786503, - "velocityX": -0.3549425234125069, - "velocityY": -0.20028182653884274, - "timestamp": 11.014978141835657 - }, - { - "x": 2.6089441252483114, - "y": 5.508126235802502, - "heading": -0.016249929633628903, - "angularVelocity": -0.16552817517013077, - "velocityX": -0.7022034590048662, - "velocityY": -0.41354639736059473, - "timestamp": 11.080777594005346 - }, - { - "x": 2.5407349136089206, - "y": 5.4655968271259, - "heading": -0.03293816472196892, - "angularVelocity": -0.2536227056313986, - "velocityX": -1.036622789251932, - "velocityY": -0.6463489782091849, - "timestamp": 11.146577046175034 - }, - { - "x": 2.4521626111212957, - "y": 5.405634188324023, - "heading": -0.055847587039488555, - "angularVelocity": -0.3481704111827463, - "velocityX": -1.3460948315984265, - "velocityY": -0.9112938911290818, - "timestamp": 11.212376498344723 - }, - { - "x": 2.347299393995622, - "y": 5.324395116461008, - "heading": -0.08569447081932854, - "angularVelocity": -0.45360383400866283, - "velocityX": -1.5936791822406984, - "velocityY": -1.2346466297851597, - "timestamp": 11.278175950514411 - }, - { - "x": 2.239880929566337, - "y": 5.216483135728542, - "heading": -0.1227261468780359, - "angularVelocity": -0.5627961151288575, - "velocityX": -1.6325130512069794, - "velocityY": -1.6400133614209302, - "timestamp": 11.3439754026841 - }, - { - "x": 2.153374403298574, - "y": 5.091739159361965, - "heading": -0.1618030539678993, - "angularVelocity": -0.5938789123819666, - "velocityX": -1.3146997948352968, - "velocityY": -1.8958208959685574, - "timestamp": 11.409774854853788 + "heading": 5.369537146588021e-27, + "angularVelocity": 8.042005177965745e-28, + "velocityX": 2.643601734591811e-26, + "velocityY": 4.252096317345015e-26, + "timestamp": 11.284612070314873 + }, + { + "x": 2.65514872822099, + "y": 5.535337362244174, + "heading": -0.005358266322899027, + "angularVelocity": -0.07847108857121549, + "velocityX": -0.34203117386479875, + "velocityY": -0.1929963969806516, + "timestamp": 11.352895388674044 + }, + { + "x": 2.608944125410673, + "y": 5.508126235941394, + "heading": -0.01624992945284291, + "angularVelocity": -0.15950693949367814, + "velocityX": -0.676660184662962, + "velocityY": -0.39850327952208153, + "timestamp": 11.421178707033215 + }, + { + "x": 2.5407349138993025, + "y": 5.465596827387689, + "heading": -0.03293816440101749, + "angularVelocity": -0.24439695300679784, + "velocityX": -0.9989147151957237, + "velocityY": -0.622837459802409, + "timestamp": 11.489462025392386 + }, + { + "x": 2.4521626115248987, + "y": 5.405634188727449, + "heading": -0.05584758658914312, + "angularVelocity": -0.33550540217775826, + "velocityX": -1.2971294380936889, + "velocityY": -0.8781447665568274, + "timestamp": 11.557745343751558 + }, + { + "x": 2.3472993944122122, + "y": 5.324395116991271, + "heading": -0.08569447031240085, + "angularVelocity": -0.43710359192361903, + "velocityX": -1.5357076901433586, + "velocityY": -1.1897352631408427, + "timestamp": 11.626028662110729 + }, + { + "x": 2.239880929754108, + "y": 5.216483136191594, + "heading": -0.12272614650889033, + "angularVelocity": -0.542323909943895, + "velocityX": -1.5731289462688671, + "velocityY": -1.580356423688402, + "timestamp": 11.6943119804699 + }, + { + "x": 2.153374403355559, + "y": 5.091739159562662, + "heading": -0.16180305380781881, + "angularVelocity": -0.5722760439582536, + "velocityX": -1.2668764271754205, + "velocityY": -1.8268587354348766, + "timestamp": 11.762595298829071 }, { "x": 2.0932393074035645, "y": 4.9619622230529785, - "heading": -0.19963034769256285, - "angularVelocity": -0.5748876696892882, - "velocityX": -0.9139148414173566, - "velocityY": -1.9723102857195454, - "timestamp": 11.475574307023477 - }, - { - "x": 2.0629011107007105, - "y": 4.862373019847467, - "heading": -0.22719939375272252, - "angularVelocity": -0.5460591677053324, - "velocityX": -0.600907641312322, - "velocityY": -1.9725599970405534, - "timestamp": 11.52606159428213 - }, - { - "x": 2.048028599586168, - "y": 4.7660067703914, - "heading": -0.2526003668147138, - "angularVelocity": -0.5031162187791786, - "velocityX": -0.29457932723437075, - "velocityY": -1.908723060567117, - "timestamp": 11.576548881540782 - }, - { - "x": 2.047740775609064, - "y": 4.675697634358311, - "heading": -0.2752320074015959, - "angularVelocity": -0.44826414362367223, - "velocityX": -0.00570091983015821, - "velocityY": -1.788750018800283, - "timestamp": 11.627036168799435 - }, - { - "x": 2.0608710405337267, - "y": 4.5937072504707785, - "heading": -0.29463334748351117, - "angularVelocity": -0.3842816902108431, - "velocityX": 0.2600707155723077, - "velocityY": -1.62398077495204, - "timestamp": 11.677523456058088 - }, - { - "x": 2.0861920181883997, - "y": 4.5217424317614965, - "heading": -0.3104741156384499, - "angularVelocity": -0.31375756185482273, - "velocityX": 0.5015317524380102, - "velocityY": -1.4254047427939316, - "timestamp": 11.72801074331674 - }, - { - "x": 2.122546344803851, - "y": 4.461060494351438, - "heading": -0.3225286157246152, - "angularVelocity": -0.23876307761216561, - "velocityX": 0.7200689240680361, - "velocityY": -1.2019250925322578, - "timestamp": 11.778498030575394 - }, - { - "x": 2.1689002066225895, - "y": 4.412584630951134, - "heading": -0.3306480218329806, - "angularVelocity": -0.16082080359692716, - "velocityX": 0.9181293813878815, - "velocityY": -0.9601597953155385, - "timestamp": 11.828985317834046 - }, - { - "x": 2.2243532862123785, - "y": 4.376998478332531, + "heading": -0.19963034769606672, + "angularVelocity": -0.5539756238745723, + "velocityX": -0.8806703803655747, + "velocityY": -1.9005657549777475, + "timestamp": 11.830878617188242 + }, + { + "x": 2.062901110669511, + "y": 4.862373019706677, + "heading": -0.22719939385993115, + "angularVelocity": -0.526195783497447, + "velocityX": -0.5790490938819385, + "velocityY": -1.9008063815925753, + "timestamp": 11.883271751129039 + }, + { + "x": 2.0480285995415843, + "y": 4.7660067701880315, + "heading": -0.2526003669786383, + "angularVelocity": -0.4848149215011564, + "velocityX": -0.28386374338156817, + "velocityY": -1.8392915687680964, + "timestamp": 11.935664885069835 + }, + { + "x": 2.0477407755531423, + "y": 4.675697634157938, + "heading": -0.27523200757833177, + "angularVelocity": -0.43195813835581237, + "velocityX": -0.005493544035129206, + "velocityY": -1.723682651473774, + "timestamp": 11.988058019010632 + }, + { + "x": 2.060871040463269, + "y": 4.593707250314659, + "heading": -0.2946333476397457, + "angularVelocity": -0.37030310275649647, + "velocityX": 0.2506104125201524, + "velocityY": -1.5649070341149178, + "timestamp": 12.040451152951428 + }, + { + "x": 2.0861920181034543, + "y": 4.521742431666527, + "heading": -0.3104741157540841, + "angularVelocity": -0.30234435169001433, + "velocityX": 0.48328809016841023, + "velocityY": -1.373554380798257, + "timestamp": 12.092844286892225 + }, + { + "x": 2.122546344711596, + "y": 4.4610604943149115, + "heading": -0.32252861579276854, + "angularVelocity": -0.23007785814655887, + "velocityX": 0.6938757786320086, + "velocityY": -1.1582040009323555, + "timestamp": 12.145237420833022 + }, + { + "x": 2.168900206537902, + "y": 4.412584630956035, + "heading": -0.3306480218589531, + "angularVelocity": -0.15497080352855533, + "velocityX": 0.8847316115635476, + "velocityY": -0.9252331309986794, + "timestamp": 12.197630554773818 + }, + { + "x": 2.224353286156776, + "y": 4.376998478351871, "heading": -0.3347371673945201, - "angularVelocity": -0.0809935685510361, - "velocityX": 1.098357281620145, - "velocityY": -0.7048537275590006, - "timestamp": 11.8794726050927 + "angularVelocity": -0.07804735521619326, + "velocityX": 1.0584035626029547, + "velocityY": -0.6792140482448454, + "timestamp": 12.250023688714615 }, { "x": 2.2881293296813965, "y": 4.354815483093262, "heading": -0.3347371673945201, - "angularVelocity": 5.296142385201319e-27, - "velocityX": 1.2632099471355256, - "velocityY": -0.43937784031895555, - "timestamp": 11.929959892351352 + "angularVelocity": 3.779464021226463e-27, + "velocityX": 1.2172595668105288, + "velocityY": -0.4233950823341772, + "timestamp": 12.302416822655411 }, { - "x": 2.4097151173059284, - "y": 4.31443319431648, + "x": 2.4097151176038345, + "y": 4.314433194202397, "heading": -0.3347371673945201, - "angularVelocity": 4.260827770605265e-29, - "velocityX": 1.6917387541138205, - "velocityY": -0.5618772081689671, - "timestamp": 12.001830201717304 + "angularVelocity": 6.573584275341322e-29, + "velocityX": 1.6302002618641742, + "velocityY": -0.5414384283676151, + "timestamp": 12.377000167270632 }, { - "x": 2.501147235462863, - "y": 4.284859003573599, + "x": 2.5011472356587063, + "y": 4.284859003501582, "heading": -0.3347371673945201, - "angularVelocity": 4.260827800538464e-29, - "velocityX": 1.2721820590944748, - "velocityY": -0.41149385613875655, - "timestamp": 12.073700511083256 + "angularVelocity": 6.573584272217567e-29, + "velocityX": 1.225905308035939, + "velocityY": -0.396525401929742, + "timestamp": 12.451583511885852 }, { - "x": 2.562139216145648, - "y": 4.265257651838079, + "x": 2.562139216220098, + "y": 4.265257651811236, "heading": -0.3347371673945201, - "angularVelocity": 4.260827801548558e-29, - "velocityX": 0.8486394621209058, - "velocityY": -0.27273225770760434, - "timestamp": 12.145570820449208 + "angularVelocity": -6.199774132429429e-28, + "velocityX": 0.8177694480725244, + "velocityY": -0.2628113795576003, + "timestamp": 12.526166856501073 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": 1.3873453317425255e-27, - "velocityX": 0.4244517046157105, - "velocityY": -0.13595549514112237, - "timestamp": 12.21744112981516 + "angularVelocity": 7.63642932425348e-28, + "velocityX": 0.40901189675210486, + "velocityY": -0.1310099931742232, + "timestamp": 12.600750201116293 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": -3.25532193935464e-28, - "velocityX": 2.1483187881556724e-26, - "velocityY": -1.7096603516619137e-26, - "timestamp": 12.289311439181112 + "angularVelocity": 1.6433960683327454e-29, + "velocityX": 1.171390593825864e-26, + "velocityY": -5.961445669521376e-27, + "timestamp": 12.675333545731513 } ], "eventMarkers": [] diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 23e383fc..aa076f38 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -111,7 +111,7 @@ public static class Vision { public static final String REAR_CAMERA_NAME = "limelight-rear"; public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.086018, 0, 0.627079, new Rotation3d(0, Math.toRadians(-40.843), 0)); - public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(0, Math.toRadians(10), Math.PI + 0.043)); + public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(0, Math.toRadians(10), Math.PI)); public static final boolean TAKE_SNAPSHOTS = true; public static final double SNAPSHOT_PERIOD_SECS = 1; diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 448e7be7..0e355ae7 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -273,7 +273,7 @@ private void configureAutoRoutines() { // autos.add(new AmpLanePADESprint(drivetrain, shooter, indexer)); // autos.add(new CenterLanePSubSprint(drivetrain, shooter, indexer)); // autos.add(new CenterLanePCBADSprint(drivetrain, shooter, indexer)); -// autos.add(new CenterLanePCBAFSprint(drivetrain, shooter, indexer)); + autos.add(new CenterLanePCBAFSprint(drivetrain, shooter, indexer)); //// autos.add(new CenterLanePCBAFE(drivetrain, shooter, indexer)); // autos.add(new CenterLanePCBA(drivetrain, shooter, indexer)); // autos.add(new CenterLanePBDA(drivetrain, shooter, indexer)); @@ -306,6 +306,7 @@ public void enableBrakeMode(boolean requireRobotDisabled) { Logger.recordOutput("brakeMode", true); } } + public void disableBrakeMode() { if (isBrakeMode && !DriverStation.isEnabled()) { shooter.setPivotBrakeMode(false); diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java index f982d442..79cf2dcd 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java @@ -30,35 +30,14 @@ public CenterLanePDEABC(Drivetrain drivetrain, Shooter shooter, Indexer indexer) Commands.parallel( new LeadingShootPrepare(drivetrain, shooter), Commands.sequence( - Commands.runOnce(drivetrain::unblockTags), -// drivetrain.commandCopyVisionPose(), -// Commands.deadline( -// Commands.waitSeconds(0.2).andThen(getPath(0).getCommand(drivetrain)), + Commands.deadline( Commands.sequence( -// Commands.waitSeconds(0.25), -// Commands.waitSeconds(0.05), - IntakeAndFeed.withDefaults(indexer).withTimeout(0.2) -// new IntakeCommand(indexer, () -> false, 1) - ), -// ) -// , -// Commands.sequence( -// Commands.waitSeconds(0.25), -// new InstantCommand(shooter::zeroPivotToCancoder) -// ), -// drivetrain.commandCopyVisionPose(), -// Commands.parallel( -// new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4), -// Commands.sequence( -// new ParallelDeadlineGroup( -// Commands.sequence( -// Commands.waitUntil(()->!indexer.isNoteStaged()), -// Commands.waitSeconds(0.2) -// ).withTimeout(1), -// IntakeAndFeed.withDefaults(indexer) -// ) -// ) -// ), + Commands.waitSeconds(0.1), + Commands.waitUntil(()->!indexer.isNoteStaged()), + Commands.waitSeconds(0.1) + ).withTimeout(1.1), + Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer)) + ), createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), createSegmentSequence(drivetrain, shooter, indexer, 1), Commands.runOnce(drivetrain::blockTags), diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java index 40d0f6d9..db2cda7c 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -20,7 +20,7 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { Pose3d limelightPose = LimelightHelpers.getBotPose3d_wpiBlue(name); inputs.estimatedPoseMeters = new Pose3d( limelightPose.getX(), - limelightPose.getY() - 10.5, + limelightPose.getY() + 0.105, limelightPose.getZ(), limelightPose.getRotation()); inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds; diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index 4c392ab0..3948e432 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -96,16 +96,19 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, new InstantCommand(shooter::zeroPivotToCancoder) ).onlyIf(()->shouldZeroCancoder), drivetrain.commandCopyVisionPose().onlyIf(()->shouldResetOdometry), - Commands.deadline( - Commands.deadline( - Commands.sequence( - Commands.waitSeconds(extraPreShotWait), - Commands.waitUntil(()->!indexer.isNoteStaged()), - Commands.waitSeconds(0.1) - ).withTimeout(1+extraPreShotWait), - Commands.waitSeconds(extraPreShotWait).andThen(IntakeAndFeed.withDefaults(indexer)) - ), - new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4).onlyIf(()->shouldRealignYaw) + Commands.parallel( + new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4).onlyIf(()->shouldRealignYaw), + Commands.sequence( +// Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10).onlyIf(()->shouldRealignYaw), + new ParallelDeadlineGroup( + Commands.sequence( + Commands.waitSeconds(extraPreShotWait), + Commands.waitUntil(()->!indexer.isNoteStaged()), + Commands.waitSeconds(0.2) + ).withTimeout(1+extraPreShotWait), + IntakeAndFeed.withDefaults(indexer) + ) + ) ) ); } diff --git a/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java b/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java index 79034e29..628c79f2 100644 --- a/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java +++ b/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java @@ -23,6 +23,6 @@ public Matrix getStdDevs() { Constants.Vision.ROT_STD_DEV_COEFF * Math.pow(avgDistance, 2.0) / numTags; - return VecBuilder.fill(xyStdDev, xyStdDev, rotStdDev/*numTags > 1 ? rotStdDev : Double.POSITIVE_INFINITY*/); + return VecBuilder.fill(xyStdDev, xyStdDev, numTags > 1 ? rotStdDev : Double.POSITIVE_INFINITY); } } From b05021ec3924aa0e24e7915741d7a617e8359e15 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Wed, 3 Apr 2024 19:18:32 -0700 Subject: [PATCH 71/75] fix: make 6 piece faster and it might work --- paths.chor | 3404 ++++++++--------- .../deploy/choreo/CenterLanePDEABC.1.traj | 1145 +++--- .../deploy/choreo/CenterLanePDEABC.2.traj | 1352 ++++--- .../deploy/choreo/CenterLanePDEABC.3.traj | 100 +- .../deploy/choreo/CenterLanePDEABC.4.traj | 415 +- .../deploy/choreo/CenterLanePDEABC.5.traj | 318 +- src/main/deploy/choreo/CenterLanePDEABC.traj | 3340 ++++++++-------- .../commands/autos/CenterLanePDEABC.java | 15 +- .../robot2024/util/auto/AutoCommand.java | 11 +- 9 files changed, 4971 insertions(+), 5129 deletions(-) diff --git a/paths.chor b/paths.chor index 8da6a0fa..2c583f13 100644 --- a/paths.chor +++ b/paths.chor @@ -3,7 +3,7 @@ "robotConfiguration": { "mass": 61.73998083590995, "rotationalInertia": 3.6816072891155396, - "motorMaxTorque": 0.65, + "motorMaxTorque": 0.7, "motorMaxVelocity": 4864, "gearing": 6.746, "wheelbase": 0.4762497428251389, @@ -28969,7 +28969,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 10 }, { "x": 2.3368515968322754, @@ -28978,7 +28978,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { "x": 5.747424125671387, @@ -28987,7 +28987,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 11 }, { "x": 7.125290870666504, @@ -29050,7 +29050,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 13 }, { "x": 3.405486822128296, @@ -29059,7 +29059,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 9 }, { "x": 2.205486822128296, @@ -29095,7 +29095,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 8 + "controlIntervalCount": 7 }, { "x": 2.0785037517547607, @@ -29147,1856 +29147,1802 @@ { "x": 1.3350272178649902, "y": 5.601006507873535, - "heading": 3.967978705267542e-25, - "angularVelocity": 7.379244608128328e-26, - "velocityX": 8.172146600807894e-26, - "velocityY": -1.420872098935511e-25, + "heading": 1.1438500584259617e-25, + "angularVelocity": 2.0139373174315835e-26, + "velocityX": -2.9204847474037974e-26, + "velocityY": 3.705827554701837e-26, "timestamp": 0 }, { - "x": 1.3486609048876415, - "y": 5.6125344531964885, - "heading": 0.0053952990645401815, - "angularVelocity": 0.09676162177278211, - "velocityX": 0.24451242670209422, - "velocityY": 0.20674714632375757, - "timestamp": 0.05575866718325111 - }, - { - "x": 1.3762911400128175, - "y": 5.635152571560341, - "heading": 0.015980040657686934, - "angularVelocity": 0.1898313235924372, - "velocityX": 0.4955325606038828, - "velocityY": 0.4056430956198756, - "timestamp": 0.11151733436650221 - }, - { - "x": 1.4183448247504244, - "y": 5.668306854106291, - "heading": 0.0314941432622812, - "angularVelocity": 0.2782366112447971, - "velocityX": 0.7542089304143026, - "velocityY": 0.594603210958904, - "timestamp": 0.1672760015497533 - }, - { - "x": 1.4753265664194932, - "y": 5.711278723492, - "heading": 0.05160335039247586, - "angularVelocity": 0.3606471988310923, - "velocityX": 1.0219351456482668, - "velocityY": 0.7706760501373088, - "timestamp": 0.22303466873300443 - }, - { - "x": 1.547831871808343, - "y": 5.763109379844883, - "heading": 0.07586790196130719, - "angularVelocity": 0.43517093923866185, - "velocityX": 1.3003414366157815, - "velocityY": 0.9295533586292857, - "timestamp": 0.27879333591625555 - }, - { - "x": 1.6365515186230641, - "y": 5.822478611934523, - "heading": 0.10369270639110653, - "angularVelocity": 0.499022050479668, - "velocityX": 1.5911364330704598, - "velocityY": 1.064753429175822, - "timestamp": 0.33455200309950667 - }, - { - "x": 1.7422414603963892, - "y": 5.887510284275809, - "heading": 0.1342490325748326, - "angularVelocity": 0.5480103404068561, - "velocityX": 1.8954890264140394, - "velocityY": 1.1663060762833288, - "timestamp": 0.3903106702827578 - }, - { - "x": 1.8655876183871098, - "y": 5.955479067462068, - "heading": 0.1663576179952195, - "angularVelocity": 0.57584922743691, - "velocityX": 2.212143227623125, - "velocityY": 1.2189814896916369, - "timestamp": 0.4460693374660089 - }, - { - "x": 2.0068104942426706, - "y": 6.022477551407248, - "heading": 0.1983353246164728, - "angularVelocity": 0.5735019905723076, - "velocityX": 2.532752000535291, - "velocityY": 1.201579724368033, - "timestamp": 0.50182800464926 - }, - { - "x": 2.164876110933553, - "y": 6.083435356761985, - "heading": 0.22790661577104523, - "angularVelocity": 0.5303442971006865, - "velocityX": 2.834816983185756, - "velocityY": 1.093243587663212, - "timestamp": 0.5575866718325111 + "x": 1.3513037651461415, + "y": 5.614995878157814, + "heading": 0.006549885022033663, + "angularVelocity": 0.1111819339605223, + "velocityX": 0.27628851481066125, + "velocityY": 0.23746451088284168, + "timestamp": 0.05891141473001675 + }, + { + "x": 1.384378906601625, + "y": 5.642351283609904, + "heading": 0.019354935835911788, + "angularVelocity": 0.21736111537232614, + "velocityX": 0.5614385871916767, + "velocityY": 0.46434813316664775, + "timestamp": 0.1178228294600335 + }, + { + "x": 1.4348748758417986, + "y": 5.682261570187258, + "heading": 0.0380334905813409, + "angularVelocity": 0.3170617244727609, + "velocityX": 0.8571508505030827, + "velocityY": 0.6774627083776107, + "timestamp": 0.17673424419005024 + }, + { + "x": 1.5035350329878103, + "y": 5.733639392412991, + "heading": 0.06208045701070026, + "angularVelocity": 0.4081885749911699, + "velocityX": 1.1654813835429356, + "velocityY": 0.8721199866136561, + "timestamp": 0.235645658920067 + }, + { + "x": 1.5912385139518137, + "y": 5.794977395067809, + "heading": 0.09080789039928205, + "angularVelocity": 0.48763781213260343, + "velocityX": 1.4887349313530633, + "velocityY": 1.0411904541067623, + "timestamp": 0.29455707365008377 + }, + { + "x": 1.6989811168528033, + "y": 5.864111533799202, + "heading": 0.12324878614182731, + "angularVelocity": 0.550672495155949, + "velocityX": 1.8288917927834498, + "velocityY": 1.1735270498633483, + "timestamp": 0.35346848838010053 + }, + { + "x": 1.8277399268235333, + "y": 5.93784849269496, + "heading": 0.15800938597032024, + "angularVelocity": 0.590048634679649, + "velocityX": 2.1856343216474197, + "velocityY": 1.2516582606899704, + "timestamp": 0.4123799031101173 + }, + { + "x": 1.978020614375797, + "y": 6.011501208422767, + "heading": 0.19307007056378325, + "angularVelocity": 0.5951424652444947, + "velocityX": 2.5509604249190123, + "velocityY": 1.250228263322945, + "timestamp": 0.47129131784013406 + }, + { + "x": 2.1488551922208927, + "y": 6.078800735840598, + "heading": 0.2256402887317746, + "angularVelocity": 0.5528676966468451, + "velocityX": 2.899855293375791, + "velocityY": 1.1423851850487134, + "timestamp": 0.5302027325701508 }, { "x": 2.3368515968322754, "y": 6.133185386657715, "heading": 0.2525545797921912, - "angularVelocity": 0.44204722362068527, - "velocityX": 3.0842825803838596, - "velocityY": 0.8922385058492518, - "timestamp": 0.6133453390157622 - }, - { - "x": 2.5269005159766844, - "y": 6.188163800352406, - "heading": 0.252554892201178, - "angularVelocity": 0.000005553817937610827, - "velocityX": 3.378574691383036, - "velocityY": 0.9773729727982717, - "timestamp": 0.6695965504005877 - }, - { - "x": 2.7281832415838734, - "y": 6.246391992377104, - "heading": 0.2525548726806658, - "angularVelocity": -3.47023853848989e-7, - "velocityX": 3.578282505423293, - "velocityY": 1.0351455656012056, - "timestamp": 0.7258477617854133 - }, - { - "x": 2.929465967191096, - "y": 6.304620184401812, - "heading": 0.2525548531602303, - "angularVelocity": -3.4702249119339613e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 0.7820989731702389 - }, - { - "x": 3.1307486927983192, - "y": 6.36284837642652, - "heading": 0.25255483363979486, - "angularVelocity": -3.4702249016578237e-7, - "velocityX": 3.5782825054238923, - "velocityY": 1.0351455656013788, - "timestamp": 0.8383501845550645 - }, - { - "x": 3.332031418405542, - "y": 6.421076568451228, - "heading": 0.2525548141193594, - "angularVelocity": -3.470224908197631e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 0.8946013959398901 - }, - { - "x": 3.5333141440127647, - "y": 6.4793047604759355, - "heading": 0.2525547945989239, - "angularVelocity": -3.4702249080835237e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 0.9508526073247157 - }, - { - "x": 3.7345968696199874, - "y": 6.537532952500643, - "heading": 0.2525547750784885, - "angularVelocity": -3.470224894869701e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 1.0071038187095414 - }, - { - "x": 3.93587959522721, - "y": 6.595761144525352, - "heading": 0.25255475555805307, - "angularVelocity": -3.470224898418443e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 1.063355030094367 - }, - { - "x": 4.137162320834433, - "y": 6.65398933655006, - "heading": 0.2525547360376176, - "angularVelocity": -3.470224909887922e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.1196062414791927 - }, - { - "x": 4.338445046441655, - "y": 6.712217528574768, - "heading": 0.2525547165171822, - "angularVelocity": -3.4702248928646837e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 1.1758574528640184 - }, - { - "x": 4.539727772048877, - "y": 6.770445720599476, - "heading": 0.25255469699674676, - "angularVelocity": -3.470224898754089e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 1.2321086642488441 - }, - { - "x": 4.7410104976561, - "y": 6.8286739126241836, - "heading": 0.25255467747631133, - "angularVelocity": -3.470224907627516e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.2883598756336698 - }, - { - "x": 4.942293223263322, - "y": 6.886902104648891, - "heading": 0.25255465795587584, - "angularVelocity": -3.4702249046766804e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 1.3446110870184955 - }, - { - "x": 5.143575948870544, - "y": 6.945130296673599, - "heading": 0.2525546384354404, - "angularVelocity": -3.47022489425383e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.4008622984033212 - }, - { - "x": 5.344858674477766, - "y": 7.003358488698307, - "heading": 0.25255461891500497, - "angularVelocity": -3.470224896167856e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.457113509788147 - }, - { - "x": 5.546141400084989, - "y": 7.061586680723015, - "heading": 0.25255459939456953, - "angularVelocity": -3.470224897168836e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 1.5133647211729726 + "angularVelocity": 0.45686037559548065, + "velocityX": 3.1911711078904768, + "velocityY": 0.9231598165882506, + "timestamp": 0.5891141473001675 + }, + { + "x": 2.5473350968820796, + "y": 6.1940752301450726, + "heading": 0.25255484664390665, + "angularVelocity": 0.000004475832490919022, + "velocityX": 3.5303834818832023, + "velocityY": 1.0212890683182199, + "timestamp": 0.6487347242516341 + }, + { + "x": 2.7606743654688186, + "y": 6.255791206318409, + "heading": 0.25255482885683184, + "angularVelocity": -2.9833785118107346e-7, + "velocityX": 3.578282524176262, + "velocityY": 1.0351455710261694, + "timestamp": 0.7083553012031008 + }, + { + "x": 2.974013634055561, + "y": 6.317507182491745, + "heading": 0.2525548110697642, + "angularVelocity": -2.983377309458156e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 0.7679758781545674 + }, + { + "x": 3.1873529026423038, + "y": 6.379223158665082, + "heading": 0.25255479328269653, + "angularVelocity": -2.983377305484016e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.827596455106034 + }, + { + "x": 3.400692171229046, + "y": 6.440939134838418, + "heading": 0.2525547754956289, + "angularVelocity": -2.9833773174604553e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 0.8872170320575006 + }, + { + "x": 3.6140314398157884, + "y": 6.502655111011755, + "heading": 0.25255475770856123, + "angularVelocity": -2.983377305928146e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.9468376090089672 + }, + { + "x": 3.827370708402531, + "y": 6.5643710871850915, + "heading": 0.25255473992149363, + "angularVelocity": -2.983377301697445e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.0064581859604338 + }, + { + "x": 4.040709976989273, + "y": 6.626087063358428, + "heading": 0.25255472213442604, + "angularVelocity": -2.983377305025075e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.0660787629119004 + }, + { + "x": 4.254049245576016, + "y": 6.687803039531764, + "heading": 0.2525547043473584, + "angularVelocity": -2.9833773095408974e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.125699339863367 + }, + { + "x": 4.467388514162758, + "y": 6.749519015705101, + "heading": 0.2525546865602907, + "angularVelocity": -2.983377319947091e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.1853199168148336 + }, + { + "x": 4.680727782749501, + "y": 6.811234991878438, + "heading": 0.252554668773223, + "angularVelocity": -2.9833773124406734e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.2449404937663002 + }, + { + "x": 4.894067051336243, + "y": 6.872950968051774, + "heading": 0.2525546509861554, + "angularVelocity": -2.983377305581398e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.3045610707177668 + }, + { + "x": 5.107406319922986, + "y": 6.9346669442251105, + "heading": 0.2525546331990878, + "angularVelocity": -2.983377313652051e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.3641816476692334 + }, + { + "x": 5.320745588509729, + "y": 6.996382920398447, + "heading": 0.2525546154120202, + "angularVelocity": -2.983377306356561e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4238022246207 + }, + { + "x": 5.534084857096471, + "y": 7.058098896571784, + "heading": 0.2525545976249525, + "angularVelocity": -2.983377306690358e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.4834228015721667 }, { "x": 5.747424125671387, "y": 7.119814872741699, "heading": 0.2525545797921912, - "angularVelocity": -3.4847922161224964e-7, - "velocityX": 3.5782825050536915, - "velocityY": 1.0351455654942852, - "timestamp": 1.5696159325577983 - }, - { - "x": 5.864122316952898, - "y": 7.149764367678907, - "heading": 0.23825610881476394, - "angularVelocity": -0.4289921260648423, - "velocityX": 3.501255852098466, - "velocityY": 0.8985644358731831, - "timestamp": 1.6029463134871351 - }, - { - "x": 5.980010019619152, - "y": 7.178464848632163, - "heading": 0.21993450293637717, - "angularVelocity": -0.5496968641681629, - "velocityX": 3.4769390398491176, - "velocityY": 0.8610906972261488, - "timestamp": 1.636276694416472 - }, - { - "x": 6.095759168980239, - "y": 7.207171809244793, - "heading": 0.2014022591938407, - "angularVelocity": -0.5560165598414963, - "velocityX": 3.4727820725027265, - "velocityY": 0.8612851042264353, - "timestamp": 1.6696070753458088 - }, - { - "x": 6.211373074459108, - "y": 7.235898623678772, - "heading": 0.18268720108591044, - "angularVelocity": -0.5615014766140173, - "velocityX": 3.4687243966392267, - "velocityY": 0.8618807716264242, - "timestamp": 1.7029374562751456 - }, - { - "x": 6.326838442963843, - "y": 7.264631799858999, - "heading": 0.16373191419922642, - "angularVelocity": -0.5687089783603383, - "velocityX": 3.464267892693179, - "velocityY": 0.8620716409195424, - "timestamp": 1.7362678372044824 - }, - { - "x": 6.442151068511899, - "y": 7.29337427879422, - "heading": 0.1445283357059787, - "angularVelocity": -0.5761583863671085, - "velocityX": 3.4596851980938594, - "velocityY": 0.8623507482904867, - "timestamp": 1.7695982181338192 - }, - { - "x": 6.557303014616615, - "y": 7.322124225476267, - "heading": 0.1250513432276187, - "angularVelocity": -0.5843615324905178, - "velocityX": 3.454864387804252, - "velocityY": 0.8625748005400329, - "timestamp": 1.802928599063156 - }, - { - "x": 6.672285593632433, - "y": 7.350880202549992, - "heading": 0.10527449948643015, - "angularVelocity": -0.5933578671998123, - "velocityX": 3.449782925061388, - "velocityY": 0.8627557283156009, - "timestamp": 1.8362589799924929 - }, - { - "x": 6.787113803468559, - "y": 7.379671810584065, - "heading": 0.08527801395894485, - "angularVelocity": -0.5999477044645701, - "velocityX": 3.4451514394501603, - "velocityY": 0.8638247518116766, - "timestamp": 1.8695893609218297 - }, - { - "x": 6.901739724666763, - "y": 7.408448228453197, - "heading": 0.0648615716893241, - "angularVelocity": -0.6125475227212472, - "velocityX": 3.4390822427508825, - "velocityY": 0.8633690064971079, - "timestamp": 1.9029197418511665 - }, - { - "x": 7.014861438150348, - "y": 7.4357314738869205, - "heading": 0.038575820671149744, - "angularVelocity": -0.7886423822728691, - "velocityX": 3.3939520140322568, - "velocityY": 0.8185698654799702, - "timestamp": 1.9362501227805033 + "angularVelocity": -2.991041384854961e-7, + "velocityX": 3.5782825239779474, + "velocityY": 1.0351455709687998, + "timestamp": 1.5430433785236333 + }, + { + "x": 5.874242280548121, + "y": 7.151641192910771, + "heading": 0.23431006193331627, + "angularVelocity": -0.5017601477559481, + "velocityX": 3.487748847149364, + "velocityY": 0.8752864413346503, + "timestamp": 1.5794044126561186 + }, + { + "x": 6.000453145534325, + "y": 7.182664404015401, + "heading": 0.21336082591069744, + "angularVelocity": -0.5761452203556151, + "velocityX": 3.4710471799658134, + "velocityY": 0.853199361481138, + "timestamp": 1.615765446788604 + }, + { + "x": 6.126521345623787, + "y": 7.213763203476394, + "heading": 0.192336966602575, + "angularVelocity": -0.5781975075714219, + "velocityX": 3.4671236145297133, + "velocityY": 0.8552781900449158, + "timestamp": 1.6521264809210894 + }, + { + "x": 6.252403333907122, + "y": 7.24487049169852, + "heading": 0.17101724585582967, + "angularVelocity": -0.5863342794119836, + "velocityX": 3.462002423381879, + "velocityY": 0.8555116476825977, + "timestamp": 1.6884875150535748 + }, + { + "x": 6.378086189949285, + "y": 7.2759799644241046, + "heading": 0.14935632557503462, + "angularVelocity": -0.5957179381056964, + "velocityX": 3.456525894841813, + "velocityY": 0.8555717258270096, + "timestamp": 1.7248485491860601 + }, + { + "x": 6.503564352522718, + "y": 7.307098484942146, + "heading": 0.12734794412408884, + "angularVelocity": -0.6052738041155772, + "velocityX": 3.4508964215989995, + "velocityY": 0.8558205579262937, + "timestamp": 1.7612095833185455 + }, + { + "x": 6.628828810809778, + "y": 7.338228595030231, + "heading": 0.10496993550474346, + "angularVelocity": -0.6154392787017191, + "velocityX": 3.4450191331370044, + "velocityY": 0.8561392939117064, + "timestamp": 1.7975706174510309 + }, + { + "x": 6.753876017529989, + "y": 7.3693809168618785, + "heading": 0.08222565272715081, + "angularVelocity": -0.6255125389096852, + "velocityX": 3.4390442874806206, + "velocityY": 0.8567501605740924, + "timestamp": 1.8339316515835162 + }, + { + "x": 6.878734995256494, + "y": 7.4006038692779965, + "heading": 0.05925591834951263, + "angularVelocity": -0.6317129016173038, + "velocityX": 3.4338676197042926, + "velocityY": 0.8586926406535731, + "timestamp": 1.8702926857160016 + }, + { + "x": 7.003212758632201, + "y": 7.431690611037589, + "heading": 0.0352626014950734, + "angularVelocity": -0.6598634342196367, + "velocityX": 3.4233834747977516, + "velocityY": 0.8549465795259067, + "timestamp": 1.906653719848487 }, { "x": 7.125290870666504, "y": 7.458, - "heading": 1.0084355524659913e-21, - "angularVelocity": -1.1573771314805474, - "velocityX": 3.313176430544709, - "velocityY": 0.668114959750724, - "timestamp": 1.9695805037098402 - }, - { - "x": 7.337738270409439, - "y": 7.481917447620179, - "heading": -0.00819620478546554, - "angularVelocity": -0.12615227280600286, - "velocityX": 3.2698941803922152, - "velocityY": 0.368126523919295, - "timestamp": 2.0345512299849795 - }, - { - "x": 7.526586631074925, - "y": 7.500039280530163, - "heading": -0.014041404226933134, - "angularVelocity": -0.08996666308937673, - "velocityX": 2.906668456586843, - "velocityY": 0.27892304656162725, - "timestamp": 2.099521956260119 - }, - { - "x": 7.6918053440422804, - "y": 7.512484823370471, - "heading": -0.017813194168849914, - "angularVelocity": -0.058053682914731695, - "velocityX": 2.5429716187515523, - "velocityY": 0.19155616004049963, - "timestamp": 2.164492682535258 - }, - { - "x": 7.833384352673449, - "y": 7.519294018241946, - "heading": -0.01960436856302971, - "angularVelocity": -0.027568945229186584, - "velocityX": 2.179119993696969, - "velocityY": 0.10480404424970338, - "timestamp": 2.2294634088103975 - }, - { - "x": 7.951318655599417, - "y": 7.520486868366768, - "heading": -0.019461534286698053, - "angularVelocity": 0.0021984405057560424, - "velocityX": 1.815191389835117, - "velocityY": 0.018359808997220107, - "timestamp": 2.294434135085537 - }, - { - "x": 8.045605259786509, - "y": 7.516075387181805, - "heading": -0.017412919986794666, - "angularVelocity": 0.031531343688969565, - "velocityX": 1.4512167185542217, - "velocityY": -0.06789952087470152, - "timestamp": 2.359404861360676 - }, - { - "x": 8.116242172260733, - "y": 7.5060675897274995, - "heading": -0.013477652900012242, - "angularVelocity": 0.06056984910584597, - "velocityX": 1.0872113723200465, - "velocityY": -0.15403548687333402, - "timestamp": 2.4243755876358155 - }, - { - "x": 8.163227969814436, - "y": 7.490469205298236, - "heading": -0.007669730702440132, - "angularVelocity": 0.08939290863052056, - "velocityX": 0.723184120718108, - "velocityY": -0.24008327016705255, - "timestamp": 2.489346313910955 + "heading": -1.1078640911459773e-20, + "angularVelocity": -0.9697909406699016, + "velocityX": 3.3573883402077644, + "velocityY": 0.7235599753997619, + "timestamp": 1.9430147539809723 + }, + { + "x": 7.338537136585377, + "y": 7.4820248133164355, + "heading": -0.00804052191695304, + "angularVelocity": -0.12790212002878093, + "velocityX": 3.392149139191805, + "velocityY": 0.38216730060631093, + "timestamp": 2.005879403254524 + }, + { + "x": 7.527981798214128, + "y": 7.500236040572311, + "heading": -0.013862931395131553, + "angularVelocity": -0.09261818121091622, + "velocityX": 3.013532467259838, + "velocityY": 0.2896894751870813, + "timestamp": 2.068744052528076 + }, + { + "x": 7.6935968882672885, + "y": 7.512744942179138, + "heading": -0.017658491883084444, + "angularVelocity": -0.06037670665172621, + "velocityX": 2.6344709143686895, + "velocityY": 0.1989814904143519, + "timestamp": 2.1316087018016274 + }, + { + "x": 7.835373213607451, + "y": 7.519588665029473, + "heading": -0.019490985187148625, + "angularVelocity": -0.029149821485366035, + "velocityX": 2.255263124482431, + "velocityY": 0.1088644083665287, + "timestamp": 2.194473351075179 + }, + { + "x": 7.953306201274802, + "y": 7.52078579458168, + "heading": -0.019392463666041717, + "angularVelocity": 0.001567200680277371, + "velocityX": 1.8759825916497619, + "velocityY": 0.01904296875969824, + "timestamp": 2.2573380003487307 + }, + { + "x": 8.047393114183997, + "y": 7.516347487170096, + "heading": -0.01738240796128744, + "angularVelocity": 0.0319743405551765, + "velocityX": 1.4966585194769915, + "velocityY": -0.0706010049029291, + "timestamp": 2.3202026496222823 + }, + { + "x": 8.11763212951421, + "y": 7.506281183777245, + "heading": -0.013474109300406987, + "angularVelocity": 0.062170054331707525, + "velocityX": 1.1173054513447094, + "velocityY": -0.1601266134333638, + "timestamp": 2.383067298895834 + }, + { + "x": 8.164021945337474, + "y": 7.490592202393592, + "heading": -0.007677398137362595, + "angularVelocity": 0.09220939319680839, + "velocityX": 0.737931673195224, + "velocityY": -0.24956762767234375, + "timestamp": 2.4459319481693855 }, { "x": 8.186561584472656, "y": 7.469284534454346, - "heading": 7.422813602625121e-23, - "angularVelocity": 0.11804902210820577, - "velocityX": 0.3591404313291331, - "velocityY": -0.3260648611834275, - "timestamp": 2.554317040186094 - }, - { - "x": 8.171272340508109, - "y": 7.427332842662171, - "heading": 0.014840203267270782, - "angularVelocity": 0.15900903302410063, - "velocityX": -0.16382039077820704, - "velocityY": -0.44950179087577585, - "timestamp": 2.6476463497544476 - }, - { - "x": 8.107175512886487, - "y": 7.3738609015025816, - "heading": 0.03350091514476021, - "angularVelocity": 0.1999448186619506, - "velocityX": -0.6867813328746101, - "velocityY": -0.572938355666582, - "timestamp": 2.7409756593228005 - }, - { - "x": 7.994271087246123, - "y": 7.308868753063173, - "heading": 0.05597944584590409, - "angularVelocity": 0.24085178391554368, - "velocityX": -1.2097424288526986, - "velocityY": -0.6963744694994183, - "timestamp": 2.8343049688911535 - }, - { - "x": 7.832559046421551, - "y": 7.232356448582914, - "heading": 0.08227288434163177, - "angularVelocity": 0.2817275582272545, - "velocityX": -1.732703708754388, - "velocityY": -0.8198100343196232, - "timestamp": 2.9276342784595064 - }, - { - "x": 7.6220393708156715, - "y": 7.144324050552095, - "heading": 0.1123783464235267, - "angularVelocity": 0.3225724289736236, - "velocityX": -2.2556651986340626, - "velocityY": -0.9432449295721523, - "timestamp": 3.0209635880278594 - }, - { - "x": 7.362712038546165, - "y": 7.04477163823656, - "heading": 0.14629325961596634, - "angularVelocity": 0.36338973629287186, - "velocityX": -2.778626922977279, - "velocityY": -1.0666789755111628, - "timestamp": 3.1142928975962123 - }, - { - "x": 7.054577023664908, - "y": 6.933699333854541, - "heading": 0.18401565672002676, - "angularVelocity": 0.4041859655720804, - "velocityX": -3.3015889253480717, - "velocityY": -1.1901117119126619, - "timestamp": 3.2076222071645653 - }, - { - "x": 6.71622230121249, - "y": 6.85383794706396, - "heading": 0.18401565782506035, - "angularVelocity": 1.1840156036202986e-8, - "velocityX": -3.6253854659088836, - "velocityY": -0.8556946061203927, - "timestamp": 3.300951516732918 - }, - { - "x": 6.377867419030211, - "y": 6.7739772370157985, - "heading": 0.1840156589300666, - "angularVelocity": 1.1839863049890439e-8, - "velocityX": -3.6253871773740243, - "velocityY": -0.8556873549961612, - "timestamp": 3.394280826301271 - }, - { - "x": 6.0395125368493074, - "y": 6.694116526961809, - "heading": 0.18401566003507294, - "angularVelocity": 1.1839864253851599e-8, - "velocityX": -3.6253871773592854, - "velocityY": -0.8556873550586043, - "timestamp": 3.487610135869624 - }, - { - "x": 5.701157654668755, - "y": 6.614255816906336, - "heading": 0.18401566114007933, - "angularVelocity": 1.18398644204111e-8, - "velocityX": -3.625387177355538, - "velocityY": -0.855687355074483, - "timestamp": 3.580939445437977 - }, - { - "x": 5.362802772497576, - "y": 6.534395106811441, - "heading": 0.18401566224512567, - "angularVelocity": 1.1840292694239514e-8, - "velocityX": -3.6253871772550927, - "velocityY": -0.8556873554969, - "timestamp": 3.67426875500633 - }, - { - "x": 5.069956587719004, - "y": 6.4652758877562615, - "heading": 0.20685757815363412, - "angularVelocity": 0.24474536471074324, - "velocityX": -3.137772968995303, - "velocityY": -0.7405949896646158, - "timestamp": 3.767598064574683 - }, - { - "x": 4.8259180814209035, - "y": 6.407676584765678, - "heading": 0.22589278292875978, - "angularVelocity": 0.20395741555534105, - "velocityX": -2.614811010890098, - "velocityY": -0.6171619961293916, - "timestamp": 3.860927374143036 - }, - { - "x": 4.6306872671740225, - "y": 6.361597170610555, - "heading": 0.2411211921270372, - "angularVelocity": 0.16316856160951593, - "velocityX": -2.0918489073777633, - "velocityY": -0.49372929434751617, - "timestamp": 3.954256683711389 - }, - { - "x": 4.484264152022265, - "y": 6.3270376241371595, - "heading": 0.25254264332581844, - "angularVelocity": 0.12237796734600637, - "velocityX": -1.5688867283917882, - "velocityY": -0.37029681922252144, - "timestamp": 4.047585993279742 - }, - { - "x": 4.386648740184571, - "y": 6.303997931141046, - "heading": 0.26015698140700544, - "angularVelocity": 0.08158571103122086, - "velocityX": -1.0459245042009158, - "velocityY": -0.24686449629458126, - "timestamp": 4.140915302848095 + "heading": 1.0473347586059488e-21, + "angularVelocity": 0.12212584061281963, + "velocityX": 0.358542350838588, + "velocityY": -0.33894514938796605, + "timestamp": 2.508796597442937 + }, + { + "x": 8.169433800706123, + "y": 7.42685355131065, + "heading": 0.014977206112370758, + "angularVelocity": 0.16513162181596408, + "velocityX": -0.1888428783219197, + "velocityY": -0.46782403935650313, + "timestamp": 2.5994951925858016 + }, + { + "x": 8.10265502996488, + "y": 7.372742454671214, + "heading": 0.033468558101012916, + "angularVelocity": 0.2038769394334648, + "velocityX": -0.7362712800132867, + "velocityY": -0.5966034705852111, + "timestamp": 2.690193787728666 + }, + { + "x": 7.986220080079777, + "y": 7.306963959658495, + "heading": 0.05493247364660356, + "angularVelocity": 0.23665102542968372, + "velocityX": -1.2837569281166943, + "velocityY": -0.7252427108612493, + "timestamp": 2.7808923828715306 + }, + { + "x": 7.820121799349139, + "y": 7.229537347680479, + "heading": 0.07855456380147917, + "angularVelocity": 0.26044604238540936, + "velocityX": -1.8313214275150225, + "velocityY": -0.8536693634124876, + "timestamp": 2.871590978014395 + }, + { + "x": 7.6043499812219455, + "y": 7.1404953741126675, + "heading": 0.10296904259791395, + "angularVelocity": 0.2691825464107588, + "velocityX": -2.37899845953864, + "velocityY": -0.9817348706179747, + "timestamp": 2.9622895731572596 + }, + { + "x": 7.338890842614761, + "y": 7.0399060968505065, + "heading": 0.12540191269228773, + "angularVelocity": 0.24733426200304945, + "velocityX": -2.926827457349739, + "velocityY": -1.1090500035167739, + "timestamp": 3.052988168300124 + }, + { + "x": 7.023762260989218, + "y": 6.927996342713902, + "heading": 0.13709315762540872, + "angularVelocity": 0.12890216121545683, + "velocityX": -3.4744593466873996, + "velocityY": -1.233864250712263, + "timestamp": 3.1436867634429886 + }, + { + "x": 6.6949878661960245, + "y": 6.850204202472232, + "heading": 0.1370931594052802, + "angularVelocity": 1.9624024712667312e-8, + "velocityX": -3.6249116568489486, + "velocityY": -0.857699505919964, + "timestamp": 3.234385358585853 + }, + { + "x": 6.366213403460125, + "y": 6.772412349378657, + "heading": 0.13709316118514803, + "angularVelocity": 1.9623984758130592e-8, + "velocityX": -3.624912405953231, + "velocityY": -0.8576963399602832, + "timestamp": 3.3250839537287176 + }, + { + "x": 6.037438940720318, + "y": 6.694620496301594, + "heading": 0.13709316296501595, + "angularVelocity": 1.9623985202632452e-8, + "velocityX": -3.624912405996305, + "velocityY": -0.8576963397782383, + "timestamp": 3.415782548871582 + }, + { + "x": 5.708664477980543, + "y": 6.6168286432244035, + "heading": 0.13709316474488378, + "angularVelocity": 1.9623984644394548e-8, + "velocityX": -3.624912405995973, + "velocityY": -0.8576963397796433, + "timestamp": 3.5064811440144465 + }, + { + "x": 5.379890015210838, + "y": 6.539036790273742, + "heading": 0.13709316652476114, + "angularVelocity": 1.9624089556639278e-8, + "velocityX": -3.6249124063259477, + "velocityY": -0.8576963383845883, + "timestamp": 3.597179739157311 + }, + { + "x": 5.082161809078407, + "y": 6.468591173031877, + "heading": 0.1733415977378681, + "angularVelocity": 0.39965813313877674, + "velocityX": -3.282611000351902, + "velocityY": -0.7767002028080265, + "timestamp": 3.6878783343001755 + }, + { + "x": 4.834054918534678, + "y": 6.409886673220785, + "heading": 0.20354854900447322, + "angularVelocity": 0.3330476201866674, + "velocityX": -2.7355097413903846, + "velocityY": -0.6472481709184398, + "timestamp": 3.77857692944304 + }, + { + "x": 4.6355693784003975, + "y": 6.36292318296314, + "heading": 0.22771447843277892, + "angularVelocity": 0.26644215811987526, + "velocityX": -2.1884080985117165, + "velocityY": -0.5177973284334796, + "timestamp": 3.8692755245859045 + }, + { + "x": 4.486705209976563, + "y": 6.327700620163418, + "heading": 0.24583924264804105, + "angularVelocity": 0.1998351152706694, + "velocityX": -1.6413062207782854, + "velocityY": -0.3883473910951022, + "timestamp": 3.959974119728769 + }, + { + "x": 4.3874624262872, + "y": 6.30421892984517, + "heading": 0.25792252534877924, + "angularVelocity": 0.13322458503028708, + "velocityX": -1.0942041994480602, + "velocityY": -0.25889805990114845, + "timestamp": 4.0506727148716335 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": 0.04079263927507296, - "velocityX": -0.5229622556382164, - "velocityY": -0.12343224899141036, - "timestamp": 4.234244612416448 + "angularVelocity": 0.06661189082061515, + "velocityX": -0.5471021053136723, + "velocityY": -0.1294490312938923, + "timestamp": 4.141371310014498 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": -3.1350638097986163e-24, - "velocityX": 6.734301517773745e-23, - "velocityY": -5.476052050130636e-23, - "timestamp": 4.327573921984801 - }, - { - "x": 4.357085465154312, - "y": 6.298335605731047, - "heading": 0.24955697801054222, - "angularVelocity": -0.24189458757929325, - "velocityX": 0.32311199814604374, - "velocityY": 0.09834717102755294, - "timestamp": 4.387133552481924 - }, - { - "x": 4.395672489468794, - "y": 6.30982325311387, - "heading": 0.2214506254853789, - "angularVelocity": -0.4719027349661102, - "velocityX": 0.6478721239942183, - "velocityY": 0.19287640448638915, - "timestamp": 4.446693182979047 - }, - { - "x": 4.453716686931878, - "y": 6.3266599763174565, - "heading": 0.18050722178606005, - "angularVelocity": -0.6874354887291741, - "velocityX": 0.9745560370104606, - "velocityY": 0.28268683104740583, - "timestamp": 4.50625281347617 - }, - { - "x": 4.531352614071991, - "y": 6.348490007100106, - "heading": 0.12779504929109745, - "angularVelocity": -0.8850318924914878, - "velocityX": 1.3034991401409508, - "velocityY": 0.366523945841208, - "timestamp": 4.565812443973293 - }, - { - "x": 4.628738799037524, - "y": 6.374849024990675, - "heading": 0.0646674309135714, - "angularVelocity": -1.0599061453306917, - "velocityX": 1.6351039143911004, - "velocityY": 0.4425651682281852, - "timestamp": 4.625372074470416 - }, - { - "x": 4.746060953896417, - "y": 6.4051070852453105, - "heading": -0.0071047467810986895, - "angularVelocity": -1.2050473969635673, - "velocityX": 1.9698267749421239, - "velocityY": 0.5080296838996932, - "timestamp": 4.684931704967539 - }, - { - "x": 4.883529200739737, - "y": 6.4383664128247835, - "heading": -0.08508741675594196, - "angularVelocity": -1.30932091626408, - "velocityX": 2.3080775635429776, - "velocityY": 0.5584206500589359, - "timestamp": 4.744491335464662 - }, - { - "x": 5.041348217191184, - "y": 6.473264113474332, - "heading": -0.1656919745439482, - "angularVelocity": -1.3533421398895296, - "velocityX": 2.6497648681529427, - "velocityY": 0.5859287634639322, - "timestamp": 4.804050965961785 - }, - { - "x": 5.219569793721533, - "y": 6.507557049328585, - "heading": -0.2431024824486723, - "angularVelocity": -1.29971437462936, - "velocityX": 2.99232172937938, - "velocityY": 0.575774825465208, - "timestamp": 4.8636105964589085 - }, - { - "x": 5.417370624809519, - "y": 6.5372140981281, - "heading": -0.30621328049041796, - "angularVelocity": -1.0596237336427745, - "velocityX": 3.3210553765530904, - "velocityY": 0.49793876409201393, - "timestamp": 4.923170226956032 - }, - { - "x": 5.630089765221455, - "y": 6.556096742386712, - "heading": -0.3316159113808185, - "angularVelocity": -0.4265075299892522, - "velocityX": 3.5715322381358874, - "velocityY": 0.31703763272213936, - "timestamp": 4.982729857453155 + "angularVelocity": -4.3850016440748696e-23, + "velocityX": 2.0622476782033507e-21, + "velocityY": 1.8080615452809687e-23, + "timestamp": 4.2320699051573625 + }, + { + "x": 4.357336302646973, + "y": 6.297583243093535, + "heading": 0.2475212314024478, + "angularVelocity": -0.28561956492945584, + "velocityX": 0.3386404194139296, + "velocityY": 0.08867859433669868, + "timestamp": 4.289639136689636 + }, + { + "x": 4.39639255106342, + "y": 6.307690301692112, + "heading": 0.21545182729872558, + "angularVelocity": -0.5570580542792894, + "velocityX": 0.6784222644096215, + "velocityY": 0.17556354895775533, + "timestamp": 4.347208368221909 + }, + { + "x": 4.455088006420721, + "y": 6.322669942772804, + "heading": 0.16876628337274657, + "angularVelocity": -0.8109461023430008, + "velocityX": 1.0195629469953096, + "velocityY": 0.2602022066647703, + "timestamp": 4.4047775997541825 + }, + { + "x": 4.53351670800356, + "y": 6.342355632767599, + "heading": 0.10873910043109954, + "angularVelocity": -1.0426955744232154, + "velocityX": 1.362337128625251, + "velocityY": 0.3419481113580211, + "timestamp": 4.462346831286456 + }, + { + "x": 4.631792821732683, + "y": 6.3665238013399605, + "heading": 0.03701268299307073, + "angularVelocity": -1.245915839571676, + "velocityX": 1.7070944168158462, + "velocityY": 0.4198105121277204, + "timestamp": 4.519916062818729 + }, + { + "x": 4.7500563078189675, + "y": 6.394856912095635, + "heading": -0.0442182245974014, + "angularVelocity": -1.4110125396572948, + "velocityX": 2.0542828684448438, + "velocityY": 0.4921571819104624, + "timestamp": 4.5774852943510025 + }, + { + "x": 4.888478268982772, + "y": 6.426870152464857, + "heading": -0.13181305030525106, + "angularVelocity": -1.52155627887344, + "velocityX": 2.4044434410455024, + "velocityY": 0.5560824683107718, + "timestamp": 4.635054525883276 + }, + { + "x": 5.047249482849422, + "y": 6.461748968300146, + "heading": -0.22073086511138704, + "angularVelocity": -1.5445371153910277, + "velocityX": 2.757917895388995, + "velocityY": 0.6058586315458515, + "timestamp": 4.692623757415549 + }, + { + "x": 5.226429049191539, + "y": 6.497909114202001, + "heading": -0.3013203844421244, + "angularVelocity": -1.39987137548568, + "velocityX": 3.1124189358280567, + "velocityY": 0.6281158344381212, + "timestamp": 4.7501929889478225 + }, + { + "x": 5.423903550460318, + "y": 6.531166211616276, + "heading": -0.3435648346153767, + "angularVelocity": -0.7338025721182303, + "velocityX": 3.430209089347909, + "velocityY": 0.5776887502073221, + "timestamp": 4.807762220480096 + }, + { + "x": 5.636616262089249, + "y": 6.55438565863319, + "heading": -0.34498723844741225, + "angularVelocity": -0.024707709208140263, + "velocityX": 3.69490274522221, + "velocityY": 0.4033308487694046, + "timestamp": 4.865331452012369 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -0.3344741491569628, - "angularVelocity": -0.04798951491618805, - "velocityX": 3.7098016045994533, - "velocityY": 0.017297899702458933, - "timestamp": 5.042289487950278 - }, - { - "x": 5.971113356144086, - "y": 6.5517128681904815, - "heading": -0.33447429817496127, - "angularVelocity": -0.000004618408354033321, - "velocityX": 3.7212182481092846, - "velocityY": -0.167796284707936, - "timestamp": 5.074555582043538 - }, - { - "x": 6.090763237120761, - "y": 6.540318338833946, - "heading": -0.33447432111787584, - "angularVelocity": -7.11053358628561e-7, - "velocityX": 3.70822327086892, - "velocityY": -0.3531425069176937, - "timestamp": 5.106821676136798 - }, - { - "x": 6.209696100342635, - "y": 6.522971764048271, - "heading": -0.3344743416762489, - "angularVelocity": -6.371509664508709e-7, - "velocityX": 3.686001251905956, - "velocityY": -0.537609998146539, - "timestamp": 5.139087770230058 - }, - { - "x": 6.327616045254045, - "y": 6.499716303685921, - "heading": -0.3344743603957426, - "angularVelocity": -5.80159891848438e-7, - "velocityX": 3.6546085984432737, - "velocityY": -0.7207398669059114, - "timestamp": 5.171353864323319 - }, - { - "x": 6.444229692072682, - "y": 6.470609819762047, - "heading": -0.3344743776866033, - "angularVelocity": -5.358832938629608e-7, - "velocityX": 3.6141234350083242, - "velocityY": -0.9020764595722645, - "timestamp": 5.203619958416579 - }, - { - "x": 6.559246912194348, - "y": 6.435724733351891, - "heading": -0.3344743938681527, - "angularVelocity": -5.015031984210286e-7, - "velocityX": 3.5646465230413518, - "velocityY": -1.081168557598769, - "timestamp": 5.235886052509839 - }, - { - "x": 6.67238155121755, - "y": 6.395147846497736, - "heading": -0.33447440919657534, - "angularVelocity": -4.750628499499889e-7, - "velocityX": 3.506301032167155, - "velocityY": -1.2575704619491082, - "timestamp": 5.268152146603099 - }, - { - "x": 6.783352144572337, - "y": 6.348980132760865, - "heading": -0.33447442388388354, - "angularVelocity": -4.551932493526412e-7, - "velocityX": 3.439232310983837, - "velocityY": -1.4308429648605667, - "timestamp": 5.300418240696359 - }, - { - "x": 6.891882636447486, - "y": 6.297336519628137, - "heading": -0.3344744381114049, - "angularVelocity": -4.409434049866837e-7, - "velocityX": 3.363607989286128, - "velocityY": -1.6005536022878626, - "timestamp": 5.33268433478962 - }, - { - "x": 6.998345027127988, - "y": 6.24155399695869, - "heading": -0.33447445218138366, - "angularVelocity": -4.360607978051808e-7, - "velocityX": 3.299512806625678, - "velocityY": -1.7288278682946643, - "timestamp": 5.36495042888288 - }, - { - "x": 7.107456437455107, - "y": 6.19114935204841, - "heading": -0.33447450198371365, - "angularVelocity": -0.00000154348803003307, - "velocityX": 3.381611979799848, - "velocityY": -1.5621551454164793, - "timestamp": 5.39721652297614 + "heading": -0.34498726323171464, + "angularVelocity": -4.305129971148693e-7, + "velocityX": 3.7246965125733036, + "velocityY": 0.047618149403998285, + "timestamp": 4.9229006835446425 + }, + { + "x": 5.970747823515829, + "y": 6.552244502208151, + "heading": -0.34498728566710496, + "angularVelocity": -6.97576178834182e-7, + "velocityX": 3.7219058963692446, + "velocityY": -0.15180985637112251, + "timestamp": 4.955062605108422 + }, + { + "x": 6.090018554613485, + "y": 6.540962017726213, + "heading": -0.3449873062008861, + "angularVelocity": -6.384500714125073e-7, + "velocityX": 3.7084454316926947, + "velocityY": -0.35080256195400317, + "timestamp": 4.987224526672201 + }, + { + "x": 6.208514441050117, + "y": 6.52331189505608, + "heading": -0.34498732528503373, + "angularVelocity": -5.933770972523468e-7, + "velocityX": 3.68435344267748, + "velocityY": -0.5487894320969752, + "timestamp": 5.019386448235981 + }, + { + "x": 6.32589577532509, + "y": 6.499344741803893, + "heading": -0.34498734327209396, + "angularVelocity": -5.592657200372931e-7, + "velocityX": 3.6496990405936383, + "velocityY": -0.7452027766642643, + "timestamp": 5.05154836979976 + }, + { + "x": 6.441826048149789, + "y": 6.4691292818938315, + "heading": -0.3449873604472264, + "angularVelocity": -5.340207168118268e-7, + "velocityX": 3.6045816663908092, + "velocityY": -0.9394793109653794, + "timestamp": 5.0837102913635395 + }, + { + "x": 6.555972918049828, + "y": 6.432752169034089, + "heading": -0.344987377050145, + "angularVelocity": -5.162290595234125e-7, + "velocityX": 3.549130908539721, + "velocityY": -1.1310615501503813, + "timestamp": 5.115872212927319 + }, + { + "x": 6.668009186278295, + "y": 6.390317785626105, + "heading": -0.34498739329087524, + "angularVelocity": -5.049676592749214e-7, + "velocityX": 3.483506668166316, + "velocityY": -1.319398261818203, + "timestamp": 5.148034134491098 + }, + { + "x": 6.777613987715612, + "y": 6.341948479079241, + "heading": -0.3449874093618285, + "angularVelocity": -4.996888394768407e-7, + "velocityX": 3.4079058746525486, + "velocityY": -1.5039308659137305, + "timestamp": 5.180196056054878 + }, + { + "x": 6.886436695218618, + "y": 6.291844380961981, + "heading": -0.3449874254078226, + "angularVelocity": -4.989127946142238e-7, + "velocityX": 3.3835884863782986, + "velocityY": -1.55787016698924, + "timestamp": 5.212357977618657 + }, + { + "x": 6.995259275146381, + "y": 6.241740005760864, + "heading": -0.3449874414538154, + "angularVelocity": -4.98912750035862e-7, + "velocityX": 3.3835845197234478, + "velocityY": -1.5578787822660598, + "timestamp": 5.244519899182436 + }, + { + "x": 7.1040824430518015, + "y": 6.191636907618568, + "heading": -0.344987457499818, + "angularVelocity": -4.989130590435776e-7, + "velocityX": 3.3836028015184674, + "velocityY": -1.5578390750981381, + "timestamp": 5.276681820746216 }, { "x": 7.214789390563965, "y": 6.145846366882324, "heading": -0.344987478573796, - "angularVelocity": -0.325821171899398, - "velocityX": 3.32649352594794, - "velocityY": -1.404043050117709, - "timestamp": 5.4294826170694 - }, - { - "x": 7.408281473848404, - "y": 6.071835314929934, - "heading": -0.36045376555379516, - "angularVelocity": -0.24376616005736862, - "velocityX": 3.0496538829742375, - "velocityY": -1.1664978129250685, - "timestamp": 5.492929844430077 - }, - { - "x": 7.581043516745906, - "y": 6.008144867615469, - "heading": -0.3717285153471776, - "angularVelocity": -0.17770279746487722, - "velocityX": 2.722925021063643, - "velocityY": -1.003833421315721, - "timestamp": 5.556377071790754 - }, - { - "x": 7.732516109743219, - "y": 5.95357268553042, - "heading": -0.3790773339263405, - "angularVelocity": -0.11582568513809473, - "velocityX": 2.3873792330788404, - "velocityY": -0.8601192574550642, - "timestamp": 5.619824299151431 - }, - { - "x": 7.862474679773266, - "y": 5.907578864525407, - "heading": -0.3826203402067485, - "angularVelocity": -0.05584178265611413, - "velocityX": 2.048293919784284, - "velocityY": -0.7249145930925164, - "timestamp": 5.683271526512108 - }, - { - "x": 7.970798961808127, - "y": 5.869857366205246, - "heading": -0.38242588059176913, - "angularVelocity": 0.003064903275819629, - "velocityX": 1.7073130937475338, - "velocityY": -0.5945334396683074, - "timestamp": 5.746718753872785 - }, - { - "x": 8.057414200976034, - "y": 5.840211247118204, - "heading": -0.378538126264348, - "angularVelocity": 0.061275401450096946, - "velocityX": 1.3651540464570049, - "velocityY": -0.46725633759397367, - "timestamp": 5.810165981233462 - }, - { - "x": 8.122269484577586, - "y": 5.818503192947792, - "heading": -0.37098804864933244, - "angularVelocity": 0.11899775497037605, - "velocityX": 1.0221925574914426, - "velocityY": -0.3421434643157637, - "timestamp": 5.873613208594139 - }, - { - "x": 8.165327926639057, - "y": 5.804632020140207, - "heading": -0.3597986070827489, - "angularVelocity": 0.1763582434103087, - "velocityX": 0.6786497038979674, - "velocityY": -0.2186253581851867, - "timestamp": 5.937060435954816 + "angularVelocity": -6.552462348846214e-7, + "velocityX": 3.4421745383783664, + "velocityY": -1.4237501526591758, + "timestamp": 5.308843742309995 + }, + { + "x": 7.410548108371569, + "y": 6.0752735070916195, + "heading": -0.3626370424214939, + "angularVelocity": -0.2874228799686078, + "velocityX": 3.1879277548585048, + "velocityY": -1.1492779529116102, + "timestamp": 5.370250001441577 + }, + { + "x": 7.584442413587634, + "y": 6.012933959955544, + "heading": -0.37637990938116833, + "angularVelocity": -0.22380238031152527, + "velocityX": 2.831866126927586, + "velocityY": -1.0151985810191508, + "timestamp": 5.431656260573159 + }, + { + "x": 7.736397172033548, + "y": 5.958610527915441, + "heading": -0.3857543231283157, + "angularVelocity": -0.15266218590290268, + "velocityX": 2.47458094003583, + "velocityY": -0.8846562680800549, + "timestamp": 5.493062519704741 + }, + { + "x": 7.86638729375416, + "y": 5.912228690109332, + "heading": -0.3906020583128712, + "angularVelocity": -0.07894529406469342, + "velocityX": 2.1168871636044173, + "velocityY": -0.7553275262497595, + "timestamp": 5.5544687788363225 + }, + { + "x": 7.974400232833448, + "y": 5.873750789689322, + "heading": -0.3908432822952403, + "angularVelocity": -0.003928328899700405, + "velocityX": 1.7589890771205783, + "velocityY": -0.6266120256171138, + "timestamp": 5.615875037967904 + }, + { + "x": 8.060428468071539, + "y": 5.84315412501972, + "heading": -0.3864300383007793, + "angularVelocity": 0.07186961161409067, + "velocityX": 1.4009685080107037, + "velocityY": -0.498266220777903, + "timestamp": 5.677281297099486 + }, + { + "x": 8.124466995265989, + "y": 5.820423539830739, + "heading": -0.377330476340927, + "angularVelocity": 0.14818622870925488, + "velocityX": 1.042866445539805, + "velocityY": -0.37016723556265896, + "timestamp": 5.738687556231068 + }, + { + "x": 8.166512252703873, + "y": 5.805548220947723, + "heading": -0.36352195759750816, + "angularVelocity": 0.2248715186155497, + "velocityX": 0.6847063806278847, + "velocityY": -0.242244342732895, + "timestamp": 5.80009381536265 }, { "x": 8.186561584472656, "y": 5.798520088195801, "heading": -0.344987478573796, - "angularVelocity": 0.23344012220985313, - "velocityX": 0.3346664419690626, - "velocityY": -0.09633095406457304, - "timestamp": 6.000507663315493 - }, - { - "x": 8.184449385960491, - "y": 5.800720366343895, - "heading": -0.3251687978756523, - "angularVelocity": 0.29387941781314064, - "velocityX": -0.03132053432391204, - "velocityY": 0.03262661481041375, - "timestamp": 6.0679458011940195 - }, - { - "x": 8.157654760081511, - "y": 5.811617201116665, - "heading": -0.30136325235519174, - "angularVelocity": 0.3529982628426134, - "velocityX": -0.39732155604955366, - "velocityY": 0.1615826758502398, - "timestamp": 6.135383939072546 - }, - { - "x": 8.106176542546697, - "y": 5.831210437992555, - "heading": -0.27368179509987206, - "angularVelocity": 0.41047185058966307, - "velocityX": -0.7633398423239571, - "velocityY": 0.29053644558191305, - "timestamp": 6.2028220769510725 - }, - { - "x": 8.03001326746699, - "y": 5.859499833662039, - "heading": -0.24226677680218867, - "angularVelocity": 0.46583460466049514, - "velocityX": -1.129379865394508, - "velocityY": 0.4194866074214659, - "timestamp": 6.270260214829599 - }, - { - "x": 7.929163035221131, - "y": 5.896484993829156, - "heading": -0.2073080079914019, - "angularVelocity": 0.5183827713890392, - "velocityX": -1.495448056817871, - "velocityY": 0.5484309224809322, - "timestamp": 6.337698352708125 - }, - { - "x": 7.80362329670357, - "y": 5.942165255300411, - "heading": -0.16907207724227352, - "angularVelocity": 0.5669778548452968, - "velocityX": -1.8615540474099375, - "velocityY": 0.677365403438884, - "timestamp": 6.405136490586652 - }, - { - "x": 7.653390486781624, - "y": 5.996539431834676, - "heading": -0.12796178084950977, - "angularVelocity": 0.6096001118360348, - "velocityX": -2.227712903232215, - "velocityY": 0.8062822943333132, - "timestamp": 6.472574628465178 - }, - { - "x": 7.478459409616276, - "y": 6.059605150575439, - "heading": -0.0846568853073795, - "angularVelocity": 0.6421425161550796, - "velocityX": -2.593948805058106, - "velocityY": 0.9351639995511196, - "timestamp": 6.540012766343705 - }, - { - "x": 7.278822723764661, - "y": 6.131356463760539, - "heading": -0.04054583544960124, - "angularVelocity": 0.6540964985900647, - "velocityX": -2.9602935687698184, - "velocityY": 1.063957508944601, - "timestamp": 6.607450904222231 - }, - { - "x": 7.05448611686489, - "y": 6.21176649036125, - "heading": -5.1297661467442016e-8, - "angularVelocity": 0.6012292958766616, - "velocityX": -3.3265539938819106, - "velocityY": 1.1923524155656642, - "timestamp": 6.674889042100758 - }, - { - "x": 6.817570799613061, - "y": 6.29528987249135, - "heading": -4.834198432704978e-8, - "angularVelocity": 4.3827976770594794e-8, - "velocityX": -3.5130762014599934, - "velocityY": 1.238518511299158, - "timestamp": 6.742327179979284 - }, - { - "x": 6.580655475027505, - "y": 6.378813233819268, - "heading": -4.538633792987658e-8, - "angularVelocity": 4.382752089770155e-8, - "velocityX": -3.5130763102074556, - "velocityY": 1.23851820283599, - "timestamp": 6.809765317857811 - }, - { - "x": 6.3437395023630385, - "y": 6.4623347568384295, - "heading": -4.243069068525663e-8, - "angularVelocity": 4.382753346398488e-8, - "velocityX": -3.5130859201838094, - "velocityY": 1.2384909436497944, - "timestamp": 6.877203455736337 - }, - { - "x": 6.09986574355872, - "y": 6.5225893218228235, - "heading": -3.943141637637637e-8, - "angularVelocity": 4.447445322827149e-8, - "velocityX": -3.6162587888117317, - "velocityY": 0.8934790740059899, - "timestamp": 6.944641593614864 + "angularVelocity": 0.30183371020853555, + "velocityX": 0.3265030642205698, + "velocityY": -0.11445303542856088, + "timestamp": 5.861500074494232 + }, + { + "x": 8.182873429152231, + "y": 5.799961021839805, + "heading": -0.3197766551885773, + "angularVelocity": 0.3842915682549792, + "velocityX": -0.056218988582698255, + "velocityY": 0.021964322281146666, + "timestamp": 5.927103448254676 + }, + { + "x": 8.154073481017848, + "y": 5.810352783086787, + "heading": -0.28949615112813754, + "angularVelocity": 0.46156931152673975, + "velocityX": -0.43900102210518965, + "velocityY": 0.15840284807498262, + "timestamp": 5.99270682201512 + }, + { + "x": 8.100157139869356, + "y": 5.829696916957608, + "heading": -0.2545716989464078, + "angularVelocity": 0.5323575630311173, + "velocityX": -0.8218531770236625, + "velocityY": 0.29486492480488535, + "timestamp": 6.058310195775564 + }, + { + "x": 8.021119019409117, + "y": 5.8579951101346595, + "heading": -0.2155530876775793, + "angularVelocity": 0.594765315139245, + "velocityX": -1.204787435915305, + "velocityY": 0.43135271183437995, + "timestamp": 6.1239135695360085 + }, + { + "x": 7.9169528918289975, + "y": 5.89524913213786, + "heading": -0.17317932530550872, + "angularVelocity": 0.6459082809795952, + "velocityX": -1.5878166260242859, + "velocityY": 0.567867471865644, + "timestamp": 6.189516943296453 + }, + { + "x": 7.787651961744212, + "y": 5.941460642017215, + "heading": -0.12849929599301224, + "angularVelocity": 0.6810629812370456, + "velocityX": -1.9709493989888005, + "velocityY": 0.7044075209927304, + "timestamp": 6.255120317056897 + }, + { + "x": 7.633210550125228, + "y": 5.99663052819492, + "heading": -0.08312171419135361, + "angularVelocity": 0.6916958564868655, + "velocityX": -2.3541687380734118, + "velocityY": 0.8409611124446534, + "timestamp": 6.320723690817341 + }, + { + "x": 7.453632499858265, + "y": 6.060756206322318, + "heading": -0.039828348341138756, + "angularVelocity": 0.659925905766733, + "velocityX": -2.7373294995880113, + "velocityY": 0.9774753103637095, + "timestamp": 6.386327064577785 + }, + { + "x": 7.248984054172477, + "y": 6.133814732067644, + "heading": -0.004587896529415136, + "angularVelocity": 0.5371743828359606, + "velocityX": -3.119480507711035, + "velocityY": 1.1136397651149037, + "timestamp": 6.45193043833823 + }, + { + "x": 7.020152924958556, + "y": 6.215343825965371, + "heading": -7.99601094738583e-8, + "angularVelocity": 0.06993263160608797, + "velocityX": -3.488100018293493, + "velocityY": 1.2427576391945405, + "timestamp": 6.517533812098674 + }, + { + "x": 6.789969979123711, + "y": 6.297403529442868, + "heading": -7.492709232609331e-8, + "angularVelocity": 7.671887677824965e-8, + "velocityX": -3.508705919231764, + "velocityY": 1.2508457838943434, + "timestamp": 6.583137185859118 + }, + { + "x": 6.559787033588477, + "y": 6.379463233760796, + "heading": -6.989407741573959e-8, + "angularVelocity": 7.671884267312441e-8, + "velocityX": -3.5087059146647626, + "velocityY": 1.2508457967051358, + "timestamp": 6.648740559619562 + }, + { + "x": 6.3296038306461995, + "y": 6.461522216031207, + "heading": -6.486106191272836e-8, + "angularVelocity": 7.671885170707308e-8, + "velocityX": -3.508709838350823, + "velocityY": 1.250834790449275, + "timestamp": 6.714343933380007 + }, + { + "x": 6.0929382858404795, + "y": 6.522410665825721, + "heading": -5.975605523435043e-8, + "angularVelocity": 7.78162217238893e-8, + "velocityX": -3.607520943510018, + "velocityY": 0.9281298552853697, + "timestamp": 6.779947307140451 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -3.6277001373267394e-8, - "angularVelocity": 4.677494222617604e-8, - "velocityX": -3.689626869559746, - "velocityY": 0.5121386527717335, - "timestamp": 7.01207973149339 - }, - { - "x": 5.675146419069591, - "y": 6.5685334564190505, - "heading": -3.325736766733182e-8, - "angularVelocity": 6.38129937324226e-8, - "velocityX": -3.7171934353117426, - "velocityY": 0.24104917118070399, - "timestamp": 7.059399776037881 - }, - { - "x": 5.498885444409021, - "y": 6.56705094919282, - "heading": -3.0392937858444436e-8, - "angularVelocity": 6.053311733876744e-8, - "velocityX": -3.724869161837878, - "velocityY": -0.03132937089348242, - "timestamp": 7.106719820582372 - }, - { - "x": 5.3232044198709785, - "y": 6.552687568221491, - "heading": -2.7611010604394772e-8, - "angularVelocity": 5.878961613051839e-8, - "velocityX": -3.7126132536258285, - "velocityY": -0.3035369283692009, - "timestamp": 7.154039865126863 - }, - { - "x": 5.148803896994517, - "y": 6.527102675424277, - "heading": -2.484990875079416e-8, - "angularVelocity": 5.8349519324830205e-8, - "velocityX": -3.6855528044249266, - "velocityY": -0.5406776989222635, - "timestamp": 7.201359909671353 - }, - { - "x": 4.974403597324897, - "y": 6.501516261174204, - "heading": -2.208880782591367e-8, - "angularVelocity": 5.834949969847243e-8, - "velocityX": -3.6855480874631454, - "velocityY": -0.5407098513192842, - "timestamp": 7.248679954215844 - }, - { - "x": 4.800003297129097, - "y": 6.475929850510635, - "heading": -1.9327706889009288e-8, - "angularVelocity": 5.8349499952569724e-8, - "velocityX": -3.6855480985827302, - "velocityY": -0.5407097755267535, - "timestamp": 7.295999998760335 - }, - { - "x": 4.625602997244748, - "y": 6.450343437724181, - "heading": -1.656660593755509e-8, - "angularVelocity": 5.8349500260046496e-8, - "velocityX": -3.685548092000944, - "velocityY": -0.5407098203890633, - "timestamp": 7.343320043304826 - }, - { - "x": 4.451202697255753, - "y": 6.4247570256510045, - "heading": -1.3805504990627123e-8, - "angularVelocity": 5.834950016439505e-8, - "velocityX": -3.685548094212386, - "velocityY": -0.5407098053155895, - "timestamp": 7.390640087849317 - }, - { - "x": 4.276802397278509, - "y": 6.3991706134977395, - "heading": -1.1044403987822918e-8, - "angularVelocity": 5.8349501345210454e-8, - "velocityX": -3.685548093964082, - "velocityY": -0.5407098070080617, - "timestamp": 7.437960132393807 - }, - { - "x": 4.102402097300047, - "y": 6.373584201352764, - "heading": -8.283303038922857e-9, - "angularVelocity": 5.834950020607066e-8, - "velocityX": -3.685548093989784, - "velocityY": -0.5407098068328745, - "timestamp": 7.485280176938298 - }, - { - "x": 3.9280017973350696, - "y": 6.347997789115884, - "heading": -5.522202092459704e-9, - "angularVelocity": 5.8349500154572286e-8, - "velocityX": -3.685548093704841, - "velocityY": -0.5407098087750815, - "timestamp": 7.532600221482789 - }, - { - "x": 3.7536014973495653, - "y": 6.322411377018916, - "heading": -2.7611011159009457e-9, - "angularVelocity": 5.834950079057347e-8, - "velocityX": -3.6855480941386265, - "velocityY": -0.5407098058183458, - "timestamp": 7.57992026602728 - }, - { - "x": 3.579201219971439, - "y": 6.296824810827607, - "heading": -5.947116132480024e-26, - "angularVelocity": 5.8349503735249215e-8, - "velocityX": -3.6855476163838365, - "velocityY": -0.5407130622468497, - "timestamp": 7.6272403105717705 + "heading": -5.43646913463662e-8, + "angularVelocity": 8.218119860224255e-8, + "velocityX": -3.6872205492775545, + "velocityY": 0.5291851788357008, + "timestamp": 6.845550680900895 + }, + { + "x": 5.661580826065391, + "y": 6.5680717657534915, + "heading": -4.9482596020305806e-8, + "angularVelocity": 9.582614101786845e-8, + "velocityX": -3.718801184433256, + "velocityY": 0.21482472211899678, + "timestamp": 6.896498107920314 + }, + { + "x": 5.471871496557958, + "y": 6.562921997343888, + "heading": -4.486806959492394e-8, + "angularVelocity": 9.057427814015043e-8, + "velocityX": -3.7236292508966913, + "velocityY": -0.10108004880484801, + "timestamp": 6.947445534939733 + }, + { + "x": 5.28328089006521, + "y": 6.541715087274231, + "heading": -4.036796232249368e-8, + "angularVelocity": 8.83284502417564e-8, + "velocityX": -3.7016708698726792, + "velocityY": -0.41625085525070266, + "timestamp": 6.998392961959151 + }, + { + "x": 5.095501405649377, + "y": 6.514237548522189, + "heading": -3.588263316902823e-8, + "angularVelocity": 8.803838419074343e-8, + "velocityX": -3.685750103616821, + "velocityY": -0.5393312353451875, + "timestamp": 7.04934038897857 + }, + { + "x": 4.907721944965666, + "y": 6.486759847587045, + "heading": -3.139730406205809e-8, + "angularVelocity": 8.803838327812946e-8, + "velocityX": -3.685749637800889, + "velocityY": -0.5393344186875336, + "timestamp": 7.100287815997989 + }, + { + "x": 4.719942484237288, + "y": 6.459282146957154, + "heading": -2.6911974883170177e-8, + "angularVelocity": 8.803838468973724e-8, + "velocityX": -3.6857496386776267, + "velocityY": -0.5393344126960062, + "timestamp": 7.151235243017408 + }, + { + "x": 4.532163023549714, + "y": 6.431804446048415, + "heading": -2.2426645714855718e-8, + "angularVelocity": 8.803838448220064e-8, + "velocityX": -3.685749637876727, + "velocityY": -0.5393344181692645, + "timestamp": 7.2021826700368266 + }, + { + "x": 4.34438356286167, + "y": 6.404326745142886, + "heading": -1.794131654726717e-8, + "angularVelocity": 8.803838446795243e-8, + "velocityX": -3.6857496378859507, + "velocityY": -0.5393344181062291, + "timestamp": 7.253130097056245 + }, + { + "x": 4.156604102201168, + "y": 6.376849044049133, + "heading": -1.3455987422255126e-8, + "angularVelocity": 8.803838363225754e-8, + "velocityX": -3.685749637345336, + "velocityY": -0.5393344218007294, + "timestamp": 7.304077524075664 + }, + { + "x": 3.9688246409539834, + "y": 6.349371346964709, + "heading": -8.97065828327331e-9, + "angularVelocity": 8.80383839064573e-8, + "velocityX": -3.685749648860808, + "velocityY": -0.5393343431053033, + "timestamp": 7.355024951095083 + }, + { + "x": 3.7810451818268453, + "y": 6.32189363539213, + "heading": -4.485329161078249e-9, + "angularVelocity": 8.80383835769656e-8, + "velocityX": -3.685749607248367, + "velocityY": -0.5393346274799247, + "timestamp": 7.405972378114502 + }, + { + "x": 3.5932657187551267, + "y": 6.294415950776365, + "heading": 7.629972110817107e-24, + "angularVelocity": 8.803838434016792e-8, + "velocityX": -3.6857496846728943, + "velocityY": -0.5393340983695197, + "timestamp": 7.4569198051339205 }, { "x": 3.405486822128296, "y": 6.266934394836426, - "heading": -3.242812966980484e-26, - "angularVelocity": 4.141833469401844e-27, - "velocityX": -3.6710531343607253, - "velocityY": -0.6316650011408507, - "timestamp": 7.674560355116261 - }, - { - "x": 3.234879221757396, - "y": 6.227705290951873, - "heading": -3.799073171453003e-26, - "angularVelocity": 3.7193578167117204e-28, - "velocityX": -3.4320264149158155, - "velocityY": -0.7891519514521484, - "timestamp": 7.724270811780997 - }, - { - "x": 3.075654096212167, - "y": 6.19701582942553, - "heading": -4.3640191392241984e-26, - "angularVelocity": -1.3753350907836377e-27, - "velocityX": -3.2030509520179686, - "velocityY": -0.6173643049252793, - "timestamp": 7.773981268445732 - }, - { - "x": 2.9276929132783946, - "y": 6.175021768989464, - "heading": -4.923363376741485e-26, - "angularVelocity": -2.4846347668894466e-28, - "velocityX": -2.9764599414500257, - "velocityY": -0.4424433391228337, - "timestamp": 7.823691725110468 - }, - { - "x": 2.790955431257819, - "y": 6.16177499390257, - "heading": -5.486689770041805e-26, - "angularVelocity": -1.0495335231555751e-27, - "velocityX": -2.7506784526800994, - "velocityY": -0.2664786440453503, - "timestamp": 7.873402181775203 - }, - { - "x": 2.6654213921010017, - "y": 6.157301437220893, - "heading": -6.044835484657667e-26, - "angularVelocity": -7.36271402334531e-30, - "velocityX": -2.525304484798085, - "velocityY": -0.08999226685541546, - "timestamp": 7.923112638439939 - }, - { - "x": 2.551078597067916, - "y": 6.16161665554964, - "heading": -6.59367177391361e-26, - "angularVelocity": 1.865367086612129e-27, - "velocityX": -2.300175912771294, - "velocityY": 0.08680705465754052, - "timestamp": 7.972823095104674 - }, - { - "x": 2.447918895369922, - "y": 6.174731018536324, - "heading": -7.161083135132275e-26, - "angularVelocity": -1.871285764514843e-27, - "velocityX": -2.075211306018359, - "velocityY": 0.2638149771009456, - "timestamp": 8.02253355176941 - }, - { - "x": 2.355936456054869, - "y": 6.196651932350337, - "heading": -7.721743669966212e-26, - "angularVelocity": -5.132563200735982e-28, - "velocityX": -1.8503639975672366, - "velocityY": 0.44097188569106954, - "timestamp": 8.072244008434145 - }, - { - "x": 2.2751269010087887, - "y": 6.227384951214515, - "heading": -8.282082994441975e-26, - "angularVelocity": -4.486400643129525e-28, - "velocityX": -1.6256047614104199, - "velocityY": 0.618240525760057, - "timestamp": 8.12195446509888 + "heading": 3.646452356482733e-24, + "angularVelocity": -1.9320923857305114e-25, + "velocityX": -3.6857385664492472, + "velocityY": -0.5394100850169479, + "timestamp": 7.507867232153339 + }, + { + "x": 3.2165781142286494, + "y": 6.223116050022905, + "heading": 3.318409186414545e-24, + "angularVelocity": -1.5338201174379227e-26, + "velocityX": -3.536673395714516, + "velocityY": -0.8203495543918979, + "timestamp": 7.561281468290765 + }, + { + "x": 3.041563345872727, + "y": 6.190252158258573, + "heading": 2.990185107220459e-24, + "angularVelocity": -1.87251095899189e-26, + "velocityX": -3.2765566075987578, + "velocityY": -0.6152646586535303, + "timestamp": 7.6146957044281915 + }, + { + "x": 2.8804424282697485, + "y": 6.168342842771101, + "heading": 2.6619349218872592e-24, + "angularVelocity": -1.9213858301671557e-26, + "velocityX": -3.0164414817885197, + "velocityY": -0.4101774558958965, + "timestamp": 7.668109940565618 + }, + { + "x": 2.733215332226118, + "y": 6.157388144124356, + "heading": 2.333764868738876e-24, + "angularVelocity": -1.7713655535187625e-26, + "velocityX": -2.7563269025291364, + "velocityY": -0.20508949371775206, + "timestamp": 7.721524176703044 + }, + { + "x": 2.5998820432630403, + "y": 6.157388082450519, + "heading": 2.0056312293962657e-24, + "angularVelocity": -1.7031930803836163e-26, + "velocityX": -2.4962125943359705, + "velocityY": -0.0000011546329394271484, + "timestamp": 7.77493841284047 + }, + { + "x": 2.4804425527424723, + "y": 6.16834266976643, + "heading": 1.6774969061310235e-24, + "angularVelocity": -1.7044734930100497e-26, + "velocityX": -2.2360984478608033, + "velocityY": 0.20508740942632245, + "timestamp": 7.828352648977896 + }, + { + "x": 2.3748968549301686, + "y": 6.190251914052291, + "heading": 1.3492831778827425e-24, + "angularVelocity": -1.853132331504253e-26, + "velocityX": -1.97598440873988, + "velocityY": 0.4101761228877669, + "timestamp": 7.881766885115322 + }, + { + "x": 2.283244945743567, + "y": 6.22311582099134, + "heading": 1.0211293460600033e-24, + "angularVelocity": -1.7409966353369085e-26, + "velocityX": -1.7158704460510679, + "velocityY": 0.6152649427485215, + "timestamp": 7.935181121252748 }, { "x": 2.205486822128296, "y": 6.266934394836426, - "heading": -8.869624024083721e-26, - "angularVelocity": -5.920668887028623e-27, - "velocityX": -1.400914084337811, - "velocityY": 0.7955960631914043, - "timestamp": 8.171664921763616 - }, - { - "x": 2.1363371710210255, - "y": 6.327839189643039, - "heading": 0.025262557050058088, - "angularVelocity": 0.415801673405611, - "velocityX": -1.1381484696439823, - "velocityY": 1.002444667372145, - "timestamp": 8.23242118770922 - }, - { - "x": 2.0845111064453854, - "y": 6.399393278580984, - "heading": 0.0757412033730169, - "angularVelocity": 0.8308385240158422, - "velocityX": -0.8530159608893926, - "velocityY": 1.177723611289897, - "timestamp": 8.293177453654822 - }, - { - "x": 2.051957537582464, - "y": 6.477853810368175, - "heading": 0.14980799257498115, - "angularVelocity": 1.2190806668118568, - "velocityX": -0.5358059511436571, - "velocityY": 1.291398188582541, - "timestamp": 8.353933719600425 - }, - { - "x": 2.0403269766878736, - "y": 6.554154175481095, - "heading": 0.23625288977037628, - "angularVelocity": 1.422814517152715, - "velocityX": -0.19142981737889736, - "velocityY": 1.255843556633874, - "timestamp": 8.414689985546028 - }, - { - "x": 2.0424863781029257, - "y": 6.6144252836562565, - "heading": 0.31360709056854197, - "angularVelocity": 1.2731888570542382, - "velocityX": 0.03554203638823312, - "velocityY": 0.9920146874912401, - "timestamp": 8.475446251491631 - }, - { - "x": 2.048130429243297, - "y": 6.654505373712402, - "heading": 0.3666123519116142, - "angularVelocity": 0.8724246053983875, - "velocityX": 0.09289660996323934, - "velocityY": 0.659686526687304, - "timestamp": 8.536202517437234 + "heading": 6.9419459427461045e-25, + "angularVelocity": 5.413163312168308e-27, + "velocityX": -1.455756540544968, + "velocityY": 0.8203538422294017, + "timestamp": 7.9885953573901745 + }, + { + "x": 2.136159340261779, + "y": 6.327632048563378, + "heading": 0.025231456124497797, + "angularVelocity": 0.43032288322850026, + "velocityX": -1.1823812996193095, + "velocityY": 1.035199444221815, + "timestamp": 8.047229134741727 + }, + { + "x": 2.084207176746768, + "y": 6.399028713640929, + "heading": 0.07562693390361644, + "angularVelocity": 0.8594956705068004, + "velocityX": -0.8860449703507888, + "velocityY": 1.2176712520067714, + "timestamp": 8.10586291209328 + }, + { + "x": 2.051566754965305, + "y": 6.477423275068575, + "heading": 0.14957404640599054, + "angularVelocity": 1.2611691731714223, + "velocityX": -0.5566829096778134, + "velocityY": 1.3370204849265277, + "timestamp": 8.164496689444832 + }, + { + "x": 2.039926841667985, + "y": 6.553838491753715, + "heading": 0.2360367067446735, + "angularVelocity": 1.4746220394479557, + "velocityX": -0.19851890536627906, + "velocityY": 1.3032627290405507, + "timestamp": 8.223130466796384 + }, + { + "x": 2.042236938900771, + "y": 6.614274854513032, + "heading": 0.313525546116211, + "angularVelocity": 1.3215733809359698, + "velocityX": 0.039398744838411756, + "velocityY": 1.0307431226365935, + "timestamp": 8.281764244147936 + }, + { + "x": 2.0480382687015357, + "y": 6.654459061696839, + "heading": 0.3665932060292166, + "angularVelocity": 0.9050697790597129, + "velocityX": 0.09894177149770778, + "velocityY": 0.6853422890166648, + "timestamp": 8.340398021499489 }, { "x": 2.052253484725952, "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": 0.4352739504266956, - "velocityX": 0.0678622265289755, - "velocityY": 0.3266075878021015, - "timestamp": 8.596958783382837 + "angularVelocity": 0.4513569988001917, + "velocityX": 0.07189057595834777, + "velocityY": 0.3392203330286158, + "timestamp": 8.399031798851041 }, { "x": 2.052253484725952, "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": -3.1389023173080445e-26, - "velocityX": -4.435068013070024e-22, - "velocityY": -1.8479842669198598e-22, - "timestamp": 8.65771504932844 + "angularVelocity": 7.733121458673518e-26, + "velocityX": -6.859499621919341e-22, + "velocityY": -2.8560302702061698e-22, + "timestamp": 8.457665576202594 }, { - "x": 2.103013204328941, - "y": 6.695498705652326, + "x": 2.103013204396169, + "y": 6.695498705680337, "heading": 0.3930579718029317, - "angularVelocity": 3.416438400428702e-17, - "velocityX": 0.5194333949867557, - "velocityY": 0.21643049229415012, - "timestamp": 8.755436374320036 + "angularVelocity": 4.836322083094603e-17, + "velocityX": 0.5390415085998568, + "velocityY": 0.22460053627515192, + "timestamp": 8.551832201333307 }, { - "x": 2.2045326425850984, - "y": 6.737798454207702, + "x": 2.204532642750927, + "y": 6.737798454276797, "heading": 0.3930579718029317, - "angularVelocity": 5.790454228147962e-17, - "velocityX": 1.0388667802538214, - "velocityY": 0.432860980538431, - "timestamp": 8.853157699311632 + "angularVelocity": 1.8057826879047683e-17, + "velocityX": 1.078083006732355, + "velocityY": 0.4492010681889061, + "timestamp": 8.64599882646402 }, { - "x": 2.356811797278175, - "y": 6.801248075919452, - "heading": 0.3930579718029318, - "angularVelocity": 1.3697605676063785e-16, - "velocityX": 1.558300142841611, - "velocityY": 0.6492914593330171, - "timestamp": 8.950879024303228 + "x": 2.356811797490316, + "y": 6.801248076007844, + "heading": 0.3930579718029317, + "angularVelocity": 9.246174062812876e-17, + "velocityX": 1.6171244804410174, + "velocityY": 0.6738015899260662, + "timestamp": 8.740165451594732 }, { - "x": 2.5598506573269275, - "y": 6.8858475661703915, - "heading": 0.3930579718029318, - "angularVelocity": 1.6886690536158693e-16, - "velocityX": 2.0777333920330334, - "velocityY": 0.8657218908791364, - "timestamp": 9.048600349294825 + "x": 2.5598506571147865, + "y": 6.885847566082, + "heading": 0.3930579718029317, + "angularVelocity": 1.8302909821567636e-16, + "velocityX": 2.1561658320305157, + "velocityY": 0.8984020607802621, + "timestamp": 8.834332076725445 }, { - "x": 2.712129812020004, - "y": 6.9492971878821415, - "heading": 0.3930579718029318, - "angularVelocity": 9.579438090360999e-17, - "velocityX": 1.5583001428416108, - "velocityY": 0.649291459333017, - "timestamp": 9.14632167428642 + "x": 2.7121298118541755, + "y": 6.9492971878130465, + "heading": 0.3930579718029317, + "angularVelocity": 1.1024323128215209e-16, + "velocityX": 1.6171244804410174, + "velocityY": 0.6738015899260661, + "timestamp": 8.928498701856158 }, { - "x": 2.8136492502761614, - "y": 6.991596936437518, + "x": 2.8136492502089334, + "y": 6.991596936409507, "heading": 0.3930579718029317, - "angularVelocity": -7.496030828989607e-17, - "velocityX": 1.0388667802538214, - "velocityY": 0.4328609805384309, - "timestamp": 9.244042999278017 + "angularVelocity": -3.9462401223913846e-17, + "velocityX": 1.078083006732355, + "velocityY": 0.44920106818890604, + "timestamp": 9.022665326986871 }, { "x": 2.8644089698791504, "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 4.893193347075666e-17, - "velocityX": 0.5194333949867557, - "velocityY": 0.21643049229415012, - "timestamp": 9.341764324269613 + "angularVelocity": 1.2061194980042807e-16, + "velocityX": 0.5390415085998568, + "velocityY": 0.22460053627515186, + "timestamp": 9.116831952117584 }, { "x": 2.8644089698791504, "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 3.090778525044755e-27, - "velocityX": 4.437436909321685e-22, - "velocityY": 1.850515832937087e-22, - "timestamp": 9.43948564926121 - }, - { - "x": 2.8419574642644285, - "y": 7.0048102452086525, - "heading": 0.3900130033247021, - "angularVelocity": -0.04733865202910357, - "velocityX": -0.3490426976251426, - "velocityY": -0.12338594795790273, - "timestamp": 9.503808741442997 - }, - { - "x": 2.7972074454620057, - "y": 6.988518822273239, - "heading": 0.3838409042111465, - "angularVelocity": -0.0959546393713819, - "velocityX": -0.6957068959923765, - "velocityY": -0.25327487194445947, - "timestamp": 9.568131833624784 - }, - { - "x": 2.7303816098102875, - "y": 6.963303209303571, - "heading": 0.37443012101631223, - "angularVelocity": -0.14630489417762893, - "velocityX": -1.0389089421083304, - "velocityY": -0.39201493762775413, - "timestamp": 9.632454925806572 - }, - { - "x": 2.6418300370011316, - "y": 6.928345468623928, - "heading": 0.3616207312094327, - "angularVelocity": -0.19914138721251212, - "velocityX": -1.3766684685943704, - "velocityY": -0.5434710847054101, - "timestamp": 9.69677801798836 - }, - { - "x": 2.5321688819092247, - "y": 6.882378878870811, - "heading": 0.34516587333325927, - "angularVelocity": -0.25581571591224617, - "velocityX": -1.7048489332880077, - "velocityY": -0.7146203360872027, - "timestamp": 9.761101110170147 - }, - { - "x": 2.4026942269657687, - "y": 6.823220010782376, - "heading": 0.324643183634766, - "angularVelocity": -0.3190563295758964, - "velocityX": -2.012879831360391, - "velocityY": -0.9197143060417909, - "timestamp": 9.825424202351934 - }, - { - "x": 2.2570878780964114, - "y": 6.7465695928484575, - "heading": 0.2992393598461131, - "angularVelocity": -0.3949409601898093, - "velocityX": -2.2636714736575443, - "velocityY": -1.1916469705357338, - "timestamp": 9.889747294533722 - }, - { - "x": 2.110110870224978, - "y": 6.646173604450097, - "heading": 0.26804931760848005, - "angularVelocity": -0.4848964995259366, - "velocityX": -2.2849804461522467, - "velocityY": -1.5608078684187743, - "timestamp": 9.95407038671551 - }, - { - "x": 1.9823765062843866, - "y": 6.531738038725912, - "heading": 0.2347990669991598, - "angularVelocity": -0.5169255625234794, - "velocityX": -1.9858243689465778, - "velocityY": -1.779074385926154, - "timestamp": 10.018393478897297 - }, - { - "x": 1.8778139110395455, - "y": 6.411761042169391, - "heading": 0.201692518308331, - "angularVelocity": -0.5146914983077023, - "velocityX": -1.6255840896039286, - "velocityY": -1.865224330594162, - "timestamp": 10.082716571079084 - }, - { - "x": 1.796996456842112, - "y": 6.289847592193697, - "heading": 0.16955186256906168, - "angularVelocity": -0.49967522780830353, - "velocityX": -1.2564298676598118, - "velocityY": -1.8953294352072878, - "timestamp": 10.147039663260871 + "angularVelocity": -7.454320302880939e-28, + "velocityX": 6.858010204674672e-22, + "velocityY": 2.856861212118512e-22, + "timestamp": 9.210998577248297 + }, + { + "x": 2.842186915849872, + "y": 7.004763611912437, + "heading": 0.3899906597418949, + "angularVelocity": -0.04969534926930016, + "velocityX": -0.36003273044640205, + "velocityY": -0.1293405609631654, + "timestamp": 9.272720893443621 + }, + { + "x": 2.7978951293903465, + "y": 6.988387145564426, + "heading": 0.38377625605112037, + "angularVelocity": -0.10068325483944376, + "velocityX": -0.7175976079601634, + "velocityY": -0.26532488340499644, + "timestamp": 9.334443209638945 + }, + { + "x": 2.7317542660001495, + "y": 6.96306112048486, + "heading": 0.37430680047739806, + "angularVelocity": -0.15342028876161432, + "velocityX": -1.071587514326101, + "velocityY": -0.41032201383078476, + "timestamp": 9.396165525834268 + }, + { + "x": 2.644108850979856, + "y": 6.927989989672691, + "heading": 0.36142823816874126, + "angularVelocity": -0.20865325707968818, + "velocityX": -1.4199955611343862, + "velocityY": -0.5682082749646841, + "timestamp": 9.457887842029592 + }, + { + "x": 2.53555910261038, + "y": 6.881949867747595, + "heading": 0.3449043845685677, + "angularVelocity": -0.267712792045632, + "velocityX": -1.7586791141467173, + "velocityY": -0.7459234319625763, + "timestamp": 9.519610158224916 + }, + { + "x": 2.407346628684672, + "y": 6.822852176062135, + "heading": 0.3243353524397527, + "angularVelocity": -0.33325113827101066, + "velocityX": -2.0772466399344327, + "velocityY": -0.9574768953653868, + "timestamp": 9.58133247442024 + }, + { + "x": 2.2628964345938334, + "y": 6.74663530878728, + "heading": 0.298961324703692, + "angularVelocity": -0.41109973345399, + "velocityX": -2.3403236138079526, + "velocityY": -1.2348348534695668, + "timestamp": 9.643054790615563 + }, + { + "x": 2.1156790935491716, + "y": 6.646996400731561, + "heading": 0.26784048264776694, + "angularVelocity": -0.5042072944482706, + "velocityX": -2.385155809428528, + "velocityY": -1.6143092838642825, + "timestamp": 9.704777106810887 + }, + { + "x": 1.9869123530220612, + "y": 6.532612256734745, + "heading": 0.23448482199122617, + "angularVelocity": -0.540414921419745, + "velocityX": -2.0862266432066443, + "velocityY": -1.8532056320576245, + "timestamp": 9.766499423006211 + }, + { + "x": 1.881043758757729, + "y": 6.412413108766105, + "heading": 0.20123842984354556, + "angularVelocity": -0.5386445972388703, + "velocityX": -1.7152401398758959, + "velocityY": -1.9474179742098816, + "timestamp": 9.828221739201535 + }, + { + "x": 1.7987115747047, + "y": 6.290184489368743, + "heading": 0.16896416101959177, + "angularVelocity": -0.5228946483767686, + "velocityX": -1.3339127422322181, + "velocityY": -1.9802986493663424, + "timestamp": 9.889944055396859 }, { "x": 1.740001559257507, "y": 6.167843341827393, - "heading": 0.1387804363394676, - "angularVelocity": -0.4783884789404854, - "velocityX": -0.8860721033672938, - "velocityY": -1.8967410649584446, - "timestamp": 10.211362755442659 - }, - { - "x": 1.706182040858774, - "y": 6.03669580225492, - "heading": 0.10729245974250749, - "angularVelocity": -0.45130411484825544, - "velocityX": -0.48472113692462804, - "velocityY": -1.8796833159166881, - "timestamp": 10.281133837870186 - }, - { - "x": 1.699651780790286, - "y": 5.911922041931935, - "heading": 0.07852355852866735, - "angularVelocity": -0.4123327346071031, - "velocityX": -0.09359551036449741, - "velocityY": -1.7883305802599465, - "timestamp": 10.350904920297713 - }, - { - "x": 1.7185433856985064, - "y": 5.798927064131694, - "heading": 0.053308691164350094, - "angularVelocity": -0.36139424080898114, - "velocityX": 0.2707655414095584, - "velocityY": -1.6195101733961197, - "timestamp": 10.42067600272524 - }, - { - "x": 1.7598996151781712, - "y": 5.702665752208848, - "heading": 0.032386709878439925, - "angularVelocity": -0.29986608431425904, - "velocityX": 0.592741692414218, - "velocityY": -1.3796734775160497, - "timestamp": 10.490447085152768 - }, - { - "x": 1.820114668725021, - "y": 5.6271104583982785, - "heading": 0.016317787108466864, - "angularVelocity": -0.2303092084985792, - "velocityX": 0.8630373996189019, - "velocityY": -1.082902703839363, - "timestamp": 10.560218167580295 - }, - { - "x": 1.8954858630630067, - "y": 5.575103001600796, - "heading": 0.005460341828234747, - "angularVelocity": -0.15561526211822638, - "velocityX": 1.0802640824194598, - "velocityY": -0.7454013179672614, - "timestamp": 10.629989250007823 - }, - { - "x": 1.9826012990017379, + "heading": 0.13807920449538028, + "angularVelocity": -0.5003855724803669, + "velocityX": -0.9511959217700356, + "velocityY": -1.9821217848370036, + "timestamp": 9.951666371592182 + }, + { + "x": 1.7034889997175517, + "y": 6.016151230649133, + "heading": 0.1019414560444192, + "angularVelocity": -0.467252098060329, + "velocityX": -0.4720983122052568, + "velocityY": -1.961341263510758, + "timestamp": 10.029007380013498 + }, + { + "x": 1.7027290435050462, + "y": 5.874298147554192, + "heading": 0.06966485107958241, + "angularVelocity": -0.4173284732597485, + "velocityX": -0.009826044785523666, + "velocityY": -1.8341250778913685, + "timestamp": 10.106348388434814 + }, + { + "x": 1.7342944275877592, + "y": 5.750595898188055, + "heading": 0.04250435573420922, + "angularVelocity": -0.35117844853297475, + "velocityX": 0.40813256417294685, + "velocityY": -1.5994393128709739, + "timestamp": 10.18368939685613 + }, + { + "x": 1.7930814617897048, + "y": 5.6520476897956975, + "heading": 0.02146340510922574, + "angularVelocity": -0.2720542575597529, + "velocityX": 0.7601017287194328, + "velocityY": -1.2742038202491095, + "timestamp": 10.261030405277445 + }, + { + "x": 1.8734294527078426, + "y": 5.583639683859189, + "heading": 0.0071877988424821045, + "angularVelocity": -0.1845800379143917, + "velocityX": 1.0388795356848963, + "velocityY": -0.8844984999918234, + "timestamp": 10.338371413698761 + }, + { + "x": 1.970064237353583, "y": 5.548515796661377, - "heading": -3.619508887306417e-27, - "angularVelocity": -0.07826081577430707, - "velocityX": 1.248589428567619, - "velocityY": -0.3810633863540251, - "timestamp": 10.69976033243535 + "heading": 3.498573366455402e-27, + "angularVelocity": -0.09293645103935767, + "velocityX": 1.249463727177213, + "velocityY": -0.45414312425918, + "timestamp": 10.415712422120077 }, { "x": 2.0785037517547607, "y": 5.548515796661377, - "heading": -1.3504694626823558e-27, - "angularVelocity": 2.3770174727505955e-28, - "velocityX": 1.3745300978043182, - "velocityY": 2.5815580956362606e-25, - "timestamp": 10.769531414862877 + "heading": 9.074763160083063e-28, + "angularVelocity": -2.8883864105140666e-27, + "velocityX": 1.4020959464409022, + "velocityY": 1.1648614692434482e-25, + "timestamp": 10.493053430541393 }, { - "x": 2.2108243531689626, + "x": 2.2093539983390484, "y": 5.548515796661377, - "heading": -3.98229506769339e-28, - "angularVelocity": 6.58921025141663e-29, - "velocityX": 1.7982508177997887, - "velocityY": 1.343058267982166e-16, - "timestamp": 10.843114365641734 + "heading": -5.091869940539703e-28, + "angularVelocity": -8.863496880486678e-29, + "velocityX": 1.8425023918344958, + "velocityY": 6.335504596026049e-17, + "timestamp": 10.56407111125876 }, { - "x": 2.366717480768576, + "x": 2.3657372440174322, "y": 5.548515796661377, - "heading": 5.540104511762132e-28, - "angularVelocity": 6.589213013653999e-29, - "velocityX": 2.118603915030903, - "velocityY": 6.71525155127488e-16, - "timestamp": 10.91669731642059 + "heading": -1.9258503020667073e-27, + "angularVelocity": -8.863493994530242e-29, + "velocityX": 2.202032565675437, + "velocityY": 3.167727717458953e-16, + "timestamp": 10.635088791976129 }, { - "x": 2.4914319874847974, + "x": 2.4908438453710073, "y": 5.548515796661377, - "heading": 1.506250408105455e-27, - "angularVelocity": 6.589211632476856e-29, - "velocityX": 1.694883195035658, - "velocityY": 5.372204471623799e-16, - "timestamp": 10.990280267199447 + "heading": -3.342513611103023e-27, + "angularVelocity": -8.863495435827685e-29, + "velocityX": 1.7616261202821912, + "velocityY": 2.534184135606925e-16, + "timestamp": 10.706106472693497 }, { - "x": 2.58496786905088, + "x": 2.5846737979725942, "y": 5.548515796661377, - "heading": 2.458490365034698e-27, - "angularVelocity": 6.589211632476792e-29, - "velocityX": 1.2711624170548799, - "velocityY": 4.029154421641203e-16, - "timestamp": 11.063863217978303 + "heading": -4.759176920139342e-27, + "angularVelocity": -8.863495435827717e-29, + "velocityX": 1.321219612549812, + "velocityY": 1.9006387498434594e-16, + "timestamp": 10.777124153410865 }, { - "x": 2.647325124044576, + "x": 2.6472271003464662, "y": 5.548515796661377, - "heading": 3.4107303219639416e-27, - "angularVelocity": 6.589211632476844e-29, - "velocityX": 0.8474416197456015, - "velocityY": 2.686103379493745e-16, - "timestamp": 11.13744616875716 + "heading": -6.175840229175665e-27, + "angularVelocity": -8.863495435827698e-29, + "velocityX": 0.8808130840377387, + "velocityY": 1.2670927613090377e-16, + "timestamp": 10.848141834128233 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 4.3629702788936556e-27, - "angularVelocity": 6.589211633112284e-29, - "velocityX": 0.42372081277207485, - "velocityY": 1.3430518397832439e-16, - "timestamp": 11.211029119536017 + "heading": -7.592503538214036e-27, + "angularVelocity": -8.863495438700938e-29, + "velocityX": 0.44040654513582195, + "velocityY": 6.3354647036682e-17, + "timestamp": 10.919159514845601 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 5.369537146588021e-27, - "angularVelocity": 8.042005177965745e-28, - "velocityX": 2.643601734591811e-26, - "velocityY": 4.252096317345015e-26, - "timestamp": 11.284612070314873 - }, - { - "x": 2.65514872822099, - "y": 5.535337362244174, - "heading": -0.005358266322899027, - "angularVelocity": -0.07847108857121549, - "velocityX": -0.34203117386479875, - "velocityY": -0.1929963969806516, - "timestamp": 11.352895388674044 - }, - { - "x": 2.608944125410673, - "y": 5.508126235941394, - "heading": -0.01624992945284291, - "angularVelocity": -0.15950693949367814, - "velocityX": -0.676660184662962, - "velocityY": -0.39850327952208153, - "timestamp": 11.421178707033215 - }, - { - "x": 2.5407349138993025, - "y": 5.465596827387689, - "heading": -0.03293816440101749, - "angularVelocity": -0.24439695300679784, - "velocityX": -0.9989147151957237, - "velocityY": -0.622837459802409, - "timestamp": 11.489462025392386 - }, - { - "x": 2.4521626115248987, - "y": 5.405634188727449, - "heading": -0.05584758658914312, - "angularVelocity": -0.33550540217775826, - "velocityX": -1.2971294380936889, - "velocityY": -0.8781447665568274, - "timestamp": 11.557745343751558 - }, - { - "x": 2.3472993944122122, - "y": 5.324395116991271, - "heading": -0.08569447031240085, - "angularVelocity": -0.43710359192361903, - "velocityX": -1.5357076901433586, - "velocityY": -1.1897352631408427, - "timestamp": 11.626028662110729 - }, - { - "x": 2.239880929754108, - "y": 5.216483136191594, - "heading": -0.12272614650889033, - "angularVelocity": -0.542323909943895, - "velocityX": -1.5731289462688671, - "velocityY": -1.580356423688402, - "timestamp": 11.6943119804699 - }, - { - "x": 2.153374403355559, - "y": 5.091739159562662, - "heading": -0.16180305380781881, - "angularVelocity": -0.5722760439582536, - "velocityX": -1.2668764271754205, - "velocityY": -1.8268587354348766, - "timestamp": 11.762595298829071 + "heading": -9.168166896056586e-27, + "angularVelocity": -2.3275147262701813e-27, + "velocityX": -7.876142714980879e-26, + "velocityY": -2.7822667519177177e-25, + "timestamp": 10.99017719556297 + }, + { + "x": 2.6551487281624913, + "y": 5.535337362195577, + "heading": -0.0053582663887861755, + "angularVelocity": -0.08143329787865013, + "velocityX": -0.3549425234125067, + "velocityY": -0.20028182653884286, + "timestamp": 11.055976647732658 + }, + { + "x": 2.608944125248311, + "y": 5.508126235802502, + "heading": -0.016249929633628882, + "angularVelocity": -0.16552817517013058, + "velocityX": -0.7022034590048659, + "velocityY": -0.41354639736059495, + "timestamp": 11.121776099902347 + }, + { + "x": 2.5407349136089206, + "y": 5.4655968271259, + "heading": -0.03293816472196888, + "angularVelocity": -0.2536227056313985, + "velocityX": -1.0366227892519315, + "velocityY": -0.6463489782091855, + "timestamp": 11.187575552072035 + }, + { + "x": 2.4521626111212957, + "y": 5.405634188324023, + "heading": -0.05584758703948852, + "angularVelocity": -0.34817041118274633, + "velocityX": -1.3460948315984256, + "velocityY": -0.9112938911290825, + "timestamp": 11.253375004241724 + }, + { + "x": 2.347299393995622, + "y": 5.324395116461008, + "heading": -0.08569447081932852, + "angularVelocity": -0.45360383400866283, + "velocityX": -1.593679182240697, + "velocityY": -1.2346466297851608, + "timestamp": 11.319174456411412 + }, + { + "x": 2.239880929566337, + "y": 5.216483135728542, + "heading": -0.12272614687803589, + "angularVelocity": -0.5627961151288577, + "velocityX": -1.6325130512069779, + "velocityY": -1.6400133614209311, + "timestamp": 11.3849739085811 + }, + { + "x": 2.1533744032985735, + "y": 5.091739159361965, + "heading": -0.16180305396789932, + "angularVelocity": -0.5938789123819668, + "velocityX": -1.314699794835295, + "velocityY": -1.8958208959685579, + "timestamp": 11.450773360750789 }, { "x": 2.0932393074035645, "y": 4.9619622230529785, - "heading": -0.19963034769606672, - "angularVelocity": -0.5539756238745723, - "velocityX": -0.8806703803655747, - "velocityY": -1.9005657549777475, - "timestamp": 11.830878617188242 - }, - { - "x": 2.062901110669511, - "y": 4.862373019706677, - "heading": -0.22719939385993115, - "angularVelocity": -0.526195783497447, - "velocityX": -0.5790490938819385, - "velocityY": -1.9008063815925753, - "timestamp": 11.883271751129039 - }, - { - "x": 2.0480285995415843, - "y": 4.7660067701880315, - "heading": -0.2526003669786383, - "angularVelocity": -0.4848149215011564, - "velocityX": -0.28386374338156817, - "velocityY": -1.8392915687680964, - "timestamp": 11.935664885069835 - }, - { - "x": 2.0477407755531423, - "y": 4.675697634157938, - "heading": -0.27523200757833177, - "angularVelocity": -0.43195813835581237, - "velocityX": -0.005493544035129206, - "velocityY": -1.723682651473774, - "timestamp": 11.988058019010632 - }, - { - "x": 2.060871040463269, - "y": 4.593707250314659, - "heading": -0.2946333476397457, - "angularVelocity": -0.37030310275649647, - "velocityX": 0.2506104125201524, - "velocityY": -1.5649070341149178, - "timestamp": 12.040451152951428 - }, - { - "x": 2.0861920181034543, - "y": 4.521742431666527, - "heading": -0.3104741157540841, - "angularVelocity": -0.30234435169001433, - "velocityX": 0.48328809016841023, - "velocityY": -1.373554380798257, - "timestamp": 12.092844286892225 - }, - { - "x": 2.122546344711596, - "y": 4.4610604943149115, - "heading": -0.32252861579276854, - "angularVelocity": -0.23007785814655887, - "velocityX": 0.6938757786320086, - "velocityY": -1.1582040009323555, - "timestamp": 12.145237420833022 - }, - { - "x": 2.168900206537902, - "y": 4.412584630956035, - "heading": -0.3306480218589531, - "angularVelocity": -0.15497080352855533, - "velocityX": 0.8847316115635476, - "velocityY": -0.9252331309986794, - "timestamp": 12.197630554773818 - }, - { - "x": 2.224353286156776, - "y": 4.376998478351871, + "heading": -0.19963034769256288, + "angularVelocity": -0.5748876696892883, + "velocityX": -0.9139148414173547, + "velocityY": -1.9723102857195456, + "timestamp": 11.516572812920478 + }, + { + "x": 2.06290111070071, + "y": 4.862373019847466, + "heading": -0.22719939375272255, + "angularVelocity": -0.5460591677053325, + "velocityX": -0.6009076413123203, + "velocityY": -1.9725599970405536, + "timestamp": 11.56706010017913 + }, + { + "x": 2.048028599586168, + "y": 4.7660067703914, + "heading": -0.2526003668147138, + "angularVelocity": -0.5031162187791788, + "velocityX": -0.29457932723436914, + "velocityY": -1.9087230605671173, + "timestamp": 11.617547387437783 + }, + { + "x": 2.047740775609064, + "y": 4.67569763435831, + "heading": -0.27523200740159587, + "angularVelocity": -0.4482641436236724, + "velocityX": -0.00570091983015679, + "velocityY": -1.788750018800283, + "timestamp": 11.668034674696436 + }, + { + "x": 2.0608710405337267, + "y": 4.5937072504707785, + "heading": -0.29463334748351117, + "angularVelocity": -0.3842816902108432, + "velocityX": 0.26007071557230893, + "velocityY": -1.62398077495204, + "timestamp": 11.718521961955089 + }, + { + "x": 2.0861920181883997, + "y": 4.5217424317614965, + "heading": -0.31047411563844995, + "angularVelocity": -0.3137575618548228, + "velocityX": 0.5015317524380114, + "velocityY": -1.4254047427939316, + "timestamp": 11.769009249213742 + }, + { + "x": 2.1225463448038515, + "y": 4.461060494351439, + "heading": -0.3225286157246152, + "angularVelocity": -0.2387630776121657, + "velocityX": 0.7200689240680372, + "velocityY": -1.2019250925322578, + "timestamp": 11.819496536472395 + }, + { + "x": 2.16890020662259, + "y": 4.412584630951135, + "heading": -0.3306480218329806, + "angularVelocity": -0.16082080359692727, + "velocityX": 0.9181293813878825, + "velocityY": -0.9601597953155385, + "timestamp": 11.869983823731047 + }, + { + "x": 2.2243532862123785, + "y": 4.376998478332531, "heading": -0.3347371673945201, - "angularVelocity": -0.07804735521619326, - "velocityX": 1.0584035626029547, - "velocityY": -0.6792140482448454, - "timestamp": 12.250023688714615 + "angularVelocity": -0.08099356855103614, + "velocityX": 1.0983572816201457, + "velocityY": -0.7048537275590007, + "timestamp": 11.9204711109897 }, { "x": 2.2881293296813965, "y": 4.354815483093262, "heading": -0.3347371673945201, - "angularVelocity": 3.779464021226463e-27, - "velocityX": 1.2172595668105288, - "velocityY": -0.4233950823341772, - "timestamp": 12.302416822655411 + "angularVelocity": -3.351040193936762e-27, + "velocityX": 1.2632099471355258, + "velocityY": -0.43937784031895566, + "timestamp": 11.970958398248353 }, { - "x": 2.4097151176038345, - "y": 4.314433194202397, + "x": 2.4097151173059284, + "y": 4.31443319431648, "heading": -0.3347371673945201, - "angularVelocity": 6.573584275341322e-29, - "velocityX": 1.6302002618641742, - "velocityY": -0.5414384283676151, - "timestamp": 12.377000167270632 + "angularVelocity": -1.1055944271274773e-28, + "velocityX": 1.6917387541138205, + "velocityY": -0.5618772081689679, + "timestamp": 12.042828707614305 }, { - "x": 2.5011472356587063, - "y": 4.284859003501582, + "x": 2.501147235462863, + "y": 4.284859003573599, "heading": -0.3347371673945201, - "angularVelocity": 6.573584272217567e-29, - "velocityX": 1.225905308035939, - "velocityY": -0.396525401929742, - "timestamp": 12.451583511885852 + "angularVelocity": -1.1055944267694559e-28, + "velocityX": 1.2721820590944748, + "velocityY": -0.4114938561387572, + "timestamp": 12.114699016980257 }, { - "x": 2.562139216220098, - "y": 4.265257651811236, + "x": 2.562139216145648, + "y": 4.265257651838079, "heading": -0.3347371673945201, - "angularVelocity": -6.199774132429429e-28, - "velocityX": 0.8177694480725244, - "velocityY": -0.2628113795576003, - "timestamp": 12.526166856501073 + "angularVelocity": -1.105594426769472e-28, + "velocityX": 0.8486394621209057, + "velocityY": -0.2727322577076049, + "timestamp": 12.18656932634621 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": 7.63642932425348e-28, - "velocityX": 0.40901189675210486, - "velocityY": -0.1310099931742232, - "timestamp": 12.600750201116293 + "angularVelocity": -1.1055944267596697e-28, + "velocityX": 0.42445170461571047, + "velocityY": -0.13595549514112273, + "timestamp": 12.258439635712161 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": 1.6433960683327454e-29, - "velocityX": 1.171390593825864e-26, - "velocityY": -5.961445669521376e-27, - "timestamp": 12.675333545731513 + "angularVelocity": -2.916408988560576e-29, + "velocityX": -2.969429335311962e-26, + "velocityY": 1.2754135405824192e-26, + "timestamp": 12.330309945078113 } ], "trajectoryWaypoints": [ @@ -31009,10 +30955,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 10 }, { - "timestamp": 0.6133453390157622, + "timestamp": 0.5891141473001675, "isStopPoint": false, "x": 2.3368515968322754, "y": 6.133185386657715, @@ -31020,10 +30966,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { - "timestamp": 1.5696159325577983, + "timestamp": 1.5430433785236333, "isStopPoint": false, "x": 5.747424125671387, "y": 7.119814872741699, @@ -31031,10 +30977,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 11 }, { - "timestamp": 1.9695805037098402, + "timestamp": 1.9430147539809723, "isStopPoint": false, "x": 7.125290870666504, "y": 7.458, @@ -31045,7 +30991,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 2.554317040186094, + "timestamp": 2.508796597442937, "isStopPoint": false, "x": 8.186561584472656, "y": 7.469284534454346, @@ -31056,7 +31002,7 @@ "controlIntervalCount": 19 }, { - "timestamp": 4.327573921984801, + "timestamp": 4.2320699051573625, "isStopPoint": true, "x": 4.337841033935547, "y": 6.292478084564209, @@ -31067,7 +31013,7 @@ "controlIntervalCount": 12 }, { - "timestamp": 5.042289487950278, + "timestamp": 4.9229006835446425, "isStopPoint": false, "x": 5.851044178009033, "y": 6.557126998901367, @@ -31078,7 +31024,7 @@ "controlIntervalCount": 12 }, { - "timestamp": 5.4294826170694, + "timestamp": 5.308843742309995, "isStopPoint": false, "x": 7.214789390563965, "y": 6.145846366882324, @@ -31089,7 +31035,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 6.000507663315493, + "timestamp": 5.861500074494232, "isStopPoint": false, "x": 8.186561584472656, "y": 5.798520088195801, @@ -31100,7 +31046,7 @@ "controlIntervalCount": 15 }, { - "timestamp": 7.01207973149339, + "timestamp": 6.845550680900895, "isStopPoint": false, "x": 5.851044178009033, "y": 6.557126998901367, @@ -31108,10 +31054,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 13 }, { - "timestamp": 7.674560355116261, + "timestamp": 7.507867232153339, "isStopPoint": false, "x": 3.405486822128296, "y": 6.266934394836426, @@ -31119,10 +31065,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 9 }, { - "timestamp": 8.171664921763616, + "timestamp": 7.9885953573901745, "isStopPoint": false, "x": 2.205486822128296, "y": 6.266934394836426, @@ -31133,7 +31079,7 @@ "controlIntervalCount": 8 }, { - "timestamp": 8.65771504932844, + "timestamp": 8.457665576202594, "isStopPoint": true, "x": 2.052253484725952, "y": 6.674348831176758, @@ -31144,7 +31090,7 @@ "controlIntervalCount": 8 }, { - "timestamp": 9.43948564926121, + "timestamp": 9.210998577248297, "isStopPoint": true, "x": 2.8644089698791504, "y": 7.012746810913086, @@ -31155,7 +31101,7 @@ "controlIntervalCount": 12 }, { - "timestamp": 10.211362755442659, + "timestamp": 9.951666371592182, "isStopPoint": false, "x": 1.7400015592575073, "y": 6.167843341827393, @@ -31163,10 +31109,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 8 + "controlIntervalCount": 7 }, { - "timestamp": 10.769531414862877, + "timestamp": 10.493053430541393, "isStopPoint": false, "x": 2.0785037517547607, "y": 5.548515796661377, @@ -31177,7 +31123,7 @@ "controlIntervalCount": 7 }, { - "timestamp": 11.284612070314873, + "timestamp": 10.99017719556297, "isStopPoint": true, "x": 2.6785037517547607, "y": 5.548515796661377, @@ -31188,7 +31134,7 @@ "controlIntervalCount": 8 }, { - "timestamp": 11.830878617188242, + "timestamp": 11.516572812920478, "isStopPoint": false, "x": 2.0932393074035645, "y": 4.9619622230529785, @@ -31199,7 +31145,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 12.302416822655411, + "timestamp": 11.970958398248353, "isStopPoint": false, "x": 2.2881293296813965, "y": 4.354815483093262, @@ -31210,7 +31156,7 @@ "controlIntervalCount": 5 }, { - "timestamp": 12.675333545731513, + "timestamp": 12.330309945078113, "isStopPoint": true, "x": 2.592644691467285, "y": 4.255486488342285, diff --git a/src/main/deploy/choreo/CenterLanePDEABC.1.traj b/src/main/deploy/choreo/CenterLanePDEABC.1.traj index d1c28fcf..a82fc57e 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.1.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.1.traj @@ -3,623 +3,596 @@ { "x": 1.3350272178649902, "y": 5.601006507873535, - "heading": 3.967978705267542e-25, - "angularVelocity": 7.379244608128328e-26, - "velocityX": 8.172146600807894e-26, - "velocityY": -1.420872098935511e-25, + "heading": 1.1438500584259617e-25, + "angularVelocity": 2.0139373174315835e-26, + "velocityX": -2.9204847474037974e-26, + "velocityY": 3.705827554701837e-26, "timestamp": 0 }, { - "x": 1.3486609048876415, - "y": 5.6125344531964885, - "heading": 0.0053952990645401815, - "angularVelocity": 0.09676162177278211, - "velocityX": 0.24451242670209422, - "velocityY": 0.20674714632375757, - "timestamp": 0.05575866718325111 - }, - { - "x": 1.3762911400128175, - "y": 5.635152571560341, - "heading": 0.015980040657686934, - "angularVelocity": 0.1898313235924372, - "velocityX": 0.4955325606038828, - "velocityY": 0.4056430956198756, - "timestamp": 0.11151733436650221 - }, - { - "x": 1.4183448247504244, - "y": 5.668306854106291, - "heading": 0.0314941432622812, - "angularVelocity": 0.2782366112447971, - "velocityX": 0.7542089304143026, - "velocityY": 0.594603210958904, - "timestamp": 0.1672760015497533 - }, - { - "x": 1.4753265664194932, - "y": 5.711278723492, - "heading": 0.05160335039247586, - "angularVelocity": 0.3606471988310923, - "velocityX": 1.0219351456482668, - "velocityY": 0.7706760501373088, - "timestamp": 0.22303466873300443 - }, - { - "x": 1.547831871808343, - "y": 5.763109379844883, - "heading": 0.07586790196130719, - "angularVelocity": 0.43517093923866185, - "velocityX": 1.3003414366157815, - "velocityY": 0.9295533586292857, - "timestamp": 0.27879333591625555 - }, - { - "x": 1.6365515186230641, - "y": 5.822478611934523, - "heading": 0.10369270639110653, - "angularVelocity": 0.499022050479668, - "velocityX": 1.5911364330704598, - "velocityY": 1.064753429175822, - "timestamp": 0.33455200309950667 - }, - { - "x": 1.7422414603963892, - "y": 5.887510284275809, - "heading": 0.1342490325748326, - "angularVelocity": 0.5480103404068561, - "velocityX": 1.8954890264140394, - "velocityY": 1.1663060762833288, - "timestamp": 0.3903106702827578 - }, - { - "x": 1.8655876183871098, - "y": 5.955479067462068, - "heading": 0.1663576179952195, - "angularVelocity": 0.57584922743691, - "velocityX": 2.212143227623125, - "velocityY": 1.2189814896916369, - "timestamp": 0.4460693374660089 - }, - { - "x": 2.0068104942426706, - "y": 6.022477551407248, - "heading": 0.1983353246164728, - "angularVelocity": 0.5735019905723076, - "velocityX": 2.532752000535291, - "velocityY": 1.201579724368033, - "timestamp": 0.50182800464926 - }, - { - "x": 2.164876110933553, - "y": 6.083435356761985, - "heading": 0.22790661577104523, - "angularVelocity": 0.5303442971006865, - "velocityX": 2.834816983185756, - "velocityY": 1.093243587663212, - "timestamp": 0.5575866718325111 + "x": 1.3513037651461415, + "y": 5.614995878157814, + "heading": 0.006549885022033663, + "angularVelocity": 0.1111819339605223, + "velocityX": 0.27628851481066125, + "velocityY": 0.23746451088284168, + "timestamp": 0.05891141473001675 + }, + { + "x": 1.384378906601625, + "y": 5.642351283609904, + "heading": 0.019354935835911788, + "angularVelocity": 0.21736111537232614, + "velocityX": 0.5614385871916767, + "velocityY": 0.46434813316664775, + "timestamp": 0.1178228294600335 + }, + { + "x": 1.4348748758417986, + "y": 5.682261570187258, + "heading": 0.0380334905813409, + "angularVelocity": 0.3170617244727609, + "velocityX": 0.8571508505030827, + "velocityY": 0.6774627083776107, + "timestamp": 0.17673424419005024 + }, + { + "x": 1.5035350329878103, + "y": 5.733639392412991, + "heading": 0.06208045701070026, + "angularVelocity": 0.4081885749911699, + "velocityX": 1.1654813835429356, + "velocityY": 0.8721199866136561, + "timestamp": 0.235645658920067 + }, + { + "x": 1.5912385139518137, + "y": 5.794977395067809, + "heading": 0.09080789039928205, + "angularVelocity": 0.48763781213260343, + "velocityX": 1.4887349313530633, + "velocityY": 1.0411904541067623, + "timestamp": 0.29455707365008377 + }, + { + "x": 1.6989811168528033, + "y": 5.864111533799202, + "heading": 0.12324878614182731, + "angularVelocity": 0.550672495155949, + "velocityX": 1.8288917927834498, + "velocityY": 1.1735270498633483, + "timestamp": 0.35346848838010053 + }, + { + "x": 1.8277399268235333, + "y": 5.93784849269496, + "heading": 0.15800938597032024, + "angularVelocity": 0.590048634679649, + "velocityX": 2.1856343216474197, + "velocityY": 1.2516582606899704, + "timestamp": 0.4123799031101173 + }, + { + "x": 1.978020614375797, + "y": 6.011501208422767, + "heading": 0.19307007056378325, + "angularVelocity": 0.5951424652444947, + "velocityX": 2.5509604249190123, + "velocityY": 1.250228263322945, + "timestamp": 0.47129131784013406 + }, + { + "x": 2.1488551922208927, + "y": 6.078800735840598, + "heading": 0.2256402887317746, + "angularVelocity": 0.5528676966468451, + "velocityX": 2.899855293375791, + "velocityY": 1.1423851850487134, + "timestamp": 0.5302027325701508 }, { "x": 2.3368515968322754, "y": 6.133185386657715, "heading": 0.2525545797921912, - "angularVelocity": 0.44204722362068527, - "velocityX": 3.0842825803838596, - "velocityY": 0.8922385058492518, - "timestamp": 0.6133453390157622 - }, - { - "x": 2.5269005159766844, - "y": 6.188163800352406, - "heading": 0.252554892201178, - "angularVelocity": 0.000005553817937610827, - "velocityX": 3.378574691383036, - "velocityY": 0.9773729727982717, - "timestamp": 0.6695965504005877 - }, - { - "x": 2.7281832415838734, - "y": 6.246391992377104, - "heading": 0.2525548726806658, - "angularVelocity": -3.47023853848989e-7, - "velocityX": 3.578282505423293, - "velocityY": 1.0351455656012056, - "timestamp": 0.7258477617854133 - }, - { - "x": 2.929465967191096, - "y": 6.304620184401812, - "heading": 0.2525548531602303, - "angularVelocity": -3.4702249119339613e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 0.7820989731702389 - }, - { - "x": 3.1307486927983192, - "y": 6.36284837642652, - "heading": 0.25255483363979486, - "angularVelocity": -3.4702249016578237e-7, - "velocityX": 3.5782825054238923, - "velocityY": 1.0351455656013788, - "timestamp": 0.8383501845550645 - }, - { - "x": 3.332031418405542, - "y": 6.421076568451228, - "heading": 0.2525548141193594, - "angularVelocity": -3.470224908197631e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 0.8946013959398901 - }, - { - "x": 3.5333141440127647, - "y": 6.4793047604759355, - "heading": 0.2525547945989239, - "angularVelocity": -3.4702249080835237e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 0.9508526073247157 - }, - { - "x": 3.7345968696199874, - "y": 6.537532952500643, - "heading": 0.2525547750784885, - "angularVelocity": -3.470224894869701e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 1.0071038187095414 - }, - { - "x": 3.93587959522721, - "y": 6.595761144525352, - "heading": 0.25255475555805307, - "angularVelocity": -3.470224898418443e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 1.063355030094367 - }, - { - "x": 4.137162320834433, - "y": 6.65398933655006, - "heading": 0.2525547360376176, - "angularVelocity": -3.470224909887922e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.1196062414791927 - }, - { - "x": 4.338445046441655, - "y": 6.712217528574768, - "heading": 0.2525547165171822, - "angularVelocity": -3.4702248928646837e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 1.1758574528640184 - }, - { - "x": 4.539727772048877, - "y": 6.770445720599476, - "heading": 0.25255469699674676, - "angularVelocity": -3.470224898754089e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 1.2321086642488441 - }, - { - "x": 4.7410104976561, - "y": 6.8286739126241836, - "heading": 0.25255467747631133, - "angularVelocity": -3.470224907627516e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.2883598756336698 - }, - { - "x": 4.942293223263322, - "y": 6.886902104648891, - "heading": 0.25255465795587584, - "angularVelocity": -3.4702249046766804e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 1.3446110870184955 - }, - { - "x": 5.143575948870544, - "y": 6.945130296673599, - "heading": 0.2525546384354404, - "angularVelocity": -3.47022489425383e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.4008622984033212 - }, - { - "x": 5.344858674477766, - "y": 7.003358488698307, - "heading": 0.25255461891500497, - "angularVelocity": -3.470224896167856e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.457113509788147 - }, - { - "x": 5.546141400084989, - "y": 7.061586680723015, - "heading": 0.25255459939456953, - "angularVelocity": -3.470224897168836e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 1.5133647211729726 + "angularVelocity": 0.45686037559548065, + "velocityX": 3.1911711078904768, + "velocityY": 0.9231598165882506, + "timestamp": 0.5891141473001675 + }, + { + "x": 2.5473350968820796, + "y": 6.1940752301450726, + "heading": 0.25255484664390665, + "angularVelocity": 0.000004475832490919022, + "velocityX": 3.5303834818832023, + "velocityY": 1.0212890683182199, + "timestamp": 0.6487347242516341 + }, + { + "x": 2.7606743654688186, + "y": 6.255791206318409, + "heading": 0.25255482885683184, + "angularVelocity": -2.9833785118107346e-7, + "velocityX": 3.578282524176262, + "velocityY": 1.0351455710261694, + "timestamp": 0.7083553012031008 + }, + { + "x": 2.974013634055561, + "y": 6.317507182491745, + "heading": 0.2525548110697642, + "angularVelocity": -2.983377309458156e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 0.7679758781545674 + }, + { + "x": 3.1873529026423038, + "y": 6.379223158665082, + "heading": 0.25255479328269653, + "angularVelocity": -2.983377305484016e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.827596455106034 + }, + { + "x": 3.400692171229046, + "y": 6.440939134838418, + "heading": 0.2525547754956289, + "angularVelocity": -2.9833773174604553e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 0.8872170320575006 + }, + { + "x": 3.6140314398157884, + "y": 6.502655111011755, + "heading": 0.25255475770856123, + "angularVelocity": -2.983377305928146e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.9468376090089672 + }, + { + "x": 3.827370708402531, + "y": 6.5643710871850915, + "heading": 0.25255473992149363, + "angularVelocity": -2.983377301697445e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.0064581859604338 + }, + { + "x": 4.040709976989273, + "y": 6.626087063358428, + "heading": 0.25255472213442604, + "angularVelocity": -2.983377305025075e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.0660787629119004 + }, + { + "x": 4.254049245576016, + "y": 6.687803039531764, + "heading": 0.2525547043473584, + "angularVelocity": -2.9833773095408974e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.125699339863367 + }, + { + "x": 4.467388514162758, + "y": 6.749519015705101, + "heading": 0.2525546865602907, + "angularVelocity": -2.983377319947091e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.1853199168148336 + }, + { + "x": 4.680727782749501, + "y": 6.811234991878438, + "heading": 0.252554668773223, + "angularVelocity": -2.9833773124406734e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.2449404937663002 + }, + { + "x": 4.894067051336243, + "y": 6.872950968051774, + "heading": 0.2525546509861554, + "angularVelocity": -2.983377305581398e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.3045610707177668 + }, + { + "x": 5.107406319922986, + "y": 6.9346669442251105, + "heading": 0.2525546331990878, + "angularVelocity": -2.983377313652051e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.3641816476692334 + }, + { + "x": 5.320745588509729, + "y": 6.996382920398447, + "heading": 0.2525546154120202, + "angularVelocity": -2.983377306356561e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4238022246207 + }, + { + "x": 5.534084857096471, + "y": 7.058098896571784, + "heading": 0.2525545976249525, + "angularVelocity": -2.983377306690358e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.4834228015721667 }, { "x": 5.747424125671387, "y": 7.119814872741699, "heading": 0.2525545797921912, - "angularVelocity": -3.4847922161224964e-7, - "velocityX": 3.5782825050536915, - "velocityY": 1.0351455654942852, - "timestamp": 1.5696159325577983 - }, - { - "x": 5.864122316952898, - "y": 7.149764367678907, - "heading": 0.23825610881476394, - "angularVelocity": -0.4289921260648423, - "velocityX": 3.501255852098466, - "velocityY": 0.8985644358731831, - "timestamp": 1.6029463134871351 - }, - { - "x": 5.980010019619152, - "y": 7.178464848632163, - "heading": 0.21993450293637717, - "angularVelocity": -0.5496968641681629, - "velocityX": 3.4769390398491176, - "velocityY": 0.8610906972261488, - "timestamp": 1.636276694416472 - }, - { - "x": 6.095759168980239, - "y": 7.207171809244793, - "heading": 0.2014022591938407, - "angularVelocity": -0.5560165598414963, - "velocityX": 3.4727820725027265, - "velocityY": 0.8612851042264353, - "timestamp": 1.6696070753458088 - }, - { - "x": 6.211373074459108, - "y": 7.235898623678772, - "heading": 0.18268720108591044, - "angularVelocity": -0.5615014766140173, - "velocityX": 3.4687243966392267, - "velocityY": 0.8618807716264242, - "timestamp": 1.7029374562751456 - }, - { - "x": 6.326838442963843, - "y": 7.264631799858999, - "heading": 0.16373191419922642, - "angularVelocity": -0.5687089783603383, - "velocityX": 3.464267892693179, - "velocityY": 0.8620716409195424, - "timestamp": 1.7362678372044824 - }, - { - "x": 6.442151068511899, - "y": 7.29337427879422, - "heading": 0.1445283357059787, - "angularVelocity": -0.5761583863671085, - "velocityX": 3.4596851980938594, - "velocityY": 0.8623507482904867, - "timestamp": 1.7695982181338192 - }, - { - "x": 6.557303014616615, - "y": 7.322124225476267, - "heading": 0.1250513432276187, - "angularVelocity": -0.5843615324905178, - "velocityX": 3.454864387804252, - "velocityY": 0.8625748005400329, - "timestamp": 1.802928599063156 - }, - { - "x": 6.672285593632433, - "y": 7.350880202549992, - "heading": 0.10527449948643015, - "angularVelocity": -0.5933578671998123, - "velocityX": 3.449782925061388, - "velocityY": 0.8627557283156009, - "timestamp": 1.8362589799924929 - }, - { - "x": 6.787113803468559, - "y": 7.379671810584065, - "heading": 0.08527801395894485, - "angularVelocity": -0.5999477044645701, - "velocityX": 3.4451514394501603, - "velocityY": 0.8638247518116766, - "timestamp": 1.8695893609218297 - }, - { - "x": 6.901739724666763, - "y": 7.408448228453197, - "heading": 0.0648615716893241, - "angularVelocity": -0.6125475227212472, - "velocityX": 3.4390822427508825, - "velocityY": 0.8633690064971079, - "timestamp": 1.9029197418511665 - }, - { - "x": 7.014861438150348, - "y": 7.4357314738869205, - "heading": 0.038575820671149744, - "angularVelocity": -0.7886423822728691, - "velocityX": 3.3939520140322568, - "velocityY": 0.8185698654799702, - "timestamp": 1.9362501227805033 + "angularVelocity": -2.991041384854961e-7, + "velocityX": 3.5782825239779474, + "velocityY": 1.0351455709687998, + "timestamp": 1.5430433785236333 + }, + { + "x": 5.874242280548121, + "y": 7.151641192910771, + "heading": 0.23431006193331627, + "angularVelocity": -0.5017601477559481, + "velocityX": 3.487748847149364, + "velocityY": 0.8752864413346503, + "timestamp": 1.5794044126561186 + }, + { + "x": 6.000453145534325, + "y": 7.182664404015401, + "heading": 0.21336082591069744, + "angularVelocity": -0.5761452203556151, + "velocityX": 3.4710471799658134, + "velocityY": 0.853199361481138, + "timestamp": 1.615765446788604 + }, + { + "x": 6.126521345623787, + "y": 7.213763203476394, + "heading": 0.192336966602575, + "angularVelocity": -0.5781975075714219, + "velocityX": 3.4671236145297133, + "velocityY": 0.8552781900449158, + "timestamp": 1.6521264809210894 + }, + { + "x": 6.252403333907122, + "y": 7.24487049169852, + "heading": 0.17101724585582967, + "angularVelocity": -0.5863342794119836, + "velocityX": 3.462002423381879, + "velocityY": 0.8555116476825977, + "timestamp": 1.6884875150535748 + }, + { + "x": 6.378086189949285, + "y": 7.2759799644241046, + "heading": 0.14935632557503462, + "angularVelocity": -0.5957179381056964, + "velocityX": 3.456525894841813, + "velocityY": 0.8555717258270096, + "timestamp": 1.7248485491860601 + }, + { + "x": 6.503564352522718, + "y": 7.307098484942146, + "heading": 0.12734794412408884, + "angularVelocity": -0.6052738041155772, + "velocityX": 3.4508964215989995, + "velocityY": 0.8558205579262937, + "timestamp": 1.7612095833185455 + }, + { + "x": 6.628828810809778, + "y": 7.338228595030231, + "heading": 0.10496993550474346, + "angularVelocity": -0.6154392787017191, + "velocityX": 3.4450191331370044, + "velocityY": 0.8561392939117064, + "timestamp": 1.7975706174510309 + }, + { + "x": 6.753876017529989, + "y": 7.3693809168618785, + "heading": 0.08222565272715081, + "angularVelocity": -0.6255125389096852, + "velocityX": 3.4390442874806206, + "velocityY": 0.8567501605740924, + "timestamp": 1.8339316515835162 + }, + { + "x": 6.878734995256494, + "y": 7.4006038692779965, + "heading": 0.05925591834951263, + "angularVelocity": -0.6317129016173038, + "velocityX": 3.4338676197042926, + "velocityY": 0.8586926406535731, + "timestamp": 1.8702926857160016 + }, + { + "x": 7.003212758632201, + "y": 7.431690611037589, + "heading": 0.0352626014950734, + "angularVelocity": -0.6598634342196367, + "velocityX": 3.4233834747977516, + "velocityY": 0.8549465795259067, + "timestamp": 1.906653719848487 }, { "x": 7.125290870666504, "y": 7.458, - "heading": 1.0084355524659913e-21, - "angularVelocity": -1.1573771314805474, - "velocityX": 3.313176430544709, - "velocityY": 0.668114959750724, - "timestamp": 1.9695805037098402 - }, - { - "x": 7.337738270409439, - "y": 7.481917447620179, - "heading": -0.00819620478546554, - "angularVelocity": -0.12615227280600286, - "velocityX": 3.2698941803922152, - "velocityY": 0.368126523919295, - "timestamp": 2.0345512299849795 - }, - { - "x": 7.526586631074925, - "y": 7.500039280530163, - "heading": -0.014041404226933134, - "angularVelocity": -0.08996666308937673, - "velocityX": 2.906668456586843, - "velocityY": 0.27892304656162725, - "timestamp": 2.099521956260119 - }, - { - "x": 7.6918053440422804, - "y": 7.512484823370471, - "heading": -0.017813194168849914, - "angularVelocity": -0.058053682914731695, - "velocityX": 2.5429716187515523, - "velocityY": 0.19155616004049963, - "timestamp": 2.164492682535258 - }, - { - "x": 7.833384352673449, - "y": 7.519294018241946, - "heading": -0.01960436856302971, - "angularVelocity": -0.027568945229186584, - "velocityX": 2.179119993696969, - "velocityY": 0.10480404424970338, - "timestamp": 2.2294634088103975 - }, - { - "x": 7.951318655599417, - "y": 7.520486868366768, - "heading": -0.019461534286698053, - "angularVelocity": 0.0021984405057560424, - "velocityX": 1.815191389835117, - "velocityY": 0.018359808997220107, - "timestamp": 2.294434135085537 - }, - { - "x": 8.045605259786509, - "y": 7.516075387181805, - "heading": -0.017412919986794666, - "angularVelocity": 0.031531343688969565, - "velocityX": 1.4512167185542217, - "velocityY": -0.06789952087470152, - "timestamp": 2.359404861360676 - }, - { - "x": 8.116242172260733, - "y": 7.5060675897274995, - "heading": -0.013477652900012242, - "angularVelocity": 0.06056984910584597, - "velocityX": 1.0872113723200465, - "velocityY": -0.15403548687333402, - "timestamp": 2.4243755876358155 - }, - { - "x": 8.163227969814436, - "y": 7.490469205298236, - "heading": -0.007669730702440132, - "angularVelocity": 0.08939290863052056, - "velocityX": 0.723184120718108, - "velocityY": -0.24008327016705255, - "timestamp": 2.489346313910955 + "heading": -1.1078640911459773e-20, + "angularVelocity": -0.9697909406699016, + "velocityX": 3.3573883402077644, + "velocityY": 0.7235599753997619, + "timestamp": 1.9430147539809723 + }, + { + "x": 7.338537136585377, + "y": 7.4820248133164355, + "heading": -0.00804052191695304, + "angularVelocity": -0.12790212002878093, + "velocityX": 3.392149139191805, + "velocityY": 0.38216730060631093, + "timestamp": 2.005879403254524 + }, + { + "x": 7.527981798214128, + "y": 7.500236040572311, + "heading": -0.013862931395131553, + "angularVelocity": -0.09261818121091622, + "velocityX": 3.013532467259838, + "velocityY": 0.2896894751870813, + "timestamp": 2.068744052528076 + }, + { + "x": 7.6935968882672885, + "y": 7.512744942179138, + "heading": -0.017658491883084444, + "angularVelocity": -0.06037670665172621, + "velocityX": 2.6344709143686895, + "velocityY": 0.1989814904143519, + "timestamp": 2.1316087018016274 + }, + { + "x": 7.835373213607451, + "y": 7.519588665029473, + "heading": -0.019490985187148625, + "angularVelocity": -0.029149821485366035, + "velocityX": 2.255263124482431, + "velocityY": 0.1088644083665287, + "timestamp": 2.194473351075179 + }, + { + "x": 7.953306201274802, + "y": 7.52078579458168, + "heading": -0.019392463666041717, + "angularVelocity": 0.001567200680277371, + "velocityX": 1.8759825916497619, + "velocityY": 0.01904296875969824, + "timestamp": 2.2573380003487307 + }, + { + "x": 8.047393114183997, + "y": 7.516347487170096, + "heading": -0.01738240796128744, + "angularVelocity": 0.0319743405551765, + "velocityX": 1.4966585194769915, + "velocityY": -0.0706010049029291, + "timestamp": 2.3202026496222823 + }, + { + "x": 8.11763212951421, + "y": 7.506281183777245, + "heading": -0.013474109300406987, + "angularVelocity": 0.062170054331707525, + "velocityX": 1.1173054513447094, + "velocityY": -0.1601266134333638, + "timestamp": 2.383067298895834 + }, + { + "x": 8.164021945337474, + "y": 7.490592202393592, + "heading": -0.007677398137362595, + "angularVelocity": 0.09220939319680839, + "velocityX": 0.737931673195224, + "velocityY": -0.24956762767234375, + "timestamp": 2.4459319481693855 }, { "x": 8.186561584472656, "y": 7.469284534454346, - "heading": 7.422813602625121e-23, - "angularVelocity": 0.11804902210820577, - "velocityX": 0.3591404313291331, - "velocityY": -0.3260648611834275, - "timestamp": 2.554317040186094 - }, - { - "x": 8.171272340508109, - "y": 7.427332842662171, - "heading": 0.014840203267270782, - "angularVelocity": 0.15900903302410063, - "velocityX": -0.16382039077820704, - "velocityY": -0.44950179087577585, - "timestamp": 2.6476463497544476 - }, - { - "x": 8.107175512886487, - "y": 7.3738609015025816, - "heading": 0.03350091514476021, - "angularVelocity": 0.1999448186619506, - "velocityX": -0.6867813328746101, - "velocityY": -0.572938355666582, - "timestamp": 2.7409756593228005 - }, - { - "x": 7.994271087246123, - "y": 7.308868753063173, - "heading": 0.05597944584590409, - "angularVelocity": 0.24085178391554368, - "velocityX": -1.2097424288526986, - "velocityY": -0.6963744694994183, - "timestamp": 2.8343049688911535 - }, - { - "x": 7.832559046421551, - "y": 7.232356448582914, - "heading": 0.08227288434163177, - "angularVelocity": 0.2817275582272545, - "velocityX": -1.732703708754388, - "velocityY": -0.8198100343196232, - "timestamp": 2.9276342784595064 - }, - { - "x": 7.6220393708156715, - "y": 7.144324050552095, - "heading": 0.1123783464235267, - "angularVelocity": 0.3225724289736236, - "velocityX": -2.2556651986340626, - "velocityY": -0.9432449295721523, - "timestamp": 3.0209635880278594 - }, - { - "x": 7.362712038546165, - "y": 7.04477163823656, - "heading": 0.14629325961596634, - "angularVelocity": 0.36338973629287186, - "velocityX": -2.778626922977279, - "velocityY": -1.0666789755111628, - "timestamp": 3.1142928975962123 - }, - { - "x": 7.054577023664908, - "y": 6.933699333854541, - "heading": 0.18401565672002676, - "angularVelocity": 0.4041859655720804, - "velocityX": -3.3015889253480717, - "velocityY": -1.1901117119126619, - "timestamp": 3.2076222071645653 - }, - { - "x": 6.71622230121249, - "y": 6.85383794706396, - "heading": 0.18401565782506035, - "angularVelocity": 1.1840156036202986e-8, - "velocityX": -3.6253854659088836, - "velocityY": -0.8556946061203927, - "timestamp": 3.300951516732918 - }, - { - "x": 6.377867419030211, - "y": 6.7739772370157985, - "heading": 0.1840156589300666, - "angularVelocity": 1.1839863049890439e-8, - "velocityX": -3.6253871773740243, - "velocityY": -0.8556873549961612, - "timestamp": 3.394280826301271 - }, - { - "x": 6.0395125368493074, - "y": 6.694116526961809, - "heading": 0.18401566003507294, - "angularVelocity": 1.1839864253851599e-8, - "velocityX": -3.6253871773592854, - "velocityY": -0.8556873550586043, - "timestamp": 3.487610135869624 - }, - { - "x": 5.701157654668755, - "y": 6.614255816906336, - "heading": 0.18401566114007933, - "angularVelocity": 1.18398644204111e-8, - "velocityX": -3.625387177355538, - "velocityY": -0.855687355074483, - "timestamp": 3.580939445437977 - }, - { - "x": 5.362802772497576, - "y": 6.534395106811441, - "heading": 0.18401566224512567, - "angularVelocity": 1.1840292694239514e-8, - "velocityX": -3.6253871772550927, - "velocityY": -0.8556873554969, - "timestamp": 3.67426875500633 - }, - { - "x": 5.069956587719004, - "y": 6.4652758877562615, - "heading": 0.20685757815363412, - "angularVelocity": 0.24474536471074324, - "velocityX": -3.137772968995303, - "velocityY": -0.7405949896646158, - "timestamp": 3.767598064574683 - }, - { - "x": 4.8259180814209035, - "y": 6.407676584765678, - "heading": 0.22589278292875978, - "angularVelocity": 0.20395741555534105, - "velocityX": -2.614811010890098, - "velocityY": -0.6171619961293916, - "timestamp": 3.860927374143036 - }, - { - "x": 4.6306872671740225, - "y": 6.361597170610555, - "heading": 0.2411211921270372, - "angularVelocity": 0.16316856160951593, - "velocityX": -2.0918489073777633, - "velocityY": -0.49372929434751617, - "timestamp": 3.954256683711389 - }, - { - "x": 4.484264152022265, - "y": 6.3270376241371595, - "heading": 0.25254264332581844, - "angularVelocity": 0.12237796734600637, - "velocityX": -1.5688867283917882, - "velocityY": -0.37029681922252144, - "timestamp": 4.047585993279742 - }, - { - "x": 4.386648740184571, - "y": 6.303997931141046, - "heading": 0.26015698140700544, - "angularVelocity": 0.08158571103122086, - "velocityX": -1.0459245042009158, - "velocityY": -0.24686449629458126, - "timestamp": 4.140915302848095 + "heading": 1.0473347586059488e-21, + "angularVelocity": 0.12212584061281963, + "velocityX": 0.358542350838588, + "velocityY": -0.33894514938796605, + "timestamp": 2.508796597442937 + }, + { + "x": 8.169433800706123, + "y": 7.42685355131065, + "heading": 0.014977206112370758, + "angularVelocity": 0.16513162181596408, + "velocityX": -0.1888428783219197, + "velocityY": -0.46782403935650313, + "timestamp": 2.5994951925858016 + }, + { + "x": 8.10265502996488, + "y": 7.372742454671214, + "heading": 0.033468558101012916, + "angularVelocity": 0.2038769394334648, + "velocityX": -0.7362712800132867, + "velocityY": -0.5966034705852111, + "timestamp": 2.690193787728666 + }, + { + "x": 7.986220080079777, + "y": 7.306963959658495, + "heading": 0.05493247364660356, + "angularVelocity": 0.23665102542968372, + "velocityX": -1.2837569281166943, + "velocityY": -0.7252427108612493, + "timestamp": 2.7808923828715306 + }, + { + "x": 7.820121799349139, + "y": 7.229537347680479, + "heading": 0.07855456380147917, + "angularVelocity": 0.26044604238540936, + "velocityX": -1.8313214275150225, + "velocityY": -0.8536693634124876, + "timestamp": 2.871590978014395 + }, + { + "x": 7.6043499812219455, + "y": 7.1404953741126675, + "heading": 0.10296904259791395, + "angularVelocity": 0.2691825464107588, + "velocityX": -2.37899845953864, + "velocityY": -0.9817348706179747, + "timestamp": 2.9622895731572596 + }, + { + "x": 7.338890842614761, + "y": 7.0399060968505065, + "heading": 0.12540191269228773, + "angularVelocity": 0.24733426200304945, + "velocityX": -2.926827457349739, + "velocityY": -1.1090500035167739, + "timestamp": 3.052988168300124 + }, + { + "x": 7.023762260989218, + "y": 6.927996342713902, + "heading": 0.13709315762540872, + "angularVelocity": 0.12890216121545683, + "velocityX": -3.4744593466873996, + "velocityY": -1.233864250712263, + "timestamp": 3.1436867634429886 + }, + { + "x": 6.6949878661960245, + "y": 6.850204202472232, + "heading": 0.1370931594052802, + "angularVelocity": 1.9624024712667312e-8, + "velocityX": -3.6249116568489486, + "velocityY": -0.857699505919964, + "timestamp": 3.234385358585853 + }, + { + "x": 6.366213403460125, + "y": 6.772412349378657, + "heading": 0.13709316118514803, + "angularVelocity": 1.9623984758130592e-8, + "velocityX": -3.624912405953231, + "velocityY": -0.8576963399602832, + "timestamp": 3.3250839537287176 + }, + { + "x": 6.037438940720318, + "y": 6.694620496301594, + "heading": 0.13709316296501595, + "angularVelocity": 1.9623985202632452e-8, + "velocityX": -3.624912405996305, + "velocityY": -0.8576963397782383, + "timestamp": 3.415782548871582 + }, + { + "x": 5.708664477980543, + "y": 6.6168286432244035, + "heading": 0.13709316474488378, + "angularVelocity": 1.9623984644394548e-8, + "velocityX": -3.624912405995973, + "velocityY": -0.8576963397796433, + "timestamp": 3.5064811440144465 + }, + { + "x": 5.379890015210838, + "y": 6.539036790273742, + "heading": 0.13709316652476114, + "angularVelocity": 1.9624089556639278e-8, + "velocityX": -3.6249124063259477, + "velocityY": -0.8576963383845883, + "timestamp": 3.597179739157311 + }, + { + "x": 5.082161809078407, + "y": 6.468591173031877, + "heading": 0.1733415977378681, + "angularVelocity": 0.39965813313877674, + "velocityX": -3.282611000351902, + "velocityY": -0.7767002028080265, + "timestamp": 3.6878783343001755 + }, + { + "x": 4.834054918534678, + "y": 6.409886673220785, + "heading": 0.20354854900447322, + "angularVelocity": 0.3330476201866674, + "velocityX": -2.7355097413903846, + "velocityY": -0.6472481709184398, + "timestamp": 3.77857692944304 + }, + { + "x": 4.6355693784003975, + "y": 6.36292318296314, + "heading": 0.22771447843277892, + "angularVelocity": 0.26644215811987526, + "velocityX": -2.1884080985117165, + "velocityY": -0.5177973284334796, + "timestamp": 3.8692755245859045 + }, + { + "x": 4.486705209976563, + "y": 6.327700620163418, + "heading": 0.24583924264804105, + "angularVelocity": 0.1998351152706694, + "velocityX": -1.6413062207782854, + "velocityY": -0.3883473910951022, + "timestamp": 3.959974119728769 + }, + { + "x": 4.3874624262872, + "y": 6.30421892984517, + "heading": 0.25792252534877924, + "angularVelocity": 0.13322458503028708, + "velocityX": -1.0942041994480602, + "velocityY": -0.25889805990114845, + "timestamp": 4.0506727148716335 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": 0.04079263927507296, - "velocityX": -0.5229622556382164, - "velocityY": -0.12343224899141036, - "timestamp": 4.234244612416448 + "angularVelocity": 0.06661189082061515, + "velocityX": -0.5471021053136723, + "velocityY": -0.1294490312938923, + "timestamp": 4.141371310014498 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": -3.1350638097986163e-24, - "velocityX": 6.734301517773745e-23, - "velocityY": -5.476052050130636e-23, - "timestamp": 4.327573921984801 + "angularVelocity": -4.3850016440748696e-23, + "velocityX": 2.0622476782033507e-21, + "velocityY": 1.8080615452809687e-23, + "timestamp": 4.2320699051573625 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePDEABC.2.traj b/src/main/deploy/choreo/CenterLanePDEABC.2.traj index e6b4b62c..ea16156f 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.2.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.2.traj @@ -4,730 +4,712 @@ "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": -3.1350638097986163e-24, - "velocityX": 6.734301517773745e-23, - "velocityY": -5.476052050130636e-23, + "angularVelocity": -4.3850016440748696e-23, + "velocityX": 2.0622476782033507e-21, + "velocityY": 1.8080615452809687e-23, "timestamp": 0 }, { - "x": 4.357085465154312, - "y": 6.298335605731047, - "heading": 0.24955697801054222, - "angularVelocity": -0.24189458757929325, - "velocityX": 0.32311199814604374, - "velocityY": 0.09834717102755294, - "timestamp": 0.059559630497123095 - }, - { - "x": 4.395672489468794, - "y": 6.30982325311387, - "heading": 0.2214506254853789, - "angularVelocity": -0.4719027349661102, - "velocityX": 0.6478721239942183, - "velocityY": 0.19287640448638915, - "timestamp": 0.11911926099424619 - }, - { - "x": 4.453716686931878, - "y": 6.3266599763174565, - "heading": 0.18050722178606005, - "angularVelocity": -0.6874354887291741, - "velocityX": 0.9745560370104606, - "velocityY": 0.28268683104740583, - "timestamp": 0.17867889149136928 - }, - { - "x": 4.531352614071991, - "y": 6.348490007100106, - "heading": 0.12779504929109745, - "angularVelocity": -0.8850318924914878, - "velocityX": 1.3034991401409508, - "velocityY": 0.366523945841208, - "timestamp": 0.23823852198849238 - }, - { - "x": 4.628738799037524, - "y": 6.374849024990675, - "heading": 0.0646674309135714, - "angularVelocity": -1.0599061453306917, - "velocityX": 1.6351039143911004, - "velocityY": 0.4425651682281852, - "timestamp": 0.2977981524856155 - }, - { - "x": 4.746060953896417, - "y": 6.4051070852453105, - "heading": -0.0071047467810986895, - "angularVelocity": -1.2050473969635673, - "velocityX": 1.9698267749421239, - "velocityY": 0.5080296838996932, - "timestamp": 0.35735778298273857 - }, - { - "x": 4.883529200739737, - "y": 6.4383664128247835, - "heading": -0.08508741675594196, - "angularVelocity": -1.30932091626408, - "velocityX": 2.3080775635429776, - "velocityY": 0.5584206500589359, - "timestamp": 0.41691741347986166 - }, - { - "x": 5.041348217191184, - "y": 6.473264113474332, - "heading": -0.1656919745439482, - "angularVelocity": -1.3533421398895296, - "velocityX": 2.6497648681529427, - "velocityY": 0.5859287634639322, - "timestamp": 0.47647704397698476 - }, - { - "x": 5.219569793721533, - "y": 6.507557049328585, - "heading": -0.2431024824486723, - "angularVelocity": -1.29971437462936, - "velocityX": 2.99232172937938, - "velocityY": 0.575774825465208, - "timestamp": 0.5360366744741079 - }, - { - "x": 5.417370624809519, - "y": 6.5372140981281, - "heading": -0.30621328049041796, - "angularVelocity": -1.0596237336427745, - "velocityX": 3.3210553765530904, - "velocityY": 0.49793876409201393, - "timestamp": 0.595596304971231 - }, - { - "x": 5.630089765221455, - "y": 6.556096742386712, - "heading": -0.3316159113808185, - "angularVelocity": -0.4265075299892522, - "velocityX": 3.5715322381358874, - "velocityY": 0.31703763272213936, - "timestamp": 0.655155935468354 + "x": 4.357336302646973, + "y": 6.297583243093535, + "heading": 0.2475212314024478, + "angularVelocity": -0.28561956492945584, + "velocityX": 0.3386404194139296, + "velocityY": 0.08867859433669868, + "timestamp": 0.057569231532273335 + }, + { + "x": 4.39639255106342, + "y": 6.307690301692112, + "heading": 0.21545182729872558, + "angularVelocity": -0.5570580542792894, + "velocityX": 0.6784222644096215, + "velocityY": 0.17556354895775533, + "timestamp": 0.11513846306454667 + }, + { + "x": 4.455088006420721, + "y": 6.322669942772804, + "heading": 0.16876628337274657, + "angularVelocity": -0.8109461023430008, + "velocityX": 1.0195629469953096, + "velocityY": 0.2602022066647703, + "timestamp": 0.17270769459682 + }, + { + "x": 4.53351670800356, + "y": 6.342355632767599, + "heading": 0.10873910043109954, + "angularVelocity": -1.0426955744232154, + "velocityX": 1.362337128625251, + "velocityY": 0.3419481113580211, + "timestamp": 0.23027692612909334 + }, + { + "x": 4.631792821732683, + "y": 6.3665238013399605, + "heading": 0.03701268299307073, + "angularVelocity": -1.245915839571676, + "velocityX": 1.7070944168158462, + "velocityY": 0.4198105121277204, + "timestamp": 0.2878461576613667 + }, + { + "x": 4.7500563078189675, + "y": 6.394856912095635, + "heading": -0.0442182245974014, + "angularVelocity": -1.4110125396572948, + "velocityX": 2.0542828684448438, + "velocityY": 0.4921571819104624, + "timestamp": 0.34541538919364 + }, + { + "x": 4.888478268982772, + "y": 6.426870152464857, + "heading": -0.13181305030525106, + "angularVelocity": -1.52155627887344, + "velocityX": 2.4044434410455024, + "velocityY": 0.5560824683107718, + "timestamp": 0.40298462072591335 + }, + { + "x": 5.047249482849422, + "y": 6.461748968300146, + "heading": -0.22073086511138704, + "angularVelocity": -1.5445371153910277, + "velocityX": 2.757917895388995, + "velocityY": 0.6058586315458515, + "timestamp": 0.4605538522581867 + }, + { + "x": 5.226429049191539, + "y": 6.497909114202001, + "heading": -0.3013203844421244, + "angularVelocity": -1.39987137548568, + "velocityX": 3.1124189358280567, + "velocityY": 0.6281158344381212, + "timestamp": 0.51812308379046 + }, + { + "x": 5.423903550460318, + "y": 6.531166211616276, + "heading": -0.3435648346153767, + "angularVelocity": -0.7338025721182303, + "velocityX": 3.430209089347909, + "velocityY": 0.5776887502073221, + "timestamp": 0.5756923153227334 + }, + { + "x": 5.636616262089249, + "y": 6.55438565863319, + "heading": -0.34498723844741225, + "angularVelocity": -0.024707709208140263, + "velocityX": 3.69490274522221, + "velocityY": 0.4033308487694046, + "timestamp": 0.6332615468550067 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -0.3344741491569628, - "angularVelocity": -0.04798951491618805, - "velocityX": 3.7098016045994533, - "velocityY": 0.017297899702458933, - "timestamp": 0.7147155659654771 - }, - { - "x": 5.971113356144086, - "y": 6.5517128681904815, - "heading": -0.33447429817496127, - "angularVelocity": -0.000004618408354033321, - "velocityX": 3.7212182481092846, - "velocityY": -0.167796284707936, - "timestamp": 0.7469816600587373 - }, - { - "x": 6.090763237120761, - "y": 6.540318338833946, - "heading": -0.33447432111787584, - "angularVelocity": -7.11053358628561e-7, - "velocityX": 3.70822327086892, - "velocityY": -0.3531425069176937, - "timestamp": 0.7792477541519975 - }, - { - "x": 6.209696100342635, - "y": 6.522971764048271, - "heading": -0.3344743416762489, - "angularVelocity": -6.371509664508709e-7, - "velocityX": 3.686001251905956, - "velocityY": -0.537609998146539, - "timestamp": 0.8115138482452577 - }, - { - "x": 6.327616045254045, - "y": 6.499716303685921, - "heading": -0.3344743603957426, - "angularVelocity": -5.80159891848438e-7, - "velocityX": 3.6546085984432737, - "velocityY": -0.7207398669059114, - "timestamp": 0.8437799423385179 - }, - { - "x": 6.444229692072682, - "y": 6.470609819762047, - "heading": -0.3344743776866033, - "angularVelocity": -5.358832938629608e-7, - "velocityX": 3.6141234350083242, - "velocityY": -0.9020764595722645, - "timestamp": 0.8760460364317781 - }, - { - "x": 6.559246912194348, - "y": 6.435724733351891, - "heading": -0.3344743938681527, - "angularVelocity": -5.015031984210286e-7, - "velocityX": 3.5646465230413518, - "velocityY": -1.081168557598769, - "timestamp": 0.9083121305250383 - }, - { - "x": 6.67238155121755, - "y": 6.395147846497736, - "heading": -0.33447440919657534, - "angularVelocity": -4.750628499499889e-7, - "velocityX": 3.506301032167155, - "velocityY": -1.2575704619491082, - "timestamp": 0.9405782246182985 - }, - { - "x": 6.783352144572337, - "y": 6.348980132760865, - "heading": -0.33447442388388354, - "angularVelocity": -4.551932493526412e-7, - "velocityX": 3.439232310983837, - "velocityY": -1.4308429648605667, - "timestamp": 0.9728443187115587 - }, - { - "x": 6.891882636447486, - "y": 6.297336519628137, - "heading": -0.3344744381114049, - "angularVelocity": -4.409434049866837e-7, - "velocityX": 3.363607989286128, - "velocityY": -1.6005536022878626, - "timestamp": 1.005110412804819 - }, - { - "x": 6.998345027127988, - "y": 6.24155399695869, - "heading": -0.33447445218138366, - "angularVelocity": -4.360607978051808e-7, - "velocityX": 3.299512806625678, - "velocityY": -1.7288278682946643, - "timestamp": 1.0373765068980791 - }, - { - "x": 7.107456437455107, - "y": 6.19114935204841, - "heading": -0.33447450198371365, - "angularVelocity": -0.00000154348803003307, - "velocityX": 3.381611979799848, - "velocityY": -1.5621551454164793, - "timestamp": 1.0696426009913393 + "heading": -0.34498726323171464, + "angularVelocity": -4.305129971148693e-7, + "velocityX": 3.7246965125733036, + "velocityY": 0.047618149403998285, + "timestamp": 0.69083077838728 + }, + { + "x": 5.970747823515829, + "y": 6.552244502208151, + "heading": -0.34498728566710496, + "angularVelocity": -6.97576178834182e-7, + "velocityX": 3.7219058963692446, + "velocityY": -0.15180985637112251, + "timestamp": 0.7229926999510594 + }, + { + "x": 6.090018554613485, + "y": 6.540962017726213, + "heading": -0.3449873062008861, + "angularVelocity": -6.384500714125073e-7, + "velocityX": 3.7084454316926947, + "velocityY": -0.35080256195400317, + "timestamp": 0.7551546215148388 + }, + { + "x": 6.208514441050117, + "y": 6.52331189505608, + "heading": -0.34498732528503373, + "angularVelocity": -5.933770972523468e-7, + "velocityX": 3.68435344267748, + "velocityY": -0.5487894320969752, + "timestamp": 0.7873165430786182 + }, + { + "x": 6.32589577532509, + "y": 6.499344741803893, + "heading": -0.34498734327209396, + "angularVelocity": -5.592657200372931e-7, + "velocityX": 3.6496990405936383, + "velocityY": -0.7452027766642643, + "timestamp": 0.8194784646423976 + }, + { + "x": 6.441826048149789, + "y": 6.4691292818938315, + "heading": -0.3449873604472264, + "angularVelocity": -5.340207168118268e-7, + "velocityX": 3.6045816663908092, + "velocityY": -0.9394793109653794, + "timestamp": 0.851640386206177 + }, + { + "x": 6.555972918049828, + "y": 6.432752169034089, + "heading": -0.344987377050145, + "angularVelocity": -5.162290595234125e-7, + "velocityX": 3.549130908539721, + "velocityY": -1.1310615501503813, + "timestamp": 0.8838023077699564 + }, + { + "x": 6.668009186278295, + "y": 6.390317785626105, + "heading": -0.34498739329087524, + "angularVelocity": -5.049676592749214e-7, + "velocityX": 3.483506668166316, + "velocityY": -1.319398261818203, + "timestamp": 0.9159642293337358 + }, + { + "x": 6.777613987715612, + "y": 6.341948479079241, + "heading": -0.3449874093618285, + "angularVelocity": -4.996888394768407e-7, + "velocityX": 3.4079058746525486, + "velocityY": -1.5039308659137305, + "timestamp": 0.9481261508975152 + }, + { + "x": 6.886436695218618, + "y": 6.291844380961981, + "heading": -0.3449874254078226, + "angularVelocity": -4.989127946142238e-7, + "velocityX": 3.3835884863782986, + "velocityY": -1.55787016698924, + "timestamp": 0.9802880724612946 + }, + { + "x": 6.995259275146381, + "y": 6.241740005760864, + "heading": -0.3449874414538154, + "angularVelocity": -4.98912750035862e-7, + "velocityX": 3.3835845197234478, + "velocityY": -1.5578787822660598, + "timestamp": 1.012449994025074 + }, + { + "x": 7.1040824430518015, + "y": 6.191636907618568, + "heading": -0.344987457499818, + "angularVelocity": -4.989130590435776e-7, + "velocityX": 3.3836028015184674, + "velocityY": -1.5578390750981381, + "timestamp": 1.0446119155888534 }, { "x": 7.214789390563965, "y": 6.145846366882324, "heading": -0.344987478573796, - "angularVelocity": -0.325821171899398, - "velocityX": 3.32649352594794, - "velocityY": -1.404043050117709, - "timestamp": 1.1019086950845995 - }, - { - "x": 7.408281473848404, - "y": 6.071835314929934, - "heading": -0.36045376555379516, - "angularVelocity": -0.24376616005736862, - "velocityX": 3.0496538829742375, - "velocityY": -1.1664978129250685, - "timestamp": 1.1653559224452765 - }, - { - "x": 7.581043516745906, - "y": 6.008144867615469, - "heading": -0.3717285153471776, - "angularVelocity": -0.17770279746487722, - "velocityX": 2.722925021063643, - "velocityY": -1.003833421315721, - "timestamp": 1.2288031498059535 - }, - { - "x": 7.732516109743219, - "y": 5.95357268553042, - "heading": -0.3790773339263405, - "angularVelocity": -0.11582568513809473, - "velocityX": 2.3873792330788404, - "velocityY": -0.8601192574550642, - "timestamp": 1.2922503771666305 - }, - { - "x": 7.862474679773266, - "y": 5.907578864525407, - "heading": -0.3826203402067485, - "angularVelocity": -0.05584178265611413, - "velocityX": 2.048293919784284, - "velocityY": -0.7249145930925164, - "timestamp": 1.3556976045273075 - }, - { - "x": 7.970798961808127, - "y": 5.869857366205246, - "heading": -0.38242588059176913, - "angularVelocity": 0.003064903275819629, - "velocityX": 1.7073130937475338, - "velocityY": -0.5945334396683074, - "timestamp": 1.4191448318879845 - }, - { - "x": 8.057414200976034, - "y": 5.840211247118204, - "heading": -0.378538126264348, - "angularVelocity": 0.061275401450096946, - "velocityX": 1.3651540464570049, - "velocityY": -0.46725633759397367, - "timestamp": 1.4825920592486614 - }, - { - "x": 8.122269484577586, - "y": 5.818503192947792, - "heading": -0.37098804864933244, - "angularVelocity": 0.11899775497037605, - "velocityX": 1.0221925574914426, - "velocityY": -0.3421434643157637, - "timestamp": 1.5460392866093384 - }, - { - "x": 8.165327926639057, - "y": 5.804632020140207, - "heading": -0.3597986070827489, - "angularVelocity": 0.1763582434103087, - "velocityX": 0.6786497038979674, - "velocityY": -0.2186253581851867, - "timestamp": 1.6094865139700154 + "angularVelocity": -6.552462348846214e-7, + "velocityX": 3.4421745383783664, + "velocityY": -1.4237501526591758, + "timestamp": 1.0767738371526328 + }, + { + "x": 7.410548108371569, + "y": 6.0752735070916195, + "heading": -0.3626370424214939, + "angularVelocity": -0.2874228799686078, + "velocityX": 3.1879277548585048, + "velocityY": -1.1492779529116102, + "timestamp": 1.1381800962842146 + }, + { + "x": 7.584442413587634, + "y": 6.012933959955544, + "heading": -0.37637990938116833, + "angularVelocity": -0.22380238031152527, + "velocityX": 2.831866126927586, + "velocityY": -1.0151985810191508, + "timestamp": 1.1995863554157964 + }, + { + "x": 7.736397172033548, + "y": 5.958610527915441, + "heading": -0.3857543231283157, + "angularVelocity": -0.15266218590290268, + "velocityX": 2.47458094003583, + "velocityY": -0.8846562680800549, + "timestamp": 1.2609926145473782 + }, + { + "x": 7.86638729375416, + "y": 5.912228690109332, + "heading": -0.3906020583128712, + "angularVelocity": -0.07894529406469342, + "velocityX": 2.1168871636044173, + "velocityY": -0.7553275262497595, + "timestamp": 1.32239887367896 + }, + { + "x": 7.974400232833448, + "y": 5.873750789689322, + "heading": -0.3908432822952403, + "angularVelocity": -0.003928328899700405, + "velocityX": 1.7589890771205783, + "velocityY": -0.6266120256171138, + "timestamp": 1.3838051328105418 + }, + { + "x": 8.060428468071539, + "y": 5.84315412501972, + "heading": -0.3864300383007793, + "angularVelocity": 0.07186961161409067, + "velocityX": 1.4009685080107037, + "velocityY": -0.498266220777903, + "timestamp": 1.4452113919421237 + }, + { + "x": 8.124466995265989, + "y": 5.820423539830739, + "heading": -0.377330476340927, + "angularVelocity": 0.14818622870925488, + "velocityX": 1.042866445539805, + "velocityY": -0.37016723556265896, + "timestamp": 1.5066176510737055 + }, + { + "x": 8.166512252703873, + "y": 5.805548220947723, + "heading": -0.36352195759750816, + "angularVelocity": 0.2248715186155497, + "velocityX": 0.6847063806278847, + "velocityY": -0.242244342732895, + "timestamp": 1.5680239102052873 }, { "x": 8.186561584472656, "y": 5.798520088195801, "heading": -0.344987478573796, - "angularVelocity": 0.23344012220985313, - "velocityX": 0.3346664419690626, - "velocityY": -0.09633095406457304, - "timestamp": 1.6729337413306924 - }, - { - "x": 8.184449385960491, - "y": 5.800720366343895, - "heading": -0.3251687978756523, - "angularVelocity": 0.29387941781314064, - "velocityX": -0.03132053432391204, - "velocityY": 0.03262661481041375, - "timestamp": 1.7403718792092189 - }, - { - "x": 8.157654760081511, - "y": 5.811617201116665, - "heading": -0.30136325235519174, - "angularVelocity": 0.3529982628426134, - "velocityX": -0.39732155604955366, - "velocityY": 0.1615826758502398, - "timestamp": 1.8078100170877454 - }, - { - "x": 8.106176542546697, - "y": 5.831210437992555, - "heading": -0.27368179509987206, - "angularVelocity": 0.41047185058966307, - "velocityX": -0.7633398423239571, - "velocityY": 0.29053644558191305, - "timestamp": 1.8752481549662718 - }, - { - "x": 8.03001326746699, - "y": 5.859499833662039, - "heading": -0.24226677680218867, - "angularVelocity": 0.46583460466049514, - "velocityX": -1.129379865394508, - "velocityY": 0.4194866074214659, - "timestamp": 1.9426862928447983 - }, - { - "x": 7.929163035221131, - "y": 5.896484993829156, - "heading": -0.2073080079914019, - "angularVelocity": 0.5183827713890392, - "velocityX": -1.495448056817871, - "velocityY": 0.5484309224809322, - "timestamp": 2.010124430723325 - }, - { - "x": 7.80362329670357, - "y": 5.942165255300411, - "heading": -0.16907207724227352, - "angularVelocity": 0.5669778548452968, - "velocityX": -1.8615540474099375, - "velocityY": 0.677365403438884, - "timestamp": 2.0775625686018513 - }, - { - "x": 7.653390486781624, - "y": 5.996539431834676, - "heading": -0.12796178084950977, - "angularVelocity": 0.6096001118360348, - "velocityX": -2.227712903232215, - "velocityY": 0.8062822943333132, - "timestamp": 2.1450007064803778 - }, - { - "x": 7.478459409616276, - "y": 6.059605150575439, - "heading": -0.0846568853073795, - "angularVelocity": 0.6421425161550796, - "velocityX": -2.593948805058106, - "velocityY": 0.9351639995511196, - "timestamp": 2.2124388443589043 - }, - { - "x": 7.278822723764661, - "y": 6.131356463760539, - "heading": -0.04054583544960124, - "angularVelocity": 0.6540964985900647, - "velocityX": -2.9602935687698184, - "velocityY": 1.063957508944601, - "timestamp": 2.2798769822374307 - }, - { - "x": 7.05448611686489, - "y": 6.21176649036125, - "heading": -5.1297661467442016e-8, - "angularVelocity": 0.6012292958766616, - "velocityX": -3.3265539938819106, - "velocityY": 1.1923524155656642, - "timestamp": 2.347315120115957 - }, - { - "x": 6.817570799613061, - "y": 6.29528987249135, - "heading": -4.834198432704978e-8, - "angularVelocity": 4.3827976770594794e-8, - "velocityX": -3.5130762014599934, - "velocityY": 1.238518511299158, - "timestamp": 2.4147532579944837 - }, - { - "x": 6.580655475027505, - "y": 6.378813233819268, - "heading": -4.538633792987658e-8, - "angularVelocity": 4.382752089770155e-8, - "velocityX": -3.5130763102074556, - "velocityY": 1.23851820283599, - "timestamp": 2.48219139587301 - }, - { - "x": 6.3437395023630385, - "y": 6.4623347568384295, - "heading": -4.243069068525663e-8, - "angularVelocity": 4.382753346398488e-8, - "velocityX": -3.5130859201838094, - "velocityY": 1.2384909436497944, - "timestamp": 2.5496295337515367 - }, - { - "x": 6.09986574355872, - "y": 6.5225893218228235, - "heading": -3.943141637637637e-8, - "angularVelocity": 4.447445322827149e-8, - "velocityX": -3.6162587888117317, - "velocityY": 0.8934790740059899, - "timestamp": 2.617067671630063 + "angularVelocity": 0.30183371020853555, + "velocityX": 0.3265030642205698, + "velocityY": -0.11445303542856088, + "timestamp": 1.6294301693368691 + }, + { + "x": 8.182873429152231, + "y": 5.799961021839805, + "heading": -0.3197766551885773, + "angularVelocity": 0.3842915682549792, + "velocityX": -0.056218988582698255, + "velocityY": 0.021964322281146666, + "timestamp": 1.6950335430973134 + }, + { + "x": 8.154073481017848, + "y": 5.810352783086787, + "heading": -0.28949615112813754, + "angularVelocity": 0.46156931152673975, + "velocityX": -0.43900102210518965, + "velocityY": 0.15840284807498262, + "timestamp": 1.7606369168577576 + }, + { + "x": 8.100157139869356, + "y": 5.829696916957608, + "heading": -0.2545716989464078, + "angularVelocity": 0.5323575630311173, + "velocityX": -0.8218531770236625, + "velocityY": 0.29486492480488535, + "timestamp": 1.8262402906182018 + }, + { + "x": 8.021119019409117, + "y": 5.8579951101346595, + "heading": -0.2155530876775793, + "angularVelocity": 0.594765315139245, + "velocityX": -1.204787435915305, + "velocityY": 0.43135271183437995, + "timestamp": 1.891843664378646 + }, + { + "x": 7.9169528918289975, + "y": 5.89524913213786, + "heading": -0.17317932530550872, + "angularVelocity": 0.6459082809795952, + "velocityX": -1.5878166260242859, + "velocityY": 0.567867471865644, + "timestamp": 1.9574470381390903 + }, + { + "x": 7.787651961744212, + "y": 5.941460642017215, + "heading": -0.12849929599301224, + "angularVelocity": 0.6810629812370456, + "velocityX": -1.9709493989888005, + "velocityY": 0.7044075209927304, + "timestamp": 2.0230504118995345 + }, + { + "x": 7.633210550125228, + "y": 5.99663052819492, + "heading": -0.08312171419135361, + "angularVelocity": 0.6916958564868655, + "velocityX": -2.3541687380734118, + "velocityY": 0.8409611124446534, + "timestamp": 2.0886537856599787 + }, + { + "x": 7.453632499858265, + "y": 6.060756206322318, + "heading": -0.039828348341138756, + "angularVelocity": 0.659925905766733, + "velocityX": -2.7373294995880113, + "velocityY": 0.9774753103637095, + "timestamp": 2.154257159420423 + }, + { + "x": 7.248984054172477, + "y": 6.133814732067644, + "heading": -0.004587896529415136, + "angularVelocity": 0.5371743828359606, + "velocityX": -3.119480507711035, + "velocityY": 1.1136397651149037, + "timestamp": 2.219860533180867 + }, + { + "x": 7.020152924958556, + "y": 6.215343825965371, + "heading": -7.99601094738583e-8, + "angularVelocity": 0.06993263160608797, + "velocityX": -3.488100018293493, + "velocityY": 1.2427576391945405, + "timestamp": 2.2854639069413114 + }, + { + "x": 6.789969979123711, + "y": 6.297403529442868, + "heading": -7.492709232609331e-8, + "angularVelocity": 7.671887677824965e-8, + "velocityX": -3.508705919231764, + "velocityY": 1.2508457838943434, + "timestamp": 2.3510672807017556 + }, + { + "x": 6.559787033588477, + "y": 6.379463233760796, + "heading": -6.989407741573959e-8, + "angularVelocity": 7.671884267312441e-8, + "velocityX": -3.5087059146647626, + "velocityY": 1.2508457967051358, + "timestamp": 2.4166706544622 + }, + { + "x": 6.3296038306461995, + "y": 6.461522216031207, + "heading": -6.486106191272836e-8, + "angularVelocity": 7.671885170707308e-8, + "velocityX": -3.508709838350823, + "velocityY": 1.250834790449275, + "timestamp": 2.482274028222644 + }, + { + "x": 6.0929382858404795, + "y": 6.522410665825721, + "heading": -5.975605523435043e-8, + "angularVelocity": 7.78162217238893e-8, + "velocityX": -3.607520943510018, + "velocityY": 0.9281298552853697, + "timestamp": 2.5478774019830883 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -3.6277001373267394e-8, - "angularVelocity": 4.677494222617604e-8, - "velocityX": -3.689626869559746, - "velocityY": 0.5121386527717335, - "timestamp": 2.6845058095085896 - }, - { - "x": 5.675146419069591, - "y": 6.5685334564190505, - "heading": -3.325736766733182e-8, - "angularVelocity": 6.38129937324226e-8, - "velocityX": -3.7171934353117426, - "velocityY": 0.24104917118070399, - "timestamp": 2.7318258540530804 - }, - { - "x": 5.498885444409021, - "y": 6.56705094919282, - "heading": -3.0392937858444436e-8, - "angularVelocity": 6.053311733876744e-8, - "velocityX": -3.724869161837878, - "velocityY": -0.03132937089348242, - "timestamp": 2.779145898597571 - }, - { - "x": 5.3232044198709785, - "y": 6.552687568221491, - "heading": -2.7611010604394772e-8, - "angularVelocity": 5.878961613051839e-8, - "velocityX": -3.7126132536258285, - "velocityY": -0.3035369283692009, - "timestamp": 2.826465943142062 - }, - { - "x": 5.148803896994517, - "y": 6.527102675424277, - "heading": -2.484990875079416e-8, - "angularVelocity": 5.8349519324830205e-8, - "velocityX": -3.6855528044249266, - "velocityY": -0.5406776989222635, - "timestamp": 2.873785987686553 - }, - { - "x": 4.974403597324897, - "y": 6.501516261174204, - "heading": -2.208880782591367e-8, - "angularVelocity": 5.834949969847243e-8, - "velocityX": -3.6855480874631454, - "velocityY": -0.5407098513192842, - "timestamp": 2.9211060322310436 - }, - { - "x": 4.800003297129097, - "y": 6.475929850510635, - "heading": -1.9327706889009288e-8, - "angularVelocity": 5.8349499952569724e-8, - "velocityX": -3.6855480985827302, - "velocityY": -0.5407097755267535, - "timestamp": 2.9684260767755344 - }, - { - "x": 4.625602997244748, - "y": 6.450343437724181, - "heading": -1.656660593755509e-8, - "angularVelocity": 5.8349500260046496e-8, - "velocityX": -3.685548092000944, - "velocityY": -0.5407098203890633, - "timestamp": 3.015746121320025 - }, - { - "x": 4.451202697255753, - "y": 6.4247570256510045, - "heading": -1.3805504990627123e-8, - "angularVelocity": 5.834950016439505e-8, - "velocityX": -3.685548094212386, - "velocityY": -0.5407098053155895, - "timestamp": 3.063066165864516 - }, - { - "x": 4.276802397278509, - "y": 6.3991706134977395, - "heading": -1.1044403987822918e-8, - "angularVelocity": 5.8349501345210454e-8, - "velocityX": -3.685548093964082, - "velocityY": -0.5407098070080617, - "timestamp": 3.1103862104090068 - }, - { - "x": 4.102402097300047, - "y": 6.373584201352764, - "heading": -8.283303038922857e-9, - "angularVelocity": 5.834950020607066e-8, - "velocityX": -3.685548093989784, - "velocityY": -0.5407098068328745, - "timestamp": 3.1577062549534975 - }, - { - "x": 3.9280017973350696, - "y": 6.347997789115884, - "heading": -5.522202092459704e-9, - "angularVelocity": 5.8349500154572286e-8, - "velocityX": -3.685548093704841, - "velocityY": -0.5407098087750815, - "timestamp": 3.2050262994979883 - }, - { - "x": 3.7536014973495653, - "y": 6.322411377018916, - "heading": -2.7611011159009457e-9, - "angularVelocity": 5.834950079057347e-8, - "velocityX": -3.6855480941386265, - "velocityY": -0.5407098058183458, - "timestamp": 3.252346344042479 - }, - { - "x": 3.579201219971439, - "y": 6.296824810827607, - "heading": -5.947116132480024e-26, - "angularVelocity": 5.8349503735249215e-8, - "velocityX": -3.6855476163838365, - "velocityY": -0.5407130622468497, - "timestamp": 3.29966638858697 + "heading": -5.43646913463662e-8, + "angularVelocity": 8.218119860224255e-8, + "velocityX": -3.6872205492775545, + "velocityY": 0.5291851788357008, + "timestamp": 2.6134807757435325 + }, + { + "x": 5.661580826065391, + "y": 6.5680717657534915, + "heading": -4.9482596020305806e-8, + "angularVelocity": 9.582614101786845e-8, + "velocityX": -3.718801184433256, + "velocityY": 0.21482472211899678, + "timestamp": 2.6644282027629513 + }, + { + "x": 5.471871496557958, + "y": 6.562921997343888, + "heading": -4.486806959492394e-8, + "angularVelocity": 9.057427814015043e-8, + "velocityX": -3.7236292508966913, + "velocityY": -0.10108004880484801, + "timestamp": 2.71537562978237 + }, + { + "x": 5.28328089006521, + "y": 6.541715087274231, + "heading": -4.036796232249368e-8, + "angularVelocity": 8.83284502417564e-8, + "velocityX": -3.7016708698726792, + "velocityY": -0.41625085525070266, + "timestamp": 2.766323056801789 + }, + { + "x": 5.095501405649377, + "y": 6.514237548522189, + "heading": -3.588263316902823e-8, + "angularVelocity": 8.803838419074343e-8, + "velocityX": -3.685750103616821, + "velocityY": -0.5393312353451875, + "timestamp": 2.8172704838212077 + }, + { + "x": 4.907721944965666, + "y": 6.486759847587045, + "heading": -3.139730406205809e-8, + "angularVelocity": 8.803838327812946e-8, + "velocityX": -3.685749637800889, + "velocityY": -0.5393344186875336, + "timestamp": 2.8682179108406265 + }, + { + "x": 4.719942484237288, + "y": 6.459282146957154, + "heading": -2.6911974883170177e-8, + "angularVelocity": 8.803838468973724e-8, + "velocityX": -3.6857496386776267, + "velocityY": -0.5393344126960062, + "timestamp": 2.9191653378600453 + }, + { + "x": 4.532163023549714, + "y": 6.431804446048415, + "heading": -2.2426645714855718e-8, + "angularVelocity": 8.803838448220064e-8, + "velocityX": -3.685749637876727, + "velocityY": -0.5393344181692645, + "timestamp": 2.970112764879464 + }, + { + "x": 4.34438356286167, + "y": 6.404326745142886, + "heading": -1.794131654726717e-8, + "angularVelocity": 8.803838446795243e-8, + "velocityX": -3.6857496378859507, + "velocityY": -0.5393344181062291, + "timestamp": 3.021060191898883 + }, + { + "x": 4.156604102201168, + "y": 6.376849044049133, + "heading": -1.3455987422255126e-8, + "angularVelocity": 8.803838363225754e-8, + "velocityX": -3.685749637345336, + "velocityY": -0.5393344218007294, + "timestamp": 3.0720076189183017 + }, + { + "x": 3.9688246409539834, + "y": 6.349371346964709, + "heading": -8.97065828327331e-9, + "angularVelocity": 8.80383839064573e-8, + "velocityX": -3.685749648860808, + "velocityY": -0.5393343431053033, + "timestamp": 3.1229550459377204 + }, + { + "x": 3.7810451818268453, + "y": 6.32189363539213, + "heading": -4.485329161078249e-9, + "angularVelocity": 8.80383835769656e-8, + "velocityX": -3.685749607248367, + "velocityY": -0.5393346274799247, + "timestamp": 3.1739024729571392 + }, + { + "x": 3.5932657187551267, + "y": 6.294415950776365, + "heading": 7.629972110817107e-24, + "angularVelocity": 8.803838434016792e-8, + "velocityX": -3.6857496846728943, + "velocityY": -0.5393340983695197, + "timestamp": 3.224849899976558 }, { "x": 3.405486822128296, "y": 6.266934394836426, - "heading": -3.242812966980484e-26, - "angularVelocity": 4.141833469401844e-27, - "velocityX": -3.6710531343607253, - "velocityY": -0.6316650011408507, - "timestamp": 3.3469864331314607 - }, - { - "x": 3.234879221757396, - "y": 6.227705290951873, - "heading": -3.799073171453003e-26, - "angularVelocity": 3.7193578167117204e-28, - "velocityX": -3.4320264149158155, - "velocityY": -0.7891519514521484, - "timestamp": 3.396696889796196 - }, - { - "x": 3.075654096212167, - "y": 6.19701582942553, - "heading": -4.3640191392241984e-26, - "angularVelocity": -1.3753350907836377e-27, - "velocityX": -3.2030509520179686, - "velocityY": -0.6173643049252793, - "timestamp": 3.4464073464609317 - }, - { - "x": 2.9276929132783946, - "y": 6.175021768989464, - "heading": -4.923363376741485e-26, - "angularVelocity": -2.4846347668894466e-28, - "velocityX": -2.9764599414500257, - "velocityY": -0.4424433391228337, - "timestamp": 3.4961178031256672 - }, - { - "x": 2.790955431257819, - "y": 6.16177499390257, - "heading": -5.486689770041805e-26, - "angularVelocity": -1.0495335231555751e-27, - "velocityX": -2.7506784526800994, - "velocityY": -0.2664786440453503, - "timestamp": 3.5458282597904027 - }, - { - "x": 2.6654213921010017, - "y": 6.157301437220893, - "heading": -6.044835484657667e-26, - "angularVelocity": -7.36271402334531e-30, - "velocityX": -2.525304484798085, - "velocityY": -0.08999226685541546, - "timestamp": 3.5955387164551382 - }, - { - "x": 2.551078597067916, - "y": 6.16161665554964, - "heading": -6.59367177391361e-26, - "angularVelocity": 1.865367086612129e-27, - "velocityX": -2.300175912771294, - "velocityY": 0.08680705465754052, - "timestamp": 3.6452491731198737 - }, - { - "x": 2.447918895369922, - "y": 6.174731018536324, - "heading": -7.161083135132275e-26, - "angularVelocity": -1.871285764514843e-27, - "velocityX": -2.075211306018359, - "velocityY": 0.2638149771009456, - "timestamp": 3.6949596297846092 - }, - { - "x": 2.355936456054869, - "y": 6.196651932350337, - "heading": -7.721743669966212e-26, - "angularVelocity": -5.132563200735982e-28, - "velocityX": -1.8503639975672366, - "velocityY": 0.44097188569106954, - "timestamp": 3.7446700864493447 - }, - { - "x": 2.2751269010087887, - "y": 6.227384951214515, - "heading": -8.282082994441975e-26, - "angularVelocity": -4.486400643129525e-28, - "velocityX": -1.6256047614104199, - "velocityY": 0.618240525760057, - "timestamp": 3.7943805431140802 + "heading": 3.646452356482733e-24, + "angularVelocity": -1.9320923857305114e-25, + "velocityX": -3.6857385664492472, + "velocityY": -0.5394100850169479, + "timestamp": 3.275797326995977 + }, + { + "x": 3.2165781142286494, + "y": 6.223116050022905, + "heading": 3.318409186414545e-24, + "angularVelocity": -1.5338201174379227e-26, + "velocityX": -3.536673395714516, + "velocityY": -0.8203495543918979, + "timestamp": 3.329211563133403 + }, + { + "x": 3.041563345872727, + "y": 6.190252158258573, + "heading": 2.990185107220459e-24, + "angularVelocity": -1.87251095899189e-26, + "velocityX": -3.2765566075987578, + "velocityY": -0.6152646586535303, + "timestamp": 3.382625799270829 + }, + { + "x": 2.8804424282697485, + "y": 6.168342842771101, + "heading": 2.6619349218872592e-24, + "angularVelocity": -1.9213858301671557e-26, + "velocityX": -3.0164414817885197, + "velocityY": -0.4101774558958965, + "timestamp": 3.436040035408255 + }, + { + "x": 2.733215332226118, + "y": 6.157388144124356, + "heading": 2.333764868738876e-24, + "angularVelocity": -1.7713655535187625e-26, + "velocityX": -2.7563269025291364, + "velocityY": -0.20508949371775206, + "timestamp": 3.4894542715456813 + }, + { + "x": 2.5998820432630403, + "y": 6.157388082450519, + "heading": 2.0056312293962657e-24, + "angularVelocity": -1.7031930803836163e-26, + "velocityX": -2.4962125943359705, + "velocityY": -0.0000011546329394271484, + "timestamp": 3.5428685076831075 + }, + { + "x": 2.4804425527424723, + "y": 6.16834266976643, + "heading": 1.6774969061310235e-24, + "angularVelocity": -1.7044734930100497e-26, + "velocityX": -2.2360984478608033, + "velocityY": 0.20508740942632245, + "timestamp": 3.5962827438205336 + }, + { + "x": 2.3748968549301686, + "y": 6.190251914052291, + "heading": 1.3492831778827425e-24, + "angularVelocity": -1.853132331504253e-26, + "velocityX": -1.97598440873988, + "velocityY": 0.4101761228877669, + "timestamp": 3.6496969799579597 + }, + { + "x": 2.283244945743567, + "y": 6.22311582099134, + "heading": 1.0211293460600033e-24, + "angularVelocity": -1.7409966353369085e-26, + "velocityX": -1.7158704460510679, + "velocityY": 0.6152649427485215, + "timestamp": 3.703111216095386 }, { "x": 2.205486822128296, "y": 6.266934394836426, - "heading": -8.869624024083721e-26, - "angularVelocity": -5.920668887028623e-27, - "velocityX": -1.400914084337811, - "velocityY": 0.7955960631914043, - "timestamp": 3.8440909997788157 - }, - { - "x": 2.1363371710210255, - "y": 6.327839189643039, - "heading": 0.025262557050058088, - "angularVelocity": 0.415801673405611, - "velocityX": -1.1381484696439823, - "velocityY": 1.002444667372145, - "timestamp": 3.9048472657244186 - }, - { - "x": 2.0845111064453854, - "y": 6.399393278580984, - "heading": 0.0757412033730169, - "angularVelocity": 0.8308385240158422, - "velocityX": -0.8530159608893926, - "velocityY": 1.177723611289897, - "timestamp": 3.9656035316700216 - }, - { - "x": 2.051957537582464, - "y": 6.477853810368175, - "heading": 0.14980799257498115, - "angularVelocity": 1.2190806668118568, - "velocityX": -0.5358059511436571, - "velocityY": 1.291398188582541, - "timestamp": 4.0263597976156245 - }, - { - "x": 2.0403269766878736, - "y": 6.554154175481095, - "heading": 0.23625288977037628, - "angularVelocity": 1.422814517152715, - "velocityX": -0.19142981737889736, - "velocityY": 1.255843556633874, - "timestamp": 4.087116063561227 - }, - { - "x": 2.0424863781029257, - "y": 6.6144252836562565, - "heading": 0.31360709056854197, - "angularVelocity": 1.2731888570542382, - "velocityX": 0.03554203638823312, - "velocityY": 0.9920146874912401, - "timestamp": 4.14787232950683 - }, - { - "x": 2.048130429243297, - "y": 6.654505373712402, - "heading": 0.3666123519116142, - "angularVelocity": 0.8724246053983875, - "velocityX": 0.09289660996323934, - "velocityY": 0.659686526687304, - "timestamp": 4.208628595452433 + "heading": 6.9419459427461045e-25, + "angularVelocity": 5.413163312168308e-27, + "velocityX": -1.455756540544968, + "velocityY": 0.8203538422294017, + "timestamp": 3.756525452232812 + }, + { + "x": 2.136159340261779, + "y": 6.327632048563378, + "heading": 0.025231456124497797, + "angularVelocity": 0.43032288322850026, + "velocityX": -1.1823812996193095, + "velocityY": 1.035199444221815, + "timestamp": 3.8151592295843644 + }, + { + "x": 2.084207176746768, + "y": 6.399028713640929, + "heading": 0.07562693390361644, + "angularVelocity": 0.8594956705068004, + "velocityX": -0.8860449703507888, + "velocityY": 1.2176712520067714, + "timestamp": 3.8737930069359168 + }, + { + "x": 2.051566754965305, + "y": 6.477423275068575, + "heading": 0.14957404640599054, + "angularVelocity": 1.2611691731714223, + "velocityX": -0.5566829096778134, + "velocityY": 1.3370204849265277, + "timestamp": 3.932426784287469 + }, + { + "x": 2.039926841667985, + "y": 6.553838491753715, + "heading": 0.2360367067446735, + "angularVelocity": 1.4746220394479557, + "velocityX": -0.19851890536627906, + "velocityY": 1.3032627290405507, + "timestamp": 3.9910605616390216 + }, + { + "x": 2.042236938900771, + "y": 6.614274854513032, + "heading": 0.313525546116211, + "angularVelocity": 1.3215733809359698, + "velocityX": 0.039398744838411756, + "velocityY": 1.0307431226365935, + "timestamp": 4.049694338990574 + }, + { + "x": 2.0480382687015357, + "y": 6.654459061696839, + "heading": 0.3665932060292166, + "angularVelocity": 0.9050697790597129, + "velocityX": 0.09894177149770778, + "velocityY": 0.6853422890166648, + "timestamp": 4.108328116342126 }, { "x": 2.052253484725952, "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": 0.4352739504266956, - "velocityX": 0.0678622265289755, - "velocityY": 0.3266075878021015, - "timestamp": 4.269384861398036 + "angularVelocity": 0.4513569988001917, + "velocityX": 0.07189057595834777, + "velocityY": 0.3392203330286158, + "timestamp": 4.166961893693679 }, { "x": 2.052253484725952, "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": -3.1389023173080445e-26, - "velocityX": -4.435068013070024e-22, - "velocityY": -1.8479842669198598e-22, - "timestamp": 4.330141127343639 + "angularVelocity": 7.733121458673518e-26, + "velocityX": -6.859499621919341e-22, + "velocityY": -2.8560302702061698e-22, + "timestamp": 4.225595671045231 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePDEABC.3.traj b/src/main/deploy/choreo/CenterLanePDEABC.3.traj index fda79d64..312e12a6 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.3.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.3.traj @@ -4,82 +4,82 @@ "x": 2.052253484725952, "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": -3.1389023173080445e-26, - "velocityX": -4.435068013070024e-22, - "velocityY": -1.8479842669198598e-22, + "angularVelocity": 7.733121458673518e-26, + "velocityX": -6.859499621919341e-22, + "velocityY": -2.8560302702061698e-22, "timestamp": 0 }, { - "x": 2.103013204328941, - "y": 6.695498705652326, + "x": 2.103013204396169, + "y": 6.695498705680337, "heading": 0.3930579718029317, - "angularVelocity": 3.416438400428702e-17, - "velocityX": 0.5194333949867557, - "velocityY": 0.21643049229415012, - "timestamp": 0.09772132499159625 + "angularVelocity": 4.836322083094603e-17, + "velocityX": 0.5390415085998568, + "velocityY": 0.22460053627515192, + "timestamp": 0.09416662513071294 }, { - "x": 2.2045326425850984, - "y": 6.737798454207702, + "x": 2.204532642750927, + "y": 6.737798454276797, "heading": 0.3930579718029317, - "angularVelocity": 5.790454228147962e-17, - "velocityX": 1.0388667802538214, - "velocityY": 0.432860980538431, - "timestamp": 0.1954426499831925 + "angularVelocity": 1.8057826879047683e-17, + "velocityX": 1.078083006732355, + "velocityY": 0.4492010681889061, + "timestamp": 0.1883332502614259 }, { - "x": 2.356811797278175, - "y": 6.801248075919452, - "heading": 0.3930579718029318, - "angularVelocity": 1.3697605676063785e-16, - "velocityX": 1.558300142841611, - "velocityY": 0.6492914593330171, - "timestamp": 0.29316397497478874 + "x": 2.356811797490316, + "y": 6.801248076007844, + "heading": 0.3930579718029317, + "angularVelocity": 9.246174062812876e-17, + "velocityX": 1.6171244804410174, + "velocityY": 0.6738015899260662, + "timestamp": 0.28249987539213883 }, { - "x": 2.5598506573269275, - "y": 6.8858475661703915, - "heading": 0.3930579718029318, - "angularVelocity": 1.6886690536158693e-16, - "velocityX": 2.0777333920330334, - "velocityY": 0.8657218908791364, - "timestamp": 0.390885299966385 + "x": 2.5598506571147865, + "y": 6.885847566082, + "heading": 0.3930579718029317, + "angularVelocity": 1.8302909821567636e-16, + "velocityX": 2.1561658320305157, + "velocityY": 0.8984020607802621, + "timestamp": 0.3766665005228518 }, { - "x": 2.712129812020004, - "y": 6.9492971878821415, - "heading": 0.3930579718029318, - "angularVelocity": 9.579438090360999e-17, - "velocityX": 1.5583001428416108, - "velocityY": 0.649291459333017, - "timestamp": 0.48860662495798124 + "x": 2.7121298118541755, + "y": 6.9492971878130465, + "heading": 0.3930579718029317, + "angularVelocity": 1.1024323128215209e-16, + "velocityX": 1.6171244804410174, + "velocityY": 0.6738015899260661, + "timestamp": 0.4708331256535647 }, { - "x": 2.8136492502761614, - "y": 6.991596936437518, + "x": 2.8136492502089334, + "y": 6.991596936409507, "heading": 0.3930579718029317, - "angularVelocity": -7.496030828989607e-17, - "velocityX": 1.0388667802538214, - "velocityY": 0.4328609805384309, - "timestamp": 0.5863279499495775 + "angularVelocity": -3.9462401223913846e-17, + "velocityX": 1.078083006732355, + "velocityY": 0.44920106818890604, + "timestamp": 0.5649997507842777 }, { "x": 2.8644089698791504, "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 4.893193347075666e-17, - "velocityX": 0.5194333949867557, - "velocityY": 0.21643049229415012, - "timestamp": 0.6840492749411737 + "angularVelocity": 1.2061194980042807e-16, + "velocityX": 0.5390415085998568, + "velocityY": 0.22460053627515186, + "timestamp": 0.6591663759149906 }, { "x": 2.8644089698791504, "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 3.090778525044755e-27, - "velocityX": 4.437436909321685e-22, - "velocityY": 1.850515832937087e-22, - "timestamp": 0.78177059993277 + "angularVelocity": -7.454320302880939e-28, + "velocityX": 6.858010204674672e-22, + "velocityY": 2.856861212118512e-22, + "timestamp": 0.7533330010457036 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePDEABC.4.traj b/src/main/deploy/choreo/CenterLanePDEABC.4.traj index 6fcd62ac..5ae8c7b4 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.4.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.4.traj @@ -4,253 +4,244 @@ "x": 2.8644089698791504, "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 3.090778525044755e-27, - "velocityX": 4.437436909321685e-22, - "velocityY": 1.850515832937087e-22, + "angularVelocity": -7.454320302880939e-28, + "velocityX": 6.858010204674672e-22, + "velocityY": 2.856861212118512e-22, "timestamp": 0 }, { - "x": 2.8419574642644285, - "y": 7.0048102452086525, - "heading": 0.3900130033247021, - "angularVelocity": -0.04733865202910357, - "velocityX": -0.3490426976251426, - "velocityY": -0.12338594795790273, - "timestamp": 0.06432309218178744 - }, - { - "x": 2.7972074454620057, - "y": 6.988518822273239, - "heading": 0.3838409042111465, - "angularVelocity": -0.0959546393713819, - "velocityX": -0.6957068959923765, - "velocityY": -0.25327487194445947, - "timestamp": 0.1286461843635749 - }, - { - "x": 2.7303816098102875, - "y": 6.963303209303571, - "heading": 0.37443012101631223, - "angularVelocity": -0.14630489417762893, - "velocityX": -1.0389089421083304, - "velocityY": -0.39201493762775413, - "timestamp": 0.19296927654536233 - }, - { - "x": 2.6418300370011316, - "y": 6.928345468623928, - "heading": 0.3616207312094327, - "angularVelocity": -0.19914138721251212, - "velocityX": -1.3766684685943704, - "velocityY": -0.5434710847054101, - "timestamp": 0.2572923687271498 - }, - { - "x": 2.5321688819092247, - "y": 6.882378878870811, - "heading": 0.34516587333325927, - "angularVelocity": -0.25581571591224617, - "velocityX": -1.7048489332880077, - "velocityY": -0.7146203360872027, - "timestamp": 0.3216154609089372 - }, - { - "x": 2.4026942269657687, - "y": 6.823220010782376, - "heading": 0.324643183634766, - "angularVelocity": -0.3190563295758964, - "velocityX": -2.012879831360391, - "velocityY": -0.9197143060417909, - "timestamp": 0.38593855309072467 - }, - { - "x": 2.2570878780964114, - "y": 6.7465695928484575, - "heading": 0.2992393598461131, - "angularVelocity": -0.3949409601898093, - "velocityX": -2.2636714736575443, - "velocityY": -1.1916469705357338, - "timestamp": 0.4502616452725121 - }, - { - "x": 2.110110870224978, - "y": 6.646173604450097, - "heading": 0.26804931760848005, - "angularVelocity": -0.4848964995259366, - "velocityX": -2.2849804461522467, - "velocityY": -1.5608078684187743, - "timestamp": 0.5145847374542996 - }, - { - "x": 1.9823765062843866, - "y": 6.531738038725912, - "heading": 0.2347990669991598, - "angularVelocity": -0.5169255625234794, - "velocityX": -1.9858243689465778, - "velocityY": -1.779074385926154, - "timestamp": 0.578907829636087 - }, - { - "x": 1.8778139110395455, - "y": 6.411761042169391, - "heading": 0.201692518308331, - "angularVelocity": -0.5146914983077023, - "velocityX": -1.6255840896039286, - "velocityY": -1.865224330594162, - "timestamp": 0.6432309218178744 - }, - { - "x": 1.796996456842112, - "y": 6.289847592193697, - "heading": 0.16955186256906168, - "angularVelocity": -0.49967522780830353, - "velocityX": -1.2564298676598118, - "velocityY": -1.8953294352072878, - "timestamp": 0.7075540139996619 + "x": 2.842186915849872, + "y": 7.004763611912437, + "heading": 0.3899906597418949, + "angularVelocity": -0.04969534926930016, + "velocityX": -0.36003273044640205, + "velocityY": -0.1293405609631654, + "timestamp": 0.061722316195323756 + }, + { + "x": 2.7978951293903465, + "y": 6.988387145564426, + "heading": 0.38377625605112037, + "angularVelocity": -0.10068325483944376, + "velocityX": -0.7175976079601634, + "velocityY": -0.26532488340499644, + "timestamp": 0.12344463239064751 + }, + { + "x": 2.7317542660001495, + "y": 6.96306112048486, + "heading": 0.37430680047739806, + "angularVelocity": -0.15342028876161432, + "velocityX": -1.071587514326101, + "velocityY": -0.41032201383078476, + "timestamp": 0.18516694858597127 + }, + { + "x": 2.644108850979856, + "y": 6.927989989672691, + "heading": 0.36142823816874126, + "angularVelocity": -0.20865325707968818, + "velocityX": -1.4199955611343862, + "velocityY": -0.5682082749646841, + "timestamp": 0.24688926478129503 + }, + { + "x": 2.53555910261038, + "y": 6.881949867747595, + "heading": 0.3449043845685677, + "angularVelocity": -0.267712792045632, + "velocityX": -1.7586791141467173, + "velocityY": -0.7459234319625763, + "timestamp": 0.3086115809766188 + }, + { + "x": 2.407346628684672, + "y": 6.822852176062135, + "heading": 0.3243353524397527, + "angularVelocity": -0.33325113827101066, + "velocityX": -2.0772466399344327, + "velocityY": -0.9574768953653868, + "timestamp": 0.37033389717194254 + }, + { + "x": 2.2628964345938334, + "y": 6.74663530878728, + "heading": 0.298961324703692, + "angularVelocity": -0.41109973345399, + "velocityX": -2.3403236138079526, + "velocityY": -1.2348348534695668, + "timestamp": 0.4320562133672663 + }, + { + "x": 2.1156790935491716, + "y": 6.646996400731561, + "heading": 0.26784048264776694, + "angularVelocity": -0.5042072944482706, + "velocityX": -2.385155809428528, + "velocityY": -1.6143092838642825, + "timestamp": 0.49377852956259005 + }, + { + "x": 1.9869123530220612, + "y": 6.532612256734745, + "heading": 0.23448482199122617, + "angularVelocity": -0.540414921419745, + "velocityX": -2.0862266432066443, + "velocityY": -1.8532056320576245, + "timestamp": 0.5555008457579138 + }, + { + "x": 1.881043758757729, + "y": 6.412413108766105, + "heading": 0.20123842984354556, + "angularVelocity": -0.5386445972388703, + "velocityX": -1.7152401398758959, + "velocityY": -1.9474179742098816, + "timestamp": 0.6172231619532376 + }, + { + "x": 1.7987115747047, + "y": 6.290184489368743, + "heading": 0.16896416101959177, + "angularVelocity": -0.5228946483767686, + "velocityX": -1.3339127422322181, + "velocityY": -1.9802986493663424, + "timestamp": 0.6789454781485613 }, { "x": 1.740001559257507, "y": 6.167843341827393, - "heading": 0.1387804363394676, - "angularVelocity": -0.4783884789404854, - "velocityX": -0.8860721033672938, - "velocityY": -1.8967410649584446, - "timestamp": 0.7718771061814493 - }, - { - "x": 1.706182040858774, - "y": 6.03669580225492, - "heading": 0.10729245974250749, - "angularVelocity": -0.45130411484825544, - "velocityX": -0.48472113692462804, - "velocityY": -1.8796833159166881, - "timestamp": 0.8416481886089766 - }, - { - "x": 1.699651780790286, - "y": 5.911922041931935, - "heading": 0.07852355852866735, - "angularVelocity": -0.4123327346071031, - "velocityX": -0.09359551036449741, - "velocityY": -1.7883305802599465, - "timestamp": 0.9114192710365039 - }, - { - "x": 1.7185433856985064, - "y": 5.798927064131694, - "heading": 0.053308691164350094, - "angularVelocity": -0.36139424080898114, - "velocityX": 0.2707655414095584, - "velocityY": -1.6195101733961197, - "timestamp": 0.9811903534640312 - }, - { - "x": 1.7598996151781712, - "y": 5.702665752208848, - "heading": 0.032386709878439925, - "angularVelocity": -0.29986608431425904, - "velocityX": 0.592741692414218, - "velocityY": -1.3796734775160497, - "timestamp": 1.0509614358915584 - }, - { - "x": 1.820114668725021, - "y": 5.6271104583982785, - "heading": 0.016317787108466864, - "angularVelocity": -0.2303092084985792, - "velocityX": 0.8630373996189019, - "velocityY": -1.082902703839363, - "timestamp": 1.1207325183190857 - }, - { - "x": 1.8954858630630067, - "y": 5.575103001600796, - "heading": 0.005460341828234747, - "angularVelocity": -0.15561526211822638, - "velocityX": 1.0802640824194598, - "velocityY": -0.7454013179672614, - "timestamp": 1.190503600746613 - }, - { - "x": 1.9826012990017379, + "heading": 0.13807920449538028, + "angularVelocity": -0.5003855724803669, + "velocityX": -0.9511959217700356, + "velocityY": -1.9821217848370036, + "timestamp": 0.7406677943438851 + }, + { + "x": 1.7034889997175517, + "y": 6.016151230649133, + "heading": 0.1019414560444192, + "angularVelocity": -0.467252098060329, + "velocityX": -0.4720983122052568, + "velocityY": -1.961341263510758, + "timestamp": 0.8180088027652008 + }, + { + "x": 1.7027290435050462, + "y": 5.874298147554192, + "heading": 0.06966485107958241, + "angularVelocity": -0.4173284732597485, + "velocityX": -0.009826044785523666, + "velocityY": -1.8341250778913685, + "timestamp": 0.8953498111865166 + }, + { + "x": 1.7342944275877592, + "y": 5.750595898188055, + "heading": 0.04250435573420922, + "angularVelocity": -0.35117844853297475, + "velocityX": 0.40813256417294685, + "velocityY": -1.5994393128709739, + "timestamp": 0.9726908196078323 + }, + { + "x": 1.7930814617897048, + "y": 5.6520476897956975, + "heading": 0.02146340510922574, + "angularVelocity": -0.2720542575597529, + "velocityX": 0.7601017287194328, + "velocityY": -1.2742038202491095, + "timestamp": 1.050031828029148 + }, + { + "x": 1.8734294527078426, + "y": 5.583639683859189, + "heading": 0.0071877988424821045, + "angularVelocity": -0.1845800379143917, + "velocityX": 1.0388795356848963, + "velocityY": -0.8844984999918234, + "timestamp": 1.1273728364504638 + }, + { + "x": 1.970064237353583, "y": 5.548515796661377, - "heading": -3.619508887306417e-27, - "angularVelocity": -0.07826081577430707, - "velocityX": 1.248589428567619, - "velocityY": -0.3810633863540251, - "timestamp": 1.2602746831741403 + "heading": 3.498573366455402e-27, + "angularVelocity": -0.09293645103935767, + "velocityX": 1.249463727177213, + "velocityY": -0.45414312425918, + "timestamp": 1.2047138448717796 }, { "x": 2.0785037517547607, "y": 5.548515796661377, - "heading": -1.3504694626823558e-27, - "angularVelocity": 2.3770174727505955e-28, - "velocityX": 1.3745300978043182, - "velocityY": 2.5815580956362606e-25, - "timestamp": 1.3300457656016675 + "heading": 9.074763160083063e-28, + "angularVelocity": -2.8883864105140666e-27, + "velocityX": 1.4020959464409022, + "velocityY": 1.1648614692434482e-25, + "timestamp": 1.2820548532930953 }, { - "x": 2.2108243531689626, + "x": 2.2093539983390484, "y": 5.548515796661377, - "heading": -3.98229506769339e-28, - "angularVelocity": 6.58921025141663e-29, - "velocityX": 1.7982508177997887, - "velocityY": 1.343058267982166e-16, - "timestamp": 1.403628716380524 + "heading": -5.091869940539703e-28, + "angularVelocity": -8.863496880486678e-29, + "velocityX": 1.8425023918344958, + "velocityY": 6.335504596026049e-17, + "timestamp": 1.3530725340104635 }, { - "x": 2.366717480768576, + "x": 2.3657372440174322, "y": 5.548515796661377, - "heading": 5.540104511762132e-28, - "angularVelocity": 6.589213013653999e-29, - "velocityX": 2.118603915030903, - "velocityY": 6.71525155127488e-16, - "timestamp": 1.4772116671593807 + "heading": -1.9258503020667073e-27, + "angularVelocity": -8.863493994530242e-29, + "velocityX": 2.202032565675437, + "velocityY": 3.167727717458953e-16, + "timestamp": 1.4240902147278316 }, { - "x": 2.4914319874847974, + "x": 2.4908438453710073, "y": 5.548515796661377, - "heading": 1.506250408105455e-27, - "angularVelocity": 6.589211632476856e-29, - "velocityX": 1.694883195035658, - "velocityY": 5.372204471623799e-16, - "timestamp": 1.5507946179382373 + "heading": -3.342513611103023e-27, + "angularVelocity": -8.863495435827685e-29, + "velocityX": 1.7616261202821912, + "velocityY": 2.534184135606925e-16, + "timestamp": 1.4951078954451997 }, { - "x": 2.58496786905088, + "x": 2.5846737979725942, "y": 5.548515796661377, - "heading": 2.458490365034698e-27, - "angularVelocity": 6.589211632476792e-29, - "velocityX": 1.2711624170548799, - "velocityY": 4.029154421641203e-16, - "timestamp": 1.6243775687170938 + "heading": -4.759176920139342e-27, + "angularVelocity": -8.863495435827717e-29, + "velocityX": 1.321219612549812, + "velocityY": 1.9006387498434594e-16, + "timestamp": 1.5661255761625679 }, { - "x": 2.647325124044576, + "x": 2.6472271003464662, "y": 5.548515796661377, - "heading": 3.4107303219639416e-27, - "angularVelocity": 6.589211632476844e-29, - "velocityX": 0.8474416197456015, - "velocityY": 2.686103379493745e-16, - "timestamp": 1.6979605194959504 + "heading": -6.175840229175665e-27, + "angularVelocity": -8.863495435827698e-29, + "velocityX": 0.8808130840377387, + "velocityY": 1.2670927613090377e-16, + "timestamp": 1.637143256879936 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 4.3629702788936556e-27, - "angularVelocity": 6.589211633112284e-29, - "velocityX": 0.42372081277207485, - "velocityY": 1.3430518397832439e-16, - "timestamp": 1.771543470274807 + "heading": -7.592503538214036e-27, + "angularVelocity": -8.863495438700938e-29, + "velocityX": 0.44040654513582195, + "velocityY": 6.3354647036682e-17, + "timestamp": 1.7081609375973041 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 5.369537146588021e-27, - "angularVelocity": 8.042005177965745e-28, - "velocityX": 2.643601734591811e-26, - "velocityY": 4.252096317345015e-26, - "timestamp": 1.8451264210536635 + "heading": -9.168166896056586e-27, + "angularVelocity": -2.3275147262701813e-27, + "velocityX": -7.876142714980879e-26, + "velocityY": -2.7822667519177177e-25, + "timestamp": 1.7791786183146723 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePDEABC.5.traj b/src/main/deploy/choreo/CenterLanePDEABC.5.traj index 81cb908c..1258a60c 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.5.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.5.traj @@ -3,209 +3,209 @@ { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 5.369537146588021e-27, - "angularVelocity": 8.042005177965745e-28, - "velocityX": 2.643601734591811e-26, - "velocityY": 4.252096317345015e-26, + "heading": -9.168166896056586e-27, + "angularVelocity": -2.3275147262701813e-27, + "velocityX": -7.876142714980879e-26, + "velocityY": -2.7822667519177177e-25, "timestamp": 0 }, { - "x": 2.65514872822099, - "y": 5.535337362244174, - "heading": -0.005358266322899027, - "angularVelocity": -0.07847108857121549, - "velocityX": -0.34203117386479875, - "velocityY": -0.1929963969806516, - "timestamp": 0.06828331835917112 + "x": 2.6551487281624913, + "y": 5.535337362195577, + "heading": -0.0053582663887861755, + "angularVelocity": -0.08143329787865013, + "velocityX": -0.3549425234125067, + "velocityY": -0.20028182653884286, + "timestamp": 0.06579945216968852 }, { - "x": 2.608944125410673, - "y": 5.508126235941394, - "heading": -0.01624992945284291, - "angularVelocity": -0.15950693949367814, - "velocityX": -0.676660184662962, - "velocityY": -0.39850327952208153, - "timestamp": 0.13656663671834224 + "x": 2.608944125248311, + "y": 5.508126235802502, + "heading": -0.016249929633628882, + "angularVelocity": -0.16552817517013058, + "velocityX": -0.7022034590048659, + "velocityY": -0.41354639736059495, + "timestamp": 0.13159890433937704 }, { - "x": 2.5407349138993025, - "y": 5.465596827387689, - "heading": -0.03293816440101749, - "angularVelocity": -0.24439695300679784, - "velocityX": -0.9989147151957237, - "velocityY": -0.622837459802409, - "timestamp": 0.20484995507751336 + "x": 2.5407349136089206, + "y": 5.4655968271259, + "heading": -0.03293816472196888, + "angularVelocity": -0.2536227056313985, + "velocityX": -1.0366227892519315, + "velocityY": -0.6463489782091855, + "timestamp": 0.19739835650906556 }, { - "x": 2.4521626115248987, - "y": 5.405634188727449, - "heading": -0.05584758658914312, - "angularVelocity": -0.33550540217775826, - "velocityX": -1.2971294380936889, - "velocityY": -0.8781447665568274, - "timestamp": 0.2731332734366845 + "x": 2.4521626111212957, + "y": 5.405634188324023, + "heading": -0.05584758703948852, + "angularVelocity": -0.34817041118274633, + "velocityX": -1.3460948315984256, + "velocityY": -0.9112938911290825, + "timestamp": 0.2631978086787541 }, { - "x": 2.3472993944122122, - "y": 5.324395116991271, - "heading": -0.08569447031240085, - "angularVelocity": -0.43710359192361903, - "velocityX": -1.5357076901433586, - "velocityY": -1.1897352631408427, - "timestamp": 0.3414165917958556 + "x": 2.347299393995622, + "y": 5.324395116461008, + "heading": -0.08569447081932852, + "angularVelocity": -0.45360383400866283, + "velocityX": -1.593679182240697, + "velocityY": -1.2346466297851608, + "timestamp": 0.3289972608484426 }, { - "x": 2.239880929754108, - "y": 5.216483136191594, - "heading": -0.12272614650889033, - "angularVelocity": -0.542323909943895, - "velocityX": -1.5731289462688671, - "velocityY": -1.580356423688402, - "timestamp": 0.4096999101550267 + "x": 2.239880929566337, + "y": 5.216483135728542, + "heading": -0.12272614687803589, + "angularVelocity": -0.5627961151288577, + "velocityX": -1.6325130512069779, + "velocityY": -1.6400133614209311, + "timestamp": 0.39479671301813113 }, { - "x": 2.153374403355559, - "y": 5.091739159562662, - "heading": -0.16180305380781881, - "angularVelocity": -0.5722760439582536, - "velocityX": -1.2668764271754205, - "velocityY": -1.8268587354348766, - "timestamp": 0.47798322851419783 + "x": 2.1533744032985735, + "y": 5.091739159361965, + "heading": -0.16180305396789932, + "angularVelocity": -0.5938789123819668, + "velocityX": -1.314699794835295, + "velocityY": -1.8958208959685579, + "timestamp": 0.46059616518781965 }, { "x": 2.0932393074035645, "y": 4.9619622230529785, - "heading": -0.19963034769606672, - "angularVelocity": -0.5539756238745723, - "velocityX": -0.8806703803655747, - "velocityY": -1.9005657549777475, - "timestamp": 0.546266546873369 - }, - { - "x": 2.062901110669511, - "y": 4.862373019706677, - "heading": -0.22719939385993115, - "angularVelocity": -0.526195783497447, - "velocityX": -0.5790490938819385, - "velocityY": -1.9008063815925753, - "timestamp": 0.5986596808141655 - }, - { - "x": 2.0480285995415843, - "y": 4.7660067701880315, - "heading": -0.2526003669786383, - "angularVelocity": -0.4848149215011564, - "velocityX": -0.28386374338156817, - "velocityY": -1.8392915687680964, - "timestamp": 0.6510528147549621 - }, - { - "x": 2.0477407755531423, - "y": 4.675697634157938, - "heading": -0.27523200757833177, - "angularVelocity": -0.43195813835581237, - "velocityX": -0.005493544035129206, - "velocityY": -1.723682651473774, - "timestamp": 0.7034459486957587 - }, - { - "x": 2.060871040463269, - "y": 4.593707250314659, - "heading": -0.2946333476397457, - "angularVelocity": -0.37030310275649647, - "velocityX": 0.2506104125201524, - "velocityY": -1.5649070341149178, - "timestamp": 0.7558390826365553 - }, - { - "x": 2.0861920181034543, - "y": 4.521742431666527, - "heading": -0.3104741157540841, - "angularVelocity": -0.30234435169001433, - "velocityX": 0.48328809016841023, - "velocityY": -1.373554380798257, - "timestamp": 0.8082322165773519 - }, - { - "x": 2.122546344711596, - "y": 4.4610604943149115, - "heading": -0.32252861579276854, - "angularVelocity": -0.23007785814655887, - "velocityX": 0.6938757786320086, - "velocityY": -1.1582040009323555, - "timestamp": 0.8606253505181485 - }, - { - "x": 2.168900206537902, - "y": 4.412584630956035, - "heading": -0.3306480218589531, - "angularVelocity": -0.15497080352855533, - "velocityX": 0.8847316115635476, - "velocityY": -0.9252331309986794, - "timestamp": 0.9130184844589451 - }, - { - "x": 2.224353286156776, - "y": 4.376998478351871, + "heading": -0.19963034769256288, + "angularVelocity": -0.5748876696892883, + "velocityX": -0.9139148414173547, + "velocityY": -1.9723102857195456, + "timestamp": 0.5263956173575082 + }, + { + "x": 2.06290111070071, + "y": 4.862373019847466, + "heading": -0.22719939375272255, + "angularVelocity": -0.5460591677053325, + "velocityX": -0.6009076413123203, + "velocityY": -1.9725599970405536, + "timestamp": 0.576882904616161 + }, + { + "x": 2.048028599586168, + "y": 4.7660067703914, + "heading": -0.2526003668147138, + "angularVelocity": -0.5031162187791788, + "velocityX": -0.29457932723436914, + "velocityY": -1.9087230605671173, + "timestamp": 0.6273701918748138 + }, + { + "x": 2.047740775609064, + "y": 4.67569763435831, + "heading": -0.27523200740159587, + "angularVelocity": -0.4482641436236724, + "velocityX": -0.00570091983015679, + "velocityY": -1.788750018800283, + "timestamp": 0.6778574791334666 + }, + { + "x": 2.0608710405337267, + "y": 4.5937072504707785, + "heading": -0.29463334748351117, + "angularVelocity": -0.3842816902108432, + "velocityX": 0.26007071557230893, + "velocityY": -1.62398077495204, + "timestamp": 0.7283447663921194 + }, + { + "x": 2.0861920181883997, + "y": 4.5217424317614965, + "heading": -0.31047411563844995, + "angularVelocity": -0.3137575618548228, + "velocityX": 0.5015317524380114, + "velocityY": -1.4254047427939316, + "timestamp": 0.7788320536507722 + }, + { + "x": 2.1225463448038515, + "y": 4.461060494351439, + "heading": -0.3225286157246152, + "angularVelocity": -0.2387630776121657, + "velocityX": 0.7200689240680372, + "velocityY": -1.2019250925322578, + "timestamp": 0.829319340909425 + }, + { + "x": 2.16890020662259, + "y": 4.412584630951135, + "heading": -0.3306480218329806, + "angularVelocity": -0.16082080359692727, + "velocityX": 0.9181293813878825, + "velocityY": -0.9601597953155385, + "timestamp": 0.8798066281680779 + }, + { + "x": 2.2243532862123785, + "y": 4.376998478332531, "heading": -0.3347371673945201, - "angularVelocity": -0.07804735521619326, - "velocityX": 1.0584035626029547, - "velocityY": -0.6792140482448454, - "timestamp": 0.9654116183997417 + "angularVelocity": -0.08099356855103614, + "velocityX": 1.0983572816201457, + "velocityY": -0.7048537275590007, + "timestamp": 0.9302939154267307 }, { "x": 2.2881293296813965, "y": 4.354815483093262, "heading": -0.3347371673945201, - "angularVelocity": 3.779464021226463e-27, - "velocityX": 1.2172595668105288, - "velocityY": -0.4233950823341772, - "timestamp": 1.0178047523405382 + "angularVelocity": -3.351040193936762e-27, + "velocityX": 1.2632099471355258, + "velocityY": -0.43937784031895566, + "timestamp": 0.9807812026853835 }, { - "x": 2.4097151176038345, - "y": 4.314433194202397, + "x": 2.4097151173059284, + "y": 4.31443319431648, "heading": -0.3347371673945201, - "angularVelocity": 6.573584275341322e-29, - "velocityX": 1.6302002618641742, - "velocityY": -0.5414384283676151, - "timestamp": 1.0923880969557587 + "angularVelocity": -1.1055944271274773e-28, + "velocityX": 1.6917387541138205, + "velocityY": -0.5618772081689679, + "timestamp": 1.0526515120513356 }, { - "x": 2.5011472356587063, - "y": 4.284859003501582, + "x": 2.501147235462863, + "y": 4.284859003573599, "heading": -0.3347371673945201, - "angularVelocity": 6.573584272217567e-29, - "velocityX": 1.225905308035939, - "velocityY": -0.396525401929742, - "timestamp": 1.166971441570979 + "angularVelocity": -1.1055944267694559e-28, + "velocityX": 1.2721820590944748, + "velocityY": -0.4114938561387572, + "timestamp": 1.1245218214172876 }, { - "x": 2.562139216220098, - "y": 4.265257651811236, + "x": 2.562139216145648, + "y": 4.265257651838079, "heading": -0.3347371673945201, - "angularVelocity": -6.199774132429429e-28, - "velocityX": 0.8177694480725244, - "velocityY": -0.2628113795576003, - "timestamp": 1.2415547861861995 + "angularVelocity": -1.105594426769472e-28, + "velocityX": 0.8486394621209057, + "velocityY": -0.2727322577076049, + "timestamp": 1.1963921307832397 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": 7.63642932425348e-28, - "velocityX": 0.40901189675210486, - "velocityY": -0.1310099931742232, - "timestamp": 1.31613813080142 + "angularVelocity": -1.1055944267596697e-28, + "velocityX": 0.42445170461571047, + "velocityY": -0.13595549514112273, + "timestamp": 1.2682624401491918 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": 1.6433960683327454e-29, - "velocityX": 1.171390593825864e-26, - "velocityY": -5.961445669521376e-27, - "timestamp": 1.3907214754166404 + "angularVelocity": -2.916408988560576e-29, + "velocityX": -2.969429335311962e-26, + "velocityY": 1.2754135405824192e-26, + "timestamp": 1.3401327495151438 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePDEABC.traj b/src/main/deploy/choreo/CenterLanePDEABC.traj index d390d04e..6b57ff2e 100644 --- a/src/main/deploy/choreo/CenterLanePDEABC.traj +++ b/src/main/deploy/choreo/CenterLanePDEABC.traj @@ -3,1856 +3,1802 @@ { "x": 1.3350272178649902, "y": 5.601006507873535, - "heading": 3.967978705267542e-25, - "angularVelocity": 7.379244608128328e-26, - "velocityX": 8.172146600807894e-26, - "velocityY": -1.420872098935511e-25, + "heading": 1.1438500584259617e-25, + "angularVelocity": 2.0139373174315835e-26, + "velocityX": -2.9204847474037974e-26, + "velocityY": 3.705827554701837e-26, "timestamp": 0 }, { - "x": 1.3486609048876415, - "y": 5.6125344531964885, - "heading": 0.0053952990645401815, - "angularVelocity": 0.09676162177278211, - "velocityX": 0.24451242670209422, - "velocityY": 0.20674714632375757, - "timestamp": 0.05575866718325111 - }, - { - "x": 1.3762911400128175, - "y": 5.635152571560341, - "heading": 0.015980040657686934, - "angularVelocity": 0.1898313235924372, - "velocityX": 0.4955325606038828, - "velocityY": 0.4056430956198756, - "timestamp": 0.11151733436650221 - }, - { - "x": 1.4183448247504244, - "y": 5.668306854106291, - "heading": 0.0314941432622812, - "angularVelocity": 0.2782366112447971, - "velocityX": 0.7542089304143026, - "velocityY": 0.594603210958904, - "timestamp": 0.1672760015497533 - }, - { - "x": 1.4753265664194932, - "y": 5.711278723492, - "heading": 0.05160335039247586, - "angularVelocity": 0.3606471988310923, - "velocityX": 1.0219351456482668, - "velocityY": 0.7706760501373088, - "timestamp": 0.22303466873300443 - }, - { - "x": 1.547831871808343, - "y": 5.763109379844883, - "heading": 0.07586790196130719, - "angularVelocity": 0.43517093923866185, - "velocityX": 1.3003414366157815, - "velocityY": 0.9295533586292857, - "timestamp": 0.27879333591625555 - }, - { - "x": 1.6365515186230641, - "y": 5.822478611934523, - "heading": 0.10369270639110653, - "angularVelocity": 0.499022050479668, - "velocityX": 1.5911364330704598, - "velocityY": 1.064753429175822, - "timestamp": 0.33455200309950667 - }, - { - "x": 1.7422414603963892, - "y": 5.887510284275809, - "heading": 0.1342490325748326, - "angularVelocity": 0.5480103404068561, - "velocityX": 1.8954890264140394, - "velocityY": 1.1663060762833288, - "timestamp": 0.3903106702827578 - }, - { - "x": 1.8655876183871098, - "y": 5.955479067462068, - "heading": 0.1663576179952195, - "angularVelocity": 0.57584922743691, - "velocityX": 2.212143227623125, - "velocityY": 1.2189814896916369, - "timestamp": 0.4460693374660089 - }, - { - "x": 2.0068104942426706, - "y": 6.022477551407248, - "heading": 0.1983353246164728, - "angularVelocity": 0.5735019905723076, - "velocityX": 2.532752000535291, - "velocityY": 1.201579724368033, - "timestamp": 0.50182800464926 - }, - { - "x": 2.164876110933553, - "y": 6.083435356761985, - "heading": 0.22790661577104523, - "angularVelocity": 0.5303442971006865, - "velocityX": 2.834816983185756, - "velocityY": 1.093243587663212, - "timestamp": 0.5575866718325111 + "x": 1.3513037651461415, + "y": 5.614995878157814, + "heading": 0.006549885022033663, + "angularVelocity": 0.1111819339605223, + "velocityX": 0.27628851481066125, + "velocityY": 0.23746451088284168, + "timestamp": 0.05891141473001675 + }, + { + "x": 1.384378906601625, + "y": 5.642351283609904, + "heading": 0.019354935835911788, + "angularVelocity": 0.21736111537232614, + "velocityX": 0.5614385871916767, + "velocityY": 0.46434813316664775, + "timestamp": 0.1178228294600335 + }, + { + "x": 1.4348748758417986, + "y": 5.682261570187258, + "heading": 0.0380334905813409, + "angularVelocity": 0.3170617244727609, + "velocityX": 0.8571508505030827, + "velocityY": 0.6774627083776107, + "timestamp": 0.17673424419005024 + }, + { + "x": 1.5035350329878103, + "y": 5.733639392412991, + "heading": 0.06208045701070026, + "angularVelocity": 0.4081885749911699, + "velocityX": 1.1654813835429356, + "velocityY": 0.8721199866136561, + "timestamp": 0.235645658920067 + }, + { + "x": 1.5912385139518137, + "y": 5.794977395067809, + "heading": 0.09080789039928205, + "angularVelocity": 0.48763781213260343, + "velocityX": 1.4887349313530633, + "velocityY": 1.0411904541067623, + "timestamp": 0.29455707365008377 + }, + { + "x": 1.6989811168528033, + "y": 5.864111533799202, + "heading": 0.12324878614182731, + "angularVelocity": 0.550672495155949, + "velocityX": 1.8288917927834498, + "velocityY": 1.1735270498633483, + "timestamp": 0.35346848838010053 + }, + { + "x": 1.8277399268235333, + "y": 5.93784849269496, + "heading": 0.15800938597032024, + "angularVelocity": 0.590048634679649, + "velocityX": 2.1856343216474197, + "velocityY": 1.2516582606899704, + "timestamp": 0.4123799031101173 + }, + { + "x": 1.978020614375797, + "y": 6.011501208422767, + "heading": 0.19307007056378325, + "angularVelocity": 0.5951424652444947, + "velocityX": 2.5509604249190123, + "velocityY": 1.250228263322945, + "timestamp": 0.47129131784013406 + }, + { + "x": 2.1488551922208927, + "y": 6.078800735840598, + "heading": 0.2256402887317746, + "angularVelocity": 0.5528676966468451, + "velocityX": 2.899855293375791, + "velocityY": 1.1423851850487134, + "timestamp": 0.5302027325701508 }, { "x": 2.3368515968322754, "y": 6.133185386657715, "heading": 0.2525545797921912, - "angularVelocity": 0.44204722362068527, - "velocityX": 3.0842825803838596, - "velocityY": 0.8922385058492518, - "timestamp": 0.6133453390157622 - }, - { - "x": 2.5269005159766844, - "y": 6.188163800352406, - "heading": 0.252554892201178, - "angularVelocity": 0.000005553817937610827, - "velocityX": 3.378574691383036, - "velocityY": 0.9773729727982717, - "timestamp": 0.6695965504005877 - }, - { - "x": 2.7281832415838734, - "y": 6.246391992377104, - "heading": 0.2525548726806658, - "angularVelocity": -3.47023853848989e-7, - "velocityX": 3.578282505423293, - "velocityY": 1.0351455656012056, - "timestamp": 0.7258477617854133 - }, - { - "x": 2.929465967191096, - "y": 6.304620184401812, - "heading": 0.2525548531602303, - "angularVelocity": -3.4702249119339613e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 0.7820989731702389 - }, - { - "x": 3.1307486927983192, - "y": 6.36284837642652, - "heading": 0.25255483363979486, - "angularVelocity": -3.4702249016578237e-7, - "velocityX": 3.5782825054238923, - "velocityY": 1.0351455656013788, - "timestamp": 0.8383501845550645 - }, - { - "x": 3.332031418405542, - "y": 6.421076568451228, - "heading": 0.2525548141193594, - "angularVelocity": -3.470224908197631e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 0.8946013959398901 - }, - { - "x": 3.5333141440127647, - "y": 6.4793047604759355, - "heading": 0.2525547945989239, - "angularVelocity": -3.4702249080835237e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 0.9508526073247157 - }, - { - "x": 3.7345968696199874, - "y": 6.537532952500643, - "heading": 0.2525547750784885, - "angularVelocity": -3.470224894869701e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 1.0071038187095414 - }, - { - "x": 3.93587959522721, - "y": 6.595761144525352, - "heading": 0.25255475555805307, - "angularVelocity": -3.470224898418443e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 1.063355030094367 - }, - { - "x": 4.137162320834433, - "y": 6.65398933655006, - "heading": 0.2525547360376176, - "angularVelocity": -3.470224909887922e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.1196062414791927 - }, - { - "x": 4.338445046441655, - "y": 6.712217528574768, - "heading": 0.2525547165171822, - "angularVelocity": -3.4702248928646837e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 1.1758574528640184 - }, - { - "x": 4.539727772048877, - "y": 6.770445720599476, - "heading": 0.25255469699674676, - "angularVelocity": -3.470224898754089e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 1.2321086642488441 - }, - { - "x": 4.7410104976561, - "y": 6.8286739126241836, - "heading": 0.25255467747631133, - "angularVelocity": -3.470224907627516e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.2883598756336698 - }, - { - "x": 4.942293223263322, - "y": 6.886902104648891, - "heading": 0.25255465795587584, - "angularVelocity": -3.4702249046766804e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013786, - "timestamp": 1.3446110870184955 - }, - { - "x": 5.143575948870544, - "y": 6.945130296673599, - "heading": 0.2525546384354404, - "angularVelocity": -3.47022489425383e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.4008622984033212 - }, - { - "x": 5.344858674477766, - "y": 7.003358488698307, - "heading": 0.25255461891500497, - "angularVelocity": -3.470224896167856e-7, - "velocityX": 3.578282505423892, - "velocityY": 1.0351455656013788, - "timestamp": 1.457113509788147 - }, - { - "x": 5.546141400084989, - "y": 7.061586680723015, - "heading": 0.25255459939456953, - "angularVelocity": -3.470224897168836e-7, - "velocityX": 3.5782825054238914, - "velocityY": 1.0351455656013788, - "timestamp": 1.5133647211729726 + "angularVelocity": 0.45686037559548065, + "velocityX": 3.1911711078904768, + "velocityY": 0.9231598165882506, + "timestamp": 0.5891141473001675 + }, + { + "x": 2.5473350968820796, + "y": 6.1940752301450726, + "heading": 0.25255484664390665, + "angularVelocity": 0.000004475832490919022, + "velocityX": 3.5303834818832023, + "velocityY": 1.0212890683182199, + "timestamp": 0.6487347242516341 + }, + { + "x": 2.7606743654688186, + "y": 6.255791206318409, + "heading": 0.25255482885683184, + "angularVelocity": -2.9833785118107346e-7, + "velocityX": 3.578282524176262, + "velocityY": 1.0351455710261694, + "timestamp": 0.7083553012031008 + }, + { + "x": 2.974013634055561, + "y": 6.317507182491745, + "heading": 0.2525548110697642, + "angularVelocity": -2.983377309458156e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 0.7679758781545674 + }, + { + "x": 3.1873529026423038, + "y": 6.379223158665082, + "heading": 0.25255479328269653, + "angularVelocity": -2.983377305484016e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.827596455106034 + }, + { + "x": 3.400692171229046, + "y": 6.440939134838418, + "heading": 0.2525547754956289, + "angularVelocity": -2.9833773174604553e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 0.8872170320575006 + }, + { + "x": 3.6140314398157884, + "y": 6.502655111011755, + "heading": 0.25255475770856123, + "angularVelocity": -2.983377305928146e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.9468376090089672 + }, + { + "x": 3.827370708402531, + "y": 6.5643710871850915, + "heading": 0.25255473992149363, + "angularVelocity": -2.983377301697445e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.0064581859604338 + }, + { + "x": 4.040709976989273, + "y": 6.626087063358428, + "heading": 0.25255472213442604, + "angularVelocity": -2.983377305025075e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.0660787629119004 + }, + { + "x": 4.254049245576016, + "y": 6.687803039531764, + "heading": 0.2525547043473584, + "angularVelocity": -2.9833773095408974e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.125699339863367 + }, + { + "x": 4.467388514162758, + "y": 6.749519015705101, + "heading": 0.2525546865602907, + "angularVelocity": -2.983377319947091e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.1853199168148336 + }, + { + "x": 4.680727782749501, + "y": 6.811234991878438, + "heading": 0.252554668773223, + "angularVelocity": -2.9833773124406734e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.2449404937663002 + }, + { + "x": 4.894067051336243, + "y": 6.872950968051774, + "heading": 0.2525546509861554, + "angularVelocity": -2.983377305581398e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.3045610707177668 + }, + { + "x": 5.107406319922986, + "y": 6.9346669442251105, + "heading": 0.2525546331990878, + "angularVelocity": -2.983377313652051e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.3641816476692334 + }, + { + "x": 5.320745588509729, + "y": 6.996382920398447, + "heading": 0.2525546154120202, + "angularVelocity": -2.983377306356561e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4238022246207 + }, + { + "x": 5.534084857096471, + "y": 7.058098896571784, + "heading": 0.2525545976249525, + "angularVelocity": -2.983377306690358e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.4834228015721667 }, { "x": 5.747424125671387, "y": 7.119814872741699, "heading": 0.2525545797921912, - "angularVelocity": -3.4847922161224964e-7, - "velocityX": 3.5782825050536915, - "velocityY": 1.0351455654942852, - "timestamp": 1.5696159325577983 - }, - { - "x": 5.864122316952898, - "y": 7.149764367678907, - "heading": 0.23825610881476394, - "angularVelocity": -0.4289921260648423, - "velocityX": 3.501255852098466, - "velocityY": 0.8985644358731831, - "timestamp": 1.6029463134871351 - }, - { - "x": 5.980010019619152, - "y": 7.178464848632163, - "heading": 0.21993450293637717, - "angularVelocity": -0.5496968641681629, - "velocityX": 3.4769390398491176, - "velocityY": 0.8610906972261488, - "timestamp": 1.636276694416472 - }, - { - "x": 6.095759168980239, - "y": 7.207171809244793, - "heading": 0.2014022591938407, - "angularVelocity": -0.5560165598414963, - "velocityX": 3.4727820725027265, - "velocityY": 0.8612851042264353, - "timestamp": 1.6696070753458088 - }, - { - "x": 6.211373074459108, - "y": 7.235898623678772, - "heading": 0.18268720108591044, - "angularVelocity": -0.5615014766140173, - "velocityX": 3.4687243966392267, - "velocityY": 0.8618807716264242, - "timestamp": 1.7029374562751456 - }, - { - "x": 6.326838442963843, - "y": 7.264631799858999, - "heading": 0.16373191419922642, - "angularVelocity": -0.5687089783603383, - "velocityX": 3.464267892693179, - "velocityY": 0.8620716409195424, - "timestamp": 1.7362678372044824 - }, - { - "x": 6.442151068511899, - "y": 7.29337427879422, - "heading": 0.1445283357059787, - "angularVelocity": -0.5761583863671085, - "velocityX": 3.4596851980938594, - "velocityY": 0.8623507482904867, - "timestamp": 1.7695982181338192 - }, - { - "x": 6.557303014616615, - "y": 7.322124225476267, - "heading": 0.1250513432276187, - "angularVelocity": -0.5843615324905178, - "velocityX": 3.454864387804252, - "velocityY": 0.8625748005400329, - "timestamp": 1.802928599063156 - }, - { - "x": 6.672285593632433, - "y": 7.350880202549992, - "heading": 0.10527449948643015, - "angularVelocity": -0.5933578671998123, - "velocityX": 3.449782925061388, - "velocityY": 0.8627557283156009, - "timestamp": 1.8362589799924929 - }, - { - "x": 6.787113803468559, - "y": 7.379671810584065, - "heading": 0.08527801395894485, - "angularVelocity": -0.5999477044645701, - "velocityX": 3.4451514394501603, - "velocityY": 0.8638247518116766, - "timestamp": 1.8695893609218297 - }, - { - "x": 6.901739724666763, - "y": 7.408448228453197, - "heading": 0.0648615716893241, - "angularVelocity": -0.6125475227212472, - "velocityX": 3.4390822427508825, - "velocityY": 0.8633690064971079, - "timestamp": 1.9029197418511665 - }, - { - "x": 7.014861438150348, - "y": 7.4357314738869205, - "heading": 0.038575820671149744, - "angularVelocity": -0.7886423822728691, - "velocityX": 3.3939520140322568, - "velocityY": 0.8185698654799702, - "timestamp": 1.9362501227805033 + "angularVelocity": -2.991041384854961e-7, + "velocityX": 3.5782825239779474, + "velocityY": 1.0351455709687998, + "timestamp": 1.5430433785236333 + }, + { + "x": 5.874242280548121, + "y": 7.151641192910771, + "heading": 0.23431006193331627, + "angularVelocity": -0.5017601477559481, + "velocityX": 3.487748847149364, + "velocityY": 0.8752864413346503, + "timestamp": 1.5794044126561186 + }, + { + "x": 6.000453145534325, + "y": 7.182664404015401, + "heading": 0.21336082591069744, + "angularVelocity": -0.5761452203556151, + "velocityX": 3.4710471799658134, + "velocityY": 0.853199361481138, + "timestamp": 1.615765446788604 + }, + { + "x": 6.126521345623787, + "y": 7.213763203476394, + "heading": 0.192336966602575, + "angularVelocity": -0.5781975075714219, + "velocityX": 3.4671236145297133, + "velocityY": 0.8552781900449158, + "timestamp": 1.6521264809210894 + }, + { + "x": 6.252403333907122, + "y": 7.24487049169852, + "heading": 0.17101724585582967, + "angularVelocity": -0.5863342794119836, + "velocityX": 3.462002423381879, + "velocityY": 0.8555116476825977, + "timestamp": 1.6884875150535748 + }, + { + "x": 6.378086189949285, + "y": 7.2759799644241046, + "heading": 0.14935632557503462, + "angularVelocity": -0.5957179381056964, + "velocityX": 3.456525894841813, + "velocityY": 0.8555717258270096, + "timestamp": 1.7248485491860601 + }, + { + "x": 6.503564352522718, + "y": 7.307098484942146, + "heading": 0.12734794412408884, + "angularVelocity": -0.6052738041155772, + "velocityX": 3.4508964215989995, + "velocityY": 0.8558205579262937, + "timestamp": 1.7612095833185455 + }, + { + "x": 6.628828810809778, + "y": 7.338228595030231, + "heading": 0.10496993550474346, + "angularVelocity": -0.6154392787017191, + "velocityX": 3.4450191331370044, + "velocityY": 0.8561392939117064, + "timestamp": 1.7975706174510309 + }, + { + "x": 6.753876017529989, + "y": 7.3693809168618785, + "heading": 0.08222565272715081, + "angularVelocity": -0.6255125389096852, + "velocityX": 3.4390442874806206, + "velocityY": 0.8567501605740924, + "timestamp": 1.8339316515835162 + }, + { + "x": 6.878734995256494, + "y": 7.4006038692779965, + "heading": 0.05925591834951263, + "angularVelocity": -0.6317129016173038, + "velocityX": 3.4338676197042926, + "velocityY": 0.8586926406535731, + "timestamp": 1.8702926857160016 + }, + { + "x": 7.003212758632201, + "y": 7.431690611037589, + "heading": 0.0352626014950734, + "angularVelocity": -0.6598634342196367, + "velocityX": 3.4233834747977516, + "velocityY": 0.8549465795259067, + "timestamp": 1.906653719848487 }, { "x": 7.125290870666504, "y": 7.458, - "heading": 1.0084355524659913e-21, - "angularVelocity": -1.1573771314805474, - "velocityX": 3.313176430544709, - "velocityY": 0.668114959750724, - "timestamp": 1.9695805037098402 - }, - { - "x": 7.337738270409439, - "y": 7.481917447620179, - "heading": -0.00819620478546554, - "angularVelocity": -0.12615227280600286, - "velocityX": 3.2698941803922152, - "velocityY": 0.368126523919295, - "timestamp": 2.0345512299849795 - }, - { - "x": 7.526586631074925, - "y": 7.500039280530163, - "heading": -0.014041404226933134, - "angularVelocity": -0.08996666308937673, - "velocityX": 2.906668456586843, - "velocityY": 0.27892304656162725, - "timestamp": 2.099521956260119 - }, - { - "x": 7.6918053440422804, - "y": 7.512484823370471, - "heading": -0.017813194168849914, - "angularVelocity": -0.058053682914731695, - "velocityX": 2.5429716187515523, - "velocityY": 0.19155616004049963, - "timestamp": 2.164492682535258 - }, - { - "x": 7.833384352673449, - "y": 7.519294018241946, - "heading": -0.01960436856302971, - "angularVelocity": -0.027568945229186584, - "velocityX": 2.179119993696969, - "velocityY": 0.10480404424970338, - "timestamp": 2.2294634088103975 - }, - { - "x": 7.951318655599417, - "y": 7.520486868366768, - "heading": -0.019461534286698053, - "angularVelocity": 0.0021984405057560424, - "velocityX": 1.815191389835117, - "velocityY": 0.018359808997220107, - "timestamp": 2.294434135085537 - }, - { - "x": 8.045605259786509, - "y": 7.516075387181805, - "heading": -0.017412919986794666, - "angularVelocity": 0.031531343688969565, - "velocityX": 1.4512167185542217, - "velocityY": -0.06789952087470152, - "timestamp": 2.359404861360676 - }, - { - "x": 8.116242172260733, - "y": 7.5060675897274995, - "heading": -0.013477652900012242, - "angularVelocity": 0.06056984910584597, - "velocityX": 1.0872113723200465, - "velocityY": -0.15403548687333402, - "timestamp": 2.4243755876358155 - }, - { - "x": 8.163227969814436, - "y": 7.490469205298236, - "heading": -0.007669730702440132, - "angularVelocity": 0.08939290863052056, - "velocityX": 0.723184120718108, - "velocityY": -0.24008327016705255, - "timestamp": 2.489346313910955 + "heading": -1.1078640911459773e-20, + "angularVelocity": -0.9697909406699016, + "velocityX": 3.3573883402077644, + "velocityY": 0.7235599753997619, + "timestamp": 1.9430147539809723 + }, + { + "x": 7.338537136585377, + "y": 7.4820248133164355, + "heading": -0.00804052191695304, + "angularVelocity": -0.12790212002878093, + "velocityX": 3.392149139191805, + "velocityY": 0.38216730060631093, + "timestamp": 2.005879403254524 + }, + { + "x": 7.527981798214128, + "y": 7.500236040572311, + "heading": -0.013862931395131553, + "angularVelocity": -0.09261818121091622, + "velocityX": 3.013532467259838, + "velocityY": 0.2896894751870813, + "timestamp": 2.068744052528076 + }, + { + "x": 7.6935968882672885, + "y": 7.512744942179138, + "heading": -0.017658491883084444, + "angularVelocity": -0.06037670665172621, + "velocityX": 2.6344709143686895, + "velocityY": 0.1989814904143519, + "timestamp": 2.1316087018016274 + }, + { + "x": 7.835373213607451, + "y": 7.519588665029473, + "heading": -0.019490985187148625, + "angularVelocity": -0.029149821485366035, + "velocityX": 2.255263124482431, + "velocityY": 0.1088644083665287, + "timestamp": 2.194473351075179 + }, + { + "x": 7.953306201274802, + "y": 7.52078579458168, + "heading": -0.019392463666041717, + "angularVelocity": 0.001567200680277371, + "velocityX": 1.8759825916497619, + "velocityY": 0.01904296875969824, + "timestamp": 2.2573380003487307 + }, + { + "x": 8.047393114183997, + "y": 7.516347487170096, + "heading": -0.01738240796128744, + "angularVelocity": 0.0319743405551765, + "velocityX": 1.4966585194769915, + "velocityY": -0.0706010049029291, + "timestamp": 2.3202026496222823 + }, + { + "x": 8.11763212951421, + "y": 7.506281183777245, + "heading": -0.013474109300406987, + "angularVelocity": 0.062170054331707525, + "velocityX": 1.1173054513447094, + "velocityY": -0.1601266134333638, + "timestamp": 2.383067298895834 + }, + { + "x": 8.164021945337474, + "y": 7.490592202393592, + "heading": -0.007677398137362595, + "angularVelocity": 0.09220939319680839, + "velocityX": 0.737931673195224, + "velocityY": -0.24956762767234375, + "timestamp": 2.4459319481693855 }, { "x": 8.186561584472656, "y": 7.469284534454346, - "heading": 7.422813602625121e-23, - "angularVelocity": 0.11804902210820577, - "velocityX": 0.3591404313291331, - "velocityY": -0.3260648611834275, - "timestamp": 2.554317040186094 - }, - { - "x": 8.171272340508109, - "y": 7.427332842662171, - "heading": 0.014840203267270782, - "angularVelocity": 0.15900903302410063, - "velocityX": -0.16382039077820704, - "velocityY": -0.44950179087577585, - "timestamp": 2.6476463497544476 - }, - { - "x": 8.107175512886487, - "y": 7.3738609015025816, - "heading": 0.03350091514476021, - "angularVelocity": 0.1999448186619506, - "velocityX": -0.6867813328746101, - "velocityY": -0.572938355666582, - "timestamp": 2.7409756593228005 - }, - { - "x": 7.994271087246123, - "y": 7.308868753063173, - "heading": 0.05597944584590409, - "angularVelocity": 0.24085178391554368, - "velocityX": -1.2097424288526986, - "velocityY": -0.6963744694994183, - "timestamp": 2.8343049688911535 - }, - { - "x": 7.832559046421551, - "y": 7.232356448582914, - "heading": 0.08227288434163177, - "angularVelocity": 0.2817275582272545, - "velocityX": -1.732703708754388, - "velocityY": -0.8198100343196232, - "timestamp": 2.9276342784595064 - }, - { - "x": 7.6220393708156715, - "y": 7.144324050552095, - "heading": 0.1123783464235267, - "angularVelocity": 0.3225724289736236, - "velocityX": -2.2556651986340626, - "velocityY": -0.9432449295721523, - "timestamp": 3.0209635880278594 - }, - { - "x": 7.362712038546165, - "y": 7.04477163823656, - "heading": 0.14629325961596634, - "angularVelocity": 0.36338973629287186, - "velocityX": -2.778626922977279, - "velocityY": -1.0666789755111628, - "timestamp": 3.1142928975962123 - }, - { - "x": 7.054577023664908, - "y": 6.933699333854541, - "heading": 0.18401565672002676, - "angularVelocity": 0.4041859655720804, - "velocityX": -3.3015889253480717, - "velocityY": -1.1901117119126619, - "timestamp": 3.2076222071645653 - }, - { - "x": 6.71622230121249, - "y": 6.85383794706396, - "heading": 0.18401565782506035, - "angularVelocity": 1.1840156036202986e-8, - "velocityX": -3.6253854659088836, - "velocityY": -0.8556946061203927, - "timestamp": 3.300951516732918 - }, - { - "x": 6.377867419030211, - "y": 6.7739772370157985, - "heading": 0.1840156589300666, - "angularVelocity": 1.1839863049890439e-8, - "velocityX": -3.6253871773740243, - "velocityY": -0.8556873549961612, - "timestamp": 3.394280826301271 - }, - { - "x": 6.0395125368493074, - "y": 6.694116526961809, - "heading": 0.18401566003507294, - "angularVelocity": 1.1839864253851599e-8, - "velocityX": -3.6253871773592854, - "velocityY": -0.8556873550586043, - "timestamp": 3.487610135869624 - }, - { - "x": 5.701157654668755, - "y": 6.614255816906336, - "heading": 0.18401566114007933, - "angularVelocity": 1.18398644204111e-8, - "velocityX": -3.625387177355538, - "velocityY": -0.855687355074483, - "timestamp": 3.580939445437977 - }, - { - "x": 5.362802772497576, - "y": 6.534395106811441, - "heading": 0.18401566224512567, - "angularVelocity": 1.1840292694239514e-8, - "velocityX": -3.6253871772550927, - "velocityY": -0.8556873554969, - "timestamp": 3.67426875500633 - }, - { - "x": 5.069956587719004, - "y": 6.4652758877562615, - "heading": 0.20685757815363412, - "angularVelocity": 0.24474536471074324, - "velocityX": -3.137772968995303, - "velocityY": -0.7405949896646158, - "timestamp": 3.767598064574683 - }, - { - "x": 4.8259180814209035, - "y": 6.407676584765678, - "heading": 0.22589278292875978, - "angularVelocity": 0.20395741555534105, - "velocityX": -2.614811010890098, - "velocityY": -0.6171619961293916, - "timestamp": 3.860927374143036 - }, - { - "x": 4.6306872671740225, - "y": 6.361597170610555, - "heading": 0.2411211921270372, - "angularVelocity": 0.16316856160951593, - "velocityX": -2.0918489073777633, - "velocityY": -0.49372929434751617, - "timestamp": 3.954256683711389 - }, - { - "x": 4.484264152022265, - "y": 6.3270376241371595, - "heading": 0.25254264332581844, - "angularVelocity": 0.12237796734600637, - "velocityX": -1.5688867283917882, - "velocityY": -0.37029681922252144, - "timestamp": 4.047585993279742 - }, - { - "x": 4.386648740184571, - "y": 6.303997931141046, - "heading": 0.26015698140700544, - "angularVelocity": 0.08158571103122086, - "velocityX": -1.0459245042009158, - "velocityY": -0.24686449629458126, - "timestamp": 4.140915302848095 + "heading": 1.0473347586059488e-21, + "angularVelocity": 0.12212584061281963, + "velocityX": 0.358542350838588, + "velocityY": -0.33894514938796605, + "timestamp": 2.508796597442937 + }, + { + "x": 8.169433800706123, + "y": 7.42685355131065, + "heading": 0.014977206112370758, + "angularVelocity": 0.16513162181596408, + "velocityX": -0.1888428783219197, + "velocityY": -0.46782403935650313, + "timestamp": 2.5994951925858016 + }, + { + "x": 8.10265502996488, + "y": 7.372742454671214, + "heading": 0.033468558101012916, + "angularVelocity": 0.2038769394334648, + "velocityX": -0.7362712800132867, + "velocityY": -0.5966034705852111, + "timestamp": 2.690193787728666 + }, + { + "x": 7.986220080079777, + "y": 7.306963959658495, + "heading": 0.05493247364660356, + "angularVelocity": 0.23665102542968372, + "velocityX": -1.2837569281166943, + "velocityY": -0.7252427108612493, + "timestamp": 2.7808923828715306 + }, + { + "x": 7.820121799349139, + "y": 7.229537347680479, + "heading": 0.07855456380147917, + "angularVelocity": 0.26044604238540936, + "velocityX": -1.8313214275150225, + "velocityY": -0.8536693634124876, + "timestamp": 2.871590978014395 + }, + { + "x": 7.6043499812219455, + "y": 7.1404953741126675, + "heading": 0.10296904259791395, + "angularVelocity": 0.2691825464107588, + "velocityX": -2.37899845953864, + "velocityY": -0.9817348706179747, + "timestamp": 2.9622895731572596 + }, + { + "x": 7.338890842614761, + "y": 7.0399060968505065, + "heading": 0.12540191269228773, + "angularVelocity": 0.24733426200304945, + "velocityX": -2.926827457349739, + "velocityY": -1.1090500035167739, + "timestamp": 3.052988168300124 + }, + { + "x": 7.023762260989218, + "y": 6.927996342713902, + "heading": 0.13709315762540872, + "angularVelocity": 0.12890216121545683, + "velocityX": -3.4744593466873996, + "velocityY": -1.233864250712263, + "timestamp": 3.1436867634429886 + }, + { + "x": 6.6949878661960245, + "y": 6.850204202472232, + "heading": 0.1370931594052802, + "angularVelocity": 1.9624024712667312e-8, + "velocityX": -3.6249116568489486, + "velocityY": -0.857699505919964, + "timestamp": 3.234385358585853 + }, + { + "x": 6.366213403460125, + "y": 6.772412349378657, + "heading": 0.13709316118514803, + "angularVelocity": 1.9623984758130592e-8, + "velocityX": -3.624912405953231, + "velocityY": -0.8576963399602832, + "timestamp": 3.3250839537287176 + }, + { + "x": 6.037438940720318, + "y": 6.694620496301594, + "heading": 0.13709316296501595, + "angularVelocity": 1.9623985202632452e-8, + "velocityX": -3.624912405996305, + "velocityY": -0.8576963397782383, + "timestamp": 3.415782548871582 + }, + { + "x": 5.708664477980543, + "y": 6.6168286432244035, + "heading": 0.13709316474488378, + "angularVelocity": 1.9623984644394548e-8, + "velocityX": -3.624912405995973, + "velocityY": -0.8576963397796433, + "timestamp": 3.5064811440144465 + }, + { + "x": 5.379890015210838, + "y": 6.539036790273742, + "heading": 0.13709316652476114, + "angularVelocity": 1.9624089556639278e-8, + "velocityX": -3.6249124063259477, + "velocityY": -0.8576963383845883, + "timestamp": 3.597179739157311 + }, + { + "x": 5.082161809078407, + "y": 6.468591173031877, + "heading": 0.1733415977378681, + "angularVelocity": 0.39965813313877674, + "velocityX": -3.282611000351902, + "velocityY": -0.7767002028080265, + "timestamp": 3.6878783343001755 + }, + { + "x": 4.834054918534678, + "y": 6.409886673220785, + "heading": 0.20354854900447322, + "angularVelocity": 0.3330476201866674, + "velocityX": -2.7355097413903846, + "velocityY": -0.6472481709184398, + "timestamp": 3.77857692944304 + }, + { + "x": 4.6355693784003975, + "y": 6.36292318296314, + "heading": 0.22771447843277892, + "angularVelocity": 0.26644215811987526, + "velocityX": -2.1884080985117165, + "velocityY": -0.5177973284334796, + "timestamp": 3.8692755245859045 + }, + { + "x": 4.486705209976563, + "y": 6.327700620163418, + "heading": 0.24583924264804105, + "angularVelocity": 0.1998351152706694, + "velocityX": -1.6413062207782854, + "velocityY": -0.3883473910951022, + "timestamp": 3.959974119728769 + }, + { + "x": 4.3874624262872, + "y": 6.30421892984517, + "heading": 0.25792252534877924, + "angularVelocity": 0.13322458503028708, + "velocityX": -1.0942041994480602, + "velocityY": -0.25889805990114845, + "timestamp": 4.0506727148716335 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": 0.04079263927507296, - "velocityX": -0.5229622556382164, - "velocityY": -0.12343224899141036, - "timestamp": 4.234244612416448 + "angularVelocity": 0.06661189082061515, + "velocityX": -0.5471021053136723, + "velocityY": -0.1294490312938923, + "timestamp": 4.141371310014498 }, { "x": 4.337841033935547, "y": 6.292478084564209, "heading": 0.2639641302660189, - "angularVelocity": -3.1350638097986163e-24, - "velocityX": 6.734301517773745e-23, - "velocityY": -5.476052050130636e-23, - "timestamp": 4.327573921984801 - }, - { - "x": 4.357085465154312, - "y": 6.298335605731047, - "heading": 0.24955697801054222, - "angularVelocity": -0.24189458757929325, - "velocityX": 0.32311199814604374, - "velocityY": 0.09834717102755294, - "timestamp": 4.387133552481924 - }, - { - "x": 4.395672489468794, - "y": 6.30982325311387, - "heading": 0.2214506254853789, - "angularVelocity": -0.4719027349661102, - "velocityX": 0.6478721239942183, - "velocityY": 0.19287640448638915, - "timestamp": 4.446693182979047 - }, - { - "x": 4.453716686931878, - "y": 6.3266599763174565, - "heading": 0.18050722178606005, - "angularVelocity": -0.6874354887291741, - "velocityX": 0.9745560370104606, - "velocityY": 0.28268683104740583, - "timestamp": 4.50625281347617 - }, - { - "x": 4.531352614071991, - "y": 6.348490007100106, - "heading": 0.12779504929109745, - "angularVelocity": -0.8850318924914878, - "velocityX": 1.3034991401409508, - "velocityY": 0.366523945841208, - "timestamp": 4.565812443973293 - }, - { - "x": 4.628738799037524, - "y": 6.374849024990675, - "heading": 0.0646674309135714, - "angularVelocity": -1.0599061453306917, - "velocityX": 1.6351039143911004, - "velocityY": 0.4425651682281852, - "timestamp": 4.625372074470416 - }, - { - "x": 4.746060953896417, - "y": 6.4051070852453105, - "heading": -0.0071047467810986895, - "angularVelocity": -1.2050473969635673, - "velocityX": 1.9698267749421239, - "velocityY": 0.5080296838996932, - "timestamp": 4.684931704967539 - }, - { - "x": 4.883529200739737, - "y": 6.4383664128247835, - "heading": -0.08508741675594196, - "angularVelocity": -1.30932091626408, - "velocityX": 2.3080775635429776, - "velocityY": 0.5584206500589359, - "timestamp": 4.744491335464662 - }, - { - "x": 5.041348217191184, - "y": 6.473264113474332, - "heading": -0.1656919745439482, - "angularVelocity": -1.3533421398895296, - "velocityX": 2.6497648681529427, - "velocityY": 0.5859287634639322, - "timestamp": 4.804050965961785 - }, - { - "x": 5.219569793721533, - "y": 6.507557049328585, - "heading": -0.2431024824486723, - "angularVelocity": -1.29971437462936, - "velocityX": 2.99232172937938, - "velocityY": 0.575774825465208, - "timestamp": 4.8636105964589085 - }, - { - "x": 5.417370624809519, - "y": 6.5372140981281, - "heading": -0.30621328049041796, - "angularVelocity": -1.0596237336427745, - "velocityX": 3.3210553765530904, - "velocityY": 0.49793876409201393, - "timestamp": 4.923170226956032 - }, - { - "x": 5.630089765221455, - "y": 6.556096742386712, - "heading": -0.3316159113808185, - "angularVelocity": -0.4265075299892522, - "velocityX": 3.5715322381358874, - "velocityY": 0.31703763272213936, - "timestamp": 4.982729857453155 + "angularVelocity": -4.3850016440748696e-23, + "velocityX": 2.0622476782033507e-21, + "velocityY": 1.8080615452809687e-23, + "timestamp": 4.2320699051573625 + }, + { + "x": 4.357336302646973, + "y": 6.297583243093535, + "heading": 0.2475212314024478, + "angularVelocity": -0.28561956492945584, + "velocityX": 0.3386404194139296, + "velocityY": 0.08867859433669868, + "timestamp": 4.289639136689636 + }, + { + "x": 4.39639255106342, + "y": 6.307690301692112, + "heading": 0.21545182729872558, + "angularVelocity": -0.5570580542792894, + "velocityX": 0.6784222644096215, + "velocityY": 0.17556354895775533, + "timestamp": 4.347208368221909 + }, + { + "x": 4.455088006420721, + "y": 6.322669942772804, + "heading": 0.16876628337274657, + "angularVelocity": -0.8109461023430008, + "velocityX": 1.0195629469953096, + "velocityY": 0.2602022066647703, + "timestamp": 4.4047775997541825 + }, + { + "x": 4.53351670800356, + "y": 6.342355632767599, + "heading": 0.10873910043109954, + "angularVelocity": -1.0426955744232154, + "velocityX": 1.362337128625251, + "velocityY": 0.3419481113580211, + "timestamp": 4.462346831286456 + }, + { + "x": 4.631792821732683, + "y": 6.3665238013399605, + "heading": 0.03701268299307073, + "angularVelocity": -1.245915839571676, + "velocityX": 1.7070944168158462, + "velocityY": 0.4198105121277204, + "timestamp": 4.519916062818729 + }, + { + "x": 4.7500563078189675, + "y": 6.394856912095635, + "heading": -0.0442182245974014, + "angularVelocity": -1.4110125396572948, + "velocityX": 2.0542828684448438, + "velocityY": 0.4921571819104624, + "timestamp": 4.5774852943510025 + }, + { + "x": 4.888478268982772, + "y": 6.426870152464857, + "heading": -0.13181305030525106, + "angularVelocity": -1.52155627887344, + "velocityX": 2.4044434410455024, + "velocityY": 0.5560824683107718, + "timestamp": 4.635054525883276 + }, + { + "x": 5.047249482849422, + "y": 6.461748968300146, + "heading": -0.22073086511138704, + "angularVelocity": -1.5445371153910277, + "velocityX": 2.757917895388995, + "velocityY": 0.6058586315458515, + "timestamp": 4.692623757415549 + }, + { + "x": 5.226429049191539, + "y": 6.497909114202001, + "heading": -0.3013203844421244, + "angularVelocity": -1.39987137548568, + "velocityX": 3.1124189358280567, + "velocityY": 0.6281158344381212, + "timestamp": 4.7501929889478225 + }, + { + "x": 5.423903550460318, + "y": 6.531166211616276, + "heading": -0.3435648346153767, + "angularVelocity": -0.7338025721182303, + "velocityX": 3.430209089347909, + "velocityY": 0.5776887502073221, + "timestamp": 4.807762220480096 + }, + { + "x": 5.636616262089249, + "y": 6.55438565863319, + "heading": -0.34498723844741225, + "angularVelocity": -0.024707709208140263, + "velocityX": 3.69490274522221, + "velocityY": 0.4033308487694046, + "timestamp": 4.865331452012369 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -0.3344741491569628, - "angularVelocity": -0.04798951491618805, - "velocityX": 3.7098016045994533, - "velocityY": 0.017297899702458933, - "timestamp": 5.042289487950278 - }, - { - "x": 5.971113356144086, - "y": 6.5517128681904815, - "heading": -0.33447429817496127, - "angularVelocity": -0.000004618408354033321, - "velocityX": 3.7212182481092846, - "velocityY": -0.167796284707936, - "timestamp": 5.074555582043538 - }, - { - "x": 6.090763237120761, - "y": 6.540318338833946, - "heading": -0.33447432111787584, - "angularVelocity": -7.11053358628561e-7, - "velocityX": 3.70822327086892, - "velocityY": -0.3531425069176937, - "timestamp": 5.106821676136798 - }, - { - "x": 6.209696100342635, - "y": 6.522971764048271, - "heading": -0.3344743416762489, - "angularVelocity": -6.371509664508709e-7, - "velocityX": 3.686001251905956, - "velocityY": -0.537609998146539, - "timestamp": 5.139087770230058 - }, - { - "x": 6.327616045254045, - "y": 6.499716303685921, - "heading": -0.3344743603957426, - "angularVelocity": -5.80159891848438e-7, - "velocityX": 3.6546085984432737, - "velocityY": -0.7207398669059114, - "timestamp": 5.171353864323319 - }, - { - "x": 6.444229692072682, - "y": 6.470609819762047, - "heading": -0.3344743776866033, - "angularVelocity": -5.358832938629608e-7, - "velocityX": 3.6141234350083242, - "velocityY": -0.9020764595722645, - "timestamp": 5.203619958416579 - }, - { - "x": 6.559246912194348, - "y": 6.435724733351891, - "heading": -0.3344743938681527, - "angularVelocity": -5.015031984210286e-7, - "velocityX": 3.5646465230413518, - "velocityY": -1.081168557598769, - "timestamp": 5.235886052509839 - }, - { - "x": 6.67238155121755, - "y": 6.395147846497736, - "heading": -0.33447440919657534, - "angularVelocity": -4.750628499499889e-7, - "velocityX": 3.506301032167155, - "velocityY": -1.2575704619491082, - "timestamp": 5.268152146603099 - }, - { - "x": 6.783352144572337, - "y": 6.348980132760865, - "heading": -0.33447442388388354, - "angularVelocity": -4.551932493526412e-7, - "velocityX": 3.439232310983837, - "velocityY": -1.4308429648605667, - "timestamp": 5.300418240696359 - }, - { - "x": 6.891882636447486, - "y": 6.297336519628137, - "heading": -0.3344744381114049, - "angularVelocity": -4.409434049866837e-7, - "velocityX": 3.363607989286128, - "velocityY": -1.6005536022878626, - "timestamp": 5.33268433478962 - }, - { - "x": 6.998345027127988, - "y": 6.24155399695869, - "heading": -0.33447445218138366, - "angularVelocity": -4.360607978051808e-7, - "velocityX": 3.299512806625678, - "velocityY": -1.7288278682946643, - "timestamp": 5.36495042888288 - }, - { - "x": 7.107456437455107, - "y": 6.19114935204841, - "heading": -0.33447450198371365, - "angularVelocity": -0.00000154348803003307, - "velocityX": 3.381611979799848, - "velocityY": -1.5621551454164793, - "timestamp": 5.39721652297614 + "heading": -0.34498726323171464, + "angularVelocity": -4.305129971148693e-7, + "velocityX": 3.7246965125733036, + "velocityY": 0.047618149403998285, + "timestamp": 4.9229006835446425 + }, + { + "x": 5.970747823515829, + "y": 6.552244502208151, + "heading": -0.34498728566710496, + "angularVelocity": -6.97576178834182e-7, + "velocityX": 3.7219058963692446, + "velocityY": -0.15180985637112251, + "timestamp": 4.955062605108422 + }, + { + "x": 6.090018554613485, + "y": 6.540962017726213, + "heading": -0.3449873062008861, + "angularVelocity": -6.384500714125073e-7, + "velocityX": 3.7084454316926947, + "velocityY": -0.35080256195400317, + "timestamp": 4.987224526672201 + }, + { + "x": 6.208514441050117, + "y": 6.52331189505608, + "heading": -0.34498732528503373, + "angularVelocity": -5.933770972523468e-7, + "velocityX": 3.68435344267748, + "velocityY": -0.5487894320969752, + "timestamp": 5.019386448235981 + }, + { + "x": 6.32589577532509, + "y": 6.499344741803893, + "heading": -0.34498734327209396, + "angularVelocity": -5.592657200372931e-7, + "velocityX": 3.6496990405936383, + "velocityY": -0.7452027766642643, + "timestamp": 5.05154836979976 + }, + { + "x": 6.441826048149789, + "y": 6.4691292818938315, + "heading": -0.3449873604472264, + "angularVelocity": -5.340207168118268e-7, + "velocityX": 3.6045816663908092, + "velocityY": -0.9394793109653794, + "timestamp": 5.0837102913635395 + }, + { + "x": 6.555972918049828, + "y": 6.432752169034089, + "heading": -0.344987377050145, + "angularVelocity": -5.162290595234125e-7, + "velocityX": 3.549130908539721, + "velocityY": -1.1310615501503813, + "timestamp": 5.115872212927319 + }, + { + "x": 6.668009186278295, + "y": 6.390317785626105, + "heading": -0.34498739329087524, + "angularVelocity": -5.049676592749214e-7, + "velocityX": 3.483506668166316, + "velocityY": -1.319398261818203, + "timestamp": 5.148034134491098 + }, + { + "x": 6.777613987715612, + "y": 6.341948479079241, + "heading": -0.3449874093618285, + "angularVelocity": -4.996888394768407e-7, + "velocityX": 3.4079058746525486, + "velocityY": -1.5039308659137305, + "timestamp": 5.180196056054878 + }, + { + "x": 6.886436695218618, + "y": 6.291844380961981, + "heading": -0.3449874254078226, + "angularVelocity": -4.989127946142238e-7, + "velocityX": 3.3835884863782986, + "velocityY": -1.55787016698924, + "timestamp": 5.212357977618657 + }, + { + "x": 6.995259275146381, + "y": 6.241740005760864, + "heading": -0.3449874414538154, + "angularVelocity": -4.98912750035862e-7, + "velocityX": 3.3835845197234478, + "velocityY": -1.5578787822660598, + "timestamp": 5.244519899182436 + }, + { + "x": 7.1040824430518015, + "y": 6.191636907618568, + "heading": -0.344987457499818, + "angularVelocity": -4.989130590435776e-7, + "velocityX": 3.3836028015184674, + "velocityY": -1.5578390750981381, + "timestamp": 5.276681820746216 }, { "x": 7.214789390563965, "y": 6.145846366882324, "heading": -0.344987478573796, - "angularVelocity": -0.325821171899398, - "velocityX": 3.32649352594794, - "velocityY": -1.404043050117709, - "timestamp": 5.4294826170694 - }, - { - "x": 7.408281473848404, - "y": 6.071835314929934, - "heading": -0.36045376555379516, - "angularVelocity": -0.24376616005736862, - "velocityX": 3.0496538829742375, - "velocityY": -1.1664978129250685, - "timestamp": 5.492929844430077 - }, - { - "x": 7.581043516745906, - "y": 6.008144867615469, - "heading": -0.3717285153471776, - "angularVelocity": -0.17770279746487722, - "velocityX": 2.722925021063643, - "velocityY": -1.003833421315721, - "timestamp": 5.556377071790754 - }, - { - "x": 7.732516109743219, - "y": 5.95357268553042, - "heading": -0.3790773339263405, - "angularVelocity": -0.11582568513809473, - "velocityX": 2.3873792330788404, - "velocityY": -0.8601192574550642, - "timestamp": 5.619824299151431 - }, - { - "x": 7.862474679773266, - "y": 5.907578864525407, - "heading": -0.3826203402067485, - "angularVelocity": -0.05584178265611413, - "velocityX": 2.048293919784284, - "velocityY": -0.7249145930925164, - "timestamp": 5.683271526512108 - }, - { - "x": 7.970798961808127, - "y": 5.869857366205246, - "heading": -0.38242588059176913, - "angularVelocity": 0.003064903275819629, - "velocityX": 1.7073130937475338, - "velocityY": -0.5945334396683074, - "timestamp": 5.746718753872785 - }, - { - "x": 8.057414200976034, - "y": 5.840211247118204, - "heading": -0.378538126264348, - "angularVelocity": 0.061275401450096946, - "velocityX": 1.3651540464570049, - "velocityY": -0.46725633759397367, - "timestamp": 5.810165981233462 - }, - { - "x": 8.122269484577586, - "y": 5.818503192947792, - "heading": -0.37098804864933244, - "angularVelocity": 0.11899775497037605, - "velocityX": 1.0221925574914426, - "velocityY": -0.3421434643157637, - "timestamp": 5.873613208594139 - }, - { - "x": 8.165327926639057, - "y": 5.804632020140207, - "heading": -0.3597986070827489, - "angularVelocity": 0.1763582434103087, - "velocityX": 0.6786497038979674, - "velocityY": -0.2186253581851867, - "timestamp": 5.937060435954816 + "angularVelocity": -6.552462348846214e-7, + "velocityX": 3.4421745383783664, + "velocityY": -1.4237501526591758, + "timestamp": 5.308843742309995 + }, + { + "x": 7.410548108371569, + "y": 6.0752735070916195, + "heading": -0.3626370424214939, + "angularVelocity": -0.2874228799686078, + "velocityX": 3.1879277548585048, + "velocityY": -1.1492779529116102, + "timestamp": 5.370250001441577 + }, + { + "x": 7.584442413587634, + "y": 6.012933959955544, + "heading": -0.37637990938116833, + "angularVelocity": -0.22380238031152527, + "velocityX": 2.831866126927586, + "velocityY": -1.0151985810191508, + "timestamp": 5.431656260573159 + }, + { + "x": 7.736397172033548, + "y": 5.958610527915441, + "heading": -0.3857543231283157, + "angularVelocity": -0.15266218590290268, + "velocityX": 2.47458094003583, + "velocityY": -0.8846562680800549, + "timestamp": 5.493062519704741 + }, + { + "x": 7.86638729375416, + "y": 5.912228690109332, + "heading": -0.3906020583128712, + "angularVelocity": -0.07894529406469342, + "velocityX": 2.1168871636044173, + "velocityY": -0.7553275262497595, + "timestamp": 5.5544687788363225 + }, + { + "x": 7.974400232833448, + "y": 5.873750789689322, + "heading": -0.3908432822952403, + "angularVelocity": -0.003928328899700405, + "velocityX": 1.7589890771205783, + "velocityY": -0.6266120256171138, + "timestamp": 5.615875037967904 + }, + { + "x": 8.060428468071539, + "y": 5.84315412501972, + "heading": -0.3864300383007793, + "angularVelocity": 0.07186961161409067, + "velocityX": 1.4009685080107037, + "velocityY": -0.498266220777903, + "timestamp": 5.677281297099486 + }, + { + "x": 8.124466995265989, + "y": 5.820423539830739, + "heading": -0.377330476340927, + "angularVelocity": 0.14818622870925488, + "velocityX": 1.042866445539805, + "velocityY": -0.37016723556265896, + "timestamp": 5.738687556231068 + }, + { + "x": 8.166512252703873, + "y": 5.805548220947723, + "heading": -0.36352195759750816, + "angularVelocity": 0.2248715186155497, + "velocityX": 0.6847063806278847, + "velocityY": -0.242244342732895, + "timestamp": 5.80009381536265 }, { "x": 8.186561584472656, "y": 5.798520088195801, "heading": -0.344987478573796, - "angularVelocity": 0.23344012220985313, - "velocityX": 0.3346664419690626, - "velocityY": -0.09633095406457304, - "timestamp": 6.000507663315493 - }, - { - "x": 8.184449385960491, - "y": 5.800720366343895, - "heading": -0.3251687978756523, - "angularVelocity": 0.29387941781314064, - "velocityX": -0.03132053432391204, - "velocityY": 0.03262661481041375, - "timestamp": 6.0679458011940195 - }, - { - "x": 8.157654760081511, - "y": 5.811617201116665, - "heading": -0.30136325235519174, - "angularVelocity": 0.3529982628426134, - "velocityX": -0.39732155604955366, - "velocityY": 0.1615826758502398, - "timestamp": 6.135383939072546 - }, - { - "x": 8.106176542546697, - "y": 5.831210437992555, - "heading": -0.27368179509987206, - "angularVelocity": 0.41047185058966307, - "velocityX": -0.7633398423239571, - "velocityY": 0.29053644558191305, - "timestamp": 6.2028220769510725 - }, - { - "x": 8.03001326746699, - "y": 5.859499833662039, - "heading": -0.24226677680218867, - "angularVelocity": 0.46583460466049514, - "velocityX": -1.129379865394508, - "velocityY": 0.4194866074214659, - "timestamp": 6.270260214829599 - }, - { - "x": 7.929163035221131, - "y": 5.896484993829156, - "heading": -0.2073080079914019, - "angularVelocity": 0.5183827713890392, - "velocityX": -1.495448056817871, - "velocityY": 0.5484309224809322, - "timestamp": 6.337698352708125 - }, - { - "x": 7.80362329670357, - "y": 5.942165255300411, - "heading": -0.16907207724227352, - "angularVelocity": 0.5669778548452968, - "velocityX": -1.8615540474099375, - "velocityY": 0.677365403438884, - "timestamp": 6.405136490586652 - }, - { - "x": 7.653390486781624, - "y": 5.996539431834676, - "heading": -0.12796178084950977, - "angularVelocity": 0.6096001118360348, - "velocityX": -2.227712903232215, - "velocityY": 0.8062822943333132, - "timestamp": 6.472574628465178 - }, - { - "x": 7.478459409616276, - "y": 6.059605150575439, - "heading": -0.0846568853073795, - "angularVelocity": 0.6421425161550796, - "velocityX": -2.593948805058106, - "velocityY": 0.9351639995511196, - "timestamp": 6.540012766343705 - }, - { - "x": 7.278822723764661, - "y": 6.131356463760539, - "heading": -0.04054583544960124, - "angularVelocity": 0.6540964985900647, - "velocityX": -2.9602935687698184, - "velocityY": 1.063957508944601, - "timestamp": 6.607450904222231 - }, - { - "x": 7.05448611686489, - "y": 6.21176649036125, - "heading": -5.1297661467442016e-8, - "angularVelocity": 0.6012292958766616, - "velocityX": -3.3265539938819106, - "velocityY": 1.1923524155656642, - "timestamp": 6.674889042100758 - }, - { - "x": 6.817570799613061, - "y": 6.29528987249135, - "heading": -4.834198432704978e-8, - "angularVelocity": 4.3827976770594794e-8, - "velocityX": -3.5130762014599934, - "velocityY": 1.238518511299158, - "timestamp": 6.742327179979284 - }, - { - "x": 6.580655475027505, - "y": 6.378813233819268, - "heading": -4.538633792987658e-8, - "angularVelocity": 4.382752089770155e-8, - "velocityX": -3.5130763102074556, - "velocityY": 1.23851820283599, - "timestamp": 6.809765317857811 - }, - { - "x": 6.3437395023630385, - "y": 6.4623347568384295, - "heading": -4.243069068525663e-8, - "angularVelocity": 4.382753346398488e-8, - "velocityX": -3.5130859201838094, - "velocityY": 1.2384909436497944, - "timestamp": 6.877203455736337 - }, - { - "x": 6.09986574355872, - "y": 6.5225893218228235, - "heading": -3.943141637637637e-8, - "angularVelocity": 4.447445322827149e-8, - "velocityX": -3.6162587888117317, - "velocityY": 0.8934790740059899, - "timestamp": 6.944641593614864 + "angularVelocity": 0.30183371020853555, + "velocityX": 0.3265030642205698, + "velocityY": -0.11445303542856088, + "timestamp": 5.861500074494232 + }, + { + "x": 8.182873429152231, + "y": 5.799961021839805, + "heading": -0.3197766551885773, + "angularVelocity": 0.3842915682549792, + "velocityX": -0.056218988582698255, + "velocityY": 0.021964322281146666, + "timestamp": 5.927103448254676 + }, + { + "x": 8.154073481017848, + "y": 5.810352783086787, + "heading": -0.28949615112813754, + "angularVelocity": 0.46156931152673975, + "velocityX": -0.43900102210518965, + "velocityY": 0.15840284807498262, + "timestamp": 5.99270682201512 + }, + { + "x": 8.100157139869356, + "y": 5.829696916957608, + "heading": -0.2545716989464078, + "angularVelocity": 0.5323575630311173, + "velocityX": -0.8218531770236625, + "velocityY": 0.29486492480488535, + "timestamp": 6.058310195775564 + }, + { + "x": 8.021119019409117, + "y": 5.8579951101346595, + "heading": -0.2155530876775793, + "angularVelocity": 0.594765315139245, + "velocityX": -1.204787435915305, + "velocityY": 0.43135271183437995, + "timestamp": 6.1239135695360085 + }, + { + "x": 7.9169528918289975, + "y": 5.89524913213786, + "heading": -0.17317932530550872, + "angularVelocity": 0.6459082809795952, + "velocityX": -1.5878166260242859, + "velocityY": 0.567867471865644, + "timestamp": 6.189516943296453 + }, + { + "x": 7.787651961744212, + "y": 5.941460642017215, + "heading": -0.12849929599301224, + "angularVelocity": 0.6810629812370456, + "velocityX": -1.9709493989888005, + "velocityY": 0.7044075209927304, + "timestamp": 6.255120317056897 + }, + { + "x": 7.633210550125228, + "y": 5.99663052819492, + "heading": -0.08312171419135361, + "angularVelocity": 0.6916958564868655, + "velocityX": -2.3541687380734118, + "velocityY": 0.8409611124446534, + "timestamp": 6.320723690817341 + }, + { + "x": 7.453632499858265, + "y": 6.060756206322318, + "heading": -0.039828348341138756, + "angularVelocity": 0.659925905766733, + "velocityX": -2.7373294995880113, + "velocityY": 0.9774753103637095, + "timestamp": 6.386327064577785 + }, + { + "x": 7.248984054172477, + "y": 6.133814732067644, + "heading": -0.004587896529415136, + "angularVelocity": 0.5371743828359606, + "velocityX": -3.119480507711035, + "velocityY": 1.1136397651149037, + "timestamp": 6.45193043833823 + }, + { + "x": 7.020152924958556, + "y": 6.215343825965371, + "heading": -7.99601094738583e-8, + "angularVelocity": 0.06993263160608797, + "velocityX": -3.488100018293493, + "velocityY": 1.2427576391945405, + "timestamp": 6.517533812098674 + }, + { + "x": 6.789969979123711, + "y": 6.297403529442868, + "heading": -7.492709232609331e-8, + "angularVelocity": 7.671887677824965e-8, + "velocityX": -3.508705919231764, + "velocityY": 1.2508457838943434, + "timestamp": 6.583137185859118 + }, + { + "x": 6.559787033588477, + "y": 6.379463233760796, + "heading": -6.989407741573959e-8, + "angularVelocity": 7.671884267312441e-8, + "velocityX": -3.5087059146647626, + "velocityY": 1.2508457967051358, + "timestamp": 6.648740559619562 + }, + { + "x": 6.3296038306461995, + "y": 6.461522216031207, + "heading": -6.486106191272836e-8, + "angularVelocity": 7.671885170707308e-8, + "velocityX": -3.508709838350823, + "velocityY": 1.250834790449275, + "timestamp": 6.714343933380007 + }, + { + "x": 6.0929382858404795, + "y": 6.522410665825721, + "heading": -5.975605523435043e-8, + "angularVelocity": 7.78162217238893e-8, + "velocityX": -3.607520943510018, + "velocityY": 0.9281298552853697, + "timestamp": 6.779947307140451 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -3.6277001373267394e-8, - "angularVelocity": 4.677494222617604e-8, - "velocityX": -3.689626869559746, - "velocityY": 0.5121386527717335, - "timestamp": 7.01207973149339 - }, - { - "x": 5.675146419069591, - "y": 6.5685334564190505, - "heading": -3.325736766733182e-8, - "angularVelocity": 6.38129937324226e-8, - "velocityX": -3.7171934353117426, - "velocityY": 0.24104917118070399, - "timestamp": 7.059399776037881 - }, - { - "x": 5.498885444409021, - "y": 6.56705094919282, - "heading": -3.0392937858444436e-8, - "angularVelocity": 6.053311733876744e-8, - "velocityX": -3.724869161837878, - "velocityY": -0.03132937089348242, - "timestamp": 7.106719820582372 - }, - { - "x": 5.3232044198709785, - "y": 6.552687568221491, - "heading": -2.7611010604394772e-8, - "angularVelocity": 5.878961613051839e-8, - "velocityX": -3.7126132536258285, - "velocityY": -0.3035369283692009, - "timestamp": 7.154039865126863 - }, - { - "x": 5.148803896994517, - "y": 6.527102675424277, - "heading": -2.484990875079416e-8, - "angularVelocity": 5.8349519324830205e-8, - "velocityX": -3.6855528044249266, - "velocityY": -0.5406776989222635, - "timestamp": 7.201359909671353 - }, - { - "x": 4.974403597324897, - "y": 6.501516261174204, - "heading": -2.208880782591367e-8, - "angularVelocity": 5.834949969847243e-8, - "velocityX": -3.6855480874631454, - "velocityY": -0.5407098513192842, - "timestamp": 7.248679954215844 - }, - { - "x": 4.800003297129097, - "y": 6.475929850510635, - "heading": -1.9327706889009288e-8, - "angularVelocity": 5.8349499952569724e-8, - "velocityX": -3.6855480985827302, - "velocityY": -0.5407097755267535, - "timestamp": 7.295999998760335 - }, - { - "x": 4.625602997244748, - "y": 6.450343437724181, - "heading": -1.656660593755509e-8, - "angularVelocity": 5.8349500260046496e-8, - "velocityX": -3.685548092000944, - "velocityY": -0.5407098203890633, - "timestamp": 7.343320043304826 - }, - { - "x": 4.451202697255753, - "y": 6.4247570256510045, - "heading": -1.3805504990627123e-8, - "angularVelocity": 5.834950016439505e-8, - "velocityX": -3.685548094212386, - "velocityY": -0.5407098053155895, - "timestamp": 7.390640087849317 - }, - { - "x": 4.276802397278509, - "y": 6.3991706134977395, - "heading": -1.1044403987822918e-8, - "angularVelocity": 5.8349501345210454e-8, - "velocityX": -3.685548093964082, - "velocityY": -0.5407098070080617, - "timestamp": 7.437960132393807 - }, - { - "x": 4.102402097300047, - "y": 6.373584201352764, - "heading": -8.283303038922857e-9, - "angularVelocity": 5.834950020607066e-8, - "velocityX": -3.685548093989784, - "velocityY": -0.5407098068328745, - "timestamp": 7.485280176938298 - }, - { - "x": 3.9280017973350696, - "y": 6.347997789115884, - "heading": -5.522202092459704e-9, - "angularVelocity": 5.8349500154572286e-8, - "velocityX": -3.685548093704841, - "velocityY": -0.5407098087750815, - "timestamp": 7.532600221482789 - }, - { - "x": 3.7536014973495653, - "y": 6.322411377018916, - "heading": -2.7611011159009457e-9, - "angularVelocity": 5.834950079057347e-8, - "velocityX": -3.6855480941386265, - "velocityY": -0.5407098058183458, - "timestamp": 7.57992026602728 - }, - { - "x": 3.579201219971439, - "y": 6.296824810827607, - "heading": -5.947116132480024e-26, - "angularVelocity": 5.8349503735249215e-8, - "velocityX": -3.6855476163838365, - "velocityY": -0.5407130622468497, - "timestamp": 7.6272403105717705 + "heading": -5.43646913463662e-8, + "angularVelocity": 8.218119860224255e-8, + "velocityX": -3.6872205492775545, + "velocityY": 0.5291851788357008, + "timestamp": 6.845550680900895 + }, + { + "x": 5.661580826065391, + "y": 6.5680717657534915, + "heading": -4.9482596020305806e-8, + "angularVelocity": 9.582614101786845e-8, + "velocityX": -3.718801184433256, + "velocityY": 0.21482472211899678, + "timestamp": 6.896498107920314 + }, + { + "x": 5.471871496557958, + "y": 6.562921997343888, + "heading": -4.486806959492394e-8, + "angularVelocity": 9.057427814015043e-8, + "velocityX": -3.7236292508966913, + "velocityY": -0.10108004880484801, + "timestamp": 6.947445534939733 + }, + { + "x": 5.28328089006521, + "y": 6.541715087274231, + "heading": -4.036796232249368e-8, + "angularVelocity": 8.83284502417564e-8, + "velocityX": -3.7016708698726792, + "velocityY": -0.41625085525070266, + "timestamp": 6.998392961959151 + }, + { + "x": 5.095501405649377, + "y": 6.514237548522189, + "heading": -3.588263316902823e-8, + "angularVelocity": 8.803838419074343e-8, + "velocityX": -3.685750103616821, + "velocityY": -0.5393312353451875, + "timestamp": 7.04934038897857 + }, + { + "x": 4.907721944965666, + "y": 6.486759847587045, + "heading": -3.139730406205809e-8, + "angularVelocity": 8.803838327812946e-8, + "velocityX": -3.685749637800889, + "velocityY": -0.5393344186875336, + "timestamp": 7.100287815997989 + }, + { + "x": 4.719942484237288, + "y": 6.459282146957154, + "heading": -2.6911974883170177e-8, + "angularVelocity": 8.803838468973724e-8, + "velocityX": -3.6857496386776267, + "velocityY": -0.5393344126960062, + "timestamp": 7.151235243017408 + }, + { + "x": 4.532163023549714, + "y": 6.431804446048415, + "heading": -2.2426645714855718e-8, + "angularVelocity": 8.803838448220064e-8, + "velocityX": -3.685749637876727, + "velocityY": -0.5393344181692645, + "timestamp": 7.2021826700368266 + }, + { + "x": 4.34438356286167, + "y": 6.404326745142886, + "heading": -1.794131654726717e-8, + "angularVelocity": 8.803838446795243e-8, + "velocityX": -3.6857496378859507, + "velocityY": -0.5393344181062291, + "timestamp": 7.253130097056245 + }, + { + "x": 4.156604102201168, + "y": 6.376849044049133, + "heading": -1.3455987422255126e-8, + "angularVelocity": 8.803838363225754e-8, + "velocityX": -3.685749637345336, + "velocityY": -0.5393344218007294, + "timestamp": 7.304077524075664 + }, + { + "x": 3.9688246409539834, + "y": 6.349371346964709, + "heading": -8.97065828327331e-9, + "angularVelocity": 8.80383839064573e-8, + "velocityX": -3.685749648860808, + "velocityY": -0.5393343431053033, + "timestamp": 7.355024951095083 + }, + { + "x": 3.7810451818268453, + "y": 6.32189363539213, + "heading": -4.485329161078249e-9, + "angularVelocity": 8.80383835769656e-8, + "velocityX": -3.685749607248367, + "velocityY": -0.5393346274799247, + "timestamp": 7.405972378114502 + }, + { + "x": 3.5932657187551267, + "y": 6.294415950776365, + "heading": 7.629972110817107e-24, + "angularVelocity": 8.803838434016792e-8, + "velocityX": -3.6857496846728943, + "velocityY": -0.5393340983695197, + "timestamp": 7.4569198051339205 }, { "x": 3.405486822128296, "y": 6.266934394836426, - "heading": -3.242812966980484e-26, - "angularVelocity": 4.141833469401844e-27, - "velocityX": -3.6710531343607253, - "velocityY": -0.6316650011408507, - "timestamp": 7.674560355116261 - }, - { - "x": 3.234879221757396, - "y": 6.227705290951873, - "heading": -3.799073171453003e-26, - "angularVelocity": 3.7193578167117204e-28, - "velocityX": -3.4320264149158155, - "velocityY": -0.7891519514521484, - "timestamp": 7.724270811780997 - }, - { - "x": 3.075654096212167, - "y": 6.19701582942553, - "heading": -4.3640191392241984e-26, - "angularVelocity": -1.3753350907836377e-27, - "velocityX": -3.2030509520179686, - "velocityY": -0.6173643049252793, - "timestamp": 7.773981268445732 - }, - { - "x": 2.9276929132783946, - "y": 6.175021768989464, - "heading": -4.923363376741485e-26, - "angularVelocity": -2.4846347668894466e-28, - "velocityX": -2.9764599414500257, - "velocityY": -0.4424433391228337, - "timestamp": 7.823691725110468 - }, - { - "x": 2.790955431257819, - "y": 6.16177499390257, - "heading": -5.486689770041805e-26, - "angularVelocity": -1.0495335231555751e-27, - "velocityX": -2.7506784526800994, - "velocityY": -0.2664786440453503, - "timestamp": 7.873402181775203 - }, - { - "x": 2.6654213921010017, - "y": 6.157301437220893, - "heading": -6.044835484657667e-26, - "angularVelocity": -7.36271402334531e-30, - "velocityX": -2.525304484798085, - "velocityY": -0.08999226685541546, - "timestamp": 7.923112638439939 - }, - { - "x": 2.551078597067916, - "y": 6.16161665554964, - "heading": -6.59367177391361e-26, - "angularVelocity": 1.865367086612129e-27, - "velocityX": -2.300175912771294, - "velocityY": 0.08680705465754052, - "timestamp": 7.972823095104674 - }, - { - "x": 2.447918895369922, - "y": 6.174731018536324, - "heading": -7.161083135132275e-26, - "angularVelocity": -1.871285764514843e-27, - "velocityX": -2.075211306018359, - "velocityY": 0.2638149771009456, - "timestamp": 8.02253355176941 - }, - { - "x": 2.355936456054869, - "y": 6.196651932350337, - "heading": -7.721743669966212e-26, - "angularVelocity": -5.132563200735982e-28, - "velocityX": -1.8503639975672366, - "velocityY": 0.44097188569106954, - "timestamp": 8.072244008434145 - }, - { - "x": 2.2751269010087887, - "y": 6.227384951214515, - "heading": -8.282082994441975e-26, - "angularVelocity": -4.486400643129525e-28, - "velocityX": -1.6256047614104199, - "velocityY": 0.618240525760057, - "timestamp": 8.12195446509888 + "heading": 3.646452356482733e-24, + "angularVelocity": -1.9320923857305114e-25, + "velocityX": -3.6857385664492472, + "velocityY": -0.5394100850169479, + "timestamp": 7.507867232153339 + }, + { + "x": 3.2165781142286494, + "y": 6.223116050022905, + "heading": 3.318409186414545e-24, + "angularVelocity": -1.5338201174379227e-26, + "velocityX": -3.536673395714516, + "velocityY": -0.8203495543918979, + "timestamp": 7.561281468290765 + }, + { + "x": 3.041563345872727, + "y": 6.190252158258573, + "heading": 2.990185107220459e-24, + "angularVelocity": -1.87251095899189e-26, + "velocityX": -3.2765566075987578, + "velocityY": -0.6152646586535303, + "timestamp": 7.6146957044281915 + }, + { + "x": 2.8804424282697485, + "y": 6.168342842771101, + "heading": 2.6619349218872592e-24, + "angularVelocity": -1.9213858301671557e-26, + "velocityX": -3.0164414817885197, + "velocityY": -0.4101774558958965, + "timestamp": 7.668109940565618 + }, + { + "x": 2.733215332226118, + "y": 6.157388144124356, + "heading": 2.333764868738876e-24, + "angularVelocity": -1.7713655535187625e-26, + "velocityX": -2.7563269025291364, + "velocityY": -0.20508949371775206, + "timestamp": 7.721524176703044 + }, + { + "x": 2.5998820432630403, + "y": 6.157388082450519, + "heading": 2.0056312293962657e-24, + "angularVelocity": -1.7031930803836163e-26, + "velocityX": -2.4962125943359705, + "velocityY": -0.0000011546329394271484, + "timestamp": 7.77493841284047 + }, + { + "x": 2.4804425527424723, + "y": 6.16834266976643, + "heading": 1.6774969061310235e-24, + "angularVelocity": -1.7044734930100497e-26, + "velocityX": -2.2360984478608033, + "velocityY": 0.20508740942632245, + "timestamp": 7.828352648977896 + }, + { + "x": 2.3748968549301686, + "y": 6.190251914052291, + "heading": 1.3492831778827425e-24, + "angularVelocity": -1.853132331504253e-26, + "velocityX": -1.97598440873988, + "velocityY": 0.4101761228877669, + "timestamp": 7.881766885115322 + }, + { + "x": 2.283244945743567, + "y": 6.22311582099134, + "heading": 1.0211293460600033e-24, + "angularVelocity": -1.7409966353369085e-26, + "velocityX": -1.7158704460510679, + "velocityY": 0.6152649427485215, + "timestamp": 7.935181121252748 }, { "x": 2.205486822128296, "y": 6.266934394836426, - "heading": -8.869624024083721e-26, - "angularVelocity": -5.920668887028623e-27, - "velocityX": -1.400914084337811, - "velocityY": 0.7955960631914043, - "timestamp": 8.171664921763616 - }, - { - "x": 2.1363371710210255, - "y": 6.327839189643039, - "heading": 0.025262557050058088, - "angularVelocity": 0.415801673405611, - "velocityX": -1.1381484696439823, - "velocityY": 1.002444667372145, - "timestamp": 8.23242118770922 - }, - { - "x": 2.0845111064453854, - "y": 6.399393278580984, - "heading": 0.0757412033730169, - "angularVelocity": 0.8308385240158422, - "velocityX": -0.8530159608893926, - "velocityY": 1.177723611289897, - "timestamp": 8.293177453654822 - }, - { - "x": 2.051957537582464, - "y": 6.477853810368175, - "heading": 0.14980799257498115, - "angularVelocity": 1.2190806668118568, - "velocityX": -0.5358059511436571, - "velocityY": 1.291398188582541, - "timestamp": 8.353933719600425 - }, - { - "x": 2.0403269766878736, - "y": 6.554154175481095, - "heading": 0.23625288977037628, - "angularVelocity": 1.422814517152715, - "velocityX": -0.19142981737889736, - "velocityY": 1.255843556633874, - "timestamp": 8.414689985546028 - }, - { - "x": 2.0424863781029257, - "y": 6.6144252836562565, - "heading": 0.31360709056854197, - "angularVelocity": 1.2731888570542382, - "velocityX": 0.03554203638823312, - "velocityY": 0.9920146874912401, - "timestamp": 8.475446251491631 - }, - { - "x": 2.048130429243297, - "y": 6.654505373712402, - "heading": 0.3666123519116142, - "angularVelocity": 0.8724246053983875, - "velocityX": 0.09289660996323934, - "velocityY": 0.659686526687304, - "timestamp": 8.536202517437234 + "heading": 6.9419459427461045e-25, + "angularVelocity": 5.413163312168308e-27, + "velocityX": -1.455756540544968, + "velocityY": 0.8203538422294017, + "timestamp": 7.9885953573901745 + }, + { + "x": 2.136159340261779, + "y": 6.327632048563378, + "heading": 0.025231456124497797, + "angularVelocity": 0.43032288322850026, + "velocityX": -1.1823812996193095, + "velocityY": 1.035199444221815, + "timestamp": 8.047229134741727 + }, + { + "x": 2.084207176746768, + "y": 6.399028713640929, + "heading": 0.07562693390361644, + "angularVelocity": 0.8594956705068004, + "velocityX": -0.8860449703507888, + "velocityY": 1.2176712520067714, + "timestamp": 8.10586291209328 + }, + { + "x": 2.051566754965305, + "y": 6.477423275068575, + "heading": 0.14957404640599054, + "angularVelocity": 1.2611691731714223, + "velocityX": -0.5566829096778134, + "velocityY": 1.3370204849265277, + "timestamp": 8.164496689444832 + }, + { + "x": 2.039926841667985, + "y": 6.553838491753715, + "heading": 0.2360367067446735, + "angularVelocity": 1.4746220394479557, + "velocityX": -0.19851890536627906, + "velocityY": 1.3032627290405507, + "timestamp": 8.223130466796384 + }, + { + "x": 2.042236938900771, + "y": 6.614274854513032, + "heading": 0.313525546116211, + "angularVelocity": 1.3215733809359698, + "velocityX": 0.039398744838411756, + "velocityY": 1.0307431226365935, + "timestamp": 8.281764244147936 + }, + { + "x": 2.0480382687015357, + "y": 6.654459061696839, + "heading": 0.3665932060292166, + "angularVelocity": 0.9050697790597129, + "velocityX": 0.09894177149770778, + "velocityY": 0.6853422890166648, + "timestamp": 8.340398021499489 }, { "x": 2.052253484725952, "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": 0.4352739504266956, - "velocityX": 0.0678622265289755, - "velocityY": 0.3266075878021015, - "timestamp": 8.596958783382837 + "angularVelocity": 0.4513569988001917, + "velocityX": 0.07189057595834777, + "velocityY": 0.3392203330286158, + "timestamp": 8.399031798851041 }, { "x": 2.052253484725952, "y": 6.674348831176758, "heading": 0.3930579718029317, - "angularVelocity": -3.1389023173080445e-26, - "velocityX": -4.435068013070024e-22, - "velocityY": -1.8479842669198598e-22, - "timestamp": 8.65771504932844 + "angularVelocity": 7.733121458673518e-26, + "velocityX": -6.859499621919341e-22, + "velocityY": -2.8560302702061698e-22, + "timestamp": 8.457665576202594 }, { - "x": 2.103013204328941, - "y": 6.695498705652326, + "x": 2.103013204396169, + "y": 6.695498705680337, "heading": 0.3930579718029317, - "angularVelocity": 3.416438400428702e-17, - "velocityX": 0.5194333949867557, - "velocityY": 0.21643049229415012, - "timestamp": 8.755436374320036 + "angularVelocity": 4.836322083094603e-17, + "velocityX": 0.5390415085998568, + "velocityY": 0.22460053627515192, + "timestamp": 8.551832201333307 }, { - "x": 2.2045326425850984, - "y": 6.737798454207702, + "x": 2.204532642750927, + "y": 6.737798454276797, "heading": 0.3930579718029317, - "angularVelocity": 5.790454228147962e-17, - "velocityX": 1.0388667802538214, - "velocityY": 0.432860980538431, - "timestamp": 8.853157699311632 + "angularVelocity": 1.8057826879047683e-17, + "velocityX": 1.078083006732355, + "velocityY": 0.4492010681889061, + "timestamp": 8.64599882646402 }, { - "x": 2.356811797278175, - "y": 6.801248075919452, - "heading": 0.3930579718029318, - "angularVelocity": 1.3697605676063785e-16, - "velocityX": 1.558300142841611, - "velocityY": 0.6492914593330171, - "timestamp": 8.950879024303228 + "x": 2.356811797490316, + "y": 6.801248076007844, + "heading": 0.3930579718029317, + "angularVelocity": 9.246174062812876e-17, + "velocityX": 1.6171244804410174, + "velocityY": 0.6738015899260662, + "timestamp": 8.740165451594732 }, { - "x": 2.5598506573269275, - "y": 6.8858475661703915, - "heading": 0.3930579718029318, - "angularVelocity": 1.6886690536158693e-16, - "velocityX": 2.0777333920330334, - "velocityY": 0.8657218908791364, - "timestamp": 9.048600349294825 + "x": 2.5598506571147865, + "y": 6.885847566082, + "heading": 0.3930579718029317, + "angularVelocity": 1.8302909821567636e-16, + "velocityX": 2.1561658320305157, + "velocityY": 0.8984020607802621, + "timestamp": 8.834332076725445 }, { - "x": 2.712129812020004, - "y": 6.9492971878821415, - "heading": 0.3930579718029318, - "angularVelocity": 9.579438090360999e-17, - "velocityX": 1.5583001428416108, - "velocityY": 0.649291459333017, - "timestamp": 9.14632167428642 + "x": 2.7121298118541755, + "y": 6.9492971878130465, + "heading": 0.3930579718029317, + "angularVelocity": 1.1024323128215209e-16, + "velocityX": 1.6171244804410174, + "velocityY": 0.6738015899260661, + "timestamp": 8.928498701856158 }, { - "x": 2.8136492502761614, - "y": 6.991596936437518, + "x": 2.8136492502089334, + "y": 6.991596936409507, "heading": 0.3930579718029317, - "angularVelocity": -7.496030828989607e-17, - "velocityX": 1.0388667802538214, - "velocityY": 0.4328609805384309, - "timestamp": 9.244042999278017 + "angularVelocity": -3.9462401223913846e-17, + "velocityX": 1.078083006732355, + "velocityY": 0.44920106818890604, + "timestamp": 9.022665326986871 }, { "x": 2.8644089698791504, "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 4.893193347075666e-17, - "velocityX": 0.5194333949867557, - "velocityY": 0.21643049229415012, - "timestamp": 9.341764324269613 + "angularVelocity": 1.2061194980042807e-16, + "velocityX": 0.5390415085998568, + "velocityY": 0.22460053627515186, + "timestamp": 9.116831952117584 }, { "x": 2.8644089698791504, "y": 7.012746810913086, "heading": 0.3930579718029317, - "angularVelocity": 3.090778525044755e-27, - "velocityX": 4.437436909321685e-22, - "velocityY": 1.850515832937087e-22, - "timestamp": 9.43948564926121 - }, - { - "x": 2.8419574642644285, - "y": 7.0048102452086525, - "heading": 0.3900130033247021, - "angularVelocity": -0.04733865202910357, - "velocityX": -0.3490426976251426, - "velocityY": -0.12338594795790273, - "timestamp": 9.503808741442997 - }, - { - "x": 2.7972074454620057, - "y": 6.988518822273239, - "heading": 0.3838409042111465, - "angularVelocity": -0.0959546393713819, - "velocityX": -0.6957068959923765, - "velocityY": -0.25327487194445947, - "timestamp": 9.568131833624784 - }, - { - "x": 2.7303816098102875, - "y": 6.963303209303571, - "heading": 0.37443012101631223, - "angularVelocity": -0.14630489417762893, - "velocityX": -1.0389089421083304, - "velocityY": -0.39201493762775413, - "timestamp": 9.632454925806572 - }, - { - "x": 2.6418300370011316, - "y": 6.928345468623928, - "heading": 0.3616207312094327, - "angularVelocity": -0.19914138721251212, - "velocityX": -1.3766684685943704, - "velocityY": -0.5434710847054101, - "timestamp": 9.69677801798836 - }, - { - "x": 2.5321688819092247, - "y": 6.882378878870811, - "heading": 0.34516587333325927, - "angularVelocity": -0.25581571591224617, - "velocityX": -1.7048489332880077, - "velocityY": -0.7146203360872027, - "timestamp": 9.761101110170147 - }, - { - "x": 2.4026942269657687, - "y": 6.823220010782376, - "heading": 0.324643183634766, - "angularVelocity": -0.3190563295758964, - "velocityX": -2.012879831360391, - "velocityY": -0.9197143060417909, - "timestamp": 9.825424202351934 - }, - { - "x": 2.2570878780964114, - "y": 6.7465695928484575, - "heading": 0.2992393598461131, - "angularVelocity": -0.3949409601898093, - "velocityX": -2.2636714736575443, - "velocityY": -1.1916469705357338, - "timestamp": 9.889747294533722 - }, - { - "x": 2.110110870224978, - "y": 6.646173604450097, - "heading": 0.26804931760848005, - "angularVelocity": -0.4848964995259366, - "velocityX": -2.2849804461522467, - "velocityY": -1.5608078684187743, - "timestamp": 9.95407038671551 - }, - { - "x": 1.9823765062843866, - "y": 6.531738038725912, - "heading": 0.2347990669991598, - "angularVelocity": -0.5169255625234794, - "velocityX": -1.9858243689465778, - "velocityY": -1.779074385926154, - "timestamp": 10.018393478897297 - }, - { - "x": 1.8778139110395455, - "y": 6.411761042169391, - "heading": 0.201692518308331, - "angularVelocity": -0.5146914983077023, - "velocityX": -1.6255840896039286, - "velocityY": -1.865224330594162, - "timestamp": 10.082716571079084 - }, - { - "x": 1.796996456842112, - "y": 6.289847592193697, - "heading": 0.16955186256906168, - "angularVelocity": -0.49967522780830353, - "velocityX": -1.2564298676598118, - "velocityY": -1.8953294352072878, - "timestamp": 10.147039663260871 + "angularVelocity": -7.454320302880939e-28, + "velocityX": 6.858010204674672e-22, + "velocityY": 2.856861212118512e-22, + "timestamp": 9.210998577248297 + }, + { + "x": 2.842186915849872, + "y": 7.004763611912437, + "heading": 0.3899906597418949, + "angularVelocity": -0.04969534926930016, + "velocityX": -0.36003273044640205, + "velocityY": -0.1293405609631654, + "timestamp": 9.272720893443621 + }, + { + "x": 2.7978951293903465, + "y": 6.988387145564426, + "heading": 0.38377625605112037, + "angularVelocity": -0.10068325483944376, + "velocityX": -0.7175976079601634, + "velocityY": -0.26532488340499644, + "timestamp": 9.334443209638945 + }, + { + "x": 2.7317542660001495, + "y": 6.96306112048486, + "heading": 0.37430680047739806, + "angularVelocity": -0.15342028876161432, + "velocityX": -1.071587514326101, + "velocityY": -0.41032201383078476, + "timestamp": 9.396165525834268 + }, + { + "x": 2.644108850979856, + "y": 6.927989989672691, + "heading": 0.36142823816874126, + "angularVelocity": -0.20865325707968818, + "velocityX": -1.4199955611343862, + "velocityY": -0.5682082749646841, + "timestamp": 9.457887842029592 + }, + { + "x": 2.53555910261038, + "y": 6.881949867747595, + "heading": 0.3449043845685677, + "angularVelocity": -0.267712792045632, + "velocityX": -1.7586791141467173, + "velocityY": -0.7459234319625763, + "timestamp": 9.519610158224916 + }, + { + "x": 2.407346628684672, + "y": 6.822852176062135, + "heading": 0.3243353524397527, + "angularVelocity": -0.33325113827101066, + "velocityX": -2.0772466399344327, + "velocityY": -0.9574768953653868, + "timestamp": 9.58133247442024 + }, + { + "x": 2.2628964345938334, + "y": 6.74663530878728, + "heading": 0.298961324703692, + "angularVelocity": -0.41109973345399, + "velocityX": -2.3403236138079526, + "velocityY": -1.2348348534695668, + "timestamp": 9.643054790615563 + }, + { + "x": 2.1156790935491716, + "y": 6.646996400731561, + "heading": 0.26784048264776694, + "angularVelocity": -0.5042072944482706, + "velocityX": -2.385155809428528, + "velocityY": -1.6143092838642825, + "timestamp": 9.704777106810887 + }, + { + "x": 1.9869123530220612, + "y": 6.532612256734745, + "heading": 0.23448482199122617, + "angularVelocity": -0.540414921419745, + "velocityX": -2.0862266432066443, + "velocityY": -1.8532056320576245, + "timestamp": 9.766499423006211 + }, + { + "x": 1.881043758757729, + "y": 6.412413108766105, + "heading": 0.20123842984354556, + "angularVelocity": -0.5386445972388703, + "velocityX": -1.7152401398758959, + "velocityY": -1.9474179742098816, + "timestamp": 9.828221739201535 + }, + { + "x": 1.7987115747047, + "y": 6.290184489368743, + "heading": 0.16896416101959177, + "angularVelocity": -0.5228946483767686, + "velocityX": -1.3339127422322181, + "velocityY": -1.9802986493663424, + "timestamp": 9.889944055396859 }, { "x": 1.740001559257507, "y": 6.167843341827393, - "heading": 0.1387804363394676, - "angularVelocity": -0.4783884789404854, - "velocityX": -0.8860721033672938, - "velocityY": -1.8967410649584446, - "timestamp": 10.211362755442659 - }, - { - "x": 1.706182040858774, - "y": 6.03669580225492, - "heading": 0.10729245974250749, - "angularVelocity": -0.45130411484825544, - "velocityX": -0.48472113692462804, - "velocityY": -1.8796833159166881, - "timestamp": 10.281133837870186 - }, - { - "x": 1.699651780790286, - "y": 5.911922041931935, - "heading": 0.07852355852866735, - "angularVelocity": -0.4123327346071031, - "velocityX": -0.09359551036449741, - "velocityY": -1.7883305802599465, - "timestamp": 10.350904920297713 - }, - { - "x": 1.7185433856985064, - "y": 5.798927064131694, - "heading": 0.053308691164350094, - "angularVelocity": -0.36139424080898114, - "velocityX": 0.2707655414095584, - "velocityY": -1.6195101733961197, - "timestamp": 10.42067600272524 - }, - { - "x": 1.7598996151781712, - "y": 5.702665752208848, - "heading": 0.032386709878439925, - "angularVelocity": -0.29986608431425904, - "velocityX": 0.592741692414218, - "velocityY": -1.3796734775160497, - "timestamp": 10.490447085152768 - }, - { - "x": 1.820114668725021, - "y": 5.6271104583982785, - "heading": 0.016317787108466864, - "angularVelocity": -0.2303092084985792, - "velocityX": 0.8630373996189019, - "velocityY": -1.082902703839363, - "timestamp": 10.560218167580295 - }, - { - "x": 1.8954858630630067, - "y": 5.575103001600796, - "heading": 0.005460341828234747, - "angularVelocity": -0.15561526211822638, - "velocityX": 1.0802640824194598, - "velocityY": -0.7454013179672614, - "timestamp": 10.629989250007823 - }, - { - "x": 1.9826012990017379, + "heading": 0.13807920449538028, + "angularVelocity": -0.5003855724803669, + "velocityX": -0.9511959217700356, + "velocityY": -1.9821217848370036, + "timestamp": 9.951666371592182 + }, + { + "x": 1.7034889997175517, + "y": 6.016151230649133, + "heading": 0.1019414560444192, + "angularVelocity": -0.467252098060329, + "velocityX": -0.4720983122052568, + "velocityY": -1.961341263510758, + "timestamp": 10.029007380013498 + }, + { + "x": 1.7027290435050462, + "y": 5.874298147554192, + "heading": 0.06966485107958241, + "angularVelocity": -0.4173284732597485, + "velocityX": -0.009826044785523666, + "velocityY": -1.8341250778913685, + "timestamp": 10.106348388434814 + }, + { + "x": 1.7342944275877592, + "y": 5.750595898188055, + "heading": 0.04250435573420922, + "angularVelocity": -0.35117844853297475, + "velocityX": 0.40813256417294685, + "velocityY": -1.5994393128709739, + "timestamp": 10.18368939685613 + }, + { + "x": 1.7930814617897048, + "y": 5.6520476897956975, + "heading": 0.02146340510922574, + "angularVelocity": -0.2720542575597529, + "velocityX": 0.7601017287194328, + "velocityY": -1.2742038202491095, + "timestamp": 10.261030405277445 + }, + { + "x": 1.8734294527078426, + "y": 5.583639683859189, + "heading": 0.0071877988424821045, + "angularVelocity": -0.1845800379143917, + "velocityX": 1.0388795356848963, + "velocityY": -0.8844984999918234, + "timestamp": 10.338371413698761 + }, + { + "x": 1.970064237353583, "y": 5.548515796661377, - "heading": -3.619508887306417e-27, - "angularVelocity": -0.07826081577430707, - "velocityX": 1.248589428567619, - "velocityY": -0.3810633863540251, - "timestamp": 10.69976033243535 + "heading": 3.498573366455402e-27, + "angularVelocity": -0.09293645103935767, + "velocityX": 1.249463727177213, + "velocityY": -0.45414312425918, + "timestamp": 10.415712422120077 }, { "x": 2.0785037517547607, "y": 5.548515796661377, - "heading": -1.3504694626823558e-27, - "angularVelocity": 2.3770174727505955e-28, - "velocityX": 1.3745300978043182, - "velocityY": 2.5815580956362606e-25, - "timestamp": 10.769531414862877 + "heading": 9.074763160083063e-28, + "angularVelocity": -2.8883864105140666e-27, + "velocityX": 1.4020959464409022, + "velocityY": 1.1648614692434482e-25, + "timestamp": 10.493053430541393 }, { - "x": 2.2108243531689626, + "x": 2.2093539983390484, "y": 5.548515796661377, - "heading": -3.98229506769339e-28, - "angularVelocity": 6.58921025141663e-29, - "velocityX": 1.7982508177997887, - "velocityY": 1.343058267982166e-16, - "timestamp": 10.843114365641734 + "heading": -5.091869940539703e-28, + "angularVelocity": -8.863496880486678e-29, + "velocityX": 1.8425023918344958, + "velocityY": 6.335504596026049e-17, + "timestamp": 10.56407111125876 }, { - "x": 2.366717480768576, + "x": 2.3657372440174322, "y": 5.548515796661377, - "heading": 5.540104511762132e-28, - "angularVelocity": 6.589213013653999e-29, - "velocityX": 2.118603915030903, - "velocityY": 6.71525155127488e-16, - "timestamp": 10.91669731642059 + "heading": -1.9258503020667073e-27, + "angularVelocity": -8.863493994530242e-29, + "velocityX": 2.202032565675437, + "velocityY": 3.167727717458953e-16, + "timestamp": 10.635088791976129 }, { - "x": 2.4914319874847974, + "x": 2.4908438453710073, "y": 5.548515796661377, - "heading": 1.506250408105455e-27, - "angularVelocity": 6.589211632476856e-29, - "velocityX": 1.694883195035658, - "velocityY": 5.372204471623799e-16, - "timestamp": 10.990280267199447 + "heading": -3.342513611103023e-27, + "angularVelocity": -8.863495435827685e-29, + "velocityX": 1.7616261202821912, + "velocityY": 2.534184135606925e-16, + "timestamp": 10.706106472693497 }, { - "x": 2.58496786905088, + "x": 2.5846737979725942, "y": 5.548515796661377, - "heading": 2.458490365034698e-27, - "angularVelocity": 6.589211632476792e-29, - "velocityX": 1.2711624170548799, - "velocityY": 4.029154421641203e-16, - "timestamp": 11.063863217978303 + "heading": -4.759176920139342e-27, + "angularVelocity": -8.863495435827717e-29, + "velocityX": 1.321219612549812, + "velocityY": 1.9006387498434594e-16, + "timestamp": 10.777124153410865 }, { - "x": 2.647325124044576, + "x": 2.6472271003464662, "y": 5.548515796661377, - "heading": 3.4107303219639416e-27, - "angularVelocity": 6.589211632476844e-29, - "velocityX": 0.8474416197456015, - "velocityY": 2.686103379493745e-16, - "timestamp": 11.13744616875716 + "heading": -6.175840229175665e-27, + "angularVelocity": -8.863495435827698e-29, + "velocityX": 0.8808130840377387, + "velocityY": 1.2670927613090377e-16, + "timestamp": 10.848141834128233 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 4.3629702788936556e-27, - "angularVelocity": 6.589211633112284e-29, - "velocityX": 0.42372081277207485, - "velocityY": 1.3430518397832439e-16, - "timestamp": 11.211029119536017 + "heading": -7.592503538214036e-27, + "angularVelocity": -8.863495438700938e-29, + "velocityX": 0.44040654513582195, + "velocityY": 6.3354647036682e-17, + "timestamp": 10.919159514845601 }, { "x": 2.6785037517547607, "y": 5.548515796661377, - "heading": 5.369537146588021e-27, - "angularVelocity": 8.042005177965745e-28, - "velocityX": 2.643601734591811e-26, - "velocityY": 4.252096317345015e-26, - "timestamp": 11.284612070314873 - }, - { - "x": 2.65514872822099, - "y": 5.535337362244174, - "heading": -0.005358266322899027, - "angularVelocity": -0.07847108857121549, - "velocityX": -0.34203117386479875, - "velocityY": -0.1929963969806516, - "timestamp": 11.352895388674044 - }, - { - "x": 2.608944125410673, - "y": 5.508126235941394, - "heading": -0.01624992945284291, - "angularVelocity": -0.15950693949367814, - "velocityX": -0.676660184662962, - "velocityY": -0.39850327952208153, - "timestamp": 11.421178707033215 - }, - { - "x": 2.5407349138993025, - "y": 5.465596827387689, - "heading": -0.03293816440101749, - "angularVelocity": -0.24439695300679784, - "velocityX": -0.9989147151957237, - "velocityY": -0.622837459802409, - "timestamp": 11.489462025392386 - }, - { - "x": 2.4521626115248987, - "y": 5.405634188727449, - "heading": -0.05584758658914312, - "angularVelocity": -0.33550540217775826, - "velocityX": -1.2971294380936889, - "velocityY": -0.8781447665568274, - "timestamp": 11.557745343751558 - }, - { - "x": 2.3472993944122122, - "y": 5.324395116991271, - "heading": -0.08569447031240085, - "angularVelocity": -0.43710359192361903, - "velocityX": -1.5357076901433586, - "velocityY": -1.1897352631408427, - "timestamp": 11.626028662110729 - }, - { - "x": 2.239880929754108, - "y": 5.216483136191594, - "heading": -0.12272614650889033, - "angularVelocity": -0.542323909943895, - "velocityX": -1.5731289462688671, - "velocityY": -1.580356423688402, - "timestamp": 11.6943119804699 - }, - { - "x": 2.153374403355559, - "y": 5.091739159562662, - "heading": -0.16180305380781881, - "angularVelocity": -0.5722760439582536, - "velocityX": -1.2668764271754205, - "velocityY": -1.8268587354348766, - "timestamp": 11.762595298829071 + "heading": -9.168166896056586e-27, + "angularVelocity": -2.3275147262701813e-27, + "velocityX": -7.876142714980879e-26, + "velocityY": -2.7822667519177177e-25, + "timestamp": 10.99017719556297 + }, + { + "x": 2.6551487281624913, + "y": 5.535337362195577, + "heading": -0.0053582663887861755, + "angularVelocity": -0.08143329787865013, + "velocityX": -0.3549425234125067, + "velocityY": -0.20028182653884286, + "timestamp": 11.055976647732658 + }, + { + "x": 2.608944125248311, + "y": 5.508126235802502, + "heading": -0.016249929633628882, + "angularVelocity": -0.16552817517013058, + "velocityX": -0.7022034590048659, + "velocityY": -0.41354639736059495, + "timestamp": 11.121776099902347 + }, + { + "x": 2.5407349136089206, + "y": 5.4655968271259, + "heading": -0.03293816472196888, + "angularVelocity": -0.2536227056313985, + "velocityX": -1.0366227892519315, + "velocityY": -0.6463489782091855, + "timestamp": 11.187575552072035 + }, + { + "x": 2.4521626111212957, + "y": 5.405634188324023, + "heading": -0.05584758703948852, + "angularVelocity": -0.34817041118274633, + "velocityX": -1.3460948315984256, + "velocityY": -0.9112938911290825, + "timestamp": 11.253375004241724 + }, + { + "x": 2.347299393995622, + "y": 5.324395116461008, + "heading": -0.08569447081932852, + "angularVelocity": -0.45360383400866283, + "velocityX": -1.593679182240697, + "velocityY": -1.2346466297851608, + "timestamp": 11.319174456411412 + }, + { + "x": 2.239880929566337, + "y": 5.216483135728542, + "heading": -0.12272614687803589, + "angularVelocity": -0.5627961151288577, + "velocityX": -1.6325130512069779, + "velocityY": -1.6400133614209311, + "timestamp": 11.3849739085811 + }, + { + "x": 2.1533744032985735, + "y": 5.091739159361965, + "heading": -0.16180305396789932, + "angularVelocity": -0.5938789123819668, + "velocityX": -1.314699794835295, + "velocityY": -1.8958208959685579, + "timestamp": 11.450773360750789 }, { "x": 2.0932393074035645, "y": 4.9619622230529785, - "heading": -0.19963034769606672, - "angularVelocity": -0.5539756238745723, - "velocityX": -0.8806703803655747, - "velocityY": -1.9005657549777475, - "timestamp": 11.830878617188242 - }, - { - "x": 2.062901110669511, - "y": 4.862373019706677, - "heading": -0.22719939385993115, - "angularVelocity": -0.526195783497447, - "velocityX": -0.5790490938819385, - "velocityY": -1.9008063815925753, - "timestamp": 11.883271751129039 - }, - { - "x": 2.0480285995415843, - "y": 4.7660067701880315, - "heading": -0.2526003669786383, - "angularVelocity": -0.4848149215011564, - "velocityX": -0.28386374338156817, - "velocityY": -1.8392915687680964, - "timestamp": 11.935664885069835 - }, - { - "x": 2.0477407755531423, - "y": 4.675697634157938, - "heading": -0.27523200757833177, - "angularVelocity": -0.43195813835581237, - "velocityX": -0.005493544035129206, - "velocityY": -1.723682651473774, - "timestamp": 11.988058019010632 - }, - { - "x": 2.060871040463269, - "y": 4.593707250314659, - "heading": -0.2946333476397457, - "angularVelocity": -0.37030310275649647, - "velocityX": 0.2506104125201524, - "velocityY": -1.5649070341149178, - "timestamp": 12.040451152951428 - }, - { - "x": 2.0861920181034543, - "y": 4.521742431666527, - "heading": -0.3104741157540841, - "angularVelocity": -0.30234435169001433, - "velocityX": 0.48328809016841023, - "velocityY": -1.373554380798257, - "timestamp": 12.092844286892225 - }, - { - "x": 2.122546344711596, - "y": 4.4610604943149115, - "heading": -0.32252861579276854, - "angularVelocity": -0.23007785814655887, - "velocityX": 0.6938757786320086, - "velocityY": -1.1582040009323555, - "timestamp": 12.145237420833022 - }, - { - "x": 2.168900206537902, - "y": 4.412584630956035, - "heading": -0.3306480218589531, - "angularVelocity": -0.15497080352855533, - "velocityX": 0.8847316115635476, - "velocityY": -0.9252331309986794, - "timestamp": 12.197630554773818 - }, - { - "x": 2.224353286156776, - "y": 4.376998478351871, + "heading": -0.19963034769256288, + "angularVelocity": -0.5748876696892883, + "velocityX": -0.9139148414173547, + "velocityY": -1.9723102857195456, + "timestamp": 11.516572812920478 + }, + { + "x": 2.06290111070071, + "y": 4.862373019847466, + "heading": -0.22719939375272255, + "angularVelocity": -0.5460591677053325, + "velocityX": -0.6009076413123203, + "velocityY": -1.9725599970405536, + "timestamp": 11.56706010017913 + }, + { + "x": 2.048028599586168, + "y": 4.7660067703914, + "heading": -0.2526003668147138, + "angularVelocity": -0.5031162187791788, + "velocityX": -0.29457932723436914, + "velocityY": -1.9087230605671173, + "timestamp": 11.617547387437783 + }, + { + "x": 2.047740775609064, + "y": 4.67569763435831, + "heading": -0.27523200740159587, + "angularVelocity": -0.4482641436236724, + "velocityX": -0.00570091983015679, + "velocityY": -1.788750018800283, + "timestamp": 11.668034674696436 + }, + { + "x": 2.0608710405337267, + "y": 4.5937072504707785, + "heading": -0.29463334748351117, + "angularVelocity": -0.3842816902108432, + "velocityX": 0.26007071557230893, + "velocityY": -1.62398077495204, + "timestamp": 11.718521961955089 + }, + { + "x": 2.0861920181883997, + "y": 4.5217424317614965, + "heading": -0.31047411563844995, + "angularVelocity": -0.3137575618548228, + "velocityX": 0.5015317524380114, + "velocityY": -1.4254047427939316, + "timestamp": 11.769009249213742 + }, + { + "x": 2.1225463448038515, + "y": 4.461060494351439, + "heading": -0.3225286157246152, + "angularVelocity": -0.2387630776121657, + "velocityX": 0.7200689240680372, + "velocityY": -1.2019250925322578, + "timestamp": 11.819496536472395 + }, + { + "x": 2.16890020662259, + "y": 4.412584630951135, + "heading": -0.3306480218329806, + "angularVelocity": -0.16082080359692727, + "velocityX": 0.9181293813878825, + "velocityY": -0.9601597953155385, + "timestamp": 11.869983823731047 + }, + { + "x": 2.2243532862123785, + "y": 4.376998478332531, "heading": -0.3347371673945201, - "angularVelocity": -0.07804735521619326, - "velocityX": 1.0584035626029547, - "velocityY": -0.6792140482448454, - "timestamp": 12.250023688714615 + "angularVelocity": -0.08099356855103614, + "velocityX": 1.0983572816201457, + "velocityY": -0.7048537275590007, + "timestamp": 11.9204711109897 }, { "x": 2.2881293296813965, "y": 4.354815483093262, "heading": -0.3347371673945201, - "angularVelocity": 3.779464021226463e-27, - "velocityX": 1.2172595668105288, - "velocityY": -0.4233950823341772, - "timestamp": 12.302416822655411 + "angularVelocity": -3.351040193936762e-27, + "velocityX": 1.2632099471355258, + "velocityY": -0.43937784031895566, + "timestamp": 11.970958398248353 }, { - "x": 2.4097151176038345, - "y": 4.314433194202397, + "x": 2.4097151173059284, + "y": 4.31443319431648, "heading": -0.3347371673945201, - "angularVelocity": 6.573584275341322e-29, - "velocityX": 1.6302002618641742, - "velocityY": -0.5414384283676151, - "timestamp": 12.377000167270632 + "angularVelocity": -1.1055944271274773e-28, + "velocityX": 1.6917387541138205, + "velocityY": -0.5618772081689679, + "timestamp": 12.042828707614305 }, { - "x": 2.5011472356587063, - "y": 4.284859003501582, + "x": 2.501147235462863, + "y": 4.284859003573599, "heading": -0.3347371673945201, - "angularVelocity": 6.573584272217567e-29, - "velocityX": 1.225905308035939, - "velocityY": -0.396525401929742, - "timestamp": 12.451583511885852 + "angularVelocity": -1.1055944267694559e-28, + "velocityX": 1.2721820590944748, + "velocityY": -0.4114938561387572, + "timestamp": 12.114699016980257 }, { - "x": 2.562139216220098, - "y": 4.265257651811236, + "x": 2.562139216145648, + "y": 4.265257651838079, "heading": -0.3347371673945201, - "angularVelocity": -6.199774132429429e-28, - "velocityX": 0.8177694480725244, - "velocityY": -0.2628113795576003, - "timestamp": 12.526166856501073 + "angularVelocity": -1.105594426769472e-28, + "velocityX": 0.8486394621209057, + "velocityY": -0.2727322577076049, + "timestamp": 12.18656932634621 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": 7.63642932425348e-28, - "velocityX": 0.40901189675210486, - "velocityY": -0.1310099931742232, - "timestamp": 12.600750201116293 + "angularVelocity": -1.1055944267596697e-28, + "velocityX": 0.42445170461571047, + "velocityY": -0.13595549514112273, + "timestamp": 12.258439635712161 }, { "x": 2.592644691467285, "y": 4.255486488342285, "heading": -0.3347371673945201, - "angularVelocity": 1.6433960683327454e-29, - "velocityX": 1.171390593825864e-26, - "velocityY": -5.961445669521376e-27, - "timestamp": 12.675333545731513 + "angularVelocity": -2.916408988560576e-29, + "velocityX": -2.969429335311962e-26, + "velocityY": 1.2754135405824192e-26, + "timestamp": 12.330309945078113 } ], "eventMarkers": [] diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java index 79cf2dcd..14e70a44 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java @@ -1,11 +1,7 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.*; -import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; -import org.team1540.robot2024.commands.indexer.IntakeCommand; -import org.team1540.robot2024.commands.shooter.AutoShootPrepare; -import org.team1540.robot2024.commands.shooter.AutoShootPrepareWhileMoving; import org.team1540.robot2024.commands.shooter.LeadingShootPrepare; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -38,12 +34,13 @@ public CenterLanePDEABC(Drivetrain drivetrain, Shooter shooter, Indexer indexer) ).withTimeout(1.1), Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer)) ), - createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), - createSegmentSequence(drivetrain, shooter, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0, 0.1), + Commands.runOnce(drivetrain::unblockTags), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0, 0.1), Commands.runOnce(drivetrain::blockTags), - createSegmentSequence(drivetrain, shooter, indexer, 2), - createSegmentSequence(drivetrain, shooter, indexer, 3), - createSegmentSequence(drivetrain, shooter, indexer, 4) + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 4, false, true, true, 0, 0.1) ) ) ); diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index 3948e432..50de24da 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -84,7 +84,7 @@ public void setPathIndex(int index){ * @param pathIndex * @return the commands to run in the segment */ - protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry, double extraPreShotWait) { + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry, double extraPreShotWait, double postShotWaitReduction) { return Commands.sequence( Commands.deadline( getPath(pathIndex).getCommand(drivetrain), @@ -102,9 +102,10 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, // Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10).onlyIf(()->shouldRealignYaw), new ParallelDeadlineGroup( Commands.sequence( + Commands.waitUntil(indexer::isNoteStaged), Commands.waitSeconds(extraPreShotWait), Commands.waitUntil(()->!indexer.isNoteStaged()), - Commands.waitSeconds(0.2) + Commands.waitSeconds(0.2 - postShotWaitReduction) ).withTimeout(1+extraPreShotWait), IntakeAndFeed.withDefaults(indexer) ) @@ -112,6 +113,12 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, ) ); } + + + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry, double extraPreShotWait) { + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, shouldZeroCancoder, shouldRealignYaw, shouldResetOdometry, extraPreShotWait, 0); + } + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex){ return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, false, true, true,0); } From 53ea55bab900881a4327ce4b5f70a7702057d450 Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Thu, 4 Apr 2024 07:57:05 -0700 Subject: [PATCH 72/75] feat: auto ordering options --- paths.chor | 30368 +++++++++------- src/main/deploy/choreo/AmpLanePAEDF.1.traj | 185 + src/main/deploy/choreo/AmpLanePAEDF.2.traj | 86 + src/main/deploy/choreo/AmpLanePAEDF.3.traj | 536 + src/main/deploy/choreo/AmpLanePAEDF.4.traj | 338 + src/main/deploy/choreo/AmpLanePAEDF.5.traj | 473 + src/main/deploy/choreo/AmpLanePAEDF.traj | 1562 + src/main/deploy/choreo/AmpLanePdChaos.traj | 770 - .../deploy/choreo/CenterLanePCBAEF.1.traj | 275 + .../deploy/choreo/CenterLanePCBAEF.2.traj | 86 + .../deploy/choreo/CenterLanePCBAEF.3.traj | 176 + .../deploy/choreo/CenterLanePCBAEF.4.traj | 644 + .../deploy/choreo/CenterLanePCBAEF.5.traj | 572 + src/main/deploy/choreo/CenterLanePCBAEF.traj | 1697 + .../deploy/choreo/CenterLanePCBAFE.1.traj | 536 +- .../deploy/choreo/CenterLanePCBAFE.2.traj | 130 +- .../deploy/choreo/CenterLanePCBAFE.3.traj | 308 +- .../deploy/choreo/CenterLanePCBAFE.4.traj | 1391 +- .../deploy/choreo/CenterLanePCBAFE.5.traj | 1135 +- src/main/deploy/choreo/CenterLanePCBAFE.traj | 3502 +- .../deploy/choreo/CenterLanePCBAFG.1.traj | 275 + .../deploy/choreo/CenterLanePCBAFG.2.traj | 86 + .../deploy/choreo/CenterLanePCBAFG.3.traj | 176 + .../deploy/choreo/CenterLanePCBAFG.4.traj | 644 + .../deploy/choreo/CenterLanePCBAFG.5.traj | 536 + src/main/deploy/choreo/CenterLanePCBAFG.traj | 1661 + .../deploy/choreo/CenterLanePCBAGF.1.traj | 275 + .../deploy/choreo/CenterLanePCBAGF.2.traj | 86 + .../deploy/choreo/CenterLanePCBAGF.3.traj | 176 + .../deploy/choreo/CenterLanePCBAGF.4.traj | 662 + .../deploy/choreo/CenterLanePCBAGF.5.traj | 509 + src/main/deploy/choreo/CenterLanePCBAGF.traj | 1652 + .../deploy/choreo/CenterLanePDEABC.6.traj | 212 + .../deploy/choreo/CenterLanePEDABC.1.traj | 599 + .../deploy/choreo/CenterLanePEDABC.2.traj | 716 + .../deploy/choreo/CenterLanePEDABC.3.traj | 86 + .../deploy/choreo/CenterLanePEDABC.4.traj | 248 + .../deploy/choreo/CenterLanePEDABC.5.traj | 212 + src/main/deploy/choreo/SourceLanephChaos.traj | 1481 - .../team1540/robot2024/RobotContainer.java | 12 +- .../commands/autos/AmpLanePAEDF.java | 68 + .../commands/autos/CenterLanePCBAEF.java | 48 + .../commands/autos/CenterLanePCBAFE.java | 26 +- .../commands/autos/CenterLanePCBAFG.java | 48 + .../commands/autos/CenterLanePCBAGF.java | 48 + ...ourceLanePGHF.java => SourceLanePHGF.java} | 4 +- 46 files changed, 36171 insertions(+), 19145 deletions(-) create mode 100644 src/main/deploy/choreo/AmpLanePAEDF.1.traj create mode 100644 src/main/deploy/choreo/AmpLanePAEDF.2.traj create mode 100644 src/main/deploy/choreo/AmpLanePAEDF.3.traj create mode 100644 src/main/deploy/choreo/AmpLanePAEDF.4.traj create mode 100644 src/main/deploy/choreo/AmpLanePAEDF.5.traj create mode 100644 src/main/deploy/choreo/AmpLanePAEDF.traj delete mode 100644 src/main/deploy/choreo/AmpLanePdChaos.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAEF.1.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAEF.2.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAEF.3.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAEF.4.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAEF.5.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAEF.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFG.1.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFG.2.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFG.3.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFG.4.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFG.5.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAFG.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAGF.1.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAGF.2.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAGF.3.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAGF.4.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAGF.5.traj create mode 100644 src/main/deploy/choreo/CenterLanePCBAGF.traj create mode 100644 src/main/deploy/choreo/CenterLanePDEABC.6.traj create mode 100644 src/main/deploy/choreo/CenterLanePEDABC.1.traj create mode 100644 src/main/deploy/choreo/CenterLanePEDABC.2.traj create mode 100644 src/main/deploy/choreo/CenterLanePEDABC.3.traj create mode 100644 src/main/deploy/choreo/CenterLanePEDABC.4.traj create mode 100644 src/main/deploy/choreo/CenterLanePEDABC.5.traj delete mode 100644 src/main/deploy/choreo/SourceLanephChaos.traj create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/AmpLanePAEDF.java create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAEF.java create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFG.java create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAGF.java rename src/main/java/org/team1540/robot2024/commands/autos/{SourceLanePGHF.java => SourceLanePHGF.java} (92%) diff --git a/paths.chor b/paths.chor index 2c583f13..31fa786d 100644 --- a/paths.chor +++ b/paths.chor @@ -20672,7 +20672,7 @@ "eventMarkers": [], "isTrajectoryStale": true }, - "CenterLanePCBAFE": { + "CenterLanePCBFSprint": { "waypoints": [ { "x": 1.2899744510650635, @@ -20708,25 +20708,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 6 - }, - { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 12 }, { "x": 4.16749906539917, @@ -20747,22 +20729,22 @@ "controlIntervalCount": 12 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, + "x": 7.196889877319336, + "y": 3.9978766441345215, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { - "x": 8.0634765625, + "x": 8.1634765625, "y": 4.073246955871582, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { "x": 5.665951728820801, @@ -20776,38 +20758,29 @@ { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -0.106122, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 20 + "controlIntervalCount": 13 }, { - "x": 7.984478950500488, - "y": 5.483555793762207, - "heading": 0.7044936776046891, + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 13 }, { - "x": 8.217175483703613, - "y": 5.698352336883545, - "heading": 0.7044936776046891, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 20 + "headingConstrained": false, + "controlIntervalCount": 13 }, { "x": 5.665951728820801, @@ -20816,12 +20789,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 15 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -20832,4028 +20805,3697 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.0042344276654815e-24, - "angularVelocity": -6.621110938678157e-25, - "velocityX": -2.249108899033261e-24, - "velocityY": -2.0825834551113612e-24, + "heading": -5.9009444568140314e-21, + "angularVelocity": -6.3286336172777184e-21, + "velocityX": -2.3802874375881527e-19, + "velocityY": -1.4090020225698018e-19, "timestamp": 0 }, { - "x": 1.3060567679582313, - "y": 5.572381918180444, - "heading": -0.010695335923114668, - "angularVelocity": -0.14212952691547795, - "velocityX": 0.2137167180313357, - "velocityY": -0.24680705612748235, - "timestamp": 0.07525062634926666 - }, - { - "x": 1.3382215476730719, - "y": 5.535237409026599, - "heading": -0.03207750843931141, - "angularVelocity": -0.2841461068636689, - "velocityX": 0.4274353752957696, - "velocityY": -0.49361063097924024, - "timestamp": 0.15050125269853332 - }, - { - "x": 1.3864692368556266, - "y": 5.479521033340712, - "heading": -0.06412446500663382, - "angularVelocity": -0.4258696322151573, - "velocityX": 0.6411599680063655, - "velocityY": -0.7404107897692007, - "timestamp": 0.22575187904779997 - }, - { - "x": 1.4508006060735943, - "y": 5.40523312636478, - "heading": -0.10680311296182225, - "angularVelocity": -0.5671533916156479, - "velocityX": 0.8548948007340368, - "velocityY": -0.987206493553073, - "timestamp": 0.30100250539706663 - }, - { - "x": 1.5312167667507302, - "y": 5.312374207143024, - "heading": -0.16007302531493992, - "angularVelocity": -0.7078999197411572, - "velocityX": 1.0686444030896685, - "velocityY": -1.2339952997967512, - "timestamp": 0.3762531317463333 - }, - { - "x": 1.6277191879065078, - "y": 5.200945106889031, - "heading": -0.22389124150432238, - "angularVelocity": -0.8480755481446488, - "velocityX": 1.2824135271362858, - "velocityY": -1.480773060104614, - "timestamp": 0.45150375809559995 - }, - { - "x": 1.7403097782501336, - "y": 5.070947160382142, - "heading": -0.2982176402551873, - "angularVelocity": -0.9877180078992028, - "velocityX": 1.496208015878168, - "velocityY": -1.7275330826287059, - "timestamp": 0.5267543844448666 - }, - { - "x": 1.8689915501245091, - "y": 4.922382870770249, - "heading": -0.3830195665066897, - "angularVelocity": -1.126926516968836, - "velocityX": 1.7100425354217623, - "velocityY": -1.9742598410058476, - "timestamp": 0.6020050107941333 - }, - { - "x": 2.021815501878503, - "y": 4.778590249332989, - "heading": -0.46245356659644804, - "angularVelocity": -1.0555925437892708, - "velocityX": 2.0308661757136712, - "velocityY": -1.9108494960541986, - "timestamp": 0.6772556371434 - }, - { - "x": 2.158552321551595, - "y": 4.6533669501943, - "heading": -0.5314334875140496, - "angularVelocity": -0.916669059968213, - "velocityX": 1.8170854690091889, - "velocityY": -1.6640831473944147, - "timestamp": 0.7525062634926667 - }, - { - "x": 2.279199772550662, - "y": 4.546710847450061, - "heading": -0.589963647479443, - "angularVelocity": -0.7778029606522173, - "velocityX": 1.6032750403843345, - "velocityY": -1.417345049717031, - "timestamp": 0.8277568898419334 - }, - { - "x": 2.383756839213222, - "y": 4.45862085446933, - "heading": -0.6380444617813997, - "angularVelocity": -0.6389423800779483, - "velocityX": 1.38945111469599, - "velocityY": -1.1706213921977682, - "timestamp": 0.9030075161912001 - }, - { - "x": 2.4722229084949126, - "y": 4.389096261559967, - "heading": -0.67567478001119, - "angularVelocity": -0.5000665118072721, - "velocityX": 1.1756190423065196, - "velocityY": -0.9239071657247342, - "timestamp": 0.9782581425404668 - }, - { - "x": 2.544597589718017, - "y": 4.338136588288561, - "heading": -0.7028528784398662, - "angularVelocity": -0.36116773703028543, - "velocityX": 0.9617817782298118, - "velocityY": -0.6771993236957614, - "timestamp": 1.0535087688897335 - }, - { - "x": 2.6008806504487216, - "y": 4.305741524929451, - "heading": -0.7195768777220212, - "angularVelocity": -0.22224398777137738, - "velocityX": 0.7479414253573617, - "velocityY": -0.4304955975881504, - "timestamp": 1.1287593952390003 - }, - { - "x": 2.6410719892543444, - "y": 4.2919109040687715, - "heading": -0.7258449001014369, - "angularVelocity": -0.08329528514916955, - "velocityX": 0.5340997245535138, - "velocityY": -0.1837940962309967, - "timestamp": 1.204010021588267 - }, - { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.055677742612701135, - "velocityX": 0.3202582509251215, - "velocityY": 0.06290689943025622, - "timestamp": 1.2792606479375337 - }, - { - "x": 2.6731116736052027, - "y": 4.320290744259811, - "heading": -0.7067971794351253, - "angularVelocity": 0.19582512759313456, - "velocityX": 0.10464854696510043, - "velocityY": 0.31165110401848706, - "timestamp": 1.355134136920331 - }, - { - "x": 2.664692786668951, - "y": 4.362810218152233, - "heading": -0.6813099274422239, - "angularVelocity": 0.3359177538109565, - "velocityX": -0.1109595334170119, - "velocityY": 0.5603996133888316, - "timestamp": 1.4310076259031281 - }, - { - "x": 2.639915179239627, - "y": 4.424203570363852, - "heading": -0.6451974716199608, - "angularVelocity": 0.47595617792732364, - "velocityX": -0.3265647561685457, - "velocityY": 0.8091541991108215, - "timestamp": 1.5068811148859254 - }, - { - "x": 2.59877917099983, - "y": 4.50447140700047, - "heading": -0.59846321314625, - "angularVelocity": 0.6159497750829267, - "velocityX": -0.5421657655564515, - "velocityY": 1.0579167732067494, - "timestamp": 1.5827546038687226 - }, - { - "x": 2.5412852001349884, - "y": 4.6036144764177696, - "heading": -0.541108297539196, - "angularVelocity": 0.755928274499912, - "velocityX": -0.7577609997330812, - "velocityY": 1.3066892105064276, - "timestamp": 1.6586280928515198 - }, - { - "x": 2.4674338294971827, - "y": 4.7216336217313835, - "heading": -0.4731286056147784, - "angularVelocity": 0.8959610640790578, - "velocityX": -0.973348815612657, - "velocityY": 1.5554727599303189, - "timestamp": 1.734501581834317 - }, - { - "x": 2.3772256595733805, - "y": 4.858529596063364, - "heading": -0.3945096344591076, - "angularVelocity": 1.0361849996577348, - "velocityX": -1.1889287171735974, - "velocityY": 1.8042662353779348, - "timestamp": 1.8103750708171142 - }, - { - "x": 2.270660565338228, - "y": 5.014302224131279, - "heading": -0.305219034790845, - "angularVelocity": 1.1768352933988244, - "velocityX": -1.4045102665479658, - "velocityY": 2.0530574006321625, - "timestamp": 1.8862485597999115 - }, - { - "x": 2.156230166958197, - "y": 5.146478435153338, - "heading": -0.2290932542035935, - "angularVelocity": 1.0033251615002383, - "velocityX": -1.5081736705949598, - "velocityY": 1.7420605377990246, - "timestamp": 1.9621220487827087 - }, - { - "x": 2.05814700584277, - "y": 5.259770794344292, - "heading": -0.16373664121599296, - "angularVelocity": 0.8613893187700765, - "velocityX": -1.292719794889953, - "velocityY": 1.4931745028443373, - "timestamp": 2.037995537765506 - }, - { - "x": 1.976411579927544, - "y": 5.3541805242021825, - "heading": -0.10920088428004432, - "angularVelocity": 0.7187722308158727, - "velocityX": -1.077259356476389, - "velocityY": 1.2443045802110966, - "timestamp": 2.113869026748303 - }, - { - "x": 1.9110235357975711, - "y": 5.4297080461749685, - "heading": -0.06553152377248019, - "angularVelocity": 0.5755549282499098, - "velocityX": -0.8618035760132025, - "velocityY": 0.9954402121920332, - "timestamp": 2.1897425157311 - }, - { - "x": 1.8619825316383047, - "y": 5.486353607136737, - "heading": -0.03276339241185007, - "angularVelocity": 0.43187853623101025, - "velocityX": -0.6463523006090516, - "velocityY": 0.7465790979324988, - "timestamp": 2.265616004713897 + "x": 1.3060573928270223, + "y": 5.572382489770416, + "heading": -0.010694903760090106, + "angularVelocity": -0.1421238741144053, + "velocityX": 0.2137251574806739, + "velocityY": -0.24679961691155075, + "timestamp": 0.0752505786007567 }, { - "x": 1.829288400520471, - "y": 5.524117342639773, - "heading": -0.010917643304867627, - "angularVelocity": 0.2879233497741951, - "velocityX": -0.43090322530503955, - "velocityY": 0.49771977022960984, - "timestamp": 2.341489493696694 + "x": 1.3382234711320444, + "y": 5.535239165388228, + "heading": -0.03207617873440696, + "angularVelocity": -0.28413435978686086, + "velocityX": 0.42745290339416875, + "velocityY": -0.4935951998359545, + "timestamp": 0.1505011572015134 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 5.072711091682045e-24, - "angularVelocity": 0.14389272789792218, - "velocityX": -0.21545347602968715, - "velocityY": 0.24886063882779058, - "timestamp": 2.417362982679491 + "x": 1.386473197712333, + "y": 5.479524643144728, + "heading": -0.06412173018225528, + "angularVelocity": -0.4258512299003913, + "velocityX": 0.6411874496855803, + "velocityY": -0.7403866293046694, + "timestamp": 0.22575173580227012 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 2.471925946464062e-24, - "angularVelocity": -1.3881126396060902e-24, - "velocityX": -6.902910153275516e-22, - "velocityY": 3.1112652084354562e-24, - "timestamp": 2.493236471662288 + "x": 1.4508074341962967, + "y": 5.405239335978357, + "heading": -0.1067984078244425, + "angularVelocity": -0.5671275681295302, + "velocityX": 0.8549334460969267, + "velocityY": -0.987172571263834, + "timestamp": 0.3010023144030268 }, { - "x": 1.8507557540076562, - "y": 5.5430527114888575, - "heading": -8.800046052500135e-19, - "angularVelocity": -9.452376611096758e-18, - "velocityX": 0.4061756726920047, - "velocityY": 0.0005740544422031257, - "timestamp": 2.586335499913798 + "x": 1.5312274285318639, + "y": 5.312383879458918, + "heading": -0.16006569861045727, + "angularVelocity": -0.707865531089473, + "velocityX": 1.0686960264083267, + "velocityY": -1.2339500671757528, + "timestamp": 0.3762528930037835 }, { - "x": 1.9263848743459357, - "y": 5.543159599309593, - "heading": -2.769921496163024e-18, - "angularVelocity": -2.0300071079064193e-17, - "velocityX": 0.8123513398439022, - "velocityY": 0.0011481088765763312, - "timestamp": 2.679434528165308 + "x": 1.6277348772170532, + "y": 5.2009592989550075, + "heading": -0.22388049710097416, + "angularVelocity": -0.8480306686954192, + "velocityX": 1.2824811513696022, + "velocityY": -1.4807139370330336, + "timestamp": 0.4515034716045402 }, { - "x": 2.0398285536670646, - "y": 5.54331993103902, - "heading": -5.903589755199969e-18, - "angularVelocity": -3.365951630081127e-17, - "velocityX": 1.2185269970236072, - "velocityY": 0.0017221632968556807, - "timestamp": 2.772533556416818 + "x": 1.7403321438675061, + "y": 5.0709673174660805, + "heading": -0.29820238787936876, + "angularVelocity": -0.9876587284824841, + "velocityX": 1.4962976862668933, + "velocityY": -1.7274549100613854, + "timestamp": 0.5267540502052969 }, { - "x": 2.191086789804772, - "y": 5.5435337066740775, - "heading": -1.0884334063348694e-17, - "angularVelocity": -5.34994231591497e-17, - "velocityX": 1.6247026309348636, - "velocityY": 0.0022962176842493674, - "timestamp": 2.865632584668328 + "x": 1.869023605292211, + "y": 4.922411602704825, + "heading": -0.3829978231491767, + "angularVelocity": -1.1268409738042233, + "velocityX": 1.7101723842877663, + "velocityY": -1.9741471430995352, + "timestamp": 0.6020046288060535 }, { - "x": 2.380159571927711, - "y": 5.543800926199458, - "heading": -4.173692512602447e-18, - "angularVelocity": 7.208068308331619e-17, - "velocityX": 2.0308781485038967, - "velocityY": 0.0028702719072147666, - "timestamp": 2.958731612919838 + "x": 2.0218400866158173, + "y": 4.778612697599142, + "heading": -0.46243678265580673, + "angularVelocity": -1.0556591189563451, + "velocityX": 2.0307681902941916, + "velocityY": -1.9109342117962438, + "timestamp": 0.6772552074068102 }, { - "x": 2.5314178080654184, - "y": 5.544014701834515, - "heading": -1.8153111851912927e-18, - "angularVelocity": 2.5331965023903977e-17, - "velocityX": 1.6247026309348636, - "velocityY": 0.002296217684249368, - "timestamp": 3.0518306411713483 + "x": 2.158571908625681, + "y": 4.653385137637619, + "heading": -0.5314200564732017, + "angularVelocity": -0.9167141980846026, + "velocityX": 1.8170202083793483, + "velocityY": -1.6641408250954677, + "timestamp": 0.7525057860075669 }, { - "x": 2.644861487386547, - "y": 5.544175033563942, - "heading": -7.161538717554587e-19, - "angularVelocity": 1.1806324234469696e-17, - "velocityX": 1.2185269970236072, - "velocityY": 0.001722163296855681, - "timestamp": 3.1449296694228583 + "x": 2.279215456782309, + "y": 4.5467256453679665, + "heading": -0.5899528580218641, + "angularVelocity": -0.7778385580157354, + "velocityX": 1.603224193088739, + "velocityY": -1.4173909922412158, + "timestamp": 0.8277563646083236 }, { - "x": 2.7204906077248263, - "y": 5.544281921384678, - "heading": -1.8348593704837507e-19, - "angularVelocity": 5.72151981311354e-18, - "velocityX": 0.8123513398439022, - "velocityY": 0.0011481088765763316, - "timestamp": 3.2380286976743684 + "x": 2.383769256289076, + "y": 4.45863275029472, + "heading": -0.6380359000069764, + "angularVelocity": -0.6389723890392618, + "velocityX": 1.3894085793213427, + "velocityY": -1.1706607007051708, + "timestamp": 0.9030069432090803 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -9.055003253623357e-30, - "angularVelocity": 1.970868445148275e-18, - "velocityX": 0.4061756726920047, - "velocityY": 0.0005740544422031258, - "timestamp": 3.3311277259258785 + "x": 2.472232464576315, + "y": 4.389105550793566, + "heading": -0.6756681804644412, + "angularVelocity": -0.500092905027661, + "velocityX": 1.1755817687008607, + "velocityY": -0.9239423907952048, + "timestamp": 0.9782575218098369 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -2.6159098195352036e-29, - "angularVelocity": -1.6705479392630998e-29, - "velocityX": 6.780641024803866e-22, - "velocityY": 9.718698194414986e-25, - "timestamp": 3.4242267541773885 - }, - { - "x": 2.746560227605712, - "y": 5.55825095759549, - "heading": 0.00742109779657271, - "angularVelocity": 0.11458935665570884, - "velocityX": -0.18135392067783562, - "velocityY": 0.21487100869168943, - "timestamp": 3.4889892941999308 - }, - { - "x": 2.7232728362575633, - "y": 5.58624983061418, - "heading": 0.022298554503038515, - "angularVelocity": 0.22972318104396974, - "velocityX": -0.35958119215279744, - "velocityY": 0.43233129844728024, - "timestamp": 3.553751834222473 - }, - { - "x": 2.6887224275841572, - "y": 5.628555352569631, - "heading": 0.04468536543200197, - "angularVelocity": 0.3456753073794072, - "velocityX": -0.5334937243255126, - "velocityY": 0.6532406224451052, - "timestamp": 3.618514374245015 - }, - { - "x": 2.6433185485147903, - "y": 5.685479106691384, - "heading": 0.07465726375014338, - "angularVelocity": 0.46279683143540923, - "velocityX": -0.7010824321214573, - "velocityY": 0.8789611108819849, - "timestamp": 3.6832769142675574 - }, - { - "x": 2.587716352440023, - "y": 5.7574833976129485, - "heading": 0.11231839668543797, - "angularVelocity": 0.5815264954429781, - "velocityX": -0.8585549000303846, - "velocityY": 1.1118200567257257, - "timestamp": 3.7480394542900997 - }, - { - "x": 2.523118270580454, - "y": 5.845312686573325, - "heading": 0.15779630703388142, - "angularVelocity": 0.7022255509529696, - "velocityX": -0.9974605973929359, - "velocityY": 1.356174247177545, - "timestamp": 3.812801994312642 - }, - { - "x": 2.4523262633845233, - "y": 5.950260122565857, - "heading": 0.21110300167691656, - "angularVelocity": 0.8231100050195744, - "velocityX": -1.093101153402716, - "velocityY": 1.6204959835732518, - "timestamp": 3.877564534335184 - }, - { - "x": 2.385035672029833, - "y": 6.073114038949963, - "heading": 0.27020607963640353, - "angularVelocity": 0.9126121047586251, - "velocityX": -1.0390357038384854, - "velocityY": 1.896990395085535, - "timestamp": 3.9423270743577263 - }, - { - "x": 2.335844965932351, - "y": 6.198572107213467, - "heading": 0.32662517660992413, - "angularVelocity": 0.8711686872362097, - "velocityX": -0.7595549229594762, - "velocityY": 1.9372011693771587, - "timestamp": 4.007089614380269 - }, - { - "x": 2.303814652096041, - "y": 6.317831411051833, - "heading": 0.37736366027868895, - "angularVelocity": 0.7834541951428096, - "velocityX": -0.4945808769260876, - "velocityY": 1.8414858928765832, - "timestamp": 4.071852154402811 - }, - { - "x": 2.2877361979926687, - "y": 6.4282644247646346, - "heading": 0.42161092558434743, - "angularVelocity": 0.6832231300726793, - "velocityX": -0.24826781188285701, - "velocityY": 1.7051989263293559, - "timestamp": 4.1366146944253535 - }, - { - "x": 2.2869063645502266, - "y": 6.528717533266064, - "heading": 0.45901061919668296, - "angularVelocity": 0.5774896043193742, - "velocityX": -0.012813478936328582, - "velocityY": 1.5510989603938259, - "timestamp": 4.201377234447896 - }, - { - "x": 2.3008846173823154, - "y": 6.618554541912377, - "heading": 0.48936477750092344, - "angularVelocity": 0.46869931743991045, - "velocityX": 0.21583855153339288, - "velocityY": 1.3871754970549879, - "timestamp": 4.266139774470438 + "x": 2.5446045532635506, + "y": 4.33814345127288, + "heading": -0.7028480646839942, + "angularVelocity": -0.36119169745865176, + "velocityX": 0.9617479364681578, + "velocityY": -0.6772319956645083, + "timestamp": 1.0535081004105937 }, { - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "angularVelocity": 0.3580100794534351, - "velocityX": 0.43988065652810937, - "velocityY": 1.21706589595492, - "timestamp": 4.33090231449298 + "x": 2.6008851981181857, + "y": 4.305746065234092, + "heading": -0.7195737326656834, + "angularVelocity": -0.2222663040297333, + "velocityX": 0.7479097955259911, + "velocityY": -0.43052673668740377, + "timestamp": 1.1287586790113504 }, { - "x": 2.3907092837867907, - "y": 6.780867422722411, - "heading": 0.5304481214064672, - "angularVelocity": 0.21222434211791516, - "velocityX": 0.7273100577595625, - "velocityY": 0.990024458199304, - "timestamp": 4.415236192632761 + "x": 2.641074232135465, + "y": 4.291913170423193, + "heading": -0.7258433490393724, + "angularVelocity": -0.0833165204875385, + "velocityX": 0.5340694352744676, + "velocityY": -0.1838244312283474, + "timestamp": 1.204009257612107 }, { - "x": 2.471644870352802, - "y": 6.840510057948011, - "heading": 0.53507672796877, - "angularVelocity": 0.05488430823293737, - "velocityX": 0.9597043128013546, - "velocityY": 0.7072203548702543, - "timestamp": 4.499570070772541 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05565716597974986, + "velocityX": 0.3202286486370387, + "velocityY": 0.06287682191127338, + "timestamp": 1.2792598362128638 }, { - "x": 2.556252265889606, - "y": 6.8694746689081185, - "heading": 0.5277386544868528, - "angularVelocity": -0.08701216692246314, - "velocityX": 1.0032432683407548, - "velocityY": 0.3434516661512893, - "timestamp": 4.5839039489123214 + "x": 2.673109423896828, + "y": 4.320288496439454, + "heading": -0.7067986875650104, + "angularVelocity": 0.19580508899656304, + "velocityX": 0.10461880978884638, + "velocityY": 0.3116212207833284, + "timestamp": 1.3551333878515968 }, { - "x": 2.617383928971812, - "y": 6.87815517989328, - "heading": 0.5183265349753615, - "angularVelocity": -0.1116054392268187, - "velocityX": 0.7248766976052303, - "velocityY": 0.1029302953526471, - "timestamp": 4.668237827052102 + "x": 2.664688214670906, + "y": 4.3628057108082405, + "heading": -0.6813129138970213, + "angularVelocity": 0.3358979923509836, + "velocityX": -0.1109900491548022, + "velocityY": 0.5603693704926264, + "timestamp": 1.4310069394903298 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -0.06849104420156939, - "velocityX": 0.36711922260971214, - "velocityY": 0.01766420444115541, - "timestamp": 4.752571705191882 + "x": 2.6399081472529042, + "y": 4.424196735750699, + "heading": -0.6452019329979721, + "angularVelocity": 0.47593634565822673, + "velocityX": -0.3265969087089792, + "velocityY": 0.8091228579205605, + "timestamp": 1.5068804911290627 }, { - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "angularVelocity": -1.2403630086849167e-25, - "velocityX": 1.2566075146445203e-23, - "velocityY": 7.418296886220231e-24, - "timestamp": 4.836905583331663 - }, - { - "x": 2.65710997558339, - "y": 6.867790932222418, - "heading": 0.5069947283791145, - "angularVelocity": -0.09538266194620745, - "velocityX": 0.1504894284206614, - "velocityY": -0.20351386841397834, - "timestamp": 4.895151926089623 - }, - { - "x": 2.6746461929799414, - "y": 6.844083289351916, - "heading": 0.4960269163567629, - "angularVelocity": -0.18830044090369472, - "velocityX": 0.3010698451819098, - "velocityY": -0.40702371596133824, - "timestamp": 4.953398268847583 - }, - { - "x": 2.7009592416637416, - "y": 6.808522263895978, - "heading": 0.4798144698796375, - "angularVelocity": -0.27834273723408604, - "velocityX": 0.4517545211918758, - "velocityY": -0.6105280395665419, - "timestamp": 5.0116446116055435 - }, - { - "x": 2.736056173569292, - "y": 6.76110833670321, - "heading": 0.4585545185998355, - "angularVelocity": -0.3650006210372158, - "velocityX": 0.6025602680565616, - "velocityY": -0.8140241077417322, - "timestamp": 5.069890954363504 - }, - { - "x": 2.7799452908871816, - "y": 6.701842252248569, - "heading": 0.4324815844111489, - "angularVelocity": -0.4476321251109512, - "velocityX": 0.7535085507474468, - "velocityY": -1.017507394428501, - "timestamp": 5.128137297121464 - }, - { - "x": 2.8326365093294883, - "y": 6.63072517677858, - "heading": 0.40187833162186215, - "angularVelocity": -0.5254107183425597, - "velocityX": 0.9046270709435361, - "velocityY": -1.2209706584585491, - "timestamp": 5.186383639879424 - }, - { - "x": 2.8941418577048204, - "y": 6.54775894755859, - "heading": 0.367091043865372, - "angularVelocity": -0.5972441548999393, - "velocityX": 1.0559521072578695, - "velocityY": -1.4244023794721719, - "timestamp": 5.244629982637385 - }, - { - "x": 2.9644761872265666, - "y": 6.452946486925878, - "heading": 0.3285529983813312, - "angularVelocity": -0.661638888542476, - "velocityX": 1.2075321160337456, - "velocityY": -1.6277839284553843, - "timestamp": 5.302876325395345 - }, - { - "x": 3.043658216776231, - "y": 6.346292539288771, - "heading": 0.2868218829421827, - "angularVelocity": -0.7164589820267359, - "velocityX": 1.359433499176103, - "velocityY": -1.8310840232545251, - "timestamp": 5.361122668153305 - }, - { - "x": 3.131712135507504, - "y": 6.227805106020448, - "heading": 0.24264403908378393, - "angularVelocity": -0.7584655407804345, - "velocityX": 1.5117501728336273, - "velocityY": -2.03424674679905, - "timestamp": 5.419369010911265 - }, - { - "x": 3.2286701423571795, - "y": 6.097498584088062, - "heading": 0.19707481942882135, - "angularVelocity": -0.7823533203504854, - "velocityX": 1.6646196526463415, - "velocityY": -2.2371622965903253, - "timestamp": 5.477615353669226 - }, - { - "x": 3.33457642569322, - "y": 5.9554017916384385, - "heading": 0.1517318182422537, - "angularVelocity": -0.7784694976470596, - "velocityX": 1.8182477786824947, - "velocityY": -2.439583083183451, - "timestamp": 5.535861696427186 - }, - { - "x": 3.449491590462662, - "y": 5.801583658823103, - "heading": 0.10942509922281701, - "angularVelocity": -0.726341209013589, - "velocityX": 1.9729163983216391, - "velocityY": -2.6408204452341257, - "timestamp": 5.594108039185146 - }, - { - "x": 3.5734690777406897, - "y": 5.636270409713289, - "heading": 0.07620523271942112, - "angularVelocity": -0.5703339459687529, - "velocityX": 2.128502518917107, - "velocityY": -2.838173888389265, - "timestamp": 5.652354381943106 - }, - { - "x": 3.704953237459551, - "y": 5.460551173556115, - "heading": 0.07376572964977675, - "angularVelocity": -0.04188251062872067, - "velocityX": 2.2573805237049305, - "velocityY": -3.016828659738646, - "timestamp": 5.710600724701067 - }, - { - "x": 3.8484687741574635, - "y": 5.2934320188975885, - "heading": 0.07376572135485625, - "angularVelocity": -1.4241101012224094e-7, - "velocityX": 2.4639407369194757, - "velocityY": -2.86917850538672, - "timestamp": 5.768847067459027 - }, - { - "x": 4.002883086238839, - "y": 5.136327781933491, - "heading": 0.07376571311141436, - "angularVelocity": -1.4152720162027837e-7, - "velocityX": 2.651055925056733, - "velocityY": -2.697237792541529, - "timestamp": 5.827093410216987 + "x": 2.5987694501527234, + "y": 4.5044620989064414, + "heading": -0.5984691831252384, + "angularVelocity": 0.6159293833409661, + "velocityX": -0.542200756545013, + "velocityY": 1.05788329954391, + "timestamp": 1.5827540427677957 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0.07376570463306892, - "angularVelocity": -1.4556013372477054e-7, - "velocityX": 2.826202837221629, - "velocityY": -2.51311892102558, - "timestamp": 5.885339752974947 - }, - { - "x": 4.268981030537756, - "y": 4.906736402889882, - "heading": 0.07376569669894775, - "angularVelocity": -2.2864637143597095e-7, - "velocityX": 2.9245183697828536, - "velocityY": -2.397995023127166, - "timestamp": 5.920040155676169 - }, - { - "x": 4.373711969606293, - "y": 4.827653183122543, - "heading": 0.07376568907042552, - "angularVelocity": -2.198395881628289e-7, - "velocityX": 3.0181476558152474, - "velocityY": -2.2790288760699386, - "timestamp": 5.954740558377391 - }, - { - "x": 4.481524054047376, - "y": 4.75282484787289, - "heading": 0.0737656816823934, - "angularVelocity": -2.129091176782688e-7, - "velocityX": 3.106940440125975, - "velocityY": -2.1564111487103386, - "timestamp": 5.989440961078612 - }, - { - "x": 4.592244511621915, - "y": 4.682371277147909, - "heading": 0.07376567447691734, - "angularVelocity": -2.0764819690223906e-7, - "velocityX": 3.1907542551556984, - "velocityY": -2.030338706198957, - "timestamp": 6.024141363779834 - }, - { - "x": 4.705695889724197, - "y": 4.616405299617974, - "heading": 0.07376566740123429, - "angularVelocity": -2.0390780833167242e-7, - "velocityX": 3.2694542215871127, - "velocityY": -1.90101475472538, - "timestamp": 6.058841766481056 - }, - { - "x": 4.821696252971789, - "y": 4.555032332206395, - "heading": 0.07376566040606643, - "angularVelocity": -2.0158751247220773e-7, - "velocityX": 3.342911154270497, - "velocityY": -1.76865288682714, - "timestamp": 6.093542169182277 - }, - { - "x": 4.939640387985606, - "y": 4.497483166935729, - "heading": 0.07376565344322925, - "angularVelocity": -2.0065580355812287e-7, - "velocityX": 3.398926981607191, - "velocityY": -1.6584581385460047, - "timestamp": 6.128242571883499 - }, - { - "x": 5.057585335343478, - "y": 4.439935666553369, - "heading": 0.07376564648040178, - "angularVelocity": -2.0065552340961329e-7, - "velocityX": 3.3989503918269923, - "velocityY": -1.658410159612758, - "timestamp": 6.162942974584721 - }, - { - "x": 5.1755309999931605, - "y": 4.382389636300601, - "heading": 0.07376563951757731, - "angularVelocity": -2.0065543705761684e-7, - "velocityX": 3.398971062820868, - "velocityY": -1.658367793257397, - "timestamp": 6.197643377285942 - }, - { - "x": 5.295248531429034, - "y": 4.328626841825402, - "heading": 0.07376563255290919, - "angularVelocity": -2.007085673925138e-7, - "velocityX": 3.45003291364278, - "velocityY": -1.5493420908716486, - "timestamp": 6.232343779987164 - }, - { - "x": 5.41702166916594, - "y": 4.27969803322079, - "heading": 0.07376562554616754, - "angularVelocity": -2.019210467909896e-7, - "velocityX": 3.5092716008341425, - "velocityY": -1.410035757391629, - "timestamp": 6.267044182688386 - }, - { - "x": 5.540655427942115, - "y": 4.235681910653619, - "heading": 0.07376561844909023, - "angularVelocity": -2.0452435036769212e-7, - "velocityX": 3.5628911814277564, - "velocityY": -1.2684614338963205, - "timestamp": 6.301744585389607 - }, - { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.07376561120324407, - "angularVelocity": -2.0881158710741676e-7, - "velocityX": 3.610802501559291, - "velocityY": -1.1248525394721576, - "timestamp": 6.336444988090829 - }, - { - "x": 5.801240954312848, - "y": 4.160760783914465, - "heading": 0.0737656041692584, - "angularVelocity": -1.900588111168926e-7, - "velocityX": 3.655524839727154, - "velocityY": -0.9697042570289608, - "timestamp": 6.37345450968449 - }, - { - "x": 5.937938725958539, - "y": 4.130679839530998, - "heading": 0.07376559729974105, - "angularVelocity": -1.8561486466113367e-7, - "velocityX": 3.6935838605680487, - "velocityY": -0.8127893333433076, - "timestamp": 6.410464031278151 - }, - { - "x": 6.075795864969434, - "y": 4.106460978217925, - "heading": 0.07376559053845443, - "angularVelocity": -1.8269046288328192e-7, - "velocityX": 3.724910052187935, - "velocityY": -0.654395416914037, - "timestamp": 6.447473552871812 - }, - { - "x": 6.214561046214486, - "y": 4.088147908867804, - "heading": 0.07376558383255526, - "angularVelocity": -1.8119388963756882e-7, - "velocityX": 3.7494454202515035, - "velocityY": -0.4948204829877394, - "timestamp": 6.484483074465473 - }, - { - "x": 6.353981153300122, - "y": 4.0757714079596425, - "heading": 0.07376557713086826, - "angularVelocity": -1.8108007594786474e-7, - "velocityX": 3.7671415647133246, - "velocityY": -0.33441396633135523, - "timestamp": 6.521492596059134 - }, - { - "x": 6.493802446577043, - "y": 4.0693566803624766, - "heading": 0.07376555560001419, - "angularVelocity": -5.817652634682061e-7, - "velocityX": 3.7779816451577393, - "velocityY": -0.17332641225670456, - "timestamp": 6.558502117652795 - }, - { - "x": 6.62912467253087, - "y": 4.064792329127301, - "heading": 0.05903973203041549, - "angularVelocity": -0.39789283772100603, - "velocityX": 3.656416514635636, - "velocityY": -0.12332910663606324, - "timestamp": 6.595511639246456 - }, - { - "x": 6.758568056447626, - "y": 4.061299539806061, - "heading": 0.04418585550364903, - "angularVelocity": -0.4013528380575057, - "velocityX": 3.4975697696920878, - "velocityY": -0.09437542477821895, - "timestamp": 6.632521160840117 - }, - { - "x": 6.882108384071856, - "y": 4.058691153499139, - "heading": 0.0304778500113165, - "angularVelocity": -0.3703913182893011, - "velocityX": 3.3380687537822458, - "velocityY": -0.07047879017620408, - "timestamp": 6.669530682433778 - }, - { - "x": 6.999742512686264, - "y": 4.056896966002031, - "heading": 0.018392940314004957, - "angularVelocity": -0.3265351503322698, - "velocityX": 3.1784828214195313, - "velocityY": -0.048479078351971995, - "timestamp": 6.706540204027439 - }, - { - "x": 7.111470092404711, - "y": 4.055880215977976, - "heading": 0.008182013942056482, - "angularVelocity": -0.2758999828221868, - "velocityX": 3.0188874351076422, - "velocityY": -0.027472660555273344, - "timestamp": 6.7435497256211 + "x": 2.5412724247726852, + "y": 4.603602430919982, + "heading": -0.5411158658534186, + "angularVelocity": 0.7559065844828777, + "velocityX": -0.7578006319478658, + "velocityY": 1.3066520529527457, + "timestamp": 1.6586275944065287 }, { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -2.0237932968568665e-24, - "angularVelocity": -0.22107861949390495, - "velocityX": 2.859298314909037, - "velocityY": -0.007077363713034477, - "timestamp": 6.780559247214761 - }, - { - "x": 7.386520754967261, - "y": 4.057483355651357, - "heading": -0.007799169879435119, - "angularVelocity": -0.11872019703645657, - "velocityX": 2.5760366812439752, - "velocityY": 0.028390383098604113, - "timestamp": 6.846252954554551 - }, - { - "x": 7.536988258489095, - "y": 4.060513288338981, - "heading": -0.011310959722615328, - "angularVelocity": -0.053457020244208106, - "velocityX": 2.2904401291217464, - "velocityY": 0.04612211443560253, - "timestamp": 6.911946661894341 - }, - { - "x": 7.668645085347693, - "y": 4.063976331623079, - "heading": -0.012040004750973126, - "angularVelocity": -0.01109763869143412, - "velocityX": 2.004101034785968, - "velocityY": 0.05271499241448283, - "timestamp": 6.9776403692341304 - }, - { - "x": 7.78147865771714, - "y": 4.06737291870627, - "heading": -0.011006488514551415, - "angularVelocity": 0.015732347560718764, - "velocityX": 1.7175704787953916, - "velocityY": 0.051703385616870434, - "timestamp": 7.04333407657392 - }, - { - "x": 7.87549031086396, - "y": 4.070341151997571, - "heading": -0.008947695308103269, - "angularVelocity": 0.03133927570565307, - "velocityX": 1.4310602484429864, - "velocityY": 0.04518291646943911, - "timestamp": 7.10902778391371 - }, - { - "x": 7.9506868858752515, - "y": 4.072607124350486, - "heading": -0.006421398747140829, - "angularVelocity": 0.03845568568532146, - "velocityX": 1.1446541542000461, - "velocityY": 0.03449298943040145, - "timestamp": 7.1747214912535 - }, - { - "x": 8.00707724488628, - "y": 4.073956458693622, - "heading": -0.003864315760597132, - "angularVelocity": 0.038924321523180636, - "velocityX": 0.8583829607812892, - "velocityY": 0.020539780715319512, - "timestamp": 7.24041519859329 - }, - { - "x": 8.044670745645819, - "y": 4.074216878653992, - "heading": -0.0016276569414438076, - "angularVelocity": 0.034046774184696094, - "velocityX": 0.5722542124939418, - "velocityY": 0.003964153811924349, - "timestamp": 7.30610890593308 + "x": 2.467417405939932, + "y": 4.7216183787073325, + "heading": -0.473137956706953, + "angularVelocity": 0.895936827501342, + "velocityX": -0.9733960943919171, + "velocityY": 1.5554293325984934, + "timestamp": 1.7345011460452617 }, { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 1.6866276328916314e-24, - "angularVelocity": 0.02477645131252876, - "velocityX": 0.286265117553968, - "velocityY": -0.014764317948939539, - "timestamp": 7.37180261327287 - }, - { - "x": 8.060961534281953, - "y": 4.070465444687161, - "heading": 0.0007560599723107442, - "angularVelocity": 0.010267428471842888, - "velocityX": -0.03415452911035645, - "velocityY": -0.03777341504058265, - "timestamp": 7.445439354389123 - }, - { - "x": 8.034839704830464, - "y": 4.066167075236277, - "heading": 0.00046881224437648735, - "angularVelocity": -0.0039008750737728444, - "velocityX": -0.3547390752973492, - "velocityY": -0.0583726192349958, - "timestamp": 7.519076095505376 - }, - { - "x": 7.985098416570273, - "y": 4.060562217226684, - "heading": -0.0008319841828060777, - "angularVelocity": -0.017665046109644415, - "velocityX": -0.6754955136004895, - "velocityY": -0.0761149654999585, - "timestamp": 7.592712836621629 - }, - { - "x": 7.9117249252468085, - "y": 4.053904178499818, - "heading": -0.0031104504440480647, - "angularVelocity": -0.03094197579500262, - "velocityX": -0.9964250211402843, - "velocityY": -0.09041734636729296, - "timestamp": 7.666349577737883 - }, - { - "x": 7.814707316285252, - "y": 4.046503723453766, - "heading": -0.00632248118792148, - "angularVelocity": -0.04361994698818144, - "velocityX": -1.3175163307185394, - "velocityY": -0.10049949161068944, - "timestamp": 7.739986318854136 - }, - { - "x": 7.69403642851654, - "y": 4.038750888931838, - "heading": -0.010412565479377186, - "angularVelocity": -0.055544069841419484, - "velocityX": -1.6387320505969085, - "velocityY": -0.10528486736925086, - "timestamp": 7.813623059970389 - }, - { - "x": 7.549709960346403, - "y": 4.031149265281445, - "heading": -0.015308761000708897, - "angularVelocity": -0.06649120326498265, - "velocityX": -1.9599790265335464, - "velocityY": -0.10323139692441612, - "timestamp": 7.887259801086643 - }, - { - "x": 7.381741634294396, - "y": 4.0243729190236985, - "heading": -0.02091427607708914, - "angularVelocity": -0.07612388858342581, - "velocityX": -2.2810396482216517, - "velocityY": -0.0920239836123183, - "timestamp": 7.960896542202896 - }, - { - "x": 7.1901832106568095, - "y": 4.019367673272749, - "heading": -0.027092292311801627, - "angularVelocity": -0.08389855581684406, - "velocityX": -2.6013973559091212, - "velocityY": -0.06797212471757884, - "timestamp": 8.03453328331915 - }, - { - "x": 6.975183472586086, - "y": 4.017547771662454, - "heading": -0.03363583639032012, - "angularVelocity": -0.08886248874305737, - "velocityX": -2.9197345620074877, - "velocityY": -0.02471458653257856, - "timestamp": 8.108170024435402 - }, - { - "x": 6.737173435740335, - "y": 4.021221144917956, - "heading": -0.04019984018910067, - "angularVelocity": -0.0891403353716819, - "velocityX": -3.232218499050576, - "velocityY": 0.04988506009116617, - "timestamp": 8.181806765551656 - }, - { - "x": 6.477592542757965, - "y": 4.034606496898433, - "heading": -0.04612188209053887, - "angularVelocity": -0.08042237898726157, - "velocityX": -3.5251545498538244, - "velocityY": 0.18177545308999152, - "timestamp": 8.255443506667909 - }, - { - "x": 6.202379983792451, - "y": 4.065741183912862, - "heading": -0.04994434785858321, - "angularVelocity": -0.05190976284528472, - "velocityX": -3.737435345366867, - "velocityY": 0.4228145697713012, - "timestamp": 8.329080247784162 - }, - { - "x": 5.92930929060311, - "y": 4.120418111584284, - "heading": -0.049944382856938176, - "angularVelocity": -4.752838655296773e-7, - "velocityX": -3.7083484283780552, - "velocityY": 0.7425223718836313, - "timestamp": 8.402716988900416 + "x": 2.37720453797495, + "y": 4.858510302970022, + "heading": -0.3945211456551798, + "angularVelocity": 1.0361556741946414, + "velocityX": -1.1889896547134935, + "velocityY": 1.8042113662279782, + "timestamp": 1.8103746976839947 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -0.05351191132153618, - "angularVelocity": -0.04844766906462906, - "velocityX": -3.5764423817525626, - "velocityY": 1.0352299927261919, - "timestamp": 8.476353730016669 - }, - { - "x": 5.427595279774474, - "y": 4.2855510061769815, - "heading": -0.058564536366975395, - "angularVelocity": -0.07109019818968403, - "velocityX": -3.3536640954167023, - "velocityY": 1.2508460219498765, - "timestamp": 8.547427171561418 - }, - { - "x": 5.210001648120265, - "y": 4.381839057848659, - "heading": -0.06451807274962795, - "angularVelocity": -0.08376597858855642, - "velocityX": -3.0615322253278787, - "velocityY": 1.3547683857556179, - "timestamp": 8.618500613106168 - }, - { - "x": 5.014433901254649, - "y": 4.478870971040357, - "heading": -0.07079419051912546, - "angularVelocity": -0.08830468362146032, - "velocityX": -2.751629056016378, - "velocityY": 1.365234482568344, - "timestamp": 8.689574054650917 - }, - { - "x": 4.840611358699117, - "y": 4.572318839972298, - "heading": -0.07701240147326043, - "angularVelocity": -0.08748993743633168, - "velocityX": -2.445675047916283, - "velocityY": 1.3148071473801226, - "timestamp": 8.760647496195666 - }, - { - "x": 4.687900440256207, - "y": 4.6594427038525446, - "heading": -0.08292539389195669, - "angularVelocity": -0.08319552691103697, - "velocityX": -2.148635483575938, - "velocityY": 1.2258286919367853, - "timestamp": 8.831720937740416 - }, - { - "x": 4.555672965258425, - "y": 4.738435503151125, - "heading": -0.088365740210127, - "angularVelocity": -0.07654541837185252, - "velocityX": -1.8604343918610988, - "velocityY": 1.1114249933829867, - "timestamp": 8.902794379285165 - }, - { - "x": 4.443385396304237, - "y": 4.808043311257638, - "heading": -0.09321471505627854, - "angularVelocity": -0.06822484940592717, - "velocityX": -1.5798808459765967, - "velocityY": 0.9793786060392976, - "timestamp": 8.973867820829915 - }, - { - "x": 4.3505830000627, - "y": 4.867355530244293, - "heading": -0.09738458875338916, - "angularVelocity": -0.058669927985478765, - "velocityX": -1.3057253768006336, - "velocityY": 0.8345201484201303, - "timestamp": 9.044941262374664 - }, - { - "x": 4.2768869568191645, - "y": 4.915685373463825, - "heading": -0.10080825239426806, - "angularVelocity": -0.04817078737805652, - "velocityX": -1.0368998833007734, - "velocityY": 0.6799986347798151, - "timestamp": 9.116014703919413 - }, - { - "x": 4.22197990513087, - "y": 4.95249852612905, - "heading": -0.10343286860563979, - "angularVelocity": -0.03692822739868577, - "velocityX": -0.7725396504645569, - "velocityY": 0.5179593370618798, - "timestamp": 9.187088145464163 - }, - { - "x": 4.185593591353172, - "y": 4.97736854606154, - "heading": -0.10521581647883109, - "angularVelocity": -0.025085993226720307, - "velocityX": -0.5119537338682039, - "velocityY": 0.3499200178287514, - "timestamp": 9.258161587008912 + "x": 2.2706323270523923, + "y": 5.014276850913776, + "heading": -0.3052336725466659, + "angularVelocity": 1.1767931140702435, + "velocityX": -1.4046029033952228, + "velocityY": 2.052975570268153, + "timestamp": 1.8862482493227277 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.012749959780663622, - "velocityX": -0.25458913429159136, - "velocityY": 0.1769894567221649, - "timestamp": 9.329235028553661 + "x": 2.156211312441972, + "y": 5.146461367106831, + "heading": -0.22910323737059946, + "angularVelocity": 1.0033856796180658, + "velocityX": -1.5080487487284289, + "velocityY": 1.742168559900677, + "timestamp": 1.9621218009614607 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": 1.153459712624117e-25, - "velocityX": -3.247224190829055e-25, - "velocityY": -4.023670796703132e-25, - "timestamp": 9.40030847009841 - }, - { - "x": 4.180652954196475, - "y": 4.972113880358222, - "heading": -0.10282545923039073, - "angularVelocity": 0.04623963021087373, - "velocityX": 0.18450581877512717, - "velocityY": -0.2501512088012644, - "timestamp": 9.471601011898064 - }, - { - "x": 4.207780209062833, - "y": 4.937082603957029, - "heading": -0.09606696501055557, - "angularVelocity": 0.09479945656626756, - "velocityX": 0.380506209788268, - "velocityY": -0.4913736488683167, - "timestamp": 9.542893553697718 - }, - { - "x": 4.249890859346282, - "y": 4.885729175148701, - "heading": -0.08564531821991797, - "angularVelocity": 0.1461814451773148, - "velocityX": 0.5906739922639744, - "velocityY": -0.7203197909907696, - "timestamp": 9.614186095497372 - }, - { - "x": 4.308238374054552, - "y": 4.819303841665251, - "heading": -0.07131614738285201, - "angularVelocity": 0.20099116226398103, - "velocityX": 0.8184238243635266, - "velocityY": -0.931729067398359, - "timestamp": 9.685478637297026 - }, - { - "x": 4.384365934500173, - "y": 4.739668033977246, - "heading": -0.05278830976865122, - "angularVelocity": 0.2598846547829301, - "velocityX": 1.0678194173459812, - "velocityY": -1.1170285934228166, - "timestamp": 9.75677117909668 - }, - { - "x": 4.480080041577144, - "y": 4.649696456670524, - "heading": -0.02973745107706772, - "angularVelocity": 0.3233277718777487, - "velocityX": 1.3425542793234355, - "velocityY": -1.262005464183884, - "timestamp": 9.828063720896333 - }, - { - "x": 4.597147014770728, - "y": 4.553865445568556, - "heading": -0.0018737410680010795, - "angularVelocity": 0.3908362544762254, - "velocityX": 1.642064797220515, - "velocityY": -1.344194058493121, - "timestamp": 9.899356262695987 - }, - { - "x": 4.7363464626810785, - "y": 4.458651640127929, - "heading": 0.030870956626381736, - "angularVelocity": 0.4593004663293065, - "velocityX": 1.9525106609542362, - "velocityY": -1.3355366920174645, - "timestamp": 9.97064880449564 - }, - { - "x": 4.8961444414371185, - "y": 4.371572487351198, - "heading": 0.06809613303796244, - "angularVelocity": 0.5221468539611136, - "velocityX": 2.2414403347422116, - "velocityY": -1.2214342563551692, - "timestamp": 10.041941346295294 - }, - { - "x": 5.072824953795758, - "y": 4.298842121244781, - "heading": 0.10897366885891092, - "angularVelocity": 0.5733774499978155, - "velocityX": 2.478246782884325, - "velocityY": -1.0201679484342572, - "timestamp": 10.113233888094948 - }, - { - "x": 5.262214444102689, - "y": 4.244271073764123, - "heading": 0.1525665304554643, - "angularVelocity": 0.6114645444828949, - "velocityX": 2.6565119650124474, - "velocityY": -0.7654524036190654, - "timestamp": 10.184526429894602 - }, - { - "x": 5.460811083302549, - "y": 4.20986764407754, - "heading": 0.19806311217601916, - "angularVelocity": 0.638167479684045, - "velocityX": 2.785657997129001, - "velocityY": -0.48256702339585567, - "timestamp": 10.255818971694255 + "x": 2.058134458607592, + "y": 5.259759379647075, + "heading": -0.16374337318870086, + "angularVelocity": 0.8614314576060107, + "velocityX": -1.2926355985202171, + "velocityY": 1.4932477799347985, + "timestamp": 2.0379953526001935 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.24483305037299086, - "angularVelocity": 0.6560284851170571, - "velocityX": 2.8774488935285283, - "velocityY": -0.18541307673169433, - "timestamp": 10.327111513493909 - }, - { - "x": 5.802993350275697, - "y": 4.197353588266771, - "heading": 0.2759731999769641, - "angularVelocity": 0.6634007595054764, - "velocityX": 2.919495150576939, - "velocityY": 0.015008756790958989, - "timestamp": 10.374051691294106 - }, - { - "x": 5.941816707126591, - "y": 4.207504283843094, - "heading": 0.3073984177151568, - "angularVelocity": 0.669473768760635, - "velocityX": 2.9574527272095095, - "velocityY": 0.21624748886826958, - "timestamp": 10.420991869094303 - }, - { - "x": 6.082189718099688, - "y": 4.227142107895406, - "heading": 0.33903536512016264, - "angularVelocity": 0.6739843964730912, - "velocityX": 2.990466111368347, - "velocityY": 0.4183585357494954, - "timestamp": 10.4679320468945 - }, - { - "x": 6.223826665767993, - "y": 4.256309424549035, - "heading": 0.37079417516105295, - "angularVelocity": 0.6765805228960279, - "velocityX": 3.01739265392617, - "velocityY": 0.6213720957295971, - "timestamp": 10.514872224694697 - }, - { - "x": 6.366368317456564, - "y": 4.295047066484075, - "heading": 0.4025622544615229, - "angularVelocity": 0.6767779925268211, - "velocityX": 3.0366662072586053, - "velocityY": 0.8252555433413251, - "timestamp": 10.561812402494894 - }, - { - "x": 6.509351710185931, - "y": 4.3433874902945675, - "heading": 0.4341948054163167, - "angularVelocity": 0.6738907357666759, - "velocityX": 3.0460769308966165, - "velocityY": 1.0298304368648719, - "timestamp": 10.608752580295091 - }, - { - "x": 6.652162883267978, - "y": 4.401339041781646, - "heading": 0.4654999103087445, - "angularVelocity": 0.6669149193613809, - "velocityX": 3.0424080132360682, - "velocityY": 1.2345831269270133, - "timestamp": 10.655692758095288 - }, - { - "x": 6.793961557546229, - "y": 4.468849069286898, - "heading": 0.4962146580453544, - "angularVelocity": 0.654338120902485, - "velocityX": 3.0208380309469742, - "velocityY": 1.4382141412546399, - "timestamp": 10.702632935895485 - }, - { - "x": 6.933563512190389, - "y": 4.545714915555555, - "heading": 0.5259679790741631, - "angularVelocity": 0.6338561638060835, - "velocityX": 2.9740397498786737, - "velocityY": 1.6375278039175576, - "timestamp": 10.749573113695682 - }, - { - "x": 7.069284940391168, - "y": 4.631368590173797, - "heading": 0.5542337619189218, - "angularVelocity": 0.6021660796657563, - "velocityX": 2.8913701345248803, - "velocityY": 1.8247411627375887, - "timestamp": 10.796513291495879 - }, - { - "x": 7.1988866969659675, - "y": 4.724417833549329, - "heading": 0.5803339735600787, - "angularVelocity": 0.5560313757705206, - "velocityX": 2.7609984164621943, - "velocityY": 1.9822942250367808, - "timestamp": 10.843453469296076 - }, - { - "x": 7.32010179733884, - "y": 4.822118790801402, - "heading": 0.6036855499367045, - "angularVelocity": 0.49747524340496785, - "velocityX": 2.5823315132896965, - "velocityY": 2.0813929948868157, - "timestamp": 10.890393647096273 - }, - { - "x": 7.431787144885325, - "y": 4.920866698749227, - "heading": 0.6241547457610245, - "angularVelocity": 0.43606984003016963, - "velocityX": 2.379312409549823, - "velocityY": 2.103696930338622, - "timestamp": 10.93733382489647 - }, - { - "x": 7.534091847804722, - "y": 5.017622526169069, - "heading": 0.6419335715808812, - "angularVelocity": 0.37875497394860314, - "velocityX": 2.1794698638522156, - "velocityY": 2.0612582217239557, - "timestamp": 10.984274002696667 - }, - { - "x": 7.627668873285194, - "y": 5.110394301950092, - "heading": 0.6572905320305523, - "angularVelocity": 0.32716025309998836, - "velocityX": 1.9935379426721815, - "velocityY": 1.9763831354859511, - "timestamp": 11.031214180496864 - }, - { - "x": 7.7131909565949455, - "y": 5.197947878934244, - "heading": 0.6704631268278166, - "angularVelocity": 0.2806251576918574, - "velocityX": 1.8219377794813416, - "velocityY": 1.8652161343918634, - "timestamp": 11.078154358297061 - }, - { - "x": 7.791223380396529, - "y": 5.279496011851437, - "heading": 0.6816390937397763, - "angularVelocity": 0.2380895734892706, - "velocityX": 1.6623802349818717, - "velocityY": 1.7372778872782495, - "timestamp": 11.125094536097258 - }, - { - "x": 7.86221923742719, - "y": 5.354510990808827, - "heading": 0.6909640471575174, - "angularVelocity": 0.19865611624721707, - "velocityX": 1.5124752473852405, - "velocityY": 1.5980974609149294, - "timestamp": 11.172034713897455 - }, - { - "x": 7.926540429785996, - "y": 5.422621631484432, - "heading": 0.6985519881256509, - "angularVelocity": 0.1616513043566187, - "velocityX": 1.370280117655087, - "velocityY": 1.4510094308871155, - "timestamp": 11.218974891697652 - }, - { - "x": 7.984478950500488, - "y": 5.483555793762207, - "heading": 0.7044936776046891, - "angularVelocity": 0.12658003777337848, - "velocityX": 1.2343055231087734, - "velocityY": 1.2981238063720744, - "timestamp": 11.265915069497849 - }, - { - "x": 8.047923980956542, - "y": 5.548807333254465, - "heading": 0.7094925329826927, - "angularVelocity": 0.08420982403970191, - "velocityX": 1.0687836408324876, - "velocityY": 1.099215769102154, - "timestamp": 11.325276972161923 - }, - { - "x": 8.10152245585906, - "y": 5.602265273787256, - "heading": 0.7122491464591354, - "angularVelocity": 0.04643741781732065, - "velocityX": 0.9029103262715175, - "velocityY": 0.9005429094027965, - "timestamp": 11.384638874825997 - }, - { - "x": 8.14525837183985, - "y": 5.64394060886005, - "heading": 0.7129744623769576, - "angularVelocity": 0.012218542285053226, - "velocityX": 0.7367674218309503, - "velocityY": 0.7020552442301676, - "timestamp": 11.444000777490071 - }, - { - "x": 8.179119065532058, - "y": 5.673842216437666, - "heading": 0.7118363633541982, - "angularVelocity": -0.01917221267653394, - "velocityX": 0.5704111925762211, - "velocityY": 0.5037171356657365, - "timestamp": 11.503362680154146 - }, - { - "x": 8.203094267741402, - "y": 5.691977414243475, - "heading": 0.7089715992476469, - "angularVelocity": -0.048259303997766526, - "velocityX": 0.40388197031045997, - "velocityY": 0.3055023001610055, - "timestamp": 11.56272458281822 - }, - { - "x": 8.217175483703613, - "y": 5.698352336883545, - "heading": 0.7044936776046891, - "angularVelocity": -0.07543426746777364, - "velocityX": 0.23720964676445846, - "velocityY": 0.10739080713341471, - "timestamp": 11.622086485482294 - }, - { - "x": 8.220424741398494, - "y": 5.690497924270521, - "heading": 0.6974771095930071, - "angularVelocity": -0.10438216452389025, - "velocityX": 0.04833767031445264, - "velocityY": -0.11684638248302778, - "timestamp": 11.689306475062756 - }, - { - "x": 8.210761211514034, - "y": 5.66775585785793, - "heading": 0.6884846443385353, - "angularVelocity": -0.1337766535013804, - "velocityX": -0.14375976468862095, - "velocityY": -0.338322968428447, - "timestamp": 11.756526464643217 - }, - { - "x": 8.187932028417118, - "y": 5.630349579704253, - "heading": 0.6774812110255055, - "angularVelocity": -0.1636928744218101, - "velocityX": -0.33961896214800763, - "velocityY": -0.556475512524468, - "timestamp": 11.823746454223679 - }, - { - "x": 8.15163887353547, - "y": 5.578552873386559, - "heading": 0.6644254264440203, - "angularVelocity": -0.19422473378782149, - "velocityX": -0.5399161039471123, - "velocityY": -0.7705551078030757, - "timestamp": 11.89096644380414 - }, - { - "x": 8.101525228747773, - "y": 5.512708245233119, - "heading": 0.6492678901971639, - "angularVelocity": -0.225491499499757, - "velocityX": -0.745516997257366, - "velocityY": -0.9795393983901896, - "timestamp": 11.958186433384602 - }, - { - "x": 8.037158859322696, - "y": 5.433255022389235, - "heading": 0.6319488484169726, - "angularVelocity": -0.2576471952507599, - "velocityX": -0.9575480422833206, - "velocityY": -1.1819880267725829, - "timestamp": 12.025406422965064 - }, - { - "x": 7.958007564987826, - "y": 5.340774148745389, - "heading": 0.6123949630340744, - "angularVelocity": -0.29089390678188665, - "velocityX": -1.177496379110975, - "velocityY": -1.3757942275957413, - "timestamp": 12.092626412545526 - }, - { - "x": 7.863405980160758, - "y": 5.236063150659359, - "heading": 0.5905148719771925, - "angularVelocity": -0.3254997686468183, - "velocityX": -1.4073430450897342, - "velocityY": -1.5577360059048004, - "timestamp": 12.159846402125988 - }, - { - "x": 7.752513083642703, - "y": 5.120268560189199, - "heading": 0.5661934059220105, - "angularVelocity": -0.36181895009176485, - "velocityX": -1.649701185765849, - "velocityY": -1.7226213689241165, - "timestamp": 12.22706639170645 - }, - { - "x": 7.624272333970784, - "y": 4.995132700896389, - "heading": 0.5392856965961144, - "angularVelocity": -0.40029326832441703, - "velocityX": -1.9077769941992746, - "velocityY": -1.8615870081774413, - "timestamp": 12.294286381286911 - }, - { - "x": 7.477443680694748, - "y": 4.863466852853807, - "heading": 0.5096191616794202, - "angularVelocity": -0.4413350121273609, - "velocityX": -2.1843004468229417, - "velocityY": -1.958730563101009, - "timestamp": 12.361506370867373 - }, - { - "x": 7.310999420897597, - "y": 4.729976120271212, - "heading": 0.47703933517045805, - "angularVelocity": -0.48467467359488975, - "velocityX": -2.4761125497932377, - "velocityY": -1.9858785075056882, - "timestamp": 12.428726360447834 - }, - { - "x": 7.12561513664023, - "y": 4.601908860623506, - "heading": 0.44160618415094194, - "angularVelocity": -0.5271222331432077, - "velocityX": -2.757874337892657, - "velocityY": -1.9051960651438506, - "timestamp": 12.495946350028296 - }, - { - "x": 6.9255695292750366, - "y": 4.487009075034074, - "heading": 0.4039304316786653, - "angularVelocity": -0.5604843545412803, - "velocityX": -2.9759839091575766, - "velocityY": -1.709309779822242, - "timestamp": 12.563166339608758 - }, - { - "x": 6.716882603619491, - "y": 4.389825418108365, - "heading": 0.36497586306620833, - "angularVelocity": -0.5795086975702185, - "velocityX": -3.1045367153136607, - "velocityY": -1.445755310767803, - "timestamp": 12.63038632918922 - }, - { - "x": 6.504347696212581, - "y": 4.311975808393504, - "heading": 0.32552791036504214, - "angularVelocity": -0.5868485393611587, - "velocityX": -3.1617813203096174, - "velocityY": -1.1581318325209748, - "timestamp": 12.697606318769681 - }, - { - "x": 6.29116656289608, - "y": 4.2538292064786, - "heading": 0.2861053737979932, - "angularVelocity": -0.5864704355519208, - "velocityX": -3.1713949175985174, - "velocityY": -0.8650195020530711, - "timestamp": 12.764826308350143 - }, - { - "x": 6.079464519610655, - "y": 4.215340449642915, - "heading": 0.24704786030835676, - "angularVelocity": -0.5810401598305069, - "velocityX": -3.1493911945942843, - "velocityY": -0.572579036026387, - "timestamp": 12.832046297930605 - }, - { - "x": 5.870707606275243, - "y": 4.1963435376359834, - "heading": 0.20858746560478297, - "angularVelocity": -0.5721571059980171, - "velocityX": -3.1055778889333454, - "velocityY": -0.28260807723263415, - "timestamp": 12.899266287511066 + "x": 1.976403642015832, + "y": 5.354173274627719, + "heading": -0.10920518354656349, + "angularVelocity": 0.7188036998955512, + "velocityX": -1.0771977168121305, + "velocityY": 1.2443584482531382, + "timestamp": 2.1138689042389265 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0.17089065340850093, - "angularVelocity": -0.560797650097213, - "velocityX": -3.0460563700229573, - "velocityY": 0.004545328262717531, - "timestamp": 12.966486277091528 - }, - { - "x": 5.441376331690843, - "y": 4.221257817184443, - "heading": 0.12955296713969652, - "angularVelocity": -0.5459093121032401, - "velocityX": -2.965763486744691, - "velocityY": 0.3249853335649047, - "timestamp": 13.042208902356046 - }, - { - "x": 5.2258290849692735, - "y": 4.2691932604024965, - "heading": 0.08990916431610245, - "angularVelocity": -0.5235397304980968, - "velocityX": -2.8465368965828737, - "velocityY": 0.6330399012263854, - "timestamp": 13.117931527620565 - }, - { - "x": 5.023235975053, - "y": 4.338521550335442, - "heading": 0.05269444989052449, - "angularVelocity": -0.4914609642174678, - "velocityX": -2.6754633665772425, - "velocityY": 0.9155558155936205, - "timestamp": 13.193654152885083 - }, - { - "x": 4.838427025225484, - "y": 4.425425000911894, - "heading": 0.018769229111462726, - "angularVelocity": -0.44801960656478235, - "velocityX": -2.440604101903904, - "velocityY": 1.1476550142427961, - "timestamp": 13.269376778149601 - }, - { - "x": 4.676043362291933, - "y": 4.523369450086871, - "heading": -0.011096289897494078, - "angularVelocity": -0.3944068091224933, - "velocityX": -2.144453686943697, - "velocityY": 1.2934634639624794, - "timestamp": 13.34509940341412 - }, - { - "x": 4.53850801944686, - "y": 4.62398511499902, - "heading": -0.03651765926491392, - "angularVelocity": -0.33571695749608116, - "velocityX": -1.8163044712809187, - "velocityY": 1.3287397863013088, - "timestamp": 13.420822028678637 - }, - { - "x": 4.425480168441604, - "y": 4.719703555950957, - "heading": -0.05754144603227514, - "angularVelocity": -0.2776420745308276, - "velocityX": -1.4926562650254422, - "velocityY": 1.2640665932747182, - "timestamp": 13.496544653943156 - }, - { - "x": 4.335238129667097, - "y": 4.805147915212772, - "heading": -0.07443003472477187, - "angularVelocity": -0.22303226589808134, - "velocityX": -1.1917447190884998, - "velocityY": 1.128386119252163, - "timestamp": 13.572267279207674 - }, - { - "x": 4.265861114202726, - "y": 4.876824239203163, - "heading": -0.08748354859792261, - "angularVelocity": -0.17238591276453477, - "velocityX": -0.9161992894728712, - "velocityY": 0.9465641707482745, - "timestamp": 13.647989904472192 - }, - { - "x": 4.215660401957236, - "y": 4.932459766827933, - "heading": -0.09697232555255134, - "angularVelocity": -0.1253096669784238, - "velocityX": -0.6629552537319688, - "velocityY": 0.7347279288115158, - "timestamp": 13.72371252973671 - }, - { - "x": 4.183247912596747, - "y": 4.970524954667524, - "heading": -0.10312305256683521, - "angularVelocity": -0.08122707041386726, - "velocityX": -0.4280423353953174, - "velocityY": 0.5026923948637503, - "timestamp": 13.799435155001229 + "x": 1.9110189688101253, + "y": 5.429703861532151, + "heading": -0.06553401442898629, + "angularVelocity": 0.5755782901203135, + "velocityX": -0.8617584361561702, + "velocityY": 0.9954797854217321, + "timestamp": 2.1897424558776595 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.03960437745903177, - "velocityX": -0.2079807341935533, - "velocityY": 0.25649983915041824, - "timestamp": 13.875157780265747 + "x": 1.8619803269894504, + "y": 5.486351581272176, + "heading": -0.032764600706343905, + "angularVelocity": 0.4318950808929029, + "velocityX": -0.6463206316489477, + "velocityY": 0.7466069337268786, + "timestamp": 2.2656160075163925 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -1.1491916765427864e-28, - "velocityX": -1.6722970053411976e-26, - "velocityY": -1.611730320150929e-26, - "timestamp": 13.950880405530265 - } - ], - "trajectoryWaypoints": [ - { - "timestamp": 0, - "isStopPoint": true, - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 1.829287687543546, + "y": 5.524116685802078, + "heading": -0.01091803536445235, + "angularVelocity": 0.2879338698406078, + "velocityX": -0.4308832094957455, + "velocityY": 0.4977374027489312, + "timestamp": 2.3414895591551255 }, { - "timestamp": 1.2792606479375337, - "isStopPoint": false, - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 1.812941193580627, + "y": 5.542999267578124, + "heading": 1.0528400336269664e-20, + "angularVelocity": 0.14389777634817838, + "velocityX": -0.2154439011994184, + "velocityY": 0.248869090323821, + "timestamp": 2.4173631107938585 }, { - "timestamp": 2.493236471662288, - "isStopPoint": true, - "x": 1.8129411935806274, + "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "heading": 4.797022054850397e-21, + "angularVelocity": -9.847000764445998e-21, + "velocityX": -1.972173923637389e-19, + "velocityY": -3.0551788776993856e-19, + "timestamp": 2.4932366624325915 }, { - "timestamp": 3.4242267541773885, - "isStopPoint": true, - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 + "x": 1.850755756182451, + "y": 5.5430527114919315, + "heading": -8.297424237870081e-19, + "angularVelocity": -8.963996805344217e-18, + "velocityX": 0.4061756697719998, + "velocityY": 0.0005740544380748758, + "timestamp": 2.586335696707713 }, { - "timestamp": 4.33090231449298, - "isStopPoint": false, - "x": 2.3293724060058594, - "y": 6.6973748207092285, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 6 + "x": 1.9263848800936074, + "y": 5.543159599317717, + "heading": -2.6749716281445808e-18, + "angularVelocity": -1.9820068153291573e-17, + "velocityX": 0.8123513256610282, + "velocityY": 0.001148108856528731, + "timestamp": 2.6794347309828344 }, { - "timestamp": 4.836905583331663, - "isStopPoint": true, - "x": 2.6483445167541504, - "y": 6.879644870758057, - "heading": 0.5125504196, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 + "x": 2.039828562987612, + "y": 5.543319931052195, + "heading": -5.8149048269782575e-18, + "angularVelocity": -3.372680740742886e-17, + "velocityX": 1.218526956560709, + "velocityY": 0.00172216323966475, + "timestamp": 2.772533765257956 }, { - "timestamp": 5.885339752974947, - "isStopPoint": false, - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.191086799436003, + "y": 5.543533706687691, + "heading": -1.0854467146744384e-17, + "angularVelocity": -5.413119866372016e-17, + "velocityX": 1.6247025291519184, + "velocityY": 0.0022962175403924926, + "timestamp": 2.8656327995330773 }, { - "timestamp": 6.336444988090829, - "isStopPoint": false, - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 + "x": 2.3801595622964795, + "y": 5.5438009261858445, + "heading": -4.1769885430235806e-18, + "angularVelocity": 7.172446691559558e-17, + "velocityX": 2.0308778102008924, + "velocityY": 0.0028702714290790255, + "timestamp": 2.9587318338081987 }, { - "timestamp": 6.780559247214761, - "isStopPoint": false, - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "x": 2.53141779874487, + "y": 5.54401470182134, + "heading": -1.8296127242246427e-18, + "angularVelocity": 2.5213750465779628e-17, + "velocityX": 1.6247025291519184, + "velocityY": 0.0022962175403924926, + "timestamp": 3.05183086808332 }, { - "timestamp": 7.37180261327287, - "isStopPoint": false, - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 + "x": 2.644861481638875, + "y": 5.544175033555818, + "heading": -7.228112419808573e-19, + "angularVelocity": 1.1888431398676619e-17, + "velocityX": 1.218526956560709, + "velocityY": 0.0017221632396647501, + "timestamp": 3.1449299023584416 }, { - "timestamp": 8.476353730016669, - "isStopPoint": false, - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.7204906055500317, + "y": 5.544281921381604, + "heading": -1.9384912404248153e-19, + "angularVelocity": 5.681714338671363e-18, + "velocityX": 0.8123513256610282, + "velocityY": 0.0011481088565287313, + "timestamp": 3.238028936633563 }, { - "timestamp": 9.40030847009841, - "isStopPoint": true, - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "timestamp": 10.327111513493909, - "isStopPoint": false, - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 20 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 9.777796865534126e-30, + "angularVelocity": 2.0821818999587538e-18, + "velocityX": 0.4061756697719998, + "velocityY": 0.0005740544380748761, + "timestamp": 3.3311279709086845 }, { - "timestamp": 11.265915069497849, - "isStopPoint": false, - "x": 7.984478950500488, - "y": 5.483555793762207, - "heading": 0.7044936776046891, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 6 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.1644118306851733e-29, + "angularVelocity": -2.560461213964373e-29, + "velocityX": -2.249094171598222e-22, + "velocityY": 3.2281317136840683e-24, + "timestamp": 3.424227005183806 }, { - "timestamp": 11.622086485482294, - "isStopPoint": false, - "x": 8.217175483703613, - "y": 5.698352336883545, - "heading": 0.7044936776046891, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 20 + "x": 2.7765323826419794, + "y": 5.537624192846842, + "heading": -2.642342456596521e-18, + "angularVelocity": -3.960137512120418e-17, + "velocityX": 0.2731753246622511, + "velocityY": -0.1005818367636714, + "timestamp": 3.4909505078012537 }, { - "timestamp": 12.966486277091528, - "isStopPoint": false, - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 2.812967100767047, + "y": 5.524148555527004, + "heading": -5.7083699786826614e-18, + "angularVelocity": -4.595123759665632e-17, + "velocityX": 0.5460552383462592, + "velocityY": -0.20196237894013302, + "timestamp": 3.5576740104187015 }, { - "timestamp": 13.950880405530265, - "isStopPoint": true, - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 2.867585856008562, + "y": 5.5038456232928015, + "heading": -9.193458656984461e-18, + "angularVelocity": -5.223180051341883e-17, + "velocityX": 0.8185834540891116, + "velocityY": -0.3042845689712529, + "timestamp": 3.624397513036149 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 2.9403602449806896, + "y": 5.476640219670028, + "heading": -1.3713804964383516e-17, + "angularVelocity": -6.774743726038035e-17, + "velocityX": 1.0906859819600914, + "velocityY": -0.40773344557094127, + "timestamp": 3.691121015653597 }, { - "scope": [ - 3 - ], - "type": "StopPoint" + "x": 3.0312551955793734, + "y": 5.442440791714766, + "heading": -1.9689161108713677e-17, + "angularVelocity": -8.955399386867554e-17, + "velocityX": 1.3622628763933429, + "velocityY": -0.51255444653948, + "timestamp": 3.7578445182710447 }, { - "scope": [ - 5 - ], - "type": "StopPoint" + "x": 3.1402263232984726, + "y": 5.401133415459366, + "heading": -2.6276370451665866e-17, + "angularVelocity": -9.872397407984698e-17, + "velocityX": 1.6331745703440297, + "velocityY": -0.6190828513939344, + "timestamp": 3.8245680208884925 }, { - "scope": [ - 2 - ], - "type": "StopPoint" + "x": 3.2672156894222453, + "y": 5.352572476953852, + "heading": -3.586672627631812e-17, + "angularVelocity": -1.4373279951454157e-16, + "velocityX": 1.9032179238529063, + "velocityY": -0.7277936049600628, + "timestamp": 3.89129152350594 }, { - "scope": [ - 11 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [], - "eventMarkers": [], - "isTrajectoryStale": true - }, - "CenterLanePCBFSprint": { - "waypoints": [ - { - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 3.4121445511915924, + "y": 5.2965653622743645, + "heading": -4.6442314764739977e-17, + "angularVelocity": -1.5849870096036839e-16, + "velocityX": 2.172081142086939, + "velocityY": -0.8393911063182322, + "timestamp": 3.958015026123388 }, { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 3.5748999018873118, + "y": 5.232845447526237, + "heading": -5.766195301574919e-17, + "angularVelocity": -1.6815121824921553e-16, + "velocityX": 2.439250703441936, + "velocityY": -0.9549845593907098, + "timestamp": 4.024738528740836 }, { - "x": 1.8129411935806274, - "y": 5.542999267578125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "x": 3.7553064852503644, + "y": 5.161019676773233, + "heading": -6.954653481566044e-17, + "angularVelocity": -1.7811687536878263e-16, + "velocityX": 2.70379366019489, + "velocityY": -1.0764688293540257, + "timestamp": 4.091462031358284 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 + "x": 3.953058182659589, + "y": 5.080452177894761, + "heading": -7.943549000582077e-17, + "angularVelocity": -1.4820797473458394e-16, + "velocityX": 2.9637487489680097, + "velocityY": -1.2074830564635766, + "timestamp": 4.158185533975732 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "heading": -8.307291105750155e-17, + "angularVelocity": -5.451483973353195e-17, + "velocityX": 3.2138732879335503, + "velocityY": -1.3564093381868512, + "timestamp": 4.224909036593179 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 + "x": 4.2829175575872735, + "y": 4.93941307796568, + "heading": -8.597616346613658e-17, + "angularVelocity": -8.369192817411254e-17, + "velocityX": 3.3271637455782836, + "velocityY": -1.45676206741062, + "timestamp": 4.259598790694249 }, { - "x": 7.196889877319336, - "y": 3.9978766441345215, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "x": 4.40183327548322, + "y": 4.884962607883731, + "heading": -8.781389044285282e-17, + "angularVelocity": -5.2976073895804537e-17, + "velocityX": 3.4279781156557663, + "velocityY": -1.5696412814950174, + "timestamp": 4.294288544795319 }, { - "x": 8.1634765625, - "y": 4.073246955871582, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 4.518840747701827, + "y": 4.825621207784163, + "heading": -9.315781416713264e-17, + "angularVelocity": -1.5404905173670211e-16, + "velocityX": 3.3729692023097777, + "velocityY": -1.7106319037798774, + "timestamp": 4.328978298896389 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 4.633380419289881, + "y": 4.7616461011987, + "heading": -9.908185769391999e-17, + "angularVelocity": -1.7077213950707558e-16, + "velocityX": 3.301830023191876, + "velocityY": -1.8442075547457024, + "timestamp": 4.3636680529974585 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 4.745269100236443, + "y": 4.693140088610349, + "heading": -1.025376712876981e-16, + "angularVelocity": -9.9620584908128e-17, + "velocityX": 3.2254100337687888, + "velocityY": -1.9748197807559171, + "timestamp": 4.398357807098528 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 + "x": 4.854330012145195, + "y": 4.620216373671222, + "heading": -1.025310951791751e-16, + "angularVelocity": 1.8956918365515503e-19, + "velocityX": 3.1438940613704167, + "velocityY": -2.1021686901170322, + "timestamp": 4.433047561199598 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "x": 4.962829609437795, + "y": 4.5464600987106305, + "heading": -9.93623379907482e-17, + "angularVelocity": 9.134562265751682e-17, + "velocityX": 3.127713069873129, + "velocityY": -2.1261688608602203, + "timestamp": 4.467737315300668 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 15 + "x": 5.073866943405842, + "y": 4.47658262622743, + "heading": -9.505129174841953e-17, + "angularVelocity": 1.2427433845795495e-16, + "velocityX": 3.2008682922495537, + "velocityY": -2.014354794202651, + "timestamp": 4.502427069401738 }, { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ + "x": 5.187610621724133, + "y": 4.411202764699172, + "heading": -8.900790995033302e-17, + "angularVelocity": 1.7421229853948998e-16, + "velocityX": 3.278883960575102, + "velocityY": -1.8847023630600785, + "timestamp": 4.537116823502807 + }, { - "x": 1.2899744510650637, - "y": 5.590954303741455, - "heading": -5.9009444568140314e-21, - "angularVelocity": -6.3286336172777184e-21, - "velocityX": -2.3802874375881527e-19, - "velocityY": -1.4090020225698018e-19, - "timestamp": 0 + "x": 5.303878904131373, + "y": 4.350425819404775, + "heading": -8.318308330059428e-17, + "angularVelocity": 1.6791201899662455e-16, + "velocityX": 3.3516606104641187, + "velocityY": -1.7520143013214118, + "timestamp": 4.571806577603877 }, { - "x": 1.3060573928270223, - "y": 5.572382489770416, - "heading": -0.010694903760090106, - "angularVelocity": -0.1421238741144053, - "velocityX": 0.2137251574806739, - "velocityY": -0.24679961691155075, - "timestamp": 0.0752505786007567 + "x": 5.422485694048398, + "y": 4.294349265955072, + "heading": -7.684855050558625e-17, + "angularVelocity": 1.8260529539962743e-16, + "velocityX": 3.4190726625349193, + "velocityY": -1.616516314480737, + "timestamp": 4.606496331704947 }, { - "x": 1.3382234711320444, - "y": 5.535239165388228, - "heading": -0.03207617873440696, - "angularVelocity": -0.28413435978686086, - "velocityX": 0.42745290339416875, - "velocityY": -0.4935951998359545, - "timestamp": 0.1505011572015134 + "x": 5.543241089708319, + "y": 4.243062964356387, + "heading": -6.896458750545335e-17, + "angularVelocity": 2.2727065107078374e-16, + "velocityX": 3.4810104248100697, + "velocityY": -1.4784279372307074, + "timestamp": 4.641186085806017 }, { - "x": 1.386473197712333, - "y": 5.479524643144728, - "heading": -0.06412173018225528, - "angularVelocity": -0.4258512299003913, - "velocityX": 0.6411874496855803, - "velocityY": -0.7403866293046694, - "timestamp": 0.22575173580227012 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -6.104113455464306e-17, + "angularVelocity": 2.2840902612270297e-16, + "velocityX": 3.5373741409339847, + "velocityY": -1.33797113887621, + "timestamp": 4.6758758399070866 }, { - "x": 1.4508074341962967, - "y": 5.405239335978357, - "heading": -0.1067984078244425, - "angularVelocity": -0.5671275681295302, - "velocityX": 0.8549334460969267, - "velocityY": -0.987172571263834, - "timestamp": 0.3010023144030268 + "x": 5.795129656406628, + "y": 4.153827521880981, + "heading": -5.26502063293767e-17, + "angularVelocity": 2.331839018689876e-16, + "velocityX": 3.589854707589498, + "velocityY": -1.1900109819379716, + "timestamp": 4.711860005871443 }, { - "x": 1.5312274285318639, - "y": 5.312383879458918, - "heading": -0.16006569861045727, - "angularVelocity": -0.707865531089473, - "velocityX": 1.0686960264083267, - "velocityY": -1.2339500671757528, - "timestamp": 0.3762528930037835 + "x": 5.925973456913311, + "y": 4.116403968556151, + "heading": -4.449297326372924e-17, + "angularVelocity": 2.2668951320100914e-16, + "velocityX": 3.636149317349419, + "velocityY": -1.0400005758616107, + "timestamp": 4.747844171835799 }, { - "x": 1.6277348772170532, - "y": 5.2009592989550075, - "heading": -0.22388049710097416, - "angularVelocity": -0.8480306686954192, - "velocityX": 1.2824811513696022, - "velocityY": -1.4807139370330336, - "timestamp": 0.4515034716045402 + "x": 6.058257665524998, + "y": 4.08444288734655, + "heading": -3.6710199664402193e-17, + "angularVelocity": 2.162832843430151e-16, + "velocityX": 3.6761782597023362, + "velocityY": -0.8881984715516176, + "timestamp": 4.783828337800155 }, { - "x": 1.7403321438675061, - "y": 5.0709673174660805, - "heading": -0.29820238787936876, - "angularVelocity": -0.9876587284824841, - "velocityX": 1.4962976862668933, - "velocityY": -1.7274549100613854, - "timestamp": 0.5267540502052969 + "x": 6.191754337287333, + "y": 4.057999339254895, + "heading": -3.0635067113443973e-17, + "angularVelocity": 1.6882793828813163e-16, + "velocityX": 3.7098726115972305, + "velocityY": -0.7348662219335074, + "timestamp": 4.819812503764511 }, { - "x": 1.869023605292211, - "y": 4.922411602704825, - "heading": -0.3829978231491767, - "angularVelocity": -1.1268409738042233, - "velocityX": 1.7101723842877663, - "velocityY": -1.9741471430995352, - "timestamp": 0.6020046288060535 + "x": 6.326233440381154, + "y": 4.037118883128164, + "heading": -2.7100684489157953e-17, + "angularVelocity": 9.822049586045954e-17, + "velocityX": 3.7371743790595944, + "velocityY": -0.5802678919226052, + "timestamp": 4.855796669728867 }, { - "x": 2.0218400866158173, - "y": 4.778612697599142, - "heading": -0.46243678265580673, - "angularVelocity": -1.0556591189563451, - "velocityX": 2.0307681902941916, - "velocityY": -1.9109342117962438, - "timestamp": 0.6772552074068102 + "x": 6.461463250762805, + "y": 4.021837500768739, + "heading": -2.6741166877102065e-17, + "angularVelocity": 9.990994712261855e-18, + "velocityX": 3.758036535169413, + "velocityY": -0.42466962759569293, + "timestamp": 4.891780835693223 }, { - "x": 2.158571908625681, - "y": 4.653385137637619, - "heading": -0.5314200564732017, - "angularVelocity": -0.9167141980846026, - "velocityX": 1.8170202083793483, - "velocityY": -1.6641408250954677, - "timestamp": 0.7525057860075669 + "x": 6.596863600046523, + "y": 4.012202729774147, + "heading": -2.0941092472280878e-17, + "angularVelocity": 1.6118407219507027e-16, + "velocityX": 3.7627758113898246, + "velocityY": -0.26775029339668854, + "timestamp": 4.927765001657579 }, { - "x": 2.279215456782309, - "y": 4.5467256453679665, - "heading": -0.5899528580218641, - "angularVelocity": -0.7778385580157354, - "velocityX": 1.603224193088739, - "velocityY": -1.4173909922412158, - "timestamp": 0.8277563646083236 + "x": 6.727539041313674, + "y": 4.005664584143169, + "heading": -1.5390888357792948e-17, + "angularVelocity": 1.5424017663331785e-16, + "velocityX": 3.631470613952549, + "velocityY": -0.1816950721451956, + "timestamp": 4.963749167621935 }, { - "x": 2.383769256289076, - "y": 4.45863275029472, - "heading": -0.6380359000069764, - "angularVelocity": -0.6389723890392618, - "velocityX": 1.3894085793213427, - "velocityY": -1.1706607007051708, - "timestamp": 0.9030069432090803 - }, - { - "x": 2.472232464576315, - "y": 4.389105550793566, - "heading": -0.6756681804644412, - "angularVelocity": -0.500092905027661, - "velocityX": 1.1755817687008607, - "velocityY": -0.9239423907952048, - "timestamp": 0.9782575218098369 + "x": 6.852985008513406, + "y": 4.001263402089072, + "heading": -1.0075825367590821e-17, + "angularVelocity": 1.4770560460742425e-16, + "velocityX": 3.4861435255715034, + "velocityY": -0.12230885268972179, + "timestamp": 4.999733333586291 }, { - "x": 2.5446045532635506, - "y": 4.33814345127288, - "heading": -0.7028480646839942, - "angularVelocity": -0.36119169745865176, - "velocityX": 0.9617479364681578, - "velocityY": -0.6772319956645083, - "timestamp": 1.0535081004105937 + "x": 6.973061816658764, + "y": 3.998618980784619, + "heading": -5.1577923312514375e-18, + "angularVelocity": 1.3667214188229055e-16, + "velocityX": 3.336934591294929, + "velocityY": -0.07348847009744364, + "timestamp": 5.035717499550647 }, { - "x": 2.6008851981181857, - "y": 4.305746065234092, - "heading": -0.7195737326656834, - "angularVelocity": -0.2222663040297333, - "velocityX": 0.7479097955259911, - "velocityY": -0.43052673668740377, - "timestamp": 1.1287586790113504 + "x": 7.087708032779949, + "y": 3.9975310881250836, + "heading": -2.6946740234793898e-18, + "angularVelocity": 6.845005967538265e-17, + "velocityX": 3.186018434740021, + "velocityY": -0.030232537850478983, + "timestamp": 5.071701665515003 }, { - "x": 2.641074232135465, - "y": 4.291913170423193, - "heading": -0.7258433490393724, - "angularVelocity": -0.0833165204875385, - "velocityX": 0.5340694352744676, - "velocityY": -0.1838244312283474, - "timestamp": 1.204009257612107 + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -6.591539659972594e-30, + "angularVelocity": 7.488499319392298e-17, + "velocityX": 3.034163544252643, + "velocityY": 0.009603001769728973, + "timestamp": 5.107685831479359 }, { - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "angularVelocity": 0.05565716597974986, - "velocityX": 0.3202286486370387, - "velocityY": 0.06287682191127338, - "timestamp": 1.2792598362128638 + "x": 7.373388400468218, + "y": 4.002743214942896, + "heading": -4.953200001247003e-18, + "angularVelocity": -7.75569723773575e-17, + "velocityX": 2.7636055642205424, + "velocityY": 0.07620053655266033, + "timestamp": 5.17155114041995 }, { - "x": 2.673109423896828, - "y": 4.320288496439454, - "heading": -0.7067986875650104, - "angularVelocity": 0.19580508899656304, - "velocityX": 0.10461880978884638, - "velocityY": 0.3116212207833284, - "timestamp": 1.3551333878515968 + "x": 7.532268457334727, + "y": 4.010110424631325, + "heading": -1.3097129093463054e-17, + "angularVelocity": -1.2751725823370097e-16, + "velocityX": 2.48773644881765, + "velocityY": 0.1153554223824672, + "timestamp": 5.2354164493605415 }, { - "x": 2.664688214670906, - "y": 4.3628057108082405, - "heading": -0.6813129138970213, - "angularVelocity": 0.3358979923509836, - "velocityX": -0.1109900491548022, - "velocityY": 0.5603693704926264, - "timestamp": 1.4310069394903298 + "x": 7.673406960142625, + "y": 4.018856353361859, + "heading": -1.8281069995800668e-17, + "angularVelocity": -8.116990254124082e-17, + "velocityX": 2.209940030810585, + "velocityY": 0.13694334022043622, + "timestamp": 5.299281758301133 }, { - "x": 2.6399081472529042, - "y": 4.424196735750699, - "heading": -0.6452019329979721, - "angularVelocity": 0.47593634565822673, - "velocityX": -0.3265969087089792, - "velocityY": 0.8091228579205605, - "timestamp": 1.5068804911290627 + "x": 7.796760795699071, + "y": 4.0282099476371425, + "heading": -2.4884139524870184e-17, + "angularVelocity": -1.03390551752411e-16, + "velocityX": 1.9314685484602037, + "velocityY": 0.1464581387053742, + "timestamp": 5.363147067241724 }, { - "x": 2.5987694501527234, - "y": 4.5044620989064414, - "heading": -0.5984691831252384, - "angularVelocity": 0.6159293833409661, - "velocityX": -0.542200756545013, - "velocityY": 1.05788329954391, - "timestamp": 1.5827540427677957 + "x": 7.902319649761861, + "y": 4.0376115999719895, + "heading": -2.509945215126813e-17, + "angularVelocity": -3.3713549672899342e-18, + "velocityX": 1.6528355661910739, + "velocityY": 0.14721062953900424, + "timestamp": 5.427012376182315 }, { - "x": 2.5412724247726852, - "y": 4.603602430919982, - "heading": -0.5411158658534186, - "angularVelocity": 0.7559065844828777, - "velocityX": -0.7578006319478658, - "velocityY": 1.3066520529527457, - "timestamp": 1.6586275944065287 + "x": 7.9900874176780725, + "y": 4.046637816644616, + "heading": -1.900477135412208e-17, + "angularVelocity": 9.543022492350273e-17, + "velocityX": 1.3742635770830707, + "velocityY": 0.14133207561907785, + "timestamp": 5.490877685122906 }, { - "x": 2.467417405939932, - "y": 4.7216183787073325, - "heading": -0.473137956706953, - "angularVelocity": 0.895936827501342, - "velocityX": -0.9733960943919171, - "velocityY": 1.5554293325984934, - "timestamp": 1.7345011460452617 + "x": 8.060074171241288, + "y": 4.054957461114281, + "heading": -1.4433757000062975e-17, + "angularVelocity": 7.157272750648916e-17, + "velocityX": 1.0958492916446645, + "velocityY": 0.13026860133729024, + "timestamp": 5.554742994063497 }, { - "x": 2.37720453797495, - "y": 4.858510302970022, - "heading": -0.3945211456551798, - "angularVelocity": 1.0361556741946414, - "velocityX": -1.1889896547134935, - "velocityY": 1.8042113662279782, - "timestamp": 1.8103746976839947 + "x": 8.112292461453118, + "y": 4.062304768239738, + "heading": -9.3901869246783e-18, + "angularVelocity": 7.897198273854564e-17, + "velocityX": 0.8176315291985099, + "velocityY": 0.11504378898864956, + "timestamp": 5.6186083030040885 }, { - "x": 2.2706323270523923, - "y": 5.014276850913776, - "heading": -0.3052336725466659, - "angularVelocity": 1.1767931140702435, - "velocityX": -1.4046029033952228, - "velocityY": 2.052975570268153, - "timestamp": 1.8862482493227277 + "x": 8.146755551516593, + "y": 4.0684618614995625, + "heading": -4.677400275001561e-18, + "angularVelocity": 7.379259143526985e-17, + "velocityX": 0.5396214413607953, + "velocityY": 0.09640747632727756, + "timestamp": 5.68247361194468 }, { - "x": 2.156211312441972, - "y": 5.146461367106831, - "heading": -0.22910323737059946, - "angularVelocity": 1.0033856796180658, - "velocityX": -1.5080487487284289, - "velocityY": 1.742168559900677, - "timestamp": 1.9621218009614607 + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -3.026226030610745e-28, + "angularVelocity": 7.323851324219789e-17, + "velocityX": 0.2618168025924992, + "velocityY": 0.07492478234891005, + "timestamp": 5.746338920885271 }, { - "x": 2.058134458607592, - "y": 5.259759379647075, - "heading": -0.16374337318870086, - "angularVelocity": 0.8614314576060107, - "velocityX": -1.2926355985202171, - "velocityY": 1.4932477799347985, - "timestamp": 2.0379953526001935 + "x": 8.160404735497007, + "y": 4.076664969326324, + "heading": 5.687417788003061e-18, + "angularVelocity": 8.091717809987449e-17, + "velocityX": -0.04370411704181345, + "velocityY": 0.04862945078972498, + "timestamp": 5.816625823147878 }, { - "x": 1.976403642015832, - "y": 5.354173274627719, - "heading": -0.10920518354656349, - "angularVelocity": 0.7188036998955512, - "velocityX": -1.0771977168121305, - "velocityY": 1.2443584482531382, - "timestamp": 2.1138689042389265 + "x": 8.13585178429954, + "y": 4.078318050056324, + "heading": 9.715319347453537e-18, + "angularVelocity": 5.730657391283969e-17, + "velocityX": -0.34932470214341615, + "velocityY": 0.02351904375900896, + "timestamp": 5.886912725410485 }, { - "x": 1.9110189688101253, - "y": 5.429703861532151, - "heading": -0.06553401442898629, - "angularVelocity": 0.5755782901203135, - "velocityX": -0.8617584361561702, - "velocityY": 0.9954797854217321, - "timestamp": 2.1897424558776595 + "x": 8.089809855979007, + "y": 4.078304537416186, + "heading": 1.421739507206778e-17, + "angularVelocity": 6.405284028537325e-17, + "velocityX": -0.6550570140153344, + "velocityY": -0.00019224976064422702, + "timestamp": 5.957199627673092 }, { - "x": 1.8619803269894504, - "y": 5.486351581272176, - "heading": -0.032764600706343905, - "angularVelocity": 0.4318950808929029, - "velocityX": -0.6463206316489477, - "velocityY": 0.7466069337268786, - "timestamp": 2.2656160075163925 + "x": 8.022270134239134, + "y": 4.076742304699779, + "heading": 2.3560062445993e-17, + "angularVelocity": 1.3292188264586088e-16, + "velocityX": -0.9609147588768534, + "velocityY": -0.022226512566613944, + "timestamp": 6.0274865299356994 }, { - "x": 1.829287687543546, - "y": 5.524116685802078, - "heading": -0.01091803536445235, - "angularVelocity": 0.2879338698406078, - "velocityX": -0.4308832094957455, - "velocityY": 0.4977374027489312, - "timestamp": 2.3414895591551255 + "x": 7.933222739684713, + "y": 4.073775206509657, + "heading": 3.0510865621673614e-17, + "angularVelocity": 9.889186963848529e-17, + "velocityX": -1.2669130618635012, + "velocityY": -0.04221409814073001, + "timestamp": 6.097773432198307 }, { - "x": 1.812941193580627, - "y": 5.542999267578124, - "heading": 1.0528400336269664e-20, - "angularVelocity": 0.14389777634817838, - "velocityX": -0.2154439011994184, - "velocityY": 0.248869090323821, - "timestamp": 2.4173631107938585 + "x": 7.822656702045201, + "y": 4.069582705061331, + "heading": 3.6286017590367637e-17, + "angularVelocity": 8.21654075361425e-17, + "velocityX": -1.573067443297094, + "velocityY": -0.05964840266629161, + "timestamp": 6.168060334460914 }, { - "x": 1.8129411935806272, - "y": 5.542999267578125, - "heading": 4.797022054850397e-21, - "angularVelocity": -9.847000764445998e-21, - "velocityX": -1.972173923637389e-19, - "velocityY": -3.0551788776993856e-19, - "timestamp": 2.4932366624325915 + "x": 7.690560152164254, + "y": 4.0643949017183925, + "heading": 4.582493898384423e-17, + "angularVelocity": 1.3571406743753222e-16, + "velocityX": -1.8793906919870382, + "velocityY": -0.07380896263653505, + "timestamp": 6.238347236723521 }, { - "x": 1.850755756182451, - "y": 5.5430527114919315, - "heading": -8.297424237870081e-19, - "angularVelocity": -8.963996805344217e-18, - "velocityX": 0.4061756697719998, - "velocityY": 0.0005740544380748758, - "timestamp": 2.586335696707713 + "x": 7.53692113699896, + "y": 4.058517364536531, + "heading": 5.495539333254171e-17, + "angularVelocity": 1.2990264266804977e-16, + "velocityX": -2.1858840014212815, + "velocityY": -0.0836220831002289, + "timestamp": 6.308634138986128 }, { - "x": 1.9263848800936074, - "y": 5.543159599317717, - "heading": -2.6749716281445808e-18, - "angularVelocity": -1.9820068153291573e-17, - "velocityX": 0.8123513256610282, - "velocityY": 0.001148108856528731, - "timestamp": 2.6794347309828344 + "x": 7.361730243304987, + "y": 4.052375214932341, + "heading": 6.396812526564607e-17, + "angularVelocity": 1.2822775856015118e-16, + "velocityX": -2.4925112368648836, + "velocityY": -0.08738683035483485, + "timestamp": 6.378921041248735 }, { - "x": 2.039828562987612, - "y": 5.543319931052195, - "heading": -5.8149048269782575e-18, - "angularVelocity": -3.372680740742886e-17, - "velocityX": 1.218526956560709, - "velocityY": 0.00172216323966475, - "timestamp": 2.772533765257956 + "x": 7.164988961620082, + "y": 4.046599375673031, + "heading": 7.356788600543696e-17, + "angularVelocity": 1.3657965326299643e-16, + "velocityX": -2.799117265829076, + "velocityY": -0.08217518589352116, + "timestamp": 6.449207943511342 }, { - "x": 2.191086799436003, - "y": 5.543533706687691, - "heading": -1.0854467146744384e-17, - "angularVelocity": -5.413119866372016e-17, - "velocityX": 1.6247025291519184, - "velocityY": 0.0022962175403924926, - "timestamp": 2.8656327995330773 + "x": 6.946739460744683, + "y": 4.042219881595439, + "heading": 8.53416334460989e-17, + "angularVelocity": 1.6750983557133948e-16, + "velocityX": -3.105123342325879, + "velocityY": -0.06230882193712492, + "timestamp": 6.519494845773949 }, { - "x": 2.3801595622964795, - "y": 5.5438009261858445, - "heading": -4.1769885430235806e-18, - "angularVelocity": 7.172446691559558e-17, - "velocityX": 2.0308778102008924, - "velocityY": 0.0028702714290790255, - "timestamp": 2.9587318338081987 + "x": 6.707199372096715, + "y": 4.041196560014842, + "heading": 9.976814238544392e-17, + "angularVelocity": 2.052517393014489e-16, + "velocityX": -3.408033089194812, + "velocityY": -0.014559207301144003, + "timestamp": 6.589781748036557 }, { - "x": 2.53141779874487, - "y": 5.54401470182134, - "heading": -1.8296127242246427e-18, - "angularVelocity": 2.5213750465779628e-17, - "velocityX": 1.6247025291519184, - "velocityY": 0.0022962175403924926, - "timestamp": 3.05183086808332 + "x": 6.447738133610024, + "y": 4.048401251612496, + "heading": 1.0469202046971274e-16, + "angularVelocity": 7.00539919377632e-17, + "velocityX": -3.691459292334275, + "velocityY": 0.10250404222876178, + "timestamp": 6.660068650299164 }, { - "x": 2.644861481638875, - "y": 5.544175033555818, - "heading": -7.228112419808573e-19, - "angularVelocity": 1.1888431398676619e-17, - "velocityX": 1.218526956560709, - "velocityY": 0.0017221632396647501, - "timestamp": 3.1449299023584416 + "x": 6.18341657905221, + "y": 4.076604236955095, + "heading": 9.517171884835342e-17, + "angularVelocity": -1.354491564534057e-16, + "velocityX": -3.7606089619691896, + "velocityY": 0.4012552045219841, + "timestamp": 6.730355552561771 }, { - "x": 2.7204906055500317, - "y": 5.544281921381604, - "heading": -1.9384912404248153e-19, - "angularVelocity": 5.681714338671363e-18, - "velocityX": 0.8123513256610282, - "velocityY": 0.0011481088565287313, - "timestamp": 3.238028936633563 + "x": 5.922248770654031, + "y": 4.126128704367341, + "heading": 8.827514914451601e-17, + "angularVelocity": -9.812026823816515e-17, + "velocityX": -3.7157393481704415, + "velocityY": 0.7046044969688833, + "timestamp": 6.800642454824378 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 9.777796865534126e-30, - "angularVelocity": 2.0821818999587538e-18, - "velocityX": 0.4061756697719998, - "velocityY": 0.0005740544380748761, - "timestamp": 3.3311279709086845 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 8.108309692704171e-17, + "angularVelocity": -1.0232421668622016e-16, + "velocityX": -3.6464409951607806, + "velocityY": 1.003321641969972, + "timestamp": 6.870929357086985 }, { - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": -1.1644118306851733e-29, - "angularVelocity": -2.560461213964373e-29, - "velocityX": -2.249094171598222e-22, - "velocityY": 3.2281317136840683e-24, - "timestamp": 3.424227005183806 + "x": 5.541094252914905, + "y": 4.236435293416023, + "heading": 7.779103781629071e-17, + "angularVelocity": -9.500999479304098e-17, + "velocityX": -3.603431085871871, + "velocityY": 1.1482444026273093, + "timestamp": 6.905578965454807 }, { - "x": 2.7765323826419794, - "y": 5.537624192846842, - "heading": -2.642342456596521e-18, - "angularVelocity": -3.960137512120418e-17, - "velocityX": 0.2731753246622511, - "velocityY": -0.1005818367636714, - "timestamp": 3.4909505078012537 + "x": 5.417926513725, + "y": 4.281179467366055, + "heading": 7.404581584331534e-17, + "angularVelocity": -1.0808843589526725e-16, + "velocityX": -3.5546646842995244, + "velocityY": 1.291332746825043, + "timestamp": 6.940228573822629 }, { - "x": 2.812967100767047, - "y": 5.524148555527004, - "heading": -5.7083699786826614e-18, - "angularVelocity": -4.595123759665632e-17, - "velocityX": 0.5460552383462592, - "velocityY": -0.20196237894013302, - "timestamp": 3.5576740104187015 + "x": 5.2966452974499845, + "y": 4.330810104553132, + "heading": 7.048745543918544e-17, + "angularVelocity": -1.0269554467404476e-16, + "velocityX": -3.5002189631570437, + "velocityY": 1.4323578108076789, + "timestamp": 6.974878182190451 }, { - "x": 2.867585856008562, - "y": 5.5038456232928015, - "heading": -9.193458656984461e-18, - "angularVelocity": -5.223180051341883e-17, - "velocityX": 0.8185834540891116, - "velocityY": -0.3042845689712529, - "timestamp": 3.624397513036149 + "x": 5.177444375077077, + "y": 4.385247904729644, + "heading": 6.777735008198806e-17, + "angularVelocity": -7.821460284920669e-17, + "velocityX": -3.440180942524166, + "velocityY": 1.5710942414883695, + "timestamp": 7.0095277905582725 }, { - "x": 2.9403602449806896, - "y": 5.476640219670028, - "heading": -1.3713804964383516e-17, - "angularVelocity": -6.774743726038035e-17, - "velocityX": 1.0906859819600914, - "velocityY": -0.40773344557094127, - "timestamp": 3.691121015653597 + "x": 5.060514192105533, + "y": 4.444405885109906, + "heading": 6.539767218111103e-17, + "angularVelocity": -6.867834914008308e-17, + "velocityX": -3.3746465971642725, + "velocityY": 1.7073203180905416, + "timestamp": 7.044177398926094 }, { - "x": 3.0312551955793734, - "y": 5.442440791714766, - "heading": -1.9689161108713677e-17, - "angularVelocity": -8.955399386867554e-17, - "velocityX": 1.3622628763933429, - "velocityY": -0.51255444653948, - "timestamp": 3.7578445182710447 + "x": 4.946041562758086, + "y": 4.5081895174709326, + "heading": 6.339841419086817e-17, + "angularVelocity": -5.769929544659045e-17, + "velocityX": -3.3037207270069695, + "velocityY": 1.8408182766146604, + "timestamp": 7.078827007293916 }, { - "x": 3.1402263232984726, - "y": 5.401133415459366, - "heading": -2.6276370451665866e-17, - "angularVelocity": -9.872397407984698e-17, - "velocityX": 1.6331745703440297, - "velocityY": -0.6190828513939344, - "timestamp": 3.8245680208884925 + "x": 4.834209367445105, + "y": 4.5764968740602265, + "heading": 6.206624882969642e-17, + "angularVelocity": -3.844676532404254e-17, + "velocityX": -3.2275168632738827, + "velocityY": 1.9713745640117823, + "timestamp": 7.113476615661738 }, { - "x": 3.2672156894222453, - "y": 5.352572476953852, - "heading": -3.586672627631812e-17, - "angularVelocity": -1.4373279951454157e-16, - "velocityX": 1.9032179238529063, - "velocityY": -0.7277936049600628, - "timestamp": 3.89129152350594 + "x": 4.725196244706173, + "y": 4.649218769978707, + "heading": 6.152068396615488e-17, + "angularVelocity": -1.574519567617046e-17, + "velocityX": -3.1461574278620072, + "velocityY": 2.098779736454826, + "timestamp": 7.14812622402956 }, { - "x": 3.4121445511915924, - "y": 5.2965653622743645, - "heading": -4.6442314764739977e-17, - "angularVelocity": -1.5849870096036839e-16, - "velocityX": 2.172081142086939, - "velocityY": -0.8393911063182322, - "timestamp": 3.958015026123388 + "x": 4.619176114911752, + "y": 4.726238684753922, + "heading": 5.2556937935057745e-17, + "angularVelocity": -2.58696893087623e-16, + "velocityX": -3.0597785888073585, + "velocityY": 2.2228220866918083, + "timestamp": 7.182775832397382 }, { - "x": 3.5748999018873118, - "y": 5.232845447526237, - "heading": -5.766195301574919e-17, - "angularVelocity": -1.6815121824921553e-16, - "velocityX": 2.439250703441936, - "velocityY": -0.9549845593907098, - "timestamp": 4.024738528740836 + "x": 4.51016287630692, + "y": 4.798960406862416, + "heading": 6.64273126982982e-17, + "angularVelocity": 4.003039403991449e-16, + "velocityX": -3.1461607717930216, + "velocityY": 2.0987747202369076, + "timestamp": 7.217425440765203 }, { - "x": 3.7553064852503644, - "y": 5.161019676773233, - "heading": -6.954653481566044e-17, - "angularVelocity": -1.7811687536878263e-16, - "velocityX": 2.70379366019489, - "velocityY": -1.0764688293540257, - "timestamp": 4.091462031358284 + "x": 4.398330563422939, + "y": 4.8672675708300295, + "heading": 7.26949465762174e-17, + "angularVelocity": 1.8088613906315835e-16, + "velocityX": -3.2275202564146483, + "velocityY": 1.9713690048817056, + "timestamp": 7.252075049133025 }, { - "x": 3.953058182659589, - "y": 5.080452177894761, - "heading": -7.943549000582077e-17, - "angularVelocity": -1.4820797473458394e-16, - "velocityX": 2.9637487489680097, - "velocityY": -1.2074830564635766, - "timestamp": 4.158185533975732 + "x": 4.283857826423259, + "y": 4.9310509990316564, + "heading": 7.854869048227426e-17, + "angularVelocity": 1.6894112751752813e-16, + "velocityX": -3.303723833888627, + "velocityY": 1.840812384501881, + "timestamp": 7.286724657500847 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": -8.307291105750155e-17, - "angularVelocity": -5.451483973353195e-17, - "velocityX": 3.2138732879335503, - "velocityY": -1.3564093381868512, - "timestamp": 4.224909036593179 - }, - { - "x": 4.2829175575872735, - "y": 4.93941307796568, - "heading": -8.597616346613658e-17, - "angularVelocity": -8.369192817411254e-17, - "velocityX": 3.3271637455782836, - "velocityY": -1.45676206741062, - "timestamp": 4.259598790694249 + "heading": 7.967166003614947e-17, + "angularVelocity": 3.240930002777194e-17, + "velocityX": -3.358155156874672, + "velocityY": 1.6997824682762939, + "timestamp": 7.321374265868669 }, { - "x": 4.40183327548322, - "y": 4.884962607883731, - "heading": -8.781389044285282e-17, - "angularVelocity": -5.2976073895804537e-17, - "velocityX": 3.4279781156557663, - "velocityY": -1.5696412814950174, - "timestamp": 4.294288544795319 + "x": 3.941212613699701, + "y": 5.086809869285441, + "heading": 7.443579239948039e-17, + "angularVelocity": -7.609481799868211e-17, + "velocityX": -3.288705435486243, + "velocityY": 1.4077326545547513, + "timestamp": 7.3901814169311395 }, { - "x": 4.518840747701827, - "y": 4.825621207784163, - "heading": -9.315781416713264e-17, - "angularVelocity": -1.5404905173670211e-16, - "velocityX": 3.3729692023097777, - "velocityY": -1.7106319037798774, - "timestamp": 4.328978298896389 + "x": 3.7303194477377524, + "y": 5.1698988992560615, + "heading": 6.400702034661796e-17, + "angularVelocity": -1.515652354772208e-16, + "velocityX": -3.0649890702563414, + "velocityY": 1.2075638750859479, + "timestamp": 7.45898856799361 }, { - "x": 4.633380419289881, - "y": 4.7616461011987, - "heading": -9.908185769391999e-17, - "angularVelocity": -1.7077213950707558e-16, - "velocityX": 3.301830023191876, - "velocityY": -1.8442075547457024, - "timestamp": 4.3636680529974585 + "x": 3.5371029822412163, + "y": 5.24230220948227, + "heading": 4.92329505445724e-17, + "angularVelocity": -2.1471706900158835e-16, + "velocityX": -2.8080869867888283, + "velocityY": 1.0522643229404085, + "timestamp": 7.527795719056081 }, { - "x": 4.745269100236443, - "y": 4.693140088610349, - "heading": -1.025376712876981e-16, - "angularVelocity": -9.9620584908128e-17, - "velocityX": 3.2254100337687888, - "velocityY": -1.9748197807559171, - "timestamp": 4.398357807098528 + "x": 3.362390045884083, + "y": 5.305525654232102, + "heading": 3.1750019865336384e-17, + "angularVelocity": -2.5408595487118374e-16, + "velocityX": -2.5391682936924758, + "velocityY": 0.9188499127428014, + "timestamp": 7.596602870118551 }, { - "x": 4.854330012145195, - "y": 4.620216373671222, - "heading": -1.025310951791751e-16, - "angularVelocity": 1.8956918365515503e-19, - "velocityX": 3.1438940613704167, - "velocityY": -2.1021686901170322, - "timestamp": 4.433047561199598 + "x": 3.2065873745181794, + "y": 5.36043931571324, + "heading": 2.169929247164939e-17, + "angularVelocity": -1.4607097138783564e-16, + "velocityX": -2.2643383566985644, + "velocityY": 0.7980807319181392, + "timestamp": 7.665410021181022 }, { - "x": 4.962829609437795, - "y": 4.5464600987106305, - "heading": -9.93623379907482e-17, - "angularVelocity": 9.134562265751682e-17, - "velocityX": 3.127713069873129, - "velocityY": -2.1261688608602203, - "timestamp": 4.467737315300668 + "x": 3.069932459879016, + "y": 5.407606100670878, + "heading": 1.5380181020622108e-17, + "angularVelocity": -9.183800453716929e-17, + "velocityX": -1.9860568636985672, + "velocityY": 0.6854924848554687, + "timestamp": 7.7342171722434925 }, { - "x": 5.073866943405842, - "y": 4.47658262622743, - "heading": -9.505129174841953e-17, - "angularVelocity": 1.2427433845795495e-16, - "velocityX": 3.2008682922495537, - "velocityY": -2.014354794202651, - "timestamp": 4.502427069401738 + "x": 2.952579630826552, + "y": 5.447418960174395, + "heading": 8.435159362125091e-18, + "angularVelocity": -1.0093459112453562e-16, + "velocityX": -1.7055324517929655, + "velocityY": 0.5786151423035916, + "timestamp": 7.803024323305963 }, { - "x": 5.187610621724133, - "y": 4.411202764699172, - "heading": -8.900790995033302e-17, - "angularVelocity": 1.7421229853948998e-16, - "velocityX": 3.278883960575102, - "velocityY": -1.8847023630600785, - "timestamp": 4.537116823502807 + "x": 2.8546367166241713, + "y": 5.480167404246393, + "heading": 5.0453569918778545e-18, + "angularVelocity": -4.926526267025923e-17, + "velocityX": -1.423440916968907, + "velocityY": 0.4759453569334053, + "timestamp": 7.871831474368434 }, { - "x": 5.303878904131373, - "y": 4.350425819404775, - "heading": -8.318308330059428e-17, - "angularVelocity": 1.6791201899662455e-16, - "velocityX": 3.3516606104641187, - "velocityY": -1.7520143013214118, - "timestamp": 4.571806577603877 + "x": 2.7761830906702163, + "y": 5.506073449022424, + "heading": 3.1520224912430142e-18, + "angularVelocity": -2.7516536752894226e-17, + "velocityX": -1.1401958189305985, + "velocityY": 0.3765022149007732, + "timestamp": 7.940638625430904 }, { - "x": 5.422485694048398, - "y": 4.294349265955072, - "heading": -7.684855050558625e-17, - "angularVelocity": 1.8260529539962743e-16, - "velocityX": 3.4190726625349193, - "velocityY": -1.616516314480737, - "timestamp": 4.606496331704947 + "x": 2.7172795158320118, + "y": 5.5253126875621055, + "heading": 4.0055040762053705e-18, + "angularVelocity": 1.2403966322481763e-17, + "velocityX": -0.8560676314693773, + "velocityY": 0.2796110323215353, + "timestamp": 8.009445776493374 }, { - "x": 5.543241089708319, - "y": 4.243062964356387, - "heading": -6.896458750545335e-17, - "angularVelocity": 2.2727065107078374e-16, - "velocityX": 3.4810104248100697, - "velocityY": -1.4784279372307074, - "timestamp": 4.641186085806017 + "x": 2.677973947928258, + "y": 5.538027438078825, + "heading": 2.2657574428082367e-18, + "angularVelocity": -2.5284386970987473e-17, + "velocityX": -0.571242484201505, + "velocityY": 0.18478821343985707, + "timestamp": 8.078252927555845 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": -6.104113455464306e-17, - "angularVelocity": 2.2840902612270297e-16, - "velocityX": 3.5373741409339847, - "velocityY": -1.33797113887621, - "timestamp": 4.6758758399070866 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -8.520572182632915e-28, + "angularVelocity": -3.292909834387365e-17, + "velocityX": -0.2858537153870097, + "velocityY": 0.09167545987855215, + "timestamp": 8.147060078618315 }, { - "x": 5.795129656406628, - "y": 4.153827521880981, - "heading": -5.26502063293767e-17, - "angularVelocity": 2.331839018689876e-16, - "velocityX": 3.589854707589498, - "velocityY": -1.1900109819379716, - "timestamp": 4.711860005871443 + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": -3.9086161010585418e-28, + "angularVelocity": 3.4350134480026537e-28, + "velocityX": 1.6920853023952865e-26, + "velocityY": 2.9127538584633355e-26, + "timestamp": 8.215867229680786 }, { - "x": 5.925973456913311, - "y": 4.116403968556151, - "heading": -4.449297326372924e-17, - "angularVelocity": 2.2668951320100914e-16, - "velocityX": 3.636149317349419, - "velocityY": -1.0400005758616107, - "timestamp": 4.747844171835799 + "x": 2.675073555498551, + "y": 5.538716972612474, + "heading": 1.6968135653440987e-18, + "angularVelocity": 2.665146289407491e-17, + "velocityX": 0.2633772278822265, + "velocityY": -0.08824680986851689, + "timestamp": 8.279534037481625 }, { - "x": 6.058257665524998, - "y": 4.08444288734655, - "heading": -3.6710199664402193e-17, - "angularVelocity": 2.162832843430151e-16, - "velocityX": 3.6761782597023362, - "velocityY": -0.8881984715516176, - "timestamp": 4.783828337800155 + "x": 2.7085920648883794, + "y": 5.5274259655637525, + "heading": 3.725095749658628e-18, + "angularVelocity": 3.185776473374715e-17, + "velocityX": 0.5264675668156642, + "velocityY": -0.17734526731797434, + "timestamp": 8.343200845282464 }, { - "x": 6.191754337287333, - "y": 4.057999339254895, - "heading": -3.0635067113443973e-17, - "angularVelocity": 1.6882793828813163e-16, - "velocityX": 3.7098726115972305, - "velocityY": -0.7348662219335074, - "timestamp": 4.819812503764511 + "x": 2.7588390752001515, + "y": 5.510398898078488, + "heading": 7.631145981641487e-18, + "angularVelocity": 6.13514383217542e-17, + "velocityX": 0.7892183077397765, + "velocityY": -0.267440257701116, + "timestamp": 8.406867653083303 }, { - "x": 6.326233440381154, - "y": 4.037118883128164, - "heading": -2.7100684489157953e-17, - "angularVelocity": 9.822049586045954e-17, - "velocityX": 3.7371743790595944, - "velocityY": -0.5802678919226052, - "timestamp": 4.855796669728867 + "x": 2.8257885977384563, + "y": 5.48756053072635, + "heading": 1.178722114456003e-17, + "angularVelocity": 6.527852277238055e-17, + "velocityX": 1.0515608501644154, + "velocityY": -0.3587170166216, + "timestamp": 8.470534460884142 }, { - "x": 6.461463250762805, - "y": 4.021837500768739, - "heading": -2.6741166877102065e-17, - "angularVelocity": 9.990994712261855e-18, - "velocityX": 3.758036535169413, - "velocityY": -0.42466962759569293, - "timestamp": 4.891780835693223 + "x": 2.909408815467416, + "y": 5.4588202121142055, + "heading": 1.707663826690775e-17, + "angularVelocity": 8.307966592022717e-17, + "velocityX": 1.3134036496778398, + "velocityY": -0.45141761625695104, + "timestamp": 8.534201268684981 }, { - "x": 6.596863600046523, - "y": 4.012202729774147, - "heading": -2.0941092472280878e-17, - "angularVelocity": 1.6118407219507027e-16, - "velocityX": 3.7627758113898246, - "velocityY": -0.26775029339668854, - "timestamp": 4.927765001657579 + "x": 3.0096598952002043, + "y": 5.424066614979104, + "heading": 2.1547525099461292e-17, + "angularVelocity": 7.022319772168498e-17, + "velocityX": 1.5746207984290834, + "velocityY": -0.5458668077692407, + "timestamp": 8.59786807648582 }, { - "x": 6.727539041313674, - "y": 4.005664584143169, - "heading": -1.5390888357792948e-17, - "angularVelocity": 1.5424017663331785e-16, - "velocityX": 3.631470613952549, - "velocityY": -0.1816950721451956, - "timestamp": 4.963749167621935 + "x": 3.1264905609256615, + "y": 5.383159775106569, + "heading": 2.7044139747893618e-17, + "angularVelocity": 8.633407011044114e-17, + "velocityX": 1.835032566591432, + "velocityY": -0.6425143852114948, + "timestamp": 8.66153488428666 }, { - "x": 6.852985008513406, - "y": 4.001263402089072, - "heading": -1.0075825367590821e-17, - "angularVelocity": 1.4770560460742425e-16, - "velocityX": 3.4861435255715034, - "velocityY": -0.12230885268972179, - "timestamp": 4.999733333586291 + "x": 3.259832418594633, + "y": 5.335918459700519, + "heading": 3.2480422722951275e-17, + "angularVelocity": 8.538645430467616e-17, + "velocityX": 2.0943700850541704, + "velocityY": -0.7420085447636868, + "timestamp": 8.725201692087499 }, { - "x": 6.973061816658764, - "y": 3.998618980784619, - "heading": -5.1577923312514375e-18, - "angularVelocity": 1.3667214188229055e-16, - "velocityX": 3.336934591294929, - "velocityY": -0.07348847009744364, - "timestamp": 5.035717499550647 + "x": 3.409589859132952, + "y": 5.282098873954419, + "heading": 3.770995869084488e-17, + "angularVelocity": 8.213912631218793e-17, + "velocityX": 2.3522058936390553, + "velocityY": -0.84533193362635, + "timestamp": 8.788868499888338 }, { - "x": 7.087708032779949, - "y": 3.9975310881250836, - "heading": -2.6946740234793898e-18, - "angularVelocity": 6.845005967538265e-17, - "velocityX": 3.186018434740021, - "velocityY": -0.030232537850478983, - "timestamp": 5.071701665515003 + "x": 3.575620300185956, + "y": 5.221355820749434, + "heading": 4.453751126086424e-17, + "angularVelocity": 1.0723880787820308e-16, + "velocityX": 2.6078021937643934, + "velocityY": -0.9540772547447327, + "timestamp": 8.852535307689177 }, { - "x": 7.196889877319336, - "y": 3.9978766441345215, - "heading": -6.591539659972594e-30, - "angularVelocity": 7.488499319392298e-17, - "velocityX": 3.034163544252643, - "velocityY": 0.009603001769728973, - "timestamp": 5.107685831479359 + "x": 3.7576900290728465, + "y": 5.153163867625262, + "heading": 5.330339940674851e-17, + "angularVelocity": 1.3768380178976122e-16, + "velocityX": 2.8597276222240717, + "velocityY": -1.0710754234370974, + "timestamp": 8.916202115490016 }, { - "x": 7.373388400468218, - "y": 4.002743214942896, - "heading": -4.953200001247003e-18, - "angularVelocity": -7.75569723773575e-17, - "velocityX": 2.7636055642205424, - "velocityY": 0.07620053655266033, - "timestamp": 5.17155114041995 + "x": 3.955353840328095, + "y": 5.076631092230983, + "heading": 6.003878998986586e-17, + "angularVelocity": 1.0579124061222185e-16, + "velocityX": 3.10466031018196, + "velocityY": -1.2020828126594152, + "timestamp": 8.979868923290855 }, { - "x": 7.532268457334727, - "y": 4.010110424631325, - "heading": -1.3097129093463054e-17, - "angularVelocity": -1.2751725823370097e-16, - "velocityX": 2.48773644881765, - "velocityY": 0.1153554223824672, - "timestamp": 5.2354164493605415 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 6.251629909739218e-17, + "angularVelocity": 3.89136693523207e-17, + "velocityX": 3.3321165674695394, + "velocityY": -1.361514725761388, + "timestamp": 9.043535731091694 }, { - "x": 7.673406960142625, - "y": 4.018856353361859, - "heading": -1.8281069995800668e-17, - "angularVelocity": -8.116990254124082e-17, - "velocityX": 2.209940030810585, - "velocityY": 0.13694334022043622, - "timestamp": 5.299281758301133 + "x": 4.285701078795114, + "y": 4.9383884935900895, + "heading": 6.191867349467079e-17, + "angularVelocity": -1.7257822842751934e-17, + "velocityX": 3.4133567862477086, + "velocityY": -1.4888942181949925, + "timestamp": 9.078164989267158 }, { - "x": 7.796760795699071, - "y": 4.0282099476371425, - "heading": -2.4884139524870184e-17, - "angularVelocity": -1.03390551752411e-16, - "velocityX": 1.9314685484602037, - "velocityY": 0.1464581387053742, - "timestamp": 5.363147067241724 + "x": 4.403715151512854, + "y": 4.881600775413148, + "heading": 5.87936361182216e-17, + "angularVelocity": -9.024268901925555e-17, + "velocityX": 3.4079295640631644, + "velocityY": -1.6398768315856658, + "timestamp": 9.112794247442622 }, { - "x": 7.902319649761861, - "y": 4.0376115999719895, - "heading": -2.509945215126813e-17, - "angularVelocity": -3.3713549672899342e-18, - "velocityX": 1.6528355661910739, - "velocityY": 0.14721062953900424, - "timestamp": 5.427012376182315 + "x": 4.5193670084290005, + "y": 4.8201449553533005, + "heading": 6.393419644700586e-17, + "angularVelocity": 1.484455804018553e-16, + "velocityX": 3.3397151140271815, + "velocityY": -1.7746790805755832, + "timestamp": 9.147423505618086 }, { - "x": 7.9900874176780725, - "y": 4.046637816644616, - "heading": -1.900477135412208e-17, - "angularVelocity": 9.543022492350273e-17, - "velocityX": 1.3742635770830707, - "velocityY": 0.14133207561907785, - "timestamp": 5.490877685122906 - }, - { - "x": 8.060074171241288, - "y": 4.054957461114281, - "heading": -1.4433757000062975e-17, - "angularVelocity": 7.157272750648916e-17, - "velocityX": 1.0958492916446645, - "velocityY": 0.13026860133729024, - "timestamp": 5.554742994063497 + "x": 4.63247211701513, + "y": 4.7541191698650405, + "heading": 6.969863736825022e-17, + "angularVelocity": 1.664615768552329e-16, + "velocityX": 3.2661718600217573, + "velocityY": -1.9066474122463712, + "timestamp": 9.18205276379355 }, { - "x": 8.112292461453118, - "y": 4.062304768239738, - "heading": -9.3901869246783e-18, - "angularVelocity": 7.897198273854564e-17, - "velocityX": 0.8176315291985099, - "velocityY": 0.11504378898864956, - "timestamp": 5.6186083030040885 + "x": 4.742850086360494, + "y": 4.6836289301368605, + "heading": 7.573971746769133e-17, + "angularVelocity": 1.7445017357345818e-16, + "velocityX": 3.187419400845542, + "velocityY": -2.035568864080495, + "timestamp": 9.216682021969014 }, { - "x": 8.146755551516593, - "y": 4.0684618614995625, - "heading": -4.677400275001561e-18, - "angularVelocity": 7.379259143526985e-17, - "velocityX": 0.5396214413607953, - "velocityY": 0.09640747632727756, - "timestamp": 5.68247361194468 + "x": 4.850325224446504, + "y": 4.608787332408603, + "heading": 7.95254691851129e-17, + "angularVelocity": 1.0932234523778094e-16, + "velocityX": 3.1035934278880775, + "velocityY": -2.161224400160131, + "timestamp": 9.251311280144478 }, { - "x": 8.1634765625, - "y": 4.073246955871582, - "heading": -3.026226030610745e-28, - "angularVelocity": 7.323851324219789e-17, - "velocityX": 0.2618168025924992, - "velocityY": 0.07492478234891005, - "timestamp": 5.746338920885271 + "x": 4.959315852672914, + "y": 4.536170440415771, + "heading": 8.132510178394684e-17, + "angularVelocity": 5.1968557625797656e-17, + "velocityX": 3.1473567142028136, + "velocityY": -2.0969808716342646, + "timestamp": 9.285940538319942 }, { - "x": 8.160404735497007, - "y": 4.076664969326324, - "heading": 5.687417788003061e-18, - "angularVelocity": 8.091717809987449e-17, - "velocityX": -0.04370411704181345, - "velocityY": 0.04862945078972498, - "timestamp": 5.816625823147878 + "x": 5.071119482307337, + "y": 4.467964000951313, + "heading": 8.196310550599571e-17, + "angularVelocity": 1.842383452283251e-17, + "velocityX": 3.2285886422377668, + "velocityY": -1.969618844240326, + "timestamp": 9.320569796495406 }, { - "x": 8.13585178429954, - "y": 4.078318050056324, - "heading": 9.715319347453537e-18, - "angularVelocity": 5.730657391283969e-17, - "velocityX": -0.34932470214341615, - "velocityY": 0.02351904375900896, - "timestamp": 5.886912725410485 + "x": 5.185557951100297, + "y": 4.404277214638869, + "heading": 8.211438156325687e-17, + "angularVelocity": 4.3684463753045605e-18, + "velocityX": 3.3046757228557744, + "velocityY": -1.8391033960284853, + "timestamp": 9.35519905467087 }, { - "x": 8.089809855979007, - "y": 4.078304537416186, - "heading": 1.421739507206778e-17, - "angularVelocity": 6.405284028537325e-17, - "velocityX": -0.6550570140153344, - "velocityY": -0.00019224976064422702, - "timestamp": 5.957199627673092 + "x": 5.302448704050349, + "y": 4.345211813287043, + "heading": 8.111338797415481e-17, + "angularVelocity": -2.8906007282049283e-17, + "velocityX": 3.37549110517396, + "velocityY": -1.7056502063239702, + "timestamp": 9.389828312846333 }, { - "x": 8.022270134239134, - "y": 4.076742304699779, - "heading": 2.3560062445993e-17, - "angularVelocity": 1.3292188264586088e-16, - "velocityX": -0.9609147588768534, - "velocityY": -0.022226512566613944, - "timestamp": 6.0274865299356994 + "x": 5.421605231445948, + "y": 4.290862097784136, + "heading": 7.868835126918448e-17, + "angularVelocity": -7.002854905419881e-17, + "velocityX": 3.4409205877827787, + "velocityY": -1.5694738601537552, + "timestamp": 9.424457571021797 }, { - "x": 7.933222739684713, - "y": 4.073775206509657, - "heading": 3.0510865621673614e-17, - "angularVelocity": 9.889186963848529e-17, - "velocityX": -1.2669130618635012, - "velocityY": -0.04221409814073001, - "timestamp": 6.097773432198307 + "x": 5.542837393133187, + "y": 4.2413148237692955, + "heading": 7.613874923845377e-17, + "angularVelocity": -7.362566122075624e-17, + "velocityX": 3.5008593332540854, + "velocityY": -1.4307922440552452, + "timestamp": 9.459086829197261 }, { - "x": 7.822656702045201, - "y": 4.069582705061331, - "heading": 3.6286017590367637e-17, - "angularVelocity": 8.21654075361425e-17, - "velocityX": -1.573067443297094, - "velocityY": -0.05964840266629161, - "timestamp": 6.168060334460914 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 7.348031788343025e-17, + "angularVelocity": -7.676835991047679e-17, + "velocityX": 3.5552114649352746, + "velocityY": -1.289826914239228, + "timestamp": 9.493716087372725 }, { - "x": 7.690560152164254, - "y": 4.0643949017183925, - "heading": 4.582493898384423e-17, - "angularVelocity": 1.3571406743753222e-16, - "velocityX": -1.8793906919870382, - "velocityY": -0.07380896263653505, - "timestamp": 6.238347236723521 + "x": 5.934272394454851, + "y": 4.1243379326499765, + "heading": 6.301578755269389e-17, + "angularVelocity": -1.4241558943688294e-16, + "velocityX": 3.6516732759640362, + "velocityY": -0.9841085621303537, + "timestamp": 9.567194913476962 }, { - "x": 7.53692113699896, - "y": 4.058517364536531, - "heading": 5.495539333254171e-17, - "angularVelocity": 1.2990264266804977e-16, - "velocityX": -2.1858840014212815, - "velocityY": -0.0836220831002289, - "timestamp": 6.308634138986128 + "x": 6.207753015610448, + "y": 4.075010136678792, + "heading": 7.880077877117389e-17, + "angularVelocity": 2.148236717340586e-16, + "velocityX": 3.721896982508106, + "velocityY": -0.6713198697704936, + "timestamp": 9.6406737395812 }, { - "x": 7.361730243304987, - "y": 4.052375214932341, - "heading": 6.396812526564607e-17, - "angularVelocity": 1.2822775856015118e-16, - "velocityX": -2.4925112368648836, - "velocityY": -0.08738683035483485, - "timestamp": 6.378921041248735 + "x": 6.484428622538185, + "y": 4.049019979746833, + "heading": 4.7380289374867153e-17, + "angularVelocity": -4.2761283844936273e-16, + "velocityX": 3.7653787029102097, + "velocityY": -0.3537094740066765, + "timestamp": 9.714152565685437 }, { - "x": 7.164988961620082, - "y": 4.046599375673031, - "heading": 7.356788600543696e-17, - "angularVelocity": 1.3657965326299643e-16, - "velocityX": -2.799117265829076, - "velocityY": -0.08217518589352116, - "timestamp": 6.449207943511342 + "x": 6.743277361498475, + "y": 4.038426387270388, + "heading": 3.80588957590445e-17, + "angularVelocity": -1.2685822719245393e-16, + "velocityX": 3.5227663897772135, + "velocityY": -0.1441720429966888, + "timestamp": 9.787631391789674 }, { - "x": 6.946739460744683, - "y": 4.042219881595439, - "heading": 8.53416334460989e-17, - "angularVelocity": 1.6750983557133948e-16, - "velocityX": -3.105123342325879, - "velocityY": -0.06230882193712492, - "timestamp": 6.519494845773949 + "x": 6.978672820957151, + "y": 4.030025293070568, + "heading": 3.301536102704029e-17, + "angularVelocity": -6.863929378502764e-17, + "velocityX": 3.20358220101046, + "velocityY": -0.11433353858840364, + "timestamp": 9.861110217893911 }, { - "x": 6.707199372096715, - "y": 4.041196560014842, - "heading": 9.976814238544392e-17, - "angularVelocity": 2.052517393014489e-16, - "velocityX": -3.408033089194812, - "velocityY": -0.014559207301144003, - "timestamp": 6.589781748036557 + "x": 7.190552691043286, + "y": 4.022995753598098, + "heading": 2.761165208046759e-17, + "angularVelocity": -7.354103532969467e-17, + "velocityX": 2.8835500146064224, + "velocityY": -0.09566755275184706, + "timestamp": 9.934589043998148 }, { - "x": 6.447738133610024, - "y": 4.048401251612496, - "heading": 1.0469202046971274e-16, - "angularVelocity": 7.00539919377632e-17, - "velocityX": -3.691459292334275, - "velocityY": 0.10250404222876178, - "timestamp": 6.660068650299164 + "x": 7.378901756507518, + "y": 4.0170458446186466, + "heading": 2.2049007606542194e-17, + "angularVelocity": -7.570404657768667e-17, + "velocityX": 2.563310758354254, + "velocityY": -0.08097446972017466, + "timestamp": 10.008067870102385 }, { - "x": 6.18341657905221, - "y": 4.076604236955095, - "heading": 9.517171884835342e-17, - "angularVelocity": -1.354491564534057e-16, - "velocityX": -3.7606089619691896, - "velocityY": 0.4012552045219841, - "timestamp": 6.730355552561771 + "x": 7.543713628095823, + "y": 4.012025993134882, + "heading": 1.7040082343642412e-17, + "angularVelocity": -6.816828096488101e-17, + "velocityX": 2.2429845484262843, + "velocityY": -0.06831697986905093, + "timestamp": 10.081546696206622 }, { - "x": 5.922248770654031, - "y": 4.126128704367341, - "heading": 8.827514914451601e-17, - "angularVelocity": -9.812026823816515e-17, - "velocityX": -3.7157393481704415, - "velocityY": 0.7046044969688833, - "timestamp": 6.800642454824378 + "x": 7.684984887163916, + "y": 4.007845275924109, + "heading": 1.249754397163312e-17, + "angularVelocity": -6.182105257790516e-17, + "velocityX": 1.922611812928072, + "velocityY": -0.05689689713935573, + "timestamp": 10.15502552231086 + }, + { + "x": 7.802713433535344, + "y": 4.004442580511056, + "heading": 8.52630628004598e-18, + "angularVelocity": -5.4046014370140334e-17, + "velocityX": 1.6022104953666365, + "velocityY": -0.046308516255087515, + "timestamp": 10.228504348415097 + }, + { + "x": 7.8968978566663255, + "y": 4.001774009946332, + "heading": 5.220002438781615e-18, + "angularVelocity": -4.499668838635962e-17, + "velocityX": 1.2817899812031746, + "velocityY": -0.03631754487937096, + "timestamp": 10.301983174519334 + }, + { + "x": 7.967537148396796, + "y": 3.9998065067615447, + "heading": 2.6237181268422074e-18, + "angularVelocity": -3.5333775041531244e-17, + "velocityX": 0.9613557466236943, + "velocityY": -0.026776464583087325, + "timestamp": 10.375462000623571 + }, + { + "x": 8.014630554418627, + "y": 3.998514279441312, + "heading": 9.593365131903801e-19, + "angularVelocity": -2.265117315883944e-17, + "velocityX": 0.6409112463912332, + "velocityY": -0.01758639037590862, + "timestamp": 10.448940826727808 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 1.1585643597990433e-28, + "angularVelocity": -1.3055958619386531e-17, + "velocityX": 0.3204587915210466, + "velocityY": -0.008677810201891931, + "timestamp": 10.522419652832046 + }, + { + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 5.650895976930055e-29, + "angularVelocity": -3.8630399964154185e-29, + "velocityX": 1.5845610208926219e-27, + "velocityY": -6.207718033855077e-27, + "timestamp": 10.595898478936283 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 1.2792598362128638, + "isStopPoint": false, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 2.4932366624325915, + "isStopPoint": true, + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 3.424227005183806, + "isStopPoint": true, + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 4.224909036593179, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { + "timestamp": 4.6758758399070866, + "isStopPoint": false, "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 8.108309692704171e-17, - "angularVelocity": -1.0232421668622016e-16, - "velocityX": -3.6464409951607806, - "velocityY": 1.003321641969972, - "timestamp": 6.870929357086985 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "timestamp": 5.107685831479359, + "isStopPoint": false, + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 5.746338920885271, + "isStopPoint": false, + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 6.870929357086985, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 7.321374265868669, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 8.215867229680786, + "isStopPoint": true, + "x": 2.6583051681518555, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 9.043535731091694, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 9.493716087372725, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 + }, + { + "timestamp": 10.595898478936283, + "isStopPoint": true, + "x": 8.038177490234375, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 10 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": true + }, + "SourceLanePGCSprint": { + "waypoints": [ + { + "x": 0.43324199318885803, + "y": 4.121134281158447, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 25 + }, + { + "x": 1.664683222770691, + "y": 3.8699920177459717, + "heading": -0.7701709445795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 1.848745584487915, + "y": 4.087520122528076, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.819255828857422, + "y": 4.10425329208374, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 1.664683222770691, + "y": 3.8699920177459717, + "heading": -0.7701709445795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "x": 4.7937421798706055, + "y": 1.3935176134109497, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 + }, + { + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.433241993188858, + "y": 4.121134281158447, + "heading": 7.738396447124189e-29, + "angularVelocity": 1.1248344329069217e-28, + "velocityX": -3.4263332588426173e-28, + "velocityY": 2.738469239483736e-27, + "timestamp": 0 + }, + { + "x": 0.4546918803218127, + "y": 4.109218526998405, + "heading": -0.00893730014505659, + "angularVelocity": -0.11893237232009289, + "velocityX": 0.28544257452644645, + "velocityY": -0.1585678993919355, + "timestamp": 0.07514606806129173 + }, + { + "x": 0.49759166351299144, + "y": 4.085387310900469, + "heading": -0.026812413293685552, + "angularVelocity": -0.23787156946188326, + "velocityX": 0.5708852678251672, + "velocityY": -0.3171319100621368, + "timestamp": 0.15029213612258346 + }, + { + "x": 0.5619414537509306, + "y": 4.049641027145361, + "heading": -0.05362171709181762, + "angularVelocity": -0.35676256243061816, + "velocityX": 0.8563294380945293, + "velocityY": -0.47569067387467856, + "timestamp": 0.2254382041838752 + }, + { + "x": 0.6477414523823173, + "y": 4.001980177868811, + "heading": -0.08935853103818607, + "angularVelocity": -0.4755646551888817, + "velocityX": 1.1417762877680475, + "velocityY": -0.634242755558099, + "timestamp": 0.30058427224516693 + }, + { + "x": 0.7549919358856848, + "y": 3.942405375866793, + "heading": -0.13401442198910787, + "angularVelocity": -0.5942545245946725, + "velocityX": 1.4272268166564648, + "velocityY": -0.7927866825104752, + "timestamp": 0.3757303403064587 + }, + { + "x": 0.8836932393387835, + "y": 3.870917341642709, + "heading": -0.18758057155461047, + "angularVelocity": -0.7128270440152931, + "velocityX": 1.7126818045639527, + "velocityY": -0.9513210214242487, + "timestamp": 0.45087640836775045 + }, + { + "x": 1.0338457421573335, + "y": 3.7875168933266514, + "heading": -0.2500489994644535, + "angularVelocity": -0.8312933666588067, + "velocityX": 1.998141841514319, + "velocityY": -1.1098444731404118, + "timestamp": 0.5260224764290422 + }, + { + "x": 1.2054498603433927, + "y": 3.692204931210594, + "heading": -0.3214134495606155, + "angularVelocity": -0.9496764360040053, + "velocityX": 2.2836074143771987, + "velocityY": -1.2683559442966215, + "timestamp": 0.601168544490334 + }, + { + "x": 1.3985060453585743, + "y": 3.5849824250394744, + "heading": -0.4016698579067676, + "angularVelocity": -1.068005424857256, + "velocityX": 2.569078995027635, + "velocityY": -1.4268545106533708, + "timestamp": 0.6763146125516257 + }, + { + "x": 1.5915937570064054, + "y": 3.477819815501255, + "heading": -0.4817685926160567, + "angularVelocity": -1.0659071961550632, + "velocityX": 2.5694985330482245, + "velocityY": -1.426057441233171, + "timestamp": 0.7514606806129175 + }, + { + "x": 1.7632284859071354, + "y": 3.3825660365490977, + "heading": -0.5529765476421505, + "angularVelocity": -0.9475938909806194, + "velocityX": 2.2840147638960824, + "velocityY": -1.2675816767214638, + "timestamp": 0.8266067486742092 + }, + { + "x": 1.9134098098681454, + "y": 3.2992204007592485, + "heading": -0.6152918417750989, + "angularVelocity": -0.8292555517624935, + "velocityX": 1.9985253764510553, + "velocityY": -1.1091150600437194, + "timestamp": 0.901752816735501 + }, + { + "x": 2.042137357772617, + "y": 3.227782305964734, + "heading": -0.6687115003563293, + "angularVelocity": -0.7108776275248302, + "velocityX": 1.7130310504001012, + "velocityY": -0.9506564566524899, + "timestamp": 0.9768988847967928 + }, + { + "x": 2.1494108084908032, + "y": 3.168251239438324, + "heading": -0.7132320417999736, + "angularVelocity": -0.5924533723751416, + "velocityX": 1.4275324509419445, + "velocityY": -0.792204676335884, + "timestamp": 1.0520449528580844 + }, + { + "x": 2.2352298936741635, + "y": 3.120626784366874, + "heading": -0.7488501306754739, + "angularVelocity": -0.4739847312629708, + "velocityX": 1.1420302804580957, + "velocityY": -0.6337584427252518, + "timestamp": 1.127191020919376 + }, + { + "x": 2.299594398786613, + "y": 3.0849086278606843, + "heading": -0.7755632624576652, + "angularVelocity": -0.3554827613921602, + "velocityX": 0.8565252550532846, + "velocityY": -0.475316372868058, + "timestamp": 1.2023370889806677 + }, + { + "x": 2.3425041622841327, + "y": 3.061096568391248, + "heading": -0.7933703919070813, + "angularVelocity": -0.2369668820847947, + "velocityX": 0.5710180799149875, + "velocityY": -0.31687698483458104, + "timestamp": 1.2774831570419594 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": -0.11846288571271453, + "velocityX": 0.2855094390135433, + "velocityY": -0.1584387241831784, + "timestamp": 1.352629225103251 + }, + { + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, + "angularVelocity": 1.9944978930198404e-24, + "velocityX": 7.05107927160795e-24, + "velocityY": 9.089761159537798e-24, + "timestamp": 1.4277752931645427 + }, + { + "x": 2.3773573652919286, + "y": 3.040801892787579, + "heading": -0.7860925568420652, + "angularVelocity": 0.2647627943666689, + "velocityX": 0.2192460319725613, + "velocityY": -0.13726925804658435, + "timestamp": 1.4888860514802038 + }, + { + "x": 2.4041795588293358, + "y": 3.024017002068538, + "heading": -0.754166261219504, + "angularVelocity": 0.5224333080216316, + "velocityX": 0.43891115536253184, + "velocityY": -0.27466343376629077, + "timestamp": 1.549996809795865 + }, + { + "x": 2.4444548115194884, + "y": 2.998827368062219, + "heading": -0.7070016065582421, + "angularVelocity": 0.7717897136480927, + "velocityX": 0.6590533942013143, + "velocityY": -0.4121963906290397, + "timestamp": 1.6111075681115261 + }, + { + "x": 2.4982173316537715, + "y": 2.9652220428664715, + "heading": -0.6452345488674253, + "angularVelocity": 1.010739506320076, + "velocityX": 0.8797554083125275, + "velocityY": -0.5499084960157551, + "timestamp": 1.6722183264271873 + }, + { + "x": 2.565507310796468, + "y": 2.9231853998386073, + "heading": -0.5696881555564717, + "angularVelocity": 1.2362208454479753, + "velocityX": 1.1011151063633902, + "velocityY": -0.687876311577224, + "timestamp": 1.7333290847428484 + }, + { + "x": 2.646370899659449, + "y": 2.872694862845298, + "heading": -0.4814293119182948, + "angularVelocity": 1.444243960814317, + "velocityX": 1.3232300022409869, + "velocityY": -0.8262135569077015, + "timestamp": 1.7944398430585096 + }, + { + "x": 2.740859365850477, + "y": 2.8137198450437673, + "heading": -0.38181759469062915, + "angularVelocity": 1.6300193283992952, + "velocityX": 1.5461838274524098, + "velocityY": -0.9650513171003614, + "timestamp": 1.8555506013741707 + }, + { + "x": 2.8490284341196963, + "y": 2.7462234286806457, + "heading": -0.27256619156885004, + "angularVelocity": 1.787760553672932, + "velocityX": 1.7700495174758504, + "velocityY": -1.1044931894720724, + "timestamp": 1.9166613596898319 + }, + { + "x": 2.97093875412634, + "y": 2.6701673256371343, + "heading": -0.15589887958558987, + "angularVelocity": 1.909112490154803, + "velocityX": 1.994907662198022, + "velocityY": -1.2445615983138625, + "timestamp": 1.977772118005493 + }, + { + "x": 3.1066533293644194, + "y": 2.5855194210396055, + "heading": -0.03500682401650803, + "angularVelocity": 1.9782450570262398, + "velocityX": 2.220796779137645, + "velocityY": -1.3851555263033848, + "timestamp": 2.038882876321154 + }, + { + "x": 3.2562037135860464, + "y": 2.492271514146211, + "heading": 0.08476246781347457, + "angularVelocity": 1.959872453412, + "velocityX": 2.4472022331835617, + "velocityY": -1.5258836490251428, + "timestamp": 2.099993634636815 + }, + { + "x": 3.419377549511949, + "y": 2.390544477111736, + "heading": 0.19301716737769203, + "angularVelocity": 1.7714507649379716, + "velocityX": 2.6701327298713187, + "velocityY": -1.6646338523409365, + "timestamp": 2.1611043929524762 + }, + { + "x": 3.5932192698224203, + "y": 2.280069793657614, + "heading": 0.2658871078344446, + "angularVelocity": 1.1924240913580317, + "velocityX": 2.8446991184843475, + "velocityY": -1.8077779837631391, + "timestamp": 2.2222151512681374 + }, + { + "x": 3.777176249725073, + "y": 2.160717630889141, + "heading": 0.3003277545805106, + "angularVelocity": 0.5635774730231045, + "velocityX": 3.010222503743814, + "velocityY": -1.9530466657273664, + "timestamp": 2.2833259095837986 + }, + { + "x": 3.9732480656956923, + "y": 2.03835976913752, + "heading": 0.3003278017850349, + "angularVelocity": 7.724421305582541e-7, + "velocityX": 3.208466420230474, + "velocityY": -2.0022311148487826, + "timestamp": 2.3444366678994597 + }, + { + "x": 4.174665289116757, + "y": 1.9250164613959089, + "heading": 0.3003278489820802, + "angularVelocity": 7.723197446110156e-7, + "velocityX": 3.295937228935466, + "velocityY": -1.8547193794609556, + "timestamp": 2.405547426215121 + }, + { + "x": 4.383567042240086, + "y": 1.82614452102736, + "heading": 0.30032789699541584, + "angularVelocity": 7.856773019778577e-7, + "velocityX": 3.4184120583853495, + "velocityY": -1.6179138190011713, + "timestamp": 2.466658184530782 + }, + { + "x": 4.598915447104952, + "y": 1.7422358380933087, + "heading": 0.30032794702414056, + "angularVelocity": 8.186565851278932e-7, + "velocityX": 3.5239033322497164, + "velocityY": -1.3730591019772607, + "timestamp": 2.527768942846443 + }, + { + "x": 4.819640328386772, + "y": 1.6737075334125113, + "heading": 0.30032800050450253, + "angularVelocity": 8.751382481764384e-7, + "velocityX": 3.611882545159867, + "velocityY": -1.1213787321509214, + "timestamp": 2.5888797011621043 + }, + { + "x": 5.044644747071726, + "y": 1.620900221581336, + "heading": 0.30032805931258105, + "angularVelocity": 9.623195678675265e-7, + "velocityX": 3.681911743309069, + "velocityY": -0.8641246367522506, + "timestamp": 2.6499904594777655 + }, + { + "x": 5.27281048204291, + "y": 1.5840763655637535, + "heading": 0.3003281261036848, + "angularVelocity": 0.0000010929516437587844, + "velocityX": 3.7336426720908507, + "velocityY": -0.6025756680578687, + "timestamp": 2.7111012177934266 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.3003281998542592, + "angularVelocity": 0.0000012068345473522654, + "velocityX": 3.7668181767205913, + "velocityY": -0.3380318248368492, + "timestamp": 2.7722119761090878 + }, + { + "x": 5.642202615777798, + "y": 1.556873162710076, + "heading": 0.30032827970972786, + "angularVelocity": 0.0000021672306415301637, + "velocityX": 3.777779819586101, + "velocityY": -0.17764976646888633, + "timestamp": 2.809058755213733 + }, + { + "x": 5.781554061367138, + "y": 1.556248726330051, + "heading": 0.3003283534295918, + "angularVelocity": 0.0000020007139214038515, + "velocityX": 3.781916601002728, + "velocityY": -0.016946837558083135, + "timestamp": 2.8459055343183786 + }, + { + "x": 5.920806157723901, + "y": 1.5615468010933287, + "heading": 0.30032842223361694, + "angularVelocity": 0.000001867300937043874, + "velocityX": 3.7792203210295474, + "velocityY": 0.14378664545497588, + "timestamp": 2.882752313423024 + }, + { + "x": 6.059707308506225, + "y": 1.5727578116463863, + "heading": 0.3003284870919326, + "angularVelocity": 0.000001760216693496236, + "velocityX": 3.7696958637237334, + "velocityY": 0.3042602589826913, + "timestamp": 2.9195990925276694 + }, + { + "x": 6.198006551988053, + "y": 1.5898614987106987, + "heading": 0.3003285487925156, + "angularVelocity": 0.0000016745176784450775, + "velocityX": 3.753360452186466, + "velocityY": 0.4641840475591571, + "timestamp": 2.9564458716323148 + }, + { + "x": 6.335454014584064, + "y": 1.6128269556768218, + "heading": 0.30032860799137623, + "angularVelocity": 0.0000016066223984499512, + "velocityX": 3.730243617919986, + "velocityY": 0.6232690488604353, + "timestamp": 2.99329265073696 + }, + { + "x": 6.47180136240203, + "y": 1.641612684542814, + "heading": 0.30032866524871465, + "angularVelocity": 0.0000015539306228254143, + "velocityX": 3.700387147292773, + "velocityY": 0.7812278186986222, + "timestamp": 3.0301394298416056 + }, + { + "x": 6.606802249418837, + "y": 1.6761666710700263, + "heading": 0.30032872317249165, + "angularVelocity": 0.0000015720173768370867, + "velocityX": 3.6638449899081134, + "velocityY": 0.937774952569887, + "timestamp": 3.066986208946251 + }, + { + "x": 6.740172242433044, + "y": 1.7164148916635715, + "heading": 0.30049565594915995, + "angularVelocity": 0.004530457769298999, + "velocityX": 3.619583482057768, + "velocityY": 1.0923131294390627, + "timestamp": 3.1038329880508964 + }, + { + "x": 6.869834708516544, + "y": 1.760792645305141, + "heading": 0.3094576493704555, + "angularVelocity": 0.2432232515043802, + "velocityX": 3.5189633730334187, + "velocityY": 1.2043862372756071, + "timestamp": 3.1406797671555418 + }, + { + "x": 6.995163652975955, + "y": 1.807928879541571, + "heading": 0.3302942112120631, + "angularVelocity": 0.5654920822911383, + "velocityX": 3.4013541347392664, + "velocityY": 1.2792497846979316, + "timestamp": 3.177526546260187 + }, + { + "x": 7.114961828122759, + "y": 1.8557263226321454, + "heading": 0.3593656658524349, + "angularVelocity": 0.7889822488366823, + "velocityX": 3.2512522955283503, + "velocityY": 1.2971946056622388, + "timestamp": 3.2143733253648326 + }, + { + "x": 7.2288849177114916, + "y": 1.9032201895276672, + "heading": 0.39122292476577963, + "angularVelocity": 0.8645873448767251, + "velocityX": 3.091805915116442, + "velocityY": 1.2889557255639152, + "timestamp": 3.251220104469478 + }, + { + "x": 7.3369415857594324, + "y": 1.9498953701157864, + "heading": 0.4232031564001013, + "angularVelocity": 0.8679247524864317, + "velocityX": 2.932594671058162, + "velocityY": 1.2667370587687046, + "timestamp": 3.2880668835741234 + }, + { + "x": 7.4391897785347085, + "y": 1.9954627576139183, + "heading": 0.4537900488962921, + "angularVelocity": 0.8301103444977762, + "velocityX": 2.774956054771842, + "velocityY": 1.236672203253364, + "timestamp": 3.3249136626787688 + }, + { + "x": 7.535687239808996, + "y": 2.0397404710658504, + "heading": 0.48199026779702103, + "angularVelocity": 0.7653374212340174, + "velocityX": 2.618884570622394, + "velocityY": 1.2016712051325453, + "timestamp": 3.361760441783414 + }, + { + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, + "angularVelocity": 0.681422832753116, + "velocityX": 2.464168636543981, + "velocityY": 1.1632956752609667, + "timestamp": 3.3986072208880596 + }, + { + "x": 7.766066221949162, + "y": 2.1519606772611084, + "heading": 0.5396117040985706, + "angularVelocity": 0.5123769157726334, + "velocityX": 2.199683557182638, + "velocityY": 1.0929921902308588, + "timestamp": 3.4620628503385116 + }, + { + "x": 7.88917922885163, + "y": 2.215677284676183, + "heading": 0.5626038110324001, + "angularVelocity": 0.36233360433028805, + "velocityX": 1.9401431830189138, + "velocityY": 1.0041127629949167, + "timestamp": 3.5255184797889636 + }, + { + "x": 7.996083890855478, + "y": 2.2729699125828824, + "heading": 0.576920084072235, + "angularVelocity": 0.2256107639908224, + "velocityX": 1.6847151770407296, + "velocityY": 0.9028769929929541, + "timestamp": 3.5889741092394156 + }, + { + "x": 8.086991794921751, + "y": 2.3232832101942007, + "heading": 0.5831794167828934, + "angularVelocity": 0.09864109401902421, + "velocityX": 1.4326215791028423, + "velocityY": 0.792889425998124, + "timestamp": 3.6524297386898676 + }, + { + "x": 8.162075125847174, + "y": 2.3662047030478557, + "heading": 0.5818525424229896, + "angularVelocity": -0.02091027023756688, + "velocityX": 1.1832414487992695, + "velocityY": 0.6764016561709427, + "timestamp": 3.7158853681403197 + }, + { + "x": 8.2214756151654, + "y": 2.4014165863300603, + "heading": 0.5733088893147004, + "angularVelocity": -0.1346397976393925, + "velocityX": 0.936094871844373, + "velocityY": 0.5549055865831228, + "timestamp": 3.7793409975907717 + }, + { + "x": 8.265311459891254, + "y": 2.4286667893871807, + "heading": 0.5578458366246501, + "angularVelocity": -0.24368291393475716, + "velocityX": 0.6908109667414808, + "velocityY": 0.4294371246982685, + "timestamp": 3.8427966270412237 + }, + { + "x": 8.29368248788513, + "y": 2.4477506399099855, + "heading": 0.5357077069480222, + "angularVelocity": -0.34887573992019155, + "velocityX": 0.44710025319389435, + "velocityY": 0.30074322306905504, + "timestamp": 3.9062522564916757 + }, + { + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, + "angularVelocity": -0.45085365638085945, + "velocityX": 0.20473385621503526, + "velocityY": 0.16937940002276766, + "timestamp": 3.9697078859421278 + }, + { + "x": 8.303679627930654, + "y": 2.4605245534000177, + "heading": 0.47064149696897567, + "angularVelocity": -0.5538247976382693, + "velocityX": -0.04548808629479942, + "velocityY": 0.03077484607623031, + "timestamp": 4.035535576538194 + }, + { + "x": 8.284168636347566, + "y": 2.453496432112781, + "heading": 0.4276658140977222, + "angularVelocity": -0.6528511403348756, + "velocityX": -0.29639489713850375, + "velocityY": -0.10676542384515514, + "timestamp": 4.1013632671342615 + }, + { + "x": 8.248089021230637, + "y": 2.4374965938671473, + "heading": 0.378472576604384, + "angularVelocity": -0.7473031037226994, + "velocityX": -0.5480917648823836, + "velocityY": -0.24305635061409855, + "timestamp": 4.1671909577303285 + }, + { + "x": 8.195380139344671, + "y": 2.412622948042365, + "heading": 0.3234149472418141, + "angularVelocity": -0.8363901097551141, + "velocityX": -0.8007098746544274, + "velocityY": -0.37785991882067893, + "timestamp": 4.233018648326396 + }, + { + "x": 8.125970395948434, + "y": 2.3789940000965495, + "heading": 0.2629133084829053, + "angularVelocity": -0.9190910118685506, + "velocityX": -1.0544155927047596, + "velocityY": -0.5108632498164021, + "timestamp": 4.298846338922463 + }, + { + "x": 8.039774042344641, + "y": 2.3367560367748186, + "heading": 0.19747799875428093, + "angularVelocity": -0.9940392733834453, + "velocityX": -1.3094239342636282, + "velocityY": -0.6416443131950763, + "timestamp": 4.36467402951853 + }, + { + "x": 7.936686649427289, + "y": 2.286094003355545, + "heading": 0.12774482040942284, + "angularVelocity": -1.0593289497691225, + "velocityX": -1.5660186766982322, + "velocityY": -0.7696158403937725, + "timestamp": 4.430501720114597 + }, + { + "x": 7.816578572926189, + "y": 2.2272487613470173, + "heading": 0.05453269620309629, + "angularVelocity": -1.1121782268737388, + "velocityX": -1.8245828679925904, + "velocityY": -0.8939283981510958, + "timestamp": 4.496329410710664 + }, + { + "x": 7.679285357941873, + "y": 2.160546156413331, + "heading": -0.02105873619647849, + "angularVelocity": -1.1483227151841096, + "velocityX": -2.085645322525112, + "velocityY": -1.0132909772422172, + "timestamp": 4.562157101306731 + }, + { + "x": 7.52459365410468, + "y": 2.086449920391606, + "heading": -0.09747489817686252, + "angularVelocity": -1.16085132697865, + "velocityX": -2.349948819964157, + "velocityY": -1.1256089246149548, + "timestamp": 4.627984791902798 + }, + { + "x": 7.352222023674252, + "y": 2.0056683293260935, + "heading": -0.17237070551570732, + "angularVelocity": -1.1377553528107507, + "velocityX": -2.6185276875067363, + "velocityY": -1.2271673263035494, + "timestamp": 4.693812482498865 + }, + { + "x": 7.1618070022795735, + "y": 1.9194015924783232, + "heading": -0.24188707800247594, + "angularVelocity": -1.0560354139314483, + "velocityX": -2.8926280060941036, + "velocityY": -1.3104931384745382, + "timestamp": 4.759640173094932 + }, + { + "x": 6.953005373134873, + "y": 1.8300479988886789, + "heading": -0.29871595232882114, + "angularVelocity": -0.8632974028370489, + "velocityX": -3.1719421911054577, + "velocityY": -1.3573861209553544, + "timestamp": 4.825467863690999 + }, + { + "x": 6.727423586928556, + "y": 1.7436722229358965, + "heading": -0.32018076563144104, + "angularVelocity": -0.3260757457577048, + "velocityX": -3.426852501791936, + "velocityY": -1.3121495706541393, + "timestamp": 4.891295554287066 + }, + { + "x": 6.489444642291127, + "y": 1.670557467554106, + "heading": -0.32018100817910516, + "angularVelocity": -0.0000036845841301644418, + "velocityX": -3.615179911106431, + "velocityY": -1.1106990799728806, + "timestamp": 4.9571232448831335 + }, + { + "x": 6.246603596847642, + "y": 1.615712184721808, + "heading": -0.3201810383505521, + "angularVelocity": -4.583397455824824e-7, + "velocityX": -3.6890409377052413, + "velocityY": -0.8331643163488851, + "timestamp": 5.022950935479201 + }, + { + "x": 6.0003008879136885, + "y": 1.5794526731151552, + "heading": -0.3201810728782718, + "angularVelocity": -5.245166498428474e-7, + "velocityX": -3.7416276752791178, + "velocityY": -0.5508246040279513, + "timestamp": 5.088778626075268 + }, + { + "x": 5.751956851390291, + "y": 1.561988034181555, + "heading": -0.3201811140764657, + "angularVelocity": -6.258489930223753e-7, + "velocityX": -3.7726378409245966, + "velocityY": -0.265308394924062, + "timestamp": 5.154606316671335 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.3201811662076607, + "angularVelocity": -7.919341312199672e-7, + "velocityX": -3.7818925725102797, + "velocityY": 0.02173781608673755, + "timestamp": 5.220434007267402 + }, + { + "x": 5.250082401017072, + "y": 1.5845061941743261, + "heading": -0.3201812130071943, + "angularVelocity": -6.973782997664008e-7, + "velocityX": -3.7688784475631176, + "velocityY": 0.3142288252976883, + "timestamp": 5.28754182190281 + }, + { + "x": 4.999550332100038, + "y": 1.6250954552906307, + "heading": -0.3201812494932628, + "angularVelocity": -5.436932891401309e-7, + "velocityX": -3.7332771194853676, + "velocityY": 0.6048365803121931, + "timestamp": 5.354649636538218 + }, + { + "x": 4.752908853379521, + "y": 1.6849435065641751, + "heading": -0.32018127950441017, + "angularVelocity": -4.4720793763185246e-7, + "velocityX": -3.6753019012838326, + "velocityY": 0.8918194043226525, + "timestamp": 5.421757451173626 + }, + { + "x": 4.5116361075787825, + "y": 1.7636916636244688, + "heading": -0.32018130527180205, + "angularVelocity": -3.839700641654236e-7, + "velocityX": -3.595300295674319, + "velocityY": 1.1734573311338945, + "timestamp": 5.488865265809034 + }, + { + "x": 4.277178056371613, + "y": 1.8608679654762732, + "heading": -0.32018132820641354, + "angularVelocity": -3.4175768691264823e-7, + "velocityX": -3.493751845160869, + "velocityY": 1.4480623811065336, + "timestamp": 5.555973080444442 + }, + { + "x": 4.05093980766007, + "y": 1.975889994456223, + "heading": -0.3201813492710647, + "angularVelocity": -3.138926699011739e-7, + "velocityX": -3.3712653279007916, + "velocityY": 1.7139885958864325, + "timestamp": 5.62308089507985 + }, + { + "x": 3.834277168836918, + "y": 2.1080683355859073, + "heading": -0.3201813691749636, + "angularVelocity": -2.965958441785772e-7, + "velocityX": -3.2285753902174523, + "velocityY": 1.9696415663034246, + "timestamp": 5.690188709715258 + }, + { + "x": 3.6284883544416036, + "y": 2.2566105084469403, + "heading": -0.32018138848592925, + "angularVelocity": -2.877603113639349e-7, + "velocityX": -3.0665402459214466, + "velocityY": 2.2134854736076965, + "timestamp": 5.757296524350666 + }, + { + "x": 3.4324267312993486, + "y": 2.4177739449564437, + "heading": -0.3201814076661106, + "angularVelocity": -2.8581144314704067e-7, + "velocityX": -2.921591534569326, + "velocityY": 2.4015598985765383, + "timestamp": 5.824404338986074 + }, + { + "x": 3.2363665756420468, + "y": 2.578939166702229, + "heading": -0.3201814268462876, + "angularVelocity": -2.858113772828912e-7, + "velocityX": -2.921569666997558, + "velocityY": 2.401586501086132, + "timestamp": 5.891512153621482 + }, + { + "x": 3.04030639781219, + "y": 2.740104361474713, + "heading": -0.3201814460264912, + "angularVelocity": -2.8581177503839375e-7, + "velocityX": -2.9215699973995357, + "velocityY": 2.4015860991463303, + "timestamp": 5.95861996825689 + }, + { + "x": 2.8448806552051593, + "y": 2.899843193504081, + "heading": -0.32455581469161493, + "angularVelocity": -0.06518419186929905, + "velocityX": -2.912116028047787, + "velocityY": 2.380331305634352, + "timestamp": 6.025727782892298 + }, + { + "x": 2.663237615437101, + "y": 3.049167443178603, + "heading": -0.36928029407596225, + "angularVelocity": -0.6664570978407269, + "velocityX": -2.706734539858174, + "velocityY": 2.225139508502705, + "timestamp": 6.092835597527706 + }, + { + "x": 2.4966866332566564, + "y": 3.186079438683932, + "heading": -0.4222560068283227, + "angularVelocity": -0.7894119789203976, + "velocityX": -2.481841840407185, + "velocityY": 2.0401796161767862, + "timestamp": 6.1599434121631145 + }, + { + "x": 2.3453101524500037, + "y": 3.3105160667275593, + "heading": -0.47671806974270053, + "angularVelocity": -0.8115606686086642, + "velocityX": -2.2557206135987404, + "velocityY": 1.8542792477997239, + "timestamp": 6.2270512267985225 + }, + { + "x": 2.209108176535601, + "y": 3.4224777520191383, + "heading": -0.5296390322123383, + "angularVelocity": -0.7885961233748087, + "velocityX": -2.029599334360373, + "velocityY": 1.6683852081886403, + "timestamp": 6.294159041433931 + }, + { + "x": 2.0880704157388417, + "y": 3.5219728662741354, + "heading": -0.579287382494677, + "angularVelocity": -0.7398296390975455, + "velocityX": -1.8036313871096692, + "velocityY": 1.4826159188098624, + "timestamp": 6.361266856069339 + }, + { + "x": 1.9821863263109802, + "y": 3.609010233643184, + "heading": -0.6245361276970436, + "angularVelocity": -0.6742693894623133, + "velocityX": -1.577820556415411, + "velocityY": 1.2969781215781884, + "timestamp": 6.428374670704747 + }, + { + "x": 1.8914467304783316, + "y": 3.6835977422656017, + "heading": -0.664590534011598, + "angularVelocity": -0.5968664980698198, + "velocityX": -1.3521464873447422, + "velocityY": 1.1114578686200178, + "timestamp": 6.495482485340155 + }, + { + "x": 1.815843887624038, + "y": 3.745742167237612, + "heading": -0.6988590085071317, + "angularVelocity": -0.510648047201536, + "velocityX": -1.1265877642572446, + "velocityY": 0.9260385740414423, + "timestamp": 6.562590299975563 + }, + { + "x": 1.7553712772903527, + "y": 3.7954492682162915, + "heading": -0.7268838015342108, + "angularVelocity": -0.41760848836064807, + "velocityX": -0.9011262050810134, + "velocityY": 0.7407051063834291, + "timestamp": 6.629698114610971 + }, + { + "x": 1.710023362676749, + "y": 3.8327239369169632, + "heading": -0.7483003159780411, + "angularVelocity": -0.3191359241868454, + "velocityX": -0.675747151952063, + "velocityY": 0.5554445321037842, + "timestamp": 6.696805929246379 + }, + { + "x": 1.679795389171883, + "y": 3.8575703362206024, + "heading": -0.7628115166816798, + "angularVelocity": -0.21623712204721673, + "velocityX": -0.4504389491610239, + "velocityY": 0.37024599055457114, + "timestamp": 6.763913743881787 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.10966573621700788, + "velocityX": -0.22519234880908606, + "velocityY": 0.1851003730766039, + "timestamp": 6.831021558517195 + }, + { + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": 8.092112436868569e-26, + "velocityX": -3.5782911869768004e-26, + "velocityY": 1.7219402564933353e-26, + "timestamp": 6.898129373152603 + }, + { + "x": 1.659892350481847, + "y": 3.8800826841683307, + "heading": -0.726477756364864, + "angularVelocity": 0.7466472297695131, + "velocityX": -0.08186840257721437, + "velocityY": 0.1724334716376635, + "timestamp": 6.956648559285806 + }, + { + "x": 1.6562452284597964, + "y": 3.902216695654719, + "heading": -0.6460681483712878, + "angularVelocity": 1.3740725616133136, + "velocityX": -0.06232352606116078, + "velocityY": 0.3782351216574767, + "timestamp": 7.015167745419008 + }, + { + "x": 1.655331933364656, + "y": 3.9350953770599486, + "heading": -0.5183992946484082, + "angularVelocity": 2.1816580536898917, + "velocityX": -0.015606763447832819, + "velocityY": 0.5618444749110212, + "timestamp": 7.073686931552211 + }, + { + "x": 1.6623538228794565, + "y": 3.9788519154462327, + "heading": -0.3621107234214173, + "angularVelocity": 2.67072359603982, + "velocityX": 0.11999294554126883, + "velocityY": 0.7477297836419755, + "timestamp": 7.1322061176854135 + }, + { + "x": 1.6832034861301444, + "y": 4.019420949275189, + "heading": -0.22540243054151132, + "angularVelocity": 2.336127720039156, + "velocityX": 0.35628764903239357, + "velocityY": 0.6932603904745416, + "timestamp": 7.190725303818616 + }, + { + "x": 1.7146647196939016, + "y": 4.052413453417189, + "heading": -0.11878753895047192, + "angularVelocity": 1.8218792610744803, + "velocityX": 0.537622541300291, + "velocityY": 0.5637895248047576, + "timestamp": 7.249244489951819 + }, + { + "x": 1.7541026360793814, + "y": 4.074977117129141, + "heading": -0.04081520050411857, + "angularVelocity": 1.3324234938755106, + "velocityX": 0.6739313888561295, + "velocityY": 0.3855771961795887, + "timestamp": 7.307763676085021 }, { - "x": 5.541094252914905, - "y": 4.236435293416023, - "heading": 7.779103781629071e-17, - "angularVelocity": -9.500999479304098e-17, - "velocityX": -3.603431085871871, - "velocityY": 1.1482444026273093, - "timestamp": 6.905578965454807 + "x": 1.7990903606675779, + "y": 4.0866639859913105, + "heading": 4.0044035166870527e-26, + "angularVelocity": 0.6974669881979226, + "velocityX": 0.7687688014969033, + "velocityY": 0.1997100375861074, + "timestamp": 7.366282862218224 }, { - "x": 5.417926513725, - "y": 4.281179467366055, - "heading": 7.404581584331534e-17, - "angularVelocity": -1.0808843589526725e-16, - "velocityX": -3.5546646842995244, - "velocityY": 1.291332746825043, - "timestamp": 6.940228573822629 + "x": 1.848745584487915, + "y": 4.087520122528076, + "heading": 1.8053103807695408e-26, + "angularVelocity": -4.864634969888421e-27, + "velocityX": 0.848528954372517, + "velocityY": 0.014630014416416372, + "timestamp": 7.4248020483514265 }, { - "x": 5.2966452974499845, - "y": 4.330810104553132, - "heading": 7.048745543918544e-17, - "angularVelocity": -1.0269554467404476e-16, - "velocityX": -3.5002189631570437, - "velocityY": 1.4323578108076789, - "timestamp": 6.974878182190451 + "x": 1.938259872491009, + "y": 4.089063493932448, + "heading": 1.6974407662610403e-26, + "angularVelocity": -2.7257088903490675e-29, + "velocityX": 1.1795648681505524, + "velocityY": 0.020337610092400867, + "timestamp": 7.500689598430333 }, { - "x": 5.177444375077077, - "y": 4.385247904729644, - "heading": 6.777735008198806e-17, - "angularVelocity": -7.821460284920669e-17, - "velocityX": -3.440180942524166, - "velocityY": 1.5710942414883695, - "timestamp": 7.0095277905582725 + "x": 2.052895663172084, + "y": 4.09104000075836, + "heading": 1.589827161160837e-26, + "angularVelocity": 6.478274543003658e-30, + "velocityX": 1.510600758120114, + "velocityY": 0.02604520535788869, + "timestamp": 7.57657714850924 }, { - "x": 5.060514192105533, - "y": 4.444405885109906, - "heading": 6.539767218111103e-17, - "angularVelocity": -6.867834914008308e-17, - "velocityX": -3.3746465971642725, - "velocityY": 1.7073203180905416, - "timestamp": 7.044177398926094 + "x": 2.1926529528780687, + "y": 4.093449642942828, + "heading": 1.4819575466507505e-26, + "angularVelocity": -2.7257089112437415e-29, + "velocityX": 1.8416365999517403, + "velocityY": 0.03175279979340049, + "timestamp": 7.652464698588147 }, { - "x": 4.946041562758086, - "y": 4.5081895174709326, - "heading": 6.339841419086817e-17, - "angularVelocity": -5.769929544659045e-17, - "velocityX": -3.3037207270069695, - "velocityY": 1.8408182766146604, - "timestamp": 7.078827007293916 + "x": 2.3575317303228487, + "y": 4.096292420291259, + "heading": 1.3740875703299928e-26, + "angularVelocity": -2.7304766323100225e-29, + "velocityX": 2.1726722930617965, + "velocityY": 0.03746039166471138, + "timestamp": 7.728352248667053 }, { - "x": 4.834209367445105, - "y": 4.5764968740602265, - "heading": 6.206624882969642e-17, - "angularVelocity": -3.844676532404254e-17, - "velocityX": -3.2275168632738827, - "velocityY": 1.9713745640117823, - "timestamp": 7.113476615661738 + "x": 2.497289491602916, + "y": 4.0987020706064285, + "heading": 1.2662183176306477e-26, + "angularVelocity": -2.720941189252962e-29, + "velocityX": 1.8416428140683072, + "velocityY": 0.031752906934841556, + "timestamp": 7.80423979874596 }, { - "x": 4.725196244706173, - "y": 4.649218769978707, - "heading": 6.152068396615488e-17, - "angularVelocity": -1.574519567617046e-17, - "velocityX": -3.1461574278620072, - "velocityY": 2.098779736454826, - "timestamp": 7.14812622402956 + "x": 2.611925763329844, + "y": 4.10067858572635, + "heading": 1.1583487031205877e-26, + "angularVelocity": -2.725708910890731e-29, + "velocityX": 1.510607097049927, + "velocityY": 0.02604531465131241, + "timestamp": 7.880127348824867 }, { - "x": 4.619176114911752, - "y": 4.726238684753922, - "heading": 5.2556937935057745e-17, - "angularVelocity": -2.58696893087623e-16, - "velocityX": -3.0597785888073585, - "velocityY": 2.2228220866918083, - "timestamp": 7.182775832397382 + "x": 2.7014405355788997, + "y": 4.1022219654799095, + "heading": 1.0506887852514804e-26, + "angularVelocity": 3.7545940988338324e-31, + "velocityX": 1.1795712492494417, + "velocityY": 0.02033772011288781, + "timestamp": 7.956014898903773 }, { - "x": 4.51016287630692, - "y": 4.798960406862416, - "heading": 6.64273126982982e-17, - "angularVelocity": 4.003039403991449e-16, - "velocityX": -3.1461607717930216, - "velocityY": 2.0987747202369076, - "timestamp": 7.217425440765203 + "x": 2.7658338049487097, + "y": 4.103332209808455, + "heading": 9.428654835122248e-27, + "angularVelocity": -2.1154273725241504e-29, + "velocityX": 0.8485353566277292, + "velocityY": 0.014630124801672608, + "timestamp": 8.031902448982681 }, { - "x": 4.398330563422939, - "y": 4.8672675708300295, - "heading": 7.26949465762174e-17, - "angularVelocity": 1.8088613906315835e-16, - "velocityX": -3.2275202564146483, - "velocityY": 1.9713690048817056, - "timestamp": 7.252075049133025 + "x": 2.8051055697205705, + "y": 4.10400931868236, + "heading": 8.349263998410887e-27, + "angularVelocity": -3.641131282577579e-29, + "velocityX": 0.5174994413579915, + "velocityY": 0.008922529099968745, + "timestamp": 8.107789999061588 }, { - "x": 4.283857826423259, - "y": 4.9310509990316564, - "heading": 7.854869048227426e-17, - "angularVelocity": 1.6894112751752813e-16, - "velocityX": -3.303723833888627, - "velocityY": 1.840812384501881, - "timestamp": 7.286724657500847 + "x": 2.819255828857422, + "y": 4.10425329208374, + "heading": 5.817602179940264e-27, + "angularVelocity": -1.9173555419457397e-26, + "velocityX": 0.18646351242249948, + "velocityY": 0.0032149331626451292, + "timestamp": 8.183677549140494 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 7.967166003614947e-17, - "angularVelocity": 3.240930002777194e-17, - "velocityX": -3.358155156874672, - "velocityY": 1.6997824682762939, - "timestamp": 7.321374265868669 + "x": 2.810470934258171, + "y": 4.100290875532643, + "heading": -0.013665725456507695, + "angularVelocity": -0.18841665432335325, + "velocityX": -0.12112203294600554, + "velocityY": -0.054631952907971924, + "timestamp": 8.256206835394856 }, { - "x": 3.941212613699701, - "y": 5.086809869285441, - "heading": 7.443579239948039e-17, - "angularVelocity": -7.609481799868211e-17, - "velocityX": -3.288705435486243, - "velocityY": 1.4077326545547513, - "timestamp": 7.3901814169311395 + "x": 2.7793765513907402, + "y": 4.0921324527065925, + "heading": -0.04098102601248858, + "angularVelocity": -0.37661063505003184, + "velocityX": -0.4287148608960741, + "velocityY": -0.11248453207493378, + "timestamp": 8.328736121649218 }, { - "x": 3.7303194477377524, - "y": 5.1698988992560615, - "heading": 6.400702034661796e-17, - "angularVelocity": -1.515652354772208e-16, - "velocityX": -3.0649890702563414, - "velocityY": 1.2075638750859479, - "timestamp": 7.45898856799361 + "x": 2.7259718598142455, + "y": 4.079777215271425, + "heading": -0.08192047187969453, + "angularVelocity": -0.5644540017067066, + "velocityX": -0.7363190007027361, + "velocityY": -0.17034825617665642, + "timestamp": 8.40126540790358 }, { - "x": 3.5371029822412163, - "y": 5.24230220948227, - "heading": 4.92329505445724e-17, - "angularVelocity": -2.1471706900158835e-16, - "velocityX": -2.8080869867888283, - "velocityY": 1.0522643229404085, - "timestamp": 7.527795719056081 + "x": 2.650255757673548, + "y": 4.0632237114983365, + "heading": -0.13644940291380203, + "angularVelocity": -0.7518194904451844, + "velocityX": -1.0439383323745826, + "velocityY": -0.22823199603860284, + "timestamp": 8.473794694157942 }, { - "x": 3.362390045884083, - "y": 5.305525654232102, - "heading": 3.1750019865336384e-17, - "angularVelocity": -2.5408595487118374e-16, - "velocityX": -2.5391682936924758, - "velocityY": 0.9188499127428014, - "timestamp": 7.596602870118551 + "x": 2.5522269795422527, + "y": 4.04246951947779, + "heading": -0.20452753616691674, + "angularVelocity": -0.9386295766700757, + "velocityX": -1.3515751111558711, + "velocityY": -0.28614912806063464, + "timestamp": 8.546323980412303 }, { - "x": 3.2065873745181794, - "y": 5.36043931571324, - "heading": 2.169929247164939e-17, - "angularVelocity": -1.4607097138783564e-16, - "velocityX": -2.2643383566985644, - "velocityY": 0.7980807319181392, - "timestamp": 7.665410021181022 + "x": 2.4318844030868068, + "y": 4.017510913730928, + "heading": -0.28611931872413315, + "angularVelocity": -1.1249494758720198, + "velocityX": -1.6592273641491695, + "velocityY": -0.344117625249086, + "timestamp": 8.618853266666665 + }, + { + "x": 2.2892276108991876, + "y": 3.988342694775403, + "heading": -0.38121408025371734, + "angularVelocity": -1.3111222575124248, + "velocityX": -1.9668853721697708, + "velocityY": -0.40215781047715543, + "timestamp": 8.691382552921027 }, { - "x": 3.069932459879016, - "y": 5.407606100670878, - "heading": 1.5380181020622108e-17, - "angularVelocity": -9.183800453716929e-17, - "velocityX": -1.9860568636985672, - "velocityY": 0.6854924848554687, - "timestamp": 7.7342171722434925 + "x": 2.133082954891742, + "y": 3.958763429178337, + "heading": -0.47806319392600216, + "angularVelocity": -1.335310447322373, + "velocityX": -2.1528497531306416, + "velocityY": -0.4078251300216943, + "timestamp": 8.763911839175389 }, { - "x": 2.952579630826552, - "y": 5.447418960174395, - "heading": 8.435159362125091e-18, - "angularVelocity": -1.0093459112453562e-16, - "velocityX": -1.7055324517929655, - "velocityY": 0.5786151423035916, - "timestamp": 7.803024323305963 + "x": 1.999249779098319, + "y": 3.933404232877465, + "heading": -0.561296583651025, + "angularVelocity": -1.1475831905076381, + "velocityX": -1.8452294611595452, + "velocityY": -0.3496407811313142, + "timestamp": 8.83644112542975 }, { - "x": 2.8546367166241713, - "y": 5.480167404246393, - "heading": 5.0453569918778545e-18, - "angularVelocity": -4.926526267025923e-17, - "velocityX": -1.423440916968907, - "velocityY": 0.4759453569334053, - "timestamp": 7.871831474368434 + "x": 1.8877256766061599, + "y": 3.91226852016899, + "heading": -0.6308164080928251, + "angularVelocity": -0.9585069429470503, + "velocityX": -1.5376423545799296, + "velocityY": -0.29140935751595565, + "timestamp": 8.908970411684113 }, { - "x": 2.7761830906702163, - "y": 5.506073449022424, - "heading": 3.1520224912430142e-18, - "angularVelocity": -2.7516536752894226e-17, - "velocityX": -1.1401958189305985, - "velocityY": 0.3765022149007732, - "timestamp": 7.940638625430904 + "x": 1.7985083626535399, + "y": 3.8953584872647626, + "heading": -0.6865270511993388, + "angularVelocity": -0.7681123858179831, + "velocityX": -1.2300867492302674, + "velocityY": -0.2331476535550589, + "timestamp": 8.981499697938474 }, { - "x": 2.7172795158320118, - "y": 5.5253126875621055, - "heading": 4.0055040762053705e-18, - "angularVelocity": 1.2403966322481763e-17, - "velocityX": -0.8560676314693773, - "velocityY": 0.2796110323215353, - "timestamp": 8.009445776493374 + "x": 1.7315960620068493, + "y": 3.8826753796533127, + "heading": -0.7283506616380105, + "angularVelocity": -0.57664445079461, + "velocityX": -0.9225556199743571, + "velocityY": -0.17486877737869985, + "timestamp": 9.054028984192836 }, { - "x": 2.677973947928258, - "y": 5.538027438078825, - "heading": 2.2657574428082367e-18, - "angularVelocity": -2.5284386970987473e-17, - "velocityX": -0.571242484201505, - "velocityY": 0.18478821343985707, - "timestamp": 8.078252927555845 + "x": 1.68698774113937, + "y": 3.874219809689345, + "heading": -0.7562373749577054, + "angularVelocity": -0.38448900795597707, + "velocityX": -0.6150387405032061, + "velocityY": -0.11658145834103056, + "timestamp": 9.126558270447198 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -8.520572182632915e-28, - "angularVelocity": -3.292909834387365e-17, - "velocityX": -0.2858537153870097, - "velocityY": 0.09167545987855215, - "timestamp": 8.147060078618315 + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": -0.1921095648592989, + "velocityX": -0.3075243052917468, + "velocityY": -0.05829082515090241, + "timestamp": 9.19908755670156 }, { - "x": 2.6583051681518555, - "y": 5.54433536529541, - "heading": -3.9086161010585418e-28, - "angularVelocity": 3.4350134480026537e-28, - "velocityX": 1.6920853023952865e-26, - "velocityY": 2.9127538584633355e-26, - "timestamp": 8.215867229680786 + "x": 1.664683222770691, + "y": 3.869992017745972, + "heading": -0.7701709445795863, + "angularVelocity": 4.278722998474624e-26, + "velocityX": 4.546149419559626e-26, + "velocityY": -6.368263045423934e-27, + "timestamp": 9.271616842955922 }, { - "x": 2.675073555498551, - "y": 5.538716972612474, - "heading": 1.6968135653440987e-18, - "angularVelocity": 2.665146289407491e-17, - "velocityX": 0.2633772278822265, - "velocityY": -0.08824680986851689, - "timestamp": 8.279534037481625 + "x": 1.6770242080176545, + "y": 3.8595866692257377, + "heading": -0.7649877966744867, + "angularVelocity": 0.08507499669287022, + "velocityX": 0.20256209128031585, + "velocityY": -0.17079099557937222, + "timestamp": 9.332541299101496 }, { - "x": 2.7085920648883794, - "y": 5.5274259655637525, - "heading": 3.725095749658628e-18, - "angularVelocity": 3.185776473374715e-17, - "velocityX": 0.5264675668156642, - "velocityY": -0.17734526731797434, - "timestamp": 8.343200845282464 + "x": 1.7017080065350787, + "y": 3.838774596868545, + "heading": -0.7547380668498572, + "angularVelocity": 0.16823670612895278, + "velocityX": 0.40515418731755265, + "velocityY": -0.3416045652908843, + "timestamp": 9.393465755247071 }, { - "x": 2.7588390752001515, - "y": 5.510398898078488, - "heading": 7.631145981641487e-18, - "angularVelocity": 6.13514383217542e-17, - "velocityX": 0.7892183077397765, - "velocityY": -0.267440257701116, - "timestamp": 8.406867653083303 + "x": 1.738736696509759, + "y": 3.8075542186994835, + "heading": -0.739558172921761, + "angularVelocity": 0.2491592849319001, + "velocityX": 0.6077803942345016, + "velocityY": -0.5124441011744508, + "timestamp": 9.454390211392646 }, { - "x": 2.8257885977384563, - "y": 5.48756053072635, - "heading": 1.178722114456003e-17, - "angularVelocity": 6.527852277238055e-17, - "velocityX": 1.0515608501644154, - "velocityY": -0.3587170166216, - "timestamp": 8.470534460884142 + "x": 1.7881126694173817, + "y": 3.76592371429674, + "heading": -0.7196097205107205, + "angularVelocity": 0.32742930627685823, + "velocityX": 0.8104458542829184, + "velocityY": -0.6833135170426513, + "timestamp": 9.51531466753822 }, { - "x": 2.909408815467416, - "y": 5.4588202121142055, - "heading": 1.707663826690775e-17, - "angularVelocity": 8.307966592022717e-17, - "velocityX": 1.3134036496778398, - "velocityY": -0.45141761625695104, - "timestamp": 8.534201268684981 + "x": 1.849838711755514, + "y": 3.713880986159023, + "heading": -0.6950869960421026, + "angularVelocity": 0.4025103549553654, + "velocityX": 1.0131570512610228, + "velocityY": -0.8542173608142635, + "timestamp": 9.576239123683795 }, { - "x": 3.0096598952002043, - "y": 5.424066614979104, - "heading": 2.1547525099461292e-17, - "angularVelocity": 7.022319772168498e-17, - "velocityX": 1.5746207984290834, - "velocityY": -0.5458668077692407, - "timestamp": 8.59786807648582 + "x": 1.923918112020319, + "y": 3.6514236137874088, + "heading": -0.6662278740694468, + "angularVelocity": 0.4736869854644038, + "velocityX": 1.2159222248582278, + "velocityY": -1.025160934098061, + "timestamp": 9.63716357982937 }, { - "x": 3.1264905609256615, - "y": 5.383159775106569, - "heading": 2.7044139747893618e-17, - "angularVelocity": 8.633407011044114e-17, - "velocityX": 1.835032566591432, - "velocityY": -0.6425143852114948, - "timestamp": 8.66153488428666 + "x": 2.010354801057128, + "y": 3.578548802416206, + "heading": -0.6333303833823517, + "angularVelocity": 0.5399718400192002, + "velocityX": 1.4187519184459274, + "velocityY": -1.1961503800226454, + "timestamp": 9.698088035974944 }, { - "x": 3.259832418594633, - "y": 5.335918459700519, - "heading": 3.2480422722951275e-17, - "angularVelocity": 8.538645430467616e-17, - "velocityX": 2.0943700850541704, - "velocityY": -0.7420085447636868, - "timestamp": 8.725201692087499 + "x": 2.109153532122565, + "y": 3.4952533361865883, + "heading": -0.5967791996957547, + "angularVelocity": 0.5999427159310254, + "velocityX": 1.6216596308937807, + "velocityY": -1.367192610313814, + "timestamp": 9.759012492120519 }, { - "x": 3.409589859132952, - "y": 5.282098873954419, - "heading": 3.770995869084488e-17, - "angularVelocity": 8.213912631218793e-17, - "velocityX": 2.3522058936390553, - "velocityY": -0.84533193362635, - "timestamp": 8.788868499888338 + "x": 2.2203200891736463, + "y": 3.4015335717559703, + "heading": -0.5570908397473109, + "angularVelocity": 0.6514356049992648, + "velocityX": 1.824662279880775, + "velocityY": -1.5382946415915537, + "timestamp": 9.819936948266093 }, { - "x": 3.575620300185956, - "y": 5.221355820749434, - "heading": 4.453751126086424e-17, - "angularVelocity": 1.0723880787820308e-16, - "velocityX": 2.6078021937643934, - "velocityY": -0.9540772547447327, - "timestamp": 8.852535307689177 + "x": 2.3438614224783176, + "y": 3.2973856025788515, + "heading": -0.5149975235515339, + "angularVelocity": 0.690910003286656, + "velocityX": 2.0277790089660783, + "velocityY": -1.7094607940079831, + "timestamp": 9.880861404411668 }, { - "x": 3.7576900290728465, - "y": 5.153163867625262, - "heading": 5.330339940674851e-17, - "angularVelocity": 1.3768380178976122e-16, - "velocityX": 2.8597276222240717, - "velocityY": -1.0710754234370974, - "timestamp": 8.916202115490016 + "x": 2.4797851741645616, + "y": 3.182806114592359, + "heading": -0.47162192283324345, + "angularVelocity": 0.7119571262917361, + "velocityX": 2.231021174181074, + "velocityY": -1.8806813426895879, + "timestamp": 9.941785860557243 }, { - "x": 3.955353840328095, - "y": 5.076631092230983, - "heading": 6.003878998986586e-17, - "angularVelocity": 1.0579124061222185e-16, - "velocityX": 3.10466031018196, - "velocityY": -1.2020828126594152, - "timestamp": 8.979868923290855 + "x": 2.628095553772068, + "y": 3.057796514272803, + "heading": -0.4289105902248852, + "angularVelocity": 0.7010539824319861, + "velocityX": 2.4343324338116106, + "velocityY": -2.0518788057927626, + "timestamp": 10.002710316702817 }, { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 6.251629909739218e-17, - "angularVelocity": 3.89136693523207e-17, - "velocityX": 3.3321165674695394, - "velocityY": -1.361514725761388, - "timestamp": 9.043535731091694 + "x": 2.788761933089725, + "y": 2.9223901324293977, + "heading": -0.3910891505574595, + "angularVelocity": 0.6207924052215403, + "velocityX": 2.6371409690347543, + "velocityY": -2.222529184665362, + "timestamp": 10.063634772848392 }, { - "x": 4.285701078795114, - "y": 4.9383884935900895, - "heading": 6.191867349467079e-17, - "angularVelocity": -1.7257822842751934e-17, - "velocityX": 3.4133567862477086, - "velocityY": -1.4888942181949925, - "timestamp": 9.078164989267158 + "x": 2.9612506555206113, + "y": 2.7772643724590593, + "heading": -0.37520163462976347, + "angularVelocity": 0.2607740295577497, + "velocityX": 2.8311901877094443, + "velocityY": -2.382060820101041, + "timestamp": 10.124559228993967 }, { - "x": 4.403715151512854, - "y": 4.881600775413148, - "heading": 5.87936361182216e-17, - "angularVelocity": -9.024268901925555e-17, - "velocityX": 3.4079295640631644, - "velocityY": -1.6398768315856658, - "timestamp": 9.112794247442622 + "x": 3.1373758975337274, + "y": 2.6287035614867915, + "heading": -0.37520161826362025, + "angularVelocity": 2.686301076874653e-7, + "velocityX": 2.8908791831030336, + "velocityY": -2.4384429565902384, + "timestamp": 10.185483685139541 }, { - "x": 4.5193670084290005, - "y": 4.8201449553533005, - "heading": 6.393419644700586e-17, - "angularVelocity": 1.484455804018553e-16, - "velocityX": 3.3397151140271815, - "velocityY": -1.7746790805755832, - "timestamp": 9.147423505618086 + "x": 3.313501125191111, + "y": 2.480142733495068, + "heading": -0.3752016018976461, + "angularVelocity": 2.686273330621961e-7, + "velocityX": 2.8908789474713403, + "velocityY": -2.4384432359436565, + "timestamp": 10.246408141285116 }, { - "x": 4.63247211701513, - "y": 4.7541191698650405, - "heading": 6.969863736825022e-17, - "angularVelocity": 1.664615768552329e-16, - "velocityX": 3.2661718600217573, - "velocityY": -1.9066474122463712, - "timestamp": 9.18205276379355 + "x": 3.4896263528475178, + "y": 2.3315819055021865, + "heading": -0.37520158553167193, + "angularVelocity": 2.686273331449891e-7, + "velocityX": 2.890878947455303, + "velocityY": -2.4384432359626693, + "timestamp": 10.30733259743069 }, { - "x": 4.742850086360494, - "y": 4.6836289301368605, - "heading": 7.573971746769133e-17, - "angularVelocity": 1.7445017357345818e-16, - "velocityX": 3.187419400845542, - "velocityY": -2.035568864080495, - "timestamp": 9.216682021969014 + "x": 3.665751580503926, + "y": 2.183021077509307, + "heading": -0.3752015691656978, + "angularVelocity": 2.6862733313245716e-7, + "velocityX": 2.8908789474553287, + "velocityY": -2.438443235962638, + "timestamp": 10.368257053576265 }, { - "x": 4.850325224446504, - "y": 4.608787332408603, - "heading": 7.95254691851129e-17, - "angularVelocity": 1.0932234523778094e-16, - "velocityX": 3.1035934278880775, - "velocityY": -2.161224400160131, - "timestamp": 9.251311280144478 + "x": 3.841876808185214, + "y": 2.0344602495459236, + "heading": -0.37520155279972356, + "angularVelocity": 2.6862733360962026e-7, + "velocityX": 2.890878947863701, + "velocityY": -2.4384432354784953, + "timestamp": 10.42918150972184 }, { - "x": 4.959315852672914, - "y": 4.536170440415771, - "heading": 8.132510178394684e-17, - "angularVelocity": 5.1968557625797656e-17, - "velocityX": 3.1473567142028136, - "velocityY": -2.0969808716342646, - "timestamp": 9.285940538319942 + "x": 4.018002401412412, + "y": 1.8858998549539654, + "heading": -0.3752015364337498, + "angularVelocity": 2.686273261605019e-7, + "velocityX": 2.8908849478501355, + "velocityY": -2.4384361222196733, + "timestamp": 10.490105965867414 }, { - "x": 5.071119482307337, - "y": 4.467964000951313, - "heading": 8.196310550599571e-17, - "angularVelocity": 1.842383452283251e-17, - "velocityX": 3.2285886422377668, - "velocityY": -1.969618844240326, - "timestamp": 9.320569796495406 + "x": 4.198462016558864, + "y": 1.7426352651038108, + "heading": -0.375201520037625, + "angularVelocity": 2.691222177389301e-7, + "velocityX": 2.962022586057329, + "velocityY": -2.351512002139716, + "timestamp": 10.551030422012989 }, { - "x": 5.185557951100297, - "y": 4.404277214638869, - "heading": 8.211438156325687e-17, - "angularVelocity": 4.3684463753045605e-18, - "velocityX": 3.3046757228557744, - "velocityY": -1.8391033960284853, - "timestamp": 9.35519905467087 + "x": 4.3885379093915455, + "y": 1.6123986719437928, + "heading": -0.3752015033732071, + "angularVelocity": 2.7352591969451763e-7, + "velocityX": 3.119861954590255, + "velocityY": -2.1376734631627445, + "timestamp": 10.611954878158564 }, { - "x": 5.302448704050349, - "y": 4.345211813287043, - "heading": 8.111338797415481e-17, - "angularVelocity": -2.8906007282049283e-17, - "velocityX": 3.37549110517396, - "velocityY": -1.7056502063239702, - "timestamp": 9.389828312846333 + "x": 4.587291846391845, + "y": 1.49583415162556, + "heading": -0.37520148607296444, + "angularVelocity": 2.839622014925957e-7, + "velocityX": 3.2623013741048372, + "velocityY": -1.9132632064816424, + "timestamp": 10.672879334304138 }, { - "x": 5.421605231445948, - "y": 4.290862097784136, - "heading": 7.868835126918448e-17, - "angularVelocity": -7.002854905419881e-17, - "velocityX": 3.4409205877827787, - "velocityY": -1.5694738601537552, - "timestamp": 9.424457571021797 + "x": 4.7937421798706055, + "y": 1.3935176134109497, + "heading": -0.3752014675107609, + "angularVelocity": 3.0467573669290407e-7, + "velocityX": 3.3886282543985438, + "velocityY": -1.679400107735582, + "timestamp": 10.733803790449713 }, { - "x": 5.542837393133187, - "y": 4.2413148237692955, - "heading": 7.613874923845377e-17, - "angularVelocity": -7.362566122075624e-17, - "velocityX": 3.5008593332540854, - "velocityY": -1.4307922440552452, - "timestamp": 9.459086829197261 + "x": 5.064781872639773, + "y": 1.2879640439263618, + "heading": -0.3752014510522667, + "angularVelocity": 2.1399857496737658e-7, + "velocityX": 3.52414428888234, + "velocityY": -1.3724410814878705, + "timestamp": 10.810713153560702 }, { - "x": 5.665951728820801, - "y": 4.196649074554443, - "heading": 7.348031788343025e-17, - "angularVelocity": -7.676835991047679e-17, - "velocityX": 3.5552114649352746, - "velocityY": -1.289826914239228, - "timestamp": 9.493716087372725 + "x": 5.344110469349436, + "y": 1.2068492677876004, + "heading": -0.3752014356488163, + "angularVelocity": 2.0028056132299027e-7, + "velocityX": 3.631919254181875, + "velocityY": -1.0546801177082226, + "timestamp": 10.88762251667169 }, { - "x": 5.934272394454851, - "y": 4.1243379326499765, - "heading": 6.301578755269389e-17, - "angularVelocity": -1.4241558943688294e-16, - "velocityX": 3.6516732759640362, - "velocityY": -0.9841085621303537, - "timestamp": 9.567194913476962 + "x": 5.629529124295685, + "y": 1.1508112126364383, + "heading": -0.37520142072574625, + "angularVelocity": 1.940345031855091e-7, + "velocityX": 3.711104128301771, + "velocityY": -0.7286246158389513, + "timestamp": 10.96453187978268 }, { - "x": 6.207753015610448, - "y": 4.075010136678792, - "heading": 7.880077877117389e-17, - "angularVelocity": 2.148236717340586e-16, - "velocityX": 3.721896982508106, - "velocityY": -0.6713198697704936, - "timestamp": 9.6406737395812 + "x": 5.917283115305387, + "y": 1.1083646233041369, + "heading": -0.3752014058811779, + "angularVelocity": 1.9301379957946635e-7, + "velocityX": 3.7414689105466086, + "velocityY": -0.5519040545303511, + "timestamp": 11.041441242893669 + }, + { + "x": 6.205037197250251, + "y": 1.0659186504442004, + "heading": -0.37520139103661015, + "angularVelocity": 1.9301379056768152e-7, + "velocityX": 3.7414700929145033, + "velocityY": -0.5518960389605939, + "timestamp": 11.118350606004658 }, { - "x": 6.484428622538185, - "y": 4.049019979746833, - "heading": 4.7380289374867153e-17, - "angularVelocity": -4.2761283844936273e-16, - "velocityX": 3.7653787029102097, - "velocityY": -0.3537094740066765, - "timestamp": 9.714152565685437 + "x": 6.492791277445097, + "y": 1.0234726657206588, + "heading": -0.37520137619194305, + "angularVelocity": 1.9301508362380675e-7, + "velocityX": 3.741470070160208, + "velocityY": -0.5518961932149626, + "timestamp": 11.195259969115646 }, { - "x": 6.743277361498475, - "y": 4.038426387270388, - "heading": 3.80588957590445e-17, - "angularVelocity": -1.2685822719245393e-16, - "velocityX": 3.5227663897772135, - "velocityY": -0.1441720429966888, - "timestamp": 9.787631391789674 + "x": 6.772643212230185, + "y": 0.9818724107444464, + "heading": -0.34826678356511775, + "angularVelocity": 0.35021213981392485, + "velocityX": 3.638723862284922, + "velocityY": -0.5408997460579521, + "timestamp": 11.272169332226635 }, { - "x": 6.978672820957151, - "y": 4.030025293070568, - "heading": 3.301536102704029e-17, - "angularVelocity": -6.863929378502764e-17, - "velocityX": 3.20358220101046, - "velocityY": -0.11433353858840364, - "timestamp": 9.861110217893911 + "x": 7.027508446069354, + "y": 0.9440808910715871, + "heading": -0.30010699502671095, + "angularVelocity": 0.6261888876768776, + "velocityX": 3.313838829628223, + "velocityY": -0.49137735828500057, + "timestamp": 11.349078695337624 }, { - "x": 7.190552691043286, - "y": 4.022995753598098, - "heading": 2.761165208046759e-17, - "angularVelocity": -7.354103532969467e-17, - "velocityX": 2.8835500146064224, - "velocityY": -0.09566755275184706, - "timestamp": 9.934589043998148 + "x": 7.256864076564165, + "y": 0.9101013911553985, + "heading": -0.24860599276707973, + "angularVelocity": 0.66963241114492, + "velocityX": 2.9821548536792237, + "velocityY": -0.44181226500540305, + "timestamp": 11.425988058448613 }, { - "x": 7.378901756507518, - "y": 4.0170458446186466, - "heading": 2.2049007606542194e-17, - "angularVelocity": -7.570404657768667e-17, - "velocityX": 2.563310758354254, - "velocityY": -0.08097446972017466, - "timestamp": 10.008067870102385 + "x": 7.460696487188184, + "y": 0.8799184472229736, + "heading": -0.19849831332672846, + "angularVelocity": 0.6515159847057984, + "velocityX": 2.6502938313228213, + "velocityY": -0.39244823661935213, + "timestamp": 11.502897421559602 }, { - "x": 7.543713628095823, - "y": 4.012025993134882, - "heading": 1.7040082343642412e-17, - "angularVelocity": -6.816828096488101e-17, - "velocityX": 2.2429845484262843, - "velocityY": -0.06831697986905093, - "timestamp": 10.081546696206622 + "x": 7.639017939225139, + "y": 0.8535218749551334, + "heading": -0.15201987225869848, + "angularVelocity": 0.6043274731186726, + "velocityX": 2.3185922340771317, + "velocityY": -0.3432166279903691, + "timestamp": 11.579806784670591 }, { - "x": 7.684984887163916, - "y": 4.007845275924109, - "heading": 1.249754397163312e-17, - "angularVelocity": -6.182105257790516e-17, - "velocityX": 1.922611812928072, - "velocityY": -0.05689689713935573, - "timestamp": 10.15502552231086 + "x": 7.7918411996459325, + "y": 0.8309051352911735, + "heading": -0.11048043087448678, + "angularVelocity": 0.5401090283931487, + "velocityX": 1.987056637047598, + "velocityY": -0.29407004230839406, + "timestamp": 11.65671614778158 }, { - "x": 7.802713433535344, - "y": 4.004442580511056, - "heading": 8.52630628004598e-18, - "angularVelocity": -5.4046014370140334e-17, - "velocityX": 1.6022104953666365, - "velocityY": -0.046308516255087515, - "timestamp": 10.228504348415097 + "x": 7.919176921526017, + "y": 0.8120637954987597, + "heading": -0.0747425922933183, + "angularVelocity": 0.46467474356269306, + "velocityX": 1.655659554693303, + "velocityY": -0.24498109242204588, + "timestamp": 11.733625510892569 }, { - "x": 7.8968978566663255, - "y": 4.001774009946332, - "heading": 5.220002438781615e-18, - "angularVelocity": -4.499668838635962e-17, - "velocityX": 1.2817899812031746, - "velocityY": -0.03631754487937096, - "timestamp": 10.301983174519334 + "x": 8.021033730291848, + "y": 0.7969946793008421, + "heading": -0.045418027645302235, + "angularVelocity": 0.3812873161580816, + "velocityX": 1.3243746228770636, + "velocityY": -0.19593344150011263, + "timestamp": 11.810534874003558 }, { - "x": 7.967537148396796, - "y": 3.9998065067615447, - "heading": 2.6237181268422074e-18, - "angularVelocity": -3.5333775041531244e-17, - "velocityX": 0.9613557466236943, - "velocityY": -0.026776464583087325, - "timestamp": 10.375462000623571 + "x": 8.097418632050843, + "y": 0.7856953915277307, + "heading": -0.022962936429777186, + "angularVelocity": 0.29196823776995817, + "velocityX": 0.9931807866977714, + "velocityY": -0.14691693333626094, + "timestamp": 11.887444237114547 }, { - "x": 8.014630554418627, - "y": 3.998514279441312, - "heading": 9.593365131903801e-19, - "angularVelocity": -2.265117315883944e-17, - "velocityX": 0.6409112463912332, - "velocityY": -0.01758639037590862, - "timestamp": 10.448940826727808 + "x": 8.148337378443449, + "y": 0.7781640366357435, + "heading": -0.007730140883461889, + "angularVelocity": 0.198061652445786, + "velocityX": 0.6620617351768197, + "velocityY": -0.09792507163423873, + "timestamp": 11.964353600225536 }, { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 1.1585643597990433e-28, - "angularVelocity": -1.3055958619386531e-17, - "velocityX": 0.3204587915210466, - "velocityY": -0.008677810201891931, - "timestamp": 10.522419652832046 + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": -6.501598853172508e-27, + "angularVelocity": 0.10050975031878098, + "velocityX": 0.33100479480942824, + "velocityY": -0.04895365601706529, + "timestamp": 12.041262963336525 }, { - "x": 8.038177490234375, - "y": 3.9978766441345215, - "heading": 5.650895976930055e-29, - "angularVelocity": -3.8630399964154185e-29, - "velocityX": 1.5845610208926219e-27, - "velocityY": -6.207718033855077e-27, - "timestamp": 10.595898478936283 + "x": 8.173794746398926, + "y": 0.7743990421295166, + "heading": -3.1768871176909477e-27, + "angularVelocity": 1.9243307208194474e-27, + "velocityX": 5.67400379522632e-27, + "velocityY": -1.0775303089806919e-26, + "timestamp": 12.118172326447514 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 1.2899744510650635, - "y": 5.590954303741455, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 - }, - { - "timestamp": 1.2792598362128638, - "isStopPoint": false, - "x": 2.6651716232299805, - "y": 4.296644687652588, - "heading": -0.7216551150961179, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "timestamp": 2.4932366624325915, - "isStopPoint": true, - "x": 1.8129411935806274, - "y": 5.542999267578125, + "x": 0.43324199318885803, + "y": 4.121134281158447, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 19 }, { - "timestamp": 3.424227005183806, + "timestamp": 1.4277752931645427, "isStopPoint": true, - "x": 2.7583051681518556, - "y": 5.54433536529541, - "heading": 0, + "x": 2.3639590740203857, + "y": 3.0491905212402344, + "heading": -0.8022724119795859, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 - }, - { - "timestamp": 4.224909036593179, - "isStopPoint": false, - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 22 }, { - "timestamp": 4.6758758399070866, + "timestamp": 2.7722119761090878, "isStopPoint": false, - "x": 5.665951728820801, - "y": 4.196649074554443, + "x": 5.5030035972595215, + "y": 1.563418984413147, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 12 + "controlIntervalCount": 17 }, { - "timestamp": 5.107685831479359, + "timestamp": 3.3986072208880596, "isStopPoint": false, - "x": 7.196889877319336, - "y": 3.9978766441345215, - "heading": 0, + "x": 7.626483917236328, + "y": 2.082604169845581, + "heading": 0.507098504392337, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 9 }, { - "timestamp": 5.746338920885271, + "timestamp": 3.9697078859421278, "isStopPoint": false, - "x": 8.1634765625, - "y": 4.073246955871582, - "heading": 0, + "x": 8.306674003601074, + "y": 2.45849871635437, + "heading": 0.507098504392337, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 19 }, { - "timestamp": 6.870929357086985, + "timestamp": 5.220434007267402, "isStopPoint": false, - "x": 5.665951728820801, - "y": 4.196649074554443, + "x": 5.5030035972595215, + "y": 1.563418984413147, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 25 }, { - "timestamp": 7.321374265868669, - "isStopPoint": false, - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": 0, + "timestamp": 6.898129373152603, + "isStopPoint": true, + "x": 1.664683222770691, + "y": 3.8699920177459717, + "heading": -0.7701709445795863, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "timestamp": 8.215867229680786, - "isStopPoint": true, - "x": 2.6583051681518555, - "y": 5.54433536529541, + "timestamp": 7.4248020483514265, + "isStopPoint": false, + "x": 1.848745584487915, + "y": 4.087520122528076, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 10 }, { - "timestamp": 9.043535731091694, + "timestamp": 8.183677549140494, "isStopPoint": false, - "x": 4.16749906539917, - "y": 4.98994779586792, + "x": 2.819255828857422, + "y": 4.10425329208374, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "timestamp": 9.493716087372725, + "timestamp": 9.271616842955922, + "isStopPoint": true, + "x": 1.664683222770691, + "y": 3.8699920177459717, + "heading": -0.7701709445795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "timestamp": 10.733803790449713, "isStopPoint": false, - "x": 5.665951728820801, - "y": 4.196649074554443, + "x": 4.7937421798706055, + "y": 1.3935176134109497, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 15 + "controlIntervalCount": 18 }, { - "timestamp": 10.595898478936283, + "timestamp": 12.118172326447514, "isStopPoint": true, - "x": 8.038177490234375, - "y": 3.9978766441345215, + "x": 8.173794746398926, + "y": 0.7743990421295166, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -24876,21 +24518,35 @@ }, { "scope": [ - 3 + 6 ], "type": "StopPoint" }, { "scope": [ - 2 + 1 ], "type": "StopPoint" }, { "scope": [ - 10 + 9 ], "type": "StopPoint" + }, + { + "scope": [ + 7, + 8 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 7, + 8 + ], + "type": "StraightLine" } ], "usesControlIntervalGuessing": true, @@ -24900,1939 +24556,2010 @@ "eventMarkers": [], "isTrajectoryStale": true }, - "SourceLanePGCSprint": { + "AmpLanePADEF": { "waypoints": [ { - "x": 0.43324199318885803, - "y": 4.121134281158447, + "x": 0.43297290802001953, + "y": 6.9807281494140625, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 6 }, { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 13 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0, + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 17 + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 20 }, { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 9 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, + "x": 8.186561584472656, + "y": 7.469284534454346, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 25 + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 1.664683222770691, - "y": 3.8699920177459717, - "heading": -0.7701709445795863, + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 12 }, { - "x": 1.848745584487915, - "y": 4.087520122528076, + "x": 5.851044178009033, + "y": 6.557126998901367, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 2.819255828857422, - "y": 4.10425329208374, - "heading": 0, + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 9 }, { - "x": 1.664683222770691, - "y": 3.8699920177459717, - "heading": -0.7701709445795863, + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 24 + "controlIntervalCount": 15 }, { - "x": 4.7937421798706055, - "y": 1.3935176134109497, + "x": 5.851044178009033, + "y": 6.557126998901367, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 18 + "controlIntervalCount": 12 }, { - "x": 8.173794746398926, - "y": 0.7743990421295166, - "heading": 0, + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.433241993188858, - "y": 4.121134281158447, - "heading": 7.738396447124189e-29, - "angularVelocity": 1.1248344329069217e-28, - "velocityX": -3.4263332588426173e-28, - "velocityY": 2.738469239483736e-27, - "timestamp": 0 - }, - { - "x": 0.4546918803218127, - "y": 4.109218526998405, - "heading": -0.00893730014505659, - "angularVelocity": -0.11893237232009289, - "velocityX": 0.28544257452644645, - "velocityY": -0.1585678993919355, - "timestamp": 0.07514606806129173 + "controlIntervalCount": 11 }, { - "x": 0.49759166351299144, - "y": 4.085387310900469, - "heading": -0.026812413293685552, - "angularVelocity": -0.23787156946188326, - "velocityX": 0.5708852678251672, - "velocityY": -0.3171319100621368, - "timestamp": 0.15029213612258346 + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 0.5619414537509306, - "y": 4.049641027145361, - "heading": -0.05362171709181762, - "angularVelocity": -0.35676256243061816, - "velocityX": 0.8563294380945293, - "velocityY": -0.47569067387467856, - "timestamp": 0.2254382041838752 + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 0.6477414523823173, - "y": 4.001980177868811, - "heading": -0.08935853103818607, - "angularVelocity": -0.4755646551888817, - "velocityX": 1.1417762877680475, - "velocityY": -0.634242755558099, - "timestamp": 0.30058427224516693 + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 0.7549919358856848, - "y": 3.942405375866793, - "heading": -0.13401442198910787, - "angularVelocity": -0.5942545245946725, - "velocityX": 1.4272268166564648, - "velocityY": -0.7927866825104752, - "timestamp": 0.3757303403064587 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": -2.0223823950436125e-24, + "angularVelocity": -1.0530930101357375e-37, + "velocityX": 9.679119214488622e-24, + "velocityY": 2.8468005585003605e-23, + "timestamp": 0 }, { - "x": 0.8836932393387835, - "y": 3.870917341642709, - "heading": -0.18758057155461047, - "angularVelocity": -0.7128270440152931, - "velocityX": 1.7126818045639527, - "velocityY": -0.9513210214242487, - "timestamp": 0.45087640836775045 + "x": 0.45358539197474673, + "y": 6.976442037280005, + "heading": -4.051643418810716e-24, + "angularVelocity": -1.1805501930230392e-25, + "velocityX": 0.3537634154949626, + "velocityY": -0.0735607445986989, + "timestamp": 0.05826629620784151 }, { - "x": 1.0338457421573335, - "y": 3.7875168933266514, - "heading": -0.2500489994644535, - "angularVelocity": -0.8312933666588067, - "velocityX": 1.998141841514319, - "velocityY": -1.1098444731404118, - "timestamp": 0.5260224764290422 + "x": 0.49483104636042347, + "y": 6.967970532727312, + "heading": -6.080913911985301e-24, + "angularVelocity": -1.1821753877066713e-25, + "velocityX": 0.7078818643036704, + "velocityY": -0.1453928789719994, + "timestamp": 0.11653259241568302 }, { - "x": 1.2054498603433927, - "y": 3.692204931210594, - "heading": -0.3214134495606155, - "angularVelocity": -0.9496764360040053, - "velocityX": 2.2836074143771987, - "velocityY": -1.2683559442966215, - "timestamp": 0.601168544490334 + "x": 0.5567354919848906, + "y": 6.95544199101986, + "heading": -8.110182775739579e-24, + "angularVelocity": -1.1818957371343575e-25, + "velocityX": 1.0624400322898162, + "velocityY": -0.21502210579442033, + "timestamp": 0.17479888862352452 }, { - "x": 1.3985060453585743, - "y": 3.5849824250394744, - "heading": -0.4016698579067676, - "angularVelocity": -1.068005424857256, - "velocityX": 2.569078995027635, - "velocityY": -1.4268545106533708, - "timestamp": 0.6763146125516257 + "x": 0.6393312280569997, + "y": 6.9390255615072824, + "heading": -1.013944960655158e-23, + "angularVelocity": -1.1815468317795835e-25, + "velocityX": 1.4175559705645637, + "velocityY": -0.28174829328466466, + "timestamp": 0.23306518483136604 }, { - "x": 1.5915937570064054, - "y": 3.477819815501255, - "heading": -0.4817685926160567, - "angularVelocity": -1.0659071961550632, - "velocityX": 2.5694985330482245, - "velocityY": -1.426057441233171, - "timestamp": 0.7514606806129175 + "x": 0.7426606798558012, + "y": 6.918954250671649, + "heading": -1.2168715161261342e-23, + "angularVelocity": -1.181327819705339e-25, + "velocityX": 1.7734000361068998, + "velocityY": -0.3444754882657602, + "timestamp": 0.29133148103920753 }, { - "x": 1.7632284859071354, - "y": 3.3825660365490977, - "heading": -0.5529765476421505, - "angularVelocity": -0.9475938909806194, - "velocityX": 2.2840147638960824, - "velocityY": -1.2675816767214638, - "timestamp": 0.8266067486742092 + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": -1.4479605554027417e-23, + "angularVelocity": -4.951541740242581e-24, + "velocityX": 2.130226970895328, + "velocityY": -0.4013456783802647, + "timestamp": 0.34959777724704905 }, { - "x": 1.9134098098681454, - "y": 3.2992204007592485, - "heading": -0.6152918417750989, - "angularVelocity": -0.8292555517624935, - "velocityX": 1.9985253764510553, - "velocityY": -1.1091150600437194, - "timestamp": 0.901752816735501 + "x": 1.0109741912909123, + "y": 6.869549473554426, + "heading": 0.010039096014815363, + "angularVelocity": 0.17297490390919232, + "velocityX": 2.484465074043925, + "velocityY": -0.44832534814487107, + "timestamp": 0.4076356531928316 }, { - "x": 2.042137357772617, - "y": 3.227782305964734, - "heading": -0.6687115003563293, - "angularVelocity": -0.7108776275248302, - "velocityX": 1.7130310504001012, - "velocityY": -0.9506564566524899, - "timestamp": 0.9768988847967928 + "x": 1.1757265724080805, + "y": 6.8408027772938045, + "heading": 0.030114435462749452, + "angularVelocity": 0.3459006574721642, + "velocityX": 2.83870452583543, + "velocityY": -0.49530924059791875, + "timestamp": 0.4656735291386142 }, { - "x": 2.1494108084908032, - "y": 3.168251239438324, - "heading": -0.7132320417999736, - "angularVelocity": -0.5924533723751416, - "velocityX": 1.4275324509419445, - "velocityY": -0.792204676335884, - "timestamp": 1.0520449528580844 + "x": 1.3610383679326326, + "y": 6.809328854359183, + "heading": 0.060221992176283975, + "angularVelocity": 0.5187570396556241, + "velocityX": 3.1929458565586635, + "velocityY": -0.5422997038007265, + "timestamp": 0.5237114050843967 }, { - "x": 2.2352298936741635, - "y": 3.120626784366874, - "heading": -0.7488501306754739, - "angularVelocity": -0.4739847312629708, - "velocityX": 1.1420302804580957, - "velocityY": -0.6337584427252518, - "timestamp": 1.127191020919376 + "x": 1.546079908472426, + "y": 6.784713461447441, + "heading": 0.1506024575679624, + "angularVelocity": 1.5572669385094218, + "velocityX": 3.188289328724817, + "velocityY": -0.4241263573246083, + "timestamp": 0.5817492810301793 }, { - "x": 2.299594398786613, - "y": 3.0849086278606843, - "heading": -0.7755632624576652, - "angularVelocity": -0.3554827613921602, - "velocityX": 0.8565252550532846, - "velocityY": -0.475316372868058, - "timestamp": 1.2023370889806677 + "x": 1.710561311165979, + "y": 6.762828401312947, + "heading": 0.2309691373975153, + "angularVelocity": 1.3847281369261228, + "velocityX": 2.834035533057883, + "velocityY": -0.37708237556692, + "timestamp": 0.6397871569759619 }, { - "x": 2.3425041622841327, - "y": 3.061096568391248, - "heading": -0.7933703919070813, - "angularVelocity": -0.2369668820847947, - "velocityX": 0.5710180799149875, - "velocityY": -0.31687698483458104, - "timestamp": 1.2774831570419594 + "x": 1.8544824093337007, + "y": 6.743675274338349, + "heading": 0.3013204880160867, + "angularVelocity": 1.2121627380762827, + "velocityX": 2.4797788654803434, + "velocityY": -0.3300108190122258, + "timestamp": 0.6978250329217445 }, { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": -0.11846288571271453, - "velocityX": 0.2855094390135433, - "velocityY": -0.1584387241831784, - "timestamp": 1.352629225103251 + "x": 1.9778431850692357, + "y": 6.727255600598568, + "heading": 0.36164875379241124, + "angularVelocity": 1.0394637087112153, + "velocityX": 2.12552188937403, + "velocityY": -0.28291307137290855, + "timestamp": 0.755862908867527 }, { - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, - "angularVelocity": 1.9944978930198404e-24, - "velocityX": 7.05107927160795e-24, - "velocityY": 9.089761159537798e-24, - "timestamp": 1.4277752931645427 + "x": 2.0806437247377927, + "y": 6.713570715572485, + "heading": 0.4119419860379565, + "angularVelocity": 0.8665588019197638, + "velocityX": 1.7712664013512611, + "velocityY": -0.23579231326223515, + "timestamp": 0.8139007848133096 }, { - "x": 2.3773573652919286, - "y": 3.040801892787579, - "heading": -0.7860925568420652, - "angularVelocity": 0.2647627943666689, - "velocityX": 0.2192460319725613, - "velocityY": -0.13726925804658435, - "timestamp": 1.4888860514802038 + "x": 2.16288415503368, + "y": 6.702621710000965, + "heading": 0.45218713038219516, + "angularVelocity": 0.6934289666602311, + "velocityX": 1.4170130962875669, + "velocityY": -0.18865276154743782, + "timestamp": 0.8719386607590922 }, { - "x": 2.4041795588293358, - "y": 3.024017002068538, - "heading": -0.754166261219504, - "angularVelocity": 0.5224333080216316, - "velocityX": 0.43891115536253184, - "velocityY": -0.27466343376629077, - "timestamp": 1.549996809795865 + "x": 2.2245645766627815, + "y": 6.694409403887936, + "heading": 0.48237322491294377, + "angularVelocity": 0.5201102562565806, + "velocityX": 1.062761526399103, + "velocityY": -0.14149908106046907, + "timestamp": 0.9299765367048748 }, { - "x": 2.4444548115194884, - "y": 2.998827368062219, - "heading": -0.7070016065582421, - "angularVelocity": 0.7717897136480927, - "velocityX": 0.6590533942013143, - "velocityY": -0.4121963906290397, - "timestamp": 1.6111075681115261 + "x": 2.265685008078599, + "y": 6.688934342765243, + "heading": 0.5024941236633007, + "angularVelocity": 0.3466856500598576, + "velocityX": 0.7085102744668164, + "velocityY": -0.09433600099024221, + "timestamp": 0.9880144126506574 }, { - "x": 2.4982173316537715, - "y": 2.9652220428664715, - "heading": -0.6452345488674253, - "angularVelocity": 1.010739506320076, - "velocityX": 0.8797554083125275, - "velocityY": -0.5499084960157551, - "timestamp": 1.6722183264271873 + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.1732712607555392, + "velocityX": 0.35425724418212445, + "velocityY": -0.04716814104583245, + "timestamp": 1.0460522885964398 }, { - "x": 2.565507310796468, - "y": 2.9231853998386073, - "heading": -0.5696881555564717, - "angularVelocity": 1.2362208454479753, - "velocityX": 1.1011151063633902, - "velocityY": -0.687876311577224, - "timestamp": 1.7333290847428484 + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -7.633423863817013e-25, + "velocityX": -5.0744692988574966e-24, + "velocityY": 1.0116864366107383e-24, + "timestamp": 1.1040901645422223 }, { - "x": 2.646370899659449, - "y": 2.872694862845298, - "heading": -0.4814293119182948, - "angularVelocity": 1.444243960814317, - "velocityX": 1.3232300022409869, - "velocityY": -0.8262135569077015, - "timestamp": 1.7944398430585096 + "x": 2.3347330882331176, + "y": 6.713955255875249, + "heading": 0.5125504196, + "angularVelocity": -3.160328817220775e-17, + "velocityX": 0.51083603498144, + "velocityY": 0.292445406541253, + "timestamp": 1.199008570534359 }, { - "x": 2.740859365850477, - "y": 2.8137198450437673, - "heading": -0.38181759469062915, - "angularVelocity": 1.6300193283992952, - "velocityX": 1.5461838274524098, - "velocityY": -0.9650513171003614, - "timestamp": 1.8555506013741707 + "x": 2.4317085702199166, + "y": 6.7694721581924355, + "heading": 0.5125504196, + "angularVelocity": -2.914232468557734e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645907, + "timestamp": 1.2939269765264956 }, { - "x": 2.8490284341196963, - "y": 2.7462234286806457, - "heading": -0.27256619156885004, - "angularVelocity": 1.787760553672932, - "velocityX": 1.7700495174758504, - "velocityY": -1.1044931894720724, - "timestamp": 1.9166613596898319 + "x": 2.577171786567949, + "y": 6.852747507871406, + "heading": 0.5125504196, + "angularVelocity": -1.252738056302556e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461287, + "timestamp": 1.3888453825186322 }, { - "x": 2.97093875412634, - "y": 2.6701673256371343, - "heading": -0.15589887958558987, - "angularVelocity": 1.909112490154803, - "velocityX": 1.994907662198022, - "velocityY": -1.2445615983138625, - "timestamp": 1.977772118005493 + "x": 2.771122709968306, + "y": 6.963781289278252, + "heading": 0.5125504196, + "angularVelocity": 1.2751776994126907e-16, + "velocityX": 2.0433436631502695, + "velocityY": 1.1697813532187185, + "timestamp": 1.4837637885107688 }, { - "x": 3.1066533293644194, - "y": 2.5855194210396055, - "heading": -0.03500682401650803, - "angularVelocity": 1.9782450570262398, - "velocityX": 2.220796779137645, - "velocityY": -1.3851555263033848, - "timestamp": 2.038882876321154 + "x": 2.9165859263163383, + "y": 7.047056638957223, + "heading": 0.5125504196, + "angularVelocity": 7.51589809363944e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461287, + "timestamp": 1.5786821945029055 }, { - "x": 3.2562037135860464, - "y": 2.492271514146211, - "heading": 0.08476246781347457, - "angularVelocity": 1.959872453412, - "velocityX": 2.4472022331835617, - "velocityY": -1.5258836490251428, - "timestamp": 2.099993634636815 + "x": 3.0135614083031372, + "y": 7.102573541274409, + "heading": 0.5125504196, + "angularVelocity": 6.994323112372533e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645907, + "timestamp": 1.6736006004950421 }, { - "x": 3.419377549511949, - "y": 2.390544477111736, - "heading": 0.19301716737769203, - "angularVelocity": 1.7714507649379716, - "velocityX": 2.6701327298713187, - "velocityY": -1.6646338523409365, - "timestamp": 2.1611043929524762 + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": -4.84539672491667e-17, + "velocityX": 0.51083603498144, + "velocityY": 0.29244540654125295, + "timestamp": 1.7685190064871787 }, { - "x": 3.5932192698224203, - "y": 2.280069793657614, - "heading": 0.2658871078344446, - "angularVelocity": 1.1924240913580317, - "velocityX": 2.8446991184843475, - "velocityY": -1.8077779837631391, - "timestamp": 2.2222151512681374 + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": 9.145149251733134e-29, + "velocityX": 3.330473944863186e-25, + "velocityY": 2.452283341762288e-25, + "timestamp": 1.8634374124793154 }, { - "x": 3.777176249725073, - "y": 2.160717630889141, - "heading": 0.3003277545805106, - "angularVelocity": 0.5635774730231045, - "velocityX": 3.010222503743814, - "velocityY": -1.9530466657273664, - "timestamp": 2.2833259095837986 + "x": 3.0904722904343656, + "y": 7.132620894810973, + "heading": 0.4951032659572505, + "angularVelocity": -0.25584290647295194, + "velocityX": 0.41679341451670293, + "velocityY": 0.03356417198946019, + "timestamp": 1.9316322038975173 }, { - "x": 3.9732480656956923, - "y": 2.03835976913752, - "heading": 0.3003278017850349, - "angularVelocity": 7.724421305582541e-7, - "velocityX": 3.208466420230474, - "velocityY": -2.0022311148487826, - "timestamp": 2.3444366678994597 + "x": 3.1473436628615548, + "y": 7.13720088972522, + "heading": 0.4609074027399865, + "angularVelocity": -0.5014439153799775, + "velocityX": 0.8339547822417649, + "velocityY": 0.06716047984016284, + "timestamp": 1.9998269953157193 }, { - "x": 4.174665289116757, - "y": 1.9250164613959089, - "heading": 0.3003278489820802, - "angularVelocity": 7.723197446110156e-7, - "velocityX": 3.295937228935466, - "velocityY": -1.8547193794609556, - "timestamp": 2.405547426215121 + "x": 3.232694908233173, + "y": 7.144074686546732, + "heading": 0.4108880311553055, + "angularVelocity": -0.7334778880389725, + "velocityX": 1.2515801221269964, + "velocityY": 0.10079650774733914, + "timestamp": 2.068021786733921 }, { - "x": 4.383567042240086, - "y": 1.82614452102736, - "heading": 0.30032789699541584, - "angularVelocity": 7.856773019778577e-7, - "velocityX": 3.4184120583853495, - "velocityY": -1.6179138190011713, - "timestamp": 2.466658184530782 + "x": 3.3465670290823137, + "y": 7.153245490430184, + "heading": 0.3463288656587193, + "angularVelocity": -0.9466876304478969, + "velocityX": 1.6698067180941178, + "velocityY": 0.1344795356468125, + "timestamp": 2.1362165781521227 }, { - "x": 4.598915447104952, - "y": 1.7422358380933087, - "heading": 0.30032794702414056, - "angularVelocity": 8.186565851278932e-7, - "velocityX": 3.5239033322497164, - "velocityY": -1.3730591019772607, - "timestamp": 2.527768942846443 + "x": 3.489013918523337, + "y": 7.164717369527402, + "heading": 0.26910560921909454, + "angularVelocity": -1.132392296151419, + "velocityX": 2.0888235960349633, + "velocityY": 0.16822221842234344, + "timestamp": 2.2044113695703245 }, { - "x": 4.819640328386772, - "y": 1.6737075334125113, - "heading": 0.30032800050450253, - "angularVelocity": 8.751382481764384e-7, - "velocityX": 3.611882545159867, - "velocityY": -1.1213787321509214, - "timestamp": 2.5888797011621043 + "x": 3.6601053694274515, + "y": 7.178495513325157, + "heading": 0.18214628797045526, + "angularVelocity": -1.2751607482067742, + "velocityX": 2.5088639080205257, + "velocityY": 0.2020409991909967, + "timestamp": 2.272606160988526 }, { - "x": 5.044644747071726, - "y": 1.620900221581336, - "heading": 0.30032805931258105, - "angularVelocity": 9.623195678675265e-7, - "velocityX": 3.681911743309069, - "velocityY": -0.8641246367522506, - "timestamp": 2.6499904594777655 + "x": 3.8599204497819497, + "y": 7.19458405385975, + "heading": 0.0905625121816468, + "angularVelocity": -1.3429731785111632, + "velocityX": 2.9300636632076498, + "velocityY": 0.23592037162970983, + "timestamp": 2.340800952406728 }, { - "x": 5.27281048204291, - "y": 1.5840763655637535, - "heading": 0.3003281261036848, - "angularVelocity": 0.0000010929516437587844, - "velocityX": 3.7336426720908507, - "velocityY": -0.6025756680578687, - "timestamp": 2.7111012177934266 + "x": 4.088431686194158, + "y": 7.212973174468024, + "heading": 0.005865325294296566, + "angularVelocity": -1.2419890892831975, + "velocityX": 3.350860551957296, + "velocityY": 0.26965579373215276, + "timestamp": 2.4089957438249296 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.3003281998542592, - "angularVelocity": 0.0000012068345473522654, - "velocityX": 3.7668181767205913, - "velocityY": -0.3380318248368492, - "timestamp": 2.7722119761090878 + "x": 4.340105997195649, + "y": 7.232705863139766, + "heading": 3.5066831801673635e-7, + "angularVelocity": -0.08600326365120493, + "velocityX": 3.6905210173326544, + "velocityY": 0.2893577098979943, + "timestamp": 2.4771905352431314 }, { - "x": 5.642202615777798, - "y": 1.556873162710076, - "heading": 0.30032827970972786, - "angularVelocity": 0.0000021672306415301637, - "velocityX": 3.777779819586101, - "velocityY": -0.17764976646888633, - "timestamp": 2.809058755213733 + "x": 4.593304664542687, + "y": 7.253186623137088, + "heading": 3.187896158571913e-7, + "angularVelocity": -4.67465351775184e-7, + "velocityX": 3.712873990541412, + "velocityY": 0.30032733543716505, + "timestamp": 2.545385326661333 }, { - "x": 5.781554061367138, - "y": 1.556248726330051, - "heading": 0.3003283534295918, - "angularVelocity": 0.0000020007139214038515, - "velocityX": 3.781916601002728, - "velocityY": -0.016946837558083135, - "timestamp": 2.8459055343183786 + "x": 4.846503330689328, + "y": 7.273667397974732, + "heading": 2.869109349284145e-7, + "angularVelocity": -4.674650404498194e-7, + "velocityX": 3.7128739729389193, + "velocityY": 0.30032755305381253, + "timestamp": 2.613580118079535 }, { - "x": 5.920806157723901, - "y": 1.5615468010933287, - "heading": 0.30032842223361694, - "angularVelocity": 0.000001867300937043874, - "velocityX": 3.7792203210295474, - "velocityY": 0.14378664545497588, - "timestamp": 2.882752313423024 + "x": 5.0997019968359405, + "y": 7.294148172812728, + "heading": 2.550322540852e-7, + "angularVelocity": -4.674650391951449e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589851, + "timestamp": 2.6817749094977366 }, { - "x": 6.059707308506225, - "y": 1.5727578116463863, - "heading": 0.3003284870919326, - "angularVelocity": 0.000001760216693496236, - "velocityX": 3.7696958637237334, - "velocityY": 0.3042602589826913, - "timestamp": 2.9195990925276694 + "x": 5.352900662982552, + "y": 7.314628947650725, + "heading": 2.2315357326181484e-7, + "angularVelocity": -4.674650389043701e-7, + "velocityX": 3.712873972938501, + "velocityY": 0.30032755305898534, + "timestamp": 2.7499697009159383 }, { - "x": 6.198006551988053, - "y": 1.5898614987106987, - "heading": 0.3003285487925156, - "angularVelocity": 0.0000016745176784450775, - "velocityX": 3.753360452186466, - "velocityY": 0.4641840475591571, - "timestamp": 2.9564458716323148 + "x": 5.606099329129164, + "y": 7.335109722488721, + "heading": 1.9127489242002956e-7, + "angularVelocity": -4.6746503917418713e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.30032755305898523, + "timestamp": 2.81816449233414 }, { - "x": 6.335454014584064, - "y": 1.6128269556768218, - "heading": 0.30032860799137623, - "angularVelocity": 0.0000016066223984499512, - "velocityX": 3.730243617919986, - "velocityY": 0.6232690488604353, - "timestamp": 2.99329265073696 + "x": 5.859297995275776, + "y": 7.355590497326717, + "heading": 1.593962115667357e-7, + "angularVelocity": -4.6746503934294765e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.30032755305898523, + "timestamp": 2.8863592837523417 }, { - "x": 6.47180136240203, - "y": 1.641612684542814, - "heading": 0.30032866524871465, - "angularVelocity": 0.0000015539306228254143, - "velocityX": 3.700387147292773, - "velocityY": 0.7812278186986222, - "timestamp": 3.0301394298416056 + "x": 6.112496661422388, + "y": 7.3760712721647135, + "heading": 1.2751753069996381e-7, + "angularVelocity": -4.674650395405871e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589853, + "timestamp": 2.9545540751705435 }, { - "x": 6.606802249418837, - "y": 1.6761666710700263, - "heading": 0.30032872317249165, - "angularVelocity": 0.0000015720173768370867, - "velocityX": 3.6638449899081134, - "velocityY": 0.937774952569887, - "timestamp": 3.066986208946251 + "x": 6.365695327569, + "y": 7.39655204700271, + "heading": 9.56388499052934e-8, + "angularVelocity": -4.674650384833005e-7, + "velocityX": 3.7128739729385014, + "velocityY": 0.3003275530589854, + "timestamp": 3.022748866588745 }, { - "x": 6.740172242433044, - "y": 1.7164148916635715, - "heading": 0.30049565594915995, - "angularVelocity": 0.004530457769298999, - "velocityX": 3.619583482057768, - "velocityY": 1.0923131294390627, - "timestamp": 3.1038329880508964 + "x": 6.618893993715612, + "y": 7.41703282184071, + "heading": 6.376016904700571e-8, + "angularVelocity": -4.6746503941617634e-7, + "velocityX": 3.7128739729384974, + "velocityY": 0.300327553059032, + "timestamp": 3.090943658006947 }, { - "x": 6.869834708516544, - "y": 1.760792645305141, - "heading": 0.3094576493704555, - "angularVelocity": 0.2432232515043802, - "velocityX": 3.5189633730334187, - "velocityY": 1.2043862372756071, - "timestamp": 3.1406797671555418 + "x": 6.872092659851402, + "y": 7.437513596812488, + "heading": 3.188148827519539e-8, + "angularVelocity": -4.674650381480829e-7, + "velocityX": 3.7128739727798186, + "velocityY": 0.3003275550207428, + "timestamp": 3.1591384494251487 }, { - "x": 6.995163652975955, - "y": 1.807928879541571, - "heading": 0.3302942112120631, - "angularVelocity": 0.5654920822911383, - "velocityX": 3.4013541347392664, - "velocityY": 1.2792497846979316, - "timestamp": 3.177526546260187 + "x": 7.125290870666504, + "y": 7.458, + "heading": 2.875044920599443e-26, + "angularVelocity": -4.675062070310236e-7, + "velocityX": 3.7128672960134566, + "velocityY": 0.300410086481236, + "timestamp": 3.2273332408433504 }, { - "x": 7.114961828122759, - "y": 1.8557263226321454, - "heading": 0.3593656658524349, - "angularVelocity": 0.7889822488366823, - "velocityX": 3.2512522955283503, - "velocityY": 1.2971946056622388, - "timestamp": 3.2143733253648326 + "x": 7.327398255820751, + "y": 7.485125041057045, + "heading": -0.0034915599283927, + "angularVelocity": -0.05859476692705873, + "velocityX": 3.3917318820879983, + "velocityY": 0.4552078415437867, + "timestamp": 3.2869214961006694 }, { - "x": 7.2288849177114916, - "y": 1.9032201895276672, - "heading": 0.39122292476577963, - "angularVelocity": 0.8645873448767251, - "velocityX": 3.091805915116442, - "velocityY": 1.2889557255639152, - "timestamp": 3.251220104469478 + "x": 7.508458450228073, + "y": 7.505782624333376, + "heading": -0.006110359017821738, + "angularVelocity": -0.04394824245348874, + "velocityX": 3.038521494302052, + "velocityY": 0.3466720612497382, + "timestamp": 3.3465097513579884 }, { - "x": 7.3369415857594324, - "y": 1.9498953701157864, - "heading": 0.4232031564001013, - "angularVelocity": 0.8679247524864317, - "velocityX": 2.932594671058162, - "velocityY": 1.2667370587687046, - "timestamp": 3.2880668835741234 + "x": 7.668471533847549, + "y": 7.519972433195907, + "heading": -0.00785620988870458, + "angularVelocity": -0.029298573407523957, + "velocityX": 2.68531244837584, + "velocityY": 0.23813096727293456, + "timestamp": 3.4060980066153075 }, { - "x": 7.4391897785347085, - "y": 1.9954627576139183, - "heading": 0.4537900488962921, - "angularVelocity": 0.8301103444977762, - "velocityX": 2.774956054771842, - "velocityY": 1.236672203253364, - "timestamp": 3.3249136626787688 + "x": 7.807437533254427, + "y": 7.527694362359166, + "heading": -0.008729041834027805, + "angularVelocity": -0.01464771776844422, + "velocityX": 2.332103848430922, + "velocityY": 0.12958810641313134, + "timestamp": 3.4656862618726265 }, { - "x": 7.535687239808996, - "y": 2.0397404710658504, - "heading": 0.48199026779702103, - "angularVelocity": 0.7653374212340174, - "velocityX": 2.618884570622394, - "velocityY": 1.2016712051325453, - "timestamp": 3.361760441783414 + "x": 7.925356461622809, + "y": 7.528948359353389, + "heading": -0.008728854851904898, + "angularVelocity": 0.0000031379022946504295, + "velocityX": 1.9788954695715617, + "velocityY": 0.021044365014674563, + "timestamp": 3.5252745171299456 }, { - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, - "angularVelocity": 0.681422832753116, - "velocityX": 2.464168636543981, - "velocityY": 1.1632956752609667, - "timestamp": 3.3986072208880596 + "x": 8.022228326704038, + "y": 7.523734392879376, + "heading": -0.007855710298573708, + "angularVelocity": 0.0146529639030494, + "velocityX": 1.6256872207939095, + "velocityY": -0.0874999016416258, + "timestamp": 3.5848627723872646 + }, + { + "x": 8.098053133481649, + "y": 7.512052442276777, + "heading": -0.006109727375299004, + "angularVelocity": 0.029300789488382373, + "velocityX": 1.272479055649084, + "velocityY": -0.19604451501648842, + "timestamp": 3.6444510276445836 }, { - "x": 7.766066221949162, - "y": 2.1519606772611084, - "heading": 0.5396117040985706, - "angularVelocity": 0.5123769157726334, - "velocityX": 2.199683557182638, - "velocityY": 1.0929921902308588, - "timestamp": 3.4620628503385116 + "x": 8.152830885308017, + "y": 7.493902493017176, + "heading": -0.0034910810079803226, + "angularVelocity": 0.04394567949691789, + "velocityX": 0.919270946763285, + "velocityY": -0.30458937220470644, + "timestamp": 3.7040392829019027 }, { - "x": 7.88917922885163, - "y": 2.215677284676183, - "heading": 0.5626038110324001, - "angularVelocity": 0.36233360433028805, - "velocityX": 1.9401431830189138, - "velocityY": 1.0041127629949167, - "timestamp": 3.5255184797889636 + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 2.1197622073376246e-26, + "angularVelocity": 0.058586729765871357, + "velocityX": 0.5660628762997177, + "velocityY": -0.41313440805613916, + "timestamp": 3.7636275381592217 }, { - "x": 7.996083890855478, - "y": 2.2729699125828824, - "heading": 0.576920084072235, - "angularVelocity": 0.2256107639908224, - "velocityX": 1.6847151770407296, - "velocityY": 0.9028769929929541, - "timestamp": 3.5889741092394156 + "x": 8.185948757189772, + "y": 7.412401444181256, + "heading": 0.007947733329215964, + "angularVelocity": 0.08230133185775951, + "velocityX": -0.0063460233869197455, + "velocityY": -0.5890426736452052, + "timestamp": 3.8601962467063924 }, { - "x": 8.086991794921751, - "y": 2.3232832101942007, - "heading": 0.5831794167828934, - "angularVelocity": 0.09864109401902421, - "velocityX": 1.4326215791028423, - "velocityY": 0.792889425998124, - "timestamp": 3.6524297386898676 + "x": 8.130028903552427, + "y": 7.338629187384324, + "heading": 0.018106821496109617, + "angularVelocity": 0.10520062160644154, + "velocityX": -0.5790680488393326, + "velocityY": -0.7639354186961682, + "timestamp": 3.956764955253563 }, { - "x": 8.162075125847174, - "y": 2.3662047030478557, - "heading": 0.5818525424229896, - "angularVelocity": -0.02091027023756688, - "velocityX": 1.1832414487992695, - "velocityY": 0.6764016561709427, - "timestamp": 3.7158853681403197 + "x": 8.018757435307506, + "y": 7.248113553962043, + "heading": 0.03036067277368815, + "angularVelocity": 0.12689256656666278, + "velocityX": -1.1522517999768782, + "velocityY": -0.9373184625128009, + "timestamp": 4.053333663800734 }, { - "x": 8.2214756151654, - "y": 2.4014165863300603, - "heading": 0.5733088893147004, - "angularVelocity": -0.1346397976393925, - "velocityX": 0.936094871844373, - "velocityY": 0.5549055865831228, - "timestamp": 3.7793409975907717 + "x": 7.852062035550644, + "y": 7.141094085596084, + "heading": 0.044518821199191705, + "angularVelocity": 0.14661217529473056, + "velocityX": -1.7261844158911652, + "velocityY": -1.1082209752622316, + "timestamp": 4.1499023723479045 }, { - "x": 8.265311459891254, - "y": 2.4286667893871807, - "heading": 0.5578458366246501, - "angularVelocity": -0.24368291393475716, - "velocityX": 0.6908109667414808, - "velocityY": 0.4294371246982685, - "timestamp": 3.8427966270412237 + "x": 7.629805209867524, + "y": 7.0180373220836785, + "heading": 0.060212869798815634, + "angularVelocity": 0.16251691501039256, + "velocityX": -2.301540830636187, + "velocityY": -1.2742923185339627, + "timestamp": 4.246471080895075 }, { - "x": 8.29368248788513, - "y": 2.4477506399099855, - "heading": 0.5357077069480222, - "angularVelocity": -0.34887573992019155, - "velocityX": 0.44710025319389435, - "velocityY": 0.30074322306905504, - "timestamp": 3.9062522564916757 + "x": 7.351625362652527, + "y": 6.880249252941204, + "heading": 0.07642107570328951, + "angularVelocity": 0.16784117907672624, + "velocityX": -2.8806416840411067, + "velocityY": -1.4268397208104806, + "timestamp": 4.343039789442246 }, { - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, - "angularVelocity": -0.45085365638085945, - "velocityX": 0.20473385621503526, - "velocityY": 0.16937940002276766, - "timestamp": 3.9697078859421278 + "x": 7.016267448829526, + "y": 6.750124387193528, + "heading": 0.07642107964742734, + "angularVelocity": 4.0842814169097235e-8, + "velocityX": -3.4727389323964073, + "velocityY": -1.347484787829737, + "timestamp": 4.439608497989417 }, { - "x": 8.303679627930654, - "y": 2.4605245534000177, - "heading": 0.47064149696897567, - "angularVelocity": -0.5538247976382693, - "velocityX": -0.04548808629479942, - "velocityY": 0.03077484607623031, - "timestamp": 4.035535576538194 + "x": 6.673392879085966, + "y": 6.641338156398843, + "heading": 0.0764210813675651, + "angularVelocity": 1.7812579245729294e-8, + "velocityX": -3.5505763192025745, + "velocityY": -1.1265163677895376, + "timestamp": 4.536177206536587 }, { - "x": 8.284168636347566, - "y": 2.453496432112781, - "heading": 0.4276658140977222, - "angularVelocity": -0.6528511403348756, - "velocityX": -0.29639489713850375, - "velocityY": -0.10676542384515514, - "timestamp": 4.1013632671342615 + "x": 6.330518244592264, + "y": 6.532552129685128, + "heading": 0.0764210830877029, + "angularVelocity": 1.7812579496548032e-8, + "velocityX": -3.550576989711091, + "velocityY": -1.1265142544655247, + "timestamp": 4.632745915083758 }, { - "x": 8.248089021230637, - "y": 2.4374965938671473, - "heading": 0.378472576604384, - "angularVelocity": -0.7473031037226994, - "velocityX": -0.5480917648823836, - "velocityY": -0.24305635061409855, - "timestamp": 4.1671909577303285 + "x": 5.987643610098044, + "y": 6.42376610297305, + "heading": 0.07642108480784073, + "angularVelocity": 1.7812579676147597e-8, + "velocityX": -3.550576989716468, + "velocityY": -1.1265142544485776, + "timestamp": 4.729314623630929 }, { - "x": 8.195380139344671, - "y": 2.412622948042365, - "heading": 0.3234149472418141, - "angularVelocity": -0.8363901097551141, - "velocityX": -0.8007098746544274, - "velocityY": -0.37785991882067893, - "timestamp": 4.233018648326396 + "x": 5.6447689756038235, + "y": 6.314980076260972, + "heading": 0.0764210865279785, + "angularVelocity": 1.7812579243167278e-8, + "velocityX": -3.550576989716468, + "velocityY": -1.1265142544485782, + "timestamp": 4.825883332178099 }, { - "x": 8.125970395948434, - "y": 2.3789940000965495, - "heading": 0.2629133084829053, - "angularVelocity": -0.9190910118685506, - "velocityX": -1.0544155927047596, - "velocityY": -0.5108632498164021, - "timestamp": 4.298846338922463 + "x": 5.3018943411135915, + "y": 6.20619404953636, + "heading": 0.07642108824812432, + "angularVelocity": 1.7812662732207566e-8, + "velocityX": -3.5505769896751667, + "velocityY": -1.1265142545783664, + "timestamp": 4.92245204072527 }, { - "x": 8.039774042344641, - "y": 2.3367560367748186, - "heading": 0.19747799875428093, - "angularVelocity": -0.9940392733834453, - "velocityX": -1.3094239342636282, - "velocityY": -0.6416443131950763, - "timestamp": 4.36467402951853 + "x": 4.971178879857472, + "y": 6.101264896222856, + "heading": 0.09246038898135349, + "angularVelocity": 0.16609211176718244, + "velocityX": -3.4246648446642007, + "velocityY": -1.0865750913739254, + "timestamp": 5.019020749272441 }, { - "x": 7.936686649427289, - "y": 2.286094003355545, - "heading": 0.12774482040942284, - "angularVelocity": -1.0593289497691225, - "velocityX": -1.5660186766982322, - "velocityY": -0.7696158403937725, - "timestamp": 4.430501720114597 + "x": 4.695582635189411, + "y": 6.01382394107076, + "heading": 0.10582487967853092, + "angularVelocity": 0.13839359455293215, + "velocityX": -2.8538876496773393, + "velocityY": -0.9054791812752113, + "timestamp": 5.1155894578196115 }, { - "x": 7.816578572926189, - "y": 2.2272487613470173, - "heading": 0.05453269620309629, - "angularVelocity": -1.1121782268737388, - "velocityX": -1.8245828679925904, - "velocityY": -0.8939283981510958, - "timestamp": 4.496329410710664 + "x": 4.475105628641342, + "y": 5.943871182253239, + "heading": 0.1165156132920598, + "angularVelocity": 0.11070598099908092, + "velocityX": -2.2831102317203804, + "velocityY": -0.7243832900939244, + "timestamp": 5.212158166366782 }, { - "x": 7.679285357941873, - "y": 2.160546156413331, - "heading": -0.02105873619647849, - "angularVelocity": -1.1483227151841096, - "velocityX": -2.085645322525112, - "velocityY": -1.0132909772422172, - "timestamp": 4.562157101306731 + "x": 4.309747868917504, + "y": 5.8914066159860985, + "heading": 0.12453333874296012, + "angularVelocity": 0.08302612276298497, + "velocityX": -1.7123327236282266, + "velocityY": -0.5432874380991931, + "timestamp": 5.308726874913953 }, { - "x": 7.52459365410468, - "y": 2.086449920391606, - "heading": -0.09747489817686252, - "angularVelocity": -1.16085132697865, - "velocityX": -2.349948819964157, - "velocityY": -1.1256089246149548, - "timestamp": 4.627984791902798 + "x": 4.19950936065589, + "y": 5.856430239360431, + "heading": 0.12987847932741303, + "angularVelocity": 0.055350647894830074, + "velocityX": -1.1415551675081796, + "velocityY": -0.36219161622714885, + "timestamp": 5.405295583461124 }, { - "x": 7.352222023674252, - "y": 2.0056683293260935, - "heading": -0.17237070551570732, - "angularVelocity": -1.1377553528107507, - "velocityX": -2.6185276875067363, - "velocityY": -1.2271673263035494, - "timestamp": 4.693812482498865 + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.027676321443494054, + "velocityX": -0.5707775871082961, + "velocityY": -0.18109580929158178, + "timestamp": 5.501864292008294 }, { - "x": 7.1618070022795735, - "y": 1.9194015924783232, - "heading": -0.24188707800247594, - "angularVelocity": -1.0560354139314483, - "velocityX": -2.8926280060941036, - "velocityY": -1.3104931384745382, - "timestamp": 4.759640173094932 + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 2.6946819475075084e-27, + "velocityX": -6.238393680699965e-27, + "velocityY": 1.2085111651652188e-26, + "timestamp": 5.598433000555465 }, { - "x": 6.953005373134873, - "y": 1.8300479988886789, - "heading": -0.29871595232882114, - "angularVelocity": -0.8632974028370489, - "velocityX": -3.1719421911054577, - "velocityY": -1.3573861209553544, - "timestamp": 4.825467863690999 + "x": 4.164497272188891, + "y": 5.856383810972102, + "heading": 0.11953980585413508, + "angularVelocity": -0.19788617668780903, + "velocityX": 0.30580479589931786, + "velocityY": 0.26526731175760876, + "timestamp": 5.664184637734952 }, { - "x": 6.727423586928556, - "y": 1.7436722229358965, - "heading": -0.32018076563144104, - "angularVelocity": -0.3260757457577048, - "velocityX": -3.426852501791936, - "velocityY": -1.3121495706541393, - "timestamp": 4.891295554287066 + "x": 4.205280500846038, + "y": 5.8906190238483225, + "heading": 0.09417342379390335, + "angularVelocity": -0.38579088138881684, + "velocityX": 0.6202617973727179, + "velocityY": 0.520674683472388, + "timestamp": 5.729936274914438 }, { - "x": 6.489444642291127, - "y": 1.670557467554106, - "heading": -0.32018100817910516, - "angularVelocity": -0.0000036845841301644418, - "velocityX": -3.615179911106431, - "velocityY": -1.1106990799728806, - "timestamp": 4.9571232448831335 + "x": 4.267438293143164, + "y": 5.9407879010107, + "heading": 0.05730532907136487, + "angularVelocity": -0.5607175167653599, + "velocityX": 0.9453421232303373, + "velocityY": 0.7630057488215584, + "timestamp": 5.795687912093925 }, { - "x": 6.246603596847642, - "y": 1.615712184721808, - "heading": -0.3201810383505521, - "angularVelocity": -4.583397455824824e-7, - "velocityX": -3.6890409377052413, - "velocityY": -0.8331643163488851, - "timestamp": 5.022950935479201 + "x": 4.351839190303838, + "y": 6.005705720740894, + "heading": 0.010093847833175575, + "angularVelocity": -0.7180274630928684, + "velocityX": 1.283631872622101, + "velocityY": 0.9873186815559245, + "timestamp": 5.8614395492734115 }, { - "x": 6.0003008879136885, - "y": 1.5794526731151552, - "heading": -0.3201810728782718, - "angularVelocity": -5.245166498428474e-7, - "velocityX": -3.7416276752791178, - "velocityY": -0.5508246040279513, - "timestamp": 5.088778626075268 + "x": 4.459568016776304, + "y": 6.083661828301211, + "heading": -0.04581026085444041, + "angularVelocity": -0.850231432793241, + "velocityX": 1.6384204423441482, + "velocityY": 1.1856146995627632, + "timestamp": 5.927191186452898 }, { - "x": 5.751956851390291, - "y": 1.561988034181555, - "heading": -0.3201811140764657, - "angularVelocity": -6.258489930223753e-7, - "velocityX": -3.7726378409245966, - "velocityY": -0.265308394924062, - "timestamp": 5.154606316671335 + "x": 4.591948294079918, + "y": 6.172045469361953, + "heading": -0.1079292379524144, + "angularVelocity": -0.944751792695351, + "velocityX": 2.013338115707256, + "velocityY": 1.344204416073704, + "timestamp": 5.992942823632385 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.3201811662076607, - "angularVelocity": -7.919341312199672e-7, - "velocityX": -3.7818925725102797, - "velocityY": 0.02173781608673755, - "timestamp": 5.220434007267402 + "x": 4.7504027579860635, + "y": 6.266641095517782, + "heading": -0.17238119577808125, + "angularVelocity": -0.9802335058171755, + "velocityX": 2.409893817147125, + "velocityY": 1.438680924363996, + "timestamp": 6.058694460811871 }, { - "x": 5.250082401017072, - "y": 1.5845061941743261, - "heading": -0.3201812130071943, - "angularVelocity": -6.973782997664008e-7, - "velocityX": -3.7688784475631176, - "velocityY": 0.3142288252976883, - "timestamp": 5.28754182190281 + "x": 4.935641035495618, + "y": 6.360501248168881, + "heading": -0.23308994891442564, + "angularVelocity": -0.9233040535648408, + "velocityX": 2.8172420559490887, + "velocityY": 1.4274952940697678, + "timestamp": 6.124446097991358 }, { - "x": 4.999550332100038, - "y": 1.6250954552906307, - "heading": -0.3201812494932628, - "angularVelocity": -5.436932891401309e-7, - "velocityX": -3.7332771194853676, - "velocityY": 0.6048365803121931, - "timestamp": 5.354649636538218 + "x": 5.145232187341836, + "y": 6.443576909061307, + "heading": -0.2814508136896339, + "angularVelocity": -0.7355081462564137, + "velocityX": 3.1876187550141757, + "velocityY": 1.2634766897993692, + "timestamp": 6.190197735170845 }, { - "x": 4.752908853379521, - "y": 1.6849435065641751, - "heading": -0.32018127950441017, - "angularVelocity": -4.4720793763185246e-7, - "velocityX": -3.6753019012838326, - "velocityY": 0.8918194043226525, - "timestamp": 5.421757451173626 + "x": 5.371831569570122, + "y": 6.506507878011031, + "heading": -0.310075861000899, + "angularVelocity": -0.4353510960210094, + "velocityX": 3.446292624010618, + "velocityY": 0.9571011711531484, + "timestamp": 6.255949372350331 }, { - "x": 4.5116361075787825, - "y": 1.7636916636244688, - "heading": -0.32018130527180205, - "angularVelocity": -3.839700641654236e-7, - "velocityX": -3.595300295674319, - "velocityY": 1.1734573311338945, - "timestamp": 5.488865265809034 + "x": 5.608723728071605, + "y": 6.544912225542492, + "heading": -0.32489986784082275, + "angularVelocity": -0.22545456624080287, + "velocityX": 3.6028328519763595, + "velocityY": 0.5840819967208853, + "timestamp": 6.321701009529818 }, { - "x": 4.277178056371613, - "y": 1.8608679654762732, - "heading": -0.32018132820641354, - "angularVelocity": -3.4175768691264823e-7, - "velocityX": -3.493751845160869, - "velocityY": 1.4480623811065336, - "timestamp": 5.555973080444442 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.3320371023810657, + "angularVelocity": -0.10854839280670678, + "velocityX": 3.6853903618544206, + "velocityY": 0.18577139494688796, + "timestamp": 6.3874526467093045 }, { - "x": 4.05093980766007, - "y": 1.975889994456223, - "heading": -0.3201813492710647, - "angularVelocity": -3.138926699011739e-7, - "velocityX": -3.3712653279007916, - "velocityY": 1.7139885958864325, - "timestamp": 5.62308089507985 + "x": 5.9711094207782285, + "y": 6.556647574574296, + "heading": -0.33458795200315017, + "angularVelocity": -0.07861324883762467, + "velocityX": 3.700225495398522, + "velocityY": -0.014775117904477024, + "timestamp": 6.419900734783533 }, { - "x": 3.834277168836918, - "y": 2.1080683355859073, - "heading": -0.3201813691749636, - "angularVelocity": -2.965958441785772e-7, - "velocityX": -3.2285753902174523, - "velocityY": 1.9696415663034246, - "timestamp": 5.690188709715258 + "x": 6.091317843363461, + "y": 6.549645373760147, + "heading": -0.3360778340908156, + "angularVelocity": -0.045915866730181235, + "velocityX": 3.704638076371226, + "velocityY": -0.21579702317530708, + "timestamp": 6.452348822857761 }, { - "x": 3.6284883544416036, - "y": 2.2566105084469403, - "heading": -0.32018138848592925, - "angularVelocity": -2.877603113639349e-7, - "velocityX": -3.0665402459214466, - "velocityY": 2.2134854736076965, - "timestamp": 5.757296524350666 + "x": 6.211267508828441, + "y": 6.536122994121453, + "heading": -0.33661432797946356, + "angularVelocity": -0.016533913721531207, + "velocityX": 3.6966635812434836, + "velocityY": -0.4167388724956589, + "timestamp": 6.48479691093199 }, { - "x": 3.4324267312993486, - "y": 2.4177739449564437, - "heading": -0.3201814076661106, - "angularVelocity": -2.8581144314704067e-7, - "velocityX": -2.921591534569326, - "velocityY": 2.4015598985765383, - "timestamp": 5.824404338986074 + "x": 6.330469045455405, + "y": 6.51611559729621, + "heading": -0.33661493169123347, + "angularVelocity": -0.000018605465091905695, + "velocityX": 3.6736074049811314, + "velocityY": -0.6165970943950513, + "timestamp": 6.517244999006218 }, { - "x": 3.2363665756420468, - "y": 2.578939166702229, - "heading": -0.3201814268462876, - "angularVelocity": -2.858113772828912e-7, - "velocityX": -2.921569666997558, - "velocityY": 2.401586501086132, - "timestamp": 5.891512153621482 + "x": 6.448416412815129, + "y": 6.489700510555554, + "heading": -0.3366150129853366, + "angularVelocity": -0.0000025053588043472207, + "velocityX": 3.6349558436203777, + "velocityY": -0.8140722091307351, + "timestamp": 6.549693087080446 }, { - "x": 3.04030639781219, - "y": 2.740104361474713, - "heading": -0.3201814460264912, - "angularVelocity": -2.8581177503839375e-7, - "velocityX": -2.9215699973995357, - "velocityY": 2.4015860991463303, - "timestamp": 5.95861996825689 + "x": 6.564765294299518, + "y": 6.456954843497344, + "heading": -0.3366150728513962, + "angularVelocity": -0.000001844979570705055, + "velocityX": 3.5856929757534926, + "velocityY": -1.0091709250573346, + "timestamp": 6.582141175154675 }, { - "x": 2.8448806552051593, - "y": 2.899843193504081, - "heading": -0.32455581469161493, - "angularVelocity": -0.06518419186929905, - "velocityX": -2.912116028047787, - "velocityY": 2.380331305634352, - "timestamp": 6.025727782892298 + "x": 6.679176170718401, + "y": 6.417974154303709, + "heading": -0.33661511946704875, + "angularVelocity": -0.0000014366224727676693, + "velocityX": 3.525966650397334, + "velocityY": -1.2013246852776633, + "timestamp": 6.614589263228903 }, { - "x": 2.663237615437101, - "y": 3.049167443178603, - "heading": -0.36928029407596225, - "angularVelocity": -0.6664570978407269, - "velocityX": -2.706734539858174, - "velocityY": 2.225139508502705, - "timestamp": 6.092835597527706 + "x": 6.791315180749945, + "y": 6.372872197072731, + "heading": -0.33661515729887875, + "angularVelocity": -0.000001165918618828322, + "velocityX": 3.4559512343227956, + "velocityY": -1.389972719742485, + "timestamp": 6.647037351303132 }, { - "x": 2.4966866332566564, - "y": 3.186079438683932, - "heading": -0.4222560068283227, - "angularVelocity": -0.7894119789203976, - "velocityX": -2.481841840407185, - "velocityY": 2.0401796161767862, - "timestamp": 6.1599434121631145 + "x": 6.900855096060357, + "y": 6.3217805921945684, + "heading": -0.3366151890219745, + "angularVelocity": -9.776568561253354e-7, + "velocityX": 3.375851145985221, + "velocityY": -1.5745644169014148, + "timestamp": 6.67948543937736 }, { - "x": 2.3453101524500037, - "y": 3.3105160667275593, - "heading": -0.47671806974270053, - "angularVelocity": -0.8115606686086642, - "velocityX": -2.2557206135987404, - "velocityY": 1.8542792477997239, - "timestamp": 6.2270512267985225 + "x": 7.007476282450098, + "y": 6.264848452420557, + "heading": -0.33661521634397956, + "angularVelocity": -8.420220318148558e-7, + "velocityX": 3.285900424882802, + "velocityY": -1.7545606891775674, + "timestamp": 6.711933527451588 }, { - "x": 2.209108176535601, - "y": 3.4224777520191383, - "heading": -0.5296390322123383, - "angularVelocity": -0.7885961233748087, - "velocityX": -2.029599334360373, - "velocityY": 1.6683852081886403, - "timestamp": 6.294159041433931 + "x": 7.110867767687471, + "y": 6.2022421991346945, + "heading": -0.33661529971413134, + "angularVelocity": -0.0000025693394187906504, + "velocityX": 3.186366019497228, + "velocityY": -1.9294281112232177, + "timestamp": 6.744381615525817 }, { - "x": 2.0880704157388417, - "y": 3.5219728662741354, - "heading": -0.579287382494677, - "angularVelocity": -0.7398296390975455, - "velocityX": -1.8036313871096692, - "velocityY": 1.4826159188098624, - "timestamp": 6.361266856069339 + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.25801763236442515, + "velocityX": 3.2027040434173877, + "velocityY": -1.73803251899956, + "timestamp": 6.776829703600045 }, { - "x": 1.9821863263109802, - "y": 3.609010233643184, - "heading": -0.6245361276970436, - "angularVelocity": -0.6742693894623133, - "velocityX": -1.577820556415411, - "velocityY": 1.2969781215781884, - "timestamp": 6.428374670704747 + "x": 7.403072746116238, + "y": 6.061144053185023, + "heading": -0.35924792619770074, + "angularVelocity": -0.23258228867402195, + "velocityX": 3.0708274318237563, + "velocityY": -1.3814614025635412, + "timestamp": 6.8381432615033795 }, { - "x": 1.8914467304783316, - "y": 3.6835977422656017, - "heading": -0.664590534011598, - "angularVelocity": -0.5968664980698198, - "velocityX": -1.3521464873447422, - "velocityY": 1.1114578686200178, - "timestamp": 6.495482485340155 + "x": 7.5736331381530135, + "y": 5.991566896909843, + "heading": -0.3703276689612211, + "angularVelocity": -0.18070624413915942, + "velocityX": 2.781772871600061, + "velocityY": -1.1347760373794369, + "timestamp": 6.899456819406714 }, { - "x": 1.815843887624038, - "y": 3.745742167237612, - "heading": -0.6988590085071317, - "angularVelocity": -0.510648047201536, - "velocityX": -1.1265877642572446, - "velocityY": 0.9260385740414423, - "timestamp": 6.562590299975563 + "x": 7.72439550755076, + "y": 5.934268464416171, + "heading": -0.37789043666294847, + "angularVelocity": -0.12334576495545502, + "velocityX": 2.4588749136926022, + "velocityY": -0.934514884685174, + "timestamp": 6.960770377310048 }, { - "x": 1.7553712772903527, - "y": 3.7954492682162915, - "heading": -0.7268838015342108, - "angularVelocity": -0.41760848836064807, - "velocityX": -0.9011262050810134, - "velocityY": 0.7407051063834291, - "timestamp": 6.629698114610971 + "x": 7.854564778976688, + "y": 5.88786008036463, + "heading": -0.38180542343665536, + "angularVelocity": -0.06385189357106222, + "velocityX": 2.1230095900021, + "velocityY": -0.7569024802753538, + "timestamp": 7.022083935213383 }, { - "x": 1.710023362676749, - "y": 3.8327239369169632, - "heading": -0.7483003159780411, - "angularVelocity": -0.3191359241868454, - "velocityX": -0.675747151952063, - "velocityY": 0.5554445321037842, - "timestamp": 6.696805929246379 + "x": 7.963733931476252, + "y": 5.851532970517725, + "heading": -0.38200457597908144, + "angularVelocity": -0.0032480995922642706, + "velocityX": 1.7805059147224407, + "velocityY": -0.592480865393233, + "timestamp": 7.083397493116717 }, { - "x": 1.679795389171883, - "y": 3.8575703362206024, - "heading": -0.7628115166816798, - "angularVelocity": -0.21623712204721673, - "velocityX": -0.4504389491610239, - "velocityY": 0.37024599055457114, - "timestamp": 6.763913743881787 + "x": 8.05165855711374, + "y": 5.8247603198900775, + "heading": -0.3784464795942301, + "angularVelocity": 0.0580311517798547, + "velocityX": 1.434016042195889, + "velocityY": -0.43665139559925853, + "timestamp": 7.144711051020051 }, { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -0.10966573621700788, - "velocityX": -0.22519234880908606, - "velocityY": 0.1851003730766039, - "timestamp": 6.831021558517195 + "x": 8.118176532906231, + "y": 5.80717243118701, + "heading": -0.3711033743685625, + "angularVelocity": 0.11976315641712676, + "velocityX": 1.084882007619954, + "velocityY": -0.2868515431904276, + "timestamp": 7.206024608923386 }, { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": 8.092112436868569e-26, - "velocityX": -3.5782911869768004e-26, - "velocityY": 1.7219402564933353e-26, - "timestamp": 6.898129373152603 + "x": 8.16317279255834, + "y": 5.798495818445154, + "heading": -0.3599553749598749, + "angularVelocity": 0.18181948316004298, + "velocityX": 0.7338712870495722, + "velocityY": -0.1415121392161809, + "timestamp": 7.26733816682672 }, { - "x": 1.659892350481847, - "y": 3.8800826841683307, - "heading": -0.726477756364864, - "angularVelocity": 0.7466472297695131, - "velocityX": -0.08186840257721437, - "velocityY": 0.1724334716376635, - "timestamp": 6.956648559285806 + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.24412049957493756, + "velocityX": 0.38146199167222045, + "velocityY": 0.0003958300819017007, + "timestamp": 7.328651724730054 + }, + { + "x": 8.186562507619959, + "y": 5.808483646148046, + "heading": -0.3243738625034631, + "angularVelocity": 0.3115588640226202, + "velocityX": 0.000013952657501305156, + "velocityY": 0.1505914724827301, + "timestamp": 7.394814554457269 }, { - "x": 1.6562452284597964, - "y": 3.902216695654719, - "heading": -0.6460681483712878, - "angularVelocity": 1.3740725616133136, - "velocityX": -0.06232352606116078, - "velocityY": 0.3782351216574767, - "timestamp": 7.015167745419008 + "x": 8.161321884729395, + "y": 5.828379592770816, + "heading": -0.29947359998774176, + "angularVelocity": 0.37634820968788424, + "velocityX": -0.38149249351379677, + "velocityY": 0.30071184537904955, + "timestamp": 7.460977384184483 }, { - "x": 1.655331933364656, - "y": 3.9350953770599486, - "heading": -0.5183992946484082, - "angularVelocity": 2.1816580536898917, - "velocityX": -0.015606763447832819, - "velocityY": 0.5618444749110212, - "timestamp": 7.073686931552211 + "x": 8.110834888181197, + "y": 5.858201520747478, + "heading": -0.2705090334908383, + "angularVelocity": 0.43777702096967036, + "velocityX": -0.7630719054241456, + "velocityY": 0.45073537663996555, + "timestamp": 7.527140213911697 }, { - "x": 1.6623538228794565, - "y": 3.9788519154462327, - "heading": -0.3621107234214173, - "angularVelocity": 2.67072359603982, - "velocityX": 0.11999294554126883, - "velocityY": 0.7477297836419755, - "timestamp": 7.1322061176854135 + "x": 8.035095319337403, + "y": 5.897940888331321, + "heading": -0.23777201719034075, + "angularVelocity": 0.494794682081624, + "velocityX": -1.144745004953147, + "velocityY": 0.600629806005663, + "timestamp": 7.593303043638912 }, { - "x": 1.6832034861301444, - "y": 4.019420949275189, - "heading": -0.22540243054151132, - "angularVelocity": 2.336127720039156, - "velocityX": 0.35628764903239357, - "velocityY": 0.6932603904745416, - "timestamp": 7.190725303818616 + "x": 7.934094943360084, + "y": 5.947585769746669, + "heading": -0.20166340599285526, + "angularVelocity": 0.5457537313074324, + "velocityX": -1.526542567688504, + "velocityY": 0.7503439864956146, + "timestamp": 7.659465873366126 }, { - "x": 1.7146647196939016, - "y": 4.052413453417189, - "heading": -0.11878753895047192, - "angularVelocity": 1.8218792610744803, - "velocityX": 0.537622541300291, - "velocityY": 0.5637895248047576, - "timestamp": 7.249244489951819 + "x": 7.8078223399404525, + "y": 6.0071184189660025, + "heading": -0.1627692568761995, + "angularVelocity": 0.587854982578802, + "velocityX": -1.9085127395585555, + "velocityY": 0.8997899495046319, + "timestamp": 7.72562870309334 }, { - "x": 1.7541026360793814, - "y": 4.074977117129141, - "heading": -0.04081520050411857, - "angularVelocity": 1.3324234938755106, - "velocityX": 0.6739313888561295, - "velocityY": 0.3855771961795887, - "timestamp": 7.307763676085021 + "x": 7.656260799272679, + "y": 6.076509799690328, + "heading": -0.12202859757436733, + "angularVelocity": 0.6157635559090158, + "velocityX": -2.290735467220094, + "velocityY": 1.0487970513114737, + "timestamp": 7.791791532820555 }, { - "x": 1.7990903606675779, - "y": 4.0866639859913105, - "heading": 4.0044035166870527e-26, - "angularVelocity": 0.6974669881979226, - "velocityX": 0.7687688014969033, - "velocityY": 0.1997100375861074, - "timestamp": 7.366282862218224 + "x": 7.479384511526562, + "y": 6.155704239729163, + "heading": -0.08118783305769839, + "angularVelocity": 0.6172765688083961, + "velocityX": -2.6733482905034087, + "velocityY": 1.1969627110168721, + "timestamp": 7.857954362547769 }, { - "x": 1.848745584487915, - "y": 4.087520122528076, - "heading": 1.8053103807695408e-26, - "angularVelocity": -4.864634969888421e-27, - "velocityX": 0.848528954372517, - "velocityY": 0.014630014416416372, - "timestamp": 7.4248020483514265 + "x": 7.277158908396328, + "y": 6.244553850532033, + "heading": -0.04460973018212138, + "angularVelocity": 0.5528497349098663, + "velocityX": -3.0564835869928735, + "velocityY": 1.342893149661115, + "timestamp": 7.924117192274983 }, { - "x": 1.938259872491009, - "y": 4.089063493932448, - "heading": 1.6974407662610403e-26, - "angularVelocity": -2.7257088903490675e-29, - "velocityX": 1.1795648681505524, - "velocityY": 0.020337610092400867, - "timestamp": 7.500689598430333 + "x": 7.0505381507723195, + "y": 6.341424218836914, + "heading": -0.04460966986271364, + "angularVelocity": 9.116811959714027e-7, + "velocityX": -3.425197479587153, + "velocityY": 1.464120695929603, + "timestamp": 7.990280022002198 }, { - "x": 2.052895663172084, - "y": 4.09104000075836, - "heading": 1.589827161160837e-26, - "angularVelocity": 6.478274543003658e-30, - "velocityX": 1.510600758120114, - "velocityY": 0.02604520535788869, - "timestamp": 7.57657714850924 + "x": 6.821387916460742, + "y": 6.432149176298842, + "heading": -0.04460966140291972, + "angularVelocity": 1.278632421205442e-7, + "velocityX": -3.4634285633844892, + "velocityY": 1.371237563991483, + "timestamp": 8.056442851729411 }, { - "x": 2.1926529528780687, - "y": 4.093449642942828, - "heading": 1.4819575466507505e-26, - "angularVelocity": -2.7257089112437415e-29, - "velocityX": 1.8416365999517403, - "velocityY": 0.03175279979340049, - "timestamp": 7.652464698588147 + "x": 6.585463067264655, + "y": 6.503425491915177, + "heading": -0.044609652839645464, + "angularVelocity": 1.2942726746927926e-7, + "velocityX": -3.5658216277748638, + "velocityY": 1.0772863843672305, + "timestamp": 8.122605681456625 }, { - "x": 2.3575317303228487, - "y": 4.096292420291259, - "heading": 1.3740875703299928e-26, - "angularVelocity": -2.7304766323100225e-29, - "velocityX": 2.1726722930617965, - "velocityY": 0.03746039166471138, - "timestamp": 7.728352248667053 + "x": 6.34313048184035, + "y": 6.548322764309462, + "heading": -0.044609643757262056, + "angularVelocity": 1.3727320078677108e-7, + "velocityX": -3.662669605024884, + "velocityY": 0.6785875480144041, + "timestamp": 8.188768511183838 }, { - "x": 2.497289491602916, - "y": 4.0987020706064285, - "heading": 1.2662183176306477e-26, - "angularVelocity": -2.720941189252962e-29, - "velocityX": 1.8416428140683072, - "velocityY": 0.031752906934841556, - "timestamp": 7.80423979874596 + "x": 6.097330137085011, + "y": 6.566295976512413, + "heading": -0.04460963356805428, + "angularVelocity": 1.5400199501081406e-7, + "velocityX": -3.715082105296292, + "velocityY": 0.27165120169518303, + "timestamp": 8.254931340911051 }, { - "x": 2.611925763329844, - "y": 4.10067858572635, - "heading": 1.1583487031205877e-26, - "angularVelocity": -2.725708910890731e-29, - "velocityX": 1.510607097049927, - "velocityY": 0.02604531465131241, - "timestamp": 7.880127348824867 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.04460962128616813, + "angularVelocity": 1.8563121019327388e-7, + "velocityX": -3.722421790171331, + "velocityY": -0.13858200516588748, + "timestamp": 8.321094170638265 }, { - "x": 2.7014405355788997, - "y": 4.1022219654799095, - "heading": 1.0506887852514804e-26, - "angularVelocity": 3.7545940988338324e-31, - "velocityX": 1.1795712492494417, - "velocityY": 0.02033772011288781, - "timestamp": 7.956014898903773 + "x": 5.594131845425269, + "y": 6.517387984060631, + "heading": -0.0446096110934515, + "angularVelocity": 1.4604851016203596e-7, + "velocityX": -3.6812230501933914, + "velocityY": -0.5694089339833474, + "timestamp": 8.390884109764682 }, { - "x": 2.7658338049487097, - "y": 4.103332209808455, - "heading": 9.428654835122248e-27, - "angularVelocity": -2.1154273725241504e-29, - "velocityX": 0.8485353566277292, - "velocityY": 0.014630124801672608, - "timestamp": 8.031902448982681 + "x": 5.343562863816817, + "y": 6.448118054894418, + "heading": -0.04460960236123058, + "angularVelocity": 1.2512148631224918e-7, + "velocityX": -3.590330995339737, + "velocityY": -0.9925489265829068, + "timestamp": 8.4606740488911 }, { - "x": 2.8051055697205705, - "y": 4.10400931868236, - "heading": 8.349263998410887e-27, - "angularVelocity": -3.641131282577579e-29, - "velocityX": 0.5174994413579915, - "velocityY": 0.008922529099968745, - "timestamp": 8.107789999061588 + "x": 5.102719675691794, + "y": 6.350252358061884, + "heading": -0.04460958274491264, + "angularVelocity": 2.810765876662287e-7, + "velocityX": -3.4509728929374397, + "velocityY": -1.4022894711981222, + "timestamp": 8.530463988017518 }, { - "x": 2.819255828857422, - "y": 4.10425329208374, - "heading": 5.817602179940264e-27, - "angularVelocity": -1.9173555419457397e-26, - "velocityX": 0.18646351242249948, - "velocityY": 0.0032149331626451292, - "timestamp": 8.183677549140494 + "x": 4.88810249039924, + "y": 6.239912108360582, + "heading": -0.010754389574110686, + "angularVelocity": 0.4851013426086585, + "velocityX": -3.0751880282313135, + "velocityY": -1.5810337576227271, + "timestamp": 8.600253927143935 }, { - "x": 2.810470934258171, - "y": 4.100290875532643, - "heading": -0.013665725456507695, - "angularVelocity": -0.18841665432335325, - "velocityX": -0.12112203294600554, - "velocityY": -0.054631952907971924, - "timestamp": 8.256206835394856 + "x": 4.7014054819057804, + "y": 6.141094298946399, + "heading": 0.022653281130584686, + "angularVelocity": 0.47868892168225635, + "velocityX": -2.6751278311803, + "velocityY": -1.415932019014718, + "timestamp": 8.670043866270353 }, { - "x": 2.7793765513907402, - "y": 4.0921324527065925, - "heading": -0.04098102601248858, - "angularVelocity": -0.37661063505003184, - "velocityX": -0.4287148608960741, - "velocityY": -0.11248453207493378, - "timestamp": 8.328736121649218 + "x": 4.541859398687963, + "y": 6.055486217406872, + "heading": 0.05282297144244177, + "angularVelocity": 0.43229283030620586, + "velocityX": -2.286090018345113, + "velocityY": -1.2266536210105516, + "timestamp": 8.73983380539677 }, { - "x": 2.7259718598142455, - "y": 4.079777215271425, - "heading": -0.08192047187969453, - "angularVelocity": -0.5644540017067066, - "velocityX": -0.7363190007027361, - "velocityY": -0.17034825617665642, - "timestamp": 8.40126540790358 + "x": 4.409161403799561, + "y": 5.983676432778196, + "heading": 0.07876234420420766, + "angularVelocity": 0.37167782471881955, + "velocityX": -1.9013914691633662, + "velocityY": -1.0289417862737966, + "timestamp": 8.809623744523188 }, { - "x": 2.650255757673548, - "y": 4.0632237114983365, - "heading": -0.13644940291380203, - "angularVelocity": -0.7518194904451844, - "velocityX": -1.0439383323745826, - "velocityY": -0.22823199603860284, - "timestamp": 8.473794694157942 + "x": 4.303150560780497, + "y": 5.925963754570679, + "heading": 0.09996559761955173, + "angularVelocity": 0.3038153304151236, + "velocityX": -1.5189989323107926, + "velocityY": -0.8269483958565483, + "timestamp": 8.879413683649606 }, { - "x": 2.5522269795422527, - "y": 4.04246951947779, - "heading": -0.20452753616691674, - "angularVelocity": -0.9386295766700757, - "velocityX": -1.3515751111558711, - "velocityY": -0.28614912806063464, - "timestamp": 8.546323980412303 + "x": 4.223727158834788, + "y": 5.882528846842074, + "heading": 0.11612624955874849, + "angularVelocity": 0.23156134167022593, + "velocityX": -1.1380351228253833, + "velocityY": -0.6223663220271061, + "timestamp": 8.949203622776023 }, { - "x": 2.4318844030868068, - "y": 4.017510913730928, - "heading": -0.28611931872413315, - "angularVelocity": -1.1249494758720198, - "velocityX": -1.6592273641491695, - "velocityY": -0.344117625249086, - "timestamp": 8.618853266666665 + "x": 4.170823380513197, + "y": 5.853492711046949, + "heading": 0.1270379483840901, + "angularVelocity": 0.15635059955527503, + "velocityX": -0.7580430500986731, + "velocityY": -0.41605045309653765, + "timestamp": 9.018993561902441 }, { - "x": 2.2892276108991876, - "y": 3.988342694775403, - "heading": -0.38121408025371734, - "angularVelocity": -1.3111222575124248, - "velocityX": -1.9668853721697708, - "velocityY": -0.40215781047715543, - "timestamp": 8.691382552921027 + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.0789970249504139, + "velocityX": -0.37875479822591473, + "velocityY": -0.2084922310471305, + "timestamp": 9.088783501028859 }, { - "x": 2.133082954891742, - "y": 3.958763429178337, - "heading": -0.47806319392600216, - "angularVelocity": -1.335310447322373, - "velocityX": -2.1528497531306416, - "velocityY": -0.4078251300216943, - "timestamp": 8.763911839175389 + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.1741097503964344e-26, + "velocityX": -8.106670406309328e-26, + "velocityY": -1.6795580659596768e-26, + "timestamp": 9.158573440155276 }, { - "x": 1.999249779098319, - "y": 3.933404232877465, - "heading": -0.561296583651025, - "angularVelocity": -1.1475831905076381, - "velocityX": -1.8452294611595452, - "velocityY": -0.3496407811313142, - "timestamp": 8.83644112542975 + "x": 4.149229218776726, + "y": 5.815100726386144, + "heading": 0.12918022174703248, + "angularVelocity": -0.05380546988265158, + "velocityX": 0.07724016042252664, + "velocityY": -0.38054657832352085, + "timestamp": 9.221223654099454 }, { - "x": 1.8877256766061599, - "y": 3.91226852016899, - "heading": -0.6308164080928251, - "angularVelocity": -0.9585069429470503, - "velocityX": -1.5376423545799296, - "velocityY": -0.29140935751595565, - "timestamp": 8.908970411684113 + "x": 4.159717289478366, + "y": 5.767595641795523, + "heading": 0.12259706469587855, + "angularVelocity": -0.1050779660069413, + "velocityX": 0.16740678189839092, + "velocityY": -0.758258936401222, + "timestamp": 9.283873868043631 }, { - "x": 1.7985083626535399, - "y": 3.8953584872647626, - "heading": -0.6865270511993388, - "angularVelocity": -0.7681123858179831, - "velocityX": -1.2300867492302674, - "velocityY": -0.2331476535550589, - "timestamp": 8.981499697938474 + "x": 4.176867312970839, + "y": 5.696690246498273, + "heading": 0.11300853276979762, + "angularVelocity": -0.1530486701070873, + "velocityX": 0.2737424569332407, + "velocityY": -1.1317662116912734, + "timestamp": 9.346524081987809 }, { - "x": 1.7315960620068493, - "y": 3.8826753796533127, - "heading": -0.7283506616380105, - "angularVelocity": -0.57664445079461, - "velocityX": -0.9225556199743571, - "velocityY": -0.17486877737869985, - "timestamp": 9.054028984192836 + "x": 4.201974040585746, + "y": 5.602790592825652, + "heading": 0.10069220099176118, + "angularVelocity": -0.19658882233044642, + "velocityX": 0.4007444832874458, + "velocityY": -1.4987922268914513, + "timestamp": 9.409174295931987 }, { - "x": 1.68698774113937, - "y": 3.874219809689345, - "heading": -0.7562373749577054, - "angularVelocity": -0.38448900795597707, - "velocityX": -0.6150387405032061, - "velocityY": -0.11658145834103056, - "timestamp": 9.126558270447198 + "x": 4.236730391014469, + "y": 5.486551757326023, + "heading": 0.08603276386217854, + "angularVelocity": -0.2339886204162749, + "velocityX": 0.5547682639949316, + "velocityY": -1.8553621477366304, + "timestamp": 9.471824509876164 }, { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": -0.1921095648592989, - "velocityX": -0.3075243052917468, - "velocityY": -0.05829082515090241, - "timestamp": 9.19908755670156 + "x": 4.283393708275682, + "y": 5.349087774330418, + "heading": 0.06958159023755481, + "angularVelocity": -0.2625876687233969, + "velocityX": 0.7448229514872947, + "velocityY": -2.194150256503298, + "timestamp": 9.534474723820342 }, { - "x": 1.664683222770691, - "y": 3.869992017745972, - "heading": -0.7701709445795863, - "angularVelocity": 4.278722998474624e-26, - "velocityX": 4.546149419559626e-26, - "velocityY": -6.368263045423934e-27, - "timestamp": 9.271616842955922 + "x": 4.344983097190331, + "y": 5.19239819137041, + "heading": 0.0521501823955064, + "angularVelocity": -0.27823381190014773, + "velocityX": 0.983067495500113, + "velocityY": -2.501022312543423, + "timestamp": 9.59712493776452 }, { - "x": 1.6770242080176545, - "y": 3.8595866692257377, - "heading": -0.7649877966744867, - "angularVelocity": 0.08507499669287022, - "velocityX": 0.20256209128031585, - "velocityY": -0.17079099557937222, - "timestamp": 9.332541299101496 + "x": 4.425324635342364, + "y": 5.020190178284826, + "heading": 0.03492497946543372, + "angularVelocity": -0.2749424438585406, + "velocityX": 1.2823825026937383, + "velocityY": -2.748721867718171, + "timestamp": 9.659775151708697 }, { - "x": 1.7017080065350787, - "y": 3.838774596868545, - "heading": -0.7547380668498572, - "angularVelocity": 0.16823670612895278, - "velocityX": 0.40515418731755265, - "velocityY": -0.3416045652908843, - "timestamp": 9.393465755247071 + "x": 4.5282801412548075, + "y": 4.838988810812342, + "heading": 0.019478906624106312, + "angularVelocity": -0.24654461443802958, + "velocityX": 1.6433384569792093, + "velocityY": -2.892270529737325, + "timestamp": 9.722425365652875 }, { - "x": 1.738736696509759, - "y": 3.8075542186994835, - "heading": -0.739558172921761, - "angularVelocity": 0.2491592849319001, - "velocityX": 0.6077803942345016, - "velocityY": -0.5124441011744508, - "timestamp": 9.454390211392646 + "x": 4.655561045937701, + "y": 4.65789803685685, + "heading": 0.007446897298022715, + "angularVelocity": -0.1920505704386623, + "velocityX": 2.031611652536461, + "velocityY": -2.890505276116785, + "timestamp": 9.785075579597052 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -3.0154063280737095e-24, + "angularVelocity": -0.11886467466269138, + "velocityX": 2.394177429870417, + "velocityY": -2.752084488523512, + "timestamp": 9.84772579354123 }, { - "x": 1.7881126694173817, - "y": 3.76592371429674, - "heading": -0.7196097205107205, - "angularVelocity": 0.32742930627685823, - "velocityX": 0.8104458542829184, - "velocityY": -0.6833135170426513, - "timestamp": 9.51531466753822 + "x": 5.0265563719767545, + "y": 4.290478604211027, + "heading": -0.0011711585949722948, + "angularVelocity": -0.014781576411206173, + "velocityX": 2.789308345003365, + "velocityY": -2.461168374898756, + "timestamp": 9.926956760685195 }, { - "x": 1.849838711755514, - "y": 3.713880986159023, - "heading": -0.6950869960421026, - "angularVelocity": 0.4025103549553654, - "velocityX": 1.0131570512610228, - "velocityY": -0.8542173608142635, - "timestamp": 9.576239123683795 + "x": 5.271648004487176, + "y": 4.1260554019145586, + "heading": -0.0011711658442560332, + "angularVelocity": -9.149558562553705e-8, + "velocityX": 3.0933818094771004, + "velocityY": -2.075239117019812, + "timestamp": 10.006187727829161 }, { - "x": 1.923918112020319, - "y": 3.6514236137874088, - "heading": -0.6662278740694468, - "angularVelocity": 0.4736869854644038, - "velocityX": 1.2159222248582278, - "velocityY": -1.025160934098061, - "timestamp": 9.63716357982937 + "x": 5.5362482788476575, + "y": 3.9953205544154557, + "heading": -0.0011711670074451403, + "angularVelocity": -1.4680990893194224e-8, + "velocityX": 3.3396067711718436, + "velocityY": -1.650047351581014, + "timestamp": 10.085418694973127 }, { - "x": 2.010354801057128, - "y": 3.578548802416206, - "heading": -0.6333303833823517, - "angularVelocity": 0.5399718400192002, - "velocityX": 1.4187519184459274, - "velocityY": -1.1961503800226454, - "timestamp": 9.698088035974944 + "x": 5.815753541872799, + "y": 3.900548632610082, + "heading": -0.0011711678822933615, + "angularVelocity": -1.1041746080675712e-8, + "velocityX": 3.5277275174146143, + "velocityY": -1.196147481491295, + "timestamp": 10.164649662117093 }, { - "x": 2.109153532122565, - "y": 3.4952533361865883, - "heading": -0.5967791996957547, - "angularVelocity": 0.5999427159310254, - "velocityX": 1.6216596308937807, - "velocityY": -1.367192610313814, - "timestamp": 9.759012492120519 + "x": 6.105300821187424, + "y": 3.8433884686557884, + "heading": -0.001171168612339158, + "angularVelocity": -9.214147233371614e-9, + "velocityX": 3.654471095733393, + "velocityY": -0.7214371604278238, + "timestamp": 10.243880629261058 }, { - "x": 2.2203200891736463, - "y": 3.4015335717559703, - "heading": -0.5570908397473109, - "angularVelocity": 0.6514356049992648, - "velocityX": 1.824662279880775, - "velocityY": -1.5382946415915537, - "timestamp": 9.819936948266093 + "x": 6.399852439542986, + "y": 3.8248343191470044, + "heading": -0.0011711692753051952, + "angularVelocity": -8.367511605727905e-9, + "velocityX": 3.717632498671273, + "velocityY": -0.2341780010721126, + "timestamp": 10.323111596405024 }, { - "x": 2.3438614224783176, - "y": 3.2973856025788515, - "heading": -0.5149975235515339, - "angularVelocity": 0.690910003286656, - "velocityX": 2.0277790089660783, - "velocityY": -1.7094607940079831, - "timestamp": 9.880861404411668 + "x": 6.694725372224164, + "y": 3.8372788433696505, + "heading": -0.0011711699217424045, + "angularVelocity": -8.15889585625018e-9, + "velocityX": 3.721687912068309, + "velocityY": 0.15706641823560874, + "timestamp": 10.40234256354899 }, { - "x": 2.4797851741645616, - "y": 3.182806114592359, - "heading": -0.47162192283324345, - "angularVelocity": 0.7119571262917361, - "velocityX": 2.231021174181074, - "velocityY": -1.8806813426895879, - "timestamp": 9.941785860557243 + "x": 6.9895982402615005, + "y": 3.8497248992335154, + "heading": -0.0011711705681793068, + "angularVelocity": -8.15889197720244e-9, + "velocityX": 3.7216870961771913, + "velocityY": 0.157085749581342, + "timestamp": 10.481573530692955 }, { - "x": 2.628095553772068, - "y": 3.057796514272803, - "heading": -0.4289105902248852, - "angularVelocity": 0.7010539824319861, - "velocityX": 2.4343324338116106, - "velocityY": -2.0518788057927626, - "timestamp": 10.002710316702817 + "x": 7.283138211598938, + "y": 3.880370995696298, + "heading": -0.0011711735014489986, + "angularVelocity": -3.702175800028382e-8, + "velocityX": 3.7048641701427636, + "velocityY": 0.386794426061941, + "timestamp": 10.560804497836921 }, { - "x": 2.788761933089725, - "y": 2.9223901324293977, - "heading": -0.3910891505574595, - "angularVelocity": 0.6207924052215403, - "velocityX": 2.6371409690347543, - "velocityY": -2.222529184665362, - "timestamp": 10.063634772848392 + "x": 7.537795284833578, + "y": 3.9128988141490386, + "heading": -0.0022060584164621646, + "angularVelocity": -0.01306162164009358, + "velocityX": 3.2141103714147428, + "velocityY": 0.4105442559300831, + "timestamp": 10.640035464980887 }, { - "x": 2.9612506555206113, - "y": 2.7772643724590593, - "heading": -0.37520163462976347, - "angularVelocity": 0.2607740295577497, - "velocityX": 2.8311901877094443, - "velocityY": -2.382060820101041, - "timestamp": 10.124559228993967 + "x": 7.753528037916665, + "y": 3.9448039763834073, + "heading": -0.003047966804873468, + "angularVelocity": -0.010626001660203437, + "velocityX": 2.722833771435503, + "velocityY": 0.40268550775602213, + "timestamp": 10.719266432124853 }, { - "x": 3.1373758975337274, - "y": 2.6287035614867915, - "heading": -0.37520161826362025, - "angularVelocity": 2.686301076874653e-7, - "velocityX": 2.8908791831030336, - "velocityY": -2.4384429565902384, - "timestamp": 10.185483685139541 + "x": 7.930345641638023, + "y": 3.9756628799518334, + "heading": -0.0034891865809292344, + "angularVelocity": -0.005568779379583401, + "velocityX": 2.23167291899988, + "velocityY": 0.3894803342783207, + "timestamp": 10.798497399268818 }, { - "x": 3.313501125191111, - "y": 2.480142733495068, - "heading": -0.3752016018976461, - "angularVelocity": 2.686273330621961e-7, - "velocityX": 2.8908789474713403, - "velocityY": -2.4384432359436565, - "timestamp": 10.246408141285116 + "x": 8.068253246603454, + "y": 4.005300685276172, + "heading": -0.003443969014949724, + "angularVelocity": 0.0005707057178457502, + "velocityX": 1.7405770740479334, + "velocityY": 0.3740684531905991, + "timestamp": 10.877728366412784 }, { - "x": 3.4896263528475178, - "y": 2.3315819055021865, - "heading": -0.37520158553167193, - "angularVelocity": 2.686273331449891e-7, - "velocityX": 2.890878947455303, - "velocityY": -2.4384432359626693, - "timestamp": 10.30733259743069 + "x": 8.167254001972685, + "y": 4.033621928989228, + "heading": -0.0028654925288464014, + "angularVelocity": 0.007301141295577129, + "velocityX": 1.2495209756728338, + "velocityY": 0.3574516976625533, + "timestamp": 10.95695933355675 }, { - "x": 3.665751580503926, - "y": 2.183021077509307, - "heading": -0.3752015691656978, - "angularVelocity": 2.6862733313245716e-7, - "velocityX": 2.8908789474553287, - "velocityY": -2.438443235962638, - "timestamp": 10.368257053576265 + "x": 8.227350013878981, + "y": 4.060566470670501, + "heading": -0.0017242628170792867, + "angularVelocity": 0.014403834168696394, + "velocityX": 0.7584914594958708, + "velocityY": 0.340075890179585, + "timestamp": 11.036190300700715 }, { - "x": 3.841876808185214, - "y": 2.0344602495459236, - "heading": -0.37520155279972356, - "angularVelocity": 2.6862733360962026e-7, - "velocityX": 2.890878947863701, - "velocityY": -2.4384432354784953, - "timestamp": 10.42918150972184 + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 1.0796890687236353e-23, + "angularVelocity": 0.021762486048494588, + "velocityX": 0.2674809172408869, + "velocityY": 0.32217804683224427, + "timestamp": 11.115421267844681 }, { - "x": 4.018002401412412, - "y": 1.8858998549539654, - "heading": -0.3752015364337498, - "angularVelocity": 2.686273261605019e-7, - "velocityX": 2.8908849478501355, - "velocityY": -2.4384361222196733, - "timestamp": 10.490105965867414 + "x": 8.231523012343859, + "y": 4.109892958457586, + "heading": 0.0022863134326364947, + "angularVelocity": 0.029215203502289427, + "velocityX": -0.21748380315842067, + "velocityY": 0.3041237094875908, + "timestamp": 11.193678926822047 }, { - "x": 4.198462016558864, - "y": 1.7426352651038108, - "heading": -0.375201520037625, - "angularVelocity": 2.691222177389301e-7, - "velocityX": 2.962022586057329, - "velocityY": -2.351512002139716, - "timestamp": 10.551030422012989 + "x": 8.176551036095814, + "y": 4.132280075994507, + "heading": 0.005155847293198806, + "angularVelocity": 0.03666777026121207, + "velocityX": -0.7024485138757453, + "velocityY": 0.2860693487316991, + "timestamp": 11.271936585799413 }, { - "x": 4.3885379093915455, - "y": 1.6123986719437928, - "heading": -0.3752015033732071, - "angularVelocity": 2.7352591969451763e-7, - "velocityX": 3.119861954590255, - "velocityY": -2.1376734631627445, - "timestamp": 10.611954878158564 + "x": 8.083626857911096, + "y": 4.153254299063425, + "heading": 0.008608587113368522, + "angularVelocity": 0.04412015213959263, + "velocityX": -1.1874132116780545, + "velocityY": 0.26801495652947127, + "timestamp": 11.350194244776779 }, { - "x": 4.587291846391845, - "y": 1.49583415162556, - "heading": -0.37520148607296444, - "angularVelocity": 2.839622014925957e-7, - "velocityX": 3.2623013741048372, - "velocityY": -1.9132632064816424, - "timestamp": 10.672879334304138 + "x": 7.952750479206828, + "y": 4.172815624183239, + "heading": 0.012644515987955914, + "angularVelocity": 0.051572317998353566, + "velocityX": -1.6723778913719014, + "velocityY": 0.24996051984474485, + "timestamp": 11.428451903754144 }, { - "x": 4.7937421798706055, - "y": 1.3935176134109497, - "heading": -0.3752014675107609, - "angularVelocity": 3.0467573669290407e-7, - "velocityX": 3.3886282543985438, - "velocityY": -1.679400107735582, - "timestamp": 10.733803790449713 + "x": 7.783921902112919, + "y": 4.190964046074225, + "heading": 0.017263614398582115, + "angularVelocity": 0.059024234445374815, + "velocityX": -2.1573425438491505, + "velocityY": 0.2319060156940586, + "timestamp": 11.50670956273151 }, { - "x": 5.064781872639773, - "y": 1.2879640439263618, - "heading": -0.3752014510522667, - "angularVelocity": 2.1399857496737658e-7, - "velocityX": 3.52414428888234, - "velocityY": -1.3724410814878705, - "timestamp": 10.810713153560702 + "x": 7.57714113018582, + "y": 4.207699555855796, + "heading": 0.022465858708652038, + "angularVelocity": 0.06647584885684583, + "velocityX": -2.642307150880996, + "velocityY": 0.2138513980645823, + "timestamp": 11.584967221708876 }, { - "x": 5.344110469349436, - "y": 1.2068492677876004, - "heading": -0.3752014356488163, - "angularVelocity": 2.0028056132299027e-7, - "velocityX": 3.631919254181875, - "velocityY": -1.0546801177082226, - "timestamp": 10.88762251667169 + "x": 7.332408170546637, + "y": 4.223022135701224, + "heading": 0.028251214754351595, + "angularVelocity": 0.07392702671278313, + "velocityX": -3.1272716669171876, + "velocityY": 0.19579655263979157, + "timestamp": 11.663224880686242 }, { - "x": 5.629529124295685, - "y": 1.1508112126364383, - "heading": -0.37520142072574625, - "angularVelocity": 1.940345031855091e-7, - "velocityX": 3.711104128301771, - "velocityY": -0.7286246158389513, - "timestamp": 10.96453187978268 + "x": 7.049723044551417, + "y": 4.23693173264099, + "heading": 0.03461960291604749, + "angularVelocity": 0.08137718716500116, + "velocityX": -3.6122359100593893, + "velocityY": 0.1777410303544707, + "timestamp": 11.741482539663608 }, { - "x": 5.917283115305387, - "y": 1.1083646233041369, - "heading": -0.3752014058811779, - "angularVelocity": 1.9301379957946635e-7, - "velocityX": 3.7414689105466086, - "velocityY": -0.5519040545303511, - "timestamp": 11.041441242893669 + "x": 6.758414971904035, + "y": 4.226087741393702, + "heading": 0.03461960351130125, + "angularVelocity": 7.606332300457526e-9, + "velocityX": -3.7224225264857154, + "velocityY": -0.13856779501191718, + "timestamp": 11.819740198640973 }, { - "x": 6.205037197250251, - "y": 1.0659186504442004, - "heading": -0.37520139103661015, - "angularVelocity": 1.9301379056768152e-7, - "velocityX": 3.7414700929145033, - "velocityY": -0.5518960389605939, - "timestamp": 11.118350606004658 + "x": 6.467106926605873, + "y": 4.215243015473263, + "heading": 0.03461960410653643, + "angularVelocity": 7.606094897280975e-9, + "velocityX": -3.7224221770091352, + "velocityY": -0.13857718288730186, + "timestamp": 11.89799785761834 }, { - "x": 6.492791277445097, - "y": 1.0234726657206588, - "heading": -0.37520137619194305, - "angularVelocity": 1.9301508362380675e-7, - "velocityX": 3.741470070160208, - "velocityY": -0.5518961932149626, - "timestamp": 11.195259969115646 + "x": 6.175798881305749, + "y": 4.204398289605515, + "heading": 0.03461960470177156, + "angularVelocity": 7.606094342014038e-9, + "velocityX": -3.7224221770342005, + "velocityY": -0.1385771822140065, + "timestamp": 11.976255516595705 }, { - "x": 6.772643212230185, - "y": 0.9818724107444464, - "heading": -0.34826678356511775, - "angularVelocity": 0.35021213981392485, - "velocityX": 3.638723862284922, - "velocityY": -0.5408997460579521, - "timestamp": 11.272169332226635 + "x": 5.8844906838954465, + "y": 4.193557650451513, + "heading": 0.03461960529700769, + "angularVelocity": 7.606107036408581e-9, + "velocityX": -3.7224241207439843, + "velocityY": -0.13852496095158084, + "timestamp": 12.05451317557307 }, { - "x": 7.027508446069354, - "y": 0.9440808910715871, - "heading": -0.30010699502671095, - "angularVelocity": 0.6261888876768776, - "velocityX": 3.313838829628223, - "velocityY": -0.49137735828500057, - "timestamp": 11.349078695337624 + "x": 5.594009037113428, + "y": 4.218019822631225, + "heading": 0.034619605911650746, + "angularVelocity": 7.854094608871324e-9, + "velocityX": -3.7118622071998786, + "velocityY": 0.31258502361778046, + "timestamp": 12.132770834550437 }, { - "x": 7.256864076564165, - "y": 0.9101013911553985, - "heading": -0.24860599276707973, - "angularVelocity": 0.66963241114492, - "velocityX": 2.9821548536792237, - "velocityY": -0.44181226500540305, - "timestamp": 11.425988058448613 + "x": 5.309172831526295, + "y": 4.280038516789021, + "heading": 0.034619599952821836, + "angularVelocity": -7.614371543607384e-8, + "velocityX": -3.639723054704678, + "velocityY": 0.7924936034150137, + "timestamp": 12.211028493527802 }, { - "x": 7.460696487188184, - "y": 0.8799184472229736, - "heading": -0.19849831332672846, - "angularVelocity": 0.6515159847057984, - "velocityX": 2.6502938313228213, - "velocityY": -0.39244823661935213, - "timestamp": 11.502897421559602 + "x": 5.03935943827081, + "y": 4.376755129785837, + "heading": 0.01971114929830447, + "angularVelocity": -0.19050468477250748, + "velocityX": -3.4477570218849003, + "velocityY": 1.235874089011402, + "timestamp": 12.289286152505168 }, { - "x": 7.639017939225139, - "y": 0.8535218749551334, - "heading": -0.15201987225869848, - "angularVelocity": 0.6043274731186726, - "velocityX": 2.3185922340771317, - "velocityY": -0.3432166279903691, - "timestamp": 11.579806784670591 + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -1.0813807424036439e-23, + "angularVelocity": -0.25187501844395294, + "velocityX": -2.987601050001106, + "velocityY": 1.389310982379468, + "timestamp": 12.367543811482534 }, { - "x": 7.7918411996459325, - "y": 0.8309051352911735, - "heading": -0.11048043087448678, - "angularVelocity": 0.5401090283931487, - "velocityX": 1.987056637047598, - "velocityY": -0.29407004230839406, - "timestamp": 11.65671614778158 + "x": 4.627457882261689, + "y": 4.587679677136334, + "heading": -0.019778853338755203, + "angularVelocity": -0.2848710655643308, + "velocityX": -2.5651244910957094, + "velocityY": 1.4719718180672714, + "timestamp": 12.43697470752518 }, { - "x": 7.919176921526017, - "y": 0.8120637954987597, - "heading": -0.0747425922933183, - "angularVelocity": 0.46467474356269306, - "velocityX": 1.655659554693303, - "velocityY": -0.24498109242204588, - "timestamp": 11.733625510892569 + "x": 4.478940134439646, + "y": 4.685562428002016, + "heading": -0.03941675041867698, + "angularVelocity": -0.28284089935782175, + "velocityX": -2.1390728953118847, + "velocityY": 1.4097866575934486, + "timestamp": 12.506405603567828 }, { - "x": 8.021033730291848, - "y": 0.7969946793008421, - "heading": -0.045418027645302235, - "angularVelocity": 0.3812873161580816, - "velocityX": 1.3243746228770636, - "velocityY": -0.19593344150011263, - "timestamp": 11.810534874003558 + "x": 4.358424662792702, + "y": 4.772986561558444, + "heading": -0.05736759927865439, + "angularVelocity": -0.2585426644782356, + "velocityX": -1.7357614335398788, + "velocityY": 1.2591531802027813, + "timestamp": 12.575836499610475 }, { - "x": 8.097418632050843, - "y": 0.7856953915277307, - "heading": -0.022962936429777186, - "angularVelocity": 0.29196823776995817, - "velocityX": 0.9931807866977714, - "velocityY": -0.14691693333626094, - "timestamp": 11.887444237114547 + "x": 4.264251963016929, + "y": 4.846292896298891, + "heading": -0.07266630938102059, + "angularVelocity": -0.220344414005101, + "velocityX": -1.3563514968599655, + "velocityY": 1.0558172070171679, + "timestamp": 12.645267395653121 + }, + { + "x": 4.19504667897931, + "y": 4.903178017800909, + "heading": -0.0846829764598402, + "angularVelocity": -0.17307377210627528, + "velocityX": -0.9967505531703156, + "velocityY": 0.8193055936809015, + "timestamp": 12.714698291695768 }, { - "x": 8.148337378443449, - "y": 0.7781640366357435, - "heading": -0.007730140883461889, - "angularVelocity": 0.198061652445786, - "velocityX": 0.6620617351768197, - "velocityY": -0.09792507163423873, - "timestamp": 11.964353600225536 + "x": 4.149718350868284, + "y": 4.942098244876492, + "heading": -0.09298341563379672, + "angularVelocity": -0.11954964788093829, + "velocityX": -0.6528552948990191, + "velocityY": 0.5605606335784127, + "timestamp": 12.784129187738415 }, { - "x": 8.173794746398926, - "y": 0.7743990421295166, - "heading": -6.501598853172508e-27, - "angularVelocity": 0.10050975031878098, - "velocityX": 0.33100479480942824, - "velocityY": -0.04895365601706529, - "timestamp": 12.041262963336525 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.06149221603273053, + "velocityX": -0.32142062647019837, + "velocityY": 0.2860902346287965, + "timestamp": 12.853560083781062 }, { - "x": 8.173794746398926, - "y": 0.7743990421295166, - "heading": -3.1768871176909477e-27, - "angularVelocity": 1.9243307208194474e-27, - "velocityX": 5.67400379522632e-27, - "velocityY": -1.0775303089806919e-26, - "timestamp": 12.118172326447514 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -7.045263803296892e-25, + "velocityX": -1.4084469220925396e-25, + "velocityY": -2.7940612123980634e-25, + "timestamp": 12.922990979823709 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 0.43324199318885803, - "y": 4.121134281158447, + "x": 0.43297290802001953, + "y": 6.9807281494140625, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 6 }, { - "timestamp": 1.4277752931645427, + "timestamp": 0.34959777724704905, + "isStopPoint": false, + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 1.1040901645422223, "isStopPoint": true, - "x": 2.3639590740203857, - "y": 3.0491905212402344, - "heading": -0.8022724119795859, + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 8 }, { - "timestamp": 2.7722119761090878, - "isStopPoint": false, - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0, + "timestamp": 1.8634374124793154, + "isStopPoint": true, + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 17 + "headingConstrained": true, + "controlIntervalCount": 20 }, { - "timestamp": 3.3986072208880596, + "timestamp": 3.2273332408433504, "isStopPoint": false, - "x": 7.626483917236328, - "y": 2.082604169845581, - "heading": 0.507098504392337, + "x": 7.125290870666504, + "y": 7.458, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 9 }, { - "timestamp": 3.9697078859421278, + "timestamp": 3.7636275381592217, "isStopPoint": false, - "x": 8.306674003601074, - "y": 2.45849871635437, - "heading": 0.507098504392337, + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 19 }, { - "timestamp": 5.220434007267402, + "timestamp": 5.598433000555465, + "isStopPoint": true, + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 6.3874526467093045, "isStopPoint": false, - "x": 5.5030035972595215, - "y": 1.563418984413147, + "x": 5.851044178009033, + "y": 6.557126998901367, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 25 + "controlIntervalCount": 12 }, { - "timestamp": 6.898129373152603, - "isStopPoint": true, - "x": 1.664683222770691, - "y": 3.8699920177459717, - "heading": -0.7701709445795863, + "timestamp": 6.776829703600045, + "isStopPoint": false, + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 9 }, { - "timestamp": 7.4248020483514265, + "timestamp": 7.328651724730054, "isStopPoint": false, - "x": 1.848745584487915, - "y": 4.087520122528076, + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 8.321094170638265, + "isStopPoint": false, + "x": 5.851044178009033, + "y": 6.557126998901367, "heading": 0, "isInitialGuess": false, "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "timestamp": 9.158573440155276, + "isStopPoint": true, + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "isInitialGuess": false, + "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 11 }, { - "timestamp": 8.183677549140494, + "timestamp": 9.84772579354123, "isStopPoint": false, - "x": 2.819255828857422, - "y": 4.10425329208374, + "x": 4.805556774139404, + "y": 4.485479354858398, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { - "timestamp": 9.271616842955922, - "isStopPoint": true, - "x": 1.664683222770691, - "y": 3.8699920177459717, - "heading": -0.7701709445795863, + "timestamp": 11.115421267844681, + "isStopPoint": false, + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 24 + "controlIntervalCount": 16 }, { - "timestamp": 10.733803790449713, + "timestamp": 12.367543811482534, "isStopPoint": false, - "x": 4.7937421798706055, - "y": 1.3935176134109497, + "x": 4.805556774139404, + "y": 4.485479354858398, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 18 + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "timestamp": 12.118172326447514, + "timestamp": 12.922990979823709, "isStopPoint": true, - "x": 8.173794746398926, - "y": 0.7743990421295166, - "heading": 0, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -26860,29 +26587,34 @@ }, { "scope": [ - 1 + 11 ], "type": "StopPoint" }, { "scope": [ - 9 + 0, + 1 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 2 ], "type": "StopPoint" }, { "scope": [ - 7, - 8 + 3 ], - "type": "ZeroAngularVelocity" + "type": "StopPoint" }, { "scope": [ - 7, - 8 + 11 ], - "type": "StraightLine" + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -26890,45 +26622,36 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": true + "isTrajectoryStale": false }, - "AmpLanePADEF": { + "CenterLanePDEABC": { "waypoints": [ { - "x": 0.43297290802001953, - "y": 6.9807281494140625, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 6 - }, - { - "x": 0.8667811155319214, - "y": 6.895569324493408, + "x": 1.3350272178649902, + "y": 5.601006507873535, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 10 }, { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, + "x": 2.3368515968322754, + "y": 6.133185386657715, + "heading": 0.2525545797921912, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 16 }, { - "x": 3.062049150466919, - "y": 7.130331993103027, - "heading": 0.5125504196, + "x": 5.747424125671387, + "y": 7.119814872741699, + "heading": 0.2525545797921912, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 20 + "controlIntervalCount": 11 }, { "x": 7.125290870666504, @@ -26949,9 +26672,9 @@ "controlIntervalCount": 19 }, { - "x": 4.144390106201172, - "y": 5.838942050933838, - "heading": 0.13255114594654763, + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -26991,38 +26714,65 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 12 + "controlIntervalCount": 13 }, { - "x": 4.144390106201172, - "y": 5.838942050933838, - "heading": 0.13255114594654763, + "x": 3.405486822128296, + "y": 6.266934394836426, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 9 }, { - "x": 4.805556774139404, - "y": 4.485479354858398, + "x": 2.205486822128296, + "y": 6.266934394836426, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 8 }, { - "x": 8.248542785644531, - "y": 4.086092948913574, + "x": 2.052253484725952, + "y": 6.674348831176758, + "heading": 0.3930579718029317, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 2.8644089698791504, + "y": 7.012746810913086, + "heading": 0.3930579718029317, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 1.7400015592575073, + "y": 6.167843341827393, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 7 + }, + { + "x": 2.0785037517547607, + "y": 5.548515796661377, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 7 }, { - "x": 4.805556774139404, - "y": 4.485479354858398, + "x": 2.6785037517547607, + "y": 5.548515796661377, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -27030,9 +26780,27 @@ "controlIntervalCount": 8 }, { - "x": 4.127401828765869, - "y": 4.96196174621582, - "heading": -0.09725287529259725, + "x": 2.0932393074035645, + "y": 4.9619622230529785, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "x": 2.2881293296813965, + "y": 4.354815483093262, + "heading": -0.3347371673945201, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -27041,1736 +26809,1842 @@ ], "trajectory": [ { - "x": 0.43297290802001953, - "y": 6.9807281494140625, - "heading": -2.0223823950436125e-24, - "angularVelocity": -1.0530930101357375e-37, - "velocityX": 9.679119214488622e-24, - "velocityY": 2.8468005585003605e-23, + "x": 1.3350272178649902, + "y": 5.601006507873535, + "heading": 1.1438500584259617e-25, + "angularVelocity": 2.0139373174315835e-26, + "velocityX": -2.9204847474037974e-26, + "velocityY": 3.705827554701837e-26, "timestamp": 0 }, { - "x": 0.45358539197474673, - "y": 6.976442037280005, - "heading": -4.051643418810716e-24, - "angularVelocity": -1.1805501930230392e-25, - "velocityX": 0.3537634154949626, - "velocityY": -0.0735607445986989, - "timestamp": 0.05826629620784151 + "x": 1.3513037651461415, + "y": 5.614995878157814, + "heading": 0.006549885022033663, + "angularVelocity": 0.1111819339605223, + "velocityX": 0.27628851481066125, + "velocityY": 0.23746451088284168, + "timestamp": 0.05891141473001675 }, { - "x": 0.49483104636042347, - "y": 6.967970532727312, - "heading": -6.080913911985301e-24, - "angularVelocity": -1.1821753877066713e-25, - "velocityX": 0.7078818643036704, - "velocityY": -0.1453928789719994, - "timestamp": 0.11653259241568302 + "x": 1.384378906601625, + "y": 5.642351283609904, + "heading": 0.019354935835911788, + "angularVelocity": 0.21736111537232614, + "velocityX": 0.5614385871916767, + "velocityY": 0.46434813316664775, + "timestamp": 0.1178228294600335 }, { - "x": 0.5567354919848906, - "y": 6.95544199101986, - "heading": -8.110182775739579e-24, - "angularVelocity": -1.1818957371343575e-25, - "velocityX": 1.0624400322898162, - "velocityY": -0.21502210579442033, - "timestamp": 0.17479888862352452 + "x": 1.4348748758417986, + "y": 5.682261570187258, + "heading": 0.0380334905813409, + "angularVelocity": 0.3170617244727609, + "velocityX": 0.8571508505030827, + "velocityY": 0.6774627083776107, + "timestamp": 0.17673424419005024 }, { - "x": 0.6393312280569997, - "y": 6.9390255615072824, - "heading": -1.013944960655158e-23, - "angularVelocity": -1.1815468317795835e-25, - "velocityX": 1.4175559705645637, - "velocityY": -0.28174829328466466, - "timestamp": 0.23306518483136604 + "x": 1.5035350329878103, + "y": 5.733639392412991, + "heading": 0.06208045701070026, + "angularVelocity": 0.4081885749911699, + "velocityX": 1.1654813835429356, + "velocityY": 0.8721199866136561, + "timestamp": 0.235645658920067 }, { - "x": 0.7426606798558012, - "y": 6.918954250671649, - "heading": -1.2168715161261342e-23, - "angularVelocity": -1.181327819705339e-25, - "velocityX": 1.7734000361068998, - "velocityY": -0.3444754882657602, - "timestamp": 0.29133148103920753 + "x": 1.5912385139518137, + "y": 5.794977395067809, + "heading": 0.09080789039928205, + "angularVelocity": 0.48763781213260343, + "velocityX": 1.4887349313530633, + "velocityY": 1.0411904541067623, + "timestamp": 0.29455707365008377 }, { - "x": 0.8667811155319214, - "y": 6.895569324493408, - "heading": -1.4479605554027417e-23, - "angularVelocity": -4.951541740242581e-24, - "velocityX": 2.130226970895328, - "velocityY": -0.4013456783802647, - "timestamp": 0.34959777724704905 + "x": 1.6989811168528033, + "y": 5.864111533799202, + "heading": 0.12324878614182731, + "angularVelocity": 0.550672495155949, + "velocityX": 1.8288917927834498, + "velocityY": 1.1735270498633483, + "timestamp": 0.35346848838010053 + }, + { + "x": 1.8277399268235333, + "y": 5.93784849269496, + "heading": 0.15800938597032024, + "angularVelocity": 0.590048634679649, + "velocityX": 2.1856343216474197, + "velocityY": 1.2516582606899704, + "timestamp": 0.4123799031101173 + }, + { + "x": 1.978020614375797, + "y": 6.011501208422767, + "heading": 0.19307007056378325, + "angularVelocity": 0.5951424652444947, + "velocityX": 2.5509604249190123, + "velocityY": 1.250228263322945, + "timestamp": 0.47129131784013406 + }, + { + "x": 2.1488551922208927, + "y": 6.078800735840598, + "heading": 0.2256402887317746, + "angularVelocity": 0.5528676966468451, + "velocityX": 2.899855293375791, + "velocityY": 1.1423851850487134, + "timestamp": 0.5302027325701508 + }, + { + "x": 2.3368515968322754, + "y": 6.133185386657715, + "heading": 0.2525545797921912, + "angularVelocity": 0.45686037559548065, + "velocityX": 3.1911711078904768, + "velocityY": 0.9231598165882506, + "timestamp": 0.5891141473001675 + }, + { + "x": 2.5473350968820796, + "y": 6.1940752301450726, + "heading": 0.25255484664390665, + "angularVelocity": 0.000004475832490919022, + "velocityX": 3.5303834818832023, + "velocityY": 1.0212890683182199, + "timestamp": 0.6487347242516341 + }, + { + "x": 2.7606743654688186, + "y": 6.255791206318409, + "heading": 0.25255482885683184, + "angularVelocity": -2.9833785118107346e-7, + "velocityX": 3.578282524176262, + "velocityY": 1.0351455710261694, + "timestamp": 0.7083553012031008 + }, + { + "x": 2.974013634055561, + "y": 6.317507182491745, + "heading": 0.2525548110697642, + "angularVelocity": -2.983377309458156e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 0.7679758781545674 + }, + { + "x": 3.1873529026423038, + "y": 6.379223158665082, + "heading": 0.25255479328269653, + "angularVelocity": -2.983377305484016e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.827596455106034 + }, + { + "x": 3.400692171229046, + "y": 6.440939134838418, + "heading": 0.2525547754956289, + "angularVelocity": -2.9833773174604553e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 0.8872170320575006 }, { - "x": 1.0109741912909123, - "y": 6.869549473554426, - "heading": 0.010039096014815363, - "angularVelocity": 0.17297490390919232, - "velocityX": 2.484465074043925, - "velocityY": -0.44832534814487107, - "timestamp": 0.4076356531928316 + "x": 3.6140314398157884, + "y": 6.502655111011755, + "heading": 0.25255475770856123, + "angularVelocity": -2.983377305928146e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.9468376090089672 }, { - "x": 1.1757265724080805, - "y": 6.8408027772938045, - "heading": 0.030114435462749452, - "angularVelocity": 0.3459006574721642, - "velocityX": 2.83870452583543, - "velocityY": -0.49530924059791875, - "timestamp": 0.4656735291386142 + "x": 3.827370708402531, + "y": 6.5643710871850915, + "heading": 0.25255473992149363, + "angularVelocity": -2.983377301697445e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.0064581859604338 }, { - "x": 1.3610383679326326, - "y": 6.809328854359183, - "heading": 0.060221992176283975, - "angularVelocity": 0.5187570396556241, - "velocityX": 3.1929458565586635, - "velocityY": -0.5422997038007265, - "timestamp": 0.5237114050843967 + "x": 4.040709976989273, + "y": 6.626087063358428, + "heading": 0.25255472213442604, + "angularVelocity": -2.983377305025075e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.0660787629119004 }, { - "x": 1.546079908472426, - "y": 6.784713461447441, - "heading": 0.1506024575679624, - "angularVelocity": 1.5572669385094218, - "velocityX": 3.188289328724817, - "velocityY": -0.4241263573246083, - "timestamp": 0.5817492810301793 + "x": 4.254049245576016, + "y": 6.687803039531764, + "heading": 0.2525547043473584, + "angularVelocity": -2.9833773095408974e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.125699339863367 }, { - "x": 1.710561311165979, - "y": 6.762828401312947, - "heading": 0.2309691373975153, - "angularVelocity": 1.3847281369261228, - "velocityX": 2.834035533057883, - "velocityY": -0.37708237556692, - "timestamp": 0.6397871569759619 + "x": 4.467388514162758, + "y": 6.749519015705101, + "heading": 0.2525546865602907, + "angularVelocity": -2.983377319947091e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.1853199168148336 }, { - "x": 1.8544824093337007, - "y": 6.743675274338349, - "heading": 0.3013204880160867, - "angularVelocity": 1.2121627380762827, - "velocityX": 2.4797788654803434, - "velocityY": -0.3300108190122258, - "timestamp": 0.6978250329217445 + "x": 4.680727782749501, + "y": 6.811234991878438, + "heading": 0.252554668773223, + "angularVelocity": -2.9833773124406734e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.2449404937663002 }, { - "x": 1.9778431850692357, - "y": 6.727255600598568, - "heading": 0.36164875379241124, - "angularVelocity": 1.0394637087112153, - "velocityX": 2.12552188937403, - "velocityY": -0.28291307137290855, - "timestamp": 0.755862908867527 + "x": 4.894067051336243, + "y": 6.872950968051774, + "heading": 0.2525546509861554, + "angularVelocity": -2.983377305581398e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.3045610707177668 }, { - "x": 2.0806437247377927, - "y": 6.713570715572485, - "heading": 0.4119419860379565, - "angularVelocity": 0.8665588019197638, - "velocityX": 1.7712664013512611, - "velocityY": -0.23579231326223515, - "timestamp": 0.8139007848133096 + "x": 5.107406319922986, + "y": 6.9346669442251105, + "heading": 0.2525546331990878, + "angularVelocity": -2.983377313652051e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.3641816476692334 }, { - "x": 2.16288415503368, - "y": 6.702621710000965, - "heading": 0.45218713038219516, - "angularVelocity": 0.6934289666602311, - "velocityX": 1.4170130962875669, - "velocityY": -0.18865276154743782, - "timestamp": 0.8719386607590922 + "x": 5.320745588509729, + "y": 6.996382920398447, + "heading": 0.2525546154120202, + "angularVelocity": -2.983377306356561e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4238022246207 }, { - "x": 2.2245645766627815, - "y": 6.694409403887936, - "heading": 0.48237322491294377, - "angularVelocity": 0.5201102562565806, - "velocityX": 1.062761526399103, - "velocityY": -0.14149908106046907, - "timestamp": 0.9299765367048748 + "x": 5.534084857096471, + "y": 7.058098896571784, + "heading": 0.2525545976249525, + "angularVelocity": -2.983377306690358e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.4834228015721667 }, { - "x": 2.265685008078599, - "y": 6.688934342765243, - "heading": 0.5024941236633007, - "angularVelocity": 0.3466856500598576, - "velocityX": 0.7085102744668164, - "velocityY": -0.09433600099024221, - "timestamp": 0.9880144126506574 + "x": 5.747424125671387, + "y": 7.119814872741699, + "heading": 0.2525545797921912, + "angularVelocity": -2.991041384854961e-7, + "velocityX": 3.5782825239779474, + "velocityY": 1.0351455709687998, + "timestamp": 1.5430433785236333 }, { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": 0.1732712607555392, - "velocityX": 0.35425724418212445, - "velocityY": -0.04716814104583245, - "timestamp": 1.0460522885964398 + "x": 5.874242280548121, + "y": 7.151641192910771, + "heading": 0.23431006193331627, + "angularVelocity": -0.5017601477559481, + "velocityX": 3.487748847149364, + "velocityY": 0.8752864413346503, + "timestamp": 1.5794044126561186 }, { - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, - "angularVelocity": -7.633423863817013e-25, - "velocityX": -5.0744692988574966e-24, - "velocityY": 1.0116864366107383e-24, - "timestamp": 1.1040901645422223 + "x": 6.000453145534325, + "y": 7.182664404015401, + "heading": 0.21336082591069744, + "angularVelocity": -0.5761452203556151, + "velocityX": 3.4710471799658134, + "velocityY": 0.853199361481138, + "timestamp": 1.615765446788604 }, { - "x": 2.3347330882331176, - "y": 6.713955255875249, - "heading": 0.5125504196, - "angularVelocity": -3.160328817220775e-17, - "velocityX": 0.51083603498144, - "velocityY": 0.292445406541253, - "timestamp": 1.199008570534359 + "x": 6.126521345623787, + "y": 7.213763203476394, + "heading": 0.192336966602575, + "angularVelocity": -0.5781975075714219, + "velocityX": 3.4671236145297133, + "velocityY": 0.8552781900449158, + "timestamp": 1.6521264809210894 }, { - "x": 2.4317085702199166, - "y": 6.7694721581924355, - "heading": 0.5125504196, - "angularVelocity": -2.914232468557734e-17, - "velocityX": 1.021672045302073, - "velocityY": 0.5848907989645907, - "timestamp": 1.2939269765264956 + "x": 6.252403333907122, + "y": 7.24487049169852, + "heading": 0.17101724585582967, + "angularVelocity": -0.5863342794119836, + "velocityX": 3.462002423381879, + "velocityY": 0.8555116476825977, + "timestamp": 1.6884875150535748 }, { - "x": 2.577171786567949, - "y": 6.852747507871406, - "heading": 0.5125504196, - "angularVelocity": -1.252738056302556e-17, - "velocityX": 1.5325079980808258, - "velocityY": 0.8773361584461287, - "timestamp": 1.3888453825186322 + "x": 6.378086189949285, + "y": 7.2759799644241046, + "heading": 0.14935632557503462, + "angularVelocity": -0.5957179381056964, + "velocityX": 3.456525894841813, + "velocityY": 0.8555717258270096, + "timestamp": 1.7248485491860601 }, { - "x": 2.771122709968306, - "y": 6.963781289278252, - "heading": 0.5125504196, - "angularVelocity": 1.2751776994126907e-16, - "velocityX": 2.0433436631502695, - "velocityY": 1.1697813532187185, - "timestamp": 1.4837637885107688 + "x": 6.503564352522718, + "y": 7.307098484942146, + "heading": 0.12734794412408884, + "angularVelocity": -0.6052738041155772, + "velocityX": 3.4508964215989995, + "velocityY": 0.8558205579262937, + "timestamp": 1.7612095833185455 }, { - "x": 2.9165859263163383, - "y": 7.047056638957223, - "heading": 0.5125504196, - "angularVelocity": 7.51589809363944e-17, - "velocityX": 1.5325079980808258, - "velocityY": 0.8773361584461287, - "timestamp": 1.5786821945029055 + "x": 6.628828810809778, + "y": 7.338228595030231, + "heading": 0.10496993550474346, + "angularVelocity": -0.6154392787017191, + "velocityX": 3.4450191331370044, + "velocityY": 0.8561392939117064, + "timestamp": 1.7975706174510309 }, { - "x": 3.0135614083031372, - "y": 7.102573541274409, - "heading": 0.5125504196, - "angularVelocity": 6.994323112372533e-17, - "velocityX": 1.021672045302073, - "velocityY": 0.5848907989645907, - "timestamp": 1.6736006004950421 + "x": 6.753876017529989, + "y": 7.3693809168618785, + "heading": 0.08222565272715081, + "angularVelocity": -0.6255125389096852, + "velocityX": 3.4390442874806206, + "velocityY": 0.8567501605740924, + "timestamp": 1.8339316515835162 }, { - "x": 3.062049150466919, - "y": 7.130331993103027, - "heading": 0.5125504196, - "angularVelocity": -4.84539672491667e-17, - "velocityX": 0.51083603498144, - "velocityY": 0.29244540654125295, - "timestamp": 1.7685190064871787 + "x": 6.878734995256494, + "y": 7.4006038692779965, + "heading": 0.05925591834951263, + "angularVelocity": -0.6317129016173038, + "velocityX": 3.4338676197042926, + "velocityY": 0.8586926406535731, + "timestamp": 1.8702926857160016 }, { - "x": 3.062049150466919, - "y": 7.130331993103027, - "heading": 0.5125504196, - "angularVelocity": 9.145149251733134e-29, - "velocityX": 3.330473944863186e-25, - "velocityY": 2.452283341762288e-25, - "timestamp": 1.8634374124793154 + "x": 7.003212758632201, + "y": 7.431690611037589, + "heading": 0.0352626014950734, + "angularVelocity": -0.6598634342196367, + "velocityX": 3.4233834747977516, + "velocityY": 0.8549465795259067, + "timestamp": 1.906653719848487 }, { - "x": 3.0904722904343656, - "y": 7.132620894810973, - "heading": 0.4951032659572505, - "angularVelocity": -0.25584290647295194, - "velocityX": 0.41679341451670293, - "velocityY": 0.03356417198946019, - "timestamp": 1.9316322038975173 + "x": 7.125290870666504, + "y": 7.458, + "heading": -1.1078640911459773e-20, + "angularVelocity": -0.9697909406699016, + "velocityX": 3.3573883402077644, + "velocityY": 0.7235599753997619, + "timestamp": 1.9430147539809723 }, { - "x": 3.1473436628615548, - "y": 7.13720088972522, - "heading": 0.4609074027399865, - "angularVelocity": -0.5014439153799775, - "velocityX": 0.8339547822417649, - "velocityY": 0.06716047984016284, - "timestamp": 1.9998269953157193 + "x": 7.338537136585377, + "y": 7.4820248133164355, + "heading": -0.00804052191695304, + "angularVelocity": -0.12790212002878093, + "velocityX": 3.392149139191805, + "velocityY": 0.38216730060631093, + "timestamp": 2.005879403254524 }, { - "x": 3.232694908233173, - "y": 7.144074686546732, - "heading": 0.4108880311553055, - "angularVelocity": -0.7334778880389725, - "velocityX": 1.2515801221269964, - "velocityY": 0.10079650774733914, - "timestamp": 2.068021786733921 + "x": 7.527981798214128, + "y": 7.500236040572311, + "heading": -0.013862931395131553, + "angularVelocity": -0.09261818121091622, + "velocityX": 3.013532467259838, + "velocityY": 0.2896894751870813, + "timestamp": 2.068744052528076 }, { - "x": 3.3465670290823137, - "y": 7.153245490430184, - "heading": 0.3463288656587193, - "angularVelocity": -0.9466876304478969, - "velocityX": 1.6698067180941178, - "velocityY": 0.1344795356468125, - "timestamp": 2.1362165781521227 + "x": 7.6935968882672885, + "y": 7.512744942179138, + "heading": -0.017658491883084444, + "angularVelocity": -0.06037670665172621, + "velocityX": 2.6344709143686895, + "velocityY": 0.1989814904143519, + "timestamp": 2.1316087018016274 }, { - "x": 3.489013918523337, - "y": 7.164717369527402, - "heading": 0.26910560921909454, - "angularVelocity": -1.132392296151419, - "velocityX": 2.0888235960349633, - "velocityY": 0.16822221842234344, - "timestamp": 2.2044113695703245 + "x": 7.835373213607451, + "y": 7.519588665029473, + "heading": -0.019490985187148625, + "angularVelocity": -0.029149821485366035, + "velocityX": 2.255263124482431, + "velocityY": 0.1088644083665287, + "timestamp": 2.194473351075179 }, { - "x": 3.6601053694274515, - "y": 7.178495513325157, - "heading": 0.18214628797045526, - "angularVelocity": -1.2751607482067742, - "velocityX": 2.5088639080205257, - "velocityY": 0.2020409991909967, - "timestamp": 2.272606160988526 + "x": 7.953306201274802, + "y": 7.52078579458168, + "heading": -0.019392463666041717, + "angularVelocity": 0.001567200680277371, + "velocityX": 1.8759825916497619, + "velocityY": 0.01904296875969824, + "timestamp": 2.2573380003487307 }, { - "x": 3.8599204497819497, - "y": 7.19458405385975, - "heading": 0.0905625121816468, - "angularVelocity": -1.3429731785111632, - "velocityX": 2.9300636632076498, - "velocityY": 0.23592037162970983, - "timestamp": 2.340800952406728 + "x": 8.047393114183997, + "y": 7.516347487170096, + "heading": -0.01738240796128744, + "angularVelocity": 0.0319743405551765, + "velocityX": 1.4966585194769915, + "velocityY": -0.0706010049029291, + "timestamp": 2.3202026496222823 }, { - "x": 4.088431686194158, - "y": 7.212973174468024, - "heading": 0.005865325294296566, - "angularVelocity": -1.2419890892831975, - "velocityX": 3.350860551957296, - "velocityY": 0.26965579373215276, - "timestamp": 2.4089957438249296 + "x": 8.11763212951421, + "y": 7.506281183777245, + "heading": -0.013474109300406987, + "angularVelocity": 0.062170054331707525, + "velocityX": 1.1173054513447094, + "velocityY": -0.1601266134333638, + "timestamp": 2.383067298895834 }, { - "x": 4.340105997195649, - "y": 7.232705863139766, - "heading": 3.5066831801673635e-7, - "angularVelocity": -0.08600326365120493, - "velocityX": 3.6905210173326544, - "velocityY": 0.2893577098979943, - "timestamp": 2.4771905352431314 + "x": 8.164021945337474, + "y": 7.490592202393592, + "heading": -0.007677398137362595, + "angularVelocity": 0.09220939319680839, + "velocityX": 0.737931673195224, + "velocityY": -0.24956762767234375, + "timestamp": 2.4459319481693855 }, { - "x": 4.593304664542687, - "y": 7.253186623137088, - "heading": 3.187896158571913e-7, - "angularVelocity": -4.67465351775184e-7, - "velocityX": 3.712873990541412, - "velocityY": 0.30032733543716505, - "timestamp": 2.545385326661333 + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 1.0473347586059488e-21, + "angularVelocity": 0.12212584061281963, + "velocityX": 0.358542350838588, + "velocityY": -0.33894514938796605, + "timestamp": 2.508796597442937 }, { - "x": 4.846503330689328, - "y": 7.273667397974732, - "heading": 2.869109349284145e-7, - "angularVelocity": -4.674650404498194e-7, - "velocityX": 3.7128739729389193, - "velocityY": 0.30032755305381253, - "timestamp": 2.613580118079535 + "x": 8.169433800706123, + "y": 7.42685355131065, + "heading": 0.014977206112370758, + "angularVelocity": 0.16513162181596408, + "velocityX": -0.1888428783219197, + "velocityY": -0.46782403935650313, + "timestamp": 2.5994951925858016 }, { - "x": 5.0997019968359405, - "y": 7.294148172812728, - "heading": 2.550322540852e-7, - "angularVelocity": -4.674650391951449e-7, - "velocityX": 3.7128739729385014, - "velocityY": 0.3003275530589851, - "timestamp": 2.6817749094977366 + "x": 8.10265502996488, + "y": 7.372742454671214, + "heading": 0.033468558101012916, + "angularVelocity": 0.2038769394334648, + "velocityX": -0.7362712800132867, + "velocityY": -0.5966034705852111, + "timestamp": 2.690193787728666 }, { - "x": 5.352900662982552, - "y": 7.314628947650725, - "heading": 2.2315357326181484e-7, - "angularVelocity": -4.674650389043701e-7, - "velocityX": 3.712873972938501, - "velocityY": 0.30032755305898534, - "timestamp": 2.7499697009159383 + "x": 7.986220080079777, + "y": 7.306963959658495, + "heading": 0.05493247364660356, + "angularVelocity": 0.23665102542968372, + "velocityX": -1.2837569281166943, + "velocityY": -0.7252427108612493, + "timestamp": 2.7808923828715306 }, { - "x": 5.606099329129164, - "y": 7.335109722488721, - "heading": 1.9127489242002956e-7, - "angularVelocity": -4.6746503917418713e-7, - "velocityX": 3.7128739729385014, - "velocityY": 0.30032755305898523, - "timestamp": 2.81816449233414 + "x": 7.820121799349139, + "y": 7.229537347680479, + "heading": 0.07855456380147917, + "angularVelocity": 0.26044604238540936, + "velocityX": -1.8313214275150225, + "velocityY": -0.8536693634124876, + "timestamp": 2.871590978014395 }, { - "x": 5.859297995275776, - "y": 7.355590497326717, - "heading": 1.593962115667357e-7, - "angularVelocity": -4.6746503934294765e-7, - "velocityX": 3.7128739729385014, - "velocityY": 0.30032755305898523, - "timestamp": 2.8863592837523417 + "x": 7.6043499812219455, + "y": 7.1404953741126675, + "heading": 0.10296904259791395, + "angularVelocity": 0.2691825464107588, + "velocityX": -2.37899845953864, + "velocityY": -0.9817348706179747, + "timestamp": 2.9622895731572596 + }, + { + "x": 7.338890842614761, + "y": 7.0399060968505065, + "heading": 0.12540191269228773, + "angularVelocity": 0.24733426200304945, + "velocityX": -2.926827457349739, + "velocityY": -1.1090500035167739, + "timestamp": 3.052988168300124 }, { - "x": 6.112496661422388, - "y": 7.3760712721647135, - "heading": 1.2751753069996381e-7, - "angularVelocity": -4.674650395405871e-7, - "velocityX": 3.7128739729385014, - "velocityY": 0.3003275530589853, - "timestamp": 2.9545540751705435 + "x": 7.023762260989218, + "y": 6.927996342713902, + "heading": 0.13709315762540872, + "angularVelocity": 0.12890216121545683, + "velocityX": -3.4744593466873996, + "velocityY": -1.233864250712263, + "timestamp": 3.1436867634429886 }, { - "x": 6.365695327569, - "y": 7.39655204700271, - "heading": 9.56388499052934e-8, - "angularVelocity": -4.674650384833005e-7, - "velocityX": 3.7128739729385014, - "velocityY": 0.3003275530589854, - "timestamp": 3.022748866588745 + "x": 6.6949878661960245, + "y": 6.850204202472232, + "heading": 0.1370931594052802, + "angularVelocity": 1.9624024712667312e-8, + "velocityX": -3.6249116568489486, + "velocityY": -0.857699505919964, + "timestamp": 3.234385358585853 }, { - "x": 6.618893993715612, - "y": 7.41703282184071, - "heading": 6.376016904700571e-8, - "angularVelocity": -4.6746503941617634e-7, - "velocityX": 3.7128739729384974, - "velocityY": 0.300327553059032, - "timestamp": 3.090943658006947 + "x": 6.366213403460125, + "y": 6.772412349378657, + "heading": 0.13709316118514803, + "angularVelocity": 1.9623984758130592e-8, + "velocityX": -3.624912405953231, + "velocityY": -0.8576963399602832, + "timestamp": 3.3250839537287176 }, { - "x": 6.872092659851402, - "y": 7.437513596812488, - "heading": 3.188148827519539e-8, - "angularVelocity": -4.674650381480829e-7, - "velocityX": 3.7128739727798186, - "velocityY": 0.3003275550207428, - "timestamp": 3.1591384494251487 + "x": 6.037438940720318, + "y": 6.694620496301594, + "heading": 0.13709316296501595, + "angularVelocity": 1.9623985202632452e-8, + "velocityX": -3.624912405996305, + "velocityY": -0.8576963397782383, + "timestamp": 3.415782548871582 }, { - "x": 7.125290870666504, - "y": 7.458, - "heading": 2.875044920599443e-26, - "angularVelocity": -4.675062070310236e-7, - "velocityX": 3.7128672960134566, - "velocityY": 0.300410086481236, - "timestamp": 3.2273332408433504 + "x": 5.708664477980543, + "y": 6.6168286432244035, + "heading": 0.13709316474488378, + "angularVelocity": 1.9623984644394548e-8, + "velocityX": -3.624912405995973, + "velocityY": -0.8576963397796433, + "timestamp": 3.5064811440144465 }, { - "x": 7.327398255820751, - "y": 7.485125041057045, - "heading": -0.0034915599283927, - "angularVelocity": -0.05859476692705873, - "velocityX": 3.3917318820879983, - "velocityY": 0.4552078415437867, - "timestamp": 3.2869214961006694 + "x": 5.379890015210838, + "y": 6.539036790273742, + "heading": 0.13709316652476114, + "angularVelocity": 1.9624089556639278e-8, + "velocityX": -3.6249124063259477, + "velocityY": -0.8576963383845883, + "timestamp": 3.597179739157311 }, { - "x": 7.508458450228073, - "y": 7.505782624333376, - "heading": -0.006110359017821738, - "angularVelocity": -0.04394824245348874, - "velocityX": 3.038521494302052, - "velocityY": 0.3466720612497382, - "timestamp": 3.3465097513579884 + "x": 5.082161809078407, + "y": 6.468591173031877, + "heading": 0.1733415977378681, + "angularVelocity": 0.39965813313877674, + "velocityX": -3.282611000351902, + "velocityY": -0.7767002028080265, + "timestamp": 3.6878783343001755 }, { - "x": 7.668471533847549, - "y": 7.519972433195907, - "heading": -0.00785620988870458, - "angularVelocity": -0.029298573407523957, - "velocityX": 2.68531244837584, - "velocityY": 0.23813096727293456, - "timestamp": 3.4060980066153075 + "x": 4.834054918534678, + "y": 6.409886673220785, + "heading": 0.20354854900447322, + "angularVelocity": 0.3330476201866674, + "velocityX": -2.7355097413903846, + "velocityY": -0.6472481709184398, + "timestamp": 3.77857692944304 }, { - "x": 7.807437533254427, - "y": 7.527694362359166, - "heading": -0.008729041834027805, - "angularVelocity": -0.01464771776844422, - "velocityX": 2.332103848430922, - "velocityY": 0.12958810641313134, - "timestamp": 3.4656862618726265 + "x": 4.6355693784003975, + "y": 6.36292318296314, + "heading": 0.22771447843277892, + "angularVelocity": 0.26644215811987526, + "velocityX": -2.1884080985117165, + "velocityY": -0.5177973284334796, + "timestamp": 3.8692755245859045 }, { - "x": 7.925356461622809, - "y": 7.528948359353389, - "heading": -0.008728854851904898, - "angularVelocity": 0.0000031379022946504295, - "velocityX": 1.9788954695715617, - "velocityY": 0.021044365014674563, - "timestamp": 3.5252745171299456 + "x": 4.486705209976563, + "y": 6.327700620163418, + "heading": 0.24583924264804105, + "angularVelocity": 0.1998351152706694, + "velocityX": -1.6413062207782854, + "velocityY": -0.3883473910951022, + "timestamp": 3.959974119728769 }, { - "x": 8.022228326704038, - "y": 7.523734392879376, - "heading": -0.007855710298573708, - "angularVelocity": 0.0146529639030494, - "velocityX": 1.6256872207939095, - "velocityY": -0.0874999016416258, - "timestamp": 3.5848627723872646 + "x": 4.3874624262872, + "y": 6.30421892984517, + "heading": 0.25792252534877924, + "angularVelocity": 0.13322458503028708, + "velocityX": -1.0942041994480602, + "velocityY": -0.25889805990114845, + "timestamp": 4.0506727148716335 }, { - "x": 8.098053133481649, - "y": 7.512052442276777, - "heading": -0.006109727375299004, - "angularVelocity": 0.029300789488382373, - "velocityX": 1.272479055649084, - "velocityY": -0.19604451501648842, - "timestamp": 3.6444510276445836 + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": 0.06661189082061515, + "velocityX": -0.5471021053136723, + "velocityY": -0.1294490312938923, + "timestamp": 4.141371310014498 }, { - "x": 8.152830885308017, - "y": 7.493902493017176, - "heading": -0.0034910810079803226, - "angularVelocity": 0.04394567949691789, - "velocityX": 0.919270946763285, - "velocityY": -0.30458937220470644, - "timestamp": 3.7040392829019027 + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": -4.3850016440748696e-23, + "velocityX": 2.0622476782033507e-21, + "velocityY": 1.8080615452809687e-23, + "timestamp": 4.2320699051573625 }, { - "x": 8.186561584472656, - "y": 7.469284534454346, - "heading": 2.1197622073376246e-26, - "angularVelocity": 0.058586729765871357, - "velocityX": 0.5660628762997177, - "velocityY": -0.41313440805613916, - "timestamp": 3.7636275381592217 + "x": 4.357336302646973, + "y": 6.297583243093535, + "heading": 0.2475212314024478, + "angularVelocity": -0.28561956492945584, + "velocityX": 0.3386404194139296, + "velocityY": 0.08867859433669868, + "timestamp": 4.289639136689636 }, { - "x": 8.185948757189772, - "y": 7.412401444181256, - "heading": 0.007947733329215964, - "angularVelocity": 0.08230133185775951, - "velocityX": -0.0063460233869197455, - "velocityY": -0.5890426736452052, - "timestamp": 3.8601962467063924 + "x": 4.39639255106342, + "y": 6.307690301692112, + "heading": 0.21545182729872558, + "angularVelocity": -0.5570580542792894, + "velocityX": 0.6784222644096215, + "velocityY": 0.17556354895775533, + "timestamp": 4.347208368221909 }, { - "x": 8.130028903552427, - "y": 7.338629187384324, - "heading": 0.018106821496109617, - "angularVelocity": 0.10520062160644154, - "velocityX": -0.5790680488393326, - "velocityY": -0.7639354186961682, - "timestamp": 3.956764955253563 + "x": 4.455088006420721, + "y": 6.322669942772804, + "heading": 0.16876628337274657, + "angularVelocity": -0.8109461023430008, + "velocityX": 1.0195629469953096, + "velocityY": 0.2602022066647703, + "timestamp": 4.4047775997541825 }, { - "x": 8.018757435307506, - "y": 7.248113553962043, - "heading": 0.03036067277368815, - "angularVelocity": 0.12689256656666278, - "velocityX": -1.1522517999768782, - "velocityY": -0.9373184625128009, - "timestamp": 4.053333663800734 + "x": 4.53351670800356, + "y": 6.342355632767599, + "heading": 0.10873910043109954, + "angularVelocity": -1.0426955744232154, + "velocityX": 1.362337128625251, + "velocityY": 0.3419481113580211, + "timestamp": 4.462346831286456 }, { - "x": 7.852062035550644, - "y": 7.141094085596084, - "heading": 0.044518821199191705, - "angularVelocity": 0.14661217529473056, - "velocityX": -1.7261844158911652, - "velocityY": -1.1082209752622316, - "timestamp": 4.1499023723479045 + "x": 4.631792821732683, + "y": 6.3665238013399605, + "heading": 0.03701268299307073, + "angularVelocity": -1.245915839571676, + "velocityX": 1.7070944168158462, + "velocityY": 0.4198105121277204, + "timestamp": 4.519916062818729 }, { - "x": 7.629805209867524, - "y": 7.0180373220836785, - "heading": 0.060212869798815634, - "angularVelocity": 0.16251691501039256, - "velocityX": -2.301540830636187, - "velocityY": -1.2742923185339627, - "timestamp": 4.246471080895075 + "x": 4.7500563078189675, + "y": 6.394856912095635, + "heading": -0.0442182245974014, + "angularVelocity": -1.4110125396572948, + "velocityX": 2.0542828684448438, + "velocityY": 0.4921571819104624, + "timestamp": 4.5774852943510025 }, { - "x": 7.351625362652527, - "y": 6.880249252941204, - "heading": 0.07642107570328951, - "angularVelocity": 0.16784117907672624, - "velocityX": -2.8806416840411067, - "velocityY": -1.4268397208104806, - "timestamp": 4.343039789442246 + "x": 4.888478268982772, + "y": 6.426870152464857, + "heading": -0.13181305030525106, + "angularVelocity": -1.52155627887344, + "velocityX": 2.4044434410455024, + "velocityY": 0.5560824683107718, + "timestamp": 4.635054525883276 }, { - "x": 7.016267448829526, - "y": 6.750124387193528, - "heading": 0.07642107964742734, - "angularVelocity": 4.0842814169097235e-8, - "velocityX": -3.4727389323964073, - "velocityY": -1.347484787829737, - "timestamp": 4.439608497989417 + "x": 5.047249482849422, + "y": 6.461748968300146, + "heading": -0.22073086511138704, + "angularVelocity": -1.5445371153910277, + "velocityX": 2.757917895388995, + "velocityY": 0.6058586315458515, + "timestamp": 4.692623757415549 }, { - "x": 6.673392879085966, - "y": 6.641338156398843, - "heading": 0.0764210813675651, - "angularVelocity": 1.7812579245729294e-8, - "velocityX": -3.5505763192025745, - "velocityY": -1.1265163677895376, - "timestamp": 4.536177206536587 + "x": 5.226429049191539, + "y": 6.497909114202001, + "heading": -0.3013203844421244, + "angularVelocity": -1.39987137548568, + "velocityX": 3.1124189358280567, + "velocityY": 0.6281158344381212, + "timestamp": 4.7501929889478225 }, { - "x": 6.330518244592264, - "y": 6.532552129685128, - "heading": 0.0764210830877029, - "angularVelocity": 1.7812579496548032e-8, - "velocityX": -3.550576989711091, - "velocityY": -1.1265142544655247, - "timestamp": 4.632745915083758 + "x": 5.423903550460318, + "y": 6.531166211616276, + "heading": -0.3435648346153767, + "angularVelocity": -0.7338025721182303, + "velocityX": 3.430209089347909, + "velocityY": 0.5776887502073221, + "timestamp": 4.807762220480096 }, { - "x": 5.987643610098044, - "y": 6.42376610297305, - "heading": 0.07642108480784073, - "angularVelocity": 1.7812579676147597e-8, - "velocityX": -3.550576989716468, - "velocityY": -1.1265142544485776, - "timestamp": 4.729314623630929 + "x": 5.636616262089249, + "y": 6.55438565863319, + "heading": -0.34498723844741225, + "angularVelocity": -0.024707709208140263, + "velocityX": 3.69490274522221, + "velocityY": 0.4033308487694046, + "timestamp": 4.865331452012369 }, { - "x": 5.6447689756038235, - "y": 6.314980076260972, - "heading": 0.0764210865279785, - "angularVelocity": 1.7812579243167278e-8, - "velocityX": -3.550576989716468, - "velocityY": -1.1265142544485782, - "timestamp": 4.825883332178099 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.34498726323171464, + "angularVelocity": -4.305129971148693e-7, + "velocityX": 3.7246965125733036, + "velocityY": 0.047618149403998285, + "timestamp": 4.9229006835446425 }, { - "x": 5.3018943411135915, - "y": 6.20619404953636, - "heading": 0.07642108824812432, - "angularVelocity": 1.7812662732207566e-8, - "velocityX": -3.5505769896751667, - "velocityY": -1.1265142545783664, - "timestamp": 4.92245204072527 + "x": 5.970747823515829, + "y": 6.552244502208151, + "heading": -0.34498728566710496, + "angularVelocity": -6.97576178834182e-7, + "velocityX": 3.7219058963692446, + "velocityY": -0.15180985637112251, + "timestamp": 4.955062605108422 }, { - "x": 4.971178879857472, - "y": 6.101264896222856, - "heading": 0.09246038898135349, - "angularVelocity": 0.16609211176718244, - "velocityX": -3.4246648446642007, - "velocityY": -1.0865750913739254, - "timestamp": 5.019020749272441 + "x": 6.090018554613485, + "y": 6.540962017726213, + "heading": -0.3449873062008861, + "angularVelocity": -6.384500714125073e-7, + "velocityX": 3.7084454316926947, + "velocityY": -0.35080256195400317, + "timestamp": 4.987224526672201 }, { - "x": 4.695582635189411, - "y": 6.01382394107076, - "heading": 0.10582487967853092, - "angularVelocity": 0.13839359455293215, - "velocityX": -2.8538876496773393, - "velocityY": -0.9054791812752113, - "timestamp": 5.1155894578196115 + "x": 6.208514441050117, + "y": 6.52331189505608, + "heading": -0.34498732528503373, + "angularVelocity": -5.933770972523468e-7, + "velocityX": 3.68435344267748, + "velocityY": -0.5487894320969752, + "timestamp": 5.019386448235981 }, { - "x": 4.475105628641342, - "y": 5.943871182253239, - "heading": 0.1165156132920598, - "angularVelocity": 0.11070598099908092, - "velocityX": -2.2831102317203804, - "velocityY": -0.7243832900939244, - "timestamp": 5.212158166366782 + "x": 6.32589577532509, + "y": 6.499344741803893, + "heading": -0.34498734327209396, + "angularVelocity": -5.592657200372931e-7, + "velocityX": 3.6496990405936383, + "velocityY": -0.7452027766642643, + "timestamp": 5.05154836979976 }, { - "x": 4.309747868917504, - "y": 5.8914066159860985, - "heading": 0.12453333874296012, - "angularVelocity": 0.08302612276298497, - "velocityX": -1.7123327236282266, - "velocityY": -0.5432874380991931, - "timestamp": 5.308726874913953 + "x": 6.441826048149789, + "y": 6.4691292818938315, + "heading": -0.3449873604472264, + "angularVelocity": -5.340207168118268e-7, + "velocityX": 3.6045816663908092, + "velocityY": -0.9394793109653794, + "timestamp": 5.0837102913635395 }, { - "x": 4.19950936065589, - "y": 5.856430239360431, - "heading": 0.12987847932741303, - "angularVelocity": 0.055350647894830074, - "velocityX": -1.1415551675081796, - "velocityY": -0.36219161622714885, - "timestamp": 5.405295583461124 + "x": 6.555972918049828, + "y": 6.432752169034089, + "heading": -0.344987377050145, + "angularVelocity": -5.162290595234125e-7, + "velocityX": 3.549130908539721, + "velocityY": -1.1310615501503813, + "timestamp": 5.115872212927319 }, { - "x": 4.144390106201172, - "y": 5.838942050933838, - "heading": 0.13255114594654763, - "angularVelocity": 0.027676321443494054, - "velocityX": -0.5707775871082961, - "velocityY": -0.18109580929158178, - "timestamp": 5.501864292008294 + "x": 6.668009186278295, + "y": 6.390317785626105, + "heading": -0.34498739329087524, + "angularVelocity": -5.049676592749214e-7, + "velocityX": 3.483506668166316, + "velocityY": -1.319398261818203, + "timestamp": 5.148034134491098 }, { - "x": 4.144390106201172, - "y": 5.838942050933838, - "heading": 0.13255114594654763, - "angularVelocity": 2.6946819475075084e-27, - "velocityX": -6.238393680699965e-27, - "velocityY": 1.2085111651652188e-26, - "timestamp": 5.598433000555465 + "x": 6.777613987715612, + "y": 6.341948479079241, + "heading": -0.3449874093618285, + "angularVelocity": -4.996888394768407e-7, + "velocityX": 3.4079058746525486, + "velocityY": -1.5039308659137305, + "timestamp": 5.180196056054878 }, { - "x": 4.164497272188891, - "y": 5.856383810972102, - "heading": 0.11953980585413508, - "angularVelocity": -0.19788617668780903, - "velocityX": 0.30580479589931786, - "velocityY": 0.26526731175760876, - "timestamp": 5.664184637734952 + "x": 6.886436695218618, + "y": 6.291844380961981, + "heading": -0.3449874254078226, + "angularVelocity": -4.989127946142238e-7, + "velocityX": 3.3835884863782986, + "velocityY": -1.55787016698924, + "timestamp": 5.212357977618657 }, { - "x": 4.205280500846038, - "y": 5.8906190238483225, - "heading": 0.09417342379390335, - "angularVelocity": -0.38579088138881684, - "velocityX": 0.6202617973727179, - "velocityY": 0.520674683472388, - "timestamp": 5.729936274914438 + "x": 6.995259275146381, + "y": 6.241740005760864, + "heading": -0.3449874414538154, + "angularVelocity": -4.98912750035862e-7, + "velocityX": 3.3835845197234478, + "velocityY": -1.5578787822660598, + "timestamp": 5.244519899182436 }, { - "x": 4.267438293143164, - "y": 5.9407879010107, - "heading": 0.05730532907136487, - "angularVelocity": -0.5607175167653599, - "velocityX": 0.9453421232303373, - "velocityY": 0.7630057488215584, - "timestamp": 5.795687912093925 + "x": 7.1040824430518015, + "y": 6.191636907618568, + "heading": -0.344987457499818, + "angularVelocity": -4.989130590435776e-7, + "velocityX": 3.3836028015184674, + "velocityY": -1.5578390750981381, + "timestamp": 5.276681820746216 }, { - "x": 4.351839190303838, - "y": 6.005705720740894, - "heading": 0.010093847833175575, - "angularVelocity": -0.7180274630928684, - "velocityX": 1.283631872622101, - "velocityY": 0.9873186815559245, - "timestamp": 5.8614395492734115 + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -6.552462348846214e-7, + "velocityX": 3.4421745383783664, + "velocityY": -1.4237501526591758, + "timestamp": 5.308843742309995 }, { - "x": 4.459568016776304, - "y": 6.083661828301211, - "heading": -0.04581026085444041, - "angularVelocity": -0.850231432793241, - "velocityX": 1.6384204423441482, - "velocityY": 1.1856146995627632, - "timestamp": 5.927191186452898 + "x": 7.410548108371569, + "y": 6.0752735070916195, + "heading": -0.3626370424214939, + "angularVelocity": -0.2874228799686078, + "velocityX": 3.1879277548585048, + "velocityY": -1.1492779529116102, + "timestamp": 5.370250001441577 }, { - "x": 4.591948294079918, - "y": 6.172045469361953, - "heading": -0.1079292379524144, - "angularVelocity": -0.944751792695351, - "velocityX": 2.013338115707256, - "velocityY": 1.344204416073704, - "timestamp": 5.992942823632385 + "x": 7.584442413587634, + "y": 6.012933959955544, + "heading": -0.37637990938116833, + "angularVelocity": -0.22380238031152527, + "velocityX": 2.831866126927586, + "velocityY": -1.0151985810191508, + "timestamp": 5.431656260573159 }, { - "x": 4.7504027579860635, - "y": 6.266641095517782, - "heading": -0.17238119577808125, - "angularVelocity": -0.9802335058171755, - "velocityX": 2.409893817147125, - "velocityY": 1.438680924363996, - "timestamp": 6.058694460811871 + "x": 7.736397172033548, + "y": 5.958610527915441, + "heading": -0.3857543231283157, + "angularVelocity": -0.15266218590290268, + "velocityX": 2.47458094003583, + "velocityY": -0.8846562680800549, + "timestamp": 5.493062519704741 }, { - "x": 4.935641035495618, - "y": 6.360501248168881, - "heading": -0.23308994891442564, - "angularVelocity": -0.9233040535648408, - "velocityX": 2.8172420559490887, - "velocityY": 1.4274952940697678, - "timestamp": 6.124446097991358 + "x": 7.86638729375416, + "y": 5.912228690109332, + "heading": -0.3906020583128712, + "angularVelocity": -0.07894529406469342, + "velocityX": 2.1168871636044173, + "velocityY": -0.7553275262497595, + "timestamp": 5.5544687788363225 }, { - "x": 5.145232187341836, - "y": 6.443576909061307, - "heading": -0.2814508136896339, - "angularVelocity": -0.7355081462564137, - "velocityX": 3.1876187550141757, - "velocityY": 1.2634766897993692, - "timestamp": 6.190197735170845 + "x": 7.974400232833448, + "y": 5.873750789689322, + "heading": -0.3908432822952403, + "angularVelocity": -0.003928328899700405, + "velocityX": 1.7589890771205783, + "velocityY": -0.6266120256171138, + "timestamp": 5.615875037967904 }, { - "x": 5.371831569570122, - "y": 6.506507878011031, - "heading": -0.310075861000899, - "angularVelocity": -0.4353510960210094, - "velocityX": 3.446292624010618, - "velocityY": 0.9571011711531484, - "timestamp": 6.255949372350331 + "x": 8.060428468071539, + "y": 5.84315412501972, + "heading": -0.3864300383007793, + "angularVelocity": 0.07186961161409067, + "velocityX": 1.4009685080107037, + "velocityY": -0.498266220777903, + "timestamp": 5.677281297099486 }, { - "x": 5.608723728071605, - "y": 6.544912225542492, - "heading": -0.32489986784082275, - "angularVelocity": -0.22545456624080287, - "velocityX": 3.6028328519763595, - "velocityY": 0.5840819967208853, - "timestamp": 6.321701009529818 + "x": 8.124466995265989, + "y": 5.820423539830739, + "heading": -0.377330476340927, + "angularVelocity": 0.14818622870925488, + "velocityX": 1.042866445539805, + "velocityY": -0.37016723556265896, + "timestamp": 5.738687556231068 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": -0.3320371023810657, - "angularVelocity": -0.10854839280670678, - "velocityX": 3.6853903618544206, - "velocityY": 0.18577139494688796, - "timestamp": 6.3874526467093045 + "x": 8.166512252703873, + "y": 5.805548220947723, + "heading": -0.36352195759750816, + "angularVelocity": 0.2248715186155497, + "velocityX": 0.6847063806278847, + "velocityY": -0.242244342732895, + "timestamp": 5.80009381536265 }, { - "x": 5.9711094207782285, - "y": 6.556647574574296, - "heading": -0.33458795200315017, - "angularVelocity": -0.07861324883762467, - "velocityX": 3.700225495398522, - "velocityY": -0.014775117904477024, - "timestamp": 6.419900734783533 + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.30183371020853555, + "velocityX": 0.3265030642205698, + "velocityY": -0.11445303542856088, + "timestamp": 5.861500074494232 }, { - "x": 6.091317843363461, - "y": 6.549645373760147, - "heading": -0.3360778340908156, - "angularVelocity": -0.045915866730181235, - "velocityX": 3.704638076371226, - "velocityY": -0.21579702317530708, - "timestamp": 6.452348822857761 + "x": 8.182873429152231, + "y": 5.799961021839805, + "heading": -0.3197766551885773, + "angularVelocity": 0.3842915682549792, + "velocityX": -0.056218988582698255, + "velocityY": 0.021964322281146666, + "timestamp": 5.927103448254676 }, { - "x": 6.211267508828441, - "y": 6.536122994121453, - "heading": -0.33661432797946356, - "angularVelocity": -0.016533913721531207, - "velocityX": 3.6966635812434836, - "velocityY": -0.4167388724956589, - "timestamp": 6.48479691093199 + "x": 8.154073481017848, + "y": 5.810352783086787, + "heading": -0.28949615112813754, + "angularVelocity": 0.46156931152673975, + "velocityX": -0.43900102210518965, + "velocityY": 0.15840284807498262, + "timestamp": 5.99270682201512 }, { - "x": 6.330469045455405, - "y": 6.51611559729621, - "heading": -0.33661493169123347, - "angularVelocity": -0.000018605465091905695, - "velocityX": 3.6736074049811314, - "velocityY": -0.6165970943950513, - "timestamp": 6.517244999006218 + "x": 8.100157139869356, + "y": 5.829696916957608, + "heading": -0.2545716989464078, + "angularVelocity": 0.5323575630311173, + "velocityX": -0.8218531770236625, + "velocityY": 0.29486492480488535, + "timestamp": 6.058310195775564 }, { - "x": 6.448416412815129, - "y": 6.489700510555554, - "heading": -0.3366150129853366, - "angularVelocity": -0.0000025053588043472207, - "velocityX": 3.6349558436203777, - "velocityY": -0.8140722091307351, - "timestamp": 6.549693087080446 + "x": 8.021119019409117, + "y": 5.8579951101346595, + "heading": -0.2155530876775793, + "angularVelocity": 0.594765315139245, + "velocityX": -1.204787435915305, + "velocityY": 0.43135271183437995, + "timestamp": 6.1239135695360085 }, { - "x": 6.564765294299518, - "y": 6.456954843497344, - "heading": -0.3366150728513962, - "angularVelocity": -0.000001844979570705055, - "velocityX": 3.5856929757534926, - "velocityY": -1.0091709250573346, - "timestamp": 6.582141175154675 + "x": 7.9169528918289975, + "y": 5.89524913213786, + "heading": -0.17317932530550872, + "angularVelocity": 0.6459082809795952, + "velocityX": -1.5878166260242859, + "velocityY": 0.567867471865644, + "timestamp": 6.189516943296453 }, { - "x": 6.679176170718401, - "y": 6.417974154303709, - "heading": -0.33661511946704875, - "angularVelocity": -0.0000014366224727676693, - "velocityX": 3.525966650397334, - "velocityY": -1.2013246852776633, - "timestamp": 6.614589263228903 + "x": 7.787651961744212, + "y": 5.941460642017215, + "heading": -0.12849929599301224, + "angularVelocity": 0.6810629812370456, + "velocityX": -1.9709493989888005, + "velocityY": 0.7044075209927304, + "timestamp": 6.255120317056897 }, { - "x": 6.791315180749945, - "y": 6.372872197072731, - "heading": -0.33661515729887875, - "angularVelocity": -0.000001165918618828322, - "velocityX": 3.4559512343227956, - "velocityY": -1.389972719742485, - "timestamp": 6.647037351303132 + "x": 7.633210550125228, + "y": 5.99663052819492, + "heading": -0.08312171419135361, + "angularVelocity": 0.6916958564868655, + "velocityX": -2.3541687380734118, + "velocityY": 0.8409611124446534, + "timestamp": 6.320723690817341 }, { - "x": 6.900855096060357, - "y": 6.3217805921945684, - "heading": -0.3366151890219745, - "angularVelocity": -9.776568561253354e-7, - "velocityX": 3.375851145985221, - "velocityY": -1.5745644169014148, - "timestamp": 6.67948543937736 + "x": 7.453632499858265, + "y": 6.060756206322318, + "heading": -0.039828348341138756, + "angularVelocity": 0.659925905766733, + "velocityX": -2.7373294995880113, + "velocityY": 0.9774753103637095, + "timestamp": 6.386327064577785 + }, + { + "x": 7.248984054172477, + "y": 6.133814732067644, + "heading": -0.004587896529415136, + "angularVelocity": 0.5371743828359606, + "velocityX": -3.119480507711035, + "velocityY": 1.1136397651149037, + "timestamp": 6.45193043833823 }, { - "x": 7.007476282450098, - "y": 6.264848452420557, - "heading": -0.33661521634397956, - "angularVelocity": -8.420220318148558e-7, - "velocityX": 3.285900424882802, - "velocityY": -1.7545606891775674, - "timestamp": 6.711933527451588 + "x": 7.020152924958556, + "y": 6.215343825965371, + "heading": -7.99601094738583e-8, + "angularVelocity": 0.06993263160608797, + "velocityX": -3.488100018293493, + "velocityY": 1.2427576391945405, + "timestamp": 6.517533812098674 }, { - "x": 7.110867767687471, - "y": 6.2022421991346945, - "heading": -0.33661529971413134, - "angularVelocity": -0.0000025693394187906504, - "velocityX": 3.186366019497228, - "velocityY": -1.9294281112232177, - "timestamp": 6.744381615525817 + "x": 6.789969979123711, + "y": 6.297403529442868, + "heading": -7.492709232609331e-8, + "angularVelocity": 7.671887677824965e-8, + "velocityX": -3.508705919231764, + "velocityY": 1.2508457838943434, + "timestamp": 6.583137185859118 }, { - "x": 7.214789390563965, - "y": 6.145846366882324, - "heading": -0.344987478573796, - "angularVelocity": -0.25801763236442515, - "velocityX": 3.2027040434173877, - "velocityY": -1.73803251899956, - "timestamp": 6.776829703600045 + "x": 6.559787033588477, + "y": 6.379463233760796, + "heading": -6.989407741573959e-8, + "angularVelocity": 7.671884267312441e-8, + "velocityX": -3.5087059146647626, + "velocityY": 1.2508457967051358, + "timestamp": 6.648740559619562 }, { - "x": 7.403072746116238, - "y": 6.061144053185023, - "heading": -0.35924792619770074, - "angularVelocity": -0.23258228867402195, - "velocityX": 3.0708274318237563, - "velocityY": -1.3814614025635412, - "timestamp": 6.8381432615033795 + "x": 6.3296038306461995, + "y": 6.461522216031207, + "heading": -6.486106191272836e-8, + "angularVelocity": 7.671885170707308e-8, + "velocityX": -3.508709838350823, + "velocityY": 1.250834790449275, + "timestamp": 6.714343933380007 }, { - "x": 7.5736331381530135, - "y": 5.991566896909843, - "heading": -0.3703276689612211, - "angularVelocity": -0.18070624413915942, - "velocityX": 2.781772871600061, - "velocityY": -1.1347760373794369, - "timestamp": 6.899456819406714 + "x": 6.0929382858404795, + "y": 6.522410665825721, + "heading": -5.975605523435043e-8, + "angularVelocity": 7.78162217238893e-8, + "velocityX": -3.607520943510018, + "velocityY": 0.9281298552853697, + "timestamp": 6.779947307140451 }, { - "x": 7.72439550755076, - "y": 5.934268464416171, - "heading": -0.37789043666294847, - "angularVelocity": -0.12334576495545502, - "velocityX": 2.4588749136926022, - "velocityY": -0.934514884685174, - "timestamp": 6.960770377310048 + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -5.43646913463662e-8, + "angularVelocity": 8.218119860224255e-8, + "velocityX": -3.6872205492775545, + "velocityY": 0.5291851788357008, + "timestamp": 6.845550680900895 }, { - "x": 7.854564778976688, - "y": 5.88786008036463, - "heading": -0.38180542343665536, - "angularVelocity": -0.06385189357106222, - "velocityX": 2.1230095900021, - "velocityY": -0.7569024802753538, - "timestamp": 7.022083935213383 + "x": 5.661580826065391, + "y": 6.5680717657534915, + "heading": -4.9482596020305806e-8, + "angularVelocity": 9.582614101786845e-8, + "velocityX": -3.718801184433256, + "velocityY": 0.21482472211899678, + "timestamp": 6.896498107920314 }, { - "x": 7.963733931476252, - "y": 5.851532970517725, - "heading": -0.38200457597908144, - "angularVelocity": -0.0032480995922642706, - "velocityX": 1.7805059147224407, - "velocityY": -0.592480865393233, - "timestamp": 7.083397493116717 + "x": 5.471871496557958, + "y": 6.562921997343888, + "heading": -4.486806959492394e-8, + "angularVelocity": 9.057427814015043e-8, + "velocityX": -3.7236292508966913, + "velocityY": -0.10108004880484801, + "timestamp": 6.947445534939733 }, { - "x": 8.05165855711374, - "y": 5.8247603198900775, - "heading": -0.3784464795942301, - "angularVelocity": 0.0580311517798547, - "velocityX": 1.434016042195889, - "velocityY": -0.43665139559925853, - "timestamp": 7.144711051020051 + "x": 5.28328089006521, + "y": 6.541715087274231, + "heading": -4.036796232249368e-8, + "angularVelocity": 8.83284502417564e-8, + "velocityX": -3.7016708698726792, + "velocityY": -0.41625085525070266, + "timestamp": 6.998392961959151 }, { - "x": 8.118176532906231, - "y": 5.80717243118701, - "heading": -0.3711033743685625, - "angularVelocity": 0.11976315641712676, - "velocityX": 1.084882007619954, - "velocityY": -0.2868515431904276, - "timestamp": 7.206024608923386 + "x": 5.095501405649377, + "y": 6.514237548522189, + "heading": -3.588263316902823e-8, + "angularVelocity": 8.803838419074343e-8, + "velocityX": -3.685750103616821, + "velocityY": -0.5393312353451875, + "timestamp": 7.04934038897857 }, { - "x": 8.16317279255834, - "y": 5.798495818445154, - "heading": -0.3599553749598749, - "angularVelocity": 0.18181948316004298, - "velocityX": 0.7338712870495722, - "velocityY": -0.1415121392161809, - "timestamp": 7.26733816682672 + "x": 4.907721944965666, + "y": 6.486759847587045, + "heading": -3.139730406205809e-8, + "angularVelocity": 8.803838327812946e-8, + "velocityX": -3.685749637800889, + "velocityY": -0.5393344186875336, + "timestamp": 7.100287815997989 }, { - "x": 8.186561584472656, - "y": 5.798520088195801, - "heading": -0.344987478573796, - "angularVelocity": 0.24412049957493756, - "velocityX": 0.38146199167222045, - "velocityY": 0.0003958300819017007, - "timestamp": 7.328651724730054 + "x": 4.719942484237288, + "y": 6.459282146957154, + "heading": -2.6911974883170177e-8, + "angularVelocity": 8.803838468973724e-8, + "velocityX": -3.6857496386776267, + "velocityY": -0.5393344126960062, + "timestamp": 7.151235243017408 }, { - "x": 8.186562507619959, - "y": 5.808483646148046, - "heading": -0.3243738625034631, - "angularVelocity": 0.3115588640226202, - "velocityX": 0.000013952657501305156, - "velocityY": 0.1505914724827301, - "timestamp": 7.394814554457269 + "x": 4.532163023549714, + "y": 6.431804446048415, + "heading": -2.2426645714855718e-8, + "angularVelocity": 8.803838448220064e-8, + "velocityX": -3.685749637876727, + "velocityY": -0.5393344181692645, + "timestamp": 7.2021826700368266 }, { - "x": 8.161321884729395, - "y": 5.828379592770816, - "heading": -0.29947359998774176, - "angularVelocity": 0.37634820968788424, - "velocityX": -0.38149249351379677, - "velocityY": 0.30071184537904955, - "timestamp": 7.460977384184483 + "x": 4.34438356286167, + "y": 6.404326745142886, + "heading": -1.794131654726717e-8, + "angularVelocity": 8.803838446795243e-8, + "velocityX": -3.6857496378859507, + "velocityY": -0.5393344181062291, + "timestamp": 7.253130097056245 }, { - "x": 8.110834888181197, - "y": 5.858201520747478, - "heading": -0.2705090334908383, - "angularVelocity": 0.43777702096967036, - "velocityX": -0.7630719054241456, - "velocityY": 0.45073537663996555, - "timestamp": 7.527140213911697 + "x": 4.156604102201168, + "y": 6.376849044049133, + "heading": -1.3455987422255126e-8, + "angularVelocity": 8.803838363225754e-8, + "velocityX": -3.685749637345336, + "velocityY": -0.5393344218007294, + "timestamp": 7.304077524075664 }, { - "x": 8.035095319337403, - "y": 5.897940888331321, - "heading": -0.23777201719034075, - "angularVelocity": 0.494794682081624, - "velocityX": -1.144745004953147, - "velocityY": 0.600629806005663, - "timestamp": 7.593303043638912 + "x": 3.9688246409539834, + "y": 6.349371346964709, + "heading": -8.97065828327331e-9, + "angularVelocity": 8.80383839064573e-8, + "velocityX": -3.685749648860808, + "velocityY": -0.5393343431053033, + "timestamp": 7.355024951095083 }, { - "x": 7.934094943360084, - "y": 5.947585769746669, - "heading": -0.20166340599285526, - "angularVelocity": 0.5457537313074324, - "velocityX": -1.526542567688504, - "velocityY": 0.7503439864956146, - "timestamp": 7.659465873366126 + "x": 3.7810451818268453, + "y": 6.32189363539213, + "heading": -4.485329161078249e-9, + "angularVelocity": 8.80383835769656e-8, + "velocityX": -3.685749607248367, + "velocityY": -0.5393346274799247, + "timestamp": 7.405972378114502 }, { - "x": 7.8078223399404525, - "y": 6.0071184189660025, - "heading": -0.1627692568761995, - "angularVelocity": 0.587854982578802, - "velocityX": -1.9085127395585555, - "velocityY": 0.8997899495046319, - "timestamp": 7.72562870309334 + "x": 3.5932657187551267, + "y": 6.294415950776365, + "heading": 7.629972110817107e-24, + "angularVelocity": 8.803838434016792e-8, + "velocityX": -3.6857496846728943, + "velocityY": -0.5393340983695197, + "timestamp": 7.4569198051339205 }, { - "x": 7.656260799272679, - "y": 6.076509799690328, - "heading": -0.12202859757436733, - "angularVelocity": 0.6157635559090158, - "velocityX": -2.290735467220094, - "velocityY": 1.0487970513114737, - "timestamp": 7.791791532820555 + "x": 3.405486822128296, + "y": 6.266934394836426, + "heading": 3.646452356482733e-24, + "angularVelocity": -1.9320923857305114e-25, + "velocityX": -3.6857385664492472, + "velocityY": -0.5394100850169479, + "timestamp": 7.507867232153339 }, { - "x": 7.479384511526562, - "y": 6.155704239729163, - "heading": -0.08118783305769839, - "angularVelocity": 0.6172765688083961, - "velocityX": -2.6733482905034087, - "velocityY": 1.1969627110168721, - "timestamp": 7.857954362547769 + "x": 3.2165781142286494, + "y": 6.223116050022905, + "heading": 3.318409186414545e-24, + "angularVelocity": -1.5338201174379227e-26, + "velocityX": -3.536673395714516, + "velocityY": -0.8203495543918979, + "timestamp": 7.561281468290765 }, { - "x": 7.277158908396328, - "y": 6.244553850532033, - "heading": -0.04460973018212138, - "angularVelocity": 0.5528497349098663, - "velocityX": -3.0564835869928735, - "velocityY": 1.342893149661115, - "timestamp": 7.924117192274983 + "x": 3.041563345872727, + "y": 6.190252158258573, + "heading": 2.990185107220459e-24, + "angularVelocity": -1.87251095899189e-26, + "velocityX": -3.2765566075987578, + "velocityY": -0.6152646586535303, + "timestamp": 7.6146957044281915 }, { - "x": 7.0505381507723195, - "y": 6.341424218836914, - "heading": -0.04460966986271364, - "angularVelocity": 9.116811959714027e-7, - "velocityX": -3.425197479587153, - "velocityY": 1.464120695929603, - "timestamp": 7.990280022002198 + "x": 2.8804424282697485, + "y": 6.168342842771101, + "heading": 2.6619349218872592e-24, + "angularVelocity": -1.9213858301671557e-26, + "velocityX": -3.0164414817885197, + "velocityY": -0.4101774558958965, + "timestamp": 7.668109940565618 }, { - "x": 6.821387916460742, - "y": 6.432149176298842, - "heading": -0.04460966140291972, - "angularVelocity": 1.278632421205442e-7, - "velocityX": -3.4634285633844892, - "velocityY": 1.371237563991483, - "timestamp": 8.056442851729411 + "x": 2.733215332226118, + "y": 6.157388144124356, + "heading": 2.333764868738876e-24, + "angularVelocity": -1.7713655535187625e-26, + "velocityX": -2.7563269025291364, + "velocityY": -0.20508949371775206, + "timestamp": 7.721524176703044 }, { - "x": 6.585463067264655, - "y": 6.503425491915177, - "heading": -0.044609652839645464, - "angularVelocity": 1.2942726746927926e-7, - "velocityX": -3.5658216277748638, - "velocityY": 1.0772863843672305, - "timestamp": 8.122605681456625 + "x": 2.5998820432630403, + "y": 6.157388082450519, + "heading": 2.0056312293962657e-24, + "angularVelocity": -1.7031930803836163e-26, + "velocityX": -2.4962125943359705, + "velocityY": -0.0000011546329394271484, + "timestamp": 7.77493841284047 }, { - "x": 6.34313048184035, - "y": 6.548322764309462, - "heading": -0.044609643757262056, - "angularVelocity": 1.3727320078677108e-7, - "velocityX": -3.662669605024884, - "velocityY": 0.6785875480144041, - "timestamp": 8.188768511183838 + "x": 2.4804425527424723, + "y": 6.16834266976643, + "heading": 1.6774969061310235e-24, + "angularVelocity": -1.7044734930100497e-26, + "velocityX": -2.2360984478608033, + "velocityY": 0.20508740942632245, + "timestamp": 7.828352648977896 }, { - "x": 6.097330137085011, - "y": 6.566295976512413, - "heading": -0.04460963356805428, - "angularVelocity": 1.5400199501081406e-7, - "velocityX": -3.715082105296292, - "velocityY": 0.27165120169518303, - "timestamp": 8.254931340911051 + "x": 2.3748968549301686, + "y": 6.190251914052291, + "heading": 1.3492831778827425e-24, + "angularVelocity": -1.853132331504253e-26, + "velocityX": -1.97598440873988, + "velocityY": 0.4101761228877669, + "timestamp": 7.881766885115322 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": -0.04460962128616813, - "angularVelocity": 1.8563121019327388e-7, - "velocityX": -3.722421790171331, - "velocityY": -0.13858200516588748, - "timestamp": 8.321094170638265 + "x": 2.283244945743567, + "y": 6.22311582099134, + "heading": 1.0211293460600033e-24, + "angularVelocity": -1.7409966353369085e-26, + "velocityX": -1.7158704460510679, + "velocityY": 0.6152649427485215, + "timestamp": 7.935181121252748 }, { - "x": 5.594131845425269, - "y": 6.517387984060631, - "heading": -0.0446096110934515, - "angularVelocity": 1.4604851016203596e-7, - "velocityX": -3.6812230501933914, - "velocityY": -0.5694089339833474, - "timestamp": 8.390884109764682 + "x": 2.205486822128296, + "y": 6.266934394836426, + "heading": 6.9419459427461045e-25, + "angularVelocity": 5.413163312168308e-27, + "velocityX": -1.455756540544968, + "velocityY": 0.8203538422294017, + "timestamp": 7.9885953573901745 }, { - "x": 5.343562863816817, - "y": 6.448118054894418, - "heading": -0.04460960236123058, - "angularVelocity": 1.2512148631224918e-7, - "velocityX": -3.590330995339737, - "velocityY": -0.9925489265829068, - "timestamp": 8.4606740488911 + "x": 2.136159340261779, + "y": 6.327632048563378, + "heading": 0.025231456124497797, + "angularVelocity": 0.43032288322850026, + "velocityX": -1.1823812996193095, + "velocityY": 1.035199444221815, + "timestamp": 8.047229134741727 }, { - "x": 5.102719675691794, - "y": 6.350252358061884, - "heading": -0.04460958274491264, - "angularVelocity": 2.810765876662287e-7, - "velocityX": -3.4509728929374397, - "velocityY": -1.4022894711981222, - "timestamp": 8.530463988017518 + "x": 2.084207176746768, + "y": 6.399028713640929, + "heading": 0.07562693390361644, + "angularVelocity": 0.8594956705068004, + "velocityX": -0.8860449703507888, + "velocityY": 1.2176712520067714, + "timestamp": 8.10586291209328 }, { - "x": 4.88810249039924, - "y": 6.239912108360582, - "heading": -0.010754389574110686, - "angularVelocity": 0.4851013426086585, - "velocityX": -3.0751880282313135, - "velocityY": -1.5810337576227271, - "timestamp": 8.600253927143935 + "x": 2.051566754965305, + "y": 6.477423275068575, + "heading": 0.14957404640599054, + "angularVelocity": 1.2611691731714223, + "velocityX": -0.5566829096778134, + "velocityY": 1.3370204849265277, + "timestamp": 8.164496689444832 }, { - "x": 4.7014054819057804, - "y": 6.141094298946399, - "heading": 0.022653281130584686, - "angularVelocity": 0.47868892168225635, - "velocityX": -2.6751278311803, - "velocityY": -1.415932019014718, - "timestamp": 8.670043866270353 + "x": 2.039926841667985, + "y": 6.553838491753715, + "heading": 0.2360367067446735, + "angularVelocity": 1.4746220394479557, + "velocityX": -0.19851890536627906, + "velocityY": 1.3032627290405507, + "timestamp": 8.223130466796384 }, { - "x": 4.541859398687963, - "y": 6.055486217406872, - "heading": 0.05282297144244177, - "angularVelocity": 0.43229283030620586, - "velocityX": -2.286090018345113, - "velocityY": -1.2266536210105516, - "timestamp": 8.73983380539677 + "x": 2.042236938900771, + "y": 6.614274854513032, + "heading": 0.313525546116211, + "angularVelocity": 1.3215733809359698, + "velocityX": 0.039398744838411756, + "velocityY": 1.0307431226365935, + "timestamp": 8.281764244147936 }, { - "x": 4.409161403799561, - "y": 5.983676432778196, - "heading": 0.07876234420420766, - "angularVelocity": 0.37167782471881955, - "velocityX": -1.9013914691633662, - "velocityY": -1.0289417862737966, - "timestamp": 8.809623744523188 + "x": 2.0480382687015357, + "y": 6.654459061696839, + "heading": 0.3665932060292166, + "angularVelocity": 0.9050697790597129, + "velocityX": 0.09894177149770778, + "velocityY": 0.6853422890166648, + "timestamp": 8.340398021499489 }, { - "x": 4.303150560780497, - "y": 5.925963754570679, - "heading": 0.09996559761955173, - "angularVelocity": 0.3038153304151236, - "velocityX": -1.5189989323107926, - "velocityY": -0.8269483958565483, - "timestamp": 8.879413683649606 + "x": 2.052253484725952, + "y": 6.674348831176758, + "heading": 0.3930579718029317, + "angularVelocity": 0.4513569988001917, + "velocityX": 0.07189057595834777, + "velocityY": 0.3392203330286158, + "timestamp": 8.399031798851041 }, { - "x": 4.223727158834788, - "y": 5.882528846842074, - "heading": 0.11612624955874849, - "angularVelocity": 0.23156134167022593, - "velocityX": -1.1380351228253833, - "velocityY": -0.6223663220271061, - "timestamp": 8.949203622776023 + "x": 2.052253484725952, + "y": 6.674348831176758, + "heading": 0.3930579718029317, + "angularVelocity": 7.733121458673518e-26, + "velocityX": -6.859499621919341e-22, + "velocityY": -2.8560302702061698e-22, + "timestamp": 8.457665576202594 + }, + { + "x": 2.103013204396169, + "y": 6.695498705680337, + "heading": 0.3930579718029317, + "angularVelocity": 4.836322083094603e-17, + "velocityX": 0.5390415085998568, + "velocityY": 0.22460053627515192, + "timestamp": 8.551832201333307 }, { - "x": 4.170823380513197, - "y": 5.853492711046949, - "heading": 0.1270379483840901, - "angularVelocity": 0.15635059955527503, - "velocityX": -0.7580430500986731, - "velocityY": -0.41605045309653765, - "timestamp": 9.018993561902441 + "x": 2.204532642750927, + "y": 6.737798454276797, + "heading": 0.3930579718029317, + "angularVelocity": 1.8057826879047683e-17, + "velocityX": 1.078083006732355, + "velocityY": 0.4492010681889061, + "timestamp": 8.64599882646402 }, { - "x": 4.144390106201172, - "y": 5.838942050933838, - "heading": 0.13255114594654763, - "angularVelocity": 0.0789970249504139, - "velocityX": -0.37875479822591473, - "velocityY": -0.2084922310471305, - "timestamp": 9.088783501028859 + "x": 2.356811797490316, + "y": 6.801248076007844, + "heading": 0.3930579718029317, + "angularVelocity": 9.246174062812876e-17, + "velocityX": 1.6171244804410174, + "velocityY": 0.6738015899260662, + "timestamp": 8.740165451594732 }, { - "x": 4.144390106201172, - "y": 5.838942050933838, - "heading": 0.13255114594654763, - "angularVelocity": 1.1741097503964344e-26, - "velocityX": -8.106670406309328e-26, - "velocityY": -1.6795580659596768e-26, - "timestamp": 9.158573440155276 + "x": 2.5598506571147865, + "y": 6.885847566082, + "heading": 0.3930579718029317, + "angularVelocity": 1.8302909821567636e-16, + "velocityX": 2.1561658320305157, + "velocityY": 0.8984020607802621, + "timestamp": 8.834332076725445 }, { - "x": 4.149229218776726, - "y": 5.815100726386144, - "heading": 0.12918022174703248, - "angularVelocity": -0.05380546988265158, - "velocityX": 0.07724016042252664, - "velocityY": -0.38054657832352085, - "timestamp": 9.221223654099454 + "x": 2.7121298118541755, + "y": 6.9492971878130465, + "heading": 0.3930579718029317, + "angularVelocity": 1.1024323128215209e-16, + "velocityX": 1.6171244804410174, + "velocityY": 0.6738015899260661, + "timestamp": 8.928498701856158 }, { - "x": 4.159717289478366, - "y": 5.767595641795523, - "heading": 0.12259706469587855, - "angularVelocity": -0.1050779660069413, - "velocityX": 0.16740678189839092, - "velocityY": -0.758258936401222, - "timestamp": 9.283873868043631 + "x": 2.8136492502089334, + "y": 6.991596936409507, + "heading": 0.3930579718029317, + "angularVelocity": -3.9462401223913846e-17, + "velocityX": 1.078083006732355, + "velocityY": 0.44920106818890604, + "timestamp": 9.022665326986871 }, { - "x": 4.176867312970839, - "y": 5.696690246498273, - "heading": 0.11300853276979762, - "angularVelocity": -0.1530486701070873, - "velocityX": 0.2737424569332407, - "velocityY": -1.1317662116912734, - "timestamp": 9.346524081987809 + "x": 2.8644089698791504, + "y": 7.012746810913086, + "heading": 0.3930579718029317, + "angularVelocity": 1.2061194980042807e-16, + "velocityX": 0.5390415085998568, + "velocityY": 0.22460053627515186, + "timestamp": 9.116831952117584 }, { - "x": 4.201974040585746, - "y": 5.602790592825652, - "heading": 0.10069220099176118, - "angularVelocity": -0.19658882233044642, - "velocityX": 0.4007444832874458, - "velocityY": -1.4987922268914513, - "timestamp": 9.409174295931987 + "x": 2.8644089698791504, + "y": 7.012746810913086, + "heading": 0.3930579718029317, + "angularVelocity": -7.454320302880939e-28, + "velocityX": 6.858010204674672e-22, + "velocityY": 2.856861212118512e-22, + "timestamp": 9.210998577248297 }, { - "x": 4.236730391014469, - "y": 5.486551757326023, - "heading": 0.08603276386217854, - "angularVelocity": -0.2339886204162749, - "velocityX": 0.5547682639949316, - "velocityY": -1.8553621477366304, - "timestamp": 9.471824509876164 + "x": 2.842186915849872, + "y": 7.004763611912437, + "heading": 0.3899906597418949, + "angularVelocity": -0.04969534926930016, + "velocityX": -0.36003273044640205, + "velocityY": -0.1293405609631654, + "timestamp": 9.272720893443621 }, { - "x": 4.283393708275682, - "y": 5.349087774330418, - "heading": 0.06958159023755481, - "angularVelocity": -0.2625876687233969, - "velocityX": 0.7448229514872947, - "velocityY": -2.194150256503298, - "timestamp": 9.534474723820342 + "x": 2.7978951293903465, + "y": 6.988387145564426, + "heading": 0.38377625605112037, + "angularVelocity": -0.10068325483944376, + "velocityX": -0.7175976079601634, + "velocityY": -0.26532488340499644, + "timestamp": 9.334443209638945 }, { - "x": 4.344983097190331, - "y": 5.19239819137041, - "heading": 0.0521501823955064, - "angularVelocity": -0.27823381190014773, - "velocityX": 0.983067495500113, - "velocityY": -2.501022312543423, - "timestamp": 9.59712493776452 + "x": 2.7317542660001495, + "y": 6.96306112048486, + "heading": 0.37430680047739806, + "angularVelocity": -0.15342028876161432, + "velocityX": -1.071587514326101, + "velocityY": -0.41032201383078476, + "timestamp": 9.396165525834268 }, { - "x": 4.425324635342364, - "y": 5.020190178284826, - "heading": 0.03492497946543372, - "angularVelocity": -0.2749424438585406, - "velocityX": 1.2823825026937383, - "velocityY": -2.748721867718171, - "timestamp": 9.659775151708697 + "x": 2.644108850979856, + "y": 6.927989989672691, + "heading": 0.36142823816874126, + "angularVelocity": -0.20865325707968818, + "velocityX": -1.4199955611343862, + "velocityY": -0.5682082749646841, + "timestamp": 9.457887842029592 }, { - "x": 4.5282801412548075, - "y": 4.838988810812342, - "heading": 0.019478906624106312, - "angularVelocity": -0.24654461443802958, - "velocityX": 1.6433384569792093, - "velocityY": -2.892270529737325, - "timestamp": 9.722425365652875 + "x": 2.53555910261038, + "y": 6.881949867747595, + "heading": 0.3449043845685677, + "angularVelocity": -0.267712792045632, + "velocityX": -1.7586791141467173, + "velocityY": -0.7459234319625763, + "timestamp": 9.519610158224916 }, { - "x": 4.655561045937701, - "y": 4.65789803685685, - "heading": 0.007446897298022715, - "angularVelocity": -0.1920505704386623, - "velocityX": 2.031611652536461, - "velocityY": -2.890505276116785, - "timestamp": 9.785075579597052 + "x": 2.407346628684672, + "y": 6.822852176062135, + "heading": 0.3243353524397527, + "angularVelocity": -0.33325113827101066, + "velocityX": -2.0772466399344327, + "velocityY": -0.9574768953653868, + "timestamp": 9.58133247442024 }, { - "x": 4.805556774139404, - "y": 4.485479354858398, - "heading": -3.0154063280737095e-24, - "angularVelocity": -0.11886467466269138, - "velocityX": 2.394177429870417, - "velocityY": -2.752084488523512, - "timestamp": 9.84772579354123 + "x": 2.2628964345938334, + "y": 6.74663530878728, + "heading": 0.298961324703692, + "angularVelocity": -0.41109973345399, + "velocityX": -2.3403236138079526, + "velocityY": -1.2348348534695668, + "timestamp": 9.643054790615563 }, { - "x": 5.0265563719767545, - "y": 4.290478604211027, - "heading": -0.0011711585949722948, - "angularVelocity": -0.014781576411206173, - "velocityX": 2.789308345003365, - "velocityY": -2.461168374898756, - "timestamp": 9.926956760685195 + "x": 2.1156790935491716, + "y": 6.646996400731561, + "heading": 0.26784048264776694, + "angularVelocity": -0.5042072944482706, + "velocityX": -2.385155809428528, + "velocityY": -1.6143092838642825, + "timestamp": 9.704777106810887 }, { - "x": 5.271648004487176, - "y": 4.1260554019145586, - "heading": -0.0011711658442560332, - "angularVelocity": -9.149558562553705e-8, - "velocityX": 3.0933818094771004, - "velocityY": -2.075239117019812, - "timestamp": 10.006187727829161 + "x": 1.9869123530220612, + "y": 6.532612256734745, + "heading": 0.23448482199122617, + "angularVelocity": -0.540414921419745, + "velocityX": -2.0862266432066443, + "velocityY": -1.8532056320576245, + "timestamp": 9.766499423006211 }, { - "x": 5.5362482788476575, - "y": 3.9953205544154557, - "heading": -0.0011711670074451403, - "angularVelocity": -1.4680990893194224e-8, - "velocityX": 3.3396067711718436, - "velocityY": -1.650047351581014, - "timestamp": 10.085418694973127 + "x": 1.881043758757729, + "y": 6.412413108766105, + "heading": 0.20123842984354556, + "angularVelocity": -0.5386445972388703, + "velocityX": -1.7152401398758959, + "velocityY": -1.9474179742098816, + "timestamp": 9.828221739201535 }, { - "x": 5.815753541872799, - "y": 3.900548632610082, - "heading": -0.0011711678822933615, - "angularVelocity": -1.1041746080675712e-8, - "velocityX": 3.5277275174146143, - "velocityY": -1.196147481491295, - "timestamp": 10.164649662117093 + "x": 1.7987115747047, + "y": 6.290184489368743, + "heading": 0.16896416101959177, + "angularVelocity": -0.5228946483767686, + "velocityX": -1.3339127422322181, + "velocityY": -1.9802986493663424, + "timestamp": 9.889944055396859 }, { - "x": 6.105300821187424, - "y": 3.8433884686557884, - "heading": -0.001171168612339158, - "angularVelocity": -9.214147233371614e-9, - "velocityX": 3.654471095733393, - "velocityY": -0.7214371604278238, - "timestamp": 10.243880629261058 + "x": 1.740001559257507, + "y": 6.167843341827393, + "heading": 0.13807920449538028, + "angularVelocity": -0.5003855724803669, + "velocityX": -0.9511959217700356, + "velocityY": -1.9821217848370036, + "timestamp": 9.951666371592182 }, { - "x": 6.399852439542986, - "y": 3.8248343191470044, - "heading": -0.0011711692753051952, - "angularVelocity": -8.367511605727905e-9, - "velocityX": 3.717632498671273, - "velocityY": -0.2341780010721126, - "timestamp": 10.323111596405024 + "x": 1.7034889997175517, + "y": 6.016151230649133, + "heading": 0.1019414560444192, + "angularVelocity": -0.467252098060329, + "velocityX": -0.4720983122052568, + "velocityY": -1.961341263510758, + "timestamp": 10.029007380013498 }, { - "x": 6.694725372224164, - "y": 3.8372788433696505, - "heading": -0.0011711699217424045, - "angularVelocity": -8.15889585625018e-9, - "velocityX": 3.721687912068309, - "velocityY": 0.15706641823560874, - "timestamp": 10.40234256354899 + "x": 1.7027290435050462, + "y": 5.874298147554192, + "heading": 0.06966485107958241, + "angularVelocity": -0.4173284732597485, + "velocityX": -0.009826044785523666, + "velocityY": -1.8341250778913685, + "timestamp": 10.106348388434814 }, { - "x": 6.9895982402615005, - "y": 3.8497248992335154, - "heading": -0.0011711705681793068, - "angularVelocity": -8.15889197720244e-9, - "velocityX": 3.7216870961771913, - "velocityY": 0.157085749581342, - "timestamp": 10.481573530692955 + "x": 1.7342944275877592, + "y": 5.750595898188055, + "heading": 0.04250435573420922, + "angularVelocity": -0.35117844853297475, + "velocityX": 0.40813256417294685, + "velocityY": -1.5994393128709739, + "timestamp": 10.18368939685613 }, { - "x": 7.283138211598938, - "y": 3.880370995696298, - "heading": -0.0011711735014489986, - "angularVelocity": -3.702175800028382e-8, - "velocityX": 3.7048641701427636, - "velocityY": 0.386794426061941, - "timestamp": 10.560804497836921 + "x": 1.7930814617897048, + "y": 5.6520476897956975, + "heading": 0.02146340510922574, + "angularVelocity": -0.2720542575597529, + "velocityX": 0.7601017287194328, + "velocityY": -1.2742038202491095, + "timestamp": 10.261030405277445 }, { - "x": 7.537795284833578, - "y": 3.9128988141490386, - "heading": -0.0022060584164621646, - "angularVelocity": -0.01306162164009358, - "velocityX": 3.2141103714147428, - "velocityY": 0.4105442559300831, - "timestamp": 10.640035464980887 + "x": 1.8734294527078426, + "y": 5.583639683859189, + "heading": 0.0071877988424821045, + "angularVelocity": -0.1845800379143917, + "velocityX": 1.0388795356848963, + "velocityY": -0.8844984999918234, + "timestamp": 10.338371413698761 }, { - "x": 7.753528037916665, - "y": 3.9448039763834073, - "heading": -0.003047966804873468, - "angularVelocity": -0.010626001660203437, - "velocityX": 2.722833771435503, - "velocityY": 0.40268550775602213, - "timestamp": 10.719266432124853 + "x": 1.970064237353583, + "y": 5.548515796661377, + "heading": 3.498573366455402e-27, + "angularVelocity": -0.09293645103935767, + "velocityX": 1.249463727177213, + "velocityY": -0.45414312425918, + "timestamp": 10.415712422120077 }, { - "x": 7.930345641638023, - "y": 3.9756628799518334, - "heading": -0.0034891865809292344, - "angularVelocity": -0.005568779379583401, - "velocityX": 2.23167291899988, - "velocityY": 0.3894803342783207, - "timestamp": 10.798497399268818 + "x": 2.0785037517547607, + "y": 5.548515796661377, + "heading": 9.074763160083063e-28, + "angularVelocity": -2.8883864105140666e-27, + "velocityX": 1.4020959464409022, + "velocityY": 1.1648614692434482e-25, + "timestamp": 10.493053430541393 }, { - "x": 8.068253246603454, - "y": 4.005300685276172, - "heading": -0.003443969014949724, - "angularVelocity": 0.0005707057178457502, - "velocityX": 1.7405770740479334, - "velocityY": 0.3740684531905991, - "timestamp": 10.877728366412784 + "x": 2.2093539983390484, + "y": 5.548515796661377, + "heading": -5.091869940539703e-28, + "angularVelocity": -8.863496880486678e-29, + "velocityX": 1.8425023918344958, + "velocityY": 6.335504596026049e-17, + "timestamp": 10.56407111125876 }, { - "x": 8.167254001972685, - "y": 4.033621928989228, - "heading": -0.0028654925288464014, - "angularVelocity": 0.007301141295577129, - "velocityX": 1.2495209756728338, - "velocityY": 0.3574516976625533, - "timestamp": 10.95695933355675 + "x": 2.3657372440174322, + "y": 5.548515796661377, + "heading": -1.9258503020667073e-27, + "angularVelocity": -8.863493994530242e-29, + "velocityX": 2.202032565675437, + "velocityY": 3.167727717458953e-16, + "timestamp": 10.635088791976129 }, { - "x": 8.227350013878981, - "y": 4.060566470670501, - "heading": -0.0017242628170792867, - "angularVelocity": 0.014403834168696394, - "velocityX": 0.7584914594958708, - "velocityY": 0.340075890179585, - "timestamp": 11.036190300700715 + "x": 2.4908438453710073, + "y": 5.548515796661377, + "heading": -3.342513611103023e-27, + "angularVelocity": -8.863495435827685e-29, + "velocityX": 1.7616261202821912, + "velocityY": 2.534184135606925e-16, + "timestamp": 10.706106472693497 }, { - "x": 8.248542785644531, - "y": 4.086092948913574, - "heading": 1.0796890687236353e-23, - "angularVelocity": 0.021762486048494588, - "velocityX": 0.2674809172408869, - "velocityY": 0.32217804683224427, - "timestamp": 11.115421267844681 + "x": 2.5846737979725942, + "y": 5.548515796661377, + "heading": -4.759176920139342e-27, + "angularVelocity": -8.863495435827717e-29, + "velocityX": 1.321219612549812, + "velocityY": 1.9006387498434594e-16, + "timestamp": 10.777124153410865 }, { - "x": 8.231523012343859, - "y": 4.109892958457586, - "heading": 0.0022863134326364947, - "angularVelocity": 0.029215203502289427, - "velocityX": -0.21748380315842067, - "velocityY": 0.3041237094875908, - "timestamp": 11.193678926822047 + "x": 2.6472271003464662, + "y": 5.548515796661377, + "heading": -6.175840229175665e-27, + "angularVelocity": -8.863495435827698e-29, + "velocityX": 0.8808130840377387, + "velocityY": 1.2670927613090377e-16, + "timestamp": 10.848141834128233 }, { - "x": 8.176551036095814, - "y": 4.132280075994507, - "heading": 0.005155847293198806, - "angularVelocity": 0.03666777026121207, - "velocityX": -0.7024485138757453, - "velocityY": 0.2860693487316991, - "timestamp": 11.271936585799413 + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": -7.592503538214036e-27, + "angularVelocity": -8.863495438700938e-29, + "velocityX": 0.44040654513582195, + "velocityY": 6.3354647036682e-17, + "timestamp": 10.919159514845601 }, { - "x": 8.083626857911096, - "y": 4.153254299063425, - "heading": 0.008608587113368522, - "angularVelocity": 0.04412015213959263, - "velocityX": -1.1874132116780545, - "velocityY": 0.26801495652947127, - "timestamp": 11.350194244776779 + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": -9.168166896056586e-27, + "angularVelocity": -2.3275147262701813e-27, + "velocityX": -7.876142714980879e-26, + "velocityY": -2.7822667519177177e-25, + "timestamp": 10.99017719556297 }, { - "x": 7.952750479206828, - "y": 4.172815624183239, - "heading": 0.012644515987955914, - "angularVelocity": 0.051572317998353566, - "velocityX": -1.6723778913719014, - "velocityY": 0.24996051984474485, - "timestamp": 11.428451903754144 + "x": 2.6551487281624913, + "y": 5.535337362195577, + "heading": -0.0053582663887861755, + "angularVelocity": -0.08143329787865013, + "velocityX": -0.3549425234125067, + "velocityY": -0.20028182653884286, + "timestamp": 11.055976647732658 }, { - "x": 7.783921902112919, - "y": 4.190964046074225, - "heading": 0.017263614398582115, - "angularVelocity": 0.059024234445374815, - "velocityX": -2.1573425438491505, - "velocityY": 0.2319060156940586, - "timestamp": 11.50670956273151 + "x": 2.608944125248311, + "y": 5.508126235802502, + "heading": -0.016249929633628882, + "angularVelocity": -0.16552817517013058, + "velocityX": -0.7022034590048659, + "velocityY": -0.41354639736059495, + "timestamp": 11.121776099902347 }, { - "x": 7.57714113018582, - "y": 4.207699555855796, - "heading": 0.022465858708652038, - "angularVelocity": 0.06647584885684583, - "velocityX": -2.642307150880996, - "velocityY": 0.2138513980645823, - "timestamp": 11.584967221708876 + "x": 2.5407349136089206, + "y": 5.4655968271259, + "heading": -0.03293816472196888, + "angularVelocity": -0.2536227056313985, + "velocityX": -1.0366227892519315, + "velocityY": -0.6463489782091855, + "timestamp": 11.187575552072035 + }, + { + "x": 2.4521626111212957, + "y": 5.405634188324023, + "heading": -0.05584758703948852, + "angularVelocity": -0.34817041118274633, + "velocityX": -1.3460948315984256, + "velocityY": -0.9112938911290825, + "timestamp": 11.253375004241724 }, { - "x": 7.332408170546637, - "y": 4.223022135701224, - "heading": 0.028251214754351595, - "angularVelocity": 0.07392702671278313, - "velocityX": -3.1272716669171876, - "velocityY": 0.19579655263979157, - "timestamp": 11.663224880686242 + "x": 2.347299393995622, + "y": 5.324395116461008, + "heading": -0.08569447081932852, + "angularVelocity": -0.45360383400866283, + "velocityX": -1.593679182240697, + "velocityY": -1.2346466297851608, + "timestamp": 11.319174456411412 }, { - "x": 7.049723044551417, - "y": 4.23693173264099, - "heading": 0.03461960291604749, - "angularVelocity": 0.08137718716500116, - "velocityX": -3.6122359100593893, - "velocityY": 0.1777410303544707, - "timestamp": 11.741482539663608 + "x": 2.239880929566337, + "y": 5.216483135728542, + "heading": -0.12272614687803589, + "angularVelocity": -0.5627961151288577, + "velocityX": -1.6325130512069779, + "velocityY": -1.6400133614209311, + "timestamp": 11.3849739085811 }, { - "x": 6.758414971904035, - "y": 4.226087741393702, - "heading": 0.03461960351130125, - "angularVelocity": 7.606332300457526e-9, - "velocityX": -3.7224225264857154, - "velocityY": -0.13856779501191718, - "timestamp": 11.819740198640973 + "x": 2.1533744032985735, + "y": 5.091739159361965, + "heading": -0.16180305396789932, + "angularVelocity": -0.5938789123819668, + "velocityX": -1.314699794835295, + "velocityY": -1.8958208959685579, + "timestamp": 11.450773360750789 }, { - "x": 6.467106926605873, - "y": 4.215243015473263, - "heading": 0.03461960410653643, - "angularVelocity": 7.606094897280975e-9, - "velocityX": -3.7224221770091352, - "velocityY": -0.13857718288730186, - "timestamp": 11.89799785761834 + "x": 2.0932393074035645, + "y": 4.9619622230529785, + "heading": -0.19963034769256288, + "angularVelocity": -0.5748876696892883, + "velocityX": -0.9139148414173547, + "velocityY": -1.9723102857195456, + "timestamp": 11.516572812920478 }, { - "x": 6.175798881305749, - "y": 4.204398289605515, - "heading": 0.03461960470177156, - "angularVelocity": 7.606094342014038e-9, - "velocityX": -3.7224221770342005, - "velocityY": -0.1385771822140065, - "timestamp": 11.976255516595705 + "x": 2.06290111070071, + "y": 4.862373019847466, + "heading": -0.22719939375272255, + "angularVelocity": -0.5460591677053325, + "velocityX": -0.6009076413123203, + "velocityY": -1.9725599970405536, + "timestamp": 11.56706010017913 }, { - "x": 5.8844906838954465, - "y": 4.193557650451513, - "heading": 0.03461960529700769, - "angularVelocity": 7.606107036408581e-9, - "velocityX": -3.7224241207439843, - "velocityY": -0.13852496095158084, - "timestamp": 12.05451317557307 + "x": 2.048028599586168, + "y": 4.7660067703914, + "heading": -0.2526003668147138, + "angularVelocity": -0.5031162187791788, + "velocityX": -0.29457932723436914, + "velocityY": -1.9087230605671173, + "timestamp": 11.617547387437783 }, { - "x": 5.594009037113428, - "y": 4.218019822631225, - "heading": 0.034619605911650746, - "angularVelocity": 7.854094608871324e-9, - "velocityX": -3.7118622071998786, - "velocityY": 0.31258502361778046, - "timestamp": 12.132770834550437 + "x": 2.047740775609064, + "y": 4.67569763435831, + "heading": -0.27523200740159587, + "angularVelocity": -0.4482641436236724, + "velocityX": -0.00570091983015679, + "velocityY": -1.788750018800283, + "timestamp": 11.668034674696436 }, { - "x": 5.309172831526295, - "y": 4.280038516789021, - "heading": 0.034619599952821836, - "angularVelocity": -7.614371543607384e-8, - "velocityX": -3.639723054704678, - "velocityY": 0.7924936034150137, - "timestamp": 12.211028493527802 + "x": 2.0608710405337267, + "y": 4.5937072504707785, + "heading": -0.29463334748351117, + "angularVelocity": -0.3842816902108432, + "velocityX": 0.26007071557230893, + "velocityY": -1.62398077495204, + "timestamp": 11.718521961955089 }, { - "x": 5.03935943827081, - "y": 4.376755129785837, - "heading": 0.01971114929830447, - "angularVelocity": -0.19050468477250748, - "velocityX": -3.4477570218849003, - "velocityY": 1.235874089011402, - "timestamp": 12.289286152505168 + "x": 2.0861920181883997, + "y": 4.5217424317614965, + "heading": -0.31047411563844995, + "angularVelocity": -0.3137575618548228, + "velocityX": 0.5015317524380114, + "velocityY": -1.4254047427939316, + "timestamp": 11.769009249213742 }, { - "x": 4.805556774139404, - "y": 4.485479354858398, - "heading": -1.0813807424036439e-23, - "angularVelocity": -0.25187501844395294, - "velocityX": -2.987601050001106, - "velocityY": 1.389310982379468, - "timestamp": 12.367543811482534 + "x": 2.1225463448038515, + "y": 4.461060494351439, + "heading": -0.3225286157246152, + "angularVelocity": -0.2387630776121657, + "velocityX": 0.7200689240680372, + "velocityY": -1.2019250925322578, + "timestamp": 11.819496536472395 }, { - "x": 4.627457882261689, - "y": 4.587679677136334, - "heading": -0.019778853338755203, - "angularVelocity": -0.2848710655643308, - "velocityX": -2.5651244910957094, - "velocityY": 1.4719718180672714, - "timestamp": 12.43697470752518 + "x": 2.16890020662259, + "y": 4.412584630951135, + "heading": -0.3306480218329806, + "angularVelocity": -0.16082080359692727, + "velocityX": 0.9181293813878825, + "velocityY": -0.9601597953155385, + "timestamp": 11.869983823731047 }, { - "x": 4.478940134439646, - "y": 4.685562428002016, - "heading": -0.03941675041867698, - "angularVelocity": -0.28284089935782175, - "velocityX": -2.1390728953118847, - "velocityY": 1.4097866575934486, - "timestamp": 12.506405603567828 + "x": 2.2243532862123785, + "y": 4.376998478332531, + "heading": -0.3347371673945201, + "angularVelocity": -0.08099356855103614, + "velocityX": 1.0983572816201457, + "velocityY": -0.7048537275590007, + "timestamp": 11.9204711109897 }, { - "x": 4.358424662792702, - "y": 4.772986561558444, - "heading": -0.05736759927865439, - "angularVelocity": -0.2585426644782356, - "velocityX": -1.7357614335398788, - "velocityY": 1.2591531802027813, - "timestamp": 12.575836499610475 + "x": 2.2881293296813965, + "y": 4.354815483093262, + "heading": -0.3347371673945201, + "angularVelocity": -3.351040193936762e-27, + "velocityX": 1.2632099471355258, + "velocityY": -0.43937784031895566, + "timestamp": 11.970958398248353 }, { - "x": 4.264251963016929, - "y": 4.846292896298891, - "heading": -0.07266630938102059, - "angularVelocity": -0.220344414005101, - "velocityX": -1.3563514968599655, - "velocityY": 1.0558172070171679, - "timestamp": 12.645267395653121 + "x": 2.4097151173059284, + "y": 4.31443319431648, + "heading": -0.3347371673945201, + "angularVelocity": -1.1055944271274773e-28, + "velocityX": 1.6917387541138205, + "velocityY": -0.5618772081689679, + "timestamp": 12.042828707614305 }, { - "x": 4.19504667897931, - "y": 4.903178017800909, - "heading": -0.0846829764598402, - "angularVelocity": -0.17307377210627528, - "velocityX": -0.9967505531703156, - "velocityY": 0.8193055936809015, - "timestamp": 12.714698291695768 + "x": 2.501147235462863, + "y": 4.284859003573599, + "heading": -0.3347371673945201, + "angularVelocity": -1.1055944267694559e-28, + "velocityX": 1.2721820590944748, + "velocityY": -0.4114938561387572, + "timestamp": 12.114699016980257 }, { - "x": 4.149718350868284, - "y": 4.942098244876492, - "heading": -0.09298341563379672, - "angularVelocity": -0.11954964788093829, - "velocityX": -0.6528552948990191, - "velocityY": 0.5605606335784127, - "timestamp": 12.784129187738415 + "x": 2.562139216145648, + "y": 4.265257651838079, + "heading": -0.3347371673945201, + "angularVelocity": -1.105594426769472e-28, + "velocityX": 0.8486394621209057, + "velocityY": -0.2727322577076049, + "timestamp": 12.18656932634621 }, { - "x": 4.127401828765869, - "y": 4.96196174621582, - "heading": -0.09725287529259724, - "angularVelocity": -0.06149221603273053, - "velocityX": -0.32142062647019837, - "velocityY": 0.2860902346287965, - "timestamp": 12.853560083781062 + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": -1.1055944267596697e-28, + "velocityX": 0.42445170461571047, + "velocityY": -0.13595549514112273, + "timestamp": 12.258439635712161 }, { - "x": 4.127401828765869, - "y": 4.96196174621582, - "heading": -0.09725287529259724, - "angularVelocity": -7.045263803296892e-25, - "velocityX": -1.4084469220925396e-25, - "velocityY": -2.7940612123980634e-25, - "timestamp": 12.922990979823709 + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": -2.916408988560576e-29, + "velocityX": -2.969429335311962e-26, + "velocityY": 1.2754135405824192e-26, + "timestamp": 12.330309945078113 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 0.43297290802001953, - "y": 6.9807281494140625, + "x": 1.3350272178649902, + "y": 5.601006507873535, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 10 }, { - "timestamp": 0.34959777724704905, + "timestamp": 0.5891141473001675, "isStopPoint": false, - "x": 0.8667811155319214, - "y": 6.895569324493408, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "timestamp": 1.1040901645422223, - "isStopPoint": true, - "x": 2.286245346069336, - "y": 6.686196804046631, - "heading": 0.5125504196, + "x": 2.3368515968322754, + "y": 6.133185386657715, + "heading": 0.2525545797921912, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 16 }, { - "timestamp": 1.8634374124793154, - "isStopPoint": true, - "x": 3.062049150466919, - "y": 7.130331993103027, - "heading": 0.5125504196, + "timestamp": 1.5430433785236333, + "isStopPoint": false, + "x": 5.747424125671387, + "y": 7.119814872741699, + "heading": 0.2525545797921912, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 20 + "controlIntervalCount": 11 }, { - "timestamp": 3.2273332408433504, + "timestamp": 1.9430147539809723, "isStopPoint": false, "x": 7.125290870666504, "y": 7.458, @@ -28781,7 +28655,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 3.7636275381592217, + "timestamp": 2.508796597442937, "isStopPoint": false, "x": 8.186561584472656, "y": 7.469284534454346, @@ -28792,18 +28666,18 @@ "controlIntervalCount": 19 }, { - "timestamp": 5.598433000555465, + "timestamp": 4.2320699051573625, "isStopPoint": true, - "x": 4.144390106201172, - "y": 5.838942050933838, - "heading": 0.13255114594654763, + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 12 }, { - "timestamp": 6.3874526467093045, + "timestamp": 4.9229006835446425, "isStopPoint": false, "x": 5.851044178009033, "y": 6.557126998901367, @@ -28814,7 +28688,7 @@ "controlIntervalCount": 12 }, { - "timestamp": 6.776829703600045, + "timestamp": 5.308843742309995, "isStopPoint": false, "x": 7.214789390563965, "y": 6.145846366882324, @@ -28825,7 +28699,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 7.328651724730054, + "timestamp": 5.861500074494232, "isStopPoint": false, "x": 8.186561584472656, "y": 5.798520088195801, @@ -28836,7 +28710,7 @@ "controlIntervalCount": 15 }, { - "timestamp": 8.321094170638265, + "timestamp": 6.845550680900895, "isStopPoint": false, "x": 5.851044178009033, "y": 6.557126998901367, @@ -28844,46 +28718,79 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 12 + "controlIntervalCount": 13 }, { - "timestamp": 9.158573440155276, - "isStopPoint": true, - "x": 4.144390106201172, - "y": 5.838942050933838, - "heading": 0.13255114594654763, + "timestamp": 7.507867232153339, + "isStopPoint": false, + "x": 3.405486822128296, + "y": 6.266934394836426, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 9 }, { - "timestamp": 9.84772579354123, + "timestamp": 7.9885953573901745, "isStopPoint": false, - "x": 4.805556774139404, - "y": 4.485479354858398, + "x": 2.205486822128296, + "y": 6.266934394836426, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 8 }, { - "timestamp": 11.115421267844681, + "timestamp": 8.457665576202594, + "isStopPoint": true, + "x": 2.052253484725952, + "y": 6.674348831176758, + "heading": 0.3930579718029317, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 9.210998577248297, + "isStopPoint": true, + "x": 2.8644089698791504, + "y": 7.012746810913086, + "heading": 0.3930579718029317, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 9.951666371592182, "isStopPoint": false, - "x": 8.248542785644531, - "y": 4.086092948913574, + "x": 1.7400015592575073, + "y": 6.167843341827393, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "headingConstrained": false, + "controlIntervalCount": 7 }, { - "timestamp": 12.367543811482534, + "timestamp": 10.493053430541393, "isStopPoint": false, - "x": 4.805556774139404, - "y": 4.485479354858398, + "x": 2.0785037517547607, + "y": 5.548515796661377, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 10.99017719556297, + "isStopPoint": true, + "x": 2.6785037517547607, + "y": 5.548515796661377, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -28891,11 +28798,33 @@ "controlIntervalCount": 8 }, { - "timestamp": 12.922990979823709, + "timestamp": 11.516572812920478, + "isStopPoint": false, + "x": 2.0932393074035645, + "y": 4.9619622230529785, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "timestamp": 11.970958398248353, + "isStopPoint": false, + "x": 2.2881293296813965, + "y": 4.354815483093262, + "heading": -0.3347371673945201, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "timestamp": 12.330309945078113, "isStopPoint": true, - "x": 4.127401828765869, - "y": 4.96196174621582, - "heading": -0.09725287529259725, + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -28917,40 +28846,75 @@ }, { "scope": [ - 6 + 5 ], "type": "StopPoint" }, { "scope": [ - 11 + 12 ], "type": "StopPoint" }, { "scope": [ - 0, - 1 + 1, + 2 ], - "type": "ZeroAngularVelocity" + "type": "StraightLine" }, { "scope": [ - 2 + 13 ], "type": "StopPoint" }, { "scope": [ - 3 + 10, + 11 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 16 ], "type": "StopPoint" }, { "scope": [ - 11 + 19 ], "type": "StopPoint" + }, + { + "scope": [ + 15, + 16 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 18, + 19 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 18 + ], + "type": "WptVelocityDirection", + "direction": -0.3347371673945201 + }, + { + "scope": [ + 15 + ], + "type": "WptVelocityDirection", + "direction": 0 } ], "usesControlIntervalGuessing": true, @@ -28960,183 +28924,156 @@ "eventMarkers": [], "isTrajectoryStale": false }, - "CenterLanePDEABC": { + "SourceLanePHGF": { "waypoints": [ { - "x": 1.3350272178649902, - "y": 5.601006507873535, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 - }, - { - "x": 2.3368515968322754, - "y": 6.133185386657715, - "heading": 0.2525545797921912, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 5.747424125671387, - "y": 7.119814872741699, - "heading": 0.2525545797921912, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 7.125290870666504, - "y": 7.458, + "x": 0.387, + "y": 4.121134281158447, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 14 }, { - "x": 8.186561584472656, - "y": 7.469284534454346, - "heading": 0, + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 10 }, { - "x": 4.337841033935547, - "y": 6.292478084564209, - "heading": 0.2639641302660189, + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 16 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": 0, + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, + "headingConstrained": true, "controlIntervalCount": 12 }, { - "x": 7.214789390563965, - "y": 6.145846366882324, - "heading": -0.344987478573796, + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 7 }, { - "x": 8.186561584472656, - "y": 5.798520088195801, - "heading": -0.344987478573796, + "x": 8.208122253417969, + "y": 0.763107419013977, + "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, + "x": 5.5030035972595215, + "y": 1.563418984413147, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 18 }, { - "x": 3.405486822128296, - "y": 6.266934394836426, - "heading": 0, + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 18 }, { - "x": 2.205486822128296, - "y": 6.266934394836426, + "x": 5.5030035972595215, + "y": 1.563418984413147, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 8 + "headingConstrained": false, + "controlIntervalCount": 19 }, { - "x": 2.052253484725952, - "y": 6.674348831176758, - "heading": 0.3930579718029317, + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 10 }, { - "x": 2.8644089698791504, - "y": 7.012746810913086, - "heading": 0.3930579718029317, + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": 0.9600708878718816, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 + "headingConstrained": false, + "controlIntervalCount": 18 }, { - "x": 1.7400015592575073, - "y": 6.167843341827393, - "heading": 0, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 7 + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 2.0785037517547607, - "y": 5.548515796661377, - "heading": 0, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 9 }, { - "x": 2.6785037517547607, - "y": 5.548515796661377, - "heading": 0, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 15 }, { - "x": 2.0932393074035645, - "y": 4.9619622230529785, + "x": 8.207907676696777, + "y": 4.097138404846191, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 9 + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 2.2881293296813965, - "y": 4.354815483093262, - "heading": -0.3347371673945201, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 9 }, { - "x": 2.592644691467285, - "y": 4.255486488342285, - "heading": -0.3347371673945201, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -29145,2022 +29082,2133 @@ ], "trajectory": [ { - "x": 1.3350272178649902, - "y": 5.601006507873535, - "heading": 1.1438500584259617e-25, - "angularVelocity": 2.0139373174315835e-26, - "velocityX": -2.9204847474037974e-26, - "velocityY": 3.705827554701837e-26, + "x": 0.387, + "y": 4.121134281158447, + "heading": 5.0336126459058196e-26, + "angularVelocity": 1.4969503738616677e-26, + "velocityX": 6.632423953121246e-26, + "velocityY": 2.3403167387234696e-25, "timestamp": 0 }, { - "x": 1.3513037651461415, - "y": 5.614995878157814, - "heading": 0.006549885022033663, - "angularVelocity": 0.1111819339605223, - "velocityX": 0.27628851481066125, - "velocityY": 0.23746451088284168, - "timestamp": 0.05891141473001675 + "x": 0.40051040035567387, + "y": 4.115348381940811, + "heading": -0.015277315995300885, + "angularVelocity": -0.30891158633599436, + "velocityX": 0.2731840597647725, + "velocityY": -0.11699249437857151, + "timestamp": 0.04945530265311636 }, { - "x": 1.384378906601625, - "y": 5.642351283609904, - "heading": 0.019354935835911788, - "angularVelocity": 0.21736111537232614, - "velocityX": 0.5614385871916767, - "velocityY": 0.46434813316664775, - "timestamp": 0.1178228294600335 + "x": 0.42755206280004615, + "y": 4.103759116150659, + "heading": -0.04541480648334463, + "angularVelocity": -0.6093884552568736, + "velocityX": 0.5467899495842703, + "velocityY": -0.23433818354000846, + "timestamp": 0.09891060530623272 }, { - "x": 1.4348748758417986, - "y": 5.682261570187258, - "heading": 0.0380334905813409, - "angularVelocity": 0.3170617244727609, - "velocityX": 0.8571508505030827, - "velocityY": 0.6774627083776107, - "timestamp": 0.17673424419005024 + "x": 0.46814907636931, + "y": 4.086345867978605, + "heading": -0.08991129386238127, + "angularVelocity": -0.8997313734209399, + "velocityX": 0.8208829264278229, + "velocityY": -0.3521007301116199, + "timestamp": 0.14836590795934906 + }, + { + "x": 0.5223311730580029, + "y": 4.063083084813687, + "heading": -0.14811250055404074, + "angularVelocity": -1.1768446166407593, + "velocityX": 1.095577092485525, + "velocityY": -0.4703799575969681, + "timestamp": 0.19782121061246544 + }, + { + "x": 0.5901369333153729, + "y": 4.033939247984461, + "heading": -0.2191152852440613, + "angularVelocity": -1.4356960908325656, + "velocityX": 1.3710513659771775, + "velocityY": -0.5892965014013483, + "timestamp": 0.2472765132655818 + }, + { + "x": 0.6716171763309451, + "y": 3.9988769782710625, + "heading": -0.3016414776381226, + "angularVelocity": -1.6687026055205227, + "velocityX": 1.6475532176414214, + "velocityY": -0.7089688634468179, + "timestamp": 0.2967318159186982 + }, + { + "x": 0.766836765570499, + "y": 3.957853748642466, + "heading": -0.39387111054068624, + "angularVelocity": -1.864908876394308, + "velocityX": 1.9253666266574487, + "velocityY": -0.8295011339094811, + "timestamp": 0.34618711857181456 + }, + { + "x": 0.8758711660110932, + "y": 3.910821247404655, + "heading": -0.49317423957562295, + "angularVelocity": -2.007936939168226, + "velocityX": 2.2047059585373607, + "velocityY": -0.9510102802868479, + "timestamp": 0.39564242122493093 + }, + { + "x": 0.998787530743033, + "y": 3.857723681334368, + "heading": -0.5954683580210677, + "angularVelocity": -2.0684155784657574, + "velocityX": 2.4854031446149563, + "velocityY": -1.0736475811849282, + "timestamp": 0.4450977238780473 + }, + { + "x": 1.1355416361282653, + "y": 3.7985302060221593, + "heading": -0.6929936795382847, + "angularVelocity": -1.9719891757870285, + "velocityX": 2.7652061164085326, + "velocityY": -1.1969085646365696, + "timestamp": 0.4945530265311637 + }, + { + "x": 1.284369196894048, + "y": 3.7333344256591525, + "heading": -0.760822351237353, + "angularVelocity": -1.3715146417124207, + "velocityX": 3.009334748382224, + "velocityY": -1.3182768452615752, + "timestamp": 0.54400832918428 + }, + { + "x": 1.444068666194049, + "y": 3.662157825629303, + "heading": -0.7899998870158123, + "angularVelocity": -0.589977903544803, + "velocityX": 3.229167768320948, + "velocityY": -1.4392106854362665, + "timestamp": 0.5934636318373964 }, { - "x": 1.5035350329878103, - "y": 5.733639392412991, - "heading": 0.06208045701070026, - "angularVelocity": 0.4081885749911699, - "velocityX": 1.1654813835429356, - "velocityY": 0.8721199866136561, - "timestamp": 0.235645658920067 + "x": 1.6133635999437081, + "y": 3.589517549083241, + "heading": -0.7899999435088091, + "angularVelocity": -0.0000011423041352586315, + "velocityX": 3.42319073319818, + "velocityY": -1.4688066324367013, + "timestamp": 0.6429189344905127 }, { - "x": 1.5912385139518137, - "y": 5.794977395067809, - "heading": 0.09080789039928205, - "angularVelocity": 0.48763781213260343, - "velocityX": 1.4887349313530633, - "velocityY": 1.0411904541067623, - "timestamp": 0.29455707365008377 + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "angularVelocity": -0.000001142267622119268, + "velocityX": 3.4231940186144136, + "velocityY": -1.4687989754737478, + "timestamp": 0.6923742371436291 }, { - "x": 1.6989811168528033, - "y": 5.864111533799202, - "heading": 0.12324878614182731, - "angularVelocity": 0.550672495155949, - "velocityX": 1.8288917927834498, - "velocityY": 1.1735270498633483, - "timestamp": 0.35346848838010053 + "x": 2.023972333671043, + "y": 3.4134441767348855, + "heading": -0.79, + "angularVelocity": -1.7456316689050396e-17, + "velocityX": 3.4237473592821543, + "velocityY": -1.4675096226857507, + "timestamp": 0.7628565514540566 }, { - "x": 1.8277399268235333, - "y": 5.93784849269496, - "heading": 0.15800938597032024, - "angularVelocity": 0.590048634679649, - "velocityX": 2.1856343216474197, - "velocityY": 1.2516582606899704, - "timestamp": 0.4123799031101173 + "x": 2.250495583067251, + "y": 3.3163502598603043, + "heading": -0.79, + "angularVelocity": -2.7198862157697797e-16, + "velocityX": 3.2139019782824447, + "velocityY": -1.37756425600538, + "timestamp": 0.8333388657644841 }, { - "x": 1.978020614375797, - "y": 6.011501208422767, - "heading": 0.19307007056378325, - "angularVelocity": 0.5951424652444947, - "velocityX": 2.5509604249190123, - "velocityY": 1.250228263322945, - "timestamp": 0.47129131784013406 + "x": 2.4487034476982825, + "y": 3.2313930734184306, + "heading": -0.79, + "angularVelocity": -2.2318862582403906e-16, + "velocityX": 2.8121645347520525, + "velocityY": -1.2053688542021173, + "timestamp": 0.9038211800749116 }, { - "x": 2.1488551922208927, - "y": 6.078800735840598, - "heading": 0.2256402887317746, - "angularVelocity": 0.5528676966468451, - "velocityX": 2.899855293375791, - "velocityY": 1.1423851850487134, - "timestamp": 0.5302027325701508 + "x": 2.6185959113515174, + "y": 3.1585726243584236, + "heading": -0.79, + "angularVelocity": -1.6717248939922967e-16, + "velocityX": 2.410426861197717, + "velocityY": -1.0331733538044932, + "timestamp": 0.9743034943853391 }, { - "x": 2.3368515968322754, - "y": 6.133185386657715, - "heading": 0.2525545797921912, - "angularVelocity": 0.45686037559548065, - "velocityX": 3.1911711078904768, - "velocityY": 0.9231598165882506, - "timestamp": 0.5891141473001675 + "x": 2.760172968622745, + "y": 3.0978889149966715, + "heading": -0.79, + "angularVelocity": -1.1384117019167077e-16, + "velocityX": 2.0086891109686777, + "velocityY": -0.8609778205420586, + "timestamp": 1.0447858086957666 }, { - "x": 2.5473350968820796, - "y": 6.1940752301450726, - "heading": 0.25255484664390665, - "angularVelocity": 0.000004475832490919022, - "velocityX": 3.5303834818832023, - "velocityY": 1.0212890683182199, - "timestamp": 0.6487347242516341 + "x": 2.8734346168098592, + "y": 3.049341946491368, + "heading": -0.79, + "angularVelocity": -1.4459082642914294e-16, + "velocityX": 1.6069513224022822, + "velocityY": -0.6887822708472163, + "timestamp": 1.1152681230061943 }, { - "x": 2.7606743654688186, - "y": 6.255791206318409, - "heading": 0.25255482885683184, - "angularVelocity": -2.9833785118107346e-7, - "velocityX": 3.578282524176262, - "velocityY": 1.0351455710261694, - "timestamp": 0.7083553012031008 + "x": 2.958380854291598, + "y": 3.0129317195374297, + "heading": -0.79, + "angularVelocity": -1.2018162111442988e-16, + "velocityX": 1.2052135108334716, + "velocityY": -0.5165867112929291, + "timestamp": 1.1857504373166219 }, { - "x": 2.974013634055561, - "y": 6.317507182491745, - "heading": 0.2525548110697642, - "angularVelocity": -2.983377309458156e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261857, - "timestamp": 0.7679758781545674 + "x": 3.015011679987118, + "y": 2.9886582345981343, + "heading": -0.79, + "angularVelocity": -6.358045799066146e-17, + "velocityX": 0.8034756839297176, + "velocityY": -0.3443911451656786, + "timestamp": 1.2562327516270495 }, { - "x": 3.1873529026423038, - "y": 6.379223158665082, - "heading": 0.25255479328269653, - "angularVelocity": -2.983377305484016e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.827596455106034 + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -1.168085544952961e-16, + "velocityX": 0.40173784607243274, + "velocityY": -0.17219557434345445, + "timestamp": 1.3267150659374771 }, { - "x": 3.400692171229046, - "y": 6.440939134838418, - "heading": 0.2525547754956289, - "angularVelocity": -2.9833773174604553e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261857, - "timestamp": 0.8872170320575006 + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 1.4132283846579156e-28, + "velocityX": -7.2110997365083e-27, + "velocityY": -2.0111921052200978e-27, + "timestamp": 1.3971973802479047 }, { - "x": 3.6140314398157884, - "y": 6.502655111011755, - "heading": 0.25255475770856123, - "angularVelocity": -2.983377305928146e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 0.9468376090089672 + "x": 3.065434104608151, + "y": 2.9635744703134126, + "heading": -0.7762472311002773, + "angularVelocity": 0.2130317791406229, + "velocityX": 0.34243984045734815, + "velocityY": -0.2005506735052959, + "timestamp": 1.4617547385688177 }, { - "x": 3.827370708402531, - "y": 6.5643710871850915, - "heading": 0.25255473992149363, - "angularVelocity": -2.983377301697445e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.0064581859604338 + "x": 3.109663922209824, + "y": 2.9376745027807694, + "heading": -0.7492642309726533, + "angularVelocity": 0.41796939697396296, + "velocityX": 0.6851243413927804, + "velocityY": -0.40119311270289487, + "timestamp": 1.5263120968897306 }, { - "x": 4.040709976989273, - "y": 6.626087063358428, - "heading": 0.25255472213442604, - "angularVelocity": -2.983377305025075e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261857, - "timestamp": 1.0660787629119004 + "x": 3.176036274163731, + "y": 2.8988141772022082, + "heading": -0.7097339632553362, + "angularVelocity": 0.612327839079368, + "velocityX": 1.028114434670201, + "velocityY": -0.6019503676929793, + "timestamp": 1.5908694552106435 }, { - "x": 4.254049245576016, - "y": 6.687803039531764, - "heading": 0.2525547043473584, - "angularVelocity": -2.9833773095408974e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.125699339863367 + "x": 3.2645764628811422, + "y": 2.8469841623353034, + "heading": -0.6585853448584378, + "angularVelocity": 0.7922972644363798, + "velocityX": 1.3714964648534713, + "velocityY": -0.8028521645706009, + "timestamp": 1.6554268135315564 }, { - "x": 4.467388514162758, - "y": 6.749519015705101, - "heading": 0.2525546865602907, - "angularVelocity": -2.983377319947091e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.1853199168148336 + "x": 3.3753176768637694, + "y": 2.782172830859228, + "heading": -0.5971442411061407, + "angularVelocity": 0.9517289020234527, + "velocityX": 1.7153925882799501, + "velocityY": -1.0039340698220625, + "timestamp": 1.7199841718524693 }, { - "x": 4.680727782749501, - "y": 6.811234991878438, - "heading": 0.252554668773223, - "angularVelocity": -2.9833773124406734e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.2449404937663002 + "x": 3.5083041287580397, + "y": 2.7043667248342107, + "heading": -0.5274363257086371, + "angularVelocity": 1.0797826492680733, + "velocityX": 2.0599735700645976, + "velocityY": -1.2052244399196936, + "timestamp": 1.7845415301733822 + }, + { + "x": 3.663593064192348, + "y": 2.6135556042232486, + "heading": -0.45293092753819847, + "angularVelocity": 1.1540961419157545, + "velocityX": 2.405441292414274, + "velocityY": -1.4066734292246417, + "timestamp": 1.8490988884942952 }, { - "x": 4.894067051336243, - "y": 6.872950968051774, - "heading": 0.2525546509861554, - "angularVelocity": -2.983377305581398e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.3045610707177668 + "x": 3.8412236037881105, + "y": 2.50976866496977, + "heading": -0.3811103115245424, + "angularVelocity": 1.1125085951726474, + "velocityX": 2.751514997140456, + "velocityY": -1.6076701704173844, + "timestamp": 1.913656246815208 }, { - "x": 5.107406319922986, - "y": 6.9346669442251105, - "heading": 0.2525546331990878, - "angularVelocity": -2.983377313652051e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.3641816476692334 + "x": 4.040074211349249, + "y": 2.394026373110605, + "heading": -0.3455558271366132, + "angularVelocity": 0.5507425537951661, + "velocityX": 3.0802159929261363, + "velocityY": -1.7928597896433947, + "timestamp": 1.978213605136121 }, { - "x": 5.320745588509729, - "y": 6.996382920398447, - "heading": 0.2525546154120202, - "angularVelocity": -2.983377306356561e-7, - "velocityX": 3.5782825241763185, - "velocityY": 1.0351455710261857, - "timestamp": 1.4238022246207 + "x": 4.2474603616320605, + "y": 2.2722898239651945, + "heading": -0.3455557994031658, + "angularVelocity": 4.2959390119270946e-7, + "velocityX": 3.2124324116842065, + "velocityY": -1.8857114403637907, + "timestamp": 2.042770963457034 }, { - "x": 5.534084857096471, - "y": 7.058098896571784, - "heading": 0.2525545976249525, - "angularVelocity": -2.983377306690358e-7, - "velocityX": 3.578282524176319, - "velocityY": 1.0351455710261859, - "timestamp": 1.4834228015721667 + "x": 4.454846495549661, + "y": 2.150553246940426, + "heading": -0.34555577166993406, + "angularVelocity": 4.2959055996150303e-7, + "velocityX": 3.21243215818542, + "velocityY": -1.8857118722178698, + "timestamp": 2.1073283217779473 }, { - "x": 5.747424125671387, - "y": 7.119814872741699, - "heading": 0.2525545797921912, - "angularVelocity": -2.991041384854961e-7, - "velocityX": 3.5782825239779474, - "velocityY": 1.0351455709687998, - "timestamp": 1.5430433785236333 + "x": 4.662232629466809, + "y": 2.0288166699148875, + "heading": -0.34555574393670246, + "angularVelocity": 4.295905587004531e-7, + "velocityX": 3.2124321581784225, + "velocityY": -1.885711872229791, + "timestamp": 2.1718856800988604 }, { - "x": 5.874242280548121, - "y": 7.151641192910771, - "heading": 0.23431006193331627, - "angularVelocity": -0.5017601477559481, - "velocityX": 3.487748847149364, - "velocityY": 0.8752864413346503, - "timestamp": 1.5794044126561186 + "x": 4.86961876338396, + "y": 1.9070800928893532, + "heading": -0.34555571620347075, + "angularVelocity": 4.295905595925436e-7, + "velocityX": 3.2124321581784563, + "velocityY": -1.8857118722297332, + "timestamp": 2.2364430384197735 }, { - "x": 6.000453145534325, - "y": 7.182664404015401, - "heading": 0.21336082591069744, - "angularVelocity": -0.5761452203556151, - "velocityX": 3.4710471799658134, - "velocityY": 0.853199361481138, - "timestamp": 1.615765446788604 + "x": 5.0770048973810855, + "y": 1.7853435160000617, + "heading": -0.3455556884702391, + "angularVelocity": 4.2959055900802054e-7, + "velocityX": 3.2124321594172818, + "velocityY": -1.8857118701193136, + "timestamp": 2.3010003967406867 }, { - "x": 6.126521345623787, - "y": 7.213763203476394, - "heading": 0.192336966602575, - "angularVelocity": -0.5781975075714219, - "velocityX": 3.4671236145297133, - "velocityY": 0.8552781900449158, - "timestamp": 1.6521264809210894 + "x": 5.284393928450512, + "y": 1.6636118745954684, + "heading": -0.3455556607369369, + "angularVelocity": 4.295916530112558e-7, + "velocityX": 3.212477035359784, + "velocityY": -1.885635418962908, + "timestamp": 2.3655577550616 }, { - "x": 6.252403333907122, - "y": 7.24487049169852, - "heading": 0.17101724585582967, - "angularVelocity": -0.5863342794119836, - "velocityX": 3.462002423381879, - "velocityY": 0.8555116476825977, - "timestamp": 1.6884875150535748 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, + "angularVelocity": 4.37946601168607e-7, + "velocityX": 3.386285847111441, + "velocityY": -1.5519979873442957, + "timestamp": 2.430115113382513 }, { - "x": 6.378086189949285, - "y": 7.2759799644241046, - "heading": 0.14935632557503462, - "angularVelocity": -0.5957179381056964, - "velocityX": 3.456525894841813, - "velocityY": 0.8555717258270096, - "timestamp": 1.7248485491860601 + "x": 5.67114193645051, + "y": 1.502092704427615, + "heading": -0.3455556386074096, + "angularVelocity": -1.278584041845331e-7, + "velocityX": 3.49949220171166, + "velocityY": -1.2763944237939575, + "timestamp": 2.478161609702393 }, { - "x": 6.503564352522718, - "y": 7.307098484942146, - "heading": 0.12734794412408884, - "angularVelocity": -0.6052738041155772, - "velocityX": 3.4508964215989995, - "velocityY": 0.8558205579262937, - "timestamp": 1.7612095833185455 + "x": 5.843643430464205, + "y": 1.4543994299893364, + "heading": -0.3455556446255623, + "angularVelocity": -1.252568492070864e-7, + "velocityX": 3.590303294234579, + "velocityY": -0.9926483321645406, + "timestamp": 2.526208106022273 }, { - "x": 6.628828810809778, - "y": 7.338228595030231, - "heading": 0.10496993550474346, - "angularVelocity": -0.6154392787017191, - "velocityX": 3.4450191331370044, - "velocityY": 0.8561392939117064, - "timestamp": 1.7975706174510309 + "x": 6.017301477098486, + "y": 1.4111062813327742, + "heading": -0.3455556506300644, + "angularVelocity": -1.2497273485529655e-7, + "velocityX": 3.6143748230487924, + "velocityY": -0.9010677567065171, + "timestamp": 2.5742546023421533 }, { - "x": 6.753876017529989, - "y": 7.3693809168618785, - "heading": 0.08222565272715081, - "angularVelocity": -0.6255125389096852, - "velocityX": 3.4390442874806206, - "velocityY": 0.8567501605740924, - "timestamp": 1.8339316515835162 + "x": 6.190959602710417, + "y": 1.3678134494736272, + "heading": -0.3455556566345664, + "angularVelocity": -1.249727336927334e-7, + "velocityX": 3.6143764668242215, + "velocityY": -0.9010611631473742, + "timestamp": 2.6223010986620334 }, { - "x": 6.878734995256494, - "y": 7.4006038692779965, - "heading": 0.05925591834951263, - "angularVelocity": -0.6317129016173038, - "velocityX": 3.4338676197042926, - "velocityY": 0.8586926406535731, - "timestamp": 1.8702926857160016 + "x": 6.364617728327444, + "y": 1.32452061763492, + "heading": -0.34555566263906834, + "angularVelocity": -1.2497273274023636e-7, + "velocityX": 3.6143764669302776, + "velocityY": -0.9010611627219542, + "timestamp": 2.6703475949819135 }, { - "x": 7.003212758632201, - "y": 7.431690611037589, - "heading": 0.0352626014950734, - "angularVelocity": -0.6598634342196367, - "velocityX": 3.4233834747977516, - "velocityY": 0.8549465795259067, - "timestamp": 1.906653719848487 + "x": 6.53827585394447, + "y": 1.2812277857962142, + "heading": -0.3455556686435703, + "angularVelocity": -1.2497273262525817e-7, + "velocityX": 3.6143764669302847, + "velocityY": -0.9010611627219266, + "timestamp": 2.7183940913017937 }, { - "x": 7.125290870666504, - "y": 7.458, - "heading": -1.1078640911459773e-20, - "angularVelocity": -0.9697909406699016, - "velocityX": 3.3573883402077644, - "velocityY": 0.7235599753997619, - "timestamp": 1.9430147539809723 + "x": 6.711933979561497, + "y": 1.2379349539575075, + "heading": -0.3455556746480723, + "angularVelocity": -1.249727344246102e-7, + "velocityX": 3.614376466930281, + "velocityY": -0.9010611627219414, + "timestamp": 2.7664405876216738 }, { - "x": 7.338537136585377, - "y": 7.4820248133164355, - "heading": -0.00804052191695304, - "angularVelocity": -0.12790212002878093, - "velocityX": 3.392149139191805, - "velocityY": 0.38216730060631093, - "timestamp": 2.005879403254524 + "x": 6.885592105175778, + "y": 1.1946421221077876, + "heading": -0.3455556806525743, + "angularVelocity": -1.2497273402138388e-7, + "velocityX": 3.6143764668731366, + "velocityY": -0.9010611629511635, + "timestamp": 2.814487083941554 }, { - "x": 7.527981798214128, - "y": 7.500236040572311, - "heading": -0.013862931395131553, - "angularVelocity": -0.09261818121091622, - "velocityX": 3.013532467259838, - "velocityY": 0.2896894751870813, - "timestamp": 2.068744052528076 + "x": 7.059250188235952, + "y": 1.1513491195635046, + "heading": -0.34555568665707626, + "angularVelocity": -1.2497273336829244e-7, + "velocityX": 3.614375581187171, + "velocityY": -0.901064715646495, + "timestamp": 2.862533580261434 }, { - "x": 7.6935968882672885, - "y": 7.512744942179138, - "heading": -0.017658491883084444, - "angularVelocity": -0.06037670665172621, - "velocityX": 2.6344709143686895, - "velocityY": 0.1989814904143519, - "timestamp": 2.1316087018016274 + "x": 7.232252247020901, + "y": 1.105504804591487, + "heading": -0.3455556930363148, + "angularVelocity": -1.3277218955785758e-7, + "velocityX": 3.600721635000194, + "velocityY": -0.954165620460628, + "timestamp": 2.910580076581314 }, { - "x": 7.835373213607451, - "y": 7.519588665029473, - "heading": -0.019490985187148625, - "angularVelocity": -0.029149821485366035, - "velocityX": 2.255263124482431, - "velocityY": 0.1088644083665287, - "timestamp": 2.194473351075179 + "x": 7.3915823056650956, + "y": 1.0554297346464303, + "heading": -0.34668228326838396, + "angularVelocity": -0.02344791646343344, + "velocityX": 3.3161639421825697, + "velocityY": -1.0422210521173303, + "timestamp": 2.9586265729011942 }, { - "x": 7.953306201274802, - "y": 7.52078579458168, - "heading": -0.019392463666041717, - "angularVelocity": 0.001567200680277371, - "velocityX": 1.8759825916497619, - "velocityY": 0.01904296875969824, - "timestamp": 2.2573380003487307 + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, + "angularVelocity": 0.023449177160012234, + "velocityX": 3.0262995808418314, + "velocityY": -0.9742709781335815, + "timestamp": 3.0066730692210744 }, { - "x": 8.047393114183997, - "y": 7.516347487170096, - "heading": -0.01738240796128744, - "angularVelocity": 0.0319743405551765, - "velocityX": 1.4966585194769915, - "velocityY": -0.0706010049029291, - "timestamp": 2.3202026496222823 + "x": 7.706721170015621, + "y": 0.9519776263380658, + "heading": -0.3399354575219549, + "angularVelocity": 0.0873776196257354, + "velocityX": 2.6389050045733367, + "velocityY": -0.8806177429462266, + "timestamp": 3.0709936007986016 }, { - "x": 8.11763212951421, - "y": 7.506281183777245, - "heading": -0.013474109300406987, - "angularVelocity": 0.062170054331707525, - "velocityX": 1.1173054513447094, - "velocityY": -0.1601266134333638, - "timestamp": 2.383067298895834 + "x": 7.851727537144501, + "y": 0.9021680166584882, + "heading": -0.3343317435174853, + "angularVelocity": 0.0871216991998166, + "velocityX": 2.254433593324705, + "velocityY": -0.7743967355049113, + "timestamp": 3.135314132376129 }, { - "x": 8.164021945337474, - "y": 7.490592202393592, - "heading": -0.007677398137362595, - "angularVelocity": 0.09220939319680839, - "velocityX": 0.737931673195224, - "velocityY": -0.24956762767234375, - "timestamp": 2.4459319481693855 + "x": 7.9720996028837305, + "y": 0.8595135021783036, + "heading": -0.3304150780634631, + "angularVelocity": 0.06089292731204182, + "velocityX": 1.8714407792812162, + "velocityY": -0.6631555031346718, + "timestamp": 3.199634663953656 }, { - "x": 8.186561584472656, - "y": 7.469284534454346, - "heading": 1.0473347586059488e-21, - "angularVelocity": 0.12212584061281963, - "velocityX": 0.358542350838588, - "velocityY": -0.33894514938796605, - "timestamp": 2.508796597442937 + "x": 8.067893377913451, + "y": 0.8241876261341342, + "heading": -0.3290917427033041, + "angularVelocity": 0.020574073747570358, + "velocityX": 1.4893187708540474, + "velocityY": -0.5492161706031644, + "timestamp": 3.2639551955311834 }, { - "x": 8.169433800706123, - "y": 7.42685355131065, - "heading": 0.014977206112370758, - "angularVelocity": 0.16513162181596408, - "velocityX": -0.1888428783219197, - "velocityY": -0.46782403935650313, - "timestamp": 2.5994951925858016 + "x": 8.139145592381213, + "y": 0.7962986910106811, + "heading": -0.33093102223192583, + "angularVelocity": -0.028595527485726017, + "velocityX": 1.107767810996388, + "velocityY": -0.4335930447004768, + "timestamp": 3.3282757271087107 }, { - "x": 8.10265502996488, - "y": 7.372742454671214, - "heading": 0.033468558101012916, - "angularVelocity": 0.2038769394334648, - "velocityX": -0.7362712800132867, - "velocityY": -0.5966034705852111, - "timestamp": 2.690193787728666 + "x": 8.185882143295247, + "y": 0.7759207035915239, + "heading": -0.33632389468863655, + "angularVelocity": -0.08384371715290594, + "velocityX": 0.726619475426766, + "velocityY": -0.3168193253284159, + "timestamp": 3.392596258686238 }, { - "x": 7.986220080079777, - "y": 7.306963959658495, - "heading": 0.05493247364660356, - "angularVelocity": 0.23665102542968372, - "velocityX": -1.2837569281166943, - "velocityY": -0.7252427108612493, - "timestamp": 2.7808923828715306 + "x": 8.208122253417969, + "y": 0.763107419013977, + "heading": -0.34555563246426124, + "angularVelocity": -0.14352707524653271, + "velocityX": 0.34576999874318054, + "velocityY": -0.19920986756931033, + "timestamp": 3.456916790263765 }, { - "x": 7.820121799349139, - "y": 7.229537347680479, - "heading": 0.07855456380147917, - "angularVelocity": 0.26044604238540936, - "velocityX": -1.8313214275150225, - "velocityY": -0.8536693634124876, - "timestamp": 2.871590978014395 + "x": 8.204294589663224, + "y": 0.758059174501978, + "heading": -0.3598356424553756, + "angularVelocity": -0.21015280737857786, + "velocityX": -0.056330092504229386, + "velocityY": -0.07429285814157252, + "timestamp": 3.5248673932999157 }, { - "x": 7.6043499812219455, - "y": 7.1404953741126675, - "heading": 0.10296904259791395, - "angularVelocity": 0.2691825464107588, - "velocityX": -2.37899845953864, - "velocityY": -0.9817348706179747, - "timestamp": 2.9622895731572596 + "x": 8.173143956099947, + "y": 0.7614990375664682, + "heading": -0.3786432717750789, + "angularVelocity": -0.2767838470792884, + "velocityX": -0.4584305682571252, + "velocityY": 0.05062299539358275, + "timestamp": 3.5928179963360662 }, { - "x": 7.338890842614761, - "y": 7.0399060968505065, - "heading": 0.12540191269228773, - "angularVelocity": 0.24733426200304945, - "velocityX": -2.926827457349739, - "velocityY": -1.1090500035167739, - "timestamp": 3.052988168300124 + "x": 8.114670323431806, + "y": 0.7734269104252286, + "heading": -0.4019794984677993, + "angularVelocity": -0.34342928024207936, + "velocityX": -0.8605314751516402, + "velocityY": 0.17553740990958688, + "timestamp": 3.6607685993722168 }, { - "x": 7.023762260989218, - "y": 6.927996342713902, - "heading": 0.13709315762540872, - "angularVelocity": 0.12890216121545683, - "velocityX": -3.4744593466873996, - "velocityY": -1.233864250712263, - "timestamp": 3.1436867634429886 + "x": 8.028873660690506, + "y": 0.7938426763627194, + "heading": -0.42984618674257646, + "angularVelocity": -0.41010214817301655, + "velocityX": -1.2626328377933065, + "velocityY": 0.3004501067728478, + "timestamp": 3.7287192024083673 }, { - "x": 6.6949878661960245, - "y": 6.850204202472232, - "heading": 0.1370931594052802, - "angularVelocity": 1.9624024712667312e-8, - "velocityX": -3.6249116568489486, - "velocityY": -0.857699505919964, - "timestamp": 3.234385358585853 + "x": 7.915753937180706, + "y": 0.8227462003684911, + "heading": -0.46224640751329055, + "angularVelocity": -0.47682020943179554, + "velocityX": -1.6647346521651787, + "velocityY": 0.4253608167449919, + "timestamp": 3.796669805444518 }, { - "x": 6.366213403460125, - "y": 6.772412349378657, - "heading": 0.13709316118514803, - "angularVelocity": 1.9623984758130592e-8, - "velocityX": -3.624912405953231, - "velocityY": -0.8576963399602832, - "timestamp": 3.3250839537287176 + "x": 7.7753111250087, + "y": 0.8601373303576515, + "heading": -0.4991848061620223, + "angularVelocity": -0.5436066347943985, + "velocityX": -2.0668368770368284, + "velocityY": 0.5502692885487409, + "timestamp": 3.8646204084806683 }, { - "x": 6.037438940720318, - "y": 6.694620496301594, - "heading": 0.13709316296501595, - "angularVelocity": 1.9623985202632452e-8, - "velocityX": -3.624912405996305, - "velocityY": -0.8576963397782383, - "timestamp": 3.415782548871582 + "x": 7.607545202545143, + "y": 0.9060158995198092, + "heading": -0.5406680081845634, + "angularVelocity": -0.6104905647485055, + "velocityX": -2.468939420218282, + "velocityY": 0.6751753054752103, + "timestamp": 3.932571011516819 }, { - "x": 5.708664477980543, - "y": 6.6168286432244035, - "heading": 0.13709316474488378, - "angularVelocity": 1.9623984644394548e-8, - "velocityX": -3.624912405995973, - "velocityY": -0.8576963397796433, - "timestamp": 3.5064811440144465 + "x": 7.412456160128696, + "y": 0.9603817316039739, + "heading": -0.5867050376005867, + "angularVelocity": -0.6775072973455604, + "velocityX": -2.8710421055815774, + "velocityY": 0.800078728591143, + "timestamp": 4.000521614552969 + }, + { + "x": 7.190044017003328, + "y": 1.023234660815804, + "heading": -0.6373076187752672, + "angularVelocity": -0.7446965724168686, + "velocityX": -3.273144507739573, + "velocityY": 0.9249797117825663, + "timestamp": 4.06847221758912 }, { - "x": 5.379890015210838, - "y": 6.539036790273742, - "heading": 0.13709316652476114, - "angularVelocity": 1.9624089556639278e-8, - "velocityX": -3.6249124063259477, - "velocityY": -0.8576963383845883, - "timestamp": 3.597179739157311 + "x": 6.9483240964505395, + "y": 1.0983293400926897, + "heading": -0.6373076247970214, + "angularVelocity": -8.861958477756183e-8, + "velocityX": -3.5572888208834765, + "velocityY": 1.1051363184655567, + "timestamp": 4.1364228206252704 }, { - "x": 5.082161809078407, - "y": 6.468591173031877, - "heading": 0.1733415977378681, - "angularVelocity": 0.39965813313877674, - "velocityX": -3.282611000351902, - "velocityY": -0.7767002028080265, - "timestamp": 3.6878783343001755 + "x": 6.70660438555466, + "y": 1.1734246942257325, + "heading": -0.6373076308180073, + "angularVelocity": -8.86082771208124e-8, + "velocityX": -3.5572857354522953, + "velocityY": 1.1051462500353575, + "timestamp": 4.204373423661421 }, { - "x": 4.834054918534678, - "y": 6.409886673220785, - "heading": 0.20354854900447322, - "angularVelocity": 0.3330476201866674, - "velocityX": -2.7355097413903846, - "velocityY": -0.6472481709184398, - "timestamp": 3.77857692944304 + "x": 6.464884674663689, + "y": 1.2485200483745762, + "heading": -0.6373076368389932, + "angularVelocity": -8.860827701986505e-8, + "velocityX": -3.557285735380054, + "velocityY": 1.105146250267891, + "timestamp": 4.2723240266975715 }, { - "x": 4.6355693784003975, - "y": 6.36292318296314, - "heading": 0.22771447843277892, - "angularVelocity": 0.26644215811987526, - "velocityX": -2.1884080985117165, - "velocityY": -0.5177973284334796, - "timestamp": 3.8692755245859045 + "x": 6.223164963772718, + "y": 1.3236154025234206, + "heading": -0.637307642859979, + "angularVelocity": -8.860827626589147e-8, + "velocityX": -3.5572857353800513, + "velocityY": 1.1051462502679001, + "timestamp": 4.340274629733722 }, { - "x": 4.486705209976563, - "y": 6.327700620163418, - "heading": 0.24583924264804105, - "angularVelocity": 0.1998351152706694, - "velocityX": -1.6413062207782854, - "velocityY": -0.3883473910951022, - "timestamp": 3.959974119728769 + "x": 5.98144525288525, + "y": 1.39871075668354, + "heading": -0.6373076488809649, + "angularVelocity": -8.860827719404325e-8, + "velocityX": -3.5572857353285006, + "velocityY": 1.1051462504338314, + "timestamp": 4.4082252327698725 }, { - "x": 4.3874624262872, - "y": 6.30421892984517, - "heading": 0.25792252534877924, - "angularVelocity": 0.13322458503028708, - "velocityX": -1.0942041994480602, - "velocityY": -0.25889805990114845, - "timestamp": 4.0506727148716335 + "x": 5.739725691606158, + "y": 1.473806592406946, + "heading": -0.6373076549019507, + "angularVelocity": -8.860827643379068e-8, + "velocityX": -3.557283533605943, + "velocityY": 1.1051533373950213, + "timestamp": 4.476175835806023 }, { - "x": 4.337841033935547, - "y": 6.292478084564209, - "heading": 0.2639641302660189, - "angularVelocity": 0.06661189082061515, - "velocityX": -0.5471021053136723, - "velocityY": -0.1294490312938923, - "timestamp": 4.141371310014498 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.6373076609624853, + "angularVelocity": -8.919029793694062e-8, + "velocityX": -3.483737947413037, + "velocityY": 1.3187872955082738, + "timestamp": 4.544126438842174 }, { - "x": 4.337841033935547, - "y": 6.292478084564209, - "heading": 0.2639641302660189, - "angularVelocity": -4.3850016440748696e-23, - "velocityX": 2.0622476782033507e-21, - "velocityY": 1.8080615452809687e-23, - "timestamp": 4.2320699051573625 + "x": 5.300661081332848, + "y": 1.6644259130191759, + "heading": -0.6373076662501911, + "angularVelocity": -8.709485563097327e-8, + "velocityX": -3.332823841916901, + "velocityY": 1.6637052194159954, + "timestamp": 4.6048384732983045 }, { - "x": 4.357336302646973, - "y": 6.297583243093535, - "heading": 0.2475212314024478, - "angularVelocity": -0.28561956492945584, - "velocityX": 0.3386404194139296, - "velocityY": 0.08867859433669868, - "timestamp": 4.289639136689636 + "x": 5.105140298152244, + "y": 1.7780767678650242, + "heading": -0.6373076714738495, + "angularVelocity": -8.603991577037157e-8, + "velocityX": -3.2204617244688527, + "velocityY": 1.8719658443989302, + "timestamp": 4.6655505077544355 }, { - "x": 4.39639255106342, - "y": 6.307690301692112, - "heading": 0.21545182729872558, - "angularVelocity": -0.5570580542792894, - "velocityX": 0.6784222644096215, - "velocityY": 0.17556354895775533, - "timestamp": 4.347208368221909 + "x": 4.909619881391782, + "y": 1.8917282530843815, + "heading": -0.6373076766975064, + "angularVelocity": -8.60398930609077e-8, + "velocityX": -3.220455689089781, + "velocityY": 1.8719762274064293, + "timestamp": 4.726262542210566 }, { - "x": 4.455088006420721, - "y": 6.322669942772804, - "heading": 0.16876628337274657, - "angularVelocity": -0.8109461023430008, - "velocityX": 1.0195629469953096, - "velocityY": 0.2602022066647703, - "timestamp": 4.4047775997541825 + "x": 4.714099464642575, + "y": 2.0053797383231027, + "heading": -0.6373076819211634, + "angularVelocity": -8.603989205375444e-8, + "velocityX": -3.2204556889043876, + "velocityY": 1.8719762277253713, + "timestamp": 4.786974576666697 }, { - "x": 4.53351670800356, - "y": 6.342355632767599, - "heading": 0.10873910043109954, - "angularVelocity": -1.0426955744232154, - "velocityX": 1.362337128625251, - "velocityY": 0.3419481113580211, - "timestamp": 4.462346831286456 + "x": 4.518579047893369, + "y": 2.1190312235618243, + "heading": -0.6373076871448202, + "angularVelocity": -8.60398922147823e-8, + "velocityX": -3.220455688904382, + "velocityY": 1.8719762277253813, + "timestamp": 4.847686611122828 }, { - "x": 4.631792821732683, - "y": 6.3665238013399605, - "heading": 0.03701268299307073, - "angularVelocity": -1.245915839571676, - "velocityX": 1.7070944168158462, - "velocityY": 0.4198105121277204, - "timestamp": 4.519916062818729 + "x": 4.323058631144163, + "y": 2.232682708800546, + "heading": -0.6373076923684772, + "angularVelocity": -8.603989163870152e-8, + "velocityX": -3.2204556889043805, + "velocityY": 1.8719762277253815, + "timestamp": 4.908398645578959 }, { - "x": 4.7500563078189675, - "y": 6.394856912095635, - "heading": -0.0442182245974014, - "angularVelocity": -1.4110125396572948, - "velocityX": 2.0542828684448438, - "velocityY": 0.4921571819104624, - "timestamp": 4.5774852943510025 + "x": 4.12753821439536, + "y": 2.3463341940399602, + "heading": -0.637307697592134, + "angularVelocity": -8.60398917504913e-8, + "velocityX": -3.2204556888977502, + "velocityY": 1.8719762277367886, + "timestamp": 4.96911068003509 }, { - "x": 4.888478268982772, - "y": 6.426870152464857, - "heading": -0.13181305030525106, - "angularVelocity": -1.52155627887344, - "velocityX": 2.4044434410455024, - "velocityY": 0.5560824683107718, - "timestamp": 4.635054525883276 + "x": 3.9320178107531194, + "y": 2.4599857018249596, + "heading": -0.637307702817472, + "angularVelocity": -8.606758041499758e-8, + "velocityX": -3.2204554730169304, + "velocityY": 1.8719765990896107, + "timestamp": 5.029822714491221 }, { - "x": 5.047249482849422, - "y": 6.461748968300146, - "heading": -0.22073086511138704, - "angularVelocity": -1.5445371153910277, - "velocityX": 2.757917895388995, - "velocityY": 0.6058586315458515, - "timestamp": 4.692623757415549 + "x": 3.754279851907353, + "y": 2.5632932295685515, + "heading": -0.6678287625734391, + "angularVelocity": -0.5027184483172089, + "velocityX": -2.9275572864255675, + "velocityY": 1.7015988455837243, + "timestamp": 5.090534748947352 }, { - "x": 5.226429049191539, - "y": 6.497909114202001, - "heading": -0.3013203844421244, - "angularVelocity": -1.39987137548568, - "velocityX": 3.1124189358280567, - "velocityY": 0.6281158344381212, - "timestamp": 4.7501929889478225 + "x": 3.5962904713608705, + "y": 2.6551219777359605, + "heading": -0.6949668036285394, + "angularVelocity": -0.4469960741425893, + "velocityX": -2.6022745236884077, + "velocityY": 1.5125295831382852, + "timestamp": 5.151246783403483 }, { - "x": 5.423903550460318, - "y": 6.531166211616276, - "heading": -0.3435648346153767, - "angularVelocity": -0.7338025721182303, - "velocityX": 3.430209089347909, - "velocityY": 0.5776887502073221, - "timestamp": 4.807762220480096 + "x": 3.4580497056890147, + "y": 2.735472006369499, + "heading": -0.7187186173095553, + "angularVelocity": -0.3912208492729469, + "velocityX": -2.276991158511501, + "velocityY": 1.3234613096617127, + "timestamp": 5.211958817859614 }, { - "x": 5.636616262089249, - "y": 6.55438565863319, - "heading": -0.34498723844741225, - "angularVelocity": -0.024707709208140263, - "velocityX": 3.69490274522221, - "velocityY": 0.4033308487694046, - "timestamp": 4.865331452012369 + "x": 3.3395575797280896, + "y": 2.804343370444312, + "heading": -0.7390815132457978, + "angularVelocity": -0.33540131077235125, + "velocityX": -1.9517073842508859, + "velocityY": 1.1343939416917022, + "timestamp": 5.272670852315745 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": -0.34498726323171464, - "angularVelocity": -4.305129971148693e-7, - "velocityX": 3.7246965125733036, - "velocityY": 0.047618149403998285, - "timestamp": 4.9229006835446425 + "x": 3.2408141126366585, + "y": 2.861736116018487, + "heading": -0.7560532042537318, + "angularVelocity": -0.2795440996166463, + "velocityX": -1.6264232944257633, + "velocityY": 0.9453273323536148, + "timestamp": 5.333382886771876 }, { - "x": 5.970747823515829, - "y": 6.552244502208151, - "heading": -0.34498728566710496, - "angularVelocity": -6.97576178834182e-7, - "velocityX": 3.7219058963692446, - "velocityY": -0.15180985637112251, - "timestamp": 4.955062605108422 + "x": 3.16181931959339, + "y": 2.907650279470937, + "heading": -0.7696318707216167, + "angularVelocity": -0.22365691727389694, + "velocityX": -1.3011389545897718, + "velocityY": 0.756261322219841, + "timestamp": 5.394094921228007 }, { - "x": 6.090018554613485, - "y": 6.540962017726213, - "heading": -0.3449873062008861, - "angularVelocity": -6.384500714125073e-7, - "velocityX": 3.7084454316926947, - "velocityY": -0.35080256195400317, - "timestamp": 4.987224526672201 + "x": 3.1025732126750043, + "y": 2.942085887277494, + "heading": -0.7798162350397829, + "angularVelocity": -0.16774869116806188, + "velocityX": -0.9758544158357173, + "velocityY": 0.5671957481747656, + "timestamp": 5.454806955684138 }, { - "x": 6.208514441050117, - "y": 6.52331189505608, - "heading": -0.34498732528503373, - "angularVelocity": -5.933770972523468e-7, - "velocityX": 3.68435344267748, - "velocityY": -0.5487894320969752, - "timestamp": 5.019386448235981 + "x": 3.0630758014096395, + "y": 2.9650429559458633, + "heading": -0.7866056220675054, + "angularVelocity": -0.11182934468500333, + "velocityX": -0.6505697201418033, + "velocityY": 0.37813044603137397, + "timestamp": 5.515518990140269 }, { - "x": 6.32589577532509, - "y": 6.499344741803893, - "heading": -0.34498734327209396, - "angularVelocity": -5.592657200372931e-7, - "velocityX": 3.6496990405936383, - "velocityY": -0.7452027766642643, - "timestamp": 5.05154836979976 + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -0.05590947433901855, + "velocityX": -0.3252849037618716, + "velocityY": 0.18906525141774544, + "timestamp": 5.5762310245963995 }, { - "x": 6.441826048149789, - "y": 6.4691292818938315, - "heading": -0.3449873604472264, - "angularVelocity": -5.340207168118268e-7, - "velocityX": 3.6045816663908092, - "velocityY": -0.9394793109653794, - "timestamp": 5.0837102913635395 + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 6.3035161532873835e-28, + "velocityX": -3.172311091955017e-27, + "velocityY": -4.980348575504513e-27, + "timestamp": 5.6369430590525305 }, { - "x": 6.555972918049828, - "y": 6.432752169034089, - "heading": -0.344987377050145, - "angularVelocity": -5.162290595234125e-7, - "velocityX": 3.549130908539721, - "velocityY": -1.1310615501503813, - "timestamp": 5.115872212927319 + "x": 3.0599792847904204, + "y": 2.966059333110192, + "heading": -0.7574164237892916, + "angularVelocity": 0.5582463314772692, + "velocityX": 0.2852978705745262, + "velocityY": -0.17924557403559396, + "timestamp": 5.695310798119411 }, { - "x": 6.668009186278295, - "y": 6.390317785626105, - "heading": -0.34498739329087524, - "angularVelocity": -5.049676592749214e-7, - "velocityX": 3.483506668166316, - "velocityY": -1.319398261818203, - "timestamp": 5.148034134491098 + "x": 3.0934694098196505, + "y": 2.9451270864719556, + "heading": -0.6941025427131003, + "angularVelocity": 1.0847410245519962, + "velocityX": 0.5737780075883294, + "velocityY": -0.35862699108922125, + "timestamp": 5.753678537186291 }, { - "x": 6.777613987715612, - "y": 6.341948479079241, - "heading": -0.3449874093618285, - "angularVelocity": -4.996888394768407e-7, - "velocityX": 3.4079058746525486, - "velocityY": -1.5039308659137305, - "timestamp": 5.180196056054878 + "x": 3.1440142929618213, + "y": 2.9136974289532884, + "heading": -0.6025701359612278, + "angularVelocity": 1.5682020276130737, + "velocityX": 0.8659729492734756, + "velocityY": -0.5384765286634409, + "timestamp": 5.812046276253171 }, { - "x": 6.886436695218618, - "y": 6.291844380961981, - "heading": -0.3449874254078226, - "angularVelocity": -4.989127946142238e-7, - "velocityX": 3.3835884863782986, - "velocityY": -1.55787016698924, - "timestamp": 5.212357977618657 + "x": 3.2118603630472147, + "y": 2.8716921302721974, + "heading": -0.4864204343113424, + "angularVelocity": 1.9899640367566072, + "velocityX": 1.162389895000939, + "velocityY": -0.7196663662603013, + "timestamp": 5.870414015320051 }, { - "x": 6.995259275146381, - "y": 6.241740005760864, - "heading": -0.3449874414538154, - "angularVelocity": -4.98912750035862e-7, - "velocityX": 3.3835845197234478, - "velocityY": -1.5578787822660598, - "timestamp": 5.244519899182436 + "x": 3.297261323571818, + "y": 2.8189697938755005, + "heading": -0.35051552340624004, + "angularVelocity": 2.3284251382322116, + "velocityX": 1.463153479814372, + "velocityY": -0.9032787159407621, + "timestamp": 5.9287817543869314 }, { - "x": 7.1040824430518015, - "y": 6.191636907618568, - "heading": -0.344987457499818, - "angularVelocity": -4.989130590435776e-7, - "velocityX": 3.3836028015184674, - "velocityY": -1.5578390750981381, - "timestamp": 5.276681820746216 + "x": 3.400457830317495, + "y": 2.755367474678837, + "heading": -0.2010210241745889, + "angularVelocity": 2.561252185224339, + "velocityX": 1.768040160463131, + "velocityY": -1.0896827633461208, + "timestamp": 5.987149493453812 }, { - "x": 7.214789390563965, - "y": 6.145846366882324, - "heading": -0.344987478573796, - "angularVelocity": -6.552462348846214e-7, - "velocityX": 3.4421745383783664, - "velocityY": -1.4237501526591758, - "timestamp": 5.308843742309995 + "x": 3.521660389826238, + "y": 2.6807799872839175, + "heading": -0.04666004939322163, + "angularVelocity": 2.644628304078967, + "velocityX": 2.076533397496587, + "velocityY": -1.277888926097578, + "timestamp": 6.045517232520692 }, { - "x": 7.410548108371569, - "y": 6.0752735070916195, - "heading": -0.3626370424214939, - "angularVelocity": -0.2874228799686078, - "velocityX": 3.1879277548585048, - "velocityY": -1.1492779529116102, - "timestamp": 5.370250001441577 + "x": 3.6607588635338297, + "y": 2.5953087461465243, + "heading": 0.09647781625085058, + "angularVelocity": 2.4523455582211025, + "velocityX": 2.383139657820331, + "velocityY": -1.4643575801258442, + "timestamp": 6.103884971587572 }, { - "x": 7.584442413587634, - "y": 6.012933959955544, - "heading": -0.37637990938116833, - "angularVelocity": -0.22380238031152527, - "velocityX": 2.831866126927586, - "velocityY": -1.0151985810191508, - "timestamp": 5.431656260573159 + "x": 3.8159145666935697, + "y": 2.50056098976987, + "heading": 0.19948059626334666, + "angularVelocity": 1.7647210883819062, + "velocityX": 2.6582441883170422, + "velocityY": -1.6232898154250606, + "timestamp": 6.162252710654452 }, { - "x": 7.736397172033548, - "y": 5.958610527915441, - "heading": -0.3857543231283157, - "angularVelocity": -0.15266218590290268, - "velocityX": 2.47458094003583, - "velocityY": -0.8846562680800549, - "timestamp": 5.493062519704741 + "x": 3.984110357979779, + "y": 2.3943443694149815, + "heading": 0.2530307730268392, + "angularVelocity": 0.9174618996657108, + "velocityX": 2.8816567846406262, + "velocityY": -1.8197830180329746, + "timestamp": 6.220620449721332 }, { - "x": 7.86638729375416, - "y": 5.912228690109332, - "heading": -0.3906020583128712, - "angularVelocity": -0.07894529406469342, - "velocityX": 2.1168871636044173, - "velocityY": -0.7553275262497595, - "timestamp": 5.5544687788363225 + "x": 4.165386797993929, + "y": 2.276764205392624, + "heading": 0.25694414020151846, + "angularVelocity": 0.06704674940715359, + "velocityX": 3.105764295691424, + "velocityY": -2.0144717938728, + "timestamp": 6.278988188788213 + }, + { + "x": 4.35049005488602, + "y": 2.162710813518879, + "heading": 0.2569442210228932, + "angularVelocity": 0.0000013846925722227273, + "velocityX": 3.171328200326413, + "velocityY": -1.9540484811833736, + "timestamp": 6.337355927855093 }, { - "x": 7.974400232833448, - "y": 5.873750789689322, - "heading": -0.3908432822952403, - "angularVelocity": -0.003928328899700405, - "velocityX": 1.7589890771205783, - "velocityY": -0.6266120256171138, - "timestamp": 5.615875037967904 + "x": 4.5355934176880135, + "y": 2.0486575935320284, + "heading": 0.2569443018441972, + "angularVelocity": 0.0000013846913602153038, + "velocityX": 3.171330014854514, + "velocityY": -1.9540455362878368, + "timestamp": 6.395723666921973 }, { - "x": 8.060428468071539, - "y": 5.84315412501972, - "heading": -0.3864300383007793, - "angularVelocity": 0.07186961161409067, - "velocityX": 1.4009685080107037, - "velocityY": -0.498266220777903, - "timestamp": 5.677281297099486 + "x": 4.72069678050449, + "y": 1.9346043735686815, + "heading": 0.25694438266550157, + "angularVelocity": 0.0000013846913664886619, + "velocityX": 3.171330015102631, + "velocityY": -1.9540455358851527, + "timestamp": 6.454091405988853 }, { - "x": 8.124466995265989, - "y": 5.820423539830739, - "heading": -0.377330476340927, - "angularVelocity": 0.14818622870925488, - "velocityX": 1.042866445539805, - "velocityY": -0.37016723556265896, - "timestamp": 5.738687556231068 + "x": 4.905800420839664, + "y": 1.8205516040072125, + "heading": 0.2569444634867951, + "angularVelocity": 0.0000013846911811343512, + "velocityX": 3.171334769761663, + "velocityY": -1.9540378192614634, + "timestamp": 6.512459145055733 }, { - "x": 8.166512252703873, - "y": 5.805548220947723, - "heading": -0.36352195759750816, - "angularVelocity": 0.2248715186155497, - "velocityX": 0.6847063806278847, - "velocityY": -0.242244342732895, - "timestamp": 5.80009381536265 + "x": 5.096209165426906, + "y": 1.7155949376896535, + "heading": 0.2569445446080598, + "angularVelocity": 0.0000013898305123819847, + "velocityX": 3.262225805407059, + "velocityY": -1.7981965379418772, + "timestamp": 6.570826884122614 }, { - "x": 8.186561584472656, - "y": 5.798520088195801, - "heading": -0.344987478573796, - "angularVelocity": 0.30183371020853555, - "velocityX": 0.3265030642205698, - "velocityY": -0.11445303542856088, - "timestamp": 5.861500074494232 + "x": 5.295905298565615, + "y": 1.6296132789604225, + "heading": 0.2569446291955456, + "angularVelocity": 0.000001449216418667715, + "velocityX": 3.4213443304680435, + "velocityY": -1.473102438158684, + "timestamp": 6.629194623189494 }, { - "x": 8.182873429152231, - "y": 5.799961021839805, - "heading": -0.3197766551885773, - "angularVelocity": 0.3842915682549792, - "velocityX": -0.056218988582698255, - "velocityY": 0.021964322281146666, - "timestamp": 5.927103448254676 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.256944722079518, + "angularVelocity": 0.0000015913580667498199, + "velocityX": 3.548163797412206, + "velocityY": -1.1340904342967109, + "timestamp": 6.687562362256374 }, { - "x": 8.154073481017848, - "y": 5.810352783086787, - "heading": -0.28949615112813754, - "angularVelocity": 0.46156931152673975, - "velocityX": -0.43900102210518965, - "velocityY": 0.15840284807498262, - "timestamp": 5.99270682201512 + "x": 5.694707875531734, + "y": 1.520245371460122, + "heading": 0.25694480525285635, + "angularVelocity": 0.0000015766499744480022, + "velocityX": 3.6339835774201763, + "velocityY": -0.8184074026058323, + "timestamp": 6.740315566608581 }, { - "x": 8.100157139869356, - "y": 5.829696916957608, - "heading": -0.2545716989464078, - "angularVelocity": 0.5323575630311173, - "velocityX": -0.8218531770236625, - "velocityY": 0.29486492480488535, - "timestamp": 6.058310195775564 + "x": 5.889460824583175, + "y": 1.4940579532692961, + "heading": 0.2569448839753191, + "angularVelocity": 0.0000014922783134293427, + "velocityX": 3.691774773550607, + "velocityY": -0.49641379158667054, + "timestamp": 6.793068770960788 }, { - "x": 8.021119019409117, - "y": 5.8579951101346595, - "heading": -0.2155530876775793, - "angularVelocity": 0.594765315139245, - "velocityX": -1.204787435915305, - "velocityY": 0.43135271183437995, - "timestamp": 6.1239135695360085 + "x": 6.085760337593024, + "y": 1.4850583071038423, + "heading": 0.25694496145000295, + "angularVelocity": 0.0000014686251728049365, + "velocityX": 3.721091740688451, + "velocityY": -0.17059904276842675, + "timestamp": 6.845821975312996 }, { - "x": 7.9169528918289975, - "y": 5.89524913213786, - "heading": -0.17317932530550872, - "angularVelocity": 0.6459082809795952, - "velocityX": -1.5878166260242859, - "velocityY": 0.567867471865644, - "timestamp": 6.189516943296453 + "x": 6.282233415675307, + "y": 1.4886391007137698, + "heading": 0.2569450396472057, + "angularVelocity": 0.0000014823213810882118, + "velocityX": 3.7243818739526797, + "velocityY": 0.06787821998490172, + "timestamp": 6.898575179665203 }, { - "x": 7.787651961744212, - "y": 5.941460642017215, - "heading": -0.12849929599301224, - "angularVelocity": 0.6810629812370456, - "velocityX": -1.9709493989888005, - "velocityY": 0.7044075209927304, - "timestamp": 6.255120317056897 + "x": 6.478706465105997, + "y": 1.4922214660505555, + "heading": 0.2569451178445693, + "angularVelocity": 0.000001482324430715833, + "velocityX": 3.7243813308274873, + "velocityY": 0.06790801394485714, + "timestamp": 6.95132838401741 }, { - "x": 7.633210550125228, - "y": 5.99663052819492, - "heading": -0.08312171419135361, - "angularVelocity": 0.6916958564868655, - "velocityX": -2.3541687380734118, - "velocityY": 0.8409611124446534, - "timestamp": 6.320723690817341 + "x": 6.6750148942106335, + "y": 1.5010244818843792, + "heading": 0.2569451969790633, + "angularVelocity": 0.0000015000888565600604, + "velocityX": 3.7212607559150324, + "velocityY": 0.16687167996564178, + "timestamp": 7.004081588369617 }, { - "x": 7.453632499858265, - "y": 6.060756206322318, - "heading": -0.039828348341138756, - "angularVelocity": 0.659925905766733, - "velocityX": -2.7373294995880113, - "velocityY": 0.9774753103637095, - "timestamp": 6.386327064577785 + "x": 6.869794082781354, + "y": 1.5270160132207926, + "heading": 0.25694528055290106, + "angularVelocity": 0.000001584241920357872, + "velocityX": 3.692272174980594, + "velocityY": 0.4927005222826037, + "timestamp": 7.0568347927218245 }, { - "x": 7.248984054172477, - "y": 6.133814732067644, - "heading": -0.004587896529415136, - "angularVelocity": 0.5371743828359606, - "velocityX": -3.119480507711035, - "velocityY": 1.1136397651149037, - "timestamp": 6.45193043833823 + "x": 7.061541469504156, + "y": 1.5699966670376497, + "heading": 0.25694631305508775, + "angularVelocity": 0.000019572312228907657, + "velocityX": 3.6348007495923222, + "velocityY": 0.814749631698122, + "timestamp": 7.109587997074032 }, { - "x": 7.020152924958556, - "y": 6.215343825965371, - "heading": -7.99601094738583e-8, - "angularVelocity": 0.06993263160608797, - "velocityX": -3.488100018293493, - "velocityY": 1.2427576391945405, - "timestamp": 6.517533812098674 + "x": 7.242025004547745, + "y": 1.623544327324134, + "heading": 0.2892009263238574, + "angularVelocity": 0.611424721300743, + "velocityX": 3.421280986811508, + "velocityY": 1.0150598611787165, + "timestamp": 7.162341201426239 }, { - "x": 6.789969979123711, - "y": 6.297403529442868, - "heading": -7.492709232609331e-8, - "angularVelocity": 7.671887677824965e-8, - "velocityX": -3.508705919231764, - "velocityY": 1.2508457838943434, - "timestamp": 6.583137185859118 + "x": 7.4101475255884415, + "y": 1.6859098332100053, + "heading": 0.3552181316610267, + "angularVelocity": 1.2514349819662982, + "velocityX": 3.186963201670605, + "velocityY": 1.1822126570641514, + "timestamp": 7.215094405778446 }, { - "x": 6.559787033588477, - "y": 6.379463233760796, - "heading": -6.989407741573959e-8, - "angularVelocity": 7.671884267312441e-8, - "velocityX": -3.5087059146647626, - "velocityY": 1.2508457967051358, - "timestamp": 6.648740559619562 + "x": 7.5650279020019084, + "y": 1.7562974315324296, + "heading": 0.45280543116466554, + "angularVelocity": 1.8498838260534038, + "velocityX": 2.935942533071662, + "velocityY": 1.3342810012540722, + "timestamp": 7.267847610130653 }, { - "x": 6.3296038306461995, - "y": 6.461522216031207, - "heading": -6.486106191272836e-8, - "angularVelocity": 7.671885170707308e-8, - "velocityX": -3.508709838350823, - "velocityY": 1.250834790449275, - "timestamp": 6.714343933380007 + "x": 7.70378035758304, + "y": 1.8320280138991303, + "heading": 0.5628377984659659, + "angularVelocity": 2.0857949512728657, + "velocityX": 2.6302185295655014, + "velocityY": 1.4355636457850884, + "timestamp": 7.3206008144828605 }, { - "x": 6.0929382858404795, - "y": 6.522410665825721, - "heading": -5.975605523435043e-8, - "angularVelocity": 7.78162217238893e-8, - "velocityX": -3.607520943510018, - "velocityY": 0.9281298552853697, - "timestamp": 6.779947307140451 + "x": 7.825662907370156, + "y": 1.911322826381737, + "heading": 0.6698617562914626, + "angularVelocity": 2.0287669562392776, + "velocityX": 2.3104293148405795, + "velocityY": 1.5031278849564027, + "timestamp": 7.373354018835068 }, { - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": -5.43646913463662e-8, - "angularVelocity": 8.218119860224255e-8, - "velocityX": -3.6872205492775545, - "velocityY": 0.5291851788357008, - "timestamp": 6.845550680900895 + "x": 7.930693514616294, + "y": 1.9932244953323592, + "heading": 0.7659443680912281, + "angularVelocity": 1.8213606733397365, + "velocityX": 1.9909806150333516, + "velocityY": 1.5525439630890379, + "timestamp": 7.426107223187275 }, { - "x": 5.661580826065391, - "y": 6.5680717657534915, - "heading": -4.9482596020305806e-8, - "angularVelocity": 9.582614101786845e-8, - "velocityX": -3.718801184433256, - "velocityY": 0.21482472211899678, - "timestamp": 6.896498107920314 + "x": 8.019011714129077, + "y": 2.0771819587981266, + "heading": 0.8464048765673222, + "angularVelocity": 1.5252250448882378, + "velocityX": 1.6741769641730917, + "velocityY": 1.5915140036844804, + "timestamp": 7.478860427539482 }, { - "x": 5.471871496557958, - "y": 6.562921997343888, - "heading": -4.486806959492394e-8, - "angularVelocity": 9.057427814015043e-8, - "velocityX": -3.7236292508966913, - "velocityY": -0.10108004880484801, - "timestamp": 6.947445534939733 + "x": 8.090768492192002, + "y": 2.1628615391483916, + "heading": 0.9080474489075738, + "angularVelocity": 1.1685085881929373, + "velocityX": 1.360235438663432, + "velocityY": 1.6241587862269713, + "timestamp": 7.531613631891689 }, { - "x": 5.28328089006521, - "y": 6.541715087274231, - "heading": -4.036796232249368e-8, - "angularVelocity": 8.83284502417564e-8, - "velocityX": -3.7016708698726792, - "velocityY": -0.41625085525070266, - "timestamp": 6.998392961959151 + "x": 8.146101418716428, + "y": 2.2500495157538962, + "heading": 0.9485346126333066, + "angularVelocity": 0.7674825486508784, + "velocityX": 1.048901715145006, + "velocityY": 1.6527522389615, + "timestamp": 7.5843668362438965 }, { - "x": 5.095501405649377, - "y": 6.514237548522189, - "heading": -3.588263316902823e-8, - "angularVelocity": 8.803838419074343e-8, - "velocityX": -3.685750103616821, - "velocityY": -0.5393312353451875, - "timestamp": 7.04934038897857 + "x": 8.185122291410421, + "y": 2.338598291571523, + "heading": 0.9662103754330295, + "angularVelocity": 0.3350651968307046, + "velocityX": 0.7396872507207403, + "velocityY": 1.6785478134452219, + "timestamp": 7.637120040596104 }, { - "x": 4.907721944965666, - "y": 6.486759847587045, - "heading": -3.139730406205809e-8, - "angularVelocity": 8.803838327812946e-8, - "velocityX": -3.685749637800889, - "velocityY": -0.5393344186875336, - "timestamp": 7.100287815997989 + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "angularVelocity": -0.11638132008356272, + "velocityX": 0.4319241942959219, + "velocityY": 1.7021818924733116, + "timestamp": 7.689873244948311 }, { - "x": 4.719942484237288, - "y": 6.459282146957154, - "heading": -2.6911974883170177e-8, - "angularVelocity": 8.803838468973724e-8, - "velocityX": -3.6857496386776267, - "velocityY": -0.5393344126960062, - "timestamp": 7.151235243017408 + "x": 8.211423054828662, + "y": 2.540472392253658, + "heading": 0.9162466453352144, + "angularVelocity": -0.6758725051077806, + "velocityX": 0.054215367725083355, + "velocityY": 1.7285138763859442, + "timestamp": 7.754714235231942 }, { - "x": 4.532163023549714, - "y": 6.431804446048415, - "heading": -2.2426645714855718e-8, - "angularVelocity": 8.803838448220064e-8, - "velocityX": -3.685749637876727, - "velocityY": -0.5393344181692645, - "timestamp": 7.2021826700368266 + "x": 8.190202888383633, + "y": 2.6538474230997973, + "heading": 0.8391734249667565, + "angularVelocity": -1.1886496494165113, + "velocityX": -0.3272646878495706, + "velocityY": 1.7485086262595175, + "timestamp": 7.819555225515574 }, { - "x": 4.34438356286167, - "y": 6.404326745142886, - "heading": -1.794131654726717e-8, - "angularVelocity": 8.803838446795243e-8, - "velocityX": -3.6857496378859507, - "velocityY": -0.5393344181062291, - "timestamp": 7.253130097056245 + "x": 8.144008306424501, + "y": 2.768019013990823, + "heading": 0.7319233890258415, + "angularVelocity": -1.6540468532601917, + "velocityX": -0.7124286929774685, + "velocityY": 1.760793448582603, + "timestamp": 7.884396215799205 }, { - "x": 4.156604102201168, - "y": 6.376849044049133, - "heading": -1.3455987422255126e-8, - "angularVelocity": 8.803838363225754e-8, - "velocityX": -3.685749637345336, - "velocityY": -0.5393344218007294, - "timestamp": 7.304077524075664 + "x": 8.072604152106418, + "y": 2.8824539716664033, + "heading": 0.5977397407756817, + "angularVelocity": -2.0694262635904495, + "velocityX": -1.1012193676521893, + "velocityY": 1.764855181498803, + "timestamp": 7.949237206082836 }, { - "x": 3.9688246409539834, - "y": 6.349371346964709, - "heading": -8.97065828327331e-9, - "angularVelocity": 8.80383839064573e-8, - "velocityX": -3.685749648860808, - "velocityY": -0.5393343431053033, - "timestamp": 7.355024951095083 + "x": 7.975691382967155, + "y": 2.9965380091631473, + "heading": 0.44171220804424666, + "angularVelocity": -2.406310145001336, + "velocityX": -1.4946219777850651, + "velocityY": 1.7594431700951976, + "timestamp": 8.014078196366468 }, { - "x": 3.7810451818268453, - "y": 6.32189363539213, - "heading": -4.485329161078249e-9, - "angularVelocity": 8.80383835769656e-8, - "velocityX": -3.685749607248367, - "velocityY": -0.5393346274799247, - "timestamp": 7.405972378114502 + "x": 7.852920805151493, + "y": 3.1093104190449847, + "heading": 0.2733111632783997, + "angularVelocity": -2.5971386931201565, + "velocityX": -1.8934099753663647, + "velocityY": 1.7392147989804203, + "timestamp": 8.078919186650099 }, { - "x": 3.5932657187551267, - "y": 6.294415950776365, - "heading": 7.629972110817107e-24, - "angularVelocity": 8.803838434016792e-8, - "velocityX": -3.6857496846728943, - "velocityY": -0.5393340983695197, - "timestamp": 7.4569198051339205 + "x": 7.704278478364254, + "y": 3.219028342207882, + "heading": 0.10912005253192102, + "angularVelocity": -2.5322116461865276, + "velocityX": -2.292412964963042, + "velocityY": 1.6921074567640455, + "timestamp": 8.14376017693373 }, { - "x": 3.405486822128296, - "y": 6.266934394836426, - "heading": 3.646452356482733e-24, - "angularVelocity": -1.9320923857305114e-25, - "velocityX": -3.6857385664492472, - "velocityY": -0.5394100850169479, - "timestamp": 7.507867232153339 + "x": 7.532421373794059, + "y": 3.323057914868166, + "heading": -0.010926874391562237, + "angularVelocity": -1.851404896784686, + "velocityX": -2.650439233245006, + "velocityY": 1.6043797635605452, + "timestamp": 8.208601167217362 }, { - "x": 3.2165781142286494, - "y": 6.223116050022905, - "heading": 3.318409186414545e-24, - "angularVelocity": -1.5338201174379227e-26, - "velocityX": -3.536673395714516, - "velocityY": -0.8203495543918979, - "timestamp": 7.561281468290765 + "x": 7.337707687800787, + "y": 3.422034456346363, + "heading": -0.08211008235538154, + "angularVelocity": -1.0978118571669786, + "velocityX": -3.0029412743627657, + "velocityY": 1.5264501829051038, + "timestamp": 8.273442157500993 }, { - "x": 3.041563345872727, - "y": 6.190252158258573, - "heading": 2.990185107220459e-24, - "angularVelocity": -1.87251095899189e-26, - "velocityX": -3.2765566075987578, - "velocityY": -0.6152646586535303, - "timestamp": 7.6146957044281915 + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": -0.09725138680843892, + "angularVelocity": -0.2335143924672555, + "velocityX": -3.3462025777705335, + "velocityY": 1.4627042932775656, + "timestamp": 8.338283147784624 }, { - "x": 2.8804424282697485, - "y": 6.168342842771101, - "heading": 2.6619349218872592e-24, - "angularVelocity": -1.9213858301671557e-26, - "velocityX": -3.0164414817885197, - "velocityY": -0.4101774558958965, - "timestamp": 7.668109940565618 + "x": 7.010606209661033, + "y": 3.566708155420985, + "heading": -0.09725146950362613, + "angularVelocity": -0.000002548325484883459, + "velocityX": -3.39376555274088, + "velocityY": 1.5355711508400656, + "timestamp": 8.370733942512778 + }, + { + "x": 6.900475999605217, + "y": 3.616539055790813, + "heading": -0.09725155219704654, + "angularVelocity": -0.0000025482710392560423, + "velocityX": -3.3937600289421765, + "velocityY": 1.5355833589676455, + "timestamp": 8.403184737240933 }, { - "x": 2.733215332226118, - "y": 6.157388144124356, - "heading": 2.333764868738876e-24, - "angularVelocity": -1.7713655535187625e-26, - "velocityX": -2.7563269025291364, - "velocityY": -0.20508949371775206, - "timestamp": 7.721524176703044 + "x": 6.790345789587328, + "y": 3.6663699562444614, + "heading": -0.09725163489046874, + "angularVelocity": -0.0000025482710945290222, + "velocityX": -3.3937600277734288, + "velocityY": 1.5355833615506584, + "timestamp": 8.435635531969087 }, { - "x": 2.5998820432630403, - "y": 6.157388082450519, - "heading": 2.0056312293962657e-24, - "angularVelocity": -1.7031930803836163e-26, - "velocityX": -2.4962125943359705, - "velocityY": -0.0000011546329394271484, - "timestamp": 7.77493841284047 + "x": 6.680215579569448, + "y": 3.7162008566981277, + "heading": -0.0972517175838927, + "angularVelocity": -0.0000025482711487845157, + "velocityX": -3.393760027773178, + "velocityY": 1.5355833615512018, + "timestamp": 8.468086326697241 }, { - "x": 2.4804425527424723, - "y": 6.16834266976643, - "heading": 1.6774969061310235e-24, - "angularVelocity": -1.7044734930100497e-26, - "velocityX": -2.2360984478608033, - "velocityY": 0.20508740942632245, - "timestamp": 7.828352648977896 + "x": 6.570085369551568, + "y": 3.7660317571517936, + "heading": -0.09725180027731849, + "angularVelocity": -0.0000025482712051303815, + "velocityX": -3.3937600277731743, + "velocityY": 1.5355833615511987, + "timestamp": 8.500537121425396 }, { - "x": 2.3748968549301686, - "y": 6.190251914052291, - "heading": 1.3492831778827425e-24, - "angularVelocity": -1.853132331504253e-26, - "velocityX": -1.97598440873988, - "velocityY": 0.4101761228877669, - "timestamp": 7.881766885115322 + "x": 6.4599551595336875, + "y": 3.8158626576054595, + "heading": -0.09725188297074605, + "angularVelocity": -0.0000025482712597994537, + "velocityX": -3.3937600277731703, + "velocityY": 1.5355833615511956, + "timestamp": 8.53298791615355 }, { - "x": 2.283244945743567, - "y": 6.22311582099134, - "heading": 1.0211293460600033e-24, - "angularVelocity": -1.7409966353369085e-26, - "velocityX": -1.7158704460510679, - "velocityY": 0.6152649427485215, - "timestamp": 7.935181121252748 + "x": 6.349824949515808, + "y": 3.8656935580591254, + "heading": -0.0972519656641754, + "angularVelocity": -0.0000025482713151989977, + "velocityX": -3.393760027773166, + "velocityY": 1.5355833615511918, + "timestamp": 8.565438710881704 }, { - "x": 2.205486822128296, - "y": 6.266934394836426, - "heading": 6.9419459427461045e-25, - "angularVelocity": 5.413163312168308e-27, - "velocityX": -1.455756540544968, - "velocityY": 0.8203538422294017, - "timestamp": 7.9885953573901745 + "x": 6.239694739497929, + "y": 3.9155244585127913, + "heading": -0.09725204835760656, + "angularVelocity": -0.00000254827137059863, + "velocityX": -3.3937600277731623, + "velocityY": 1.5355833615511885, + "timestamp": 8.597889505609858 }, { - "x": 2.136159340261779, - "y": 6.327632048563378, - "heading": 0.025231456124497797, - "angularVelocity": 0.43032288322850026, - "velocityX": -1.1823812996193095, - "velocityY": 1.035199444221815, - "timestamp": 8.047229134741727 + "x": 6.129564529480049, + "y": 3.9653553589664567, + "heading": -0.09725213105103951, + "angularVelocity": -0.000002548271425616177, + "velocityX": -3.3937600277731588, + "velocityY": 1.535583361551185, + "timestamp": 8.630340300338013 }, { - "x": 2.084207176746768, - "y": 6.399028713640929, - "heading": 0.07562693390361644, - "angularVelocity": 0.8594956705068004, - "velocityX": -0.8860449703507888, - "velocityY": 1.2176712520067714, - "timestamp": 8.10586291209328 + "x": 6.019434319462169, + "y": 4.015186259420122, + "heading": -0.09725221374447425, + "angularVelocity": -0.0000025482714812860075, + "velocityX": -3.393760027773155, + "velocityY": 1.5355833615511818, + "timestamp": 8.662791095066167 }, { - "x": 2.051566754965305, - "y": 6.477423275068575, - "heading": 0.14957404640599054, - "angularVelocity": 1.2611691731714223, - "velocityX": -0.5566829096778134, - "velocityY": 1.3370204849265277, - "timestamp": 8.164496689444832 + "x": 5.90930410944429, + "y": 4.065017159873787, + "heading": -0.09725229643791082, + "angularVelocity": -0.0000025482715370955637, + "velocityX": -3.3937600277731512, + "velocityY": 1.5355833615511787, + "timestamp": 8.695241889794321 }, { - "x": 2.039926841667985, - "y": 6.553838491753715, - "heading": 0.2360367067446735, - "angularVelocity": 1.4746220394479557, - "velocityX": -0.19851890536627906, - "velocityY": 1.3032627290405507, - "timestamp": 8.223130466796384 + "x": 5.79917389942641, + "y": 4.114848060327453, + "heading": -0.09725237913134915, + "angularVelocity": -0.0000025482715921357367, + "velocityX": -3.3937600277731477, + "velocityY": 1.5355833615511751, + "timestamp": 8.727692684522475 }, { - "x": 2.042236938900771, - "y": 6.614274854513032, - "heading": 0.313525546116211, - "angularVelocity": 1.3215733809359698, - "velocityX": 0.039398744838411756, - "velocityY": 1.0307431226365935, - "timestamp": 8.281764244147936 + "x": 5.689043689408531, + "y": 4.164678960781118, + "heading": -0.09725246182478928, + "angularVelocity": -0.0000025482716465928655, + "velocityX": -3.3937600277731437, + "velocityY": 1.5355833615511723, + "timestamp": 8.76014347925063 }, { - "x": 2.0480382687015357, - "y": 6.654459061696839, - "heading": 0.3665932060292166, - "angularVelocity": 0.9050697790597129, - "velocityX": 0.09894177149770778, - "velocityY": 0.6853422890166648, - "timestamp": 8.340398021499489 + "x": 5.5789134793906525, + "y": 4.2145098612347835, + "heading": -0.09725254451823122, + "angularVelocity": -0.0000025482717030707046, + "velocityX": -3.39376002777314, + "velocityY": 1.535583361551169, + "timestamp": 8.792594273978784 }, { - "x": 2.052253484725952, - "y": 6.674348831176758, - "heading": 0.3930579718029317, - "angularVelocity": 0.4513569988001917, - "velocityX": 0.07189057595834777, - "velocityY": 0.3392203330286158, - "timestamp": 8.399031798851041 + "x": 5.468783269372774, + "y": 4.264340761688448, + "heading": -0.09725262721167492, + "angularVelocity": -0.000002548271757487181, + "velocityX": -3.3937600277731357, + "velocityY": 1.535583361551166, + "timestamp": 8.825045068706938 }, { - "x": 2.052253484725952, - "y": 6.674348831176758, - "heading": 0.3930579718029317, - "angularVelocity": 7.733121458673518e-26, - "velocityX": -6.859499621919341e-22, - "velocityY": -2.8560302702061698e-22, - "timestamp": 8.457665576202594 + "x": 5.358653059354908, + "y": 4.314171662142141, + "heading": -0.09725270990512043, + "angularVelocity": -0.000002548271812724623, + "velocityX": -3.3937600277727378, + "velocityY": 1.5355833615520338, + "timestamp": 8.857495863435092 }, { - "x": 2.103013204396169, - "y": 6.695498705680337, - "heading": 0.3930579718029317, - "angularVelocity": 4.836322083094603e-17, - "velocityX": 0.5390415085998568, - "velocityY": 0.22460053627515192, - "timestamp": 8.551832201333307 + "x": 5.248522849397515, + "y": 4.364002562729485, + "heading": -0.09725279259856773, + "angularVelocity": -0.000002548271867828041, + "velocityX": -3.393760025909188, + "velocityY": 1.5355833656706142, + "timestamp": 8.889946658163247 }, { - "x": 2.204532642750927, - "y": 6.737798454276797, - "heading": 0.3930579718029317, - "angularVelocity": 1.8057826879047683e-17, - "velocityX": 1.078083006732355, - "velocityY": 0.4492010681889061, - "timestamp": 8.64599882646402 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.000002548289809515153, + "velocityX": -3.3937512180408453, + "velocityY": 1.5356028315849852, + "timestamp": 8.922397452891401 }, { - "x": 2.356811797490316, - "y": 6.801248076007844, - "heading": 0.3930579718029317, - "angularVelocity": 9.246174062812876e-17, - "velocityX": 1.6171244804410174, - "velocityY": 0.6738015899260662, - "timestamp": 8.740165451594732 + "x": 4.9137282295128415, + "y": 4.535640155229253, + "heading": -0.09725287529313524, + "angularVelocity": -7.495790555066974e-12, + "velocityX": -3.1302347498710463, + "velocityY": 1.6971138308958609, + "timestamp": 8.994169928025691 }, { - "x": 2.5598506571147865, - "y": 6.885847566082, - "heading": 0.3930579718029317, - "angularVelocity": 1.8302909821567636e-16, - "velocityX": 2.1561658320305157, - "velocityY": 0.8984020607802621, - "timestamp": 8.834332076725445 + "x": 4.717146626097375, + "y": 4.642220517268037, + "heading": -0.09725287529322811, + "angularVelocity": -1.2941727290342862e-12, + "velocityX": -2.738955331380901, + "velocityY": 1.4849754288028527, + "timestamp": 9.065942403159982 }, { - "x": 2.7121298118541755, - "y": 6.9492971878130465, - "heading": 0.3930579718029317, - "angularVelocity": 1.1024323128215209e-16, - "velocityX": 1.6171244804410174, - "velocityY": 0.6738015899260661, - "timestamp": 8.928498701856158 + "x": 4.5486481109529855, + "y": 4.733575136181824, + "heading": -0.09725287529316204, + "angularVelocity": 9.207220397125275e-13, + "velocityX": -2.347675969501116, + "velocityY": 1.2728364006237491, + "timestamp": 9.137714878294272 }, { - "x": 2.8136492502089334, - "y": 6.991596936409507, - "heading": 0.3930579718029317, - "angularVelocity": -3.9462401223913846e-17, - "velocityX": 1.078083006732355, - "velocityY": 0.44920106818890604, - "timestamp": 9.022665326986871 + "x": 4.408232682725307, + "y": 4.809703996992033, + "heading": -0.09725287529303236, + "angularVelocity": 1.806653851545192e-12, + "velocityX": -1.9563966264916148, + "velocityY": 1.0606971637493046, + "timestamp": 9.209487353428562 }, { - "x": 2.8644089698791504, - "y": 7.012746810913086, - "heading": 0.3930579718029317, - "angularVelocity": 1.2061194980042807e-16, - "velocityX": 0.5390415085998568, - "velocityY": 0.22460053627515186, - "timestamp": 9.116831952117584 + "x": 4.2959003407371545, + "y": 4.870607092209374, + "heading": -0.09725287529288679, + "angularVelocity": 2.028301137290635e-12, + "velocityX": -1.5651172929172836, + "velocityY": 0.8485578225272127, + "timestamp": 9.281259828562852 }, { - "x": 2.8644089698791504, - "y": 7.012746810913086, - "heading": 0.3930579718029317, - "angularVelocity": -7.454320302880939e-28, - "velocityX": 6.858010204674672e-22, - "velocityY": 2.856861212118512e-22, - "timestamp": 9.210998577248297 + "x": 4.211651084582215, + "y": 4.916284417340276, + "heading": -0.09725287529275393, + "angularVelocity": 1.850997831523441e-12, + "velocityX": -1.1738379650040627, + "velocityY": 0.6364184186965437, + "timestamp": 9.353032303697143 }, { - "x": 2.842186915849872, - "y": 7.004763611912437, - "heading": 0.3899906597418949, - "angularVelocity": -0.04969534926930016, - "velocityX": -0.36003273044640205, - "velocityY": -0.1293405609631654, - "timestamp": 9.272720893443621 + "x": 4.155484913989614, + "y": 4.946735969389024, + "heading": -0.09725287529265289, + "angularVelocity": 1.4078899342405594e-12, + "velocityX": -0.7825586408649186, + "velocityY": 0.424278973126829, + "timestamp": 9.424804778831433 }, { - "x": 2.7978951293903465, - "y": 6.988387145564426, - "heading": 0.38377625605112037, - "angularVelocity": -0.10068325483944376, - "velocityX": -0.7175976079601634, - "velocityY": -0.26532488340499644, - "timestamp": 9.334443209638945 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 7.752391774433126e-13, + "velocityX": -0.3912793194215449, + "velocityY": 0.2121394977435137, + "timestamp": 9.496577253965723 }, { - "x": 2.7317542660001495, - "y": 6.96306112048486, - "heading": 0.37430680047739806, - "angularVelocity": -0.15342028876161432, - "velocityX": -1.071587514326101, - "velocityY": -0.41032201383078476, - "timestamp": 9.396165525834268 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -8.979320165657004e-28, + "velocityX": -3.317577006729804e-27, + "velocityY": -2.7701124132227202e-27, + "timestamp": 9.568349729100014 }, { - "x": 2.644108850979856, - "y": 6.927989989672691, - "heading": 0.36142823816874126, - "angularVelocity": -0.20865325707968818, - "velocityX": -1.4199955611343862, - "velocityY": -0.5682082749646841, - "timestamp": 9.457887842029592 + "x": 4.149345287192321, + "y": 4.948730090302555, + "heading": -0.09725287542061174, + "angularVelocity": -1.9914932525911006e-9, + "velocityX": 0.34136955009475556, + "velocityY": -0.20584195701235133, + "timestamp": 9.632630384537388 }, { - "x": 2.53555910261038, - "y": 6.881949867747595, - "heading": 0.3449043845685677, - "angularVelocity": -0.267712792045632, - "velocityX": -1.7586791141467173, - "velocityY": -0.7459234319625763, - "timestamp": 9.519610158224916 + "x": 4.193331867161711, + "y": 4.922433486987429, + "heading": -0.09725287565526207, + "angularVelocity": -3.6504037180002943e-9, + "velocityX": 0.6842895373437039, + "velocityY": -0.4090904664272751, + "timestamp": 9.696911039974761 }, { - "x": 2.407346628684672, - "y": 6.822852176062135, - "heading": 0.3243353524397527, - "angularVelocity": -0.33325113827101066, - "velocityX": -2.0772466399344327, - "velocityY": -0.9574768953653868, - "timestamp": 9.58133247442024 + "x": 4.259487364434434, + "y": 4.883286547964955, + "heading": -0.09725287596919822, + "angularVelocity": -4.883835550017299e-9, + "velocityX": 1.02916650153289, + "velocityY": -0.6090003089749592, + "timestamp": 9.761191695412135 }, { - "x": 2.2628964345938334, - "y": 6.74663530878728, - "heading": 0.298961324703692, - "angularVelocity": -0.41109973345399, - "velocityX": -2.3403236138079526, - "velocityY": -1.2348348534695668, - "timestamp": 9.643054790615563 + "x": 4.347975400700828, + "y": 4.831575784808056, + "heading": -0.09725287632620161, + "angularVelocity": -5.55382285919441e-9, + "velocityX": 1.3765888923239984, + "velocityY": -0.8044529540816497, + "timestamp": 9.825472350849509 }, { - "x": 2.1156790935491716, - "y": 6.646996400731561, - "heading": 0.26784048264776694, - "angularVelocity": -0.5042072944482706, - "velocityX": -2.385155809428528, - "velocityY": -1.6143092838642825, - "timestamp": 9.704777106810887 + "x": 4.45901715765165, + "y": 4.76770271955414, + "heading": -0.09725287667606397, + "angularVelocity": -5.442731637489258e-9, + "velocityX": 1.7274521579669766, + "velocityY": -0.9936592092802461, + "timestamp": 9.889753006286883 }, { - "x": 1.9869123530220612, - "y": 6.532612256734745, - "heading": 0.23448482199122617, - "angularVelocity": -0.540414921419745, - "velocityX": -2.0862266432066443, - "velocityY": -1.8532056320576245, - "timestamp": 9.766499423006211 + "x": 4.592927185358519, + "y": 4.69226963680943, + "heading": -0.09725287694462323, + "angularVelocity": -4.177917358984547e-9, + "velocityX": 2.083208809800217, + "velocityY": -1.1734958555019974, + "timestamp": 9.954033661724257 }, { - "x": 1.881043758757729, - "y": 6.412413108766105, - "heading": 0.20123842984354556, - "angularVelocity": -0.5386445972388703, - "velocityX": -1.7152401398758959, - "velocityY": -1.9474179742098816, - "timestamp": 9.828221739201535 + "x": 4.750184246245984, + "y": 4.606276833638444, + "heading": -0.09725287701156489, + "angularVelocity": -1.0413967703177253e-9, + "velocityX": 2.446413463233511, + "velocityY": -1.3377711005881947, + "timestamp": 10.01831431716163 }, { - "x": 1.7987115747047, - "y": 6.290184489368743, - "heading": 0.16896416101959177, - "angularVelocity": -0.5228946483767686, - "velocityX": -1.3339127422322181, - "velocityY": -1.9802986493663424, - "timestamp": 9.889944055396859 + "x": 4.931581310093376, + "y": 4.511690334370361, + "heading": -0.0972528766497097, + "angularVelocity": 5.629301248871626e-9, + "velocityX": 2.821954172886779, + "velocityY": -1.4714613381662662, + "timestamp": 10.082594972599004 }, { - "x": 1.740001559257507, - "y": 6.167843341827393, - "heading": 0.13807920449538028, - "angularVelocity": -0.5003855724803669, - "velocityX": -0.9511959217700356, - "velocityY": -1.9821217848370036, - "timestamp": 9.951666371592182 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 2.1112299822496666e-8, + "velocityX": 3.2173227507078654, + "velocityY": -1.5223279648179473, + "timestamp": 10.146875628036378 }, { - "x": 1.7034889997175517, - "y": 6.016151230649133, - "heading": 0.1019414560444192, - "angularVelocity": -0.467252098060329, - "velocityX": -0.4720983122052568, - "velocityY": -1.961341263510758, - "timestamp": 10.029007380013498 + "x": 5.398868955921396, + "y": 4.32613897098341, + "heading": -0.09725286997797288, + "angularVelocity": 7.20303645148975e-8, + "velocityX": 3.5302934161705415, + "velocityY": -1.1885528129676433, + "timestamp": 10.220658738943204 }, { - "x": 1.7027290435050462, - "y": 5.874298147554192, - "heading": 0.06966485107958241, - "angularVelocity": -0.4173284732597485, - "velocityX": -0.009826044785523666, - "velocityY": -1.8341250778913685, - "timestamp": 10.106348388434814 + "x": 5.6681314885139065, + "y": 4.271040046607528, + "heading": -0.09725286670603911, + "angularVelocity": 4.4345294143454955e-8, + "velocityX": 3.6493789606207083, + "velocityY": -0.7467687889368465, + "timestamp": 10.29444184985003 }, { - "x": 1.7342944275877592, - "y": 5.750595898188055, - "heading": 0.04250435573420922, - "angularVelocity": -0.35117844853297475, - "velocityX": 0.40813256417294685, - "velocityY": -1.5994393128709739, - "timestamp": 10.18368939685613 + "x": 5.942117714678701, + "y": 4.249366302589977, + "heading": -0.0972528635577222, + "angularVelocity": 4.26698858409276e-8, + "velocityX": 3.713400299843557, + "velocityY": -0.29374939266142885, + "timestamp": 10.368224960756855 }, { - "x": 1.7930814617897048, - "y": 5.6520476897956975, - "heading": 0.02146340510922574, - "angularVelocity": -0.2720542575597529, - "velocityX": 0.7601017287194328, - "velocityY": -1.2742038202491095, - "timestamp": 10.261030405277445 + "x": 6.216382330790907, + "y": 4.231558335942029, + "heading": -0.09725286041527656, + "angularVelocity": 4.259031080949294e-8, + "velocityX": 3.7171733848218165, + "velocityY": -0.24135559519084493, + "timestamp": 10.44200807166368 }, { - "x": 1.8734294527078426, - "y": 5.583639683859189, - "heading": 0.0071877988424821045, - "angularVelocity": -0.1845800379143917, - "velocityX": 1.0388795356848963, - "velocityY": -0.8844984999918234, - "timestamp": 10.338371413698761 + "x": 6.4906469514050675, + "y": 4.21375043862986, + "heading": -0.09725285727283085, + "angularVelocity": 4.259031210936323e-8, + "velocityX": 3.717173445837876, + "velocityY": -0.241354655466577, + "timestamp": 10.515791182570506 }, { - "x": 1.970064237353583, - "y": 5.548515796661377, - "heading": 3.498573366455402e-27, - "angularVelocity": -0.09293645103935767, - "velocityX": 1.249463727177213, - "velocityY": -0.45414312425918, - "timestamp": 10.415712422120077 + "x": 6.764911572019293, + "y": 4.195942541318692, + "heading": -0.09725285413038519, + "angularVelocity": 4.259031138372869e-8, + "velocityX": 3.717173445838756, + "velocityY": -0.24135465545301274, + "timestamp": 10.589574293477332 }, { - "x": 2.0785037517547607, - "y": 5.548515796661377, - "heading": 9.074763160083063e-28, - "angularVelocity": -2.8883864105140666e-27, - "velocityX": 1.4020959464409022, - "velocityY": 1.1648614692434482e-25, - "timestamp": 10.493053430541393 + "x": 7.039176191804645, + "y": 4.178134631242245, + "heading": -0.09725285098790416, + "angularVelocity": 4.2590790558228534e-8, + "velocityX": 3.717173434604845, + "velocityY": -0.24135482846385797, + "timestamp": 10.663357404384158 }, { - "x": 2.2093539983390484, - "y": 5.548515796661377, - "heading": -5.091869940539703e-28, - "angularVelocity": -8.863496880486678e-29, - "velocityX": 1.8425023918344958, - "velocityY": 6.335504596026049e-17, - "timestamp": 10.56407111125876 + "x": 7.303151625715878, + "y": 4.160355738175747, + "heading": -0.07493794888540253, + "angularVelocity": 0.3024391602392781, + "velocityX": 3.5777216583424782, + "velocityY": -0.24096155404655678, + "timestamp": 10.737140515290983 }, { - "x": 2.3657372440174322, - "y": 5.548515796661377, - "heading": -1.9258503020667073e-27, - "angularVelocity": -8.863493994530242e-29, - "velocityX": 2.202032565675437, - "velocityY": 3.167727717458953e-16, - "timestamp": 10.635088791976129 + "x": 7.533445930864736, + "y": 4.144763761571347, + "heading": -0.05552477724704528, + "angularVelocity": 0.26311131910489216, + "velocityX": 3.1212333326481616, + "velocityY": -0.21132175660214023, + "timestamp": 10.810923626197809 }, { - "x": 2.4908438453710073, - "y": 5.548515796661377, - "heading": -3.342513611103023e-27, - "angularVelocity": -8.863495435827685e-29, - "velocityX": 1.7616261202821912, - "velocityY": 2.534184135606925e-16, - "timestamp": 10.706106472693497 + "x": 7.730059092320318, + "y": 4.131358730965977, + "heading": -0.03901370974052792, + "angularVelocity": 0.22377841356361258, + "velocityX": 2.664744804591801, + "velocityY": -0.1816815588366564, + "timestamp": 10.884706737104635 }, { - "x": 2.5846737979725942, - "y": 5.548515796661377, - "heading": -4.759176920139342e-27, - "angularVelocity": -8.863495435827717e-29, - "velocityX": 1.321219612549812, - "velocityY": 1.9006387498434594e-16, - "timestamp": 10.777124153410865 + "x": 7.892991106532594, + "y": 4.120140669647355, + "heading": -0.02540508118930727, + "angularVelocity": 0.18444097008061852, + "velocityX": 2.20825622842103, + "velocityY": -0.15204104544723035, + "timestamp": 10.95848984801146 }, { - "x": 2.6472271003464662, - "y": 5.548515796661377, - "heading": -6.175840229175665e-27, - "angularVelocity": -8.863495435827698e-29, - "velocityX": 0.8808130840377387, - "velocityY": 1.2670927613090377e-16, - "timestamp": 10.848141834128233 + "x": 8.02224197249394, + "y": 4.111109595697515, + "heading": -0.014699203415963554, + "angularVelocity": 0.14509930039224503, + "velocityX": 1.7517676385937064, + "velocityY": -0.12240028698767837, + "timestamp": 11.032272958918286 }, { - "x": 2.6785037517547607, - "y": 5.548515796661377, - "heading": -7.592503538214036e-27, - "angularVelocity": -8.863495438700938e-29, - "velocityX": 0.44040654513582195, - "velocityY": 6.3354647036682e-17, - "timestamp": 10.919159514845601 + "x": 8.117811689845258, + "y": 4.104265522227424, + "heading": -0.0068963232026660135, + "angularVelocity": 0.10575428600660669, + "velocityX": 1.2952790438994197, + "velocityY": -0.09275935083211628, + "timestamp": 11.106056069825112 }, { - "x": 2.6785037517547607, - "y": 5.548515796661377, - "heading": -9.168166896056586e-27, - "angularVelocity": -2.3275147262701813e-27, - "velocityX": -7.876142714980879e-26, - "velocityY": -2.7822667519177177e-25, - "timestamp": 10.99017719556297 + "x": 8.179700258175622, + "y": 4.099608457473234, + "heading": -0.001996583897562948, + "angularVelocity": 0.06640732878951901, + "velocityX": 0.8387904436357886, + "velocityY": -0.06311830305000711, + "timestamp": 11.179839180731937 }, { - "x": 2.6551487281624913, - "y": 5.535337362195577, - "heading": -0.0053582663887861755, - "angularVelocity": -0.08143329787865013, - "velocityX": -0.3549425234125067, - "velocityY": -0.20028182653884286, - "timestamp": 11.055976647732658 + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": 5.455800259144416e-28, + "angularVelocity": 0.027060175059361254, + "velocityX": 0.38230183268875023, + "velocityY": -0.03347720903449647, + "timestamp": 11.253622291638763 }, { - "x": 2.608944125248311, - "y": 5.508126235802502, - "heading": -0.016249929633628882, - "angularVelocity": -0.16552817517013058, - "velocityX": -0.7022034590048659, - "velocityY": -0.41354639736059495, - "timestamp": 11.121776099902347 + "x": 8.200639571345327, + "y": 4.096943687462525, + "heading": -0.0010802899918473188, + "angularVelocity": -0.0140223298346035, + "velocityX": -0.0943411225502494, + "velocityY": -0.002527461513954009, + "timestamp": 11.330662983491656 }, { - "x": 2.5407349136089206, - "y": 5.4655968271259, - "heading": -0.03293816472196888, - "angularVelocity": -0.2536227056313985, - "velocityX": -1.0366227892519315, - "velocityY": -0.6463489782091855, - "timestamp": 11.187575552072035 + "x": 8.15665056358688, + "y": 4.099133352364744, + "heading": -0.0053255986809540795, + "angularVelocity": -0.05510475810903991, + "velocityX": -0.5709840695932866, + "velocityY": 0.028422186373926028, + "timestamp": 11.40770367534455 }, { - "x": 2.4521626111212957, - "y": 5.405634188324023, - "heading": -0.05584758703948852, - "angularVelocity": -0.34817041118274633, - "velocityX": -1.3460948315984256, - "velocityY": -0.9112938911290825, - "timestamp": 11.253375004241724 + "x": 8.075940653456616, + "y": 4.103707386175576, + "heading": -0.012735784669566218, + "angularVelocity": -0.09618535101893501, + "velocityX": -1.0476270161796533, + "velocityY": 0.05937166062275022, + "timestamp": 11.484744367197443 }, { - "x": 2.347299393995622, - "y": 5.324395116461008, - "heading": -0.08569447081932852, - "angularVelocity": -0.45360383400866283, - "velocityX": -1.593679182240697, - "velocityY": -1.2346466297851608, - "timestamp": 11.319174456411412 + "x": 7.958509840643352, + "y": 4.110665769785427, + "heading": -0.023310616019513113, + "angularVelocity": -0.13726293333579098, + "velocityX": -1.5242699668052715, + "velocityY": 0.0903208868261235, + "timestamp": 11.561785059050337 }, { - "x": 2.239880929566337, - "y": 5.216483135728542, - "heading": -0.12272614687803589, - "angularVelocity": -0.5627961151288577, - "velocityX": -1.6325130512069779, - "velocityY": -1.6400133614209311, - "timestamp": 11.3849739085811 + "x": 7.804358124937036, + "y": 4.120008478318513, + "heading": -0.037049836652876506, + "angularVelocity": -0.17833719172197993, + "velocityX": -2.000912920157348, + "velocityY": 0.12126979013798393, + "timestamp": 11.63882575090323 + }, + { + "x": 7.613485507099167, + "y": 4.131735481105862, + "heading": -0.05395325351914213, + "angularVelocity": -0.21940894428287547, + "velocityX": -2.477555863625089, + "velocityY": 0.15221829536189968, + "timestamp": 11.715866442756123 }, { - "x": 2.1533744032985735, - "y": 5.091739159361965, - "heading": -0.16180305396789932, - "angularVelocity": -0.5938789123819668, - "velocityX": -1.314699794835295, - "velocityY": -1.8958208959685579, - "timestamp": 11.450773360750789 + "x": 7.385891991055361, + "y": 4.145846741676364, + "heading": -0.07402084166347275, + "angularVelocity": -0.2604803729261552, + "velocityX": -2.954198756137707, + "velocityY": 0.18316632718520298, + "timestamp": 11.792907134609017 }, { - "x": 2.0932393074035645, - "y": 4.9619622230529785, - "heading": -0.19963034769256288, - "angularVelocity": -0.5748876696892883, - "velocityX": -0.9139148414173547, - "velocityY": -1.9723102857195456, - "timestamp": 11.516572812920478 + "x": 7.121577593711165, + "y": 4.162342217760912, + "heading": -0.09725284930983928, + "angularVelocity": -0.30155502355466035, + "velocityX": -3.4308414292137566, + "velocityY": 0.214113810349038, + "timestamp": 11.86994782646191 }, { - "x": 2.06290111070071, - "y": 4.862373019847466, - "heading": -0.22719939375272255, - "angularVelocity": -0.5460591677053325, - "velocityX": -0.6009076413123203, - "velocityY": -1.9725599970405536, - "timestamp": 11.56706010017913 + "x": 6.835203978246458, + "y": 4.180936324910101, + "heading": -0.09725285259123628, + "angularVelocity": -4.2593036391257635e-8, + "velocityX": -3.717173464790893, + "velocityY": 0.24135436354456943, + "timestamp": 11.946988518314804 }, { - "x": 2.048028599586168, - "y": 4.7660067703914, - "heading": -0.2526003668147138, - "angularVelocity": -0.5031162187791788, - "velocityX": -0.29457932723436914, - "velocityY": -1.9087230605671173, - "timestamp": 11.617547387437783 + "x": 6.5488303639215975, + "y": 4.199530449615619, + "heading": -0.09725285587256693, + "angularVelocity": -4.259217541458275e-8, + "velocityX": -3.717173449995519, + "velocityY": 0.2413545914284215, + "timestamp": 12.024029210167697 }, { - "x": 2.047740775609064, - "y": 4.67569763435831, - "heading": -0.27523200740159587, - "angularVelocity": -0.4482641436236724, - "velocityX": -0.00570091983015679, - "velocityY": -1.788750018800283, - "timestamp": 11.668034674696436 + "x": 6.262456749596831, + "y": 4.218124574322583, + "heading": -0.0972528591538976, + "angularVelocity": -4.259217545578449e-8, + "velocityX": -3.7171734499942994, + "velocityY": 0.2413545914471941, + "timestamp": 12.10106990202059 }, { - "x": 2.0608710405337267, - "y": 4.5937072504707785, - "heading": -0.29463334748351117, - "angularVelocity": -0.3842816902108432, - "velocityX": 0.26007071557230893, - "velocityY": -1.62398077495204, - "timestamp": 11.718521961955089 + "x": 5.976083139919176, + "y": 4.236718770600949, + "heading": -0.09725286243522821, + "angularVelocity": -4.259217464314882e-8, + "velocityX": -3.717173389674077, + "velocityY": 0.24135552045497397, + "timestamp": 12.178110593873484 }, { - "x": 2.0861920181883997, - "y": 4.5217424317614965, - "heading": -0.31047411563844995, - "angularVelocity": -0.3137575618548228, - "velocityX": 0.5015317524380114, - "velocityY": -1.4254047427939316, - "timestamp": 11.769009249213742 + "x": 5.690024921889411, + "y": 4.259659658304738, + "heading": -0.09725286572349409, + "angularVelocity": -4.268219562622653e-8, + "velocityX": -3.71307955769639, + "velocityY": 0.29777624203574676, + "timestamp": 12.255151285726377 }, { - "x": 2.1225463448038515, - "y": 4.461060494351439, - "heading": -0.3225286157246152, - "angularVelocity": -0.2387630776121657, - "velocityX": 0.7200689240680372, - "velocityY": -1.2019250925322578, - "timestamp": 11.819496536472395 + "x": 5.4092552652272206, + "y": 4.319022844462834, + "heading": -0.09725286915389227, + "angularVelocity": -4.452709474069528e-8, + "velocityX": -3.6444332197627625, + "velocityY": 0.7705432639604031, + "timestamp": 12.33219197757927 }, { - "x": 2.16890020662259, - "y": 4.412584630951135, - "heading": -0.3306480218329806, - "angularVelocity": -0.16082080359692727, - "velocityX": 0.9181293813878825, - "velocityY": -0.9601597953155385, - "timestamp": 11.869983823731047 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -7.968133230975529e-8, + "velocityX": -3.5158347290283003, + "velocityY": 1.2306645781352268, + "timestamp": 12.409232669432164 }, { - "x": 2.2243532862123785, - "y": 4.376998478332531, - "heading": -0.3347371673945201, - "angularVelocity": -0.08099356855103614, - "velocityX": 1.0983572816201457, - "velocityY": -0.7048537275590007, - "timestamp": 11.9204711109897 + "x": 4.909018653493503, + "y": 4.524275328875486, + "heading": -0.09725287687080802, + "angularVelocity": -2.1964896173308048e-8, + "velocityX": -3.192337867898694, + "velocityY": 1.5370761958403907, + "timestamp": 12.481084171767012 }, { - "x": 2.2881293296813965, - "y": 4.354815483093262, - "heading": -0.3347371673945201, - "angularVelocity": -3.351040193936762e-27, - "velocityX": 1.2632099471355258, - "velocityY": -0.43937784031895566, - "timestamp": 11.970958398248353 + "x": 4.71108590609518, + "y": 4.628682093882472, + "heading": -0.09725287720527087, + "angularVelocity": -4.654917836145884e-9, + "velocityX": -2.754747513502237, + "velocityY": 1.4530909113134631, + "timestamp": 12.55293567410186 }, { - "x": 2.4097151173059284, - "y": 4.31443319431648, - "heading": -0.3347371673945201, - "angularVelocity": -1.1055944271274773e-28, - "velocityX": 1.6917387541138205, - "velocityY": -0.5618772081689679, - "timestamp": 12.042828707614305 + "x": 4.54294597125426, + "y": 4.721368217002082, + "heading": -0.09725287703485491, + "angularVelocity": 2.3717800410462983e-9, + "velocityX": -2.3401032598781417, + "velocityY": 1.2899677822694202, + "timestamp": 12.624787176436708 }, { - "x": 2.501147235462863, - "y": 4.284859003573599, - "heading": -0.3347371673945201, - "angularVelocity": -1.1055944267694559e-28, - "velocityX": 1.2721820590944748, - "velocityY": -0.4114938561387572, - "timestamp": 12.114699016980257 + "x": 4.40369795049572, + "y": 4.80026148816612, + "heading": -0.0972528766496691, + "angularVelocity": 5.3608595378330946e-9, + "velocityX": -1.9379973449908554, + "velocityY": 1.0980044759032759, + "timestamp": 12.696638678771556 }, { - "x": 2.562139216145648, - "y": 4.265257651838079, - "heading": -0.3347371673945201, - "angularVelocity": -1.105594426769472e-28, - "velocityX": 0.8486394621209057, - "velocityY": -0.2727322577076049, - "timestamp": 12.18656932634621 + "x": 4.292814205558175, + "y": 4.864307082758427, + "heading": -0.09725287620265412, + "angularVelocity": 6.221372869321198e-9, + "velocityX": -1.5432348849269113, + "velocityY": 0.8913605493429585, + "timestamp": 12.768490181106404 }, { - "x": 2.592644691467285, - "y": 4.255486488342285, - "heading": -0.3347371673945201, - "angularVelocity": -1.1055944267596697e-28, - "velocityX": 0.42445170461571047, - "velocityY": -0.13595549514112273, - "timestamp": 12.258439635712161 + "x": 4.209952469660844, + "y": 4.91286872561946, + "heading": -0.09725287578795568, + "angularVelocity": 5.771604384474185e-9, + "velocityX": -1.1532359547775561, + "velocityY": 0.6758612037744437, + "timestamp": 12.840341683441252 }, { - "x": 2.592644691467285, - "y": 4.255486488342285, - "heading": -0.3347371673945201, - "angularVelocity": -2.916408988560576e-29, - "velocityX": -2.969429335311962e-26, - "velocityY": 1.2754135405824192e-26, - "timestamp": 12.330309945078113 - } - ], - "trajectoryWaypoints": [ - { - "timestamp": 0, - "isStopPoint": true, - "x": 1.3350272178649902, - "y": 5.601006507873535, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 + "x": 4.154873707750821, + "y": 4.945521497298995, + "heading": -0.09725287546928488, + "angularVelocity": 4.4351304984872785e-9, + "velocityX": -0.7665638173206214, + "velocityY": 0.4544480020384788, + "timestamp": 12.9121931857761 }, { - "timestamp": 0.5891141473001675, - "isStopPoint": false, - "x": 2.3368515968322754, - "y": 6.133185386657715, - "heading": 0.2525545797921912, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 2.459066716778208e-9, + "velocityX": -0.38234244368231685, + "velocityY": 0.22880870103743695, + "timestamp": 12.984044688110949 }, { - "timestamp": 1.5430433785236333, - "isStopPoint": false, - "x": 5.747424125671387, - "y": 7.119814872741699, - "heading": 0.2525545797921912, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -3.871307616700285e-29, + "velocityX": 3.757481365144797e-28, + "velocityY": -1.0154842203400723e-27, + "timestamp": 13.055896190445797 + } + ], + "trajectoryWaypoints": [ { - "timestamp": 1.9430147539809723, - "isStopPoint": false, - "x": 7.125290870666504, - "y": 7.458, + "timestamp": 0, + "isStopPoint": true, + "x": 0.387, + "y": 4.121134281158447, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 14 }, { - "timestamp": 2.508796597442937, + "timestamp": 0.6923742371436291, "isStopPoint": false, - "x": 8.186561584472656, - "y": 7.469284534454346, - "heading": 0, + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 10 }, { - "timestamp": 4.2320699051573625, + "timestamp": 1.3971973802479047, "isStopPoint": true, - "x": 4.337841033935547, - "y": 6.292478084564209, - "heading": 0.2639641302660189, + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 16 }, { - "timestamp": 4.9229006835446425, + "timestamp": 2.430115113382513, "isStopPoint": false, - "x": 5.851044178009033, - "y": 6.557126998901367, - "heading": 0, + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, + "headingConstrained": true, "controlIntervalCount": 12 }, { - "timestamp": 5.308843742309995, + "timestamp": 3.0066730692210744, "isStopPoint": false, - "x": 7.214789390563965, - "y": 6.145846366882324, - "heading": -0.344987478573796, + "x": 7.536985397338867, + "y": 1.0086194276809692, + "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 7 }, { - "timestamp": 5.861500074494232, + "timestamp": 3.456916790263765, "isStopPoint": false, - "x": 8.186561584472656, - "y": 5.798520088195801, - "heading": -0.344987478573796, + "x": 8.208122253417969, + "y": 0.763107419013977, + "heading": -0.34555563246426124, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { - "timestamp": 6.845550680900895, + "timestamp": 4.544126438842174, "isStopPoint": false, - "x": 5.851044178009033, - "y": 6.557126998901367, + "x": 5.5030035972595215, + "y": 1.563418984413147, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 18 }, { - "timestamp": 7.507867232153339, - "isStopPoint": false, - "x": 3.405486822128296, - "y": 6.266934394836426, - "heading": 0, + "timestamp": 5.6369430590525305, + "isStopPoint": true, + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 18 }, { - "timestamp": 7.9885953573901745, + "timestamp": 6.687562362256374, "isStopPoint": false, - "x": 2.205486822128296, - "y": 6.266934394836426, + "x": 5.5030035972595215, + "y": 1.563418984413147, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 8 + "headingConstrained": false, + "controlIntervalCount": 19 }, { - "timestamp": 8.457665576202594, - "isStopPoint": true, - "x": 2.052253484725952, - "y": 6.674348831176758, - "heading": 0.3930579718029317, + "timestamp": 7.689873244948311, + "isStopPoint": false, + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 10 }, { - "timestamp": 9.210998577248297, - "isStopPoint": true, - "x": 2.8644089698791504, - "y": 7.012746810913086, - "heading": 0.3930579718029317, + "timestamp": 8.338283147784624, + "isStopPoint": false, + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": 0.9600708878718816, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 + "headingConstrained": false, + "controlIntervalCount": 18 }, { - "timestamp": 9.951666371592182, + "timestamp": 8.922397452891401, "isStopPoint": false, - "x": 1.7400015592575073, - "y": 6.167843341827393, - "heading": 0, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 7 + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "timestamp": 10.493053430541393, - "isStopPoint": false, - "x": 2.0785037517547607, - "y": 5.548515796661377, - "heading": 0, + "timestamp": 9.568349729100014, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 9 }, { - "timestamp": 10.99017719556297, - "isStopPoint": true, - "x": 2.6785037517547607, - "y": 5.548515796661377, - "heading": 0, + "timestamp": 10.146875628036378, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 15 }, { - "timestamp": 11.516572812920478, + "timestamp": 11.253622291638763, "isStopPoint": false, - "x": 2.0932393074035645, - "y": 4.9619622230529785, + "x": 8.207907676696777, + "y": 4.097138404846191, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 9 + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "timestamp": 11.970958398248353, + "timestamp": 12.409232669432164, "isStopPoint": false, - "x": 2.2881293296813965, - "y": 4.354815483093262, - "heading": -0.3347371673945201, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 9 }, { - "timestamp": 12.330309945078113, + "timestamp": 13.055896190445797, "isStopPoint": true, - "x": 2.592644691467285, - "y": 4.255486488342285, - "heading": -0.3347371673945201, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -31182,36 +31230,10 @@ }, { "scope": [ - 5 - ], - "type": "StopPoint" - }, - { - "scope": [ - 12 - ], - "type": "StopPoint" - }, - { - "scope": [ - 1, 2 ], - "type": "StraightLine" - }, - { - "scope": [ - 13 - ], "type": "StopPoint" }, - { - "scope": [ - 10, - 11 - ], - "type": "ZeroAngularVelocity" - }, { "scope": [ 16 @@ -31220,37 +31242,15 @@ }, { "scope": [ - 19 + 12 ], "type": "StopPoint" }, { "scope": [ - 15, - 16 - ], - "type": "ZeroAngularVelocity" - }, - { - "scope": [ - 18, - 19 - ], - "type": "ZeroAngularVelocity" - }, - { - "scope": [ - 18 - ], - "type": "WptVelocityDirection", - "direction": -0.3347371673945201 - }, - { - "scope": [ - 15 - ], - "type": "WptVelocityDirection", - "direction": 0 + 7 + ], + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -31260,83 +31260,74 @@ "eventMarkers": [], "isTrajectoryStale": false }, - "SourceLanePHGF": { + "CenterLanePCBAFE": { "waypoints": [ { - "x": 0.387, - "y": 4.121134281158447, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 15 }, { - "x": 1.7826586961746216, - "y": 3.5168776512145996, - "heading": -0.79, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 14 }, { - "x": 3.0433270931243896, - "y": 2.9765214920043945, - "heading": -0.79, + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 8 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 12 }, { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { - "x": 8.208122253417969, - "y": 0.763107419013977, - "heading": -0.34555563246426124, + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 16 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, + "x": 4.16749906539917, + "y": 4.98994779586792, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 18 - }, - { - "x": 3.0433270931243896, - "y": 2.9765214920043945, - "heading": -0.79, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 11 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, + "x": 5.665951728820801, + "y": 4.196649074554443, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -31344,67 +31335,67 @@ "controlIntervalCount": 19 }, { - "x": 8.207907676696777, - "y": 2.428393840789795, - "heading": 0.9600708878718816, + "x": 8.220098495483398, + "y": 5.723839282989502, + "heading": 0.9263929393086, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 19 }, { - "x": 7.120736598968506, - "y": 3.5168776512145996, - "heading": 0.9600708878718816, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 18 + "controlIntervalCount": 12 }, { - "x": 5.138392925262451, - "y": 4.413834095001221, + "x": 4.127401828765869, + "y": 4.96196174621582, "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 12 }, { - "x": 4.127401828765869, - "y": 4.96196174621582, - "heading": -0.09725287529259725, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "headingConstrained": false, + "controlIntervalCount": 10 }, { - "x": 5.138392925262451, - "y": 4.413834095001221, - "heading": -0.09725287529259725, + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 8 }, { - "x": 8.207907676696777, - "y": 4.097138404846191, + "x": 8.1634765625, + "y": 4.073246955871582, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 13 }, { - "x": 5.138392925262451, - "y": 4.413834095001221, - "heading": -0.09725287529259725, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 12 }, { "x": 4.127401828765869, @@ -31418,2085 +31409,7954 @@ ], "trajectory": [ { - "x": 0.387, - "y": 4.121134281158447, - "heading": 5.0336126459058196e-26, - "angularVelocity": 1.4969503738616677e-26, - "velocityX": 6.632423953121246e-26, - "velocityY": 2.3403167387234696e-25, + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -5.237204901969609e-25, + "angularVelocity": 6.164254262445062e-25, + "velocityX": 1.3402750827177572e-25, + "velocityY": -9.971922068875304e-25, "timestamp": 0 }, { - "x": 0.40051040035567387, - "y": 4.115348381940811, - "heading": -0.015277315995300885, - "angularVelocity": -0.30891158633599436, - "velocityX": 0.2731840597647725, - "velocityY": -0.11699249437857151, - "timestamp": 0.04945530265311636 - }, - { - "x": 0.42755206280004615, - "y": 4.103759116150659, - "heading": -0.04541480648334463, - "angularVelocity": -0.6093884552568736, - "velocityX": 0.5467899495842703, - "velocityY": -0.23433818354000846, - "timestamp": 0.09891060530623272 - }, - { - "x": 0.46814907636931, - "y": 4.086345867978605, - "heading": -0.08991129386238127, - "angularVelocity": -0.8997313734209399, - "velocityX": 0.8208829264278229, - "velocityY": -0.3521007301116199, - "timestamp": 0.14836590795934906 - }, - { - "x": 0.5223311730580029, - "y": 4.063083084813687, - "heading": -0.14811250055404074, - "angularVelocity": -1.1768446166407593, - "velocityX": 1.095577092485525, - "velocityY": -0.4703799575969681, - "timestamp": 0.19782121061246544 - }, - { - "x": 0.5901369333153729, - "y": 4.033939247984461, - "heading": -0.2191152852440613, - "angularVelocity": -1.4356960908325656, - "velocityX": 1.3710513659771775, - "velocityY": -0.5892965014013483, - "timestamp": 0.2472765132655818 - }, - { - "x": 0.6716171763309451, - "y": 3.9988769782710625, - "heading": -0.3016414776381226, - "angularVelocity": -1.6687026055205227, - "velocityX": 1.6475532176414214, - "velocityY": -0.7089688634468179, - "timestamp": 0.2967318159186982 - }, - { - "x": 0.766836765570499, - "y": 3.957853748642466, - "heading": -0.39387111054068624, - "angularVelocity": -1.864908876394308, - "velocityX": 1.9253666266574487, - "velocityY": -0.8295011339094811, - "timestamp": 0.34618711857181456 - }, - { - "x": 0.8758711660110932, - "y": 3.910821247404655, - "heading": -0.49317423957562295, - "angularVelocity": -2.007936939168226, - "velocityX": 2.2047059585373607, - "velocityY": -0.9510102802868479, - "timestamp": 0.39564242122493093 - }, - { - "x": 0.998787530743033, - "y": 3.857723681334368, - "heading": -0.5954683580210677, - "angularVelocity": -2.0684155784657574, - "velocityX": 2.4854031446149563, - "velocityY": -1.0736475811849282, - "timestamp": 0.4450977238780473 - }, - { - "x": 1.1355416361282653, - "y": 3.7985302060221593, - "heading": -0.6929936795382847, - "angularVelocity": -1.9719891757870285, - "velocityX": 2.7652061164085326, - "velocityY": -1.1969085646365696, - "timestamp": 0.4945530265311637 - }, - { - "x": 1.284369196894048, - "y": 3.7333344256591525, - "heading": -0.760822351237353, - "angularVelocity": -1.3715146417124207, - "velocityX": 3.009334748382224, - "velocityY": -1.3182768452615752, - "timestamp": 0.54400832918428 + "x": 1.3104780876502728, + "y": 5.567275466796967, + "heading": -0.013632783425449295, + "angularVelocity": -0.19128878469698626, + "velocityX": 0.2876973543738418, + "velocityY": -0.3322502676668159, + "timestamp": 0.07126807484842616 + }, + { + "x": 1.3514855787219004, + "y": 5.519918104138971, + "heading": -0.04088178026950747, + "angularVelocity": -0.38234506687618114, + "velocityX": 0.5753977662346411, + "velocityY": -0.6644961682873589, + "timestamp": 0.14253614969685233 + }, + { + "x": 1.4129976098576382, + "y": 5.448882530124809, + "heading": -0.08170955132000574, + "angularVelocity": -0.5728760196950918, + "velocityX": 0.8631077977981357, + "velocityY": -0.996737657994004, + "timestamp": 0.21380422454527848 + }, + { + "x": 1.4950153477239438, + "y": 5.354169209270334, + "heading": -0.1360632338085868, + "angularVelocity": -0.7626652270905468, + "velocityX": 1.1508341994748963, + "velocityY": -1.3289726298333888, + "timestamp": 0.28507229939370465 + }, + { + "x": 1.5975404069407602, + "y": 5.235778918310868, + "heading": -0.20388243920444382, + "angularVelocity": -0.951607091114721, + "velocityX": 1.4385832567368764, + "velocityY": -1.6611967028891954, + "timestamp": 0.35634037424213083 + }, + { + "x": 1.7205747481569098, + "y": 5.093712881376462, + "heading": -0.2851089622983034, + "angularVelocity": -1.1397322471052174, + "velocityX": 1.7263598248980438, + "velocityY": -1.9934035995297235, + "timestamp": 0.427608449090557 + }, + { + "x": 1.8641205194488986, + "y": 4.927972835971714, + "heading": -0.3796964281011733, + "angularVelocity": -1.3272066911311928, + "velocityX": 2.0141665338552186, + "velocityY": -2.325586116325524, + "timestamp": 0.4988765239389832 + }, + { + "x": 2.0360561465973834, + "y": 4.766214064350323, + "heading": -0.4690836216896802, + "angularVelocity": -1.2542389250532822, + "velocityX": 2.412519596104705, + "velocityY": -2.26972276107388, + "timestamp": 0.5701445987874093 + }, + { + "x": 2.187477556502562, + "y": 4.628126287278847, + "heading": -0.5451441122583921, + "angularVelocity": -1.0672449161911324, + "velocityX": 2.1246737789286905, + "velocityY": -1.9375825341874753, + "timestamp": 0.6414126736358354 + }, + { + "x": 2.3183837856397878, + "y": 4.513708351773291, + "heading": -0.6078811753474637, + "angularVelocity": -0.8802968681629402, + "velocityX": 1.8368144420294628, + "velocityY": -1.6054584854284524, + "timestamp": 0.7126807484842616 + }, + { + "x": 2.428774184877553, + "y": 4.4229594060006505, + "heading": -0.6572940712821052, + "angularVelocity": -0.6933384413670975, + "velocityX": 1.5489459968231887, + "velocityY": -1.2733463891882395, + "timestamp": 0.7839488233326877 + }, + { + "x": 2.5186483273099793, + "y": 4.355878850846579, + "heading": -0.6933805665691075, + "angularVelocity": -0.5063486752483727, + "velocityX": 1.261071561475065, + "velocityY": -0.94124269943785, + "timestamp": 0.8552168981811138 + }, + { + "x": 2.588005969972471, + "y": 4.312466305335481, + "heading": -0.7161380105196834, + "angularVelocity": -0.31932171591524877, + "velocityX": 0.9731937169623598, + "velocityY": -0.6091443553572609, + "timestamp": 0.9264849730295399 + }, + { + "x": 2.6368470386589604, + "y": 4.292721584024785, + "heading": -0.7255637248082996, + "angularVelocity": -0.1322571755819541, + "velocityX": 0.6853148312251345, + "velocityY": -0.2770486133193386, + "timestamp": 0.997753047877966 }, { - "x": 1.444068666194049, - "y": 3.662157825629303, - "heading": -0.7899998870158123, - "angularVelocity": -0.589977903544803, - "velocityX": 3.229167768320948, - "velocityY": -1.4392106854362665, - "timestamp": 0.5934636318373964 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.054843767289835074, + "velocityX": 0.3974372063685045, + "velocityY": 0.05504713907518853, + "timestamp": 1.0690211227263922 + }, + { + "x": 2.6726523875585464, + "y": 4.325520941215549, + "heading": -0.7036434191881636, + "angularVelocity": 0.24661858694943486, + "velocityX": 0.10242764131932201, + "velocityY": 0.39537758612425, + "timestamp": 1.1420557472887627 + }, + { + "x": 2.658587549090021, + "y": 4.379253724773331, + "heading": -0.6716319783828552, + "angularVelocity": 0.4383049957074976, + "velocityX": -0.19257767877637322, + "velocityY": 0.7357165711435307, + "timestamp": 1.2150903718511332 + }, + { + "x": 2.6229776230328583, + "y": 4.457843935727211, + "heading": -0.625626931450262, + "angularVelocity": 0.629907351591922, + "velocityX": -0.4875759445679413, + "velocityY": 1.076067843502979, + "timestamp": 1.2881249964135038 + }, + { + "x": 2.565823384293886, + "y": 4.561292786214385, + "heading": -0.5656322001624833, + "angularVelocity": 0.8214560100400625, + "velocityX": -0.7825636002299563, + "velocityY": 1.4164357126095721, + "timestamp": 1.3611596209758743 + }, + { + "x": 2.4871259686686438, + "y": 4.6896018233859875, + "heading": -0.4916449942707574, + "angularVelocity": 1.01304287295325, + "velocityX": -1.0775357044251765, + "velocityY": 1.7568247655196478, + "timestamp": 1.4341942455382448 + }, + { + "x": 2.3868870212616646, + "y": 4.8427728778807975, + "heading": -0.4036470135354823, + "angularVelocity": 1.2048803052328438, + "velocityX": -1.3724852836256678, + "velocityY": 2.097238883784559, + "timestamp": 1.5072288701006153 + }, + { + "x": 2.2651088471232064, + "y": 5.020807849871808, + "heading": -0.30158987603954596, + "angularVelocity": 1.3973801892933821, + "velocityX": -1.6674033017649226, + "velocityY": 2.43767901947618, + "timestamp": 1.5802634946629859 + }, + { + "x": 2.1359147758136054, + "y": 5.170005063650777, + "heading": -0.2156034423971957, + "angularVelocity": 1.1773379292025947, + "velocityX": -1.7689427731537262, + "velocityY": 2.0428285169256424, + "timestamp": 1.6532981192253564 + }, + { + "x": 2.0282555085514997, + "y": 5.294336258780011, + "heading": -0.14382060600259022, + "angularVelocity": 0.9828603463731643, + "velocityX": -1.4740853110043226, + "velocityY": 1.7023596119544246, + "timestamp": 1.726332743787727 + }, + { + "x": 1.9421294204612731, + "y": 5.39380126769391, + "heading": -0.08631841339161836, + "angularVelocity": 0.7873278319089028, + "velocityX": -1.1792500968725612, + "velocityY": 1.3618884126522468, + "timestamp": 1.7993673683500975 + }, + { + "x": 1.8775353609204413, + "y": 5.468400102933885, + "heading": -0.04315875828225115, + "angularVelocity": 0.5909478602509897, + "velocityX": -0.8844306372201511, + "velocityY": 1.021417385068757, + "timestamp": 1.872401992912468 + }, + { + "x": 1.8344726769226216, + "y": 5.518132795390121, + "heading": -0.014381433416099416, + "angularVelocity": 0.3940230409697844, + "velocityX": -0.5896201186198287, + "velocityY": 0.6809467804378745, + "timestamp": 1.9454366174748385 }, { - "x": 1.6133635999437081, - "y": 3.589517549083241, - "heading": -0.7899999435088091, - "angularVelocity": -0.0000011423041352586315, - "velocityX": 3.42319073319818, - "velocityY": -1.4688066324367013, - "timestamp": 0.6429189344905127 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.6699672496632016e-24, + "angularVelocity": 0.19691253980251358, + "velocityX": -0.29481199459862734, + "velocityY": 0.3404751148788101, + "timestamp": 2.018471242037209 }, { - "x": 1.7826586961746216, - "y": 3.5168776512145996, - "heading": -0.79, - "angularVelocity": -0.000001142267622119268, - "velocityX": 3.4231940186144136, - "velocityY": -1.4687989754737478, - "timestamp": 0.6923742371436291 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 7.316181849831016e-25, + "angularVelocity": -2.1571909437539165e-24, + "velocityX": -1.0558547549960102e-21, + "velocityY": -3.684849193710572e-24, + "timestamp": 2.0915058665995794 + }, + { + "x": 1.8720264440667018, + "y": 5.543082773688388, + "heading": -5.729619384838823e-18, + "angularVelocity": -5.86988662374941e-17, + "velocityX": 0.6053171317615014, + "velocityY": 0.0008555041864670528, + "timestamp": 2.1891162717670882 + }, + { + "x": 1.9901969439319853, + "y": 5.54324978590735, + "heading": 1.1892133440280875e-18, + "angularVelocity": 7.088212283450037e-17, + "velocityX": 1.2106342521833764, + "velocityY": 0.001711008356907634, + "timestamp": 2.286726676934597 + }, + { + "x": 2.1674526905937914, + "y": 5.543500304231363, + "heading": 2.3439205825147678e-18, + "angularVelocity": 1.1829755615161062e-17, + "velocityX": 1.8159513461461236, + "velocityY": 0.0025665124899531154, + "timestamp": 2.384337082102106 + }, + { + "x": 2.403793671138691, + "y": 5.5438343286421725, + "heading": -2.263663737810736e-18, + "angularVelocity": -4.7203823324743326e-17, + "velocityX": 2.4212683078132478, + "velocityY": 0.00342201643602312, + "timestamp": 2.481947487269615 + }, + { + "x": 2.5810494178004975, + "y": 5.544084846966185, + "heading": -1.1285361572651988e-18, + "angularVelocity": 1.1629165750714285e-17, + "velocityX": 1.8159513461461234, + "velocityY": 0.002566512489953115, + "timestamp": 2.5795578924371236 + }, + { + "x": 2.699219917665781, + "y": 5.544251859185147, + "heading": 5.747781298910251e-18, + "angularVelocity": 7.04465619676184e-17, + "velocityX": 1.2106342521833764, + "velocityY": 0.001711008356907634, + "timestamp": 2.6771682976046325 }, { - "x": 2.023972333671043, - "y": 3.4134441767348855, - "heading": -0.79, - "angularVelocity": -1.7456316689050396e-17, - "velocityX": 3.4237473592821543, - "velocityY": -1.4675096226857507, - "timestamp": 0.7628565514540566 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 7.720128293472343e-28, + "angularVelocity": -5.888492408296726e-17, + "velocityX": 0.6053171317615014, + "velocityY": 0.000855504186467053, + "timestamp": 2.7747787027721413 }, { - "x": 2.250495583067251, - "y": 3.3163502598603043, - "heading": -0.79, - "angularVelocity": -2.7198862157697797e-16, - "velocityX": 3.2139019782824447, - "velocityY": -1.37756425600538, - "timestamp": 0.8333388657644841 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 6.467452452708822e-28, + "angularVelocity": 1.6941291064933935e-28, + "velocityX": 1.0543917599744842e-21, + "velocityY": 1.5169407123402652e-24, + "timestamp": 2.87238910793965 + }, + { + "x": 2.7303741241412878, + "y": 5.568056862351408, + "heading": 0.006778500636213837, + "angularVelocity": 0.08813486156991056, + "velocityX": -0.36316271539799655, + "velocityY": 0.3084296913821901, + "timestamp": 2.949299657471029 + }, + { + "x": 2.6747561237908846, + "y": 5.615779928168, + "heading": 0.02059737759786194, + "angularVelocity": 0.1796746616146605, + "velocityX": -0.7231517742271669, + "velocityY": 0.6205009079686918, + "timestamp": 3.026210207002408 + }, + { + "x": 2.591861580665438, + "y": 5.687962474226551, + "heading": 0.041894032741262306, + "angularVelocity": 0.27690161197862034, + "velocityX": -1.0778045876739721, + "velocityY": 0.938525943428621, + "timestamp": 3.103120756533787 + }, + { + "x": 2.482522842737762, + "y": 5.785486601053792, + "heading": 0.07152934014357436, + "angularVelocity": 0.38532174822416326, + "velocityX": -1.4216351149989686, + "velocityY": 1.2680201535610198, + "timestamp": 3.1800313060651657 + }, + { + "x": 2.3492929926954265, + "y": 5.910724214608568, + "heading": 0.11194451008519192, + "angularVelocity": 0.525482787314222, + "velocityX": -1.7322701612992522, + "velocityY": 1.6283541636076846, + "timestamp": 3.2569418555965446 + }, + { + "x": 2.232288592300856, + "y": 6.068262989450489, + "heading": 0.17809331854346394, + "angularVelocity": 0.8600745783422566, + "velocityX": -1.5213049589098766, + "velocityY": 2.0483376572110616, + "timestamp": 3.3338524051279235 + }, + { + "x": 2.1483046945309274, + "y": 6.20982532534887, + "heading": 0.24392330820994265, + "angularVelocity": 0.8559292589584283, + "velocityX": -1.091968504732421, + "velocityY": 1.8406101212503223, + "timestamp": 3.4107629546593023 + }, + { + "x": 2.095435520919896, + "y": 6.331973650947482, + "heading": 0.3066258894331499, + "angularVelocity": 0.8152663269897057, + "velocityX": -0.6874112060460749, + "velocityY": 1.588186878690508, + "timestamp": 3.4876735041906812 + }, + { + "x": 2.072969838348887, + "y": 6.433621733055283, + "heading": 0.3652444017006222, + "angularVelocity": 0.7621647826551591, + "velocityX": -0.29210144392276055, + "velocityY": 1.3216403045765417, + "timestamp": 3.56458405372206 + }, + { + "x": 2.080539684915293, + "y": 6.514239840456833, + "heading": 0.4192892074806503, + "angularVelocity": 0.7026969136136278, + "velocityX": 0.0984240343168718, + "velocityY": 1.0482061029697753, + "timestamp": 3.641494603253439 + }, + { + "x": 2.1179206171311256, + "y": 6.5735146979492365, + "heading": 0.46845941042093314, + "angularVelocity": 0.6393167548519689, + "velocityX": 0.48603127195940116, + "velocityY": 0.7706986603732366, + "timestamp": 3.718405152784818 }, { - "x": 2.4487034476982825, - "y": 3.2313930734184306, - "heading": -0.79, - "angularVelocity": -2.2318862582403906e-16, - "velocityX": 2.8121645347520525, - "velocityY": -1.2053688542021173, - "timestamp": 0.9038211800749116 + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.5732764808952251, + "velocityX": 0.8716741807939861, + "velocityY": 0.49050144055915196, + "timestamp": 3.7953157023161967 }, { - "x": 2.6185959113515174, - "y": 3.1585726243584236, - "heading": -0.79, - "angularVelocity": -1.6717248939922967e-16, - "velocityX": 2.410426861197717, - "velocityY": -1.0331733538044932, - "timestamp": 0.9743034943853391 + "x": 2.2917885277794023, + "y": 6.672276624724165, + "heading": 0.5461832199677006, + "angularVelocity": 0.41150844936023123, + "velocityX": 1.30706335645214, + "velocityY": 0.7468102485181378, + "timestamp": 3.8770462243018065 }, { - "x": 2.760172968622745, - "y": 3.0978889149966715, - "heading": -0.79, - "angularVelocity": -1.1384117019167077e-16, - "velocityX": 2.0086891109686777, - "velocityY": -0.8609778205420586, - "timestamp": 1.0447858086957666 + "x": 2.434116611335813, + "y": 6.754285363860807, + "heading": 0.5638407865548148, + "angularVelocity": 0.2160461741602869, + "velocityX": 1.74143123154738, + "velocityY": 1.0034040789691918, + "timestamp": 3.958776746287416 }, { - "x": 2.8734346168098592, - "y": 3.049341946491368, - "heading": -0.79, - "angularVelocity": -1.4459082642914294e-16, - "velocityX": 1.6069513224022822, - "velocityY": -0.6887822708472163, - "timestamp": 1.1152681230061943 + "x": 2.5412609256604743, + "y": 6.8169454314474, + "heading": 0.5395666812512137, + "angularVelocity": -0.2970017162973107, + "velocityX": 1.3109461645616995, + "velocityY": 0.7666666756101815, + "timestamp": 4.040507268273026 }, { - "x": 2.958380854291598, - "y": 3.0129317195374297, - "heading": -0.79, - "angularVelocity": -1.2018162111442988e-16, - "velocityX": 1.2052135108334716, - "velocityY": -0.5165867112929291, - "timestamp": 1.1857504373166219 + "x": 2.6126555907805695, + "y": 6.858741825974231, + "heading": 0.5217952234534814, + "angularVelocity": -0.21743967083510435, + "velocityX": 0.8735373687282384, + "velocityY": 0.5113927271159627, + "timestamp": 4.122237790258636 }, { - "x": 3.015011679987118, - "y": 2.9886582345981343, - "heading": -0.79, - "angularVelocity": -6.358045799066146e-17, - "velocityX": 0.8034756839297176, - "velocityY": -0.3443911451656786, - "timestamp": 1.2562327516270495 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.11311323638810435, + "velocityX": 0.4366658269950187, + "velocityY": 0.25575567457534326, + "timestamp": 4.203968312244245 }, { - "x": 3.0433270931243896, - "y": 2.9765214920043945, - "heading": -0.79, - "angularVelocity": -1.168085544952961e-16, - "velocityX": 0.40173784607243274, - "velocityY": -0.17219557434345445, - "timestamp": 1.3267150659374771 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 1.6798438100344207e-27, + "velocityX": 4.181549744276816e-25, + "velocityY": 2.1523905294069243e-25, + "timestamp": 4.285698834229855 + }, + { + "x": 2.661250521617956, + "y": 6.863588942839657, + "heading": 0.5202450670683688, + "angularVelocity": 0.13324461766431578, + "velocityX": 0.22348726055621218, + "velocityY": -0.27803300742854703, + "timestamp": 4.343447114072227 + }, + { + "x": 2.6870669895732706, + "y": 6.83147467268314, + "heading": 0.535409558228526, + "angularVelocity": 0.2625964132879752, + "velocityX": 0.447051722160081, + "velocityY": -0.5561078225043913, + "timestamp": 4.401195393914598 + }, + { + "x": 2.725799371823277, + "y": 6.783299196063953, + "heading": 0.5577628193465138, + "angularVelocity": 0.38708098629089316, + "velocityX": 0.6707105797043527, + "velocityY": -0.8342322360196058, + "timestamp": 4.45894367375697 + }, + { + "x": 2.7774544304477864, + "y": 6.719058990822734, + "heading": 0.5869404585342344, + "angularVelocity": 0.5052555550981419, + "velocityX": 0.8944865330275743, + "velocityY": -1.112417641123979, + "timestamp": 4.5166919535993415 + }, + { + "x": 2.8420407207473644, + "y": 6.638749608593502, + "heading": 0.6224502537914576, + "angularVelocity": 0.6149065453403929, + "velocityX": 1.118410634496313, + "velocityY": -1.3906800765051686, + "timestamp": 4.574440233441713 + }, + { + "x": 2.9195693565379317, + "y": 6.542365376975582, + "heading": 0.6635926087059805, + "angularVelocity": 0.7124429511463173, + "velocityX": 1.342527188726434, + "velocityY": -1.6690407382004828, + "timestamp": 4.632188513284085 + }, + { + "x": 3.0100552630481547, + "y": 6.429899310890966, + "heading": 0.70930037739609, + "angularVelocity": 0.7915000899571744, + "velocityX": 1.5669021961729532, + "velocityY": -1.947522357230365, + "timestamp": 4.689936793126456 + }, + { + "x": 3.1135190276304736, + "y": 6.301344715989459, + "heading": 0.7577535982781642, + "angularVelocity": 0.8390418037442976, + "velocityX": 1.7916337051896702, + "velocityY": -2.2261198991970987, + "timestamp": 4.747685072968828 + }, + { + "x": 3.2299855403363114, + "y": 6.1567104859066015, + "heading": 0.8051333898440217, + "angularVelocity": 0.8204537294475949, + "velocityX": 2.016796223605989, + "velocityY": -2.5045634342294933, + "timestamp": 4.8054333528112 + }, + { + "x": 3.3593492107693024, + "y": 5.9962947499990635, + "heading": 0.8376127381025854, + "angularVelocity": 0.5624297095466465, + "velocityX": 2.240130282427429, + "velocityY": -2.777844402385742, + "timestamp": 4.863181632653571 + }, + { + "x": 3.4940407564436287, + "y": 5.828570179937074, + "heading": 0.8376127456087376, + "angularVelocity": 1.2998053164779303e-7, + "velocityX": 2.332390610455871, + "velocityY": -2.9044080710248896, + "timestamp": 4.920929912495943 + }, + { + "x": 3.6287322938415287, + "y": 5.660845603228664, + "heading": 0.837612753114844, + "angularVelocity": 1.2997973902929816e-7, + "velocityX": 2.3323904671368516, + "velocityY": -2.9044081861178794, + "timestamp": 4.9786781923383145 + }, + { + "x": 3.763423831239301, + "y": 5.49312102652015, + "heading": 0.8376127606209502, + "angularVelocity": 1.2997973824676034e-7, + "velocityX": 2.3323904671346476, + "velocityY": -2.9044081861196496, + "timestamp": 5.036426472180686 + }, + { + "x": 3.8981153686370735, + "y": 5.325396449811636, + "heading": 0.8376127681270565, + "angularVelocity": 1.2997973875283622e-7, + "velocityX": 2.3323904671346543, + "velocityY": -2.904408186119644, + "timestamp": 5.094174752023058 + }, + { + "x": 4.03280690604441, + "y": 5.157671873110804, + "heading": 0.8376127756331628, + "angularVelocity": 1.2997973760288775e-7, + "velocityX": 2.3323904673002676, + "velocityY": -2.9044081859866484, + "timestamp": 5.151923031865429 }, { - "x": 3.0433270931243896, - "y": 2.9765214920043945, - "heading": -0.79, - "angularVelocity": 1.4132283846579156e-28, - "velocityX": -7.2110997365083e-27, - "velocityY": -2.0111921052200978e-27, - "timestamp": 1.3971973802479047 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.837612783139272, + "angularVelocity": 1.2997978965886135e-7, + "velocityX": 2.3324012372734173, + "velocityY": -2.9043995371065314, + "timestamp": 5.209671311707801 + }, + { + "x": 4.272973770448513, + "y": 4.872239726327977, + "heading": 0.8376127902271764, + "angularVelocity": 1.6705024403847005e-7, + "velocityX": 2.4858652687195653, + "velocityY": -2.7741855431639553, + "timestamp": 5.252101087117018 + }, + { + "x": 4.386494636503838, + "y": 4.762271022457296, + "heading": 0.8376127978646637, + "angularVelocity": 1.80003009789001e-7, + "velocityX": 2.67550004591034, + "velocityY": -2.5917814273132675, + "timestamp": 5.294530862526235 + }, + { + "x": 4.507495275149132, + "y": 4.6605904045479685, + "heading": 0.8376128062844473, + "angularVelocity": 1.984404462541964e-7, + "velocityX": 2.85178597997034, + "velocityY": -2.3964448769445874, + "timestamp": 5.336960637935452 + }, + { + "x": 4.635371955120012, + "y": 4.567705222787466, + "heading": 0.8376128158092191, + "angularVelocity": 2.2448320171896186e-7, + "velocityX": 3.013842961400671, + "velocityY": -2.189150917360859, + "timestamp": 5.3793904133446695 + }, + { + "x": 4.769486631120928, + "y": 4.484078938092311, + "heading": 0.8376128269135948, + "angularVelocity": 2.6171186455813295e-7, + "velocityX": 3.16086226494103, + "velocityY": -1.9709339464707416, + "timestamp": 5.421820188753887 + }, + { + "x": 4.909170130626689, + "y": 4.4101288120215045, + "heading": 0.8376128403409839, + "angularVelocity": 3.1646147045577477e-7, + "velocityX": 3.2921102729998752, + "velocityY": -1.7428828071228117, + "timestamp": 5.464249964163104 + }, + { + "x": 5.05372549373233, + "y": 4.346223825602605, + "heading": 0.8376128573444204, + "angularVelocity": 4.007430216220621e-7, + "velocityX": 3.406932082752448, + "velocityY": -1.5061353920110019, + "timestamp": 5.506679739572321 + }, + { + "x": 5.2024314508880005, + "y": 4.292682838626271, + "heading": 0.8376128802411438, + "angularVelocity": 5.396381015902925e-7, + "velocityX": 3.5047547558634515, + "velocityY": -1.261872976228496, + "timestamp": 5.549109514981538 + }, + { + "x": 5.354546021326616, + "y": 4.249772998903826, + "heading": 0.8376129138984257, + "angularVelocity": 7.932467614948602e-7, + "velocityX": 3.58509016301729, + "velocityY": -1.0113143260504642, + "timestamp": 5.591539290390755 + }, + { + "x": 5.509310213426411, + "y": 4.217708409612342, + "heading": 0.8376129707519607, + "angularVelocity": 0.0000013399442828266796, + "velocityX": 3.647537386355726, + "velocityY": -0.7557096162361191, + "timestamp": 5.633969065799972 }, { - "x": 3.065434104608151, - "y": 2.9635744703134126, - "heading": -0.7762472311002773, - "angularVelocity": 0.2130317791406229, - "velocityX": 0.34243984045734815, - "velocityY": -0.2005506735052959, - "timestamp": 1.4617547385688177 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.837613299622043, + "angularVelocity": 0.000007750926774500913, + "velocityX": 3.691782807795942, + "velocityY": -0.49633388003567147, + "timestamp": 5.676398841209189 + }, + { + "x": 5.886145215418506, + "y": 4.18891028787692, + "heading": 0.8376134089297179, + "angularVelocity": 0.0000018480104187891978, + "velocityX": 3.7227016115303835, + "velocityY": -0.13083581208893053, + "timestamp": 5.735547682447417 + }, + { + "x": 6.106032301025365, + "y": 4.202865338919715, + "heading": 0.8376134508974878, + "angularVelocity": 7.095281843173525e-7, + "velocityX": 3.7175214425797836, + "velocityY": 0.23593109773003054, + "timestamp": 5.794696523685645 + }, + { + "x": 6.3234808347180325, + "y": 4.238378912467147, + "heading": 0.8376134741732665, + "angularVelocity": 3.9351199795834263e-7, + "velocityX": 3.676294059876367, + "velocityY": 0.6004103005906283, + "timestamp": 5.853845364923873 + }, + { + "x": 6.536382325934404, + "y": 4.2951066501716255, + "heading": 0.8376134896107453, + "angularVelocity": 2.6099376502854354e-7, + "velocityX": 3.599419477363716, + "velocityY": 0.9590676083746476, + "timestamp": 5.912994206162101 + }, + { + "x": 6.742672378442076, + "y": 4.372498490127808, + "heading": 0.8376135010511353, + "angularVelocity": 1.9341697541887733e-7, + "velocityX": 3.4876431759130977, + "velocityY": 1.3084252934808847, + "timestamp": 5.972143047400329 + }, + { + "x": 6.940350706341849, + "y": 4.46980400008761, + "heading": 0.8376135102232026, + "angularVelocity": 1.550675727090784e-7, + "velocityX": 3.3420490369980995, + "velocityY": 1.64509579431818, + "timestamp": 6.031291888638557 + }, + { + "x": 7.12750053081492, + "y": 4.586079652731035, + "heading": 0.837613518040013, + "angularVelocity": 1.3215492103145434e-7, + "velocityX": 3.164048873236707, + "velocityY": 1.9658145486758316, + "timestamp": 6.090440729876785 + }, + { + "x": 7.302307170565869, + "y": 4.720197970950524, + "heading": 0.837613525049849, + "angularVelocity": 1.185118059136636e-7, + "velocityX": 2.955368796607497, + "velocityY": 2.2674716091108755, + "timestamp": 6.149589571115013 + }, + { + "x": 7.4630756537468415, + "y": 4.8708584478992005, + "heading": 0.8376135339238266, + "angularVelocity": 1.5002791962179725e-7, + "velocityX": 2.718032675119729, + "velocityY": 2.5471416479974107, + "timestamp": 6.208738412353241 + }, + { + "x": 7.604661415508242, + "y": 5.027831186839046, + "heading": 0.8715939059006107, + "angularVelocity": 0.5744892252398454, + "velocityX": 2.3937199579472788, + "velocityY": 2.6538599176883673, + "timestamp": 6.267887253591469 + }, + { + "x": 7.730952710188879, + "y": 5.1694158341932015, + "heading": 0.9053997225906723, + "angularVelocity": 0.5715381059437028, + "velocityX": 2.1351440203534295, + "velocityY": 2.3937011172189955, + "timestamp": 6.327036094829697 + }, + { + "x": 7.842513418943255, + "y": 5.295119262651075, + "heading": 0.934282004580144, + "angularVelocity": 0.48829835690517676, + "velocityX": 1.886101340600306, + "velocityY": 2.125205258909303, + "timestamp": 6.386184936067925 + }, + { + "x": 7.939539266004037, + "y": 5.404796625949478, + "heading": 0.9566775785409475, + "angularVelocity": 0.3786308149402807, + "velocityX": 1.6403676729692935, + "velocityY": 1.8542605569679314, + "timestamp": 6.4453337773061525 + }, + { + "x": 8.0221293723114, + "y": 5.498379145291686, + "heading": 0.9718066727965543, + "angularVelocity": 0.25578006160210215, + "velocityX": 1.3963097937071225, + "velocityY": 1.5821530461652626, + "timestamp": 6.5044826185443805 + }, + { + "x": 8.090343615854179, + "y": 5.575826687480091, + "heading": 0.9792023087722939, + "angularVelocity": 0.12503433407854694, + "velocityX": 1.153264241780091, + "velocityY": 1.3093670233788228, + "timestamp": 6.563631459782608 + }, + { + "x": 8.14422212483701, + "y": 5.637112925793437, + "heading": 0.9785539274051055, + "angularVelocity": -0.010961860851625928, + "velocityX": 0.9108971174232933, + "velocityY": 1.0361359078280012, + "timestamp": 6.622780301020836 + }, + { + "x": 8.183793718837835, + "y": 5.682219218209322, + "heading": 0.9696403414250478, + "angularVelocity": -0.15069755879337374, + "velocityX": 0.6690172313173101, + "velocityY": 0.7625896208890375, + "timestamp": 6.681929142259064 + }, + { + "x": 8.209080155266193, + "y": 5.71113162509295, + "heading": 0.9522961336408167, + "angularVelocity": -0.29322988280320866, + "velocityX": 0.4275051868981633, + "velocityY": 0.4888076634871, + "timestamp": 6.741077983497292 + }, + { + "x": 8.220098495483398, + "y": 5.723839282989502, + "heading": 0.9263929393086, + "angularVelocity": -0.4379324056051909, + "velocityX": 0.18628159041742642, + "velocityY": 0.21484204306506113, + "timestamp": 6.80022682473552 + }, + { + "x": 8.216352100309892, + "y": 5.7197578257583315, + "heading": 0.8905883238088871, + "angularVelocity": -0.5885448940291729, + "velocityX": -0.06158205358748059, + "velocityY": -0.06708969723814164, + "timestamp": 6.8610626519815865 + }, + { + "x": 8.197510123572037, + "y": 5.69852730877679, + "heading": 0.8459980645635137, + "angularVelocity": -0.7329605146161121, + "velocityX": -0.3097184272952048, + "velocityY": -0.3489804929530894, + "timestamp": 6.921898479227653 + }, + { + "x": 8.163552039603486, + "y": 5.660151329305612, + "heading": 0.7930933731491355, + "angularVelocity": -0.8696305090155458, + "velocityX": -0.5581921953851212, + "velocityY": -0.6308121580389989, + "timestamp": 6.982734306473719 + }, + { + "x": 8.114451794656734, + "y": 5.604635385719368, + "heading": 0.7324785026638988, + "angularVelocity": -0.9963679829660959, + "velocityX": -0.8070942266989081, + "velocityY": -0.9125534425906069, + "timestamp": 7.043570133719785 + }, + { + "x": 8.05017522631974, + "y": 5.531988399816786, + "heading": 0.664956367976081, + "angularVelocity": -1.1099073974075735, + "velocityX": -1.0565578088880443, + "velocityY": -1.1941480734492433, + "timestamp": 7.104405960965852 + }, + { + "x": 7.970675640275654, + "y": 5.442225859030612, + "heading": 0.5916510965236437, + "angularVelocity": -1.2049687621725864, + "velocityX": -1.3067889374221797, + "velocityY": -1.4754881268090099, + "timestamp": 7.165241788211918 + }, + { + "x": 7.87588580389167, + "y": 5.335377165777019, + "heading": 0.5142662873089652, + "angularVelocity": -1.2720269077903064, + "velocityX": -1.558125214613782, + "velocityY": -1.7563448725931903, + "timestamp": 7.226077615457984 + }, + { + "x": 7.765703024679293, + "y": 5.211506902101234, + "heading": 0.4357243406909052, + "angularVelocity": -1.2910475647906803, + "velocityX": -1.8111495183046493, + "velocityY": -2.0361400392364293, + "timestamp": 7.2869134427040505 + }, + { + "x": 7.639967535619313, + "y": 5.070804612535801, + "heading": 0.36221787896188584, + "angularVelocity": -1.2082758640184799, + "velocityX": -2.0668000214973703, + "velocityY": -2.3128195330742662, + "timestamp": 7.347749269950117 + }, + { + "x": 7.498615166036793, + "y": 4.914564832190124, + "heading": 0.31493629004406687, + "angularVelocity": -0.7771997367041067, + "velocityX": -2.32350534185693, + "velocityY": -2.5682198700730225, + "timestamp": 7.408585097196183 + }, + { + "x": 7.337231161361771, + "y": 4.755477157818062, + "heading": 0.3149362776036475, + "angularVelocity": -2.0449166092827028e-7, + "velocityX": -2.652778995217139, + "velocityY": -2.615032647268723, + "timestamp": 7.469420924442249 + }, + { + "x": 7.160583323022439, + "y": 4.613529185957489, + "heading": 0.3149362685790419, + "angularVelocity": -1.4834359997406843e-7, + "velocityX": -2.903681043488322, + "velocityY": -2.33329566287359, + "timestamp": 7.530256751688316 + }, + { + "x": 6.970483558560949, + "y": 4.490176988250006, + "heading": 0.31493625863244995, + "angularVelocity": -1.6349891854082722e-7, + "velocityX": -3.124799531246346, + "velocityY": -2.0276242354452334, + "timestamp": 7.591092578934382 + }, + { + "x": 6.76888179398853, + "y": 4.386685862868273, + "heading": 0.31493624706316514, + "angularVelocity": -1.9017222732229534e-7, + "velocityX": -3.313865754746604, + "velocityY": -1.701154238655088, + "timestamp": 7.651928406180448 + }, + { + "x": 6.557845947797341, + "y": 4.304117375120236, + "heading": 0.31493623270578724, + "angularVelocity": -2.3600201712655827e-7, + "velocityX": -3.4689401910752493, + "velocityY": -1.3572345686049037, + "timestamp": 7.7127642334265145 + }, + { + "x": 6.339540712904722, + "y": 4.243318473621985, + "heading": 0.3149362133315886, + "angularVelocity": -3.184669214162522e-7, + "velocityX": -3.58843209297751, + "velocityY": -0.9993930262891738, + "timestamp": 7.773600060672581 + }, + { + "x": 6.116205355143182, + "y": 4.204912805417524, + "heading": 0.3149361681436048, + "angularVelocity": -7.427857215236805e-7, + "velocityX": -3.671115654566562, + "velocityY": -0.6313001720042296, + "timestamp": 7.834435887918647 + }, + { + "x": 5.89050677740902, + "y": 4.189334421603551, + "heading": 0.31368110305887587, + "angularVelocity": -0.02063036111356669, + "velocityX": -3.7099615136532513, + "velocityY": -0.25607252369499406, + "timestamp": 7.895271715164713 }, { - "x": 3.109663922209824, - "y": 2.9376745027807694, - "heading": -0.7492642309726533, - "angularVelocity": 0.41796939697396296, - "velocityX": 0.6851243413927804, - "velocityY": -0.40119311270289487, - "timestamp": 1.5263120968897306 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.30754704224604024, + "angularVelocity": -0.10082974277681633, + "velocityX": -3.691164544865137, + "velocityY": 0.12023594125393282, + "timestamp": 7.95610754241078 + }, + { + "x": 5.419243898269495, + "y": 4.233180640603145, + "heading": 0.29443096525634566, + "angularVelocity": -0.19257128900213308, + "velocityX": -3.6221840549970006, + "velocityY": 0.5363593678805259, + "timestamp": 8.024217783857775 + }, + { + "x": 5.182010137227644, + "y": 4.296746845588596, + "heading": 0.2703885847974574, + "angularVelocity": -0.35299214843626897, + "velocityX": -3.483085010445408, + "velocityY": 0.9332840940656272, + "timestamp": 8.092328025304772 + }, + { + "x": 4.96090074800009, + "y": 4.383753869716349, + "heading": 0.22896738213665188, + "angularVelocity": -0.6081494028036839, + "velocityX": -3.2463456967719417, + "velocityY": 1.2774440712482642, + "timestamp": 8.160438266751768 + }, + { + "x": 4.76451355722713, + "y": 4.485181561673583, + "heading": 0.17582209664433623, + "angularVelocity": -0.7802833224967138, + "velocityX": -2.8833724062744586, + "velocityY": 1.489169467064151, + "timestamp": 8.228548508198765 + }, + { + "x": 4.596735729440321, + "y": 4.5894452853276455, + "heading": 0.11854658262999372, + "angularVelocity": -0.8409236672419458, + "velocityX": -2.4633274559359006, + "velocityY": 1.5308083107472321, + "timestamp": 8.296658749645761 + }, + { + "x": 4.457111536094513, + "y": 4.687809707372902, + "heading": 0.06296288370262228, + "angularVelocity": -0.8160843031312246, + "velocityX": -2.049973548463538, + "velocityY": 1.444194293773058, + "timestamp": 8.364768991092758 + }, + { + "x": 4.343947453919847, + "y": 4.774964113624853, + "heading": 0.012793626993843597, + "angularVelocity": -0.7365890304150604, + "velocityX": -1.6614840847793941, + "velocityY": 1.2796079473565642, + "timestamp": 8.432879232539754 + }, + { + "x": 4.255603402479856, + "y": 4.847699861550099, + "heading": -0.029623031457827412, + "angularVelocity": -0.622764764160759, + "velocityX": -1.2970744129389131, + "velocityY": 1.0679120552207846, + "timestamp": 8.50098947398675 + }, + { + "x": 4.190737568538254, + "y": 4.903969414198745, + "heading": -0.06275873479049468, + "angularVelocity": -0.4865010404999608, + "velocityX": -0.9523653501078979, + "velocityY": 0.8261540621968694, + "timestamp": 8.569099715433747 + }, + { + "x": 4.148285991242422, + "y": 4.94238614886978, + "heading": -0.0855582962836916, + "angularVelocity": -0.3347449812072629, + "velocityX": -0.6232774454171954, + "velocityY": 0.5640375640267039, + "timestamp": 8.637209956880744 }, { - "x": 3.176036274163731, - "y": 2.8988141772022082, - "heading": -0.7097339632553362, - "angularVelocity": 0.612327839079368, - "velocityX": 1.028114434670201, - "velocityY": -0.6019503676929793, - "timestamp": 1.5908694552106435 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.17170074221519835, + "velocityX": -0.3066229399994914, + "velocityY": 0.28741048233214816, + "timestamp": 8.70532019832774 }, { - "x": 3.2645764628811422, - "y": 2.8469841623353034, - "heading": -0.6585853448584378, - "angularVelocity": 0.7922972644363798, - "velocityX": 1.3714964648534713, - "velocityY": -0.8028521645706009, - "timestamp": 1.6554268135315564 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -9.317687373474837e-25, + "velocityX": -1.6510663048617945e-23, + "velocityY": -3.5986344010888627e-23, + "timestamp": 8.773430439774737 + }, + { + "x": 4.147821870498822, + "y": 4.951212055722382, + "heading": -0.09513615443770512, + "angularVelocity": 0.0346947715699437, + "velocityX": 0.3347010455989641, + "velocityY": -0.1761961456823413, + "timestamp": 8.83444023344422 + }, + { + "x": 4.188661953327048, + "y": 4.929712670962105, + "heading": -0.09090262623774811, + "angularVelocity": 0.06939096078396689, + "velocityX": 0.6694020807458456, + "velocityY": -0.3523923532137878, + "timestamp": 8.895450027113702 + }, + { + "x": 4.2499220745882305, + "y": 4.89746358708528, + "heading": -0.08455181431110974, + "angularVelocity": 0.10409495827905127, + "velocityX": 1.004103072255173, + "velocityY": -0.5285886402359044, + "timestamp": 8.956459820783184 + }, + { + "x": 4.331602229572044, + "y": 4.854464798258643, + "heading": -0.07608283990612706, + "angularVelocity": 0.13881335922659968, + "velocityX": 1.3388039865584789, + "velocityY": -0.7047850228699344, + "timestamp": 9.017469614452667 + }, + { + "x": 4.433702411475643, + "y": 4.800716297797124, + "heading": -0.06549440517861618, + "angularVelocity": 0.17355303289293356, + "velocityX": 1.6735047893575954, + "velocityY": -0.8809815150777099, + "timestamp": 9.07847940812215 + }, + { + "x": 4.556222611314502, + "y": 4.736218078349401, + "heading": -0.05278477279271126, + "angularVelocity": 0.20832118290316787, + "velocityX": 2.0082054448930773, + "velocityY": -1.0571781277795957, + "timestamp": 9.139489201791632 + }, + { + "x": 4.699162817715129, + "y": 4.660970132179323, + "heading": -0.03795174297125847, + "angularVelocity": 0.24312538904507433, + "velocityX": 2.3429059140077895, + "velocityY": -1.2333748672832447, + "timestamp": 9.200498995461114 + }, + { + "x": 4.862523016289189, + "y": 4.574972451706886, + "heading": -0.02099263124198345, + "angularVelocity": 0.27797359586478865, + "velocityX": 2.677606147286025, + "velocityY": -1.4095717310293654, + "timestamp": 9.261508789130596 + }, + { + "x": 5.046303186466893, + "y": 4.478225031445458, + "heading": -0.0019042656986956292, + "angularVelocity": 0.3128737927995283, + "velocityX": 3.0123060434087328, + "velocityY": -1.5857686847058003, + "timestamp": 9.322518582800079 + }, + { + "x": 5.247403827752691, + "y": 4.372365027625972, + "heading": -0.0019042643151741538, + "angularVelocity": 2.2677039080484546e-8, + "velocityX": 3.2962026125714896, + "velocityY": -1.735131319948009, + "timestamp": 9.383528376469561 + }, + { + "x": 5.4522102780182164, + "y": 4.273865655817337, + "heading": -0.0019042629287868294, + "angularVelocity": 2.2724012670003775e-8, + "velocityX": 3.356943827331282, + "velocityY": -1.6144845914780435, + "timestamp": 9.444538170139044 }, { - "x": 3.3753176768637694, - "y": 2.782172830859228, - "heading": -0.5971442411061407, - "angularVelocity": 0.9517289020234527, - "velocityX": 1.7153925882799501, - "velocityY": -1.0039340698220625, - "timestamp": 1.7199841718524693 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.001904261510303375, + "angularVelocity": 2.3250094273072788e-8, + "velocityX": 3.503395732831278, + "velocityY": -1.265642393108385, + "timestamp": 9.505547963808526 + }, + { + "x": 5.815067432185188, + "y": 4.154195032102827, + "heading": -0.0019042601141906403, + "angularVelocity": 3.3542788088424975e-8, + "velocityX": 3.5826307679474456, + "velocityY": -1.0199942412452387, + "timestamp": 9.547169809045405 + }, + { + "x": 5.966765079106725, + "y": 4.122169062842087, + "heading": -0.0019042587680997514, + "angularVelocity": 3.234097098196836e-8, + "velocityX": 3.6446641435091407, + "velocityY": -0.7694509716826321, + "timestamp": 9.588791654282284 + }, + { + "x": 6.1203162156456985, + "y": 4.100724080023554, + "heading": -0.0019042574433880786, + "angularVelocity": 3.182731724746444e-8, + "velocityX": 3.6891957976654504, + "velocityY": -0.5152338320534735, + "timestamp": 9.630413499519163 + }, + { + "x": 6.274166659063129, + "y": 4.081543884935332, + "heading": -0.001904256120802589, + "angularVelocity": 3.177623389629938e-8, + "velocityX": 3.6963868983183503, + "velocityY": -0.46082039321089135, + "timestamp": 9.672035344756042 + }, + { + "x": 6.428017114313601, + "y": 4.062363784763932, + "heading": -0.0019042547982171636, + "angularVelocity": 3.1776232361520234e-8, + "velocityX": 3.696387182617153, + "velocityY": -0.4608181127540811, + "timestamp": 9.71365718999292 + }, + { + "x": 6.581867569564624, + "y": 4.043183684596952, + "heading": -0.0019042534756317344, + "angularVelocity": 3.177623245008594e-8, + "velocityX": 3.696387182630397, + "velocityY": -0.46081811264784767, + "timestamp": 9.7552790352298 + }, + { + "x": 6.7357180248600645, + "y": 4.024003584786261, + "heading": -0.0019042521530462726, + "angularVelocity": 3.1776233230368237e-8, + "velocityX": 3.6963871836975604, + "velocityY": -0.46081810408775026, + "timestamp": 9.796900880466678 + }, + { + "x": 6.88956959654859, + "y": 4.00483244208887, + "heading": -0.0019042508304574486, + "angularVelocity": 3.1776314012648856e-8, + "velocityX": 3.696414005984728, + "velocityY": -0.4606029018724078, + "timestamp": 9.838522725703557 + }, + { + "x": 7.044365422308842, + "y": 3.996109423647668, + "heading": -0.001904249277450269, + "angularVelocity": 3.7312309699136395e-8, + "velocityX": 3.7191005079010213, + "velocityY": -0.20957788852360654, + "timestamp": 9.880144570940436 }, { - "x": 3.5083041287580397, - "y": 2.7043667248342107, - "heading": -0.5274363257086371, - "angularVelocity": 1.0797826492680733, - "velocityX": 2.0599735700645976, - "velocityY": -1.2052244399196936, - "timestamp": 1.7845415301733822 + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -4.70962468044917e-24, + "angularVelocity": 0.04575119787728785, + "velocityX": 3.664528906453967, + "velocityY": 0.04245896540136021, + "timestamp": 9.921766416177315 + }, + { + "x": 7.4117376833527, + "y": 4.005713687749523, + "heading": 0.0009657064358366508, + "angularVelocity": 0.014669397578232305, + "velocityX": 3.2636086584468615, + "velocityY": 0.11904726359911169, + "timestamp": 9.987597779100469 + }, + { + "x": 7.599748212147598, + "y": 4.014970056100235, + "heading": 0.001284059261189157, + "angularVelocity": 0.004835883858642321, + "velocityX": 2.8559416127289494, + "velocityY": 0.14060727197030365, + "timestamp": 10.053429142023623 + }, + { + "x": 7.760889475419079, + "y": 4.024789101797907, + "heading": 0.0012842515922686665, + "angularVelocity": 0.0000029215721955193207, + "velocityX": 2.447788654468283, + "velocityY": 0.14915452546736505, + "timestamp": 10.119260504946777 + }, + { + "x": 7.8951561065625, + "y": 4.03478916193552, + "heading": 0.0011129009866924844, + "angularVelocity": -0.002602871913440434, + "velocityX": 2.0395541757224747, + "velocityY": 0.15190419419518753, + "timestamp": 10.18509186786993 + }, + { + "x": 8.002547492527386, + "y": 4.044754390856472, + "heading": 0.0008529249221134859, + "angularVelocity": -0.003949121710915689, + "velocityX": 1.6313103845388242, + "velocityY": 0.1513750965870757, + "timestamp": 10.250923230793084 + }, + { + "x": 8.083064163245998, + "y": 4.054546008914513, + "heading": 0.0005576380815616831, + "angularVelocity": -0.004485503982296319, + "velocityX": 1.223074643200086, + "velocityY": 0.1487378906232164, + "timestamp": 10.316754593716238 + }, + { + "x": 8.136706915836411, + "y": 4.064067288015535, + "heading": 0.00026420208220015364, + "angularVelocity": -0.004457389097413355, + "velocityX": 0.8148510103464127, + "velocityY": 0.14463135317639367, + "timestamp": 10.382585956639392 }, { - "x": 3.663593064192348, - "y": 2.6135556042232486, - "heading": -0.45293092753819847, - "angularVelocity": 1.1540961419157545, - "velocityX": 2.405441292414274, - "velocityY": -1.4066734292246417, - "timestamp": 1.8490988884942952 + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 7.663456254811148e-24, + "angularVelocity": -0.004013316305004348, + "velocityX": 0.40663971509805397, + "velocityY": 0.13944216629333933, + "timestamp": 10.448417319562546 + }, + { + "x": 8.158068779301736, + "y": 4.083431553094837, + "heading": -0.0002400975033088844, + "angularVelocity": -0.0031214685775050673, + "velocityX": -0.07030570953345122, + "velocityY": 0.1324082915016235, + "timestamp": 10.52533544234589 + }, + { + "x": 8.115975249648498, + "y": 4.093075117537315, + "heading": -0.0004115957026821067, + "angularVelocity": -0.002229620187901403, + "velocityX": -0.5472511305534997, + "velocityY": 0.12537441234287078, + "timestamp": 10.602253565129233 + }, + { + "x": 8.037195973910658, + "y": 4.10217764875176, + "heading": -0.0005144945686699745, + "angularVelocity": -0.0013377714154270807, + "velocityX": -1.024196546758428, + "velocityY": 0.11834052736940634, + "timestamp": 10.679171687912577 + }, + { + "x": 7.921730952606701, + "y": 4.110739146113785, + "heading": -0.000548794095929675, + "angularVelocity": -0.0004459225734917171, + "velocityX": -1.501141956222563, + "velocityY": 0.11130663427836081, + "timestamp": 10.75608981069592 + }, + { + "x": 7.769580186514303, + "y": 4.11875960869081, + "heading": -0.0005144943077910091, + "angularVelocity": 0.00044592596513670077, + "velocityX": -1.9780873555763094, + "velocityY": 0.10427272906302729, + "timestamp": 10.833007933479264 + }, + { + "x": 7.580743676929442, + "y": 4.126239034937878, + "heading": -0.00041159526627641076, + "angularVelocity": 0.001337773697421547, + "velocityX": -2.455032738081228, + "velocityY": 0.09723880376198106, + "timestamp": 10.909926056262607 + }, + { + "x": 7.35522142644369, + "y": 4.133177421791385, + "heading": -0.00024009710111365826, + "angularVelocity": 0.0022296197431366516, + "velocityX": -2.9319780868935523, + "velocityY": 0.09020483863145769, + "timestamp": 10.986844179045951 + }, + { + "x": 7.093013442829981, + "y": 4.139574760181112, + "heading": -1.4530480095899495e-10, + "angularVelocity": 0.003121461459546291, + "velocityX": -3.4089233346512358, + "velocityY": 0.08317075558053526, + "timestamp": 11.063762301829295 + }, + { + "x": 6.806524506833513, + "y": 4.13534985058908, + "heading": -1.168340521484347e-10, + "angularVelocity": 3.7014357319604335e-10, + "velocityX": -3.7245960461544843, + "velocityY": -0.05492736222818408, + "timestamp": 11.140680424612638 + }, + { + "x": 6.520035572895023, + "y": 4.131124801447752, + "heading": -8.836340568265332e-11, + "angularVelocity": 3.701422426282247e-10, + "velocityX": -3.7245960193990517, + "velocityY": -0.054929176485862014, + "timestamp": 11.217598547395982 + }, + { + "x": 6.233546635992582, + "y": 4.126899953288603, + "heading": -5.989279962924638e-11, + "angularVelocity": 3.7014171723351935e-10, + "velocityX": -3.724596057932901, + "velocityY": -0.05492656354927405, + "timestamp": 11.294516670179325 + }, + { + "x": 5.947511708921411, + "y": 4.143566708826361, + "heading": -3.09908077767978e-11, + "angularVelocity": 3.7575009382191746e-10, + "velocityX": -3.7186935499823517, + "velocityY": 0.21668177712428321, + "timestamp": 11.371434792962669 }, { - "x": 3.8412236037881105, - "y": 2.50976866496977, - "heading": -0.3811103115245424, - "angularVelocity": 1.1125085951726474, - "velocityX": 2.751514997140456, - "velocityY": -1.6076701704173844, - "timestamp": 1.913656246815208 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -1.0560376042139496e-24, + "angularVelocity": 4.029064498114124e-10, + "velocityX": -3.6605154924761147, + "velocityY": 0.6901152005178285, + "timestamp": 11.448352915746012 + }, + { + "x": 5.429536630692184, + "y": 4.268976584999924, + "heading": -2.8551532586117325e-9, + "angularVelocity": -4.3018194916474803e-8, + "velocityX": -3.5620332259990137, + "velocityY": 1.0897484864542637, + "timestamp": 11.514723744744192 + }, + { + "x": 5.202544196069625, + "y": 4.366944958856273, + "heading": -5.536673846717203e-9, + "angularVelocity": -4.040209574870605e-8, + "velocityX": -3.4200632725076114, + "velocityY": 1.4760757901492323, + "timestamp": 11.581094573742371 + }, + { + "x": 4.987571660053834, + "y": 4.485916904335734, + "heading": -0.004584961527914818, + "angularVelocity": -0.0690808908137442, + "velocityX": -3.2389611409205945, + "velocityY": 1.7925336669024117, + "timestamp": 11.64746540274055 + }, + { + "x": 4.796407595296518, + "y": 4.591709425707398, + "heading": -0.01882928955824306, + "angularVelocity": -0.21461729867377147, + "velocityX": -2.8802422335655495, + "velocityY": 1.5939611267258074, + "timestamp": 11.71383623173873 + }, + { + "x": 4.629141679517556, + "y": 4.68427797767877, + "heading": -0.035106225739037614, + "angularVelocity": -0.24524232146084635, + "velocityX": -2.5201721645445794, + "velocityY": 1.394717428855822, + "timestamp": 11.78020706073691 + }, + { + "x": 4.485777671005229, + "y": 4.763620051213405, + "heading": -0.05103365270171862, + "angularVelocity": -0.23997631494278487, + "velocityX": -2.160045469919592, + "velocityY": 1.195435927684602, + "timestamp": 11.846577889735089 + }, + { + "x": 4.366313170637034, + "y": 4.829736321202412, + "heading": -0.06545153313706903, + "angularVelocity": -0.21723218849271467, + "velocityX": -1.799954922537895, + "velocityY": 0.9961645950033218, + "timestamp": 11.912948718733269 + }, + { + "x": 4.270745458949128, + "y": 4.88262778520947, + "heading": -0.0776743935119962, + "angularVelocity": -0.1841601281680891, + "velocityX": -1.4399053489376459, + "velocityY": 0.7969082924745271, + "timestamp": 11.979319547731448 + }, + { + "x": 4.199072211550319, + "y": 4.922295352851354, + "heading": -0.08724968344217902, + "angularVelocity": -0.1442695544822162, + "velocityX": -1.0798907966145075, + "velocityY": 0.5976656950144733, + "timestamp": 12.045690376729628 + }, + { + "x": 4.151291522892566, + "y": 4.948739793023477, + "heading": -0.0938560173878007, + "angularVelocity": -0.0995367098066956, + "velocityX": -0.7199049549172076, + "velocityY": 0.39843468239409807, + "timestamp": 12.112061205727807 }, { - "x": 4.040074211349249, - "y": 2.394026373110605, - "heading": -0.3455558271366132, - "angularVelocity": 0.5507425537951661, - "velocityX": 3.0802159929261363, - "velocityY": -1.7928597896433947, - "timestamp": 1.978213605136121 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.051179983074939046, + "velocityX": -0.3599426809532897, + "velocityY": 0.19921331994672406, + "timestamp": 12.178432034725986 }, { - "x": 4.2474603616320605, - "y": 2.2722898239651945, - "heading": -0.3455557994031658, - "angularVelocity": 4.2959390119270946e-7, - "velocityX": 3.2124324116842065, - "velocityY": -1.8857114403637907, - "timestamp": 2.042770963457034 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.064087865436599e-25, + "velocityX": -5.747994530578863e-24, + "velocityY": -1.0853274710472143e-23, + "timestamp": 12.244802863724166 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 4.454846495549661, - "y": 2.150553246940426, - "heading": -0.34555577166993406, - "angularVelocity": 4.2959055996150303e-7, - "velocityX": 3.21243215818542, - "velocityY": -1.8857118722178698, - "timestamp": 2.1073283217779473 + "timestamp": 1.0690211227263922, + "isStopPoint": false, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 4.662232629466809, - "y": 2.0288166699148875, - "heading": -0.34555574393670246, - "angularVelocity": 4.295905587004531e-7, - "velocityX": 3.2124321581784225, - "velocityY": -1.885711872229791, - "timestamp": 2.1718856800988604 + "timestamp": 2.0915058665995794, + "isStopPoint": true, + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 4.86961876338396, - "y": 1.9070800928893532, - "heading": -0.34555571620347075, - "angularVelocity": 4.295905595925436e-7, - "velocityX": 3.2124321581784563, - "velocityY": -1.8857118722297332, - "timestamp": 2.2364430384197735 + "timestamp": 2.87238910793965, + "isStopPoint": true, + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 5.0770048973810855, - "y": 1.7853435160000617, - "heading": -0.3455556884702391, - "angularVelocity": 4.2959055900802054e-7, - "velocityX": 3.2124321594172818, - "velocityY": -1.8857118701193136, - "timestamp": 2.3010003967406867 + "timestamp": 3.7953157023161967, + "isStopPoint": false, + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { - "x": 5.284393928450512, - "y": 1.6636118745954684, - "heading": -0.3455556607369369, - "angularVelocity": 4.295916530112558e-7, - "velocityX": 3.212477035359784, - "velocityY": -1.885635418962908, - "timestamp": 2.3655577550616 + "timestamp": 4.285698834229855, + "isStopPoint": true, + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, - "angularVelocity": 4.37946601168607e-7, - "velocityX": 3.386285847111441, - "velocityY": -1.5519979873442957, - "timestamp": 2.430115113382513 + "timestamp": 5.209671311707801, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 }, { - "x": 5.67114193645051, - "y": 1.502092704427615, - "heading": -0.3455556386074096, - "angularVelocity": -1.278584041845331e-7, - "velocityX": 3.49949220171166, - "velocityY": -1.2763944237939575, - "timestamp": 2.478161609702393 + "timestamp": 5.676398841209189, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 19 }, { - "x": 5.843643430464205, - "y": 1.4543994299893364, - "heading": -0.3455556446255623, - "angularVelocity": -1.252568492070864e-7, - "velocityX": 3.590303294234579, - "velocityY": -0.9926483321645406, - "timestamp": 2.526208106022273 + "timestamp": 6.80022682473552, + "isStopPoint": false, + "x": 8.220098495483398, + "y": 5.723839282989502, + "heading": 0.9263929393086, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 6.017301477098486, - "y": 1.4111062813327742, - "heading": -0.3455556506300644, - "angularVelocity": -1.2497273485529655e-7, - "velocityX": 3.6143748230487924, - "velocityY": -0.9010677567065171, - "timestamp": 2.5742546023421533 + "timestamp": 7.95610754241078, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 6.190959602710417, - "y": 1.3678134494736272, - "heading": -0.3455556566345664, - "angularVelocity": -1.249727336927334e-7, - "velocityX": 3.6143764668242215, - "velocityY": -0.9010611631473742, - "timestamp": 2.6223010986620334 + "timestamp": 8.773430439774737, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 6.364617728327444, - "y": 1.32452061763492, - "heading": -0.34555566263906834, - "angularVelocity": -1.2497273274023636e-7, - "velocityX": 3.6143764669302776, - "velocityY": -0.9010611627219542, - "timestamp": 2.6703475949819135 + "timestamp": 9.505547963808526, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 }, { - "x": 6.53827585394447, - "y": 1.2812277857962142, - "heading": -0.3455556686435703, - "angularVelocity": -1.2497273262525817e-7, - "velocityX": 3.6143764669302847, - "velocityY": -0.9010611627219266, - "timestamp": 2.7183940913017937 + "timestamp": 9.921766416177315, + "isStopPoint": false, + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 6.711933979561497, - "y": 1.2379349539575075, - "heading": -0.3455556746480723, - "angularVelocity": -1.249727344246102e-7, - "velocityX": 3.614376466930281, - "velocityY": -0.9010611627219414, - "timestamp": 2.7664405876216738 + "timestamp": 10.448417319562546, + "isStopPoint": false, + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 }, { - "x": 6.885592105175778, - "y": 1.1946421221077876, - "heading": -0.3455556806525743, - "angularVelocity": -1.2497273402138388e-7, - "velocityX": 3.6143764668731366, - "velocityY": -0.9010611629511635, - "timestamp": 2.814487083941554 + "timestamp": 11.448352915746012, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 7.059250188235952, - "y": 1.1513491195635046, - "heading": -0.34555568665707626, - "angularVelocity": -1.2497273336829244e-7, - "velocityX": 3.614375581187171, - "velocityY": -0.901064715646495, - "timestamp": 2.862533580261434 + "timestamp": 12.244802863724166, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 7.232252247020901, - "y": 1.105504804591487, - "heading": -0.3455556930363148, - "angularVelocity": -1.3277218955785758e-7, - "velocityX": 3.600721635000194, - "velocityY": -0.954165620460628, - "timestamp": 2.910580076581314 + "scope": [ + "last" + ], + "type": "StopPoint" }, { - "x": 7.3915823056650956, - "y": 1.0554297346464303, - "heading": -0.34668228326838396, - "angularVelocity": -0.02344791646343344, - "velocityX": 3.3161639421825697, - "velocityY": -1.0422210521173303, - "timestamp": 2.9586265729011942 + "scope": [ + 3 + ], + "type": "StopPoint" }, { - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, - "angularVelocity": 0.023449177160012234, - "velocityX": 3.0262995808418314, - "velocityY": -0.9742709781335815, - "timestamp": 3.0066730692210744 + "scope": [ + 5 + ], + "type": "StopPoint" }, { - "x": 7.706721170015621, - "y": 0.9519776263380658, - "heading": -0.3399354575219549, - "angularVelocity": 0.0873776196257354, - "velocityX": 2.6389050045733367, - "velocityY": -0.8806177429462266, - "timestamp": 3.0709936007986016 + "scope": [ + 2 + ], + "type": "StopPoint" }, { - "x": 7.851727537144501, - "y": 0.9021680166584882, - "heading": -0.3343317435174853, - "angularVelocity": 0.0871216991998166, - "velocityX": 2.254433593324705, - "velocityY": -0.7743967355049113, - "timestamp": 3.135314132376129 + "scope": [ + 4 + ], + "type": "WptVelocityDirection", + "direction": 0.5125504196 }, { - "x": 7.9720996028837305, - "y": 0.8595135021783036, - "heading": -0.3304150780634631, - "angularVelocity": 0.06089292731204182, - "velocityX": 1.8714407792812162, - "velocityY": -0.6631555031346718, - "timestamp": 3.199634663953656 + "scope": [ + 10 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "CenterLanePCBAFG": { + "waypoints": [ + { + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 8.067893377913451, - "y": 0.8241876261341342, - "heading": -0.3290917427033041, - "angularVelocity": 0.020574073747570358, - "velocityX": 1.4893187708540474, - "velocityY": -0.5492161706031644, - "timestamp": 3.2639551955311834 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 8.139145592381213, - "y": 0.7962986910106811, - "heading": -0.33093102223192583, - "angularVelocity": -0.028595527485726017, - "velocityX": 1.107767810996388, - "velocityY": -0.4335930447004768, - "timestamp": 3.3282757271087107 + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 8.185882143295247, - "y": 0.7759207035915239, - "heading": -0.33632389468863655, - "angularVelocity": -0.08384371715290594, - "velocityX": 0.726619475426766, - "velocityY": -0.3168193253284159, - "timestamp": 3.392596258686238 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 8.208122253417969, - "y": 0.763107419013977, - "heading": -0.34555563246426124, - "angularVelocity": -0.14352707524653271, - "velocityX": 0.34576999874318054, - "velocityY": -0.19920986756931033, - "timestamp": 3.456916790263765 + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { - "x": 8.204294589663224, - "y": 0.758059174501978, - "heading": -0.3598356424553756, - "angularVelocity": -0.21015280737857786, - "velocityX": -0.056330092504229386, - "velocityY": -0.07429285814157252, - "timestamp": 3.5248673932999157 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 8.173143956099947, - "y": 0.7614990375664682, - "heading": -0.3786432717750789, - "angularVelocity": -0.2767838470792884, - "velocityX": -0.4584305682571252, - "velocityY": 0.05062299539358275, - "timestamp": 3.5928179963360662 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 }, { - "x": 8.114670323431806, - "y": 0.7734269104252286, - "heading": -0.4019794984677993, - "angularVelocity": -0.34342928024207936, - "velocityX": -0.8605314751516402, - "velocityY": 0.17553740990958688, - "timestamp": 3.6607685993722168 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 }, { - "x": 8.028873660690506, - "y": 0.7938426763627194, - "heading": -0.42984618674257646, - "angularVelocity": -0.41010214817301655, - "velocityX": -1.2626328377933065, - "velocityY": 0.3004501067728478, - "timestamp": 3.7287192024083673 + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 7.915753937180706, - "y": 0.8227462003684911, - "heading": -0.46224640751329055, - "angularVelocity": -0.47682020943179554, - "velocityX": -1.6647346521651787, - "velocityY": 0.4253608167449919, - "timestamp": 3.796669805444518 + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 }, { - "x": 7.7753111250087, - "y": 0.8601373303576515, - "heading": -0.4991848061620223, - "angularVelocity": -0.5436066347943985, - "velocityX": -2.0668368770368284, - "velocityY": 0.5502692885487409, - "timestamp": 3.8646204084806683 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 7.607545202545143, - "y": 0.9060158995198092, - "heading": -0.5406680081845634, - "angularVelocity": -0.6104905647485055, - "velocityX": -2.468939420218282, - "velocityY": 0.6751753054752103, - "timestamp": 3.932571011516819 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 7.412456160128696, - "y": 0.9603817316039739, - "heading": -0.5867050376005867, - "angularVelocity": -0.6775072973455604, - "velocityX": -2.8710421055815774, - "velocityY": 0.800078728591143, - "timestamp": 4.000521614552969 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 }, { - "x": 7.190044017003328, - "y": 1.023234660815804, - "heading": -0.6373076187752672, - "angularVelocity": -0.7446965724168686, - "velocityX": -3.273144507739573, - "velocityY": 0.9249797117825663, - "timestamp": 4.06847221758912 + "x": 8.118325233459473, + "y": 2.496157169342041, + "heading": -0.5880028838627427, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 6.9483240964505395, - "y": 1.0983293400926897, - "heading": -0.6373076247970214, - "angularVelocity": -8.861958477756183e-8, - "velocityX": -3.5572888208834765, - "velocityY": 1.1051363184655567, - "timestamp": 4.1364228206252704 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 6.70660438555466, - "y": 1.1734246942257325, - "heading": -0.6373076308180073, - "angularVelocity": -8.86082771208124e-8, - "velocityX": -3.5572857354522953, - "velocityY": 1.1051462500353575, - "timestamp": 4.204373423661421 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -1.120688542766024e-26, + "angularVelocity": 1.2570906532337121e-26, + "velocityX": 3.544719035734486e-27, + "velocityY": -1.5437798405468174e-26, + "timestamp": 0 }, { - "x": 6.464884674663689, - "y": 1.2485200483745762, - "heading": -0.6373076368389932, - "angularVelocity": -8.860827701986505e-8, - "velocityX": -3.557285735380054, - "velocityY": 1.105146250267891, - "timestamp": 4.2723240266975715 + "x": 1.3104780876502795, + "y": 5.567275466796953, + "heading": -0.013632783425389957, + "angularVelocity": -0.19128878469611282, + "velocityX": 0.28769735437387517, + "velocityY": -0.33225026766694243, + "timestamp": 0.07126807484844137 + }, + { + "x": 1.3514855787219209, + "y": 5.519918104138928, + "heading": -0.04088178026931428, + "angularVelocity": -0.3823450668742212, + "velocityX": 0.575397766234711, + "velocityY": -0.6644961682876167, + "timestamp": 0.14253614969688275 + }, + { + "x": 1.4129976098576797, + "y": 5.448882530124723, + "heading": -0.08170955131958318, + "angularVelocity": -0.5728760196917512, + "velocityX": 0.8631077977982459, + "velocityY": -0.9967376579943998, + "timestamp": 0.2138042245453241 + }, + { + "x": 1.4950153477240138, + "y": 5.354169209270188, + "heading": -0.13606323380780802, + "angularVelocity": -0.7626652270853859, + "velocityX": 1.1508341994750528, + "velocityY": -1.3289726298339328, + "timestamp": 0.2850722993937655 + }, + { + "x": 1.5975404069408672, + "y": 5.235778918310646, + "heading": -0.20388243920312946, + "angularVelocity": -0.9516070911070027, + "velocityX": 1.4385832567370889, + "velocityY": -1.6611967028899048, + "timestamp": 0.3563403742422069 + }, + { + "x": 1.7205747481570635, + "y": 5.093712881376144, + "heading": -0.28510896229616783, + "angularVelocity": -1.139732247093452, + "velocityX": 1.7263598248983312, + "velocityY": -1.9934035995306334, + "timestamp": 0.42760844909064827 + }, + { + "x": 1.8641205194491128, + "y": 4.927972835971274, + "heading": -0.37969642809760956, + "angularVelocity": -1.3272066911108702, + "velocityX": 2.0141665338556387, + "velocityY": -2.32558611632674, + "timestamp": 0.49887652393908966 + }, + { + "x": 2.036056146597576, + "y": 4.7662140643499225, + "heading": -0.4690836216877677, + "angularVelocity": -1.2542389250761836, + "velocityX": 2.4125195961038894, + "velocityY": -2.2697227610728588, + "timestamp": 0.570144598787531 + }, + { + "x": 2.187477556502736, + "y": 4.628126287278482, + "heading": -0.5451441122574506, + "angularVelocity": -1.0672449162045279, + "velocityX": 2.1246737789279755, + "velocityY": -1.9375825341865685, + "timestamp": 0.6414126736359723 + }, + { + "x": 2.318383785639942, + "y": 4.513708351772966, + "heading": -0.607881175347142, + "angularVelocity": -0.8802968681714505, + "velocityX": 1.8368144420287889, + "velocityY": -1.6054584854275527, + "timestamp": 0.7126807484844137 + }, + { + "x": 2.428774184877684, + "y": 4.4229594060003725, + "heading": -0.6572940712821624, + "angularVelocity": -0.6933384413722671, + "velocityX": 1.548945996822536, + "velocityY": -1.2733463891873111, + "timestamp": 0.783948823332855 + }, + { + "x": 2.518648327310084, + "y": 4.355878850846356, + "heading": -0.6933805665693583, + "angularVelocity": -0.5063486752509772, + "velocityX": 1.2610715614744235, + "velocityY": -0.9412426994368749, + "timestamp": 0.8552168981812963 + }, + { + "x": 2.5880059699725453, + "y": 4.312466305335323, + "heading": -0.7161380105199752, + "angularVelocity": -0.31932171591575625, + "velocityX": 0.9731937169617235, + "velocityY": -0.6091443553562279, + "timestamp": 0.9264849730297376 + }, + { + "x": 2.636847038659, + "y": 4.292721584024701, + "heading": -0.7255637248085023, + "angularVelocity": -0.13225717558067804, + "velocityX": 0.6853148312244995, + "velocityY": -0.2770486133182406, + "timestamp": 0.997753047878179 }, { - "x": 6.223164963772718, - "y": 1.3236154025234206, - "heading": -0.637307642859979, - "angularVelocity": -8.860827626589147e-8, - "velocityX": -3.5572857353800513, - "velocityY": 1.1051462502679001, - "timestamp": 4.340274629733722 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.054843767292669265, + "velocityX": 0.39743720636786783, + "velocityY": 0.055047139076356964, + "timestamp": 1.0690211227266204 + }, + { + "x": 2.6726523875585073, + "y": 4.325520941215623, + "heading": -0.7036434191878628, + "angularVelocity": 0.24661858695360772, + "velocityX": 0.10242764131881017, + "velocityY": 0.3953775861253462, + "timestamp": 1.1420557472889747 + }, + { + "x": 2.658587549089958, + "y": 4.379253724773467, + "heading": -0.6716319783821333, + "angularVelocity": 0.43830499571335924, + "velocityX": -0.19257767877674353, + "velocityY": 0.7357165711445571, + "timestamp": 1.215090371851329 + }, + { + "x": 2.622977623032788, + "y": 4.4578439357274, + "heading": -0.6256269314489635, + "angularVelocity": 0.6299073515999585, + "velocityX": -0.48757594456814685, + "velocityY": 1.0760678435039392, + "timestamp": 1.2881249964136834 + }, + { + "x": 2.5658233842938287, + "y": 4.561292786214616, + "heading": -0.5656322001603995, + "angularVelocity": 0.8214560100509974, + "velocityX": -0.782563600229962, + "velocityY": 1.4164357126104716, + "timestamp": 1.3611596209760377 + }, + { + "x": 2.4871259686686225, + "y": 4.689601823386252, + "heading": -0.49164499426759223, + "angularVelocity": 1.0130428729682832, + "velocityX": -1.0775357044249236, + "velocityY": 1.7568247655204965, + "timestamp": 1.434194245538392 + }, + { + "x": 2.3868870212617113, + "y": 4.842772877881088, + "heading": -0.40364701353076443, + "angularVelocity": 1.204880305254371, + "velocityX": -1.3724852836250385, + "velocityY": 2.0972388837853764, + "timestamp": 1.5072288701007464 + }, + { + "x": 2.26510884712338, + "y": 5.020807849872121, + "heading": -0.3015898760322786, + "angularVelocity": 1.397380189328601, + "velocityX": -1.6674033017635614, + "velocityY": 2.437679019477028, + "timestamp": 1.5802634946631007 + }, + { + "x": 2.1359147758136907, + "y": 5.1700050636509935, + "heading": -0.2156034423927932, + "angularVelocity": 1.1773379291636314, + "velocityX": -1.7689427731553284, + "velocityY": 2.0428285169247893, + "timestamp": 1.653298119225455 + }, + { + "x": 2.0282555085515424, + "y": 5.294336258780153, + "heading": -0.1438206059999422, + "angularVelocity": 0.9828603463493605, + "velocityX": -1.4740853110052332, + "velocityY": 1.7023596119537827, + "timestamp": 1.7263327437878093 + }, + { + "x": 1.9421294204612929, + "y": 5.393801267693995, + "heading": -0.08631841339014902, + "angularVelocity": 0.7873278318929399, + "velocityX": -1.1792500968731383, + "velocityY": 1.361888412651758, + "timestamp": 1.7993673683501636 + }, + { + "x": 1.8775353609204488, + "y": 5.468400102933928, + "heading": -0.043158758281562314, + "angularVelocity": 0.5909478602404344, + "velocityX": -0.8844306372205144, + "velocityY": 1.0214173850684016, + "timestamp": 1.872401992912518 + }, + { + "x": 1.8344726769226236, + "y": 5.518132795390135, + "heading": -0.014381433415882237, + "angularVelocity": 0.3940230409634143, + "velocityX": -0.5896201186200382, + "velocityY": 0.6809467804376429, + "timestamp": 1.9454366174748723 }, { - "x": 5.98144525288525, - "y": 1.39871075668354, - "heading": -0.6373076488809649, - "angularVelocity": -8.860827719404325e-8, - "velocityX": -3.5572857353285006, - "velocityY": 1.1051462504338314, - "timestamp": 4.4082252327698725 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 3.4376441721489434e-26, + "angularVelocity": 0.1969125397995838, + "velocityX": -0.2948119945987197, + "velocityY": 0.34047511487869625, + "timestamp": 2.0184712420372266 }, { - "x": 5.739725691606158, - "y": 1.473806592406946, - "heading": -0.6373076549019507, - "angularVelocity": -8.860827643379068e-8, - "velocityX": -3.557283533605943, - "velocityY": 1.1051533373950213, - "timestamp": 4.476175835806023 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.5116489541153283e-26, + "angularVelocity": -4.402301586321663e-26, + "velocityX": -5.546242066692014e-22, + "velocityY": -8.311642516359436e-25, + "timestamp": 2.0915058665995807 + }, + { + "x": 1.8720264440667018, + "y": 5.543082773688388, + "heading": 5.813054707764239e-20, + "angularVelocity": 5.955362224136031e-19, + "velocityX": 0.6053171317615015, + "velocityY": 0.0008555041864665229, + "timestamp": 2.1891162717670896 + }, + { + "x": 1.9901969439319853, + "y": 5.543249785907351, + "heading": 8.928720004263094e-20, + "angularVelocity": 3.1919397232381416e-19, + "velocityX": 1.2106342521833766, + "velocityY": 0.0017110083569065741, + "timestamp": 2.2867266769345984 + }, + { + "x": 2.1674526905937914, + "y": 5.543500304231363, + "heading": 9.074204012627455e-20, + "angularVelocity": 1.4904559423917405e-20, + "velocityX": 1.8159513461461239, + "velocityY": 0.0025665124899515255, + "timestamp": 2.3843370821021073 + }, + { + "x": 2.403793671138691, + "y": 5.5438343286421725, + "heading": 4.918611066370257e-20, + "angularVelocity": -4.2573257831162476e-19, + "velocityX": 2.4212683078132486, + "velocityY": 0.0034220164360210005, + "timestamp": 2.481947487269616 + }, + { + "x": 2.5810494178004975, + "y": 5.544084846966184, + "heading": 1.0185426849288302e-20, + "angularVelocity": -3.9955457358178204e-19, + "velocityX": 1.8159513461461239, + "velocityY": 0.0025665124899515255, + "timestamp": 2.579557892437125 + }, + { + "x": 2.699219917665781, + "y": 5.544251859185147, + "heading": 2.0104392985221674e-20, + "angularVelocity": 1.0161791787953617e-19, + "velocityX": 1.2106342521833766, + "velocityY": 0.001711008356906574, + "timestamp": 2.677168297604634 }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.6373076609624853, - "angularVelocity": -8.919029793694062e-8, - "velocityX": -3.483737947413037, - "velocityY": 1.3187872955082738, - "timestamp": 4.544126438842174 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.4565183072234727e-28, + "angularVelocity": -2.0596567646457299e-19, + "velocityX": 0.6053171317615015, + "velocityY": 0.0008555041864665228, + "timestamp": 2.7747787027721427 }, { - "x": 5.300661081332848, - "y": 1.6644259130191759, - "heading": -0.6373076662501911, - "angularVelocity": -8.709485563097327e-8, - "velocityX": -3.332823841916901, - "velocityY": 1.6637052194159954, - "timestamp": 4.6048384732983045 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.5120860158326505e-28, + "angularVelocity": -5.112250671135627e-29, + "velocityX": 5.545899463618144e-22, + "velocityY": 7.793513704496499e-25, + "timestamp": 2.8723891079396515 + }, + { + "x": 2.7303741241412878, + "y": 5.568056862351408, + "heading": 0.006778500636213844, + "angularVelocity": 0.08813486156991067, + "velocityX": -0.36316271539799666, + "velocityY": 0.30842969138218984, + "timestamp": 2.9492996574710304 + }, + { + "x": 2.6747561237908846, + "y": 5.615779928168, + "heading": 0.020597377597861958, + "angularVelocity": 0.17967466161466059, + "velocityX": -0.7231517742271671, + "velocityY": 0.6205009079686912, + "timestamp": 3.0262102070024093 + }, + { + "x": 2.591861580665438, + "y": 5.687962474226551, + "heading": 0.04189403274126234, + "angularVelocity": 0.2769016119786207, + "velocityX": -1.0778045876739724, + "velocityY": 0.9385259434286201, + "timestamp": 3.103120756533788 + }, + { + "x": 2.4825228427377626, + "y": 5.785486601053793, + "heading": 0.07152934014357441, + "angularVelocity": 0.38532174822416343, + "velocityX": -1.4216351149989688, + "velocityY": 1.2680201535610187, + "timestamp": 3.180031306065167 + }, + { + "x": 2.3492929926954265, + "y": 5.910724214608569, + "heading": 0.11194451008519196, + "angularVelocity": 0.5254827873142219, + "velocityX": -1.7322701612992524, + "velocityY": 1.6283541636076828, + "timestamp": 3.256941855596546 + }, + { + "x": 2.2322885923008555, + "y": 6.0682629894504885, + "heading": 0.178093318543464, + "angularVelocity": 0.8600745783422568, + "velocityX": -1.5213049589098766, + "velocityY": 2.04833765721106, + "timestamp": 3.333852405127925 + }, + { + "x": 2.148304694530927, + "y": 6.209825325348869, + "heading": 0.2439233082099427, + "angularVelocity": 0.8559292589584284, + "velocityX": -1.091968504732421, + "velocityY": 1.840610121250321, + "timestamp": 3.4107629546593037 + }, + { + "x": 2.0954355209198954, + "y": 6.331973650947481, + "heading": 0.3066258894331499, + "angularVelocity": 0.8152663269897058, + "velocityX": -0.6874112060460749, + "velocityY": 1.5881868786905071, + "timestamp": 3.4876735041906826 + }, + { + "x": 2.072969838348887, + "y": 6.433621733055282, + "heading": 0.3652444017006222, + "angularVelocity": 0.7621647826551591, + "velocityX": -0.2921014439227605, + "velocityY": 1.321640304576541, + "timestamp": 3.5645840537220614 + }, + { + "x": 2.080539684915293, + "y": 6.514239840456833, + "heading": 0.4192892074806503, + "angularVelocity": 0.7026969136136281, + "velocityX": 0.09842403431687198, + "velocityY": 1.048206102969775, + "timestamp": 3.6414946032534403 + }, + { + "x": 2.1179206171311256, + "y": 6.5735146979492365, + "heading": 0.46845941042093314, + "angularVelocity": 0.6393167548519696, + "velocityX": 0.4860312719594013, + "velocityY": 0.7706986603732364, + "timestamp": 3.718405152784819 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.5732764808952261, + "velocityX": 0.8716741807939862, + "velocityY": 0.490501440559152, + "timestamp": 3.795315702316198 }, { - "x": 5.105140298152244, - "y": 1.7780767678650242, - "heading": -0.6373076714738495, - "angularVelocity": -8.603991577037157e-8, - "velocityX": -3.2204617244688527, - "velocityY": 1.8719658443989302, - "timestamp": 4.6655505077544355 + "x": 2.2917885277794023, + "y": 6.672276624724165, + "heading": 0.5461832199677008, + "angularVelocity": 0.41150844936023273, + "velocityX": 1.3070633564521403, + "velocityY": 0.7468102485181382, + "timestamp": 3.8770462243018082 }, { - "x": 4.909619881391782, - "y": 1.8917282530843815, - "heading": -0.6373076766975064, - "angularVelocity": -8.60398930609077e-8, - "velocityX": -3.220455689089781, - "velocityY": 1.8719762274064293, - "timestamp": 4.726262542210566 + "x": 2.4341166113358135, + "y": 6.754285363860808, + "heading": 0.5638407865548152, + "angularVelocity": 0.21604617416028918, + "velocityX": 1.7414312315473808, + "velocityY": 1.0034040789691927, + "timestamp": 3.9587767462874184 }, { - "x": 4.714099464642575, - "y": 2.0053797383231027, - "heading": -0.6373076819211634, - "angularVelocity": -8.603989205375444e-8, - "velocityX": -3.2204556889043876, - "velocityY": 1.8719762277253713, - "timestamp": 4.786974576666697 + "x": 2.5412609256604743, + "y": 6.8169454314474, + "heading": 0.5395666812512139, + "angularVelocity": -0.29700171629731165, + "velocityX": 1.3109461645616998, + "velocityY": 0.7666666756101825, + "timestamp": 4.0405072682730285 }, { - "x": 4.518579047893369, - "y": 2.1190312235618243, - "heading": -0.6373076871448202, - "angularVelocity": -8.60398922147823e-8, - "velocityX": -3.220455688904382, - "velocityY": 1.8719762277253813, - "timestamp": 4.847686611122828 + "x": 2.6126555907805695, + "y": 6.858741825974232, + "heading": 0.5217952234534815, + "angularVelocity": -0.21743967083510496, + "velocityX": 0.8735373687282387, + "velocityY": 0.5113927271159635, + "timestamp": 4.122237790258638 }, { - "x": 4.323058631144163, - "y": 2.232682708800546, - "heading": -0.6373076923684772, - "angularVelocity": -8.603989163870152e-8, - "velocityX": -3.2204556889043805, - "velocityY": 1.8719762277253815, - "timestamp": 4.908398645578959 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.11311323638810476, + "velocityX": 0.4366658269950189, + "velocityY": 0.2557556745753436, + "timestamp": 4.203968312244248 }, { - "x": 4.12753821439536, - "y": 2.3463341940399602, - "heading": -0.637307697592134, - "angularVelocity": -8.60398917504913e-8, - "velocityX": -3.2204556888977502, - "velocityY": 1.8719762277367886, - "timestamp": 4.96911068003509 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 6.389972915056163e-27, + "velocityX": -1.0690163263683836e-25, + "velocityY": -1.1312672794497157e-25, + "timestamp": 4.285698834229858 + }, + { + "x": 2.660849775777501, + "y": 6.863303095829213, + "heading": 0.49944853016087754, + "angularVelocity": -0.22618164286906187, + "velocityX": 0.21588184235159683, + "velocityY": -0.2821127073294775, + "timestamp": 4.343625239188846 + }, + { + "x": 2.6858699737019993, + "y": 6.830605675150758, + "heading": 0.4737478272074287, + "angularVelocity": -0.4436785430002931, + "velocityX": 0.43193079118602823, + "velocityY": -0.5644648705819807, + "timestamp": 4.401551644147834 + }, + { + "x": 2.7234170479654534, + "y": 6.7815357680679345, + "heading": 0.4360912802839983, + "angularVelocity": -0.6500756770611135, + "velocityX": 0.6481858194037288, + "velocityY": -0.847107758846162, + "timestamp": 4.459478049106822 + }, + { + "x": 2.773506189570542, + "y": 6.716073180513463, + "heading": 0.38731316065986704, + "angularVelocity": -0.8420705489779003, + "velocityX": 0.864703094220184, + "velocityY": -1.1300992630359112, + "timestamp": 4.517404454065811 + }, + { + "x": 2.8361569303832073, + "y": 6.63419375338078, + "heading": 0.3285222984900142, + "angularVelocity": -1.0149233706368752, + "velocityX": 1.0815575531922228, + "velocityY": -1.4135078327518056, + "timestamp": 4.575330859024799 + }, + { + "x": 2.9113941307930915, + "y": 6.535868410078246, + "heading": 0.2612558665356734, + "angularVelocity": -1.1612395418283843, + "velocityX": 1.2988411841396363, + "velocityY": -1.6974183599370378, + "timestamp": 4.633257263983787 + }, + { + "x": 2.99924805742508, + "y": 6.421062364749842, + "heading": 0.187813414307632, + "angularVelocity": -1.2678579359454234, + "velocityX": 1.5166473164386636, + "velocityY": -1.9819294052459604, + "timestamp": 4.6911836689427755 + }, + { + "x": 3.0997480806917785, + "y": 6.289740322129515, + "heading": 0.11213231017946661, + "angularVelocity": -1.306504420250965, + "velocityX": 1.734960478521888, + "velocityY": -2.26704976276885, + "timestamp": 4.749110073901764 + }, + { + "x": 3.2128556613445705, + "y": 6.14193527325681, + "heading": 0.04279587609271538, + "angularVelocity": -1.1969745772388503, + "velocityX": 1.952608326597726, + "velocityY": -2.551600586595203, + "timestamp": 4.807036478860752 + }, + { + "x": 3.336513792781727, + "y": 5.978631812899107, + "heading": 0.010716004519218999, + "angularVelocity": -0.5538039447849203, + "velocityX": 2.134745484804496, + "velocityY": -2.8191540709857907, + "timestamp": 4.86496288381974 + }, + { + "x": 3.4677695577152434, + "y": 5.807368362842113, + "heading": 0.010715992273577517, + "angularVelocity": -2.1139999092239637e-7, + "velocityX": 2.265905592215594, + "velocityY": -2.956569636562943, + "timestamp": 4.922889288778729 + }, + { + "x": 3.5990253485117636, + "y": 5.636104932606414, + "heading": 0.01071598002800544, + "angularVelocity": -2.11398792746924e-7, + "velocityX": 2.265906038695981, + "velocityY": -2.9565692943823, + "timestamp": 4.980815693737717 + }, + { + "x": 3.730281139327178, + "y": 5.464841502385196, + "heading": 0.010715967782433288, + "angularVelocity": -2.113987940232789e-7, + "velocityX": 2.2659060390221635, + "velocityY": -2.956569294132315, + "timestamp": 5.038742098696705 + }, + { + "x": 3.8615381491076497, + "y": 5.293579006382116, + "heading": 0.010715955536865166, + "angularVelocity": -2.1139872447064547e-7, + "velocityX": 2.2659270823625586, + "velocityY": -2.9565531664589524, + "timestamp": 5.096668503655693 + }, + { + "x": 4.007189928421941, + "y": 5.1343784904281335, + "heading": 0.010715943225165272, + "angularVelocity": -2.1254037607347718e-7, + "velocityX": 2.5144280819328126, + "velocityY": -2.748323775084899, + "timestamp": 5.154594908614682 }, { - "x": 3.9320178107531194, - "y": 2.4599857018249596, - "heading": -0.637307702817472, - "angularVelocity": -8.606758041499758e-8, - "velocityX": -3.2204554730169304, - "velocityY": 1.8719765990896107, - "timestamp": 5.029822714491221 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.010715930460413511, + "angularVelocity": -2.203615392689942e-7, + "velocityX": 2.7674622150421295, + "velocityY": -2.4933481486115037, + "timestamp": 5.21252131357367 + }, + { + "x": 4.289285126852667, + "y": 4.894606292568033, + "heading": 0.010715918114469883, + "angularVelocity": -2.9733998228105175e-7, + "velocityX": 2.933098226238877, + "velocityY": -2.2962068965718774, + "timestamp": 5.2540426170077055 + }, + { + "x": 4.417366677446148, + "y": 4.807905868510879, + "heading": 0.010715906356446534, + "angularVelocity": -2.831804971331414e-7, + "velocityX": 3.0847189274046736, + "velocityY": -2.088094950942351, + "timestamp": 5.295563920441741 + }, + { + "x": 4.551131631929394, + "y": 4.730260623000039, + "heading": 0.010715894919665146, + "angularVelocity": -2.7544369861896277e-7, + "velocityX": 3.221598153722609, + "velocityY": -1.8700098284292712, + "timestamp": 5.337085223875777 + }, + { + "x": 4.688895654984909, + "y": 4.659954711675372, + "heading": 0.010715883576398903, + "angularVelocity": -2.7319147770282054e-7, + "velocityX": 3.3179118106053243, + "velocityY": -1.6932491398387937, + "timestamp": 5.378606527309812 + }, + { + "x": 4.826660072114789, + "y": 4.5896495725424655, + "heading": 0.010715872233137535, + "angularVelocity": -2.7319136037670126e-7, + "velocityX": 3.31792130150117, + "velocityY": -1.6932305423551623, + "timestamp": 5.420127830743848 + }, + { + "x": 4.964424489260567, + "y": 4.51934443344071, + "heading": 0.010715860889876156, + "angularVelocity": -2.731913605852805e-7, + "velocityX": 3.3179213018840272, + "velocityY": -1.6932305416049456, + "timestamp": 5.4616491341778834 + }, + { + "x": 5.102188906406363, + "y": 4.449039294338991, + "heading": 0.01071584954661479, + "angularVelocity": -2.731913603225307e-7, + "velocityX": 3.3179213018844735, + "velocityY": -1.6932305416040707, + "timestamp": 5.503170437611919 + }, + { + "x": 5.239953323697088, + "y": 4.378734155521264, + "heading": 0.0107158382033534, + "angularVelocity": -2.7319136082771354e-7, + "velocityX": 3.3179213053749512, + "velocityY": -1.6932305347644059, + "timestamp": 5.544691741045955 + }, + { + "x": 5.377721333912445, + "y": 4.308436057560326, + "heading": 0.01071582686006505, + "angularVelocity": -2.731920101868387e-7, + "velocityX": 3.3180078374520967, + "velocityY": -1.6930609626121105, + "timestamp": 5.58621304447999 + }, + { + "x": 5.519903612293123, + "y": 4.247558747282141, + "heading": 0.01071581542197781, + "angularVelocity": -2.7547514875541596e-7, + "velocityX": 3.4243211706145282, + "velocityY": -1.4661705014848305, + "timestamp": 5.627734347914026 }, { - "x": 3.754279851907353, - "y": 2.5632932295685515, - "heading": -0.6678287625734391, - "angularVelocity": -0.5027184483172089, - "velocityX": -2.9275572864255675, - "velocityY": 1.7015988455837243, - "timestamp": 5.090534748947352 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.010715803649590386, + "angularVelocity": -2.8352644184424e-7, + "velocityX": 3.517426102957061, + "velocityY": -1.2261096959197675, + "timestamp": 5.669255651348061 + }, + { + "x": 5.815511009397966, + "y": 4.155881926930556, + "heading": 0.010715791923418018, + "angularVelocity": -2.8177750483647635e-7, + "velocityX": 3.59387868290116, + "velocityY": -0.9796261538753469, + "timestamp": 5.710870656687205 + }, + { + "x": 5.967533992937965, + "y": 4.125567717701148, + "heading": 0.0107157805290767, + "angularVelocity": -2.738036730972178e-7, + "velocityX": 3.653080957243209, + "velocityY": -0.7284441989700993, + "timestamp": 5.7524856620263485 + }, + { + "x": 6.121241175717782, + "y": 4.105466951776193, + "heading": 0.010715769236027265, + "angularVelocity": -2.713696500137211e-7, + "velocityX": 3.6935519178040117, + "velocityY": -0.4830172617098897, + "timestamp": 5.794100667365492 + }, + { + "x": 6.274949114201427, + "y": 4.0853719654365035, + "heading": 0.010715757943004396, + "angularVelocity": -2.713690116930231e-7, + "velocityX": 3.693570077211222, + "velocityY": -0.4828783794673188, + "timestamp": 5.8357156727046355 + }, + { + "x": 6.428657052715288, + "y": 4.065276979327946, + "heading": 0.010715746649981548, + "angularVelocity": -2.7136901112829927e-7, + "velocityX": 3.6935700779373306, + "velocityY": -0.48287837391326544, + "timestamp": 5.877330678043779 + }, + { + "x": 6.582364991229262, + "y": 4.045181993220238, + "heading": 0.010715735356958701, + "angularVelocity": -2.71369011116775e-7, + "velocityX": 3.69357007794, + "velocityY": -0.48287837389284793, + "timestamp": 5.918945683382923 + }, + { + "x": 6.73607293245351, + "y": 4.025087027843618, + "heading": 0.010715724063935807, + "angularVelocity": -2.713690122742687e-7, + "velocityX": 3.693570143067337, + "velocityY": -0.4828778757290756, + "timestamp": 5.960560688722066 + }, + { + "x": 6.889847609682978, + "y": 4.005509298883675, + "heading": 0.010715712767818455, + "angularVelocity": -2.714433714426062e-7, + "velocityX": 3.695173795516192, + "velocityY": -0.47044879125674055, + "timestamp": 6.00217569406121 + }, + { + "x": 7.044608530639083, + "y": 3.9966213295297166, + "heading": 0.010715699615645665, + "angularVelocity": -3.1604400106558483e-7, + "velocityX": 3.7188730289681042, + "velocityY": -0.21357607145609414, + "timestamp": 6.043790699400353 }, { - "x": 3.5962904713608705, - "y": 2.6551219777359605, - "heading": -0.6949668036285394, - "angularVelocity": -0.4469960741425893, - "velocityX": -2.6022745236884077, - "velocityY": 1.5125295831382852, - "timestamp": 5.151246783403483 + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 4.739357818257744e-26, + "angularVelocity": -0.2574960528856741, + "velocityX": 3.6592893702458946, + "velocityY": 0.030164951189468058, + "timestamp": 6.085405704739497 + }, + { + "x": 7.411732273884802, + "y": 4.005484253819352, + "heading": -0.0035958440097681034, + "angularVelocity": -0.05461699297217377, + "velocityX": 3.263224331051482, + "velocityY": 0.11555138753593294, + "timestamp": 6.1512431632682025 + }, + { + "x": 7.599754753311007, + "y": 4.014664501087166, + "heading": -0.0036927014054620503, + "angularVelocity": -0.0014711593955546794, + "velocityX": 2.855858710649135, + "velocityY": 0.13943805658615363, + "timestamp": 6.217080621796908 + }, + { + "x": 7.7609064467329985, + "y": 4.024479661032519, + "heading": -0.0024912592269265435, + "angularVelocity": 0.018248611130875694, + "velocityX": 2.4477204470419824, + "velocityY": 0.14908169550731518, + "timestamp": 6.282918080325614 + }, + { + "x": 7.895178919331714, + "y": 4.034515490111863, + "heading": -0.0009603004800075886, + "angularVelocity": 0.023253612474294444, + "velocityX": 2.039454067628866, + "velocityY": 0.15243342169669197, + "timestamp": 6.34875553885432 + }, + { + "x": 8.002571195010699, + "y": 4.044538744835798, + "heading": 0.0003549668988073664, + "angularVelocity": 0.01997749318105112, + "velocityX": 1.6311728623631503, + "velocityY": 0.1522424307974114, + "timestamp": 6.414592997383026 + }, + { + "x": 8.083084077776899, + "y": 4.054399862715161, + "heading": 0.001104922913523553, + "angularVelocity": 0.011391023157268422, + "velocityX": 1.2229038690959988, + "velocityY": 0.1497797469667492, + "timestamp": 6.480430455911732 + }, + { + "x": 8.136718786187338, + "y": 4.063994789685242, + "heading": 0.0010462868235224848, + "angularVelocity": -0.0008906189775764543, + "velocityX": 0.814653384395943, + "velocityY": 0.1457365940985966, + "timestamp": 6.546267914440437 }, { - "x": 3.4580497056890147, - "y": 2.735472006369499, - "heading": -0.7187186173095553, - "angularVelocity": -0.3912208492729469, - "velocityX": -2.276991158511501, - "velocityY": 1.3234613096617127, - "timestamp": 5.211958817859614 + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -6.959895417141788e-26, + "angularVelocity": -0.015891968598184786, + "velocityX": 0.40642176825516096, + "velocityY": 0.14053042740563873, + "timestamp": 6.612105372969143 + }, + { + "x": 8.158055086430046, + "y": 4.083508099899822, + "heading": -0.0027577804882154954, + "angularVelocity": -0.035854270657099625, + "velocityX": -0.07048533094049422, + "velocityY": 0.13340649729451912, + "timestamp": 6.689021746881477 + }, + { + "x": 8.11595164588489, + "y": 4.093221294829269, + "heading": -0.007050996192854494, + "angularVelocity": -0.05581666797673314, + "velocityX": -0.5473924263921114, + "velocityY": 0.12628253823454794, + "timestamp": 6.765938120793811 + }, + { + "x": 8.037166241278735, + "y": 4.102386537647484, + "heading": -0.012879673428225855, + "angularVelocity": -0.07577940741219352, + "velocityX": -1.024299516458623, + "velocityY": 0.11915854000945891, + "timestamp": 6.8428544947061445 + }, + { + "x": 7.921698873219381, + "y": 4.111003824490319, + "heading": -0.02024386647880979, + "angularVelocity": -0.09574285260739591, + "velocityX": -1.5012065986230485, + "velocityY": 0.11203449154606203, + "timestamp": 6.919770868618478 + }, + { + "x": 7.769549542634506, + "y": 4.119073150509933, + "heading": -0.02914366903151699, + "angularVelocity": -0.11570751583857591, + "velocityX": -1.97811366872662, + "velocityY": 0.10491038005524037, + "timestamp": 6.996687242530812 + }, + { + "x": 7.580718251048934, + "y": 4.126594509566205, + "heading": -0.039579227850997996, + "angularVelocity": -0.13567408717648377, + "velocityX": -2.4550207190057343, + "velocityY": 0.09778618873589355, + "timestamp": 7.073603616443146 + }, + { + "x": 7.355205001381359, + "y": 4.133567893300724, + "heading": -0.051550757950875034, + "angularVelocity": -0.15564345393507104, + "velocityX": -2.931927731338533, + "velocityY": 0.09066188874772477, + "timestamp": 7.1505199903554795 + }, + { + "x": 7.093009801840762, + "y": 4.139993286528261, + "heading": -0.06505855463043991, + "angularVelocity": -0.1756166599189986, + "velocityX": -3.4088346369452576, + "velocityY": 0.08353739133439493, + "timestamp": 7.227436364267813 + }, + { + "x": 6.806528186494828, + "y": 4.135714113450545, + "heading": -0.06505855526802462, + "angularVelocity": -8.289323637518806e-9, + "velocityX": -3.724585556678138, + "velocityY": -0.05563409791773408, + "timestamp": 7.304352738180147 + }, + { + "x": 6.520046573292198, + "y": 4.131434796884326, + "heading": -0.0650585559056006, + "angularVelocity": -8.28921001864462e-9, + "velocityX": -3.7245855288127556, + "velocityY": -0.055635963430836564, + "timestamp": 7.381269112092481 + }, + { + "x": 6.233564957107684, + "y": 4.127155679947005, + "heading": -0.06505855654317651, + "angularVelocity": -8.289209203416631e-9, + "velocityX": -3.7245855675806383, + "velocityY": -0.05563336802900127, + "timestamp": 7.4581854860048145 + }, + { + "x": 5.947529123843822, + "y": 4.143694400517207, + "heading": -0.06505855718976304, + "angularVelocity": -8.406357481668178e-9, + "velocityX": -3.718789884581325, + "velocityY": 0.2150221042537956, + "timestamp": 7.535101859917148 }, { - "x": 3.3395575797280896, - "y": 2.804343370444312, - "heading": -0.7390815132457978, - "angularVelocity": -0.33540131077235125, - "velocityX": -1.9517073842508859, - "velocityY": 1.1343939416917022, - "timestamp": 5.272670852315745 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.06505855788186755, + "angularVelocity": -8.99814278746362e-9, + "velocityX": -3.660825136452102, + "velocityY": 0.6884707552333696, + "timestamp": 7.612018233829482 + }, + { + "x": 5.429536143877612, + "y": 4.268856603157875, + "heading": -0.06505855854289869, + "angularVelocity": -9.961057942866914e-9, + "velocityX": -3.5625392585748106, + "velocityY": 1.088093052224975, + "timestamp": 7.678379771986332 + }, + { + "x": 5.202528472751969, + "y": 4.366702264809053, + "heading": -0.06505855916311704, + "angularVelocity": -9.346051554197093e-9, + "velocityX": -3.420771691413992, + "velocityY": 1.4744332992992566, + "timestamp": 7.744741310143183 + }, + { + "x": 4.9875020778910555, + "y": 4.485753657004608, + "heading": -0.06936550467343977, + "angularVelocity": -0.06490123089285486, + "velocityX": -3.240226203809265, + "velocityY": 1.7939818078684182, + "timestamp": 7.811102848300033 + }, + { + "x": 4.796367419741578, + "y": 4.59157781663034, + "heading": -0.07462073630187449, + "angularVelocity": -0.07919092556314197, + "velocityX": -2.880202350007574, + "velocityY": 1.5946610425998575, + "timestamp": 7.877464386456883 + }, + { + "x": 4.629125206769459, + "y": 4.684173938079997, + "heading": -0.07977888872827656, + "angularVelocity": -0.07772804202052125, + "velocityX": -2.5201678203544975, + "velocityY": 1.3953281376751576, + "timestamp": 7.9438259246137335 + }, + { + "x": 4.485775178787585, + "y": 4.763541978929841, + "heading": -0.08449510558147229, + "angularVelocity": -0.07106852831000718, + "velocityX": -2.1601372114530464, + "velocityY": 1.1959945934684657, + "timestamp": 8.010187462770583 + }, + { + "x": 4.36631711494158, + "y": 4.829681959675743, + "heading": -0.08859775332946503, + "angularVelocity": -0.0618226741263267, + "velocityX": -1.8001099305995214, + "velocityY": 0.9966613581134071, + "timestamp": 8.076549000927432 + }, + { + "x": 4.270750854060208, + "y": 4.8825939059492045, + "heading": -0.09198414721152182, + "angularVelocity": -0.051029466406471824, + "velocityX": -1.440085078430444, + "velocityY": 0.7973285089986916, + "timestamp": 8.142910539084282 + }, + { + "x": 4.199076276840986, + "y": 4.9222778404051555, + "heading": -0.09458595257220126, + "angularVelocity": -0.03920652584227169, + "velocityX": -1.0800620240268282, + "velocityY": 0.5979960012704257, + "timestamp": 8.209272077241131 + }, + { + "x": 4.151293292248578, + "y": 4.948733781946395, + "heading": -0.09635439465344492, + "angularVelocity": -0.026648599932446167, + "velocityX": -0.7200403414319525, + "velocityY": 0.3986637783878444, + "timestamp": 8.27563361539798 }, { - "x": 3.2408141126366585, - "y": 2.861736116018487, - "heading": -0.7560532042537318, - "angularVelocity": -0.2795440996166463, - "velocityX": -1.6264232944257633, - "velocityY": 0.9453273323536148, - "timestamp": 5.333382886771876 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.013539177422752055, + "velocityX": -0.360019736526293, + "velocityY": 0.19933179122763733, + "timestamp": 8.34199515355483 }, { - "x": 3.16181931959339, - "y": 2.907650279470937, - "heading": -0.7696318707216167, - "angularVelocity": -0.22365691727389694, - "velocityX": -1.3011389545897718, - "velocityY": 0.756261322219841, - "timestamp": 5.394094921228007 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 3.915017632839986e-26, + "velocityX": -2.9397369327092406e-26, + "velocityY": 2.263261398601171e-26, + "timestamp": 8.40835669171168 + }, + { + "x": 4.148029983348262, + "y": 4.951774833695287, + "heading": -0.10587352183363513, + "angularVelocity": -0.1412289747597874, + "velocityX": 0.33794369238887556, + "velocityY": -0.16688855115376777, + "timestamp": 8.469396903578264 + }, + { + "x": 4.189291025705269, + "y": 4.931399019893449, + "heading": -0.12288563814036063, + "angularVelocity": -0.27870342822382294, + "velocityX": 0.6759649269762051, + "velocityY": -0.333809683465285, + "timestamp": 8.53043711544485 + }, + { + "x": 4.251190863307712, + "y": 4.900831857369696, + "heading": -0.14799474838457438, + "angularVelocity": -0.41135358932067295, + "velocityX": 1.0140829415490538, + "velocityY": -0.5007709113225891, + "timestamp": 8.591477327311434 + }, + { + "x": 4.333736987997449, + "y": 4.860070212810069, + "heading": -0.18081174216294543, + "angularVelocity": -0.5376290935899647, + "velocityX": 1.3523236922925204, + "velocityY": -0.6677834711439198, + "timestamp": 8.65251753917802 + }, + { + "x": 4.436939103038805, + "y": 4.809109928035341, + "heading": -0.22080172732119155, + "angularVelocity": -0.6551416506491224, + "velocityX": 1.690723408151419, + "velocityY": -0.8348641529310338, + "timestamp": 8.713557751044604 + }, + { + "x": 4.560810084348815, + "y": 4.747945301360263, + "heading": -0.2671863409578564, + "angularVelocity": -0.7599025661648731, + "velocityX": 2.0293340655624834, + "velocityY": -1.0020382433921913, + "timestamp": 8.774597962911189 + }, + { + "x": 4.705367311819798, + "y": 4.676568329714506, + "heading": -0.3187309488078688, + "angularVelocity": -0.8444369092734083, + "velocityX": 2.368229451544859, + "velocityY": -1.169343445297431, + "timestamp": 8.835638174777774 + }, + { + "x": 4.870632910411114, + "y": 4.594968271425701, + "heading": -0.37317308736223054, + "angularVelocity": -0.891906120400698, + "velocityX": 2.7074873028379653, + "velocityY": -1.3368246241864, + "timestamp": 8.896678386644359 + }, + { + "x": 5.056607795018724, + "y": 4.503143287545809, + "heading": -0.4249356353221189, + "angularVelocity": -0.848007344290121, + "velocityX": 3.0467601425449904, + "velocityY": -1.5043359299046106, + "timestamp": 8.957718598510944 + }, + { + "x": 5.260529254341505, + "y": 4.402568167992326, + "heading": -0.4249356801729229, + "angularVelocity": -7.347747104408298e-7, + "velocityX": 3.3407724692779857, + "velocityY": -1.6476862788961175, + "timestamp": 9.018758810377529 + }, + { + "x": 5.4644220623717725, + "y": 4.301934949251132, + "heading": -0.42493568721051356, + "angularVelocity": -1.1529433576821183e-7, + "velocityX": 3.3403030853811013, + "velocityY": -1.648638097147308, + "timestamp": 9.079799022244114 }, { - "x": 3.1025732126750043, - "y": 2.942085887277494, - "heading": -0.7798162350397829, - "angularVelocity": -0.16774869116806188, - "velocityX": -0.9758544158357173, - "velocityY": 0.5671957481747656, - "timestamp": 5.454806955684138 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.4249356942701639, + "angularVelocity": -1.1565573156948564e-7, + "velocityX": 3.301588580483846, + "velocityY": -1.7248608986943215, + "timestamp": 9.140839234110699 + }, + { + "x": 5.864577127291663, + "y": 4.06418051122573, + "heading": -0.42493570026957905, + "angularVelocity": -9.360477410609904e-8, + "velocityX": 3.09901633677209, + "velocityY": -2.0668164546168653, + "timestamp": 9.204932280705526 + }, + { + "x": 6.060491998416757, + "y": 3.9277351651374044, + "heading": -0.4249357062560635, + "angularVelocity": -9.340302536538937e-8, + "velocityX": 3.056725831174745, + "velocityY": -2.1288634779819966, + "timestamp": 9.269025327300353 + }, + { + "x": 6.256406839411796, + "y": 3.7912897757868893, + "heading": -0.4249357122425479, + "angularVelocity": -9.340302451557142e-8, + "velocityX": 3.0567253610760865, + "velocityY": -2.1288641529723815, + "timestamp": 9.33311837389518 + }, + { + "x": 6.4523216804065155, + "y": 3.654844386435917, + "heading": -0.4249357182290323, + "angularVelocity": -9.340302467625888e-8, + "velocityX": 3.05672536107112, + "velocityY": -2.1288641529795123, + "timestamp": 9.397211420490008 + }, + { + "x": 6.648236521401236, + "y": 3.5183989970849443, + "heading": -0.4249357242155167, + "angularVelocity": -9.340302391578915e-8, + "velocityX": 3.05672536107112, + "velocityY": -2.1288641529795123, + "timestamp": 9.461304467084835 + }, + { + "x": 6.844151362395955, + "y": 3.3819536077339722, + "heading": -0.4249357302020011, + "angularVelocity": -9.340302522406469e-8, + "velocityX": 3.0567253610711203, + "velocityY": -2.128864152979512, + "timestamp": 9.525397513679662 + }, + { + "x": 7.040066203390847, + "y": 3.2455082183832458, + "heading": -0.4249357361884855, + "angularVelocity": -9.340302460522487e-8, + "velocityX": 3.056725361073794, + "velocityY": -2.1288641529756727, + "timestamp": 9.58949056027449 + }, + { + "x": 7.235981060605469, + "y": 3.1090628523216983, + "heading": -0.4249357421751277, + "angularVelocity": -9.340548585202334e-8, + "velocityX": 3.0567256141391645, + "velocityY": -2.128863789610536, + "timestamp": 9.653583606869317 + }, + { + "x": 7.417177244420043, + "y": 2.9830454990225337, + "heading": -0.48590548097831143, + "angularVelocity": -0.9512691632309586, + "velocityX": 2.8270802129290376, + "velocityY": -1.966162633769009, + "timestamp": 9.717676653464144 + }, + { + "x": 7.5775843375603, + "y": 2.871507967151711, + "heading": -0.5361633770520781, + "angularVelocity": -0.7841396024045931, + "velocityX": 2.502722239969217, + "velocityY": -1.7402438766239745, + "timestamp": 9.781769700058971 + }, + { + "x": 7.717202062000419, + "y": 2.774449785512018, + "heading": -0.5757086599849341, + "angularVelocity": -0.6169980213742552, + "velocityX": 2.1783599291625744, + "velocityY": -1.514332471247, + "timestamp": 9.845862746653799 + }, + { + "x": 7.836030204991866, + "y": 2.691870623138966, + "heading": -0.6045409513138722, + "angularVelocity": -0.44985053544427944, + "velocityX": 1.8539942989859028, + "velocityY": -1.2884262296827267, + "timestamp": 9.909955793248626 + }, + { + "x": 7.934068628103717, + "y": 2.6237702758314256, + "heading": -0.6226600347771661, + "angularVelocity": -0.28269967533040424, + "velocityX": 1.5296265089661738, + "velocityY": -1.0625231741291041, + "timestamp": 9.974048839843453 + }, + { + "x": 8.011317274299664, + "y": 2.5701486603074475, + "heading": -0.6300657818408324, + "angularVelocity": -0.11554680978863106, + "velocityX": 1.2052578290478972, + "velocityY": -0.836621417966838, + "timestamp": 10.03814188643828 + }, + { + "x": 8.06777617103265, + "y": 2.5310058125218204, + "heading": -0.6267581325303803, + "angularVelocity": 0.051606991494130075, + "velocityX": 0.8808895774591454, + "velocityY": -0.6107191008265523, + "timestamp": 10.102234933033108 + }, + { + "x": 8.10344542848898, + "y": 2.5063418886445565, + "heading": -0.612737112733536, + "angularVelocity": 0.21876038886839116, + "velocityX": 0.5565230450320063, + "velocityY": -0.3848143470723113, + "timestamp": 10.166327979627935 + }, + { + "x": 8.118325233459473, + "y": 2.496157169342041, + "heading": -0.5880028838627427, + "angularVelocity": 0.38591126783462537, + "velocityX": 0.23215942697429998, + "velocityY": -0.15890521427230228, + "timestamp": 10.230421026222762 + }, + { + "x": 8.111774217431309, + "y": 2.500903203120534, + "heading": -0.5514605400771017, + "angularVelocity": 0.5570006800274493, + "velocityX": -0.09985457977086463, + "velocityY": 0.0723419400124114, + "timestamp": 10.296026590265951 + }, + { + "x": 8.083429359560292, + "y": 2.5208307056412647, + "heading": -0.504313305894281, + "angularVelocity": 0.7186468841542555, + "velocityX": -0.432049602566591, + "velocityY": 0.3037471411359584, + "timestamp": 10.36163215430914 + }, + { + "x": 8.033276752823499, + "y": 2.5559518951025, + "heading": -0.44733468663642767, + "angularVelocity": 0.8685028486355681, + "velocityX": -0.7644566046833565, + "velocityY": 0.5353385794856407, + "timestamp": 10.427237718352329 + }, + { + "x": 7.9612999822666515, + "y": 2.606281084085956, + "heading": -0.38151750427277176, + "angularVelocity": 1.0032256154421146, + "velocityX": -1.097113813539709, + "velocityY": 0.7671481789307238, + "timestamp": 10.492843282395517 + }, + { + "x": 7.867479640224459, + "y": 2.6718348304324313, + "heading": -0.3081892028790952, + "angularVelocity": 1.117714670441717, + "velocityX": -1.430066845861252, + "velocityY": 0.999210163078856, + "timestamp": 10.558448846438706 + }, + { + "x": 7.751793438011374, + "y": 2.75263175880398, + "heading": -0.2292374531912587, + "angularVelocity": 1.2034306973698263, + "velocityX": -1.763359615914976, + "velocityY": 1.2315560356795054, + "timestamp": 10.624054410481895 + }, + { + "x": 7.614220064693883, + "y": 2.8486908030805904, + "heading": -0.14760413079417548, + "angularVelocity": 1.244304863278703, + "velocityX": -2.0969772202083976, + "velocityY": 1.4641905100209518, + "timestamp": 10.689659974525084 + }, + { + "x": 7.454764083734287, + "y": 2.9600197368919443, + "heading": -0.06854432318452272, + "angularVelocity": 1.2050777820857683, + "velocityX": -2.4305252654275167, + "velocityY": 1.696943474764787, + "timestamp": 10.755265538568272 + }, + { + "x": 7.27363003976687, + "y": 3.0865189810411944, + "heading": -0.0037505079236450347, + "angularVelocity": 0.987626830221633, + "velocityX": -2.760955516641443, + "velocityY": 1.928178592687242, + "timestamp": 10.820871102611461 + }, + { + "x": 7.07280859084814, + "y": 3.2235171993435245, + "heading": -7.171638886724292e-8, + "angularVelocity": 0.057166434919867046, + "velocityX": -3.0610429442619163, + "velocityY": 2.0882103568554258, + "timestamp": 10.88647666665465 + }, + { + "x": 6.872223980539225, + "y": 3.3631158565871417, + "heading": -6.147329807763224e-8, + "angularVelocity": 1.5613143395684282e-7, + "velocityX": -3.0574329057953955, + "velocityY": 2.12784783241428, + "timestamp": 10.952082230697838 + }, + { + "x": 6.671639384359248, + "y": 3.502714534132154, + "heading": -5.123020973688295e-8, + "angularVelocity": 1.561313966298048e-7, + "velocityX": -3.0574326904335125, + "velocityY": 2.1278481418605137, + "timestamp": 11.017687794741027 + }, + { + "x": 6.471054788179423, + "y": 3.6423132116773846, + "heading": -4.098712138128988e-8, + "angularVelocity": 1.561313968560626e-7, + "velocityX": -3.057432690431191, + "velocityY": 2.1278481418638497, + "timestamp": 11.083293358784216 + }, + { + "x": 6.270470191999598, + "y": 3.781911889222615, + "heading": -3.074403308674018e-8, + "angularVelocity": 1.5613139592560247e-7, + "velocityX": -3.057432690431191, + "velocityY": 2.1278481418638493, + "timestamp": 11.148898922827405 + }, + { + "x": 6.069885595819435, + "y": 3.921510566767361, + "heading": -2.0500944793387612e-8, + "angularVelocity": 1.5613139590735494e-7, + "velocityX": -3.0574326904363347, + "velocityY": 2.1278481418564588, + "timestamp": 11.214504486870593 + }, + { + "x": 5.869300968338538, + "y": 4.0611091993371335, + "heading": -1.0257856528303386e-8, + "angularVelocity": 1.5613139547647166e-7, + "velocityX": -3.057433167541219, + "velocityY": 2.127847456320497, + "timestamp": 11.280110050913782 }, { - "x": 3.0630758014096395, - "y": 2.9650429559458633, - "heading": -0.7866056220675054, - "angularVelocity": -0.11182934468500333, - "velocityX": -0.6505697201418033, - "velocityY": 0.37813044603137397, - "timestamp": 5.515518990140269 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 3.3501373579040154e-26, + "angularVelocity": 1.563565023471243e-7, + "velocityX": -3.099573069501661, + "velocityY": 2.0659814025542267, + "timestamp": 11.345715614956971 + }, + { + "x": 5.446743415639457, + "y": 4.310089409453816, + "heading": -2.545629671266297e-9, + "angularVelocity": -3.841829867702764e-8, + "velocityX": -3.3082622124289722, + "velocityY": 1.712026190368146, + "timestamp": 11.411976479152617 + }, + { + "x": 5.225460228694709, + "y": 4.4194271674986805, + "heading": -5.081465433806644e-9, + "angularVelocity": -3.827049033125913e-8, + "velocityX": -3.3395759266189464, + "velocityY": 1.650110655394207, + "timestamp": 11.478237343348264 + }, + { + "x": 5.005890824462741, + "y": 4.527909625118252, + "heading": -0.005806060945576971, + "angularVelocity": -0.08762420977438817, + "velocityX": -3.3137117497238457, + "velocityY": 1.6372025770635645, + "timestamp": 11.54449820754391 + }, + { + "x": 4.810655372355854, + "y": 4.624369173325347, + "heading": -0.020290065438414114, + "angularVelocity": -0.21859063670028128, + "velocityX": -2.946467035661095, + "velocityY": 1.455754454428526, + "timestamp": 11.610759071739556 + }, + { + "x": 4.639828140240907, + "y": 4.708771468826467, + "heading": -0.03646660670869356, + "angularVelocity": -0.24413417281301306, + "velocityX": -2.5781014809971645, + "velocityY": 1.2737880274532598, + "timestamp": 11.677019935935203 + }, + { + "x": 4.493411439660615, + "y": 4.781114493443874, + "heading": -0.05214975334121426, + "angularVelocity": -0.23668792767648847, + "velocityX": -2.2097010408432496, + "velocityY": 1.091791142412547, + "timestamp": 11.743280800130849 + }, + { + "x": 4.3714027429262465, + "y": 4.841398729732693, + "heading": -0.06627328599748628, + "angularVelocity": -0.21315044450024043, + "velocityX": -1.8413387482257655, + "velocityY": 0.9098015400285159, + "timestamp": 11.809541664326495 + }, + { + "x": 4.2737994273361055, + "y": 4.889624930300945, + "heading": -0.07820644553038021, + "angularVelocity": -0.1800936295919612, + "velocityX": -1.4730160370675427, + "velocityY": 0.7278232959029468, + "timestamp": 11.875802528522142 + }, + { + "x": 4.20059930110606, + "y": 4.925793789498138, + "heading": -0.08753245479698174, + "angularVelocity": -0.14074687041607162, + "velocityX": -1.1047264040189804, + "velocityY": 0.5458555308062137, + "timestamp": 11.942063392717788 + }, + { + "x": 4.151800585484216, + "y": 4.949905897329157, + "heading": -0.09395516408530516, + "angularVelocity": -0.09693065984408723, + "velocityX": -0.7364636156533751, + "velocityY": 0.363896669983421, + "timestamp": 12.008324256913435 }, { - "x": 3.0433270931243896, - "y": 2.9765214920043945, - "heading": -0.79, - "angularVelocity": -0.05590947433901855, - "velocityX": -0.3252849037618716, - "velocityY": 0.18906525141774544, - "timestamp": 5.5762310245963995 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.04976861149222346, + "velocityX": -0.36822273621886326, + "velocityY": 0.1819452407240944, + "timestamp": 12.07458512110908 }, { - "x": 3.0433270931243896, - "y": 2.9765214920043945, - "heading": -0.79, - "angularVelocity": 6.3035161532873835e-28, - "velocityX": -3.172311091955017e-27, - "velocityY": -4.980348575504513e-27, - "timestamp": 5.6369430590525305 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -1.4138878397118624e-28, + "velocityX": 4.127775100583887e-27, + "velocityY": 5.8037818722321745e-27, + "timestamp": 12.140845985304727 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 3.0599792847904204, - "y": 2.966059333110192, - "heading": -0.7574164237892916, - "angularVelocity": 0.5582463314772692, - "velocityX": 0.2852978705745262, - "velocityY": -0.17924557403559396, - "timestamp": 5.695310798119411 + "timestamp": 1.0690211227266204, + "isStopPoint": false, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 3.0934694098196505, - "y": 2.9451270864719556, - "heading": -0.6941025427131003, - "angularVelocity": 1.0847410245519962, - "velocityX": 0.5737780075883294, - "velocityY": -0.35862699108922125, - "timestamp": 5.753678537186291 + "timestamp": 2.0915058665995807, + "isStopPoint": true, + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 3.1440142929618213, - "y": 2.9136974289532884, - "heading": -0.6025701359612278, - "angularVelocity": 1.5682020276130737, - "velocityX": 0.8659729492734756, - "velocityY": -0.5384765286634409, - "timestamp": 5.812046276253171 + "timestamp": 2.8723891079396515, + "isStopPoint": true, + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 3.2118603630472147, - "y": 2.8716921302721974, - "heading": -0.4864204343113424, - "angularVelocity": 1.9899640367566072, - "velocityX": 1.162389895000939, - "velocityY": -0.7196663662603013, - "timestamp": 5.870414015320051 + "timestamp": 3.795315702316198, + "isStopPoint": false, + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { - "x": 3.297261323571818, - "y": 2.8189697938755005, - "heading": -0.35051552340624004, - "angularVelocity": 2.3284251382322116, - "velocityX": 1.463153479814372, - "velocityY": -0.9032787159407621, - "timestamp": 5.9287817543869314 + "timestamp": 4.285698834229858, + "isStopPoint": true, + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 3.400457830317495, - "y": 2.755367474678837, - "heading": -0.2010210241745889, - "angularVelocity": 2.561252185224339, - "velocityX": 1.768040160463131, - "velocityY": -1.0896827633461208, - "timestamp": 5.987149493453812 + "timestamp": 5.21252131357367, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 }, { - "x": 3.521660389826238, - "y": 2.6807799872839175, - "heading": -0.04666004939322163, - "angularVelocity": 2.644628304078967, - "velocityX": 2.076533397496587, - "velocityY": -1.277888926097578, - "timestamp": 6.045517232520692 + "timestamp": 5.669255651348061, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 }, { - "x": 3.6607588635338297, - "y": 2.5953087461465243, - "heading": 0.09647781625085058, - "angularVelocity": 2.4523455582211025, - "velocityX": 2.383139657820331, - "velocityY": -1.4643575801258442, - "timestamp": 6.103884971587572 + "timestamp": 6.085405704739497, + "isStopPoint": false, + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 3.8159145666935697, - "y": 2.50056098976987, - "heading": 0.19948059626334666, - "angularVelocity": 1.7647210883819062, - "velocityX": 2.6582441883170422, - "velocityY": -1.6232898154250606, - "timestamp": 6.162252710654452 + "timestamp": 6.612105372969143, + "isStopPoint": false, + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 }, { - "x": 3.984110357979779, - "y": 2.3943443694149815, - "heading": 0.2530307730268392, - "angularVelocity": 0.9174618996657108, - "velocityX": 2.8816567846406262, - "velocityY": -1.8197830180329746, - "timestamp": 6.220620449721332 + "timestamp": 7.612018233829482, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 4.165386797993929, - "y": 2.276764205392624, - "heading": 0.25694414020151846, - "angularVelocity": 0.06704674940715359, - "velocityX": 3.105764295691424, - "velocityY": -2.0144717938728, - "timestamp": 6.278988188788213 + "timestamp": 8.40835669171168, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 4.35049005488602, - "y": 2.162710813518879, - "heading": 0.2569442210228932, - "angularVelocity": 0.0000013846925722227273, - "velocityX": 3.171328200326413, - "velocityY": -1.9540484811833736, - "timestamp": 6.337355927855093 + "timestamp": 9.140839234110699, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 }, { - "x": 4.5355934176880135, - "y": 2.0486575935320284, - "heading": 0.2569443018441972, - "angularVelocity": 0.0000013846913602153038, - "velocityX": 3.171330014854514, - "velocityY": -1.9540455362878368, - "timestamp": 6.395723666921973 + "timestamp": 10.230421026222762, + "isStopPoint": false, + "x": 8.118325233459473, + "y": 2.496157169342041, + "heading": -0.5880028838627427, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 4.72069678050449, - "y": 1.9346043735686815, - "heading": 0.25694438266550157, - "angularVelocity": 0.0000013846913664886619, - "velocityX": 3.171330015102631, - "velocityY": -1.9540455358851527, - "timestamp": 6.454091405988853 + "timestamp": 11.345715614956971, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 4.905800420839664, - "y": 1.8205516040072125, - "heading": 0.2569444634867951, - "angularVelocity": 0.0000013846911811343512, - "velocityX": 3.171334769761663, - "velocityY": -1.9540378192614634, - "timestamp": 6.512459145055733 - }, + "timestamp": 12.140845985304727, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ { - "x": 5.096209165426906, - "y": 1.7155949376896535, - "heading": 0.2569445446080598, - "angularVelocity": 0.0000013898305123819847, - "velocityX": 3.262225805407059, - "velocityY": -1.7981965379418772, - "timestamp": 6.570826884122614 + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 5.295905298565615, - "y": 1.6296132789604225, - "heading": 0.2569446291955456, - "angularVelocity": 0.000001449216418667715, - "velocityX": 3.4213443304680435, - "velocityY": -1.473102438158684, - "timestamp": 6.629194623189494 + "scope": [ + "last" + ], + "type": "StopPoint" }, { - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": 0.256944722079518, - "angularVelocity": 0.0000015913580667498199, - "velocityX": 3.548163797412206, - "velocityY": -1.1340904342967109, - "timestamp": 6.687562362256374 + "scope": [ + 3 + ], + "type": "StopPoint" }, { - "x": 5.694707875531734, - "y": 1.520245371460122, - "heading": 0.25694480525285635, - "angularVelocity": 0.0000015766499744480022, - "velocityX": 3.6339835774201763, - "velocityY": -0.8184074026058323, - "timestamp": 6.740315566608581 + "scope": [ + 5 + ], + "type": "StopPoint" }, { - "x": 5.889460824583175, - "y": 1.4940579532692961, - "heading": 0.2569448839753191, - "angularVelocity": 0.0000014922783134293427, - "velocityX": 3.691774773550607, - "velocityY": -0.49641379158667054, - "timestamp": 6.793068770960788 + "scope": [ + 2 + ], + "type": "StopPoint" }, { - "x": 6.085760337593024, - "y": 1.4850583071038423, - "heading": 0.25694496145000295, - "angularVelocity": 0.0000014686251728049365, - "velocityX": 3.721091740688451, - "velocityY": -0.17059904276842675, - "timestamp": 6.845821975312996 + "scope": [ + 4 + ], + "type": "WptVelocityDirection", + "direction": 0.5125504196 }, { - "x": 6.282233415675307, - "y": 1.4886391007137698, - "heading": 0.2569450396472057, - "angularVelocity": 0.0000014823213810882118, - "velocityX": 3.7243818739526797, - "velocityY": 0.06787821998490172, - "timestamp": 6.898575179665203 + "scope": [ + 11 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "CenterLanePCBAGF": { + "waypoints": [ + { + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 6.478706465105997, - "y": 1.4922214660505555, - "heading": 0.2569451178445693, - "angularVelocity": 0.000001482324430715833, - "velocityX": 3.7243813308274873, - "velocityY": 0.06790801394485714, - "timestamp": 6.95132838401741 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 6.6750148942106335, - "y": 1.5010244818843792, - "heading": 0.2569451969790633, - "angularVelocity": 0.0000015000888565600604, - "velocityX": 3.7212607559150324, - "velocityY": 0.16687167996564178, - "timestamp": 7.004081588369617 + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 6.869794082781354, - "y": 1.5270160132207926, - "heading": 0.25694528055290106, - "angularVelocity": 0.000001584241920357872, - "velocityX": 3.692272174980594, - "velocityY": 0.4927005222826037, - "timestamp": 7.0568347927218245 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 7.061541469504156, - "y": 1.5699966670376497, - "heading": 0.25694631305508775, - "angularVelocity": 0.000019572312228907657, - "velocityX": 3.6348007495923222, - "velocityY": 0.814749631698122, - "timestamp": 7.109587997074032 + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { - "x": 7.242025004547745, - "y": 1.623544327324134, - "heading": 0.2892009263238574, - "angularVelocity": 0.611424721300743, - "velocityX": 3.421280986811508, - "velocityY": 1.0150598611787165, - "timestamp": 7.162341201426239 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 7.4101475255884415, - "y": 1.6859098332100053, - "heading": 0.3552181316610267, - "angularVelocity": 1.2514349819662982, - "velocityX": 3.186963201670605, - "velocityY": 1.1822126570641514, - "timestamp": 7.215094405778446 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 }, { - "x": 7.5650279020019084, - "y": 1.7562974315324296, - "heading": 0.45280543116466554, - "angularVelocity": 1.8498838260534038, - "velocityX": 2.935942533071662, - "velocityY": 1.3342810012540722, - "timestamp": 7.267847610130653 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 }, { - "x": 7.70378035758304, - "y": 1.8320280138991303, - "heading": 0.5628377984659659, - "angularVelocity": 2.0857949512728657, - "velocityX": 2.6302185295655014, - "velocityY": 1.4355636457850884, - "timestamp": 7.3206008144828605 + "x": 7.402774333953857, + "y": 2.964965343475342, + "heading": -0.5880028838627427, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 7.825662907370156, - "y": 1.911322826381737, - "heading": 0.6698617562914626, - "angularVelocity": 2.0287669562392776, - "velocityX": 2.3104293148405795, - "velocityY": 1.5031278849564027, - "timestamp": 7.373354018835068 + "x": 8.118325233459473, + "y": 2.496157169342041, + "heading": -0.5880028838627427, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 }, { - "x": 7.930693514616294, - "y": 1.9932244953323592, - "heading": 0.7659443680912281, - "angularVelocity": 1.8213606733397365, - "velocityX": 1.9909806150333516, - "velocityY": 1.5525439630890379, - "timestamp": 7.426107223187275 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 8.019011714129077, - "y": 2.0771819587981266, - "heading": 0.8464048765673222, - "angularVelocity": 1.5252250448882378, - "velocityX": 1.6741769641730917, - "velocityY": 1.5915140036844804, - "timestamp": 7.478860427539482 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 }, { - "x": 8.090768492192002, - "y": 2.1628615391483916, - "heading": 0.9080474489075738, - "angularVelocity": 1.1685085881929373, - "velocityX": 1.360235438663432, - "velocityY": 1.6241587862269713, - "timestamp": 7.531613631891689 + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 8.146101418716428, - "y": 2.2500495157538962, - "heading": 0.9485346126333066, - "angularVelocity": 0.7674825486508784, - "velocityX": 1.048901715145006, - "velocityY": 1.6527522389615, - "timestamp": 7.5843668362438965 + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 }, { - "x": 8.185122291410421, - "y": 2.338598291571523, - "heading": 0.9662103754330295, - "angularVelocity": 0.3350651968307046, - "velocityX": 0.7396872507207403, - "velocityY": 1.6785478134452219, - "timestamp": 7.637120040596104 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 8.207907676696777, - "y": 2.428393840789795, - "heading": 0.9600708878718816, - "angularVelocity": -0.11638132008356272, - "velocityX": 0.4319241942959219, - "velocityY": 1.7021818924733116, - "timestamp": 7.689873244948311 - }, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ { - "x": 8.211423054828662, - "y": 2.540472392253658, - "heading": 0.9162466453352144, - "angularVelocity": -0.6758725051077806, - "velocityX": 0.054215367725083355, - "velocityY": 1.7285138763859442, - "timestamp": 7.754714235231942 + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -9.687220582798775e-29, + "angularVelocity": -9.134521375874455e-29, + "velocityX": 4.127432494607337e-27, + "velocityY": -1.3653566194328874e-27, + "timestamp": 0 }, { - "x": 8.190202888383633, - "y": 2.6538474230997973, - "heading": 0.8391734249667565, - "angularVelocity": -1.1886496494165113, - "velocityX": -0.3272646878495706, - "velocityY": 1.7485086262595175, - "timestamp": 7.819555225515574 + "x": 1.3104781299456403, + "y": 5.567275491596599, + "heading": -0.013632838685405388, + "angularVelocity": -0.19128951975675332, + "velocityX": 0.2876978871987266, + "velocityY": -0.33224984965521687, + "timestamp": 0.07126808987100346 + }, + { + "x": 1.3514857086951348, + "y": 5.519918181736226, + "heading": -0.040881942077731624, + "angularVelocity": -0.3823464813164996, + "velocityX": 0.575398875200937, + "velocityY": -0.6644952873872657, + "timestamp": 0.1425361797420069 + }, + { + "x": 1.4129978772139142, + "y": 5.448882692998005, + "heading": -0.08170986553530107, + "angularVelocity": -0.5728780374424056, + "velocityX": 0.8631095435575385, + "velocityY": -0.9967362513404827, + "timestamp": 0.21380426961301036 + }, + { + "x": 1.4950158086435297, + "y": 5.354169496620718, + "heading": -0.13606373855442938, + "angularVelocity": -0.7626677397627718, + "velocityX": 1.150836672879393, + "velocityY": -1.3289706030948822, + "timestamp": 0.2850723594840138 + }, + { + "x": 1.5975411283703898, + "y": 5.235779380543047, + "heading": -0.20388316047242194, + "angularVelocity": -0.9516099286615785, + "velocityX": 1.438586608851632, + "velocityY": -1.6611938988677692, + "timestamp": 0.3563404493550173 + }, + { + "x": 1.7205758185191724, + "y": 5.093713591292254, + "heading": -0.28510990320243546, + "angularVelocity": -1.1397350886916635, + "velocityX": 1.7263643570562608, + "velocityY": -1.9933997039619569, + "timestamp": 0.42760853922602077 + }, + { + "x": 1.8641220914189134, + "y": 4.92797393358567, + "heading": -0.3796975284198533, + "angularVelocity": -1.3272086481989789, + "velocityX": 2.0141731476115345, + "velocityY": -2.3255801861194376, + "timestamp": 0.49887662909702424 + }, + { + "x": 2.036057344584012, + "y": 4.766214838118392, + "heading": -0.4690840637578516, + "angularVelocity": -1.254229424414062, + "velocityX": 2.412513840013176, + "velocityY": -2.2697268266915005, + "timestamp": 0.5701447189680277 + }, + { + "x": 2.1874785084173527, + "y": 4.628126850696092, + "heading": -0.5451442530827, + "angularVelocity": -1.067240464316059, + "velocityX": 2.1246698783062077, + "velocityY": -1.9375850773079741, + "timestamp": 0.6414128088390311 + }, + { + "x": 2.3183845436377744, + "y": 4.5137087594261915, + "heading": -0.6078811711978208, + "angularVelocity": -0.8802946483998079, + "velocityX": 1.836811333899418, + "velocityY": -1.605460332625711, + "timestamp": 0.7126808987100345 + }, + { + "x": 2.4287747758121854, + "y": 4.422959692661206, + "heading": -0.6572940075938836, + "angularVelocity": -0.6933374598014469, + "velocityX": 1.548943326167706, + "velocityY": -1.273347818487104, + "timestamp": 0.7839489885810379 + }, + { + "x": 2.5186487653652114, + "y": 4.355879041329831, + "heading": -0.6933804923945726, + "angularVelocity": -0.506348421376329, + "velocityX": 1.2610691505230385, + "velocityY": -0.9412438505478742, + "timestamp": 0.8552170784520413 + }, + { + "x": 2.58800626172225, + "y": 4.312466418462692, + "heading": -0.7161379527262242, + "angularVelocity": -0.31932187845700755, + "velocityX": 0.9731914589345287, + "velocityY": -0.6091453123791657, + "timestamp": 0.9264851683230447 + }, + { + "x": 2.6368471855998568, + "y": 4.292721634612479, + "heading": -0.7255636953209235, + "angularVelocity": -0.1322575448810274, + "velocityX": 0.685312654878354, + "velocityY": -0.27704943244516045, + "timestamp": 0.9977532581940481 }, { - "x": 8.144008306424501, - "y": 2.768019013990823, - "heading": 0.7319233890258415, - "angularVelocity": -1.6540468532601917, - "velocityX": -0.7124286929774685, - "velocityY": 1.760793448582603, - "timestamp": 7.884396215799205 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05484334197647404, + "velocityX": 0.3974350607879547, + "velocityY": 0.05504641764931743, + "timestamp": 1.0690213480650517 + }, + { + "x": 2.672652235661854, + "y": 4.3255208850960845, + "heading": -0.7036434508654703, + "angularVelocity": 0.2466181683415971, + "velocityX": 0.1024255678097115, + "velocityY": 0.3953768419726611, + "timestamp": 1.1420559681490803 + }, + { + "x": 2.6585872493180758, + "y": 4.379253606612061, + "heading": -0.6716320440965468, + "angularVelocity": 0.43830455655267986, + "velocityX": -0.19257971531304016, + "velocityY": 0.7357157667713767, + "timestamp": 1.215090588233109 + }, + { + "x": 2.6229771758211826, + "y": 4.457843745767889, + "heading": -0.6256270362767148, + "angularVelocity": 0.6299068546793523, + "velocityX": -0.48757799323009304, + "velocityY": 1.0760669264166411, + "timestamp": 1.2881252083171375 + }, + { + "x": 2.5658227847055692, + "y": 4.561292508951217, + "heading": -0.5656323531358879, + "angularVelocity": 0.821455401175512, + "velocityX": -0.7825657345770498, + "velocityY": 1.4164346040864937, + "timestamp": 1.3611598284011661 + }, + { + "x": 2.4871252028274746, + "y": 4.689601433735054, + "heading": -0.49164521078608, + "angularVelocity": 1.0130420650464593, + "velocityX": -1.077538046854364, + "velocityY": 1.7568233344161037, + "timestamp": 1.4341944484851947 + }, + { + "x": 2.3868860574551665, + "y": 4.842772331616221, + "heading": -0.40364732106038387, + "angularVelocity": 1.2048791329981834, + "velocityX": -1.3724880783521554, + "velocityY": 2.097236868007793, + "timestamp": 1.5072290685692233 + }, + { + "x": 2.26510760033775, + "y": 5.020807045399027, + "heading": -0.30159033464993307, + "angularVelocity": 1.3973782062949196, + "velocityX": -1.6674072785934422, + "velocityY": 2.437675633527935, + "timestamp": 1.580263688653252 + }, + { + "x": 2.1359139807220116, + "y": 5.17000455286456, + "heading": -0.21560374484110897, + "angularVelocity": 1.1773401396473877, + "velocityX": -1.7689366969677838, + "velocityY": 2.042832663384521, + "timestamp": 1.6532983087372806 + }, + { + "x": 2.028255013080415, + "y": 5.294335941405348, + "heading": -0.1438207992307524, + "angularVelocity": 0.9828619020372543, + "velocityX": -1.474081298947423, + "velocityY": 1.7023623645572525, + "timestamp": 1.7263329288213092 + }, + { + "x": 1.9421291374445764, + "y": 5.393801086818426, + "heading": -0.08631852593953135, + "angularVelocity": 0.7873289848713239, + "velocityX": -1.1792472602273896, + "velocityY": 1.3618903651260308, + "timestamp": 1.7993675489053378 + }, + { + "x": 1.87753522481553, + "y": 5.468400016112323, + "heading": -0.043158813254579896, + "angularVelocity": 0.5909486848195399, + "velocityX": -0.8844286799154853, + "velocityY": 1.0214187354992572, + "timestamp": 1.8724021689893664 + }, + { + "x": 1.834472632995246, + "y": 5.518132767413434, + "heading": -0.01438145137891416, + "angularVelocity": 0.3940235718698405, + "velocityX": -0.5896188926667782, + "velocityY": 0.680947627904294, + "timestamp": 1.945436789073395 }, { - "x": 8.072604152106418, - "y": 2.8824539716664033, - "heading": 0.5977397407756817, - "angularVelocity": -2.0694262635904495, - "velocityX": -1.1012193676521893, - "velocityY": 1.764855181498803, - "timestamp": 7.949237206082836 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -2.0788362526419016e-27, + "angularVelocity": 0.19691279782612486, + "velocityX": -0.29481141121630017, + "velocityY": 0.34047551881670896, + "timestamp": 2.0184714091574234 }, { - "x": 7.975691382967155, - "y": 2.9965380091631473, - "heading": 0.44171220804424666, - "angularVelocity": -2.406310145001336, - "velocityX": -1.4946219777850651, - "velocityY": 1.7594431700951976, - "timestamp": 8.014078196366468 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -9.689019627790258e-28, + "angularVelocity": 4.903263424537125e-28, + "velocityX": -7.04775279749202e-27, + "velocityY": -4.880044267923792e-28, + "timestamp": 2.091506029241452 + }, + { + "x": 1.8720264471920118, + "y": 5.543082773692806, + "heading": 4.603106809112672e-20, + "angularVelocity": 4.715795015985417e-19, + "velocityX": 0.6053171264251316, + "velocityY": 0.0008555041789240918, + "timestamp": 2.1891164404325725 + }, + { + "x": 1.9901969516410831, + "y": 5.543249785918246, + "heading": 4.290138941133409e-20, + "angularVelocity": -3.206295969142147e-20, + "velocityX": 1.210634224434258, + "velocityY": 0.0017110083176873983, + "timestamp": 2.286726851623693 + }, + { + "x": 2.1674527004558795, + "y": 5.543500304245301, + "heading": 2.9796741833873356e-20, + "angularVelocity": -1.3425460888073973e-19, + "velocityX": 1.8159512561393765, + "velocityY": 0.0025665123627422117, + "timestamp": 2.384337262814814 + }, + { + "x": 2.4037936612766035, + "y": 5.543834328628234, + "heading": -1.7822834862290494e-20, + "angularVelocity": -4.878534586092089e-19, + "velocityX": 2.421267956324555, + "velocityY": 0.0034220159392546997, + "timestamp": 2.4819476740059345 + }, + { + "x": 2.5810494100914, + "y": 5.544084846955289, + "heading": -6.57207439293443e-21, + "angularVelocity": 1.1526188991905598e-19, + "velocityX": 1.8159512561393765, + "velocityY": 0.0025665123627422117, + "timestamp": 2.579558085197055 + }, + { + "x": 2.699219914540471, + "y": 5.544251859180729, + "heading": -1.8054002593168886e-20, + "angularVelocity": -1.1763015911739438e-19, + "velocityX": 1.210634224434258, + "velocityY": 0.0017110083176873983, + "timestamp": 2.677168496388176 }, { - "x": 7.852920805151493, - "y": 3.1093104190449847, - "heading": 0.2733111632783997, - "angularVelocity": -2.5971386931201565, - "velocityX": -1.8934099753663647, - "velocityY": 1.7392147989804203, - "timestamp": 8.078919186650099 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -3.155242334732113e-28, + "angularVelocity": 1.84959801497032e-19, + "velocityX": 0.6053171264251316, + "velocityY": 0.0008555041789240918, + "timestamp": 2.7747789075792966 }, { - "x": 7.704278478364254, - "y": 3.219028342207882, - "heading": 0.10912005253192102, - "angularVelocity": -2.5322116461865276, - "velocityX": -2.292412964963042, - "velocityY": 1.6921074567640455, - "timestamp": 8.14376017693373 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -3.309635664969631e-28, + "angularVelocity": -1.5501074190844928e-28, + "velocityX": -3.9535178243549605e-27, + "velocityY": -3.789490097440697e-27, + "timestamp": 2.8723893187704173 + }, + { + "x": 2.7303741375078747, + "y": 5.568056872076371, + "heading": 0.006778509195380246, + "angularVelocity": 0.08813497362119738, + "velocityX": -0.3631625447521428, + "velocityY": 0.30842982050092854, + "timestamp": 2.9492998676350957 + }, + { + "x": 2.674756164732603, + "y": 5.615779956875509, + "heading": 0.020597402851140194, + "angularVelocity": 0.17967488023097886, + "velocityX": -0.7231514219607195, + "velocityY": 0.6205011601608272, + "timestamp": 3.026210416499774 + }, + { + "x": 2.591861664751003, + "y": 5.687962530350053, + "heading": 0.041894082151838256, + "angularVelocity": 0.27690192847497247, + "velocityX": -1.0778040360555798, + "velocityY": 0.9385263080302098, + "timestamp": 3.1031209653644525 + }, + { + "x": 2.482522988073185, + "y": 5.785486691208239, + "heading": 0.07152941986388299, + "angularVelocity": 0.38532214565504047, + "velocityX": -1.4216343309445878, + "velocityY": 1.2680206070272062, + "timestamp": 3.180031514229131 + }, + { + "x": 2.3492932233323143, + "y": 5.910724338795779, + "heading": 0.11194462213648758, + "angularVelocity": 0.5254832122406744, + "velocityX": -1.7322690672157808, + "velocityY": 1.628354620221101, + "timestamp": 3.2569420630938093 + }, + { + "x": 2.2322888283047253, + "y": 6.068263103797439, + "heading": 0.17809343518700713, + "angularVelocity": 0.8600746455067716, + "velocityX": -1.5213049023152156, + "velocityY": 2.0483375470228995, + "timestamp": 3.3338526119584877 + }, + { + "x": 2.1483049194140715, + "y": 6.209825434589826, + "heading": 0.2439234240091836, + "angularVelocity": 0.85592925540035, + "velocityX": -1.0919686587911488, + "velocityY": 1.8406100708169548, + "timestamp": 3.410763160823166 + }, + { + "x": 2.0954357223517674, + "y": 6.331973751888774, + "heading": 0.3066259975705838, + "angularVelocity": 0.8152662344371413, + "velocityX": -0.687411516921112, + "velocityY": 1.5881867845445197, + "timestamp": 3.4876737096878445 + }, + { + "x": 2.072970005140484, + "y": 6.433621819948826, + "heading": 0.36524449476389237, + "angularVelocity": 0.7621645932659499, + "velocityX": -0.29210189685177457, + "velocityY": 1.32164013338272, + "timestamp": 3.564584258552523 + }, + { + "x": 2.080539806403903, + "y": 6.514239906298576, + "heading": 0.41928927766619223, + "angularVelocity": 0.7026966222460568, + "velocityX": 0.09842344613530389, + "velocityY": 1.0482058383381778, + "timestamp": 3.6414948074172013 + }, + { + "x": 2.1179206829564796, + "y": 6.573514734989947, + "heading": 0.4684594496501494, + "angularVelocity": 0.6393163578961122, + "velocityX": 0.48603055243236887, + "velocityY": 0.770698292579654, + "timestamp": 3.7184053562818797 }, { - "x": 7.532421373794059, - "y": 3.323057914868166, - "heading": -0.010926874391562237, - "angularVelocity": -1.851404896784686, - "velocityX": -2.650439233245006, - "velocityY": 1.6043797635605452, - "timestamp": 8.208601167217362 + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.5732759758018013, + "velocityX": 0.8716733324811171, + "velocityY": 0.49050096320339975, + "timestamp": 3.795315905146558 }, { - "x": 7.337707687800787, - "y": 3.422034456346363, - "heading": -0.08211008235538154, - "angularVelocity": -1.0978118571669786, - "velocityX": -3.0029412743627657, - "velocityY": 1.5264501829051038, - "timestamp": 8.273442157500993 + "x": 2.2917884862212228, + "y": 6.6722766025245654, + "heading": 0.5461831512487827, + "angularVelocity": 0.4115075108626729, + "velocityX": 1.3070625376552998, + "velocityY": 0.7468097995934108, + "timestamp": 3.877046446536376 }, { - "x": 7.120736598968506, - "y": 3.5168776512145996, - "heading": -0.09725138680843892, - "angularVelocity": -0.2335143924672555, - "velocityX": -3.3462025777705335, - "velocityY": 1.4627042932775656, - "timestamp": 8.338283147784624 + "x": 2.4341165316393667, + "y": 6.754285323763742, + "heading": 0.5638405564624909, + "angularVelocity": 0.21604414841069464, + "velocityX": 1.7414303514680458, + "velocityY": 1.0034036217627857, + "timestamp": 3.9587769879261936 }, { - "x": 7.010606209661033, - "y": 3.566708155420985, - "heading": -0.09725146950362613, - "angularVelocity": -0.000002548325484883459, - "velocityX": -3.39376555274088, - "velocityY": 1.5355711508400656, - "timestamp": 8.370733942512778 + "x": 2.5412608832572645, + "y": 6.816945409398408, + "heading": 0.539566584836395, + "angularVelocity": -0.2970000101959444, + "velocityX": 1.3109463096159757, + "velocityY": 0.766666714414698, + "timestamp": 4.040507529316011 }, { - "x": 6.900475999605217, - "y": 3.616539055790813, - "heading": -0.09725155219704654, - "angularVelocity": -0.0000025482710392560423, - "velocityX": -3.3937600289421765, - "velocityY": 1.5355833589676455, - "timestamp": 8.403184737240933 + "x": 2.6126555762150003, + "y": 6.858741818277979, + "heading": 0.521795194780163, + "angularVelocity": -0.21743879037177083, + "velocityX": 0.8735375019384187, + "velocityY": 0.511392781313182, + "timestamp": 4.122238070705829 }, { - "x": 6.790345789587328, - "y": 3.6663699562444614, - "heading": -0.09725163489046874, - "angularVelocity": -0.0000025482710945290222, - "velocityX": -3.3937600277734288, - "velocityY": 1.5355833615506584, - "timestamp": 8.435635531969087 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.1131128587056526, + "velocityX": 0.4366659015377109, + "velocityY": 0.2557557080208239, + "timestamp": 4.203968612095647 }, { - "x": 6.680215579569448, - "y": 3.7162008566981277, - "heading": -0.0972517175838927, - "angularVelocity": -0.0000025482711487845157, - "velocityX": -3.393760027773178, - "velocityY": 1.5355833615512018, - "timestamp": 8.468086326697241 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -4.764407701562395e-26, + "velocityX": 1.4812581731979975e-26, + "velocityY": -5.1560471026288735e-26, + "timestamp": 4.2856991534854645 + }, + { + "x": 2.6602506971919606, + "y": 6.863647804987902, + "heading": 0.4793942538974517, + "angularVelocity": -0.5654499433512294, + "velocityX": 0.20304968657976344, + "velocityY": -0.272816224127646, + "timestamp": 4.344335936621462 + }, + { + "x": 2.6841390735646424, + "y": 6.831518157238345, + "heading": 0.41523186966938014, + "angularVelocity": -1.0942343832071746, + "velocityX": 0.40739575220688923, + "velocityY": -0.5479435608709636, + "timestamp": 4.40297271975746 + }, + { + "x": 2.720131144955563, + "y": 6.7831009440148105, + "heading": 0.3230087558895857, + "angularVelocity": -1.5727860371517781, + "velocityX": 0.6138138804006831, + "velocityY": -0.8257140080696285, + "timestamp": 4.461609502893458 + }, + { + "x": 2.7684071443805327, + "y": 6.718246852790197, + "heading": 0.20639646027764702, + "angularVelocity": -1.98872259655644, + "velocityX": 0.8233057279592204, + "velocityY": -1.1060308522415998, + "timestamp": 4.520246286029455 + }, + { + "x": 2.829185609402161, + "y": 6.636825404453338, + "heading": 0.06956994287591697, + "angularVelocity": -2.333458796407444, + "velocityX": 1.03652454604584, + "velocityY": -1.388572905645519, + "timestamp": 4.578883069165453 + }, + { + "x": 2.9026837898482625, + "y": 6.53870263495317, + "heading": -0.0825332150617159, + "angularVelocity": -2.59398878660952, + "velocityX": 1.253448373449048, + "velocityY": -1.6733996009397407, + "timestamp": 4.637519852301451 + }, + { + "x": 2.98910296697184, + "y": 6.423727552121541, + "heading": -0.24244596371477412, + "angularVelocity": -2.7271746521661684, + "velocityX": 1.4738048798335797, + "velocityY": -1.9608013380434501, + "timestamp": 4.696156635437449 + }, + { + "x": 3.08851698550526, + "y": 6.291921017444985, + "heading": -0.39600744918619013, + "angularVelocity": -2.6188593108059237, + "velocityX": 1.6954207447370835, + "velocityY": -2.247847300402779, + "timestamp": 4.754793418573446 + }, + { + "x": 3.20017208933701, + "y": 6.146474995005627, + "heading": -0.5026655989158746, + "angularVelocity": -1.8189631836096676, + "velocityX": 1.9041819462159983, + "velocityY": -2.4804570554633094, + "timestamp": 4.813430201709444 + }, + { + "x": 3.3235802395453247, + "y": 5.987781312389556, + "heading": -0.5570167491322654, + "angularVelocity": -0.926912209531219, + "velocityX": 2.1046200628382103, + "velocityY": -2.706384527405065, + "timestamp": 4.872066984845442 + }, + { + "x": 3.458298567775881, + "y": 5.815853629795619, + "heading": -0.5570170670277983, + "angularVelocity": -0.000005421435418997904, + "velocityX": 2.297505439855057, + "velocityY": -2.9320790363819964, + "timestamp": 4.93070376798144 + }, + { + "x": 3.5900045442013857, + "y": 5.641607467957256, + "heading": -0.5570171376680287, + "angularVelocity": -0.0000012047084903437067, + "velocityX": 2.246132365754707, + "velocityY": -2.9716187096115108, + "timestamp": 4.989340551117437 + }, + { + "x": 3.721710444891145, + "y": 5.467361248873177, + "heading": -0.5570172083082521, + "angularVelocity": -0.0000012047083681560634, + "velocityX": 2.246131074146577, + "velocityY": -2.9716196858880317, + "timestamp": 5.047977334253435 + }, + { + "x": 3.8545074444088514, + "y": 5.293945142218947, + "heading": -0.5570172791057042, + "angularVelocity": -0.0000012073897706381049, + "velocityX": 2.264738828010173, + "velocityY": -2.9574628310018474, + "timestamp": 5.106614117389433 + }, + { + "x": 4.00357560161076, + "y": 5.134299054707746, + "heading": -0.5570173534542538, + "angularVelocity": -0.00000126795069051409, + "velocityX": 2.542229454439387, + "velocityY": -2.722626975305397, + "timestamp": 5.165250900525431 }, { - "x": 6.570085369551568, - "y": 3.7660317571517936, - "heading": -0.09725180027731849, - "angularVelocity": -0.0000025482712051303815, - "velocityX": -3.3937600277731743, - "velocityY": 1.5355833615511987, - "timestamp": 8.500537121425396 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.55701743434988, + "angularVelocity": -0.0000013796054608952668, + "velocityX": 2.795573955825942, + "velocityY": -2.461786802066348, + "timestamp": 5.223887683661428 + }, + { + "x": 4.2902143593852005, + "y": 4.896089807808244, + "heading": -0.5570175109527072, + "angularVelocity": -0.000001846969607519395, + "velocityX": 2.958786599579285, + "velocityY": -2.2630085323028566, + "timestamp": 5.265362555152153 + }, + { + "x": 4.419113803049847, + "y": 4.8109235045890575, + "heading": -0.5570175835802155, + "angularVelocity": -0.0000017511207539159885, + "velocityX": 3.107892539063629, + "velocityY": -2.0534434504090724, + "timestamp": 5.306837426642878 + }, + { + "x": 4.553582724143636, + "y": 4.7348546191881615, + "heading": -0.5570176536447519, + "angularVelocity": -0.0000016893249788320206, + "velocityX": 3.2421781252260224, + "velocityY": -1.8340957468163943, + "timestamp": 5.348312298133603 + }, + { + "x": 4.692974428091918, + "y": 4.668233908840813, + "heading": -0.5570177224005147, + "angularVelocity": -0.0000016577691599272835, + "velocityX": 3.3608712682678954, + "velocityY": -1.606290940822974, + "timestamp": 5.389787169624328 + }, + { + "x": 4.832547821463298, + "y": 4.601994690531772, + "heading": -0.5570177911147437, + "angularVelocity": -0.0000016567677375032224, + "velocityX": 3.3652519792036677, + "velocityY": -1.5970927920500795, + "timestamp": 5.431262041115053 + }, + { + "x": 4.9721212332499025, + "y": 4.535755511025668, + "heading": -0.5570178598289695, + "angularVelocity": -0.0000016567676584676064, + "velocityX": 3.3652524232128536, + "velocityY": -1.5970918564730696, + "timestamp": 5.472736912605778 + }, + { + "x": 5.111694645038376, + "y": 4.469516331523502, + "heading": -0.5570179285431947, + "angularVelocity": -0.000001656767647060506, + "velocityX": 3.365252423257915, + "velocityY": -1.5970918563781218, + "timestamp": 5.514211784096503 + }, + { + "x": 5.251268056826837, + "y": 4.4032771520213085, + "heading": -0.5570179972574194, + "angularVelocity": -0.0000016567676356654265, + "velocityX": 3.3652524232576075, + "velocityY": -1.597091856378771, + "timestamp": 5.555686655587228 + }, + { + "x": 5.390841468488021, + "y": 4.33703797225093, + "heading": -0.5570180659716438, + "angularVelocity": -0.0000016567676246630092, + "velocityX": 3.3652524201888485, + "velocityY": -1.5970918628449942, + "timestamp": 5.597161527077953 + }, + { + "x": 5.530413626022683, + "y": 4.27079614995943, + "heading": -0.5570181346861351, + "angularVelocity": -0.0000016567740615756042, + "velocityX": 3.365222181963228, + "velocityY": -1.5971555766318444, + "timestamp": 5.638636398568678 }, { - "x": 6.4599551595336875, - "y": 3.8158626576054595, - "heading": -0.09725188297074605, - "angularVelocity": -0.0000025482712597994537, - "velocityX": -3.3937600277731703, - "velocityY": 1.5355833615511956, - "timestamp": 8.53298791615355 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.5570182043959618, + "angularVelocity": -0.0000016807725799241125, + "velocityX": 3.2679571491481956, + "velocityY": -1.7877590150355749, + "timestamp": 5.6801112700594025 + }, + { + "x": 5.786627291659718, + "y": 4.120246269797506, + "heading": -0.5570182737203393, + "angularVelocity": -0.00000180799570053011, + "velocityX": 3.1472464214958245, + "velocityY": -1.9926027126510082, + "timestamp": 5.718454490347048 + }, + { + "x": 5.902360023994386, + "y": 4.0365449270340825, + "heading": -0.557018342200347, + "angularVelocity": -0.0000017859743423988426, + "velocityX": 3.018336265615098, + "velocityY": -2.182950261754457, + "timestamp": 5.756797710634693 + }, + { + "x": 6.018082629955809, + "y": 3.952829584452034, + "heading": -0.5570184106790251, + "angularVelocity": -0.0000017859396699979741, + "velocityX": 3.0180721674728543, + "velocityY": -2.1833153802426746, + "timestamp": 5.795140930922338 + }, + { + "x": 6.133805234621251, + "y": 3.869114240078507, + "heading": -0.5570184791577035, + "angularVelocity": -0.0000017859396766231999, + "velocityX": 3.018072133673387, + "velocityY": -2.183315426964841, + "timestamp": 5.8334841512099835 + }, + { + "x": 6.249527839286528, + "y": 3.7853988957047506, + "heading": -0.5570185476363823, + "angularVelocity": -0.0000017859396843832207, + "velocityX": 3.01807213366906, + "velocityY": -2.1833154269708217, + "timestamp": 5.871827371497629 + }, + { + "x": 6.365250443951803, + "y": 3.701683551330994, + "heading": -0.5570186161150613, + "angularVelocity": -0.000001785939693521326, + "velocityX": 3.0180721336690577, + "velocityY": -2.1833154269708244, + "timestamp": 5.910170591785274 + }, + { + "x": 6.480973048617079, + "y": 3.6179682069572374, + "heading": -0.5570186845937407, + "angularVelocity": -0.0000017859397005145362, + "velocityX": 3.0180721336690555, + "velocityY": -2.183315426970826, + "timestamp": 5.948513812072919 + }, + { + "x": 6.596695653282355, + "y": 3.534252862583481, + "heading": -0.5570187530724204, + "angularVelocity": -0.0000017859397100037096, + "velocityX": 3.0180721336690537, + "velocityY": -2.1833154269708275, + "timestamp": 5.9868570323605645 + }, + { + "x": 6.71241825794763, + "y": 3.4505375182097238, + "heading": -0.5570188215511004, + "angularVelocity": -0.000001785939717860173, + "velocityX": 3.0180721336690515, + "velocityY": -2.1833154269708293, + "timestamp": 6.02520025264821 + }, + { + "x": 6.828140862612906, + "y": 3.366822173835967, + "heading": -0.5570188900297807, + "angularVelocity": -0.000001785939724980264, + "velocityX": 3.01807213366905, + "velocityY": -2.183315426970831, + "timestamp": 6.063543472935855 + }, + { + "x": 6.943863467278195, + "y": 3.283106829462228, + "heading": -0.5570189585084613, + "angularVelocity": -0.0000017859397331931199, + "velocityX": 3.018072133669382, + "velocityY": -2.1833154269703705, + "timestamp": 6.1018866932235 + }, + { + "x": 7.0595860720436105, + "y": 3.199391485226898, + "heading": -0.5570190269871422, + "angularVelocity": -0.0000017859397430016153, + "velocityX": 3.018072136280727, + "velocityY": -2.1833154233606176, + "timestamp": 6.1402299135111456 + }, + { + "x": 7.175309459223025, + "y": 3.1156772225606058, + "heading": -0.557019095467055, + "angularVelocity": -0.0000017859718654957953, + "velocityX": 3.018092541817673, + "velocityY": -2.183287215791487, + "timestamp": 6.178573133798791 + }, + { + "x": 7.292415293414065, + "y": 3.037748831863909, + "heading": -0.5654204714615869, + "angularVelocity": -0.2191098173681297, + "velocityX": 3.0541470776979107, + "velocityY": -2.0323903446838734, + "timestamp": 6.216916354086436 + }, + { + "x": 7.402774333953857, + "y": 2.964965343475342, + "heading": -0.5880028838627427, + "angularVelocity": -0.5889545069961751, + "velocityX": 2.8781891482221704, + "velocityY": -1.8982101097027282, + "timestamp": 6.255259574374081 + }, + { + "x": 7.566192905422352, + "y": 2.8587474703903673, + "heading": -0.6090225737312371, + "angularVelocity": -0.3298358622780655, + "velocityX": 2.5643244866971804, + "velocityY": -1.6667450365591048, + "timestamp": 6.318987301623988 + }, + { + "x": 7.708728086420993, + "y": 2.766445154767202, + "heading": -0.6215083339078488, + "angularVelocity": -0.19592351265955282, + "velocityX": 2.2366274014400647, + "velocityY": -1.4483854925690898, + "timestamp": 6.3827150288738945 + }, + { + "x": 7.830157777215144, + "y": 2.687793357486252, + "heading": -0.6276507250569074, + "angularVelocity": -0.09638490832995429, + "velocityX": 1.905445181152729, + "velocityY": -1.2341848779969686, + "timestamp": 6.446442756123801 + }, + { + "x": 7.930381443513673, + "y": 2.622663002090807, + "heading": -0.6284782322496459, + "angularVelocity": -0.012985041652176142, + "velocityX": 1.5726854012148237, + "velocityY": -1.0220096997973582, + "timestamp": 6.510170483373708 + }, + { + "x": 8.00934183537338, + "y": 2.570977801328826, + "heading": -0.6245889277954535, + "angularVelocity": 0.06103001977366375, + "velocityX": 1.2390272690891182, + "velocityY": -0.8110316026067975, + "timestamp": 6.573898210623614 + }, + { + "x": 8.06700200718074, + "y": 2.5326873953153237, + "heading": -0.6163742376236303, + "angularVelocity": 0.12890292069587517, + "velocityX": 0.9047893953795837, + "velocityY": -0.6008437404859456, + "timestamp": 6.637625937873521 + }, + { + "x": 8.103336152705973, + "y": 2.5077560657950095, + "heading": -0.6041104163963888, + "angularVelocity": 0.19244090063261127, + "velocityX": 0.5701465765874425, + "velocityY": -0.39121636054188286, + "timestamp": 6.701353665123428 + }, + { + "x": 8.118325233459473, + "y": 2.496157169342041, + "heading": -0.5880028838627427, + "angularVelocity": 0.2527554838175985, + "velocityX": 0.23520501044577988, + "velocityY": -0.1820070627575325, + "timestamp": 6.765081392373334 + }, + { + "x": 8.099145036916765, + "y": 2.5050376276993562, + "heading": -0.5592453728804849, + "angularVelocity": 0.33147524461009054, + "velocityX": -0.2210817321633438, + "velocityY": 0.10236115733579505, + "timestamp": 6.851837527106969 + }, + { + "x": 8.040379283371006, + "y": 2.538588966986019, + "heading": -0.5236584542354643, + "angularVelocity": 0.4101948381435164, + "velocityX": -0.6773671248285091, + "velocityY": 0.3867316056630989, + "timestamp": 6.9385936618406046 + }, + { + "x": 7.942028118157561, + "y": 2.596811412402258, + "heading": -0.48124018408036706, + "angularVelocity": 0.4889368375542877, + "velocityX": -1.1336508422765768, + "velocityY": 0.6711046497748767, + "timestamp": 7.02534979657424 + }, + { + "x": 7.804091717790056, + "y": 2.679705216007733, + "heading": -0.4319855329045032, + "angularVelocity": 0.5677368099338319, + "velocityX": -1.5899325251292724, + "velocityY": 0.9554805992680714, + "timestamp": 7.112105931307875 + }, + { + "x": 7.626570292486765, + "y": 2.7872706495099675, + "heading": -0.3758851382218846, + "angularVelocity": 0.6466446995922762, + "velocityX": -2.0462117848879524, + "velocityY": 1.2398596806149733, + "timestamp": 7.19886206604151 + }, + { + "x": 7.409464087423223, + "y": 2.9195079940918838, + "heading": -0.31292409072865957, + "angularVelocity": 0.7257244422718045, + "velocityX": -2.502488218615496, + "velocityY": 1.5242420030343713, + "timestamp": 7.285618200775145 + }, + { + "x": 7.152773379306078, + "y": 3.0764175249240475, + "heading": -0.24308164175732724, + "angularVelocity": 0.8050433457618579, + "velocityX": -2.9587614628666286, + "velocityY": 1.8086274972244747, + "timestamp": 7.3723743355087805 + }, + { + "x": 6.87850975407444, + "y": 3.2473448836663836, + "heading": -0.2430816363695133, + "angularVelocity": 6.210297334840108e-8, + "velocityX": -3.1613167884173445, + "velocityY": 1.9702048652481876, + "timestamp": 7.459130470242416 + }, + { + "x": 6.604246152600556, + "y": 3.418272280529599, + "heading": -0.2430816309817874, + "angularVelocity": 6.210195880646742e-8, + "velocityX": -3.1613165145721047, + "velocityY": 1.9702053046508896, + "timestamp": 7.545886604976051 + }, + { + "x": 6.329982551126931, + "y": 3.5891996773932293, + "heading": -0.24308162559406143, + "angularVelocity": 6.210195950465955e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.9702053046556713, + "timestamp": 7.632642739709686 + }, + { + "x": 6.055718949653306, + "y": 3.76012707425686, + "heading": -0.24308162020633542, + "angularVelocity": 6.210195974983747e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.9702053046556718, + "timestamp": 7.719398874443321 + }, + { + "x": 5.781455348179681, + "y": 3.9310544711204902, + "heading": -0.24308161481860946, + "angularVelocity": 6.210195955610095e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.9702053046556716, + "timestamp": 7.8061550091769565 + }, + { + "x": 5.507191746706056, + "y": 4.1019818679841205, + "heading": -0.24308160943088358, + "angularVelocity": 6.210195862911563e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.970205304655672, + "timestamp": 7.892911143910592 + }, + { + "x": 5.23292814523241, + "y": 4.272909264847718, + "heading": -0.24308160404315773, + "angularVelocity": 6.210195842256675e-8, + "velocityX": -3.1613165145693576, + "velocityY": 1.970205304655297, + "timestamp": 7.979667278644227 + }, + { + "x": 4.958664541898313, + "y": 4.443836658724973, + "heading": -0.24308159865475076, + "angularVelocity": 6.210980925268262e-8, + "velocityX": -3.161316536013975, + "velocityY": 1.970205270233036, + "timestamp": 8.066423413377862 + }, + { + "x": 4.72116068045686, + "y": 4.591872176458927, + "heading": -0.20144636604906935, + "angularVelocity": 0.4799111063847273, + "velocityX": -2.7376030775305313, + "velocityY": 1.7063406315698904, + "timestamp": 8.153179548111497 + }, + { + "x": 4.523240963216832, + "y": 4.715235252344186, + "heading": -0.16672852006699063, + "angularVelocity": 0.40017741786989297, + "velocityX": -2.2813339696114237, + "velocityY": 1.4219521912083464, + "timestamp": 8.239935682845132 + }, + { + "x": 4.364905283308217, + "y": 4.813925798348148, + "heading": -0.1389420316269777, + "angularVelocity": 0.3202826926917032, + "velocityX": -1.8250660935361869, + "velocityY": 1.1375627361335074, + "timestamp": 8.326691817578768 + }, + { + "x": 4.246153560219927, + "y": 4.887943754022767, + "heading": -0.11809715693821225, + "angularVelocity": 0.24026974867846324, + "velocityX": -1.3687991454770243, + "velocityY": 0.853172584300505, + "timestamp": 8.413447952312403 + }, + { + "x": 4.166985746432812, + "y": 4.937289078389794, + "heading": -0.10420012908374074, + "angularVelocity": 0.16018495864458598, + "velocityX": -0.9125327451503197, + "velocityY": 0.5687819601291682, + "timestamp": 8.500204087046038 }, { - "x": 6.349824949515808, - "y": 3.8656935580591254, - "heading": -0.0972519656641754, - "angularVelocity": -0.0000025482713151989977, - "velocityX": -3.393760027773166, - "velocityY": 1.5355833615511918, - "timestamp": 8.565438710881704 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 0.0800779542850017, + "velocityX": -0.45626649675525716, + "velocityY": 0.284391045103359, + "timestamp": 8.586960221779673 }, { - "x": 6.239694739497929, - "y": 3.9155244585127913, - "heading": -0.09725204835760656, - "angularVelocity": -0.00000254827137059863, - "velocityX": -3.3937600277731623, - "velocityY": 1.5355833615511885, - "timestamp": 8.597889505609858 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -4.230647512546783e-26, + "velocityX": -2.5287498438634255e-25, + "velocityY": -6.744044988840678e-26, + "timestamp": 8.673716356513308 + }, + { + "x": 4.14782187195376, + "y": 4.951212056223932, + "heading": -0.09513612979057086, + "angularVelocity": 0.03469517276527526, + "velocityX": 0.3347010425202365, + "velocityY": -0.17619612328674322, + "timestamp": 8.734726155090963 + }, + { + "x": 4.188661957426492, + "y": 4.929712672608232, + "heading": -0.09090255484394089, + "angularVelocity": 0.06939172141736155, + "velocityX": 0.6694020702387493, + "velocityY": -0.3523923061036675, + "timestamp": 8.795735953668617 + }, + { + "x": 4.249922082180478, + "y": 4.897463590700952, + "heading": -0.08455167733440473, + "angularVelocity": 0.10409602486152425, + "velocityX": 1.004103048726068, + "velocityY": -0.5285885654290785, + "timestamp": 8.85674575224627 + }, + { + "x": 4.331602241050322, + "y": 4.85446480491151, + "heading": -0.07608262285012946, + "angularVelocity": 0.13881466062366435, + "velocityX": 1.3388039425483322, + "velocityY": -0.7047849163886669, + "timestamp": 8.917755550823925 + }, + { + "x": 4.433702426596031, + "y": 4.80071630889475, + "heading": -0.06549409960856242, + "angularVelocity": 0.17355446974783373, + "velocityX": 1.67350471442309, + "velocityY": -0.8809813713504947, + "timestamp": 8.978765349401579 + }, + { + "x": 4.5562226288772765, + "y": 4.736218095809515, + "heading": -0.05278437933675508, + "angularVelocity": 0.2083226066650599, + "velocityX": 2.0082053233678367, + "velocityY": -1.0571779384444293, + "timestamp": 9.039775147979233 + }, + { + "x": 4.699162834927203, + "y": 4.6609701587705015, + "heading": -0.03795127726949169, + "angularVelocity": 0.24312655365323685, + "velocityX": 2.3429057197753003, + "velocityY": -1.23337461839406, + "timestamp": 9.100784946556887 + }, + { + "x": 4.862523027168912, + "y": 4.5749724919009225, + "heading": -0.020992138559226412, + "angularVelocity": 0.2779740157423932, + "velocityX": 2.6776058280831623, + "velocityY": -1.4095713946689838, + "timestamp": 9.161794745134541 + }, + { + "x": 5.046303175452206, + "y": 4.478225094831912, + "heading": -0.0019038772119415763, + "angularVelocity": 0.31287205977231325, + "velocityX": 3.012305442204888, + "velocityY": -1.5857681769899301, + "timestamp": 9.222804543712195 + }, + { + "x": 5.247403902929994, + "y": 4.372365264246757, + "heading": -0.0019038737446312225, + "angularVelocity": 5.683202427287112e-8, + "velocityX": 3.2962037601520997, + "velocityY": -1.7351283409076275, + "timestamp": 9.28381434228985 + }, + { + "x": 5.452210430229311, + "y": 4.273866062948841, + "heading": -0.0019038702704038527, + "angularVelocity": 5.69453997654849e-8, + "velocityX": 3.3569448199150136, + "velocityY": -1.614481666785775, + "timestamp": 9.344824140867503 }, { - "x": 6.129564529480049, - "y": 3.9653553589664567, - "heading": -0.09725213105103951, - "angularVelocity": -0.000002548271425616177, - "velocityX": -3.3937600277731588, - "velocityY": 1.535583361551185, - "timestamp": 8.630340300338013 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.0019038667157456788, + "angularVelocity": 5.82637257740846e-8, + "velocityX": 3.5033929561238337, + "velocityY": -1.2656489645038427, + "timestamp": 9.405833939445158 + }, + { + "x": 5.815067334204625, + "y": 4.15419466258127, + "heading": -0.0019038632171495887, + "angularVelocity": 8.405671096693256e-8, + "velocityX": 3.5826277225448937, + "velocityY": -1.020002922483729, + "timestamp": 9.44745579271377 + }, + { + "x": 5.9667648631867, + "y": 4.122168099202404, + "heading": -0.0019038598438992009, + "angularVelocity": 8.104517513702809e-8, + "velocityX": 3.6446606066067324, + "velocityY": -0.7694650973895661, + "timestamp": 9.489077645982382 + }, + { + "x": 6.120315713709697, + "y": 4.100721014443213, + "heading": -0.0019038565242215854, + "angularVelocity": 7.975804426962125e-8, + "velocityX": 3.689188213990329, + "velocityY": -0.5152842335198107, + "timestamp": 9.530699499250995 + }, + { + "x": 6.274166009693199, + "y": 4.08153957617663, + "heading": -0.0019038532098746212, + "angularVelocity": 7.96299708870391e-8, + "velocityX": 3.6963826428056987, + "velocityY": -0.4608501726915756, + "timestamp": 9.572321352519607 + }, + { + "x": 6.4280163353397715, + "y": 4.062358375832809, + "heading": -0.0019038498955278329, + "angularVelocity": 7.962996666259074e-8, + "velocityX": 3.6963833554858816, + "velocityY": -0.46084445639728905, + "timestamp": 9.61394320578822 + }, + { + "x": 6.581866660989341, + "y": 4.043177175513034, + "heading": -0.001903846581181053, + "angularVelocity": 7.962996646753637e-8, + "velocityX": 3.69638335555791, + "velocityY": -0.46084445581955213, + "timestamp": 9.655565059056832 + }, + { + "x": 6.735716986905282, + "y": 4.023995977329788, + "heading": -0.0019038432668342794, + "angularVelocity": 7.962996631106344e-8, + "velocityX": 3.6963833619576825, + "velocityY": -0.4608444044876696, + "timestamp": 9.697186912325444 + }, + { + "x": 6.88956997054089, + "y": 4.004836108474471, + "heading": -0.001903839952467122, + "angularVelocity": 7.963045605311032e-8, + "velocityX": 3.6964472159060078, + "velocityY": -0.4603319494609365, + "timestamp": 9.738808765594056 + }, + { + "x": 7.044365710177195, + "y": 3.9961114372411974, + "heading": -0.0019038360600849265, + "angularVelocity": 9.351775304985098e-8, + "velocityX": 3.7190977210291876, + "velocityY": -0.20961755779992794, + "timestamp": 9.780430618862669 }, { - "x": 6.019434319462169, - "y": 4.015186259420122, - "heading": -0.09725221374447425, - "angularVelocity": -0.0000025482714812860075, - "velocityX": -3.393760027773155, - "velocityY": 1.5355833615511818, - "timestamp": 8.662791095066167 + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -1.3190054521749074e-28, + "angularVelocity": 0.04574126115428558, + "velocityX": 3.664521283033867, + "velocityY": 0.042410578931505474, + "timestamp": 9.822052472131281 + }, + { + "x": 7.411737697803866, + "y": 4.005712621161443, + "heading": 0.0009655084122524671, + "angularVelocity": 0.014666383344284814, + "velocityX": 3.263607500383174, + "velocityY": 0.11903101153279746, + "timestamp": 9.887883862842115 + }, + { + "x": 7.599748268821247, + "y": 4.014968631330143, + "heading": 0.0012837965610470823, + "angularVelocity": 0.004834899359679463, + "velocityX": 2.8559410485983, + "velocityY": 0.1406017717194758, + "timestamp": 9.953715253552948 + }, + { + "x": 7.760889567084852, + "y": 4.024787670821748, + "heading": 0.0012839891285129092, + "angularVelocity": 0.000002925161746511023, + "velocityX": 2.4477881527890566, + "velocityY": 0.1491543682365084, + "timestamp": 10.019546644263782 + }, + { + "x": 7.895156217687927, + "y": 4.03478791644303, + "heading": 0.0011126736496287965, + "angularVelocity": -0.0026023372290070947, + "velocityX": 2.039553610417369, + "velocityY": 0.15190694763243642, + "timestamp": 10.085378034974616 + }, + { + "x": 8.002547604891067, + "y": 4.044753431600936, + "heading": 0.0008527507121031434, + "angularVelocity": -0.003948313026947509, + "velocityX": 1.631309714766018, + "velocityY": 0.15137938072250434, + "timestamp": 10.15120942568545 + }, + { + "x": 8.083064257665658, + "y": 4.054545378094414, + "heading": 0.0005575241657673266, + "angularVelocity": -0.004484586200413281, + "velocityX": 1.2230738543602167, + "velocityY": 0.1487428168803028, + "timestamp": 10.217040816396283 + }, + { + "x": 8.13670697281611, + "y": 4.064066986984161, + "heading": 0.00026414808704782584, + "angularVelocity": -0.004456477002106289, + "velocityX": 0.8148500976696773, + "velocityY": 0.14463630172376873, + "timestamp": 10.282872207107117 }, { - "x": 5.90930410944429, - "y": 4.065017159873787, - "heading": -0.09725229643791082, - "angularVelocity": -0.0000025482715370955637, - "velocityX": -3.3937600277731512, - "velocityY": 1.5355833615511787, - "timestamp": 8.695241889794321 + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -1.276053827019493e-27, + "angularVelocity": -0.004012494407236614, + "velocityX": 0.40663867791392916, + "velocityY": 0.13944668019766568, + "timestamp": 10.34870359781795 + }, + { + "x": 8.158068705558701, + "y": 4.083431829444424, + "heading": -0.000240048236913116, + "angularVelocity": -0.0031208282305151794, + "velocityX": -0.07030667180071454, + "velocityY": 0.13241189095614034, + "timestamp": 10.425621716721755 + }, + { + "x": 8.115975110193533, + "y": 4.093075599420407, + "heading": -0.0004115111549292854, + "angularVelocity": -0.0022291616131513174, + "velocityX": -0.5472520124655176, + "velocityY": 0.12537709077419898, + "timestamp": 10.50253983562556 + }, + { + "x": 8.037195777332599, + "y": 4.102178264678885, + "heading": -0.000514388747142335, + "angularVelocity": -0.0013374949060014165, + "velocityX": -1.0241973410641676, + "velocityY": 0.11834227602292711, + "timestamp": 10.579457954529365 + }, + { + "x": 7.921730708275182, + "y": 4.110739823655246, + "heading": -0.0005486810396251135, + "angularVelocity": -0.00044582853782039575, + "velocityX": -1.5011426527710625, + "velocityY": 0.1113074409303779, + "timestamp": 10.65637607343317 + }, + { + "x": 7.769579904970056, + "y": 4.118760274012496, + "heading": -0.0005143881026246048, + "angularVelocity": 0.00044583691709096753, + "velocityX": -1.9780879391422628, + "velocityY": 0.1042725754549578, + "timestamp": 10.733294192336976 + }, + { + "x": 7.58074337066483, + "y": 4.126239611878995, + "heading": -0.00041151007577159475, + "angularVelocity": 0.0013375005566851135, + "velocityX": -2.4550331832918157, + "velocityY": 0.09723765964495061, + "timestamp": 10.81021231124078 + }, + { + "x": 7.355221111853749, + "y": 4.133177829577417, + "heading": -0.0002400472412179255, + "angularVelocity": 0.0022291605280688772, + "velocityX": -2.9319783430107655, + "velocityY": 0.09020264402331184, + "timestamp": 10.887130430144586 + }, + { + "x": 7.093013148015058, + "y": 4.139574904378433, + "heading": -3.6404571271115587e-10, + "angularVelocity": 0.0031208105527440306, + "velocityX": -3.4089232494961896, + "velocityY": 0.08316733290131334, + "timestamp": 10.96404854904839 + }, + { + "x": 6.806524254043931, + "y": 4.135349685277542, + "heading": -2.9271426416436637e-10, + "angularVelocity": 9.273686039566091e-10, + "velocityX": -3.7245956876482667, + "velocityY": -0.05493138887308735, + "timestamp": 11.040966667952196 + }, + { + "x": 6.520035365230175, + "y": 4.131124116490654, + "heading": -2.213841067469342e-10, + "angularVelocity": 9.273518181930497e-10, + "velocityX": -3.724595620598135, + "velocityY": -0.05493593508406917, + "timestamp": 11.117884786856001 + }, + { + "x": 6.2335464689880125, + "y": 4.126899051371689, + "heading": -1.5005388112258718e-10, + "angularVelocity": 9.273527049400969e-10, + "velocityX": -3.724595717173642, + "velocityY": -0.05492938697902426, + "timestamp": 11.194802905759806 + }, + { + "x": 5.947511615310419, + "y": 4.143566419634921, + "heading": -7.76428044505519e-11, + "angularVelocity": 9.414046742691968e-10, + "velocityX": -3.718692783365069, + "velocityY": 0.21668975399771281, + "timestamp": 11.271721024663611 }, { - "x": 5.79917389942641, - "y": 4.114848060327453, - "heading": -0.09725237913134915, - "angularVelocity": -0.0000025482715921357367, - "velocityX": -3.3937600277731477, - "velocityY": 1.5355833615511751, - "timestamp": 8.727692684522475 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 8.328200008246131e-28, + "angularVelocity": 1.0094215193646874e-9, + "velocityX": -3.6605144600811688, + "velocityY": 0.6901189950564038, + "timestamp": 11.348639143567416 + }, + { + "x": 5.429536691787251, + "y": 4.268976766885781, + "heading": -7.1547225200391066e-9, + "angularVelocity": -1.0779919590068591e-7, + "velocityX": -3.5620320452215446, + "velocityY": 1.089751147278287, + "timestamp": 11.415009977415107 + }, + { + "x": 5.202544249135573, + "y": 4.366945110997213, + "heading": -1.3874386645688143e-8, + "angularVelocity": -1.0124423238480708e-7, + "velocityX": -3.4200631435878064, + "velocityY": 1.4760752341344967, + "timestamp": 11.481380811262797 + }, + { + "x": 4.987571628122558, + "y": 4.485916838228238, + "heading": -0.004584937869464128, + "angularVelocity": -0.06908040368453193, + "velocityX": -3.2389621849009536, + "velocityY": 1.7925302476091356, + "timestamp": 11.547751645110488 + }, + { + "x": 4.796407538252732, + "y": 4.591709293955959, + "heading": -0.018829281267826372, + "angularVelocity": -0.21461751454035435, + "velocityX": -2.8802424014818646, + "velocityY": 1.593960021211954, + "timestamp": 11.614122478958178 + }, + { + "x": 4.629141619838775, + "y": 4.684277836657996, + "heading": -0.035106228890285536, + "angularVelocity": -0.24524247593169948, + "velocityX": -2.5201720201045186, + "velocityY": 1.3947171872883817, + "timestamp": 11.680493312805869 + }, + { + "x": 4.4857776191021586, + "y": 4.763619927271578, + "heading": -0.051033661297585665, + "angularVelocity": -0.2399763794417698, + "velocityX": -2.160045194936232, + "velocityY": 1.1954360976638891, + "timestamp": 11.746864146653559 + }, + { + "x": 4.366313131027418, + "y": 4.829736226023804, + "heading": -0.06545154271306548, + "angularVelocity": -0.21723218738770492, + "velocityX": -1.7999546057970206, + "velocityY": 0.9961649555880353, + "timestamp": 11.81323498050125 + }, + { + "x": 4.27074543266692, + "y": 4.88262772178767, + "heading": -0.07767440134084834, + "angularVelocity": -0.18416008838810272, + "velocityX": -1.4399050429260685, + "velocityY": 0.7969087127222595, + "timestamp": 11.87960581434894 + }, + { + "x": 4.199072197330148, + "y": 4.922295318430048, + "heading": -0.08724968829906798, + "angularVelocity": -0.1442694991627378, + "velocityX": -1.0798905359732165, + "velocityY": 0.5976660882912562, + "timestamp": 11.94597664819663 + }, + { + "x": 4.15129151783854, + "y": 4.948739780760436, + "heading": -0.09385601929230014, + "angularVelocity": -0.09953665805062048, + "velocityX": -0.7199047642109825, + "velocityY": 0.3984349871371742, + "timestamp": 12.012347482044321 }, { - "x": 5.689043689408531, - "y": 4.164678960781118, - "heading": -0.09725246182478928, - "angularVelocity": -0.0000025482716465928655, - "velocityX": -3.3937600277731437, - "velocityY": 1.5355833615511723, - "timestamp": 8.76014347925063 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.05117995064055202, + "velocityX": -0.3599425785050827, + "velocityY": 0.19921349015633877, + "timestamp": 12.078718315892012 }, { - "x": 5.5789134793906525, - "y": 4.2145098612347835, - "heading": -0.09725254451823122, - "angularVelocity": -0.0000025482717030707046, - "velocityX": -3.39376002777314, - "velocityY": 1.535583361551169, - "timestamp": 8.792594273978784 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -1.5406552461687784e-28, + "velocityX": 2.4367660918840807e-27, + "velocityY": 6.301744727068757e-27, + "timestamp": 12.145089149739702 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 5.468783269372774, - "y": 4.264340761688448, - "heading": -0.09725262721167492, - "angularVelocity": -0.000002548271757487181, - "velocityX": -3.3937600277731357, - "velocityY": 1.535583361551166, - "timestamp": 8.825045068706938 + "timestamp": 1.0690213480650517, + "isStopPoint": false, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 5.358653059354908, - "y": 4.314171662142141, - "heading": -0.09725270990512043, - "angularVelocity": -0.000002548271812724623, - "velocityX": -3.3937600277727378, - "velocityY": 1.5355833615520338, - "timestamp": 8.857495863435092 + "timestamp": 2.091506029241452, + "isStopPoint": true, + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 5.248522849397515, - "y": 4.364002562729485, - "heading": -0.09725279259856773, - "angularVelocity": -0.000002548271867828041, - "velocityX": -3.393760025909188, - "velocityY": 1.5355833656706142, - "timestamp": 8.889946658163247 + "timestamp": 2.8723893187704173, + "isStopPoint": true, + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 5.138392925262451, - "y": 4.413834095001221, - "heading": -0.09725287529259724, - "angularVelocity": -0.000002548289809515153, - "velocityX": -3.3937512180408453, - "velocityY": 1.5356028315849852, - "timestamp": 8.922397452891401 + "timestamp": 3.795315905146558, + "isStopPoint": false, + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { - "x": 4.9137282295128415, - "y": 4.535640155229253, - "heading": -0.09725287529313524, - "angularVelocity": -7.495790555066974e-12, - "velocityX": -3.1302347498710463, - "velocityY": 1.6971138308958609, - "timestamp": 8.994169928025691 + "timestamp": 4.2856991534854645, + "isStopPoint": true, + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 4.717146626097375, - "y": 4.642220517268037, - "heading": -0.09725287529322811, - "angularVelocity": -1.2941727290342862e-12, - "velocityX": -2.738955331380901, - "velocityY": 1.4849754288028527, - "timestamp": 9.065942403159982 + "timestamp": 5.223887683661428, + "isStopPoint": false, + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 + }, + { + "timestamp": 5.6801112700594025, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 }, { - "x": 4.5486481109529855, - "y": 4.733575136181824, - "heading": -0.09725287529316204, - "angularVelocity": 9.207220397125275e-13, - "velocityX": -2.347675969501116, - "velocityY": 1.2728364006237491, - "timestamp": 9.137714878294272 + "timestamp": 6.255259574374081, + "isStopPoint": false, + "x": 7.402774333953857, + "y": 2.964965343475342, + "heading": -0.5880028838627427, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 4.408232682725307, - "y": 4.809703996992033, - "heading": -0.09725287529303236, - "angularVelocity": 1.806653851545192e-12, - "velocityX": -1.9563966264916148, - "velocityY": 1.0606971637493046, - "timestamp": 9.209487353428562 + "timestamp": 6.765081392373334, + "isStopPoint": false, + "x": 8.118325233459473, + "y": 2.496157169342041, + "heading": -0.5880028838627427, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 }, { - "x": 4.2959003407371545, - "y": 4.870607092209374, - "heading": -0.09725287529288679, - "angularVelocity": 2.028301137290635e-12, - "velocityX": -1.5651172929172836, - "velocityY": 0.8485578225272127, - "timestamp": 9.281259828562852 + "timestamp": 8.673716356513308, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 4.211651084582215, - "y": 4.916284417340276, - "heading": -0.09725287529275393, - "angularVelocity": 1.850997831523441e-12, - "velocityX": -1.1738379650040627, - "velocityY": 0.6364184186965437, - "timestamp": 9.353032303697143 + "timestamp": 9.405833939445158, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 }, { - "x": 4.155484913989614, - "y": 4.946735969389024, - "heading": -0.09725287529265289, - "angularVelocity": 1.4078899342405594e-12, - "velocityX": -0.7825586408649186, - "velocityY": 0.424278973126829, - "timestamp": 9.424804778831433 + "timestamp": 9.822052472131281, + "isStopPoint": false, + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 4.127401828765869, - "y": 4.96196174621582, - "heading": -0.09725287529259724, - "angularVelocity": 7.752391774433126e-13, - "velocityX": -0.3912793194215449, - "velocityY": 0.2121394977435137, - "timestamp": 9.496577253965723 + "timestamp": 10.34870359781795, + "isStopPoint": false, + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 11.348639143567416, + "isStopPoint": false, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { + "timestamp": 12.145089149739702, + "isStopPoint": true, "x": 4.127401828765869, "y": 4.96196174621582, - "heading": -0.09725287529259724, - "angularVelocity": -8.979320165657004e-28, - "velocityX": -3.317577006729804e-27, - "velocityY": -2.7701124132227202e-27, - "timestamp": 9.568349729100014 - }, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ { - "x": 4.149345287192321, - "y": 4.948730090302555, - "heading": -0.09725287542061174, - "angularVelocity": -1.9914932525911006e-9, - "velocityX": 0.34136955009475556, - "velocityY": -0.20584195701235133, - "timestamp": 9.632630384537388 + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 4.193331867161711, - "y": 4.922433486987429, - "heading": -0.09725287565526207, - "angularVelocity": -3.6504037180002943e-9, - "velocityX": 0.6842895373437039, - "velocityY": -0.4090904664272751, - "timestamp": 9.696911039974761 + "scope": [ + "last" + ], + "type": "StopPoint" }, { - "x": 4.259487364434434, - "y": 4.883286547964955, - "heading": -0.09725287596919822, - "angularVelocity": -4.883835550017299e-9, - "velocityX": 1.02916650153289, - "velocityY": -0.6090003089749592, - "timestamp": 9.761191695412135 + "scope": [ + 3 + ], + "type": "StopPoint" }, { - "x": 4.347975400700828, - "y": 4.831575784808056, - "heading": -0.09725287632620161, - "angularVelocity": -5.55382285919441e-9, - "velocityX": 1.3765888923239984, - "velocityY": -0.8044529540816497, - "timestamp": 9.825472350849509 + "scope": [ + 5 + ], + "type": "StopPoint" }, { - "x": 4.45901715765165, - "y": 4.76770271955414, - "heading": -0.09725287667606397, - "angularVelocity": -5.442731637489258e-9, - "velocityX": 1.7274521579669766, - "velocityY": -0.9936592092802461, - "timestamp": 9.889753006286883 + "scope": [ + 2 + ], + "type": "StopPoint" }, { - "x": 4.592927185358519, - "y": 4.69226963680943, - "heading": -0.09725287694462323, - "angularVelocity": -4.177917358984547e-9, - "velocityX": 2.083208809800217, - "velocityY": -1.1734958555019974, - "timestamp": 9.954033661724257 + "scope": [ + 4 + ], + "type": "WptVelocityDirection", + "direction": 0.5125504196 }, { - "x": 4.750184246245984, - "y": 4.606276833638444, - "heading": -0.09725287701156489, - "angularVelocity": -1.0413967703177253e-9, - "velocityX": 2.446413463233511, - "velocityY": -1.3377711005881947, - "timestamp": 10.01831431716163 - }, + "scope": [ + 10 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "CenterLanePCBAEF": { + "waypoints": [ { - "x": 4.931581310093376, - "y": 4.511690334370361, - "heading": -0.0972528766497097, - "angularVelocity": 5.629301248871626e-9, - "velocityX": 2.821954172886779, - "velocityY": -1.4714613381662662, - "timestamp": 10.082594972599004 + "x": 1.2899744510650635, + "y": 5.590954303741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 5.138392925262451, - "y": 4.413834095001221, - "heading": -0.09725287529259724, - "angularVelocity": 2.1112299822496666e-8, - "velocityX": 3.2173227507078654, - "velocityY": -1.5223279648179473, - "timestamp": 10.146875628036378 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 5.398868955921396, - "y": 4.32613897098341, - "heading": -0.09725286997797288, - "angularVelocity": 7.20303645148975e-8, - "velocityX": 3.5302934161705415, - "velocityY": -1.1885528129676433, - "timestamp": 10.220658738943204 + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 5.6681314885139065, - "y": 4.271040046607528, - "heading": -0.09725286670603911, - "angularVelocity": 4.4345294143454955e-8, - "velocityX": 3.6493789606207083, - "velocityY": -0.7467687889368465, - "timestamp": 10.29444184985003 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 5.942117714678701, - "y": 4.249366302589977, - "heading": -0.0972528635577222, - "angularVelocity": 4.26698858409276e-8, - "velocityX": 3.713400299843557, - "velocityY": -0.29374939266142885, - "timestamp": 10.368224960756855 + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 }, { - "x": 6.216382330790907, - "y": 4.231558335942029, - "heading": -0.09725286041527656, - "angularVelocity": 4.259031080949294e-8, - "velocityX": 3.7171733848218165, - "velocityY": -0.24135559519084493, - "timestamp": 10.44200807166368 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "x": 6.4906469514050675, - "y": 4.21375043862986, - "heading": -0.09725285727283085, - "angularVelocity": 4.259031210936323e-8, - "velocityX": 3.717173445837876, - "velocityY": -0.241354655466577, - "timestamp": 10.515791182570506 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 }, { - "x": 6.764911572019293, - "y": 4.195942541318692, - "heading": -0.09725285413038519, - "angularVelocity": 4.259031138372869e-8, - "velocityX": 3.717173445838756, - "velocityY": -0.24135465545301274, - "timestamp": 10.589574293477332 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 }, { - "x": 7.039176191804645, - "y": 4.178134631242245, - "heading": -0.09725285098790416, - "angularVelocity": 4.2590790558228534e-8, - "velocityX": 3.717173434604845, - "velocityY": -0.24135482846385797, - "timestamp": 10.663357404384158 + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 7.303151625715878, - "y": 4.160355738175747, - "heading": -0.07493794888540253, - "angularVelocity": 0.3024391602392781, - "velocityX": 3.5777216583424782, - "velocityY": -0.24096155404655678, - "timestamp": 10.737140515290983 + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 }, { - "x": 7.533445930864736, - "y": 4.144763761571347, - "heading": -0.05552477724704528, - "angularVelocity": 0.26311131910489216, - "velocityX": 3.1212333326481616, - "velocityY": -0.21132175660214023, - "timestamp": 10.810923626197809 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 7.730059092320318, - "y": 4.131358730965977, - "heading": -0.03901370974052792, - "angularVelocity": 0.22377841356361258, - "velocityX": 2.664744804591801, - "velocityY": -0.1816815588366564, - "timestamp": 10.884706737104635 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 7.892991106532594, - "y": 4.120140669647355, - "heading": -0.02540508118930727, - "angularVelocity": 0.18444097008061852, - "velocityX": 2.20825622842103, - "velocityY": -0.15204104544723035, - "timestamp": 10.95848984801146 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 19 }, { - "x": 8.02224197249394, - "y": 4.111109595697515, - "heading": -0.014699203415963554, - "angularVelocity": 0.14509930039224503, - "velocityX": 1.7517676385937064, - "velocityY": -0.12240028698767837, - "timestamp": 11.032272958918286 + "x": 8.220098495483398, + "y": 5.723839282989502, + "heading": 0.9263929393086, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 8.117811689845258, - "y": 4.104265522227424, - "heading": -0.0068963232026660135, - "angularVelocity": 0.10575428600660669, - "velocityX": 1.2952790438994197, - "velocityY": -0.09275935083211628, - "timestamp": 11.106056069825112 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 }, { - "x": 8.179700258175622, - "y": 4.099608457473234, - "heading": -0.001996583897562948, - "angularVelocity": 0.06640732878951901, - "velocityX": 0.8387904436357886, - "velocityY": -0.06311830305000711, - "timestamp": 11.179839180731937 - }, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ { - "x": 8.207907676696777, - "y": 4.097138404846191, - "heading": 5.455800259144416e-28, - "angularVelocity": 0.027060175059361254, - "velocityX": 0.38230183268875023, - "velocityY": -0.03347720903449647, - "timestamp": 11.253622291638763 + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -8.290804854356393e-28, + "angularVelocity": -1.4522370736587165e-27, + "velocityX": 4.9023959214285697e-26, + "velocityY": -8.208515528255786e-26, + "timestamp": 0 }, { - "x": 8.200639571345327, - "y": 4.096943687462525, - "heading": -0.0010802899918473188, - "angularVelocity": -0.0140223298346035, - "velocityX": -0.0943411225502494, - "velocityY": -0.002527461513954009, - "timestamp": 11.330662983491656 + "x": 1.310478129945639, + "y": 5.567275491596602, + "heading": -0.01363283868540513, + "angularVelocity": -0.19128951975675812, + "velocityX": 0.2876978871987184, + "velocityY": -0.332249849655198, + "timestamp": 0.07126808987100033 + }, + { + "x": 1.3514857086951306, + "y": 5.519918181736233, + "heading": -0.04088194207773102, + "angularVelocity": -0.38234648131651156, + "velocityX": 0.5753988752009218, + "velocityY": -0.6644952873872267, + "timestamp": 0.14253617974200067 + }, + { + "x": 1.4129978772139058, + "y": 5.448882692998019, + "heading": -0.08170986553530027, + "angularVelocity": -0.5728780374424279, + "velocityX": 0.8631095435575179, + "velocityY": -0.9967362513404223, + "timestamp": 0.213804269613001 + }, + { + "x": 1.495015808643516, + "y": 5.354169496620742, + "heading": -0.13606373855442885, + "angularVelocity": -0.7626677397628091, + "velocityX": 1.1508366728793693, + "velocityY": -1.3289706030947979, + "timestamp": 0.28507235948400134 + }, + { + "x": 1.59754112837037, + "y": 5.235779380543083, + "heading": -0.20388316047242275, + "angularVelocity": -0.9516099286616388, + "velocityX": 1.4385866088516093, + "velocityY": -1.6611938988676576, + "timestamp": 0.35634044935500164 + }, + { + "x": 1.7205758185191462, + "y": 5.093713591292308, + "heading": -0.28510990320243984, + "angularVelocity": -1.1397350886917637, + "velocityX": 1.7263643570562468, + "velocityY": -1.99339970396181, + "timestamp": 0.42760853922600195 + }, + { + "x": 1.864122091418882, + "y": 4.927973933585744, + "heading": -0.3796975284198671, + "angularVelocity": -1.3272086481991685, + "velocityX": 2.0141731476115536, + "velocityY": -2.325580186119234, + "timestamp": 0.49887662909700226 + }, + { + "x": 2.0360573445839716, + "y": 4.766214838118459, + "heading": -0.469084063757855, + "angularVelocity": -1.2542294244139716, + "velocityX": 2.4125138400131556, + "velocityY": -2.2697268266917123, + "timestamp": 0.5701447189680026 + }, + { + "x": 2.1874785084173087, + "y": 4.628126850696153, + "heading": -0.5451442530827014, + "angularVelocity": -1.0672404643160782, + "velocityX": 2.1246698783062508, + "velocityY": -1.937585077308154, + "timestamp": 0.6414128088390029 + }, + { + "x": 2.3183845436377304, + "y": 4.513708759426245, + "heading": -0.6078811711978229, + "angularVelocity": -0.8802946483998556, + "velocityX": 1.8368113338995, + "velocityY": -1.6054603326258816, + "timestamp": 0.7126808987100032 + }, + { + "x": 2.428774775812145, + "y": 4.422959692661252, + "heading": -0.657294007593887, + "angularVelocity": -0.6933374598014952, + "velocityX": 1.5489433261678196, + "velocityY": -1.273347818487273, + "timestamp": 0.7839489885810035 + }, + { + "x": 2.518648765365177, + "y": 4.355879041329867, + "heading": -0.6933804923945769, + "angularVelocity": -0.5063484213763644, + "velocityX": 1.2610691505231797, + "velocityY": -0.9412438505480457, + "timestamp": 0.8552170784520038 + }, + { + "x": 2.588006261722225, + "y": 4.3124664184627175, + "heading": -0.7161379527262285, + "angularVelocity": -0.3193218784570213, + "velocityX": 0.9731914589346954, + "velocityY": -0.6091453123793419, + "timestamp": 0.9264851683230041 + }, + { + "x": 2.6368471855998425, + "y": 4.2927216346124935, + "heading": -0.7255636953209265, + "angularVelocity": -0.13225754488101377, + "velocityX": 0.6853126548785442, + "velocityY": -0.2770494324453428, + "timestamp": 0.9977532581940044 }, { - "x": 8.15665056358688, - "y": 4.099133352364744, - "heading": -0.0053255986809540795, - "angularVelocity": -0.05510475810903991, - "velocityX": -0.5709840695932866, - "velocityY": 0.028422186373926028, - "timestamp": 11.40770367534455 + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05484334197651896, + "velocityX": 0.3974350607881677, + "velocityY": 0.05504641764912788, + "timestamp": 1.0690213480650048 + }, + { + "x": 2.6726522356618694, + "y": 4.325520885096074, + "heading": -0.7036434508654623, + "angularVelocity": 0.24661816834169437, + "velocityX": 0.10242556780992029, + "velocityY": 0.395376841972494, + "timestamp": 1.1420559681490368 + }, + { + "x": 2.658587249318106, + "y": 4.379253606612043, + "heading": -0.671632044096526, + "angularVelocity": 0.43830455655283357, + "velocityX": -0.19257971531283274, + "velocityY": 0.7357157667712343, + "timestamp": 1.2150905882330687 + }, + { + "x": 2.6229771758212266, + "y": 4.457843745767866, + "heading": -0.6256270362766762, + "angularVelocity": 0.6299068546795684, + "velocityX": -0.487577993229883, + "velocityY": 1.0760669264165268, + "timestamp": 1.2881252083171006 + }, + { + "x": 2.5658227847056265, + "y": 4.561292508951193, + "heading": -0.5656323531358255, + "angularVelocity": 0.8214554011758, + "velocityX": -0.7825657345768313, + "velocityY": 1.4164346040864118, + "timestamp": 1.3611598284011326 + }, + { + "x": 2.4871252028275452, + "y": 4.6896014337350325, + "heading": -0.4916452107859868, + "angularVelocity": 1.0130420650468346, + "velocityX": -1.077538046854127, + "velocityY": 1.7568233344160629, + "timestamp": 1.4341944484851645 + }, + { + "x": 2.3868860574552526, + "y": 4.8427723316162075, + "heading": -0.4036473210602506, + "angularVelocity": 1.2048791329986765, + "velocityX": -1.3724880783518802, + "velocityY": 2.097236868007809, + "timestamp": 1.5072290685691965 + }, + { + "x": 2.2651076003378585, + "y": 5.020807045399031, + "heading": -0.30159033464974355, + "angularVelocity": 1.3973782062956255, + "velocityX": -1.6674072785930696, + "velocityY": 2.4376756335280563, + "timestamp": 1.5802636886532284 + }, + { + "x": 2.1359139807220826, + "y": 5.170004552864557, + "heading": -0.21560374484097594, + "angularVelocity": 1.1773401396465593, + "velocityX": -1.7689366969682099, + "velocityY": 2.0428326633843468, + "timestamp": 1.6532983087372604 + }, + { + "x": 2.0282550130804604, + "y": 5.294335941405344, + "heading": -0.14382079923066451, + "angularVelocity": 0.9828619020365901, + "velocityX": -1.4740812989477097, + "velocityY": 1.7023623645571657, + "timestamp": 1.7263329288212923 + }, + { + "x": 1.9421291374446024, + "y": 5.393801086818423, + "heading": -0.08631852593947899, + "angularVelocity": 0.7873289848708013, + "velocityX": -1.1792472602275945, + "velocityY": 1.3618903651259822, + "timestamp": 1.7993675489053242 + }, + { + "x": 1.8775352248155426, + "y": 5.468400016112322, + "heading": -0.04315881325455388, + "angularVelocity": 0.590948684819152, + "velocityX": -0.8844286799156282, + "velocityY": 1.0214187354992301, + "timestamp": 1.8724021689893562 + }, + { + "x": 1.83447263299525, + "y": 5.518132767413434, + "heading": -0.014381451378905535, + "angularVelocity": 0.39402357186958425, + "velocityX": -0.5896188926668684, + "velocityY": 0.6809476279042802, + "timestamp": 1.9454367890733881 }, { - "x": 8.075940653456616, - "y": 4.103707386175576, - "heading": -0.012735784669566218, - "angularVelocity": -0.09618535101893501, - "velocityX": -1.0476270161796533, - "velocityY": 0.05937166062275022, - "timestamp": 11.484744367197443 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -8.684305119206267e-26, + "angularVelocity": 0.1969127978259977, + "velocityX": -0.2948114112163433, + "velocityY": 0.34047551881670374, + "timestamp": 2.0184714091574203 }, { - "x": 7.958509840643352, - "y": 4.110665769785427, - "heading": -0.023310616019513113, - "angularVelocity": -0.13726293333579098, - "velocityX": -1.5242699668052715, - "velocityY": 0.0903208868261235, - "timestamp": 11.561785059050337 + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -4.2442319768143513e-26, + "angularVelocity": 2.0248829096214925e-26, + "velocityX": -2.1409957925018854e-22, + "velocityY": -3.6446450287003043e-25, + "timestamp": 2.0915060292414522 + }, + { + "x": 1.8720264471920118, + "y": 5.543082773692806, + "heading": 1.803287488569909e-20, + "angularVelocity": 1.8474379021994642e-19, + "velocityX": 0.6053171264251317, + "velocityY": 0.0008555041789240009, + "timestamp": 2.189116440432573 + }, + { + "x": 1.9901969516410831, + "y": 5.543249785918246, + "heading": 2.6710707578067816e-20, + "angularVelocity": 8.890273677603703e-20, + "velocityX": 1.210634224434258, + "velocityY": 0.0017110083176872164, + "timestamp": 2.2867268516236936 + }, + { + "x": 2.1674527004558795, + "y": 5.543500304245301, + "heading": 2.745376939135264e-20, + "angularVelocity": 7.612526210817117e-21, + "velocityX": 1.8159512561393767, + "velocityY": 0.0025665123627419384, + "timestamp": 2.3843372628148143 + }, + { + "x": 2.4037936612766035, + "y": 5.543834328628234, + "heading": 2.868215450901532e-21, + "angularVelocity": -2.5187429947361393e-19, + "velocityX": 2.4212679563245554, + "velocityY": 0.0034220159392543354, + "timestamp": 2.481947674005935 + }, + { + "x": 2.5810494100914, + "y": 5.544084846955289, + "heading": 4.197057323633921e-22, + "angularVelocity": -2.508451391919409e-20, + "velocityX": 1.815951256139377, + "velocityY": 0.0025665123627419384, + "timestamp": 2.5795580851970557 + }, + { + "x": 2.699219914540471, + "y": 5.544251859180729, + "heading": 1.3007601681932514e-20, + "angularVelocity": 1.2896058726029048e-19, + "velocityX": 1.2106342244342583, + "velocityY": 0.0017110083176872164, + "timestamp": 2.6771684963881763 }, { - "x": 7.804358124937036, - "y": 4.120008478318513, - "heading": -0.037049836652876506, - "angularVelocity": -0.17833719172197993, - "velocityX": -2.000912920157348, - "velocityY": 0.12126979013798393, - "timestamp": 11.63882575090323 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.0578906521828803e-28, + "angularVelocity": -1.3326039434355583e-19, + "velocityX": 0.6053171264251317, + "velocityY": 0.0008555041789240007, + "timestamp": 2.774778907579297 }, { - "x": 7.613485507099167, - "y": 4.131735481105862, - "heading": -0.05395325351914213, - "angularVelocity": -0.21940894428287547, - "velocityX": -2.477555863625089, - "velocityY": 0.15221829536189968, - "timestamp": 11.715866442756123 + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.171991433780983e-28, + "angularVelocity": -1.132446733749095e-28, + "velocityX": 2.142784500904642e-22, + "velocityY": 2.9856908340513677e-25, + "timestamp": 2.8723893187704177 + }, + { + "x": 2.7303741375078747, + "y": 5.568056872076371, + "heading": 0.006778509195380263, + "angularVelocity": 0.08813497362119757, + "velocityX": -0.36316254475214277, + "velocityY": 0.3084298205009288, + "timestamp": 2.949299867635096 + }, + { + "x": 2.674756164732603, + "y": 5.615779956875509, + "heading": 0.02059740285114023, + "angularVelocity": 0.17967488023097908, + "velocityX": -0.7231514219607195, + "velocityY": 0.6205011601608276, + "timestamp": 3.0262104164997745 + }, + { + "x": 2.591861664751003, + "y": 5.687962530350053, + "heading": 0.041894082151838305, + "angularVelocity": 0.2769019284749726, + "velocityX": -1.07780403605558, + "velocityY": 0.9385263080302103, + "timestamp": 3.103120965364453 + }, + { + "x": 2.4825229880731845, + "y": 5.785486691208239, + "heading": 0.07152941986388306, + "angularVelocity": 0.38532214565504064, + "velocityX": -1.4216343309445878, + "velocityY": 1.2680206070272069, + "timestamp": 3.1800315142291313 + }, + { + "x": 2.3492932233323143, + "y": 5.910724338795779, + "heading": 0.11194462213648768, + "angularVelocity": 0.5254832122406747, + "velocityX": -1.7322690672157808, + "velocityY": 1.628354620221102, + "timestamp": 3.2569420630938097 + }, + { + "x": 2.2322888283047253, + "y": 6.068263103797439, + "heading": 0.1780934351870072, + "angularVelocity": 0.8600746455067713, + "velocityX": -1.5213049023152154, + "velocityY": 2.0483375470229004, + "timestamp": 3.333852611958488 + }, + { + "x": 2.1483049194140715, + "y": 6.209825434589826, + "heading": 0.24392342400918363, + "angularVelocity": 0.8559292554003495, + "velocityX": -1.0919686587911486, + "velocityY": 1.8406100708169557, + "timestamp": 3.4107631608231666 + }, + { + "x": 2.0954357223517674, + "y": 6.331973751888774, + "heading": 0.3066259975705838, + "angularVelocity": 0.815266234437141, + "velocityX": -0.6874115169211118, + "velocityY": 1.5881867845445201, + "timestamp": 3.487673709687845 + }, + { + "x": 2.072970005140484, + "y": 6.433621819948826, + "heading": 0.3652444947638923, + "angularVelocity": 0.7621645932659495, + "velocityX": -0.2921018968517744, + "velocityY": 1.3216401333827206, + "timestamp": 3.5645842585525234 + }, + { + "x": 2.080539806403903, + "y": 6.514239906298576, + "heading": 0.4192892776661922, + "angularVelocity": 0.7026966222460566, + "velocityX": 0.09842344613530404, + "velocityY": 1.0482058383381783, + "timestamp": 3.6414948074172018 + }, + { + "x": 2.1179206829564796, + "y": 6.573514734989947, + "heading": 0.46845944965014935, + "angularVelocity": 0.6393163578961119, + "velocityX": 0.48603055243236887, + "velocityY": 0.7706982925796543, + "timestamp": 3.71840535628188 }, { - "x": 7.385891991055361, - "y": 4.145846741676364, - "heading": -0.07402084166347275, - "angularVelocity": -0.2604803729261552, - "velocityX": -2.954198756137707, - "velocityY": 0.18316632718520298, - "timestamp": 11.792907134609017 + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.5732759758018011, + "velocityX": 0.871673332481117, + "velocityY": 0.4905009632033997, + "timestamp": 3.7953159051465586 }, { - "x": 7.121577593711165, - "y": 4.162342217760912, - "heading": -0.09725284930983928, - "angularVelocity": -0.30155502355466035, - "velocityX": -3.4308414292137566, - "velocityY": 0.214113810349038, - "timestamp": 11.86994782646191 + "x": 2.2917884862212223, + "y": 6.6722766025245654, + "heading": 0.5461831512487826, + "angularVelocity": 0.4115075108626725, + "velocityX": 1.3070625376553004, + "velocityY": 0.7468097995934098, + "timestamp": 3.8770464465363763 }, { - "x": 6.835203978246458, - "y": 4.180936324910101, - "heading": -0.09725285259123628, - "angularVelocity": -4.2593036391257635e-8, - "velocityX": -3.717173464790893, - "velocityY": 0.24135436354456943, - "timestamp": 11.946988518314804 + "x": 2.4341165316393667, + "y": 6.754285323763742, + "heading": 0.5638405564624906, + "angularVelocity": 0.21604414841069292, + "velocityX": 1.741430351468047, + "velocityY": 1.003403621762784, + "timestamp": 3.958776987926194 }, { - "x": 6.5488303639215975, - "y": 4.199530449615619, - "heading": -0.09725285587256693, - "angularVelocity": -4.259217541458275e-8, - "velocityX": -3.717173449995519, - "velocityY": 0.2413545914284215, - "timestamp": 12.024029210167697 + "x": 2.541260883257264, + "y": 6.816945409398408, + "heading": 0.5395665848363949, + "angularVelocity": -0.2970000101959424, + "velocityX": 1.3109463096159768, + "velocityY": 0.7666667144146957, + "timestamp": 4.040507529316012 }, { - "x": 6.262456749596831, - "y": 4.218124574322583, - "heading": -0.0972528591538976, - "angularVelocity": -4.259217545578449e-8, - "velocityX": -3.7171734499942994, - "velocityY": 0.2413545914471941, - "timestamp": 12.10106990202059 + "x": 2.6126555762150003, + "y": 6.858741818277979, + "heading": 0.521795194780163, + "angularVelocity": -0.21743879037176994, + "velocityX": 0.8735375019384195, + "velocityY": 0.5113927813131806, + "timestamp": 4.12223807070583 }, { - "x": 5.976083139919176, - "y": 4.236718770600949, - "heading": -0.09725286243522821, - "angularVelocity": -4.259217464314882e-8, - "velocityX": -3.717173389674077, - "velocityY": 0.24135552045497397, - "timestamp": 12.178110593873484 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.1131128587056523, + "velocityX": 0.4366659015377113, + "velocityY": 0.2557557080208232, + "timestamp": 4.203968612095648 }, { - "x": 5.690024921889411, - "y": 4.259659658304738, - "heading": -0.09725286572349409, - "angularVelocity": -4.268219562622653e-8, - "velocityX": -3.71307955769639, - "velocityY": 0.29777624203574676, - "timestamp": 12.255151285726377 + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 1.4611171491066952e-26, + "velocityX": -3.269712364266787e-25, + "velocityY": -3.777106652858518e-25, + "timestamp": 4.285699153485465 + }, + { + "x": 2.660849769218119, + "y": 6.863303091211252, + "heading": 0.49944854128498545, + "angularVelocity": -0.22618144187200803, + "velocityX": 0.2158817205646462, + "velocityY": -0.2821127758769456, + "timestamp": 4.343625560738757 + }, + { + "x": 2.685869953878005, + "y": 6.830605661533361, + "heading": 0.4737478609731425, + "angularVelocity": -0.4436781345589612, + "velocityX": 0.4319305450876828, + "velocityY": -0.5644650035849392, + "timestamp": 4.401551967992049 + }, + { + "x": 2.7234170079843754, + "y": 6.781535741371895, + "heading": 0.43609134859082577, + "angularVelocity": -0.6500750550203873, + "velocityX": 0.6481854457534552, + "velocityY": -0.8471079510749036, + "timestamp": 4.45947837524534 + }, + { + "x": 2.7735061222918693, + "y": 6.716073137055524, + "heading": 0.3873132758064447, + "angularVelocity": -0.8420697070179436, + "velocityX": 0.8647025887255888, + "velocityY": -1.1300995076412468, + "timestamp": 4.517404782498632 + }, + { + "x": 2.8361568283253176, + "y": 6.63419369001641, + "heading": 0.3285224732338341, + "angularVelocity": -1.0149223015944975, + "velocityX": 1.0815569099512647, + "velocityY": -1.4135081204170898, + "timestamp": 4.575331189751924 + }, + { + "x": 2.9113939859812374, + "y": 6.535868324439057, + "heading": 0.2612561141808071, + "angularVelocity": -1.1612382373188714, + "velocityX": 1.298840394622348, + "velocityY": -1.6974186772435556, + "timestamp": 4.6332575970052154 + }, + { + "x": 2.999247861116449, + "y": 6.4210622556853, + "heading": 0.18781374862867117, + "angularVelocity": -1.267856389418368, + "velocityX": 1.5166463673650283, + "velocityY": -1.9819297311457886, + "timestamp": 4.691184004258507 + }, + { + "x": 3.09974782280069, + "y": 6.289740190661489, + "heading": 0.11213274399438687, + "angularVelocity": -1.3065026509128828, + "velocityX": 1.734959346689516, + "velocityY": -2.2670500597350514, + "timestamp": 4.749110411511799 + }, + { + "x": 3.212855329084297, + "y": 6.141935125074281, + "heading": 0.04279640937177349, + "angularVelocity": -1.1969728127524046, + "velocityX": 1.9526069654040226, + "velocityY": -2.551600774080633, + "timestamp": 4.8070368187650905 + }, + { + "x": 3.3365134053253396, + "y": 5.978631674871108, + "heading": 0.010716435347090821, + "angularVelocity": -0.5538056914941833, + "velocityX": 2.1347444473870243, + "velocityY": -2.819153784026439, + "timestamp": 4.864963226018382 + }, + { + "x": 3.4677690523177653, + "y": 5.807368153650049, + "heading": 0.010716404660462071, + "angularVelocity": -5.297519767950936e-7, + "velocityX": 2.2659034664188993, + "velocityY": -2.95657074798692, + "timestamp": 4.922889633271674 + }, + { + "x": 3.5990247641115705, + "y": 5.636104682092312, + "heading": 0.010716373974269203, + "angularVelocity": -5.297444520108962e-7, + "velocityX": 2.265904585103496, + "velocityY": -2.9565698906349174, + "timestamp": 4.9808160405249655 + }, + { + "x": 3.730280476023843, + "y": 5.4648412106253685, + "heading": 0.010716343288076406, + "angularVelocity": -5.297444507915692e-7, + "velocityX": 2.265904587148631, + "velocityY": -2.9565698890675334, + "timestamp": 5.038742447778257 + }, + { + "x": 3.8615392378596742, + "y": 5.293580076652075, + "heading": 0.010716312601908511, + "angularVelocity": -5.297440208932243e-7, + "velocityX": 2.265957238844176, + "velocityY": -2.956529536251564, + "timestamp": 5.096668855031549 + }, + { + "x": 4.007190539027271, + "y": 5.134379144193632, + "heading": 0.010716281749960886, + "angularVelocity": -5.326059234198986e-7, + "velocityX": 2.5144197279613127, + "velocityY": -2.748330856466123, + "timestamp": 5.1545952622848406 }, { - "x": 5.4092552652272206, - "y": 4.319022844462834, - "heading": -0.09725286915389227, - "angularVelocity": -4.452709474069528e-8, - "velocityX": -3.6444332197627625, - "velocityY": 0.7705432639604031, - "timestamp": 12.33219197757927 + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.010716249762746122, + "angularVelocity": -5.522043620508212e-7, + "velocityX": 2.76745156437766, + "velocityY": -2.4933593359962067, + "timestamp": 5.212521669538132 + }, + { + "x": 4.289284709391487, + "y": 4.8946057330854265, + "heading": 0.010716218824995563, + "angularVelocity": -7.451052814449057e-7, + "velocityX": 2.933087406208596, + "velocityY": -2.2962197715760517, + "timestamp": 5.254042983814184 + }, + { + "x": 4.417365814004084, + "y": 4.807904620313661, + "heading": 0.010716189360474368, + "angularVelocity": -7.096240017556084e-7, + "velocityX": 3.084707380914231, + "velocityY": -2.08811099271423, + "timestamp": 5.295564298090236 + }, + { + "x": 4.551130165096487, + "y": 4.730258301325536, + "heading": 0.010716160700942695, + "angularVelocity": -6.902366211867451e-7, + "velocityX": 3.2215827804264254, + "velocityY": -1.870035193777778, + "timestamp": 5.3370856123662875 + }, + { + "x": 4.688893145382921, + "y": 4.6599503090237695, + "heading": 0.010716132275769374, + "angularVelocity": -6.845923308774473e-7, + "velocityX": 3.3178858301672403, + "velocityY": -1.693298815984659, + "timestamp": 5.378606926642339 + }, + { + "x": 4.826657112545421, + "y": 4.589644250461509, + "heading": 0.0107161038506266, + "angularVelocity": -6.845915951833867e-7, + "velocityX": 3.317909598106278, + "velocityY": -1.693252243771356, + "timestamp": 5.420128240918391 + }, + { + "x": 4.964421079807684, + "y": 4.5193381920947315, + "heading": 0.01071607542548375, + "angularVelocity": -6.845915970495296e-7, + "velocityX": 3.317909600508956, + "velocityY": -1.6932522390633336, + "timestamp": 5.461649555194443 + }, + { + "x": 5.102185047070045, + "y": 4.4490321337281475, + "heading": 0.010716047000340928, + "angularVelocity": -6.845915962954477e-7, + "velocityX": 3.3179096005113364, + "velocityY": -1.6932522390586708, + "timestamp": 5.503170869470495 + }, + { + "x": 5.2399490152099615, + "y": 4.378726077081124, + "heading": 0.01071601857519813, + "angularVelocity": -6.84591595754071e-7, + "velocityX": 3.317909621646396, + "velocityY": -1.6932521976447459, + "timestamp": 5.544692183746546 + }, + { + "x": 5.377721663167995, + "y": 4.308437031039157, + "heading": 0.010715990149891885, + "angularVelocity": -6.845955322087529e-7, + "velocityX": 3.3181186665253675, + "velocityY": -1.6928425139593435, + "timestamp": 5.586213498022598 + }, + { + "x": 5.519903819666949, + "y": 4.247559392833526, + "heading": 0.010715961486816888, + "angularVelocity": -6.903219586500289e-7, + "velocityX": 3.4243173410567787, + "velocityY": -1.4661780164493292, + "timestamp": 5.62773481229865 }, { - "x": 5.138392925262451, - "y": 4.413834095001221, - "heading": -0.09725287529259724, - "angularVelocity": -7.968133230975529e-8, - "velocityX": -3.5158347290283003, - "velocityY": 1.2306645781352268, - "timestamp": 12.409232669432164 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.01071593198598864, + "angularVelocity": -7.104984214494266e-7, + "velocityX": 3.5174201900946844, + "velocityY": -1.22612492322878, + "timestamp": 5.669256126574702 + }, + { + "x": 5.815510824369738, + "y": 4.155881163603153, + "heading": 0.010715902600881807, + "angularVelocity": -7.061178508232372e-7, + "velocityX": 3.593873172685508, + "velocityY": -0.97964420643565, + "timestamp": 5.7108711442346545 + }, + { + "x": 5.9675336119930025, + "y": 4.125565856378816, + "heading": 0.010715874047412989, + "angularVelocity": -6.861337666816658e-7, + "velocityX": 3.653075167851236, + "velocityY": -0.7284703678862195, + "timestamp": 5.752486161894607 + }, + { + "x": 6.121239827015959, + "y": 4.105457516553604, + "heading": 0.010715845747967176, + "angularVelocity": -6.800296480422641e-7, + "velocityX": 3.6935275692763176, + "velocityY": -0.483199117912718, + "timestamp": 5.79410117955456 + }, + { + "x": 6.274947903964394, + "y": 4.085363414233198, + "heading": 0.010715817448685471, + "angularVelocity": -6.800257045912017e-7, + "velocityX": 3.693572310948509, + "velocityY": -0.4828569937083839, + "timestamp": 5.835716197214513 + }, + { + "x": 6.4286559810993475, + "y": 4.0652693133395585, + "heading": 0.01071578914940375, + "angularVelocity": -6.800257049819807e-7, + "velocityX": 3.693572315430539, + "velocityY": -0.48285695942348605, + "timestamp": 5.877331214874466 + }, + { + "x": 6.582364058235009, + "y": 4.045175212451323, + "heading": 0.010715760850122012, + "angularVelocity": -6.800257053652886e-7, + "velocityX": 3.693572315447514, + "velocityY": -0.482856959293641, + "timestamp": 5.918946232534418 + }, + { + "x": 6.736072142233211, + "y": 4.025081164057568, + "heading": 0.010715732550840138, + "angularVelocity": -6.80025708616856e-7, + "velocityX": 3.693572480352915, + "velocityY": -0.4828556978624376, + "timestamp": 5.960561250194371 + }, + { + "x": 6.88984765379033, + "y": 4.0055098097098245, + "heading": 0.01071570424372189, + "angularVelocity": -6.802140150471018e-7, + "velocityX": 3.69519275021481, + "velocityY": -0.47029547140088923, + "timestamp": 6.002176267854324 + }, + { + "x": 7.0446085905714275, + "y": 3.9966217347495547, + "heading": 0.010715671283588182, + "angularVelocity": -7.920249842947072e-7, + "velocityX": 3.7188723082058996, + "velocityY": -0.21357854592052494, + "timestamp": 6.043791285514277 }, { - "x": 4.909018653493503, - "y": 4.524275328875486, - "heading": -0.09725287687080802, - "angularVelocity": -2.1964896173308048e-8, - "velocityX": -3.192337867898694, - "velocityY": 1.5370761958403907, - "timestamp": 12.481084171767012 + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -1.8890515062078487e-25, + "angularVelocity": -0.2574952958364392, + "velocityX": 3.6592868466916966, + "velocityY": 0.030155204912350568, + "timestamp": 6.08540630317423 + }, + { + "x": 7.411732272025267, + "y": 4.005484051327251, + "heading": -0.0035958107444463492, + "angularVelocity": -0.054616477872747704, + "velocityX": 3.2632237151822827, + "velocityY": 0.11554829109191134, + "timestamp": 6.151243773558613 + }, + { + "x": 7.5997547622463255, + "y": 4.014664246157471, + "heading": -0.0036926162217638513, + "angularVelocity": -0.0014703705466251413, + "velocityX": 2.855858360342751, + "velocityY": 0.139437235006489, + "timestamp": 6.217081243942996 + }, + { + "x": 7.760906466600038, + "y": 4.024479422852048, + "heading": -0.002491126392030619, + "angularVelocity": 0.018249331614937442, + "velocityX": 2.44772017230992, + "velocityY": 0.149081923064063, + "timestamp": 6.282918714327379 + }, + { + "x": 7.8951789466062605, + "y": 4.0345153008905195, + "heading": -0.0009601367346007951, + "angularVelocity": 0.023254077784145503, + "velocityX": 2.0394538128863453, + "velocityY": 0.1524341378834605, + "timestamp": 6.348756184711762 + }, + { + "x": 8.002571224633423, + "y": 4.044538615665461, + "heading": 0.0003551372183222516, + "angularVelocity": 0.01997758943720037, + "velocityX": 1.6311726042961088, + "velocityY": 0.15224331549225237, + "timestamp": 6.414593655096145 + }, + { + "x": 8.08308410390753, + "y": 4.05439979104189, + "heading": 0.0011050703957921194, + "angularVelocity": 0.01139067423294795, + "velocityX": 1.222903595840534, + "velocityY": 0.14978059331343505, + "timestamp": 6.4804311254805285 + }, + { + "x": 8.13671880253707, + "y": 4.063994763416068, + "heading": 0.0010463784362104483, + "angularVelocity": -0.0008914674157285515, + "velocityX": 0.8146530891360332, + "velocityY": 0.14573725749423916, + "timestamp": 6.546268595864912 }, { - "x": 4.71108590609518, - "y": 4.628682093882472, - "heading": -0.09725287720527087, - "angularVelocity": -4.654917836145884e-9, - "velocityX": -2.754747513502237, - "velocityY": 1.4530909113134631, - "timestamp": 12.55293567410186 + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -6.673758454276996e-26, + "angularVelocity": -0.01589335723412987, + "velocityX": 0.4064214467340168, + "velocityY": 0.14053080109998423, + "timestamp": 6.612106066249295 + }, + { + "x": 8.158055062338372, + "y": 4.083508095993073, + "heading": -0.002757938692971577, + "angularVelocity": -0.035856325054586746, + "velocityX": -0.07048563935601859, + "velocityY": 0.13340643741083522, + "timestamp": 6.6890224454034035 + }, + { + "x": 8.11595159662588, + "y": 4.09322125310912, + "heading": -0.007051362960883781, + "angularVelocity": -0.05581937573153214, + "velocityX": -0.5473927162917097, + "velocityY": 0.12628203801150484, + "timestamp": 6.765938824557512 + }, + { + "x": 8.037166166331591, + "y": 4.102386423514656, + "heading": -0.012880297853755818, + "angularVelocity": -0.07578275208708442, + "velocityX": -1.0242997806284444, + "velocityY": 0.11915759044211331, + "timestamp": 6.842855203711621 + }, + { + "x": 7.921698772840109, + "y": 4.111003602378457, + "heading": -0.020244795899094583, + "angularVelocity": -0.0957468113596885, + "velocityX": -1.501206826963765, + "velocityY": 0.11203308006134147, + "timestamp": 6.91977158286573 + }, + { + "x": 7.769549418244295, + "y": 4.119072783408171, + "heading": -0.029144948174511114, + "angularVelocity": -0.11571205474433797, + "velocityX": -1.9781138460895251, + "velocityY": 0.10490848787287595, + "timestamp": 6.996687962019839 + }, + { + "x": 7.5807181060109095, + "y": 4.1265939580705435, + "heading": -0.0395808971630345, + "angularVelocity": -0.1356791505696594, + "velocityX": -2.4550208201434414, + "velocityY": 0.09778378474242518, + "timestamp": 7.073604341173947 + }, + { + "x": 7.355204842942577, + "y": 4.133567113261441, + "heading": -0.051552849562924184, + "angularVelocity": -0.1556489337063405, + "velocityX": -2.931927705755565, + "velocityY": 0.0906589112434205, + "timestamp": 7.150520720328056 + }, + { + "x": 7.093009648902004, + "y": 4.139992219744277, + "heading": -0.06506107751515841, + "angularVelocity": -0.17562225498380826, + "velocityX": -3.408834333130018, + "velocityY": 0.08353365763568768, + "timestamp": 7.227437099482165 + }, + { + "x": 6.806528037248827, + "y": 4.135713032696913, + "heading": -0.06506107911297748, + "angularVelocity": -2.077345661840568e-8, + "velocityX": -3.7245852548412777, + "velocityY": -0.05563427574756843, + "timestamp": 7.304353478636274 + }, + { + "x": 6.520046430966287, + "y": 4.131433486105525, + "heading": -0.06506108071074132, + "angularVelocity": -2.0772738551017004e-8, + "velocityX": -3.7245851850169447, + "velocityY": -0.0556389502268829, + "timestamp": 7.381269857790382 + }, + { + "x": 6.233564817210998, + "y": 4.127154439784588, + "heading": -0.06506108230850521, + "angularVelocity": -2.07727392722992e-8, + "velocityX": -3.724585282171128, + "velocityY": -0.055632446144705854, + "timestamp": 7.458186236944491 + }, + { + "x": 5.94752903267143, + "y": 4.143693939737266, + "heading": -0.06506108392885115, + "angularVelocity": -2.1066331425696132e-8, + "velocityX": -3.7187889976784825, + "velocityY": 0.21503222245471107, + "timestamp": 7.5351026160986 }, { - "x": 4.54294597125426, - "y": 4.721368217002082, - "heading": -0.09725287703485491, - "angularVelocity": 2.3717800410462983e-9, - "velocityX": -2.3401032598781417, - "velocityY": 1.2899677822694202, - "timestamp": 12.624787176436708 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.06506108566326727, + "angularVelocity": -2.2549372865536112e-8, + "velocityX": -3.660823701626197, + "velocityY": 0.68847669897561, + "timestamp": 7.612018995252709 + }, + { + "x": 5.429536251240902, + "y": 4.26885692196241, + "heading": -0.06506108731980774, + "angularVelocity": -2.4962356651562853e-8, + "velocityX": -3.5625374415593805, + "velocityY": 1.0880977954514335, + "timestamp": 7.678380537119456 + }, + { + "x": 5.202528633144186, + "y": 4.366702684523792, + "heading": -0.06506108887411338, + "angularVelocity": -2.342178313955022e-8, + "velocityX": -3.420770701086799, + "velocityY": 1.4744347374847653, + "timestamp": 7.744742078986204 + }, + { + "x": 4.987502129020376, + "y": 4.48575380362361, + "heading": -0.06936699261461025, + "angularVelocity": -0.06488552886765389, + "velocityX": -3.2402276691457503, + "velocityY": 1.7939775923059436, + "timestamp": 7.811103620852951 + }, + { + "x": 4.796367424971955, + "y": 4.591577847199328, + "heading": -0.07462166054562926, + "angularVelocity": -0.0791824267972894, + "velocityX": -2.8802028806415447, + "velocityY": 1.5946592046973391, + "timestamp": 7.877465162719699 + }, + { + "x": 4.62912519228471, + "y": 4.68417391671917, + "heading": -0.07977945899466835, + "angularVelocity": -0.0777227036013692, + "velocityX": -2.5201679765527953, + "velocityY": 1.3953272771415286, + "timestamp": 7.943826704586447 + }, + { + "x": 4.48577515824327, + "y": 4.763541939192526, + "heading": -0.08449544521256436, + "angularVelocity": -0.07106504890084675, + "velocityX": -2.1601371820034427, + "velocityY": 1.195994249692469, + "timestamp": 8.010188246453195 + }, + { + "x": 4.366317095706272, + "y": 4.829681919907118, + "heading": -0.08859794312005527, + "angularVelocity": -0.06182041272833338, + "velocityX": -1.8001098102402053, + "velocityY": 0.9966613019239372, + "timestamp": 8.076549788319943 + }, + { + "x": 4.270750839723337, + "y": 4.882593875393496, + "heading": -0.09198424265030704, + "angularVelocity": -0.05102804176930339, + "velocityX": -1.4400849241090516, + "velocityY": 0.7973286032537331, + "timestamp": 8.14291133018669 + }, + { + "x": 4.199076268469758, + "y": 4.922277822249225, + "heading": -0.09458599237732916, + "angularVelocity": -0.03920568530861384, + "velocityX": -1.0800618737505943, + "velocityY": 0.597996154691727, + "timestamp": 8.209272872053438 + }, + { + "x": 4.151293289107276, + "y": 4.948733775055273, + "heading": -0.09635440561696425, + "angularVelocity": -0.026648163829376063, + "velocityX": -0.7200402223689916, + "velocityY": 0.3986639258498654, + "timestamp": 8.275634413920185 }, { - "x": 4.40369795049572, - "y": 4.80026148816612, - "heading": -0.0972528766496691, - "angularVelocity": 5.3608595378330946e-9, - "velocityX": -1.9379973449908554, - "velocityY": 1.0980044759032759, - "timestamp": 12.696638678771556 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.013539011456923473, + "velocityX": -0.3600196690634489, + "velocityY": 0.19933188392621148, + "timestamp": 8.341995955786933 }, { - "x": 4.292814205558175, - "y": 4.864307082758427, - "heading": -0.09725287620265412, - "angularVelocity": 6.221372869321198e-9, - "velocityX": -1.5432348849269113, - "velocityY": 0.8913605493429585, - "timestamp": 12.768490181106404 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -2.1027730559805923e-24, + "velocityX": -4.370401644398185e-24, + "velocityY": -7.227687528271155e-24, + "timestamp": 8.40835749765368 + }, + { + "x": 4.144916563575742, + "y": 4.94494062571017, + "heading": -0.0787144976857821, + "angularVelocity": 0.2928853385841611, + "velocityX": 0.27671294348408426, + "velocityY": -0.2689143973713598, + "timestamp": 8.471653180860056 + }, + { + "x": 4.1805101351641145, + "y": 4.911442857139667, + "heading": -0.04246897747139415, + "angularVelocity": 0.5726381070287113, + "velocityX": 0.5623380582261821, + "velocityY": -0.529226747759122, + "timestamp": 8.53494886406643 + }, + { + "x": 4.234877563186883, + "y": 4.862193017904391, + "heading": 0.010407773982345214, + "angularVelocity": 0.8353926962338748, + "velocityX": 0.858943695188552, + "velocityY": -0.7780915970951138, + "timestamp": 8.598244547272806 + }, + { + "x": 4.308888949041241, + "y": 4.798194192640054, + "heading": 0.078455258557249, + "angularVelocity": 1.0750730717770254, + "velocityX": 1.1692959472930267, + "velocityY": -1.0111088469599114, + "timestamp": 8.661540230479181 + }, + { + "x": 4.4036437297615265, + "y": 4.720901995225723, + "heading": 0.15957624341444238, + "angularVelocity": 1.2816195473030647, + "velocityX": 1.497018057476968, + "velocityY": -1.2211290486006587, + "timestamp": 8.724835913685556 + }, + { + "x": 4.520513474307112, + "y": 4.632549416340532, + "heading": 0.2505777849388087, + "angularVelocity": 1.4377211353838413, + "velocityX": 1.8464094014836858, + "velocityY": -1.395870530334874, + "timestamp": 8.788131596891931 + }, + { + "x": 4.661061641006965, + "y": 4.536766015518807, + "heading": 0.3463605077982218, + "angularVelocity": 1.5132583773069228, + "velocityX": 2.2205016136976554, + "velocityY": -1.5132690883424573, + "timestamp": 8.851427280098306 + }, + { + "x": 4.826402761674467, + "y": 4.439605699543979, + "heading": 0.43881735404698696, + "angularVelocity": 1.4607132993147334, + "velocityX": 2.612202164378375, + "velocityY": -1.535022785962128, + "timestamp": 8.914722963304682 + }, + { + "x": 5.0150915047408, + "y": 4.350093359802789, + "heading": 0.5163398586345646, + "angularVelocity": 1.2247676407064818, + "velocityX": 2.98106811567406, + "velocityY": -1.4141934363728348, + "timestamp": 8.978018646511057 + }, + { + "x": 5.2211399062787285, + "y": 4.277069011475663, + "heading": 0.5664924385126362, + "angularVelocity": 0.7923538752958722, + "velocityX": 3.255331028912471, + "velocityY": -1.1537018739339473, + "timestamp": 9.041314329717432 + }, + { + "x": 5.439515498788918, + "y": 4.225186735026024, + "heading": 0.5993129773415641, + "angularVelocity": 0.518527286006478, + "velocityX": 3.450086663859443, + "velocityY": -0.8196811191764456, + "timestamp": 9.104610012923807 }, { - "x": 4.209952469660844, - "y": 4.91286872561946, - "heading": -0.09725287578795568, - "angularVelocity": 5.771604384474185e-9, - "velocityX": -1.1532359547775561, - "velocityY": 0.6758612037744437, - "timestamp": 12.840341683441252 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.6211417582867742, + "angularVelocity": 0.3448699791111725, + "velocityX": 3.577435593728997, + "velocityY": -0.450862665918832, + "timestamp": 9.167905696130182 + }, + { + "x": 5.882912329849662, + "y": 4.1913117565500215, + "heading": 0.6344672511131947, + "angularVelocity": 0.22398342506550498, + "velocityX": 3.6468128538078894, + "velocityY": -0.08971306223841023, + "timestamp": 9.227398905605497 + }, + { + "x": 6.101970858853418, + "y": 4.2077445367688195, + "heading": 0.6401776935966378, + "angularVelocity": 0.09598477765454098, + "velocityX": 3.682076171981447, + "velocityY": 0.27621270332737524, + "timestamp": 9.286892115080812 + }, + { + "x": 6.320243538678639, + "y": 4.246069175734349, + "heading": 0.6401793813797093, + "angularVelocity": 0.0000283693397346701, + "velocityX": 3.6688671152594092, + "velocityY": 0.6441850978208127, + "timestamp": 9.346385324556127 + }, + { + "x": 6.533654958768337, + "y": 4.305798072358943, + "heading": 0.6401794602105909, + "angularVelocity": 0.0000013250399886292195, + "velocityX": 3.5871559455579396, + "velocityY": 1.003961580680503, + "timestamp": 9.405878534031443 + }, + { + "x": 6.7401111102822036, + "y": 4.386345181993656, + "heading": 0.640179519772512, + "angularVelocity": 0.0000010011549482699353, + "velocityX": 3.470247333009187, + "velocityY": 1.3538874494262292, + "timestamp": 9.465371743506758 + }, + { + "x": 6.937586723137597, + "y": 4.486920348849836, + "heading": 0.6401795682436345, + "angularVelocity": 8.147336982383455e-7, + "velocityX": 3.3192966827135675, + "velocityY": 1.6905318731864938, + "timestamp": 9.524864952982073 + }, + { + "x": 7.124144636672047, + "y": 4.606536937733858, + "heading": 0.6401796100323616, + "angularVelocity": 7.024117120417555e-7, + "velocityX": 3.1357849942833846, + "velocityY": 2.010592300179313, + "timestamp": 9.584358162457388 + }, + { + "x": 7.297954825460976, + "y": 4.744021492057199, + "heading": 0.6401796478532076, + "angularVelocity": 6.357170231929654e-7, + "velocityX": 2.9215130654709105, + "velocityY": 2.310928516646711, + "timestamp": 9.643851371932703 + }, + { + "x": 7.457312504235246, + "y": 4.898025112547281, + "heading": 0.6401796836083543, + "angularVelocity": 6.009954258140389e-7, + "velocityX": 2.6785860130876458, + "velocityY": 2.588591569496418, + "timestamp": 9.703344581408018 + }, + { + "x": 7.598334922177398, + "y": 5.05313142847436, + "heading": 0.681855456134976, + "angularVelocity": 0.7005130987917911, + "velocityX": 2.3703951961217746, + "velocityY": 2.6071263812290466, + "timestamp": 9.762837790883333 + }, + { + "x": 7.725243929451597, + "y": 5.1927289365333404, + "heading": 0.750546465230805, + "angularVelocity": 1.1546025118098764, + "velocityX": 2.133167942920592, + "velocityY": 2.3464443974384026, + "timestamp": 9.822331000358648 + }, + { + "x": 7.837539775743062, + "y": 5.315957707872993, + "heading": 0.8170716120028277, + "angularVelocity": 1.1181973095538917, + "velocityX": 1.8875405660886297, + "velocityY": 2.071308178301995, + "timestamp": 9.881824209833963 + }, + { + "x": 7.935320352876818, + "y": 5.422866587549723, + "heading": 0.8748533666706206, + "angularVelocity": 0.9712327705528885, + "velocityX": 1.643558617800391, + "velocityY": 1.79699297818334, + "timestamp": 9.941317419309279 + }, + { + "x": 8.018658578193438, + "y": 5.513509965801917, + "heading": 0.9207921483680607, + "angularVelocity": 0.7721684895231855, + "velocityX": 1.400802310912472, + "velocityY": 1.5235920040556004, + "timestamp": 10.000810628784594 + }, + { + "x": 8.08760540363553, + "y": 5.587927848241841, + "heading": 0.9530800723202797, + "angularVelocity": 0.5427161223436167, + "velocityX": 1.1589024369360976, + "velocityY": 1.250863469902435, + "timestamp": 10.060303838259909 + }, + { + "x": 8.142197837704975, + "y": 5.6461493904163484, + "heading": 0.9705333871837108, + "angularVelocity": 0.2933665037969271, + "velocityX": 0.917624625581801, + "velocityY": 0.9786250008694746, + "timestamp": 10.119797047735224 + }, + { + "x": 8.182463827907348, + "y": 5.6881968245095615, + "heading": 0.9723096580243064, + "angularVelocity": 0.029856698878088846, + "velocityX": 0.6768165738155562, + "velocityY": 0.7067602246380643, + "timestamp": 10.179290257210539 + }, + { + "x": 8.208425076414684, + "y": 5.714088220117355, + "heading": 0.9577673202803244, + "angularVelocity": -0.24443693443730044, + "velocityX": 0.4363733060679247, + "velocityY": 0.43519917375674694, + "timestamp": 10.238783466685854 + }, + { + "x": 8.220098495483398, + "y": 5.723839282989502, + "heading": 0.9263929393086, + "angularVelocity": -0.5273607063465391, + "velocityX": 0.1962143103669291, + "velocityY": 0.1639021151849818, + "timestamp": 10.29827667616117 + }, + { + "x": 8.217115474998435, + "y": 5.7169576815494905, + "heading": 0.8762998539218844, + "angularVelocity": -0.8237354847591428, + "velocityX": -0.0490530740172862, + "velocityY": -0.11316171192782937, + "timestamp": 10.359088777027946 + }, + { + "x": 8.199143140937736, + "y": 5.693225588730865, + "heading": 0.8092972371375105, + "angularVelocity": -1.1017974355327707, + "velocityX": -0.29553877936354017, + "velocityY": -0.39025280298433057, + "timestamp": 10.419900877894722 + }, + { + "x": 8.16609422727989, + "y": 5.652642007461555, + "heading": 0.7267788786337702, + "angularVelocity": -1.356939775596919, + "velocityX": -0.5434594955080971, + "velocityY": -0.6673602899892838, + "timestamp": 10.480712978761499 + }, + { + "x": 8.117864668551345, + "y": 5.595209979313778, + "heading": 0.6304892819422186, + "angularVelocity": -1.5833953328219617, + "velocityX": -0.7930914742479169, + "velocityY": -0.9444177610899582, + "timestamp": 10.541525079628276 + }, + { + "x": 8.054326469941712, + "y": 5.520944740821225, + "heading": 0.5226315224214131, + "angularVelocity": -1.7736233082473956, + "velocityX": -1.044828211885481, + "velocityY": -1.2212246811740535, + "timestamp": 10.602337180495052 + }, + { + "x": 7.975314688312655, + "y": 5.4298894198944145, + "heading": 0.40611975005056616, + "angularVelocity": -1.9159307228358207, + "velocityX": -1.2992772902575926, + "velocityY": -1.4973224017747822, + "timestamp": 10.663149281361829 + }, + { + "x": 7.880609390155109, + "y": 5.32214652648837, + "heading": 0.28521663340036824, + "angularVelocity": -1.9881424079570305, + "velocityX": -1.5573429762774083, + "velocityY": -1.7717344388756155, + "timestamp": 10.723961382228605 + }, + { + "x": 7.769927978869046, + "y": 5.19796449901651, + "heading": 0.1670617076218344, + "angularVelocity": -1.9429508945494507, + "velocityX": -1.8200557078029118, + "velocityY": -2.0420611309566765, + "timestamp": 10.784773483095382 + }, + { + "x": 7.64302287953294, + "y": 5.0580816434833595, + "heading": 0.06529420310247565, + "angularVelocity": -1.6734745728042795, + "velocityX": -2.086839585005026, + "velocityY": -2.300247048520789, + "timestamp": 10.845585583962158 + }, + { + "x": 7.499740913666761, + "y": 4.907357720220894, + "heading": 0.011577036219802711, + "angularVelocity": -0.8833302273235669, + "velocityX": -2.3561423437758418, + "velocityY": -2.478518602616007, + "timestamp": 10.906397684828935 + }, + { + "x": 7.338349817846371, + "y": 4.749390248030348, + "heading": 0.009577494655868879, + "angularVelocity": -0.03288065262396252, + "velocityX": -2.6539306078893232, + "velocityY": -2.597632213638097, + "timestamp": 10.967209785695712 + }, + { + "x": 7.161261092895849, + "y": 4.608134428437688, + "heading": 0.009577439039588426, + "angularVelocity": -9.145594324092402e-7, + "velocityX": -2.9120639219236804, + "velocityY": -2.322824200764184, + "timestamp": 11.028021886562488 + }, + { + "x": 6.970797581586152, + "y": 4.485507869855581, + "heading": 0.009577379210160273, + "angularVelocity": -9.83840835982591e-7, + "velocityX": -3.132000187379736, + "velocityY": -2.016482851838167, + "timestamp": 11.088833987429265 + }, + { + "x": 6.76891141017589, + "y": 4.382767451700059, + "heading": 0.009577307882139067, + "angularVelocity": -0.000001172924799341256, + "velocityX": -3.3198355020252337, + "velocityY": -1.6894732576432487, + "timestamp": 11.149646088296041 + }, + { + "x": 6.5576717954256925, + "y": 4.300966222617904, + "heading": 0.009577215489609455, + "angularVelocity": -0.0000015193115891924993, + "velocityX": -3.473644418451679, + "velocityY": -1.3451472308342272, + "timestamp": 11.210458189162818 + }, + { + "x": 6.33924383214455, + "y": 4.24094261156842, + "heading": 0.009577080518783282, + "angularVelocity": -0.000002219473168173005, + "velocityX": -3.5918503088663734, + "velocityY": -0.9870339980685793, + "timestamp": 11.271270290029594 + }, + { + "x": 6.115866515513707, + "y": 4.203311883044666, + "heading": 0.009576062223691489, + "angularVelocity": -0.000016744941833606887, + "velocityX": -3.673237948483439, + "velocityY": -0.6188032971627346, + "timestamp": 11.332082390896371 + }, + { + "x": 5.890604902143948, + "y": 4.188526246713883, + "heading": 0.006634290856698977, + "angularVelocity": -0.04837476957813366, + "velocityX": -3.704223504187922, + "velocityY": -0.24313641725970658, + "timestamp": 11.392894491763148 }, { - "x": 4.154873707750821, - "y": 4.945521497298995, - "heading": -0.09725287546928488, - "angularVelocity": 4.4351304984872785e-9, - "velocityX": -0.7665638173206214, - "velocityY": 0.4544480020384788, - "timestamp": 12.9121931857761 + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 1.4617092924271447e-24, + "angularVelocity": -0.10909491305411462, + "velocityX": -3.6942182578974845, + "velocityY": 0.1335725575137668, + "timestamp": 11.453706592629924 + }, + { + "x": 5.418474509439448, + "y": 4.234112857220888, + "heading": -0.009420810708608588, + "angularVelocity": -0.13872442581307998, + "velocityX": -3.644180551162595, + "velocityY": 0.551666082669452, + "timestamp": 11.521616844580091 + }, + { + "x": 5.180225703278332, + "y": 4.298642990605948, + "heading": -0.020608459493002298, + "angularVelocity": -0.164741676891483, + "velocityX": -3.5082892393911758, + "velocityY": 0.9502266820098406, + "timestamp": 11.589527096530258 + }, + { + "x": 4.959251461441715, + "y": 4.385964306023392, + "heading": -0.03296814464402756, + "angularVelocity": -0.1820002841411158, + "velocityX": -3.253915800500449, + "velocityY": 1.2858340664310908, + "timestamp": 11.657437348480425 + }, + { + "x": 4.763381292287379, + "y": 4.486986357550731, + "heading": -0.04558833602673758, + "angularVelocity": -0.18583632103103812, + "velocityX": -2.8842503676479536, + "velocityY": 1.4875817513012504, + "timestamp": 11.725347600430592 + }, + { + "x": 4.59599065624695, + "y": 4.59062036623284, + "heading": -0.05760567577443394, + "angularVelocity": -0.17695913949067177, + "velocityX": -2.464880209298302, + "velocityY": 1.5260436488758087, + "timestamp": 11.793257852380759 + }, + { + "x": 4.456604919229509, + "y": 4.6884604808720365, + "heading": -0.06845454912247914, + "angularVelocity": -0.1597531011371608, + "velocityX": -2.0524991884836625, + "velocityY": 1.440726721364413, + "timestamp": 11.861168104330925 + }, + { + "x": 4.343601982653564, + "y": 4.775267107804887, + "heading": -0.07780582197928933, + "angularVelocity": -0.13770045888906648, + "velocityX": -1.6640040837849672, + "velocityY": 1.2782551152446038, + "timestamp": 11.929078356281092 + }, + { + "x": 4.255382643641957, + "y": 4.8478074673531255, + "heading": -0.0854646750430389, + "angularVelocity": -0.11277904062805744, + "velocityX": -1.2990577486937123, + "velocityY": 1.0681798029768506, + "timestamp": 11.99698860823126 + }, + { + "x": 4.190618071556959, + "y": 4.903989277014325, + "heading": -0.09130900754804393, + "angularVelocity": -0.08605964986396453, + "velocityX": -0.9536788662265924, + "velocityY": 0.8272949672227069, + "timestamp": 12.064898860181426 + }, + { + "x": 4.148242739278531, + "y": 4.942382522588693, + "heading": -0.09525748910421675, + "angularVelocity": -0.058142643309145876, + "velocityX": -0.6239902085700291, + "velocityY": 0.5653527187992231, + "timestamp": 12.132809112131593 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 2.459066716778208e-9, - "velocityX": -0.38234244368231685, - "velocityY": 0.22880870103743695, - "timestamp": 12.984044688110949 + "angularVelocity": -0.029382694528135574, + "velocityX": -0.30688901769875077, + "velocityY": 0.28831027812257853, + "timestamp": 12.20071936408176 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": -3.871307616700285e-29, - "velocityX": 3.757481365144797e-28, - "velocityY": -1.0154842203400723e-27, - "timestamp": 13.055896190445797 + "angularVelocity": 4.130042429684847e-26, + "velocityX": 1.7558924014550056e-25, + "velocityY": 1.7099094571039823e-25, + "timestamp": 12.268629616031927 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 0.387, - "y": 4.121134281158447, + "x": 1.2899744510650635, + "y": 5.590954303741455, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 15 }, { - "timestamp": 0.6923742371436291, + "timestamp": 1.0690213480650048, "isStopPoint": false, - "x": 1.7826586961746216, - "y": 3.5168776512145996, - "heading": -0.79, + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 14 }, { - "timestamp": 1.3971973802479047, + "timestamp": 2.0915060292414522, "isStopPoint": true, - "x": 3.0433270931243896, - "y": 2.9765214920043945, - "heading": -0.79, + "x": 1.8129411935806274, + "y": 5.542999267578125, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 8 }, { - "timestamp": 2.430115113382513, - "isStopPoint": false, - "x": 5.5030035972595215, - "y": 1.563418984413147, - "heading": -0.34555563246426124, + "timestamp": 2.8723893187704177, + "isStopPoint": true, + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 12 }, { - "timestamp": 3.0066730692210744, + "timestamp": 3.7953159051465586, "isStopPoint": false, - "x": 7.536985397338867, - "y": 1.0086194276809692, - "heading": -0.34555563246426124, + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { - "timestamp": 3.456916790263765, - "isStopPoint": false, - "x": 8.208122253417969, - "y": 0.763107419013977, - "heading": -0.34555563246426124, + "timestamp": 4.285699153485465, + "isStopPoint": true, + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 16 }, { - "timestamp": 4.544126438842174, + "timestamp": 5.212521669538132, "isStopPoint": false, - "x": 5.5030035972595215, - "y": 1.563418984413147, + "x": 4.16749906539917, + "y": 4.98994779586792, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 18 - }, - { - "timestamp": 5.6369430590525305, - "isStopPoint": true, - "x": 3.0433270931243896, - "y": 2.9765214920043945, - "heading": -0.79, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 11 }, { - "timestamp": 6.687562362256374, + "timestamp": 5.669256126574702, "isStopPoint": false, - "x": 5.5030035972595215, - "y": 1.563418984413147, + "x": 5.665951728820801, + "y": 4.196649074554443, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 19 + "controlIntervalCount": 10 }, { - "timestamp": 7.689873244948311, + "timestamp": 6.08540630317423, "isStopPoint": false, - "x": 8.207907676696777, - "y": 2.428393840789795, - "heading": 0.9600708878718816, + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 8 }, { - "timestamp": 8.338283147784624, + "timestamp": 6.612106066249295, "isStopPoint": false, - "x": 7.120736598968506, - "y": 3.5168776512145996, - "heading": 0.9600708878718816, + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 18 + "headingConstrained": true, + "controlIntervalCount": 13 }, { - "timestamp": 8.922397452891401, + "timestamp": 7.612018995252709, "isStopPoint": false, - "x": 5.138392925262451, - "y": 4.413834095001221, - "heading": -0.09725287529259725, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "timestamp": 9.568349729100014, + "timestamp": 8.40835749765368, "isStopPoint": true, "x": 4.127401828765869, "y": 4.96196174621582, @@ -33504,43 +39364,43 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 12 }, { - "timestamp": 10.146875628036378, + "timestamp": 9.167905696130182, "isStopPoint": false, - "x": 5.138392925262451, - "y": 4.413834095001221, - "heading": -0.09725287529259725, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 + "headingConstrained": false, + "controlIntervalCount": 19 }, { - "timestamp": 11.253622291638763, + "timestamp": 10.29827667616117, "isStopPoint": false, - "x": 8.207907676696777, - "y": 4.097138404846191, - "heading": 0, + "x": 8.220098495483398, + "y": 5.723839282989502, + "heading": 0.9263929393086, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 19 }, { - "timestamp": 12.409232669432164, + "timestamp": 11.453706592629924, "isStopPoint": false, - "x": 5.138392925262451, - "y": 4.413834095001221, - "heading": -0.09725287529259725, + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 12 }, { - "timestamp": 13.055896190445797, + "timestamp": 12.268629616031927, "isStopPoint": true, "x": 4.127401828765869, "y": 4.96196174621582, @@ -33566,25 +39426,32 @@ }, { "scope": [ - 2 + 3 ], "type": "StopPoint" }, { "scope": [ - 16 + 5 ], "type": "StopPoint" }, { "scope": [ - 12 + 2 ], "type": "StopPoint" }, { "scope": [ - 7 + 4 + ], + "type": "WptVelocityDirection", + "direction": 0.5125504196 + }, + { + "scope": [ + 11 ], "type": "StopPoint" } @@ -33596,3610 +39463,1844 @@ "eventMarkers": [], "isTrajectoryStale": false }, - "AmpLanepdChaos": { + "AmpLanePAEDF": { "waypoints": [ { - "x": 0.4269569218158722, - "y": 6.9908647537231445, + "x": 0.43297290802001953, + "y": 6.9807281494140625, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 6 }, { - "x": 1.5002212524414062, - "y": 6.694304943084717, + "x": 0.8667811155319214, + "y": 6.895569324493408, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 13 }, { - "x": 2.319291353225708, - "y": 6.185916423797607, - "heading": 0.2985001179522956, + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 8 }, { - "x": 3.8162126541137695, - "y": 6.581329345703125, - "heading": 0.2985001179522956, + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 20 + "controlIntervalCount": 22 }, { - "x": 8.264610290527344, - "y": 7.456887245178223, - "heading": 0, + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 9 }, { - "x": 8.377585411071777, - "y": 6.553085803985596, - "heading": 1.0808392342145412, + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 15 }, { - "x": 8.377585411071777, - "y": 1, - "heading": 1.0808392342145412, + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.4269569218158722, - "y": 6.9908647537231445, - "heading": -4.078401909973915e-26, - "angularVelocity": -1.0353470347432896e-26, - "velocityX": -4.552508560212021e-27, - "velocityY": 2.1159417555564544e-25, - "timestamp": 0 + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 0.4542226293224775, - "y": 6.990998038288888, - "heading": -0.0026449196556089733, - "angularVelocity": -0.03988238422563954, - "velocityX": 0.41113589997196526, - "velocityY": 0.002009779862709322, - "timestamp": 0.06631799244109911 - }, - { - "x": 0.5087389045980273, - "y": 6.990153139430081, - "heading": -0.007565779067559187, - "angularVelocity": -0.07420097066901893, - "velocityX": 0.8220435098961852, - "velocityY": -0.012740115128746688, - "timestamp": 0.13263598488219822 - }, - { - "x": 0.5904087421733784, - "y": 6.986785246694186, - "heading": -0.014243760110711712, - "angularVelocity": -0.10069636907485748, - "velocityX": 1.2314883875275162, - "velocityY": -0.050783997101327, - "timestamp": 0.19895397732329734 - }, - { - "x": 0.698928204332834, - "y": 6.978630694988513, - "heading": -0.02190403949445005, - "angularVelocity": -0.11550831232627977, - "velocityX": 1.6363502296279249, - "velocityY": -0.12296137753138935, - "timestamp": 0.26527196976439643 - }, - { - "x": 0.8334233004336311, - "y": 6.962161301124643, - "heading": -0.029295015883550983, - "angularVelocity": -0.1114475290497566, - "velocityX": 2.028033285540875, - "velocityY": -0.24833975302400757, - "timestamp": 0.3315899622054955 - }, - { - "x": 0.9912778011031088, - "y": 6.931626408492899, - "heading": -0.03421092005184831, - "angularVelocity": -0.07412625122305122, - "velocityX": 2.3802665741077376, - "velocityY": -0.46043149841822084, - "timestamp": 0.3979079546465946 - }, - { - "x": 1.1646969055280092, - "y": 6.878746569678396, - "heading": -0.03283456831141496, - "angularVelocity": 0.020753820943174132, - "velocityX": 2.6149631199847305, - "velocityY": -0.7973679067783651, - "timestamp": 0.4642259470876937 - }, - { - "x": 1.3384870937620608, - "y": 6.798699012794488, - "heading": -0.021425411587726872, - "angularVelocity": 0.17203712452274886, - "velocityX": 2.6205586423383487, - "velocityY": -1.2070262373367564, - "timestamp": 0.5305439395287929 - }, - { - "x": 1.5002212524414062, - "y": 6.694304943084717, - "heading": 2.0169023598696312e-25, - "angularVelocity": 0.3230708711026805, - "velocityX": 2.438767410261869, - "velocityY": -1.5741439972340743, - "timestamp": 0.596861931969892 - }, - { - "x": 1.618361440068333, - "y": 6.597461102589199, - "heading": 0.022914254950403036, - "angularVelocity": 0.43061521013807685, - "velocityX": 2.2201447016642413, - "velocityY": -1.8199339588312498, - "timestamp": 0.6500747642408552 - }, - { - "x": 1.720748899087832, - "y": 6.493018845204374, - "heading": 0.052155873821770504, - "angularVelocity": 0.5495219409195748, - "velocityX": 1.9241121859128945, - "velocityY": -1.9627269011541884, - "timestamp": 0.7032875965118184 - }, - { - "x": 1.8070404982970254, - "y": 6.395496343697618, - "heading": 0.0861590476515803, - "angularVelocity": 0.6390032700507929, - "velocityX": 1.6216313908981774, - "velocityY": -1.8326876684587161, - "timestamp": 0.7565004287827816 - }, - { - "x": 1.884907399599166, - "y": 6.313378100766396, - "heading": 0.12134509518920418, - "angularVelocity": 0.6612323764022606, - "velocityX": 1.4633105959411692, - "velocityY": -1.5432037616992567, - "timestamp": 0.8097132610537447 - }, - { - "x": 1.9587492444943766, - "y": 6.248351408561538, - "heading": 0.15586853154597385, - "angularVelocity": 0.6487802825636894, - "velocityX": 1.3876698860756558, - "velocityY": -1.2220114853826567, - "timestamp": 0.8629260933247079 - }, - { - "x": 2.0307407284297647, - "y": 6.200783008350587, - "heading": 0.18886810446406907, - "angularVelocity": 0.6201431404752012, - "velocityX": 1.3528970525155128, - "velocityY": -0.8939272386165963, - "timestamp": 0.9161389255956711 - }, - { - "x": 2.102117652739723, - "y": 6.170757252045742, - "heading": 0.21987485065465232, - "angularVelocity": 0.5826930247331111, - "velocityX": 1.341347965590374, - "velocityY": -0.5642578119494339, - "timestamp": 0.9693517578666343 - }, - { - "x": 2.173665780366198, - "y": 6.158282055577678, - "heading": 0.24860100716919956, - "angularVelocity": 0.5398351354100418, - "velocityX": 1.3445652969973032, - "velocityY": -0.23443962547489916, - "timestamp": 1.0225645901375975 - }, - { - "x": 2.2459257714568976, - "y": 6.1633423033591725, - "heading": 0.2748547098138841, - "angularVelocity": 0.49337164597815464, - "velocityX": 1.3579429623807138, - "velocityY": 0.09509450193755564, - "timestamp": 1.0757774224085606 - }, - { - "x": 2.319291353225708, - "y": 6.185916423797607, - "heading": 0.2985001179522956, - "angularVelocity": 0.4443553768761555, - "velocityX": 1.3787197305196603, - "velocityY": 0.42422324606752454, - "timestamp": 1.1289902546795236 - }, - { - "x": 2.405902969061742, - "y": 6.211704806035944, - "heading": 0.3149900926576246, - "angularVelocity": 0.32101497976485666, - "velocityX": 1.6860927079542964, - "velocityY": 0.5020296968516651, - "timestamp": 1.1803584952438984 - }, - { - "x": 2.5083069945465017, - "y": 6.241490500500008, - "heading": 0.32528072647931233, - "angularVelocity": 0.20033066557519108, - "velocityX": 1.9935279923871816, - "velocityY": 0.5798464992535075, - "timestamp": 1.231726735808273 - }, - { - "x": 2.626507788844443, - "y": 6.275274316431955, - "heading": 0.3295638724656478, - "angularVelocity": 0.08338120868609038, - "velocityX": 2.301048137901694, - "velocityY": 0.6576790554001625, - "timestamp": 1.2830949763726478 - }, - { - "x": 2.760511656599945, - "y": 6.31305748289524, - "heading": 0.32812899320961725, - "angularVelocity": -0.027933198417265603, - "velocityX": 2.6086910177032148, - "velocityY": 0.7355355380711394, - "timestamp": 1.3344632169370225 - }, - { - "x": 2.910328504957554, - "y": 6.354841933969171, - "heading": 0.32146257997595, - "angularVelocity": -0.12977694311552027, - "velocityX": 2.9165267626766083, - "velocityY": 0.8134296720084567, - "timestamp": 1.3858314575013972 - }, - { - "x": 3.0759758332063107, - "y": 6.400630905749405, - "heading": 0.31055238687433734, - "angularVelocity": -0.21239180049275855, - "velocityX": 3.2247031712360825, - "velocityY": 0.8913868039309617, - "timestamp": 1.437199698065772 - }, - { - "x": 3.2574854985287596, - "y": 6.450428350805819, - "heading": 0.2985001681214898, - "angularVelocity": -0.23462393534276735, - "velocityX": 3.5334997525364127, - "velocityY": 0.9694208816439399, - "timestamp": 1.4885679386301467 - }, - { - "x": 3.442976613157377, - "y": 6.497402957699113, - "heading": 0.298500151417318, - "angularVelocity": -3.251848140670727e-7, - "velocityX": 3.6110077470175392, - "velocityY": 0.9144678964510323, - "timestamp": 1.5399361791945214 - }, - { - "x": 3.628468017220463, - "y": 6.544376421672586, - "heading": 0.29850013471325804, - "angularVelocity": -3.251826373487328e-7, - "velocityX": 3.6110133815198173, - "velocityY": 0.9144456469091344, - "timestamp": 1.5913044197588961 - }, - { - "x": 3.8162126541137695, - "y": 6.581329345703125, - "heading": 0.2985001179522956, - "angularVelocity": -3.2629037443593046e-7, - "velocityX": 3.654877699344691, - "velocityY": 0.7193729749071157, - "timestamp": 1.6426726603232709 - }, - { - "x": 4.069115062985546, - "y": 6.6311069780063585, - "heading": 0.29850007681370144, - "angularVelocity": -5.945239460622418e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 1.711868517582718 - }, - { - "x": 4.322017471857322, - "y": 6.68088461030959, - "heading": 0.2985000356751073, - "angularVelocity": -5.945239466780917e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 1.781064374842165 - }, - { - "x": 4.574919880729098, - "y": 6.730662242612822, - "heading": 0.29849999453651316, - "angularVelocity": -5.9452394681569e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020963, - "timestamp": 1.8502602321016122 - }, - { - "x": 4.827822289600874, - "y": 6.780439874916056, - "heading": 0.2984999533979189, - "angularVelocity": -5.945239480024863e-7, - "velocityX": 3.6548778913675166, - "velocityY": 0.7193730127020963, - "timestamp": 1.9194560893610593 - }, - { - "x": 5.08072469847265, - "y": 6.830217507219288, - "heading": 0.2984999122593246, - "angularVelocity": -5.945239490427592e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 1.9886519466205064 - }, - { - "x": 5.333627107344427, - "y": 6.8799951395225225, - "heading": 0.29849987112073023, - "angularVelocity": -5.945239498975491e-7, - "velocityX": 3.6548778913675157, - "velocityY": 0.7193730127020962, - "timestamp": 2.0578478038799535 - }, - { - "x": 5.586529516216203, - "y": 6.929772771825754, - "heading": 0.2984998299821357, - "angularVelocity": -5.945239517809028e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.1270436611394006 - }, - { - "x": 5.839431925087979, - "y": 6.979550404128987, - "heading": 0.29849978884354117, - "angularVelocity": -5.94523951964415e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.1962395183988477 - }, - { - "x": 6.092334333959755, - "y": 7.0293280364322195, - "heading": 0.29849974770494664, - "angularVelocity": -5.945239518467797e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.265435375658295 - }, - { - "x": 6.345236742831531, - "y": 7.079105668735451, - "heading": 0.2984997065663521, - "angularVelocity": -5.945239526667429e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.334631232917742 - }, - { - "x": 6.5981391517033074, - "y": 7.128883301038686, - "heading": 0.29849966542775747, - "angularVelocity": -5.945239534790066e-7, - "velocityX": 3.6548778913675157, - "velocityY": 0.7193730127020961, - "timestamp": 2.403827090177189 - }, - { - "x": 6.851041560575084, - "y": 7.178660933341917, - "heading": 0.2984996242891628, - "angularVelocity": -5.945239543358476e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.473022947436636 - }, - { - "x": 7.103943969446842, - "y": 7.228438565645149, - "heading": 0.29849958315051817, - "angularVelocity": -5.94524676762677e-7, - "velocityX": 3.654877891367282, - "velocityY": 0.7193730127020501, - "timestamp": 2.542218804696083 - }, - { - "x": 7.353433038122888, - "y": 7.277544365703102, - "heading": 0.285740099482384, - "angularVelocity": -0.18439664126557498, - "velocityX": 3.605549212875508, - "velocityY": 0.7096638729950698, - "timestamp": 2.6114146619555303 - }, - { - "x": 7.577591625127541, - "y": 7.321664482119973, - "heading": 0.20789341833488353, - "angularVelocity": -1.1250193903316694, - "velocityX": 3.2394798747008724, - "velocityY": 0.6376121080694793, - "timestamp": 2.6806105192149774 - }, - { - "x": 7.772618972429793, - "y": 7.360050828957848, - "heading": 0.13154587005742394, - "angularVelocity": -1.1033543235283216, - "velocityX": 2.818482999220663, - "velocityY": 0.5547492054899759, - "timestamp": 2.7498063764744245 - }, - { - "x": 7.938688452208001, - "y": 7.392737530275035, - "heading": 0.06829117903160017, - "angularVelocity": -0.9141398565040214, - "velocityX": 2.399991652036878, - "velocityY": 0.47237945466343967, - "timestamp": 2.8190022337338716 - }, - { - "x": 8.075968311155869, - "y": 7.419757701303976, - "heading": 0.02342953211486201, - "angularVelocity": -0.648328508288165, - "velocityX": 1.9839317610177574, - "velocityY": 0.3904882763086567, - "timestamp": 2.8881980909933187 - }, - { - "x": 8.184579349560671, - "y": 7.441135118616469, - "heading": -3.788367805348886e-26, - "angularVelocity": -0.33859732421572447, - "velocityX": 1.569617643402696, - "velocityY": 0.3089407106026408, - "timestamp": 2.957393948252766 - }, - { - "x": 8.264610290527344, - "y": 7.456887245178223, - "heading": -1.8720614918263104e-26, - "angularVelocity": 4.317468010745751e-27, - "velocityX": 1.1565857283420875, - "velocityY": 0.22764551500088193, - "timestamp": 3.026589805512213 - }, - { - "x": 8.309090579989254, - "y": 7.457165429033815, - "heading": 0.013618338743486997, - "angularVelocity": 0.312683368289724, - "velocityX": 1.021288058215154, - "velocityY": 0.006387230234839926, - "timestamp": 3.070142933451108 - }, - { - "x": 8.347694581932577, - "y": 7.447770213119958, - "heading": 0.040485197139557096, - "angularVelocity": 0.6168755188780982, - "velocityX": 0.8863657737162873, - "velocityY": -0.21571851112597287, - "timestamp": 3.1136960613900033 - }, - { - "x": 8.38043908621251, - "y": 7.428658664796905, - "heading": 0.08013490839874948, - "angularVelocity": 0.910375744190425, - "velocityX": 0.7518289920731597, - "velocityY": -0.43881000579032564, - "timestamp": 3.1572491893288985 - }, - { - "x": 8.407344118270583, - "y": 7.3997816960160065, - "heading": 0.13202626739741974, - "angularVelocity": 1.1914496490693778, - "velocityX": 0.6177520038473495, - "velocityY": -0.6630285847990722, - "timestamp": 3.2008023172677937 - }, - { - "x": 8.42843706760286, - "y": 7.361082970163005, - "heading": 0.19556012614559412, - "angularVelocity": 1.4587668384533072, - "velocityX": 0.4843038911435242, - "velocityY": -0.8885406785775672, - "timestamp": 3.244355445206689 - }, - { - "x": 8.44375820295339, - "y": 7.312496178717057, - "heading": 0.2700814705675387, - "angularVelocity": 1.7110446011248115, - "velocityX": 0.35178036746348396, - "velocityY": -1.1155752467220192, - "timestamp": 3.287908573145584 - }, - { - "x": 8.453367300002943, - "y": 7.253940317433788, - "heading": 0.3548489824478594, - "angularVelocity": 1.9463013540439453, - "velocityX": 0.22062932111410738, - "velocityY": -1.3444697098546627, - "timestamp": 3.3314617010844794 - }, - { - "x": 8.457351243336406, - "y": 7.185313206736467, - "heading": 0.4489562663311582, - "angularVelocity": 2.1607468472834177, - "velocityX": 0.09147318509597613, - "velocityY": -1.575710263419076, - "timestamp": 3.3750148290233746 - }, - { - "x": 8.455833781713947, - "y": 7.106484187241007, - "heading": 0.5511965710397227, - "angularVelocity": 2.3474847742740175, - "velocityX": -0.034841622043504016, - "velocityY": -1.8099508169896936, - "timestamp": 3.41856795696227 - }, - { - "x": 8.448992060952785, - "y": 7.01728730637676, - "heading": 0.6598709993298483, - "angularVelocity": 2.4952152332800406, - "velocityX": -0.157089079130233, - "velocityY": -2.048001718484822, - "timestamp": 3.462121084901165 - }, - { - "x": 8.437091795531538, - "y": 6.917516589235633, - "heading": 0.7725277452001952, - "angularVelocity": 2.5866510903282087, - "velocityX": -0.27323560865576907, - "velocityY": -2.2907818993185733, - "timestamp": 3.50567421284006 - }, - { - "x": 8.420568790056167, - "y": 6.8069293867126985, - "heading": 0.8855178727469111, - "angularVelocity": 2.5943056881067363, - "velocityX": -0.3793758624765617, - "velocityY": -2.539133416045067, - "timestamp": 3.5492273407789554 - }, - { - "x": 8.400226551820241, - "y": 6.685307877393556, - "heading": 0.9927665754284469, - "angularVelocity": 2.4624799126254615, - "velocityX": -0.46706721649169625, - "velocityY": -2.7924862133846577, - "timestamp": 3.5927804687178506 - }, - { - "x": 8.377585411071777, - "y": 6.553085803985596, - "heading": 1.0808392342145412, - "angularVelocity": 2.022189058605845, - "velocityX": -0.5198510834911644, - "velocityY": -3.035880077166161, - "timestamp": 3.636333596656746 - }, - { - "x": 8.33115655720319, - "y": 6.248001091638859, - "heading": 1.1393178439925082, - "angularVelocity": 0.6668090360059423, - "velocityX": -0.529410316156976, - "velocityY": -3.478763324102234, - "timestamp": 3.724032782810915 - }, - { - "x": 8.325096274978753, - "y": 5.9213777688010065, - "heading": 1.139317843108129, - "angularVelocity": -1.0084235369622052e-8, - "velocityX": -0.06910306115935677, - "velocityY": -3.7243597935295676, - "timestamp": 3.811731968965084 - }, - { - "x": 8.328579577191661, - "y": 5.594716799276763, - "heading": 1.1393178410314753, - "angularVelocity": -2.3679281998093647e-8, - "velocityX": 0.03971875185687836, - "velocityY": -3.7247890641766865, - "timestamp": 3.8994311551192533 - }, - { - "x": 8.332062981879394, - "y": 5.2680558308452605, - "heading": 1.1393178389548215, - "angularVelocity": -2.3679282006774464e-8, - "velocityX": 0.03971992033781079, - "velocityY": -3.7247890517165767, - "timestamp": 3.9871303412734225 - }, - { - "x": 8.335546386568186, - "y": 4.94139486241377, - "heading": 1.1393178368781678, - "angularVelocity": -2.367928278808999e-8, - "velocityX": 0.039719920349857205, - "velocityY": -3.7247890517164484, - "timestamp": 4.074829527427592 - }, - { - "x": 8.339029791256978, - "y": 4.614733893982279, - "heading": 1.139317834801514, - "angularVelocity": -2.3679282128211672e-8, - "velocityX": 0.039719920349857364, - "velocityY": -3.724789051716449, - "timestamp": 4.162528713581761 - }, - { - "x": 8.34251319594577, - "y": 4.288072925550788, - "heading": 1.1393178327248603, - "angularVelocity": -2.3679283665933347e-8, - "velocityX": 0.03971992034985746, - "velocityY": -3.724789051716448, - "timestamp": 4.25022789973593 - }, - { - "x": 8.345996600634562, - "y": 3.9614119571192963, - "heading": 1.1393178306482066, - "angularVelocity": -2.367928231652133e-8, - "velocityX": 0.039719920349857295, - "velocityY": -3.724789051716449, - "timestamp": 4.337927085890099 - }, - { - "x": 8.349480005323354, - "y": 3.634750988687805, - "heading": 1.1393178285715528, - "angularVelocity": -2.3679282106335388e-8, - "velocityX": 0.03971992034985752, - "velocityY": -3.7247890517164484, - "timestamp": 4.425626272044268 - }, - { - "x": 8.352963410012144, - "y": 3.308090020256313, - "heading": 1.139317826494899, - "angularVelocity": -2.367928328328809e-8, - "velocityX": 0.03971992034985731, - "velocityY": -3.724789051716449, - "timestamp": 4.5133254581984374 - }, - { - "x": 8.356446814700936, - "y": 2.9814290518248217, - "heading": 1.1393178244182454, - "angularVelocity": -2.3679282343675033e-8, - "velocityX": 0.03971992034985722, - "velocityY": -3.7247890517164484, - "timestamp": 4.601024644352607 - }, - { - "x": 8.359930219389728, - "y": 2.6547680833933303, - "heading": 1.1393178223415916, - "angularVelocity": -2.3679282346751086e-8, - "velocityX": 0.03971992034985726, - "velocityY": -3.7247890517164484, - "timestamp": 4.688723830506776 - }, - { - "x": 8.363413624078524, - "y": 2.3281071149618393, - "heading": 1.1393178202649379, - "angularVelocity": -2.36792827125054e-8, - "velocityX": 0.039719920349909635, - "velocityY": -3.724789051716448, - "timestamp": 4.776423016660945 - }, - { - "x": 8.366897029211264, - "y": 2.001446146535253, - "heading": 1.139317818188205, - "angularVelocity": -2.368018344998753e-8, - "velocityX": 0.03971992541203934, - "velocityY": -3.7247890516605238, - "timestamp": 4.864122202815114 - }, - { - "x": 8.369950836003104, - "y": 1.7153186940356544, - "heading": 1.1226153865660347, - "angularVelocity": -0.19045138677579856, - "velocityX": 0.03482138119813859, - "velocityY": -3.262601000613699, - "timestamp": 4.951821388969283 - }, - { - "x": 8.372495686328458, - "y": 1.4768791360484632, - "heading": 1.108692502981797, - "angularVelocity": -0.1587572723851997, - "velocityX": 0.02901794688128086, - "velocityY": -2.718834329523098, - "timestamp": 5.039520575123452 - }, - { - "x": 8.374531573132332, - "y": 1.2861274855011322, - "heading": 1.0975518982602839, - "angularVelocity": -0.1270320194525941, - "velocityX": 0.023214432119068158, - "velocityY": -2.175067511025735, - "timestamp": 5.127219761277622 - }, - { - "x": 8.376058491392183, - "y": 1.1430637450377987, - "heading": 1.089195515779546, - "angularVelocity": -0.09528460692950849, - "velocityX": 0.01741086008672813, - "velocityY": -1.6313006623782995, - "timestamp": 5.214918947431791 - }, - { - "x": 8.377076437882632, - "y": 1.0476879159528336, - "heading": 1.083624509980256, - "angularVelocity": -0.06352403076462655, - "velocityX": 0.011607251276645152, - "velocityY": -1.0875337989716474, - "timestamp": 5.30261813358596 - }, - { - "x": 8.377585411071777, - "y": 1, - "heading": 1.0808392342145412, - "angularVelocity": -0.03175942546169668, - "velocityX": 0.005803624998872994, - "velocityY": -0.5437669155675123, - "timestamp": 5.390317319740129 - }, - { - "x": 8.377585411071777, - "y": 1, - "heading": 1.0808392342145412, - "angularVelocity": -2.228121369995549e-27, - "velocityX": -4.3209221674019054e-27, - "velocityY": -1.4392739239520783e-27, - "timestamp": 5.478016505894298 - } - ], - "trajectoryWaypoints": [ - { - "timestamp": 0, - "isStopPoint": true, - "x": 0.4269569218158722, - "y": 6.9908647537231445, - "heading": 0, + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 18 }, { - "timestamp": 0.596861931969892, - "isStopPoint": false, - "x": 1.5002212524414062, - "y": 6.694304943084717, - "heading": 0, + "x": 8.056792259216309, + "y": 7.362460613250732, + "heading": 0.24497813041233202, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 18 }, { - "timestamp": 1.1289902546795236, - "isStopPoint": false, - "x": 2.319291353225708, - "y": 6.185916423797607, - "heading": 0.2985001179522956, + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 11 }, { - "timestamp": 1.6426726603232709, - "isStopPoint": false, - "x": 3.8162126541137695, - "y": 6.581329345703125, - "heading": 0.2985001179522956, + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 20 + "controlIntervalCount": 16 }, { - "timestamp": 3.026589805512213, - "isStopPoint": false, - "x": 8.264610290527344, - "y": 7.456887245178223, + "x": 8.248542785644531, + "y": 4.086092948913574, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 16 }, { - "timestamp": 3.636333596656746, - "isStopPoint": false, - "x": 8.377585411071777, - "y": 6.553085803985596, - "heading": 1.0808392342145412, + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 8 }, { - "timestamp": 5.478016505894298, - "isStopPoint": true, - "x": 8.377585411071777, - "y": 1, - "heading": 1.0808392342145412, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 40 } ], - "constraints": [ + "trajectory": [ { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": -4.820407208942378e-26, + "angularVelocity": -1.0568380673400748e-29, + "velocityX": 2.0900789241101526e-25, + "velocityY": 6.12105194243679e-25, + "timestamp": 0 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 0.4535853919747023, + "y": 6.976442037279776, + "heading": -9.657051327438745e-26, + "angularVelocity": -2.7866726754125085e-27, + "velocityX": 0.3537634154941742, + "velocityY": -0.07356074460262349, + "timestamp": 0.05826629620784589 }, { - "scope": [ - 4, - 4 - ], - "type": "ZeroAngularVelocity" + "x": 0.49483104636031155, + "y": 6.967970532726724, + "heading": -1.449381860201486e-25, + "angularVelocity": -2.8078094367592775e-27, + "velocityX": 0.7078818643024581, + "velocityY": -0.1453928789781493, + "timestamp": 0.11653259241569178 }, { - "scope": [ - 3, - 4 - ], - "type": "StraightLine" + "x": 0.5567354919847137, + "y": 6.955441991018911, + "heading": -1.9330585876590946e-25, + "angularVelocity": -2.807809436759289e-27, + "velocityX": 1.0624400322886223, + "velocityY": -0.21502210580061862, + "timestamp": 0.17479888862353765 }, { - "scope": [ - 2 - ], - "type": "WptVelocityDirection", - "direction": 0.2985001179522956 - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [], - "eventMarkers": [], - "isTrajectoryStale": false - }, - "SourceLanephChaos": { - "waypoints": [ + "x": 0.6393312280567922, + "y": 6.939025561506136, + "heading": -2.416735315116705e-25, + "angularVelocity": -2.8078094367592704e-27, + "velocityX": 1.417555970563931, + "velocityY": -0.2817482932880241, + "timestamp": 0.23306518483138355 + }, { - "x": 1.4578553438186646, - "y": 1.4268364906311035, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 + "x": 0.7426606798556364, + "y": 6.918954250670703, + "heading": -2.9004008719315308e-25, + "angularVelocity": -2.7886377311391825e-27, + "velocityX": 1.7734000361075017, + "velocityY": -0.3444754882622709, + "timestamp": 0.29133148103922946 }, { - "x": 3.095994472503662, - "y": 1.2997395992279053, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": -3.4495163504680925e-25, + "angularVelocity": -1.1511759285858433e-25, + "velocityX": 2.130226970897995, + "velocityY": -0.40134567836399193, + "timestamp": 0.34959777724707536 + }, + { + "x": 1.01097419129112, + "y": 6.869549473556677, + "heading": 0.010039096018647366, + "angularVelocity": 0.17297490397522428, + "velocityX": 2.4844650740475953, + "velocityY": -0.44832534810612334, + "timestamp": 0.4076356531928559 + }, + { + "x": 1.175726572408581, + "y": 6.840802777300111, + "heading": 0.03011443547557583, + "angularVelocity": 0.34590065762715055, + "velocityX": 2.8387045258405714, + "velocityY": -0.4953092405280373, + "timestamp": 0.4656735291386364 + }, + { + "x": 1.3610383679335918, + "y": 6.809328854372862, + "heading": 0.06022199220724938, + "angularVelocity": 0.5187570399681801, + "velocityX": 3.1929458565666793, + "velocityY": -0.5422997036737339, + "timestamp": 0.5237114050844169 + }, + { + "x": 1.5460799084730097, + "y": 6.784713461455284, + "heading": 0.15060245758508448, + "angularVelocity": 1.5572669382709545, + "velocityX": 3.1882893287184593, + "velocityY": -0.4241263574251743, + "timestamp": 0.5817492810301974 + }, + { + "x": 1.7105613111663502, + "y": 6.762828401317669, + "heading": 0.23096913740746994, + "angularVelocity": 1.384728136802675, + "velocityX": 2.83403553305432, + "velocityY": -0.37708237562070457, + "timestamp": 0.6397871569759779 + }, + { + "x": 1.8544824093339345, + "y": 6.743675274341158, + "heading": 0.3013204880217799, + "angularVelocity": 1.2121627380029003, + "velocityX": 2.4797788654780635, + "velocityY": -0.33001081904520096, + "timestamp": 0.6978250329217583 + }, + { + "x": 1.977843185069378, + "y": 6.727255600600169, + "heading": 0.36164875379550304, + "angularVelocity": 1.0394637086664282, + "velocityX": 2.1255218893725236, + "velocityY": -0.2829130713937313, + "timestamp": 0.7558629088675388 + }, + { + "x": 2.0806437247378744, + "y": 6.713570715573333, + "heading": 0.41194198603949084, + "angularVelocity": 0.8665588018929588, + "velocityX": 1.7712664013502706, + "velocityY": -0.2357923132752318, + "timestamp": 0.8139007848133193 + }, + { + "x": 2.162884155033722, + "y": 6.702621710001363, + "heading": 0.45218713038285063, + "angularVelocity": 0.6934289666451124, + "velocityX": 1.4170130962869354, + "velocityY": -0.18865276155520178, + "timestamp": 0.8719386607590998 + }, + { + "x": 2.2245645766627997, + "y": 6.694409403888087, + "heading": 0.4823732249131576, + "angularVelocity": 0.5201102562489893, + "velocityX": 1.0627615263987253, + "velocityY": -0.1414990810647272, + "timestamp": 0.9299765367048802 + }, + { + "x": 2.265685008078604, + "y": 6.688934342765278, + "heading": 0.5024941236633388, + "angularVelocity": 0.34668565005684077, + "velocityX": 0.7085102744666172, + "velocityY": -0.09433600099222642, + "timestamp": 0.9880144126506607 }, { - "x": 6.202812194824219, - "y": 1.003179669380188, - "heading": 0.7165423509110156, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.17327126075488852, + "velocityX": 0.354257244182047, + "velocityY": -0.047168141046465786, + "timestamp": 1.0460522885964412 }, { - "x": 8.25048828125, - "y": 0.7348634600639343, - "heading": 0.7165423509110156, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 25 + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -1.480616560828957e-26, + "velocityX": -5.749006986314809e-23, + "velocityY": -3.283580548358229e-23, + "timestamp": 1.1040901645422216 }, { - "x": 8.377585411071777, - "y": 2.1329314708709717, - "heading": 3.7458973568177814, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 + "x": 2.3347330882331176, + "y": 6.713955255875249, + "heading": 0.5125504196, + "angularVelocity": -2.553583698003105e-17, + "velocityX": 0.5108360349814401, + "velocityY": 0.2924454065412527, + "timestamp": 1.1990085705343583 }, { - "x": 8.34934139251709, - "y": 2.556588649749756, - "heading": 5.290290323982317, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 2.4317085702199166, + "y": 6.7694721581924355, + "heading": 0.5125504196, + "angularVelocity": -7.602638325412282e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645902, + "timestamp": 1.293926976526495 }, { - "x": 8.377585411071777, - "y": 3.7993156909942627, - "heading": 3.770388538466339, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 + "x": 2.577171786567949, + "y": 6.852747507871406, + "heading": 0.5125504196, + "angularVelocity": -2.864728625251783e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461279, + "timestamp": 1.3888453825186315 + }, + { + "x": 2.771122709968306, + "y": 6.963781289278251, + "heading": 0.5125504196, + "angularVelocity": -4.95735356219891e-19, + "velocityX": 2.0433436631502695, + "velocityY": 1.1697813532187173, + "timestamp": 1.4837637885107682 + }, + { + "x": 2.9165859263163383, + "y": 7.047056638957223, + "heading": 0.5125504196, + "angularVelocity": -2.1000816812429605e-17, + "velocityX": 1.532507998080826, + "velocityY": 0.8773361584461278, + "timestamp": 1.5786821945029048 + }, + { + "x": 3.0135614083031372, + "y": 7.102573541274409, + "heading": 0.5125504196, + "angularVelocity": 1.0493910343453442e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645902, + "timestamp": 1.6736006004950414 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": 3.728395973644304e-17, + "velocityX": 0.5108360349814401, + "velocityY": 0.2924454065412527, + "timestamp": 1.768519006487178 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": -2.9402212487685217e-25, + "velocityX": 6.030156719342467e-23, + "velocityY": 3.0718511862526725e-23, + "timestamp": 1.8634374124793147 + }, + { + "x": 3.0865020118467, + "y": 7.124545249523675, + "heading": 0.48610131944841944, + "angularVelocity": -0.40878852104321894, + "velocityX": 0.3779353165675877, + "velocityY": -0.08943798979559645, + "timestamp": 1.9281385936281472 + }, + { + "x": 3.135458110441477, + "y": 7.112972562266447, + "heading": 0.4339971852577861, + "angularVelocity": -0.8053042195748789, + "velocityX": 0.7566492253389184, + "velocityY": -0.17886361657922661, + "timestamp": 1.9928397747769797 + }, + { + "x": 3.2089771746406726, + "y": 7.095606129050513, + "heading": 0.357225026215491, + "angularVelocity": -1.1865650314125755, + "velocityX": 1.136286276290371, + "velocityY": -0.2684098328280398, + "timestamp": 2.057540955925812 + }, + { + "x": 3.307133671873185, + "y": 7.072412045867193, + "heading": 0.2571531716690748, + "angularVelocity": -1.5466773986122515, + "velocityX": 1.5170742711284095, + "velocityY": -0.3584800581919169, + "timestamp": 2.1222421370746445 + }, + { + "x": 3.4300337444037163, + "y": 7.043313631479606, + "heading": 0.13605141213735358, + "angularVelocity": -1.8717086362480815, + "velocityX": 1.8995027656114087, + "velocityY": -0.44973544332447146, + "timestamp": 2.1869433182234768 + }, + { + "x": 3.5778386039995205, + "y": 7.008182862001031, + "heading": -0.0018831098932495121, + "angularVelocity": -2.1318702314461877, + "velocityX": 2.2844228957089365, + "velocityY": -0.5429695231956211, + "timestamp": 2.251644499372309 + }, + { + "x": 3.75075226330582, + "y": 6.966825747454083, + "heading": -0.14853260580564892, + "angularVelocity": -2.266565977753341, + "velocityX": 2.672496177597522, + "velocityY": -0.6392018478273882, + "timestamp": 2.3163456805211413 + }, + { + "x": 3.9486334270649626, + "y": 6.918918967044097, + "heading": -0.28405555900160556, + "angularVelocity": -2.094597823248583, + "velocityX": 3.0583856468393638, + "velocityY": -0.7404313114436918, + "timestamp": 2.3810468616699736 + }, + { + "x": 4.166070195160817, + "y": 6.865183550075389, + "heading": -0.3449865942152846, + "angularVelocity": -0.9417298746605423, + "velocityX": 3.360630582549694, + "velocityY": -0.8305167852361092, + "timestamp": 2.445748042818806 + }, + { + "x": 4.4009794486877345, + "y": 6.8112911031395695, + "heading": -0.3449866586118039, + "angularVelocity": -9.952912480962177e-7, + "velocityX": 3.6306795232463367, + "velocityY": -0.8329437883344079, + "timestamp": 2.510449223967638 + }, + { + "x": 4.635888809481111, + "y": 6.757399123762019, + "heading": -0.34498672300630057, + "angularVelocity": -9.952599862485997e-7, + "velocityX": 3.630681181121147, + "velocityY": -0.8329365619088571, + "timestamp": 2.5751504051164704 + }, + { + "x": 4.87079817027739, + "y": 6.703507144397122, + "heading": -0.3449867874007968, + "angularVelocity": -9.952599802438338e-7, + "velocityX": 3.630681181166014, + "velocityY": -0.8329365617132907, + "timestamp": 2.6398515862653027 + }, + { + "x": 5.105707531073669, + "y": 6.649615165032224, + "heading": -0.3449868517952927, + "angularVelocity": -9.952599738581064e-7, + "velocityX": 3.6306811811660147, + "velocityY": -0.8329365617132863, + "timestamp": 2.704552767414135 + }, + { + "x": 5.340616891869949, + "y": 6.595723185667327, + "heading": -0.3449869161897882, + "angularVelocity": -9.952599684354273e-7, + "velocityX": 3.630681181166015, + "velocityY": -0.8329365617132873, + "timestamp": 2.7692539485629672 + }, + { + "x": 5.575526252666228, + "y": 6.54183120630243, + "heading": -0.34498698058428334, + "angularVelocity": -9.952599624061458e-7, + "velocityX": 3.6306811811660156, + "velocityY": -0.8329365617132882, + "timestamp": 2.8339551297117995 + }, + { + "x": 5.8104356134625075, + "y": 6.4879392269375336, + "heading": -0.344987044978778, + "angularVelocity": -9.952599561054567e-7, + "velocityX": 3.6306811811660156, + "velocityY": -0.8329365617132889, + "timestamp": 2.898656310860632 + }, + { + "x": 6.0453449742587875, + "y": 6.434047247572637, + "heading": -0.3449871093732723, + "angularVelocity": -9.952599496144382e-7, + "velocityX": 3.6306811811660165, + "velocityY": -0.8329365617132901, + "timestamp": 2.963357492009464 + }, + { + "x": 6.280254335055067, + "y": 6.38015526820774, + "heading": -0.3449871737677662, + "angularVelocity": -9.952599439036616e-7, + "velocityX": 3.630681181166016, + "velocityY": -0.832936561713291, + "timestamp": 3.0280586731582964 + }, + { + "x": 6.515163695851346, + "y": 6.326263288842841, + "heading": -0.3449872381622598, + "angularVelocity": -9.952599382292206e-7, + "velocityX": 3.6306811811660133, + "velocityY": -0.8329365617133051, + "timestamp": 3.0927598543071286 + }, + { + "x": 6.750073056640409, + "y": 6.2723713094464895, + "heading": -0.34498730255675286, + "angularVelocity": -9.952599312963826e-7, + "velocityX": 3.6306811810544866, + "velocityY": -0.832936562199441, + "timestamp": 3.157461035455961 + }, + { + "x": 6.984982150775984, + "y": 6.2184781677494305, + "heading": -0.3449873669513136, + "angularVelocity": -9.95260976649588e-7, + "velocityX": 3.6306770597466, + "velocityY": -0.8329545263337457, + "timestamp": 3.222162216604793 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.00000172520007220815, + "velocityX": 3.5518244907979937, + "velocityY": -1.1225730284587905, + "timestamp": 3.2868633977536255 + }, + { + "x": 7.409091443638604, + "y": 6.073965873280571, + "heading": -0.3572291452032454, + "angularVelocity": -0.20035361948729366, + "velocityX": 3.1800506242888202, + "velocityY": -1.1764343450588084, + "timestamp": 3.347963699613786 + }, + { + "x": 7.58146433123023, + "y": 6.0094940876522065, + "heading": -0.36768136673125634, + "angularVelocity": -0.17106661030796205, + "velocityX": 2.8211462520452746, + "velocityY": -1.0551794944633763, + "timestamp": 3.4090640014739466 + }, + { + "x": 7.73220907640328, + "y": 5.953248314526567, + "heading": -0.37521177845828957, + "angularVelocity": -0.1232467188831242, + "velocityX": 2.4671685831938674, + "velocityY": -0.920548203744869, + "timestamp": 3.470164303334107 + }, + { + "x": 7.861444501366057, + "y": 5.905524901339061, + "heading": -0.37940510209885836, + "angularVelocity": -0.06863016241991673, + "velocityX": 2.1151356217282813, + "velocityY": -0.7810667334627726, + "timestamp": 3.5312646051942678 + }, + { + "x": 7.969234018825332, + "y": 5.866476846712542, + "heading": -0.38004587758642694, + "angularVelocity": -0.010487272043845954, + "velocityX": 1.7641405063099576, + "velocityY": -0.6390812064380308, + "timestamp": 3.5923649070544283 + }, + { + "x": 8.055617039018077, + "y": 5.8361975486879665, + "heading": -0.37700219526471646, + "angularVelocity": 0.04981452184436869, + "velocityX": 1.413790399766751, + "velocityY": -0.4955670774569341, + "timestamp": 3.653465208914589 + }, + { + "x": 8.12062042791295, + "y": 5.814749961981242, + "heading": -0.37018491999207215, + "angularVelocity": 0.11157514881427125, + "velocityX": 1.0638799959392298, + "velocityY": -0.3510225981503441, + "timestamp": 3.7145655107747495 + }, + { + "x": 8.164263678264032, + "y": 5.802179408572721, + "heading": -0.3595296539883727, + "angularVelocity": 0.17438974406519217, + "velocityX": 0.7142886208805774, + "velocityY": -0.20573635523587247, + "timestamp": 3.77566581263491 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.23800496841831012, + "velocityX": 0.3649393788537573, + "velocityY": -0.05989038131588724, + "timestamp": 3.8367661144950707 + }, + { + "x": 8.18566399540182, + "y": 5.805056139735724, + "heading": -0.32461060520225765, + "angularVelocity": 0.30765327861825625, + "velocityX": -0.013551942707795956, + "velocityY": 0.09868234683560065, + "timestamp": 3.9029993542741352 + }, + { + "x": 8.159694473473342, + "y": 5.822092590454781, + "heading": -0.2998178925011234, + "angularVelocity": 0.3743243239170494, + "velocityX": -0.3920919770058792, + "velocityY": 0.25721904554096775, + "timestamp": 3.9692325940532 + }, + { + "x": 8.108649030585239, + "y": 5.849626323109156, + "heading": -0.2708591800534793, + "angularVelocity": 0.4372232514103506, + "velocityX": -0.7706922243027172, + "velocityY": 0.41570867960287095, + "timestamp": 4.035465833832265 + }, + { + "x": 8.03252261804608, + "y": 5.887653107737229, + "heading": -0.2380618973255709, + "angularVelocity": 0.4951785966881702, + "velocityX": -1.1493686975466637, + "velocityY": 0.5741344490307231, + "timestamp": 4.1016990736113295 + }, + { + "x": 7.9313086642803885, + "y": 5.936166919703684, + "heading": -0.20187479302243566, + "angularVelocity": 0.5463586625664867, + "velocityX": -1.5281443894834816, + "velocityY": 0.7324692575553122, + "timestamp": 4.167932313390394 + }, + { + "x": 7.804998346649701, + "y": 5.9951585907413225, + "heading": -0.16295187123346636, + "angularVelocity": 0.5876644705710469, + "velocityX": -1.9070532870205719, + "velocityY": 0.8906656421219618, + "timestamp": 4.234165553169459 + }, + { + "x": 7.653579505512198, + "y": 6.0646127178391085, + "heading": -0.12233575906363518, + "angularVelocity": 0.6132285285351425, + "velocityX": -2.286145772765958, + "velocityY": 1.048629469575472, + "timestamp": 4.300398792948523 + }, + { + "x": 7.4770359425587705, + "y": 6.144498725543435, + "heading": -0.08194804569131871, + "angularVelocity": 0.6097801271240603, + "velocityX": -2.6654828231614, + "velocityY": 1.206131663962129, + "timestamp": 4.366632032727588 + }, + { + "x": 7.27536197665765, + "y": 6.234730628199731, + "heading": -0.046490249768474004, + "angularVelocity": 0.5353474485186261, + "velocityX": -3.044905648188839, + "velocityY": 1.362335633245254, + "timestamp": 4.432865272506652 + }, + { + "x": 7.049560259358204, + "y": 6.334147758763648, + "heading": -0.04649017070950518, + "angularVelocity": 0.0000011936448991960951, + "velocityX": -3.4091902804793617, + "velocityY": 1.5010156666885635, + "timestamp": 4.499098512285717 + }, + { + "x": 6.822106687595601, + "y": 6.429725315002885, + "heading": -0.04649016173832054, + "angularVelocity": 1.354483741042957e-7, + "velocityX": -3.4341302421763333, + "velocityY": 1.4430451621882883, + "timestamp": 4.565331752064782 + }, + { + "x": 6.586112566537963, + "y": 6.501676487205841, + "heading": -0.04649015261662469, + "angularVelocity": 1.3772081626072917e-7, + "velocityX": -3.563076815279552, + "velocityY": 1.086330254158866, + "timestamp": 4.631564991843846 + }, + { + "x": 6.343631547831337, + "y": 6.547208357601844, + "heading": -0.046490142857452815, + "angularVelocity": 1.4734553082512417e-7, + "velocityX": -3.6610170288434327, + "velocityY": 0.6874474289327321, + "timestamp": 4.697798231622911 + }, + { + "x": 6.097611695820765, + "y": 6.565767095818739, + "heading": -0.046490131800601416, + "angularVelocity": 1.6693810300562863e-7, + "velocityX": -3.7144468975279485, + "velocityY": 0.28020278456559833, + "timestamp": 4.764031471401975 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.04649011830549673, + "angularVelocity": 2.0375123930314497e-7, + "velocityX": -3.722715642994527, + "velocityY": -0.13044955895548105, + "timestamp": 4.83026471118104 + }, + { + "x": 5.59400062961639, + "y": 6.517937013895863, + "heading": -0.046490107217199095, + "angularVelocity": 1.588527062833638e-7, + "velocityX": -3.682446546399627, + "velocityY": -0.5614419262393832, + "timestamp": 4.900067095317195 + }, + { + "x": 5.343239064324899, + "y": 6.449191978638034, + "heading": -0.046490097789975614, + "angularVelocity": 1.3505589536533548e-7, + "velocityX": -3.5924498624912715, + "velocityY": -0.984852252664273, + "timestamp": 4.96986947945335 + }, + { + "x": 5.10214573961629, + "y": 6.351820274741271, + "heading": -0.04649007357112031, + "angularVelocity": 3.469631532168612e-7, + "velocityX": -3.453941118090443, + "velocityY": -1.3949624371974614, + "timestamp": 5.039671863589505 + }, + { + "x": 4.887305900136772, + "y": 6.241894655501948, + "heading": -0.013339477589168081, + "angularVelocity": 0.4749206834724951, + "velocityX": -3.0778295345966584, + "velocityY": -1.574811814807135, + "timestamp": 5.10947424772566 + }, + { + "x": 4.700625263351654, + "y": 6.142924243380359, + "heading": 0.02019201613120645, + "angularVelocity": 0.4803774847427688, + "velocityX": -2.6744163411522406, + "velocityY": -1.417865784190692, + "timestamp": 5.179276631861815 + }, + { + "x": 4.541203570947359, + "y": 6.05696873798045, + "heading": 0.05079450877042205, + "angularVelocity": 0.4384161517968095, + "velocityX": -2.283900390756435, + "velocityY": -1.2314121711408288, + "timestamp": 5.24907901599797 + }, + { + "x": 4.408671715563658, + "y": 5.984755616609754, + "heading": 0.07726906366461818, + "angularVelocity": 0.3792786624960486, + "velocityX": -1.8986723308073217, + "velocityY": -1.034536603074175, + "timestamp": 5.318881400134125 + }, + { + "x": 4.302830646527999, + "y": 5.92665565579208, + "heading": 0.09900038404891696, + "angularVelocity": 0.3113263343829381, + "velocityX": -1.5162959022890692, + "velocityY": -0.8323492318592651, + "timestamp": 5.38868378427028 + }, + { + "x": 4.22355609558814, + "y": 5.882893451015234, + "heading": 0.11561440292356019, + "angularVelocity": 0.23801506324248706, + "velocityX": -1.1356997604154628, + "velocityY": -0.6269442701482041, + "timestamp": 5.458486168406435 + }, + { + "x": 4.170763147512419, + "y": 5.853619589907405, + "heading": 0.1268589116332185, + "angularVelocity": 0.1610906109986881, + "velocityX": -0.756320127586832, + "velocityY": -0.4193819662481466, + "timestamp": 5.52828855254259 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.08154784945777828, + "velocityX": -0.37782436284418364, + "velocityY": -0.21027274576950902, + "timestamp": 5.598090936678745 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.1000727380078476e-26, + "velocityX": 7.869680140840765e-27, + "velocityY": 7.286496207304452e-27, + "timestamp": 5.6678933208149 + }, + { + "x": 4.195350058507561, + "y": 5.858786392293566, + "heading": 0.13540679339101, + "angularVelocity": 0.03040787160499198, + "velocityX": 0.5426383042255808, + "velocityY": 0.2113090624413005, + "timestamp": 5.7618047751106225 + }, + { + "x": 4.297269963634785, + "y": 5.898475069782893, + "heading": 0.14111775296085857, + "angularVelocity": 0.06081217262236255, + "velocityX": 1.085276613929165, + "velocityY": 0.42261806919047107, + "timestamp": 5.855716229406345 + }, + { + "x": 4.450149823252348, + "y": 5.958008075795516, + "heading": 0.14968325491595807, + "angularVelocity": 0.09120827719403884, + "velocityX": 1.6279149414101537, + "velocityY": 0.6339269949452273, + "timestamp": 5.949627683702067 + }, + { + "x": 4.653989639578877, + "y": 6.037385400048414, + "heading": 0.16110213512106505, + "angularVelocity": 0.12159198567141313, + "velocityX": 2.1705532925158373, + "velocityY": 0.8452358112030016, + "timestamp": 6.043539137997789 + }, + { + "x": 4.908789413625767, + "y": 6.136607028819001, + "heading": 0.17537288795521153, + "angularVelocity": 0.15195966180236836, + "velocityX": 2.7131916543911667, + "velocityY": 1.0565444813381684, + "timestamp": 6.137450592293511 + }, + { + "x": 5.214549136636689, + "y": 6.255672941521673, + "heading": 0.19249371968896806, + "angularVelocity": 0.1823082377134106, + "velocityX": 3.2558299230262406, + "velocityY": 1.2678529322710714, + "timestamp": 6.231362046589234 + }, + { + "x": 5.540526297381237, + "y": 6.382610310333338, + "heading": 0.1924937215917483, + "angularVelocity": 2.0261428816305725e-8, + "velocityX": 3.4711118381583637, + "velocityY": 1.3516707814144493, + "timestamp": 6.325273500884956 + }, + { + "x": 5.866503458135056, + "y": 6.50954767912126, + "heading": 0.19249372349451602, + "angularVelocity": 2.0261295290604937e-8, + "velocityX": 3.471111838257083, + "velocityY": 1.351670781161622, + "timestamp": 6.419184955180678 + }, + { + "x": 6.192480618888875, + "y": 6.636485047909182, + "heading": 0.19249372539728374, + "angularVelocity": 2.0261295568537564e-8, + "velocityX": 3.471111838257084, + "velocityY": 1.3516707811616195, + "timestamp": 6.5130964094764 + }, + { + "x": 6.518457779642694, + "y": 6.763422416697105, + "heading": 0.19249372730005146, + "angularVelocity": 2.0261295333491658e-8, + "velocityX": 3.4711118382570834, + "velocityY": 1.35167078116162, + "timestamp": 6.607007863772123 + }, + { + "x": 6.844434940394272, + "y": 6.890359785490778, + "heading": 0.19249372920281918, + "angularVelocity": 2.0261295628050687e-8, + "velocityX": 3.471111838233232, + "velocityY": 1.3516707812228705, + "timestamp": 6.700919318067845 + }, + { + "x": 7.170411838536006, + "y": 7.017297824914972, + "heading": 0.1924937316314852, + "angularVelocity": 2.586123310059887e-8, + "velocityX": 3.4711090418773667, + "velocityY": 1.351677922316834, + "timestamp": 6.794830772363567 + }, + { + "x": 7.445544445617428, + "y": 7.124436352045788, + "heading": 0.20746316204367837, + "angularVelocity": 0.15939941005551134, + "velocityX": 2.929702336575951, + "velocityY": 1.1408462144931066, + "timestamp": 6.888742226659289 + }, + { + "x": 7.669715374136242, + "y": 7.211730123643104, + "heading": 0.22019672303892965, + "angularVelocity": 0.13559113838397205, + "velocityX": 2.38704565060736, + "velocityY": 0.9295327417934781, + "timestamp": 6.9826536809550115 + }, + { + "x": 7.842925396345479, + "y": 7.279179365267254, + "heading": 0.23039470264394565, + "angularVelocity": 0.10859143521410211, + "velocityX": 1.8443971878425902, + "velocityY": 0.7182216709343664, + "timestamp": 7.076565135250734 + }, + { + "x": 7.96517479602962, + "y": 7.326784165564593, + "heading": 0.23795514608622043, + "angularVelocity": 0.08050608415100667, + "velocityX": 1.3017517469081525, + "velocityY": 0.506911544010745, + "timestamp": 7.170476589546456 + }, + { + "x": 8.0364637201711, + "y": 7.354544571992242, + "heading": 0.24282665608297047, + "angularVelocity": 0.05187343794517228, + "velocityX": 0.7591078710909263, + "velocityY": 0.2956019224261278, + "timestamp": 7.264388043842178 + }, + { + "x": 8.056792259216309, + "y": 7.362460613250732, + "heading": 0.244978130412332, + "angularVelocity": 0.022909605068905204, + "velocityX": 0.21646495837660984, + "velocityY": 0.0842926064542015, + "timestamp": 7.3582994981379 + }, + { + "x": 8.02239522092899, + "y": 7.349066085390599, + "heading": 0.24423351408772678, + "angularVelocity": -0.00758829012549376, + "velocityX": -0.3505358361841942, + "velocityY": -0.13650192741958841, + "timestamp": 7.4564265217876144 + }, + { + "x": 7.932360084343089, + "y": 7.314005643971181, + "heading": 0.24049622236652166, + "angularVelocity": -0.03808626392813261, + "velocityX": -0.9175366095613122, + "velocityY": -0.35729649300863325, + "timestamp": 7.5545535454373285 + }, + { + "x": 7.786686853677988, + "y": 7.257279283570004, + "heading": 0.23376594617518753, + "angularVelocity": -0.06858738745974144, + "velocityX": -1.4845373399392365, + "velocityY": -0.578091113857428, + "timestamp": 7.6526805690870425 + }, + { + "x": 7.5853755359232595, + "y": 7.178886996735875, + "heading": 0.2240420388382404, + "angularVelocity": -0.09909510117884326, + "velocityX": -2.0515379990873206, + "velocityY": -0.7988858106403784, + "timestamp": 7.750807592736757 + }, + { + "x": 7.328426142666821, + "y": 7.078828774757454, + "heading": 0.21132346369891478, + "angularVelocity": -0.1296133793350078, + "velocityX": -2.618538540144425, + "velocityY": -1.0196805961994813, + "timestamp": 7.848934616386471 + }, + { + "x": 7.01583869900668, + "y": 6.957104611217254, + "heading": 0.19560873409309495, + "angularVelocity": -0.1601468078958253, + "velocityX": -3.1855388254309114, + "velocityY": -1.2404754471583816, + "timestamp": 7.947061640036185 + }, + { + "x": 6.675228781735299, + "y": 6.824469292432169, + "heading": 0.19560873219026645, + "angularVelocity": -1.939148261699155e-8, + "velocityX": -3.471112284901898, + "velocityY": -1.3516696405524002, + "timestamp": 8.045188663685899 + }, + { + "x": 6.334618864455473, + "y": 6.691833973668682, + "heading": 0.19560873028745154, + "angularVelocity": -1.9391344572203613e-8, + "velocityX": -3.4711122849879685, + "velocityY": -1.3516696403322965, + "timestamp": 8.143315687335612 + }, + { + "x": 5.994008947175647, + "y": 6.559198654905194, + "heading": 0.19560872838463667, + "angularVelocity": -1.9391344282542406e-8, + "velocityX": -3.471112284987969, + "velocityY": -1.3516696403322948, + "timestamp": 8.241442710985325 + }, + { + "x": 5.65339902989582, + "y": 6.426563336141707, + "heading": 0.19560872648182165, + "angularVelocity": -1.9391345115512158e-8, + "velocityX": -3.4711122849879685, + "velocityY": -1.3516696403322956, + "timestamp": 8.339569734635038 + }, + { + "x": 5.312789112621013, + "y": 6.293928017365348, + "heading": 0.195608724578999, + "angularVelocity": -1.939142318059707e-8, + "velocityX": -3.4711122849368206, + "velocityY": -1.3516696404634736, + "timestamp": 8.437696758284751 + }, + { + "x": 4.978960833752885, + "y": 6.1639320605180385, + "heading": 0.17759686591379087, + "angularVelocity": -0.18355655756466713, + "velocityX": -3.402001471682276, + "velocityY": -1.324772239208634, + "timestamp": 8.535823781934464 + }, + { + "x": 4.700770593105711, + "y": 6.055602072933751, + "heading": 0.16258358561165714, + "angularVelocity": -0.15299842738252176, + "velocityX": -2.8350013105486314, + "velocityY": -1.1039771059498866, + "timestamp": 8.633950805584178 + }, + { + "x": 4.478218399750123, + "y": 5.968938070584376, + "heading": 0.1505712213556835, + "angularVelocity": -0.1224164741697909, + "velocityX": -2.2680010569772926, + "velocityY": -0.8831818099236476, + "timestamp": 8.73207782923389 + }, + { + "x": 4.311304254190931, + "y": 5.9039400626948755, + "heading": 0.1415612155720731, + "angularVelocity": -0.09181982137533895, + "velocityX": -1.701000798261533, + "velocityY": -0.6623864198869991, + "timestamp": 8.830204852883604 + }, + { + "x": 4.2000281561602835, + "y": 5.860608055094718, + "heading": 0.13555442251274583, + "angularVelocity": -0.06121446300837455, + "velocityX": -1.1340005422754125, + "velocityY": -0.4415909704429648, + "timestamp": 8.928331876533317 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": -0.030606008971789937, + "velocityX": -0.5670002807557282, + "velocityY": -0.2207954888983711, + "timestamp": 9.02645890018303 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.2638912367197548e-25, + "velocityX": -6.105559884328861e-26, + "velocityY": -2.4380402560227299e-26, + "timestamp": 9.124585923832743 + }, + { + "x": 4.149229218776726, + "y": 5.815100726386144, + "heading": 0.12918022174703217, + "angularVelocity": -0.05380546988265665, + "velocityX": 0.07724016042252432, + "velocityY": -0.3805465783235216, + "timestamp": 9.187236137776921 + }, + { + "x": 4.159717289478365, + "y": 5.7675956417955225, + "heading": 0.12259706469587767, + "angularVelocity": -0.10507796600695042, + "velocityX": 0.16740678189838681, + "velocityY": -0.7582589364012235, + "timestamp": 9.249886351721099 }, { - "x": 8.179879188537598, - "y": 4.208850860595703, - "heading": 5.281700753397833, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 4.176867312970838, + "y": 5.696690246498273, + "heading": 0.11300853276979594, + "angularVelocity": -0.15304867010709985, + "velocityX": 0.2737424569332353, + "velocityY": -1.1317662116912757, + "timestamp": 9.312536565665276 }, { - "x": 8.44819450378418, - "y": 5.38096809387207, - "heading": 3.7759750997570256, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 + "x": 4.201974040585745, + "y": 5.602790592825652, + "heading": 0.10069220099175857, + "angularVelocity": -0.19658882233046115, + "velocityX": 0.40074448328743983, + "velocityY": -1.498792226891454, + "timestamp": 9.375186779609454 }, { - "x": 8.34934139251709, - "y": 5.974088191986084, - "heading": 5.326548029576795, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 4.2367303910144685, + "y": 5.486551757326023, + "heading": 0.08603276386217494, + "angularVelocity": -0.2339886204162901, + "velocityX": 0.5547682639949261, + "velocityY": -1.8553621477366333, + "timestamp": 9.437836993553631 }, { - "x": 8.44819450378418, - "y": 6.821402072906494, - "heading": 3.7101172753280305, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 7 + "x": 4.283393708275681, + "y": 5.349087774330418, + "heading": 0.06958159023755035, + "angularVelocity": -0.2625876687234105, + "velocityX": 0.7448229514872903, + "velocityY": -2.1941502565033004, + "timestamp": 9.500487207497809 }, { - "x": 8.321097373962402, - "y": 7.527496814727783, - "heading": 3.7090325363569563, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ + "x": 4.3449830971903305, + "y": 5.19239819137041, + "heading": 0.05215018239550137, + "angularVelocity": -0.2782338119001568, + "velocityX": 0.9830674955001105, + "velocityY": -2.5010223125434243, + "timestamp": 9.563137421441986 + }, { - "x": 1.4578553438186646, - "y": 1.4268364906311035, - "heading": 1.9268906266143656e-30, - "angularVelocity": 1.1190631191470915e-30, - "velocityX": -2.439449667203012e-26, - "velocityY": -3.284743628951617e-25, - "timestamp": 0 + "x": 4.425324635342363, + "y": 5.020190178284826, + "heading": 0.03492497946542863, + "angularVelocity": -0.2749424438585412, + "velocityX": 1.2823825026937377, + "velocityY": -2.7487218677181704, + "timestamp": 9.625787635386164 }, { - "x": 1.4836061256801238, - "y": 1.4248385832903434, - "heading": -1.6036305967396036e-15, - "angularVelocity": -2.484855550358542e-14, - "velocityX": 0.3990131727631876, - "velocityY": -0.030957947265928064, - "timestamp": 0.0645361697788919 - }, - { - "x": 1.5351076891841593, - "y": 1.420842768625806, - "heading": -4.81143470802221e-15, - "angularVelocity": -4.970552361990032e-14, - "velocityX": 0.7980263421347389, - "velocityY": -0.061915894268711684, - "timestamp": 0.1290723395577838 - }, - { - "x": 1.6123600340493498, - "y": 1.4148490466593255, - "heading": -9.625413675640745e-15, - "angularVelocity": -7.459350290095871e-14, - "velocityX": 1.1970395071456144, - "velocityY": -0.09287384093316672, - "timestamp": 0.1936085093366757 - }, - { - "x": 1.7153631599004666, - "y": 1.4068574174200144, - "heading": -1.6044392495578035e-14, - "angularVelocity": -9.946327527539101e-14, - "velocityX": 1.5960526663422558, - "velocityY": -0.12383178714651694, - "timestamp": 0.2581446791155676 - }, - { - "x": 1.8441170662121902, - "y": 1.39686788094863, - "heading": -2.407162986647994e-14, - "angularVelocity": -1.2438354179375857e-13, - "velocityX": 1.9950658173989684, - "velocityY": -0.15478973272832033, - "timestamp": 0.32268084889445947 - }, - { - "x": 1.9986217521965406, - "y": 1.3848804373063093, - "heading": -3.3704756925123664e-14, - "angularVelocity": -1.4926710233420884e-13, - "velocityX": 2.3940789562457856, - "velocityY": -0.1857476773628035, - "timestamp": 0.38721701867335134 - }, - { - "x": 2.1788772165402177, - "y": 1.3708950865949459, - "heading": -4.494558790872857e-14, - "angularVelocity": -1.7417877481910134e-13, - "velocityX": 2.793092074742775, - "velocityY": -0.2167056204184192, - "timestamp": 0.4517531884522432 - }, - { - "x": 2.3848834566166217, - "y": 1.3549118290183289, - "heading": -5.77927518216841e-14, - "angularVelocity": -1.9906920347103564e-13, - "velocityX": 3.1921051525401003, - "velocityY": -0.2476635603162993, - "timestamp": 0.5162893582311351 - }, - { - "x": 2.6166404645459465, - "y": 1.3369306651878214, - "heading": -7.224642245471218e-14, - "angularVelocity": -2.239623250426539e-13, - "velocityX": 3.5911181082383807, - "velocityY": -0.27862149074096887, - "timestamp": 0.580825528010027 - }, - { - "x": 2.856317459743021, - "y": 1.3183350190196794, - "heading": -7.226675860496505e-14, - "angularVelocity": -3.151124450451814e-16, - "velocityX": 3.7138397896595787, - "velocityY": -0.2881430092900255, - "timestamp": 0.6453616977889188 - }, - { - "x": 3.095994472503662, - "y": 1.2997395992279053, - "heading": 8.152675815601398e-24, - "angularVelocity": 1.119786917331966e-12, - "velocityX": 3.7138400618103087, - "velocityY": -0.28813950154594287, - "timestamp": 0.7098978675678107 - }, - { - "x": 3.27382254607751, - "y": 1.289661887920689, - "heading": 0.031144225689139507, - "angularVelocity": 0.6214983848668802, - "velocityX": 3.548646918156489, - "velocityY": -0.20110569975653228, - "timestamp": 0.7600093826537434 - }, - { - "x": 3.4512411724559873, - "y": 1.2794797717459663, - "heading": 0.06298682604506026, - "angularVelocity": 0.6354347958012462, - "velocityX": 3.5404761974215813, - "velocityY": -0.20318915038314128, - "timestamp": 0.8101208977396761 - }, - { - "x": 3.62822231090223, - "y": 1.2691838802186675, - "heading": 0.09560610924482929, - "angularVelocity": 0.6509338850328615, - "velocityX": 3.5317459099520483, - "velocityY": -0.20545959366110889, - "timestamp": 0.8602324128256088 - }, - { - "x": 3.8047300835293036, - "y": 1.258765647314116, - "heading": 0.12910425796921293, - "angularVelocity": 0.6684720800586451, - "velocityX": 3.5222996615526942, - "velocityY": -0.20790097618653766, - "timestamp": 0.9103439279115415 - }, - { - "x": 3.9807203074364863, - "y": 1.248215472268555, - "heading": 0.16360694737365913, - "angularVelocity": 0.6885181847980438, - "velocityX": 3.5119717215771487, - "velocityY": -0.21053394668808642, - "timestamp": 0.9604554429974742 - }, - { - "x": 4.156137440439902, - "y": 1.2375224650571883, - "heading": 0.19927204657899403, - "angularVelocity": 0.7117146457091811, - "velocityX": 3.50053540992732, - "velocityY": -0.2133842330057237, - "timestamp": 1.010566958083407 - }, - { - "x": 4.330909871790797, - "y": 1.2266740963817457, - "heading": 0.23630304667549967, - "angularVelocity": 0.7389718716946344, - "velocityX": 3.487670070465641, - "velocityY": -0.21648454765016575, - "timestamp": 1.0606784731693397 - }, - { - "x": 4.504942288680689, - "y": 1.2156556997827002, - "heading": 0.27497084629517315, - "angularVelocity": 0.7716350134966949, - "velocityX": 3.4729027168996156, - "velocityY": -0.21987753872839294, - "timestamp": 1.1107899882552725 - }, - { - "x": 4.678102490303239, - "y": 1.204449734224157, - "heading": 0.31565141892272963, - "angularVelocity": 0.8118008916273269, - "velocityX": 3.4554972310378296, - "velocityY": -0.22362056982965403, - "timestamp": 1.1609015033412053 - }, - { - "x": 4.850196619661247, - "y": 1.1930346345230565, - "heading": 0.3588966354181107, - "angularVelocity": 0.8629796249668915, - "velocityX": 3.4342232331809583, - "velocityY": -0.22779394479542386, - "timestamp": 1.2110130184271382 - }, - { - "x": 5.020916905370504, - "y": 1.1813828932131683, - "heading": 0.40558384210290876, - "angularVelocity": 0.931666236886622, - "velocityX": 3.4068075055503675, - "velocityY": -0.23251624481733327, - "timestamp": 1.261124533513071 - }, - { - "x": 5.189710400965398, - "y": 1.16945753417408, - "heading": 0.45729195406784345, - "angularVelocity": 1.0318608782086127, - "velocityX": 3.3683574584692115, - "velocityY": -0.2379764215597622, - "timestamp": 1.3112360485990038 - }, - { - "x": 5.35533172203286, - "y": 1.1572047491345996, - "heading": 0.5175844077279528, - "angularVelocity": 1.2031656507834203, - "velocityX": 3.3050551511653112, - "velocityY": -0.244510368893633, - "timestamp": 1.3613475636849366 - }, - { - "x": 5.512680992157926, - "y": 1.1445116485154183, - "heading": 0.6010844246558354, - "angularVelocity": 1.6662840224386364, - "velocityX": 3.1399822945931812, - "velocityY": -0.25329708346305213, - "timestamp": 1.4114590787708694 - }, - { - "x": 5.678954750693719, - "y": 1.128816110929195, - "heading": 0.6579086700766517, - "angularVelocity": 1.1339558447469087, - "velocityX": 3.3180748626470646, - "velocityY": -0.3132121940298074, - "timestamp": 1.4615705938568022 - }, - { - "x": 5.854338980055963, - "y": 1.1023762612022525, - "heading": 0.6848690400817262, - "angularVelocity": 0.5380074810917644, - "velocityX": 3.4998788015387303, - "velocityY": -0.5276202422058656, - "timestamp": 1.511682108942735 - }, - { - "x": 6.029328591050374, - "y": 1.0604680679443284, - "heading": 0.7046293616016649, - "angularVelocity": 0.39432696229705316, - "velocityX": 3.4920039973713513, - "velocityY": -0.8362986668046034, - "timestamp": 1.5617936240286678 - }, - { - "x": 6.202812194824219, - "y": 1.003179669380188, - "heading": 0.7165423509110156, - "angularVelocity": 0.2377295775017328, - "velocityX": 3.4619508804782524, - "velocityY": -1.1432182496558028, - "timestamp": 1.6119051391146006 - }, - { - "x": 6.402547568535553, - "y": 0.9154521762914801, - "heading": 0.7165423052284949, - "angularVelocity": -7.80040343805035e-7, - "velocityX": 3.4105309321224193, - "velocityY": -1.4979686533092873, - "timestamp": 1.6704694441118393 - }, - { - "x": 6.592790671542091, - "y": 0.8086912505756066, - "heading": 0.7165422622385289, - "angularVelocity": -7.34064308321193e-7, - "velocityX": 3.2484480609051705, - "velocityY": -1.8229692253823835, - "timestamp": 1.729033749109078 - }, - { - "x": 6.792526057186022, - "y": 0.7209637957417591, - "heading": 0.7165421996040524, - "angularVelocity": -0.0000010694991862536609, - "velocityX": 3.410531135874468, - "velocityY": -1.4979680000980806, - "timestamp": 1.7875980541063166 - }, - { - "x": 6.995358135542775, - "y": 0.6540115645265273, - "heading": 0.702913894528627, - "angularVelocity": -0.23270668158817906, - "velocityX": 3.4634079302455083, - "velocityY": -1.1432259158268625, - "timestamp": 1.8461623591035552 - }, - { - "x": 7.192981165480996, - "y": 0.6064556135405038, - "heading": 0.6594934468717042, - "angularVelocity": -0.7414148884541547, - "velocityX": 3.3744621394813583, - "velocityY": -0.8120296311595578, - "timestamp": 1.9047266641007938 - }, - { - "x": 7.3826916988169575, - "y": 0.5778864686152094, - "heading": 0.5928355536169142, - "angularVelocity": -1.138200022316206, - "velocityX": 3.239354301991779, - "velocityY": -0.4878252192464604, - "timestamp": 1.9632909690980325 - }, - { - "x": 7.56045020150032, - "y": 0.5667919314464598, - "heading": 0.5348000913331561, - "angularVelocity": -0.9909698798012667, - "velocityX": 3.0352704209798658, - "velocityY": -0.18944196758201154, - "timestamp": 2.021855274095271 - }, - { - "x": 7.725161762075186, - "y": 0.5716432246000058, - "heading": 0.4994377275755845, - "angularVelocity": -0.6038211118400366, - "velocityX": 2.812490655914593, - "velocityY": 0.08283703108531969, - "timestamp": 2.0804195790925095 - }, - { - "x": 7.876544251370993, - "y": 0.5914886747454028, - "heading": 0.49476791383000396, - "angularVelocity": -0.07973822528587567, - "velocityX": 2.5848934654470064, - "velocityY": 0.3388659721366544, - "timestamp": 2.138983884089748 - }, - { - "x": 8.014557697135269, - "y": 0.6256824372518744, - "heading": 0.5263224795620095, - "angularVelocity": 0.5388020182855987, - "velocityX": 2.35661373887698, - "velocityY": 0.5838669562984516, - "timestamp": 2.197548189086987 - }, - { - "x": 8.139220711960105, - "y": 0.6736709753190644, - "heading": 0.5988363569407734, - "angularVelocity": 1.238192400339819, - "velocityX": 2.128651826922783, - "velocityY": 0.8194161626173599, - "timestamp": 2.2561124940842254 - }, - { - "x": 8.25048828125, - "y": 0.7348634600639343, - "heading": 0.7165423509110156, - "angularVelocity": 2.0098589742641386, - "velocityX": 1.8999212796112497, - "velocityY": 1.044876819553399, - "timestamp": 2.314676799081464 - }, - { - "x": 8.302330173893822, - "y": 0.7683170831186208, - "heading": 0.7866903105882892, - "angularVelocity": 2.4170681239593663, - "velocityX": 1.7863012234653046, - "velocityY": 1.152701893090581, - "timestamp": 2.343698719887885 - }, - { - "x": 8.350761252955918, - "y": 0.804862507674631, - "heading": 0.8681048778617475, - "angularVelocity": 2.805278389962577, - "velocityX": 1.6687757983055662, - "velocityY": 1.2592352105076432, - "timestamp": 2.372720640694306 - }, - { - "x": 8.395659126629422, - "y": 0.8444668269434722, - "heading": 0.9600582095499061, - "angularVelocity": 3.168409572250435, - "velocityX": 1.5470331537659474, - "velocityY": 1.36463466815329, - "timestamp": 2.4017425615007273 - }, - { - "x": 8.436895225190787, - "y": 0.8871049250141315, - "heading": 1.0616235787520496, - "angularVelocity": 3.499608791561171, - "velocityX": 1.420860419143676, - "velocityY": 1.469168714057877, - "timestamp": 2.4307644823071484 - }, - { - "x": 8.474338814577639, - "y": 0.9327584709704907, - "heading": 1.1716968936719283, - "angularVelocity": 3.792764636568266, - "velocityX": 1.2901830184364116, - "velocityY": 1.5730711368442005, - "timestamp": 2.4597864031135694 - }, - { - "x": 8.50785937445302, - "y": 0.9814068916097549, - "heading": 1.2890700861945108, - "angularVelocity": 4.0442944250820885, - "velocityX": 1.1550083159196811, - "velocityY": 1.676264674683452, - "timestamp": 2.4888083239199905 - }, - { - "x": 8.537323038411628, - "y": 1.0330097209040148, - "heading": 1.4125130240648722, - "angularVelocity": 4.253437899363618, - "velocityX": 1.0152210170765539, - "velocityY": 1.7780638862070914, - "timestamp": 2.5178302447264116 - }, - { - "x": 8.56258232091714, - "y": 1.0874855013153344, - "heading": 1.5407726955584362, - "angularVelocity": 4.419406708090351, - "velocityX": 0.870351851415892, - "velocityY": 1.877056338713015, - "timestamp": 2.5468521655328327 - }, - { - "x": 8.583466101858683, - "y": 1.1446918380868547, - "heading": 1.6724319994758665, - "angularVelocity": 4.536546867301084, - "velocityX": 0.7195864491822832, - "velocityY": 1.9711423359291682, - "timestamp": 2.5758740863392537 - }, - { - "x": 8.599781102269066, - "y": 1.2044006523964297, - "heading": 1.8056614857101974, - "angularVelocity": 4.590650188972121, - "velocityX": 0.5621612890203437, - "velocityY": 2.057369486597329, - "timestamp": 2.604896007145675 - }, - { - "x": 8.61133776585303, - "y": 1.2662515535930832, - "heading": 1.9379574799883315, - "angularVelocity": 4.558485124418899, - "velocityX": 0.3982046419686604, - "velocityY": 2.131178759986453, - "timestamp": 2.633917927952096 - }, - { - "x": 8.61803446841016, - "y": 1.329807234083184, - "heading": 2.0667068830619315, - "angularVelocity": 4.43628124865926, - "velocityX": 0.23074635899526613, - "velocityY": 2.189919851067852, - "timestamp": 2.662939848758517 - }, - { - "x": 8.619867506604743, - "y": 1.394964606545626, - "heading": 2.191275336084359, - "angularVelocity": 4.292219452093143, - "velocityX": 0.06316047124545049, - "velocityY": 2.245108891897538, - "timestamp": 2.691961769564938 - }, - { - "x": 8.616839586460362, - "y": 1.4616264782251567, - "heading": 2.311063053299335, - "angularVelocity": 4.12749100977746, - "velocityX": -0.10433217582589409, - "velocityY": 2.2969489898401783, - "timestamp": 2.720983690371359 - }, - { - "x": 8.608949954788589, - "y": 1.5296853158108266, - "heading": 2.4255531996922417, - "angularVelocity": 3.9449541316223122, - "velocityX": -0.2718507752949118, - "velocityY": 2.345083843334451, - "timestamp": 2.75000561117778 - }, - { - "x": 8.596173618614296, - "y": 1.5989753133446671, - "heading": 2.534426417053211, - "angularVelocity": 3.7514132192408356, - "velocityX": -0.44023055053838084, - "velocityY": 2.3875055684980784, - "timestamp": 2.7790275319842013 - }, - { - "x": 8.57842938977896, - "y": 1.6691611767333963, - "heading": 2.6378090883600067, - "angularVelocity": 3.5622270488699845, - "velocityX": -0.6114078028705309, - "velocityY": 2.4183741612719314, - "timestamp": 2.8080494527906223 - }, - { - "x": 8.555564514046702, - "y": 1.7394263904616403, - "heading": 2.737024931725787, - "angularVelocity": 3.4186518538024746, - "velocityX": -0.7878484640892429, - "velocityY": 2.421108313158164, - "timestamp": 2.8370713735970434 - }, - { - "x": 8.527959255725655, - "y": 1.8075255209054102, - "heading": 2.8376495221341176, - "angularVelocity": 3.4671926465345364, - "velocityX": -0.9511864671252386, - "velocityY": 2.3464722027875915, - "timestamp": 2.8660932944034645 - }, - { - "x": 8.499577272044599, - "y": 1.8711589675729616, - "heading": 2.9502161532820788, - "angularVelocity": 3.878676118606718, - "velocityX": -0.9779498700435901, - "velocityY": 2.1925994179362642, - "timestamp": 2.8951152152098856 - }, - { - "x": 8.472091457883874, - "y": 1.93067768486564, - "heading": 3.076977426653027, - "angularVelocity": 4.3677768338097795, - "velocityX": -0.9470708139567483, - "velocityY": 2.0508193682173372, - "timestamp": 2.9241371360163066 - }, - { - "x": 8.445935000391486, - "y": 1.9864273544904458, - "heading": 3.219502150557654, - "angularVelocity": 4.910933526946052, - "velocityX": -0.9012655525750104, - "velocityY": 1.920950373914328, - "timestamp": 2.9531590568227277 - }, - { - "x": 8.421236309719301, - "y": 2.038741873704351, - "heading": 3.37984910584556, - "angularVelocity": 5.52502904123526, - "velocityX": -0.8510356994262128, - "velocityY": 1.8025863816129806, - "timestamp": 2.982180977629149 - }, - { - "x": 8.398072066125595, - "y": 2.08798012126862, - "heading": 3.5600291132631146, - "angularVelocity": 6.208410829158125, - "velocityX": -0.7981636966145051, - "velocityY": 1.6965881718406162, - "timestamp": 3.01120289843557 - }, - { - "x": 8.377585411071777, - "y": 2.1329314708709717, - "heading": 3.745897356817782, - "angularVelocity": 6.404408750007486, - "velocityX": -0.7059027963884773, - "velocityY": 1.5488757585061539, - "timestamp": 3.040224819241991 - }, - { - "x": 8.364981537655835, - "y": 2.1618201695949115, - "heading": 3.87540454879606, - "angularVelocity": 6.50944417189865, - "velocityX": -0.6335108428921614, - "velocityY": 1.4520380580394667, - "timestamp": 3.0601200957906958 - }, - { - "x": 8.354284801682704, - "y": 2.189254221668137, - "heading": 4.007147516190271, - "angularVelocity": 6.621821369092149, - "velocityX": -0.5376520375047442, - "velocityY": 1.3789228818240207, - "timestamp": 3.0800153723394006 - }, - { - "x": 8.345932812421994, - "y": 2.2160755919920243, - "heading": 4.140533212375188, - "angularVelocity": 6.704390153028566, - "velocityX": -0.41979759568875646, - "velocityY": 1.3481275446574927, - "timestamp": 3.0999106488881054 - }, - { - "x": 8.339986678909588, - "y": 2.2433741294975276, - "heading": 4.274319407548544, - "angularVelocity": 6.724520508465367, - "velocityX": -0.2988716189920965, - "velocityY": 1.3721114878033795, - "timestamp": 3.1198059254368102 - }, - { - "x": 8.33607844515623, - "y": 2.2720346987144775, - "heading": 4.407573079371785, - "angularVelocity": 6.697754187886208, - "velocityX": -0.1964402828877463, - "velocityY": 1.4405715420334793, - "timestamp": 3.139701201985515 - }, - { - "x": 8.333720429129459, - "y": 2.3025901920416527, - "heading": 4.540110627397886, - "angularVelocity": 6.661759523756392, - "velocityX": -0.11852139984079614, - "velocityY": 1.5358164664046496, - "timestamp": 3.15959647853422 - }, - { - "x": 8.332603722599888, - "y": 2.3352255445188432, - "heading": 4.671035459898787, - "angularVelocity": 6.580699302188081, - "velocityX": -0.05612922880648972, - "velocityY": 1.640356815211755, - "timestamp": 3.1794917550829247 - }, - { - "x": 8.332608537566271, - "y": 2.3698002053105385, - "heading": 4.797912823563958, - "angularVelocity": 6.377260620357163, - "velocityX": 0.0002420155543911525, - "velocityY": 1.7378326311300352, - "timestamp": 3.1993870316316295 - }, - { - "x": 8.33396980572602, - "y": 2.4053616174482886, - "heading": 4.91661874197118, - "angularVelocity": 5.966537741590232, - "velocityX": 0.06842167568854002, - "velocityY": 1.787429898282315, - "timestamp": 3.2192823081803343 - }, - { - "x": 8.336666983434197, - "y": 2.4421571036885257, - "heading": 5.027955549931603, - "angularVelocity": 5.596142767247511, - "velocityX": 0.13556874676122466, - "velocityY": 1.8494583953211146, - "timestamp": 3.239177584729039 - }, - { - "x": 8.340246531495488, - "y": 2.479675325798265, - "heading": 5.127878006964601, - "angularVelocity": 5.02242111530254, - "velocityX": 0.17991949257545845, - "velocityY": 1.8857854032786403, - "timestamp": 3.259072861277744 - }, - { - "x": 8.344506054620522, - "y": 2.517833586551145, - "heading": 5.215460170451448, - "angularVelocity": 4.402158636621092, - "velocityX": 0.2140972061688483, - "velocityY": 1.9179557851063929, - "timestamp": 3.278968137826449 - }, - { - "x": 8.34934139251709, - "y": 2.556588649749756, - "heading": 5.290290323982317, - "angularVelocity": 3.7612019791572497, - "velocityX": 0.24303949154619145, - "velocityY": 1.9479529778707136, - "timestamp": 3.2988634143751536 - }, - { - "x": 8.360489719138714, - "y": 2.633118600966112, - "heading": 5.386097875540369, - "angularVelocity": 2.5082503010878967, - "velocityX": 0.2918641918155103, - "velocityY": 2.0035609934605225, - "timestamp": 3.3370603804102004 - }, - { - "x": 8.3734961558476, - "y": 2.711837506326731, - "heading": 5.434143906525109, - "angularVelocity": 1.2578494046008082, - "velocityX": 0.3405096807153113, - "velocityY": 2.0608680094746767, - "timestamp": 3.375257346445247 - }, - { - "x": 8.388354991269559, - "y": 2.792730047490086, - "heading": 5.4343775006036195, - "angularVelocity": 0.006115513946711611, - "velocityX": 0.3890056453259781, - "velocityY": 2.117773989932431, - "timestamp": 3.413454312480294 - }, - { - "x": 8.405064352616025, - "y": 2.875700202856259, - "heading": 5.386603131855514, - "angularVelocity": -1.2507372628567064, - "velocityX": 0.43745258016393523, - "velocityY": 2.1721661162838264, - "timestamp": 3.4516512785153406 - }, - { - "x": 8.423595902184939, - "y": 2.960570562063504, - "heading": 5.290443702807007, - "angularVelocity": -2.51746248537848, - "velocityX": 0.4851576314179031, - "velocityY": 2.221913623437374, - "timestamp": 3.4898482445503873 - }, - { - "x": 8.443874909764538, - "y": 3.0470824289690306, - "heading": 5.145320206366309, - "angularVelocity": -3.799346165544723, - "velocityX": 0.5309062390188924, - "velocityY": 2.264888442347729, - "timestamp": 3.528045210585434 - }, - { - "x": 8.46822613284771, - "y": 3.1277535966232044, - "heading": 4.9755072400440925, - "angularVelocity": -4.445718703585181, - "velocityX": 0.6375172064929964, - "velocityY": 2.1119784115878613, - "timestamp": 3.566242176620481 - }, - { - "x": 8.485889521575103, - "y": 3.2052285744044853, - "heading": 4.782579464036899, - "angularVelocity": -5.050866496312229, - "velocityX": 0.4624291026461906, - "velocityY": 2.02830187377173, - "timestamp": 3.6044391426555276 - }, - { - "x": 8.496466363696742, - "y": 3.2817681103248395, - "heading": 4.564012968163081, - "angularVelocity": -5.722090484183368, - "velocityX": 0.2769026762998172, - "velocityY": 2.0038119218716672, - "timestamp": 3.6426361086905743 - }, - { - "x": 8.498864361133254, - "y": 3.3553108945925123, - "heading": 4.355441183612215, - "angularVelocity": -5.460428044455047, - "velocityX": 0.06277978817249102, - "velocityY": 1.9253566945656204, - "timestamp": 3.680833074725621 - }, - { - "x": 8.49244817179446, - "y": 3.427344053675241, - "heading": 4.152515229757634, - "angularVelocity": -5.312619689961475, - "velocityX": -0.16797641291473847, - "velocityY": 1.8858345716944014, - "timestamp": 3.719030040760668 - }, - { - "x": 8.480949544401803, - "y": 3.4955676666301225, - "heading": 3.980239273291126, - "angularVelocity": -4.5102000066822105, - "velocityX": -0.3010350974500928, - "velocityY": 1.7861003120584202, - "timestamp": 3.7572270067957145 - }, - { - "x": 8.465880541095904, - "y": 3.5608797001839374, - "heading": 3.848658427278945, - "angularVelocity": -3.4447983615099473, - "velocityX": -0.39450785939576005, - "velocityY": 1.7098749019461128, - "timestamp": 3.7954239728307613 - }, - { - "x": 8.447723716276025, - "y": 3.6237520788842326, - "heading": 3.7606807673271616, - "angularVelocity": -2.3032630358929707, - "velocityX": -0.4753473038465876, - "velocityY": 1.6460045188564885, - "timestamp": 3.833620938865808 - }, - { - "x": 8.426810965043074, - "y": 3.6844175741296494, - "heading": 3.717839244299634, - "angularVelocity": -1.1215949190367396, - "velocityX": -0.5474977047591038, - "velocityY": 1.5882281118807744, - "timestamp": 3.8718179049008548 - }, - { - "x": 8.403377538803907, - "y": 3.7429507831754503, - "heading": 3.7209535641751312, - "angularVelocity": 0.08153317393427642, - "velocityX": -0.6134892027200903, - "velocityY": 1.5324046677449423, - "timestamp": 3.9100148709359015 - }, - { - "x": 8.377585411071777, - "y": 3.7993156909942623, - "heading": 3.770388538466339, - "angularVelocity": 1.2942120650590392, - "velocityX": -0.6752402195625785, - "velocityY": 1.475638346959163, - "timestamp": 3.9482118369709482 - }, - { - "x": 8.359121603366816, - "y": 3.8364193104219853, - "heading": 3.8251043907760733, - "angularVelocity": 2.1181774191421603, - "velocityX": -0.7147767767674121, - "velocityY": 1.4363670768638492, - "timestamp": 3.974043408875767 - }, - { - "x": 8.339698595709613, - "y": 3.872452168004382, - "heading": 3.901119172271844, - "angularVelocity": 2.9427083173978534, - "velocityX": -0.7519096293779934, - "velocityY": 1.3949154048838457, - "timestamp": 3.999874980780586 - }, - { - "x": 8.31940846552165, - "y": 3.907347005013746, - "heading": 3.9984628782143856, - "angularVelocity": 3.7684004016953554, - "velocityX": -0.7854779516602693, - "velocityY": 1.3508599917163486, - "timestamp": 4.025706552685405 - }, - { - "x": 8.29838582091067, - "y": 3.941024765667179, - "heading": 4.1171731868845285, - "angularVelocity": 4.595551099544112, - "velocityX": -0.8138352822058595, - "velocityY": 1.3037441460211887, - "timestamp": 4.051538124590223 - }, - { - "x": 8.276832604055052, - "y": 3.9733768568490917, - "heading": 4.2572182344953795, - "angularVelocity": 5.421468276374085, - "velocityX": -0.8343749631278609, - "velocityY": 1.2524244092121188, - "timestamp": 4.077369696495042 - }, - { - "x": 8.255105530373472, - "y": 4.004178929302719, - "heading": 4.418037307026107, - "angularVelocity": 6.225678914287337, - "velocityX": -0.841105363685868, - "velocityY": 1.1924195928580044, - "timestamp": 4.103201268399861 - }, - { - "x": 8.236226871427698, - "y": 4.03234435965831, - "heading": 4.584751742520452, - "angularVelocity": 6.45390207412214, - "velocityX": -0.7308366295065699, - "velocityY": 1.0903490681625025, - "timestamp": 4.12903284030468 - }, - { - "x": 8.22111995531124, - "y": 4.060012965907288, - "heading": 4.744248346923677, - "angularVelocity": 6.174483108922656, - "velocityX": -0.5848237254830212, - "velocityY": 1.0711158558576532, - "timestamp": 4.154864412209498 - }, - { - "x": 8.208790974794852, - "y": 4.088137716132201, - "heading": 4.888562116667079, - "angularVelocity": 5.5867204007233076, - "velocityX": -0.4772834019477146, - "velocityY": 1.0887742460483674, - "timestamp": 4.180695984114317 - }, - { - "x": 8.198725383178298, - "y": 4.116963981676649, - "heading": 5.015233890568161, - "angularVelocity": 4.903757865290863, - "velocityX": -0.3896623733794405, - "velocityY": 1.115931529473434, - "timestamp": 4.206527556019136 - }, - { - "x": 8.190656129284886, - "y": 4.146632428651468, - "heading": 5.123285713831358, - "angularVelocity": 4.182936433807978, - "velocityX": -0.31237951461661634, - "velocityY": 1.148534323971415, - "timestamp": 4.232359127923955 - }, - { - "x": 8.184413831367804, - "y": 4.177239551429493, - "heading": 5.212211909192924, - "angularVelocity": 3.4425390637949445, - "velocityX": -0.241653815729148, - "velocityY": 1.184872639218488, - "timestamp": 4.2581906998287735 - }, - { - "x": 8.179879188537598, - "y": 4.208850860595703, - "heading": 5.281700753397833, - "angularVelocity": 2.6900741643192534, - "velocityX": -0.17554653069182086, - "velocityY": 1.223747020997694, - "timestamp": 4.284022271733592 - }, - { - "x": 8.176711633108404, - "y": 4.259079542498045, - "heading": 5.341834535732932, - "angularVelocity": 1.5389696629172125, - "velocityX": -0.08106544311437222, - "velocityY": 1.2854740655637256, - "timestamp": 4.323096324838129 - }, - { - "x": 8.177255357527843, - "y": 4.31178579369655, - "heading": 5.357219600158119, - "angularVelocity": 0.3937411965947536, - "velocityX": 0.013915229576633022, - "velocityY": 1.3488810863182197, - "timestamp": 4.362170377942666 - }, - { - "x": 8.181626457446923, - "y": 4.366954653737556, - "heading": 5.328216932710745, - "angularVelocity": -0.7422487595484647, - "velocityX": 0.11186707218176441, - "velocityY": 1.411905232697765, - "timestamp": 4.401244431047203 - }, - { - "x": 8.190025046391563, - "y": 4.424472904744573, - "heading": 5.255283063236909, - "angularVelocity": -1.8665550071990427, - "velocityX": 0.21494030635039485, - "velocityY": 1.4720318584083123, - "timestamp": 4.4403184841517405 - }, - { - "x": 8.202703493077177, - "y": 4.484103126840556, - "heading": 5.138917873952012, - "angularVelocity": -2.978068053845803, - "velocityX": 0.3244722694032999, - "velocityY": 1.526082332345974, - "timestamp": 4.4793925372562775 - }, - { - "x": 8.219928011035833, - "y": 4.545471721328882, - "heading": 4.979694935972509, - "angularVelocity": -4.074902021388053, - "velocityX": 0.44081728385262037, - "velocityY": 1.570571507494814, - "timestamp": 4.5184665903608145 - }, - { - "x": 8.242002423270923, - "y": 4.608108641713836, - "heading": 4.778755843149729, - "angularVelocity": -5.142519827292898, - "velocityX": 0.5649378674905671, - "velocityY": 1.6030310502311427, - "timestamp": 4.557540643465352 - }, - { - "x": 8.271074244498946, - "y": 4.672945765445566, - "heading": 4.551673202567828, - "angularVelocity": -5.811596764082065, - "velocityX": 0.7440185728940091, - "velocityY": 1.6593396021207916, - "timestamp": 4.596614696569889 - }, - { - "x": 8.307044890775797, - "y": 4.743652286364594, - "heading": 4.33581324080306, - "angularVelocity": -5.524381133108038, - "velocityX": 0.9205762755303734, - "velocityY": 1.8095517434514046, - "timestamp": 4.635688749674426 - }, - { - "x": 8.343074596799024, - "y": 4.821913482728032, - "heading": 4.143267038130374, - "angularVelocity": -4.927725366947583, - "velocityX": 0.9220877580023329, - "velocityY": 2.0028942519492667, - "timestamp": 4.674762802778963 - }, - { - "x": 8.37162830890395, - "y": 4.904380364973872, - "heading": 3.9684903103687414, - "angularVelocity": -4.472961310003866, - "velocityX": 0.7307589010163777, - "velocityY": 2.1105279768446303, - "timestamp": 4.7138368558835 - }, - { - "x": 8.39410907934667, - "y": 4.988084023708393, - "heading": 3.8243195102940244, - "angularVelocity": -3.6896812237268355, - "velocityX": 0.5753375617977339, - "velocityY": 2.142180093541445, - "timestamp": 4.752910908988037 - }, - { - "x": 8.412533295857541, - "y": 5.0701333500179695, - "heading": 3.7241871610957364, - "angularVelocity": -2.5626302173055078, - "velocityX": 0.47152048602634916, - "velocityY": 2.099841705442333, - "timestamp": 4.791984962092574 - }, - { - "x": 8.427022508757116, - "y": 5.150508503599114, - "heading": 3.668854256125942, - "angularVelocity": -1.4161035411852148, - "velocityX": 0.370814178421956, - "velocityY": 2.0569955557492743, - "timestamp": 4.831059015197111 - }, - { - "x": 8.437687476391535, - "y": 5.229173684079572, - "heading": 3.658771745830396, - "angularVelocity": -0.25803594698946425, - "velocityX": 0.27294244612622637, - "velocityY": 2.013233187506785, - "timestamp": 4.870133068301648 - }, - { - "x": 8.444672138848645, - "y": 5.306037054642886, - "heading": 3.694319619572902, - "angularVelocity": 0.9097564987026582, - "velocityX": 0.17875449056755022, - "velocityY": 1.9671204918946052, - "timestamp": 4.909207121406185 - }, - { - "x": 8.44819450378418, - "y": 5.38096809387207, - "heading": 3.7759750997570256, - "angularVelocity": 2.0897622257323873, - "velocityX": 0.09014588085116931, - "velocityY": 1.9176674359508388, - "timestamp": 4.948281174510722 - }, - { - "x": 8.449106377133136, - "y": 5.428397890121427, - "heading": 3.847877974347001, - "angularVelocity": 2.856844558406781, - "velocityX": 0.03623054613292597, - "velocityY": 1.8844803645751356, - "timestamp": 4.973449808337294 - }, - { - "x": 8.448712080913069, - "y": 5.474928555589069, - "heading": 3.9390652949611624, - "angularVelocity": 3.623054045861213, - "velocityX": -0.015666174921664548, - "velocityY": 1.848756106043276, - "timestamp": 4.998618442163867 - }, - { - "x": 8.447075648100045, - "y": 5.520495348927803, - "heading": 4.049507624303278, - "angularVelocity": 4.388093930847932, - "velocityX": -0.06501873817623247, - "velocityY": 1.8104595447141671, - "timestamp": 5.023787075990439 - }, - { - "x": 8.444260693041544, - "y": 5.565024011584617, - "heading": 4.179082223329722, - "angularVelocity": 5.148257148929752, - "velocityX": -0.11184377657913579, - "velocityY": 1.7692125430265968, - "timestamp": 5.048955709817012 - }, - { - "x": 8.440260130020729, - "y": 5.60821579730256, - "heading": 4.326669444296132, - "angularVelocity": 5.86393452991448, - "velocityX": -0.15895034463857272, - "velocityY": 1.7160957569474122, - "timestamp": 5.074124343643584 - }, - { - "x": 8.433318308538809, - "y": 5.649473049584276, - "heading": 4.481200094717335, - "angularVelocity": 6.1398108250935115, - "velocityX": -0.275812407211048, - "velocityY": 1.639232886695548, - "timestamp": 5.0992929774701565 - }, - { - "x": 8.423269771904357, - "y": 5.688774009331345, - "heading": 4.641460146861477, - "angularVelocity": 6.367451378109614, - "velocityX": -0.3992483940007152, - "velocityY": 1.5615054840830123, - "timestamp": 5.124461611296729 - }, - { - "x": 8.409301302318001, - "y": 5.727844142412751, - "heading": 4.801252594095005, - "angularVelocity": 6.3488725027588, - "velocityX": -0.5549951452513271, - "velocityY": 1.552334280462891, - "timestamp": 5.149630245123301 - }, - { - "x": 8.396208121715038, - "y": 5.767480930820443, - "heading": 4.939805836150058, - "angularVelocity": 5.504996536950433, - "velocityX": -0.5202181688995966, - "velocityY": 1.5748486263026762, - "timestamp": 5.174798878949874 - }, - { - "x": 8.384341280819799, - "y": 5.8076306100276724, - "heading": 5.057844576112943, - "angularVelocity": 4.689914469583332, - "velocityX": -0.4714932473891813, - "velocityY": 1.5952268003057297, - "timestamp": 5.199967512776446 - }, - { - "x": 8.373739781221703, - "y": 5.848339028395987, - "heading": 5.1555398013279, - "angularVelocity": 3.881626070296041, - "velocityX": -0.42121871497465463, - "velocityY": 1.6174266211198407, - "timestamp": 5.225136146603019 - }, - { - "x": 8.364388866933238, - "y": 5.889641090063779, - "heading": 5.232909954944061, - "angularVelocity": 3.07407045409338, - "velocityX": -0.37153046736263995, - "velocityY": 1.6410132529397037, - "timestamp": 5.250304780429591 - }, - { - "x": 8.356262991610826, - "y": 5.931555344979487, - "heading": 5.289924578574465, - "angularVelocity": 2.2653046654526636, - "velocityX": -0.32285722691204854, - "velocityY": 1.665336911193694, - "timestamp": 5.2754734142561635 - }, - { - "x": 8.34934139251709, - "y": 5.974088191986084, - "heading": 5.326548029576795, - "angularVelocity": 1.4551227235728712, - "velocityX": -0.27500893141166805, - "velocityY": 1.689914808236125, - "timestamp": 5.300642048082736 - }, - { - "x": 8.343221165925502, - "y": 6.021143146137171, - "heading": 5.342226898752842, - "angularVelocity": 0.5719929961678595, - "velocityX": -0.22327673673661108, - "velocityY": 1.7166483058956983, - "timestamp": 5.328052994162652 - }, - { - "x": 8.338553091700128, - "y": 6.068939036747707, - "heading": 5.3338112220665685, - "angularVelocity": -0.3070188333426256, - "velocityX": -0.17029963912102522, - "velocityY": 1.7436789839791385, - "timestamp": 5.3554639402425686 - }, - { - "x": 8.335401457891965, - "y": 6.11747737229338, - "heading": 5.301487591207973, - "angularVelocity": -1.1792234665799601, - "velocityX": -0.1149771992171805, - "velocityY": 1.7707646939350166, - "timestamp": 5.382874886322485 - }, - { - "x": 8.333863965332327, - "y": 6.166745972097111, - "heading": 5.245527796242896, - "angularVelocity": -2.0415127154650743, - "velocityX": -0.05609045945204774, - "velocityY": 1.7974060311558808, - "timestamp": 5.410285832402401 - }, - { - "x": 8.334074611539513, - "y": 6.216707641134325, - "heading": 5.16630466616314, - "angularVelocity": -2.890200500514666, - "velocityX": 0.007684747785513327, - "velocityY": 1.8226904278148732, - "timestamp": 5.437696778482318 - }, - { - "x": 8.336209337170322, - "y": 6.2672871852177625, - "heading": 5.064334139872564, - "angularVelocity": -3.7200659179468194, - "velocityX": 0.07787858268680638, - "velocityY": 1.845231606963613, - "timestamp": 5.465107724562234 - }, - { - "x": 8.34050875980612, - "y": 6.318358761986823, - "heading": 4.9404202596625035, - "angularVelocity": -4.520598444460426, - "velocityX": 0.1568505743385533, - "velocityY": 1.8631818332778705, - "timestamp": 5.49251867064215 - }, - { - "x": 8.347373076329852, - "y": 6.369731891617242, - "heading": 4.796226953445604, - "angularVelocity": -5.260427925271259, - "velocityX": 0.2504224591051665, - "velocityY": 1.874183017274954, - "timestamp": 5.519929616722067 - }, - { - "x": 8.357734742408361, - "y": 6.421155881623346, - "heading": 4.637107940656009, - "angularVelocity": -5.804944211910156, - "velocityX": 0.37801198281516796, - "velocityY": 1.876038494117501, - "timestamp": 5.547340562801983 - }, - { - "x": 8.372714822209472, - "y": 6.472180587497517, - "heading": 4.476004426648119, - "angularVelocity": -5.877342341201648, - "velocityX": 0.546499918588575, - "velocityY": 1.8614718997808002, - "timestamp": 5.574751508881899 - }, - { - "x": 8.392163518645132, - "y": 6.523832787745315, - "heading": 4.319730445846118, - "angularVelocity": -5.701152391689906, - "velocityX": 0.7095229905220347, - "velocityY": 1.8843640090788911, - "timestamp": 5.602162454961816 - }, - { - "x": 8.409770328909872, - "y": 6.578150492835037, - "heading": 4.178613291017239, - "angularVelocity": -5.148204458811868, - "velocityX": 0.642327711469996, - "velocityY": 1.9816063601511025, - "timestamp": 5.629573401041732 - }, - { - "x": 8.424485615502517, - "y": 6.635136223401266, - "heading": 4.049096201405813, - "angularVelocity": -4.725013475777802, - "velocityX": 0.5368397920211534, - "velocityY": 2.0789406684499903, - "timestamp": 5.656984347121648 - }, - { - "x": 8.435886735748374, - "y": 6.694743962812389, - "heading": 3.9283767340305045, - "angularVelocity": -4.404060590369644, - "velocityX": 0.41593311710644926, - "velocityY": 2.1745962082934467, - "timestamp": 5.684395293201565 - }, - { - "x": 8.4437690723566, - "y": 6.756967833759458, - "heading": 3.814495508904072, - "angularVelocity": -4.154589367124023, - "velocityX": 0.28756163998295975, - "velocityY": 2.2700373334672657, - "timestamp": 5.711806239281481 - }, - { - "x": 8.44819450378418, - "y": 6.821402072906494, - "heading": 3.7101172753280305, - "angularVelocity": -3.8079033562623543, - "velocityX": 0.16144759887806323, - "velocityY": 2.350675491432431, - "timestamp": 5.739217185361397 - }, - { - "x": 8.442296007576482, - "y": 7.005937519917937, - "heading": 3.5641292983287713, - "angularVelocity": -1.9075537985667739, - "velocityX": -0.07707277734850891, - "velocityY": 2.41123481640327, - "timestamp": 5.815748697662097 - }, - { - "x": 8.41659744566655, - "y": 7.180425726585724, - "heading": 3.5626950047371615, - "angularVelocity": -0.01874121585334014, - "velocityX": -0.3357905931475581, - "velocityY": 2.279952419889553, - "timestamp": 5.892280209962797 - }, - { - "x": 8.38385523357143, - "y": 7.320664328992628, - "heading": 3.601572492187995, - "angularVelocity": 0.5079931949871856, - "velocityX": -0.4278265398241839, - "velocityY": 1.8324295207429526, - "timestamp": 5.968811722263497 - }, - { - "x": 8.354355980334272, - "y": 7.424774646283286, - "heading": 3.6482652707450245, - "angularVelocity": 0.6101117977855888, - "velocityX": -0.38545237576453223, - "velocityY": 1.3603588137863938, - "timestamp": 6.045343234564196 - }, - { - "x": 8.33265202622526, - "y": 7.4934610231290915, - "heading": 3.6869536728033228, - "angularVelocity": 0.5055225082484697, - "velocityX": -0.2835949984071486, - "velocityY": 0.8974914356315113, - "timestamp": 6.121874746864896 - }, - { - "x": 8.321097373962402, - "y": 7.527496814727783, - "heading": 3.709032536356956, - "angularVelocity": 0.2884937575371967, - "velocityX": -0.15097901394471921, - "velocityY": 0.44472911321759667, - "timestamp": 6.198406259165596 - }, - { - "x": 8.321097373962402, - "y": 7.527496814727783, - "heading": 3.709032536356956, - "angularVelocity": -5.082729725441752e-29, - "velocityX": 4.680465584518782e-28, - "velocityY": 3.556470741173404e-29, - "timestamp": 6.274937771466296 - } - ], - "trajectoryWaypoints": [ + "x": 4.528280141254807, + "y": 4.838988810812342, + "heading": 0.019478906624101934, + "angularVelocity": -0.24654461443801812, + "velocityX": 1.6433384569792095, + "velocityY": -2.8922705297373237, + "timestamp": 9.688437849330342 + }, { - "timestamp": 0, - "isStopPoint": true, - "x": 1.4578553438186646, - "y": 1.4268364906311035, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 + "x": 4.6555610459377, + "y": 4.65789803685685, + "heading": 0.007446897298019998, + "angularVelocity": -0.1920505704386356, + "velocityX": 2.0316116525364616, + "velocityY": -2.8905052761167873, + "timestamp": 9.75108806327452 }, { - "timestamp": 0.7098978675678107, - "isStopPoint": false, - "x": 3.095994472503662, - "y": 1.2997395992279053, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -3.9652014929192097e-25, + "angularVelocity": -0.1188646746626479, + "velocityX": 2.3941774298704206, + "velocityY": -2.7520844885235216, + "timestamp": 9.813738277218697 + }, + { + "x": 5.026556371976755, + "y": 4.290478604211026, + "heading": -0.0011711585949671858, + "angularVelocity": -0.014781576411141743, + "velocityX": 2.7893083450033753, + "velocityY": -2.4611683748987776, + "timestamp": 9.892969244362662 + }, + { + "x": 5.271648004487175, + "y": 4.126055401914557, + "heading": -0.0011711658442509346, + "angularVelocity": -9.149558575332566e-8, + "velocityX": 3.0933818094770933, + "velocityY": -2.075239117019822, + "timestamp": 9.972200211506628 + }, + { + "x": 5.536248278847656, + "y": 3.995320554415454, + "heading": -0.0011711670074400294, + "angularVelocity": -1.4680990736292476e-8, + "velocityX": 3.3396067711718374, + "velocityY": -1.6500473515810268, + "timestamp": 10.051431178650594 + }, + { + "x": 5.8157535418727955, + "y": 3.900548632610079, + "heading": -0.001171167882288204, + "angularVelocity": -1.1041745495501586e-8, + "velocityX": 3.5277275174146094, + "velocityY": -1.19614748149131, + "timestamp": 10.13066214579456 + }, + { + "x": 6.105300821187418, + "y": 3.843388468655785, + "heading": -0.001171168612334034, + "angularVelocity": -9.214147655131007e-9, + "velocityX": 3.654471095733389, + "velocityY": -0.7214371604278409, + "timestamp": 10.209893112938525 + }, + { + "x": 6.399852439542981, + "y": 3.8248343191469996, + "heading": -0.0011711692753000216, + "angularVelocity": -8.367510982006514e-9, + "velocityX": 3.717632498671272, + "velocityY": -0.23417800107213171, + "timestamp": 10.289124080082491 + }, + { + "x": 6.694725372224158, + "y": 3.837278843369643, + "heading": -0.0011711699217371897, + "angularVelocity": -8.158895331165977e-9, + "velocityX": 3.721687912068311, + "velocityY": 0.15706641823558024, + "timestamp": 10.368355047226457 + }, + { + "x": 6.989598240261493, + "y": 3.849724899233505, + "heading": -0.001171170568174097, + "angularVelocity": -8.158892044748454e-9, + "velocityX": 3.721687096177193, + "velocityY": 0.15708574958131358, + "timestamp": 10.447586014370422 + }, + { + "x": 7.2831382115989305, + "y": 3.8803709956962793, + "heading": -0.0011711735014437953, + "angularVelocity": -3.702175808154048e-8, + "velocityX": 3.7048641701427742, + "velocityY": 0.38679442606183717, + "timestamp": 10.526816981514388 + }, + { + "x": 7.537795284833571, + "y": 3.9128988141490195, + "heading": -0.0022060584163901163, + "angularVelocity": -0.013061621639249952, + "velocityX": 3.2141103714147587, + "velocityY": 0.4105442559300826, + "timestamp": 10.606047948658354 + }, + { + "x": 7.7535280379166585, + "y": 3.9448039763833895, + "heading": -0.0030479668047439287, + "angularVelocity": -0.010626001659477862, + "velocityX": 2.7228337714355204, + "velocityY": 0.40268550775604034, + "timestamp": 10.68527891580232 + }, + { + "x": 7.9303456416380165, + "y": 3.9756628799518183, + "heading": -0.0034891865807647085, + "angularVelocity": -0.005568779379141843, + "velocityX": 2.2316729189998985, + "velocityY": 0.38948033427835, + "timestamp": 10.764509882946285 + }, + { + "x": 8.06825324660345, + "y": 4.0053006852761595, + "heading": -0.003443969014778176, + "angularVelocity": 0.000570705717934386, + "velocityX": 1.7405770740479534, + "velocityY": 0.374068453190637, + "timestamp": 10.843740850090251 + }, + { + "x": 8.167254001972681, + "y": 4.03362192898922, + "heading": -0.002865492528698777, + "angularVelocity": 0.007301141295275209, + "velocityX": 1.2495209756728547, + "velocityY": 0.3574516976625985, + "timestamp": 10.922971817234217 + }, + { + "x": 8.22735001387898, + "y": 4.060566470670497, + "heading": -0.0017242628169884097, + "angularVelocity": 0.014403834167980214, + "velocityX": 0.7584914594958928, + "velocityY": 0.34007589017963685, + "timestamp": 11.002202784378182 }, { - "timestamp": 1.6119051391146006, - "isStopPoint": false, - "x": 6.202812194824219, - "y": 1.003179669380188, - "heading": 0.7165423509110156, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 1.4182707321197947e-24, + "angularVelocity": 0.021762486047347675, + "velocityX": 0.26748091724090983, + "velocityY": 0.32217804683230217, + "timestamp": 11.081433751522148 + }, + { + "x": 8.23152301234386, + "y": 4.109892958457591, + "heading": 0.0022863134325125296, + "angularVelocity": 0.02921520350070526, + "velocityX": -0.21748380315840005, + "velocityY": 0.30412370948765427, + "timestamp": 11.159691410499514 + }, + { + "x": 8.176551036095818, + "y": 4.132280075994518, + "heading": 0.005155847292928311, + "angularVelocity": 0.03666777025933954, + "velocityX": -0.7024485138757267, + "velocityY": 0.2860693487317698, + "timestamp": 11.23794906947688 + }, + { + "x": 8.0836268579111, + "y": 4.153254299063441, + "heading": 0.008608587112944467, + "angularVelocity": 0.044120152137630285, + "velocityX": -1.1874132116780383, + "velocityY": 0.2680149565295516, + "timestamp": 11.316206728454246 + }, + { + "x": 7.952750479206832, + "y": 4.172815624183264, + "heading": 0.012644515987393022, + "angularVelocity": 0.05157231799657927, + "velocityX": -1.672377891371887, + "velocityY": 0.24996051984483794, + "timestamp": 11.394464387431611 + }, + { + "x": 7.783921902112923, + "y": 4.190964046074257, + "heading": 0.01726361439792771, + "angularVelocity": 0.05902423444420523, + "velocityX": -2.157342543849138, + "velocityY": 0.2319060156941693, + "timestamp": 11.472722046408977 + }, + { + "x": 7.577141130185825, + "y": 4.207699555855839, + "heading": 0.022465858708007803, + "angularVelocity": 0.06647584885697558, + "velocityX": -2.642307150880985, + "velocityY": 0.2138513980647186, + "timestamp": 11.550979705386343 + }, + { + "x": 7.3324081705466435, + "y": 4.2230221357012825, + "heading": 0.028251214753927917, + "angularVelocity": 0.0739270267156012, + "velocityX": -3.127271666917177, + "velocityY": 0.19579655263996953, + "timestamp": 11.629237364363709 + }, + { + "x": 7.049723044551423, + "y": 4.236931732641069, + "heading": 0.03461960291638085, + "angularVelocity": 0.08137718717467465, + "velocityX": -3.612235910059377, + "velocityY": 0.17774103035473854, + "timestamp": 11.707495023341075 + }, + { + "x": 6.758414971904043, + "y": 4.226087741393719, + "heading": 0.03461960351163466, + "angularVelocity": 7.606332865983503e-9, + "velocityX": -3.722422526485686, + "velocityY": -0.13856779501270336, + "timestamp": 11.78575268231844 + }, + { + "x": 6.467106926605879, + "y": 4.215243015473276, + "heading": 0.03461960410686983, + "angularVelocity": 7.606094882154123e-9, + "velocityX": -3.7224221770091335, + "velocityY": -0.13857718288735063, + "timestamp": 11.864010341295806 + }, + { + "x": 6.175798881305754, + "y": 4.204398289605525, + "heading": 0.03461960470210498, + "angularVelocity": 7.606094432841078e-9, + "velocityX": -3.7224221770341983, + "velocityY": -0.13857718221405538, + "timestamp": 11.942268000273172 + }, + { + "x": 5.884490683895451, + "y": 4.1935576504515195, + "heading": 0.03461960529734107, + "angularVelocity": 7.606106615779635e-9, + "velocityX": -3.7224241207439825, + "velocityY": -0.13852496095162967, + "timestamp": 12.020525659250538 + }, + { + "x": 5.594009037113431, + "y": 4.2180198226312315, + "heading": 0.034619605911984125, + "angularVelocity": 7.854094508972923e-9, + "velocityX": -3.7118622071998786, + "velocityY": 0.31258502361778023, + "timestamp": 12.098783318227904 }, { - "timestamp": 2.314676799081464, - "isStopPoint": false, - "x": 8.25048828125, - "y": 0.7348634600639343, - "heading": 0.7165423509110156, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 25 + "x": 5.309172831526297, + "y": 4.280038516789027, + "heading": 0.03461959995315513, + "angularVelocity": -7.614371636753198e-8, + "velocityX": -3.6397230547046773, + "velocityY": 0.7924936034150152, + "timestamp": 12.17704097720527 }, { - "timestamp": 3.040224819241991, - "isStopPoint": false, - "x": 8.377585411071777, - "y": 2.1329314708709717, - "heading": 3.7458973568177814, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 + "x": 5.03935943827081, + "y": 4.37675512978584, + "heading": 0.01971114929844739, + "angularVelocity": -0.1905046847749395, + "velocityX": -3.447757021884906, + "velocityY": 1.235874089011348, + "timestamp": 12.255298636182635 }, { - "timestamp": 3.2988634143751536, - "isStopPoint": false, - "x": 8.34934139251709, - "y": 2.556588649749756, - "heading": 5.290290323982317, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -1.4078898091571754e-24, + "angularVelocity": -0.25187501844577836, + "velocityX": -2.9876010500011123, + "velocityY": 1.3893109823794343, + "timestamp": 12.333556295160001 }, { - "timestamp": 3.9482118369709482, - "isStopPoint": false, - "x": 8.377585411071777, - "y": 3.7993156909942627, - "heading": 3.770388538466339, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 + "x": 4.62745788226169, + "y": 4.587679677136332, + "heading": -0.019778853338814184, + "angularVelocity": -0.28487106556518027, + "velocityX": -2.5651244910957125, + "velocityY": 1.47197181806725, + "timestamp": 12.402987191202648 }, { - "timestamp": 4.284022271733592, - "isStopPoint": false, - "x": 8.179879188537598, - "y": 4.208850860595703, - "heading": 5.281700753397833, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 + "x": 4.478940134439646, + "y": 4.685562428002014, + "heading": -0.039416750418752866, + "angularVelocity": -0.2828408993580651, + "velocityX": -2.139072895311886, + "velocityY": 1.4097866575934404, + "timestamp": 12.472418087245295 }, { - "timestamp": 4.948281174510722, - "isStopPoint": false, - "x": 8.44819450378418, - "y": 5.38096809387207, - "heading": 3.7759750997570256, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 + "x": 4.358424662792702, + "y": 4.772986561558442, + "heading": -0.05736759927872339, + "angularVelocity": -0.2585426644781364, + "velocityX": -1.7357614335398783, + "velocityY": 1.2591531802027822, + "timestamp": 12.541848983287942 }, { - "timestamp": 5.300642048082736, - "isStopPoint": false, - "x": 8.34934139251709, - "y": 5.974088191986084, - "heading": 5.326548029576795, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 + "x": 4.264251963016928, + "y": 4.846292896298889, + "heading": -0.07266630938107123, + "angularVelocity": -0.22034441400483662, + "velocityX": -1.3563514968599637, + "velocityY": 1.0558172070171734, + "timestamp": 12.611279879330588 }, { - "timestamp": 5.739217185361397, - "isStopPoint": false, - "x": 8.44819450378418, - "y": 6.821402072906494, - "heading": 3.7101172753280305, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 7 + "x": 4.195046678979309, + "y": 4.903178017800908, + "heading": -0.08468297645986948, + "angularVelocity": -0.17307377210596744, + "velocityX": -0.9967505531703128, + "velocityY": 0.8193055936809087, + "timestamp": 12.680710775373235 }, { - "timestamp": 6.274937771466296, - "isStopPoint": true, - "x": 8.321097373962402, - "y": 7.527496814727783, - "heading": 3.7090325363569563, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "constraints": [ + "x": 4.149718350868284, + "y": 4.942098244876491, + "heading": -0.09298341563380762, + "angularVelocity": -0.11954964788067375, + "velocityX": -0.6528552948990165, + "velocityY": 0.5605606335784191, + "timestamp": 12.750141671415882 + }, { - "scope": [ - "first" - ], - "type": "StopPoint" + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.06149221603257356, + "velocityX": -0.32142062647019676, + "velocityY": 0.2860902346288004, + "timestamp": 12.819572567458529 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -9.169895235461756e-26, + "velocityX": -3.1744204454242983e-26, + "velocityY": -4.8447793914370994e-26, + "timestamp": 12.889003463501176 } ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [], - "eventMarkers": [], - "isTrajectoryStale": false - }, - "AmpLanePdChaos": { - "waypoints": [ + "trajectoryWaypoints": [ { - "x": 0.4269569218158722, - "y": 6.9908647537231445, + "timestamp": 0, + "isStopPoint": true, + "x": 0.43297290802001953, + "y": 6.9807281494140625, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 6 }, { - "x": 1.5002212524414062, - "y": 6.694304943084717, + "timestamp": 0.34959777724707536, + "isStopPoint": false, + "x": 0.8667811155319214, + "y": 6.895569324493408, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 13 }, { - "x": 2.319291353225708, - "y": 6.185916423797607, - "heading": 0.2985001179522956, + "timestamp": 1.1040901645422216, + "isStopPoint": true, + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 8 }, { - "x": 3.8162126541137695, - "y": 6.581329345703125, - "heading": 0.2985001179522956, + "timestamp": 1.8634374124793147, + "isStopPoint": true, + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 20 + "controlIntervalCount": 22 }, { - "x": 8.264610290527344, - "y": 7.456887245178223, - "heading": 0, + "timestamp": 3.2868633977536255, + "isStopPoint": false, + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 9 }, { - "x": 8.377585411071777, - "y": 6.553085803985596, - "heading": 1.0808392342145412, + "timestamp": 3.8367661144950707, + "isStopPoint": false, + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 15 }, { - "x": 8.377585411071777, - "y": 1, - "heading": 1.0808392342145412, + "timestamp": 4.83026471118104, + "isStopPoint": false, + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.4269569218158722, - "y": 6.9908647537231445, - "heading": 1.0147582935939444e-26, - "angularVelocity": 4.802793352791378e-28, - "velocityX": 7.569777584926927e-27, - "velocityY": 3.719655610214095e-26, - "timestamp": 0 + "headingConstrained": false, + "controlIntervalCount": 12 }, { - "x": 0.45154320453389124, - "y": 6.9860595473156515, - "heading": -0.0020588646196638234, - "angularVelocity": -0.03238986683838223, - "velocityX": 0.38678911458368936, - "velocityY": -0.07559506058978287, - "timestamp": 0.06356508441164857 - }, - { - "x": 0.500665330018184, - "y": 6.976191418092733, - "heading": -0.005903955941845083, - "angularVelocity": -0.06049061930415103, - "velocityX": 0.7727847125345899, - "velocityY": -0.15524449175608063, - "timestamp": 0.12713016882329714 - }, - { - "x": 0.5742528189740612, - "y": 6.960923422240335, - "heading": -0.01117745758299664, - "angularVelocity": -0.08296223768067817, - "velocityX": 1.1576715367720332, - "velocityY": -0.24019469168832944, - "timestamp": 0.1906952532349457 - }, - { - "x": 0.6722013148089163, - "y": 6.9397965653748726, - "heading": -0.017389498609317183, - "angularVelocity": -0.09772725205698236, - "velocityX": 1.5409166327938608, - "velocityY": -0.3323657486025543, - "timestamp": 0.2542603376465943 - }, - { - "x": 0.7943433521763879, - "y": 6.912149808159037, - "heading": -0.023829197654655073, - "angularVelocity": -0.10130874685282076, - "velocityX": 1.9215271795515552, - "velocityY": -0.43493621493200585, - "timestamp": 0.3178254220582428 - }, - { - "x": 0.940376377090631, - "y": 6.876952520080898, - "heading": -0.029373547858602904, - "angularVelocity": -0.08722320209696448, - "velocityX": 2.2973779751243755, - "velocityY": -0.553720464684682, - "timestamp": 0.38139050646989137 - }, - { - "x": 1.109639548430514, - "y": 6.832391950283203, - "heading": -0.03199489164843748, - "angularVelocity": -0.04123873686470252, - "velocityX": 2.6628324796004614, - "velocityY": -0.7010227424401742, - "timestamp": 0.4449555908815399 - }, - { - "x": 1.3001020816519337, - "y": 6.774603331599785, - "heading": -0.02710028887606378, - "angularVelocity": 0.07700143589326758, - "velocityX": 2.9963388703770333, - "velocityY": -0.909125178048654, - "timestamp": 0.5085206752931885 - }, - { - "x": 1.5002212524414062, - "y": 6.694304943084717, - "heading": 4.6927585573960014e-26, - "angularVelocity": 0.426339225801414, - "velocityX": 3.148256195075543, - "velocityY": -1.2632467849024598, - "timestamp": 0.572085759704837 - }, - { - "x": 1.6698109310260505, - "y": 6.606230489693705, - "heading": 0.043636088677614475, - "angularVelocity": 0.7393180152975136, - "velocityX": 2.8733259186555666, - "velocityY": -1.492228842060727, - "timestamp": 0.6311078419140127 - }, - { - "x": 1.8178691248742131, - "y": 6.519462655469564, - "heading": 0.09197841408379424, - "angularVelocity": 0.8190548960108419, - "velocityX": 2.508522036268403, - "velocityY": -1.4700910401065466, - "timestamp": 0.6901299241231884 - }, - { - "x": 1.945478769087034, - "y": 6.439654918114978, - "heading": 0.13899529896762525, - "angularVelocity": 0.7965982073828233, - "velocityX": 2.1620661189242583, - "velocityY": -1.3521674323814197, - "timestamp": 0.7491520063323641 - }, - { - "x": 2.05361266916171, - "y": 6.369142283614203, - "heading": 0.1819641027220741, - "angularVelocity": 0.7280123327768478, - "velocityX": 1.8320922615275888, - "velocityY": -1.1946822589361732, - "timestamp": 0.8081740885415398 - }, - { - "x": 2.1429230510824855, - "y": 6.309142790648472, - "heading": 0.2193838977387365, - "angularVelocity": 0.6339965249623981, - "velocityX": 1.5131689458914253, - "velocityY": -1.0165600859876827, - "timestamp": 0.8671961707507155 - }, - { - "x": 2.2138592843219276, - "y": 6.2603940232890976, - "heading": 0.25031075711822803, - "angularVelocity": 0.5239879418331249, - "velocityX": 1.2018592124222665, - "velocityY": -0.8259411653185552, - "timestamp": 0.9262182529598912 - }, - { - "x": 2.2667458111920182, - "y": 6.2233878173394785, - "heading": 0.2740979405057225, - "angularVelocity": 0.4030217589273135, - "velocityX": 0.8960464438150413, - "velocityY": -0.6269891634535774, - "timestamp": 0.985240335169067 - }, - { - "x": 2.301826622003525, - "y": 6.198474614172422, - "heading": 0.2902775915680301, - "angularVelocity": 0.27412877446387834, - "velocityX": 0.5943675569963707, - "velocityY": -0.4220996995457338, - "timestamp": 1.0442624173782427 - }, - { - "x": 2.319291353225708, - "y": 6.185916423797607, - "heading": 0.2985001179522956, - "angularVelocity": 0.1393127127424044, - "velocityX": 0.2959016450874765, - "velocityY": -0.2127710494914169, - "timestamp": 1.1032844995874185 - }, - { - "x": 2.319291353225708, - "y": 6.185916423797607, - "heading": 0.2985001179522956, - "angularVelocity": -6.615842884891707e-27, - "velocityX": 2.924635996734034e-23, - "velocityY": -9.88114513956692e-23, - "timestamp": 1.162306581796594 - }, - { - "x": 2.347136097791038, - "y": 6.1936465997058665, - "heading": 0.2985001179522956, - "angularVelocity": 1.0620750257524312e-16, - "velocityX": 0.4079003982651746, - "velocityY": 0.11324010619816775, - "timestamp": 1.2305701693874758 - }, - { - "x": 2.402825586267742, - "y": 6.209106951340729, - "heading": 0.2985001179522956, - "angularVelocity": 1.523734708742185e-16, - "velocityX": 0.8158007869504784, - "velocityY": 0.22648020973522806, - "timestamp": 1.2988337569783575 - }, - { - "x": 2.486359817783878, - "y": 6.2322974784599845, - "heading": 0.2985001179522956, - "angularVelocity": 1.8006542596605785e-16, - "velocityX": 1.2237011628626233, - "velocityY": 0.3397203097241459, - "timestamp": 1.3670973445692391 - }, - { - "x": 2.5977387911187284, - "y": 6.263218180724542, - "heading": 0.2985001179522956, - "angularVelocity": 1.3885125036469104e-16, - "velocityX": 1.6316015208923507, - "velocityY": 0.45296040474566596, - "timestamp": 1.4353609321601208 - }, - { - "x": 2.7369625044412174, - "y": 6.301869057625762, - "heading": 0.2985001179522956, - "angularVelocity": 1.7915030698796464e-16, - "velocityX": 2.0395018520984625, - "velocityY": 0.5662004923160927, - "timestamp": 1.5036245197510025 - }, - { - "x": 2.904030954699552, - "y": 6.348250108315915, - "heading": 0.2985001179522956, - "angularVelocity": 1.7115442013810006e-16, - "velocityX": 2.447402138598578, - "velocityY": 0.6794405674680408, - "timestamp": 1.5718881073418842 - }, - { - "x": 3.098944135790158, - "y": 6.402361331099544, - "heading": 0.2985001179522956, - "angularVelocity": 1.731300305768192e-16, - "velocityX": 2.8553023356868215, - "velocityY": 0.7926806177830709, - "timestamp": 1.640151694932766 - }, - { - "x": 3.3217020294023807, - "y": 6.464202720890301, - "heading": 0.2985001179522956, - "angularVelocity": -4.291109894147179e-17, - "velocityX": 3.263202264540478, - "velocityY": 0.9059205935876805, - "timestamp": 1.7084152825236476 - }, - { - "x": 3.5667175760590744, - "y": 6.53222236284647, - "heading": 0.2985001179522956, - "angularVelocity": 2.176874790341123e-16, - "velocityX": 3.589256810308954, - "velocityY": 0.9964264164348671, - "timestamp": 1.7766788701145293 - }, - { - "x": 3.8162126541137695, - "y": 6.581329345703125, - "heading": 0.2985001179522956, - "angularVelocity": -3.979347495805123e-16, - "velocityX": 3.6548779057727367, - "velocityY": 0.7193730155374103, - "timestamp": 1.844942457705411 - }, - { - "x": 4.06911506298554, - "y": 6.631106978006357, - "heading": 0.2985000768137015, - "angularVelocity": -5.945239455017304e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 1.914138314964857 - }, - { - "x": 4.322017471857311, - "y": 6.6808846103095885, - "heading": 0.2985000356751073, - "angularVelocity": -5.945239469422448e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 1.983334172224303 - }, - { - "x": 4.574919880729083, - "y": 6.730662242612821, - "heading": 0.29849999453651305, - "angularVelocity": -5.945239484047347e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.0525300294837487 - }, - { - "x": 4.827822289600854, - "y": 6.780439874916052, - "heading": 0.29849995339791874, - "angularVelocity": -5.945239490070907e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.1217258867431945 - }, - { - "x": 5.080724698472625, - "y": 6.830217507219284, - "heading": 0.29849991225932443, - "angularVelocity": -5.945239490087136e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.1909217440026403 - }, - { - "x": 5.3336271073443955, - "y": 6.879995139522516, - "heading": 0.29849987112073, - "angularVelocity": -5.945239499004978e-7, - "velocityX": 3.6548778913675157, - "velocityY": 0.7193730127020961, - "timestamp": 2.260117601262086 - }, - { - "x": 5.586529516216166, - "y": 6.929772771825747, - "heading": 0.2984998299821356, - "angularVelocity": -5.945239509711758e-7, - "velocityX": 3.6548778913675157, - "velocityY": 0.7193730127020961, - "timestamp": 2.329313458521532 - }, - { - "x": 5.839431925087938, - "y": 6.979550404128979, - "heading": 0.2984997888435411, - "angularVelocity": -5.945239507812541e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.3985093157809776 - }, - { - "x": 6.092334333959709, - "y": 7.029328036432211, - "heading": 0.2984997477049465, - "angularVelocity": -5.945239524665447e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.4677051730404234 - }, - { - "x": 6.34523674283148, - "y": 7.079105668735443, - "heading": 0.29849970656635194, - "angularVelocity": -5.945239530479041e-7, - "velocityX": 3.6548778913675166, - "velocityY": 0.7193730127020963, - "timestamp": 2.536901030299869 - }, - { - "x": 6.598139151703251, - "y": 7.128883301038674, - "heading": 0.29849966542775724, - "angularVelocity": -5.945239544565078e-7, - "velocityX": 3.6548778913675157, - "velocityY": 0.7193730127020962, - "timestamp": 2.606096887559315 - }, - { - "x": 6.8510415605750214, - "y": 7.178660933341907, - "heading": 0.29849962428916255, - "angularVelocity": -5.945239549845622e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.6752927448187607 - }, - { - "x": 7.103943969446777, - "y": 7.228438565645134, - "heading": 0.29849958315051783, - "angularVelocity": -5.945246772244776e-7, - "velocityX": 3.654877891367282, - "velocityY": 0.7193730127020502, - "timestamp": 2.7444886020782064 - }, - { - "x": 7.353433038122826, - "y": 7.27754436570309, - "heading": 0.2857400994824136, - "angularVelocity": -0.18439664126514535, - "velocityX": 3.605549212875623, - "velocityY": 0.7096638729950924, - "timestamp": 2.813684459337652 - }, - { - "x": 7.577591625127487, - "y": 7.32166448211996, - "heading": 0.20789341833491348, - "angularVelocity": -1.1250193903316845, - "velocityX": 3.239479874701043, - "velocityY": 0.6376121080695129, - "timestamp": 2.882880316597098 - }, - { - "x": 7.772618972429747, - "y": 7.360050828957839, - "heading": 0.13154587005744633, - "angularVelocity": -1.1033543235284506, - "velocityX": 2.818482999220841, - "velocityY": 0.5547492054900108, - "timestamp": 2.9520761738565438 - }, - { - "x": 7.938688452207964, - "y": 7.392737530275028, - "heading": 0.06829117903161311, - "angularVelocity": -0.9141398565041748, - "velocityX": 2.3999916520370626, - "velocityY": 0.47237945466347603, - "timestamp": 3.0212720311159895 - }, - { - "x": 8.075968311155844, - "y": 7.41975770130397, - "heading": 0.023429532114866793, - "angularVelocity": -0.6483285082882946, - "velocityX": 1.9839317610179508, - "velocityY": 0.3904882763086947, - "timestamp": 3.0904678883754353 - }, - { - "x": 8.184579349560657, - "y": 7.441135118616467, - "heading": 4.707584249891564e-24, - "angularVelocity": -0.33859732421579963, - "velocityX": 1.5696176434028992, - "velocityY": 0.3089407106026808, - "timestamp": 3.159663745634881 - }, - { - "x": 8.264610290527344, - "y": 7.456887245178223, - "heading": 4.305949741153467e-24, - "angularVelocity": 2.0877324046258187e-24, - "velocityX": 1.156585728342302, - "velocityY": 0.22764551500092414, - "timestamp": 3.228859602894327 - }, - { - "x": 8.309090579989265, - "y": 7.457165429033817, - "heading": 0.013618338743485297, - "angularVelocity": 0.3126833682896741, - "velocityX": 1.021288058215364, - "velocityY": 0.006387230234869135, - "timestamp": 3.2724127308332234 - }, - { - "x": 8.347694581932599, - "y": 7.44777021311996, - "heading": 0.040485197139552155, - "angularVelocity": 0.6168755188780024, - "velocityX": 0.886365773716491, - "velocityY": -0.2157185111259548, - "timestamp": 3.31596585877212 - }, - { - "x": 8.38043908621254, - "y": 7.428658664796906, - "heading": 0.08013490839873996, - "angularVelocity": 0.9103757441902883, - "velocityX": 0.7518289920733539, - "velocityY": -0.4388100057903166, - "timestamp": 3.3595189867110165 - }, - { - "x": 8.407344118270622, - "y": 7.3997816960160065, - "heading": 0.13202626739740458, - "angularVelocity": 1.1914496490692066, - "velocityX": 0.6177520038475312, - "velocityY": -0.6630285847990697, - "timestamp": 3.403072114649913 - }, - { - "x": 8.428437067602909, - "y": 7.361082970163004, - "heading": 0.19556012614557247, - "angularVelocity": 1.4587668384531083, - "velocityX": 0.4843038911436893, - "velocityY": -0.8885406785775682, - "timestamp": 3.4466252425888095 - }, - { - "x": 8.443758202953443, - "y": 7.312496178717053, - "heading": 0.2700814705675102, - "angularVelocity": 1.7110446011245937, - "velocityX": 0.35178036746362773, - "velocityY": -1.1155752467220204, - "timestamp": 3.490178370527706 - }, - { - "x": 8.453367300003004, - "y": 7.253940317433782, - "heading": 0.354848982447824, - "angularVelocity": 1.9463013540437195, - "velocityX": 0.22062932111422345, - "velocityY": -1.3444697098546596, - "timestamp": 3.5337314984666026 - }, - { - "x": 8.45735124333647, - "y": 7.18531320673646, - "heading": 0.4489562663311165, - "angularVelocity": 2.1607468472831988, - "velocityX": 0.09147318509605634, - "velocityY": -1.5757102634190636, - "timestamp": 3.577284626405499 - }, - { - "x": 8.455833781714013, - "y": 7.106484187240998, - "heading": 0.5511965710396762, - "angularVelocity": 2.347484774273826, - "velocityX": -0.03484162204347062, - "velocityY": -1.8099508169896654, - "timestamp": 3.6208377543443957 - }, - { - "x": 8.448992060952849, - "y": 7.01728730637675, - "heading": 0.6598709993297996, - "angularVelocity": 2.4952152332799042, - "velocityX": -0.15708907913026152, - "velocityY": -2.0480017184847705, - "timestamp": 3.6643908822832922 - }, - { - "x": 8.437091795531597, - "y": 6.917516589235624, - "heading": 0.7725277452001486, - "angularVelocity": 2.5866510903281643, - "velocityX": -0.2732356086558815, - "velocityY": -2.2907818993184903, - "timestamp": 3.707944010222189 - }, - { - "x": 8.420568790056215, - "y": 6.806929386712691, - "heading": 0.8855178727468727, - "angularVelocity": 2.594305688106834, - "velocityX": -0.37937586247679217, - "velocityY": -2.539133416044943, - "timestamp": 3.7514971381610853 - }, - { - "x": 8.40022655182027, - "y": 6.685307877393552, - "heading": 0.992766575428426, - "angularVelocity": 2.462479912625774, - "velocityX": -0.46706721649209976, - "velocityY": -2.7924862133844903, - "timestamp": 3.795050266099982 - }, - { - "x": 8.377585411071777, - "y": 6.553085803985596, - "heading": 1.0808392342145412, - "angularVelocity": 2.0221890586062603, - "velocityX": -0.519851083491793, - "velocityY": -3.035880077165969, - "timestamp": 3.8386033940388784 - }, - { - "x": 8.331156557203096, - "y": 6.248001091638873, - "heading": 1.1393178439925227, - "angularVelocity": 0.6668090360061052, - "velocityX": -0.5294103161580237, - "velocityY": -3.478763324102056, - "timestamp": 3.9263025801930476 - }, - { - "x": 8.32509627497856, - "y": 5.9213777688010225, - "heading": 1.1393178431081437, - "angularVelocity": -1.0084234488389344e-8, - "velocityX": -0.06910306116049933, - "velocityY": -3.7243597935295467, - "timestamp": 4.014001766347217 - }, - { - "x": 8.328579577192095, - "y": 5.594716799276784, - "heading": 1.13931784103149, - "angularVelocity": -2.3679282154855844e-8, - "velocityX": 0.03971875186402571, - "velocityY": -3.7247890641766106, - "timestamp": 4.101700952501386 - }, - { - "x": 8.33206298187959, - "y": 5.268055830845277, - "heading": 1.139317838954836, - "angularVelocity": -2.3679282513207347e-8, - "velocityX": 0.03971992033507496, - "velocityY": -3.724789051716606, - "timestamp": 4.1894001386555555 - }, - { - "x": 8.335546386568366, - "y": 4.941394862413785, - "heading": 1.1393178368781822, - "angularVelocity": -2.3679282515836517e-8, - "velocityX": 0.0397199203496886, - "velocityY": -3.7247890517164506, - "timestamp": 4.277099324809725 - }, - { - "x": 8.339029791257143, - "y": 4.614733893982293, - "heading": 1.1393178348015287, - "angularVelocity": -2.3679282075329618e-8, - "velocityX": 0.039719920349696757, - "velocityY": -3.7247890517164497, - "timestamp": 4.364798510963894 - }, - { - "x": 8.34251319594592, - "y": 4.2880729255508, - "heading": 1.139317832724875, - "angularVelocity": -2.3679282013341874e-8, - "velocityX": 0.03971992034969426, - "velocityY": -3.72478905171645, - "timestamp": 4.452497697118063 - }, - { - "x": 8.345996600634695, - "y": 3.9614119571193074, - "heading": 1.139317830648221, - "angularVelocity": -2.3679283402849396e-8, - "velocityX": 0.039719920349694765, - "velocityY": -3.7247890517164497, - "timestamp": 4.540196883272232 - }, - { - "x": 8.349480005323471, - "y": 3.6347509886878155, - "heading": 1.139317828571567, - "angularVelocity": -2.3679282271688366e-8, - "velocityX": 0.03971992034969471, - "velocityY": -3.7247890517164506, - "timestamp": 4.627896069426401 - }, - { - "x": 8.352963410012249, - "y": 3.3080900202563233, - "heading": 1.139317826494913, - "angularVelocity": -2.367928303185141e-8, - "velocityX": 0.03971992034969481, - "velocityY": -3.72478905171645, - "timestamp": 4.7155952555805705 - }, - { - "x": 8.356446814701025, - "y": 2.981429051824831, - "heading": 1.1393178244182591, - "angularVelocity": -2.3679282848596997e-8, - "velocityX": 0.03971992034969499, - "velocityY": -3.72478905171645, - "timestamp": 4.80329444173474 - }, - { - "x": 8.3599302193898, - "y": 2.6547680833933387, - "heading": 1.1393178223416056, - "angularVelocity": -2.3679282487737324e-8, - "velocityX": 0.0397199203496951, - "velocityY": -3.72478905171645, - "timestamp": 4.890993627888909 - }, - { - "x": 8.363413624078582, - "y": 2.3281071149618464, - "heading": 1.1393178202649519, - "angularVelocity": -2.3679282618768576e-8, - "velocityX": 0.039719920349732145, - "velocityY": -3.7247890517164497, - "timestamp": 4.978692814043078 - }, - { - "x": 8.366897029211325, - "y": 2.001446146535259, - "heading": 1.139317818188219, - "angularVelocity": -2.3680183399714216e-8, - "velocityX": 0.03971992541207313, - "velocityY": -3.724789051660523, - "timestamp": 5.066392000197247 - }, - { - "x": 8.369950836003147, - "y": 1.715318694035658, - "heading": 1.1226153865662416, - "angularVelocity": -0.190451386773599, - "velocityX": 0.03482138119793182, - "velocityY": -3.262601000613718, - "timestamp": 5.154091186351416 - }, - { - "x": 8.372495686328486, - "y": 1.4768791360484652, - "heading": 1.1086925029820063, - "angularVelocity": -0.15875727238517048, - "velocityX": 0.029017946881124475, - "velocityY": -2.718834329523107, - "timestamp": 5.241790372505585 - }, - { - "x": 8.374531573132348, - "y": 1.2861274855011333, - "heading": 1.0975518982604389, - "angularVelocity": -0.12703201945321188, - "velocityX": 0.02321443211894871, - "velocityY": -2.1750675110257403, - "timestamp": 5.329489558659755 - }, - { - "x": 8.376058491392191, - "y": 1.1430637450377992, - "heading": 1.0891955157796347, - "angularVelocity": -0.09528460693026461, - "velocityX": 0.017410860086641132, - "velocityY": -1.6313006623783024, - "timestamp": 5.417188744813924 - }, - { - "x": 8.377076437882634, - "y": 1.0476879159528336, - "heading": 1.0836245099802884, - "angularVelocity": -0.06352403076526633, - "velocityX": 0.01160725127658835, - "velocityY": -1.0875337989716487, - "timestamp": 5.504887930968093 - }, - { - "x": 8.377585411071777, - "y": 1, - "heading": 1.0808392342145412, - "angularVelocity": -0.03175942546206756, - "velocityX": 0.005803624998845044, - "velocityY": -0.5437669155675129, - "timestamp": 5.592587117122262 - }, - { - "x": 8.377585411071777, - "y": 1, - "heading": 1.0808392342145412, - "angularVelocity": 4.871302260765609e-25, - "velocityX": -2.093358596809338e-24, - "velocityY": 4.245353600645279e-24, - "timestamp": 5.680286303276431 - } - ], - "trajectoryWaypoints": [ - { - "timestamp": 0, + "timestamp": 5.6678933208149, "isStopPoint": true, - "x": 0.4269569218158722, - "y": 6.9908647537231445, - "heading": 0, + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 18 }, { - "timestamp": 0.572085759704837, + "timestamp": 7.3582994981379, "isStopPoint": false, - "x": 1.5002212524414062, - "y": 6.694304943084717, - "heading": 0, + "x": 8.056792259216309, + "y": 7.362460613250732, + "heading": 0.24497813041233202, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 18 }, { - "timestamp": 1.162306581796594, + "timestamp": 9.124585923832743, "isStopPoint": true, - "x": 2.319291353225708, - "y": 6.185916423797607, - "heading": 0.2985001179522956, + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 11 }, { - "timestamp": 1.844942457705411, + "timestamp": 9.813738277218697, "isStopPoint": false, - "x": 3.8162126541137695, - "y": 6.581329345703125, - "heading": 0.2985001179522956, + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 20 + "controlIntervalCount": 16 }, { - "timestamp": 3.228859602894327, + "timestamp": 11.081433751522148, "isStopPoint": false, - "x": 8.264610290527344, - "y": 7.456887245178223, + "x": 8.248542785644531, + "y": 4.086092948913574, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 16 }, { - "timestamp": 3.8386033940388784, + "timestamp": 12.333556295160001, "isStopPoint": false, - "x": 8.377585411071777, - "y": 6.553085803985596, - "heading": 1.0808392342145412, + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 8 }, { - "timestamp": 5.680286303276431, + "timestamp": 12.889003463501176, "isStopPoint": true, - "x": 8.377585411071777, - "y": 1, - "heading": 1.0808392342145412, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -37211,38 +41312,55 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ - 4, - 4 + "last" ], - "type": "ZeroAngularVelocity" + "type": "StopPoint", + "direction": 0 }, { "scope": [ - 3, - 4 + 7 ], - "type": "StraightLine" + "type": "StopPoint", + "direction": 0 }, { "scope": [ - 2 + 0, + 1 ], - "type": "WptVelocityDirection", - "direction": 0.2985001179522956 + "type": "ZeroAngularVelocity", + "direction": 0 }, { "scope": [ 2 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ - "last" + 3 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 7 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 9 ], "type": "StopPoint" } diff --git a/src/main/deploy/choreo/AmpLanePAEDF.1.traj b/src/main/deploy/choreo/AmpLanePAEDF.1.traj new file mode 100644 index 00000000..0c0a4c8d --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePAEDF.1.traj @@ -0,0 +1,185 @@ +{ + "samples": [ + { + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": -4.820407208942378e-26, + "angularVelocity": -1.0568380673400748e-29, + "velocityX": 2.0900789241101526e-25, + "velocityY": 6.12105194243679e-25, + "timestamp": 0 + }, + { + "x": 0.4535853919747023, + "y": 6.976442037279776, + "heading": -9.657051327438745e-26, + "angularVelocity": -2.7866726754125085e-27, + "velocityX": 0.3537634154941742, + "velocityY": -0.07356074460262349, + "timestamp": 0.05826629620784589 + }, + { + "x": 0.49483104636031155, + "y": 6.967970532726724, + "heading": -1.449381860201486e-25, + "angularVelocity": -2.8078094367592775e-27, + "velocityX": 0.7078818643024581, + "velocityY": -0.1453928789781493, + "timestamp": 0.11653259241569178 + }, + { + "x": 0.5567354919847137, + "y": 6.955441991018911, + "heading": -1.9330585876590946e-25, + "angularVelocity": -2.807809436759289e-27, + "velocityX": 1.0624400322886223, + "velocityY": -0.21502210580061862, + "timestamp": 0.17479888862353765 + }, + { + "x": 0.6393312280567922, + "y": 6.939025561506136, + "heading": -2.416735315116705e-25, + "angularVelocity": -2.8078094367592704e-27, + "velocityX": 1.417555970563931, + "velocityY": -0.2817482932880241, + "timestamp": 0.23306518483138355 + }, + { + "x": 0.7426606798556364, + "y": 6.918954250670703, + "heading": -2.9004008719315308e-25, + "angularVelocity": -2.7886377311391825e-27, + "velocityX": 1.7734000361075017, + "velocityY": -0.3444754882622709, + "timestamp": 0.29133148103922946 + }, + { + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": -3.4495163504680925e-25, + "angularVelocity": -1.1511759285858433e-25, + "velocityX": 2.130226970897995, + "velocityY": -0.40134567836399193, + "timestamp": 0.34959777724707536 + }, + { + "x": 1.01097419129112, + "y": 6.869549473556677, + "heading": 0.010039096018647366, + "angularVelocity": 0.17297490397522428, + "velocityX": 2.4844650740475953, + "velocityY": -0.44832534810612334, + "timestamp": 0.4076356531928559 + }, + { + "x": 1.175726572408581, + "y": 6.840802777300111, + "heading": 0.03011443547557583, + "angularVelocity": 0.34590065762715055, + "velocityX": 2.8387045258405714, + "velocityY": -0.4953092405280373, + "timestamp": 0.4656735291386364 + }, + { + "x": 1.3610383679335918, + "y": 6.809328854372862, + "heading": 0.06022199220724938, + "angularVelocity": 0.5187570399681801, + "velocityX": 3.1929458565666793, + "velocityY": -0.5422997036737339, + "timestamp": 0.5237114050844169 + }, + { + "x": 1.5460799084730097, + "y": 6.784713461455284, + "heading": 0.15060245758508448, + "angularVelocity": 1.5572669382709545, + "velocityX": 3.1882893287184593, + "velocityY": -0.4241263574251743, + "timestamp": 0.5817492810301974 + }, + { + "x": 1.7105613111663502, + "y": 6.762828401317669, + "heading": 0.23096913740746994, + "angularVelocity": 1.384728136802675, + "velocityX": 2.83403553305432, + "velocityY": -0.37708237562070457, + "timestamp": 0.6397871569759779 + }, + { + "x": 1.8544824093339345, + "y": 6.743675274341158, + "heading": 0.3013204880217799, + "angularVelocity": 1.2121627380029003, + "velocityX": 2.4797788654780635, + "velocityY": -0.33001081904520096, + "timestamp": 0.6978250329217583 + }, + { + "x": 1.977843185069378, + "y": 6.727255600600169, + "heading": 0.36164875379550304, + "angularVelocity": 1.0394637086664282, + "velocityX": 2.1255218893725236, + "velocityY": -0.2829130713937313, + "timestamp": 0.7558629088675388 + }, + { + "x": 2.0806437247378744, + "y": 6.713570715573333, + "heading": 0.41194198603949084, + "angularVelocity": 0.8665588018929588, + "velocityX": 1.7712664013502706, + "velocityY": -0.2357923132752318, + "timestamp": 0.8139007848133193 + }, + { + "x": 2.162884155033722, + "y": 6.702621710001363, + "heading": 0.45218713038285063, + "angularVelocity": 0.6934289666451124, + "velocityX": 1.4170130962869354, + "velocityY": -0.18865276155520178, + "timestamp": 0.8719386607590998 + }, + { + "x": 2.2245645766627997, + "y": 6.694409403888087, + "heading": 0.4823732249131576, + "angularVelocity": 0.5201102562489893, + "velocityX": 1.0627615263987253, + "velocityY": -0.1414990810647272, + "timestamp": 0.9299765367048802 + }, + { + "x": 2.265685008078604, + "y": 6.688934342765278, + "heading": 0.5024941236633388, + "angularVelocity": 0.34668565005684077, + "velocityX": 0.7085102744666172, + "velocityY": -0.09433600099222642, + "timestamp": 0.9880144126506607 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.17327126075488852, + "velocityX": 0.354257244182047, + "velocityY": -0.047168141046465786, + "timestamp": 1.0460522885964412 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -1.480616560828957e-26, + "velocityX": -5.749006986314809e-23, + "velocityY": -3.283580548358229e-23, + "timestamp": 1.1040901645422216 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePAEDF.2.traj b/src/main/deploy/choreo/AmpLanePAEDF.2.traj new file mode 100644 index 00000000..66aab758 --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePAEDF.2.traj @@ -0,0 +1,86 @@ +{ + "samples": [ + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -1.480616560828957e-26, + "velocityX": -5.749006986314809e-23, + "velocityY": -3.283580548358229e-23, + "timestamp": 0 + }, + { + "x": 2.3347330882331176, + "y": 6.713955255875249, + "heading": 0.5125504196, + "angularVelocity": -2.553583698003105e-17, + "velocityX": 0.5108360349814401, + "velocityY": 0.2924454065412527, + "timestamp": 0.09491840599213663 + }, + { + "x": 2.4317085702199166, + "y": 6.7694721581924355, + "heading": 0.5125504196, + "angularVelocity": -7.602638325412282e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645902, + "timestamp": 0.18983681198427327 + }, + { + "x": 2.577171786567949, + "y": 6.852747507871406, + "heading": 0.5125504196, + "angularVelocity": -2.864728625251783e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461279, + "timestamp": 0.2847552179764099 + }, + { + "x": 2.771122709968306, + "y": 6.963781289278251, + "heading": 0.5125504196, + "angularVelocity": -4.95735356219891e-19, + "velocityX": 2.0433436631502695, + "velocityY": 1.1697813532187173, + "timestamp": 0.37967362396854654 + }, + { + "x": 2.9165859263163383, + "y": 7.047056638957223, + "heading": 0.5125504196, + "angularVelocity": -2.1000816812429605e-17, + "velocityX": 1.532507998080826, + "velocityY": 0.8773361584461278, + "timestamp": 0.47459202996068317 + }, + { + "x": 3.0135614083031372, + "y": 7.102573541274409, + "heading": 0.5125504196, + "angularVelocity": 1.0493910343453442e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645902, + "timestamp": 0.5695104359528198 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": 3.728395973644304e-17, + "velocityX": 0.5108360349814401, + "velocityY": 0.2924454065412527, + "timestamp": 0.6644288419449564 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": -2.9402212487685217e-25, + "velocityX": 6.030156719342467e-23, + "velocityY": 3.0718511862526725e-23, + "timestamp": 0.7593472479370931 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePAEDF.3.traj b/src/main/deploy/choreo/AmpLanePAEDF.3.traj new file mode 100644 index 00000000..3bdab8ec --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePAEDF.3.traj @@ -0,0 +1,536 @@ +{ + "samples": [ + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": -2.9402212487685217e-25, + "velocityX": 6.030156719342467e-23, + "velocityY": 3.0718511862526725e-23, + "timestamp": 0 + }, + { + "x": 3.0865020118467, + "y": 7.124545249523675, + "heading": 0.48610131944841944, + "angularVelocity": -0.40878852104321894, + "velocityX": 0.3779353165675877, + "velocityY": -0.08943798979559645, + "timestamp": 0.0647011811488325 + }, + { + "x": 3.135458110441477, + "y": 7.112972562266447, + "heading": 0.4339971852577861, + "angularVelocity": -0.8053042195748789, + "velocityX": 0.7566492253389184, + "velocityY": -0.17886361657922661, + "timestamp": 0.129402362297665 + }, + { + "x": 3.2089771746406726, + "y": 7.095606129050513, + "heading": 0.357225026215491, + "angularVelocity": -1.1865650314125755, + "velocityX": 1.136286276290371, + "velocityY": -0.2684098328280398, + "timestamp": 0.1941035434464975 + }, + { + "x": 3.307133671873185, + "y": 7.072412045867193, + "heading": 0.2571531716690748, + "angularVelocity": -1.5466773986122515, + "velocityX": 1.5170742711284095, + "velocityY": -0.3584800581919169, + "timestamp": 0.25880472459532977 + }, + { + "x": 3.4300337444037163, + "y": 7.043313631479606, + "heading": 0.13605141213735358, + "angularVelocity": -1.8717086362480815, + "velocityX": 1.8995027656114087, + "velocityY": -0.44973544332447146, + "timestamp": 0.32350590574416205 + }, + { + "x": 3.5778386039995205, + "y": 7.008182862001031, + "heading": -0.0018831098932495121, + "angularVelocity": -2.1318702314461877, + "velocityX": 2.2844228957089365, + "velocityY": -0.5429695231956211, + "timestamp": 0.3882070868929943 + }, + { + "x": 3.75075226330582, + "y": 6.966825747454083, + "heading": -0.14853260580564892, + "angularVelocity": -2.266565977753341, + "velocityX": 2.672496177597522, + "velocityY": -0.6392018478273882, + "timestamp": 0.4529082680418266 + }, + { + "x": 3.9486334270649626, + "y": 6.918918967044097, + "heading": -0.28405555900160556, + "angularVelocity": -2.094597823248583, + "velocityX": 3.0583856468393638, + "velocityY": -0.7404313114436918, + "timestamp": 0.5176094491906589 + }, + { + "x": 4.166070195160817, + "y": 6.865183550075389, + "heading": -0.3449865942152846, + "angularVelocity": -0.9417298746605423, + "velocityX": 3.360630582549694, + "velocityY": -0.8305167852361092, + "timestamp": 0.5823106303394912 + }, + { + "x": 4.4009794486877345, + "y": 6.8112911031395695, + "heading": -0.3449866586118039, + "angularVelocity": -9.952912480962177e-7, + "velocityX": 3.6306795232463367, + "velocityY": -0.8329437883344079, + "timestamp": 0.6470118114883234 + }, + { + "x": 4.635888809481111, + "y": 6.757399123762019, + "heading": -0.34498672300630057, + "angularVelocity": -9.952599862485997e-7, + "velocityX": 3.630681181121147, + "velocityY": -0.8329365619088571, + "timestamp": 0.7117129926371557 + }, + { + "x": 4.87079817027739, + "y": 6.703507144397122, + "heading": -0.3449867874007968, + "angularVelocity": -9.952599802438338e-7, + "velocityX": 3.630681181166014, + "velocityY": -0.8329365617132907, + "timestamp": 0.776414173785988 + }, + { + "x": 5.105707531073669, + "y": 6.649615165032224, + "heading": -0.3449868517952927, + "angularVelocity": -9.952599738581064e-7, + "velocityX": 3.6306811811660147, + "velocityY": -0.8329365617132863, + "timestamp": 0.8411153549348203 + }, + { + "x": 5.340616891869949, + "y": 6.595723185667327, + "heading": -0.3449869161897882, + "angularVelocity": -9.952599684354273e-7, + "velocityX": 3.630681181166015, + "velocityY": -0.8329365617132873, + "timestamp": 0.9058165360836525 + }, + { + "x": 5.575526252666228, + "y": 6.54183120630243, + "heading": -0.34498698058428334, + "angularVelocity": -9.952599624061458e-7, + "velocityX": 3.6306811811660156, + "velocityY": -0.8329365617132882, + "timestamp": 0.9705177172324848 + }, + { + "x": 5.8104356134625075, + "y": 6.4879392269375336, + "heading": -0.344987044978778, + "angularVelocity": -9.952599561054567e-7, + "velocityX": 3.6306811811660156, + "velocityY": -0.8329365617132889, + "timestamp": 1.035218898381317 + }, + { + "x": 6.0453449742587875, + "y": 6.434047247572637, + "heading": -0.3449871093732723, + "angularVelocity": -9.952599496144382e-7, + "velocityX": 3.6306811811660165, + "velocityY": -0.8329365617132901, + "timestamp": 1.0999200795301494 + }, + { + "x": 6.280254335055067, + "y": 6.38015526820774, + "heading": -0.3449871737677662, + "angularVelocity": -9.952599439036616e-7, + "velocityX": 3.630681181166016, + "velocityY": -0.832936561713291, + "timestamp": 1.1646212606789816 + }, + { + "x": 6.515163695851346, + "y": 6.326263288842841, + "heading": -0.3449872381622598, + "angularVelocity": -9.952599382292206e-7, + "velocityX": 3.6306811811660133, + "velocityY": -0.8329365617133051, + "timestamp": 1.229322441827814 + }, + { + "x": 6.750073056640409, + "y": 6.2723713094464895, + "heading": -0.34498730255675286, + "angularVelocity": -9.952599312963826e-7, + "velocityX": 3.6306811810544866, + "velocityY": -0.832936562199441, + "timestamp": 1.2940236229766462 + }, + { + "x": 6.984982150775984, + "y": 6.2184781677494305, + "heading": -0.3449873669513136, + "angularVelocity": -9.95260976649588e-7, + "velocityX": 3.6306770597466, + "velocityY": -0.8329545263337457, + "timestamp": 1.3587248041254785 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.00000172520007220815, + "velocityX": 3.5518244907979937, + "velocityY": -1.1225730284587905, + "timestamp": 1.4234259852743107 + }, + { + "x": 7.409091443638604, + "y": 6.073965873280571, + "heading": -0.3572291452032454, + "angularVelocity": -0.20035361948729366, + "velocityX": 3.1800506242888202, + "velocityY": -1.1764343450588084, + "timestamp": 1.4845262871344713 + }, + { + "x": 7.58146433123023, + "y": 6.0094940876522065, + "heading": -0.36768136673125634, + "angularVelocity": -0.17106661030796205, + "velocityX": 2.8211462520452746, + "velocityY": -1.0551794944633763, + "timestamp": 1.545626588994632 + }, + { + "x": 7.73220907640328, + "y": 5.953248314526567, + "heading": -0.37521177845828957, + "angularVelocity": -0.1232467188831242, + "velocityX": 2.4671685831938674, + "velocityY": -0.920548203744869, + "timestamp": 1.6067268908547925 + }, + { + "x": 7.861444501366057, + "y": 5.905524901339061, + "heading": -0.37940510209885836, + "angularVelocity": -0.06863016241991673, + "velocityX": 2.1151356217282813, + "velocityY": -0.7810667334627726, + "timestamp": 1.667827192714953 + }, + { + "x": 7.969234018825332, + "y": 5.866476846712542, + "heading": -0.38004587758642694, + "angularVelocity": -0.010487272043845954, + "velocityX": 1.7641405063099576, + "velocityY": -0.6390812064380308, + "timestamp": 1.7289274945751136 + }, + { + "x": 8.055617039018077, + "y": 5.8361975486879665, + "heading": -0.37700219526471646, + "angularVelocity": 0.04981452184436869, + "velocityX": 1.413790399766751, + "velocityY": -0.4955670774569341, + "timestamp": 1.7900277964352742 + }, + { + "x": 8.12062042791295, + "y": 5.814749961981242, + "heading": -0.37018491999207215, + "angularVelocity": 0.11157514881427125, + "velocityX": 1.0638799959392298, + "velocityY": -0.3510225981503441, + "timestamp": 1.8511280982954348 + }, + { + "x": 8.164263678264032, + "y": 5.802179408572721, + "heading": -0.3595296539883727, + "angularVelocity": 0.17438974406519217, + "velocityX": 0.7142886208805774, + "velocityY": -0.20573635523587247, + "timestamp": 1.9122284001555954 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.23800496841831012, + "velocityX": 0.3649393788537573, + "velocityY": -0.05989038131588724, + "timestamp": 1.973328702015756 + }, + { + "x": 8.18566399540182, + "y": 5.805056139735724, + "heading": -0.32461060520225765, + "angularVelocity": 0.30765327861825625, + "velocityX": -0.013551942707795956, + "velocityY": 0.09868234683560065, + "timestamp": 2.0395619417948208 + }, + { + "x": 8.159694473473342, + "y": 5.822092590454781, + "heading": -0.2998178925011234, + "angularVelocity": 0.3743243239170494, + "velocityX": -0.3920919770058792, + "velocityY": 0.25721904554096775, + "timestamp": 2.1057951815738853 + }, + { + "x": 8.108649030585239, + "y": 5.849626323109156, + "heading": -0.2708591800534793, + "angularVelocity": 0.4372232514103506, + "velocityX": -0.7706922243027172, + "velocityY": 0.41570867960287095, + "timestamp": 2.17202842135295 + }, + { + "x": 8.03252261804608, + "y": 5.887653107737229, + "heading": -0.2380618973255709, + "angularVelocity": 0.4951785966881702, + "velocityX": -1.1493686975466637, + "velocityY": 0.5741344490307231, + "timestamp": 2.2382616611320145 + }, + { + "x": 7.9313086642803885, + "y": 5.936166919703684, + "heading": -0.20187479302243566, + "angularVelocity": 0.5463586625664867, + "velocityX": -1.5281443894834816, + "velocityY": 0.7324692575553122, + "timestamp": 2.304494900911079 + }, + { + "x": 7.804998346649701, + "y": 5.9951585907413225, + "heading": -0.16295187123346636, + "angularVelocity": 0.5876644705710469, + "velocityX": -1.9070532870205719, + "velocityY": 0.8906656421219618, + "timestamp": 2.3707281406901437 + }, + { + "x": 7.653579505512198, + "y": 6.0646127178391085, + "heading": -0.12233575906363518, + "angularVelocity": 0.6132285285351425, + "velocityX": -2.286145772765958, + "velocityY": 1.048629469575472, + "timestamp": 2.4369613804692083 + }, + { + "x": 7.4770359425587705, + "y": 6.144498725543435, + "heading": -0.08194804569131871, + "angularVelocity": 0.6097801271240603, + "velocityX": -2.6654828231614, + "velocityY": 1.206131663962129, + "timestamp": 2.503194620248273 + }, + { + "x": 7.27536197665765, + "y": 6.234730628199731, + "heading": -0.046490249768474004, + "angularVelocity": 0.5353474485186261, + "velocityX": -3.044905648188839, + "velocityY": 1.362335633245254, + "timestamp": 2.5694278600273375 + }, + { + "x": 7.049560259358204, + "y": 6.334147758763648, + "heading": -0.04649017070950518, + "angularVelocity": 0.0000011936448991960951, + "velocityX": -3.4091902804793617, + "velocityY": 1.5010156666885635, + "timestamp": 2.635661099806402 + }, + { + "x": 6.822106687595601, + "y": 6.429725315002885, + "heading": -0.04649016173832054, + "angularVelocity": 1.354483741042957e-7, + "velocityX": -3.4341302421763333, + "velocityY": 1.4430451621882883, + "timestamp": 2.7018943395854667 + }, + { + "x": 6.586112566537963, + "y": 6.501676487205841, + "heading": -0.04649015261662469, + "angularVelocity": 1.3772081626072917e-7, + "velocityX": -3.563076815279552, + "velocityY": 1.086330254158866, + "timestamp": 2.7681275793645312 + }, + { + "x": 6.343631547831337, + "y": 6.547208357601844, + "heading": -0.046490142857452815, + "angularVelocity": 1.4734553082512417e-7, + "velocityX": -3.6610170288434327, + "velocityY": 0.6874474289327321, + "timestamp": 2.834360819143596 + }, + { + "x": 6.097611695820765, + "y": 6.565767095818739, + "heading": -0.046490131800601416, + "angularVelocity": 1.6693810300562863e-7, + "velocityX": -3.7144468975279485, + "velocityY": 0.28020278456559833, + "timestamp": 2.9005940589226604 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.04649011830549673, + "angularVelocity": 2.0375123930314497e-7, + "velocityX": -3.722715642994527, + "velocityY": -0.13044955895548105, + "timestamp": 2.966827298701725 + }, + { + "x": 5.59400062961639, + "y": 6.517937013895863, + "heading": -0.046490107217199095, + "angularVelocity": 1.588527062833638e-7, + "velocityX": -3.682446546399627, + "velocityY": -0.5614419262393832, + "timestamp": 3.03662968283788 + }, + { + "x": 5.343239064324899, + "y": 6.449191978638034, + "heading": -0.046490097789975614, + "angularVelocity": 1.3505589536533548e-7, + "velocityX": -3.5924498624912715, + "velocityY": -0.984852252664273, + "timestamp": 3.106432066974035 + }, + { + "x": 5.10214573961629, + "y": 6.351820274741271, + "heading": -0.04649007357112031, + "angularVelocity": 3.469631532168612e-7, + "velocityX": -3.453941118090443, + "velocityY": -1.3949624371974614, + "timestamp": 3.17623445111019 + }, + { + "x": 4.887305900136772, + "y": 6.241894655501948, + "heading": -0.013339477589168081, + "angularVelocity": 0.4749206834724951, + "velocityX": -3.0778295345966584, + "velocityY": -1.574811814807135, + "timestamp": 3.246036835246345 + }, + { + "x": 4.700625263351654, + "y": 6.142924243380359, + "heading": 0.02019201613120645, + "angularVelocity": 0.4803774847427688, + "velocityX": -2.6744163411522406, + "velocityY": -1.417865784190692, + "timestamp": 3.3158392193825 + }, + { + "x": 4.541203570947359, + "y": 6.05696873798045, + "heading": 0.05079450877042205, + "angularVelocity": 0.4384161517968095, + "velocityX": -2.283900390756435, + "velocityY": -1.2314121711408288, + "timestamp": 3.385641603518655 + }, + { + "x": 4.408671715563658, + "y": 5.984755616609754, + "heading": 0.07726906366461818, + "angularVelocity": 0.3792786624960486, + "velocityX": -1.8986723308073217, + "velocityY": -1.034536603074175, + "timestamp": 3.45544398765481 + }, + { + "x": 4.302830646527999, + "y": 5.92665565579208, + "heading": 0.09900038404891696, + "angularVelocity": 0.3113263343829381, + "velocityX": -1.5162959022890692, + "velocityY": -0.8323492318592651, + "timestamp": 3.5252463717909652 + }, + { + "x": 4.22355609558814, + "y": 5.882893451015234, + "heading": 0.11561440292356019, + "angularVelocity": 0.23801506324248706, + "velocityX": -1.1356997604154628, + "velocityY": -0.6269442701482041, + "timestamp": 3.5950487559271203 + }, + { + "x": 4.170763147512419, + "y": 5.853619589907405, + "heading": 0.1268589116332185, + "angularVelocity": 0.1610906109986881, + "velocityX": -0.756320127586832, + "velocityY": -0.4193819662481466, + "timestamp": 3.6648511400632753 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.08154784945777828, + "velocityX": -0.37782436284418364, + "velocityY": -0.21027274576950902, + "timestamp": 3.7346535241994303 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.1000727380078476e-26, + "velocityX": 7.869680140840765e-27, + "velocityY": 7.286496207304452e-27, + "timestamp": 3.8044559083355853 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePAEDF.4.traj b/src/main/deploy/choreo/AmpLanePAEDF.4.traj new file mode 100644 index 00000000..5eec9f40 --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePAEDF.4.traj @@ -0,0 +1,338 @@ +{ + "samples": [ + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.1000727380078476e-26, + "velocityX": 7.869680140840765e-27, + "velocityY": 7.286496207304452e-27, + "timestamp": 0 + }, + { + "x": 4.195350058507561, + "y": 5.858786392293566, + "heading": 0.13540679339101, + "angularVelocity": 0.03040787160499198, + "velocityX": 0.5426383042255808, + "velocityY": 0.2113090624413005, + "timestamp": 0.09391145429572223 + }, + { + "x": 4.297269963634785, + "y": 5.898475069782893, + "heading": 0.14111775296085857, + "angularVelocity": 0.06081217262236255, + "velocityX": 1.085276613929165, + "velocityY": 0.42261806919047107, + "timestamp": 0.18782290859144446 + }, + { + "x": 4.450149823252348, + "y": 5.958008075795516, + "heading": 0.14968325491595807, + "angularVelocity": 0.09120827719403884, + "velocityX": 1.6279149414101537, + "velocityY": 0.6339269949452273, + "timestamp": 0.2817343628871667 + }, + { + "x": 4.653989639578877, + "y": 6.037385400048414, + "heading": 0.16110213512106505, + "angularVelocity": 0.12159198567141313, + "velocityX": 2.1705532925158373, + "velocityY": 0.8452358112030016, + "timestamp": 0.3756458171828889 + }, + { + "x": 4.908789413625767, + "y": 6.136607028819001, + "heading": 0.17537288795521153, + "angularVelocity": 0.15195966180236836, + "velocityX": 2.7131916543911667, + "velocityY": 1.0565444813381684, + "timestamp": 0.46955727147861115 + }, + { + "x": 5.214549136636689, + "y": 6.255672941521673, + "heading": 0.19249371968896806, + "angularVelocity": 0.1823082377134106, + "velocityX": 3.2558299230262406, + "velocityY": 1.2678529322710714, + "timestamp": 0.5634687257743334 + }, + { + "x": 5.540526297381237, + "y": 6.382610310333338, + "heading": 0.1924937215917483, + "angularVelocity": 2.0261428816305725e-8, + "velocityX": 3.4711118381583637, + "velocityY": 1.3516707814144493, + "timestamp": 0.6573801800700556 + }, + { + "x": 5.866503458135056, + "y": 6.50954767912126, + "heading": 0.19249372349451602, + "angularVelocity": 2.0261295290604937e-8, + "velocityX": 3.471111838257083, + "velocityY": 1.351670781161622, + "timestamp": 0.7512916343657778 + }, + { + "x": 6.192480618888875, + "y": 6.636485047909182, + "heading": 0.19249372539728374, + "angularVelocity": 2.0261295568537564e-8, + "velocityX": 3.471111838257084, + "velocityY": 1.3516707811616195, + "timestamp": 0.8452030886615001 + }, + { + "x": 6.518457779642694, + "y": 6.763422416697105, + "heading": 0.19249372730005146, + "angularVelocity": 2.0261295333491658e-8, + "velocityX": 3.4711118382570834, + "velocityY": 1.35167078116162, + "timestamp": 0.9391145429572223 + }, + { + "x": 6.844434940394272, + "y": 6.890359785490778, + "heading": 0.19249372920281918, + "angularVelocity": 2.0261295628050687e-8, + "velocityX": 3.471111838233232, + "velocityY": 1.3516707812228705, + "timestamp": 1.0330259972529445 + }, + { + "x": 7.170411838536006, + "y": 7.017297824914972, + "heading": 0.1924937316314852, + "angularVelocity": 2.586123310059887e-8, + "velocityX": 3.4711090418773667, + "velocityY": 1.351677922316834, + "timestamp": 1.1269374515486668 + }, + { + "x": 7.445544445617428, + "y": 7.124436352045788, + "heading": 0.20746316204367837, + "angularVelocity": 0.15939941005551134, + "velocityX": 2.929702336575951, + "velocityY": 1.1408462144931066, + "timestamp": 1.220848905844389 + }, + { + "x": 7.669715374136242, + "y": 7.211730123643104, + "heading": 0.22019672303892965, + "angularVelocity": 0.13559113838397205, + "velocityX": 2.38704565060736, + "velocityY": 0.9295327417934781, + "timestamp": 1.3147603601401112 + }, + { + "x": 7.842925396345479, + "y": 7.279179365267254, + "heading": 0.23039470264394565, + "angularVelocity": 0.10859143521410211, + "velocityX": 1.8443971878425902, + "velocityY": 0.7182216709343664, + "timestamp": 1.4086718144358334 + }, + { + "x": 7.96517479602962, + "y": 7.326784165564593, + "heading": 0.23795514608622043, + "angularVelocity": 0.08050608415100667, + "velocityX": 1.3017517469081525, + "velocityY": 0.506911544010745, + "timestamp": 1.5025832687315557 + }, + { + "x": 8.0364637201711, + "y": 7.354544571992242, + "heading": 0.24282665608297047, + "angularVelocity": 0.05187343794517228, + "velocityX": 0.7591078710909263, + "velocityY": 0.2956019224261278, + "timestamp": 1.596494723027278 + }, + { + "x": 8.056792259216309, + "y": 7.362460613250732, + "heading": 0.244978130412332, + "angularVelocity": 0.022909605068905204, + "velocityX": 0.21646495837660984, + "velocityY": 0.0842926064542015, + "timestamp": 1.6904061773230001 + }, + { + "x": 8.02239522092899, + "y": 7.349066085390599, + "heading": 0.24423351408772678, + "angularVelocity": -0.00758829012549376, + "velocityX": -0.3505358361841942, + "velocityY": -0.13650192741958841, + "timestamp": 1.7885332009727142 + }, + { + "x": 7.932360084343089, + "y": 7.314005643971181, + "heading": 0.24049622236652166, + "angularVelocity": -0.03808626392813261, + "velocityX": -0.9175366095613122, + "velocityY": -0.35729649300863325, + "timestamp": 1.8866602246224282 + }, + { + "x": 7.786686853677988, + "y": 7.257279283570004, + "heading": 0.23376594617518753, + "angularVelocity": -0.06858738745974144, + "velocityX": -1.4845373399392365, + "velocityY": -0.578091113857428, + "timestamp": 1.9847872482721423 + }, + { + "x": 7.5853755359232595, + "y": 7.178886996735875, + "heading": 0.2240420388382404, + "angularVelocity": -0.09909510117884326, + "velocityX": -2.0515379990873206, + "velocityY": -0.7988858106403784, + "timestamp": 2.0829142719218563 + }, + { + "x": 7.328426142666821, + "y": 7.078828774757454, + "heading": 0.21132346369891478, + "angularVelocity": -0.1296133793350078, + "velocityX": -2.618538540144425, + "velocityY": -1.0196805961994813, + "timestamp": 2.1810412955715703 + }, + { + "x": 7.01583869900668, + "y": 6.957104611217254, + "heading": 0.19560873409309495, + "angularVelocity": -0.1601468078958253, + "velocityX": -3.1855388254309114, + "velocityY": -1.2404754471583816, + "timestamp": 2.2791683192212844 + }, + { + "x": 6.675228781735299, + "y": 6.824469292432169, + "heading": 0.19560873219026645, + "angularVelocity": -1.939148261699155e-8, + "velocityX": -3.471112284901898, + "velocityY": -1.3516696405524002, + "timestamp": 2.3772953428709984 + }, + { + "x": 6.334618864455473, + "y": 6.691833973668682, + "heading": 0.19560873028745154, + "angularVelocity": -1.9391344572203613e-8, + "velocityX": -3.4711122849879685, + "velocityY": -1.3516696403322965, + "timestamp": 2.4754223665207116 + }, + { + "x": 5.994008947175647, + "y": 6.559198654905194, + "heading": 0.19560872838463667, + "angularVelocity": -1.9391344282542406e-8, + "velocityX": -3.471112284987969, + "velocityY": -1.3516696403322948, + "timestamp": 2.5735493901704247 + }, + { + "x": 5.65339902989582, + "y": 6.426563336141707, + "heading": 0.19560872648182165, + "angularVelocity": -1.9391345115512158e-8, + "velocityX": -3.4711122849879685, + "velocityY": -1.3516696403322956, + "timestamp": 2.671676413820138 + }, + { + "x": 5.312789112621013, + "y": 6.293928017365348, + "heading": 0.195608724578999, + "angularVelocity": -1.939142318059707e-8, + "velocityX": -3.4711122849368206, + "velocityY": -1.3516696404634736, + "timestamp": 2.769803437469851 + }, + { + "x": 4.978960833752885, + "y": 6.1639320605180385, + "heading": 0.17759686591379087, + "angularVelocity": -0.18355655756466713, + "velocityX": -3.402001471682276, + "velocityY": -1.324772239208634, + "timestamp": 2.867930461119564 + }, + { + "x": 4.700770593105711, + "y": 6.055602072933751, + "heading": 0.16258358561165714, + "angularVelocity": -0.15299842738252176, + "velocityX": -2.8350013105486314, + "velocityY": -1.1039771059498866, + "timestamp": 2.9660574847692773 + }, + { + "x": 4.478218399750123, + "y": 5.968938070584376, + "heading": 0.1505712213556835, + "angularVelocity": -0.1224164741697909, + "velocityX": -2.2680010569772926, + "velocityY": -0.8831818099236476, + "timestamp": 3.0641845084189905 + }, + { + "x": 4.311304254190931, + "y": 5.9039400626948755, + "heading": 0.1415612155720731, + "angularVelocity": -0.09181982137533895, + "velocityX": -1.701000798261533, + "velocityY": -0.6623864198869991, + "timestamp": 3.1623115320687036 + }, + { + "x": 4.2000281561602835, + "y": 5.860608055094718, + "heading": 0.13555442251274583, + "angularVelocity": -0.06121446300837455, + "velocityX": -1.1340005422754125, + "velocityY": -0.4415909704429648, + "timestamp": 3.260438555718417 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": -0.030606008971789937, + "velocityX": -0.5670002807557282, + "velocityY": -0.2207954888983711, + "timestamp": 3.35856557936813 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.2638912367197548e-25, + "velocityX": -6.105559884328861e-26, + "velocityY": -2.4380402560227299e-26, + "timestamp": 3.456692603017843 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePAEDF.5.traj b/src/main/deploy/choreo/AmpLanePAEDF.5.traj new file mode 100644 index 00000000..7b5c4b70 --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePAEDF.5.traj @@ -0,0 +1,473 @@ +{ + "samples": [ + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.2638912367197548e-25, + "velocityX": -6.105559884328861e-26, + "velocityY": -2.4380402560227299e-26, + "timestamp": 0 + }, + { + "x": 4.149229218776726, + "y": 5.815100726386144, + "heading": 0.12918022174703217, + "angularVelocity": -0.05380546988265665, + "velocityX": 0.07724016042252432, + "velocityY": -0.3805465783235216, + "timestamp": 0.06265021394417758 + }, + { + "x": 4.159717289478365, + "y": 5.7675956417955225, + "heading": 0.12259706469587767, + "angularVelocity": -0.10507796600695042, + "velocityX": 0.16740678189838681, + "velocityY": -0.7582589364012235, + "timestamp": 0.12530042788835516 + }, + { + "x": 4.176867312970838, + "y": 5.696690246498273, + "heading": 0.11300853276979594, + "angularVelocity": -0.15304867010709985, + "velocityX": 0.2737424569332353, + "velocityY": -1.1317662116912757, + "timestamp": 0.18795064183253274 + }, + { + "x": 4.201974040585745, + "y": 5.602790592825652, + "heading": 0.10069220099175857, + "angularVelocity": -0.19658882233046115, + "velocityX": 0.40074448328743983, + "velocityY": -1.498792226891454, + "timestamp": 0.2506008557767103 + }, + { + "x": 4.2367303910144685, + "y": 5.486551757326023, + "heading": 0.08603276386217494, + "angularVelocity": -0.2339886204162901, + "velocityX": 0.5547682639949261, + "velocityY": -1.8553621477366333, + "timestamp": 0.3132510697208879 + }, + { + "x": 4.283393708275681, + "y": 5.349087774330418, + "heading": 0.06958159023755035, + "angularVelocity": -0.2625876687234105, + "velocityX": 0.7448229514872903, + "velocityY": -2.1941502565033004, + "timestamp": 0.3759012836650655 + }, + { + "x": 4.3449830971903305, + "y": 5.19239819137041, + "heading": 0.05215018239550137, + "angularVelocity": -0.2782338119001568, + "velocityX": 0.9830674955001105, + "velocityY": -2.5010223125434243, + "timestamp": 0.43855149760924306 + }, + { + "x": 4.425324635342363, + "y": 5.020190178284826, + "heading": 0.03492497946542863, + "angularVelocity": -0.2749424438585412, + "velocityX": 1.2823825026937377, + "velocityY": -2.7487218677181704, + "timestamp": 0.5012017115534206 + }, + { + "x": 4.528280141254807, + "y": 4.838988810812342, + "heading": 0.019478906624101934, + "angularVelocity": -0.24654461443801812, + "velocityX": 1.6433384569792095, + "velocityY": -2.8922705297373237, + "timestamp": 0.5638519254975982 + }, + { + "x": 4.6555610459377, + "y": 4.65789803685685, + "heading": 0.007446897298019998, + "angularVelocity": -0.1920505704386356, + "velocityX": 2.0316116525364616, + "velocityY": -2.8905052761167873, + "timestamp": 0.6265021394417758 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -3.9652014929192097e-25, + "angularVelocity": -0.1188646746626479, + "velocityX": 2.3941774298704206, + "velocityY": -2.7520844885235216, + "timestamp": 0.6891523533859534 + }, + { + "x": 5.026556371976755, + "y": 4.290478604211026, + "heading": -0.0011711585949671858, + "angularVelocity": -0.014781576411141743, + "velocityX": 2.7893083450033753, + "velocityY": -2.4611683748987776, + "timestamp": 0.7683833205299191 + }, + { + "x": 5.271648004487175, + "y": 4.126055401914557, + "heading": -0.0011711658442509346, + "angularVelocity": -9.149558575332566e-8, + "velocityX": 3.0933818094770933, + "velocityY": -2.075239117019822, + "timestamp": 0.8476142876738848 + }, + { + "x": 5.536248278847656, + "y": 3.995320554415454, + "heading": -0.0011711670074400294, + "angularVelocity": -1.4680990736292476e-8, + "velocityX": 3.3396067711718374, + "velocityY": -1.6500473515810268, + "timestamp": 0.9268452548178505 + }, + { + "x": 5.8157535418727955, + "y": 3.900548632610079, + "heading": -0.001171167882288204, + "angularVelocity": -1.1041745495501586e-8, + "velocityX": 3.5277275174146094, + "velocityY": -1.19614748149131, + "timestamp": 1.0060762219618162 + }, + { + "x": 6.105300821187418, + "y": 3.843388468655785, + "heading": -0.001171168612334034, + "angularVelocity": -9.214147655131007e-9, + "velocityX": 3.654471095733389, + "velocityY": -0.7214371604278409, + "timestamp": 1.085307189105782 + }, + { + "x": 6.399852439542981, + "y": 3.8248343191469996, + "heading": -0.0011711692753000216, + "angularVelocity": -8.367510982006514e-9, + "velocityX": 3.717632498671272, + "velocityY": -0.23417800107213171, + "timestamp": 1.1645381562497477 + }, + { + "x": 6.694725372224158, + "y": 3.837278843369643, + "heading": -0.0011711699217371897, + "angularVelocity": -8.158895331165977e-9, + "velocityX": 3.721687912068311, + "velocityY": 0.15706641823558024, + "timestamp": 1.2437691233937134 + }, + { + "x": 6.989598240261493, + "y": 3.849724899233505, + "heading": -0.001171170568174097, + "angularVelocity": -8.158892044748454e-9, + "velocityX": 3.721687096177193, + "velocityY": 0.15708574958131358, + "timestamp": 1.323000090537679 + }, + { + "x": 7.2831382115989305, + "y": 3.8803709956962793, + "heading": -0.0011711735014437953, + "angularVelocity": -3.702175808154048e-8, + "velocityX": 3.7048641701427742, + "velocityY": 0.38679442606183717, + "timestamp": 1.4022310576816448 + }, + { + "x": 7.537795284833571, + "y": 3.9128988141490195, + "heading": -0.0022060584163901163, + "angularVelocity": -0.013061621639249952, + "velocityX": 3.2141103714147587, + "velocityY": 0.4105442559300826, + "timestamp": 1.4814620248256105 + }, + { + "x": 7.7535280379166585, + "y": 3.9448039763833895, + "heading": -0.0030479668047439287, + "angularVelocity": -0.010626001659477862, + "velocityX": 2.7228337714355204, + "velocityY": 0.40268550775604034, + "timestamp": 1.5606929919695762 + }, + { + "x": 7.9303456416380165, + "y": 3.9756628799518183, + "heading": -0.0034891865807647085, + "angularVelocity": -0.005568779379141843, + "velocityX": 2.2316729189998985, + "velocityY": 0.38948033427835, + "timestamp": 1.639923959113542 + }, + { + "x": 8.06825324660345, + "y": 4.0053006852761595, + "heading": -0.003443969014778176, + "angularVelocity": 0.000570705717934386, + "velocityX": 1.7405770740479534, + "velocityY": 0.374068453190637, + "timestamp": 1.7191549262575077 + }, + { + "x": 8.167254001972681, + "y": 4.03362192898922, + "heading": -0.002865492528698777, + "angularVelocity": 0.007301141295275209, + "velocityX": 1.2495209756728547, + "velocityY": 0.3574516976625985, + "timestamp": 1.7983858934014734 + }, + { + "x": 8.22735001387898, + "y": 4.060566470670497, + "heading": -0.0017242628169884097, + "angularVelocity": 0.014403834167980214, + "velocityX": 0.7584914594958928, + "velocityY": 0.34007589017963685, + "timestamp": 1.877616860545439 + }, + { + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 1.4182707321197947e-24, + "angularVelocity": 0.021762486047347675, + "velocityX": 0.26748091724090983, + "velocityY": 0.32217804683230217, + "timestamp": 1.9568478276894048 + }, + { + "x": 8.23152301234386, + "y": 4.109892958457591, + "heading": 0.0022863134325125296, + "angularVelocity": 0.02921520350070526, + "velocityX": -0.21748380315840005, + "velocityY": 0.30412370948765427, + "timestamp": 2.0351054866667706 + }, + { + "x": 8.176551036095818, + "y": 4.132280075994518, + "heading": 0.005155847292928311, + "angularVelocity": 0.03666777025933954, + "velocityX": -0.7024485138757267, + "velocityY": 0.2860693487317698, + "timestamp": 2.1133631456441364 + }, + { + "x": 8.0836268579111, + "y": 4.153254299063441, + "heading": 0.008608587112944467, + "angularVelocity": 0.044120152137630285, + "velocityX": -1.1874132116780383, + "velocityY": 0.2680149565295516, + "timestamp": 2.191620804621502 + }, + { + "x": 7.952750479206832, + "y": 4.172815624183264, + "heading": 0.012644515987393022, + "angularVelocity": 0.05157231799657927, + "velocityX": -1.672377891371887, + "velocityY": 0.24996051984483794, + "timestamp": 2.269878463598868 + }, + { + "x": 7.783921902112923, + "y": 4.190964046074257, + "heading": 0.01726361439792771, + "angularVelocity": 0.05902423444420523, + "velocityX": -2.157342543849138, + "velocityY": 0.2319060156941693, + "timestamp": 2.348136122576234 + }, + { + "x": 7.577141130185825, + "y": 4.207699555855839, + "heading": 0.022465858708007803, + "angularVelocity": 0.06647584885697558, + "velocityX": -2.642307150880985, + "velocityY": 0.2138513980647186, + "timestamp": 2.4263937815535996 + }, + { + "x": 7.3324081705466435, + "y": 4.2230221357012825, + "heading": 0.028251214753927917, + "angularVelocity": 0.0739270267156012, + "velocityX": -3.127271666917177, + "velocityY": 0.19579655263996953, + "timestamp": 2.5046514405309654 + }, + { + "x": 7.049723044551423, + "y": 4.236931732641069, + "heading": 0.03461960291638085, + "angularVelocity": 0.08137718717467465, + "velocityX": -3.612235910059377, + "velocityY": 0.17774103035473854, + "timestamp": 2.5829090995083313 + }, + { + "x": 6.758414971904043, + "y": 4.226087741393719, + "heading": 0.03461960351163466, + "angularVelocity": 7.606332865983503e-9, + "velocityX": -3.722422526485686, + "velocityY": -0.13856779501270336, + "timestamp": 2.661166758485697 + }, + { + "x": 6.467106926605879, + "y": 4.215243015473276, + "heading": 0.03461960410686983, + "angularVelocity": 7.606094882154123e-9, + "velocityX": -3.7224221770091335, + "velocityY": -0.13857718288735063, + "timestamp": 2.739424417463063 + }, + { + "x": 6.175798881305754, + "y": 4.204398289605525, + "heading": 0.03461960470210498, + "angularVelocity": 7.606094432841078e-9, + "velocityX": -3.7224221770341983, + "velocityY": -0.13857718221405538, + "timestamp": 2.8176820764404287 + }, + { + "x": 5.884490683895451, + "y": 4.1935576504515195, + "heading": 0.03461960529734107, + "angularVelocity": 7.606106615779635e-9, + "velocityX": -3.7224241207439825, + "velocityY": -0.13852496095162967, + "timestamp": 2.8959397354177945 + }, + { + "x": 5.594009037113431, + "y": 4.2180198226312315, + "heading": 0.034619605911984125, + "angularVelocity": 7.854094508972923e-9, + "velocityX": -3.7118622071998786, + "velocityY": 0.31258502361778023, + "timestamp": 2.9741973943951603 + }, + { + "x": 5.309172831526297, + "y": 4.280038516789027, + "heading": 0.03461959995315513, + "angularVelocity": -7.614371636753198e-8, + "velocityX": -3.6397230547046773, + "velocityY": 0.7924936034150152, + "timestamp": 3.052455053372526 + }, + { + "x": 5.03935943827081, + "y": 4.37675512978584, + "heading": 0.01971114929844739, + "angularVelocity": -0.1905046847749395, + "velocityX": -3.447757021884906, + "velocityY": 1.235874089011348, + "timestamp": 3.130712712349892 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -1.4078898091571754e-24, + "angularVelocity": -0.25187501844577836, + "velocityX": -2.9876010500011123, + "velocityY": 1.3893109823794343, + "timestamp": 3.2089703713272577 + }, + { + "x": 4.62745788226169, + "y": 4.587679677136332, + "heading": -0.019778853338814184, + "angularVelocity": -0.28487106556518027, + "velocityX": -2.5651244910957125, + "velocityY": 1.47197181806725, + "timestamp": 3.2784012673699046 + }, + { + "x": 4.478940134439646, + "y": 4.685562428002014, + "heading": -0.039416750418752866, + "angularVelocity": -0.2828408993580651, + "velocityX": -2.139072895311886, + "velocityY": 1.4097866575934404, + "timestamp": 3.3478321634125514 + }, + { + "x": 4.358424662792702, + "y": 4.772986561558442, + "heading": -0.05736759927872339, + "angularVelocity": -0.2585426644781364, + "velocityX": -1.7357614335398783, + "velocityY": 1.2591531802027822, + "timestamp": 3.4172630594551983 + }, + { + "x": 4.264251963016928, + "y": 4.846292896298889, + "heading": -0.07266630938107123, + "angularVelocity": -0.22034441400483662, + "velocityX": -1.3563514968599637, + "velocityY": 1.0558172070171734, + "timestamp": 3.486693955497845 + }, + { + "x": 4.195046678979309, + "y": 4.903178017800908, + "heading": -0.08468297645986948, + "angularVelocity": -0.17307377210596744, + "velocityX": -0.9967505531703128, + "velocityY": 0.8193055936809087, + "timestamp": 3.556124851540492 + }, + { + "x": 4.149718350868284, + "y": 4.942098244876491, + "heading": -0.09298341563380762, + "angularVelocity": -0.11954964788067375, + "velocityX": -0.6528552948990165, + "velocityY": 0.5605606335784191, + "timestamp": 3.625555747583139 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.06149221603257356, + "velocityX": -0.32142062647019676, + "velocityY": 0.2860902346288004, + "timestamp": 3.6949866436257857 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -9.169895235461756e-26, + "velocityX": -3.1744204454242983e-26, + "velocityY": -4.8447793914370994e-26, + "timestamp": 3.7644175396684325 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePAEDF.traj b/src/main/deploy/choreo/AmpLanePAEDF.traj new file mode 100644 index 00000000..9be9694f --- /dev/null +++ b/src/main/deploy/choreo/AmpLanePAEDF.traj @@ -0,0 +1,1562 @@ +{ + "samples": [ + { + "x": 0.43297290802001953, + "y": 6.9807281494140625, + "heading": -4.820407208942378e-26, + "angularVelocity": -1.0568380673400748e-29, + "velocityX": 2.0900789241101526e-25, + "velocityY": 6.12105194243679e-25, + "timestamp": 0 + }, + { + "x": 0.4535853919747023, + "y": 6.976442037279776, + "heading": -9.657051327438745e-26, + "angularVelocity": -2.7866726754125085e-27, + "velocityX": 0.3537634154941742, + "velocityY": -0.07356074460262349, + "timestamp": 0.05826629620784589 + }, + { + "x": 0.49483104636031155, + "y": 6.967970532726724, + "heading": -1.449381860201486e-25, + "angularVelocity": -2.8078094367592775e-27, + "velocityX": 0.7078818643024581, + "velocityY": -0.1453928789781493, + "timestamp": 0.11653259241569178 + }, + { + "x": 0.5567354919847137, + "y": 6.955441991018911, + "heading": -1.9330585876590946e-25, + "angularVelocity": -2.807809436759289e-27, + "velocityX": 1.0624400322886223, + "velocityY": -0.21502210580061862, + "timestamp": 0.17479888862353765 + }, + { + "x": 0.6393312280567922, + "y": 6.939025561506136, + "heading": -2.416735315116705e-25, + "angularVelocity": -2.8078094367592704e-27, + "velocityX": 1.417555970563931, + "velocityY": -0.2817482932880241, + "timestamp": 0.23306518483138355 + }, + { + "x": 0.7426606798556364, + "y": 6.918954250670703, + "heading": -2.9004008719315308e-25, + "angularVelocity": -2.7886377311391825e-27, + "velocityX": 1.7734000361075017, + "velocityY": -0.3444754882622709, + "timestamp": 0.29133148103922946 + }, + { + "x": 0.8667811155319214, + "y": 6.895569324493408, + "heading": -3.4495163504680925e-25, + "angularVelocity": -1.1511759285858433e-25, + "velocityX": 2.130226970897995, + "velocityY": -0.40134567836399193, + "timestamp": 0.34959777724707536 + }, + { + "x": 1.01097419129112, + "y": 6.869549473556677, + "heading": 0.010039096018647366, + "angularVelocity": 0.17297490397522428, + "velocityX": 2.4844650740475953, + "velocityY": -0.44832534810612334, + "timestamp": 0.4076356531928559 + }, + { + "x": 1.175726572408581, + "y": 6.840802777300111, + "heading": 0.03011443547557583, + "angularVelocity": 0.34590065762715055, + "velocityX": 2.8387045258405714, + "velocityY": -0.4953092405280373, + "timestamp": 0.4656735291386364 + }, + { + "x": 1.3610383679335918, + "y": 6.809328854372862, + "heading": 0.06022199220724938, + "angularVelocity": 0.5187570399681801, + "velocityX": 3.1929458565666793, + "velocityY": -0.5422997036737339, + "timestamp": 0.5237114050844169 + }, + { + "x": 1.5460799084730097, + "y": 6.784713461455284, + "heading": 0.15060245758508448, + "angularVelocity": 1.5572669382709545, + "velocityX": 3.1882893287184593, + "velocityY": -0.4241263574251743, + "timestamp": 0.5817492810301974 + }, + { + "x": 1.7105613111663502, + "y": 6.762828401317669, + "heading": 0.23096913740746994, + "angularVelocity": 1.384728136802675, + "velocityX": 2.83403553305432, + "velocityY": -0.37708237562070457, + "timestamp": 0.6397871569759779 + }, + { + "x": 1.8544824093339345, + "y": 6.743675274341158, + "heading": 0.3013204880217799, + "angularVelocity": 1.2121627380029003, + "velocityX": 2.4797788654780635, + "velocityY": -0.33001081904520096, + "timestamp": 0.6978250329217583 + }, + { + "x": 1.977843185069378, + "y": 6.727255600600169, + "heading": 0.36164875379550304, + "angularVelocity": 1.0394637086664282, + "velocityX": 2.1255218893725236, + "velocityY": -0.2829130713937313, + "timestamp": 0.7558629088675388 + }, + { + "x": 2.0806437247378744, + "y": 6.713570715573333, + "heading": 0.41194198603949084, + "angularVelocity": 0.8665588018929588, + "velocityX": 1.7712664013502706, + "velocityY": -0.2357923132752318, + "timestamp": 0.8139007848133193 + }, + { + "x": 2.162884155033722, + "y": 6.702621710001363, + "heading": 0.45218713038285063, + "angularVelocity": 0.6934289666451124, + "velocityX": 1.4170130962869354, + "velocityY": -0.18865276155520178, + "timestamp": 0.8719386607590998 + }, + { + "x": 2.2245645766627997, + "y": 6.694409403888087, + "heading": 0.4823732249131576, + "angularVelocity": 0.5201102562489893, + "velocityX": 1.0627615263987253, + "velocityY": -0.1414990810647272, + "timestamp": 0.9299765367048802 + }, + { + "x": 2.265685008078604, + "y": 6.688934342765278, + "heading": 0.5024941236633388, + "angularVelocity": 0.34668565005684077, + "velocityX": 0.7085102744666172, + "velocityY": -0.09433600099222642, + "timestamp": 0.9880144126506607 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": 0.17327126075488852, + "velocityX": 0.354257244182047, + "velocityY": -0.047168141046465786, + "timestamp": 1.0460522885964412 + }, + { + "x": 2.286245346069336, + "y": 6.686196804046631, + "heading": 0.5125504196, + "angularVelocity": -1.480616560828957e-26, + "velocityX": -5.749006986314809e-23, + "velocityY": -3.283580548358229e-23, + "timestamp": 1.1040901645422216 + }, + { + "x": 2.3347330882331176, + "y": 6.713955255875249, + "heading": 0.5125504196, + "angularVelocity": -2.553583698003105e-17, + "velocityX": 0.5108360349814401, + "velocityY": 0.2924454065412527, + "timestamp": 1.1990085705343583 + }, + { + "x": 2.4317085702199166, + "y": 6.7694721581924355, + "heading": 0.5125504196, + "angularVelocity": -7.602638325412282e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645902, + "timestamp": 1.293926976526495 + }, + { + "x": 2.577171786567949, + "y": 6.852747507871406, + "heading": 0.5125504196, + "angularVelocity": -2.864728625251783e-17, + "velocityX": 1.5325079980808258, + "velocityY": 0.8773361584461279, + "timestamp": 1.3888453825186315 + }, + { + "x": 2.771122709968306, + "y": 6.963781289278251, + "heading": 0.5125504196, + "angularVelocity": -4.95735356219891e-19, + "velocityX": 2.0433436631502695, + "velocityY": 1.1697813532187173, + "timestamp": 1.4837637885107682 + }, + { + "x": 2.9165859263163383, + "y": 7.047056638957223, + "heading": 0.5125504196, + "angularVelocity": -2.1000816812429605e-17, + "velocityX": 1.532507998080826, + "velocityY": 0.8773361584461278, + "timestamp": 1.5786821945029048 + }, + { + "x": 3.0135614083031372, + "y": 7.102573541274409, + "heading": 0.5125504196, + "angularVelocity": 1.0493910343453442e-17, + "velocityX": 1.021672045302073, + "velocityY": 0.5848907989645902, + "timestamp": 1.6736006004950414 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": 3.728395973644304e-17, + "velocityX": 0.5108360349814401, + "velocityY": 0.2924454065412527, + "timestamp": 1.768519006487178 + }, + { + "x": 3.062049150466919, + "y": 7.130331993103027, + "heading": 0.5125504196, + "angularVelocity": -2.9402212487685217e-25, + "velocityX": 6.030156719342467e-23, + "velocityY": 3.0718511862526725e-23, + "timestamp": 1.8634374124793147 + }, + { + "x": 3.0865020118467, + "y": 7.124545249523675, + "heading": 0.48610131944841944, + "angularVelocity": -0.40878852104321894, + "velocityX": 0.3779353165675877, + "velocityY": -0.08943798979559645, + "timestamp": 1.9281385936281472 + }, + { + "x": 3.135458110441477, + "y": 7.112972562266447, + "heading": 0.4339971852577861, + "angularVelocity": -0.8053042195748789, + "velocityX": 0.7566492253389184, + "velocityY": -0.17886361657922661, + "timestamp": 1.9928397747769797 + }, + { + "x": 3.2089771746406726, + "y": 7.095606129050513, + "heading": 0.357225026215491, + "angularVelocity": -1.1865650314125755, + "velocityX": 1.136286276290371, + "velocityY": -0.2684098328280398, + "timestamp": 2.057540955925812 + }, + { + "x": 3.307133671873185, + "y": 7.072412045867193, + "heading": 0.2571531716690748, + "angularVelocity": -1.5466773986122515, + "velocityX": 1.5170742711284095, + "velocityY": -0.3584800581919169, + "timestamp": 2.1222421370746445 + }, + { + "x": 3.4300337444037163, + "y": 7.043313631479606, + "heading": 0.13605141213735358, + "angularVelocity": -1.8717086362480815, + "velocityX": 1.8995027656114087, + "velocityY": -0.44973544332447146, + "timestamp": 2.1869433182234768 + }, + { + "x": 3.5778386039995205, + "y": 7.008182862001031, + "heading": -0.0018831098932495121, + "angularVelocity": -2.1318702314461877, + "velocityX": 2.2844228957089365, + "velocityY": -0.5429695231956211, + "timestamp": 2.251644499372309 + }, + { + "x": 3.75075226330582, + "y": 6.966825747454083, + "heading": -0.14853260580564892, + "angularVelocity": -2.266565977753341, + "velocityX": 2.672496177597522, + "velocityY": -0.6392018478273882, + "timestamp": 2.3163456805211413 + }, + { + "x": 3.9486334270649626, + "y": 6.918918967044097, + "heading": -0.28405555900160556, + "angularVelocity": -2.094597823248583, + "velocityX": 3.0583856468393638, + "velocityY": -0.7404313114436918, + "timestamp": 2.3810468616699736 + }, + { + "x": 4.166070195160817, + "y": 6.865183550075389, + "heading": -0.3449865942152846, + "angularVelocity": -0.9417298746605423, + "velocityX": 3.360630582549694, + "velocityY": -0.8305167852361092, + "timestamp": 2.445748042818806 + }, + { + "x": 4.4009794486877345, + "y": 6.8112911031395695, + "heading": -0.3449866586118039, + "angularVelocity": -9.952912480962177e-7, + "velocityX": 3.6306795232463367, + "velocityY": -0.8329437883344079, + "timestamp": 2.510449223967638 + }, + { + "x": 4.635888809481111, + "y": 6.757399123762019, + "heading": -0.34498672300630057, + "angularVelocity": -9.952599862485997e-7, + "velocityX": 3.630681181121147, + "velocityY": -0.8329365619088571, + "timestamp": 2.5751504051164704 + }, + { + "x": 4.87079817027739, + "y": 6.703507144397122, + "heading": -0.3449867874007968, + "angularVelocity": -9.952599802438338e-7, + "velocityX": 3.630681181166014, + "velocityY": -0.8329365617132907, + "timestamp": 2.6398515862653027 + }, + { + "x": 5.105707531073669, + "y": 6.649615165032224, + "heading": -0.3449868517952927, + "angularVelocity": -9.952599738581064e-7, + "velocityX": 3.6306811811660147, + "velocityY": -0.8329365617132863, + "timestamp": 2.704552767414135 + }, + { + "x": 5.340616891869949, + "y": 6.595723185667327, + "heading": -0.3449869161897882, + "angularVelocity": -9.952599684354273e-7, + "velocityX": 3.630681181166015, + "velocityY": -0.8329365617132873, + "timestamp": 2.7692539485629672 + }, + { + "x": 5.575526252666228, + "y": 6.54183120630243, + "heading": -0.34498698058428334, + "angularVelocity": -9.952599624061458e-7, + "velocityX": 3.6306811811660156, + "velocityY": -0.8329365617132882, + "timestamp": 2.8339551297117995 + }, + { + "x": 5.8104356134625075, + "y": 6.4879392269375336, + "heading": -0.344987044978778, + "angularVelocity": -9.952599561054567e-7, + "velocityX": 3.6306811811660156, + "velocityY": -0.8329365617132889, + "timestamp": 2.898656310860632 + }, + { + "x": 6.0453449742587875, + "y": 6.434047247572637, + "heading": -0.3449871093732723, + "angularVelocity": -9.952599496144382e-7, + "velocityX": 3.6306811811660165, + "velocityY": -0.8329365617132901, + "timestamp": 2.963357492009464 + }, + { + "x": 6.280254335055067, + "y": 6.38015526820774, + "heading": -0.3449871737677662, + "angularVelocity": -9.952599439036616e-7, + "velocityX": 3.630681181166016, + "velocityY": -0.832936561713291, + "timestamp": 3.0280586731582964 + }, + { + "x": 6.515163695851346, + "y": 6.326263288842841, + "heading": -0.3449872381622598, + "angularVelocity": -9.952599382292206e-7, + "velocityX": 3.6306811811660133, + "velocityY": -0.8329365617133051, + "timestamp": 3.0927598543071286 + }, + { + "x": 6.750073056640409, + "y": 6.2723713094464895, + "heading": -0.34498730255675286, + "angularVelocity": -9.952599312963826e-7, + "velocityX": 3.6306811810544866, + "velocityY": -0.832936562199441, + "timestamp": 3.157461035455961 + }, + { + "x": 6.984982150775984, + "y": 6.2184781677494305, + "heading": -0.3449873669513136, + "angularVelocity": -9.95260976649588e-7, + "velocityX": 3.6306770597466, + "velocityY": -0.8329545263337457, + "timestamp": 3.222162216604793 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -0.00000172520007220815, + "velocityX": 3.5518244907979937, + "velocityY": -1.1225730284587905, + "timestamp": 3.2868633977536255 + }, + { + "x": 7.409091443638604, + "y": 6.073965873280571, + "heading": -0.3572291452032454, + "angularVelocity": -0.20035361948729366, + "velocityX": 3.1800506242888202, + "velocityY": -1.1764343450588084, + "timestamp": 3.347963699613786 + }, + { + "x": 7.58146433123023, + "y": 6.0094940876522065, + "heading": -0.36768136673125634, + "angularVelocity": -0.17106661030796205, + "velocityX": 2.8211462520452746, + "velocityY": -1.0551794944633763, + "timestamp": 3.4090640014739466 + }, + { + "x": 7.73220907640328, + "y": 5.953248314526567, + "heading": -0.37521177845828957, + "angularVelocity": -0.1232467188831242, + "velocityX": 2.4671685831938674, + "velocityY": -0.920548203744869, + "timestamp": 3.470164303334107 + }, + { + "x": 7.861444501366057, + "y": 5.905524901339061, + "heading": -0.37940510209885836, + "angularVelocity": -0.06863016241991673, + "velocityX": 2.1151356217282813, + "velocityY": -0.7810667334627726, + "timestamp": 3.5312646051942678 + }, + { + "x": 7.969234018825332, + "y": 5.866476846712542, + "heading": -0.38004587758642694, + "angularVelocity": -0.010487272043845954, + "velocityX": 1.7641405063099576, + "velocityY": -0.6390812064380308, + "timestamp": 3.5923649070544283 + }, + { + "x": 8.055617039018077, + "y": 5.8361975486879665, + "heading": -0.37700219526471646, + "angularVelocity": 0.04981452184436869, + "velocityX": 1.413790399766751, + "velocityY": -0.4955670774569341, + "timestamp": 3.653465208914589 + }, + { + "x": 8.12062042791295, + "y": 5.814749961981242, + "heading": -0.37018491999207215, + "angularVelocity": 0.11157514881427125, + "velocityX": 1.0638799959392298, + "velocityY": -0.3510225981503441, + "timestamp": 3.7145655107747495 + }, + { + "x": 8.164263678264032, + "y": 5.802179408572721, + "heading": -0.3595296539883727, + "angularVelocity": 0.17438974406519217, + "velocityX": 0.7142886208805774, + "velocityY": -0.20573635523587247, + "timestamp": 3.77566581263491 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.23800496841831012, + "velocityX": 0.3649393788537573, + "velocityY": -0.05989038131588724, + "timestamp": 3.8367661144950707 + }, + { + "x": 8.18566399540182, + "y": 5.805056139735724, + "heading": -0.32461060520225765, + "angularVelocity": 0.30765327861825625, + "velocityX": -0.013551942707795956, + "velocityY": 0.09868234683560065, + "timestamp": 3.9029993542741352 + }, + { + "x": 8.159694473473342, + "y": 5.822092590454781, + "heading": -0.2998178925011234, + "angularVelocity": 0.3743243239170494, + "velocityX": -0.3920919770058792, + "velocityY": 0.25721904554096775, + "timestamp": 3.9692325940532 + }, + { + "x": 8.108649030585239, + "y": 5.849626323109156, + "heading": -0.2708591800534793, + "angularVelocity": 0.4372232514103506, + "velocityX": -0.7706922243027172, + "velocityY": 0.41570867960287095, + "timestamp": 4.035465833832265 + }, + { + "x": 8.03252261804608, + "y": 5.887653107737229, + "heading": -0.2380618973255709, + "angularVelocity": 0.4951785966881702, + "velocityX": -1.1493686975466637, + "velocityY": 0.5741344490307231, + "timestamp": 4.1016990736113295 + }, + { + "x": 7.9313086642803885, + "y": 5.936166919703684, + "heading": -0.20187479302243566, + "angularVelocity": 0.5463586625664867, + "velocityX": -1.5281443894834816, + "velocityY": 0.7324692575553122, + "timestamp": 4.167932313390394 + }, + { + "x": 7.804998346649701, + "y": 5.9951585907413225, + "heading": -0.16295187123346636, + "angularVelocity": 0.5876644705710469, + "velocityX": -1.9070532870205719, + "velocityY": 0.8906656421219618, + "timestamp": 4.234165553169459 + }, + { + "x": 7.653579505512198, + "y": 6.0646127178391085, + "heading": -0.12233575906363518, + "angularVelocity": 0.6132285285351425, + "velocityX": -2.286145772765958, + "velocityY": 1.048629469575472, + "timestamp": 4.300398792948523 + }, + { + "x": 7.4770359425587705, + "y": 6.144498725543435, + "heading": -0.08194804569131871, + "angularVelocity": 0.6097801271240603, + "velocityX": -2.6654828231614, + "velocityY": 1.206131663962129, + "timestamp": 4.366632032727588 + }, + { + "x": 7.27536197665765, + "y": 6.234730628199731, + "heading": -0.046490249768474004, + "angularVelocity": 0.5353474485186261, + "velocityX": -3.044905648188839, + "velocityY": 1.362335633245254, + "timestamp": 4.432865272506652 + }, + { + "x": 7.049560259358204, + "y": 6.334147758763648, + "heading": -0.04649017070950518, + "angularVelocity": 0.0000011936448991960951, + "velocityX": -3.4091902804793617, + "velocityY": 1.5010156666885635, + "timestamp": 4.499098512285717 + }, + { + "x": 6.822106687595601, + "y": 6.429725315002885, + "heading": -0.04649016173832054, + "angularVelocity": 1.354483741042957e-7, + "velocityX": -3.4341302421763333, + "velocityY": 1.4430451621882883, + "timestamp": 4.565331752064782 + }, + { + "x": 6.586112566537963, + "y": 6.501676487205841, + "heading": -0.04649015261662469, + "angularVelocity": 1.3772081626072917e-7, + "velocityX": -3.563076815279552, + "velocityY": 1.086330254158866, + "timestamp": 4.631564991843846 + }, + { + "x": 6.343631547831337, + "y": 6.547208357601844, + "heading": -0.046490142857452815, + "angularVelocity": 1.4734553082512417e-7, + "velocityX": -3.6610170288434327, + "velocityY": 0.6874474289327321, + "timestamp": 4.697798231622911 + }, + { + "x": 6.097611695820765, + "y": 6.565767095818739, + "heading": -0.046490131800601416, + "angularVelocity": 1.6693810300562863e-7, + "velocityX": -3.7144468975279485, + "velocityY": 0.28020278456559833, + "timestamp": 4.764031471401975 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.04649011830549673, + "angularVelocity": 2.0375123930314497e-7, + "velocityX": -3.722715642994527, + "velocityY": -0.13044955895548105, + "timestamp": 4.83026471118104 + }, + { + "x": 5.59400062961639, + "y": 6.517937013895863, + "heading": -0.046490107217199095, + "angularVelocity": 1.588527062833638e-7, + "velocityX": -3.682446546399627, + "velocityY": -0.5614419262393832, + "timestamp": 4.900067095317195 + }, + { + "x": 5.343239064324899, + "y": 6.449191978638034, + "heading": -0.046490097789975614, + "angularVelocity": 1.3505589536533548e-7, + "velocityX": -3.5924498624912715, + "velocityY": -0.984852252664273, + "timestamp": 4.96986947945335 + }, + { + "x": 5.10214573961629, + "y": 6.351820274741271, + "heading": -0.04649007357112031, + "angularVelocity": 3.469631532168612e-7, + "velocityX": -3.453941118090443, + "velocityY": -1.3949624371974614, + "timestamp": 5.039671863589505 + }, + { + "x": 4.887305900136772, + "y": 6.241894655501948, + "heading": -0.013339477589168081, + "angularVelocity": 0.4749206834724951, + "velocityX": -3.0778295345966584, + "velocityY": -1.574811814807135, + "timestamp": 5.10947424772566 + }, + { + "x": 4.700625263351654, + "y": 6.142924243380359, + "heading": 0.02019201613120645, + "angularVelocity": 0.4803774847427688, + "velocityX": -2.6744163411522406, + "velocityY": -1.417865784190692, + "timestamp": 5.179276631861815 + }, + { + "x": 4.541203570947359, + "y": 6.05696873798045, + "heading": 0.05079450877042205, + "angularVelocity": 0.4384161517968095, + "velocityX": -2.283900390756435, + "velocityY": -1.2314121711408288, + "timestamp": 5.24907901599797 + }, + { + "x": 4.408671715563658, + "y": 5.984755616609754, + "heading": 0.07726906366461818, + "angularVelocity": 0.3792786624960486, + "velocityX": -1.8986723308073217, + "velocityY": -1.034536603074175, + "timestamp": 5.318881400134125 + }, + { + "x": 4.302830646527999, + "y": 5.92665565579208, + "heading": 0.09900038404891696, + "angularVelocity": 0.3113263343829381, + "velocityX": -1.5162959022890692, + "velocityY": -0.8323492318592651, + "timestamp": 5.38868378427028 + }, + { + "x": 4.22355609558814, + "y": 5.882893451015234, + "heading": 0.11561440292356019, + "angularVelocity": 0.23801506324248706, + "velocityX": -1.1356997604154628, + "velocityY": -0.6269442701482041, + "timestamp": 5.458486168406435 + }, + { + "x": 4.170763147512419, + "y": 5.853619589907405, + "heading": 0.1268589116332185, + "angularVelocity": 0.1610906109986881, + "velocityX": -0.756320127586832, + "velocityY": -0.4193819662481466, + "timestamp": 5.52828855254259 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 0.08154784945777828, + "velocityX": -0.37782436284418364, + "velocityY": -0.21027274576950902, + "timestamp": 5.598090936678745 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.1000727380078476e-26, + "velocityX": 7.869680140840765e-27, + "velocityY": 7.286496207304452e-27, + "timestamp": 5.6678933208149 + }, + { + "x": 4.195350058507561, + "y": 5.858786392293566, + "heading": 0.13540679339101, + "angularVelocity": 0.03040787160499198, + "velocityX": 0.5426383042255808, + "velocityY": 0.2113090624413005, + "timestamp": 5.7618047751106225 + }, + { + "x": 4.297269963634785, + "y": 5.898475069782893, + "heading": 0.14111775296085857, + "angularVelocity": 0.06081217262236255, + "velocityX": 1.085276613929165, + "velocityY": 0.42261806919047107, + "timestamp": 5.855716229406345 + }, + { + "x": 4.450149823252348, + "y": 5.958008075795516, + "heading": 0.14968325491595807, + "angularVelocity": 0.09120827719403884, + "velocityX": 1.6279149414101537, + "velocityY": 0.6339269949452273, + "timestamp": 5.949627683702067 + }, + { + "x": 4.653989639578877, + "y": 6.037385400048414, + "heading": 0.16110213512106505, + "angularVelocity": 0.12159198567141313, + "velocityX": 2.1705532925158373, + "velocityY": 0.8452358112030016, + "timestamp": 6.043539137997789 + }, + { + "x": 4.908789413625767, + "y": 6.136607028819001, + "heading": 0.17537288795521153, + "angularVelocity": 0.15195966180236836, + "velocityX": 2.7131916543911667, + "velocityY": 1.0565444813381684, + "timestamp": 6.137450592293511 + }, + { + "x": 5.214549136636689, + "y": 6.255672941521673, + "heading": 0.19249371968896806, + "angularVelocity": 0.1823082377134106, + "velocityX": 3.2558299230262406, + "velocityY": 1.2678529322710714, + "timestamp": 6.231362046589234 + }, + { + "x": 5.540526297381237, + "y": 6.382610310333338, + "heading": 0.1924937215917483, + "angularVelocity": 2.0261428816305725e-8, + "velocityX": 3.4711118381583637, + "velocityY": 1.3516707814144493, + "timestamp": 6.325273500884956 + }, + { + "x": 5.866503458135056, + "y": 6.50954767912126, + "heading": 0.19249372349451602, + "angularVelocity": 2.0261295290604937e-8, + "velocityX": 3.471111838257083, + "velocityY": 1.351670781161622, + "timestamp": 6.419184955180678 + }, + { + "x": 6.192480618888875, + "y": 6.636485047909182, + "heading": 0.19249372539728374, + "angularVelocity": 2.0261295568537564e-8, + "velocityX": 3.471111838257084, + "velocityY": 1.3516707811616195, + "timestamp": 6.5130964094764 + }, + { + "x": 6.518457779642694, + "y": 6.763422416697105, + "heading": 0.19249372730005146, + "angularVelocity": 2.0261295333491658e-8, + "velocityX": 3.4711118382570834, + "velocityY": 1.35167078116162, + "timestamp": 6.607007863772123 + }, + { + "x": 6.844434940394272, + "y": 6.890359785490778, + "heading": 0.19249372920281918, + "angularVelocity": 2.0261295628050687e-8, + "velocityX": 3.471111838233232, + "velocityY": 1.3516707812228705, + "timestamp": 6.700919318067845 + }, + { + "x": 7.170411838536006, + "y": 7.017297824914972, + "heading": 0.1924937316314852, + "angularVelocity": 2.586123310059887e-8, + "velocityX": 3.4711090418773667, + "velocityY": 1.351677922316834, + "timestamp": 6.794830772363567 + }, + { + "x": 7.445544445617428, + "y": 7.124436352045788, + "heading": 0.20746316204367837, + "angularVelocity": 0.15939941005551134, + "velocityX": 2.929702336575951, + "velocityY": 1.1408462144931066, + "timestamp": 6.888742226659289 + }, + { + "x": 7.669715374136242, + "y": 7.211730123643104, + "heading": 0.22019672303892965, + "angularVelocity": 0.13559113838397205, + "velocityX": 2.38704565060736, + "velocityY": 0.9295327417934781, + "timestamp": 6.9826536809550115 + }, + { + "x": 7.842925396345479, + "y": 7.279179365267254, + "heading": 0.23039470264394565, + "angularVelocity": 0.10859143521410211, + "velocityX": 1.8443971878425902, + "velocityY": 0.7182216709343664, + "timestamp": 7.076565135250734 + }, + { + "x": 7.96517479602962, + "y": 7.326784165564593, + "heading": 0.23795514608622043, + "angularVelocity": 0.08050608415100667, + "velocityX": 1.3017517469081525, + "velocityY": 0.506911544010745, + "timestamp": 7.170476589546456 + }, + { + "x": 8.0364637201711, + "y": 7.354544571992242, + "heading": 0.24282665608297047, + "angularVelocity": 0.05187343794517228, + "velocityX": 0.7591078710909263, + "velocityY": 0.2956019224261278, + "timestamp": 7.264388043842178 + }, + { + "x": 8.056792259216309, + "y": 7.362460613250732, + "heading": 0.244978130412332, + "angularVelocity": 0.022909605068905204, + "velocityX": 0.21646495837660984, + "velocityY": 0.0842926064542015, + "timestamp": 7.3582994981379 + }, + { + "x": 8.02239522092899, + "y": 7.349066085390599, + "heading": 0.24423351408772678, + "angularVelocity": -0.00758829012549376, + "velocityX": -0.3505358361841942, + "velocityY": -0.13650192741958841, + "timestamp": 7.4564265217876144 + }, + { + "x": 7.932360084343089, + "y": 7.314005643971181, + "heading": 0.24049622236652166, + "angularVelocity": -0.03808626392813261, + "velocityX": -0.9175366095613122, + "velocityY": -0.35729649300863325, + "timestamp": 7.5545535454373285 + }, + { + "x": 7.786686853677988, + "y": 7.257279283570004, + "heading": 0.23376594617518753, + "angularVelocity": -0.06858738745974144, + "velocityX": -1.4845373399392365, + "velocityY": -0.578091113857428, + "timestamp": 7.6526805690870425 + }, + { + "x": 7.5853755359232595, + "y": 7.178886996735875, + "heading": 0.2240420388382404, + "angularVelocity": -0.09909510117884326, + "velocityX": -2.0515379990873206, + "velocityY": -0.7988858106403784, + "timestamp": 7.750807592736757 + }, + { + "x": 7.328426142666821, + "y": 7.078828774757454, + "heading": 0.21132346369891478, + "angularVelocity": -0.1296133793350078, + "velocityX": -2.618538540144425, + "velocityY": -1.0196805961994813, + "timestamp": 7.848934616386471 + }, + { + "x": 7.01583869900668, + "y": 6.957104611217254, + "heading": 0.19560873409309495, + "angularVelocity": -0.1601468078958253, + "velocityX": -3.1855388254309114, + "velocityY": -1.2404754471583816, + "timestamp": 7.947061640036185 + }, + { + "x": 6.675228781735299, + "y": 6.824469292432169, + "heading": 0.19560873219026645, + "angularVelocity": -1.939148261699155e-8, + "velocityX": -3.471112284901898, + "velocityY": -1.3516696405524002, + "timestamp": 8.045188663685899 + }, + { + "x": 6.334618864455473, + "y": 6.691833973668682, + "heading": 0.19560873028745154, + "angularVelocity": -1.9391344572203613e-8, + "velocityX": -3.4711122849879685, + "velocityY": -1.3516696403322965, + "timestamp": 8.143315687335612 + }, + { + "x": 5.994008947175647, + "y": 6.559198654905194, + "heading": 0.19560872838463667, + "angularVelocity": -1.9391344282542406e-8, + "velocityX": -3.471112284987969, + "velocityY": -1.3516696403322948, + "timestamp": 8.241442710985325 + }, + { + "x": 5.65339902989582, + "y": 6.426563336141707, + "heading": 0.19560872648182165, + "angularVelocity": -1.9391345115512158e-8, + "velocityX": -3.4711122849879685, + "velocityY": -1.3516696403322956, + "timestamp": 8.339569734635038 + }, + { + "x": 5.312789112621013, + "y": 6.293928017365348, + "heading": 0.195608724578999, + "angularVelocity": -1.939142318059707e-8, + "velocityX": -3.4711122849368206, + "velocityY": -1.3516696404634736, + "timestamp": 8.437696758284751 + }, + { + "x": 4.978960833752885, + "y": 6.1639320605180385, + "heading": 0.17759686591379087, + "angularVelocity": -0.18355655756466713, + "velocityX": -3.402001471682276, + "velocityY": -1.324772239208634, + "timestamp": 8.535823781934464 + }, + { + "x": 4.700770593105711, + "y": 6.055602072933751, + "heading": 0.16258358561165714, + "angularVelocity": -0.15299842738252176, + "velocityX": -2.8350013105486314, + "velocityY": -1.1039771059498866, + "timestamp": 8.633950805584178 + }, + { + "x": 4.478218399750123, + "y": 5.968938070584376, + "heading": 0.1505712213556835, + "angularVelocity": -0.1224164741697909, + "velocityX": -2.2680010569772926, + "velocityY": -0.8831818099236476, + "timestamp": 8.73207782923389 + }, + { + "x": 4.311304254190931, + "y": 5.9039400626948755, + "heading": 0.1415612155720731, + "angularVelocity": -0.09181982137533895, + "velocityX": -1.701000798261533, + "velocityY": -0.6623864198869991, + "timestamp": 8.830204852883604 + }, + { + "x": 4.2000281561602835, + "y": 5.860608055094718, + "heading": 0.13555442251274583, + "angularVelocity": -0.06121446300837455, + "velocityX": -1.1340005422754125, + "velocityY": -0.4415909704429648, + "timestamp": 8.928331876533317 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": -0.030606008971789937, + "velocityX": -0.5670002807557282, + "velocityY": -0.2207954888983711, + "timestamp": 9.02645890018303 + }, + { + "x": 4.144390106201172, + "y": 5.838942050933838, + "heading": 0.13255114594654763, + "angularVelocity": 1.2638912367197548e-25, + "velocityX": -6.105559884328861e-26, + "velocityY": -2.4380402560227299e-26, + "timestamp": 9.124585923832743 + }, + { + "x": 4.149229218776726, + "y": 5.815100726386144, + "heading": 0.12918022174703217, + "angularVelocity": -0.05380546988265665, + "velocityX": 0.07724016042252432, + "velocityY": -0.3805465783235216, + "timestamp": 9.187236137776921 + }, + { + "x": 4.159717289478365, + "y": 5.7675956417955225, + "heading": 0.12259706469587767, + "angularVelocity": -0.10507796600695042, + "velocityX": 0.16740678189838681, + "velocityY": -0.7582589364012235, + "timestamp": 9.249886351721099 + }, + { + "x": 4.176867312970838, + "y": 5.696690246498273, + "heading": 0.11300853276979594, + "angularVelocity": -0.15304867010709985, + "velocityX": 0.2737424569332353, + "velocityY": -1.1317662116912757, + "timestamp": 9.312536565665276 + }, + { + "x": 4.201974040585745, + "y": 5.602790592825652, + "heading": 0.10069220099175857, + "angularVelocity": -0.19658882233046115, + "velocityX": 0.40074448328743983, + "velocityY": -1.498792226891454, + "timestamp": 9.375186779609454 + }, + { + "x": 4.2367303910144685, + "y": 5.486551757326023, + "heading": 0.08603276386217494, + "angularVelocity": -0.2339886204162901, + "velocityX": 0.5547682639949261, + "velocityY": -1.8553621477366333, + "timestamp": 9.437836993553631 + }, + { + "x": 4.283393708275681, + "y": 5.349087774330418, + "heading": 0.06958159023755035, + "angularVelocity": -0.2625876687234105, + "velocityX": 0.7448229514872903, + "velocityY": -2.1941502565033004, + "timestamp": 9.500487207497809 + }, + { + "x": 4.3449830971903305, + "y": 5.19239819137041, + "heading": 0.05215018239550137, + "angularVelocity": -0.2782338119001568, + "velocityX": 0.9830674955001105, + "velocityY": -2.5010223125434243, + "timestamp": 9.563137421441986 + }, + { + "x": 4.425324635342363, + "y": 5.020190178284826, + "heading": 0.03492497946542863, + "angularVelocity": -0.2749424438585412, + "velocityX": 1.2823825026937377, + "velocityY": -2.7487218677181704, + "timestamp": 9.625787635386164 + }, + { + "x": 4.528280141254807, + "y": 4.838988810812342, + "heading": 0.019478906624101934, + "angularVelocity": -0.24654461443801812, + "velocityX": 1.6433384569792095, + "velocityY": -2.8922705297373237, + "timestamp": 9.688437849330342 + }, + { + "x": 4.6555610459377, + "y": 4.65789803685685, + "heading": 0.007446897298019998, + "angularVelocity": -0.1920505704386356, + "velocityX": 2.0316116525364616, + "velocityY": -2.8905052761167873, + "timestamp": 9.75108806327452 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -3.9652014929192097e-25, + "angularVelocity": -0.1188646746626479, + "velocityX": 2.3941774298704206, + "velocityY": -2.7520844885235216, + "timestamp": 9.813738277218697 + }, + { + "x": 5.026556371976755, + "y": 4.290478604211026, + "heading": -0.0011711585949671858, + "angularVelocity": -0.014781576411141743, + "velocityX": 2.7893083450033753, + "velocityY": -2.4611683748987776, + "timestamp": 9.892969244362662 + }, + { + "x": 5.271648004487175, + "y": 4.126055401914557, + "heading": -0.0011711658442509346, + "angularVelocity": -9.149558575332566e-8, + "velocityX": 3.0933818094770933, + "velocityY": -2.075239117019822, + "timestamp": 9.972200211506628 + }, + { + "x": 5.536248278847656, + "y": 3.995320554415454, + "heading": -0.0011711670074400294, + "angularVelocity": -1.4680990736292476e-8, + "velocityX": 3.3396067711718374, + "velocityY": -1.6500473515810268, + "timestamp": 10.051431178650594 + }, + { + "x": 5.8157535418727955, + "y": 3.900548632610079, + "heading": -0.001171167882288204, + "angularVelocity": -1.1041745495501586e-8, + "velocityX": 3.5277275174146094, + "velocityY": -1.19614748149131, + "timestamp": 10.13066214579456 + }, + { + "x": 6.105300821187418, + "y": 3.843388468655785, + "heading": -0.001171168612334034, + "angularVelocity": -9.214147655131007e-9, + "velocityX": 3.654471095733389, + "velocityY": -0.7214371604278409, + "timestamp": 10.209893112938525 + }, + { + "x": 6.399852439542981, + "y": 3.8248343191469996, + "heading": -0.0011711692753000216, + "angularVelocity": -8.367510982006514e-9, + "velocityX": 3.717632498671272, + "velocityY": -0.23417800107213171, + "timestamp": 10.289124080082491 + }, + { + "x": 6.694725372224158, + "y": 3.837278843369643, + "heading": -0.0011711699217371897, + "angularVelocity": -8.158895331165977e-9, + "velocityX": 3.721687912068311, + "velocityY": 0.15706641823558024, + "timestamp": 10.368355047226457 + }, + { + "x": 6.989598240261493, + "y": 3.849724899233505, + "heading": -0.001171170568174097, + "angularVelocity": -8.158892044748454e-9, + "velocityX": 3.721687096177193, + "velocityY": 0.15708574958131358, + "timestamp": 10.447586014370422 + }, + { + "x": 7.2831382115989305, + "y": 3.8803709956962793, + "heading": -0.0011711735014437953, + "angularVelocity": -3.702175808154048e-8, + "velocityX": 3.7048641701427742, + "velocityY": 0.38679442606183717, + "timestamp": 10.526816981514388 + }, + { + "x": 7.537795284833571, + "y": 3.9128988141490195, + "heading": -0.0022060584163901163, + "angularVelocity": -0.013061621639249952, + "velocityX": 3.2141103714147587, + "velocityY": 0.4105442559300826, + "timestamp": 10.606047948658354 + }, + { + "x": 7.7535280379166585, + "y": 3.9448039763833895, + "heading": -0.0030479668047439287, + "angularVelocity": -0.010626001659477862, + "velocityX": 2.7228337714355204, + "velocityY": 0.40268550775604034, + "timestamp": 10.68527891580232 + }, + { + "x": 7.9303456416380165, + "y": 3.9756628799518183, + "heading": -0.0034891865807647085, + "angularVelocity": -0.005568779379141843, + "velocityX": 2.2316729189998985, + "velocityY": 0.38948033427835, + "timestamp": 10.764509882946285 + }, + { + "x": 8.06825324660345, + "y": 4.0053006852761595, + "heading": -0.003443969014778176, + "angularVelocity": 0.000570705717934386, + "velocityX": 1.7405770740479534, + "velocityY": 0.374068453190637, + "timestamp": 10.843740850090251 + }, + { + "x": 8.167254001972681, + "y": 4.03362192898922, + "heading": -0.002865492528698777, + "angularVelocity": 0.007301141295275209, + "velocityX": 1.2495209756728547, + "velocityY": 0.3574516976625985, + "timestamp": 10.922971817234217 + }, + { + "x": 8.22735001387898, + "y": 4.060566470670497, + "heading": -0.0017242628169884097, + "angularVelocity": 0.014403834167980214, + "velocityX": 0.7584914594958928, + "velocityY": 0.34007589017963685, + "timestamp": 11.002202784378182 + }, + { + "x": 8.248542785644531, + "y": 4.086092948913574, + "heading": 1.4182707321197947e-24, + "angularVelocity": 0.021762486047347675, + "velocityX": 0.26748091724090983, + "velocityY": 0.32217804683230217, + "timestamp": 11.081433751522148 + }, + { + "x": 8.23152301234386, + "y": 4.109892958457591, + "heading": 0.0022863134325125296, + "angularVelocity": 0.02921520350070526, + "velocityX": -0.21748380315840005, + "velocityY": 0.30412370948765427, + "timestamp": 11.159691410499514 + }, + { + "x": 8.176551036095818, + "y": 4.132280075994518, + "heading": 0.005155847292928311, + "angularVelocity": 0.03666777025933954, + "velocityX": -0.7024485138757267, + "velocityY": 0.2860693487317698, + "timestamp": 11.23794906947688 + }, + { + "x": 8.0836268579111, + "y": 4.153254299063441, + "heading": 0.008608587112944467, + "angularVelocity": 0.044120152137630285, + "velocityX": -1.1874132116780383, + "velocityY": 0.2680149565295516, + "timestamp": 11.316206728454246 + }, + { + "x": 7.952750479206832, + "y": 4.172815624183264, + "heading": 0.012644515987393022, + "angularVelocity": 0.05157231799657927, + "velocityX": -1.672377891371887, + "velocityY": 0.24996051984483794, + "timestamp": 11.394464387431611 + }, + { + "x": 7.783921902112923, + "y": 4.190964046074257, + "heading": 0.01726361439792771, + "angularVelocity": 0.05902423444420523, + "velocityX": -2.157342543849138, + "velocityY": 0.2319060156941693, + "timestamp": 11.472722046408977 + }, + { + "x": 7.577141130185825, + "y": 4.207699555855839, + "heading": 0.022465858708007803, + "angularVelocity": 0.06647584885697558, + "velocityX": -2.642307150880985, + "velocityY": 0.2138513980647186, + "timestamp": 11.550979705386343 + }, + { + "x": 7.3324081705466435, + "y": 4.2230221357012825, + "heading": 0.028251214753927917, + "angularVelocity": 0.0739270267156012, + "velocityX": -3.127271666917177, + "velocityY": 0.19579655263996953, + "timestamp": 11.629237364363709 + }, + { + "x": 7.049723044551423, + "y": 4.236931732641069, + "heading": 0.03461960291638085, + "angularVelocity": 0.08137718717467465, + "velocityX": -3.612235910059377, + "velocityY": 0.17774103035473854, + "timestamp": 11.707495023341075 + }, + { + "x": 6.758414971904043, + "y": 4.226087741393719, + "heading": 0.03461960351163466, + "angularVelocity": 7.606332865983503e-9, + "velocityX": -3.722422526485686, + "velocityY": -0.13856779501270336, + "timestamp": 11.78575268231844 + }, + { + "x": 6.467106926605879, + "y": 4.215243015473276, + "heading": 0.03461960410686983, + "angularVelocity": 7.606094882154123e-9, + "velocityX": -3.7224221770091335, + "velocityY": -0.13857718288735063, + "timestamp": 11.864010341295806 + }, + { + "x": 6.175798881305754, + "y": 4.204398289605525, + "heading": 0.03461960470210498, + "angularVelocity": 7.606094432841078e-9, + "velocityX": -3.7224221770341983, + "velocityY": -0.13857718221405538, + "timestamp": 11.942268000273172 + }, + { + "x": 5.884490683895451, + "y": 4.1935576504515195, + "heading": 0.03461960529734107, + "angularVelocity": 7.606106615779635e-9, + "velocityX": -3.7224241207439825, + "velocityY": -0.13852496095162967, + "timestamp": 12.020525659250538 + }, + { + "x": 5.594009037113431, + "y": 4.2180198226312315, + "heading": 0.034619605911984125, + "angularVelocity": 7.854094508972923e-9, + "velocityX": -3.7118622071998786, + "velocityY": 0.31258502361778023, + "timestamp": 12.098783318227904 + }, + { + "x": 5.309172831526297, + "y": 4.280038516789027, + "heading": 0.03461959995315513, + "angularVelocity": -7.614371636753198e-8, + "velocityX": -3.6397230547046773, + "velocityY": 0.7924936034150152, + "timestamp": 12.17704097720527 + }, + { + "x": 5.03935943827081, + "y": 4.37675512978584, + "heading": 0.01971114929844739, + "angularVelocity": -0.1905046847749395, + "velocityX": -3.447757021884906, + "velocityY": 1.235874089011348, + "timestamp": 12.255298636182635 + }, + { + "x": 4.805556774139404, + "y": 4.485479354858398, + "heading": -1.4078898091571754e-24, + "angularVelocity": -0.25187501844577836, + "velocityX": -2.9876010500011123, + "velocityY": 1.3893109823794343, + "timestamp": 12.333556295160001 + }, + { + "x": 4.62745788226169, + "y": 4.587679677136332, + "heading": -0.019778853338814184, + "angularVelocity": -0.28487106556518027, + "velocityX": -2.5651244910957125, + "velocityY": 1.47197181806725, + "timestamp": 12.402987191202648 + }, + { + "x": 4.478940134439646, + "y": 4.685562428002014, + "heading": -0.039416750418752866, + "angularVelocity": -0.2828408993580651, + "velocityX": -2.139072895311886, + "velocityY": 1.4097866575934404, + "timestamp": 12.472418087245295 + }, + { + "x": 4.358424662792702, + "y": 4.772986561558442, + "heading": -0.05736759927872339, + "angularVelocity": -0.2585426644781364, + "velocityX": -1.7357614335398783, + "velocityY": 1.2591531802027822, + "timestamp": 12.541848983287942 + }, + { + "x": 4.264251963016928, + "y": 4.846292896298889, + "heading": -0.07266630938107123, + "angularVelocity": -0.22034441400483662, + "velocityX": -1.3563514968599637, + "velocityY": 1.0558172070171734, + "timestamp": 12.611279879330588 + }, + { + "x": 4.195046678979309, + "y": 4.903178017800908, + "heading": -0.08468297645986948, + "angularVelocity": -0.17307377210596744, + "velocityX": -0.9967505531703128, + "velocityY": 0.8193055936809087, + "timestamp": 12.680710775373235 + }, + { + "x": 4.149718350868284, + "y": 4.942098244876491, + "heading": -0.09298341563380762, + "angularVelocity": -0.11954964788067375, + "velocityX": -0.6528552948990165, + "velocityY": 0.5605606335784191, + "timestamp": 12.750141671415882 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.06149221603257356, + "velocityX": -0.32142062647019676, + "velocityY": 0.2860902346288004, + "timestamp": 12.819572567458529 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -9.169895235461756e-26, + "velocityX": -3.1744204454242983e-26, + "velocityY": -4.8447793914370994e-26, + "timestamp": 12.889003463501176 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePdChaos.traj b/src/main/deploy/choreo/AmpLanePdChaos.traj deleted file mode 100644 index f4f10ac8..00000000 --- a/src/main/deploy/choreo/AmpLanePdChaos.traj +++ /dev/null @@ -1,770 +0,0 @@ -{ - "samples": [ - { - "x": 0.4269569218158722, - "y": 6.9908647537231445, - "heading": 1.0147582935939444e-26, - "angularVelocity": 4.802793352791378e-28, - "velocityX": 7.569777584926927e-27, - "velocityY": 3.719655610214095e-26, - "timestamp": 0 - }, - { - "x": 0.45154320453389124, - "y": 6.9860595473156515, - "heading": -0.0020588646196638234, - "angularVelocity": -0.03238986683838223, - "velocityX": 0.38678911458368936, - "velocityY": -0.07559506058978287, - "timestamp": 0.06356508441164857 - }, - { - "x": 0.500665330018184, - "y": 6.976191418092733, - "heading": -0.005903955941845083, - "angularVelocity": -0.06049061930415103, - "velocityX": 0.7727847125345899, - "velocityY": -0.15524449175608063, - "timestamp": 0.12713016882329714 - }, - { - "x": 0.5742528189740612, - "y": 6.960923422240335, - "heading": -0.01117745758299664, - "angularVelocity": -0.08296223768067817, - "velocityX": 1.1576715367720332, - "velocityY": -0.24019469168832944, - "timestamp": 0.1906952532349457 - }, - { - "x": 0.6722013148089163, - "y": 6.9397965653748726, - "heading": -0.017389498609317183, - "angularVelocity": -0.09772725205698236, - "velocityX": 1.5409166327938608, - "velocityY": -0.3323657486025543, - "timestamp": 0.2542603376465943 - }, - { - "x": 0.7943433521763879, - "y": 6.912149808159037, - "heading": -0.023829197654655073, - "angularVelocity": -0.10130874685282076, - "velocityX": 1.9215271795515552, - "velocityY": -0.43493621493200585, - "timestamp": 0.3178254220582428 - }, - { - "x": 0.940376377090631, - "y": 6.876952520080898, - "heading": -0.029373547858602904, - "angularVelocity": -0.08722320209696448, - "velocityX": 2.2973779751243755, - "velocityY": -0.553720464684682, - "timestamp": 0.38139050646989137 - }, - { - "x": 1.109639548430514, - "y": 6.832391950283203, - "heading": -0.03199489164843748, - "angularVelocity": -0.04123873686470252, - "velocityX": 2.6628324796004614, - "velocityY": -0.7010227424401742, - "timestamp": 0.4449555908815399 - }, - { - "x": 1.3001020816519337, - "y": 6.774603331599785, - "heading": -0.02710028887606378, - "angularVelocity": 0.07700143589326758, - "velocityX": 2.9963388703770333, - "velocityY": -0.909125178048654, - "timestamp": 0.5085206752931885 - }, - { - "x": 1.5002212524414062, - "y": 6.694304943084717, - "heading": 4.6927585573960014e-26, - "angularVelocity": 0.426339225801414, - "velocityX": 3.148256195075543, - "velocityY": -1.2632467849024598, - "timestamp": 0.572085759704837 - }, - { - "x": 1.6698109310260505, - "y": 6.606230489693705, - "heading": 0.043636088677614475, - "angularVelocity": 0.7393180152975136, - "velocityX": 2.8733259186555666, - "velocityY": -1.492228842060727, - "timestamp": 0.6311078419140127 - }, - { - "x": 1.8178691248742131, - "y": 6.519462655469564, - "heading": 0.09197841408379424, - "angularVelocity": 0.8190548960108419, - "velocityX": 2.508522036268403, - "velocityY": -1.4700910401065466, - "timestamp": 0.6901299241231884 - }, - { - "x": 1.945478769087034, - "y": 6.439654918114978, - "heading": 0.13899529896762525, - "angularVelocity": 0.7965982073828233, - "velocityX": 2.1620661189242583, - "velocityY": -1.3521674323814197, - "timestamp": 0.7491520063323641 - }, - { - "x": 2.05361266916171, - "y": 6.369142283614203, - "heading": 0.1819641027220741, - "angularVelocity": 0.7280123327768478, - "velocityX": 1.8320922615275888, - "velocityY": -1.1946822589361732, - "timestamp": 0.8081740885415398 - }, - { - "x": 2.1429230510824855, - "y": 6.309142790648472, - "heading": 0.2193838977387365, - "angularVelocity": 0.6339965249623981, - "velocityX": 1.5131689458914253, - "velocityY": -1.0165600859876827, - "timestamp": 0.8671961707507155 - }, - { - "x": 2.2138592843219276, - "y": 6.2603940232890976, - "heading": 0.25031075711822803, - "angularVelocity": 0.5239879418331249, - "velocityX": 1.2018592124222665, - "velocityY": -0.8259411653185552, - "timestamp": 0.9262182529598912 - }, - { - "x": 2.2667458111920182, - "y": 6.2233878173394785, - "heading": 0.2740979405057225, - "angularVelocity": 0.4030217589273135, - "velocityX": 0.8960464438150413, - "velocityY": -0.6269891634535774, - "timestamp": 0.985240335169067 - }, - { - "x": 2.301826622003525, - "y": 6.198474614172422, - "heading": 0.2902775915680301, - "angularVelocity": 0.27412877446387834, - "velocityX": 0.5943675569963707, - "velocityY": -0.4220996995457338, - "timestamp": 1.0442624173782427 - }, - { - "x": 2.319291353225708, - "y": 6.185916423797607, - "heading": 0.2985001179522956, - "angularVelocity": 0.1393127127424044, - "velocityX": 0.2959016450874765, - "velocityY": -0.2127710494914169, - "timestamp": 1.1032844995874185 - }, - { - "x": 2.319291353225708, - "y": 6.185916423797607, - "heading": 0.2985001179522956, - "angularVelocity": -6.615842884891707e-27, - "velocityX": 2.924635996734034e-23, - "velocityY": -9.88114513956692e-23, - "timestamp": 1.162306581796594 - }, - { - "x": 2.347136097791038, - "y": 6.1936465997058665, - "heading": 0.2985001179522956, - "angularVelocity": 1.0620750257524312e-16, - "velocityX": 0.4079003982651746, - "velocityY": 0.11324010619816775, - "timestamp": 1.2305701693874758 - }, - { - "x": 2.402825586267742, - "y": 6.209106951340729, - "heading": 0.2985001179522956, - "angularVelocity": 1.523734708742185e-16, - "velocityX": 0.8158007869504784, - "velocityY": 0.22648020973522806, - "timestamp": 1.2988337569783575 - }, - { - "x": 2.486359817783878, - "y": 6.2322974784599845, - "heading": 0.2985001179522956, - "angularVelocity": 1.8006542596605785e-16, - "velocityX": 1.2237011628626233, - "velocityY": 0.3397203097241459, - "timestamp": 1.3670973445692391 - }, - { - "x": 2.5977387911187284, - "y": 6.263218180724542, - "heading": 0.2985001179522956, - "angularVelocity": 1.3885125036469104e-16, - "velocityX": 1.6316015208923507, - "velocityY": 0.45296040474566596, - "timestamp": 1.4353609321601208 - }, - { - "x": 2.7369625044412174, - "y": 6.301869057625762, - "heading": 0.2985001179522956, - "angularVelocity": 1.7915030698796464e-16, - "velocityX": 2.0395018520984625, - "velocityY": 0.5662004923160927, - "timestamp": 1.5036245197510025 - }, - { - "x": 2.904030954699552, - "y": 6.348250108315915, - "heading": 0.2985001179522956, - "angularVelocity": 1.7115442013810006e-16, - "velocityX": 2.447402138598578, - "velocityY": 0.6794405674680408, - "timestamp": 1.5718881073418842 - }, - { - "x": 3.098944135790158, - "y": 6.402361331099544, - "heading": 0.2985001179522956, - "angularVelocity": 1.731300305768192e-16, - "velocityX": 2.8553023356868215, - "velocityY": 0.7926806177830709, - "timestamp": 1.640151694932766 - }, - { - "x": 3.3217020294023807, - "y": 6.464202720890301, - "heading": 0.2985001179522956, - "angularVelocity": -4.291109894147179e-17, - "velocityX": 3.263202264540478, - "velocityY": 0.9059205935876805, - "timestamp": 1.7084152825236476 - }, - { - "x": 3.5667175760590744, - "y": 6.53222236284647, - "heading": 0.2985001179522956, - "angularVelocity": 2.176874790341123e-16, - "velocityX": 3.589256810308954, - "velocityY": 0.9964264164348671, - "timestamp": 1.7766788701145293 - }, - { - "x": 3.8162126541137695, - "y": 6.581329345703125, - "heading": 0.2985001179522956, - "angularVelocity": -3.979347495805123e-16, - "velocityX": 3.6548779057727367, - "velocityY": 0.7193730155374103, - "timestamp": 1.844942457705411 - }, - { - "x": 4.06911506298554, - "y": 6.631106978006357, - "heading": 0.2985000768137015, - "angularVelocity": -5.945239455017304e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 1.914138314964857 - }, - { - "x": 4.322017471857311, - "y": 6.6808846103095885, - "heading": 0.2985000356751073, - "angularVelocity": -5.945239469422448e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 1.983334172224303 - }, - { - "x": 4.574919880729083, - "y": 6.730662242612821, - "heading": 0.29849999453651305, - "angularVelocity": -5.945239484047347e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.0525300294837487 - }, - { - "x": 4.827822289600854, - "y": 6.780439874916052, - "heading": 0.29849995339791874, - "angularVelocity": -5.945239490070907e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.1217258867431945 - }, - { - "x": 5.080724698472625, - "y": 6.830217507219284, - "heading": 0.29849991225932443, - "angularVelocity": -5.945239490087136e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.1909217440026403 - }, - { - "x": 5.3336271073443955, - "y": 6.879995139522516, - "heading": 0.29849987112073, - "angularVelocity": -5.945239499004978e-7, - "velocityX": 3.6548778913675157, - "velocityY": 0.7193730127020961, - "timestamp": 2.260117601262086 - }, - { - "x": 5.586529516216166, - "y": 6.929772771825747, - "heading": 0.2984998299821356, - "angularVelocity": -5.945239509711758e-7, - "velocityX": 3.6548778913675157, - "velocityY": 0.7193730127020961, - "timestamp": 2.329313458521532 - }, - { - "x": 5.839431925087938, - "y": 6.979550404128979, - "heading": 0.2984997888435411, - "angularVelocity": -5.945239507812541e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.3985093157809776 - }, - { - "x": 6.092334333959709, - "y": 7.029328036432211, - "heading": 0.2984997477049465, - "angularVelocity": -5.945239524665447e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.4677051730404234 - }, - { - "x": 6.34523674283148, - "y": 7.079105668735443, - "heading": 0.29849970656635194, - "angularVelocity": -5.945239530479041e-7, - "velocityX": 3.6548778913675166, - "velocityY": 0.7193730127020963, - "timestamp": 2.536901030299869 - }, - { - "x": 6.598139151703251, - "y": 7.128883301038674, - "heading": 0.29849966542775724, - "angularVelocity": -5.945239544565078e-7, - "velocityX": 3.6548778913675157, - "velocityY": 0.7193730127020962, - "timestamp": 2.606096887559315 - }, - { - "x": 6.8510415605750214, - "y": 7.178660933341907, - "heading": 0.29849962428916255, - "angularVelocity": -5.945239549845622e-7, - "velocityX": 3.654877891367516, - "velocityY": 0.7193730127020962, - "timestamp": 2.6752927448187607 - }, - { - "x": 7.103943969446777, - "y": 7.228438565645134, - "heading": 0.29849958315051783, - "angularVelocity": -5.945246772244776e-7, - "velocityX": 3.654877891367282, - "velocityY": 0.7193730127020502, - "timestamp": 2.7444886020782064 - }, - { - "x": 7.353433038122826, - "y": 7.27754436570309, - "heading": 0.2857400994824136, - "angularVelocity": -0.18439664126514535, - "velocityX": 3.605549212875623, - "velocityY": 0.7096638729950924, - "timestamp": 2.813684459337652 - }, - { - "x": 7.577591625127487, - "y": 7.32166448211996, - "heading": 0.20789341833491348, - "angularVelocity": -1.1250193903316845, - "velocityX": 3.239479874701043, - "velocityY": 0.6376121080695129, - "timestamp": 2.882880316597098 - }, - { - "x": 7.772618972429747, - "y": 7.360050828957839, - "heading": 0.13154587005744633, - "angularVelocity": -1.1033543235284506, - "velocityX": 2.818482999220841, - "velocityY": 0.5547492054900108, - "timestamp": 2.9520761738565438 - }, - { - "x": 7.938688452207964, - "y": 7.392737530275028, - "heading": 0.06829117903161311, - "angularVelocity": -0.9141398565041748, - "velocityX": 2.3999916520370626, - "velocityY": 0.47237945466347603, - "timestamp": 3.0212720311159895 - }, - { - "x": 8.075968311155844, - "y": 7.41975770130397, - "heading": 0.023429532114866793, - "angularVelocity": -0.6483285082882946, - "velocityX": 1.9839317610179508, - "velocityY": 0.3904882763086947, - "timestamp": 3.0904678883754353 - }, - { - "x": 8.184579349560657, - "y": 7.441135118616467, - "heading": 4.707584249891564e-24, - "angularVelocity": -0.33859732421579963, - "velocityX": 1.5696176434028992, - "velocityY": 0.3089407106026808, - "timestamp": 3.159663745634881 - }, - { - "x": 8.264610290527344, - "y": 7.456887245178223, - "heading": 4.305949741153467e-24, - "angularVelocity": 2.0877324046258187e-24, - "velocityX": 1.156585728342302, - "velocityY": 0.22764551500092414, - "timestamp": 3.228859602894327 - }, - { - "x": 8.309090579989265, - "y": 7.457165429033817, - "heading": 0.013618338743485297, - "angularVelocity": 0.3126833682896741, - "velocityX": 1.021288058215364, - "velocityY": 0.006387230234869135, - "timestamp": 3.2724127308332234 - }, - { - "x": 8.347694581932599, - "y": 7.44777021311996, - "heading": 0.040485197139552155, - "angularVelocity": 0.6168755188780024, - "velocityX": 0.886365773716491, - "velocityY": -0.2157185111259548, - "timestamp": 3.31596585877212 - }, - { - "x": 8.38043908621254, - "y": 7.428658664796906, - "heading": 0.08013490839873996, - "angularVelocity": 0.9103757441902883, - "velocityX": 0.7518289920733539, - "velocityY": -0.4388100057903166, - "timestamp": 3.3595189867110165 - }, - { - "x": 8.407344118270622, - "y": 7.3997816960160065, - "heading": 0.13202626739740458, - "angularVelocity": 1.1914496490692066, - "velocityX": 0.6177520038475312, - "velocityY": -0.6630285847990697, - "timestamp": 3.403072114649913 - }, - { - "x": 8.428437067602909, - "y": 7.361082970163004, - "heading": 0.19556012614557247, - "angularVelocity": 1.4587668384531083, - "velocityX": 0.4843038911436893, - "velocityY": -0.8885406785775682, - "timestamp": 3.4466252425888095 - }, - { - "x": 8.443758202953443, - "y": 7.312496178717053, - "heading": 0.2700814705675102, - "angularVelocity": 1.7110446011245937, - "velocityX": 0.35178036746362773, - "velocityY": -1.1155752467220204, - "timestamp": 3.490178370527706 - }, - { - "x": 8.453367300003004, - "y": 7.253940317433782, - "heading": 0.354848982447824, - "angularVelocity": 1.9463013540437195, - "velocityX": 0.22062932111422345, - "velocityY": -1.3444697098546596, - "timestamp": 3.5337314984666026 - }, - { - "x": 8.45735124333647, - "y": 7.18531320673646, - "heading": 0.4489562663311165, - "angularVelocity": 2.1607468472831988, - "velocityX": 0.09147318509605634, - "velocityY": -1.5757102634190636, - "timestamp": 3.577284626405499 - }, - { - "x": 8.455833781714013, - "y": 7.106484187240998, - "heading": 0.5511965710396762, - "angularVelocity": 2.347484774273826, - "velocityX": -0.03484162204347062, - "velocityY": -1.8099508169896654, - "timestamp": 3.6208377543443957 - }, - { - "x": 8.448992060952849, - "y": 7.01728730637675, - "heading": 0.6598709993297996, - "angularVelocity": 2.4952152332799042, - "velocityX": -0.15708907913026152, - "velocityY": -2.0480017184847705, - "timestamp": 3.6643908822832922 - }, - { - "x": 8.437091795531597, - "y": 6.917516589235624, - "heading": 0.7725277452001486, - "angularVelocity": 2.5866510903281643, - "velocityX": -0.2732356086558815, - "velocityY": -2.2907818993184903, - "timestamp": 3.707944010222189 - }, - { - "x": 8.420568790056215, - "y": 6.806929386712691, - "heading": 0.8855178727468727, - "angularVelocity": 2.594305688106834, - "velocityX": -0.37937586247679217, - "velocityY": -2.539133416044943, - "timestamp": 3.7514971381610853 - }, - { - "x": 8.40022655182027, - "y": 6.685307877393552, - "heading": 0.992766575428426, - "angularVelocity": 2.462479912625774, - "velocityX": -0.46706721649209976, - "velocityY": -2.7924862133844903, - "timestamp": 3.795050266099982 - }, - { - "x": 8.377585411071777, - "y": 6.553085803985596, - "heading": 1.0808392342145412, - "angularVelocity": 2.0221890586062603, - "velocityX": -0.519851083491793, - "velocityY": -3.035880077165969, - "timestamp": 3.8386033940388784 - }, - { - "x": 8.331156557203096, - "y": 6.248001091638873, - "heading": 1.1393178439925227, - "angularVelocity": 0.6668090360061052, - "velocityX": -0.5294103161580237, - "velocityY": -3.478763324102056, - "timestamp": 3.9263025801930476 - }, - { - "x": 8.32509627497856, - "y": 5.9213777688010225, - "heading": 1.1393178431081437, - "angularVelocity": -1.0084234488389344e-8, - "velocityX": -0.06910306116049933, - "velocityY": -3.7243597935295467, - "timestamp": 4.014001766347217 - }, - { - "x": 8.328579577192095, - "y": 5.594716799276784, - "heading": 1.13931784103149, - "angularVelocity": -2.3679282154855844e-8, - "velocityX": 0.03971875186402571, - "velocityY": -3.7247890641766106, - "timestamp": 4.101700952501386 - }, - { - "x": 8.33206298187959, - "y": 5.268055830845277, - "heading": 1.139317838954836, - "angularVelocity": -2.3679282513207347e-8, - "velocityX": 0.03971992033507496, - "velocityY": -3.724789051716606, - "timestamp": 4.1894001386555555 - }, - { - "x": 8.335546386568366, - "y": 4.941394862413785, - "heading": 1.1393178368781822, - "angularVelocity": -2.3679282515836517e-8, - "velocityX": 0.0397199203496886, - "velocityY": -3.7247890517164506, - "timestamp": 4.277099324809725 - }, - { - "x": 8.339029791257143, - "y": 4.614733893982293, - "heading": 1.1393178348015287, - "angularVelocity": -2.3679282075329618e-8, - "velocityX": 0.039719920349696757, - "velocityY": -3.7247890517164497, - "timestamp": 4.364798510963894 - }, - { - "x": 8.34251319594592, - "y": 4.2880729255508, - "heading": 1.139317832724875, - "angularVelocity": -2.3679282013341874e-8, - "velocityX": 0.03971992034969426, - "velocityY": -3.72478905171645, - "timestamp": 4.452497697118063 - }, - { - "x": 8.345996600634695, - "y": 3.9614119571193074, - "heading": 1.139317830648221, - "angularVelocity": -2.3679283402849396e-8, - "velocityX": 0.039719920349694765, - "velocityY": -3.7247890517164497, - "timestamp": 4.540196883272232 - }, - { - "x": 8.349480005323471, - "y": 3.6347509886878155, - "heading": 1.139317828571567, - "angularVelocity": -2.3679282271688366e-8, - "velocityX": 0.03971992034969471, - "velocityY": -3.7247890517164506, - "timestamp": 4.627896069426401 - }, - { - "x": 8.352963410012249, - "y": 3.3080900202563233, - "heading": 1.139317826494913, - "angularVelocity": -2.367928303185141e-8, - "velocityX": 0.03971992034969481, - "velocityY": -3.72478905171645, - "timestamp": 4.7155952555805705 - }, - { - "x": 8.356446814701025, - "y": 2.981429051824831, - "heading": 1.1393178244182591, - "angularVelocity": -2.3679282848596997e-8, - "velocityX": 0.03971992034969499, - "velocityY": -3.72478905171645, - "timestamp": 4.80329444173474 - }, - { - "x": 8.3599302193898, - "y": 2.6547680833933387, - "heading": 1.1393178223416056, - "angularVelocity": -2.3679282487737324e-8, - "velocityX": 0.0397199203496951, - "velocityY": -3.72478905171645, - "timestamp": 4.890993627888909 - }, - { - "x": 8.363413624078582, - "y": 2.3281071149618464, - "heading": 1.1393178202649519, - "angularVelocity": -2.3679282618768576e-8, - "velocityX": 0.039719920349732145, - "velocityY": -3.7247890517164497, - "timestamp": 4.978692814043078 - }, - { - "x": 8.366897029211325, - "y": 2.001446146535259, - "heading": 1.139317818188219, - "angularVelocity": -2.3680183399714216e-8, - "velocityX": 0.03971992541207313, - "velocityY": -3.724789051660523, - "timestamp": 5.066392000197247 - }, - { - "x": 8.369950836003147, - "y": 1.715318694035658, - "heading": 1.1226153865662416, - "angularVelocity": -0.190451386773599, - "velocityX": 0.03482138119793182, - "velocityY": -3.262601000613718, - "timestamp": 5.154091186351416 - }, - { - "x": 8.372495686328486, - "y": 1.4768791360484652, - "heading": 1.1086925029820063, - "angularVelocity": -0.15875727238517048, - "velocityX": 0.029017946881124475, - "velocityY": -2.718834329523107, - "timestamp": 5.241790372505585 - }, - { - "x": 8.374531573132348, - "y": 1.2861274855011333, - "heading": 1.0975518982604389, - "angularVelocity": -0.12703201945321188, - "velocityX": 0.02321443211894871, - "velocityY": -2.1750675110257403, - "timestamp": 5.329489558659755 - }, - { - "x": 8.376058491392191, - "y": 1.1430637450377992, - "heading": 1.0891955157796347, - "angularVelocity": -0.09528460693026461, - "velocityX": 0.017410860086641132, - "velocityY": -1.6313006623783024, - "timestamp": 5.417188744813924 - }, - { - "x": 8.377076437882634, - "y": 1.0476879159528336, - "heading": 1.0836245099802884, - "angularVelocity": -0.06352403076526633, - "velocityX": 0.01160725127658835, - "velocityY": -1.0875337989716487, - "timestamp": 5.504887930968093 - }, - { - "x": 8.377585411071777, - "y": 1, - "heading": 1.0808392342145412, - "angularVelocity": -0.03175942546206756, - "velocityX": 0.005803624998845044, - "velocityY": -0.5437669155675129, - "timestamp": 5.592587117122262 - }, - { - "x": 8.377585411071777, - "y": 1, - "heading": 1.0808392342145412, - "angularVelocity": 4.871302260765609e-25, - "velocityX": -2.093358596809338e-24, - "velocityY": 4.245353600645279e-24, - "timestamp": 5.680286303276431 - } - ], - "eventMarkers": [] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAEF.1.traj b/src/main/deploy/choreo/CenterLanePCBAEF.1.traj new file mode 100644 index 00000000..d049a791 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAEF.1.traj @@ -0,0 +1,275 @@ +{ + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -8.290804854356393e-28, + "angularVelocity": -1.4522370736587165e-27, + "velocityX": 4.9023959214285697e-26, + "velocityY": -8.208515528255786e-26, + "timestamp": 0 + }, + { + "x": 1.310478129945639, + "y": 5.567275491596602, + "heading": -0.01363283868540513, + "angularVelocity": -0.19128951975675812, + "velocityX": 0.2876978871987184, + "velocityY": -0.332249849655198, + "timestamp": 0.07126808987100033 + }, + { + "x": 1.3514857086951306, + "y": 5.519918181736233, + "heading": -0.04088194207773102, + "angularVelocity": -0.38234648131651156, + "velocityX": 0.5753988752009218, + "velocityY": -0.6644952873872267, + "timestamp": 0.14253617974200067 + }, + { + "x": 1.4129978772139058, + "y": 5.448882692998019, + "heading": -0.08170986553530027, + "angularVelocity": -0.5728780374424279, + "velocityX": 0.8631095435575179, + "velocityY": -0.9967362513404223, + "timestamp": 0.213804269613001 + }, + { + "x": 1.495015808643516, + "y": 5.354169496620742, + "heading": -0.13606373855442885, + "angularVelocity": -0.7626677397628091, + "velocityX": 1.1508366728793693, + "velocityY": -1.3289706030947979, + "timestamp": 0.28507235948400134 + }, + { + "x": 1.59754112837037, + "y": 5.235779380543083, + "heading": -0.20388316047242275, + "angularVelocity": -0.9516099286616388, + "velocityX": 1.4385866088516093, + "velocityY": -1.6611938988676576, + "timestamp": 0.35634044935500164 + }, + { + "x": 1.7205758185191462, + "y": 5.093713591292308, + "heading": -0.28510990320243984, + "angularVelocity": -1.1397350886917637, + "velocityX": 1.7263643570562468, + "velocityY": -1.99339970396181, + "timestamp": 0.42760853922600195 + }, + { + "x": 1.864122091418882, + "y": 4.927973933585744, + "heading": -0.3796975284198671, + "angularVelocity": -1.3272086481991685, + "velocityX": 2.0141731476115536, + "velocityY": -2.325580186119234, + "timestamp": 0.49887662909700226 + }, + { + "x": 2.0360573445839716, + "y": 4.766214838118459, + "heading": -0.469084063757855, + "angularVelocity": -1.2542294244139716, + "velocityX": 2.4125138400131556, + "velocityY": -2.2697268266917123, + "timestamp": 0.5701447189680026 + }, + { + "x": 2.1874785084173087, + "y": 4.628126850696153, + "heading": -0.5451442530827014, + "angularVelocity": -1.0672404643160782, + "velocityX": 2.1246698783062508, + "velocityY": -1.937585077308154, + "timestamp": 0.6414128088390029 + }, + { + "x": 2.3183845436377304, + "y": 4.513708759426245, + "heading": -0.6078811711978229, + "angularVelocity": -0.8802946483998556, + "velocityX": 1.8368113338995, + "velocityY": -1.6054603326258816, + "timestamp": 0.7126808987100032 + }, + { + "x": 2.428774775812145, + "y": 4.422959692661252, + "heading": -0.657294007593887, + "angularVelocity": -0.6933374598014952, + "velocityX": 1.5489433261678196, + "velocityY": -1.273347818487273, + "timestamp": 0.7839489885810035 + }, + { + "x": 2.518648765365177, + "y": 4.355879041329867, + "heading": -0.6933804923945769, + "angularVelocity": -0.5063484213763644, + "velocityX": 1.2610691505231797, + "velocityY": -0.9412438505480457, + "timestamp": 0.8552170784520038 + }, + { + "x": 2.588006261722225, + "y": 4.3124664184627175, + "heading": -0.7161379527262285, + "angularVelocity": -0.3193218784570213, + "velocityX": 0.9731914589346954, + "velocityY": -0.6091453123793419, + "timestamp": 0.9264851683230041 + }, + { + "x": 2.6368471855998425, + "y": 4.2927216346124935, + "heading": -0.7255636953209265, + "angularVelocity": -0.13225754488101377, + "velocityX": 0.6853126548785442, + "velocityY": -0.2770494324453428, + "timestamp": 0.9977532581940044 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05484334197651896, + "velocityX": 0.3974350607881677, + "velocityY": 0.05504641764912788, + "timestamp": 1.0690213480650048 + }, + { + "x": 2.6726522356618694, + "y": 4.325520885096074, + "heading": -0.7036434508654623, + "angularVelocity": 0.24661816834169437, + "velocityX": 0.10242556780992029, + "velocityY": 0.395376841972494, + "timestamp": 1.1420559681490368 + }, + { + "x": 2.658587249318106, + "y": 4.379253606612043, + "heading": -0.671632044096526, + "angularVelocity": 0.43830455655283357, + "velocityX": -0.19257971531283274, + "velocityY": 0.7357157667712343, + "timestamp": 1.2150905882330687 + }, + { + "x": 2.6229771758212266, + "y": 4.457843745767866, + "heading": -0.6256270362766762, + "angularVelocity": 0.6299068546795684, + "velocityX": -0.487577993229883, + "velocityY": 1.0760669264165268, + "timestamp": 1.2881252083171006 + }, + { + "x": 2.5658227847056265, + "y": 4.561292508951193, + "heading": -0.5656323531358255, + "angularVelocity": 0.8214554011758, + "velocityX": -0.7825657345768313, + "velocityY": 1.4164346040864118, + "timestamp": 1.3611598284011326 + }, + { + "x": 2.4871252028275452, + "y": 4.6896014337350325, + "heading": -0.4916452107859868, + "angularVelocity": 1.0130420650468346, + "velocityX": -1.077538046854127, + "velocityY": 1.7568233344160629, + "timestamp": 1.4341944484851645 + }, + { + "x": 2.3868860574552526, + "y": 4.8427723316162075, + "heading": -0.4036473210602506, + "angularVelocity": 1.2048791329986765, + "velocityX": -1.3724880783518802, + "velocityY": 2.097236868007809, + "timestamp": 1.5072290685691965 + }, + { + "x": 2.2651076003378585, + "y": 5.020807045399031, + "heading": -0.30159033464974355, + "angularVelocity": 1.3973782062956255, + "velocityX": -1.6674072785930696, + "velocityY": 2.4376756335280563, + "timestamp": 1.5802636886532284 + }, + { + "x": 2.1359139807220826, + "y": 5.170004552864557, + "heading": -0.21560374484097594, + "angularVelocity": 1.1773401396465593, + "velocityX": -1.7689366969682099, + "velocityY": 2.0428326633843468, + "timestamp": 1.6532983087372604 + }, + { + "x": 2.0282550130804604, + "y": 5.294335941405344, + "heading": -0.14382079923066451, + "angularVelocity": 0.9828619020365901, + "velocityX": -1.4740812989477097, + "velocityY": 1.7023623645571657, + "timestamp": 1.7263329288212923 + }, + { + "x": 1.9421291374446024, + "y": 5.393801086818423, + "heading": -0.08631852593947899, + "angularVelocity": 0.7873289848708013, + "velocityX": -1.1792472602275945, + "velocityY": 1.3618903651259822, + "timestamp": 1.7993675489053242 + }, + { + "x": 1.8775352248155426, + "y": 5.468400016112322, + "heading": -0.04315881325455388, + "angularVelocity": 0.590948684819152, + "velocityX": -0.8844286799156282, + "velocityY": 1.0214187354992301, + "timestamp": 1.8724021689893562 + }, + { + "x": 1.83447263299525, + "y": 5.518132767413434, + "heading": -0.014381451378905535, + "angularVelocity": 0.39402357186958425, + "velocityX": -0.5896188926668684, + "velocityY": 0.6809476279042802, + "timestamp": 1.9454367890733881 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -8.684305119206267e-26, + "angularVelocity": 0.1969127978259977, + "velocityX": -0.2948114112163433, + "velocityY": 0.34047551881670374, + "timestamp": 2.0184714091574203 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -4.2442319768143513e-26, + "angularVelocity": 2.0248829096214925e-26, + "velocityX": -2.1409957925018854e-22, + "velocityY": -3.6446450287003043e-25, + "timestamp": 2.0915060292414522 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAEF.2.traj b/src/main/deploy/choreo/CenterLanePCBAEF.2.traj new file mode 100644 index 00000000..aefeb454 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAEF.2.traj @@ -0,0 +1,86 @@ +{ + "samples": [ + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -4.2442319768143513e-26, + "angularVelocity": 2.0248829096214925e-26, + "velocityX": -2.1409957925018854e-22, + "velocityY": -3.6446450287003043e-25, + "timestamp": 0 + }, + { + "x": 1.8720264471920118, + "y": 5.543082773692806, + "heading": 1.803287488569909e-20, + "angularVelocity": 1.8474379021994642e-19, + "velocityX": 0.6053171264251317, + "velocityY": 0.0008555041789240009, + "timestamp": 0.09761041119112068 + }, + { + "x": 1.9901969516410831, + "y": 5.543249785918246, + "heading": 2.6710707578067816e-20, + "angularVelocity": 8.890273677603703e-20, + "velocityX": 1.210634224434258, + "velocityY": 0.0017110083176872164, + "timestamp": 0.19522082238224137 + }, + { + "x": 2.1674527004558795, + "y": 5.543500304245301, + "heading": 2.745376939135264e-20, + "angularVelocity": 7.612526210817117e-21, + "velocityX": 1.8159512561393767, + "velocityY": 0.0025665123627419384, + "timestamp": 0.29283123357336205 + }, + { + "x": 2.4037936612766035, + "y": 5.543834328628234, + "heading": 2.868215450901532e-21, + "angularVelocity": -2.5187429947361393e-19, + "velocityX": 2.4212679563245554, + "velocityY": 0.0034220159392543354, + "timestamp": 0.39044164476448273 + }, + { + "x": 2.5810494100914, + "y": 5.544084846955289, + "heading": 4.197057323633921e-22, + "angularVelocity": -2.508451391919409e-20, + "velocityX": 1.815951256139377, + "velocityY": 0.0025665123627419384, + "timestamp": 0.4880520559556034 + }, + { + "x": 2.699219914540471, + "y": 5.544251859180729, + "heading": 1.3007601681932514e-20, + "angularVelocity": 1.2896058726029048e-19, + "velocityX": 1.2106342244342583, + "velocityY": 0.0017110083176872164, + "timestamp": 0.5856624671467241 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.0578906521828803e-28, + "angularVelocity": -1.3326039434355583e-19, + "velocityX": 0.6053171264251317, + "velocityY": 0.0008555041789240007, + "timestamp": 0.6832728783378448 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.171991433780983e-28, + "angularVelocity": -1.132446733749095e-28, + "velocityX": 2.142784500904642e-22, + "velocityY": 2.9856908340513677e-25, + "timestamp": 0.7808832895289655 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAEF.3.traj b/src/main/deploy/choreo/CenterLanePCBAEF.3.traj new file mode 100644 index 00000000..0821e2da --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAEF.3.traj @@ -0,0 +1,176 @@ +{ + "samples": [ + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.171991433780983e-28, + "angularVelocity": -1.132446733749095e-28, + "velocityX": 2.142784500904642e-22, + "velocityY": 2.9856908340513677e-25, + "timestamp": 0 + }, + { + "x": 2.7303741375078747, + "y": 5.568056872076371, + "heading": 0.006778509195380263, + "angularVelocity": 0.08813497362119757, + "velocityX": -0.36316254475214277, + "velocityY": 0.3084298205009288, + "timestamp": 0.07691054886467841 + }, + { + "x": 2.674756164732603, + "y": 5.615779956875509, + "heading": 0.02059740285114023, + "angularVelocity": 0.17967488023097908, + "velocityX": -0.7231514219607195, + "velocityY": 0.6205011601608276, + "timestamp": 0.15382109772935681 + }, + { + "x": 2.591861664751003, + "y": 5.687962530350053, + "heading": 0.041894082151838305, + "angularVelocity": 0.2769019284749726, + "velocityX": -1.07780403605558, + "velocityY": 0.9385263080302103, + "timestamp": 0.23073164659403522 + }, + { + "x": 2.4825229880731845, + "y": 5.785486691208239, + "heading": 0.07152941986388306, + "angularVelocity": 0.38532214565504064, + "velocityX": -1.4216343309445878, + "velocityY": 1.2680206070272069, + "timestamp": 0.30764219545871363 + }, + { + "x": 2.3492932233323143, + "y": 5.910724338795779, + "heading": 0.11194462213648768, + "angularVelocity": 0.5254832122406747, + "velocityX": -1.7322690672157808, + "velocityY": 1.628354620221102, + "timestamp": 0.38455274432339204 + }, + { + "x": 2.2322888283047253, + "y": 6.068263103797439, + "heading": 0.1780934351870072, + "angularVelocity": 0.8600746455067713, + "velocityX": -1.5213049023152154, + "velocityY": 2.0483375470229004, + "timestamp": 0.46146329318807044 + }, + { + "x": 2.1483049194140715, + "y": 6.209825434589826, + "heading": 0.24392342400918363, + "angularVelocity": 0.8559292554003495, + "velocityX": -1.0919686587911486, + "velocityY": 1.8406100708169557, + "timestamp": 0.5383738420527489 + }, + { + "x": 2.0954357223517674, + "y": 6.331973751888774, + "heading": 0.3066259975705838, + "angularVelocity": 0.815266234437141, + "velocityX": -0.6874115169211118, + "velocityY": 1.5881867845445201, + "timestamp": 0.6152843909174273 + }, + { + "x": 2.072970005140484, + "y": 6.433621819948826, + "heading": 0.3652444947638923, + "angularVelocity": 0.7621645932659495, + "velocityX": -0.2921018968517744, + "velocityY": 1.3216401333827206, + "timestamp": 0.6921949397821057 + }, + { + "x": 2.080539806403903, + "y": 6.514239906298576, + "heading": 0.4192892776661922, + "angularVelocity": 0.7026966222460566, + "velocityX": 0.09842344613530404, + "velocityY": 1.0482058383381783, + "timestamp": 0.7691054886467841 + }, + { + "x": 2.1179206829564796, + "y": 6.573514734989947, + "heading": 0.46845944965014935, + "angularVelocity": 0.6393163578961119, + "velocityX": 0.48603055243236887, + "velocityY": 0.7706982925796543, + "timestamp": 0.8460160375114625 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.5732759758018011, + "velocityX": 0.871673332481117, + "velocityY": 0.4905009632033997, + "timestamp": 0.9229265863761409 + }, + { + "x": 2.2917884862212223, + "y": 6.6722766025245654, + "heading": 0.5461831512487826, + "angularVelocity": 0.4115075108626725, + "velocityX": 1.3070625376553004, + "velocityY": 0.7468097995934098, + "timestamp": 1.0046571277659586 + }, + { + "x": 2.4341165316393667, + "y": 6.754285323763742, + "heading": 0.5638405564624906, + "angularVelocity": 0.21604414841069292, + "velocityX": 1.741430351468047, + "velocityY": 1.003403621762784, + "timestamp": 1.0863876691557763 + }, + { + "x": 2.541260883257264, + "y": 6.816945409398408, + "heading": 0.5395665848363949, + "angularVelocity": -0.2970000101959424, + "velocityX": 1.3109463096159768, + "velocityY": 0.7666667144146957, + "timestamp": 1.1681182105455945 + }, + { + "x": 2.6126555762150003, + "y": 6.858741818277979, + "heading": 0.521795194780163, + "angularVelocity": -0.21743879037176994, + "velocityX": 0.8735375019384195, + "velocityY": 0.5113927813131806, + "timestamp": 1.2498487519354122 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.1131128587056523, + "velocityX": 0.4366659015377113, + "velocityY": 0.2557557080208232, + "timestamp": 1.33157929332523 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 1.4611171491066952e-26, + "velocityX": -3.269712364266787e-25, + "velocityY": -3.777106652858518e-25, + "timestamp": 1.4133098347150477 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAEF.4.traj b/src/main/deploy/choreo/CenterLanePCBAEF.4.traj new file mode 100644 index 00000000..e5e2ff40 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAEF.4.traj @@ -0,0 +1,644 @@ +{ + "samples": [ + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 1.4611171491066952e-26, + "velocityX": -3.269712364266787e-25, + "velocityY": -3.777106652858518e-25, + "timestamp": 0 + }, + { + "x": 2.660849769218119, + "y": 6.863303091211252, + "heading": 0.49944854128498545, + "angularVelocity": -0.22618144187200803, + "velocityX": 0.2158817205646462, + "velocityY": -0.2821127758769456, + "timestamp": 0.05792640725329168 + }, + { + "x": 2.685869953878005, + "y": 6.830605661533361, + "heading": 0.4737478609731425, + "angularVelocity": -0.4436781345589612, + "velocityX": 0.4319305450876828, + "velocityY": -0.5644650035849392, + "timestamp": 0.11585281450658336 + }, + { + "x": 2.7234170079843754, + "y": 6.781535741371895, + "heading": 0.43609134859082577, + "angularVelocity": -0.6500750550203873, + "velocityX": 0.6481854457534552, + "velocityY": -0.8471079510749036, + "timestamp": 0.17377922175987504 + }, + { + "x": 2.7735061222918693, + "y": 6.716073137055524, + "heading": 0.3873132758064447, + "angularVelocity": -0.8420697070179436, + "velocityX": 0.8647025887255888, + "velocityY": -1.1300995076412468, + "timestamp": 0.23170562901316671 + }, + { + "x": 2.8361568283253176, + "y": 6.63419369001641, + "heading": 0.3285224732338341, + "angularVelocity": -1.0149223015944975, + "velocityX": 1.0815569099512647, + "velocityY": -1.4135081204170898, + "timestamp": 0.2896320362664584 + }, + { + "x": 2.9113939859812374, + "y": 6.535868324439057, + "heading": 0.2612561141808071, + "angularVelocity": -1.1612382373188714, + "velocityX": 1.298840394622348, + "velocityY": -1.6974186772435556, + "timestamp": 0.34755844351975007 + }, + { + "x": 2.999247861116449, + "y": 6.4210622556853, + "heading": 0.18781374862867117, + "angularVelocity": -1.267856389418368, + "velocityX": 1.5166463673650283, + "velocityY": -1.9819297311457886, + "timestamp": 0.40548485077304175 + }, + { + "x": 3.09974782280069, + "y": 6.289740190661489, + "heading": 0.11213274399438687, + "angularVelocity": -1.3065026509128828, + "velocityX": 1.734959346689516, + "velocityY": -2.2670500597350514, + "timestamp": 0.46341125802633343 + }, + { + "x": 3.212855329084297, + "y": 6.141935125074281, + "heading": 0.04279640937177349, + "angularVelocity": -1.1969728127524046, + "velocityX": 1.9526069654040226, + "velocityY": -2.551600774080633, + "timestamp": 0.5213376652796251 + }, + { + "x": 3.3365134053253396, + "y": 5.978631674871108, + "heading": 0.010716435347090821, + "angularVelocity": -0.5538056914941833, + "velocityX": 2.1347444473870243, + "velocityY": -2.819153784026439, + "timestamp": 0.5792640725329168 + }, + { + "x": 3.4677690523177653, + "y": 5.807368153650049, + "heading": 0.010716404660462071, + "angularVelocity": -5.297519767950936e-7, + "velocityX": 2.2659034664188993, + "velocityY": -2.95657074798692, + "timestamp": 0.6371904797862085 + }, + { + "x": 3.5990247641115705, + "y": 5.636104682092312, + "heading": 0.010716373974269203, + "angularVelocity": -5.297444520108962e-7, + "velocityX": 2.265904585103496, + "velocityY": -2.9565698906349174, + "timestamp": 0.6951168870395001 + }, + { + "x": 3.730280476023843, + "y": 5.4648412106253685, + "heading": 0.010716343288076406, + "angularVelocity": -5.297444507915692e-7, + "velocityX": 2.265904587148631, + "velocityY": -2.9565698890675334, + "timestamp": 0.7530432942927918 + }, + { + "x": 3.8615392378596742, + "y": 5.293580076652075, + "heading": 0.010716312601908511, + "angularVelocity": -5.297440208932243e-7, + "velocityX": 2.265957238844176, + "velocityY": -2.956529536251564, + "timestamp": 0.8109697015460835 + }, + { + "x": 4.007190539027271, + "y": 5.134379144193632, + "heading": 0.010716281749960886, + "angularVelocity": -5.326059234198986e-7, + "velocityX": 2.5144197279613127, + "velocityY": -2.748330856466123, + "timestamp": 0.8688961087993752 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.010716249762746122, + "angularVelocity": -5.522043620508212e-7, + "velocityX": 2.76745156437766, + "velocityY": -2.4933593359962067, + "timestamp": 0.9268225160526669 + }, + { + "x": 4.289284709391487, + "y": 4.8946057330854265, + "heading": 0.010716218824995563, + "angularVelocity": -7.451052814449057e-7, + "velocityX": 2.933087406208596, + "velocityY": -2.2962197715760517, + "timestamp": 0.9683438303287186 + }, + { + "x": 4.417365814004084, + "y": 4.807904620313661, + "heading": 0.010716189360474368, + "angularVelocity": -7.096240017556084e-7, + "velocityX": 3.084707380914231, + "velocityY": -2.08811099271423, + "timestamp": 1.0098651446047704 + }, + { + "x": 4.551130165096487, + "y": 4.730258301325536, + "heading": 0.010716160700942695, + "angularVelocity": -6.902366211867451e-7, + "velocityX": 3.2215827804264254, + "velocityY": -1.870035193777778, + "timestamp": 1.0513864588808222 + }, + { + "x": 4.688893145382921, + "y": 4.6599503090237695, + "heading": 0.010716132275769374, + "angularVelocity": -6.845923308774473e-7, + "velocityX": 3.3178858301672403, + "velocityY": -1.693298815984659, + "timestamp": 1.092907773156874 + }, + { + "x": 4.826657112545421, + "y": 4.589644250461509, + "heading": 0.0107161038506266, + "angularVelocity": -6.845915951833867e-7, + "velocityX": 3.317909598106278, + "velocityY": -1.693252243771356, + "timestamp": 1.1344290874329257 + }, + { + "x": 4.964421079807684, + "y": 4.5193381920947315, + "heading": 0.01071607542548375, + "angularVelocity": -6.845915970495296e-7, + "velocityX": 3.317909600508956, + "velocityY": -1.6932522390633336, + "timestamp": 1.1759504017089775 + }, + { + "x": 5.102185047070045, + "y": 4.4490321337281475, + "heading": 0.010716047000340928, + "angularVelocity": -6.845915962954477e-7, + "velocityX": 3.3179096005113364, + "velocityY": -1.6932522390586708, + "timestamp": 1.2174717159850292 + }, + { + "x": 5.2399490152099615, + "y": 4.378726077081124, + "heading": 0.01071601857519813, + "angularVelocity": -6.84591595754071e-7, + "velocityX": 3.317909621646396, + "velocityY": -1.6932521976447459, + "timestamp": 1.258993030261081 + }, + { + "x": 5.377721663167995, + "y": 4.308437031039157, + "heading": 0.010715990149891885, + "angularVelocity": -6.845955322087529e-7, + "velocityX": 3.3181186665253675, + "velocityY": -1.6928425139593435, + "timestamp": 1.3005143445371328 + }, + { + "x": 5.519903819666949, + "y": 4.247559392833526, + "heading": 0.010715961486816888, + "angularVelocity": -6.903219586500289e-7, + "velocityX": 3.4243173410567787, + "velocityY": -1.4661780164493292, + "timestamp": 1.3420356588131845 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.01071593198598864, + "angularVelocity": -7.104984214494266e-7, + "velocityX": 3.5174201900946844, + "velocityY": -1.22612492322878, + "timestamp": 1.3835569730892363 + }, + { + "x": 5.815510824369738, + "y": 4.155881163603153, + "heading": 0.010715902600881807, + "angularVelocity": -7.061178508232372e-7, + "velocityX": 3.593873172685508, + "velocityY": -0.97964420643565, + "timestamp": 1.425171990749189 + }, + { + "x": 5.9675336119930025, + "y": 4.125565856378816, + "heading": 0.010715874047412989, + "angularVelocity": -6.861337666816658e-7, + "velocityX": 3.653075167851236, + "velocityY": -0.7284703678862195, + "timestamp": 1.466787008409142 + }, + { + "x": 6.121239827015959, + "y": 4.105457516553604, + "heading": 0.010715845747967176, + "angularVelocity": -6.800296480422641e-7, + "velocityX": 3.6935275692763176, + "velocityY": -0.483199117912718, + "timestamp": 1.5084020260690947 + }, + { + "x": 6.274947903964394, + "y": 4.085363414233198, + "heading": 0.010715817448685471, + "angularVelocity": -6.800257045912017e-7, + "velocityX": 3.693572310948509, + "velocityY": -0.4828569937083839, + "timestamp": 1.5500170437290475 + }, + { + "x": 6.4286559810993475, + "y": 4.0652693133395585, + "heading": 0.01071578914940375, + "angularVelocity": -6.800257049819807e-7, + "velocityX": 3.693572315430539, + "velocityY": -0.48285695942348605, + "timestamp": 1.5916320613890003 + }, + { + "x": 6.582364058235009, + "y": 4.045175212451323, + "heading": 0.010715760850122012, + "angularVelocity": -6.800257053652886e-7, + "velocityX": 3.693572315447514, + "velocityY": -0.482856959293641, + "timestamp": 1.633247079048953 + }, + { + "x": 6.736072142233211, + "y": 4.025081164057568, + "heading": 0.010715732550840138, + "angularVelocity": -6.80025708616856e-7, + "velocityX": 3.693572480352915, + "velocityY": -0.4828556978624376, + "timestamp": 1.6748620967089058 + }, + { + "x": 6.88984765379033, + "y": 4.0055098097098245, + "heading": 0.01071570424372189, + "angularVelocity": -6.802140150471018e-7, + "velocityX": 3.69519275021481, + "velocityY": -0.47029547140088923, + "timestamp": 1.7164771143688586 + }, + { + "x": 7.0446085905714275, + "y": 3.9966217347495547, + "heading": 0.010715671283588182, + "angularVelocity": -7.920249842947072e-7, + "velocityX": 3.7188723082058996, + "velocityY": -0.21357854592052494, + "timestamp": 1.7580921320288114 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -1.8890515062078487e-25, + "angularVelocity": -0.2574952958364392, + "velocityX": 3.6592868466916966, + "velocityY": 0.030155204912350568, + "timestamp": 1.7997071496887642 + }, + { + "x": 7.411732272025267, + "y": 4.005484051327251, + "heading": -0.0035958107444463492, + "angularVelocity": -0.054616477872747704, + "velocityX": 3.2632237151822827, + "velocityY": 0.11554829109191134, + "timestamp": 1.8655446200731474 + }, + { + "x": 7.5997547622463255, + "y": 4.014664246157471, + "heading": -0.0036926162217638513, + "angularVelocity": -0.0014703705466251413, + "velocityX": 2.855858360342751, + "velocityY": 0.139437235006489, + "timestamp": 1.9313820904575305 + }, + { + "x": 7.760906466600038, + "y": 4.024479422852048, + "heading": -0.002491126392030619, + "angularVelocity": 0.018249331614937442, + "velocityX": 2.44772017230992, + "velocityY": 0.149081923064063, + "timestamp": 1.9972195608419137 + }, + { + "x": 7.8951789466062605, + "y": 4.0345153008905195, + "heading": -0.0009601367346007951, + "angularVelocity": 0.023254077784145503, + "velocityX": 2.0394538128863453, + "velocityY": 0.1524341378834605, + "timestamp": 2.063057031226297 + }, + { + "x": 8.002571224633423, + "y": 4.044538615665461, + "heading": 0.0003551372183222516, + "angularVelocity": 0.01997758943720037, + "velocityX": 1.6311726042961088, + "velocityY": 0.15224331549225237, + "timestamp": 2.12889450161068 + }, + { + "x": 8.08308410390753, + "y": 4.05439979104189, + "heading": 0.0011050703957921194, + "angularVelocity": 0.01139067423294795, + "velocityX": 1.222903595840534, + "velocityY": 0.14978059331343505, + "timestamp": 2.194731971995063 + }, + { + "x": 8.13671880253707, + "y": 4.063994763416068, + "heading": 0.0010463784362104483, + "angularVelocity": -0.0008914674157285515, + "velocityX": 0.8146530891360332, + "velocityY": 0.14573725749423916, + "timestamp": 2.2605694423794462 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -6.673758454276996e-26, + "angularVelocity": -0.01589335723412987, + "velocityX": 0.4064214467340168, + "velocityY": 0.14053080109998423, + "timestamp": 2.3264069127638294 + }, + { + "x": 8.158055062338372, + "y": 4.083508095993073, + "heading": -0.002757938692971577, + "angularVelocity": -0.035856325054586746, + "velocityX": -0.07048563935601859, + "velocityY": 0.13340643741083522, + "timestamp": 2.403323291917938 + }, + { + "x": 8.11595159662588, + "y": 4.09322125310912, + "heading": -0.007051362960883781, + "angularVelocity": -0.05581937573153214, + "velocityX": -0.5473927162917097, + "velocityY": 0.12628203801150484, + "timestamp": 2.480239671072047 + }, + { + "x": 8.037166166331591, + "y": 4.102386423514656, + "heading": -0.012880297853755818, + "angularVelocity": -0.07578275208708442, + "velocityX": -1.0242997806284444, + "velocityY": 0.11915759044211331, + "timestamp": 2.5571560502261557 + }, + { + "x": 7.921698772840109, + "y": 4.111003602378457, + "heading": -0.020244795899094583, + "angularVelocity": -0.0957468113596885, + "velocityX": -1.501206826963765, + "velocityY": 0.11203308006134147, + "timestamp": 2.6340724293802644 + }, + { + "x": 7.769549418244295, + "y": 4.119072783408171, + "heading": -0.029144948174511114, + "angularVelocity": -0.11571205474433797, + "velocityX": -1.9781138460895251, + "velocityY": 0.10490848787287595, + "timestamp": 2.710988808534373 + }, + { + "x": 7.5807181060109095, + "y": 4.1265939580705435, + "heading": -0.0395808971630345, + "angularVelocity": -0.1356791505696594, + "velocityX": -2.4550208201434414, + "velocityY": 0.09778378474242518, + "timestamp": 2.787905187688482 + }, + { + "x": 7.355204842942577, + "y": 4.133567113261441, + "heading": -0.051552849562924184, + "angularVelocity": -0.1556489337063405, + "velocityX": -2.931927705755565, + "velocityY": 0.0906589112434205, + "timestamp": 2.8648215668425907 + }, + { + "x": 7.093009648902004, + "y": 4.139992219744277, + "heading": -0.06506107751515841, + "angularVelocity": -0.17562225498380826, + "velocityX": -3.408834333130018, + "velocityY": 0.08353365763568768, + "timestamp": 2.9417379459966995 + }, + { + "x": 6.806528037248827, + "y": 4.135713032696913, + "heading": -0.06506107911297748, + "angularVelocity": -2.077345661840568e-8, + "velocityX": -3.7245852548412777, + "velocityY": -0.05563427574756843, + "timestamp": 3.0186543251508082 + }, + { + "x": 6.520046430966287, + "y": 4.131433486105525, + "heading": -0.06506108071074132, + "angularVelocity": -2.0772738551017004e-8, + "velocityX": -3.7245851850169447, + "velocityY": -0.0556389502268829, + "timestamp": 3.095570704304917 + }, + { + "x": 6.233564817210998, + "y": 4.127154439784588, + "heading": -0.06506108230850521, + "angularVelocity": -2.07727392722992e-8, + "velocityX": -3.724585282171128, + "velocityY": -0.055632446144705854, + "timestamp": 3.1724870834590257 + }, + { + "x": 5.94752903267143, + "y": 4.143693939737266, + "heading": -0.06506108392885115, + "angularVelocity": -2.1066331425696132e-8, + "velocityX": -3.7187889976784825, + "velocityY": 0.21503222245471107, + "timestamp": 3.2494034626131345 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.06506108566326727, + "angularVelocity": -2.2549372865536112e-8, + "velocityX": -3.660823701626197, + "velocityY": 0.68847669897561, + "timestamp": 3.3263198417672433 + }, + { + "x": 5.429536251240902, + "y": 4.26885692196241, + "heading": -0.06506108731980774, + "angularVelocity": -2.4962356651562853e-8, + "velocityX": -3.5625374415593805, + "velocityY": 1.0880977954514335, + "timestamp": 3.392681383633991 + }, + { + "x": 5.202528633144186, + "y": 4.366702684523792, + "heading": -0.06506108887411338, + "angularVelocity": -2.342178313955022e-8, + "velocityX": -3.420770701086799, + "velocityY": 1.4744347374847653, + "timestamp": 3.4590429255007384 + }, + { + "x": 4.987502129020376, + "y": 4.48575380362361, + "heading": -0.06936699261461025, + "angularVelocity": -0.06488552886765389, + "velocityX": -3.2402276691457503, + "velocityY": 1.7939775923059436, + "timestamp": 3.525404467367486 + }, + { + "x": 4.796367424971955, + "y": 4.591577847199328, + "heading": -0.07462166054562926, + "angularVelocity": -0.0791824267972894, + "velocityX": -2.8802028806415447, + "velocityY": 1.5946592046973391, + "timestamp": 3.5917660092342336 + }, + { + "x": 4.62912519228471, + "y": 4.68417391671917, + "heading": -0.07977945899466835, + "angularVelocity": -0.0777227036013692, + "velocityX": -2.5201679765527953, + "velocityY": 1.3953272771415286, + "timestamp": 3.658127551100981 + }, + { + "x": 4.48577515824327, + "y": 4.763541939192526, + "heading": -0.08449544521256436, + "angularVelocity": -0.07106504890084675, + "velocityX": -2.1601371820034427, + "velocityY": 1.195994249692469, + "timestamp": 3.7244890929677297 + }, + { + "x": 4.366317095706272, + "y": 4.829681919907118, + "heading": -0.08859794312005527, + "angularVelocity": -0.06182041272833338, + "velocityX": -1.8001098102402053, + "velocityY": 0.9966613019239372, + "timestamp": 3.7908506348344773 + }, + { + "x": 4.270750839723337, + "y": 4.882593875393496, + "heading": -0.09198424265030704, + "angularVelocity": -0.05102804176930339, + "velocityX": -1.4400849241090516, + "velocityY": 0.7973286032537331, + "timestamp": 3.857212176701225 + }, + { + "x": 4.199076268469758, + "y": 4.922277822249225, + "heading": -0.09458599237732916, + "angularVelocity": -0.03920568530861384, + "velocityX": -1.0800618737505943, + "velocityY": 0.597996154691727, + "timestamp": 3.9235737185679724 + }, + { + "x": 4.151293289107276, + "y": 4.948733775055273, + "heading": -0.09635440561696425, + "angularVelocity": -0.026648163829376063, + "velocityX": -0.7200402223689916, + "velocityY": 0.3986639258498654, + "timestamp": 3.98993526043472 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.013539011456923473, + "velocityX": -0.3600196690634489, + "velocityY": 0.19933188392621148, + "timestamp": 4.056296802301468 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -2.1027730559805923e-24, + "velocityX": -4.370401644398185e-24, + "velocityY": -7.227687528271155e-24, + "timestamp": 4.122658344168215 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAEF.5.traj b/src/main/deploy/choreo/CenterLanePCBAEF.5.traj new file mode 100644 index 00000000..e03c4c05 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAEF.5.traj @@ -0,0 +1,572 @@ +{ + "samples": [ + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -2.1027730559805923e-24, + "velocityX": -4.370401644398185e-24, + "velocityY": -7.227687528271155e-24, + "timestamp": 0 + }, + { + "x": 4.144916563575742, + "y": 4.94494062571017, + "heading": -0.0787144976857821, + "angularVelocity": 0.2928853385841611, + "velocityX": 0.27671294348408426, + "velocityY": -0.2689143973713598, + "timestamp": 0.06329568320637513 + }, + { + "x": 4.1805101351641145, + "y": 4.911442857139667, + "heading": -0.04246897747139415, + "angularVelocity": 0.5726381070287113, + "velocityX": 0.5623380582261821, + "velocityY": -0.529226747759122, + "timestamp": 0.12659136641275026 + }, + { + "x": 4.234877563186883, + "y": 4.862193017904391, + "heading": 0.010407773982345214, + "angularVelocity": 0.8353926962338748, + "velocityX": 0.858943695188552, + "velocityY": -0.7780915970951138, + "timestamp": 0.1898870496191254 + }, + { + "x": 4.308888949041241, + "y": 4.798194192640054, + "heading": 0.078455258557249, + "angularVelocity": 1.0750730717770254, + "velocityX": 1.1692959472930267, + "velocityY": -1.0111088469599114, + "timestamp": 0.2531827328255005 + }, + { + "x": 4.4036437297615265, + "y": 4.720901995225723, + "heading": 0.15957624341444238, + "angularVelocity": 1.2816195473030647, + "velocityX": 1.497018057476968, + "velocityY": -1.2211290486006587, + "timestamp": 0.31647841603187565 + }, + { + "x": 4.520513474307112, + "y": 4.632549416340532, + "heading": 0.2505777849388087, + "angularVelocity": 1.4377211353838413, + "velocityX": 1.8464094014836858, + "velocityY": -1.395870530334874, + "timestamp": 0.3797740992382508 + }, + { + "x": 4.661061641006965, + "y": 4.536766015518807, + "heading": 0.3463605077982218, + "angularVelocity": 1.5132583773069228, + "velocityX": 2.2205016136976554, + "velocityY": -1.5132690883424573, + "timestamp": 0.4430697824446259 + }, + { + "x": 4.826402761674467, + "y": 4.439605699543979, + "heading": 0.43881735404698696, + "angularVelocity": 1.4607132993147334, + "velocityX": 2.612202164378375, + "velocityY": -1.535022785962128, + "timestamp": 0.506365465651001 + }, + { + "x": 5.0150915047408, + "y": 4.350093359802789, + "heading": 0.5163398586345646, + "angularVelocity": 1.2247676407064818, + "velocityX": 2.98106811567406, + "velocityY": -1.4141934363728348, + "timestamp": 0.5696611488573762 + }, + { + "x": 5.2211399062787285, + "y": 4.277069011475663, + "heading": 0.5664924385126362, + "angularVelocity": 0.7923538752958722, + "velocityX": 3.255331028912471, + "velocityY": -1.1537018739339473, + "timestamp": 0.6329568320637513 + }, + { + "x": 5.439515498788918, + "y": 4.225186735026024, + "heading": 0.5993129773415641, + "angularVelocity": 0.518527286006478, + "velocityX": 3.450086663859443, + "velocityY": -0.8196811191764456, + "timestamp": 0.6962525152701264 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.6211417582867742, + "angularVelocity": 0.3448699791111725, + "velocityX": 3.577435593728997, + "velocityY": -0.450862665918832, + "timestamp": 0.7595481984765016 + }, + { + "x": 5.882912329849662, + "y": 4.1913117565500215, + "heading": 0.6344672511131947, + "angularVelocity": 0.22398342506550498, + "velocityX": 3.6468128538078894, + "velocityY": -0.08971306223841023, + "timestamp": 0.8190414079518167 + }, + { + "x": 6.101970858853418, + "y": 4.2077445367688195, + "heading": 0.6401776935966378, + "angularVelocity": 0.09598477765454098, + "velocityX": 3.682076171981447, + "velocityY": 0.27621270332737524, + "timestamp": 0.8785346174271318 + }, + { + "x": 6.320243538678639, + "y": 4.246069175734349, + "heading": 0.6401793813797093, + "angularVelocity": 0.0000283693397346701, + "velocityX": 3.6688671152594092, + "velocityY": 0.6441850978208127, + "timestamp": 0.9380278269024469 + }, + { + "x": 6.533654958768337, + "y": 4.305798072358943, + "heading": 0.6401794602105909, + "angularVelocity": 0.0000013250399886292195, + "velocityX": 3.5871559455579396, + "velocityY": 1.003961580680503, + "timestamp": 0.997521036377762 + }, + { + "x": 6.7401111102822036, + "y": 4.386345181993656, + "heading": 0.640179519772512, + "angularVelocity": 0.0000010011549482699353, + "velocityX": 3.470247333009187, + "velocityY": 1.3538874494262292, + "timestamp": 1.0570142458530771 + }, + { + "x": 6.937586723137597, + "y": 4.486920348849836, + "heading": 0.6401795682436345, + "angularVelocity": 8.147336982383455e-7, + "velocityX": 3.3192966827135675, + "velocityY": 1.6905318731864938, + "timestamp": 1.1165074553283922 + }, + { + "x": 7.124144636672047, + "y": 4.606536937733858, + "heading": 0.6401796100323616, + "angularVelocity": 7.024117120417555e-7, + "velocityX": 3.1357849942833846, + "velocityY": 2.010592300179313, + "timestamp": 1.1760006648037074 + }, + { + "x": 7.297954825460976, + "y": 4.744021492057199, + "heading": 0.6401796478532076, + "angularVelocity": 6.357170231929654e-7, + "velocityX": 2.9215130654709105, + "velocityY": 2.310928516646711, + "timestamp": 1.2354938742790225 + }, + { + "x": 7.457312504235246, + "y": 4.898025112547281, + "heading": 0.6401796836083543, + "angularVelocity": 6.009954258140389e-7, + "velocityX": 2.6785860130876458, + "velocityY": 2.588591569496418, + "timestamp": 1.2949870837543376 + }, + { + "x": 7.598334922177398, + "y": 5.05313142847436, + "heading": 0.681855456134976, + "angularVelocity": 0.7005130987917911, + "velocityX": 2.3703951961217746, + "velocityY": 2.6071263812290466, + "timestamp": 1.3544802932296527 + }, + { + "x": 7.725243929451597, + "y": 5.1927289365333404, + "heading": 0.750546465230805, + "angularVelocity": 1.1546025118098764, + "velocityX": 2.133167942920592, + "velocityY": 2.3464443974384026, + "timestamp": 1.4139735027049678 + }, + { + "x": 7.837539775743062, + "y": 5.315957707872993, + "heading": 0.8170716120028277, + "angularVelocity": 1.1181973095538917, + "velocityX": 1.8875405660886297, + "velocityY": 2.071308178301995, + "timestamp": 1.473466712180283 + }, + { + "x": 7.935320352876818, + "y": 5.422866587549723, + "heading": 0.8748533666706206, + "angularVelocity": 0.9712327705528885, + "velocityX": 1.643558617800391, + "velocityY": 1.79699297818334, + "timestamp": 1.532959921655598 + }, + { + "x": 8.018658578193438, + "y": 5.513509965801917, + "heading": 0.9207921483680607, + "angularVelocity": 0.7721684895231855, + "velocityX": 1.400802310912472, + "velocityY": 1.5235920040556004, + "timestamp": 1.5924531311309131 + }, + { + "x": 8.08760540363553, + "y": 5.587927848241841, + "heading": 0.9530800723202797, + "angularVelocity": 0.5427161223436167, + "velocityX": 1.1589024369360976, + "velocityY": 1.250863469902435, + "timestamp": 1.6519463406062282 + }, + { + "x": 8.142197837704975, + "y": 5.6461493904163484, + "heading": 0.9705333871837108, + "angularVelocity": 0.2933665037969271, + "velocityX": 0.917624625581801, + "velocityY": 0.9786250008694746, + "timestamp": 1.7114395500815434 + }, + { + "x": 8.182463827907348, + "y": 5.6881968245095615, + "heading": 0.9723096580243064, + "angularVelocity": 0.029856698878088846, + "velocityX": 0.6768165738155562, + "velocityY": 0.7067602246380643, + "timestamp": 1.7709327595568585 + }, + { + "x": 8.208425076414684, + "y": 5.714088220117355, + "heading": 0.9577673202803244, + "angularVelocity": -0.24443693443730044, + "velocityX": 0.4363733060679247, + "velocityY": 0.43519917375674694, + "timestamp": 1.8304259690321736 + }, + { + "x": 8.220098495483398, + "y": 5.723839282989502, + "heading": 0.9263929393086, + "angularVelocity": -0.5273607063465391, + "velocityX": 0.1962143103669291, + "velocityY": 0.1639021151849818, + "timestamp": 1.8899191785074887 + }, + { + "x": 8.217115474998435, + "y": 5.7169576815494905, + "heading": 0.8762998539218844, + "angularVelocity": -0.8237354847591428, + "velocityX": -0.0490530740172862, + "velocityY": -0.11316171192782937, + "timestamp": 1.9507312793742653 + }, + { + "x": 8.199143140937736, + "y": 5.693225588730865, + "heading": 0.8092972371375105, + "angularVelocity": -1.1017974355327707, + "velocityX": -0.29553877936354017, + "velocityY": -0.39025280298433057, + "timestamp": 2.011543380241042 + }, + { + "x": 8.16609422727989, + "y": 5.652642007461555, + "heading": 0.7267788786337702, + "angularVelocity": -1.356939775596919, + "velocityX": -0.5434594955080971, + "velocityY": -0.6673602899892838, + "timestamp": 2.0723554811078184 + }, + { + "x": 8.117864668551345, + "y": 5.595209979313778, + "heading": 0.6304892819422186, + "angularVelocity": -1.5833953328219617, + "velocityX": -0.7930914742479169, + "velocityY": -0.9444177610899582, + "timestamp": 2.133167581974595 + }, + { + "x": 8.054326469941712, + "y": 5.520944740821225, + "heading": 0.5226315224214131, + "angularVelocity": -1.7736233082473956, + "velocityX": -1.044828211885481, + "velocityY": -1.2212246811740535, + "timestamp": 2.1939796828413716 + }, + { + "x": 7.975314688312655, + "y": 5.4298894198944145, + "heading": 0.40611975005056616, + "angularVelocity": -1.9159307228358207, + "velocityX": -1.2992772902575926, + "velocityY": -1.4973224017747822, + "timestamp": 2.254791783708148 + }, + { + "x": 7.880609390155109, + "y": 5.32214652648837, + "heading": 0.28521663340036824, + "angularVelocity": -1.9881424079570305, + "velocityX": -1.5573429762774083, + "velocityY": -1.7717344388756155, + "timestamp": 2.3156038845749247 + }, + { + "x": 7.769927978869046, + "y": 5.19796449901651, + "heading": 0.1670617076218344, + "angularVelocity": -1.9429508945494507, + "velocityX": -1.8200557078029118, + "velocityY": -2.0420611309566765, + "timestamp": 2.3764159854417013 + }, + { + "x": 7.64302287953294, + "y": 5.0580816434833595, + "heading": 0.06529420310247565, + "angularVelocity": -1.6734745728042795, + "velocityX": -2.086839585005026, + "velocityY": -2.300247048520789, + "timestamp": 2.437228086308478 + }, + { + "x": 7.499740913666761, + "y": 4.907357720220894, + "heading": 0.011577036219802711, + "angularVelocity": -0.8833302273235669, + "velocityX": -2.3561423437758418, + "velocityY": -2.478518602616007, + "timestamp": 2.4980401871752544 + }, + { + "x": 7.338349817846371, + "y": 4.749390248030348, + "heading": 0.009577494655868879, + "angularVelocity": -0.03288065262396252, + "velocityX": -2.6539306078893232, + "velocityY": -2.597632213638097, + "timestamp": 2.558852288042031 + }, + { + "x": 7.161261092895849, + "y": 4.608134428437688, + "heading": 0.009577439039588426, + "angularVelocity": -9.145594324092402e-7, + "velocityX": -2.9120639219236804, + "velocityY": -2.322824200764184, + "timestamp": 2.6196643889088076 + }, + { + "x": 6.970797581586152, + "y": 4.485507869855581, + "heading": 0.009577379210160273, + "angularVelocity": -9.83840835982591e-7, + "velocityX": -3.132000187379736, + "velocityY": -2.016482851838167, + "timestamp": 2.680476489775584 + }, + { + "x": 6.76891141017589, + "y": 4.382767451700059, + "heading": 0.009577307882139067, + "angularVelocity": -0.000001172924799341256, + "velocityX": -3.3198355020252337, + "velocityY": -1.6894732576432487, + "timestamp": 2.7412885906423607 + }, + { + "x": 6.5576717954256925, + "y": 4.300966222617904, + "heading": 0.009577215489609455, + "angularVelocity": -0.0000015193115891924993, + "velocityX": -3.473644418451679, + "velocityY": -1.3451472308342272, + "timestamp": 2.8021006915091373 + }, + { + "x": 6.33924383214455, + "y": 4.24094261156842, + "heading": 0.009577080518783282, + "angularVelocity": -0.000002219473168173005, + "velocityX": -3.5918503088663734, + "velocityY": -0.9870339980685793, + "timestamp": 2.862912792375914 + }, + { + "x": 6.115866515513707, + "y": 4.203311883044666, + "heading": 0.009576062223691489, + "angularVelocity": -0.000016744941833606887, + "velocityX": -3.673237948483439, + "velocityY": -0.6188032971627346, + "timestamp": 2.9237248932426905 + }, + { + "x": 5.890604902143948, + "y": 4.188526246713883, + "heading": 0.006634290856698977, + "angularVelocity": -0.04837476957813366, + "velocityX": -3.704223504187922, + "velocityY": -0.24313641725970658, + "timestamp": 2.984536994109467 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 1.4617092924271447e-24, + "angularVelocity": -0.10909491305411462, + "velocityX": -3.6942182578974845, + "velocityY": 0.1335725575137668, + "timestamp": 3.0453490949762436 + }, + { + "x": 5.418474509439448, + "y": 4.234112857220888, + "heading": -0.009420810708608588, + "angularVelocity": -0.13872442581307998, + "velocityX": -3.644180551162595, + "velocityY": 0.551666082669452, + "timestamp": 3.1132593469264105 + }, + { + "x": 5.180225703278332, + "y": 4.298642990605948, + "heading": -0.020608459493002298, + "angularVelocity": -0.164741676891483, + "velocityX": -3.5082892393911758, + "velocityY": 0.9502266820098406, + "timestamp": 3.1811695988765774 + }, + { + "x": 4.959251461441715, + "y": 4.385964306023392, + "heading": -0.03296814464402756, + "angularVelocity": -0.1820002841411158, + "velocityX": -3.253915800500449, + "velocityY": 1.2858340664310908, + "timestamp": 3.249079850826744 + }, + { + "x": 4.763381292287379, + "y": 4.486986357550731, + "heading": -0.04558833602673758, + "angularVelocity": -0.18583632103103812, + "velocityX": -2.8842503676479536, + "velocityY": 1.4875817513012504, + "timestamp": 3.316990102776911 + }, + { + "x": 4.59599065624695, + "y": 4.59062036623284, + "heading": -0.05760567577443394, + "angularVelocity": -0.17695913949067177, + "velocityX": -2.464880209298302, + "velocityY": 1.5260436488758087, + "timestamp": 3.384900354727078 + }, + { + "x": 4.456604919229509, + "y": 4.6884604808720365, + "heading": -0.06845454912247914, + "angularVelocity": -0.1597531011371608, + "velocityX": -2.0524991884836625, + "velocityY": 1.440726721364413, + "timestamp": 3.452810606677245 + }, + { + "x": 4.343601982653564, + "y": 4.775267107804887, + "heading": -0.07780582197928933, + "angularVelocity": -0.13770045888906648, + "velocityX": -1.6640040837849672, + "velocityY": 1.2782551152446038, + "timestamp": 3.5207208586274117 + }, + { + "x": 4.255382643641957, + "y": 4.8478074673531255, + "heading": -0.0854646750430389, + "angularVelocity": -0.11277904062805744, + "velocityX": -1.2990577486937123, + "velocityY": 1.0681798029768506, + "timestamp": 3.5886311105775786 + }, + { + "x": 4.190618071556959, + "y": 4.903989277014325, + "heading": -0.09130900754804393, + "angularVelocity": -0.08605964986396453, + "velocityX": -0.9536788662265924, + "velocityY": 0.8272949672227069, + "timestamp": 3.6565413625277454 + }, + { + "x": 4.148242739278531, + "y": 4.942382522588693, + "heading": -0.09525748910421675, + "angularVelocity": -0.058142643309145876, + "velocityX": -0.6239902085700291, + "velocityY": 0.5653527187992231, + "timestamp": 3.7244516144779123 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.029382694528135574, + "velocityX": -0.30688901769875077, + "velocityY": 0.28831027812257853, + "timestamp": 3.792361866428079 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 4.130042429684847e-26, + "velocityX": 1.7558924014550056e-25, + "velocityY": 1.7099094571039823e-25, + "timestamp": 3.860272118378246 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAEF.traj b/src/main/deploy/choreo/CenterLanePCBAEF.traj new file mode 100644 index 00000000..64611595 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAEF.traj @@ -0,0 +1,1697 @@ +{ + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -8.290804854356393e-28, + "angularVelocity": -1.4522370736587165e-27, + "velocityX": 4.9023959214285697e-26, + "velocityY": -8.208515528255786e-26, + "timestamp": 0 + }, + { + "x": 1.310478129945639, + "y": 5.567275491596602, + "heading": -0.01363283868540513, + "angularVelocity": -0.19128951975675812, + "velocityX": 0.2876978871987184, + "velocityY": -0.332249849655198, + "timestamp": 0.07126808987100033 + }, + { + "x": 1.3514857086951306, + "y": 5.519918181736233, + "heading": -0.04088194207773102, + "angularVelocity": -0.38234648131651156, + "velocityX": 0.5753988752009218, + "velocityY": -0.6644952873872267, + "timestamp": 0.14253617974200067 + }, + { + "x": 1.4129978772139058, + "y": 5.448882692998019, + "heading": -0.08170986553530027, + "angularVelocity": -0.5728780374424279, + "velocityX": 0.8631095435575179, + "velocityY": -0.9967362513404223, + "timestamp": 0.213804269613001 + }, + { + "x": 1.495015808643516, + "y": 5.354169496620742, + "heading": -0.13606373855442885, + "angularVelocity": -0.7626677397628091, + "velocityX": 1.1508366728793693, + "velocityY": -1.3289706030947979, + "timestamp": 0.28507235948400134 + }, + { + "x": 1.59754112837037, + "y": 5.235779380543083, + "heading": -0.20388316047242275, + "angularVelocity": -0.9516099286616388, + "velocityX": 1.4385866088516093, + "velocityY": -1.6611938988676576, + "timestamp": 0.35634044935500164 + }, + { + "x": 1.7205758185191462, + "y": 5.093713591292308, + "heading": -0.28510990320243984, + "angularVelocity": -1.1397350886917637, + "velocityX": 1.7263643570562468, + "velocityY": -1.99339970396181, + "timestamp": 0.42760853922600195 + }, + { + "x": 1.864122091418882, + "y": 4.927973933585744, + "heading": -0.3796975284198671, + "angularVelocity": -1.3272086481991685, + "velocityX": 2.0141731476115536, + "velocityY": -2.325580186119234, + "timestamp": 0.49887662909700226 + }, + { + "x": 2.0360573445839716, + "y": 4.766214838118459, + "heading": -0.469084063757855, + "angularVelocity": -1.2542294244139716, + "velocityX": 2.4125138400131556, + "velocityY": -2.2697268266917123, + "timestamp": 0.5701447189680026 + }, + { + "x": 2.1874785084173087, + "y": 4.628126850696153, + "heading": -0.5451442530827014, + "angularVelocity": -1.0672404643160782, + "velocityX": 2.1246698783062508, + "velocityY": -1.937585077308154, + "timestamp": 0.6414128088390029 + }, + { + "x": 2.3183845436377304, + "y": 4.513708759426245, + "heading": -0.6078811711978229, + "angularVelocity": -0.8802946483998556, + "velocityX": 1.8368113338995, + "velocityY": -1.6054603326258816, + "timestamp": 0.7126808987100032 + }, + { + "x": 2.428774775812145, + "y": 4.422959692661252, + "heading": -0.657294007593887, + "angularVelocity": -0.6933374598014952, + "velocityX": 1.5489433261678196, + "velocityY": -1.273347818487273, + "timestamp": 0.7839489885810035 + }, + { + "x": 2.518648765365177, + "y": 4.355879041329867, + "heading": -0.6933804923945769, + "angularVelocity": -0.5063484213763644, + "velocityX": 1.2610691505231797, + "velocityY": -0.9412438505480457, + "timestamp": 0.8552170784520038 + }, + { + "x": 2.588006261722225, + "y": 4.3124664184627175, + "heading": -0.7161379527262285, + "angularVelocity": -0.3193218784570213, + "velocityX": 0.9731914589346954, + "velocityY": -0.6091453123793419, + "timestamp": 0.9264851683230041 + }, + { + "x": 2.6368471855998425, + "y": 4.2927216346124935, + "heading": -0.7255636953209265, + "angularVelocity": -0.13225754488101377, + "velocityX": 0.6853126548785442, + "velocityY": -0.2770494324453428, + "timestamp": 0.9977532581940044 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05484334197651896, + "velocityX": 0.3974350607881677, + "velocityY": 0.05504641764912788, + "timestamp": 1.0690213480650048 + }, + { + "x": 2.6726522356618694, + "y": 4.325520885096074, + "heading": -0.7036434508654623, + "angularVelocity": 0.24661816834169437, + "velocityX": 0.10242556780992029, + "velocityY": 0.395376841972494, + "timestamp": 1.1420559681490368 + }, + { + "x": 2.658587249318106, + "y": 4.379253606612043, + "heading": -0.671632044096526, + "angularVelocity": 0.43830455655283357, + "velocityX": -0.19257971531283274, + "velocityY": 0.7357157667712343, + "timestamp": 1.2150905882330687 + }, + { + "x": 2.6229771758212266, + "y": 4.457843745767866, + "heading": -0.6256270362766762, + "angularVelocity": 0.6299068546795684, + "velocityX": -0.487577993229883, + "velocityY": 1.0760669264165268, + "timestamp": 1.2881252083171006 + }, + { + "x": 2.5658227847056265, + "y": 4.561292508951193, + "heading": -0.5656323531358255, + "angularVelocity": 0.8214554011758, + "velocityX": -0.7825657345768313, + "velocityY": 1.4164346040864118, + "timestamp": 1.3611598284011326 + }, + { + "x": 2.4871252028275452, + "y": 4.6896014337350325, + "heading": -0.4916452107859868, + "angularVelocity": 1.0130420650468346, + "velocityX": -1.077538046854127, + "velocityY": 1.7568233344160629, + "timestamp": 1.4341944484851645 + }, + { + "x": 2.3868860574552526, + "y": 4.8427723316162075, + "heading": -0.4036473210602506, + "angularVelocity": 1.2048791329986765, + "velocityX": -1.3724880783518802, + "velocityY": 2.097236868007809, + "timestamp": 1.5072290685691965 + }, + { + "x": 2.2651076003378585, + "y": 5.020807045399031, + "heading": -0.30159033464974355, + "angularVelocity": 1.3973782062956255, + "velocityX": -1.6674072785930696, + "velocityY": 2.4376756335280563, + "timestamp": 1.5802636886532284 + }, + { + "x": 2.1359139807220826, + "y": 5.170004552864557, + "heading": -0.21560374484097594, + "angularVelocity": 1.1773401396465593, + "velocityX": -1.7689366969682099, + "velocityY": 2.0428326633843468, + "timestamp": 1.6532983087372604 + }, + { + "x": 2.0282550130804604, + "y": 5.294335941405344, + "heading": -0.14382079923066451, + "angularVelocity": 0.9828619020365901, + "velocityX": -1.4740812989477097, + "velocityY": 1.7023623645571657, + "timestamp": 1.7263329288212923 + }, + { + "x": 1.9421291374446024, + "y": 5.393801086818423, + "heading": -0.08631852593947899, + "angularVelocity": 0.7873289848708013, + "velocityX": -1.1792472602275945, + "velocityY": 1.3618903651259822, + "timestamp": 1.7993675489053242 + }, + { + "x": 1.8775352248155426, + "y": 5.468400016112322, + "heading": -0.04315881325455388, + "angularVelocity": 0.590948684819152, + "velocityX": -0.8844286799156282, + "velocityY": 1.0214187354992301, + "timestamp": 1.8724021689893562 + }, + { + "x": 1.83447263299525, + "y": 5.518132767413434, + "heading": -0.014381451378905535, + "angularVelocity": 0.39402357186958425, + "velocityX": -0.5896188926668684, + "velocityY": 0.6809476279042802, + "timestamp": 1.9454367890733881 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -8.684305119206267e-26, + "angularVelocity": 0.1969127978259977, + "velocityX": -0.2948114112163433, + "velocityY": 0.34047551881670374, + "timestamp": 2.0184714091574203 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -4.2442319768143513e-26, + "angularVelocity": 2.0248829096214925e-26, + "velocityX": -2.1409957925018854e-22, + "velocityY": -3.6446450287003043e-25, + "timestamp": 2.0915060292414522 + }, + { + "x": 1.8720264471920118, + "y": 5.543082773692806, + "heading": 1.803287488569909e-20, + "angularVelocity": 1.8474379021994642e-19, + "velocityX": 0.6053171264251317, + "velocityY": 0.0008555041789240009, + "timestamp": 2.189116440432573 + }, + { + "x": 1.9901969516410831, + "y": 5.543249785918246, + "heading": 2.6710707578067816e-20, + "angularVelocity": 8.890273677603703e-20, + "velocityX": 1.210634224434258, + "velocityY": 0.0017110083176872164, + "timestamp": 2.2867268516236936 + }, + { + "x": 2.1674527004558795, + "y": 5.543500304245301, + "heading": 2.745376939135264e-20, + "angularVelocity": 7.612526210817117e-21, + "velocityX": 1.8159512561393767, + "velocityY": 0.0025665123627419384, + "timestamp": 2.3843372628148143 + }, + { + "x": 2.4037936612766035, + "y": 5.543834328628234, + "heading": 2.868215450901532e-21, + "angularVelocity": -2.5187429947361393e-19, + "velocityX": 2.4212679563245554, + "velocityY": 0.0034220159392543354, + "timestamp": 2.481947674005935 + }, + { + "x": 2.5810494100914, + "y": 5.544084846955289, + "heading": 4.197057323633921e-22, + "angularVelocity": -2.508451391919409e-20, + "velocityX": 1.815951256139377, + "velocityY": 0.0025665123627419384, + "timestamp": 2.5795580851970557 + }, + { + "x": 2.699219914540471, + "y": 5.544251859180729, + "heading": 1.3007601681932514e-20, + "angularVelocity": 1.2896058726029048e-19, + "velocityX": 1.2106342244342583, + "velocityY": 0.0017110083176872164, + "timestamp": 2.6771684963881763 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.0578906521828803e-28, + "angularVelocity": -1.3326039434355583e-19, + "velocityX": 0.6053171264251317, + "velocityY": 0.0008555041789240007, + "timestamp": 2.774778907579297 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -2.171991433780983e-28, + "angularVelocity": -1.132446733749095e-28, + "velocityX": 2.142784500904642e-22, + "velocityY": 2.9856908340513677e-25, + "timestamp": 2.8723893187704177 + }, + { + "x": 2.7303741375078747, + "y": 5.568056872076371, + "heading": 0.006778509195380263, + "angularVelocity": 0.08813497362119757, + "velocityX": -0.36316254475214277, + "velocityY": 0.3084298205009288, + "timestamp": 2.949299867635096 + }, + { + "x": 2.674756164732603, + "y": 5.615779956875509, + "heading": 0.02059740285114023, + "angularVelocity": 0.17967488023097908, + "velocityX": -0.7231514219607195, + "velocityY": 0.6205011601608276, + "timestamp": 3.0262104164997745 + }, + { + "x": 2.591861664751003, + "y": 5.687962530350053, + "heading": 0.041894082151838305, + "angularVelocity": 0.2769019284749726, + "velocityX": -1.07780403605558, + "velocityY": 0.9385263080302103, + "timestamp": 3.103120965364453 + }, + { + "x": 2.4825229880731845, + "y": 5.785486691208239, + "heading": 0.07152941986388306, + "angularVelocity": 0.38532214565504064, + "velocityX": -1.4216343309445878, + "velocityY": 1.2680206070272069, + "timestamp": 3.1800315142291313 + }, + { + "x": 2.3492932233323143, + "y": 5.910724338795779, + "heading": 0.11194462213648768, + "angularVelocity": 0.5254832122406747, + "velocityX": -1.7322690672157808, + "velocityY": 1.628354620221102, + "timestamp": 3.2569420630938097 + }, + { + "x": 2.2322888283047253, + "y": 6.068263103797439, + "heading": 0.1780934351870072, + "angularVelocity": 0.8600746455067713, + "velocityX": -1.5213049023152154, + "velocityY": 2.0483375470229004, + "timestamp": 3.333852611958488 + }, + { + "x": 2.1483049194140715, + "y": 6.209825434589826, + "heading": 0.24392342400918363, + "angularVelocity": 0.8559292554003495, + "velocityX": -1.0919686587911486, + "velocityY": 1.8406100708169557, + "timestamp": 3.4107631608231666 + }, + { + "x": 2.0954357223517674, + "y": 6.331973751888774, + "heading": 0.3066259975705838, + "angularVelocity": 0.815266234437141, + "velocityX": -0.6874115169211118, + "velocityY": 1.5881867845445201, + "timestamp": 3.487673709687845 + }, + { + "x": 2.072970005140484, + "y": 6.433621819948826, + "heading": 0.3652444947638923, + "angularVelocity": 0.7621645932659495, + "velocityX": -0.2921018968517744, + "velocityY": 1.3216401333827206, + "timestamp": 3.5645842585525234 + }, + { + "x": 2.080539806403903, + "y": 6.514239906298576, + "heading": 0.4192892776661922, + "angularVelocity": 0.7026966222460566, + "velocityX": 0.09842344613530404, + "velocityY": 1.0482058383381783, + "timestamp": 3.6414948074172018 + }, + { + "x": 2.1179206829564796, + "y": 6.573514734989947, + "heading": 0.46845944965014935, + "angularVelocity": 0.6393163578961119, + "velocityX": 0.48603055243236887, + "velocityY": 0.7706982925796543, + "timestamp": 3.71840535628188 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.5732759758018011, + "velocityX": 0.871673332481117, + "velocityY": 0.4905009632033997, + "timestamp": 3.7953159051465586 + }, + { + "x": 2.2917884862212223, + "y": 6.6722766025245654, + "heading": 0.5461831512487826, + "angularVelocity": 0.4115075108626725, + "velocityX": 1.3070625376553004, + "velocityY": 0.7468097995934098, + "timestamp": 3.8770464465363763 + }, + { + "x": 2.4341165316393667, + "y": 6.754285323763742, + "heading": 0.5638405564624906, + "angularVelocity": 0.21604414841069292, + "velocityX": 1.741430351468047, + "velocityY": 1.003403621762784, + "timestamp": 3.958776987926194 + }, + { + "x": 2.541260883257264, + "y": 6.816945409398408, + "heading": 0.5395665848363949, + "angularVelocity": -0.2970000101959424, + "velocityX": 1.3109463096159768, + "velocityY": 0.7666667144146957, + "timestamp": 4.040507529316012 + }, + { + "x": 2.6126555762150003, + "y": 6.858741818277979, + "heading": 0.521795194780163, + "angularVelocity": -0.21743879037176994, + "velocityX": 0.8735375019384195, + "velocityY": 0.5113927813131806, + "timestamp": 4.12223807070583 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.1131128587056523, + "velocityX": 0.4366659015377113, + "velocityY": 0.2557557080208232, + "timestamp": 4.203968612095648 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 1.4611171491066952e-26, + "velocityX": -3.269712364266787e-25, + "velocityY": -3.777106652858518e-25, + "timestamp": 4.285699153485465 + }, + { + "x": 2.660849769218119, + "y": 6.863303091211252, + "heading": 0.49944854128498545, + "angularVelocity": -0.22618144187200803, + "velocityX": 0.2158817205646462, + "velocityY": -0.2821127758769456, + "timestamp": 4.343625560738757 + }, + { + "x": 2.685869953878005, + "y": 6.830605661533361, + "heading": 0.4737478609731425, + "angularVelocity": -0.4436781345589612, + "velocityX": 0.4319305450876828, + "velocityY": -0.5644650035849392, + "timestamp": 4.401551967992049 + }, + { + "x": 2.7234170079843754, + "y": 6.781535741371895, + "heading": 0.43609134859082577, + "angularVelocity": -0.6500750550203873, + "velocityX": 0.6481854457534552, + "velocityY": -0.8471079510749036, + "timestamp": 4.45947837524534 + }, + { + "x": 2.7735061222918693, + "y": 6.716073137055524, + "heading": 0.3873132758064447, + "angularVelocity": -0.8420697070179436, + "velocityX": 0.8647025887255888, + "velocityY": -1.1300995076412468, + "timestamp": 4.517404782498632 + }, + { + "x": 2.8361568283253176, + "y": 6.63419369001641, + "heading": 0.3285224732338341, + "angularVelocity": -1.0149223015944975, + "velocityX": 1.0815569099512647, + "velocityY": -1.4135081204170898, + "timestamp": 4.575331189751924 + }, + { + "x": 2.9113939859812374, + "y": 6.535868324439057, + "heading": 0.2612561141808071, + "angularVelocity": -1.1612382373188714, + "velocityX": 1.298840394622348, + "velocityY": -1.6974186772435556, + "timestamp": 4.6332575970052154 + }, + { + "x": 2.999247861116449, + "y": 6.4210622556853, + "heading": 0.18781374862867117, + "angularVelocity": -1.267856389418368, + "velocityX": 1.5166463673650283, + "velocityY": -1.9819297311457886, + "timestamp": 4.691184004258507 + }, + { + "x": 3.09974782280069, + "y": 6.289740190661489, + "heading": 0.11213274399438687, + "angularVelocity": -1.3065026509128828, + "velocityX": 1.734959346689516, + "velocityY": -2.2670500597350514, + "timestamp": 4.749110411511799 + }, + { + "x": 3.212855329084297, + "y": 6.141935125074281, + "heading": 0.04279640937177349, + "angularVelocity": -1.1969728127524046, + "velocityX": 1.9526069654040226, + "velocityY": -2.551600774080633, + "timestamp": 4.8070368187650905 + }, + { + "x": 3.3365134053253396, + "y": 5.978631674871108, + "heading": 0.010716435347090821, + "angularVelocity": -0.5538056914941833, + "velocityX": 2.1347444473870243, + "velocityY": -2.819153784026439, + "timestamp": 4.864963226018382 + }, + { + "x": 3.4677690523177653, + "y": 5.807368153650049, + "heading": 0.010716404660462071, + "angularVelocity": -5.297519767950936e-7, + "velocityX": 2.2659034664188993, + "velocityY": -2.95657074798692, + "timestamp": 4.922889633271674 + }, + { + "x": 3.5990247641115705, + "y": 5.636104682092312, + "heading": 0.010716373974269203, + "angularVelocity": -5.297444520108962e-7, + "velocityX": 2.265904585103496, + "velocityY": -2.9565698906349174, + "timestamp": 4.9808160405249655 + }, + { + "x": 3.730280476023843, + "y": 5.4648412106253685, + "heading": 0.010716343288076406, + "angularVelocity": -5.297444507915692e-7, + "velocityX": 2.265904587148631, + "velocityY": -2.9565698890675334, + "timestamp": 5.038742447778257 + }, + { + "x": 3.8615392378596742, + "y": 5.293580076652075, + "heading": 0.010716312601908511, + "angularVelocity": -5.297440208932243e-7, + "velocityX": 2.265957238844176, + "velocityY": -2.956529536251564, + "timestamp": 5.096668855031549 + }, + { + "x": 4.007190539027271, + "y": 5.134379144193632, + "heading": 0.010716281749960886, + "angularVelocity": -5.326059234198986e-7, + "velocityX": 2.5144197279613127, + "velocityY": -2.748330856466123, + "timestamp": 5.1545952622848406 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.010716249762746122, + "angularVelocity": -5.522043620508212e-7, + "velocityX": 2.76745156437766, + "velocityY": -2.4933593359962067, + "timestamp": 5.212521669538132 + }, + { + "x": 4.289284709391487, + "y": 4.8946057330854265, + "heading": 0.010716218824995563, + "angularVelocity": -7.451052814449057e-7, + "velocityX": 2.933087406208596, + "velocityY": -2.2962197715760517, + "timestamp": 5.254042983814184 + }, + { + "x": 4.417365814004084, + "y": 4.807904620313661, + "heading": 0.010716189360474368, + "angularVelocity": -7.096240017556084e-7, + "velocityX": 3.084707380914231, + "velocityY": -2.08811099271423, + "timestamp": 5.295564298090236 + }, + { + "x": 4.551130165096487, + "y": 4.730258301325536, + "heading": 0.010716160700942695, + "angularVelocity": -6.902366211867451e-7, + "velocityX": 3.2215827804264254, + "velocityY": -1.870035193777778, + "timestamp": 5.3370856123662875 + }, + { + "x": 4.688893145382921, + "y": 4.6599503090237695, + "heading": 0.010716132275769374, + "angularVelocity": -6.845923308774473e-7, + "velocityX": 3.3178858301672403, + "velocityY": -1.693298815984659, + "timestamp": 5.378606926642339 + }, + { + "x": 4.826657112545421, + "y": 4.589644250461509, + "heading": 0.0107161038506266, + "angularVelocity": -6.845915951833867e-7, + "velocityX": 3.317909598106278, + "velocityY": -1.693252243771356, + "timestamp": 5.420128240918391 + }, + { + "x": 4.964421079807684, + "y": 4.5193381920947315, + "heading": 0.01071607542548375, + "angularVelocity": -6.845915970495296e-7, + "velocityX": 3.317909600508956, + "velocityY": -1.6932522390633336, + "timestamp": 5.461649555194443 + }, + { + "x": 5.102185047070045, + "y": 4.4490321337281475, + "heading": 0.010716047000340928, + "angularVelocity": -6.845915962954477e-7, + "velocityX": 3.3179096005113364, + "velocityY": -1.6932522390586708, + "timestamp": 5.503170869470495 + }, + { + "x": 5.2399490152099615, + "y": 4.378726077081124, + "heading": 0.01071601857519813, + "angularVelocity": -6.84591595754071e-7, + "velocityX": 3.317909621646396, + "velocityY": -1.6932521976447459, + "timestamp": 5.544692183746546 + }, + { + "x": 5.377721663167995, + "y": 4.308437031039157, + "heading": 0.010715990149891885, + "angularVelocity": -6.845955322087529e-7, + "velocityX": 3.3181186665253675, + "velocityY": -1.6928425139593435, + "timestamp": 5.586213498022598 + }, + { + "x": 5.519903819666949, + "y": 4.247559392833526, + "heading": 0.010715961486816888, + "angularVelocity": -6.903219586500289e-7, + "velocityX": 3.4243173410567787, + "velocityY": -1.4661780164493292, + "timestamp": 5.62773481229865 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.01071593198598864, + "angularVelocity": -7.104984214494266e-7, + "velocityX": 3.5174201900946844, + "velocityY": -1.22612492322878, + "timestamp": 5.669256126574702 + }, + { + "x": 5.815510824369738, + "y": 4.155881163603153, + "heading": 0.010715902600881807, + "angularVelocity": -7.061178508232372e-7, + "velocityX": 3.593873172685508, + "velocityY": -0.97964420643565, + "timestamp": 5.7108711442346545 + }, + { + "x": 5.9675336119930025, + "y": 4.125565856378816, + "heading": 0.010715874047412989, + "angularVelocity": -6.861337666816658e-7, + "velocityX": 3.653075167851236, + "velocityY": -0.7284703678862195, + "timestamp": 5.752486161894607 + }, + { + "x": 6.121239827015959, + "y": 4.105457516553604, + "heading": 0.010715845747967176, + "angularVelocity": -6.800296480422641e-7, + "velocityX": 3.6935275692763176, + "velocityY": -0.483199117912718, + "timestamp": 5.79410117955456 + }, + { + "x": 6.274947903964394, + "y": 4.085363414233198, + "heading": 0.010715817448685471, + "angularVelocity": -6.800257045912017e-7, + "velocityX": 3.693572310948509, + "velocityY": -0.4828569937083839, + "timestamp": 5.835716197214513 + }, + { + "x": 6.4286559810993475, + "y": 4.0652693133395585, + "heading": 0.01071578914940375, + "angularVelocity": -6.800257049819807e-7, + "velocityX": 3.693572315430539, + "velocityY": -0.48285695942348605, + "timestamp": 5.877331214874466 + }, + { + "x": 6.582364058235009, + "y": 4.045175212451323, + "heading": 0.010715760850122012, + "angularVelocity": -6.800257053652886e-7, + "velocityX": 3.693572315447514, + "velocityY": -0.482856959293641, + "timestamp": 5.918946232534418 + }, + { + "x": 6.736072142233211, + "y": 4.025081164057568, + "heading": 0.010715732550840138, + "angularVelocity": -6.80025708616856e-7, + "velocityX": 3.693572480352915, + "velocityY": -0.4828556978624376, + "timestamp": 5.960561250194371 + }, + { + "x": 6.88984765379033, + "y": 4.0055098097098245, + "heading": 0.01071570424372189, + "angularVelocity": -6.802140150471018e-7, + "velocityX": 3.69519275021481, + "velocityY": -0.47029547140088923, + "timestamp": 6.002176267854324 + }, + { + "x": 7.0446085905714275, + "y": 3.9966217347495547, + "heading": 0.010715671283588182, + "angularVelocity": -7.920249842947072e-7, + "velocityX": 3.7188723082058996, + "velocityY": -0.21357854592052494, + "timestamp": 6.043791285514277 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -1.8890515062078487e-25, + "angularVelocity": -0.2574952958364392, + "velocityX": 3.6592868466916966, + "velocityY": 0.030155204912350568, + "timestamp": 6.08540630317423 + }, + { + "x": 7.411732272025267, + "y": 4.005484051327251, + "heading": -0.0035958107444463492, + "angularVelocity": -0.054616477872747704, + "velocityX": 3.2632237151822827, + "velocityY": 0.11554829109191134, + "timestamp": 6.151243773558613 + }, + { + "x": 7.5997547622463255, + "y": 4.014664246157471, + "heading": -0.0036926162217638513, + "angularVelocity": -0.0014703705466251413, + "velocityX": 2.855858360342751, + "velocityY": 0.139437235006489, + "timestamp": 6.217081243942996 + }, + { + "x": 7.760906466600038, + "y": 4.024479422852048, + "heading": -0.002491126392030619, + "angularVelocity": 0.018249331614937442, + "velocityX": 2.44772017230992, + "velocityY": 0.149081923064063, + "timestamp": 6.282918714327379 + }, + { + "x": 7.8951789466062605, + "y": 4.0345153008905195, + "heading": -0.0009601367346007951, + "angularVelocity": 0.023254077784145503, + "velocityX": 2.0394538128863453, + "velocityY": 0.1524341378834605, + "timestamp": 6.348756184711762 + }, + { + "x": 8.002571224633423, + "y": 4.044538615665461, + "heading": 0.0003551372183222516, + "angularVelocity": 0.01997758943720037, + "velocityX": 1.6311726042961088, + "velocityY": 0.15224331549225237, + "timestamp": 6.414593655096145 + }, + { + "x": 8.08308410390753, + "y": 4.05439979104189, + "heading": 0.0011050703957921194, + "angularVelocity": 0.01139067423294795, + "velocityX": 1.222903595840534, + "velocityY": 0.14978059331343505, + "timestamp": 6.4804311254805285 + }, + { + "x": 8.13671880253707, + "y": 4.063994763416068, + "heading": 0.0010463784362104483, + "angularVelocity": -0.0008914674157285515, + "velocityX": 0.8146530891360332, + "velocityY": 0.14573725749423916, + "timestamp": 6.546268595864912 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -6.673758454276996e-26, + "angularVelocity": -0.01589335723412987, + "velocityX": 0.4064214467340168, + "velocityY": 0.14053080109998423, + "timestamp": 6.612106066249295 + }, + { + "x": 8.158055062338372, + "y": 4.083508095993073, + "heading": -0.002757938692971577, + "angularVelocity": -0.035856325054586746, + "velocityX": -0.07048563935601859, + "velocityY": 0.13340643741083522, + "timestamp": 6.6890224454034035 + }, + { + "x": 8.11595159662588, + "y": 4.09322125310912, + "heading": -0.007051362960883781, + "angularVelocity": -0.05581937573153214, + "velocityX": -0.5473927162917097, + "velocityY": 0.12628203801150484, + "timestamp": 6.765938824557512 + }, + { + "x": 8.037166166331591, + "y": 4.102386423514656, + "heading": -0.012880297853755818, + "angularVelocity": -0.07578275208708442, + "velocityX": -1.0242997806284444, + "velocityY": 0.11915759044211331, + "timestamp": 6.842855203711621 + }, + { + "x": 7.921698772840109, + "y": 4.111003602378457, + "heading": -0.020244795899094583, + "angularVelocity": -0.0957468113596885, + "velocityX": -1.501206826963765, + "velocityY": 0.11203308006134147, + "timestamp": 6.91977158286573 + }, + { + "x": 7.769549418244295, + "y": 4.119072783408171, + "heading": -0.029144948174511114, + "angularVelocity": -0.11571205474433797, + "velocityX": -1.9781138460895251, + "velocityY": 0.10490848787287595, + "timestamp": 6.996687962019839 + }, + { + "x": 7.5807181060109095, + "y": 4.1265939580705435, + "heading": -0.0395808971630345, + "angularVelocity": -0.1356791505696594, + "velocityX": -2.4550208201434414, + "velocityY": 0.09778378474242518, + "timestamp": 7.073604341173947 + }, + { + "x": 7.355204842942577, + "y": 4.133567113261441, + "heading": -0.051552849562924184, + "angularVelocity": -0.1556489337063405, + "velocityX": -2.931927705755565, + "velocityY": 0.0906589112434205, + "timestamp": 7.150520720328056 + }, + { + "x": 7.093009648902004, + "y": 4.139992219744277, + "heading": -0.06506107751515841, + "angularVelocity": -0.17562225498380826, + "velocityX": -3.408834333130018, + "velocityY": 0.08353365763568768, + "timestamp": 7.227437099482165 + }, + { + "x": 6.806528037248827, + "y": 4.135713032696913, + "heading": -0.06506107911297748, + "angularVelocity": -2.077345661840568e-8, + "velocityX": -3.7245852548412777, + "velocityY": -0.05563427574756843, + "timestamp": 7.304353478636274 + }, + { + "x": 6.520046430966287, + "y": 4.131433486105525, + "heading": -0.06506108071074132, + "angularVelocity": -2.0772738551017004e-8, + "velocityX": -3.7245851850169447, + "velocityY": -0.0556389502268829, + "timestamp": 7.381269857790382 + }, + { + "x": 6.233564817210998, + "y": 4.127154439784588, + "heading": -0.06506108230850521, + "angularVelocity": -2.07727392722992e-8, + "velocityX": -3.724585282171128, + "velocityY": -0.055632446144705854, + "timestamp": 7.458186236944491 + }, + { + "x": 5.94752903267143, + "y": 4.143693939737266, + "heading": -0.06506108392885115, + "angularVelocity": -2.1066331425696132e-8, + "velocityX": -3.7187889976784825, + "velocityY": 0.21503222245471107, + "timestamp": 7.5351026160986 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.06506108566326727, + "angularVelocity": -2.2549372865536112e-8, + "velocityX": -3.660823701626197, + "velocityY": 0.68847669897561, + "timestamp": 7.612018995252709 + }, + { + "x": 5.429536251240902, + "y": 4.26885692196241, + "heading": -0.06506108731980774, + "angularVelocity": -2.4962356651562853e-8, + "velocityX": -3.5625374415593805, + "velocityY": 1.0880977954514335, + "timestamp": 7.678380537119456 + }, + { + "x": 5.202528633144186, + "y": 4.366702684523792, + "heading": -0.06506108887411338, + "angularVelocity": -2.342178313955022e-8, + "velocityX": -3.420770701086799, + "velocityY": 1.4744347374847653, + "timestamp": 7.744742078986204 + }, + { + "x": 4.987502129020376, + "y": 4.48575380362361, + "heading": -0.06936699261461025, + "angularVelocity": -0.06488552886765389, + "velocityX": -3.2402276691457503, + "velocityY": 1.7939775923059436, + "timestamp": 7.811103620852951 + }, + { + "x": 4.796367424971955, + "y": 4.591577847199328, + "heading": -0.07462166054562926, + "angularVelocity": -0.0791824267972894, + "velocityX": -2.8802028806415447, + "velocityY": 1.5946592046973391, + "timestamp": 7.877465162719699 + }, + { + "x": 4.62912519228471, + "y": 4.68417391671917, + "heading": -0.07977945899466835, + "angularVelocity": -0.0777227036013692, + "velocityX": -2.5201679765527953, + "velocityY": 1.3953272771415286, + "timestamp": 7.943826704586447 + }, + { + "x": 4.48577515824327, + "y": 4.763541939192526, + "heading": -0.08449544521256436, + "angularVelocity": -0.07106504890084675, + "velocityX": -2.1601371820034427, + "velocityY": 1.195994249692469, + "timestamp": 8.010188246453195 + }, + { + "x": 4.366317095706272, + "y": 4.829681919907118, + "heading": -0.08859794312005527, + "angularVelocity": -0.06182041272833338, + "velocityX": -1.8001098102402053, + "velocityY": 0.9966613019239372, + "timestamp": 8.076549788319943 + }, + { + "x": 4.270750839723337, + "y": 4.882593875393496, + "heading": -0.09198424265030704, + "angularVelocity": -0.05102804176930339, + "velocityX": -1.4400849241090516, + "velocityY": 0.7973286032537331, + "timestamp": 8.14291133018669 + }, + { + "x": 4.199076268469758, + "y": 4.922277822249225, + "heading": -0.09458599237732916, + "angularVelocity": -0.03920568530861384, + "velocityX": -1.0800618737505943, + "velocityY": 0.597996154691727, + "timestamp": 8.209272872053438 + }, + { + "x": 4.151293289107276, + "y": 4.948733775055273, + "heading": -0.09635440561696425, + "angularVelocity": -0.026648163829376063, + "velocityX": -0.7200402223689916, + "velocityY": 0.3986639258498654, + "timestamp": 8.275634413920185 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.013539011456923473, + "velocityX": -0.3600196690634489, + "velocityY": 0.19933188392621148, + "timestamp": 8.341995955786933 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -2.1027730559805923e-24, + "velocityX": -4.370401644398185e-24, + "velocityY": -7.227687528271155e-24, + "timestamp": 8.40835749765368 + }, + { + "x": 4.144916563575742, + "y": 4.94494062571017, + "heading": -0.0787144976857821, + "angularVelocity": 0.2928853385841611, + "velocityX": 0.27671294348408426, + "velocityY": -0.2689143973713598, + "timestamp": 8.471653180860056 + }, + { + "x": 4.1805101351641145, + "y": 4.911442857139667, + "heading": -0.04246897747139415, + "angularVelocity": 0.5726381070287113, + "velocityX": 0.5623380582261821, + "velocityY": -0.529226747759122, + "timestamp": 8.53494886406643 + }, + { + "x": 4.234877563186883, + "y": 4.862193017904391, + "heading": 0.010407773982345214, + "angularVelocity": 0.8353926962338748, + "velocityX": 0.858943695188552, + "velocityY": -0.7780915970951138, + "timestamp": 8.598244547272806 + }, + { + "x": 4.308888949041241, + "y": 4.798194192640054, + "heading": 0.078455258557249, + "angularVelocity": 1.0750730717770254, + "velocityX": 1.1692959472930267, + "velocityY": -1.0111088469599114, + "timestamp": 8.661540230479181 + }, + { + "x": 4.4036437297615265, + "y": 4.720901995225723, + "heading": 0.15957624341444238, + "angularVelocity": 1.2816195473030647, + "velocityX": 1.497018057476968, + "velocityY": -1.2211290486006587, + "timestamp": 8.724835913685556 + }, + { + "x": 4.520513474307112, + "y": 4.632549416340532, + "heading": 0.2505777849388087, + "angularVelocity": 1.4377211353838413, + "velocityX": 1.8464094014836858, + "velocityY": -1.395870530334874, + "timestamp": 8.788131596891931 + }, + { + "x": 4.661061641006965, + "y": 4.536766015518807, + "heading": 0.3463605077982218, + "angularVelocity": 1.5132583773069228, + "velocityX": 2.2205016136976554, + "velocityY": -1.5132690883424573, + "timestamp": 8.851427280098306 + }, + { + "x": 4.826402761674467, + "y": 4.439605699543979, + "heading": 0.43881735404698696, + "angularVelocity": 1.4607132993147334, + "velocityX": 2.612202164378375, + "velocityY": -1.535022785962128, + "timestamp": 8.914722963304682 + }, + { + "x": 5.0150915047408, + "y": 4.350093359802789, + "heading": 0.5163398586345646, + "angularVelocity": 1.2247676407064818, + "velocityX": 2.98106811567406, + "velocityY": -1.4141934363728348, + "timestamp": 8.978018646511057 + }, + { + "x": 5.2211399062787285, + "y": 4.277069011475663, + "heading": 0.5664924385126362, + "angularVelocity": 0.7923538752958722, + "velocityX": 3.255331028912471, + "velocityY": -1.1537018739339473, + "timestamp": 9.041314329717432 + }, + { + "x": 5.439515498788918, + "y": 4.225186735026024, + "heading": 0.5993129773415641, + "angularVelocity": 0.518527286006478, + "velocityX": 3.450086663859443, + "velocityY": -0.8196811191764456, + "timestamp": 9.104610012923807 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.6211417582867742, + "angularVelocity": 0.3448699791111725, + "velocityX": 3.577435593728997, + "velocityY": -0.450862665918832, + "timestamp": 9.167905696130182 + }, + { + "x": 5.882912329849662, + "y": 4.1913117565500215, + "heading": 0.6344672511131947, + "angularVelocity": 0.22398342506550498, + "velocityX": 3.6468128538078894, + "velocityY": -0.08971306223841023, + "timestamp": 9.227398905605497 + }, + { + "x": 6.101970858853418, + "y": 4.2077445367688195, + "heading": 0.6401776935966378, + "angularVelocity": 0.09598477765454098, + "velocityX": 3.682076171981447, + "velocityY": 0.27621270332737524, + "timestamp": 9.286892115080812 + }, + { + "x": 6.320243538678639, + "y": 4.246069175734349, + "heading": 0.6401793813797093, + "angularVelocity": 0.0000283693397346701, + "velocityX": 3.6688671152594092, + "velocityY": 0.6441850978208127, + "timestamp": 9.346385324556127 + }, + { + "x": 6.533654958768337, + "y": 4.305798072358943, + "heading": 0.6401794602105909, + "angularVelocity": 0.0000013250399886292195, + "velocityX": 3.5871559455579396, + "velocityY": 1.003961580680503, + "timestamp": 9.405878534031443 + }, + { + "x": 6.7401111102822036, + "y": 4.386345181993656, + "heading": 0.640179519772512, + "angularVelocity": 0.0000010011549482699353, + "velocityX": 3.470247333009187, + "velocityY": 1.3538874494262292, + "timestamp": 9.465371743506758 + }, + { + "x": 6.937586723137597, + "y": 4.486920348849836, + "heading": 0.6401795682436345, + "angularVelocity": 8.147336982383455e-7, + "velocityX": 3.3192966827135675, + "velocityY": 1.6905318731864938, + "timestamp": 9.524864952982073 + }, + { + "x": 7.124144636672047, + "y": 4.606536937733858, + "heading": 0.6401796100323616, + "angularVelocity": 7.024117120417555e-7, + "velocityX": 3.1357849942833846, + "velocityY": 2.010592300179313, + "timestamp": 9.584358162457388 + }, + { + "x": 7.297954825460976, + "y": 4.744021492057199, + "heading": 0.6401796478532076, + "angularVelocity": 6.357170231929654e-7, + "velocityX": 2.9215130654709105, + "velocityY": 2.310928516646711, + "timestamp": 9.643851371932703 + }, + { + "x": 7.457312504235246, + "y": 4.898025112547281, + "heading": 0.6401796836083543, + "angularVelocity": 6.009954258140389e-7, + "velocityX": 2.6785860130876458, + "velocityY": 2.588591569496418, + "timestamp": 9.703344581408018 + }, + { + "x": 7.598334922177398, + "y": 5.05313142847436, + "heading": 0.681855456134976, + "angularVelocity": 0.7005130987917911, + "velocityX": 2.3703951961217746, + "velocityY": 2.6071263812290466, + "timestamp": 9.762837790883333 + }, + { + "x": 7.725243929451597, + "y": 5.1927289365333404, + "heading": 0.750546465230805, + "angularVelocity": 1.1546025118098764, + "velocityX": 2.133167942920592, + "velocityY": 2.3464443974384026, + "timestamp": 9.822331000358648 + }, + { + "x": 7.837539775743062, + "y": 5.315957707872993, + "heading": 0.8170716120028277, + "angularVelocity": 1.1181973095538917, + "velocityX": 1.8875405660886297, + "velocityY": 2.071308178301995, + "timestamp": 9.881824209833963 + }, + { + "x": 7.935320352876818, + "y": 5.422866587549723, + "heading": 0.8748533666706206, + "angularVelocity": 0.9712327705528885, + "velocityX": 1.643558617800391, + "velocityY": 1.79699297818334, + "timestamp": 9.941317419309279 + }, + { + "x": 8.018658578193438, + "y": 5.513509965801917, + "heading": 0.9207921483680607, + "angularVelocity": 0.7721684895231855, + "velocityX": 1.400802310912472, + "velocityY": 1.5235920040556004, + "timestamp": 10.000810628784594 + }, + { + "x": 8.08760540363553, + "y": 5.587927848241841, + "heading": 0.9530800723202797, + "angularVelocity": 0.5427161223436167, + "velocityX": 1.1589024369360976, + "velocityY": 1.250863469902435, + "timestamp": 10.060303838259909 + }, + { + "x": 8.142197837704975, + "y": 5.6461493904163484, + "heading": 0.9705333871837108, + "angularVelocity": 0.2933665037969271, + "velocityX": 0.917624625581801, + "velocityY": 0.9786250008694746, + "timestamp": 10.119797047735224 + }, + { + "x": 8.182463827907348, + "y": 5.6881968245095615, + "heading": 0.9723096580243064, + "angularVelocity": 0.029856698878088846, + "velocityX": 0.6768165738155562, + "velocityY": 0.7067602246380643, + "timestamp": 10.179290257210539 + }, + { + "x": 8.208425076414684, + "y": 5.714088220117355, + "heading": 0.9577673202803244, + "angularVelocity": -0.24443693443730044, + "velocityX": 0.4363733060679247, + "velocityY": 0.43519917375674694, + "timestamp": 10.238783466685854 + }, + { + "x": 8.220098495483398, + "y": 5.723839282989502, + "heading": 0.9263929393086, + "angularVelocity": -0.5273607063465391, + "velocityX": 0.1962143103669291, + "velocityY": 0.1639021151849818, + "timestamp": 10.29827667616117 + }, + { + "x": 8.217115474998435, + "y": 5.7169576815494905, + "heading": 0.8762998539218844, + "angularVelocity": -0.8237354847591428, + "velocityX": -0.0490530740172862, + "velocityY": -0.11316171192782937, + "timestamp": 10.359088777027946 + }, + { + "x": 8.199143140937736, + "y": 5.693225588730865, + "heading": 0.8092972371375105, + "angularVelocity": -1.1017974355327707, + "velocityX": -0.29553877936354017, + "velocityY": -0.39025280298433057, + "timestamp": 10.419900877894722 + }, + { + "x": 8.16609422727989, + "y": 5.652642007461555, + "heading": 0.7267788786337702, + "angularVelocity": -1.356939775596919, + "velocityX": -0.5434594955080971, + "velocityY": -0.6673602899892838, + "timestamp": 10.480712978761499 + }, + { + "x": 8.117864668551345, + "y": 5.595209979313778, + "heading": 0.6304892819422186, + "angularVelocity": -1.5833953328219617, + "velocityX": -0.7930914742479169, + "velocityY": -0.9444177610899582, + "timestamp": 10.541525079628276 + }, + { + "x": 8.054326469941712, + "y": 5.520944740821225, + "heading": 0.5226315224214131, + "angularVelocity": -1.7736233082473956, + "velocityX": -1.044828211885481, + "velocityY": -1.2212246811740535, + "timestamp": 10.602337180495052 + }, + { + "x": 7.975314688312655, + "y": 5.4298894198944145, + "heading": 0.40611975005056616, + "angularVelocity": -1.9159307228358207, + "velocityX": -1.2992772902575926, + "velocityY": -1.4973224017747822, + "timestamp": 10.663149281361829 + }, + { + "x": 7.880609390155109, + "y": 5.32214652648837, + "heading": 0.28521663340036824, + "angularVelocity": -1.9881424079570305, + "velocityX": -1.5573429762774083, + "velocityY": -1.7717344388756155, + "timestamp": 10.723961382228605 + }, + { + "x": 7.769927978869046, + "y": 5.19796449901651, + "heading": 0.1670617076218344, + "angularVelocity": -1.9429508945494507, + "velocityX": -1.8200557078029118, + "velocityY": -2.0420611309566765, + "timestamp": 10.784773483095382 + }, + { + "x": 7.64302287953294, + "y": 5.0580816434833595, + "heading": 0.06529420310247565, + "angularVelocity": -1.6734745728042795, + "velocityX": -2.086839585005026, + "velocityY": -2.300247048520789, + "timestamp": 10.845585583962158 + }, + { + "x": 7.499740913666761, + "y": 4.907357720220894, + "heading": 0.011577036219802711, + "angularVelocity": -0.8833302273235669, + "velocityX": -2.3561423437758418, + "velocityY": -2.478518602616007, + "timestamp": 10.906397684828935 + }, + { + "x": 7.338349817846371, + "y": 4.749390248030348, + "heading": 0.009577494655868879, + "angularVelocity": -0.03288065262396252, + "velocityX": -2.6539306078893232, + "velocityY": -2.597632213638097, + "timestamp": 10.967209785695712 + }, + { + "x": 7.161261092895849, + "y": 4.608134428437688, + "heading": 0.009577439039588426, + "angularVelocity": -9.145594324092402e-7, + "velocityX": -2.9120639219236804, + "velocityY": -2.322824200764184, + "timestamp": 11.028021886562488 + }, + { + "x": 6.970797581586152, + "y": 4.485507869855581, + "heading": 0.009577379210160273, + "angularVelocity": -9.83840835982591e-7, + "velocityX": -3.132000187379736, + "velocityY": -2.016482851838167, + "timestamp": 11.088833987429265 + }, + { + "x": 6.76891141017589, + "y": 4.382767451700059, + "heading": 0.009577307882139067, + "angularVelocity": -0.000001172924799341256, + "velocityX": -3.3198355020252337, + "velocityY": -1.6894732576432487, + "timestamp": 11.149646088296041 + }, + { + "x": 6.5576717954256925, + "y": 4.300966222617904, + "heading": 0.009577215489609455, + "angularVelocity": -0.0000015193115891924993, + "velocityX": -3.473644418451679, + "velocityY": -1.3451472308342272, + "timestamp": 11.210458189162818 + }, + { + "x": 6.33924383214455, + "y": 4.24094261156842, + "heading": 0.009577080518783282, + "angularVelocity": -0.000002219473168173005, + "velocityX": -3.5918503088663734, + "velocityY": -0.9870339980685793, + "timestamp": 11.271270290029594 + }, + { + "x": 6.115866515513707, + "y": 4.203311883044666, + "heading": 0.009576062223691489, + "angularVelocity": -0.000016744941833606887, + "velocityX": -3.673237948483439, + "velocityY": -0.6188032971627346, + "timestamp": 11.332082390896371 + }, + { + "x": 5.890604902143948, + "y": 4.188526246713883, + "heading": 0.006634290856698977, + "angularVelocity": -0.04837476957813366, + "velocityX": -3.704223504187922, + "velocityY": -0.24313641725970658, + "timestamp": 11.392894491763148 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 1.4617092924271447e-24, + "angularVelocity": -0.10909491305411462, + "velocityX": -3.6942182578974845, + "velocityY": 0.1335725575137668, + "timestamp": 11.453706592629924 + }, + { + "x": 5.418474509439448, + "y": 4.234112857220888, + "heading": -0.009420810708608588, + "angularVelocity": -0.13872442581307998, + "velocityX": -3.644180551162595, + "velocityY": 0.551666082669452, + "timestamp": 11.521616844580091 + }, + { + "x": 5.180225703278332, + "y": 4.298642990605948, + "heading": -0.020608459493002298, + "angularVelocity": -0.164741676891483, + "velocityX": -3.5082892393911758, + "velocityY": 0.9502266820098406, + "timestamp": 11.589527096530258 + }, + { + "x": 4.959251461441715, + "y": 4.385964306023392, + "heading": -0.03296814464402756, + "angularVelocity": -0.1820002841411158, + "velocityX": -3.253915800500449, + "velocityY": 1.2858340664310908, + "timestamp": 11.657437348480425 + }, + { + "x": 4.763381292287379, + "y": 4.486986357550731, + "heading": -0.04558833602673758, + "angularVelocity": -0.18583632103103812, + "velocityX": -2.8842503676479536, + "velocityY": 1.4875817513012504, + "timestamp": 11.725347600430592 + }, + { + "x": 4.59599065624695, + "y": 4.59062036623284, + "heading": -0.05760567577443394, + "angularVelocity": -0.17695913949067177, + "velocityX": -2.464880209298302, + "velocityY": 1.5260436488758087, + "timestamp": 11.793257852380759 + }, + { + "x": 4.456604919229509, + "y": 4.6884604808720365, + "heading": -0.06845454912247914, + "angularVelocity": -0.1597531011371608, + "velocityX": -2.0524991884836625, + "velocityY": 1.440726721364413, + "timestamp": 11.861168104330925 + }, + { + "x": 4.343601982653564, + "y": 4.775267107804887, + "heading": -0.07780582197928933, + "angularVelocity": -0.13770045888906648, + "velocityX": -1.6640040837849672, + "velocityY": 1.2782551152446038, + "timestamp": 11.929078356281092 + }, + { + "x": 4.255382643641957, + "y": 4.8478074673531255, + "heading": -0.0854646750430389, + "angularVelocity": -0.11277904062805744, + "velocityX": -1.2990577486937123, + "velocityY": 1.0681798029768506, + "timestamp": 11.99698860823126 + }, + { + "x": 4.190618071556959, + "y": 4.903989277014325, + "heading": -0.09130900754804393, + "angularVelocity": -0.08605964986396453, + "velocityX": -0.9536788662265924, + "velocityY": 0.8272949672227069, + "timestamp": 12.064898860181426 + }, + { + "x": 4.148242739278531, + "y": 4.942382522588693, + "heading": -0.09525748910421675, + "angularVelocity": -0.058142643309145876, + "velocityX": -0.6239902085700291, + "velocityY": 0.5653527187992231, + "timestamp": 12.132809112131593 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.029382694528135574, + "velocityX": -0.30688901769875077, + "velocityY": 0.28831027812257853, + "timestamp": 12.20071936408176 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 4.130042429684847e-26, + "velocityX": 1.7558924014550056e-25, + "velocityY": 1.7099094571039823e-25, + "timestamp": 12.268629616031927 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.1.traj b/src/main/deploy/choreo/CenterLanePCBAFE.1.traj index 54e8c43e..0e593afd 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.1.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.1.traj @@ -3,308 +3,272 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.0042344276654815e-24, - "angularVelocity": -6.621110938678157e-25, - "velocityX": -2.249108899033261e-24, - "velocityY": -2.0825834551113612e-24, + "heading": -5.237204901969609e-25, + "angularVelocity": 6.164254262445062e-25, + "velocityX": 1.3402750827177572e-25, + "velocityY": -9.971922068875304e-25, "timestamp": 0 }, { - "x": 1.3060567679582313, - "y": 5.572381918180444, - "heading": -0.010695335923114668, - "angularVelocity": -0.14212952691547795, - "velocityX": 0.2137167180313357, - "velocityY": -0.24680705612748235, - "timestamp": 0.07525062634926666 - }, - { - "x": 1.3382215476730719, - "y": 5.535237409026599, - "heading": -0.03207750843931141, - "angularVelocity": -0.2841461068636689, - "velocityX": 0.4274353752957696, - "velocityY": -0.49361063097924024, - "timestamp": 0.15050125269853332 - }, - { - "x": 1.3864692368556266, - "y": 5.479521033340712, - "heading": -0.06412446500663382, - "angularVelocity": -0.4258696322151573, - "velocityX": 0.6411599680063655, - "velocityY": -0.7404107897692007, - "timestamp": 0.22575187904779997 - }, - { - "x": 1.4508006060735943, - "y": 5.40523312636478, - "heading": -0.10680311296182225, - "angularVelocity": -0.5671533916156479, - "velocityX": 0.8548948007340368, - "velocityY": -0.987206493553073, - "timestamp": 0.30100250539706663 - }, - { - "x": 1.5312167667507302, - "y": 5.312374207143024, - "heading": -0.16007302531493992, - "angularVelocity": -0.7078999197411572, - "velocityX": 1.0686444030896685, - "velocityY": -1.2339952997967512, - "timestamp": 0.3762531317463333 - }, - { - "x": 1.6277191879065078, - "y": 5.200945106889031, - "heading": -0.22389124150432238, - "angularVelocity": -0.8480755481446488, - "velocityX": 1.2824135271362858, - "velocityY": -1.480773060104614, - "timestamp": 0.45150375809559995 - }, - { - "x": 1.7403097782501336, - "y": 5.070947160382142, - "heading": -0.2982176402551873, - "angularVelocity": -0.9877180078992028, - "velocityX": 1.496208015878168, - "velocityY": -1.7275330826287059, - "timestamp": 0.5267543844448666 - }, - { - "x": 1.8689915501245091, - "y": 4.922382870770249, - "heading": -0.3830195665066897, - "angularVelocity": -1.126926516968836, - "velocityX": 1.7100425354217623, - "velocityY": -1.9742598410058476, - "timestamp": 0.6020050107941333 - }, - { - "x": 2.021815501878503, - "y": 4.778590249332989, - "heading": -0.46245356659644804, - "angularVelocity": -1.0555925437892708, - "velocityX": 2.0308661757136712, - "velocityY": -1.9108494960541986, - "timestamp": 0.6772556371434 - }, - { - "x": 2.158552321551595, - "y": 4.6533669501943, - "heading": -0.5314334875140496, - "angularVelocity": -0.916669059968213, - "velocityX": 1.8170854690091889, - "velocityY": -1.6640831473944147, - "timestamp": 0.7525062634926667 - }, - { - "x": 2.279199772550662, - "y": 4.546710847450061, - "heading": -0.589963647479443, - "angularVelocity": -0.7778029606522173, - "velocityX": 1.6032750403843345, - "velocityY": -1.417345049717031, - "timestamp": 0.8277568898419334 - }, - { - "x": 2.383756839213222, - "y": 4.45862085446933, - "heading": -0.6380444617813997, - "angularVelocity": -0.6389423800779483, - "velocityX": 1.38945111469599, - "velocityY": -1.1706213921977682, - "timestamp": 0.9030075161912001 - }, - { - "x": 2.4722229084949126, - "y": 4.389096261559967, - "heading": -0.67567478001119, - "angularVelocity": -0.5000665118072721, - "velocityX": 1.1756190423065196, - "velocityY": -0.9239071657247342, - "timestamp": 0.9782581425404668 - }, - { - "x": 2.544597589718017, - "y": 4.338136588288561, - "heading": -0.7028528784398662, - "angularVelocity": -0.36116773703028543, - "velocityX": 0.9617817782298118, - "velocityY": -0.6771993236957614, - "timestamp": 1.0535087688897335 - }, - { - "x": 2.6008806504487216, - "y": 4.305741524929451, - "heading": -0.7195768777220212, - "angularVelocity": -0.22224398777137738, - "velocityX": 0.7479414253573617, - "velocityY": -0.4304955975881504, - "timestamp": 1.1287593952390003 - }, - { - "x": 2.6410719892543444, - "y": 4.2919109040687715, - "heading": -0.7258449001014369, - "angularVelocity": -0.08329528514916955, - "velocityX": 0.5340997245535138, - "velocityY": -0.1837940962309967, - "timestamp": 1.204010021588267 + "x": 1.3104780876502728, + "y": 5.567275466796967, + "heading": -0.013632783425449295, + "angularVelocity": -0.19128878469698626, + "velocityX": 0.2876973543738418, + "velocityY": -0.3322502676668159, + "timestamp": 0.07126807484842616 + }, + { + "x": 1.3514855787219004, + "y": 5.519918104138971, + "heading": -0.04088178026950747, + "angularVelocity": -0.38234506687618114, + "velocityX": 0.5753977662346411, + "velocityY": -0.6644961682873589, + "timestamp": 0.14253614969685233 + }, + { + "x": 1.4129976098576382, + "y": 5.448882530124809, + "heading": -0.08170955132000574, + "angularVelocity": -0.5728760196950918, + "velocityX": 0.8631077977981357, + "velocityY": -0.996737657994004, + "timestamp": 0.21380422454527848 + }, + { + "x": 1.4950153477239438, + "y": 5.354169209270334, + "heading": -0.1360632338085868, + "angularVelocity": -0.7626652270905468, + "velocityX": 1.1508341994748963, + "velocityY": -1.3289726298333888, + "timestamp": 0.28507229939370465 + }, + { + "x": 1.5975404069407602, + "y": 5.235778918310868, + "heading": -0.20388243920444382, + "angularVelocity": -0.951607091114721, + "velocityX": 1.4385832567368764, + "velocityY": -1.6611967028891954, + "timestamp": 0.35634037424213083 + }, + { + "x": 1.7205747481569098, + "y": 5.093712881376462, + "heading": -0.2851089622983034, + "angularVelocity": -1.1397322471052174, + "velocityX": 1.7263598248980438, + "velocityY": -1.9934035995297235, + "timestamp": 0.427608449090557 + }, + { + "x": 1.8641205194488986, + "y": 4.927972835971714, + "heading": -0.3796964281011733, + "angularVelocity": -1.3272066911311928, + "velocityX": 2.0141665338552186, + "velocityY": -2.325586116325524, + "timestamp": 0.4988765239389832 + }, + { + "x": 2.0360561465973834, + "y": 4.766214064350323, + "heading": -0.4690836216896802, + "angularVelocity": -1.2542389250532822, + "velocityX": 2.412519596104705, + "velocityY": -2.26972276107388, + "timestamp": 0.5701445987874093 + }, + { + "x": 2.187477556502562, + "y": 4.628126287278847, + "heading": -0.5451441122583921, + "angularVelocity": -1.0672449161911324, + "velocityX": 2.1246737789286905, + "velocityY": -1.9375825341874753, + "timestamp": 0.6414126736358354 + }, + { + "x": 2.3183837856397878, + "y": 4.513708351773291, + "heading": -0.6078811753474637, + "angularVelocity": -0.8802968681629402, + "velocityX": 1.8368144420294628, + "velocityY": -1.6054584854284524, + "timestamp": 0.7126807484842616 + }, + { + "x": 2.428774184877553, + "y": 4.4229594060006505, + "heading": -0.6572940712821052, + "angularVelocity": -0.6933384413670975, + "velocityX": 1.5489459968231887, + "velocityY": -1.2733463891882395, + "timestamp": 0.7839488233326877 + }, + { + "x": 2.5186483273099793, + "y": 4.355878850846579, + "heading": -0.6933805665691075, + "angularVelocity": -0.5063486752483727, + "velocityX": 1.261071561475065, + "velocityY": -0.94124269943785, + "timestamp": 0.8552168981811138 + }, + { + "x": 2.588005969972471, + "y": 4.312466305335481, + "heading": -0.7161380105196834, + "angularVelocity": -0.31932171591524877, + "velocityX": 0.9731937169623598, + "velocityY": -0.6091443553572609, + "timestamp": 0.9264849730295399 + }, + { + "x": 2.6368470386589604, + "y": 4.292721584024785, + "heading": -0.7255637248082996, + "angularVelocity": -0.1322571755819541, + "velocityX": 0.6853148312251345, + "velocityY": -0.2770486133193386, + "timestamp": 0.997753047877966 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.055677742612701135, - "velocityX": 0.3202582509251215, - "velocityY": 0.06290689943025622, - "timestamp": 1.2792606479375337 - }, - { - "x": 2.6731116736052027, - "y": 4.320290744259811, - "heading": -0.7067971794351253, - "angularVelocity": 0.19582512759313456, - "velocityX": 0.10464854696510043, - "velocityY": 0.31165110401848706, - "timestamp": 1.355134136920331 - }, - { - "x": 2.664692786668951, - "y": 4.362810218152233, - "heading": -0.6813099274422239, - "angularVelocity": 0.3359177538109565, - "velocityX": -0.1109595334170119, - "velocityY": 0.5603996133888316, - "timestamp": 1.4310076259031281 - }, - { - "x": 2.639915179239627, - "y": 4.424203570363852, - "heading": -0.6451974716199608, - "angularVelocity": 0.47595617792732364, - "velocityX": -0.3265647561685457, - "velocityY": 0.8091541991108215, - "timestamp": 1.5068811148859254 - }, - { - "x": 2.59877917099983, - "y": 4.50447140700047, - "heading": -0.59846321314625, - "angularVelocity": 0.6159497750829267, - "velocityX": -0.5421657655564515, - "velocityY": 1.0579167732067494, - "timestamp": 1.5827546038687226 - }, - { - "x": 2.5412852001349884, - "y": 4.6036144764177696, - "heading": -0.541108297539196, - "angularVelocity": 0.755928274499912, - "velocityX": -0.7577609997330812, - "velocityY": 1.3066892105064276, - "timestamp": 1.6586280928515198 - }, - { - "x": 2.4674338294971827, - "y": 4.7216336217313835, - "heading": -0.4731286056147784, - "angularVelocity": 0.8959610640790578, - "velocityX": -0.973348815612657, - "velocityY": 1.5554727599303189, - "timestamp": 1.734501581834317 - }, - { - "x": 2.3772256595733805, - "y": 4.858529596063364, - "heading": -0.3945096344591076, - "angularVelocity": 1.0361849996577348, - "velocityX": -1.1889287171735974, - "velocityY": 1.8042662353779348, - "timestamp": 1.8103750708171142 - }, - { - "x": 2.270660565338228, - "y": 5.014302224131279, - "heading": -0.305219034790845, - "angularVelocity": 1.1768352933988244, - "velocityX": -1.4045102665479658, - "velocityY": 2.0530574006321625, - "timestamp": 1.8862485597999115 - }, - { - "x": 2.156230166958197, - "y": 5.146478435153338, - "heading": -0.2290932542035935, - "angularVelocity": 1.0033251615002383, - "velocityX": -1.5081736705949598, - "velocityY": 1.7420605377990246, - "timestamp": 1.9621220487827087 - }, - { - "x": 2.05814700584277, - "y": 5.259770794344292, - "heading": -0.16373664121599296, - "angularVelocity": 0.8613893187700765, - "velocityX": -1.292719794889953, - "velocityY": 1.4931745028443373, - "timestamp": 2.037995537765506 - }, - { - "x": 1.976411579927544, - "y": 5.3541805242021825, - "heading": -0.10920088428004432, - "angularVelocity": 0.7187722308158727, - "velocityX": -1.077259356476389, - "velocityY": 1.2443045802110966, - "timestamp": 2.113869026748303 - }, - { - "x": 1.9110235357975711, - "y": 5.4297080461749685, - "heading": -0.06553152377248019, - "angularVelocity": 0.5755549282499098, - "velocityX": -0.8618035760132025, - "velocityY": 0.9954402121920332, - "timestamp": 2.1897425157311 - }, - { - "x": 1.8619825316383047, - "y": 5.486353607136737, - "heading": -0.03276339241185007, - "angularVelocity": 0.43187853623101025, - "velocityX": -0.6463523006090516, - "velocityY": 0.7465790979324988, - "timestamp": 2.265616004713897 - }, - { - "x": 1.829288400520471, - "y": 5.524117342639773, - "heading": -0.010917643304867627, - "angularVelocity": 0.2879233497741951, - "velocityX": -0.43090322530503955, - "velocityY": 0.49771977022960984, - "timestamp": 2.341489493696694 + "angularVelocity": 0.054843767289835074, + "velocityX": 0.3974372063685045, + "velocityY": 0.05504713907518853, + "timestamp": 1.0690211227263922 + }, + { + "x": 2.6726523875585464, + "y": 4.325520941215549, + "heading": -0.7036434191881636, + "angularVelocity": 0.24661858694943486, + "velocityX": 0.10242764131932201, + "velocityY": 0.39537758612425, + "timestamp": 1.1420557472887627 + }, + { + "x": 2.658587549090021, + "y": 4.379253724773331, + "heading": -0.6716319783828552, + "angularVelocity": 0.4383049957074976, + "velocityX": -0.19257767877637322, + "velocityY": 0.7357165711435307, + "timestamp": 1.2150903718511332 + }, + { + "x": 2.6229776230328583, + "y": 4.457843935727211, + "heading": -0.625626931450262, + "angularVelocity": 0.629907351591922, + "velocityX": -0.4875759445679413, + "velocityY": 1.076067843502979, + "timestamp": 1.2881249964135038 + }, + { + "x": 2.565823384293886, + "y": 4.561292786214385, + "heading": -0.5656322001624833, + "angularVelocity": 0.8214560100400625, + "velocityX": -0.7825636002299563, + "velocityY": 1.4164357126095721, + "timestamp": 1.3611596209758743 + }, + { + "x": 2.4871259686686438, + "y": 4.6896018233859875, + "heading": -0.4916449942707574, + "angularVelocity": 1.01304287295325, + "velocityX": -1.0775357044251765, + "velocityY": 1.7568247655196478, + "timestamp": 1.4341942455382448 + }, + { + "x": 2.3868870212616646, + "y": 4.8427728778807975, + "heading": -0.4036470135354823, + "angularVelocity": 1.2048803052328438, + "velocityX": -1.3724852836256678, + "velocityY": 2.097238883784559, + "timestamp": 1.5072288701006153 + }, + { + "x": 2.2651088471232064, + "y": 5.020807849871808, + "heading": -0.30158987603954596, + "angularVelocity": 1.3973801892933821, + "velocityX": -1.6674033017649226, + "velocityY": 2.43767901947618, + "timestamp": 1.5802634946629859 + }, + { + "x": 2.1359147758136054, + "y": 5.170005063650777, + "heading": -0.2156034423971957, + "angularVelocity": 1.1773379292025947, + "velocityX": -1.7689427731537262, + "velocityY": 2.0428285169256424, + "timestamp": 1.6532981192253564 + }, + { + "x": 2.0282555085514997, + "y": 5.294336258780011, + "heading": -0.14382060600259022, + "angularVelocity": 0.9828603463731643, + "velocityX": -1.4740853110043226, + "velocityY": 1.7023596119544246, + "timestamp": 1.726332743787727 + }, + { + "x": 1.9421294204612731, + "y": 5.39380126769391, + "heading": -0.08631841339161836, + "angularVelocity": 0.7873278319089028, + "velocityX": -1.1792500968725612, + "velocityY": 1.3618884126522468, + "timestamp": 1.7993673683500975 + }, + { + "x": 1.8775353609204413, + "y": 5.468400102933885, + "heading": -0.04315875828225115, + "angularVelocity": 0.5909478602509897, + "velocityX": -0.8844306372201511, + "velocityY": 1.021417385068757, + "timestamp": 1.872401992912468 + }, + { + "x": 1.8344726769226216, + "y": 5.518132795390121, + "heading": -0.014381433416099416, + "angularVelocity": 0.3940230409697844, + "velocityX": -0.5896201186198287, + "velocityY": 0.6809467804378745, + "timestamp": 1.9454366174748385 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 5.072711091682045e-24, - "angularVelocity": 0.14389272789792218, - "velocityX": -0.21545347602968715, - "velocityY": 0.24886063882779058, - "timestamp": 2.417362982679491 + "heading": 1.6699672496632016e-24, + "angularVelocity": 0.19691253980251358, + "velocityX": -0.29481199459862734, + "velocityY": 0.3404751148788101, + "timestamp": 2.018471242037209 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 2.471925946464062e-24, - "angularVelocity": -1.3881126396060902e-24, - "velocityX": -6.902910153275516e-22, - "velocityY": 3.1112652084354562e-24, - "timestamp": 2.493236471662288 + "heading": 7.316181849831016e-25, + "angularVelocity": -2.1571909437539165e-24, + "velocityX": -1.0558547549960102e-21, + "velocityY": -3.684849193710572e-24, + "timestamp": 2.0915058665995794 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.2.traj b/src/main/deploy/choreo/CenterLanePCBAFE.2.traj index c5fcbaa6..31952cf7 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.2.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.2.traj @@ -3,101 +3,83 @@ { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 2.471925946464062e-24, - "angularVelocity": -1.3881126396060902e-24, - "velocityX": -6.902910153275516e-22, - "velocityY": 3.1112652084354562e-24, + "heading": 7.316181849831016e-25, + "angularVelocity": -2.1571909437539165e-24, + "velocityX": -1.0558547549960102e-21, + "velocityY": -3.684849193710572e-24, "timestamp": 0 }, { - "x": 1.8507557540076562, - "y": 5.5430527114888575, - "heading": -8.800046052500135e-19, - "angularVelocity": -9.452376611096758e-18, - "velocityX": 0.4061756726920047, - "velocityY": 0.0005740544422031257, - "timestamp": 0.09309902825151006 + "x": 1.8720264440667018, + "y": 5.543082773688388, + "heading": -5.729619384838823e-18, + "angularVelocity": -5.86988662374941e-17, + "velocityX": 0.6053171317615014, + "velocityY": 0.0008555041864670528, + "timestamp": 0.09761040516750885 }, { - "x": 1.9263848743459357, - "y": 5.543159599309593, - "heading": -2.769921496163024e-18, - "angularVelocity": -2.0300071079064193e-17, - "velocityX": 0.8123513398439022, - "velocityY": 0.0011481088765763312, - "timestamp": 0.18619805650302013 + "x": 1.9901969439319853, + "y": 5.54324978590735, + "heading": 1.1892133440280875e-18, + "angularVelocity": 7.088212283450037e-17, + "velocityX": 1.2106342521833764, + "velocityY": 0.001711008356907634, + "timestamp": 0.1952208103350177 }, { - "x": 2.0398285536670646, - "y": 5.54331993103902, - "heading": -5.903589755199969e-18, - "angularVelocity": -3.365951630081127e-17, - "velocityX": 1.2185269970236072, - "velocityY": 0.0017221632968556807, - "timestamp": 0.2792970847545302 + "x": 2.1674526905937914, + "y": 5.543500304231363, + "heading": 2.3439205825147678e-18, + "angularVelocity": 1.1829755615161062e-17, + "velocityX": 1.8159513461461236, + "velocityY": 0.0025665124899531154, + "timestamp": 0.29283121550252655 }, { - "x": 2.191086789804772, - "y": 5.5435337066740775, - "heading": -1.0884334063348694e-17, - "angularVelocity": -5.34994231591497e-17, - "velocityX": 1.6247026309348636, - "velocityY": 0.0022962176842493674, - "timestamp": 0.37239611300604025 + "x": 2.403793671138691, + "y": 5.5438343286421725, + "heading": -2.263663737810736e-18, + "angularVelocity": -4.7203823324743326e-17, + "velocityX": 2.4212683078132478, + "velocityY": 0.00342201643602312, + "timestamp": 0.3904416206700354 }, { - "x": 2.380159571927711, - "y": 5.543800926199458, - "heading": -4.173692512602447e-18, - "angularVelocity": 7.208068308331619e-17, - "velocityX": 2.0308781485038967, - "velocityY": 0.0028702719072147666, - "timestamp": 0.4654951412575503 + "x": 2.5810494178004975, + "y": 5.544084846966185, + "heading": -1.1285361572651988e-18, + "angularVelocity": 1.1629165750714285e-17, + "velocityX": 1.8159513461461234, + "velocityY": 0.002566512489953115, + "timestamp": 0.48805202583754426 }, { - "x": 2.5314178080654184, - "y": 5.544014701834515, - "heading": -1.8153111851912927e-18, - "angularVelocity": 2.5331965023903977e-17, - "velocityX": 1.6247026309348636, - "velocityY": 0.002296217684249368, - "timestamp": 0.5585941695090604 - }, - { - "x": 2.644861487386547, - "y": 5.544175033563942, - "heading": -7.161538717554587e-19, - "angularVelocity": 1.1806324234469696e-17, - "velocityX": 1.2185269970236072, - "velocityY": 0.001722163296855681, - "timestamp": 0.6516931977605704 - }, - { - "x": 2.7204906077248263, - "y": 5.544281921384678, - "heading": -1.8348593704837507e-19, - "angularVelocity": 5.72151981311354e-18, - "velocityX": 0.8123513398439022, - "velocityY": 0.0011481088765763316, - "timestamp": 0.7447922260120805 + "x": 2.699219917665781, + "y": 5.544251859185147, + "heading": 5.747781298910251e-18, + "angularVelocity": 7.04465619676184e-17, + "velocityX": 1.2106342521833764, + "velocityY": 0.001711008356907634, + "timestamp": 0.5856624310050531 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -9.055003253623357e-30, - "angularVelocity": 1.970868445148275e-18, - "velocityX": 0.4061756726920047, - "velocityY": 0.0005740544422031258, - "timestamp": 0.8378912542635906 + "heading": 7.720128293472343e-28, + "angularVelocity": -5.888492408296726e-17, + "velocityX": 0.6053171317615014, + "velocityY": 0.000855504186467053, + "timestamp": 0.683272836172562 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -2.6159098195352036e-29, - "angularVelocity": -1.6705479392630998e-29, - "velocityX": 6.780641024803866e-22, - "velocityY": 9.718698194414986e-25, - "timestamp": 0.9309902825151006 + "heading": 6.467452452708822e-28, + "angularVelocity": 1.6941291064933935e-28, + "velocityX": 1.0543917599744842e-21, + "velocityY": 1.5169407123402652e-24, + "timestamp": 0.7808832413400708 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.3.traj b/src/main/deploy/choreo/CenterLanePCBAFE.3.traj index 25831f55..86e5f34f 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.3.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.3.traj @@ -3,191 +3,173 @@ { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -2.6159098195352036e-29, - "angularVelocity": -1.6705479392630998e-29, - "velocityX": 6.780641024803866e-22, - "velocityY": 9.718698194414986e-25, + "heading": 6.467452452708822e-28, + "angularVelocity": 1.6941291064933935e-28, + "velocityX": 1.0543917599744842e-21, + "velocityY": 1.5169407123402652e-24, "timestamp": 0 }, { - "x": 2.746560227605712, - "y": 5.55825095759549, - "heading": 0.00742109779657271, - "angularVelocity": 0.11458935665570884, - "velocityX": -0.18135392067783562, - "velocityY": 0.21487100869168943, - "timestamp": 0.06476254002254223 - }, - { - "x": 2.7232728362575633, - "y": 5.58624983061418, - "heading": 0.022298554503038515, - "angularVelocity": 0.22972318104396974, - "velocityX": -0.35958119215279744, - "velocityY": 0.43233129844728024, - "timestamp": 0.12952508004508445 - }, - { - "x": 2.6887224275841572, - "y": 5.628555352569631, - "heading": 0.04468536543200197, - "angularVelocity": 0.3456753073794072, - "velocityX": -0.5334937243255126, - "velocityY": 0.6532406224451052, - "timestamp": 0.19428762006762668 - }, - { - "x": 2.6433185485147903, - "y": 5.685479106691384, - "heading": 0.07465726375014338, - "angularVelocity": 0.46279683143540923, - "velocityX": -0.7010824321214573, - "velocityY": 0.8789611108819849, - "timestamp": 0.2590501600901689 - }, - { - "x": 2.587716352440023, - "y": 5.7574833976129485, - "heading": 0.11231839668543797, - "angularVelocity": 0.5815264954429781, - "velocityX": -0.8585549000303846, - "velocityY": 1.1118200567257257, - "timestamp": 0.3238127001127111 - }, - { - "x": 2.523118270580454, - "y": 5.845312686573325, - "heading": 0.15779630703388142, - "angularVelocity": 0.7022255509529696, - "velocityX": -0.9974605973929359, - "velocityY": 1.356174247177545, - "timestamp": 0.38857524013525335 - }, - { - "x": 2.4523262633845233, - "y": 5.950260122565857, - "heading": 0.21110300167691656, - "angularVelocity": 0.8231100050195744, - "velocityX": -1.093101153402716, - "velocityY": 1.6204959835732518, - "timestamp": 0.4533377801577956 - }, - { - "x": 2.385035672029833, - "y": 6.073114038949963, - "heading": 0.27020607963640353, - "angularVelocity": 0.9126121047586251, - "velocityX": -1.0390357038384854, - "velocityY": 1.896990395085535, - "timestamp": 0.5181003201803378 - }, - { - "x": 2.335844965932351, - "y": 6.198572107213467, - "heading": 0.32662517660992413, - "angularVelocity": 0.8711686872362097, - "velocityX": -0.7595549229594762, - "velocityY": 1.9372011693771587, - "timestamp": 0.5828628602028805 - }, - { - "x": 2.303814652096041, - "y": 6.317831411051833, - "heading": 0.37736366027868895, - "angularVelocity": 0.7834541951428096, - "velocityX": -0.4945808769260876, - "velocityY": 1.8414858928765832, - "timestamp": 0.6476254002254227 - }, - { - "x": 2.2877361979926687, - "y": 6.4282644247646346, - "heading": 0.42161092558434743, - "angularVelocity": 0.6832231300726793, - "velocityX": -0.24826781188285701, - "velocityY": 1.7051989263293559, - "timestamp": 0.7123879402479649 - }, - { - "x": 2.2869063645502266, - "y": 6.528717533266064, - "heading": 0.45901061919668296, - "angularVelocity": 0.5774896043193742, - "velocityX": -0.012813478936328582, - "velocityY": 1.5510989603938259, - "timestamp": 0.7771504802705071 - }, - { - "x": 2.3008846173823154, - "y": 6.618554541912377, - "heading": 0.48936477750092344, - "angularVelocity": 0.46869931743991045, - "velocityX": 0.21583855153339288, - "velocityY": 1.3871754970549879, - "timestamp": 0.8419130202930494 - }, - { - "x": 2.3293724060058594, - "y": 6.6973748207092285, + "x": 2.7303741241412878, + "y": 5.568056862351408, + "heading": 0.006778500636213837, + "angularVelocity": 0.08813486156991056, + "velocityX": -0.36316271539799655, + "velocityY": 0.3084296913821901, + "timestamp": 0.07691054953137888 + }, + { + "x": 2.6747561237908846, + "y": 5.615779928168, + "heading": 0.02059737759786194, + "angularVelocity": 0.1796746616146605, + "velocityX": -0.7231517742271669, + "velocityY": 0.6205009079686918, + "timestamp": 0.15382109906275776 + }, + { + "x": 2.591861580665438, + "y": 5.687962474226551, + "heading": 0.041894032741262306, + "angularVelocity": 0.27690161197862034, + "velocityX": -1.0778045876739721, + "velocityY": 0.938525943428621, + "timestamp": 0.23073164859413664 + }, + { + "x": 2.482522842737762, + "y": 5.785486601053792, + "heading": 0.07152934014357436, + "angularVelocity": 0.38532174822416326, + "velocityX": -1.4216351149989686, + "velocityY": 1.2680201535610198, + "timestamp": 0.3076421981255155 + }, + { + "x": 2.3492929926954265, + "y": 5.910724214608568, + "heading": 0.11194451008519192, + "angularVelocity": 0.525482787314222, + "velocityX": -1.7322701612992522, + "velocityY": 1.6283541636076846, + "timestamp": 0.3845527476568944 + }, + { + "x": 2.232288592300856, + "y": 6.068262989450489, + "heading": 0.17809331854346394, + "angularVelocity": 0.8600745783422566, + "velocityX": -1.5213049589098766, + "velocityY": 2.0483376572110616, + "timestamp": 0.4614632971882733 + }, + { + "x": 2.1483046945309274, + "y": 6.20982532534887, + "heading": 0.24392330820994265, + "angularVelocity": 0.8559292589584283, + "velocityX": -1.091968504732421, + "velocityY": 1.8406101212503223, + "timestamp": 0.5383738467196522 + }, + { + "x": 2.095435520919896, + "y": 6.331973650947482, + "heading": 0.3066258894331499, + "angularVelocity": 0.8152663269897057, + "velocityX": -0.6874112060460749, + "velocityY": 1.588186878690508, + "timestamp": 0.615284396251031 + }, + { + "x": 2.072969838348887, + "y": 6.433621733055283, + "heading": 0.3652444017006222, + "angularVelocity": 0.7621647826551591, + "velocityX": -0.29210144392276055, + "velocityY": 1.3216403045765417, + "timestamp": 0.6921949457824099 + }, + { + "x": 2.080539684915293, + "y": 6.514239840456833, + "heading": 0.4192892074806503, + "angularVelocity": 0.7026969136136278, + "velocityX": 0.0984240343168718, + "velocityY": 1.0482061029697753, + "timestamp": 0.7691054953137888 + }, + { + "x": 2.1179206171311256, + "y": 6.5735146979492365, + "heading": 0.46845941042093314, + "angularVelocity": 0.6393167548519689, + "velocityX": 0.48603127195940116, + "velocityY": 0.7706986603732366, + "timestamp": 0.8460160448451677 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, "heading": 0.5125504196, - "angularVelocity": 0.3580100794534351, - "velocityX": 0.43988065652810937, - "velocityY": 1.21706589595492, - "timestamp": 0.9066755603155916 + "angularVelocity": 0.5732764808952251, + "velocityX": 0.8716741807939861, + "velocityY": 0.49050144055915196, + "timestamp": 0.9229265943765466 }, { - "x": 2.3907092837867907, - "y": 6.780867422722411, - "heading": 0.5304481214064672, - "angularVelocity": 0.21222434211791516, - "velocityX": 0.7273100577595625, - "velocityY": 0.990024458199304, - "timestamp": 0.991009438455372 + "x": 2.2917885277794023, + "y": 6.672276624724165, + "heading": 0.5461832199677006, + "angularVelocity": 0.41150844936023123, + "velocityX": 1.30706335645214, + "velocityY": 0.7468102485181378, + "timestamp": 1.0046571163621563 }, { - "x": 2.471644870352802, - "y": 6.840510057948011, - "heading": 0.53507672796877, - "angularVelocity": 0.05488430823293737, - "velocityX": 0.9597043128013546, - "velocityY": 0.7072203548702543, - "timestamp": 1.0753433165951525 + "x": 2.434116611335813, + "y": 6.754285363860807, + "heading": 0.5638407865548148, + "angularVelocity": 0.2160461741602869, + "velocityX": 1.74143123154738, + "velocityY": 1.0034040789691918, + "timestamp": 1.086387638347766 }, { - "x": 2.556252265889606, - "y": 6.8694746689081185, - "heading": 0.5277386544868528, - "angularVelocity": -0.08701216692246314, - "velocityX": 1.0032432683407548, - "velocityY": 0.3434516661512893, - "timestamp": 1.159677194734933 + "x": 2.5412609256604743, + "y": 6.8169454314474, + "heading": 0.5395666812512137, + "angularVelocity": -0.2970017162973107, + "velocityX": 1.3109461645616995, + "velocityY": 0.7666666756101815, + "timestamp": 1.1681181603333757 }, { - "x": 2.617383928971812, - "y": 6.87815517989328, - "heading": 0.5183265349753615, - "angularVelocity": -0.1116054392268187, - "velocityX": 0.7248766976052303, - "velocityY": 0.1029302953526471, - "timestamp": 1.2440110728747134 + "x": 2.6126555907805695, + "y": 6.858741825974231, + "heading": 0.5217952234534814, + "angularVelocity": -0.21743967083510435, + "velocityX": 0.8735373687282384, + "velocityY": 0.5113927271159627, + "timestamp": 1.2498486823189854 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.06849104420156939, - "velocityX": 0.36711922260971214, - "velocityY": 0.01766420444115541, - "timestamp": 1.3283449510144938 + "angularVelocity": -0.11311323638810435, + "velocityX": 0.4366658269950187, + "velocityY": 0.25575567457534326, + "timestamp": 1.331579204304595 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -1.2403630086849167e-25, - "velocityX": 1.2566075146445203e-23, - "velocityY": 7.418296886220231e-24, - "timestamp": 1.4126788291542742 + "angularVelocity": 1.6798438100344207e-27, + "velocityX": 4.181549744276816e-25, + "velocityY": 2.1523905294069243e-25, + "timestamp": 1.4133097262902048 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.4.traj b/src/main/deploy/choreo/CenterLanePCBAFE.4.traj index a66f4247..f9cc278f 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.4.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.4.traj @@ -4,730 +4,703 @@ "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -1.2403630086849167e-25, - "velocityX": 1.2566075146445203e-23, - "velocityY": 7.418296886220231e-24, + "angularVelocity": 1.6798438100344207e-27, + "velocityX": 4.181549744276816e-25, + "velocityY": 2.1523905294069243e-25, "timestamp": 0 }, { - "x": 2.65710997558339, - "y": 6.867790932222418, - "heading": 0.5069947283791145, - "angularVelocity": -0.09538266194620745, - "velocityX": 0.1504894284206614, - "velocityY": -0.20351386841397834, - "timestamp": 0.05824634275796026 - }, - { - "x": 2.6746461929799414, - "y": 6.844083289351916, - "heading": 0.4960269163567629, - "angularVelocity": -0.18830044090369472, - "velocityX": 0.3010698451819098, - "velocityY": -0.40702371596133824, - "timestamp": 0.11649268551592051 - }, - { - "x": 2.7009592416637416, - "y": 6.808522263895978, - "heading": 0.4798144698796375, - "angularVelocity": -0.27834273723408604, - "velocityX": 0.4517545211918758, - "velocityY": -0.6105280395665419, - "timestamp": 0.17473902827388077 - }, - { - "x": 2.736056173569292, - "y": 6.76110833670321, - "heading": 0.4585545185998355, - "angularVelocity": -0.3650006210372158, - "velocityX": 0.6025602680565616, - "velocityY": -0.8140241077417322, - "timestamp": 0.23298537103184103 - }, - { - "x": 2.7799452908871816, - "y": 6.701842252248569, - "heading": 0.4324815844111489, - "angularVelocity": -0.4476321251109512, - "velocityX": 0.7535085507474468, - "velocityY": -1.017507394428501, - "timestamp": 0.2912317137898013 - }, - { - "x": 2.8326365093294883, - "y": 6.63072517677858, - "heading": 0.40187833162186215, - "angularVelocity": -0.5254107183425597, - "velocityX": 0.9046270709435361, - "velocityY": -1.2209706584585491, - "timestamp": 0.34947805654776154 - }, - { - "x": 2.8941418577048204, - "y": 6.54775894755859, - "heading": 0.367091043865372, - "angularVelocity": -0.5972441548999393, - "velocityX": 1.0559521072578695, - "velocityY": -1.4244023794721719, - "timestamp": 0.4077243993057218 - }, - { - "x": 2.9644761872265666, - "y": 6.452946486925878, - "heading": 0.3285529983813312, - "angularVelocity": -0.661638888542476, - "velocityX": 1.2075321160337456, - "velocityY": -1.6277839284553843, - "timestamp": 0.46597074206368205 - }, - { - "x": 3.043658216776231, - "y": 6.346292539288771, - "heading": 0.2868218829421827, - "angularVelocity": -0.7164589820267359, - "velocityX": 1.359433499176103, - "velocityY": -1.8310840232545251, - "timestamp": 0.5242170848216423 - }, - { - "x": 3.131712135507504, - "y": 6.227805106020448, - "heading": 0.24264403908378393, - "angularVelocity": -0.7584655407804345, - "velocityX": 1.5117501728336273, - "velocityY": -2.03424674679905, - "timestamp": 0.5824634275796026 - }, - { - "x": 3.2286701423571795, - "y": 6.097498584088062, - "heading": 0.19707481942882135, - "angularVelocity": -0.7823533203504854, - "velocityX": 1.6646196526463415, - "velocityY": -2.2371622965903253, - "timestamp": 0.6407097703375628 - }, - { - "x": 3.33457642569322, - "y": 5.9554017916384385, - "heading": 0.1517318182422537, - "angularVelocity": -0.7784694976470596, - "velocityX": 1.8182477786824947, - "velocityY": -2.439583083183451, - "timestamp": 0.6989561130955231 - }, - { - "x": 3.449491590462662, - "y": 5.801583658823103, - "heading": 0.10942509922281701, - "angularVelocity": -0.726341209013589, - "velocityX": 1.9729163983216391, - "velocityY": -2.6408204452341257, - "timestamp": 0.7572024558534833 - }, - { - "x": 3.5734690777406897, - "y": 5.636270409713289, - "heading": 0.07620523271942112, - "angularVelocity": -0.5703339459687529, - "velocityX": 2.128502518917107, - "velocityY": -2.838173888389265, - "timestamp": 0.8154487986114436 - }, - { - "x": 3.704953237459551, - "y": 5.460551173556115, - "heading": 0.07376572964977675, - "angularVelocity": -0.04188251062872067, - "velocityX": 2.2573805237049305, - "velocityY": -3.016828659738646, - "timestamp": 0.8736951413694038 - }, - { - "x": 3.8484687741574635, - "y": 5.2934320188975885, - "heading": 0.07376572135485625, - "angularVelocity": -1.4241101012224094e-7, - "velocityX": 2.4639407369194757, - "velocityY": -2.86917850538672, - "timestamp": 0.9319414841273641 - }, - { - "x": 4.002883086238839, - "y": 5.136327781933491, - "heading": 0.07376571311141436, - "angularVelocity": -1.4152720162027837e-7, - "velocityX": 2.651055925056733, - "velocityY": -2.697237792541529, - "timestamp": 0.9901878268853244 + "x": 2.661250521617956, + "y": 6.863588942839657, + "heading": 0.5202450670683688, + "angularVelocity": 0.13324461766431578, + "velocityX": 0.22348726055621218, + "velocityY": -0.27803300742854703, + "timestamp": 0.05774827984237163 + }, + { + "x": 2.6870669895732706, + "y": 6.83147467268314, + "heading": 0.535409558228526, + "angularVelocity": 0.2625964132879752, + "velocityX": 0.447051722160081, + "velocityY": -0.5561078225043913, + "timestamp": 0.11549655968474326 + }, + { + "x": 2.725799371823277, + "y": 6.783299196063953, + "heading": 0.5577628193465138, + "angularVelocity": 0.38708098629089316, + "velocityX": 0.6707105797043527, + "velocityY": -0.8342322360196058, + "timestamp": 0.1732448395271149 + }, + { + "x": 2.7774544304477864, + "y": 6.719058990822734, + "heading": 0.5869404585342344, + "angularVelocity": 0.5052555550981419, + "velocityX": 0.8944865330275743, + "velocityY": -1.112417641123979, + "timestamp": 0.23099311936948652 + }, + { + "x": 2.8420407207473644, + "y": 6.638749608593502, + "heading": 0.6224502537914576, + "angularVelocity": 0.6149065453403929, + "velocityX": 1.118410634496313, + "velocityY": -1.3906800765051686, + "timestamp": 0.28874139921185815 + }, + { + "x": 2.9195693565379317, + "y": 6.542365376975582, + "heading": 0.6635926087059805, + "angularVelocity": 0.7124429511463173, + "velocityX": 1.342527188726434, + "velocityY": -1.6690407382004828, + "timestamp": 0.3464896790542298 + }, + { + "x": 3.0100552630481547, + "y": 6.429899310890966, + "heading": 0.70930037739609, + "angularVelocity": 0.7915000899571744, + "velocityX": 1.5669021961729532, + "velocityY": -1.947522357230365, + "timestamp": 0.4042379588966014 + }, + { + "x": 3.1135190276304736, + "y": 6.301344715989459, + "heading": 0.7577535982781642, + "angularVelocity": 0.8390418037442976, + "velocityX": 1.7916337051896702, + "velocityY": -2.2261198991970987, + "timestamp": 0.46198623873897304 + }, + { + "x": 3.2299855403363114, + "y": 6.1567104859066015, + "heading": 0.8051333898440217, + "angularVelocity": 0.8204537294475949, + "velocityX": 2.016796223605989, + "velocityY": -2.5045634342294933, + "timestamp": 0.5197345185813447 + }, + { + "x": 3.3593492107693024, + "y": 5.9962947499990635, + "heading": 0.8376127381025854, + "angularVelocity": 0.5624297095466465, + "velocityX": 2.240130282427429, + "velocityY": -2.777844402385742, + "timestamp": 0.5774827984237163 + }, + { + "x": 3.4940407564436287, + "y": 5.828570179937074, + "heading": 0.8376127456087376, + "angularVelocity": 1.2998053164779303e-7, + "velocityX": 2.332390610455871, + "velocityY": -2.9044080710248896, + "timestamp": 0.6352310782660879 + }, + { + "x": 3.6287322938415287, + "y": 5.660845603228664, + "heading": 0.837612753114844, + "angularVelocity": 1.2997973902929816e-7, + "velocityX": 2.3323904671368516, + "velocityY": -2.9044081861178794, + "timestamp": 0.6929793581084596 + }, + { + "x": 3.763423831239301, + "y": 5.49312102652015, + "heading": 0.8376127606209502, + "angularVelocity": 1.2997973824676034e-7, + "velocityX": 2.3323904671346476, + "velocityY": -2.9044081861196496, + "timestamp": 0.7507276379508312 + }, + { + "x": 3.8981153686370735, + "y": 5.325396449811636, + "heading": 0.8376127681270565, + "angularVelocity": 1.2997973875283622e-7, + "velocityX": 2.3323904671346543, + "velocityY": -2.904408186119644, + "timestamp": 0.8084759177932028 + }, + { + "x": 4.03280690604441, + "y": 5.157671873110804, + "heading": 0.8376127756331628, + "angularVelocity": 1.2997973760288775e-7, + "velocityX": 2.3323904673002676, + "velocityY": -2.9044081859866484, + "timestamp": 0.8662241976355745 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.07376570463306892, - "angularVelocity": -1.4556013372477054e-7, - "velocityX": 2.826202837221629, - "velocityY": -2.51311892102558, - "timestamp": 1.0484341696432846 - }, - { - "x": 4.268981030537756, - "y": 4.906736402889882, - "heading": 0.07376569669894775, - "angularVelocity": -2.2864637143597095e-7, - "velocityX": 2.9245183697828536, - "velocityY": -2.397995023127166, - "timestamp": 1.0831345723445063 - }, - { - "x": 4.373711969606293, - "y": 4.827653183122543, - "heading": 0.07376568907042552, - "angularVelocity": -2.198395881628289e-7, - "velocityX": 3.0181476558152474, - "velocityY": -2.2790288760699386, - "timestamp": 1.117834975045728 - }, - { - "x": 4.481524054047376, - "y": 4.75282484787289, - "heading": 0.0737656816823934, - "angularVelocity": -2.129091176782688e-7, - "velocityX": 3.106940440125975, - "velocityY": -2.1564111487103386, - "timestamp": 1.1525353777469496 - }, - { - "x": 4.592244511621915, - "y": 4.682371277147909, - "heading": 0.07376567447691734, - "angularVelocity": -2.0764819690223906e-7, - "velocityX": 3.1907542551556984, - "velocityY": -2.030338706198957, - "timestamp": 1.1872357804481712 - }, - { - "x": 4.705695889724197, - "y": 4.616405299617974, - "heading": 0.07376566740123429, - "angularVelocity": -2.0390780833167242e-7, - "velocityX": 3.2694542215871127, - "velocityY": -1.90101475472538, - "timestamp": 1.221936183149393 - }, - { - "x": 4.821696252971789, - "y": 4.555032332206395, - "heading": 0.07376566040606643, - "angularVelocity": -2.0158751247220773e-7, - "velocityX": 3.342911154270497, - "velocityY": -1.76865288682714, - "timestamp": 1.2566365858506146 - }, - { - "x": 4.939640387985606, - "y": 4.497483166935729, - "heading": 0.07376565344322925, - "angularVelocity": -2.0065580355812287e-7, - "velocityX": 3.398926981607191, - "velocityY": -1.6584581385460047, - "timestamp": 1.2913369885518362 - }, - { - "x": 5.057585335343478, - "y": 4.439935666553369, - "heading": 0.07376564648040178, - "angularVelocity": -2.0065552340961329e-7, - "velocityX": 3.3989503918269923, - "velocityY": -1.658410159612758, - "timestamp": 1.3260373912530579 - }, - { - "x": 5.1755309999931605, - "y": 4.382389636300601, - "heading": 0.07376563951757731, - "angularVelocity": -2.0065543705761684e-7, - "velocityX": 3.398971062820868, - "velocityY": -1.658367793257397, - "timestamp": 1.3607377939542795 - }, - { - "x": 5.295248531429034, - "y": 4.328626841825402, - "heading": 0.07376563255290919, - "angularVelocity": -2.007085673925138e-7, - "velocityX": 3.45003291364278, - "velocityY": -1.5493420908716486, - "timestamp": 1.3954381966555012 - }, - { - "x": 5.41702166916594, - "y": 4.27969803322079, - "heading": 0.07376562554616754, - "angularVelocity": -2.019210467909896e-7, - "velocityX": 3.5092716008341425, - "velocityY": -1.410035757391629, - "timestamp": 1.4301385993567228 - }, - { - "x": 5.540655427942115, - "y": 4.235681910653619, - "heading": 0.07376561844909023, - "angularVelocity": -2.0452435036769212e-7, - "velocityX": 3.5628911814277564, - "velocityY": -1.2684614338963205, - "timestamp": 1.4648390020579445 + "heading": 0.837612783139272, + "angularVelocity": 1.2997978965886135e-7, + "velocityX": 2.3324012372734173, + "velocityY": -2.9043995371065314, + "timestamp": 0.9239724774779461 + }, + { + "x": 4.272973770448513, + "y": 4.872239726327977, + "heading": 0.8376127902271764, + "angularVelocity": 1.6705024403847005e-7, + "velocityX": 2.4858652687195653, + "velocityY": -2.7741855431639553, + "timestamp": 0.9664022528871632 + }, + { + "x": 4.386494636503838, + "y": 4.762271022457296, + "heading": 0.8376127978646637, + "angularVelocity": 1.80003009789001e-7, + "velocityX": 2.67550004591034, + "velocityY": -2.5917814273132675, + "timestamp": 1.0088320282963803 + }, + { + "x": 4.507495275149132, + "y": 4.6605904045479685, + "heading": 0.8376128062844473, + "angularVelocity": 1.984404462541964e-7, + "velocityX": 2.85178597997034, + "velocityY": -2.3964448769445874, + "timestamp": 1.0512618037055974 + }, + { + "x": 4.635371955120012, + "y": 4.567705222787466, + "heading": 0.8376128158092191, + "angularVelocity": 2.2448320171896186e-7, + "velocityX": 3.013842961400671, + "velocityY": -2.189150917360859, + "timestamp": 1.0936915791148145 + }, + { + "x": 4.769486631120928, + "y": 4.484078938092311, + "heading": 0.8376128269135948, + "angularVelocity": 2.6171186455813295e-7, + "velocityX": 3.16086226494103, + "velocityY": -1.9709339464707416, + "timestamp": 1.1361213545240316 + }, + { + "x": 4.909170130626689, + "y": 4.4101288120215045, + "heading": 0.8376128403409839, + "angularVelocity": 3.1646147045577477e-7, + "velocityX": 3.2921102729998752, + "velocityY": -1.7428828071228117, + "timestamp": 1.1785511299332487 + }, + { + "x": 5.05372549373233, + "y": 4.346223825602605, + "heading": 0.8376128573444204, + "angularVelocity": 4.007430216220621e-7, + "velocityX": 3.406932082752448, + "velocityY": -1.5061353920110019, + "timestamp": 1.2209809053424658 + }, + { + "x": 5.2024314508880005, + "y": 4.292682838626271, + "heading": 0.8376128802411438, + "angularVelocity": 5.396381015902925e-7, + "velocityX": 3.5047547558634515, + "velocityY": -1.261872976228496, + "timestamp": 1.263410680751683 + }, + { + "x": 5.354546021326616, + "y": 4.249772998903826, + "heading": 0.8376129138984257, + "angularVelocity": 7.932467614948602e-7, + "velocityX": 3.58509016301729, + "velocityY": -1.0113143260504642, + "timestamp": 1.3058404561609 + }, + { + "x": 5.509310213426411, + "y": 4.217708409612342, + "heading": 0.8376129707519607, + "angularVelocity": 0.0000013399442828266796, + "velocityX": 3.647537386355726, + "velocityY": -0.7557096162361191, + "timestamp": 1.3482702315701172 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.07376561120324407, - "angularVelocity": -2.0881158710741676e-7, - "velocityX": 3.610802501559291, - "velocityY": -1.1248525394721576, - "timestamp": 1.4995394047591661 - }, - { - "x": 5.801240954312848, - "y": 4.160760783914465, - "heading": 0.0737656041692584, - "angularVelocity": -1.900588111168926e-7, - "velocityX": 3.655524839727154, - "velocityY": -0.9697042570289608, - "timestamp": 1.5365489263528271 - }, - { - "x": 5.937938725958539, - "y": 4.130679839530998, - "heading": 0.07376559729974105, - "angularVelocity": -1.8561486466113367e-7, - "velocityX": 3.6935838605680487, - "velocityY": -0.8127893333433076, - "timestamp": 1.5735584479464881 - }, - { - "x": 6.075795864969434, - "y": 4.106460978217925, - "heading": 0.07376559053845443, - "angularVelocity": -1.8269046288328192e-7, - "velocityX": 3.724910052187935, - "velocityY": -0.654395416914037, - "timestamp": 1.6105679695401491 - }, - { - "x": 6.214561046214486, - "y": 4.088147908867804, - "heading": 0.07376558383255526, - "angularVelocity": -1.8119388963756882e-7, - "velocityX": 3.7494454202515035, - "velocityY": -0.4948204829877394, - "timestamp": 1.6475774911338101 - }, - { - "x": 6.353981153300122, - "y": 4.0757714079596425, - "heading": 0.07376557713086826, - "angularVelocity": -1.8108007594786474e-7, - "velocityX": 3.7671415647133246, - "velocityY": -0.33441396633135523, - "timestamp": 1.6845870127274711 - }, - { - "x": 6.493802446577043, - "y": 4.0693566803624766, - "heading": 0.07376555560001419, - "angularVelocity": -5.817652634682061e-7, - "velocityX": 3.7779816451577393, - "velocityY": -0.17332641225670456, - "timestamp": 1.7215965343211321 - }, - { - "x": 6.62912467253087, - "y": 4.064792329127301, - "heading": 0.05903973203041549, - "angularVelocity": -0.39789283772100603, - "velocityX": 3.656416514635636, - "velocityY": -0.12332910663606324, - "timestamp": 1.7586060559147931 - }, - { - "x": 6.758568056447626, - "y": 4.061299539806061, - "heading": 0.04418585550364903, - "angularVelocity": -0.4013528380575057, - "velocityX": 3.4975697696920878, - "velocityY": -0.09437542477821895, - "timestamp": 1.7956155775084541 - }, - { - "x": 6.882108384071856, - "y": 4.058691153499139, - "heading": 0.0304778500113165, - "angularVelocity": -0.3703913182893011, - "velocityX": 3.3380687537822458, - "velocityY": -0.07047879017620408, - "timestamp": 1.8326250991021151 - }, - { - "x": 6.999742512686264, - "y": 4.056896966002031, - "heading": 0.018392940314004957, - "angularVelocity": -0.3265351503322698, - "velocityX": 3.1784828214195313, - "velocityY": -0.048479078351971995, - "timestamp": 1.8696346206957761 - }, - { - "x": 7.111470092404711, - "y": 4.055880215977976, - "heading": 0.008182013942056482, - "angularVelocity": -0.2758999828221868, - "velocityX": 3.0188874351076422, - "velocityY": -0.027472660555273344, - "timestamp": 1.9066441422894371 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -2.0237932968568665e-24, - "angularVelocity": -0.22107861949390495, - "velocityX": 2.859298314909037, - "velocityY": -0.007077363713034477, - "timestamp": 1.9436536638830981 - }, - { - "x": 7.386520754967261, - "y": 4.057483355651357, - "heading": -0.007799169879435119, - "angularVelocity": -0.11872019703645657, - "velocityX": 2.5760366812439752, - "velocityY": 0.028390383098604113, - "timestamp": 2.009347371222888 - }, - { - "x": 7.536988258489095, - "y": 4.060513288338981, - "heading": -0.011310959722615328, - "angularVelocity": -0.053457020244208106, - "velocityX": 2.2904401291217464, - "velocityY": 0.04612211443560253, - "timestamp": 2.075041078562678 - }, - { - "x": 7.668645085347693, - "y": 4.063976331623079, - "heading": -0.012040004750973126, - "angularVelocity": -0.01109763869143412, - "velocityX": 2.004101034785968, - "velocityY": 0.05271499241448283, - "timestamp": 2.1407347859024677 - }, - { - "x": 7.78147865771714, - "y": 4.06737291870627, - "heading": -0.011006488514551415, - "angularVelocity": 0.015732347560718764, - "velocityX": 1.7175704787953916, - "velocityY": 0.051703385616870434, - "timestamp": 2.2064284932422575 - }, - { - "x": 7.87549031086396, - "y": 4.070341151997571, - "heading": -0.008947695308103269, - "angularVelocity": 0.03133927570565307, - "velocityX": 1.4310602484429864, - "velocityY": 0.04518291646943911, - "timestamp": 2.2721222005820474 - }, - { - "x": 7.9506868858752515, - "y": 4.072607124350486, - "heading": -0.006421398747140829, - "angularVelocity": 0.03845568568532146, - "velocityX": 1.1446541542000461, - "velocityY": 0.03449298943040145, - "timestamp": 2.3378159079218372 - }, - { - "x": 8.00707724488628, - "y": 4.073956458693622, - "heading": -0.003864315760597132, - "angularVelocity": 0.038924321523180636, - "velocityX": 0.8583829607812892, - "velocityY": 0.020539780715319512, - "timestamp": 2.403509615261627 - }, - { - "x": 8.044670745645819, - "y": 4.074216878653992, - "heading": -0.0016276569414438076, - "angularVelocity": 0.034046774184696094, - "velocityX": 0.5722542124939418, - "velocityY": 0.003964153811924349, - "timestamp": 2.469203322601417 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 1.6866276328916314e-24, - "angularVelocity": 0.02477645131252876, - "velocityX": 0.286265117553968, - "velocityY": -0.014764317948939539, - "timestamp": 2.534897029941207 - }, - { - "x": 8.060961534281953, - "y": 4.070465444687161, - "heading": 0.0007560599723107442, - "angularVelocity": 0.010267428471842888, - "velocityX": -0.03415452911035645, - "velocityY": -0.03777341504058265, - "timestamp": 2.60853377105746 - }, - { - "x": 8.034839704830464, - "y": 4.066167075236277, - "heading": 0.00046881224437648735, - "angularVelocity": -0.0039008750737728444, - "velocityX": -0.3547390752973492, - "velocityY": -0.0583726192349958, - "timestamp": 2.6821705121737134 - }, - { - "x": 7.985098416570273, - "y": 4.060562217226684, - "heading": -0.0008319841828060777, - "angularVelocity": -0.017665046109644415, - "velocityX": -0.6754955136004895, - "velocityY": -0.0761149654999585, - "timestamp": 2.7558072532899667 - }, - { - "x": 7.9117249252468085, - "y": 4.053904178499818, - "heading": -0.0031104504440480647, - "angularVelocity": -0.03094197579500262, - "velocityX": -0.9964250211402843, - "velocityY": -0.09041734636729296, - "timestamp": 2.82944399440622 - }, - { - "x": 7.814707316285252, - "y": 4.046503723453766, - "heading": -0.00632248118792148, - "angularVelocity": -0.04361994698818144, - "velocityX": -1.3175163307185394, - "velocityY": -0.10049949161068944, - "timestamp": 2.9030807355224733 - }, - { - "x": 7.69403642851654, - "y": 4.038750888931838, - "heading": -0.010412565479377186, - "angularVelocity": -0.055544069841419484, - "velocityX": -1.6387320505969085, - "velocityY": -0.10528486736925086, - "timestamp": 2.9767174766387265 - }, - { - "x": 7.549709960346403, - "y": 4.031149265281445, - "heading": -0.015308761000708897, - "angularVelocity": -0.06649120326498265, - "velocityX": -1.9599790265335464, - "velocityY": -0.10323139692441612, - "timestamp": 3.05035421775498 - }, - { - "x": 7.381741634294396, - "y": 4.0243729190236985, - "heading": -0.02091427607708914, - "angularVelocity": -0.07612388858342581, - "velocityX": -2.2810396482216517, - "velocityY": -0.0920239836123183, - "timestamp": 3.123990958871233 - }, - { - "x": 7.1901832106568095, - "y": 4.019367673272749, - "heading": -0.027092292311801627, - "angularVelocity": -0.08389855581684406, - "velocityX": -2.6013973559091212, - "velocityY": -0.06797212471757884, - "timestamp": 3.1976276999874864 - }, - { - "x": 6.975183472586086, - "y": 4.017547771662454, - "heading": -0.03363583639032012, - "angularVelocity": -0.08886248874305737, - "velocityX": -2.9197345620074877, - "velocityY": -0.02471458653257856, - "timestamp": 3.2712644411037397 - }, - { - "x": 6.737173435740335, - "y": 4.021221144917956, - "heading": -0.04019984018910067, - "angularVelocity": -0.0891403353716819, - "velocityX": -3.232218499050576, - "velocityY": 0.04988506009116617, - "timestamp": 3.344901182219993 - }, - { - "x": 6.477592542757965, - "y": 4.034606496898433, - "heading": -0.04612188209053887, - "angularVelocity": -0.08042237898726157, - "velocityX": -3.5251545498538244, - "velocityY": 0.18177545308999152, - "timestamp": 3.4185379233362463 - }, - { - "x": 6.202379983792451, - "y": 4.065741183912862, - "heading": -0.04994434785858321, - "angularVelocity": -0.05190976284528472, - "velocityX": -3.737435345366867, - "velocityY": 0.4228145697713012, - "timestamp": 3.4921746644524996 - }, - { - "x": 5.92930929060311, - "y": 4.120418111584284, - "heading": -0.049944382856938176, - "angularVelocity": -4.752838655296773e-7, - "velocityX": -3.7083484283780552, - "velocityY": 0.7425223718836313, - "timestamp": 3.565811405568753 + "heading": 0.837613299622043, + "angularVelocity": 0.000007750926774500913, + "velocityX": 3.691782807795942, + "velocityY": -0.49633388003567147, + "timestamp": 1.3907000069793343 + }, + { + "x": 5.886145215418506, + "y": 4.18891028787692, + "heading": 0.8376134089297179, + "angularVelocity": 0.0000018480104187891978, + "velocityX": 3.7227016115303835, + "velocityY": -0.13083581208893053, + "timestamp": 1.4498488482175622 + }, + { + "x": 6.106032301025365, + "y": 4.202865338919715, + "heading": 0.8376134508974878, + "angularVelocity": 7.095281843173525e-7, + "velocityX": 3.7175214425797836, + "velocityY": 0.23593109773003054, + "timestamp": 1.5089976894557902 + }, + { + "x": 6.3234808347180325, + "y": 4.238378912467147, + "heading": 0.8376134741732665, + "angularVelocity": 3.9351199795834263e-7, + "velocityX": 3.676294059876367, + "velocityY": 0.6004103005906283, + "timestamp": 1.568146530694018 + }, + { + "x": 6.536382325934404, + "y": 4.2951066501716255, + "heading": 0.8376134896107453, + "angularVelocity": 2.6099376502854354e-7, + "velocityX": 3.599419477363716, + "velocityY": 0.9590676083746476, + "timestamp": 1.627295371932246 + }, + { + "x": 6.742672378442076, + "y": 4.372498490127808, + "heading": 0.8376135010511353, + "angularVelocity": 1.9341697541887733e-7, + "velocityX": 3.4876431759130977, + "velocityY": 1.3084252934808847, + "timestamp": 1.686444213170474 + }, + { + "x": 6.940350706341849, + "y": 4.46980400008761, + "heading": 0.8376135102232026, + "angularVelocity": 1.550675727090784e-7, + "velocityX": 3.3420490369980995, + "velocityY": 1.64509579431818, + "timestamp": 1.745593054408702 + }, + { + "x": 7.12750053081492, + "y": 4.586079652731035, + "heading": 0.837613518040013, + "angularVelocity": 1.3215492103145434e-7, + "velocityX": 3.164048873236707, + "velocityY": 1.9658145486758316, + "timestamp": 1.8047418956469299 + }, + { + "x": 7.302307170565869, + "y": 4.720197970950524, + "heading": 0.837613525049849, + "angularVelocity": 1.185118059136636e-7, + "velocityX": 2.955368796607497, + "velocityY": 2.2674716091108755, + "timestamp": 1.8638907368851578 + }, + { + "x": 7.4630756537468415, + "y": 4.8708584478992005, + "heading": 0.8376135339238266, + "angularVelocity": 1.5002791962179725e-7, + "velocityX": 2.718032675119729, + "velocityY": 2.5471416479974107, + "timestamp": 1.9230395781233858 + }, + { + "x": 7.604661415508242, + "y": 5.027831186839046, + "heading": 0.8715939059006107, + "angularVelocity": 0.5744892252398454, + "velocityX": 2.3937199579472788, + "velocityY": 2.6538599176883673, + "timestamp": 1.9821884193616137 + }, + { + "x": 7.730952710188879, + "y": 5.1694158341932015, + "heading": 0.9053997225906723, + "angularVelocity": 0.5715381059437028, + "velocityX": 2.1351440203534295, + "velocityY": 2.3937011172189955, + "timestamp": 2.0413372605998417 + }, + { + "x": 7.842513418943255, + "y": 5.295119262651075, + "heading": 0.934282004580144, + "angularVelocity": 0.48829835690517676, + "velocityX": 1.886101340600306, + "velocityY": 2.125205258909303, + "timestamp": 2.1004861018380696 + }, + { + "x": 7.939539266004037, + "y": 5.404796625949478, + "heading": 0.9566775785409475, + "angularVelocity": 0.3786308149402807, + "velocityX": 1.6403676729692935, + "velocityY": 1.8542605569679314, + "timestamp": 2.1596349430762976 + }, + { + "x": 8.0221293723114, + "y": 5.498379145291686, + "heading": 0.9718066727965543, + "angularVelocity": 0.25578006160210215, + "velocityX": 1.3963097937071225, + "velocityY": 1.5821530461652626, + "timestamp": 2.2187837843145255 + }, + { + "x": 8.090343615854179, + "y": 5.575826687480091, + "heading": 0.9792023087722939, + "angularVelocity": 0.12503433407854694, + "velocityX": 1.153264241780091, + "velocityY": 1.3093670233788228, + "timestamp": 2.2779326255527534 + }, + { + "x": 8.14422212483701, + "y": 5.637112925793437, + "heading": 0.9785539274051055, + "angularVelocity": -0.010961860851625928, + "velocityX": 0.9108971174232933, + "velocityY": 1.0361359078280012, + "timestamp": 2.3370814667909814 + }, + { + "x": 8.183793718837835, + "y": 5.682219218209322, + "heading": 0.9696403414250478, + "angularVelocity": -0.15069755879337374, + "velocityX": 0.6690172313173101, + "velocityY": 0.7625896208890375, + "timestamp": 2.3962303080292093 + }, + { + "x": 8.209080155266193, + "y": 5.71113162509295, + "heading": 0.9522961336408167, + "angularVelocity": -0.29322988280320866, + "velocityX": 0.4275051868981633, + "velocityY": 0.4888076634871, + "timestamp": 2.4553791492674373 + }, + { + "x": 8.220098495483398, + "y": 5.723839282989502, + "heading": 0.9263929393086, + "angularVelocity": -0.4379324056051909, + "velocityX": 0.18628159041742642, + "velocityY": 0.21484204306506113, + "timestamp": 2.5145279905056652 + }, + { + "x": 8.216352100309892, + "y": 5.7197578257583315, + "heading": 0.8905883238088871, + "angularVelocity": -0.5885448940291729, + "velocityX": -0.06158205358748059, + "velocityY": -0.06708969723814164, + "timestamp": 2.5753638177517315 + }, + { + "x": 8.197510123572037, + "y": 5.69852730877679, + "heading": 0.8459980645635137, + "angularVelocity": -0.7329605146161121, + "velocityX": -0.3097184272952048, + "velocityY": -0.3489804929530894, + "timestamp": 2.636199644997798 + }, + { + "x": 8.163552039603486, + "y": 5.660151329305612, + "heading": 0.7930933731491355, + "angularVelocity": -0.8696305090155458, + "velocityX": -0.5581921953851212, + "velocityY": -0.6308121580389989, + "timestamp": 2.697035472243864 + }, + { + "x": 8.114451794656734, + "y": 5.604635385719368, + "heading": 0.7324785026638988, + "angularVelocity": -0.9963679829660959, + "velocityX": -0.8070942266989081, + "velocityY": -0.9125534425906069, + "timestamp": 2.7578712994899304 + }, + { + "x": 8.05017522631974, + "y": 5.531988399816786, + "heading": 0.664956367976081, + "angularVelocity": -1.1099073974075735, + "velocityX": -1.0565578088880443, + "velocityY": -1.1941480734492433, + "timestamp": 2.8187071267359967 + }, + { + "x": 7.970675640275654, + "y": 5.442225859030612, + "heading": 0.5916510965236437, + "angularVelocity": -1.2049687621725864, + "velocityX": -1.3067889374221797, + "velocityY": -1.4754881268090099, + "timestamp": 2.879542953982063 + }, + { + "x": 7.87588580389167, + "y": 5.335377165777019, + "heading": 0.5142662873089652, + "angularVelocity": -1.2720269077903064, + "velocityX": -1.558125214613782, + "velocityY": -1.7563448725931903, + "timestamp": 2.9403787812281292 + }, + { + "x": 7.765703024679293, + "y": 5.211506902101234, + "heading": 0.4357243406909052, + "angularVelocity": -1.2910475647906803, + "velocityX": -1.8111495183046493, + "velocityY": -2.0361400392364293, + "timestamp": 3.0012146084741955 + }, + { + "x": 7.639967535619313, + "y": 5.070804612535801, + "heading": 0.36221787896188584, + "angularVelocity": -1.2082758640184799, + "velocityX": -2.0668000214973703, + "velocityY": -2.3128195330742662, + "timestamp": 3.062050435720262 + }, + { + "x": 7.498615166036793, + "y": 4.914564832190124, + "heading": 0.31493629004406687, + "angularVelocity": -0.7771997367041067, + "velocityX": -2.32350534185693, + "velocityY": -2.5682198700730225, + "timestamp": 3.122886262966328 + }, + { + "x": 7.337231161361771, + "y": 4.755477157818062, + "heading": 0.3149362776036475, + "angularVelocity": -2.0449166092827028e-7, + "velocityX": -2.652778995217139, + "velocityY": -2.615032647268723, + "timestamp": 3.1837220902123944 + }, + { + "x": 7.160583323022439, + "y": 4.613529185957489, + "heading": 0.3149362685790419, + "angularVelocity": -1.4834359997406843e-7, + "velocityX": -2.903681043488322, + "velocityY": -2.33329566287359, + "timestamp": 3.2445579174584607 + }, + { + "x": 6.970483558560949, + "y": 4.490176988250006, + "heading": 0.31493625863244995, + "angularVelocity": -1.6349891854082722e-7, + "velocityX": -3.124799531246346, + "velocityY": -2.0276242354452334, + "timestamp": 3.305393744704527 + }, + { + "x": 6.76888179398853, + "y": 4.386685862868273, + "heading": 0.31493624706316514, + "angularVelocity": -1.9017222732229534e-7, + "velocityX": -3.313865754746604, + "velocityY": -1.701154238655088, + "timestamp": 3.3662295719505932 + }, + { + "x": 6.557845947797341, + "y": 4.304117375120236, + "heading": 0.31493623270578724, + "angularVelocity": -2.3600201712655827e-7, + "velocityX": -3.4689401910752493, + "velocityY": -1.3572345686049037, + "timestamp": 3.4270653991966595 + }, + { + "x": 6.339540712904722, + "y": 4.243318473621985, + "heading": 0.3149362133315886, + "angularVelocity": -3.184669214162522e-7, + "velocityX": -3.58843209297751, + "velocityY": -0.9993930262891738, + "timestamp": 3.487901226442726 + }, + { + "x": 6.116205355143182, + "y": 4.204912805417524, + "heading": 0.3149361681436048, + "angularVelocity": -7.427857215236805e-7, + "velocityX": -3.671115654566562, + "velocityY": -0.6313001720042296, + "timestamp": 3.548737053688792 + }, + { + "x": 5.89050677740902, + "y": 4.189334421603551, + "heading": 0.31368110305887587, + "angularVelocity": -0.02063036111356669, + "velocityX": -3.7099615136532513, + "velocityY": -0.25607252369499406, + "timestamp": 3.6095728809348584 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -0.05351191132153618, - "angularVelocity": -0.04844766906462906, - "velocityX": -3.5764423817525626, - "velocityY": 1.0352299927261919, - "timestamp": 3.639448146685006 - }, - { - "x": 5.427595279774474, - "y": 4.2855510061769815, - "heading": -0.058564536366975395, - "angularVelocity": -0.07109019818968403, - "velocityX": -3.3536640954167023, - "velocityY": 1.2508460219498765, - "timestamp": 3.7105215882297555 - }, - { - "x": 5.210001648120265, - "y": 4.381839057848659, - "heading": -0.06451807274962795, - "angularVelocity": -0.08376597858855642, - "velocityX": -3.0615322253278787, - "velocityY": 1.3547683857556179, - "timestamp": 3.781595029774505 - }, - { - "x": 5.014433901254649, - "y": 4.478870971040357, - "heading": -0.07079419051912546, - "angularVelocity": -0.08830468362146032, - "velocityX": -2.751629056016378, - "velocityY": 1.365234482568344, - "timestamp": 3.8526684713192543 - }, - { - "x": 4.840611358699117, - "y": 4.572318839972298, - "heading": -0.07701240147326043, - "angularVelocity": -0.08748993743633168, - "velocityX": -2.445675047916283, - "velocityY": 1.3148071473801226, - "timestamp": 3.9237419128640036 - }, - { - "x": 4.687900440256207, - "y": 4.6594427038525446, - "heading": -0.08292539389195669, - "angularVelocity": -0.08319552691103697, - "velocityX": -2.148635483575938, - "velocityY": 1.2258286919367853, - "timestamp": 3.994815354408753 - }, - { - "x": 4.555672965258425, - "y": 4.738435503151125, - "heading": -0.088365740210127, - "angularVelocity": -0.07654541837185252, - "velocityX": -1.8604343918610988, - "velocityY": 1.1114249933829867, - "timestamp": 4.065888795953502 - }, - { - "x": 4.443385396304237, - "y": 4.808043311257638, - "heading": -0.09321471505627854, - "angularVelocity": -0.06822484940592717, - "velocityX": -1.5798808459765967, - "velocityY": 0.9793786060392976, - "timestamp": 4.136962237498252 - }, - { - "x": 4.3505830000627, - "y": 4.867355530244293, - "heading": -0.09738458875338916, - "angularVelocity": -0.058669927985478765, - "velocityX": -1.3057253768006336, - "velocityY": 0.8345201484201303, - "timestamp": 4.208035679043001 - }, - { - "x": 4.2768869568191645, - "y": 4.915685373463825, - "heading": -0.10080825239426806, - "angularVelocity": -0.04817078737805652, - "velocityX": -1.0368998833007734, - "velocityY": 0.6799986347798151, - "timestamp": 4.2791091205877505 - }, - { - "x": 4.22197990513087, - "y": 4.95249852612905, - "heading": -0.10343286860563979, - "angularVelocity": -0.03692822739868577, - "velocityX": -0.7725396504645569, - "velocityY": 0.5179593370618798, - "timestamp": 4.3501825621325 - }, - { - "x": 4.185593591353172, - "y": 4.97736854606154, - "heading": -0.10521581647883109, - "angularVelocity": -0.025085993226720307, - "velocityX": -0.5119537338682039, - "velocityY": 0.3499200178287514, - "timestamp": 4.421256003677249 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.012749959780663622, - "velocityX": -0.25458913429159136, - "velocityY": 0.1769894567221649, - "timestamp": 4.492329445221999 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": 1.153459712624117e-25, - "velocityX": -3.247224190829055e-25, - "velocityY": -4.023670796703132e-25, - "timestamp": 4.563402886766748 + "heading": 0.30754704224604024, + "angularVelocity": -0.10082974277681633, + "velocityX": -3.691164544865137, + "velocityY": 0.12023594125393282, + "timestamp": 3.6704087081809247 + }, + { + "x": 5.419243898269495, + "y": 4.233180640603145, + "heading": 0.29443096525634566, + "angularVelocity": -0.19257128900213308, + "velocityX": -3.6221840549970006, + "velocityY": 0.5363593678805259, + "timestamp": 3.7385189496279203 + }, + { + "x": 5.182010137227644, + "y": 4.296746845588596, + "heading": 0.2703885847974574, + "angularVelocity": -0.35299214843626897, + "velocityX": -3.483085010445408, + "velocityY": 0.9332840940656272, + "timestamp": 3.806629191074917 + }, + { + "x": 4.96090074800009, + "y": 4.383753869716349, + "heading": 0.22896738213665188, + "angularVelocity": -0.6081494028036839, + "velocityX": -3.2463456967719417, + "velocityY": 1.2774440712482642, + "timestamp": 3.8747394325219133 + }, + { + "x": 4.76451355722713, + "y": 4.485181561673583, + "heading": 0.17582209664433623, + "angularVelocity": -0.7802833224967138, + "velocityX": -2.8833724062744586, + "velocityY": 1.489169467064151, + "timestamp": 3.94284967396891 + }, + { + "x": 4.596735729440321, + "y": 4.5894452853276455, + "heading": 0.11854658262999372, + "angularVelocity": -0.8409236672419458, + "velocityX": -2.4633274559359006, + "velocityY": 1.5308083107472321, + "timestamp": 4.010959915415906 + }, + { + "x": 4.457111536094513, + "y": 4.687809707372902, + "heading": 0.06296288370262228, + "angularVelocity": -0.8160843031312246, + "velocityX": -2.049973548463538, + "velocityY": 1.444194293773058, + "timestamp": 4.079070156862903 + }, + { + "x": 4.343947453919847, + "y": 4.774964113624853, + "heading": 0.012793626993843597, + "angularVelocity": -0.7365890304150604, + "velocityX": -1.6614840847793941, + "velocityY": 1.2796079473565642, + "timestamp": 4.147180398309899 + }, + { + "x": 4.255603402479856, + "y": 4.847699861550099, + "heading": -0.029623031457827412, + "angularVelocity": -0.622764764160759, + "velocityX": -1.2970744129389131, + "velocityY": 1.0679120552207846, + "timestamp": 4.215290639756896 + }, + { + "x": 4.190737568538254, + "y": 4.903969414198745, + "heading": -0.06275873479049468, + "angularVelocity": -0.4865010404999608, + "velocityX": -0.9523653501078979, + "velocityY": 0.8261540621968694, + "timestamp": 4.283400881203892 + }, + { + "x": 4.148285991242422, + "y": 4.94238614886978, + "heading": -0.0855582962836916, + "angularVelocity": -0.3347449812072629, + "velocityX": -0.6232774454171954, + "velocityY": 0.5640375640267039, + "timestamp": 4.351511122650889 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.17170074221519835, + "velocityX": -0.3066229399994914, + "velocityY": 0.28741048233214816, + "timestamp": 4.419621364097885 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -9.317687373474837e-25, + "velocityX": -1.6510663048617945e-23, + "velocityY": -3.5986344010888627e-23, + "timestamp": 4.487731605544882 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.5.traj b/src/main/deploy/choreo/CenterLanePCBAFE.5.traj index 2eceaa32..9959d2fa 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.5.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.5.traj @@ -1,661 +1,508 @@ { "samples": [ { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": 1.153459712624117e-25, - "velocityX": -3.247224190829055e-25, - "velocityY": -4.023670796703132e-25, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -9.317687373474837e-25, + "velocityX": -1.6510663048617945e-23, + "velocityY": -3.5986344010888627e-23, "timestamp": 0 }, { - "x": 4.180652954196475, - "y": 4.972113880358222, - "heading": -0.10282545923039073, - "angularVelocity": 0.04623963021087373, - "velocityX": 0.18450581877512717, - "velocityY": -0.2501512088012644, - "timestamp": 0.07129254179965372 - }, - { - "x": 4.207780209062833, - "y": 4.937082603957029, - "heading": -0.09606696501055557, - "angularVelocity": 0.09479945656626756, - "velocityX": 0.380506209788268, - "velocityY": -0.4913736488683167, - "timestamp": 0.14258508359930744 - }, - { - "x": 4.249890859346282, - "y": 4.885729175148701, - "heading": -0.08564531821991797, - "angularVelocity": 0.1461814451773148, - "velocityX": 0.5906739922639744, - "velocityY": -0.7203197909907696, - "timestamp": 0.21387762539896116 - }, - { - "x": 4.308238374054552, - "y": 4.819303841665251, - "heading": -0.07131614738285201, - "angularVelocity": 0.20099116226398103, - "velocityX": 0.8184238243635266, - "velocityY": -0.931729067398359, - "timestamp": 0.2851701671986149 - }, - { - "x": 4.384365934500173, - "y": 4.739668033977246, - "heading": -0.05278830976865122, - "angularVelocity": 0.2598846547829301, - "velocityX": 1.0678194173459812, - "velocityY": -1.1170285934228166, - "timestamp": 0.3564627089982686 - }, - { - "x": 4.480080041577144, - "y": 4.649696456670524, - "heading": -0.02973745107706772, - "angularVelocity": 0.3233277718777487, - "velocityX": 1.3425542793234355, - "velocityY": -1.262005464183884, - "timestamp": 0.4277552507979223 - }, - { - "x": 4.597147014770728, - "y": 4.553865445568556, - "heading": -0.0018737410680010795, - "angularVelocity": 0.3908362544762254, - "velocityX": 1.642064797220515, - "velocityY": -1.344194058493121, - "timestamp": 0.49904779259757603 - }, - { - "x": 4.7363464626810785, - "y": 4.458651640127929, - "heading": 0.030870956626381736, - "angularVelocity": 0.4593004663293065, - "velocityX": 1.9525106609542362, - "velocityY": -1.3355366920174645, - "timestamp": 0.5703403343972298 - }, - { - "x": 4.8961444414371185, - "y": 4.371572487351198, - "heading": 0.06809613303796244, - "angularVelocity": 0.5221468539611136, - "velocityX": 2.2414403347422116, - "velocityY": -1.2214342563551692, - "timestamp": 0.6416328761968835 - }, - { - "x": 5.072824953795758, - "y": 4.298842121244781, - "heading": 0.10897366885891092, - "angularVelocity": 0.5733774499978155, - "velocityX": 2.478246782884325, - "velocityY": -1.0201679484342572, - "timestamp": 0.7129254179965372 - }, - { - "x": 5.262214444102689, - "y": 4.244271073764123, - "heading": 0.1525665304554643, - "angularVelocity": 0.6114645444828949, - "velocityX": 2.6565119650124474, - "velocityY": -0.7654524036190654, - "timestamp": 0.7842179597961909 - }, - { - "x": 5.460811083302549, - "y": 4.20986764407754, - "heading": 0.19806311217601916, - "angularVelocity": 0.638167479684045, - "velocityX": 2.785657997129001, - "velocityY": -0.48256702339585567, - "timestamp": 0.8555105015958446 + "x": 4.147821870498822, + "y": 4.951212055722382, + "heading": -0.09513615443770512, + "angularVelocity": 0.0346947715699437, + "velocityX": 0.3347010455989641, + "velocityY": -0.1761961456823413, + "timestamp": 0.06100979366948245 + }, + { + "x": 4.188661953327048, + "y": 4.929712670962105, + "heading": -0.09090262623774811, + "angularVelocity": 0.06939096078396689, + "velocityX": 0.6694020807458456, + "velocityY": -0.3523923532137878, + "timestamp": 0.1220195873389649 + }, + { + "x": 4.2499220745882305, + "y": 4.89746358708528, + "heading": -0.08455181431110974, + "angularVelocity": 0.10409495827905127, + "velocityX": 1.004103072255173, + "velocityY": -0.5285886402359044, + "timestamp": 0.18302938100844734 + }, + { + "x": 4.331602229572044, + "y": 4.854464798258643, + "heading": -0.07608283990612706, + "angularVelocity": 0.13881335922659968, + "velocityX": 1.3388039865584789, + "velocityY": -0.7047850228699344, + "timestamp": 0.2440391746779298 + }, + { + "x": 4.433702411475643, + "y": 4.800716297797124, + "heading": -0.06549440517861618, + "angularVelocity": 0.17355303289293356, + "velocityX": 1.6735047893575954, + "velocityY": -0.8809815150777099, + "timestamp": 0.30504896834741224 + }, + { + "x": 4.556222611314502, + "y": 4.736218078349401, + "heading": -0.05278477279271126, + "angularVelocity": 0.20832118290316787, + "velocityX": 2.0082054448930773, + "velocityY": -1.0571781277795957, + "timestamp": 0.3660587620168947 + }, + { + "x": 4.699162817715129, + "y": 4.660970132179323, + "heading": -0.03795174297125847, + "angularVelocity": 0.24312538904507433, + "velocityX": 2.3429059140077895, + "velocityY": -1.2333748672832447, + "timestamp": 0.42706855568637714 + }, + { + "x": 4.862523016289189, + "y": 4.574972451706886, + "heading": -0.02099263124198345, + "angularVelocity": 0.27797359586478865, + "velocityX": 2.677606147286025, + "velocityY": -1.4095717310293654, + "timestamp": 0.4880783493558596 + }, + { + "x": 5.046303186466893, + "y": 4.478225031445458, + "heading": -0.0019042656986956292, + "angularVelocity": 0.3128737927995283, + "velocityX": 3.0123060434087328, + "velocityY": -1.5857686847058003, + "timestamp": 0.549088143025342 + }, + { + "x": 5.247403827752691, + "y": 4.372365027625972, + "heading": -0.0019042643151741538, + "angularVelocity": 2.2677039080484546e-8, + "velocityX": 3.2962026125714896, + "velocityY": -1.735131319948009, + "timestamp": 0.6100979366948245 + }, + { + "x": 5.4522102780182164, + "y": 4.273865655817337, + "heading": -0.0019042629287868294, + "angularVelocity": 2.2724012670003775e-8, + "velocityX": 3.356943827331282, + "velocityY": -1.6144845914780435, + "timestamp": 0.6711077303643069 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.24483305037299086, - "angularVelocity": 0.6560284851170571, - "velocityX": 2.8774488935285283, - "velocityY": -0.18541307673169433, - "timestamp": 0.9268030433954983 - }, - { - "x": 5.802993350275697, - "y": 4.197353588266771, - "heading": 0.2759731999769641, - "angularVelocity": 0.6634007595054764, - "velocityX": 2.919495150576939, - "velocityY": 0.015008756790958989, - "timestamp": 0.9737432211956953 - }, - { - "x": 5.941816707126591, - "y": 4.207504283843094, - "heading": 0.3073984177151568, - "angularVelocity": 0.669473768760635, - "velocityX": 2.9574527272095095, - "velocityY": 0.21624748886826958, - "timestamp": 1.0206833989958923 - }, - { - "x": 6.082189718099688, - "y": 4.227142107895406, - "heading": 0.33903536512016264, - "angularVelocity": 0.6739843964730912, - "velocityX": 2.990466111368347, - "velocityY": 0.4183585357494954, - "timestamp": 1.0676235767960893 - }, - { - "x": 6.223826665767993, - "y": 4.256309424549035, - "heading": 0.37079417516105295, - "angularVelocity": 0.6765805228960279, - "velocityX": 3.01739265392617, - "velocityY": 0.6213720957295971, - "timestamp": 1.1145637545962863 - }, - { - "x": 6.366368317456564, - "y": 4.295047066484075, - "heading": 0.4025622544615229, - "angularVelocity": 0.6767779925268211, - "velocityX": 3.0366662072586053, - "velocityY": 0.8252555433413251, - "timestamp": 1.1615039323964833 - }, - { - "x": 6.509351710185931, - "y": 4.3433874902945675, - "heading": 0.4341948054163167, - "angularVelocity": 0.6738907357666759, - "velocityX": 3.0460769308966165, - "velocityY": 1.0298304368648719, - "timestamp": 1.2084441101966803 - }, - { - "x": 6.652162883267978, - "y": 4.401339041781646, - "heading": 0.4654999103087445, - "angularVelocity": 0.6669149193613809, - "velocityX": 3.0424080132360682, - "velocityY": 1.2345831269270133, - "timestamp": 1.2553842879968773 - }, - { - "x": 6.793961557546229, - "y": 4.468849069286898, - "heading": 0.4962146580453544, - "angularVelocity": 0.654338120902485, - "velocityX": 3.0208380309469742, - "velocityY": 1.4382141412546399, - "timestamp": 1.3023244657970743 - }, - { - "x": 6.933563512190389, - "y": 4.545714915555555, - "heading": 0.5259679790741631, - "angularVelocity": 0.6338561638060835, - "velocityX": 2.9740397498786737, - "velocityY": 1.6375278039175576, - "timestamp": 1.3492646435972713 - }, - { - "x": 7.069284940391168, - "y": 4.631368590173797, - "heading": 0.5542337619189218, - "angularVelocity": 0.6021660796657563, - "velocityX": 2.8913701345248803, - "velocityY": 1.8247411627375887, - "timestamp": 1.3962048213974683 - }, - { - "x": 7.1988866969659675, - "y": 4.724417833549329, - "heading": 0.5803339735600787, - "angularVelocity": 0.5560313757705206, - "velocityX": 2.7609984164621943, - "velocityY": 1.9822942250367808, - "timestamp": 1.4431449991976653 - }, - { - "x": 7.32010179733884, - "y": 4.822118790801402, - "heading": 0.6036855499367045, - "angularVelocity": 0.49747524340496785, - "velocityX": 2.5823315132896965, - "velocityY": 2.0813929948868157, - "timestamp": 1.4900851769978622 - }, - { - "x": 7.431787144885325, - "y": 4.920866698749227, - "heading": 0.6241547457610245, - "angularVelocity": 0.43606984003016963, - "velocityX": 2.379312409549823, - "velocityY": 2.103696930338622, - "timestamp": 1.5370253547980592 - }, - { - "x": 7.534091847804722, - "y": 5.017622526169069, - "heading": 0.6419335715808812, - "angularVelocity": 0.37875497394860314, - "velocityX": 2.1794698638522156, - "velocityY": 2.0612582217239557, - "timestamp": 1.5839655325982562 - }, - { - "x": 7.627668873285194, - "y": 5.110394301950092, - "heading": 0.6572905320305523, - "angularVelocity": 0.32716025309998836, - "velocityX": 1.9935379426721815, - "velocityY": 1.9763831354859511, - "timestamp": 1.6309057103984532 - }, - { - "x": 7.7131909565949455, - "y": 5.197947878934244, - "heading": 0.6704631268278166, - "angularVelocity": 0.2806251576918574, - "velocityX": 1.8219377794813416, - "velocityY": 1.8652161343918634, - "timestamp": 1.6778458881986502 - }, - { - "x": 7.791223380396529, - "y": 5.279496011851437, - "heading": 0.6816390937397763, - "angularVelocity": 0.2380895734892706, - "velocityX": 1.6623802349818717, - "velocityY": 1.7372778872782495, - "timestamp": 1.7247860659988472 - }, - { - "x": 7.86221923742719, - "y": 5.354510990808827, - "heading": 0.6909640471575174, - "angularVelocity": 0.19865611624721707, - "velocityX": 1.5124752473852405, - "velocityY": 1.5980974609149294, - "timestamp": 1.7717262437990442 - }, - { - "x": 7.926540429785996, - "y": 5.422621631484432, - "heading": 0.6985519881256509, - "angularVelocity": 0.1616513043566187, - "velocityX": 1.370280117655087, - "velocityY": 1.4510094308871155, - "timestamp": 1.8186664215992412 - }, - { - "x": 7.984478950500488, - "y": 5.483555793762207, - "heading": 0.7044936776046891, - "angularVelocity": 0.12658003777337848, - "velocityX": 1.2343055231087734, - "velocityY": 1.2981238063720744, - "timestamp": 1.8656065993994382 - }, - { - "x": 8.047923980956542, - "y": 5.548807333254465, - "heading": 0.7094925329826927, - "angularVelocity": 0.08420982403970191, - "velocityX": 1.0687836408324876, - "velocityY": 1.099215769102154, - "timestamp": 1.9249685020635123 - }, - { - "x": 8.10152245585906, - "y": 5.602265273787256, - "heading": 0.7122491464591354, - "angularVelocity": 0.04643741781732065, - "velocityX": 0.9029103262715175, - "velocityY": 0.9005429094027965, - "timestamp": 1.9843304047275865 - }, - { - "x": 8.14525837183985, - "y": 5.64394060886005, - "heading": 0.7129744623769576, - "angularVelocity": 0.012218542285053226, - "velocityX": 0.7367674218309503, - "velocityY": 0.7020552442301676, - "timestamp": 2.0436923073916606 - }, - { - "x": 8.179119065532058, - "y": 5.673842216437666, - "heading": 0.7118363633541982, - "angularVelocity": -0.01917221267653394, - "velocityX": 0.5704111925762211, - "velocityY": 0.5037171356657365, - "timestamp": 2.103054210055735 - }, - { - "x": 8.203094267741402, - "y": 5.691977414243475, - "heading": 0.7089715992476469, - "angularVelocity": -0.048259303997766526, - "velocityX": 0.40388197031045997, - "velocityY": 0.3055023001610055, - "timestamp": 2.162416112719809 - }, - { - "x": 8.217175483703613, - "y": 5.698352336883545, - "heading": 0.7044936776046891, - "angularVelocity": -0.07543426746777364, - "velocityX": 0.23720964676445846, - "velocityY": 0.10739080713341471, - "timestamp": 2.221778015383883 - }, - { - "x": 8.220424741398494, - "y": 5.690497924270521, - "heading": 0.6974771095930071, - "angularVelocity": -0.10438216452389025, - "velocityX": 0.04833767031445264, - "velocityY": -0.11684638248302778, - "timestamp": 2.288998004964345 - }, - { - "x": 8.210761211514034, - "y": 5.66775585785793, - "heading": 0.6884846443385353, - "angularVelocity": -0.1337766535013804, - "velocityX": -0.14375976468862095, - "velocityY": -0.338322968428447, - "timestamp": 2.3562179945448065 - }, - { - "x": 8.187932028417118, - "y": 5.630349579704253, - "heading": 0.6774812110255055, - "angularVelocity": -0.1636928744218101, - "velocityX": -0.33961896214800763, - "velocityY": -0.556475512524468, - "timestamp": 2.4234379841252682 - }, - { - "x": 8.15163887353547, - "y": 5.578552873386559, - "heading": 0.6644254264440203, - "angularVelocity": -0.19422473378782149, - "velocityX": -0.5399161039471123, - "velocityY": -0.7705551078030757, - "timestamp": 2.49065797370573 - }, - { - "x": 8.101525228747773, - "y": 5.512708245233119, - "heading": 0.6492678901971639, - "angularVelocity": -0.225491499499757, - "velocityX": -0.745516997257366, - "velocityY": -0.9795393983901896, - "timestamp": 2.5578779632861917 - }, - { - "x": 8.037158859322696, - "y": 5.433255022389235, - "heading": 0.6319488484169726, - "angularVelocity": -0.2576471952507599, - "velocityX": -0.9575480422833206, - "velocityY": -1.1819880267725829, - "timestamp": 2.6250979528666534 - }, - { - "x": 7.958007564987826, - "y": 5.340774148745389, - "heading": 0.6123949630340744, - "angularVelocity": -0.29089390678188665, - "velocityX": -1.177496379110975, - "velocityY": -1.3757942275957413, - "timestamp": 2.692317942447115 - }, - { - "x": 7.863405980160758, - "y": 5.236063150659359, - "heading": 0.5905148719771925, - "angularVelocity": -0.3254997686468183, - "velocityX": -1.4073430450897342, - "velocityY": -1.5577360059048004, - "timestamp": 2.759537932027577 - }, - { - "x": 7.752513083642703, - "y": 5.120268560189199, - "heading": 0.5661934059220105, - "angularVelocity": -0.36181895009176485, - "velocityX": -1.649701185765849, - "velocityY": -1.7226213689241165, - "timestamp": 2.8267579216080385 - }, - { - "x": 7.624272333970784, - "y": 4.995132700896389, - "heading": 0.5392856965961144, - "angularVelocity": -0.40029326832441703, - "velocityX": -1.9077769941992746, - "velocityY": -1.8615870081774413, - "timestamp": 2.8939779111885002 - }, - { - "x": 7.477443680694748, - "y": 4.863466852853807, - "heading": 0.5096191616794202, - "angularVelocity": -0.4413350121273609, - "velocityX": -2.1843004468229417, - "velocityY": -1.958730563101009, - "timestamp": 2.961197900768962 - }, - { - "x": 7.310999420897597, - "y": 4.729976120271212, - "heading": 0.47703933517045805, - "angularVelocity": -0.48467467359488975, - "velocityX": -2.4761125497932377, - "velocityY": -1.9858785075056882, - "timestamp": 3.0284178903494237 - }, - { - "x": 7.12561513664023, - "y": 4.601908860623506, - "heading": 0.44160618415094194, - "angularVelocity": -0.5271222331432077, - "velocityX": -2.757874337892657, - "velocityY": -1.9051960651438506, - "timestamp": 3.0956378799298854 - }, - { - "x": 6.9255695292750366, - "y": 4.487009075034074, - "heading": 0.4039304316786653, - "angularVelocity": -0.5604843545412803, - "velocityX": -2.9759839091575766, - "velocityY": -1.709309779822242, - "timestamp": 3.162857869510347 - }, - { - "x": 6.716882603619491, - "y": 4.389825418108365, - "heading": 0.36497586306620833, - "angularVelocity": -0.5795086975702185, - "velocityX": -3.1045367153136607, - "velocityY": -1.445755310767803, - "timestamp": 3.230077859090809 - }, - { - "x": 6.504347696212581, - "y": 4.311975808393504, - "heading": 0.32552791036504214, - "angularVelocity": -0.5868485393611587, - "velocityX": -3.1617813203096174, - "velocityY": -1.1581318325209748, - "timestamp": 3.2972978486712705 - }, - { - "x": 6.29116656289608, - "y": 4.2538292064786, - "heading": 0.2861053737979932, - "angularVelocity": -0.5864704355519208, - "velocityX": -3.1713949175985174, - "velocityY": -0.8650195020530711, - "timestamp": 3.3645178382517322 - }, - { - "x": 6.079464519610655, - "y": 4.215340449642915, - "heading": 0.24704786030835676, - "angularVelocity": -0.5810401598305069, - "velocityX": -3.1493911945942843, - "velocityY": -0.572579036026387, - "timestamp": 3.431737827832194 - }, - { - "x": 5.870707606275243, - "y": 4.1963435376359834, - "heading": 0.20858746560478297, - "angularVelocity": -0.5721571059980171, - "velocityX": -3.1055778889333454, - "velocityY": -0.28260807723263415, - "timestamp": 3.4989578174126557 + "heading": -0.001904261510303375, + "angularVelocity": 2.3250094273072788e-8, + "velocityX": 3.503395732831278, + "velocityY": -1.265642393108385, + "timestamp": 0.7321175240337894 + }, + { + "x": 5.815067432185188, + "y": 4.154195032102827, + "heading": -0.0019042601141906403, + "angularVelocity": 3.3542788088424975e-8, + "velocityX": 3.5826307679474456, + "velocityY": -1.0199942412452387, + "timestamp": 0.7737393692706682 + }, + { + "x": 5.966765079106725, + "y": 4.122169062842087, + "heading": -0.0019042587680997514, + "angularVelocity": 3.234097098196836e-8, + "velocityX": 3.6446641435091407, + "velocityY": -0.7694509716826321, + "timestamp": 0.8153612145075471 + }, + { + "x": 6.1203162156456985, + "y": 4.100724080023554, + "heading": -0.0019042574433880786, + "angularVelocity": 3.182731724746444e-8, + "velocityX": 3.6891957976654504, + "velocityY": -0.5152338320534735, + "timestamp": 0.8569830597444259 + }, + { + "x": 6.274166659063129, + "y": 4.081543884935332, + "heading": -0.001904256120802589, + "angularVelocity": 3.177623389629938e-8, + "velocityX": 3.6963868983183503, + "velocityY": -0.46082039321089135, + "timestamp": 0.8986049049813047 + }, + { + "x": 6.428017114313601, + "y": 4.062363784763932, + "heading": -0.0019042547982171636, + "angularVelocity": 3.1776232361520234e-8, + "velocityX": 3.696387182617153, + "velocityY": -0.4608181127540811, + "timestamp": 0.9402267502181836 + }, + { + "x": 6.581867569564624, + "y": 4.043183684596952, + "heading": -0.0019042534756317344, + "angularVelocity": 3.177623245008594e-8, + "velocityX": 3.696387182630397, + "velocityY": -0.46081811264784767, + "timestamp": 0.9818485954550624 + }, + { + "x": 6.7357180248600645, + "y": 4.024003584786261, + "heading": -0.0019042521530462726, + "angularVelocity": 3.1776233230368237e-8, + "velocityX": 3.6963871836975604, + "velocityY": -0.46081810408775026, + "timestamp": 1.0234704406919413 + }, + { + "x": 6.88956959654859, + "y": 4.00483244208887, + "heading": -0.0019042508304574486, + "angularVelocity": 3.1776314012648856e-8, + "velocityX": 3.696414005984728, + "velocityY": -0.4606029018724078, + "timestamp": 1.06509228592882 + }, + { + "x": 7.044365422308842, + "y": 3.996109423647668, + "heading": -0.001904249277450269, + "angularVelocity": 3.7312309699136395e-8, + "velocityX": 3.7191005079010213, + "velocityY": -0.20957788852360654, + "timestamp": 1.106714131165699 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -4.70962468044917e-24, + "angularVelocity": 0.04575119787728785, + "velocityX": 3.664528906453967, + "velocityY": 0.04245896540136021, + "timestamp": 1.1483359764025778 + }, + { + "x": 7.4117376833527, + "y": 4.005713687749523, + "heading": 0.0009657064358366508, + "angularVelocity": 0.014669397578232305, + "velocityX": 3.2636086584468615, + "velocityY": 0.11904726359911169, + "timestamp": 1.2141673393257317 + }, + { + "x": 7.599748212147598, + "y": 4.014970056100235, + "heading": 0.001284059261189157, + "angularVelocity": 0.004835883858642321, + "velocityX": 2.8559416127289494, + "velocityY": 0.14060727197030365, + "timestamp": 1.2799987022488857 + }, + { + "x": 7.760889475419079, + "y": 4.024789101797907, + "heading": 0.0012842515922686665, + "angularVelocity": 0.0000029215721955193207, + "velocityX": 2.447788654468283, + "velocityY": 0.14915452546736505, + "timestamp": 1.3458300651720396 + }, + { + "x": 7.8951561065625, + "y": 4.03478916193552, + "heading": 0.0011129009866924844, + "angularVelocity": -0.002602871913440434, + "velocityX": 2.0395541757224747, + "velocityY": 0.15190419419518753, + "timestamp": 1.4116614280951936 + }, + { + "x": 8.002547492527386, + "y": 4.044754390856472, + "heading": 0.0008529249221134859, + "angularVelocity": -0.003949121710915689, + "velocityX": 1.6313103845388242, + "velocityY": 0.1513750965870757, + "timestamp": 1.4774927910183475 + }, + { + "x": 8.083064163245998, + "y": 4.054546008914513, + "heading": 0.0005576380815616831, + "angularVelocity": -0.004485503982296319, + "velocityX": 1.223074643200086, + "velocityY": 0.1487378906232164, + "timestamp": 1.5433241539415015 + }, + { + "x": 8.136706915836411, + "y": 4.064067288015535, + "heading": 0.00026420208220015364, + "angularVelocity": -0.004457389097413355, + "velocityX": 0.8148510103464127, + "velocityY": 0.14463135317639367, + "timestamp": 1.6091555168646554 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 7.663456254811148e-24, + "angularVelocity": -0.004013316305004348, + "velocityX": 0.40663971509805397, + "velocityY": 0.13944216629333933, + "timestamp": 1.6749868797878094 + }, + { + "x": 8.158068779301736, + "y": 4.083431553094837, + "heading": -0.0002400975033088844, + "angularVelocity": -0.0031214685775050673, + "velocityX": -0.07030570953345122, + "velocityY": 0.1324082915016235, + "timestamp": 1.751905002571153 + }, + { + "x": 8.115975249648498, + "y": 4.093075117537315, + "heading": -0.0004115957026821067, + "angularVelocity": -0.002229620187901403, + "velocityX": -0.5472511305534997, + "velocityY": 0.12537441234287078, + "timestamp": 1.8288231253544964 + }, + { + "x": 8.037195973910658, + "y": 4.10217764875176, + "heading": -0.0005144945686699745, + "angularVelocity": -0.0013377714154270807, + "velocityX": -1.024196546758428, + "velocityY": 0.11834052736940634, + "timestamp": 1.90574124813784 + }, + { + "x": 7.921730952606701, + "y": 4.110739146113785, + "heading": -0.000548794095929675, + "angularVelocity": -0.0004459225734917171, + "velocityX": -1.501141956222563, + "velocityY": 0.11130663427836081, + "timestamp": 1.9826593709211835 + }, + { + "x": 7.769580186514303, + "y": 4.11875960869081, + "heading": -0.0005144943077910091, + "angularVelocity": 0.00044592596513670077, + "velocityX": -1.9780873555763094, + "velocityY": 0.10427272906302729, + "timestamp": 2.059577493704527 + }, + { + "x": 7.580743676929442, + "y": 4.126239034937878, + "heading": -0.00041159526627641076, + "angularVelocity": 0.001337773697421547, + "velocityX": -2.455032738081228, + "velocityY": 0.09723880376198106, + "timestamp": 2.1364956164878706 + }, + { + "x": 7.35522142644369, + "y": 4.133177421791385, + "heading": -0.00024009710111365826, + "angularVelocity": 0.0022296197431366516, + "velocityX": -2.9319780868935523, + "velocityY": 0.09020483863145769, + "timestamp": 2.213413739271214 + }, + { + "x": 7.093013442829981, + "y": 4.139574760181112, + "heading": -1.4530480095899495e-10, + "angularVelocity": 0.003121461459546291, + "velocityX": -3.4089233346512358, + "velocityY": 0.08317075558053526, + "timestamp": 2.2903318620545576 + }, + { + "x": 6.806524506833513, + "y": 4.13534985058908, + "heading": -1.168340521484347e-10, + "angularVelocity": 3.7014357319604335e-10, + "velocityX": -3.7245960461544843, + "velocityY": -0.05492736222818408, + "timestamp": 2.367249984837901 + }, + { + "x": 6.520035572895023, + "y": 4.131124801447752, + "heading": -8.836340568265332e-11, + "angularVelocity": 3.701422426282247e-10, + "velocityX": -3.7245960193990517, + "velocityY": -0.054929176485862014, + "timestamp": 2.4441681076212447 + }, + { + "x": 6.233546635992582, + "y": 4.126899953288603, + "heading": -5.989279962924638e-11, + "angularVelocity": 3.7014171723351935e-10, + "velocityX": -3.724596057932901, + "velocityY": -0.05492656354927405, + "timestamp": 2.5210862304045882 + }, + { + "x": 5.947511708921411, + "y": 4.143566708826361, + "heading": -3.09908077767978e-11, + "angularVelocity": 3.7575009382191746e-10, + "velocityX": -3.7186935499823517, + "velocityY": 0.21668177712428321, + "timestamp": 2.5980043531879318 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.17089065340850093, - "angularVelocity": -0.560797650097213, - "velocityX": -3.0460563700229573, - "velocityY": 0.004545328262717531, - "timestamp": 3.5661778069931174 - }, - { - "x": 5.441376331690843, - "y": 4.221257817184443, - "heading": 0.12955296713969652, - "angularVelocity": -0.5459093121032401, - "velocityX": -2.965763486744691, - "velocityY": 0.3249853335649047, - "timestamp": 3.6419004322576356 - }, - { - "x": 5.2258290849692735, - "y": 4.2691932604024965, - "heading": 0.08990916431610245, - "angularVelocity": -0.5235397304980968, - "velocityX": -2.8465368965828737, - "velocityY": 0.6330399012263854, - "timestamp": 3.717623057522154 - }, - { - "x": 5.023235975053, - "y": 4.338521550335442, - "heading": 0.05269444989052449, - "angularVelocity": -0.4914609642174678, - "velocityX": -2.6754633665772425, - "velocityY": 0.9155558155936205, - "timestamp": 3.793345682786672 - }, - { - "x": 4.838427025225484, - "y": 4.425425000911894, - "heading": 0.018769229111462726, - "angularVelocity": -0.44801960656478235, - "velocityX": -2.440604101903904, - "velocityY": 1.1476550142427961, - "timestamp": 3.8690683080511903 - }, - { - "x": 4.676043362291933, - "y": 4.523369450086871, - "heading": -0.011096289897494078, - "angularVelocity": -0.3944068091224933, - "velocityX": -2.144453686943697, - "velocityY": 1.2934634639624794, - "timestamp": 3.9447909333157085 - }, - { - "x": 4.53850801944686, - "y": 4.62398511499902, - "heading": -0.03651765926491392, - "angularVelocity": -0.33571695749608116, - "velocityX": -1.8163044712809187, - "velocityY": 1.3287397863013088, - "timestamp": 4.020513558580227 - }, - { - "x": 4.425480168441604, - "y": 4.719703555950957, - "heading": -0.05754144603227514, - "angularVelocity": -0.2776420745308276, - "velocityX": -1.4926562650254422, - "velocityY": 1.2640665932747182, - "timestamp": 4.096236183844745 - }, - { - "x": 4.335238129667097, - "y": 4.805147915212772, - "heading": -0.07443003472477187, - "angularVelocity": -0.22303226589808134, - "velocityX": -1.1917447190884998, - "velocityY": 1.128386119252163, - "timestamp": 4.171958809109263 - }, - { - "x": 4.265861114202726, - "y": 4.876824239203163, - "heading": -0.08748354859792261, - "angularVelocity": -0.17238591276453477, - "velocityX": -0.9161992894728712, - "velocityY": 0.9465641707482745, - "timestamp": 4.247681434373781 - }, - { - "x": 4.215660401957236, - "y": 4.932459766827933, - "heading": -0.09697232555255134, - "angularVelocity": -0.1253096669784238, - "velocityX": -0.6629552537319688, - "velocityY": 0.7347279288115158, - "timestamp": 4.3234040596383 - }, - { - "x": 4.183247912596747, - "y": 4.970524954667524, - "heading": -0.10312305256683521, - "angularVelocity": -0.08122707041386726, - "velocityX": -0.4280423353953174, - "velocityY": 0.5026923948637503, - "timestamp": 4.399126684902818 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.03960437745903177, - "velocityX": -0.2079807341935533, - "velocityY": 0.25649983915041824, - "timestamp": 4.474849310167336 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -1.1491916765427864e-28, - "velocityX": -1.6722970053411976e-26, - "velocityY": -1.611730320150929e-26, - "timestamp": 4.550571935431854 + "heading": -1.0560376042139496e-24, + "angularVelocity": 4.029064498114124e-10, + "velocityX": -3.6605154924761147, + "velocityY": 0.6901152005178285, + "timestamp": 2.6749224759712753 + }, + { + "x": 5.429536630692184, + "y": 4.268976584999924, + "heading": -2.8551532586117325e-9, + "angularVelocity": -4.3018194916474803e-8, + "velocityX": -3.5620332259990137, + "velocityY": 1.0897484864542637, + "timestamp": 2.741293304969455 + }, + { + "x": 5.202544196069625, + "y": 4.366944958856273, + "heading": -5.536673846717203e-9, + "angularVelocity": -4.040209574870605e-8, + "velocityX": -3.4200632725076114, + "velocityY": 1.4760757901492323, + "timestamp": 2.8076641339676343 + }, + { + "x": 4.987571660053834, + "y": 4.485916904335734, + "heading": -0.004584961527914818, + "angularVelocity": -0.0690808908137442, + "velocityX": -3.2389611409205945, + "velocityY": 1.7925336669024117, + "timestamp": 2.8740349629658137 + }, + { + "x": 4.796407595296518, + "y": 4.591709425707398, + "heading": -0.01882928955824306, + "angularVelocity": -0.21461729867377147, + "velocityX": -2.8802422335655495, + "velocityY": 1.5939611267258074, + "timestamp": 2.940405791963993 + }, + { + "x": 4.629141679517556, + "y": 4.68427797767877, + "heading": -0.035106225739037614, + "angularVelocity": -0.24524232146084635, + "velocityX": -2.5201721645445794, + "velocityY": 1.394717428855822, + "timestamp": 3.0067766209621727 + }, + { + "x": 4.485777671005229, + "y": 4.763620051213405, + "heading": -0.05103365270171862, + "angularVelocity": -0.23997631494278487, + "velocityX": -2.160045469919592, + "velocityY": 1.195435927684602, + "timestamp": 3.073147449960352 + }, + { + "x": 4.366313170637034, + "y": 4.829736321202412, + "heading": -0.06545153313706903, + "angularVelocity": -0.21723218849271467, + "velocityX": -1.799954922537895, + "velocityY": 0.9961645950033218, + "timestamp": 3.1395182789585316 + }, + { + "x": 4.270745458949128, + "y": 4.88262778520947, + "heading": -0.0776743935119962, + "angularVelocity": -0.1841601281680891, + "velocityX": -1.4399053489376459, + "velocityY": 0.7969082924745271, + "timestamp": 3.205889107956711 + }, + { + "x": 4.199072211550319, + "y": 4.922295352851354, + "heading": -0.08724968344217902, + "angularVelocity": -0.1442695544822162, + "velocityX": -1.0798907966145075, + "velocityY": 0.5976656950144733, + "timestamp": 3.2722599369548906 + }, + { + "x": 4.151291522892566, + "y": 4.948739793023477, + "heading": -0.0938560173878007, + "angularVelocity": -0.0995367098066956, + "velocityX": -0.7199049549172076, + "velocityY": 0.39843468239409807, + "timestamp": 3.33863076595307 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.051179983074939046, + "velocityX": -0.3599426809532897, + "velocityY": 0.19921331994672406, + "timestamp": 3.4050015949512495 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.064087865436599e-25, + "velocityX": -5.747994530578863e-24, + "velocityY": -1.0853274710472143e-23, + "timestamp": 3.471372423949429 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePCBAFE.traj b/src/main/deploy/choreo/CenterLanePCBAFE.traj index 3ba776c9..57961ff6 100644 --- a/src/main/deploy/choreo/CenterLanePCBAFE.traj +++ b/src/main/deploy/choreo/CenterLanePCBAFE.traj @@ -3,1946 +3,1694 @@ { "x": 1.2899744510650637, "y": 5.590954303741455, - "heading": -1.0042344276654815e-24, - "angularVelocity": -6.621110938678157e-25, - "velocityX": -2.249108899033261e-24, - "velocityY": -2.0825834551113612e-24, + "heading": -5.237204901969609e-25, + "angularVelocity": 6.164254262445062e-25, + "velocityX": 1.3402750827177572e-25, + "velocityY": -9.971922068875304e-25, "timestamp": 0 }, { - "x": 1.3060567679582313, - "y": 5.572381918180444, - "heading": -0.010695335923114668, - "angularVelocity": -0.14212952691547795, - "velocityX": 0.2137167180313357, - "velocityY": -0.24680705612748235, - "timestamp": 0.07525062634926666 - }, - { - "x": 1.3382215476730719, - "y": 5.535237409026599, - "heading": -0.03207750843931141, - "angularVelocity": -0.2841461068636689, - "velocityX": 0.4274353752957696, - "velocityY": -0.49361063097924024, - "timestamp": 0.15050125269853332 - }, - { - "x": 1.3864692368556266, - "y": 5.479521033340712, - "heading": -0.06412446500663382, - "angularVelocity": -0.4258696322151573, - "velocityX": 0.6411599680063655, - "velocityY": -0.7404107897692007, - "timestamp": 0.22575187904779997 - }, - { - "x": 1.4508006060735943, - "y": 5.40523312636478, - "heading": -0.10680311296182225, - "angularVelocity": -0.5671533916156479, - "velocityX": 0.8548948007340368, - "velocityY": -0.987206493553073, - "timestamp": 0.30100250539706663 - }, - { - "x": 1.5312167667507302, - "y": 5.312374207143024, - "heading": -0.16007302531493992, - "angularVelocity": -0.7078999197411572, - "velocityX": 1.0686444030896685, - "velocityY": -1.2339952997967512, - "timestamp": 0.3762531317463333 - }, - { - "x": 1.6277191879065078, - "y": 5.200945106889031, - "heading": -0.22389124150432238, - "angularVelocity": -0.8480755481446488, - "velocityX": 1.2824135271362858, - "velocityY": -1.480773060104614, - "timestamp": 0.45150375809559995 - }, - { - "x": 1.7403097782501336, - "y": 5.070947160382142, - "heading": -0.2982176402551873, - "angularVelocity": -0.9877180078992028, - "velocityX": 1.496208015878168, - "velocityY": -1.7275330826287059, - "timestamp": 0.5267543844448666 - }, - { - "x": 1.8689915501245091, - "y": 4.922382870770249, - "heading": -0.3830195665066897, - "angularVelocity": -1.126926516968836, - "velocityX": 1.7100425354217623, - "velocityY": -1.9742598410058476, - "timestamp": 0.6020050107941333 - }, - { - "x": 2.021815501878503, - "y": 4.778590249332989, - "heading": -0.46245356659644804, - "angularVelocity": -1.0555925437892708, - "velocityX": 2.0308661757136712, - "velocityY": -1.9108494960541986, - "timestamp": 0.6772556371434 - }, - { - "x": 2.158552321551595, - "y": 4.6533669501943, - "heading": -0.5314334875140496, - "angularVelocity": -0.916669059968213, - "velocityX": 1.8170854690091889, - "velocityY": -1.6640831473944147, - "timestamp": 0.7525062634926667 - }, - { - "x": 2.279199772550662, - "y": 4.546710847450061, - "heading": -0.589963647479443, - "angularVelocity": -0.7778029606522173, - "velocityX": 1.6032750403843345, - "velocityY": -1.417345049717031, - "timestamp": 0.8277568898419334 - }, - { - "x": 2.383756839213222, - "y": 4.45862085446933, - "heading": -0.6380444617813997, - "angularVelocity": -0.6389423800779483, - "velocityX": 1.38945111469599, - "velocityY": -1.1706213921977682, - "timestamp": 0.9030075161912001 - }, - { - "x": 2.4722229084949126, - "y": 4.389096261559967, - "heading": -0.67567478001119, - "angularVelocity": -0.5000665118072721, - "velocityX": 1.1756190423065196, - "velocityY": -0.9239071657247342, - "timestamp": 0.9782581425404668 - }, - { - "x": 2.544597589718017, - "y": 4.338136588288561, - "heading": -0.7028528784398662, - "angularVelocity": -0.36116773703028543, - "velocityX": 0.9617817782298118, - "velocityY": -0.6771993236957614, - "timestamp": 1.0535087688897335 - }, - { - "x": 2.6008806504487216, - "y": 4.305741524929451, - "heading": -0.7195768777220212, - "angularVelocity": -0.22224398777137738, - "velocityX": 0.7479414253573617, - "velocityY": -0.4304955975881504, - "timestamp": 1.1287593952390003 - }, - { - "x": 2.6410719892543444, - "y": 4.2919109040687715, - "heading": -0.7258449001014369, - "angularVelocity": -0.08329528514916955, - "velocityX": 0.5340997245535138, - "velocityY": -0.1837940962309967, - "timestamp": 1.204010021588267 + "x": 1.3104780876502728, + "y": 5.567275466796967, + "heading": -0.013632783425449295, + "angularVelocity": -0.19128878469698626, + "velocityX": 0.2876973543738418, + "velocityY": -0.3322502676668159, + "timestamp": 0.07126807484842616 + }, + { + "x": 1.3514855787219004, + "y": 5.519918104138971, + "heading": -0.04088178026950747, + "angularVelocity": -0.38234506687618114, + "velocityX": 0.5753977662346411, + "velocityY": -0.6644961682873589, + "timestamp": 0.14253614969685233 + }, + { + "x": 1.4129976098576382, + "y": 5.448882530124809, + "heading": -0.08170955132000574, + "angularVelocity": -0.5728760196950918, + "velocityX": 0.8631077977981357, + "velocityY": -0.996737657994004, + "timestamp": 0.21380422454527848 + }, + { + "x": 1.4950153477239438, + "y": 5.354169209270334, + "heading": -0.1360632338085868, + "angularVelocity": -0.7626652270905468, + "velocityX": 1.1508341994748963, + "velocityY": -1.3289726298333888, + "timestamp": 0.28507229939370465 + }, + { + "x": 1.5975404069407602, + "y": 5.235778918310868, + "heading": -0.20388243920444382, + "angularVelocity": -0.951607091114721, + "velocityX": 1.4385832567368764, + "velocityY": -1.6611967028891954, + "timestamp": 0.35634037424213083 + }, + { + "x": 1.7205747481569098, + "y": 5.093712881376462, + "heading": -0.2851089622983034, + "angularVelocity": -1.1397322471052174, + "velocityX": 1.7263598248980438, + "velocityY": -1.9934035995297235, + "timestamp": 0.427608449090557 + }, + { + "x": 1.8641205194488986, + "y": 4.927972835971714, + "heading": -0.3796964281011733, + "angularVelocity": -1.3272066911311928, + "velocityX": 2.0141665338552186, + "velocityY": -2.325586116325524, + "timestamp": 0.4988765239389832 + }, + { + "x": 2.0360561465973834, + "y": 4.766214064350323, + "heading": -0.4690836216896802, + "angularVelocity": -1.2542389250532822, + "velocityX": 2.412519596104705, + "velocityY": -2.26972276107388, + "timestamp": 0.5701445987874093 + }, + { + "x": 2.187477556502562, + "y": 4.628126287278847, + "heading": -0.5451441122583921, + "angularVelocity": -1.0672449161911324, + "velocityX": 2.1246737789286905, + "velocityY": -1.9375825341874753, + "timestamp": 0.6414126736358354 + }, + { + "x": 2.3183837856397878, + "y": 4.513708351773291, + "heading": -0.6078811753474637, + "angularVelocity": -0.8802968681629402, + "velocityX": 1.8368144420294628, + "velocityY": -1.6054584854284524, + "timestamp": 0.7126807484842616 + }, + { + "x": 2.428774184877553, + "y": 4.4229594060006505, + "heading": -0.6572940712821052, + "angularVelocity": -0.6933384413670975, + "velocityX": 1.5489459968231887, + "velocityY": -1.2733463891882395, + "timestamp": 0.7839488233326877 + }, + { + "x": 2.5186483273099793, + "y": 4.355878850846579, + "heading": -0.6933805665691075, + "angularVelocity": -0.5063486752483727, + "velocityX": 1.261071561475065, + "velocityY": -0.94124269943785, + "timestamp": 0.8552168981811138 + }, + { + "x": 2.588005969972471, + "y": 4.312466305335481, + "heading": -0.7161380105196834, + "angularVelocity": -0.31932171591524877, + "velocityX": 0.9731937169623598, + "velocityY": -0.6091443553572609, + "timestamp": 0.9264849730295399 + }, + { + "x": 2.6368470386589604, + "y": 4.292721584024785, + "heading": -0.7255637248082996, + "angularVelocity": -0.1322571755819541, + "velocityX": 0.6853148312251345, + "velocityY": -0.2770486133193386, + "timestamp": 0.997753047877966 }, { "x": 2.6651716232299805, "y": 4.296644687652588, "heading": -0.7216551150961179, - "angularVelocity": 0.055677742612701135, - "velocityX": 0.3202582509251215, - "velocityY": 0.06290689943025622, - "timestamp": 1.2792606479375337 - }, - { - "x": 2.6731116736052027, - "y": 4.320290744259811, - "heading": -0.7067971794351253, - "angularVelocity": 0.19582512759313456, - "velocityX": 0.10464854696510043, - "velocityY": 0.31165110401848706, - "timestamp": 1.355134136920331 - }, - { - "x": 2.664692786668951, - "y": 4.362810218152233, - "heading": -0.6813099274422239, - "angularVelocity": 0.3359177538109565, - "velocityX": -0.1109595334170119, - "velocityY": 0.5603996133888316, - "timestamp": 1.4310076259031281 - }, - { - "x": 2.639915179239627, - "y": 4.424203570363852, - "heading": -0.6451974716199608, - "angularVelocity": 0.47595617792732364, - "velocityX": -0.3265647561685457, - "velocityY": 0.8091541991108215, - "timestamp": 1.5068811148859254 - }, - { - "x": 2.59877917099983, - "y": 4.50447140700047, - "heading": -0.59846321314625, - "angularVelocity": 0.6159497750829267, - "velocityX": -0.5421657655564515, - "velocityY": 1.0579167732067494, - "timestamp": 1.5827546038687226 - }, - { - "x": 2.5412852001349884, - "y": 4.6036144764177696, - "heading": -0.541108297539196, - "angularVelocity": 0.755928274499912, - "velocityX": -0.7577609997330812, - "velocityY": 1.3066892105064276, - "timestamp": 1.6586280928515198 - }, - { - "x": 2.4674338294971827, - "y": 4.7216336217313835, - "heading": -0.4731286056147784, - "angularVelocity": 0.8959610640790578, - "velocityX": -0.973348815612657, - "velocityY": 1.5554727599303189, - "timestamp": 1.734501581834317 - }, - { - "x": 2.3772256595733805, - "y": 4.858529596063364, - "heading": -0.3945096344591076, - "angularVelocity": 1.0361849996577348, - "velocityX": -1.1889287171735974, - "velocityY": 1.8042662353779348, - "timestamp": 1.8103750708171142 - }, - { - "x": 2.270660565338228, - "y": 5.014302224131279, - "heading": -0.305219034790845, - "angularVelocity": 1.1768352933988244, - "velocityX": -1.4045102665479658, - "velocityY": 2.0530574006321625, - "timestamp": 1.8862485597999115 - }, - { - "x": 2.156230166958197, - "y": 5.146478435153338, - "heading": -0.2290932542035935, - "angularVelocity": 1.0033251615002383, - "velocityX": -1.5081736705949598, - "velocityY": 1.7420605377990246, - "timestamp": 1.9621220487827087 - }, - { - "x": 2.05814700584277, - "y": 5.259770794344292, - "heading": -0.16373664121599296, - "angularVelocity": 0.8613893187700765, - "velocityX": -1.292719794889953, - "velocityY": 1.4931745028443373, - "timestamp": 2.037995537765506 - }, - { - "x": 1.976411579927544, - "y": 5.3541805242021825, - "heading": -0.10920088428004432, - "angularVelocity": 0.7187722308158727, - "velocityX": -1.077259356476389, - "velocityY": 1.2443045802110966, - "timestamp": 2.113869026748303 - }, - { - "x": 1.9110235357975711, - "y": 5.4297080461749685, - "heading": -0.06553152377248019, - "angularVelocity": 0.5755549282499098, - "velocityX": -0.8618035760132025, - "velocityY": 0.9954402121920332, - "timestamp": 2.1897425157311 - }, - { - "x": 1.8619825316383047, - "y": 5.486353607136737, - "heading": -0.03276339241185007, - "angularVelocity": 0.43187853623101025, - "velocityX": -0.6463523006090516, - "velocityY": 0.7465790979324988, - "timestamp": 2.265616004713897 - }, - { - "x": 1.829288400520471, - "y": 5.524117342639773, - "heading": -0.010917643304867627, - "angularVelocity": 0.2879233497741951, - "velocityX": -0.43090322530503955, - "velocityY": 0.49771977022960984, - "timestamp": 2.341489493696694 + "angularVelocity": 0.054843767289835074, + "velocityX": 0.3974372063685045, + "velocityY": 0.05504713907518853, + "timestamp": 1.0690211227263922 + }, + { + "x": 2.6726523875585464, + "y": 4.325520941215549, + "heading": -0.7036434191881636, + "angularVelocity": 0.24661858694943486, + "velocityX": 0.10242764131932201, + "velocityY": 0.39537758612425, + "timestamp": 1.1420557472887627 + }, + { + "x": 2.658587549090021, + "y": 4.379253724773331, + "heading": -0.6716319783828552, + "angularVelocity": 0.4383049957074976, + "velocityX": -0.19257767877637322, + "velocityY": 0.7357165711435307, + "timestamp": 1.2150903718511332 + }, + { + "x": 2.6229776230328583, + "y": 4.457843935727211, + "heading": -0.625626931450262, + "angularVelocity": 0.629907351591922, + "velocityX": -0.4875759445679413, + "velocityY": 1.076067843502979, + "timestamp": 1.2881249964135038 + }, + { + "x": 2.565823384293886, + "y": 4.561292786214385, + "heading": -0.5656322001624833, + "angularVelocity": 0.8214560100400625, + "velocityX": -0.7825636002299563, + "velocityY": 1.4164357126095721, + "timestamp": 1.3611596209758743 + }, + { + "x": 2.4871259686686438, + "y": 4.6896018233859875, + "heading": -0.4916449942707574, + "angularVelocity": 1.01304287295325, + "velocityX": -1.0775357044251765, + "velocityY": 1.7568247655196478, + "timestamp": 1.4341942455382448 + }, + { + "x": 2.3868870212616646, + "y": 4.8427728778807975, + "heading": -0.4036470135354823, + "angularVelocity": 1.2048803052328438, + "velocityX": -1.3724852836256678, + "velocityY": 2.097238883784559, + "timestamp": 1.5072288701006153 + }, + { + "x": 2.2651088471232064, + "y": 5.020807849871808, + "heading": -0.30158987603954596, + "angularVelocity": 1.3973801892933821, + "velocityX": -1.6674033017649226, + "velocityY": 2.43767901947618, + "timestamp": 1.5802634946629859 + }, + { + "x": 2.1359147758136054, + "y": 5.170005063650777, + "heading": -0.2156034423971957, + "angularVelocity": 1.1773379292025947, + "velocityX": -1.7689427731537262, + "velocityY": 2.0428285169256424, + "timestamp": 1.6532981192253564 + }, + { + "x": 2.0282555085514997, + "y": 5.294336258780011, + "heading": -0.14382060600259022, + "angularVelocity": 0.9828603463731643, + "velocityX": -1.4740853110043226, + "velocityY": 1.7023596119544246, + "timestamp": 1.726332743787727 + }, + { + "x": 1.9421294204612731, + "y": 5.39380126769391, + "heading": -0.08631841339161836, + "angularVelocity": 0.7873278319089028, + "velocityX": -1.1792500968725612, + "velocityY": 1.3618884126522468, + "timestamp": 1.7993673683500975 + }, + { + "x": 1.8775353609204413, + "y": 5.468400102933885, + "heading": -0.04315875828225115, + "angularVelocity": 0.5909478602509897, + "velocityX": -0.8844306372201511, + "velocityY": 1.021417385068757, + "timestamp": 1.872401992912468 + }, + { + "x": 1.8344726769226216, + "y": 5.518132795390121, + "heading": -0.014381433416099416, + "angularVelocity": 0.3940230409697844, + "velocityX": -0.5896201186198287, + "velocityY": 0.6809467804378745, + "timestamp": 1.9454366174748385 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 5.072711091682045e-24, - "angularVelocity": 0.14389272789792218, - "velocityX": -0.21545347602968715, - "velocityY": 0.24886063882779058, - "timestamp": 2.417362982679491 + "heading": 1.6699672496632016e-24, + "angularVelocity": 0.19691253980251358, + "velocityX": -0.29481199459862734, + "velocityY": 0.3404751148788101, + "timestamp": 2.018471242037209 }, { "x": 1.8129411935806272, "y": 5.542999267578125, - "heading": 2.471925946464062e-24, - "angularVelocity": -1.3881126396060902e-24, - "velocityX": -6.902910153275516e-22, - "velocityY": 3.1112652084354562e-24, - "timestamp": 2.493236471662288 - }, - { - "x": 1.8507557540076562, - "y": 5.5430527114888575, - "heading": -8.800046052500135e-19, - "angularVelocity": -9.452376611096758e-18, - "velocityX": 0.4061756726920047, - "velocityY": 0.0005740544422031257, - "timestamp": 2.586335499913798 - }, - { - "x": 1.9263848743459357, - "y": 5.543159599309593, - "heading": -2.769921496163024e-18, - "angularVelocity": -2.0300071079064193e-17, - "velocityX": 0.8123513398439022, - "velocityY": 0.0011481088765763312, - "timestamp": 2.679434528165308 - }, - { - "x": 2.0398285536670646, - "y": 5.54331993103902, - "heading": -5.903589755199969e-18, - "angularVelocity": -3.365951630081127e-17, - "velocityX": 1.2185269970236072, - "velocityY": 0.0017221632968556807, - "timestamp": 2.772533556416818 - }, - { - "x": 2.191086789804772, - "y": 5.5435337066740775, - "heading": -1.0884334063348694e-17, - "angularVelocity": -5.34994231591497e-17, - "velocityX": 1.6247026309348636, - "velocityY": 0.0022962176842493674, - "timestamp": 2.865632584668328 - }, - { - "x": 2.380159571927711, - "y": 5.543800926199458, - "heading": -4.173692512602447e-18, - "angularVelocity": 7.208068308331619e-17, - "velocityX": 2.0308781485038967, - "velocityY": 0.0028702719072147666, - "timestamp": 2.958731612919838 - }, - { - "x": 2.5314178080654184, - "y": 5.544014701834515, - "heading": -1.8153111851912927e-18, - "angularVelocity": 2.5331965023903977e-17, - "velocityX": 1.6247026309348636, - "velocityY": 0.002296217684249368, - "timestamp": 3.0518306411713483 - }, - { - "x": 2.644861487386547, - "y": 5.544175033563942, - "heading": -7.161538717554587e-19, - "angularVelocity": 1.1806324234469696e-17, - "velocityX": 1.2185269970236072, - "velocityY": 0.001722163296855681, - "timestamp": 3.1449296694228583 - }, - { - "x": 2.7204906077248263, - "y": 5.544281921384678, - "heading": -1.8348593704837507e-19, - "angularVelocity": 5.72151981311354e-18, - "velocityX": 0.8123513398439022, - "velocityY": 0.0011481088765763316, - "timestamp": 3.2380286976743684 + "heading": 7.316181849831016e-25, + "angularVelocity": -2.1571909437539165e-24, + "velocityX": -1.0558547549960102e-21, + "velocityY": -3.684849193710572e-24, + "timestamp": 2.0915058665995794 + }, + { + "x": 1.8720264440667018, + "y": 5.543082773688388, + "heading": -5.729619384838823e-18, + "angularVelocity": -5.86988662374941e-17, + "velocityX": 0.6053171317615014, + "velocityY": 0.0008555041864670528, + "timestamp": 2.1891162717670882 + }, + { + "x": 1.9901969439319853, + "y": 5.54324978590735, + "heading": 1.1892133440280875e-18, + "angularVelocity": 7.088212283450037e-17, + "velocityX": 1.2106342521833764, + "velocityY": 0.001711008356907634, + "timestamp": 2.286726676934597 + }, + { + "x": 2.1674526905937914, + "y": 5.543500304231363, + "heading": 2.3439205825147678e-18, + "angularVelocity": 1.1829755615161062e-17, + "velocityX": 1.8159513461461236, + "velocityY": 0.0025665124899531154, + "timestamp": 2.384337082102106 + }, + { + "x": 2.403793671138691, + "y": 5.5438343286421725, + "heading": -2.263663737810736e-18, + "angularVelocity": -4.7203823324743326e-17, + "velocityX": 2.4212683078132478, + "velocityY": 0.00342201643602312, + "timestamp": 2.481947487269615 + }, + { + "x": 2.5810494178004975, + "y": 5.544084846966185, + "heading": -1.1285361572651988e-18, + "angularVelocity": 1.1629165750714285e-17, + "velocityX": 1.8159513461461234, + "velocityY": 0.002566512489953115, + "timestamp": 2.5795578924371236 + }, + { + "x": 2.699219917665781, + "y": 5.544251859185147, + "heading": 5.747781298910251e-18, + "angularVelocity": 7.04465619676184e-17, + "velocityX": 1.2106342521833764, + "velocityY": 0.001711008356907634, + "timestamp": 2.6771682976046325 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -9.055003253623357e-30, - "angularVelocity": 1.970868445148275e-18, - "velocityX": 0.4061756726920047, - "velocityY": 0.0005740544422031258, - "timestamp": 3.3311277259258785 + "heading": 7.720128293472343e-28, + "angularVelocity": -5.888492408296726e-17, + "velocityX": 0.6053171317615014, + "velocityY": 0.000855504186467053, + "timestamp": 2.7747787027721413 }, { "x": 2.7583051681518556, "y": 5.54433536529541, - "heading": -2.6159098195352036e-29, - "angularVelocity": -1.6705479392630998e-29, - "velocityX": 6.780641024803866e-22, - "velocityY": 9.718698194414986e-25, - "timestamp": 3.4242267541773885 - }, - { - "x": 2.746560227605712, - "y": 5.55825095759549, - "heading": 0.00742109779657271, - "angularVelocity": 0.11458935665570884, - "velocityX": -0.18135392067783562, - "velocityY": 0.21487100869168943, - "timestamp": 3.4889892941999308 - }, - { - "x": 2.7232728362575633, - "y": 5.58624983061418, - "heading": 0.022298554503038515, - "angularVelocity": 0.22972318104396974, - "velocityX": -0.35958119215279744, - "velocityY": 0.43233129844728024, - "timestamp": 3.553751834222473 - }, - { - "x": 2.6887224275841572, - "y": 5.628555352569631, - "heading": 0.04468536543200197, - "angularVelocity": 0.3456753073794072, - "velocityX": -0.5334937243255126, - "velocityY": 0.6532406224451052, - "timestamp": 3.618514374245015 - }, - { - "x": 2.6433185485147903, - "y": 5.685479106691384, - "heading": 0.07465726375014338, - "angularVelocity": 0.46279683143540923, - "velocityX": -0.7010824321214573, - "velocityY": 0.8789611108819849, - "timestamp": 3.6832769142675574 - }, - { - "x": 2.587716352440023, - "y": 5.7574833976129485, - "heading": 0.11231839668543797, - "angularVelocity": 0.5815264954429781, - "velocityX": -0.8585549000303846, - "velocityY": 1.1118200567257257, - "timestamp": 3.7480394542900997 - }, - { - "x": 2.523118270580454, - "y": 5.845312686573325, - "heading": 0.15779630703388142, - "angularVelocity": 0.7022255509529696, - "velocityX": -0.9974605973929359, - "velocityY": 1.356174247177545, - "timestamp": 3.812801994312642 - }, - { - "x": 2.4523262633845233, - "y": 5.950260122565857, - "heading": 0.21110300167691656, - "angularVelocity": 0.8231100050195744, - "velocityX": -1.093101153402716, - "velocityY": 1.6204959835732518, - "timestamp": 3.877564534335184 - }, - { - "x": 2.385035672029833, - "y": 6.073114038949963, - "heading": 0.27020607963640353, - "angularVelocity": 0.9126121047586251, - "velocityX": -1.0390357038384854, - "velocityY": 1.896990395085535, - "timestamp": 3.9423270743577263 - }, - { - "x": 2.335844965932351, - "y": 6.198572107213467, - "heading": 0.32662517660992413, - "angularVelocity": 0.8711686872362097, - "velocityX": -0.7595549229594762, - "velocityY": 1.9372011693771587, - "timestamp": 4.007089614380269 - }, - { - "x": 2.303814652096041, - "y": 6.317831411051833, - "heading": 0.37736366027868895, - "angularVelocity": 0.7834541951428096, - "velocityX": -0.4945808769260876, - "velocityY": 1.8414858928765832, - "timestamp": 4.071852154402811 - }, - { - "x": 2.2877361979926687, - "y": 6.4282644247646346, - "heading": 0.42161092558434743, - "angularVelocity": 0.6832231300726793, - "velocityX": -0.24826781188285701, - "velocityY": 1.7051989263293559, - "timestamp": 4.1366146944253535 - }, - { - "x": 2.2869063645502266, - "y": 6.528717533266064, - "heading": 0.45901061919668296, - "angularVelocity": 0.5774896043193742, - "velocityX": -0.012813478936328582, - "velocityY": 1.5510989603938259, - "timestamp": 4.201377234447896 - }, - { - "x": 2.3008846173823154, - "y": 6.618554541912377, - "heading": 0.48936477750092344, - "angularVelocity": 0.46869931743991045, - "velocityX": 0.21583855153339288, - "velocityY": 1.3871754970549879, - "timestamp": 4.266139774470438 - }, - { - "x": 2.3293724060058594, - "y": 6.6973748207092285, + "heading": 6.467452452708822e-28, + "angularVelocity": 1.6941291064933935e-28, + "velocityX": 1.0543917599744842e-21, + "velocityY": 1.5169407123402652e-24, + "timestamp": 2.87238910793965 + }, + { + "x": 2.7303741241412878, + "y": 5.568056862351408, + "heading": 0.006778500636213837, + "angularVelocity": 0.08813486156991056, + "velocityX": -0.36316271539799655, + "velocityY": 0.3084296913821901, + "timestamp": 2.949299657471029 + }, + { + "x": 2.6747561237908846, + "y": 5.615779928168, + "heading": 0.02059737759786194, + "angularVelocity": 0.1796746616146605, + "velocityX": -0.7231517742271669, + "velocityY": 0.6205009079686918, + "timestamp": 3.026210207002408 + }, + { + "x": 2.591861580665438, + "y": 5.687962474226551, + "heading": 0.041894032741262306, + "angularVelocity": 0.27690161197862034, + "velocityX": -1.0778045876739721, + "velocityY": 0.938525943428621, + "timestamp": 3.103120756533787 + }, + { + "x": 2.482522842737762, + "y": 5.785486601053792, + "heading": 0.07152934014357436, + "angularVelocity": 0.38532174822416326, + "velocityX": -1.4216351149989686, + "velocityY": 1.2680201535610198, + "timestamp": 3.1800313060651657 + }, + { + "x": 2.3492929926954265, + "y": 5.910724214608568, + "heading": 0.11194451008519192, + "angularVelocity": 0.525482787314222, + "velocityX": -1.7322701612992522, + "velocityY": 1.6283541636076846, + "timestamp": 3.2569418555965446 + }, + { + "x": 2.232288592300856, + "y": 6.068262989450489, + "heading": 0.17809331854346394, + "angularVelocity": 0.8600745783422566, + "velocityX": -1.5213049589098766, + "velocityY": 2.0483376572110616, + "timestamp": 3.3338524051279235 + }, + { + "x": 2.1483046945309274, + "y": 6.20982532534887, + "heading": 0.24392330820994265, + "angularVelocity": 0.8559292589584283, + "velocityX": -1.091968504732421, + "velocityY": 1.8406101212503223, + "timestamp": 3.4107629546593023 + }, + { + "x": 2.095435520919896, + "y": 6.331973650947482, + "heading": 0.3066258894331499, + "angularVelocity": 0.8152663269897057, + "velocityX": -0.6874112060460749, + "velocityY": 1.588186878690508, + "timestamp": 3.4876735041906812 + }, + { + "x": 2.072969838348887, + "y": 6.433621733055283, + "heading": 0.3652444017006222, + "angularVelocity": 0.7621647826551591, + "velocityX": -0.29210144392276055, + "velocityY": 1.3216403045765417, + "timestamp": 3.56458405372206 + }, + { + "x": 2.080539684915293, + "y": 6.514239840456833, + "heading": 0.4192892074806503, + "angularVelocity": 0.7026969136136278, + "velocityX": 0.0984240343168718, + "velocityY": 1.0482061029697753, + "timestamp": 3.641494603253439 + }, + { + "x": 2.1179206171311256, + "y": 6.5735146979492365, + "heading": 0.46845941042093314, + "angularVelocity": 0.6393167548519689, + "velocityX": 0.48603127195940116, + "velocityY": 0.7706986603732366, + "timestamp": 3.718405152784818 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, "heading": 0.5125504196, - "angularVelocity": 0.3580100794534351, - "velocityX": 0.43988065652810937, - "velocityY": 1.21706589595492, - "timestamp": 4.33090231449298 + "angularVelocity": 0.5732764808952251, + "velocityX": 0.8716741807939861, + "velocityY": 0.49050144055915196, + "timestamp": 3.7953157023161967 }, { - "x": 2.3907092837867907, - "y": 6.780867422722411, - "heading": 0.5304481214064672, - "angularVelocity": 0.21222434211791516, - "velocityX": 0.7273100577595625, - "velocityY": 0.990024458199304, - "timestamp": 4.415236192632761 + "x": 2.2917885277794023, + "y": 6.672276624724165, + "heading": 0.5461832199677006, + "angularVelocity": 0.41150844936023123, + "velocityX": 1.30706335645214, + "velocityY": 0.7468102485181378, + "timestamp": 3.8770462243018065 }, { - "x": 2.471644870352802, - "y": 6.840510057948011, - "heading": 0.53507672796877, - "angularVelocity": 0.05488430823293737, - "velocityX": 0.9597043128013546, - "velocityY": 0.7072203548702543, - "timestamp": 4.499570070772541 + "x": 2.434116611335813, + "y": 6.754285363860807, + "heading": 0.5638407865548148, + "angularVelocity": 0.2160461741602869, + "velocityX": 1.74143123154738, + "velocityY": 1.0034040789691918, + "timestamp": 3.958776746287416 }, { - "x": 2.556252265889606, - "y": 6.8694746689081185, - "heading": 0.5277386544868528, - "angularVelocity": -0.08701216692246314, - "velocityX": 1.0032432683407548, - "velocityY": 0.3434516661512893, - "timestamp": 4.5839039489123214 + "x": 2.5412609256604743, + "y": 6.8169454314474, + "heading": 0.5395666812512137, + "angularVelocity": -0.2970017162973107, + "velocityX": 1.3109461645616995, + "velocityY": 0.7666666756101815, + "timestamp": 4.040507268273026 }, { - "x": 2.617383928971812, - "y": 6.87815517989328, - "heading": 0.5183265349753615, - "angularVelocity": -0.1116054392268187, - "velocityX": 0.7248766976052303, - "velocityY": 0.1029302953526471, - "timestamp": 4.668237827052102 + "x": 2.6126555907805695, + "y": 6.858741825974231, + "heading": 0.5217952234534814, + "angularVelocity": -0.21743967083510435, + "velocityX": 0.8735373687282384, + "velocityY": 0.5113927271159627, + "timestamp": 4.122237790258636 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -0.06849104420156939, - "velocityX": 0.36711922260971214, - "velocityY": 0.01766420444115541, - "timestamp": 4.752571705191882 + "angularVelocity": -0.11311323638810435, + "velocityX": 0.4366658269950187, + "velocityY": 0.25575567457534326, + "timestamp": 4.203968312244245 }, { "x": 2.6483445167541504, "y": 6.879644870758057, "heading": 0.5125504196, - "angularVelocity": -1.2403630086849167e-25, - "velocityX": 1.2566075146445203e-23, - "velocityY": 7.418296886220231e-24, - "timestamp": 4.836905583331663 - }, - { - "x": 2.65710997558339, - "y": 6.867790932222418, - "heading": 0.5069947283791145, - "angularVelocity": -0.09538266194620745, - "velocityX": 0.1504894284206614, - "velocityY": -0.20351386841397834, - "timestamp": 4.895151926089623 - }, - { - "x": 2.6746461929799414, - "y": 6.844083289351916, - "heading": 0.4960269163567629, - "angularVelocity": -0.18830044090369472, - "velocityX": 0.3010698451819098, - "velocityY": -0.40702371596133824, - "timestamp": 4.953398268847583 - }, - { - "x": 2.7009592416637416, - "y": 6.808522263895978, - "heading": 0.4798144698796375, - "angularVelocity": -0.27834273723408604, - "velocityX": 0.4517545211918758, - "velocityY": -0.6105280395665419, - "timestamp": 5.0116446116055435 - }, - { - "x": 2.736056173569292, - "y": 6.76110833670321, - "heading": 0.4585545185998355, - "angularVelocity": -0.3650006210372158, - "velocityX": 0.6025602680565616, - "velocityY": -0.8140241077417322, - "timestamp": 5.069890954363504 - }, - { - "x": 2.7799452908871816, - "y": 6.701842252248569, - "heading": 0.4324815844111489, - "angularVelocity": -0.4476321251109512, - "velocityX": 0.7535085507474468, - "velocityY": -1.017507394428501, - "timestamp": 5.128137297121464 - }, - { - "x": 2.8326365093294883, - "y": 6.63072517677858, - "heading": 0.40187833162186215, - "angularVelocity": -0.5254107183425597, - "velocityX": 0.9046270709435361, - "velocityY": -1.2209706584585491, - "timestamp": 5.186383639879424 - }, - { - "x": 2.8941418577048204, - "y": 6.54775894755859, - "heading": 0.367091043865372, - "angularVelocity": -0.5972441548999393, - "velocityX": 1.0559521072578695, - "velocityY": -1.4244023794721719, - "timestamp": 5.244629982637385 - }, - { - "x": 2.9644761872265666, - "y": 6.452946486925878, - "heading": 0.3285529983813312, - "angularVelocity": -0.661638888542476, - "velocityX": 1.2075321160337456, - "velocityY": -1.6277839284553843, - "timestamp": 5.302876325395345 - }, - { - "x": 3.043658216776231, - "y": 6.346292539288771, - "heading": 0.2868218829421827, - "angularVelocity": -0.7164589820267359, - "velocityX": 1.359433499176103, - "velocityY": -1.8310840232545251, - "timestamp": 5.361122668153305 - }, - { - "x": 3.131712135507504, - "y": 6.227805106020448, - "heading": 0.24264403908378393, - "angularVelocity": -0.7584655407804345, - "velocityX": 1.5117501728336273, - "velocityY": -2.03424674679905, - "timestamp": 5.419369010911265 - }, - { - "x": 3.2286701423571795, - "y": 6.097498584088062, - "heading": 0.19707481942882135, - "angularVelocity": -0.7823533203504854, - "velocityX": 1.6646196526463415, - "velocityY": -2.2371622965903253, - "timestamp": 5.477615353669226 - }, - { - "x": 3.33457642569322, - "y": 5.9554017916384385, - "heading": 0.1517318182422537, - "angularVelocity": -0.7784694976470596, - "velocityX": 1.8182477786824947, - "velocityY": -2.439583083183451, - "timestamp": 5.535861696427186 - }, - { - "x": 3.449491590462662, - "y": 5.801583658823103, - "heading": 0.10942509922281701, - "angularVelocity": -0.726341209013589, - "velocityX": 1.9729163983216391, - "velocityY": -2.6408204452341257, - "timestamp": 5.594108039185146 - }, - { - "x": 3.5734690777406897, - "y": 5.636270409713289, - "heading": 0.07620523271942112, - "angularVelocity": -0.5703339459687529, - "velocityX": 2.128502518917107, - "velocityY": -2.838173888389265, - "timestamp": 5.652354381943106 - }, - { - "x": 3.704953237459551, - "y": 5.460551173556115, - "heading": 0.07376572964977675, - "angularVelocity": -0.04188251062872067, - "velocityX": 2.2573805237049305, - "velocityY": -3.016828659738646, - "timestamp": 5.710600724701067 - }, - { - "x": 3.8484687741574635, - "y": 5.2934320188975885, - "heading": 0.07376572135485625, - "angularVelocity": -1.4241101012224094e-7, - "velocityX": 2.4639407369194757, - "velocityY": -2.86917850538672, - "timestamp": 5.768847067459027 - }, - { - "x": 4.002883086238839, - "y": 5.136327781933491, - "heading": 0.07376571311141436, - "angularVelocity": -1.4152720162027837e-7, - "velocityX": 2.651055925056733, - "velocityY": -2.697237792541529, - "timestamp": 5.827093410216987 + "angularVelocity": 1.6798438100344207e-27, + "velocityX": 4.181549744276816e-25, + "velocityY": 2.1523905294069243e-25, + "timestamp": 4.285698834229855 + }, + { + "x": 2.661250521617956, + "y": 6.863588942839657, + "heading": 0.5202450670683688, + "angularVelocity": 0.13324461766431578, + "velocityX": 0.22348726055621218, + "velocityY": -0.27803300742854703, + "timestamp": 4.343447114072227 + }, + { + "x": 2.6870669895732706, + "y": 6.83147467268314, + "heading": 0.535409558228526, + "angularVelocity": 0.2625964132879752, + "velocityX": 0.447051722160081, + "velocityY": -0.5561078225043913, + "timestamp": 4.401195393914598 + }, + { + "x": 2.725799371823277, + "y": 6.783299196063953, + "heading": 0.5577628193465138, + "angularVelocity": 0.38708098629089316, + "velocityX": 0.6707105797043527, + "velocityY": -0.8342322360196058, + "timestamp": 4.45894367375697 + }, + { + "x": 2.7774544304477864, + "y": 6.719058990822734, + "heading": 0.5869404585342344, + "angularVelocity": 0.5052555550981419, + "velocityX": 0.8944865330275743, + "velocityY": -1.112417641123979, + "timestamp": 4.5166919535993415 + }, + { + "x": 2.8420407207473644, + "y": 6.638749608593502, + "heading": 0.6224502537914576, + "angularVelocity": 0.6149065453403929, + "velocityX": 1.118410634496313, + "velocityY": -1.3906800765051686, + "timestamp": 4.574440233441713 + }, + { + "x": 2.9195693565379317, + "y": 6.542365376975582, + "heading": 0.6635926087059805, + "angularVelocity": 0.7124429511463173, + "velocityX": 1.342527188726434, + "velocityY": -1.6690407382004828, + "timestamp": 4.632188513284085 + }, + { + "x": 3.0100552630481547, + "y": 6.429899310890966, + "heading": 0.70930037739609, + "angularVelocity": 0.7915000899571744, + "velocityX": 1.5669021961729532, + "velocityY": -1.947522357230365, + "timestamp": 4.689936793126456 + }, + { + "x": 3.1135190276304736, + "y": 6.301344715989459, + "heading": 0.7577535982781642, + "angularVelocity": 0.8390418037442976, + "velocityX": 1.7916337051896702, + "velocityY": -2.2261198991970987, + "timestamp": 4.747685072968828 + }, + { + "x": 3.2299855403363114, + "y": 6.1567104859066015, + "heading": 0.8051333898440217, + "angularVelocity": 0.8204537294475949, + "velocityX": 2.016796223605989, + "velocityY": -2.5045634342294933, + "timestamp": 4.8054333528112 + }, + { + "x": 3.3593492107693024, + "y": 5.9962947499990635, + "heading": 0.8376127381025854, + "angularVelocity": 0.5624297095466465, + "velocityX": 2.240130282427429, + "velocityY": -2.777844402385742, + "timestamp": 4.863181632653571 + }, + { + "x": 3.4940407564436287, + "y": 5.828570179937074, + "heading": 0.8376127456087376, + "angularVelocity": 1.2998053164779303e-7, + "velocityX": 2.332390610455871, + "velocityY": -2.9044080710248896, + "timestamp": 4.920929912495943 + }, + { + "x": 3.6287322938415287, + "y": 5.660845603228664, + "heading": 0.837612753114844, + "angularVelocity": 1.2997973902929816e-7, + "velocityX": 2.3323904671368516, + "velocityY": -2.9044081861178794, + "timestamp": 4.9786781923383145 + }, + { + "x": 3.763423831239301, + "y": 5.49312102652015, + "heading": 0.8376127606209502, + "angularVelocity": 1.2997973824676034e-7, + "velocityX": 2.3323904671346476, + "velocityY": -2.9044081861196496, + "timestamp": 5.036426472180686 + }, + { + "x": 3.8981153686370735, + "y": 5.325396449811636, + "heading": 0.8376127681270565, + "angularVelocity": 1.2997973875283622e-7, + "velocityX": 2.3323904671346543, + "velocityY": -2.904408186119644, + "timestamp": 5.094174752023058 + }, + { + "x": 4.03280690604441, + "y": 5.157671873110804, + "heading": 0.8376127756331628, + "angularVelocity": 1.2997973760288775e-7, + "velocityX": 2.3323904673002676, + "velocityY": -2.9044081859866484, + "timestamp": 5.151923031865429 }, { "x": 4.16749906539917, "y": 4.98994779586792, - "heading": 0.07376570463306892, - "angularVelocity": -1.4556013372477054e-7, - "velocityX": 2.826202837221629, - "velocityY": -2.51311892102558, - "timestamp": 5.885339752974947 - }, - { - "x": 4.268981030537756, - "y": 4.906736402889882, - "heading": 0.07376569669894775, - "angularVelocity": -2.2864637143597095e-7, - "velocityX": 2.9245183697828536, - "velocityY": -2.397995023127166, - "timestamp": 5.920040155676169 - }, - { - "x": 4.373711969606293, - "y": 4.827653183122543, - "heading": 0.07376568907042552, - "angularVelocity": -2.198395881628289e-7, - "velocityX": 3.0181476558152474, - "velocityY": -2.2790288760699386, - "timestamp": 5.954740558377391 - }, - { - "x": 4.481524054047376, - "y": 4.75282484787289, - "heading": 0.0737656816823934, - "angularVelocity": -2.129091176782688e-7, - "velocityX": 3.106940440125975, - "velocityY": -2.1564111487103386, - "timestamp": 5.989440961078612 - }, - { - "x": 4.592244511621915, - "y": 4.682371277147909, - "heading": 0.07376567447691734, - "angularVelocity": -2.0764819690223906e-7, - "velocityX": 3.1907542551556984, - "velocityY": -2.030338706198957, - "timestamp": 6.024141363779834 - }, - { - "x": 4.705695889724197, - "y": 4.616405299617974, - "heading": 0.07376566740123429, - "angularVelocity": -2.0390780833167242e-7, - "velocityX": 3.2694542215871127, - "velocityY": -1.90101475472538, - "timestamp": 6.058841766481056 - }, - { - "x": 4.821696252971789, - "y": 4.555032332206395, - "heading": 0.07376566040606643, - "angularVelocity": -2.0158751247220773e-7, - "velocityX": 3.342911154270497, - "velocityY": -1.76865288682714, - "timestamp": 6.093542169182277 - }, - { - "x": 4.939640387985606, - "y": 4.497483166935729, - "heading": 0.07376565344322925, - "angularVelocity": -2.0065580355812287e-7, - "velocityX": 3.398926981607191, - "velocityY": -1.6584581385460047, - "timestamp": 6.128242571883499 - }, - { - "x": 5.057585335343478, - "y": 4.439935666553369, - "heading": 0.07376564648040178, - "angularVelocity": -2.0065552340961329e-7, - "velocityX": 3.3989503918269923, - "velocityY": -1.658410159612758, - "timestamp": 6.162942974584721 - }, - { - "x": 5.1755309999931605, - "y": 4.382389636300601, - "heading": 0.07376563951757731, - "angularVelocity": -2.0065543705761684e-7, - "velocityX": 3.398971062820868, - "velocityY": -1.658367793257397, - "timestamp": 6.197643377285942 - }, - { - "x": 5.295248531429034, - "y": 4.328626841825402, - "heading": 0.07376563255290919, - "angularVelocity": -2.007085673925138e-7, - "velocityX": 3.45003291364278, - "velocityY": -1.5493420908716486, - "timestamp": 6.232343779987164 - }, - { - "x": 5.41702166916594, - "y": 4.27969803322079, - "heading": 0.07376562554616754, - "angularVelocity": -2.019210467909896e-7, - "velocityX": 3.5092716008341425, - "velocityY": -1.410035757391629, - "timestamp": 6.267044182688386 - }, - { - "x": 5.540655427942115, - "y": 4.235681910653619, - "heading": 0.07376561844909023, - "angularVelocity": -2.0452435036769212e-7, - "velocityX": 3.5628911814277564, - "velocityY": -1.2684614338963205, - "timestamp": 6.301744585389607 + "heading": 0.837612783139272, + "angularVelocity": 1.2997978965886135e-7, + "velocityX": 2.3324012372734173, + "velocityY": -2.9043995371065314, + "timestamp": 5.209671311707801 + }, + { + "x": 4.272973770448513, + "y": 4.872239726327977, + "heading": 0.8376127902271764, + "angularVelocity": 1.6705024403847005e-7, + "velocityX": 2.4858652687195653, + "velocityY": -2.7741855431639553, + "timestamp": 5.252101087117018 + }, + { + "x": 4.386494636503838, + "y": 4.762271022457296, + "heading": 0.8376127978646637, + "angularVelocity": 1.80003009789001e-7, + "velocityX": 2.67550004591034, + "velocityY": -2.5917814273132675, + "timestamp": 5.294530862526235 + }, + { + "x": 4.507495275149132, + "y": 4.6605904045479685, + "heading": 0.8376128062844473, + "angularVelocity": 1.984404462541964e-7, + "velocityX": 2.85178597997034, + "velocityY": -2.3964448769445874, + "timestamp": 5.336960637935452 + }, + { + "x": 4.635371955120012, + "y": 4.567705222787466, + "heading": 0.8376128158092191, + "angularVelocity": 2.2448320171896186e-7, + "velocityX": 3.013842961400671, + "velocityY": -2.189150917360859, + "timestamp": 5.3793904133446695 + }, + { + "x": 4.769486631120928, + "y": 4.484078938092311, + "heading": 0.8376128269135948, + "angularVelocity": 2.6171186455813295e-7, + "velocityX": 3.16086226494103, + "velocityY": -1.9709339464707416, + "timestamp": 5.421820188753887 + }, + { + "x": 4.909170130626689, + "y": 4.4101288120215045, + "heading": 0.8376128403409839, + "angularVelocity": 3.1646147045577477e-7, + "velocityX": 3.2921102729998752, + "velocityY": -1.7428828071228117, + "timestamp": 5.464249964163104 + }, + { + "x": 5.05372549373233, + "y": 4.346223825602605, + "heading": 0.8376128573444204, + "angularVelocity": 4.007430216220621e-7, + "velocityX": 3.406932082752448, + "velocityY": -1.5061353920110019, + "timestamp": 5.506679739572321 + }, + { + "x": 5.2024314508880005, + "y": 4.292682838626271, + "heading": 0.8376128802411438, + "angularVelocity": 5.396381015902925e-7, + "velocityX": 3.5047547558634515, + "velocityY": -1.261872976228496, + "timestamp": 5.549109514981538 + }, + { + "x": 5.354546021326616, + "y": 4.249772998903826, + "heading": 0.8376129138984257, + "angularVelocity": 7.932467614948602e-7, + "velocityX": 3.58509016301729, + "velocityY": -1.0113143260504642, + "timestamp": 5.591539290390755 + }, + { + "x": 5.509310213426411, + "y": 4.217708409612342, + "heading": 0.8376129707519607, + "angularVelocity": 0.0000013399442828266796, + "velocityX": 3.647537386355726, + "velocityY": -0.7557096162361191, + "timestamp": 5.633969065799972 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.07376561120324407, - "angularVelocity": -2.0881158710741676e-7, - "velocityX": 3.610802501559291, - "velocityY": -1.1248525394721576, - "timestamp": 6.336444988090829 - }, - { - "x": 5.801240954312848, - "y": 4.160760783914465, - "heading": 0.0737656041692584, - "angularVelocity": -1.900588111168926e-7, - "velocityX": 3.655524839727154, - "velocityY": -0.9697042570289608, - "timestamp": 6.37345450968449 - }, - { - "x": 5.937938725958539, - "y": 4.130679839530998, - "heading": 0.07376559729974105, - "angularVelocity": -1.8561486466113367e-7, - "velocityX": 3.6935838605680487, - "velocityY": -0.8127893333433076, - "timestamp": 6.410464031278151 - }, - { - "x": 6.075795864969434, - "y": 4.106460978217925, - "heading": 0.07376559053845443, - "angularVelocity": -1.8269046288328192e-7, - "velocityX": 3.724910052187935, - "velocityY": -0.654395416914037, - "timestamp": 6.447473552871812 - }, - { - "x": 6.214561046214486, - "y": 4.088147908867804, - "heading": 0.07376558383255526, - "angularVelocity": -1.8119388963756882e-7, - "velocityX": 3.7494454202515035, - "velocityY": -0.4948204829877394, - "timestamp": 6.484483074465473 - }, - { - "x": 6.353981153300122, - "y": 4.0757714079596425, - "heading": 0.07376557713086826, - "angularVelocity": -1.8108007594786474e-7, - "velocityX": 3.7671415647133246, - "velocityY": -0.33441396633135523, - "timestamp": 6.521492596059134 - }, - { - "x": 6.493802446577043, - "y": 4.0693566803624766, - "heading": 0.07376555560001419, - "angularVelocity": -5.817652634682061e-7, - "velocityX": 3.7779816451577393, - "velocityY": -0.17332641225670456, - "timestamp": 6.558502117652795 - }, - { - "x": 6.62912467253087, - "y": 4.064792329127301, - "heading": 0.05903973203041549, - "angularVelocity": -0.39789283772100603, - "velocityX": 3.656416514635636, - "velocityY": -0.12332910663606324, - "timestamp": 6.595511639246456 - }, - { - "x": 6.758568056447626, - "y": 4.061299539806061, - "heading": 0.04418585550364903, - "angularVelocity": -0.4013528380575057, - "velocityX": 3.4975697696920878, - "velocityY": -0.09437542477821895, - "timestamp": 6.632521160840117 - }, - { - "x": 6.882108384071856, - "y": 4.058691153499139, - "heading": 0.0304778500113165, - "angularVelocity": -0.3703913182893011, - "velocityX": 3.3380687537822458, - "velocityY": -0.07047879017620408, - "timestamp": 6.669530682433778 - }, - { - "x": 6.999742512686264, - "y": 4.056896966002031, - "heading": 0.018392940314004957, - "angularVelocity": -0.3265351503322698, - "velocityX": 3.1784828214195313, - "velocityY": -0.048479078351971995, - "timestamp": 6.706540204027439 - }, - { - "x": 7.111470092404711, - "y": 4.055880215977976, - "heading": 0.008182013942056482, - "angularVelocity": -0.2758999828221868, - "velocityX": 3.0188874351076422, - "velocityY": -0.027472660555273344, - "timestamp": 6.7435497256211 - }, - { - "x": 7.217291355133057, - "y": 4.0556182861328125, - "heading": -2.0237932968568665e-24, - "angularVelocity": -0.22107861949390495, - "velocityX": 2.859298314909037, - "velocityY": -0.007077363713034477, - "timestamp": 6.780559247214761 - }, - { - "x": 7.386520754967261, - "y": 4.057483355651357, - "heading": -0.007799169879435119, - "angularVelocity": -0.11872019703645657, - "velocityX": 2.5760366812439752, - "velocityY": 0.028390383098604113, - "timestamp": 6.846252954554551 - }, - { - "x": 7.536988258489095, - "y": 4.060513288338981, - "heading": -0.011310959722615328, - "angularVelocity": -0.053457020244208106, - "velocityX": 2.2904401291217464, - "velocityY": 0.04612211443560253, - "timestamp": 6.911946661894341 - }, - { - "x": 7.668645085347693, - "y": 4.063976331623079, - "heading": -0.012040004750973126, - "angularVelocity": -0.01109763869143412, - "velocityX": 2.004101034785968, - "velocityY": 0.05271499241448283, - "timestamp": 6.9776403692341304 - }, - { - "x": 7.78147865771714, - "y": 4.06737291870627, - "heading": -0.011006488514551415, - "angularVelocity": 0.015732347560718764, - "velocityX": 1.7175704787953916, - "velocityY": 0.051703385616870434, - "timestamp": 7.04333407657392 - }, - { - "x": 7.87549031086396, - "y": 4.070341151997571, - "heading": -0.008947695308103269, - "angularVelocity": 0.03133927570565307, - "velocityX": 1.4310602484429864, - "velocityY": 0.04518291646943911, - "timestamp": 7.10902778391371 - }, - { - "x": 7.9506868858752515, - "y": 4.072607124350486, - "heading": -0.006421398747140829, - "angularVelocity": 0.03845568568532146, - "velocityX": 1.1446541542000461, - "velocityY": 0.03449298943040145, - "timestamp": 7.1747214912535 - }, - { - "x": 8.00707724488628, - "y": 4.073956458693622, - "heading": -0.003864315760597132, - "angularVelocity": 0.038924321523180636, - "velocityX": 0.8583829607812892, - "velocityY": 0.020539780715319512, - "timestamp": 7.24041519859329 - }, - { - "x": 8.044670745645819, - "y": 4.074216878653992, - "heading": -0.0016276569414438076, - "angularVelocity": 0.034046774184696094, - "velocityX": 0.5722542124939418, - "velocityY": 0.003964153811924349, - "timestamp": 7.30610890593308 - }, - { - "x": 8.0634765625, - "y": 4.073246955871582, - "heading": 1.6866276328916314e-24, - "angularVelocity": 0.02477645131252876, - "velocityX": 0.286265117553968, - "velocityY": -0.014764317948939539, - "timestamp": 7.37180261327287 - }, - { - "x": 8.060961534281953, - "y": 4.070465444687161, - "heading": 0.0007560599723107442, - "angularVelocity": 0.010267428471842888, - "velocityX": -0.03415452911035645, - "velocityY": -0.03777341504058265, - "timestamp": 7.445439354389123 - }, - { - "x": 8.034839704830464, - "y": 4.066167075236277, - "heading": 0.00046881224437648735, - "angularVelocity": -0.0039008750737728444, - "velocityX": -0.3547390752973492, - "velocityY": -0.0583726192349958, - "timestamp": 7.519076095505376 - }, - { - "x": 7.985098416570273, - "y": 4.060562217226684, - "heading": -0.0008319841828060777, - "angularVelocity": -0.017665046109644415, - "velocityX": -0.6754955136004895, - "velocityY": -0.0761149654999585, - "timestamp": 7.592712836621629 - }, - { - "x": 7.9117249252468085, - "y": 4.053904178499818, - "heading": -0.0031104504440480647, - "angularVelocity": -0.03094197579500262, - "velocityX": -0.9964250211402843, - "velocityY": -0.09041734636729296, - "timestamp": 7.666349577737883 - }, - { - "x": 7.814707316285252, - "y": 4.046503723453766, - "heading": -0.00632248118792148, - "angularVelocity": -0.04361994698818144, - "velocityX": -1.3175163307185394, - "velocityY": -0.10049949161068944, - "timestamp": 7.739986318854136 - }, - { - "x": 7.69403642851654, - "y": 4.038750888931838, - "heading": -0.010412565479377186, - "angularVelocity": -0.055544069841419484, - "velocityX": -1.6387320505969085, - "velocityY": -0.10528486736925086, - "timestamp": 7.813623059970389 - }, - { - "x": 7.549709960346403, - "y": 4.031149265281445, - "heading": -0.015308761000708897, - "angularVelocity": -0.06649120326498265, - "velocityX": -1.9599790265335464, - "velocityY": -0.10323139692441612, - "timestamp": 7.887259801086643 - }, - { - "x": 7.381741634294396, - "y": 4.0243729190236985, - "heading": -0.02091427607708914, - "angularVelocity": -0.07612388858342581, - "velocityX": -2.2810396482216517, - "velocityY": -0.0920239836123183, - "timestamp": 7.960896542202896 - }, - { - "x": 7.1901832106568095, - "y": 4.019367673272749, - "heading": -0.027092292311801627, - "angularVelocity": -0.08389855581684406, - "velocityX": -2.6013973559091212, - "velocityY": -0.06797212471757884, - "timestamp": 8.03453328331915 - }, - { - "x": 6.975183472586086, - "y": 4.017547771662454, - "heading": -0.03363583639032012, - "angularVelocity": -0.08886248874305737, - "velocityX": -2.9197345620074877, - "velocityY": -0.02471458653257856, - "timestamp": 8.108170024435402 - }, - { - "x": 6.737173435740335, - "y": 4.021221144917956, - "heading": -0.04019984018910067, - "angularVelocity": -0.0891403353716819, - "velocityX": -3.232218499050576, - "velocityY": 0.04988506009116617, - "timestamp": 8.181806765551656 - }, - { - "x": 6.477592542757965, - "y": 4.034606496898433, - "heading": -0.04612188209053887, - "angularVelocity": -0.08042237898726157, - "velocityX": -3.5251545498538244, - "velocityY": 0.18177545308999152, - "timestamp": 8.255443506667909 - }, - { - "x": 6.202379983792451, - "y": 4.065741183912862, - "heading": -0.04994434785858321, - "angularVelocity": -0.05190976284528472, - "velocityX": -3.737435345366867, - "velocityY": 0.4228145697713012, - "timestamp": 8.329080247784162 - }, - { - "x": 5.92930929060311, - "y": 4.120418111584284, - "heading": -0.049944382856938176, - "angularVelocity": -4.752838655296773e-7, - "velocityX": -3.7083484283780552, - "velocityY": 0.7425223718836313, - "timestamp": 8.402716988900416 + "heading": 0.837613299622043, + "angularVelocity": 0.000007750926774500913, + "velocityX": 3.691782807795942, + "velocityY": -0.49633388003567147, + "timestamp": 5.676398841209189 + }, + { + "x": 5.886145215418506, + "y": 4.18891028787692, + "heading": 0.8376134089297179, + "angularVelocity": 0.0000018480104187891978, + "velocityX": 3.7227016115303835, + "velocityY": -0.13083581208893053, + "timestamp": 5.735547682447417 + }, + { + "x": 6.106032301025365, + "y": 4.202865338919715, + "heading": 0.8376134508974878, + "angularVelocity": 7.095281843173525e-7, + "velocityX": 3.7175214425797836, + "velocityY": 0.23593109773003054, + "timestamp": 5.794696523685645 + }, + { + "x": 6.3234808347180325, + "y": 4.238378912467147, + "heading": 0.8376134741732665, + "angularVelocity": 3.9351199795834263e-7, + "velocityX": 3.676294059876367, + "velocityY": 0.6004103005906283, + "timestamp": 5.853845364923873 + }, + { + "x": 6.536382325934404, + "y": 4.2951066501716255, + "heading": 0.8376134896107453, + "angularVelocity": 2.6099376502854354e-7, + "velocityX": 3.599419477363716, + "velocityY": 0.9590676083746476, + "timestamp": 5.912994206162101 + }, + { + "x": 6.742672378442076, + "y": 4.372498490127808, + "heading": 0.8376135010511353, + "angularVelocity": 1.9341697541887733e-7, + "velocityX": 3.4876431759130977, + "velocityY": 1.3084252934808847, + "timestamp": 5.972143047400329 + }, + { + "x": 6.940350706341849, + "y": 4.46980400008761, + "heading": 0.8376135102232026, + "angularVelocity": 1.550675727090784e-7, + "velocityX": 3.3420490369980995, + "velocityY": 1.64509579431818, + "timestamp": 6.031291888638557 + }, + { + "x": 7.12750053081492, + "y": 4.586079652731035, + "heading": 0.837613518040013, + "angularVelocity": 1.3215492103145434e-7, + "velocityX": 3.164048873236707, + "velocityY": 1.9658145486758316, + "timestamp": 6.090440729876785 + }, + { + "x": 7.302307170565869, + "y": 4.720197970950524, + "heading": 0.837613525049849, + "angularVelocity": 1.185118059136636e-7, + "velocityX": 2.955368796607497, + "velocityY": 2.2674716091108755, + "timestamp": 6.149589571115013 + }, + { + "x": 7.4630756537468415, + "y": 4.8708584478992005, + "heading": 0.8376135339238266, + "angularVelocity": 1.5002791962179725e-7, + "velocityX": 2.718032675119729, + "velocityY": 2.5471416479974107, + "timestamp": 6.208738412353241 + }, + { + "x": 7.604661415508242, + "y": 5.027831186839046, + "heading": 0.8715939059006107, + "angularVelocity": 0.5744892252398454, + "velocityX": 2.3937199579472788, + "velocityY": 2.6538599176883673, + "timestamp": 6.267887253591469 + }, + { + "x": 7.730952710188879, + "y": 5.1694158341932015, + "heading": 0.9053997225906723, + "angularVelocity": 0.5715381059437028, + "velocityX": 2.1351440203534295, + "velocityY": 2.3937011172189955, + "timestamp": 6.327036094829697 + }, + { + "x": 7.842513418943255, + "y": 5.295119262651075, + "heading": 0.934282004580144, + "angularVelocity": 0.48829835690517676, + "velocityX": 1.886101340600306, + "velocityY": 2.125205258909303, + "timestamp": 6.386184936067925 + }, + { + "x": 7.939539266004037, + "y": 5.404796625949478, + "heading": 0.9566775785409475, + "angularVelocity": 0.3786308149402807, + "velocityX": 1.6403676729692935, + "velocityY": 1.8542605569679314, + "timestamp": 6.4453337773061525 + }, + { + "x": 8.0221293723114, + "y": 5.498379145291686, + "heading": 0.9718066727965543, + "angularVelocity": 0.25578006160210215, + "velocityX": 1.3963097937071225, + "velocityY": 1.5821530461652626, + "timestamp": 6.5044826185443805 + }, + { + "x": 8.090343615854179, + "y": 5.575826687480091, + "heading": 0.9792023087722939, + "angularVelocity": 0.12503433407854694, + "velocityX": 1.153264241780091, + "velocityY": 1.3093670233788228, + "timestamp": 6.563631459782608 + }, + { + "x": 8.14422212483701, + "y": 5.637112925793437, + "heading": 0.9785539274051055, + "angularVelocity": -0.010961860851625928, + "velocityX": 0.9108971174232933, + "velocityY": 1.0361359078280012, + "timestamp": 6.622780301020836 + }, + { + "x": 8.183793718837835, + "y": 5.682219218209322, + "heading": 0.9696403414250478, + "angularVelocity": -0.15069755879337374, + "velocityX": 0.6690172313173101, + "velocityY": 0.7625896208890375, + "timestamp": 6.681929142259064 + }, + { + "x": 8.209080155266193, + "y": 5.71113162509295, + "heading": 0.9522961336408167, + "angularVelocity": -0.29322988280320866, + "velocityX": 0.4275051868981633, + "velocityY": 0.4888076634871, + "timestamp": 6.741077983497292 + }, + { + "x": 8.220098495483398, + "y": 5.723839282989502, + "heading": 0.9263929393086, + "angularVelocity": -0.4379324056051909, + "velocityX": 0.18628159041742642, + "velocityY": 0.21484204306506113, + "timestamp": 6.80022682473552 + }, + { + "x": 8.216352100309892, + "y": 5.7197578257583315, + "heading": 0.8905883238088871, + "angularVelocity": -0.5885448940291729, + "velocityX": -0.06158205358748059, + "velocityY": -0.06708969723814164, + "timestamp": 6.8610626519815865 + }, + { + "x": 8.197510123572037, + "y": 5.69852730877679, + "heading": 0.8459980645635137, + "angularVelocity": -0.7329605146161121, + "velocityX": -0.3097184272952048, + "velocityY": -0.3489804929530894, + "timestamp": 6.921898479227653 + }, + { + "x": 8.163552039603486, + "y": 5.660151329305612, + "heading": 0.7930933731491355, + "angularVelocity": -0.8696305090155458, + "velocityX": -0.5581921953851212, + "velocityY": -0.6308121580389989, + "timestamp": 6.982734306473719 + }, + { + "x": 8.114451794656734, + "y": 5.604635385719368, + "heading": 0.7324785026638988, + "angularVelocity": -0.9963679829660959, + "velocityX": -0.8070942266989081, + "velocityY": -0.9125534425906069, + "timestamp": 7.043570133719785 + }, + { + "x": 8.05017522631974, + "y": 5.531988399816786, + "heading": 0.664956367976081, + "angularVelocity": -1.1099073974075735, + "velocityX": -1.0565578088880443, + "velocityY": -1.1941480734492433, + "timestamp": 7.104405960965852 + }, + { + "x": 7.970675640275654, + "y": 5.442225859030612, + "heading": 0.5916510965236437, + "angularVelocity": -1.2049687621725864, + "velocityX": -1.3067889374221797, + "velocityY": -1.4754881268090099, + "timestamp": 7.165241788211918 + }, + { + "x": 7.87588580389167, + "y": 5.335377165777019, + "heading": 0.5142662873089652, + "angularVelocity": -1.2720269077903064, + "velocityX": -1.558125214613782, + "velocityY": -1.7563448725931903, + "timestamp": 7.226077615457984 + }, + { + "x": 7.765703024679293, + "y": 5.211506902101234, + "heading": 0.4357243406909052, + "angularVelocity": -1.2910475647906803, + "velocityX": -1.8111495183046493, + "velocityY": -2.0361400392364293, + "timestamp": 7.2869134427040505 + }, + { + "x": 7.639967535619313, + "y": 5.070804612535801, + "heading": 0.36221787896188584, + "angularVelocity": -1.2082758640184799, + "velocityX": -2.0668000214973703, + "velocityY": -2.3128195330742662, + "timestamp": 7.347749269950117 + }, + { + "x": 7.498615166036793, + "y": 4.914564832190124, + "heading": 0.31493629004406687, + "angularVelocity": -0.7771997367041067, + "velocityX": -2.32350534185693, + "velocityY": -2.5682198700730225, + "timestamp": 7.408585097196183 + }, + { + "x": 7.337231161361771, + "y": 4.755477157818062, + "heading": 0.3149362776036475, + "angularVelocity": -2.0449166092827028e-7, + "velocityX": -2.652778995217139, + "velocityY": -2.615032647268723, + "timestamp": 7.469420924442249 + }, + { + "x": 7.160583323022439, + "y": 4.613529185957489, + "heading": 0.3149362685790419, + "angularVelocity": -1.4834359997406843e-7, + "velocityX": -2.903681043488322, + "velocityY": -2.33329566287359, + "timestamp": 7.530256751688316 + }, + { + "x": 6.970483558560949, + "y": 4.490176988250006, + "heading": 0.31493625863244995, + "angularVelocity": -1.6349891854082722e-7, + "velocityX": -3.124799531246346, + "velocityY": -2.0276242354452334, + "timestamp": 7.591092578934382 + }, + { + "x": 6.76888179398853, + "y": 4.386685862868273, + "heading": 0.31493624706316514, + "angularVelocity": -1.9017222732229534e-7, + "velocityX": -3.313865754746604, + "velocityY": -1.701154238655088, + "timestamp": 7.651928406180448 + }, + { + "x": 6.557845947797341, + "y": 4.304117375120236, + "heading": 0.31493623270578724, + "angularVelocity": -2.3600201712655827e-7, + "velocityX": -3.4689401910752493, + "velocityY": -1.3572345686049037, + "timestamp": 7.7127642334265145 + }, + { + "x": 6.339540712904722, + "y": 4.243318473621985, + "heading": 0.3149362133315886, + "angularVelocity": -3.184669214162522e-7, + "velocityX": -3.58843209297751, + "velocityY": -0.9993930262891738, + "timestamp": 7.773600060672581 + }, + { + "x": 6.116205355143182, + "y": 4.204912805417524, + "heading": 0.3149361681436048, + "angularVelocity": -7.427857215236805e-7, + "velocityX": -3.671115654566562, + "velocityY": -0.6313001720042296, + "timestamp": 7.834435887918647 + }, + { + "x": 5.89050677740902, + "y": 4.189334421603551, + "heading": 0.31368110305887587, + "angularVelocity": -0.02063036111356669, + "velocityX": -3.7099615136532513, + "velocityY": -0.25607252369499406, + "timestamp": 7.895271715164713 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": -0.05351191132153618, - "angularVelocity": -0.04844766906462906, - "velocityX": -3.5764423817525626, - "velocityY": 1.0352299927261919, - "timestamp": 8.476353730016669 - }, - { - "x": 5.427595279774474, - "y": 4.2855510061769815, - "heading": -0.058564536366975395, - "angularVelocity": -0.07109019818968403, - "velocityX": -3.3536640954167023, - "velocityY": 1.2508460219498765, - "timestamp": 8.547427171561418 - }, - { - "x": 5.210001648120265, - "y": 4.381839057848659, - "heading": -0.06451807274962795, - "angularVelocity": -0.08376597858855642, - "velocityX": -3.0615322253278787, - "velocityY": 1.3547683857556179, - "timestamp": 8.618500613106168 - }, - { - "x": 5.014433901254649, - "y": 4.478870971040357, - "heading": -0.07079419051912546, - "angularVelocity": -0.08830468362146032, - "velocityX": -2.751629056016378, - "velocityY": 1.365234482568344, - "timestamp": 8.689574054650917 - }, - { - "x": 4.840611358699117, - "y": 4.572318839972298, - "heading": -0.07701240147326043, - "angularVelocity": -0.08748993743633168, - "velocityX": -2.445675047916283, - "velocityY": 1.3148071473801226, - "timestamp": 8.760647496195666 - }, - { - "x": 4.687900440256207, - "y": 4.6594427038525446, - "heading": -0.08292539389195669, - "angularVelocity": -0.08319552691103697, - "velocityX": -2.148635483575938, - "velocityY": 1.2258286919367853, - "timestamp": 8.831720937740416 - }, - { - "x": 4.555672965258425, - "y": 4.738435503151125, - "heading": -0.088365740210127, - "angularVelocity": -0.07654541837185252, - "velocityX": -1.8604343918610988, - "velocityY": 1.1114249933829867, - "timestamp": 8.902794379285165 - }, - { - "x": 4.443385396304237, - "y": 4.808043311257638, - "heading": -0.09321471505627854, - "angularVelocity": -0.06822484940592717, - "velocityX": -1.5798808459765967, - "velocityY": 0.9793786060392976, - "timestamp": 8.973867820829915 - }, - { - "x": 4.3505830000627, - "y": 4.867355530244293, - "heading": -0.09738458875338916, - "angularVelocity": -0.058669927985478765, - "velocityX": -1.3057253768006336, - "velocityY": 0.8345201484201303, - "timestamp": 9.044941262374664 - }, - { - "x": 4.2768869568191645, - "y": 4.915685373463825, - "heading": -0.10080825239426806, - "angularVelocity": -0.04817078737805652, - "velocityX": -1.0368998833007734, - "velocityY": 0.6799986347798151, - "timestamp": 9.116014703919413 - }, - { - "x": 4.22197990513087, - "y": 4.95249852612905, - "heading": -0.10343286860563979, - "angularVelocity": -0.03692822739868577, - "velocityX": -0.7725396504645569, - "velocityY": 0.5179593370618798, - "timestamp": 9.187088145464163 - }, - { - "x": 4.185593591353172, - "y": 4.97736854606154, - "heading": -0.10521581647883109, - "angularVelocity": -0.025085993226720307, - "velocityX": -0.5119537338682039, - "velocityY": 0.3499200178287514, - "timestamp": 9.258161587008912 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.012749959780663622, - "velocityX": -0.25458913429159136, - "velocityY": 0.1769894567221649, - "timestamp": 9.329235028553661 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": 1.153459712624117e-25, - "velocityX": -3.247224190829055e-25, - "velocityY": -4.023670796703132e-25, - "timestamp": 9.40030847009841 - }, - { - "x": 4.180652954196475, - "y": 4.972113880358222, - "heading": -0.10282545923039073, - "angularVelocity": 0.04623963021087373, - "velocityX": 0.18450581877512717, - "velocityY": -0.2501512088012644, - "timestamp": 9.471601011898064 - }, - { - "x": 4.207780209062833, - "y": 4.937082603957029, - "heading": -0.09606696501055557, - "angularVelocity": 0.09479945656626756, - "velocityX": 0.380506209788268, - "velocityY": -0.4913736488683167, - "timestamp": 9.542893553697718 - }, - { - "x": 4.249890859346282, - "y": 4.885729175148701, - "heading": -0.08564531821991797, - "angularVelocity": 0.1461814451773148, - "velocityX": 0.5906739922639744, - "velocityY": -0.7203197909907696, - "timestamp": 9.614186095497372 - }, - { - "x": 4.308238374054552, - "y": 4.819303841665251, - "heading": -0.07131614738285201, - "angularVelocity": 0.20099116226398103, - "velocityX": 0.8184238243635266, - "velocityY": -0.931729067398359, - "timestamp": 9.685478637297026 - }, - { - "x": 4.384365934500173, - "y": 4.739668033977246, - "heading": -0.05278830976865122, - "angularVelocity": 0.2598846547829301, - "velocityX": 1.0678194173459812, - "velocityY": -1.1170285934228166, - "timestamp": 9.75677117909668 - }, - { - "x": 4.480080041577144, - "y": 4.649696456670524, - "heading": -0.02973745107706772, - "angularVelocity": 0.3233277718777487, - "velocityX": 1.3425542793234355, - "velocityY": -1.262005464183884, - "timestamp": 9.828063720896333 - }, - { - "x": 4.597147014770728, - "y": 4.553865445568556, - "heading": -0.0018737410680010795, - "angularVelocity": 0.3908362544762254, - "velocityX": 1.642064797220515, - "velocityY": -1.344194058493121, - "timestamp": 9.899356262695987 - }, - { - "x": 4.7363464626810785, - "y": 4.458651640127929, - "heading": 0.030870956626381736, - "angularVelocity": 0.4593004663293065, - "velocityX": 1.9525106609542362, - "velocityY": -1.3355366920174645, - "timestamp": 9.97064880449564 - }, - { - "x": 4.8961444414371185, - "y": 4.371572487351198, - "heading": 0.06809613303796244, - "angularVelocity": 0.5221468539611136, - "velocityX": 2.2414403347422116, - "velocityY": -1.2214342563551692, - "timestamp": 10.041941346295294 - }, - { - "x": 5.072824953795758, - "y": 4.298842121244781, - "heading": 0.10897366885891092, - "angularVelocity": 0.5733774499978155, - "velocityX": 2.478246782884325, - "velocityY": -1.0201679484342572, - "timestamp": 10.113233888094948 - }, - { - "x": 5.262214444102689, - "y": 4.244271073764123, - "heading": 0.1525665304554643, - "angularVelocity": 0.6114645444828949, - "velocityX": 2.6565119650124474, - "velocityY": -0.7654524036190654, - "timestamp": 10.184526429894602 - }, - { - "x": 5.460811083302549, - "y": 4.20986764407754, - "heading": 0.19806311217601916, - "angularVelocity": 0.638167479684045, - "velocityX": 2.785657997129001, - "velocityY": -0.48256702339585567, - "timestamp": 10.255818971694255 + "heading": 0.30754704224604024, + "angularVelocity": -0.10082974277681633, + "velocityX": -3.691164544865137, + "velocityY": 0.12023594125393282, + "timestamp": 7.95610754241078 + }, + { + "x": 5.419243898269495, + "y": 4.233180640603145, + "heading": 0.29443096525634566, + "angularVelocity": -0.19257128900213308, + "velocityX": -3.6221840549970006, + "velocityY": 0.5363593678805259, + "timestamp": 8.024217783857775 + }, + { + "x": 5.182010137227644, + "y": 4.296746845588596, + "heading": 0.2703885847974574, + "angularVelocity": -0.35299214843626897, + "velocityX": -3.483085010445408, + "velocityY": 0.9332840940656272, + "timestamp": 8.092328025304772 + }, + { + "x": 4.96090074800009, + "y": 4.383753869716349, + "heading": 0.22896738213665188, + "angularVelocity": -0.6081494028036839, + "velocityX": -3.2463456967719417, + "velocityY": 1.2774440712482642, + "timestamp": 8.160438266751768 + }, + { + "x": 4.76451355722713, + "y": 4.485181561673583, + "heading": 0.17582209664433623, + "angularVelocity": -0.7802833224967138, + "velocityX": -2.8833724062744586, + "velocityY": 1.489169467064151, + "timestamp": 8.228548508198765 + }, + { + "x": 4.596735729440321, + "y": 4.5894452853276455, + "heading": 0.11854658262999372, + "angularVelocity": -0.8409236672419458, + "velocityX": -2.4633274559359006, + "velocityY": 1.5308083107472321, + "timestamp": 8.296658749645761 + }, + { + "x": 4.457111536094513, + "y": 4.687809707372902, + "heading": 0.06296288370262228, + "angularVelocity": -0.8160843031312246, + "velocityX": -2.049973548463538, + "velocityY": 1.444194293773058, + "timestamp": 8.364768991092758 + }, + { + "x": 4.343947453919847, + "y": 4.774964113624853, + "heading": 0.012793626993843597, + "angularVelocity": -0.7365890304150604, + "velocityX": -1.6614840847793941, + "velocityY": 1.2796079473565642, + "timestamp": 8.432879232539754 + }, + { + "x": 4.255603402479856, + "y": 4.847699861550099, + "heading": -0.029623031457827412, + "angularVelocity": -0.622764764160759, + "velocityX": -1.2970744129389131, + "velocityY": 1.0679120552207846, + "timestamp": 8.50098947398675 + }, + { + "x": 4.190737568538254, + "y": 4.903969414198745, + "heading": -0.06275873479049468, + "angularVelocity": -0.4865010404999608, + "velocityX": -0.9523653501078979, + "velocityY": 0.8261540621968694, + "timestamp": 8.569099715433747 + }, + { + "x": 4.148285991242422, + "y": 4.94238614886978, + "heading": -0.0855582962836916, + "angularVelocity": -0.3347449812072629, + "velocityX": -0.6232774454171954, + "velocityY": 0.5640375640267039, + "timestamp": 8.637209956880744 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.17170074221519835, + "velocityX": -0.3066229399994914, + "velocityY": 0.28741048233214816, + "timestamp": 8.70532019832774 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -9.317687373474837e-25, + "velocityX": -1.6510663048617945e-23, + "velocityY": -3.5986344010888627e-23, + "timestamp": 8.773430439774737 + }, + { + "x": 4.147821870498822, + "y": 4.951212055722382, + "heading": -0.09513615443770512, + "angularVelocity": 0.0346947715699437, + "velocityX": 0.3347010455989641, + "velocityY": -0.1761961456823413, + "timestamp": 8.83444023344422 + }, + { + "x": 4.188661953327048, + "y": 4.929712670962105, + "heading": -0.09090262623774811, + "angularVelocity": 0.06939096078396689, + "velocityX": 0.6694020807458456, + "velocityY": -0.3523923532137878, + "timestamp": 8.895450027113702 + }, + { + "x": 4.2499220745882305, + "y": 4.89746358708528, + "heading": -0.08455181431110974, + "angularVelocity": 0.10409495827905127, + "velocityX": 1.004103072255173, + "velocityY": -0.5285886402359044, + "timestamp": 8.956459820783184 + }, + { + "x": 4.331602229572044, + "y": 4.854464798258643, + "heading": -0.07608283990612706, + "angularVelocity": 0.13881335922659968, + "velocityX": 1.3388039865584789, + "velocityY": -0.7047850228699344, + "timestamp": 9.017469614452667 + }, + { + "x": 4.433702411475643, + "y": 4.800716297797124, + "heading": -0.06549440517861618, + "angularVelocity": 0.17355303289293356, + "velocityX": 1.6735047893575954, + "velocityY": -0.8809815150777099, + "timestamp": 9.07847940812215 + }, + { + "x": 4.556222611314502, + "y": 4.736218078349401, + "heading": -0.05278477279271126, + "angularVelocity": 0.20832118290316787, + "velocityX": 2.0082054448930773, + "velocityY": -1.0571781277795957, + "timestamp": 9.139489201791632 + }, + { + "x": 4.699162817715129, + "y": 4.660970132179323, + "heading": -0.03795174297125847, + "angularVelocity": 0.24312538904507433, + "velocityX": 2.3429059140077895, + "velocityY": -1.2333748672832447, + "timestamp": 9.200498995461114 + }, + { + "x": 4.862523016289189, + "y": 4.574972451706886, + "heading": -0.02099263124198345, + "angularVelocity": 0.27797359586478865, + "velocityX": 2.677606147286025, + "velocityY": -1.4095717310293654, + "timestamp": 9.261508789130596 + }, + { + "x": 5.046303186466893, + "y": 4.478225031445458, + "heading": -0.0019042656986956292, + "angularVelocity": 0.3128737927995283, + "velocityX": 3.0123060434087328, + "velocityY": -1.5857686847058003, + "timestamp": 9.322518582800079 + }, + { + "x": 5.247403827752691, + "y": 4.372365027625972, + "heading": -0.0019042643151741538, + "angularVelocity": 2.2677039080484546e-8, + "velocityX": 3.2962026125714896, + "velocityY": -1.735131319948009, + "timestamp": 9.383528376469561 + }, + { + "x": 5.4522102780182164, + "y": 4.273865655817337, + "heading": -0.0019042629287868294, + "angularVelocity": 2.2724012670003775e-8, + "velocityX": 3.356943827331282, + "velocityY": -1.6144845914780435, + "timestamp": 9.444538170139044 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.24483305037299086, - "angularVelocity": 0.6560284851170571, - "velocityX": 2.8774488935285283, - "velocityY": -0.18541307673169433, - "timestamp": 10.327111513493909 - }, - { - "x": 5.802993350275697, - "y": 4.197353588266771, - "heading": 0.2759731999769641, - "angularVelocity": 0.6634007595054764, - "velocityX": 2.919495150576939, - "velocityY": 0.015008756790958989, - "timestamp": 10.374051691294106 - }, - { - "x": 5.941816707126591, - "y": 4.207504283843094, - "heading": 0.3073984177151568, - "angularVelocity": 0.669473768760635, - "velocityX": 2.9574527272095095, - "velocityY": 0.21624748886826958, - "timestamp": 10.420991869094303 - }, - { - "x": 6.082189718099688, - "y": 4.227142107895406, - "heading": 0.33903536512016264, - "angularVelocity": 0.6739843964730912, - "velocityX": 2.990466111368347, - "velocityY": 0.4183585357494954, - "timestamp": 10.4679320468945 - }, - { - "x": 6.223826665767993, - "y": 4.256309424549035, - "heading": 0.37079417516105295, - "angularVelocity": 0.6765805228960279, - "velocityX": 3.01739265392617, - "velocityY": 0.6213720957295971, - "timestamp": 10.514872224694697 - }, - { - "x": 6.366368317456564, - "y": 4.295047066484075, - "heading": 0.4025622544615229, - "angularVelocity": 0.6767779925268211, - "velocityX": 3.0366662072586053, - "velocityY": 0.8252555433413251, - "timestamp": 10.561812402494894 - }, - { - "x": 6.509351710185931, - "y": 4.3433874902945675, - "heading": 0.4341948054163167, - "angularVelocity": 0.6738907357666759, - "velocityX": 3.0460769308966165, - "velocityY": 1.0298304368648719, - "timestamp": 10.608752580295091 - }, - { - "x": 6.652162883267978, - "y": 4.401339041781646, - "heading": 0.4654999103087445, - "angularVelocity": 0.6669149193613809, - "velocityX": 3.0424080132360682, - "velocityY": 1.2345831269270133, - "timestamp": 10.655692758095288 - }, - { - "x": 6.793961557546229, - "y": 4.468849069286898, - "heading": 0.4962146580453544, - "angularVelocity": 0.654338120902485, - "velocityX": 3.0208380309469742, - "velocityY": 1.4382141412546399, - "timestamp": 10.702632935895485 - }, - { - "x": 6.933563512190389, - "y": 4.545714915555555, - "heading": 0.5259679790741631, - "angularVelocity": 0.6338561638060835, - "velocityX": 2.9740397498786737, - "velocityY": 1.6375278039175576, - "timestamp": 10.749573113695682 - }, - { - "x": 7.069284940391168, - "y": 4.631368590173797, - "heading": 0.5542337619189218, - "angularVelocity": 0.6021660796657563, - "velocityX": 2.8913701345248803, - "velocityY": 1.8247411627375887, - "timestamp": 10.796513291495879 - }, - { - "x": 7.1988866969659675, - "y": 4.724417833549329, - "heading": 0.5803339735600787, - "angularVelocity": 0.5560313757705206, - "velocityX": 2.7609984164621943, - "velocityY": 1.9822942250367808, - "timestamp": 10.843453469296076 - }, - { - "x": 7.32010179733884, - "y": 4.822118790801402, - "heading": 0.6036855499367045, - "angularVelocity": 0.49747524340496785, - "velocityX": 2.5823315132896965, - "velocityY": 2.0813929948868157, - "timestamp": 10.890393647096273 - }, - { - "x": 7.431787144885325, - "y": 4.920866698749227, - "heading": 0.6241547457610245, - "angularVelocity": 0.43606984003016963, - "velocityX": 2.379312409549823, - "velocityY": 2.103696930338622, - "timestamp": 10.93733382489647 - }, - { - "x": 7.534091847804722, - "y": 5.017622526169069, - "heading": 0.6419335715808812, - "angularVelocity": 0.37875497394860314, - "velocityX": 2.1794698638522156, - "velocityY": 2.0612582217239557, - "timestamp": 10.984274002696667 - }, - { - "x": 7.627668873285194, - "y": 5.110394301950092, - "heading": 0.6572905320305523, - "angularVelocity": 0.32716025309998836, - "velocityX": 1.9935379426721815, - "velocityY": 1.9763831354859511, - "timestamp": 11.031214180496864 - }, - { - "x": 7.7131909565949455, - "y": 5.197947878934244, - "heading": 0.6704631268278166, - "angularVelocity": 0.2806251576918574, - "velocityX": 1.8219377794813416, - "velocityY": 1.8652161343918634, - "timestamp": 11.078154358297061 - }, - { - "x": 7.791223380396529, - "y": 5.279496011851437, - "heading": 0.6816390937397763, - "angularVelocity": 0.2380895734892706, - "velocityX": 1.6623802349818717, - "velocityY": 1.7372778872782495, - "timestamp": 11.125094536097258 - }, - { - "x": 7.86221923742719, - "y": 5.354510990808827, - "heading": 0.6909640471575174, - "angularVelocity": 0.19865611624721707, - "velocityX": 1.5124752473852405, - "velocityY": 1.5980974609149294, - "timestamp": 11.172034713897455 - }, - { - "x": 7.926540429785996, - "y": 5.422621631484432, - "heading": 0.6985519881256509, - "angularVelocity": 0.1616513043566187, - "velocityX": 1.370280117655087, - "velocityY": 1.4510094308871155, - "timestamp": 11.218974891697652 - }, - { - "x": 7.984478950500488, - "y": 5.483555793762207, - "heading": 0.7044936776046891, - "angularVelocity": 0.12658003777337848, - "velocityX": 1.2343055231087734, - "velocityY": 1.2981238063720744, - "timestamp": 11.265915069497849 - }, - { - "x": 8.047923980956542, - "y": 5.548807333254465, - "heading": 0.7094925329826927, - "angularVelocity": 0.08420982403970191, - "velocityX": 1.0687836408324876, - "velocityY": 1.099215769102154, - "timestamp": 11.325276972161923 - }, - { - "x": 8.10152245585906, - "y": 5.602265273787256, - "heading": 0.7122491464591354, - "angularVelocity": 0.04643741781732065, - "velocityX": 0.9029103262715175, - "velocityY": 0.9005429094027965, - "timestamp": 11.384638874825997 - }, - { - "x": 8.14525837183985, - "y": 5.64394060886005, - "heading": 0.7129744623769576, - "angularVelocity": 0.012218542285053226, - "velocityX": 0.7367674218309503, - "velocityY": 0.7020552442301676, - "timestamp": 11.444000777490071 - }, - { - "x": 8.179119065532058, - "y": 5.673842216437666, - "heading": 0.7118363633541982, - "angularVelocity": -0.01917221267653394, - "velocityX": 0.5704111925762211, - "velocityY": 0.5037171356657365, - "timestamp": 11.503362680154146 - }, - { - "x": 8.203094267741402, - "y": 5.691977414243475, - "heading": 0.7089715992476469, - "angularVelocity": -0.048259303997766526, - "velocityX": 0.40388197031045997, - "velocityY": 0.3055023001610055, - "timestamp": 11.56272458281822 - }, - { - "x": 8.217175483703613, - "y": 5.698352336883545, - "heading": 0.7044936776046891, - "angularVelocity": -0.07543426746777364, - "velocityX": 0.23720964676445846, - "velocityY": 0.10739080713341471, - "timestamp": 11.622086485482294 - }, - { - "x": 8.220424741398494, - "y": 5.690497924270521, - "heading": 0.6974771095930071, - "angularVelocity": -0.10438216452389025, - "velocityX": 0.04833767031445264, - "velocityY": -0.11684638248302778, - "timestamp": 11.689306475062756 - }, - { - "x": 8.210761211514034, - "y": 5.66775585785793, - "heading": 0.6884846443385353, - "angularVelocity": -0.1337766535013804, - "velocityX": -0.14375976468862095, - "velocityY": -0.338322968428447, - "timestamp": 11.756526464643217 - }, - { - "x": 8.187932028417118, - "y": 5.630349579704253, - "heading": 0.6774812110255055, - "angularVelocity": -0.1636928744218101, - "velocityX": -0.33961896214800763, - "velocityY": -0.556475512524468, - "timestamp": 11.823746454223679 - }, - { - "x": 8.15163887353547, - "y": 5.578552873386559, - "heading": 0.6644254264440203, - "angularVelocity": -0.19422473378782149, - "velocityX": -0.5399161039471123, - "velocityY": -0.7705551078030757, - "timestamp": 11.89096644380414 - }, - { - "x": 8.101525228747773, - "y": 5.512708245233119, - "heading": 0.6492678901971639, - "angularVelocity": -0.225491499499757, - "velocityX": -0.745516997257366, - "velocityY": -0.9795393983901896, - "timestamp": 11.958186433384602 - }, - { - "x": 8.037158859322696, - "y": 5.433255022389235, - "heading": 0.6319488484169726, - "angularVelocity": -0.2576471952507599, - "velocityX": -0.9575480422833206, - "velocityY": -1.1819880267725829, - "timestamp": 12.025406422965064 - }, - { - "x": 7.958007564987826, - "y": 5.340774148745389, - "heading": 0.6123949630340744, - "angularVelocity": -0.29089390678188665, - "velocityX": -1.177496379110975, - "velocityY": -1.3757942275957413, - "timestamp": 12.092626412545526 - }, - { - "x": 7.863405980160758, - "y": 5.236063150659359, - "heading": 0.5905148719771925, - "angularVelocity": -0.3254997686468183, - "velocityX": -1.4073430450897342, - "velocityY": -1.5577360059048004, - "timestamp": 12.159846402125988 - }, - { - "x": 7.752513083642703, - "y": 5.120268560189199, - "heading": 0.5661934059220105, - "angularVelocity": -0.36181895009176485, - "velocityX": -1.649701185765849, - "velocityY": -1.7226213689241165, - "timestamp": 12.22706639170645 - }, - { - "x": 7.624272333970784, - "y": 4.995132700896389, - "heading": 0.5392856965961144, - "angularVelocity": -0.40029326832441703, - "velocityX": -1.9077769941992746, - "velocityY": -1.8615870081774413, - "timestamp": 12.294286381286911 - }, - { - "x": 7.477443680694748, - "y": 4.863466852853807, - "heading": 0.5096191616794202, - "angularVelocity": -0.4413350121273609, - "velocityX": -2.1843004468229417, - "velocityY": -1.958730563101009, - "timestamp": 12.361506370867373 - }, - { - "x": 7.310999420897597, - "y": 4.729976120271212, - "heading": 0.47703933517045805, - "angularVelocity": -0.48467467359488975, - "velocityX": -2.4761125497932377, - "velocityY": -1.9858785075056882, - "timestamp": 12.428726360447834 - }, - { - "x": 7.12561513664023, - "y": 4.601908860623506, - "heading": 0.44160618415094194, - "angularVelocity": -0.5271222331432077, - "velocityX": -2.757874337892657, - "velocityY": -1.9051960651438506, - "timestamp": 12.495946350028296 - }, - { - "x": 6.9255695292750366, - "y": 4.487009075034074, - "heading": 0.4039304316786653, - "angularVelocity": -0.5604843545412803, - "velocityX": -2.9759839091575766, - "velocityY": -1.709309779822242, - "timestamp": 12.563166339608758 - }, - { - "x": 6.716882603619491, - "y": 4.389825418108365, - "heading": 0.36497586306620833, - "angularVelocity": -0.5795086975702185, - "velocityX": -3.1045367153136607, - "velocityY": -1.445755310767803, - "timestamp": 12.63038632918922 - }, - { - "x": 6.504347696212581, - "y": 4.311975808393504, - "heading": 0.32552791036504214, - "angularVelocity": -0.5868485393611587, - "velocityX": -3.1617813203096174, - "velocityY": -1.1581318325209748, - "timestamp": 12.697606318769681 - }, - { - "x": 6.29116656289608, - "y": 4.2538292064786, - "heading": 0.2861053737979932, - "angularVelocity": -0.5864704355519208, - "velocityX": -3.1713949175985174, - "velocityY": -0.8650195020530711, - "timestamp": 12.764826308350143 - }, - { - "x": 6.079464519610655, - "y": 4.215340449642915, - "heading": 0.24704786030835676, - "angularVelocity": -0.5810401598305069, - "velocityX": -3.1493911945942843, - "velocityY": -0.572579036026387, - "timestamp": 12.832046297930605 - }, - { - "x": 5.870707606275243, - "y": 4.1963435376359834, - "heading": 0.20858746560478297, - "angularVelocity": -0.5721571059980171, - "velocityX": -3.1055778889333454, - "velocityY": -0.28260807723263415, - "timestamp": 12.899266287511066 + "heading": -0.001904261510303375, + "angularVelocity": 2.3250094273072788e-8, + "velocityX": 3.503395732831278, + "velocityY": -1.265642393108385, + "timestamp": 9.505547963808526 + }, + { + "x": 5.815067432185188, + "y": 4.154195032102827, + "heading": -0.0019042601141906403, + "angularVelocity": 3.3542788088424975e-8, + "velocityX": 3.5826307679474456, + "velocityY": -1.0199942412452387, + "timestamp": 9.547169809045405 + }, + { + "x": 5.966765079106725, + "y": 4.122169062842087, + "heading": -0.0019042587680997514, + "angularVelocity": 3.234097098196836e-8, + "velocityX": 3.6446641435091407, + "velocityY": -0.7694509716826321, + "timestamp": 9.588791654282284 + }, + { + "x": 6.1203162156456985, + "y": 4.100724080023554, + "heading": -0.0019042574433880786, + "angularVelocity": 3.182731724746444e-8, + "velocityX": 3.6891957976654504, + "velocityY": -0.5152338320534735, + "timestamp": 9.630413499519163 + }, + { + "x": 6.274166659063129, + "y": 4.081543884935332, + "heading": -0.001904256120802589, + "angularVelocity": 3.177623389629938e-8, + "velocityX": 3.6963868983183503, + "velocityY": -0.46082039321089135, + "timestamp": 9.672035344756042 + }, + { + "x": 6.428017114313601, + "y": 4.062363784763932, + "heading": -0.0019042547982171636, + "angularVelocity": 3.1776232361520234e-8, + "velocityX": 3.696387182617153, + "velocityY": -0.4608181127540811, + "timestamp": 9.71365718999292 + }, + { + "x": 6.581867569564624, + "y": 4.043183684596952, + "heading": -0.0019042534756317344, + "angularVelocity": 3.177623245008594e-8, + "velocityX": 3.696387182630397, + "velocityY": -0.46081811264784767, + "timestamp": 9.7552790352298 + }, + { + "x": 6.7357180248600645, + "y": 4.024003584786261, + "heading": -0.0019042521530462726, + "angularVelocity": 3.1776233230368237e-8, + "velocityX": 3.6963871836975604, + "velocityY": -0.46081810408775026, + "timestamp": 9.796900880466678 + }, + { + "x": 6.88956959654859, + "y": 4.00483244208887, + "heading": -0.0019042508304574486, + "angularVelocity": 3.1776314012648856e-8, + "velocityX": 3.696414005984728, + "velocityY": -0.4606029018724078, + "timestamp": 9.838522725703557 + }, + { + "x": 7.044365422308842, + "y": 3.996109423647668, + "heading": -0.001904249277450269, + "angularVelocity": 3.7312309699136395e-8, + "velocityX": 3.7191005079010213, + "velocityY": -0.20957788852360654, + "timestamp": 9.880144570940436 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -4.70962468044917e-24, + "angularVelocity": 0.04575119787728785, + "velocityX": 3.664528906453967, + "velocityY": 0.04245896540136021, + "timestamp": 9.921766416177315 + }, + { + "x": 7.4117376833527, + "y": 4.005713687749523, + "heading": 0.0009657064358366508, + "angularVelocity": 0.014669397578232305, + "velocityX": 3.2636086584468615, + "velocityY": 0.11904726359911169, + "timestamp": 9.987597779100469 + }, + { + "x": 7.599748212147598, + "y": 4.014970056100235, + "heading": 0.001284059261189157, + "angularVelocity": 0.004835883858642321, + "velocityX": 2.8559416127289494, + "velocityY": 0.14060727197030365, + "timestamp": 10.053429142023623 + }, + { + "x": 7.760889475419079, + "y": 4.024789101797907, + "heading": 0.0012842515922686665, + "angularVelocity": 0.0000029215721955193207, + "velocityX": 2.447788654468283, + "velocityY": 0.14915452546736505, + "timestamp": 10.119260504946777 + }, + { + "x": 7.8951561065625, + "y": 4.03478916193552, + "heading": 0.0011129009866924844, + "angularVelocity": -0.002602871913440434, + "velocityX": 2.0395541757224747, + "velocityY": 0.15190419419518753, + "timestamp": 10.18509186786993 + }, + { + "x": 8.002547492527386, + "y": 4.044754390856472, + "heading": 0.0008529249221134859, + "angularVelocity": -0.003949121710915689, + "velocityX": 1.6313103845388242, + "velocityY": 0.1513750965870757, + "timestamp": 10.250923230793084 + }, + { + "x": 8.083064163245998, + "y": 4.054546008914513, + "heading": 0.0005576380815616831, + "angularVelocity": -0.004485503982296319, + "velocityX": 1.223074643200086, + "velocityY": 0.1487378906232164, + "timestamp": 10.316754593716238 + }, + { + "x": 8.136706915836411, + "y": 4.064067288015535, + "heading": 0.00026420208220015364, + "angularVelocity": -0.004457389097413355, + "velocityX": 0.8148510103464127, + "velocityY": 0.14463135317639367, + "timestamp": 10.382585956639392 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": 7.663456254811148e-24, + "angularVelocity": -0.004013316305004348, + "velocityX": 0.40663971509805397, + "velocityY": 0.13944216629333933, + "timestamp": 10.448417319562546 + }, + { + "x": 8.158068779301736, + "y": 4.083431553094837, + "heading": -0.0002400975033088844, + "angularVelocity": -0.0031214685775050673, + "velocityX": -0.07030570953345122, + "velocityY": 0.1324082915016235, + "timestamp": 10.52533544234589 + }, + { + "x": 8.115975249648498, + "y": 4.093075117537315, + "heading": -0.0004115957026821067, + "angularVelocity": -0.002229620187901403, + "velocityX": -0.5472511305534997, + "velocityY": 0.12537441234287078, + "timestamp": 10.602253565129233 + }, + { + "x": 8.037195973910658, + "y": 4.10217764875176, + "heading": -0.0005144945686699745, + "angularVelocity": -0.0013377714154270807, + "velocityX": -1.024196546758428, + "velocityY": 0.11834052736940634, + "timestamp": 10.679171687912577 + }, + { + "x": 7.921730952606701, + "y": 4.110739146113785, + "heading": -0.000548794095929675, + "angularVelocity": -0.0004459225734917171, + "velocityX": -1.501141956222563, + "velocityY": 0.11130663427836081, + "timestamp": 10.75608981069592 + }, + { + "x": 7.769580186514303, + "y": 4.11875960869081, + "heading": -0.0005144943077910091, + "angularVelocity": 0.00044592596513670077, + "velocityX": -1.9780873555763094, + "velocityY": 0.10427272906302729, + "timestamp": 10.833007933479264 + }, + { + "x": 7.580743676929442, + "y": 4.126239034937878, + "heading": -0.00041159526627641076, + "angularVelocity": 0.001337773697421547, + "velocityX": -2.455032738081228, + "velocityY": 0.09723880376198106, + "timestamp": 10.909926056262607 + }, + { + "x": 7.35522142644369, + "y": 4.133177421791385, + "heading": -0.00024009710111365826, + "angularVelocity": 0.0022296197431366516, + "velocityX": -2.9319780868935523, + "velocityY": 0.09020483863145769, + "timestamp": 10.986844179045951 + }, + { + "x": 7.093013442829981, + "y": 4.139574760181112, + "heading": -1.4530480095899495e-10, + "angularVelocity": 0.003121461459546291, + "velocityX": -3.4089233346512358, + "velocityY": 0.08317075558053526, + "timestamp": 11.063762301829295 + }, + { + "x": 6.806524506833513, + "y": 4.13534985058908, + "heading": -1.168340521484347e-10, + "angularVelocity": 3.7014357319604335e-10, + "velocityX": -3.7245960461544843, + "velocityY": -0.05492736222818408, + "timestamp": 11.140680424612638 + }, + { + "x": 6.520035572895023, + "y": 4.131124801447752, + "heading": -8.836340568265332e-11, + "angularVelocity": 3.701422426282247e-10, + "velocityX": -3.7245960193990517, + "velocityY": -0.054929176485862014, + "timestamp": 11.217598547395982 + }, + { + "x": 6.233546635992582, + "y": 4.126899953288603, + "heading": -5.989279962924638e-11, + "angularVelocity": 3.7014171723351935e-10, + "velocityX": -3.724596057932901, + "velocityY": -0.05492656354927405, + "timestamp": 11.294516670179325 + }, + { + "x": 5.947511708921411, + "y": 4.143566708826361, + "heading": -3.09908077767978e-11, + "angularVelocity": 3.7575009382191746e-10, + "velocityX": -3.7186935499823517, + "velocityY": 0.21668177712428321, + "timestamp": 11.371434792962669 }, { "x": 5.665951728820801, "y": 4.196649074554443, - "heading": 0.17089065340850093, - "angularVelocity": -0.560797650097213, - "velocityX": -3.0460563700229573, - "velocityY": 0.004545328262717531, - "timestamp": 12.966486277091528 - }, - { - "x": 5.441376331690843, - "y": 4.221257817184443, - "heading": 0.12955296713969652, - "angularVelocity": -0.5459093121032401, - "velocityX": -2.965763486744691, - "velocityY": 0.3249853335649047, - "timestamp": 13.042208902356046 - }, - { - "x": 5.2258290849692735, - "y": 4.2691932604024965, - "heading": 0.08990916431610245, - "angularVelocity": -0.5235397304980968, - "velocityX": -2.8465368965828737, - "velocityY": 0.6330399012263854, - "timestamp": 13.117931527620565 - }, - { - "x": 5.023235975053, - "y": 4.338521550335442, - "heading": 0.05269444989052449, - "angularVelocity": -0.4914609642174678, - "velocityX": -2.6754633665772425, - "velocityY": 0.9155558155936205, - "timestamp": 13.193654152885083 - }, - { - "x": 4.838427025225484, - "y": 4.425425000911894, - "heading": 0.018769229111462726, - "angularVelocity": -0.44801960656478235, - "velocityX": -2.440604101903904, - "velocityY": 1.1476550142427961, - "timestamp": 13.269376778149601 - }, - { - "x": 4.676043362291933, - "y": 4.523369450086871, - "heading": -0.011096289897494078, - "angularVelocity": -0.3944068091224933, - "velocityX": -2.144453686943697, - "velocityY": 1.2934634639624794, - "timestamp": 13.34509940341412 - }, - { - "x": 4.53850801944686, - "y": 4.62398511499902, - "heading": -0.03651765926491392, - "angularVelocity": -0.33571695749608116, - "velocityX": -1.8163044712809187, - "velocityY": 1.3287397863013088, - "timestamp": 13.420822028678637 - }, - { - "x": 4.425480168441604, - "y": 4.719703555950957, - "heading": -0.05754144603227514, - "angularVelocity": -0.2776420745308276, - "velocityX": -1.4926562650254422, - "velocityY": 1.2640665932747182, - "timestamp": 13.496544653943156 - }, - { - "x": 4.335238129667097, - "y": 4.805147915212772, - "heading": -0.07443003472477187, - "angularVelocity": -0.22303226589808134, - "velocityX": -1.1917447190884998, - "velocityY": 1.128386119252163, - "timestamp": 13.572267279207674 - }, - { - "x": 4.265861114202726, - "y": 4.876824239203163, - "heading": -0.08748354859792261, - "angularVelocity": -0.17238591276453477, - "velocityX": -0.9161992894728712, - "velocityY": 0.9465641707482745, - "timestamp": 13.647989904472192 - }, - { - "x": 4.215660401957236, - "y": 4.932459766827933, - "heading": -0.09697232555255134, - "angularVelocity": -0.1253096669784238, - "velocityX": -0.6629552537319688, - "velocityY": 0.7347279288115158, - "timestamp": 13.72371252973671 - }, - { - "x": 4.183247912596747, - "y": 4.970524954667524, - "heading": -0.10312305256683521, - "angularVelocity": -0.08122707041386726, - "velocityX": -0.4280423353953174, - "velocityY": 0.5026923948637503, - "timestamp": 13.799435155001229 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -0.03960437745903177, - "velocityX": -0.2079807341935533, - "velocityY": 0.25649983915041824, - "timestamp": 13.875157780265747 - }, - { - "x": 4.16749906539917, - "y": 4.98994779586792, - "heading": -0.106122, - "angularVelocity": -1.1491916765427864e-28, - "velocityX": -1.6722970053411976e-26, - "velocityY": -1.611730320150929e-26, - "timestamp": 13.950880405530265 + "heading": -1.0560376042139496e-24, + "angularVelocity": 4.029064498114124e-10, + "velocityX": -3.6605154924761147, + "velocityY": 0.6901152005178285, + "timestamp": 11.448352915746012 + }, + { + "x": 5.429536630692184, + "y": 4.268976584999924, + "heading": -2.8551532586117325e-9, + "angularVelocity": -4.3018194916474803e-8, + "velocityX": -3.5620332259990137, + "velocityY": 1.0897484864542637, + "timestamp": 11.514723744744192 + }, + { + "x": 5.202544196069625, + "y": 4.366944958856273, + "heading": -5.536673846717203e-9, + "angularVelocity": -4.040209574870605e-8, + "velocityX": -3.4200632725076114, + "velocityY": 1.4760757901492323, + "timestamp": 11.581094573742371 + }, + { + "x": 4.987571660053834, + "y": 4.485916904335734, + "heading": -0.004584961527914818, + "angularVelocity": -0.0690808908137442, + "velocityX": -3.2389611409205945, + "velocityY": 1.7925336669024117, + "timestamp": 11.64746540274055 + }, + { + "x": 4.796407595296518, + "y": 4.591709425707398, + "heading": -0.01882928955824306, + "angularVelocity": -0.21461729867377147, + "velocityX": -2.8802422335655495, + "velocityY": 1.5939611267258074, + "timestamp": 11.71383623173873 + }, + { + "x": 4.629141679517556, + "y": 4.68427797767877, + "heading": -0.035106225739037614, + "angularVelocity": -0.24524232146084635, + "velocityX": -2.5201721645445794, + "velocityY": 1.394717428855822, + "timestamp": 11.78020706073691 + }, + { + "x": 4.485777671005229, + "y": 4.763620051213405, + "heading": -0.05103365270171862, + "angularVelocity": -0.23997631494278487, + "velocityX": -2.160045469919592, + "velocityY": 1.195435927684602, + "timestamp": 11.846577889735089 + }, + { + "x": 4.366313170637034, + "y": 4.829736321202412, + "heading": -0.06545153313706903, + "angularVelocity": -0.21723218849271467, + "velocityX": -1.799954922537895, + "velocityY": 0.9961645950033218, + "timestamp": 11.912948718733269 + }, + { + "x": 4.270745458949128, + "y": 4.88262778520947, + "heading": -0.0776743935119962, + "angularVelocity": -0.1841601281680891, + "velocityX": -1.4399053489376459, + "velocityY": 0.7969082924745271, + "timestamp": 11.979319547731448 + }, + { + "x": 4.199072211550319, + "y": 4.922295352851354, + "heading": -0.08724968344217902, + "angularVelocity": -0.1442695544822162, + "velocityX": -1.0798907966145075, + "velocityY": 0.5976656950144733, + "timestamp": 12.045690376729628 + }, + { + "x": 4.151291522892566, + "y": 4.948739793023477, + "heading": -0.0938560173878007, + "angularVelocity": -0.0995367098066956, + "velocityX": -0.7199049549172076, + "velocityY": 0.39843468239409807, + "timestamp": 12.112061205727807 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.051179983074939046, + "velocityX": -0.3599426809532897, + "velocityY": 0.19921331994672406, + "timestamp": 12.178432034725986 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.064087865436599e-25, + "velocityX": -5.747994530578863e-24, + "velocityY": -1.0853274710472143e-23, + "timestamp": 12.244802863724166 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/CenterLanePCBAFG.1.traj b/src/main/deploy/choreo/CenterLanePCBAFG.1.traj new file mode 100644 index 00000000..3738b150 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFG.1.traj @@ -0,0 +1,275 @@ +{ + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -1.120688542766024e-26, + "angularVelocity": 1.2570906532337121e-26, + "velocityX": 3.544719035734486e-27, + "velocityY": -1.5437798405468174e-26, + "timestamp": 0 + }, + { + "x": 1.3104780876502795, + "y": 5.567275466796953, + "heading": -0.013632783425389957, + "angularVelocity": -0.19128878469611282, + "velocityX": 0.28769735437387517, + "velocityY": -0.33225026766694243, + "timestamp": 0.07126807484844137 + }, + { + "x": 1.3514855787219209, + "y": 5.519918104138928, + "heading": -0.04088178026931428, + "angularVelocity": -0.3823450668742212, + "velocityX": 0.575397766234711, + "velocityY": -0.6644961682876167, + "timestamp": 0.14253614969688275 + }, + { + "x": 1.4129976098576797, + "y": 5.448882530124723, + "heading": -0.08170955131958318, + "angularVelocity": -0.5728760196917512, + "velocityX": 0.8631077977982459, + "velocityY": -0.9967376579943998, + "timestamp": 0.2138042245453241 + }, + { + "x": 1.4950153477240138, + "y": 5.354169209270188, + "heading": -0.13606323380780802, + "angularVelocity": -0.7626652270853859, + "velocityX": 1.1508341994750528, + "velocityY": -1.3289726298339328, + "timestamp": 0.2850722993937655 + }, + { + "x": 1.5975404069408672, + "y": 5.235778918310646, + "heading": -0.20388243920312946, + "angularVelocity": -0.9516070911070027, + "velocityX": 1.4385832567370889, + "velocityY": -1.6611967028899048, + "timestamp": 0.3563403742422069 + }, + { + "x": 1.7205747481570635, + "y": 5.093712881376144, + "heading": -0.28510896229616783, + "angularVelocity": -1.139732247093452, + "velocityX": 1.7263598248983312, + "velocityY": -1.9934035995306334, + "timestamp": 0.42760844909064827 + }, + { + "x": 1.8641205194491128, + "y": 4.927972835971274, + "heading": -0.37969642809760956, + "angularVelocity": -1.3272066911108702, + "velocityX": 2.0141665338556387, + "velocityY": -2.32558611632674, + "timestamp": 0.49887652393908966 + }, + { + "x": 2.036056146597576, + "y": 4.7662140643499225, + "heading": -0.4690836216877677, + "angularVelocity": -1.2542389250761836, + "velocityX": 2.4125195961038894, + "velocityY": -2.2697227610728588, + "timestamp": 0.570144598787531 + }, + { + "x": 2.187477556502736, + "y": 4.628126287278482, + "heading": -0.5451441122574506, + "angularVelocity": -1.0672449162045279, + "velocityX": 2.1246737789279755, + "velocityY": -1.9375825341865685, + "timestamp": 0.6414126736359723 + }, + { + "x": 2.318383785639942, + "y": 4.513708351772966, + "heading": -0.607881175347142, + "angularVelocity": -0.8802968681714505, + "velocityX": 1.8368144420287889, + "velocityY": -1.6054584854275527, + "timestamp": 0.7126807484844137 + }, + { + "x": 2.428774184877684, + "y": 4.4229594060003725, + "heading": -0.6572940712821624, + "angularVelocity": -0.6933384413722671, + "velocityX": 1.548945996822536, + "velocityY": -1.2733463891873111, + "timestamp": 0.783948823332855 + }, + { + "x": 2.518648327310084, + "y": 4.355878850846356, + "heading": -0.6933805665693583, + "angularVelocity": -0.5063486752509772, + "velocityX": 1.2610715614744235, + "velocityY": -0.9412426994368749, + "timestamp": 0.8552168981812963 + }, + { + "x": 2.5880059699725453, + "y": 4.312466305335323, + "heading": -0.7161380105199752, + "angularVelocity": -0.31932171591575625, + "velocityX": 0.9731937169617235, + "velocityY": -0.6091443553562279, + "timestamp": 0.9264849730297376 + }, + { + "x": 2.636847038659, + "y": 4.292721584024701, + "heading": -0.7255637248085023, + "angularVelocity": -0.13225717558067804, + "velocityX": 0.6853148312244995, + "velocityY": -0.2770486133182406, + "timestamp": 0.997753047878179 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.054843767292669265, + "velocityX": 0.39743720636786783, + "velocityY": 0.055047139076356964, + "timestamp": 1.0690211227266204 + }, + { + "x": 2.6726523875585073, + "y": 4.325520941215623, + "heading": -0.7036434191878628, + "angularVelocity": 0.24661858695360772, + "velocityX": 0.10242764131881017, + "velocityY": 0.3953775861253462, + "timestamp": 1.1420557472889747 + }, + { + "x": 2.658587549089958, + "y": 4.379253724773467, + "heading": -0.6716319783821333, + "angularVelocity": 0.43830499571335924, + "velocityX": -0.19257767877674353, + "velocityY": 0.7357165711445571, + "timestamp": 1.215090371851329 + }, + { + "x": 2.622977623032788, + "y": 4.4578439357274, + "heading": -0.6256269314489635, + "angularVelocity": 0.6299073515999585, + "velocityX": -0.48757594456814685, + "velocityY": 1.0760678435039392, + "timestamp": 1.2881249964136834 + }, + { + "x": 2.5658233842938287, + "y": 4.561292786214616, + "heading": -0.5656322001603995, + "angularVelocity": 0.8214560100509974, + "velocityX": -0.782563600229962, + "velocityY": 1.4164357126104716, + "timestamp": 1.3611596209760377 + }, + { + "x": 2.4871259686686225, + "y": 4.689601823386252, + "heading": -0.49164499426759223, + "angularVelocity": 1.0130428729682832, + "velocityX": -1.0775357044249236, + "velocityY": 1.7568247655204965, + "timestamp": 1.434194245538392 + }, + { + "x": 2.3868870212617113, + "y": 4.842772877881088, + "heading": -0.40364701353076443, + "angularVelocity": 1.204880305254371, + "velocityX": -1.3724852836250385, + "velocityY": 2.0972388837853764, + "timestamp": 1.5072288701007464 + }, + { + "x": 2.26510884712338, + "y": 5.020807849872121, + "heading": -0.3015898760322786, + "angularVelocity": 1.397380189328601, + "velocityX": -1.6674033017635614, + "velocityY": 2.437679019477028, + "timestamp": 1.5802634946631007 + }, + { + "x": 2.1359147758136907, + "y": 5.1700050636509935, + "heading": -0.2156034423927932, + "angularVelocity": 1.1773379291636314, + "velocityX": -1.7689427731553284, + "velocityY": 2.0428285169247893, + "timestamp": 1.653298119225455 + }, + { + "x": 2.0282555085515424, + "y": 5.294336258780153, + "heading": -0.1438206059999422, + "angularVelocity": 0.9828603463493605, + "velocityX": -1.4740853110052332, + "velocityY": 1.7023596119537827, + "timestamp": 1.7263327437878093 + }, + { + "x": 1.9421294204612929, + "y": 5.393801267693995, + "heading": -0.08631841339014902, + "angularVelocity": 0.7873278318929399, + "velocityX": -1.1792500968731383, + "velocityY": 1.361888412651758, + "timestamp": 1.7993673683501636 + }, + { + "x": 1.8775353609204488, + "y": 5.468400102933928, + "heading": -0.043158758281562314, + "angularVelocity": 0.5909478602404344, + "velocityX": -0.8844306372205144, + "velocityY": 1.0214173850684016, + "timestamp": 1.872401992912518 + }, + { + "x": 1.8344726769226236, + "y": 5.518132795390135, + "heading": -0.014381433415882237, + "angularVelocity": 0.3940230409634143, + "velocityX": -0.5896201186200382, + "velocityY": 0.6809467804376429, + "timestamp": 1.9454366174748723 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 3.4376441721489434e-26, + "angularVelocity": 0.1969125397995838, + "velocityX": -0.2948119945987197, + "velocityY": 0.34047511487869625, + "timestamp": 2.0184712420372266 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.5116489541153283e-26, + "angularVelocity": -4.402301586321663e-26, + "velocityX": -5.546242066692014e-22, + "velocityY": -8.311642516359436e-25, + "timestamp": 2.0915058665995807 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFG.2.traj b/src/main/deploy/choreo/CenterLanePCBAFG.2.traj new file mode 100644 index 00000000..fd70a0fb --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFG.2.traj @@ -0,0 +1,86 @@ +{ + "samples": [ + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.5116489541153283e-26, + "angularVelocity": -4.402301586321663e-26, + "velocityX": -5.546242066692014e-22, + "velocityY": -8.311642516359436e-25, + "timestamp": 0 + }, + { + "x": 1.8720264440667018, + "y": 5.543082773688388, + "heading": 5.813054707764239e-20, + "angularVelocity": 5.955362224136031e-19, + "velocityX": 0.6053171317615015, + "velocityY": 0.0008555041864665229, + "timestamp": 0.09761040516750885 + }, + { + "x": 1.9901969439319853, + "y": 5.543249785907351, + "heading": 8.928720004263094e-20, + "angularVelocity": 3.1919397232381416e-19, + "velocityX": 1.2106342521833766, + "velocityY": 0.0017110083569065741, + "timestamp": 0.1952208103350177 + }, + { + "x": 2.1674526905937914, + "y": 5.543500304231363, + "heading": 9.074204012627455e-20, + "angularVelocity": 1.4904559423917405e-20, + "velocityX": 1.8159513461461239, + "velocityY": 0.0025665124899515255, + "timestamp": 0.29283121550252655 + }, + { + "x": 2.403793671138691, + "y": 5.5438343286421725, + "heading": 4.918611066370257e-20, + "angularVelocity": -4.2573257831162476e-19, + "velocityX": 2.4212683078132486, + "velocityY": 0.0034220164360210005, + "timestamp": 0.3904416206700354 + }, + { + "x": 2.5810494178004975, + "y": 5.544084846966184, + "heading": 1.0185426849288302e-20, + "angularVelocity": -3.9955457358178204e-19, + "velocityX": 1.8159513461461239, + "velocityY": 0.0025665124899515255, + "timestamp": 0.48805202583754426 + }, + { + "x": 2.699219917665781, + "y": 5.544251859185147, + "heading": 2.0104392985221674e-20, + "angularVelocity": 1.0161791787953617e-19, + "velocityX": 1.2106342521833766, + "velocityY": 0.001711008356906574, + "timestamp": 0.5856624310050531 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.4565183072234727e-28, + "angularVelocity": -2.0596567646457299e-19, + "velocityX": 0.6053171317615015, + "velocityY": 0.0008555041864665228, + "timestamp": 0.683272836172562 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.5120860158326505e-28, + "angularVelocity": -5.112250671135627e-29, + "velocityX": 5.545899463618144e-22, + "velocityY": 7.793513704496499e-25, + "timestamp": 0.7808832413400708 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFG.3.traj b/src/main/deploy/choreo/CenterLanePCBAFG.3.traj new file mode 100644 index 00000000..9a67615a --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFG.3.traj @@ -0,0 +1,176 @@ +{ + "samples": [ + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.5120860158326505e-28, + "angularVelocity": -5.112250671135627e-29, + "velocityX": 5.545899463618144e-22, + "velocityY": 7.793513704496499e-25, + "timestamp": 0 + }, + { + "x": 2.7303741241412878, + "y": 5.568056862351408, + "heading": 0.006778500636213844, + "angularVelocity": 0.08813486156991067, + "velocityX": -0.36316271539799666, + "velocityY": 0.30842969138218984, + "timestamp": 0.07691054953137888 + }, + { + "x": 2.6747561237908846, + "y": 5.615779928168, + "heading": 0.020597377597861958, + "angularVelocity": 0.17967466161466059, + "velocityX": -0.7231517742271671, + "velocityY": 0.6205009079686912, + "timestamp": 0.15382109906275776 + }, + { + "x": 2.591861580665438, + "y": 5.687962474226551, + "heading": 0.04189403274126234, + "angularVelocity": 0.2769016119786207, + "velocityX": -1.0778045876739724, + "velocityY": 0.9385259434286201, + "timestamp": 0.23073164859413664 + }, + { + "x": 2.4825228427377626, + "y": 5.785486601053793, + "heading": 0.07152934014357441, + "angularVelocity": 0.38532174822416343, + "velocityX": -1.4216351149989688, + "velocityY": 1.2680201535610187, + "timestamp": 0.3076421981255155 + }, + { + "x": 2.3492929926954265, + "y": 5.910724214608569, + "heading": 0.11194451008519196, + "angularVelocity": 0.5254827873142219, + "velocityX": -1.7322701612992524, + "velocityY": 1.6283541636076828, + "timestamp": 0.3845527476568944 + }, + { + "x": 2.2322885923008555, + "y": 6.0682629894504885, + "heading": 0.178093318543464, + "angularVelocity": 0.8600745783422568, + "velocityX": -1.5213049589098766, + "velocityY": 2.04833765721106, + "timestamp": 0.4614632971882733 + }, + { + "x": 2.148304694530927, + "y": 6.209825325348869, + "heading": 0.2439233082099427, + "angularVelocity": 0.8559292589584284, + "velocityX": -1.091968504732421, + "velocityY": 1.840610121250321, + "timestamp": 0.5383738467196522 + }, + { + "x": 2.0954355209198954, + "y": 6.331973650947481, + "heading": 0.3066258894331499, + "angularVelocity": 0.8152663269897058, + "velocityX": -0.6874112060460749, + "velocityY": 1.5881868786905071, + "timestamp": 0.615284396251031 + }, + { + "x": 2.072969838348887, + "y": 6.433621733055282, + "heading": 0.3652444017006222, + "angularVelocity": 0.7621647826551591, + "velocityX": -0.2921014439227605, + "velocityY": 1.321640304576541, + "timestamp": 0.6921949457824099 + }, + { + "x": 2.080539684915293, + "y": 6.514239840456833, + "heading": 0.4192892074806503, + "angularVelocity": 0.7026969136136281, + "velocityX": 0.09842403431687198, + "velocityY": 1.048206102969775, + "timestamp": 0.7691054953137888 + }, + { + "x": 2.1179206171311256, + "y": 6.5735146979492365, + "heading": 0.46845941042093314, + "angularVelocity": 0.6393167548519696, + "velocityX": 0.4860312719594013, + "velocityY": 0.7706986603732364, + "timestamp": 0.8460160448451677 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.5732764808952261, + "velocityX": 0.8716741807939862, + "velocityY": 0.490501440559152, + "timestamp": 0.9229265943765466 + }, + { + "x": 2.2917885277794023, + "y": 6.672276624724165, + "heading": 0.5461832199677008, + "angularVelocity": 0.41150844936023273, + "velocityX": 1.3070633564521403, + "velocityY": 0.7468102485181382, + "timestamp": 1.0046571163621567 + }, + { + "x": 2.4341166113358135, + "y": 6.754285363860808, + "heading": 0.5638407865548152, + "angularVelocity": 0.21604617416028918, + "velocityX": 1.7414312315473808, + "velocityY": 1.0034040789691927, + "timestamp": 1.0863876383477669 + }, + { + "x": 2.5412609256604743, + "y": 6.8169454314474, + "heading": 0.5395666812512139, + "angularVelocity": -0.29700171629731165, + "velocityX": 1.3109461645616998, + "velocityY": 0.7666666756101825, + "timestamp": 1.168118160333377 + }, + { + "x": 2.6126555907805695, + "y": 6.858741825974232, + "heading": 0.5217952234534815, + "angularVelocity": -0.21743967083510496, + "velocityX": 0.8735373687282387, + "velocityY": 0.5113927271159635, + "timestamp": 1.2498486823189867 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.11311323638810476, + "velocityX": 0.4366658269950189, + "velocityY": 0.2557556745753436, + "timestamp": 1.3315792043045964 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 6.389972915056163e-27, + "velocityX": -1.0690163263683836e-25, + "velocityY": -1.1312672794497157e-25, + "timestamp": 1.4133097262902061 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFG.4.traj b/src/main/deploy/choreo/CenterLanePCBAFG.4.traj new file mode 100644 index 00000000..7fd42c1a --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFG.4.traj @@ -0,0 +1,644 @@ +{ + "samples": [ + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 6.389972915056163e-27, + "velocityX": -1.0690163263683836e-25, + "velocityY": -1.1312672794497157e-25, + "timestamp": 0 + }, + { + "x": 2.660849775777501, + "y": 6.863303095829213, + "heading": 0.49944853016087754, + "angularVelocity": -0.22618164286906187, + "velocityX": 0.21588184235159683, + "velocityY": -0.2821127073294775, + "timestamp": 0.057926404958988265 + }, + { + "x": 2.6858699737019993, + "y": 6.830605675150758, + "heading": 0.4737478272074287, + "angularVelocity": -0.4436785430002931, + "velocityX": 0.43193079118602823, + "velocityY": -0.5644648705819807, + "timestamp": 0.11585280991797653 + }, + { + "x": 2.7234170479654534, + "y": 6.7815357680679345, + "heading": 0.4360912802839983, + "angularVelocity": -0.6500756770611135, + "velocityX": 0.6481858194037288, + "velocityY": -0.847107758846162, + "timestamp": 0.1737792148769648 + }, + { + "x": 2.773506189570542, + "y": 6.716073180513463, + "heading": 0.38731316065986704, + "angularVelocity": -0.8420705489779003, + "velocityX": 0.864703094220184, + "velocityY": -1.1300992630359112, + "timestamp": 0.23170561983595306 + }, + { + "x": 2.8361569303832073, + "y": 6.63419375338078, + "heading": 0.3285222984900142, + "angularVelocity": -1.0149233706368752, + "velocityX": 1.0815575531922228, + "velocityY": -1.4135078327518056, + "timestamp": 0.2896320247949413 + }, + { + "x": 2.9113941307930915, + "y": 6.535868410078246, + "heading": 0.2612558665356734, + "angularVelocity": -1.1612395418283843, + "velocityX": 1.2988411841396363, + "velocityY": -1.6974183599370378, + "timestamp": 0.3475584297539296 + }, + { + "x": 2.99924805742508, + "y": 6.421062364749842, + "heading": 0.187813414307632, + "angularVelocity": -1.2678579359454234, + "velocityX": 1.5166473164386636, + "velocityY": -1.9819294052459604, + "timestamp": 0.40548483471291785 + }, + { + "x": 3.0997480806917785, + "y": 6.289740322129515, + "heading": 0.11213231017946661, + "angularVelocity": -1.306504420250965, + "velocityX": 1.734960478521888, + "velocityY": -2.26704976276885, + "timestamp": 0.4634112396719061 + }, + { + "x": 3.2128556613445705, + "y": 6.14193527325681, + "heading": 0.04279587609271538, + "angularVelocity": -1.1969745772388503, + "velocityX": 1.952608326597726, + "velocityY": -2.551600586595203, + "timestamp": 0.5213376446308944 + }, + { + "x": 3.336513792781727, + "y": 5.978631812899107, + "heading": 0.010716004519218999, + "angularVelocity": -0.5538039447849203, + "velocityX": 2.134745484804496, + "velocityY": -2.8191540709857907, + "timestamp": 0.5792640495898826 + }, + { + "x": 3.4677695577152434, + "y": 5.807368362842113, + "heading": 0.010715992273577517, + "angularVelocity": -2.1139999092239637e-7, + "velocityX": 2.265905592215594, + "velocityY": -2.956569636562943, + "timestamp": 0.6371904545488709 + }, + { + "x": 3.5990253485117636, + "y": 5.636104932606414, + "heading": 0.01071598002800544, + "angularVelocity": -2.11398792746924e-7, + "velocityX": 2.265906038695981, + "velocityY": -2.9565692943823, + "timestamp": 0.6951168595078592 + }, + { + "x": 3.730281139327178, + "y": 5.464841502385196, + "heading": 0.010715967782433288, + "angularVelocity": -2.113987940232789e-7, + "velocityX": 2.2659060390221635, + "velocityY": -2.956569294132315, + "timestamp": 0.7530432644668474 + }, + { + "x": 3.8615381491076497, + "y": 5.293579006382116, + "heading": 0.010715955536865166, + "angularVelocity": -2.1139872447064547e-7, + "velocityX": 2.2659270823625586, + "velocityY": -2.9565531664589524, + "timestamp": 0.8109696694258357 + }, + { + "x": 4.007189928421941, + "y": 5.1343784904281335, + "heading": 0.010715943225165272, + "angularVelocity": -2.1254037607347718e-7, + "velocityX": 2.5144280819328126, + "velocityY": -2.748323775084899, + "timestamp": 0.868896074384824 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.010715930460413511, + "angularVelocity": -2.203615392689942e-7, + "velocityX": 2.7674622150421295, + "velocityY": -2.4933481486115037, + "timestamp": 0.9268224793438122 + }, + { + "x": 4.289285126852667, + "y": 4.894606292568033, + "heading": 0.010715918114469883, + "angularVelocity": -2.9733998228105175e-7, + "velocityX": 2.933098226238877, + "velocityY": -2.2962068965718774, + "timestamp": 0.9683437827778478 + }, + { + "x": 4.417366677446148, + "y": 4.807905868510879, + "heading": 0.010715906356446534, + "angularVelocity": -2.831804971331414e-7, + "velocityX": 3.0847189274046736, + "velocityY": -2.088094950942351, + "timestamp": 1.0098650862118834 + }, + { + "x": 4.551131631929394, + "y": 4.730260623000039, + "heading": 0.010715894919665146, + "angularVelocity": -2.7544369861896277e-7, + "velocityX": 3.221598153722609, + "velocityY": -1.8700098284292712, + "timestamp": 1.051386389645919 + }, + { + "x": 4.688895654984909, + "y": 4.659954711675372, + "heading": 0.010715883576398903, + "angularVelocity": -2.7319147770282054e-7, + "velocityX": 3.3179118106053243, + "velocityY": -1.6932491398387937, + "timestamp": 1.0929076930799546 + }, + { + "x": 4.826660072114789, + "y": 4.5896495725424655, + "heading": 0.010715872233137535, + "angularVelocity": -2.7319136037670126e-7, + "velocityX": 3.31792130150117, + "velocityY": -1.6932305423551623, + "timestamp": 1.1344289965139902 + }, + { + "x": 4.964424489260567, + "y": 4.51934443344071, + "heading": 0.010715860889876156, + "angularVelocity": -2.731913605852805e-7, + "velocityX": 3.3179213018840272, + "velocityY": -1.6932305416049456, + "timestamp": 1.1759502999480258 + }, + { + "x": 5.102188906406363, + "y": 4.449039294338991, + "heading": 0.01071584954661479, + "angularVelocity": -2.731913603225307e-7, + "velocityX": 3.3179213018844735, + "velocityY": -1.6932305416040707, + "timestamp": 1.2174716033820614 + }, + { + "x": 5.239953323697088, + "y": 4.378734155521264, + "heading": 0.0107158382033534, + "angularVelocity": -2.7319136082771354e-7, + "velocityX": 3.3179213053749512, + "velocityY": -1.6932305347644059, + "timestamp": 1.258992906816097 + }, + { + "x": 5.377721333912445, + "y": 4.308436057560326, + "heading": 0.01071582686006505, + "angularVelocity": -2.731920101868387e-7, + "velocityX": 3.3180078374520967, + "velocityY": -1.6930609626121105, + "timestamp": 1.3005142102501326 + }, + { + "x": 5.519903612293123, + "y": 4.247558747282141, + "heading": 0.01071581542197781, + "angularVelocity": -2.7547514875541596e-7, + "velocityX": 3.4243211706145282, + "velocityY": -1.4661705014848305, + "timestamp": 1.3420355136841682 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.010715803649590386, + "angularVelocity": -2.8352644184424e-7, + "velocityX": 3.517426102957061, + "velocityY": -1.2261096959197675, + "timestamp": 1.3835568171182038 + }, + { + "x": 5.815511009397966, + "y": 4.155881926930556, + "heading": 0.010715791923418018, + "angularVelocity": -2.8177750483647635e-7, + "velocityX": 3.59387868290116, + "velocityY": -0.9796261538753469, + "timestamp": 1.4251718224573473 + }, + { + "x": 5.967533992937965, + "y": 4.125567717701148, + "heading": 0.0107157805290767, + "angularVelocity": -2.738036730972178e-7, + "velocityX": 3.653080957243209, + "velocityY": -0.7284441989700993, + "timestamp": 1.4667868277964908 + }, + { + "x": 6.121241175717782, + "y": 4.105466951776193, + "heading": 0.010715769236027265, + "angularVelocity": -2.713696500137211e-7, + "velocityX": 3.6935519178040117, + "velocityY": -0.4830172617098897, + "timestamp": 1.5084018331356344 + }, + { + "x": 6.274949114201427, + "y": 4.0853719654365035, + "heading": 0.010715757943004396, + "angularVelocity": -2.713690116930231e-7, + "velocityX": 3.693570077211222, + "velocityY": -0.4828783794673188, + "timestamp": 1.550016838474778 + }, + { + "x": 6.428657052715288, + "y": 4.065276979327946, + "heading": 0.010715746649981548, + "angularVelocity": -2.7136901112829927e-7, + "velocityX": 3.6935700779373306, + "velocityY": -0.48287837391326544, + "timestamp": 1.5916318438139214 + }, + { + "x": 6.582364991229262, + "y": 4.045181993220238, + "heading": 0.010715735356958701, + "angularVelocity": -2.71369011116775e-7, + "velocityX": 3.69357007794, + "velocityY": -0.48287837389284793, + "timestamp": 1.633246849153065 + }, + { + "x": 6.73607293245351, + "y": 4.025087027843618, + "heading": 0.010715724063935807, + "angularVelocity": -2.713690122742687e-7, + "velocityX": 3.693570143067337, + "velocityY": -0.4828778757290756, + "timestamp": 1.6748618544922085 + }, + { + "x": 6.889847609682978, + "y": 4.005509298883675, + "heading": 0.010715712767818455, + "angularVelocity": -2.714433714426062e-7, + "velocityX": 3.695173795516192, + "velocityY": -0.47044879125674055, + "timestamp": 1.716476859831352 + }, + { + "x": 7.044608530639083, + "y": 3.9966213295297166, + "heading": 0.010715699615645665, + "angularVelocity": -3.1604400106558483e-7, + "velocityX": 3.7188730289681042, + "velocityY": -0.21357607145609414, + "timestamp": 1.7580918651704955 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 4.739357818257744e-26, + "angularVelocity": -0.2574960528856741, + "velocityX": 3.6592893702458946, + "velocityY": 0.030164951189468058, + "timestamp": 1.799706870509639 + }, + { + "x": 7.411732273884802, + "y": 4.005484253819352, + "heading": -0.0035958440097681034, + "angularVelocity": -0.05461699297217377, + "velocityX": 3.263224331051482, + "velocityY": 0.11555138753593294, + "timestamp": 1.8655443290383449 + }, + { + "x": 7.599754753311007, + "y": 4.014664501087166, + "heading": -0.0036927014054620503, + "angularVelocity": -0.0014711593955546794, + "velocityX": 2.855858710649135, + "velocityY": 0.13943805658615363, + "timestamp": 1.9313817875670507 + }, + { + "x": 7.7609064467329985, + "y": 4.024479661032519, + "heading": -0.0024912592269265435, + "angularVelocity": 0.018248611130875694, + "velocityX": 2.4477204470419824, + "velocityY": 0.14908169550731518, + "timestamp": 1.9972192460957565 + }, + { + "x": 7.895178919331714, + "y": 4.034515490111863, + "heading": -0.0009603004800075886, + "angularVelocity": 0.023253612474294444, + "velocityX": 2.039454067628866, + "velocityY": 0.15243342169669197, + "timestamp": 2.0630567046244623 + }, + { + "x": 8.002571195010699, + "y": 4.044538744835798, + "heading": 0.0003549668988073664, + "angularVelocity": 0.01997749318105112, + "velocityX": 1.6311728623631503, + "velocityY": 0.1522424307974114, + "timestamp": 2.128894163153168 + }, + { + "x": 8.083084077776899, + "y": 4.054399862715161, + "heading": 0.001104922913523553, + "angularVelocity": 0.011391023157268422, + "velocityX": 1.2229038690959988, + "velocityY": 0.1497797469667492, + "timestamp": 2.194731621681874 + }, + { + "x": 8.136718786187338, + "y": 4.063994789685242, + "heading": 0.0010462868235224848, + "angularVelocity": -0.0008906189775764543, + "velocityX": 0.814653384395943, + "velocityY": 0.1457365940985966, + "timestamp": 2.26056908021058 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -6.959895417141788e-26, + "angularVelocity": -0.015891968598184786, + "velocityX": 0.40642176825516096, + "velocityY": 0.14053042740563873, + "timestamp": 2.3264065387392856 + }, + { + "x": 8.158055086430046, + "y": 4.083508099899822, + "heading": -0.0027577804882154954, + "angularVelocity": -0.035854270657099625, + "velocityX": -0.07048533094049422, + "velocityY": 0.13340649729451912, + "timestamp": 2.4033229126516193 + }, + { + "x": 8.11595164588489, + "y": 4.093221294829269, + "heading": -0.007050996192854494, + "angularVelocity": -0.05581666797673314, + "velocityX": -0.5473924263921114, + "velocityY": 0.12628253823454794, + "timestamp": 2.480239286563953 + }, + { + "x": 8.037166241278735, + "y": 4.102386537647484, + "heading": -0.012879673428225855, + "angularVelocity": -0.07577940741219352, + "velocityX": -1.024299516458623, + "velocityY": 0.11915854000945891, + "timestamp": 2.557155660476287 + }, + { + "x": 7.921698873219381, + "y": 4.111003824490319, + "heading": -0.02024386647880979, + "angularVelocity": -0.09574285260739591, + "velocityX": -1.5012065986230485, + "velocityY": 0.11203449154606203, + "timestamp": 2.6340720343886206 + }, + { + "x": 7.769549542634506, + "y": 4.119073150509933, + "heading": -0.02914366903151699, + "angularVelocity": -0.11570751583857591, + "velocityX": -1.97811366872662, + "velocityY": 0.10491038005524037, + "timestamp": 2.7109884083009543 + }, + { + "x": 7.580718251048934, + "y": 4.126594509566205, + "heading": -0.039579227850997996, + "angularVelocity": -0.13567408717648377, + "velocityX": -2.4550207190057343, + "velocityY": 0.09778618873589355, + "timestamp": 2.787904782213288 + }, + { + "x": 7.355205001381359, + "y": 4.133567893300724, + "heading": -0.051550757950875034, + "angularVelocity": -0.15564345393507104, + "velocityX": -2.931927731338533, + "velocityY": 0.09066188874772477, + "timestamp": 2.864821156125622 + }, + { + "x": 7.093009801840762, + "y": 4.139993286528261, + "heading": -0.06505855463043991, + "angularVelocity": -0.1756166599189986, + "velocityX": -3.4088346369452576, + "velocityY": 0.08353739133439493, + "timestamp": 2.9417375300379556 + }, + { + "x": 6.806528186494828, + "y": 4.135714113450545, + "heading": -0.06505855526802462, + "angularVelocity": -8.289323637518806e-9, + "velocityX": -3.724585556678138, + "velocityY": -0.05563409791773408, + "timestamp": 3.0186539039502893 + }, + { + "x": 6.520046573292198, + "y": 4.131434796884326, + "heading": -0.0650585559056006, + "angularVelocity": -8.28921001864462e-9, + "velocityX": -3.7245855288127556, + "velocityY": -0.055635963430836564, + "timestamp": 3.095570277862623 + }, + { + "x": 6.233564957107684, + "y": 4.127155679947005, + "heading": -0.06505855654317651, + "angularVelocity": -8.289209203416631e-9, + "velocityX": -3.7245855675806383, + "velocityY": -0.05563336802900127, + "timestamp": 3.172486651774957 + }, + { + "x": 5.947529123843822, + "y": 4.143694400517207, + "heading": -0.06505855718976304, + "angularVelocity": -8.406357481668178e-9, + "velocityX": -3.718789884581325, + "velocityY": 0.2150221042537956, + "timestamp": 3.2494030256872906 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.06505855788186755, + "angularVelocity": -8.99814278746362e-9, + "velocityX": -3.660825136452102, + "velocityY": 0.6884707552333696, + "timestamp": 3.3263193995996243 + }, + { + "x": 5.429536143877612, + "y": 4.268856603157875, + "heading": -0.06505855854289869, + "angularVelocity": -9.961057942866914e-9, + "velocityX": -3.5625392585748106, + "velocityY": 1.088093052224975, + "timestamp": 3.3926809377564746 + }, + { + "x": 5.202528472751969, + "y": 4.366702264809053, + "heading": -0.06505855916311704, + "angularVelocity": -9.346051554197093e-9, + "velocityX": -3.420771691413992, + "velocityY": 1.4744332992992566, + "timestamp": 3.459042475913325 + }, + { + "x": 4.9875020778910555, + "y": 4.485753657004608, + "heading": -0.06936550467343977, + "angularVelocity": -0.06490123089285486, + "velocityX": -3.240226203809265, + "velocityY": 1.7939818078684182, + "timestamp": 3.5254040140701752 + }, + { + "x": 4.796367419741578, + "y": 4.59157781663034, + "heading": -0.07462073630187449, + "angularVelocity": -0.07919092556314197, + "velocityX": -2.880202350007574, + "velocityY": 1.5946610425998575, + "timestamp": 3.5917655522270255 + }, + { + "x": 4.629125206769459, + "y": 4.684173938079997, + "heading": -0.07977888872827656, + "angularVelocity": -0.07772804202052125, + "velocityX": -2.5201678203544975, + "velocityY": 1.3953281376751576, + "timestamp": 3.658127090383876 + }, + { + "x": 4.485775178787585, + "y": 4.763541978929841, + "heading": -0.08449510558147229, + "angularVelocity": -0.07106852831000718, + "velocityX": -2.1601372114530464, + "velocityY": 1.1959945934684657, + "timestamp": 3.7244886285407253 + }, + { + "x": 4.36631711494158, + "y": 4.829681959675743, + "heading": -0.08859775332946503, + "angularVelocity": -0.0618226741263267, + "velocityX": -1.8001099305995214, + "velocityY": 0.9966613581134071, + "timestamp": 3.7908501666975747 + }, + { + "x": 4.270750854060208, + "y": 4.8825939059492045, + "heading": -0.09198414721152182, + "angularVelocity": -0.051029466406471824, + "velocityX": -1.440085078430444, + "velocityY": 0.7973285089986916, + "timestamp": 3.857211704854424 + }, + { + "x": 4.199076276840986, + "y": 4.9222778404051555, + "heading": -0.09458595257220126, + "angularVelocity": -0.03920652584227169, + "velocityX": -1.0800620240268282, + "velocityY": 0.5979960012704257, + "timestamp": 3.9235732430112735 + }, + { + "x": 4.151293292248578, + "y": 4.948733781946395, + "heading": -0.09635439465344492, + "angularVelocity": -0.026648599932446167, + "velocityX": -0.7200403414319525, + "velocityY": 0.3986637783878444, + "timestamp": 3.989934781168123 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.013539177422752055, + "velocityX": -0.360019736526293, + "velocityY": 0.19933179122763733, + "timestamp": 4.056296319324972 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 3.915017632839986e-26, + "velocityX": -2.9397369327092406e-26, + "velocityY": 2.263261398601171e-26, + "timestamp": 4.122657857481822 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFG.5.traj b/src/main/deploy/choreo/CenterLanePCBAFG.5.traj new file mode 100644 index 00000000..123af814 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFG.5.traj @@ -0,0 +1,536 @@ +{ + "samples": [ + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 3.915017632839986e-26, + "velocityX": -2.9397369327092406e-26, + "velocityY": 2.263261398601171e-26, + "timestamp": 0 + }, + { + "x": 4.148029983348262, + "y": 4.951774833695287, + "heading": -0.10587352183363513, + "angularVelocity": -0.1412289747597874, + "velocityX": 0.33794369238887556, + "velocityY": -0.16688855115376777, + "timestamp": 0.061040211866584926 + }, + { + "x": 4.189291025705269, + "y": 4.931399019893449, + "heading": -0.12288563814036063, + "angularVelocity": -0.27870342822382294, + "velocityX": 0.6759649269762051, + "velocityY": -0.333809683465285, + "timestamp": 0.12208042373316985 + }, + { + "x": 4.251190863307712, + "y": 4.900831857369696, + "heading": -0.14799474838457438, + "angularVelocity": -0.41135358932067295, + "velocityX": 1.0140829415490538, + "velocityY": -0.5007709113225891, + "timestamp": 0.18312063559975478 + }, + { + "x": 4.333736987997449, + "y": 4.860070212810069, + "heading": -0.18081174216294543, + "angularVelocity": -0.5376290935899647, + "velocityX": 1.3523236922925204, + "velocityY": -0.6677834711439198, + "timestamp": 0.2441608474663397 + }, + { + "x": 4.436939103038805, + "y": 4.809109928035341, + "heading": -0.22080172732119155, + "angularVelocity": -0.6551416506491224, + "velocityX": 1.690723408151419, + "velocityY": -0.8348641529310338, + "timestamp": 0.30520105933292463 + }, + { + "x": 4.560810084348815, + "y": 4.747945301360263, + "heading": -0.2671863409578564, + "angularVelocity": -0.7599025661648731, + "velocityX": 2.0293340655624834, + "velocityY": -1.0020382433921913, + "timestamp": 0.36624127119950955 + }, + { + "x": 4.705367311819798, + "y": 4.676568329714506, + "heading": -0.3187309488078688, + "angularVelocity": -0.8444369092734083, + "velocityX": 2.368229451544859, + "velocityY": -1.169343445297431, + "timestamp": 0.4272814830660945 + }, + { + "x": 4.870632910411114, + "y": 4.594968271425701, + "heading": -0.37317308736223054, + "angularVelocity": -0.891906120400698, + "velocityX": 2.7074873028379653, + "velocityY": -1.3368246241864, + "timestamp": 0.4883216949326794 + }, + { + "x": 5.056607795018724, + "y": 4.503143287545809, + "heading": -0.4249356353221189, + "angularVelocity": -0.848007344290121, + "velocityX": 3.0467601425449904, + "velocityY": -1.5043359299046106, + "timestamp": 0.5493619067992643 + }, + { + "x": 5.260529254341505, + "y": 4.402568167992326, + "heading": -0.4249356801729229, + "angularVelocity": -7.347747104408298e-7, + "velocityX": 3.3407724692779857, + "velocityY": -1.6476862788961175, + "timestamp": 0.6104021186658493 + }, + { + "x": 5.4644220623717725, + "y": 4.301934949251132, + "heading": -0.42493568721051356, + "angularVelocity": -1.1529433576821183e-7, + "velocityX": 3.3403030853811013, + "velocityY": -1.648638097147308, + "timestamp": 0.6714423305324342 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.4249356942701639, + "angularVelocity": -1.1565573156948564e-7, + "velocityX": 3.301588580483846, + "velocityY": -1.7248608986943215, + "timestamp": 0.7324825423990191 + }, + { + "x": 5.864577127291663, + "y": 4.06418051122573, + "heading": -0.42493570026957905, + "angularVelocity": -9.360477410609904e-8, + "velocityX": 3.09901633677209, + "velocityY": -2.0668164546168653, + "timestamp": 0.7965755889938464 + }, + { + "x": 6.060491998416757, + "y": 3.9277351651374044, + "heading": -0.4249357062560635, + "angularVelocity": -9.340302536538937e-8, + "velocityX": 3.056725831174745, + "velocityY": -2.1288634779819966, + "timestamp": 0.8606686355886737 + }, + { + "x": 6.256406839411796, + "y": 3.7912897757868893, + "heading": -0.4249357122425479, + "angularVelocity": -9.340302451557142e-8, + "velocityX": 3.0567253610760865, + "velocityY": -2.1288641529723815, + "timestamp": 0.924761682183501 + }, + { + "x": 6.4523216804065155, + "y": 3.654844386435917, + "heading": -0.4249357182290323, + "angularVelocity": -9.340302467625888e-8, + "velocityX": 3.05672536107112, + "velocityY": -2.1288641529795123, + "timestamp": 0.9888547287783283 + }, + { + "x": 6.648236521401236, + "y": 3.5183989970849443, + "heading": -0.4249357242155167, + "angularVelocity": -9.340302391578915e-8, + "velocityX": 3.05672536107112, + "velocityY": -2.1288641529795123, + "timestamp": 1.0529477753731555 + }, + { + "x": 6.844151362395955, + "y": 3.3819536077339722, + "heading": -0.4249357302020011, + "angularVelocity": -9.340302522406469e-8, + "velocityX": 3.0567253610711203, + "velocityY": -2.128864152979512, + "timestamp": 1.1170408219679828 + }, + { + "x": 7.040066203390847, + "y": 3.2455082183832458, + "heading": -0.4249357361884855, + "angularVelocity": -9.340302460522487e-8, + "velocityX": 3.056725361073794, + "velocityY": -2.1288641529756727, + "timestamp": 1.18113386856281 + }, + { + "x": 7.235981060605469, + "y": 3.1090628523216983, + "heading": -0.4249357421751277, + "angularVelocity": -9.340548585202334e-8, + "velocityX": 3.0567256141391645, + "velocityY": -2.128863789610536, + "timestamp": 1.2452269151576374 + }, + { + "x": 7.417177244420043, + "y": 2.9830454990225337, + "heading": -0.48590548097831143, + "angularVelocity": -0.9512691632309586, + "velocityX": 2.8270802129290376, + "velocityY": -1.966162633769009, + "timestamp": 1.3093199617524647 + }, + { + "x": 7.5775843375603, + "y": 2.871507967151711, + "heading": -0.5361633770520781, + "angularVelocity": -0.7841396024045931, + "velocityX": 2.502722239969217, + "velocityY": -1.7402438766239745, + "timestamp": 1.373413008347292 + }, + { + "x": 7.717202062000419, + "y": 2.774449785512018, + "heading": -0.5757086599849341, + "angularVelocity": -0.6169980213742552, + "velocityX": 2.1783599291625744, + "velocityY": -1.514332471247, + "timestamp": 1.4375060549421192 + }, + { + "x": 7.836030204991866, + "y": 2.691870623138966, + "heading": -0.6045409513138722, + "angularVelocity": -0.44985053544427944, + "velocityX": 1.8539942989859028, + "velocityY": -1.2884262296827267, + "timestamp": 1.5015991015369465 + }, + { + "x": 7.934068628103717, + "y": 2.6237702758314256, + "heading": -0.6226600347771661, + "angularVelocity": -0.28269967533040424, + "velocityX": 1.5296265089661738, + "velocityY": -1.0625231741291041, + "timestamp": 1.5656921481317738 + }, + { + "x": 8.011317274299664, + "y": 2.5701486603074475, + "heading": -0.6300657818408324, + "angularVelocity": -0.11554680978863106, + "velocityX": 1.2052578290478972, + "velocityY": -0.836621417966838, + "timestamp": 1.629785194726601 + }, + { + "x": 8.06777617103265, + "y": 2.5310058125218204, + "heading": -0.6267581325303803, + "angularVelocity": 0.051606991494130075, + "velocityX": 0.8808895774591454, + "velocityY": -0.6107191008265523, + "timestamp": 1.6938782413214284 + }, + { + "x": 8.10344542848898, + "y": 2.5063418886445565, + "heading": -0.612737112733536, + "angularVelocity": 0.21876038886839116, + "velocityX": 0.5565230450320063, + "velocityY": -0.3848143470723113, + "timestamp": 1.7579712879162557 + }, + { + "x": 8.118325233459473, + "y": 2.496157169342041, + "heading": -0.5880028838627427, + "angularVelocity": 0.38591126783462537, + "velocityX": 0.23215942697429998, + "velocityY": -0.15890521427230228, + "timestamp": 1.822064334511083 + }, + { + "x": 8.111774217431309, + "y": 2.500903203120534, + "heading": -0.5514605400771017, + "angularVelocity": 0.5570006800274493, + "velocityX": -0.09985457977086463, + "velocityY": 0.0723419400124114, + "timestamp": 1.8876698985542717 + }, + { + "x": 8.083429359560292, + "y": 2.5208307056412647, + "heading": -0.504313305894281, + "angularVelocity": 0.7186468841542555, + "velocityX": -0.432049602566591, + "velocityY": 0.3037471411359584, + "timestamp": 1.9532754625974604 + }, + { + "x": 8.033276752823499, + "y": 2.5559518951025, + "heading": -0.44733468663642767, + "angularVelocity": 0.8685028486355681, + "velocityX": -0.7644566046833565, + "velocityY": 0.5353385794856407, + "timestamp": 2.018881026640649 + }, + { + "x": 7.9612999822666515, + "y": 2.606281084085956, + "heading": -0.38151750427277176, + "angularVelocity": 1.0032256154421146, + "velocityX": -1.097113813539709, + "velocityY": 0.7671481789307238, + "timestamp": 2.084486590683838 + }, + { + "x": 7.867479640224459, + "y": 2.6718348304324313, + "heading": -0.3081892028790952, + "angularVelocity": 1.117714670441717, + "velocityX": -1.430066845861252, + "velocityY": 0.999210163078856, + "timestamp": 2.1500921547270266 + }, + { + "x": 7.751793438011374, + "y": 2.75263175880398, + "heading": -0.2292374531912587, + "angularVelocity": 1.2034306973698263, + "velocityX": -1.763359615914976, + "velocityY": 1.2315560356795054, + "timestamp": 2.2156977187702154 + }, + { + "x": 7.614220064693883, + "y": 2.8486908030805904, + "heading": -0.14760413079417548, + "angularVelocity": 1.244304863278703, + "velocityX": -2.0969772202083976, + "velocityY": 1.4641905100209518, + "timestamp": 2.281303282813404 + }, + { + "x": 7.454764083734287, + "y": 2.9600197368919443, + "heading": -0.06854432318452272, + "angularVelocity": 1.2050777820857683, + "velocityX": -2.4305252654275167, + "velocityY": 1.696943474764787, + "timestamp": 2.346908846856593 + }, + { + "x": 7.27363003976687, + "y": 3.0865189810411944, + "heading": -0.0037505079236450347, + "angularVelocity": 0.987626830221633, + "velocityX": -2.760955516641443, + "velocityY": 1.928178592687242, + "timestamp": 2.4125144108997816 + }, + { + "x": 7.07280859084814, + "y": 3.2235171993435245, + "heading": -7.171638886724292e-8, + "angularVelocity": 0.057166434919867046, + "velocityX": -3.0610429442619163, + "velocityY": 2.0882103568554258, + "timestamp": 2.4781199749429703 + }, + { + "x": 6.872223980539225, + "y": 3.3631158565871417, + "heading": -6.147329807763224e-8, + "angularVelocity": 1.5613143395684282e-7, + "velocityX": -3.0574329057953955, + "velocityY": 2.12784783241428, + "timestamp": 2.543725538986159 + }, + { + "x": 6.671639384359248, + "y": 3.502714534132154, + "heading": -5.123020973688295e-8, + "angularVelocity": 1.561313966298048e-7, + "velocityX": -3.0574326904335125, + "velocityY": 2.1278481418605137, + "timestamp": 2.609331103029348 + }, + { + "x": 6.471054788179423, + "y": 3.6423132116773846, + "heading": -4.098712138128988e-8, + "angularVelocity": 1.561313968560626e-7, + "velocityX": -3.057432690431191, + "velocityY": 2.1278481418638497, + "timestamp": 2.6749366670725365 + }, + { + "x": 6.270470191999598, + "y": 3.781911889222615, + "heading": -3.074403308674018e-8, + "angularVelocity": 1.5613139592560247e-7, + "velocityX": -3.057432690431191, + "velocityY": 2.1278481418638493, + "timestamp": 2.7405422311157253 + }, + { + "x": 6.069885595819435, + "y": 3.921510566767361, + "heading": -2.0500944793387612e-8, + "angularVelocity": 1.5613139590735494e-7, + "velocityX": -3.0574326904363347, + "velocityY": 2.1278481418564588, + "timestamp": 2.806147795158914 + }, + { + "x": 5.869300968338538, + "y": 4.0611091993371335, + "heading": -1.0257856528303386e-8, + "angularVelocity": 1.5613139547647166e-7, + "velocityX": -3.057433167541219, + "velocityY": 2.127847456320497, + "timestamp": 2.8717533592021027 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 3.3501373579040154e-26, + "angularVelocity": 1.563565023471243e-7, + "velocityX": -3.099573069501661, + "velocityY": 2.0659814025542267, + "timestamp": 2.9373589232452915 + }, + { + "x": 5.446743415639457, + "y": 4.310089409453816, + "heading": -2.545629671266297e-9, + "angularVelocity": -3.841829867702764e-8, + "velocityX": -3.3082622124289722, + "velocityY": 1.712026190368146, + "timestamp": 3.003619787440938 + }, + { + "x": 5.225460228694709, + "y": 4.4194271674986805, + "heading": -5.081465433806644e-9, + "angularVelocity": -3.827049033125913e-8, + "velocityX": -3.3395759266189464, + "velocityY": 1.650110655394207, + "timestamp": 3.069880651636584 + }, + { + "x": 5.005890824462741, + "y": 4.527909625118252, + "heading": -0.005806060945576971, + "angularVelocity": -0.08762420977438817, + "velocityX": -3.3137117497238457, + "velocityY": 1.6372025770635645, + "timestamp": 3.1361415158322306 + }, + { + "x": 4.810655372355854, + "y": 4.624369173325347, + "heading": -0.020290065438414114, + "angularVelocity": -0.21859063670028128, + "velocityX": -2.946467035661095, + "velocityY": 1.455754454428526, + "timestamp": 3.202402380027877 + }, + { + "x": 4.639828140240907, + "y": 4.708771468826467, + "heading": -0.03646660670869356, + "angularVelocity": -0.24413417281301306, + "velocityX": -2.5781014809971645, + "velocityY": 1.2737880274532598, + "timestamp": 3.2686632442235233 + }, + { + "x": 4.493411439660615, + "y": 4.781114493443874, + "heading": -0.05214975334121426, + "angularVelocity": -0.23668792767648847, + "velocityX": -2.2097010408432496, + "velocityY": 1.091791142412547, + "timestamp": 3.3349241084191696 + }, + { + "x": 4.3714027429262465, + "y": 4.841398729732693, + "heading": -0.06627328599748628, + "angularVelocity": -0.21315044450024043, + "velocityX": -1.8413387482257655, + "velocityY": 0.9098015400285159, + "timestamp": 3.401184972614816 + }, + { + "x": 4.2737994273361055, + "y": 4.889624930300945, + "heading": -0.07820644553038021, + "angularVelocity": -0.1800936295919612, + "velocityX": -1.4730160370675427, + "velocityY": 0.7278232959029468, + "timestamp": 3.4674458368104624 + }, + { + "x": 4.20059930110606, + "y": 4.925793789498138, + "heading": -0.08753245479698174, + "angularVelocity": -0.14074687041607162, + "velocityX": -1.1047264040189804, + "velocityY": 0.5458555308062137, + "timestamp": 3.5337067010061087 + }, + { + "x": 4.151800585484216, + "y": 4.949905897329157, + "heading": -0.09395516408530516, + "angularVelocity": -0.09693065984408723, + "velocityX": -0.7364636156533751, + "velocityY": 0.363896669983421, + "timestamp": 3.599967565201755 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.04976861149222346, + "velocityX": -0.36822273621886326, + "velocityY": 0.1819452407240944, + "timestamp": 3.6662284293974015 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -1.4138878397118624e-28, + "velocityX": 4.127775100583887e-27, + "velocityY": 5.8037818722321745e-27, + "timestamp": 3.732489293593048 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAFG.traj b/src/main/deploy/choreo/CenterLanePCBAFG.traj new file mode 100644 index 00000000..59e9adee --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAFG.traj @@ -0,0 +1,1661 @@ +{ + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -1.120688542766024e-26, + "angularVelocity": 1.2570906532337121e-26, + "velocityX": 3.544719035734486e-27, + "velocityY": -1.5437798405468174e-26, + "timestamp": 0 + }, + { + "x": 1.3104780876502795, + "y": 5.567275466796953, + "heading": -0.013632783425389957, + "angularVelocity": -0.19128878469611282, + "velocityX": 0.28769735437387517, + "velocityY": -0.33225026766694243, + "timestamp": 0.07126807484844137 + }, + { + "x": 1.3514855787219209, + "y": 5.519918104138928, + "heading": -0.04088178026931428, + "angularVelocity": -0.3823450668742212, + "velocityX": 0.575397766234711, + "velocityY": -0.6644961682876167, + "timestamp": 0.14253614969688275 + }, + { + "x": 1.4129976098576797, + "y": 5.448882530124723, + "heading": -0.08170955131958318, + "angularVelocity": -0.5728760196917512, + "velocityX": 0.8631077977982459, + "velocityY": -0.9967376579943998, + "timestamp": 0.2138042245453241 + }, + { + "x": 1.4950153477240138, + "y": 5.354169209270188, + "heading": -0.13606323380780802, + "angularVelocity": -0.7626652270853859, + "velocityX": 1.1508341994750528, + "velocityY": -1.3289726298339328, + "timestamp": 0.2850722993937655 + }, + { + "x": 1.5975404069408672, + "y": 5.235778918310646, + "heading": -0.20388243920312946, + "angularVelocity": -0.9516070911070027, + "velocityX": 1.4385832567370889, + "velocityY": -1.6611967028899048, + "timestamp": 0.3563403742422069 + }, + { + "x": 1.7205747481570635, + "y": 5.093712881376144, + "heading": -0.28510896229616783, + "angularVelocity": -1.139732247093452, + "velocityX": 1.7263598248983312, + "velocityY": -1.9934035995306334, + "timestamp": 0.42760844909064827 + }, + { + "x": 1.8641205194491128, + "y": 4.927972835971274, + "heading": -0.37969642809760956, + "angularVelocity": -1.3272066911108702, + "velocityX": 2.0141665338556387, + "velocityY": -2.32558611632674, + "timestamp": 0.49887652393908966 + }, + { + "x": 2.036056146597576, + "y": 4.7662140643499225, + "heading": -0.4690836216877677, + "angularVelocity": -1.2542389250761836, + "velocityX": 2.4125195961038894, + "velocityY": -2.2697227610728588, + "timestamp": 0.570144598787531 + }, + { + "x": 2.187477556502736, + "y": 4.628126287278482, + "heading": -0.5451441122574506, + "angularVelocity": -1.0672449162045279, + "velocityX": 2.1246737789279755, + "velocityY": -1.9375825341865685, + "timestamp": 0.6414126736359723 + }, + { + "x": 2.318383785639942, + "y": 4.513708351772966, + "heading": -0.607881175347142, + "angularVelocity": -0.8802968681714505, + "velocityX": 1.8368144420287889, + "velocityY": -1.6054584854275527, + "timestamp": 0.7126807484844137 + }, + { + "x": 2.428774184877684, + "y": 4.4229594060003725, + "heading": -0.6572940712821624, + "angularVelocity": -0.6933384413722671, + "velocityX": 1.548945996822536, + "velocityY": -1.2733463891873111, + "timestamp": 0.783948823332855 + }, + { + "x": 2.518648327310084, + "y": 4.355878850846356, + "heading": -0.6933805665693583, + "angularVelocity": -0.5063486752509772, + "velocityX": 1.2610715614744235, + "velocityY": -0.9412426994368749, + "timestamp": 0.8552168981812963 + }, + { + "x": 2.5880059699725453, + "y": 4.312466305335323, + "heading": -0.7161380105199752, + "angularVelocity": -0.31932171591575625, + "velocityX": 0.9731937169617235, + "velocityY": -0.6091443553562279, + "timestamp": 0.9264849730297376 + }, + { + "x": 2.636847038659, + "y": 4.292721584024701, + "heading": -0.7255637248085023, + "angularVelocity": -0.13225717558067804, + "velocityX": 0.6853148312244995, + "velocityY": -0.2770486133182406, + "timestamp": 0.997753047878179 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.054843767292669265, + "velocityX": 0.39743720636786783, + "velocityY": 0.055047139076356964, + "timestamp": 1.0690211227266204 + }, + { + "x": 2.6726523875585073, + "y": 4.325520941215623, + "heading": -0.7036434191878628, + "angularVelocity": 0.24661858695360772, + "velocityX": 0.10242764131881017, + "velocityY": 0.3953775861253462, + "timestamp": 1.1420557472889747 + }, + { + "x": 2.658587549089958, + "y": 4.379253724773467, + "heading": -0.6716319783821333, + "angularVelocity": 0.43830499571335924, + "velocityX": -0.19257767877674353, + "velocityY": 0.7357165711445571, + "timestamp": 1.215090371851329 + }, + { + "x": 2.622977623032788, + "y": 4.4578439357274, + "heading": -0.6256269314489635, + "angularVelocity": 0.6299073515999585, + "velocityX": -0.48757594456814685, + "velocityY": 1.0760678435039392, + "timestamp": 1.2881249964136834 + }, + { + "x": 2.5658233842938287, + "y": 4.561292786214616, + "heading": -0.5656322001603995, + "angularVelocity": 0.8214560100509974, + "velocityX": -0.782563600229962, + "velocityY": 1.4164357126104716, + "timestamp": 1.3611596209760377 + }, + { + "x": 2.4871259686686225, + "y": 4.689601823386252, + "heading": -0.49164499426759223, + "angularVelocity": 1.0130428729682832, + "velocityX": -1.0775357044249236, + "velocityY": 1.7568247655204965, + "timestamp": 1.434194245538392 + }, + { + "x": 2.3868870212617113, + "y": 4.842772877881088, + "heading": -0.40364701353076443, + "angularVelocity": 1.204880305254371, + "velocityX": -1.3724852836250385, + "velocityY": 2.0972388837853764, + "timestamp": 1.5072288701007464 + }, + { + "x": 2.26510884712338, + "y": 5.020807849872121, + "heading": -0.3015898760322786, + "angularVelocity": 1.397380189328601, + "velocityX": -1.6674033017635614, + "velocityY": 2.437679019477028, + "timestamp": 1.5802634946631007 + }, + { + "x": 2.1359147758136907, + "y": 5.1700050636509935, + "heading": -0.2156034423927932, + "angularVelocity": 1.1773379291636314, + "velocityX": -1.7689427731553284, + "velocityY": 2.0428285169247893, + "timestamp": 1.653298119225455 + }, + { + "x": 2.0282555085515424, + "y": 5.294336258780153, + "heading": -0.1438206059999422, + "angularVelocity": 0.9828603463493605, + "velocityX": -1.4740853110052332, + "velocityY": 1.7023596119537827, + "timestamp": 1.7263327437878093 + }, + { + "x": 1.9421294204612929, + "y": 5.393801267693995, + "heading": -0.08631841339014902, + "angularVelocity": 0.7873278318929399, + "velocityX": -1.1792500968731383, + "velocityY": 1.361888412651758, + "timestamp": 1.7993673683501636 + }, + { + "x": 1.8775353609204488, + "y": 5.468400102933928, + "heading": -0.043158758281562314, + "angularVelocity": 0.5909478602404344, + "velocityX": -0.8844306372205144, + "velocityY": 1.0214173850684016, + "timestamp": 1.872401992912518 + }, + { + "x": 1.8344726769226236, + "y": 5.518132795390135, + "heading": -0.014381433415882237, + "angularVelocity": 0.3940230409634143, + "velocityX": -0.5896201186200382, + "velocityY": 0.6809467804376429, + "timestamp": 1.9454366174748723 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 3.4376441721489434e-26, + "angularVelocity": 0.1969125397995838, + "velocityX": -0.2948119945987197, + "velocityY": 0.34047511487869625, + "timestamp": 2.0184712420372266 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": 1.5116489541153283e-26, + "angularVelocity": -4.402301586321663e-26, + "velocityX": -5.546242066692014e-22, + "velocityY": -8.311642516359436e-25, + "timestamp": 2.0915058665995807 + }, + { + "x": 1.8720264440667018, + "y": 5.543082773688388, + "heading": 5.813054707764239e-20, + "angularVelocity": 5.955362224136031e-19, + "velocityX": 0.6053171317615015, + "velocityY": 0.0008555041864665229, + "timestamp": 2.1891162717670896 + }, + { + "x": 1.9901969439319853, + "y": 5.543249785907351, + "heading": 8.928720004263094e-20, + "angularVelocity": 3.1919397232381416e-19, + "velocityX": 1.2106342521833766, + "velocityY": 0.0017110083569065741, + "timestamp": 2.2867266769345984 + }, + { + "x": 2.1674526905937914, + "y": 5.543500304231363, + "heading": 9.074204012627455e-20, + "angularVelocity": 1.4904559423917405e-20, + "velocityX": 1.8159513461461239, + "velocityY": 0.0025665124899515255, + "timestamp": 2.3843370821021073 + }, + { + "x": 2.403793671138691, + "y": 5.5438343286421725, + "heading": 4.918611066370257e-20, + "angularVelocity": -4.2573257831162476e-19, + "velocityX": 2.4212683078132486, + "velocityY": 0.0034220164360210005, + "timestamp": 2.481947487269616 + }, + { + "x": 2.5810494178004975, + "y": 5.544084846966184, + "heading": 1.0185426849288302e-20, + "angularVelocity": -3.9955457358178204e-19, + "velocityX": 1.8159513461461239, + "velocityY": 0.0025665124899515255, + "timestamp": 2.579557892437125 + }, + { + "x": 2.699219917665781, + "y": 5.544251859185147, + "heading": 2.0104392985221674e-20, + "angularVelocity": 1.0161791787953617e-19, + "velocityX": 1.2106342521833766, + "velocityY": 0.001711008356906574, + "timestamp": 2.677168297604634 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.4565183072234727e-28, + "angularVelocity": -2.0596567646457299e-19, + "velocityX": 0.6053171317615015, + "velocityY": 0.0008555041864665228, + "timestamp": 2.7747787027721427 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -1.5120860158326505e-28, + "angularVelocity": -5.112250671135627e-29, + "velocityX": 5.545899463618144e-22, + "velocityY": 7.793513704496499e-25, + "timestamp": 2.8723891079396515 + }, + { + "x": 2.7303741241412878, + "y": 5.568056862351408, + "heading": 0.006778500636213844, + "angularVelocity": 0.08813486156991067, + "velocityX": -0.36316271539799666, + "velocityY": 0.30842969138218984, + "timestamp": 2.9492996574710304 + }, + { + "x": 2.6747561237908846, + "y": 5.615779928168, + "heading": 0.020597377597861958, + "angularVelocity": 0.17967466161466059, + "velocityX": -0.7231517742271671, + "velocityY": 0.6205009079686912, + "timestamp": 3.0262102070024093 + }, + { + "x": 2.591861580665438, + "y": 5.687962474226551, + "heading": 0.04189403274126234, + "angularVelocity": 0.2769016119786207, + "velocityX": -1.0778045876739724, + "velocityY": 0.9385259434286201, + "timestamp": 3.103120756533788 + }, + { + "x": 2.4825228427377626, + "y": 5.785486601053793, + "heading": 0.07152934014357441, + "angularVelocity": 0.38532174822416343, + "velocityX": -1.4216351149989688, + "velocityY": 1.2680201535610187, + "timestamp": 3.180031306065167 + }, + { + "x": 2.3492929926954265, + "y": 5.910724214608569, + "heading": 0.11194451008519196, + "angularVelocity": 0.5254827873142219, + "velocityX": -1.7322701612992524, + "velocityY": 1.6283541636076828, + "timestamp": 3.256941855596546 + }, + { + "x": 2.2322885923008555, + "y": 6.0682629894504885, + "heading": 0.178093318543464, + "angularVelocity": 0.8600745783422568, + "velocityX": -1.5213049589098766, + "velocityY": 2.04833765721106, + "timestamp": 3.333852405127925 + }, + { + "x": 2.148304694530927, + "y": 6.209825325348869, + "heading": 0.2439233082099427, + "angularVelocity": 0.8559292589584284, + "velocityX": -1.091968504732421, + "velocityY": 1.840610121250321, + "timestamp": 3.4107629546593037 + }, + { + "x": 2.0954355209198954, + "y": 6.331973650947481, + "heading": 0.3066258894331499, + "angularVelocity": 0.8152663269897058, + "velocityX": -0.6874112060460749, + "velocityY": 1.5881868786905071, + "timestamp": 3.4876735041906826 + }, + { + "x": 2.072969838348887, + "y": 6.433621733055282, + "heading": 0.3652444017006222, + "angularVelocity": 0.7621647826551591, + "velocityX": -0.2921014439227605, + "velocityY": 1.321640304576541, + "timestamp": 3.5645840537220614 + }, + { + "x": 2.080539684915293, + "y": 6.514239840456833, + "heading": 0.4192892074806503, + "angularVelocity": 0.7026969136136281, + "velocityX": 0.09842403431687198, + "velocityY": 1.048206102969775, + "timestamp": 3.6414946032534403 + }, + { + "x": 2.1179206171311256, + "y": 6.5735146979492365, + "heading": 0.46845941042093314, + "angularVelocity": 0.6393167548519696, + "velocityX": 0.4860312719594013, + "velocityY": 0.7706986603732364, + "timestamp": 3.718405152784819 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.5732764808952261, + "velocityX": 0.8716741807939862, + "velocityY": 0.490501440559152, + "timestamp": 3.795315702316198 + }, + { + "x": 2.2917885277794023, + "y": 6.672276624724165, + "heading": 0.5461832199677008, + "angularVelocity": 0.41150844936023273, + "velocityX": 1.3070633564521403, + "velocityY": 0.7468102485181382, + "timestamp": 3.8770462243018082 + }, + { + "x": 2.4341166113358135, + "y": 6.754285363860808, + "heading": 0.5638407865548152, + "angularVelocity": 0.21604617416028918, + "velocityX": 1.7414312315473808, + "velocityY": 1.0034040789691927, + "timestamp": 3.9587767462874184 + }, + { + "x": 2.5412609256604743, + "y": 6.8169454314474, + "heading": 0.5395666812512139, + "angularVelocity": -0.29700171629731165, + "velocityX": 1.3109461645616998, + "velocityY": 0.7666666756101825, + "timestamp": 4.0405072682730285 + }, + { + "x": 2.6126555907805695, + "y": 6.858741825974232, + "heading": 0.5217952234534815, + "angularVelocity": -0.21743967083510496, + "velocityX": 0.8735373687282387, + "velocityY": 0.5113927271159635, + "timestamp": 4.122237790258638 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.11311323638810476, + "velocityX": 0.4366658269950189, + "velocityY": 0.2557556745753436, + "timestamp": 4.203968312244248 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": 6.389972915056163e-27, + "velocityX": -1.0690163263683836e-25, + "velocityY": -1.1312672794497157e-25, + "timestamp": 4.285698834229858 + }, + { + "x": 2.660849775777501, + "y": 6.863303095829213, + "heading": 0.49944853016087754, + "angularVelocity": -0.22618164286906187, + "velocityX": 0.21588184235159683, + "velocityY": -0.2821127073294775, + "timestamp": 4.343625239188846 + }, + { + "x": 2.6858699737019993, + "y": 6.830605675150758, + "heading": 0.4737478272074287, + "angularVelocity": -0.4436785430002931, + "velocityX": 0.43193079118602823, + "velocityY": -0.5644648705819807, + "timestamp": 4.401551644147834 + }, + { + "x": 2.7234170479654534, + "y": 6.7815357680679345, + "heading": 0.4360912802839983, + "angularVelocity": -0.6500756770611135, + "velocityX": 0.6481858194037288, + "velocityY": -0.847107758846162, + "timestamp": 4.459478049106822 + }, + { + "x": 2.773506189570542, + "y": 6.716073180513463, + "heading": 0.38731316065986704, + "angularVelocity": -0.8420705489779003, + "velocityX": 0.864703094220184, + "velocityY": -1.1300992630359112, + "timestamp": 4.517404454065811 + }, + { + "x": 2.8361569303832073, + "y": 6.63419375338078, + "heading": 0.3285222984900142, + "angularVelocity": -1.0149233706368752, + "velocityX": 1.0815575531922228, + "velocityY": -1.4135078327518056, + "timestamp": 4.575330859024799 + }, + { + "x": 2.9113941307930915, + "y": 6.535868410078246, + "heading": 0.2612558665356734, + "angularVelocity": -1.1612395418283843, + "velocityX": 1.2988411841396363, + "velocityY": -1.6974183599370378, + "timestamp": 4.633257263983787 + }, + { + "x": 2.99924805742508, + "y": 6.421062364749842, + "heading": 0.187813414307632, + "angularVelocity": -1.2678579359454234, + "velocityX": 1.5166473164386636, + "velocityY": -1.9819294052459604, + "timestamp": 4.6911836689427755 + }, + { + "x": 3.0997480806917785, + "y": 6.289740322129515, + "heading": 0.11213231017946661, + "angularVelocity": -1.306504420250965, + "velocityX": 1.734960478521888, + "velocityY": -2.26704976276885, + "timestamp": 4.749110073901764 + }, + { + "x": 3.2128556613445705, + "y": 6.14193527325681, + "heading": 0.04279587609271538, + "angularVelocity": -1.1969745772388503, + "velocityX": 1.952608326597726, + "velocityY": -2.551600586595203, + "timestamp": 4.807036478860752 + }, + { + "x": 3.336513792781727, + "y": 5.978631812899107, + "heading": 0.010716004519218999, + "angularVelocity": -0.5538039447849203, + "velocityX": 2.134745484804496, + "velocityY": -2.8191540709857907, + "timestamp": 4.86496288381974 + }, + { + "x": 3.4677695577152434, + "y": 5.807368362842113, + "heading": 0.010715992273577517, + "angularVelocity": -2.1139999092239637e-7, + "velocityX": 2.265905592215594, + "velocityY": -2.956569636562943, + "timestamp": 4.922889288778729 + }, + { + "x": 3.5990253485117636, + "y": 5.636104932606414, + "heading": 0.01071598002800544, + "angularVelocity": -2.11398792746924e-7, + "velocityX": 2.265906038695981, + "velocityY": -2.9565692943823, + "timestamp": 4.980815693737717 + }, + { + "x": 3.730281139327178, + "y": 5.464841502385196, + "heading": 0.010715967782433288, + "angularVelocity": -2.113987940232789e-7, + "velocityX": 2.2659060390221635, + "velocityY": -2.956569294132315, + "timestamp": 5.038742098696705 + }, + { + "x": 3.8615381491076497, + "y": 5.293579006382116, + "heading": 0.010715955536865166, + "angularVelocity": -2.1139872447064547e-7, + "velocityX": 2.2659270823625586, + "velocityY": -2.9565531664589524, + "timestamp": 5.096668503655693 + }, + { + "x": 4.007189928421941, + "y": 5.1343784904281335, + "heading": 0.010715943225165272, + "angularVelocity": -2.1254037607347718e-7, + "velocityX": 2.5144280819328126, + "velocityY": -2.748323775084899, + "timestamp": 5.154594908614682 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": 0.010715930460413511, + "angularVelocity": -2.203615392689942e-7, + "velocityX": 2.7674622150421295, + "velocityY": -2.4933481486115037, + "timestamp": 5.21252131357367 + }, + { + "x": 4.289285126852667, + "y": 4.894606292568033, + "heading": 0.010715918114469883, + "angularVelocity": -2.9733998228105175e-7, + "velocityX": 2.933098226238877, + "velocityY": -2.2962068965718774, + "timestamp": 5.2540426170077055 + }, + { + "x": 4.417366677446148, + "y": 4.807905868510879, + "heading": 0.010715906356446534, + "angularVelocity": -2.831804971331414e-7, + "velocityX": 3.0847189274046736, + "velocityY": -2.088094950942351, + "timestamp": 5.295563920441741 + }, + { + "x": 4.551131631929394, + "y": 4.730260623000039, + "heading": 0.010715894919665146, + "angularVelocity": -2.7544369861896277e-7, + "velocityX": 3.221598153722609, + "velocityY": -1.8700098284292712, + "timestamp": 5.337085223875777 + }, + { + "x": 4.688895654984909, + "y": 4.659954711675372, + "heading": 0.010715883576398903, + "angularVelocity": -2.7319147770282054e-7, + "velocityX": 3.3179118106053243, + "velocityY": -1.6932491398387937, + "timestamp": 5.378606527309812 + }, + { + "x": 4.826660072114789, + "y": 4.5896495725424655, + "heading": 0.010715872233137535, + "angularVelocity": -2.7319136037670126e-7, + "velocityX": 3.31792130150117, + "velocityY": -1.6932305423551623, + "timestamp": 5.420127830743848 + }, + { + "x": 4.964424489260567, + "y": 4.51934443344071, + "heading": 0.010715860889876156, + "angularVelocity": -2.731913605852805e-7, + "velocityX": 3.3179213018840272, + "velocityY": -1.6932305416049456, + "timestamp": 5.4616491341778834 + }, + { + "x": 5.102188906406363, + "y": 4.449039294338991, + "heading": 0.01071584954661479, + "angularVelocity": -2.731913603225307e-7, + "velocityX": 3.3179213018844735, + "velocityY": -1.6932305416040707, + "timestamp": 5.503170437611919 + }, + { + "x": 5.239953323697088, + "y": 4.378734155521264, + "heading": 0.0107158382033534, + "angularVelocity": -2.7319136082771354e-7, + "velocityX": 3.3179213053749512, + "velocityY": -1.6932305347644059, + "timestamp": 5.544691741045955 + }, + { + "x": 5.377721333912445, + "y": 4.308436057560326, + "heading": 0.01071582686006505, + "angularVelocity": -2.731920101868387e-7, + "velocityX": 3.3180078374520967, + "velocityY": -1.6930609626121105, + "timestamp": 5.58621304447999 + }, + { + "x": 5.519903612293123, + "y": 4.247558747282141, + "heading": 0.01071581542197781, + "angularVelocity": -2.7547514875541596e-7, + "velocityX": 3.4243211706145282, + "velocityY": -1.4661705014848305, + "timestamp": 5.627734347914026 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 0.010715803649590386, + "angularVelocity": -2.8352644184424e-7, + "velocityX": 3.517426102957061, + "velocityY": -1.2261096959197675, + "timestamp": 5.669255651348061 + }, + { + "x": 5.815511009397966, + "y": 4.155881926930556, + "heading": 0.010715791923418018, + "angularVelocity": -2.8177750483647635e-7, + "velocityX": 3.59387868290116, + "velocityY": -0.9796261538753469, + "timestamp": 5.710870656687205 + }, + { + "x": 5.967533992937965, + "y": 4.125567717701148, + "heading": 0.0107157805290767, + "angularVelocity": -2.738036730972178e-7, + "velocityX": 3.653080957243209, + "velocityY": -0.7284441989700993, + "timestamp": 5.7524856620263485 + }, + { + "x": 6.121241175717782, + "y": 4.105466951776193, + "heading": 0.010715769236027265, + "angularVelocity": -2.713696500137211e-7, + "velocityX": 3.6935519178040117, + "velocityY": -0.4830172617098897, + "timestamp": 5.794100667365492 + }, + { + "x": 6.274949114201427, + "y": 4.0853719654365035, + "heading": 0.010715757943004396, + "angularVelocity": -2.713690116930231e-7, + "velocityX": 3.693570077211222, + "velocityY": -0.4828783794673188, + "timestamp": 5.8357156727046355 + }, + { + "x": 6.428657052715288, + "y": 4.065276979327946, + "heading": 0.010715746649981548, + "angularVelocity": -2.7136901112829927e-7, + "velocityX": 3.6935700779373306, + "velocityY": -0.48287837391326544, + "timestamp": 5.877330678043779 + }, + { + "x": 6.582364991229262, + "y": 4.045181993220238, + "heading": 0.010715735356958701, + "angularVelocity": -2.71369011116775e-7, + "velocityX": 3.69357007794, + "velocityY": -0.48287837389284793, + "timestamp": 5.918945683382923 + }, + { + "x": 6.73607293245351, + "y": 4.025087027843618, + "heading": 0.010715724063935807, + "angularVelocity": -2.713690122742687e-7, + "velocityX": 3.693570143067337, + "velocityY": -0.4828778757290756, + "timestamp": 5.960560688722066 + }, + { + "x": 6.889847609682978, + "y": 4.005509298883675, + "heading": 0.010715712767818455, + "angularVelocity": -2.714433714426062e-7, + "velocityX": 3.695173795516192, + "velocityY": -0.47044879125674055, + "timestamp": 6.00217569406121 + }, + { + "x": 7.044608530639083, + "y": 3.9966213295297166, + "heading": 0.010715699615645665, + "angularVelocity": -3.1604400106558483e-7, + "velocityX": 3.7188730289681042, + "velocityY": -0.21357607145609414, + "timestamp": 6.043790699400353 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": 4.739357818257744e-26, + "angularVelocity": -0.2574960528856741, + "velocityX": 3.6592893702458946, + "velocityY": 0.030164951189468058, + "timestamp": 6.085405704739497 + }, + { + "x": 7.411732273884802, + "y": 4.005484253819352, + "heading": -0.0035958440097681034, + "angularVelocity": -0.05461699297217377, + "velocityX": 3.263224331051482, + "velocityY": 0.11555138753593294, + "timestamp": 6.1512431632682025 + }, + { + "x": 7.599754753311007, + "y": 4.014664501087166, + "heading": -0.0036927014054620503, + "angularVelocity": -0.0014711593955546794, + "velocityX": 2.855858710649135, + "velocityY": 0.13943805658615363, + "timestamp": 6.217080621796908 + }, + { + "x": 7.7609064467329985, + "y": 4.024479661032519, + "heading": -0.0024912592269265435, + "angularVelocity": 0.018248611130875694, + "velocityX": 2.4477204470419824, + "velocityY": 0.14908169550731518, + "timestamp": 6.282918080325614 + }, + { + "x": 7.895178919331714, + "y": 4.034515490111863, + "heading": -0.0009603004800075886, + "angularVelocity": 0.023253612474294444, + "velocityX": 2.039454067628866, + "velocityY": 0.15243342169669197, + "timestamp": 6.34875553885432 + }, + { + "x": 8.002571195010699, + "y": 4.044538744835798, + "heading": 0.0003549668988073664, + "angularVelocity": 0.01997749318105112, + "velocityX": 1.6311728623631503, + "velocityY": 0.1522424307974114, + "timestamp": 6.414592997383026 + }, + { + "x": 8.083084077776899, + "y": 4.054399862715161, + "heading": 0.001104922913523553, + "angularVelocity": 0.011391023157268422, + "velocityX": 1.2229038690959988, + "velocityY": 0.1497797469667492, + "timestamp": 6.480430455911732 + }, + { + "x": 8.136718786187338, + "y": 4.063994789685242, + "heading": 0.0010462868235224848, + "angularVelocity": -0.0008906189775764543, + "velocityX": 0.814653384395943, + "velocityY": 0.1457365940985966, + "timestamp": 6.546267914440437 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -6.959895417141788e-26, + "angularVelocity": -0.015891968598184786, + "velocityX": 0.40642176825516096, + "velocityY": 0.14053042740563873, + "timestamp": 6.612105372969143 + }, + { + "x": 8.158055086430046, + "y": 4.083508099899822, + "heading": -0.0027577804882154954, + "angularVelocity": -0.035854270657099625, + "velocityX": -0.07048533094049422, + "velocityY": 0.13340649729451912, + "timestamp": 6.689021746881477 + }, + { + "x": 8.11595164588489, + "y": 4.093221294829269, + "heading": -0.007050996192854494, + "angularVelocity": -0.05581666797673314, + "velocityX": -0.5473924263921114, + "velocityY": 0.12628253823454794, + "timestamp": 6.765938120793811 + }, + { + "x": 8.037166241278735, + "y": 4.102386537647484, + "heading": -0.012879673428225855, + "angularVelocity": -0.07577940741219352, + "velocityX": -1.024299516458623, + "velocityY": 0.11915854000945891, + "timestamp": 6.8428544947061445 + }, + { + "x": 7.921698873219381, + "y": 4.111003824490319, + "heading": -0.02024386647880979, + "angularVelocity": -0.09574285260739591, + "velocityX": -1.5012065986230485, + "velocityY": 0.11203449154606203, + "timestamp": 6.919770868618478 + }, + { + "x": 7.769549542634506, + "y": 4.119073150509933, + "heading": -0.02914366903151699, + "angularVelocity": -0.11570751583857591, + "velocityX": -1.97811366872662, + "velocityY": 0.10491038005524037, + "timestamp": 6.996687242530812 + }, + { + "x": 7.580718251048934, + "y": 4.126594509566205, + "heading": -0.039579227850997996, + "angularVelocity": -0.13567408717648377, + "velocityX": -2.4550207190057343, + "velocityY": 0.09778618873589355, + "timestamp": 7.073603616443146 + }, + { + "x": 7.355205001381359, + "y": 4.133567893300724, + "heading": -0.051550757950875034, + "angularVelocity": -0.15564345393507104, + "velocityX": -2.931927731338533, + "velocityY": 0.09066188874772477, + "timestamp": 7.1505199903554795 + }, + { + "x": 7.093009801840762, + "y": 4.139993286528261, + "heading": -0.06505855463043991, + "angularVelocity": -0.1756166599189986, + "velocityX": -3.4088346369452576, + "velocityY": 0.08353739133439493, + "timestamp": 7.227436364267813 + }, + { + "x": 6.806528186494828, + "y": 4.135714113450545, + "heading": -0.06505855526802462, + "angularVelocity": -8.289323637518806e-9, + "velocityX": -3.724585556678138, + "velocityY": -0.05563409791773408, + "timestamp": 7.304352738180147 + }, + { + "x": 6.520046573292198, + "y": 4.131434796884326, + "heading": -0.0650585559056006, + "angularVelocity": -8.28921001864462e-9, + "velocityX": -3.7245855288127556, + "velocityY": -0.055635963430836564, + "timestamp": 7.381269112092481 + }, + { + "x": 6.233564957107684, + "y": 4.127155679947005, + "heading": -0.06505855654317651, + "angularVelocity": -8.289209203416631e-9, + "velocityX": -3.7245855675806383, + "velocityY": -0.05563336802900127, + "timestamp": 7.4581854860048145 + }, + { + "x": 5.947529123843822, + "y": 4.143694400517207, + "heading": -0.06505855718976304, + "angularVelocity": -8.406357481668178e-9, + "velocityX": -3.718789884581325, + "velocityY": 0.2150221042537956, + "timestamp": 7.535101859917148 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.06505855788186755, + "angularVelocity": -8.99814278746362e-9, + "velocityX": -3.660825136452102, + "velocityY": 0.6884707552333696, + "timestamp": 7.612018233829482 + }, + { + "x": 5.429536143877612, + "y": 4.268856603157875, + "heading": -0.06505855854289869, + "angularVelocity": -9.961057942866914e-9, + "velocityX": -3.5625392585748106, + "velocityY": 1.088093052224975, + "timestamp": 7.678379771986332 + }, + { + "x": 5.202528472751969, + "y": 4.366702264809053, + "heading": -0.06505855916311704, + "angularVelocity": -9.346051554197093e-9, + "velocityX": -3.420771691413992, + "velocityY": 1.4744332992992566, + "timestamp": 7.744741310143183 + }, + { + "x": 4.9875020778910555, + "y": 4.485753657004608, + "heading": -0.06936550467343977, + "angularVelocity": -0.06490123089285486, + "velocityX": -3.240226203809265, + "velocityY": 1.7939818078684182, + "timestamp": 7.811102848300033 + }, + { + "x": 4.796367419741578, + "y": 4.59157781663034, + "heading": -0.07462073630187449, + "angularVelocity": -0.07919092556314197, + "velocityX": -2.880202350007574, + "velocityY": 1.5946610425998575, + "timestamp": 7.877464386456883 + }, + { + "x": 4.629125206769459, + "y": 4.684173938079997, + "heading": -0.07977888872827656, + "angularVelocity": -0.07772804202052125, + "velocityX": -2.5201678203544975, + "velocityY": 1.3953281376751576, + "timestamp": 7.9438259246137335 + }, + { + "x": 4.485775178787585, + "y": 4.763541978929841, + "heading": -0.08449510558147229, + "angularVelocity": -0.07106852831000718, + "velocityX": -2.1601372114530464, + "velocityY": 1.1959945934684657, + "timestamp": 8.010187462770583 + }, + { + "x": 4.36631711494158, + "y": 4.829681959675743, + "heading": -0.08859775332946503, + "angularVelocity": -0.0618226741263267, + "velocityX": -1.8001099305995214, + "velocityY": 0.9966613581134071, + "timestamp": 8.076549000927432 + }, + { + "x": 4.270750854060208, + "y": 4.8825939059492045, + "heading": -0.09198414721152182, + "angularVelocity": -0.051029466406471824, + "velocityX": -1.440085078430444, + "velocityY": 0.7973285089986916, + "timestamp": 8.142910539084282 + }, + { + "x": 4.199076276840986, + "y": 4.9222778404051555, + "heading": -0.09458595257220126, + "angularVelocity": -0.03920652584227169, + "velocityX": -1.0800620240268282, + "velocityY": 0.5979960012704257, + "timestamp": 8.209272077241131 + }, + { + "x": 4.151293292248578, + "y": 4.948733781946395, + "heading": -0.09635439465344492, + "angularVelocity": -0.026648599932446167, + "velocityX": -0.7200403414319525, + "velocityY": 0.3986637783878444, + "timestamp": 8.27563361539798 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.013539177422752055, + "velocityX": -0.360019736526293, + "velocityY": 0.19933179122763733, + "timestamp": 8.34199515355483 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 3.915017632839986e-26, + "velocityX": -2.9397369327092406e-26, + "velocityY": 2.263261398601171e-26, + "timestamp": 8.40835669171168 + }, + { + "x": 4.148029983348262, + "y": 4.951774833695287, + "heading": -0.10587352183363513, + "angularVelocity": -0.1412289747597874, + "velocityX": 0.33794369238887556, + "velocityY": -0.16688855115376777, + "timestamp": 8.469396903578264 + }, + { + "x": 4.189291025705269, + "y": 4.931399019893449, + "heading": -0.12288563814036063, + "angularVelocity": -0.27870342822382294, + "velocityX": 0.6759649269762051, + "velocityY": -0.333809683465285, + "timestamp": 8.53043711544485 + }, + { + "x": 4.251190863307712, + "y": 4.900831857369696, + "heading": -0.14799474838457438, + "angularVelocity": -0.41135358932067295, + "velocityX": 1.0140829415490538, + "velocityY": -0.5007709113225891, + "timestamp": 8.591477327311434 + }, + { + "x": 4.333736987997449, + "y": 4.860070212810069, + "heading": -0.18081174216294543, + "angularVelocity": -0.5376290935899647, + "velocityX": 1.3523236922925204, + "velocityY": -0.6677834711439198, + "timestamp": 8.65251753917802 + }, + { + "x": 4.436939103038805, + "y": 4.809109928035341, + "heading": -0.22080172732119155, + "angularVelocity": -0.6551416506491224, + "velocityX": 1.690723408151419, + "velocityY": -0.8348641529310338, + "timestamp": 8.713557751044604 + }, + { + "x": 4.560810084348815, + "y": 4.747945301360263, + "heading": -0.2671863409578564, + "angularVelocity": -0.7599025661648731, + "velocityX": 2.0293340655624834, + "velocityY": -1.0020382433921913, + "timestamp": 8.774597962911189 + }, + { + "x": 4.705367311819798, + "y": 4.676568329714506, + "heading": -0.3187309488078688, + "angularVelocity": -0.8444369092734083, + "velocityX": 2.368229451544859, + "velocityY": -1.169343445297431, + "timestamp": 8.835638174777774 + }, + { + "x": 4.870632910411114, + "y": 4.594968271425701, + "heading": -0.37317308736223054, + "angularVelocity": -0.891906120400698, + "velocityX": 2.7074873028379653, + "velocityY": -1.3368246241864, + "timestamp": 8.896678386644359 + }, + { + "x": 5.056607795018724, + "y": 4.503143287545809, + "heading": -0.4249356353221189, + "angularVelocity": -0.848007344290121, + "velocityX": 3.0467601425449904, + "velocityY": -1.5043359299046106, + "timestamp": 8.957718598510944 + }, + { + "x": 5.260529254341505, + "y": 4.402568167992326, + "heading": -0.4249356801729229, + "angularVelocity": -7.347747104408298e-7, + "velocityX": 3.3407724692779857, + "velocityY": -1.6476862788961175, + "timestamp": 9.018758810377529 + }, + { + "x": 5.4644220623717725, + "y": 4.301934949251132, + "heading": -0.42493568721051356, + "angularVelocity": -1.1529433576821183e-7, + "velocityX": 3.3403030853811013, + "velocityY": -1.648638097147308, + "timestamp": 9.079799022244114 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.4249356942701639, + "angularVelocity": -1.1565573156948564e-7, + "velocityX": 3.301588580483846, + "velocityY": -1.7248608986943215, + "timestamp": 9.140839234110699 + }, + { + "x": 5.864577127291663, + "y": 4.06418051122573, + "heading": -0.42493570026957905, + "angularVelocity": -9.360477410609904e-8, + "velocityX": 3.09901633677209, + "velocityY": -2.0668164546168653, + "timestamp": 9.204932280705526 + }, + { + "x": 6.060491998416757, + "y": 3.9277351651374044, + "heading": -0.4249357062560635, + "angularVelocity": -9.340302536538937e-8, + "velocityX": 3.056725831174745, + "velocityY": -2.1288634779819966, + "timestamp": 9.269025327300353 + }, + { + "x": 6.256406839411796, + "y": 3.7912897757868893, + "heading": -0.4249357122425479, + "angularVelocity": -9.340302451557142e-8, + "velocityX": 3.0567253610760865, + "velocityY": -2.1288641529723815, + "timestamp": 9.33311837389518 + }, + { + "x": 6.4523216804065155, + "y": 3.654844386435917, + "heading": -0.4249357182290323, + "angularVelocity": -9.340302467625888e-8, + "velocityX": 3.05672536107112, + "velocityY": -2.1288641529795123, + "timestamp": 9.397211420490008 + }, + { + "x": 6.648236521401236, + "y": 3.5183989970849443, + "heading": -0.4249357242155167, + "angularVelocity": -9.340302391578915e-8, + "velocityX": 3.05672536107112, + "velocityY": -2.1288641529795123, + "timestamp": 9.461304467084835 + }, + { + "x": 6.844151362395955, + "y": 3.3819536077339722, + "heading": -0.4249357302020011, + "angularVelocity": -9.340302522406469e-8, + "velocityX": 3.0567253610711203, + "velocityY": -2.128864152979512, + "timestamp": 9.525397513679662 + }, + { + "x": 7.040066203390847, + "y": 3.2455082183832458, + "heading": -0.4249357361884855, + "angularVelocity": -9.340302460522487e-8, + "velocityX": 3.056725361073794, + "velocityY": -2.1288641529756727, + "timestamp": 9.58949056027449 + }, + { + "x": 7.235981060605469, + "y": 3.1090628523216983, + "heading": -0.4249357421751277, + "angularVelocity": -9.340548585202334e-8, + "velocityX": 3.0567256141391645, + "velocityY": -2.128863789610536, + "timestamp": 9.653583606869317 + }, + { + "x": 7.417177244420043, + "y": 2.9830454990225337, + "heading": -0.48590548097831143, + "angularVelocity": -0.9512691632309586, + "velocityX": 2.8270802129290376, + "velocityY": -1.966162633769009, + "timestamp": 9.717676653464144 + }, + { + "x": 7.5775843375603, + "y": 2.871507967151711, + "heading": -0.5361633770520781, + "angularVelocity": -0.7841396024045931, + "velocityX": 2.502722239969217, + "velocityY": -1.7402438766239745, + "timestamp": 9.781769700058971 + }, + { + "x": 7.717202062000419, + "y": 2.774449785512018, + "heading": -0.5757086599849341, + "angularVelocity": -0.6169980213742552, + "velocityX": 2.1783599291625744, + "velocityY": -1.514332471247, + "timestamp": 9.845862746653799 + }, + { + "x": 7.836030204991866, + "y": 2.691870623138966, + "heading": -0.6045409513138722, + "angularVelocity": -0.44985053544427944, + "velocityX": 1.8539942989859028, + "velocityY": -1.2884262296827267, + "timestamp": 9.909955793248626 + }, + { + "x": 7.934068628103717, + "y": 2.6237702758314256, + "heading": -0.6226600347771661, + "angularVelocity": -0.28269967533040424, + "velocityX": 1.5296265089661738, + "velocityY": -1.0625231741291041, + "timestamp": 9.974048839843453 + }, + { + "x": 8.011317274299664, + "y": 2.5701486603074475, + "heading": -0.6300657818408324, + "angularVelocity": -0.11554680978863106, + "velocityX": 1.2052578290478972, + "velocityY": -0.836621417966838, + "timestamp": 10.03814188643828 + }, + { + "x": 8.06777617103265, + "y": 2.5310058125218204, + "heading": -0.6267581325303803, + "angularVelocity": 0.051606991494130075, + "velocityX": 0.8808895774591454, + "velocityY": -0.6107191008265523, + "timestamp": 10.102234933033108 + }, + { + "x": 8.10344542848898, + "y": 2.5063418886445565, + "heading": -0.612737112733536, + "angularVelocity": 0.21876038886839116, + "velocityX": 0.5565230450320063, + "velocityY": -0.3848143470723113, + "timestamp": 10.166327979627935 + }, + { + "x": 8.118325233459473, + "y": 2.496157169342041, + "heading": -0.5880028838627427, + "angularVelocity": 0.38591126783462537, + "velocityX": 0.23215942697429998, + "velocityY": -0.15890521427230228, + "timestamp": 10.230421026222762 + }, + { + "x": 8.111774217431309, + "y": 2.500903203120534, + "heading": -0.5514605400771017, + "angularVelocity": 0.5570006800274493, + "velocityX": -0.09985457977086463, + "velocityY": 0.0723419400124114, + "timestamp": 10.296026590265951 + }, + { + "x": 8.083429359560292, + "y": 2.5208307056412647, + "heading": -0.504313305894281, + "angularVelocity": 0.7186468841542555, + "velocityX": -0.432049602566591, + "velocityY": 0.3037471411359584, + "timestamp": 10.36163215430914 + }, + { + "x": 8.033276752823499, + "y": 2.5559518951025, + "heading": -0.44733468663642767, + "angularVelocity": 0.8685028486355681, + "velocityX": -0.7644566046833565, + "velocityY": 0.5353385794856407, + "timestamp": 10.427237718352329 + }, + { + "x": 7.9612999822666515, + "y": 2.606281084085956, + "heading": -0.38151750427277176, + "angularVelocity": 1.0032256154421146, + "velocityX": -1.097113813539709, + "velocityY": 0.7671481789307238, + "timestamp": 10.492843282395517 + }, + { + "x": 7.867479640224459, + "y": 2.6718348304324313, + "heading": -0.3081892028790952, + "angularVelocity": 1.117714670441717, + "velocityX": -1.430066845861252, + "velocityY": 0.999210163078856, + "timestamp": 10.558448846438706 + }, + { + "x": 7.751793438011374, + "y": 2.75263175880398, + "heading": -0.2292374531912587, + "angularVelocity": 1.2034306973698263, + "velocityX": -1.763359615914976, + "velocityY": 1.2315560356795054, + "timestamp": 10.624054410481895 + }, + { + "x": 7.614220064693883, + "y": 2.8486908030805904, + "heading": -0.14760413079417548, + "angularVelocity": 1.244304863278703, + "velocityX": -2.0969772202083976, + "velocityY": 1.4641905100209518, + "timestamp": 10.689659974525084 + }, + { + "x": 7.454764083734287, + "y": 2.9600197368919443, + "heading": -0.06854432318452272, + "angularVelocity": 1.2050777820857683, + "velocityX": -2.4305252654275167, + "velocityY": 1.696943474764787, + "timestamp": 10.755265538568272 + }, + { + "x": 7.27363003976687, + "y": 3.0865189810411944, + "heading": -0.0037505079236450347, + "angularVelocity": 0.987626830221633, + "velocityX": -2.760955516641443, + "velocityY": 1.928178592687242, + "timestamp": 10.820871102611461 + }, + { + "x": 7.07280859084814, + "y": 3.2235171993435245, + "heading": -7.171638886724292e-8, + "angularVelocity": 0.057166434919867046, + "velocityX": -3.0610429442619163, + "velocityY": 2.0882103568554258, + "timestamp": 10.88647666665465 + }, + { + "x": 6.872223980539225, + "y": 3.3631158565871417, + "heading": -6.147329807763224e-8, + "angularVelocity": 1.5613143395684282e-7, + "velocityX": -3.0574329057953955, + "velocityY": 2.12784783241428, + "timestamp": 10.952082230697838 + }, + { + "x": 6.671639384359248, + "y": 3.502714534132154, + "heading": -5.123020973688295e-8, + "angularVelocity": 1.561313966298048e-7, + "velocityX": -3.0574326904335125, + "velocityY": 2.1278481418605137, + "timestamp": 11.017687794741027 + }, + { + "x": 6.471054788179423, + "y": 3.6423132116773846, + "heading": -4.098712138128988e-8, + "angularVelocity": 1.561313968560626e-7, + "velocityX": -3.057432690431191, + "velocityY": 2.1278481418638497, + "timestamp": 11.083293358784216 + }, + { + "x": 6.270470191999598, + "y": 3.781911889222615, + "heading": -3.074403308674018e-8, + "angularVelocity": 1.5613139592560247e-7, + "velocityX": -3.057432690431191, + "velocityY": 2.1278481418638493, + "timestamp": 11.148898922827405 + }, + { + "x": 6.069885595819435, + "y": 3.921510566767361, + "heading": -2.0500944793387612e-8, + "angularVelocity": 1.5613139590735494e-7, + "velocityX": -3.0574326904363347, + "velocityY": 2.1278481418564588, + "timestamp": 11.214504486870593 + }, + { + "x": 5.869300968338538, + "y": 4.0611091993371335, + "heading": -1.0257856528303386e-8, + "angularVelocity": 1.5613139547647166e-7, + "velocityX": -3.057433167541219, + "velocityY": 2.127847456320497, + "timestamp": 11.280110050913782 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 3.3501373579040154e-26, + "angularVelocity": 1.563565023471243e-7, + "velocityX": -3.099573069501661, + "velocityY": 2.0659814025542267, + "timestamp": 11.345715614956971 + }, + { + "x": 5.446743415639457, + "y": 4.310089409453816, + "heading": -2.545629671266297e-9, + "angularVelocity": -3.841829867702764e-8, + "velocityX": -3.3082622124289722, + "velocityY": 1.712026190368146, + "timestamp": 11.411976479152617 + }, + { + "x": 5.225460228694709, + "y": 4.4194271674986805, + "heading": -5.081465433806644e-9, + "angularVelocity": -3.827049033125913e-8, + "velocityX": -3.3395759266189464, + "velocityY": 1.650110655394207, + "timestamp": 11.478237343348264 + }, + { + "x": 5.005890824462741, + "y": 4.527909625118252, + "heading": -0.005806060945576971, + "angularVelocity": -0.08762420977438817, + "velocityX": -3.3137117497238457, + "velocityY": 1.6372025770635645, + "timestamp": 11.54449820754391 + }, + { + "x": 4.810655372355854, + "y": 4.624369173325347, + "heading": -0.020290065438414114, + "angularVelocity": -0.21859063670028128, + "velocityX": -2.946467035661095, + "velocityY": 1.455754454428526, + "timestamp": 11.610759071739556 + }, + { + "x": 4.639828140240907, + "y": 4.708771468826467, + "heading": -0.03646660670869356, + "angularVelocity": -0.24413417281301306, + "velocityX": -2.5781014809971645, + "velocityY": 1.2737880274532598, + "timestamp": 11.677019935935203 + }, + { + "x": 4.493411439660615, + "y": 4.781114493443874, + "heading": -0.05214975334121426, + "angularVelocity": -0.23668792767648847, + "velocityX": -2.2097010408432496, + "velocityY": 1.091791142412547, + "timestamp": 11.743280800130849 + }, + { + "x": 4.3714027429262465, + "y": 4.841398729732693, + "heading": -0.06627328599748628, + "angularVelocity": -0.21315044450024043, + "velocityX": -1.8413387482257655, + "velocityY": 0.9098015400285159, + "timestamp": 11.809541664326495 + }, + { + "x": 4.2737994273361055, + "y": 4.889624930300945, + "heading": -0.07820644553038021, + "angularVelocity": -0.1800936295919612, + "velocityX": -1.4730160370675427, + "velocityY": 0.7278232959029468, + "timestamp": 11.875802528522142 + }, + { + "x": 4.20059930110606, + "y": 4.925793789498138, + "heading": -0.08753245479698174, + "angularVelocity": -0.14074687041607162, + "velocityX": -1.1047264040189804, + "velocityY": 0.5458555308062137, + "timestamp": 11.942063392717788 + }, + { + "x": 4.151800585484216, + "y": 4.949905897329157, + "heading": -0.09395516408530516, + "angularVelocity": -0.09693065984408723, + "velocityX": -0.7364636156533751, + "velocityY": 0.363896669983421, + "timestamp": 12.008324256913435 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.04976861149222346, + "velocityX": -0.36822273621886326, + "velocityY": 0.1819452407240944, + "timestamp": 12.07458512110908 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -1.4138878397118624e-28, + "velocityX": 4.127775100583887e-27, + "velocityY": 5.8037818722321745e-27, + "timestamp": 12.140845985304727 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAGF.1.traj b/src/main/deploy/choreo/CenterLanePCBAGF.1.traj new file mode 100644 index 00000000..f6191fee --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAGF.1.traj @@ -0,0 +1,275 @@ +{ + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -9.687220582798775e-29, + "angularVelocity": -9.134521375874455e-29, + "velocityX": 4.127432494607337e-27, + "velocityY": -1.3653566194328874e-27, + "timestamp": 0 + }, + { + "x": 1.3104781299456403, + "y": 5.567275491596599, + "heading": -0.013632838685405388, + "angularVelocity": -0.19128951975675332, + "velocityX": 0.2876978871987266, + "velocityY": -0.33224984965521687, + "timestamp": 0.07126808987100346 + }, + { + "x": 1.3514857086951348, + "y": 5.519918181736226, + "heading": -0.040881942077731624, + "angularVelocity": -0.3823464813164996, + "velocityX": 0.575398875200937, + "velocityY": -0.6644952873872657, + "timestamp": 0.1425361797420069 + }, + { + "x": 1.4129978772139142, + "y": 5.448882692998005, + "heading": -0.08170986553530107, + "angularVelocity": -0.5728780374424056, + "velocityX": 0.8631095435575385, + "velocityY": -0.9967362513404827, + "timestamp": 0.21380426961301036 + }, + { + "x": 1.4950158086435297, + "y": 5.354169496620718, + "heading": -0.13606373855442938, + "angularVelocity": -0.7626677397627718, + "velocityX": 1.150836672879393, + "velocityY": -1.3289706030948822, + "timestamp": 0.2850723594840138 + }, + { + "x": 1.5975411283703898, + "y": 5.235779380543047, + "heading": -0.20388316047242194, + "angularVelocity": -0.9516099286615785, + "velocityX": 1.438586608851632, + "velocityY": -1.6611938988677692, + "timestamp": 0.3563404493550173 + }, + { + "x": 1.7205758185191724, + "y": 5.093713591292254, + "heading": -0.28510990320243546, + "angularVelocity": -1.1397350886916635, + "velocityX": 1.7263643570562608, + "velocityY": -1.9933997039619569, + "timestamp": 0.42760853922602077 + }, + { + "x": 1.8641220914189134, + "y": 4.92797393358567, + "heading": -0.3796975284198533, + "angularVelocity": -1.3272086481989789, + "velocityX": 2.0141731476115345, + "velocityY": -2.3255801861194376, + "timestamp": 0.49887662909702424 + }, + { + "x": 2.036057344584012, + "y": 4.766214838118392, + "heading": -0.4690840637578516, + "angularVelocity": -1.254229424414062, + "velocityX": 2.412513840013176, + "velocityY": -2.2697268266915005, + "timestamp": 0.5701447189680277 + }, + { + "x": 2.1874785084173527, + "y": 4.628126850696092, + "heading": -0.5451442530827, + "angularVelocity": -1.067240464316059, + "velocityX": 2.1246698783062077, + "velocityY": -1.9375850773079741, + "timestamp": 0.6414128088390311 + }, + { + "x": 2.3183845436377744, + "y": 4.5137087594261915, + "heading": -0.6078811711978208, + "angularVelocity": -0.8802946483998079, + "velocityX": 1.836811333899418, + "velocityY": -1.605460332625711, + "timestamp": 0.7126808987100345 + }, + { + "x": 2.4287747758121854, + "y": 4.422959692661206, + "heading": -0.6572940075938836, + "angularVelocity": -0.6933374598014469, + "velocityX": 1.548943326167706, + "velocityY": -1.273347818487104, + "timestamp": 0.7839489885810379 + }, + { + "x": 2.5186487653652114, + "y": 4.355879041329831, + "heading": -0.6933804923945726, + "angularVelocity": -0.506348421376329, + "velocityX": 1.2610691505230385, + "velocityY": -0.9412438505478742, + "timestamp": 0.8552170784520413 + }, + { + "x": 2.58800626172225, + "y": 4.312466418462692, + "heading": -0.7161379527262242, + "angularVelocity": -0.31932187845700755, + "velocityX": 0.9731914589345287, + "velocityY": -0.6091453123791657, + "timestamp": 0.9264851683230447 + }, + { + "x": 2.6368471855998568, + "y": 4.292721634612479, + "heading": -0.7255636953209235, + "angularVelocity": -0.1322575448810274, + "velocityX": 0.685312654878354, + "velocityY": -0.27704943244516045, + "timestamp": 0.9977532581940481 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05484334197647404, + "velocityX": 0.3974350607879547, + "velocityY": 0.05504641764931743, + "timestamp": 1.0690213480650517 + }, + { + "x": 2.672652235661854, + "y": 4.3255208850960845, + "heading": -0.7036434508654703, + "angularVelocity": 0.2466181683415971, + "velocityX": 0.1024255678097115, + "velocityY": 0.3953768419726611, + "timestamp": 1.1420559681490803 + }, + { + "x": 2.6585872493180758, + "y": 4.379253606612061, + "heading": -0.6716320440965468, + "angularVelocity": 0.43830455655267986, + "velocityX": -0.19257971531304016, + "velocityY": 0.7357157667713767, + "timestamp": 1.215090588233109 + }, + { + "x": 2.6229771758211826, + "y": 4.457843745767889, + "heading": -0.6256270362767148, + "angularVelocity": 0.6299068546793523, + "velocityX": -0.48757799323009304, + "velocityY": 1.0760669264166411, + "timestamp": 1.2881252083171375 + }, + { + "x": 2.5658227847055692, + "y": 4.561292508951217, + "heading": -0.5656323531358879, + "angularVelocity": 0.821455401175512, + "velocityX": -0.7825657345770498, + "velocityY": 1.4164346040864937, + "timestamp": 1.3611598284011661 + }, + { + "x": 2.4871252028274746, + "y": 4.689601433735054, + "heading": -0.49164521078608, + "angularVelocity": 1.0130420650464593, + "velocityX": -1.077538046854364, + "velocityY": 1.7568233344161037, + "timestamp": 1.4341944484851947 + }, + { + "x": 2.3868860574551665, + "y": 4.842772331616221, + "heading": -0.40364732106038387, + "angularVelocity": 1.2048791329981834, + "velocityX": -1.3724880783521554, + "velocityY": 2.097236868007793, + "timestamp": 1.5072290685692233 + }, + { + "x": 2.26510760033775, + "y": 5.020807045399027, + "heading": -0.30159033464993307, + "angularVelocity": 1.3973782062949196, + "velocityX": -1.6674072785934422, + "velocityY": 2.437675633527935, + "timestamp": 1.580263688653252 + }, + { + "x": 2.1359139807220116, + "y": 5.17000455286456, + "heading": -0.21560374484110897, + "angularVelocity": 1.1773401396473877, + "velocityX": -1.7689366969677838, + "velocityY": 2.042832663384521, + "timestamp": 1.6532983087372806 + }, + { + "x": 2.028255013080415, + "y": 5.294335941405348, + "heading": -0.1438207992307524, + "angularVelocity": 0.9828619020372543, + "velocityX": -1.474081298947423, + "velocityY": 1.7023623645572525, + "timestamp": 1.7263329288213092 + }, + { + "x": 1.9421291374445764, + "y": 5.393801086818426, + "heading": -0.08631852593953135, + "angularVelocity": 0.7873289848713239, + "velocityX": -1.1792472602273896, + "velocityY": 1.3618903651260308, + "timestamp": 1.7993675489053378 + }, + { + "x": 1.87753522481553, + "y": 5.468400016112323, + "heading": -0.043158813254579896, + "angularVelocity": 0.5909486848195399, + "velocityX": -0.8844286799154853, + "velocityY": 1.0214187354992572, + "timestamp": 1.8724021689893664 + }, + { + "x": 1.834472632995246, + "y": 5.518132767413434, + "heading": -0.01438145137891416, + "angularVelocity": 0.3940235718698405, + "velocityX": -0.5896188926667782, + "velocityY": 0.680947627904294, + "timestamp": 1.945436789073395 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -2.0788362526419016e-27, + "angularVelocity": 0.19691279782612486, + "velocityX": -0.29481141121630017, + "velocityY": 0.34047551881670896, + "timestamp": 2.0184714091574234 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -9.689019627790258e-28, + "angularVelocity": 4.903263424537125e-28, + "velocityX": -7.04775279749202e-27, + "velocityY": -4.880044267923792e-28, + "timestamp": 2.091506029241452 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAGF.2.traj b/src/main/deploy/choreo/CenterLanePCBAGF.2.traj new file mode 100644 index 00000000..af8452c1 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAGF.2.traj @@ -0,0 +1,86 @@ +{ + "samples": [ + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -9.689019627790258e-28, + "angularVelocity": 4.903263424537125e-28, + "velocityX": -7.04775279749202e-27, + "velocityY": -4.880044267923792e-28, + "timestamp": 0 + }, + { + "x": 1.8720264471920118, + "y": 5.543082773692806, + "heading": 4.603106809112672e-20, + "angularVelocity": 4.715795015985417e-19, + "velocityX": 0.6053171264251316, + "velocityY": 0.0008555041789240918, + "timestamp": 0.09761041119112068 + }, + { + "x": 1.9901969516410831, + "y": 5.543249785918246, + "heading": 4.290138941133409e-20, + "angularVelocity": -3.206295969142147e-20, + "velocityX": 1.210634224434258, + "velocityY": 0.0017110083176873983, + "timestamp": 0.19522082238224137 + }, + { + "x": 2.1674527004558795, + "y": 5.543500304245301, + "heading": 2.9796741833873356e-20, + "angularVelocity": -1.3425460888073973e-19, + "velocityX": 1.8159512561393765, + "velocityY": 0.0025665123627422117, + "timestamp": 0.29283123357336205 + }, + { + "x": 2.4037936612766035, + "y": 5.543834328628234, + "heading": -1.7822834862290494e-20, + "angularVelocity": -4.878534586092089e-19, + "velocityX": 2.421267956324555, + "velocityY": 0.0034220159392546997, + "timestamp": 0.39044164476448273 + }, + { + "x": 2.5810494100914, + "y": 5.544084846955289, + "heading": -6.57207439293443e-21, + "angularVelocity": 1.1526188991905598e-19, + "velocityX": 1.8159512561393765, + "velocityY": 0.0025665123627422117, + "timestamp": 0.4880520559556034 + }, + { + "x": 2.699219914540471, + "y": 5.544251859180729, + "heading": -1.8054002593168886e-20, + "angularVelocity": -1.1763015911739438e-19, + "velocityX": 1.210634224434258, + "velocityY": 0.0017110083176873983, + "timestamp": 0.5856624671467241 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -3.155242334732113e-28, + "angularVelocity": 1.84959801497032e-19, + "velocityX": 0.6053171264251316, + "velocityY": 0.0008555041789240918, + "timestamp": 0.6832728783378448 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -3.309635664969631e-28, + "angularVelocity": -1.5501074190844928e-28, + "velocityX": -3.9535178243549605e-27, + "velocityY": -3.789490097440697e-27, + "timestamp": 0.7808832895289655 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAGF.3.traj b/src/main/deploy/choreo/CenterLanePCBAGF.3.traj new file mode 100644 index 00000000..3b05b4a3 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAGF.3.traj @@ -0,0 +1,176 @@ +{ + "samples": [ + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -3.309635664969631e-28, + "angularVelocity": -1.5501074190844928e-28, + "velocityX": -3.9535178243549605e-27, + "velocityY": -3.789490097440697e-27, + "timestamp": 0 + }, + { + "x": 2.7303741375078747, + "y": 5.568056872076371, + "heading": 0.006778509195380246, + "angularVelocity": 0.08813497362119738, + "velocityX": -0.3631625447521428, + "velocityY": 0.30842982050092854, + "timestamp": 0.07691054886467841 + }, + { + "x": 2.674756164732603, + "y": 5.615779956875509, + "heading": 0.020597402851140194, + "angularVelocity": 0.17967488023097886, + "velocityX": -0.7231514219607195, + "velocityY": 0.6205011601608272, + "timestamp": 0.15382109772935681 + }, + { + "x": 2.591861664751003, + "y": 5.687962530350053, + "heading": 0.041894082151838256, + "angularVelocity": 0.27690192847497247, + "velocityX": -1.0778040360555798, + "velocityY": 0.9385263080302098, + "timestamp": 0.23073164659403522 + }, + { + "x": 2.482522988073185, + "y": 5.785486691208239, + "heading": 0.07152941986388299, + "angularVelocity": 0.38532214565504047, + "velocityX": -1.4216343309445878, + "velocityY": 1.2680206070272062, + "timestamp": 0.30764219545871363 + }, + { + "x": 2.3492932233323143, + "y": 5.910724338795779, + "heading": 0.11194462213648758, + "angularVelocity": 0.5254832122406744, + "velocityX": -1.7322690672157808, + "velocityY": 1.628354620221101, + "timestamp": 0.38455274432339204 + }, + { + "x": 2.2322888283047253, + "y": 6.068263103797439, + "heading": 0.17809343518700713, + "angularVelocity": 0.8600746455067716, + "velocityX": -1.5213049023152156, + "velocityY": 2.0483375470228995, + "timestamp": 0.46146329318807044 + }, + { + "x": 2.1483049194140715, + "y": 6.209825434589826, + "heading": 0.2439234240091836, + "angularVelocity": 0.85592925540035, + "velocityX": -1.0919686587911488, + "velocityY": 1.8406100708169548, + "timestamp": 0.5383738420527489 + }, + { + "x": 2.0954357223517674, + "y": 6.331973751888774, + "heading": 0.3066259975705838, + "angularVelocity": 0.8152662344371413, + "velocityX": -0.687411516921112, + "velocityY": 1.5881867845445197, + "timestamp": 0.6152843909174273 + }, + { + "x": 2.072970005140484, + "y": 6.433621819948826, + "heading": 0.36524449476389237, + "angularVelocity": 0.7621645932659499, + "velocityX": -0.29210189685177457, + "velocityY": 1.32164013338272, + "timestamp": 0.6921949397821057 + }, + { + "x": 2.080539806403903, + "y": 6.514239906298576, + "heading": 0.41928927766619223, + "angularVelocity": 0.7026966222460568, + "velocityX": 0.09842344613530389, + "velocityY": 1.0482058383381778, + "timestamp": 0.7691054886467841 + }, + { + "x": 2.1179206829564796, + "y": 6.573514734989947, + "heading": 0.4684594496501494, + "angularVelocity": 0.6393163578961122, + "velocityX": 0.48603055243236887, + "velocityY": 0.770698292579654, + "timestamp": 0.8460160375114625 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.5732759758018013, + "velocityX": 0.8716733324811171, + "velocityY": 0.49050096320339975, + "timestamp": 0.9229265863761409 + }, + { + "x": 2.2917884862212228, + "y": 6.6722766025245654, + "heading": 0.5461831512487827, + "angularVelocity": 0.4115075108626729, + "velocityX": 1.3070625376552998, + "velocityY": 0.7468097995934108, + "timestamp": 1.0046571277659586 + }, + { + "x": 2.4341165316393667, + "y": 6.754285323763742, + "heading": 0.5638405564624909, + "angularVelocity": 0.21604414841069464, + "velocityX": 1.7414303514680458, + "velocityY": 1.0034036217627857, + "timestamp": 1.0863876691557763 + }, + { + "x": 2.5412608832572645, + "y": 6.816945409398408, + "heading": 0.539566584836395, + "angularVelocity": -0.2970000101959444, + "velocityX": 1.3109463096159757, + "velocityY": 0.766666714414698, + "timestamp": 1.168118210545594 + }, + { + "x": 2.6126555762150003, + "y": 6.858741818277979, + "heading": 0.521795194780163, + "angularVelocity": -0.21743879037177083, + "velocityX": 0.8735375019384187, + "velocityY": 0.511392781313182, + "timestamp": 1.2498487519354118 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.1131128587056526, + "velocityX": 0.4366659015377109, + "velocityY": 0.2557557080208239, + "timestamp": 1.3315792933252295 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -4.764407701562395e-26, + "velocityX": 1.4812581731979975e-26, + "velocityY": -5.1560471026288735e-26, + "timestamp": 1.4133098347150472 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAGF.4.traj b/src/main/deploy/choreo/CenterLanePCBAGF.4.traj new file mode 100644 index 00000000..c449dc9f --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAGF.4.traj @@ -0,0 +1,662 @@ +{ + "samples": [ + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -4.764407701562395e-26, + "velocityX": 1.4812581731979975e-26, + "velocityY": -5.1560471026288735e-26, + "timestamp": 0 + }, + { + "x": 2.6602506971919606, + "y": 6.863647804987902, + "heading": 0.4793942538974517, + "angularVelocity": -0.5654499433512294, + "velocityX": 0.20304968657976344, + "velocityY": -0.272816224127646, + "timestamp": 0.05863678313599774 + }, + { + "x": 2.6841390735646424, + "y": 6.831518157238345, + "heading": 0.41523186966938014, + "angularVelocity": -1.0942343832071746, + "velocityX": 0.40739575220688923, + "velocityY": -0.5479435608709636, + "timestamp": 0.11727356627199548 + }, + { + "x": 2.720131144955563, + "y": 6.7831009440148105, + "heading": 0.3230087558895857, + "angularVelocity": -1.5727860371517781, + "velocityX": 0.6138138804006831, + "velocityY": -0.8257140080696285, + "timestamp": 0.17591034940799322 + }, + { + "x": 2.7684071443805327, + "y": 6.718246852790197, + "heading": 0.20639646027764702, + "angularVelocity": -1.98872259655644, + "velocityX": 0.8233057279592204, + "velocityY": -1.1060308522415998, + "timestamp": 0.23454713254399095 + }, + { + "x": 2.829185609402161, + "y": 6.636825404453338, + "heading": 0.06956994287591697, + "angularVelocity": -2.333458796407444, + "velocityX": 1.03652454604584, + "velocityY": -1.388572905645519, + "timestamp": 0.2931839156799887 + }, + { + "x": 2.9026837898482625, + "y": 6.53870263495317, + "heading": -0.0825332150617159, + "angularVelocity": -2.59398878660952, + "velocityX": 1.253448373449048, + "velocityY": -1.6733996009397407, + "timestamp": 0.35182069881598643 + }, + { + "x": 2.98910296697184, + "y": 6.423727552121541, + "heading": -0.24244596371477412, + "angularVelocity": -2.7271746521661684, + "velocityX": 1.4738048798335797, + "velocityY": -1.9608013380434501, + "timestamp": 0.41045748195198417 + }, + { + "x": 3.08851698550526, + "y": 6.291921017444985, + "heading": -0.39600744918619013, + "angularVelocity": -2.6188593108059237, + "velocityX": 1.6954207447370835, + "velocityY": -2.247847300402779, + "timestamp": 0.4690942650879819 + }, + { + "x": 3.20017208933701, + "y": 6.146474995005627, + "heading": -0.5026655989158746, + "angularVelocity": -1.8189631836096676, + "velocityX": 1.9041819462159983, + "velocityY": -2.4804570554633094, + "timestamp": 0.5277310482239796 + }, + { + "x": 3.3235802395453247, + "y": 5.987781312389556, + "heading": -0.5570167491322654, + "angularVelocity": -0.926912209531219, + "velocityX": 2.1046200628382103, + "velocityY": -2.706384527405065, + "timestamp": 0.5863678313599774 + }, + { + "x": 3.458298567775881, + "y": 5.815853629795619, + "heading": -0.5570170670277983, + "angularVelocity": -0.000005421435418997904, + "velocityX": 2.297505439855057, + "velocityY": -2.9320790363819964, + "timestamp": 0.6450046144959751 + }, + { + "x": 3.5900045442013857, + "y": 5.641607467957256, + "heading": -0.5570171376680287, + "angularVelocity": -0.0000012047084903437067, + "velocityX": 2.246132365754707, + "velocityY": -2.9716187096115108, + "timestamp": 0.7036413976319729 + }, + { + "x": 3.721710444891145, + "y": 5.467361248873177, + "heading": -0.5570172083082521, + "angularVelocity": -0.0000012047083681560634, + "velocityX": 2.246131074146577, + "velocityY": -2.9716196858880317, + "timestamp": 0.7622781807679706 + }, + { + "x": 3.8545074444088514, + "y": 5.293945142218947, + "heading": -0.5570172791057042, + "angularVelocity": -0.0000012073897706381049, + "velocityX": 2.264738828010173, + "velocityY": -2.9574628310018474, + "timestamp": 0.8209149639039683 + }, + { + "x": 4.00357560161076, + "y": 5.134299054707746, + "heading": -0.5570173534542538, + "angularVelocity": -0.00000126795069051409, + "velocityX": 2.542229454439387, + "velocityY": -2.722626975305397, + "timestamp": 0.8795517470399661 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.55701743434988, + "angularVelocity": -0.0000013796054608952668, + "velocityX": 2.795573955825942, + "velocityY": -2.461786802066348, + "timestamp": 0.9381885301759638 + }, + { + "x": 4.2902143593852005, + "y": 4.896089807808244, + "heading": -0.5570175109527072, + "angularVelocity": -0.000001846969607519395, + "velocityX": 2.958786599579285, + "velocityY": -2.2630085323028566, + "timestamp": 0.9796634016666887 + }, + { + "x": 4.419113803049847, + "y": 4.8109235045890575, + "heading": -0.5570175835802155, + "angularVelocity": -0.0000017511207539159885, + "velocityX": 3.107892539063629, + "velocityY": -2.0534434504090724, + "timestamp": 1.0211382731574137 + }, + { + "x": 4.553582724143636, + "y": 4.7348546191881615, + "heading": -0.5570176536447519, + "angularVelocity": -0.0000016893249788320206, + "velocityX": 3.2421781252260224, + "velocityY": -1.8340957468163943, + "timestamp": 1.0626131446481386 + }, + { + "x": 4.692974428091918, + "y": 4.668233908840813, + "heading": -0.5570177224005147, + "angularVelocity": -0.0000016577691599272835, + "velocityX": 3.3608712682678954, + "velocityY": -1.606290940822974, + "timestamp": 1.1040880161388635 + }, + { + "x": 4.832547821463298, + "y": 4.601994690531772, + "heading": -0.5570177911147437, + "angularVelocity": -0.0000016567677375032224, + "velocityX": 3.3652519792036677, + "velocityY": -1.5970927920500795, + "timestamp": 1.1455628876295885 + }, + { + "x": 4.9721212332499025, + "y": 4.535755511025668, + "heading": -0.5570178598289695, + "angularVelocity": -0.0000016567676584676064, + "velocityX": 3.3652524232128536, + "velocityY": -1.5970918564730696, + "timestamp": 1.1870377591203134 + }, + { + "x": 5.111694645038376, + "y": 4.469516331523502, + "heading": -0.5570179285431947, + "angularVelocity": -0.000001656767647060506, + "velocityX": 3.365252423257915, + "velocityY": -1.5970918563781218, + "timestamp": 1.2285126306110383 + }, + { + "x": 5.251268056826837, + "y": 4.4032771520213085, + "heading": -0.5570179972574194, + "angularVelocity": -0.0000016567676356654265, + "velocityX": 3.3652524232576075, + "velocityY": -1.597091856378771, + "timestamp": 1.2699875021017633 + }, + { + "x": 5.390841468488021, + "y": 4.33703797225093, + "heading": -0.5570180659716438, + "angularVelocity": -0.0000016567676246630092, + "velocityX": 3.3652524201888485, + "velocityY": -1.5970918628449942, + "timestamp": 1.3114623735924882 + }, + { + "x": 5.530413626022683, + "y": 4.27079614995943, + "heading": -0.5570181346861351, + "angularVelocity": -0.0000016567740615756042, + "velocityX": 3.365222181963228, + "velocityY": -1.5971555766318444, + "timestamp": 1.3529372450832131 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.5570182043959618, + "angularVelocity": -0.0000016807725799241125, + "velocityX": 3.2679571491481956, + "velocityY": -1.7877590150355749, + "timestamp": 1.394412116573938 + }, + { + "x": 5.786627291659718, + "y": 4.120246269797506, + "heading": -0.5570182737203393, + "angularVelocity": -0.00000180799570053011, + "velocityX": 3.1472464214958245, + "velocityY": -1.9926027126510082, + "timestamp": 1.4327553368615833 + }, + { + "x": 5.902360023994386, + "y": 4.0365449270340825, + "heading": -0.557018342200347, + "angularVelocity": -0.0000017859743423988426, + "velocityX": 3.018336265615098, + "velocityY": -2.182950261754457, + "timestamp": 1.4710985571492285 + }, + { + "x": 6.018082629955809, + "y": 3.952829584452034, + "heading": -0.5570184106790251, + "angularVelocity": -0.0000017859396699979741, + "velocityX": 3.0180721674728543, + "velocityY": -2.1833153802426746, + "timestamp": 1.5094417774368738 + }, + { + "x": 6.133805234621251, + "y": 3.869114240078507, + "heading": -0.5570184791577035, + "angularVelocity": -0.0000017859396766231999, + "velocityX": 3.018072133673387, + "velocityY": -2.183315426964841, + "timestamp": 1.547784997724519 + }, + { + "x": 6.249527839286528, + "y": 3.7853988957047506, + "heading": -0.5570185476363823, + "angularVelocity": -0.0000017859396843832207, + "velocityX": 3.01807213366906, + "velocityY": -2.1833154269708217, + "timestamp": 1.5861282180121643 + }, + { + "x": 6.365250443951803, + "y": 3.701683551330994, + "heading": -0.5570186161150613, + "angularVelocity": -0.000001785939693521326, + "velocityX": 3.0180721336690577, + "velocityY": -2.1833154269708244, + "timestamp": 1.6244714382998096 + }, + { + "x": 6.480973048617079, + "y": 3.6179682069572374, + "heading": -0.5570186845937407, + "angularVelocity": -0.0000017859397005145362, + "velocityX": 3.0180721336690555, + "velocityY": -2.183315426970826, + "timestamp": 1.6628146585874548 + }, + { + "x": 6.596695653282355, + "y": 3.534252862583481, + "heading": -0.5570187530724204, + "angularVelocity": -0.0000017859397100037096, + "velocityX": 3.0180721336690537, + "velocityY": -2.1833154269708275, + "timestamp": 1.7011578788751 + }, + { + "x": 6.71241825794763, + "y": 3.4505375182097238, + "heading": -0.5570188215511004, + "angularVelocity": -0.000001785939717860173, + "velocityX": 3.0180721336690515, + "velocityY": -2.1833154269708293, + "timestamp": 1.7395010991627453 + }, + { + "x": 6.828140862612906, + "y": 3.366822173835967, + "heading": -0.5570188900297807, + "angularVelocity": -0.000001785939724980264, + "velocityX": 3.01807213366905, + "velocityY": -2.183315426970831, + "timestamp": 1.7778443194503906 + }, + { + "x": 6.943863467278195, + "y": 3.283106829462228, + "heading": -0.5570189585084613, + "angularVelocity": -0.0000017859397331931199, + "velocityX": 3.018072133669382, + "velocityY": -2.1833154269703705, + "timestamp": 1.8161875397380358 + }, + { + "x": 7.0595860720436105, + "y": 3.199391485226898, + "heading": -0.5570190269871422, + "angularVelocity": -0.0000017859397430016153, + "velocityX": 3.018072136280727, + "velocityY": -2.1833154233606176, + "timestamp": 1.854530760025681 + }, + { + "x": 7.175309459223025, + "y": 3.1156772225606058, + "heading": -0.557019095467055, + "angularVelocity": -0.0000017859718654957953, + "velocityX": 3.018092541817673, + "velocityY": -2.183287215791487, + "timestamp": 1.8928739803133263 + }, + { + "x": 7.292415293414065, + "y": 3.037748831863909, + "heading": -0.5654204714615869, + "angularVelocity": -0.2191098173681297, + "velocityX": 3.0541470776979107, + "velocityY": -2.0323903446838734, + "timestamp": 1.9312172006009716 + }, + { + "x": 7.402774333953857, + "y": 2.964965343475342, + "heading": -0.5880028838627427, + "angularVelocity": -0.5889545069961751, + "velocityX": 2.8781891482221704, + "velocityY": -1.8982101097027282, + "timestamp": 1.9695604208886168 + }, + { + "x": 7.566192905422352, + "y": 2.8587474703903673, + "heading": -0.6090225737312371, + "angularVelocity": -0.3298358622780655, + "velocityX": 2.5643244866971804, + "velocityY": -1.6667450365591048, + "timestamp": 2.0332881481385234 + }, + { + "x": 7.708728086420993, + "y": 2.766445154767202, + "heading": -0.6215083339078488, + "angularVelocity": -0.19592351265955282, + "velocityX": 2.2366274014400647, + "velocityY": -1.4483854925690898, + "timestamp": 2.09701587538843 + }, + { + "x": 7.830157777215144, + "y": 2.687793357486252, + "heading": -0.6276507250569074, + "angularVelocity": -0.09638490832995429, + "velocityX": 1.905445181152729, + "velocityY": -1.2341848779969686, + "timestamp": 2.1607436026383366 + }, + { + "x": 7.930381443513673, + "y": 2.622663002090807, + "heading": -0.6284782322496459, + "angularVelocity": -0.012985041652176142, + "velocityX": 1.5726854012148237, + "velocityY": -1.0220096997973582, + "timestamp": 2.2244713298882433 + }, + { + "x": 8.00934183537338, + "y": 2.570977801328826, + "heading": -0.6245889277954535, + "angularVelocity": 0.06103001977366375, + "velocityX": 1.2390272690891182, + "velocityY": -0.8110316026067975, + "timestamp": 2.28819905713815 + }, + { + "x": 8.06700200718074, + "y": 2.5326873953153237, + "heading": -0.6163742376236303, + "angularVelocity": 0.12890292069587517, + "velocityX": 0.9047893953795837, + "velocityY": -0.6008437404859456, + "timestamp": 2.3519267843880565 + }, + { + "x": 8.103336152705973, + "y": 2.5077560657950095, + "heading": -0.6041104163963888, + "angularVelocity": 0.19244090063261127, + "velocityX": 0.5701465765874425, + "velocityY": -0.39121636054188286, + "timestamp": 2.415654511637963 + }, + { + "x": 8.118325233459473, + "y": 2.496157169342041, + "heading": -0.5880028838627427, + "angularVelocity": 0.2527554838175985, + "velocityX": 0.23520501044577988, + "velocityY": -0.1820070627575325, + "timestamp": 2.4793822388878697 + }, + { + "x": 8.099145036916765, + "y": 2.5050376276993562, + "heading": -0.5592453728804849, + "angularVelocity": 0.33147524461009054, + "velocityX": -0.2210817321633438, + "velocityY": 0.10236115733579505, + "timestamp": 2.566138373621505 + }, + { + "x": 8.040379283371006, + "y": 2.538588966986019, + "heading": -0.5236584542354643, + "angularVelocity": 0.4101948381435164, + "velocityX": -0.6773671248285091, + "velocityY": 0.3867316056630989, + "timestamp": 2.65289450835514 + }, + { + "x": 7.942028118157561, + "y": 2.596811412402258, + "heading": -0.48124018408036706, + "angularVelocity": 0.4889368375542877, + "velocityX": -1.1336508422765768, + "velocityY": 0.6711046497748767, + "timestamp": 2.7396506430887753 + }, + { + "x": 7.804091717790056, + "y": 2.679705216007733, + "heading": -0.4319855329045032, + "angularVelocity": 0.5677368099338319, + "velocityX": -1.5899325251292724, + "velocityY": 0.9554805992680714, + "timestamp": 2.8264067778224105 + }, + { + "x": 7.626570292486765, + "y": 2.7872706495099675, + "heading": -0.3758851382218846, + "angularVelocity": 0.6466446995922762, + "velocityX": -2.0462117848879524, + "velocityY": 1.2398596806149733, + "timestamp": 2.9131629125560456 + }, + { + "x": 7.409464087423223, + "y": 2.9195079940918838, + "heading": -0.31292409072865957, + "angularVelocity": 0.7257244422718045, + "velocityX": -2.502488218615496, + "velocityY": 1.5242420030343713, + "timestamp": 2.999919047289681 + }, + { + "x": 7.152773379306078, + "y": 3.0764175249240475, + "heading": -0.24308164175732724, + "angularVelocity": 0.8050433457618579, + "velocityX": -2.9587614628666286, + "velocityY": 1.8086274972244747, + "timestamp": 3.086675182023316 + }, + { + "x": 6.87850975407444, + "y": 3.2473448836663836, + "heading": -0.2430816363695133, + "angularVelocity": 6.210297334840108e-8, + "velocityX": -3.1613167884173445, + "velocityY": 1.9702048652481876, + "timestamp": 3.1734313167569512 + }, + { + "x": 6.604246152600556, + "y": 3.418272280529599, + "heading": -0.2430816309817874, + "angularVelocity": 6.210195880646742e-8, + "velocityX": -3.1613165145721047, + "velocityY": 1.9702053046508896, + "timestamp": 3.2601874514905864 + }, + { + "x": 6.329982551126931, + "y": 3.5891996773932293, + "heading": -0.24308162559406143, + "angularVelocity": 6.210195950465955e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.9702053046556713, + "timestamp": 3.3469435862242216 + }, + { + "x": 6.055718949653306, + "y": 3.76012707425686, + "heading": -0.24308162020633542, + "angularVelocity": 6.210195974983747e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.9702053046556718, + "timestamp": 3.433699720957857 + }, + { + "x": 5.781455348179681, + "y": 3.9310544711204902, + "heading": -0.24308161481860946, + "angularVelocity": 6.210195955610095e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.9702053046556716, + "timestamp": 3.520455855691492 + }, + { + "x": 5.507191746706056, + "y": 4.1019818679841205, + "heading": -0.24308160943088358, + "angularVelocity": 6.210195862911563e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.970205304655672, + "timestamp": 3.607211990425127 + }, + { + "x": 5.23292814523241, + "y": 4.272909264847718, + "heading": -0.24308160404315773, + "angularVelocity": 6.210195842256675e-8, + "velocityX": -3.1613165145693576, + "velocityY": 1.970205304655297, + "timestamp": 3.6939681251587624 + }, + { + "x": 4.958664541898313, + "y": 4.443836658724973, + "heading": -0.24308159865475076, + "angularVelocity": 6.210980925268262e-8, + "velocityX": -3.161316536013975, + "velocityY": 1.970205270233036, + "timestamp": 3.7807242598923976 + }, + { + "x": 4.72116068045686, + "y": 4.591872176458927, + "heading": -0.20144636604906935, + "angularVelocity": 0.4799111063847273, + "velocityX": -2.7376030775305313, + "velocityY": 1.7063406315698904, + "timestamp": 3.8674803946260328 + }, + { + "x": 4.523240963216832, + "y": 4.715235252344186, + "heading": -0.16672852006699063, + "angularVelocity": 0.40017741786989297, + "velocityX": -2.2813339696114237, + "velocityY": 1.4219521912083464, + "timestamp": 3.954236529359668 + }, + { + "x": 4.364905283308217, + "y": 4.813925798348148, + "heading": -0.1389420316269777, + "angularVelocity": 0.3202826926917032, + "velocityX": -1.8250660935361869, + "velocityY": 1.1375627361335074, + "timestamp": 4.040992664093303 + }, + { + "x": 4.246153560219927, + "y": 4.887943754022767, + "heading": -0.11809715693821225, + "angularVelocity": 0.24026974867846324, + "velocityX": -1.3687991454770243, + "velocityY": 0.853172584300505, + "timestamp": 4.127748798826938 + }, + { + "x": 4.166985746432812, + "y": 4.937289078389794, + "heading": -0.10420012908374074, + "angularVelocity": 0.16018495864458598, + "velocityX": -0.9125327451503197, + "velocityY": 0.5687819601291682, + "timestamp": 4.2145049335605735 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 0.0800779542850017, + "velocityX": -0.45626649675525716, + "velocityY": 0.284391045103359, + "timestamp": 4.301261068294209 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -4.230647512546783e-26, + "velocityX": -2.5287498438634255e-25, + "velocityY": -6.744044988840678e-26, + "timestamp": 4.388017203027844 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAGF.5.traj b/src/main/deploy/choreo/CenterLanePCBAGF.5.traj new file mode 100644 index 00000000..b86af882 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAGF.5.traj @@ -0,0 +1,509 @@ +{ + "samples": [ + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -4.230647512546783e-26, + "velocityX": -2.5287498438634255e-25, + "velocityY": -6.744044988840678e-26, + "timestamp": 0 + }, + { + "x": 4.14782187195376, + "y": 4.951212056223932, + "heading": -0.09513612979057086, + "angularVelocity": 0.03469517276527526, + "velocityX": 0.3347010425202365, + "velocityY": -0.17619612328674322, + "timestamp": 0.061009798577654095 + }, + { + "x": 4.188661957426492, + "y": 4.929712672608232, + "heading": -0.09090255484394089, + "angularVelocity": 0.06939172141736155, + "velocityX": 0.6694020702387493, + "velocityY": -0.3523923061036675, + "timestamp": 0.12201959715530819 + }, + { + "x": 4.249922082180478, + "y": 4.897463590700952, + "heading": -0.08455167733440473, + "angularVelocity": 0.10409602486152425, + "velocityX": 1.004103048726068, + "velocityY": -0.5285885654290785, + "timestamp": 0.18302939573296229 + }, + { + "x": 4.331602241050322, + "y": 4.85446480491151, + "heading": -0.07608262285012946, + "angularVelocity": 0.13881466062366435, + "velocityX": 1.3388039425483322, + "velocityY": -0.7047849163886669, + "timestamp": 0.24403919431061638 + }, + { + "x": 4.433702426596031, + "y": 4.80071630889475, + "heading": -0.06549409960856242, + "angularVelocity": 0.17355446974783373, + "velocityX": 1.67350471442309, + "velocityY": -0.8809813713504947, + "timestamp": 0.3050489928882705 + }, + { + "x": 4.5562226288772765, + "y": 4.736218095809515, + "heading": -0.05278437933675508, + "angularVelocity": 0.2083226066650599, + "velocityX": 2.0082053233678367, + "velocityY": -1.0571779384444293, + "timestamp": 0.36605879146592457 + }, + { + "x": 4.699162834927203, + "y": 4.6609701587705015, + "heading": -0.03795127726949169, + "angularVelocity": 0.24312655365323685, + "velocityX": 2.3429057197753003, + "velocityY": -1.23337461839406, + "timestamp": 0.42706859004357867 + }, + { + "x": 4.862523027168912, + "y": 4.5749724919009225, + "heading": -0.020992138559226412, + "angularVelocity": 0.2779740157423932, + "velocityX": 2.6776058280831623, + "velocityY": -1.4095713946689838, + "timestamp": 0.48807838862123276 + }, + { + "x": 5.046303175452206, + "y": 4.478225094831912, + "heading": -0.0019038772119415763, + "angularVelocity": 0.31287205977231325, + "velocityX": 3.012305442204888, + "velocityY": -1.5857681769899301, + "timestamp": 0.5490881871988869 + }, + { + "x": 5.247403902929994, + "y": 4.372365264246757, + "heading": -0.0019038737446312225, + "angularVelocity": 5.683202427287112e-8, + "velocityX": 3.2962037601520997, + "velocityY": -1.7351283409076275, + "timestamp": 0.610097985776541 + }, + { + "x": 5.452210430229311, + "y": 4.273866062948841, + "heading": -0.0019038702704038527, + "angularVelocity": 5.69453997654849e-8, + "velocityX": 3.3569448199150136, + "velocityY": -1.614481666785775, + "timestamp": 0.671107784354195 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.0019038667157456788, + "angularVelocity": 5.82637257740846e-8, + "velocityX": 3.5033929561238337, + "velocityY": -1.2656489645038427, + "timestamp": 0.7321175829318491 + }, + { + "x": 5.815067334204625, + "y": 4.15419466258127, + "heading": -0.0019038632171495887, + "angularVelocity": 8.405671096693256e-8, + "velocityX": 3.5826277225448937, + "velocityY": -1.020002922483729, + "timestamp": 0.7737394362004615 + }, + { + "x": 5.9667648631867, + "y": 4.122168099202404, + "heading": -0.0019038598438992009, + "angularVelocity": 8.104517513702809e-8, + "velocityX": 3.6446606066067324, + "velocityY": -0.7694650973895661, + "timestamp": 0.8153612894690738 + }, + { + "x": 6.120315713709697, + "y": 4.100721014443213, + "heading": -0.0019038565242215854, + "angularVelocity": 7.975804426962125e-8, + "velocityX": 3.689188213990329, + "velocityY": -0.5152842335198107, + "timestamp": 0.8569831427376862 + }, + { + "x": 6.274166009693199, + "y": 4.08153957617663, + "heading": -0.0019038532098746212, + "angularVelocity": 7.96299708870391e-8, + "velocityX": 3.6963826428056987, + "velocityY": -0.4608501726915756, + "timestamp": 0.8986049960062985 + }, + { + "x": 6.4280163353397715, + "y": 4.062358375832809, + "heading": -0.0019038498955278329, + "angularVelocity": 7.962996666259074e-8, + "velocityX": 3.6963833554858816, + "velocityY": -0.46084445639728905, + "timestamp": 0.9402268492749108 + }, + { + "x": 6.581866660989341, + "y": 4.043177175513034, + "heading": -0.001903846581181053, + "angularVelocity": 7.962996646753637e-8, + "velocityX": 3.69638335555791, + "velocityY": -0.46084445581955213, + "timestamp": 0.9818487025435232 + }, + { + "x": 6.735716986905282, + "y": 4.023995977329788, + "heading": -0.0019038432668342794, + "angularVelocity": 7.962996631106344e-8, + "velocityX": 3.6963833619576825, + "velocityY": -0.4608444044876696, + "timestamp": 1.0234705558121355 + }, + { + "x": 6.88956997054089, + "y": 4.004836108474471, + "heading": -0.001903839952467122, + "angularVelocity": 7.963045605311032e-8, + "velocityX": 3.6964472159060078, + "velocityY": -0.4603319494609365, + "timestamp": 1.0650924090807479 + }, + { + "x": 7.044365710177195, + "y": 3.9961114372411974, + "heading": -0.0019038360600849265, + "angularVelocity": 9.351775304985098e-8, + "velocityX": 3.7190977210291876, + "velocityY": -0.20961755779992794, + "timestamp": 1.1067142623493602 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -1.3190054521749074e-28, + "angularVelocity": 0.04574126115428558, + "velocityX": 3.664521283033867, + "velocityY": 0.042410578931505474, + "timestamp": 1.1483361156179726 + }, + { + "x": 7.411737697803866, + "y": 4.005712621161443, + "heading": 0.0009655084122524671, + "angularVelocity": 0.014666383344284814, + "velocityX": 3.263607500383174, + "velocityY": 0.11903101153279746, + "timestamp": 1.2141675063288062 + }, + { + "x": 7.599748268821247, + "y": 4.014968631330143, + "heading": 0.0012837965610470823, + "angularVelocity": 0.004834899359679463, + "velocityX": 2.8559410485983, + "velocityY": 0.1406017717194758, + "timestamp": 1.2799988970396399 + }, + { + "x": 7.760889567084852, + "y": 4.024787670821748, + "heading": 0.0012839891285129092, + "angularVelocity": 0.000002925161746511023, + "velocityX": 2.4477881527890566, + "velocityY": 0.1491543682365084, + "timestamp": 1.3458302877504735 + }, + { + "x": 7.895156217687927, + "y": 4.03478791644303, + "heading": 0.0011126736496287965, + "angularVelocity": -0.0026023372290070947, + "velocityX": 2.039553610417369, + "velocityY": 0.15190694763243642, + "timestamp": 1.4116616784613072 + }, + { + "x": 8.002547604891067, + "y": 4.044753431600936, + "heading": 0.0008527507121031434, + "angularVelocity": -0.003948313026947509, + "velocityX": 1.631309714766018, + "velocityY": 0.15137938072250434, + "timestamp": 1.4774930691721408 + }, + { + "x": 8.083064257665658, + "y": 4.054545378094414, + "heading": 0.0005575241657673266, + "angularVelocity": -0.004484586200413281, + "velocityX": 1.2230738543602167, + "velocityY": 0.1487428168803028, + "timestamp": 1.5433244598829745 + }, + { + "x": 8.13670697281611, + "y": 4.064066986984161, + "heading": 0.00026414808704782584, + "angularVelocity": -0.004456477002106289, + "velocityX": 0.8148500976696773, + "velocityY": 0.14463630172376873, + "timestamp": 1.6091558505938082 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -1.276053827019493e-27, + "angularVelocity": -0.004012494407236614, + "velocityX": 0.40663867791392916, + "velocityY": 0.13944668019766568, + "timestamp": 1.6749872413046418 + }, + { + "x": 8.158068705558701, + "y": 4.083431829444424, + "heading": -0.000240048236913116, + "angularVelocity": -0.0031208282305151794, + "velocityX": -0.07030667180071454, + "velocityY": 0.13241189095614034, + "timestamp": 1.751905360208447 + }, + { + "x": 8.115975110193533, + "y": 4.093075599420407, + "heading": -0.0004115111549292854, + "angularVelocity": -0.0022291616131513174, + "velocityX": -0.5472520124655176, + "velocityY": 0.12537709077419898, + "timestamp": 1.828823479112252 + }, + { + "x": 8.037195777332599, + "y": 4.102178264678885, + "heading": -0.000514388747142335, + "angularVelocity": -0.0013374949060014165, + "velocityX": -1.0241973410641676, + "velocityY": 0.11834227602292711, + "timestamp": 1.905741598016057 + }, + { + "x": 7.921730708275182, + "y": 4.110739823655246, + "heading": -0.0005486810396251135, + "angularVelocity": -0.00044582853782039575, + "velocityX": -1.5011426527710625, + "velocityY": 0.1113074409303779, + "timestamp": 1.982659716919862 + }, + { + "x": 7.769579904970056, + "y": 4.118760274012496, + "heading": -0.0005143881026246048, + "angularVelocity": 0.00044583691709096753, + "velocityX": -1.9780879391422628, + "velocityY": 0.1042725754549578, + "timestamp": 2.059577835823667 + }, + { + "x": 7.58074337066483, + "y": 4.126239611878995, + "heading": -0.00041151007577159475, + "angularVelocity": 0.0013375005566851135, + "velocityX": -2.4550331832918157, + "velocityY": 0.09723765964495061, + "timestamp": 2.1364959547274722 + }, + { + "x": 7.355221111853749, + "y": 4.133177829577417, + "heading": -0.0002400472412179255, + "angularVelocity": 0.0022291605280688772, + "velocityX": -2.9319783430107655, + "velocityY": 0.09020264402331184, + "timestamp": 2.2134140736312773 + }, + { + "x": 7.093013148015058, + "y": 4.139574904378433, + "heading": -3.6404571271115587e-10, + "angularVelocity": 0.0031208105527440306, + "velocityX": -3.4089232494961896, + "velocityY": 0.08316733290131334, + "timestamp": 2.2903321925350824 + }, + { + "x": 6.806524254043931, + "y": 4.135349685277542, + "heading": -2.9271426416436637e-10, + "angularVelocity": 9.273686039566091e-10, + "velocityX": -3.7245956876482667, + "velocityY": -0.05493138887308735, + "timestamp": 2.3672503114388874 + }, + { + "x": 6.520035365230175, + "y": 4.131124116490654, + "heading": -2.213841067469342e-10, + "angularVelocity": 9.273518181930497e-10, + "velocityX": -3.724595620598135, + "velocityY": -0.05493593508406917, + "timestamp": 2.4441684303426925 + }, + { + "x": 6.2335464689880125, + "y": 4.126899051371689, + "heading": -1.5005388112258718e-10, + "angularVelocity": 9.273527049400969e-10, + "velocityX": -3.724595717173642, + "velocityY": -0.05492938697902426, + "timestamp": 2.5210865492464976 + }, + { + "x": 5.947511615310419, + "y": 4.143566419634921, + "heading": -7.76428044505519e-11, + "angularVelocity": 9.414046742691968e-10, + "velocityX": -3.718692783365069, + "velocityY": 0.21668975399771281, + "timestamp": 2.5980046681503026 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 8.328200008246131e-28, + "angularVelocity": 1.0094215193646874e-9, + "velocityX": -3.6605144600811688, + "velocityY": 0.6901189950564038, + "timestamp": 2.6749227870541077 + }, + { + "x": 5.429536691787251, + "y": 4.268976766885781, + "heading": -7.1547225200391066e-9, + "angularVelocity": -1.0779919590068591e-7, + "velocityX": -3.5620320452215446, + "velocityY": 1.089751147278287, + "timestamp": 2.741293620901798 + }, + { + "x": 5.202544249135573, + "y": 4.366945110997213, + "heading": -1.3874386645688143e-8, + "angularVelocity": -1.0124423238480708e-7, + "velocityX": -3.4200631435878064, + "velocityY": 1.4760752341344967, + "timestamp": 2.8076644547494887 + }, + { + "x": 4.987571628122558, + "y": 4.485916838228238, + "heading": -0.004584937869464128, + "angularVelocity": -0.06908040368453193, + "velocityX": -3.2389621849009536, + "velocityY": 1.7925302476091356, + "timestamp": 2.874035288597179 + }, + { + "x": 4.796407538252732, + "y": 4.591709293955959, + "heading": -0.018829281267826372, + "angularVelocity": -0.21461751454035435, + "velocityX": -2.8802424014818646, + "velocityY": 1.593960021211954, + "timestamp": 2.9404061224448697 + }, + { + "x": 4.629141619838775, + "y": 4.684277836657996, + "heading": -0.035106228890285536, + "angularVelocity": -0.24524247593169948, + "velocityX": -2.5201720201045186, + "velocityY": 1.3947171872883817, + "timestamp": 3.00677695629256 + }, + { + "x": 4.4857776191021586, + "y": 4.763619927271578, + "heading": -0.051033661297585665, + "angularVelocity": -0.2399763794417698, + "velocityX": -2.160045194936232, + "velocityY": 1.1954360976638891, + "timestamp": 3.0731477901402506 + }, + { + "x": 4.366313131027418, + "y": 4.829736226023804, + "heading": -0.06545154271306548, + "angularVelocity": -0.21723218738770492, + "velocityX": -1.7999546057970206, + "velocityY": 0.9961649555880353, + "timestamp": 3.139518623987941 + }, + { + "x": 4.27074543266692, + "y": 4.88262772178767, + "heading": -0.07767440134084834, + "angularVelocity": -0.18416008838810272, + "velocityX": -1.4399050429260685, + "velocityY": 0.7969087127222595, + "timestamp": 3.2058894578356316 + }, + { + "x": 4.199072197330148, + "y": 4.922295318430048, + "heading": -0.08724968829906798, + "angularVelocity": -0.1442694991627378, + "velocityX": -1.0798905359732165, + "velocityY": 0.5976660882912562, + "timestamp": 3.272260291683322 + }, + { + "x": 4.15129151783854, + "y": 4.948739780760436, + "heading": -0.09385601929230014, + "angularVelocity": -0.09953665805062048, + "velocityX": -0.7199047642109825, + "velocityY": 0.3984349871371742, + "timestamp": 3.3386311255310126 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.05117995064055202, + "velocityX": -0.3599425785050827, + "velocityY": 0.19921349015633877, + "timestamp": 3.405001959378703 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -1.5406552461687784e-28, + "velocityX": 2.4367660918840807e-27, + "velocityY": 6.301744727068757e-27, + "timestamp": 3.4713727932263936 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePCBAGF.traj b/src/main/deploy/choreo/CenterLanePCBAGF.traj new file mode 100644 index 00000000..e3a1dc17 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePCBAGF.traj @@ -0,0 +1,1652 @@ +{ + "samples": [ + { + "x": 1.2899744510650637, + "y": 5.590954303741455, + "heading": -9.687220582798775e-29, + "angularVelocity": -9.134521375874455e-29, + "velocityX": 4.127432494607337e-27, + "velocityY": -1.3653566194328874e-27, + "timestamp": 0 + }, + { + "x": 1.3104781299456403, + "y": 5.567275491596599, + "heading": -0.013632838685405388, + "angularVelocity": -0.19128951975675332, + "velocityX": 0.2876978871987266, + "velocityY": -0.33224984965521687, + "timestamp": 0.07126808987100346 + }, + { + "x": 1.3514857086951348, + "y": 5.519918181736226, + "heading": -0.040881942077731624, + "angularVelocity": -0.3823464813164996, + "velocityX": 0.575398875200937, + "velocityY": -0.6644952873872657, + "timestamp": 0.1425361797420069 + }, + { + "x": 1.4129978772139142, + "y": 5.448882692998005, + "heading": -0.08170986553530107, + "angularVelocity": -0.5728780374424056, + "velocityX": 0.8631095435575385, + "velocityY": -0.9967362513404827, + "timestamp": 0.21380426961301036 + }, + { + "x": 1.4950158086435297, + "y": 5.354169496620718, + "heading": -0.13606373855442938, + "angularVelocity": -0.7626677397627718, + "velocityX": 1.150836672879393, + "velocityY": -1.3289706030948822, + "timestamp": 0.2850723594840138 + }, + { + "x": 1.5975411283703898, + "y": 5.235779380543047, + "heading": -0.20388316047242194, + "angularVelocity": -0.9516099286615785, + "velocityX": 1.438586608851632, + "velocityY": -1.6611938988677692, + "timestamp": 0.3563404493550173 + }, + { + "x": 1.7205758185191724, + "y": 5.093713591292254, + "heading": -0.28510990320243546, + "angularVelocity": -1.1397350886916635, + "velocityX": 1.7263643570562608, + "velocityY": -1.9933997039619569, + "timestamp": 0.42760853922602077 + }, + { + "x": 1.8641220914189134, + "y": 4.92797393358567, + "heading": -0.3796975284198533, + "angularVelocity": -1.3272086481989789, + "velocityX": 2.0141731476115345, + "velocityY": -2.3255801861194376, + "timestamp": 0.49887662909702424 + }, + { + "x": 2.036057344584012, + "y": 4.766214838118392, + "heading": -0.4690840637578516, + "angularVelocity": -1.254229424414062, + "velocityX": 2.412513840013176, + "velocityY": -2.2697268266915005, + "timestamp": 0.5701447189680277 + }, + { + "x": 2.1874785084173527, + "y": 4.628126850696092, + "heading": -0.5451442530827, + "angularVelocity": -1.067240464316059, + "velocityX": 2.1246698783062077, + "velocityY": -1.9375850773079741, + "timestamp": 0.6414128088390311 + }, + { + "x": 2.3183845436377744, + "y": 4.5137087594261915, + "heading": -0.6078811711978208, + "angularVelocity": -0.8802946483998079, + "velocityX": 1.836811333899418, + "velocityY": -1.605460332625711, + "timestamp": 0.7126808987100345 + }, + { + "x": 2.4287747758121854, + "y": 4.422959692661206, + "heading": -0.6572940075938836, + "angularVelocity": -0.6933374598014469, + "velocityX": 1.548943326167706, + "velocityY": -1.273347818487104, + "timestamp": 0.7839489885810379 + }, + { + "x": 2.5186487653652114, + "y": 4.355879041329831, + "heading": -0.6933804923945726, + "angularVelocity": -0.506348421376329, + "velocityX": 1.2610691505230385, + "velocityY": -0.9412438505478742, + "timestamp": 0.8552170784520413 + }, + { + "x": 2.58800626172225, + "y": 4.312466418462692, + "heading": -0.7161379527262242, + "angularVelocity": -0.31932187845700755, + "velocityX": 0.9731914589345287, + "velocityY": -0.6091453123791657, + "timestamp": 0.9264851683230447 + }, + { + "x": 2.6368471855998568, + "y": 4.292721634612479, + "heading": -0.7255636953209235, + "angularVelocity": -0.1322575448810274, + "velocityX": 0.685312654878354, + "velocityY": -0.27704943244516045, + "timestamp": 0.9977532581940481 + }, + { + "x": 2.6651716232299805, + "y": 4.296644687652588, + "heading": -0.7216551150961179, + "angularVelocity": 0.05484334197647404, + "velocityX": 0.3974350607879547, + "velocityY": 0.05504641764931743, + "timestamp": 1.0690213480650517 + }, + { + "x": 2.672652235661854, + "y": 4.3255208850960845, + "heading": -0.7036434508654703, + "angularVelocity": 0.2466181683415971, + "velocityX": 0.1024255678097115, + "velocityY": 0.3953768419726611, + "timestamp": 1.1420559681490803 + }, + { + "x": 2.6585872493180758, + "y": 4.379253606612061, + "heading": -0.6716320440965468, + "angularVelocity": 0.43830455655267986, + "velocityX": -0.19257971531304016, + "velocityY": 0.7357157667713767, + "timestamp": 1.215090588233109 + }, + { + "x": 2.6229771758211826, + "y": 4.457843745767889, + "heading": -0.6256270362767148, + "angularVelocity": 0.6299068546793523, + "velocityX": -0.48757799323009304, + "velocityY": 1.0760669264166411, + "timestamp": 1.2881252083171375 + }, + { + "x": 2.5658227847055692, + "y": 4.561292508951217, + "heading": -0.5656323531358879, + "angularVelocity": 0.821455401175512, + "velocityX": -0.7825657345770498, + "velocityY": 1.4164346040864937, + "timestamp": 1.3611598284011661 + }, + { + "x": 2.4871252028274746, + "y": 4.689601433735054, + "heading": -0.49164521078608, + "angularVelocity": 1.0130420650464593, + "velocityX": -1.077538046854364, + "velocityY": 1.7568233344161037, + "timestamp": 1.4341944484851947 + }, + { + "x": 2.3868860574551665, + "y": 4.842772331616221, + "heading": -0.40364732106038387, + "angularVelocity": 1.2048791329981834, + "velocityX": -1.3724880783521554, + "velocityY": 2.097236868007793, + "timestamp": 1.5072290685692233 + }, + { + "x": 2.26510760033775, + "y": 5.020807045399027, + "heading": -0.30159033464993307, + "angularVelocity": 1.3973782062949196, + "velocityX": -1.6674072785934422, + "velocityY": 2.437675633527935, + "timestamp": 1.580263688653252 + }, + { + "x": 2.1359139807220116, + "y": 5.17000455286456, + "heading": -0.21560374484110897, + "angularVelocity": 1.1773401396473877, + "velocityX": -1.7689366969677838, + "velocityY": 2.042832663384521, + "timestamp": 1.6532983087372806 + }, + { + "x": 2.028255013080415, + "y": 5.294335941405348, + "heading": -0.1438207992307524, + "angularVelocity": 0.9828619020372543, + "velocityX": -1.474081298947423, + "velocityY": 1.7023623645572525, + "timestamp": 1.7263329288213092 + }, + { + "x": 1.9421291374445764, + "y": 5.393801086818426, + "heading": -0.08631852593953135, + "angularVelocity": 0.7873289848713239, + "velocityX": -1.1792472602273896, + "velocityY": 1.3618903651260308, + "timestamp": 1.7993675489053378 + }, + { + "x": 1.87753522481553, + "y": 5.468400016112323, + "heading": -0.043158813254579896, + "angularVelocity": 0.5909486848195399, + "velocityX": -0.8844286799154853, + "velocityY": 1.0214187354992572, + "timestamp": 1.8724021689893664 + }, + { + "x": 1.834472632995246, + "y": 5.518132767413434, + "heading": -0.01438145137891416, + "angularVelocity": 0.3940235718698405, + "velocityX": -0.5896188926667782, + "velocityY": 0.680947627904294, + "timestamp": 1.945436789073395 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -2.0788362526419016e-27, + "angularVelocity": 0.19691279782612486, + "velocityX": -0.29481141121630017, + "velocityY": 0.34047551881670896, + "timestamp": 2.0184714091574234 + }, + { + "x": 1.8129411935806272, + "y": 5.542999267578125, + "heading": -9.689019627790258e-28, + "angularVelocity": 4.903263424537125e-28, + "velocityX": -7.04775279749202e-27, + "velocityY": -4.880044267923792e-28, + "timestamp": 2.091506029241452 + }, + { + "x": 1.8720264471920118, + "y": 5.543082773692806, + "heading": 4.603106809112672e-20, + "angularVelocity": 4.715795015985417e-19, + "velocityX": 0.6053171264251316, + "velocityY": 0.0008555041789240918, + "timestamp": 2.1891164404325725 + }, + { + "x": 1.9901969516410831, + "y": 5.543249785918246, + "heading": 4.290138941133409e-20, + "angularVelocity": -3.206295969142147e-20, + "velocityX": 1.210634224434258, + "velocityY": 0.0017110083176873983, + "timestamp": 2.286726851623693 + }, + { + "x": 2.1674527004558795, + "y": 5.543500304245301, + "heading": 2.9796741833873356e-20, + "angularVelocity": -1.3425460888073973e-19, + "velocityX": 1.8159512561393765, + "velocityY": 0.0025665123627422117, + "timestamp": 2.384337262814814 + }, + { + "x": 2.4037936612766035, + "y": 5.543834328628234, + "heading": -1.7822834862290494e-20, + "angularVelocity": -4.878534586092089e-19, + "velocityX": 2.421267956324555, + "velocityY": 0.0034220159392546997, + "timestamp": 2.4819476740059345 + }, + { + "x": 2.5810494100914, + "y": 5.544084846955289, + "heading": -6.57207439293443e-21, + "angularVelocity": 1.1526188991905598e-19, + "velocityX": 1.8159512561393765, + "velocityY": 0.0025665123627422117, + "timestamp": 2.579558085197055 + }, + { + "x": 2.699219914540471, + "y": 5.544251859180729, + "heading": -1.8054002593168886e-20, + "angularVelocity": -1.1763015911739438e-19, + "velocityX": 1.210634224434258, + "velocityY": 0.0017110083176873983, + "timestamp": 2.677168496388176 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -3.155242334732113e-28, + "angularVelocity": 1.84959801497032e-19, + "velocityX": 0.6053171264251316, + "velocityY": 0.0008555041789240918, + "timestamp": 2.7747789075792966 + }, + { + "x": 2.7583051681518556, + "y": 5.54433536529541, + "heading": -3.309635664969631e-28, + "angularVelocity": -1.5501074190844928e-28, + "velocityX": -3.9535178243549605e-27, + "velocityY": -3.789490097440697e-27, + "timestamp": 2.8723893187704173 + }, + { + "x": 2.7303741375078747, + "y": 5.568056872076371, + "heading": 0.006778509195380246, + "angularVelocity": 0.08813497362119738, + "velocityX": -0.3631625447521428, + "velocityY": 0.30842982050092854, + "timestamp": 2.9492998676350957 + }, + { + "x": 2.674756164732603, + "y": 5.615779956875509, + "heading": 0.020597402851140194, + "angularVelocity": 0.17967488023097886, + "velocityX": -0.7231514219607195, + "velocityY": 0.6205011601608272, + "timestamp": 3.026210416499774 + }, + { + "x": 2.591861664751003, + "y": 5.687962530350053, + "heading": 0.041894082151838256, + "angularVelocity": 0.27690192847497247, + "velocityX": -1.0778040360555798, + "velocityY": 0.9385263080302098, + "timestamp": 3.1031209653644525 + }, + { + "x": 2.482522988073185, + "y": 5.785486691208239, + "heading": 0.07152941986388299, + "angularVelocity": 0.38532214565504047, + "velocityX": -1.4216343309445878, + "velocityY": 1.2680206070272062, + "timestamp": 3.180031514229131 + }, + { + "x": 2.3492932233323143, + "y": 5.910724338795779, + "heading": 0.11194462213648758, + "angularVelocity": 0.5254832122406744, + "velocityX": -1.7322690672157808, + "velocityY": 1.628354620221101, + "timestamp": 3.2569420630938093 + }, + { + "x": 2.2322888283047253, + "y": 6.068263103797439, + "heading": 0.17809343518700713, + "angularVelocity": 0.8600746455067716, + "velocityX": -1.5213049023152156, + "velocityY": 2.0483375470228995, + "timestamp": 3.3338526119584877 + }, + { + "x": 2.1483049194140715, + "y": 6.209825434589826, + "heading": 0.2439234240091836, + "angularVelocity": 0.85592925540035, + "velocityX": -1.0919686587911488, + "velocityY": 1.8406100708169548, + "timestamp": 3.410763160823166 + }, + { + "x": 2.0954357223517674, + "y": 6.331973751888774, + "heading": 0.3066259975705838, + "angularVelocity": 0.8152662344371413, + "velocityX": -0.687411516921112, + "velocityY": 1.5881867845445197, + "timestamp": 3.4876737096878445 + }, + { + "x": 2.072970005140484, + "y": 6.433621819948826, + "heading": 0.36524449476389237, + "angularVelocity": 0.7621645932659499, + "velocityX": -0.29210189685177457, + "velocityY": 1.32164013338272, + "timestamp": 3.564584258552523 + }, + { + "x": 2.080539806403903, + "y": 6.514239906298576, + "heading": 0.41928927766619223, + "angularVelocity": 0.7026966222460568, + "velocityX": 0.09842344613530389, + "velocityY": 1.0482058383381778, + "timestamp": 3.6414948074172013 + }, + { + "x": 2.1179206829564796, + "y": 6.573514734989947, + "heading": 0.4684594496501494, + "angularVelocity": 0.6393163578961122, + "velocityX": 0.48603055243236887, + "velocityY": 0.770698292579654, + "timestamp": 3.7184053562818797 + }, + { + "x": 2.1849615573883057, + "y": 6.611239433288574, + "heading": 0.5125504196, + "angularVelocity": 0.5732759758018013, + "velocityX": 0.8716733324811171, + "velocityY": 0.49050096320339975, + "timestamp": 3.795315905146558 + }, + { + "x": 2.2917884862212228, + "y": 6.6722766025245654, + "heading": 0.5461831512487827, + "angularVelocity": 0.4115075108626729, + "velocityX": 1.3070625376552998, + "velocityY": 0.7468097995934108, + "timestamp": 3.877046446536376 + }, + { + "x": 2.4341165316393667, + "y": 6.754285323763742, + "heading": 0.5638405564624909, + "angularVelocity": 0.21604414841069464, + "velocityX": 1.7414303514680458, + "velocityY": 1.0034036217627857, + "timestamp": 3.9587769879261936 + }, + { + "x": 2.5412608832572645, + "y": 6.816945409398408, + "heading": 0.539566584836395, + "angularVelocity": -0.2970000101959444, + "velocityX": 1.3109463096159757, + "velocityY": 0.766666714414698, + "timestamp": 4.040507529316011 + }, + { + "x": 2.6126555762150003, + "y": 6.858741818277979, + "heading": 0.521795194780163, + "angularVelocity": -0.21743879037177083, + "velocityX": 0.8735375019384187, + "velocityY": 0.511392781313182, + "timestamp": 4.122238070705829 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -0.1131128587056526, + "velocityX": 0.4366659015377109, + "velocityY": 0.2557557080208239, + "timestamp": 4.203968612095647 + }, + { + "x": 2.6483445167541504, + "y": 6.879644870758057, + "heading": 0.5125504196, + "angularVelocity": -4.764407701562395e-26, + "velocityX": 1.4812581731979975e-26, + "velocityY": -5.1560471026288735e-26, + "timestamp": 4.2856991534854645 + }, + { + "x": 2.6602506971919606, + "y": 6.863647804987902, + "heading": 0.4793942538974517, + "angularVelocity": -0.5654499433512294, + "velocityX": 0.20304968657976344, + "velocityY": -0.272816224127646, + "timestamp": 4.344335936621462 + }, + { + "x": 2.6841390735646424, + "y": 6.831518157238345, + "heading": 0.41523186966938014, + "angularVelocity": -1.0942343832071746, + "velocityX": 0.40739575220688923, + "velocityY": -0.5479435608709636, + "timestamp": 4.40297271975746 + }, + { + "x": 2.720131144955563, + "y": 6.7831009440148105, + "heading": 0.3230087558895857, + "angularVelocity": -1.5727860371517781, + "velocityX": 0.6138138804006831, + "velocityY": -0.8257140080696285, + "timestamp": 4.461609502893458 + }, + { + "x": 2.7684071443805327, + "y": 6.718246852790197, + "heading": 0.20639646027764702, + "angularVelocity": -1.98872259655644, + "velocityX": 0.8233057279592204, + "velocityY": -1.1060308522415998, + "timestamp": 4.520246286029455 + }, + { + "x": 2.829185609402161, + "y": 6.636825404453338, + "heading": 0.06956994287591697, + "angularVelocity": -2.333458796407444, + "velocityX": 1.03652454604584, + "velocityY": -1.388572905645519, + "timestamp": 4.578883069165453 + }, + { + "x": 2.9026837898482625, + "y": 6.53870263495317, + "heading": -0.0825332150617159, + "angularVelocity": -2.59398878660952, + "velocityX": 1.253448373449048, + "velocityY": -1.6733996009397407, + "timestamp": 4.637519852301451 + }, + { + "x": 2.98910296697184, + "y": 6.423727552121541, + "heading": -0.24244596371477412, + "angularVelocity": -2.7271746521661684, + "velocityX": 1.4738048798335797, + "velocityY": -1.9608013380434501, + "timestamp": 4.696156635437449 + }, + { + "x": 3.08851698550526, + "y": 6.291921017444985, + "heading": -0.39600744918619013, + "angularVelocity": -2.6188593108059237, + "velocityX": 1.6954207447370835, + "velocityY": -2.247847300402779, + "timestamp": 4.754793418573446 + }, + { + "x": 3.20017208933701, + "y": 6.146474995005627, + "heading": -0.5026655989158746, + "angularVelocity": -1.8189631836096676, + "velocityX": 1.9041819462159983, + "velocityY": -2.4804570554633094, + "timestamp": 4.813430201709444 + }, + { + "x": 3.3235802395453247, + "y": 5.987781312389556, + "heading": -0.5570167491322654, + "angularVelocity": -0.926912209531219, + "velocityX": 2.1046200628382103, + "velocityY": -2.706384527405065, + "timestamp": 4.872066984845442 + }, + { + "x": 3.458298567775881, + "y": 5.815853629795619, + "heading": -0.5570170670277983, + "angularVelocity": -0.000005421435418997904, + "velocityX": 2.297505439855057, + "velocityY": -2.9320790363819964, + "timestamp": 4.93070376798144 + }, + { + "x": 3.5900045442013857, + "y": 5.641607467957256, + "heading": -0.5570171376680287, + "angularVelocity": -0.0000012047084903437067, + "velocityX": 2.246132365754707, + "velocityY": -2.9716187096115108, + "timestamp": 4.989340551117437 + }, + { + "x": 3.721710444891145, + "y": 5.467361248873177, + "heading": -0.5570172083082521, + "angularVelocity": -0.0000012047083681560634, + "velocityX": 2.246131074146577, + "velocityY": -2.9716196858880317, + "timestamp": 5.047977334253435 + }, + { + "x": 3.8545074444088514, + "y": 5.293945142218947, + "heading": -0.5570172791057042, + "angularVelocity": -0.0000012073897706381049, + "velocityX": 2.264738828010173, + "velocityY": -2.9574628310018474, + "timestamp": 5.106614117389433 + }, + { + "x": 4.00357560161076, + "y": 5.134299054707746, + "heading": -0.5570173534542538, + "angularVelocity": -0.00000126795069051409, + "velocityX": 2.542229454439387, + "velocityY": -2.722626975305397, + "timestamp": 5.165250900525431 + }, + { + "x": 4.16749906539917, + "y": 4.98994779586792, + "heading": -0.55701743434988, + "angularVelocity": -0.0000013796054608952668, + "velocityX": 2.795573955825942, + "velocityY": -2.461786802066348, + "timestamp": 5.223887683661428 + }, + { + "x": 4.2902143593852005, + "y": 4.896089807808244, + "heading": -0.5570175109527072, + "angularVelocity": -0.000001846969607519395, + "velocityX": 2.958786599579285, + "velocityY": -2.2630085323028566, + "timestamp": 5.265362555152153 + }, + { + "x": 4.419113803049847, + "y": 4.8109235045890575, + "heading": -0.5570175835802155, + "angularVelocity": -0.0000017511207539159885, + "velocityX": 3.107892539063629, + "velocityY": -2.0534434504090724, + "timestamp": 5.306837426642878 + }, + { + "x": 4.553582724143636, + "y": 4.7348546191881615, + "heading": -0.5570176536447519, + "angularVelocity": -0.0000016893249788320206, + "velocityX": 3.2421781252260224, + "velocityY": -1.8340957468163943, + "timestamp": 5.348312298133603 + }, + { + "x": 4.692974428091918, + "y": 4.668233908840813, + "heading": -0.5570177224005147, + "angularVelocity": -0.0000016577691599272835, + "velocityX": 3.3608712682678954, + "velocityY": -1.606290940822974, + "timestamp": 5.389787169624328 + }, + { + "x": 4.832547821463298, + "y": 4.601994690531772, + "heading": -0.5570177911147437, + "angularVelocity": -0.0000016567677375032224, + "velocityX": 3.3652519792036677, + "velocityY": -1.5970927920500795, + "timestamp": 5.431262041115053 + }, + { + "x": 4.9721212332499025, + "y": 4.535755511025668, + "heading": -0.5570178598289695, + "angularVelocity": -0.0000016567676584676064, + "velocityX": 3.3652524232128536, + "velocityY": -1.5970918564730696, + "timestamp": 5.472736912605778 + }, + { + "x": 5.111694645038376, + "y": 4.469516331523502, + "heading": -0.5570179285431947, + "angularVelocity": -0.000001656767647060506, + "velocityX": 3.365252423257915, + "velocityY": -1.5970918563781218, + "timestamp": 5.514211784096503 + }, + { + "x": 5.251268056826837, + "y": 4.4032771520213085, + "heading": -0.5570179972574194, + "angularVelocity": -0.0000016567676356654265, + "velocityX": 3.3652524232576075, + "velocityY": -1.597091856378771, + "timestamp": 5.555686655587228 + }, + { + "x": 5.390841468488021, + "y": 4.33703797225093, + "heading": -0.5570180659716438, + "angularVelocity": -0.0000016567676246630092, + "velocityX": 3.3652524201888485, + "velocityY": -1.5970918628449942, + "timestamp": 5.597161527077953 + }, + { + "x": 5.530413626022683, + "y": 4.27079614995943, + "heading": -0.5570181346861351, + "angularVelocity": -0.0000016567740615756042, + "velocityX": 3.365222181963228, + "velocityY": -1.5971555766318444, + "timestamp": 5.638636398568678 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.5570182043959618, + "angularVelocity": -0.0000016807725799241125, + "velocityX": 3.2679571491481956, + "velocityY": -1.7877590150355749, + "timestamp": 5.6801112700594025 + }, + { + "x": 5.786627291659718, + "y": 4.120246269797506, + "heading": -0.5570182737203393, + "angularVelocity": -0.00000180799570053011, + "velocityX": 3.1472464214958245, + "velocityY": -1.9926027126510082, + "timestamp": 5.718454490347048 + }, + { + "x": 5.902360023994386, + "y": 4.0365449270340825, + "heading": -0.557018342200347, + "angularVelocity": -0.0000017859743423988426, + "velocityX": 3.018336265615098, + "velocityY": -2.182950261754457, + "timestamp": 5.756797710634693 + }, + { + "x": 6.018082629955809, + "y": 3.952829584452034, + "heading": -0.5570184106790251, + "angularVelocity": -0.0000017859396699979741, + "velocityX": 3.0180721674728543, + "velocityY": -2.1833153802426746, + "timestamp": 5.795140930922338 + }, + { + "x": 6.133805234621251, + "y": 3.869114240078507, + "heading": -0.5570184791577035, + "angularVelocity": -0.0000017859396766231999, + "velocityX": 3.018072133673387, + "velocityY": -2.183315426964841, + "timestamp": 5.8334841512099835 + }, + { + "x": 6.249527839286528, + "y": 3.7853988957047506, + "heading": -0.5570185476363823, + "angularVelocity": -0.0000017859396843832207, + "velocityX": 3.01807213366906, + "velocityY": -2.1833154269708217, + "timestamp": 5.871827371497629 + }, + { + "x": 6.365250443951803, + "y": 3.701683551330994, + "heading": -0.5570186161150613, + "angularVelocity": -0.000001785939693521326, + "velocityX": 3.0180721336690577, + "velocityY": -2.1833154269708244, + "timestamp": 5.910170591785274 + }, + { + "x": 6.480973048617079, + "y": 3.6179682069572374, + "heading": -0.5570186845937407, + "angularVelocity": -0.0000017859397005145362, + "velocityX": 3.0180721336690555, + "velocityY": -2.183315426970826, + "timestamp": 5.948513812072919 + }, + { + "x": 6.596695653282355, + "y": 3.534252862583481, + "heading": -0.5570187530724204, + "angularVelocity": -0.0000017859397100037096, + "velocityX": 3.0180721336690537, + "velocityY": -2.1833154269708275, + "timestamp": 5.9868570323605645 + }, + { + "x": 6.71241825794763, + "y": 3.4505375182097238, + "heading": -0.5570188215511004, + "angularVelocity": -0.000001785939717860173, + "velocityX": 3.0180721336690515, + "velocityY": -2.1833154269708293, + "timestamp": 6.02520025264821 + }, + { + "x": 6.828140862612906, + "y": 3.366822173835967, + "heading": -0.5570188900297807, + "angularVelocity": -0.000001785939724980264, + "velocityX": 3.01807213366905, + "velocityY": -2.183315426970831, + "timestamp": 6.063543472935855 + }, + { + "x": 6.943863467278195, + "y": 3.283106829462228, + "heading": -0.5570189585084613, + "angularVelocity": -0.0000017859397331931199, + "velocityX": 3.018072133669382, + "velocityY": -2.1833154269703705, + "timestamp": 6.1018866932235 + }, + { + "x": 7.0595860720436105, + "y": 3.199391485226898, + "heading": -0.5570190269871422, + "angularVelocity": -0.0000017859397430016153, + "velocityX": 3.018072136280727, + "velocityY": -2.1833154233606176, + "timestamp": 6.1402299135111456 + }, + { + "x": 7.175309459223025, + "y": 3.1156772225606058, + "heading": -0.557019095467055, + "angularVelocity": -0.0000017859718654957953, + "velocityX": 3.018092541817673, + "velocityY": -2.183287215791487, + "timestamp": 6.178573133798791 + }, + { + "x": 7.292415293414065, + "y": 3.037748831863909, + "heading": -0.5654204714615869, + "angularVelocity": -0.2191098173681297, + "velocityX": 3.0541470776979107, + "velocityY": -2.0323903446838734, + "timestamp": 6.216916354086436 + }, + { + "x": 7.402774333953857, + "y": 2.964965343475342, + "heading": -0.5880028838627427, + "angularVelocity": -0.5889545069961751, + "velocityX": 2.8781891482221704, + "velocityY": -1.8982101097027282, + "timestamp": 6.255259574374081 + }, + { + "x": 7.566192905422352, + "y": 2.8587474703903673, + "heading": -0.6090225737312371, + "angularVelocity": -0.3298358622780655, + "velocityX": 2.5643244866971804, + "velocityY": -1.6667450365591048, + "timestamp": 6.318987301623988 + }, + { + "x": 7.708728086420993, + "y": 2.766445154767202, + "heading": -0.6215083339078488, + "angularVelocity": -0.19592351265955282, + "velocityX": 2.2366274014400647, + "velocityY": -1.4483854925690898, + "timestamp": 6.3827150288738945 + }, + { + "x": 7.830157777215144, + "y": 2.687793357486252, + "heading": -0.6276507250569074, + "angularVelocity": -0.09638490832995429, + "velocityX": 1.905445181152729, + "velocityY": -1.2341848779969686, + "timestamp": 6.446442756123801 + }, + { + "x": 7.930381443513673, + "y": 2.622663002090807, + "heading": -0.6284782322496459, + "angularVelocity": -0.012985041652176142, + "velocityX": 1.5726854012148237, + "velocityY": -1.0220096997973582, + "timestamp": 6.510170483373708 + }, + { + "x": 8.00934183537338, + "y": 2.570977801328826, + "heading": -0.6245889277954535, + "angularVelocity": 0.06103001977366375, + "velocityX": 1.2390272690891182, + "velocityY": -0.8110316026067975, + "timestamp": 6.573898210623614 + }, + { + "x": 8.06700200718074, + "y": 2.5326873953153237, + "heading": -0.6163742376236303, + "angularVelocity": 0.12890292069587517, + "velocityX": 0.9047893953795837, + "velocityY": -0.6008437404859456, + "timestamp": 6.637625937873521 + }, + { + "x": 8.103336152705973, + "y": 2.5077560657950095, + "heading": -0.6041104163963888, + "angularVelocity": 0.19244090063261127, + "velocityX": 0.5701465765874425, + "velocityY": -0.39121636054188286, + "timestamp": 6.701353665123428 + }, + { + "x": 8.118325233459473, + "y": 2.496157169342041, + "heading": -0.5880028838627427, + "angularVelocity": 0.2527554838175985, + "velocityX": 0.23520501044577988, + "velocityY": -0.1820070627575325, + "timestamp": 6.765081392373334 + }, + { + "x": 8.099145036916765, + "y": 2.5050376276993562, + "heading": -0.5592453728804849, + "angularVelocity": 0.33147524461009054, + "velocityX": -0.2210817321633438, + "velocityY": 0.10236115733579505, + "timestamp": 6.851837527106969 + }, + { + "x": 8.040379283371006, + "y": 2.538588966986019, + "heading": -0.5236584542354643, + "angularVelocity": 0.4101948381435164, + "velocityX": -0.6773671248285091, + "velocityY": 0.3867316056630989, + "timestamp": 6.9385936618406046 + }, + { + "x": 7.942028118157561, + "y": 2.596811412402258, + "heading": -0.48124018408036706, + "angularVelocity": 0.4889368375542877, + "velocityX": -1.1336508422765768, + "velocityY": 0.6711046497748767, + "timestamp": 7.02534979657424 + }, + { + "x": 7.804091717790056, + "y": 2.679705216007733, + "heading": -0.4319855329045032, + "angularVelocity": 0.5677368099338319, + "velocityX": -1.5899325251292724, + "velocityY": 0.9554805992680714, + "timestamp": 7.112105931307875 + }, + { + "x": 7.626570292486765, + "y": 2.7872706495099675, + "heading": -0.3758851382218846, + "angularVelocity": 0.6466446995922762, + "velocityX": -2.0462117848879524, + "velocityY": 1.2398596806149733, + "timestamp": 7.19886206604151 + }, + { + "x": 7.409464087423223, + "y": 2.9195079940918838, + "heading": -0.31292409072865957, + "angularVelocity": 0.7257244422718045, + "velocityX": -2.502488218615496, + "velocityY": 1.5242420030343713, + "timestamp": 7.285618200775145 + }, + { + "x": 7.152773379306078, + "y": 3.0764175249240475, + "heading": -0.24308164175732724, + "angularVelocity": 0.8050433457618579, + "velocityX": -2.9587614628666286, + "velocityY": 1.8086274972244747, + "timestamp": 7.3723743355087805 + }, + { + "x": 6.87850975407444, + "y": 3.2473448836663836, + "heading": -0.2430816363695133, + "angularVelocity": 6.210297334840108e-8, + "velocityX": -3.1613167884173445, + "velocityY": 1.9702048652481876, + "timestamp": 7.459130470242416 + }, + { + "x": 6.604246152600556, + "y": 3.418272280529599, + "heading": -0.2430816309817874, + "angularVelocity": 6.210195880646742e-8, + "velocityX": -3.1613165145721047, + "velocityY": 1.9702053046508896, + "timestamp": 7.545886604976051 + }, + { + "x": 6.329982551126931, + "y": 3.5891996773932293, + "heading": -0.24308162559406143, + "angularVelocity": 6.210195950465955e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.9702053046556713, + "timestamp": 7.632642739709686 + }, + { + "x": 6.055718949653306, + "y": 3.76012707425686, + "heading": -0.24308162020633542, + "angularVelocity": 6.210195974983747e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.9702053046556718, + "timestamp": 7.719398874443321 + }, + { + "x": 5.781455348179681, + "y": 3.9310544711204902, + "heading": -0.24308161481860946, + "angularVelocity": 6.210195955610095e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.9702053046556716, + "timestamp": 7.8061550091769565 + }, + { + "x": 5.507191746706056, + "y": 4.1019818679841205, + "heading": -0.24308160943088358, + "angularVelocity": 6.210195862911563e-8, + "velocityX": -3.161316514569124, + "velocityY": 1.970205304655672, + "timestamp": 7.892911143910592 + }, + { + "x": 5.23292814523241, + "y": 4.272909264847718, + "heading": -0.24308160404315773, + "angularVelocity": 6.210195842256675e-8, + "velocityX": -3.1613165145693576, + "velocityY": 1.970205304655297, + "timestamp": 7.979667278644227 + }, + { + "x": 4.958664541898313, + "y": 4.443836658724973, + "heading": -0.24308159865475076, + "angularVelocity": 6.210980925268262e-8, + "velocityX": -3.161316536013975, + "velocityY": 1.970205270233036, + "timestamp": 8.066423413377862 + }, + { + "x": 4.72116068045686, + "y": 4.591872176458927, + "heading": -0.20144636604906935, + "angularVelocity": 0.4799111063847273, + "velocityX": -2.7376030775305313, + "velocityY": 1.7063406315698904, + "timestamp": 8.153179548111497 + }, + { + "x": 4.523240963216832, + "y": 4.715235252344186, + "heading": -0.16672852006699063, + "angularVelocity": 0.40017741786989297, + "velocityX": -2.2813339696114237, + "velocityY": 1.4219521912083464, + "timestamp": 8.239935682845132 + }, + { + "x": 4.364905283308217, + "y": 4.813925798348148, + "heading": -0.1389420316269777, + "angularVelocity": 0.3202826926917032, + "velocityX": -1.8250660935361869, + "velocityY": 1.1375627361335074, + "timestamp": 8.326691817578768 + }, + { + "x": 4.246153560219927, + "y": 4.887943754022767, + "heading": -0.11809715693821225, + "angularVelocity": 0.24026974867846324, + "velocityX": -1.3687991454770243, + "velocityY": 0.853172584300505, + "timestamp": 8.413447952312403 + }, + { + "x": 4.166985746432812, + "y": 4.937289078389794, + "heading": -0.10420012908374074, + "angularVelocity": 0.16018495864458598, + "velocityX": -0.9125327451503197, + "velocityY": 0.5687819601291682, + "timestamp": 8.500204087046038 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 0.0800779542850017, + "velocityX": -0.45626649675525716, + "velocityY": 0.284391045103359, + "timestamp": 8.586960221779673 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -4.230647512546783e-26, + "velocityX": -2.5287498438634255e-25, + "velocityY": -6.744044988840678e-26, + "timestamp": 8.673716356513308 + }, + { + "x": 4.14782187195376, + "y": 4.951212056223932, + "heading": -0.09513612979057086, + "angularVelocity": 0.03469517276527526, + "velocityX": 0.3347010425202365, + "velocityY": -0.17619612328674322, + "timestamp": 8.734726155090963 + }, + { + "x": 4.188661957426492, + "y": 4.929712672608232, + "heading": -0.09090255484394089, + "angularVelocity": 0.06939172141736155, + "velocityX": 0.6694020702387493, + "velocityY": -0.3523923061036675, + "timestamp": 8.795735953668617 + }, + { + "x": 4.249922082180478, + "y": 4.897463590700952, + "heading": -0.08455167733440473, + "angularVelocity": 0.10409602486152425, + "velocityX": 1.004103048726068, + "velocityY": -0.5285885654290785, + "timestamp": 8.85674575224627 + }, + { + "x": 4.331602241050322, + "y": 4.85446480491151, + "heading": -0.07608262285012946, + "angularVelocity": 0.13881466062366435, + "velocityX": 1.3388039425483322, + "velocityY": -0.7047849163886669, + "timestamp": 8.917755550823925 + }, + { + "x": 4.433702426596031, + "y": 4.80071630889475, + "heading": -0.06549409960856242, + "angularVelocity": 0.17355446974783373, + "velocityX": 1.67350471442309, + "velocityY": -0.8809813713504947, + "timestamp": 8.978765349401579 + }, + { + "x": 4.5562226288772765, + "y": 4.736218095809515, + "heading": -0.05278437933675508, + "angularVelocity": 0.2083226066650599, + "velocityX": 2.0082053233678367, + "velocityY": -1.0571779384444293, + "timestamp": 9.039775147979233 + }, + { + "x": 4.699162834927203, + "y": 4.6609701587705015, + "heading": -0.03795127726949169, + "angularVelocity": 0.24312655365323685, + "velocityX": 2.3429057197753003, + "velocityY": -1.23337461839406, + "timestamp": 9.100784946556887 + }, + { + "x": 4.862523027168912, + "y": 4.5749724919009225, + "heading": -0.020992138559226412, + "angularVelocity": 0.2779740157423932, + "velocityX": 2.6776058280831623, + "velocityY": -1.4095713946689838, + "timestamp": 9.161794745134541 + }, + { + "x": 5.046303175452206, + "y": 4.478225094831912, + "heading": -0.0019038772119415763, + "angularVelocity": 0.31287205977231325, + "velocityX": 3.012305442204888, + "velocityY": -1.5857681769899301, + "timestamp": 9.222804543712195 + }, + { + "x": 5.247403902929994, + "y": 4.372365264246757, + "heading": -0.0019038737446312225, + "angularVelocity": 5.683202427287112e-8, + "velocityX": 3.2962037601520997, + "velocityY": -1.7351283409076275, + "timestamp": 9.28381434228985 + }, + { + "x": 5.452210430229311, + "y": 4.273866062948841, + "heading": -0.0019038702704038527, + "angularVelocity": 5.69453997654849e-8, + "velocityX": 3.3569448199150136, + "velocityY": -1.614481666785775, + "timestamp": 9.344824140867503 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": -0.0019038667157456788, + "angularVelocity": 5.82637257740846e-8, + "velocityX": 3.5033929561238337, + "velocityY": -1.2656489645038427, + "timestamp": 9.405833939445158 + }, + { + "x": 5.815067334204625, + "y": 4.15419466258127, + "heading": -0.0019038632171495887, + "angularVelocity": 8.405671096693256e-8, + "velocityX": 3.5826277225448937, + "velocityY": -1.020002922483729, + "timestamp": 9.44745579271377 + }, + { + "x": 5.9667648631867, + "y": 4.122168099202404, + "heading": -0.0019038598438992009, + "angularVelocity": 8.104517513702809e-8, + "velocityX": 3.6446606066067324, + "velocityY": -0.7694650973895661, + "timestamp": 9.489077645982382 + }, + { + "x": 6.120315713709697, + "y": 4.100721014443213, + "heading": -0.0019038565242215854, + "angularVelocity": 7.975804426962125e-8, + "velocityX": 3.689188213990329, + "velocityY": -0.5152842335198107, + "timestamp": 9.530699499250995 + }, + { + "x": 6.274166009693199, + "y": 4.08153957617663, + "heading": -0.0019038532098746212, + "angularVelocity": 7.96299708870391e-8, + "velocityX": 3.6963826428056987, + "velocityY": -0.4608501726915756, + "timestamp": 9.572321352519607 + }, + { + "x": 6.4280163353397715, + "y": 4.062358375832809, + "heading": -0.0019038498955278329, + "angularVelocity": 7.962996666259074e-8, + "velocityX": 3.6963833554858816, + "velocityY": -0.46084445639728905, + "timestamp": 9.61394320578822 + }, + { + "x": 6.581866660989341, + "y": 4.043177175513034, + "heading": -0.001903846581181053, + "angularVelocity": 7.962996646753637e-8, + "velocityX": 3.69638335555791, + "velocityY": -0.46084445581955213, + "timestamp": 9.655565059056832 + }, + { + "x": 6.735716986905282, + "y": 4.023995977329788, + "heading": -0.0019038432668342794, + "angularVelocity": 7.962996631106344e-8, + "velocityX": 3.6963833619576825, + "velocityY": -0.4608444044876696, + "timestamp": 9.697186912325444 + }, + { + "x": 6.88956997054089, + "y": 4.004836108474471, + "heading": -0.001903839952467122, + "angularVelocity": 7.963045605311032e-8, + "velocityX": 3.6964472159060078, + "velocityY": -0.4603319494609365, + "timestamp": 9.738808765594056 + }, + { + "x": 7.044365710177195, + "y": 3.9961114372411974, + "heading": -0.0019038360600849265, + "angularVelocity": 9.351775304985098e-8, + "velocityX": 3.7190977210291876, + "velocityY": -0.20961755779992794, + "timestamp": 9.780430618862669 + }, + { + "x": 7.196889877319336, + "y": 3.9978766441345215, + "heading": -1.3190054521749074e-28, + "angularVelocity": 0.04574126115428558, + "velocityX": 3.664521283033867, + "velocityY": 0.042410578931505474, + "timestamp": 9.822052472131281 + }, + { + "x": 7.411737697803866, + "y": 4.005712621161443, + "heading": 0.0009655084122524671, + "angularVelocity": 0.014666383344284814, + "velocityX": 3.263607500383174, + "velocityY": 0.11903101153279746, + "timestamp": 9.887883862842115 + }, + { + "x": 7.599748268821247, + "y": 4.014968631330143, + "heading": 0.0012837965610470823, + "angularVelocity": 0.004834899359679463, + "velocityX": 2.8559410485983, + "velocityY": 0.1406017717194758, + "timestamp": 9.953715253552948 + }, + { + "x": 7.760889567084852, + "y": 4.024787670821748, + "heading": 0.0012839891285129092, + "angularVelocity": 0.000002925161746511023, + "velocityX": 2.4477881527890566, + "velocityY": 0.1491543682365084, + "timestamp": 10.019546644263782 + }, + { + "x": 7.895156217687927, + "y": 4.03478791644303, + "heading": 0.0011126736496287965, + "angularVelocity": -0.0026023372290070947, + "velocityX": 2.039553610417369, + "velocityY": 0.15190694763243642, + "timestamp": 10.085378034974616 + }, + { + "x": 8.002547604891067, + "y": 4.044753431600936, + "heading": 0.0008527507121031434, + "angularVelocity": -0.003948313026947509, + "velocityX": 1.631309714766018, + "velocityY": 0.15137938072250434, + "timestamp": 10.15120942568545 + }, + { + "x": 8.083064257665658, + "y": 4.054545378094414, + "heading": 0.0005575241657673266, + "angularVelocity": -0.004484586200413281, + "velocityX": 1.2230738543602167, + "velocityY": 0.1487428168803028, + "timestamp": 10.217040816396283 + }, + { + "x": 8.13670697281611, + "y": 4.064066986984161, + "heading": 0.00026414808704782584, + "angularVelocity": -0.004456477002106289, + "velocityX": 0.8148500976696773, + "velocityY": 0.14463630172376873, + "timestamp": 10.282872207107117 + }, + { + "x": 8.1634765625, + "y": 4.073246955871582, + "heading": -1.276053827019493e-27, + "angularVelocity": -0.004012494407236614, + "velocityX": 0.40663867791392916, + "velocityY": 0.13944668019766568, + "timestamp": 10.34870359781795 + }, + { + "x": 8.158068705558701, + "y": 4.083431829444424, + "heading": -0.000240048236913116, + "angularVelocity": -0.0031208282305151794, + "velocityX": -0.07030667180071454, + "velocityY": 0.13241189095614034, + "timestamp": 10.425621716721755 + }, + { + "x": 8.115975110193533, + "y": 4.093075599420407, + "heading": -0.0004115111549292854, + "angularVelocity": -0.0022291616131513174, + "velocityX": -0.5472520124655176, + "velocityY": 0.12537709077419898, + "timestamp": 10.50253983562556 + }, + { + "x": 8.037195777332599, + "y": 4.102178264678885, + "heading": -0.000514388747142335, + "angularVelocity": -0.0013374949060014165, + "velocityX": -1.0241973410641676, + "velocityY": 0.11834227602292711, + "timestamp": 10.579457954529365 + }, + { + "x": 7.921730708275182, + "y": 4.110739823655246, + "heading": -0.0005486810396251135, + "angularVelocity": -0.00044582853782039575, + "velocityX": -1.5011426527710625, + "velocityY": 0.1113074409303779, + "timestamp": 10.65637607343317 + }, + { + "x": 7.769579904970056, + "y": 4.118760274012496, + "heading": -0.0005143881026246048, + "angularVelocity": 0.00044583691709096753, + "velocityX": -1.9780879391422628, + "velocityY": 0.1042725754549578, + "timestamp": 10.733294192336976 + }, + { + "x": 7.58074337066483, + "y": 4.126239611878995, + "heading": -0.00041151007577159475, + "angularVelocity": 0.0013375005566851135, + "velocityX": -2.4550331832918157, + "velocityY": 0.09723765964495061, + "timestamp": 10.81021231124078 + }, + { + "x": 7.355221111853749, + "y": 4.133177829577417, + "heading": -0.0002400472412179255, + "angularVelocity": 0.0022291605280688772, + "velocityX": -2.9319783430107655, + "velocityY": 0.09020264402331184, + "timestamp": 10.887130430144586 + }, + { + "x": 7.093013148015058, + "y": 4.139574904378433, + "heading": -3.6404571271115587e-10, + "angularVelocity": 0.0031208105527440306, + "velocityX": -3.4089232494961896, + "velocityY": 0.08316733290131334, + "timestamp": 10.96404854904839 + }, + { + "x": 6.806524254043931, + "y": 4.135349685277542, + "heading": -2.9271426416436637e-10, + "angularVelocity": 9.273686039566091e-10, + "velocityX": -3.7245956876482667, + "velocityY": -0.05493138887308735, + "timestamp": 11.040966667952196 + }, + { + "x": 6.520035365230175, + "y": 4.131124116490654, + "heading": -2.213841067469342e-10, + "angularVelocity": 9.273518181930497e-10, + "velocityX": -3.724595620598135, + "velocityY": -0.05493593508406917, + "timestamp": 11.117884786856001 + }, + { + "x": 6.2335464689880125, + "y": 4.126899051371689, + "heading": -1.5005388112258718e-10, + "angularVelocity": 9.273527049400969e-10, + "velocityX": -3.724595717173642, + "velocityY": -0.05492938697902426, + "timestamp": 11.194802905759806 + }, + { + "x": 5.947511615310419, + "y": 4.143566419634921, + "heading": -7.76428044505519e-11, + "angularVelocity": 9.414046742691968e-10, + "velocityX": -3.718692783365069, + "velocityY": 0.21668975399771281, + "timestamp": 11.271721024663611 + }, + { + "x": 5.665951728820801, + "y": 4.196649074554443, + "heading": 8.328200008246131e-28, + "angularVelocity": 1.0094215193646874e-9, + "velocityX": -3.6605144600811688, + "velocityY": 0.6901189950564038, + "timestamp": 11.348639143567416 + }, + { + "x": 5.429536691787251, + "y": 4.268976766885781, + "heading": -7.1547225200391066e-9, + "angularVelocity": -1.0779919590068591e-7, + "velocityX": -3.5620320452215446, + "velocityY": 1.089751147278287, + "timestamp": 11.415009977415107 + }, + { + "x": 5.202544249135573, + "y": 4.366945110997213, + "heading": -1.3874386645688143e-8, + "angularVelocity": -1.0124423238480708e-7, + "velocityX": -3.4200631435878064, + "velocityY": 1.4760752341344967, + "timestamp": 11.481380811262797 + }, + { + "x": 4.987571628122558, + "y": 4.485916838228238, + "heading": -0.004584937869464128, + "angularVelocity": -0.06908040368453193, + "velocityX": -3.2389621849009536, + "velocityY": 1.7925302476091356, + "timestamp": 11.547751645110488 + }, + { + "x": 4.796407538252732, + "y": 4.591709293955959, + "heading": -0.018829281267826372, + "angularVelocity": -0.21461751454035435, + "velocityX": -2.8802424014818646, + "velocityY": 1.593960021211954, + "timestamp": 11.614122478958178 + }, + { + "x": 4.629141619838775, + "y": 4.684277836657996, + "heading": -0.035106228890285536, + "angularVelocity": -0.24524247593169948, + "velocityX": -2.5201720201045186, + "velocityY": 1.3947171872883817, + "timestamp": 11.680493312805869 + }, + { + "x": 4.4857776191021586, + "y": 4.763619927271578, + "heading": -0.051033661297585665, + "angularVelocity": -0.2399763794417698, + "velocityX": -2.160045194936232, + "velocityY": 1.1954360976638891, + "timestamp": 11.746864146653559 + }, + { + "x": 4.366313131027418, + "y": 4.829736226023804, + "heading": -0.06545154271306548, + "angularVelocity": -0.21723218738770492, + "velocityX": -1.7999546057970206, + "velocityY": 0.9961649555880353, + "timestamp": 11.81323498050125 + }, + { + "x": 4.27074543266692, + "y": 4.88262772178767, + "heading": -0.07767440134084834, + "angularVelocity": -0.18416008838810272, + "velocityX": -1.4399050429260685, + "velocityY": 0.7969087127222595, + "timestamp": 11.87960581434894 + }, + { + "x": 4.199072197330148, + "y": 4.922295318430048, + "heading": -0.08724968829906798, + "angularVelocity": -0.1442694991627378, + "velocityX": -1.0798905359732165, + "velocityY": 0.5976660882912562, + "timestamp": 11.94597664819663 + }, + { + "x": 4.15129151783854, + "y": 4.948739780760436, + "heading": -0.09385601929230014, + "angularVelocity": -0.09953665805062048, + "velocityX": -0.7199047642109825, + "velocityY": 0.3984349871371742, + "timestamp": 12.012347482044321 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -0.05117995064055202, + "velocityX": -0.3599425785050827, + "velocityY": 0.19921349015633877, + "timestamp": 12.078718315892012 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -1.5406552461687784e-28, + "velocityX": 2.4367660918840807e-27, + "velocityY": 6.301744727068757e-27, + "timestamp": 12.145089149739702 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePDEABC.6.traj b/src/main/deploy/choreo/CenterLanePDEABC.6.traj new file mode 100644 index 00000000..484cfcb0 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePDEABC.6.traj @@ -0,0 +1,212 @@ +{ + "samples": [ + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": 1.1225694758097516e-26, + "angularVelocity": -6.884915403521558e-28, + "velocityX": -2.0833783846425874e-26, + "velocityY": -2.541249409937563e-27, + "timestamp": 0 + }, + { + "x": 2.655148725322121, + "y": 5.535337360850629, + "heading": -0.005358266888722308, + "angularVelocity": -0.0814332965124142, + "velocityX": -0.3549425275077926, + "velocityY": -0.20028182493209068, + "timestamp": 0.0657994594128386 + }, + { + "x": 2.6089441174458443, + "y": 5.508126231725926, + "heading": -0.016249931047209468, + "angularVelocity": -0.16552817083421043, + "velocityX": -0.7022034571193023, + "velocityY": -0.4135463933521678, + "timestamp": 0.1315989188256772 + }, + { + "x": 2.5407348998629304, + "y": 5.46559681890727, + "heading": -0.032938167352964134, + "angularVelocity": -0.25362269621470973, + "velocityX": -1.0366227654691733, + "velocityY": -0.6463489700092897, + "timestamp": 0.1973983782385158 + }, + { + "x": 2.452162592477794, + "y": 5.405634174678793, + "heading": -0.05584759102955525, + "angularVelocity": -0.348170393511179, + "velocityX": -1.346094757852274, + "velocityY": -0.9112938732863877, + "timestamp": 0.2631978376513544 + }, + { + "x": 2.3472993756188387, + "y": 5.324395097264509, + "heading": -0.08569447590064297, + "angularVelocity": -0.45360380066076583, + "velocityX": -1.5936790027562684, + "velocityY": -1.234646578242739, + "timestamp": 0.328997297064193 + }, + { + "x": 2.239880921610295, + "y": 5.2164831190740735, + "heading": -0.12272615053693274, + "angularVelocity": -0.5627960315592481, + "velocityX": -1.6325127131300283, + "velocityY": -1.6400131422565938, + "timestamp": 0.3947967564770316 + }, + { + "x": 2.1533744008316598, + "y": 5.0917391522416, + "heading": -0.16180305511717838, + "angularVelocity": -0.5938788088678526, + "velocityX": -1.3146995666921153, + "velocityY": -1.8958205423817192, + "timestamp": 0.4605962158898702 + }, + { + "x": 2.0932393074035645, + "y": 4.9619622230529785, + "heading": -0.1996303470492587, + "angularVelocity": -0.5748875791629854, + "velocityX": -0.9139147033229457, + "velocityY": -1.9723099603960959, + "timestamp": 0.5263956753027088 + }, + { + "x": 2.0629011120815957, + "y": 4.862373025273896, + "heading": -0.2271993917353058, + "angularVelocity": -0.5460590922861188, + "velocityX": -0.6009075609173273, + "velocityY": -1.9725597154359094, + "timestamp": 0.5768829670180189 + }, + { + "x": 2.0480286015059237, + "y": 4.766006778427224, + "heading": -0.2526003641629946, + "angularVelocity": -0.5031161618040544, + "velocityX": -0.29457929055762255, + "velocityY": -1.9087228403944643, + "timestamp": 0.627370258733329 + }, + { + "x": 2.0477407778807875, + "y": 4.675697642485875, + "heading": -0.2752320047517768, + "angularVelocity": -0.4482641040917418, + "velocityX": -0.00570091235551334, + "velocityY": -1.788749859085082, + "timestamp": 0.677857550448639 + }, + { + "x": 2.060871043270714, + "y": 4.593707256979004, + "heading": -0.2946333452673921, + "angularVelocity": -0.38428166487947496, + "velocityX": 0.26007070183058306, + "velocityY": -1.6239806636727634, + "timestamp": 0.7283448421639491 + }, + { + "x": 2.086192021454777, + "y": 4.521742435847718, + "heading": -0.31047411407212144, + "angularVelocity": -0.3137575470289188, + "velocityX": 0.5015317186519693, + "velocityY": -1.425404664941897, + "timestamp": 0.7788321338792592 + }, + { + "x": 2.1225463484050855, + "y": 4.461060496013595, + "heading": -0.32252861483773954, + "angularVelocity": -0.23876306999375915, + "velocityX": 0.7200688671379776, + "velocityY": -1.201925034448228, + "timestamp": 0.8293194255945693 + }, + { + "x": 2.1689002100212633, + "y": 4.412584630822924, + "heading": -0.33064802150676154, + "angularVelocity": -0.16082080050571804, + "velocityX": 0.9181292963298462, + "velocityY": -0.9601597460211961, + "timestamp": 0.8798067173098794 + }, + { + "x": 2.224353288519031, + "y": 4.376998477530217, + "heading": -0.3347371673945201, + "angularVelocity": -0.08099356786291009, + "velocityX": 1.09835716303537, + "velocityY": -0.7048536786914947, + "timestamp": 0.9302940090251894 + }, + { + "x": 2.2881293296813965, + "y": 4.354815483093262, + "heading": -0.3347371673945201, + "angularVelocity": -4.071422399716035e-28, + "velocityX": 1.2632097899405608, + "velocityY": -0.4393777856423879, + "timestamp": 0.9807813007404995 + }, + { + "x": 2.409715105027215, + "y": 4.314433199191984, + "heading": -0.3347371673945201, + "angularVelocity": 1.9851016888222166e-24, + "velocityX": 1.6917384210190034, + "velocityY": -0.5618770864435482, + "timestamp": 1.0526516169993112 + }, + { + "x": 2.5011472273821256, + "y": 4.284859006631005, + "heading": -0.3347371673945201, + "angularVelocity": 1.985101688829323e-24, + "velocityX": 1.2721819954938658, + "velocityY": -0.4114938419705683, + "timestamp": 1.1245219332581229 + }, + { + "x": 2.5621392130721086, + "y": 4.26525765297352, + "heading": -0.3347371673945201, + "angularVelocity": 1.985101688829324e-24, + "velocityX": 0.8486394504004271, + "velocityY": -0.2727322582928377, + "timestamp": 1.1963922495169346 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": 1.9851016888217372e-24, + "velocityX": 0.424451706672944, + "velocityY": -0.1359554979005115, + "timestamp": 1.2682625657757463 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": 5.276957932584006e-25, + "velocityX": 3.771846154825318e-26, + "velocityY": 1.8284888574913443e-25, + "timestamp": 1.340132882034558 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePEDABC.1.traj b/src/main/deploy/choreo/CenterLanePEDABC.1.traj new file mode 100644 index 00000000..a82fc57e --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePEDABC.1.traj @@ -0,0 +1,599 @@ +{ + "samples": [ + { + "x": 1.3350272178649902, + "y": 5.601006507873535, + "heading": 1.1438500584259617e-25, + "angularVelocity": 2.0139373174315835e-26, + "velocityX": -2.9204847474037974e-26, + "velocityY": 3.705827554701837e-26, + "timestamp": 0 + }, + { + "x": 1.3513037651461415, + "y": 5.614995878157814, + "heading": 0.006549885022033663, + "angularVelocity": 0.1111819339605223, + "velocityX": 0.27628851481066125, + "velocityY": 0.23746451088284168, + "timestamp": 0.05891141473001675 + }, + { + "x": 1.384378906601625, + "y": 5.642351283609904, + "heading": 0.019354935835911788, + "angularVelocity": 0.21736111537232614, + "velocityX": 0.5614385871916767, + "velocityY": 0.46434813316664775, + "timestamp": 0.1178228294600335 + }, + { + "x": 1.4348748758417986, + "y": 5.682261570187258, + "heading": 0.0380334905813409, + "angularVelocity": 0.3170617244727609, + "velocityX": 0.8571508505030827, + "velocityY": 0.6774627083776107, + "timestamp": 0.17673424419005024 + }, + { + "x": 1.5035350329878103, + "y": 5.733639392412991, + "heading": 0.06208045701070026, + "angularVelocity": 0.4081885749911699, + "velocityX": 1.1654813835429356, + "velocityY": 0.8721199866136561, + "timestamp": 0.235645658920067 + }, + { + "x": 1.5912385139518137, + "y": 5.794977395067809, + "heading": 0.09080789039928205, + "angularVelocity": 0.48763781213260343, + "velocityX": 1.4887349313530633, + "velocityY": 1.0411904541067623, + "timestamp": 0.29455707365008377 + }, + { + "x": 1.6989811168528033, + "y": 5.864111533799202, + "heading": 0.12324878614182731, + "angularVelocity": 0.550672495155949, + "velocityX": 1.8288917927834498, + "velocityY": 1.1735270498633483, + "timestamp": 0.35346848838010053 + }, + { + "x": 1.8277399268235333, + "y": 5.93784849269496, + "heading": 0.15800938597032024, + "angularVelocity": 0.590048634679649, + "velocityX": 2.1856343216474197, + "velocityY": 1.2516582606899704, + "timestamp": 0.4123799031101173 + }, + { + "x": 1.978020614375797, + "y": 6.011501208422767, + "heading": 0.19307007056378325, + "angularVelocity": 0.5951424652444947, + "velocityX": 2.5509604249190123, + "velocityY": 1.250228263322945, + "timestamp": 0.47129131784013406 + }, + { + "x": 2.1488551922208927, + "y": 6.078800735840598, + "heading": 0.2256402887317746, + "angularVelocity": 0.5528676966468451, + "velocityX": 2.899855293375791, + "velocityY": 1.1423851850487134, + "timestamp": 0.5302027325701508 + }, + { + "x": 2.3368515968322754, + "y": 6.133185386657715, + "heading": 0.2525545797921912, + "angularVelocity": 0.45686037559548065, + "velocityX": 3.1911711078904768, + "velocityY": 0.9231598165882506, + "timestamp": 0.5891141473001675 + }, + { + "x": 2.5473350968820796, + "y": 6.1940752301450726, + "heading": 0.25255484664390665, + "angularVelocity": 0.000004475832490919022, + "velocityX": 3.5303834818832023, + "velocityY": 1.0212890683182199, + "timestamp": 0.6487347242516341 + }, + { + "x": 2.7606743654688186, + "y": 6.255791206318409, + "heading": 0.25255482885683184, + "angularVelocity": -2.9833785118107346e-7, + "velocityX": 3.578282524176262, + "velocityY": 1.0351455710261694, + "timestamp": 0.7083553012031008 + }, + { + "x": 2.974013634055561, + "y": 6.317507182491745, + "heading": 0.2525548110697642, + "angularVelocity": -2.983377309458156e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 0.7679758781545674 + }, + { + "x": 3.1873529026423038, + "y": 6.379223158665082, + "heading": 0.25255479328269653, + "angularVelocity": -2.983377305484016e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.827596455106034 + }, + { + "x": 3.400692171229046, + "y": 6.440939134838418, + "heading": 0.2525547754956289, + "angularVelocity": -2.9833773174604553e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 0.8872170320575006 + }, + { + "x": 3.6140314398157884, + "y": 6.502655111011755, + "heading": 0.25255475770856123, + "angularVelocity": -2.983377305928146e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 0.9468376090089672 + }, + { + "x": 3.827370708402531, + "y": 6.5643710871850915, + "heading": 0.25255473992149363, + "angularVelocity": -2.983377301697445e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.0064581859604338 + }, + { + "x": 4.040709976989273, + "y": 6.626087063358428, + "heading": 0.25255472213442604, + "angularVelocity": -2.983377305025075e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261857, + "timestamp": 1.0660787629119004 + }, + { + "x": 4.254049245576016, + "y": 6.687803039531764, + "heading": 0.2525547043473584, + "angularVelocity": -2.9833773095408974e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.125699339863367 + }, + { + "x": 4.467388514162758, + "y": 6.749519015705101, + "heading": 0.2525546865602907, + "angularVelocity": -2.983377319947091e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.1853199168148336 + }, + { + "x": 4.680727782749501, + "y": 6.811234991878438, + "heading": 0.252554668773223, + "angularVelocity": -2.9833773124406734e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.2449404937663002 + }, + { + "x": 4.894067051336243, + "y": 6.872950968051774, + "heading": 0.2525546509861554, + "angularVelocity": -2.983377305581398e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.3045610707177668 + }, + { + "x": 5.107406319922986, + "y": 6.9346669442251105, + "heading": 0.2525546331990878, + "angularVelocity": -2.983377313652051e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.3641816476692334 + }, + { + "x": 5.320745588509729, + "y": 6.996382920398447, + "heading": 0.2525546154120202, + "angularVelocity": -2.983377306356561e-7, + "velocityX": 3.5782825241763185, + "velocityY": 1.0351455710261857, + "timestamp": 1.4238022246207 + }, + { + "x": 5.534084857096471, + "y": 7.058098896571784, + "heading": 0.2525545976249525, + "angularVelocity": -2.983377306690358e-7, + "velocityX": 3.578282524176319, + "velocityY": 1.0351455710261859, + "timestamp": 1.4834228015721667 + }, + { + "x": 5.747424125671387, + "y": 7.119814872741699, + "heading": 0.2525545797921912, + "angularVelocity": -2.991041384854961e-7, + "velocityX": 3.5782825239779474, + "velocityY": 1.0351455709687998, + "timestamp": 1.5430433785236333 + }, + { + "x": 5.874242280548121, + "y": 7.151641192910771, + "heading": 0.23431006193331627, + "angularVelocity": -0.5017601477559481, + "velocityX": 3.487748847149364, + "velocityY": 0.8752864413346503, + "timestamp": 1.5794044126561186 + }, + { + "x": 6.000453145534325, + "y": 7.182664404015401, + "heading": 0.21336082591069744, + "angularVelocity": -0.5761452203556151, + "velocityX": 3.4710471799658134, + "velocityY": 0.853199361481138, + "timestamp": 1.615765446788604 + }, + { + "x": 6.126521345623787, + "y": 7.213763203476394, + "heading": 0.192336966602575, + "angularVelocity": -0.5781975075714219, + "velocityX": 3.4671236145297133, + "velocityY": 0.8552781900449158, + "timestamp": 1.6521264809210894 + }, + { + "x": 6.252403333907122, + "y": 7.24487049169852, + "heading": 0.17101724585582967, + "angularVelocity": -0.5863342794119836, + "velocityX": 3.462002423381879, + "velocityY": 0.8555116476825977, + "timestamp": 1.6884875150535748 + }, + { + "x": 6.378086189949285, + "y": 7.2759799644241046, + "heading": 0.14935632557503462, + "angularVelocity": -0.5957179381056964, + "velocityX": 3.456525894841813, + "velocityY": 0.8555717258270096, + "timestamp": 1.7248485491860601 + }, + { + "x": 6.503564352522718, + "y": 7.307098484942146, + "heading": 0.12734794412408884, + "angularVelocity": -0.6052738041155772, + "velocityX": 3.4508964215989995, + "velocityY": 0.8558205579262937, + "timestamp": 1.7612095833185455 + }, + { + "x": 6.628828810809778, + "y": 7.338228595030231, + "heading": 0.10496993550474346, + "angularVelocity": -0.6154392787017191, + "velocityX": 3.4450191331370044, + "velocityY": 0.8561392939117064, + "timestamp": 1.7975706174510309 + }, + { + "x": 6.753876017529989, + "y": 7.3693809168618785, + "heading": 0.08222565272715081, + "angularVelocity": -0.6255125389096852, + "velocityX": 3.4390442874806206, + "velocityY": 0.8567501605740924, + "timestamp": 1.8339316515835162 + }, + { + "x": 6.878734995256494, + "y": 7.4006038692779965, + "heading": 0.05925591834951263, + "angularVelocity": -0.6317129016173038, + "velocityX": 3.4338676197042926, + "velocityY": 0.8586926406535731, + "timestamp": 1.8702926857160016 + }, + { + "x": 7.003212758632201, + "y": 7.431690611037589, + "heading": 0.0352626014950734, + "angularVelocity": -0.6598634342196367, + "velocityX": 3.4233834747977516, + "velocityY": 0.8549465795259067, + "timestamp": 1.906653719848487 + }, + { + "x": 7.125290870666504, + "y": 7.458, + "heading": -1.1078640911459773e-20, + "angularVelocity": -0.9697909406699016, + "velocityX": 3.3573883402077644, + "velocityY": 0.7235599753997619, + "timestamp": 1.9430147539809723 + }, + { + "x": 7.338537136585377, + "y": 7.4820248133164355, + "heading": -0.00804052191695304, + "angularVelocity": -0.12790212002878093, + "velocityX": 3.392149139191805, + "velocityY": 0.38216730060631093, + "timestamp": 2.005879403254524 + }, + { + "x": 7.527981798214128, + "y": 7.500236040572311, + "heading": -0.013862931395131553, + "angularVelocity": -0.09261818121091622, + "velocityX": 3.013532467259838, + "velocityY": 0.2896894751870813, + "timestamp": 2.068744052528076 + }, + { + "x": 7.6935968882672885, + "y": 7.512744942179138, + "heading": -0.017658491883084444, + "angularVelocity": -0.06037670665172621, + "velocityX": 2.6344709143686895, + "velocityY": 0.1989814904143519, + "timestamp": 2.1316087018016274 + }, + { + "x": 7.835373213607451, + "y": 7.519588665029473, + "heading": -0.019490985187148625, + "angularVelocity": -0.029149821485366035, + "velocityX": 2.255263124482431, + "velocityY": 0.1088644083665287, + "timestamp": 2.194473351075179 + }, + { + "x": 7.953306201274802, + "y": 7.52078579458168, + "heading": -0.019392463666041717, + "angularVelocity": 0.001567200680277371, + "velocityX": 1.8759825916497619, + "velocityY": 0.01904296875969824, + "timestamp": 2.2573380003487307 + }, + { + "x": 8.047393114183997, + "y": 7.516347487170096, + "heading": -0.01738240796128744, + "angularVelocity": 0.0319743405551765, + "velocityX": 1.4966585194769915, + "velocityY": -0.0706010049029291, + "timestamp": 2.3202026496222823 + }, + { + "x": 8.11763212951421, + "y": 7.506281183777245, + "heading": -0.013474109300406987, + "angularVelocity": 0.062170054331707525, + "velocityX": 1.1173054513447094, + "velocityY": -0.1601266134333638, + "timestamp": 2.383067298895834 + }, + { + "x": 8.164021945337474, + "y": 7.490592202393592, + "heading": -0.007677398137362595, + "angularVelocity": 0.09220939319680839, + "velocityX": 0.737931673195224, + "velocityY": -0.24956762767234375, + "timestamp": 2.4459319481693855 + }, + { + "x": 8.186561584472656, + "y": 7.469284534454346, + "heading": 1.0473347586059488e-21, + "angularVelocity": 0.12212584061281963, + "velocityX": 0.358542350838588, + "velocityY": -0.33894514938796605, + "timestamp": 2.508796597442937 + }, + { + "x": 8.169433800706123, + "y": 7.42685355131065, + "heading": 0.014977206112370758, + "angularVelocity": 0.16513162181596408, + "velocityX": -0.1888428783219197, + "velocityY": -0.46782403935650313, + "timestamp": 2.5994951925858016 + }, + { + "x": 8.10265502996488, + "y": 7.372742454671214, + "heading": 0.033468558101012916, + "angularVelocity": 0.2038769394334648, + "velocityX": -0.7362712800132867, + "velocityY": -0.5966034705852111, + "timestamp": 2.690193787728666 + }, + { + "x": 7.986220080079777, + "y": 7.306963959658495, + "heading": 0.05493247364660356, + "angularVelocity": 0.23665102542968372, + "velocityX": -1.2837569281166943, + "velocityY": -0.7252427108612493, + "timestamp": 2.7808923828715306 + }, + { + "x": 7.820121799349139, + "y": 7.229537347680479, + "heading": 0.07855456380147917, + "angularVelocity": 0.26044604238540936, + "velocityX": -1.8313214275150225, + "velocityY": -0.8536693634124876, + "timestamp": 2.871590978014395 + }, + { + "x": 7.6043499812219455, + "y": 7.1404953741126675, + "heading": 0.10296904259791395, + "angularVelocity": 0.2691825464107588, + "velocityX": -2.37899845953864, + "velocityY": -0.9817348706179747, + "timestamp": 2.9622895731572596 + }, + { + "x": 7.338890842614761, + "y": 7.0399060968505065, + "heading": 0.12540191269228773, + "angularVelocity": 0.24733426200304945, + "velocityX": -2.926827457349739, + "velocityY": -1.1090500035167739, + "timestamp": 3.052988168300124 + }, + { + "x": 7.023762260989218, + "y": 6.927996342713902, + "heading": 0.13709315762540872, + "angularVelocity": 0.12890216121545683, + "velocityX": -3.4744593466873996, + "velocityY": -1.233864250712263, + "timestamp": 3.1436867634429886 + }, + { + "x": 6.6949878661960245, + "y": 6.850204202472232, + "heading": 0.1370931594052802, + "angularVelocity": 1.9624024712667312e-8, + "velocityX": -3.6249116568489486, + "velocityY": -0.857699505919964, + "timestamp": 3.234385358585853 + }, + { + "x": 6.366213403460125, + "y": 6.772412349378657, + "heading": 0.13709316118514803, + "angularVelocity": 1.9623984758130592e-8, + "velocityX": -3.624912405953231, + "velocityY": -0.8576963399602832, + "timestamp": 3.3250839537287176 + }, + { + "x": 6.037438940720318, + "y": 6.694620496301594, + "heading": 0.13709316296501595, + "angularVelocity": 1.9623985202632452e-8, + "velocityX": -3.624912405996305, + "velocityY": -0.8576963397782383, + "timestamp": 3.415782548871582 + }, + { + "x": 5.708664477980543, + "y": 6.6168286432244035, + "heading": 0.13709316474488378, + "angularVelocity": 1.9623984644394548e-8, + "velocityX": -3.624912405995973, + "velocityY": -0.8576963397796433, + "timestamp": 3.5064811440144465 + }, + { + "x": 5.379890015210838, + "y": 6.539036790273742, + "heading": 0.13709316652476114, + "angularVelocity": 1.9624089556639278e-8, + "velocityX": -3.6249124063259477, + "velocityY": -0.8576963383845883, + "timestamp": 3.597179739157311 + }, + { + "x": 5.082161809078407, + "y": 6.468591173031877, + "heading": 0.1733415977378681, + "angularVelocity": 0.39965813313877674, + "velocityX": -3.282611000351902, + "velocityY": -0.7767002028080265, + "timestamp": 3.6878783343001755 + }, + { + "x": 4.834054918534678, + "y": 6.409886673220785, + "heading": 0.20354854900447322, + "angularVelocity": 0.3330476201866674, + "velocityX": -2.7355097413903846, + "velocityY": -0.6472481709184398, + "timestamp": 3.77857692944304 + }, + { + "x": 4.6355693784003975, + "y": 6.36292318296314, + "heading": 0.22771447843277892, + "angularVelocity": 0.26644215811987526, + "velocityX": -2.1884080985117165, + "velocityY": -0.5177973284334796, + "timestamp": 3.8692755245859045 + }, + { + "x": 4.486705209976563, + "y": 6.327700620163418, + "heading": 0.24583924264804105, + "angularVelocity": 0.1998351152706694, + "velocityX": -1.6413062207782854, + "velocityY": -0.3883473910951022, + "timestamp": 3.959974119728769 + }, + { + "x": 4.3874624262872, + "y": 6.30421892984517, + "heading": 0.25792252534877924, + "angularVelocity": 0.13322458503028708, + "velocityX": -1.0942041994480602, + "velocityY": -0.25889805990114845, + "timestamp": 4.0506727148716335 + }, + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": 0.06661189082061515, + "velocityX": -0.5471021053136723, + "velocityY": -0.1294490312938923, + "timestamp": 4.141371310014498 + }, + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": -4.3850016440748696e-23, + "velocityX": 2.0622476782033507e-21, + "velocityY": 1.8080615452809687e-23, + "timestamp": 4.2320699051573625 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePEDABC.2.traj b/src/main/deploy/choreo/CenterLanePEDABC.2.traj new file mode 100644 index 00000000..ea16156f --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePEDABC.2.traj @@ -0,0 +1,716 @@ +{ + "samples": [ + { + "x": 4.337841033935547, + "y": 6.292478084564209, + "heading": 0.2639641302660189, + "angularVelocity": -4.3850016440748696e-23, + "velocityX": 2.0622476782033507e-21, + "velocityY": 1.8080615452809687e-23, + "timestamp": 0 + }, + { + "x": 4.357336302646973, + "y": 6.297583243093535, + "heading": 0.2475212314024478, + "angularVelocity": -0.28561956492945584, + "velocityX": 0.3386404194139296, + "velocityY": 0.08867859433669868, + "timestamp": 0.057569231532273335 + }, + { + "x": 4.39639255106342, + "y": 6.307690301692112, + "heading": 0.21545182729872558, + "angularVelocity": -0.5570580542792894, + "velocityX": 0.6784222644096215, + "velocityY": 0.17556354895775533, + "timestamp": 0.11513846306454667 + }, + { + "x": 4.455088006420721, + "y": 6.322669942772804, + "heading": 0.16876628337274657, + "angularVelocity": -0.8109461023430008, + "velocityX": 1.0195629469953096, + "velocityY": 0.2602022066647703, + "timestamp": 0.17270769459682 + }, + { + "x": 4.53351670800356, + "y": 6.342355632767599, + "heading": 0.10873910043109954, + "angularVelocity": -1.0426955744232154, + "velocityX": 1.362337128625251, + "velocityY": 0.3419481113580211, + "timestamp": 0.23027692612909334 + }, + { + "x": 4.631792821732683, + "y": 6.3665238013399605, + "heading": 0.03701268299307073, + "angularVelocity": -1.245915839571676, + "velocityX": 1.7070944168158462, + "velocityY": 0.4198105121277204, + "timestamp": 0.2878461576613667 + }, + { + "x": 4.7500563078189675, + "y": 6.394856912095635, + "heading": -0.0442182245974014, + "angularVelocity": -1.4110125396572948, + "velocityX": 2.0542828684448438, + "velocityY": 0.4921571819104624, + "timestamp": 0.34541538919364 + }, + { + "x": 4.888478268982772, + "y": 6.426870152464857, + "heading": -0.13181305030525106, + "angularVelocity": -1.52155627887344, + "velocityX": 2.4044434410455024, + "velocityY": 0.5560824683107718, + "timestamp": 0.40298462072591335 + }, + { + "x": 5.047249482849422, + "y": 6.461748968300146, + "heading": -0.22073086511138704, + "angularVelocity": -1.5445371153910277, + "velocityX": 2.757917895388995, + "velocityY": 0.6058586315458515, + "timestamp": 0.4605538522581867 + }, + { + "x": 5.226429049191539, + "y": 6.497909114202001, + "heading": -0.3013203844421244, + "angularVelocity": -1.39987137548568, + "velocityX": 3.1124189358280567, + "velocityY": 0.6281158344381212, + "timestamp": 0.51812308379046 + }, + { + "x": 5.423903550460318, + "y": 6.531166211616276, + "heading": -0.3435648346153767, + "angularVelocity": -0.7338025721182303, + "velocityX": 3.430209089347909, + "velocityY": 0.5776887502073221, + "timestamp": 0.5756923153227334 + }, + { + "x": 5.636616262089249, + "y": 6.55438565863319, + "heading": -0.34498723844741225, + "angularVelocity": -0.024707709208140263, + "velocityX": 3.69490274522221, + "velocityY": 0.4033308487694046, + "timestamp": 0.6332615468550067 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -0.34498726323171464, + "angularVelocity": -4.305129971148693e-7, + "velocityX": 3.7246965125733036, + "velocityY": 0.047618149403998285, + "timestamp": 0.69083077838728 + }, + { + "x": 5.970747823515829, + "y": 6.552244502208151, + "heading": -0.34498728566710496, + "angularVelocity": -6.97576178834182e-7, + "velocityX": 3.7219058963692446, + "velocityY": -0.15180985637112251, + "timestamp": 0.7229926999510594 + }, + { + "x": 6.090018554613485, + "y": 6.540962017726213, + "heading": -0.3449873062008861, + "angularVelocity": -6.384500714125073e-7, + "velocityX": 3.7084454316926947, + "velocityY": -0.35080256195400317, + "timestamp": 0.7551546215148388 + }, + { + "x": 6.208514441050117, + "y": 6.52331189505608, + "heading": -0.34498732528503373, + "angularVelocity": -5.933770972523468e-7, + "velocityX": 3.68435344267748, + "velocityY": -0.5487894320969752, + "timestamp": 0.7873165430786182 + }, + { + "x": 6.32589577532509, + "y": 6.499344741803893, + "heading": -0.34498734327209396, + "angularVelocity": -5.592657200372931e-7, + "velocityX": 3.6496990405936383, + "velocityY": -0.7452027766642643, + "timestamp": 0.8194784646423976 + }, + { + "x": 6.441826048149789, + "y": 6.4691292818938315, + "heading": -0.3449873604472264, + "angularVelocity": -5.340207168118268e-7, + "velocityX": 3.6045816663908092, + "velocityY": -0.9394793109653794, + "timestamp": 0.851640386206177 + }, + { + "x": 6.555972918049828, + "y": 6.432752169034089, + "heading": -0.344987377050145, + "angularVelocity": -5.162290595234125e-7, + "velocityX": 3.549130908539721, + "velocityY": -1.1310615501503813, + "timestamp": 0.8838023077699564 + }, + { + "x": 6.668009186278295, + "y": 6.390317785626105, + "heading": -0.34498739329087524, + "angularVelocity": -5.049676592749214e-7, + "velocityX": 3.483506668166316, + "velocityY": -1.319398261818203, + "timestamp": 0.9159642293337358 + }, + { + "x": 6.777613987715612, + "y": 6.341948479079241, + "heading": -0.3449874093618285, + "angularVelocity": -4.996888394768407e-7, + "velocityX": 3.4079058746525486, + "velocityY": -1.5039308659137305, + "timestamp": 0.9481261508975152 + }, + { + "x": 6.886436695218618, + "y": 6.291844380961981, + "heading": -0.3449874254078226, + "angularVelocity": -4.989127946142238e-7, + "velocityX": 3.3835884863782986, + "velocityY": -1.55787016698924, + "timestamp": 0.9802880724612946 + }, + { + "x": 6.995259275146381, + "y": 6.241740005760864, + "heading": -0.3449874414538154, + "angularVelocity": -4.98912750035862e-7, + "velocityX": 3.3835845197234478, + "velocityY": -1.5578787822660598, + "timestamp": 1.012449994025074 + }, + { + "x": 7.1040824430518015, + "y": 6.191636907618568, + "heading": -0.344987457499818, + "angularVelocity": -4.989130590435776e-7, + "velocityX": 3.3836028015184674, + "velocityY": -1.5578390750981381, + "timestamp": 1.0446119155888534 + }, + { + "x": 7.214789390563965, + "y": 6.145846366882324, + "heading": -0.344987478573796, + "angularVelocity": -6.552462348846214e-7, + "velocityX": 3.4421745383783664, + "velocityY": -1.4237501526591758, + "timestamp": 1.0767738371526328 + }, + { + "x": 7.410548108371569, + "y": 6.0752735070916195, + "heading": -0.3626370424214939, + "angularVelocity": -0.2874228799686078, + "velocityX": 3.1879277548585048, + "velocityY": -1.1492779529116102, + "timestamp": 1.1381800962842146 + }, + { + "x": 7.584442413587634, + "y": 6.012933959955544, + "heading": -0.37637990938116833, + "angularVelocity": -0.22380238031152527, + "velocityX": 2.831866126927586, + "velocityY": -1.0151985810191508, + "timestamp": 1.1995863554157964 + }, + { + "x": 7.736397172033548, + "y": 5.958610527915441, + "heading": -0.3857543231283157, + "angularVelocity": -0.15266218590290268, + "velocityX": 2.47458094003583, + "velocityY": -0.8846562680800549, + "timestamp": 1.2609926145473782 + }, + { + "x": 7.86638729375416, + "y": 5.912228690109332, + "heading": -0.3906020583128712, + "angularVelocity": -0.07894529406469342, + "velocityX": 2.1168871636044173, + "velocityY": -0.7553275262497595, + "timestamp": 1.32239887367896 + }, + { + "x": 7.974400232833448, + "y": 5.873750789689322, + "heading": -0.3908432822952403, + "angularVelocity": -0.003928328899700405, + "velocityX": 1.7589890771205783, + "velocityY": -0.6266120256171138, + "timestamp": 1.3838051328105418 + }, + { + "x": 8.060428468071539, + "y": 5.84315412501972, + "heading": -0.3864300383007793, + "angularVelocity": 0.07186961161409067, + "velocityX": 1.4009685080107037, + "velocityY": -0.498266220777903, + "timestamp": 1.4452113919421237 + }, + { + "x": 8.124466995265989, + "y": 5.820423539830739, + "heading": -0.377330476340927, + "angularVelocity": 0.14818622870925488, + "velocityX": 1.042866445539805, + "velocityY": -0.37016723556265896, + "timestamp": 1.5066176510737055 + }, + { + "x": 8.166512252703873, + "y": 5.805548220947723, + "heading": -0.36352195759750816, + "angularVelocity": 0.2248715186155497, + "velocityX": 0.6847063806278847, + "velocityY": -0.242244342732895, + "timestamp": 1.5680239102052873 + }, + { + "x": 8.186561584472656, + "y": 5.798520088195801, + "heading": -0.344987478573796, + "angularVelocity": 0.30183371020853555, + "velocityX": 0.3265030642205698, + "velocityY": -0.11445303542856088, + "timestamp": 1.6294301693368691 + }, + { + "x": 8.182873429152231, + "y": 5.799961021839805, + "heading": -0.3197766551885773, + "angularVelocity": 0.3842915682549792, + "velocityX": -0.056218988582698255, + "velocityY": 0.021964322281146666, + "timestamp": 1.6950335430973134 + }, + { + "x": 8.154073481017848, + "y": 5.810352783086787, + "heading": -0.28949615112813754, + "angularVelocity": 0.46156931152673975, + "velocityX": -0.43900102210518965, + "velocityY": 0.15840284807498262, + "timestamp": 1.7606369168577576 + }, + { + "x": 8.100157139869356, + "y": 5.829696916957608, + "heading": -0.2545716989464078, + "angularVelocity": 0.5323575630311173, + "velocityX": -0.8218531770236625, + "velocityY": 0.29486492480488535, + "timestamp": 1.8262402906182018 + }, + { + "x": 8.021119019409117, + "y": 5.8579951101346595, + "heading": -0.2155530876775793, + "angularVelocity": 0.594765315139245, + "velocityX": -1.204787435915305, + "velocityY": 0.43135271183437995, + "timestamp": 1.891843664378646 + }, + { + "x": 7.9169528918289975, + "y": 5.89524913213786, + "heading": -0.17317932530550872, + "angularVelocity": 0.6459082809795952, + "velocityX": -1.5878166260242859, + "velocityY": 0.567867471865644, + "timestamp": 1.9574470381390903 + }, + { + "x": 7.787651961744212, + "y": 5.941460642017215, + "heading": -0.12849929599301224, + "angularVelocity": 0.6810629812370456, + "velocityX": -1.9709493989888005, + "velocityY": 0.7044075209927304, + "timestamp": 2.0230504118995345 + }, + { + "x": 7.633210550125228, + "y": 5.99663052819492, + "heading": -0.08312171419135361, + "angularVelocity": 0.6916958564868655, + "velocityX": -2.3541687380734118, + "velocityY": 0.8409611124446534, + "timestamp": 2.0886537856599787 + }, + { + "x": 7.453632499858265, + "y": 6.060756206322318, + "heading": -0.039828348341138756, + "angularVelocity": 0.659925905766733, + "velocityX": -2.7373294995880113, + "velocityY": 0.9774753103637095, + "timestamp": 2.154257159420423 + }, + { + "x": 7.248984054172477, + "y": 6.133814732067644, + "heading": -0.004587896529415136, + "angularVelocity": 0.5371743828359606, + "velocityX": -3.119480507711035, + "velocityY": 1.1136397651149037, + "timestamp": 2.219860533180867 + }, + { + "x": 7.020152924958556, + "y": 6.215343825965371, + "heading": -7.99601094738583e-8, + "angularVelocity": 0.06993263160608797, + "velocityX": -3.488100018293493, + "velocityY": 1.2427576391945405, + "timestamp": 2.2854639069413114 + }, + { + "x": 6.789969979123711, + "y": 6.297403529442868, + "heading": -7.492709232609331e-8, + "angularVelocity": 7.671887677824965e-8, + "velocityX": -3.508705919231764, + "velocityY": 1.2508457838943434, + "timestamp": 2.3510672807017556 + }, + { + "x": 6.559787033588477, + "y": 6.379463233760796, + "heading": -6.989407741573959e-8, + "angularVelocity": 7.671884267312441e-8, + "velocityX": -3.5087059146647626, + "velocityY": 1.2508457967051358, + "timestamp": 2.4166706544622 + }, + { + "x": 6.3296038306461995, + "y": 6.461522216031207, + "heading": -6.486106191272836e-8, + "angularVelocity": 7.671885170707308e-8, + "velocityX": -3.508709838350823, + "velocityY": 1.250834790449275, + "timestamp": 2.482274028222644 + }, + { + "x": 6.0929382858404795, + "y": 6.522410665825721, + "heading": -5.975605523435043e-8, + "angularVelocity": 7.78162217238893e-8, + "velocityX": -3.607520943510018, + "velocityY": 0.9281298552853697, + "timestamp": 2.5478774019830883 + }, + { + "x": 5.851044178009033, + "y": 6.557126998901367, + "heading": -5.43646913463662e-8, + "angularVelocity": 8.218119860224255e-8, + "velocityX": -3.6872205492775545, + "velocityY": 0.5291851788357008, + "timestamp": 2.6134807757435325 + }, + { + "x": 5.661580826065391, + "y": 6.5680717657534915, + "heading": -4.9482596020305806e-8, + "angularVelocity": 9.582614101786845e-8, + "velocityX": -3.718801184433256, + "velocityY": 0.21482472211899678, + "timestamp": 2.6644282027629513 + }, + { + "x": 5.471871496557958, + "y": 6.562921997343888, + "heading": -4.486806959492394e-8, + "angularVelocity": 9.057427814015043e-8, + "velocityX": -3.7236292508966913, + "velocityY": -0.10108004880484801, + "timestamp": 2.71537562978237 + }, + { + "x": 5.28328089006521, + "y": 6.541715087274231, + "heading": -4.036796232249368e-8, + "angularVelocity": 8.83284502417564e-8, + "velocityX": -3.7016708698726792, + "velocityY": -0.41625085525070266, + "timestamp": 2.766323056801789 + }, + { + "x": 5.095501405649377, + "y": 6.514237548522189, + "heading": -3.588263316902823e-8, + "angularVelocity": 8.803838419074343e-8, + "velocityX": -3.685750103616821, + "velocityY": -0.5393312353451875, + "timestamp": 2.8172704838212077 + }, + { + "x": 4.907721944965666, + "y": 6.486759847587045, + "heading": -3.139730406205809e-8, + "angularVelocity": 8.803838327812946e-8, + "velocityX": -3.685749637800889, + "velocityY": -0.5393344186875336, + "timestamp": 2.8682179108406265 + }, + { + "x": 4.719942484237288, + "y": 6.459282146957154, + "heading": -2.6911974883170177e-8, + "angularVelocity": 8.803838468973724e-8, + "velocityX": -3.6857496386776267, + "velocityY": -0.5393344126960062, + "timestamp": 2.9191653378600453 + }, + { + "x": 4.532163023549714, + "y": 6.431804446048415, + "heading": -2.2426645714855718e-8, + "angularVelocity": 8.803838448220064e-8, + "velocityX": -3.685749637876727, + "velocityY": -0.5393344181692645, + "timestamp": 2.970112764879464 + }, + { + "x": 4.34438356286167, + "y": 6.404326745142886, + "heading": -1.794131654726717e-8, + "angularVelocity": 8.803838446795243e-8, + "velocityX": -3.6857496378859507, + "velocityY": -0.5393344181062291, + "timestamp": 3.021060191898883 + }, + { + "x": 4.156604102201168, + "y": 6.376849044049133, + "heading": -1.3455987422255126e-8, + "angularVelocity": 8.803838363225754e-8, + "velocityX": -3.685749637345336, + "velocityY": -0.5393344218007294, + "timestamp": 3.0720076189183017 + }, + { + "x": 3.9688246409539834, + "y": 6.349371346964709, + "heading": -8.97065828327331e-9, + "angularVelocity": 8.80383839064573e-8, + "velocityX": -3.685749648860808, + "velocityY": -0.5393343431053033, + "timestamp": 3.1229550459377204 + }, + { + "x": 3.7810451818268453, + "y": 6.32189363539213, + "heading": -4.485329161078249e-9, + "angularVelocity": 8.80383835769656e-8, + "velocityX": -3.685749607248367, + "velocityY": -0.5393346274799247, + "timestamp": 3.1739024729571392 + }, + { + "x": 3.5932657187551267, + "y": 6.294415950776365, + "heading": 7.629972110817107e-24, + "angularVelocity": 8.803838434016792e-8, + "velocityX": -3.6857496846728943, + "velocityY": -0.5393340983695197, + "timestamp": 3.224849899976558 + }, + { + "x": 3.405486822128296, + "y": 6.266934394836426, + "heading": 3.646452356482733e-24, + "angularVelocity": -1.9320923857305114e-25, + "velocityX": -3.6857385664492472, + "velocityY": -0.5394100850169479, + "timestamp": 3.275797326995977 + }, + { + "x": 3.2165781142286494, + "y": 6.223116050022905, + "heading": 3.318409186414545e-24, + "angularVelocity": -1.5338201174379227e-26, + "velocityX": -3.536673395714516, + "velocityY": -0.8203495543918979, + "timestamp": 3.329211563133403 + }, + { + "x": 3.041563345872727, + "y": 6.190252158258573, + "heading": 2.990185107220459e-24, + "angularVelocity": -1.87251095899189e-26, + "velocityX": -3.2765566075987578, + "velocityY": -0.6152646586535303, + "timestamp": 3.382625799270829 + }, + { + "x": 2.8804424282697485, + "y": 6.168342842771101, + "heading": 2.6619349218872592e-24, + "angularVelocity": -1.9213858301671557e-26, + "velocityX": -3.0164414817885197, + "velocityY": -0.4101774558958965, + "timestamp": 3.436040035408255 + }, + { + "x": 2.733215332226118, + "y": 6.157388144124356, + "heading": 2.333764868738876e-24, + "angularVelocity": -1.7713655535187625e-26, + "velocityX": -2.7563269025291364, + "velocityY": -0.20508949371775206, + "timestamp": 3.4894542715456813 + }, + { + "x": 2.5998820432630403, + "y": 6.157388082450519, + "heading": 2.0056312293962657e-24, + "angularVelocity": -1.7031930803836163e-26, + "velocityX": -2.4962125943359705, + "velocityY": -0.0000011546329394271484, + "timestamp": 3.5428685076831075 + }, + { + "x": 2.4804425527424723, + "y": 6.16834266976643, + "heading": 1.6774969061310235e-24, + "angularVelocity": -1.7044734930100497e-26, + "velocityX": -2.2360984478608033, + "velocityY": 0.20508740942632245, + "timestamp": 3.5962827438205336 + }, + { + "x": 2.3748968549301686, + "y": 6.190251914052291, + "heading": 1.3492831778827425e-24, + "angularVelocity": -1.853132331504253e-26, + "velocityX": -1.97598440873988, + "velocityY": 0.4101761228877669, + "timestamp": 3.6496969799579597 + }, + { + "x": 2.283244945743567, + "y": 6.22311582099134, + "heading": 1.0211293460600033e-24, + "angularVelocity": -1.7409966353369085e-26, + "velocityX": -1.7158704460510679, + "velocityY": 0.6152649427485215, + "timestamp": 3.703111216095386 + }, + { + "x": 2.205486822128296, + "y": 6.266934394836426, + "heading": 6.9419459427461045e-25, + "angularVelocity": 5.413163312168308e-27, + "velocityX": -1.455756540544968, + "velocityY": 0.8203538422294017, + "timestamp": 3.756525452232812 + }, + { + "x": 2.136159340261779, + "y": 6.327632048563378, + "heading": 0.025231456124497797, + "angularVelocity": 0.43032288322850026, + "velocityX": -1.1823812996193095, + "velocityY": 1.035199444221815, + "timestamp": 3.8151592295843644 + }, + { + "x": 2.084207176746768, + "y": 6.399028713640929, + "heading": 0.07562693390361644, + "angularVelocity": 0.8594956705068004, + "velocityX": -0.8860449703507888, + "velocityY": 1.2176712520067714, + "timestamp": 3.8737930069359168 + }, + { + "x": 2.051566754965305, + "y": 6.477423275068575, + "heading": 0.14957404640599054, + "angularVelocity": 1.2611691731714223, + "velocityX": -0.5566829096778134, + "velocityY": 1.3370204849265277, + "timestamp": 3.932426784287469 + }, + { + "x": 2.039926841667985, + "y": 6.553838491753715, + "heading": 0.2360367067446735, + "angularVelocity": 1.4746220394479557, + "velocityX": -0.19851890536627906, + "velocityY": 1.3032627290405507, + "timestamp": 3.9910605616390216 + }, + { + "x": 2.042236938900771, + "y": 6.614274854513032, + "heading": 0.313525546116211, + "angularVelocity": 1.3215733809359698, + "velocityX": 0.039398744838411756, + "velocityY": 1.0307431226365935, + "timestamp": 4.049694338990574 + }, + { + "x": 2.0480382687015357, + "y": 6.654459061696839, + "heading": 0.3665932060292166, + "angularVelocity": 0.9050697790597129, + "velocityX": 0.09894177149770778, + "velocityY": 0.6853422890166648, + "timestamp": 4.108328116342126 + }, + { + "x": 2.052253484725952, + "y": 6.674348831176758, + "heading": 0.3930579718029317, + "angularVelocity": 0.4513569988001917, + "velocityX": 0.07189057595834777, + "velocityY": 0.3392203330286158, + "timestamp": 4.166961893693679 + }, + { + "x": 2.052253484725952, + "y": 6.674348831176758, + "heading": 0.3930579718029317, + "angularVelocity": 7.733121458673518e-26, + "velocityX": -6.859499621919341e-22, + "velocityY": -2.8560302702061698e-22, + "timestamp": 4.225595671045231 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePEDABC.3.traj b/src/main/deploy/choreo/CenterLanePEDABC.3.traj new file mode 100644 index 00000000..312e12a6 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePEDABC.3.traj @@ -0,0 +1,86 @@ +{ + "samples": [ + { + "x": 2.052253484725952, + "y": 6.674348831176758, + "heading": 0.3930579718029317, + "angularVelocity": 7.733121458673518e-26, + "velocityX": -6.859499621919341e-22, + "velocityY": -2.8560302702061698e-22, + "timestamp": 0 + }, + { + "x": 2.103013204396169, + "y": 6.695498705680337, + "heading": 0.3930579718029317, + "angularVelocity": 4.836322083094603e-17, + "velocityX": 0.5390415085998568, + "velocityY": 0.22460053627515192, + "timestamp": 0.09416662513071294 + }, + { + "x": 2.204532642750927, + "y": 6.737798454276797, + "heading": 0.3930579718029317, + "angularVelocity": 1.8057826879047683e-17, + "velocityX": 1.078083006732355, + "velocityY": 0.4492010681889061, + "timestamp": 0.1883332502614259 + }, + { + "x": 2.356811797490316, + "y": 6.801248076007844, + "heading": 0.3930579718029317, + "angularVelocity": 9.246174062812876e-17, + "velocityX": 1.6171244804410174, + "velocityY": 0.6738015899260662, + "timestamp": 0.28249987539213883 + }, + { + "x": 2.5598506571147865, + "y": 6.885847566082, + "heading": 0.3930579718029317, + "angularVelocity": 1.8302909821567636e-16, + "velocityX": 2.1561658320305157, + "velocityY": 0.8984020607802621, + "timestamp": 0.3766665005228518 + }, + { + "x": 2.7121298118541755, + "y": 6.9492971878130465, + "heading": 0.3930579718029317, + "angularVelocity": 1.1024323128215209e-16, + "velocityX": 1.6171244804410174, + "velocityY": 0.6738015899260661, + "timestamp": 0.4708331256535647 + }, + { + "x": 2.8136492502089334, + "y": 6.991596936409507, + "heading": 0.3930579718029317, + "angularVelocity": -3.9462401223913846e-17, + "velocityX": 1.078083006732355, + "velocityY": 0.44920106818890604, + "timestamp": 0.5649997507842777 + }, + { + "x": 2.8644089698791504, + "y": 7.012746810913086, + "heading": 0.3930579718029317, + "angularVelocity": 1.2061194980042807e-16, + "velocityX": 0.5390415085998568, + "velocityY": 0.22460053627515186, + "timestamp": 0.6591663759149906 + }, + { + "x": 2.8644089698791504, + "y": 7.012746810913086, + "heading": 0.3930579718029317, + "angularVelocity": -7.454320302880939e-28, + "velocityX": 6.858010204674672e-22, + "velocityY": 2.856861212118512e-22, + "timestamp": 0.7533330010457036 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePEDABC.4.traj b/src/main/deploy/choreo/CenterLanePEDABC.4.traj new file mode 100644 index 00000000..5ae8c7b4 --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePEDABC.4.traj @@ -0,0 +1,248 @@ +{ + "samples": [ + { + "x": 2.8644089698791504, + "y": 7.012746810913086, + "heading": 0.3930579718029317, + "angularVelocity": -7.454320302880939e-28, + "velocityX": 6.858010204674672e-22, + "velocityY": 2.856861212118512e-22, + "timestamp": 0 + }, + { + "x": 2.842186915849872, + "y": 7.004763611912437, + "heading": 0.3899906597418949, + "angularVelocity": -0.04969534926930016, + "velocityX": -0.36003273044640205, + "velocityY": -0.1293405609631654, + "timestamp": 0.061722316195323756 + }, + { + "x": 2.7978951293903465, + "y": 6.988387145564426, + "heading": 0.38377625605112037, + "angularVelocity": -0.10068325483944376, + "velocityX": -0.7175976079601634, + "velocityY": -0.26532488340499644, + "timestamp": 0.12344463239064751 + }, + { + "x": 2.7317542660001495, + "y": 6.96306112048486, + "heading": 0.37430680047739806, + "angularVelocity": -0.15342028876161432, + "velocityX": -1.071587514326101, + "velocityY": -0.41032201383078476, + "timestamp": 0.18516694858597127 + }, + { + "x": 2.644108850979856, + "y": 6.927989989672691, + "heading": 0.36142823816874126, + "angularVelocity": -0.20865325707968818, + "velocityX": -1.4199955611343862, + "velocityY": -0.5682082749646841, + "timestamp": 0.24688926478129503 + }, + { + "x": 2.53555910261038, + "y": 6.881949867747595, + "heading": 0.3449043845685677, + "angularVelocity": -0.267712792045632, + "velocityX": -1.7586791141467173, + "velocityY": -0.7459234319625763, + "timestamp": 0.3086115809766188 + }, + { + "x": 2.407346628684672, + "y": 6.822852176062135, + "heading": 0.3243353524397527, + "angularVelocity": -0.33325113827101066, + "velocityX": -2.0772466399344327, + "velocityY": -0.9574768953653868, + "timestamp": 0.37033389717194254 + }, + { + "x": 2.2628964345938334, + "y": 6.74663530878728, + "heading": 0.298961324703692, + "angularVelocity": -0.41109973345399, + "velocityX": -2.3403236138079526, + "velocityY": -1.2348348534695668, + "timestamp": 0.4320562133672663 + }, + { + "x": 2.1156790935491716, + "y": 6.646996400731561, + "heading": 0.26784048264776694, + "angularVelocity": -0.5042072944482706, + "velocityX": -2.385155809428528, + "velocityY": -1.6143092838642825, + "timestamp": 0.49377852956259005 + }, + { + "x": 1.9869123530220612, + "y": 6.532612256734745, + "heading": 0.23448482199122617, + "angularVelocity": -0.540414921419745, + "velocityX": -2.0862266432066443, + "velocityY": -1.8532056320576245, + "timestamp": 0.5555008457579138 + }, + { + "x": 1.881043758757729, + "y": 6.412413108766105, + "heading": 0.20123842984354556, + "angularVelocity": -0.5386445972388703, + "velocityX": -1.7152401398758959, + "velocityY": -1.9474179742098816, + "timestamp": 0.6172231619532376 + }, + { + "x": 1.7987115747047, + "y": 6.290184489368743, + "heading": 0.16896416101959177, + "angularVelocity": -0.5228946483767686, + "velocityX": -1.3339127422322181, + "velocityY": -1.9802986493663424, + "timestamp": 0.6789454781485613 + }, + { + "x": 1.740001559257507, + "y": 6.167843341827393, + "heading": 0.13807920449538028, + "angularVelocity": -0.5003855724803669, + "velocityX": -0.9511959217700356, + "velocityY": -1.9821217848370036, + "timestamp": 0.7406677943438851 + }, + { + "x": 1.7034889997175517, + "y": 6.016151230649133, + "heading": 0.1019414560444192, + "angularVelocity": -0.467252098060329, + "velocityX": -0.4720983122052568, + "velocityY": -1.961341263510758, + "timestamp": 0.8180088027652008 + }, + { + "x": 1.7027290435050462, + "y": 5.874298147554192, + "heading": 0.06966485107958241, + "angularVelocity": -0.4173284732597485, + "velocityX": -0.009826044785523666, + "velocityY": -1.8341250778913685, + "timestamp": 0.8953498111865166 + }, + { + "x": 1.7342944275877592, + "y": 5.750595898188055, + "heading": 0.04250435573420922, + "angularVelocity": -0.35117844853297475, + "velocityX": 0.40813256417294685, + "velocityY": -1.5994393128709739, + "timestamp": 0.9726908196078323 + }, + { + "x": 1.7930814617897048, + "y": 5.6520476897956975, + "heading": 0.02146340510922574, + "angularVelocity": -0.2720542575597529, + "velocityX": 0.7601017287194328, + "velocityY": -1.2742038202491095, + "timestamp": 1.050031828029148 + }, + { + "x": 1.8734294527078426, + "y": 5.583639683859189, + "heading": 0.0071877988424821045, + "angularVelocity": -0.1845800379143917, + "velocityX": 1.0388795356848963, + "velocityY": -0.8844984999918234, + "timestamp": 1.1273728364504638 + }, + { + "x": 1.970064237353583, + "y": 5.548515796661377, + "heading": 3.498573366455402e-27, + "angularVelocity": -0.09293645103935767, + "velocityX": 1.249463727177213, + "velocityY": -0.45414312425918, + "timestamp": 1.2047138448717796 + }, + { + "x": 2.0785037517547607, + "y": 5.548515796661377, + "heading": 9.074763160083063e-28, + "angularVelocity": -2.8883864105140666e-27, + "velocityX": 1.4020959464409022, + "velocityY": 1.1648614692434482e-25, + "timestamp": 1.2820548532930953 + }, + { + "x": 2.2093539983390484, + "y": 5.548515796661377, + "heading": -5.091869940539703e-28, + "angularVelocity": -8.863496880486678e-29, + "velocityX": 1.8425023918344958, + "velocityY": 6.335504596026049e-17, + "timestamp": 1.3530725340104635 + }, + { + "x": 2.3657372440174322, + "y": 5.548515796661377, + "heading": -1.9258503020667073e-27, + "angularVelocity": -8.863493994530242e-29, + "velocityX": 2.202032565675437, + "velocityY": 3.167727717458953e-16, + "timestamp": 1.4240902147278316 + }, + { + "x": 2.4908438453710073, + "y": 5.548515796661377, + "heading": -3.342513611103023e-27, + "angularVelocity": -8.863495435827685e-29, + "velocityX": 1.7616261202821912, + "velocityY": 2.534184135606925e-16, + "timestamp": 1.4951078954451997 + }, + { + "x": 2.5846737979725942, + "y": 5.548515796661377, + "heading": -4.759176920139342e-27, + "angularVelocity": -8.863495435827717e-29, + "velocityX": 1.321219612549812, + "velocityY": 1.9006387498434594e-16, + "timestamp": 1.5661255761625679 + }, + { + "x": 2.6472271003464662, + "y": 5.548515796661377, + "heading": -6.175840229175665e-27, + "angularVelocity": -8.863495435827698e-29, + "velocityX": 0.8808130840377387, + "velocityY": 1.2670927613090377e-16, + "timestamp": 1.637143256879936 + }, + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": -7.592503538214036e-27, + "angularVelocity": -8.863495438700938e-29, + "velocityX": 0.44040654513582195, + "velocityY": 6.3354647036682e-17, + "timestamp": 1.7081609375973041 + }, + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": -9.168166896056586e-27, + "angularVelocity": -2.3275147262701813e-27, + "velocityX": -7.876142714980879e-26, + "velocityY": -2.7822667519177177e-25, + "timestamp": 1.7791786183146723 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterLanePEDABC.5.traj b/src/main/deploy/choreo/CenterLanePEDABC.5.traj new file mode 100644 index 00000000..1258a60c --- /dev/null +++ b/src/main/deploy/choreo/CenterLanePEDABC.5.traj @@ -0,0 +1,212 @@ +{ + "samples": [ + { + "x": 2.6785037517547607, + "y": 5.548515796661377, + "heading": -9.168166896056586e-27, + "angularVelocity": -2.3275147262701813e-27, + "velocityX": -7.876142714980879e-26, + "velocityY": -2.7822667519177177e-25, + "timestamp": 0 + }, + { + "x": 2.6551487281624913, + "y": 5.535337362195577, + "heading": -0.0053582663887861755, + "angularVelocity": -0.08143329787865013, + "velocityX": -0.3549425234125067, + "velocityY": -0.20028182653884286, + "timestamp": 0.06579945216968852 + }, + { + "x": 2.608944125248311, + "y": 5.508126235802502, + "heading": -0.016249929633628882, + "angularVelocity": -0.16552817517013058, + "velocityX": -0.7022034590048659, + "velocityY": -0.41354639736059495, + "timestamp": 0.13159890433937704 + }, + { + "x": 2.5407349136089206, + "y": 5.4655968271259, + "heading": -0.03293816472196888, + "angularVelocity": -0.2536227056313985, + "velocityX": -1.0366227892519315, + "velocityY": -0.6463489782091855, + "timestamp": 0.19739835650906556 + }, + { + "x": 2.4521626111212957, + "y": 5.405634188324023, + "heading": -0.05584758703948852, + "angularVelocity": -0.34817041118274633, + "velocityX": -1.3460948315984256, + "velocityY": -0.9112938911290825, + "timestamp": 0.2631978086787541 + }, + { + "x": 2.347299393995622, + "y": 5.324395116461008, + "heading": -0.08569447081932852, + "angularVelocity": -0.45360383400866283, + "velocityX": -1.593679182240697, + "velocityY": -1.2346466297851608, + "timestamp": 0.3289972608484426 + }, + { + "x": 2.239880929566337, + "y": 5.216483135728542, + "heading": -0.12272614687803589, + "angularVelocity": -0.5627961151288577, + "velocityX": -1.6325130512069779, + "velocityY": -1.6400133614209311, + "timestamp": 0.39479671301813113 + }, + { + "x": 2.1533744032985735, + "y": 5.091739159361965, + "heading": -0.16180305396789932, + "angularVelocity": -0.5938789123819668, + "velocityX": -1.314699794835295, + "velocityY": -1.8958208959685579, + "timestamp": 0.46059616518781965 + }, + { + "x": 2.0932393074035645, + "y": 4.9619622230529785, + "heading": -0.19963034769256288, + "angularVelocity": -0.5748876696892883, + "velocityX": -0.9139148414173547, + "velocityY": -1.9723102857195456, + "timestamp": 0.5263956173575082 + }, + { + "x": 2.06290111070071, + "y": 4.862373019847466, + "heading": -0.22719939375272255, + "angularVelocity": -0.5460591677053325, + "velocityX": -0.6009076413123203, + "velocityY": -1.9725599970405536, + "timestamp": 0.576882904616161 + }, + { + "x": 2.048028599586168, + "y": 4.7660067703914, + "heading": -0.2526003668147138, + "angularVelocity": -0.5031162187791788, + "velocityX": -0.29457932723436914, + "velocityY": -1.9087230605671173, + "timestamp": 0.6273701918748138 + }, + { + "x": 2.047740775609064, + "y": 4.67569763435831, + "heading": -0.27523200740159587, + "angularVelocity": -0.4482641436236724, + "velocityX": -0.00570091983015679, + "velocityY": -1.788750018800283, + "timestamp": 0.6778574791334666 + }, + { + "x": 2.0608710405337267, + "y": 4.5937072504707785, + "heading": -0.29463334748351117, + "angularVelocity": -0.3842816902108432, + "velocityX": 0.26007071557230893, + "velocityY": -1.62398077495204, + "timestamp": 0.7283447663921194 + }, + { + "x": 2.0861920181883997, + "y": 4.5217424317614965, + "heading": -0.31047411563844995, + "angularVelocity": -0.3137575618548228, + "velocityX": 0.5015317524380114, + "velocityY": -1.4254047427939316, + "timestamp": 0.7788320536507722 + }, + { + "x": 2.1225463448038515, + "y": 4.461060494351439, + "heading": -0.3225286157246152, + "angularVelocity": -0.2387630776121657, + "velocityX": 0.7200689240680372, + "velocityY": -1.2019250925322578, + "timestamp": 0.829319340909425 + }, + { + "x": 2.16890020662259, + "y": 4.412584630951135, + "heading": -0.3306480218329806, + "angularVelocity": -0.16082080359692727, + "velocityX": 0.9181293813878825, + "velocityY": -0.9601597953155385, + "timestamp": 0.8798066281680779 + }, + { + "x": 2.2243532862123785, + "y": 4.376998478332531, + "heading": -0.3347371673945201, + "angularVelocity": -0.08099356855103614, + "velocityX": 1.0983572816201457, + "velocityY": -0.7048537275590007, + "timestamp": 0.9302939154267307 + }, + { + "x": 2.2881293296813965, + "y": 4.354815483093262, + "heading": -0.3347371673945201, + "angularVelocity": -3.351040193936762e-27, + "velocityX": 1.2632099471355258, + "velocityY": -0.43937784031895566, + "timestamp": 0.9807812026853835 + }, + { + "x": 2.4097151173059284, + "y": 4.31443319431648, + "heading": -0.3347371673945201, + "angularVelocity": -1.1055944271274773e-28, + "velocityX": 1.6917387541138205, + "velocityY": -0.5618772081689679, + "timestamp": 1.0526515120513356 + }, + { + "x": 2.501147235462863, + "y": 4.284859003573599, + "heading": -0.3347371673945201, + "angularVelocity": -1.1055944267694559e-28, + "velocityX": 1.2721820590944748, + "velocityY": -0.4114938561387572, + "timestamp": 1.1245218214172876 + }, + { + "x": 2.562139216145648, + "y": 4.265257651838079, + "heading": -0.3347371673945201, + "angularVelocity": -1.105594426769472e-28, + "velocityX": 0.8486394621209057, + "velocityY": -0.2727322577076049, + "timestamp": 1.1963921307832397 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": -1.1055944267596697e-28, + "velocityX": 0.42445170461571047, + "velocityY": -0.13595549514112273, + "timestamp": 1.2682624401491918 + }, + { + "x": 2.592644691467285, + "y": 4.255486488342285, + "heading": -0.3347371673945201, + "angularVelocity": -2.916408988560576e-29, + "velocityX": -2.969429335311962e-26, + "velocityY": 1.2754135405824192e-26, + "timestamp": 1.3401327495151438 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanephChaos.traj b/src/main/deploy/choreo/SourceLanephChaos.traj deleted file mode 100644 index 308ed012..00000000 --- a/src/main/deploy/choreo/SourceLanephChaos.traj +++ /dev/null @@ -1,1481 +0,0 @@ -{ - "samples": [ - { - "x": 1.4578553438186646, - "y": 1.4268364906311035, - "heading": 1.9268906266143656e-30, - "angularVelocity": 1.1190631191470915e-30, - "velocityX": -2.439449667203012e-26, - "velocityY": -3.284743628951617e-25, - "timestamp": 0 - }, - { - "x": 1.4836061256801238, - "y": 1.4248385832903434, - "heading": -1.6036305967396036e-15, - "angularVelocity": -2.484855550358542e-14, - "velocityX": 0.3990131727631876, - "velocityY": -0.030957947265928064, - "timestamp": 0.0645361697788919 - }, - { - "x": 1.5351076891841593, - "y": 1.420842768625806, - "heading": -4.81143470802221e-15, - "angularVelocity": -4.970552361990032e-14, - "velocityX": 0.7980263421347389, - "velocityY": -0.061915894268711684, - "timestamp": 0.1290723395577838 - }, - { - "x": 1.6123600340493498, - "y": 1.4148490466593255, - "heading": -9.625413675640745e-15, - "angularVelocity": -7.459350290095871e-14, - "velocityX": 1.1970395071456144, - "velocityY": -0.09287384093316672, - "timestamp": 0.1936085093366757 - }, - { - "x": 1.7153631599004666, - "y": 1.4068574174200144, - "heading": -1.6044392495578035e-14, - "angularVelocity": -9.946327527539101e-14, - "velocityX": 1.5960526663422558, - "velocityY": -0.12383178714651694, - "timestamp": 0.2581446791155676 - }, - { - "x": 1.8441170662121902, - "y": 1.39686788094863, - "heading": -2.407162986647994e-14, - "angularVelocity": -1.2438354179375857e-13, - "velocityX": 1.9950658173989684, - "velocityY": -0.15478973272832033, - "timestamp": 0.32268084889445947 - }, - { - "x": 1.9986217521965406, - "y": 1.3848804373063093, - "heading": -3.3704756925123664e-14, - "angularVelocity": -1.4926710233420884e-13, - "velocityX": 2.3940789562457856, - "velocityY": -0.1857476773628035, - "timestamp": 0.38721701867335134 - }, - { - "x": 2.1788772165402177, - "y": 1.3708950865949459, - "heading": -4.494558790872857e-14, - "angularVelocity": -1.7417877481910134e-13, - "velocityX": 2.793092074742775, - "velocityY": -0.2167056204184192, - "timestamp": 0.4517531884522432 - }, - { - "x": 2.3848834566166217, - "y": 1.3549118290183289, - "heading": -5.77927518216841e-14, - "angularVelocity": -1.9906920347103564e-13, - "velocityX": 3.1921051525401003, - "velocityY": -0.2476635603162993, - "timestamp": 0.5162893582311351 - }, - { - "x": 2.6166404645459465, - "y": 1.3369306651878214, - "heading": -7.224642245471218e-14, - "angularVelocity": -2.239623250426539e-13, - "velocityX": 3.5911181082383807, - "velocityY": -0.27862149074096887, - "timestamp": 0.580825528010027 - }, - { - "x": 2.856317459743021, - "y": 1.3183350190196794, - "heading": -7.226675860496505e-14, - "angularVelocity": -3.151124450451814e-16, - "velocityX": 3.7138397896595787, - "velocityY": -0.2881430092900255, - "timestamp": 0.6453616977889188 - }, - { - "x": 3.095994472503662, - "y": 1.2997395992279053, - "heading": 8.152675815601398e-24, - "angularVelocity": 1.119786917331966e-12, - "velocityX": 3.7138400618103087, - "velocityY": -0.28813950154594287, - "timestamp": 0.7098978675678107 - }, - { - "x": 3.27382254607751, - "y": 1.289661887920689, - "heading": 0.031144225689139507, - "angularVelocity": 0.6214983848668802, - "velocityX": 3.548646918156489, - "velocityY": -0.20110569975653228, - "timestamp": 0.7600093826537434 - }, - { - "x": 3.4512411724559873, - "y": 1.2794797717459663, - "heading": 0.06298682604506026, - "angularVelocity": 0.6354347958012462, - "velocityX": 3.5404761974215813, - "velocityY": -0.20318915038314128, - "timestamp": 0.8101208977396761 - }, - { - "x": 3.62822231090223, - "y": 1.2691838802186675, - "heading": 0.09560610924482929, - "angularVelocity": 0.6509338850328615, - "velocityX": 3.5317459099520483, - "velocityY": -0.20545959366110889, - "timestamp": 0.8602324128256088 - }, - { - "x": 3.8047300835293036, - "y": 1.258765647314116, - "heading": 0.12910425796921293, - "angularVelocity": 0.6684720800586451, - "velocityX": 3.5222996615526942, - "velocityY": -0.20790097618653766, - "timestamp": 0.9103439279115415 - }, - { - "x": 3.9807203074364863, - "y": 1.248215472268555, - "heading": 0.16360694737365913, - "angularVelocity": 0.6885181847980438, - "velocityX": 3.5119717215771487, - "velocityY": -0.21053394668808642, - "timestamp": 0.9604554429974742 - }, - { - "x": 4.156137440439902, - "y": 1.2375224650571883, - "heading": 0.19927204657899403, - "angularVelocity": 0.7117146457091811, - "velocityX": 3.50053540992732, - "velocityY": -0.2133842330057237, - "timestamp": 1.010566958083407 - }, - { - "x": 4.330909871790797, - "y": 1.2266740963817457, - "heading": 0.23630304667549967, - "angularVelocity": 0.7389718716946344, - "velocityX": 3.487670070465641, - "velocityY": -0.21648454765016575, - "timestamp": 1.0606784731693397 - }, - { - "x": 4.504942288680689, - "y": 1.2156556997827002, - "heading": 0.27497084629517315, - "angularVelocity": 0.7716350134966949, - "velocityX": 3.4729027168996156, - "velocityY": -0.21987753872839294, - "timestamp": 1.1107899882552725 - }, - { - "x": 4.678102490303239, - "y": 1.204449734224157, - "heading": 0.31565141892272963, - "angularVelocity": 0.8118008916273269, - "velocityX": 3.4554972310378296, - "velocityY": -0.22362056982965403, - "timestamp": 1.1609015033412053 - }, - { - "x": 4.850196619661247, - "y": 1.1930346345230565, - "heading": 0.3588966354181107, - "angularVelocity": 0.8629796249668915, - "velocityX": 3.4342232331809583, - "velocityY": -0.22779394479542386, - "timestamp": 1.2110130184271382 - }, - { - "x": 5.020916905370504, - "y": 1.1813828932131683, - "heading": 0.40558384210290876, - "angularVelocity": 0.931666236886622, - "velocityX": 3.4068075055503675, - "velocityY": -0.23251624481733327, - "timestamp": 1.261124533513071 - }, - { - "x": 5.189710400965398, - "y": 1.16945753417408, - "heading": 0.45729195406784345, - "angularVelocity": 1.0318608782086127, - "velocityX": 3.3683574584692115, - "velocityY": -0.2379764215597622, - "timestamp": 1.3112360485990038 - }, - { - "x": 5.35533172203286, - "y": 1.1572047491345996, - "heading": 0.5175844077279528, - "angularVelocity": 1.2031656507834203, - "velocityX": 3.3050551511653112, - "velocityY": -0.244510368893633, - "timestamp": 1.3613475636849366 - }, - { - "x": 5.512680992157926, - "y": 1.1445116485154183, - "heading": 0.6010844246558354, - "angularVelocity": 1.6662840224386364, - "velocityX": 3.1399822945931812, - "velocityY": -0.25329708346305213, - "timestamp": 1.4114590787708694 - }, - { - "x": 5.678954750693719, - "y": 1.128816110929195, - "heading": 0.6579086700766517, - "angularVelocity": 1.1339558447469087, - "velocityX": 3.3180748626470646, - "velocityY": -0.3132121940298074, - "timestamp": 1.4615705938568022 - }, - { - "x": 5.854338980055963, - "y": 1.1023762612022525, - "heading": 0.6848690400817262, - "angularVelocity": 0.5380074810917644, - "velocityX": 3.4998788015387303, - "velocityY": -0.5276202422058656, - "timestamp": 1.511682108942735 - }, - { - "x": 6.029328591050374, - "y": 1.0604680679443284, - "heading": 0.7046293616016649, - "angularVelocity": 0.39432696229705316, - "velocityX": 3.4920039973713513, - "velocityY": -0.8362986668046034, - "timestamp": 1.5617936240286678 - }, - { - "x": 6.202812194824219, - "y": 1.003179669380188, - "heading": 0.7165423509110156, - "angularVelocity": 0.2377295775017328, - "velocityX": 3.4619508804782524, - "velocityY": -1.1432182496558028, - "timestamp": 1.6119051391146006 - }, - { - "x": 6.402547568535553, - "y": 0.9154521762914801, - "heading": 0.7165423052284949, - "angularVelocity": -7.80040343805035e-7, - "velocityX": 3.4105309321224193, - "velocityY": -1.4979686533092873, - "timestamp": 1.6704694441118393 - }, - { - "x": 6.592790671542091, - "y": 0.8086912505756066, - "heading": 0.7165422622385289, - "angularVelocity": -7.34064308321193e-7, - "velocityX": 3.2484480609051705, - "velocityY": -1.8229692253823835, - "timestamp": 1.729033749109078 - }, - { - "x": 6.792526057186022, - "y": 0.7209637957417591, - "heading": 0.7165421996040524, - "angularVelocity": -0.0000010694991862536609, - "velocityX": 3.410531135874468, - "velocityY": -1.4979680000980806, - "timestamp": 1.7875980541063166 - }, - { - "x": 6.995358135542775, - "y": 0.6540115645265273, - "heading": 0.702913894528627, - "angularVelocity": -0.23270668158817906, - "velocityX": 3.4634079302455083, - "velocityY": -1.1432259158268625, - "timestamp": 1.8461623591035552 - }, - { - "x": 7.192981165480996, - "y": 0.6064556135405038, - "heading": 0.6594934468717042, - "angularVelocity": -0.7414148884541547, - "velocityX": 3.3744621394813583, - "velocityY": -0.8120296311595578, - "timestamp": 1.9047266641007938 - }, - { - "x": 7.3826916988169575, - "y": 0.5778864686152094, - "heading": 0.5928355536169142, - "angularVelocity": -1.138200022316206, - "velocityX": 3.239354301991779, - "velocityY": -0.4878252192464604, - "timestamp": 1.9632909690980325 - }, - { - "x": 7.56045020150032, - "y": 0.5667919314464598, - "heading": 0.5348000913331561, - "angularVelocity": -0.9909698798012667, - "velocityX": 3.0352704209798658, - "velocityY": -0.18944196758201154, - "timestamp": 2.021855274095271 - }, - { - "x": 7.725161762075186, - "y": 0.5716432246000058, - "heading": 0.4994377275755845, - "angularVelocity": -0.6038211118400366, - "velocityX": 2.812490655914593, - "velocityY": 0.08283703108531969, - "timestamp": 2.0804195790925095 - }, - { - "x": 7.876544251370993, - "y": 0.5914886747454028, - "heading": 0.49476791383000396, - "angularVelocity": -0.07973822528587567, - "velocityX": 2.5848934654470064, - "velocityY": 0.3388659721366544, - "timestamp": 2.138983884089748 - }, - { - "x": 8.014557697135269, - "y": 0.6256824372518744, - "heading": 0.5263224795620095, - "angularVelocity": 0.5388020182855987, - "velocityX": 2.35661373887698, - "velocityY": 0.5838669562984516, - "timestamp": 2.197548189086987 - }, - { - "x": 8.139220711960105, - "y": 0.6736709753190644, - "heading": 0.5988363569407734, - "angularVelocity": 1.238192400339819, - "velocityX": 2.128651826922783, - "velocityY": 0.8194161626173599, - "timestamp": 2.2561124940842254 - }, - { - "x": 8.25048828125, - "y": 0.7348634600639343, - "heading": 0.7165423509110156, - "angularVelocity": 2.0098589742641386, - "velocityX": 1.8999212796112497, - "velocityY": 1.044876819553399, - "timestamp": 2.314676799081464 - }, - { - "x": 8.302330173893822, - "y": 0.7683170831186208, - "heading": 0.7866903105882892, - "angularVelocity": 2.4170681239593663, - "velocityX": 1.7863012234653046, - "velocityY": 1.152701893090581, - "timestamp": 2.343698719887885 - }, - { - "x": 8.350761252955918, - "y": 0.804862507674631, - "heading": 0.8681048778617475, - "angularVelocity": 2.805278389962577, - "velocityX": 1.6687757983055662, - "velocityY": 1.2592352105076432, - "timestamp": 2.372720640694306 - }, - { - "x": 8.395659126629422, - "y": 0.8444668269434722, - "heading": 0.9600582095499061, - "angularVelocity": 3.168409572250435, - "velocityX": 1.5470331537659474, - "velocityY": 1.36463466815329, - "timestamp": 2.4017425615007273 - }, - { - "x": 8.436895225190787, - "y": 0.8871049250141315, - "heading": 1.0616235787520496, - "angularVelocity": 3.499608791561171, - "velocityX": 1.420860419143676, - "velocityY": 1.469168714057877, - "timestamp": 2.4307644823071484 - }, - { - "x": 8.474338814577639, - "y": 0.9327584709704907, - "heading": 1.1716968936719283, - "angularVelocity": 3.792764636568266, - "velocityX": 1.2901830184364116, - "velocityY": 1.5730711368442005, - "timestamp": 2.4597864031135694 - }, - { - "x": 8.50785937445302, - "y": 0.9814068916097549, - "heading": 1.2890700861945108, - "angularVelocity": 4.0442944250820885, - "velocityX": 1.1550083159196811, - "velocityY": 1.676264674683452, - "timestamp": 2.4888083239199905 - }, - { - "x": 8.537323038411628, - "y": 1.0330097209040148, - "heading": 1.4125130240648722, - "angularVelocity": 4.253437899363618, - "velocityX": 1.0152210170765539, - "velocityY": 1.7780638862070914, - "timestamp": 2.5178302447264116 - }, - { - "x": 8.56258232091714, - "y": 1.0874855013153344, - "heading": 1.5407726955584362, - "angularVelocity": 4.419406708090351, - "velocityX": 0.870351851415892, - "velocityY": 1.877056338713015, - "timestamp": 2.5468521655328327 - }, - { - "x": 8.583466101858683, - "y": 1.1446918380868547, - "heading": 1.6724319994758665, - "angularVelocity": 4.536546867301084, - "velocityX": 0.7195864491822832, - "velocityY": 1.9711423359291682, - "timestamp": 2.5758740863392537 - }, - { - "x": 8.599781102269066, - "y": 1.2044006523964297, - "heading": 1.8056614857101974, - "angularVelocity": 4.590650188972121, - "velocityX": 0.5621612890203437, - "velocityY": 2.057369486597329, - "timestamp": 2.604896007145675 - }, - { - "x": 8.61133776585303, - "y": 1.2662515535930832, - "heading": 1.9379574799883315, - "angularVelocity": 4.558485124418899, - "velocityX": 0.3982046419686604, - "velocityY": 2.131178759986453, - "timestamp": 2.633917927952096 - }, - { - "x": 8.61803446841016, - "y": 1.329807234083184, - "heading": 2.0667068830619315, - "angularVelocity": 4.43628124865926, - "velocityX": 0.23074635899526613, - "velocityY": 2.189919851067852, - "timestamp": 2.662939848758517 - }, - { - "x": 8.619867506604743, - "y": 1.394964606545626, - "heading": 2.191275336084359, - "angularVelocity": 4.292219452093143, - "velocityX": 0.06316047124545049, - "velocityY": 2.245108891897538, - "timestamp": 2.691961769564938 - }, - { - "x": 8.616839586460362, - "y": 1.4616264782251567, - "heading": 2.311063053299335, - "angularVelocity": 4.12749100977746, - "velocityX": -0.10433217582589409, - "velocityY": 2.2969489898401783, - "timestamp": 2.720983690371359 - }, - { - "x": 8.608949954788589, - "y": 1.5296853158108266, - "heading": 2.4255531996922417, - "angularVelocity": 3.9449541316223122, - "velocityX": -0.2718507752949118, - "velocityY": 2.345083843334451, - "timestamp": 2.75000561117778 - }, - { - "x": 8.596173618614296, - "y": 1.5989753133446671, - "heading": 2.534426417053211, - "angularVelocity": 3.7514132192408356, - "velocityX": -0.44023055053838084, - "velocityY": 2.3875055684980784, - "timestamp": 2.7790275319842013 - }, - { - "x": 8.57842938977896, - "y": 1.6691611767333963, - "heading": 2.6378090883600067, - "angularVelocity": 3.5622270488699845, - "velocityX": -0.6114078028705309, - "velocityY": 2.4183741612719314, - "timestamp": 2.8080494527906223 - }, - { - "x": 8.555564514046702, - "y": 1.7394263904616403, - "heading": 2.737024931725787, - "angularVelocity": 3.4186518538024746, - "velocityX": -0.7878484640892429, - "velocityY": 2.421108313158164, - "timestamp": 2.8370713735970434 - }, - { - "x": 8.527959255725655, - "y": 1.8075255209054102, - "heading": 2.8376495221341176, - "angularVelocity": 3.4671926465345364, - "velocityX": -0.9511864671252386, - "velocityY": 2.3464722027875915, - "timestamp": 2.8660932944034645 - }, - { - "x": 8.499577272044599, - "y": 1.8711589675729616, - "heading": 2.9502161532820788, - "angularVelocity": 3.878676118606718, - "velocityX": -0.9779498700435901, - "velocityY": 2.1925994179362642, - "timestamp": 2.8951152152098856 - }, - { - "x": 8.472091457883874, - "y": 1.93067768486564, - "heading": 3.076977426653027, - "angularVelocity": 4.3677768338097795, - "velocityX": -0.9470708139567483, - "velocityY": 2.0508193682173372, - "timestamp": 2.9241371360163066 - }, - { - "x": 8.445935000391486, - "y": 1.9864273544904458, - "heading": 3.219502150557654, - "angularVelocity": 4.910933526946052, - "velocityX": -0.9012655525750104, - "velocityY": 1.920950373914328, - "timestamp": 2.9531590568227277 - }, - { - "x": 8.421236309719301, - "y": 2.038741873704351, - "heading": 3.37984910584556, - "angularVelocity": 5.52502904123526, - "velocityX": -0.8510356994262128, - "velocityY": 1.8025863816129806, - "timestamp": 2.982180977629149 - }, - { - "x": 8.398072066125595, - "y": 2.08798012126862, - "heading": 3.5600291132631146, - "angularVelocity": 6.208410829158125, - "velocityX": -0.7981636966145051, - "velocityY": 1.6965881718406162, - "timestamp": 3.01120289843557 - }, - { - "x": 8.377585411071777, - "y": 2.1329314708709717, - "heading": 3.745897356817782, - "angularVelocity": 6.404408750007486, - "velocityX": -0.7059027963884773, - "velocityY": 1.5488757585061539, - "timestamp": 3.040224819241991 - }, - { - "x": 8.364981537655835, - "y": 2.1618201695949115, - "heading": 3.87540454879606, - "angularVelocity": 6.50944417189865, - "velocityX": -0.6335108428921614, - "velocityY": 1.4520380580394667, - "timestamp": 3.0601200957906958 - }, - { - "x": 8.354284801682704, - "y": 2.189254221668137, - "heading": 4.007147516190271, - "angularVelocity": 6.621821369092149, - "velocityX": -0.5376520375047442, - "velocityY": 1.3789228818240207, - "timestamp": 3.0800153723394006 - }, - { - "x": 8.345932812421994, - "y": 2.2160755919920243, - "heading": 4.140533212375188, - "angularVelocity": 6.704390153028566, - "velocityX": -0.41979759568875646, - "velocityY": 1.3481275446574927, - "timestamp": 3.0999106488881054 - }, - { - "x": 8.339986678909588, - "y": 2.2433741294975276, - "heading": 4.274319407548544, - "angularVelocity": 6.724520508465367, - "velocityX": -0.2988716189920965, - "velocityY": 1.3721114878033795, - "timestamp": 3.1198059254368102 - }, - { - "x": 8.33607844515623, - "y": 2.2720346987144775, - "heading": 4.407573079371785, - "angularVelocity": 6.697754187886208, - "velocityX": -0.1964402828877463, - "velocityY": 1.4405715420334793, - "timestamp": 3.139701201985515 - }, - { - "x": 8.333720429129459, - "y": 2.3025901920416527, - "heading": 4.540110627397886, - "angularVelocity": 6.661759523756392, - "velocityX": -0.11852139984079614, - "velocityY": 1.5358164664046496, - "timestamp": 3.15959647853422 - }, - { - "x": 8.332603722599888, - "y": 2.3352255445188432, - "heading": 4.671035459898787, - "angularVelocity": 6.580699302188081, - "velocityX": -0.05612922880648972, - "velocityY": 1.640356815211755, - "timestamp": 3.1794917550829247 - }, - { - "x": 8.332608537566271, - "y": 2.3698002053105385, - "heading": 4.797912823563958, - "angularVelocity": 6.377260620357163, - "velocityX": 0.0002420155543911525, - "velocityY": 1.7378326311300352, - "timestamp": 3.1993870316316295 - }, - { - "x": 8.33396980572602, - "y": 2.4053616174482886, - "heading": 4.91661874197118, - "angularVelocity": 5.966537741590232, - "velocityX": 0.06842167568854002, - "velocityY": 1.787429898282315, - "timestamp": 3.2192823081803343 - }, - { - "x": 8.336666983434197, - "y": 2.4421571036885257, - "heading": 5.027955549931603, - "angularVelocity": 5.596142767247511, - "velocityX": 0.13556874676122466, - "velocityY": 1.8494583953211146, - "timestamp": 3.239177584729039 - }, - { - "x": 8.340246531495488, - "y": 2.479675325798265, - "heading": 5.127878006964601, - "angularVelocity": 5.02242111530254, - "velocityX": 0.17991949257545845, - "velocityY": 1.8857854032786403, - "timestamp": 3.259072861277744 - }, - { - "x": 8.344506054620522, - "y": 2.517833586551145, - "heading": 5.215460170451448, - "angularVelocity": 4.402158636621092, - "velocityX": 0.2140972061688483, - "velocityY": 1.9179557851063929, - "timestamp": 3.278968137826449 - }, - { - "x": 8.34934139251709, - "y": 2.556588649749756, - "heading": 5.290290323982317, - "angularVelocity": 3.7612019791572497, - "velocityX": 0.24303949154619145, - "velocityY": 1.9479529778707136, - "timestamp": 3.2988634143751536 - }, - { - "x": 8.360489719138714, - "y": 2.633118600966112, - "heading": 5.386097875540369, - "angularVelocity": 2.5082503010878967, - "velocityX": 0.2918641918155103, - "velocityY": 2.0035609934605225, - "timestamp": 3.3370603804102004 - }, - { - "x": 8.3734961558476, - "y": 2.711837506326731, - "heading": 5.434143906525109, - "angularVelocity": 1.2578494046008082, - "velocityX": 0.3405096807153113, - "velocityY": 2.0608680094746767, - "timestamp": 3.375257346445247 - }, - { - "x": 8.388354991269559, - "y": 2.792730047490086, - "heading": 5.4343775006036195, - "angularVelocity": 0.006115513946711611, - "velocityX": 0.3890056453259781, - "velocityY": 2.117773989932431, - "timestamp": 3.413454312480294 - }, - { - "x": 8.405064352616025, - "y": 2.875700202856259, - "heading": 5.386603131855514, - "angularVelocity": -1.2507372628567064, - "velocityX": 0.43745258016393523, - "velocityY": 2.1721661162838264, - "timestamp": 3.4516512785153406 - }, - { - "x": 8.423595902184939, - "y": 2.960570562063504, - "heading": 5.290443702807007, - "angularVelocity": -2.51746248537848, - "velocityX": 0.4851576314179031, - "velocityY": 2.221913623437374, - "timestamp": 3.4898482445503873 - }, - { - "x": 8.443874909764538, - "y": 3.0470824289690306, - "heading": 5.145320206366309, - "angularVelocity": -3.799346165544723, - "velocityX": 0.5309062390188924, - "velocityY": 2.264888442347729, - "timestamp": 3.528045210585434 - }, - { - "x": 8.46822613284771, - "y": 3.1277535966232044, - "heading": 4.9755072400440925, - "angularVelocity": -4.445718703585181, - "velocityX": 0.6375172064929964, - "velocityY": 2.1119784115878613, - "timestamp": 3.566242176620481 - }, - { - "x": 8.485889521575103, - "y": 3.2052285744044853, - "heading": 4.782579464036899, - "angularVelocity": -5.050866496312229, - "velocityX": 0.4624291026461906, - "velocityY": 2.02830187377173, - "timestamp": 3.6044391426555276 - }, - { - "x": 8.496466363696742, - "y": 3.2817681103248395, - "heading": 4.564012968163081, - "angularVelocity": -5.722090484183368, - "velocityX": 0.2769026762998172, - "velocityY": 2.0038119218716672, - "timestamp": 3.6426361086905743 - }, - { - "x": 8.498864361133254, - "y": 3.3553108945925123, - "heading": 4.355441183612215, - "angularVelocity": -5.460428044455047, - "velocityX": 0.06277978817249102, - "velocityY": 1.9253566945656204, - "timestamp": 3.680833074725621 - }, - { - "x": 8.49244817179446, - "y": 3.427344053675241, - "heading": 4.152515229757634, - "angularVelocity": -5.312619689961475, - "velocityX": -0.16797641291473847, - "velocityY": 1.8858345716944014, - "timestamp": 3.719030040760668 - }, - { - "x": 8.480949544401803, - "y": 3.4955676666301225, - "heading": 3.980239273291126, - "angularVelocity": -4.5102000066822105, - "velocityX": -0.3010350974500928, - "velocityY": 1.7861003120584202, - "timestamp": 3.7572270067957145 - }, - { - "x": 8.465880541095904, - "y": 3.5608797001839374, - "heading": 3.848658427278945, - "angularVelocity": -3.4447983615099473, - "velocityX": -0.39450785939576005, - "velocityY": 1.7098749019461128, - "timestamp": 3.7954239728307613 - }, - { - "x": 8.447723716276025, - "y": 3.6237520788842326, - "heading": 3.7606807673271616, - "angularVelocity": -2.3032630358929707, - "velocityX": -0.4753473038465876, - "velocityY": 1.6460045188564885, - "timestamp": 3.833620938865808 - }, - { - "x": 8.426810965043074, - "y": 3.6844175741296494, - "heading": 3.717839244299634, - "angularVelocity": -1.1215949190367396, - "velocityX": -0.5474977047591038, - "velocityY": 1.5882281118807744, - "timestamp": 3.8718179049008548 - }, - { - "x": 8.403377538803907, - "y": 3.7429507831754503, - "heading": 3.7209535641751312, - "angularVelocity": 0.08153317393427642, - "velocityX": -0.6134892027200903, - "velocityY": 1.5324046677449423, - "timestamp": 3.9100148709359015 - }, - { - "x": 8.377585411071777, - "y": 3.7993156909942623, - "heading": 3.770388538466339, - "angularVelocity": 1.2942120650590392, - "velocityX": -0.6752402195625785, - "velocityY": 1.475638346959163, - "timestamp": 3.9482118369709482 - }, - { - "x": 8.359121603366816, - "y": 3.8364193104219853, - "heading": 3.8251043907760733, - "angularVelocity": 2.1181774191421603, - "velocityX": -0.7147767767674121, - "velocityY": 1.4363670768638492, - "timestamp": 3.974043408875767 - }, - { - "x": 8.339698595709613, - "y": 3.872452168004382, - "heading": 3.901119172271844, - "angularVelocity": 2.9427083173978534, - "velocityX": -0.7519096293779934, - "velocityY": 1.3949154048838457, - "timestamp": 3.999874980780586 - }, - { - "x": 8.31940846552165, - "y": 3.907347005013746, - "heading": 3.9984628782143856, - "angularVelocity": 3.7684004016953554, - "velocityX": -0.7854779516602693, - "velocityY": 1.3508599917163486, - "timestamp": 4.025706552685405 - }, - { - "x": 8.29838582091067, - "y": 3.941024765667179, - "heading": 4.1171731868845285, - "angularVelocity": 4.595551099544112, - "velocityX": -0.8138352822058595, - "velocityY": 1.3037441460211887, - "timestamp": 4.051538124590223 - }, - { - "x": 8.276832604055052, - "y": 3.9733768568490917, - "heading": 4.2572182344953795, - "angularVelocity": 5.421468276374085, - "velocityX": -0.8343749631278609, - "velocityY": 1.2524244092121188, - "timestamp": 4.077369696495042 - }, - { - "x": 8.255105530373472, - "y": 4.004178929302719, - "heading": 4.418037307026107, - "angularVelocity": 6.225678914287337, - "velocityX": -0.841105363685868, - "velocityY": 1.1924195928580044, - "timestamp": 4.103201268399861 - }, - { - "x": 8.236226871427698, - "y": 4.03234435965831, - "heading": 4.584751742520452, - "angularVelocity": 6.45390207412214, - "velocityX": -0.7308366295065699, - "velocityY": 1.0903490681625025, - "timestamp": 4.12903284030468 - }, - { - "x": 8.22111995531124, - "y": 4.060012965907288, - "heading": 4.744248346923677, - "angularVelocity": 6.174483108922656, - "velocityX": -0.5848237254830212, - "velocityY": 1.0711158558576532, - "timestamp": 4.154864412209498 - }, - { - "x": 8.208790974794852, - "y": 4.088137716132201, - "heading": 4.888562116667079, - "angularVelocity": 5.5867204007233076, - "velocityX": -0.4772834019477146, - "velocityY": 1.0887742460483674, - "timestamp": 4.180695984114317 - }, - { - "x": 8.198725383178298, - "y": 4.116963981676649, - "heading": 5.015233890568161, - "angularVelocity": 4.903757865290863, - "velocityX": -0.3896623733794405, - "velocityY": 1.115931529473434, - "timestamp": 4.206527556019136 - }, - { - "x": 8.190656129284886, - "y": 4.146632428651468, - "heading": 5.123285713831358, - "angularVelocity": 4.182936433807978, - "velocityX": -0.31237951461661634, - "velocityY": 1.148534323971415, - "timestamp": 4.232359127923955 - }, - { - "x": 8.184413831367804, - "y": 4.177239551429493, - "heading": 5.212211909192924, - "angularVelocity": 3.4425390637949445, - "velocityX": -0.241653815729148, - "velocityY": 1.184872639218488, - "timestamp": 4.2581906998287735 - }, - { - "x": 8.179879188537598, - "y": 4.208850860595703, - "heading": 5.281700753397833, - "angularVelocity": 2.6900741643192534, - "velocityX": -0.17554653069182086, - "velocityY": 1.223747020997694, - "timestamp": 4.284022271733592 - }, - { - "x": 8.176711633108404, - "y": 4.259079542498045, - "heading": 5.341834535732932, - "angularVelocity": 1.5389696629172125, - "velocityX": -0.08106544311437222, - "velocityY": 1.2854740655637256, - "timestamp": 4.323096324838129 - }, - { - "x": 8.177255357527843, - "y": 4.31178579369655, - "heading": 5.357219600158119, - "angularVelocity": 0.3937411965947536, - "velocityX": 0.013915229576633022, - "velocityY": 1.3488810863182197, - "timestamp": 4.362170377942666 - }, - { - "x": 8.181626457446923, - "y": 4.366954653737556, - "heading": 5.328216932710745, - "angularVelocity": -0.7422487595484647, - "velocityX": 0.11186707218176441, - "velocityY": 1.411905232697765, - "timestamp": 4.401244431047203 - }, - { - "x": 8.190025046391563, - "y": 4.424472904744573, - "heading": 5.255283063236909, - "angularVelocity": -1.8665550071990427, - "velocityX": 0.21494030635039485, - "velocityY": 1.4720318584083123, - "timestamp": 4.4403184841517405 - }, - { - "x": 8.202703493077177, - "y": 4.484103126840556, - "heading": 5.138917873952012, - "angularVelocity": -2.978068053845803, - "velocityX": 0.3244722694032999, - "velocityY": 1.526082332345974, - "timestamp": 4.4793925372562775 - }, - { - "x": 8.219928011035833, - "y": 4.545471721328882, - "heading": 4.979694935972509, - "angularVelocity": -4.074902021388053, - "velocityX": 0.44081728385262037, - "velocityY": 1.570571507494814, - "timestamp": 4.5184665903608145 - }, - { - "x": 8.242002423270923, - "y": 4.608108641713836, - "heading": 4.778755843149729, - "angularVelocity": -5.142519827292898, - "velocityX": 0.5649378674905671, - "velocityY": 1.6030310502311427, - "timestamp": 4.557540643465352 - }, - { - "x": 8.271074244498946, - "y": 4.672945765445566, - "heading": 4.551673202567828, - "angularVelocity": -5.811596764082065, - "velocityX": 0.7440185728940091, - "velocityY": 1.6593396021207916, - "timestamp": 4.596614696569889 - }, - { - "x": 8.307044890775797, - "y": 4.743652286364594, - "heading": 4.33581324080306, - "angularVelocity": -5.524381133108038, - "velocityX": 0.9205762755303734, - "velocityY": 1.8095517434514046, - "timestamp": 4.635688749674426 - }, - { - "x": 8.343074596799024, - "y": 4.821913482728032, - "heading": 4.143267038130374, - "angularVelocity": -4.927725366947583, - "velocityX": 0.9220877580023329, - "velocityY": 2.0028942519492667, - "timestamp": 4.674762802778963 - }, - { - "x": 8.37162830890395, - "y": 4.904380364973872, - "heading": 3.9684903103687414, - "angularVelocity": -4.472961310003866, - "velocityX": 0.7307589010163777, - "velocityY": 2.1105279768446303, - "timestamp": 4.7138368558835 - }, - { - "x": 8.39410907934667, - "y": 4.988084023708393, - "heading": 3.8243195102940244, - "angularVelocity": -3.6896812237268355, - "velocityX": 0.5753375617977339, - "velocityY": 2.142180093541445, - "timestamp": 4.752910908988037 - }, - { - "x": 8.412533295857541, - "y": 5.0701333500179695, - "heading": 3.7241871610957364, - "angularVelocity": -2.5626302173055078, - "velocityX": 0.47152048602634916, - "velocityY": 2.099841705442333, - "timestamp": 4.791984962092574 - }, - { - "x": 8.427022508757116, - "y": 5.150508503599114, - "heading": 3.668854256125942, - "angularVelocity": -1.4161035411852148, - "velocityX": 0.370814178421956, - "velocityY": 2.0569955557492743, - "timestamp": 4.831059015197111 - }, - { - "x": 8.437687476391535, - "y": 5.229173684079572, - "heading": 3.658771745830396, - "angularVelocity": -0.25803594698946425, - "velocityX": 0.27294244612622637, - "velocityY": 2.013233187506785, - "timestamp": 4.870133068301648 - }, - { - "x": 8.444672138848645, - "y": 5.306037054642886, - "heading": 3.694319619572902, - "angularVelocity": 0.9097564987026582, - "velocityX": 0.17875449056755022, - "velocityY": 1.9671204918946052, - "timestamp": 4.909207121406185 - }, - { - "x": 8.44819450378418, - "y": 5.38096809387207, - "heading": 3.7759750997570256, - "angularVelocity": 2.0897622257323873, - "velocityX": 0.09014588085116931, - "velocityY": 1.9176674359508388, - "timestamp": 4.948281174510722 - }, - { - "x": 8.449106377133136, - "y": 5.428397890121427, - "heading": 3.847877974347001, - "angularVelocity": 2.856844558406781, - "velocityX": 0.03623054613292597, - "velocityY": 1.8844803645751356, - "timestamp": 4.973449808337294 - }, - { - "x": 8.448712080913069, - "y": 5.474928555589069, - "heading": 3.9390652949611624, - "angularVelocity": 3.623054045861213, - "velocityX": -0.015666174921664548, - "velocityY": 1.848756106043276, - "timestamp": 4.998618442163867 - }, - { - "x": 8.447075648100045, - "y": 5.520495348927803, - "heading": 4.049507624303278, - "angularVelocity": 4.388093930847932, - "velocityX": -0.06501873817623247, - "velocityY": 1.8104595447141671, - "timestamp": 5.023787075990439 - }, - { - "x": 8.444260693041544, - "y": 5.565024011584617, - "heading": 4.179082223329722, - "angularVelocity": 5.148257148929752, - "velocityX": -0.11184377657913579, - "velocityY": 1.7692125430265968, - "timestamp": 5.048955709817012 - }, - { - "x": 8.440260130020729, - "y": 5.60821579730256, - "heading": 4.326669444296132, - "angularVelocity": 5.86393452991448, - "velocityX": -0.15895034463857272, - "velocityY": 1.7160957569474122, - "timestamp": 5.074124343643584 - }, - { - "x": 8.433318308538809, - "y": 5.649473049584276, - "heading": 4.481200094717335, - "angularVelocity": 6.1398108250935115, - "velocityX": -0.275812407211048, - "velocityY": 1.639232886695548, - "timestamp": 5.0992929774701565 - }, - { - "x": 8.423269771904357, - "y": 5.688774009331345, - "heading": 4.641460146861477, - "angularVelocity": 6.367451378109614, - "velocityX": -0.3992483940007152, - "velocityY": 1.5615054840830123, - "timestamp": 5.124461611296729 - }, - { - "x": 8.409301302318001, - "y": 5.727844142412751, - "heading": 4.801252594095005, - "angularVelocity": 6.3488725027588, - "velocityX": -0.5549951452513271, - "velocityY": 1.552334280462891, - "timestamp": 5.149630245123301 - }, - { - "x": 8.396208121715038, - "y": 5.767480930820443, - "heading": 4.939805836150058, - "angularVelocity": 5.504996536950433, - "velocityX": -0.5202181688995966, - "velocityY": 1.5748486263026762, - "timestamp": 5.174798878949874 - }, - { - "x": 8.384341280819799, - "y": 5.8076306100276724, - "heading": 5.057844576112943, - "angularVelocity": 4.689914469583332, - "velocityX": -0.4714932473891813, - "velocityY": 1.5952268003057297, - "timestamp": 5.199967512776446 - }, - { - "x": 8.373739781221703, - "y": 5.848339028395987, - "heading": 5.1555398013279, - "angularVelocity": 3.881626070296041, - "velocityX": -0.42121871497465463, - "velocityY": 1.6174266211198407, - "timestamp": 5.225136146603019 - }, - { - "x": 8.364388866933238, - "y": 5.889641090063779, - "heading": 5.232909954944061, - "angularVelocity": 3.07407045409338, - "velocityX": -0.37153046736263995, - "velocityY": 1.6410132529397037, - "timestamp": 5.250304780429591 - }, - { - "x": 8.356262991610826, - "y": 5.931555344979487, - "heading": 5.289924578574465, - "angularVelocity": 2.2653046654526636, - "velocityX": -0.32285722691204854, - "velocityY": 1.665336911193694, - "timestamp": 5.2754734142561635 - }, - { - "x": 8.34934139251709, - "y": 5.974088191986084, - "heading": 5.326548029576795, - "angularVelocity": 1.4551227235728712, - "velocityX": -0.27500893141166805, - "velocityY": 1.689914808236125, - "timestamp": 5.300642048082736 - }, - { - "x": 8.343221165925502, - "y": 6.021143146137171, - "heading": 5.342226898752842, - "angularVelocity": 0.5719929961678595, - "velocityX": -0.22327673673661108, - "velocityY": 1.7166483058956983, - "timestamp": 5.328052994162652 - }, - { - "x": 8.338553091700128, - "y": 6.068939036747707, - "heading": 5.3338112220665685, - "angularVelocity": -0.3070188333426256, - "velocityX": -0.17029963912102522, - "velocityY": 1.7436789839791385, - "timestamp": 5.3554639402425686 - }, - { - "x": 8.335401457891965, - "y": 6.11747737229338, - "heading": 5.301487591207973, - "angularVelocity": -1.1792234665799601, - "velocityX": -0.1149771992171805, - "velocityY": 1.7707646939350166, - "timestamp": 5.382874886322485 - }, - { - "x": 8.333863965332327, - "y": 6.166745972097111, - "heading": 5.245527796242896, - "angularVelocity": -2.0415127154650743, - "velocityX": -0.05609045945204774, - "velocityY": 1.7974060311558808, - "timestamp": 5.410285832402401 - }, - { - "x": 8.334074611539513, - "y": 6.216707641134325, - "heading": 5.16630466616314, - "angularVelocity": -2.890200500514666, - "velocityX": 0.007684747785513327, - "velocityY": 1.8226904278148732, - "timestamp": 5.437696778482318 - }, - { - "x": 8.336209337170322, - "y": 6.2672871852177625, - "heading": 5.064334139872564, - "angularVelocity": -3.7200659179468194, - "velocityX": 0.07787858268680638, - "velocityY": 1.845231606963613, - "timestamp": 5.465107724562234 - }, - { - "x": 8.34050875980612, - "y": 6.318358761986823, - "heading": 4.9404202596625035, - "angularVelocity": -4.520598444460426, - "velocityX": 0.1568505743385533, - "velocityY": 1.8631818332778705, - "timestamp": 5.49251867064215 - }, - { - "x": 8.347373076329852, - "y": 6.369731891617242, - "heading": 4.796226953445604, - "angularVelocity": -5.260427925271259, - "velocityX": 0.2504224591051665, - "velocityY": 1.874183017274954, - "timestamp": 5.519929616722067 - }, - { - "x": 8.357734742408361, - "y": 6.421155881623346, - "heading": 4.637107940656009, - "angularVelocity": -5.804944211910156, - "velocityX": 0.37801198281516796, - "velocityY": 1.876038494117501, - "timestamp": 5.547340562801983 - }, - { - "x": 8.372714822209472, - "y": 6.472180587497517, - "heading": 4.476004426648119, - "angularVelocity": -5.877342341201648, - "velocityX": 0.546499918588575, - "velocityY": 1.8614718997808002, - "timestamp": 5.574751508881899 - }, - { - "x": 8.392163518645132, - "y": 6.523832787745315, - "heading": 4.319730445846118, - "angularVelocity": -5.701152391689906, - "velocityX": 0.7095229905220347, - "velocityY": 1.8843640090788911, - "timestamp": 5.602162454961816 - }, - { - "x": 8.409770328909872, - "y": 6.578150492835037, - "heading": 4.178613291017239, - "angularVelocity": -5.148204458811868, - "velocityX": 0.642327711469996, - "velocityY": 1.9816063601511025, - "timestamp": 5.629573401041732 - }, - { - "x": 8.424485615502517, - "y": 6.635136223401266, - "heading": 4.049096201405813, - "angularVelocity": -4.725013475777802, - "velocityX": 0.5368397920211534, - "velocityY": 2.0789406684499903, - "timestamp": 5.656984347121648 - }, - { - "x": 8.435886735748374, - "y": 6.694743962812389, - "heading": 3.9283767340305045, - "angularVelocity": -4.404060590369644, - "velocityX": 0.41593311710644926, - "velocityY": 2.1745962082934467, - "timestamp": 5.684395293201565 - }, - { - "x": 8.4437690723566, - "y": 6.756967833759458, - "heading": 3.814495508904072, - "angularVelocity": -4.154589367124023, - "velocityX": 0.28756163998295975, - "velocityY": 2.2700373334672657, - "timestamp": 5.711806239281481 - }, - { - "x": 8.44819450378418, - "y": 6.821402072906494, - "heading": 3.7101172753280305, - "angularVelocity": -3.8079033562623543, - "velocityX": 0.16144759887806323, - "velocityY": 2.350675491432431, - "timestamp": 5.739217185361397 - }, - { - "x": 8.442296007576482, - "y": 7.005937519917937, - "heading": 3.5641292983287713, - "angularVelocity": -1.9075537985667739, - "velocityX": -0.07707277734850891, - "velocityY": 2.41123481640327, - "timestamp": 5.815748697662097 - }, - { - "x": 8.41659744566655, - "y": 7.180425726585724, - "heading": 3.5626950047371615, - "angularVelocity": -0.01874121585334014, - "velocityX": -0.3357905931475581, - "velocityY": 2.279952419889553, - "timestamp": 5.892280209962797 - }, - { - "x": 8.38385523357143, - "y": 7.320664328992628, - "heading": 3.601572492187995, - "angularVelocity": 0.5079931949871856, - "velocityX": -0.4278265398241839, - "velocityY": 1.8324295207429526, - "timestamp": 5.968811722263497 - }, - { - "x": 8.354355980334272, - "y": 7.424774646283286, - "heading": 3.6482652707450245, - "angularVelocity": 0.6101117977855888, - "velocityX": -0.38545237576453223, - "velocityY": 1.3603588137863938, - "timestamp": 6.045343234564196 - }, - { - "x": 8.33265202622526, - "y": 7.4934610231290915, - "heading": 3.6869536728033228, - "angularVelocity": 0.5055225082484697, - "velocityX": -0.2835949984071486, - "velocityY": 0.8974914356315113, - "timestamp": 6.121874746864896 - }, - { - "x": 8.321097373962402, - "y": 7.527496814727783, - "heading": 3.709032536356956, - "angularVelocity": 0.2884937575371967, - "velocityX": -0.15097901394471921, - "velocityY": 0.44472911321759667, - "timestamp": 6.198406259165596 - }, - { - "x": 8.321097373962402, - "y": 7.527496814727783, - "heading": 3.709032536356956, - "angularVelocity": -5.082729725441752e-29, - "velocityX": 4.680465584518782e-28, - "velocityY": 3.556470741173404e-29, - "timestamp": 6.274937771466296 - } - ], - "eventMarkers": [] -} \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 0e355ae7..89ad8907 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -262,10 +262,7 @@ private void configureAutoRoutines() { } autos.addDefault(new AutoCommand("Dwayne :skull:")); autos.add(new AmpLanePADEF(drivetrain, shooter, indexer)); - autos.add(new CenterLanePDEABC(drivetrain, shooter, indexer)); - autos.add(new SourceLanePGHF(drivetrain, shooter, indexer)); - autos.add(new AmpLanepdChaos(drivetrain, shooter, indexer)); - autos.add(new SoureLanephChaos(drivetrain, shooter, indexer)); + autos.add(new AmpLanePAEDF(drivetrain, shooter, indexer)); // autos.add(new AutoCommand("SubwooferShot", new ShootSequence(shooter, indexer))); // autos.add(new DriveSinglePath("Taxi", drivetrain)); // autos.add(new DriveSinglePath("Sprint", drivetrain)); @@ -274,12 +271,17 @@ private void configureAutoRoutines() { // autos.add(new CenterLanePSubSprint(drivetrain, shooter, indexer)); // autos.add(new CenterLanePCBADSprint(drivetrain, shooter, indexer)); autos.add(new CenterLanePCBAFSprint(drivetrain, shooter, indexer)); -//// autos.add(new CenterLanePCBAFE(drivetrain, shooter, indexer)); + autos.add(new CenterLanePCBAFE(drivetrain, shooter, indexer)); + autos.add(new CenterLanePDEABC(drivetrain, shooter, indexer)); + autos.add(new CenterLanePCBAFG(drivetrain, shooter, indexer)); + autos.add(new CenterLanePCBAEF(drivetrain, shooter, indexer)); + autos.add(new CenterLanePCBAGF(drivetrain, shooter, indexer)); // autos.add(new CenterLanePCBA(drivetrain, shooter, indexer)); // autos.add(new CenterLanePBDA(drivetrain, shooter, indexer)); // autos.add(new CenterLanePSubCSubBSubASubFSub(drivetrain, shooter, indexer)); //// autos.add(new CenterLanePSubCSubBSubFSub(drivetrain, shooter, indexer)); // autos.add(new CenterLanePSubCSubBSubASub(drivetrain, shooter, indexer)); + autos.add(new SourceLanePHGF(drivetrain, shooter, indexer)); autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer)); // autos.addDefault(new ATestAuto(drivetrain, shooter, indexer)); } diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePAEDF.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePAEDF.java new file mode 100644 index 00000000..dd20fd74 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePAEDF.java @@ -0,0 +1,68 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.commands.shooter.LeadingShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class AmpLanePAEDF extends AutoCommand { + + public AmpLanePAEDF(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("AmpLanePAEDF"); + + addPath( + PathHelper.fromChoreoPath("AmpLanePAEDF.1", true, true), + PathHelper.fromChoreoPath("AmpLanePAEDF.2"), + PathHelper.fromChoreoPath("AmpLanePAEDF.3"), + PathHelper.fromChoreoPath("AmpLanePAEDF.4"), + PathHelper.fromChoreoPath("AmpLanePAEDF.5") + ); + + addCommands( +// Commands.runOnce(drivetrain::unblockTags), + Commands.parallel( + new AutoShootPrepare(drivetrain, shooter), +// Commands.sequence( +// Commands.deadline( +// getPath(0).getCommand(drivetrain), +// Commands.sequence( +// Commands.waitSeconds(0.25), +// new InstantCommand(shooter::zeroPivotToCancoder), +// Commands.waitSeconds(1), +// IntakeAndFeed.withDefaults(indexer).withTimeout(2), +// new IntakeCommand(indexer, () -> false, 1) +// ) +// ), +// Commands.sequence( +// Commands.waitSeconds(0.25), +// new InstantCommand(shooter::zeroPivotToCancoder) +// ), +// drivetrain.commandCopyVisionPose(), +// Commands.parallel( +// new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4), +// Commands.sequence( +// new ParallelDeadlineGroup( +// Commands.sequence( +// Commands.waitUntil(()->!indexer.isNoteStaged()), +// Commands.waitSeconds(0.2) +// ).withTimeout(1), +// IntakeAndFeed.withDefaults(indexer) +// ) +// ) +// ), + Commands.sequence( + createCancoderSegmentSequence(drivetrain, shooter, indexer, 0), + createSegmentSequence(drivetrain, shooter, indexer, 1), + createSegmentSequence(drivetrain, shooter, indexer, 2), + createSegmentSequence(drivetrain, shooter, indexer, 3), + createSegmentSequence(drivetrain, shooter, indexer, 4) + ) +// ) + ) + ); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAEF.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAEF.java new file mode 100644 index 00000000..acd63041 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAEF.java @@ -0,0 +1,48 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.indexer.IntakeAndFeed; +import org.team1540.robot2024.commands.shooter.LeadingShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class CenterLanePCBAEF extends AutoCommand { + + public CenterLanePCBAEF(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("CenterLanePCBAEF"); + addPath( + PathHelper.fromChoreoPath("CenterLanePCBAEF.1", false, true), + PathHelper.fromChoreoPath("CenterLanePCBAEF.2"), + PathHelper.fromChoreoPath("CenterLanePCBAEF.3"), + PathHelper.fromChoreoPath("CenterLanePCBAEF.4"), + PathHelper.fromChoreoPath("CenterLanePCBAEF.5") + + ); + + addCommands( + Commands.parallel( + new LeadingShootPrepare(drivetrain, shooter), + Commands.sequence( + Commands.deadline( + Commands.sequence( + Commands.waitSeconds(0.1), + Commands.waitUntil(()->!indexer.isNoteStaged()), + Commands.waitSeconds(0.1) + ).withTimeout(1.1), + Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer)) + ), + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 4, false, true, true, 0, 0.1) + ) + ) + ); + + addRequirements(drivetrain, shooter, indexer); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFE.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFE.java index 1e3b3b54..544ceccc 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFE.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFE.java @@ -1,7 +1,9 @@ package org.team1540.robot2024.commands.autos; import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.indexer.IntakeAndFeed; import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.commands.shooter.LeadingShootPrepare; import org.team1540.robot2024.commands.shooter.ShootSequence; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; @@ -12,7 +14,7 @@ public class CenterLanePCBAFE extends AutoCommand { public CenterLanePCBAFE(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { - super("!CenterLanePCBAFE"); + super("CenterLanePCBAFE"); addPath( PathHelper.fromChoreoPath("CenterLanePCBAFE.1", false, true), PathHelper.fromChoreoPath("CenterLanePCBAFE.2"), @@ -23,16 +25,22 @@ public CenterLanePCBAFE(Drivetrain drivetrain, Shooter shooter, Indexer indexer) ); addCommands( - ShootSequence.forAutoSubwoofer(shooter, indexer), Commands.parallel( - new AutoShootPrepare(drivetrain, shooter), + new LeadingShootPrepare(drivetrain, shooter), Commands.sequence( - drivetrain.commandCopyVisionPose(), - createSegmentSequence(drivetrain, shooter, indexer, 0, false, false, false), - createSegmentSequence(drivetrain, shooter, indexer, 1, false, false, true), - createSegmentSequence(drivetrain, shooter, indexer, 2, false, false, true), - createSegmentSequence(drivetrain, shooter, indexer, 3, false, false, true), - createSegmentSequence(drivetrain, shooter, indexer, 4, false, false, true) + Commands.deadline( + Commands.sequence( + Commands.waitSeconds(0.1), + Commands.waitUntil(()->!indexer.isNoteStaged()), + Commands.waitSeconds(0.1) + ).withTimeout(1.1), + Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer)) + ), + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 4, false, true, true, 0, 0.1) ) ) ); diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFG.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFG.java new file mode 100644 index 00000000..b7d692aa --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFG.java @@ -0,0 +1,48 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.indexer.IntakeAndFeed; +import org.team1540.robot2024.commands.shooter.LeadingShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class CenterLanePCBAFG extends AutoCommand { + + public CenterLanePCBAFG(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("CenterLanePCBAFG"); + addPath( + PathHelper.fromChoreoPath("CenterLanePCBAFG.1", false, true), + PathHelper.fromChoreoPath("CenterLanePCBAFG.2"), + PathHelper.fromChoreoPath("CenterLanePCBAFG.3"), + PathHelper.fromChoreoPath("CenterLanePCBAFG.4"), + PathHelper.fromChoreoPath("CenterLanePCBAFG.5") + + ); + + addCommands( + Commands.parallel( + new LeadingShootPrepare(drivetrain, shooter), + Commands.sequence( + Commands.deadline( + Commands.sequence( + Commands.waitSeconds(0.1), + Commands.waitUntil(()->!indexer.isNoteStaged()), + Commands.waitSeconds(0.1) + ).withTimeout(1.1), + Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer)) + ), + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 4, false, true, true, 0, 0.1) + ) + ) + ); + + addRequirements(drivetrain, shooter, indexer); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAGF.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAGF.java new file mode 100644 index 00000000..f2800b6f --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAGF.java @@ -0,0 +1,48 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.indexer.IntakeAndFeed; +import org.team1540.robot2024.commands.shooter.LeadingShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class CenterLanePCBAGF extends AutoCommand { + + public CenterLanePCBAGF(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { + super("CenterLanePCBAGF"); + addPath( + PathHelper.fromChoreoPath("CenterLanePCBAGF.1", false, true), + PathHelper.fromChoreoPath("CenterLanePCBAGF.2"), + PathHelper.fromChoreoPath("CenterLanePCBAGF.3"), + PathHelper.fromChoreoPath("CenterLanePCBAGF.4"), + PathHelper.fromChoreoPath("CenterLanePCBAGF.5") + + ); + + addCommands( + Commands.parallel( + new LeadingShootPrepare(drivetrain, shooter), + Commands.sequence( + Commands.deadline( + Commands.sequence( + Commands.waitSeconds(0.1), + Commands.waitUntil(()->!indexer.isNoteStaged()), + Commands.waitSeconds(0.1) + ).withTimeout(1.1), + Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer)) + ), + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0, 0.1), + createSegmentSequence(drivetrain, shooter, indexer, 4, false, true, true, 0, 0.1) + ) + ) + ); + + addRequirements(drivetrain, shooter, indexer); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHF.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGF.java similarity index 92% rename from src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHF.java rename to src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGF.java index 25cce318..b53fd7cd 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGHF.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGF.java @@ -8,9 +8,9 @@ import org.team1540.robot2024.util.auto.AutoCommand; import org.team1540.robot2024.util.auto.PathHelper; -public class SourceLanePGHF extends AutoCommand { +public class SourceLanePHGF extends AutoCommand { private static final double SHOT_WAIT = 0.2; - public SourceLanePGHF(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ + public SourceLanePHGF(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ super("!SourceLanePHGF"); addPath( PathHelper.fromChoreoPath("SourceLanePHGF.1", true, true), From 9b6fb25f2ca1cb8a1119e1cd8e199c970067e988 Mon Sep 17 00:00:00 2001 From: Zach R Date: Thu, 4 Apr 2024 21:31:54 -0700 Subject: [PATCH 73/75] event: eod day 1 --- .../team1540/robot2024/RobotContainer.java | 5 ++-- .../indexer/ContinuousIntakeCommand.java | 26 ++++++++++++++++--- .../robot2024/subsystems/indexer/Indexer.java | 8 ++++++ .../robot2024/subsystems/led/Leds.java | 10 ++++++- 4 files changed, 42 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 89ad8907..e471cc97 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -175,7 +175,7 @@ private void configureButtonBindings() { copilot.back().onTrue(Commands.runOnce(shooter::zeroPivotToCancoder).andThen(Commands.print("BACK IS PRESSED"))); copilot.leftBumper().whileTrue(new AmpScoreSequence(tramp, indexer, elevator)); - Command intakeCommand = new ContinuousIntakeCommand(indexer, 1) + Command intakeCommand = new ContinuousIntakeCommand(indexer, leds, 1) .deadlineWith(CommandUtils.rumbleCommand(driver, 0.5), CommandUtils.rumbleCommand(copilot, 0.5)); copilot.rightBumper().whileTrue(intakeCommand); @@ -206,7 +206,7 @@ private void configureButtonBindings() { new Trigger(() -> elevator.getPosition() > 0.1).debounce(0.1) .whileTrue(PrepareShooterCommand.lowerPivot(shooter)); - new Trigger(indexer::isNoteStaged).debounce(0.1) + new Trigger(indexer::isNoteStaged).debounce(0.05) .onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 1, 1)) .whileTrue(leds.commandShowIntakePattern(SimpleLedPattern.solid("#ff0000"))); @@ -284,6 +284,7 @@ private void configureAutoRoutines() { autos.add(new SourceLanePHGF(drivetrain, shooter, indexer)); autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer)); // autos.addDefault(new ATestAuto(drivetrain, shooter, indexer)); + autos.add(new AutoCommand("Subwoofer Shot", ShootSequence.forAutoSubwoofer(shooter, indexer))); } diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java index a9369a94..adf1ad2b 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java @@ -1,27 +1,44 @@ package org.team1540.robot2024.commands.indexer; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import org.team1540.robot2024.subsystems.indexer.Indexer; - -import java.util.function.BooleanSupplier; +import org.team1540.robot2024.subsystems.led.Leds; +import org.team1540.robot2024.subsystems.led.patterns.LedPattern; +import org.team1540.robot2024.subsystems.led.patterns.SimpleLedPattern; public class ContinuousIntakeCommand extends Command { private final Indexer indexer; + private final Leds leds; private final double percent; + private final LedPattern detected = SimpleLedPattern.solid("#ffff00"); + private final Timer timer = new Timer(); - - public ContinuousIntakeCommand(Indexer indexer, double percent) { + public ContinuousIntakeCommand(Indexer indexer, Leds leds, double percent) { this.indexer = indexer; + this.leds = leds; this.percent = percent; addRequirements(indexer); } + @Override + public void initialize() { + timer.restart(); + } + @Override public void execute() { if (indexer.isNoteStaged()) { indexer.stopIntake(); + leds.clearPatternAll(Leds.PatternLevel.INTAKE_PREREADY); } else { + if (indexer.getIntakeVoltage() == 0) { + timer.restart(); + } + if (timer.hasElapsed(0.3) && indexer.getIntakeCurrent() > 30) { + leds.setPatternAll(detected, Leds.PatternLevel.INTAKE_PREREADY); + } indexer.setIntakePercent(percent); } } @@ -29,5 +46,6 @@ public void execute() { @Override public void end(boolean interrupted) { indexer.stopIntake(); + leds.clearPatternAll(Leds.PatternLevel.INTAKE_PREREADY); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index 0acdb199..db69b3ac 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -137,4 +137,12 @@ public void setIntakeBrakeMode(boolean isBrake) { io.setIntakeBrakeMode(isBrake); } + public double getIntakeVoltage() { + return inputs.intakeVoltage; + } + + public double getIntakeCurrent() { + return inputs.intakeCurrentAmps; + } + } diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index 463cf3da..4eab4262 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -9,7 +9,6 @@ import org.team1540.robot2024.Robot; import org.team1540.robot2024.subsystems.led.patterns.LedPattern; -import java.util.function.Supplier; import static org.team1540.robot2024.Constants.Leds.*; @@ -118,4 +117,13 @@ public Command commandClear(PatternLevel priority) { return Commands.runOnce(() -> this.clearPattern(Leds.Zone.MAIN, priority)).ignoringDisable(true); } + public void setPatternAll(LedPattern pattern, PatternLevel level) { + this.setPattern(Zone.MAIN, pattern, level); + this.setPattern(Zone.TOP, pattern, level); + } + public void clearPatternAll(PatternLevel level) { + this.clearPattern(Zone.MAIN, level); + this.clearPattern(Zone.TOP, level); + } + } From 31ca59d24c9f5a481c538df51cf3d8281b7dc26a Mon Sep 17 00:00:00 2001 From: Zach R Date: Sat, 6 Apr 2024 07:35:31 -0700 Subject: [PATCH 74/75] event: eod day 2 --- paths.chor | 2294 ++++++++++++++++- src/main/deploy/choreo/SourceLanePGFE.1.traj | 230 ++ src/main/deploy/choreo/SourceLanePGFE.2.traj | 680 +++++ src/main/deploy/choreo/SourceLanePGFE.3.traj | 446 ++++ src/main/deploy/choreo/SourceLanePGFE.4.traj | 572 ++++ src/main/deploy/choreo/SourceLanePGFE.traj | 1886 ++++++++++++++ .../team1540/robot2024/RobotContainer.java | 2 + .../commands/autos/CenterLanePCBA.java | 14 +- .../commands/autos/SourceLanePGFE.java | 35 + .../robot2024/subsystems/led/Leds.java | 1 + 10 files changed, 6140 insertions(+), 20 deletions(-) create mode 100644 src/main/deploy/choreo/SourceLanePGFE.1.traj create mode 100644 src/main/deploy/choreo/SourceLanePGFE.2.traj create mode 100644 src/main/deploy/choreo/SourceLanePGFE.3.traj create mode 100644 src/main/deploy/choreo/SourceLanePGFE.4.traj create mode 100644 src/main/deploy/choreo/SourceLanePGFE.traj create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFE.java diff --git a/paths.chor b/paths.chor index 31fa786d..a5087ecc 100644 --- a/paths.chor +++ b/paths.chor @@ -41312,51 +41312,44 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ "last" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 7 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 0, 1 ], - "type": "ZeroAngularVelocity", - "direction": 0 + "type": "ZeroAngularVelocity" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 3 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 7 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ @@ -41371,6 +41364,2279 @@ "circleObstacles": [], "eventMarkers": [], "isTrajectoryStale": false + }, + "SourceLanePGFE": { + "waypoints": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 19 + }, + { + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 7.501500129699707, + "y": 5.105462551116943, + "heading": 0.6747401864044066, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 8.1323881149292, + "y": 5.628637313842773, + "heading": 0.6747401864044066, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": 1.1679855597934721e-27, + "angularVelocity": 5.433585185667626e-28, + "velocityX": 7.442598398670663e-27, + "velocityY": 2.3270251613663043e-26, + "timestamp": 0 + }, + { + "x": 0.4005104003556791, + "y": 4.115348381940823, + "heading": -0.015277315995294262, + "angularVelocity": -0.30891158633586036, + "velocityX": 0.2731840597648779, + "velocityY": -0.11699249437834791, + "timestamp": 0.04945530265311637 + }, + { + "x": 0.4275520628000622, + "y": 4.103759116150694, + "heading": -0.04541480648332661, + "angularVelocity": -0.6093884552566431, + "velocityX": 0.5467899495844878, + "velocityY": -0.23433818353953934, + "timestamp": 0.09891060530623275 + }, + { + "x": 0.4681490763693427, + "y": 4.086345867978677, + "heading": -0.08991129386234921, + "angularVelocity": -0.899731373420656, + "velocityX": 0.8208829264281612, + "velocityY": -0.35210073011087883, + "timestamp": 0.14836590795934912 + }, + { + "x": 0.5223311730580589, + "y": 4.063083084813811, + "heading": -0.14811250055399447, + "angularVelocity": -1.176844616640472, + "velocityX": 1.0955770924859953, + "velocityY": -0.47037995759592216, + "timestamp": 0.1978212106124655 + }, + { + "x": 0.5901369333154594, + "y": 4.033939247984653, + "heading": -0.2191152852440036, + "angularVelocity": -1.435696090832334, + "velocityX": 1.3710513659777948, + "velocityY": -0.5892965013999555, + "timestamp": 0.24727651326558187 + }, + { + "x": 0.6716171763310703, + "y": 3.9988769782713436, + "heading": -0.3016414776380596, + "angularVelocity": -1.6687026055204155, + "velocityX": 1.647553217642207, + "velocityY": -0.708968863445023, + "timestamp": 0.29673181591869824 + }, + { + "x": 0.7668367655706729, + "y": 3.9578537486428593, + "heading": -0.39387111054062807, + "angularVelocity": -1.8649088763944044, + "velocityX": 1.9253666266584337, + "velocityY": -0.8295011339072073, + "timestamp": 0.3461871185718146 + }, + { + "x": 0.8758711660113281, + "y": 3.91082124740519, + "heading": -0.49317423957558376, + "angularVelocity": -2.00793693916861, + "velocityX": 2.2047059585385944, + "velocityY": -0.9510102802839819, + "timestamp": 0.395642421224931 + }, + { + "x": 0.9987875307433454, + "y": 3.857723681335083, + "heading": -0.5954683580210642, + "angularVelocity": -2.068415578466479, + "velocityX": 2.4854031446165235, + "velocityY": -1.0736475811812884, + "timestamp": 0.44509772387804736 + }, + { + "x": 1.1355416361286799, + "y": 3.7985302060231096, + "heading": -0.6929936795383219, + "angularVelocity": -1.9719891757878503, + "velocityX": 2.765206116410594, + "velocityY": -1.1969085646318132, + "timestamp": 0.49455302653116373 + }, + { + "x": 1.2843691968945923, + "y": 3.733334425660418, + "heading": -0.76082235123736, + "angularVelocity": -1.3715146417118123, + "velocityX": 3.0093347483848483, + "velocityY": -1.3182768452551967, + "timestamp": 0.5440083291842801 + }, + { + "x": 1.4440686661947881, + "y": 3.662157825631026, + "heading": -0.7899998870158123, + "angularVelocity": -0.5899779035446591, + "velocityX": 3.2291677683248934, + "velocityY": -1.439210685427017, + "timestamp": 0.5934636318373965 + }, + { + "x": 1.613363599943702, + "y": 3.5895175490832267, + "heading": -0.789999943508809, + "angularVelocity": -0.0000011423041349866365, + "velocityX": 3.423190733183105, + "velocityY": -1.468806632471835, + "timestamp": 0.6429189344905129 + }, + { + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "angularVelocity": -0.0000011422676237323725, + "velocityX": 3.423194018614537, + "velocityY": -1.468798975473458, + "timestamp": 0.6923742371436292 + }, + { + "x": 2.0239723336710425, + "y": 3.4134441767348855, + "heading": -0.79, + "angularVelocity": -4.2462946476416713e-16, + "velocityX": 3.4237473592821535, + "velocityY": -1.467509622685751, + "timestamp": 0.7628565514540567 + }, + { + "x": 2.250495583067251, + "y": 3.3163502598603043, + "heading": -0.79, + "angularVelocity": 2.583795012505271e-16, + "velocityX": 3.2139019782824456, + "velocityY": -1.37756425600538, + "timestamp": 0.8333388657644842 + }, + { + "x": 2.4487034476982825, + "y": 3.23139307341843, + "heading": -0.79, + "angularVelocity": 2.1975738712772502e-16, + "velocityX": 2.812164534752054, + "velocityY": -1.2053688542021173, + "timestamp": 0.9038211800749117 + }, + { + "x": 2.6185959113515174, + "y": 3.1585726243584236, + "heading": -0.79, + "angularVelocity": 1.8378454574977131e-16, + "velocityX": 2.4104268611977178, + "velocityY": -1.0331733538044932, + "timestamp": 0.9743034943853393 + }, + { + "x": 2.760172968622745, + "y": 3.0978889149966715, + "heading": -0.79, + "angularVelocity": 1.432046748255306e-16, + "velocityX": 2.0086891109686786, + "velocityY": -0.8609778205420586, + "timestamp": 1.0447858086957669 + }, + { + "x": 2.8734346168098597, + "y": 3.049341946491368, + "heading": -0.79, + "angularVelocity": 9.739728624843172e-17, + "velocityX": 1.6069513224022827, + "velocityY": -0.6887822708472163, + "timestamp": 1.1152681230061945 + }, + { + "x": 2.9583808542915984, + "y": 3.0129317195374297, + "heading": -0.79, + "angularVelocity": 7.507925930031045e-17, + "velocityX": 1.2052135108334718, + "velocityY": -0.5165867112929291, + "timestamp": 1.185750437316622 + }, + { + "x": 3.0150116799871185, + "y": 2.9886582345981343, + "heading": -0.79, + "angularVelocity": 2.284084326875716e-17, + "velocityX": 0.8034756839297177, + "velocityY": -0.3443911451656786, + "timestamp": 1.2562327516270497 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 4.059583267317121e-17, + "velocityX": 0.40173784607243285, + "velocityY": -0.17219557434345442, + "timestamp": 1.3267150659374773 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -5.776011429095129e-29, + "velocityX": -2.265299874372296e-27, + "velocityY": 1.6360733128925837e-27, + "timestamp": 1.397197380247905 + }, + { + "x": 3.05997928479042, + "y": 2.9660593331101914, + "heading": -0.757416423789292, + "angularVelocity": 0.5582463314772621, + "velocityX": 0.2852978705745241, + "velocityY": -0.17924557403559996, + "timestamp": 1.4555651193147854 + }, + { + "x": 3.09346940981965, + "y": 2.9451270864719548, + "heading": -0.6941025427131016, + "angularVelocity": 1.0847410245519795, + "velocityX": 0.5737780075883255, + "velocityY": -0.35862699108923296, + "timestamp": 1.5139328583816658 + }, + { + "x": 3.144014292961821, + "y": 2.9136974289532866, + "heading": -0.6025701359612305, + "angularVelocity": 1.5682020276130446, + "velocityX": 0.8659729492734705, + "velocityY": -0.5384765286634581, + "timestamp": 1.5723005974485462 + }, + { + "x": 3.211860363047214, + "y": 2.8716921302721943, + "heading": -0.4864204343113474, + "angularVelocity": 1.9899640367565647, + "velocityX": 1.1623898950009328, + "velocityY": -0.7196663662603233, + "timestamp": 1.6306683365154266 + }, + { + "x": 3.2972613235718167, + "y": 2.8189697938754956, + "heading": -0.350515523406248, + "angularVelocity": 2.3284251382321566, + "velocityX": 1.463153479814365, + "velocityY": -0.903278715940788, + "timestamp": 1.689036075582307 + }, + { + "x": 3.4004578303174937, + "y": 2.7553674746788306, + "heading": -0.2010210241746004, + "angularVelocity": 2.5612521852242733, + "velocityX": 1.7680401604631244, + "velocityY": -1.0896827633461483, + "timestamp": 1.7474038146491875 + }, + { + "x": 3.521660389826237, + "y": 2.6807799872839095, + "heading": -0.04666004939323709, + "angularVelocity": 2.644628304078894, + "velocityX": 2.0765333974965823, + "velocityY": -1.2778889260976047, + "timestamp": 1.805771553716068 + }, + { + "x": 3.6607588635338284, + "y": 2.5953087461465145, + "heading": 0.09647781625083168, + "angularVelocity": 2.4523455582210394, + "velocityX": 2.383139657820331, + "velocityY": -1.4643575801258646, + "timestamp": 1.8641392927829483 + }, + { + "x": 3.8159145666935705, + "y": 2.5005609897698613, + "heading": 0.19948059626332554, + "angularVelocity": 1.7647210883818645, + "velocityX": 2.6582441883170693, + "velocityY": -1.6232898154250444, + "timestamp": 1.9225070318498287 + }, + { + "x": 3.9841103579797825, + "y": 2.394344369414975, + "heading": 0.2530307730268158, + "angularVelocity": 0.9174618996656708, + "velocityX": 2.8816567846406667, + "velocityY": -1.8197830180329346, + "timestamp": 1.9808747709167092 + }, + { + "x": 4.165386797993937, + "y": 2.276764205392623, + "heading": 0.2569441402014935, + "angularVelocity": 0.06704674940712647, + "velocityX": 3.1057642956915044, + "velocityY": -2.014471793872694, + "timestamp": 2.0392425099835894 + }, + { + "x": 4.350490054886, + "y": 2.162710813518831, + "heading": 0.2569442210228683, + "angularVelocity": 0.0000013846925729023855, + "velocityX": 3.171328200325918, + "velocityY": -1.9540484811841776, + "timestamp": 2.0976102490504696 + }, + { + "x": 4.53559341768799, + "y": 2.048657593531974, + "heading": 0.2569443018441724, + "angularVelocity": 0.0000013846913619086116, + "velocityX": 3.171330014854447, + "velocityY": -1.9540455362879443, + "timestamp": 2.1559779881173498 + }, + { + "x": 4.720696780504463, + "y": 1.934604373568621, + "heading": 0.25694438266547676, + "angularVelocity": 0.0000013846913668711097, + "velocityX": 3.1713300151025647, + "velocityY": -1.9540455358852609, + "timestamp": 2.21434572718423 + }, + { + "x": 4.9058004208396335, + "y": 1.8205516040071454, + "heading": 0.25694446348677025, + "angularVelocity": 0.0000013846911809077847, + "velocityX": 3.1713347697615966, + "velocityY": -1.9540378192615708, + "timestamp": 2.27271346625111 + }, + { + "x": 5.096209165426888, + "y": 1.7155949376896078, + "heading": 0.25694454460803495, + "angularVelocity": 0.0000013898305117986064, + "velocityX": 3.2622258054072626, + "velocityY": -1.7981965379415077, + "timestamp": 2.3310812053179903 + }, + { + "x": 5.295905298565607, + "y": 1.629613278960399, + "heading": 0.25694462919552075, + "angularVelocity": 0.0000014492164191385246, + "velocityX": 3.4213443304682105, + "velocityY": -1.4731024381582958, + "timestamp": 2.3894489443848705 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.2569447220794932, + "angularVelocity": 0.0000015913580676492416, + "velocityX": 3.5481637974123346, + "velocityY": -1.1340904342963076, + "timestamp": 2.4478166834517507 + }, + { + "x": 5.694707875531767, + "y": 1.52024537146014, + "heading": 0.25694480525283153, + "angularVelocity": 0.0000015766499748638697, + "velocityX": 3.6339835774202793, + "velocityY": -0.8184074026053739, + "timestamp": 2.500569887803966 + }, + { + "x": 5.889460824583239, + "y": 1.4940579532693374, + "heading": 0.25694488397529436, + "angularVelocity": 0.0000014922783143570964, + "velocityX": 3.6917747735506756, + "velocityY": -0.49641379158615856, + "timestamp": 2.553323092156181 + }, + { + "x": 6.0857603375931175, + "y": 1.485058307103912, + "heading": 0.2569449614499782, + "angularVelocity": 0.0000014686251720162004, + "velocityX": 3.7210917406884763, + "velocityY": -0.17059904276786395, + "timestamp": 2.6060762965083963 + }, + { + "x": 6.282233415675428, + "y": 1.4886391007138935, + "heading": 0.2569450396471809, + "angularVelocity": 0.0000014823213805790595, + "velocityX": 3.724381873952661, + "velocityY": 0.06787821998591004, + "timestamp": 2.6588295008606115 + }, + { + "x": 6.478706465106145, + "y": 1.492221466050733, + "heading": 0.25694511784454455, + "angularVelocity": 0.0000014823244316657533, + "velocityX": 3.724381330827469, + "velocityY": 0.06790801394586561, + "timestamp": 2.7115827052128267 + }, + { + "x": 6.675014894210808, + "y": 1.5010244818845797, + "heading": 0.2569451969790386, + "angularVelocity": 0.0000015000888575462877, + "velocityX": 3.7212607559150137, + "velocityY": 0.16687167996605787, + "timestamp": 2.764335909565042 + }, + { + "x": 6.869794082781554, + "y": 1.5270160132210213, + "heading": 0.25694528055287635, + "angularVelocity": 0.0000015842419202847933, + "velocityX": 3.6922721749805327, + "velocityY": 0.4927005222830628, + "timestamp": 2.817089113917257 + }, + { + "x": 7.061541469504376, + "y": 1.5699966670379106, + "heading": 0.256946313055063, + "angularVelocity": 0.000019572312228011845, + "velocityX": 3.6348007495922103, + "velocityY": 0.8147496316986196, + "timestamp": 2.8698423182694723 + }, + { + "x": 7.242025004547978, + "y": 1.6235443273244181, + "heading": 0.2892009263238711, + "angularVelocity": 0.6114247213013846, + "velocityX": 3.4212809868112584, + "velocityY": 1.0150598611790123, + "timestamp": 2.9225955226216875 + }, + { + "x": 7.410147525588679, + "y": 1.6859098332103064, + "heading": 0.355218131661103, + "angularVelocity": 1.2514349819673058, + "velocityX": 3.1869632016702596, + "velocityY": 1.1822126570643, + "timestamp": 2.9753487269739027 + }, + { + "x": 7.565027902002143, + "y": 1.7562974315327362, + "heading": 0.45280543116483274, + "angularVelocity": 1.8498838260548631, + "velocityX": 2.935942533071192, + "velocityY": 1.3342810012539876, + "timestamp": 3.028101931326118 + }, + { + "x": 7.703780357583263, + "y": 1.8320280138994296, + "heading": 0.5628377984662216, + "angularVelocity": 2.0857949512742477, + "velocityX": 2.630218529564891, + "velocityY": 1.4355636457847423, + "timestamp": 3.080855135678333 + }, + { + "x": 7.825662907370359, + "y": 1.9113228263820174, + "heading": 0.6698617562917842, + "angularVelocity": 2.028766956240237, + "velocityX": 2.3104293148398787, + "velocityY": 1.503127884955833, + "timestamp": 3.1336083400305483 + }, + { + "x": 7.930693514616472, + "y": 1.993224495332611, + "heading": 0.7659443680915856, + "angularVelocity": 1.821360673340157, + "velocityX": 1.990980615032594, + "velocityY": 1.552543963088276, + "timestamp": 3.1863615443827635 + }, + { + "x": 8.019011714129224, + "y": 2.0771819587983416, + "heading": 0.8464048765676822, + "angularVelocity": 1.525225044888065, + "velocityX": 1.6741769641722983, + "velocityY": 1.5915140036835516, + "timestamp": 3.2391147487349787 + }, + { + "x": 8.090768492192119, + "y": 2.1628615391485617, + "heading": 0.9080474489079001, + "angularVelocity": 1.1685085881921318, + "velocityX": 1.3602354386626168, + "velocityY": 1.6241587862258966, + "timestamp": 3.291867953087194 + }, + { + "x": 8.146101418716508, + "y": 2.2500495157540152, + "heading": 0.9485346126335614, + "angularVelocity": 0.7674825486494136, + "velocityX": 1.048901715144179, + "velocityY": 1.6527522389602969, + "timestamp": 3.344621157439409 + }, + { + "x": 8.185122291410464, + "y": 2.3385982915715853, + "heading": 0.9662103754331748, + "angularVelocity": 0.33506519682858266, + "velocityX": 0.7396872507199079, + "velocityY": 1.6785478134439036, + "timestamp": 3.3973743617916243 + }, + { + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "angularVelocity": -0.11638132008630137, + "velocityX": 0.43192419429508555, + "velocityY": 1.702181892471887, + "timestamp": 3.4501275661438395 + }, + { + "x": 8.211423054828616, + "y": 2.5404723922535313, + "heading": 0.9162466453350178, + "angularVelocity": -0.6758725051109615, + "velocityX": 0.054215367724374734, + "velocityY": 1.7285138763843786, + "timestamp": 3.5149685564274566 + }, + { + "x": 8.190202888383553, + "y": 2.653847423099536, + "heading": 0.8391734249663445, + "angularVelocity": -1.188649649420099, + "velocityX": -0.32726468785015433, + "velocityY": 1.748508626257838, + "timestamp": 3.5798095467110738 + }, + { + "x": 8.144008306424402, + "y": 2.7680190139904237, + "heading": 0.7319233890251923, + "angularVelocity": -1.654046853264217, + "velocityX": -0.7124286929779271, + "velocityY": 1.760793448580852, + "timestamp": 3.644650536994691 + }, + { + "x": 8.072604152106313, + "y": 2.8824539716658655, + "heading": 0.5977397407747723, + "angularVelocity": -2.06942626359492, + "velocityX": -1.101219367652527, + "velocityY": 1.764855181497056, + "timestamp": 3.709491527278308 + }, + { + "x": 7.975691382967057, + "y": 2.9965380091624785, + "heading": 0.44171220804306943, + "angularVelocity": -2.4063101450059983, + "velocityX": -1.4946219777853038, + "velocityY": 1.759443170093563, + "timestamp": 3.774332517561925 + }, + { + "x": 7.852920805151411, + "y": 3.109310419044201, + "heading": 0.2733111632769955, + "angularVelocity": -2.5971386931242324, + "velocityX": -1.893409975366543, + "velocityY": 1.7392147989790332, + "timestamp": 3.8391735078455422 + }, + { + "x": 7.704278478364198, + "y": 3.2190283422070105, + "heading": 0.10912005253045581, + "angularVelocity": -2.53221164618803, + "velocityX": -2.292412964963152, + "velocityY": 1.6921074567630605, + "timestamp": 3.9040144981291593 + }, + { + "x": 7.532421373794031, + "y": 3.3230579148672983, + "heading": -0.010926874392962598, + "angularVelocity": -1.8514048967840961, + "velocityX": -2.650439233245175, + "velocityY": 1.6043797635609574, + "timestamp": 3.9688554884127765 + }, + { + "x": 7.337707687800783, + "y": 3.422034456345691, + "heading": -0.0821100823564174, + "angularVelocity": -1.0978118571616002, + "velocityX": -3.002941274363059, + "velocityY": 1.5264501829084578, + "timestamp": 4.033696478696394 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": -0.09725138680843894, + "angularVelocity": -0.23351439245133218, + "velocityX": -3.3462025777712125, + "velocityY": 1.46270429328826, + "timestamp": 4.098537468980011 + }, + { + "x": 7.010606209659238, + "y": 3.566708155417018, + "heading": -0.09725146950362612, + "angularVelocity": -0.0000025483254840822735, + "velocityX": -3.3937655527961965, + "velocityY": 1.535571150717811, + "timestamp": 4.130988263708164 + }, + { + "x": 6.900475999603528, + "y": 3.6165390557870793, + "heading": -0.09725155219704651, + "angularVelocity": -0.000002548271039111256, + "velocityX": -3.3937600289389183, + "velocityY": 1.5355833589748464, + "timestamp": 4.1634390584363175 + }, + { + "x": 6.7903457895857455, + "y": 3.666369956240961, + "heading": -0.0972516348904687, + "angularVelocity": -0.000002548271094167607, + "velocityX": -3.3937600277701754, + "velocityY": 1.5355833615578496, + "timestamp": 4.195889853164471 + }, + { + "x": 6.680215579567971, + "y": 3.7162008566948606, + "heading": -0.09725171758389267, + "angularVelocity": -0.0000025482711490074116, + "velocityX": -3.393760027769923, + "velocityY": 1.535583361558393, + "timestamp": 4.228340647892624 + }, + { + "x": 6.570085369550196, + "y": 3.7660317571487596, + "heading": -0.09725180027731847, + "angularVelocity": -0.0000025482712052155066, + "velocityX": -3.39376002776992, + "velocityY": 1.53558336155839, + "timestamp": 4.260791442620778 + }, + { + "x": 6.459955159532422, + "y": 3.815862657602659, + "heading": -0.09725188297074605, + "angularVelocity": -0.000002548271260506862, + "velocityX": -3.3937600277699165, + "velocityY": 1.5355833615583867, + "timestamp": 4.293242237348931 + }, + { + "x": 6.349824949514647, + "y": 3.865693558056558, + "heading": -0.09725196566417542, + "angularVelocity": -0.0000025482713153661305, + "velocityX": -3.3937600277699125, + "velocityY": 1.5355833615583832, + "timestamp": 4.325693032077084 + }, + { + "x": 6.239694739496873, + "y": 3.915524458510457, + "heading": -0.09725204835760656, + "angularVelocity": -0.000002548271370425602, + "velocityX": -3.3937600277699085, + "velocityY": 1.5355833615583803, + "timestamp": 4.358143826805238 + }, + { + "x": 6.129564529479099, + "y": 3.965355358964356, + "heading": -0.09725213105103951, + "angularVelocity": -0.0000025482714255163693, + "velocityX": -3.3937600277699054, + "velocityY": 1.5355833615583767, + "timestamp": 4.390594621533391 + }, + { + "x": 6.019434319461326, + "y": 4.015186259418255, + "heading": -0.09725221374447428, + "angularVelocity": -0.000002548271481519915, + "velocityX": -3.3937600277699014, + "velocityY": 1.5355833615583734, + "timestamp": 4.4230454162615445 + }, + { + "x": 5.9093041094435526, + "y": 4.065017159872154, + "heading": -0.09725229643791083, + "angularVelocity": -0.000002548271536879473, + "velocityX": -3.3937600277698974, + "velocityY": 1.5355833615583703, + "timestamp": 4.455496210989698 + }, + { + "x": 5.799173899425779, + "y": 4.114848060326053, + "heading": -0.09725237913134915, + "angularVelocity": -0.000002548271591483022, + "velocityX": -3.3937600277698934, + "velocityY": 1.535583361558367, + "timestamp": 4.487947005717851 + }, + { + "x": 5.689043689408005, + "y": 4.164678960779951, + "heading": -0.09725246182478926, + "angularVelocity": -0.0000025482716464166734, + "velocityX": -3.39376002776989, + "velocityY": 1.5355833615583636, + "timestamp": 4.520397800446005 + }, + { + "x": 5.5789134793902315, + "y": 4.214509861233849, + "heading": -0.09725254451823119, + "angularVelocity": -0.000002548271702485751, + "velocityX": -3.3937600277698863, + "velocityY": 1.5355833615583605, + "timestamp": 4.552848595174158 + }, + { + "x": 5.468783269372458, + "y": 4.264340761687747, + "heading": -0.09725262721167492, + "angularVelocity": -0.0000025482717576806813, + "velocityX": -3.3937600277698823, + "velocityY": 1.5355833615583572, + "timestamp": 4.585299389902311 + }, + { + "x": 5.3586530593546975, + "y": 4.314171662141674, + "heading": -0.09725270990512043, + "angularVelocity": -0.0000025482718127641656, + "velocityX": -3.3937600277694844, + "velocityY": 1.5355833615592254, + "timestamp": 4.617750184630465 + }, + { + "x": 5.2485228493974105, + "y": 4.364002562729252, + "heading": -0.09725279259856771, + "angularVelocity": -0.0000025482718674912185, + "velocityX": -3.393760025905934, + "velocityY": 1.5355833656778057, + "timestamp": 4.650200979358618 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.0000025482898100852076, + "velocityX": -3.393751218037592, + "velocityY": 1.5356028315921753, + "timestamp": 4.6826517740867715 + }, + { + "x": 4.9137282295128415, + "y": 4.535640155229253, + "heading": -0.09725287529313088, + "angularVelocity": -7.435036019118785e-12, + "velocityX": -3.1302347498710454, + "velocityY": 1.6971138308958593, + "timestamp": 4.754424249221062 + }, + { + "x": 4.717146626097375, + "y": 4.642220517268037, + "heading": -0.097252875293223, + "angularVelocity": -1.283652180157178e-12, + "velocityX": -2.7389553313809003, + "velocityY": 1.4849754288028512, + "timestamp": 4.826196724355352 + }, + { + "x": 4.548648110952986, + "y": 4.733575136181824, + "heading": -0.09725287529315746, + "angularVelocity": 9.133130247777463e-13, + "velocityX": -2.347675969501115, + "velocityY": 1.2728364006237478, + "timestamp": 4.897969199489642 + }, + { + "x": 4.408232682725308, + "y": 4.809703996992033, + "heading": -0.09725287529302883, + "angularVelocity": 1.7921853517363252e-12, + "velocityX": -1.9563966264916142, + "velocityY": 1.0606971637493035, + "timestamp": 4.969741674623933 + }, + { + "x": 4.295900340737155, + "y": 4.870607092209374, + "heading": -0.09725287529288444, + "angularVelocity": 2.0118085530494987e-12, + "velocityX": -1.5651172929172832, + "velocityY": 0.8485578225272118, + "timestamp": 5.041514149758223 + }, + { + "x": 4.211651084582216, + "y": 4.916284417340276, + "heading": -0.09725287529275266, + "angularVelocity": 1.8359969573857423e-12, + "velocityX": -1.1738379650040625, + "velocityY": 0.636418418696543, + "timestamp": 5.113286624892513 + }, + { + "x": 4.155484913989614, + "y": 4.946735969389024, + "heading": -0.09725287529265242, + "angularVelocity": 1.3966146295291797e-12, + "velocityX": -0.7825586408649183, + "velocityY": 0.42427897312682855, + "timestamp": 5.185059100026804 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 7.688238334265391e-13, + "velocityX": -0.3912793194215448, + "velocityY": 0.21213949774351348, + "timestamp": 5.256831575161094 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 3.652934582681632e-29, + "velocityX": -2.1119125295164495e-27, + "velocityY": 7.525449232438864e-28, + "timestamp": 5.328604050295384 + }, + { + "x": 4.149345287192321, + "y": 4.948730090302555, + "heading": -0.09725287542061173, + "angularVelocity": -1.9914932727158263e-9, + "velocityX": 0.34136955009475556, + "velocityY": -0.2058419570123514, + "timestamp": 5.392884705732758 + }, + { + "x": 4.193331867161711, + "y": 4.922433486987429, + "heading": -0.09725287565526207, + "angularVelocity": -3.650403639422262e-9, + "velocityX": 0.6842895373437039, + "velocityY": -0.4090904664272752, + "timestamp": 5.457165361170132 + }, + { + "x": 4.259487364434434, + "y": 4.883286547964955, + "heading": -0.09725287596919821, + "angularVelocity": -4.883835485414618e-9, + "velocityX": 1.0291665015328901, + "velocityY": -0.6090003089749593, + "timestamp": 5.521446016607506 + }, + { + "x": 4.347975400700828, + "y": 4.831575784808056, + "heading": -0.09725287632620158, + "angularVelocity": -5.553822878939374e-9, + "velocityX": 1.3765888923239986, + "velocityY": -0.8044529540816499, + "timestamp": 5.5857266720448795 + }, + { + "x": 4.45901715765165, + "y": 4.76770271955414, + "heading": -0.09725287667606394, + "angularVelocity": -5.442731629415375e-9, + "velocityX": 1.7274521579669766, + "velocityY": -0.9936592092802463, + "timestamp": 5.650007327482253 + }, + { + "x": 4.592927185358519, + "y": 4.69226963680943, + "heading": -0.09725287694462321, + "angularVelocity": -4.1779173473054744e-9, + "velocityX": 2.083208809800217, + "velocityY": -1.1734958555019974, + "timestamp": 5.714287982919627 + }, + { + "x": 4.750184246245984, + "y": 4.606276833638444, + "heading": -0.09725287701156488, + "angularVelocity": -1.0413967826345418e-9, + "velocityX": 2.446413463233511, + "velocityY": -1.337771100588195, + "timestamp": 5.778568638357001 + }, + { + "x": 4.931581310093376, + "y": 4.511690334370361, + "heading": -0.0972528766497097, + "angularVelocity": 5.6293012279094655e-9, + "velocityX": 2.821954172886779, + "velocityY": -1.4714613381662665, + "timestamp": 5.842849293794375 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 2.111229970452055e-8, + "velocityX": 3.2173227507078654, + "velocityY": -1.5223279648179473, + "timestamp": 5.907129949231749 + }, + { + "x": 5.3988689559213965, + "y": 4.32613897098341, + "heading": -0.0972528699779729, + "angularVelocity": 7.203036405891024e-8, + "velocityX": 3.5302934161705415, + "velocityY": -1.1885528129676428, + "timestamp": 5.980913060138573 + }, + { + "x": 5.668131488513907, + "y": 4.271040046607528, + "heading": -0.09725286670603908, + "angularVelocity": 4.4345294956463885e-8, + "velocityX": 3.6493789606207083, + "velocityY": -0.7467687889368456, + "timestamp": 6.054696171045398 + }, + { + "x": 5.942117714678702, + "y": 4.249366302589977, + "heading": -0.09725286355772216, + "angularVelocity": 4.266988599202702e-8, + "velocityX": 3.713400299843557, + "velocityY": -0.2937493926614278, + "timestamp": 6.128479281952223 + }, + { + "x": 6.216382330790908, + "y": 4.231558335942029, + "heading": -0.09725286041527645, + "angularVelocity": 4.2590311894180227e-8, + "velocityX": 3.7171733848218165, + "velocityY": -0.2413555951908442, + "timestamp": 6.202262392859048 + }, + { + "x": 6.490646951405068, + "y": 4.21375043862986, + "heading": -0.09725285727283077, + "angularVelocity": 4.259031148289501e-8, + "velocityX": 3.717173445837876, + "velocityY": -0.2413546554665762, + "timestamp": 6.2760455037658724 + }, + { + "x": 6.764911572019294, + "y": 4.195942541318692, + "heading": -0.09725285413038512, + "angularVelocity": 4.2590311325076504e-8, + "velocityX": 3.717173445838756, + "velocityY": -0.241354655453012, + "timestamp": 6.349828614672697 + }, + { + "x": 7.039176191804646, + "y": 4.178134631242246, + "heading": -0.09725285098790415, + "angularVelocity": 4.259078987217418e-8, + "velocityX": 3.7171734346048453, + "velocityY": -0.24135482846385062, + "timestamp": 6.423611725579522 + }, + { + "x": 7.303151625715879, + "y": 4.160355738175748, + "heading": -0.07493794888540205, + "angularVelocity": 0.302439160239284, + "velocityX": 3.5777216583424787, + "velocityY": -0.24096155404655972, + "timestamp": 6.497394836486347 + }, + { + "x": 7.533445930864737, + "y": 4.1447637615713475, + "heading": -0.055524777247044715, + "angularVelocity": 0.2631113191048932, + "velocityX": 3.1212333326481616, + "velocityY": -0.21132175660214245, + "timestamp": 6.5711779473931715 + }, + { + "x": 7.730059092320319, + "y": 4.131358730965977, + "heading": -0.0390137097405274, + "angularVelocity": 0.22377841356361194, + "velocityX": 2.664744804591801, + "velocityY": -0.18168155883665832, + "timestamp": 6.644961058299996 + }, + { + "x": 7.892991106532595, + "y": 4.120140669647355, + "heading": -0.025405081189306844, + "angularVelocity": 0.1844409700806171, + "velocityX": 2.20825622842103, + "velocityY": -0.15204104544723207, + "timestamp": 6.718744169206821 + }, + { + "x": 8.02224197249394, + "y": 4.111109595697515, + "heading": -0.014699203415963246, + "angularVelocity": 0.1450993003922434, + "velocityX": 1.751767638593706, + "velocityY": -0.12240028698767993, + "timestamp": 6.792527280113646 + }, + { + "x": 8.117811689845258, + "y": 4.104265522227424, + "heading": -0.006896323202665822, + "angularVelocity": 0.10575428600660502, + "velocityX": 1.2952790438994193, + "velocityY": -0.09275935083211777, + "timestamp": 6.866310391020471 + }, + { + "x": 8.179700258175622, + "y": 4.099608457473234, + "heading": -0.0019965838975628653, + "angularVelocity": 0.06640732878951748, + "velocityX": 0.8387904436357877, + "velocityY": -0.06311830305000854, + "timestamp": 6.940093501927295 + }, + { + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": -2.1241979189811956e-28, + "angularVelocity": 0.02706017505936011, + "velocityX": 0.3823018326887492, + "velocityY": -0.03347720903449786, + "timestamp": 7.01387661283412 + }, + { + "x": 8.200639571345327, + "y": 4.096943687462525, + "heading": -0.0010802899918473687, + "angularVelocity": -0.014022329834604143, + "velocityX": -0.09434112255025054, + "velocityY": -0.0025274615139553986, + "timestamp": 7.0909173046870135 + }, + { + "x": 8.15665056358688, + "y": 4.099133352364744, + "heading": -0.005325598680954133, + "angularVelocity": -0.05510475810903996, + "velocityX": -0.5709840695932878, + "velocityY": 0.02842218637392462, + "timestamp": 7.167957996539907 + }, + { + "x": 8.075940653456616, + "y": 4.103707386175575, + "heading": -0.012735784669566238, + "angularVelocity": -0.09618535101893455, + "velocityX": -1.0476270161796544, + "velocityY": 0.05937166062274877, + "timestamp": 7.2449986883928 + }, + { + "x": 7.958509840643352, + "y": 4.1106657697854265, + "heading": -0.023310616019513074, + "angularVelocity": -0.1372629333357902, + "velocityX": -1.5242699668052726, + "velocityY": 0.09032088682612197, + "timestamp": 7.322039380245694 + }, + { + "x": 7.804358124937036, + "y": 4.120008478318512, + "heading": -0.037049836652876394, + "angularVelocity": -0.17833719172197895, + "velocityX": -2.0009129201573495, + "velocityY": 0.12126979013798225, + "timestamp": 7.399080072098587 + }, + { + "x": 7.613485507099167, + "y": 4.131735481105861, + "heading": -0.05395325351914194, + "angularVelocity": -0.21940894428287439, + "velocityX": -2.4775558636250903, + "velocityY": 0.15221829536189774, + "timestamp": 7.476120763951481 + }, + { + "x": 7.385891991055361, + "y": 4.145846741676362, + "heading": -0.07402084166347252, + "angularVelocity": -0.2604803729261546, + "velocityX": -2.9541987561377083, + "velocityY": 0.18316632718520057, + "timestamp": 7.553161455804374 + }, + { + "x": 7.121577593711164, + "y": 4.16234221776091, + "heading": -0.09725284930983917, + "angularVelocity": -0.30155502355466174, + "velocityX": -3.430841429213758, + "velocityY": 0.21411381034903446, + "timestamp": 7.6302021476572675 + }, + { + "x": 6.835203978246458, + "y": 4.1809363249101, + "heading": -0.09725285259123628, + "angularVelocity": -4.259303804512275e-8, + "velocityX": -3.7171734647908923, + "velocityY": 0.24135436354458084, + "timestamp": 7.707242839510161 + }, + { + "x": 6.5488303639215975, + "y": 4.199530449615618, + "heading": -0.09725285587256685, + "angularVelocity": -4.259217424526975e-8, + "velocityX": -3.7171734499955185, + "velocityY": 0.24135459142842275, + "timestamp": 7.784283531363054 + }, + { + "x": 6.262456749596831, + "y": 4.218124574322582, + "heading": -0.09725285915389743, + "angularVelocity": -4.259217438362485e-8, + "velocityX": -3.7171734499943, + "velocityY": 0.24135459144719548, + "timestamp": 7.861324223215948 + }, + { + "x": 5.976083139919176, + "y": 4.236718770600948, + "heading": -0.09725286243522813, + "angularVelocity": -4.259217585749446e-8, + "velocityX": -3.717173389674077, + "velocityY": 0.24135552045497527, + "timestamp": 7.938364915068841 + }, + { + "x": 5.690024921889411, + "y": 4.259659658304737, + "heading": -0.09725286572349402, + "angularVelocity": -4.26821957688407e-8, + "velocityX": -3.71307955769639, + "velocityY": 0.2977762420357482, + "timestamp": 8.015405606921734 + }, + { + "x": 5.4092552652272206, + "y": 4.319022844462833, + "heading": -0.09725286915389218, + "angularVelocity": -4.452709450605306e-8, + "velocityX": -3.644433219762762, + "velocityY": 0.7705432639604047, + "timestamp": 8.092446298774627 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -7.968133349338986e-8, + "velocityX": -3.5158347290283, + "velocityY": 1.2306645781352283, + "timestamp": 8.16948699062752 + }, + { + "x": 4.909018653493503, + "y": 4.524275328875486, + "heading": -0.09725287687080804, + "angularVelocity": -2.1964896325716688e-8, + "velocityX": -3.192337867898693, + "velocityY": 1.5370761958403918, + "timestamp": 8.241338492962369 + }, + { + "x": 4.71108590609518, + "y": 4.628682093882472, + "heading": -0.09725287720527089, + "angularVelocity": -4.654917845961093e-9, + "velocityX": -2.754747513502236, + "velocityY": 1.453090911313464, + "timestamp": 8.313189995297217 + }, + { + "x": 4.54294597125426, + "y": 4.721368217002082, + "heading": -0.09725287703485493, + "angularVelocity": 2.3717800726027118e-9, + "velocityX": -2.3401032598781413, + "velocityY": 1.2899677822694209, + "timestamp": 8.385041497632065 + }, + { + "x": 4.40369795049572, + "y": 4.80026148816612, + "heading": -0.09725287664966911, + "angularVelocity": 5.360859554294991e-9, + "velocityX": -1.937997344990855, + "velocityY": 1.0980044759032763, + "timestamp": 8.456892999966913 + }, + { + "x": 4.292814205558175, + "y": 4.864307082758427, + "heading": -0.09725287620265413, + "angularVelocity": 6.221372875954885e-9, + "velocityX": -1.5432348849269109, + "velocityY": 0.8913605493429588, + "timestamp": 8.52874450230176 + }, + { + "x": 4.209952469660844, + "y": 4.91286872561946, + "heading": -0.09725287578795568, + "angularVelocity": 5.7716044742121175e-9, + "velocityX": -1.1532359547775557, + "velocityY": 0.6758612037744439, + "timestamp": 8.600596004636609 + }, + { + "x": 4.154873707750821, + "y": 4.945521497298995, + "heading": -0.09725287546928488, + "angularVelocity": 4.435130638682436e-9, + "velocityX": -0.7665638173206213, + "velocityY": 0.4544480020384788, + "timestamp": 8.672447506971457 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 2.459066789540458e-9, + "velocityX": -0.38234244368231685, + "velocityY": 0.22880870103743692, + "timestamp": 8.744299009306305 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.7012427908311302e-26, + "velocityX": 2.173382321303034e-25, + "velocityY": 2.8154271739064046e-25, + "timestamp": 8.816150511641153 + }, + { + "x": 4.147908938552217, + "y": 4.9451046130456335, + "heading": -0.10362139740935014, + "angularVelocity": -0.09725274804530627, + "velocityX": 0.31316100417438686, + "velocityY": -0.2574227575741351, + "timestamp": 8.881634749822124 + }, + { + "x": 4.189500528478716, + "y": 4.912093615670984, + "heading": -0.11521450306933155, + "angularVelocity": -0.17703658135172537, + "velocityX": 0.6351389446045882, + "velocityY": -0.5041060000334822, + "timestamp": 8.947118988003096 + }, + { + "x": 4.2528908246594135, + "y": 4.863879621749849, + "heading": -0.13053557559089707, + "angularVelocity": -0.23396580531675798, + "velocityX": 0.9680237251216575, + "velocityY": -0.7362686848076517, + "timestamp": 9.012603226184067 + }, + { + "x": 4.338972089495466, + "y": 4.801805387831604, + "heading": -0.1475645296449939, + "angularVelocity": -0.2600466085752675, + "velocityX": 1.3145341112186142, + "velocityY": -0.9479263352914945, + "timestamp": 9.078087464365039 + }, + { + "x": 4.4488531713485, + "y": 4.727871318118375, + "heading": -0.16346474025801716, + "angularVelocity": -0.24280973642972667, + "velocityX": 1.677977554680654, + "velocityY": -1.1290361126124002, + "timestamp": 9.14357170254601 + }, + { + "x": 4.583832471873963, + "y": 4.645249641206774, + "heading": -0.17403705358129207, + "angularVelocity": -0.1614482143635436, + "velocityX": 2.0612486954866247, + "velocityY": -1.26170326183331, + "timestamp": 9.209055940726982 + }, + { + "x": 4.74502129849887, + "y": 4.559256593692149, + "heading": -0.17264089042553551, + "angularVelocity": 0.021320598582793455, + "velocityX": 2.4614904456771134, + "velocityY": -1.3131869577069004, + "timestamp": 9.274540178907953 + }, + { + "x": 4.931721031095368, + "y": 4.478536439161179, + "heading": -0.14954301236675954, + "angularVelocity": 0.3527242386930233, + "velocityX": 2.8510636724601572, + "velocityY": -1.2326653981663842, + "timestamp": 9.340024417088925 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 0.7985148568065102, + "velocityX": 3.1560555624992586, + "velocityY": -0.9880598134339889, + "timestamp": 9.405508655269896 + }, + { + "x": 5.280245938471686, + "y": 4.380962564632748, + "heading": -0.04650808406049355, + "angularVelocity": 1.167166985692144, + "velocityX": 3.2627221399232984, + "velocityY": -0.7560690286372685, + "timestamp": 9.448985545769855 + }, + { + "x": 5.424678195182071, + "y": 4.359487062895353, + "heading": 0.008191327783648325, + "angularVelocity": 1.2581261266647945, + "velocityX": 3.322046610268118, + "velocityY": -0.493952108590108, + "timestamp": 9.492462436269813 + }, + { + "x": 5.570537921062946, + "y": 4.349646196769418, + "heading": 0.06315884602952881, + "angularVelocity": 1.264292768268082, + "velocityX": 3.354879436030857, + "velocityY": -0.22634705501637128, + "timestamp": 9.535939326769771 + }, + { + "x": 5.717103514016266, + "y": 4.35150566579831, + "heading": 0.11770080768702691, + "angularVelocity": 1.2545046582287314, + "velocityX": 3.3711148904142765, + "velocityY": 0.04276913568356193, + "timestamp": 9.57941621726973 + }, + { + "x": 5.863572420206036, + "y": 4.365085009600068, + "heading": 0.17138870164708653, + "angularVelocity": 1.2348604820326599, + "velocityX": 3.368891024759707, + "velocityY": 0.3123347517635909, + "timestamp": 9.622893107769688 + }, + { + "x": 6.009034373006399, + "y": 4.3903410665362745, + "heading": 0.22417378599549884, + "angularVelocity": 1.2140952064744144, + "velocityX": 3.345730366814133, + "velocityY": 0.5809076188700988, + "timestamp": 9.666369998269646 + }, + { + "x": 6.152424125530893, + "y": 4.427134155183534, + "heading": 0.2765912547304818, + "angularVelocity": 1.205639780863199, + "velocityX": 3.2980682582309235, + "velocityY": 0.8462677119766714, + "timestamp": 9.709846888769604 + }, + { + "x": 6.292378614943397, + "y": 4.475129707153334, + "heading": 0.330369231652926, + "angularVelocity": 1.2369324554730028, + "velocityX": 3.2190547162667764, + "velocityY": 1.1039324896026255, + "timestamp": 9.753323779269563 + }, + { + "x": 6.427989965818357, + "y": 4.530754098625934, + "heading": 0.3892449871584701, + "angularVelocity": 1.3541850585105815, + "velocityX": 3.1191593813520333, + "velocityY": 1.2794013286818071, + "timestamp": 9.796800669769521 + }, + { + "x": 6.568195373885816, + "y": 4.593631496863788, + "heading": 0.42157526724387395, + "angularVelocity": 0.743619879748183, + "velocityX": 3.224826027233803, + "velocityY": 1.4462257423381046, + "timestamp": 9.84027756026948 + }, + { + "x": 6.710207768570418, + "y": 4.660649558034501, + "heading": 0.440928265304377, + "angularVelocity": 0.44513298531599427, + "velocityX": 3.26638802940008, + "velocityY": 1.5414639915606807, + "timestamp": 9.883754450769437 + }, + { + "x": 6.85219596425133, + "y": 4.730282709551517, + "heading": 0.4559517105455699, + "angularVelocity": 0.34555013176959587, + "velocityX": 3.265831434772203, + "velocityY": 1.6016129653311566, + "timestamp": 9.927231341269396 + }, + { + "x": 6.992339110580002, + "y": 4.802100760534676, + "heading": 0.47358684784417715, + "angularVelocity": 0.4056209424320313, + "velocityX": 3.223393962105149, + "velocityY": 1.6518672369917733, + "timestamp": 9.970708231769354 + }, + { + "x": 7.1318594634446475, + "y": 4.8743385269135056, + "heading": 0.49238243611774024, + "angularVelocity": 0.4323121561230585, + "velocityX": 3.2090692609394345, + "velocityY": 1.6615209953641499, + "timestamp": 10.014185122269312 + }, + { + "x": 7.264750404712455, + "y": 4.9484671577912, + "heading": 0.5297243146446315, + "angularVelocity": 0.8588902770525219, + "velocityX": 3.0565879882309943, + "velocityY": 1.7050122496171904, + "timestamp": 10.05766201276927 + }, + { + "x": 7.388081081229855, + "y": 5.024967733696522, + "heading": 0.592130827533757, + "angularVelocity": 1.4353950379497669, + "velocityX": 2.8366949682733105, + "velocityY": 1.7595687047903361, + "timestamp": 10.101138903269229 + }, + { + "x": 7.501500129699707, + "y": 5.105462551116943, + "heading": 0.6747401864044066, + "angularVelocity": 1.9000751415451151, + "velocityX": 2.608720337761042, + "velocityY": 1.8514391552564737, + "timestamp": 10.144615793769187 + }, + { + "x": 7.646324439274632, + "y": 5.2181193934144225, + "heading": 0.716438811665056, + "angularVelocity": 0.6791497141368164, + "velocityX": 2.3587681328355696, + "velocityY": 1.8348533498079573, + "timestamp": 10.206014075168227 + }, + { + "x": 7.772039173775546, + "y": 5.317779255820348, + "heading": 0.7438971482542158, + "angularVelocity": 0.44721669668085284, + "velocityX": 2.047528556766434, + "velocityY": 1.62317022781495, + "timestamp": 10.267412356567267 + }, + { + "x": 7.878893815894361, + "y": 5.4039493449354215, + "heading": 0.7593796608518738, + "angularVelocity": 0.2521652437962253, + "velocityX": 1.7403523304560495, + "velocityY": 1.4034609300387069, + "timestamp": 10.328810637966306 + }, + { + "x": 7.966980879790036, + "y": 5.476461142089676, + "heading": 0.7636631361192312, + "angularVelocity": 0.06976539358680627, + "velocityX": 1.4346828915809557, + "velocityY": 1.1810069516927688, + "timestamp": 10.390208919365346 + }, + { + "x": 8.036348554918035, + "y": 5.535229545877986, + "heading": 0.7571399208631822, + "angularVelocity": -0.10624426461798567, + "velocityX": 1.1297983192260301, + "velocityY": 0.9571669181807075, + "timestamp": 10.451607200764386 + }, + { + "x": 8.087026476479373, + "y": 5.580203129397342, + "heading": 0.7400467384743379, + "angularVelocity": -0.27839838509082054, + "velocityX": 0.8253964183780998, + "velocityY": 0.7324892895138765, + "timestamp": 10.513005482163425 + }, + { + "x": 8.119034784241132, + "y": 5.611347352514501, + "heading": 0.7125421599400041, + "angularVelocity": -0.44796984390452543, + "velocityX": 0.5213225359473886, + "velocityY": 0.5072491022142075, + "timestamp": 10.574403763562465 + }, + { + "x": 8.1323881149292, + "y": 5.628637313842773, + "heading": 0.6747401864044066, + "angularVelocity": -0.6156845545873708, + "velocityX": 0.2174870433470383, + "velocityY": 0.2816033435187223, + "timestamp": 10.635802044961505 + }, + { + "x": 8.125741105489272, + "y": 5.631444479838457, + "heading": 0.6235311710708692, + "angularVelocity": -0.7910339249344661, + "velocityX": -0.1026774276383878, + "velocityY": 0.04336274620875669, + "timestamp": 10.70053885803533 + }, + { + "x": 8.09835684603502, + "y": 5.618815221035742, + "heading": 0.5616094020000137, + "angularVelocity": -0.9565155609411891, + "velocityX": -0.42300907557840733, + "velocityY": -0.19508619907365696, + "timestamp": 10.765275671109155 + }, + { + "x": 8.050222803941077, + "y": 5.59073345676368, + "heading": 0.48977295422299394, + "angularVelocity": -1.1096692031332702, + "velocityX": -0.7435343170052635, + "velocityY": -0.4337835450756361, + "timestamp": 10.83001248418298 + }, + { + "x": 7.981324451485502, + "y": 5.547180062425158, + "heading": 0.40904421087349874, + "angularVelocity": -1.2470299280477726, + "velocityX": -1.0642839704976186, + "velocityY": -0.6727763118158692, + "timestamp": 10.894749297256805 + }, + { + "x": 7.891645067392915, + "y": 5.488132407343552, + "heading": 0.32078665814595236, + "angularVelocity": -1.3633286616518716, + "velocityX": -1.3852919202296423, + "velocityY": -0.9121186582704987, + "timestamp": 10.95948611033063 + }, + { + "x": 7.781166411701721, + "y": 5.413563935212295, + "heading": 0.22693931715370197, + "angularVelocity": -1.4496750231622744, + "velocityX": -1.7065816255922823, + "velocityY": -1.1518712242788174, + "timestamp": 11.024222923404455 + }, + { + "x": 7.649874183396069, + "y": 5.323444723125603, + "heading": 0.13053857941250757, + "angularVelocity": -1.4891177548585681, + "velocityX": -2.0280922410549644, + "velocityY": -1.3920860142423153, + "timestamp": 11.08895973647828 + }, + { + "x": 7.497788734174063, + "y": 5.217751261949818, + "heading": 0.0370488934612777, + "angularVelocity": -1.4441502680185108, + "velocityX": -2.3492884805522913, + "velocityY": -1.6326639535877772, + "timestamp": 11.153696549552105 + }, + { + "x": 7.325168258315647, + "y": 5.096564905445784, + "heading": -0.04136336755772595, + "angularVelocity": -1.2112468516110346, + "velocityX": -2.66649635133507, + "velocityY": -1.871985208259067, + "timestamp": 11.21843336262593 + }, + { + "x": 7.133849861870766, + "y": 4.963599555351033, + "heading": -0.06503952290078549, + "angularVelocity": -0.36572939288900796, + "velocityX": -2.955326148457454, + "velocityY": -2.0539372233711526, + "timestamp": 11.283170175699755 + }, + { + "x": 6.940221298800236, + "y": 4.819868100403324, + "heading": -0.06503954882158174, + "angularVelocity": -4.004027232662301e-7, + "velocityX": -2.9910116651821412, + "velocityY": -2.2202429826719707, + "timestamp": 11.34790698877358 + }, + { + "x": 6.740356177089726, + "y": 4.684943363475215, + "heading": -0.06503957483267325, + "angularVelocity": -4.017975287461778e-7, + "velocityX": -3.08734879306747, + "velocityY": -2.0842041880288593, + "timestamp": 11.412643801847405 + }, + { + "x": 6.527131993072097, + "y": 4.57231033799875, + "heading": -0.06503960182577956, + "angularVelocity": -4.1696686974459996e-7, + "velocityX": -3.293708384663109, + "velocityY": -1.739860523378229, + "timestamp": 11.47738061492123 + }, + { + "x": 6.303024971058329, + "y": 4.483277800206398, + "heading": -0.06503963136622377, + "angularVelocity": -4.5631600956130875e-7, + "velocityX": -3.461817339667266, + "velocityY": -1.3752999810296351, + "timestamp": 11.542117427995056 + }, + { + "x": 6.070638079320141, + "y": 4.41887996664659, + "heading": -0.06503966569966328, + "angularVelocity": -5.303541813322711e-7, + "velocityX": -3.5897178236619824, + "velocityY": -0.9947637287360487, + "timestamp": 11.60685424106888 + }, + { + "x": 5.832670509915108, + "y": 4.379864860722445, + "heading": -0.06503973197653827, + "angularVelocity": -0.0000010237895848927841, + "velocityX": -3.67592345229684, + "velocityY": -0.60267263820436, + "timestamp": 11.671591054142706 + }, + { + "x": 5.593035616597729, + "y": 4.366766567771227, + "heading": -0.0691739587008402, + "angularVelocity": -0.06386206746982208, + "velocityX": -3.7016788738750415, + "velocityY": -0.20233144526718327, + "timestamp": 11.73632786721653 + }, + { + "x": 5.359733281234782, + "y": 4.378764088602827, + "heading": -0.08321570293446177, + "angularVelocity": -0.2169050894984338, + "velocityX": -3.6038588290852247, + "velocityY": 0.1853276406720581, + "timestamp": 11.801064680290356 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.21683446699990425, + "velocityX": -3.419080202788392, + "velocityY": 0.5417320490954629, + "timestamp": 11.86580149336418 + }, + { + "x": 4.905765298437056, + "y": 4.481387310799775, + "heading": -0.10673913036260185, + "angularVelocity": -0.12897497729525575, + "velocityX": -3.1628016184093535, + "velocityY": 0.9184524777738038, + "timestamp": 11.939352621411974 + }, + { + "x": 4.700482894835419, + "y": 4.568326227070152, + "heading": -0.11059429780349972, + "angularVelocity": -0.05241479693408341, + "velocityX": -2.791016386155829, + "velocityY": 1.1820201617286428, + "timestamp": 12.012903749459767 + }, + { + "x": 4.527923671898321, + "y": 4.662591020242044, + "heading": -0.11045427798457029, + "angularVelocity": 0.0019037072937678628, + "velocityX": -2.3461125276687587, + "velocityY": 1.2816226708397902, + "timestamp": 12.08645487750756 + }, + { + "x": 4.3886983023339585, + "y": 4.75311454797141, + "heading": -0.10805031221492864, + "angularVelocity": 0.03268428144405263, + "velocityX": -1.8929059724807213, + "velocityY": 1.2307564837148823, + "timestamp": 12.160006005555353 + }, + { + "x": 4.281048300464299, + "y": 4.832306401053517, + "heading": -0.1047189693449996, + "angularVelocity": 0.04529288616436113, + "velocityX": -1.4636077613889036, + "velocityY": 1.076691210373392, + "timestamp": 12.233557133603146 + }, + { + "x": 4.20282348849849, + "y": 4.895385445772344, + "heading": -0.10139996108706777, + "angularVelocity": 0.04512518497031254, + "velocityX": -1.0635433343045229, + "velocityY": 0.8576217169346194, + "timestamp": 12.307108261650939 + }, + { + "x": 4.15212537277285, + "y": 4.939288518901389, + "heading": -0.09875289338109836, + "angularVelocity": 0.0359894916125467, + "velocityX": -0.6892907977250395, + "velocityY": 0.5969055036180668, + "timestamp": 12.380659389698732 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 0.020394222744298442, + "velocityX": -0.3361409221475862, + "velocityY": 0.3082648480890461, + "timestamp": 12.454210517746525 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.761901225942554e-27, + "velocityX": -1.9870172205028814e-26, + "velocityY": -2.1545999896560787e-26, + "timestamp": 12.527761645794318 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.387, + "y": 4.121134281158447, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 0.6923742371436292, + "isStopPoint": false, + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 1.397197380247905, + "isStopPoint": true, + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 2.4478166834517507, + "isStopPoint": false, + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 19 + }, + { + "timestamp": 3.4501275661438395, + "isStopPoint": false, + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 4.098537468980011, + "isStopPoint": false, + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 + }, + { + "timestamp": 4.6826517740867715, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 5.328604050295384, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 5.907129949231749, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 7.01387661283412, + "isStopPoint": false, + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 8.16948699062752, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 8.816150511641153, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 9.405508655269896, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 10.144615793769187, + "isStopPoint": false, + "x": 7.501500129699707, + "y": 5.105462551116943, + "heading": 0.6747401864044066, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 10.635802044961505, + "isStopPoint": false, + "x": 8.1323881149292, + "y": 5.628637313842773, + "heading": 0.6747401864044066, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 11.86580149336418, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 12.527761645794318, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 11 + ], + "type": "StopPoint" + }, + { + "scope": [ + 7 + ], + "type": "StopPoint" + }, + { + "scope": [ + 11 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/deploy/choreo/SourceLanePGFE.1.traj b/src/main/deploy/choreo/SourceLanePGFE.1.traj new file mode 100644 index 00000000..c2e1f5e0 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGFE.1.traj @@ -0,0 +1,230 @@ +{ + "samples": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": 1.1679855597934721e-27, + "angularVelocity": 5.433585185667626e-28, + "velocityX": 7.442598398670663e-27, + "velocityY": 2.3270251613663043e-26, + "timestamp": 0 + }, + { + "x": 0.4005104003556791, + "y": 4.115348381940823, + "heading": -0.015277315995294262, + "angularVelocity": -0.30891158633586036, + "velocityX": 0.2731840597648779, + "velocityY": -0.11699249437834791, + "timestamp": 0.04945530265311637 + }, + { + "x": 0.4275520628000622, + "y": 4.103759116150694, + "heading": -0.04541480648332661, + "angularVelocity": -0.6093884552566431, + "velocityX": 0.5467899495844878, + "velocityY": -0.23433818353953934, + "timestamp": 0.09891060530623275 + }, + { + "x": 0.4681490763693427, + "y": 4.086345867978677, + "heading": -0.08991129386234921, + "angularVelocity": -0.899731373420656, + "velocityX": 0.8208829264281612, + "velocityY": -0.35210073011087883, + "timestamp": 0.14836590795934912 + }, + { + "x": 0.5223311730580589, + "y": 4.063083084813811, + "heading": -0.14811250055399447, + "angularVelocity": -1.176844616640472, + "velocityX": 1.0955770924859953, + "velocityY": -0.47037995759592216, + "timestamp": 0.1978212106124655 + }, + { + "x": 0.5901369333154594, + "y": 4.033939247984653, + "heading": -0.2191152852440036, + "angularVelocity": -1.435696090832334, + "velocityX": 1.3710513659777948, + "velocityY": -0.5892965013999555, + "timestamp": 0.24727651326558187 + }, + { + "x": 0.6716171763310703, + "y": 3.9988769782713436, + "heading": -0.3016414776380596, + "angularVelocity": -1.6687026055204155, + "velocityX": 1.647553217642207, + "velocityY": -0.708968863445023, + "timestamp": 0.29673181591869824 + }, + { + "x": 0.7668367655706729, + "y": 3.9578537486428593, + "heading": -0.39387111054062807, + "angularVelocity": -1.8649088763944044, + "velocityX": 1.9253666266584337, + "velocityY": -0.8295011339072073, + "timestamp": 0.3461871185718146 + }, + { + "x": 0.8758711660113281, + "y": 3.91082124740519, + "heading": -0.49317423957558376, + "angularVelocity": -2.00793693916861, + "velocityX": 2.2047059585385944, + "velocityY": -0.9510102802839819, + "timestamp": 0.395642421224931 + }, + { + "x": 0.9987875307433454, + "y": 3.857723681335083, + "heading": -0.5954683580210642, + "angularVelocity": -2.068415578466479, + "velocityX": 2.4854031446165235, + "velocityY": -1.0736475811812884, + "timestamp": 0.44509772387804736 + }, + { + "x": 1.1355416361286799, + "y": 3.7985302060231096, + "heading": -0.6929936795383219, + "angularVelocity": -1.9719891757878503, + "velocityX": 2.765206116410594, + "velocityY": -1.1969085646318132, + "timestamp": 0.49455302653116373 + }, + { + "x": 1.2843691968945923, + "y": 3.733334425660418, + "heading": -0.76082235123736, + "angularVelocity": -1.3715146417118123, + "velocityX": 3.0093347483848483, + "velocityY": -1.3182768452551967, + "timestamp": 0.5440083291842801 + }, + { + "x": 1.4440686661947881, + "y": 3.662157825631026, + "heading": -0.7899998870158123, + "angularVelocity": -0.5899779035446591, + "velocityX": 3.2291677683248934, + "velocityY": -1.439210685427017, + "timestamp": 0.5934636318373965 + }, + { + "x": 1.613363599943702, + "y": 3.5895175490832267, + "heading": -0.789999943508809, + "angularVelocity": -0.0000011423041349866365, + "velocityX": 3.423190733183105, + "velocityY": -1.468806632471835, + "timestamp": 0.6429189344905129 + }, + { + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "angularVelocity": -0.0000011422676237323725, + "velocityX": 3.423194018614537, + "velocityY": -1.468798975473458, + "timestamp": 0.6923742371436292 + }, + { + "x": 2.0239723336710425, + "y": 3.4134441767348855, + "heading": -0.79, + "angularVelocity": -4.2462946476416713e-16, + "velocityX": 3.4237473592821535, + "velocityY": -1.467509622685751, + "timestamp": 0.7628565514540567 + }, + { + "x": 2.250495583067251, + "y": 3.3163502598603043, + "heading": -0.79, + "angularVelocity": 2.583795012505271e-16, + "velocityX": 3.2139019782824456, + "velocityY": -1.37756425600538, + "timestamp": 0.8333388657644842 + }, + { + "x": 2.4487034476982825, + "y": 3.23139307341843, + "heading": -0.79, + "angularVelocity": 2.1975738712772502e-16, + "velocityX": 2.812164534752054, + "velocityY": -1.2053688542021173, + "timestamp": 0.9038211800749117 + }, + { + "x": 2.6185959113515174, + "y": 3.1585726243584236, + "heading": -0.79, + "angularVelocity": 1.8378454574977131e-16, + "velocityX": 2.4104268611977178, + "velocityY": -1.0331733538044932, + "timestamp": 0.9743034943853393 + }, + { + "x": 2.760172968622745, + "y": 3.0978889149966715, + "heading": -0.79, + "angularVelocity": 1.432046748255306e-16, + "velocityX": 2.0086891109686786, + "velocityY": -0.8609778205420586, + "timestamp": 1.0447858086957669 + }, + { + "x": 2.8734346168098597, + "y": 3.049341946491368, + "heading": -0.79, + "angularVelocity": 9.739728624843172e-17, + "velocityX": 1.6069513224022827, + "velocityY": -0.6887822708472163, + "timestamp": 1.1152681230061945 + }, + { + "x": 2.9583808542915984, + "y": 3.0129317195374297, + "heading": -0.79, + "angularVelocity": 7.507925930031045e-17, + "velocityX": 1.2052135108334718, + "velocityY": -0.5165867112929291, + "timestamp": 1.185750437316622 + }, + { + "x": 3.0150116799871185, + "y": 2.9886582345981343, + "heading": -0.79, + "angularVelocity": 2.284084326875716e-17, + "velocityX": 0.8034756839297177, + "velocityY": -0.3443911451656786, + "timestamp": 1.2562327516270497 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 4.059583267317121e-17, + "velocityX": 0.40173784607243285, + "velocityY": -0.17219557434345442, + "timestamp": 1.3267150659374773 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -5.776011429095129e-29, + "velocityX": -2.265299874372296e-27, + "velocityY": 1.6360733128925837e-27, + "timestamp": 1.397197380247905 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGFE.2.traj b/src/main/deploy/choreo/SourceLanePGFE.2.traj new file mode 100644 index 00000000..7523865d --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGFE.2.traj @@ -0,0 +1,680 @@ +{ + "samples": [ + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -5.776011429095129e-29, + "velocityX": -2.265299874372296e-27, + "velocityY": 1.6360733128925837e-27, + "timestamp": 0 + }, + { + "x": 3.05997928479042, + "y": 2.9660593331101914, + "heading": -0.757416423789292, + "angularVelocity": 0.5582463314772621, + "velocityX": 0.2852978705745241, + "velocityY": -0.17924557403559996, + "timestamp": 0.05836773906688042 + }, + { + "x": 3.09346940981965, + "y": 2.9451270864719548, + "heading": -0.6941025427131016, + "angularVelocity": 1.0847410245519795, + "velocityX": 0.5737780075883255, + "velocityY": -0.35862699108923296, + "timestamp": 0.11673547813376084 + }, + { + "x": 3.144014292961821, + "y": 2.9136974289532866, + "heading": -0.6025701359612305, + "angularVelocity": 1.5682020276130446, + "velocityX": 0.8659729492734705, + "velocityY": -0.5384765286634581, + "timestamp": 0.17510321720064126 + }, + { + "x": 3.211860363047214, + "y": 2.8716921302721943, + "heading": -0.4864204343113474, + "angularVelocity": 1.9899640367565647, + "velocityX": 1.1623898950009328, + "velocityY": -0.7196663662603233, + "timestamp": 0.23347095626752168 + }, + { + "x": 3.2972613235718167, + "y": 2.8189697938754956, + "heading": -0.350515523406248, + "angularVelocity": 2.3284251382321566, + "velocityX": 1.463153479814365, + "velocityY": -0.903278715940788, + "timestamp": 0.2918386953344021 + }, + { + "x": 3.4004578303174937, + "y": 2.7553674746788306, + "heading": -0.2010210241746004, + "angularVelocity": 2.5612521852242733, + "velocityX": 1.7680401604631244, + "velocityY": -1.0896827633461483, + "timestamp": 0.3502064344012825 + }, + { + "x": 3.521660389826237, + "y": 2.6807799872839095, + "heading": -0.04666004939323709, + "angularVelocity": 2.644628304078894, + "velocityX": 2.0765333974965823, + "velocityY": -1.2778889260976047, + "timestamp": 0.40857417346816294 + }, + { + "x": 3.6607588635338284, + "y": 2.5953087461465145, + "heading": 0.09647781625083168, + "angularVelocity": 2.4523455582210394, + "velocityX": 2.383139657820331, + "velocityY": -1.4643575801258646, + "timestamp": 0.46694191253504336 + }, + { + "x": 3.8159145666935705, + "y": 2.5005609897698613, + "heading": 0.19948059626332554, + "angularVelocity": 1.7647210883818645, + "velocityX": 2.6582441883170693, + "velocityY": -1.6232898154250444, + "timestamp": 0.5253096516019238 + }, + { + "x": 3.9841103579797825, + "y": 2.394344369414975, + "heading": 0.2530307730268158, + "angularVelocity": 0.9174618996656708, + "velocityX": 2.8816567846406667, + "velocityY": -1.8197830180329346, + "timestamp": 0.5836773906688042 + }, + { + "x": 4.165386797993937, + "y": 2.276764205392623, + "heading": 0.2569441402014935, + "angularVelocity": 0.06704674940712647, + "velocityX": 3.1057642956915044, + "velocityY": -2.014471793872694, + "timestamp": 0.6420451297356844 + }, + { + "x": 4.350490054886, + "y": 2.162710813518831, + "heading": 0.2569442210228683, + "angularVelocity": 0.0000013846925729023855, + "velocityX": 3.171328200325918, + "velocityY": -1.9540484811841776, + "timestamp": 0.7004128688025646 + }, + { + "x": 4.53559341768799, + "y": 2.048657593531974, + "heading": 0.2569443018441724, + "angularVelocity": 0.0000013846913619086116, + "velocityX": 3.171330014854447, + "velocityY": -1.9540455362879443, + "timestamp": 0.7587806078694448 + }, + { + "x": 4.720696780504463, + "y": 1.934604373568621, + "heading": 0.25694438266547676, + "angularVelocity": 0.0000013846913668711097, + "velocityX": 3.1713300151025647, + "velocityY": -1.9540455358852609, + "timestamp": 0.817148346936325 + }, + { + "x": 4.9058004208396335, + "y": 1.8205516040071454, + "heading": 0.25694446348677025, + "angularVelocity": 0.0000013846911809077847, + "velocityX": 3.1713347697615966, + "velocityY": -1.9540378192615708, + "timestamp": 0.8755160860032052 + }, + { + "x": 5.096209165426888, + "y": 1.7155949376896078, + "heading": 0.25694454460803495, + "angularVelocity": 0.0000013898305117986064, + "velocityX": 3.2622258054072626, + "velocityY": -1.7981965379415077, + "timestamp": 0.9338838250700854 + }, + { + "x": 5.295905298565607, + "y": 1.629613278960399, + "heading": 0.25694462919552075, + "angularVelocity": 0.0000014492164191385246, + "velocityX": 3.4213443304682105, + "velocityY": -1.4731024381582958, + "timestamp": 0.9922515641369656 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.2569447220794932, + "angularVelocity": 0.0000015913580676492416, + "velocityX": 3.5481637974123346, + "velocityY": -1.1340904342963076, + "timestamp": 1.0506193032038458 + }, + { + "x": 5.694707875531767, + "y": 1.52024537146014, + "heading": 0.25694480525283153, + "angularVelocity": 0.0000015766499748638697, + "velocityX": 3.6339835774202793, + "velocityY": -0.8184074026053739, + "timestamp": 1.103372507556061 + }, + { + "x": 5.889460824583239, + "y": 1.4940579532693374, + "heading": 0.25694488397529436, + "angularVelocity": 0.0000014922783143570964, + "velocityX": 3.6917747735506756, + "velocityY": -0.49641379158615856, + "timestamp": 1.1561257119082762 + }, + { + "x": 6.0857603375931175, + "y": 1.485058307103912, + "heading": 0.2569449614499782, + "angularVelocity": 0.0000014686251720162004, + "velocityX": 3.7210917406884763, + "velocityY": -0.17059904276786395, + "timestamp": 1.2088789162604914 + }, + { + "x": 6.282233415675428, + "y": 1.4886391007138935, + "heading": 0.2569450396471809, + "angularVelocity": 0.0000014823213805790595, + "velocityX": 3.724381873952661, + "velocityY": 0.06787821998591004, + "timestamp": 1.2616321206127066 + }, + { + "x": 6.478706465106145, + "y": 1.492221466050733, + "heading": 0.25694511784454455, + "angularVelocity": 0.0000014823244316657533, + "velocityX": 3.724381330827469, + "velocityY": 0.06790801394586561, + "timestamp": 1.3143853249649218 + }, + { + "x": 6.675014894210808, + "y": 1.5010244818845797, + "heading": 0.2569451969790386, + "angularVelocity": 0.0000015000888575462877, + "velocityX": 3.7212607559150137, + "velocityY": 0.16687167996605787, + "timestamp": 1.367138529317137 + }, + { + "x": 6.869794082781554, + "y": 1.5270160132210213, + "heading": 0.25694528055287635, + "angularVelocity": 0.0000015842419202847933, + "velocityX": 3.6922721749805327, + "velocityY": 0.4927005222830628, + "timestamp": 1.4198917336693522 + }, + { + "x": 7.061541469504376, + "y": 1.5699966670379106, + "heading": 0.256946313055063, + "angularVelocity": 0.000019572312228011845, + "velocityX": 3.6348007495922103, + "velocityY": 0.8147496316986196, + "timestamp": 1.4726449380215674 + }, + { + "x": 7.242025004547978, + "y": 1.6235443273244181, + "heading": 0.2892009263238711, + "angularVelocity": 0.6114247213013846, + "velocityX": 3.4212809868112584, + "velocityY": 1.0150598611790123, + "timestamp": 1.5253981423737826 + }, + { + "x": 7.410147525588679, + "y": 1.6859098332103064, + "heading": 0.355218131661103, + "angularVelocity": 1.2514349819673058, + "velocityX": 3.1869632016702596, + "velocityY": 1.1822126570643, + "timestamp": 1.5781513467259978 + }, + { + "x": 7.565027902002143, + "y": 1.7562974315327362, + "heading": 0.45280543116483274, + "angularVelocity": 1.8498838260548631, + "velocityX": 2.935942533071192, + "velocityY": 1.3342810012539876, + "timestamp": 1.630904551078213 + }, + { + "x": 7.703780357583263, + "y": 1.8320280138994296, + "heading": 0.5628377984662216, + "angularVelocity": 2.0857949512742477, + "velocityX": 2.630218529564891, + "velocityY": 1.4355636457847423, + "timestamp": 1.6836577554304282 + }, + { + "x": 7.825662907370359, + "y": 1.9113228263820174, + "heading": 0.6698617562917842, + "angularVelocity": 2.028766956240237, + "velocityX": 2.3104293148398787, + "velocityY": 1.503127884955833, + "timestamp": 1.7364109597826434 + }, + { + "x": 7.930693514616472, + "y": 1.993224495332611, + "heading": 0.7659443680915856, + "angularVelocity": 1.821360673340157, + "velocityX": 1.990980615032594, + "velocityY": 1.552543963088276, + "timestamp": 1.7891641641348586 + }, + { + "x": 8.019011714129224, + "y": 2.0771819587983416, + "heading": 0.8464048765676822, + "angularVelocity": 1.525225044888065, + "velocityX": 1.6741769641722983, + "velocityY": 1.5915140036835516, + "timestamp": 1.8419173684870738 + }, + { + "x": 8.090768492192119, + "y": 2.1628615391485617, + "heading": 0.9080474489079001, + "angularVelocity": 1.1685085881921318, + "velocityX": 1.3602354386626168, + "velocityY": 1.6241587862258966, + "timestamp": 1.894670572839289 + }, + { + "x": 8.146101418716508, + "y": 2.2500495157540152, + "heading": 0.9485346126335614, + "angularVelocity": 0.7674825486494136, + "velocityX": 1.048901715144179, + "velocityY": 1.6527522389602969, + "timestamp": 1.9474237771915042 + }, + { + "x": 8.185122291410464, + "y": 2.3385982915715853, + "heading": 0.9662103754331748, + "angularVelocity": 0.33506519682858266, + "velocityX": 0.7396872507199079, + "velocityY": 1.6785478134439036, + "timestamp": 2.0001769815437194 + }, + { + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "angularVelocity": -0.11638132008630137, + "velocityX": 0.43192419429508555, + "velocityY": 1.702181892471887, + "timestamp": 2.0529301858959346 + }, + { + "x": 8.211423054828616, + "y": 2.5404723922535313, + "heading": 0.9162466453350178, + "angularVelocity": -0.6758725051109615, + "velocityX": 0.054215367724374734, + "velocityY": 1.7285138763843786, + "timestamp": 2.1177711761795517 + }, + { + "x": 8.190202888383553, + "y": 2.653847423099536, + "heading": 0.8391734249663445, + "angularVelocity": -1.188649649420099, + "velocityX": -0.32726468785015433, + "velocityY": 1.748508626257838, + "timestamp": 2.182612166463169 + }, + { + "x": 8.144008306424402, + "y": 2.7680190139904237, + "heading": 0.7319233890251923, + "angularVelocity": -1.654046853264217, + "velocityX": -0.7124286929779271, + "velocityY": 1.760793448580852, + "timestamp": 2.247453156746786 + }, + { + "x": 8.072604152106313, + "y": 2.8824539716658655, + "heading": 0.5977397407747723, + "angularVelocity": -2.06942626359492, + "velocityX": -1.101219367652527, + "velocityY": 1.764855181497056, + "timestamp": 2.312294147030403 + }, + { + "x": 7.975691382967057, + "y": 2.9965380091624785, + "heading": 0.44171220804306943, + "angularVelocity": -2.4063101450059983, + "velocityX": -1.4946219777853038, + "velocityY": 1.759443170093563, + "timestamp": 2.37713513731402 + }, + { + "x": 7.852920805151411, + "y": 3.109310419044201, + "heading": 0.2733111632769955, + "angularVelocity": -2.5971386931242324, + "velocityX": -1.893409975366543, + "velocityY": 1.7392147989790332, + "timestamp": 2.4419761275976373 + }, + { + "x": 7.704278478364198, + "y": 3.2190283422070105, + "heading": 0.10912005253045581, + "angularVelocity": -2.53221164618803, + "velocityX": -2.292412964963152, + "velocityY": 1.6921074567630605, + "timestamp": 2.5068171178812544 + }, + { + "x": 7.532421373794031, + "y": 3.3230579148672983, + "heading": -0.010926874392962598, + "angularVelocity": -1.8514048967840961, + "velocityX": -2.650439233245175, + "velocityY": 1.6043797635609574, + "timestamp": 2.5716581081648715 + }, + { + "x": 7.337707687800783, + "y": 3.422034456345691, + "heading": -0.0821100823564174, + "angularVelocity": -1.0978118571616002, + "velocityX": -3.002941274363059, + "velocityY": 1.5264501829084578, + "timestamp": 2.6364990984484886 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": -0.09725138680843894, + "angularVelocity": -0.23351439245133218, + "velocityX": -3.3462025777712125, + "velocityY": 1.46270429328826, + "timestamp": 2.7013400887321057 + }, + { + "x": 7.010606209659238, + "y": 3.566708155417018, + "heading": -0.09725146950362612, + "angularVelocity": -0.0000025483254840822735, + "velocityX": -3.3937655527961965, + "velocityY": 1.535571150717811, + "timestamp": 2.733790883460259 + }, + { + "x": 6.900475999603528, + "y": 3.6165390557870793, + "heading": -0.09725155219704651, + "angularVelocity": -0.000002548271039111256, + "velocityX": -3.3937600289389183, + "velocityY": 1.5355833589748464, + "timestamp": 2.7662416781884125 + }, + { + "x": 6.7903457895857455, + "y": 3.666369956240961, + "heading": -0.0972516348904687, + "angularVelocity": -0.000002548271094167607, + "velocityX": -3.3937600277701754, + "velocityY": 1.5355833615578496, + "timestamp": 2.798692472916566 + }, + { + "x": 6.680215579567971, + "y": 3.7162008566948606, + "heading": -0.09725171758389267, + "angularVelocity": -0.0000025482711490074116, + "velocityX": -3.393760027769923, + "velocityY": 1.535583361558393, + "timestamp": 2.8311432676447192 + }, + { + "x": 6.570085369550196, + "y": 3.7660317571487596, + "heading": -0.09725180027731847, + "angularVelocity": -0.0000025482712052155066, + "velocityX": -3.39376002776992, + "velocityY": 1.53558336155839, + "timestamp": 2.8635940623728726 + }, + { + "x": 6.459955159532422, + "y": 3.815862657602659, + "heading": -0.09725188297074605, + "angularVelocity": -0.000002548271260506862, + "velocityX": -3.3937600277699165, + "velocityY": 1.5355833615583867, + "timestamp": 2.896044857101026 + }, + { + "x": 6.349824949514647, + "y": 3.865693558056558, + "heading": -0.09725196566417542, + "angularVelocity": -0.0000025482713153661305, + "velocityX": -3.3937600277699125, + "velocityY": 1.5355833615583832, + "timestamp": 2.9284956518291794 + }, + { + "x": 6.239694739496873, + "y": 3.915524458510457, + "heading": -0.09725204835760656, + "angularVelocity": -0.000002548271370425602, + "velocityX": -3.3937600277699085, + "velocityY": 1.5355833615583803, + "timestamp": 2.9609464465573327 + }, + { + "x": 6.129564529479099, + "y": 3.965355358964356, + "heading": -0.09725213105103951, + "angularVelocity": -0.0000025482714255163693, + "velocityX": -3.3937600277699054, + "velocityY": 1.5355833615583767, + "timestamp": 2.993397241285486 + }, + { + "x": 6.019434319461326, + "y": 4.015186259418255, + "heading": -0.09725221374447428, + "angularVelocity": -0.000002548271481519915, + "velocityX": -3.3937600277699014, + "velocityY": 1.5355833615583734, + "timestamp": 3.0258480360136395 + }, + { + "x": 5.9093041094435526, + "y": 4.065017159872154, + "heading": -0.09725229643791083, + "angularVelocity": -0.000002548271536879473, + "velocityX": -3.3937600277698974, + "velocityY": 1.5355833615583703, + "timestamp": 3.058298830741793 + }, + { + "x": 5.799173899425779, + "y": 4.114848060326053, + "heading": -0.09725237913134915, + "angularVelocity": -0.000002548271591483022, + "velocityX": -3.3937600277698934, + "velocityY": 1.535583361558367, + "timestamp": 3.0907496254699462 + }, + { + "x": 5.689043689408005, + "y": 4.164678960779951, + "heading": -0.09725246182478926, + "angularVelocity": -0.0000025482716464166734, + "velocityX": -3.39376002776989, + "velocityY": 1.5355833615583636, + "timestamp": 3.1232004201980996 + }, + { + "x": 5.5789134793902315, + "y": 4.214509861233849, + "heading": -0.09725254451823119, + "angularVelocity": -0.000002548271702485751, + "velocityX": -3.3937600277698863, + "velocityY": 1.5355833615583605, + "timestamp": 3.155651214926253 + }, + { + "x": 5.468783269372458, + "y": 4.264340761687747, + "heading": -0.09725262721167492, + "angularVelocity": -0.0000025482717576806813, + "velocityX": -3.3937600277698823, + "velocityY": 1.5355833615583572, + "timestamp": 3.1881020096544064 + }, + { + "x": 5.3586530593546975, + "y": 4.314171662141674, + "heading": -0.09725270990512043, + "angularVelocity": -0.0000025482718127641656, + "velocityX": -3.3937600277694844, + "velocityY": 1.5355833615592254, + "timestamp": 3.2205528043825598 + }, + { + "x": 5.2485228493974105, + "y": 4.364002562729252, + "heading": -0.09725279259856771, + "angularVelocity": -0.0000025482718674912185, + "velocityX": -3.393760025905934, + "velocityY": 1.5355833656778057, + "timestamp": 3.253003599110713 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.0000025482898100852076, + "velocityX": -3.393751218037592, + "velocityY": 1.5356028315921753, + "timestamp": 3.2854543938388665 + }, + { + "x": 4.9137282295128415, + "y": 4.535640155229253, + "heading": -0.09725287529313088, + "angularVelocity": -7.435036019118785e-12, + "velocityX": -3.1302347498710454, + "velocityY": 1.6971138308958593, + "timestamp": 3.357226868973157 + }, + { + "x": 4.717146626097375, + "y": 4.642220517268037, + "heading": -0.097252875293223, + "angularVelocity": -1.283652180157178e-12, + "velocityX": -2.7389553313809003, + "velocityY": 1.4849754288028512, + "timestamp": 3.428999344107447 + }, + { + "x": 4.548648110952986, + "y": 4.733575136181824, + "heading": -0.09725287529315746, + "angularVelocity": 9.133130247777463e-13, + "velocityX": -2.347675969501115, + "velocityY": 1.2728364006237478, + "timestamp": 3.5007718192417374 + }, + { + "x": 4.408232682725308, + "y": 4.809703996992033, + "heading": -0.09725287529302883, + "angularVelocity": 1.7921853517363252e-12, + "velocityX": -1.9563966264916142, + "velocityY": 1.0606971637493035, + "timestamp": 3.5725442943760277 + }, + { + "x": 4.295900340737155, + "y": 4.870607092209374, + "heading": -0.09725287529288444, + "angularVelocity": 2.0118085530494987e-12, + "velocityX": -1.5651172929172832, + "velocityY": 0.8485578225272118, + "timestamp": 3.644316769510318 + }, + { + "x": 4.211651084582216, + "y": 4.916284417340276, + "heading": -0.09725287529275266, + "angularVelocity": 1.8359969573857423e-12, + "velocityX": -1.1738379650040625, + "velocityY": 0.636418418696543, + "timestamp": 3.7160892446446083 + }, + { + "x": 4.155484913989614, + "y": 4.946735969389024, + "heading": -0.09725287529265242, + "angularVelocity": 1.3966146295291797e-12, + "velocityX": -0.7825586408649183, + "velocityY": 0.42427897312682855, + "timestamp": 3.7878617197788986 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 7.688238334265391e-13, + "velocityX": -0.3912793194215448, + "velocityY": 0.21213949774351348, + "timestamp": 3.859634194913189 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 3.652934582681632e-29, + "velocityX": -2.1119125295164495e-27, + "velocityY": 7.525449232438864e-28, + "timestamp": 3.9314066700474792 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGFE.3.traj b/src/main/deploy/choreo/SourceLanePGFE.3.traj new file mode 100644 index 00000000..f20802e1 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGFE.3.traj @@ -0,0 +1,446 @@ +{ + "samples": [ + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 3.652934582681632e-29, + "velocityX": -2.1119125295164495e-27, + "velocityY": 7.525449232438864e-28, + "timestamp": 0 + }, + { + "x": 4.149345287192321, + "y": 4.948730090302555, + "heading": -0.09725287542061173, + "angularVelocity": -1.9914932727158263e-9, + "velocityX": 0.34136955009475556, + "velocityY": -0.2058419570123514, + "timestamp": 0.06428065543737382 + }, + { + "x": 4.193331867161711, + "y": 4.922433486987429, + "heading": -0.09725287565526207, + "angularVelocity": -3.650403639422262e-9, + "velocityX": 0.6842895373437039, + "velocityY": -0.4090904664272752, + "timestamp": 0.12856131087474765 + }, + { + "x": 4.259487364434434, + "y": 4.883286547964955, + "heading": -0.09725287596919821, + "angularVelocity": -4.883835485414618e-9, + "velocityX": 1.0291665015328901, + "velocityY": -0.6090003089749593, + "timestamp": 0.19284196631212147 + }, + { + "x": 4.347975400700828, + "y": 4.831575784808056, + "heading": -0.09725287632620158, + "angularVelocity": -5.553822878939374e-9, + "velocityX": 1.3765888923239986, + "velocityY": -0.8044529540816499, + "timestamp": 0.2571226217494953 + }, + { + "x": 4.45901715765165, + "y": 4.76770271955414, + "heading": -0.09725287667606394, + "angularVelocity": -5.442731629415375e-9, + "velocityX": 1.7274521579669766, + "velocityY": -0.9936592092802463, + "timestamp": 0.3214032771868691 + }, + { + "x": 4.592927185358519, + "y": 4.69226963680943, + "heading": -0.09725287694462321, + "angularVelocity": -4.1779173473054744e-9, + "velocityX": 2.083208809800217, + "velocityY": -1.1734958555019974, + "timestamp": 0.38568393262424294 + }, + { + "x": 4.750184246245984, + "y": 4.606276833638444, + "heading": -0.09725287701156488, + "angularVelocity": -1.0413967826345418e-9, + "velocityX": 2.446413463233511, + "velocityY": -1.337771100588195, + "timestamp": 0.44996458806161677 + }, + { + "x": 4.931581310093376, + "y": 4.511690334370361, + "heading": -0.0972528766497097, + "angularVelocity": 5.6293012279094655e-9, + "velocityX": 2.821954172886779, + "velocityY": -1.4714613381662665, + "timestamp": 0.5142452434989906 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 2.111229970452055e-8, + "velocityX": 3.2173227507078654, + "velocityY": -1.5223279648179473, + "timestamp": 0.5785258989363644 + }, + { + "x": 5.3988689559213965, + "y": 4.32613897098341, + "heading": -0.0972528699779729, + "angularVelocity": 7.203036405891024e-8, + "velocityX": 3.5302934161705415, + "velocityY": -1.1885528129676428, + "timestamp": 0.6523090098431892 + }, + { + "x": 5.668131488513907, + "y": 4.271040046607528, + "heading": -0.09725286670603908, + "angularVelocity": 4.4345294956463885e-8, + "velocityX": 3.6493789606207083, + "velocityY": -0.7467687889368456, + "timestamp": 0.726092120750014 + }, + { + "x": 5.942117714678702, + "y": 4.249366302589977, + "heading": -0.09725286355772216, + "angularVelocity": 4.266988599202702e-8, + "velocityX": 3.713400299843557, + "velocityY": -0.2937493926614278, + "timestamp": 0.7998752316568387 + }, + { + "x": 6.216382330790908, + "y": 4.231558335942029, + "heading": -0.09725286041527645, + "angularVelocity": 4.2590311894180227e-8, + "velocityX": 3.7171733848218165, + "velocityY": -0.2413555951908442, + "timestamp": 0.8736583425636635 + }, + { + "x": 6.490646951405068, + "y": 4.21375043862986, + "heading": -0.09725285727283077, + "angularVelocity": 4.259031148289501e-8, + "velocityX": 3.717173445837876, + "velocityY": -0.2413546554665762, + "timestamp": 0.9474414534704882 + }, + { + "x": 6.764911572019294, + "y": 4.195942541318692, + "heading": -0.09725285413038512, + "angularVelocity": 4.2590311325076504e-8, + "velocityX": 3.717173445838756, + "velocityY": -0.241354655453012, + "timestamp": 1.021224564377313 + }, + { + "x": 7.039176191804646, + "y": 4.178134631242246, + "heading": -0.09725285098790415, + "angularVelocity": 4.259078987217418e-8, + "velocityX": 3.7171734346048453, + "velocityY": -0.24135482846385062, + "timestamp": 1.0950076752841378 + }, + { + "x": 7.303151625715879, + "y": 4.160355738175748, + "heading": -0.07493794888540205, + "angularVelocity": 0.302439160239284, + "velocityX": 3.5777216583424787, + "velocityY": -0.24096155404655972, + "timestamp": 1.1687907861909625 + }, + { + "x": 7.533445930864737, + "y": 4.1447637615713475, + "heading": -0.055524777247044715, + "angularVelocity": 0.2631113191048932, + "velocityX": 3.1212333326481616, + "velocityY": -0.21132175660214245, + "timestamp": 1.2425738970977873 + }, + { + "x": 7.730059092320319, + "y": 4.131358730965977, + "heading": -0.0390137097405274, + "angularVelocity": 0.22377841356361194, + "velocityX": 2.664744804591801, + "velocityY": -0.18168155883665832, + "timestamp": 1.316357008004612 + }, + { + "x": 7.892991106532595, + "y": 4.120140669647355, + "heading": -0.025405081189306844, + "angularVelocity": 0.1844409700806171, + "velocityX": 2.20825622842103, + "velocityY": -0.15204104544723207, + "timestamp": 1.3901401189114369 + }, + { + "x": 8.02224197249394, + "y": 4.111109595697515, + "heading": -0.014699203415963246, + "angularVelocity": 0.1450993003922434, + "velocityX": 1.751767638593706, + "velocityY": -0.12240028698767993, + "timestamp": 1.4639232298182616 + }, + { + "x": 8.117811689845258, + "y": 4.104265522227424, + "heading": -0.006896323202665822, + "angularVelocity": 0.10575428600660502, + "velocityX": 1.2952790438994193, + "velocityY": -0.09275935083211777, + "timestamp": 1.5377063407250864 + }, + { + "x": 8.179700258175622, + "y": 4.099608457473234, + "heading": -0.0019965838975628653, + "angularVelocity": 0.06640732878951748, + "velocityX": 0.8387904436357877, + "velocityY": -0.06311830305000854, + "timestamp": 1.6114894516319112 + }, + { + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": -2.1241979189811956e-28, + "angularVelocity": 0.02706017505936011, + "velocityX": 0.3823018326887492, + "velocityY": -0.03347720903449786, + "timestamp": 1.685272562538736 + }, + { + "x": 8.200639571345327, + "y": 4.096943687462525, + "heading": -0.0010802899918473687, + "angularVelocity": -0.014022329834604143, + "velocityX": -0.09434112255025054, + "velocityY": -0.0025274615139553986, + "timestamp": 1.7623132543916293 + }, + { + "x": 8.15665056358688, + "y": 4.099133352364744, + "heading": -0.005325598680954133, + "angularVelocity": -0.05510475810903996, + "velocityX": -0.5709840695932878, + "velocityY": 0.02842218637392462, + "timestamp": 1.8393539462445228 + }, + { + "x": 8.075940653456616, + "y": 4.103707386175575, + "heading": -0.012735784669566238, + "angularVelocity": -0.09618535101893455, + "velocityX": -1.0476270161796544, + "velocityY": 0.05937166062274877, + "timestamp": 1.9163946380974162 + }, + { + "x": 7.958509840643352, + "y": 4.1106657697854265, + "heading": -0.023310616019513074, + "angularVelocity": -0.1372629333357902, + "velocityX": -1.5242699668052726, + "velocityY": 0.09032088682612197, + "timestamp": 1.9934353299503096 + }, + { + "x": 7.804358124937036, + "y": 4.120008478318512, + "heading": -0.037049836652876394, + "angularVelocity": -0.17833719172197895, + "velocityX": -2.0009129201573495, + "velocityY": 0.12126979013798225, + "timestamp": 2.070476021803203 + }, + { + "x": 7.613485507099167, + "y": 4.131735481105861, + "heading": -0.05395325351914194, + "angularVelocity": -0.21940894428287439, + "velocityX": -2.4775558636250903, + "velocityY": 0.15221829536189774, + "timestamp": 2.1475167136560964 + }, + { + "x": 7.385891991055361, + "y": 4.145846741676362, + "heading": -0.07402084166347252, + "angularVelocity": -0.2604803729261546, + "velocityX": -2.9541987561377083, + "velocityY": 0.18316632718520057, + "timestamp": 2.22455740550899 + }, + { + "x": 7.121577593711164, + "y": 4.16234221776091, + "heading": -0.09725284930983917, + "angularVelocity": -0.30155502355466174, + "velocityX": -3.430841429213758, + "velocityY": 0.21411381034903446, + "timestamp": 2.3015980973618833 + }, + { + "x": 6.835203978246458, + "y": 4.1809363249101, + "heading": -0.09725285259123628, + "angularVelocity": -4.259303804512275e-8, + "velocityX": -3.7171734647908923, + "velocityY": 0.24135436354458084, + "timestamp": 2.3786387892147767 + }, + { + "x": 6.5488303639215975, + "y": 4.199530449615618, + "heading": -0.09725285587256685, + "angularVelocity": -4.259217424526975e-8, + "velocityX": -3.7171734499955185, + "velocityY": 0.24135459142842275, + "timestamp": 2.45567948106767 + }, + { + "x": 6.262456749596831, + "y": 4.218124574322582, + "heading": -0.09725285915389743, + "angularVelocity": -4.259217438362485e-8, + "velocityX": -3.7171734499943, + "velocityY": 0.24135459144719548, + "timestamp": 2.5327201729205635 + }, + { + "x": 5.976083139919176, + "y": 4.236718770600948, + "heading": -0.09725286243522813, + "angularVelocity": -4.259217585749446e-8, + "velocityX": -3.717173389674077, + "velocityY": 0.24135552045497527, + "timestamp": 2.609760864773457 + }, + { + "x": 5.690024921889411, + "y": 4.259659658304737, + "heading": -0.09725286572349402, + "angularVelocity": -4.26821957688407e-8, + "velocityX": -3.71307955769639, + "velocityY": 0.2977762420357482, + "timestamp": 2.6868015566263495 + }, + { + "x": 5.4092552652272206, + "y": 4.319022844462833, + "heading": -0.09725286915389218, + "angularVelocity": -4.452709450605306e-8, + "velocityX": -3.644433219762762, + "velocityY": 0.7705432639604047, + "timestamp": 2.763842248479243 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -7.968133349338986e-8, + "velocityX": -3.5158347290283, + "velocityY": 1.2306645781352283, + "timestamp": 2.8408829403321363 + }, + { + "x": 4.909018653493503, + "y": 4.524275328875486, + "heading": -0.09725287687080804, + "angularVelocity": -2.1964896325716688e-8, + "velocityX": -3.192337867898693, + "velocityY": 1.5370761958403918, + "timestamp": 2.9127344426669843 + }, + { + "x": 4.71108590609518, + "y": 4.628682093882472, + "heading": -0.09725287720527089, + "angularVelocity": -4.654917845961093e-9, + "velocityX": -2.754747513502236, + "velocityY": 1.453090911313464, + "timestamp": 2.9845859450018324 + }, + { + "x": 4.54294597125426, + "y": 4.721368217002082, + "heading": -0.09725287703485493, + "angularVelocity": 2.3717800726027118e-9, + "velocityX": -2.3401032598781413, + "velocityY": 1.2899677822694209, + "timestamp": 3.0564374473366804 + }, + { + "x": 4.40369795049572, + "y": 4.80026148816612, + "heading": -0.09725287664966911, + "angularVelocity": 5.360859554294991e-9, + "velocityX": -1.937997344990855, + "velocityY": 1.0980044759032763, + "timestamp": 3.1282889496715285 + }, + { + "x": 4.292814205558175, + "y": 4.864307082758427, + "heading": -0.09725287620265413, + "angularVelocity": 6.221372875954885e-9, + "velocityX": -1.5432348849269109, + "velocityY": 0.8913605493429588, + "timestamp": 3.2001404520063765 + }, + { + "x": 4.209952469660844, + "y": 4.91286872561946, + "heading": -0.09725287578795568, + "angularVelocity": 5.7716044742121175e-9, + "velocityX": -1.1532359547775557, + "velocityY": 0.6758612037744439, + "timestamp": 3.2719919543412246 + }, + { + "x": 4.154873707750821, + "y": 4.945521497298995, + "heading": -0.09725287546928488, + "angularVelocity": 4.435130638682436e-9, + "velocityX": -0.7665638173206213, + "velocityY": 0.4544480020384788, + "timestamp": 3.3438434566760726 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 2.459066789540458e-9, + "velocityX": -0.38234244368231685, + "velocityY": 0.22880870103743692, + "timestamp": 3.4156949590109207 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.7012427908311302e-26, + "velocityX": 2.173382321303034e-25, + "velocityY": 2.8154271739064046e-25, + "timestamp": 3.4875464613457687 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGFE.4.traj b/src/main/deploy/choreo/SourceLanePGFE.4.traj new file mode 100644 index 00000000..a806f7df --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGFE.4.traj @@ -0,0 +1,572 @@ +{ + "samples": [ + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.7012427908311302e-26, + "velocityX": 2.173382321303034e-25, + "velocityY": 2.8154271739064046e-25, + "timestamp": 0 + }, + { + "x": 4.147908938552217, + "y": 4.9451046130456335, + "heading": -0.10362139740935014, + "angularVelocity": -0.09725274804530627, + "velocityX": 0.31316100417438686, + "velocityY": -0.2574227575741351, + "timestamp": 0.06548423818097149 + }, + { + "x": 4.189500528478716, + "y": 4.912093615670984, + "heading": -0.11521450306933155, + "angularVelocity": -0.17703658135172537, + "velocityX": 0.6351389446045882, + "velocityY": -0.5041060000334822, + "timestamp": 0.13096847636194298 + }, + { + "x": 4.2528908246594135, + "y": 4.863879621749849, + "heading": -0.13053557559089707, + "angularVelocity": -0.23396580531675798, + "velocityX": 0.9680237251216575, + "velocityY": -0.7362686848076517, + "timestamp": 0.19645271454291446 + }, + { + "x": 4.338972089495466, + "y": 4.801805387831604, + "heading": -0.1475645296449939, + "angularVelocity": -0.2600466085752675, + "velocityX": 1.3145341112186142, + "velocityY": -0.9479263352914945, + "timestamp": 0.26193695272388595 + }, + { + "x": 4.4488531713485, + "y": 4.727871318118375, + "heading": -0.16346474025801716, + "angularVelocity": -0.24280973642972667, + "velocityX": 1.677977554680654, + "velocityY": -1.1290361126124002, + "timestamp": 0.32742119090485744 + }, + { + "x": 4.583832471873963, + "y": 4.645249641206774, + "heading": -0.17403705358129207, + "angularVelocity": -0.1614482143635436, + "velocityX": 2.0612486954866247, + "velocityY": -1.26170326183331, + "timestamp": 0.39290542908582893 + }, + { + "x": 4.74502129849887, + "y": 4.559256593692149, + "heading": -0.17264089042553551, + "angularVelocity": 0.021320598582793455, + "velocityX": 2.4614904456771134, + "velocityY": -1.3131869577069004, + "timestamp": 0.4583896672668004 + }, + { + "x": 4.931721031095368, + "y": 4.478536439161179, + "heading": -0.14954301236675954, + "angularVelocity": 0.3527242386930233, + "velocityX": 2.8510636724601572, + "velocityY": -1.2326653981663842, + "timestamp": 0.5238739054477719 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 0.7985148568065102, + "velocityX": 3.1560555624992586, + "velocityY": -0.9880598134339889, + "timestamp": 0.5893581436287434 + }, + { + "x": 5.280245938471686, + "y": 4.380962564632748, + "heading": -0.04650808406049355, + "angularVelocity": 1.167166985692144, + "velocityX": 3.2627221399232984, + "velocityY": -0.7560690286372685, + "timestamp": 0.6328350341287017 + }, + { + "x": 5.424678195182071, + "y": 4.359487062895353, + "heading": 0.008191327783648325, + "angularVelocity": 1.2581261266647945, + "velocityX": 3.322046610268118, + "velocityY": -0.493952108590108, + "timestamp": 0.67631192462866 + }, + { + "x": 5.570537921062946, + "y": 4.349646196769418, + "heading": 0.06315884602952881, + "angularVelocity": 1.264292768268082, + "velocityX": 3.354879436030857, + "velocityY": -0.22634705501637128, + "timestamp": 0.7197888151286183 + }, + { + "x": 5.717103514016266, + "y": 4.35150566579831, + "heading": 0.11770080768702691, + "angularVelocity": 1.2545046582287314, + "velocityX": 3.3711148904142765, + "velocityY": 0.04276913568356193, + "timestamp": 0.7632657056285765 + }, + { + "x": 5.863572420206036, + "y": 4.365085009600068, + "heading": 0.17138870164708653, + "angularVelocity": 1.2348604820326599, + "velocityX": 3.368891024759707, + "velocityY": 0.3123347517635909, + "timestamp": 0.8067425961285348 + }, + { + "x": 6.009034373006399, + "y": 4.3903410665362745, + "heading": 0.22417378599549884, + "angularVelocity": 1.2140952064744144, + "velocityX": 3.345730366814133, + "velocityY": 0.5809076188700988, + "timestamp": 0.8502194866284931 + }, + { + "x": 6.152424125530893, + "y": 4.427134155183534, + "heading": 0.2765912547304818, + "angularVelocity": 1.205639780863199, + "velocityX": 3.2980682582309235, + "velocityY": 0.8462677119766714, + "timestamp": 0.8936963771284514 + }, + { + "x": 6.292378614943397, + "y": 4.475129707153334, + "heading": 0.330369231652926, + "angularVelocity": 1.2369324554730028, + "velocityX": 3.2190547162667764, + "velocityY": 1.1039324896026255, + "timestamp": 0.9371732676284097 + }, + { + "x": 6.427989965818357, + "y": 4.530754098625934, + "heading": 0.3892449871584701, + "angularVelocity": 1.3541850585105815, + "velocityX": 3.1191593813520333, + "velocityY": 1.2794013286818071, + "timestamp": 0.980650158128368 + }, + { + "x": 6.568195373885816, + "y": 4.593631496863788, + "heading": 0.42157526724387395, + "angularVelocity": 0.743619879748183, + "velocityX": 3.224826027233803, + "velocityY": 1.4462257423381046, + "timestamp": 1.0241270486283263 + }, + { + "x": 6.710207768570418, + "y": 4.660649558034501, + "heading": 0.440928265304377, + "angularVelocity": 0.44513298531599427, + "velocityX": 3.26638802940008, + "velocityY": 1.5414639915606807, + "timestamp": 1.0676039391282846 + }, + { + "x": 6.85219596425133, + "y": 4.730282709551517, + "heading": 0.4559517105455699, + "angularVelocity": 0.34555013176959587, + "velocityX": 3.265831434772203, + "velocityY": 1.6016129653311566, + "timestamp": 1.1110808296282428 + }, + { + "x": 6.992339110580002, + "y": 4.802100760534676, + "heading": 0.47358684784417715, + "angularVelocity": 0.4056209424320313, + "velocityX": 3.223393962105149, + "velocityY": 1.6518672369917733, + "timestamp": 1.1545577201282011 + }, + { + "x": 7.1318594634446475, + "y": 4.8743385269135056, + "heading": 0.49238243611774024, + "angularVelocity": 0.4323121561230585, + "velocityX": 3.2090692609394345, + "velocityY": 1.6615209953641499, + "timestamp": 1.1980346106281594 + }, + { + "x": 7.264750404712455, + "y": 4.9484671577912, + "heading": 0.5297243146446315, + "angularVelocity": 0.8588902770525219, + "velocityX": 3.0565879882309943, + "velocityY": 1.7050122496171904, + "timestamp": 1.2415115011281177 + }, + { + "x": 7.388081081229855, + "y": 5.024967733696522, + "heading": 0.592130827533757, + "angularVelocity": 1.4353950379497669, + "velocityX": 2.8366949682733105, + "velocityY": 1.7595687047903361, + "timestamp": 1.284988391628076 + }, + { + "x": 7.501500129699707, + "y": 5.105462551116943, + "heading": 0.6747401864044066, + "angularVelocity": 1.9000751415451151, + "velocityX": 2.608720337761042, + "velocityY": 1.8514391552564737, + "timestamp": 1.3284652821280343 + }, + { + "x": 7.646324439274632, + "y": 5.2181193934144225, + "heading": 0.716438811665056, + "angularVelocity": 0.6791497141368164, + "velocityX": 2.3587681328355696, + "velocityY": 1.8348533498079573, + "timestamp": 1.389863563527074 + }, + { + "x": 7.772039173775546, + "y": 5.317779255820348, + "heading": 0.7438971482542158, + "angularVelocity": 0.44721669668085284, + "velocityX": 2.047528556766434, + "velocityY": 1.62317022781495, + "timestamp": 1.4512618449261137 + }, + { + "x": 7.878893815894361, + "y": 5.4039493449354215, + "heading": 0.7593796608518738, + "angularVelocity": 0.2521652437962253, + "velocityX": 1.7403523304560495, + "velocityY": 1.4034609300387069, + "timestamp": 1.5126601263251533 + }, + { + "x": 7.966980879790036, + "y": 5.476461142089676, + "heading": 0.7636631361192312, + "angularVelocity": 0.06976539358680627, + "velocityX": 1.4346828915809557, + "velocityY": 1.1810069516927688, + "timestamp": 1.574058407724193 + }, + { + "x": 8.036348554918035, + "y": 5.535229545877986, + "heading": 0.7571399208631822, + "angularVelocity": -0.10624426461798567, + "velocityX": 1.1297983192260301, + "velocityY": 0.9571669181807075, + "timestamp": 1.6354566891232327 + }, + { + "x": 8.087026476479373, + "y": 5.580203129397342, + "heading": 0.7400467384743379, + "angularVelocity": -0.27839838509082054, + "velocityX": 0.8253964183780998, + "velocityY": 0.7324892895138765, + "timestamp": 1.6968549705222724 + }, + { + "x": 8.119034784241132, + "y": 5.611347352514501, + "heading": 0.7125421599400041, + "angularVelocity": -0.44796984390452543, + "velocityX": 0.5213225359473886, + "velocityY": 0.5072491022142075, + "timestamp": 1.758253251921312 + }, + { + "x": 8.1323881149292, + "y": 5.628637313842773, + "heading": 0.6747401864044066, + "angularVelocity": -0.6156845545873708, + "velocityX": 0.2174870433470383, + "velocityY": 0.2816033435187223, + "timestamp": 1.8196515333203518 + }, + { + "x": 8.125741105489272, + "y": 5.631444479838457, + "heading": 0.6235311710708692, + "angularVelocity": -0.7910339249344661, + "velocityX": -0.1026774276383878, + "velocityY": 0.04336274620875669, + "timestamp": 1.8843883463941768 + }, + { + "x": 8.09835684603502, + "y": 5.618815221035742, + "heading": 0.5616094020000137, + "angularVelocity": -0.9565155609411891, + "velocityX": -0.42300907557840733, + "velocityY": -0.19508619907365696, + "timestamp": 1.949125159468002 + }, + { + "x": 8.050222803941077, + "y": 5.59073345676368, + "heading": 0.48977295422299394, + "angularVelocity": -1.1096692031332702, + "velocityX": -0.7435343170052635, + "velocityY": -0.4337835450756361, + "timestamp": 2.013861972541827 + }, + { + "x": 7.981324451485502, + "y": 5.547180062425158, + "heading": 0.40904421087349874, + "angularVelocity": -1.2470299280477726, + "velocityX": -1.0642839704976186, + "velocityY": -0.6727763118158692, + "timestamp": 2.078598785615652 + }, + { + "x": 7.891645067392915, + "y": 5.488132407343552, + "heading": 0.32078665814595236, + "angularVelocity": -1.3633286616518716, + "velocityX": -1.3852919202296423, + "velocityY": -0.9121186582704987, + "timestamp": 2.143335598689477 + }, + { + "x": 7.781166411701721, + "y": 5.413563935212295, + "heading": 0.22693931715370197, + "angularVelocity": -1.4496750231622744, + "velocityX": -1.7065816255922823, + "velocityY": -1.1518712242788174, + "timestamp": 2.208072411763302 + }, + { + "x": 7.649874183396069, + "y": 5.323444723125603, + "heading": 0.13053857941250757, + "angularVelocity": -1.4891177548585681, + "velocityX": -2.0280922410549644, + "velocityY": -1.3920860142423153, + "timestamp": 2.272809224837127 + }, + { + "x": 7.497788734174063, + "y": 5.217751261949818, + "heading": 0.0370488934612777, + "angularVelocity": -1.4441502680185108, + "velocityX": -2.3492884805522913, + "velocityY": -1.6326639535877772, + "timestamp": 2.3375460379109523 + }, + { + "x": 7.325168258315647, + "y": 5.096564905445784, + "heading": -0.04136336755772595, + "angularVelocity": -1.2112468516110346, + "velocityX": -2.66649635133507, + "velocityY": -1.871985208259067, + "timestamp": 2.4022828509847773 + }, + { + "x": 7.133849861870766, + "y": 4.963599555351033, + "heading": -0.06503952290078549, + "angularVelocity": -0.36572939288900796, + "velocityX": -2.955326148457454, + "velocityY": -2.0539372233711526, + "timestamp": 2.4670196640586024 + }, + { + "x": 6.940221298800236, + "y": 4.819868100403324, + "heading": -0.06503954882158174, + "angularVelocity": -4.004027232662301e-7, + "velocityX": -2.9910116651821412, + "velocityY": -2.2202429826719707, + "timestamp": 2.5317564771324275 + }, + { + "x": 6.740356177089726, + "y": 4.684943363475215, + "heading": -0.06503957483267325, + "angularVelocity": -4.017975287461778e-7, + "velocityX": -3.08734879306747, + "velocityY": -2.0842041880288593, + "timestamp": 2.5964932902062525 + }, + { + "x": 6.527131993072097, + "y": 4.57231033799875, + "heading": -0.06503960182577956, + "angularVelocity": -4.1696686974459996e-7, + "velocityX": -3.293708384663109, + "velocityY": -1.739860523378229, + "timestamp": 2.6612301032800776 + }, + { + "x": 6.303024971058329, + "y": 4.483277800206398, + "heading": -0.06503963136622377, + "angularVelocity": -4.5631600956130875e-7, + "velocityX": -3.461817339667266, + "velocityY": -1.3752999810296351, + "timestamp": 2.7259669163539026 + }, + { + "x": 6.070638079320141, + "y": 4.41887996664659, + "heading": -0.06503966569966328, + "angularVelocity": -5.303541813322711e-7, + "velocityX": -3.5897178236619824, + "velocityY": -0.9947637287360487, + "timestamp": 2.7907037294277277 + }, + { + "x": 5.832670509915108, + "y": 4.379864860722445, + "heading": -0.06503973197653827, + "angularVelocity": -0.0000010237895848927841, + "velocityX": -3.67592345229684, + "velocityY": -0.60267263820436, + "timestamp": 2.8554405425015528 + }, + { + "x": 5.593035616597729, + "y": 4.366766567771227, + "heading": -0.0691739587008402, + "angularVelocity": -0.06386206746982208, + "velocityX": -3.7016788738750415, + "velocityY": -0.20233144526718327, + "timestamp": 2.920177355575378 + }, + { + "x": 5.359733281234782, + "y": 4.378764088602827, + "heading": -0.08321570293446177, + "angularVelocity": -0.2169050894984338, + "velocityX": -3.6038588290852247, + "velocityY": 0.1853276406720581, + "timestamp": 2.984914168649203 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.21683446699990425, + "velocityX": -3.419080202788392, + "velocityY": 0.5417320490954629, + "timestamp": 3.049650981723028 + }, + { + "x": 4.905765298437056, + "y": 4.481387310799775, + "heading": -0.10673913036260185, + "angularVelocity": -0.12897497729525575, + "velocityX": -3.1628016184093535, + "velocityY": 0.9184524777738038, + "timestamp": 3.123202109770821 + }, + { + "x": 4.700482894835419, + "y": 4.568326227070152, + "heading": -0.11059429780349972, + "angularVelocity": -0.05241479693408341, + "velocityX": -2.791016386155829, + "velocityY": 1.1820201617286428, + "timestamp": 3.196753237818614 + }, + { + "x": 4.527923671898321, + "y": 4.662591020242044, + "heading": -0.11045427798457029, + "angularVelocity": 0.0019037072937678628, + "velocityX": -2.3461125276687587, + "velocityY": 1.2816226708397902, + "timestamp": 3.270304365866407 + }, + { + "x": 4.3886983023339585, + "y": 4.75311454797141, + "heading": -0.10805031221492864, + "angularVelocity": 0.03268428144405263, + "velocityX": -1.8929059724807213, + "velocityY": 1.2307564837148823, + "timestamp": 3.3438554939142 + }, + { + "x": 4.281048300464299, + "y": 4.832306401053517, + "heading": -0.1047189693449996, + "angularVelocity": 0.04529288616436113, + "velocityX": -1.4636077613889036, + "velocityY": 1.076691210373392, + "timestamp": 3.4174066219619927 + }, + { + "x": 4.20282348849849, + "y": 4.895385445772344, + "heading": -0.10139996108706777, + "angularVelocity": 0.04512518497031254, + "velocityX": -1.0635433343045229, + "velocityY": 0.8576217169346194, + "timestamp": 3.4909577500097857 + }, + { + "x": 4.15212537277285, + "y": 4.939288518901389, + "heading": -0.09875289338109836, + "angularVelocity": 0.0359894916125467, + "velocityX": -0.6892907977250395, + "velocityY": 0.5969055036180668, + "timestamp": 3.5645088780575787 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 0.020394222744298442, + "velocityX": -0.3361409221475862, + "velocityY": 0.3082648480890461, + "timestamp": 3.6380600061053716 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.761901225942554e-27, + "velocityX": -1.9870172205028814e-26, + "velocityY": -2.1545999896560787e-26, + "timestamp": 3.7116111341531646 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGFE.traj b/src/main/deploy/choreo/SourceLanePGFE.traj new file mode 100644 index 00000000..355234d2 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGFE.traj @@ -0,0 +1,1886 @@ +{ + "samples": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": 1.1679855597934721e-27, + "angularVelocity": 5.433585185667626e-28, + "velocityX": 7.442598398670663e-27, + "velocityY": 2.3270251613663043e-26, + "timestamp": 0 + }, + { + "x": 0.4005104003556791, + "y": 4.115348381940823, + "heading": -0.015277315995294262, + "angularVelocity": -0.30891158633586036, + "velocityX": 0.2731840597648779, + "velocityY": -0.11699249437834791, + "timestamp": 0.04945530265311637 + }, + { + "x": 0.4275520628000622, + "y": 4.103759116150694, + "heading": -0.04541480648332661, + "angularVelocity": -0.6093884552566431, + "velocityX": 0.5467899495844878, + "velocityY": -0.23433818353953934, + "timestamp": 0.09891060530623275 + }, + { + "x": 0.4681490763693427, + "y": 4.086345867978677, + "heading": -0.08991129386234921, + "angularVelocity": -0.899731373420656, + "velocityX": 0.8208829264281612, + "velocityY": -0.35210073011087883, + "timestamp": 0.14836590795934912 + }, + { + "x": 0.5223311730580589, + "y": 4.063083084813811, + "heading": -0.14811250055399447, + "angularVelocity": -1.176844616640472, + "velocityX": 1.0955770924859953, + "velocityY": -0.47037995759592216, + "timestamp": 0.1978212106124655 + }, + { + "x": 0.5901369333154594, + "y": 4.033939247984653, + "heading": -0.2191152852440036, + "angularVelocity": -1.435696090832334, + "velocityX": 1.3710513659777948, + "velocityY": -0.5892965013999555, + "timestamp": 0.24727651326558187 + }, + { + "x": 0.6716171763310703, + "y": 3.9988769782713436, + "heading": -0.3016414776380596, + "angularVelocity": -1.6687026055204155, + "velocityX": 1.647553217642207, + "velocityY": -0.708968863445023, + "timestamp": 0.29673181591869824 + }, + { + "x": 0.7668367655706729, + "y": 3.9578537486428593, + "heading": -0.39387111054062807, + "angularVelocity": -1.8649088763944044, + "velocityX": 1.9253666266584337, + "velocityY": -0.8295011339072073, + "timestamp": 0.3461871185718146 + }, + { + "x": 0.8758711660113281, + "y": 3.91082124740519, + "heading": -0.49317423957558376, + "angularVelocity": -2.00793693916861, + "velocityX": 2.2047059585385944, + "velocityY": -0.9510102802839819, + "timestamp": 0.395642421224931 + }, + { + "x": 0.9987875307433454, + "y": 3.857723681335083, + "heading": -0.5954683580210642, + "angularVelocity": -2.068415578466479, + "velocityX": 2.4854031446165235, + "velocityY": -1.0736475811812884, + "timestamp": 0.44509772387804736 + }, + { + "x": 1.1355416361286799, + "y": 3.7985302060231096, + "heading": -0.6929936795383219, + "angularVelocity": -1.9719891757878503, + "velocityX": 2.765206116410594, + "velocityY": -1.1969085646318132, + "timestamp": 0.49455302653116373 + }, + { + "x": 1.2843691968945923, + "y": 3.733334425660418, + "heading": -0.76082235123736, + "angularVelocity": -1.3715146417118123, + "velocityX": 3.0093347483848483, + "velocityY": -1.3182768452551967, + "timestamp": 0.5440083291842801 + }, + { + "x": 1.4440686661947881, + "y": 3.662157825631026, + "heading": -0.7899998870158123, + "angularVelocity": -0.5899779035446591, + "velocityX": 3.2291677683248934, + "velocityY": -1.439210685427017, + "timestamp": 0.5934636318373965 + }, + { + "x": 1.613363599943702, + "y": 3.5895175490832267, + "heading": -0.789999943508809, + "angularVelocity": -0.0000011423041349866365, + "velocityX": 3.423190733183105, + "velocityY": -1.468806632471835, + "timestamp": 0.6429189344905129 + }, + { + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "angularVelocity": -0.0000011422676237323725, + "velocityX": 3.423194018614537, + "velocityY": -1.468798975473458, + "timestamp": 0.6923742371436292 + }, + { + "x": 2.0239723336710425, + "y": 3.4134441767348855, + "heading": -0.79, + "angularVelocity": -4.2462946476416713e-16, + "velocityX": 3.4237473592821535, + "velocityY": -1.467509622685751, + "timestamp": 0.7628565514540567 + }, + { + "x": 2.250495583067251, + "y": 3.3163502598603043, + "heading": -0.79, + "angularVelocity": 2.583795012505271e-16, + "velocityX": 3.2139019782824456, + "velocityY": -1.37756425600538, + "timestamp": 0.8333388657644842 + }, + { + "x": 2.4487034476982825, + "y": 3.23139307341843, + "heading": -0.79, + "angularVelocity": 2.1975738712772502e-16, + "velocityX": 2.812164534752054, + "velocityY": -1.2053688542021173, + "timestamp": 0.9038211800749117 + }, + { + "x": 2.6185959113515174, + "y": 3.1585726243584236, + "heading": -0.79, + "angularVelocity": 1.8378454574977131e-16, + "velocityX": 2.4104268611977178, + "velocityY": -1.0331733538044932, + "timestamp": 0.9743034943853393 + }, + { + "x": 2.760172968622745, + "y": 3.0978889149966715, + "heading": -0.79, + "angularVelocity": 1.432046748255306e-16, + "velocityX": 2.0086891109686786, + "velocityY": -0.8609778205420586, + "timestamp": 1.0447858086957669 + }, + { + "x": 2.8734346168098597, + "y": 3.049341946491368, + "heading": -0.79, + "angularVelocity": 9.739728624843172e-17, + "velocityX": 1.6069513224022827, + "velocityY": -0.6887822708472163, + "timestamp": 1.1152681230061945 + }, + { + "x": 2.9583808542915984, + "y": 3.0129317195374297, + "heading": -0.79, + "angularVelocity": 7.507925930031045e-17, + "velocityX": 1.2052135108334718, + "velocityY": -0.5165867112929291, + "timestamp": 1.185750437316622 + }, + { + "x": 3.0150116799871185, + "y": 2.9886582345981343, + "heading": -0.79, + "angularVelocity": 2.284084326875716e-17, + "velocityX": 0.8034756839297177, + "velocityY": -0.3443911451656786, + "timestamp": 1.2562327516270497 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 4.059583267317121e-17, + "velocityX": 0.40173784607243285, + "velocityY": -0.17219557434345442, + "timestamp": 1.3267150659374773 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -5.776011429095129e-29, + "velocityX": -2.265299874372296e-27, + "velocityY": 1.6360733128925837e-27, + "timestamp": 1.397197380247905 + }, + { + "x": 3.05997928479042, + "y": 2.9660593331101914, + "heading": -0.757416423789292, + "angularVelocity": 0.5582463314772621, + "velocityX": 0.2852978705745241, + "velocityY": -0.17924557403559996, + "timestamp": 1.4555651193147854 + }, + { + "x": 3.09346940981965, + "y": 2.9451270864719548, + "heading": -0.6941025427131016, + "angularVelocity": 1.0847410245519795, + "velocityX": 0.5737780075883255, + "velocityY": -0.35862699108923296, + "timestamp": 1.5139328583816658 + }, + { + "x": 3.144014292961821, + "y": 2.9136974289532866, + "heading": -0.6025701359612305, + "angularVelocity": 1.5682020276130446, + "velocityX": 0.8659729492734705, + "velocityY": -0.5384765286634581, + "timestamp": 1.5723005974485462 + }, + { + "x": 3.211860363047214, + "y": 2.8716921302721943, + "heading": -0.4864204343113474, + "angularVelocity": 1.9899640367565647, + "velocityX": 1.1623898950009328, + "velocityY": -0.7196663662603233, + "timestamp": 1.6306683365154266 + }, + { + "x": 3.2972613235718167, + "y": 2.8189697938754956, + "heading": -0.350515523406248, + "angularVelocity": 2.3284251382321566, + "velocityX": 1.463153479814365, + "velocityY": -0.903278715940788, + "timestamp": 1.689036075582307 + }, + { + "x": 3.4004578303174937, + "y": 2.7553674746788306, + "heading": -0.2010210241746004, + "angularVelocity": 2.5612521852242733, + "velocityX": 1.7680401604631244, + "velocityY": -1.0896827633461483, + "timestamp": 1.7474038146491875 + }, + { + "x": 3.521660389826237, + "y": 2.6807799872839095, + "heading": -0.04666004939323709, + "angularVelocity": 2.644628304078894, + "velocityX": 2.0765333974965823, + "velocityY": -1.2778889260976047, + "timestamp": 1.805771553716068 + }, + { + "x": 3.6607588635338284, + "y": 2.5953087461465145, + "heading": 0.09647781625083168, + "angularVelocity": 2.4523455582210394, + "velocityX": 2.383139657820331, + "velocityY": -1.4643575801258646, + "timestamp": 1.8641392927829483 + }, + { + "x": 3.8159145666935705, + "y": 2.5005609897698613, + "heading": 0.19948059626332554, + "angularVelocity": 1.7647210883818645, + "velocityX": 2.6582441883170693, + "velocityY": -1.6232898154250444, + "timestamp": 1.9225070318498287 + }, + { + "x": 3.9841103579797825, + "y": 2.394344369414975, + "heading": 0.2530307730268158, + "angularVelocity": 0.9174618996656708, + "velocityX": 2.8816567846406667, + "velocityY": -1.8197830180329346, + "timestamp": 1.9808747709167092 + }, + { + "x": 4.165386797993937, + "y": 2.276764205392623, + "heading": 0.2569441402014935, + "angularVelocity": 0.06704674940712647, + "velocityX": 3.1057642956915044, + "velocityY": -2.014471793872694, + "timestamp": 2.0392425099835894 + }, + { + "x": 4.350490054886, + "y": 2.162710813518831, + "heading": 0.2569442210228683, + "angularVelocity": 0.0000013846925729023855, + "velocityX": 3.171328200325918, + "velocityY": -1.9540484811841776, + "timestamp": 2.0976102490504696 + }, + { + "x": 4.53559341768799, + "y": 2.048657593531974, + "heading": 0.2569443018441724, + "angularVelocity": 0.0000013846913619086116, + "velocityX": 3.171330014854447, + "velocityY": -1.9540455362879443, + "timestamp": 2.1559779881173498 + }, + { + "x": 4.720696780504463, + "y": 1.934604373568621, + "heading": 0.25694438266547676, + "angularVelocity": 0.0000013846913668711097, + "velocityX": 3.1713300151025647, + "velocityY": -1.9540455358852609, + "timestamp": 2.21434572718423 + }, + { + "x": 4.9058004208396335, + "y": 1.8205516040071454, + "heading": 0.25694446348677025, + "angularVelocity": 0.0000013846911809077847, + "velocityX": 3.1713347697615966, + "velocityY": -1.9540378192615708, + "timestamp": 2.27271346625111 + }, + { + "x": 5.096209165426888, + "y": 1.7155949376896078, + "heading": 0.25694454460803495, + "angularVelocity": 0.0000013898305117986064, + "velocityX": 3.2622258054072626, + "velocityY": -1.7981965379415077, + "timestamp": 2.3310812053179903 + }, + { + "x": 5.295905298565607, + "y": 1.629613278960399, + "heading": 0.25694462919552075, + "angularVelocity": 0.0000014492164191385246, + "velocityX": 3.4213443304682105, + "velocityY": -1.4731024381582958, + "timestamp": 2.3894489443848705 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.2569447220794932, + "angularVelocity": 0.0000015913580676492416, + "velocityX": 3.5481637974123346, + "velocityY": -1.1340904342963076, + "timestamp": 2.4478166834517507 + }, + { + "x": 5.694707875531767, + "y": 1.52024537146014, + "heading": 0.25694480525283153, + "angularVelocity": 0.0000015766499748638697, + "velocityX": 3.6339835774202793, + "velocityY": -0.8184074026053739, + "timestamp": 2.500569887803966 + }, + { + "x": 5.889460824583239, + "y": 1.4940579532693374, + "heading": 0.25694488397529436, + "angularVelocity": 0.0000014922783143570964, + "velocityX": 3.6917747735506756, + "velocityY": -0.49641379158615856, + "timestamp": 2.553323092156181 + }, + { + "x": 6.0857603375931175, + "y": 1.485058307103912, + "heading": 0.2569449614499782, + "angularVelocity": 0.0000014686251720162004, + "velocityX": 3.7210917406884763, + "velocityY": -0.17059904276786395, + "timestamp": 2.6060762965083963 + }, + { + "x": 6.282233415675428, + "y": 1.4886391007138935, + "heading": 0.2569450396471809, + "angularVelocity": 0.0000014823213805790595, + "velocityX": 3.724381873952661, + "velocityY": 0.06787821998591004, + "timestamp": 2.6588295008606115 + }, + { + "x": 6.478706465106145, + "y": 1.492221466050733, + "heading": 0.25694511784454455, + "angularVelocity": 0.0000014823244316657533, + "velocityX": 3.724381330827469, + "velocityY": 0.06790801394586561, + "timestamp": 2.7115827052128267 + }, + { + "x": 6.675014894210808, + "y": 1.5010244818845797, + "heading": 0.2569451969790386, + "angularVelocity": 0.0000015000888575462877, + "velocityX": 3.7212607559150137, + "velocityY": 0.16687167996605787, + "timestamp": 2.764335909565042 + }, + { + "x": 6.869794082781554, + "y": 1.5270160132210213, + "heading": 0.25694528055287635, + "angularVelocity": 0.0000015842419202847933, + "velocityX": 3.6922721749805327, + "velocityY": 0.4927005222830628, + "timestamp": 2.817089113917257 + }, + { + "x": 7.061541469504376, + "y": 1.5699966670379106, + "heading": 0.256946313055063, + "angularVelocity": 0.000019572312228011845, + "velocityX": 3.6348007495922103, + "velocityY": 0.8147496316986196, + "timestamp": 2.8698423182694723 + }, + { + "x": 7.242025004547978, + "y": 1.6235443273244181, + "heading": 0.2892009263238711, + "angularVelocity": 0.6114247213013846, + "velocityX": 3.4212809868112584, + "velocityY": 1.0150598611790123, + "timestamp": 2.9225955226216875 + }, + { + "x": 7.410147525588679, + "y": 1.6859098332103064, + "heading": 0.355218131661103, + "angularVelocity": 1.2514349819673058, + "velocityX": 3.1869632016702596, + "velocityY": 1.1822126570643, + "timestamp": 2.9753487269739027 + }, + { + "x": 7.565027902002143, + "y": 1.7562974315327362, + "heading": 0.45280543116483274, + "angularVelocity": 1.8498838260548631, + "velocityX": 2.935942533071192, + "velocityY": 1.3342810012539876, + "timestamp": 3.028101931326118 + }, + { + "x": 7.703780357583263, + "y": 1.8320280138994296, + "heading": 0.5628377984662216, + "angularVelocity": 2.0857949512742477, + "velocityX": 2.630218529564891, + "velocityY": 1.4355636457847423, + "timestamp": 3.080855135678333 + }, + { + "x": 7.825662907370359, + "y": 1.9113228263820174, + "heading": 0.6698617562917842, + "angularVelocity": 2.028766956240237, + "velocityX": 2.3104293148398787, + "velocityY": 1.503127884955833, + "timestamp": 3.1336083400305483 + }, + { + "x": 7.930693514616472, + "y": 1.993224495332611, + "heading": 0.7659443680915856, + "angularVelocity": 1.821360673340157, + "velocityX": 1.990980615032594, + "velocityY": 1.552543963088276, + "timestamp": 3.1863615443827635 + }, + { + "x": 8.019011714129224, + "y": 2.0771819587983416, + "heading": 0.8464048765676822, + "angularVelocity": 1.525225044888065, + "velocityX": 1.6741769641722983, + "velocityY": 1.5915140036835516, + "timestamp": 3.2391147487349787 + }, + { + "x": 8.090768492192119, + "y": 2.1628615391485617, + "heading": 0.9080474489079001, + "angularVelocity": 1.1685085881921318, + "velocityX": 1.3602354386626168, + "velocityY": 1.6241587862258966, + "timestamp": 3.291867953087194 + }, + { + "x": 8.146101418716508, + "y": 2.2500495157540152, + "heading": 0.9485346126335614, + "angularVelocity": 0.7674825486494136, + "velocityX": 1.048901715144179, + "velocityY": 1.6527522389602969, + "timestamp": 3.344621157439409 + }, + { + "x": 8.185122291410464, + "y": 2.3385982915715853, + "heading": 0.9662103754331748, + "angularVelocity": 0.33506519682858266, + "velocityX": 0.7396872507199079, + "velocityY": 1.6785478134439036, + "timestamp": 3.3973743617916243 + }, + { + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "angularVelocity": -0.11638132008630137, + "velocityX": 0.43192419429508555, + "velocityY": 1.702181892471887, + "timestamp": 3.4501275661438395 + }, + { + "x": 8.211423054828616, + "y": 2.5404723922535313, + "heading": 0.9162466453350178, + "angularVelocity": -0.6758725051109615, + "velocityX": 0.054215367724374734, + "velocityY": 1.7285138763843786, + "timestamp": 3.5149685564274566 + }, + { + "x": 8.190202888383553, + "y": 2.653847423099536, + "heading": 0.8391734249663445, + "angularVelocity": -1.188649649420099, + "velocityX": -0.32726468785015433, + "velocityY": 1.748508626257838, + "timestamp": 3.5798095467110738 + }, + { + "x": 8.144008306424402, + "y": 2.7680190139904237, + "heading": 0.7319233890251923, + "angularVelocity": -1.654046853264217, + "velocityX": -0.7124286929779271, + "velocityY": 1.760793448580852, + "timestamp": 3.644650536994691 + }, + { + "x": 8.072604152106313, + "y": 2.8824539716658655, + "heading": 0.5977397407747723, + "angularVelocity": -2.06942626359492, + "velocityX": -1.101219367652527, + "velocityY": 1.764855181497056, + "timestamp": 3.709491527278308 + }, + { + "x": 7.975691382967057, + "y": 2.9965380091624785, + "heading": 0.44171220804306943, + "angularVelocity": -2.4063101450059983, + "velocityX": -1.4946219777853038, + "velocityY": 1.759443170093563, + "timestamp": 3.774332517561925 + }, + { + "x": 7.852920805151411, + "y": 3.109310419044201, + "heading": 0.2733111632769955, + "angularVelocity": -2.5971386931242324, + "velocityX": -1.893409975366543, + "velocityY": 1.7392147989790332, + "timestamp": 3.8391735078455422 + }, + { + "x": 7.704278478364198, + "y": 3.2190283422070105, + "heading": 0.10912005253045581, + "angularVelocity": -2.53221164618803, + "velocityX": -2.292412964963152, + "velocityY": 1.6921074567630605, + "timestamp": 3.9040144981291593 + }, + { + "x": 7.532421373794031, + "y": 3.3230579148672983, + "heading": -0.010926874392962598, + "angularVelocity": -1.8514048967840961, + "velocityX": -2.650439233245175, + "velocityY": 1.6043797635609574, + "timestamp": 3.9688554884127765 + }, + { + "x": 7.337707687800783, + "y": 3.422034456345691, + "heading": -0.0821100823564174, + "angularVelocity": -1.0978118571616002, + "velocityX": -3.002941274363059, + "velocityY": 1.5264501829084578, + "timestamp": 4.033696478696394 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": -0.09725138680843894, + "angularVelocity": -0.23351439245133218, + "velocityX": -3.3462025777712125, + "velocityY": 1.46270429328826, + "timestamp": 4.098537468980011 + }, + { + "x": 7.010606209659238, + "y": 3.566708155417018, + "heading": -0.09725146950362612, + "angularVelocity": -0.0000025483254840822735, + "velocityX": -3.3937655527961965, + "velocityY": 1.535571150717811, + "timestamp": 4.130988263708164 + }, + { + "x": 6.900475999603528, + "y": 3.6165390557870793, + "heading": -0.09725155219704651, + "angularVelocity": -0.000002548271039111256, + "velocityX": -3.3937600289389183, + "velocityY": 1.5355833589748464, + "timestamp": 4.1634390584363175 + }, + { + "x": 6.7903457895857455, + "y": 3.666369956240961, + "heading": -0.0972516348904687, + "angularVelocity": -0.000002548271094167607, + "velocityX": -3.3937600277701754, + "velocityY": 1.5355833615578496, + "timestamp": 4.195889853164471 + }, + { + "x": 6.680215579567971, + "y": 3.7162008566948606, + "heading": -0.09725171758389267, + "angularVelocity": -0.0000025482711490074116, + "velocityX": -3.393760027769923, + "velocityY": 1.535583361558393, + "timestamp": 4.228340647892624 + }, + { + "x": 6.570085369550196, + "y": 3.7660317571487596, + "heading": -0.09725180027731847, + "angularVelocity": -0.0000025482712052155066, + "velocityX": -3.39376002776992, + "velocityY": 1.53558336155839, + "timestamp": 4.260791442620778 + }, + { + "x": 6.459955159532422, + "y": 3.815862657602659, + "heading": -0.09725188297074605, + "angularVelocity": -0.000002548271260506862, + "velocityX": -3.3937600277699165, + "velocityY": 1.5355833615583867, + "timestamp": 4.293242237348931 + }, + { + "x": 6.349824949514647, + "y": 3.865693558056558, + "heading": -0.09725196566417542, + "angularVelocity": -0.0000025482713153661305, + "velocityX": -3.3937600277699125, + "velocityY": 1.5355833615583832, + "timestamp": 4.325693032077084 + }, + { + "x": 6.239694739496873, + "y": 3.915524458510457, + "heading": -0.09725204835760656, + "angularVelocity": -0.000002548271370425602, + "velocityX": -3.3937600277699085, + "velocityY": 1.5355833615583803, + "timestamp": 4.358143826805238 + }, + { + "x": 6.129564529479099, + "y": 3.965355358964356, + "heading": -0.09725213105103951, + "angularVelocity": -0.0000025482714255163693, + "velocityX": -3.3937600277699054, + "velocityY": 1.5355833615583767, + "timestamp": 4.390594621533391 + }, + { + "x": 6.019434319461326, + "y": 4.015186259418255, + "heading": -0.09725221374447428, + "angularVelocity": -0.000002548271481519915, + "velocityX": -3.3937600277699014, + "velocityY": 1.5355833615583734, + "timestamp": 4.4230454162615445 + }, + { + "x": 5.9093041094435526, + "y": 4.065017159872154, + "heading": -0.09725229643791083, + "angularVelocity": -0.000002548271536879473, + "velocityX": -3.3937600277698974, + "velocityY": 1.5355833615583703, + "timestamp": 4.455496210989698 + }, + { + "x": 5.799173899425779, + "y": 4.114848060326053, + "heading": -0.09725237913134915, + "angularVelocity": -0.000002548271591483022, + "velocityX": -3.3937600277698934, + "velocityY": 1.535583361558367, + "timestamp": 4.487947005717851 + }, + { + "x": 5.689043689408005, + "y": 4.164678960779951, + "heading": -0.09725246182478926, + "angularVelocity": -0.0000025482716464166734, + "velocityX": -3.39376002776989, + "velocityY": 1.5355833615583636, + "timestamp": 4.520397800446005 + }, + { + "x": 5.5789134793902315, + "y": 4.214509861233849, + "heading": -0.09725254451823119, + "angularVelocity": -0.000002548271702485751, + "velocityX": -3.3937600277698863, + "velocityY": 1.5355833615583605, + "timestamp": 4.552848595174158 + }, + { + "x": 5.468783269372458, + "y": 4.264340761687747, + "heading": -0.09725262721167492, + "angularVelocity": -0.0000025482717576806813, + "velocityX": -3.3937600277698823, + "velocityY": 1.5355833615583572, + "timestamp": 4.585299389902311 + }, + { + "x": 5.3586530593546975, + "y": 4.314171662141674, + "heading": -0.09725270990512043, + "angularVelocity": -0.0000025482718127641656, + "velocityX": -3.3937600277694844, + "velocityY": 1.5355833615592254, + "timestamp": 4.617750184630465 + }, + { + "x": 5.2485228493974105, + "y": 4.364002562729252, + "heading": -0.09725279259856771, + "angularVelocity": -0.0000025482718674912185, + "velocityX": -3.393760025905934, + "velocityY": 1.5355833656778057, + "timestamp": 4.650200979358618 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.0000025482898100852076, + "velocityX": -3.393751218037592, + "velocityY": 1.5356028315921753, + "timestamp": 4.6826517740867715 + }, + { + "x": 4.9137282295128415, + "y": 4.535640155229253, + "heading": -0.09725287529313088, + "angularVelocity": -7.435036019118785e-12, + "velocityX": -3.1302347498710454, + "velocityY": 1.6971138308958593, + "timestamp": 4.754424249221062 + }, + { + "x": 4.717146626097375, + "y": 4.642220517268037, + "heading": -0.097252875293223, + "angularVelocity": -1.283652180157178e-12, + "velocityX": -2.7389553313809003, + "velocityY": 1.4849754288028512, + "timestamp": 4.826196724355352 + }, + { + "x": 4.548648110952986, + "y": 4.733575136181824, + "heading": -0.09725287529315746, + "angularVelocity": 9.133130247777463e-13, + "velocityX": -2.347675969501115, + "velocityY": 1.2728364006237478, + "timestamp": 4.897969199489642 + }, + { + "x": 4.408232682725308, + "y": 4.809703996992033, + "heading": -0.09725287529302883, + "angularVelocity": 1.7921853517363252e-12, + "velocityX": -1.9563966264916142, + "velocityY": 1.0606971637493035, + "timestamp": 4.969741674623933 + }, + { + "x": 4.295900340737155, + "y": 4.870607092209374, + "heading": -0.09725287529288444, + "angularVelocity": 2.0118085530494987e-12, + "velocityX": -1.5651172929172832, + "velocityY": 0.8485578225272118, + "timestamp": 5.041514149758223 + }, + { + "x": 4.211651084582216, + "y": 4.916284417340276, + "heading": -0.09725287529275266, + "angularVelocity": 1.8359969573857423e-12, + "velocityX": -1.1738379650040625, + "velocityY": 0.636418418696543, + "timestamp": 5.113286624892513 + }, + { + "x": 4.155484913989614, + "y": 4.946735969389024, + "heading": -0.09725287529265242, + "angularVelocity": 1.3966146295291797e-12, + "velocityX": -0.7825586408649183, + "velocityY": 0.42427897312682855, + "timestamp": 5.185059100026804 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 7.688238334265391e-13, + "velocityX": -0.3912793194215448, + "velocityY": 0.21213949774351348, + "timestamp": 5.256831575161094 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 3.652934582681632e-29, + "velocityX": -2.1119125295164495e-27, + "velocityY": 7.525449232438864e-28, + "timestamp": 5.328604050295384 + }, + { + "x": 4.149345287192321, + "y": 4.948730090302555, + "heading": -0.09725287542061173, + "angularVelocity": -1.9914932727158263e-9, + "velocityX": 0.34136955009475556, + "velocityY": -0.2058419570123514, + "timestamp": 5.392884705732758 + }, + { + "x": 4.193331867161711, + "y": 4.922433486987429, + "heading": -0.09725287565526207, + "angularVelocity": -3.650403639422262e-9, + "velocityX": 0.6842895373437039, + "velocityY": -0.4090904664272752, + "timestamp": 5.457165361170132 + }, + { + "x": 4.259487364434434, + "y": 4.883286547964955, + "heading": -0.09725287596919821, + "angularVelocity": -4.883835485414618e-9, + "velocityX": 1.0291665015328901, + "velocityY": -0.6090003089749593, + "timestamp": 5.521446016607506 + }, + { + "x": 4.347975400700828, + "y": 4.831575784808056, + "heading": -0.09725287632620158, + "angularVelocity": -5.553822878939374e-9, + "velocityX": 1.3765888923239986, + "velocityY": -0.8044529540816499, + "timestamp": 5.5857266720448795 + }, + { + "x": 4.45901715765165, + "y": 4.76770271955414, + "heading": -0.09725287667606394, + "angularVelocity": -5.442731629415375e-9, + "velocityX": 1.7274521579669766, + "velocityY": -0.9936592092802463, + "timestamp": 5.650007327482253 + }, + { + "x": 4.592927185358519, + "y": 4.69226963680943, + "heading": -0.09725287694462321, + "angularVelocity": -4.1779173473054744e-9, + "velocityX": 2.083208809800217, + "velocityY": -1.1734958555019974, + "timestamp": 5.714287982919627 + }, + { + "x": 4.750184246245984, + "y": 4.606276833638444, + "heading": -0.09725287701156488, + "angularVelocity": -1.0413967826345418e-9, + "velocityX": 2.446413463233511, + "velocityY": -1.337771100588195, + "timestamp": 5.778568638357001 + }, + { + "x": 4.931581310093376, + "y": 4.511690334370361, + "heading": -0.0972528766497097, + "angularVelocity": 5.6293012279094655e-9, + "velocityX": 2.821954172886779, + "velocityY": -1.4714613381662665, + "timestamp": 5.842849293794375 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 2.111229970452055e-8, + "velocityX": 3.2173227507078654, + "velocityY": -1.5223279648179473, + "timestamp": 5.907129949231749 + }, + { + "x": 5.3988689559213965, + "y": 4.32613897098341, + "heading": -0.0972528699779729, + "angularVelocity": 7.203036405891024e-8, + "velocityX": 3.5302934161705415, + "velocityY": -1.1885528129676428, + "timestamp": 5.980913060138573 + }, + { + "x": 5.668131488513907, + "y": 4.271040046607528, + "heading": -0.09725286670603908, + "angularVelocity": 4.4345294956463885e-8, + "velocityX": 3.6493789606207083, + "velocityY": -0.7467687889368456, + "timestamp": 6.054696171045398 + }, + { + "x": 5.942117714678702, + "y": 4.249366302589977, + "heading": -0.09725286355772216, + "angularVelocity": 4.266988599202702e-8, + "velocityX": 3.713400299843557, + "velocityY": -0.2937493926614278, + "timestamp": 6.128479281952223 + }, + { + "x": 6.216382330790908, + "y": 4.231558335942029, + "heading": -0.09725286041527645, + "angularVelocity": 4.2590311894180227e-8, + "velocityX": 3.7171733848218165, + "velocityY": -0.2413555951908442, + "timestamp": 6.202262392859048 + }, + { + "x": 6.490646951405068, + "y": 4.21375043862986, + "heading": -0.09725285727283077, + "angularVelocity": 4.259031148289501e-8, + "velocityX": 3.717173445837876, + "velocityY": -0.2413546554665762, + "timestamp": 6.2760455037658724 + }, + { + "x": 6.764911572019294, + "y": 4.195942541318692, + "heading": -0.09725285413038512, + "angularVelocity": 4.2590311325076504e-8, + "velocityX": 3.717173445838756, + "velocityY": -0.241354655453012, + "timestamp": 6.349828614672697 + }, + { + "x": 7.039176191804646, + "y": 4.178134631242246, + "heading": -0.09725285098790415, + "angularVelocity": 4.259078987217418e-8, + "velocityX": 3.7171734346048453, + "velocityY": -0.24135482846385062, + "timestamp": 6.423611725579522 + }, + { + "x": 7.303151625715879, + "y": 4.160355738175748, + "heading": -0.07493794888540205, + "angularVelocity": 0.302439160239284, + "velocityX": 3.5777216583424787, + "velocityY": -0.24096155404655972, + "timestamp": 6.497394836486347 + }, + { + "x": 7.533445930864737, + "y": 4.1447637615713475, + "heading": -0.055524777247044715, + "angularVelocity": 0.2631113191048932, + "velocityX": 3.1212333326481616, + "velocityY": -0.21132175660214245, + "timestamp": 6.5711779473931715 + }, + { + "x": 7.730059092320319, + "y": 4.131358730965977, + "heading": -0.0390137097405274, + "angularVelocity": 0.22377841356361194, + "velocityX": 2.664744804591801, + "velocityY": -0.18168155883665832, + "timestamp": 6.644961058299996 + }, + { + "x": 7.892991106532595, + "y": 4.120140669647355, + "heading": -0.025405081189306844, + "angularVelocity": 0.1844409700806171, + "velocityX": 2.20825622842103, + "velocityY": -0.15204104544723207, + "timestamp": 6.718744169206821 + }, + { + "x": 8.02224197249394, + "y": 4.111109595697515, + "heading": -0.014699203415963246, + "angularVelocity": 0.1450993003922434, + "velocityX": 1.751767638593706, + "velocityY": -0.12240028698767993, + "timestamp": 6.792527280113646 + }, + { + "x": 8.117811689845258, + "y": 4.104265522227424, + "heading": -0.006896323202665822, + "angularVelocity": 0.10575428600660502, + "velocityX": 1.2952790438994193, + "velocityY": -0.09275935083211777, + "timestamp": 6.866310391020471 + }, + { + "x": 8.179700258175622, + "y": 4.099608457473234, + "heading": -0.0019965838975628653, + "angularVelocity": 0.06640732878951748, + "velocityX": 0.8387904436357877, + "velocityY": -0.06311830305000854, + "timestamp": 6.940093501927295 + }, + { + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": -2.1241979189811956e-28, + "angularVelocity": 0.02706017505936011, + "velocityX": 0.3823018326887492, + "velocityY": -0.03347720903449786, + "timestamp": 7.01387661283412 + }, + { + "x": 8.200639571345327, + "y": 4.096943687462525, + "heading": -0.0010802899918473687, + "angularVelocity": -0.014022329834604143, + "velocityX": -0.09434112255025054, + "velocityY": -0.0025274615139553986, + "timestamp": 7.0909173046870135 + }, + { + "x": 8.15665056358688, + "y": 4.099133352364744, + "heading": -0.005325598680954133, + "angularVelocity": -0.05510475810903996, + "velocityX": -0.5709840695932878, + "velocityY": 0.02842218637392462, + "timestamp": 7.167957996539907 + }, + { + "x": 8.075940653456616, + "y": 4.103707386175575, + "heading": -0.012735784669566238, + "angularVelocity": -0.09618535101893455, + "velocityX": -1.0476270161796544, + "velocityY": 0.05937166062274877, + "timestamp": 7.2449986883928 + }, + { + "x": 7.958509840643352, + "y": 4.1106657697854265, + "heading": -0.023310616019513074, + "angularVelocity": -0.1372629333357902, + "velocityX": -1.5242699668052726, + "velocityY": 0.09032088682612197, + "timestamp": 7.322039380245694 + }, + { + "x": 7.804358124937036, + "y": 4.120008478318512, + "heading": -0.037049836652876394, + "angularVelocity": -0.17833719172197895, + "velocityX": -2.0009129201573495, + "velocityY": 0.12126979013798225, + "timestamp": 7.399080072098587 + }, + { + "x": 7.613485507099167, + "y": 4.131735481105861, + "heading": -0.05395325351914194, + "angularVelocity": -0.21940894428287439, + "velocityX": -2.4775558636250903, + "velocityY": 0.15221829536189774, + "timestamp": 7.476120763951481 + }, + { + "x": 7.385891991055361, + "y": 4.145846741676362, + "heading": -0.07402084166347252, + "angularVelocity": -0.2604803729261546, + "velocityX": -2.9541987561377083, + "velocityY": 0.18316632718520057, + "timestamp": 7.553161455804374 + }, + { + "x": 7.121577593711164, + "y": 4.16234221776091, + "heading": -0.09725284930983917, + "angularVelocity": -0.30155502355466174, + "velocityX": -3.430841429213758, + "velocityY": 0.21411381034903446, + "timestamp": 7.6302021476572675 + }, + { + "x": 6.835203978246458, + "y": 4.1809363249101, + "heading": -0.09725285259123628, + "angularVelocity": -4.259303804512275e-8, + "velocityX": -3.7171734647908923, + "velocityY": 0.24135436354458084, + "timestamp": 7.707242839510161 + }, + { + "x": 6.5488303639215975, + "y": 4.199530449615618, + "heading": -0.09725285587256685, + "angularVelocity": -4.259217424526975e-8, + "velocityX": -3.7171734499955185, + "velocityY": 0.24135459142842275, + "timestamp": 7.784283531363054 + }, + { + "x": 6.262456749596831, + "y": 4.218124574322582, + "heading": -0.09725285915389743, + "angularVelocity": -4.259217438362485e-8, + "velocityX": -3.7171734499943, + "velocityY": 0.24135459144719548, + "timestamp": 7.861324223215948 + }, + { + "x": 5.976083139919176, + "y": 4.236718770600948, + "heading": -0.09725286243522813, + "angularVelocity": -4.259217585749446e-8, + "velocityX": -3.717173389674077, + "velocityY": 0.24135552045497527, + "timestamp": 7.938364915068841 + }, + { + "x": 5.690024921889411, + "y": 4.259659658304737, + "heading": -0.09725286572349402, + "angularVelocity": -4.26821957688407e-8, + "velocityX": -3.71307955769639, + "velocityY": 0.2977762420357482, + "timestamp": 8.015405606921734 + }, + { + "x": 5.4092552652272206, + "y": 4.319022844462833, + "heading": -0.09725286915389218, + "angularVelocity": -4.452709450605306e-8, + "velocityX": -3.644433219762762, + "velocityY": 0.7705432639604047, + "timestamp": 8.092446298774627 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -7.968133349338986e-8, + "velocityX": -3.5158347290283, + "velocityY": 1.2306645781352283, + "timestamp": 8.16948699062752 + }, + { + "x": 4.909018653493503, + "y": 4.524275328875486, + "heading": -0.09725287687080804, + "angularVelocity": -2.1964896325716688e-8, + "velocityX": -3.192337867898693, + "velocityY": 1.5370761958403918, + "timestamp": 8.241338492962369 + }, + { + "x": 4.71108590609518, + "y": 4.628682093882472, + "heading": -0.09725287720527089, + "angularVelocity": -4.654917845961093e-9, + "velocityX": -2.754747513502236, + "velocityY": 1.453090911313464, + "timestamp": 8.313189995297217 + }, + { + "x": 4.54294597125426, + "y": 4.721368217002082, + "heading": -0.09725287703485493, + "angularVelocity": 2.3717800726027118e-9, + "velocityX": -2.3401032598781413, + "velocityY": 1.2899677822694209, + "timestamp": 8.385041497632065 + }, + { + "x": 4.40369795049572, + "y": 4.80026148816612, + "heading": -0.09725287664966911, + "angularVelocity": 5.360859554294991e-9, + "velocityX": -1.937997344990855, + "velocityY": 1.0980044759032763, + "timestamp": 8.456892999966913 + }, + { + "x": 4.292814205558175, + "y": 4.864307082758427, + "heading": -0.09725287620265413, + "angularVelocity": 6.221372875954885e-9, + "velocityX": -1.5432348849269109, + "velocityY": 0.8913605493429588, + "timestamp": 8.52874450230176 + }, + { + "x": 4.209952469660844, + "y": 4.91286872561946, + "heading": -0.09725287578795568, + "angularVelocity": 5.7716044742121175e-9, + "velocityX": -1.1532359547775557, + "velocityY": 0.6758612037744439, + "timestamp": 8.600596004636609 + }, + { + "x": 4.154873707750821, + "y": 4.945521497298995, + "heading": -0.09725287546928488, + "angularVelocity": 4.435130638682436e-9, + "velocityX": -0.7665638173206213, + "velocityY": 0.4544480020384788, + "timestamp": 8.672447506971457 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 2.459066789540458e-9, + "velocityX": -0.38234244368231685, + "velocityY": 0.22880870103743692, + "timestamp": 8.744299009306305 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.7012427908311302e-26, + "velocityX": 2.173382321303034e-25, + "velocityY": 2.8154271739064046e-25, + "timestamp": 8.816150511641153 + }, + { + "x": 4.147908938552217, + "y": 4.9451046130456335, + "heading": -0.10362139740935014, + "angularVelocity": -0.09725274804530627, + "velocityX": 0.31316100417438686, + "velocityY": -0.2574227575741351, + "timestamp": 8.881634749822124 + }, + { + "x": 4.189500528478716, + "y": 4.912093615670984, + "heading": -0.11521450306933155, + "angularVelocity": -0.17703658135172537, + "velocityX": 0.6351389446045882, + "velocityY": -0.5041060000334822, + "timestamp": 8.947118988003096 + }, + { + "x": 4.2528908246594135, + "y": 4.863879621749849, + "heading": -0.13053557559089707, + "angularVelocity": -0.23396580531675798, + "velocityX": 0.9680237251216575, + "velocityY": -0.7362686848076517, + "timestamp": 9.012603226184067 + }, + { + "x": 4.338972089495466, + "y": 4.801805387831604, + "heading": -0.1475645296449939, + "angularVelocity": -0.2600466085752675, + "velocityX": 1.3145341112186142, + "velocityY": -0.9479263352914945, + "timestamp": 9.078087464365039 + }, + { + "x": 4.4488531713485, + "y": 4.727871318118375, + "heading": -0.16346474025801716, + "angularVelocity": -0.24280973642972667, + "velocityX": 1.677977554680654, + "velocityY": -1.1290361126124002, + "timestamp": 9.14357170254601 + }, + { + "x": 4.583832471873963, + "y": 4.645249641206774, + "heading": -0.17403705358129207, + "angularVelocity": -0.1614482143635436, + "velocityX": 2.0612486954866247, + "velocityY": -1.26170326183331, + "timestamp": 9.209055940726982 + }, + { + "x": 4.74502129849887, + "y": 4.559256593692149, + "heading": -0.17264089042553551, + "angularVelocity": 0.021320598582793455, + "velocityX": 2.4614904456771134, + "velocityY": -1.3131869577069004, + "timestamp": 9.274540178907953 + }, + { + "x": 4.931721031095368, + "y": 4.478536439161179, + "heading": -0.14954301236675954, + "angularVelocity": 0.3527242386930233, + "velocityX": 2.8510636724601572, + "velocityY": -1.2326653981663842, + "timestamp": 9.340024417088925 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 0.7985148568065102, + "velocityX": 3.1560555624992586, + "velocityY": -0.9880598134339889, + "timestamp": 9.405508655269896 + }, + { + "x": 5.280245938471686, + "y": 4.380962564632748, + "heading": -0.04650808406049355, + "angularVelocity": 1.167166985692144, + "velocityX": 3.2627221399232984, + "velocityY": -0.7560690286372685, + "timestamp": 9.448985545769855 + }, + { + "x": 5.424678195182071, + "y": 4.359487062895353, + "heading": 0.008191327783648325, + "angularVelocity": 1.2581261266647945, + "velocityX": 3.322046610268118, + "velocityY": -0.493952108590108, + "timestamp": 9.492462436269813 + }, + { + "x": 5.570537921062946, + "y": 4.349646196769418, + "heading": 0.06315884602952881, + "angularVelocity": 1.264292768268082, + "velocityX": 3.354879436030857, + "velocityY": -0.22634705501637128, + "timestamp": 9.535939326769771 + }, + { + "x": 5.717103514016266, + "y": 4.35150566579831, + "heading": 0.11770080768702691, + "angularVelocity": 1.2545046582287314, + "velocityX": 3.3711148904142765, + "velocityY": 0.04276913568356193, + "timestamp": 9.57941621726973 + }, + { + "x": 5.863572420206036, + "y": 4.365085009600068, + "heading": 0.17138870164708653, + "angularVelocity": 1.2348604820326599, + "velocityX": 3.368891024759707, + "velocityY": 0.3123347517635909, + "timestamp": 9.622893107769688 + }, + { + "x": 6.009034373006399, + "y": 4.3903410665362745, + "heading": 0.22417378599549884, + "angularVelocity": 1.2140952064744144, + "velocityX": 3.345730366814133, + "velocityY": 0.5809076188700988, + "timestamp": 9.666369998269646 + }, + { + "x": 6.152424125530893, + "y": 4.427134155183534, + "heading": 0.2765912547304818, + "angularVelocity": 1.205639780863199, + "velocityX": 3.2980682582309235, + "velocityY": 0.8462677119766714, + "timestamp": 9.709846888769604 + }, + { + "x": 6.292378614943397, + "y": 4.475129707153334, + "heading": 0.330369231652926, + "angularVelocity": 1.2369324554730028, + "velocityX": 3.2190547162667764, + "velocityY": 1.1039324896026255, + "timestamp": 9.753323779269563 + }, + { + "x": 6.427989965818357, + "y": 4.530754098625934, + "heading": 0.3892449871584701, + "angularVelocity": 1.3541850585105815, + "velocityX": 3.1191593813520333, + "velocityY": 1.2794013286818071, + "timestamp": 9.796800669769521 + }, + { + "x": 6.568195373885816, + "y": 4.593631496863788, + "heading": 0.42157526724387395, + "angularVelocity": 0.743619879748183, + "velocityX": 3.224826027233803, + "velocityY": 1.4462257423381046, + "timestamp": 9.84027756026948 + }, + { + "x": 6.710207768570418, + "y": 4.660649558034501, + "heading": 0.440928265304377, + "angularVelocity": 0.44513298531599427, + "velocityX": 3.26638802940008, + "velocityY": 1.5414639915606807, + "timestamp": 9.883754450769437 + }, + { + "x": 6.85219596425133, + "y": 4.730282709551517, + "heading": 0.4559517105455699, + "angularVelocity": 0.34555013176959587, + "velocityX": 3.265831434772203, + "velocityY": 1.6016129653311566, + "timestamp": 9.927231341269396 + }, + { + "x": 6.992339110580002, + "y": 4.802100760534676, + "heading": 0.47358684784417715, + "angularVelocity": 0.4056209424320313, + "velocityX": 3.223393962105149, + "velocityY": 1.6518672369917733, + "timestamp": 9.970708231769354 + }, + { + "x": 7.1318594634446475, + "y": 4.8743385269135056, + "heading": 0.49238243611774024, + "angularVelocity": 0.4323121561230585, + "velocityX": 3.2090692609394345, + "velocityY": 1.6615209953641499, + "timestamp": 10.014185122269312 + }, + { + "x": 7.264750404712455, + "y": 4.9484671577912, + "heading": 0.5297243146446315, + "angularVelocity": 0.8588902770525219, + "velocityX": 3.0565879882309943, + "velocityY": 1.7050122496171904, + "timestamp": 10.05766201276927 + }, + { + "x": 7.388081081229855, + "y": 5.024967733696522, + "heading": 0.592130827533757, + "angularVelocity": 1.4353950379497669, + "velocityX": 2.8366949682733105, + "velocityY": 1.7595687047903361, + "timestamp": 10.101138903269229 + }, + { + "x": 7.501500129699707, + "y": 5.105462551116943, + "heading": 0.6747401864044066, + "angularVelocity": 1.9000751415451151, + "velocityX": 2.608720337761042, + "velocityY": 1.8514391552564737, + "timestamp": 10.144615793769187 + }, + { + "x": 7.646324439274632, + "y": 5.2181193934144225, + "heading": 0.716438811665056, + "angularVelocity": 0.6791497141368164, + "velocityX": 2.3587681328355696, + "velocityY": 1.8348533498079573, + "timestamp": 10.206014075168227 + }, + { + "x": 7.772039173775546, + "y": 5.317779255820348, + "heading": 0.7438971482542158, + "angularVelocity": 0.44721669668085284, + "velocityX": 2.047528556766434, + "velocityY": 1.62317022781495, + "timestamp": 10.267412356567267 + }, + { + "x": 7.878893815894361, + "y": 5.4039493449354215, + "heading": 0.7593796608518738, + "angularVelocity": 0.2521652437962253, + "velocityX": 1.7403523304560495, + "velocityY": 1.4034609300387069, + "timestamp": 10.328810637966306 + }, + { + "x": 7.966980879790036, + "y": 5.476461142089676, + "heading": 0.7636631361192312, + "angularVelocity": 0.06976539358680627, + "velocityX": 1.4346828915809557, + "velocityY": 1.1810069516927688, + "timestamp": 10.390208919365346 + }, + { + "x": 8.036348554918035, + "y": 5.535229545877986, + "heading": 0.7571399208631822, + "angularVelocity": -0.10624426461798567, + "velocityX": 1.1297983192260301, + "velocityY": 0.9571669181807075, + "timestamp": 10.451607200764386 + }, + { + "x": 8.087026476479373, + "y": 5.580203129397342, + "heading": 0.7400467384743379, + "angularVelocity": -0.27839838509082054, + "velocityX": 0.8253964183780998, + "velocityY": 0.7324892895138765, + "timestamp": 10.513005482163425 + }, + { + "x": 8.119034784241132, + "y": 5.611347352514501, + "heading": 0.7125421599400041, + "angularVelocity": -0.44796984390452543, + "velocityX": 0.5213225359473886, + "velocityY": 0.5072491022142075, + "timestamp": 10.574403763562465 + }, + { + "x": 8.1323881149292, + "y": 5.628637313842773, + "heading": 0.6747401864044066, + "angularVelocity": -0.6156845545873708, + "velocityX": 0.2174870433470383, + "velocityY": 0.2816033435187223, + "timestamp": 10.635802044961505 + }, + { + "x": 8.125741105489272, + "y": 5.631444479838457, + "heading": 0.6235311710708692, + "angularVelocity": -0.7910339249344661, + "velocityX": -0.1026774276383878, + "velocityY": 0.04336274620875669, + "timestamp": 10.70053885803533 + }, + { + "x": 8.09835684603502, + "y": 5.618815221035742, + "heading": 0.5616094020000137, + "angularVelocity": -0.9565155609411891, + "velocityX": -0.42300907557840733, + "velocityY": -0.19508619907365696, + "timestamp": 10.765275671109155 + }, + { + "x": 8.050222803941077, + "y": 5.59073345676368, + "heading": 0.48977295422299394, + "angularVelocity": -1.1096692031332702, + "velocityX": -0.7435343170052635, + "velocityY": -0.4337835450756361, + "timestamp": 10.83001248418298 + }, + { + "x": 7.981324451485502, + "y": 5.547180062425158, + "heading": 0.40904421087349874, + "angularVelocity": -1.2470299280477726, + "velocityX": -1.0642839704976186, + "velocityY": -0.6727763118158692, + "timestamp": 10.894749297256805 + }, + { + "x": 7.891645067392915, + "y": 5.488132407343552, + "heading": 0.32078665814595236, + "angularVelocity": -1.3633286616518716, + "velocityX": -1.3852919202296423, + "velocityY": -0.9121186582704987, + "timestamp": 10.95948611033063 + }, + { + "x": 7.781166411701721, + "y": 5.413563935212295, + "heading": 0.22693931715370197, + "angularVelocity": -1.4496750231622744, + "velocityX": -1.7065816255922823, + "velocityY": -1.1518712242788174, + "timestamp": 11.024222923404455 + }, + { + "x": 7.649874183396069, + "y": 5.323444723125603, + "heading": 0.13053857941250757, + "angularVelocity": -1.4891177548585681, + "velocityX": -2.0280922410549644, + "velocityY": -1.3920860142423153, + "timestamp": 11.08895973647828 + }, + { + "x": 7.497788734174063, + "y": 5.217751261949818, + "heading": 0.0370488934612777, + "angularVelocity": -1.4441502680185108, + "velocityX": -2.3492884805522913, + "velocityY": -1.6326639535877772, + "timestamp": 11.153696549552105 + }, + { + "x": 7.325168258315647, + "y": 5.096564905445784, + "heading": -0.04136336755772595, + "angularVelocity": -1.2112468516110346, + "velocityX": -2.66649635133507, + "velocityY": -1.871985208259067, + "timestamp": 11.21843336262593 + }, + { + "x": 7.133849861870766, + "y": 4.963599555351033, + "heading": -0.06503952290078549, + "angularVelocity": -0.36572939288900796, + "velocityX": -2.955326148457454, + "velocityY": -2.0539372233711526, + "timestamp": 11.283170175699755 + }, + { + "x": 6.940221298800236, + "y": 4.819868100403324, + "heading": -0.06503954882158174, + "angularVelocity": -4.004027232662301e-7, + "velocityX": -2.9910116651821412, + "velocityY": -2.2202429826719707, + "timestamp": 11.34790698877358 + }, + { + "x": 6.740356177089726, + "y": 4.684943363475215, + "heading": -0.06503957483267325, + "angularVelocity": -4.017975287461778e-7, + "velocityX": -3.08734879306747, + "velocityY": -2.0842041880288593, + "timestamp": 11.412643801847405 + }, + { + "x": 6.527131993072097, + "y": 4.57231033799875, + "heading": -0.06503960182577956, + "angularVelocity": -4.1696686974459996e-7, + "velocityX": -3.293708384663109, + "velocityY": -1.739860523378229, + "timestamp": 11.47738061492123 + }, + { + "x": 6.303024971058329, + "y": 4.483277800206398, + "heading": -0.06503963136622377, + "angularVelocity": -4.5631600956130875e-7, + "velocityX": -3.461817339667266, + "velocityY": -1.3752999810296351, + "timestamp": 11.542117427995056 + }, + { + "x": 6.070638079320141, + "y": 4.41887996664659, + "heading": -0.06503966569966328, + "angularVelocity": -5.303541813322711e-7, + "velocityX": -3.5897178236619824, + "velocityY": -0.9947637287360487, + "timestamp": 11.60685424106888 + }, + { + "x": 5.832670509915108, + "y": 4.379864860722445, + "heading": -0.06503973197653827, + "angularVelocity": -0.0000010237895848927841, + "velocityX": -3.67592345229684, + "velocityY": -0.60267263820436, + "timestamp": 11.671591054142706 + }, + { + "x": 5.593035616597729, + "y": 4.366766567771227, + "heading": -0.0691739587008402, + "angularVelocity": -0.06386206746982208, + "velocityX": -3.7016788738750415, + "velocityY": -0.20233144526718327, + "timestamp": 11.73632786721653 + }, + { + "x": 5.359733281234782, + "y": 4.378764088602827, + "heading": -0.08321570293446177, + "angularVelocity": -0.2169050894984338, + "velocityX": -3.6038588290852247, + "velocityY": 0.1853276406720581, + "timestamp": 11.801064680290356 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.21683446699990425, + "velocityX": -3.419080202788392, + "velocityY": 0.5417320490954629, + "timestamp": 11.86580149336418 + }, + { + "x": 4.905765298437056, + "y": 4.481387310799775, + "heading": -0.10673913036260185, + "angularVelocity": -0.12897497729525575, + "velocityX": -3.1628016184093535, + "velocityY": 0.9184524777738038, + "timestamp": 11.939352621411974 + }, + { + "x": 4.700482894835419, + "y": 4.568326227070152, + "heading": -0.11059429780349972, + "angularVelocity": -0.05241479693408341, + "velocityX": -2.791016386155829, + "velocityY": 1.1820201617286428, + "timestamp": 12.012903749459767 + }, + { + "x": 4.527923671898321, + "y": 4.662591020242044, + "heading": -0.11045427798457029, + "angularVelocity": 0.0019037072937678628, + "velocityX": -2.3461125276687587, + "velocityY": 1.2816226708397902, + "timestamp": 12.08645487750756 + }, + { + "x": 4.3886983023339585, + "y": 4.75311454797141, + "heading": -0.10805031221492864, + "angularVelocity": 0.03268428144405263, + "velocityX": -1.8929059724807213, + "velocityY": 1.2307564837148823, + "timestamp": 12.160006005555353 + }, + { + "x": 4.281048300464299, + "y": 4.832306401053517, + "heading": -0.1047189693449996, + "angularVelocity": 0.04529288616436113, + "velocityX": -1.4636077613889036, + "velocityY": 1.076691210373392, + "timestamp": 12.233557133603146 + }, + { + "x": 4.20282348849849, + "y": 4.895385445772344, + "heading": -0.10139996108706777, + "angularVelocity": 0.04512518497031254, + "velocityX": -1.0635433343045229, + "velocityY": 0.8576217169346194, + "timestamp": 12.307108261650939 + }, + { + "x": 4.15212537277285, + "y": 4.939288518901389, + "heading": -0.09875289338109836, + "angularVelocity": 0.0359894916125467, + "velocityX": -0.6892907977250395, + "velocityY": 0.5969055036180668, + "timestamp": 12.380659389698732 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 0.020394222744298442, + "velocityX": -0.3361409221475862, + "velocityY": 0.3082648480890461, + "timestamp": 12.454210517746525 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.761901225942554e-27, + "velocityX": -1.9870172205028814e-26, + "velocityY": -2.1545999896560787e-26, + "timestamp": 12.527761645794318 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index e471cc97..5aed1bce 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -276,6 +276,7 @@ private void configureAutoRoutines() { autos.add(new CenterLanePCBAFG(drivetrain, shooter, indexer)); autos.add(new CenterLanePCBAEF(drivetrain, shooter, indexer)); autos.add(new CenterLanePCBAGF(drivetrain, shooter, indexer)); + autos.add(new CenterLanePCBA(drivetrain, shooter, indexer)); // autos.add(new CenterLanePCBA(drivetrain, shooter, indexer)); // autos.add(new CenterLanePBDA(drivetrain, shooter, indexer)); // autos.add(new CenterLanePSubCSubBSubASubFSub(drivetrain, shooter, indexer)); @@ -283,6 +284,7 @@ private void configureAutoRoutines() { // autos.add(new CenterLanePSubCSubBSubASub(drivetrain, shooter, indexer)); autos.add(new SourceLanePHGF(drivetrain, shooter, indexer)); autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer)); + autos.add(new SourceLanePGFE(drivetrain, shooter, indexer)); // autos.addDefault(new ATestAuto(drivetrain, shooter, indexer)); autos.add(new AutoCommand("Subwoofer Shot", ShootSequence.forAutoSubwoofer(shooter, indexer))); } diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java index 0f822eed..1338bab0 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBA.java @@ -14,9 +14,10 @@ public class CenterLanePCBA extends AutoCommand { public CenterLanePCBA(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { super("!CenterLanePCBA"); addPath( - PathHelper.fromChoreoPath("CenterLanePCBA.1", true, true), - PathHelper.fromChoreoPath("CenterLanePCBA.2"), - PathHelper.fromChoreoPath("CenterLanePCBA.3") + PathHelper.fromChoreoPath("CenterLanePCBAFSprint.1", false, true), + PathHelper.fromChoreoPath("CenterLanePCBAFSprint.2"), + PathHelper.fromChoreoPath("CenterLanePCBAFSprint.3") + ); addCommands( @@ -24,9 +25,10 @@ public CenterLanePCBA(Drivetrain drivetrain, Shooter shooter, Indexer indexer) { Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, shooter, indexer, 0), - createSegmentSequence(drivetrain, shooter, indexer, 1), - createSegmentSequence(drivetrain, shooter, indexer, 1) + drivetrain.commandCopyVisionPose(), + createSegmentSequence(drivetrain, shooter, indexer, 0, false, false, false), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, false, true), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, false, true) ) ) ); diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFE.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFE.java new file mode 100644 index 00000000..20aa7edd --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFE.java @@ -0,0 +1,35 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class SourceLanePGFE extends AutoCommand { + private static final double SHOT_WAIT = 0.2; + public SourceLanePGFE(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ + super("SourceLanePGFE"); + addPath( + PathHelper.fromChoreoPath("SourceLanePGFE.1", true, true), + PathHelper.fromChoreoPath("SourceLanePGFE.2"), + PathHelper.fromChoreoPath("SourceLanePGFE.3"), + PathHelper.fromChoreoPath("SourceLanePGFE.4") + ); + + addCommands( + Commands.parallel( + new AutoShootPrepare(drivetrain, shooter), + Commands.sequence( + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0) + ) + ) + ); + } + +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index 4eab4262..e3954d18 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -73,6 +73,7 @@ public enum Zone { public enum PatternLevel { DEFAULT, TRAMP_STATE, + INTAKE_PREREADY, INTAKE_STATE, COAST_STATE, DRIVER_LOCK, From 57ebbc1b7577d98e494cf2feda4e1ee21266746f Mon Sep 17 00:00:00 2001 From: Zach R Date: Sat, 6 Apr 2024 22:46:44 -0700 Subject: [PATCH 75/75] event: eod day 3 --- paths.chor | 8749 +++++++++++------ src/main/deploy/choreo/AmpLanePAEDF.1.traj | 302 +- src/main/deploy/choreo/AmpLanePAEDF.2.traj | 94 +- src/main/deploy/choreo/AmpLanePAEDF.3.traj | 998 +- src/main/deploy/choreo/AmpLanePAEDF.4.traj | 620 +- src/main/deploy/choreo/AmpLanePAEDF.5.traj | 876 +- src/main/deploy/choreo/AmpLanePAEDF.traj | 2878 +++--- src/main/deploy/choreo/SourceLanePGFE.1.traj | 358 +- src/main/deploy/choreo/SourceLanePGFE.2.traj | 1278 +-- src/main/deploy/choreo/SourceLanePGFE.3.traj | 790 +- src/main/deploy/choreo/SourceLanePGFE.4.traj | 1030 +- src/main/deploy/choreo/SourceLanePGFE.traj | 3506 +++---- src/main/deploy/choreo/SourceLanePGFH.1.traj | 230 + src/main/deploy/choreo/SourceLanePGFH.2.traj | 680 ++ src/main/deploy/choreo/SourceLanePGFH.3.traj | 446 + src/main/deploy/choreo/SourceLanePGFH.4.traj | 752 ++ src/main/deploy/choreo/SourceLanePGFH.traj | 2066 ++++ .../org/team1540/robot2024/Constants.java | 4 +- .../team1540/robot2024/RobotContainer.java | 3 +- .../commands/autos/CenterLanePDEABC.java | 9 +- .../commands/autos/SourceLanePGFE.java | 8 +- .../commands/autos/SourceLanePGFH.java | 35 + .../commands/autos/SourceLanePHGF.java | 8 +- .../indexer/ContinuousIntakeCommand.java | 28 +- .../robot2024/subsystems/led/Leds.java | 2 +- .../robot2024/util/auto/AutoCommand.java | 13 +- 26 files changed, 16236 insertions(+), 9527 deletions(-) create mode 100644 src/main/deploy/choreo/SourceLanePGFH.1.traj create mode 100644 src/main/deploy/choreo/SourceLanePGFH.2.traj create mode 100644 src/main/deploy/choreo/SourceLanePGFH.3.traj create mode 100644 src/main/deploy/choreo/SourceLanePGFH.4.traj create mode 100644 src/main/deploy/choreo/SourceLanePGFH.traj create mode 100644 src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFH.java diff --git a/paths.chor b/paths.chor index a5087ecc..f14f8ee2 100644 --- a/paths.chor +++ b/paths.chor @@ -10,7 +10,7 @@ "trackWidth": 0.5016497291091463, "bumperLength": 0.8635995336562519, "bumperWidth": 0.8000995679462333, - "wheelRadius": 0.04933448212790457 + "wheelRadius": 0.049399110412327286 }, "paths": { "Taxi": { @@ -39596,1559 +39596,1559 @@ { "x": 0.43297290802001953, "y": 6.9807281494140625, - "heading": -4.820407208942378e-26, - "angularVelocity": -1.0568380673400748e-29, - "velocityX": 2.0900789241101526e-25, - "velocityY": 6.12105194243679e-25, + "heading": -2.8662975018215305e-26, + "angularVelocity": -1.4927020760415103e-39, + "velocityX": 1.0727948153345131e-25, + "velocityY": 3.1703902069351426e-25, "timestamp": 0 }, { - "x": 0.4535853919747023, - "y": 6.976442037279776, - "heading": -9.657051327438745e-26, - "angularVelocity": -2.7866726754125085e-27, - "velocityX": 0.3537634154941742, - "velocityY": -0.07356074460262349, - "timestamp": 0.05826629620784589 + "x": 0.45358539196480147, + "y": 6.976442037232183, + "heading": -5.74233401454689e-26, + "angularVelocity": -1.6703718504854773e-27, + "velocityX": 0.3535319273172983, + "velocityY": -0.07351261033930959, + "timestamp": 0.05830444820414204 }, { - "x": 0.49483104636031155, - "y": 6.967970532726724, - "heading": -1.449381860201486e-25, - "angularVelocity": -2.8078094367592775e-27, - "velocityX": 0.7078818643024581, - "velocityY": -0.1453928789781493, - "timestamp": 0.11653259241569178 + "x": 0.49483104633484415, + "y": 6.967970532602043, + "heading": -8.618370527272265e-26, + "angularVelocity": -1.670371850488467e-27, + "velocityX": 0.7074186557023707, + "velocityY": -0.1452977412714415, + "timestamp": 0.11660889640828408 }, { - "x": 0.5567354919847137, - "y": 6.955441991018911, - "heading": -1.9330585876590946e-25, - "angularVelocity": -2.807809436759289e-27, - "velocityX": 1.0624400322886223, - "velocityY": -0.21502210580061862, - "timestamp": 0.17479888862353765 + "x": 0.5567354919435141, + "y": 6.955441990812605, + "heading": -1.1494407039997645e-25, + "angularVelocity": -1.6703718504884638e-27, + "velocityX": 1.0617448156257865, + "velocityY": -0.21488140571319628, + "timestamp": 0.1749133446124261 }, { - "x": 0.6393312280567922, - "y": 6.939025561506136, - "heading": -2.416735315116705e-25, - "angularVelocity": -2.8078094367592704e-27, - "velocityX": 1.417555970563931, - "velocityY": -0.2817482932880241, - "timestamp": 0.23306518483138355 + "x": 0.6393312280070567, + "y": 6.939025561249793, + "heading": -1.4370443552723021e-25, + "angularVelocity": -1.6703718504884706e-27, + "velocityX": 1.4166283809830285, + "velocityY": -0.28156392982801115, + "timestamp": 0.23321779281656815 }, { - "x": 0.7426606798556364, - "y": 6.918954250670703, - "heading": -2.9004008719315308e-25, - "angularVelocity": -2.7886377311391825e-27, - "velocityX": 1.7734000361075017, - "velocityY": -0.3444754882622709, - "timestamp": 0.29133148103922946 + "x": 0.742660679814762, + "y": 6.9189542504517005, + "heading": -1.7246480065448678e-25, + "angularVelocity": -1.6703718505364274e-27, + "velocityX": 1.7722395973274057, + "velocityY": -0.3442500772465394, + "timestamp": 0.29152224102071017 }, { "x": 0.8667811155319214, "y": 6.895569324493408, - "heading": -3.4495163504680925e-25, - "angularVelocity": -1.1511759285858433e-25, - "velocityX": 2.130226970897995, - "velocityY": -0.40134567836399193, - "timestamp": 0.34959777724707536 - }, - { - "x": 1.01097419129112, - "y": 6.869549473556677, - "heading": 0.010039096018647366, - "angularVelocity": 0.17297490397522428, - "velocityX": 2.4844650740475953, - "velocityY": -0.44832534810612334, - "timestamp": 0.4076356531928559 - }, - { - "x": 1.175726572408581, - "y": 6.840802777300111, - "heading": 0.03011443547557583, - "angularVelocity": 0.34590065762715055, - "velocityX": 2.8387045258405714, - "velocityY": -0.4953092405280373, - "timestamp": 0.4656735291386364 - }, - { - "x": 1.3610383679335918, - "y": 6.809328854372862, - "heading": 0.06022199220724938, - "angularVelocity": 0.5187570399681801, - "velocityX": 3.1929458565666793, - "velocityY": -0.5422997036737339, - "timestamp": 0.5237114050844169 - }, - { - "x": 1.5460799084730097, - "y": 6.784713461455284, - "heading": 0.15060245758508448, - "angularVelocity": 1.5572669382709545, - "velocityX": 3.1882893287184593, - "velocityY": -0.4241263574251743, - "timestamp": 0.5817492810301974 - }, - { - "x": 1.7105613111663502, - "y": 6.762828401317669, - "heading": 0.23096913740746994, - "angularVelocity": 1.384728136802675, - "velocityX": 2.83403553305432, - "velocityY": -0.37708237562070457, - "timestamp": 0.6397871569759779 - }, - { - "x": 1.8544824093339345, - "y": 6.743675274341158, - "heading": 0.3013204880217799, - "angularVelocity": 1.2121627380029003, - "velocityX": 2.4797788654780635, - "velocityY": -0.33001081904520096, - "timestamp": 0.6978250329217583 - }, - { - "x": 1.977843185069378, - "y": 6.727255600600169, - "heading": 0.36164875379550304, - "angularVelocity": 1.0394637086664282, - "velocityX": 2.1255218893725236, - "velocityY": -0.2829130713937313, - "timestamp": 0.7558629088675388 - }, - { - "x": 2.0806437247378744, - "y": 6.713570715573333, - "heading": 0.41194198603949084, - "angularVelocity": 0.8665588018929588, - "velocityX": 1.7712664013502706, - "velocityY": -0.2357923132752318, - "timestamp": 0.8139007848133193 - }, - { - "x": 2.162884155033722, - "y": 6.702621710001363, - "heading": 0.45218713038285063, - "angularVelocity": 0.6934289666451124, - "velocityX": 1.4170130962869354, - "velocityY": -0.18865276155520178, - "timestamp": 0.8719386607590998 - }, - { - "x": 2.2245645766627997, - "y": 6.694409403888087, - "heading": 0.4823732249131576, - "angularVelocity": 0.5201102562489893, - "velocityX": 1.0627615263987253, - "velocityY": -0.1414990810647272, - "timestamp": 0.9299765367048802 - }, - { - "x": 2.265685008078604, - "y": 6.688934342765278, - "heading": 0.5024941236633388, - "angularVelocity": 0.34668565005684077, - "velocityX": 0.7085102744666172, - "velocityY": -0.09433600099222642, - "timestamp": 0.9880144126506607 + "heading": -2.0459528719631722e-25, + "angularVelocity": -5.947250390698025e-26, + "velocityX": 2.1288330400208064, + "velocityY": -0.4010830507547883, + "timestamp": 0.3498266892248522 + }, + { + "x": 1.010974192105577, + "y": 6.8695494740311895, + "heading": 0.010039070916729844, + "angularVelocity": 0.17286128420602442, + "velocityX": 2.482839358032844, + "velocityY": -0.4480319745776911, + "timestamp": 0.4079025675965806 + }, + { + "x": 1.1757265748164618, + "y": 6.8408027784970535, + "heading": 0.030114360354405487, + "angularVelocity": 0.3456734534289609, + "velocityX": 2.836847023756549, + "velocityY": -0.49498511843652804, + "timestamp": 0.46597844596830895 + }, + { + "x": 1.361038372709757, + "y": 6.809328856521608, + "heading": 0.06022184256964511, + "angularVelocity": 0.5184163039692586, + "velocityX": 3.1908565671131695, + "velocityY": -0.541944829038827, + "timestamp": 0.5240543243400373 + }, + { + "x": 1.5460799068169642, + "y": 6.7847134615337374, + "heading": 0.15060251572580788, + "angularVelocity": 1.5562515056192512, + "velocityX": 3.1862029347676004, + "velocityY": -0.42384886252280385, + "timestamp": 0.5821302027117656 + }, + { + "x": 1.7105613074532942, + "y": 6.762828400607154, + "heading": 0.2309692612234495, + "angularVelocity": 1.383823159474839, + "velocityX": 2.832181023307612, + "velocityY": -0.37683564226962823, + "timestamp": 0.640206081083494 + }, + { + "x": 1.8544824052632503, + "y": 6.743675273377495, + "heading": 0.30132062249757485, + "angularVelocity": 1.2113697329522057, + "velocityX": 2.478156195740254, + "velocityY": -0.32979487812590036, + "timestamp": 0.6982819594552223 + }, + { + "x": 1.9778431814510589, + "y": 6.7272555996641294, + "heading": 0.36164887282711905, + "angularVelocity": 1.0387832611570498, + "velocityX": 2.124131044531241, + "velocityY": -0.28272794443621835, + "timestamp": 0.7563578378269507 + }, + { + "x": 2.080643721940094, + "y": 6.71357071481128, + "heading": 0.4119420778554084, + "angularVelocity": 0.865991293431256, + "velocityX": 1.7701073728241554, + "velocityY": -0.23563801765090975, + "timestamp": 0.814433716198679 + }, + { + "x": 2.162884153160651, + "y": 6.702621709473572, + "heading": 0.45218719175687055, + "angularVelocity": 0.692974691555483, + "velocityX": 1.4160858781017074, + "velocityY": -0.18852931104418616, + "timestamp": 0.8725095945704073 + }, + { + "x": 2.224564575642773, + "y": 6.694409403593854, + "heading": 0.48237325830124134, + "angularVelocity": 0.5197694359637186, + "velocityX": 1.062066114391284, + "velocityY": -0.1414064859622663, + "timestamp": 0.9305854729421357 + }, + { + "x": 2.2656850077143114, + "y": 6.688934342658481, + "heading": 0.5024941355793361, + "angularVelocity": 0.3464584237418892, + "velocityX": 0.7080466662654255, + "velocityY": -0.09427426823109995, + "timestamp": 0.988661351313864 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 0.17327126075488852, - "velocityX": 0.354257244182047, - "velocityY": -0.047168141046465786, - "timestamp": 1.0460522885964412 + "angularVelocity": 0.17315767410862712, + "velocityX": 0.3540254393299603, + "velocityY": -0.04713727434869594, + "timestamp": 1.0467372296855924 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": -1.480616560828957e-26, - "velocityX": -5.749006986314809e-23, - "velocityY": -3.283580548358229e-23, - "timestamp": 1.1040901645422216 + "angularVelocity": 1.537853498112127e-28, + "velocityX": 2.8833537662590268e-24, + "velocityY": 1.7087922239161212e-24, + "timestamp": 1.1048131080573207 }, { - "x": 2.3347330882331176, - "y": 6.713955255875249, + "x": 2.3347330882302457, + "y": 6.713955255873605, "heading": 0.5125504196, - "angularVelocity": -2.553583698003105e-17, - "velocityX": 0.5108360349814401, - "velocityY": 0.2924454065412527, - "timestamp": 1.1990085705343583 + "angularVelocity": 2.6668696602250958e-17, + "velocityX": 0.5105017651842132, + "velocityY": 0.29225404246344755, + "timestamp": 1.1997936653594352 }, { - "x": 2.4317085702199166, - "y": 6.7694721581924355, + "x": 2.4317085702128325, + "y": 6.76947215818838, "heading": 0.5125504196, - "angularVelocity": -7.602638325412282e-17, - "velocityX": 1.021672045302073, - "velocityY": 0.5848907989645902, - "timestamp": 1.293926976526495 + "angularVelocity": 7.804843369806144e-17, + "velocityX": 1.0210035057398825, + "velocityY": 0.5845080708274503, + "timestamp": 1.2947742226615497 }, { - "x": 2.577171786567949, - "y": 6.852747507871406, + "x": 2.5771717865588863, + "y": 6.852747507866218, "heading": 0.5125504196, - "angularVelocity": -2.864728625251783e-17, - "velocityX": 1.5325079980808258, - "velocityY": 0.8773361584461279, - "timestamp": 1.3888453825186315 + "angularVelocity": 1.13517713021376e-16, + "velocityX": 1.5315051888289535, + "velocityY": 0.8767620662927504, + "timestamp": 1.3897547799636643 }, { - "x": 2.771122709968306, - "y": 6.963781289278251, + "x": 2.7711227099773685, + "y": 6.963781289283441, "heading": 0.5125504196, - "angularVelocity": -4.95735356219891e-19, - "velocityX": 2.0433436631502695, - "velocityY": 1.1697813532187173, - "timestamp": 1.4837637885107682 + "angularVelocity": 6.94376955873156e-17, + "velocityX": 2.042006584585122, + "velocityY": 1.1690158972645894, + "timestamp": 1.4847353372657788 }, { - "x": 2.9165859263163383, - "y": 7.047056638957223, + "x": 2.9165859263234224, + "y": 7.047056638961279, "heading": 0.5125504196, - "angularVelocity": -2.1000816812429605e-17, - "velocityX": 1.532507998080826, - "velocityY": 0.8773361584461278, - "timestamp": 1.5786821945029048 + "angularVelocity": 3.9533104900387686e-17, + "velocityX": 1.5315051888289533, + "velocityY": 0.8767620662927503, + "timestamp": 1.5797158945678933 }, { - "x": 3.0135614083031372, - "y": 7.102573541274409, + "x": 3.013561408306009, + "y": 7.102573541276054, "heading": 0.5125504196, - "angularVelocity": 1.0493910343453442e-17, - "velocityX": 1.021672045302073, - "velocityY": 0.5848907989645902, - "timestamp": 1.6736006004950414 + "angularVelocity": -1.917838345664243e-18, + "velocityX": 1.0210035057398823, + "velocityY": 0.5845080708274502, + "timestamp": 1.6746964518700078 }, { "x": 3.062049150466919, "y": 7.130331993103027, "heading": 0.5125504196, - "angularVelocity": 3.728395973644304e-17, - "velocityX": 0.5108360349814401, - "velocityY": 0.2924454065412527, - "timestamp": 1.768519006487178 + "angularVelocity": -6.474946504344956e-17, + "velocityX": 0.5105017651842131, + "velocityY": 0.29225404246344755, + "timestamp": 1.7696770091721223 }, { "x": 3.062049150466919, "y": 7.130331993103027, "heading": 0.5125504196, - "angularVelocity": -2.9402212487685217e-25, - "velocityX": 6.030156719342467e-23, - "velocityY": 3.0718511862526725e-23, - "timestamp": 1.8634374124793147 - }, - { - "x": 3.0865020118467, - "y": 7.124545249523675, - "heading": 0.48610131944841944, - "angularVelocity": -0.40878852104321894, - "velocityX": 0.3779353165675877, - "velocityY": -0.08943798979559645, - "timestamp": 1.9281385936281472 - }, - { - "x": 3.135458110441477, - "y": 7.112972562266447, - "heading": 0.4339971852577861, - "angularVelocity": -0.8053042195748789, - "velocityX": 0.7566492253389184, - "velocityY": -0.17886361657922661, - "timestamp": 1.9928397747769797 - }, - { - "x": 3.2089771746406726, - "y": 7.095606129050513, - "heading": 0.357225026215491, - "angularVelocity": -1.1865650314125755, - "velocityX": 1.136286276290371, - "velocityY": -0.2684098328280398, - "timestamp": 2.057540955925812 - }, - { - "x": 3.307133671873185, - "y": 7.072412045867193, - "heading": 0.2571531716690748, - "angularVelocity": -1.5466773986122515, - "velocityX": 1.5170742711284095, - "velocityY": -0.3584800581919169, - "timestamp": 2.1222421370746445 - }, - { - "x": 3.4300337444037163, - "y": 7.043313631479606, - "heading": 0.13605141213735358, - "angularVelocity": -1.8717086362480815, - "velocityX": 1.8995027656114087, - "velocityY": -0.44973544332447146, - "timestamp": 2.1869433182234768 - }, - { - "x": 3.5778386039995205, - "y": 7.008182862001031, - "heading": -0.0018831098932495121, - "angularVelocity": -2.1318702314461877, - "velocityX": 2.2844228957089365, - "velocityY": -0.5429695231956211, - "timestamp": 2.251644499372309 - }, - { - "x": 3.75075226330582, - "y": 6.966825747454083, - "heading": -0.14853260580564892, - "angularVelocity": -2.266565977753341, - "velocityX": 2.672496177597522, - "velocityY": -0.6392018478273882, - "timestamp": 2.3163456805211413 - }, - { - "x": 3.9486334270649626, - "y": 6.918918967044097, - "heading": -0.28405555900160556, - "angularVelocity": -2.094597823248583, - "velocityX": 3.0583856468393638, - "velocityY": -0.7404313114436918, - "timestamp": 2.3810468616699736 - }, - { - "x": 4.166070195160817, - "y": 6.865183550075389, - "heading": -0.3449865942152846, - "angularVelocity": -0.9417298746605423, - "velocityX": 3.360630582549694, - "velocityY": -0.8305167852361092, - "timestamp": 2.445748042818806 - }, - { - "x": 4.4009794486877345, - "y": 6.8112911031395695, - "heading": -0.3449866586118039, - "angularVelocity": -9.952912480962177e-7, - "velocityX": 3.6306795232463367, - "velocityY": -0.8329437883344079, - "timestamp": 2.510449223967638 - }, - { - "x": 4.635888809481111, - "y": 6.757399123762019, - "heading": -0.34498672300630057, - "angularVelocity": -9.952599862485997e-7, - "velocityX": 3.630681181121147, - "velocityY": -0.8329365619088571, - "timestamp": 2.5751504051164704 - }, - { - "x": 4.87079817027739, - "y": 6.703507144397122, - "heading": -0.3449867874007968, - "angularVelocity": -9.952599802438338e-7, - "velocityX": 3.630681181166014, - "velocityY": -0.8329365617132907, - "timestamp": 2.6398515862653027 - }, - { - "x": 5.105707531073669, - "y": 6.649615165032224, - "heading": -0.3449868517952927, - "angularVelocity": -9.952599738581064e-7, - "velocityX": 3.6306811811660147, - "velocityY": -0.8329365617132863, - "timestamp": 2.704552767414135 - }, - { - "x": 5.340616891869949, - "y": 6.595723185667327, - "heading": -0.3449869161897882, - "angularVelocity": -9.952599684354273e-7, - "velocityX": 3.630681181166015, - "velocityY": -0.8329365617132873, - "timestamp": 2.7692539485629672 - }, - { - "x": 5.575526252666228, - "y": 6.54183120630243, - "heading": -0.34498698058428334, - "angularVelocity": -9.952599624061458e-7, - "velocityX": 3.6306811811660156, - "velocityY": -0.8329365617132882, - "timestamp": 2.8339551297117995 - }, - { - "x": 5.8104356134625075, - "y": 6.4879392269375336, - "heading": -0.344987044978778, - "angularVelocity": -9.952599561054567e-7, - "velocityX": 3.6306811811660156, - "velocityY": -0.8329365617132889, - "timestamp": 2.898656310860632 - }, - { - "x": 6.0453449742587875, - "y": 6.434047247572637, - "heading": -0.3449871093732723, - "angularVelocity": -9.952599496144382e-7, - "velocityX": 3.6306811811660165, - "velocityY": -0.8329365617132901, - "timestamp": 2.963357492009464 - }, - { - "x": 6.280254335055067, - "y": 6.38015526820774, - "heading": -0.3449871737677662, - "angularVelocity": -9.952599439036616e-7, - "velocityX": 3.630681181166016, - "velocityY": -0.832936561713291, - "timestamp": 3.0280586731582964 - }, - { - "x": 6.515163695851346, - "y": 6.326263288842841, - "heading": -0.3449872381622598, - "angularVelocity": -9.952599382292206e-7, - "velocityX": 3.6306811811660133, - "velocityY": -0.8329365617133051, - "timestamp": 3.0927598543071286 - }, - { - "x": 6.750073056640409, - "y": 6.2723713094464895, - "heading": -0.34498730255675286, - "angularVelocity": -9.952599312963826e-7, - "velocityX": 3.6306811810544866, - "velocityY": -0.832936562199441, - "timestamp": 3.157461035455961 - }, - { - "x": 6.984982150775984, - "y": 6.2184781677494305, - "heading": -0.3449873669513136, - "angularVelocity": -9.95260976649588e-7, - "velocityX": 3.6306770597466, - "velocityY": -0.8329545263337457, - "timestamp": 3.222162216604793 + "angularVelocity": 3.837127558436768e-26, + "velocityX": -2.7920148574402887e-24, + "velocityY": -2.697407810456559e-24, + "timestamp": 1.8646575664742369 + }, + { + "x": 3.0864612629057913, + "y": 7.12456423366324, + "heading": 0.4864079011679127, + "angularVelocity": -0.404266786795747, + "velocityX": 0.377507862534246, + "velocityY": -0.08919238526276327, + "timestamp": 1.9293240671266776 + }, + { + "x": 3.135333250268508, + "y": 7.113029742760453, + "heading": 0.43488377787747834, + "angularVelocity": -0.7967668386350167, + "velocityX": 0.7557543220930696, + "velocityY": -0.17836887393645673, + "timestamp": 1.9939905677791183 + }, + { + "x": 3.2087217801138035, + "y": 7.095721914438121, + "heading": 0.3589213764077576, + "angularVelocity": -1.1746793270597933, + "velocityX": 1.1348770863562407, + "velocityY": -0.26764751683960536, + "timestamp": 2.058657068431559 + }, + { + "x": 3.306697290004189, + "y": 7.0726100183064045, + "heading": 0.2598184881364111, + "angularVelocity": -1.5325228251330487, + "velocityX": 1.5150890940731196, + "velocityY": -0.3574013731767354, + "timestamp": 2.1233235690839996 + }, + { + "x": 3.4293598616620984, + "y": 7.043622922718144, + "heading": 0.13971196471311126, + "angularVelocity": -1.85732214069893, + "velocityX": 1.8968487612647713, + "velocityY": -0.44825520626292115, + "timestamp": 2.1879900697364403 + }, + { + "x": 3.5768625085702324, + "y": 7.008640009161994, + "heading": 0.002547090154321883, + "angularVelocity": -2.1211117529925048, + "velocityX": 2.2809746224077885, + "velocityY": -0.540974278849118, + "timestamp": 2.252656570388881 + }, + { + "x": 3.74940588673651, + "y": 6.967477445012277, + "heading": -0.14404002070042093, + "angularVelocity": -2.266816812039918, + "velocityX": 2.6682034194742714, + "velocityY": -0.6365361312954025, + "timestamp": 2.3173230710413217 + }, + { + "x": 3.946923390754574, + "y": 6.9198227973610145, + "heading": -0.2817650462278063, + "angularVelocity": -2.129773903610591, + "velocityX": 3.0544022333858827, + "velocityY": -0.7369294328664762, + "timestamp": 2.3819895716937625 + }, + { + "x": 4.163931894345135, + "y": 6.866332868698949, + "heading": -0.34498660423093713, + "angularVelocity": -0.9776554686780423, + "velocityX": 3.355810217053556, + "velocityY": -0.8271659688152148, + "timestamp": 2.446656072346203 + }, + { + "x": 4.399029324107109, + "y": 6.812427188093308, + "heading": -0.34498666703392, + "angularVelocity": -9.711826405660571e-7, + "velocityX": 3.635536597620139, + "velocityY": -0.8335951390870039, + "timestamp": 2.511322572998644 + }, + { + "x": 4.63412689461735, + "y": 6.758522121329691, + "heading": -0.3449867298342717, + "angularVelocity": -9.711419520664826e-7, + "velocityX": 3.6355387741453202, + "velocityY": -0.8335856466601779, + "timestamp": 2.5759890736510846 + }, + { + "x": 4.869224465131426, + "y": 6.7046170545827986, + "heading": -0.344986792634623, + "angularVelocity": -9.711419462634673e-7, + "velocityX": 3.6355387742046146, + "velocityY": -0.8335856464015791, + "timestamp": 2.6406555743035254 + }, + { + "x": 5.1043220356455015, + "y": 6.650711987835906, + "heading": -0.344986855434974, + "angularVelocity": -9.711419414707099e-7, + "velocityX": 3.6355387742046164, + "velocityY": -0.8335856464015731, + "timestamp": 2.705322074955966 + }, + { + "x": 5.339419606159577, + "y": 6.596806921089014, + "heading": -0.3449869182353246, + "angularVelocity": -9.711419349644554e-7, + "velocityX": 3.635538774204617, + "velocityY": -0.833585646401574, + "timestamp": 2.769988575608407 + }, + { + "x": 5.574517176673654, + "y": 6.542901854342122, + "heading": -0.3449869810356748, + "angularVelocity": -9.711419302688624e-7, + "velocityX": 3.635538774204617, + "velocityY": -0.8335856464015745, + "timestamp": 2.8346550762608476 + }, + { + "x": 5.809614747187731, + "y": 6.488996787595229, + "heading": -0.3449870438360248, + "angularVelocity": -9.711419254509242e-7, + "velocityX": 3.6355387742046172, + "velocityY": -0.8335856464015756, + "timestamp": 2.8993215769132883 + }, + { + "x": 6.044712317701808, + "y": 6.435091720848337, + "heading": -0.3449871066363744, + "angularVelocity": -9.711419195368686e-7, + "velocityX": 3.6355387742046172, + "velocityY": -0.8335856464015763, + "timestamp": 2.963988077565729 + }, + { + "x": 6.279809888215884, + "y": 6.381186654101444, + "heading": -0.34498716943672364, + "angularVelocity": -9.71141914262753e-7, + "velocityX": 3.6355387742046177, + "velocityY": -0.8335856464015771, + "timestamp": 3.0286545782181697 + }, + { + "x": 6.51490745872996, + "y": 6.32728158735455, + "heading": -0.34498723223707245, + "angularVelocity": -9.71141908290338e-7, + "velocityX": 3.635538774204614, + "velocityY": -0.8335856464015945, + "timestamp": 3.0933210788706105 + }, + { + "x": 6.750005029235043, + "y": 6.273376520568434, + "heading": -0.344987295037421, + "angularVelocity": -9.71141902973265e-7, + "velocityX": 3.6355387740655463, + "velocityY": -0.8335856470081151, + "timestamp": 3.157987579523051 + }, + { + "x": 6.985102269626149, + "y": 6.219470014067963, + "heading": -0.34498735783785006, + "angularVelocity": -9.711431495756142e-7, + "velocityX": 3.6355336691971325, + "velocityY": -0.8336079106893309, + "timestamp": 3.222654080175492 }, { "x": 7.214789390563965, "y": 6.145846366882324, "heading": -0.344987478573796, - "angularVelocity": -0.00000172520007220815, - "velocityX": 3.5518244907979937, - "velocityY": -1.1225730284587905, - "timestamp": 3.2868633977536255 - }, - { - "x": 7.409091443638604, - "y": 6.073965873280571, - "heading": -0.3572291452032454, - "angularVelocity": -0.20035361948729366, - "velocityX": 3.1800506242888202, - "velocityY": -1.1764343450588084, - "timestamp": 3.347963699613786 - }, - { - "x": 7.58146433123023, - "y": 6.0094940876522065, - "heading": -0.36768136673125634, - "angularVelocity": -0.17106661030796205, - "velocityX": 2.8211462520452746, - "velocityY": -1.0551794944633763, - "timestamp": 3.4090640014739466 - }, - { - "x": 7.73220907640328, - "y": 5.953248314526567, - "heading": -0.37521177845828957, - "angularVelocity": -0.1232467188831242, - "velocityX": 2.4671685831938674, - "velocityY": -0.920548203744869, - "timestamp": 3.470164303334107 - }, - { - "x": 7.861444501366057, - "y": 5.905524901339061, - "heading": -0.37940510209885836, - "angularVelocity": -0.06863016241991673, - "velocityX": 2.1151356217282813, - "velocityY": -0.7810667334627726, - "timestamp": 3.5312646051942678 - }, - { - "x": 7.969234018825332, - "y": 5.866476846712542, - "heading": -0.38004587758642694, - "angularVelocity": -0.010487272043845954, - "velocityX": 1.7641405063099576, - "velocityY": -0.6390812064380308, - "timestamp": 3.5923649070544283 - }, - { - "x": 8.055617039018077, - "y": 5.8361975486879665, - "heading": -0.37700219526471646, - "angularVelocity": 0.04981452184436869, - "velocityX": 1.413790399766751, - "velocityY": -0.4955670774569341, - "timestamp": 3.653465208914589 - }, - { - "x": 8.12062042791295, - "y": 5.814749961981242, - "heading": -0.37018491999207215, - "angularVelocity": 0.11157514881427125, - "velocityX": 1.0638799959392298, - "velocityY": -0.3510225981503441, - "timestamp": 3.7145655107747495 - }, - { - "x": 8.164263678264032, - "y": 5.802179408572721, - "heading": -0.3595296539883727, - "angularVelocity": 0.17438974406519217, - "velocityX": 0.7142886208805774, - "velocityY": -0.20573635523587247, - "timestamp": 3.77566581263491 + "angularVelocity": -0.0000018670555043319872, + "velocityX": 3.551871813387612, + "velocityY": -1.1385129308502273, + "timestamp": 3.2873205808279327 + }, + { + "x": 7.409712349690949, + "y": 6.073735935167582, + "heading": -0.3570370868218633, + "angularVelocity": -0.19637949309616867, + "velocityX": 3.176773146322184, + "velocityY": -1.1752257613319905, + "timestamp": 3.348679371698049 + }, + { + "x": 7.582537346896194, + "y": 6.009055207663653, + "heading": -0.36748967160875157, + "angularVelocity": -0.17035187034591742, + "velocityX": 2.816629773084668, + "velocityY": -1.0541395387149, + "timestamp": 3.4100381625681653 + }, + { + "x": 7.733574876390023, + "y": 5.952649624668501, + "heading": -0.37509136643476676, + "angularVelocity": -0.12388925398002781, + "velocityX": 2.4615467050767728, + "velocityY": -0.9192746824909085, + "timestamp": 3.4713969534382816 + }, + { + "x": 7.862949786289389, + "y": 5.90483023458532, + "heading": -0.37937521943526015, + "angularVelocity": -0.06981645074397583, + "velocityX": 2.1084983596437845, + "velocityY": -0.7793404890328669, + "timestamp": 3.532755744308398 + }, + { + "x": 7.9707292244718095, + "y": 5.86575856751758, + "heading": -0.3800974258169581, + "angularVelocity": -0.011770218602036829, + "velocityX": 1.7565443623321622, + "velocityY": -0.636773745272234, + "timestamp": 3.594114535178514 + }, + { + "x": 8.056955102704071, + "y": 5.835533544523746, + "heading": -0.37710820320869415, + "angularVelocity": 0.04871710419769366, + "velocityX": 1.4052734255273058, + "velocityY": -0.49259482732986287, + "timestamp": 3.6554733260486305 + }, + { + "x": 8.121656070762867, + "y": 5.8142219773756105, + "heading": -0.3703061215705459, + "angularVelocity": 0.11085749151326833, + "velocityX": 1.0544694108421286, + "velocityY": -0.34732703897714834, + "timestamp": 3.716832116918747 + }, + { + "x": 8.164852955153219, + "y": 5.801872031339644, + "heading": -0.3596178039887861, + "angularVelocity": 0.1741937451861573, + "velocityX": 0.7040048178555333, + "velocityY": -0.2012742731861952, + "timestamp": 3.778190907788863 }, { "x": 8.186561584472656, "y": 5.798520088195801, "heading": -0.344987478573796, - "angularVelocity": 0.23800496841831012, - "velocityX": 0.3649393788537573, - "velocityY": -0.05989038131588724, - "timestamp": 3.8367661144950707 - }, - { - "x": 8.18566399540182, - "y": 5.805056139735724, - "heading": -0.32461060520225765, - "angularVelocity": 0.30765327861825625, - "velocityX": -0.013551942707795956, - "velocityY": 0.09868234683560065, - "timestamp": 3.9029993542741352 - }, - { - "x": 8.159694473473342, - "y": 5.822092590454781, - "heading": -0.2998178925011234, - "angularVelocity": 0.3743243239170494, - "velocityX": -0.3920919770058792, - "velocityY": 0.25721904554096775, - "timestamp": 3.9692325940532 - }, - { - "x": 8.108649030585239, - "y": 5.849626323109156, - "heading": -0.2708591800534793, - "angularVelocity": 0.4372232514103506, - "velocityX": -0.7706922243027172, - "velocityY": 0.41570867960287095, - "timestamp": 4.035465833832265 - }, - { - "x": 8.03252261804608, - "y": 5.887653107737229, - "heading": -0.2380618973255709, - "angularVelocity": 0.4951785966881702, - "velocityX": -1.1493686975466637, - "velocityY": 0.5741344490307231, - "timestamp": 4.1016990736113295 - }, - { - "x": 7.9313086642803885, - "y": 5.936166919703684, - "heading": -0.20187479302243566, - "angularVelocity": 0.5463586625664867, - "velocityX": -1.5281443894834816, - "velocityY": 0.7324692575553122, - "timestamp": 4.167932313390394 - }, - { - "x": 7.804998346649701, - "y": 5.9951585907413225, - "heading": -0.16295187123346636, - "angularVelocity": 0.5876644705710469, - "velocityX": -1.9070532870205719, - "velocityY": 0.8906656421219618, - "timestamp": 4.234165553169459 - }, - { - "x": 7.653579505512198, - "y": 6.0646127178391085, - "heading": -0.12233575906363518, - "angularVelocity": 0.6132285285351425, - "velocityX": -2.286145772765958, - "velocityY": 1.048629469575472, - "timestamp": 4.300398792948523 - }, - { - "x": 7.4770359425587705, - "y": 6.144498725543435, - "heading": -0.08194804569131871, - "angularVelocity": 0.6097801271240603, - "velocityX": -2.6654828231614, - "velocityY": 1.206131663962129, - "timestamp": 4.366632032727588 - }, - { - "x": 7.27536197665765, - "y": 6.234730628199731, - "heading": -0.046490249768474004, - "angularVelocity": 0.5353474485186261, - "velocityX": -3.044905648188839, - "velocityY": 1.362335633245254, - "timestamp": 4.432865272506652 - }, - { - "x": 7.049560259358204, - "y": 6.334147758763648, - "heading": -0.04649017070950518, - "angularVelocity": 0.0000011936448991960951, - "velocityX": -3.4091902804793617, - "velocityY": 1.5010156666885635, - "timestamp": 4.499098512285717 - }, - { - "x": 6.822106687595601, - "y": 6.429725315002885, - "heading": -0.04649016173832054, - "angularVelocity": 1.354483741042957e-7, - "velocityX": -3.4341302421763333, - "velocityY": 1.4430451621882883, - "timestamp": 4.565331752064782 - }, - { - "x": 6.586112566537963, - "y": 6.501676487205841, - "heading": -0.04649015261662469, - "angularVelocity": 1.3772081626072917e-7, - "velocityX": -3.563076815279552, - "velocityY": 1.086330254158866, - "timestamp": 4.631564991843846 - }, - { - "x": 6.343631547831337, - "y": 6.547208357601844, - "heading": -0.046490142857452815, - "angularVelocity": 1.4734553082512417e-7, - "velocityX": -3.6610170288434327, - "velocityY": 0.6874474289327321, - "timestamp": 4.697798231622911 - }, - { - "x": 6.097611695820765, - "y": 6.565767095818739, - "heading": -0.046490131800601416, - "angularVelocity": 1.6693810300562863e-7, - "velocityX": -3.7144468975279485, - "velocityY": 0.28020278456559833, - "timestamp": 4.764031471401975 + "angularVelocity": 0.2384389458710078, + "velocityX": 0.35379819275433294, + "velocityY": -0.054628572309033785, + "timestamp": 3.8395496986589794 + }, + { + "x": 8.185020658343282, + "y": 5.805387130671382, + "heading": -0.32459673237759007, + "angularVelocity": 0.30843585413704155, + "velocityX": -0.023308458763729726, + "velocityY": 0.10387271220830593, + "timestamp": 3.9056598668599376 + }, + { + "x": 8.158545676834562, + "y": 5.822731510098044, + "heading": -0.29984074692215335, + "angularVelocity": 0.37446562501830083, + "velocityX": -0.40046761684590937, + "velocityY": 0.2623556995640975, + "timestamp": 3.9717700350608958 + }, + { + "x": 8.10713245572277, + "y": 5.850551569484812, + "heading": -0.2710499624922478, + "angularVelocity": 0.43549706820271483, + "velocityX": -0.7776900666098594, + "velocityY": 0.42081362283335544, + "timestamp": 4.037880203261854 + }, + { + "x": 8.030775877182073, + "y": 5.8888449452190175, + "heading": -0.23865422460782199, + "angularVelocity": 0.49002655364528946, + "velocityX": -1.154989929969518, + "velocityY": 0.5792357934683214, + "timestamp": 4.103990371462812 + }, + { + "x": 7.929469606523289, + "y": 5.937608095309845, + "heading": -0.2032366176335679, + "angularVelocity": 0.5357361497945922, + "velocityX": -1.5323856135237626, + "velocityY": 0.7376043870074612, + "timestamp": 4.17010053966377 + }, + { + "x": 7.803205805922035, + "y": 5.996835346003344, + "heading": -0.16563448060225386, + "angularVelocity": 0.5687799328692251, + "velocityX": -1.909899854095725, + "velocityY": 0.8958871578342301, + "timestamp": 4.236210707864728 + }, + { + "x": 7.651975275778253, + "y": 6.066516654618355, + "heading": -0.12715303116567858, + "angularVelocity": 0.5820806463477988, + "velocityX": -2.2875532502667433, + "velocityY": 1.0540180203928478, + "timestamp": 4.302320876065687 + }, + { + "x": 7.4757708248918915, + "y": 6.146630988469932, + "heading": -0.09010777903671587, + "angularVelocity": 0.5603563436165322, + "velocityX": -2.665315422443717, + "velocityY": 1.211830736960907, + "timestamp": 4.368431044266645 + }, + { + "x": 7.274619001255966, + "y": 6.237116150274765, + "heading": -0.05972677615109367, + "angularVelocity": 0.45955113581426293, + "velocityX": -3.0426760226123557, + "velocityY": 1.368702640867323, + "timestamp": 4.434541212467603 + }, + { + "x": 7.049305084903872, + "y": 6.337299858985659, + "heading": -0.05972649238552697, + "angularVelocity": 0.000004292313488445514, + "velocityX": -3.408158267986829, + "velocityY": 1.515405442720417, + "timestamp": 4.500651380668561 + }, + { + "x": 6.822055990090289, + "y": 6.433013154965271, + "heading": -0.05972648235247219, + "angularVelocity": 1.5176265701728567e-7, + "velocityX": -3.4374302924597684, + "velocityY": 1.4477847899080587, + "timestamp": 4.566761548869519 + }, + { + "x": 6.585901540176483, + "y": 6.503965686132618, + "heading": -0.05972647213388985, + "angularVelocity": 1.5456899617029753e-7, + "velocityX": -3.5721350639429144, + "velocityY": 1.0732468710663359, + "timestamp": 4.632871717070477 + }, + { + "x": 6.343393108903057, + "y": 6.54860698900868, + "heading": -0.05972646117550161, + "angularVelocity": 1.65759497255032e-7, + "velocityX": -3.6682470771555504, + "velocityY": 0.6752562289716734, + "timestamp": 4.698981885271436 + }, + { + "x": 6.097452808494555, + "y": 6.566398909819071, + "heading": -0.059726448733939674, + "angularVelocity": 1.8819437727240375e-7, + "velocityX": -3.720158443114319, + "velocityY": 0.26912532965138075, + "timestamp": 4.765092053472394 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -0.04649011830549673, - "angularVelocity": 2.0375123930314497e-7, - "velocityX": -3.722715642994527, - "velocityY": -0.13044955895548105, - "timestamp": 4.83026471118104 - }, - { - "x": 5.59400062961639, - "y": 6.517937013895863, - "heading": -0.046490107217199095, - "angularVelocity": 1.588527062833638e-7, - "velocityX": -3.682446546399627, - "velocityY": -0.5614419262393832, - "timestamp": 4.900067095317195 - }, - { - "x": 5.343239064324899, - "y": 6.449191978638034, - "heading": -0.046490097789975614, - "angularVelocity": 1.3505589536533548e-7, - "velocityX": -3.5924498624912715, - "velocityY": -0.984852252664273, - "timestamp": 4.96986947945335 - }, - { - "x": 5.10214573961629, - "y": 6.351820274741271, - "heading": -0.04649007357112031, - "angularVelocity": 3.469631532168612e-7, - "velocityX": -3.453941118090443, - "velocityY": -1.3949624371974614, - "timestamp": 5.039671863589505 - }, - { - "x": 4.887305900136772, - "y": 6.241894655501948, - "heading": -0.013339477589168081, - "angularVelocity": 0.4749206834724951, - "velocityX": -3.0778295345966584, - "velocityY": -1.574811814807135, - "timestamp": 5.10947424772566 - }, - { - "x": 4.700625263351654, - "y": 6.142924243380359, - "heading": 0.02019201613120645, - "angularVelocity": 0.4803774847427688, - "velocityX": -2.6744163411522406, - "velocityY": -1.417865784190692, - "timestamp": 5.179276631861815 - }, - { - "x": 4.541203570947359, - "y": 6.05696873798045, - "heading": 0.05079450877042205, - "angularVelocity": 0.4384161517968095, - "velocityX": -2.283900390756435, - "velocityY": -1.2314121711408288, - "timestamp": 5.24907901599797 - }, - { - "x": 4.408671715563658, - "y": 5.984755616609754, - "heading": 0.07726906366461818, - "angularVelocity": 0.3792786624960486, - "velocityX": -1.8986723308073217, - "velocityY": -1.034536603074175, - "timestamp": 5.318881400134125 - }, - { - "x": 4.302830646527999, - "y": 5.92665565579208, - "heading": 0.09900038404891696, - "angularVelocity": 0.3113263343829381, - "velocityX": -1.5162959022890692, - "velocityY": -0.8323492318592651, - "timestamp": 5.38868378427028 - }, - { - "x": 4.22355609558814, - "y": 5.882893451015234, - "heading": 0.11561440292356019, - "angularVelocity": 0.23801506324248706, - "velocityX": -1.1356997604154628, - "velocityY": -0.6269442701482041, - "timestamp": 5.458486168406435 - }, - { - "x": 4.170763147512419, - "y": 5.853619589907405, - "heading": 0.1268589116332185, - "angularVelocity": 0.1610906109986881, - "velocityX": -0.756320127586832, - "velocityY": -0.4193819662481466, - "timestamp": 5.52828855254259 + "heading": -0.05972643351340637, + "angularVelocity": 2.3022983790495482e-7, + "velocityX": -3.7272425285094304, + "velocityY": -0.14024939234037223, + "timestamp": 4.831202221673352 + }, + { + "x": 5.593733281594629, + "y": 6.517293519804324, + "heading": -0.059726421475485736, + "angularVelocity": 1.7244301744192055e-7, + "velocityX": -3.6859744116289104, + "velocityY": -0.5706139410489331, + "timestamp": 4.901010328751527 + }, + { + "x": 5.342760368520485, + "y": 6.447952318722012, + "heading": -0.059726411223577615, + "angularVelocity": 1.4685841743171806e-7, + "velocityX": -3.595182903227699, + "velocityY": -0.9933115791932222, + "timestamp": 4.970818435829702 + }, + { + "x": 5.101497403322433, + "y": 6.350035089382074, + "heading": -0.05972638349654275, + "angularVelocity": 3.9718932395532394e-7, + "velocityX": -3.456088057621621, + "velocityY": -1.4026627198225616, + "timestamp": 5.0406265429078765 + }, + { + "x": 4.886759838624346, + "y": 6.240596211657777, + "heading": -0.024369209517199585, + "angularVelocity": 0.5064909429466152, + "velocityX": -3.0761121263123843, + "velocityY": -1.5677101457820914, + "timestamp": 5.110434649986051 + }, + { + "x": 4.700188679669267, + "y": 6.141997521730213, + "heading": 0.01157815322069568, + "angularVelocity": 0.5149453873264229, + "velocityX": -2.672628821551406, + "velocityY": -1.4124246316713311, + "timestamp": 5.180242757064226 + }, + { + "x": 4.540876156189366, + "y": 6.0563320226623665, + "heading": 0.04446453476629188, + "angularVelocity": 0.47109688146633416, + "velocityX": -2.2821493111322217, + "velocityY": -1.2271568826800237, + "timestamp": 5.250050864142401 + }, + { + "x": 4.40844486564244, + "y": 5.984344508560839, + "heading": 0.07295632714737806, + "angularVelocity": 0.40814446306615176, + "velocityX": -1.8970760859999023, + "velocityY": -1.031219970209356, + "timestamp": 5.319858971220576 + }, + { + "x": 4.302690180424501, + "y": 5.9264156183199805, + "heading": 0.09636693816788014, + "angularVelocity": 0.3353566226095424, + "velocityX": -1.5149341479709326, + "velocityY": -0.8298304117598572, + "timestamp": 5.389667078298751 + }, + { + "x": 4.223483980380201, + "y": 5.882776245569646, + "heading": 0.11427821119992405, + "angularVelocity": 0.2565786952507661, + "velocityX": -1.1346275290862768, + "velocityY": -0.6251333058131043, + "timestamp": 5.459475185376926 + }, + { + "x": 4.170738558104569, + "y": 5.853581333825438, + "heading": 0.12640786347483376, + "angularVelocity": 0.17375707181583125, + "velocityX": -0.7555773173532012, + "velocityY": -0.41821663652209046, + "timestamp": 5.5292832924551005 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 0.08154784945777828, - "velocityX": -0.37782436284418364, - "velocityY": -0.21027274576950902, - "timestamp": 5.598090936678745 + "angularVelocity": 0.08800242162180778, + "velocityX": -0.37744114553759195, + "velocityY": -0.20970748963591992, + "timestamp": 5.599091399533275 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 1.1000727380078476e-26, - "velocityX": 7.869680140840765e-27, - "velocityY": 7.286496207304452e-27, - "timestamp": 5.6678933208149 - }, - { - "x": 4.195350058507561, - "y": 5.858786392293566, - "heading": 0.13540679339101, - "angularVelocity": 0.03040787160499198, - "velocityX": 0.5426383042255808, - "velocityY": 0.2113090624413005, - "timestamp": 5.7618047751106225 - }, - { - "x": 4.297269963634785, - "y": 5.898475069782893, - "heading": 0.14111775296085857, - "angularVelocity": 0.06081217262236255, - "velocityX": 1.085276613929165, - "velocityY": 0.42261806919047107, - "timestamp": 5.855716229406345 - }, - { - "x": 4.450149823252348, - "y": 5.958008075795516, - "heading": 0.14968325491595807, - "angularVelocity": 0.09120827719403884, - "velocityX": 1.6279149414101537, - "velocityY": 0.6339269949452273, - "timestamp": 5.949627683702067 - }, - { - "x": 4.653989639578877, - "y": 6.037385400048414, - "heading": 0.16110213512106505, - "angularVelocity": 0.12159198567141313, - "velocityX": 2.1705532925158373, - "velocityY": 0.8452358112030016, - "timestamp": 6.043539137997789 - }, - { - "x": 4.908789413625767, - "y": 6.136607028819001, - "heading": 0.17537288795521153, - "angularVelocity": 0.15195966180236836, - "velocityX": 2.7131916543911667, - "velocityY": 1.0565444813381684, - "timestamp": 6.137450592293511 - }, - { - "x": 5.214549136636689, - "y": 6.255672941521673, - "heading": 0.19249371968896806, - "angularVelocity": 0.1823082377134106, - "velocityX": 3.2558299230262406, - "velocityY": 1.2678529322710714, - "timestamp": 6.231362046589234 - }, - { - "x": 5.540526297381237, - "y": 6.382610310333338, - "heading": 0.1924937215917483, - "angularVelocity": 2.0261428816305725e-8, - "velocityX": 3.4711118381583637, - "velocityY": 1.3516707814144493, - "timestamp": 6.325273500884956 - }, - { - "x": 5.866503458135056, - "y": 6.50954767912126, - "heading": 0.19249372349451602, - "angularVelocity": 2.0261295290604937e-8, - "velocityX": 3.471111838257083, - "velocityY": 1.351670781161622, - "timestamp": 6.419184955180678 - }, - { - "x": 6.192480618888875, - "y": 6.636485047909182, - "heading": 0.19249372539728374, - "angularVelocity": 2.0261295568537564e-8, - "velocityX": 3.471111838257084, - "velocityY": 1.3516707811616195, - "timestamp": 6.5130964094764 - }, - { - "x": 6.518457779642694, - "y": 6.763422416697105, - "heading": 0.19249372730005146, - "angularVelocity": 2.0261295333491658e-8, - "velocityX": 3.4711118382570834, - "velocityY": 1.35167078116162, - "timestamp": 6.607007863772123 - }, - { - "x": 6.844434940394272, - "y": 6.890359785490778, - "heading": 0.19249372920281918, - "angularVelocity": 2.0261295628050687e-8, - "velocityX": 3.471111838233232, - "velocityY": 1.3516707812228705, - "timestamp": 6.700919318067845 - }, - { - "x": 7.170411838536006, - "y": 7.017297824914972, - "heading": 0.1924937316314852, - "angularVelocity": 2.586123310059887e-8, - "velocityX": 3.4711090418773667, - "velocityY": 1.351677922316834, - "timestamp": 6.794830772363567 - }, - { - "x": 7.445544445617428, - "y": 7.124436352045788, - "heading": 0.20746316204367837, - "angularVelocity": 0.15939941005551134, - "velocityX": 2.929702336575951, - "velocityY": 1.1408462144931066, - "timestamp": 6.888742226659289 - }, - { - "x": 7.669715374136242, - "y": 7.211730123643104, - "heading": 0.22019672303892965, - "angularVelocity": 0.13559113838397205, - "velocityX": 2.38704565060736, - "velocityY": 0.9295327417934781, - "timestamp": 6.9826536809550115 - }, - { - "x": 7.842925396345479, - "y": 7.279179365267254, - "heading": 0.23039470264394565, - "angularVelocity": 0.10859143521410211, - "velocityX": 1.8443971878425902, - "velocityY": 0.7182216709343664, - "timestamp": 7.076565135250734 - }, - { - "x": 7.96517479602962, - "y": 7.326784165564593, - "heading": 0.23795514608622043, - "angularVelocity": 0.08050608415100667, - "velocityX": 1.3017517469081525, - "velocityY": 0.506911544010745, - "timestamp": 7.170476589546456 - }, - { - "x": 8.0364637201711, - "y": 7.354544571992242, - "heading": 0.24282665608297047, - "angularVelocity": 0.05187343794517228, - "velocityX": 0.7591078710909263, - "velocityY": 0.2956019224261278, - "timestamp": 7.264388043842178 + "angularVelocity": 6.956965199088368e-27, + "velocityX": 5.711893177801191e-27, + "velocityY": 1.0709622806033376e-27, + "timestamp": 5.66889950661145 + }, + { + "x": 4.195152179176686, + "y": 5.858709351822529, + "heading": 0.13547549704031217, + "angularVelocity": 0.03117959203710152, + "velocityX": 0.5412280111334526, + "velocityY": 0.210760048168702, + "timestamp": 5.762690056862549 + }, + { + "x": 4.296676325767576, + "y": 5.898243947995512, + "heading": 0.1413238484362874, + "angularVelocity": 0.06235544391538225, + "velocityX": 1.0824560290891385, + "velocityY": 0.42152003658300596, + "timestamp": 5.856480607113649 + }, + { + "x": 4.448962547900073, + "y": 5.95754583130188, + "heading": 0.15009539524373325, + "angularVelocity": 0.0935227140043684, + "velocityX": 1.6236840675824051, + "velocityY": 0.6322799380918802, + "timestamp": 5.950271157364748 + }, + { + "x": 4.6520108481753235, + "y": 6.036614990735129, + "heading": 0.16178892165721923, + "angularVelocity": 0.1246770211090534, + "velocityX": 2.1649121338092594, + "velocityY": 0.8430397222488009, + "timestamp": 6.044061707615847 + }, + { + "x": 4.905821228102917, + "y": 6.135451411658796, + "heading": 0.17640285839515626, + "angularVelocity": 0.15581459644721318, + "velocityX": 2.706140216131426, + "velocityY": 1.0537993503509646, + "timestamp": 6.137852257866946 + }, + { + "x": 5.210393679513956, + "y": 6.254055072364673, + "heading": 0.19393534293153245, + "angularVelocity": 0.18693231343069955, + "velocityX": 3.2473682113563522, + "velocityY": 1.2645587469989996, + "timestamp": 6.231642808118045 + }, + { + "x": 5.53637764515154, + "y": 6.380995104775572, + "heading": 0.1939353448909336, + "angularVelocity": 2.0891242641251563e-8, + "velocityX": 3.475658952472825, + "velocityY": 1.3534416001510998, + "timestamp": 6.325433358369144 + }, + { + "x": 5.862361610799536, + "y": 6.507935137159803, + "heading": 0.19393534685032104, + "angularVelocity": 2.089109662304858e-8, + "velocityX": 3.47565895258384, + "velocityY": 1.353441599866765, + "timestamp": 6.419223908620244 + }, + { + "x": 6.188345576447532, + "y": 6.634875169544034, + "heading": 0.19393534880970845, + "angularVelocity": 2.0891096217109697e-8, + "velocityX": 3.4756589525838413, + "velocityY": 1.3534415998667624, + "timestamp": 6.513014458871343 + }, + { + "x": 6.5143295420955285, + "y": 6.761815201928266, + "heading": 0.19393535076909593, + "angularVelocity": 2.0891097152342323e-8, + "velocityX": 3.475658952583841, + "velocityY": 1.3534415998667637, + "timestamp": 6.606805009122442 + }, + { + "x": 6.840313507740267, + "y": 6.888755234320864, + "heading": 0.1939353527284834, + "angularVelocity": 2.0891096624592066e-8, + "velocityX": 3.475658952549103, + "velocityY": 1.353441599955972, + "timestamp": 6.700595559373541 + }, + { + "x": 7.1662970943707105, + "y": 7.015696232741894, + "heading": 0.19393535571019038, + "angularVelocity": 3.179112387470943e-8, + "velocityX": 3.475654911477867, + "velocityY": 1.353451899804194, + "timestamp": 6.79438610962464 + }, + { + "x": 7.441622589341096, + "y": 7.122909656814238, + "heading": 0.20815324472161514, + "angularVelocity": 0.15159191382671508, + "velocityX": 2.9355355548429496, + "velocityY": 1.143115418187742, + "timestamp": 6.8881766598757395 + }, + { + "x": 7.66618318620661, + "y": 7.210355021181115, + "heading": 0.2205253443109958, + "angularVelocity": 0.13191200559392907, + "velocityX": 2.3942774220250835, + "velocityY": 0.9323472794728952, + "timestamp": 6.981967210126839 + }, + { + "x": 7.839980071520317, + "y": 7.278032678531449, + "heading": 0.23053454012619146, + "angularVelocity": 0.10671859572631527, + "velocityX": 1.8530319403011595, + "velocityY": 0.7215829011467113, + "timestamp": 7.075757760377938 + }, + { + "x": 7.963013724050306, + "y": 7.325942779672703, + "heading": 0.23800124017348176, + "angularVelocity": 0.0796103661541618, + "velocityX": 1.311791563228909, + "velocityY": 0.5108201307379786, + "timestamp": 7.169548310629037 + }, + { + "x": 8.03528439999784, + "y": 7.354085407438329, + "heading": 0.24283421360763968, + "angularVelocity": 0.05152942829761583, + "velocityX": 0.7705539177886093, + "velocityY": 0.3000582435040803, + "timestamp": 7.263338860880136 }, { "x": 8.056792259216309, "y": 7.362460613250732, "heading": 0.244978130412332, - "angularVelocity": 0.022909605068905204, - "velocityX": 0.21646495837660984, - "velocityY": 0.0842926064542015, - "timestamp": 7.3582994981379 - }, - { - "x": 8.02239522092899, - "y": 7.349066085390599, - "heading": 0.24423351408772678, - "angularVelocity": -0.00758829012549376, - "velocityX": -0.3505358361841942, - "velocityY": -0.13650192741958841, - "timestamp": 7.4564265217876144 - }, - { - "x": 7.932360084343089, - "y": 7.314005643971181, - "heading": 0.24049622236652166, - "angularVelocity": -0.03808626392813261, - "velocityX": -0.9175366095613122, - "velocityY": -0.35729649300863325, - "timestamp": 7.5545535454373285 - }, - { - "x": 7.786686853677988, - "y": 7.257279283570004, - "heading": 0.23376594617518753, - "angularVelocity": -0.06858738745974144, - "velocityX": -1.4845373399392365, - "velocityY": -0.578091113857428, - "timestamp": 7.6526805690870425 - }, - { - "x": 7.5853755359232595, - "y": 7.178886996735875, - "heading": 0.2240420388382404, - "angularVelocity": -0.09909510117884326, - "velocityX": -2.0515379990873206, - "velocityY": -0.7988858106403784, - "timestamp": 7.750807592736757 - }, - { - "x": 7.328426142666821, - "y": 7.078828774757454, - "heading": 0.21132346369891478, - "angularVelocity": -0.1296133793350078, - "velocityX": -2.618538540144425, - "velocityY": -1.0196805961994813, - "timestamp": 7.848934616386471 - }, - { - "x": 7.01583869900668, - "y": 6.957104611217254, - "heading": 0.19560873409309495, - "angularVelocity": -0.1601468078958253, - "velocityX": -3.1855388254309114, - "velocityY": -1.2404754471583816, - "timestamp": 7.947061640036185 - }, - { - "x": 6.675228781735299, - "y": 6.824469292432169, - "heading": 0.19560873219026645, - "angularVelocity": -1.939148261699155e-8, - "velocityX": -3.471112284901898, - "velocityY": -1.3516696405524002, - "timestamp": 8.045188663685899 - }, - { - "x": 6.334618864455473, - "y": 6.691833973668682, - "heading": 0.19560873028745154, - "angularVelocity": -1.9391344572203613e-8, - "velocityX": -3.4711122849879685, - "velocityY": -1.3516696403322965, - "timestamp": 8.143315687335612 - }, - { - "x": 5.994008947175647, - "y": 6.559198654905194, - "heading": 0.19560872838463667, - "angularVelocity": -1.9391344282542406e-8, - "velocityX": -3.471112284987969, - "velocityY": -1.3516696403322948, - "timestamp": 8.241442710985325 - }, - { - "x": 5.65339902989582, - "y": 6.426563336141707, - "heading": 0.19560872648182165, - "angularVelocity": -1.9391345115512158e-8, - "velocityX": -3.4711122849879685, - "velocityY": -1.3516696403322956, - "timestamp": 8.339569734635038 - }, - { - "x": 5.312789112621013, - "y": 6.293928017365348, - "heading": 0.195608724578999, - "angularVelocity": -1.939142318059707e-8, - "velocityX": -3.4711122849368206, - "velocityY": -1.3516696404634736, - "timestamp": 8.437696758284751 - }, - { - "x": 4.978960833752885, - "y": 6.1639320605180385, - "heading": 0.17759686591379087, - "angularVelocity": -0.18355655756466713, - "velocityX": -3.402001471682276, - "velocityY": -1.324772239208634, - "timestamp": 8.535823781934464 - }, - { - "x": 4.700770593105711, - "y": 6.055602072933751, - "heading": 0.16258358561165714, - "angularVelocity": -0.15299842738252176, - "velocityX": -2.8350013105486314, - "velocityY": -1.1039771059498866, - "timestamp": 8.633950805584178 - }, - { - "x": 4.478218399750123, - "y": 5.968938070584376, - "heading": 0.1505712213556835, - "angularVelocity": -0.1224164741697909, - "velocityX": -2.2680010569772926, - "velocityY": -0.8831818099236476, - "timestamp": 8.73207782923389 - }, - { - "x": 4.311304254190931, - "y": 5.9039400626948755, - "heading": 0.1415612155720731, - "angularVelocity": -0.09181982137533895, - "velocityX": -1.701000798261533, - "velocityY": -0.6623864198869991, - "timestamp": 8.830204852883604 - }, - { - "x": 4.2000281561602835, - "y": 5.860608055094718, - "heading": 0.13555442251274583, - "angularVelocity": -0.06121446300837455, - "velocityX": -1.1340005422754125, - "velocityY": -0.4415909704429648, - "timestamp": 8.928331876533317 + "angularVelocity": 0.022858558766875252, + "velocityX": 0.2293179767139361, + "velocityY": 0.08929690453869228, + "timestamp": 7.357129411131235 + }, + { + "x": 8.023609935096944, + "y": 7.3495390353004035, + "heading": 0.24423184787745655, + "angularVelocity": -0.007595056263321516, + "velocityX": -0.33770268880325044, + "velocityY": -0.13150530389944523, + "timestamp": 7.455388398161973 + }, + { + "x": 7.934712736835736, + "y": 7.314921652906921, + "heading": 0.24049321600439108, + "angularVelocity": -0.03804875244537132, + "velocityX": -0.9047233331786764, + "velocityY": -0.3523075439669781, + "timestamp": 7.55364738519271 + }, + { + "x": 7.790100668647838, + "y": 7.258608460656236, + "heading": 0.2337619254421963, + "angularVelocity": -0.06850559694950989, + "velocityX": -1.4717439346556647, + "velocityY": -0.5731098391342927, + "timestamp": 7.651906372223448 + }, + { + "x": 7.589773737515311, + "y": 7.180599451109558, + "heading": 0.22403732916629288, + "angularVelocity": -0.09896902634322269, + "velocityX": -2.038764465074942, + "velocityY": -0.7939122100075828, + "timestamp": 7.7501653592541855 + }, + { + "x": 7.3337319550141755, + "y": 7.080894615571415, + "heading": 0.21131838994573934, + "angularVelocity": -0.12944301182928752, + "velocityX": -2.6057848776829022, + "velocityY": -1.0147146693762836, + "timestamp": 7.848424346284923 + }, + { + "x": 7.021975346220784, + "y": 6.959493947641481, + "heading": 0.19560361962226216, + "angularVelocity": -0.15993214257909347, + "velocityX": -3.1728050350841532, + "velocityY": -1.235517193882298, + "timestamp": 7.946683333315661 + }, + { + "x": 6.680460557220924, + "y": 6.826506302650429, + "heading": 0.19560361772383378, + "angularVelocity": -1.9320659091620353e-8, + "velocityX": -3.4756595739484712, + "velocityY": -1.3534400161224025, + "timestamp": 8.044942320346397 + }, + { + "x": 6.3389457682116035, + "y": 6.69351865768357, + "heading": 0.19560361582542005, + "angularVelocity": -1.9320509691565887e-8, + "velocityX": -3.4756595740447467, + "velocityY": -1.3534400158761875, + "timestamp": 8.143201307377135 + }, + { + "x": 5.997430979202282, + "y": 6.560531012716711, + "heading": 0.19560361392700626, + "angularVelocity": -1.9320510544397003e-8, + "velocityX": -3.4756595740447476, + "velocityY": -1.3534400158761857, + "timestamp": 8.241460294407872 + }, + { + "x": 5.655916190192961, + "y": 6.427543367749852, + "heading": 0.1956036120285926, + "angularVelocity": -1.932050909755801e-8, + "velocityX": -3.475659574044747, + "velocityY": -1.3534400158761868, + "timestamp": 8.33971928143861 + }, + { + "x": 5.314401401188735, + "y": 6.29455572276993, + "heading": 0.19560361013017105, + "angularVelocity": -1.932058966623249e-8, + "velocityX": -3.4756595739929064, + "velocityY": -1.353440016009134, + "timestamp": 8.437978268469347 + }, + { + "x": 4.980112469171968, + "y": 6.164380421516108, + "heading": 0.177592992488041, + "angularVelocity": -0.18329740806808847, + "velocityX": -3.4021206824796124, + "velocityY": -1.3248182704458449, + "timestamp": 8.536237255500085 + }, + { + "x": 4.701538350318157, + "y": 6.055900980257848, + "heading": 0.16258092336836558, + "angularVelocity": -0.15278062163392098, + "velocityX": -2.8351006586976912, + "velocityY": -1.10401546501111, + "timestamp": 8.634496242530822 + }, + { + "x": 4.478679054186577, + "y": 5.969117414975404, + "heading": 0.1505695910339925, + "angularVelocity": -0.12224156484145013, + "velocityX": -2.2680805376292548, + "velocityY": -0.8832124969423502, + "timestamp": 8.73275522956156 + }, + { + "x": 4.311534581449365, + "y": 5.904029734889891, + "heading": 0.14156038794424536, + "angularVelocity": -0.09168833673126392, + "velocityX": -1.701060409720345, + "velocityY": -0.6624094350285984, + "timestamp": 8.831014216592298 + }, + { + "x": 4.200104931923326, + "y": 5.860637945826706, + "heading": 0.1355541433245536, + "angularVelocity": -0.06112666943953846, + "velocityX": -1.1340402836758565, + "velocityY": -0.4416063138286828, + "timestamp": 8.929273203623035 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": -0.030606008971789937, - "velocityX": -0.5670002807557282, - "velocityY": -0.2207954888983711, - "timestamp": 9.02645890018303 + "angularVelocity": -0.03056206326518059, + "velocityX": -0.567020151599215, + "velocityY": -0.22080316059111527, + "timestamp": 9.027532190653773 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 1.2638912367197548e-25, - "velocityX": -6.105559884328861e-26, - "velocityY": -2.4380402560227299e-26, - "timestamp": 9.124585923832743 - }, - { - "x": 4.149229218776726, - "y": 5.815100726386144, - "heading": 0.12918022174703217, - "angularVelocity": -0.05380546988265665, - "velocityX": 0.07724016042252432, - "velocityY": -0.3805465783235216, - "timestamp": 9.187236137776921 - }, - { - "x": 4.159717289478365, - "y": 5.7675956417955225, - "heading": 0.12259706469587767, - "angularVelocity": -0.10507796600695042, - "velocityX": 0.16740678189838681, - "velocityY": -0.7582589364012235, - "timestamp": 9.249886351721099 - }, - { - "x": 4.176867312970838, - "y": 5.696690246498273, - "heading": 0.11300853276979594, - "angularVelocity": -0.15304867010709985, - "velocityX": 0.2737424569332353, - "velocityY": -1.1317662116912757, - "timestamp": 9.312536565665276 - }, - { - "x": 4.201974040585745, - "y": 5.602790592825652, - "heading": 0.10069220099175857, - "angularVelocity": -0.19658882233046115, - "velocityX": 0.40074448328743983, - "velocityY": -1.498792226891454, - "timestamp": 9.375186779609454 - }, - { - "x": 4.2367303910144685, - "y": 5.486551757326023, - "heading": 0.08603276386217494, - "angularVelocity": -0.2339886204162901, - "velocityX": 0.5547682639949261, - "velocityY": -1.8553621477366333, - "timestamp": 9.437836993553631 - }, - { - "x": 4.283393708275681, - "y": 5.349087774330418, - "heading": 0.06958159023755035, - "angularVelocity": -0.2625876687234105, - "velocityX": 0.7448229514872903, - "velocityY": -2.1941502565033004, - "timestamp": 9.500487207497809 - }, - { - "x": 4.3449830971903305, - "y": 5.19239819137041, - "heading": 0.05215018239550137, - "angularVelocity": -0.2782338119001568, - "velocityX": 0.9830674955001105, - "velocityY": -2.5010223125434243, - "timestamp": 9.563137421441986 - }, - { - "x": 4.425324635342363, - "y": 5.020190178284826, - "heading": 0.03492497946542863, - "angularVelocity": -0.2749424438585412, - "velocityX": 1.2823825026937377, - "velocityY": -2.7487218677181704, - "timestamp": 9.625787635386164 - }, - { - "x": 4.528280141254807, - "y": 4.838988810812342, - "heading": 0.019478906624101934, - "angularVelocity": -0.24654461443801812, - "velocityX": 1.6433384569792095, - "velocityY": -2.8922705297373237, - "timestamp": 9.688437849330342 - }, - { - "x": 4.6555610459377, - "y": 4.65789803685685, - "heading": 0.007446897298019998, - "angularVelocity": -0.1920505704386356, - "velocityX": 2.0316116525364616, - "velocityY": -2.8905052761167873, - "timestamp": 9.75108806327452 + "angularVelocity": 5.970204758228253e-26, + "velocityX": -1.332775736754173e-26, + "velocityY": -2.1157650772928713e-26, + "timestamp": 9.12579117768451 + }, + { + "x": 4.149233388012969, + "y": 5.815110549500247, + "heading": 0.12920898299918354, + "angularVelocity": -0.053321361684181494, + "velocityX": 0.07727043393529022, + "velocityY": -0.38021129652575064, + "timestamp": 9.188470802544044 + }, + { + "x": 4.15973054187138, + "y": 5.767625542791529, + "heading": 0.12267986707327648, + "angularVelocity": -0.10416648058342459, + "velocityX": 0.16747314429426893, + "velocityY": -0.7575828160288474, + "timestamp": 9.251150427403578 + }, + { + "x": 4.176895053438104, + "y": 5.696750993270707, + "heading": 0.11316614819920229, + "angularVelocity": -0.1517832771876739, + "velocityX": 0.27384515470838755, + "velocityY": -1.13074303937925, + "timestamp": 9.313830052263112 + }, + { + "x": 4.20202157406695, + "y": 5.602893467398182, + "heading": 0.10093938344413095, + "angularVelocity": -0.19506761220207594, + "velocityX": 0.4008722241901565, + "velocityY": -1.4974168413237998, + "timestamp": 9.376509677122646 + }, + { + "x": 4.236801648856746, + "y": 5.486708244559792, + "heading": 0.08637594484329676, + "angularVelocity": -0.23234725213290444, + "velocityX": 0.5548864542144063, + "velocityY": -1.8536362190865294, + "timestamp": 9.43918930198218 + }, + { + "x": 4.283488644505547, + "y": 5.349308215099438, + "heading": 0.07001524095922855, + "angularVelocity": -0.2610210881244517, + "velocityX": 0.7448512296208976, + "velocityY": -2.1921003798007317, + "timestamp": 9.501868926841714 + }, + { + "x": 4.3450929137179735, + "y": 5.192687240127482, + "heading": 0.052651049006290945, + "angularVelocity": -0.2770308851058188, + "velocityX": 0.9828436170523044, + "velocityY": -2.4987541856375906, + "timestamp": 9.564548551701249 + }, + { + "x": 4.4254260370024205, + "y": 5.02053381378003, + "heading": 0.035443983400084915, + "angularVelocity": -0.2745240681444267, + "velocityX": 1.2816465233235492, + "velocityY": -2.7465612108120876, + "timestamp": 9.627228176560783 + }, + { + "x": 4.52834083069929, + "y": 4.839329489599335, + "heading": 0.019934884277187374, + "angularVelocity": -0.24743445988474677, + "velocityX": 1.6419178309937554, + "velocityY": -2.8909605727024488, + "timestamp": 9.689907801420317 + }, + { + "x": 4.655573182869098, + "y": 4.658127319631667, + "heading": 0.0077322780217831035, + "angularVelocity": -0.19468218392739808, + "velocityX": 2.029883753371783, + "velocityY": -2.890926204069373, + "timestamp": 9.75258742627985 }, { "x": 4.805556774139404, "y": 4.485479354858398, - "heading": -3.9652014929192097e-25, - "angularVelocity": -0.1188646746626479, - "velocityX": 2.3941774298704206, - "velocityY": -2.7520844885235216, - "timestamp": 9.813738277218697 - }, - { - "x": 5.026556371976755, - "y": 4.290478604211026, - "heading": -0.0011711585949671858, - "angularVelocity": -0.014781576411141743, - "velocityX": 2.7893083450033753, - "velocityY": -2.4611683748987776, - "timestamp": 9.892969244362662 - }, - { - "x": 5.271648004487175, - "y": 4.126055401914557, - "heading": -0.0011711658442509346, - "angularVelocity": -9.149558575332566e-8, - "velocityX": 3.0933818094770933, - "velocityY": -2.075239117019822, - "timestamp": 9.972200211506628 - }, - { - "x": 5.536248278847656, - "y": 3.995320554415454, - "heading": -0.0011711670074400294, - "angularVelocity": -1.4680990736292476e-8, - "velocityX": 3.3396067711718374, - "velocityY": -1.6500473515810268, - "timestamp": 10.051431178650594 - }, - { - "x": 5.8157535418727955, - "y": 3.900548632610079, - "heading": -0.001171167882288204, - "angularVelocity": -1.1041745495501586e-8, - "velocityX": 3.5277275174146094, - "velocityY": -1.19614748149131, - "timestamp": 10.13066214579456 - }, - { - "x": 6.105300821187418, - "y": 3.843388468655785, - "heading": -0.001171168612334034, - "angularVelocity": -9.214147655131007e-9, - "velocityX": 3.654471095733389, - "velocityY": -0.7214371604278409, - "timestamp": 10.209893112938525 - }, - { - "x": 6.399852439542981, - "y": 3.8248343191469996, - "heading": -0.0011711692753000216, - "angularVelocity": -8.367510982006514e-9, - "velocityX": 3.717632498671272, - "velocityY": -0.23417800107213171, - "timestamp": 10.289124080082491 - }, - { - "x": 6.694725372224158, - "y": 3.837278843369643, - "heading": -0.0011711699217371897, - "angularVelocity": -8.158895331165977e-9, - "velocityX": 3.721687912068311, - "velocityY": 0.15706641823558024, - "timestamp": 10.368355047226457 - }, - { - "x": 6.989598240261493, - "y": 3.849724899233505, - "heading": -0.001171170568174097, - "angularVelocity": -8.158892044748454e-9, - "velocityX": 3.721687096177193, - "velocityY": 0.15708574958131358, - "timestamp": 10.447586014370422 - }, - { - "x": 7.2831382115989305, - "y": 3.8803709956962793, - "heading": -0.0011711735014437953, - "angularVelocity": -3.702175808154048e-8, - "velocityX": 3.7048641701427742, - "velocityY": 0.38679442606183717, - "timestamp": 10.526816981514388 - }, - { - "x": 7.537795284833571, - "y": 3.9128988141490195, - "heading": -0.0022060584163901163, - "angularVelocity": -0.013061621639249952, - "velocityX": 3.2141103714147587, - "velocityY": 0.4105442559300826, - "timestamp": 10.606047948658354 - }, - { - "x": 7.7535280379166585, - "y": 3.9448039763833895, - "heading": -0.0030479668047439287, - "angularVelocity": -0.010626001659477862, - "velocityX": 2.7228337714355204, - "velocityY": 0.40268550775604034, - "timestamp": 10.68527891580232 - }, - { - "x": 7.9303456416380165, - "y": 3.9756628799518183, - "heading": -0.0034891865807647085, - "angularVelocity": -0.005568779379141843, - "velocityX": 2.2316729189998985, - "velocityY": 0.38948033427835, - "timestamp": 10.764509882946285 - }, - { - "x": 8.06825324660345, - "y": 4.0053006852761595, - "heading": -0.003443969014778176, - "angularVelocity": 0.000570705717934386, - "velocityX": 1.7405770740479534, - "velocityY": 0.374068453190637, - "timestamp": 10.843740850090251 - }, - { - "x": 8.167254001972681, - "y": 4.03362192898922, - "heading": -0.002865492528698777, - "angularVelocity": 0.007301141295275209, - "velocityX": 1.2495209756728547, - "velocityY": 0.3574516976625985, - "timestamp": 10.922971817234217 - }, - { - "x": 8.22735001387898, - "y": 4.060566470670497, - "heading": -0.0017242628169884097, - "angularVelocity": 0.014403834167980214, - "velocityX": 0.7584914594958928, - "velocityY": 0.34007589017963685, - "timestamp": 11.002202784378182 + "heading": -5.922112096142328e-27, + "angularVelocity": -0.1233619065064789, + "velocityX": 2.392860384956347, + "velocityY": -2.754451149957796, + "timestamp": 9.815267051139385 + }, + { + "x": 5.0260464642582185, + "y": 4.290440402432839, + "heading": -0.0017206330074737138, + "angularVelocity": -0.02175742700503041, + "velocityX": 2.7880950308894734, + "velocityY": -2.4662701180928828, + "timestamp": 9.894349610261651 + }, + { + "x": 5.270702243030263, + "y": 4.125667987076678, + "heading": -0.001720643347085146, + "angularVelocity": -1.3074452252065364e-7, + "velocityX": 3.0936755396824163, + "velocityY": -2.0835493588594303, + "timestamp": 9.973432169383917 + }, + { + "x": 5.534838657282035, + "y": 3.9943730439929475, + "heading": -0.0017206445243311077, + "angularVelocity": -1.4886290666410382e-8, + "velocityX": 3.3400084314848875, + "velocityY": -1.660226281761329, + "timestamp": 10.052514728506184 + }, + { + "x": 5.813901235574451, + "y": 3.898819454109309, + "heading": -0.001720645408427364, + "angularVelocity": -1.1179408784311949e-8, + "velocityX": 3.528749972050966, + "velocityY": -1.2082764005639604, + "timestamp": 10.13159728762845 + }, + { + "x": 6.10307814292353, + "y": 3.8406547782709533, + "heading": -0.0017206461449007905, + "angularVelocity": -9.31271616103322e-9, + "velocityX": 3.656645795971196, + "velocityY": -0.7354930908144911, + "timestamp": 10.210679846750716 + }, + { + "x": 6.397383154789235, + "y": 3.8208817077449715, + "heading": -0.0017206468122835479, + "angularVelocity": -8.439063742592553e-9, + "velocityX": 3.721490745016124, + "velocityY": -0.25003073680773497, + "timestamp": 10.289762405872983 + }, + { + "x": 6.692083031584466, + "y": 3.833467304039097, + "heading": -0.0017206474619802302, + "angularVelocity": -8.215423090919723e-9, + "velocityX": 3.7264838172422694, + "velocityY": 0.1591450306339672, + "timestamp": 10.36884496499525 + }, + { + "x": 6.9867828265259435, + "y": 3.8460548168458266, + "heading": -0.0017206481116765608, + "angularVelocity": -8.21541864124069e-9, + "velocityX": 3.726482782200477, + "velocityY": 0.15916926496104583, + "timestamp": 10.447927524117516 + }, + { + "x": 7.280008906046239, + "y": 3.8780682629545473, + "heading": -0.0017206520775067291, + "angularVelocity": -5.014797462981314e-8, + "velocityX": 3.7078476313209716, + "velocityY": 0.40481044700672053, + "timestamp": 10.527010083239782 + }, + { + "x": 7.534517790856317, + "y": 3.9111592304770704, + "heading": -0.0026190122393155452, + "angularVelocity": -0.011359776059091569, + "velocityX": 3.218268195097111, + "velocityY": 0.41843571945316527, + "timestamp": 10.606092642362048 + }, + { + "x": 7.750301298941706, + "y": 3.943488528624649, + "heading": -0.0033468758613748313, + "angularVelocity": -0.009203845072008415, + "velocityX": 2.728585297192714, + "velocityY": 0.40880440019139475, + "timestamp": 10.685175201484315 + }, + { + "x": 7.927368234125033, + "y": 3.974697332887919, + "heading": -0.003695626110523714, + "angularVelocity": -0.004409951486391499, + "velocityX": 2.239013723741158, + "velocityY": 0.39463574029034193, + "timestamp": 10.764257760606581 + }, + { + "x": 8.065723377902886, + "y": 4.004632819289759, + "heading": -0.0035763939938957814, + "angularVelocity": 0.0015076916826071924, + "velocityX": 1.7495026123768658, + "velocityY": 0.37853461918901893, + "timestamp": 10.843340319728847 + }, + { + "x": 8.165369644244953, + "y": 4.03321033626126, + "heading": -0.0029399507835581986, + "angularVelocity": 0.008047832763651503, + "velocityX": 1.2600283481975725, + "velocityY": 0.36136307788571737, + "timestamp": 10.922422878851114 + }, + { + "x": 8.226308982581841, + "y": 4.060376108268979, + "heading": -0.0017550259023952847, + "angularVelocity": 0.014983390703517166, + "velocityX": 0.7705787345939596, + "velocityY": 0.34351154425489977, + "timestamp": 11.00150543797338 }, { "x": 8.248542785644531, "y": 4.086092948913574, - "heading": 1.4182707321197947e-24, - "angularVelocity": 0.021762486047347675, - "velocityX": 0.26748091724090983, - "velocityY": 0.32217804683230217, - "timestamp": 11.081433751522148 - }, - { - "x": 8.23152301234386, - "y": 4.109892958457591, - "heading": 0.0022863134325125296, - "angularVelocity": 0.02921520350070526, - "velocityX": -0.21748380315840005, - "velocityY": 0.30412370948765427, - "timestamp": 11.159691410499514 - }, - { - "x": 8.176551036095818, - "y": 4.132280075994518, - "heading": 0.005155847292928311, - "angularVelocity": 0.03666777025933954, - "velocityX": -0.7024485138757267, - "velocityY": 0.2860693487317698, - "timestamp": 11.23794906947688 - }, - { - "x": 8.0836268579111, - "y": 4.153254299063441, - "heading": 0.008608587112944467, - "angularVelocity": 0.044120152137630285, - "velocityX": -1.1874132116780383, - "velocityY": 0.2680149565295516, - "timestamp": 11.316206728454246 - }, - { - "x": 7.952750479206832, - "y": 4.172815624183264, - "heading": 0.012644515987393022, - "angularVelocity": 0.05157231799657927, - "velocityX": -1.672377891371887, - "velocityY": 0.24996051984483794, - "timestamp": 11.394464387431611 - }, - { - "x": 7.783921902112923, - "y": 4.190964046074257, - "heading": 0.01726361439792771, - "angularVelocity": 0.05902423444420523, - "velocityX": -2.157342543849138, - "velocityY": 0.2319060156941693, - "timestamp": 11.472722046408977 - }, - { - "x": 7.577141130185825, - "y": 4.207699555855839, - "heading": 0.022465858708007803, - "angularVelocity": 0.06647584885697558, - "velocityX": -2.642307150880985, - "velocityY": 0.2138513980647186, - "timestamp": 11.550979705386343 - }, - { - "x": 7.3324081705466435, - "y": 4.2230221357012825, - "heading": 0.028251214753927917, - "angularVelocity": 0.0739270267156012, - "velocityX": -3.127271666917177, - "velocityY": 0.19579655263996953, - "timestamp": 11.629237364363709 - }, - { - "x": 7.049723044551423, - "y": 4.236931732641069, - "heading": 0.03461960291638085, - "angularVelocity": 0.08137718717467465, - "velocityX": -3.612235910059377, - "velocityY": 0.17774103035473854, - "timestamp": 11.707495023341075 - }, - { - "x": 6.758414971904043, - "y": 4.226087741393719, - "heading": 0.03461960351163466, - "angularVelocity": 7.606332865983503e-9, - "velocityX": -3.722422526485686, - "velocityY": -0.13856779501270336, - "timestamp": 11.78575268231844 - }, - { - "x": 6.467106926605879, - "y": 4.215243015473276, - "heading": 0.03461960410686983, - "angularVelocity": 7.606094882154123e-9, - "velocityX": -3.7224221770091335, - "velocityY": -0.13857718288735063, - "timestamp": 11.864010341295806 - }, - { - "x": 6.175798881305754, - "y": 4.204398289605525, - "heading": 0.03461960470210498, - "angularVelocity": 7.606094432841078e-9, - "velocityX": -3.7224221770341983, - "velocityY": -0.13857718221405538, - "timestamp": 11.942268000273172 - }, - { - "x": 5.884490683895451, - "y": 4.1935576504515195, - "heading": 0.03461960529734107, - "angularVelocity": 7.606106615779635e-9, - "velocityX": -3.7224241207439825, - "velocityY": -0.13852496095162967, - "timestamp": 12.020525659250538 - }, - { - "x": 5.594009037113431, - "y": 4.2180198226312315, - "heading": 0.034619605911984125, - "angularVelocity": 7.854094508972923e-9, - "velocityX": -3.7118622071998786, - "velocityY": 0.31258502361778023, - "timestamp": 12.098783318227904 - }, - { - "x": 5.309172831526297, - "y": 4.280038516789027, - "heading": 0.03461959995315513, - "angularVelocity": -7.614371636753198e-8, - "velocityX": -3.6397230547046773, - "velocityY": 0.7924936034150152, - "timestamp": 12.17704097720527 - }, - { - "x": 5.03935943827081, - "y": 4.37675512978584, - "heading": 0.01971114929844739, - "angularVelocity": -0.1905046847749395, - "velocityX": -3.447757021884906, - "velocityY": 1.235874089011348, - "timestamp": 12.255298636182635 + "heading": 2.0912283688050628e-26, + "angularVelocity": 0.022192325613564278, + "velocityX": 0.2811467320918988, + "velocityY": 0.3251897881154181, + "timestamp": 11.080587997095646 + }, + { + "x": 8.232549205371914, + "y": 4.110135442061636, + "heading": 0.002315542172399013, + "angularVelocity": 0.029537040889687716, + "velocityX": -0.2040140059273348, + "velocityY": 0.3066858862123803, + "timestamp": 11.158982518098847 + }, + { + "x": 8.1785216821941, + "y": 4.132727328793288, + "heading": 0.00520685837502888, + "angularVelocity": 0.03688161067419279, + "velocityX": -0.6891747342344035, + "velocityY": 0.28818196020011727, + "timestamp": 11.237377039102048 + }, + { + "x": 8.086460217126783, + "y": 4.153868606569999, + "heading": 0.008673934737020163, + "angularVelocity": 0.044226003521977544, + "velocityX": -1.1743354495852854, + "velocityY": 0.26967800180637214, + "timestamp": 11.315771560105249 + }, + { + "x": 7.956364811594128, + "y": 4.173559271802363, + "heading": 0.012716755269955655, + "angularVelocity": 0.05157019242161638, + "velocityX": -1.6594961467695446, + "velocityY": 0.25117399762619275, + "timestamp": 11.39416608110845 + }, + { + "x": 7.788235467736609, + "y": 4.191799319049532, + "heading": 0.01733530192089736, + "angularVelocity": 0.0589141510380956, + "velocityX": -2.14465681664992, + "velocityY": 0.23266992404257889, + "timestamp": 11.47256060211165 + }, + { + "x": 7.582072189128265, + "y": 4.208588739164989, + "heading": 0.022529553625480057, + "angularVelocity": 0.06625784095766901, + "velocityX": -2.6298174409398722, + "velocityY": 0.21416573378605752, + "timestamp": 11.550955123114852 + }, + { + "x": 7.337874982925346, + "y": 4.223927513793021, + "heading": 0.02829948161506035, + "angularVelocity": 0.07360116390461449, + "velocityX": -3.1149779739447685, + "velocityY": 0.1956613093841805, + "timestamp": 11.629349644118053 + }, + { + "x": 7.055643870589831, + "y": 4.237815588350764, + "heading": 0.034645023037832345, + "angularVelocity": 0.08094368511433264, + "velocityX": -3.600138233180768, + "velocityY": 0.17715618872364539, + "timestamp": 11.707744165121253 + }, + { + "x": 6.763454078794012, + "y": 4.226672459763479, + "heading": 0.03464502362353886, + "angularVelocity": 7.47126845639964e-9, + "velocityX": -3.7271710836002327, + "velocityY": -0.1421416757789859, + "timestamp": 11.786138686124454 + }, + { + "x": 6.471264316590495, + "y": 4.215528555246823, + "heading": 0.03464502420922547, + "angularVelocity": 7.471014571114214e-9, + "velocityX": -3.7271707061210075, + "velocityY": -0.14215157352898217, + "timestamp": 11.864533207127655 + }, + { + "x": 6.1790745543846715, + "y": 4.2043846507906375, + "heading": 0.034645024794912035, + "angularVelocity": 7.471014019515784e-9, + "velocityX": -3.727170706150427, + "velocityY": -0.1421515727576172, + "timestamp": 11.942927728130856 + }, + { + "x": 5.8868846155421455, + "y": 4.193245378659894, + "heading": 0.034645025380599624, + "angularVelocity": 7.471027140146409e-9, + "velocityX": -3.727172959327064, + "velocityY": -0.1420924828444054, + "timestamp": 12.021322249134057 + }, + { + "x": 5.595514516394948, + "y": 4.2177914315394665, + "heading": 0.034645025985590286, + "angularVelocity": 7.717256902963699e-9, + "velocityX": -3.716715089506096, + "velocityY": 0.3131092908721317, + "timestamp": 12.099716770137258 + }, + { + "x": 5.309801234566853, + "y": 4.279976327218559, + "heading": 0.03464501988589696, + "angularVelocity": -7.78076483461541e-8, + "velocityX": -3.6445567645783616, + "velocityY": 0.7932301248011192, + "timestamp": 12.178111291140459 + }, + { + "x": 5.039631118107501, + "y": 4.376720178141923, + "heading": 0.01972472706533464, + "angularVelocity": -0.19032315817011253, + "velocityX": -3.446288248235145, + "velocityY": 1.2340639331084704, + "timestamp": 12.25650581214366 }, { "x": 4.805556774139404, "y": 4.485479354858398, - "heading": -1.4078898091571754e-24, - "angularVelocity": -0.25187501844577836, - "velocityX": -2.9876010500011123, - "velocityY": 1.3893109823794343, - "timestamp": 12.333556295160001 - }, - { - "x": 4.62745788226169, - "y": 4.587679677136332, - "heading": -0.019778853338814184, - "angularVelocity": -0.28487106556518027, - "velocityX": -2.5651244910957125, - "velocityY": 1.47197181806725, - "timestamp": 12.402987191202648 - }, - { - "x": 4.478940134439646, - "y": 4.685562428002014, - "heading": -0.039416750418752866, - "angularVelocity": -0.2828408993580651, - "velocityX": -2.139072895311886, - "velocityY": 1.4097866575934404, - "timestamp": 12.472418087245295 - }, - { - "x": 4.358424662792702, - "y": 4.772986561558442, - "heading": -0.05736759927872339, - "angularVelocity": -0.2585426644781364, - "velocityX": -1.7357614335398783, - "velocityY": 1.2591531802027822, - "timestamp": 12.541848983287942 - }, - { - "x": 4.264251963016928, - "y": 4.846292896298889, - "heading": -0.07266630938107123, - "angularVelocity": -0.22034441400483662, - "velocityX": -1.3563514968599637, - "velocityY": 1.0558172070171734, - "timestamp": 12.611279879330588 - }, - { - "x": 4.195046678979309, - "y": 4.903178017800908, - "heading": -0.08468297645986948, - "angularVelocity": -0.17307377210596744, - "velocityX": -0.9967505531703128, - "velocityY": 0.8193055936809087, - "timestamp": 12.680710775373235 - }, - { - "x": 4.149718350868284, - "y": 4.942098244876491, - "heading": -0.09298341563380762, - "angularVelocity": -0.11954964788067375, - "velocityX": -0.6528552948990165, - "velocityY": 0.5605606335784191, - "timestamp": 12.750141671415882 + "heading": -2.403769428999081e-26, + "angularVelocity": -0.2516084901460064, + "velocityX": -2.9858508091214753, + "velocityY": 1.3873313507717573, + "timestamp": 12.33490033314686 + }, + { + "x": 4.627436042049824, + "y": 4.587623100442906, + "heading": -0.019769634491549008, + "angularVelocity": -0.2845427298125247, + "velocityX": -2.5636771062530377, + "velocityY": 1.4701465631201296, + "timestamp": 12.40437894714826 + }, + { + "x": 4.478903462198178, + "y": 4.685483985234687, + "heading": -0.03940302192904774, + "angularVelocity": -0.28258173712422013, + "velocityX": -2.137817254798061, + "velocityY": 1.4085036985598087, + "timestamp": 12.473857561149659 + }, + { + "x": 4.358385068299864, + "y": 4.77291258513252, + "heading": -0.057354073333619894, + "angularVelocity": -0.2583680124104157, + "velocityX": -1.734611371146319, + "velocityY": 1.2583526766390536, + "timestamp": 12.543336175151058 + }, + { + "x": 4.264219265878193, + "y": 4.846237576154262, + "heading": -0.07265588839993739, + "angularVelocity": -0.22023777080540505, + "velocityX": -1.355320680688504, + "velocityY": 1.0553605893788425, + "timestamp": 12.612814789152457 + }, + { + "x": 4.195026094794195, + "y": 4.903145738441645, + "heading": -0.08467676120352872, + "angularVelocity": -0.17301543757549837, + "velocityX": -0.9958916434718147, + "velocityY": 0.8190745181853494, + "timestamp": 12.682293403153857 + }, + { + "x": 4.149710172245756, + "y": 4.942086169590921, + "heading": -0.09298105070953734, + "angularVelocity": -0.11952295861632048, + "velocityX": -0.6522283611979665, + "velocityY": 0.5604664357364799, + "timestamp": 12.751772017155256 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": -0.06149221603257356, - "velocityX": -0.32142062647019676, - "velocityY": 0.2860902346288004, - "timestamp": 12.819572567458529 + "angularVelocity": -0.061484021298608874, + "velocityX": -0.3210821603240091, + "velocityY": 0.2860675462596191, + "timestamp": 12.821250631156655 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": -9.169895235461756e-26, - "velocityX": -3.1744204454242983e-26, - "velocityY": -4.8447793914370994e-26, - "timestamp": 12.889003463501176 + "angularVelocity": -1.5452872782226126e-27, + "velocityX": -2.8121806375160504e-27, + "velocityY": 1.64906391853344e-27, + "timestamp": 12.890729245158054 } ], "trajectoryWaypoints": [ @@ -41164,7 +41164,7 @@ "controlIntervalCount": 6 }, { - "timestamp": 0.34959777724707536, + "timestamp": 0.3498266892248522, "isStopPoint": false, "x": 0.8667811155319214, "y": 6.895569324493408, @@ -41175,7 +41175,7 @@ "controlIntervalCount": 13 }, { - "timestamp": 1.1040901645422216, + "timestamp": 1.1048131080573207, "isStopPoint": true, "x": 2.286245346069336, "y": 6.686196804046631, @@ -41186,7 +41186,7 @@ "controlIntervalCount": 8 }, { - "timestamp": 1.8634374124793147, + "timestamp": 1.8646575664742369, "isStopPoint": true, "x": 3.062049150466919, "y": 7.130331993103027, @@ -41197,7 +41197,7 @@ "controlIntervalCount": 22 }, { - "timestamp": 3.2868633977536255, + "timestamp": 3.2873205808279327, "isStopPoint": false, "x": 7.214789390563965, "y": 6.145846366882324, @@ -41208,7 +41208,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 3.8367661144950707, + "timestamp": 3.8395496986589794, "isStopPoint": false, "x": 8.186561584472656, "y": 5.798520088195801, @@ -41219,7 +41219,7 @@ "controlIntervalCount": 15 }, { - "timestamp": 4.83026471118104, + "timestamp": 4.831202221673352, "isStopPoint": false, "x": 5.851044178009033, "y": 6.557126998901367, @@ -41230,7 +41230,7 @@ "controlIntervalCount": 12 }, { - "timestamp": 5.6678933208149, + "timestamp": 5.66889950661145, "isStopPoint": true, "x": 4.144390106201172, "y": 5.838942050933838, @@ -41241,7 +41241,7 @@ "controlIntervalCount": 18 }, { - "timestamp": 7.3582994981379, + "timestamp": 7.357129411131235, "isStopPoint": false, "x": 8.056792259216309, "y": 7.362460613250732, @@ -41252,7 +41252,7 @@ "controlIntervalCount": 18 }, { - "timestamp": 9.124585923832743, + "timestamp": 9.12579117768451, "isStopPoint": true, "x": 4.144390106201172, "y": 5.838942050933838, @@ -41263,7 +41263,7 @@ "controlIntervalCount": 11 }, { - "timestamp": 9.813738277218697, + "timestamp": 9.815267051139385, "isStopPoint": false, "x": 4.805556774139404, "y": 4.485479354858398, @@ -41274,7 +41274,7 @@ "controlIntervalCount": 16 }, { - "timestamp": 11.081433751522148, + "timestamp": 11.080587997095646, "isStopPoint": false, "x": 8.248542785644531, "y": 4.086092948913574, @@ -41285,7 +41285,7 @@ "controlIntervalCount": 16 }, { - "timestamp": 12.333556295160001, + "timestamp": 12.33490033314686, "isStopPoint": false, "x": 4.805556774139404, "y": 4.485479354858398, @@ -41296,7 +41296,7 @@ "controlIntervalCount": 8 }, { - "timestamp": 12.889003463501176, + "timestamp": 12.890729245158054, "isStopPoint": true, "x": 4.127401828765869, "y": 4.96196174621582, @@ -41525,1883 +41525,4345 @@ { "x": 0.387, "y": 4.121134281158447, - "heading": 1.1679855597934721e-27, - "angularVelocity": 5.433585185667626e-28, - "velocityX": 7.442598398670663e-27, - "velocityY": 2.3270251613663043e-26, + "heading": 3.980405256540069e-28, + "angularVelocity": 1.0894029802546308e-28, + "velocityX": -2.179791595178488e-28, + "velocityY": 6.106168293685067e-28, "timestamp": 0 }, { - "x": 0.4005104003556791, - "y": 4.115348381940823, - "heading": -0.015277315995294262, - "angularVelocity": -0.30891158633586036, - "velocityX": 0.2731840597648779, - "velocityY": -0.11699249437834791, - "timestamp": 0.04945530265311637 - }, - { - "x": 0.4275520628000622, - "y": 4.103759116150694, - "heading": -0.04541480648332661, - "angularVelocity": -0.6093884552566431, - "velocityX": 0.5467899495844878, - "velocityY": -0.23433818353953934, - "timestamp": 0.09891060530623275 - }, - { - "x": 0.4681490763693427, - "y": 4.086345867978677, - "heading": -0.08991129386234921, - "angularVelocity": -0.899731373420656, - "velocityX": 0.8208829264281612, - "velocityY": -0.35210073011087883, - "timestamp": 0.14836590795934912 - }, - { - "x": 0.5223311730580589, - "y": 4.063083084813811, - "heading": -0.14811250055399447, - "angularVelocity": -1.176844616640472, - "velocityX": 1.0955770924859953, - "velocityY": -0.47037995759592216, - "timestamp": 0.1978212106124655 - }, - { - "x": 0.5901369333154594, - "y": 4.033939247984653, - "heading": -0.2191152852440036, - "angularVelocity": -1.435696090832334, - "velocityX": 1.3710513659777948, - "velocityY": -0.5892965013999555, - "timestamp": 0.24727651326558187 - }, - { - "x": 0.6716171763310703, - "y": 3.9988769782713436, - "heading": -0.3016414776380596, - "angularVelocity": -1.6687026055204155, - "velocityX": 1.647553217642207, - "velocityY": -0.708968863445023, - "timestamp": 0.29673181591869824 - }, - { - "x": 0.7668367655706729, - "y": 3.9578537486428593, - "heading": -0.39387111054062807, - "angularVelocity": -1.8649088763944044, - "velocityX": 1.9253666266584337, - "velocityY": -0.8295011339072073, - "timestamp": 0.3461871185718146 - }, - { - "x": 0.8758711660113281, - "y": 3.91082124740519, - "heading": -0.49317423957558376, - "angularVelocity": -2.00793693916861, - "velocityX": 2.2047059585385944, - "velocityY": -0.9510102802839819, - "timestamp": 0.395642421224931 - }, - { - "x": 0.9987875307433454, - "y": 3.857723681335083, - "heading": -0.5954683580210642, - "angularVelocity": -2.068415578466479, - "velocityX": 2.4854031446165235, - "velocityY": -1.0736475811812884, - "timestamp": 0.44509772387804736 - }, - { - "x": 1.1355416361286799, - "y": 3.7985302060231096, - "heading": -0.6929936795383219, - "angularVelocity": -1.9719891757878503, - "velocityX": 2.765206116410594, - "velocityY": -1.1969085646318132, - "timestamp": 0.49455302653116373 - }, - { - "x": 1.2843691968945923, - "y": 3.733334425660418, - "heading": -0.76082235123736, - "angularVelocity": -1.3715146417118123, - "velocityX": 3.0093347483848483, - "velocityY": -1.3182768452551967, - "timestamp": 0.5440083291842801 - }, - { - "x": 1.4440686661947881, - "y": 3.662157825631026, - "heading": -0.7899998870158123, - "angularVelocity": -0.5899779035446591, - "velocityX": 3.2291677683248934, - "velocityY": -1.439210685427017, - "timestamp": 0.5934636318373965 - }, - { - "x": 1.613363599943702, - "y": 3.5895175490832267, - "heading": -0.789999943508809, - "angularVelocity": -0.0000011423041349866365, - "velocityX": 3.423190733183105, - "velocityY": -1.468806632471835, - "timestamp": 0.6429189344905129 + "x": 0.40050242656101365, + "y": 4.1153504673922905, + "heading": -0.015208632237685107, + "angularVelocity": -0.3074428187596394, + "velocityX": 0.2729518353219713, + "velocityY": -0.11691991624610248, + "timestamp": 0.04946816549185787 + }, + { + "x": 0.42752783346651274, + "y": 4.103765611590411, + "heading": -0.045213195112198845, + "angularVelocity": -0.6065428660266832, + "velocityX": 0.5463191658066905, + "velocityY": -0.23418810232180645, + "timestamp": 0.09893633098371574 + }, + { + "x": 0.46809995023859424, + "y": 4.086359376332899, + "heading": -0.08951802911831011, + "angularVelocity": -0.8956231460292083, + "velocityX": 0.8201661890768802, + "velocityY": -0.35186740976632624, + "timestamp": 0.1484044964755736 + }, + { + "x": 0.5222480472443771, + "y": 4.063106542737477, + "heading": -0.14747661029050188, + "angularVelocity": -1.171633930547341, + "velocityX": 1.094604913430544, + "velocityY": -0.47005651744352356, + "timestamp": 0.19787266196743147 + }, + { + "x": 0.5900100832649373, + "y": 4.033975964464713, + "heading": -0.21819722216411808, + "angularVelocity": -1.4296186480830055, + "velocityX": 1.3698109753375314, + "velocityY": -0.5888752490236961, + "timestamp": 0.24734082745928934 + }, + { + "x": 0.6714360586881531, + "y": 3.998930617330974, + "heading": -0.3004183343874937, + "angularVelocity": -1.6621015031759903, + "velocityX": 1.64602779613119, + "velocityY": -0.7084424252503851, + "timestamp": 0.2968089929511472 + }, + { + "x": 0.7665898631710647, + "y": 3.95792823980715, + "heading": -0.3923434473802076, + "angularVelocity": -1.8582680816785926, + "velocityX": 1.9235361476781112, + "velocityY": -0.8288639191718736, + "timestamp": 0.3462771584430051 + }, + { + "x": 0.8755460489765932, + "y": 3.910920647028603, + "heading": -0.4913759605059717, + "angularVelocity": -2.0019443240130714, + "velocityX": 2.202551574779177, + "velocityY": -0.9502594711397636, + "timestamp": 0.39574532393486295 + }, + { + "x": 0.998371594898006, + "y": 3.857851824015994, + "heading": -0.593488171000786, + "angularVelocity": -2.064200470737515, + "velocityX": 2.482920979586945, + "velocityY": -1.072787367086499, + "timestamp": 0.4452134894267208 + }, + { + "x": 1.1350267865800456, + "y": 3.7986882165536997, + "heading": -0.6910493311323551, + "angularVelocity": -1.972200892463397, + "velocityX": 2.7624875578725923, + "velocityY": -1.1959935622037938, + "timestamp": 0.49468165491857863 + }, + { + "x": 1.2838522334103695, + "y": 3.7335275480002674, + "heading": -0.7598417160155273, + "angularVelocity": -1.39063949914405, + "velocityX": 3.008509520225074, + "velocityY": -1.3172242775842857, + "timestamp": 0.5441498204104365 + }, + { + "x": 1.4435365539100036, + "y": 3.6623850951483443, + "heading": -0.7899998876377602, + "angularVelocity": -0.6096480700744137, + "velocityX": 3.2280218785537347, + "velocityY": -1.4381461722819, + "timestamp": 0.5936179859022943 + }, + { + "x": 1.6130975326130776, + "y": 3.5896311577651607, + "heading": -0.7899999438199378, + "angularVelocity": -0.000001135723893796383, + "velocityX": 3.427678730697678, + "velocityY": -1.4707223657840804, + "timestamp": 0.6430861513941522 }, { "x": 1.7826586961746216, "y": 3.5168776512145996, "heading": -0.79, - "angularVelocity": -0.0000011422676237323725, - "velocityX": 3.423194018614537, - "velocityY": -1.468798975473458, - "timestamp": 0.6923742371436292 + "angularVelocity": -0.0000011356811339822919, + "velocityX": 3.427682467615514, + "velocityY": -1.4707136564936016, + "timestamp": 0.69255431688601 }, { - "x": 2.0239723336710425, - "y": 3.4134441767348855, + "x": 2.0243963751949394, + "y": 3.413262421214484, "heading": -0.79, - "angularVelocity": -4.2462946476416713e-16, - "velocityX": 3.4237473592821535, - "velocityY": -1.467509622685751, - "timestamp": 0.7628565514540567 + "angularVelocity": -4.062908416081592e-16, + "velocityX": 3.4282324773027724, + "velocityY": -1.4694320640008167, + "timestamp": 0.7630681109827596 }, { - "x": 2.250495583067251, - "y": 3.3163502598603043, + "x": 2.2508253931606723, + "y": 3.316208894447284, "heading": -0.79, - "angularVelocity": 2.583795012505271e-16, - "velocityX": 3.2139019782824456, - "velocityY": -1.37756425600538, - "timestamp": 0.8333388657644842 + "angularVelocity": -3.3647602923838933e-16, + "velocityX": 3.2111308271833634, + "velocityY": -1.3763764666249076, + "timestamp": 0.8335819050795092 }, { - "x": 2.4487034476982825, - "y": 3.23139307341843, + "x": 2.4489508052764966, + "y": 3.2312870493551733, "heading": -0.79, - "angularVelocity": 2.1975738712772502e-16, - "velocityX": 2.812164534752054, - "velocityY": -1.2053688542021173, - "timestamp": 0.9038211800749117 + "angularVelocity": -3.271122096502082e-16, + "velocityX": 2.8097397772127017, + "velocityY": -1.2043295383537609, + "timestamp": 0.9040956991762589 }, { - "x": 2.6185959113515174, - "y": 3.1585726243584236, + "x": 2.6187725953400465, + "y": 3.158496892882915, "heading": -0.79, - "angularVelocity": 1.8378454574977131e-16, - "velocityX": 2.4104268611977178, - "velocityY": -1.0331733538044932, - "timestamp": 0.9743034943853393 + "angularVelocity": -2.53261943200864e-16, + "velocityX": 2.4083484974662293, + "velocityY": -1.0322825115946104, + "timestamp": 0.9746094932730085 }, { - "x": 2.760172968622745, - "y": 3.0978889149966715, + "x": 2.7602907579505302, + "y": 3.0978384273454327, "heading": -0.79, - "angularVelocity": 1.432046748255306e-16, - "velocityX": 2.0086891109686786, - "velocityY": -0.8609778205420586, - "timestamp": 1.0447858086957669 + "angularVelocity": -2.2579655206340545e-16, + "velocityX": 2.0069571411277565, + "velocityY": -0.8602354520060983, + "timestamp": 1.0451232873697582 }, { - "x": 2.8734346168098597, - "y": 3.049341946491368, + "x": 2.873505290407551, + "y": 3.0493116539001877, "heading": -0.79, - "angularVelocity": 9.739728624843172e-17, - "velocityX": 1.6069513224022827, - "velocityY": -0.6887822708472163, - "timestamp": 1.1152681230061945 + "angularVelocity": -1.4176471677644102e-16, + "velocityX": 1.6055657464932764, + "velocityY": -0.6881883760029022, + "timestamp": 1.115637081466508 }, { - "x": 2.9583808542915984, - "y": 3.0129317195374297, + "x": 2.9584161910908713, + "y": 3.0129165732416565, "heading": -0.79, - "angularVelocity": 7.507925930031045e-17, - "velocityX": 1.2052135108334718, - "velocityY": -0.5165867112929291, - "timestamp": 1.185750437316622 + "angularVelocity": -1.2154347758080432e-16, + "velocityX": 1.2041743288811908, + "velocityY": -0.5161412901508954, + "timestamp": 1.1861508755632577 }, { - "x": 3.0150116799871185, - "y": 2.9886582345981343, + "x": 3.0150234589203313, + "y": 2.9886531858328245, "heading": -0.79, - "angularVelocity": 2.284084326875716e-17, - "velocityX": 0.8034756839297177, - "velocityY": -0.3443911451656786, - "timestamp": 1.2562327516270497 + "angularVelocity": -8.455163929533081e-17, + "velocityX": 0.8027828959507013, + "velocityY": -0.3440941977330144, + "timestamp": 1.2566646696600074 }, { "x": 3.0433270931243896, "y": 2.9765214920043945, "heading": -0.79, - "angularVelocity": 4.059583267317121e-17, - "velocityX": 0.40173784607243285, - "velocityY": -0.17219557434345442, - "timestamp": 1.3267150659374773 + "angularVelocity": -3.205512140199398e-17, + "velocityX": 0.4013914520784945, + "velocityY": -0.17204710062522346, + "timestamp": 1.3271784637567572 }, { "x": 3.0433270931243896, "y": 2.9765214920043945, "heading": -0.79, - "angularVelocity": -5.776011429095129e-29, - "velocityX": -2.265299874372296e-27, - "velocityY": 1.6360733128925837e-27, - "timestamp": 1.397197380247905 - }, - { - "x": 3.05997928479042, - "y": 2.9660593331101914, - "heading": -0.757416423789292, - "angularVelocity": 0.5582463314772621, - "velocityX": 0.2852978705745241, - "velocityY": -0.17924557403559996, - "timestamp": 1.4555651193147854 - }, - { - "x": 3.09346940981965, - "y": 2.9451270864719548, - "heading": -0.6941025427131016, - "angularVelocity": 1.0847410245519795, - "velocityX": 0.5737780075883255, - "velocityY": -0.35862699108923296, - "timestamp": 1.5139328583816658 - }, - { - "x": 3.144014292961821, - "y": 2.9136974289532866, - "heading": -0.6025701359612305, - "angularVelocity": 1.5682020276130446, - "velocityX": 0.8659729492734705, - "velocityY": -0.5384765286634581, - "timestamp": 1.5723005974485462 - }, - { - "x": 3.211860363047214, - "y": 2.8716921302721943, - "heading": -0.4864204343113474, - "angularVelocity": 1.9899640367565647, - "velocityX": 1.1623898950009328, - "velocityY": -0.7196663662603233, - "timestamp": 1.6306683365154266 - }, - { - "x": 3.2972613235718167, - "y": 2.8189697938754956, - "heading": -0.350515523406248, - "angularVelocity": 2.3284251382321566, - "velocityX": 1.463153479814365, - "velocityY": -0.903278715940788, - "timestamp": 1.689036075582307 - }, - { - "x": 3.4004578303174937, - "y": 2.7553674746788306, - "heading": -0.2010210241746004, - "angularVelocity": 2.5612521852242733, - "velocityX": 1.7680401604631244, - "velocityY": -1.0896827633461483, - "timestamp": 1.7474038146491875 - }, - { - "x": 3.521660389826237, - "y": 2.6807799872839095, - "heading": -0.04666004939323709, - "angularVelocity": 2.644628304078894, - "velocityX": 2.0765333974965823, - "velocityY": -1.2778889260976047, - "timestamp": 1.805771553716068 - }, - { - "x": 3.6607588635338284, - "y": 2.5953087461465145, - "heading": 0.09647781625083168, - "angularVelocity": 2.4523455582210394, - "velocityX": 2.383139657820331, - "velocityY": -1.4643575801258646, - "timestamp": 1.8641392927829483 - }, - { - "x": 3.8159145666935705, - "y": 2.5005609897698613, - "heading": 0.19948059626332554, - "angularVelocity": 1.7647210883818645, - "velocityX": 2.6582441883170693, - "velocityY": -1.6232898154250444, - "timestamp": 1.9225070318498287 - }, - { - "x": 3.9841103579797825, - "y": 2.394344369414975, - "heading": 0.2530307730268158, - "angularVelocity": 0.9174618996656708, - "velocityX": 2.8816567846406667, - "velocityY": -1.8197830180329346, - "timestamp": 1.9808747709167092 - }, - { - "x": 4.165386797993937, - "y": 2.276764205392623, - "heading": 0.2569441402014935, - "angularVelocity": 0.06704674940712647, - "velocityX": 3.1057642956915044, - "velocityY": -2.014471793872694, - "timestamp": 2.0392425099835894 - }, - { - "x": 4.350490054886, - "y": 2.162710813518831, - "heading": 0.2569442210228683, - "angularVelocity": 0.0000013846925729023855, - "velocityX": 3.171328200325918, - "velocityY": -1.9540484811841776, - "timestamp": 2.0976102490504696 - }, - { - "x": 4.53559341768799, - "y": 2.048657593531974, - "heading": 0.2569443018441724, - "angularVelocity": 0.0000013846913619086116, - "velocityX": 3.171330014854447, - "velocityY": -1.9540455362879443, - "timestamp": 2.1559779881173498 - }, - { - "x": 4.720696780504463, - "y": 1.934604373568621, - "heading": 0.25694438266547676, - "angularVelocity": 0.0000013846913668711097, - "velocityX": 3.1713300151025647, - "velocityY": -1.9540455358852609, - "timestamp": 2.21434572718423 - }, - { - "x": 4.9058004208396335, - "y": 1.8205516040071454, - "heading": 0.25694446348677025, - "angularVelocity": 0.0000013846911809077847, - "velocityX": 3.1713347697615966, - "velocityY": -1.9540378192615708, - "timestamp": 2.27271346625111 - }, - { - "x": 5.096209165426888, - "y": 1.7155949376896078, - "heading": 0.25694454460803495, - "angularVelocity": 0.0000013898305117986064, - "velocityX": 3.2622258054072626, - "velocityY": -1.7981965379415077, - "timestamp": 2.3310812053179903 - }, - { - "x": 5.295905298565607, - "y": 1.629613278960399, - "heading": 0.25694462919552075, - "angularVelocity": 0.0000014492164191385246, - "velocityX": 3.4213443304682105, - "velocityY": -1.4731024381582958, - "timestamp": 2.3894489443848705 + "angularVelocity": -3.649032455220026e-29, + "velocityX": -7.926410421294837e-28, + "velocityY": -2.2668198189886467e-28, + "timestamp": 1.397692257853507 + }, + { + "x": 3.0599636784357562, + "y": 2.9660757817861065, + "heading": -0.7576340217382036, + "angularVelocity": 0.5546537792406501, + "velocityX": 0.2851001394727132, + "velocityY": -0.17900749368867203, + "timestamp": 1.4560457379399623 + }, + { + "x": 3.0934189065832722, + "y": 2.945176443498765, + "heading": -0.694726478822482, + "angularVelocity": 1.0780426946690942, + "velocityX": 0.573320187552645, + "velocityY": -0.35815067509902887, + "timestamp": 1.5143992180264176 + }, + { + "x": 3.1439054612634614, + "y": 2.91379685342996, + "heading": -0.6037471047717198, + "angularVelocity": 1.5591079386519706, + "velocityX": 0.8651849830616652, + "velocityY": -0.5377501054318186, + "timestamp": 1.572752698112873 + }, + { + "x": 3.211665554237461, + "y": 2.871860906483434, + "heading": -0.4882361709178232, + "angularVelocity": 1.9795037705164775, + "velocityX": 1.1612005466273392, + "velocityY": -0.7186537441193689, + "timestamp": 1.6311061781993283 + }, + { + "x": 3.2969494486430677, + "y": 2.819230447903095, + "heading": -0.3529801383687169, + "angularVelocity": 2.317874312701035, + "velocityX": 1.4615048541963882, + "velocityY": -0.9019249323667187, + "timestamp": 1.6894596582857837 + }, + { + "x": 3.3999954148233655, + "y": 2.7557448974167267, + "heading": -0.20405555239709897, + "angularVelocity": 2.552111472203101, + "velocityX": 1.765892385983268, + "velocityY": -1.0879479748647278, + "timestamp": 1.747813138372239 + }, + { + "x": 3.5210139565582126, + "y": 2.681298833579533, + "heading": -0.05002957366551629, + "angularVelocity": 2.6395337262384486, + "velocityX": 2.0738873081013947, + "velocityY": -1.275777618179687, + "timestamp": 1.8061666184586944 + }, + { + "x": 3.6599142962012903, + "y": 2.5959840440263875, + "heading": 0.09335553646223951, + "angularVelocity": 2.457181815297378, + "velocityX": 2.3803265792765944, + "velocityY": -1.4620343024399716, + "timestamp": 1.8645200985451498 + }, + { + "x": 3.8149142954051776, + "y": 2.5012941331840683, + "heading": 0.19750147837600968, + "angularVelocity": 1.7847426024886535, + "velocityX": 2.656225455178376, + "velocityY": -1.6226951794824969, + "timestamp": 1.9228735786316051 + }, + { + "x": 3.982928508847516, + "y": 2.3951557532582775, + "heading": 0.25228608290098037, + "angularVelocity": 0.9388403989582608, + "velocityX": 2.879249244319485, + "velocityY": -1.8188868901826996, + "timestamp": 1.9812270587180605 + }, + { + "x": 4.164002224660317, + "y": 2.277675094812323, + "heading": 0.2575212486101159, + "angularVelocity": 0.08971471283939278, + "velocityX": 3.103049133394018, + "velocityY": -2.0132588197292973, + "timestamp": 2.039580538804516 + }, + { + "x": 4.349312428482631, + "y": 2.163516001344807, + "heading": 0.25752132885359325, + "angularVelocity": 0.0000013751275369508418, + "velocityX": 3.175649567905162, + "velocityY": -1.9563373649417417, + "timestamp": 2.0979340188909714 + }, + { + "x": 4.534622740292831, + "y": 2.0493570831701313, + "heading": 0.2575214090969788, + "angularVelocity": 0.0000013751259629129977, + "velocityX": 3.175651418486927, + "velocityY": -1.9563343609591082, + "timestamp": 2.1562874989774268 + }, + { + "x": 4.719933052117692, + "y": 1.9351981650192527, + "heading": 0.25752148934036456, + "angularVelocity": 0.0000013751259684025227, + "velocityX": 3.175651418738155, + "velocityY": -1.9563343605512986, + "timestamp": 2.214640979063882 + }, + { + "x": 4.9052436421415075, + "y": 1.8210396984605661, + "heading": 0.2575215695837395, + "angularVelocity": 0.0000013751257818103147, + "velocityX": 3.17565618621654, + "velocityY": -1.9563266216436723, + "timestamp": 2.2729944591503375 + }, + { + "x": 5.095847642006684, + "y": 1.7159574192298102, + "heading": 0.2575216501181558, + "angularVelocity": 0.0000013801133386874064, + "velocityX": 3.266369025168363, + "velocityY": -1.8007885575130747, + "timestamp": 2.331347939236793 + }, + { + "x": 5.295726350893371, + "y": 1.6298141007255773, + "heading": 0.25752173405231565, + "angularVelocity": 0.0000014383745358820107, + "velocityX": 3.4253091433544425, + "velocityY": -1.4762327521272869, + "timestamp": 2.389701419323248 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": 0.2569447220794932, - "angularVelocity": 0.0000015913580676492416, - "velocityX": 3.5481637974123346, - "velocityY": -1.1340904342963076, - "timestamp": 2.4478166834517507 - }, - { - "x": 5.694707875531767, - "y": 1.52024537146014, - "heading": 0.25694480525283153, - "angularVelocity": 0.0000015766499748638697, - "velocityX": 3.6339835774202793, - "velocityY": -0.8184074026053739, - "timestamp": 2.500569887803966 - }, - { - "x": 5.889460824583239, - "y": 1.4940579532693374, - "heading": 0.25694488397529436, - "angularVelocity": 0.0000014922783143570964, - "velocityX": 3.6917747735506756, - "velocityY": -0.49641379158615856, - "timestamp": 2.553323092156181 - }, - { - "x": 6.0857603375931175, - "y": 1.485058307103912, - "heading": 0.2569449614499782, - "angularVelocity": 0.0000014686251720162004, - "velocityX": 3.7210917406884763, - "velocityY": -0.17059904276786395, - "timestamp": 2.6060762965083963 - }, - { - "x": 6.282233415675428, - "y": 1.4886391007138935, - "heading": 0.2569450396471809, - "angularVelocity": 0.0000014823213805790595, - "velocityX": 3.724381873952661, - "velocityY": 0.06787821998591004, - "timestamp": 2.6588295008606115 - }, - { - "x": 6.478706465106145, - "y": 1.492221466050733, - "heading": 0.25694511784454455, - "angularVelocity": 0.0000014823244316657533, - "velocityX": 3.724381330827469, - "velocityY": 0.06790801394586561, - "timestamp": 2.7115827052128267 - }, - { - "x": 6.675014894210808, - "y": 1.5010244818845797, - "heading": 0.2569451969790386, - "angularVelocity": 0.0000015000888575462877, - "velocityX": 3.7212607559150137, - "velocityY": 0.16687167996605787, - "timestamp": 2.764335909565042 - }, - { - "x": 6.869794082781554, - "y": 1.5270160132210213, - "heading": 0.25694528055287635, - "angularVelocity": 0.0000015842419202847933, - "velocityX": 3.6922721749805327, - "velocityY": 0.4927005222830628, - "timestamp": 2.817089113917257 - }, - { - "x": 7.061541469504376, - "y": 1.5699966670379106, - "heading": 0.256946313055063, - "angularVelocity": 0.000019572312228011845, - "velocityX": 3.6348007495922103, - "velocityY": 0.8147496316986196, - "timestamp": 2.8698423182694723 - }, - { - "x": 7.242025004547978, - "y": 1.6235443273244181, - "heading": 0.2892009263238711, - "angularVelocity": 0.6114247213013846, - "velocityX": 3.4212809868112584, - "velocityY": 1.0150598611790123, - "timestamp": 2.9225955226216875 - }, - { - "x": 7.410147525588679, - "y": 1.6859098332103064, - "heading": 0.355218131661103, - "angularVelocity": 1.2514349819673058, - "velocityX": 3.1869632016702596, - "velocityY": 1.1822126570643, - "timestamp": 2.9753487269739027 - }, - { - "x": 7.565027902002143, - "y": 1.7562974315327362, - "heading": 0.45280543116483274, - "angularVelocity": 1.8498838260548631, - "velocityX": 2.935942533071192, - "velocityY": 1.3342810012539876, - "timestamp": 3.028101931326118 - }, - { - "x": 7.703780357583263, - "y": 1.8320280138994296, - "heading": 0.5628377984662216, - "angularVelocity": 2.0857949512742477, - "velocityX": 2.630218529564891, - "velocityY": 1.4355636457847423, - "timestamp": 3.080855135678333 - }, - { - "x": 7.825662907370359, - "y": 1.9113228263820174, - "heading": 0.6698617562917842, - "angularVelocity": 2.028766956240237, - "velocityX": 2.3104293148398787, - "velocityY": 1.503127884955833, - "timestamp": 3.1336083400305483 - }, - { - "x": 7.930693514616472, - "y": 1.993224495332611, - "heading": 0.7659443680915856, - "angularVelocity": 1.821360673340157, - "velocityX": 1.990980615032594, - "velocityY": 1.552543963088276, - "timestamp": 3.1863615443827635 - }, - { - "x": 8.019011714129224, - "y": 2.0771819587983416, - "heading": 0.8464048765676822, - "angularVelocity": 1.525225044888065, - "velocityX": 1.6741769641722983, - "velocityY": 1.5915140036835516, - "timestamp": 3.2391147487349787 - }, - { - "x": 8.090768492192119, - "y": 2.1628615391485617, - "heading": 0.9080474489079001, - "angularVelocity": 1.1685085881921318, - "velocityX": 1.3602354386626168, - "velocityY": 1.6241587862258966, - "timestamp": 3.291867953087194 - }, - { - "x": 8.146101418716508, - "y": 2.2500495157540152, - "heading": 0.9485346126335614, - "angularVelocity": 0.7674825486494136, - "velocityX": 1.048901715144179, - "velocityY": 1.6527522389602969, - "timestamp": 3.344621157439409 - }, - { - "x": 8.185122291410464, - "y": 2.3385982915715853, - "heading": 0.9662103754331748, - "angularVelocity": 0.33506519682858266, - "velocityX": 0.7396872507199079, - "velocityY": 1.6785478134439036, - "timestamp": 3.3973743617916243 + "heading": 0.25752182612435603, + "angularVelocity": 0.000001577832894961981, + "velocityX": 3.55209742519301, + "velocityY": -1.137809025512455, + "timestamp": 2.4480548994097036 + }, + { + "x": 5.6948016559329036, + "y": 1.5200403690670627, + "heading": 0.2575219089946678, + "angularVelocity": 0.0000015718708675388777, + "velocityX": 3.637995011021535, + "velocityY": -0.8227986628520415, + "timestamp": 2.5007757135098654 + }, + { + "x": 5.889658517300143, + "y": 1.4936016847783737, + "heading": 0.25752198743316407, + "angularVelocity": 0.0000014878088960886408, + "velocityX": 3.6960138930525672, + "velocityY": -0.5014847501872678, + "timestamp": 2.553496527610027 + }, + { + "x": 6.0860809574321495, + "y": 1.4843051335513961, + "heading": 0.2575220646083203, + "angularVelocity": 0.000001463846063481971, + "velocityX": 3.725709541564224, + "velocityY": -0.17633550212854351, + "timestamp": 2.606217341710189 + }, + { + "x": 6.282694965484122, + "y": 1.4876417105496467, + "heading": 0.25752214249001004, + "angularVelocity": 0.0000014772474802220589, + "velocityX": 3.7293431713409246, + "velocityY": 0.06328766076926912, + "timestamp": 2.658938155810351 + }, + { + "x": 6.479308945028881, + "y": 1.4909799669650579, + "heading": 0.2575222203718718, + "angularVelocity": 0.0000014772507428858513, + "velocityX": 3.729342630620652, + "velocityY": 0.06331951568632599, + "timestamp": 2.7116589699105127 + }, + { + "x": 6.675718854872107, + "y": 1.5005375890883978, + "heading": 0.2575222993699314, + "angularVelocity": 0.0000014984226051915619, + "velocityX": 3.7254718690435245, + "velocityY": 0.18128745328518545, + "timestamp": 2.7643797840106745 + }, + { + "x": 6.870540482169492, + "y": 1.5272346660517908, + "heading": 0.25752238291998725, + "angularVelocity": 0.000001584764144444983, + "velocityX": 3.6953455788306457, + "velocityY": 0.5063859012623091, + "timestamp": 2.8171005981108364 + }, + { + "x": 7.062280223610006, + "y": 1.5708674152899489, + "heading": 0.2575250023601284, + "angularVelocity": 0.00004968512314911554, + "velocityX": 3.6368888590422164, + "velocityY": 0.8276190340168537, + "timestamp": 2.869821412210998 + }, + { + "x": 7.242655823175531, + "y": 1.624760581479399, + "heading": 0.29034566188783406, + "angularVelocity": 0.6225370394575329, + "velocityX": 3.4213356270037516, + "velocityY": 1.0222369875977493, + "timestamp": 2.92254222631116 + }, + { + "x": 7.410675905679771, + "y": 1.6874034797847524, + "heading": 0.35686793752538104, + "angularVelocity": 1.261783922971381, + "velocityX": 3.1869781484221007, + "velocityY": 1.188200511971245, + "timestamp": 2.975263040411322 + }, + { + "x": 7.565444154381843, + "y": 1.757937293066464, + "heading": 0.45500614255643107, + "angularVelocity": 1.8614698332351587, + "velocityX": 2.9356194767409227, + "velocityY": 1.337874129707252, + "timestamp": 3.0279838545114837 + }, + { + "x": 7.704030497624387, + "y": 1.8336235028030063, + "heading": 0.5647068511850917, + "angularVelocity": 2.0807855588163995, + "velocityX": 2.628683672813762, + "velocityY": 1.4356039645508802, + "timestamp": 3.0807046686116455 + }, + { + "x": 7.825774409585393, + "y": 1.9127779886685334, + "heading": 0.6711471283329911, + "angularVelocity": 2.018942214087941, + "velocityX": 2.309219120359376, + "velocityY": 1.5013896734436838, + "timestamp": 3.1334254827118073 + }, + { + "x": 7.930703401936529, + "y": 1.9944869953972368, + "heading": 0.7666407114919108, + "angularVelocity": 1.8113070670247255, + "velocityX": 1.990276404908808, + "velocityY": 1.5498434180752323, + "timestamp": 3.186146296811969 + }, + { + "x": 8.018956078741207, + "y": 2.0782208825678086, + "heading": 0.8466105266240161, + "angularVelocity": 1.516854709037202, + "velocityX": 1.6739627092444165, + "velocityY": 1.5882510276015418, + "timestamp": 3.238867110912131 + }, + { + "x": 8.090681009003735, + "y": 2.1636571823312836, + "heading": 0.9079143826816493, + "angularVelocity": 1.1628017720129442, + "velocityX": 1.3604670467770388, + "velocityY": 1.620542118358762, + "timestamp": 3.291587925012293 + }, + { + "x": 8.146013476632273, + "y": 2.250588470468436, + "heading": 0.9482462307641644, + "angularVelocity": 0.7650080669446885, + "velocityX": 1.049537427920118, + "velocityY": 1.6488988195818726, + "timestamp": 3.3443087391124546 + }, + { + "x": 8.185063693678202, + "y": 2.3388710656404412, + "heading": 0.9659659222521745, + "angularVelocity": 0.33610428424616584, + "velocityX": 0.7406982936898286, + "velocityY": 1.6745301960679422, + "timestamp": 3.3970295532126165 }, { "x": 8.207907676696777, "y": 2.428393840789795, "heading": 0.9600708878718816, - "angularVelocity": -0.11638132008630137, - "velocityX": 0.43192419429508555, - "velocityY": 1.702181892471887, - "timestamp": 3.4501275661438395 - }, - { - "x": 8.211423054828616, - "y": 2.5404723922535313, - "heading": 0.9162466453350178, - "angularVelocity": -0.6758725051109615, - "velocityX": 0.054215367724374734, - "velocityY": 1.7285138763843786, - "timestamp": 3.5149685564274566 - }, - { - "x": 8.190202888383553, - "y": 2.653847423099536, - "heading": 0.8391734249663445, - "angularVelocity": -1.188649649420099, - "velocityX": -0.32726468785015433, - "velocityY": 1.748508626257838, - "timestamp": 3.5798095467110738 - }, - { - "x": 8.144008306424402, - "y": 2.7680190139904237, - "heading": 0.7319233890251923, - "angularVelocity": -1.654046853264217, - "velocityX": -0.7124286929779271, - "velocityY": 1.760793448580852, - "timestamp": 3.644650536994691 - }, - { - "x": 8.072604152106313, - "y": 2.8824539716658655, - "heading": 0.5977397407747723, - "angularVelocity": -2.06942626359492, - "velocityX": -1.101219367652527, - "velocityY": 1.764855181497056, - "timestamp": 3.709491527278308 - }, - { - "x": 7.975691382967057, - "y": 2.9965380091624785, - "heading": 0.44171220804306943, - "angularVelocity": -2.4063101450059983, - "velocityX": -1.4946219777853038, - "velocityY": 1.759443170093563, - "timestamp": 3.774332517561925 - }, - { - "x": 7.852920805151411, - "y": 3.109310419044201, - "heading": 0.2733111632769955, - "angularVelocity": -2.5971386931242324, - "velocityX": -1.893409975366543, - "velocityY": 1.7392147989790332, - "timestamp": 3.8391735078455422 - }, - { - "x": 7.704278478364198, - "y": 3.2190283422070105, - "heading": 0.10912005253045581, - "angularVelocity": -2.53221164618803, - "velocityX": -2.292412964963152, - "velocityY": 1.6921074567630605, - "timestamp": 3.9040144981291593 - }, - { - "x": 7.532421373794031, - "y": 3.3230579148672983, - "heading": -0.010926874392962598, - "angularVelocity": -1.8514048967840961, - "velocityX": -2.650439233245175, - "velocityY": 1.6043797635609574, - "timestamp": 3.9688554884127765 - }, - { - "x": 7.337707687800783, - "y": 3.422034456345691, - "heading": -0.0821100823564174, - "angularVelocity": -1.0978118571616002, - "velocityX": -3.002941274363059, - "velocityY": 1.5264501829084578, - "timestamp": 4.033696478696394 + "angularVelocity": -0.11181607266331907, + "velocityX": 0.43330102936525317, + "velocityY": 1.698053732237021, + "timestamp": 3.4497503673127783 + }, + { + "x": 8.211510403454756, + "y": 2.5402965981935948, + "heading": 0.9167116042815631, + "angularVelocity": -0.6681435764586997, + "velocityX": 0.05551610962541542, + "velocityY": 1.7243621747491076, + "timestamp": 3.5146455262547107 + }, + { + "x": 8.19035769452273, + "y": 2.653506399419357, + "heading": 0.8402263386800068, + "angularVelocity": -1.1785974000001183, + "velocityX": -0.3259520321223525, + "velocityY": 1.7445030272144093, + "timestamp": 3.579540685196643 + }, + { + "x": 8.144214527086703, + "y": 2.7675347256027623, + "heading": 0.7336527994831279, + "angularVelocity": -1.6422417470652866, + "velocityX": -0.7110417508540927, + "velocityY": 1.757116062932159, + "timestamp": 3.6444358441385756 + }, + { + "x": 8.072849371790616, + "y": 2.8818580666466, + "heading": 0.6001927782610759, + "angularVelocity": -2.0565481832238106, + "velocityX": -1.0996992142348316, + "velocityY": 1.7616620855514487, + "timestamp": 3.709331003080508 + }, + { + "x": 7.975969161360382, + "y": 2.9958746653060953, + "heading": 0.4448175731480801, + "angularVelocity": -2.3942495502942416, + "velocityX": -1.4928726889616013, + "velocityY": 1.756935348005165, + "timestamp": 3.7742261620224404 + }, + { + "x": 7.853227191663504, + "y": 3.108650032284083, + "heading": 0.2767627605160218, + "angularVelocity": -2.5896355810212537, + "velocityX": -1.8913886905910138, + "velocityY": 1.7378086257389034, + "timestamp": 3.839121320964373 + }, + { + "x": 7.704583468021456, + "y": 3.218490505664157, + "heading": 0.11219872525944127, + "angularVelocity": -2.5358445520386232, + "velocityX": -2.2905209890163487, + "velocityY": 1.692583471108498, + "timestamp": 3.9040164799063053 + }, + { + "x": 7.532642084855597, + "y": 3.3226846755511135, + "heading": -0.008852877537121141, + "angularVelocity": -1.8653410326782291, + "velocityX": -2.6495255727736016, + "velocityY": 1.6055769272433476, + "timestamp": 3.9689116388482377 + }, + { + "x": 7.337826737893983, + "y": 3.4218449378655498, + "heading": -0.08105102953452195, + "angularVelocity": -1.1125352518514178, + "velocityX": -3.002001229952035, + "velocityY": 1.5280070798988774, + "timestamp": 4.03380679779017 }, { "x": 7.120736598968506, "y": 3.5168776512145996, - "heading": -0.09725138680843894, - "angularVelocity": -0.23351439245133218, - "velocityX": -3.3462025777712125, - "velocityY": 1.46270429328826, - "timestamp": 4.098537468980011 - }, - { - "x": 7.010606209659238, - "y": 3.566708155417018, - "heading": -0.09725146950362612, - "angularVelocity": -0.0000025483254840822735, - "velocityX": -3.3937655527961965, - "velocityY": 1.535571150717811, - "timestamp": 4.130988263708164 - }, - { - "x": 6.900475999603528, - "y": 3.6165390557870793, - "heading": -0.09725155219704651, - "angularVelocity": -0.000002548271039111256, - "velocityX": -3.3937600289389183, - "velocityY": 1.5355833589748464, - "timestamp": 4.1634390584363175 - }, - { - "x": 6.7903457895857455, - "y": 3.666369956240961, - "heading": -0.0972516348904687, - "angularVelocity": -0.000002548271094167607, - "velocityX": -3.3937600277701754, - "velocityY": 1.5355833615578496, - "timestamp": 4.195889853164471 - }, - { - "x": 6.680215579567971, - "y": 3.7162008566948606, - "heading": -0.09725171758389267, - "angularVelocity": -0.0000025482711490074116, - "velocityX": -3.393760027769923, - "velocityY": 1.535583361558393, - "timestamp": 4.228340647892624 - }, - { - "x": 6.570085369550196, - "y": 3.7660317571487596, - "heading": -0.09725180027731847, - "angularVelocity": -0.0000025482712052155066, - "velocityX": -3.39376002776992, - "velocityY": 1.53558336155839, - "timestamp": 4.260791442620778 - }, - { - "x": 6.459955159532422, - "y": 3.815862657602659, - "heading": -0.09725188297074605, - "angularVelocity": -0.000002548271260506862, - "velocityX": -3.3937600277699165, - "velocityY": 1.5355833615583867, - "timestamp": 4.293242237348931 + "heading": -0.09725140170443469, + "angularVelocity": -0.249639147727624, + "velocityX": -3.3452439668069256, + "velocityY": 1.4644037382524164, + "timestamp": 4.098701956732103 + }, + { + "x": 7.010606208807297, + "y": 3.5667081535340293, + "heading": -0.09725148357223665, + "angularVelocity": -0.000002526133789142618, + "velocityX": -3.3982114234212517, + "velocityY": 1.5375826960099699, + "timestamp": 4.131110296425792 + }, + { + "x": 6.900475998308352, + "y": 3.616539052924532, + "heading": -0.09725156543808965, + "angularVelocity": -0.000002526073652213915, + "velocityX": -3.3982058797165378, + "velocityY": 1.5375949481363274, + "timestamp": 4.163518636119481 + }, + { + "x": 6.7903457878477695, + "y": 3.666369952399819, + "heading": -0.09725164730394434, + "angularVelocity": -0.0000025260737041987077, + "velocityX": -3.3982058785328007, + "velocityY": 1.537594950752468, + "timestamp": 4.195926975813171 + }, + { + "x": 6.680215577387196, + "y": 3.7162008518751244, + "heading": -0.0972517291698008, + "angularVelocity": -0.0000025260737583939243, + "velocityX": -3.3982058785325444, + "velocityY": 1.5375949507530235, + "timestamp": 4.22833531550686 + }, + { + "x": 6.570085366926623, + "y": 3.766031751350429, + "heading": -0.09725181103565897, + "angularVelocity": -0.0000025260738115534393, + "velocityX": -3.3982058785325413, + "velocityY": 1.5375949507530207, + "timestamp": 4.2607436552005495 + }, + { + "x": 6.45995515646605, + "y": 3.8158626508257334, + "heading": -0.09725189290151888, + "angularVelocity": -0.000002526073864680103, + "velocityX": -3.3982058785325373, + "velocityY": 1.5375949507530176, + "timestamp": 4.293151994894239 + }, + { + "x": 6.3498249460054765, + "y": 3.8656935503010383, + "heading": -0.0972519747673805, + "angularVelocity": -0.000002526073917550957, + "velocityX": -3.3982058785325338, + "velocityY": 1.5375949507530144, + "timestamp": 4.325560334587928 + }, + { + "x": 6.239694735544903, + "y": 3.915524449776343, + "heading": -0.09725205663324382, + "angularVelocity": -0.0000025260739709897513, + "velocityX": -3.39820587853253, + "velocityY": 1.5375949507530113, + "timestamp": 4.357968674281618 + }, + { + "x": 6.12956452508433, + "y": 3.965355349251648, + "heading": -0.0972521384991089, + "angularVelocity": -0.0000025260740241290968, + "velocityX": -3.398205878532526, + "velocityY": 1.5375949507530078, + "timestamp": 4.390377013975307 + }, + { + "x": 6.019434314623757, + "y": 4.015186248726953, + "heading": -0.09725222036497569, + "angularVelocity": -0.0000025260740776848576, + "velocityX": -3.3982058785325227, + "velocityY": 1.537594950753005, + "timestamp": 4.4227853536689965 + }, + { + "x": 5.909304104163184, + "y": 4.065017148202257, + "heading": -0.09725230223084419, + "angularVelocity": -0.000002526074130335594, + "velocityX": -3.398205878532519, + "velocityY": 1.5375949507530018, + "timestamp": 4.455193693362686 + }, + { + "x": 5.799173893702611, + "y": 4.114848047677562, + "heading": -0.09725238409671443, + "angularVelocity": -0.0000025260741837826664, + "velocityX": -3.3982058785325155, + "velocityY": 1.5375949507529985, + "timestamp": 4.487602033056375 + }, + { + "x": 5.689043683242039, + "y": 4.164678947152866, + "heading": -0.09725246596258641, + "angularVelocity": -0.0000025260742375974643, + "velocityX": -3.3982058785325115, + "velocityY": 1.5375949507529956, + "timestamp": 4.520010372750065 + }, + { + "x": 5.578913472781466, + "y": 4.214509846628169, + "heading": -0.09725254782846013, + "angularVelocity": -0.000002526074291245016, + "velocityX": -3.398205878532508, + "velocityY": 1.5375949507529922, + "timestamp": 4.552418712443754 + }, + { + "x": 5.468783262320894, + "y": 4.264340746103473, + "heading": -0.09725262969433558, + "angularVelocity": -0.0000025260743444666228, + "velocityX": -3.398205878532505, + "velocityY": 1.5375949507529894, + "timestamp": 4.5848270521374435 + }, + { + "x": 5.358653051860335, + "y": 4.314171645578806, + "heading": -0.09725271156021274, + "angularVelocity": -0.0000025260743976709886, + "velocityX": -3.398205878532087, + "velocityY": 1.537594950753901, + "timestamp": 4.617235391831133 + }, + { + "x": 5.248522841462595, + "y": 4.364002545192973, + "heading": -0.0972527934260916, + "angularVelocity": -0.0000025260744491958753, + "velocityX": -3.398205876593744, + "velocityY": 1.53759495503778, + "timestamp": 4.649643731524822 }, { - "x": 6.349824949514647, - "y": 3.865693558056558, - "heading": -0.09725196566417542, - "angularVelocity": -0.0000025482713153661305, - "velocityX": -3.3937600277699125, - "velocityY": 1.5355833615583832, - "timestamp": 4.325693032077084 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.000002526093790111687, + "velocityX": -3.398196798757616, + "velocityY": 1.5376150175922905, + "timestamp": 4.682052071218512 + }, + { + "x": 4.913728228480229, + "y": 4.53564015337073, + "heading": -0.09725287529314537, + "angularVelocity": -7.632140537797702e-12, + "velocityX": -3.128186469396145, + "velocityY": 1.6960032846252107, + "timestamp": 4.753871542014187 + }, + { + "x": 4.717146624886484, + "y": 4.642220515088643, + "heading": -0.09725287529324, + "angularVelocity": -1.3176073581660395e-12, + "velocityX": -2.737163075915943, + "velocityY": 1.4840037184502801, + "timestamp": 4.825691012809862 + }, + { + "x": 4.548648109868937, + "y": 4.7335751342307235, + "heading": -0.09725287529317267, + "angularVelocity": 9.375034048852172e-13, + "velocityX": -2.346139746656194, + "velocityY": 1.2720035128354326, + "timestamp": 4.8975104836055365 + }, + { + "x": 4.40823268189015, + "y": 4.809703995488892, + "heading": -0.09725287529304055, + "angularVelocity": 1.8396343305490373e-12, + "velocityX": -1.9551164388034394, + "velocityY": 1.0600030940739407, + "timestamp": 4.9693299544012115 + }, + { + "x": 4.295900340181399, + "y": 4.87060709120911, + "heading": -0.09725287529289224, + "angularVelocity": 2.065048279063882e-12, + "velocityX": -1.5640931416542116, + "velocityY": 0.8480025687391443, + "timestamp": 5.0411494251968865 + }, + { + "x": 4.211651084281453, + "y": 4.916284416798956, + "heading": -0.09725287529275689, + "angularVelocity": 1.8846216293321442e-12, + "velocityX": -1.1730698509271091, + "velocityY": 0.6360019794603747, + "timestamp": 5.112968895992561 + }, + { + "x": 4.155484913882821, + "y": 4.946735969196816, + "heading": -0.09725287529265393, + "angularVelocity": 1.4335935528367604e-12, + "velocityX": -0.7820465644814268, + "velocityY": 0.42400134755229474, + "timestamp": 5.184788366788236 }, { - "x": 6.239694739496873, - "y": 3.915524458510457, - "heading": -0.09725204835760656, - "angularVelocity": -0.000002548271370425602, - "velocityX": -3.3937600277699085, - "velocityY": 1.5355833615583803, - "timestamp": 4.358143826805238 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 7.893009586439382e-13, + "velocityX": -0.3910232810939034, + "velocityY": 0.21200068519471058, + "timestamp": 5.256607837583911 }, { - "x": 6.129564529479099, - "y": 3.965355358964356, - "heading": -0.09725213105103951, - "angularVelocity": -0.0000025482714255163693, - "velocityX": -3.3937600277699054, - "velocityY": 1.5355833615583767, - "timestamp": 4.390594621533391 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -2.6496856571629414e-29, + "velocityX": 3.729224891945568e-30, + "velocityY": 1.9440418039137549e-28, + "timestamp": 5.328427308379586 + }, + { + "x": 4.149339896779893, + "y": 4.948719586004848, + "heading": -0.09725287542661647, + "angularVelocity": -2.0835104193721486e-9, + "velocityX": 0.3410569741164579, + "velocityY": -0.2058673119908494, + "timestamp": 5.3927510715011495 + }, + { + "x": 4.193317137132987, + "y": 4.9224042284052185, + "heading": -0.09725287567219071, + "angularVelocity": -3.8177839227409805e-9, + "velocityX": 0.6836857518734218, + "velocityY": -0.4091078681123934, + "timestamp": 5.457074834622713 + }, + { + "x": 4.259461092392441, + "y": 4.883233120266127, + "heading": -0.09725287600059242, + "angularVelocity": -5.105449510153574e-9, + "velocityX": 1.0282973515472125, + "velocityY": -0.6089679185134605, + "timestamp": 5.521398597744276 + }, + { + "x": 4.3479375321320015, + "y": 4.831496437672959, + "heading": -0.0972528763737988, + "angularVelocity": -5.801998468175167e-9, + "velocityX": 1.3754860637172643, + "velocityY": -0.8043167887331668, + "timestamp": 5.585722360865839 + }, + { + "x": 4.4589703097902795, + "y": 4.767600598916816, + "heading": -0.09725287673913857, + "angularVelocity": -5.679701582955187e-9, + "velocityX": 1.7261548807155662, + "velocityY": -0.993347336277392, + "timestamp": 5.650046123987402 + }, + { + "x": 4.592877291375008, + "y": 4.69215468843508, + "heading": -0.09725287701889467, + "angularVelocity": -4.34918718104189e-9, + "velocityX": 2.081765355233693, + "velocityY": -1.1729088414674105, + "timestamp": 5.714369887108965 + }, + { + "x": 4.750141105667559, + "y": 4.606168779323296, + "heading": -0.0972528770872025, + "angularVelocity": -1.061937612005659e-9, + "velocityX": 2.4448789476968895, + "velocityY": -1.3367673926241201, + "timestamp": 5.778693650230529 + }, + { + "x": 4.931557529116976, + "y": 4.511622240510161, + "heading": -0.09725287670716622, + "angularVelocity": 5.908178394469377e-9, + "velocityX": 2.8203639626395, + "velocityY": -1.4698539734756233, + "timestamp": 5.843017413352092 }, { - "x": 6.019434319461326, - "y": 4.015186259418255, - "heading": -0.09725221374447428, - "angularVelocity": -0.000002548271481519915, - "velocityX": -3.3937600277699014, - "velocityY": 1.5355833615583734, - "timestamp": 4.4230454162615445 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 2.199139032346849e-8, + "velocityX": 3.215536313610633, + "velocityY": -1.5202491390955153, + "timestamp": 5.907341176473655 + }, + { + "x": 5.399102164920658, + "y": 4.3258479650681645, + "heading": -0.09725286980895109, + "angularVelocity": 7.433360780041783e-8, + "velocityX": 3.5340461244770793, + "velocityY": -1.1926966681553401, + "timestamp": 5.981111928318832 + }, + { + "x": 5.668612858103736, + "y": 4.270396554343484, + "heading": -0.0972528665281375, + "angularVelocity": 4.447309416058914e-8, + "velocityX": 3.653354296140342, + "velocityY": -0.7516720290591195, + "timestamp": 6.054882680164009 + }, + { + "x": 5.942881064370487, + "y": 4.248309813681979, + "heading": -0.09725286337373323, + "angularVelocity": 4.275955167599945e-8, + "velocityX": 3.7178448017225065, + "velocityY": -0.29939698470009923, + "timestamp": 6.128653432009186 + }, + { + "x": 6.2174671961178944, + "y": 4.2306087019584515, + "heading": -0.09725286022607692, + "angularVelocity": 4.266807980333545e-8, + "velocityX": 3.7221544430465427, + "velocityY": -0.23994755754525748, + "timestamp": 6.202424183854363 + }, + { + "x": 6.492053332978151, + "y": 4.212907669547494, + "heading": -0.09725285707842057, + "angularVelocity": 4.2668079985198815e-8, + "velocityX": 3.722154512353812, + "velocityY": -0.23994648242309302, + "timestamp": 6.2761949356995395 + }, + { + "x": 6.766639469838484, + "y": 4.195206637137713, + "heading": -0.09725285393076424, + "angularVelocity": 4.2668079920669555e-8, + "velocityX": 3.7221545123548396, + "velocityY": -0.2399464824071602, + "timestamp": 6.349965687544716 + }, + { + "x": 7.041225605850418, + "y": 4.177505591567667, + "heading": -0.09725285078307094, + "angularVelocity": 4.266858084816784e-8, + "velocityX": 3.722154500854352, + "velocityY": -0.23994666080122462, + "timestamp": 6.423736439389893 + }, + { + "x": 7.304754629426189, + "y": 4.1598724351575065, + "heading": -0.0749365249784548, + "angularVelocity": 0.3025091278919278, + "velocityX": 3.5722697272876305, + "velocityY": -0.23902638876674762, + "timestamp": 6.49750719123507 + }, + { + "x": 7.534656876904849, + "y": 4.144407021780378, + "heading": -0.055522378055544455, + "angularVelocity": 0.26316861950458387, + "velocityX": 3.116441702548429, + "velocityY": -0.20964153123429294, + "timestamp": 6.571277943080247 + }, + { + "x": 7.730932333154636, + "y": 4.131109381030838, + "heading": -0.03901074502265599, + "angularVelocity": 0.22382356990940835, + "velocityX": 2.660613472690525, + "velocityY": -0.1802562725326004, + "timestamp": 6.645048694925424 + }, + { + "x": 7.8935809945490485, + "y": 4.119979536241971, + "heading": -0.02540194557699689, + "angularVelocity": 0.18447418665625143, + "velocityX": 2.204785193673574, + "velocityY": -0.15087069753913437, + "timestamp": 6.718819446770601 + }, + { + "x": 8.022602860038761, + "y": 4.111017505530848, + "heading": -0.014696283077208026, + "angularVelocity": 0.14512069122268073, + "velocityX": 1.7489569004324652, + "velocityY": -0.12148487695953097, + "timestamp": 6.792590198615778 + }, + { + "x": 8.117997929239245, + "y": 4.10422330203326, + "heading": -0.006893999068061529, + "angularVelocity": 0.10576392152707405, + "velocityX": 1.2931286019788828, + "velocityY": -0.09209887831761811, + "timestamp": 6.866360950460955 + }, + { + "x": 8.179766201724098, + "y": 4.0995969339999005, + "heading": -0.0019952337355516638, + "angularVelocity": 0.06640525153913249, + "velocityX": 0.8373002977452005, + "velocityY": -0.06271276783336986, + "timestamp": 6.9401317023061315 }, { - "x": 5.9093041094435526, - "y": 4.065017159872154, - "heading": -0.09725229643791083, - "angularVelocity": -0.000002548271536879473, - "velocityX": -3.3937600277698974, - "velocityY": 1.5355833615583703, - "timestamp": 4.455496210989698 + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": -1.3488010425840145e-28, + "angularVelocity": 0.027046406409671455, + "velocityX": 0.3814719827139668, + "velocityY": -0.03332661105134125, + "timestamp": 7.013902454151308 + }, + { + "x": 8.20063541706578, + "y": 4.096934548496511, + "heading": -0.0010815835377122093, + "angularVelocity": -0.014043212095795144, + "velocityX": -0.09442255808535296, + "velocityY": -0.0026468579224898756, + "timestamp": 7.090920698291911 + }, + { + "x": 8.156710596136428, + "y": 4.099093585175751, + "heading": -0.005327811233814745, + "angularVelocity": -0.05513275125242723, + "velocityX": -0.5703170907033845, + "velocityY": 0.028032795389339775, + "timestamp": 7.167938942432514 + }, + { + "x": 8.076133213947529, + "y": 4.103615501486163, + "heading": -0.012738542680979896, + "angularVelocity": -0.09622046736921626, + "velocityX": -1.0462116228175795, + "velocityY": 0.05871227474567349, + "timestamp": 7.244957186573116 + }, + { + "x": 7.958903270196668, + "y": 4.11050027828992, + "heading": -0.023313548124616522, + "angularVelocity": -0.1373052003669573, + "velocityX": -1.5221061588582727, + "velocityY": 0.08939150561765219, + "timestamp": 7.321975430713719 + }, + { + "x": 7.805020764688039, + "y": 4.119747890676612, + "heading": -0.03705257485110129, + "angularVelocity": -0.17838665214703153, + "velocityX": -1.9980006974413145, + "velocityY": 0.12007041305446839, + "timestamp": 7.398993674854322 + }, + { + "x": 7.614485698203272, + "y": 4.131358307939121, + "heading": -0.05395543434106899, + "angularVelocity": -0.21946565620361758, + "velocityX": -2.47389522587572, + "velocityY": 0.15074892179199947, + "timestamp": 7.476011918994924 + }, + { + "x": 7.387298074694608, + "y": 4.1453314935724395, + "heading": -0.07402210734318278, + "angularVelocity": -0.26054441030206643, + "velocityX": -2.949789702994487, + "velocityY": 0.1814269565508347, + "timestamp": 7.553030163135527 + }, + { + "x": 7.1234579111014344, + "y": 4.161667405314612, + "heading": -0.09725284904387498, + "angularVelocity": -0.3016264777249771, + "velocityX": -3.4256839601733207, + "velocityY": 0.21210444258311068, + "timestamp": 7.63004840727613 + }, + { + "x": 6.836784104612281, + "y": 4.180147638379387, + "heading": -0.09725285233031565, + "angularVelocity": -4.2670937406098884e-8, + "velocityX": -3.722154532188625, + "velocityY": 0.23994617471463595, + "timestamp": 7.707066651416732 + }, + { + "x": 6.550110299323958, + "y": 4.198627890073297, + "heading": -0.09725285561668488, + "angularVelocity": -4.2670009668741826e-8, + "velocityX": -3.7221545165971346, + "velocityY": 0.23994641659413532, + "timestamp": 7.784084895557335 + }, + { + "x": 6.26343649403574, + "y": 4.217108141768841, + "heading": -0.0972528589030541, + "angularVelocity": -4.267000957382731e-8, + "velocityX": -3.722154516595767, + "velocityY": 0.23994641661534885, + "timestamp": 7.861103139697938 + }, + { + "x": 5.976762694007825, + "y": 4.235588475064355, + "heading": -0.0972528621894233, + "angularVelocity": -4.2670009669994304e-8, + "velocityX": -3.722154448296313, + "velocityY": 0.23994747610419156, + "timestamp": 7.93812138383854 + }, + { + "x": 5.690447941814378, + "y": 4.258981914103625, + "heading": -0.09725286548373305, + "angularVelocity": -4.277310873834796e-8, + "velocityX": -3.7174925939724037, + "velocityY": 0.3037389296562527, + "timestamp": 8.015139627979142 + }, + { + "x": 5.409459513474409, + "y": 4.318722126567167, + "heading": -0.0972528689233277, + "angularVelocity": -4.4659478908136544e-8, + "velocityX": -3.6483359426761925, + "velocityY": 0.7756631319000459, + "timestamp": 8.092157872119744 }, { - "x": 5.799173899425779, - "y": 4.114848060326053, - "heading": -0.09725237913134915, - "angularVelocity": -0.000002548271591483022, - "velocityX": -3.3937600277698934, - "velocityY": 1.535583361558367, - "timestamp": 4.487947005717851 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -8.269819203889138e-8, + "velocityX": -3.5195113993654044, + "velocityY": 1.2349277693272307, + "timestamp": 8.169176116260346 + }, + { + "x": 4.908992808994229, + "y": 4.524204010558568, + "heading": -0.09725287694865485, + "angularVelocity": -2.3032931953732708e-8, + "velocityX": -3.1905636858497752, + "velocityY": 1.535056957756043, + "timestamp": 8.241075673472013 + }, + { + "x": 4.711042301236892, + "y": 4.628576938226348, + "heading": -0.09725287730265034, + "angularVelocity": -4.9234724349367645e-9, + "velocityX": -2.7531533633035683, + "velocityY": 1.4516491021010645, + "timestamp": 8.31297523068368 + }, + { + "x": 4.542898981308052, + "y": 4.721263372143452, + "heading": -0.0972528771251371, + "angularVelocity": 2.468905958013942e-9, + "velocityX": -2.3385863063640335, + "velocityY": 1.2891099404721302, + "timestamp": 8.384874787895347 + }, + { + "x": 4.403657347383129, + "y": 4.800175356394304, + "heading": -0.09725287672080585, + "angularVelocity": 5.623556763444036e-9, + "velocityX": -1.9366132327492167, + "velocityY": 1.0975308793424334, + "timestamp": 8.456774345107014 + }, + { + "x": 4.2927849587410325, + "y": 4.864247225534064, + "heading": -0.09725287625075327, + "angularVelocity": 6.537628358410225e-9, + "velocityX": -1.5420454998867026, + "velocityY": 0.8911302325706711, + "timestamp": 8.528673902318681 + }, + { + "x": 4.20993568647492, + "y": 4.9128352917829865, + "heading": -0.09725287581430106, + "angularVelocity": 6.070304666979789e-9, + "velocityX": -1.1522918287550885, + "velocityY": 0.6757769885269709, + "timestamp": 8.600573459530349 + }, + { + "x": 4.15486747115832, + "y": 4.945509334282957, + "heading": -0.09725287547872855, + "angularVelocity": 4.6672402894194346e-9, + "velocityX": -0.7659047906857641, + "velocityY": 0.45444010738176244, + "timestamp": 8.672473016742016 }, { - "x": 5.689043689408005, - "y": 4.164678960779951, - "heading": -0.09725246182478926, - "angularVelocity": -0.0000025482716464166734, - "velocityX": -3.39376002776989, - "velocityY": 1.5355833615583636, - "timestamp": 4.520397800446005 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 2.5887684361837973e-9, + "velocityX": -0.3820001604682187, + "velocityY": 0.22882494094405095, + "timestamp": 8.744372573953683 }, { - "x": 5.5789134793902315, - "y": 4.214509861233849, - "heading": -0.09725254451823119, - "angularVelocity": -0.000002548271702485751, - "velocityX": -3.3937600277698863, - "velocityY": 1.5355833615583605, - "timestamp": 4.552848595174158 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 8.091887340768401e-27, + "velocityX": 1.1127966749801626e-25, + "velocityY": 1.4331677688644632e-25, + "timestamp": 8.81627213116535 + }, + { + "x": 4.147895492623476, + "y": 4.945083891393231, + "heading": -0.10366816031073668, + "angularVelocity": -0.09789638985446751, + "velocityX": 0.31273056471816124, + "velocityY": -0.2575538032912827, + "timestamp": 8.881803505996194 + }, + { + "x": 4.189465649349023, + "y": 4.9120367483382665, + "heading": -0.11534265064860806, + "angularVelocity": -0.17815115840323564, + "velocityX": 0.6343550220463237, + "velocityY": -0.5042949753499016, + "timestamp": 8.947334880827038 + }, + { + "x": 4.252832779954382, + "y": 4.863778368414071, + "heading": -0.13076421865787793, + "angularVelocity": -0.23533106163387682, + "velocityX": 0.9669739230853712, + "velocityY": -0.7364164119670211, + "timestamp": 9.012866255657881 + }, + { + "x": 4.338895975770433, + "y": 4.801661455148063, + "heading": -0.14789285263671287, + "angularVelocity": -0.2613806596160893, + "velocityX": 1.3133128373730498, + "velocityY": -0.9478957739914682, + "timestamp": 9.078397630488725 + }, + { + "x": 4.448770493584648, + "y": 4.7277002158009624, + "heading": -0.16386651500043536, + "angularVelocity": -0.24375594751301116, + "velocityX": 1.6766704208149943, + "velocityY": -1.1286386030816227, + "timestamp": 9.143929005319569 + }, + { + "x": 4.583757647526343, + "y": 4.645084735489715, + "heading": -0.1744545811620451, + "angularVelocity": -0.16157247103301028, + "velocityX": 2.0598858835197476, + "velocityY": -1.260701160695988, + "timestamp": 9.209460380150412 + }, + { + "x": 4.744961446623189, + "y": 4.559146001551583, + "heading": -0.17298362527772781, + "angularVelocity": 0.022446589715449678, + "velocityX": 2.459948376071222, + "velocityY": -1.3114135657914419, + "timestamp": 9.274991754981256 + }, + { + "x": 4.9316701909697125, + "y": 4.478506356002848, + "heading": -0.14972977650571964, + "angularVelocity": 0.3548506167012916, + "velocityX": 2.8491504234189664, + "velocityY": -1.2305501869431363, + "timestamp": 9.3405231298121 }, { - "x": 5.468783269372458, - "y": 4.264340761687747, - "heading": -0.09725262721167492, - "angularVelocity": -0.0000025482717576806813, - "velocityX": -3.3937600277698823, - "velocityY": 1.5355833615583572, - "timestamp": 4.585299389902311 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 0.8007904816369491, + "velocityX": 3.154561228516173, + "velocityY": -0.9868900380705385, + "timestamp": 9.406054504642944 + }, + { + "x": 5.280076264990419, + "y": 4.380996510846826, + "heading": -0.04650142784626664, + "angularVelocity": 1.168572303123448, + "velocityX": 3.262315361452851, + "velocityY": -0.7560984617214831, + "timestamp": 9.449484806140488 + }, + { + "x": 5.424449362393288, + "y": 4.359481998207729, + "heading": 0.008484168318164002, + "angularVelocity": 1.2660652647677388, + "velocityX": 3.3242481038505836, + "velocityY": -0.49538022756564715, + "timestamp": 9.492915107638032 + }, + { + "x": 5.570294583588623, + "y": 4.349555966969111, + "heading": 0.06357759732287922, + "angularVelocity": 1.2685481588892704, + "velocityX": 3.3581443408488143, + "velocityY": -0.22855082503120916, + "timestamp": 9.536345409135576 + }, + { + "x": 5.7169007597582056, + "y": 4.351285887819555, + "heading": 0.11807254033661835, + "angularVelocity": 1.2547677804359936, + "velocityX": 3.3756656323897083, + "velocityY": 0.039832116996496504, + "timestamp": 9.57977571063312 + }, + { + "x": 5.863478153664376, + "y": 4.364694740352783, + "heading": 0.1715206036233513, + "angularVelocity": 1.2306629575149417, + "velocityX": 3.375002909304243, + "velocityY": 0.3087441733276142, + "timestamp": 9.623206012130664 + }, + { + "x": 6.009132990316209, + "y": 4.389745616694493, + "heading": 0.223839780266437, + "angularVelocity": 1.2046698926565014, + "velocityX": 3.3537606608618478, + "velocityY": 0.5768064111442623, + "timestamp": 9.666636313628208 + }, + { + "x": 6.152825143314219, + "y": 4.426310930170149, + "heading": 0.27549834591380606, + "angularVelocity": 1.189459061210764, + "velocityX": 3.3085690875560085, + "velocityY": 0.8419309149332661, + "timestamp": 9.710066615125752 + }, + { + "x": 6.293269117022505, + "y": 4.47409538643224, + "heading": 0.327953495669367, + "angularVelocity": 1.2078007277598, + "velocityX": 3.233778464932586, + "velocityY": 1.1002561486890332, + "timestamp": 9.753496916623297 + }, + { + "x": 6.42854194701545, + "y": 4.529460812617931, + "heading": 0.38848470588787376, + "angularVelocity": 1.3937552384233347, + "velocityX": 3.1147108200618043, + "velocityY": 1.2748110023786727, + "timestamp": 9.79692721812084 + }, + { + "x": 6.568582228965848, + "y": 4.592274312002758, + "heading": 0.42163559085903973, + "angularVelocity": 0.7633123378855858, + "velocityX": 3.2244833013263468, + "velocityY": 1.4463058560249087, + "timestamp": 9.840357519618385 + }, + { + "x": 6.710554540851547, + "y": 4.659324904773962, + "heading": 0.44122441359236847, + "angularVelocity": 0.451040450051598, + "velocityX": 3.2689690605469455, + "velocityY": 1.5438666198298632, + "timestamp": 9.883787821115929 + }, + { + "x": 6.8526306110413895, + "y": 4.729009831535272, + "heading": 0.45600063364560606, + "angularVelocity": 0.3402283554046515, + "velocityX": 3.2713581368500897, + "velocityY": 1.604523209797458, + "timestamp": 9.927218122613473 + }, + { + "x": 6.993061785936844, + "y": 4.80083300788703, + "heading": 0.47277807951263723, + "angularVelocity": 0.3863073773038364, + "velocityX": 3.233483767166502, + "velocityY": 1.6537572587613216, + "timestamp": 9.970648424111017 + }, + { + "x": 7.132717903538657, + "y": 4.873239187590198, + "heading": 0.4909920458715961, + "angularVelocity": 0.4193838341184207, + "velocityX": 3.2156377641013956, + "velocityY": 1.6671811432685018, + "timestamp": 10.014078725608561 + }, + { + "x": 7.265296328377782, + "y": 4.947694128111601, + "heading": 0.5290109485090398, + "angularVelocity": 0.8754003846736805, + "velocityX": 3.052671067609889, + "velocityY": 1.7143546775886709, + "timestamp": 10.057509027106105 + }, + { + "x": 7.388337158351145, + "y": 5.0245588832069705, + "heading": 0.5919048706788004, + "angularVelocity": 1.4481576226984543, + "velocityX": 2.833064144864831, + "velocityY": 1.7698416185233745, + "timestamp": 10.10093932860365 }, { - "x": 5.3586530593546975, - "y": 4.314171662141674, - "heading": -0.09725270990512043, - "angularVelocity": -0.0000025482718127641656, - "velocityX": -3.3937600277694844, - "velocityY": 1.5355833615592254, - "timestamp": 4.617750184630465 + "x": 7.501500129699707, + "y": 5.105462551116943, + "heading": 0.6747401864044066, + "angularVelocity": 1.90731615644646, + "velocityX": 2.605622513464768, + "velocityY": 1.8628391956833978, + "timestamp": 10.144369630101194 + }, + { + "x": 7.646241899595399, + "y": 5.218086028321965, + "heading": 0.7163743700532657, + "angularVelocity": 0.6780169831508551, + "velocityX": 2.357134680201247, + "velocityY": 1.8340849646658135, + "timestamp": 10.205775442467985 + }, + { + "x": 7.771890778556216, + "y": 5.317734467190133, + "heading": 0.7437412121017494, + "angularVelocity": 0.445671850817889, + "velocityX": 2.0462049782891385, + "velocityY": 1.6227851245244513, + "timestamp": 10.267181254834776 + }, + { + "x": 7.878702605098028, + "y": 5.403903098685203, + "heading": 0.7591487568785513, + "angularVelocity": 0.25091345888837385, + "velocityX": 1.7394416330460891, + "velocityY": 1.4032650684655832, + "timestamp": 10.328587067201568 + }, + { + "x": 7.966772466463858, + "y": 5.476419180889776, + "heading": 0.7633894511988232, + "angularVelocity": 0.06906014523415532, + "velocityX": 1.4342267933818382, + "velocityY": 1.180931892430925, + "timestamp": 10.389992879568359 + }, + { + "x": 8.036149930453574, + "y": 5.535195459636144, + "heading": 0.7568637162743023, + "angularVelocity": -0.10627226760784944, + "velocityX": 1.1298191704607576, + "velocityY": 0.9571777732583954, + "timestamp": 10.45139869193515 + }, + { + "x": 8.086865488706417, + "y": 5.5801792035680275, + "heading": 0.7398132249499985, + "angularVelocity": -0.27766901319466764, + "velocityX": 0.8259081070356892, + "velocityY": 0.7325649184996513, + "timestamp": 10.512804504301942 + }, + { + "x": 8.118939863866766, + "y": 5.611334997780328, + "heading": 0.7123999066441988, + "angularVelocity": -0.4464287214710813, + "velocityX": 0.5223345140157242, + "velocityY": 0.5073753283516645, + "timestamp": 10.574210316668733 }, { - "x": 5.2485228493974105, - "y": 4.364002562729252, - "heading": -0.09725279259856771, - "angularVelocity": -0.0000025482718674912185, - "velocityX": -3.393760025905934, - "velocityY": 1.5355833656778057, - "timestamp": 4.650200979358618 + "x": 8.1323881149292, + "y": 5.628637313842773, + "heading": 0.6747401864044066, + "angularVelocity": -0.6132924358176748, + "velocityX": 0.21900615827864758, + "velocityY": 0.28177000507860306, + "timestamp": 10.635616129035524 + }, + { + "x": 8.125871504088575, + "y": 5.631456874364778, + "heading": 0.6237350082465105, + "angularVelocity": -0.7877273460269657, + "velocityX": -0.10064296896850039, + "velocityY": 0.04354547924694944, + "timestamp": 10.700365916401863 + }, + { + "x": 8.098646920538245, + "y": 5.618838281388744, + "heading": 0.5620672547636553, + "angularVelocity": -0.9524008647928723, + "velocityX": -0.420458269558492, + "velocityY": -0.19488238478129047, + "timestamp": 10.7651157037682 + }, + { + "x": 8.050701885841999, + "y": 5.590765848775558, + "heading": 0.4905264994241073, + "angularVelocity": -1.1048801586758534, + "velocityX": -0.7404662879429073, + "velocityY": -0.43355250657982763, + "timestamp": 10.82986549113454 + }, + { + "x": 7.982021903063244, + "y": 5.547220937725296, + "heading": 0.4101242431657529, + "angularVelocity": -1.2417377651521577, + "velocityX": -1.0606981979751946, + "velocityY": -0.6725104872375124, + "timestamp": 10.894615278500877 + }, + { + "x": 7.892590228702109, + "y": 5.488181520264753, + "heading": 0.3222094538660446, + "angularVelocity": -1.3577618224799397, + "velocityX": -1.3811886957273356, + "velocityY": -0.9118086693707895, + "timestamp": 10.959365065867216 + }, + { + "x": 7.782388469757928, + "y": 5.4136218029079215, + "heading": 0.22870015350775494, + "angularVelocity": -1.444163821407427, + "velocityX": -1.7019632561986164, + "velocityY": -1.151505207808494, + "timestamp": 11.024114853233554 + }, + { + "x": 7.6514017789834075, + "y": 5.323512852150783, + "heading": 0.13259842236832783, + "angularVelocity": -1.4842014939092711, + "velocityX": -2.0229671185394063, + "velocityY": -1.3916485971965442, + "timestamp": 11.088864640599892 + }, + { + "x": 7.49964848633853, + "y": 5.217832335954841, + "heading": 0.039303344877862086, + "angularVelocity": -1.4408553492635434, + "velocityX": -2.3436878917654713, + "velocityY": -1.6321368840646113, + "timestamp": 11.15361442796623 + }, + { + "x": 7.327377192032374, + "y": 5.096661162563075, + "heading": -0.03916824690732434, + "angularVelocity": -1.211920455293751, + "velocityX": -2.660569266914921, + "velocityY": -1.871375618675154, + "timestamp": 11.218364215332569 + }, + { + "x": 7.13646132842574, + "y": 4.963335099646079, + "heading": -0.06425840185235647, + "angularVelocity": -0.3874940129622072, + "velocityX": -2.9485172287358923, + "velocityY": -2.059096536683113, + "timestamp": 11.283114002698907 + }, + { + "x": 6.942657433468542, + "y": 4.819228787923636, + "heading": -0.06425842751021618, + "angularVelocity": -3.962616830885222e-7, + "velocityX": -2.993120175989199, + "velocityY": -2.2255874124670814, + "timestamp": 11.347863790065245 + }, + { + "x": 6.742262463352352, + "y": 4.684438451996059, + "heading": -0.06425845326497977, + "angularVelocity": -3.977582728745682e-7, + "velocityX": -3.0949131768171627, + "velocityY": -2.0817108659363774, + "timestamp": 11.412613577431584 + }, + { + "x": 6.528555264699025, + "y": 4.57194040086737, + "heading": -0.06425848001742611, + "angularVelocity": -4.1316655101928246e-7, + "velocityX": -3.3005081150957944, + "velocityY": -1.7374273446212585, + "timestamp": 11.477363364797922 + }, + { + "x": 6.30400572206588, + "y": 4.483035506841414, + "heading": -0.06425850931637903, + "angularVelocity": -4.5249496742682505e-7, + "velocityX": -3.4679579928610247, + "velocityY": -1.3730530653784834, + "timestamp": 11.54211315216426 + }, + { + "x": 6.071209363502924, + "y": 4.41875151951687, + "heading": -0.06425854338858503, + "angularVelocity": -5.262134035469361e-7, + "velocityX": -3.5953223636990725, + "velocityY": -0.9928061533367126, + "timestamp": 11.606862939530599 + }, + { + "x": 5.832857094389743, + "y": 4.379831537545609, + "heading": -0.06425861345468233, + "angularVelocity": -0.0000010821054427575679, + "velocityX": -3.6811282138216472, + "velocityY": -0.6010827765512304, + "timestamp": 11.671612726896937 + }, + { + "x": 5.593007284030673, + "y": 4.3668097640136185, + "heading": -0.06894618880268864, + "angularVelocity": -0.07239522380954139, + "velocityX": -3.704256339902067, + "velocityY": -0.20110913196235056, + "timestamp": 11.736362514263275 + }, + { + "x": 5.359682212088612, + "y": 4.37881821771202, + "heading": -0.08316362699074491, + "angularVelocity": -0.2195750560170572, + "velocityX": -3.6034878481062056, + "velocityY": 0.1854593534100846, + "timestamp": 11.801112301629614 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": -0.0000025482898100852076, - "velocityX": -3.393751218037592, - "velocityY": 1.5356028315921753, - "timestamp": 4.6826517740867715 + "angularVelocity": -0.21759528293334457, + "velocityX": -3.417606386475971, + "velocityY": 0.5407875255417884, + "timestamp": 11.865862088995952 + }, + { + "x": 4.905742763187506, + "y": 4.481316212156489, + "heading": -0.10678575188012002, + "angularVelocity": -0.1295126274523243, + "velocityX": -3.1607598704228184, + "velocityY": 0.916804725056799, + "timestamp": 11.939467858371852 + }, + { + "x": 4.7004457351805575, + "y": 4.5681827460061015, + "heading": -0.11066751280962474, + "angularVelocity": -0.052737183000978086, + "velocityX": -2.7891431574949794, + "velocityY": 1.1801593079747448, + "timestamp": 12.013073627747753 + }, + { + "x": 4.527868340415521, + "y": 4.662417244866579, + "heading": -0.11053335894042673, + "angularVelocity": 0.001822599917580837, + "velocityX": -2.344617768801401, + "velocityY": 1.280259681537001, + "timestamp": 12.086679397123653 + }, + { + "x": 4.3886358627816255, + "y": 4.752956990495115, + "heading": -0.10811901035095504, + "angularVelocity": 0.03280107809405142, + "velocityX": -1.8915973410024705, + "velocityY": 1.2300631648336415, + "timestamp": 12.160285166499554 + }, + { + "x": 4.28099452080793, + "y": 4.832191591556716, + "heading": -0.1047684534601318, + "angularVelocity": 0.04552030254193966, + "velocityX": -1.462403598065479, + "velocityY": 1.0764726968201856, + "timestamp": 12.233890935875454 + }, + { + "x": 4.202788693101616, + "y": 4.895319939941382, + "heading": -0.10142823841231594, + "angularVelocity": 0.045379799384441775, + "velocityX": -1.0624958935884563, + "velocityY": 0.8576548947171904, + "timestamp": 12.307496705251355 + }, + { + "x": 4.1521113166286625, + "y": 4.9392645069883585, + "heading": -0.0987633314851406, + "angularVelocity": 0.03620513649637662, + "velocityX": -0.6884973406656041, + "velocityY": 0.5970261219953313, + "timestamp": 12.381102474627255 }, { - "x": 4.9137282295128415, - "y": 4.535640155229253, - "heading": -0.09725287529313088, - "angularVelocity": -7.435036019118785e-12, - "velocityX": -3.1302347498710454, - "velocityY": 1.6971138308958593, - "timestamp": 4.754424249221062 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 0.020520894018912154, + "velocityX": -0.33570042229438307, + "velocityY": 0.30836223056847767, + "timestamp": 12.454708244003156 }, { - "x": 4.717146626097375, - "y": 4.642220517268037, - "heading": -0.097252875293223, - "angularVelocity": -1.283652180157178e-12, - "velocityX": -2.7389553313809003, - "velocityY": 1.4849754288028512, - "timestamp": 4.826196724355352 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 6.898541114622747e-27, + "velocityX": -7.290217474147722e-26, + "velocityY": -6.920027775581766e-26, + "timestamp": 12.528314013379056 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.387, + "y": 4.121134281158447, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 4.548648110952986, - "y": 4.733575136181824, - "heading": -0.09725287529315746, - "angularVelocity": 9.133130247777463e-13, - "velocityX": -2.347675969501115, - "velocityY": 1.2728364006237478, - "timestamp": 4.897969199489642 + "timestamp": 0.69255431688601, + "isStopPoint": false, + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 }, { - "x": 4.408232682725308, - "y": 4.809703996992033, - "heading": -0.09725287529302883, - "angularVelocity": 1.7921853517363252e-12, - "velocityX": -1.9563966264916142, - "velocityY": 1.0606971637493035, - "timestamp": 4.969741674623933 + "timestamp": 1.397692257853507, + "isStopPoint": true, + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 }, { - "x": 4.295900340737155, - "y": 4.870607092209374, - "heading": -0.09725287529288444, - "angularVelocity": 2.0118085530494987e-12, - "velocityX": -1.5651172929172832, - "velocityY": 0.8485578225272118, - "timestamp": 5.041514149758223 + "timestamp": 2.4480548994097036, + "isStopPoint": false, + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 19 }, { - "x": 4.211651084582216, - "y": 4.916284417340276, - "heading": -0.09725287529275266, - "angularVelocity": 1.8359969573857423e-12, - "velocityX": -1.1738379650040625, - "velocityY": 0.636418418696543, - "timestamp": 5.113286624892513 + "timestamp": 3.4497503673127783, + "isStopPoint": false, + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 }, { - "x": 4.155484913989614, - "y": 4.946735969389024, - "heading": -0.09725287529265242, - "angularVelocity": 1.3966146295291797e-12, - "velocityX": -0.7825586408649183, - "velocityY": 0.42427897312682855, - "timestamp": 5.185059100026804 + "timestamp": 4.098701956732103, + "isStopPoint": false, + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 }, { - "x": 4.127401828765869, - "y": 4.96196174621582, - "heading": -0.09725287529259724, - "angularVelocity": 7.688238334265391e-13, - "velocityX": -0.3912793194215448, - "velocityY": 0.21213949774351348, - "timestamp": 5.256831575161094 + "timestamp": 4.682052071218512, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { + "timestamp": 5.328427308379586, + "isStopPoint": true, "x": 4.127401828765869, "y": 4.96196174621582, - "heading": -0.09725287529259724, - "angularVelocity": 3.652934582681632e-29, - "velocityX": -2.1119125295164495e-27, - "velocityY": 7.525449232438864e-28, - "timestamp": 5.328604050295384 - }, - { - "x": 4.149345287192321, - "y": 4.948730090302555, - "heading": -0.09725287542061173, - "angularVelocity": -1.9914932727158263e-9, - "velocityX": 0.34136955009475556, - "velocityY": -0.2058419570123514, - "timestamp": 5.392884705732758 + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 4.193331867161711, - "y": 4.922433486987429, - "heading": -0.09725287565526207, - "angularVelocity": -3.650403639422262e-9, - "velocityX": 0.6842895373437039, - "velocityY": -0.4090904664272752, - "timestamp": 5.457165361170132 + "timestamp": 5.907341176473655, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 4.259487364434434, - "y": 4.883286547964955, - "heading": -0.09725287596919821, - "angularVelocity": -4.883835485414618e-9, - "velocityX": 1.0291665015328901, - "velocityY": -0.6090003089749593, - "timestamp": 5.521446016607506 + "timestamp": 7.013902454151308, + "isStopPoint": false, + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 4.347975400700828, - "y": 4.831575784808056, - "heading": -0.09725287632620158, - "angularVelocity": -5.553822878939374e-9, - "velocityX": 1.3765888923239986, - "velocityY": -0.8044529540816499, - "timestamp": 5.5857266720448795 + "timestamp": 8.169176116260346, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 4.45901715765165, - "y": 4.76770271955414, - "heading": -0.09725287667606394, - "angularVelocity": -5.442731629415375e-9, - "velocityX": 1.7274521579669766, - "velocityY": -0.9936592092802463, - "timestamp": 5.650007327482253 + "timestamp": 8.81627213116535, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 4.592927185358519, - "y": 4.69226963680943, - "heading": -0.09725287694462321, - "angularVelocity": -4.1779173473054744e-9, - "velocityX": 2.083208809800217, - "velocityY": -1.1734958555019974, - "timestamp": 5.714287982919627 + "timestamp": 9.406054504642944, + "isStopPoint": false, + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "x": 4.750184246245984, - "y": 4.606276833638444, - "heading": -0.09725287701156488, - "angularVelocity": -1.0413967826345418e-9, - "velocityX": 2.446413463233511, - "velocityY": -1.337771100588195, - "timestamp": 5.778568638357001 + "timestamp": 10.144369630101194, + "isStopPoint": false, + "x": 7.501500129699707, + "y": 5.105462551116943, + "heading": 0.6747401864044066, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 4.931581310093376, - "y": 4.511690334370361, - "heading": -0.0972528766497097, - "angularVelocity": 5.6293012279094655e-9, - "velocityX": 2.821954172886779, - "velocityY": -1.4714613381662665, - "timestamp": 5.842849293794375 + "timestamp": 10.635616129035524, + "isStopPoint": false, + "x": 8.1323881149292, + "y": 5.628637313842773, + "heading": 0.6747401864044066, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { + "timestamp": 11.865862088995952, + "isStopPoint": false, "x": 5.138392925262451, "y": 4.413834095001221, - "heading": -0.09725287529259724, - "angularVelocity": 2.111229970452055e-8, - "velocityX": 3.2173227507078654, - "velocityY": -1.5223279648179473, - "timestamp": 5.907129949231749 + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 5.3988689559213965, - "y": 4.32613897098341, - "heading": -0.0972528699779729, - "angularVelocity": 7.203036405891024e-8, - "velocityX": 3.5302934161705415, - "velocityY": -1.1885528129676428, - "timestamp": 5.980913060138573 + "timestamp": 12.528314013379056, + "isStopPoint": true, + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 5.668131488513907, - "y": 4.271040046607528, - "heading": -0.09725286670603908, - "angularVelocity": 4.4345294956463885e-8, - "velocityX": 3.6493789606207083, - "velocityY": -0.7467687889368456, - "timestamp": 6.054696171045398 + "scope": [ + "last" + ], + "type": "StopPoint" }, { - "x": 5.942117714678702, - "y": 4.249366302589977, - "heading": -0.09725286355772216, - "angularVelocity": 4.266988599202702e-8, - "velocityX": 3.713400299843557, - "velocityY": -0.2937493926614278, - "timestamp": 6.128479281952223 + "scope": [ + 2 + ], + "type": "StopPoint" }, { - "x": 6.216382330790908, - "y": 4.231558335942029, - "heading": -0.09725286041527645, - "angularVelocity": 4.2590311894180227e-8, - "velocityX": 3.7171733848218165, - "velocityY": -0.2413555951908442, - "timestamp": 6.202262392859048 + "scope": [ + 11 + ], + "type": "StopPoint" }, { - "x": 6.490646951405068, - "y": 4.21375043862986, - "heading": -0.09725285727283077, - "angularVelocity": 4.259031148289501e-8, - "velocityX": 3.717173445837876, - "velocityY": -0.2413546554665762, - "timestamp": 6.2760455037658724 + "scope": [ + 7 + ], + "type": "StopPoint" }, { - "x": 6.764911572019294, - "y": 4.195942541318692, - "heading": -0.09725285413038512, - "angularVelocity": 4.2590311325076504e-8, - "velocityX": 3.717173445838756, - "velocityY": -0.241354655453012, - "timestamp": 6.349828614672697 + "scope": [ + 11 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "SourceLanePGFH": { + "waypoints": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 }, { - "x": 7.039176191804646, - "y": 4.178134631242246, - "heading": -0.09725285098790415, - "angularVelocity": 4.259078987217418e-8, - "velocityX": 3.7171734346048453, - "velocityY": -0.24135482846385062, - "timestamp": 6.423611725579522 + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 }, { - "x": 7.303151625715879, - "y": 4.160355738175748, - "heading": -0.07493794888540205, - "angularVelocity": 0.302439160239284, - "velocityX": 3.5777216583424787, - "velocityY": -0.24096155404655972, - "timestamp": 6.497394836486347 + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 }, { - "x": 7.533445930864737, - "y": 4.1447637615713475, - "heading": -0.055524777247044715, - "angularVelocity": 0.2631113191048932, - "velocityX": 3.1212333326481616, - "velocityY": -0.21132175660214245, - "timestamp": 6.5711779473931715 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 19 }, { - "x": 7.730059092320319, - "y": 4.131358730965977, - "heading": -0.0390137097405274, - "angularVelocity": 0.22377841356361194, - "velocityX": 2.664744804591801, - "velocityY": -0.18168155883665832, - "timestamp": 6.644961058299996 + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 }, { - "x": 7.892991106532595, - "y": 4.120140669647355, - "heading": -0.025405081189306844, - "angularVelocity": 0.1844409700806171, - "velocityX": 2.20825622842103, - "velocityY": -0.15204104544723207, - "timestamp": 6.718744169206821 + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": 0.9600708878718816, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 }, { - "x": 8.02224197249394, - "y": 4.111109595697515, - "heading": -0.014699203415963246, - "angularVelocity": 0.1450993003922434, - "velocityX": 1.751767638593706, - "velocityY": -0.12240028698767993, - "timestamp": 6.792527280113646 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 8.117811689845258, - "y": 4.104265522227424, - "heading": -0.006896323202665822, - "angularVelocity": 0.10575428600660502, - "velocityX": 1.2952790438994193, - "velocityY": -0.09275935083211777, - "timestamp": 6.866310391020471 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 8.179700258175622, - "y": 4.099608457473234, - "heading": -0.0019965838975628653, - "angularVelocity": 0.06640732878951748, - "velocityX": 0.8387904436357877, - "velocityY": -0.06311830305000854, - "timestamp": 6.940093501927295 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { "x": 8.207907676696777, "y": 4.097138404846191, - "heading": -2.1241979189811956e-28, - "angularVelocity": 0.02706017505936011, - "velocityX": 0.3823018326887492, - "velocityY": -0.03347720903449786, - "timestamp": 7.01387661283412 + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 }, { - "x": 8.200639571345327, - "y": 4.096943687462525, - "heading": -0.0010802899918473687, - "angularVelocity": -0.014022329834604143, - "velocityX": -0.09434112255025054, - "velocityY": -0.0025274615139553986, - "timestamp": 7.0909173046870135 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 8.15665056358688, - "y": 4.099133352364744, - "heading": -0.005325598680954133, - "angularVelocity": -0.05510475810903996, - "velocityX": -0.5709840695932878, - "velocityY": 0.02842218637392462, - "timestamp": 7.167957996539907 + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 8.075940653456616, - "y": 4.103707386175575, - "heading": -0.012735784669566238, - "angularVelocity": -0.09618535101893455, - "velocityX": -1.0476270161796544, - "velocityY": 0.05937166062274877, - "timestamp": 7.2449986883928 + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 }, { - "x": 7.958509840643352, - "y": 4.1106657697854265, - "heading": -0.023310616019513074, - "angularVelocity": -0.1372629333357902, - "velocityX": -1.5242699668052726, - "velocityY": 0.09032088682612197, - "timestamp": 7.322039380245694 + "x": 6.594663143157959, + "y": 3.3646228313446045, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 17 }, { - "x": 7.804358124937036, - "y": 4.120008478318512, - "heading": -0.037049836652876394, - "angularVelocity": -0.17833719172197895, - "velocityX": -2.0009129201573495, - "velocityY": 0.12126979013798225, - "timestamp": 7.399080072098587 + "x": 7.744622230529785, + "y": 1.5909570455551147, + "heading": -0.9652518160942603, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 7.613485507099167, - "y": 4.131735481105861, - "heading": -0.05395325351914194, - "angularVelocity": -0.21940894428287439, - "velocityX": -2.4775558636250903, - "velocityY": 0.15221829536189774, - "timestamp": 7.476120763951481 + "x": 8.21240234375, + "y": 0.9087779521942139, + "heading": -0.9652518160942603, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 7.385891991055361, - "y": 4.145846741676362, - "heading": -0.07402084166347252, - "angularVelocity": -0.2604803729261546, - "velocityX": -2.9541987561377083, - "velocityY": 0.18316632718520057, - "timestamp": 7.553161455804374 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 }, { - "x": 7.121577593711164, - "y": 4.16234221776091, - "heading": -0.09725284930983917, - "angularVelocity": -0.30155502355466174, - "velocityX": -3.430841429213758, - "velocityY": 0.21411381034903446, - "timestamp": 7.6302021476572675 + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": -2.8840116321326e-26, + "angularVelocity": -5.669163571419092e-28, + "velocityX": 2.108709871721435e-25, + "velocityY": 6.042704411837295e-25, + "timestamp": 0 }, { - "x": 6.835203978246458, - "y": 4.1809363249101, - "heading": -0.09725285259123628, - "angularVelocity": -4.259303804512275e-8, - "velocityX": -3.7171734647908923, - "velocityY": 0.24135436354458084, - "timestamp": 7.707242839510161 + "x": 0.4005024246667089, + "y": 4.115350466677215, + "heading": -0.015208625308300914, + "angularVelocity": -0.3074427174555653, + "velocityX": 0.27295183145226637, + "velocityY": -0.11691994544690736, + "timestamp": 0.04946815925311036 + }, + { + "x": 0.42752782779164794, + "y": 4.103765609299543, + "heading": -0.04521317655829015, + "angularVelocity": -0.6065427075316668, + "velocityX": 0.5463191582824014, + "velocityY": -0.23418816371145634, + "timestamp": 0.09893631850622071 + }, + { + "x": 0.46809993891055535, + "y": 4.086359371416315, + "heading": -0.08951799702765434, + "angularVelocity": -0.8956229853363399, + "velocityX": 0.8201661782342632, + "velocityY": -0.3518675072214983, + "timestamp": 0.14840447775933108 + }, + { + "x": 0.5222480284136939, + "y": 4.063106533888518, + "heading": -0.14747656628903638, + "angularVelocity": -1.1716338375323283, + "velocityX": 1.0946048998120714, + "velocityY": -0.4700566562184248, + "timestamp": 0.19787263701244143 + }, + { + "x": 0.5900100551308932, + "y": 4.033975950010924, + "heading": -0.21819717233761302, + "angularVelocity": -1.4296187106280898, + "velocityX": 1.3698109600255401, + "velocityY": -0.5888754365923217, + "timestamp": 0.24734079626555178 + }, + { + "x": 0.6714360195671307, + "y": 3.9989305950603806, + "heading": -0.30041829025546435, + "angularVelocity": -1.6621018279082538, + "velocityX": 1.6460277816203115, + "velocityY": -0.708442672613487, + "timestamp": 0.29680895551866215 + }, + { + "x": 0.7665898116570742, + "y": 3.9579282066959594, + "heading": -0.39234342678059203, + "angularVelocity": -1.858268791745021, + "velocityX": 1.923536139743475, + "velocityY": -0.82886424284816, + "timestamp": 0.34627711477177253 + }, + { + "x": 0.8755459842805163, + "y": 3.9109205987886493, + "heading": -0.49137598841221336, + "angularVelocity": -2.0019455570381792, + "velocityX": 2.2025515860809297, + "velocityY": -0.9502598968113863, + "timestamp": 0.3957452740248829 + }, + { + "x": 0.9983715175527711, + "y": 3.8578517541730113, + "heading": -0.5934882805424371, + "angularVelocity": -2.06420238132882, + "velocityX": 2.482921037021039, + "velocityY": -1.0727879390883417, + "timestamp": 0.4452134332779933 + }, + { + "x": 1.1350267000973346, + "y": 3.798688114099643, + "heading": -0.6910495610477184, + "angularVelocity": -1.9722035745477453, + "velocityX": 2.7624877215533625, + "velocityY": -1.1959943722718582, + "timestamp": 0.49468159253110366 + }, + { + "x": 1.2838521273183288, + "y": 3.733527390579999, + "heading": -0.7598419105522805, + "angularVelocity": -1.3906389593471014, + "velocityX": 3.008509503244484, + "velocityY": -1.3172255548511609, + "timestamp": 0.544149751784214 + }, + { + "x": 1.4435364367922139, + "y": 3.6623848351594903, + "heading": -0.7899999551615338, + "angularVelocity": -0.6096455793906863, + "velocityX": 3.2280220627745444, + "velocityY": -1.4381484270821239, + "timestamp": 0.5936179110373243 + }, + { + "x": 1.613097529588805, + "y": 3.58963115719978, + "heading": -0.7899999775809353, + "angularVelocity": -4.532087301884173e-7, + "velocityX": 3.4276814693874025, + "velocityY": -1.4707173070147306, + "timestamp": 0.6430860702904346 }, { - "x": 6.5488303639215975, - "y": 4.199530449615618, - "heading": -0.09725285587256685, - "angularVelocity": -4.259217424526975e-8, - "velocityX": -3.7171734499955185, - "velocityY": 0.24135459142842275, - "timestamp": 7.784283531363054 + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "angularVelocity": -4.532019196899546e-7, + "velocityX": 3.427682961038322, + "velocityY": -1.4707138305455802, + "timestamp": 0.692554229543545 }, { - "x": 6.262456749596831, - "y": 4.218124574322582, - "heading": -0.09725285915389743, - "angularVelocity": -4.259217438362485e-8, - "velocityX": -3.7171734499943, - "velocityY": 0.24135459144719548, - "timestamp": 7.861324223215948 + "x": 2.0243963764510555, + "y": 3.413262421692379, + "heading": -0.79, + "angularVelocity": 4.0902201996407894e-16, + "velocityX": 3.428232758686476, + "velocityY": -1.469432170196615, + "timestamp": 0.7630680182190419 }, { - "x": 5.976083139919176, - "y": 4.236718770600948, - "heading": -0.09725286243522813, - "angularVelocity": -4.259217585749446e-8, - "velocityX": -3.717173389674077, - "velocityY": 0.24135552045497527, - "timestamp": 7.938364915068841 + "x": 2.250825412444176, + "y": 3.3162088869723196, + "heading": -0.79, + "angularVelocity": -2.4813985391407045e-16, + "velocityX": 3.211131329719671, + "velocityY": -1.3763766852281583, + "timestamp": 0.8335818068945389 }, { - "x": 5.690024921889411, - "y": 4.259659658304737, - "heading": -0.09725286572349402, - "angularVelocity": -4.26821957688407e-8, - "velocityX": -3.71307955769639, - "velocityY": 0.2977762420357482, - "timestamp": 8.015405606921734 + "x": 2.4489508274763203, + "y": 3.231287040432583, + "heading": -0.79, + "angularVelocity": -2.180277121693513e-16, + "velocityX": 2.8097400345897383, + "velocityY": -1.2043296514749058, + "timestamp": 0.9040955955700358 }, { - "x": 5.4092552652272206, - "y": 4.319022844462833, - "heading": -0.09725286915389218, - "angularVelocity": -4.452709450605306e-8, - "velocityX": -3.644433219762762, - "velocityY": 0.7705432639604047, - "timestamp": 8.092446298774627 + "x": 2.618772615081807, + "y": 3.1584968848445327, + "heading": -0.79, + "angularVelocity": -1.8012058014682582e-16, + "velocityX": 2.40834864776595, + "velocityY": -1.0322825784192244, + "timestamp": 0.9746093842455328 }, { - "x": 5.138392925262451, - "y": 4.413834095001221, - "heading": -0.09725287529259724, - "angularVelocity": -7.968133349338986e-8, - "velocityX": -3.5158347290283, - "velocityY": 1.2306645781352283, - "timestamp": 8.16948699062752 + "x": 2.760290773105409, + "y": 3.0978384211319567, + "heading": -0.79, + "angularVelocity": -1.4736426874017007e-16, + "velocityX": 2.006957230377533, + "velocityY": -0.8602354922627292, + "timestamp": 1.0451231729210297 }, { - "x": 4.909018653493503, - "y": 4.524275328875486, - "heading": -0.09725287687080804, - "angularVelocity": -2.1964896325716688e-8, - "velocityX": -3.192337867898693, - "velocityY": 1.5370761958403918, - "timestamp": 8.241338492962369 + "x": 2.8735053004695117, + "y": 3.0493116497567487, + "heading": -0.79, + "angularVelocity": -8.058066541294709e-17, + "velocityX": 1.6055657977068005, + "velocityY": -0.6881883995558266, + "timestamp": 1.1156369615965267 }, { - "x": 4.71108590609518, - "y": 4.628682093882472, - "heading": -0.09725287720527089, - "angularVelocity": -4.654917845961093e-9, - "velocityX": -2.754747513502236, - "velocityY": 1.453090911313464, - "timestamp": 8.313189995297217 + "x": 2.9584161965275473, + "y": 3.0129165709960453, + "heading": -0.79, + "angularVelocity": -9.176897771941303e-17, + "velocityX": 1.2041743558666786, + "velocityY": -0.5161413029186794, + "timestamp": 1.1861507502720237 }, { - "x": 4.54294597125426, - "y": 4.721368217002082, - "heading": -0.09725287703485493, - "angularVelocity": 2.3717800726027118e-9, - "velocityX": -2.3401032598781413, - "velocityY": 1.2899677822694209, - "timestamp": 8.385041497632065 + "x": 3.01502346084847, + "y": 2.988653185034604, + "heading": -0.79, + "angularVelocity": -1.736982373628201e-17, + "velocityX": 0.8027829079136304, + "velocityY": -0.3440942036613692, + "timestamp": 1.2566645389475206 }, { - "x": 4.40369795049572, - "y": 4.80026148816612, - "heading": -0.09725287664966911, - "angularVelocity": 5.360859554294991e-9, - "velocityX": -1.937997344990855, - "velocityY": 1.0980044759032763, - "timestamp": 8.456892999966913 + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -2.7589092071586022e-17, + "velocityX": 0.4013914555942062, + "velocityY": -0.172047102532514, + "timestamp": 1.3271783276230176 }, { - "x": 4.292814205558175, - "y": 4.864307082758427, - "heading": -0.09725287620265413, - "angularVelocity": 6.221372875954885e-9, - "velocityX": -1.5432348849269109, - "velocityY": 0.8913605493429588, - "timestamp": 8.52874450230176 + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -2.3649328254113597e-28, + "velocityX": 1.8626809073397165e-27, + "velocityY": -1.7812474793700614e-27, + "timestamp": 1.3976921162985145 + }, + { + "x": 3.0599636803008443, + "y": 2.9660757998478573, + "heading": -0.7576339587685645, + "angularVelocity": 0.5546549208152399, + "velocityX": 0.2851002035438971, + "velocityY": -0.17900720432616107, + "timestamp": 1.4560455898129356 + }, + { + "x": 3.0934189139338764, + "y": 2.9451764981868647, + "heading": -0.6947262972943399, + "angularVelocity": 1.078044847813185, + "velocityX": 0.573320346127542, + "velocityY": -0.3581500877720281, + "timestamp": 1.5143990633273567 + }, + { + "x": 3.1439054797379202, + "y": 2.91379696376208, + "heading": -0.6037467592544796, + "angularVelocity": 1.5591109245172228, + "velocityX": 0.8651852711315816, + "velocityY": -0.537749212427429, + "timestamp": 1.5727525368417778 + }, + { + "x": 3.211665591806839, + "y": 2.871861091639316, + "heading": -0.4882356317885913, + "angularVelocity": 1.9795073113743915, + "velocityX": 1.1612010046355354, + "velocityY": -0.7186525428070697, + "timestamp": 1.631106010356199 + }, + { + "x": 3.2969495157584743, + "y": 2.8192307268976187, + "heading": -0.3529793972138387, + "angularVelocity": 2.3178780358520816, + "velocityX": 1.461505525126263, + "velocityY": -0.9019234258384052, + "timestamp": 1.68945948387062 + }, + { + "x": 3.3999955244455466, + "y": 2.755745289292902, + "heading": -0.2040546245446104, + "angularVelocity": 2.552114959059364, + "velocityX": 1.7658933133021877, + "velocityY": -1.0879461629482488, + "timestamp": 1.7478129573850412 + }, + { + "x": 3.5210141241877033, + "y": 2.681299358314012, + "heading": -0.05002850479601411, + "angularVelocity": 2.63953644011494, + "velocityX": 2.073888535739847, + "velocityY": -1.2757754850787486, + "timestamp": 1.8061664308994623 + }, + { + "x": 3.659914538226331, + "y": 2.5959847244718666, + "heading": 0.09335665207111186, + "angularVelocity": 2.4571828930062045, + "velocityX": 2.380328122271953, + "velocityY": -1.4620317986908105, + "timestamp": 1.8645199044138834 + }, + { + "x": 3.8149146487433727, + "y": 2.5012950214969263, + "heading": 0.197502595567386, + "angularVelocity": 1.7847428306137818, + "velocityX": 2.6562276619014966, + "velocityY": -1.622691800026939, + "timestamp": 1.9228733779283045 + }, + { + "x": 3.982928880488226, + "y": 2.395156661504235, + "heading": 0.2522871475824207, + "angularVelocity": 0.9388396048351036, + "velocityX": 2.8792498822427737, + "velocityY": -1.818886753441695, + "timestamp": 1.9812268514427256 + }, + { + "x": 4.164002347253148, + "y": 2.2776755114172382, + "heading": 0.25752210352171573, + "angularVelocity": 0.08971112813020911, + "velocityX": 3.1030452149549066, + "velocityY": -2.013267471694952, + "timestamp": 2.0395803249571465 + }, + { + "x": 4.349312713000023, + "y": 2.1635166721855787, + "heading": 0.25752213554345277, + "angularVelocity": 5.487546005300222e-7, + "velocityX": 3.1756527004528508, + "velocityY": -1.9563332284486414, + "timestamp": 2.0979337984715674 + }, + { + "x": 4.534623121850304, + "y": 2.049357902922309, + "heading": 0.25752216756517515, + "angularVelocity": 5.487543495141043e-7, + "velocityX": 3.1756534391133453, + "velocityY": -1.9563320294045097, + "timestamp": 2.1562872719859882 + }, + { + "x": 4.719933530702919, + "y": 1.9351991336628294, + "heading": 0.25752219958689765, + "angularVelocity": 5.487543505371369e-7, + "velocityX": 3.1756534391533564, + "velocityY": -1.9563320293395612, + "timestamp": 2.214640745500409 + }, + { + "x": 4.905244050568848, + "y": 1.8210405446080342, + "heading": 0.2575222316086183, + "angularVelocity": 5.487543204113664e-7, + "velocityX": 3.1756553415819, + "velocityY": -1.9563289411826175, + "timestamp": 2.27299421901483 + }, + { + "x": 5.095847730511029, + "y": 1.715957675476469, + "heading": 0.25752226374645537, + "angularVelocity": 5.507442001002949e-7, + "velocityX": 3.2663639105404108, + "velocityY": -1.8007988694211323, + "timestamp": 2.331347692529251 + }, + { + "x": 5.295726367846681, + "y": 1.629814177194248, + "heading": 0.2575222972409487, + "angularVelocity": 5.739931368957973e-7, + "velocityX": 3.4253083029625735, + "velocityY": -1.4762359992320253, + "timestamp": 2.3897011660436718 }, { - "x": 4.209952469660844, - "y": 4.91286872561946, - "heading": -0.09725287578795568, - "angularVelocity": 5.7716044742121175e-9, - "velocityX": -1.1532359547775557, - "velocityY": 0.6758612037744439, - "timestamp": 8.600596004636609 + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.25752233398290164, + "angularVelocity": 6.296446587487592e-7, + "velocityX": 3.5520975347185666, + "velocityY": -1.1378104640967532, + "timestamp": 2.4480546395580927 + }, + { + "x": 5.694801641426809, + "y": 1.520040343984435, + "heading": 0.25752236705287185, + "angularVelocity": 6.272660461318871e-7, + "velocityX": 3.637995437353645, + "velocityY": -0.8227992972681996, + "timestamp": 2.5007754434925817 + }, + { + "x": 5.889658502180153, + "y": 1.4936017291226997, + "heading": 0.25752239835429896, + "angularVelocity": 5.937205950740546e-7, + "velocityX": 3.6960145940770253, + "velocityY": -0.5014835300043611, + "timestamp": 2.553496247427071 + }, + { + "x": 6.086080950951087, + "y": 1.484305583780122, + "heading": 0.25752242915160917, + "angularVelocity": 5.841585845609402e-7, + "velocityX": 3.7257104238207144, + "velocityY": -0.17632783737761368, + "timestamp": 2.60621705136156 + }, + { + "x": 6.282694937149082, + "y": 1.4876428211340982, + "heading": 0.257522460230909, + "angularVelocity": 5.895073207644364e-7, + "velocityX": 3.7293434759133812, + "velocityY": 0.06330019849702921, + "timestamp": 2.658937855296049 + }, + { + "x": 6.479308911969213, + "y": 1.490980728749722, + "heading": 0.25752249131023625, + "angularVelocity": 5.895078402009839e-7, + "velocityX": 3.7293432600998244, + "velocityY": 0.06331291191559962, + "timestamp": 2.711658659230538 + }, + { + "x": 6.675718826372289, + "y": 1.500538040759823, + "heading": 0.25752252283498844, + "angularVelocity": 5.979565905391123e-7, + "velocityX": 3.725472673882885, + "velocityY": 0.18128160606156274, + "timestamp": 2.764379463165027 + }, + { + "x": 6.870540392297196, + "y": 1.5272354931118775, + "heading": 0.25752255617569925, + "angularVelocity": 6.324014114235091e-7, + "velocityX": 3.695345127266883, + "velocityY": 0.5063931192177723, + "timestamp": 2.817100267099516 + }, + { + "x": 7.062280392097909, + "y": 1.5708688177166172, + "heading": 0.25752360307904704, + "angularVelocity": 0.000019857499690926623, + "velocityX": 3.6368944608464226, + "velocityY": 0.8276301070628356, + "timestamp": 2.8698210710340053 + }, + { + "x": 7.242656103418754, + "y": 1.6247622711713998, + "heading": 0.29034352591414114, + "angularVelocity": 0.622523186024936, + "velocityX": 3.4213384064662518, + "velocityY": 1.0222426335104984, + "timestamp": 2.9225418749684944 + }, + { + "x": 7.410676149226542, + "y": 1.6874049568089853, + "heading": 0.35686615036971664, + "angularVelocity": 1.2617907825957404, + "velocityX": 3.1869780668855143, + "velocityY": 1.188196707232031, + "timestamp": 2.9752626789029835 + }, + { + "x": 7.5654443716076845, + "y": 1.7579385900879527, + "heading": 0.45500472309026196, + "angularVelocity": 1.8614771664425407, + "velocityX": 2.935619543538406, + "velocityY": 1.337870973413318, + "timestamp": 3.0279834828374725 + }, + { + "x": 7.704030677677, + "y": 1.8336246154274285, + "heading": 0.5647056674911662, + "angularVelocity": 2.080790432126545, + "velocityX": 2.628683474582893, + "velocityY": 1.4356007437504827, + "timestamp": 3.0807042867719616 + }, + { + "x": 7.825774553014182, + "y": 1.9127789189520943, + "heading": 0.6711461233913874, + "angularVelocity": 2.0189459939283965, + "velocityX": 2.3092188709500836, + "velocityY": 1.5013865043299246, + "timestamp": 3.1334250907064507 + }, + { + "x": 7.930703511686336, + "y": 1.9944877491725297, + "heading": 0.7666398660798261, + "angularVelocity": 1.811310442213658, + "velocityX": 1.9902761498580215, + "velocityY": 1.5498403689360842, + "timestamp": 3.18614589464094 + }, + { + "x": 8.018956158365414, + "y": 2.0782214675210318, + "heading": 0.8466098380495345, + "angularVelocity": 1.5168579763897216, + "velocityX": 1.6739624606017243, + "velocityY": 1.5882481316587975, + "timestamp": 3.238866698575429 + }, + { + "x": 8.090681062335408, + "y": 2.163657607085554, + "heading": 0.9079138555243396, + "angularVelocity": 1.1628050579612106, + "velocityX": 1.360466810390799, + "velocityY": 1.6205393922043627, + "timestamp": 3.291587502509918 + }, + { + "x": 8.146013507746817, + "y": 2.250588744141006, + "heading": 0.9482458723141572, + "angularVelocity": 0.7650114144680767, + "velocityX": 1.0495372088818153, + "velocityY": 1.6488962718298632, + "timestamp": 3.344308306444407 + }, + { + "x": 8.18506370691749, + "y": 2.3388711976421583, + "heading": 0.9659657399574388, + "angularVelocity": 0.33610769033985816, + "velocityX": 0.7406980974568496, + "velocityY": 1.6745278317616754, + "timestamp": 3.397029110378896 }, { - "x": 4.154873707750821, - "y": 4.945521497298995, - "heading": -0.09725287546928488, - "angularVelocity": 4.435130638682436e-9, - "velocityX": -0.7665638173206213, - "velocityY": 0.4544480020384788, - "timestamp": 8.672447506971457 + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "angularVelocity": -0.11181263648563311, + "velocityX": 0.4333008617940415, + "velocityY": 1.6980515558692484, + "timestamp": 3.449749914313385 + }, + { + "x": 8.211510391317606, + "y": 2.540296472438403, + "heading": 0.9167118198425396, + "angularVelocity": -0.6681402574181382, + "velocityX": 0.055515922817721344, + "velocityY": 1.7243602437412162, + "timestamp": 3.514645072998917 + }, + { + "x": 8.190357670268208, + "y": 2.653506168063042, + "heading": 0.8402267432816896, + "angularVelocity": -1.1785944916396733, + "velocityX": -0.32595222013246916, + "velocityY": 1.7445014068496127, + "timestamp": 3.5795402316844487 + }, + { + "x": 8.14421449138386, + "y": 2.767534412566691, + "heading": 0.7336533483609798, + "angularVelocity": -1.6422395303344925, + "velocityX": -0.711041930076003, + "velocityY": 1.7571148112327775, + "timestamp": 3.6444353903699804 + }, + { + "x": 8.072849326580373, + "y": 2.8818576996728056, + "heading": 0.6001933866162775, + "angularVelocity": -2.056547274834806, + "velocityX": -1.0996993650837186, + "velocityY": 1.7616612613600555, + "timestamp": 3.709330549055512 + }, + { + "x": 7.975969110351945, + "y": 2.995874279761309, + "heading": 0.444818066858235, + "angularVelocity": -2.3942513263733436, + "velocityX": -1.492872784207061, + "velocityY": 1.756935068777684, + "timestamp": 3.774225707741044 + }, + { + "x": 7.853227135461901, + "y": 3.1086496791956706, + "heading": 0.2767628269059192, + "angularVelocity": -2.589642176031597, + "velocityX": -1.8913887780878373, + "velocityY": 1.7378091327405185, + "timestamp": 3.8391208664265757 + }, + { + "x": 7.704583384688799, + "y": 3.218490261217407, + "heading": 0.11219782786286417, + "angularVelocity": -2.5358594134965102, + "velocityX": -2.290521416141349, + "velocityY": 1.6925851519063353, + "timestamp": 3.9040160251121074 + }, + { + "x": 7.53264205332364, + "y": 3.322684523690155, + "heading": -0.008853804183353416, + "angularVelocity": -1.8653414907698829, + "velocityX": -2.6495247850205796, + "velocityY": 1.6055783602849627, + "timestamp": 3.968911183797639 + }, + { + "x": 7.337826739220927, + "y": 3.421844874305526, + "heading": -0.08105196591494265, + "angularVelocity": -1.112535406245734, + "velocityX": -3.00200073547469, + "velocityY": 1.5280084466066417, + "timestamp": 4.033806342483171 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": -0.09725228724922261, + "angularVelocity": -0.2496383653637915, + "velocityX": -3.3452440004715016, + "velocityY": 1.4644047234645439, + "timestamp": 4.098701501168702 + }, + { + "x": 7.0106063205998925, + "y": 3.566708400606869, + "heading": -0.09725231991858449, + "angularVelocity": -0.0000010080544024334555, + "velocityX": -3.398208767574138, + "velocityY": 1.5375906788578377, + "timestamp": 4.1311098332934035 + }, + { + "x": 6.900476113958648, + "y": 3.616539308522766, + "heading": -0.09725235258763598, + "angularVelocity": -0.000001008044825380236, + "velocityX": -3.398206554335633, + "velocityY": 1.5375955703044946, + "timestamp": 4.163518165418105 + }, + { + "x": 6.790345907323517, + "y": 3.6663702164521736, + "heading": -0.09725238525668778, + "angularVelocity": -0.0000010080448348715788, + "velocityX": -3.3982065541469955, + "velocityY": 1.5375955707213955, + "timestamp": 4.195926497542806 + }, + { + "x": 6.680215700688386, + "y": 3.7162011243815836, + "heading": -0.09725241792573983, + "angularVelocity": -0.000001008044842341437, + "velocityX": -3.3982065541469777, + "velocityY": 1.5375955707214333, + "timestamp": 4.228334829667507 + }, + { + "x": 6.570085494053255, + "y": 3.7660320323109926, + "heading": -0.09725245059479215, + "angularVelocity": -0.0000010080448508340198, + "velocityX": -3.398206554146978, + "velocityY": 1.5375955707214313, + "timestamp": 4.260743161792209 + }, + { + "x": 6.459955287418124, + "y": 3.8158629402404016, + "heading": -0.09725248326384477, + "angularVelocity": -0.0000010080448595181056, + "velocityX": -3.3982065541469764, + "velocityY": 1.5375955707214315, + "timestamp": 4.29315149391691 + }, + { + "x": 6.349825080782993, + "y": 3.8656938481698107, + "heading": -0.09725251593289766, + "angularVelocity": -0.0000010080448688287921, + "velocityX": -3.3982065541469764, + "velocityY": 1.5375955707214306, + "timestamp": 4.325559826041611 + }, + { + "x": 6.239694874147862, + "y": 3.91552475609922, + "heading": -0.09725254860195084, + "angularVelocity": -0.0000010080448772415697, + "velocityX": -3.3982065541469755, + "velocityY": 1.5375955707214306, + "timestamp": 4.3579681581663126 + }, + { + "x": 6.129564667512731, + "y": 3.965355664028629, + "heading": -0.09725258127100428, + "angularVelocity": -0.0000010080448851885182, + "velocityX": -3.3982065541469755, + "velocityY": 1.5375955707214297, + "timestamp": 4.390376490291014 + }, + { + "x": 6.0194344608776, + "y": 4.015186571958038, + "heading": -0.09725261394005799, + "angularVelocity": -0.0000010080448937492915, + "velocityX": -3.3982065541469746, + "velocityY": 1.5375955707214293, + "timestamp": 4.422784822415715 + }, + { + "x": 5.9093042542424685, + "y": 4.065017479887447, + "heading": -0.09725264660911201, + "angularVelocity": -0.0000010080449031799624, + "velocityX": -3.398206554146974, + "velocityY": 1.5375955707214286, + "timestamp": 4.455193154540416 + }, + { + "x": 5.7991740476073375, + "y": 4.114848387816856, + "heading": -0.0972526792781663, + "angularVelocity": -0.000001008044911324413, + "velocityX": -3.3982065541469737, + "velocityY": 1.5375955707214282, + "timestamp": 4.487601486665118 + }, + { + "x": 5.689043840972206, + "y": 4.164679295746265, + "heading": -0.09725271194722086, + "angularVelocity": -0.0000010080449199147737, + "velocityX": -3.398206554146973, + "velocityY": 1.5375955707214277, + "timestamp": 4.520009818789819 + }, + { + "x": 5.578913634337075, + "y": 4.214510203675674, + "heading": -0.09725274461627566, + "angularVelocity": -0.0000010080449273058883, + "velocityX": -3.398206554146972, + "velocityY": 1.5375955707214277, + "timestamp": 4.55241815091452 + }, + { + "x": 5.468783427701944, + "y": 4.264341111605084, + "heading": -0.09725277728533077, + "angularVelocity": -0.000001008044937071081, + "velocityX": -3.3982065541469724, + "velocityY": 1.537595570721425, + "timestamp": 4.584826483039222 + }, + { + "x": 5.358653221066815, + "y": 4.314172019534495, + "heading": -0.09725280995438615, + "angularVelocity": -0.0000010080449450667608, + "velocityX": -3.3982065541469435, + "velocityY": 1.5375955707214868, + "timestamp": 4.617234815163923 + }, + { + "x": 5.2485230144416954, + "y": 4.364002927486029, + "heading": -0.0972528426234418, + "angularVelocity": -0.0000010080449533993329, + "velocityX": -3.3982065538380732, + "velocityY": 1.5375955714041123, + "timestamp": 4.649643147288624 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.0000010080480324849667, + "velocityX": -3.398202929897321, + "velocityY": 1.5376035805684636, + "timestamp": 4.6820514794133254 + }, + { + "x": 4.91372823368964, + "y": 4.535640205255565, + "heading": -0.09725287529265084, + "angularVelocity": -7.461622400229302e-13, + "velocityX": -3.1281866300757337, + "velocityY": 1.6960041335008067, + "timestamp": 4.753870944854674 + }, + { + "x": 4.717146630995297, + "y": 4.64222057593122, + "heading": -0.0972528752926601, + "angularVelocity": -1.288593597138777e-13, + "velocityX": -2.7371632674554296, + "velocityY": 1.4840039538124186, + "timestamp": 4.825690410296022 + }, + { + "x": 4.5486481153378495, + "y": 4.733575188699939, + "heading": -0.0972528752926535, + "angularVelocity": 9.166586601280667e-14, + "velocityX": -2.3461399304768347, + "velocityY": 1.2720035189251644, + "timestamp": 4.89750987573737 + }, + { + "x": 4.408232686103436, + "y": 4.809704037452307, + "heading": -0.0972528752926406, + "angularVelocity": 1.7989350116390222e-13, + "velocityX": -1.955116602045504, + "velocityY": 1.0600029989716206, + "timestamp": 4.969329341178718 + }, + { + "x": 4.295900342985128, + "y": 4.8706071191336155, + "heading": -0.09725287529262608, + "angularVelocity": 2.0200921674140544e-13, + "velocityX": -1.5640932778878103, + "velocityY": 0.8480024364849348, + "timestamp": 5.0411488066200665 + }, + { + "x": 4.211651085798766, + "y": 4.916284431911041, + "heading": -0.09725287529261284, + "angularVelocity": 1.8429670420443223e-13, + "velocityX": -1.1730699562943, + "velocityY": 0.6360018484783654, + "timestamp": 5.112968272061415 + }, + { + "x": 4.155484914421578, + "y": 4.9467359745627, + "heading": -0.09725287529260278, + "angularVelocity": 1.401436382547871e-13, + "velocityX": -0.7820466364102463, + "velocityY": 0.4240012434585409, + "timestamp": 5.184787737502763 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 2.459066789540458e-9, - "velocityX": -0.38234244368231685, - "velocityY": 0.22880870103743692, - "timestamp": 8.744299009306305 + "angularVelocity": 7.711346668937439e-14, + "velocityX": -0.39102331774723287, + "velocityY": 0.21200062628639202, + "timestamp": 5.256607202944111 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 1.7012427908311302e-26, - "velocityX": 2.173382321303034e-25, - "velocityY": 2.8154271739064046e-25, - "timestamp": 8.816150511641153 - }, - { - "x": 4.147908938552217, - "y": 4.9451046130456335, - "heading": -0.10362139740935014, - "angularVelocity": -0.09725274804530627, - "velocityX": 0.31316100417438686, - "velocityY": -0.2574227575741351, - "timestamp": 8.881634749822124 - }, - { - "x": 4.189500528478716, - "y": 4.912093615670984, - "heading": -0.11521450306933155, - "angularVelocity": -0.17703658135172537, - "velocityX": 0.6351389446045882, - "velocityY": -0.5041060000334822, - "timestamp": 8.947118988003096 - }, - { - "x": 4.2528908246594135, - "y": 4.863879621749849, - "heading": -0.13053557559089707, - "angularVelocity": -0.23396580531675798, - "velocityX": 0.9680237251216575, - "velocityY": -0.7362686848076517, - "timestamp": 9.012603226184067 - }, - { - "x": 4.338972089495466, - "y": 4.801805387831604, - "heading": -0.1475645296449939, - "angularVelocity": -0.2600466085752675, - "velocityX": 1.3145341112186142, - "velocityY": -0.9479263352914945, - "timestamp": 9.078087464365039 - }, - { - "x": 4.4488531713485, - "y": 4.727871318118375, - "heading": -0.16346474025801716, - "angularVelocity": -0.24280973642972667, - "velocityX": 1.677977554680654, - "velocityY": -1.1290361126124002, - "timestamp": 9.14357170254601 - }, - { - "x": 4.583832471873963, - "y": 4.645249641206774, - "heading": -0.17403705358129207, - "angularVelocity": -0.1614482143635436, - "velocityX": 2.0612486954866247, - "velocityY": -1.26170326183331, - "timestamp": 9.209055940726982 - }, - { - "x": 4.74502129849887, - "y": 4.559256593692149, - "heading": -0.17264089042553551, - "angularVelocity": 0.021320598582793455, - "velocityX": 2.4614904456771134, - "velocityY": -1.3131869577069004, - "timestamp": 9.274540178907953 - }, - { - "x": 4.931721031095368, - "y": 4.478536439161179, - "heading": -0.14954301236675954, - "angularVelocity": 0.3527242386930233, - "velocityX": 2.8510636724601572, - "velocityY": -1.2326653981663842, - "timestamp": 9.340024417088925 + "angularVelocity": -5.652824755464892e-28, + "velocityX": 8.289180813930789e-28, + "velocityY": 1.3058854376408088e-27, + "timestamp": 5.328426668385459 + }, + { + "x": 4.149339902607208, + "y": 4.948719603159426, + "heading": -0.09725287534607682, + "angularVelocity": -8.314126318446111e-10, + "velocityX": 0.3410571082022834, + "velocityY": -0.20586707155231002, + "timestamp": 5.39275042330432 + }, + { + "x": 4.1933171531744176, + "y": 4.922404277066319, + "heading": -0.09725287544407178, + "angularVelocity": -1.5234644431523042e-9, + "velocityX": 0.6836859978507643, + "velocityY": -0.4091074304710872, + "timestamp": 5.457074178223181 + }, + { + "x": 4.259461121252347, + "y": 4.8832332111646055, + "heading": -0.0972528755751186, + "angularVelocity": -2.0373002769674516e-9, + "velocityX": 1.0282976819584715, + "velocityY": -0.6089673395330295, + "timestamp": 5.521397933142041 + }, + { + "x": 4.34793757416226, + "y": 4.831496576688514, + "heading": -0.09725287572404445, + "angularVelocity": -2.3152544478823804e-9, + "velocityX": 1.375486443873169, + "velocityY": -0.8043161432561301, + "timestamp": 5.585721688060902 + }, + { + "x": 4.458970362431496, + "y": 4.767600785112478, + "heading": -0.09725287586983122, + "angularVelocity": -2.2664532339146123e-9, + "velocityX": 1.7261552658002488, + "velocityY": -0.9933467294724277, + "timestamp": 5.650045442979763 + }, + { + "x": 4.59287734823679, + "y": 4.692154910649705, + "heading": -0.09725287598146638, + "angularVelocity": -1.7355199749767777e-9, + "velocityX": 2.081765686319258, + "velocityY": -1.1729084310756062, + "timestamp": 5.714369197898623 + }, + { + "x": 4.750141155397852, + "y": 4.606169009566351, + "heading": -0.09725287600872443, + "angularVelocity": -4.2376330088015544e-10, + "velocityX": 2.444879148604377, + "velocityY": -1.3367674382787311, + "timestamp": 5.778692952817484 + }, + { + "x": 4.93155755546803, + "y": 4.511622419087021, + "heading": -0.0972528758570734, + "angularVelocity": 2.3576208162700097e-9, + "velocityX": 2.8203639588363623, + "velocityY": -1.469854964135602, + "timestamp": 5.843016707736345 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": 0.7985148568065102, - "velocityX": 3.1560555624992586, - "velocityY": -0.9880598134339889, - "timestamp": 9.405508655269896 - }, - { - "x": 5.280245938471686, - "y": 4.380962564632748, - "heading": -0.04650808406049355, - "angularVelocity": 1.167166985692144, - "velocityX": 3.2627221399232984, - "velocityY": -0.7560690286372685, - "timestamp": 9.448985545769855 - }, - { - "x": 5.424678195182071, - "y": 4.359487062895353, - "heading": 0.008191327783648325, - "angularVelocity": 1.2581261266647945, - "velocityX": 3.322046610268118, - "velocityY": -0.493952108590108, - "timestamp": 9.492462436269813 - }, - { - "x": 5.570537921062946, - "y": 4.349646196769418, - "heading": 0.06315884602952881, - "angularVelocity": 1.264292768268082, - "velocityX": 3.354879436030857, - "velocityY": -0.22634705501637128, - "timestamp": 9.535939326769771 - }, - { - "x": 5.717103514016266, - "y": 4.35150566579831, - "heading": 0.11770080768702691, - "angularVelocity": 1.2545046582287314, - "velocityX": 3.3711148904142765, - "velocityY": 0.04276913568356193, - "timestamp": 9.57941621726973 - }, - { - "x": 5.863572420206036, - "y": 4.365085009600068, - "heading": 0.17138870164708653, - "angularVelocity": 1.2348604820326599, - "velocityX": 3.368891024759707, - "velocityY": 0.3123347517635909, - "timestamp": 9.622893107769688 - }, - { - "x": 6.009034373006399, - "y": 4.3903410665362745, - "heading": 0.22417378599549884, - "angularVelocity": 1.2140952064744144, - "velocityX": 3.345730366814133, - "velocityY": 0.5809076188700988, - "timestamp": 9.666369998269646 - }, - { - "x": 6.152424125530893, - "y": 4.427134155183534, - "heading": 0.2765912547304818, - "angularVelocity": 1.205639780863199, - "velocityX": 3.2980682582309235, - "velocityY": 0.8462677119766714, - "timestamp": 9.709846888769604 - }, - { - "x": 6.292378614943397, - "y": 4.475129707153334, - "heading": 0.330369231652926, - "angularVelocity": 1.2369324554730028, - "velocityX": 3.2190547162667764, - "velocityY": 1.1039324896026255, - "timestamp": 9.753323779269563 - }, - { - "x": 6.427989965818357, - "y": 4.530754098625934, - "heading": 0.3892449871584701, - "angularVelocity": 1.3541850585105815, - "velocityX": 3.1191593813520333, - "velocityY": 1.2794013286818071, - "timestamp": 9.796800669769521 - }, - { - "x": 6.568195373885816, - "y": 4.593631496863788, - "heading": 0.42157526724387395, - "angularVelocity": 0.743619879748183, - "velocityX": 3.224826027233803, - "velocityY": 1.4462257423381046, - "timestamp": 9.84027756026948 - }, - { - "x": 6.710207768570418, - "y": 4.660649558034501, - "heading": 0.440928265304377, - "angularVelocity": 0.44513298531599427, - "velocityX": 3.26638802940008, - "velocityY": 1.5414639915606807, - "timestamp": 9.883754450769437 - }, - { - "x": 6.85219596425133, - "y": 4.730282709551517, - "heading": 0.4559517105455699, - "angularVelocity": 0.34555013176959587, - "velocityX": 3.265831434772203, - "velocityY": 1.6016129653311566, - "timestamp": 9.927231341269396 - }, - { - "x": 6.992339110580002, - "y": 4.802100760534676, - "heading": 0.47358684784417715, - "angularVelocity": 0.4056209424320313, - "velocityX": 3.223393962105149, - "velocityY": 1.6518672369917733, - "timestamp": 9.970708231769354 - }, - { - "x": 7.1318594634446475, - "y": 4.8743385269135056, - "heading": 0.49238243611774024, - "angularVelocity": 0.4323121561230585, - "velocityX": 3.2090692609394345, - "velocityY": 1.6615209953641499, - "timestamp": 10.014185122269312 - }, - { - "x": 7.264750404712455, - "y": 4.9484671577912, - "heading": 0.5297243146446315, - "angularVelocity": 0.8588902770525219, - "velocityX": 3.0565879882309943, - "velocityY": 1.7050122496171904, - "timestamp": 10.05766201276927 - }, - { - "x": 7.388081081229855, - "y": 5.024967733696522, - "heading": 0.592130827533757, - "angularVelocity": 1.4353950379497669, - "velocityX": 2.8366949682733105, - "velocityY": 1.7595687047903361, - "timestamp": 10.101138903269229 - }, - { - "x": 7.501500129699707, - "y": 5.105462551116943, - "heading": 0.6747401864044066, - "angularVelocity": 1.9000751415451151, - "velocityX": 2.608720337761042, - "velocityY": 1.8514391552564737, - "timestamp": 10.144615793769187 - }, - { - "x": 7.646324439274632, - "y": 5.2181193934144225, - "heading": 0.716438811665056, - "angularVelocity": 0.6791497141368164, - "velocityX": 2.3587681328355696, - "velocityY": 1.8348533498079573, - "timestamp": 10.206014075168227 - }, - { - "x": 7.772039173775546, - "y": 5.317779255820348, - "heading": 0.7438971482542158, - "angularVelocity": 0.44721669668085284, - "velocityX": 2.047528556766434, - "velocityY": 1.62317022781495, - "timestamp": 10.267412356567267 - }, - { - "x": 7.878893815894361, - "y": 5.4039493449354215, - "heading": 0.7593796608518738, - "angularVelocity": 0.2521652437962253, - "velocityX": 1.7403523304560495, - "velocityY": 1.4034609300387069, - "timestamp": 10.328810637966306 - }, - { - "x": 7.966980879790036, - "y": 5.476461142089676, - "heading": 0.7636631361192312, - "angularVelocity": 0.06976539358680627, - "velocityX": 1.4346828915809557, - "velocityY": 1.1810069516927688, - "timestamp": 10.390208919365346 - }, - { - "x": 8.036348554918035, - "y": 5.535229545877986, - "heading": 0.7571399208631822, - "angularVelocity": -0.10624426461798567, - "velocityX": 1.1297983192260301, - "velocityY": 0.9571669181807075, - "timestamp": 10.451607200764386 - }, - { - "x": 8.087026476479373, - "y": 5.580203129397342, - "heading": 0.7400467384743379, - "angularVelocity": -0.27839838509082054, - "velocityX": 0.8253964183780998, - "velocityY": 0.7324892895138765, - "timestamp": 10.513005482163425 - }, - { - "x": 8.119034784241132, - "y": 5.611347352514501, - "heading": 0.7125421599400041, - "angularVelocity": -0.44796984390452543, - "velocityX": 0.5213225359473886, - "velocityY": 0.5072491022142075, - "timestamp": 10.574403763562465 + "angularVelocity": 8.775547292964994e-9, + "velocityX": 3.2155363139998, + "velocityY": -1.5202521091803838, + "timestamp": 5.907340462655205 + }, + { + "x": 5.399102105862753, + "y": 4.325847836745161, + "heading": -0.09725287310436229, + "angularVelocity": 2.9662640662901703e-8, + "velocityX": 3.534045888591934, + "velocityY": -1.1926985982098404, + "timestamp": 5.98111120271319 + }, + { + "x": 5.668612776819124, + "y": 4.270396424489206, + "heading": -0.09725287179514892, + "angularVelocity": 1.7747054810662862e-8, + "velocityX": 3.653354578584066, + "velocityY": -0.7516721699195277, + "timestamp": 6.054881942771174 + }, + { + "x": 5.942881070814964, + "y": 4.248311045956059, + "heading": -0.09725287053637834, + "angularVelocity": 1.706327712659208e-8, + "velocityX": 3.7178465849775284, + "velocityY": -0.2993785681936777, + "timestamp": 6.128652682829158 + }, + { + "x": 6.217467173732042, + "y": 4.230609827633618, + "heading": -0.09725286928029948, + "angularVelocity": 1.702678935422503e-8, + "velocityX": 3.722154646967784, + "velocityY": -0.23994904088704855, + "timestamp": 6.202423422887143 + }, + { + "x": 6.492053278688794, + "y": 4.212908640951192, + "heading": -0.09725286802422056, + "angularVelocity": 1.702679012699192e-8, + "velocityX": 3.722154674616614, + "velocityY": -0.23994861199050874, + "timestamp": 6.276194162945127 + }, + { + "x": 6.7666393836455585, + "y": 4.195207454268953, + "heading": -0.09725286676814171, + "angularVelocity": 1.70267893622157e-8, + "velocityX": 3.7221546746167777, + "velocityY": -0.23994861198797246, + "timestamp": 6.349964903003111 + }, + { + "x": 7.041225488263755, + "y": 4.177506262334816, + "heading": -0.09725286551205693, + "angularVelocity": 1.702686968579546e-8, + "velocityX": 3.7221546700273103, + "velocityY": -0.23994868318012236, + "timestamp": 6.423735643061096 + }, + { + "x": 7.304754521125749, + "y": 4.159872953315235, + "heading": -0.07493632738083242, + "angularVelocity": 0.30251205442271084, + "velocityX": 3.572270423949385, + "velocityY": -0.23902849565725026, + "timestamp": 6.49750638311908 + }, + { + "x": 7.534656768824059, + "y": 4.1444074082490925, + "heading": -0.055522130294338126, + "angularVelocity": 0.26316934154699656, + "velocityX": 3.116442203475328, + "velocityY": -0.2096433498428548, + "timestamp": 6.571277123177064 + }, + { + "x": 7.730932226657292, + "y": 4.131109655486069, + "heading": -0.03901051064994992, + "angularVelocity": 0.22382342418430445, + "velocityX": 2.660613919271525, + "velocityY": -0.18025781973409782, + "timestamp": 6.645047863235049 + }, + { + "x": 7.89358089446588, + "y": 4.119979717938594, + "heading": -0.025401753521899054, + "angularVelocity": 0.18447364249503723, + "velocityX": 2.2047856329046707, + "velocityY": -0.1508719790356963, + "timestamp": 6.718818603293033 + }, + { + "x": 8.022602772932235, + "y": 4.1110176135114935, + "heading": -0.014696144661245043, + "angularVelocity": 0.14511998730444314, + "velocityX": 1.7489573557882623, + "velocityY": -0.12148589562821704, + "timestamp": 6.792589343351017 + }, + { + "x": 8.117997862710347, + "y": 4.104223355212346, + "heading": -0.006893914960978184, + "angularVelocity": 0.10576320224162433, + "velocityX": 1.2931290875370185, + "velocityY": -0.09209963589639363, + "timestamp": 6.866360083409002 + }, + { + "x": 8.179766164065947, + "y": 4.099596951205949, + "heading": -0.001995197471495412, + "angularVelocity": 0.06640461361282764, + "velocityX": 0.8373008228879115, + "velocityY": -0.0627132654865662, + "timestamp": 6.940130823466986 }, { - "x": 8.1323881149292, - "y": 5.628637313842773, - "heading": 0.6747401864044066, - "angularVelocity": -0.6156845545873708, - "velocityX": 0.2174870433470383, - "velocityY": 0.2816033435187223, - "timestamp": 10.635802044961505 - }, - { - "x": 8.125741105489272, - "y": 5.631444479838457, - "heading": 0.6235311710708692, - "angularVelocity": -0.7910339249344661, - "velocityX": -0.1026774276383878, - "velocityY": 0.04336274620875669, - "timestamp": 10.70053885803533 - }, - { - "x": 8.09835684603502, - "y": 5.618815221035742, - "heading": 0.5616094020000137, - "angularVelocity": -0.9565155609411891, - "velocityX": -0.42300907557840733, - "velocityY": -0.19508619907365696, - "timestamp": 10.765275671109155 - }, - { - "x": 8.050222803941077, - "y": 5.59073345676368, - "heading": 0.48977295422299394, - "angularVelocity": -1.1096692031332702, - "velocityX": -0.7435343170052635, - "velocityY": -0.4337835450756361, - "timestamp": 10.83001248418298 - }, - { - "x": 7.981324451485502, - "y": 5.547180062425158, - "heading": 0.40904421087349874, - "angularVelocity": -1.2470299280477726, - "velocityX": -1.0642839704976186, - "velocityY": -0.6727763118158692, - "timestamp": 10.894749297256805 - }, - { - "x": 7.891645067392915, - "y": 5.488132407343552, - "heading": 0.32078665814595236, - "angularVelocity": -1.3633286616518716, - "velocityX": -1.3852919202296423, - "velocityY": -0.9121186582704987, - "timestamp": 10.95948611033063 - }, - { - "x": 7.781166411701721, - "y": 5.413563935212295, - "heading": 0.22693931715370197, - "angularVelocity": -1.4496750231622744, - "velocityX": -1.7065816255922823, - "velocityY": -1.1518712242788174, - "timestamp": 11.024222923404455 - }, - { - "x": 7.649874183396069, - "y": 5.323444723125603, - "heading": 0.13053857941250757, - "angularVelocity": -1.4891177548585681, - "velocityX": -2.0280922410549644, - "velocityY": -1.3920860142423153, - "timestamp": 11.08895973647828 - }, - { - "x": 7.497788734174063, - "y": 5.217751261949818, - "heading": 0.0370488934612777, - "angularVelocity": -1.4441502680185108, - "velocityX": -2.3492884805522913, - "velocityY": -1.6326639535877772, - "timestamp": 11.153696549552105 - }, - { - "x": 7.325168258315647, - "y": 5.096564905445784, - "heading": -0.04136336755772595, - "angularVelocity": -1.2112468516110346, - "velocityX": -2.66649635133507, - "velocityY": -1.871985208259067, - "timestamp": 11.21843336262593 - }, - { - "x": 7.133849861870766, - "y": 4.963599555351033, - "heading": -0.06503952290078549, - "angularVelocity": -0.36572939288900796, - "velocityX": -2.955326148457454, - "velocityY": -2.0539372233711526, - "timestamp": 11.283170175699755 - }, - { - "x": 6.940221298800236, - "y": 4.819868100403324, - "heading": -0.06503954882158174, - "angularVelocity": -4.004027232662301e-7, - "velocityX": -2.9910116651821412, - "velocityY": -2.2202429826719707, - "timestamp": 11.34790698877358 - }, - { - "x": 6.740356177089726, - "y": 4.684943363475215, - "heading": -0.06503957483267325, - "angularVelocity": -4.017975287461778e-7, - "velocityX": -3.08734879306747, - "velocityY": -2.0842041880288593, - "timestamp": 11.412643801847405 - }, - { - "x": 6.527131993072097, - "y": 4.57231033799875, - "heading": -0.06503960182577956, - "angularVelocity": -4.1696686974459996e-7, - "velocityX": -3.293708384663109, - "velocityY": -1.739860523378229, - "timestamp": 11.47738061492123 - }, - { - "x": 6.303024971058329, - "y": 4.483277800206398, - "heading": -0.06503963136622377, - "angularVelocity": -4.5631600956130875e-7, - "velocityX": -3.461817339667266, - "velocityY": -1.3752999810296351, - "timestamp": 11.542117427995056 - }, - { - "x": 6.070638079320141, - "y": 4.41887996664659, - "heading": -0.06503966569966328, - "angularVelocity": -5.303541813322711e-7, - "velocityX": -3.5897178236619824, - "velocityY": -0.9947637287360487, - "timestamp": 11.60685424106888 - }, - { - "x": 5.832670509915108, - "y": 4.379864860722445, - "heading": -0.06503973197653827, - "angularVelocity": -0.0000010237895848927841, - "velocityX": -3.67592345229684, - "velocityY": -0.60267263820436, - "timestamp": 11.671591054142706 - }, - { - "x": 5.593035616597729, - "y": 4.366766567771227, - "heading": -0.0691739587008402, - "angularVelocity": -0.06386206746982208, - "velocityX": -3.7016788738750415, - "velocityY": -0.20233144526718327, - "timestamp": 11.73632786721653 - }, - { - "x": 5.359733281234782, - "y": 4.378764088602827, - "heading": -0.08321570293446177, - "angularVelocity": -0.2169050894984338, - "velocityX": -3.6038588290852247, - "velocityY": 0.1853276406720581, - "timestamp": 11.801064680290356 + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": -7.002250121980633e-28, + "angularVelocity": 0.027045919153409353, + "velocityX": 0.38147255414152553, + "velocityY": -0.0333268496130705, + "timestamp": 7.01390156352497 + }, + { + "x": 8.200635459356112, + "y": 4.096934551280871, + "heading": -0.0010816052785507836, + "angularVelocity": -0.014043494340470785, + "velocityX": -0.09442200874161932, + "velocityY": -0.0026468217635775776, + "timestamp": 7.09091980786846 + }, + { + "x": 8.156710678515955, + "y": 4.0990936119074455, + "heading": -0.0053278404451381055, + "angularVelocity": -0.055132848103493455, + "velocityX": -0.5703165686854601, + "velocityY": 0.028033106246169795, + "timestamp": 7.167938052211949 + }, + { + "x": 8.076133333676754, + "y": 4.103615573325919, + "heading": -0.012738567096053588, + "angularVelocity": -0.0962204048415442, + "velocityX": -1.0462111351154568, + "velocityY": 0.05871286027121356, + "timestamp": 7.244956296555438 + }, + { + "x": 7.958903423782304, + "y": 4.110500416395447, + "heading": -0.023313558262368205, + "angularVelocity": -0.13730501462941505, + "velocityX": -1.522105715259157, + "velocityY": 0.08939236577274852, + "timestamp": 7.321974540898927 + }, + { + "x": 7.8050209475058985, + "y": 4.1197481162013645, + "heading": -0.03705256536677336, + "angularVelocity": -0.17838639690527597, + "velocityX": -1.9980003126287051, + "velocityY": 0.12007154778384964, + "timestamp": 7.398992785242417 + }, + { + "x": 7.614485903743769, + "y": 4.131358642030129, + "heading": -0.05395540666928083, + "angularVelocity": -0.21946541948065604, + "velocityX": -2.4738949243295387, + "velocityY": 0.15075033101226304, + "timestamp": 7.476011029585906 + }, + { + "x": 7.38729829267515, + "y": 4.145331957366106, + "heading": -0.07402207604262086, + "angularVelocity": -0.2605443624999541, + "velocityX": -2.9497895337031945, + "velocityY": 0.18142864012399953, + "timestamp": 7.553029273929395 + }, + { + "x": 7.123458119903768, + "y": 4.1616680199277996, + "heading": -0.09725286481798223, + "angularVelocity": -0.30162708814493094, + "velocityX": -3.425684070318428, + "velocityY": 0.21210640025547228, + "timestamp": 7.630047518272884 + }, + { + "x": 6.836784301080243, + "y": 4.180148431298723, + "heading": -0.0972528661294337, + "angularVelocity": -1.7027802603385206e-8, + "velocityX": -3.722154682532179, + "velocityY": 0.2399484891982639, + "timestamp": 7.707065762616374 + }, + { + "x": 6.550110482735962, + "y": 4.198628850104045, + "heading": -0.09725286744087382, + "angularVelocity": -1.7027655300024213e-8, + "velocityX": -3.722154676309698, + "velocityY": 0.23994858572603794, + "timestamp": 7.784084006959863 + }, + { + "x": 6.263436664391698, + "y": 4.217109268909628, + "heading": -0.09725286875231377, + "angularVelocity": -1.7027653194201686e-8, + "velocityX": -3.7221546763094806, + "velocityY": 0.23994858572941535, + "timestamp": 7.861102251303352 + }, + { + "x": 5.976762848146004, + "y": 4.235589720268832, + "heading": -0.09725287006375381, + "angularVelocity": -1.702765423071752e-8, + "velocityX": -3.722154649061779, + "velocityY": 0.23994900840357883, + "timestamp": 7.938120495646841 + }, + { + "x": 5.690447961543492, + "y": 4.258981806583179, + "heading": -0.09725287137836151, + "angularVelocity": -1.7068782885666804e-8, + "velocityX": -3.717494329338312, + "velocityY": 0.30372136516150916, + "timestamp": 8.01513873999033 + }, + { + "x": 5.409459505180122, + "y": 4.318722004074591, + "heading": -0.09725287275094441, + "angularVelocity": -1.7821529284929368e-8, + "velocityX": -3.6483362969195534, + "velocityY": 0.7756629354595511, + "timestamp": 8.092156984333819 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": -0.21683446699990425, - "velocityX": -3.419080202788392, - "velocityY": 0.5417320490954629, - "timestamp": 11.86580149336418 - }, - { - "x": 4.905765298437056, - "y": 4.481387310799775, - "heading": -0.10673913036260185, - "angularVelocity": -0.12897497729525575, - "velocityX": -3.1628016184093535, - "velocityY": 0.9184524777738038, - "timestamp": 11.939352621411974 - }, - { - "x": 4.700482894835419, - "y": 4.568326227070152, - "heading": -0.11059429780349972, - "angularVelocity": -0.05241479693408341, - "velocityX": -2.791016386155829, - "velocityY": 1.1820201617286428, - "timestamp": 12.012903749459767 - }, - { - "x": 4.527923671898321, - "y": 4.662591020242044, - "heading": -0.11045427798457029, - "angularVelocity": 0.0019037072937678628, - "velocityX": -2.3461125276687587, - "velocityY": 1.2816226708397902, - "timestamp": 12.08645487750756 - }, - { - "x": 4.3886983023339585, - "y": 4.75311454797141, - "heading": -0.10805031221492864, - "angularVelocity": 0.03268428144405263, - "velocityX": -1.8929059724807213, - "velocityY": 1.2307564837148823, - "timestamp": 12.160006005555353 - }, - { - "x": 4.281048300464299, - "y": 4.832306401053517, - "heading": -0.1047189693449996, - "angularVelocity": 0.04529288616436113, - "velocityX": -1.4636077613889036, - "velocityY": 1.076691210373392, - "timestamp": 12.233557133603146 - }, - { - "x": 4.20282348849849, - "y": 4.895385445772344, - "heading": -0.10139996108706777, - "angularVelocity": 0.04512518497031254, - "velocityX": -1.0635433343045229, - "velocityY": 0.8576217169346194, - "timestamp": 12.307108261650939 - }, - { - "x": 4.15212537277285, - "y": 4.939288518901389, - "heading": -0.09875289338109836, - "angularVelocity": 0.0359894916125467, - "velocityX": -0.6892907977250395, - "velocityY": 0.5969055036180668, - "timestamp": 12.380659389698732 + "angularVelocity": -3.300065929591643e-8, + "velocityX": -3.519511282401575, + "velocityY": 1.2349293565099368, + "timestamp": 8.169175228677307 + }, + { + "x": 4.908992840849997, + "y": 4.524204173518216, + "heading": -0.09725287595344081, + "angularVelocity": -9.191205906949272e-9, + "velocityX": -3.1905635663992595, + "velocityY": 1.535059379942558, + "timestamp": 8.24107477859643 + }, + { + "x": 4.7110423526588185, + "y": 4.628577138266545, + "heading": -0.097252876094701, + "angularVelocity": -1.964688046810948e-9, + "velocityX": -2.7531533704153888, + "velocityY": 1.4516497650644007, + "timestamp": 8.312974328515551 + }, + { + "x": 4.542899034827415, + "y": 4.721263554864837, + "heading": -0.09725287602386487, + "angularVelocity": 9.85209742042839e-10, + "velocityX": -2.338586514387707, + "velocityY": 1.2891098303473851, + "timestamp": 8.384873878434673 + }, + { + "x": 4.403657392505783, + "y": 4.800175498734494, + "heading": -0.09725287586251803, + "angularVelocity": 2.2440590575017265e-9, + "velocityX": -1.936613545957686, + "velocityY": 1.0975304290280408, + "timestamp": 8.456773428353795 + }, + { + "x": 4.29278499065898, + "y": 4.864247320916007, + "heading": -0.09725287567494544, + "angularVelocity": 2.608814644263043e-9, + "velocityX": -1.5420458399464478, + "velocityY": 0.8911296698461443, + "timestamp": 8.528672978272917 + }, + { + "x": 4.209935704536306, + "y": 4.912835343646817, + "heading": -0.09725287550078096, + "angularVelocity": 2.4223304424020015e-9, + "velocityX": -1.152292138349535, + "velocityY": 0.6757764518062407, + "timestamp": 8.600572528192039 + }, + { + "x": 4.1548674777952215, + "y": 4.945509352761423, + "heading": -0.09725287536687216, + "angularVelocity": 1.862443038330612e-9, + "velocityX": -0.7659050272641558, + "velocityY": 0.4544396891407544, + "timestamp": 8.672472078111161 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 0.020394222744298442, - "velocityX": -0.3361409221475862, - "velocityY": 0.3082648480890461, - "timestamp": 12.454210517746525 + "angularVelocity": 1.0330372741830232e-9, + "velocityX": -0.3820002915212732, + "velocityY": 0.2288247071491247, + "timestamp": 8.744371628030283 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 1.761901225942554e-27, - "velocityX": -1.9870172205028814e-26, - "velocityY": -2.1545999896560787e-26, - "timestamp": 12.527761645794318 + "angularVelocity": -3.6520722228130864e-26, + "velocityX": -9.590252743264302e-26, + "velocityY": -2.0342869025815665e-25, + "timestamp": 8.816271177949405 + }, + { + "x": 4.149907885471957, + "y": 4.949853513181501, + "heading": -0.09710925579665566, + "angularVelocity": 0.0022357452830962274, + "velocityX": 0.35035501128758395, + "velocityY": -0.18849059952223363, + "timestamp": 8.88050903949269 + }, + { + "x": 4.194913909182307, + "y": 4.925625720140559, + "heading": -0.09684438286421601, + "angularVelocity": 0.004123314912361655, + "velocityX": 0.7006152233138045, + "velocityY": -0.3771575276461672, + "timestamp": 8.944746901035975 + }, + { + "x": 4.262412058477699, + "y": 4.889263805728157, + "heading": -0.09648702221681861, + "angularVelocity": 0.0055630844304586, + "velocityX": 1.0507533668428728, + "velocityY": -0.5660511346240961, + "timestamp": 9.00898476257926 + }, + { + "x": 4.3523918574804155, + "y": 4.840748358129665, + "heading": -0.09607554613145339, + "angularVelocity": 0.006405507211474483, + "velocityX": 1.4007284308816241, + "velocityY": -0.755246928103313, + "timestamp": 9.073222624122545 + }, + { + "x": 4.464838600314873, + "y": 4.78005220731639, + "heading": -0.09566371008606342, + "angularVelocity": 0.006411110760784919, + "velocityX": 1.7504745664469084, + "velocityY": -0.9448656813143913, + "timestamp": 9.13746048566583 + }, + { + "x": 4.599730141458809, + "y": 4.7071346128101945, + "heading": -0.09533221857083674, + "angularVelocity": 0.005160375941271345, + "velocityX": 2.0998759594922722, + "velocityY": -1.1351186473893293, + "timestamp": 9.201698347209115 + }, + { + "x": 4.757029340577212, + "y": 4.621927713991256, + "heading": -0.09521575794797991, + "angularVelocity": 0.0018129592121981597, + "velocityX": 2.4486991836179413, + "velocityY": -1.3264280094617478, + "timestamp": 9.2659362087524 + }, + { + "x": 4.936661051016691, + "y": 4.524295949358237, + "heading": -0.09558433952634979, + "angularVelocity": -0.005737762271577307, + "velocityX": 2.7963525890169896, + "velocityY": -1.519847676860057, + "timestamp": 9.330174070295685 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.025974335480068365, + "velocityX": 3.1403890073431286, + "velocityY": -1.7195755229583027, + "timestamp": 9.39441193183897 + }, + { + "x": 5.282623687046255, + "y": 4.319887903045294, + "heading": -0.12350374979021995, + "angularVelocity": -0.54152818219314, + "velocityX": 2.9753302981273193, + "velocityY": -1.9380120292170788, + "timestamp": 9.442887478192457 + }, + { + "x": 5.415965984922444, + "y": 4.230982927805077, + "heading": -0.18642712240012974, + "angularVelocity": -1.298043598128203, + "velocityX": 2.750712635683371, + "velocityY": -1.834016982334084, + "timestamp": 9.491363024545944 + }, + { + "x": 5.544046179661802, + "y": 4.15236621254376, + "heading": -0.28429132793206696, + "angularVelocity": -2.0188365659317435, + "velocityX": 2.642160932140699, + "velocityY": -1.6217809014062547, + "timestamp": 9.53983857089943 + }, + { + "x": 5.6705798620807135, + "y": 4.085599938426855, + "heading": -0.4169505782108561, + "angularVelocity": -2.736622075622024, + "velocityX": 2.610257994746929, + "velocityY": -1.3773186511409734, + "timestamp": 9.588314117252917 + }, + { + "x": 5.804006769424575, + "y": 4.008650582245615, + "heading": -0.5169463778011086, + "angularVelocity": -2.06280912980491, + "velocityX": 2.7524580408213564, + "velocityY": -1.5873850213078682, + "timestamp": 9.636789663606404 + }, + { + "x": 5.938212060885805, + "y": 3.9187237193410076, + "heading": -0.5903545372750207, + "angularVelocity": -1.5143338238751507, + "velocityX": 2.7685152939298026, + "velocityY": -1.8550974598379135, + "timestamp": 9.68526520995989 + }, + { + "x": 6.076913949313874, + "y": 3.818161785200274, + "heading": -0.6273225494125774, + "angularVelocity": -0.7626115622913053, + "velocityX": 2.8612754029969545, + "velocityY": -2.07448789555523, + "timestamp": 9.733740756313377 + }, + { + "x": 6.214509922576719, + "y": 3.712335803608528, + "heading": -0.6556184225519457, + "angularVelocity": -0.5837143728723201, + "velocityX": 2.838461525724493, + "velocityY": -2.1830797082730613, + "timestamp": 9.782216302666864 + }, + { + "x": 6.348284689884717, + "y": 3.603969355784692, + "heading": -0.6891441796191574, + "angularVelocity": -0.6916014277124424, + "velocityX": 2.7596340293414254, + "velocityY": -2.2354868789641342, + "timestamp": 9.83069184902035 + }, + { + "x": 6.474985027954153, + "y": 3.4904345524521627, + "heading": -0.730650696928258, + "angularVelocity": -0.8562361939447342, + "velocityX": 2.613695927128511, + "velocityY": -2.342104666642176, + "timestamp": 9.879167395373837 + }, + { + "x": 6.594663143157959, + "y": 3.3646228313446045, + "heading": -0.7575105881170743, + "angularVelocity": -0.554091561814539, + "velocityX": 2.468834787979625, + "velocityY": -2.595364685322631, + "timestamp": 9.927642941727324 + }, + { + "x": 6.674801371049327, + "y": 3.2699580042827607, + "heading": -0.7721877645618775, + "angularVelocity": -0.4274597319602108, + "velocityX": 2.333954731895263, + "velocityY": -2.757029034439579, + "timestamp": 9.961978752442167 + }, + { + "x": 6.750751589732745, + "y": 3.1695424250982125, + "heading": -0.7798246346604031, + "angularVelocity": -0.2224170607750979, + "velocityX": 2.211982682284112, + "velocityY": -2.9245145838697275, + "timestamp": 9.99631456315701 + }, + { + "x": 6.822476452924599, + "y": 3.063443372222415, + "heading": -0.7798255890957266, + "angularVelocity": -0.000027797081344724893, + "velocityX": 2.0889229553228312, + "velocityY": -3.0900407087208346, + "timestamp": 10.030650373871854 + }, + { + "x": 6.888038953713801, + "y": 2.9534294027046353, + "heading": -0.779825865993983, + "angularVelocity": -0.000008064415854472034, + "velocityX": 1.9094496219615917, + "velocityY": -3.2040591798294837, + "timestamp": 10.064986184586697 + }, + { + "x": 6.951397199301839, + "y": 2.842131382226383, + "heading": -0.7798261188046057, + "angularVelocity": -0.0000073628849152658005, + "velocityX": 1.8452526464053884, + "velocityY": -3.241456023933041, + "timestamp": 10.09932199530154 + }, + { + "x": 7.014755263905195, + "y": 2.730833258719293, + "heading": -0.7798263716136229, + "angularVelocity": -0.000007362838153289018, + "velocityX": 1.8452473753871865, + "velocityY": -3.2414590245564723, + "timestamp": 10.133657806016384 + }, + { + "x": 7.078113328495587, + "y": 2.6195351352048375, + "heading": -0.7798266244227116, + "angularVelocity": -0.000007362840235334639, + "velocityX": 1.8452473750096865, + "velocityY": -3.241459024770976, + "timestamp": 10.167993616731227 + }, + { + "x": 7.1414713930859905, + "y": 2.5082370116904005, + "heading": -0.7798268772318718, + "angularVelocity": -0.000007362842320190408, + "velocityX": 1.84524737500996, + "velocityY": -3.241459024770425, + "timestamp": 10.20232942744607 + }, + { + "x": 7.204829457676403, + "y": 2.396938888175983, + "heading": -0.7798271300411037, + "angularVelocity": -0.000007362844406233281, + "velocityX": 1.845247375010246, + "velocityY": -3.2414590247698674, + "timestamp": 10.236665238160914 + }, + { + "x": 7.2681875222668415, + "y": 2.285640764661595, + "heading": -0.7798273828504071, + "angularVelocity": -0.0000073628464912696794, + "velocityX": 1.845247375011028, + "velocityY": -3.2414590247690276, + "timestamp": 10.271001048875757 + }, + { + "x": 7.3315455870489, + "y": 2.1743426412563025, + "heading": -0.7798276356597839, + "angularVelocity": -0.000007362848626587696, + "velocityX": 1.8452473805917786, + "velocityY": -3.241459021591694, + "timestamp": 10.3053368595906 + }, + { + "x": 7.394906330223783, + "y": 2.0630460426228225, + "heading": -0.7798278885161463, + "angularVelocity": -0.0000073642170417804714, + "velocityX": 1.8453253864046084, + "velocityY": -3.241414613966507, + "timestamp": 10.339672670305443 + }, + { + "x": 7.462756763097692, + "y": 1.9565771896637063, + "heading": -0.7859464592787083, + "angularVelocity": -0.1781979407265627, + "velocityX": 1.976083612453597, + "velocityY": -3.100810807798736, + "timestamp": 10.374008481020287 + }, + { + "x": 7.530997298306203, + "y": 1.8560643499724379, + "heading": -0.8086349640155738, + "angularVelocity": -0.6607825551373272, + "velocityX": 1.9874449965735268, + "velocityY": -2.9273472097694815, + "timestamp": 10.40834429173513 + }, + { + "x": 7.600392480009413, + "y": 1.761540500711219, + "heading": -0.8474765480671682, + "angularVelocity": -1.1312266477169204, + "velocityX": 2.021073050510832, + "velocityY": -2.7529231811718247, + "timestamp": 10.442680102449973 + }, + { + "x": 7.671333460317134, + "y": 1.6729216107462341, + "heading": -0.9025012077772409, + "angularVelocity": -1.6025443571741933, + "velocityX": 2.0660930623389726, + "velocityY": -2.580946484734556, + "timestamp": 10.477015913164816 + }, + { + "x": 7.744622230529785, + "y": 1.5909570455551147, + "heading": -0.9652518160942604, + "angularVelocity": -1.827555750413467, + "velocityX": 2.134470358696664, + "velocityY": -2.387145184129499, + "timestamp": 10.51135172387966 + }, + { + "x": 7.860586157347494, + "y": 1.4682098558928158, + "heading": -0.9995269320627294, + "angularVelocity": -0.6040459024385552, + "velocityX": 2.0436848379845327, + "velocityY": -2.1632293533176865, + "timestamp": 10.568094292333646 + }, + { + "x": 7.962380466980202, + "y": 1.3584906150903546, + "heading": -1.012418619935978, + "angularVelocity": -0.22719605799485526, + "velocityX": 1.7939672525620034, + "velocityY": -1.933631906201723, + "timestamp": 10.624836860787632 + }, + { + "x": 8.04779389711619, + "y": 1.259831390089905, + "heading": -1.014642889543101, + "angularVelocity": -0.0391993113411274, + "velocityX": 1.505279589260271, + "velocityY": -1.7387162352450838, + "timestamp": 10.681579429241618 + }, + { + "x": 8.116074558332897, + "y": 1.171249663466821, + "heading": -1.0104296347158561, + "angularVelocity": 0.07425209929757598, + "velocityX": 1.2033410379031306, + "velocityY": -1.5611159141468582, + "timestamp": 10.738321997695603 + }, + { + "x": 8.166855716911636, + "y": 1.0921738551192879, + "heading": -1.002112257895268, + "angularVelocity": 0.14658090120353398, + "velocityX": 0.8949393720151817, + "velocityY": -1.3935888082270873, + "timestamp": 10.79506456614959 + }, + { + "x": 8.199923294477061, + "y": 1.022232267079584, + "heading": -0.9911733302853878, + "angularVelocity": 0.19278167886867675, + "velocityX": 0.582764905896743, + "velocityY": -1.2326123040486314, + "timestamp": 10.851807134603575 + }, + { + "x": 8.215137995862136, + "y": 0.9611644433512547, + "heading": -0.978636960554262, + "angularVelocity": 0.22093412534351606, + "velocityX": 0.2681355779200118, + "velocityY": -1.0762259339362041, + "timestamp": 10.908549703057561 + }, + { + "x": 8.21240234375, + "y": 0.908777952194214, + "heading": -0.9652518160942604, + "angularVelocity": 0.23589246706123476, + "velocityX": -0.04821163698918966, + "velocityY": -0.9232308755907478, + "timestamp": 10.965292271511547 + }, + { + "x": 8.1921938424561, + "y": 0.8654303279065881, + "heading": -0.9517870451764023, + "angularVelocity": 0.24072872246907334, + "velocityX": -0.3612959128063065, + "velocityY": -0.7749866878900871, + "timestamp": 11.02122565071015 + }, + { + "x": 8.15448402268934, + "y": 0.8303967725236208, + "heading": -0.938073387974666, + "angularVelocity": 0.24517841400289994, + "velocityX": -0.6741916956753717, + "velocityY": -0.6263443382988405, + "timestamp": 11.077159029908753 + }, + { + "x": 8.099285775198217, + "y": 0.8037044120128763, + "heading": -0.9241370640104126, + "angularVelocity": 0.2491593421303856, + "velocityX": -0.9868570124313375, + "velocityY": -0.47721701948254364, + "timestamp": 11.133092409107356 + }, + { + "x": 8.026615220367114, + "y": 0.7853870096388412, + "heading": -0.9100105816047303, + "angularVelocity": 0.25255907309879116, + "velocityX": -1.299234122313104, + "velocityY": -0.3274860671120066, + "timestamp": 11.189025788305958 + }, + { + "x": 7.93649309479659, + "y": 0.7754877410206693, + "heading": -0.895735376567552, + "angularVelocity": 0.25521799758407426, + "velocityX": -1.6112404947058143, + "velocityY": -0.1769832032322341, + "timestamp": 11.244959167504561 + }, + { + "x": 7.828947063371523, + "y": 0.7740637661410563, + "heading": -0.8813661690986453, + "angularVelocity": 0.2568986117911059, + "velocityX": -1.9227522628876286, + "velocityY": -0.025458409629728278, + "timestamp": 11.300892546703164 + }, + { + "x": 7.7040158825175595, + "y": 0.7811943329260619, + "heading": -0.8669787071780914, + "angularVelocity": 0.2572249723991828, + "velocityX": -2.2335711277226653, + "velocityY": 0.1274832110480382, + "timestamp": 11.356825925901767 + }, + { + "x": 7.561757726851445, + "y": 0.7969966352924417, + "heading": -0.8526849753347752, + "angularVelocity": 0.2555492274579613, + "velocityX": -2.5433499227893406, + "velocityY": 0.2825200728579338, + "timestamp": 11.41275930510037 + }, + { + "x": 7.402269597446514, + "y": 0.821661602526662, + "heading": -0.8386677065817341, + "angularVelocity": 0.2506065064166751, + "velocityX": -2.8513944926987937, + "velocityY": 0.4409704471214242, + "timestamp": 11.468692684298972 + }, + { + "x": 7.2257452662755925, + "y": 0.8555551558075168, + "heading": -0.8252789625517808, + "angularVelocity": 0.23936948244113093, + "velocityX": -3.1559747274366425, + "velocityY": 0.605962910992889, + "timestamp": 11.524626063497575 + }, + { + "x": 7.032757795836431, + "y": 0.8996580788692503, + "heading": -0.813478587431489, + "angularVelocity": 0.21097196860558187, + "velocityX": -3.4503095147160385, + "velocityY": 0.7884902305855099, + "timestamp": 11.580559442696178 + }, + { + "x": 6.833636314284489, + "y": 0.9619072197023563, + "heading": -0.813478584540231, + "angularVelocity": 5.169110280772625e-8, + "velocityX": -3.5599758928370466, + "velocityY": 1.112915788836524, + "timestamp": 11.63649282189478 + }, + { + "x": 6.641148526278714, + "y": 1.0423607611427537, + "heading": -0.8134785841610236, + "angularVelocity": 6.7796258946599015e-9, + "velocityX": -3.441375986283731, + "velocityY": 1.438381563801643, + "timestamp": 11.692426201093383 + }, + { + "x": 6.45145754132324, + "y": 1.1292034413279326, + "heading": -0.8134785837843238, + "angularVelocity": 6.734793245125226e-9, + "velocityX": -3.3913735889608208, + "velocityY": 1.5526092188499014, + "timestamp": 11.748359580291986 + }, + { + "x": 6.261766612851187, + "y": 1.21604624489008, + "heading": -0.8134785834076242, + "angularVelocity": 6.734791446466924e-9, + "velocityX": -3.3913725791269433, + "velocityY": 1.5526114246341831, + "timestamp": 11.804292959490589 + }, + { + "x": 6.072075684380091, + "y": 1.302889048454317, + "heading": -0.8134785830309245, + "angularVelocity": 6.734793280802982e-9, + "velocityX": -3.3913725791098397, + "velocityY": 1.552611424671542, + "timestamp": 11.860226338689191 + }, + { + "x": 5.882384755908996, + "y": 1.3897318520185546, + "heading": -0.8134785826542248, + "angularVelocity": 6.734792910911008e-9, + "velocityX": -3.3913725791098357, + "velocityY": 1.552611424671551, + "timestamp": 11.916159717887794 + }, + { + "x": 5.692693827449727, + "y": 1.476574655608625, + "heading": -0.8134785822775251, + "angularVelocity": 6.734793219804119e-9, + "velocityX": -3.3913725788983964, + "velocityY": 1.5526114251333973, + "timestamp": 11.972093097086397 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.8134785819008252, + "angularVelocity": 6.73479370214728e-9, + "velocityX": -3.391360094956347, + "velocityY": 1.5526386935458116, + "timestamp": 12.028026476285 + }, + { + "x": 5.306759747272106, + "y": 1.6761609741986419, + "heading": -0.8134785815780706, + "angularVelocity": 5.31909195889925e-9, + "velocityX": -3.234156424503322, + "velocityY": 1.8580211843552186, + "timestamp": 12.08870500392225 + }, + { + "x": 5.11051667299417, + "y": 1.788904314208696, + "heading": -0.8134785812553164, + "angularVelocity": 5.3190855034609695e-9, + "velocityX": -3.2341436405827086, + "velocityY": 1.8580434364534788, + "timestamp": 12.1493835315595 + }, + { + "x": 4.914273598725787, + "y": 1.9016476542353784, + "heading": -0.8134785809325621, + "angularVelocity": 5.319086694543979e-9, + "velocityX": -3.2341436404252732, + "velocityY": 1.8580434367275134, + "timestamp": 12.210062059196751 + }, + { + "x": 4.718030524457404, + "y": 2.0143909942620604, + "heading": -0.8134785806098077, + "angularVelocity": 5.319085466039386e-9, + "velocityX": -3.234143640425272, + "velocityY": 1.8580434367275172, + "timestamp": 12.270740586834002 + }, + { + "x": 4.521787450189022, + "y": 2.1271343342887428, + "heading": -0.8134785802870533, + "angularVelocity": 5.319086975081976e-9, + "velocityX": -3.2341436404252715, + "velocityY": 1.8580434367275167, + "timestamp": 12.331419114471252 + }, + { + "x": 4.325544375920639, + "y": 2.239877674315425, + "heading": -0.813478579964299, + "angularVelocity": 5.319087023675505e-9, + "velocityX": -3.2341436404252715, + "velocityY": 1.8580434367275163, + "timestamp": 12.392097642108503 + }, + { + "x": 4.129301301652256, + "y": 2.3526210143421062, + "heading": -0.8134785796415447, + "angularVelocity": 5.319086160384219e-9, + "velocityX": -3.2341436404252857, + "velocityY": 1.8580434367274927, + "timestamp": 12.452776169745754 + }, + { + "x": 3.9330582273170225, + "y": 2.465364354252196, + "heading": -0.8134785793187616, + "angularVelocity": 5.3195605243469425e-9, + "velocityX": -3.234143641526992, + "velocityY": 1.858043434806029, + "timestamp": 12.513454697383004 + }, + { + "x": 3.755112012570184, + "y": 2.567595773148683, + "heading": -0.8087823717593017, + "angularVelocity": 0.07739488320373908, + "velocityX": -2.9326060086797106, + "velocityY": 1.684803881657269, + "timestamp": 12.574133225020255 + }, + { + "x": 3.596937591266501, + "y": 2.6584681509636754, + "heading": -0.8046081994450863, + "angularVelocity": 0.06879158866822643, + "velocityX": -2.6067610316664966, + "velocityY": 1.4976035403865926, + "timestamp": 12.634811752657505 + }, + { + "x": 3.4585349694595866, + "y": 2.737981483942003, + "heading": -0.8009559747359442, + "angularVelocity": 0.06018973846854076, + "velocityX": -2.2809159548879583, + "velocityY": 1.3104031372296205, + "timestamp": 12.695490280294756 + }, + { + "x": 3.339904149133476, + "y": 2.8061357707001306, + "heading": -0.7978256145396123, + "angularVelocity": 0.051589257653807806, + "velocityX": -1.9550708454119348, + "velocityY": 1.1232027112715945, + "timestamp": 12.756168807932006 + }, + { + "x": 3.2410451312582933, + "y": 2.862931010476089, + "heading": -0.7952170508581512, + "angularVelocity": 0.04298989746513998, + "velocityX": -1.6292257199479807, + "velocityY": 0.9360022727560687, + "timestamp": 12.816847335569257 + }, + { + "x": 3.1619579164016844, + "y": 2.908367202779582, + "heading": -0.7931302326117163, + "angularVelocity": 0.03439137908734864, + "velocityX": -1.30338058512905, + "velocityY": 0.748801826160328, + "timestamp": 12.877525863206507 + }, + { + "x": 3.1026425049328332, + "y": 2.9424443472754236, + "heading": -0.7915651260524916, + "angularVelocity": 0.02579341688350326, + "velocityX": -0.9775354442258696, + "velocityY": 0.5616013740406165, + "timestamp": 12.938204390843758 + }, + { + "x": 3.0630988971100095, + "y": 2.9651624437336137, + "heading": -0.7905217148332285, + "angularVelocity": 0.017195724087124466, + "velocityX": -0.6516902990663221, + "velocityY": 0.37440091813044435, + "timestamp": 12.998882918481009 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 0.008598014051155055, + "velocityX": -0.3258451507561328, + "velocityY": 0.18720045975221852, + "timestamp": 13.05956144611826 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -9.910581864004145e-25, + "velocityX": -2.631400747419988e-26, + "velocityY": 1.5241001239999535e-26, + "timestamp": 13.12023997375551 } ], "trajectoryWaypoints": [ @@ -43417,7 +45879,7 @@ "controlIntervalCount": 14 }, { - "timestamp": 0.6923742371436292, + "timestamp": 0.692554229543545, "isStopPoint": false, "x": 1.7826586961746216, "y": 3.5168776512145996, @@ -43428,7 +45890,7 @@ "controlIntervalCount": 10 }, { - "timestamp": 1.397197380247905, + "timestamp": 1.3976921162985145, "isStopPoint": true, "x": 3.0433270931243896, "y": 2.9765214920043945, @@ -43439,7 +45901,7 @@ "controlIntervalCount": 18 }, { - "timestamp": 2.4478166834517507, + "timestamp": 2.4480546395580927, "isStopPoint": false, "x": 5.5030035972595215, "y": 1.563418984413147, @@ -43450,7 +45912,7 @@ "controlIntervalCount": 19 }, { - "timestamp": 3.4501275661438395, + "timestamp": 3.449749914313385, "isStopPoint": false, "x": 8.207907676696777, "y": 2.428393840789795, @@ -43461,7 +45923,7 @@ "controlIntervalCount": 10 }, { - "timestamp": 4.098537468980011, + "timestamp": 4.098701501168702, "isStopPoint": false, "x": 7.120736598968506, "y": 3.5168776512145996, @@ -43472,7 +45934,7 @@ "controlIntervalCount": 18 }, { - "timestamp": 4.6826517740867715, + "timestamp": 4.6820514794133254, "isStopPoint": false, "x": 5.138392925262451, "y": 4.413834095001221, @@ -43483,7 +45945,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 5.328604050295384, + "timestamp": 5.328426668385459, "isStopPoint": true, "x": 4.127401828765869, "y": 4.96196174621582, @@ -43494,7 +45956,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 5.907129949231749, + "timestamp": 5.907340462655205, "isStopPoint": false, "x": 5.138392925262451, "y": 4.413834095001221, @@ -43505,7 +45967,7 @@ "controlIntervalCount": 15 }, { - "timestamp": 7.01387661283412, + "timestamp": 7.01390156352497, "isStopPoint": false, "x": 8.207907676696777, "y": 4.097138404846191, @@ -43516,7 +45978,7 @@ "controlIntervalCount": 15 }, { - "timestamp": 8.16948699062752, + "timestamp": 8.169175228677307, "isStopPoint": false, "x": 5.138392925262451, "y": 4.413834095001221, @@ -43527,7 +45989,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 8.816150511641153, + "timestamp": 8.816271177949405, "isStopPoint": true, "x": 4.127401828765869, "y": 4.96196174621582, @@ -43538,7 +46000,7 @@ "controlIntervalCount": 9 }, { - "timestamp": 9.405508655269896, + "timestamp": 9.39441193183897, "isStopPoint": false, "x": 5.138392925262451, "y": 4.413834095001221, @@ -43546,47 +46008,58 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 9.927642941727324, + "isStopPoint": false, + "x": 6.594663143157959, + "y": 3.3646228313446045, + "heading": -0.09725287529259725, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, "controlIntervalCount": 17 }, { - "timestamp": 10.144615793769187, + "timestamp": 10.51135172387966, "isStopPoint": false, - "x": 7.501500129699707, - "y": 5.105462551116943, - "heading": 0.6747401864044066, + "x": 7.744622230529785, + "y": 1.5909570455551147, + "heading": -0.9652518160942603, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 8 }, { - "timestamp": 10.635802044961505, + "timestamp": 10.965292271511547, "isStopPoint": false, - "x": 8.1323881149292, - "y": 5.628637313842773, - "heading": 0.6747401864044066, + "x": 8.21240234375, + "y": 0.9087779521942139, + "heading": -0.9652518160942603, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 19 }, { - "timestamp": 11.86580149336418, + "timestamp": 12.028026476285, "isStopPoint": false, - "x": 5.138392925262451, - "y": 4.413834095001221, - "heading": -0.09725287529259725, + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 + "headingConstrained": false, + "controlIntervalCount": 18 }, { - "timestamp": 12.527761645794318, + "timestamp": 13.12023997375551, "isStopPoint": true, - "x": 4.127401828765869, - "y": 4.96196174621582, - "heading": -0.09725287529259725, + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -43608,7 +46081,7 @@ }, { "scope": [ - 2 + 17 ], "type": "StopPoint" }, @@ -43622,12 +46095,20 @@ "scope": [ 7 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 11 ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 2 + ], "type": "StopPoint" } ], diff --git a/src/main/deploy/choreo/AmpLanePAEDF.1.traj b/src/main/deploy/choreo/AmpLanePAEDF.1.traj index 0c0a4c8d..8cb9e4a8 100644 --- a/src/main/deploy/choreo/AmpLanePAEDF.1.traj +++ b/src/main/deploy/choreo/AmpLanePAEDF.1.traj @@ -3,182 +3,182 @@ { "x": 0.43297290802001953, "y": 6.9807281494140625, - "heading": -4.820407208942378e-26, - "angularVelocity": -1.0568380673400748e-29, - "velocityX": 2.0900789241101526e-25, - "velocityY": 6.12105194243679e-25, + "heading": -2.8662975018215305e-26, + "angularVelocity": -1.4927020760415103e-39, + "velocityX": 1.0727948153345131e-25, + "velocityY": 3.1703902069351426e-25, "timestamp": 0 }, { - "x": 0.4535853919747023, - "y": 6.976442037279776, - "heading": -9.657051327438745e-26, - "angularVelocity": -2.7866726754125085e-27, - "velocityX": 0.3537634154941742, - "velocityY": -0.07356074460262349, - "timestamp": 0.05826629620784589 + "x": 0.45358539196480147, + "y": 6.976442037232183, + "heading": -5.74233401454689e-26, + "angularVelocity": -1.6703718504854773e-27, + "velocityX": 0.3535319273172983, + "velocityY": -0.07351261033930959, + "timestamp": 0.05830444820414204 }, { - "x": 0.49483104636031155, - "y": 6.967970532726724, - "heading": -1.449381860201486e-25, - "angularVelocity": -2.8078094367592775e-27, - "velocityX": 0.7078818643024581, - "velocityY": -0.1453928789781493, - "timestamp": 0.11653259241569178 + "x": 0.49483104633484415, + "y": 6.967970532602043, + "heading": -8.618370527272265e-26, + "angularVelocity": -1.670371850488467e-27, + "velocityX": 0.7074186557023707, + "velocityY": -0.1452977412714415, + "timestamp": 0.11660889640828408 }, { - "x": 0.5567354919847137, - "y": 6.955441991018911, - "heading": -1.9330585876590946e-25, - "angularVelocity": -2.807809436759289e-27, - "velocityX": 1.0624400322886223, - "velocityY": -0.21502210580061862, - "timestamp": 0.17479888862353765 + "x": 0.5567354919435141, + "y": 6.955441990812605, + "heading": -1.1494407039997645e-25, + "angularVelocity": -1.6703718504884638e-27, + "velocityX": 1.0617448156257865, + "velocityY": -0.21488140571319628, + "timestamp": 0.1749133446124261 }, { - "x": 0.6393312280567922, - "y": 6.939025561506136, - "heading": -2.416735315116705e-25, - "angularVelocity": -2.8078094367592704e-27, - "velocityX": 1.417555970563931, - "velocityY": -0.2817482932880241, - "timestamp": 0.23306518483138355 + "x": 0.6393312280070567, + "y": 6.939025561249793, + "heading": -1.4370443552723021e-25, + "angularVelocity": -1.6703718504884706e-27, + "velocityX": 1.4166283809830285, + "velocityY": -0.28156392982801115, + "timestamp": 0.23321779281656815 }, { - "x": 0.7426606798556364, - "y": 6.918954250670703, - "heading": -2.9004008719315308e-25, - "angularVelocity": -2.7886377311391825e-27, - "velocityX": 1.7734000361075017, - "velocityY": -0.3444754882622709, - "timestamp": 0.29133148103922946 + "x": 0.742660679814762, + "y": 6.9189542504517005, + "heading": -1.7246480065448678e-25, + "angularVelocity": -1.6703718505364274e-27, + "velocityX": 1.7722395973274057, + "velocityY": -0.3442500772465394, + "timestamp": 0.29152224102071017 }, { "x": 0.8667811155319214, "y": 6.895569324493408, - "heading": -3.4495163504680925e-25, - "angularVelocity": -1.1511759285858433e-25, - "velocityX": 2.130226970897995, - "velocityY": -0.40134567836399193, - "timestamp": 0.34959777724707536 - }, - { - "x": 1.01097419129112, - "y": 6.869549473556677, - "heading": 0.010039096018647366, - "angularVelocity": 0.17297490397522428, - "velocityX": 2.4844650740475953, - "velocityY": -0.44832534810612334, - "timestamp": 0.4076356531928559 - }, - { - "x": 1.175726572408581, - "y": 6.840802777300111, - "heading": 0.03011443547557583, - "angularVelocity": 0.34590065762715055, - "velocityX": 2.8387045258405714, - "velocityY": -0.4953092405280373, - "timestamp": 0.4656735291386364 - }, - { - "x": 1.3610383679335918, - "y": 6.809328854372862, - "heading": 0.06022199220724938, - "angularVelocity": 0.5187570399681801, - "velocityX": 3.1929458565666793, - "velocityY": -0.5422997036737339, - "timestamp": 0.5237114050844169 - }, - { - "x": 1.5460799084730097, - "y": 6.784713461455284, - "heading": 0.15060245758508448, - "angularVelocity": 1.5572669382709545, - "velocityX": 3.1882893287184593, - "velocityY": -0.4241263574251743, - "timestamp": 0.5817492810301974 - }, - { - "x": 1.7105613111663502, - "y": 6.762828401317669, - "heading": 0.23096913740746994, - "angularVelocity": 1.384728136802675, - "velocityX": 2.83403553305432, - "velocityY": -0.37708237562070457, - "timestamp": 0.6397871569759779 - }, - { - "x": 1.8544824093339345, - "y": 6.743675274341158, - "heading": 0.3013204880217799, - "angularVelocity": 1.2121627380029003, - "velocityX": 2.4797788654780635, - "velocityY": -0.33001081904520096, - "timestamp": 0.6978250329217583 - }, - { - "x": 1.977843185069378, - "y": 6.727255600600169, - "heading": 0.36164875379550304, - "angularVelocity": 1.0394637086664282, - "velocityX": 2.1255218893725236, - "velocityY": -0.2829130713937313, - "timestamp": 0.7558629088675388 - }, - { - "x": 2.0806437247378744, - "y": 6.713570715573333, - "heading": 0.41194198603949084, - "angularVelocity": 0.8665588018929588, - "velocityX": 1.7712664013502706, - "velocityY": -0.2357923132752318, - "timestamp": 0.8139007848133193 - }, - { - "x": 2.162884155033722, - "y": 6.702621710001363, - "heading": 0.45218713038285063, - "angularVelocity": 0.6934289666451124, - "velocityX": 1.4170130962869354, - "velocityY": -0.18865276155520178, - "timestamp": 0.8719386607590998 - }, - { - "x": 2.2245645766627997, - "y": 6.694409403888087, - "heading": 0.4823732249131576, - "angularVelocity": 0.5201102562489893, - "velocityX": 1.0627615263987253, - "velocityY": -0.1414990810647272, - "timestamp": 0.9299765367048802 - }, - { - "x": 2.265685008078604, - "y": 6.688934342765278, - "heading": 0.5024941236633388, - "angularVelocity": 0.34668565005684077, - "velocityX": 0.7085102744666172, - "velocityY": -0.09433600099222642, - "timestamp": 0.9880144126506607 + "heading": -2.0459528719631722e-25, + "angularVelocity": -5.947250390698025e-26, + "velocityX": 2.1288330400208064, + "velocityY": -0.4010830507547883, + "timestamp": 0.3498266892248522 + }, + { + "x": 1.010974192105577, + "y": 6.8695494740311895, + "heading": 0.010039070916729844, + "angularVelocity": 0.17286128420602442, + "velocityX": 2.482839358032844, + "velocityY": -0.4480319745776911, + "timestamp": 0.4079025675965806 + }, + { + "x": 1.1757265748164618, + "y": 6.8408027784970535, + "heading": 0.030114360354405487, + "angularVelocity": 0.3456734534289609, + "velocityX": 2.836847023756549, + "velocityY": -0.49498511843652804, + "timestamp": 0.46597844596830895 + }, + { + "x": 1.361038372709757, + "y": 6.809328856521608, + "heading": 0.06022184256964511, + "angularVelocity": 0.5184163039692586, + "velocityX": 3.1908565671131695, + "velocityY": -0.541944829038827, + "timestamp": 0.5240543243400373 + }, + { + "x": 1.5460799068169642, + "y": 6.7847134615337374, + "heading": 0.15060251572580788, + "angularVelocity": 1.5562515056192512, + "velocityX": 3.1862029347676004, + "velocityY": -0.42384886252280385, + "timestamp": 0.5821302027117656 + }, + { + "x": 1.7105613074532942, + "y": 6.762828400607154, + "heading": 0.2309692612234495, + "angularVelocity": 1.383823159474839, + "velocityX": 2.832181023307612, + "velocityY": -0.37683564226962823, + "timestamp": 0.640206081083494 + }, + { + "x": 1.8544824052632503, + "y": 6.743675273377495, + "heading": 0.30132062249757485, + "angularVelocity": 1.2113697329522057, + "velocityX": 2.478156195740254, + "velocityY": -0.32979487812590036, + "timestamp": 0.6982819594552223 + }, + { + "x": 1.9778431814510589, + "y": 6.7272555996641294, + "heading": 0.36164887282711905, + "angularVelocity": 1.0387832611570498, + "velocityX": 2.124131044531241, + "velocityY": -0.28272794443621835, + "timestamp": 0.7563578378269507 + }, + { + "x": 2.080643721940094, + "y": 6.71357071481128, + "heading": 0.4119420778554084, + "angularVelocity": 0.865991293431256, + "velocityX": 1.7701073728241554, + "velocityY": -0.23563801765090975, + "timestamp": 0.814433716198679 + }, + { + "x": 2.162884153160651, + "y": 6.702621709473572, + "heading": 0.45218719175687055, + "angularVelocity": 0.692974691555483, + "velocityX": 1.4160858781017074, + "velocityY": -0.18852931104418616, + "timestamp": 0.8725095945704073 + }, + { + "x": 2.224564575642773, + "y": 6.694409403593854, + "heading": 0.48237325830124134, + "angularVelocity": 0.5197694359637186, + "velocityX": 1.062066114391284, + "velocityY": -0.1414064859622663, + "timestamp": 0.9305854729421357 + }, + { + "x": 2.2656850077143114, + "y": 6.688934342658481, + "heading": 0.5024941355793361, + "angularVelocity": 0.3464584237418892, + "velocityX": 0.7080466662654255, + "velocityY": -0.09427426823109995, + "timestamp": 0.988661351313864 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 0.17327126075488852, - "velocityX": 0.354257244182047, - "velocityY": -0.047168141046465786, - "timestamp": 1.0460522885964412 + "angularVelocity": 0.17315767410862712, + "velocityX": 0.3540254393299603, + "velocityY": -0.04713727434869594, + "timestamp": 1.0467372296855924 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": -1.480616560828957e-26, - "velocityX": -5.749006986314809e-23, - "velocityY": -3.283580548358229e-23, - "timestamp": 1.1040901645422216 + "angularVelocity": 1.537853498112127e-28, + "velocityX": 2.8833537662590268e-24, + "velocityY": 1.7087922239161212e-24, + "timestamp": 1.1048131080573207 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/AmpLanePAEDF.2.traj b/src/main/deploy/choreo/AmpLanePAEDF.2.traj index 66aab758..92b6329d 100644 --- a/src/main/deploy/choreo/AmpLanePAEDF.2.traj +++ b/src/main/deploy/choreo/AmpLanePAEDF.2.traj @@ -4,82 +4,82 @@ "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": -1.480616560828957e-26, - "velocityX": -5.749006986314809e-23, - "velocityY": -3.283580548358229e-23, + "angularVelocity": 1.537853498112127e-28, + "velocityX": 2.8833537662590268e-24, + "velocityY": 1.7087922239161212e-24, "timestamp": 0 }, { - "x": 2.3347330882331176, - "y": 6.713955255875249, + "x": 2.3347330882302457, + "y": 6.713955255873605, "heading": 0.5125504196, - "angularVelocity": -2.553583698003105e-17, - "velocityX": 0.5108360349814401, - "velocityY": 0.2924454065412527, - "timestamp": 0.09491840599213663 + "angularVelocity": 2.6668696602250958e-17, + "velocityX": 0.5105017651842132, + "velocityY": 0.29225404246344755, + "timestamp": 0.09498055730211452 }, { - "x": 2.4317085702199166, - "y": 6.7694721581924355, + "x": 2.4317085702128325, + "y": 6.76947215818838, "heading": 0.5125504196, - "angularVelocity": -7.602638325412282e-17, - "velocityX": 1.021672045302073, - "velocityY": 0.5848907989645902, - "timestamp": 0.18983681198427327 + "angularVelocity": 7.804843369806144e-17, + "velocityX": 1.0210035057398825, + "velocityY": 0.5845080708274503, + "timestamp": 0.18996111460422904 }, { - "x": 2.577171786567949, - "y": 6.852747507871406, + "x": 2.5771717865588863, + "y": 6.852747507866218, "heading": 0.5125504196, - "angularVelocity": -2.864728625251783e-17, - "velocityX": 1.5325079980808258, - "velocityY": 0.8773361584461279, - "timestamp": 0.2847552179764099 + "angularVelocity": 1.13517713021376e-16, + "velocityX": 1.5315051888289535, + "velocityY": 0.8767620662927504, + "timestamp": 0.28494167190634356 }, { - "x": 2.771122709968306, - "y": 6.963781289278251, + "x": 2.7711227099773685, + "y": 6.963781289283441, "heading": 0.5125504196, - "angularVelocity": -4.95735356219891e-19, - "velocityX": 2.0433436631502695, - "velocityY": 1.1697813532187173, - "timestamp": 0.37967362396854654 + "angularVelocity": 6.94376955873156e-17, + "velocityX": 2.042006584585122, + "velocityY": 1.1690158972645894, + "timestamp": 0.3799222292084581 }, { - "x": 2.9165859263163383, - "y": 7.047056638957223, + "x": 2.9165859263234224, + "y": 7.047056638961279, "heading": 0.5125504196, - "angularVelocity": -2.1000816812429605e-17, - "velocityX": 1.532507998080826, - "velocityY": 0.8773361584461278, - "timestamp": 0.47459202996068317 + "angularVelocity": 3.9533104900387686e-17, + "velocityX": 1.5315051888289533, + "velocityY": 0.8767620662927503, + "timestamp": 0.4749027865105726 }, { - "x": 3.0135614083031372, - "y": 7.102573541274409, + "x": 3.013561408306009, + "y": 7.102573541276054, "heading": 0.5125504196, - "angularVelocity": 1.0493910343453442e-17, - "velocityX": 1.021672045302073, - "velocityY": 0.5848907989645902, - "timestamp": 0.5695104359528198 + "angularVelocity": -1.917838345664243e-18, + "velocityX": 1.0210035057398823, + "velocityY": 0.5845080708274502, + "timestamp": 0.5698833438126871 }, { "x": 3.062049150466919, "y": 7.130331993103027, "heading": 0.5125504196, - "angularVelocity": 3.728395973644304e-17, - "velocityX": 0.5108360349814401, - "velocityY": 0.2924454065412527, - "timestamp": 0.6644288419449564 + "angularVelocity": -6.474946504344956e-17, + "velocityX": 0.5105017651842131, + "velocityY": 0.29225404246344755, + "timestamp": 0.6648639011148016 }, { "x": 3.062049150466919, "y": 7.130331993103027, "heading": 0.5125504196, - "angularVelocity": -2.9402212487685217e-25, - "velocityX": 6.030156719342467e-23, - "velocityY": 3.0718511862526725e-23, - "timestamp": 0.7593472479370931 + "angularVelocity": 3.837127558436768e-26, + "velocityX": -2.7920148574402887e-24, + "velocityY": -2.697407810456559e-24, + "timestamp": 0.7598444584169162 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/AmpLanePAEDF.3.traj b/src/main/deploy/choreo/AmpLanePAEDF.3.traj index 3bdab8ec..d0c422a5 100644 --- a/src/main/deploy/choreo/AmpLanePAEDF.3.traj +++ b/src/main/deploy/choreo/AmpLanePAEDF.3.traj @@ -4,532 +4,532 @@ "x": 3.062049150466919, "y": 7.130331993103027, "heading": 0.5125504196, - "angularVelocity": -2.9402212487685217e-25, - "velocityX": 6.030156719342467e-23, - "velocityY": 3.0718511862526725e-23, + "angularVelocity": 3.837127558436768e-26, + "velocityX": -2.7920148574402887e-24, + "velocityY": -2.697407810456559e-24, "timestamp": 0 }, { - "x": 3.0865020118467, - "y": 7.124545249523675, - "heading": 0.48610131944841944, - "angularVelocity": -0.40878852104321894, - "velocityX": 0.3779353165675877, - "velocityY": -0.08943798979559645, - "timestamp": 0.0647011811488325 - }, - { - "x": 3.135458110441477, - "y": 7.112972562266447, - "heading": 0.4339971852577861, - "angularVelocity": -0.8053042195748789, - "velocityX": 0.7566492253389184, - "velocityY": -0.17886361657922661, - "timestamp": 0.129402362297665 - }, - { - "x": 3.2089771746406726, - "y": 7.095606129050513, - "heading": 0.357225026215491, - "angularVelocity": -1.1865650314125755, - "velocityX": 1.136286276290371, - "velocityY": -0.2684098328280398, - "timestamp": 0.1941035434464975 - }, - { - "x": 3.307133671873185, - "y": 7.072412045867193, - "heading": 0.2571531716690748, - "angularVelocity": -1.5466773986122515, - "velocityX": 1.5170742711284095, - "velocityY": -0.3584800581919169, - "timestamp": 0.25880472459532977 - }, - { - "x": 3.4300337444037163, - "y": 7.043313631479606, - "heading": 0.13605141213735358, - "angularVelocity": -1.8717086362480815, - "velocityX": 1.8995027656114087, - "velocityY": -0.44973544332447146, - "timestamp": 0.32350590574416205 - }, - { - "x": 3.5778386039995205, - "y": 7.008182862001031, - "heading": -0.0018831098932495121, - "angularVelocity": -2.1318702314461877, - "velocityX": 2.2844228957089365, - "velocityY": -0.5429695231956211, - "timestamp": 0.3882070868929943 - }, - { - "x": 3.75075226330582, - "y": 6.966825747454083, - "heading": -0.14853260580564892, - "angularVelocity": -2.266565977753341, - "velocityX": 2.672496177597522, - "velocityY": -0.6392018478273882, - "timestamp": 0.4529082680418266 - }, - { - "x": 3.9486334270649626, - "y": 6.918918967044097, - "heading": -0.28405555900160556, - "angularVelocity": -2.094597823248583, - "velocityX": 3.0583856468393638, - "velocityY": -0.7404313114436918, - "timestamp": 0.5176094491906589 - }, - { - "x": 4.166070195160817, - "y": 6.865183550075389, - "heading": -0.3449865942152846, - "angularVelocity": -0.9417298746605423, - "velocityX": 3.360630582549694, - "velocityY": -0.8305167852361092, - "timestamp": 0.5823106303394912 - }, - { - "x": 4.4009794486877345, - "y": 6.8112911031395695, - "heading": -0.3449866586118039, - "angularVelocity": -9.952912480962177e-7, - "velocityX": 3.6306795232463367, - "velocityY": -0.8329437883344079, - "timestamp": 0.6470118114883234 - }, - { - "x": 4.635888809481111, - "y": 6.757399123762019, - "heading": -0.34498672300630057, - "angularVelocity": -9.952599862485997e-7, - "velocityX": 3.630681181121147, - "velocityY": -0.8329365619088571, - "timestamp": 0.7117129926371557 - }, - { - "x": 4.87079817027739, - "y": 6.703507144397122, - "heading": -0.3449867874007968, - "angularVelocity": -9.952599802438338e-7, - "velocityX": 3.630681181166014, - "velocityY": -0.8329365617132907, - "timestamp": 0.776414173785988 - }, - { - "x": 5.105707531073669, - "y": 6.649615165032224, - "heading": -0.3449868517952927, - "angularVelocity": -9.952599738581064e-7, - "velocityX": 3.6306811811660147, - "velocityY": -0.8329365617132863, - "timestamp": 0.8411153549348203 - }, - { - "x": 5.340616891869949, - "y": 6.595723185667327, - "heading": -0.3449869161897882, - "angularVelocity": -9.952599684354273e-7, - "velocityX": 3.630681181166015, - "velocityY": -0.8329365617132873, - "timestamp": 0.9058165360836525 - }, - { - "x": 5.575526252666228, - "y": 6.54183120630243, - "heading": -0.34498698058428334, - "angularVelocity": -9.952599624061458e-7, - "velocityX": 3.6306811811660156, - "velocityY": -0.8329365617132882, - "timestamp": 0.9705177172324848 - }, - { - "x": 5.8104356134625075, - "y": 6.4879392269375336, - "heading": -0.344987044978778, - "angularVelocity": -9.952599561054567e-7, - "velocityX": 3.6306811811660156, - "velocityY": -0.8329365617132889, - "timestamp": 1.035218898381317 - }, - { - "x": 6.0453449742587875, - "y": 6.434047247572637, - "heading": -0.3449871093732723, - "angularVelocity": -9.952599496144382e-7, - "velocityX": 3.6306811811660165, - "velocityY": -0.8329365617132901, - "timestamp": 1.0999200795301494 - }, - { - "x": 6.280254335055067, - "y": 6.38015526820774, - "heading": -0.3449871737677662, - "angularVelocity": -9.952599439036616e-7, - "velocityX": 3.630681181166016, - "velocityY": -0.832936561713291, - "timestamp": 1.1646212606789816 - }, - { - "x": 6.515163695851346, - "y": 6.326263288842841, - "heading": -0.3449872381622598, - "angularVelocity": -9.952599382292206e-7, - "velocityX": 3.6306811811660133, - "velocityY": -0.8329365617133051, - "timestamp": 1.229322441827814 - }, - { - "x": 6.750073056640409, - "y": 6.2723713094464895, - "heading": -0.34498730255675286, - "angularVelocity": -9.952599312963826e-7, - "velocityX": 3.6306811810544866, - "velocityY": -0.832936562199441, - "timestamp": 1.2940236229766462 - }, - { - "x": 6.984982150775984, - "y": 6.2184781677494305, - "heading": -0.3449873669513136, - "angularVelocity": -9.95260976649588e-7, - "velocityX": 3.6306770597466, - "velocityY": -0.8329545263337457, - "timestamp": 1.3587248041254785 + "x": 3.0864612629057913, + "y": 7.12456423366324, + "heading": 0.4864079011679127, + "angularVelocity": -0.404266786795747, + "velocityX": 0.377507862534246, + "velocityY": -0.08919238526276327, + "timestamp": 0.06466650065244073 + }, + { + "x": 3.135333250268508, + "y": 7.113029742760453, + "heading": 0.43488377787747834, + "angularVelocity": -0.7967668386350167, + "velocityX": 0.7557543220930696, + "velocityY": -0.17836887393645673, + "timestamp": 0.12933300130488146 + }, + { + "x": 3.2087217801138035, + "y": 7.095721914438121, + "heading": 0.3589213764077576, + "angularVelocity": -1.1746793270597933, + "velocityX": 1.1348770863562407, + "velocityY": -0.26764751683960536, + "timestamp": 0.19399950195732196 + }, + { + "x": 3.306697290004189, + "y": 7.0726100183064045, + "heading": 0.2598184881364111, + "angularVelocity": -1.5325228251330487, + "velocityX": 1.5150890940731196, + "velocityY": -0.3574013731767354, + "timestamp": 0.2586660026097627 + }, + { + "x": 3.4293598616620984, + "y": 7.043622922718144, + "heading": 0.13971196471311126, + "angularVelocity": -1.85732214069893, + "velocityX": 1.8968487612647713, + "velocityY": -0.44825520626292115, + "timestamp": 0.3233325032622034 + }, + { + "x": 3.5768625085702324, + "y": 7.008640009161994, + "heading": 0.002547090154321883, + "angularVelocity": -2.1211117529925048, + "velocityX": 2.2809746224077885, + "velocityY": -0.540974278849118, + "timestamp": 0.38799900391464415 + }, + { + "x": 3.74940588673651, + "y": 6.967477445012277, + "heading": -0.14404002070042093, + "angularVelocity": -2.266816812039918, + "velocityX": 2.6682034194742714, + "velocityY": -0.6365361312954025, + "timestamp": 0.4526655045670849 + }, + { + "x": 3.946923390754574, + "y": 6.9198227973610145, + "heading": -0.2817650462278063, + "angularVelocity": -2.129773903610591, + "velocityX": 3.0544022333858827, + "velocityY": -0.7369294328664762, + "timestamp": 0.5173320052195256 + }, + { + "x": 4.163931894345135, + "y": 6.866332868698949, + "heading": -0.34498660423093713, + "angularVelocity": -0.9776554686780423, + "velocityX": 3.355810217053556, + "velocityY": -0.8271659688152148, + "timestamp": 0.5819985058719663 + }, + { + "x": 4.399029324107109, + "y": 6.812427188093308, + "heading": -0.34498666703392, + "angularVelocity": -9.711826405660571e-7, + "velocityX": 3.635536597620139, + "velocityY": -0.8335951390870039, + "timestamp": 0.6466650065244071 + }, + { + "x": 4.63412689461735, + "y": 6.758522121329691, + "heading": -0.3449867298342717, + "angularVelocity": -9.711419520664826e-7, + "velocityX": 3.6355387741453202, + "velocityY": -0.8335856466601779, + "timestamp": 0.7113315071768478 + }, + { + "x": 4.869224465131426, + "y": 6.7046170545827986, + "heading": -0.344986792634623, + "angularVelocity": -9.711419462634673e-7, + "velocityX": 3.6355387742046146, + "velocityY": -0.8335856464015791, + "timestamp": 0.7759980078292885 + }, + { + "x": 5.1043220356455015, + "y": 6.650711987835906, + "heading": -0.344986855434974, + "angularVelocity": -9.711419414707099e-7, + "velocityX": 3.6355387742046164, + "velocityY": -0.8335856464015731, + "timestamp": 0.8406645084817292 + }, + { + "x": 5.339419606159577, + "y": 6.596806921089014, + "heading": -0.3449869182353246, + "angularVelocity": -9.711419349644554e-7, + "velocityX": 3.635538774204617, + "velocityY": -0.833585646401574, + "timestamp": 0.90533100913417 + }, + { + "x": 5.574517176673654, + "y": 6.542901854342122, + "heading": -0.3449869810356748, + "angularVelocity": -9.711419302688624e-7, + "velocityX": 3.635538774204617, + "velocityY": -0.8335856464015745, + "timestamp": 0.9699975097866107 + }, + { + "x": 5.809614747187731, + "y": 6.488996787595229, + "heading": -0.3449870438360248, + "angularVelocity": -9.711419254509242e-7, + "velocityX": 3.6355387742046172, + "velocityY": -0.8335856464015756, + "timestamp": 1.0346640104390514 + }, + { + "x": 6.044712317701808, + "y": 6.435091720848337, + "heading": -0.3449871066363744, + "angularVelocity": -9.711419195368686e-7, + "velocityX": 3.6355387742046172, + "velocityY": -0.8335856464015763, + "timestamp": 1.0993305110914922 + }, + { + "x": 6.279809888215884, + "y": 6.381186654101444, + "heading": -0.34498716943672364, + "angularVelocity": -9.71141914262753e-7, + "velocityX": 3.6355387742046177, + "velocityY": -0.8335856464015771, + "timestamp": 1.1639970117439329 + }, + { + "x": 6.51490745872996, + "y": 6.32728158735455, + "heading": -0.34498723223707245, + "angularVelocity": -9.71141908290338e-7, + "velocityX": 3.635538774204614, + "velocityY": -0.8335856464015945, + "timestamp": 1.2286635123963736 + }, + { + "x": 6.750005029235043, + "y": 6.273376520568434, + "heading": -0.344987295037421, + "angularVelocity": -9.71141902973265e-7, + "velocityX": 3.6355387740655463, + "velocityY": -0.8335856470081151, + "timestamp": 1.2933300130488143 + }, + { + "x": 6.985102269626149, + "y": 6.219470014067963, + "heading": -0.34498735783785006, + "angularVelocity": -9.711431495756142e-7, + "velocityX": 3.6355336691971325, + "velocityY": -0.8336079106893309, + "timestamp": 1.357996513701255 }, { "x": 7.214789390563965, "y": 6.145846366882324, "heading": -0.344987478573796, - "angularVelocity": -0.00000172520007220815, - "velocityX": 3.5518244907979937, - "velocityY": -1.1225730284587905, - "timestamp": 1.4234259852743107 - }, - { - "x": 7.409091443638604, - "y": 6.073965873280571, - "heading": -0.3572291452032454, - "angularVelocity": -0.20035361948729366, - "velocityX": 3.1800506242888202, - "velocityY": -1.1764343450588084, - "timestamp": 1.4845262871344713 - }, - { - "x": 7.58146433123023, - "y": 6.0094940876522065, - "heading": -0.36768136673125634, - "angularVelocity": -0.17106661030796205, - "velocityX": 2.8211462520452746, - "velocityY": -1.0551794944633763, - "timestamp": 1.545626588994632 - }, - { - "x": 7.73220907640328, - "y": 5.953248314526567, - "heading": -0.37521177845828957, - "angularVelocity": -0.1232467188831242, - "velocityX": 2.4671685831938674, - "velocityY": -0.920548203744869, - "timestamp": 1.6067268908547925 - }, - { - "x": 7.861444501366057, - "y": 5.905524901339061, - "heading": -0.37940510209885836, - "angularVelocity": -0.06863016241991673, - "velocityX": 2.1151356217282813, - "velocityY": -0.7810667334627726, - "timestamp": 1.667827192714953 - }, - { - "x": 7.969234018825332, - "y": 5.866476846712542, - "heading": -0.38004587758642694, - "angularVelocity": -0.010487272043845954, - "velocityX": 1.7641405063099576, - "velocityY": -0.6390812064380308, - "timestamp": 1.7289274945751136 - }, - { - "x": 8.055617039018077, - "y": 5.8361975486879665, - "heading": -0.37700219526471646, - "angularVelocity": 0.04981452184436869, - "velocityX": 1.413790399766751, - "velocityY": -0.4955670774569341, - "timestamp": 1.7900277964352742 - }, - { - "x": 8.12062042791295, - "y": 5.814749961981242, - "heading": -0.37018491999207215, - "angularVelocity": 0.11157514881427125, - "velocityX": 1.0638799959392298, - "velocityY": -0.3510225981503441, - "timestamp": 1.8511280982954348 - }, - { - "x": 8.164263678264032, - "y": 5.802179408572721, - "heading": -0.3595296539883727, - "angularVelocity": 0.17438974406519217, - "velocityX": 0.7142886208805774, - "velocityY": -0.20573635523587247, - "timestamp": 1.9122284001555954 + "angularVelocity": -0.0000018670555043319872, + "velocityX": 3.551871813387612, + "velocityY": -1.1385129308502273, + "timestamp": 1.4226630143536958 + }, + { + "x": 7.409712349690949, + "y": 6.073735935167582, + "heading": -0.3570370868218633, + "angularVelocity": -0.19637949309616867, + "velocityX": 3.176773146322184, + "velocityY": -1.1752257613319905, + "timestamp": 1.484021805223812 + }, + { + "x": 7.582537346896194, + "y": 6.009055207663653, + "heading": -0.36748967160875157, + "angularVelocity": -0.17035187034591742, + "velocityX": 2.816629773084668, + "velocityY": -1.0541395387149, + "timestamp": 1.5453805960939284 + }, + { + "x": 7.733574876390023, + "y": 5.952649624668501, + "heading": -0.37509136643476676, + "angularVelocity": -0.12388925398002781, + "velocityX": 2.4615467050767728, + "velocityY": -0.9192746824909085, + "timestamp": 1.6067393869640447 + }, + { + "x": 7.862949786289389, + "y": 5.90483023458532, + "heading": -0.37937521943526015, + "angularVelocity": -0.06981645074397583, + "velocityX": 2.1084983596437845, + "velocityY": -0.7793404890328669, + "timestamp": 1.668098177834161 + }, + { + "x": 7.9707292244718095, + "y": 5.86575856751758, + "heading": -0.3800974258169581, + "angularVelocity": -0.011770218602036829, + "velocityX": 1.7565443623321622, + "velocityY": -0.636773745272234, + "timestamp": 1.7294569687042773 + }, + { + "x": 8.056955102704071, + "y": 5.835533544523746, + "heading": -0.37710820320869415, + "angularVelocity": 0.04871710419769366, + "velocityX": 1.4052734255273058, + "velocityY": -0.49259482732986287, + "timestamp": 1.7908157595743937 + }, + { + "x": 8.121656070762867, + "y": 5.8142219773756105, + "heading": -0.3703061215705459, + "angularVelocity": 0.11085749151326833, + "velocityX": 1.0544694108421286, + "velocityY": -0.34732703897714834, + "timestamp": 1.85217455044451 + }, + { + "x": 8.164852955153219, + "y": 5.801872031339644, + "heading": -0.3596178039887861, + "angularVelocity": 0.1741937451861573, + "velocityX": 0.7040048178555333, + "velocityY": -0.2012742731861952, + "timestamp": 1.9135333413146263 }, { "x": 8.186561584472656, "y": 5.798520088195801, "heading": -0.344987478573796, - "angularVelocity": 0.23800496841831012, - "velocityX": 0.3649393788537573, - "velocityY": -0.05989038131588724, - "timestamp": 1.973328702015756 - }, - { - "x": 8.18566399540182, - "y": 5.805056139735724, - "heading": -0.32461060520225765, - "angularVelocity": 0.30765327861825625, - "velocityX": -0.013551942707795956, - "velocityY": 0.09868234683560065, - "timestamp": 2.0395619417948208 - }, - { - "x": 8.159694473473342, - "y": 5.822092590454781, - "heading": -0.2998178925011234, - "angularVelocity": 0.3743243239170494, - "velocityX": -0.3920919770058792, - "velocityY": 0.25721904554096775, - "timestamp": 2.1057951815738853 - }, - { - "x": 8.108649030585239, - "y": 5.849626323109156, - "heading": -0.2708591800534793, - "angularVelocity": 0.4372232514103506, - "velocityX": -0.7706922243027172, - "velocityY": 0.41570867960287095, - "timestamp": 2.17202842135295 - }, - { - "x": 8.03252261804608, - "y": 5.887653107737229, - "heading": -0.2380618973255709, - "angularVelocity": 0.4951785966881702, - "velocityX": -1.1493686975466637, - "velocityY": 0.5741344490307231, - "timestamp": 2.2382616611320145 - }, - { - "x": 7.9313086642803885, - "y": 5.936166919703684, - "heading": -0.20187479302243566, - "angularVelocity": 0.5463586625664867, - "velocityX": -1.5281443894834816, - "velocityY": 0.7324692575553122, - "timestamp": 2.304494900911079 - }, - { - "x": 7.804998346649701, - "y": 5.9951585907413225, - "heading": -0.16295187123346636, - "angularVelocity": 0.5876644705710469, - "velocityX": -1.9070532870205719, - "velocityY": 0.8906656421219618, - "timestamp": 2.3707281406901437 - }, - { - "x": 7.653579505512198, - "y": 6.0646127178391085, - "heading": -0.12233575906363518, - "angularVelocity": 0.6132285285351425, - "velocityX": -2.286145772765958, - "velocityY": 1.048629469575472, - "timestamp": 2.4369613804692083 - }, - { - "x": 7.4770359425587705, - "y": 6.144498725543435, - "heading": -0.08194804569131871, - "angularVelocity": 0.6097801271240603, - "velocityX": -2.6654828231614, - "velocityY": 1.206131663962129, - "timestamp": 2.503194620248273 - }, - { - "x": 7.27536197665765, - "y": 6.234730628199731, - "heading": -0.046490249768474004, - "angularVelocity": 0.5353474485186261, - "velocityX": -3.044905648188839, - "velocityY": 1.362335633245254, - "timestamp": 2.5694278600273375 - }, - { - "x": 7.049560259358204, - "y": 6.334147758763648, - "heading": -0.04649017070950518, - "angularVelocity": 0.0000011936448991960951, - "velocityX": -3.4091902804793617, - "velocityY": 1.5010156666885635, - "timestamp": 2.635661099806402 - }, - { - "x": 6.822106687595601, - "y": 6.429725315002885, - "heading": -0.04649016173832054, - "angularVelocity": 1.354483741042957e-7, - "velocityX": -3.4341302421763333, - "velocityY": 1.4430451621882883, - "timestamp": 2.7018943395854667 - }, - { - "x": 6.586112566537963, - "y": 6.501676487205841, - "heading": -0.04649015261662469, - "angularVelocity": 1.3772081626072917e-7, - "velocityX": -3.563076815279552, - "velocityY": 1.086330254158866, - "timestamp": 2.7681275793645312 - }, - { - "x": 6.343631547831337, - "y": 6.547208357601844, - "heading": -0.046490142857452815, - "angularVelocity": 1.4734553082512417e-7, - "velocityX": -3.6610170288434327, - "velocityY": 0.6874474289327321, - "timestamp": 2.834360819143596 - }, - { - "x": 6.097611695820765, - "y": 6.565767095818739, - "heading": -0.046490131800601416, - "angularVelocity": 1.6693810300562863e-7, - "velocityX": -3.7144468975279485, - "velocityY": 0.28020278456559833, - "timestamp": 2.9005940589226604 + "angularVelocity": 0.2384389458710078, + "velocityX": 0.35379819275433294, + "velocityY": -0.054628572309033785, + "timestamp": 1.9748921321847426 + }, + { + "x": 8.185020658343282, + "y": 5.805387130671382, + "heading": -0.32459673237759007, + "angularVelocity": 0.30843585413704155, + "velocityX": -0.023308458763729726, + "velocityY": 0.10387271220830593, + "timestamp": 2.041002300385701 + }, + { + "x": 8.158545676834562, + "y": 5.822731510098044, + "heading": -0.29984074692215335, + "angularVelocity": 0.37446562501830083, + "velocityX": -0.40046761684590937, + "velocityY": 0.2623556995640975, + "timestamp": 2.107112468586659 + }, + { + "x": 8.10713245572277, + "y": 5.850551569484812, + "heading": -0.2710499624922478, + "angularVelocity": 0.43549706820271483, + "velocityX": -0.7776900666098594, + "velocityY": 0.42081362283335544, + "timestamp": 2.1732226367876173 + }, + { + "x": 8.030775877182073, + "y": 5.8888449452190175, + "heading": -0.23865422460782199, + "angularVelocity": 0.49002655364528946, + "velocityX": -1.154989929969518, + "velocityY": 0.5792357934683214, + "timestamp": 2.2393328049885755 + }, + { + "x": 7.929469606523289, + "y": 5.937608095309845, + "heading": -0.2032366176335679, + "angularVelocity": 0.5357361497945922, + "velocityX": -1.5323856135237626, + "velocityY": 0.7376043870074612, + "timestamp": 2.3054429731895336 + }, + { + "x": 7.803205805922035, + "y": 5.996835346003344, + "heading": -0.16563448060225386, + "angularVelocity": 0.5687799328692251, + "velocityX": -1.909899854095725, + "velocityY": 0.8958871578342301, + "timestamp": 2.371553141390492 + }, + { + "x": 7.651975275778253, + "y": 6.066516654618355, + "heading": -0.12715303116567858, + "angularVelocity": 0.5820806463477988, + "velocityX": -2.2875532502667433, + "velocityY": 1.0540180203928478, + "timestamp": 2.43766330959145 + }, + { + "x": 7.4757708248918915, + "y": 6.146630988469932, + "heading": -0.09010777903671587, + "angularVelocity": 0.5603563436165322, + "velocityX": -2.665315422443717, + "velocityY": 1.211830736960907, + "timestamp": 2.503773477792408 + }, + { + "x": 7.274619001255966, + "y": 6.237116150274765, + "heading": -0.05972677615109367, + "angularVelocity": 0.45955113581426293, + "velocityX": -3.0426760226123557, + "velocityY": 1.368702640867323, + "timestamp": 2.5698836459933663 + }, + { + "x": 7.049305084903872, + "y": 6.337299858985659, + "heading": -0.05972649238552697, + "angularVelocity": 0.000004292313488445514, + "velocityX": -3.408158267986829, + "velocityY": 1.515405442720417, + "timestamp": 2.6359938141943244 + }, + { + "x": 6.822055990090289, + "y": 6.433013154965271, + "heading": -0.05972648235247219, + "angularVelocity": 1.5176265701728567e-7, + "velocityX": -3.4374302924597684, + "velocityY": 1.4477847899080587, + "timestamp": 2.7021039823952826 + }, + { + "x": 6.585901540176483, + "y": 6.503965686132618, + "heading": -0.05972647213388985, + "angularVelocity": 1.5456899617029753e-7, + "velocityX": -3.5721350639429144, + "velocityY": 1.0732468710663359, + "timestamp": 2.7682141505962408 + }, + { + "x": 6.343393108903057, + "y": 6.54860698900868, + "heading": -0.05972646117550161, + "angularVelocity": 1.65759497255032e-7, + "velocityX": -3.6682470771555504, + "velocityY": 0.6752562289716734, + "timestamp": 2.834324318797199 + }, + { + "x": 6.097452808494555, + "y": 6.566398909819071, + "heading": -0.059726448733939674, + "angularVelocity": 1.8819437727240375e-7, + "velocityX": -3.720158443114319, + "velocityY": 0.26912532965138075, + "timestamp": 2.900434486998157 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -0.04649011830549673, - "angularVelocity": 2.0375123930314497e-7, - "velocityX": -3.722715642994527, - "velocityY": -0.13044955895548105, - "timestamp": 2.966827298701725 - }, - { - "x": 5.59400062961639, - "y": 6.517937013895863, - "heading": -0.046490107217199095, - "angularVelocity": 1.588527062833638e-7, - "velocityX": -3.682446546399627, - "velocityY": -0.5614419262393832, - "timestamp": 3.03662968283788 - }, - { - "x": 5.343239064324899, - "y": 6.449191978638034, - "heading": -0.046490097789975614, - "angularVelocity": 1.3505589536533548e-7, - "velocityX": -3.5924498624912715, - "velocityY": -0.984852252664273, - "timestamp": 3.106432066974035 - }, - { - "x": 5.10214573961629, - "y": 6.351820274741271, - "heading": -0.04649007357112031, - "angularVelocity": 3.469631532168612e-7, - "velocityX": -3.453941118090443, - "velocityY": -1.3949624371974614, - "timestamp": 3.17623445111019 - }, - { - "x": 4.887305900136772, - "y": 6.241894655501948, - "heading": -0.013339477589168081, - "angularVelocity": 0.4749206834724951, - "velocityX": -3.0778295345966584, - "velocityY": -1.574811814807135, - "timestamp": 3.246036835246345 - }, - { - "x": 4.700625263351654, - "y": 6.142924243380359, - "heading": 0.02019201613120645, - "angularVelocity": 0.4803774847427688, - "velocityX": -2.6744163411522406, - "velocityY": -1.417865784190692, - "timestamp": 3.3158392193825 - }, - { - "x": 4.541203570947359, - "y": 6.05696873798045, - "heading": 0.05079450877042205, - "angularVelocity": 0.4384161517968095, - "velocityX": -2.283900390756435, - "velocityY": -1.2314121711408288, - "timestamp": 3.385641603518655 - }, - { - "x": 4.408671715563658, - "y": 5.984755616609754, - "heading": 0.07726906366461818, - "angularVelocity": 0.3792786624960486, - "velocityX": -1.8986723308073217, - "velocityY": -1.034536603074175, - "timestamp": 3.45544398765481 - }, - { - "x": 4.302830646527999, - "y": 5.92665565579208, - "heading": 0.09900038404891696, - "angularVelocity": 0.3113263343829381, - "velocityX": -1.5162959022890692, - "velocityY": -0.8323492318592651, - "timestamp": 3.5252463717909652 - }, - { - "x": 4.22355609558814, - "y": 5.882893451015234, - "heading": 0.11561440292356019, - "angularVelocity": 0.23801506324248706, - "velocityX": -1.1356997604154628, - "velocityY": -0.6269442701482041, - "timestamp": 3.5950487559271203 - }, - { - "x": 4.170763147512419, - "y": 5.853619589907405, - "heading": 0.1268589116332185, - "angularVelocity": 0.1610906109986881, - "velocityX": -0.756320127586832, - "velocityY": -0.4193819662481466, - "timestamp": 3.6648511400632753 + "heading": -0.05972643351340637, + "angularVelocity": 2.3022983790495482e-7, + "velocityX": -3.7272425285094304, + "velocityY": -0.14024939234037223, + "timestamp": 2.9665446551991153 + }, + { + "x": 5.593733281594629, + "y": 6.517293519804324, + "heading": -0.059726421475485736, + "angularVelocity": 1.7244301744192055e-7, + "velocityX": -3.6859744116289104, + "velocityY": -0.5706139410489331, + "timestamp": 3.03635276227729 + }, + { + "x": 5.342760368520485, + "y": 6.447952318722012, + "heading": -0.059726411223577615, + "angularVelocity": 1.4685841743171806e-7, + "velocityX": -3.595182903227699, + "velocityY": -0.9933115791932222, + "timestamp": 3.106160869355465 + }, + { + "x": 5.101497403322433, + "y": 6.350035089382074, + "heading": -0.05972638349654275, + "angularVelocity": 3.9718932395532394e-7, + "velocityX": -3.456088057621621, + "velocityY": -1.4026627198225616, + "timestamp": 3.17596897643364 + }, + { + "x": 4.886759838624346, + "y": 6.240596211657777, + "heading": -0.024369209517199585, + "angularVelocity": 0.5064909429466152, + "velocityX": -3.0761121263123843, + "velocityY": -1.5677101457820914, + "timestamp": 3.2457770835118147 + }, + { + "x": 4.700188679669267, + "y": 6.141997521730213, + "heading": 0.01157815322069568, + "angularVelocity": 0.5149453873264229, + "velocityX": -2.672628821551406, + "velocityY": -1.4124246316713311, + "timestamp": 3.3155851905899896 + }, + { + "x": 4.540876156189366, + "y": 6.0563320226623665, + "heading": 0.04446453476629188, + "angularVelocity": 0.47109688146633416, + "velocityX": -2.2821493111322217, + "velocityY": -1.2271568826800237, + "timestamp": 3.3853932976681644 + }, + { + "x": 4.40844486564244, + "y": 5.984344508560839, + "heading": 0.07295632714737806, + "angularVelocity": 0.40814446306615176, + "velocityX": -1.8970760859999023, + "velocityY": -1.031219970209356, + "timestamp": 3.4552014047463393 + }, + { + "x": 4.302690180424501, + "y": 5.9264156183199805, + "heading": 0.09636693816788014, + "angularVelocity": 0.3353566226095424, + "velocityX": -1.5149341479709326, + "velocityY": -0.8298304117598572, + "timestamp": 3.525009511824514 + }, + { + "x": 4.223483980380201, + "y": 5.882776245569646, + "heading": 0.11427821119992405, + "angularVelocity": 0.2565786952507661, + "velocityX": -1.1346275290862768, + "velocityY": -0.6251333058131043, + "timestamp": 3.594817618902689 + }, + { + "x": 4.170738558104569, + "y": 5.853581333825438, + "heading": 0.12640786347483376, + "angularVelocity": 0.17375707181583125, + "velocityX": -0.7555773173532012, + "velocityY": -0.41821663652209046, + "timestamp": 3.664625725980864 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 0.08154784945777828, - "velocityX": -0.37782436284418364, - "velocityY": -0.21027274576950902, - "timestamp": 3.7346535241994303 + "angularVelocity": 0.08800242162180778, + "velocityX": -0.37744114553759195, + "velocityY": -0.20970748963591992, + "timestamp": 3.7344338330590388 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 1.1000727380078476e-26, - "velocityX": 7.869680140840765e-27, - "velocityY": 7.286496207304452e-27, - "timestamp": 3.8044559083355853 + "angularVelocity": 6.956965199088368e-27, + "velocityX": 5.711893177801191e-27, + "velocityY": 1.0709622806033376e-27, + "timestamp": 3.8042419401372136 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/AmpLanePAEDF.4.traj b/src/main/deploy/choreo/AmpLanePAEDF.4.traj index 5eec9f40..44d4bb45 100644 --- a/src/main/deploy/choreo/AmpLanePAEDF.4.traj +++ b/src/main/deploy/choreo/AmpLanePAEDF.4.traj @@ -4,334 +4,334 @@ "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 1.1000727380078476e-26, - "velocityX": 7.869680140840765e-27, - "velocityY": 7.286496207304452e-27, + "angularVelocity": 6.956965199088368e-27, + "velocityX": 5.711893177801191e-27, + "velocityY": 1.0709622806033376e-27, "timestamp": 0 }, { - "x": 4.195350058507561, - "y": 5.858786392293566, - "heading": 0.13540679339101, - "angularVelocity": 0.03040787160499198, - "velocityX": 0.5426383042255808, - "velocityY": 0.2113090624413005, - "timestamp": 0.09391145429572223 - }, - { - "x": 4.297269963634785, - "y": 5.898475069782893, - "heading": 0.14111775296085857, - "angularVelocity": 0.06081217262236255, - "velocityX": 1.085276613929165, - "velocityY": 0.42261806919047107, - "timestamp": 0.18782290859144446 - }, - { - "x": 4.450149823252348, - "y": 5.958008075795516, - "heading": 0.14968325491595807, - "angularVelocity": 0.09120827719403884, - "velocityX": 1.6279149414101537, - "velocityY": 0.6339269949452273, - "timestamp": 0.2817343628871667 - }, - { - "x": 4.653989639578877, - "y": 6.037385400048414, - "heading": 0.16110213512106505, - "angularVelocity": 0.12159198567141313, - "velocityX": 2.1705532925158373, - "velocityY": 0.8452358112030016, - "timestamp": 0.3756458171828889 - }, - { - "x": 4.908789413625767, - "y": 6.136607028819001, - "heading": 0.17537288795521153, - "angularVelocity": 0.15195966180236836, - "velocityX": 2.7131916543911667, - "velocityY": 1.0565444813381684, - "timestamp": 0.46955727147861115 - }, - { - "x": 5.214549136636689, - "y": 6.255672941521673, - "heading": 0.19249371968896806, - "angularVelocity": 0.1823082377134106, - "velocityX": 3.2558299230262406, - "velocityY": 1.2678529322710714, - "timestamp": 0.5634687257743334 - }, - { - "x": 5.540526297381237, - "y": 6.382610310333338, - "heading": 0.1924937215917483, - "angularVelocity": 2.0261428816305725e-8, - "velocityX": 3.4711118381583637, - "velocityY": 1.3516707814144493, - "timestamp": 0.6573801800700556 - }, - { - "x": 5.866503458135056, - "y": 6.50954767912126, - "heading": 0.19249372349451602, - "angularVelocity": 2.0261295290604937e-8, - "velocityX": 3.471111838257083, - "velocityY": 1.351670781161622, - "timestamp": 0.7512916343657778 - }, - { - "x": 6.192480618888875, - "y": 6.636485047909182, - "heading": 0.19249372539728374, - "angularVelocity": 2.0261295568537564e-8, - "velocityX": 3.471111838257084, - "velocityY": 1.3516707811616195, - "timestamp": 0.8452030886615001 - }, - { - "x": 6.518457779642694, - "y": 6.763422416697105, - "heading": 0.19249372730005146, - "angularVelocity": 2.0261295333491658e-8, - "velocityX": 3.4711118382570834, - "velocityY": 1.35167078116162, - "timestamp": 0.9391145429572223 - }, - { - "x": 6.844434940394272, - "y": 6.890359785490778, - "heading": 0.19249372920281918, - "angularVelocity": 2.0261295628050687e-8, - "velocityX": 3.471111838233232, - "velocityY": 1.3516707812228705, - "timestamp": 1.0330259972529445 - }, - { - "x": 7.170411838536006, - "y": 7.017297824914972, - "heading": 0.1924937316314852, - "angularVelocity": 2.586123310059887e-8, - "velocityX": 3.4711090418773667, - "velocityY": 1.351677922316834, - "timestamp": 1.1269374515486668 - }, - { - "x": 7.445544445617428, - "y": 7.124436352045788, - "heading": 0.20746316204367837, - "angularVelocity": 0.15939941005551134, - "velocityX": 2.929702336575951, - "velocityY": 1.1408462144931066, - "timestamp": 1.220848905844389 - }, - { - "x": 7.669715374136242, - "y": 7.211730123643104, - "heading": 0.22019672303892965, - "angularVelocity": 0.13559113838397205, - "velocityX": 2.38704565060736, - "velocityY": 0.9295327417934781, - "timestamp": 1.3147603601401112 - }, - { - "x": 7.842925396345479, - "y": 7.279179365267254, - "heading": 0.23039470264394565, - "angularVelocity": 0.10859143521410211, - "velocityX": 1.8443971878425902, - "velocityY": 0.7182216709343664, - "timestamp": 1.4086718144358334 - }, - { - "x": 7.96517479602962, - "y": 7.326784165564593, - "heading": 0.23795514608622043, - "angularVelocity": 0.08050608415100667, - "velocityX": 1.3017517469081525, - "velocityY": 0.506911544010745, - "timestamp": 1.5025832687315557 - }, - { - "x": 8.0364637201711, - "y": 7.354544571992242, - "heading": 0.24282665608297047, - "angularVelocity": 0.05187343794517228, - "velocityX": 0.7591078710909263, - "velocityY": 0.2956019224261278, - "timestamp": 1.596494723027278 + "x": 4.195152179176686, + "y": 5.858709351822529, + "heading": 0.13547549704031217, + "angularVelocity": 0.03117959203710152, + "velocityX": 0.5412280111334526, + "velocityY": 0.210760048168702, + "timestamp": 0.09379055025109917 + }, + { + "x": 4.296676325767576, + "y": 5.898243947995512, + "heading": 0.1413238484362874, + "angularVelocity": 0.06235544391538225, + "velocityX": 1.0824560290891385, + "velocityY": 0.42152003658300596, + "timestamp": 0.18758110050219834 + }, + { + "x": 4.448962547900073, + "y": 5.95754583130188, + "heading": 0.15009539524373325, + "angularVelocity": 0.0935227140043684, + "velocityX": 1.6236840675824051, + "velocityY": 0.6322799380918802, + "timestamp": 0.2813716507532975 + }, + { + "x": 4.6520108481753235, + "y": 6.036614990735129, + "heading": 0.16178892165721923, + "angularVelocity": 0.1246770211090534, + "velocityX": 2.1649121338092594, + "velocityY": 0.8430397222488009, + "timestamp": 0.3751622010043967 + }, + { + "x": 4.905821228102917, + "y": 6.135451411658796, + "heading": 0.17640285839515626, + "angularVelocity": 0.15581459644721318, + "velocityX": 2.706140216131426, + "velocityY": 1.0537993503509646, + "timestamp": 0.46895275125549585 + }, + { + "x": 5.210393679513956, + "y": 6.254055072364673, + "heading": 0.19393534293153245, + "angularVelocity": 0.18693231343069955, + "velocityX": 3.2473682113563522, + "velocityY": 1.2645587469989996, + "timestamp": 0.562743301506595 + }, + { + "x": 5.53637764515154, + "y": 6.380995104775572, + "heading": 0.1939353448909336, + "angularVelocity": 2.0891242641251563e-8, + "velocityX": 3.475658952472825, + "velocityY": 1.3534416001510998, + "timestamp": 0.6565338517576942 + }, + { + "x": 5.862361610799536, + "y": 6.507935137159803, + "heading": 0.19393534685032104, + "angularVelocity": 2.089109662304858e-8, + "velocityX": 3.47565895258384, + "velocityY": 1.353441599866765, + "timestamp": 0.7503244020087934 + }, + { + "x": 6.188345576447532, + "y": 6.634875169544034, + "heading": 0.19393534880970845, + "angularVelocity": 2.0891096217109697e-8, + "velocityX": 3.4756589525838413, + "velocityY": 1.3534415998667624, + "timestamp": 0.8441149522598925 + }, + { + "x": 6.5143295420955285, + "y": 6.761815201928266, + "heading": 0.19393535076909593, + "angularVelocity": 2.0891097152342323e-8, + "velocityX": 3.475658952583841, + "velocityY": 1.3534415998667637, + "timestamp": 0.9379055025109917 + }, + { + "x": 6.840313507740267, + "y": 6.888755234320864, + "heading": 0.1939353527284834, + "angularVelocity": 2.0891096624592066e-8, + "velocityX": 3.475658952549103, + "velocityY": 1.353441599955972, + "timestamp": 1.0316960527620909 + }, + { + "x": 7.1662970943707105, + "y": 7.015696232741894, + "heading": 0.19393535571019038, + "angularVelocity": 3.179112387470943e-8, + "velocityX": 3.475654911477867, + "velocityY": 1.353451899804194, + "timestamp": 1.12548660301319 + }, + { + "x": 7.441622589341096, + "y": 7.122909656814238, + "heading": 0.20815324472161514, + "angularVelocity": 0.15159191382671508, + "velocityX": 2.9355355548429496, + "velocityY": 1.143115418187742, + "timestamp": 1.2192771532642892 + }, + { + "x": 7.66618318620661, + "y": 7.210355021181115, + "heading": 0.2205253443109958, + "angularVelocity": 0.13191200559392907, + "velocityX": 2.3942774220250835, + "velocityY": 0.9323472794728952, + "timestamp": 1.3130677035153884 + }, + { + "x": 7.839980071520317, + "y": 7.278032678531449, + "heading": 0.23053454012619146, + "angularVelocity": 0.10671859572631527, + "velocityX": 1.8530319403011595, + "velocityY": 0.7215829011467113, + "timestamp": 1.4068582537664875 + }, + { + "x": 7.963013724050306, + "y": 7.325942779672703, + "heading": 0.23800124017348176, + "angularVelocity": 0.0796103661541618, + "velocityX": 1.311791563228909, + "velocityY": 0.5108201307379786, + "timestamp": 1.5006488040175867 + }, + { + "x": 8.03528439999784, + "y": 7.354085407438329, + "heading": 0.24283421360763968, + "angularVelocity": 0.05152942829761583, + "velocityX": 0.7705539177886093, + "velocityY": 0.3000582435040803, + "timestamp": 1.5944393542686859 }, { "x": 8.056792259216309, "y": 7.362460613250732, "heading": 0.244978130412332, - "angularVelocity": 0.022909605068905204, - "velocityX": 0.21646495837660984, - "velocityY": 0.0842926064542015, - "timestamp": 1.6904061773230001 - }, - { - "x": 8.02239522092899, - "y": 7.349066085390599, - "heading": 0.24423351408772678, - "angularVelocity": -0.00758829012549376, - "velocityX": -0.3505358361841942, - "velocityY": -0.13650192741958841, - "timestamp": 1.7885332009727142 - }, - { - "x": 7.932360084343089, - "y": 7.314005643971181, - "heading": 0.24049622236652166, - "angularVelocity": -0.03808626392813261, - "velocityX": -0.9175366095613122, - "velocityY": -0.35729649300863325, - "timestamp": 1.8866602246224282 - }, - { - "x": 7.786686853677988, - "y": 7.257279283570004, - "heading": 0.23376594617518753, - "angularVelocity": -0.06858738745974144, - "velocityX": -1.4845373399392365, - "velocityY": -0.578091113857428, - "timestamp": 1.9847872482721423 - }, - { - "x": 7.5853755359232595, - "y": 7.178886996735875, - "heading": 0.2240420388382404, - "angularVelocity": -0.09909510117884326, - "velocityX": -2.0515379990873206, - "velocityY": -0.7988858106403784, - "timestamp": 2.0829142719218563 - }, - { - "x": 7.328426142666821, - "y": 7.078828774757454, - "heading": 0.21132346369891478, - "angularVelocity": -0.1296133793350078, - "velocityX": -2.618538540144425, - "velocityY": -1.0196805961994813, - "timestamp": 2.1810412955715703 - }, - { - "x": 7.01583869900668, - "y": 6.957104611217254, - "heading": 0.19560873409309495, - "angularVelocity": -0.1601468078958253, - "velocityX": -3.1855388254309114, - "velocityY": -1.2404754471583816, - "timestamp": 2.2791683192212844 - }, - { - "x": 6.675228781735299, - "y": 6.824469292432169, - "heading": 0.19560873219026645, - "angularVelocity": -1.939148261699155e-8, - "velocityX": -3.471112284901898, - "velocityY": -1.3516696405524002, - "timestamp": 2.3772953428709984 - }, - { - "x": 6.334618864455473, - "y": 6.691833973668682, - "heading": 0.19560873028745154, - "angularVelocity": -1.9391344572203613e-8, - "velocityX": -3.4711122849879685, - "velocityY": -1.3516696403322965, - "timestamp": 2.4754223665207116 - }, - { - "x": 5.994008947175647, - "y": 6.559198654905194, - "heading": 0.19560872838463667, - "angularVelocity": -1.9391344282542406e-8, - "velocityX": -3.471112284987969, - "velocityY": -1.3516696403322948, - "timestamp": 2.5735493901704247 - }, - { - "x": 5.65339902989582, - "y": 6.426563336141707, - "heading": 0.19560872648182165, - "angularVelocity": -1.9391345115512158e-8, - "velocityX": -3.4711122849879685, - "velocityY": -1.3516696403322956, - "timestamp": 2.671676413820138 - }, - { - "x": 5.312789112621013, - "y": 6.293928017365348, - "heading": 0.195608724578999, - "angularVelocity": -1.939142318059707e-8, - "velocityX": -3.4711122849368206, - "velocityY": -1.3516696404634736, - "timestamp": 2.769803437469851 - }, - { - "x": 4.978960833752885, - "y": 6.1639320605180385, - "heading": 0.17759686591379087, - "angularVelocity": -0.18355655756466713, - "velocityX": -3.402001471682276, - "velocityY": -1.324772239208634, - "timestamp": 2.867930461119564 - }, - { - "x": 4.700770593105711, - "y": 6.055602072933751, - "heading": 0.16258358561165714, - "angularVelocity": -0.15299842738252176, - "velocityX": -2.8350013105486314, - "velocityY": -1.1039771059498866, - "timestamp": 2.9660574847692773 - }, - { - "x": 4.478218399750123, - "y": 5.968938070584376, - "heading": 0.1505712213556835, - "angularVelocity": -0.1224164741697909, - "velocityX": -2.2680010569772926, - "velocityY": -0.8831818099236476, - "timestamp": 3.0641845084189905 - }, - { - "x": 4.311304254190931, - "y": 5.9039400626948755, - "heading": 0.1415612155720731, - "angularVelocity": -0.09181982137533895, - "velocityX": -1.701000798261533, - "velocityY": -0.6623864198869991, - "timestamp": 3.1623115320687036 - }, - { - "x": 4.2000281561602835, - "y": 5.860608055094718, - "heading": 0.13555442251274583, - "angularVelocity": -0.06121446300837455, - "velocityX": -1.1340005422754125, - "velocityY": -0.4415909704429648, - "timestamp": 3.260438555718417 + "angularVelocity": 0.022858558766875252, + "velocityX": 0.2293179767139361, + "velocityY": 0.08929690453869228, + "timestamp": 1.688229904519785 + }, + { + "x": 8.023609935096944, + "y": 7.3495390353004035, + "heading": 0.24423184787745655, + "angularVelocity": -0.007595056263321516, + "velocityX": -0.33770268880325044, + "velocityY": -0.13150530389944523, + "timestamp": 1.7864888915505226 + }, + { + "x": 7.934712736835736, + "y": 7.314921652906921, + "heading": 0.24049321600439108, + "angularVelocity": -0.03804875244537132, + "velocityX": -0.9047233331786764, + "velocityY": -0.3523075439669781, + "timestamp": 1.8847478785812601 + }, + { + "x": 7.790100668647838, + "y": 7.258608460656236, + "heading": 0.2337619254421963, + "angularVelocity": -0.06850559694950989, + "velocityX": -1.4717439346556647, + "velocityY": -0.5731098391342927, + "timestamp": 1.9830068656119977 + }, + { + "x": 7.589773737515311, + "y": 7.180599451109558, + "heading": 0.22403732916629288, + "angularVelocity": -0.09896902634322269, + "velocityX": -2.038764465074942, + "velocityY": -0.7939122100075828, + "timestamp": 2.0812658526427352 + }, + { + "x": 7.3337319550141755, + "y": 7.080894615571415, + "heading": 0.21131838994573934, + "angularVelocity": -0.12944301182928752, + "velocityX": -2.6057848776829022, + "velocityY": -1.0147146693762836, + "timestamp": 2.1795248396734728 + }, + { + "x": 7.021975346220784, + "y": 6.959493947641481, + "heading": 0.19560361962226216, + "angularVelocity": -0.15993214257909347, + "velocityX": -3.1728050350841532, + "velocityY": -1.235517193882298, + "timestamp": 2.2777838267042103 + }, + { + "x": 6.680460557220924, + "y": 6.826506302650429, + "heading": 0.19560361772383378, + "angularVelocity": -1.9320659091620353e-8, + "velocityX": -3.4756595739484712, + "velocityY": -1.3534400161224025, + "timestamp": 2.376042813734947 + }, + { + "x": 6.3389457682116035, + "y": 6.69351865768357, + "heading": 0.19560361582542005, + "angularVelocity": -1.9320509691565887e-8, + "velocityX": -3.4756595740447467, + "velocityY": -1.3534400158761875, + "timestamp": 2.4743018007656845 + }, + { + "x": 5.997430979202282, + "y": 6.560531012716711, + "heading": 0.19560361392700626, + "angularVelocity": -1.9320510544397003e-8, + "velocityX": -3.4756595740447476, + "velocityY": -1.3534400158761857, + "timestamp": 2.572560787796422 + }, + { + "x": 5.655916190192961, + "y": 6.427543367749852, + "heading": 0.1956036120285926, + "angularVelocity": -1.932050909755801e-8, + "velocityX": -3.475659574044747, + "velocityY": -1.3534400158761868, + "timestamp": 2.6708197748271596 + }, + { + "x": 5.314401401188735, + "y": 6.29455572276993, + "heading": 0.19560361013017105, + "angularVelocity": -1.932058966623249e-8, + "velocityX": -3.4756595739929064, + "velocityY": -1.353440016009134, + "timestamp": 2.769078761857897 + }, + { + "x": 4.980112469171968, + "y": 6.164380421516108, + "heading": 0.177592992488041, + "angularVelocity": -0.18329740806808847, + "velocityX": -3.4021206824796124, + "velocityY": -1.3248182704458449, + "timestamp": 2.8673377488886347 + }, + { + "x": 4.701538350318157, + "y": 6.055900980257848, + "heading": 0.16258092336836558, + "angularVelocity": -0.15278062163392098, + "velocityX": -2.8351006586976912, + "velocityY": -1.10401546501111, + "timestamp": 2.9655967359193722 + }, + { + "x": 4.478679054186577, + "y": 5.969117414975404, + "heading": 0.1505695910339925, + "angularVelocity": -0.12224156484145013, + "velocityX": -2.2680805376292548, + "velocityY": -0.8832124969423502, + "timestamp": 3.0638557229501098 + }, + { + "x": 4.311534581449365, + "y": 5.904029734889891, + "heading": 0.14156038794424536, + "angularVelocity": -0.09168833673126392, + "velocityX": -1.701060409720345, + "velocityY": -0.6624094350285984, + "timestamp": 3.1621147099808473 + }, + { + "x": 4.200104931923326, + "y": 5.860637945826706, + "heading": 0.1355541433245536, + "angularVelocity": -0.06112666943953846, + "velocityX": -1.1340402836758565, + "velocityY": -0.4416063138286828, + "timestamp": 3.260373697011585 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": -0.030606008971789937, - "velocityX": -0.5670002807557282, - "velocityY": -0.2207954888983711, - "timestamp": 3.35856557936813 + "angularVelocity": -0.03056206326518059, + "velocityX": -0.567020151599215, + "velocityY": -0.22080316059111527, + "timestamp": 3.3586326840423224 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 1.2638912367197548e-25, - "velocityX": -6.105559884328861e-26, - "velocityY": -2.4380402560227299e-26, - "timestamp": 3.456692603017843 + "angularVelocity": 5.970204758228253e-26, + "velocityX": -1.332775736754173e-26, + "velocityY": -2.1157650772928713e-26, + "timestamp": 3.45689167107306 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/AmpLanePAEDF.5.traj b/src/main/deploy/choreo/AmpLanePAEDF.5.traj index 7b5c4b70..74101f62 100644 --- a/src/main/deploy/choreo/AmpLanePAEDF.5.traj +++ b/src/main/deploy/choreo/AmpLanePAEDF.5.traj @@ -4,469 +4,469 @@ "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 1.2638912367197548e-25, - "velocityX": -6.105559884328861e-26, - "velocityY": -2.4380402560227299e-26, + "angularVelocity": 5.970204758228253e-26, + "velocityX": -1.332775736754173e-26, + "velocityY": -2.1157650772928713e-26, "timestamp": 0 }, { - "x": 4.149229218776726, - "y": 5.815100726386144, - "heading": 0.12918022174703217, - "angularVelocity": -0.05380546988265665, - "velocityX": 0.07724016042252432, - "velocityY": -0.3805465783235216, - "timestamp": 0.06265021394417758 - }, - { - "x": 4.159717289478365, - "y": 5.7675956417955225, - "heading": 0.12259706469587767, - "angularVelocity": -0.10507796600695042, - "velocityX": 0.16740678189838681, - "velocityY": -0.7582589364012235, - "timestamp": 0.12530042788835516 - }, - { - "x": 4.176867312970838, - "y": 5.696690246498273, - "heading": 0.11300853276979594, - "angularVelocity": -0.15304867010709985, - "velocityX": 0.2737424569332353, - "velocityY": -1.1317662116912757, - "timestamp": 0.18795064183253274 - }, - { - "x": 4.201974040585745, - "y": 5.602790592825652, - "heading": 0.10069220099175857, - "angularVelocity": -0.19658882233046115, - "velocityX": 0.40074448328743983, - "velocityY": -1.498792226891454, - "timestamp": 0.2506008557767103 - }, - { - "x": 4.2367303910144685, - "y": 5.486551757326023, - "heading": 0.08603276386217494, - "angularVelocity": -0.2339886204162901, - "velocityX": 0.5547682639949261, - "velocityY": -1.8553621477366333, - "timestamp": 0.3132510697208879 - }, - { - "x": 4.283393708275681, - "y": 5.349087774330418, - "heading": 0.06958159023755035, - "angularVelocity": -0.2625876687234105, - "velocityX": 0.7448229514872903, - "velocityY": -2.1941502565033004, - "timestamp": 0.3759012836650655 - }, - { - "x": 4.3449830971903305, - "y": 5.19239819137041, - "heading": 0.05215018239550137, - "angularVelocity": -0.2782338119001568, - "velocityX": 0.9830674955001105, - "velocityY": -2.5010223125434243, - "timestamp": 0.43855149760924306 - }, - { - "x": 4.425324635342363, - "y": 5.020190178284826, - "heading": 0.03492497946542863, - "angularVelocity": -0.2749424438585412, - "velocityX": 1.2823825026937377, - "velocityY": -2.7487218677181704, - "timestamp": 0.5012017115534206 - }, - { - "x": 4.528280141254807, - "y": 4.838988810812342, - "heading": 0.019478906624101934, - "angularVelocity": -0.24654461443801812, - "velocityX": 1.6433384569792095, - "velocityY": -2.8922705297373237, - "timestamp": 0.5638519254975982 - }, - { - "x": 4.6555610459377, - "y": 4.65789803685685, - "heading": 0.007446897298019998, - "angularVelocity": -0.1920505704386356, - "velocityX": 2.0316116525364616, - "velocityY": -2.8905052761167873, - "timestamp": 0.6265021394417758 + "x": 4.149233388012969, + "y": 5.815110549500247, + "heading": 0.12920898299918354, + "angularVelocity": -0.053321361684181494, + "velocityX": 0.07727043393529022, + "velocityY": -0.38021129652575064, + "timestamp": 0.06267962485953404 + }, + { + "x": 4.15973054187138, + "y": 5.767625542791529, + "heading": 0.12267986707327648, + "angularVelocity": -0.10416648058342459, + "velocityX": 0.16747314429426893, + "velocityY": -0.7575828160288474, + "timestamp": 0.1253592497190681 + }, + { + "x": 4.176895053438104, + "y": 5.696750993270707, + "heading": 0.11316614819920229, + "angularVelocity": -0.1517832771876739, + "velocityX": 0.27384515470838755, + "velocityY": -1.13074303937925, + "timestamp": 0.18803887457860213 + }, + { + "x": 4.20202157406695, + "y": 5.602893467398182, + "heading": 0.10093938344413095, + "angularVelocity": -0.19506761220207594, + "velocityX": 0.4008722241901565, + "velocityY": -1.4974168413237998, + "timestamp": 0.2507184994381362 + }, + { + "x": 4.236801648856746, + "y": 5.486708244559792, + "heading": 0.08637594484329676, + "angularVelocity": -0.23234725213290444, + "velocityX": 0.5548864542144063, + "velocityY": -1.8536362190865294, + "timestamp": 0.3133981242976702 + }, + { + "x": 4.283488644505547, + "y": 5.349308215099438, + "heading": 0.07001524095922855, + "angularVelocity": -0.2610210881244517, + "velocityX": 0.7448512296208976, + "velocityY": -2.1921003798007317, + "timestamp": 0.37607774915720427 + }, + { + "x": 4.3450929137179735, + "y": 5.192687240127482, + "heading": 0.052651049006290945, + "angularVelocity": -0.2770308851058188, + "velocityX": 0.9828436170523044, + "velocityY": -2.4987541856375906, + "timestamp": 0.4387573740167383 + }, + { + "x": 4.4254260370024205, + "y": 5.02053381378003, + "heading": 0.035443983400084915, + "angularVelocity": -0.2745240681444267, + "velocityX": 1.2816465233235492, + "velocityY": -2.7465612108120876, + "timestamp": 0.5014369988762724 + }, + { + "x": 4.52834083069929, + "y": 4.839329489599335, + "heading": 0.019934884277187374, + "angularVelocity": -0.24743445988474677, + "velocityX": 1.6419178309937554, + "velocityY": -2.8909605727024488, + "timestamp": 0.5641166237358064 + }, + { + "x": 4.655573182869098, + "y": 4.658127319631667, + "heading": 0.0077322780217831035, + "angularVelocity": -0.19468218392739808, + "velocityX": 2.029883753371783, + "velocityY": -2.890926204069373, + "timestamp": 0.6267962485953404 }, { "x": 4.805556774139404, "y": 4.485479354858398, - "heading": -3.9652014929192097e-25, - "angularVelocity": -0.1188646746626479, - "velocityX": 2.3941774298704206, - "velocityY": -2.7520844885235216, - "timestamp": 0.6891523533859534 - }, - { - "x": 5.026556371976755, - "y": 4.290478604211026, - "heading": -0.0011711585949671858, - "angularVelocity": -0.014781576411141743, - "velocityX": 2.7893083450033753, - "velocityY": -2.4611683748987776, - "timestamp": 0.7683833205299191 - }, - { - "x": 5.271648004487175, - "y": 4.126055401914557, - "heading": -0.0011711658442509346, - "angularVelocity": -9.149558575332566e-8, - "velocityX": 3.0933818094770933, - "velocityY": -2.075239117019822, - "timestamp": 0.8476142876738848 - }, - { - "x": 5.536248278847656, - "y": 3.995320554415454, - "heading": -0.0011711670074400294, - "angularVelocity": -1.4680990736292476e-8, - "velocityX": 3.3396067711718374, - "velocityY": -1.6500473515810268, - "timestamp": 0.9268452548178505 - }, - { - "x": 5.8157535418727955, - "y": 3.900548632610079, - "heading": -0.001171167882288204, - "angularVelocity": -1.1041745495501586e-8, - "velocityX": 3.5277275174146094, - "velocityY": -1.19614748149131, - "timestamp": 1.0060762219618162 - }, - { - "x": 6.105300821187418, - "y": 3.843388468655785, - "heading": -0.001171168612334034, - "angularVelocity": -9.214147655131007e-9, - "velocityX": 3.654471095733389, - "velocityY": -0.7214371604278409, - "timestamp": 1.085307189105782 - }, - { - "x": 6.399852439542981, - "y": 3.8248343191469996, - "heading": -0.0011711692753000216, - "angularVelocity": -8.367510982006514e-9, - "velocityX": 3.717632498671272, - "velocityY": -0.23417800107213171, - "timestamp": 1.1645381562497477 - }, - { - "x": 6.694725372224158, - "y": 3.837278843369643, - "heading": -0.0011711699217371897, - "angularVelocity": -8.158895331165977e-9, - "velocityX": 3.721687912068311, - "velocityY": 0.15706641823558024, - "timestamp": 1.2437691233937134 - }, - { - "x": 6.989598240261493, - "y": 3.849724899233505, - "heading": -0.001171170568174097, - "angularVelocity": -8.158892044748454e-9, - "velocityX": 3.721687096177193, - "velocityY": 0.15708574958131358, - "timestamp": 1.323000090537679 - }, - { - "x": 7.2831382115989305, - "y": 3.8803709956962793, - "heading": -0.0011711735014437953, - "angularVelocity": -3.702175808154048e-8, - "velocityX": 3.7048641701427742, - "velocityY": 0.38679442606183717, - "timestamp": 1.4022310576816448 - }, - { - "x": 7.537795284833571, - "y": 3.9128988141490195, - "heading": -0.0022060584163901163, - "angularVelocity": -0.013061621639249952, - "velocityX": 3.2141103714147587, - "velocityY": 0.4105442559300826, - "timestamp": 1.4814620248256105 - }, - { - "x": 7.7535280379166585, - "y": 3.9448039763833895, - "heading": -0.0030479668047439287, - "angularVelocity": -0.010626001659477862, - "velocityX": 2.7228337714355204, - "velocityY": 0.40268550775604034, - "timestamp": 1.5606929919695762 - }, - { - "x": 7.9303456416380165, - "y": 3.9756628799518183, - "heading": -0.0034891865807647085, - "angularVelocity": -0.005568779379141843, - "velocityX": 2.2316729189998985, - "velocityY": 0.38948033427835, - "timestamp": 1.639923959113542 - }, - { - "x": 8.06825324660345, - "y": 4.0053006852761595, - "heading": -0.003443969014778176, - "angularVelocity": 0.000570705717934386, - "velocityX": 1.7405770740479534, - "velocityY": 0.374068453190637, - "timestamp": 1.7191549262575077 - }, - { - "x": 8.167254001972681, - "y": 4.03362192898922, - "heading": -0.002865492528698777, - "angularVelocity": 0.007301141295275209, - "velocityX": 1.2495209756728547, - "velocityY": 0.3574516976625985, - "timestamp": 1.7983858934014734 - }, - { - "x": 8.22735001387898, - "y": 4.060566470670497, - "heading": -0.0017242628169884097, - "angularVelocity": 0.014403834167980214, - "velocityX": 0.7584914594958928, - "velocityY": 0.34007589017963685, - "timestamp": 1.877616860545439 + "heading": -5.922112096142328e-27, + "angularVelocity": -0.1233619065064789, + "velocityX": 2.392860384956347, + "velocityY": -2.754451149957796, + "timestamp": 0.6894758734548745 + }, + { + "x": 5.0260464642582185, + "y": 4.290440402432839, + "heading": -0.0017206330074737138, + "angularVelocity": -0.02175742700503041, + "velocityX": 2.7880950308894734, + "velocityY": -2.4662701180928828, + "timestamp": 0.7685584325771408 + }, + { + "x": 5.270702243030263, + "y": 4.125667987076678, + "heading": -0.001720643347085146, + "angularVelocity": -1.3074452252065364e-7, + "velocityX": 3.0936755396824163, + "velocityY": -2.0835493588594303, + "timestamp": 0.8476409916994072 + }, + { + "x": 5.534838657282035, + "y": 3.9943730439929475, + "heading": -0.0017206445243311077, + "angularVelocity": -1.4886290666410382e-8, + "velocityX": 3.3400084314848875, + "velocityY": -1.660226281761329, + "timestamp": 0.9267235508216736 + }, + { + "x": 5.813901235574451, + "y": 3.898819454109309, + "heading": -0.001720645408427364, + "angularVelocity": -1.1179408784311949e-8, + "velocityX": 3.528749972050966, + "velocityY": -1.2082764005639604, + "timestamp": 1.00580610994394 + }, + { + "x": 6.10307814292353, + "y": 3.8406547782709533, + "heading": -0.0017206461449007905, + "angularVelocity": -9.31271616103322e-9, + "velocityX": 3.656645795971196, + "velocityY": -0.7354930908144911, + "timestamp": 1.0848886690662063 + }, + { + "x": 6.397383154789235, + "y": 3.8208817077449715, + "heading": -0.0017206468122835479, + "angularVelocity": -8.439063742592553e-9, + "velocityX": 3.721490745016124, + "velocityY": -0.25003073680773497, + "timestamp": 1.1639712281884727 + }, + { + "x": 6.692083031584466, + "y": 3.833467304039097, + "heading": -0.0017206474619802302, + "angularVelocity": -8.215423090919723e-9, + "velocityX": 3.7264838172422694, + "velocityY": 0.1591450306339672, + "timestamp": 1.243053787310739 + }, + { + "x": 6.9867828265259435, + "y": 3.8460548168458266, + "heading": -0.0017206481116765608, + "angularVelocity": -8.21541864124069e-9, + "velocityX": 3.726482782200477, + "velocityY": 0.15916926496104583, + "timestamp": 1.3221363464330054 + }, + { + "x": 7.280008906046239, + "y": 3.8780682629545473, + "heading": -0.0017206520775067291, + "angularVelocity": -5.014797462981314e-8, + "velocityX": 3.7078476313209716, + "velocityY": 0.40481044700672053, + "timestamp": 1.4012189055552717 + }, + { + "x": 7.534517790856317, + "y": 3.9111592304770704, + "heading": -0.0026190122393155452, + "angularVelocity": -0.011359776059091569, + "velocityX": 3.218268195097111, + "velocityY": 0.41843571945316527, + "timestamp": 1.480301464677538 + }, + { + "x": 7.750301298941706, + "y": 3.943488528624649, + "heading": -0.0033468758613748313, + "angularVelocity": -0.009203845072008415, + "velocityX": 2.728585297192714, + "velocityY": 0.40880440019139475, + "timestamp": 1.5593840237998045 + }, + { + "x": 7.927368234125033, + "y": 3.974697332887919, + "heading": -0.003695626110523714, + "angularVelocity": -0.004409951486391499, + "velocityX": 2.239013723741158, + "velocityY": 0.39463574029034193, + "timestamp": 1.6384665829220708 + }, + { + "x": 8.065723377902886, + "y": 4.004632819289759, + "heading": -0.0035763939938957814, + "angularVelocity": 0.0015076916826071924, + "velocityX": 1.7495026123768658, + "velocityY": 0.37853461918901893, + "timestamp": 1.7175491420443372 + }, + { + "x": 8.165369644244953, + "y": 4.03321033626126, + "heading": -0.0029399507835581986, + "angularVelocity": 0.008047832763651503, + "velocityX": 1.2600283481975725, + "velocityY": 0.36136307788571737, + "timestamp": 1.7966317011666035 + }, + { + "x": 8.226308982581841, + "y": 4.060376108268979, + "heading": -0.0017550259023952847, + "angularVelocity": 0.014983390703517166, + "velocityX": 0.7705787345939596, + "velocityY": 0.34351154425489977, + "timestamp": 1.87571426028887 }, { "x": 8.248542785644531, "y": 4.086092948913574, - "heading": 1.4182707321197947e-24, - "angularVelocity": 0.021762486047347675, - "velocityX": 0.26748091724090983, - "velocityY": 0.32217804683230217, - "timestamp": 1.9568478276894048 - }, - { - "x": 8.23152301234386, - "y": 4.109892958457591, - "heading": 0.0022863134325125296, - "angularVelocity": 0.02921520350070526, - "velocityX": -0.21748380315840005, - "velocityY": 0.30412370948765427, - "timestamp": 2.0351054866667706 - }, - { - "x": 8.176551036095818, - "y": 4.132280075994518, - "heading": 0.005155847292928311, - "angularVelocity": 0.03666777025933954, - "velocityX": -0.7024485138757267, - "velocityY": 0.2860693487317698, - "timestamp": 2.1133631456441364 - }, - { - "x": 8.0836268579111, - "y": 4.153254299063441, - "heading": 0.008608587112944467, - "angularVelocity": 0.044120152137630285, - "velocityX": -1.1874132116780383, - "velocityY": 0.2680149565295516, - "timestamp": 2.191620804621502 - }, - { - "x": 7.952750479206832, - "y": 4.172815624183264, - "heading": 0.012644515987393022, - "angularVelocity": 0.05157231799657927, - "velocityX": -1.672377891371887, - "velocityY": 0.24996051984483794, - "timestamp": 2.269878463598868 - }, - { - "x": 7.783921902112923, - "y": 4.190964046074257, - "heading": 0.01726361439792771, - "angularVelocity": 0.05902423444420523, - "velocityX": -2.157342543849138, - "velocityY": 0.2319060156941693, - "timestamp": 2.348136122576234 - }, - { - "x": 7.577141130185825, - "y": 4.207699555855839, - "heading": 0.022465858708007803, - "angularVelocity": 0.06647584885697558, - "velocityX": -2.642307150880985, - "velocityY": 0.2138513980647186, - "timestamp": 2.4263937815535996 - }, - { - "x": 7.3324081705466435, - "y": 4.2230221357012825, - "heading": 0.028251214753927917, - "angularVelocity": 0.0739270267156012, - "velocityX": -3.127271666917177, - "velocityY": 0.19579655263996953, - "timestamp": 2.5046514405309654 - }, - { - "x": 7.049723044551423, - "y": 4.236931732641069, - "heading": 0.03461960291638085, - "angularVelocity": 0.08137718717467465, - "velocityX": -3.612235910059377, - "velocityY": 0.17774103035473854, - "timestamp": 2.5829090995083313 - }, - { - "x": 6.758414971904043, - "y": 4.226087741393719, - "heading": 0.03461960351163466, - "angularVelocity": 7.606332865983503e-9, - "velocityX": -3.722422526485686, - "velocityY": -0.13856779501270336, - "timestamp": 2.661166758485697 - }, - { - "x": 6.467106926605879, - "y": 4.215243015473276, - "heading": 0.03461960410686983, - "angularVelocity": 7.606094882154123e-9, - "velocityX": -3.7224221770091335, - "velocityY": -0.13857718288735063, - "timestamp": 2.739424417463063 - }, - { - "x": 6.175798881305754, - "y": 4.204398289605525, - "heading": 0.03461960470210498, - "angularVelocity": 7.606094432841078e-9, - "velocityX": -3.7224221770341983, - "velocityY": -0.13857718221405538, - "timestamp": 2.8176820764404287 - }, - { - "x": 5.884490683895451, - "y": 4.1935576504515195, - "heading": 0.03461960529734107, - "angularVelocity": 7.606106615779635e-9, - "velocityX": -3.7224241207439825, - "velocityY": -0.13852496095162967, - "timestamp": 2.8959397354177945 - }, - { - "x": 5.594009037113431, - "y": 4.2180198226312315, - "heading": 0.034619605911984125, - "angularVelocity": 7.854094508972923e-9, - "velocityX": -3.7118622071998786, - "velocityY": 0.31258502361778023, - "timestamp": 2.9741973943951603 - }, - { - "x": 5.309172831526297, - "y": 4.280038516789027, - "heading": 0.03461959995315513, - "angularVelocity": -7.614371636753198e-8, - "velocityX": -3.6397230547046773, - "velocityY": 0.7924936034150152, - "timestamp": 3.052455053372526 - }, - { - "x": 5.03935943827081, - "y": 4.37675512978584, - "heading": 0.01971114929844739, - "angularVelocity": -0.1905046847749395, - "velocityX": -3.447757021884906, - "velocityY": 1.235874089011348, - "timestamp": 3.130712712349892 + "heading": 2.0912283688050628e-26, + "angularVelocity": 0.022192325613564278, + "velocityX": 0.2811467320918988, + "velocityY": 0.3251897881154181, + "timestamp": 1.9547968194111363 + }, + { + "x": 8.232549205371914, + "y": 4.110135442061636, + "heading": 0.002315542172399013, + "angularVelocity": 0.029537040889687716, + "velocityX": -0.2040140059273348, + "velocityY": 0.3066858862123803, + "timestamp": 2.033191340414337 + }, + { + "x": 8.1785216821941, + "y": 4.132727328793288, + "heading": 0.00520685837502888, + "angularVelocity": 0.03688161067419279, + "velocityX": -0.6891747342344035, + "velocityY": 0.28818196020011727, + "timestamp": 2.111585861417538 + }, + { + "x": 8.086460217126783, + "y": 4.153868606569999, + "heading": 0.008673934737020163, + "angularVelocity": 0.044226003521977544, + "velocityX": -1.1743354495852854, + "velocityY": 0.26967800180637214, + "timestamp": 2.189980382420739 + }, + { + "x": 7.956364811594128, + "y": 4.173559271802363, + "heading": 0.012716755269955655, + "angularVelocity": 0.05157019242161638, + "velocityX": -1.6594961467695446, + "velocityY": 0.25117399762619275, + "timestamp": 2.2683749034239398 + }, + { + "x": 7.788235467736609, + "y": 4.191799319049532, + "heading": 0.01733530192089736, + "angularVelocity": 0.0589141510380956, + "velocityX": -2.14465681664992, + "velocityY": 0.23266992404257889, + "timestamp": 2.3467694244271406 + }, + { + "x": 7.582072189128265, + "y": 4.208588739164989, + "heading": 0.022529553625480057, + "angularVelocity": 0.06625784095766901, + "velocityX": -2.6298174409398722, + "velocityY": 0.21416573378605752, + "timestamp": 2.4251639454303415 + }, + { + "x": 7.337874982925346, + "y": 4.223927513793021, + "heading": 0.02829948161506035, + "angularVelocity": 0.07360116390461449, + "velocityX": -3.1149779739447685, + "velocityY": 0.1956613093841805, + "timestamp": 2.5035584664335424 + }, + { + "x": 7.055643870589831, + "y": 4.237815588350764, + "heading": 0.034645023037832345, + "angularVelocity": 0.08094368511433264, + "velocityX": -3.600138233180768, + "velocityY": 0.17715618872364539, + "timestamp": 2.5819529874367433 + }, + { + "x": 6.763454078794012, + "y": 4.226672459763479, + "heading": 0.03464502362353886, + "angularVelocity": 7.47126845639964e-9, + "velocityX": -3.7271710836002327, + "velocityY": -0.1421416757789859, + "timestamp": 2.660347508439944 + }, + { + "x": 6.471264316590495, + "y": 4.215528555246823, + "heading": 0.03464502420922547, + "angularVelocity": 7.471014571114214e-9, + "velocityX": -3.7271707061210075, + "velocityY": -0.14215157352898217, + "timestamp": 2.738742029443145 + }, + { + "x": 6.1790745543846715, + "y": 4.2043846507906375, + "heading": 0.034645024794912035, + "angularVelocity": 7.471014019515784e-9, + "velocityX": -3.727170706150427, + "velocityY": -0.1421515727576172, + "timestamp": 2.817136550446346 + }, + { + "x": 5.8868846155421455, + "y": 4.193245378659894, + "heading": 0.034645025380599624, + "angularVelocity": 7.471027140146409e-9, + "velocityX": -3.727172959327064, + "velocityY": -0.1420924828444054, + "timestamp": 2.895531071449547 + }, + { + "x": 5.595514516394948, + "y": 4.2177914315394665, + "heading": 0.034645025985590286, + "angularVelocity": 7.717256902963699e-9, + "velocityX": -3.716715089506096, + "velocityY": 0.3131092908721317, + "timestamp": 2.9739255924527477 + }, + { + "x": 5.309801234566853, + "y": 4.279976327218559, + "heading": 0.03464501988589696, + "angularVelocity": -7.78076483461541e-8, + "velocityX": -3.6445567645783616, + "velocityY": 0.7932301248011192, + "timestamp": 3.0523201134559486 + }, + { + "x": 5.039631118107501, + "y": 4.376720178141923, + "heading": 0.01972472706533464, + "angularVelocity": -0.19032315817011253, + "velocityX": -3.446288248235145, + "velocityY": 1.2340639331084704, + "timestamp": 3.1307146344591494 }, { "x": 4.805556774139404, "y": 4.485479354858398, - "heading": -1.4078898091571754e-24, - "angularVelocity": -0.25187501844577836, - "velocityX": -2.9876010500011123, - "velocityY": 1.3893109823794343, - "timestamp": 3.2089703713272577 - }, - { - "x": 4.62745788226169, - "y": 4.587679677136332, - "heading": -0.019778853338814184, - "angularVelocity": -0.28487106556518027, - "velocityX": -2.5651244910957125, - "velocityY": 1.47197181806725, - "timestamp": 3.2784012673699046 - }, - { - "x": 4.478940134439646, - "y": 4.685562428002014, - "heading": -0.039416750418752866, - "angularVelocity": -0.2828408993580651, - "velocityX": -2.139072895311886, - "velocityY": 1.4097866575934404, - "timestamp": 3.3478321634125514 - }, - { - "x": 4.358424662792702, - "y": 4.772986561558442, - "heading": -0.05736759927872339, - "angularVelocity": -0.2585426644781364, - "velocityX": -1.7357614335398783, - "velocityY": 1.2591531802027822, - "timestamp": 3.4172630594551983 - }, - { - "x": 4.264251963016928, - "y": 4.846292896298889, - "heading": -0.07266630938107123, - "angularVelocity": -0.22034441400483662, - "velocityX": -1.3563514968599637, - "velocityY": 1.0558172070171734, - "timestamp": 3.486693955497845 - }, - { - "x": 4.195046678979309, - "y": 4.903178017800908, - "heading": -0.08468297645986948, - "angularVelocity": -0.17307377210596744, - "velocityX": -0.9967505531703128, - "velocityY": 0.8193055936809087, - "timestamp": 3.556124851540492 - }, - { - "x": 4.149718350868284, - "y": 4.942098244876491, - "heading": -0.09298341563380762, - "angularVelocity": -0.11954964788067375, - "velocityX": -0.6528552948990165, - "velocityY": 0.5605606335784191, - "timestamp": 3.625555747583139 + "heading": -2.403769428999081e-26, + "angularVelocity": -0.2516084901460064, + "velocityX": -2.9858508091214753, + "velocityY": 1.3873313507717573, + "timestamp": 3.2091091554623503 + }, + { + "x": 4.627436042049824, + "y": 4.587623100442906, + "heading": -0.019769634491549008, + "angularVelocity": -0.2845427298125247, + "velocityX": -2.5636771062530377, + "velocityY": 1.4701465631201296, + "timestamp": 3.2785877694637495 + }, + { + "x": 4.478903462198178, + "y": 4.685483985234687, + "heading": -0.03940302192904774, + "angularVelocity": -0.28258173712422013, + "velocityX": -2.137817254798061, + "velocityY": 1.4085036985598087, + "timestamp": 3.3480663834651487 + }, + { + "x": 4.358385068299864, + "y": 4.77291258513252, + "heading": -0.057354073333619894, + "angularVelocity": -0.2583680124104157, + "velocityX": -1.734611371146319, + "velocityY": 1.2583526766390536, + "timestamp": 3.417544997466548 + }, + { + "x": 4.264219265878193, + "y": 4.846237576154262, + "heading": -0.07265588839993739, + "angularVelocity": -0.22023777080540505, + "velocityX": -1.355320680688504, + "velocityY": 1.0553605893788425, + "timestamp": 3.487023611467947 + }, + { + "x": 4.195026094794195, + "y": 4.903145738441645, + "heading": -0.08467676120352872, + "angularVelocity": -0.17301543757549837, + "velocityX": -0.9958916434718147, + "velocityY": 0.8190745181853494, + "timestamp": 3.5565022254693464 + }, + { + "x": 4.149710172245756, + "y": 4.942086169590921, + "heading": -0.09298105070953734, + "angularVelocity": -0.11952295861632048, + "velocityX": -0.6522283611979665, + "velocityY": 0.5604664357364799, + "timestamp": 3.6259808394707456 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": -0.06149221603257356, - "velocityX": -0.32142062647019676, - "velocityY": 0.2860902346288004, - "timestamp": 3.6949866436257857 + "angularVelocity": -0.061484021298608874, + "velocityX": -0.3210821603240091, + "velocityY": 0.2860675462596191, + "timestamp": 3.6954594534721448 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": -9.169895235461756e-26, - "velocityX": -3.1744204454242983e-26, - "velocityY": -4.8447793914370994e-26, - "timestamp": 3.7644175396684325 + "angularVelocity": -1.5452872782226126e-27, + "velocityX": -2.8121806375160504e-27, + "velocityY": 1.64906391853344e-27, + "timestamp": 3.764938067473544 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/AmpLanePAEDF.traj b/src/main/deploy/choreo/AmpLanePAEDF.traj index 9be9694f..7b9ccca6 100644 --- a/src/main/deploy/choreo/AmpLanePAEDF.traj +++ b/src/main/deploy/choreo/AmpLanePAEDF.traj @@ -3,1559 +3,1559 @@ { "x": 0.43297290802001953, "y": 6.9807281494140625, - "heading": -4.820407208942378e-26, - "angularVelocity": -1.0568380673400748e-29, - "velocityX": 2.0900789241101526e-25, - "velocityY": 6.12105194243679e-25, + "heading": -2.8662975018215305e-26, + "angularVelocity": -1.4927020760415103e-39, + "velocityX": 1.0727948153345131e-25, + "velocityY": 3.1703902069351426e-25, "timestamp": 0 }, { - "x": 0.4535853919747023, - "y": 6.976442037279776, - "heading": -9.657051327438745e-26, - "angularVelocity": -2.7866726754125085e-27, - "velocityX": 0.3537634154941742, - "velocityY": -0.07356074460262349, - "timestamp": 0.05826629620784589 + "x": 0.45358539196480147, + "y": 6.976442037232183, + "heading": -5.74233401454689e-26, + "angularVelocity": -1.6703718504854773e-27, + "velocityX": 0.3535319273172983, + "velocityY": -0.07351261033930959, + "timestamp": 0.05830444820414204 }, { - "x": 0.49483104636031155, - "y": 6.967970532726724, - "heading": -1.449381860201486e-25, - "angularVelocity": -2.8078094367592775e-27, - "velocityX": 0.7078818643024581, - "velocityY": -0.1453928789781493, - "timestamp": 0.11653259241569178 + "x": 0.49483104633484415, + "y": 6.967970532602043, + "heading": -8.618370527272265e-26, + "angularVelocity": -1.670371850488467e-27, + "velocityX": 0.7074186557023707, + "velocityY": -0.1452977412714415, + "timestamp": 0.11660889640828408 }, { - "x": 0.5567354919847137, - "y": 6.955441991018911, - "heading": -1.9330585876590946e-25, - "angularVelocity": -2.807809436759289e-27, - "velocityX": 1.0624400322886223, - "velocityY": -0.21502210580061862, - "timestamp": 0.17479888862353765 + "x": 0.5567354919435141, + "y": 6.955441990812605, + "heading": -1.1494407039997645e-25, + "angularVelocity": -1.6703718504884638e-27, + "velocityX": 1.0617448156257865, + "velocityY": -0.21488140571319628, + "timestamp": 0.1749133446124261 }, { - "x": 0.6393312280567922, - "y": 6.939025561506136, - "heading": -2.416735315116705e-25, - "angularVelocity": -2.8078094367592704e-27, - "velocityX": 1.417555970563931, - "velocityY": -0.2817482932880241, - "timestamp": 0.23306518483138355 + "x": 0.6393312280070567, + "y": 6.939025561249793, + "heading": -1.4370443552723021e-25, + "angularVelocity": -1.6703718504884706e-27, + "velocityX": 1.4166283809830285, + "velocityY": -0.28156392982801115, + "timestamp": 0.23321779281656815 }, { - "x": 0.7426606798556364, - "y": 6.918954250670703, - "heading": -2.9004008719315308e-25, - "angularVelocity": -2.7886377311391825e-27, - "velocityX": 1.7734000361075017, - "velocityY": -0.3444754882622709, - "timestamp": 0.29133148103922946 + "x": 0.742660679814762, + "y": 6.9189542504517005, + "heading": -1.7246480065448678e-25, + "angularVelocity": -1.6703718505364274e-27, + "velocityX": 1.7722395973274057, + "velocityY": -0.3442500772465394, + "timestamp": 0.29152224102071017 }, { "x": 0.8667811155319214, "y": 6.895569324493408, - "heading": -3.4495163504680925e-25, - "angularVelocity": -1.1511759285858433e-25, - "velocityX": 2.130226970897995, - "velocityY": -0.40134567836399193, - "timestamp": 0.34959777724707536 - }, - { - "x": 1.01097419129112, - "y": 6.869549473556677, - "heading": 0.010039096018647366, - "angularVelocity": 0.17297490397522428, - "velocityX": 2.4844650740475953, - "velocityY": -0.44832534810612334, - "timestamp": 0.4076356531928559 - }, - { - "x": 1.175726572408581, - "y": 6.840802777300111, - "heading": 0.03011443547557583, - "angularVelocity": 0.34590065762715055, - "velocityX": 2.8387045258405714, - "velocityY": -0.4953092405280373, - "timestamp": 0.4656735291386364 - }, - { - "x": 1.3610383679335918, - "y": 6.809328854372862, - "heading": 0.06022199220724938, - "angularVelocity": 0.5187570399681801, - "velocityX": 3.1929458565666793, - "velocityY": -0.5422997036737339, - "timestamp": 0.5237114050844169 - }, - { - "x": 1.5460799084730097, - "y": 6.784713461455284, - "heading": 0.15060245758508448, - "angularVelocity": 1.5572669382709545, - "velocityX": 3.1882893287184593, - "velocityY": -0.4241263574251743, - "timestamp": 0.5817492810301974 - }, - { - "x": 1.7105613111663502, - "y": 6.762828401317669, - "heading": 0.23096913740746994, - "angularVelocity": 1.384728136802675, - "velocityX": 2.83403553305432, - "velocityY": -0.37708237562070457, - "timestamp": 0.6397871569759779 - }, - { - "x": 1.8544824093339345, - "y": 6.743675274341158, - "heading": 0.3013204880217799, - "angularVelocity": 1.2121627380029003, - "velocityX": 2.4797788654780635, - "velocityY": -0.33001081904520096, - "timestamp": 0.6978250329217583 - }, - { - "x": 1.977843185069378, - "y": 6.727255600600169, - "heading": 0.36164875379550304, - "angularVelocity": 1.0394637086664282, - "velocityX": 2.1255218893725236, - "velocityY": -0.2829130713937313, - "timestamp": 0.7558629088675388 - }, - { - "x": 2.0806437247378744, - "y": 6.713570715573333, - "heading": 0.41194198603949084, - "angularVelocity": 0.8665588018929588, - "velocityX": 1.7712664013502706, - "velocityY": -0.2357923132752318, - "timestamp": 0.8139007848133193 - }, - { - "x": 2.162884155033722, - "y": 6.702621710001363, - "heading": 0.45218713038285063, - "angularVelocity": 0.6934289666451124, - "velocityX": 1.4170130962869354, - "velocityY": -0.18865276155520178, - "timestamp": 0.8719386607590998 - }, - { - "x": 2.2245645766627997, - "y": 6.694409403888087, - "heading": 0.4823732249131576, - "angularVelocity": 0.5201102562489893, - "velocityX": 1.0627615263987253, - "velocityY": -0.1414990810647272, - "timestamp": 0.9299765367048802 - }, - { - "x": 2.265685008078604, - "y": 6.688934342765278, - "heading": 0.5024941236633388, - "angularVelocity": 0.34668565005684077, - "velocityX": 0.7085102744666172, - "velocityY": -0.09433600099222642, - "timestamp": 0.9880144126506607 + "heading": -2.0459528719631722e-25, + "angularVelocity": -5.947250390698025e-26, + "velocityX": 2.1288330400208064, + "velocityY": -0.4010830507547883, + "timestamp": 0.3498266892248522 + }, + { + "x": 1.010974192105577, + "y": 6.8695494740311895, + "heading": 0.010039070916729844, + "angularVelocity": 0.17286128420602442, + "velocityX": 2.482839358032844, + "velocityY": -0.4480319745776911, + "timestamp": 0.4079025675965806 + }, + { + "x": 1.1757265748164618, + "y": 6.8408027784970535, + "heading": 0.030114360354405487, + "angularVelocity": 0.3456734534289609, + "velocityX": 2.836847023756549, + "velocityY": -0.49498511843652804, + "timestamp": 0.46597844596830895 + }, + { + "x": 1.361038372709757, + "y": 6.809328856521608, + "heading": 0.06022184256964511, + "angularVelocity": 0.5184163039692586, + "velocityX": 3.1908565671131695, + "velocityY": -0.541944829038827, + "timestamp": 0.5240543243400373 + }, + { + "x": 1.5460799068169642, + "y": 6.7847134615337374, + "heading": 0.15060251572580788, + "angularVelocity": 1.5562515056192512, + "velocityX": 3.1862029347676004, + "velocityY": -0.42384886252280385, + "timestamp": 0.5821302027117656 + }, + { + "x": 1.7105613074532942, + "y": 6.762828400607154, + "heading": 0.2309692612234495, + "angularVelocity": 1.383823159474839, + "velocityX": 2.832181023307612, + "velocityY": -0.37683564226962823, + "timestamp": 0.640206081083494 + }, + { + "x": 1.8544824052632503, + "y": 6.743675273377495, + "heading": 0.30132062249757485, + "angularVelocity": 1.2113697329522057, + "velocityX": 2.478156195740254, + "velocityY": -0.32979487812590036, + "timestamp": 0.6982819594552223 + }, + { + "x": 1.9778431814510589, + "y": 6.7272555996641294, + "heading": 0.36164887282711905, + "angularVelocity": 1.0387832611570498, + "velocityX": 2.124131044531241, + "velocityY": -0.28272794443621835, + "timestamp": 0.7563578378269507 + }, + { + "x": 2.080643721940094, + "y": 6.71357071481128, + "heading": 0.4119420778554084, + "angularVelocity": 0.865991293431256, + "velocityX": 1.7701073728241554, + "velocityY": -0.23563801765090975, + "timestamp": 0.814433716198679 + }, + { + "x": 2.162884153160651, + "y": 6.702621709473572, + "heading": 0.45218719175687055, + "angularVelocity": 0.692974691555483, + "velocityX": 1.4160858781017074, + "velocityY": -0.18852931104418616, + "timestamp": 0.8725095945704073 + }, + { + "x": 2.224564575642773, + "y": 6.694409403593854, + "heading": 0.48237325830124134, + "angularVelocity": 0.5197694359637186, + "velocityX": 1.062066114391284, + "velocityY": -0.1414064859622663, + "timestamp": 0.9305854729421357 + }, + { + "x": 2.2656850077143114, + "y": 6.688934342658481, + "heading": 0.5024941355793361, + "angularVelocity": 0.3464584237418892, + "velocityX": 0.7080466662654255, + "velocityY": -0.09427426823109995, + "timestamp": 0.988661351313864 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": 0.17327126075488852, - "velocityX": 0.354257244182047, - "velocityY": -0.047168141046465786, - "timestamp": 1.0460522885964412 + "angularVelocity": 0.17315767410862712, + "velocityX": 0.3540254393299603, + "velocityY": -0.04713727434869594, + "timestamp": 1.0467372296855924 }, { "x": 2.286245346069336, "y": 6.686196804046631, "heading": 0.5125504196, - "angularVelocity": -1.480616560828957e-26, - "velocityX": -5.749006986314809e-23, - "velocityY": -3.283580548358229e-23, - "timestamp": 1.1040901645422216 + "angularVelocity": 1.537853498112127e-28, + "velocityX": 2.8833537662590268e-24, + "velocityY": 1.7087922239161212e-24, + "timestamp": 1.1048131080573207 }, { - "x": 2.3347330882331176, - "y": 6.713955255875249, + "x": 2.3347330882302457, + "y": 6.713955255873605, "heading": 0.5125504196, - "angularVelocity": -2.553583698003105e-17, - "velocityX": 0.5108360349814401, - "velocityY": 0.2924454065412527, - "timestamp": 1.1990085705343583 + "angularVelocity": 2.6668696602250958e-17, + "velocityX": 0.5105017651842132, + "velocityY": 0.29225404246344755, + "timestamp": 1.1997936653594352 }, { - "x": 2.4317085702199166, - "y": 6.7694721581924355, + "x": 2.4317085702128325, + "y": 6.76947215818838, "heading": 0.5125504196, - "angularVelocity": -7.602638325412282e-17, - "velocityX": 1.021672045302073, - "velocityY": 0.5848907989645902, - "timestamp": 1.293926976526495 + "angularVelocity": 7.804843369806144e-17, + "velocityX": 1.0210035057398825, + "velocityY": 0.5845080708274503, + "timestamp": 1.2947742226615497 }, { - "x": 2.577171786567949, - "y": 6.852747507871406, + "x": 2.5771717865588863, + "y": 6.852747507866218, "heading": 0.5125504196, - "angularVelocity": -2.864728625251783e-17, - "velocityX": 1.5325079980808258, - "velocityY": 0.8773361584461279, - "timestamp": 1.3888453825186315 + "angularVelocity": 1.13517713021376e-16, + "velocityX": 1.5315051888289535, + "velocityY": 0.8767620662927504, + "timestamp": 1.3897547799636643 }, { - "x": 2.771122709968306, - "y": 6.963781289278251, + "x": 2.7711227099773685, + "y": 6.963781289283441, "heading": 0.5125504196, - "angularVelocity": -4.95735356219891e-19, - "velocityX": 2.0433436631502695, - "velocityY": 1.1697813532187173, - "timestamp": 1.4837637885107682 + "angularVelocity": 6.94376955873156e-17, + "velocityX": 2.042006584585122, + "velocityY": 1.1690158972645894, + "timestamp": 1.4847353372657788 }, { - "x": 2.9165859263163383, - "y": 7.047056638957223, + "x": 2.9165859263234224, + "y": 7.047056638961279, "heading": 0.5125504196, - "angularVelocity": -2.1000816812429605e-17, - "velocityX": 1.532507998080826, - "velocityY": 0.8773361584461278, - "timestamp": 1.5786821945029048 + "angularVelocity": 3.9533104900387686e-17, + "velocityX": 1.5315051888289533, + "velocityY": 0.8767620662927503, + "timestamp": 1.5797158945678933 }, { - "x": 3.0135614083031372, - "y": 7.102573541274409, + "x": 3.013561408306009, + "y": 7.102573541276054, "heading": 0.5125504196, - "angularVelocity": 1.0493910343453442e-17, - "velocityX": 1.021672045302073, - "velocityY": 0.5848907989645902, - "timestamp": 1.6736006004950414 + "angularVelocity": -1.917838345664243e-18, + "velocityX": 1.0210035057398823, + "velocityY": 0.5845080708274502, + "timestamp": 1.6746964518700078 }, { "x": 3.062049150466919, "y": 7.130331993103027, "heading": 0.5125504196, - "angularVelocity": 3.728395973644304e-17, - "velocityX": 0.5108360349814401, - "velocityY": 0.2924454065412527, - "timestamp": 1.768519006487178 + "angularVelocity": -6.474946504344956e-17, + "velocityX": 0.5105017651842131, + "velocityY": 0.29225404246344755, + "timestamp": 1.7696770091721223 }, { "x": 3.062049150466919, "y": 7.130331993103027, "heading": 0.5125504196, - "angularVelocity": -2.9402212487685217e-25, - "velocityX": 6.030156719342467e-23, - "velocityY": 3.0718511862526725e-23, - "timestamp": 1.8634374124793147 - }, - { - "x": 3.0865020118467, - "y": 7.124545249523675, - "heading": 0.48610131944841944, - "angularVelocity": -0.40878852104321894, - "velocityX": 0.3779353165675877, - "velocityY": -0.08943798979559645, - "timestamp": 1.9281385936281472 - }, - { - "x": 3.135458110441477, - "y": 7.112972562266447, - "heading": 0.4339971852577861, - "angularVelocity": -0.8053042195748789, - "velocityX": 0.7566492253389184, - "velocityY": -0.17886361657922661, - "timestamp": 1.9928397747769797 - }, - { - "x": 3.2089771746406726, - "y": 7.095606129050513, - "heading": 0.357225026215491, - "angularVelocity": -1.1865650314125755, - "velocityX": 1.136286276290371, - "velocityY": -0.2684098328280398, - "timestamp": 2.057540955925812 - }, - { - "x": 3.307133671873185, - "y": 7.072412045867193, - "heading": 0.2571531716690748, - "angularVelocity": -1.5466773986122515, - "velocityX": 1.5170742711284095, - "velocityY": -0.3584800581919169, - "timestamp": 2.1222421370746445 - }, - { - "x": 3.4300337444037163, - "y": 7.043313631479606, - "heading": 0.13605141213735358, - "angularVelocity": -1.8717086362480815, - "velocityX": 1.8995027656114087, - "velocityY": -0.44973544332447146, - "timestamp": 2.1869433182234768 - }, - { - "x": 3.5778386039995205, - "y": 7.008182862001031, - "heading": -0.0018831098932495121, - "angularVelocity": -2.1318702314461877, - "velocityX": 2.2844228957089365, - "velocityY": -0.5429695231956211, - "timestamp": 2.251644499372309 - }, - { - "x": 3.75075226330582, - "y": 6.966825747454083, - "heading": -0.14853260580564892, - "angularVelocity": -2.266565977753341, - "velocityX": 2.672496177597522, - "velocityY": -0.6392018478273882, - "timestamp": 2.3163456805211413 - }, - { - "x": 3.9486334270649626, - "y": 6.918918967044097, - "heading": -0.28405555900160556, - "angularVelocity": -2.094597823248583, - "velocityX": 3.0583856468393638, - "velocityY": -0.7404313114436918, - "timestamp": 2.3810468616699736 - }, - { - "x": 4.166070195160817, - "y": 6.865183550075389, - "heading": -0.3449865942152846, - "angularVelocity": -0.9417298746605423, - "velocityX": 3.360630582549694, - "velocityY": -0.8305167852361092, - "timestamp": 2.445748042818806 - }, - { - "x": 4.4009794486877345, - "y": 6.8112911031395695, - "heading": -0.3449866586118039, - "angularVelocity": -9.952912480962177e-7, - "velocityX": 3.6306795232463367, - "velocityY": -0.8329437883344079, - "timestamp": 2.510449223967638 - }, - { - "x": 4.635888809481111, - "y": 6.757399123762019, - "heading": -0.34498672300630057, - "angularVelocity": -9.952599862485997e-7, - "velocityX": 3.630681181121147, - "velocityY": -0.8329365619088571, - "timestamp": 2.5751504051164704 - }, - { - "x": 4.87079817027739, - "y": 6.703507144397122, - "heading": -0.3449867874007968, - "angularVelocity": -9.952599802438338e-7, - "velocityX": 3.630681181166014, - "velocityY": -0.8329365617132907, - "timestamp": 2.6398515862653027 - }, - { - "x": 5.105707531073669, - "y": 6.649615165032224, - "heading": -0.3449868517952927, - "angularVelocity": -9.952599738581064e-7, - "velocityX": 3.6306811811660147, - "velocityY": -0.8329365617132863, - "timestamp": 2.704552767414135 - }, - { - "x": 5.340616891869949, - "y": 6.595723185667327, - "heading": -0.3449869161897882, - "angularVelocity": -9.952599684354273e-7, - "velocityX": 3.630681181166015, - "velocityY": -0.8329365617132873, - "timestamp": 2.7692539485629672 - }, - { - "x": 5.575526252666228, - "y": 6.54183120630243, - "heading": -0.34498698058428334, - "angularVelocity": -9.952599624061458e-7, - "velocityX": 3.6306811811660156, - "velocityY": -0.8329365617132882, - "timestamp": 2.8339551297117995 - }, - { - "x": 5.8104356134625075, - "y": 6.4879392269375336, - "heading": -0.344987044978778, - "angularVelocity": -9.952599561054567e-7, - "velocityX": 3.6306811811660156, - "velocityY": -0.8329365617132889, - "timestamp": 2.898656310860632 - }, - { - "x": 6.0453449742587875, - "y": 6.434047247572637, - "heading": -0.3449871093732723, - "angularVelocity": -9.952599496144382e-7, - "velocityX": 3.6306811811660165, - "velocityY": -0.8329365617132901, - "timestamp": 2.963357492009464 - }, - { - "x": 6.280254335055067, - "y": 6.38015526820774, - "heading": -0.3449871737677662, - "angularVelocity": -9.952599439036616e-7, - "velocityX": 3.630681181166016, - "velocityY": -0.832936561713291, - "timestamp": 3.0280586731582964 - }, - { - "x": 6.515163695851346, - "y": 6.326263288842841, - "heading": -0.3449872381622598, - "angularVelocity": -9.952599382292206e-7, - "velocityX": 3.6306811811660133, - "velocityY": -0.8329365617133051, - "timestamp": 3.0927598543071286 - }, - { - "x": 6.750073056640409, - "y": 6.2723713094464895, - "heading": -0.34498730255675286, - "angularVelocity": -9.952599312963826e-7, - "velocityX": 3.6306811810544866, - "velocityY": -0.832936562199441, - "timestamp": 3.157461035455961 - }, - { - "x": 6.984982150775984, - "y": 6.2184781677494305, - "heading": -0.3449873669513136, - "angularVelocity": -9.95260976649588e-7, - "velocityX": 3.6306770597466, - "velocityY": -0.8329545263337457, - "timestamp": 3.222162216604793 + "angularVelocity": 3.837127558436768e-26, + "velocityX": -2.7920148574402887e-24, + "velocityY": -2.697407810456559e-24, + "timestamp": 1.8646575664742369 + }, + { + "x": 3.0864612629057913, + "y": 7.12456423366324, + "heading": 0.4864079011679127, + "angularVelocity": -0.404266786795747, + "velocityX": 0.377507862534246, + "velocityY": -0.08919238526276327, + "timestamp": 1.9293240671266776 + }, + { + "x": 3.135333250268508, + "y": 7.113029742760453, + "heading": 0.43488377787747834, + "angularVelocity": -0.7967668386350167, + "velocityX": 0.7557543220930696, + "velocityY": -0.17836887393645673, + "timestamp": 1.9939905677791183 + }, + { + "x": 3.2087217801138035, + "y": 7.095721914438121, + "heading": 0.3589213764077576, + "angularVelocity": -1.1746793270597933, + "velocityX": 1.1348770863562407, + "velocityY": -0.26764751683960536, + "timestamp": 2.058657068431559 + }, + { + "x": 3.306697290004189, + "y": 7.0726100183064045, + "heading": 0.2598184881364111, + "angularVelocity": -1.5325228251330487, + "velocityX": 1.5150890940731196, + "velocityY": -0.3574013731767354, + "timestamp": 2.1233235690839996 + }, + { + "x": 3.4293598616620984, + "y": 7.043622922718144, + "heading": 0.13971196471311126, + "angularVelocity": -1.85732214069893, + "velocityX": 1.8968487612647713, + "velocityY": -0.44825520626292115, + "timestamp": 2.1879900697364403 + }, + { + "x": 3.5768625085702324, + "y": 7.008640009161994, + "heading": 0.002547090154321883, + "angularVelocity": -2.1211117529925048, + "velocityX": 2.2809746224077885, + "velocityY": -0.540974278849118, + "timestamp": 2.252656570388881 + }, + { + "x": 3.74940588673651, + "y": 6.967477445012277, + "heading": -0.14404002070042093, + "angularVelocity": -2.266816812039918, + "velocityX": 2.6682034194742714, + "velocityY": -0.6365361312954025, + "timestamp": 2.3173230710413217 + }, + { + "x": 3.946923390754574, + "y": 6.9198227973610145, + "heading": -0.2817650462278063, + "angularVelocity": -2.129773903610591, + "velocityX": 3.0544022333858827, + "velocityY": -0.7369294328664762, + "timestamp": 2.3819895716937625 + }, + { + "x": 4.163931894345135, + "y": 6.866332868698949, + "heading": -0.34498660423093713, + "angularVelocity": -0.9776554686780423, + "velocityX": 3.355810217053556, + "velocityY": -0.8271659688152148, + "timestamp": 2.446656072346203 + }, + { + "x": 4.399029324107109, + "y": 6.812427188093308, + "heading": -0.34498666703392, + "angularVelocity": -9.711826405660571e-7, + "velocityX": 3.635536597620139, + "velocityY": -0.8335951390870039, + "timestamp": 2.511322572998644 + }, + { + "x": 4.63412689461735, + "y": 6.758522121329691, + "heading": -0.3449867298342717, + "angularVelocity": -9.711419520664826e-7, + "velocityX": 3.6355387741453202, + "velocityY": -0.8335856466601779, + "timestamp": 2.5759890736510846 + }, + { + "x": 4.869224465131426, + "y": 6.7046170545827986, + "heading": -0.344986792634623, + "angularVelocity": -9.711419462634673e-7, + "velocityX": 3.6355387742046146, + "velocityY": -0.8335856464015791, + "timestamp": 2.6406555743035254 + }, + { + "x": 5.1043220356455015, + "y": 6.650711987835906, + "heading": -0.344986855434974, + "angularVelocity": -9.711419414707099e-7, + "velocityX": 3.6355387742046164, + "velocityY": -0.8335856464015731, + "timestamp": 2.705322074955966 + }, + { + "x": 5.339419606159577, + "y": 6.596806921089014, + "heading": -0.3449869182353246, + "angularVelocity": -9.711419349644554e-7, + "velocityX": 3.635538774204617, + "velocityY": -0.833585646401574, + "timestamp": 2.769988575608407 + }, + { + "x": 5.574517176673654, + "y": 6.542901854342122, + "heading": -0.3449869810356748, + "angularVelocity": -9.711419302688624e-7, + "velocityX": 3.635538774204617, + "velocityY": -0.8335856464015745, + "timestamp": 2.8346550762608476 + }, + { + "x": 5.809614747187731, + "y": 6.488996787595229, + "heading": -0.3449870438360248, + "angularVelocity": -9.711419254509242e-7, + "velocityX": 3.6355387742046172, + "velocityY": -0.8335856464015756, + "timestamp": 2.8993215769132883 + }, + { + "x": 6.044712317701808, + "y": 6.435091720848337, + "heading": -0.3449871066363744, + "angularVelocity": -9.711419195368686e-7, + "velocityX": 3.6355387742046172, + "velocityY": -0.8335856464015763, + "timestamp": 2.963988077565729 + }, + { + "x": 6.279809888215884, + "y": 6.381186654101444, + "heading": -0.34498716943672364, + "angularVelocity": -9.71141914262753e-7, + "velocityX": 3.6355387742046177, + "velocityY": -0.8335856464015771, + "timestamp": 3.0286545782181697 + }, + { + "x": 6.51490745872996, + "y": 6.32728158735455, + "heading": -0.34498723223707245, + "angularVelocity": -9.71141908290338e-7, + "velocityX": 3.635538774204614, + "velocityY": -0.8335856464015945, + "timestamp": 3.0933210788706105 + }, + { + "x": 6.750005029235043, + "y": 6.273376520568434, + "heading": -0.344987295037421, + "angularVelocity": -9.71141902973265e-7, + "velocityX": 3.6355387740655463, + "velocityY": -0.8335856470081151, + "timestamp": 3.157987579523051 + }, + { + "x": 6.985102269626149, + "y": 6.219470014067963, + "heading": -0.34498735783785006, + "angularVelocity": -9.711431495756142e-7, + "velocityX": 3.6355336691971325, + "velocityY": -0.8336079106893309, + "timestamp": 3.222654080175492 }, { "x": 7.214789390563965, "y": 6.145846366882324, "heading": -0.344987478573796, - "angularVelocity": -0.00000172520007220815, - "velocityX": 3.5518244907979937, - "velocityY": -1.1225730284587905, - "timestamp": 3.2868633977536255 - }, - { - "x": 7.409091443638604, - "y": 6.073965873280571, - "heading": -0.3572291452032454, - "angularVelocity": -0.20035361948729366, - "velocityX": 3.1800506242888202, - "velocityY": -1.1764343450588084, - "timestamp": 3.347963699613786 - }, - { - "x": 7.58146433123023, - "y": 6.0094940876522065, - "heading": -0.36768136673125634, - "angularVelocity": -0.17106661030796205, - "velocityX": 2.8211462520452746, - "velocityY": -1.0551794944633763, - "timestamp": 3.4090640014739466 - }, - { - "x": 7.73220907640328, - "y": 5.953248314526567, - "heading": -0.37521177845828957, - "angularVelocity": -0.1232467188831242, - "velocityX": 2.4671685831938674, - "velocityY": -0.920548203744869, - "timestamp": 3.470164303334107 - }, - { - "x": 7.861444501366057, - "y": 5.905524901339061, - "heading": -0.37940510209885836, - "angularVelocity": -0.06863016241991673, - "velocityX": 2.1151356217282813, - "velocityY": -0.7810667334627726, - "timestamp": 3.5312646051942678 - }, - { - "x": 7.969234018825332, - "y": 5.866476846712542, - "heading": -0.38004587758642694, - "angularVelocity": -0.010487272043845954, - "velocityX": 1.7641405063099576, - "velocityY": -0.6390812064380308, - "timestamp": 3.5923649070544283 - }, - { - "x": 8.055617039018077, - "y": 5.8361975486879665, - "heading": -0.37700219526471646, - "angularVelocity": 0.04981452184436869, - "velocityX": 1.413790399766751, - "velocityY": -0.4955670774569341, - "timestamp": 3.653465208914589 - }, - { - "x": 8.12062042791295, - "y": 5.814749961981242, - "heading": -0.37018491999207215, - "angularVelocity": 0.11157514881427125, - "velocityX": 1.0638799959392298, - "velocityY": -0.3510225981503441, - "timestamp": 3.7145655107747495 - }, - { - "x": 8.164263678264032, - "y": 5.802179408572721, - "heading": -0.3595296539883727, - "angularVelocity": 0.17438974406519217, - "velocityX": 0.7142886208805774, - "velocityY": -0.20573635523587247, - "timestamp": 3.77566581263491 + "angularVelocity": -0.0000018670555043319872, + "velocityX": 3.551871813387612, + "velocityY": -1.1385129308502273, + "timestamp": 3.2873205808279327 + }, + { + "x": 7.409712349690949, + "y": 6.073735935167582, + "heading": -0.3570370868218633, + "angularVelocity": -0.19637949309616867, + "velocityX": 3.176773146322184, + "velocityY": -1.1752257613319905, + "timestamp": 3.348679371698049 + }, + { + "x": 7.582537346896194, + "y": 6.009055207663653, + "heading": -0.36748967160875157, + "angularVelocity": -0.17035187034591742, + "velocityX": 2.816629773084668, + "velocityY": -1.0541395387149, + "timestamp": 3.4100381625681653 + }, + { + "x": 7.733574876390023, + "y": 5.952649624668501, + "heading": -0.37509136643476676, + "angularVelocity": -0.12388925398002781, + "velocityX": 2.4615467050767728, + "velocityY": -0.9192746824909085, + "timestamp": 3.4713969534382816 + }, + { + "x": 7.862949786289389, + "y": 5.90483023458532, + "heading": -0.37937521943526015, + "angularVelocity": -0.06981645074397583, + "velocityX": 2.1084983596437845, + "velocityY": -0.7793404890328669, + "timestamp": 3.532755744308398 + }, + { + "x": 7.9707292244718095, + "y": 5.86575856751758, + "heading": -0.3800974258169581, + "angularVelocity": -0.011770218602036829, + "velocityX": 1.7565443623321622, + "velocityY": -0.636773745272234, + "timestamp": 3.594114535178514 + }, + { + "x": 8.056955102704071, + "y": 5.835533544523746, + "heading": -0.37710820320869415, + "angularVelocity": 0.04871710419769366, + "velocityX": 1.4052734255273058, + "velocityY": -0.49259482732986287, + "timestamp": 3.6554733260486305 + }, + { + "x": 8.121656070762867, + "y": 5.8142219773756105, + "heading": -0.3703061215705459, + "angularVelocity": 0.11085749151326833, + "velocityX": 1.0544694108421286, + "velocityY": -0.34732703897714834, + "timestamp": 3.716832116918747 + }, + { + "x": 8.164852955153219, + "y": 5.801872031339644, + "heading": -0.3596178039887861, + "angularVelocity": 0.1741937451861573, + "velocityX": 0.7040048178555333, + "velocityY": -0.2012742731861952, + "timestamp": 3.778190907788863 }, { "x": 8.186561584472656, "y": 5.798520088195801, "heading": -0.344987478573796, - "angularVelocity": 0.23800496841831012, - "velocityX": 0.3649393788537573, - "velocityY": -0.05989038131588724, - "timestamp": 3.8367661144950707 - }, - { - "x": 8.18566399540182, - "y": 5.805056139735724, - "heading": -0.32461060520225765, - "angularVelocity": 0.30765327861825625, - "velocityX": -0.013551942707795956, - "velocityY": 0.09868234683560065, - "timestamp": 3.9029993542741352 - }, - { - "x": 8.159694473473342, - "y": 5.822092590454781, - "heading": -0.2998178925011234, - "angularVelocity": 0.3743243239170494, - "velocityX": -0.3920919770058792, - "velocityY": 0.25721904554096775, - "timestamp": 3.9692325940532 - }, - { - "x": 8.108649030585239, - "y": 5.849626323109156, - "heading": -0.2708591800534793, - "angularVelocity": 0.4372232514103506, - "velocityX": -0.7706922243027172, - "velocityY": 0.41570867960287095, - "timestamp": 4.035465833832265 - }, - { - "x": 8.03252261804608, - "y": 5.887653107737229, - "heading": -0.2380618973255709, - "angularVelocity": 0.4951785966881702, - "velocityX": -1.1493686975466637, - "velocityY": 0.5741344490307231, - "timestamp": 4.1016990736113295 - }, - { - "x": 7.9313086642803885, - "y": 5.936166919703684, - "heading": -0.20187479302243566, - "angularVelocity": 0.5463586625664867, - "velocityX": -1.5281443894834816, - "velocityY": 0.7324692575553122, - "timestamp": 4.167932313390394 - }, - { - "x": 7.804998346649701, - "y": 5.9951585907413225, - "heading": -0.16295187123346636, - "angularVelocity": 0.5876644705710469, - "velocityX": -1.9070532870205719, - "velocityY": 0.8906656421219618, - "timestamp": 4.234165553169459 - }, - { - "x": 7.653579505512198, - "y": 6.0646127178391085, - "heading": -0.12233575906363518, - "angularVelocity": 0.6132285285351425, - "velocityX": -2.286145772765958, - "velocityY": 1.048629469575472, - "timestamp": 4.300398792948523 - }, - { - "x": 7.4770359425587705, - "y": 6.144498725543435, - "heading": -0.08194804569131871, - "angularVelocity": 0.6097801271240603, - "velocityX": -2.6654828231614, - "velocityY": 1.206131663962129, - "timestamp": 4.366632032727588 - }, - { - "x": 7.27536197665765, - "y": 6.234730628199731, - "heading": -0.046490249768474004, - "angularVelocity": 0.5353474485186261, - "velocityX": -3.044905648188839, - "velocityY": 1.362335633245254, - "timestamp": 4.432865272506652 - }, - { - "x": 7.049560259358204, - "y": 6.334147758763648, - "heading": -0.04649017070950518, - "angularVelocity": 0.0000011936448991960951, - "velocityX": -3.4091902804793617, - "velocityY": 1.5010156666885635, - "timestamp": 4.499098512285717 - }, - { - "x": 6.822106687595601, - "y": 6.429725315002885, - "heading": -0.04649016173832054, - "angularVelocity": 1.354483741042957e-7, - "velocityX": -3.4341302421763333, - "velocityY": 1.4430451621882883, - "timestamp": 4.565331752064782 - }, - { - "x": 6.586112566537963, - "y": 6.501676487205841, - "heading": -0.04649015261662469, - "angularVelocity": 1.3772081626072917e-7, - "velocityX": -3.563076815279552, - "velocityY": 1.086330254158866, - "timestamp": 4.631564991843846 - }, - { - "x": 6.343631547831337, - "y": 6.547208357601844, - "heading": -0.046490142857452815, - "angularVelocity": 1.4734553082512417e-7, - "velocityX": -3.6610170288434327, - "velocityY": 0.6874474289327321, - "timestamp": 4.697798231622911 - }, - { - "x": 6.097611695820765, - "y": 6.565767095818739, - "heading": -0.046490131800601416, - "angularVelocity": 1.6693810300562863e-7, - "velocityX": -3.7144468975279485, - "velocityY": 0.28020278456559833, - "timestamp": 4.764031471401975 + "angularVelocity": 0.2384389458710078, + "velocityX": 0.35379819275433294, + "velocityY": -0.054628572309033785, + "timestamp": 3.8395496986589794 + }, + { + "x": 8.185020658343282, + "y": 5.805387130671382, + "heading": -0.32459673237759007, + "angularVelocity": 0.30843585413704155, + "velocityX": -0.023308458763729726, + "velocityY": 0.10387271220830593, + "timestamp": 3.9056598668599376 + }, + { + "x": 8.158545676834562, + "y": 5.822731510098044, + "heading": -0.29984074692215335, + "angularVelocity": 0.37446562501830083, + "velocityX": -0.40046761684590937, + "velocityY": 0.2623556995640975, + "timestamp": 3.9717700350608958 + }, + { + "x": 8.10713245572277, + "y": 5.850551569484812, + "heading": -0.2710499624922478, + "angularVelocity": 0.43549706820271483, + "velocityX": -0.7776900666098594, + "velocityY": 0.42081362283335544, + "timestamp": 4.037880203261854 + }, + { + "x": 8.030775877182073, + "y": 5.8888449452190175, + "heading": -0.23865422460782199, + "angularVelocity": 0.49002655364528946, + "velocityX": -1.154989929969518, + "velocityY": 0.5792357934683214, + "timestamp": 4.103990371462812 + }, + { + "x": 7.929469606523289, + "y": 5.937608095309845, + "heading": -0.2032366176335679, + "angularVelocity": 0.5357361497945922, + "velocityX": -1.5323856135237626, + "velocityY": 0.7376043870074612, + "timestamp": 4.17010053966377 + }, + { + "x": 7.803205805922035, + "y": 5.996835346003344, + "heading": -0.16563448060225386, + "angularVelocity": 0.5687799328692251, + "velocityX": -1.909899854095725, + "velocityY": 0.8958871578342301, + "timestamp": 4.236210707864728 + }, + { + "x": 7.651975275778253, + "y": 6.066516654618355, + "heading": -0.12715303116567858, + "angularVelocity": 0.5820806463477988, + "velocityX": -2.2875532502667433, + "velocityY": 1.0540180203928478, + "timestamp": 4.302320876065687 + }, + { + "x": 7.4757708248918915, + "y": 6.146630988469932, + "heading": -0.09010777903671587, + "angularVelocity": 0.5603563436165322, + "velocityX": -2.665315422443717, + "velocityY": 1.211830736960907, + "timestamp": 4.368431044266645 + }, + { + "x": 7.274619001255966, + "y": 6.237116150274765, + "heading": -0.05972677615109367, + "angularVelocity": 0.45955113581426293, + "velocityX": -3.0426760226123557, + "velocityY": 1.368702640867323, + "timestamp": 4.434541212467603 + }, + { + "x": 7.049305084903872, + "y": 6.337299858985659, + "heading": -0.05972649238552697, + "angularVelocity": 0.000004292313488445514, + "velocityX": -3.408158267986829, + "velocityY": 1.515405442720417, + "timestamp": 4.500651380668561 + }, + { + "x": 6.822055990090289, + "y": 6.433013154965271, + "heading": -0.05972648235247219, + "angularVelocity": 1.5176265701728567e-7, + "velocityX": -3.4374302924597684, + "velocityY": 1.4477847899080587, + "timestamp": 4.566761548869519 + }, + { + "x": 6.585901540176483, + "y": 6.503965686132618, + "heading": -0.05972647213388985, + "angularVelocity": 1.5456899617029753e-7, + "velocityX": -3.5721350639429144, + "velocityY": 1.0732468710663359, + "timestamp": 4.632871717070477 + }, + { + "x": 6.343393108903057, + "y": 6.54860698900868, + "heading": -0.05972646117550161, + "angularVelocity": 1.65759497255032e-7, + "velocityX": -3.6682470771555504, + "velocityY": 0.6752562289716734, + "timestamp": 4.698981885271436 + }, + { + "x": 6.097452808494555, + "y": 6.566398909819071, + "heading": -0.059726448733939674, + "angularVelocity": 1.8819437727240375e-7, + "velocityX": -3.720158443114319, + "velocityY": 0.26912532965138075, + "timestamp": 4.765092053472394 }, { "x": 5.851044178009033, "y": 6.557126998901367, - "heading": -0.04649011830549673, - "angularVelocity": 2.0375123930314497e-7, - "velocityX": -3.722715642994527, - "velocityY": -0.13044955895548105, - "timestamp": 4.83026471118104 - }, - { - "x": 5.59400062961639, - "y": 6.517937013895863, - "heading": -0.046490107217199095, - "angularVelocity": 1.588527062833638e-7, - "velocityX": -3.682446546399627, - "velocityY": -0.5614419262393832, - "timestamp": 4.900067095317195 - }, - { - "x": 5.343239064324899, - "y": 6.449191978638034, - "heading": -0.046490097789975614, - "angularVelocity": 1.3505589536533548e-7, - "velocityX": -3.5924498624912715, - "velocityY": -0.984852252664273, - "timestamp": 4.96986947945335 - }, - { - "x": 5.10214573961629, - "y": 6.351820274741271, - "heading": -0.04649007357112031, - "angularVelocity": 3.469631532168612e-7, - "velocityX": -3.453941118090443, - "velocityY": -1.3949624371974614, - "timestamp": 5.039671863589505 - }, - { - "x": 4.887305900136772, - "y": 6.241894655501948, - "heading": -0.013339477589168081, - "angularVelocity": 0.4749206834724951, - "velocityX": -3.0778295345966584, - "velocityY": -1.574811814807135, - "timestamp": 5.10947424772566 - }, - { - "x": 4.700625263351654, - "y": 6.142924243380359, - "heading": 0.02019201613120645, - "angularVelocity": 0.4803774847427688, - "velocityX": -2.6744163411522406, - "velocityY": -1.417865784190692, - "timestamp": 5.179276631861815 - }, - { - "x": 4.541203570947359, - "y": 6.05696873798045, - "heading": 0.05079450877042205, - "angularVelocity": 0.4384161517968095, - "velocityX": -2.283900390756435, - "velocityY": -1.2314121711408288, - "timestamp": 5.24907901599797 - }, - { - "x": 4.408671715563658, - "y": 5.984755616609754, - "heading": 0.07726906366461818, - "angularVelocity": 0.3792786624960486, - "velocityX": -1.8986723308073217, - "velocityY": -1.034536603074175, - "timestamp": 5.318881400134125 - }, - { - "x": 4.302830646527999, - "y": 5.92665565579208, - "heading": 0.09900038404891696, - "angularVelocity": 0.3113263343829381, - "velocityX": -1.5162959022890692, - "velocityY": -0.8323492318592651, - "timestamp": 5.38868378427028 - }, - { - "x": 4.22355609558814, - "y": 5.882893451015234, - "heading": 0.11561440292356019, - "angularVelocity": 0.23801506324248706, - "velocityX": -1.1356997604154628, - "velocityY": -0.6269442701482041, - "timestamp": 5.458486168406435 - }, - { - "x": 4.170763147512419, - "y": 5.853619589907405, - "heading": 0.1268589116332185, - "angularVelocity": 0.1610906109986881, - "velocityX": -0.756320127586832, - "velocityY": -0.4193819662481466, - "timestamp": 5.52828855254259 + "heading": -0.05972643351340637, + "angularVelocity": 2.3022983790495482e-7, + "velocityX": -3.7272425285094304, + "velocityY": -0.14024939234037223, + "timestamp": 4.831202221673352 + }, + { + "x": 5.593733281594629, + "y": 6.517293519804324, + "heading": -0.059726421475485736, + "angularVelocity": 1.7244301744192055e-7, + "velocityX": -3.6859744116289104, + "velocityY": -0.5706139410489331, + "timestamp": 4.901010328751527 + }, + { + "x": 5.342760368520485, + "y": 6.447952318722012, + "heading": -0.059726411223577615, + "angularVelocity": 1.4685841743171806e-7, + "velocityX": -3.595182903227699, + "velocityY": -0.9933115791932222, + "timestamp": 4.970818435829702 + }, + { + "x": 5.101497403322433, + "y": 6.350035089382074, + "heading": -0.05972638349654275, + "angularVelocity": 3.9718932395532394e-7, + "velocityX": -3.456088057621621, + "velocityY": -1.4026627198225616, + "timestamp": 5.0406265429078765 + }, + { + "x": 4.886759838624346, + "y": 6.240596211657777, + "heading": -0.024369209517199585, + "angularVelocity": 0.5064909429466152, + "velocityX": -3.0761121263123843, + "velocityY": -1.5677101457820914, + "timestamp": 5.110434649986051 + }, + { + "x": 4.700188679669267, + "y": 6.141997521730213, + "heading": 0.01157815322069568, + "angularVelocity": 0.5149453873264229, + "velocityX": -2.672628821551406, + "velocityY": -1.4124246316713311, + "timestamp": 5.180242757064226 + }, + { + "x": 4.540876156189366, + "y": 6.0563320226623665, + "heading": 0.04446453476629188, + "angularVelocity": 0.47109688146633416, + "velocityX": -2.2821493111322217, + "velocityY": -1.2271568826800237, + "timestamp": 5.250050864142401 + }, + { + "x": 4.40844486564244, + "y": 5.984344508560839, + "heading": 0.07295632714737806, + "angularVelocity": 0.40814446306615176, + "velocityX": -1.8970760859999023, + "velocityY": -1.031219970209356, + "timestamp": 5.319858971220576 + }, + { + "x": 4.302690180424501, + "y": 5.9264156183199805, + "heading": 0.09636693816788014, + "angularVelocity": 0.3353566226095424, + "velocityX": -1.5149341479709326, + "velocityY": -0.8298304117598572, + "timestamp": 5.389667078298751 + }, + { + "x": 4.223483980380201, + "y": 5.882776245569646, + "heading": 0.11427821119992405, + "angularVelocity": 0.2565786952507661, + "velocityX": -1.1346275290862768, + "velocityY": -0.6251333058131043, + "timestamp": 5.459475185376926 + }, + { + "x": 4.170738558104569, + "y": 5.853581333825438, + "heading": 0.12640786347483376, + "angularVelocity": 0.17375707181583125, + "velocityX": -0.7555773173532012, + "velocityY": -0.41821663652209046, + "timestamp": 5.5292832924551005 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 0.08154784945777828, - "velocityX": -0.37782436284418364, - "velocityY": -0.21027274576950902, - "timestamp": 5.598090936678745 + "angularVelocity": 0.08800242162180778, + "velocityX": -0.37744114553759195, + "velocityY": -0.20970748963591992, + "timestamp": 5.599091399533275 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 1.1000727380078476e-26, - "velocityX": 7.869680140840765e-27, - "velocityY": 7.286496207304452e-27, - "timestamp": 5.6678933208149 - }, - { - "x": 4.195350058507561, - "y": 5.858786392293566, - "heading": 0.13540679339101, - "angularVelocity": 0.03040787160499198, - "velocityX": 0.5426383042255808, - "velocityY": 0.2113090624413005, - "timestamp": 5.7618047751106225 - }, - { - "x": 4.297269963634785, - "y": 5.898475069782893, - "heading": 0.14111775296085857, - "angularVelocity": 0.06081217262236255, - "velocityX": 1.085276613929165, - "velocityY": 0.42261806919047107, - "timestamp": 5.855716229406345 - }, - { - "x": 4.450149823252348, - "y": 5.958008075795516, - "heading": 0.14968325491595807, - "angularVelocity": 0.09120827719403884, - "velocityX": 1.6279149414101537, - "velocityY": 0.6339269949452273, - "timestamp": 5.949627683702067 - }, - { - "x": 4.653989639578877, - "y": 6.037385400048414, - "heading": 0.16110213512106505, - "angularVelocity": 0.12159198567141313, - "velocityX": 2.1705532925158373, - "velocityY": 0.8452358112030016, - "timestamp": 6.043539137997789 - }, - { - "x": 4.908789413625767, - "y": 6.136607028819001, - "heading": 0.17537288795521153, - "angularVelocity": 0.15195966180236836, - "velocityX": 2.7131916543911667, - "velocityY": 1.0565444813381684, - "timestamp": 6.137450592293511 - }, - { - "x": 5.214549136636689, - "y": 6.255672941521673, - "heading": 0.19249371968896806, - "angularVelocity": 0.1823082377134106, - "velocityX": 3.2558299230262406, - "velocityY": 1.2678529322710714, - "timestamp": 6.231362046589234 - }, - { - "x": 5.540526297381237, - "y": 6.382610310333338, - "heading": 0.1924937215917483, - "angularVelocity": 2.0261428816305725e-8, - "velocityX": 3.4711118381583637, - "velocityY": 1.3516707814144493, - "timestamp": 6.325273500884956 - }, - { - "x": 5.866503458135056, - "y": 6.50954767912126, - "heading": 0.19249372349451602, - "angularVelocity": 2.0261295290604937e-8, - "velocityX": 3.471111838257083, - "velocityY": 1.351670781161622, - "timestamp": 6.419184955180678 - }, - { - "x": 6.192480618888875, - "y": 6.636485047909182, - "heading": 0.19249372539728374, - "angularVelocity": 2.0261295568537564e-8, - "velocityX": 3.471111838257084, - "velocityY": 1.3516707811616195, - "timestamp": 6.5130964094764 - }, - { - "x": 6.518457779642694, - "y": 6.763422416697105, - "heading": 0.19249372730005146, - "angularVelocity": 2.0261295333491658e-8, - "velocityX": 3.4711118382570834, - "velocityY": 1.35167078116162, - "timestamp": 6.607007863772123 - }, - { - "x": 6.844434940394272, - "y": 6.890359785490778, - "heading": 0.19249372920281918, - "angularVelocity": 2.0261295628050687e-8, - "velocityX": 3.471111838233232, - "velocityY": 1.3516707812228705, - "timestamp": 6.700919318067845 - }, - { - "x": 7.170411838536006, - "y": 7.017297824914972, - "heading": 0.1924937316314852, - "angularVelocity": 2.586123310059887e-8, - "velocityX": 3.4711090418773667, - "velocityY": 1.351677922316834, - "timestamp": 6.794830772363567 - }, - { - "x": 7.445544445617428, - "y": 7.124436352045788, - "heading": 0.20746316204367837, - "angularVelocity": 0.15939941005551134, - "velocityX": 2.929702336575951, - "velocityY": 1.1408462144931066, - "timestamp": 6.888742226659289 - }, - { - "x": 7.669715374136242, - "y": 7.211730123643104, - "heading": 0.22019672303892965, - "angularVelocity": 0.13559113838397205, - "velocityX": 2.38704565060736, - "velocityY": 0.9295327417934781, - "timestamp": 6.9826536809550115 - }, - { - "x": 7.842925396345479, - "y": 7.279179365267254, - "heading": 0.23039470264394565, - "angularVelocity": 0.10859143521410211, - "velocityX": 1.8443971878425902, - "velocityY": 0.7182216709343664, - "timestamp": 7.076565135250734 - }, - { - "x": 7.96517479602962, - "y": 7.326784165564593, - "heading": 0.23795514608622043, - "angularVelocity": 0.08050608415100667, - "velocityX": 1.3017517469081525, - "velocityY": 0.506911544010745, - "timestamp": 7.170476589546456 - }, - { - "x": 8.0364637201711, - "y": 7.354544571992242, - "heading": 0.24282665608297047, - "angularVelocity": 0.05187343794517228, - "velocityX": 0.7591078710909263, - "velocityY": 0.2956019224261278, - "timestamp": 7.264388043842178 + "angularVelocity": 6.956965199088368e-27, + "velocityX": 5.711893177801191e-27, + "velocityY": 1.0709622806033376e-27, + "timestamp": 5.66889950661145 + }, + { + "x": 4.195152179176686, + "y": 5.858709351822529, + "heading": 0.13547549704031217, + "angularVelocity": 0.03117959203710152, + "velocityX": 0.5412280111334526, + "velocityY": 0.210760048168702, + "timestamp": 5.762690056862549 + }, + { + "x": 4.296676325767576, + "y": 5.898243947995512, + "heading": 0.1413238484362874, + "angularVelocity": 0.06235544391538225, + "velocityX": 1.0824560290891385, + "velocityY": 0.42152003658300596, + "timestamp": 5.856480607113649 + }, + { + "x": 4.448962547900073, + "y": 5.95754583130188, + "heading": 0.15009539524373325, + "angularVelocity": 0.0935227140043684, + "velocityX": 1.6236840675824051, + "velocityY": 0.6322799380918802, + "timestamp": 5.950271157364748 + }, + { + "x": 4.6520108481753235, + "y": 6.036614990735129, + "heading": 0.16178892165721923, + "angularVelocity": 0.1246770211090534, + "velocityX": 2.1649121338092594, + "velocityY": 0.8430397222488009, + "timestamp": 6.044061707615847 + }, + { + "x": 4.905821228102917, + "y": 6.135451411658796, + "heading": 0.17640285839515626, + "angularVelocity": 0.15581459644721318, + "velocityX": 2.706140216131426, + "velocityY": 1.0537993503509646, + "timestamp": 6.137852257866946 + }, + { + "x": 5.210393679513956, + "y": 6.254055072364673, + "heading": 0.19393534293153245, + "angularVelocity": 0.18693231343069955, + "velocityX": 3.2473682113563522, + "velocityY": 1.2645587469989996, + "timestamp": 6.231642808118045 + }, + { + "x": 5.53637764515154, + "y": 6.380995104775572, + "heading": 0.1939353448909336, + "angularVelocity": 2.0891242641251563e-8, + "velocityX": 3.475658952472825, + "velocityY": 1.3534416001510998, + "timestamp": 6.325433358369144 + }, + { + "x": 5.862361610799536, + "y": 6.507935137159803, + "heading": 0.19393534685032104, + "angularVelocity": 2.089109662304858e-8, + "velocityX": 3.47565895258384, + "velocityY": 1.353441599866765, + "timestamp": 6.419223908620244 + }, + { + "x": 6.188345576447532, + "y": 6.634875169544034, + "heading": 0.19393534880970845, + "angularVelocity": 2.0891096217109697e-8, + "velocityX": 3.4756589525838413, + "velocityY": 1.3534415998667624, + "timestamp": 6.513014458871343 + }, + { + "x": 6.5143295420955285, + "y": 6.761815201928266, + "heading": 0.19393535076909593, + "angularVelocity": 2.0891097152342323e-8, + "velocityX": 3.475658952583841, + "velocityY": 1.3534415998667637, + "timestamp": 6.606805009122442 + }, + { + "x": 6.840313507740267, + "y": 6.888755234320864, + "heading": 0.1939353527284834, + "angularVelocity": 2.0891096624592066e-8, + "velocityX": 3.475658952549103, + "velocityY": 1.353441599955972, + "timestamp": 6.700595559373541 + }, + { + "x": 7.1662970943707105, + "y": 7.015696232741894, + "heading": 0.19393535571019038, + "angularVelocity": 3.179112387470943e-8, + "velocityX": 3.475654911477867, + "velocityY": 1.353451899804194, + "timestamp": 6.79438610962464 + }, + { + "x": 7.441622589341096, + "y": 7.122909656814238, + "heading": 0.20815324472161514, + "angularVelocity": 0.15159191382671508, + "velocityX": 2.9355355548429496, + "velocityY": 1.143115418187742, + "timestamp": 6.8881766598757395 + }, + { + "x": 7.66618318620661, + "y": 7.210355021181115, + "heading": 0.2205253443109958, + "angularVelocity": 0.13191200559392907, + "velocityX": 2.3942774220250835, + "velocityY": 0.9323472794728952, + "timestamp": 6.981967210126839 + }, + { + "x": 7.839980071520317, + "y": 7.278032678531449, + "heading": 0.23053454012619146, + "angularVelocity": 0.10671859572631527, + "velocityX": 1.8530319403011595, + "velocityY": 0.7215829011467113, + "timestamp": 7.075757760377938 + }, + { + "x": 7.963013724050306, + "y": 7.325942779672703, + "heading": 0.23800124017348176, + "angularVelocity": 0.0796103661541618, + "velocityX": 1.311791563228909, + "velocityY": 0.5108201307379786, + "timestamp": 7.169548310629037 + }, + { + "x": 8.03528439999784, + "y": 7.354085407438329, + "heading": 0.24283421360763968, + "angularVelocity": 0.05152942829761583, + "velocityX": 0.7705539177886093, + "velocityY": 0.3000582435040803, + "timestamp": 7.263338860880136 }, { "x": 8.056792259216309, "y": 7.362460613250732, "heading": 0.244978130412332, - "angularVelocity": 0.022909605068905204, - "velocityX": 0.21646495837660984, - "velocityY": 0.0842926064542015, - "timestamp": 7.3582994981379 - }, - { - "x": 8.02239522092899, - "y": 7.349066085390599, - "heading": 0.24423351408772678, - "angularVelocity": -0.00758829012549376, - "velocityX": -0.3505358361841942, - "velocityY": -0.13650192741958841, - "timestamp": 7.4564265217876144 - }, - { - "x": 7.932360084343089, - "y": 7.314005643971181, - "heading": 0.24049622236652166, - "angularVelocity": -0.03808626392813261, - "velocityX": -0.9175366095613122, - "velocityY": -0.35729649300863325, - "timestamp": 7.5545535454373285 - }, - { - "x": 7.786686853677988, - "y": 7.257279283570004, - "heading": 0.23376594617518753, - "angularVelocity": -0.06858738745974144, - "velocityX": -1.4845373399392365, - "velocityY": -0.578091113857428, - "timestamp": 7.6526805690870425 - }, - { - "x": 7.5853755359232595, - "y": 7.178886996735875, - "heading": 0.2240420388382404, - "angularVelocity": -0.09909510117884326, - "velocityX": -2.0515379990873206, - "velocityY": -0.7988858106403784, - "timestamp": 7.750807592736757 - }, - { - "x": 7.328426142666821, - "y": 7.078828774757454, - "heading": 0.21132346369891478, - "angularVelocity": -0.1296133793350078, - "velocityX": -2.618538540144425, - "velocityY": -1.0196805961994813, - "timestamp": 7.848934616386471 - }, - { - "x": 7.01583869900668, - "y": 6.957104611217254, - "heading": 0.19560873409309495, - "angularVelocity": -0.1601468078958253, - "velocityX": -3.1855388254309114, - "velocityY": -1.2404754471583816, - "timestamp": 7.947061640036185 - }, - { - "x": 6.675228781735299, - "y": 6.824469292432169, - "heading": 0.19560873219026645, - "angularVelocity": -1.939148261699155e-8, - "velocityX": -3.471112284901898, - "velocityY": -1.3516696405524002, - "timestamp": 8.045188663685899 - }, - { - "x": 6.334618864455473, - "y": 6.691833973668682, - "heading": 0.19560873028745154, - "angularVelocity": -1.9391344572203613e-8, - "velocityX": -3.4711122849879685, - "velocityY": -1.3516696403322965, - "timestamp": 8.143315687335612 - }, - { - "x": 5.994008947175647, - "y": 6.559198654905194, - "heading": 0.19560872838463667, - "angularVelocity": -1.9391344282542406e-8, - "velocityX": -3.471112284987969, - "velocityY": -1.3516696403322948, - "timestamp": 8.241442710985325 - }, - { - "x": 5.65339902989582, - "y": 6.426563336141707, - "heading": 0.19560872648182165, - "angularVelocity": -1.9391345115512158e-8, - "velocityX": -3.4711122849879685, - "velocityY": -1.3516696403322956, - "timestamp": 8.339569734635038 - }, - { - "x": 5.312789112621013, - "y": 6.293928017365348, - "heading": 0.195608724578999, - "angularVelocity": -1.939142318059707e-8, - "velocityX": -3.4711122849368206, - "velocityY": -1.3516696404634736, - "timestamp": 8.437696758284751 - }, - { - "x": 4.978960833752885, - "y": 6.1639320605180385, - "heading": 0.17759686591379087, - "angularVelocity": -0.18355655756466713, - "velocityX": -3.402001471682276, - "velocityY": -1.324772239208634, - "timestamp": 8.535823781934464 - }, - { - "x": 4.700770593105711, - "y": 6.055602072933751, - "heading": 0.16258358561165714, - "angularVelocity": -0.15299842738252176, - "velocityX": -2.8350013105486314, - "velocityY": -1.1039771059498866, - "timestamp": 8.633950805584178 - }, - { - "x": 4.478218399750123, - "y": 5.968938070584376, - "heading": 0.1505712213556835, - "angularVelocity": -0.1224164741697909, - "velocityX": -2.2680010569772926, - "velocityY": -0.8831818099236476, - "timestamp": 8.73207782923389 - }, - { - "x": 4.311304254190931, - "y": 5.9039400626948755, - "heading": 0.1415612155720731, - "angularVelocity": -0.09181982137533895, - "velocityX": -1.701000798261533, - "velocityY": -0.6623864198869991, - "timestamp": 8.830204852883604 - }, - { - "x": 4.2000281561602835, - "y": 5.860608055094718, - "heading": 0.13555442251274583, - "angularVelocity": -0.06121446300837455, - "velocityX": -1.1340005422754125, - "velocityY": -0.4415909704429648, - "timestamp": 8.928331876533317 + "angularVelocity": 0.022858558766875252, + "velocityX": 0.2293179767139361, + "velocityY": 0.08929690453869228, + "timestamp": 7.357129411131235 + }, + { + "x": 8.023609935096944, + "y": 7.3495390353004035, + "heading": 0.24423184787745655, + "angularVelocity": -0.007595056263321516, + "velocityX": -0.33770268880325044, + "velocityY": -0.13150530389944523, + "timestamp": 7.455388398161973 + }, + { + "x": 7.934712736835736, + "y": 7.314921652906921, + "heading": 0.24049321600439108, + "angularVelocity": -0.03804875244537132, + "velocityX": -0.9047233331786764, + "velocityY": -0.3523075439669781, + "timestamp": 7.55364738519271 + }, + { + "x": 7.790100668647838, + "y": 7.258608460656236, + "heading": 0.2337619254421963, + "angularVelocity": -0.06850559694950989, + "velocityX": -1.4717439346556647, + "velocityY": -0.5731098391342927, + "timestamp": 7.651906372223448 + }, + { + "x": 7.589773737515311, + "y": 7.180599451109558, + "heading": 0.22403732916629288, + "angularVelocity": -0.09896902634322269, + "velocityX": -2.038764465074942, + "velocityY": -0.7939122100075828, + "timestamp": 7.7501653592541855 + }, + { + "x": 7.3337319550141755, + "y": 7.080894615571415, + "heading": 0.21131838994573934, + "angularVelocity": -0.12944301182928752, + "velocityX": -2.6057848776829022, + "velocityY": -1.0147146693762836, + "timestamp": 7.848424346284923 + }, + { + "x": 7.021975346220784, + "y": 6.959493947641481, + "heading": 0.19560361962226216, + "angularVelocity": -0.15993214257909347, + "velocityX": -3.1728050350841532, + "velocityY": -1.235517193882298, + "timestamp": 7.946683333315661 + }, + { + "x": 6.680460557220924, + "y": 6.826506302650429, + "heading": 0.19560361772383378, + "angularVelocity": -1.9320659091620353e-8, + "velocityX": -3.4756595739484712, + "velocityY": -1.3534400161224025, + "timestamp": 8.044942320346397 + }, + { + "x": 6.3389457682116035, + "y": 6.69351865768357, + "heading": 0.19560361582542005, + "angularVelocity": -1.9320509691565887e-8, + "velocityX": -3.4756595740447467, + "velocityY": -1.3534400158761875, + "timestamp": 8.143201307377135 + }, + { + "x": 5.997430979202282, + "y": 6.560531012716711, + "heading": 0.19560361392700626, + "angularVelocity": -1.9320510544397003e-8, + "velocityX": -3.4756595740447476, + "velocityY": -1.3534400158761857, + "timestamp": 8.241460294407872 + }, + { + "x": 5.655916190192961, + "y": 6.427543367749852, + "heading": 0.1956036120285926, + "angularVelocity": -1.932050909755801e-8, + "velocityX": -3.475659574044747, + "velocityY": -1.3534400158761868, + "timestamp": 8.33971928143861 + }, + { + "x": 5.314401401188735, + "y": 6.29455572276993, + "heading": 0.19560361013017105, + "angularVelocity": -1.932058966623249e-8, + "velocityX": -3.4756595739929064, + "velocityY": -1.353440016009134, + "timestamp": 8.437978268469347 + }, + { + "x": 4.980112469171968, + "y": 6.164380421516108, + "heading": 0.177592992488041, + "angularVelocity": -0.18329740806808847, + "velocityX": -3.4021206824796124, + "velocityY": -1.3248182704458449, + "timestamp": 8.536237255500085 + }, + { + "x": 4.701538350318157, + "y": 6.055900980257848, + "heading": 0.16258092336836558, + "angularVelocity": -0.15278062163392098, + "velocityX": -2.8351006586976912, + "velocityY": -1.10401546501111, + "timestamp": 8.634496242530822 + }, + { + "x": 4.478679054186577, + "y": 5.969117414975404, + "heading": 0.1505695910339925, + "angularVelocity": -0.12224156484145013, + "velocityX": -2.2680805376292548, + "velocityY": -0.8832124969423502, + "timestamp": 8.73275522956156 + }, + { + "x": 4.311534581449365, + "y": 5.904029734889891, + "heading": 0.14156038794424536, + "angularVelocity": -0.09168833673126392, + "velocityX": -1.701060409720345, + "velocityY": -0.6624094350285984, + "timestamp": 8.831014216592298 + }, + { + "x": 4.200104931923326, + "y": 5.860637945826706, + "heading": 0.1355541433245536, + "angularVelocity": -0.06112666943953846, + "velocityX": -1.1340402836758565, + "velocityY": -0.4416063138286828, + "timestamp": 8.929273203623035 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": -0.030606008971789937, - "velocityX": -0.5670002807557282, - "velocityY": -0.2207954888983711, - "timestamp": 9.02645890018303 + "angularVelocity": -0.03056206326518059, + "velocityX": -0.567020151599215, + "velocityY": -0.22080316059111527, + "timestamp": 9.027532190653773 }, { "x": 4.144390106201172, "y": 5.838942050933838, "heading": 0.13255114594654763, - "angularVelocity": 1.2638912367197548e-25, - "velocityX": -6.105559884328861e-26, - "velocityY": -2.4380402560227299e-26, - "timestamp": 9.124585923832743 - }, - { - "x": 4.149229218776726, - "y": 5.815100726386144, - "heading": 0.12918022174703217, - "angularVelocity": -0.05380546988265665, - "velocityX": 0.07724016042252432, - "velocityY": -0.3805465783235216, - "timestamp": 9.187236137776921 - }, - { - "x": 4.159717289478365, - "y": 5.7675956417955225, - "heading": 0.12259706469587767, - "angularVelocity": -0.10507796600695042, - "velocityX": 0.16740678189838681, - "velocityY": -0.7582589364012235, - "timestamp": 9.249886351721099 - }, - { - "x": 4.176867312970838, - "y": 5.696690246498273, - "heading": 0.11300853276979594, - "angularVelocity": -0.15304867010709985, - "velocityX": 0.2737424569332353, - "velocityY": -1.1317662116912757, - "timestamp": 9.312536565665276 - }, - { - "x": 4.201974040585745, - "y": 5.602790592825652, - "heading": 0.10069220099175857, - "angularVelocity": -0.19658882233046115, - "velocityX": 0.40074448328743983, - "velocityY": -1.498792226891454, - "timestamp": 9.375186779609454 - }, - { - "x": 4.2367303910144685, - "y": 5.486551757326023, - "heading": 0.08603276386217494, - "angularVelocity": -0.2339886204162901, - "velocityX": 0.5547682639949261, - "velocityY": -1.8553621477366333, - "timestamp": 9.437836993553631 - }, - { - "x": 4.283393708275681, - "y": 5.349087774330418, - "heading": 0.06958159023755035, - "angularVelocity": -0.2625876687234105, - "velocityX": 0.7448229514872903, - "velocityY": -2.1941502565033004, - "timestamp": 9.500487207497809 - }, - { - "x": 4.3449830971903305, - "y": 5.19239819137041, - "heading": 0.05215018239550137, - "angularVelocity": -0.2782338119001568, - "velocityX": 0.9830674955001105, - "velocityY": -2.5010223125434243, - "timestamp": 9.563137421441986 - }, - { - "x": 4.425324635342363, - "y": 5.020190178284826, - "heading": 0.03492497946542863, - "angularVelocity": -0.2749424438585412, - "velocityX": 1.2823825026937377, - "velocityY": -2.7487218677181704, - "timestamp": 9.625787635386164 - }, - { - "x": 4.528280141254807, - "y": 4.838988810812342, - "heading": 0.019478906624101934, - "angularVelocity": -0.24654461443801812, - "velocityX": 1.6433384569792095, - "velocityY": -2.8922705297373237, - "timestamp": 9.688437849330342 - }, - { - "x": 4.6555610459377, - "y": 4.65789803685685, - "heading": 0.007446897298019998, - "angularVelocity": -0.1920505704386356, - "velocityX": 2.0316116525364616, - "velocityY": -2.8905052761167873, - "timestamp": 9.75108806327452 + "angularVelocity": 5.970204758228253e-26, + "velocityX": -1.332775736754173e-26, + "velocityY": -2.1157650772928713e-26, + "timestamp": 9.12579117768451 + }, + { + "x": 4.149233388012969, + "y": 5.815110549500247, + "heading": 0.12920898299918354, + "angularVelocity": -0.053321361684181494, + "velocityX": 0.07727043393529022, + "velocityY": -0.38021129652575064, + "timestamp": 9.188470802544044 + }, + { + "x": 4.15973054187138, + "y": 5.767625542791529, + "heading": 0.12267986707327648, + "angularVelocity": -0.10416648058342459, + "velocityX": 0.16747314429426893, + "velocityY": -0.7575828160288474, + "timestamp": 9.251150427403578 + }, + { + "x": 4.176895053438104, + "y": 5.696750993270707, + "heading": 0.11316614819920229, + "angularVelocity": -0.1517832771876739, + "velocityX": 0.27384515470838755, + "velocityY": -1.13074303937925, + "timestamp": 9.313830052263112 + }, + { + "x": 4.20202157406695, + "y": 5.602893467398182, + "heading": 0.10093938344413095, + "angularVelocity": -0.19506761220207594, + "velocityX": 0.4008722241901565, + "velocityY": -1.4974168413237998, + "timestamp": 9.376509677122646 + }, + { + "x": 4.236801648856746, + "y": 5.486708244559792, + "heading": 0.08637594484329676, + "angularVelocity": -0.23234725213290444, + "velocityX": 0.5548864542144063, + "velocityY": -1.8536362190865294, + "timestamp": 9.43918930198218 + }, + { + "x": 4.283488644505547, + "y": 5.349308215099438, + "heading": 0.07001524095922855, + "angularVelocity": -0.2610210881244517, + "velocityX": 0.7448512296208976, + "velocityY": -2.1921003798007317, + "timestamp": 9.501868926841714 + }, + { + "x": 4.3450929137179735, + "y": 5.192687240127482, + "heading": 0.052651049006290945, + "angularVelocity": -0.2770308851058188, + "velocityX": 0.9828436170523044, + "velocityY": -2.4987541856375906, + "timestamp": 9.564548551701249 + }, + { + "x": 4.4254260370024205, + "y": 5.02053381378003, + "heading": 0.035443983400084915, + "angularVelocity": -0.2745240681444267, + "velocityX": 1.2816465233235492, + "velocityY": -2.7465612108120876, + "timestamp": 9.627228176560783 + }, + { + "x": 4.52834083069929, + "y": 4.839329489599335, + "heading": 0.019934884277187374, + "angularVelocity": -0.24743445988474677, + "velocityX": 1.6419178309937554, + "velocityY": -2.8909605727024488, + "timestamp": 9.689907801420317 + }, + { + "x": 4.655573182869098, + "y": 4.658127319631667, + "heading": 0.0077322780217831035, + "angularVelocity": -0.19468218392739808, + "velocityX": 2.029883753371783, + "velocityY": -2.890926204069373, + "timestamp": 9.75258742627985 }, { "x": 4.805556774139404, "y": 4.485479354858398, - "heading": -3.9652014929192097e-25, - "angularVelocity": -0.1188646746626479, - "velocityX": 2.3941774298704206, - "velocityY": -2.7520844885235216, - "timestamp": 9.813738277218697 - }, - { - "x": 5.026556371976755, - "y": 4.290478604211026, - "heading": -0.0011711585949671858, - "angularVelocity": -0.014781576411141743, - "velocityX": 2.7893083450033753, - "velocityY": -2.4611683748987776, - "timestamp": 9.892969244362662 - }, - { - "x": 5.271648004487175, - "y": 4.126055401914557, - "heading": -0.0011711658442509346, - "angularVelocity": -9.149558575332566e-8, - "velocityX": 3.0933818094770933, - "velocityY": -2.075239117019822, - "timestamp": 9.972200211506628 - }, - { - "x": 5.536248278847656, - "y": 3.995320554415454, - "heading": -0.0011711670074400294, - "angularVelocity": -1.4680990736292476e-8, - "velocityX": 3.3396067711718374, - "velocityY": -1.6500473515810268, - "timestamp": 10.051431178650594 - }, - { - "x": 5.8157535418727955, - "y": 3.900548632610079, - "heading": -0.001171167882288204, - "angularVelocity": -1.1041745495501586e-8, - "velocityX": 3.5277275174146094, - "velocityY": -1.19614748149131, - "timestamp": 10.13066214579456 - }, - { - "x": 6.105300821187418, - "y": 3.843388468655785, - "heading": -0.001171168612334034, - "angularVelocity": -9.214147655131007e-9, - "velocityX": 3.654471095733389, - "velocityY": -0.7214371604278409, - "timestamp": 10.209893112938525 - }, - { - "x": 6.399852439542981, - "y": 3.8248343191469996, - "heading": -0.0011711692753000216, - "angularVelocity": -8.367510982006514e-9, - "velocityX": 3.717632498671272, - "velocityY": -0.23417800107213171, - "timestamp": 10.289124080082491 - }, - { - "x": 6.694725372224158, - "y": 3.837278843369643, - "heading": -0.0011711699217371897, - "angularVelocity": -8.158895331165977e-9, - "velocityX": 3.721687912068311, - "velocityY": 0.15706641823558024, - "timestamp": 10.368355047226457 - }, - { - "x": 6.989598240261493, - "y": 3.849724899233505, - "heading": -0.001171170568174097, - "angularVelocity": -8.158892044748454e-9, - "velocityX": 3.721687096177193, - "velocityY": 0.15708574958131358, - "timestamp": 10.447586014370422 - }, - { - "x": 7.2831382115989305, - "y": 3.8803709956962793, - "heading": -0.0011711735014437953, - "angularVelocity": -3.702175808154048e-8, - "velocityX": 3.7048641701427742, - "velocityY": 0.38679442606183717, - "timestamp": 10.526816981514388 - }, - { - "x": 7.537795284833571, - "y": 3.9128988141490195, - "heading": -0.0022060584163901163, - "angularVelocity": -0.013061621639249952, - "velocityX": 3.2141103714147587, - "velocityY": 0.4105442559300826, - "timestamp": 10.606047948658354 - }, - { - "x": 7.7535280379166585, - "y": 3.9448039763833895, - "heading": -0.0030479668047439287, - "angularVelocity": -0.010626001659477862, - "velocityX": 2.7228337714355204, - "velocityY": 0.40268550775604034, - "timestamp": 10.68527891580232 - }, - { - "x": 7.9303456416380165, - "y": 3.9756628799518183, - "heading": -0.0034891865807647085, - "angularVelocity": -0.005568779379141843, - "velocityX": 2.2316729189998985, - "velocityY": 0.38948033427835, - "timestamp": 10.764509882946285 - }, - { - "x": 8.06825324660345, - "y": 4.0053006852761595, - "heading": -0.003443969014778176, - "angularVelocity": 0.000570705717934386, - "velocityX": 1.7405770740479534, - "velocityY": 0.374068453190637, - "timestamp": 10.843740850090251 - }, - { - "x": 8.167254001972681, - "y": 4.03362192898922, - "heading": -0.002865492528698777, - "angularVelocity": 0.007301141295275209, - "velocityX": 1.2495209756728547, - "velocityY": 0.3574516976625985, - "timestamp": 10.922971817234217 - }, - { - "x": 8.22735001387898, - "y": 4.060566470670497, - "heading": -0.0017242628169884097, - "angularVelocity": 0.014403834167980214, - "velocityX": 0.7584914594958928, - "velocityY": 0.34007589017963685, - "timestamp": 11.002202784378182 + "heading": -5.922112096142328e-27, + "angularVelocity": -0.1233619065064789, + "velocityX": 2.392860384956347, + "velocityY": -2.754451149957796, + "timestamp": 9.815267051139385 + }, + { + "x": 5.0260464642582185, + "y": 4.290440402432839, + "heading": -0.0017206330074737138, + "angularVelocity": -0.02175742700503041, + "velocityX": 2.7880950308894734, + "velocityY": -2.4662701180928828, + "timestamp": 9.894349610261651 + }, + { + "x": 5.270702243030263, + "y": 4.125667987076678, + "heading": -0.001720643347085146, + "angularVelocity": -1.3074452252065364e-7, + "velocityX": 3.0936755396824163, + "velocityY": -2.0835493588594303, + "timestamp": 9.973432169383917 + }, + { + "x": 5.534838657282035, + "y": 3.9943730439929475, + "heading": -0.0017206445243311077, + "angularVelocity": -1.4886290666410382e-8, + "velocityX": 3.3400084314848875, + "velocityY": -1.660226281761329, + "timestamp": 10.052514728506184 + }, + { + "x": 5.813901235574451, + "y": 3.898819454109309, + "heading": -0.001720645408427364, + "angularVelocity": -1.1179408784311949e-8, + "velocityX": 3.528749972050966, + "velocityY": -1.2082764005639604, + "timestamp": 10.13159728762845 + }, + { + "x": 6.10307814292353, + "y": 3.8406547782709533, + "heading": -0.0017206461449007905, + "angularVelocity": -9.31271616103322e-9, + "velocityX": 3.656645795971196, + "velocityY": -0.7354930908144911, + "timestamp": 10.210679846750716 + }, + { + "x": 6.397383154789235, + "y": 3.8208817077449715, + "heading": -0.0017206468122835479, + "angularVelocity": -8.439063742592553e-9, + "velocityX": 3.721490745016124, + "velocityY": -0.25003073680773497, + "timestamp": 10.289762405872983 + }, + { + "x": 6.692083031584466, + "y": 3.833467304039097, + "heading": -0.0017206474619802302, + "angularVelocity": -8.215423090919723e-9, + "velocityX": 3.7264838172422694, + "velocityY": 0.1591450306339672, + "timestamp": 10.36884496499525 + }, + { + "x": 6.9867828265259435, + "y": 3.8460548168458266, + "heading": -0.0017206481116765608, + "angularVelocity": -8.21541864124069e-9, + "velocityX": 3.726482782200477, + "velocityY": 0.15916926496104583, + "timestamp": 10.447927524117516 + }, + { + "x": 7.280008906046239, + "y": 3.8780682629545473, + "heading": -0.0017206520775067291, + "angularVelocity": -5.014797462981314e-8, + "velocityX": 3.7078476313209716, + "velocityY": 0.40481044700672053, + "timestamp": 10.527010083239782 + }, + { + "x": 7.534517790856317, + "y": 3.9111592304770704, + "heading": -0.0026190122393155452, + "angularVelocity": -0.011359776059091569, + "velocityX": 3.218268195097111, + "velocityY": 0.41843571945316527, + "timestamp": 10.606092642362048 + }, + { + "x": 7.750301298941706, + "y": 3.943488528624649, + "heading": -0.0033468758613748313, + "angularVelocity": -0.009203845072008415, + "velocityX": 2.728585297192714, + "velocityY": 0.40880440019139475, + "timestamp": 10.685175201484315 + }, + { + "x": 7.927368234125033, + "y": 3.974697332887919, + "heading": -0.003695626110523714, + "angularVelocity": -0.004409951486391499, + "velocityX": 2.239013723741158, + "velocityY": 0.39463574029034193, + "timestamp": 10.764257760606581 + }, + { + "x": 8.065723377902886, + "y": 4.004632819289759, + "heading": -0.0035763939938957814, + "angularVelocity": 0.0015076916826071924, + "velocityX": 1.7495026123768658, + "velocityY": 0.37853461918901893, + "timestamp": 10.843340319728847 + }, + { + "x": 8.165369644244953, + "y": 4.03321033626126, + "heading": -0.0029399507835581986, + "angularVelocity": 0.008047832763651503, + "velocityX": 1.2600283481975725, + "velocityY": 0.36136307788571737, + "timestamp": 10.922422878851114 + }, + { + "x": 8.226308982581841, + "y": 4.060376108268979, + "heading": -0.0017550259023952847, + "angularVelocity": 0.014983390703517166, + "velocityX": 0.7705787345939596, + "velocityY": 0.34351154425489977, + "timestamp": 11.00150543797338 }, { "x": 8.248542785644531, "y": 4.086092948913574, - "heading": 1.4182707321197947e-24, - "angularVelocity": 0.021762486047347675, - "velocityX": 0.26748091724090983, - "velocityY": 0.32217804683230217, - "timestamp": 11.081433751522148 - }, - { - "x": 8.23152301234386, - "y": 4.109892958457591, - "heading": 0.0022863134325125296, - "angularVelocity": 0.02921520350070526, - "velocityX": -0.21748380315840005, - "velocityY": 0.30412370948765427, - "timestamp": 11.159691410499514 - }, - { - "x": 8.176551036095818, - "y": 4.132280075994518, - "heading": 0.005155847292928311, - "angularVelocity": 0.03666777025933954, - "velocityX": -0.7024485138757267, - "velocityY": 0.2860693487317698, - "timestamp": 11.23794906947688 - }, - { - "x": 8.0836268579111, - "y": 4.153254299063441, - "heading": 0.008608587112944467, - "angularVelocity": 0.044120152137630285, - "velocityX": -1.1874132116780383, - "velocityY": 0.2680149565295516, - "timestamp": 11.316206728454246 - }, - { - "x": 7.952750479206832, - "y": 4.172815624183264, - "heading": 0.012644515987393022, - "angularVelocity": 0.05157231799657927, - "velocityX": -1.672377891371887, - "velocityY": 0.24996051984483794, - "timestamp": 11.394464387431611 - }, - { - "x": 7.783921902112923, - "y": 4.190964046074257, - "heading": 0.01726361439792771, - "angularVelocity": 0.05902423444420523, - "velocityX": -2.157342543849138, - "velocityY": 0.2319060156941693, - "timestamp": 11.472722046408977 - }, - { - "x": 7.577141130185825, - "y": 4.207699555855839, - "heading": 0.022465858708007803, - "angularVelocity": 0.06647584885697558, - "velocityX": -2.642307150880985, - "velocityY": 0.2138513980647186, - "timestamp": 11.550979705386343 - }, - { - "x": 7.3324081705466435, - "y": 4.2230221357012825, - "heading": 0.028251214753927917, - "angularVelocity": 0.0739270267156012, - "velocityX": -3.127271666917177, - "velocityY": 0.19579655263996953, - "timestamp": 11.629237364363709 - }, - { - "x": 7.049723044551423, - "y": 4.236931732641069, - "heading": 0.03461960291638085, - "angularVelocity": 0.08137718717467465, - "velocityX": -3.612235910059377, - "velocityY": 0.17774103035473854, - "timestamp": 11.707495023341075 - }, - { - "x": 6.758414971904043, - "y": 4.226087741393719, - "heading": 0.03461960351163466, - "angularVelocity": 7.606332865983503e-9, - "velocityX": -3.722422526485686, - "velocityY": -0.13856779501270336, - "timestamp": 11.78575268231844 - }, - { - "x": 6.467106926605879, - "y": 4.215243015473276, - "heading": 0.03461960410686983, - "angularVelocity": 7.606094882154123e-9, - "velocityX": -3.7224221770091335, - "velocityY": -0.13857718288735063, - "timestamp": 11.864010341295806 - }, - { - "x": 6.175798881305754, - "y": 4.204398289605525, - "heading": 0.03461960470210498, - "angularVelocity": 7.606094432841078e-9, - "velocityX": -3.7224221770341983, - "velocityY": -0.13857718221405538, - "timestamp": 11.942268000273172 - }, - { - "x": 5.884490683895451, - "y": 4.1935576504515195, - "heading": 0.03461960529734107, - "angularVelocity": 7.606106615779635e-9, - "velocityX": -3.7224241207439825, - "velocityY": -0.13852496095162967, - "timestamp": 12.020525659250538 - }, - { - "x": 5.594009037113431, - "y": 4.2180198226312315, - "heading": 0.034619605911984125, - "angularVelocity": 7.854094508972923e-9, - "velocityX": -3.7118622071998786, - "velocityY": 0.31258502361778023, - "timestamp": 12.098783318227904 - }, - { - "x": 5.309172831526297, - "y": 4.280038516789027, - "heading": 0.03461959995315513, - "angularVelocity": -7.614371636753198e-8, - "velocityX": -3.6397230547046773, - "velocityY": 0.7924936034150152, - "timestamp": 12.17704097720527 - }, - { - "x": 5.03935943827081, - "y": 4.37675512978584, - "heading": 0.01971114929844739, - "angularVelocity": -0.1905046847749395, - "velocityX": -3.447757021884906, - "velocityY": 1.235874089011348, - "timestamp": 12.255298636182635 + "heading": 2.0912283688050628e-26, + "angularVelocity": 0.022192325613564278, + "velocityX": 0.2811467320918988, + "velocityY": 0.3251897881154181, + "timestamp": 11.080587997095646 + }, + { + "x": 8.232549205371914, + "y": 4.110135442061636, + "heading": 0.002315542172399013, + "angularVelocity": 0.029537040889687716, + "velocityX": -0.2040140059273348, + "velocityY": 0.3066858862123803, + "timestamp": 11.158982518098847 + }, + { + "x": 8.1785216821941, + "y": 4.132727328793288, + "heading": 0.00520685837502888, + "angularVelocity": 0.03688161067419279, + "velocityX": -0.6891747342344035, + "velocityY": 0.28818196020011727, + "timestamp": 11.237377039102048 + }, + { + "x": 8.086460217126783, + "y": 4.153868606569999, + "heading": 0.008673934737020163, + "angularVelocity": 0.044226003521977544, + "velocityX": -1.1743354495852854, + "velocityY": 0.26967800180637214, + "timestamp": 11.315771560105249 + }, + { + "x": 7.956364811594128, + "y": 4.173559271802363, + "heading": 0.012716755269955655, + "angularVelocity": 0.05157019242161638, + "velocityX": -1.6594961467695446, + "velocityY": 0.25117399762619275, + "timestamp": 11.39416608110845 + }, + { + "x": 7.788235467736609, + "y": 4.191799319049532, + "heading": 0.01733530192089736, + "angularVelocity": 0.0589141510380956, + "velocityX": -2.14465681664992, + "velocityY": 0.23266992404257889, + "timestamp": 11.47256060211165 + }, + { + "x": 7.582072189128265, + "y": 4.208588739164989, + "heading": 0.022529553625480057, + "angularVelocity": 0.06625784095766901, + "velocityX": -2.6298174409398722, + "velocityY": 0.21416573378605752, + "timestamp": 11.550955123114852 + }, + { + "x": 7.337874982925346, + "y": 4.223927513793021, + "heading": 0.02829948161506035, + "angularVelocity": 0.07360116390461449, + "velocityX": -3.1149779739447685, + "velocityY": 0.1956613093841805, + "timestamp": 11.629349644118053 + }, + { + "x": 7.055643870589831, + "y": 4.237815588350764, + "heading": 0.034645023037832345, + "angularVelocity": 0.08094368511433264, + "velocityX": -3.600138233180768, + "velocityY": 0.17715618872364539, + "timestamp": 11.707744165121253 + }, + { + "x": 6.763454078794012, + "y": 4.226672459763479, + "heading": 0.03464502362353886, + "angularVelocity": 7.47126845639964e-9, + "velocityX": -3.7271710836002327, + "velocityY": -0.1421416757789859, + "timestamp": 11.786138686124454 + }, + { + "x": 6.471264316590495, + "y": 4.215528555246823, + "heading": 0.03464502420922547, + "angularVelocity": 7.471014571114214e-9, + "velocityX": -3.7271707061210075, + "velocityY": -0.14215157352898217, + "timestamp": 11.864533207127655 + }, + { + "x": 6.1790745543846715, + "y": 4.2043846507906375, + "heading": 0.034645024794912035, + "angularVelocity": 7.471014019515784e-9, + "velocityX": -3.727170706150427, + "velocityY": -0.1421515727576172, + "timestamp": 11.942927728130856 + }, + { + "x": 5.8868846155421455, + "y": 4.193245378659894, + "heading": 0.034645025380599624, + "angularVelocity": 7.471027140146409e-9, + "velocityX": -3.727172959327064, + "velocityY": -0.1420924828444054, + "timestamp": 12.021322249134057 + }, + { + "x": 5.595514516394948, + "y": 4.2177914315394665, + "heading": 0.034645025985590286, + "angularVelocity": 7.717256902963699e-9, + "velocityX": -3.716715089506096, + "velocityY": 0.3131092908721317, + "timestamp": 12.099716770137258 + }, + { + "x": 5.309801234566853, + "y": 4.279976327218559, + "heading": 0.03464501988589696, + "angularVelocity": -7.78076483461541e-8, + "velocityX": -3.6445567645783616, + "velocityY": 0.7932301248011192, + "timestamp": 12.178111291140459 + }, + { + "x": 5.039631118107501, + "y": 4.376720178141923, + "heading": 0.01972472706533464, + "angularVelocity": -0.19032315817011253, + "velocityX": -3.446288248235145, + "velocityY": 1.2340639331084704, + "timestamp": 12.25650581214366 }, { "x": 4.805556774139404, "y": 4.485479354858398, - "heading": -1.4078898091571754e-24, - "angularVelocity": -0.25187501844577836, - "velocityX": -2.9876010500011123, - "velocityY": 1.3893109823794343, - "timestamp": 12.333556295160001 - }, - { - "x": 4.62745788226169, - "y": 4.587679677136332, - "heading": -0.019778853338814184, - "angularVelocity": -0.28487106556518027, - "velocityX": -2.5651244910957125, - "velocityY": 1.47197181806725, - "timestamp": 12.402987191202648 - }, - { - "x": 4.478940134439646, - "y": 4.685562428002014, - "heading": -0.039416750418752866, - "angularVelocity": -0.2828408993580651, - "velocityX": -2.139072895311886, - "velocityY": 1.4097866575934404, - "timestamp": 12.472418087245295 - }, - { - "x": 4.358424662792702, - "y": 4.772986561558442, - "heading": -0.05736759927872339, - "angularVelocity": -0.2585426644781364, - "velocityX": -1.7357614335398783, - "velocityY": 1.2591531802027822, - "timestamp": 12.541848983287942 - }, - { - "x": 4.264251963016928, - "y": 4.846292896298889, - "heading": -0.07266630938107123, - "angularVelocity": -0.22034441400483662, - "velocityX": -1.3563514968599637, - "velocityY": 1.0558172070171734, - "timestamp": 12.611279879330588 - }, - { - "x": 4.195046678979309, - "y": 4.903178017800908, - "heading": -0.08468297645986948, - "angularVelocity": -0.17307377210596744, - "velocityX": -0.9967505531703128, - "velocityY": 0.8193055936809087, - "timestamp": 12.680710775373235 - }, - { - "x": 4.149718350868284, - "y": 4.942098244876491, - "heading": -0.09298341563380762, - "angularVelocity": -0.11954964788067375, - "velocityX": -0.6528552948990165, - "velocityY": 0.5605606335784191, - "timestamp": 12.750141671415882 + "heading": -2.403769428999081e-26, + "angularVelocity": -0.2516084901460064, + "velocityX": -2.9858508091214753, + "velocityY": 1.3873313507717573, + "timestamp": 12.33490033314686 + }, + { + "x": 4.627436042049824, + "y": 4.587623100442906, + "heading": -0.019769634491549008, + "angularVelocity": -0.2845427298125247, + "velocityX": -2.5636771062530377, + "velocityY": 1.4701465631201296, + "timestamp": 12.40437894714826 + }, + { + "x": 4.478903462198178, + "y": 4.685483985234687, + "heading": -0.03940302192904774, + "angularVelocity": -0.28258173712422013, + "velocityX": -2.137817254798061, + "velocityY": 1.4085036985598087, + "timestamp": 12.473857561149659 + }, + { + "x": 4.358385068299864, + "y": 4.77291258513252, + "heading": -0.057354073333619894, + "angularVelocity": -0.2583680124104157, + "velocityX": -1.734611371146319, + "velocityY": 1.2583526766390536, + "timestamp": 12.543336175151058 + }, + { + "x": 4.264219265878193, + "y": 4.846237576154262, + "heading": -0.07265588839993739, + "angularVelocity": -0.22023777080540505, + "velocityX": -1.355320680688504, + "velocityY": 1.0553605893788425, + "timestamp": 12.612814789152457 + }, + { + "x": 4.195026094794195, + "y": 4.903145738441645, + "heading": -0.08467676120352872, + "angularVelocity": -0.17301543757549837, + "velocityX": -0.9958916434718147, + "velocityY": 0.8190745181853494, + "timestamp": 12.682293403153857 + }, + { + "x": 4.149710172245756, + "y": 4.942086169590921, + "heading": -0.09298105070953734, + "angularVelocity": -0.11952295861632048, + "velocityX": -0.6522283611979665, + "velocityY": 0.5604664357364799, + "timestamp": 12.751772017155256 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": -0.06149221603257356, - "velocityX": -0.32142062647019676, - "velocityY": 0.2860902346288004, - "timestamp": 12.819572567458529 + "angularVelocity": -0.061484021298608874, + "velocityX": -0.3210821603240091, + "velocityY": 0.2860675462596191, + "timestamp": 12.821250631156655 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": -9.169895235461756e-26, - "velocityX": -3.1744204454242983e-26, - "velocityY": -4.8447793914370994e-26, - "timestamp": 12.889003463501176 + "angularVelocity": -1.5452872782226126e-27, + "velocityX": -2.8121806375160504e-27, + "velocityY": 1.64906391853344e-27, + "timestamp": 12.890729245158054 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceLanePGFE.1.traj b/src/main/deploy/choreo/SourceLanePGFE.1.traj index c2e1f5e0..5bc0b1e7 100644 --- a/src/main/deploy/choreo/SourceLanePGFE.1.traj +++ b/src/main/deploy/choreo/SourceLanePGFE.1.traj @@ -3,227 +3,227 @@ { "x": 0.387, "y": 4.121134281158447, - "heading": 1.1679855597934721e-27, - "angularVelocity": 5.433585185667626e-28, - "velocityX": 7.442598398670663e-27, - "velocityY": 2.3270251613663043e-26, + "heading": 3.980405256540069e-28, + "angularVelocity": 1.0894029802546308e-28, + "velocityX": -2.179791595178488e-28, + "velocityY": 6.106168293685067e-28, "timestamp": 0 }, { - "x": 0.4005104003556791, - "y": 4.115348381940823, - "heading": -0.015277315995294262, - "angularVelocity": -0.30891158633586036, - "velocityX": 0.2731840597648779, - "velocityY": -0.11699249437834791, - "timestamp": 0.04945530265311637 - }, - { - "x": 0.4275520628000622, - "y": 4.103759116150694, - "heading": -0.04541480648332661, - "angularVelocity": -0.6093884552566431, - "velocityX": 0.5467899495844878, - "velocityY": -0.23433818353953934, - "timestamp": 0.09891060530623275 - }, - { - "x": 0.4681490763693427, - "y": 4.086345867978677, - "heading": -0.08991129386234921, - "angularVelocity": -0.899731373420656, - "velocityX": 0.8208829264281612, - "velocityY": -0.35210073011087883, - "timestamp": 0.14836590795934912 - }, - { - "x": 0.5223311730580589, - "y": 4.063083084813811, - "heading": -0.14811250055399447, - "angularVelocity": -1.176844616640472, - "velocityX": 1.0955770924859953, - "velocityY": -0.47037995759592216, - "timestamp": 0.1978212106124655 - }, - { - "x": 0.5901369333154594, - "y": 4.033939247984653, - "heading": -0.2191152852440036, - "angularVelocity": -1.435696090832334, - "velocityX": 1.3710513659777948, - "velocityY": -0.5892965013999555, - "timestamp": 0.24727651326558187 - }, - { - "x": 0.6716171763310703, - "y": 3.9988769782713436, - "heading": -0.3016414776380596, - "angularVelocity": -1.6687026055204155, - "velocityX": 1.647553217642207, - "velocityY": -0.708968863445023, - "timestamp": 0.29673181591869824 - }, - { - "x": 0.7668367655706729, - "y": 3.9578537486428593, - "heading": -0.39387111054062807, - "angularVelocity": -1.8649088763944044, - "velocityX": 1.9253666266584337, - "velocityY": -0.8295011339072073, - "timestamp": 0.3461871185718146 - }, - { - "x": 0.8758711660113281, - "y": 3.91082124740519, - "heading": -0.49317423957558376, - "angularVelocity": -2.00793693916861, - "velocityX": 2.2047059585385944, - "velocityY": -0.9510102802839819, - "timestamp": 0.395642421224931 - }, - { - "x": 0.9987875307433454, - "y": 3.857723681335083, - "heading": -0.5954683580210642, - "angularVelocity": -2.068415578466479, - "velocityX": 2.4854031446165235, - "velocityY": -1.0736475811812884, - "timestamp": 0.44509772387804736 - }, - { - "x": 1.1355416361286799, - "y": 3.7985302060231096, - "heading": -0.6929936795383219, - "angularVelocity": -1.9719891757878503, - "velocityX": 2.765206116410594, - "velocityY": -1.1969085646318132, - "timestamp": 0.49455302653116373 - }, - { - "x": 1.2843691968945923, - "y": 3.733334425660418, - "heading": -0.76082235123736, - "angularVelocity": -1.3715146417118123, - "velocityX": 3.0093347483848483, - "velocityY": -1.3182768452551967, - "timestamp": 0.5440083291842801 - }, - { - "x": 1.4440686661947881, - "y": 3.662157825631026, - "heading": -0.7899998870158123, - "angularVelocity": -0.5899779035446591, - "velocityX": 3.2291677683248934, - "velocityY": -1.439210685427017, - "timestamp": 0.5934636318373965 - }, - { - "x": 1.613363599943702, - "y": 3.5895175490832267, - "heading": -0.789999943508809, - "angularVelocity": -0.0000011423041349866365, - "velocityX": 3.423190733183105, - "velocityY": -1.468806632471835, - "timestamp": 0.6429189344905129 + "x": 0.40050242656101365, + "y": 4.1153504673922905, + "heading": -0.015208632237685107, + "angularVelocity": -0.3074428187596394, + "velocityX": 0.2729518353219713, + "velocityY": -0.11691991624610248, + "timestamp": 0.04946816549185787 + }, + { + "x": 0.42752783346651274, + "y": 4.103765611590411, + "heading": -0.045213195112198845, + "angularVelocity": -0.6065428660266832, + "velocityX": 0.5463191658066905, + "velocityY": -0.23418810232180645, + "timestamp": 0.09893633098371574 + }, + { + "x": 0.46809995023859424, + "y": 4.086359376332899, + "heading": -0.08951802911831011, + "angularVelocity": -0.8956231460292083, + "velocityX": 0.8201661890768802, + "velocityY": -0.35186740976632624, + "timestamp": 0.1484044964755736 + }, + { + "x": 0.5222480472443771, + "y": 4.063106542737477, + "heading": -0.14747661029050188, + "angularVelocity": -1.171633930547341, + "velocityX": 1.094604913430544, + "velocityY": -0.47005651744352356, + "timestamp": 0.19787266196743147 + }, + { + "x": 0.5900100832649373, + "y": 4.033975964464713, + "heading": -0.21819722216411808, + "angularVelocity": -1.4296186480830055, + "velocityX": 1.3698109753375314, + "velocityY": -0.5888752490236961, + "timestamp": 0.24734082745928934 + }, + { + "x": 0.6714360586881531, + "y": 3.998930617330974, + "heading": -0.3004183343874937, + "angularVelocity": -1.6621015031759903, + "velocityX": 1.64602779613119, + "velocityY": -0.7084424252503851, + "timestamp": 0.2968089929511472 + }, + { + "x": 0.7665898631710647, + "y": 3.95792823980715, + "heading": -0.3923434473802076, + "angularVelocity": -1.8582680816785926, + "velocityX": 1.9235361476781112, + "velocityY": -0.8288639191718736, + "timestamp": 0.3462771584430051 + }, + { + "x": 0.8755460489765932, + "y": 3.910920647028603, + "heading": -0.4913759605059717, + "angularVelocity": -2.0019443240130714, + "velocityX": 2.202551574779177, + "velocityY": -0.9502594711397636, + "timestamp": 0.39574532393486295 + }, + { + "x": 0.998371594898006, + "y": 3.857851824015994, + "heading": -0.593488171000786, + "angularVelocity": -2.064200470737515, + "velocityX": 2.482920979586945, + "velocityY": -1.072787367086499, + "timestamp": 0.4452134894267208 + }, + { + "x": 1.1350267865800456, + "y": 3.7986882165536997, + "heading": -0.6910493311323551, + "angularVelocity": -1.972200892463397, + "velocityX": 2.7624875578725923, + "velocityY": -1.1959935622037938, + "timestamp": 0.49468165491857863 + }, + { + "x": 1.2838522334103695, + "y": 3.7335275480002674, + "heading": -0.7598417160155273, + "angularVelocity": -1.39063949914405, + "velocityX": 3.008509520225074, + "velocityY": -1.3172242775842857, + "timestamp": 0.5441498204104365 + }, + { + "x": 1.4435365539100036, + "y": 3.6623850951483443, + "heading": -0.7899998876377602, + "angularVelocity": -0.6096480700744137, + "velocityX": 3.2280218785537347, + "velocityY": -1.4381461722819, + "timestamp": 0.5936179859022943 + }, + { + "x": 1.6130975326130776, + "y": 3.5896311577651607, + "heading": -0.7899999438199378, + "angularVelocity": -0.000001135723893796383, + "velocityX": 3.427678730697678, + "velocityY": -1.4707223657840804, + "timestamp": 0.6430861513941522 }, { "x": 1.7826586961746216, "y": 3.5168776512145996, "heading": -0.79, - "angularVelocity": -0.0000011422676237323725, - "velocityX": 3.423194018614537, - "velocityY": -1.468798975473458, - "timestamp": 0.6923742371436292 + "angularVelocity": -0.0000011356811339822919, + "velocityX": 3.427682467615514, + "velocityY": -1.4707136564936016, + "timestamp": 0.69255431688601 }, { - "x": 2.0239723336710425, - "y": 3.4134441767348855, + "x": 2.0243963751949394, + "y": 3.413262421214484, "heading": -0.79, - "angularVelocity": -4.2462946476416713e-16, - "velocityX": 3.4237473592821535, - "velocityY": -1.467509622685751, - "timestamp": 0.7628565514540567 + "angularVelocity": -4.062908416081592e-16, + "velocityX": 3.4282324773027724, + "velocityY": -1.4694320640008167, + "timestamp": 0.7630681109827596 }, { - "x": 2.250495583067251, - "y": 3.3163502598603043, + "x": 2.2508253931606723, + "y": 3.316208894447284, "heading": -0.79, - "angularVelocity": 2.583795012505271e-16, - "velocityX": 3.2139019782824456, - "velocityY": -1.37756425600538, - "timestamp": 0.8333388657644842 + "angularVelocity": -3.3647602923838933e-16, + "velocityX": 3.2111308271833634, + "velocityY": -1.3763764666249076, + "timestamp": 0.8335819050795092 }, { - "x": 2.4487034476982825, - "y": 3.23139307341843, + "x": 2.4489508052764966, + "y": 3.2312870493551733, "heading": -0.79, - "angularVelocity": 2.1975738712772502e-16, - "velocityX": 2.812164534752054, - "velocityY": -1.2053688542021173, - "timestamp": 0.9038211800749117 + "angularVelocity": -3.271122096502082e-16, + "velocityX": 2.8097397772127017, + "velocityY": -1.2043295383537609, + "timestamp": 0.9040956991762589 }, { - "x": 2.6185959113515174, - "y": 3.1585726243584236, + "x": 2.6187725953400465, + "y": 3.158496892882915, "heading": -0.79, - "angularVelocity": 1.8378454574977131e-16, - "velocityX": 2.4104268611977178, - "velocityY": -1.0331733538044932, - "timestamp": 0.9743034943853393 + "angularVelocity": -2.53261943200864e-16, + "velocityX": 2.4083484974662293, + "velocityY": -1.0322825115946104, + "timestamp": 0.9746094932730085 }, { - "x": 2.760172968622745, - "y": 3.0978889149966715, + "x": 2.7602907579505302, + "y": 3.0978384273454327, "heading": -0.79, - "angularVelocity": 1.432046748255306e-16, - "velocityX": 2.0086891109686786, - "velocityY": -0.8609778205420586, - "timestamp": 1.0447858086957669 + "angularVelocity": -2.2579655206340545e-16, + "velocityX": 2.0069571411277565, + "velocityY": -0.8602354520060983, + "timestamp": 1.0451232873697582 }, { - "x": 2.8734346168098597, - "y": 3.049341946491368, + "x": 2.873505290407551, + "y": 3.0493116539001877, "heading": -0.79, - "angularVelocity": 9.739728624843172e-17, - "velocityX": 1.6069513224022827, - "velocityY": -0.6887822708472163, - "timestamp": 1.1152681230061945 + "angularVelocity": -1.4176471677644102e-16, + "velocityX": 1.6055657464932764, + "velocityY": -0.6881883760029022, + "timestamp": 1.115637081466508 }, { - "x": 2.9583808542915984, - "y": 3.0129317195374297, + "x": 2.9584161910908713, + "y": 3.0129165732416565, "heading": -0.79, - "angularVelocity": 7.507925930031045e-17, - "velocityX": 1.2052135108334718, - "velocityY": -0.5165867112929291, - "timestamp": 1.185750437316622 + "angularVelocity": -1.2154347758080432e-16, + "velocityX": 1.2041743288811908, + "velocityY": -0.5161412901508954, + "timestamp": 1.1861508755632577 }, { - "x": 3.0150116799871185, - "y": 2.9886582345981343, + "x": 3.0150234589203313, + "y": 2.9886531858328245, "heading": -0.79, - "angularVelocity": 2.284084326875716e-17, - "velocityX": 0.8034756839297177, - "velocityY": -0.3443911451656786, - "timestamp": 1.2562327516270497 + "angularVelocity": -8.455163929533081e-17, + "velocityX": 0.8027828959507013, + "velocityY": -0.3440941977330144, + "timestamp": 1.2566646696600074 }, { "x": 3.0433270931243896, "y": 2.9765214920043945, "heading": -0.79, - "angularVelocity": 4.059583267317121e-17, - "velocityX": 0.40173784607243285, - "velocityY": -0.17219557434345442, - "timestamp": 1.3267150659374773 + "angularVelocity": -3.205512140199398e-17, + "velocityX": 0.4013914520784945, + "velocityY": -0.17204710062522346, + "timestamp": 1.3271784637567572 }, { "x": 3.0433270931243896, "y": 2.9765214920043945, "heading": -0.79, - "angularVelocity": -5.776011429095129e-29, - "velocityX": -2.265299874372296e-27, - "velocityY": 1.6360733128925837e-27, - "timestamp": 1.397197380247905 + "angularVelocity": -3.649032455220026e-29, + "velocityX": -7.926410421294837e-28, + "velocityY": -2.2668198189886467e-28, + "timestamp": 1.397692257853507 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceLanePGFE.2.traj b/src/main/deploy/choreo/SourceLanePGFE.2.traj index 7523865d..8315fb08 100644 --- a/src/main/deploy/choreo/SourceLanePGFE.2.traj +++ b/src/main/deploy/choreo/SourceLanePGFE.2.traj @@ -4,676 +4,676 @@ "x": 3.0433270931243896, "y": 2.9765214920043945, "heading": -0.79, - "angularVelocity": -5.776011429095129e-29, - "velocityX": -2.265299874372296e-27, - "velocityY": 1.6360733128925837e-27, + "angularVelocity": -3.649032455220026e-29, + "velocityX": -7.926410421294837e-28, + "velocityY": -2.2668198189886467e-28, "timestamp": 0 }, { - "x": 3.05997928479042, - "y": 2.9660593331101914, - "heading": -0.757416423789292, - "angularVelocity": 0.5582463314772621, - "velocityX": 0.2852978705745241, - "velocityY": -0.17924557403559996, - "timestamp": 0.05836773906688042 - }, - { - "x": 3.09346940981965, - "y": 2.9451270864719548, - "heading": -0.6941025427131016, - "angularVelocity": 1.0847410245519795, - "velocityX": 0.5737780075883255, - "velocityY": -0.35862699108923296, - "timestamp": 0.11673547813376084 - }, - { - "x": 3.144014292961821, - "y": 2.9136974289532866, - "heading": -0.6025701359612305, - "angularVelocity": 1.5682020276130446, - "velocityX": 0.8659729492734705, - "velocityY": -0.5384765286634581, - "timestamp": 0.17510321720064126 - }, - { - "x": 3.211860363047214, - "y": 2.8716921302721943, - "heading": -0.4864204343113474, - "angularVelocity": 1.9899640367565647, - "velocityX": 1.1623898950009328, - "velocityY": -0.7196663662603233, - "timestamp": 0.23347095626752168 - }, - { - "x": 3.2972613235718167, - "y": 2.8189697938754956, - "heading": -0.350515523406248, - "angularVelocity": 2.3284251382321566, - "velocityX": 1.463153479814365, - "velocityY": -0.903278715940788, - "timestamp": 0.2918386953344021 - }, - { - "x": 3.4004578303174937, - "y": 2.7553674746788306, - "heading": -0.2010210241746004, - "angularVelocity": 2.5612521852242733, - "velocityX": 1.7680401604631244, - "velocityY": -1.0896827633461483, - "timestamp": 0.3502064344012825 - }, - { - "x": 3.521660389826237, - "y": 2.6807799872839095, - "heading": -0.04666004939323709, - "angularVelocity": 2.644628304078894, - "velocityX": 2.0765333974965823, - "velocityY": -1.2778889260976047, - "timestamp": 0.40857417346816294 - }, - { - "x": 3.6607588635338284, - "y": 2.5953087461465145, - "heading": 0.09647781625083168, - "angularVelocity": 2.4523455582210394, - "velocityX": 2.383139657820331, - "velocityY": -1.4643575801258646, - "timestamp": 0.46694191253504336 - }, - { - "x": 3.8159145666935705, - "y": 2.5005609897698613, - "heading": 0.19948059626332554, - "angularVelocity": 1.7647210883818645, - "velocityX": 2.6582441883170693, - "velocityY": -1.6232898154250444, - "timestamp": 0.5253096516019238 - }, - { - "x": 3.9841103579797825, - "y": 2.394344369414975, - "heading": 0.2530307730268158, - "angularVelocity": 0.9174618996656708, - "velocityX": 2.8816567846406667, - "velocityY": -1.8197830180329346, - "timestamp": 0.5836773906688042 - }, - { - "x": 4.165386797993937, - "y": 2.276764205392623, - "heading": 0.2569441402014935, - "angularVelocity": 0.06704674940712647, - "velocityX": 3.1057642956915044, - "velocityY": -2.014471793872694, - "timestamp": 0.6420451297356844 - }, - { - "x": 4.350490054886, - "y": 2.162710813518831, - "heading": 0.2569442210228683, - "angularVelocity": 0.0000013846925729023855, - "velocityX": 3.171328200325918, - "velocityY": -1.9540484811841776, - "timestamp": 0.7004128688025646 - }, - { - "x": 4.53559341768799, - "y": 2.048657593531974, - "heading": 0.2569443018441724, - "angularVelocity": 0.0000013846913619086116, - "velocityX": 3.171330014854447, - "velocityY": -1.9540455362879443, - "timestamp": 0.7587806078694448 - }, - { - "x": 4.720696780504463, - "y": 1.934604373568621, - "heading": 0.25694438266547676, - "angularVelocity": 0.0000013846913668711097, - "velocityX": 3.1713300151025647, - "velocityY": -1.9540455358852609, - "timestamp": 0.817148346936325 - }, - { - "x": 4.9058004208396335, - "y": 1.8205516040071454, - "heading": 0.25694446348677025, - "angularVelocity": 0.0000013846911809077847, - "velocityX": 3.1713347697615966, - "velocityY": -1.9540378192615708, - "timestamp": 0.8755160860032052 - }, - { - "x": 5.096209165426888, - "y": 1.7155949376896078, - "heading": 0.25694454460803495, - "angularVelocity": 0.0000013898305117986064, - "velocityX": 3.2622258054072626, - "velocityY": -1.7981965379415077, - "timestamp": 0.9338838250700854 - }, - { - "x": 5.295905298565607, - "y": 1.629613278960399, - "heading": 0.25694462919552075, - "angularVelocity": 0.0000014492164191385246, - "velocityX": 3.4213443304682105, - "velocityY": -1.4731024381582958, - "timestamp": 0.9922515641369656 + "x": 3.0599636784357562, + "y": 2.9660757817861065, + "heading": -0.7576340217382036, + "angularVelocity": 0.5546537792406501, + "velocityX": 0.2851001394727132, + "velocityY": -0.17900749368867203, + "timestamp": 0.058353480086455356 + }, + { + "x": 3.0934189065832722, + "y": 2.945176443498765, + "heading": -0.694726478822482, + "angularVelocity": 1.0780426946690942, + "velocityX": 0.573320187552645, + "velocityY": -0.35815067509902887, + "timestamp": 0.11670696017291071 + }, + { + "x": 3.1439054612634614, + "y": 2.91379685342996, + "heading": -0.6037471047717198, + "angularVelocity": 1.5591079386519706, + "velocityX": 0.8651849830616652, + "velocityY": -0.5377501054318186, + "timestamp": 0.17506044025936607 + }, + { + "x": 3.211665554237461, + "y": 2.871860906483434, + "heading": -0.4882361709178232, + "angularVelocity": 1.9795037705164775, + "velocityX": 1.1612005466273392, + "velocityY": -0.7186537441193689, + "timestamp": 0.23341392034582142 + }, + { + "x": 3.2969494486430677, + "y": 2.819230447903095, + "heading": -0.3529801383687169, + "angularVelocity": 2.317874312701035, + "velocityX": 1.4615048541963882, + "velocityY": -0.9019249323667187, + "timestamp": 0.2917674004322768 + }, + { + "x": 3.3999954148233655, + "y": 2.7557448974167267, + "heading": -0.20405555239709897, + "angularVelocity": 2.552111472203101, + "velocityX": 1.765892385983268, + "velocityY": -1.0879479748647278, + "timestamp": 0.35012088051873214 + }, + { + "x": 3.5210139565582126, + "y": 2.681298833579533, + "heading": -0.05002957366551629, + "angularVelocity": 2.6395337262384486, + "velocityX": 2.0738873081013947, + "velocityY": -1.275777618179687, + "timestamp": 0.4084743606051875 + }, + { + "x": 3.6599142962012903, + "y": 2.5959840440263875, + "heading": 0.09335553646223951, + "angularVelocity": 2.457181815297378, + "velocityX": 2.3803265792765944, + "velocityY": -1.4620343024399716, + "timestamp": 0.46682784069164285 + }, + { + "x": 3.8149142954051776, + "y": 2.5012941331840683, + "heading": 0.19750147837600968, + "angularVelocity": 1.7847426024886535, + "velocityX": 2.656225455178376, + "velocityY": -1.6226951794824969, + "timestamp": 0.5251813207780982 + }, + { + "x": 3.982928508847516, + "y": 2.3951557532582775, + "heading": 0.25228608290098037, + "angularVelocity": 0.9388403989582608, + "velocityX": 2.879249244319485, + "velocityY": -1.8188868901826996, + "timestamp": 0.5835348008645536 + }, + { + "x": 4.164002224660317, + "y": 2.277675094812323, + "heading": 0.2575212486101159, + "angularVelocity": 0.08971471283939278, + "velocityX": 3.103049133394018, + "velocityY": -2.0132588197292973, + "timestamp": 0.6418882809510091 + }, + { + "x": 4.349312428482631, + "y": 2.163516001344807, + "heading": 0.25752132885359325, + "angularVelocity": 0.0000013751275369508418, + "velocityX": 3.175649567905162, + "velocityY": -1.9563373649417417, + "timestamp": 0.7002417610374645 + }, + { + "x": 4.534622740292831, + "y": 2.0493570831701313, + "heading": 0.2575214090969788, + "angularVelocity": 0.0000013751259629129977, + "velocityX": 3.175651418486927, + "velocityY": -1.9563343609591082, + "timestamp": 0.7585952411239199 + }, + { + "x": 4.719933052117692, + "y": 1.9351981650192527, + "heading": 0.25752148934036456, + "angularVelocity": 0.0000013751259684025227, + "velocityX": 3.175651418738155, + "velocityY": -1.9563343605512986, + "timestamp": 0.8169487212103752 + }, + { + "x": 4.9052436421415075, + "y": 1.8210396984605661, + "heading": 0.2575215695837395, + "angularVelocity": 0.0000013751257818103147, + "velocityX": 3.17565618621654, + "velocityY": -1.9563266216436723, + "timestamp": 0.8753022012968306 + }, + { + "x": 5.095847642006684, + "y": 1.7159574192298102, + "heading": 0.2575216501181558, + "angularVelocity": 0.0000013801133386874064, + "velocityX": 3.266369025168363, + "velocityY": -1.8007885575130747, + "timestamp": 0.9336556813832859 + }, + { + "x": 5.295726350893371, + "y": 1.6298141007255773, + "heading": 0.25752173405231565, + "angularVelocity": 0.0000014383745358820107, + "velocityX": 3.4253091433544425, + "velocityY": -1.4762327521272869, + "timestamp": 0.9920091614697413 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": 0.2569447220794932, - "angularVelocity": 0.0000015913580676492416, - "velocityX": 3.5481637974123346, - "velocityY": -1.1340904342963076, - "timestamp": 1.0506193032038458 - }, - { - "x": 5.694707875531767, - "y": 1.52024537146014, - "heading": 0.25694480525283153, - "angularVelocity": 0.0000015766499748638697, - "velocityX": 3.6339835774202793, - "velocityY": -0.8184074026053739, - "timestamp": 1.103372507556061 - }, - { - "x": 5.889460824583239, - "y": 1.4940579532693374, - "heading": 0.25694488397529436, - "angularVelocity": 0.0000014922783143570964, - "velocityX": 3.6917747735506756, - "velocityY": -0.49641379158615856, - "timestamp": 1.1561257119082762 - }, - { - "x": 6.0857603375931175, - "y": 1.485058307103912, - "heading": 0.2569449614499782, - "angularVelocity": 0.0000014686251720162004, - "velocityX": 3.7210917406884763, - "velocityY": -0.17059904276786395, - "timestamp": 1.2088789162604914 - }, - { - "x": 6.282233415675428, - "y": 1.4886391007138935, - "heading": 0.2569450396471809, - "angularVelocity": 0.0000014823213805790595, - "velocityX": 3.724381873952661, - "velocityY": 0.06787821998591004, - "timestamp": 1.2616321206127066 - }, - { - "x": 6.478706465106145, - "y": 1.492221466050733, - "heading": 0.25694511784454455, - "angularVelocity": 0.0000014823244316657533, - "velocityX": 3.724381330827469, - "velocityY": 0.06790801394586561, - "timestamp": 1.3143853249649218 - }, - { - "x": 6.675014894210808, - "y": 1.5010244818845797, - "heading": 0.2569451969790386, - "angularVelocity": 0.0000015000888575462877, - "velocityX": 3.7212607559150137, - "velocityY": 0.16687167996605787, - "timestamp": 1.367138529317137 - }, - { - "x": 6.869794082781554, - "y": 1.5270160132210213, - "heading": 0.25694528055287635, - "angularVelocity": 0.0000015842419202847933, - "velocityX": 3.6922721749805327, - "velocityY": 0.4927005222830628, - "timestamp": 1.4198917336693522 - }, - { - "x": 7.061541469504376, - "y": 1.5699966670379106, - "heading": 0.256946313055063, - "angularVelocity": 0.000019572312228011845, - "velocityX": 3.6348007495922103, - "velocityY": 0.8147496316986196, - "timestamp": 1.4726449380215674 - }, - { - "x": 7.242025004547978, - "y": 1.6235443273244181, - "heading": 0.2892009263238711, - "angularVelocity": 0.6114247213013846, - "velocityX": 3.4212809868112584, - "velocityY": 1.0150598611790123, - "timestamp": 1.5253981423737826 - }, - { - "x": 7.410147525588679, - "y": 1.6859098332103064, - "heading": 0.355218131661103, - "angularVelocity": 1.2514349819673058, - "velocityX": 3.1869632016702596, - "velocityY": 1.1822126570643, - "timestamp": 1.5781513467259978 - }, - { - "x": 7.565027902002143, - "y": 1.7562974315327362, - "heading": 0.45280543116483274, - "angularVelocity": 1.8498838260548631, - "velocityX": 2.935942533071192, - "velocityY": 1.3342810012539876, - "timestamp": 1.630904551078213 - }, - { - "x": 7.703780357583263, - "y": 1.8320280138994296, - "heading": 0.5628377984662216, - "angularVelocity": 2.0857949512742477, - "velocityX": 2.630218529564891, - "velocityY": 1.4355636457847423, - "timestamp": 1.6836577554304282 - }, - { - "x": 7.825662907370359, - "y": 1.9113228263820174, - "heading": 0.6698617562917842, - "angularVelocity": 2.028766956240237, - "velocityX": 2.3104293148398787, - "velocityY": 1.503127884955833, - "timestamp": 1.7364109597826434 - }, - { - "x": 7.930693514616472, - "y": 1.993224495332611, - "heading": 0.7659443680915856, - "angularVelocity": 1.821360673340157, - "velocityX": 1.990980615032594, - "velocityY": 1.552543963088276, - "timestamp": 1.7891641641348586 - }, - { - "x": 8.019011714129224, - "y": 2.0771819587983416, - "heading": 0.8464048765676822, - "angularVelocity": 1.525225044888065, - "velocityX": 1.6741769641722983, - "velocityY": 1.5915140036835516, - "timestamp": 1.8419173684870738 - }, - { - "x": 8.090768492192119, - "y": 2.1628615391485617, - "heading": 0.9080474489079001, - "angularVelocity": 1.1685085881921318, - "velocityX": 1.3602354386626168, - "velocityY": 1.6241587862258966, - "timestamp": 1.894670572839289 - }, - { - "x": 8.146101418716508, - "y": 2.2500495157540152, - "heading": 0.9485346126335614, - "angularVelocity": 0.7674825486494136, - "velocityX": 1.048901715144179, - "velocityY": 1.6527522389602969, - "timestamp": 1.9474237771915042 - }, - { - "x": 8.185122291410464, - "y": 2.3385982915715853, - "heading": 0.9662103754331748, - "angularVelocity": 0.33506519682858266, - "velocityX": 0.7396872507199079, - "velocityY": 1.6785478134439036, - "timestamp": 2.0001769815437194 + "heading": 0.25752182612435603, + "angularVelocity": 0.000001577832894961981, + "velocityX": 3.55209742519301, + "velocityY": -1.137809025512455, + "timestamp": 1.0503626415561966 + }, + { + "x": 5.6948016559329036, + "y": 1.5200403690670627, + "heading": 0.2575219089946678, + "angularVelocity": 0.0000015718708675388777, + "velocityX": 3.637995011021535, + "velocityY": -0.8227986628520415, + "timestamp": 1.1030834556563585 + }, + { + "x": 5.889658517300143, + "y": 1.4936016847783737, + "heading": 0.25752198743316407, + "angularVelocity": 0.0000014878088960886408, + "velocityX": 3.6960138930525672, + "velocityY": -0.5014847501872678, + "timestamp": 1.1558042697565203 + }, + { + "x": 6.0860809574321495, + "y": 1.4843051335513961, + "heading": 0.2575220646083203, + "angularVelocity": 0.000001463846063481971, + "velocityX": 3.725709541564224, + "velocityY": -0.17633550212854351, + "timestamp": 1.2085250838566821 + }, + { + "x": 6.282694965484122, + "y": 1.4876417105496467, + "heading": 0.25752214249001004, + "angularVelocity": 0.0000014772474802220589, + "velocityX": 3.7293431713409246, + "velocityY": 0.06328766076926912, + "timestamp": 1.261245897956844 + }, + { + "x": 6.479308945028881, + "y": 1.4909799669650579, + "heading": 0.2575222203718718, + "angularVelocity": 0.0000014772507428858513, + "velocityX": 3.729342630620652, + "velocityY": 0.06331951568632599, + "timestamp": 1.3139667120570058 + }, + { + "x": 6.675718854872107, + "y": 1.5005375890883978, + "heading": 0.2575222993699314, + "angularVelocity": 0.0000014984226051915619, + "velocityX": 3.7254718690435245, + "velocityY": 0.18128745328518545, + "timestamp": 1.3666875261571676 + }, + { + "x": 6.870540482169492, + "y": 1.5272346660517908, + "heading": 0.25752238291998725, + "angularVelocity": 0.000001584764144444983, + "velocityX": 3.6953455788306457, + "velocityY": 0.5063859012623091, + "timestamp": 1.4194083402573294 + }, + { + "x": 7.062280223610006, + "y": 1.5708674152899489, + "heading": 0.2575250023601284, + "angularVelocity": 0.00004968512314911554, + "velocityX": 3.6368888590422164, + "velocityY": 0.8276190340168537, + "timestamp": 1.4721291543574913 + }, + { + "x": 7.242655823175531, + "y": 1.624760581479399, + "heading": 0.29034566188783406, + "angularVelocity": 0.6225370394575329, + "velocityX": 3.4213356270037516, + "velocityY": 1.0222369875977493, + "timestamp": 1.524849968457653 + }, + { + "x": 7.410675905679771, + "y": 1.6874034797847524, + "heading": 0.35686793752538104, + "angularVelocity": 1.261783922971381, + "velocityX": 3.1869781484221007, + "velocityY": 1.188200511971245, + "timestamp": 1.577570782557815 + }, + { + "x": 7.565444154381843, + "y": 1.757937293066464, + "heading": 0.45500614255643107, + "angularVelocity": 1.8614698332351587, + "velocityX": 2.9356194767409227, + "velocityY": 1.337874129707252, + "timestamp": 1.6302915966579767 + }, + { + "x": 7.704030497624387, + "y": 1.8336235028030063, + "heading": 0.5647068511850917, + "angularVelocity": 2.0807855588163995, + "velocityX": 2.628683672813762, + "velocityY": 1.4356039645508802, + "timestamp": 1.6830124107581386 + }, + { + "x": 7.825774409585393, + "y": 1.9127779886685334, + "heading": 0.6711471283329911, + "angularVelocity": 2.018942214087941, + "velocityX": 2.309219120359376, + "velocityY": 1.5013896734436838, + "timestamp": 1.7357332248583004 + }, + { + "x": 7.930703401936529, + "y": 1.9944869953972368, + "heading": 0.7666407114919108, + "angularVelocity": 1.8113070670247255, + "velocityX": 1.990276404908808, + "velocityY": 1.5498434180752323, + "timestamp": 1.7884540389584622 + }, + { + "x": 8.018956078741207, + "y": 2.0782208825678086, + "heading": 0.8466105266240161, + "angularVelocity": 1.516854709037202, + "velocityX": 1.6739627092444165, + "velocityY": 1.5882510276015418, + "timestamp": 1.841174853058624 + }, + { + "x": 8.090681009003735, + "y": 2.1636571823312836, + "heading": 0.9079143826816493, + "angularVelocity": 1.1628017720129442, + "velocityX": 1.3604670467770388, + "velocityY": 1.620542118358762, + "timestamp": 1.8938956671587859 + }, + { + "x": 8.146013476632273, + "y": 2.250588470468436, + "heading": 0.9482462307641644, + "angularVelocity": 0.7650080669446885, + "velocityX": 1.049537427920118, + "velocityY": 1.6488988195818726, + "timestamp": 1.9466164812589477 + }, + { + "x": 8.185063693678202, + "y": 2.3388710656404412, + "heading": 0.9659659222521745, + "angularVelocity": 0.33610428424616584, + "velocityX": 0.7406982936898286, + "velocityY": 1.6745301960679422, + "timestamp": 1.9993372953591095 }, { "x": 8.207907676696777, "y": 2.428393840789795, "heading": 0.9600708878718816, - "angularVelocity": -0.11638132008630137, - "velocityX": 0.43192419429508555, - "velocityY": 1.702181892471887, - "timestamp": 2.0529301858959346 - }, - { - "x": 8.211423054828616, - "y": 2.5404723922535313, - "heading": 0.9162466453350178, - "angularVelocity": -0.6758725051109615, - "velocityX": 0.054215367724374734, - "velocityY": 1.7285138763843786, - "timestamp": 2.1177711761795517 - }, - { - "x": 8.190202888383553, - "y": 2.653847423099536, - "heading": 0.8391734249663445, - "angularVelocity": -1.188649649420099, - "velocityX": -0.32726468785015433, - "velocityY": 1.748508626257838, - "timestamp": 2.182612166463169 - }, - { - "x": 8.144008306424402, - "y": 2.7680190139904237, - "heading": 0.7319233890251923, - "angularVelocity": -1.654046853264217, - "velocityX": -0.7124286929779271, - "velocityY": 1.760793448580852, - "timestamp": 2.247453156746786 - }, - { - "x": 8.072604152106313, - "y": 2.8824539716658655, - "heading": 0.5977397407747723, - "angularVelocity": -2.06942626359492, - "velocityX": -1.101219367652527, - "velocityY": 1.764855181497056, - "timestamp": 2.312294147030403 - }, - { - "x": 7.975691382967057, - "y": 2.9965380091624785, - "heading": 0.44171220804306943, - "angularVelocity": -2.4063101450059983, - "velocityX": -1.4946219777853038, - "velocityY": 1.759443170093563, - "timestamp": 2.37713513731402 - }, - { - "x": 7.852920805151411, - "y": 3.109310419044201, - "heading": 0.2733111632769955, - "angularVelocity": -2.5971386931242324, - "velocityX": -1.893409975366543, - "velocityY": 1.7392147989790332, - "timestamp": 2.4419761275976373 - }, - { - "x": 7.704278478364198, - "y": 3.2190283422070105, - "heading": 0.10912005253045581, - "angularVelocity": -2.53221164618803, - "velocityX": -2.292412964963152, - "velocityY": 1.6921074567630605, - "timestamp": 2.5068171178812544 - }, - { - "x": 7.532421373794031, - "y": 3.3230579148672983, - "heading": -0.010926874392962598, - "angularVelocity": -1.8514048967840961, - "velocityX": -2.650439233245175, - "velocityY": 1.6043797635609574, - "timestamp": 2.5716581081648715 - }, - { - "x": 7.337707687800783, - "y": 3.422034456345691, - "heading": -0.0821100823564174, - "angularVelocity": -1.0978118571616002, - "velocityX": -3.002941274363059, - "velocityY": 1.5264501829084578, - "timestamp": 2.6364990984484886 + "angularVelocity": -0.11181607266331907, + "velocityX": 0.43330102936525317, + "velocityY": 1.698053732237021, + "timestamp": 2.0520581094592716 + }, + { + "x": 8.211510403454756, + "y": 2.5402965981935948, + "heading": 0.9167116042815631, + "angularVelocity": -0.6681435764586997, + "velocityX": 0.05551610962541542, + "velocityY": 1.7243621747491076, + "timestamp": 2.116953268401204 + }, + { + "x": 8.19035769452273, + "y": 2.653506399419357, + "heading": 0.8402263386800068, + "angularVelocity": -1.1785974000001183, + "velocityX": -0.3259520321223525, + "velocityY": 1.7445030272144093, + "timestamp": 2.1818484273431364 + }, + { + "x": 8.144214527086703, + "y": 2.7675347256027623, + "heading": 0.7336527994831279, + "angularVelocity": -1.6422417470652866, + "velocityX": -0.7110417508540927, + "velocityY": 1.757116062932159, + "timestamp": 2.246743586285069 + }, + { + "x": 8.072849371790616, + "y": 2.8818580666466, + "heading": 0.6001927782610759, + "angularVelocity": -2.0565481832238106, + "velocityX": -1.0996992142348316, + "velocityY": 1.7616620855514487, + "timestamp": 2.3116387452270013 + }, + { + "x": 7.975969161360382, + "y": 2.9958746653060953, + "heading": 0.4448175731480801, + "angularVelocity": -2.3942495502942416, + "velocityX": -1.4928726889616013, + "velocityY": 1.756935348005165, + "timestamp": 2.3765339041689337 + }, + { + "x": 7.853227191663504, + "y": 3.108650032284083, + "heading": 0.2767627605160218, + "angularVelocity": -2.5896355810212537, + "velocityX": -1.8913886905910138, + "velocityY": 1.7378086257389034, + "timestamp": 2.441429063110866 + }, + { + "x": 7.704583468021456, + "y": 3.218490505664157, + "heading": 0.11219872525944127, + "angularVelocity": -2.5358445520386232, + "velocityX": -2.2905209890163487, + "velocityY": 1.692583471108498, + "timestamp": 2.5063242220527986 + }, + { + "x": 7.532642084855597, + "y": 3.3226846755511135, + "heading": -0.008852877537121141, + "angularVelocity": -1.8653410326782291, + "velocityX": -2.6495255727736016, + "velocityY": 1.6055769272433476, + "timestamp": 2.571219380994731 + }, + { + "x": 7.337826737893983, + "y": 3.4218449378655498, + "heading": -0.08105102953452195, + "angularVelocity": -1.1125352518514178, + "velocityX": -3.002001229952035, + "velocityY": 1.5280070798988774, + "timestamp": 2.6361145399366634 }, { "x": 7.120736598968506, "y": 3.5168776512145996, - "heading": -0.09725138680843894, - "angularVelocity": -0.23351439245133218, - "velocityX": -3.3462025777712125, - "velocityY": 1.46270429328826, - "timestamp": 2.7013400887321057 - }, - { - "x": 7.010606209659238, - "y": 3.566708155417018, - "heading": -0.09725146950362612, - "angularVelocity": -0.0000025483254840822735, - "velocityX": -3.3937655527961965, - "velocityY": 1.535571150717811, - "timestamp": 2.733790883460259 - }, - { - "x": 6.900475999603528, - "y": 3.6165390557870793, - "heading": -0.09725155219704651, - "angularVelocity": -0.000002548271039111256, - "velocityX": -3.3937600289389183, - "velocityY": 1.5355833589748464, - "timestamp": 2.7662416781884125 - }, - { - "x": 6.7903457895857455, - "y": 3.666369956240961, - "heading": -0.0972516348904687, - "angularVelocity": -0.000002548271094167607, - "velocityX": -3.3937600277701754, - "velocityY": 1.5355833615578496, - "timestamp": 2.798692472916566 - }, - { - "x": 6.680215579567971, - "y": 3.7162008566948606, - "heading": -0.09725171758389267, - "angularVelocity": -0.0000025482711490074116, - "velocityX": -3.393760027769923, - "velocityY": 1.535583361558393, - "timestamp": 2.8311432676447192 - }, - { - "x": 6.570085369550196, - "y": 3.7660317571487596, - "heading": -0.09725180027731847, - "angularVelocity": -0.0000025482712052155066, - "velocityX": -3.39376002776992, - "velocityY": 1.53558336155839, - "timestamp": 2.8635940623728726 - }, - { - "x": 6.459955159532422, - "y": 3.815862657602659, - "heading": -0.09725188297074605, - "angularVelocity": -0.000002548271260506862, - "velocityX": -3.3937600277699165, - "velocityY": 1.5355833615583867, - "timestamp": 2.896044857101026 - }, - { - "x": 6.349824949514647, - "y": 3.865693558056558, - "heading": -0.09725196566417542, - "angularVelocity": -0.0000025482713153661305, - "velocityX": -3.3937600277699125, - "velocityY": 1.5355833615583832, - "timestamp": 2.9284956518291794 - }, - { - "x": 6.239694739496873, - "y": 3.915524458510457, - "heading": -0.09725204835760656, - "angularVelocity": -0.000002548271370425602, - "velocityX": -3.3937600277699085, - "velocityY": 1.5355833615583803, - "timestamp": 2.9609464465573327 - }, - { - "x": 6.129564529479099, - "y": 3.965355358964356, - "heading": -0.09725213105103951, - "angularVelocity": -0.0000025482714255163693, - "velocityX": -3.3937600277699054, - "velocityY": 1.5355833615583767, - "timestamp": 2.993397241285486 - }, - { - "x": 6.019434319461326, - "y": 4.015186259418255, - "heading": -0.09725221374447428, - "angularVelocity": -0.000002548271481519915, - "velocityX": -3.3937600277699014, - "velocityY": 1.5355833615583734, - "timestamp": 3.0258480360136395 - }, - { - "x": 5.9093041094435526, - "y": 4.065017159872154, - "heading": -0.09725229643791083, - "angularVelocity": -0.000002548271536879473, - "velocityX": -3.3937600277698974, - "velocityY": 1.5355833615583703, - "timestamp": 3.058298830741793 - }, - { - "x": 5.799173899425779, - "y": 4.114848060326053, - "heading": -0.09725237913134915, - "angularVelocity": -0.000002548271591483022, - "velocityX": -3.3937600277698934, - "velocityY": 1.535583361558367, - "timestamp": 3.0907496254699462 - }, - { - "x": 5.689043689408005, - "y": 4.164678960779951, - "heading": -0.09725246182478926, - "angularVelocity": -0.0000025482716464166734, - "velocityX": -3.39376002776989, - "velocityY": 1.5355833615583636, - "timestamp": 3.1232004201980996 - }, - { - "x": 5.5789134793902315, - "y": 4.214509861233849, - "heading": -0.09725254451823119, - "angularVelocity": -0.000002548271702485751, - "velocityX": -3.3937600277698863, - "velocityY": 1.5355833615583605, - "timestamp": 3.155651214926253 - }, - { - "x": 5.468783269372458, - "y": 4.264340761687747, - "heading": -0.09725262721167492, - "angularVelocity": -0.0000025482717576806813, - "velocityX": -3.3937600277698823, - "velocityY": 1.5355833615583572, - "timestamp": 3.1881020096544064 - }, - { - "x": 5.3586530593546975, - "y": 4.314171662141674, - "heading": -0.09725270990512043, - "angularVelocity": -0.0000025482718127641656, - "velocityX": -3.3937600277694844, - "velocityY": 1.5355833615592254, - "timestamp": 3.2205528043825598 - }, - { - "x": 5.2485228493974105, - "y": 4.364002562729252, - "heading": -0.09725279259856771, - "angularVelocity": -0.0000025482718674912185, - "velocityX": -3.393760025905934, - "velocityY": 1.5355833656778057, - "timestamp": 3.253003599110713 + "heading": -0.09725140170443469, + "angularVelocity": -0.249639147727624, + "velocityX": -3.3452439668069256, + "velocityY": 1.4644037382524164, + "timestamp": 2.701009698878596 + }, + { + "x": 7.010606208807297, + "y": 3.5667081535340293, + "heading": -0.09725148357223665, + "angularVelocity": -0.000002526133789142618, + "velocityX": -3.3982114234212517, + "velocityY": 1.5375826960099699, + "timestamp": 2.7334180385722853 + }, + { + "x": 6.900475998308352, + "y": 3.616539052924532, + "heading": -0.09725156543808965, + "angularVelocity": -0.000002526073652213915, + "velocityX": -3.3982058797165378, + "velocityY": 1.5375949481363274, + "timestamp": 2.7658263782659747 + }, + { + "x": 6.7903457878477695, + "y": 3.666369952399819, + "heading": -0.09725164730394434, + "angularVelocity": -0.0000025260737041987077, + "velocityX": -3.3982058785328007, + "velocityY": 1.537594950752468, + "timestamp": 2.798234717959664 + }, + { + "x": 6.680215577387196, + "y": 3.7162008518751244, + "heading": -0.0972517291698008, + "angularVelocity": -0.0000025260737583939243, + "velocityX": -3.3982058785325444, + "velocityY": 1.5375949507530235, + "timestamp": 2.8306430576533534 + }, + { + "x": 6.570085366926623, + "y": 3.766031751350429, + "heading": -0.09725181103565897, + "angularVelocity": -0.0000025260738115534393, + "velocityX": -3.3982058785325413, + "velocityY": 1.5375949507530207, + "timestamp": 2.863051397347043 + }, + { + "x": 6.45995515646605, + "y": 3.8158626508257334, + "heading": -0.09725189290151888, + "angularVelocity": -0.000002526073864680103, + "velocityX": -3.3982058785325373, + "velocityY": 1.5375949507530176, + "timestamp": 2.8954597370407322 + }, + { + "x": 6.3498249460054765, + "y": 3.8656935503010383, + "heading": -0.0972519747673805, + "angularVelocity": -0.000002526073917550957, + "velocityX": -3.3982058785325338, + "velocityY": 1.5375949507530144, + "timestamp": 2.9278680767344216 + }, + { + "x": 6.239694735544903, + "y": 3.915524449776343, + "heading": -0.09725205663324382, + "angularVelocity": -0.0000025260739709897513, + "velocityX": -3.39820587853253, + "velocityY": 1.5375949507530113, + "timestamp": 2.960276416428111 + }, + { + "x": 6.12956452508433, + "y": 3.965355349251648, + "heading": -0.0972521384991089, + "angularVelocity": -0.0000025260740241290968, + "velocityX": -3.398205878532526, + "velocityY": 1.5375949507530078, + "timestamp": 2.9926847561218004 + }, + { + "x": 6.019434314623757, + "y": 4.015186248726953, + "heading": -0.09725222036497569, + "angularVelocity": -0.0000025260740776848576, + "velocityX": -3.3982058785325227, + "velocityY": 1.537594950753005, + "timestamp": 3.02509309581549 + }, + { + "x": 5.909304104163184, + "y": 4.065017148202257, + "heading": -0.09725230223084419, + "angularVelocity": -0.000002526074130335594, + "velocityX": -3.398205878532519, + "velocityY": 1.5375949507530018, + "timestamp": 3.057501435509179 + }, + { + "x": 5.799173893702611, + "y": 4.114848047677562, + "heading": -0.09725238409671443, + "angularVelocity": -0.0000025260741837826664, + "velocityX": -3.3982058785325155, + "velocityY": 1.5375949507529985, + "timestamp": 3.0899097752028686 + }, + { + "x": 5.689043683242039, + "y": 4.164678947152866, + "heading": -0.09725246596258641, + "angularVelocity": -0.0000025260742375974643, + "velocityX": -3.3982058785325115, + "velocityY": 1.5375949507529956, + "timestamp": 3.122318114896558 + }, + { + "x": 5.578913472781466, + "y": 4.214509846628169, + "heading": -0.09725254782846013, + "angularVelocity": -0.000002526074291245016, + "velocityX": -3.398205878532508, + "velocityY": 1.5375949507529922, + "timestamp": 3.1547264545902474 + }, + { + "x": 5.468783262320894, + "y": 4.264340746103473, + "heading": -0.09725262969433558, + "angularVelocity": -0.0000025260743444666228, + "velocityX": -3.398205878532505, + "velocityY": 1.5375949507529894, + "timestamp": 3.1871347942839368 + }, + { + "x": 5.358653051860335, + "y": 4.314171645578806, + "heading": -0.09725271156021274, + "angularVelocity": -0.0000025260743976709886, + "velocityX": -3.398205878532087, + "velocityY": 1.537594950753901, + "timestamp": 3.219543133977626 + }, + { + "x": 5.248522841462595, + "y": 4.364002545192973, + "heading": -0.0972527934260916, + "angularVelocity": -0.0000025260744491958753, + "velocityX": -3.398205876593744, + "velocityY": 1.53759495503778, + "timestamp": 3.2519514736713155 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": -0.0000025482898100852076, - "velocityX": -3.393751218037592, - "velocityY": 1.5356028315921753, - "timestamp": 3.2854543938388665 - }, - { - "x": 4.9137282295128415, - "y": 4.535640155229253, - "heading": -0.09725287529313088, - "angularVelocity": -7.435036019118785e-12, - "velocityX": -3.1302347498710454, - "velocityY": 1.6971138308958593, - "timestamp": 3.357226868973157 - }, - { - "x": 4.717146626097375, - "y": 4.642220517268037, - "heading": -0.097252875293223, - "angularVelocity": -1.283652180157178e-12, - "velocityX": -2.7389553313809003, - "velocityY": 1.4849754288028512, - "timestamp": 3.428999344107447 - }, - { - "x": 4.548648110952986, - "y": 4.733575136181824, - "heading": -0.09725287529315746, - "angularVelocity": 9.133130247777463e-13, - "velocityX": -2.347675969501115, - "velocityY": 1.2728364006237478, - "timestamp": 3.5007718192417374 - }, - { - "x": 4.408232682725308, - "y": 4.809703996992033, - "heading": -0.09725287529302883, - "angularVelocity": 1.7921853517363252e-12, - "velocityX": -1.9563966264916142, - "velocityY": 1.0606971637493035, - "timestamp": 3.5725442943760277 - }, - { - "x": 4.295900340737155, - "y": 4.870607092209374, - "heading": -0.09725287529288444, - "angularVelocity": 2.0118085530494987e-12, - "velocityX": -1.5651172929172832, - "velocityY": 0.8485578225272118, - "timestamp": 3.644316769510318 - }, - { - "x": 4.211651084582216, - "y": 4.916284417340276, - "heading": -0.09725287529275266, - "angularVelocity": 1.8359969573857423e-12, - "velocityX": -1.1738379650040625, - "velocityY": 0.636418418696543, - "timestamp": 3.7160892446446083 - }, - { - "x": 4.155484913989614, - "y": 4.946735969389024, - "heading": -0.09725287529265242, - "angularVelocity": 1.3966146295291797e-12, - "velocityX": -0.7825586408649183, - "velocityY": 0.42427897312682855, - "timestamp": 3.7878617197788986 + "angularVelocity": -0.000002526093790111687, + "velocityX": -3.398196798757616, + "velocityY": 1.5376150175922905, + "timestamp": 3.284359813365005 + }, + { + "x": 4.913728228480229, + "y": 4.53564015337073, + "heading": -0.09725287529314537, + "angularVelocity": -7.632140537797702e-12, + "velocityX": -3.128186469396145, + "velocityY": 1.6960032846252107, + "timestamp": 3.35617928416068 + }, + { + "x": 4.717146624886484, + "y": 4.642220515088643, + "heading": -0.09725287529324, + "angularVelocity": -1.3176073581660395e-12, + "velocityX": -2.737163075915943, + "velocityY": 1.4840037184502801, + "timestamp": 3.427998754956355 + }, + { + "x": 4.548648109868937, + "y": 4.7335751342307235, + "heading": -0.09725287529317267, + "angularVelocity": 9.375034048852172e-13, + "velocityX": -2.346139746656194, + "velocityY": 1.2720035128354326, + "timestamp": 3.49981822575203 + }, + { + "x": 4.40823268189015, + "y": 4.809703995488892, + "heading": -0.09725287529304055, + "angularVelocity": 1.8396343305490373e-12, + "velocityX": -1.9551164388034394, + "velocityY": 1.0600030940739407, + "timestamp": 3.571637696547705 + }, + { + "x": 4.295900340181399, + "y": 4.87060709120911, + "heading": -0.09725287529289224, + "angularVelocity": 2.065048279063882e-12, + "velocityX": -1.5640931416542116, + "velocityY": 0.8480025687391443, + "timestamp": 3.6434571673433798 + }, + { + "x": 4.211651084281453, + "y": 4.916284416798956, + "heading": -0.09725287529275689, + "angularVelocity": 1.8846216293321442e-12, + "velocityX": -1.1730698509271091, + "velocityY": 0.6360019794603747, + "timestamp": 3.7152766381390547 + }, + { + "x": 4.155484913882821, + "y": 4.946735969196816, + "heading": -0.09725287529265393, + "angularVelocity": 1.4335935528367604e-12, + "velocityX": -0.7820465644814268, + "velocityY": 0.42400134755229474, + "timestamp": 3.7870961089347297 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 7.688238334265391e-13, - "velocityX": -0.3912793194215448, - "velocityY": 0.21213949774351348, - "timestamp": 3.859634194913189 + "angularVelocity": 7.893009586439382e-13, + "velocityX": -0.3910232810939034, + "velocityY": 0.21200068519471058, + "timestamp": 3.8589155797304047 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 3.652934582681632e-29, - "velocityX": -2.1119125295164495e-27, - "velocityY": 7.525449232438864e-28, - "timestamp": 3.9314066700474792 + "angularVelocity": -2.6496856571629414e-29, + "velocityX": 3.729224891945568e-30, + "velocityY": 1.9440418039137549e-28, + "timestamp": 3.9307350505260796 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceLanePGFE.3.traj b/src/main/deploy/choreo/SourceLanePGFE.3.traj index f20802e1..f0cb6cc5 100644 --- a/src/main/deploy/choreo/SourceLanePGFE.3.traj +++ b/src/main/deploy/choreo/SourceLanePGFE.3.traj @@ -4,442 +4,442 @@ "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 3.652934582681632e-29, - "velocityX": -2.1119125295164495e-27, - "velocityY": 7.525449232438864e-28, + "angularVelocity": -2.6496856571629414e-29, + "velocityX": 3.729224891945568e-30, + "velocityY": 1.9440418039137549e-28, "timestamp": 0 }, { - "x": 4.149345287192321, - "y": 4.948730090302555, - "heading": -0.09725287542061173, - "angularVelocity": -1.9914932727158263e-9, - "velocityX": 0.34136955009475556, - "velocityY": -0.2058419570123514, - "timestamp": 0.06428065543737382 + "x": 4.149339896779893, + "y": 4.948719586004848, + "heading": -0.09725287542661647, + "angularVelocity": -2.0835104193721486e-9, + "velocityX": 0.3410569741164579, + "velocityY": -0.2058673119908494, + "timestamp": 0.06432376312156318 }, { - "x": 4.193331867161711, - "y": 4.922433486987429, - "heading": -0.09725287565526207, - "angularVelocity": -3.650403639422262e-9, - "velocityX": 0.6842895373437039, - "velocityY": -0.4090904664272752, - "timestamp": 0.12856131087474765 + "x": 4.193317137132987, + "y": 4.9224042284052185, + "heading": -0.09725287567219071, + "angularVelocity": -3.8177839227409805e-9, + "velocityX": 0.6836857518734218, + "velocityY": -0.4091078681123934, + "timestamp": 0.12864752624312636 }, { - "x": 4.259487364434434, - "y": 4.883286547964955, - "heading": -0.09725287596919821, - "angularVelocity": -4.883835485414618e-9, - "velocityX": 1.0291665015328901, - "velocityY": -0.6090003089749593, - "timestamp": 0.19284196631212147 + "x": 4.259461092392441, + "y": 4.883233120266127, + "heading": -0.09725287600059242, + "angularVelocity": -5.105449510153574e-9, + "velocityX": 1.0282973515472125, + "velocityY": -0.6089679185134605, + "timestamp": 0.19297128936468955 }, { - "x": 4.347975400700828, - "y": 4.831575784808056, - "heading": -0.09725287632620158, - "angularVelocity": -5.553822878939374e-9, - "velocityX": 1.3765888923239986, - "velocityY": -0.8044529540816499, - "timestamp": 0.2571226217494953 + "x": 4.3479375321320015, + "y": 4.831496437672959, + "heading": -0.0972528763737988, + "angularVelocity": -5.801998468175167e-9, + "velocityX": 1.3754860637172643, + "velocityY": -0.8043167887331668, + "timestamp": 0.25729505248625273 }, { - "x": 4.45901715765165, - "y": 4.76770271955414, - "heading": -0.09725287667606394, - "angularVelocity": -5.442731629415375e-9, - "velocityX": 1.7274521579669766, - "velocityY": -0.9936592092802463, - "timestamp": 0.3214032771868691 + "x": 4.4589703097902795, + "y": 4.767600598916816, + "heading": -0.09725287673913857, + "angularVelocity": -5.679701582955187e-9, + "velocityX": 1.7261548807155662, + "velocityY": -0.993347336277392, + "timestamp": 0.3216188156078159 }, { - "x": 4.592927185358519, - "y": 4.69226963680943, - "heading": -0.09725287694462321, - "angularVelocity": -4.1779173473054744e-9, - "velocityX": 2.083208809800217, - "velocityY": -1.1734958555019974, - "timestamp": 0.38568393262424294 + "x": 4.592877291375008, + "y": 4.69215468843508, + "heading": -0.09725287701889467, + "angularVelocity": -4.34918718104189e-9, + "velocityX": 2.081765355233693, + "velocityY": -1.1729088414674105, + "timestamp": 0.3859425787293791 }, { - "x": 4.750184246245984, - "y": 4.606276833638444, - "heading": -0.09725287701156488, - "angularVelocity": -1.0413967826345418e-9, - "velocityX": 2.446413463233511, - "velocityY": -1.337771100588195, - "timestamp": 0.44996458806161677 + "x": 4.750141105667559, + "y": 4.606168779323296, + "heading": -0.0972528770872025, + "angularVelocity": -1.061937612005659e-9, + "velocityX": 2.4448789476968895, + "velocityY": -1.3367673926241201, + "timestamp": 0.4502663418509423 }, { - "x": 4.931581310093376, - "y": 4.511690334370361, - "heading": -0.0972528766497097, - "angularVelocity": 5.6293012279094655e-9, - "velocityX": 2.821954172886779, - "velocityY": -1.4714613381662665, - "timestamp": 0.5142452434989906 + "x": 4.931557529116976, + "y": 4.511622240510161, + "heading": -0.09725287670716622, + "angularVelocity": 5.908178394469377e-9, + "velocityX": 2.8203639626395, + "velocityY": -1.4698539734756233, + "timestamp": 0.5145901049725055 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": 2.111229970452055e-8, - "velocityX": 3.2173227507078654, - "velocityY": -1.5223279648179473, - "timestamp": 0.5785258989363644 - }, - { - "x": 5.3988689559213965, - "y": 4.32613897098341, - "heading": -0.0972528699779729, - "angularVelocity": 7.203036405891024e-8, - "velocityX": 3.5302934161705415, - "velocityY": -1.1885528129676428, - "timestamp": 0.6523090098431892 - }, - { - "x": 5.668131488513907, - "y": 4.271040046607528, - "heading": -0.09725286670603908, - "angularVelocity": 4.4345294956463885e-8, - "velocityX": 3.6493789606207083, - "velocityY": -0.7467687889368456, - "timestamp": 0.726092120750014 - }, - { - "x": 5.942117714678702, - "y": 4.249366302589977, - "heading": -0.09725286355772216, - "angularVelocity": 4.266988599202702e-8, - "velocityX": 3.713400299843557, - "velocityY": -0.2937493926614278, - "timestamp": 0.7998752316568387 - }, - { - "x": 6.216382330790908, - "y": 4.231558335942029, - "heading": -0.09725286041527645, - "angularVelocity": 4.2590311894180227e-8, - "velocityX": 3.7171733848218165, - "velocityY": -0.2413555951908442, - "timestamp": 0.8736583425636635 - }, - { - "x": 6.490646951405068, - "y": 4.21375043862986, - "heading": -0.09725285727283077, - "angularVelocity": 4.259031148289501e-8, - "velocityX": 3.717173445837876, - "velocityY": -0.2413546554665762, - "timestamp": 0.9474414534704882 - }, - { - "x": 6.764911572019294, - "y": 4.195942541318692, - "heading": -0.09725285413038512, - "angularVelocity": 4.2590311325076504e-8, - "velocityX": 3.717173445838756, - "velocityY": -0.241354655453012, - "timestamp": 1.021224564377313 - }, - { - "x": 7.039176191804646, - "y": 4.178134631242246, - "heading": -0.09725285098790415, - "angularVelocity": 4.259078987217418e-8, - "velocityX": 3.7171734346048453, - "velocityY": -0.24135482846385062, - "timestamp": 1.0950076752841378 - }, - { - "x": 7.303151625715879, - "y": 4.160355738175748, - "heading": -0.07493794888540205, - "angularVelocity": 0.302439160239284, - "velocityX": 3.5777216583424787, - "velocityY": -0.24096155404655972, - "timestamp": 1.1687907861909625 - }, - { - "x": 7.533445930864737, - "y": 4.1447637615713475, - "heading": -0.055524777247044715, - "angularVelocity": 0.2631113191048932, - "velocityX": 3.1212333326481616, - "velocityY": -0.21132175660214245, - "timestamp": 1.2425738970977873 - }, - { - "x": 7.730059092320319, - "y": 4.131358730965977, - "heading": -0.0390137097405274, - "angularVelocity": 0.22377841356361194, - "velocityX": 2.664744804591801, - "velocityY": -0.18168155883665832, - "timestamp": 1.316357008004612 - }, - { - "x": 7.892991106532595, - "y": 4.120140669647355, - "heading": -0.025405081189306844, - "angularVelocity": 0.1844409700806171, - "velocityX": 2.20825622842103, - "velocityY": -0.15204104544723207, - "timestamp": 1.3901401189114369 - }, - { - "x": 8.02224197249394, - "y": 4.111109595697515, - "heading": -0.014699203415963246, - "angularVelocity": 0.1450993003922434, - "velocityX": 1.751767638593706, - "velocityY": -0.12240028698767993, - "timestamp": 1.4639232298182616 - }, - { - "x": 8.117811689845258, - "y": 4.104265522227424, - "heading": -0.006896323202665822, - "angularVelocity": 0.10575428600660502, - "velocityX": 1.2952790438994193, - "velocityY": -0.09275935083211777, - "timestamp": 1.5377063407250864 - }, - { - "x": 8.179700258175622, - "y": 4.099608457473234, - "heading": -0.0019965838975628653, - "angularVelocity": 0.06640732878951748, - "velocityX": 0.8387904436357877, - "velocityY": -0.06311830305000854, - "timestamp": 1.6114894516319112 + "angularVelocity": 2.199139032346849e-8, + "velocityX": 3.215536313610633, + "velocityY": -1.5202491390955153, + "timestamp": 0.5789138680940686 + }, + { + "x": 5.399102164920658, + "y": 4.3258479650681645, + "heading": -0.09725286980895109, + "angularVelocity": 7.433360780041783e-8, + "velocityX": 3.5340461244770793, + "velocityY": -1.1926966681553401, + "timestamp": 0.6526846199392455 + }, + { + "x": 5.668612858103736, + "y": 4.270396554343484, + "heading": -0.0972528665281375, + "angularVelocity": 4.447309416058914e-8, + "velocityX": 3.653354296140342, + "velocityY": -0.7516720290591195, + "timestamp": 0.7264553717844224 + }, + { + "x": 5.942881064370487, + "y": 4.248309813681979, + "heading": -0.09725286337373323, + "angularVelocity": 4.275955167599945e-8, + "velocityX": 3.7178448017225065, + "velocityY": -0.29939698470009923, + "timestamp": 0.8002261236295993 + }, + { + "x": 6.2174671961178944, + "y": 4.2306087019584515, + "heading": -0.09725286022607692, + "angularVelocity": 4.266807980333545e-8, + "velocityX": 3.7221544430465427, + "velocityY": -0.23994755754525748, + "timestamp": 0.8739968754747762 + }, + { + "x": 6.492053332978151, + "y": 4.212907669547494, + "heading": -0.09725285707842057, + "angularVelocity": 4.2668079985198815e-8, + "velocityX": 3.722154512353812, + "velocityY": -0.23994648242309302, + "timestamp": 0.9477676273199531 + }, + { + "x": 6.766639469838484, + "y": 4.195206637137713, + "heading": -0.09725285393076424, + "angularVelocity": 4.2668079920669555e-8, + "velocityX": 3.7221545123548396, + "velocityY": -0.2399464824071602, + "timestamp": 1.02153837916513 + }, + { + "x": 7.041225605850418, + "y": 4.177505591567667, + "heading": -0.09725285078307094, + "angularVelocity": 4.266858084816784e-8, + "velocityX": 3.722154500854352, + "velocityY": -0.23994666080122462, + "timestamp": 1.095309131010307 + }, + { + "x": 7.304754629426189, + "y": 4.1598724351575065, + "heading": -0.0749365249784548, + "angularVelocity": 0.3025091278919278, + "velocityX": 3.5722697272876305, + "velocityY": -0.23902638876674762, + "timestamp": 1.1690798828554838 + }, + { + "x": 7.534656876904849, + "y": 4.144407021780378, + "heading": -0.055522378055544455, + "angularVelocity": 0.26316861950458387, + "velocityX": 3.116441702548429, + "velocityY": -0.20964153123429294, + "timestamp": 1.2428506347006607 + }, + { + "x": 7.730932333154636, + "y": 4.131109381030838, + "heading": -0.03901074502265599, + "angularVelocity": 0.22382356990940835, + "velocityX": 2.660613472690525, + "velocityY": -0.1802562725326004, + "timestamp": 1.3166213865458376 + }, + { + "x": 7.8935809945490485, + "y": 4.119979536241971, + "heading": -0.02540194557699689, + "angularVelocity": 0.18447418665625143, + "velocityX": 2.204785193673574, + "velocityY": -0.15087069753913437, + "timestamp": 1.3903921383910145 + }, + { + "x": 8.022602860038761, + "y": 4.111017505530848, + "heading": -0.014696283077208026, + "angularVelocity": 0.14512069122268073, + "velocityX": 1.7489569004324652, + "velocityY": -0.12148487695953097, + "timestamp": 1.4641628902361914 + }, + { + "x": 8.117997929239245, + "y": 4.10422330203326, + "heading": -0.006893999068061529, + "angularVelocity": 0.10576392152707405, + "velocityX": 1.2931286019788828, + "velocityY": -0.09209887831761811, + "timestamp": 1.5379336420813683 + }, + { + "x": 8.179766201724098, + "y": 4.0995969339999005, + "heading": -0.0019952337355516638, + "angularVelocity": 0.06640525153913249, + "velocityX": 0.8373002977452005, + "velocityY": -0.06271276783336986, + "timestamp": 1.6117043939265452 }, { "x": 8.207907676696777, "y": 4.097138404846191, - "heading": -2.1241979189811956e-28, - "angularVelocity": 0.02706017505936011, - "velocityX": 0.3823018326887492, - "velocityY": -0.03347720903449786, - "timestamp": 1.685272562538736 - }, - { - "x": 8.200639571345327, - "y": 4.096943687462525, - "heading": -0.0010802899918473687, - "angularVelocity": -0.014022329834604143, - "velocityX": -0.09434112255025054, - "velocityY": -0.0025274615139553986, - "timestamp": 1.7623132543916293 - }, - { - "x": 8.15665056358688, - "y": 4.099133352364744, - "heading": -0.005325598680954133, - "angularVelocity": -0.05510475810903996, - "velocityX": -0.5709840695932878, - "velocityY": 0.02842218637392462, - "timestamp": 1.8393539462445228 - }, - { - "x": 8.075940653456616, - "y": 4.103707386175575, - "heading": -0.012735784669566238, - "angularVelocity": -0.09618535101893455, - "velocityX": -1.0476270161796544, - "velocityY": 0.05937166062274877, - "timestamp": 1.9163946380974162 - }, - { - "x": 7.958509840643352, - "y": 4.1106657697854265, - "heading": -0.023310616019513074, - "angularVelocity": -0.1372629333357902, - "velocityX": -1.5242699668052726, - "velocityY": 0.09032088682612197, - "timestamp": 1.9934353299503096 - }, - { - "x": 7.804358124937036, - "y": 4.120008478318512, - "heading": -0.037049836652876394, - "angularVelocity": -0.17833719172197895, - "velocityX": -2.0009129201573495, - "velocityY": 0.12126979013798225, - "timestamp": 2.070476021803203 - }, - { - "x": 7.613485507099167, - "y": 4.131735481105861, - "heading": -0.05395325351914194, - "angularVelocity": -0.21940894428287439, - "velocityX": -2.4775558636250903, - "velocityY": 0.15221829536189774, - "timestamp": 2.1475167136560964 - }, - { - "x": 7.385891991055361, - "y": 4.145846741676362, - "heading": -0.07402084166347252, - "angularVelocity": -0.2604803729261546, - "velocityX": -2.9541987561377083, - "velocityY": 0.18316632718520057, - "timestamp": 2.22455740550899 - }, - { - "x": 7.121577593711164, - "y": 4.16234221776091, - "heading": -0.09725284930983917, - "angularVelocity": -0.30155502355466174, - "velocityX": -3.430841429213758, - "velocityY": 0.21411381034903446, - "timestamp": 2.3015980973618833 - }, - { - "x": 6.835203978246458, - "y": 4.1809363249101, - "heading": -0.09725285259123628, - "angularVelocity": -4.259303804512275e-8, - "velocityX": -3.7171734647908923, - "velocityY": 0.24135436354458084, - "timestamp": 2.3786387892147767 - }, - { - "x": 6.5488303639215975, - "y": 4.199530449615618, - "heading": -0.09725285587256685, - "angularVelocity": -4.259217424526975e-8, - "velocityX": -3.7171734499955185, - "velocityY": 0.24135459142842275, - "timestamp": 2.45567948106767 - }, - { - "x": 6.262456749596831, - "y": 4.218124574322582, - "heading": -0.09725285915389743, - "angularVelocity": -4.259217438362485e-8, - "velocityX": -3.7171734499943, - "velocityY": 0.24135459144719548, - "timestamp": 2.5327201729205635 - }, - { - "x": 5.976083139919176, - "y": 4.236718770600948, - "heading": -0.09725286243522813, - "angularVelocity": -4.259217585749446e-8, - "velocityX": -3.717173389674077, - "velocityY": 0.24135552045497527, - "timestamp": 2.609760864773457 - }, - { - "x": 5.690024921889411, - "y": 4.259659658304737, - "heading": -0.09725286572349402, - "angularVelocity": -4.26821957688407e-8, - "velocityX": -3.71307955769639, - "velocityY": 0.2977762420357482, - "timestamp": 2.6868015566263495 - }, - { - "x": 5.4092552652272206, - "y": 4.319022844462833, - "heading": -0.09725286915389218, - "angularVelocity": -4.452709450605306e-8, - "velocityX": -3.644433219762762, - "velocityY": 0.7705432639604047, - "timestamp": 2.763842248479243 + "heading": -1.3488010425840145e-28, + "angularVelocity": 0.027046406409671455, + "velocityX": 0.3814719827139668, + "velocityY": -0.03332661105134125, + "timestamp": 1.685475145771722 + }, + { + "x": 8.20063541706578, + "y": 4.096934548496511, + "heading": -0.0010815835377122093, + "angularVelocity": -0.014043212095795144, + "velocityX": -0.09442255808535296, + "velocityY": -0.0026468579224898756, + "timestamp": 1.7624933899123247 + }, + { + "x": 8.156710596136428, + "y": 4.099093585175751, + "heading": -0.005327811233814745, + "angularVelocity": -0.05513275125242723, + "velocityX": -0.5703170907033845, + "velocityY": 0.028032795389339775, + "timestamp": 1.8395116340529274 + }, + { + "x": 8.076133213947529, + "y": 4.103615501486163, + "heading": -0.012738542680979896, + "angularVelocity": -0.09622046736921626, + "velocityX": -1.0462116228175795, + "velocityY": 0.05871227474567349, + "timestamp": 1.91652987819353 + }, + { + "x": 7.958903270196668, + "y": 4.11050027828992, + "heading": -0.023313548124616522, + "angularVelocity": -0.1373052003669573, + "velocityX": -1.5221061588582727, + "velocityY": 0.08939150561765219, + "timestamp": 1.9935481223341327 + }, + { + "x": 7.805020764688039, + "y": 4.119747890676612, + "heading": -0.03705257485110129, + "angularVelocity": -0.17838665214703153, + "velocityX": -1.9980006974413145, + "velocityY": 0.12007041305446839, + "timestamp": 2.0705663664747354 + }, + { + "x": 7.614485698203272, + "y": 4.131358307939121, + "heading": -0.05395543434106899, + "angularVelocity": -0.21946565620361758, + "velocityX": -2.47389522587572, + "velocityY": 0.15074892179199947, + "timestamp": 2.147584610615338 + }, + { + "x": 7.387298074694608, + "y": 4.1453314935724395, + "heading": -0.07402210734318278, + "angularVelocity": -0.26054441030206643, + "velocityX": -2.949789702994487, + "velocityY": 0.1814269565508347, + "timestamp": 2.2246028547559407 + }, + { + "x": 7.1234579111014344, + "y": 4.161667405314612, + "heading": -0.09725284904387498, + "angularVelocity": -0.3016264777249771, + "velocityX": -3.4256839601733207, + "velocityY": 0.21210444258311068, + "timestamp": 2.3016210988965433 + }, + { + "x": 6.836784104612281, + "y": 4.180147638379387, + "heading": -0.09725285233031565, + "angularVelocity": -4.2670937406098884e-8, + "velocityX": -3.722154532188625, + "velocityY": 0.23994617471463595, + "timestamp": 2.378639343037146 + }, + { + "x": 6.550110299323958, + "y": 4.198627890073297, + "heading": -0.09725285561668488, + "angularVelocity": -4.2670009668741826e-8, + "velocityX": -3.7221545165971346, + "velocityY": 0.23994641659413532, + "timestamp": 2.4556575871777486 + }, + { + "x": 6.26343649403574, + "y": 4.217108141768841, + "heading": -0.0972528589030541, + "angularVelocity": -4.267000957382731e-8, + "velocityX": -3.722154516595767, + "velocityY": 0.23994641661534885, + "timestamp": 2.5326758313183513 + }, + { + "x": 5.976762694007825, + "y": 4.235588475064355, + "heading": -0.0972528621894233, + "angularVelocity": -4.2670009669994304e-8, + "velocityX": -3.722154448296313, + "velocityY": 0.23994747610419156, + "timestamp": 2.609694075458954 + }, + { + "x": 5.690447941814378, + "y": 4.258981914103625, + "heading": -0.09725286548373305, + "angularVelocity": -4.277310873834796e-8, + "velocityX": -3.7174925939724037, + "velocityY": 0.3037389296562527, + "timestamp": 2.6867123195995557 + }, + { + "x": 5.409459513474409, + "y": 4.318722126567167, + "heading": -0.0972528689233277, + "angularVelocity": -4.4659478908136544e-8, + "velocityX": -3.6483359426761925, + "velocityY": 0.7756631319000459, + "timestamp": 2.7637305637401575 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": -7.968133349338986e-8, - "velocityX": -3.5158347290283, - "velocityY": 1.2306645781352283, - "timestamp": 2.8408829403321363 - }, - { - "x": 4.909018653493503, - "y": 4.524275328875486, - "heading": -0.09725287687080804, - "angularVelocity": -2.1964896325716688e-8, - "velocityX": -3.192337867898693, - "velocityY": 1.5370761958403918, - "timestamp": 2.9127344426669843 - }, - { - "x": 4.71108590609518, - "y": 4.628682093882472, - "heading": -0.09725287720527089, - "angularVelocity": -4.654917845961093e-9, - "velocityX": -2.754747513502236, - "velocityY": 1.453090911313464, - "timestamp": 2.9845859450018324 - }, - { - "x": 4.54294597125426, - "y": 4.721368217002082, - "heading": -0.09725287703485493, - "angularVelocity": 2.3717800726027118e-9, - "velocityX": -2.3401032598781413, - "velocityY": 1.2899677822694209, - "timestamp": 3.0564374473366804 - }, - { - "x": 4.40369795049572, - "y": 4.80026148816612, - "heading": -0.09725287664966911, - "angularVelocity": 5.360859554294991e-9, - "velocityX": -1.937997344990855, - "velocityY": 1.0980044759032763, - "timestamp": 3.1282889496715285 - }, - { - "x": 4.292814205558175, - "y": 4.864307082758427, - "heading": -0.09725287620265413, - "angularVelocity": 6.221372875954885e-9, - "velocityX": -1.5432348849269109, - "velocityY": 0.8913605493429588, - "timestamp": 3.2001404520063765 - }, - { - "x": 4.209952469660844, - "y": 4.91286872561946, - "heading": -0.09725287578795568, - "angularVelocity": 5.7716044742121175e-9, - "velocityX": -1.1532359547775557, - "velocityY": 0.6758612037744439, - "timestamp": 3.2719919543412246 - }, - { - "x": 4.154873707750821, - "y": 4.945521497298995, - "heading": -0.09725287546928488, - "angularVelocity": 4.435130638682436e-9, - "velocityX": -0.7665638173206213, - "velocityY": 0.4544480020384788, - "timestamp": 3.3438434566760726 + "angularVelocity": -8.269819203889138e-8, + "velocityX": -3.5195113993654044, + "velocityY": 1.2349277693272307, + "timestamp": 2.8407488078807592 + }, + { + "x": 4.908992808994229, + "y": 4.524204010558568, + "heading": -0.09725287694865485, + "angularVelocity": -2.3032931953732708e-8, + "velocityX": -3.1905636858497752, + "velocityY": 1.535056957756043, + "timestamp": 2.9126483650924264 + }, + { + "x": 4.711042301236892, + "y": 4.628576938226348, + "heading": -0.09725287730265034, + "angularVelocity": -4.9234724349367645e-9, + "velocityX": -2.7531533633035683, + "velocityY": 1.4516491021010645, + "timestamp": 2.9845479223040936 + }, + { + "x": 4.542898981308052, + "y": 4.721263372143452, + "heading": -0.0972528771251371, + "angularVelocity": 2.468905958013942e-9, + "velocityX": -2.3385863063640335, + "velocityY": 1.2891099404721302, + "timestamp": 3.056447479515761 + }, + { + "x": 4.403657347383129, + "y": 4.800175356394304, + "heading": -0.09725287672080585, + "angularVelocity": 5.623556763444036e-9, + "velocityX": -1.9366132327492167, + "velocityY": 1.0975308793424334, + "timestamp": 3.128347036727428 + }, + { + "x": 4.2927849587410325, + "y": 4.864247225534064, + "heading": -0.09725287625075327, + "angularVelocity": 6.537628358410225e-9, + "velocityX": -1.5420454998867026, + "velocityY": 0.8911302325706711, + "timestamp": 3.200246593939095 + }, + { + "x": 4.20993568647492, + "y": 4.9128352917829865, + "heading": -0.09725287581430106, + "angularVelocity": 6.070304666979789e-9, + "velocityX": -1.1522918287550885, + "velocityY": 0.6757769885269709, + "timestamp": 3.2721461511507623 + }, + { + "x": 4.15486747115832, + "y": 4.945509334282957, + "heading": -0.09725287547872855, + "angularVelocity": 4.6672402894194346e-9, + "velocityX": -0.7659047906857641, + "velocityY": 0.45444010738176244, + "timestamp": 3.3440457083624295 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 2.459066789540458e-9, - "velocityX": -0.38234244368231685, - "velocityY": 0.22880870103743692, - "timestamp": 3.4156949590109207 + "angularVelocity": 2.5887684361837973e-9, + "velocityX": -0.3820001604682187, + "velocityY": 0.22882494094405095, + "timestamp": 3.4159452655740967 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 1.7012427908311302e-26, - "velocityX": 2.173382321303034e-25, - "velocityY": 2.8154271739064046e-25, - "timestamp": 3.4875464613457687 + "angularVelocity": 8.091887340768401e-27, + "velocityX": 1.1127966749801626e-25, + "velocityY": 1.4331677688644632e-25, + "timestamp": 3.487844822785764 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceLanePGFE.4.traj b/src/main/deploy/choreo/SourceLanePGFE.4.traj index a806f7df..8a9939e8 100644 --- a/src/main/deploy/choreo/SourceLanePGFE.4.traj +++ b/src/main/deploy/choreo/SourceLanePGFE.4.traj @@ -4,568 +4,568 @@ "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 1.7012427908311302e-26, - "velocityX": 2.173382321303034e-25, - "velocityY": 2.8154271739064046e-25, + "angularVelocity": 8.091887340768401e-27, + "velocityX": 1.1127966749801626e-25, + "velocityY": 1.4331677688644632e-25, "timestamp": 0 }, { - "x": 4.147908938552217, - "y": 4.9451046130456335, - "heading": -0.10362139740935014, - "angularVelocity": -0.09725274804530627, - "velocityX": 0.31316100417438686, - "velocityY": -0.2574227575741351, - "timestamp": 0.06548423818097149 + "x": 4.147895492623476, + "y": 4.945083891393231, + "heading": -0.10366816031073668, + "angularVelocity": -0.09789638985446751, + "velocityX": 0.31273056471816124, + "velocityY": -0.2575538032912827, + "timestamp": 0.0655313748308437 }, { - "x": 4.189500528478716, - "y": 4.912093615670984, - "heading": -0.11521450306933155, - "angularVelocity": -0.17703658135172537, - "velocityX": 0.6351389446045882, - "velocityY": -0.5041060000334822, - "timestamp": 0.13096847636194298 + "x": 4.189465649349023, + "y": 4.9120367483382665, + "heading": -0.11534265064860806, + "angularVelocity": -0.17815115840323564, + "velocityX": 0.6343550220463237, + "velocityY": -0.5042949753499016, + "timestamp": 0.1310627496616874 }, { - "x": 4.2528908246594135, - "y": 4.863879621749849, - "heading": -0.13053557559089707, - "angularVelocity": -0.23396580531675798, - "velocityX": 0.9680237251216575, - "velocityY": -0.7362686848076517, - "timestamp": 0.19645271454291446 + "x": 4.252832779954382, + "y": 4.863778368414071, + "heading": -0.13076421865787793, + "angularVelocity": -0.23533106163387682, + "velocityX": 0.9669739230853712, + "velocityY": -0.7364164119670211, + "timestamp": 0.1965941244925311 }, { - "x": 4.338972089495466, - "y": 4.801805387831604, - "heading": -0.1475645296449939, - "angularVelocity": -0.2600466085752675, - "velocityX": 1.3145341112186142, - "velocityY": -0.9479263352914945, - "timestamp": 0.26193695272388595 + "x": 4.338895975770433, + "y": 4.801661455148063, + "heading": -0.14789285263671287, + "angularVelocity": -0.2613806596160893, + "velocityX": 1.3133128373730498, + "velocityY": -0.9478957739914682, + "timestamp": 0.2621254993233748 }, { - "x": 4.4488531713485, - "y": 4.727871318118375, - "heading": -0.16346474025801716, - "angularVelocity": -0.24280973642972667, - "velocityX": 1.677977554680654, - "velocityY": -1.1290361126124002, - "timestamp": 0.32742119090485744 + "x": 4.448770493584648, + "y": 4.7277002158009624, + "heading": -0.16386651500043536, + "angularVelocity": -0.24375594751301116, + "velocityX": 1.6766704208149943, + "velocityY": -1.1286386030816227, + "timestamp": 0.3276568741542185 }, { - "x": 4.583832471873963, - "y": 4.645249641206774, - "heading": -0.17403705358129207, - "angularVelocity": -0.1614482143635436, - "velocityX": 2.0612486954866247, - "velocityY": -1.26170326183331, - "timestamp": 0.39290542908582893 + "x": 4.583757647526343, + "y": 4.645084735489715, + "heading": -0.1744545811620451, + "angularVelocity": -0.16157247103301028, + "velocityX": 2.0598858835197476, + "velocityY": -1.260701160695988, + "timestamp": 0.3931882489850622 }, { - "x": 4.74502129849887, - "y": 4.559256593692149, - "heading": -0.17264089042553551, - "angularVelocity": 0.021320598582793455, - "velocityX": 2.4614904456771134, - "velocityY": -1.3131869577069004, - "timestamp": 0.4583896672668004 + "x": 4.744961446623189, + "y": 4.559146001551583, + "heading": -0.17298362527772781, + "angularVelocity": 0.022446589715449678, + "velocityX": 2.459948376071222, + "velocityY": -1.3114135657914419, + "timestamp": 0.4587196238159059 }, { - "x": 4.931721031095368, - "y": 4.478536439161179, - "heading": -0.14954301236675954, - "angularVelocity": 0.3527242386930233, - "velocityX": 2.8510636724601572, - "velocityY": -1.2326653981663842, - "timestamp": 0.5238739054477719 + "x": 4.9316701909697125, + "y": 4.478506356002848, + "heading": -0.14972977650571964, + "angularVelocity": 0.3548506167012916, + "velocityX": 2.8491504234189664, + "velocityY": -1.2305501869431363, + "timestamp": 0.5242509986467496 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": 0.7985148568065102, - "velocityX": 3.1560555624992586, - "velocityY": -0.9880598134339889, - "timestamp": 0.5893581436287434 - }, - { - "x": 5.280245938471686, - "y": 4.380962564632748, - "heading": -0.04650808406049355, - "angularVelocity": 1.167166985692144, - "velocityX": 3.2627221399232984, - "velocityY": -0.7560690286372685, - "timestamp": 0.6328350341287017 - }, - { - "x": 5.424678195182071, - "y": 4.359487062895353, - "heading": 0.008191327783648325, - "angularVelocity": 1.2581261266647945, - "velocityX": 3.322046610268118, - "velocityY": -0.493952108590108, - "timestamp": 0.67631192462866 - }, - { - "x": 5.570537921062946, - "y": 4.349646196769418, - "heading": 0.06315884602952881, - "angularVelocity": 1.264292768268082, - "velocityX": 3.354879436030857, - "velocityY": -0.22634705501637128, - "timestamp": 0.7197888151286183 - }, - { - "x": 5.717103514016266, - "y": 4.35150566579831, - "heading": 0.11770080768702691, - "angularVelocity": 1.2545046582287314, - "velocityX": 3.3711148904142765, - "velocityY": 0.04276913568356193, - "timestamp": 0.7632657056285765 - }, - { - "x": 5.863572420206036, - "y": 4.365085009600068, - "heading": 0.17138870164708653, - "angularVelocity": 1.2348604820326599, - "velocityX": 3.368891024759707, - "velocityY": 0.3123347517635909, - "timestamp": 0.8067425961285348 - }, - { - "x": 6.009034373006399, - "y": 4.3903410665362745, - "heading": 0.22417378599549884, - "angularVelocity": 1.2140952064744144, - "velocityX": 3.345730366814133, - "velocityY": 0.5809076188700988, - "timestamp": 0.8502194866284931 - }, - { - "x": 6.152424125530893, - "y": 4.427134155183534, - "heading": 0.2765912547304818, - "angularVelocity": 1.205639780863199, - "velocityX": 3.2980682582309235, - "velocityY": 0.8462677119766714, - "timestamp": 0.8936963771284514 - }, - { - "x": 6.292378614943397, - "y": 4.475129707153334, - "heading": 0.330369231652926, - "angularVelocity": 1.2369324554730028, - "velocityX": 3.2190547162667764, - "velocityY": 1.1039324896026255, - "timestamp": 0.9371732676284097 - }, - { - "x": 6.427989965818357, - "y": 4.530754098625934, - "heading": 0.3892449871584701, - "angularVelocity": 1.3541850585105815, - "velocityX": 3.1191593813520333, - "velocityY": 1.2794013286818071, - "timestamp": 0.980650158128368 - }, - { - "x": 6.568195373885816, - "y": 4.593631496863788, - "heading": 0.42157526724387395, - "angularVelocity": 0.743619879748183, - "velocityX": 3.224826027233803, - "velocityY": 1.4462257423381046, - "timestamp": 1.0241270486283263 - }, - { - "x": 6.710207768570418, - "y": 4.660649558034501, - "heading": 0.440928265304377, - "angularVelocity": 0.44513298531599427, - "velocityX": 3.26638802940008, - "velocityY": 1.5414639915606807, - "timestamp": 1.0676039391282846 - }, - { - "x": 6.85219596425133, - "y": 4.730282709551517, - "heading": 0.4559517105455699, - "angularVelocity": 0.34555013176959587, - "velocityX": 3.265831434772203, - "velocityY": 1.6016129653311566, - "timestamp": 1.1110808296282428 - }, - { - "x": 6.992339110580002, - "y": 4.802100760534676, - "heading": 0.47358684784417715, - "angularVelocity": 0.4056209424320313, - "velocityX": 3.223393962105149, - "velocityY": 1.6518672369917733, - "timestamp": 1.1545577201282011 - }, - { - "x": 7.1318594634446475, - "y": 4.8743385269135056, - "heading": 0.49238243611774024, - "angularVelocity": 0.4323121561230585, - "velocityX": 3.2090692609394345, - "velocityY": 1.6615209953641499, - "timestamp": 1.1980346106281594 - }, - { - "x": 7.264750404712455, - "y": 4.9484671577912, - "heading": 0.5297243146446315, - "angularVelocity": 0.8588902770525219, - "velocityX": 3.0565879882309943, - "velocityY": 1.7050122496171904, - "timestamp": 1.2415115011281177 - }, - { - "x": 7.388081081229855, - "y": 5.024967733696522, - "heading": 0.592130827533757, - "angularVelocity": 1.4353950379497669, - "velocityX": 2.8366949682733105, - "velocityY": 1.7595687047903361, - "timestamp": 1.284988391628076 + "angularVelocity": 0.8007904816369491, + "velocityX": 3.154561228516173, + "velocityY": -0.9868900380705385, + "timestamp": 0.5897823734775933 + }, + { + "x": 5.280076264990419, + "y": 4.380996510846826, + "heading": -0.04650142784626664, + "angularVelocity": 1.168572303123448, + "velocityX": 3.262315361452851, + "velocityY": -0.7560984617214831, + "timestamp": 0.6332126749751374 + }, + { + "x": 5.424449362393288, + "y": 4.359481998207729, + "heading": 0.008484168318164002, + "angularVelocity": 1.2660652647677388, + "velocityX": 3.3242481038505836, + "velocityY": -0.49538022756564715, + "timestamp": 0.6766429764726816 + }, + { + "x": 5.570294583588623, + "y": 4.349555966969111, + "heading": 0.06357759732287922, + "angularVelocity": 1.2685481588892704, + "velocityX": 3.3581443408488143, + "velocityY": -0.22855082503120916, + "timestamp": 0.7200732779702257 + }, + { + "x": 5.7169007597582056, + "y": 4.351285887819555, + "heading": 0.11807254033661835, + "angularVelocity": 1.2547677804359936, + "velocityX": 3.3756656323897083, + "velocityY": 0.039832116996496504, + "timestamp": 0.7635035794677698 + }, + { + "x": 5.863478153664376, + "y": 4.364694740352783, + "heading": 0.1715206036233513, + "angularVelocity": 1.2306629575149417, + "velocityX": 3.375002909304243, + "velocityY": 0.3087441733276142, + "timestamp": 0.8069338809653139 + }, + { + "x": 6.009132990316209, + "y": 4.389745616694493, + "heading": 0.223839780266437, + "angularVelocity": 1.2046698926565014, + "velocityX": 3.3537606608618478, + "velocityY": 0.5768064111442623, + "timestamp": 0.850364182462858 + }, + { + "x": 6.152825143314219, + "y": 4.426310930170149, + "heading": 0.27549834591380606, + "angularVelocity": 1.189459061210764, + "velocityX": 3.3085690875560085, + "velocityY": 0.8419309149332661, + "timestamp": 0.8937944839604022 + }, + { + "x": 6.293269117022505, + "y": 4.47409538643224, + "heading": 0.327953495669367, + "angularVelocity": 1.2078007277598, + "velocityX": 3.233778464932586, + "velocityY": 1.1002561486890332, + "timestamp": 0.9372247854579463 + }, + { + "x": 6.42854194701545, + "y": 4.529460812617931, + "heading": 0.38848470588787376, + "angularVelocity": 1.3937552384233347, + "velocityX": 3.1147108200618043, + "velocityY": 1.2748110023786727, + "timestamp": 0.9806550869554904 + }, + { + "x": 6.568582228965848, + "y": 4.592274312002758, + "heading": 0.42163559085903973, + "angularVelocity": 0.7633123378855858, + "velocityX": 3.2244833013263468, + "velocityY": 1.4463058560249087, + "timestamp": 1.0240853884530345 + }, + { + "x": 6.710554540851547, + "y": 4.659324904773962, + "heading": 0.44122441359236847, + "angularVelocity": 0.451040450051598, + "velocityX": 3.2689690605469455, + "velocityY": 1.5438666198298632, + "timestamp": 1.0675156899505787 + }, + { + "x": 6.8526306110413895, + "y": 4.729009831535272, + "heading": 0.45600063364560606, + "angularVelocity": 0.3402283554046515, + "velocityX": 3.2713581368500897, + "velocityY": 1.604523209797458, + "timestamp": 1.1109459914481228 + }, + { + "x": 6.993061785936844, + "y": 4.80083300788703, + "heading": 0.47277807951263723, + "angularVelocity": 0.3863073773038364, + "velocityX": 3.233483767166502, + "velocityY": 1.6537572587613216, + "timestamp": 1.154376292945667 + }, + { + "x": 7.132717903538657, + "y": 4.873239187590198, + "heading": 0.4909920458715961, + "angularVelocity": 0.4193838341184207, + "velocityX": 3.2156377641013956, + "velocityY": 1.6671811432685018, + "timestamp": 1.197806594443211 + }, + { + "x": 7.265296328377782, + "y": 4.947694128111601, + "heading": 0.5290109485090398, + "angularVelocity": 0.8754003846736805, + "velocityX": 3.052671067609889, + "velocityY": 1.7143546775886709, + "timestamp": 1.2412368959407551 + }, + { + "x": 7.388337158351145, + "y": 5.0245588832069705, + "heading": 0.5919048706788004, + "angularVelocity": 1.4481576226984543, + "velocityX": 2.833064144864831, + "velocityY": 1.7698416185233745, + "timestamp": 1.2846671974382993 }, { "x": 7.501500129699707, "y": 5.105462551116943, "heading": 0.6747401864044066, - "angularVelocity": 1.9000751415451151, - "velocityX": 2.608720337761042, - "velocityY": 1.8514391552564737, - "timestamp": 1.3284652821280343 - }, - { - "x": 7.646324439274632, - "y": 5.2181193934144225, - "heading": 0.716438811665056, - "angularVelocity": 0.6791497141368164, - "velocityX": 2.3587681328355696, - "velocityY": 1.8348533498079573, - "timestamp": 1.389863563527074 - }, - { - "x": 7.772039173775546, - "y": 5.317779255820348, - "heading": 0.7438971482542158, - "angularVelocity": 0.44721669668085284, - "velocityX": 2.047528556766434, - "velocityY": 1.62317022781495, - "timestamp": 1.4512618449261137 - }, - { - "x": 7.878893815894361, - "y": 5.4039493449354215, - "heading": 0.7593796608518738, - "angularVelocity": 0.2521652437962253, - "velocityX": 1.7403523304560495, - "velocityY": 1.4034609300387069, - "timestamp": 1.5126601263251533 - }, - { - "x": 7.966980879790036, - "y": 5.476461142089676, - "heading": 0.7636631361192312, - "angularVelocity": 0.06976539358680627, - "velocityX": 1.4346828915809557, - "velocityY": 1.1810069516927688, - "timestamp": 1.574058407724193 - }, - { - "x": 8.036348554918035, - "y": 5.535229545877986, - "heading": 0.7571399208631822, - "angularVelocity": -0.10624426461798567, - "velocityX": 1.1297983192260301, - "velocityY": 0.9571669181807075, - "timestamp": 1.6354566891232327 - }, - { - "x": 8.087026476479373, - "y": 5.580203129397342, - "heading": 0.7400467384743379, - "angularVelocity": -0.27839838509082054, - "velocityX": 0.8253964183780998, - "velocityY": 0.7324892895138765, - "timestamp": 1.6968549705222724 - }, - { - "x": 8.119034784241132, - "y": 5.611347352514501, - "heading": 0.7125421599400041, - "angularVelocity": -0.44796984390452543, - "velocityX": 0.5213225359473886, - "velocityY": 0.5072491022142075, - "timestamp": 1.758253251921312 + "angularVelocity": 1.90731615644646, + "velocityX": 2.605622513464768, + "velocityY": 1.8628391956833978, + "timestamp": 1.3280974989358434 + }, + { + "x": 7.646241899595399, + "y": 5.218086028321965, + "heading": 0.7163743700532657, + "angularVelocity": 0.6780169831508551, + "velocityX": 2.357134680201247, + "velocityY": 1.8340849646658135, + "timestamp": 1.3895033113026347 + }, + { + "x": 7.771890778556216, + "y": 5.317734467190133, + "heading": 0.7437412121017494, + "angularVelocity": 0.445671850817889, + "velocityX": 2.0462049782891385, + "velocityY": 1.6227851245244513, + "timestamp": 1.450909123669426 + }, + { + "x": 7.878702605098028, + "y": 5.403903098685203, + "heading": 0.7591487568785513, + "angularVelocity": 0.25091345888837385, + "velocityX": 1.7394416330460891, + "velocityY": 1.4032650684655832, + "timestamp": 1.5123149360362174 + }, + { + "x": 7.966772466463858, + "y": 5.476419180889776, + "heading": 0.7633894511988232, + "angularVelocity": 0.06906014523415532, + "velocityX": 1.4342267933818382, + "velocityY": 1.180931892430925, + "timestamp": 1.5737207484030087 + }, + { + "x": 8.036149930453574, + "y": 5.535195459636144, + "heading": 0.7568637162743023, + "angularVelocity": -0.10627226760784944, + "velocityX": 1.1298191704607576, + "velocityY": 0.9571777732583954, + "timestamp": 1.6351265607698 + }, + { + "x": 8.086865488706417, + "y": 5.5801792035680275, + "heading": 0.7398132249499985, + "angularVelocity": -0.27766901319466764, + "velocityX": 0.8259081070356892, + "velocityY": 0.7325649184996513, + "timestamp": 1.6965323731365913 + }, + { + "x": 8.118939863866766, + "y": 5.611334997780328, + "heading": 0.7123999066441988, + "angularVelocity": -0.4464287214710813, + "velocityX": 0.5223345140157242, + "velocityY": 0.5073753283516645, + "timestamp": 1.7579381855033827 }, { "x": 8.1323881149292, "y": 5.628637313842773, "heading": 0.6747401864044066, - "angularVelocity": -0.6156845545873708, - "velocityX": 0.2174870433470383, - "velocityY": 0.2816033435187223, - "timestamp": 1.8196515333203518 - }, - { - "x": 8.125741105489272, - "y": 5.631444479838457, - "heading": 0.6235311710708692, - "angularVelocity": -0.7910339249344661, - "velocityX": -0.1026774276383878, - "velocityY": 0.04336274620875669, - "timestamp": 1.8843883463941768 - }, - { - "x": 8.09835684603502, - "y": 5.618815221035742, - "heading": 0.5616094020000137, - "angularVelocity": -0.9565155609411891, - "velocityX": -0.42300907557840733, - "velocityY": -0.19508619907365696, - "timestamp": 1.949125159468002 - }, - { - "x": 8.050222803941077, - "y": 5.59073345676368, - "heading": 0.48977295422299394, - "angularVelocity": -1.1096692031332702, - "velocityX": -0.7435343170052635, - "velocityY": -0.4337835450756361, - "timestamp": 2.013861972541827 - }, - { - "x": 7.981324451485502, - "y": 5.547180062425158, - "heading": 0.40904421087349874, - "angularVelocity": -1.2470299280477726, - "velocityX": -1.0642839704976186, - "velocityY": -0.6727763118158692, - "timestamp": 2.078598785615652 - }, - { - "x": 7.891645067392915, - "y": 5.488132407343552, - "heading": 0.32078665814595236, - "angularVelocity": -1.3633286616518716, - "velocityX": -1.3852919202296423, - "velocityY": -0.9121186582704987, - "timestamp": 2.143335598689477 - }, - { - "x": 7.781166411701721, - "y": 5.413563935212295, - "heading": 0.22693931715370197, - "angularVelocity": -1.4496750231622744, - "velocityX": -1.7065816255922823, - "velocityY": -1.1518712242788174, - "timestamp": 2.208072411763302 - }, - { - "x": 7.649874183396069, - "y": 5.323444723125603, - "heading": 0.13053857941250757, - "angularVelocity": -1.4891177548585681, - "velocityX": -2.0280922410549644, - "velocityY": -1.3920860142423153, - "timestamp": 2.272809224837127 - }, - { - "x": 7.497788734174063, - "y": 5.217751261949818, - "heading": 0.0370488934612777, - "angularVelocity": -1.4441502680185108, - "velocityX": -2.3492884805522913, - "velocityY": -1.6326639535877772, - "timestamp": 2.3375460379109523 - }, - { - "x": 7.325168258315647, - "y": 5.096564905445784, - "heading": -0.04136336755772595, - "angularVelocity": -1.2112468516110346, - "velocityX": -2.66649635133507, - "velocityY": -1.871985208259067, - "timestamp": 2.4022828509847773 - }, - { - "x": 7.133849861870766, - "y": 4.963599555351033, - "heading": -0.06503952290078549, - "angularVelocity": -0.36572939288900796, - "velocityX": -2.955326148457454, - "velocityY": -2.0539372233711526, - "timestamp": 2.4670196640586024 - }, - { - "x": 6.940221298800236, - "y": 4.819868100403324, - "heading": -0.06503954882158174, - "angularVelocity": -4.004027232662301e-7, - "velocityX": -2.9910116651821412, - "velocityY": -2.2202429826719707, - "timestamp": 2.5317564771324275 - }, - { - "x": 6.740356177089726, - "y": 4.684943363475215, - "heading": -0.06503957483267325, - "angularVelocity": -4.017975287461778e-7, - "velocityX": -3.08734879306747, - "velocityY": -2.0842041880288593, - "timestamp": 2.5964932902062525 - }, - { - "x": 6.527131993072097, - "y": 4.57231033799875, - "heading": -0.06503960182577956, - "angularVelocity": -4.1696686974459996e-7, - "velocityX": -3.293708384663109, - "velocityY": -1.739860523378229, - "timestamp": 2.6612301032800776 - }, - { - "x": 6.303024971058329, - "y": 4.483277800206398, - "heading": -0.06503963136622377, - "angularVelocity": -4.5631600956130875e-7, - "velocityX": -3.461817339667266, - "velocityY": -1.3752999810296351, - "timestamp": 2.7259669163539026 - }, - { - "x": 6.070638079320141, - "y": 4.41887996664659, - "heading": -0.06503966569966328, - "angularVelocity": -5.303541813322711e-7, - "velocityX": -3.5897178236619824, - "velocityY": -0.9947637287360487, - "timestamp": 2.7907037294277277 - }, - { - "x": 5.832670509915108, - "y": 4.379864860722445, - "heading": -0.06503973197653827, - "angularVelocity": -0.0000010237895848927841, - "velocityX": -3.67592345229684, - "velocityY": -0.60267263820436, - "timestamp": 2.8554405425015528 - }, - { - "x": 5.593035616597729, - "y": 4.366766567771227, - "heading": -0.0691739587008402, - "angularVelocity": -0.06386206746982208, - "velocityX": -3.7016788738750415, - "velocityY": -0.20233144526718327, - "timestamp": 2.920177355575378 - }, - { - "x": 5.359733281234782, - "y": 4.378764088602827, - "heading": -0.08321570293446177, - "angularVelocity": -0.2169050894984338, - "velocityX": -3.6038588290852247, - "velocityY": 0.1853276406720581, - "timestamp": 2.984914168649203 + "angularVelocity": -0.6132924358176748, + "velocityX": 0.21900615827864758, + "velocityY": 0.28177000507860306, + "timestamp": 1.819343997870174 + }, + { + "x": 8.125871504088575, + "y": 5.631456874364778, + "heading": 0.6237350082465105, + "angularVelocity": -0.7877273460269657, + "velocityX": -0.10064296896850039, + "velocityY": 0.04354547924694944, + "timestamp": 1.8840937852365123 + }, + { + "x": 8.098646920538245, + "y": 5.618838281388744, + "heading": 0.5620672547636553, + "angularVelocity": -0.9524008647928723, + "velocityX": -0.420458269558492, + "velocityY": -0.19488238478129047, + "timestamp": 1.9488435726028506 + }, + { + "x": 8.050701885841999, + "y": 5.590765848775558, + "heading": 0.4905264994241073, + "angularVelocity": -1.1048801586758534, + "velocityX": -0.7404662879429073, + "velocityY": -0.43355250657982763, + "timestamp": 2.013593359969189 + }, + { + "x": 7.982021903063244, + "y": 5.547220937725296, + "heading": 0.4101242431657529, + "angularVelocity": -1.2417377651521577, + "velocityX": -1.0606981979751946, + "velocityY": -0.6725104872375124, + "timestamp": 2.078343147335527 + }, + { + "x": 7.892590228702109, + "y": 5.488181520264753, + "heading": 0.3222094538660446, + "angularVelocity": -1.3577618224799397, + "velocityX": -1.3811886957273356, + "velocityY": -0.9118086693707895, + "timestamp": 2.1430929347018655 + }, + { + "x": 7.782388469757928, + "y": 5.4136218029079215, + "heading": 0.22870015350775494, + "angularVelocity": -1.444163821407427, + "velocityX": -1.7019632561986164, + "velocityY": -1.151505207808494, + "timestamp": 2.2078427220682038 + }, + { + "x": 7.6514017789834075, + "y": 5.323512852150783, + "heading": 0.13259842236832783, + "angularVelocity": -1.4842014939092711, + "velocityX": -2.0229671185394063, + "velocityY": -1.3916485971965442, + "timestamp": 2.272592509434542 + }, + { + "x": 7.49964848633853, + "y": 5.217832335954841, + "heading": 0.039303344877862086, + "angularVelocity": -1.4408553492635434, + "velocityX": -2.3436878917654713, + "velocityY": -1.6321368840646113, + "timestamp": 2.3373422968008803 + }, + { + "x": 7.327377192032374, + "y": 5.096661162563075, + "heading": -0.03916824690732434, + "angularVelocity": -1.211920455293751, + "velocityX": -2.660569266914921, + "velocityY": -1.871375618675154, + "timestamp": 2.4020920841672186 + }, + { + "x": 7.13646132842574, + "y": 4.963335099646079, + "heading": -0.06425840185235647, + "angularVelocity": -0.3874940129622072, + "velocityX": -2.9485172287358923, + "velocityY": -2.059096536683113, + "timestamp": 2.466841871533557 + }, + { + "x": 6.942657433468542, + "y": 4.819228787923636, + "heading": -0.06425842751021618, + "angularVelocity": -3.962616830885222e-7, + "velocityX": -2.993120175989199, + "velocityY": -2.2255874124670814, + "timestamp": 2.5315916588998952 + }, + { + "x": 6.742262463352352, + "y": 4.684438451996059, + "heading": -0.06425845326497977, + "angularVelocity": -3.977582728745682e-7, + "velocityX": -3.0949131768171627, + "velocityY": -2.0817108659363774, + "timestamp": 2.5963414462662335 + }, + { + "x": 6.528555264699025, + "y": 4.57194040086737, + "heading": -0.06425848001742611, + "angularVelocity": -4.1316655101928246e-7, + "velocityX": -3.3005081150957944, + "velocityY": -1.7374273446212585, + "timestamp": 2.661091233632572 + }, + { + "x": 6.30400572206588, + "y": 4.483035506841414, + "heading": -0.06425850931637903, + "angularVelocity": -4.5249496742682505e-7, + "velocityX": -3.4679579928610247, + "velocityY": -1.3730530653784834, + "timestamp": 2.72584102099891 + }, + { + "x": 6.071209363502924, + "y": 4.41875151951687, + "heading": -0.06425854338858503, + "angularVelocity": -5.262134035469361e-7, + "velocityX": -3.5953223636990725, + "velocityY": -0.9928061533367126, + "timestamp": 2.7905908083652484 + }, + { + "x": 5.832857094389743, + "y": 4.379831537545609, + "heading": -0.06425861345468233, + "angularVelocity": -0.0000010821054427575679, + "velocityX": -3.6811282138216472, + "velocityY": -0.6010827765512304, + "timestamp": 2.8553405957315867 + }, + { + "x": 5.593007284030673, + "y": 4.3668097640136185, + "heading": -0.06894618880268864, + "angularVelocity": -0.07239522380954139, + "velocityX": -3.704256339902067, + "velocityY": -0.20110913196235056, + "timestamp": 2.920090383097925 + }, + { + "x": 5.359682212088612, + "y": 4.37881821771202, + "heading": -0.08316362699074491, + "angularVelocity": -0.2195750560170572, + "velocityX": -3.6034878481062056, + "velocityY": 0.1854593534100846, + "timestamp": 2.9848401704642633 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": -0.21683446699990425, - "velocityX": -3.419080202788392, - "velocityY": 0.5417320490954629, - "timestamp": 3.049650981723028 - }, - { - "x": 4.905765298437056, - "y": 4.481387310799775, - "heading": -0.10673913036260185, - "angularVelocity": -0.12897497729525575, - "velocityX": -3.1628016184093535, - "velocityY": 0.9184524777738038, - "timestamp": 3.123202109770821 - }, - { - "x": 4.700482894835419, - "y": 4.568326227070152, - "heading": -0.11059429780349972, - "angularVelocity": -0.05241479693408341, - "velocityX": -2.791016386155829, - "velocityY": 1.1820201617286428, - "timestamp": 3.196753237818614 - }, - { - "x": 4.527923671898321, - "y": 4.662591020242044, - "heading": -0.11045427798457029, - "angularVelocity": 0.0019037072937678628, - "velocityX": -2.3461125276687587, - "velocityY": 1.2816226708397902, - "timestamp": 3.270304365866407 - }, - { - "x": 4.3886983023339585, - "y": 4.75311454797141, - "heading": -0.10805031221492864, - "angularVelocity": 0.03268428144405263, - "velocityX": -1.8929059724807213, - "velocityY": 1.2307564837148823, - "timestamp": 3.3438554939142 - }, - { - "x": 4.281048300464299, - "y": 4.832306401053517, - "heading": -0.1047189693449996, - "angularVelocity": 0.04529288616436113, - "velocityX": -1.4636077613889036, - "velocityY": 1.076691210373392, - "timestamp": 3.4174066219619927 - }, - { - "x": 4.20282348849849, - "y": 4.895385445772344, - "heading": -0.10139996108706777, - "angularVelocity": 0.04512518497031254, - "velocityX": -1.0635433343045229, - "velocityY": 0.8576217169346194, - "timestamp": 3.4909577500097857 - }, - { - "x": 4.15212537277285, - "y": 4.939288518901389, - "heading": -0.09875289338109836, - "angularVelocity": 0.0359894916125467, - "velocityX": -0.6892907977250395, - "velocityY": 0.5969055036180668, - "timestamp": 3.5645088780575787 + "angularVelocity": -0.21759528293334457, + "velocityX": -3.417606386475971, + "velocityY": 0.5407875255417884, + "timestamp": 3.0495899578306016 + }, + { + "x": 4.905742763187506, + "y": 4.481316212156489, + "heading": -0.10678575188012002, + "angularVelocity": -0.1295126274523243, + "velocityX": -3.1607598704228184, + "velocityY": 0.916804725056799, + "timestamp": 3.123195727206502 + }, + { + "x": 4.7004457351805575, + "y": 4.5681827460061015, + "heading": -0.11066751280962474, + "angularVelocity": -0.052737183000978086, + "velocityX": -2.7891431574949794, + "velocityY": 1.1801593079747448, + "timestamp": 3.1968014965824025 + }, + { + "x": 4.527868340415521, + "y": 4.662417244866579, + "heading": -0.11053335894042673, + "angularVelocity": 0.001822599917580837, + "velocityX": -2.344617768801401, + "velocityY": 1.280259681537001, + "timestamp": 3.270407265958303 + }, + { + "x": 4.3886358627816255, + "y": 4.752956990495115, + "heading": -0.10811901035095504, + "angularVelocity": 0.03280107809405142, + "velocityX": -1.8915973410024705, + "velocityY": 1.2300631648336415, + "timestamp": 3.3440130353342035 + }, + { + "x": 4.28099452080793, + "y": 4.832191591556716, + "heading": -0.1047684534601318, + "angularVelocity": 0.04552030254193966, + "velocityX": -1.462403598065479, + "velocityY": 1.0764726968201856, + "timestamp": 3.417618804710104 + }, + { + "x": 4.202788693101616, + "y": 4.895319939941382, + "heading": -0.10142823841231594, + "angularVelocity": 0.045379799384441775, + "velocityX": -1.0624958935884563, + "velocityY": 0.8576548947171904, + "timestamp": 3.4912245740860044 + }, + { + "x": 4.1521113166286625, + "y": 4.9392645069883585, + "heading": -0.0987633314851406, + "angularVelocity": 0.03620513649637662, + "velocityX": -0.6884973406656041, + "velocityY": 0.5970261219953313, + "timestamp": 3.564830343461905 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 0.020394222744298442, - "velocityX": -0.3361409221475862, - "velocityY": 0.3082648480890461, - "timestamp": 3.6380600061053716 + "angularVelocity": 0.020520894018912154, + "velocityX": -0.33570042229438307, + "velocityY": 0.30836223056847767, + "timestamp": 3.6384361128378053 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 1.761901225942554e-27, - "velocityX": -1.9870172205028814e-26, - "velocityY": -2.1545999896560787e-26, - "timestamp": 3.7116111341531646 + "angularVelocity": 6.898541114622747e-27, + "velocityX": -7.290217474147722e-26, + "velocityY": -6.920027775581766e-26, + "timestamp": 3.712041882213706 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceLanePGFE.traj b/src/main/deploy/choreo/SourceLanePGFE.traj index 355234d2..8902fb5e 100644 --- a/src/main/deploy/choreo/SourceLanePGFE.traj +++ b/src/main/deploy/choreo/SourceLanePGFE.traj @@ -3,1883 +3,1883 @@ { "x": 0.387, "y": 4.121134281158447, - "heading": 1.1679855597934721e-27, - "angularVelocity": 5.433585185667626e-28, - "velocityX": 7.442598398670663e-27, - "velocityY": 2.3270251613663043e-26, + "heading": 3.980405256540069e-28, + "angularVelocity": 1.0894029802546308e-28, + "velocityX": -2.179791595178488e-28, + "velocityY": 6.106168293685067e-28, "timestamp": 0 }, { - "x": 0.4005104003556791, - "y": 4.115348381940823, - "heading": -0.015277315995294262, - "angularVelocity": -0.30891158633586036, - "velocityX": 0.2731840597648779, - "velocityY": -0.11699249437834791, - "timestamp": 0.04945530265311637 - }, - { - "x": 0.4275520628000622, - "y": 4.103759116150694, - "heading": -0.04541480648332661, - "angularVelocity": -0.6093884552566431, - "velocityX": 0.5467899495844878, - "velocityY": -0.23433818353953934, - "timestamp": 0.09891060530623275 - }, - { - "x": 0.4681490763693427, - "y": 4.086345867978677, - "heading": -0.08991129386234921, - "angularVelocity": -0.899731373420656, - "velocityX": 0.8208829264281612, - "velocityY": -0.35210073011087883, - "timestamp": 0.14836590795934912 - }, - { - "x": 0.5223311730580589, - "y": 4.063083084813811, - "heading": -0.14811250055399447, - "angularVelocity": -1.176844616640472, - "velocityX": 1.0955770924859953, - "velocityY": -0.47037995759592216, - "timestamp": 0.1978212106124655 - }, - { - "x": 0.5901369333154594, - "y": 4.033939247984653, - "heading": -0.2191152852440036, - "angularVelocity": -1.435696090832334, - "velocityX": 1.3710513659777948, - "velocityY": -0.5892965013999555, - "timestamp": 0.24727651326558187 - }, - { - "x": 0.6716171763310703, - "y": 3.9988769782713436, - "heading": -0.3016414776380596, - "angularVelocity": -1.6687026055204155, - "velocityX": 1.647553217642207, - "velocityY": -0.708968863445023, - "timestamp": 0.29673181591869824 - }, - { - "x": 0.7668367655706729, - "y": 3.9578537486428593, - "heading": -0.39387111054062807, - "angularVelocity": -1.8649088763944044, - "velocityX": 1.9253666266584337, - "velocityY": -0.8295011339072073, - "timestamp": 0.3461871185718146 - }, - { - "x": 0.8758711660113281, - "y": 3.91082124740519, - "heading": -0.49317423957558376, - "angularVelocity": -2.00793693916861, - "velocityX": 2.2047059585385944, - "velocityY": -0.9510102802839819, - "timestamp": 0.395642421224931 - }, - { - "x": 0.9987875307433454, - "y": 3.857723681335083, - "heading": -0.5954683580210642, - "angularVelocity": -2.068415578466479, - "velocityX": 2.4854031446165235, - "velocityY": -1.0736475811812884, - "timestamp": 0.44509772387804736 - }, - { - "x": 1.1355416361286799, - "y": 3.7985302060231096, - "heading": -0.6929936795383219, - "angularVelocity": -1.9719891757878503, - "velocityX": 2.765206116410594, - "velocityY": -1.1969085646318132, - "timestamp": 0.49455302653116373 - }, - { - "x": 1.2843691968945923, - "y": 3.733334425660418, - "heading": -0.76082235123736, - "angularVelocity": -1.3715146417118123, - "velocityX": 3.0093347483848483, - "velocityY": -1.3182768452551967, - "timestamp": 0.5440083291842801 - }, - { - "x": 1.4440686661947881, - "y": 3.662157825631026, - "heading": -0.7899998870158123, - "angularVelocity": -0.5899779035446591, - "velocityX": 3.2291677683248934, - "velocityY": -1.439210685427017, - "timestamp": 0.5934636318373965 - }, - { - "x": 1.613363599943702, - "y": 3.5895175490832267, - "heading": -0.789999943508809, - "angularVelocity": -0.0000011423041349866365, - "velocityX": 3.423190733183105, - "velocityY": -1.468806632471835, - "timestamp": 0.6429189344905129 + "x": 0.40050242656101365, + "y": 4.1153504673922905, + "heading": -0.015208632237685107, + "angularVelocity": -0.3074428187596394, + "velocityX": 0.2729518353219713, + "velocityY": -0.11691991624610248, + "timestamp": 0.04946816549185787 + }, + { + "x": 0.42752783346651274, + "y": 4.103765611590411, + "heading": -0.045213195112198845, + "angularVelocity": -0.6065428660266832, + "velocityX": 0.5463191658066905, + "velocityY": -0.23418810232180645, + "timestamp": 0.09893633098371574 + }, + { + "x": 0.46809995023859424, + "y": 4.086359376332899, + "heading": -0.08951802911831011, + "angularVelocity": -0.8956231460292083, + "velocityX": 0.8201661890768802, + "velocityY": -0.35186740976632624, + "timestamp": 0.1484044964755736 + }, + { + "x": 0.5222480472443771, + "y": 4.063106542737477, + "heading": -0.14747661029050188, + "angularVelocity": -1.171633930547341, + "velocityX": 1.094604913430544, + "velocityY": -0.47005651744352356, + "timestamp": 0.19787266196743147 + }, + { + "x": 0.5900100832649373, + "y": 4.033975964464713, + "heading": -0.21819722216411808, + "angularVelocity": -1.4296186480830055, + "velocityX": 1.3698109753375314, + "velocityY": -0.5888752490236961, + "timestamp": 0.24734082745928934 + }, + { + "x": 0.6714360586881531, + "y": 3.998930617330974, + "heading": -0.3004183343874937, + "angularVelocity": -1.6621015031759903, + "velocityX": 1.64602779613119, + "velocityY": -0.7084424252503851, + "timestamp": 0.2968089929511472 + }, + { + "x": 0.7665898631710647, + "y": 3.95792823980715, + "heading": -0.3923434473802076, + "angularVelocity": -1.8582680816785926, + "velocityX": 1.9235361476781112, + "velocityY": -0.8288639191718736, + "timestamp": 0.3462771584430051 + }, + { + "x": 0.8755460489765932, + "y": 3.910920647028603, + "heading": -0.4913759605059717, + "angularVelocity": -2.0019443240130714, + "velocityX": 2.202551574779177, + "velocityY": -0.9502594711397636, + "timestamp": 0.39574532393486295 + }, + { + "x": 0.998371594898006, + "y": 3.857851824015994, + "heading": -0.593488171000786, + "angularVelocity": -2.064200470737515, + "velocityX": 2.482920979586945, + "velocityY": -1.072787367086499, + "timestamp": 0.4452134894267208 + }, + { + "x": 1.1350267865800456, + "y": 3.7986882165536997, + "heading": -0.6910493311323551, + "angularVelocity": -1.972200892463397, + "velocityX": 2.7624875578725923, + "velocityY": -1.1959935622037938, + "timestamp": 0.49468165491857863 + }, + { + "x": 1.2838522334103695, + "y": 3.7335275480002674, + "heading": -0.7598417160155273, + "angularVelocity": -1.39063949914405, + "velocityX": 3.008509520225074, + "velocityY": -1.3172242775842857, + "timestamp": 0.5441498204104365 + }, + { + "x": 1.4435365539100036, + "y": 3.6623850951483443, + "heading": -0.7899998876377602, + "angularVelocity": -0.6096480700744137, + "velocityX": 3.2280218785537347, + "velocityY": -1.4381461722819, + "timestamp": 0.5936179859022943 + }, + { + "x": 1.6130975326130776, + "y": 3.5896311577651607, + "heading": -0.7899999438199378, + "angularVelocity": -0.000001135723893796383, + "velocityX": 3.427678730697678, + "velocityY": -1.4707223657840804, + "timestamp": 0.6430861513941522 }, { "x": 1.7826586961746216, "y": 3.5168776512145996, "heading": -0.79, - "angularVelocity": -0.0000011422676237323725, - "velocityX": 3.423194018614537, - "velocityY": -1.468798975473458, - "timestamp": 0.6923742371436292 + "angularVelocity": -0.0000011356811339822919, + "velocityX": 3.427682467615514, + "velocityY": -1.4707136564936016, + "timestamp": 0.69255431688601 }, { - "x": 2.0239723336710425, - "y": 3.4134441767348855, + "x": 2.0243963751949394, + "y": 3.413262421214484, "heading": -0.79, - "angularVelocity": -4.2462946476416713e-16, - "velocityX": 3.4237473592821535, - "velocityY": -1.467509622685751, - "timestamp": 0.7628565514540567 + "angularVelocity": -4.062908416081592e-16, + "velocityX": 3.4282324773027724, + "velocityY": -1.4694320640008167, + "timestamp": 0.7630681109827596 }, { - "x": 2.250495583067251, - "y": 3.3163502598603043, + "x": 2.2508253931606723, + "y": 3.316208894447284, "heading": -0.79, - "angularVelocity": 2.583795012505271e-16, - "velocityX": 3.2139019782824456, - "velocityY": -1.37756425600538, - "timestamp": 0.8333388657644842 + "angularVelocity": -3.3647602923838933e-16, + "velocityX": 3.2111308271833634, + "velocityY": -1.3763764666249076, + "timestamp": 0.8335819050795092 }, { - "x": 2.4487034476982825, - "y": 3.23139307341843, + "x": 2.4489508052764966, + "y": 3.2312870493551733, "heading": -0.79, - "angularVelocity": 2.1975738712772502e-16, - "velocityX": 2.812164534752054, - "velocityY": -1.2053688542021173, - "timestamp": 0.9038211800749117 + "angularVelocity": -3.271122096502082e-16, + "velocityX": 2.8097397772127017, + "velocityY": -1.2043295383537609, + "timestamp": 0.9040956991762589 }, { - "x": 2.6185959113515174, - "y": 3.1585726243584236, + "x": 2.6187725953400465, + "y": 3.158496892882915, "heading": -0.79, - "angularVelocity": 1.8378454574977131e-16, - "velocityX": 2.4104268611977178, - "velocityY": -1.0331733538044932, - "timestamp": 0.9743034943853393 + "angularVelocity": -2.53261943200864e-16, + "velocityX": 2.4083484974662293, + "velocityY": -1.0322825115946104, + "timestamp": 0.9746094932730085 }, { - "x": 2.760172968622745, - "y": 3.0978889149966715, + "x": 2.7602907579505302, + "y": 3.0978384273454327, "heading": -0.79, - "angularVelocity": 1.432046748255306e-16, - "velocityX": 2.0086891109686786, - "velocityY": -0.8609778205420586, - "timestamp": 1.0447858086957669 + "angularVelocity": -2.2579655206340545e-16, + "velocityX": 2.0069571411277565, + "velocityY": -0.8602354520060983, + "timestamp": 1.0451232873697582 }, { - "x": 2.8734346168098597, - "y": 3.049341946491368, + "x": 2.873505290407551, + "y": 3.0493116539001877, "heading": -0.79, - "angularVelocity": 9.739728624843172e-17, - "velocityX": 1.6069513224022827, - "velocityY": -0.6887822708472163, - "timestamp": 1.1152681230061945 + "angularVelocity": -1.4176471677644102e-16, + "velocityX": 1.6055657464932764, + "velocityY": -0.6881883760029022, + "timestamp": 1.115637081466508 }, { - "x": 2.9583808542915984, - "y": 3.0129317195374297, + "x": 2.9584161910908713, + "y": 3.0129165732416565, "heading": -0.79, - "angularVelocity": 7.507925930031045e-17, - "velocityX": 1.2052135108334718, - "velocityY": -0.5165867112929291, - "timestamp": 1.185750437316622 + "angularVelocity": -1.2154347758080432e-16, + "velocityX": 1.2041743288811908, + "velocityY": -0.5161412901508954, + "timestamp": 1.1861508755632577 }, { - "x": 3.0150116799871185, - "y": 2.9886582345981343, + "x": 3.0150234589203313, + "y": 2.9886531858328245, "heading": -0.79, - "angularVelocity": 2.284084326875716e-17, - "velocityX": 0.8034756839297177, - "velocityY": -0.3443911451656786, - "timestamp": 1.2562327516270497 + "angularVelocity": -8.455163929533081e-17, + "velocityX": 0.8027828959507013, + "velocityY": -0.3440941977330144, + "timestamp": 1.2566646696600074 }, { "x": 3.0433270931243896, "y": 2.9765214920043945, "heading": -0.79, - "angularVelocity": 4.059583267317121e-17, - "velocityX": 0.40173784607243285, - "velocityY": -0.17219557434345442, - "timestamp": 1.3267150659374773 + "angularVelocity": -3.205512140199398e-17, + "velocityX": 0.4013914520784945, + "velocityY": -0.17204710062522346, + "timestamp": 1.3271784637567572 }, { "x": 3.0433270931243896, "y": 2.9765214920043945, "heading": -0.79, - "angularVelocity": -5.776011429095129e-29, - "velocityX": -2.265299874372296e-27, - "velocityY": 1.6360733128925837e-27, - "timestamp": 1.397197380247905 - }, - { - "x": 3.05997928479042, - "y": 2.9660593331101914, - "heading": -0.757416423789292, - "angularVelocity": 0.5582463314772621, - "velocityX": 0.2852978705745241, - "velocityY": -0.17924557403559996, - "timestamp": 1.4555651193147854 - }, - { - "x": 3.09346940981965, - "y": 2.9451270864719548, - "heading": -0.6941025427131016, - "angularVelocity": 1.0847410245519795, - "velocityX": 0.5737780075883255, - "velocityY": -0.35862699108923296, - "timestamp": 1.5139328583816658 - }, - { - "x": 3.144014292961821, - "y": 2.9136974289532866, - "heading": -0.6025701359612305, - "angularVelocity": 1.5682020276130446, - "velocityX": 0.8659729492734705, - "velocityY": -0.5384765286634581, - "timestamp": 1.5723005974485462 - }, - { - "x": 3.211860363047214, - "y": 2.8716921302721943, - "heading": -0.4864204343113474, - "angularVelocity": 1.9899640367565647, - "velocityX": 1.1623898950009328, - "velocityY": -0.7196663662603233, - "timestamp": 1.6306683365154266 - }, - { - "x": 3.2972613235718167, - "y": 2.8189697938754956, - "heading": -0.350515523406248, - "angularVelocity": 2.3284251382321566, - "velocityX": 1.463153479814365, - "velocityY": -0.903278715940788, - "timestamp": 1.689036075582307 - }, - { - "x": 3.4004578303174937, - "y": 2.7553674746788306, - "heading": -0.2010210241746004, - "angularVelocity": 2.5612521852242733, - "velocityX": 1.7680401604631244, - "velocityY": -1.0896827633461483, - "timestamp": 1.7474038146491875 - }, - { - "x": 3.521660389826237, - "y": 2.6807799872839095, - "heading": -0.04666004939323709, - "angularVelocity": 2.644628304078894, - "velocityX": 2.0765333974965823, - "velocityY": -1.2778889260976047, - "timestamp": 1.805771553716068 - }, - { - "x": 3.6607588635338284, - "y": 2.5953087461465145, - "heading": 0.09647781625083168, - "angularVelocity": 2.4523455582210394, - "velocityX": 2.383139657820331, - "velocityY": -1.4643575801258646, - "timestamp": 1.8641392927829483 - }, - { - "x": 3.8159145666935705, - "y": 2.5005609897698613, - "heading": 0.19948059626332554, - "angularVelocity": 1.7647210883818645, - "velocityX": 2.6582441883170693, - "velocityY": -1.6232898154250444, - "timestamp": 1.9225070318498287 - }, - { - "x": 3.9841103579797825, - "y": 2.394344369414975, - "heading": 0.2530307730268158, - "angularVelocity": 0.9174618996656708, - "velocityX": 2.8816567846406667, - "velocityY": -1.8197830180329346, - "timestamp": 1.9808747709167092 - }, - { - "x": 4.165386797993937, - "y": 2.276764205392623, - "heading": 0.2569441402014935, - "angularVelocity": 0.06704674940712647, - "velocityX": 3.1057642956915044, - "velocityY": -2.014471793872694, - "timestamp": 2.0392425099835894 - }, - { - "x": 4.350490054886, - "y": 2.162710813518831, - "heading": 0.2569442210228683, - "angularVelocity": 0.0000013846925729023855, - "velocityX": 3.171328200325918, - "velocityY": -1.9540484811841776, - "timestamp": 2.0976102490504696 - }, - { - "x": 4.53559341768799, - "y": 2.048657593531974, - "heading": 0.2569443018441724, - "angularVelocity": 0.0000013846913619086116, - "velocityX": 3.171330014854447, - "velocityY": -1.9540455362879443, - "timestamp": 2.1559779881173498 - }, - { - "x": 4.720696780504463, - "y": 1.934604373568621, - "heading": 0.25694438266547676, - "angularVelocity": 0.0000013846913668711097, - "velocityX": 3.1713300151025647, - "velocityY": -1.9540455358852609, - "timestamp": 2.21434572718423 - }, - { - "x": 4.9058004208396335, - "y": 1.8205516040071454, - "heading": 0.25694446348677025, - "angularVelocity": 0.0000013846911809077847, - "velocityX": 3.1713347697615966, - "velocityY": -1.9540378192615708, - "timestamp": 2.27271346625111 - }, - { - "x": 5.096209165426888, - "y": 1.7155949376896078, - "heading": 0.25694454460803495, - "angularVelocity": 0.0000013898305117986064, - "velocityX": 3.2622258054072626, - "velocityY": -1.7981965379415077, - "timestamp": 2.3310812053179903 - }, - { - "x": 5.295905298565607, - "y": 1.629613278960399, - "heading": 0.25694462919552075, - "angularVelocity": 0.0000014492164191385246, - "velocityX": 3.4213443304682105, - "velocityY": -1.4731024381582958, - "timestamp": 2.3894489443848705 + "angularVelocity": -3.649032455220026e-29, + "velocityX": -7.926410421294837e-28, + "velocityY": -2.2668198189886467e-28, + "timestamp": 1.397692257853507 + }, + { + "x": 3.0599636784357562, + "y": 2.9660757817861065, + "heading": -0.7576340217382036, + "angularVelocity": 0.5546537792406501, + "velocityX": 0.2851001394727132, + "velocityY": -0.17900749368867203, + "timestamp": 1.4560457379399623 + }, + { + "x": 3.0934189065832722, + "y": 2.945176443498765, + "heading": -0.694726478822482, + "angularVelocity": 1.0780426946690942, + "velocityX": 0.573320187552645, + "velocityY": -0.35815067509902887, + "timestamp": 1.5143992180264176 + }, + { + "x": 3.1439054612634614, + "y": 2.91379685342996, + "heading": -0.6037471047717198, + "angularVelocity": 1.5591079386519706, + "velocityX": 0.8651849830616652, + "velocityY": -0.5377501054318186, + "timestamp": 1.572752698112873 + }, + { + "x": 3.211665554237461, + "y": 2.871860906483434, + "heading": -0.4882361709178232, + "angularVelocity": 1.9795037705164775, + "velocityX": 1.1612005466273392, + "velocityY": -0.7186537441193689, + "timestamp": 1.6311061781993283 + }, + { + "x": 3.2969494486430677, + "y": 2.819230447903095, + "heading": -0.3529801383687169, + "angularVelocity": 2.317874312701035, + "velocityX": 1.4615048541963882, + "velocityY": -0.9019249323667187, + "timestamp": 1.6894596582857837 + }, + { + "x": 3.3999954148233655, + "y": 2.7557448974167267, + "heading": -0.20405555239709897, + "angularVelocity": 2.552111472203101, + "velocityX": 1.765892385983268, + "velocityY": -1.0879479748647278, + "timestamp": 1.747813138372239 + }, + { + "x": 3.5210139565582126, + "y": 2.681298833579533, + "heading": -0.05002957366551629, + "angularVelocity": 2.6395337262384486, + "velocityX": 2.0738873081013947, + "velocityY": -1.275777618179687, + "timestamp": 1.8061666184586944 + }, + { + "x": 3.6599142962012903, + "y": 2.5959840440263875, + "heading": 0.09335553646223951, + "angularVelocity": 2.457181815297378, + "velocityX": 2.3803265792765944, + "velocityY": -1.4620343024399716, + "timestamp": 1.8645200985451498 + }, + { + "x": 3.8149142954051776, + "y": 2.5012941331840683, + "heading": 0.19750147837600968, + "angularVelocity": 1.7847426024886535, + "velocityX": 2.656225455178376, + "velocityY": -1.6226951794824969, + "timestamp": 1.9228735786316051 + }, + { + "x": 3.982928508847516, + "y": 2.3951557532582775, + "heading": 0.25228608290098037, + "angularVelocity": 0.9388403989582608, + "velocityX": 2.879249244319485, + "velocityY": -1.8188868901826996, + "timestamp": 1.9812270587180605 + }, + { + "x": 4.164002224660317, + "y": 2.277675094812323, + "heading": 0.2575212486101159, + "angularVelocity": 0.08971471283939278, + "velocityX": 3.103049133394018, + "velocityY": -2.0132588197292973, + "timestamp": 2.039580538804516 + }, + { + "x": 4.349312428482631, + "y": 2.163516001344807, + "heading": 0.25752132885359325, + "angularVelocity": 0.0000013751275369508418, + "velocityX": 3.175649567905162, + "velocityY": -1.9563373649417417, + "timestamp": 2.0979340188909714 + }, + { + "x": 4.534622740292831, + "y": 2.0493570831701313, + "heading": 0.2575214090969788, + "angularVelocity": 0.0000013751259629129977, + "velocityX": 3.175651418486927, + "velocityY": -1.9563343609591082, + "timestamp": 2.1562874989774268 + }, + { + "x": 4.719933052117692, + "y": 1.9351981650192527, + "heading": 0.25752148934036456, + "angularVelocity": 0.0000013751259684025227, + "velocityX": 3.175651418738155, + "velocityY": -1.9563343605512986, + "timestamp": 2.214640979063882 + }, + { + "x": 4.9052436421415075, + "y": 1.8210396984605661, + "heading": 0.2575215695837395, + "angularVelocity": 0.0000013751257818103147, + "velocityX": 3.17565618621654, + "velocityY": -1.9563266216436723, + "timestamp": 2.2729944591503375 + }, + { + "x": 5.095847642006684, + "y": 1.7159574192298102, + "heading": 0.2575216501181558, + "angularVelocity": 0.0000013801133386874064, + "velocityX": 3.266369025168363, + "velocityY": -1.8007885575130747, + "timestamp": 2.331347939236793 + }, + { + "x": 5.295726350893371, + "y": 1.6298141007255773, + "heading": 0.25752173405231565, + "angularVelocity": 0.0000014383745358820107, + "velocityX": 3.4253091433544425, + "velocityY": -1.4762327521272869, + "timestamp": 2.389701419323248 }, { "x": 5.5030035972595215, "y": 1.563418984413147, - "heading": 0.2569447220794932, - "angularVelocity": 0.0000015913580676492416, - "velocityX": 3.5481637974123346, - "velocityY": -1.1340904342963076, - "timestamp": 2.4478166834517507 - }, - { - "x": 5.694707875531767, - "y": 1.52024537146014, - "heading": 0.25694480525283153, - "angularVelocity": 0.0000015766499748638697, - "velocityX": 3.6339835774202793, - "velocityY": -0.8184074026053739, - "timestamp": 2.500569887803966 - }, - { - "x": 5.889460824583239, - "y": 1.4940579532693374, - "heading": 0.25694488397529436, - "angularVelocity": 0.0000014922783143570964, - "velocityX": 3.6917747735506756, - "velocityY": -0.49641379158615856, - "timestamp": 2.553323092156181 - }, - { - "x": 6.0857603375931175, - "y": 1.485058307103912, - "heading": 0.2569449614499782, - "angularVelocity": 0.0000014686251720162004, - "velocityX": 3.7210917406884763, - "velocityY": -0.17059904276786395, - "timestamp": 2.6060762965083963 - }, - { - "x": 6.282233415675428, - "y": 1.4886391007138935, - "heading": 0.2569450396471809, - "angularVelocity": 0.0000014823213805790595, - "velocityX": 3.724381873952661, - "velocityY": 0.06787821998591004, - "timestamp": 2.6588295008606115 - }, - { - "x": 6.478706465106145, - "y": 1.492221466050733, - "heading": 0.25694511784454455, - "angularVelocity": 0.0000014823244316657533, - "velocityX": 3.724381330827469, - "velocityY": 0.06790801394586561, - "timestamp": 2.7115827052128267 - }, - { - "x": 6.675014894210808, - "y": 1.5010244818845797, - "heading": 0.2569451969790386, - "angularVelocity": 0.0000015000888575462877, - "velocityX": 3.7212607559150137, - "velocityY": 0.16687167996605787, - "timestamp": 2.764335909565042 - }, - { - "x": 6.869794082781554, - "y": 1.5270160132210213, - "heading": 0.25694528055287635, - "angularVelocity": 0.0000015842419202847933, - "velocityX": 3.6922721749805327, - "velocityY": 0.4927005222830628, - "timestamp": 2.817089113917257 - }, - { - "x": 7.061541469504376, - "y": 1.5699966670379106, - "heading": 0.256946313055063, - "angularVelocity": 0.000019572312228011845, - "velocityX": 3.6348007495922103, - "velocityY": 0.8147496316986196, - "timestamp": 2.8698423182694723 - }, - { - "x": 7.242025004547978, - "y": 1.6235443273244181, - "heading": 0.2892009263238711, - "angularVelocity": 0.6114247213013846, - "velocityX": 3.4212809868112584, - "velocityY": 1.0150598611790123, - "timestamp": 2.9225955226216875 - }, - { - "x": 7.410147525588679, - "y": 1.6859098332103064, - "heading": 0.355218131661103, - "angularVelocity": 1.2514349819673058, - "velocityX": 3.1869632016702596, - "velocityY": 1.1822126570643, - "timestamp": 2.9753487269739027 - }, - { - "x": 7.565027902002143, - "y": 1.7562974315327362, - "heading": 0.45280543116483274, - "angularVelocity": 1.8498838260548631, - "velocityX": 2.935942533071192, - "velocityY": 1.3342810012539876, - "timestamp": 3.028101931326118 - }, - { - "x": 7.703780357583263, - "y": 1.8320280138994296, - "heading": 0.5628377984662216, - "angularVelocity": 2.0857949512742477, - "velocityX": 2.630218529564891, - "velocityY": 1.4355636457847423, - "timestamp": 3.080855135678333 - }, - { - "x": 7.825662907370359, - "y": 1.9113228263820174, - "heading": 0.6698617562917842, - "angularVelocity": 2.028766956240237, - "velocityX": 2.3104293148398787, - "velocityY": 1.503127884955833, - "timestamp": 3.1336083400305483 - }, - { - "x": 7.930693514616472, - "y": 1.993224495332611, - "heading": 0.7659443680915856, - "angularVelocity": 1.821360673340157, - "velocityX": 1.990980615032594, - "velocityY": 1.552543963088276, - "timestamp": 3.1863615443827635 - }, - { - "x": 8.019011714129224, - "y": 2.0771819587983416, - "heading": 0.8464048765676822, - "angularVelocity": 1.525225044888065, - "velocityX": 1.6741769641722983, - "velocityY": 1.5915140036835516, - "timestamp": 3.2391147487349787 - }, - { - "x": 8.090768492192119, - "y": 2.1628615391485617, - "heading": 0.9080474489079001, - "angularVelocity": 1.1685085881921318, - "velocityX": 1.3602354386626168, - "velocityY": 1.6241587862258966, - "timestamp": 3.291867953087194 - }, - { - "x": 8.146101418716508, - "y": 2.2500495157540152, - "heading": 0.9485346126335614, - "angularVelocity": 0.7674825486494136, - "velocityX": 1.048901715144179, - "velocityY": 1.6527522389602969, - "timestamp": 3.344621157439409 - }, - { - "x": 8.185122291410464, - "y": 2.3385982915715853, - "heading": 0.9662103754331748, - "angularVelocity": 0.33506519682858266, - "velocityX": 0.7396872507199079, - "velocityY": 1.6785478134439036, - "timestamp": 3.3973743617916243 + "heading": 0.25752182612435603, + "angularVelocity": 0.000001577832894961981, + "velocityX": 3.55209742519301, + "velocityY": -1.137809025512455, + "timestamp": 2.4480548994097036 + }, + { + "x": 5.6948016559329036, + "y": 1.5200403690670627, + "heading": 0.2575219089946678, + "angularVelocity": 0.0000015718708675388777, + "velocityX": 3.637995011021535, + "velocityY": -0.8227986628520415, + "timestamp": 2.5007757135098654 + }, + { + "x": 5.889658517300143, + "y": 1.4936016847783737, + "heading": 0.25752198743316407, + "angularVelocity": 0.0000014878088960886408, + "velocityX": 3.6960138930525672, + "velocityY": -0.5014847501872678, + "timestamp": 2.553496527610027 + }, + { + "x": 6.0860809574321495, + "y": 1.4843051335513961, + "heading": 0.2575220646083203, + "angularVelocity": 0.000001463846063481971, + "velocityX": 3.725709541564224, + "velocityY": -0.17633550212854351, + "timestamp": 2.606217341710189 + }, + { + "x": 6.282694965484122, + "y": 1.4876417105496467, + "heading": 0.25752214249001004, + "angularVelocity": 0.0000014772474802220589, + "velocityX": 3.7293431713409246, + "velocityY": 0.06328766076926912, + "timestamp": 2.658938155810351 + }, + { + "x": 6.479308945028881, + "y": 1.4909799669650579, + "heading": 0.2575222203718718, + "angularVelocity": 0.0000014772507428858513, + "velocityX": 3.729342630620652, + "velocityY": 0.06331951568632599, + "timestamp": 2.7116589699105127 + }, + { + "x": 6.675718854872107, + "y": 1.5005375890883978, + "heading": 0.2575222993699314, + "angularVelocity": 0.0000014984226051915619, + "velocityX": 3.7254718690435245, + "velocityY": 0.18128745328518545, + "timestamp": 2.7643797840106745 + }, + { + "x": 6.870540482169492, + "y": 1.5272346660517908, + "heading": 0.25752238291998725, + "angularVelocity": 0.000001584764144444983, + "velocityX": 3.6953455788306457, + "velocityY": 0.5063859012623091, + "timestamp": 2.8171005981108364 + }, + { + "x": 7.062280223610006, + "y": 1.5708674152899489, + "heading": 0.2575250023601284, + "angularVelocity": 0.00004968512314911554, + "velocityX": 3.6368888590422164, + "velocityY": 0.8276190340168537, + "timestamp": 2.869821412210998 + }, + { + "x": 7.242655823175531, + "y": 1.624760581479399, + "heading": 0.29034566188783406, + "angularVelocity": 0.6225370394575329, + "velocityX": 3.4213356270037516, + "velocityY": 1.0222369875977493, + "timestamp": 2.92254222631116 + }, + { + "x": 7.410675905679771, + "y": 1.6874034797847524, + "heading": 0.35686793752538104, + "angularVelocity": 1.261783922971381, + "velocityX": 3.1869781484221007, + "velocityY": 1.188200511971245, + "timestamp": 2.975263040411322 + }, + { + "x": 7.565444154381843, + "y": 1.757937293066464, + "heading": 0.45500614255643107, + "angularVelocity": 1.8614698332351587, + "velocityX": 2.9356194767409227, + "velocityY": 1.337874129707252, + "timestamp": 3.0279838545114837 + }, + { + "x": 7.704030497624387, + "y": 1.8336235028030063, + "heading": 0.5647068511850917, + "angularVelocity": 2.0807855588163995, + "velocityX": 2.628683672813762, + "velocityY": 1.4356039645508802, + "timestamp": 3.0807046686116455 + }, + { + "x": 7.825774409585393, + "y": 1.9127779886685334, + "heading": 0.6711471283329911, + "angularVelocity": 2.018942214087941, + "velocityX": 2.309219120359376, + "velocityY": 1.5013896734436838, + "timestamp": 3.1334254827118073 + }, + { + "x": 7.930703401936529, + "y": 1.9944869953972368, + "heading": 0.7666407114919108, + "angularVelocity": 1.8113070670247255, + "velocityX": 1.990276404908808, + "velocityY": 1.5498434180752323, + "timestamp": 3.186146296811969 + }, + { + "x": 8.018956078741207, + "y": 2.0782208825678086, + "heading": 0.8466105266240161, + "angularVelocity": 1.516854709037202, + "velocityX": 1.6739627092444165, + "velocityY": 1.5882510276015418, + "timestamp": 3.238867110912131 + }, + { + "x": 8.090681009003735, + "y": 2.1636571823312836, + "heading": 0.9079143826816493, + "angularVelocity": 1.1628017720129442, + "velocityX": 1.3604670467770388, + "velocityY": 1.620542118358762, + "timestamp": 3.291587925012293 + }, + { + "x": 8.146013476632273, + "y": 2.250588470468436, + "heading": 0.9482462307641644, + "angularVelocity": 0.7650080669446885, + "velocityX": 1.049537427920118, + "velocityY": 1.6488988195818726, + "timestamp": 3.3443087391124546 + }, + { + "x": 8.185063693678202, + "y": 2.3388710656404412, + "heading": 0.9659659222521745, + "angularVelocity": 0.33610428424616584, + "velocityX": 0.7406982936898286, + "velocityY": 1.6745301960679422, + "timestamp": 3.3970295532126165 }, { "x": 8.207907676696777, "y": 2.428393840789795, "heading": 0.9600708878718816, - "angularVelocity": -0.11638132008630137, - "velocityX": 0.43192419429508555, - "velocityY": 1.702181892471887, - "timestamp": 3.4501275661438395 - }, - { - "x": 8.211423054828616, - "y": 2.5404723922535313, - "heading": 0.9162466453350178, - "angularVelocity": -0.6758725051109615, - "velocityX": 0.054215367724374734, - "velocityY": 1.7285138763843786, - "timestamp": 3.5149685564274566 - }, - { - "x": 8.190202888383553, - "y": 2.653847423099536, - "heading": 0.8391734249663445, - "angularVelocity": -1.188649649420099, - "velocityX": -0.32726468785015433, - "velocityY": 1.748508626257838, - "timestamp": 3.5798095467110738 - }, - { - "x": 8.144008306424402, - "y": 2.7680190139904237, - "heading": 0.7319233890251923, - "angularVelocity": -1.654046853264217, - "velocityX": -0.7124286929779271, - "velocityY": 1.760793448580852, - "timestamp": 3.644650536994691 - }, - { - "x": 8.072604152106313, - "y": 2.8824539716658655, - "heading": 0.5977397407747723, - "angularVelocity": -2.06942626359492, - "velocityX": -1.101219367652527, - "velocityY": 1.764855181497056, - "timestamp": 3.709491527278308 - }, - { - "x": 7.975691382967057, - "y": 2.9965380091624785, - "heading": 0.44171220804306943, - "angularVelocity": -2.4063101450059983, - "velocityX": -1.4946219777853038, - "velocityY": 1.759443170093563, - "timestamp": 3.774332517561925 - }, - { - "x": 7.852920805151411, - "y": 3.109310419044201, - "heading": 0.2733111632769955, - "angularVelocity": -2.5971386931242324, - "velocityX": -1.893409975366543, - "velocityY": 1.7392147989790332, - "timestamp": 3.8391735078455422 - }, - { - "x": 7.704278478364198, - "y": 3.2190283422070105, - "heading": 0.10912005253045581, - "angularVelocity": -2.53221164618803, - "velocityX": -2.292412964963152, - "velocityY": 1.6921074567630605, - "timestamp": 3.9040144981291593 - }, - { - "x": 7.532421373794031, - "y": 3.3230579148672983, - "heading": -0.010926874392962598, - "angularVelocity": -1.8514048967840961, - "velocityX": -2.650439233245175, - "velocityY": 1.6043797635609574, - "timestamp": 3.9688554884127765 - }, - { - "x": 7.337707687800783, - "y": 3.422034456345691, - "heading": -0.0821100823564174, - "angularVelocity": -1.0978118571616002, - "velocityX": -3.002941274363059, - "velocityY": 1.5264501829084578, - "timestamp": 4.033696478696394 + "angularVelocity": -0.11181607266331907, + "velocityX": 0.43330102936525317, + "velocityY": 1.698053732237021, + "timestamp": 3.4497503673127783 + }, + { + "x": 8.211510403454756, + "y": 2.5402965981935948, + "heading": 0.9167116042815631, + "angularVelocity": -0.6681435764586997, + "velocityX": 0.05551610962541542, + "velocityY": 1.7243621747491076, + "timestamp": 3.5146455262547107 + }, + { + "x": 8.19035769452273, + "y": 2.653506399419357, + "heading": 0.8402263386800068, + "angularVelocity": -1.1785974000001183, + "velocityX": -0.3259520321223525, + "velocityY": 1.7445030272144093, + "timestamp": 3.579540685196643 + }, + { + "x": 8.144214527086703, + "y": 2.7675347256027623, + "heading": 0.7336527994831279, + "angularVelocity": -1.6422417470652866, + "velocityX": -0.7110417508540927, + "velocityY": 1.757116062932159, + "timestamp": 3.6444358441385756 + }, + { + "x": 8.072849371790616, + "y": 2.8818580666466, + "heading": 0.6001927782610759, + "angularVelocity": -2.0565481832238106, + "velocityX": -1.0996992142348316, + "velocityY": 1.7616620855514487, + "timestamp": 3.709331003080508 + }, + { + "x": 7.975969161360382, + "y": 2.9958746653060953, + "heading": 0.4448175731480801, + "angularVelocity": -2.3942495502942416, + "velocityX": -1.4928726889616013, + "velocityY": 1.756935348005165, + "timestamp": 3.7742261620224404 + }, + { + "x": 7.853227191663504, + "y": 3.108650032284083, + "heading": 0.2767627605160218, + "angularVelocity": -2.5896355810212537, + "velocityX": -1.8913886905910138, + "velocityY": 1.7378086257389034, + "timestamp": 3.839121320964373 + }, + { + "x": 7.704583468021456, + "y": 3.218490505664157, + "heading": 0.11219872525944127, + "angularVelocity": -2.5358445520386232, + "velocityX": -2.2905209890163487, + "velocityY": 1.692583471108498, + "timestamp": 3.9040164799063053 + }, + { + "x": 7.532642084855597, + "y": 3.3226846755511135, + "heading": -0.008852877537121141, + "angularVelocity": -1.8653410326782291, + "velocityX": -2.6495255727736016, + "velocityY": 1.6055769272433476, + "timestamp": 3.9689116388482377 + }, + { + "x": 7.337826737893983, + "y": 3.4218449378655498, + "heading": -0.08105102953452195, + "angularVelocity": -1.1125352518514178, + "velocityX": -3.002001229952035, + "velocityY": 1.5280070798988774, + "timestamp": 4.03380679779017 }, { "x": 7.120736598968506, "y": 3.5168776512145996, - "heading": -0.09725138680843894, - "angularVelocity": -0.23351439245133218, - "velocityX": -3.3462025777712125, - "velocityY": 1.46270429328826, - "timestamp": 4.098537468980011 - }, - { - "x": 7.010606209659238, - "y": 3.566708155417018, - "heading": -0.09725146950362612, - "angularVelocity": -0.0000025483254840822735, - "velocityX": -3.3937655527961965, - "velocityY": 1.535571150717811, - "timestamp": 4.130988263708164 - }, - { - "x": 6.900475999603528, - "y": 3.6165390557870793, - "heading": -0.09725155219704651, - "angularVelocity": -0.000002548271039111256, - "velocityX": -3.3937600289389183, - "velocityY": 1.5355833589748464, - "timestamp": 4.1634390584363175 - }, - { - "x": 6.7903457895857455, - "y": 3.666369956240961, - "heading": -0.0972516348904687, - "angularVelocity": -0.000002548271094167607, - "velocityX": -3.3937600277701754, - "velocityY": 1.5355833615578496, - "timestamp": 4.195889853164471 - }, - { - "x": 6.680215579567971, - "y": 3.7162008566948606, - "heading": -0.09725171758389267, - "angularVelocity": -0.0000025482711490074116, - "velocityX": -3.393760027769923, - "velocityY": 1.535583361558393, - "timestamp": 4.228340647892624 - }, - { - "x": 6.570085369550196, - "y": 3.7660317571487596, - "heading": -0.09725180027731847, - "angularVelocity": -0.0000025482712052155066, - "velocityX": -3.39376002776992, - "velocityY": 1.53558336155839, - "timestamp": 4.260791442620778 - }, - { - "x": 6.459955159532422, - "y": 3.815862657602659, - "heading": -0.09725188297074605, - "angularVelocity": -0.000002548271260506862, - "velocityX": -3.3937600277699165, - "velocityY": 1.5355833615583867, - "timestamp": 4.293242237348931 - }, - { - "x": 6.349824949514647, - "y": 3.865693558056558, - "heading": -0.09725196566417542, - "angularVelocity": -0.0000025482713153661305, - "velocityX": -3.3937600277699125, - "velocityY": 1.5355833615583832, - "timestamp": 4.325693032077084 - }, - { - "x": 6.239694739496873, - "y": 3.915524458510457, - "heading": -0.09725204835760656, - "angularVelocity": -0.000002548271370425602, - "velocityX": -3.3937600277699085, - "velocityY": 1.5355833615583803, - "timestamp": 4.358143826805238 - }, - { - "x": 6.129564529479099, - "y": 3.965355358964356, - "heading": -0.09725213105103951, - "angularVelocity": -0.0000025482714255163693, - "velocityX": -3.3937600277699054, - "velocityY": 1.5355833615583767, - "timestamp": 4.390594621533391 - }, - { - "x": 6.019434319461326, - "y": 4.015186259418255, - "heading": -0.09725221374447428, - "angularVelocity": -0.000002548271481519915, - "velocityX": -3.3937600277699014, - "velocityY": 1.5355833615583734, - "timestamp": 4.4230454162615445 - }, - { - "x": 5.9093041094435526, - "y": 4.065017159872154, - "heading": -0.09725229643791083, - "angularVelocity": -0.000002548271536879473, - "velocityX": -3.3937600277698974, - "velocityY": 1.5355833615583703, - "timestamp": 4.455496210989698 - }, - { - "x": 5.799173899425779, - "y": 4.114848060326053, - "heading": -0.09725237913134915, - "angularVelocity": -0.000002548271591483022, - "velocityX": -3.3937600277698934, - "velocityY": 1.535583361558367, - "timestamp": 4.487947005717851 - }, - { - "x": 5.689043689408005, - "y": 4.164678960779951, - "heading": -0.09725246182478926, - "angularVelocity": -0.0000025482716464166734, - "velocityX": -3.39376002776989, - "velocityY": 1.5355833615583636, - "timestamp": 4.520397800446005 - }, - { - "x": 5.5789134793902315, - "y": 4.214509861233849, - "heading": -0.09725254451823119, - "angularVelocity": -0.000002548271702485751, - "velocityX": -3.3937600277698863, - "velocityY": 1.5355833615583605, - "timestamp": 4.552848595174158 - }, - { - "x": 5.468783269372458, - "y": 4.264340761687747, - "heading": -0.09725262721167492, - "angularVelocity": -0.0000025482717576806813, - "velocityX": -3.3937600277698823, - "velocityY": 1.5355833615583572, - "timestamp": 4.585299389902311 - }, - { - "x": 5.3586530593546975, - "y": 4.314171662141674, - "heading": -0.09725270990512043, - "angularVelocity": -0.0000025482718127641656, - "velocityX": -3.3937600277694844, - "velocityY": 1.5355833615592254, - "timestamp": 4.617750184630465 - }, - { - "x": 5.2485228493974105, - "y": 4.364002562729252, - "heading": -0.09725279259856771, - "angularVelocity": -0.0000025482718674912185, - "velocityX": -3.393760025905934, - "velocityY": 1.5355833656778057, - "timestamp": 4.650200979358618 + "heading": -0.09725140170443469, + "angularVelocity": -0.249639147727624, + "velocityX": -3.3452439668069256, + "velocityY": 1.4644037382524164, + "timestamp": 4.098701956732103 + }, + { + "x": 7.010606208807297, + "y": 3.5667081535340293, + "heading": -0.09725148357223665, + "angularVelocity": -0.000002526133789142618, + "velocityX": -3.3982114234212517, + "velocityY": 1.5375826960099699, + "timestamp": 4.131110296425792 + }, + { + "x": 6.900475998308352, + "y": 3.616539052924532, + "heading": -0.09725156543808965, + "angularVelocity": -0.000002526073652213915, + "velocityX": -3.3982058797165378, + "velocityY": 1.5375949481363274, + "timestamp": 4.163518636119481 + }, + { + "x": 6.7903457878477695, + "y": 3.666369952399819, + "heading": -0.09725164730394434, + "angularVelocity": -0.0000025260737041987077, + "velocityX": -3.3982058785328007, + "velocityY": 1.537594950752468, + "timestamp": 4.195926975813171 + }, + { + "x": 6.680215577387196, + "y": 3.7162008518751244, + "heading": -0.0972517291698008, + "angularVelocity": -0.0000025260737583939243, + "velocityX": -3.3982058785325444, + "velocityY": 1.5375949507530235, + "timestamp": 4.22833531550686 + }, + { + "x": 6.570085366926623, + "y": 3.766031751350429, + "heading": -0.09725181103565897, + "angularVelocity": -0.0000025260738115534393, + "velocityX": -3.3982058785325413, + "velocityY": 1.5375949507530207, + "timestamp": 4.2607436552005495 + }, + { + "x": 6.45995515646605, + "y": 3.8158626508257334, + "heading": -0.09725189290151888, + "angularVelocity": -0.000002526073864680103, + "velocityX": -3.3982058785325373, + "velocityY": 1.5375949507530176, + "timestamp": 4.293151994894239 + }, + { + "x": 6.3498249460054765, + "y": 3.8656935503010383, + "heading": -0.0972519747673805, + "angularVelocity": -0.000002526073917550957, + "velocityX": -3.3982058785325338, + "velocityY": 1.5375949507530144, + "timestamp": 4.325560334587928 + }, + { + "x": 6.239694735544903, + "y": 3.915524449776343, + "heading": -0.09725205663324382, + "angularVelocity": -0.0000025260739709897513, + "velocityX": -3.39820587853253, + "velocityY": 1.5375949507530113, + "timestamp": 4.357968674281618 + }, + { + "x": 6.12956452508433, + "y": 3.965355349251648, + "heading": -0.0972521384991089, + "angularVelocity": -0.0000025260740241290968, + "velocityX": -3.398205878532526, + "velocityY": 1.5375949507530078, + "timestamp": 4.390377013975307 + }, + { + "x": 6.019434314623757, + "y": 4.015186248726953, + "heading": -0.09725222036497569, + "angularVelocity": -0.0000025260740776848576, + "velocityX": -3.3982058785325227, + "velocityY": 1.537594950753005, + "timestamp": 4.4227853536689965 + }, + { + "x": 5.909304104163184, + "y": 4.065017148202257, + "heading": -0.09725230223084419, + "angularVelocity": -0.000002526074130335594, + "velocityX": -3.398205878532519, + "velocityY": 1.5375949507530018, + "timestamp": 4.455193693362686 + }, + { + "x": 5.799173893702611, + "y": 4.114848047677562, + "heading": -0.09725238409671443, + "angularVelocity": -0.0000025260741837826664, + "velocityX": -3.3982058785325155, + "velocityY": 1.5375949507529985, + "timestamp": 4.487602033056375 + }, + { + "x": 5.689043683242039, + "y": 4.164678947152866, + "heading": -0.09725246596258641, + "angularVelocity": -0.0000025260742375974643, + "velocityX": -3.3982058785325115, + "velocityY": 1.5375949507529956, + "timestamp": 4.520010372750065 + }, + { + "x": 5.578913472781466, + "y": 4.214509846628169, + "heading": -0.09725254782846013, + "angularVelocity": -0.000002526074291245016, + "velocityX": -3.398205878532508, + "velocityY": 1.5375949507529922, + "timestamp": 4.552418712443754 + }, + { + "x": 5.468783262320894, + "y": 4.264340746103473, + "heading": -0.09725262969433558, + "angularVelocity": -0.0000025260743444666228, + "velocityX": -3.398205878532505, + "velocityY": 1.5375949507529894, + "timestamp": 4.5848270521374435 + }, + { + "x": 5.358653051860335, + "y": 4.314171645578806, + "heading": -0.09725271156021274, + "angularVelocity": -0.0000025260743976709886, + "velocityX": -3.398205878532087, + "velocityY": 1.537594950753901, + "timestamp": 4.617235391831133 + }, + { + "x": 5.248522841462595, + "y": 4.364002545192973, + "heading": -0.0972527934260916, + "angularVelocity": -0.0000025260744491958753, + "velocityX": -3.398205876593744, + "velocityY": 1.53759495503778, + "timestamp": 4.649643731524822 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": -0.0000025482898100852076, - "velocityX": -3.393751218037592, - "velocityY": 1.5356028315921753, - "timestamp": 4.6826517740867715 - }, - { - "x": 4.9137282295128415, - "y": 4.535640155229253, - "heading": -0.09725287529313088, - "angularVelocity": -7.435036019118785e-12, - "velocityX": -3.1302347498710454, - "velocityY": 1.6971138308958593, - "timestamp": 4.754424249221062 - }, - { - "x": 4.717146626097375, - "y": 4.642220517268037, - "heading": -0.097252875293223, - "angularVelocity": -1.283652180157178e-12, - "velocityX": -2.7389553313809003, - "velocityY": 1.4849754288028512, - "timestamp": 4.826196724355352 - }, - { - "x": 4.548648110952986, - "y": 4.733575136181824, - "heading": -0.09725287529315746, - "angularVelocity": 9.133130247777463e-13, - "velocityX": -2.347675969501115, - "velocityY": 1.2728364006237478, - "timestamp": 4.897969199489642 - }, - { - "x": 4.408232682725308, - "y": 4.809703996992033, - "heading": -0.09725287529302883, - "angularVelocity": 1.7921853517363252e-12, - "velocityX": -1.9563966264916142, - "velocityY": 1.0606971637493035, - "timestamp": 4.969741674623933 - }, - { - "x": 4.295900340737155, - "y": 4.870607092209374, - "heading": -0.09725287529288444, - "angularVelocity": 2.0118085530494987e-12, - "velocityX": -1.5651172929172832, - "velocityY": 0.8485578225272118, - "timestamp": 5.041514149758223 - }, - { - "x": 4.211651084582216, - "y": 4.916284417340276, - "heading": -0.09725287529275266, - "angularVelocity": 1.8359969573857423e-12, - "velocityX": -1.1738379650040625, - "velocityY": 0.636418418696543, - "timestamp": 5.113286624892513 - }, - { - "x": 4.155484913989614, - "y": 4.946735969389024, - "heading": -0.09725287529265242, - "angularVelocity": 1.3966146295291797e-12, - "velocityX": -0.7825586408649183, - "velocityY": 0.42427897312682855, - "timestamp": 5.185059100026804 + "angularVelocity": -0.000002526093790111687, + "velocityX": -3.398196798757616, + "velocityY": 1.5376150175922905, + "timestamp": 4.682052071218512 + }, + { + "x": 4.913728228480229, + "y": 4.53564015337073, + "heading": -0.09725287529314537, + "angularVelocity": -7.632140537797702e-12, + "velocityX": -3.128186469396145, + "velocityY": 1.6960032846252107, + "timestamp": 4.753871542014187 + }, + { + "x": 4.717146624886484, + "y": 4.642220515088643, + "heading": -0.09725287529324, + "angularVelocity": -1.3176073581660395e-12, + "velocityX": -2.737163075915943, + "velocityY": 1.4840037184502801, + "timestamp": 4.825691012809862 + }, + { + "x": 4.548648109868937, + "y": 4.7335751342307235, + "heading": -0.09725287529317267, + "angularVelocity": 9.375034048852172e-13, + "velocityX": -2.346139746656194, + "velocityY": 1.2720035128354326, + "timestamp": 4.8975104836055365 + }, + { + "x": 4.40823268189015, + "y": 4.809703995488892, + "heading": -0.09725287529304055, + "angularVelocity": 1.8396343305490373e-12, + "velocityX": -1.9551164388034394, + "velocityY": 1.0600030940739407, + "timestamp": 4.9693299544012115 + }, + { + "x": 4.295900340181399, + "y": 4.87060709120911, + "heading": -0.09725287529289224, + "angularVelocity": 2.065048279063882e-12, + "velocityX": -1.5640931416542116, + "velocityY": 0.8480025687391443, + "timestamp": 5.0411494251968865 + }, + { + "x": 4.211651084281453, + "y": 4.916284416798956, + "heading": -0.09725287529275689, + "angularVelocity": 1.8846216293321442e-12, + "velocityX": -1.1730698509271091, + "velocityY": 0.6360019794603747, + "timestamp": 5.112968895992561 + }, + { + "x": 4.155484913882821, + "y": 4.946735969196816, + "heading": -0.09725287529265393, + "angularVelocity": 1.4335935528367604e-12, + "velocityX": -0.7820465644814268, + "velocityY": 0.42400134755229474, + "timestamp": 5.184788366788236 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 7.688238334265391e-13, - "velocityX": -0.3912793194215448, - "velocityY": 0.21213949774351348, - "timestamp": 5.256831575161094 + "angularVelocity": 7.893009586439382e-13, + "velocityX": -0.3910232810939034, + "velocityY": 0.21200068519471058, + "timestamp": 5.256607837583911 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 3.652934582681632e-29, - "velocityX": -2.1119125295164495e-27, - "velocityY": 7.525449232438864e-28, - "timestamp": 5.328604050295384 - }, - { - "x": 4.149345287192321, - "y": 4.948730090302555, - "heading": -0.09725287542061173, - "angularVelocity": -1.9914932727158263e-9, - "velocityX": 0.34136955009475556, - "velocityY": -0.2058419570123514, - "timestamp": 5.392884705732758 - }, - { - "x": 4.193331867161711, - "y": 4.922433486987429, - "heading": -0.09725287565526207, - "angularVelocity": -3.650403639422262e-9, - "velocityX": 0.6842895373437039, - "velocityY": -0.4090904664272752, - "timestamp": 5.457165361170132 - }, - { - "x": 4.259487364434434, - "y": 4.883286547964955, - "heading": -0.09725287596919821, - "angularVelocity": -4.883835485414618e-9, - "velocityX": 1.0291665015328901, - "velocityY": -0.6090003089749593, - "timestamp": 5.521446016607506 - }, - { - "x": 4.347975400700828, - "y": 4.831575784808056, - "heading": -0.09725287632620158, - "angularVelocity": -5.553822878939374e-9, - "velocityX": 1.3765888923239986, - "velocityY": -0.8044529540816499, - "timestamp": 5.5857266720448795 - }, - { - "x": 4.45901715765165, - "y": 4.76770271955414, - "heading": -0.09725287667606394, - "angularVelocity": -5.442731629415375e-9, - "velocityX": 1.7274521579669766, - "velocityY": -0.9936592092802463, - "timestamp": 5.650007327482253 - }, - { - "x": 4.592927185358519, - "y": 4.69226963680943, - "heading": -0.09725287694462321, - "angularVelocity": -4.1779173473054744e-9, - "velocityX": 2.083208809800217, - "velocityY": -1.1734958555019974, - "timestamp": 5.714287982919627 - }, - { - "x": 4.750184246245984, - "y": 4.606276833638444, - "heading": -0.09725287701156488, - "angularVelocity": -1.0413967826345418e-9, - "velocityX": 2.446413463233511, - "velocityY": -1.337771100588195, - "timestamp": 5.778568638357001 - }, - { - "x": 4.931581310093376, - "y": 4.511690334370361, - "heading": -0.0972528766497097, - "angularVelocity": 5.6293012279094655e-9, - "velocityX": 2.821954172886779, - "velocityY": -1.4714613381662665, - "timestamp": 5.842849293794375 + "angularVelocity": -2.6496856571629414e-29, + "velocityX": 3.729224891945568e-30, + "velocityY": 1.9440418039137549e-28, + "timestamp": 5.328427308379586 + }, + { + "x": 4.149339896779893, + "y": 4.948719586004848, + "heading": -0.09725287542661647, + "angularVelocity": -2.0835104193721486e-9, + "velocityX": 0.3410569741164579, + "velocityY": -0.2058673119908494, + "timestamp": 5.3927510715011495 + }, + { + "x": 4.193317137132987, + "y": 4.9224042284052185, + "heading": -0.09725287567219071, + "angularVelocity": -3.8177839227409805e-9, + "velocityX": 0.6836857518734218, + "velocityY": -0.4091078681123934, + "timestamp": 5.457074834622713 + }, + { + "x": 4.259461092392441, + "y": 4.883233120266127, + "heading": -0.09725287600059242, + "angularVelocity": -5.105449510153574e-9, + "velocityX": 1.0282973515472125, + "velocityY": -0.6089679185134605, + "timestamp": 5.521398597744276 + }, + { + "x": 4.3479375321320015, + "y": 4.831496437672959, + "heading": -0.0972528763737988, + "angularVelocity": -5.801998468175167e-9, + "velocityX": 1.3754860637172643, + "velocityY": -0.8043167887331668, + "timestamp": 5.585722360865839 + }, + { + "x": 4.4589703097902795, + "y": 4.767600598916816, + "heading": -0.09725287673913857, + "angularVelocity": -5.679701582955187e-9, + "velocityX": 1.7261548807155662, + "velocityY": -0.993347336277392, + "timestamp": 5.650046123987402 + }, + { + "x": 4.592877291375008, + "y": 4.69215468843508, + "heading": -0.09725287701889467, + "angularVelocity": -4.34918718104189e-9, + "velocityX": 2.081765355233693, + "velocityY": -1.1729088414674105, + "timestamp": 5.714369887108965 + }, + { + "x": 4.750141105667559, + "y": 4.606168779323296, + "heading": -0.0972528770872025, + "angularVelocity": -1.061937612005659e-9, + "velocityX": 2.4448789476968895, + "velocityY": -1.3367673926241201, + "timestamp": 5.778693650230529 + }, + { + "x": 4.931557529116976, + "y": 4.511622240510161, + "heading": -0.09725287670716622, + "angularVelocity": 5.908178394469377e-9, + "velocityX": 2.8203639626395, + "velocityY": -1.4698539734756233, + "timestamp": 5.843017413352092 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": 2.111229970452055e-8, - "velocityX": 3.2173227507078654, - "velocityY": -1.5223279648179473, - "timestamp": 5.907129949231749 - }, - { - "x": 5.3988689559213965, - "y": 4.32613897098341, - "heading": -0.0972528699779729, - "angularVelocity": 7.203036405891024e-8, - "velocityX": 3.5302934161705415, - "velocityY": -1.1885528129676428, - "timestamp": 5.980913060138573 - }, - { - "x": 5.668131488513907, - "y": 4.271040046607528, - "heading": -0.09725286670603908, - "angularVelocity": 4.4345294956463885e-8, - "velocityX": 3.6493789606207083, - "velocityY": -0.7467687889368456, - "timestamp": 6.054696171045398 - }, - { - "x": 5.942117714678702, - "y": 4.249366302589977, - "heading": -0.09725286355772216, - "angularVelocity": 4.266988599202702e-8, - "velocityX": 3.713400299843557, - "velocityY": -0.2937493926614278, - "timestamp": 6.128479281952223 - }, - { - "x": 6.216382330790908, - "y": 4.231558335942029, - "heading": -0.09725286041527645, - "angularVelocity": 4.2590311894180227e-8, - "velocityX": 3.7171733848218165, - "velocityY": -0.2413555951908442, - "timestamp": 6.202262392859048 - }, - { - "x": 6.490646951405068, - "y": 4.21375043862986, - "heading": -0.09725285727283077, - "angularVelocity": 4.259031148289501e-8, - "velocityX": 3.717173445837876, - "velocityY": -0.2413546554665762, - "timestamp": 6.2760455037658724 - }, - { - "x": 6.764911572019294, - "y": 4.195942541318692, - "heading": -0.09725285413038512, - "angularVelocity": 4.2590311325076504e-8, - "velocityX": 3.717173445838756, - "velocityY": -0.241354655453012, - "timestamp": 6.349828614672697 - }, - { - "x": 7.039176191804646, - "y": 4.178134631242246, - "heading": -0.09725285098790415, - "angularVelocity": 4.259078987217418e-8, - "velocityX": 3.7171734346048453, - "velocityY": -0.24135482846385062, - "timestamp": 6.423611725579522 - }, - { - "x": 7.303151625715879, - "y": 4.160355738175748, - "heading": -0.07493794888540205, - "angularVelocity": 0.302439160239284, - "velocityX": 3.5777216583424787, - "velocityY": -0.24096155404655972, - "timestamp": 6.497394836486347 - }, - { - "x": 7.533445930864737, - "y": 4.1447637615713475, - "heading": -0.055524777247044715, - "angularVelocity": 0.2631113191048932, - "velocityX": 3.1212333326481616, - "velocityY": -0.21132175660214245, - "timestamp": 6.5711779473931715 - }, - { - "x": 7.730059092320319, - "y": 4.131358730965977, - "heading": -0.0390137097405274, - "angularVelocity": 0.22377841356361194, - "velocityX": 2.664744804591801, - "velocityY": -0.18168155883665832, - "timestamp": 6.644961058299996 - }, - { - "x": 7.892991106532595, - "y": 4.120140669647355, - "heading": -0.025405081189306844, - "angularVelocity": 0.1844409700806171, - "velocityX": 2.20825622842103, - "velocityY": -0.15204104544723207, - "timestamp": 6.718744169206821 - }, - { - "x": 8.02224197249394, - "y": 4.111109595697515, - "heading": -0.014699203415963246, - "angularVelocity": 0.1450993003922434, - "velocityX": 1.751767638593706, - "velocityY": -0.12240028698767993, - "timestamp": 6.792527280113646 - }, - { - "x": 8.117811689845258, - "y": 4.104265522227424, - "heading": -0.006896323202665822, - "angularVelocity": 0.10575428600660502, - "velocityX": 1.2952790438994193, - "velocityY": -0.09275935083211777, - "timestamp": 6.866310391020471 - }, - { - "x": 8.179700258175622, - "y": 4.099608457473234, - "heading": -0.0019965838975628653, - "angularVelocity": 0.06640732878951748, - "velocityX": 0.8387904436357877, - "velocityY": -0.06311830305000854, - "timestamp": 6.940093501927295 + "angularVelocity": 2.199139032346849e-8, + "velocityX": 3.215536313610633, + "velocityY": -1.5202491390955153, + "timestamp": 5.907341176473655 + }, + { + "x": 5.399102164920658, + "y": 4.3258479650681645, + "heading": -0.09725286980895109, + "angularVelocity": 7.433360780041783e-8, + "velocityX": 3.5340461244770793, + "velocityY": -1.1926966681553401, + "timestamp": 5.981111928318832 + }, + { + "x": 5.668612858103736, + "y": 4.270396554343484, + "heading": -0.0972528665281375, + "angularVelocity": 4.447309416058914e-8, + "velocityX": 3.653354296140342, + "velocityY": -0.7516720290591195, + "timestamp": 6.054882680164009 + }, + { + "x": 5.942881064370487, + "y": 4.248309813681979, + "heading": -0.09725286337373323, + "angularVelocity": 4.275955167599945e-8, + "velocityX": 3.7178448017225065, + "velocityY": -0.29939698470009923, + "timestamp": 6.128653432009186 + }, + { + "x": 6.2174671961178944, + "y": 4.2306087019584515, + "heading": -0.09725286022607692, + "angularVelocity": 4.266807980333545e-8, + "velocityX": 3.7221544430465427, + "velocityY": -0.23994755754525748, + "timestamp": 6.202424183854363 + }, + { + "x": 6.492053332978151, + "y": 4.212907669547494, + "heading": -0.09725285707842057, + "angularVelocity": 4.2668079985198815e-8, + "velocityX": 3.722154512353812, + "velocityY": -0.23994648242309302, + "timestamp": 6.2761949356995395 + }, + { + "x": 6.766639469838484, + "y": 4.195206637137713, + "heading": -0.09725285393076424, + "angularVelocity": 4.2668079920669555e-8, + "velocityX": 3.7221545123548396, + "velocityY": -0.2399464824071602, + "timestamp": 6.349965687544716 + }, + { + "x": 7.041225605850418, + "y": 4.177505591567667, + "heading": -0.09725285078307094, + "angularVelocity": 4.266858084816784e-8, + "velocityX": 3.722154500854352, + "velocityY": -0.23994666080122462, + "timestamp": 6.423736439389893 + }, + { + "x": 7.304754629426189, + "y": 4.1598724351575065, + "heading": -0.0749365249784548, + "angularVelocity": 0.3025091278919278, + "velocityX": 3.5722697272876305, + "velocityY": -0.23902638876674762, + "timestamp": 6.49750719123507 + }, + { + "x": 7.534656876904849, + "y": 4.144407021780378, + "heading": -0.055522378055544455, + "angularVelocity": 0.26316861950458387, + "velocityX": 3.116441702548429, + "velocityY": -0.20964153123429294, + "timestamp": 6.571277943080247 + }, + { + "x": 7.730932333154636, + "y": 4.131109381030838, + "heading": -0.03901074502265599, + "angularVelocity": 0.22382356990940835, + "velocityX": 2.660613472690525, + "velocityY": -0.1802562725326004, + "timestamp": 6.645048694925424 + }, + { + "x": 7.8935809945490485, + "y": 4.119979536241971, + "heading": -0.02540194557699689, + "angularVelocity": 0.18447418665625143, + "velocityX": 2.204785193673574, + "velocityY": -0.15087069753913437, + "timestamp": 6.718819446770601 + }, + { + "x": 8.022602860038761, + "y": 4.111017505530848, + "heading": -0.014696283077208026, + "angularVelocity": 0.14512069122268073, + "velocityX": 1.7489569004324652, + "velocityY": -0.12148487695953097, + "timestamp": 6.792590198615778 + }, + { + "x": 8.117997929239245, + "y": 4.10422330203326, + "heading": -0.006893999068061529, + "angularVelocity": 0.10576392152707405, + "velocityX": 1.2931286019788828, + "velocityY": -0.09209887831761811, + "timestamp": 6.866360950460955 + }, + { + "x": 8.179766201724098, + "y": 4.0995969339999005, + "heading": -0.0019952337355516638, + "angularVelocity": 0.06640525153913249, + "velocityX": 0.8373002977452005, + "velocityY": -0.06271276783336986, + "timestamp": 6.9401317023061315 }, { "x": 8.207907676696777, "y": 4.097138404846191, - "heading": -2.1241979189811956e-28, - "angularVelocity": 0.02706017505936011, - "velocityX": 0.3823018326887492, - "velocityY": -0.03347720903449786, - "timestamp": 7.01387661283412 - }, - { - "x": 8.200639571345327, - "y": 4.096943687462525, - "heading": -0.0010802899918473687, - "angularVelocity": -0.014022329834604143, - "velocityX": -0.09434112255025054, - "velocityY": -0.0025274615139553986, - "timestamp": 7.0909173046870135 - }, - { - "x": 8.15665056358688, - "y": 4.099133352364744, - "heading": -0.005325598680954133, - "angularVelocity": -0.05510475810903996, - "velocityX": -0.5709840695932878, - "velocityY": 0.02842218637392462, - "timestamp": 7.167957996539907 - }, - { - "x": 8.075940653456616, - "y": 4.103707386175575, - "heading": -0.012735784669566238, - "angularVelocity": -0.09618535101893455, - "velocityX": -1.0476270161796544, - "velocityY": 0.05937166062274877, - "timestamp": 7.2449986883928 - }, - { - "x": 7.958509840643352, - "y": 4.1106657697854265, - "heading": -0.023310616019513074, - "angularVelocity": -0.1372629333357902, - "velocityX": -1.5242699668052726, - "velocityY": 0.09032088682612197, - "timestamp": 7.322039380245694 - }, - { - "x": 7.804358124937036, - "y": 4.120008478318512, - "heading": -0.037049836652876394, - "angularVelocity": -0.17833719172197895, - "velocityX": -2.0009129201573495, - "velocityY": 0.12126979013798225, - "timestamp": 7.399080072098587 - }, - { - "x": 7.613485507099167, - "y": 4.131735481105861, - "heading": -0.05395325351914194, - "angularVelocity": -0.21940894428287439, - "velocityX": -2.4775558636250903, - "velocityY": 0.15221829536189774, - "timestamp": 7.476120763951481 - }, - { - "x": 7.385891991055361, - "y": 4.145846741676362, - "heading": -0.07402084166347252, - "angularVelocity": -0.2604803729261546, - "velocityX": -2.9541987561377083, - "velocityY": 0.18316632718520057, - "timestamp": 7.553161455804374 - }, - { - "x": 7.121577593711164, - "y": 4.16234221776091, - "heading": -0.09725284930983917, - "angularVelocity": -0.30155502355466174, - "velocityX": -3.430841429213758, - "velocityY": 0.21411381034903446, - "timestamp": 7.6302021476572675 - }, - { - "x": 6.835203978246458, - "y": 4.1809363249101, - "heading": -0.09725285259123628, - "angularVelocity": -4.259303804512275e-8, - "velocityX": -3.7171734647908923, - "velocityY": 0.24135436354458084, - "timestamp": 7.707242839510161 - }, - { - "x": 6.5488303639215975, - "y": 4.199530449615618, - "heading": -0.09725285587256685, - "angularVelocity": -4.259217424526975e-8, - "velocityX": -3.7171734499955185, - "velocityY": 0.24135459142842275, - "timestamp": 7.784283531363054 - }, - { - "x": 6.262456749596831, - "y": 4.218124574322582, - "heading": -0.09725285915389743, - "angularVelocity": -4.259217438362485e-8, - "velocityX": -3.7171734499943, - "velocityY": 0.24135459144719548, - "timestamp": 7.861324223215948 - }, - { - "x": 5.976083139919176, - "y": 4.236718770600948, - "heading": -0.09725286243522813, - "angularVelocity": -4.259217585749446e-8, - "velocityX": -3.717173389674077, - "velocityY": 0.24135552045497527, - "timestamp": 7.938364915068841 - }, - { - "x": 5.690024921889411, - "y": 4.259659658304737, - "heading": -0.09725286572349402, - "angularVelocity": -4.26821957688407e-8, - "velocityX": -3.71307955769639, - "velocityY": 0.2977762420357482, - "timestamp": 8.015405606921734 - }, - { - "x": 5.4092552652272206, - "y": 4.319022844462833, - "heading": -0.09725286915389218, - "angularVelocity": -4.452709450605306e-8, - "velocityX": -3.644433219762762, - "velocityY": 0.7705432639604047, - "timestamp": 8.092446298774627 + "heading": -1.3488010425840145e-28, + "angularVelocity": 0.027046406409671455, + "velocityX": 0.3814719827139668, + "velocityY": -0.03332661105134125, + "timestamp": 7.013902454151308 + }, + { + "x": 8.20063541706578, + "y": 4.096934548496511, + "heading": -0.0010815835377122093, + "angularVelocity": -0.014043212095795144, + "velocityX": -0.09442255808535296, + "velocityY": -0.0026468579224898756, + "timestamp": 7.090920698291911 + }, + { + "x": 8.156710596136428, + "y": 4.099093585175751, + "heading": -0.005327811233814745, + "angularVelocity": -0.05513275125242723, + "velocityX": -0.5703170907033845, + "velocityY": 0.028032795389339775, + "timestamp": 7.167938942432514 + }, + { + "x": 8.076133213947529, + "y": 4.103615501486163, + "heading": -0.012738542680979896, + "angularVelocity": -0.09622046736921626, + "velocityX": -1.0462116228175795, + "velocityY": 0.05871227474567349, + "timestamp": 7.244957186573116 + }, + { + "x": 7.958903270196668, + "y": 4.11050027828992, + "heading": -0.023313548124616522, + "angularVelocity": -0.1373052003669573, + "velocityX": -1.5221061588582727, + "velocityY": 0.08939150561765219, + "timestamp": 7.321975430713719 + }, + { + "x": 7.805020764688039, + "y": 4.119747890676612, + "heading": -0.03705257485110129, + "angularVelocity": -0.17838665214703153, + "velocityX": -1.9980006974413145, + "velocityY": 0.12007041305446839, + "timestamp": 7.398993674854322 + }, + { + "x": 7.614485698203272, + "y": 4.131358307939121, + "heading": -0.05395543434106899, + "angularVelocity": -0.21946565620361758, + "velocityX": -2.47389522587572, + "velocityY": 0.15074892179199947, + "timestamp": 7.476011918994924 + }, + { + "x": 7.387298074694608, + "y": 4.1453314935724395, + "heading": -0.07402210734318278, + "angularVelocity": -0.26054441030206643, + "velocityX": -2.949789702994487, + "velocityY": 0.1814269565508347, + "timestamp": 7.553030163135527 + }, + { + "x": 7.1234579111014344, + "y": 4.161667405314612, + "heading": -0.09725284904387498, + "angularVelocity": -0.3016264777249771, + "velocityX": -3.4256839601733207, + "velocityY": 0.21210444258311068, + "timestamp": 7.63004840727613 + }, + { + "x": 6.836784104612281, + "y": 4.180147638379387, + "heading": -0.09725285233031565, + "angularVelocity": -4.2670937406098884e-8, + "velocityX": -3.722154532188625, + "velocityY": 0.23994617471463595, + "timestamp": 7.707066651416732 + }, + { + "x": 6.550110299323958, + "y": 4.198627890073297, + "heading": -0.09725285561668488, + "angularVelocity": -4.2670009668741826e-8, + "velocityX": -3.7221545165971346, + "velocityY": 0.23994641659413532, + "timestamp": 7.784084895557335 + }, + { + "x": 6.26343649403574, + "y": 4.217108141768841, + "heading": -0.0972528589030541, + "angularVelocity": -4.267000957382731e-8, + "velocityX": -3.722154516595767, + "velocityY": 0.23994641661534885, + "timestamp": 7.861103139697938 + }, + { + "x": 5.976762694007825, + "y": 4.235588475064355, + "heading": -0.0972528621894233, + "angularVelocity": -4.2670009669994304e-8, + "velocityX": -3.722154448296313, + "velocityY": 0.23994747610419156, + "timestamp": 7.93812138383854 + }, + { + "x": 5.690447941814378, + "y": 4.258981914103625, + "heading": -0.09725286548373305, + "angularVelocity": -4.277310873834796e-8, + "velocityX": -3.7174925939724037, + "velocityY": 0.3037389296562527, + "timestamp": 8.015139627979142 + }, + { + "x": 5.409459513474409, + "y": 4.318722126567167, + "heading": -0.0972528689233277, + "angularVelocity": -4.4659478908136544e-8, + "velocityX": -3.6483359426761925, + "velocityY": 0.7756631319000459, + "timestamp": 8.092157872119744 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": -7.968133349338986e-8, - "velocityX": -3.5158347290283, - "velocityY": 1.2306645781352283, - "timestamp": 8.16948699062752 - }, - { - "x": 4.909018653493503, - "y": 4.524275328875486, - "heading": -0.09725287687080804, - "angularVelocity": -2.1964896325716688e-8, - "velocityX": -3.192337867898693, - "velocityY": 1.5370761958403918, - "timestamp": 8.241338492962369 - }, - { - "x": 4.71108590609518, - "y": 4.628682093882472, - "heading": -0.09725287720527089, - "angularVelocity": -4.654917845961093e-9, - "velocityX": -2.754747513502236, - "velocityY": 1.453090911313464, - "timestamp": 8.313189995297217 - }, - { - "x": 4.54294597125426, - "y": 4.721368217002082, - "heading": -0.09725287703485493, - "angularVelocity": 2.3717800726027118e-9, - "velocityX": -2.3401032598781413, - "velocityY": 1.2899677822694209, - "timestamp": 8.385041497632065 - }, - { - "x": 4.40369795049572, - "y": 4.80026148816612, - "heading": -0.09725287664966911, - "angularVelocity": 5.360859554294991e-9, - "velocityX": -1.937997344990855, - "velocityY": 1.0980044759032763, - "timestamp": 8.456892999966913 - }, - { - "x": 4.292814205558175, - "y": 4.864307082758427, - "heading": -0.09725287620265413, - "angularVelocity": 6.221372875954885e-9, - "velocityX": -1.5432348849269109, - "velocityY": 0.8913605493429588, - "timestamp": 8.52874450230176 - }, - { - "x": 4.209952469660844, - "y": 4.91286872561946, - "heading": -0.09725287578795568, - "angularVelocity": 5.7716044742121175e-9, - "velocityX": -1.1532359547775557, - "velocityY": 0.6758612037744439, - "timestamp": 8.600596004636609 - }, - { - "x": 4.154873707750821, - "y": 4.945521497298995, - "heading": -0.09725287546928488, - "angularVelocity": 4.435130638682436e-9, - "velocityX": -0.7665638173206213, - "velocityY": 0.4544480020384788, - "timestamp": 8.672447506971457 + "angularVelocity": -8.269819203889138e-8, + "velocityX": -3.5195113993654044, + "velocityY": 1.2349277693272307, + "timestamp": 8.169176116260346 + }, + { + "x": 4.908992808994229, + "y": 4.524204010558568, + "heading": -0.09725287694865485, + "angularVelocity": -2.3032931953732708e-8, + "velocityX": -3.1905636858497752, + "velocityY": 1.535056957756043, + "timestamp": 8.241075673472013 + }, + { + "x": 4.711042301236892, + "y": 4.628576938226348, + "heading": -0.09725287730265034, + "angularVelocity": -4.9234724349367645e-9, + "velocityX": -2.7531533633035683, + "velocityY": 1.4516491021010645, + "timestamp": 8.31297523068368 + }, + { + "x": 4.542898981308052, + "y": 4.721263372143452, + "heading": -0.0972528771251371, + "angularVelocity": 2.468905958013942e-9, + "velocityX": -2.3385863063640335, + "velocityY": 1.2891099404721302, + "timestamp": 8.384874787895347 + }, + { + "x": 4.403657347383129, + "y": 4.800175356394304, + "heading": -0.09725287672080585, + "angularVelocity": 5.623556763444036e-9, + "velocityX": -1.9366132327492167, + "velocityY": 1.0975308793424334, + "timestamp": 8.456774345107014 + }, + { + "x": 4.2927849587410325, + "y": 4.864247225534064, + "heading": -0.09725287625075327, + "angularVelocity": 6.537628358410225e-9, + "velocityX": -1.5420454998867026, + "velocityY": 0.8911302325706711, + "timestamp": 8.528673902318681 + }, + { + "x": 4.20993568647492, + "y": 4.9128352917829865, + "heading": -0.09725287581430106, + "angularVelocity": 6.070304666979789e-9, + "velocityX": -1.1522918287550885, + "velocityY": 0.6757769885269709, + "timestamp": 8.600573459530349 + }, + { + "x": 4.15486747115832, + "y": 4.945509334282957, + "heading": -0.09725287547872855, + "angularVelocity": 4.6672402894194346e-9, + "velocityX": -0.7659047906857641, + "velocityY": 0.45444010738176244, + "timestamp": 8.672473016742016 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 2.459066789540458e-9, - "velocityX": -0.38234244368231685, - "velocityY": 0.22880870103743692, - "timestamp": 8.744299009306305 + "angularVelocity": 2.5887684361837973e-9, + "velocityX": -0.3820001604682187, + "velocityY": 0.22882494094405095, + "timestamp": 8.744372573953683 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 1.7012427908311302e-26, - "velocityX": 2.173382321303034e-25, - "velocityY": 2.8154271739064046e-25, - "timestamp": 8.816150511641153 - }, - { - "x": 4.147908938552217, - "y": 4.9451046130456335, - "heading": -0.10362139740935014, - "angularVelocity": -0.09725274804530627, - "velocityX": 0.31316100417438686, - "velocityY": -0.2574227575741351, - "timestamp": 8.881634749822124 - }, - { - "x": 4.189500528478716, - "y": 4.912093615670984, - "heading": -0.11521450306933155, - "angularVelocity": -0.17703658135172537, - "velocityX": 0.6351389446045882, - "velocityY": -0.5041060000334822, - "timestamp": 8.947118988003096 - }, - { - "x": 4.2528908246594135, - "y": 4.863879621749849, - "heading": -0.13053557559089707, - "angularVelocity": -0.23396580531675798, - "velocityX": 0.9680237251216575, - "velocityY": -0.7362686848076517, - "timestamp": 9.012603226184067 - }, - { - "x": 4.338972089495466, - "y": 4.801805387831604, - "heading": -0.1475645296449939, - "angularVelocity": -0.2600466085752675, - "velocityX": 1.3145341112186142, - "velocityY": -0.9479263352914945, - "timestamp": 9.078087464365039 - }, - { - "x": 4.4488531713485, - "y": 4.727871318118375, - "heading": -0.16346474025801716, - "angularVelocity": -0.24280973642972667, - "velocityX": 1.677977554680654, - "velocityY": -1.1290361126124002, - "timestamp": 9.14357170254601 - }, - { - "x": 4.583832471873963, - "y": 4.645249641206774, - "heading": -0.17403705358129207, - "angularVelocity": -0.1614482143635436, - "velocityX": 2.0612486954866247, - "velocityY": -1.26170326183331, - "timestamp": 9.209055940726982 - }, - { - "x": 4.74502129849887, - "y": 4.559256593692149, - "heading": -0.17264089042553551, - "angularVelocity": 0.021320598582793455, - "velocityX": 2.4614904456771134, - "velocityY": -1.3131869577069004, - "timestamp": 9.274540178907953 - }, - { - "x": 4.931721031095368, - "y": 4.478536439161179, - "heading": -0.14954301236675954, - "angularVelocity": 0.3527242386930233, - "velocityX": 2.8510636724601572, - "velocityY": -1.2326653981663842, - "timestamp": 9.340024417088925 + "angularVelocity": 8.091887340768401e-27, + "velocityX": 1.1127966749801626e-25, + "velocityY": 1.4331677688644632e-25, + "timestamp": 8.81627213116535 + }, + { + "x": 4.147895492623476, + "y": 4.945083891393231, + "heading": -0.10366816031073668, + "angularVelocity": -0.09789638985446751, + "velocityX": 0.31273056471816124, + "velocityY": -0.2575538032912827, + "timestamp": 8.881803505996194 + }, + { + "x": 4.189465649349023, + "y": 4.9120367483382665, + "heading": -0.11534265064860806, + "angularVelocity": -0.17815115840323564, + "velocityX": 0.6343550220463237, + "velocityY": -0.5042949753499016, + "timestamp": 8.947334880827038 + }, + { + "x": 4.252832779954382, + "y": 4.863778368414071, + "heading": -0.13076421865787793, + "angularVelocity": -0.23533106163387682, + "velocityX": 0.9669739230853712, + "velocityY": -0.7364164119670211, + "timestamp": 9.012866255657881 + }, + { + "x": 4.338895975770433, + "y": 4.801661455148063, + "heading": -0.14789285263671287, + "angularVelocity": -0.2613806596160893, + "velocityX": 1.3133128373730498, + "velocityY": -0.9478957739914682, + "timestamp": 9.078397630488725 + }, + { + "x": 4.448770493584648, + "y": 4.7277002158009624, + "heading": -0.16386651500043536, + "angularVelocity": -0.24375594751301116, + "velocityX": 1.6766704208149943, + "velocityY": -1.1286386030816227, + "timestamp": 9.143929005319569 + }, + { + "x": 4.583757647526343, + "y": 4.645084735489715, + "heading": -0.1744545811620451, + "angularVelocity": -0.16157247103301028, + "velocityX": 2.0598858835197476, + "velocityY": -1.260701160695988, + "timestamp": 9.209460380150412 + }, + { + "x": 4.744961446623189, + "y": 4.559146001551583, + "heading": -0.17298362527772781, + "angularVelocity": 0.022446589715449678, + "velocityX": 2.459948376071222, + "velocityY": -1.3114135657914419, + "timestamp": 9.274991754981256 + }, + { + "x": 4.9316701909697125, + "y": 4.478506356002848, + "heading": -0.14972977650571964, + "angularVelocity": 0.3548506167012916, + "velocityX": 2.8491504234189664, + "velocityY": -1.2305501869431363, + "timestamp": 9.3405231298121 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": 0.7985148568065102, - "velocityX": 3.1560555624992586, - "velocityY": -0.9880598134339889, - "timestamp": 9.405508655269896 - }, - { - "x": 5.280245938471686, - "y": 4.380962564632748, - "heading": -0.04650808406049355, - "angularVelocity": 1.167166985692144, - "velocityX": 3.2627221399232984, - "velocityY": -0.7560690286372685, - "timestamp": 9.448985545769855 - }, - { - "x": 5.424678195182071, - "y": 4.359487062895353, - "heading": 0.008191327783648325, - "angularVelocity": 1.2581261266647945, - "velocityX": 3.322046610268118, - "velocityY": -0.493952108590108, - "timestamp": 9.492462436269813 - }, - { - "x": 5.570537921062946, - "y": 4.349646196769418, - "heading": 0.06315884602952881, - "angularVelocity": 1.264292768268082, - "velocityX": 3.354879436030857, - "velocityY": -0.22634705501637128, - "timestamp": 9.535939326769771 - }, - { - "x": 5.717103514016266, - "y": 4.35150566579831, - "heading": 0.11770080768702691, - "angularVelocity": 1.2545046582287314, - "velocityX": 3.3711148904142765, - "velocityY": 0.04276913568356193, - "timestamp": 9.57941621726973 - }, - { - "x": 5.863572420206036, - "y": 4.365085009600068, - "heading": 0.17138870164708653, - "angularVelocity": 1.2348604820326599, - "velocityX": 3.368891024759707, - "velocityY": 0.3123347517635909, - "timestamp": 9.622893107769688 - }, - { - "x": 6.009034373006399, - "y": 4.3903410665362745, - "heading": 0.22417378599549884, - "angularVelocity": 1.2140952064744144, - "velocityX": 3.345730366814133, - "velocityY": 0.5809076188700988, - "timestamp": 9.666369998269646 - }, - { - "x": 6.152424125530893, - "y": 4.427134155183534, - "heading": 0.2765912547304818, - "angularVelocity": 1.205639780863199, - "velocityX": 3.2980682582309235, - "velocityY": 0.8462677119766714, - "timestamp": 9.709846888769604 - }, - { - "x": 6.292378614943397, - "y": 4.475129707153334, - "heading": 0.330369231652926, - "angularVelocity": 1.2369324554730028, - "velocityX": 3.2190547162667764, - "velocityY": 1.1039324896026255, - "timestamp": 9.753323779269563 - }, - { - "x": 6.427989965818357, - "y": 4.530754098625934, - "heading": 0.3892449871584701, - "angularVelocity": 1.3541850585105815, - "velocityX": 3.1191593813520333, - "velocityY": 1.2794013286818071, - "timestamp": 9.796800669769521 - }, - { - "x": 6.568195373885816, - "y": 4.593631496863788, - "heading": 0.42157526724387395, - "angularVelocity": 0.743619879748183, - "velocityX": 3.224826027233803, - "velocityY": 1.4462257423381046, - "timestamp": 9.84027756026948 - }, - { - "x": 6.710207768570418, - "y": 4.660649558034501, - "heading": 0.440928265304377, - "angularVelocity": 0.44513298531599427, - "velocityX": 3.26638802940008, - "velocityY": 1.5414639915606807, - "timestamp": 9.883754450769437 - }, - { - "x": 6.85219596425133, - "y": 4.730282709551517, - "heading": 0.4559517105455699, - "angularVelocity": 0.34555013176959587, - "velocityX": 3.265831434772203, - "velocityY": 1.6016129653311566, - "timestamp": 9.927231341269396 - }, - { - "x": 6.992339110580002, - "y": 4.802100760534676, - "heading": 0.47358684784417715, - "angularVelocity": 0.4056209424320313, - "velocityX": 3.223393962105149, - "velocityY": 1.6518672369917733, - "timestamp": 9.970708231769354 - }, - { - "x": 7.1318594634446475, - "y": 4.8743385269135056, - "heading": 0.49238243611774024, - "angularVelocity": 0.4323121561230585, - "velocityX": 3.2090692609394345, - "velocityY": 1.6615209953641499, - "timestamp": 10.014185122269312 - }, - { - "x": 7.264750404712455, - "y": 4.9484671577912, - "heading": 0.5297243146446315, - "angularVelocity": 0.8588902770525219, - "velocityX": 3.0565879882309943, - "velocityY": 1.7050122496171904, - "timestamp": 10.05766201276927 - }, - { - "x": 7.388081081229855, - "y": 5.024967733696522, - "heading": 0.592130827533757, - "angularVelocity": 1.4353950379497669, - "velocityX": 2.8366949682733105, - "velocityY": 1.7595687047903361, - "timestamp": 10.101138903269229 + "angularVelocity": 0.8007904816369491, + "velocityX": 3.154561228516173, + "velocityY": -0.9868900380705385, + "timestamp": 9.406054504642944 + }, + { + "x": 5.280076264990419, + "y": 4.380996510846826, + "heading": -0.04650142784626664, + "angularVelocity": 1.168572303123448, + "velocityX": 3.262315361452851, + "velocityY": -0.7560984617214831, + "timestamp": 9.449484806140488 + }, + { + "x": 5.424449362393288, + "y": 4.359481998207729, + "heading": 0.008484168318164002, + "angularVelocity": 1.2660652647677388, + "velocityX": 3.3242481038505836, + "velocityY": -0.49538022756564715, + "timestamp": 9.492915107638032 + }, + { + "x": 5.570294583588623, + "y": 4.349555966969111, + "heading": 0.06357759732287922, + "angularVelocity": 1.2685481588892704, + "velocityX": 3.3581443408488143, + "velocityY": -0.22855082503120916, + "timestamp": 9.536345409135576 + }, + { + "x": 5.7169007597582056, + "y": 4.351285887819555, + "heading": 0.11807254033661835, + "angularVelocity": 1.2547677804359936, + "velocityX": 3.3756656323897083, + "velocityY": 0.039832116996496504, + "timestamp": 9.57977571063312 + }, + { + "x": 5.863478153664376, + "y": 4.364694740352783, + "heading": 0.1715206036233513, + "angularVelocity": 1.2306629575149417, + "velocityX": 3.375002909304243, + "velocityY": 0.3087441733276142, + "timestamp": 9.623206012130664 + }, + { + "x": 6.009132990316209, + "y": 4.389745616694493, + "heading": 0.223839780266437, + "angularVelocity": 1.2046698926565014, + "velocityX": 3.3537606608618478, + "velocityY": 0.5768064111442623, + "timestamp": 9.666636313628208 + }, + { + "x": 6.152825143314219, + "y": 4.426310930170149, + "heading": 0.27549834591380606, + "angularVelocity": 1.189459061210764, + "velocityX": 3.3085690875560085, + "velocityY": 0.8419309149332661, + "timestamp": 9.710066615125752 + }, + { + "x": 6.293269117022505, + "y": 4.47409538643224, + "heading": 0.327953495669367, + "angularVelocity": 1.2078007277598, + "velocityX": 3.233778464932586, + "velocityY": 1.1002561486890332, + "timestamp": 9.753496916623297 + }, + { + "x": 6.42854194701545, + "y": 4.529460812617931, + "heading": 0.38848470588787376, + "angularVelocity": 1.3937552384233347, + "velocityX": 3.1147108200618043, + "velocityY": 1.2748110023786727, + "timestamp": 9.79692721812084 + }, + { + "x": 6.568582228965848, + "y": 4.592274312002758, + "heading": 0.42163559085903973, + "angularVelocity": 0.7633123378855858, + "velocityX": 3.2244833013263468, + "velocityY": 1.4463058560249087, + "timestamp": 9.840357519618385 + }, + { + "x": 6.710554540851547, + "y": 4.659324904773962, + "heading": 0.44122441359236847, + "angularVelocity": 0.451040450051598, + "velocityX": 3.2689690605469455, + "velocityY": 1.5438666198298632, + "timestamp": 9.883787821115929 + }, + { + "x": 6.8526306110413895, + "y": 4.729009831535272, + "heading": 0.45600063364560606, + "angularVelocity": 0.3402283554046515, + "velocityX": 3.2713581368500897, + "velocityY": 1.604523209797458, + "timestamp": 9.927218122613473 + }, + { + "x": 6.993061785936844, + "y": 4.80083300788703, + "heading": 0.47277807951263723, + "angularVelocity": 0.3863073773038364, + "velocityX": 3.233483767166502, + "velocityY": 1.6537572587613216, + "timestamp": 9.970648424111017 + }, + { + "x": 7.132717903538657, + "y": 4.873239187590198, + "heading": 0.4909920458715961, + "angularVelocity": 0.4193838341184207, + "velocityX": 3.2156377641013956, + "velocityY": 1.6671811432685018, + "timestamp": 10.014078725608561 + }, + { + "x": 7.265296328377782, + "y": 4.947694128111601, + "heading": 0.5290109485090398, + "angularVelocity": 0.8754003846736805, + "velocityX": 3.052671067609889, + "velocityY": 1.7143546775886709, + "timestamp": 10.057509027106105 + }, + { + "x": 7.388337158351145, + "y": 5.0245588832069705, + "heading": 0.5919048706788004, + "angularVelocity": 1.4481576226984543, + "velocityX": 2.833064144864831, + "velocityY": 1.7698416185233745, + "timestamp": 10.10093932860365 }, { "x": 7.501500129699707, "y": 5.105462551116943, "heading": 0.6747401864044066, - "angularVelocity": 1.9000751415451151, - "velocityX": 2.608720337761042, - "velocityY": 1.8514391552564737, - "timestamp": 10.144615793769187 - }, - { - "x": 7.646324439274632, - "y": 5.2181193934144225, - "heading": 0.716438811665056, - "angularVelocity": 0.6791497141368164, - "velocityX": 2.3587681328355696, - "velocityY": 1.8348533498079573, - "timestamp": 10.206014075168227 - }, - { - "x": 7.772039173775546, - "y": 5.317779255820348, - "heading": 0.7438971482542158, - "angularVelocity": 0.44721669668085284, - "velocityX": 2.047528556766434, - "velocityY": 1.62317022781495, - "timestamp": 10.267412356567267 - }, - { - "x": 7.878893815894361, - "y": 5.4039493449354215, - "heading": 0.7593796608518738, - "angularVelocity": 0.2521652437962253, - "velocityX": 1.7403523304560495, - "velocityY": 1.4034609300387069, - "timestamp": 10.328810637966306 - }, - { - "x": 7.966980879790036, - "y": 5.476461142089676, - "heading": 0.7636631361192312, - "angularVelocity": 0.06976539358680627, - "velocityX": 1.4346828915809557, - "velocityY": 1.1810069516927688, - "timestamp": 10.390208919365346 - }, - { - "x": 8.036348554918035, - "y": 5.535229545877986, - "heading": 0.7571399208631822, - "angularVelocity": -0.10624426461798567, - "velocityX": 1.1297983192260301, - "velocityY": 0.9571669181807075, - "timestamp": 10.451607200764386 - }, - { - "x": 8.087026476479373, - "y": 5.580203129397342, - "heading": 0.7400467384743379, - "angularVelocity": -0.27839838509082054, - "velocityX": 0.8253964183780998, - "velocityY": 0.7324892895138765, - "timestamp": 10.513005482163425 - }, - { - "x": 8.119034784241132, - "y": 5.611347352514501, - "heading": 0.7125421599400041, - "angularVelocity": -0.44796984390452543, - "velocityX": 0.5213225359473886, - "velocityY": 0.5072491022142075, - "timestamp": 10.574403763562465 + "angularVelocity": 1.90731615644646, + "velocityX": 2.605622513464768, + "velocityY": 1.8628391956833978, + "timestamp": 10.144369630101194 + }, + { + "x": 7.646241899595399, + "y": 5.218086028321965, + "heading": 0.7163743700532657, + "angularVelocity": 0.6780169831508551, + "velocityX": 2.357134680201247, + "velocityY": 1.8340849646658135, + "timestamp": 10.205775442467985 + }, + { + "x": 7.771890778556216, + "y": 5.317734467190133, + "heading": 0.7437412121017494, + "angularVelocity": 0.445671850817889, + "velocityX": 2.0462049782891385, + "velocityY": 1.6227851245244513, + "timestamp": 10.267181254834776 + }, + { + "x": 7.878702605098028, + "y": 5.403903098685203, + "heading": 0.7591487568785513, + "angularVelocity": 0.25091345888837385, + "velocityX": 1.7394416330460891, + "velocityY": 1.4032650684655832, + "timestamp": 10.328587067201568 + }, + { + "x": 7.966772466463858, + "y": 5.476419180889776, + "heading": 0.7633894511988232, + "angularVelocity": 0.06906014523415532, + "velocityX": 1.4342267933818382, + "velocityY": 1.180931892430925, + "timestamp": 10.389992879568359 + }, + { + "x": 8.036149930453574, + "y": 5.535195459636144, + "heading": 0.7568637162743023, + "angularVelocity": -0.10627226760784944, + "velocityX": 1.1298191704607576, + "velocityY": 0.9571777732583954, + "timestamp": 10.45139869193515 + }, + { + "x": 8.086865488706417, + "y": 5.5801792035680275, + "heading": 0.7398132249499985, + "angularVelocity": -0.27766901319466764, + "velocityX": 0.8259081070356892, + "velocityY": 0.7325649184996513, + "timestamp": 10.512804504301942 + }, + { + "x": 8.118939863866766, + "y": 5.611334997780328, + "heading": 0.7123999066441988, + "angularVelocity": -0.4464287214710813, + "velocityX": 0.5223345140157242, + "velocityY": 0.5073753283516645, + "timestamp": 10.574210316668733 }, { "x": 8.1323881149292, "y": 5.628637313842773, "heading": 0.6747401864044066, - "angularVelocity": -0.6156845545873708, - "velocityX": 0.2174870433470383, - "velocityY": 0.2816033435187223, - "timestamp": 10.635802044961505 - }, - { - "x": 8.125741105489272, - "y": 5.631444479838457, - "heading": 0.6235311710708692, - "angularVelocity": -0.7910339249344661, - "velocityX": -0.1026774276383878, - "velocityY": 0.04336274620875669, - "timestamp": 10.70053885803533 - }, - { - "x": 8.09835684603502, - "y": 5.618815221035742, - "heading": 0.5616094020000137, - "angularVelocity": -0.9565155609411891, - "velocityX": -0.42300907557840733, - "velocityY": -0.19508619907365696, - "timestamp": 10.765275671109155 - }, - { - "x": 8.050222803941077, - "y": 5.59073345676368, - "heading": 0.48977295422299394, - "angularVelocity": -1.1096692031332702, - "velocityX": -0.7435343170052635, - "velocityY": -0.4337835450756361, - "timestamp": 10.83001248418298 - }, - { - "x": 7.981324451485502, - "y": 5.547180062425158, - "heading": 0.40904421087349874, - "angularVelocity": -1.2470299280477726, - "velocityX": -1.0642839704976186, - "velocityY": -0.6727763118158692, - "timestamp": 10.894749297256805 - }, - { - "x": 7.891645067392915, - "y": 5.488132407343552, - "heading": 0.32078665814595236, - "angularVelocity": -1.3633286616518716, - "velocityX": -1.3852919202296423, - "velocityY": -0.9121186582704987, - "timestamp": 10.95948611033063 - }, - { - "x": 7.781166411701721, - "y": 5.413563935212295, - "heading": 0.22693931715370197, - "angularVelocity": -1.4496750231622744, - "velocityX": -1.7065816255922823, - "velocityY": -1.1518712242788174, - "timestamp": 11.024222923404455 - }, - { - "x": 7.649874183396069, - "y": 5.323444723125603, - "heading": 0.13053857941250757, - "angularVelocity": -1.4891177548585681, - "velocityX": -2.0280922410549644, - "velocityY": -1.3920860142423153, - "timestamp": 11.08895973647828 - }, - { - "x": 7.497788734174063, - "y": 5.217751261949818, - "heading": 0.0370488934612777, - "angularVelocity": -1.4441502680185108, - "velocityX": -2.3492884805522913, - "velocityY": -1.6326639535877772, - "timestamp": 11.153696549552105 - }, - { - "x": 7.325168258315647, - "y": 5.096564905445784, - "heading": -0.04136336755772595, - "angularVelocity": -1.2112468516110346, - "velocityX": -2.66649635133507, - "velocityY": -1.871985208259067, - "timestamp": 11.21843336262593 - }, - { - "x": 7.133849861870766, - "y": 4.963599555351033, - "heading": -0.06503952290078549, - "angularVelocity": -0.36572939288900796, - "velocityX": -2.955326148457454, - "velocityY": -2.0539372233711526, - "timestamp": 11.283170175699755 - }, - { - "x": 6.940221298800236, - "y": 4.819868100403324, - "heading": -0.06503954882158174, - "angularVelocity": -4.004027232662301e-7, - "velocityX": -2.9910116651821412, - "velocityY": -2.2202429826719707, - "timestamp": 11.34790698877358 - }, - { - "x": 6.740356177089726, - "y": 4.684943363475215, - "heading": -0.06503957483267325, - "angularVelocity": -4.017975287461778e-7, - "velocityX": -3.08734879306747, - "velocityY": -2.0842041880288593, - "timestamp": 11.412643801847405 - }, - { - "x": 6.527131993072097, - "y": 4.57231033799875, - "heading": -0.06503960182577956, - "angularVelocity": -4.1696686974459996e-7, - "velocityX": -3.293708384663109, - "velocityY": -1.739860523378229, - "timestamp": 11.47738061492123 - }, - { - "x": 6.303024971058329, - "y": 4.483277800206398, - "heading": -0.06503963136622377, - "angularVelocity": -4.5631600956130875e-7, - "velocityX": -3.461817339667266, - "velocityY": -1.3752999810296351, - "timestamp": 11.542117427995056 - }, - { - "x": 6.070638079320141, - "y": 4.41887996664659, - "heading": -0.06503966569966328, - "angularVelocity": -5.303541813322711e-7, - "velocityX": -3.5897178236619824, - "velocityY": -0.9947637287360487, - "timestamp": 11.60685424106888 - }, - { - "x": 5.832670509915108, - "y": 4.379864860722445, - "heading": -0.06503973197653827, - "angularVelocity": -0.0000010237895848927841, - "velocityX": -3.67592345229684, - "velocityY": -0.60267263820436, - "timestamp": 11.671591054142706 - }, - { - "x": 5.593035616597729, - "y": 4.366766567771227, - "heading": -0.0691739587008402, - "angularVelocity": -0.06386206746982208, - "velocityX": -3.7016788738750415, - "velocityY": -0.20233144526718327, - "timestamp": 11.73632786721653 - }, - { - "x": 5.359733281234782, - "y": 4.378764088602827, - "heading": -0.08321570293446177, - "angularVelocity": -0.2169050894984338, - "velocityX": -3.6038588290852247, - "velocityY": 0.1853276406720581, - "timestamp": 11.801064680290356 + "angularVelocity": -0.6132924358176748, + "velocityX": 0.21900615827864758, + "velocityY": 0.28177000507860306, + "timestamp": 10.635616129035524 + }, + { + "x": 8.125871504088575, + "y": 5.631456874364778, + "heading": 0.6237350082465105, + "angularVelocity": -0.7877273460269657, + "velocityX": -0.10064296896850039, + "velocityY": 0.04354547924694944, + "timestamp": 10.700365916401863 + }, + { + "x": 8.098646920538245, + "y": 5.618838281388744, + "heading": 0.5620672547636553, + "angularVelocity": -0.9524008647928723, + "velocityX": -0.420458269558492, + "velocityY": -0.19488238478129047, + "timestamp": 10.7651157037682 + }, + { + "x": 8.050701885841999, + "y": 5.590765848775558, + "heading": 0.4905264994241073, + "angularVelocity": -1.1048801586758534, + "velocityX": -0.7404662879429073, + "velocityY": -0.43355250657982763, + "timestamp": 10.82986549113454 + }, + { + "x": 7.982021903063244, + "y": 5.547220937725296, + "heading": 0.4101242431657529, + "angularVelocity": -1.2417377651521577, + "velocityX": -1.0606981979751946, + "velocityY": -0.6725104872375124, + "timestamp": 10.894615278500877 + }, + { + "x": 7.892590228702109, + "y": 5.488181520264753, + "heading": 0.3222094538660446, + "angularVelocity": -1.3577618224799397, + "velocityX": -1.3811886957273356, + "velocityY": -0.9118086693707895, + "timestamp": 10.959365065867216 + }, + { + "x": 7.782388469757928, + "y": 5.4136218029079215, + "heading": 0.22870015350775494, + "angularVelocity": -1.444163821407427, + "velocityX": -1.7019632561986164, + "velocityY": -1.151505207808494, + "timestamp": 11.024114853233554 + }, + { + "x": 7.6514017789834075, + "y": 5.323512852150783, + "heading": 0.13259842236832783, + "angularVelocity": -1.4842014939092711, + "velocityX": -2.0229671185394063, + "velocityY": -1.3916485971965442, + "timestamp": 11.088864640599892 + }, + { + "x": 7.49964848633853, + "y": 5.217832335954841, + "heading": 0.039303344877862086, + "angularVelocity": -1.4408553492635434, + "velocityX": -2.3436878917654713, + "velocityY": -1.6321368840646113, + "timestamp": 11.15361442796623 + }, + { + "x": 7.327377192032374, + "y": 5.096661162563075, + "heading": -0.03916824690732434, + "angularVelocity": -1.211920455293751, + "velocityX": -2.660569266914921, + "velocityY": -1.871375618675154, + "timestamp": 11.218364215332569 + }, + { + "x": 7.13646132842574, + "y": 4.963335099646079, + "heading": -0.06425840185235647, + "angularVelocity": -0.3874940129622072, + "velocityX": -2.9485172287358923, + "velocityY": -2.059096536683113, + "timestamp": 11.283114002698907 + }, + { + "x": 6.942657433468542, + "y": 4.819228787923636, + "heading": -0.06425842751021618, + "angularVelocity": -3.962616830885222e-7, + "velocityX": -2.993120175989199, + "velocityY": -2.2255874124670814, + "timestamp": 11.347863790065245 + }, + { + "x": 6.742262463352352, + "y": 4.684438451996059, + "heading": -0.06425845326497977, + "angularVelocity": -3.977582728745682e-7, + "velocityX": -3.0949131768171627, + "velocityY": -2.0817108659363774, + "timestamp": 11.412613577431584 + }, + { + "x": 6.528555264699025, + "y": 4.57194040086737, + "heading": -0.06425848001742611, + "angularVelocity": -4.1316655101928246e-7, + "velocityX": -3.3005081150957944, + "velocityY": -1.7374273446212585, + "timestamp": 11.477363364797922 + }, + { + "x": 6.30400572206588, + "y": 4.483035506841414, + "heading": -0.06425850931637903, + "angularVelocity": -4.5249496742682505e-7, + "velocityX": -3.4679579928610247, + "velocityY": -1.3730530653784834, + "timestamp": 11.54211315216426 + }, + { + "x": 6.071209363502924, + "y": 4.41875151951687, + "heading": -0.06425854338858503, + "angularVelocity": -5.262134035469361e-7, + "velocityX": -3.5953223636990725, + "velocityY": -0.9928061533367126, + "timestamp": 11.606862939530599 + }, + { + "x": 5.832857094389743, + "y": 4.379831537545609, + "heading": -0.06425861345468233, + "angularVelocity": -0.0000010821054427575679, + "velocityX": -3.6811282138216472, + "velocityY": -0.6010827765512304, + "timestamp": 11.671612726896937 + }, + { + "x": 5.593007284030673, + "y": 4.3668097640136185, + "heading": -0.06894618880268864, + "angularVelocity": -0.07239522380954139, + "velocityX": -3.704256339902067, + "velocityY": -0.20110913196235056, + "timestamp": 11.736362514263275 + }, + { + "x": 5.359682212088612, + "y": 4.37881821771202, + "heading": -0.08316362699074491, + "angularVelocity": -0.2195750560170572, + "velocityX": -3.6034878481062056, + "velocityY": 0.1854593534100846, + "timestamp": 11.801112301629614 }, { "x": 5.138392925262451, "y": 4.413834095001221, "heading": -0.09725287529259724, - "angularVelocity": -0.21683446699990425, - "velocityX": -3.419080202788392, - "velocityY": 0.5417320490954629, - "timestamp": 11.86580149336418 - }, - { - "x": 4.905765298437056, - "y": 4.481387310799775, - "heading": -0.10673913036260185, - "angularVelocity": -0.12897497729525575, - "velocityX": -3.1628016184093535, - "velocityY": 0.9184524777738038, - "timestamp": 11.939352621411974 - }, - { - "x": 4.700482894835419, - "y": 4.568326227070152, - "heading": -0.11059429780349972, - "angularVelocity": -0.05241479693408341, - "velocityX": -2.791016386155829, - "velocityY": 1.1820201617286428, - "timestamp": 12.012903749459767 - }, - { - "x": 4.527923671898321, - "y": 4.662591020242044, - "heading": -0.11045427798457029, - "angularVelocity": 0.0019037072937678628, - "velocityX": -2.3461125276687587, - "velocityY": 1.2816226708397902, - "timestamp": 12.08645487750756 - }, - { - "x": 4.3886983023339585, - "y": 4.75311454797141, - "heading": -0.10805031221492864, - "angularVelocity": 0.03268428144405263, - "velocityX": -1.8929059724807213, - "velocityY": 1.2307564837148823, - "timestamp": 12.160006005555353 - }, - { - "x": 4.281048300464299, - "y": 4.832306401053517, - "heading": -0.1047189693449996, - "angularVelocity": 0.04529288616436113, - "velocityX": -1.4636077613889036, - "velocityY": 1.076691210373392, - "timestamp": 12.233557133603146 - }, - { - "x": 4.20282348849849, - "y": 4.895385445772344, - "heading": -0.10139996108706777, - "angularVelocity": 0.04512518497031254, - "velocityX": -1.0635433343045229, - "velocityY": 0.8576217169346194, - "timestamp": 12.307108261650939 - }, - { - "x": 4.15212537277285, - "y": 4.939288518901389, - "heading": -0.09875289338109836, - "angularVelocity": 0.0359894916125467, - "velocityX": -0.6892907977250395, - "velocityY": 0.5969055036180668, - "timestamp": 12.380659389698732 + "angularVelocity": -0.21759528293334457, + "velocityX": -3.417606386475971, + "velocityY": 0.5407875255417884, + "timestamp": 11.865862088995952 + }, + { + "x": 4.905742763187506, + "y": 4.481316212156489, + "heading": -0.10678575188012002, + "angularVelocity": -0.1295126274523243, + "velocityX": -3.1607598704228184, + "velocityY": 0.916804725056799, + "timestamp": 11.939467858371852 + }, + { + "x": 4.7004457351805575, + "y": 4.5681827460061015, + "heading": -0.11066751280962474, + "angularVelocity": -0.052737183000978086, + "velocityX": -2.7891431574949794, + "velocityY": 1.1801593079747448, + "timestamp": 12.013073627747753 + }, + { + "x": 4.527868340415521, + "y": 4.662417244866579, + "heading": -0.11053335894042673, + "angularVelocity": 0.001822599917580837, + "velocityX": -2.344617768801401, + "velocityY": 1.280259681537001, + "timestamp": 12.086679397123653 + }, + { + "x": 4.3886358627816255, + "y": 4.752956990495115, + "heading": -0.10811901035095504, + "angularVelocity": 0.03280107809405142, + "velocityX": -1.8915973410024705, + "velocityY": 1.2300631648336415, + "timestamp": 12.160285166499554 + }, + { + "x": 4.28099452080793, + "y": 4.832191591556716, + "heading": -0.1047684534601318, + "angularVelocity": 0.04552030254193966, + "velocityX": -1.462403598065479, + "velocityY": 1.0764726968201856, + "timestamp": 12.233890935875454 + }, + { + "x": 4.202788693101616, + "y": 4.895319939941382, + "heading": -0.10142823841231594, + "angularVelocity": 0.045379799384441775, + "velocityX": -1.0624958935884563, + "velocityY": 0.8576548947171904, + "timestamp": 12.307496705251355 + }, + { + "x": 4.1521113166286625, + "y": 4.9392645069883585, + "heading": -0.0987633314851406, + "angularVelocity": 0.03620513649637662, + "velocityX": -0.6884973406656041, + "velocityY": 0.5970261219953313, + "timestamp": 12.381102474627255 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 0.020394222744298442, - "velocityX": -0.3361409221475862, - "velocityY": 0.3082648480890461, - "timestamp": 12.454210517746525 + "angularVelocity": 0.020520894018912154, + "velocityX": -0.33570042229438307, + "velocityY": 0.30836223056847767, + "timestamp": 12.454708244003156 }, { "x": 4.127401828765869, "y": 4.96196174621582, "heading": -0.09725287529259724, - "angularVelocity": 1.761901225942554e-27, - "velocityX": -1.9870172205028814e-26, - "velocityY": -2.1545999896560787e-26, - "timestamp": 12.527761645794318 + "angularVelocity": 6.898541114622747e-27, + "velocityX": -7.290217474147722e-26, + "velocityY": -6.920027775581766e-26, + "timestamp": 12.528314013379056 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceLanePGFH.1.traj b/src/main/deploy/choreo/SourceLanePGFH.1.traj new file mode 100644 index 00000000..e3a7ea65 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGFH.1.traj @@ -0,0 +1,230 @@ +{ + "samples": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": -2.8840116321326e-26, + "angularVelocity": -5.669163571419092e-28, + "velocityX": 2.108709871721435e-25, + "velocityY": 6.042704411837295e-25, + "timestamp": 0 + }, + { + "x": 0.4005024246667089, + "y": 4.115350466677215, + "heading": -0.015208625308300914, + "angularVelocity": -0.3074427174555653, + "velocityX": 0.27295183145226637, + "velocityY": -0.11691994544690736, + "timestamp": 0.04946815925311036 + }, + { + "x": 0.42752782779164794, + "y": 4.103765609299543, + "heading": -0.04521317655829015, + "angularVelocity": -0.6065427075316668, + "velocityX": 0.5463191582824014, + "velocityY": -0.23418816371145634, + "timestamp": 0.09893631850622071 + }, + { + "x": 0.46809993891055535, + "y": 4.086359371416315, + "heading": -0.08951799702765434, + "angularVelocity": -0.8956229853363399, + "velocityX": 0.8201661782342632, + "velocityY": -0.3518675072214983, + "timestamp": 0.14840447775933108 + }, + { + "x": 0.5222480284136939, + "y": 4.063106533888518, + "heading": -0.14747656628903638, + "angularVelocity": -1.1716338375323283, + "velocityX": 1.0946048998120714, + "velocityY": -0.4700566562184248, + "timestamp": 0.19787263701244143 + }, + { + "x": 0.5900100551308932, + "y": 4.033975950010924, + "heading": -0.21819717233761302, + "angularVelocity": -1.4296187106280898, + "velocityX": 1.3698109600255401, + "velocityY": -0.5888754365923217, + "timestamp": 0.24734079626555178 + }, + { + "x": 0.6714360195671307, + "y": 3.9989305950603806, + "heading": -0.30041829025546435, + "angularVelocity": -1.6621018279082538, + "velocityX": 1.6460277816203115, + "velocityY": -0.708442672613487, + "timestamp": 0.29680895551866215 + }, + { + "x": 0.7665898116570742, + "y": 3.9579282066959594, + "heading": -0.39234342678059203, + "angularVelocity": -1.858268791745021, + "velocityX": 1.923536139743475, + "velocityY": -0.82886424284816, + "timestamp": 0.34627711477177253 + }, + { + "x": 0.8755459842805163, + "y": 3.9109205987886493, + "heading": -0.49137598841221336, + "angularVelocity": -2.0019455570381792, + "velocityX": 2.2025515860809297, + "velocityY": -0.9502598968113863, + "timestamp": 0.3957452740248829 + }, + { + "x": 0.9983715175527711, + "y": 3.8578517541730113, + "heading": -0.5934882805424371, + "angularVelocity": -2.06420238132882, + "velocityX": 2.482921037021039, + "velocityY": -1.0727879390883417, + "timestamp": 0.4452134332779933 + }, + { + "x": 1.1350267000973346, + "y": 3.798688114099643, + "heading": -0.6910495610477184, + "angularVelocity": -1.9722035745477453, + "velocityX": 2.7624877215533625, + "velocityY": -1.1959943722718582, + "timestamp": 0.49468159253110366 + }, + { + "x": 1.2838521273183288, + "y": 3.733527390579999, + "heading": -0.7598419105522805, + "angularVelocity": -1.3906389593471014, + "velocityX": 3.008509503244484, + "velocityY": -1.3172255548511609, + "timestamp": 0.544149751784214 + }, + { + "x": 1.4435364367922139, + "y": 3.6623848351594903, + "heading": -0.7899999551615338, + "angularVelocity": -0.6096455793906863, + "velocityX": 3.2280220627745444, + "velocityY": -1.4381484270821239, + "timestamp": 0.5936179110373243 + }, + { + "x": 1.613097529588805, + "y": 3.58963115719978, + "heading": -0.7899999775809353, + "angularVelocity": -4.532087301884173e-7, + "velocityX": 3.4276814693874025, + "velocityY": -1.4707173070147306, + "timestamp": 0.6430860702904346 + }, + { + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "angularVelocity": -4.532019196899546e-7, + "velocityX": 3.427682961038322, + "velocityY": -1.4707138305455802, + "timestamp": 0.692554229543545 + }, + { + "x": 2.0243963764510555, + "y": 3.413262421692379, + "heading": -0.79, + "angularVelocity": 4.0902201996407894e-16, + "velocityX": 3.428232758686476, + "velocityY": -1.469432170196615, + "timestamp": 0.7630680182190419 + }, + { + "x": 2.250825412444176, + "y": 3.3162088869723196, + "heading": -0.79, + "angularVelocity": -2.4813985391407045e-16, + "velocityX": 3.211131329719671, + "velocityY": -1.3763766852281583, + "timestamp": 0.8335818068945389 + }, + { + "x": 2.4489508274763203, + "y": 3.231287040432583, + "heading": -0.79, + "angularVelocity": -2.180277121693513e-16, + "velocityX": 2.8097400345897383, + "velocityY": -1.2043296514749058, + "timestamp": 0.9040955955700358 + }, + { + "x": 2.618772615081807, + "y": 3.1584968848445327, + "heading": -0.79, + "angularVelocity": -1.8012058014682582e-16, + "velocityX": 2.40834864776595, + "velocityY": -1.0322825784192244, + "timestamp": 0.9746093842455328 + }, + { + "x": 2.760290773105409, + "y": 3.0978384211319567, + "heading": -0.79, + "angularVelocity": -1.4736426874017007e-16, + "velocityX": 2.006957230377533, + "velocityY": -0.8602354922627292, + "timestamp": 1.0451231729210297 + }, + { + "x": 2.8735053004695117, + "y": 3.0493116497567487, + "heading": -0.79, + "angularVelocity": -8.058066541294709e-17, + "velocityX": 1.6055657977068005, + "velocityY": -0.6881883995558266, + "timestamp": 1.1156369615965267 + }, + { + "x": 2.9584161965275473, + "y": 3.0129165709960453, + "heading": -0.79, + "angularVelocity": -9.176897771941303e-17, + "velocityX": 1.2041743558666786, + "velocityY": -0.5161413029186794, + "timestamp": 1.1861507502720237 + }, + { + "x": 3.01502346084847, + "y": 2.988653185034604, + "heading": -0.79, + "angularVelocity": -1.736982373628201e-17, + "velocityX": 0.8027829079136304, + "velocityY": -0.3440942036613692, + "timestamp": 1.2566645389475206 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -2.7589092071586022e-17, + "velocityX": 0.4013914555942062, + "velocityY": -0.172047102532514, + "timestamp": 1.3271783276230176 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -2.3649328254113597e-28, + "velocityX": 1.8626809073397165e-27, + "velocityY": -1.7812474793700614e-27, + "timestamp": 1.3976921162985145 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGFH.2.traj b/src/main/deploy/choreo/SourceLanePGFH.2.traj new file mode 100644 index 00000000..24cf7189 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGFH.2.traj @@ -0,0 +1,680 @@ +{ + "samples": [ + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -2.3649328254113597e-28, + "velocityX": 1.8626809073397165e-27, + "velocityY": -1.7812474793700614e-27, + "timestamp": 0 + }, + { + "x": 3.0599636803008443, + "y": 2.9660757998478573, + "heading": -0.7576339587685645, + "angularVelocity": 0.5546549208152399, + "velocityX": 0.2851002035438971, + "velocityY": -0.17900720432616107, + "timestamp": 0.058353473514421106 + }, + { + "x": 3.0934189139338764, + "y": 2.9451764981868647, + "heading": -0.6947262972943399, + "angularVelocity": 1.078044847813185, + "velocityX": 0.573320346127542, + "velocityY": -0.3581500877720281, + "timestamp": 0.11670694702884221 + }, + { + "x": 3.1439054797379202, + "y": 2.91379696376208, + "heading": -0.6037467592544796, + "angularVelocity": 1.5591109245172228, + "velocityX": 0.8651852711315816, + "velocityY": -0.537749212427429, + "timestamp": 0.17506042054326332 + }, + { + "x": 3.211665591806839, + "y": 2.871861091639316, + "heading": -0.4882356317885913, + "angularVelocity": 1.9795073113743915, + "velocityX": 1.1612010046355354, + "velocityY": -0.7186525428070697, + "timestamp": 0.23341389405768442 + }, + { + "x": 3.2969495157584743, + "y": 2.8192307268976187, + "heading": -0.3529793972138387, + "angularVelocity": 2.3178780358520816, + "velocityX": 1.461505525126263, + "velocityY": -0.9019234258384052, + "timestamp": 0.29176736757210553 + }, + { + "x": 3.3999955244455466, + "y": 2.755745289292902, + "heading": -0.2040546245446104, + "angularVelocity": 2.552114959059364, + "velocityX": 1.7658933133021877, + "velocityY": -1.0879461629482488, + "timestamp": 0.35012084108652664 + }, + { + "x": 3.5210141241877033, + "y": 2.681299358314012, + "heading": -0.05002850479601411, + "angularVelocity": 2.63953644011494, + "velocityX": 2.073888535739847, + "velocityY": -1.2757754850787486, + "timestamp": 0.40847431460094774 + }, + { + "x": 3.659914538226331, + "y": 2.5959847244718666, + "heading": 0.09335665207111186, + "angularVelocity": 2.4571828930062045, + "velocityX": 2.380328122271953, + "velocityY": -1.4620317986908105, + "timestamp": 0.46682778811536885 + }, + { + "x": 3.8149146487433727, + "y": 2.5012950214969263, + "heading": 0.197502595567386, + "angularVelocity": 1.7847428306137818, + "velocityX": 2.6562276619014966, + "velocityY": -1.622691800026939, + "timestamp": 0.52518126162979 + }, + { + "x": 3.982928880488226, + "y": 2.395156661504235, + "heading": 0.2522871475824207, + "angularVelocity": 0.9388396048351036, + "velocityX": 2.8792498822427737, + "velocityY": -1.818886753441695, + "timestamp": 0.5835347351442111 + }, + { + "x": 4.164002347253148, + "y": 2.2776755114172382, + "heading": 0.25752210352171573, + "angularVelocity": 0.08971112813020911, + "velocityX": 3.1030452149549066, + "velocityY": -2.013267471694952, + "timestamp": 0.6418882086586319 + }, + { + "x": 4.349312713000023, + "y": 2.1635166721855787, + "heading": 0.25752213554345277, + "angularVelocity": 5.487546005300222e-7, + "velocityX": 3.1756527004528508, + "velocityY": -1.9563332284486414, + "timestamp": 0.7002416821730528 + }, + { + "x": 4.534623121850304, + "y": 2.049357902922309, + "heading": 0.25752216756517515, + "angularVelocity": 5.487543495141043e-7, + "velocityX": 3.1756534391133453, + "velocityY": -1.9563320294045097, + "timestamp": 0.7585951556874737 + }, + { + "x": 4.719933530702919, + "y": 1.9351991336628294, + "heading": 0.25752219958689765, + "angularVelocity": 5.487543505371369e-7, + "velocityX": 3.1756534391533564, + "velocityY": -1.9563320293395612, + "timestamp": 0.8169486292018946 + }, + { + "x": 4.905244050568848, + "y": 1.8210405446080342, + "heading": 0.2575222316086183, + "angularVelocity": 5.487543204113664e-7, + "velocityX": 3.1756553415819, + "velocityY": -1.9563289411826175, + "timestamp": 0.8753021027163155 + }, + { + "x": 5.095847730511029, + "y": 1.715957675476469, + "heading": 0.25752226374645537, + "angularVelocity": 5.507442001002949e-7, + "velocityX": 3.2663639105404108, + "velocityY": -1.8007988694211323, + "timestamp": 0.9336555762307364 + }, + { + "x": 5.295726367846681, + "y": 1.629814177194248, + "heading": 0.2575222972409487, + "angularVelocity": 5.739931368957973e-7, + "velocityX": 3.4253083029625735, + "velocityY": -1.4762359992320253, + "timestamp": 0.9920090497451572 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.25752233398290164, + "angularVelocity": 6.296446587487592e-7, + "velocityX": 3.5520975347185666, + "velocityY": -1.1378104640967532, + "timestamp": 1.0503625232595781 + }, + { + "x": 5.694801641426809, + "y": 1.520040343984435, + "heading": 0.25752236705287185, + "angularVelocity": 6.272660461318871e-7, + "velocityX": 3.637995437353645, + "velocityY": -0.8227992972681996, + "timestamp": 1.1030833271940672 + }, + { + "x": 5.889658502180153, + "y": 1.4936017291226997, + "heading": 0.25752239835429896, + "angularVelocity": 5.937205950740546e-7, + "velocityX": 3.6960145940770253, + "velocityY": -0.5014835300043611, + "timestamp": 1.1558041311285563 + }, + { + "x": 6.086080950951087, + "y": 1.484305583780122, + "heading": 0.25752242915160917, + "angularVelocity": 5.841585845609402e-7, + "velocityX": 3.7257104238207144, + "velocityY": -0.17632783737761368, + "timestamp": 1.2085249350630454 + }, + { + "x": 6.282694937149082, + "y": 1.4876428211340982, + "heading": 0.257522460230909, + "angularVelocity": 5.895073207644364e-7, + "velocityX": 3.7293434759133812, + "velocityY": 0.06330019849702921, + "timestamp": 1.2612457389975344 + }, + { + "x": 6.479308911969213, + "y": 1.490980728749722, + "heading": 0.25752249131023625, + "angularVelocity": 5.895078402009839e-7, + "velocityX": 3.7293432600998244, + "velocityY": 0.06331291191559962, + "timestamp": 1.3139665429320235 + }, + { + "x": 6.675718826372289, + "y": 1.500538040759823, + "heading": 0.25752252283498844, + "angularVelocity": 5.979565905391123e-7, + "velocityX": 3.725472673882885, + "velocityY": 0.18128160606156274, + "timestamp": 1.3666873468665126 + }, + { + "x": 6.870540392297196, + "y": 1.5272354931118775, + "heading": 0.25752255617569925, + "angularVelocity": 6.324014114235091e-7, + "velocityX": 3.695345127266883, + "velocityY": 0.5063931192177723, + "timestamp": 1.4194081508010017 + }, + { + "x": 7.062280392097909, + "y": 1.5708688177166172, + "heading": 0.25752360307904704, + "angularVelocity": 0.000019857499690926623, + "velocityX": 3.6368944608464226, + "velocityY": 0.8276301070628356, + "timestamp": 1.4721289547354908 + }, + { + "x": 7.242656103418754, + "y": 1.6247622711713998, + "heading": 0.29034352591414114, + "angularVelocity": 0.622523186024936, + "velocityX": 3.4213384064662518, + "velocityY": 1.0222426335104984, + "timestamp": 1.5248497586699798 + }, + { + "x": 7.410676149226542, + "y": 1.6874049568089853, + "heading": 0.35686615036971664, + "angularVelocity": 1.2617907825957404, + "velocityX": 3.1869780668855143, + "velocityY": 1.188196707232031, + "timestamp": 1.577570562604469 + }, + { + "x": 7.5654443716076845, + "y": 1.7579385900879527, + "heading": 0.45500472309026196, + "angularVelocity": 1.8614771664425407, + "velocityX": 2.935619543538406, + "velocityY": 1.337870973413318, + "timestamp": 1.630291366538958 + }, + { + "x": 7.704030677677, + "y": 1.8336246154274285, + "heading": 0.5647056674911662, + "angularVelocity": 2.080790432126545, + "velocityX": 2.628683474582893, + "velocityY": 1.4356007437504827, + "timestamp": 1.683012170473447 + }, + { + "x": 7.825774553014182, + "y": 1.9127789189520943, + "heading": 0.6711461233913874, + "angularVelocity": 2.0189459939283965, + "velocityX": 2.3092188709500836, + "velocityY": 1.5013865043299246, + "timestamp": 1.7357329744079362 + }, + { + "x": 7.930703511686336, + "y": 1.9944877491725297, + "heading": 0.7666398660798261, + "angularVelocity": 1.811310442213658, + "velocityX": 1.9902761498580215, + "velocityY": 1.5498403689360842, + "timestamp": 1.7884537783424252 + }, + { + "x": 8.018956158365414, + "y": 2.0782214675210318, + "heading": 0.8466098380495345, + "angularVelocity": 1.5168579763897216, + "velocityX": 1.6739624606017243, + "velocityY": 1.5882481316587975, + "timestamp": 1.8411745822769143 + }, + { + "x": 8.090681062335408, + "y": 2.163657607085554, + "heading": 0.9079138555243396, + "angularVelocity": 1.1628050579612106, + "velocityX": 1.360466810390799, + "velocityY": 1.6205393922043627, + "timestamp": 1.8938953862114034 + }, + { + "x": 8.146013507746817, + "y": 2.250588744141006, + "heading": 0.9482458723141572, + "angularVelocity": 0.7650114144680767, + "velocityX": 1.0495372088818153, + "velocityY": 1.6488962718298632, + "timestamp": 1.9466161901458925 + }, + { + "x": 8.18506370691749, + "y": 2.3388711976421583, + "heading": 0.9659657399574388, + "angularVelocity": 0.33610769033985816, + "velocityX": 0.7406980974568496, + "velocityY": 1.6745278317616754, + "timestamp": 1.9993369940803816 + }, + { + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "angularVelocity": -0.11181263648563311, + "velocityX": 0.4333008617940415, + "velocityY": 1.6980515558692484, + "timestamp": 2.0520577980148706 + }, + { + "x": 8.211510391317606, + "y": 2.540296472438403, + "heading": 0.9167118198425396, + "angularVelocity": -0.6681402574181382, + "velocityX": 0.055515922817721344, + "velocityY": 1.7243602437412162, + "timestamp": 2.1169529567004024 + }, + { + "x": 8.190357670268208, + "y": 2.653506168063042, + "heading": 0.8402267432816896, + "angularVelocity": -1.1785944916396733, + "velocityX": -0.32595222013246916, + "velocityY": 1.7445014068496127, + "timestamp": 2.181848115385934 + }, + { + "x": 8.14421449138386, + "y": 2.767534412566691, + "heading": 0.7336533483609798, + "angularVelocity": -1.6422395303344925, + "velocityX": -0.711041930076003, + "velocityY": 1.7571148112327775, + "timestamp": 2.246743274071466 + }, + { + "x": 8.072849326580373, + "y": 2.8818576996728056, + "heading": 0.6001933866162775, + "angularVelocity": -2.056547274834806, + "velocityX": -1.0996993650837186, + "velocityY": 1.7616612613600555, + "timestamp": 2.3116384327569977 + }, + { + "x": 7.975969110351945, + "y": 2.995874279761309, + "heading": 0.444818066858235, + "angularVelocity": -2.3942513263733436, + "velocityX": -1.492872784207061, + "velocityY": 1.756935068777684, + "timestamp": 2.3765335914425294 + }, + { + "x": 7.853227135461901, + "y": 3.1086496791956706, + "heading": 0.2767628269059192, + "angularVelocity": -2.589642176031597, + "velocityX": -1.8913887780878373, + "velocityY": 1.7378091327405185, + "timestamp": 2.441428750128061 + }, + { + "x": 7.704583384688799, + "y": 3.218490261217407, + "heading": 0.11219782786286417, + "angularVelocity": -2.5358594134965102, + "velocityX": -2.290521416141349, + "velocityY": 1.6925851519063353, + "timestamp": 2.506323908813593 + }, + { + "x": 7.53264205332364, + "y": 3.322684523690155, + "heading": -0.008853804183353416, + "angularVelocity": -1.8653414907698829, + "velocityX": -2.6495247850205796, + "velocityY": 1.6055783602849627, + "timestamp": 2.5712190674991247 + }, + { + "x": 7.337826739220927, + "y": 3.421844874305526, + "heading": -0.08105196591494265, + "angularVelocity": -1.112535406245734, + "velocityX": -3.00200073547469, + "velocityY": 1.5280084466066417, + "timestamp": 2.6361142261846564 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": -0.09725228724922261, + "angularVelocity": -0.2496383653637915, + "velocityX": -3.3452440004715016, + "velocityY": 1.4644047234645439, + "timestamp": 2.7010093848701877 + }, + { + "x": 7.0106063205998925, + "y": 3.566708400606869, + "heading": -0.09725231991858449, + "angularVelocity": -0.0000010080544024334555, + "velocityX": -3.398208767574138, + "velocityY": 1.5375906788578377, + "timestamp": 2.733417716994889 + }, + { + "x": 6.900476113958648, + "y": 3.616539308522766, + "heading": -0.09725235258763598, + "angularVelocity": -0.000001008044825380236, + "velocityX": -3.398206554335633, + "velocityY": 1.5375955703044946, + "timestamp": 2.7658260491195903 + }, + { + "x": 6.790345907323517, + "y": 3.6663702164521736, + "heading": -0.09725238525668778, + "angularVelocity": -0.0000010080448348715788, + "velocityX": -3.3982065541469955, + "velocityY": 1.5375955707213955, + "timestamp": 2.7982343812442916 + }, + { + "x": 6.680215700688386, + "y": 3.7162011243815836, + "heading": -0.09725241792573983, + "angularVelocity": -0.000001008044842341437, + "velocityX": -3.3982065541469777, + "velocityY": 1.5375955707214333, + "timestamp": 2.830642713368993 + }, + { + "x": 6.570085494053255, + "y": 3.7660320323109926, + "heading": -0.09725245059479215, + "angularVelocity": -0.0000010080448508340198, + "velocityX": -3.398206554146978, + "velocityY": 1.5375955707214313, + "timestamp": 2.863051045493694 + }, + { + "x": 6.459955287418124, + "y": 3.8158629402404016, + "heading": -0.09725248326384477, + "angularVelocity": -0.0000010080448595181056, + "velocityX": -3.3982065541469764, + "velocityY": 1.5375955707214315, + "timestamp": 2.8954593776183954 + }, + { + "x": 6.349825080782993, + "y": 3.8656938481698107, + "heading": -0.09725251593289766, + "angularVelocity": -0.0000010080448688287921, + "velocityX": -3.3982065541469764, + "velocityY": 1.5375955707214306, + "timestamp": 2.9278677097430967 + }, + { + "x": 6.239694874147862, + "y": 3.91552475609922, + "heading": -0.09725254860195084, + "angularVelocity": -0.0000010080448772415697, + "velocityX": -3.3982065541469755, + "velocityY": 1.5375955707214306, + "timestamp": 2.960276041867798 + }, + { + "x": 6.129564667512731, + "y": 3.965355664028629, + "heading": -0.09725258127100428, + "angularVelocity": -0.0000010080448851885182, + "velocityX": -3.3982065541469755, + "velocityY": 1.5375955707214297, + "timestamp": 2.9926843739924993 + }, + { + "x": 6.0194344608776, + "y": 4.015186571958038, + "heading": -0.09725261394005799, + "angularVelocity": -0.0000010080448937492915, + "velocityX": -3.3982065541469746, + "velocityY": 1.5375955707214293, + "timestamp": 3.0250927061172006 + }, + { + "x": 5.9093042542424685, + "y": 4.065017479887447, + "heading": -0.09725264660911201, + "angularVelocity": -0.0000010080449031799624, + "velocityX": -3.398206554146974, + "velocityY": 1.5375955707214286, + "timestamp": 3.057501038241902 + }, + { + "x": 5.7991740476073375, + "y": 4.114848387816856, + "heading": -0.0972526792781663, + "angularVelocity": -0.000001008044911324413, + "velocityX": -3.3982065541469737, + "velocityY": 1.5375955707214282, + "timestamp": 3.089909370366603 + }, + { + "x": 5.689043840972206, + "y": 4.164679295746265, + "heading": -0.09725271194722086, + "angularVelocity": -0.0000010080449199147737, + "velocityX": -3.398206554146973, + "velocityY": 1.5375955707214277, + "timestamp": 3.1223177024913045 + }, + { + "x": 5.578913634337075, + "y": 4.214510203675674, + "heading": -0.09725274461627566, + "angularVelocity": -0.0000010080449273058883, + "velocityX": -3.398206554146972, + "velocityY": 1.5375955707214277, + "timestamp": 3.1547260346160058 + }, + { + "x": 5.468783427701944, + "y": 4.264341111605084, + "heading": -0.09725277728533077, + "angularVelocity": -0.000001008044937071081, + "velocityX": -3.3982065541469724, + "velocityY": 1.537595570721425, + "timestamp": 3.187134366740707 + }, + { + "x": 5.358653221066815, + "y": 4.314172019534495, + "heading": -0.09725280995438615, + "angularVelocity": -0.0000010080449450667608, + "velocityX": -3.3982065541469435, + "velocityY": 1.5375955707214868, + "timestamp": 3.2195426988654083 + }, + { + "x": 5.2485230144416954, + "y": 4.364002927486029, + "heading": -0.0972528426234418, + "angularVelocity": -0.0000010080449533993329, + "velocityX": -3.3982065538380732, + "velocityY": 1.5375955714041123, + "timestamp": 3.2519510309901096 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.0000010080480324849667, + "velocityX": -3.398202929897321, + "velocityY": 1.5376035805684636, + "timestamp": 3.284359363114811 + }, + { + "x": 4.91372823368964, + "y": 4.535640205255565, + "heading": -0.09725287529265084, + "angularVelocity": -7.461622400229302e-13, + "velocityX": -3.1281866300757337, + "velocityY": 1.6960041335008067, + "timestamp": 3.356178828556159 + }, + { + "x": 4.717146630995297, + "y": 4.64222057593122, + "heading": -0.0972528752926601, + "angularVelocity": -1.288593597138777e-13, + "velocityX": -2.7371632674554296, + "velocityY": 1.4840039538124186, + "timestamp": 3.4279982939975073 + }, + { + "x": 4.5486481153378495, + "y": 4.733575188699939, + "heading": -0.0972528752926535, + "angularVelocity": 9.166586601280667e-14, + "velocityX": -2.3461399304768347, + "velocityY": 1.2720035189251644, + "timestamp": 3.4998177594388555 + }, + { + "x": 4.408232686103436, + "y": 4.809704037452307, + "heading": -0.0972528752926406, + "angularVelocity": 1.7989350116390222e-13, + "velocityX": -1.955116602045504, + "velocityY": 1.0600029989716206, + "timestamp": 3.5716372248802037 + }, + { + "x": 4.295900342985128, + "y": 4.8706071191336155, + "heading": -0.09725287529262608, + "angularVelocity": 2.0200921674140544e-13, + "velocityX": -1.5640932778878103, + "velocityY": 0.8480024364849348, + "timestamp": 3.643456690321552 + }, + { + "x": 4.211651085798766, + "y": 4.916284431911041, + "heading": -0.09725287529261284, + "angularVelocity": 1.8429670420443223e-13, + "velocityX": -1.1730699562943, + "velocityY": 0.6360018484783654, + "timestamp": 3.7152761557629 + }, + { + "x": 4.155484914421578, + "y": 4.9467359745627, + "heading": -0.09725287529260278, + "angularVelocity": 1.401436382547871e-13, + "velocityX": -0.7820466364102463, + "velocityY": 0.4240012434585409, + "timestamp": 3.7870956212042484 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 7.711346668937439e-14, + "velocityX": -0.39102331774723287, + "velocityY": 0.21200062628639202, + "timestamp": 3.8589150866455966 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -5.652824755464892e-28, + "velocityX": 8.289180813930789e-28, + "velocityY": 1.3058854376408088e-27, + "timestamp": 3.9307345520869448 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGFH.3.traj b/src/main/deploy/choreo/SourceLanePGFH.3.traj new file mode 100644 index 00000000..85c9e2d2 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGFH.3.traj @@ -0,0 +1,446 @@ +{ + "samples": [ + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -5.652824755464892e-28, + "velocityX": 8.289180813930789e-28, + "velocityY": 1.3058854376408088e-27, + "timestamp": 0 + }, + { + "x": 4.149339902607208, + "y": 4.948719603159426, + "heading": -0.09725287534607682, + "angularVelocity": -8.314126318446111e-10, + "velocityX": 0.3410571082022834, + "velocityY": -0.20586707155231002, + "timestamp": 0.06432375491886066 + }, + { + "x": 4.1933171531744176, + "y": 4.922404277066319, + "heading": -0.09725287544407178, + "angularVelocity": -1.5234644431523042e-9, + "velocityX": 0.6836859978507643, + "velocityY": -0.4091074304710872, + "timestamp": 0.12864750983772133 + }, + { + "x": 4.259461121252347, + "y": 4.8832332111646055, + "heading": -0.0972528755751186, + "angularVelocity": -2.0373002769674516e-9, + "velocityX": 1.0282976819584715, + "velocityY": -0.6089673395330295, + "timestamp": 0.192971264756582 + }, + { + "x": 4.34793757416226, + "y": 4.831496576688514, + "heading": -0.09725287572404445, + "angularVelocity": -2.3152544478823804e-9, + "velocityX": 1.375486443873169, + "velocityY": -0.8043161432561301, + "timestamp": 0.25729501967544266 + }, + { + "x": 4.458970362431496, + "y": 4.767600785112478, + "heading": -0.09725287586983122, + "angularVelocity": -2.2664532339146123e-9, + "velocityX": 1.7261552658002488, + "velocityY": -0.9933467294724277, + "timestamp": 0.3216187745943033 + }, + { + "x": 4.59287734823679, + "y": 4.692154910649705, + "heading": -0.09725287598146638, + "angularVelocity": -1.7355199749767777e-9, + "velocityX": 2.081765686319258, + "velocityY": -1.1729084310756062, + "timestamp": 0.385942529513164 + }, + { + "x": 4.750141155397852, + "y": 4.606169009566351, + "heading": -0.09725287600872443, + "angularVelocity": -4.2376330088015544e-10, + "velocityX": 2.444879148604377, + "velocityY": -1.3367674382787311, + "timestamp": 0.45026628443202465 + }, + { + "x": 4.93155755546803, + "y": 4.511622419087021, + "heading": -0.0972528758570734, + "angularVelocity": 2.3576208162700097e-9, + "velocityX": 2.8203639588363623, + "velocityY": -1.469854964135602, + "timestamp": 0.5145900393508853 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 8.775547292964994e-9, + "velocityX": 3.2155363139998, + "velocityY": -1.5202521091803838, + "timestamp": 0.578913794269746 + }, + { + "x": 5.399102105862753, + "y": 4.325847836745161, + "heading": -0.09725287310436229, + "angularVelocity": 2.9662640662901703e-8, + "velocityX": 3.534045888591934, + "velocityY": -1.1926985982098404, + "timestamp": 0.6526845343277303 + }, + { + "x": 5.668612776819124, + "y": 4.270396424489206, + "heading": -0.09725287179514892, + "angularVelocity": 1.7747054810662862e-8, + "velocityX": 3.653354578584066, + "velocityY": -0.7516721699195277, + "timestamp": 0.7264552743857147 + }, + { + "x": 5.942881070814964, + "y": 4.248311045956059, + "heading": -0.09725287053637834, + "angularVelocity": 1.706327712659208e-8, + "velocityX": 3.7178465849775284, + "velocityY": -0.2993785681936777, + "timestamp": 0.800226014443699 + }, + { + "x": 6.217467173732042, + "y": 4.230609827633618, + "heading": -0.09725286928029948, + "angularVelocity": 1.702678935422503e-8, + "velocityX": 3.722154646967784, + "velocityY": -0.23994904088704855, + "timestamp": 0.8739967545016833 + }, + { + "x": 6.492053278688794, + "y": 4.212908640951192, + "heading": -0.09725286802422056, + "angularVelocity": 1.702679012699192e-8, + "velocityX": 3.722154674616614, + "velocityY": -0.23994861199050874, + "timestamp": 0.9477674945596677 + }, + { + "x": 6.7666393836455585, + "y": 4.195207454268953, + "heading": -0.09725286676814171, + "angularVelocity": 1.70267893622157e-8, + "velocityX": 3.7221546746167777, + "velocityY": -0.23994861198797246, + "timestamp": 1.021538234617652 + }, + { + "x": 7.041225488263755, + "y": 4.177506262334816, + "heading": -0.09725286551205693, + "angularVelocity": 1.702686968579546e-8, + "velocityX": 3.7221546700273103, + "velocityY": -0.23994868318012236, + "timestamp": 1.0953089746756364 + }, + { + "x": 7.304754521125749, + "y": 4.159872953315235, + "heading": -0.07493632738083242, + "angularVelocity": 0.30251205442271084, + "velocityX": 3.572270423949385, + "velocityY": -0.23902849565725026, + "timestamp": 1.1690797147336207 + }, + { + "x": 7.534656768824059, + "y": 4.1444074082490925, + "heading": -0.055522130294338126, + "angularVelocity": 0.26316934154699656, + "velocityX": 3.116442203475328, + "velocityY": -0.2096433498428548, + "timestamp": 1.242850454791605 + }, + { + "x": 7.730932226657292, + "y": 4.131109655486069, + "heading": -0.03901051064994992, + "angularVelocity": 0.22382342418430445, + "velocityX": 2.660613919271525, + "velocityY": -0.18025781973409782, + "timestamp": 1.3166211948495894 + }, + { + "x": 7.89358089446588, + "y": 4.119979717938594, + "heading": -0.025401753521899054, + "angularVelocity": 0.18447364249503723, + "velocityX": 2.2047856329046707, + "velocityY": -0.1508719790356963, + "timestamp": 1.3903919349075737 + }, + { + "x": 8.022602772932235, + "y": 4.1110176135114935, + "heading": -0.014696144661245043, + "angularVelocity": 0.14511998730444314, + "velocityX": 1.7489573557882623, + "velocityY": -0.12148589562821704, + "timestamp": 1.464162674965558 + }, + { + "x": 8.117997862710347, + "y": 4.104223355212346, + "heading": -0.006893914960978184, + "angularVelocity": 0.10576320224162433, + "velocityX": 1.2931290875370185, + "velocityY": -0.09209963589639363, + "timestamp": 1.5379334150235424 + }, + { + "x": 8.179766164065947, + "y": 4.099596951205949, + "heading": -0.001995197471495412, + "angularVelocity": 0.06640461361282764, + "velocityX": 0.8373008228879115, + "velocityY": -0.0627132654865662, + "timestamp": 1.6117041550815268 + }, + { + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": -7.002250121980633e-28, + "angularVelocity": 0.027045919153409353, + "velocityX": 0.38147255414152553, + "velocityY": -0.0333268496130705, + "timestamp": 1.685474895139511 + }, + { + "x": 8.200635459356112, + "y": 4.096934551280871, + "heading": -0.0010816052785507836, + "angularVelocity": -0.014043494340470785, + "velocityX": -0.09442200874161932, + "velocityY": -0.0026468217635775776, + "timestamp": 1.7624931394830003 + }, + { + "x": 8.156710678515955, + "y": 4.0990936119074455, + "heading": -0.0053278404451381055, + "angularVelocity": -0.055132848103493455, + "velocityX": -0.5703165686854601, + "velocityY": 0.028033106246169795, + "timestamp": 1.8395113838264896 + }, + { + "x": 8.076133333676754, + "y": 4.103615573325919, + "heading": -0.012738567096053588, + "angularVelocity": -0.0962204048415442, + "velocityX": -1.0462111351154568, + "velocityY": 0.05871286027121356, + "timestamp": 1.9165296281699788 + }, + { + "x": 7.958903423782304, + "y": 4.110500416395447, + "heading": -0.023313558262368205, + "angularVelocity": -0.13730501462941505, + "velocityX": -1.522105715259157, + "velocityY": 0.08939236577274852, + "timestamp": 1.993547872513468 + }, + { + "x": 7.8050209475058985, + "y": 4.1197481162013645, + "heading": -0.03705256536677336, + "angularVelocity": -0.17838639690527597, + "velocityX": -1.9980003126287051, + "velocityY": 0.12007154778384964, + "timestamp": 2.0705661168569574 + }, + { + "x": 7.614485903743769, + "y": 4.131358642030129, + "heading": -0.05395540666928083, + "angularVelocity": -0.21946541948065604, + "velocityX": -2.4738949243295387, + "velocityY": 0.15075033101226304, + "timestamp": 2.1475843612004466 + }, + { + "x": 7.38729829267515, + "y": 4.145331957366106, + "heading": -0.07402207604262086, + "angularVelocity": -0.2605443624999541, + "velocityX": -2.9497895337031945, + "velocityY": 0.18142864012399953, + "timestamp": 2.224602605543936 + }, + { + "x": 7.123458119903768, + "y": 4.1616680199277996, + "heading": -0.09725286481798223, + "angularVelocity": -0.30162708814493094, + "velocityX": -3.425684070318428, + "velocityY": 0.21210640025547228, + "timestamp": 2.301620849887425 + }, + { + "x": 6.836784301080243, + "y": 4.180148431298723, + "heading": -0.0972528661294337, + "angularVelocity": -1.7027802603385206e-8, + "velocityX": -3.722154682532179, + "velocityY": 0.2399484891982639, + "timestamp": 2.3786390942309144 + }, + { + "x": 6.550110482735962, + "y": 4.198628850104045, + "heading": -0.09725286744087382, + "angularVelocity": -1.7027655300024213e-8, + "velocityX": -3.722154676309698, + "velocityY": 0.23994858572603794, + "timestamp": 2.4556573385744036 + }, + { + "x": 6.263436664391698, + "y": 4.217109268909628, + "heading": -0.09725286875231377, + "angularVelocity": -1.7027653194201686e-8, + "velocityX": -3.7221546763094806, + "velocityY": 0.23994858572941535, + "timestamp": 2.532675582917893 + }, + { + "x": 5.976762848146004, + "y": 4.235589720268832, + "heading": -0.09725287006375381, + "angularVelocity": -1.702765423071752e-8, + "velocityX": -3.722154649061779, + "velocityY": 0.23994900840357883, + "timestamp": 2.609693827261382 + }, + { + "x": 5.690447961543492, + "y": 4.258981806583179, + "heading": -0.09725287137836151, + "angularVelocity": -1.7068782885666804e-8, + "velocityX": -3.717494329338312, + "velocityY": 0.30372136516150916, + "timestamp": 2.6867120716048714 + }, + { + "x": 5.409459505180122, + "y": 4.318722004074591, + "heading": -0.09725287275094441, + "angularVelocity": -1.7821529284929368e-8, + "velocityX": -3.6483362969195534, + "velocityY": 0.7756629354595511, + "timestamp": 2.7637303159483597 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -3.300065929591643e-8, + "velocityX": -3.519511282401575, + "velocityY": 1.2349293565099368, + "timestamp": 2.840748560291848 + }, + { + "x": 4.908992840849997, + "y": 4.524204173518216, + "heading": -0.09725287595344081, + "angularVelocity": -9.191205906949272e-9, + "velocityX": -3.1905635663992595, + "velocityY": 1.535059379942558, + "timestamp": 2.91264811021097 + }, + { + "x": 4.7110423526588185, + "y": 4.628577138266545, + "heading": -0.097252876094701, + "angularVelocity": -1.964688046810948e-9, + "velocityX": -2.7531533704153888, + "velocityY": 1.4516497650644007, + "timestamp": 2.984547660130092 + }, + { + "x": 4.542899034827415, + "y": 4.721263554864837, + "heading": -0.09725287602386487, + "angularVelocity": 9.85209742042839e-10, + "velocityX": -2.338586514387707, + "velocityY": 1.2891098303473851, + "timestamp": 3.056447210049214 + }, + { + "x": 4.403657392505783, + "y": 4.800175498734494, + "heading": -0.09725287586251803, + "angularVelocity": 2.2440590575017265e-9, + "velocityX": -1.936613545957686, + "velocityY": 1.0975304290280408, + "timestamp": 3.128346759968336 + }, + { + "x": 4.29278499065898, + "y": 4.864247320916007, + "heading": -0.09725287567494544, + "angularVelocity": 2.608814644263043e-9, + "velocityX": -1.5420458399464478, + "velocityY": 0.8911296698461443, + "timestamp": 3.200246309887458 + }, + { + "x": 4.209935704536306, + "y": 4.912835343646817, + "heading": -0.09725287550078096, + "angularVelocity": 2.4223304424020015e-9, + "velocityX": -1.152292138349535, + "velocityY": 0.6757764518062407, + "timestamp": 3.27214585980658 + }, + { + "x": 4.1548674777952215, + "y": 4.945509352761423, + "heading": -0.09725287536687216, + "angularVelocity": 1.862443038330612e-9, + "velocityX": -0.7659050272641558, + "velocityY": 0.4544396891407544, + "timestamp": 3.3440454097257017 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.0330372741830232e-9, + "velocityX": -0.3820002915212732, + "velocityY": 0.2288247071491247, + "timestamp": 3.4159449596448237 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -3.6520722228130864e-26, + "velocityX": -9.590252743264302e-26, + "velocityY": -2.0342869025815665e-25, + "timestamp": 3.4878445095639456 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGFH.4.traj b/src/main/deploy/choreo/SourceLanePGFH.4.traj new file mode 100644 index 00000000..a964f716 --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGFH.4.traj @@ -0,0 +1,752 @@ +{ + "samples": [ + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -3.6520722228130864e-26, + "velocityX": -9.590252743264302e-26, + "velocityY": -2.0342869025815665e-25, + "timestamp": 0 + }, + { + "x": 4.149907885471957, + "y": 4.949853513181501, + "heading": -0.09710925579665566, + "angularVelocity": 0.0022357452830962274, + "velocityX": 0.35035501128758395, + "velocityY": -0.18849059952223363, + "timestamp": 0.06423786154328504 + }, + { + "x": 4.194913909182307, + "y": 4.925625720140559, + "heading": -0.09684438286421601, + "angularVelocity": 0.004123314912361655, + "velocityX": 0.7006152233138045, + "velocityY": -0.3771575276461672, + "timestamp": 0.12847572308657007 + }, + { + "x": 4.262412058477699, + "y": 4.889263805728157, + "heading": -0.09648702221681861, + "angularVelocity": 0.0055630844304586, + "velocityX": 1.0507533668428728, + "velocityY": -0.5660511346240961, + "timestamp": 0.1927135846298551 + }, + { + "x": 4.3523918574804155, + "y": 4.840748358129665, + "heading": -0.09607554613145339, + "angularVelocity": 0.006405507211474483, + "velocityX": 1.4007284308816241, + "velocityY": -0.755246928103313, + "timestamp": 0.25695144617314014 + }, + { + "x": 4.464838600314873, + "y": 4.78005220731639, + "heading": -0.09566371008606342, + "angularVelocity": 0.006411110760784919, + "velocityX": 1.7504745664469084, + "velocityY": -0.9448656813143913, + "timestamp": 0.3211893077164252 + }, + { + "x": 4.599730141458809, + "y": 4.7071346128101945, + "heading": -0.09533221857083674, + "angularVelocity": 0.005160375941271345, + "velocityX": 2.0998759594922722, + "velocityY": -1.1351186473893293, + "timestamp": 0.3854271692597102 + }, + { + "x": 4.757029340577212, + "y": 4.621927713991256, + "heading": -0.09521575794797991, + "angularVelocity": 0.0018129592121981597, + "velocityX": 2.4486991836179413, + "velocityY": -1.3264280094617478, + "timestamp": 0.44966503080299525 + }, + { + "x": 4.936661051016691, + "y": 4.524295949358237, + "heading": -0.09558433952634979, + "angularVelocity": -0.005737762271577307, + "velocityX": 2.7963525890169896, + "velocityY": -1.519847676860057, + "timestamp": 0.5139028923462803 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.025974335480068365, + "velocityX": 3.1403890073431286, + "velocityY": -1.7195755229583027, + "timestamp": 0.5781407538895653 + }, + { + "x": 5.282623687046255, + "y": 4.319887903045294, + "heading": -0.12350374979021995, + "angularVelocity": -0.54152818219314, + "velocityX": 2.9753302981273193, + "velocityY": -1.9380120292170788, + "timestamp": 0.626616300243052 + }, + { + "x": 5.415965984922444, + "y": 4.230982927805077, + "heading": -0.18642712240012974, + "angularVelocity": -1.298043598128203, + "velocityX": 2.750712635683371, + "velocityY": -1.834016982334084, + "timestamp": 0.6750918465965388 + }, + { + "x": 5.544046179661802, + "y": 4.15236621254376, + "heading": -0.28429132793206696, + "angularVelocity": -2.0188365659317435, + "velocityX": 2.642160932140699, + "velocityY": -1.6217809014062547, + "timestamp": 0.7235673929500255 + }, + { + "x": 5.6705798620807135, + "y": 4.085599938426855, + "heading": -0.4169505782108561, + "angularVelocity": -2.736622075622024, + "velocityX": 2.610257994746929, + "velocityY": -1.3773186511409734, + "timestamp": 0.7720429393035122 + }, + { + "x": 5.804006769424575, + "y": 4.008650582245615, + "heading": -0.5169463778011086, + "angularVelocity": -2.06280912980491, + "velocityX": 2.7524580408213564, + "velocityY": -1.5873850213078682, + "timestamp": 0.8205184856569989 + }, + { + "x": 5.938212060885805, + "y": 3.9187237193410076, + "heading": -0.5903545372750207, + "angularVelocity": -1.5143338238751507, + "velocityX": 2.7685152939298026, + "velocityY": -1.8550974598379135, + "timestamp": 0.8689940320104856 + }, + { + "x": 6.076913949313874, + "y": 3.818161785200274, + "heading": -0.6273225494125774, + "angularVelocity": -0.7626115622913053, + "velocityX": 2.8612754029969545, + "velocityY": -2.07448789555523, + "timestamp": 0.9174695783639724 + }, + { + "x": 6.214509922576719, + "y": 3.712335803608528, + "heading": -0.6556184225519457, + "angularVelocity": -0.5837143728723201, + "velocityX": 2.838461525724493, + "velocityY": -2.1830797082730613, + "timestamp": 0.9659451247174591 + }, + { + "x": 6.348284689884717, + "y": 3.603969355784692, + "heading": -0.6891441796191574, + "angularVelocity": -0.6916014277124424, + "velocityX": 2.7596340293414254, + "velocityY": -2.2354868789641342, + "timestamp": 1.0144206710709458 + }, + { + "x": 6.474985027954153, + "y": 3.4904345524521627, + "heading": -0.730650696928258, + "angularVelocity": -0.8562361939447342, + "velocityX": 2.613695927128511, + "velocityY": -2.342104666642176, + "timestamp": 1.0628962174244325 + }, + { + "x": 6.594663143157959, + "y": 3.3646228313446045, + "heading": -0.7575105881170743, + "angularVelocity": -0.554091561814539, + "velocityX": 2.468834787979625, + "velocityY": -2.595364685322631, + "timestamp": 1.1113717637779192 + }, + { + "x": 6.674801371049327, + "y": 3.2699580042827607, + "heading": -0.7721877645618775, + "angularVelocity": -0.4274597319602108, + "velocityX": 2.333954731895263, + "velocityY": -2.757029034439579, + "timestamp": 1.1457075744927625 + }, + { + "x": 6.750751589732745, + "y": 3.1695424250982125, + "heading": -0.7798246346604031, + "angularVelocity": -0.2224170607750979, + "velocityX": 2.211982682284112, + "velocityY": -2.9245145838697275, + "timestamp": 1.1800433852076058 + }, + { + "x": 6.822476452924599, + "y": 3.063443372222415, + "heading": -0.7798255890957266, + "angularVelocity": -0.000027797081344724893, + "velocityX": 2.0889229553228312, + "velocityY": -3.0900407087208346, + "timestamp": 1.214379195922449 + }, + { + "x": 6.888038953713801, + "y": 2.9534294027046353, + "heading": -0.779825865993983, + "angularVelocity": -0.000008064415854472034, + "velocityX": 1.9094496219615917, + "velocityY": -3.2040591798294837, + "timestamp": 1.2487150066372923 + }, + { + "x": 6.951397199301839, + "y": 2.842131382226383, + "heading": -0.7798261188046057, + "angularVelocity": -0.0000073628849152658005, + "velocityX": 1.8452526464053884, + "velocityY": -3.241456023933041, + "timestamp": 1.2830508173521356 + }, + { + "x": 7.014755263905195, + "y": 2.730833258719293, + "heading": -0.7798263716136229, + "angularVelocity": -0.000007362838153289018, + "velocityX": 1.8452473753871865, + "velocityY": -3.2414590245564723, + "timestamp": 1.3173866280669788 + }, + { + "x": 7.078113328495587, + "y": 2.6195351352048375, + "heading": -0.7798266244227116, + "angularVelocity": -0.000007362840235334639, + "velocityX": 1.8452473750096865, + "velocityY": -3.241459024770976, + "timestamp": 1.351722438781822 + }, + { + "x": 7.1414713930859905, + "y": 2.5082370116904005, + "heading": -0.7798268772318718, + "angularVelocity": -0.000007362842320190408, + "velocityX": 1.84524737500996, + "velocityY": -3.241459024770425, + "timestamp": 1.3860582494966653 + }, + { + "x": 7.204829457676403, + "y": 2.396938888175983, + "heading": -0.7798271300411037, + "angularVelocity": -0.000007362844406233281, + "velocityX": 1.845247375010246, + "velocityY": -3.2414590247698674, + "timestamp": 1.4203940602115086 + }, + { + "x": 7.2681875222668415, + "y": 2.285640764661595, + "heading": -0.7798273828504071, + "angularVelocity": -0.0000073628464912696794, + "velocityX": 1.845247375011028, + "velocityY": -3.2414590247690276, + "timestamp": 1.4547298709263519 + }, + { + "x": 7.3315455870489, + "y": 2.1743426412563025, + "heading": -0.7798276356597839, + "angularVelocity": -0.000007362848626587696, + "velocityX": 1.8452473805917786, + "velocityY": -3.241459021591694, + "timestamp": 1.4890656816411951 + }, + { + "x": 7.394906330223783, + "y": 2.0630460426228225, + "heading": -0.7798278885161463, + "angularVelocity": -0.0000073642170417804714, + "velocityX": 1.8453253864046084, + "velocityY": -3.241414613966507, + "timestamp": 1.5234014923560384 + }, + { + "x": 7.462756763097692, + "y": 1.9565771896637063, + "heading": -0.7859464592787083, + "angularVelocity": -0.1781979407265627, + "velocityX": 1.976083612453597, + "velocityY": -3.100810807798736, + "timestamp": 1.5577373030708817 + }, + { + "x": 7.530997298306203, + "y": 1.8560643499724379, + "heading": -0.8086349640155738, + "angularVelocity": -0.6607825551373272, + "velocityX": 1.9874449965735268, + "velocityY": -2.9273472097694815, + "timestamp": 1.592073113785725 + }, + { + "x": 7.600392480009413, + "y": 1.761540500711219, + "heading": -0.8474765480671682, + "angularVelocity": -1.1312266477169204, + "velocityX": 2.021073050510832, + "velocityY": -2.7529231811718247, + "timestamp": 1.6264089245005682 + }, + { + "x": 7.671333460317134, + "y": 1.6729216107462341, + "heading": -0.9025012077772409, + "angularVelocity": -1.6025443571741933, + "velocityX": 2.0660930623389726, + "velocityY": -2.580946484734556, + "timestamp": 1.6607447352154114 + }, + { + "x": 7.744622230529785, + "y": 1.5909570455551147, + "heading": -0.9652518160942604, + "angularVelocity": -1.827555750413467, + "velocityX": 2.134470358696664, + "velocityY": -2.387145184129499, + "timestamp": 1.6950805459302547 + }, + { + "x": 7.860586157347494, + "y": 1.4682098558928158, + "heading": -0.9995269320627294, + "angularVelocity": -0.6040459024385552, + "velocityX": 2.0436848379845327, + "velocityY": -2.1632293533176865, + "timestamp": 1.7518231143842407 + }, + { + "x": 7.962380466980202, + "y": 1.3584906150903546, + "heading": -1.012418619935978, + "angularVelocity": -0.22719605799485526, + "velocityX": 1.7939672525620034, + "velocityY": -1.933631906201723, + "timestamp": 1.8085656828382266 + }, + { + "x": 8.04779389711619, + "y": 1.259831390089905, + "heading": -1.014642889543101, + "angularVelocity": -0.0391993113411274, + "velocityX": 1.505279589260271, + "velocityY": -1.7387162352450838, + "timestamp": 1.8653082512922126 + }, + { + "x": 8.116074558332897, + "y": 1.171249663466821, + "heading": -1.0104296347158561, + "angularVelocity": 0.07425209929757598, + "velocityX": 1.2033410379031306, + "velocityY": -1.5611159141468582, + "timestamp": 1.9220508197461985 + }, + { + "x": 8.166855716911636, + "y": 1.0921738551192879, + "heading": -1.002112257895268, + "angularVelocity": 0.14658090120353398, + "velocityX": 0.8949393720151817, + "velocityY": -1.3935888082270873, + "timestamp": 1.9787933882001845 + }, + { + "x": 8.199923294477061, + "y": 1.022232267079584, + "heading": -0.9911733302853878, + "angularVelocity": 0.19278167886867675, + "velocityX": 0.582764905896743, + "velocityY": -1.2326123040486314, + "timestamp": 2.0355359566541704 + }, + { + "x": 8.215137995862136, + "y": 0.9611644433512547, + "heading": -0.978636960554262, + "angularVelocity": 0.22093412534351606, + "velocityX": 0.2681355779200118, + "velocityY": -1.0762259339362041, + "timestamp": 2.0922785251081564 + }, + { + "x": 8.21240234375, + "y": 0.908777952194214, + "heading": -0.9652518160942604, + "angularVelocity": 0.23589246706123476, + "velocityX": -0.04821163698918966, + "velocityY": -0.9232308755907478, + "timestamp": 2.1490210935621423 + }, + { + "x": 8.1921938424561, + "y": 0.8654303279065881, + "heading": -0.9517870451764023, + "angularVelocity": 0.24072872246907334, + "velocityX": -0.3612959128063065, + "velocityY": -0.7749866878900871, + "timestamp": 2.204954472760745 + }, + { + "x": 8.15448402268934, + "y": 0.8303967725236208, + "heading": -0.938073387974666, + "angularVelocity": 0.24517841400289994, + "velocityX": -0.6741916956753717, + "velocityY": -0.6263443382988405, + "timestamp": 2.260887851959348 + }, + { + "x": 8.099285775198217, + "y": 0.8037044120128763, + "heading": -0.9241370640104126, + "angularVelocity": 0.2491593421303856, + "velocityX": -0.9868570124313375, + "velocityY": -0.47721701948254364, + "timestamp": 2.3168212311579506 + }, + { + "x": 8.026615220367114, + "y": 0.7853870096388412, + "heading": -0.9100105816047303, + "angularVelocity": 0.25255907309879116, + "velocityX": -1.299234122313104, + "velocityY": -0.3274860671120066, + "timestamp": 2.3727546103565533 + }, + { + "x": 7.93649309479659, + "y": 0.7754877410206693, + "heading": -0.895735376567552, + "angularVelocity": 0.25521799758407426, + "velocityX": -1.6112404947058143, + "velocityY": -0.1769832032322341, + "timestamp": 2.428687989555156 + }, + { + "x": 7.828947063371523, + "y": 0.7740637661410563, + "heading": -0.8813661690986453, + "angularVelocity": 0.2568986117911059, + "velocityX": -1.9227522628876286, + "velocityY": -0.025458409629728278, + "timestamp": 2.484621368753759 + }, + { + "x": 7.7040158825175595, + "y": 0.7811943329260619, + "heading": -0.8669787071780914, + "angularVelocity": 0.2572249723991828, + "velocityX": -2.2335711277226653, + "velocityY": 0.1274832110480382, + "timestamp": 2.5405547479523616 + }, + { + "x": 7.561757726851445, + "y": 0.7969966352924417, + "heading": -0.8526849753347752, + "angularVelocity": 0.2555492274579613, + "velocityX": -2.5433499227893406, + "velocityY": 0.2825200728579338, + "timestamp": 2.5964881271509643 + }, + { + "x": 7.402269597446514, + "y": 0.821661602526662, + "heading": -0.8386677065817341, + "angularVelocity": 0.2506065064166751, + "velocityX": -2.8513944926987937, + "velocityY": 0.4409704471214242, + "timestamp": 2.652421506349567 + }, + { + "x": 7.2257452662755925, + "y": 0.8555551558075168, + "heading": -0.8252789625517808, + "angularVelocity": 0.23936948244113093, + "velocityX": -3.1559747274366425, + "velocityY": 0.605962910992889, + "timestamp": 2.70835488554817 + }, + { + "x": 7.032757795836431, + "y": 0.8996580788692503, + "heading": -0.813478587431489, + "angularVelocity": 0.21097196860558187, + "velocityX": -3.4503095147160385, + "velocityY": 0.7884902305855099, + "timestamp": 2.7642882647467726 + }, + { + "x": 6.833636314284489, + "y": 0.9619072197023563, + "heading": -0.813478584540231, + "angularVelocity": 5.169110280772625e-8, + "velocityX": -3.5599758928370466, + "velocityY": 1.112915788836524, + "timestamp": 2.8202216439453753 + }, + { + "x": 6.641148526278714, + "y": 1.0423607611427537, + "heading": -0.8134785841610236, + "angularVelocity": 6.7796258946599015e-9, + "velocityX": -3.441375986283731, + "velocityY": 1.438381563801643, + "timestamp": 2.876155023143978 + }, + { + "x": 6.45145754132324, + "y": 1.1292034413279326, + "heading": -0.8134785837843238, + "angularVelocity": 6.734793245125226e-9, + "velocityX": -3.3913735889608208, + "velocityY": 1.5526092188499014, + "timestamp": 2.932088402342581 + }, + { + "x": 6.261766612851187, + "y": 1.21604624489008, + "heading": -0.8134785834076242, + "angularVelocity": 6.734791446466924e-9, + "velocityX": -3.3913725791269433, + "velocityY": 1.5526114246341831, + "timestamp": 2.9880217815411836 + }, + { + "x": 6.072075684380091, + "y": 1.302889048454317, + "heading": -0.8134785830309245, + "angularVelocity": 6.734793280802982e-9, + "velocityX": -3.3913725791098397, + "velocityY": 1.552611424671542, + "timestamp": 3.0439551607397863 + }, + { + "x": 5.882384755908996, + "y": 1.3897318520185546, + "heading": -0.8134785826542248, + "angularVelocity": 6.734792910911008e-9, + "velocityX": -3.3913725791098357, + "velocityY": 1.552611424671551, + "timestamp": 3.099888539938389 + }, + { + "x": 5.692693827449727, + "y": 1.476574655608625, + "heading": -0.8134785822775251, + "angularVelocity": 6.734793219804119e-9, + "velocityX": -3.3913725788983964, + "velocityY": 1.5526114251333973, + "timestamp": 3.155821919136992 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.8134785819008252, + "angularVelocity": 6.73479370214728e-9, + "velocityX": -3.391360094956347, + "velocityY": 1.5526386935458116, + "timestamp": 3.2117552983355946 + }, + { + "x": 5.306759747272106, + "y": 1.6761609741986419, + "heading": -0.8134785815780706, + "angularVelocity": 5.31909195889925e-9, + "velocityX": -3.234156424503322, + "velocityY": 1.8580211843552186, + "timestamp": 3.272433825972845 + }, + { + "x": 5.11051667299417, + "y": 1.788904314208696, + "heading": -0.8134785812553164, + "angularVelocity": 5.3190855034609695e-9, + "velocityX": -3.2341436405827086, + "velocityY": 1.8580434364534788, + "timestamp": 3.3331123536100957 + }, + { + "x": 4.914273598725787, + "y": 1.9016476542353784, + "heading": -0.8134785809325621, + "angularVelocity": 5.319086694543979e-9, + "velocityX": -3.2341436404252732, + "velocityY": 1.8580434367275134, + "timestamp": 3.3937908812473463 + }, + { + "x": 4.718030524457404, + "y": 2.0143909942620604, + "heading": -0.8134785806098077, + "angularVelocity": 5.319085466039386e-9, + "velocityX": -3.234143640425272, + "velocityY": 1.8580434367275172, + "timestamp": 3.454469408884597 + }, + { + "x": 4.521787450189022, + "y": 2.1271343342887428, + "heading": -0.8134785802870533, + "angularVelocity": 5.319086975081976e-9, + "velocityX": -3.2341436404252715, + "velocityY": 1.8580434367275167, + "timestamp": 3.5151479365218474 + }, + { + "x": 4.325544375920639, + "y": 2.239877674315425, + "heading": -0.813478579964299, + "angularVelocity": 5.319087023675505e-9, + "velocityX": -3.2341436404252715, + "velocityY": 1.8580434367275163, + "timestamp": 3.575826464159098 + }, + { + "x": 4.129301301652256, + "y": 2.3526210143421062, + "heading": -0.8134785796415447, + "angularVelocity": 5.319086160384219e-9, + "velocityX": -3.2341436404252857, + "velocityY": 1.8580434367274927, + "timestamp": 3.6365049917963486 + }, + { + "x": 3.9330582273170225, + "y": 2.465364354252196, + "heading": -0.8134785793187616, + "angularVelocity": 5.3195605243469425e-9, + "velocityX": -3.234143641526992, + "velocityY": 1.858043434806029, + "timestamp": 3.697183519433599 + }, + { + "x": 3.755112012570184, + "y": 2.567595773148683, + "heading": -0.8087823717593017, + "angularVelocity": 0.07739488320373908, + "velocityX": -2.9326060086797106, + "velocityY": 1.684803881657269, + "timestamp": 3.7578620470708497 + }, + { + "x": 3.596937591266501, + "y": 2.6584681509636754, + "heading": -0.8046081994450863, + "angularVelocity": 0.06879158866822643, + "velocityX": -2.6067610316664966, + "velocityY": 1.4976035403865926, + "timestamp": 3.8185405747081003 + }, + { + "x": 3.4585349694595866, + "y": 2.737981483942003, + "heading": -0.8009559747359442, + "angularVelocity": 0.06018973846854076, + "velocityX": -2.2809159548879583, + "velocityY": 1.3104031372296205, + "timestamp": 3.879219102345351 + }, + { + "x": 3.339904149133476, + "y": 2.8061357707001306, + "heading": -0.7978256145396123, + "angularVelocity": 0.051589257653807806, + "velocityX": -1.9550708454119348, + "velocityY": 1.1232027112715945, + "timestamp": 3.9398976299826014 + }, + { + "x": 3.2410451312582933, + "y": 2.862931010476089, + "heading": -0.7952170508581512, + "angularVelocity": 0.04298989746513998, + "velocityX": -1.6292257199479807, + "velocityY": 0.9360022727560687, + "timestamp": 4.000576157619852 + }, + { + "x": 3.1619579164016844, + "y": 2.908367202779582, + "heading": -0.7931302326117163, + "angularVelocity": 0.03439137908734864, + "velocityX": -1.30338058512905, + "velocityY": 0.748801826160328, + "timestamp": 4.0612546852571025 + }, + { + "x": 3.1026425049328332, + "y": 2.9424443472754236, + "heading": -0.7915651260524916, + "angularVelocity": 0.02579341688350326, + "velocityX": -0.9775354442258696, + "velocityY": 0.5616013740406165, + "timestamp": 4.121933212894353 + }, + { + "x": 3.0630988971100095, + "y": 2.9651624437336137, + "heading": -0.7905217148332285, + "angularVelocity": 0.017195724087124466, + "velocityX": -0.6516902990663221, + "velocityY": 0.37440091813044435, + "timestamp": 4.182611740531604 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 0.008598014051155055, + "velocityX": -0.3258451507561328, + "velocityY": 0.18720045975221852, + "timestamp": 4.243290268168854 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -9.910581864004145e-25, + "velocityX": -2.631400747419988e-26, + "velocityY": 1.5241001239999535e-26, + "timestamp": 4.303968795806105 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePGFH.traj b/src/main/deploy/choreo/SourceLanePGFH.traj new file mode 100644 index 00000000..d3a19e9a --- /dev/null +++ b/src/main/deploy/choreo/SourceLanePGFH.traj @@ -0,0 +1,2066 @@ +{ + "samples": [ + { + "x": 0.387, + "y": 4.121134281158447, + "heading": -2.8840116321326e-26, + "angularVelocity": -5.669163571419092e-28, + "velocityX": 2.108709871721435e-25, + "velocityY": 6.042704411837295e-25, + "timestamp": 0 + }, + { + "x": 0.4005024246667089, + "y": 4.115350466677215, + "heading": -0.015208625308300914, + "angularVelocity": -0.3074427174555653, + "velocityX": 0.27295183145226637, + "velocityY": -0.11691994544690736, + "timestamp": 0.04946815925311036 + }, + { + "x": 0.42752782779164794, + "y": 4.103765609299543, + "heading": -0.04521317655829015, + "angularVelocity": -0.6065427075316668, + "velocityX": 0.5463191582824014, + "velocityY": -0.23418816371145634, + "timestamp": 0.09893631850622071 + }, + { + "x": 0.46809993891055535, + "y": 4.086359371416315, + "heading": -0.08951799702765434, + "angularVelocity": -0.8956229853363399, + "velocityX": 0.8201661782342632, + "velocityY": -0.3518675072214983, + "timestamp": 0.14840447775933108 + }, + { + "x": 0.5222480284136939, + "y": 4.063106533888518, + "heading": -0.14747656628903638, + "angularVelocity": -1.1716338375323283, + "velocityX": 1.0946048998120714, + "velocityY": -0.4700566562184248, + "timestamp": 0.19787263701244143 + }, + { + "x": 0.5900100551308932, + "y": 4.033975950010924, + "heading": -0.21819717233761302, + "angularVelocity": -1.4296187106280898, + "velocityX": 1.3698109600255401, + "velocityY": -0.5888754365923217, + "timestamp": 0.24734079626555178 + }, + { + "x": 0.6714360195671307, + "y": 3.9989305950603806, + "heading": -0.30041829025546435, + "angularVelocity": -1.6621018279082538, + "velocityX": 1.6460277816203115, + "velocityY": -0.708442672613487, + "timestamp": 0.29680895551866215 + }, + { + "x": 0.7665898116570742, + "y": 3.9579282066959594, + "heading": -0.39234342678059203, + "angularVelocity": -1.858268791745021, + "velocityX": 1.923536139743475, + "velocityY": -0.82886424284816, + "timestamp": 0.34627711477177253 + }, + { + "x": 0.8755459842805163, + "y": 3.9109205987886493, + "heading": -0.49137598841221336, + "angularVelocity": -2.0019455570381792, + "velocityX": 2.2025515860809297, + "velocityY": -0.9502598968113863, + "timestamp": 0.3957452740248829 + }, + { + "x": 0.9983715175527711, + "y": 3.8578517541730113, + "heading": -0.5934882805424371, + "angularVelocity": -2.06420238132882, + "velocityX": 2.482921037021039, + "velocityY": -1.0727879390883417, + "timestamp": 0.4452134332779933 + }, + { + "x": 1.1350267000973346, + "y": 3.798688114099643, + "heading": -0.6910495610477184, + "angularVelocity": -1.9722035745477453, + "velocityX": 2.7624877215533625, + "velocityY": -1.1959943722718582, + "timestamp": 0.49468159253110366 + }, + { + "x": 1.2838521273183288, + "y": 3.733527390579999, + "heading": -0.7598419105522805, + "angularVelocity": -1.3906389593471014, + "velocityX": 3.008509503244484, + "velocityY": -1.3172255548511609, + "timestamp": 0.544149751784214 + }, + { + "x": 1.4435364367922139, + "y": 3.6623848351594903, + "heading": -0.7899999551615338, + "angularVelocity": -0.6096455793906863, + "velocityX": 3.2280220627745444, + "velocityY": -1.4381484270821239, + "timestamp": 0.5936179110373243 + }, + { + "x": 1.613097529588805, + "y": 3.58963115719978, + "heading": -0.7899999775809353, + "angularVelocity": -4.532087301884173e-7, + "velocityX": 3.4276814693874025, + "velocityY": -1.4707173070147306, + "timestamp": 0.6430860702904346 + }, + { + "x": 1.7826586961746216, + "y": 3.5168776512145996, + "heading": -0.79, + "angularVelocity": -4.532019196899546e-7, + "velocityX": 3.427682961038322, + "velocityY": -1.4707138305455802, + "timestamp": 0.692554229543545 + }, + { + "x": 2.0243963764510555, + "y": 3.413262421692379, + "heading": -0.79, + "angularVelocity": 4.0902201996407894e-16, + "velocityX": 3.428232758686476, + "velocityY": -1.469432170196615, + "timestamp": 0.7630680182190419 + }, + { + "x": 2.250825412444176, + "y": 3.3162088869723196, + "heading": -0.79, + "angularVelocity": -2.4813985391407045e-16, + "velocityX": 3.211131329719671, + "velocityY": -1.3763766852281583, + "timestamp": 0.8335818068945389 + }, + { + "x": 2.4489508274763203, + "y": 3.231287040432583, + "heading": -0.79, + "angularVelocity": -2.180277121693513e-16, + "velocityX": 2.8097400345897383, + "velocityY": -1.2043296514749058, + "timestamp": 0.9040955955700358 + }, + { + "x": 2.618772615081807, + "y": 3.1584968848445327, + "heading": -0.79, + "angularVelocity": -1.8012058014682582e-16, + "velocityX": 2.40834864776595, + "velocityY": -1.0322825784192244, + "timestamp": 0.9746093842455328 + }, + { + "x": 2.760290773105409, + "y": 3.0978384211319567, + "heading": -0.79, + "angularVelocity": -1.4736426874017007e-16, + "velocityX": 2.006957230377533, + "velocityY": -0.8602354922627292, + "timestamp": 1.0451231729210297 + }, + { + "x": 2.8735053004695117, + "y": 3.0493116497567487, + "heading": -0.79, + "angularVelocity": -8.058066541294709e-17, + "velocityX": 1.6055657977068005, + "velocityY": -0.6881883995558266, + "timestamp": 1.1156369615965267 + }, + { + "x": 2.9584161965275473, + "y": 3.0129165709960453, + "heading": -0.79, + "angularVelocity": -9.176897771941303e-17, + "velocityX": 1.2041743558666786, + "velocityY": -0.5161413029186794, + "timestamp": 1.1861507502720237 + }, + { + "x": 3.01502346084847, + "y": 2.988653185034604, + "heading": -0.79, + "angularVelocity": -1.736982373628201e-17, + "velocityX": 0.8027829079136304, + "velocityY": -0.3440942036613692, + "timestamp": 1.2566645389475206 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -2.7589092071586022e-17, + "velocityX": 0.4013914555942062, + "velocityY": -0.172047102532514, + "timestamp": 1.3271783276230176 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -2.3649328254113597e-28, + "velocityX": 1.8626809073397165e-27, + "velocityY": -1.7812474793700614e-27, + "timestamp": 1.3976921162985145 + }, + { + "x": 3.0599636803008443, + "y": 2.9660757998478573, + "heading": -0.7576339587685645, + "angularVelocity": 0.5546549208152399, + "velocityX": 0.2851002035438971, + "velocityY": -0.17900720432616107, + "timestamp": 1.4560455898129356 + }, + { + "x": 3.0934189139338764, + "y": 2.9451764981868647, + "heading": -0.6947262972943399, + "angularVelocity": 1.078044847813185, + "velocityX": 0.573320346127542, + "velocityY": -0.3581500877720281, + "timestamp": 1.5143990633273567 + }, + { + "x": 3.1439054797379202, + "y": 2.91379696376208, + "heading": -0.6037467592544796, + "angularVelocity": 1.5591109245172228, + "velocityX": 0.8651852711315816, + "velocityY": -0.537749212427429, + "timestamp": 1.5727525368417778 + }, + { + "x": 3.211665591806839, + "y": 2.871861091639316, + "heading": -0.4882356317885913, + "angularVelocity": 1.9795073113743915, + "velocityX": 1.1612010046355354, + "velocityY": -0.7186525428070697, + "timestamp": 1.631106010356199 + }, + { + "x": 3.2969495157584743, + "y": 2.8192307268976187, + "heading": -0.3529793972138387, + "angularVelocity": 2.3178780358520816, + "velocityX": 1.461505525126263, + "velocityY": -0.9019234258384052, + "timestamp": 1.68945948387062 + }, + { + "x": 3.3999955244455466, + "y": 2.755745289292902, + "heading": -0.2040546245446104, + "angularVelocity": 2.552114959059364, + "velocityX": 1.7658933133021877, + "velocityY": -1.0879461629482488, + "timestamp": 1.7478129573850412 + }, + { + "x": 3.5210141241877033, + "y": 2.681299358314012, + "heading": -0.05002850479601411, + "angularVelocity": 2.63953644011494, + "velocityX": 2.073888535739847, + "velocityY": -1.2757754850787486, + "timestamp": 1.8061664308994623 + }, + { + "x": 3.659914538226331, + "y": 2.5959847244718666, + "heading": 0.09335665207111186, + "angularVelocity": 2.4571828930062045, + "velocityX": 2.380328122271953, + "velocityY": -1.4620317986908105, + "timestamp": 1.8645199044138834 + }, + { + "x": 3.8149146487433727, + "y": 2.5012950214969263, + "heading": 0.197502595567386, + "angularVelocity": 1.7847428306137818, + "velocityX": 2.6562276619014966, + "velocityY": -1.622691800026939, + "timestamp": 1.9228733779283045 + }, + { + "x": 3.982928880488226, + "y": 2.395156661504235, + "heading": 0.2522871475824207, + "angularVelocity": 0.9388396048351036, + "velocityX": 2.8792498822427737, + "velocityY": -1.818886753441695, + "timestamp": 1.9812268514427256 + }, + { + "x": 4.164002347253148, + "y": 2.2776755114172382, + "heading": 0.25752210352171573, + "angularVelocity": 0.08971112813020911, + "velocityX": 3.1030452149549066, + "velocityY": -2.013267471694952, + "timestamp": 2.0395803249571465 + }, + { + "x": 4.349312713000023, + "y": 2.1635166721855787, + "heading": 0.25752213554345277, + "angularVelocity": 5.487546005300222e-7, + "velocityX": 3.1756527004528508, + "velocityY": -1.9563332284486414, + "timestamp": 2.0979337984715674 + }, + { + "x": 4.534623121850304, + "y": 2.049357902922309, + "heading": 0.25752216756517515, + "angularVelocity": 5.487543495141043e-7, + "velocityX": 3.1756534391133453, + "velocityY": -1.9563320294045097, + "timestamp": 2.1562872719859882 + }, + { + "x": 4.719933530702919, + "y": 1.9351991336628294, + "heading": 0.25752219958689765, + "angularVelocity": 5.487543505371369e-7, + "velocityX": 3.1756534391533564, + "velocityY": -1.9563320293395612, + "timestamp": 2.214640745500409 + }, + { + "x": 4.905244050568848, + "y": 1.8210405446080342, + "heading": 0.2575222316086183, + "angularVelocity": 5.487543204113664e-7, + "velocityX": 3.1756553415819, + "velocityY": -1.9563289411826175, + "timestamp": 2.27299421901483 + }, + { + "x": 5.095847730511029, + "y": 1.715957675476469, + "heading": 0.25752226374645537, + "angularVelocity": 5.507442001002949e-7, + "velocityX": 3.2663639105404108, + "velocityY": -1.8007988694211323, + "timestamp": 2.331347692529251 + }, + { + "x": 5.295726367846681, + "y": 1.629814177194248, + "heading": 0.2575222972409487, + "angularVelocity": 5.739931368957973e-7, + "velocityX": 3.4253083029625735, + "velocityY": -1.4762359992320253, + "timestamp": 2.3897011660436718 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": 0.25752233398290164, + "angularVelocity": 6.296446587487592e-7, + "velocityX": 3.5520975347185666, + "velocityY": -1.1378104640967532, + "timestamp": 2.4480546395580927 + }, + { + "x": 5.694801641426809, + "y": 1.520040343984435, + "heading": 0.25752236705287185, + "angularVelocity": 6.272660461318871e-7, + "velocityX": 3.637995437353645, + "velocityY": -0.8227992972681996, + "timestamp": 2.5007754434925817 + }, + { + "x": 5.889658502180153, + "y": 1.4936017291226997, + "heading": 0.25752239835429896, + "angularVelocity": 5.937205950740546e-7, + "velocityX": 3.6960145940770253, + "velocityY": -0.5014835300043611, + "timestamp": 2.553496247427071 + }, + { + "x": 6.086080950951087, + "y": 1.484305583780122, + "heading": 0.25752242915160917, + "angularVelocity": 5.841585845609402e-7, + "velocityX": 3.7257104238207144, + "velocityY": -0.17632783737761368, + "timestamp": 2.60621705136156 + }, + { + "x": 6.282694937149082, + "y": 1.4876428211340982, + "heading": 0.257522460230909, + "angularVelocity": 5.895073207644364e-7, + "velocityX": 3.7293434759133812, + "velocityY": 0.06330019849702921, + "timestamp": 2.658937855296049 + }, + { + "x": 6.479308911969213, + "y": 1.490980728749722, + "heading": 0.25752249131023625, + "angularVelocity": 5.895078402009839e-7, + "velocityX": 3.7293432600998244, + "velocityY": 0.06331291191559962, + "timestamp": 2.711658659230538 + }, + { + "x": 6.675718826372289, + "y": 1.500538040759823, + "heading": 0.25752252283498844, + "angularVelocity": 5.979565905391123e-7, + "velocityX": 3.725472673882885, + "velocityY": 0.18128160606156274, + "timestamp": 2.764379463165027 + }, + { + "x": 6.870540392297196, + "y": 1.5272354931118775, + "heading": 0.25752255617569925, + "angularVelocity": 6.324014114235091e-7, + "velocityX": 3.695345127266883, + "velocityY": 0.5063931192177723, + "timestamp": 2.817100267099516 + }, + { + "x": 7.062280392097909, + "y": 1.5708688177166172, + "heading": 0.25752360307904704, + "angularVelocity": 0.000019857499690926623, + "velocityX": 3.6368944608464226, + "velocityY": 0.8276301070628356, + "timestamp": 2.8698210710340053 + }, + { + "x": 7.242656103418754, + "y": 1.6247622711713998, + "heading": 0.29034352591414114, + "angularVelocity": 0.622523186024936, + "velocityX": 3.4213384064662518, + "velocityY": 1.0222426335104984, + "timestamp": 2.9225418749684944 + }, + { + "x": 7.410676149226542, + "y": 1.6874049568089853, + "heading": 0.35686615036971664, + "angularVelocity": 1.2617907825957404, + "velocityX": 3.1869780668855143, + "velocityY": 1.188196707232031, + "timestamp": 2.9752626789029835 + }, + { + "x": 7.5654443716076845, + "y": 1.7579385900879527, + "heading": 0.45500472309026196, + "angularVelocity": 1.8614771664425407, + "velocityX": 2.935619543538406, + "velocityY": 1.337870973413318, + "timestamp": 3.0279834828374725 + }, + { + "x": 7.704030677677, + "y": 1.8336246154274285, + "heading": 0.5647056674911662, + "angularVelocity": 2.080790432126545, + "velocityX": 2.628683474582893, + "velocityY": 1.4356007437504827, + "timestamp": 3.0807042867719616 + }, + { + "x": 7.825774553014182, + "y": 1.9127789189520943, + "heading": 0.6711461233913874, + "angularVelocity": 2.0189459939283965, + "velocityX": 2.3092188709500836, + "velocityY": 1.5013865043299246, + "timestamp": 3.1334250907064507 + }, + { + "x": 7.930703511686336, + "y": 1.9944877491725297, + "heading": 0.7666398660798261, + "angularVelocity": 1.811310442213658, + "velocityX": 1.9902761498580215, + "velocityY": 1.5498403689360842, + "timestamp": 3.18614589464094 + }, + { + "x": 8.018956158365414, + "y": 2.0782214675210318, + "heading": 0.8466098380495345, + "angularVelocity": 1.5168579763897216, + "velocityX": 1.6739624606017243, + "velocityY": 1.5882481316587975, + "timestamp": 3.238866698575429 + }, + { + "x": 8.090681062335408, + "y": 2.163657607085554, + "heading": 0.9079138555243396, + "angularVelocity": 1.1628050579612106, + "velocityX": 1.360466810390799, + "velocityY": 1.6205393922043627, + "timestamp": 3.291587502509918 + }, + { + "x": 8.146013507746817, + "y": 2.250588744141006, + "heading": 0.9482458723141572, + "angularVelocity": 0.7650114144680767, + "velocityX": 1.0495372088818153, + "velocityY": 1.6488962718298632, + "timestamp": 3.344308306444407 + }, + { + "x": 8.18506370691749, + "y": 2.3388711976421583, + "heading": 0.9659657399574388, + "angularVelocity": 0.33610769033985816, + "velocityX": 0.7406980974568496, + "velocityY": 1.6745278317616754, + "timestamp": 3.397029110378896 + }, + { + "x": 8.207907676696777, + "y": 2.428393840789795, + "heading": 0.9600708878718816, + "angularVelocity": -0.11181263648563311, + "velocityX": 0.4333008617940415, + "velocityY": 1.6980515558692484, + "timestamp": 3.449749914313385 + }, + { + "x": 8.211510391317606, + "y": 2.540296472438403, + "heading": 0.9167118198425396, + "angularVelocity": -0.6681402574181382, + "velocityX": 0.055515922817721344, + "velocityY": 1.7243602437412162, + "timestamp": 3.514645072998917 + }, + { + "x": 8.190357670268208, + "y": 2.653506168063042, + "heading": 0.8402267432816896, + "angularVelocity": -1.1785944916396733, + "velocityX": -0.32595222013246916, + "velocityY": 1.7445014068496127, + "timestamp": 3.5795402316844487 + }, + { + "x": 8.14421449138386, + "y": 2.767534412566691, + "heading": 0.7336533483609798, + "angularVelocity": -1.6422395303344925, + "velocityX": -0.711041930076003, + "velocityY": 1.7571148112327775, + "timestamp": 3.6444353903699804 + }, + { + "x": 8.072849326580373, + "y": 2.8818576996728056, + "heading": 0.6001933866162775, + "angularVelocity": -2.056547274834806, + "velocityX": -1.0996993650837186, + "velocityY": 1.7616612613600555, + "timestamp": 3.709330549055512 + }, + { + "x": 7.975969110351945, + "y": 2.995874279761309, + "heading": 0.444818066858235, + "angularVelocity": -2.3942513263733436, + "velocityX": -1.492872784207061, + "velocityY": 1.756935068777684, + "timestamp": 3.774225707741044 + }, + { + "x": 7.853227135461901, + "y": 3.1086496791956706, + "heading": 0.2767628269059192, + "angularVelocity": -2.589642176031597, + "velocityX": -1.8913887780878373, + "velocityY": 1.7378091327405185, + "timestamp": 3.8391208664265757 + }, + { + "x": 7.704583384688799, + "y": 3.218490261217407, + "heading": 0.11219782786286417, + "angularVelocity": -2.5358594134965102, + "velocityX": -2.290521416141349, + "velocityY": 1.6925851519063353, + "timestamp": 3.9040160251121074 + }, + { + "x": 7.53264205332364, + "y": 3.322684523690155, + "heading": -0.008853804183353416, + "angularVelocity": -1.8653414907698829, + "velocityX": -2.6495247850205796, + "velocityY": 1.6055783602849627, + "timestamp": 3.968911183797639 + }, + { + "x": 7.337826739220927, + "y": 3.421844874305526, + "heading": -0.08105196591494265, + "angularVelocity": -1.112535406245734, + "velocityX": -3.00200073547469, + "velocityY": 1.5280084466066417, + "timestamp": 4.033806342483171 + }, + { + "x": 7.120736598968506, + "y": 3.5168776512145996, + "heading": -0.09725228724922261, + "angularVelocity": -0.2496383653637915, + "velocityX": -3.3452440004715016, + "velocityY": 1.4644047234645439, + "timestamp": 4.098701501168702 + }, + { + "x": 7.0106063205998925, + "y": 3.566708400606869, + "heading": -0.09725231991858449, + "angularVelocity": -0.0000010080544024334555, + "velocityX": -3.398208767574138, + "velocityY": 1.5375906788578377, + "timestamp": 4.1311098332934035 + }, + { + "x": 6.900476113958648, + "y": 3.616539308522766, + "heading": -0.09725235258763598, + "angularVelocity": -0.000001008044825380236, + "velocityX": -3.398206554335633, + "velocityY": 1.5375955703044946, + "timestamp": 4.163518165418105 + }, + { + "x": 6.790345907323517, + "y": 3.6663702164521736, + "heading": -0.09725238525668778, + "angularVelocity": -0.0000010080448348715788, + "velocityX": -3.3982065541469955, + "velocityY": 1.5375955707213955, + "timestamp": 4.195926497542806 + }, + { + "x": 6.680215700688386, + "y": 3.7162011243815836, + "heading": -0.09725241792573983, + "angularVelocity": -0.000001008044842341437, + "velocityX": -3.3982065541469777, + "velocityY": 1.5375955707214333, + "timestamp": 4.228334829667507 + }, + { + "x": 6.570085494053255, + "y": 3.7660320323109926, + "heading": -0.09725245059479215, + "angularVelocity": -0.0000010080448508340198, + "velocityX": -3.398206554146978, + "velocityY": 1.5375955707214313, + "timestamp": 4.260743161792209 + }, + { + "x": 6.459955287418124, + "y": 3.8158629402404016, + "heading": -0.09725248326384477, + "angularVelocity": -0.0000010080448595181056, + "velocityX": -3.3982065541469764, + "velocityY": 1.5375955707214315, + "timestamp": 4.29315149391691 + }, + { + "x": 6.349825080782993, + "y": 3.8656938481698107, + "heading": -0.09725251593289766, + "angularVelocity": -0.0000010080448688287921, + "velocityX": -3.3982065541469764, + "velocityY": 1.5375955707214306, + "timestamp": 4.325559826041611 + }, + { + "x": 6.239694874147862, + "y": 3.91552475609922, + "heading": -0.09725254860195084, + "angularVelocity": -0.0000010080448772415697, + "velocityX": -3.3982065541469755, + "velocityY": 1.5375955707214306, + "timestamp": 4.3579681581663126 + }, + { + "x": 6.129564667512731, + "y": 3.965355664028629, + "heading": -0.09725258127100428, + "angularVelocity": -0.0000010080448851885182, + "velocityX": -3.3982065541469755, + "velocityY": 1.5375955707214297, + "timestamp": 4.390376490291014 + }, + { + "x": 6.0194344608776, + "y": 4.015186571958038, + "heading": -0.09725261394005799, + "angularVelocity": -0.0000010080448937492915, + "velocityX": -3.3982065541469746, + "velocityY": 1.5375955707214293, + "timestamp": 4.422784822415715 + }, + { + "x": 5.9093042542424685, + "y": 4.065017479887447, + "heading": -0.09725264660911201, + "angularVelocity": -0.0000010080449031799624, + "velocityX": -3.398206554146974, + "velocityY": 1.5375955707214286, + "timestamp": 4.455193154540416 + }, + { + "x": 5.7991740476073375, + "y": 4.114848387816856, + "heading": -0.0972526792781663, + "angularVelocity": -0.000001008044911324413, + "velocityX": -3.3982065541469737, + "velocityY": 1.5375955707214282, + "timestamp": 4.487601486665118 + }, + { + "x": 5.689043840972206, + "y": 4.164679295746265, + "heading": -0.09725271194722086, + "angularVelocity": -0.0000010080449199147737, + "velocityX": -3.398206554146973, + "velocityY": 1.5375955707214277, + "timestamp": 4.520009818789819 + }, + { + "x": 5.578913634337075, + "y": 4.214510203675674, + "heading": -0.09725274461627566, + "angularVelocity": -0.0000010080449273058883, + "velocityX": -3.398206554146972, + "velocityY": 1.5375955707214277, + "timestamp": 4.55241815091452 + }, + { + "x": 5.468783427701944, + "y": 4.264341111605084, + "heading": -0.09725277728533077, + "angularVelocity": -0.000001008044937071081, + "velocityX": -3.3982065541469724, + "velocityY": 1.537595570721425, + "timestamp": 4.584826483039222 + }, + { + "x": 5.358653221066815, + "y": 4.314172019534495, + "heading": -0.09725280995438615, + "angularVelocity": -0.0000010080449450667608, + "velocityX": -3.3982065541469435, + "velocityY": 1.5375955707214868, + "timestamp": 4.617234815163923 + }, + { + "x": 5.2485230144416954, + "y": 4.364002927486029, + "heading": -0.0972528426234418, + "angularVelocity": -0.0000010080449533993329, + "velocityX": -3.3982065538380732, + "velocityY": 1.5375955714041123, + "timestamp": 4.649643147288624 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.0000010080480324849667, + "velocityX": -3.398202929897321, + "velocityY": 1.5376035805684636, + "timestamp": 4.6820514794133254 + }, + { + "x": 4.91372823368964, + "y": 4.535640205255565, + "heading": -0.09725287529265084, + "angularVelocity": -7.461622400229302e-13, + "velocityX": -3.1281866300757337, + "velocityY": 1.6960041335008067, + "timestamp": 4.753870944854674 + }, + { + "x": 4.717146630995297, + "y": 4.64222057593122, + "heading": -0.0972528752926601, + "angularVelocity": -1.288593597138777e-13, + "velocityX": -2.7371632674554296, + "velocityY": 1.4840039538124186, + "timestamp": 4.825690410296022 + }, + { + "x": 4.5486481153378495, + "y": 4.733575188699939, + "heading": -0.0972528752926535, + "angularVelocity": 9.166586601280667e-14, + "velocityX": -2.3461399304768347, + "velocityY": 1.2720035189251644, + "timestamp": 4.89750987573737 + }, + { + "x": 4.408232686103436, + "y": 4.809704037452307, + "heading": -0.0972528752926406, + "angularVelocity": 1.7989350116390222e-13, + "velocityX": -1.955116602045504, + "velocityY": 1.0600029989716206, + "timestamp": 4.969329341178718 + }, + { + "x": 4.295900342985128, + "y": 4.8706071191336155, + "heading": -0.09725287529262608, + "angularVelocity": 2.0200921674140544e-13, + "velocityX": -1.5640932778878103, + "velocityY": 0.8480024364849348, + "timestamp": 5.0411488066200665 + }, + { + "x": 4.211651085798766, + "y": 4.916284431911041, + "heading": -0.09725287529261284, + "angularVelocity": 1.8429670420443223e-13, + "velocityX": -1.1730699562943, + "velocityY": 0.6360018484783654, + "timestamp": 5.112968272061415 + }, + { + "x": 4.155484914421578, + "y": 4.9467359745627, + "heading": -0.09725287529260278, + "angularVelocity": 1.401436382547871e-13, + "velocityX": -0.7820466364102463, + "velocityY": 0.4240012434585409, + "timestamp": 5.184787737502763 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 7.711346668937439e-14, + "velocityX": -0.39102331774723287, + "velocityY": 0.21200062628639202, + "timestamp": 5.256607202944111 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -5.652824755464892e-28, + "velocityX": 8.289180813930789e-28, + "velocityY": 1.3058854376408088e-27, + "timestamp": 5.328426668385459 + }, + { + "x": 4.149339902607208, + "y": 4.948719603159426, + "heading": -0.09725287534607682, + "angularVelocity": -8.314126318446111e-10, + "velocityX": 0.3410571082022834, + "velocityY": -0.20586707155231002, + "timestamp": 5.39275042330432 + }, + { + "x": 4.1933171531744176, + "y": 4.922404277066319, + "heading": -0.09725287544407178, + "angularVelocity": -1.5234644431523042e-9, + "velocityX": 0.6836859978507643, + "velocityY": -0.4091074304710872, + "timestamp": 5.457074178223181 + }, + { + "x": 4.259461121252347, + "y": 4.8832332111646055, + "heading": -0.0972528755751186, + "angularVelocity": -2.0373002769674516e-9, + "velocityX": 1.0282976819584715, + "velocityY": -0.6089673395330295, + "timestamp": 5.521397933142041 + }, + { + "x": 4.34793757416226, + "y": 4.831496576688514, + "heading": -0.09725287572404445, + "angularVelocity": -2.3152544478823804e-9, + "velocityX": 1.375486443873169, + "velocityY": -0.8043161432561301, + "timestamp": 5.585721688060902 + }, + { + "x": 4.458970362431496, + "y": 4.767600785112478, + "heading": -0.09725287586983122, + "angularVelocity": -2.2664532339146123e-9, + "velocityX": 1.7261552658002488, + "velocityY": -0.9933467294724277, + "timestamp": 5.650045442979763 + }, + { + "x": 4.59287734823679, + "y": 4.692154910649705, + "heading": -0.09725287598146638, + "angularVelocity": -1.7355199749767777e-9, + "velocityX": 2.081765686319258, + "velocityY": -1.1729084310756062, + "timestamp": 5.714369197898623 + }, + { + "x": 4.750141155397852, + "y": 4.606169009566351, + "heading": -0.09725287600872443, + "angularVelocity": -4.2376330088015544e-10, + "velocityX": 2.444879148604377, + "velocityY": -1.3367674382787311, + "timestamp": 5.778692952817484 + }, + { + "x": 4.93155755546803, + "y": 4.511622419087021, + "heading": -0.0972528758570734, + "angularVelocity": 2.3576208162700097e-9, + "velocityX": 2.8203639588363623, + "velocityY": -1.469854964135602, + "timestamp": 5.843016707736345 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": 8.775547292964994e-9, + "velocityX": 3.2155363139998, + "velocityY": -1.5202521091803838, + "timestamp": 5.907340462655205 + }, + { + "x": 5.399102105862753, + "y": 4.325847836745161, + "heading": -0.09725287310436229, + "angularVelocity": 2.9662640662901703e-8, + "velocityX": 3.534045888591934, + "velocityY": -1.1926985982098404, + "timestamp": 5.98111120271319 + }, + { + "x": 5.668612776819124, + "y": 4.270396424489206, + "heading": -0.09725287179514892, + "angularVelocity": 1.7747054810662862e-8, + "velocityX": 3.653354578584066, + "velocityY": -0.7516721699195277, + "timestamp": 6.054881942771174 + }, + { + "x": 5.942881070814964, + "y": 4.248311045956059, + "heading": -0.09725287053637834, + "angularVelocity": 1.706327712659208e-8, + "velocityX": 3.7178465849775284, + "velocityY": -0.2993785681936777, + "timestamp": 6.128652682829158 + }, + { + "x": 6.217467173732042, + "y": 4.230609827633618, + "heading": -0.09725286928029948, + "angularVelocity": 1.702678935422503e-8, + "velocityX": 3.722154646967784, + "velocityY": -0.23994904088704855, + "timestamp": 6.202423422887143 + }, + { + "x": 6.492053278688794, + "y": 4.212908640951192, + "heading": -0.09725286802422056, + "angularVelocity": 1.702679012699192e-8, + "velocityX": 3.722154674616614, + "velocityY": -0.23994861199050874, + "timestamp": 6.276194162945127 + }, + { + "x": 6.7666393836455585, + "y": 4.195207454268953, + "heading": -0.09725286676814171, + "angularVelocity": 1.70267893622157e-8, + "velocityX": 3.7221546746167777, + "velocityY": -0.23994861198797246, + "timestamp": 6.349964903003111 + }, + { + "x": 7.041225488263755, + "y": 4.177506262334816, + "heading": -0.09725286551205693, + "angularVelocity": 1.702686968579546e-8, + "velocityX": 3.7221546700273103, + "velocityY": -0.23994868318012236, + "timestamp": 6.423735643061096 + }, + { + "x": 7.304754521125749, + "y": 4.159872953315235, + "heading": -0.07493632738083242, + "angularVelocity": 0.30251205442271084, + "velocityX": 3.572270423949385, + "velocityY": -0.23902849565725026, + "timestamp": 6.49750638311908 + }, + { + "x": 7.534656768824059, + "y": 4.1444074082490925, + "heading": -0.055522130294338126, + "angularVelocity": 0.26316934154699656, + "velocityX": 3.116442203475328, + "velocityY": -0.2096433498428548, + "timestamp": 6.571277123177064 + }, + { + "x": 7.730932226657292, + "y": 4.131109655486069, + "heading": -0.03901051064994992, + "angularVelocity": 0.22382342418430445, + "velocityX": 2.660613919271525, + "velocityY": -0.18025781973409782, + "timestamp": 6.645047863235049 + }, + { + "x": 7.89358089446588, + "y": 4.119979717938594, + "heading": -0.025401753521899054, + "angularVelocity": 0.18447364249503723, + "velocityX": 2.2047856329046707, + "velocityY": -0.1508719790356963, + "timestamp": 6.718818603293033 + }, + { + "x": 8.022602772932235, + "y": 4.1110176135114935, + "heading": -0.014696144661245043, + "angularVelocity": 0.14511998730444314, + "velocityX": 1.7489573557882623, + "velocityY": -0.12148589562821704, + "timestamp": 6.792589343351017 + }, + { + "x": 8.117997862710347, + "y": 4.104223355212346, + "heading": -0.006893914960978184, + "angularVelocity": 0.10576320224162433, + "velocityX": 1.2931290875370185, + "velocityY": -0.09209963589639363, + "timestamp": 6.866360083409002 + }, + { + "x": 8.179766164065947, + "y": 4.099596951205949, + "heading": -0.001995197471495412, + "angularVelocity": 0.06640461361282764, + "velocityX": 0.8373008228879115, + "velocityY": -0.0627132654865662, + "timestamp": 6.940130823466986 + }, + { + "x": 8.207907676696777, + "y": 4.097138404846191, + "heading": -7.002250121980633e-28, + "angularVelocity": 0.027045919153409353, + "velocityX": 0.38147255414152553, + "velocityY": -0.0333268496130705, + "timestamp": 7.01390156352497 + }, + { + "x": 8.200635459356112, + "y": 4.096934551280871, + "heading": -0.0010816052785507836, + "angularVelocity": -0.014043494340470785, + "velocityX": -0.09442200874161932, + "velocityY": -0.0026468217635775776, + "timestamp": 7.09091980786846 + }, + { + "x": 8.156710678515955, + "y": 4.0990936119074455, + "heading": -0.0053278404451381055, + "angularVelocity": -0.055132848103493455, + "velocityX": -0.5703165686854601, + "velocityY": 0.028033106246169795, + "timestamp": 7.167938052211949 + }, + { + "x": 8.076133333676754, + "y": 4.103615573325919, + "heading": -0.012738567096053588, + "angularVelocity": -0.0962204048415442, + "velocityX": -1.0462111351154568, + "velocityY": 0.05871286027121356, + "timestamp": 7.244956296555438 + }, + { + "x": 7.958903423782304, + "y": 4.110500416395447, + "heading": -0.023313558262368205, + "angularVelocity": -0.13730501462941505, + "velocityX": -1.522105715259157, + "velocityY": 0.08939236577274852, + "timestamp": 7.321974540898927 + }, + { + "x": 7.8050209475058985, + "y": 4.1197481162013645, + "heading": -0.03705256536677336, + "angularVelocity": -0.17838639690527597, + "velocityX": -1.9980003126287051, + "velocityY": 0.12007154778384964, + "timestamp": 7.398992785242417 + }, + { + "x": 7.614485903743769, + "y": 4.131358642030129, + "heading": -0.05395540666928083, + "angularVelocity": -0.21946541948065604, + "velocityX": -2.4738949243295387, + "velocityY": 0.15075033101226304, + "timestamp": 7.476011029585906 + }, + { + "x": 7.38729829267515, + "y": 4.145331957366106, + "heading": -0.07402207604262086, + "angularVelocity": -0.2605443624999541, + "velocityX": -2.9497895337031945, + "velocityY": 0.18142864012399953, + "timestamp": 7.553029273929395 + }, + { + "x": 7.123458119903768, + "y": 4.1616680199277996, + "heading": -0.09725286481798223, + "angularVelocity": -0.30162708814493094, + "velocityX": -3.425684070318428, + "velocityY": 0.21210640025547228, + "timestamp": 7.630047518272884 + }, + { + "x": 6.836784301080243, + "y": 4.180148431298723, + "heading": -0.0972528661294337, + "angularVelocity": -1.7027802603385206e-8, + "velocityX": -3.722154682532179, + "velocityY": 0.2399484891982639, + "timestamp": 7.707065762616374 + }, + { + "x": 6.550110482735962, + "y": 4.198628850104045, + "heading": -0.09725286744087382, + "angularVelocity": -1.7027655300024213e-8, + "velocityX": -3.722154676309698, + "velocityY": 0.23994858572603794, + "timestamp": 7.784084006959863 + }, + { + "x": 6.263436664391698, + "y": 4.217109268909628, + "heading": -0.09725286875231377, + "angularVelocity": -1.7027653194201686e-8, + "velocityX": -3.7221546763094806, + "velocityY": 0.23994858572941535, + "timestamp": 7.861102251303352 + }, + { + "x": 5.976762848146004, + "y": 4.235589720268832, + "heading": -0.09725287006375381, + "angularVelocity": -1.702765423071752e-8, + "velocityX": -3.722154649061779, + "velocityY": 0.23994900840357883, + "timestamp": 7.938120495646841 + }, + { + "x": 5.690447961543492, + "y": 4.258981806583179, + "heading": -0.09725287137836151, + "angularVelocity": -1.7068782885666804e-8, + "velocityX": -3.717494329338312, + "velocityY": 0.30372136516150916, + "timestamp": 8.01513873999033 + }, + { + "x": 5.409459505180122, + "y": 4.318722004074591, + "heading": -0.09725287275094441, + "angularVelocity": -1.7821529284929368e-8, + "velocityX": -3.6483362969195534, + "velocityY": 0.7756629354595511, + "timestamp": 8.092156984333819 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -3.300065929591643e-8, + "velocityX": -3.519511282401575, + "velocityY": 1.2349293565099368, + "timestamp": 8.169175228677307 + }, + { + "x": 4.908992840849997, + "y": 4.524204173518216, + "heading": -0.09725287595344081, + "angularVelocity": -9.191205906949272e-9, + "velocityX": -3.1905635663992595, + "velocityY": 1.535059379942558, + "timestamp": 8.24107477859643 + }, + { + "x": 4.7110423526588185, + "y": 4.628577138266545, + "heading": -0.097252876094701, + "angularVelocity": -1.964688046810948e-9, + "velocityX": -2.7531533704153888, + "velocityY": 1.4516497650644007, + "timestamp": 8.312974328515551 + }, + { + "x": 4.542899034827415, + "y": 4.721263554864837, + "heading": -0.09725287602386487, + "angularVelocity": 9.85209742042839e-10, + "velocityX": -2.338586514387707, + "velocityY": 1.2891098303473851, + "timestamp": 8.384873878434673 + }, + { + "x": 4.403657392505783, + "y": 4.800175498734494, + "heading": -0.09725287586251803, + "angularVelocity": 2.2440590575017265e-9, + "velocityX": -1.936613545957686, + "velocityY": 1.0975304290280408, + "timestamp": 8.456773428353795 + }, + { + "x": 4.29278499065898, + "y": 4.864247320916007, + "heading": -0.09725287567494544, + "angularVelocity": 2.608814644263043e-9, + "velocityX": -1.5420458399464478, + "velocityY": 0.8911296698461443, + "timestamp": 8.528672978272917 + }, + { + "x": 4.209935704536306, + "y": 4.912835343646817, + "heading": -0.09725287550078096, + "angularVelocity": 2.4223304424020015e-9, + "velocityX": -1.152292138349535, + "velocityY": 0.6757764518062407, + "timestamp": 8.600572528192039 + }, + { + "x": 4.1548674777952215, + "y": 4.945509352761423, + "heading": -0.09725287536687216, + "angularVelocity": 1.862443038330612e-9, + "velocityX": -0.7659050272641558, + "velocityY": 0.4544396891407544, + "timestamp": 8.672472078111161 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": 1.0330372741830232e-9, + "velocityX": -0.3820002915212732, + "velocityY": 0.2288247071491247, + "timestamp": 8.744371628030283 + }, + { + "x": 4.127401828765869, + "y": 4.96196174621582, + "heading": -0.09725287529259724, + "angularVelocity": -3.6520722228130864e-26, + "velocityX": -9.590252743264302e-26, + "velocityY": -2.0342869025815665e-25, + "timestamp": 8.816271177949405 + }, + { + "x": 4.149907885471957, + "y": 4.949853513181501, + "heading": -0.09710925579665566, + "angularVelocity": 0.0022357452830962274, + "velocityX": 0.35035501128758395, + "velocityY": -0.18849059952223363, + "timestamp": 8.88050903949269 + }, + { + "x": 4.194913909182307, + "y": 4.925625720140559, + "heading": -0.09684438286421601, + "angularVelocity": 0.004123314912361655, + "velocityX": 0.7006152233138045, + "velocityY": -0.3771575276461672, + "timestamp": 8.944746901035975 + }, + { + "x": 4.262412058477699, + "y": 4.889263805728157, + "heading": -0.09648702221681861, + "angularVelocity": 0.0055630844304586, + "velocityX": 1.0507533668428728, + "velocityY": -0.5660511346240961, + "timestamp": 9.00898476257926 + }, + { + "x": 4.3523918574804155, + "y": 4.840748358129665, + "heading": -0.09607554613145339, + "angularVelocity": 0.006405507211474483, + "velocityX": 1.4007284308816241, + "velocityY": -0.755246928103313, + "timestamp": 9.073222624122545 + }, + { + "x": 4.464838600314873, + "y": 4.78005220731639, + "heading": -0.09566371008606342, + "angularVelocity": 0.006411110760784919, + "velocityX": 1.7504745664469084, + "velocityY": -0.9448656813143913, + "timestamp": 9.13746048566583 + }, + { + "x": 4.599730141458809, + "y": 4.7071346128101945, + "heading": -0.09533221857083674, + "angularVelocity": 0.005160375941271345, + "velocityX": 2.0998759594922722, + "velocityY": -1.1351186473893293, + "timestamp": 9.201698347209115 + }, + { + "x": 4.757029340577212, + "y": 4.621927713991256, + "heading": -0.09521575794797991, + "angularVelocity": 0.0018129592121981597, + "velocityX": 2.4486991836179413, + "velocityY": -1.3264280094617478, + "timestamp": 9.2659362087524 + }, + { + "x": 4.936661051016691, + "y": 4.524295949358237, + "heading": -0.09558433952634979, + "angularVelocity": -0.005737762271577307, + "velocityX": 2.7963525890169896, + "velocityY": -1.519847676860057, + "timestamp": 9.330174070295685 + }, + { + "x": 5.138392925262451, + "y": 4.413834095001221, + "heading": -0.09725287529259724, + "angularVelocity": -0.025974335480068365, + "velocityX": 3.1403890073431286, + "velocityY": -1.7195755229583027, + "timestamp": 9.39441193183897 + }, + { + "x": 5.282623687046255, + "y": 4.319887903045294, + "heading": -0.12350374979021995, + "angularVelocity": -0.54152818219314, + "velocityX": 2.9753302981273193, + "velocityY": -1.9380120292170788, + "timestamp": 9.442887478192457 + }, + { + "x": 5.415965984922444, + "y": 4.230982927805077, + "heading": -0.18642712240012974, + "angularVelocity": -1.298043598128203, + "velocityX": 2.750712635683371, + "velocityY": -1.834016982334084, + "timestamp": 9.491363024545944 + }, + { + "x": 5.544046179661802, + "y": 4.15236621254376, + "heading": -0.28429132793206696, + "angularVelocity": -2.0188365659317435, + "velocityX": 2.642160932140699, + "velocityY": -1.6217809014062547, + "timestamp": 9.53983857089943 + }, + { + "x": 5.6705798620807135, + "y": 4.085599938426855, + "heading": -0.4169505782108561, + "angularVelocity": -2.736622075622024, + "velocityX": 2.610257994746929, + "velocityY": -1.3773186511409734, + "timestamp": 9.588314117252917 + }, + { + "x": 5.804006769424575, + "y": 4.008650582245615, + "heading": -0.5169463778011086, + "angularVelocity": -2.06280912980491, + "velocityX": 2.7524580408213564, + "velocityY": -1.5873850213078682, + "timestamp": 9.636789663606404 + }, + { + "x": 5.938212060885805, + "y": 3.9187237193410076, + "heading": -0.5903545372750207, + "angularVelocity": -1.5143338238751507, + "velocityX": 2.7685152939298026, + "velocityY": -1.8550974598379135, + "timestamp": 9.68526520995989 + }, + { + "x": 6.076913949313874, + "y": 3.818161785200274, + "heading": -0.6273225494125774, + "angularVelocity": -0.7626115622913053, + "velocityX": 2.8612754029969545, + "velocityY": -2.07448789555523, + "timestamp": 9.733740756313377 + }, + { + "x": 6.214509922576719, + "y": 3.712335803608528, + "heading": -0.6556184225519457, + "angularVelocity": -0.5837143728723201, + "velocityX": 2.838461525724493, + "velocityY": -2.1830797082730613, + "timestamp": 9.782216302666864 + }, + { + "x": 6.348284689884717, + "y": 3.603969355784692, + "heading": -0.6891441796191574, + "angularVelocity": -0.6916014277124424, + "velocityX": 2.7596340293414254, + "velocityY": -2.2354868789641342, + "timestamp": 9.83069184902035 + }, + { + "x": 6.474985027954153, + "y": 3.4904345524521627, + "heading": -0.730650696928258, + "angularVelocity": -0.8562361939447342, + "velocityX": 2.613695927128511, + "velocityY": -2.342104666642176, + "timestamp": 9.879167395373837 + }, + { + "x": 6.594663143157959, + "y": 3.3646228313446045, + "heading": -0.7575105881170743, + "angularVelocity": -0.554091561814539, + "velocityX": 2.468834787979625, + "velocityY": -2.595364685322631, + "timestamp": 9.927642941727324 + }, + { + "x": 6.674801371049327, + "y": 3.2699580042827607, + "heading": -0.7721877645618775, + "angularVelocity": -0.4274597319602108, + "velocityX": 2.333954731895263, + "velocityY": -2.757029034439579, + "timestamp": 9.961978752442167 + }, + { + "x": 6.750751589732745, + "y": 3.1695424250982125, + "heading": -0.7798246346604031, + "angularVelocity": -0.2224170607750979, + "velocityX": 2.211982682284112, + "velocityY": -2.9245145838697275, + "timestamp": 9.99631456315701 + }, + { + "x": 6.822476452924599, + "y": 3.063443372222415, + "heading": -0.7798255890957266, + "angularVelocity": -0.000027797081344724893, + "velocityX": 2.0889229553228312, + "velocityY": -3.0900407087208346, + "timestamp": 10.030650373871854 + }, + { + "x": 6.888038953713801, + "y": 2.9534294027046353, + "heading": -0.779825865993983, + "angularVelocity": -0.000008064415854472034, + "velocityX": 1.9094496219615917, + "velocityY": -3.2040591798294837, + "timestamp": 10.064986184586697 + }, + { + "x": 6.951397199301839, + "y": 2.842131382226383, + "heading": -0.7798261188046057, + "angularVelocity": -0.0000073628849152658005, + "velocityX": 1.8452526464053884, + "velocityY": -3.241456023933041, + "timestamp": 10.09932199530154 + }, + { + "x": 7.014755263905195, + "y": 2.730833258719293, + "heading": -0.7798263716136229, + "angularVelocity": -0.000007362838153289018, + "velocityX": 1.8452473753871865, + "velocityY": -3.2414590245564723, + "timestamp": 10.133657806016384 + }, + { + "x": 7.078113328495587, + "y": 2.6195351352048375, + "heading": -0.7798266244227116, + "angularVelocity": -0.000007362840235334639, + "velocityX": 1.8452473750096865, + "velocityY": -3.241459024770976, + "timestamp": 10.167993616731227 + }, + { + "x": 7.1414713930859905, + "y": 2.5082370116904005, + "heading": -0.7798268772318718, + "angularVelocity": -0.000007362842320190408, + "velocityX": 1.84524737500996, + "velocityY": -3.241459024770425, + "timestamp": 10.20232942744607 + }, + { + "x": 7.204829457676403, + "y": 2.396938888175983, + "heading": -0.7798271300411037, + "angularVelocity": -0.000007362844406233281, + "velocityX": 1.845247375010246, + "velocityY": -3.2414590247698674, + "timestamp": 10.236665238160914 + }, + { + "x": 7.2681875222668415, + "y": 2.285640764661595, + "heading": -0.7798273828504071, + "angularVelocity": -0.0000073628464912696794, + "velocityX": 1.845247375011028, + "velocityY": -3.2414590247690276, + "timestamp": 10.271001048875757 + }, + { + "x": 7.3315455870489, + "y": 2.1743426412563025, + "heading": -0.7798276356597839, + "angularVelocity": -0.000007362848626587696, + "velocityX": 1.8452473805917786, + "velocityY": -3.241459021591694, + "timestamp": 10.3053368595906 + }, + { + "x": 7.394906330223783, + "y": 2.0630460426228225, + "heading": -0.7798278885161463, + "angularVelocity": -0.0000073642170417804714, + "velocityX": 1.8453253864046084, + "velocityY": -3.241414613966507, + "timestamp": 10.339672670305443 + }, + { + "x": 7.462756763097692, + "y": 1.9565771896637063, + "heading": -0.7859464592787083, + "angularVelocity": -0.1781979407265627, + "velocityX": 1.976083612453597, + "velocityY": -3.100810807798736, + "timestamp": 10.374008481020287 + }, + { + "x": 7.530997298306203, + "y": 1.8560643499724379, + "heading": -0.8086349640155738, + "angularVelocity": -0.6607825551373272, + "velocityX": 1.9874449965735268, + "velocityY": -2.9273472097694815, + "timestamp": 10.40834429173513 + }, + { + "x": 7.600392480009413, + "y": 1.761540500711219, + "heading": -0.8474765480671682, + "angularVelocity": -1.1312266477169204, + "velocityX": 2.021073050510832, + "velocityY": -2.7529231811718247, + "timestamp": 10.442680102449973 + }, + { + "x": 7.671333460317134, + "y": 1.6729216107462341, + "heading": -0.9025012077772409, + "angularVelocity": -1.6025443571741933, + "velocityX": 2.0660930623389726, + "velocityY": -2.580946484734556, + "timestamp": 10.477015913164816 + }, + { + "x": 7.744622230529785, + "y": 1.5909570455551147, + "heading": -0.9652518160942604, + "angularVelocity": -1.827555750413467, + "velocityX": 2.134470358696664, + "velocityY": -2.387145184129499, + "timestamp": 10.51135172387966 + }, + { + "x": 7.860586157347494, + "y": 1.4682098558928158, + "heading": -0.9995269320627294, + "angularVelocity": -0.6040459024385552, + "velocityX": 2.0436848379845327, + "velocityY": -2.1632293533176865, + "timestamp": 10.568094292333646 + }, + { + "x": 7.962380466980202, + "y": 1.3584906150903546, + "heading": -1.012418619935978, + "angularVelocity": -0.22719605799485526, + "velocityX": 1.7939672525620034, + "velocityY": -1.933631906201723, + "timestamp": 10.624836860787632 + }, + { + "x": 8.04779389711619, + "y": 1.259831390089905, + "heading": -1.014642889543101, + "angularVelocity": -0.0391993113411274, + "velocityX": 1.505279589260271, + "velocityY": -1.7387162352450838, + "timestamp": 10.681579429241618 + }, + { + "x": 8.116074558332897, + "y": 1.171249663466821, + "heading": -1.0104296347158561, + "angularVelocity": 0.07425209929757598, + "velocityX": 1.2033410379031306, + "velocityY": -1.5611159141468582, + "timestamp": 10.738321997695603 + }, + { + "x": 8.166855716911636, + "y": 1.0921738551192879, + "heading": -1.002112257895268, + "angularVelocity": 0.14658090120353398, + "velocityX": 0.8949393720151817, + "velocityY": -1.3935888082270873, + "timestamp": 10.79506456614959 + }, + { + "x": 8.199923294477061, + "y": 1.022232267079584, + "heading": -0.9911733302853878, + "angularVelocity": 0.19278167886867675, + "velocityX": 0.582764905896743, + "velocityY": -1.2326123040486314, + "timestamp": 10.851807134603575 + }, + { + "x": 8.215137995862136, + "y": 0.9611644433512547, + "heading": -0.978636960554262, + "angularVelocity": 0.22093412534351606, + "velocityX": 0.2681355779200118, + "velocityY": -1.0762259339362041, + "timestamp": 10.908549703057561 + }, + { + "x": 8.21240234375, + "y": 0.908777952194214, + "heading": -0.9652518160942604, + "angularVelocity": 0.23589246706123476, + "velocityX": -0.04821163698918966, + "velocityY": -0.9232308755907478, + "timestamp": 10.965292271511547 + }, + { + "x": 8.1921938424561, + "y": 0.8654303279065881, + "heading": -0.9517870451764023, + "angularVelocity": 0.24072872246907334, + "velocityX": -0.3612959128063065, + "velocityY": -0.7749866878900871, + "timestamp": 11.02122565071015 + }, + { + "x": 8.15448402268934, + "y": 0.8303967725236208, + "heading": -0.938073387974666, + "angularVelocity": 0.24517841400289994, + "velocityX": -0.6741916956753717, + "velocityY": -0.6263443382988405, + "timestamp": 11.077159029908753 + }, + { + "x": 8.099285775198217, + "y": 0.8037044120128763, + "heading": -0.9241370640104126, + "angularVelocity": 0.2491593421303856, + "velocityX": -0.9868570124313375, + "velocityY": -0.47721701948254364, + "timestamp": 11.133092409107356 + }, + { + "x": 8.026615220367114, + "y": 0.7853870096388412, + "heading": -0.9100105816047303, + "angularVelocity": 0.25255907309879116, + "velocityX": -1.299234122313104, + "velocityY": -0.3274860671120066, + "timestamp": 11.189025788305958 + }, + { + "x": 7.93649309479659, + "y": 0.7754877410206693, + "heading": -0.895735376567552, + "angularVelocity": 0.25521799758407426, + "velocityX": -1.6112404947058143, + "velocityY": -0.1769832032322341, + "timestamp": 11.244959167504561 + }, + { + "x": 7.828947063371523, + "y": 0.7740637661410563, + "heading": -0.8813661690986453, + "angularVelocity": 0.2568986117911059, + "velocityX": -1.9227522628876286, + "velocityY": -0.025458409629728278, + "timestamp": 11.300892546703164 + }, + { + "x": 7.7040158825175595, + "y": 0.7811943329260619, + "heading": -0.8669787071780914, + "angularVelocity": 0.2572249723991828, + "velocityX": -2.2335711277226653, + "velocityY": 0.1274832110480382, + "timestamp": 11.356825925901767 + }, + { + "x": 7.561757726851445, + "y": 0.7969966352924417, + "heading": -0.8526849753347752, + "angularVelocity": 0.2555492274579613, + "velocityX": -2.5433499227893406, + "velocityY": 0.2825200728579338, + "timestamp": 11.41275930510037 + }, + { + "x": 7.402269597446514, + "y": 0.821661602526662, + "heading": -0.8386677065817341, + "angularVelocity": 0.2506065064166751, + "velocityX": -2.8513944926987937, + "velocityY": 0.4409704471214242, + "timestamp": 11.468692684298972 + }, + { + "x": 7.2257452662755925, + "y": 0.8555551558075168, + "heading": -0.8252789625517808, + "angularVelocity": 0.23936948244113093, + "velocityX": -3.1559747274366425, + "velocityY": 0.605962910992889, + "timestamp": 11.524626063497575 + }, + { + "x": 7.032757795836431, + "y": 0.8996580788692503, + "heading": -0.813478587431489, + "angularVelocity": 0.21097196860558187, + "velocityX": -3.4503095147160385, + "velocityY": 0.7884902305855099, + "timestamp": 11.580559442696178 + }, + { + "x": 6.833636314284489, + "y": 0.9619072197023563, + "heading": -0.813478584540231, + "angularVelocity": 5.169110280772625e-8, + "velocityX": -3.5599758928370466, + "velocityY": 1.112915788836524, + "timestamp": 11.63649282189478 + }, + { + "x": 6.641148526278714, + "y": 1.0423607611427537, + "heading": -0.8134785841610236, + "angularVelocity": 6.7796258946599015e-9, + "velocityX": -3.441375986283731, + "velocityY": 1.438381563801643, + "timestamp": 11.692426201093383 + }, + { + "x": 6.45145754132324, + "y": 1.1292034413279326, + "heading": -0.8134785837843238, + "angularVelocity": 6.734793245125226e-9, + "velocityX": -3.3913735889608208, + "velocityY": 1.5526092188499014, + "timestamp": 11.748359580291986 + }, + { + "x": 6.261766612851187, + "y": 1.21604624489008, + "heading": -0.8134785834076242, + "angularVelocity": 6.734791446466924e-9, + "velocityX": -3.3913725791269433, + "velocityY": 1.5526114246341831, + "timestamp": 11.804292959490589 + }, + { + "x": 6.072075684380091, + "y": 1.302889048454317, + "heading": -0.8134785830309245, + "angularVelocity": 6.734793280802982e-9, + "velocityX": -3.3913725791098397, + "velocityY": 1.552611424671542, + "timestamp": 11.860226338689191 + }, + { + "x": 5.882384755908996, + "y": 1.3897318520185546, + "heading": -0.8134785826542248, + "angularVelocity": 6.734792910911008e-9, + "velocityX": -3.3913725791098357, + "velocityY": 1.552611424671551, + "timestamp": 11.916159717887794 + }, + { + "x": 5.692693827449727, + "y": 1.476574655608625, + "heading": -0.8134785822775251, + "angularVelocity": 6.734793219804119e-9, + "velocityX": -3.3913725788983964, + "velocityY": 1.5526114251333973, + "timestamp": 11.972093097086397 + }, + { + "x": 5.5030035972595215, + "y": 1.563418984413147, + "heading": -0.8134785819008252, + "angularVelocity": 6.73479370214728e-9, + "velocityX": -3.391360094956347, + "velocityY": 1.5526386935458116, + "timestamp": 12.028026476285 + }, + { + "x": 5.306759747272106, + "y": 1.6761609741986419, + "heading": -0.8134785815780706, + "angularVelocity": 5.31909195889925e-9, + "velocityX": -3.234156424503322, + "velocityY": 1.8580211843552186, + "timestamp": 12.08870500392225 + }, + { + "x": 5.11051667299417, + "y": 1.788904314208696, + "heading": -0.8134785812553164, + "angularVelocity": 5.3190855034609695e-9, + "velocityX": -3.2341436405827086, + "velocityY": 1.8580434364534788, + "timestamp": 12.1493835315595 + }, + { + "x": 4.914273598725787, + "y": 1.9016476542353784, + "heading": -0.8134785809325621, + "angularVelocity": 5.319086694543979e-9, + "velocityX": -3.2341436404252732, + "velocityY": 1.8580434367275134, + "timestamp": 12.210062059196751 + }, + { + "x": 4.718030524457404, + "y": 2.0143909942620604, + "heading": -0.8134785806098077, + "angularVelocity": 5.319085466039386e-9, + "velocityX": -3.234143640425272, + "velocityY": 1.8580434367275172, + "timestamp": 12.270740586834002 + }, + { + "x": 4.521787450189022, + "y": 2.1271343342887428, + "heading": -0.8134785802870533, + "angularVelocity": 5.319086975081976e-9, + "velocityX": -3.2341436404252715, + "velocityY": 1.8580434367275167, + "timestamp": 12.331419114471252 + }, + { + "x": 4.325544375920639, + "y": 2.239877674315425, + "heading": -0.813478579964299, + "angularVelocity": 5.319087023675505e-9, + "velocityX": -3.2341436404252715, + "velocityY": 1.8580434367275163, + "timestamp": 12.392097642108503 + }, + { + "x": 4.129301301652256, + "y": 2.3526210143421062, + "heading": -0.8134785796415447, + "angularVelocity": 5.319086160384219e-9, + "velocityX": -3.2341436404252857, + "velocityY": 1.8580434367274927, + "timestamp": 12.452776169745754 + }, + { + "x": 3.9330582273170225, + "y": 2.465364354252196, + "heading": -0.8134785793187616, + "angularVelocity": 5.3195605243469425e-9, + "velocityX": -3.234143641526992, + "velocityY": 1.858043434806029, + "timestamp": 12.513454697383004 + }, + { + "x": 3.755112012570184, + "y": 2.567595773148683, + "heading": -0.8087823717593017, + "angularVelocity": 0.07739488320373908, + "velocityX": -2.9326060086797106, + "velocityY": 1.684803881657269, + "timestamp": 12.574133225020255 + }, + { + "x": 3.596937591266501, + "y": 2.6584681509636754, + "heading": -0.8046081994450863, + "angularVelocity": 0.06879158866822643, + "velocityX": -2.6067610316664966, + "velocityY": 1.4976035403865926, + "timestamp": 12.634811752657505 + }, + { + "x": 3.4585349694595866, + "y": 2.737981483942003, + "heading": -0.8009559747359442, + "angularVelocity": 0.06018973846854076, + "velocityX": -2.2809159548879583, + "velocityY": 1.3104031372296205, + "timestamp": 12.695490280294756 + }, + { + "x": 3.339904149133476, + "y": 2.8061357707001306, + "heading": -0.7978256145396123, + "angularVelocity": 0.051589257653807806, + "velocityX": -1.9550708454119348, + "velocityY": 1.1232027112715945, + "timestamp": 12.756168807932006 + }, + { + "x": 3.2410451312582933, + "y": 2.862931010476089, + "heading": -0.7952170508581512, + "angularVelocity": 0.04298989746513998, + "velocityX": -1.6292257199479807, + "velocityY": 0.9360022727560687, + "timestamp": 12.816847335569257 + }, + { + "x": 3.1619579164016844, + "y": 2.908367202779582, + "heading": -0.7931302326117163, + "angularVelocity": 0.03439137908734864, + "velocityX": -1.30338058512905, + "velocityY": 0.748801826160328, + "timestamp": 12.877525863206507 + }, + { + "x": 3.1026425049328332, + "y": 2.9424443472754236, + "heading": -0.7915651260524916, + "angularVelocity": 0.02579341688350326, + "velocityX": -0.9775354442258696, + "velocityY": 0.5616013740406165, + "timestamp": 12.938204390843758 + }, + { + "x": 3.0630988971100095, + "y": 2.9651624437336137, + "heading": -0.7905217148332285, + "angularVelocity": 0.017195724087124466, + "velocityX": -0.6516902990663221, + "velocityY": 0.37440091813044435, + "timestamp": 12.998882918481009 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": 0.008598014051155055, + "velocityX": -0.3258451507561328, + "velocityY": 0.18720045975221852, + "timestamp": 13.05956144611826 + }, + { + "x": 3.0433270931243896, + "y": 2.9765214920043945, + "heading": -0.79, + "angularVelocity": -9.910581864004145e-25, + "velocityX": -2.631400747419988e-26, + "velocityY": 1.5241001239999535e-26, + "timestamp": 13.12023997375551 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index aa076f38..a9cbe21a 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -65,7 +65,7 @@ public static class Drivetrain { public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (IS_L3 ? 16.0 / 28.0 : 17.0 / 27.0) * (45.0 / 15.0); public static final double TURN_GEAR_RATIO = 150.0 / 7.0; public static final boolean IS_TURN_MOTOR_INVERTED = true; - public static final double WHEEL_RADIUS = Units.inchesToMeters(1.9423034948238156); + public static final double WHEEL_RADIUS = Units.inchesToMeters(1.9448479168443666); public static final double MAX_LINEAR_SPEED = Units.feetToMeters(IS_L3 ? 16.0 : 15.7); public static final double TRACK_WIDTH_X = Units.inchesToMeters(18.75); @@ -111,7 +111,7 @@ public static class Vision { public static final String REAR_CAMERA_NAME = "limelight-rear"; public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.086018, 0, 0.627079, new Rotation3d(0, Math.toRadians(-40.843), 0)); - public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(0, Math.toRadians(10), Math.PI)); + public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(0, Math.toRadians(10), Math.PI+Math.toRadians(1.55))); public static final boolean TAKE_SNAPSHOTS = true; public static final double SNAPSHOT_PERIOD_SECS = 1; diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 5aed1bce..11f56803 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -258,8 +258,8 @@ private void configureAutoRoutines() { ) ); - autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.COUNTER_CLOCKWISE))); } + autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.COUNTER_CLOCKWISE))); autos.addDefault(new AutoCommand("Dwayne :skull:")); autos.add(new AmpLanePADEF(drivetrain, shooter, indexer)); autos.add(new AmpLanePAEDF(drivetrain, shooter, indexer)); @@ -285,6 +285,7 @@ private void configureAutoRoutines() { autos.add(new SourceLanePHGF(drivetrain, shooter, indexer)); autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer)); autos.add(new SourceLanePGFE(drivetrain, shooter, indexer)); + autos.add(new SourceLanePGFH(drivetrain, shooter, indexer)); // autos.addDefault(new ATestAuto(drivetrain, shooter, indexer)); autos.add(new AutoCommand("Subwoofer Shot", ShootSequence.forAutoSubwoofer(shooter, indexer))); } diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java index 14e70a44..2170ee0a 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java @@ -28,11 +28,10 @@ public CenterLanePDEABC(Drivetrain drivetrain, Shooter shooter, Indexer indexer) Commands.sequence( Commands.deadline( Commands.sequence( - Commands.waitSeconds(0.1), Commands.waitUntil(()->!indexer.isNoteStaged()), Commands.waitSeconds(0.1) ).withTimeout(1.1), - Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer)) + IntakeAndFeed.withDefaults(indexer) ), createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0, 0.1), Commands.runOnce(drivetrain::unblockTags), @@ -40,7 +39,11 @@ public CenterLanePDEABC(Drivetrain drivetrain, Shooter shooter, Indexer indexer) Commands.runOnce(drivetrain::blockTags), createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0, 0.1), createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0, 0.1), - createSegmentSequence(drivetrain, shooter, indexer, 4, false, true, true, 0, 0.1) + Commands.parallel( + getPath(4).getCommand(drivetrain), + IntakeAndFeed.withDefaults(indexer) + ) +// createSegmentSequence(drivetrain, shooter, indexer, 4, false, true, true, 0, 0.1) ) ) ); diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFE.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFE.java index 20aa7edd..8ae83472 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFE.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFE.java @@ -23,10 +23,10 @@ public SourceLanePGFE(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0), - createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0), - createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0), - createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0) + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0, 0, true), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0,0 , true), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0, 0, true), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0, 0, false) ) ) ); diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFH.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFH.java new file mode 100644 index 00000000..89ce14ae --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePGFH.java @@ -0,0 +1,35 @@ +package org.team1540.robot2024.commands.autos; + +import edu.wpi.first.wpilibj2.command.Commands; +import org.team1540.robot2024.commands.shooter.AutoShootPrepare; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.shooter.Shooter; +import org.team1540.robot2024.util.auto.AutoCommand; +import org.team1540.robot2024.util.auto.PathHelper; + +public class SourceLanePGFH extends AutoCommand { + private static final double SHOT_WAIT = 0.2; + public SourceLanePGFH(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ + super("!SourceLanePGFH"); + addPath( + PathHelper.fromChoreoPath("SourceLanePGFH.1", true, true), + PathHelper.fromChoreoPath("SourceLanePGFH.2"), + PathHelper.fromChoreoPath("SourceLanePGFH.3"), + PathHelper.fromChoreoPath("SourceLanePGFH.4") + ); + + addCommands( + Commands.parallel( + new AutoShootPrepare(drivetrain, shooter), + Commands.sequence( + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0, 0, true), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0, 0, true), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0,0, true), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0, 0, false) + ) + ) + ); + } + +} diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGF.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGF.java index b53fd7cd..21b70423 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGF.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGF.java @@ -23,10 +23,10 @@ public SourceLanePHGF(Drivetrain drivetrain, Shooter shooter, Indexer indexer){ Commands.parallel( new AutoShootPrepare(drivetrain, shooter), Commands.sequence( - createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0), - createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0), - createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0), - createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0) + createSegmentSequence(drivetrain, shooter, indexer, 0, true, true, true, 0, 0, true), + createSegmentSequence(drivetrain, shooter, indexer, 1, false, true, true, 0, 0, true), + createSegmentSequence(drivetrain, shooter, indexer, 2, false, true, true, 0,0, true), + createSegmentSequence(drivetrain, shooter, indexer, 3, false, true, true, 0, 0, false) ) ) ); diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java index adf1ad2b..ce644fba 100644 --- a/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java @@ -11,7 +11,7 @@ public class ContinuousIntakeCommand extends Command { private final Indexer indexer; private final Leds leds; private final double percent; - private final LedPattern detected = SimpleLedPattern.solid("#ffff00"); + private static final LedPattern detected = SimpleLedPattern.solid("#ffff00"); private final Timer timer = new Timer(); @@ -21,6 +21,12 @@ public ContinuousIntakeCommand(Indexer indexer, Leds leds, double percent) { this.percent = percent; addRequirements(indexer); } + public ContinuousIntakeCommand(Indexer indexer, double percent) { + this.indexer = indexer; + this.leds = null; + this.percent = percent; + addRequirements(indexer); + } @Override public void initialize() { @@ -31,13 +37,17 @@ public void initialize() { public void execute() { if (indexer.isNoteStaged()) { indexer.stopIntake(); - leds.clearPatternAll(Leds.PatternLevel.INTAKE_PREREADY); - } else { - if (indexer.getIntakeVoltage() == 0) { - timer.restart(); + if (leds != null) { + leds.clearPatternAll(Leds.PatternLevel.INTAKE_PREREADY); } - if (timer.hasElapsed(0.3) && indexer.getIntakeCurrent() > 30) { - leds.setPatternAll(detected, Leds.PatternLevel.INTAKE_PREREADY); + } else { + if (leds != null) { + if (indexer.getIntakeVoltage() == 0) { + timer.restart(); + } + if (timer.hasElapsed(0.3) && indexer.getIntakeCurrent() > 30) { + leds.setPatternAll(detected, Leds.PatternLevel.INTAKE_PREREADY); + } } indexer.setIntakePercent(percent); } @@ -46,6 +56,8 @@ public void execute() { @Override public void end(boolean interrupted) { indexer.stopIntake(); - leds.clearPatternAll(Leds.PatternLevel.INTAKE_PREREADY); + if (leds != null) { + leds.clearPatternAll(Leds.PatternLevel.INTAKE_PREREADY); + } } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index e3954d18..0a99878c 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -72,8 +72,8 @@ public enum Zone { static final int LEVEL_COUNT = PatternLevel.values().length; public enum PatternLevel { DEFAULT, - TRAMP_STATE, INTAKE_PREREADY, + TRAMP_STATE, INTAKE_STATE, COAST_STATE, DRIVER_LOCK, diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java index 50de24da..4a9b17a4 100644 --- a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.*; import org.team1540.robot2024.commands.drivetrain.DriveWithTargetingCommand; +import org.team1540.robot2024.commands.indexer.ContinuousIntakeCommand; import org.team1540.robot2024.commands.indexer.IntakeAndFeed; import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; @@ -84,11 +85,11 @@ public void setPathIndex(int index){ * @param pathIndex * @return the commands to run in the segment */ - protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry, double extraPreShotWait, double postShotWaitReduction) { + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry, double extraPreShotWait, double postShotWaitReduction, boolean shouldSkipIfEmpty) { return Commands.sequence( Commands.deadline( getPath(pathIndex).getCommand(drivetrain), - new IntakeCommand(indexer, () -> false, 1) + new ContinuousIntakeCommand(indexer, 1) ), drivetrain.commandStop(), Commands.sequence( @@ -110,13 +111,15 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, IntakeAndFeed.withDefaults(indexer) ) ) - ) + ).unless(() -> shouldSkipIfEmpty && !indexer.isNoteStaged()) ); } - + protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry, double extraPreShotWait, double postShotWaitReduction) { + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, shouldZeroCancoder, shouldRealignYaw, shouldResetOdometry, extraPreShotWait, postShotWaitReduction, false); + } protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex, boolean shouldZeroCancoder, boolean shouldRealignYaw, boolean shouldResetOdometry, double extraPreShotWait) { - return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, shouldZeroCancoder, shouldRealignYaw, shouldResetOdometry, extraPreShotWait, 0); + return createSegmentSequence(drivetrain, shooter, indexer, pathIndex, shouldZeroCancoder, shouldRealignYaw, shouldResetOdometry, extraPreShotWait, 0, false); } protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter, Indexer indexer, int pathIndex){